terrain_analysis_ext（扩展远场地形与近远场结果合并）

terrain_analysis_ext（扩展远场地形与近远场结果合并）

实车先看：坡道、落差与近远场衔接

这个节点给全局代价地图提供地形点。当前4 m内借用 terrain_analysis 的结果，4 m外才输出自己计算的结果。 因此，改本块的地面估计或高度筛选参数，不会同时改变近场；4 m是两种结果的交接距离，不是雷达最大量程。

这些参数帮助系统看清地形，不能给出实车允许的坡度或台阶高度。车辆爬坡能力还取决于底盘、重心、附着力和实际测量；目前没有坡道或实物验证。

参数

ros__parameters（ROS节点参数）

参数表

terrain_analysis_ext:
  ros__parameters:
    scanVoxelSize: 0.1
    decayTime: 0.2
    noDecayDis: 0.0
    clearingDis: 20.0
    useSorting: true
    quantileZ: 0.2
    vehicleHeight: 1.0
    voxelPointUpdateThre: 100
    voxelTimeUpdateThre: 2.0
    lowerBoundZ: -1.5
    upperBoundZ: 0.5
    disRatioZ: 0.2
    checkTerrainConn: False
    terrainUnderVehicle: -0.75
    terrainConnThre: 0.5
    ceilingFilteringThre: 2.0
    localTerrainMapRadius: 4.0

参数详解

输入、输出和高度基准

自身远场保留点的 x、y、z，不把点移到地面上；改写的是 intensity，它已经不是雷达反射强度。当前全局代价地图的 IntensityVoxelLayer 使用本输出，并以 min_obstacle_intensity=0.1、max_obstacle_intensity=2.0 筛选高度差。达到10 cm只是满足其中一项条件，还要通过坐标高度、距离等筛选才可能成为障碍。

下文的“高于／低于雷达”都指点的 odom z 减去输入位姿 z；距离指 odom 的水平距离。源码没有在这里查询雷达→底盘 TF，也没有按云台姿态重新定义竖直方向。上游 loam_interface 把输入当雷达位姿，而 Point-LIO 原始输出是内部 IMU 位姿；此前记录的 IMU→雷达转换缺口仍会影响本块的高度基准，不能靠扩大高度范围代替修正接口。

lowerBoundZ、upperBoundZ 与 disRatioZ：能不能看到远处上下坡

这三项先决定坡点有没有资格留下，再谈坡会不会被当成障碍。当前上下边界相对输入位姿，并随距离扩大：

比如5 m外的下坡点比输入原点低2.0 m，能通过当前窗口；低3.0 m则会被删。5 m外的上坡点高1.0 m能通过，高2.0 m则会被删。这里只举高度筛选，仍需满足后面的输出条件。

所以：下坡地面缺失，检查下界；上坡地面缺失，检查上界；越远越缺，连同距离放宽比例一起检查。 下界的“调大”是数值变大，例如-1.5到-1.0，反而缩小向下保留范围。

与上一块不同，扩展节点在入缓存、缓存更新、选地面和自身远场输出时，都使用这个随距离扩大的窗口；没有在最终输出时退回固定上下界。disRatioZ=0.2 表示每增加1 m水平距离，上下各放宽0.2 m，不代表允许的坡度或坡面识别结果。

上实物前应先核对原点离地高度、odom 竖直方向和位姿转换。窗口过窄会丢坡面，过宽会引入顶棚或其他楼层；是否成为障碍还取决于局部地面估计及后续筛选。当前不据这些数值直接修改重力方向或安装外参。

useSorting 与 quantileZ：正常坡为什么也可能像障碍

扩展节点把水平地面分成写死的0.4 m格子，每格收集附近高度，选一个“这片区域的地面高度”。当前先排序，再取 floor(0.2×样本数) 处的高度；10个样本取从低到高第3个。False 则取最低高度。格子和邻域内有坡、障碍或离群点时，这个代表高度未必等于每个点下面的真实地面。

本节点自己的远场始终计算 abs(点高度−估计地面高度)，没有 considerDrop 开关。 一个点比估计地面高15 cm，或低15 cm，都会输出0.15 m的高度差；满足其他条件时，两者都可能达到全局代价地图的10 cm障碍门槛。

这就是下坡尤其要看的地方：正常坡的较低侧，可能只是低于这一小片的代表地面，并不是真正的坑。反过来，真正坑洼只有被雷达看到且留下有效回波，才可能通过这个高度差表现出来；没有回波的断崖不因此自动得到可靠识别。扩展节点也没有近场那种 noDataObstacle 合成障碍功能，没有点的区域如何影响规划，还取决于下游的未知区域设置。

把 quantileZ 调大，通常会抬高代表地面：较高点的高差可能减小，较低点的高差却可能增大。因此不能照搬“调大就更容易上下坡”。应一起看正常坡面、小台阶和低障碍，确认地面估计是否偏高或偏低。这里没有正负高差相加抵消，也没有允许上坡／禁止下坡的专用开关。

vehicleHeight：哪些障碍点能进入远场输出

当前自身远场只保留绝对高差 h<1.0 m 的点，1.0 m边界不保留。这个值没有更改机器人尺寸，也不表示能跨越1 m台阶。

例如：障碍有离地20 cm的有效回波，可能进入输出；若它只有离地1.2 m处的回波，而“地面高度”选得正确，这些回波会被本项删掉。调小可能漏掉部分障碍，调大可能把悬空横梁等结构引入结果。应结合真实车辆需要避让的高度、雷达能看见的部位和地面估计核对，不能直接把名字当整车高度填入。

与下游10 cm门槛共同作用时，自身远场进入障碍筛选的高差范围为 0.1～不足1.0 m，仍需通过其他条件。4 m内借入的近场点沿用近场当前0.5 m上限，不受本项重新筛选。

localTerrainMapRadius：为什么4 m附近要单独看

当前以输入里程计位置为中心：d≤4 m 直接复制 terrain_map 点；d>4 m 使用扩展节点自算点。近场点不会再次套用本节点的 vehicleHeight、排序、高度窗口、衰减或连接检查。

两侧当前有这些区别：

因此，同一坡面或物体随着车辆接近，跨过4 m时可能改变高度差或可见点；这是需要观察的源码差异，目前没有确认发生实际交接故障。扩大半径会借用更多近场结果，缩小会更早使用较粗远场；它不改变代价地图大小、雷达量程或车辆半径。

还有两个接口前提：近场没有点，扩展节点不会用自己的4 m内计算补洞；近场停止更新，源码也不检查其年龄。 它会继续合入最近收到的近场结果，并把合并消息标成当前扫描时间。不能仅凭 terrain_map_ext 的时间戳新，就断定其中所有点都新。

checkTerrainConn 及三个配套参数：当前先知道没有生效

当前 checkTerrainConn=False，因此 terrainUnderVehicle、terrainConnThre、ceilingFilteringThre 都不影响输出。其他高度筛选仍在执行；这里只是专门的连接检查与顶棚筛选关闭。

若以后考虑启用，它从输入位置附近的地面出发，尝试把高度接近的地面区域连起来；自身远场只输出连上的区域。三项含义是：

• terrainUnderVehicle=-0.75 m：仅当中心格没有地面样本时，假定地面在输入原点下方0.75 m，作为检查起点。若使用前提成立，应测量该原点到地面的关系；它不是底盘离地间隙，也不是直接读取的实车高度。
• terrainConnThre=0.5 m：候选两片地面的高度差小于0.5 m才接上。过小可能断开正常坡，过大可能接上真实台阶或跨过缺口；不是允许爬50 cm台阶。
• ceilingFilteringThre=2.0 m：连接检查中，高度相差超过2.0 m的候选可被标记排除。源码用绝对差，远低于当前地面的区域也可能命中；不是“高于雷达2 m就删”的顶棚线，也不是车顶净空。

源码一次能比较横纵各±10个地面格，即各±4 m，并非只检查紧贴的小格；中间是否连续有地面没有逐段证明。因此这个检查也不能直接当作车辆可通行性或可靠断崖判据。当前关闭，无需优先调整这三项；启用的适用性要另行验证。

scanVoxelSize、decayTime 与更新门槛：细障碍和旧点

scanVoxelSize=0.1 m 是自身累计点云的三维降采样尺度，不是地面格子大小，也不保证10 cm以上障碍都能看见。细杆、障碍边缘和远处稀疏回波应重点观察；调小能保留更多细节，但点数与计算量会上升。

历史缓存按写死的2 m水平分块管理。每块新入点达到100，或距上次更新达到2.0秒，或发生清图，才重新降采样并检查年龄；这些步骤仍由新扫描驱动。decayTime=0.2 s 是检查时使用的过期门槛，不能覆盖这个触发条件。

所以，近处点密集的块可能很快更新，远处点少的块可能等到时间门槛，旧点保留超过0.2秒。没有新扫描时也不会单靠计时继续处理。noDecayDis=0.0 没有免衰减区域；以后调大，只让指定范围的自身历史绕过年龄检查，不会自动恢复近场缺失的输出。

这两个更新门槛不是发布频率，源码100 Hz循环也不等于每秒输出100帧。先沿用门槛；若实物发现旧障碍残留、地面缺点或负担过大，再结合点密度、衰减时间一起判断。扩展节点没有近场那组 clearDyObs 动态筛选参数，不能把它当成同一套清理算法。

clearingDis：按事件清半径内，且只清自己

当前20 m表示收到清图事件后，删除水平距离小于20 m的自身缓存点。平时没有自动执行，也不是最大输出范围。

joy 第6个按钮可触发；cloud_clearing 的 Float32 消息还会用消息数值覆盖半径。近场节点对应话题是 map_clearing，二者不同。扩展节点清图后仍会借入 terrain_map 的近场结果，因此近场点可以继续出现在合并输出中；不能把清自身缓存理解为清掉整套地图。本轮没有发送清图事件。

启动来源与当前验证范围

整套导航 launch 会同时安排两个地形分析节点，扩展节点本身始终是独立进程，不随导航组件开关变成组件。本轮在 ysu_ros_humble 中只查询环境与文件，确认包路径、安装后的独立 launch、导航 launch 和两份 YAML 与当前源码文件一致；没有启动节点或读取运行节点的有效参数。

17项都在启动时声明和读取，没有动态参数更新回调；运行时仅写同名参数不会更新算法中的成员变量。clearingDis 可由清图消息改变量，这与重新读取 ROS 参数是两回事。衰减计时使用输入点云时间戳，不是电脑墙钟；本块未列 use_sim_time，也不能据此证明全链路时钟或点内时间单位已经正确。

输入里程计与扫描没有时间配对或插值，也没有等待有效里程计的保护；近场点云同样没有同步与新鲜度检查。节点直接使用输入坐标，未校验参考系，最后却固定写 odom。这些前提对坡面高度和距离分区都有影响。

• 源码能确认： 17项读取与当前门控、随距离扩大的高度窗口、远场绝对高差、4 m近远场合并、衰减与清图触发方式，以及三个未生效参数；上面的行为解释按当前源码复核。
• 仿真已观察： 2026-10-08 的静止和约20 cm往返只覆盖独立 Point-LIO 输出，没有验证扩展地形、上下坡、坑洼、顶棚、4 m交接或全局代价地图。
• 必须实物核实： 输入原点到地面的高度、内部 IMU→雷达→底盘关系、odom 竖直方向、坡面及低障碍的回波密度、下坡与真实落差是否被正确区分、交接处一致性和障碍离开后的残留。MID360 随实际 yaw、不随实际 pitch 转动的机械约定继续保留；本轮未作运行测试。

源码位置：src/pb2025_sentry_nav/terrain_analysis_ext/src/terrainAnalysisExt.cpp；启动来源：src/pb2025_sentry_nav/pb2025_nav_bringup/launch/navigation_launch.py 和两份 nav2_params.yaml；下游高度差筛选：src/pb2025_sentry_nav/pb_nav2_plugins/src/layers/intensity_voxel_layer.cpp。本块仅更新日志解释，工程源码、配置和参数值均保留。