跳到正文

Wiki

导航参数

约 22 分钟阅读

1.导航参数

1.1.实车处理建议说明

下面各参数表的“实车处理建议”以当前 MID360 实物配置为准：use_imu_as_input=False、imu_en=True，建图时不使用先验 PCD。切换估计模型、数据格式、先验地图或启动方式后，需要重新核对参数是否参与计算及是否被 launch 覆盖。

表格中的标签 实物测试时具体怎么做
通常保持当前值，不作为优先调试项 初次测试沿用当前值。主要是算法工作方式或已有默认设置，没有明确问题和理由时，不优先尝试不同值。
上实物前必须核对并正确配置 上实物前确认实际输入、单位和坐标约定。当前值符合实物就保持，不符合则先修正，再进行正式测试。
实物运行时重点观察，有问题再针对性调整 先用当前值测试，重点观察地图重影、定位漂移、有效点数量和计算负载，再结合参数作用决定是否调整以及调整方向。
按设备型号和驱动核对，通常保持当前值 对照雷达型号、驱动实现或厂家资料核对一次。设备和驱动未变时，通常沿用当前值，不反复调试固定硬件或格式参数。
根据实际功能需求开启或设置 根据轨迹显示、状态记录、点云保存、先验定位等需求选择开关或相关值；这类主要是功能选择。
当前运行不使用这个参数，暂不调整 当前源码或配置分支没有使用它，改数值不会改善本轮结果。换模型、数据格式或启用对应功能后，再检查是否需要配置。
已确认当前值错误，必须修改 已有源码、设备资料或实测证据确认当前值错误时使用；说明错误原因和正确配置方式，不为换成实物而强行修改。

“通常保持”表示初次测试不优先调这个参数；“按设备型号和驱动核对”仍需要完成一次设备适配检查；“上实物前必须核对”则强调实际输入、单位和坐标约定必须明确。核对正确就保持，不代表每次上车都要改数值。

具体检查事项、生效条件和源码疑点写在“生效条件／备注”列。当前 gravity 的模长已修正，仍需核实方向与目标参考系；gravity_init 在当前 IMU 初始化路径中不参与计算；lidar_meas_cov 的源码含义需要先核实，再决定是否调整。

1.2.章节目录

1.3.通用参数修改

参数中文名 所属节点 ros__parameters 下的参数路径 作用 当前值 → 建议值
局部车体占用半径 local_costmap.local_costmap robot_radius 告诉局部避障系统，车辆在平面上需要占多大空间。 0.2 → 0.42 m，按当前模型推算
全局车体占用半径 global_costmap.global_costmap robot_radius 让全局规划按相同车体尺寸判断通道是否可通行。 0.2 → 0.42 m，与局部一致
局部最低障碍高差 local_costmap.local_costmap intensity_voxel_layer.min_obstacle_intensity 过滤上游给出的地面高差不足门限的点；当前近场主要用于筛选凸起，不能解释成允许通过的坑深。 当前 0.1 m；候选 0.03～0.05 m，验证后定值
全局最低障碍高差 global_costmap.global_costmap intensity_voxel_layer.min_obstacle_intensity 按上游地形高差筛选全局实时障碍；远场与近场的高差计算不同，不能直接用该值判断实际坑深。 当前 0.1 m；与局部门限一起验证
地面样本不足区域合成障碍开关 terrain_analysis noDataObstacle 开启后，在初始化完成且满足规则时，为地面样本不足的格子合成障碍点；不测量坑深，也不能把欠数据直接判成坑。 当前 False；已确认平整的场地首测可暂保留。防坑需求下单独验证 True 及整条链路，不能将开关开启视为可靠防坑保证。
障碍膨胀范围 两张代价地图 inflation_layer.inflation_radius 设置障碍周围多远仍写入膨胀代价；不等于整圈都是禁止区。 保留 0.7 m
膨胀代价衰减系数 两张代价地图 inflation_layer.cost_scaling_factor 控制外围代价随距离下降的速度；数值越大，下降越快。 保留 4.0
近场地形输出高度上限 terrain_analysis vehicleHeight 限制输出地形点相对估计地面的高度差。 暂保留 0.5 m，不要直接填底盘厚度
远场地形输出高度上限 terrain_analysis_ext vehicleHeight 限制远场地形输出点的高度差。 暂保留 1.0 m
体素起始高度 两张代价地图 intensity_voxel_layer.origin_z 设置体素窗口在对应地图坐标系中的起始高度。 平地首测保留 0.0 m；坡地另核对
每层体素高度 两张代价地图 intensity_voxel_layer.z_resolution 决定竖直方向每层体素的厚度。 保留 0.05 m
竖直体素层数 两张代价地图 intensity_voxel_layer.z_voxels 与每层高度共同决定竖直覆盖范围；目前总高约 0.8 m。 暂保留 16，结合需要检测的高度验证
局部地图覆盖宽、长 local_costmap.local_costmap width、height 决定车周围滚动地图的范围，不是车辆尺寸。 保留 5 × 5 m
地图格子大小 两张代价地图 resolution 决定二维网格精细程度；越小，计算量通常越大。 保留 0.05 m

这些参数分为六类：车体尺寸、低矮凸起筛选、坑洼与欠数据处理、障碍膨胀、竖直感知范围和地图网格。它们的取值依据不同，不能全部按底盘长宽直接计算。

车体尺寸：为什么两处 robot_radius 建议设为 0.42 m

当前模型的底盘碰撞盒是 0.525 × 0.525 m，参考点位于底盘平面中心。用圆形轮廓表示车辆时，需要覆盖四个角，因此半径至少为：

R = √((0.525/2)² + (0.525/2)²)
  ≈ 0.371 m

取 0.42 m，相当于在模型外接圆基础上留约 5 cm 余量。这个余量用于应对定位、地图离散化和控制误差，是第一版测试的工程起点，并非已经测定的误差上限。

局部、全局代价地图应使用一致的车体尺寸：全局负责规划能通过的路线，局部负责跟踪和碰撞判断。当前 0.2 m 半径不足以包住模型底盘；降低车速也不能弥补这个几何尺寸错误。

低矮凸起筛选：为什么 min_obstacle_intensity 需要验证

这里接收的是地形分析点云，intensity 表示算法计算的地面高差，单位是米。近场先计算“点高度减去估计地面高度”；扩展节点自身远场对这个差值取绝对值。它是相对于局部估计地面的结果，不一定等于从正常路面量出来的真实凸起高度或坑深。

当前 0.1 相当于 10 cm。假设地面估计正确，且其他筛选条件满足：地面上高5 cm的凸起点会低于该门限，高15 cm的点则达到这道门限。因而，几厘米高、车辆却不能通过的物体可能没有进入实时障碍地图。这不表示深度小于10 cm的坑允许通过，也不保证更深的坑一定被识别。

0.03～0.05 m 是平地低速测试的候选范围，用来检查能否保留这些低矮障碍。最终门限应同时满足：

  • 必须绕开的最低障碍能够被保留。
  • 正常地面起伏和测量噪声不会大量变成假障碍。

例如，若 3 cm 高的物体必须绕开，最终采用 0.05 m 就可能将它过滤掉。局部、全局地图的两个门限应一起核对，避免两张地图对同一障碍判断不同。

坑洼与欠数据处理：noDataObstacle 能做什么，不能做什么

坑洼要区分两种情况：

  • 雷达看到了坑底。 当前 terrain_analysis.ros__parameters.considerDrop=False，近场低于估计地面的负高差点会被过滤。降低 min_obstacle_intensity 不能恢复已经被上游删掉的点。即使以后考虑开启 considerDrop，地面估计、坑底能否被看到和下游高度筛选仍会影响结果，不能把取绝对值当作完整的防坑检测。
  • 雷达没有看到坑底或地面。 此时没有有效点可用来计算坑深。noDataObstacle 针对的是这类地面样本不足的区域，但遮挡、扫描盲区、点云稀疏和点被前级过滤，同样可能造成样本不足；欠数据不等于已经发现真实坑洞。

noDataObstacle 的完整配置位置是：

terrain_analysis:
  ros__parameters:
    noDataObstacle: False

它的两种取值含义是：

  • False：本节点不为欠数据格合成额外障碍点。静态地图、其他障碍来源或下游未知区域策略仍可能阻止通行，不能因此断言“没有点就一定能走”。
  • True：完成无数据处理初始化后，将地面高度样本数不足 minBlockPointNum 的格子作为欠数据候选，再按 noDataBlockSkipNum 的边界跳过规则合成障碍点。当前 minBlockPointNum=10、noDataBlockSkipNum=0；它判断的是样本是否足够，不是只判断点数是否为零，也没有计算坑的深度。

开启后可以让欠数据区域更保守，但也可能把正常的遮挡或稀疏地面变成障碍，导致原本能走的区域被封住。合成点还需通过下游高度、量程和体素窗口等筛选，最后才可能进入代价地图。因此，应分别检查地形点云和最终代价地图，不能只看开关是否为 True。

该开关属于近场 terrain_analysis，terrain_analysis_ext 没有同名开关。扩展节点会合并近场范围内的地形结果，因此合成点也可能随之进入全局障碍链路，但这不等于远场全范围都具备同样的欠数据保护。远场对高差取绝对值，也不能保证近车的坑、断崖或楼梯边缘会被可靠识别。

第一版在已确认无坑和断崖的平整场地进行低速测试时，可暂保留当前 False。若后续确实需要防坑，再单独验证开启后的地形输出、代价地图和停车行为；既不能用10 cm门限判断坑是否可通过，也不能把 noDataObstacle=True 当作已经具备可靠的防跌落保护。 实现细节见 近场地形分析 与 扩展地形分析。

障碍外围代价：为什么先保留 inflation_radius=0.7、cost_scaling_factor=4.0

这两个值目前沿用已有配置，尚没有证据需要为首次低速测试调整；它们不是根据底盘尺寸推算出来的唯一正确值。

inflation_radius=0.7 m 表示障碍周围写入膨胀代价的范围。它大于拟采用的车体半径，在车体碰撞边界之外留出外围代价区域，供规划器考虑靠近障碍的代价。这个范围并非全部都是硬性禁止区。

cost_scaling_factor=4.0 控制外围代价下降的速度。数值越大，代价随距离下降越快；数值越小，较高代价维持得更远。

你的跟踪控制器主要检查高代价格子，没有直接利用全部外围软代价给轨迹打分。因此，先正确配置 robot_radius；扩大膨胀范围并不能替代车体尺寸适配，也不保证控制器自动更早减速。

竖直感知范围：为什么高度参数暂时保留

terrain_analysis.vehicleHeight=0.5 和 terrain_analysis_ext.vehicleHeight=1.0，限制的是输出地形点相对估计地面的高度差。它们不表示底盘厚度，也不是车辆能跨越的台阶高度。

这两个值暂时保留，是因为单凭底盘长宽无法推出合适的高度筛选范围。后续需要结合整车外形，检查可能碰到车身或云台的结构是否仍被保留。不能直接把底盘厚度 0.20 m 填进去。

两张代价地图的竖直体素参数则决定另一道空间窗口：

origin_z = 0.0 m
z_resolution = 0.05 m
z_voxels = 16

竖直覆盖总高 = 0.05 × 16 = 0.8 m

这些高度按对应地图坐标系解释。当前平地首测可以先保留，但上坡、下坡或地图高度基准变化时，需要重新核对窗口是否仍覆盖有效障碍。即使前置筛选写了 max_obstacle_height=2.0，点也仍要通过这道体素窗口。

地图网格：为什么先保留 5 × 5 m 和 0.05 m

局部地图的 width=5、height=5 表示整张滚动地图覆盖约 5 × 5 m，中心两侧各约 2.5 m。这是观察和计算范围，与车辆外形大小无关。

resolution=0.05 m 表示每个格子边长为 5 cm。局部地图大约包含：

(5 / 0.05) × (5 / 0.05) = 100 × 100 个格子

这组设置可以作为低速首测的起点。减小格距能表达更细的障碍，但会增加计算量；扩大地图范围同样会增加处理开销。目前先观察实际障碍表达与计算负载，再决定是否调整。

全局地图最终使用的格距还可能受静态地图影响，应以运行时实际地图信息为准。

1.4.测试

1.4.1.低速测试

以下用于第一版实车导航测试：保留北极熊算法源码和原有 launch 接线，通过导航参数降速，并在自己的串口节点增加最终限幅。限制的是目标指令，实车速度和停车响应仍需验证。

1. 导航参数

配置文件：src/pb2025_sentry_nav/pb2025_nav_bringup/config/reality/nav2_params.yaml。表中的 FollowPath 参数位于 controller_server.ros__parameters.FollowPath 下。

参数 建议值 作用与说明
FollowPath.v_linear_min/max -0.2 / 0.2 m/s 限制路径跟踪控制器输出的线速度标量，之后再分解成 vx、vy。这里的负值是反向输出边界，不是要求至少跑 0.2 m/s。
FollowPath.min_approach_linear_velocity 0.03 m/s 接近终点减速时使用的最低速度参考，避免过早降得太低。它不是全程最低速度，到点后仍应停车。
controller_server.min_y_velocity_threshold 0.001 m/s 将里程计中绝对值很小的横向速度视为零，用于过滤噪声。不限制下发速度；设成 0.5 会把正常低速横移也当成静止。
velocity_smoother.max_velocity [0.14, 0.14, 0.2] 分别限制 vx、vy、wz 的正向最大值。前两项单位为 m/s，第三项为 rad/s；两个平移分量同时达到上限时，合速度约为 0.198 m/s。
velocity_smoother.min_velocity [-0.14, -0.14, -0.2] 分别限制 vx、vy、wz 的负向边界，允许后退、右移和反向旋转。不是最低运动速度，零速度仍然允许。
velocity_smoother.max_accel [0.15, 0.15, 0.3] 限制各轴加速的快慢，使起步更平缓。前两项单位为 m/s²，第三项为 rad/s²。
velocity_smoother.max_decel [-0.3, -0.3, -0.6] 限制正常减速的快慢，按此组件要求填写负值。绝对值越大，允许减速越快；绝对值过小会增加停止时间和距离。
velocity_smoother.velocity_timeout 0.2 s 超过这段时间没收到上游速度，就把目标速度设为零，再按减速度限制降速。不是保证 0.2 秒内停住。
FollowPath.enable_rotation false 正常路径跟踪时输出的导航角速度 wz_nav2 为零；这个开关不禁止手柄或其他来源请求底盘旋转。
fake_vel_transform.init_spin_speed 0.0 rad/s 自旋速度的启动初值。之后收到非零 cmd_spin 仍会改变自旋速度，首测不要额外发布自旋指令。

x、y 各设 0.14，是因为平滑器按轴限速，对角合速度约为 sqrt(0.14² + 0.14²) = 0.198 m/s，沿单轴移动的上限则为 0.14 m/s。 当前 enable_rotation=false 且自旋项为零时，正常跟踪下发的底盘角速度为零。

2. 恢复动作单独降速，保持原 launch 接线

以下两个行为树都使用相同的低速恢复参数：

  • src/pb2025_sentry_nav/pb2025_nav_bringup/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
  • src/pb2025_sentry_nav/pb2025_nav_bringup/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
参数 原始值 → 低速测试值 作用与说明
backup_speed 1.0 → 0.10 m/s 限制恢复动作请求的平移速度。当前插件是 BackUpFreeSpace，会选择空闲方向移动，未必固定向车后方退。
backup_dist 1.0 → 0.20 m 限制一次恢复动作的目标移动距离。只降低速度不会同时缩短移动距离。 
<BackUp backup_dist="0.20" backup_speed="0.10"/>

本轮沿用实车默认的 use_composition=True。该分支中，恢复速度经过云台坐标变换后到达串口，但绕过 velocity_smoother； 因此，上表中的平滑器加减速度参数不约束这条恢复路径。 Nav2 Humble 官方默认启动接线同样让恢复动作绕过速度平滑器，本轮不要求修改 launch 将恢复动作接入平滑器。 非组合启动分支不在本轮验证范围内。

3. 串口最终限幅与断流保护

修改文件：src/serial_comm/src/serial_comm.cpp。 以下两项是速度回调中的 C++ 常量，不属于 nav2_params.yaml；CMD_VEL_TIMEOUT 是串口节点自己的接收超时门限。

参数 低速测试值 作用与说明
MAX_LINEAR_SPEED 0.2 m/s 对最终下发的平移合速度 hypot(vx, vy) 限幅；超限时同比缩小两个分量，保持方向。覆盖经过该串口出口的速度来源。
MAX_ANGULAR_SPEED 0.2 rad/s 限制最终下发的底盘角速度，包括叠加后的自旋。首测希望禁止所有串口旋转指令时，可以设为 0.0。
CMD_VEL_TIMEOUT 200 ms 本节点超过该时间未收到速度消息，就把速度有效位置零，并把本次发送的 vx、vy、wz 清零。与上游平滑器的 velocity_timeout 是两个独立门限。

速度保护按以下顺序执行：

  1. 收到速度时，先检查 vx、vy、wz 是否都是有限值；任一字段为 NaN/Inf，就清零速度并标记为无效，不刷新有效指令的接收时间。
  2. 在 double 精度下计算平移合速度，超限则同比缩小 vx、vy；再限制 wz，最后转换成协议使用的 float32。
  3. 在发送定时器中检查接收超时，过期后同时发送无效标志和零速度，保留原协议结构及云台角度字段。

vx、vy 表示底盘平移速度在云台 yaw 坐标系下的分量，单位为 m/s；wz 是底盘角速度，单位为 rad/s；rel_pitch、rel_yaw 是云台目标相对角，单位为 rad。串口速度限幅不会替代正常加减速控制。

4. 加载与测试

修改 C++ 后，需要在实际运行环境重新编译并重启 serial_comm。导航 YAML 和行为树 XML 也需要重新加载：若安装目录使用符号链接，重启导航即可；若安装目录是复制文件，则先重新安装对应包。修改控制器速度上限后应重启导航，使 PID 重新读取上下限。

启动后核对实际参数；使用命名空间时，为节点名加上对应前缀：

ros2 param get /controller_server FollowPath.v_linear_max
ros2 param get /controller_server min_y_velocity_threshold
ros2 param get /velocity_smoother max_velocity
ros2 param get /velocity_smoother velocity_timeout
ros2 topic info /cmd_vel --verbose

对应预期为 0.2、0.001、[0.14, 0.14, 0.2]、0.2。检查 /cmd_vel 发布者，避免独立启动的手柄转换节点与导航直接竞争同一个出口；需要手柄接管时，应使用明确的指令仲裁。

先架空核对方向、单位、零指令和断流停车，再落地进行短距离单目标测试。重点观察实际速度、取消导航、到点停车以及恢复动作结束后的表现；保持硬件急停可用。

本轮保留的 fake_vel_transform 旧速度缓存问题尚未修改：串口超时只能检测自己是否持续收到消息，无法识别上游重发的旧非零指令。 因此，串口限速和断流清零不能代替对整套导航取消、到点停车行为的验证。