terrain_analysis（近场地面与障碍高差分析）

terrain_analysis（近场地面与障碍高差分析）

地形分析节点分工

两个节点都订阅 loam_interface 的 lidar_odometry 和 registered_scan；ext 还订阅 terrain_map，当前以输入里程计的位置为中心，4 m内采用近场结果、4 m外计算扩展结果，再合并输出。本块只整理 terrain_analysis，terrain_analysis_ext 的参数留到下一块。

本节点在 odom 的水平格子内估计地面高度，输出点的 intensity 改为相对地面的高度差，单位为 m，已经不是雷达反射强度。当前局部代价地图的 IntensityVoxelLayer 按该字段筛选障碍，允许范围为0.1～2.0 m，并另外检查点坐标高度、距离等条件；不是把正负高度相加为通行代价。

上坡下坡先看哪些参数

先抓住两个问题：正常的坡会不会被当成障碍，以及坡上的地面点有没有被过滤掉。 这段讲的是导航系统如何看待坡；目前没有坡道测试，不能从这些数值判断实车最多能爬几度。

这里的高低按代码当前使用的高度方向计算，实际坐标方向仍需核对。正常下坡是一片连续降低的地面；坑、断崖或向下台阶则涉及地面突然降低，不能把它们都归为同一个“下坡开关”。

例如，其他条件都满足时：同一个坡上的点，若被算成比这一小片地面高6 cm，会低于当前10 cm障碍门槛；若地面高度估计偏低，把它算成高15 cm，就可能被画成障碍。这里是局部地面高度差，不是整条坡的总高度。调整地面估计或抬高障碍门槛，可能减少坡面误判，也可能让真实低障碍被忽略，不能只看坡能不能通过。

vehicleHeight=0.5 限制的是正常输出点相对估计地面的高度差。它没有表示“车辆能爬0.5 m高的台阶”，也没有规定允许的坡度。

所以，坡道分析先沿着这条顺序看：地面高度选得对不对 → 坡点有没有被删掉 → 下游是否把正常高差当成障碍。 动态物体清理、旧点保留时间和更新次数主要解决其他问题；当坡面点缺失时再检查它们的间接影响。

参数

ros__parameters（ROS节点参数）

参数表

terrain_analysis:
  ros__parameters:
    scanVoxelSize: 0.05
    decayTime: 0.5
    noDecayDis: 0.0
    clearingDis: 0.0
    useSorting: True
    quantileZ: 0.2
    considerDrop: False
    limitGroundLift: False
    maxGroundLift: 0.3
    clearDyObs: True
    minDyObsDis: 0.3
    minDyObsAngle: 0.0
    minDyObsRelZ: -0.3
    absDyObsRelZThre: 0.2
    minDyObsVFOV: -28.0
    maxDyObsVFOV: 33.0
    minDyObsPointNum: 1
    noDataObstacle: False
    noDataBlockSkipNum: 0
    minBlockPointNum: 10
    vehicleHeight: 0.5
    voxelPointUpdateThre: 100
    voxelTimeUpdateThre: 1.0
    minRelZ: -1.5
    maxRelZ: 0.5
    disRatioZ: 0.2

参数详解

scanVoxelSize（点云降采样尺度）

当前0.05 m表示累计点云重采样时的三维体素边长。同一小体素中的点合成代表点，可以压缩重复扫描；调得更大可能减少窄障碍、边缘和有效地面样本，调得更小则增加点数和处理负担。

源码另外固定了1.0 m的历史缓存分块和0.2 m的平面地面格子。scanVoxelSize 不改变这两种格子的尺寸，也不等于代价地图分辨率。地面样本来自平面格子的3×3邻域，minBlockPointNum 判断的是这个邻域收集到的高度样本数。

decayTime、noDecayDis 与 clearingDis（旧点保留、免衰减与事件清图）

新点进入缓存时，intensity 暂存“该帧时间戳减首次点云时间戳”，用于历史管理；输出时再改为高度差。容器 PCL 的体素滤波默认平均各字段，因此年龄判定针对重采样代表点，不能理解为逐个原始点精确保留时间。

缓存格只有满足以下任一条件才重做降采样和筛选：新入点累计达到100、距离上次更新达到1.0秒、收到清图事件。检查时按如下关系保留点：

高度在缓存允许窗口内
且（代表点年龄 < decayTime，或水平距离 < noDecayDis）
且不属于本次触发清图的半径内

因此 decayTime=0.5 是更新时检查的年龄阈值，不是点到0.5秒就立刻消失。稀疏缓存格可能等到时间更新门槛才重新检查；没有新点云时，这段处理不会执行。增大衰减时间有利于保留地面样本，也可能延长旧物体残留；减小则需观察稀疏格是否出现空洞。

当前 noDecayDis=0，没有近距离免衰减区域。这个值还参与无数据功能的初始化：从记录位置移动至少该距离才进入无数据处理状态，0时首次里程计回调即可满足距离条件；当前 noDataObstacle=False，后者不会生成障碍。

clearingDis 仅在清图事件触发后使用，清掉的是 d < clearingDis 的点。当前0没有正半径清除区；joy 的第6个按钮可触发清图，map_clearing 的 Float32 消息会覆盖清图半径并触发处理。原注释“该距离外的点会被清除”方向相反，不能照此理解。清图只影响本节点缓存，不是删除 Point-LIO 地图；本轮未发布任何清图消息。

useSorting、quantileZ 与 considerDrop（地面高度选择与低处高差处理）

每个0.2 m平面格先收集邻域高度。useSorting=True 时排序并取下标 floor(quantileZ×N) 的高度；当前 quantileZ=0.2，例如10个样本会取下标2，即从低到高第3个样本。它可以减少极低离群点对“最低地面”的影响，但分位比例过高也可能把障碍底部或坡面较高部分当作地面。

地面高度记为 g，原始高度差为 h=z−g：

• 当前 considerDrop=False：只输出非负高度差，低于估计地面的点被过滤。
• True：先取 abs(z−g)，凹下去的点也可能作为高度差输出；仍需通过其他高度和样本数条件。

源码没有坡面正负高度相加抵消的步骤，也没有禁止这两个开关同时开启。它们可以同时执行，但坡面较低部分取绝对值后可能被当作障碍。useSorting 是地面估计方式，是否能上坡还取决于坐标轴、坡度、高度窗口、下游障碍阈值及车辆能力，不能保证“开启就能上坡”。

当前局部代价地图的强度下限为0.1 m。地面估计抬高会降低凸起的高度差，可能让低障碍低于该门槛；估计过低则可能把正常地面标成障碍。后续实物应同时检查地面、低障碍和坡面，不能只看点云是否连续。

limitGroundLift 与 maxGroundLift（地面估计抬升限制）

启用限制且使用排序时，地面估计不会高于“邻域最低高度 + maxGroundLift”。它限制的是分位地面相对最低样本的抬升，不是云台升降、底盘离地高度或每帧地面变化率。

当前 limitGroundLift=False，所以 maxGroundLift=0.3 不影响结果，暂不调整。最低样本本身若为低离群点，开启限制也可能将地面压低。

clearDyObs 及其几何门槛（动态残留点筛选）

这个开关用旧点与最新回波做格子级候选筛选，不估计物体速度，也不创建“动态物体走过就永久禁行”的区域。候选计数每轮重建，最后只筛掉本轮仍被标记的格子输出；缓存老点的移除另由衰减等规则处理。

当前过程是：

1. 累计点云中，水平距离不大于 minDyObsDis=0.3 m 的点直接给格子增加 minDyObsPointNum；不是忽略近点。
2. 较远的点先满足 angle1 > minDyObsAngle，再转到输入位姿对应的雷达自身系；自身仰角在视场窗口内，或 abs(z_L)<absDyObsRelZThre，就给格子计数。
3. 最新一帧裁剪点中，只要该格有满足 angle1 > minDyObsAngle 的点，就把计数归零；这一轮没有再次要求视场窗口或0.3 m距离门槛。
4. 最终计数达到 minDyObsPointNum=1 的格子不进入正常地形输出。

这里的第一个仰角按 odom 中相对输入位姿的位置计算：

[ \theta_1=\operatorname{atan2}(\Delta z-\text{minDyObsRelZ},d)\frac{180}{\pi} ]

当前0°角门槛和-0.3 m高度偏置，在非零水平距离处等价于要求 Δz>-0.3 m。minDyObsAngle 与 minDyObsRelZ 同时影响历史候选和最新回波重置，调整后的净清理效果不能只按一个方向推断。

第二个仰角在雷达自身系中计算，单位同样是度。当前窗口为(-28°,33°)，另外 abs(z_L)<0.2 m 可绕过该窗口。MID360 官方标称垂直视场为 -7°～52°；这与当前软件窗口不同，但窗口还承担候选筛选，不能据此直接判定两项数值必须替换。Livox 官方规格

上实物前需要核对输入位姿究竟代表 IMU 还是雷达、逆旋转后的轴向，以及这个窗口的筛选目的。雷达自身系已经扣除安装姿态，不应把整机倾斜角随意加到视场上下界；这里度制的阈值也不能与弧度制 JointState 混用。MID360 随实际 yaw、不随实际 pitch 转动的机械约定继续保留。

遮挡、稀疏回波或位姿错误可能使静态障碍也暂时缺少“本帧确认”，从而被误筛。当前 minDyObsPointNum=1 比较敏感；应观察障碍离开后的残留和静态物体被遮挡时的结果，再判断是否需要调整，而不是直接根据原注释关闭或修改。

noDataObstacle、noDataBlockSkipNum 与 minBlockPointNum（缺数据区域与最少地面样本）

noDataObstacle=True 且初始化完成时，地面样本不足的格子会生成4个障碍标记点，z 设为输入位姿高度，intensity 设为 vehicleHeight。这表示主动阻挡欠数据区域；False 只是不生成这些点，未知区域怎样通行仍由下游决定。

noDataBlockSkipNum 控制欠数据格边界的跳过迭代次数，不能按米或跳过几帧理解。当前无数据功能关闭，这项不影响结果，简述其用途即可。

minBlockPointNum=10 仍然生效：正常地形点输出要求邻域地面样本数至少10。提高它会减少稀疏格输出，降低它可能保留更多细小障碍，但也更依赖少量、不稳定的地面样本。它同时受降采样尺度、衰减时间和实际点密度影响。

vehicleHeight（地形输出高差上限）

正常地形点要求 0≤h<vehicleHeight，当前即0～0.5 m，不包含0.5 m上界。h 相对估计地面，minRelZ/maxRelZ 则相对输入雷达位姿的 odom z，两者参考基准不同。

该参数没有直接控制模型尺寸或碰撞包络。若障碍只有高于0.5 m的有效回波，这些点会被过滤；调小可能漏障，调大则可能把更多悬空结构引入输出。需结合真实车辆包络、雷达可见的障碍部位和下游筛选核实，不能仅凭名字把它设成整车高度。

当前正常输出高度差还与下游0.1 m障碍下限共同作用，因此进入局部障碍标记的正常点，高度差通常需落在0.1～0.5 m之间，并通过其他过滤。当前数值是否适合实车尚无测量结论。

voxelPointUpdateThre 与 voxelTimeUpdateThre（缓存点云更新门槛）

每个1.0 m缓存分块的新入点计数达到100，或距离该块上次更新达到1.0秒，就重新降采样并筛除过期点；清图事件也会强制更新。计数在更新后清零，计时采用输入点云时间。

增大门槛通常减少这一阶段的更新频率，减小则增加处理次数。它们不是发布频率参数，也不是地面样本数门槛；发布由新点云处理驱动，实际频率还受负载影响。先沿用当前值，若历史残留或计算负担异常，再与 decayTime 一起检查。

minRelZ、maxRelZ 与 disRatioZ（相对输入位置的高度窗口）

设 Δz=z_point−z_input_pose，d 为 odom 中的水平距离。入缓存以及缓存更新时使用：

[ \text{minRelZ}-\text{disRatioZ},d <\Delta z< \text{maxRelZ}+\text{disRatioZ},d ]

当前在5 m水平距离处，缓存候选窗口为(-2.5,1.5) m。但源码在收集地面样本和输出正常点时，又要求固定的 -1.5<Δz<0.5，没有继续使用距离扩展。

所以 disRatioZ 能放宽缓存候选范围，却不能保证远处坡面超出固定窗口的点最终被保留。原注释只解释了前一阶段，不能据此认定整条链都随距离扩大高度范围。

这些高度沿 odom 的 z 轴，并相对输入 lidar_odometry 的位置，不是相对底盘原点或云台 pitch 层。需先确认雷达到地面高度、odom 的方向以及 IMU／雷达位姿转换；参考系错误不能通过单纯加大窗口解决。本轮不修改重力方向或任何工程参数。

启动来源与当前验证范围

整套 reality／simulation 启动分别默认使用对应目录的 nav2_params.yaml，经 navigation_launch.py 将处理后的参数文件交给该节点，并把节点名设为 terrain_analysis 以匹配 YAML。包内独立 XML launch 和 C++ 默认值是另外的来源，例如独立 XML 的 decayTime=1.0、noDecayDis=1.75、vehicleHeight=1.5，不能套用本表；直接运行可执行文件也不会自动加载整套 YAML。

这26项均在启动时声明和读取，没有动态参数更新回调。maxGroundLift 和 noDataBlockSkipNum 当前因开关关闭而不影响结果；clearingDis 由事件使用且可被消息覆盖，其变量变化也不能只靠启动参数表判断。

源码还存在接口前提：两个输入分别回调，使用最近收到的里程计，没有时间配对、插值或“已获得有效里程计”的保护；输出参考系固定写成 odom，没有校验或变换输入参考系。上游位姿与坐标关系仍需核对，尤其会影响本块的高度窗口和动态候选判断。

• 源码能确认： 参数读取、单位、门控分支、衰减与清图条件、分位地面估计、动态候选筛选和高度输出范围；本轮在容器中确认安装后的导航 launch 与两份 YAML 对应当前源码。
• 仿真已观察： 2026-10-08 仅验证独立 Point-LIO 的静止和短距离往返输出，没有验证本节点地面估计、坡面、动态清理、欠数据障碍或最终代价地图。
• 必须实物核实： 雷达原点到地面的高度、安装与内部外参、目标参考系方向、实际点密度、可见障碍高度、坡面与落差特征，以及动态物体离开／静态物体被遮挡时的筛选表现。当前无实物，本轮未启动节点或进行运行测试。

源码位置：src/pb2025_sentry_nav/terrain_analysis/src/terrainAnalysis.cpp；下游强度筛选位于 src/pb2025_sentry_nav/pb_nav2_plugins/src/layers/intensity_voxel_layer.cpp。本块纠正的是日志中的参数解释，工程源码、配置和参数值均保留。