3D 雷达导航跑起来了，但机器人为什么老是绕路、贴墙、停住？Nav2 costmap 实机调参思路
GitHub - Ikunio/Lidar_nav2_ws: 基于 Livox MID-360 3D LiDAR 的 ROS 2 自主导航工作空间，集成 LIO 里程计、重定位、Nav2 导航，支持仿真与实机部署。 · GitHub基于 Livox MID-360 3D LiDAR 的 ROS 2 自主导航工作空间，集成 LIO 里程计、重定位、Nav2 导航，支持仿真与实机部署。 - Ikunio/Lidar_nav2_ws
https://github.com/Ikunio/Lidar_nav2_ws
1. 问题现象
很多人把 ROS2 Nav2 跑起来之后，会遇到一个很尴尬的问题：
Nav2 能启动；
RViz 里路径也能规划出来；
/cmd_vel 也有输出；
TF 树也基本连通；
机器人确实会动；
但是导航效果很差。
具体表现通常是：
1. 机器人离墙很近，像是在“贴边走”；
2. 明明前面能过去，机器人却绕很远；
3. 走着走着突然停住；
4. 原地转圈，不知道怎么绕障碍；
5. RViz 里 costmap 看起来一片红；
6. 机器人把自己车体当成障碍物；
7. 障碍物已经移走了，但地图里还残留“幽灵障碍物”。
很多人第一反应是调 controller 参数，比如 DWB、Regulated Pure Pursuit、MPPI。
但在实机导航里，我更建议先不要急着调控制器。因为这类问题，大部分不是控制器本身导致的，而是 Nav2 costmap 的输入、footprint、障碍物层、膨胀层、清障逻辑、点云高度过滤 没有配好。
这篇文章就从 3D 雷达接 Nav2 的角度，把这些问题讲清楚。
2. 先说结论
如果你的机器人已经满足下面几个条件：
1. map → odom → base_footprint / base_link TF 链路正常；
2. /scan 或 /pointcloud 能正常发布；
3. Nav2 lifecycle 节点已经 active；
4. RViz 能看到 global path；
5. /cmd_vel 有输出。
但机器人依然出现：
贴墙、绕路、停住、原地转圈、误识别障碍物
那么优先排查顺序应该是：
1. 雷达数据是否正确进入 costmap；
2. 点云转 LaserScan 的高度范围是否合理；
3. robot footprint / robot_radius 是否正确；
4. obstacle_layer 是否正确标记障碍物；
5. clearing 是否能清除已经消失的障碍物；
6. inflation_layer 是否把障碍膨胀得太大或太小；
7. 最后再调 controller。
不要一上来就调 DWB 参数。
因为 controller 只是在 costmap 上做局部运动决策。如果 costmap 本身就是错的，controller 再怎么调也只是“在错误地图上努力”。
3. Nav2 costmap 在导航链路里的位置
对于 3D LiDAR + ROS2 Nav2 的移动机器人，常见数据链路大概是这样：
flowchart TD
A[3D LiDAR PointCloud2] --> B[点云预处理]
B --> C[高度裁剪 / 地面过滤]
C --> D[PointCloud2 转 LaserScan]
D --> E[Nav2 obstacle_layer]
E --> F[local_costmap]
F --> G[controller_server]
G --> H[/cmd_vel]
H --> I[底盘运动]
J[全局地图 map] --> K[global_costmap]
K --> L[planner_server]
L --> M[global path]
M --> G
简单理解：
global_costmap 主要服务于全局路径规划；
local_costmap 主要服务于局部避障和控制器；
controller_server 根据 local_costmap 和 global path 输出 /cmd_vel。
所以当机器人表现异常时，要先判断问题在哪一层。
| 现象 | 更可能的问题 |
|---|---|
| 路径绕很远 | global_costmap / inflation_layer |
| 贴墙走 | footprint 太小 / inflation_radius 太小 |
| 突然停住 | local_costmap 误识别障碍物 |
| 原地转圈 | 局部障碍物堵死 / controller 无可行速度 |
| 障碍物消失后仍然存在 | clearing 配置异常 |
| 机器人把自己当障碍物 | 车体点云未过滤 / footprint 配置错误 |
| RViz 里 costmap 一片红 | obstacle_layer 或 inflation_layer 参数异常 |
4. 为什么 3D 雷达接 Nav2 更容易出问题？
2D 雷达比较简单。它天然输出一圈平面扫描数据，Nav2 的 obstacle_layer 使用起来比较直接。
但 3D 雷达不一样。
3D 雷达看到的是三维空间：
地面点；
墙面点；
桌面点；
椅子腿；
机器人车体；
天花板；
斜坡；
玻璃；
动态行人；
雷达近距离盲区；
反射噪声点。
如果你直接把这些点投影到 2D costmap，很容易出现误判。
例如：
1. 地面点被当成障碍物；
2. 机器人车体边缘被雷达扫到，然后被当成障碍物；
3. 高处横梁被投影到地面，导致机器人以为前方过不去；
4. 桌面点被当成低矮障碍物；
5. 人走开后，障碍物没有被清除；
6. 点云时间戳滞后，导致 costmap 报 TF extrapolation。
所以 3D 雷达接 Nav2，不是简单地把 /points 接进去就完事。中间至少要考虑：
PointCloud2 → 高度过滤 → LaserScan / obstacle source → costmap
5. 典型系统结构
如果你的系统是 FAST-LIO / Point-LIO + Nav2，常见结构可以设计成这样：
flowchart LR
A[LiDAR] --> B[FAST-LIO / Point-LIO]
C[IMU] --> B
B --> D[/odom]
B --> E[odom → base_footprint TF]
A --> F[/points_raw]
F --> G[点云裁剪]
G --> H[PointCloud2 to LaserScan]
H --> I[/scan]
I --> J[Nav2 obstacle_layer]
J --> K[local_costmap]
K --> L[controller_server]
L --> M[/cmd_vel]
N[map_server / SLAM map] --> O[global_costmap]
O --> P[planner_server]
P --> L
这里要注意一个核心点：
LIO 主要负责定位和里程计；
LaserScan / PointCloud2 主要负责避障；
Nav2 costmap 负责把障碍物变成可规划、可控制的代价地图。
很多导航效果差的问题，其实不是 LIO 漂了，而是避障数据进 costmap 的方式不对。
6. 第一个关键参数：robot_radius / footprint
Nav2 必须知道机器人本体有多大。
如果机器人是圆形底盘，可以用：
robot_radius: 0.28
如果机器人是矩形底盘，更推荐使用 footprint：
footprint: "[[0.35, 0.25], [0.35, -0.25], [-0.35, -0.25], [-0.35, 0.25]]"
含义是机器人外轮廓相对于 base_link 或 base_footprint 的坐标。
例如：
前方 0.35 m；
后方 0.35 m；
左侧 0.25 m；
右侧 0.25 m。
6.1 footprint 配小了会怎样？
机器人会贴墙；
局部规划器认为还能过；
实际车体可能已经擦到障碍物；
窄通道里容易碰撞。
6.2 footprint 配大了会怎样？
机器人会觉得哪里都过不去；
路径绕得很远；
狭窄区域无法通过；
local_costmap 中可通行区域变少。
6.3 实机建议
实机不要只看底盘外壳尺寸，最好把这些也考虑进去：
1. 轮子外沿；
2. 雷达支架；
3. 外露线缆；
4. 急停按钮；
5. 机械臂或相机支架；
6. 实际控制误差余量。
例如底盘真实宽度是 0.48 m，不要刚好写 0.24 m 半宽。可以适当给一点安全余量，比如 0.26 m 或 0.28 m。
7. 第二个关键参数：inflation_radius
inflation_layer 的作用是给障碍物周围加一圈安全区。
典型配置：
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
inflation_radius: 0.45
cost_scaling_factor: 3.0
简单理解：
inflation_radius 决定障碍物周围膨胀多远；
cost_scaling_factor 决定代价值衰减得快还是慢。
7.1 inflation_radius 太小
表现：
机器人贴墙；
绕障碍物距离太近；
底盘容易擦边；
在真实环境里看起来很危险。
7.2 inflation_radius 太大
表现：
机器人绕路；
窄通道过不去；
global path 离障碍物很远；
局部可行区域变得很小。
7.3 cost_scaling_factor 的影响
cost_scaling_factor 越大，代价值衰减越快。
可以粗略理解为：
cost_scaling_factor 大：
障碍物附近很危险，但稍远一点代价迅速降低。
cost_scaling_factor 小：
障碍物影响范围更平滑、更宽。
一般实机调参不要孤立调 inflation_radius。它和 footprint 是一起影响机器人安全距离的。
建议顺序：
1. 先把 footprint / robot_radius 配准；
2. 再调 inflation_radius；
3. 最后微调 cost_scaling_factor。
8. 第三个关键参数：obstacle_max_range 和 raytrace_max_range
在 obstacle_layer 里，两个参数很关键：
obstacle_max_range: 2.5
raytrace_max_range: 3.0
它们经常被混淆。
8.1 obstacle_max_range
表示多远以内的障碍物会被标记进 costmap。
雷达看到一个障碍物；
距离小于 obstacle_max_range；
costmap 把它标记为障碍。
8.2 raytrace_max_range
表示多远以内可以做清障。
雷达某个方向没有看到障碍物；
系统认为这条射线方向是空的；
于是清除这条射线范围内原来残留的障碍物。
8.3 关键原则
一般建议：
raytrace_max_range >= obstacle_max_range
例如：
obstacle_max_range: 2.5
raytrace_max_range: 3.0
如果清障范围太短，就容易出现：
障碍物移走了；
但 costmap 里还残留；
机器人以为那里还是过不去；
表现为突然停住或绕远路。
这就是常说的“幽灵障碍物”。
9. 第四个关键问题：marking 和 clearing
典型 obstacle_layer 配置：
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: true
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 1.2
clearing: true
marking: true
data_type: "LaserScan"
obstacle_max_range: 2.5
raytrace_max_range: 3.0
这里最关键的是：
marking: true
clearing: true
含义：
marking：把看到的障碍物标记到 costmap；
clearing：把已经消失的障碍物从 costmap 清掉。
如果只有 marking，没有 clearing，机器人会越来越“胆小”。
因为它会不断把看到过的东西加进去，但不会清除已经不存在的东西。
表现就是：
一开始还能走；
走着走着 costmap 越来越脏；
最后机器人觉得周围全是障碍物；
然后停住或原地转圈。
10. 第五个关键问题：3D 点云高度过滤
如果你用 3D 雷达生成 /scan，高度过滤非常关键。
例如：
min_height: 0.05
max_height: 1.20
这个参数通常出现在点云转 LaserScan 节点里，例如 pointcloud_to_laserscan。
10.1 min_height 太低
可能导致：
地面点被保留下来；
地面起伏被当成障碍物；
车体下方反射点被投影进 scan；
local_costmap 出现大量噪声。
10.2 min_height 太高
可能导致：
低矮障碍物被过滤掉；
机器人看不到地上的箱子、台阶、低矮物体；
避障不安全。
10.3 max_height 太低
可能导致：
较高障碍物被过滤；
桌腿、栏杆、人体部分点云不完整；
障碍物检测不稳定。
10.4 max_height 太高
可能导致：
桌面、门框、横梁、墙上结构被投影成地面障碍；
机器人以为前面过不去；
路径绕远。
10.5 实机建议
高度范围不要照抄别人的配置。它要结合：
1. 雷达安装高度；
2. 雷达俯仰角；
3. 底盘高度；
4. 需要避开的障碍物高度；
5. 环境里是否有桌子、椅子、门槛、斜坡；
6. 是否需要过滤车体自身点云。
比如你的雷达安装在 0.45 m 高度，机器人只做地面导航，可以先尝试：
min_height: -0.10
max_height: 0.80
如果地面噪声太多，可以提高 min_height：
min_height: 0.03
max_height: 0.80
如果桌面被错误投影，可以降低 max_height。
11. 机器人为什么会把自己当成障碍物？
这是 3D 雷达实机导航里很常见的问题。原因通常有几类。
11.1 雷达扫到了车体
例如：
雷达安装位置太低；
雷达视野里能看到机器人外壳；
能看到轮子；
能看到支架；
能看到线束。
这些点如果没有过滤，就会进入 costmap。
结果就是：
机器人前方或侧方一直有障碍物；
controller 认为不能走；
机器人停住或原地转圈。
11.2 footprint 没有覆盖车体
如果 footprint 比真实车体小，costmap 可能无法正确理解哪些区域属于机器人自身。
表现：
机器人边缘点被当成外部障碍物；
贴墙时尤其明显；
local_costmap 里车体周围出现异常障碍点。
11.3 点云裁剪范围不对
可以在点云预处理阶段直接裁剪掉机器人本体附近的点。
例如逻辑上做一个 box filter：
如果点落在机器人底盘范围内，就丢弃。
伪代码：
bool isInsideRobotBody(float x, float y, float z)
{
return x > -0.40 && x < 0.40 &&
y > -0.30 && y < 0.30 &&
z > -0.20 && z < 0.50;
}
如果点在机器人自身包围盒内，就不送进 costmap。
12. RViz 里应该看什么？
调 Nav2 costmap，不能只看机器人模型和路径。
建议打开这些显示项：
1. TF；
2. RobotModel；
3. LaserScan；
4. PointCloud2；
5. Global Costmap；
6. Local Costmap；
7. Global Path；
8. Local Plan；
9. Footprint。
重点看：
/scan 是否和真实障碍物一致；
local_costmap 是否有奇怪噪声；
障碍物移走后 costmap 是否能清除；
footprint 是否覆盖真实车体；
inflation 是否过大或过小；
global path 是否被膨胀层挤到很远；
local plan 是否频繁震荡。
如果 RViz 里 /scan 本身就是错的，就不要继续调 Nav2 参数。应该先回到点云过滤或 LaserScan 生成节点。
13. 推荐排查命令
13.1 查看话题
ros2 topic list
重点确认：
/scan
/odom
/tf
/tf_static
/cmd_vel
/local_costmap/costmap
/global_costmap/costmap
13.2 查看 scan 是否正常
ros2 topic echo /scan --once
重点看：
header.frame_id 是否是雷达坐标系；
range_min / range_max 是否合理；
ranges 是否大量 inf 或 0；
时间戳是否正常更新。
13.3 查看频率
ros2 topic hz /scan
ros2 topic hz /local_costmap/costmap
ros2 topic hz /cmd_vel
如果 /scan 频率太低，局部避障会变迟钝。
13.4 查看 TF
ros2 run tf2_ros tf2_echo base_footprint lidar_frame
或者：
ros2 run tf2_ros tf2_echo map base_footprint
如果 costmap 报 TF 错误，先查 TF，不要调 costmap。
13.5 查看 Nav2 lifecycle 状态
ros2 lifecycle nodes
ros2 lifecycle get /controller_server
ros2 lifecycle get /planner_server
ros2 lifecycle get /bt_navigator
确保关键节点是：
active
14. 一个推荐的调参顺序
我个人更建议按下面顺序调：
flowchart TD
A[Nav2 效果异常] --> B{TF 是否正常?}
B -->|否| C[先修 TF]
B -->|是| D{scan / pointcloud 是否正常?}
D -->|否| E[先修传感器数据]
D -->|是| F[检查高度过滤]
F --> G[检查 footprint / robot_radius]
G --> H[检查 obstacle_layer]
H --> I[检查 clearing / marking]
I --> J[检查 inflation_layer]
J --> K[观察 local_costmap]
K --> L[再调 controller 参数]
不要反过来。
错误顺序通常是：
机器人不走 → 调 DWB 参数 → 还是不走 → 继续调速度 → 还是不走
正确顺序应该是：
机器人不走 → 看 local_costmap → 看 /scan → 看 TF → 看 obstacle_layer → 再看 controller
15. 一个基础配置示例
下面给一个简化版配置，仅用于说明参数关系。
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0
publish_frequency: 5.0
global_frame: odom
robot_base_frame: base_footprint
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05
footprint: "[[0.35, 0.25], [0.35, -0.25], [-0.35, -0.25], [-0.35, 0.25]]"
plugins: ["obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: true
observation_sources: scan
scan:
topic: /scan
data_type: "LaserScan"
clearing: true
marking: true
obstacle_max_range: 2.5
raytrace_max_range: 3.0
max_obstacle_height: 1.2
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
enabled: true
inflation_radius: 0.45
cost_scaling_factor: 3.0
这里有几个重点：
1. local_costmap 的 global_frame 通常用 odom；
2. robot_base_frame 要和 TF 树一致；
3. footprint 要接近真实底盘尺寸；
4. obstacle_layer 必须正确订阅 /scan；
5. clearing 和 marking 都建议打开；
6. inflation_radius 不要盲目设太大。
16. 常见现象和对应排查
| 现象 | 优先检查 |
|---|---|
| 机器人贴墙 | footprint 是否太小，inflation_radius 是否太小 |
| 机器人绕很远 | inflation_radius 是否太大，global_costmap 是否膨胀过度 |
| 机器人突然停住 | local_costmap 是否出现误障碍 |
| 原地转圈 | local_costmap 是否堵死，controller 是否找不到可行速度 |
| 障碍物移走后还在 | clearing / raytrace_max_range |
| costmap 一片红 | obstacle_layer 输入异常，高度过滤异常 |
| 看得到路径但不走 | local_costmap、controller_server、/cmd_vel |
| scan 正常但 costmap 没障碍 | observation_sources、frame_id、QoS、TF |
| scan 里全是噪声 | 点云转 LaserScan 参数、高度过滤、雷达外参 |
| 机器人把自己当障碍物 | 车体点云裁剪、雷达安装位置、footprint |
17. 一个实机排查案例
假设机器人表现是：
Nav2 能启动；
路径能规划；
机器人一靠近墙就开始抖动；
有时原地转圈；
local_costmap 里机器人侧边一直有障碍物。
我会按下面方式排查。
17.1 第一步：看 /scan
ros2 topic echo /scan --once
确认 frame：
header.frame_id: lidar_frame
然后在 RViz 打开 LaserScan，看扫描点是否落在机器人车体上。
如果雷达能扫到车体侧边，就先不要调 Nav2。应该先裁剪车体点云。
17.2 第二步：看 footprint
如果底盘实际尺寸是：
长 0.70 m；
宽 0.50 m。
那么 footprint 至少应该接近：
footprint: "[[0.35, 0.25], [0.35, -0.25], [-0.35, -0.25], [-0.35, 0.25]]"
如果你只配置：
robot_radius: 0.20
那机器人当然容易贴墙。
17.3 第三步：看 inflation
如果机器人还是贴墙，可以适当增大：
inflation_radius: 0.50
如果增大后机器人过不了窄门，说明膨胀过大，需要回退。
17.4 第四步：看 clearing
如果人从机器人前面走过，走开之后障碍物还留在 costmap 里，检查：
clearing: true
raytrace_max_range: 3.0
并确认 /scan 里对应方向确实有空闲射线。
18. 不建议的调参方式
18.1 不看 RViz，只改 yaml
这是最容易浪费时间的方式。
costmap 问题必须可视化。你至少要看：
/scan；
local_costmap；
global_costmap；
footprint；
local plan。
18.2 不管 footprint，直接调 controller
如果机器人尺寸都不对，controller 调参没有意义。
18.3 直接复制别人的参数
Nav2 参数和机器人强相关：
底盘尺寸；
最大速度；
最小转弯半径；
雷达安装高度；
雷达视场角；
环境宽度；
障碍物类型；
地面平整程度。
别人的参数只能作为初始参考，不能直接当最终配置。
18.4 只看 global path
global path 正常，不代表机器人能正常走。局部避障主要看 local_costmap 和 controller。
19. 我的建议参数范围
下面不是标准答案，只是实机初调时可以参考的范围。
| 参数 | 建议初始范围 |
|---|---|
| resolution | 0.03 ~ 0.05 m |
| local_costmap width | 3.0 ~ 6.0 m |
| local_costmap height | 3.0 ~ 6.0 m |
| inflation_radius | 0.35 ~ 0.70 m |
| cost_scaling_factor | 2.0 ~ 5.0 |
| obstacle_max_range | 2.0 ~ 4.0 m |
| raytrace_max_range | obstacle_max_range + 0.3 ~ 1.0 m |
| update_frequency | 5 ~ 15 Hz |
| publish_frequency | 2 ~ 10 Hz |
如果是小车，可以从下面开始：
resolution: 0.05
width: 4.0
height: 4.0
inflation_radius: 0.45
cost_scaling_factor: 3.0
obstacle_max_range: 2.5
raytrace_max_range: 3.0
然后根据实际表现调整。
20. 总结
3D 雷达接 Nav2 后，机器人能跑起来只是第一步。真正难的是让它在实机环境里稳定、自然、安全地跑。
这篇文章的核心结论：
1. 机器人绕路、贴墙、停住，不一定是 controller 的问题；
2. Nav2 的导航效果高度依赖 costmap 质量；
3. 3D 雷达数据进入 costmap 前必须做好高度过滤和车体点云裁剪；
4. footprint / robot_radius 决定机器人对自身尺寸的理解；
5. inflation_layer 决定机器人离障碍物多远；
6. marking 负责添加障碍物，clearing 负责清除障碍物；
7. 调参顺序应该是 TF → scan → costmap → footprint → inflation → controller。
最后给一个实机排查 checklist：
[ ] TF 树是否完整？
[ ] /scan 是否正常？
[ ] /scan 的 frame_id 是否能通过 TF 连到 base_footprint？
[ ] local_costmap 是否能看到真实障碍物？
[ ] 障碍物移走后是否能清除？
[ ] footprint 是否符合真实底盘尺寸？
[ ] inflation_radius 是否过大或过小？
[ ] 机器人是否把自身点云当成障碍物？
[ ] controller_server 是否 active？
[ ] /cmd_vel 是否正常输出？
一句话总结：
Nav2 实机导航不要一上来就调控制器。
先把 costmap 看明白，很多问题自然就清楚了。
更多推荐
5
0- 0
已为社区贡献1条内容
所有评论(0)