ROS 激光雷达建图导航实战

ROS
7.5k 词

从”照着配置跑通 move_base demo”升级为”理解建图/定位/规划/代价地图每层的数学原理，知道参数为什么这么设、导航失败往哪查”。激光雷达数据模型、gmapping 与 cartographer 的取舍、AMCL 粒子滤波、代价地图三层结构与衰减公式、A*/DWA/TEB 的算法本质、move_base 状态机与恢复行为——最后给出可量化的调参顺序与评估指标。

一、激光雷达数据：LaserScan 的几何与噪声模型

LaserScan 的结构

1
2
3
4
5
6
7
8
# 典型 2D 雷达（如 rplidar A1）：360°、每圈 720 点
angle_min: -3.14159     # 起始角（rad）
angle_max: 3.14159      # 结束角
angle_increment: 0.00873  # 0.5°/点 = 2π/720
range_min: 0.12
range_max: 12.0
ranges: [ ... 720 个浮点数 ... ]   # 单位米，无效点 = inf / NaN
intensities: [ ... ]              # 反射强度（可用作地面/障碍区分特征）

几何要点：第 i 个点的角度是 angle_min + i * angle_increment，距离是 ranges[i]，则该点在雷达系下的坐标：

$$x_i = r_i \cos(\theta_i), \quad y_i = r_i \sin(\theta_i)$$

运动畸变（关键坑）：激光是逐点采样的——一帧 720 点不是同一时刻测的，而是雷达旋转一圈（100ms）内连续测的。车在转圈/加速时，一帧内车已经移动了几厘米到几十厘米，直接按”所有点都在帧起始时刻”转换，点云就扭曲了。影响：

  • 建图/定位对运动畸变敏感（gmapping 默认不处理，靠里程计好 + 车速慢来兜底）
  • cartographer 在 ScanMatcher 里显式做畸变补偿（用 IMU/里程计把每个点外推到统一时刻）——这是它精度高于 gmapping 的原因之一
  • 工程对策：建图时控制车速（< 0.5 m/s）、减少急转弯；导航时把 scan 时间戳与里程计对齐

噪声模型

雷达测距噪声近似高斯 + 少量离群点（镜面、玻璃、吸光材料返回 inf/NaN）：

  • 无效点处理：ranges[i] 为 inf/NaN 时，代价地图该方向不产生障碍也不清除——别把 NaN 当 0（0 会被当成”障碍就在脸上”，代价地图直接红一圈，导航原地卡死）
  • 角度分辨率：720 点/圈在 5m 处相邻点间距 ≈ $5 \times 2\pi/720 \approx 4.4$cm——远距离小障碍（桌腿）可能正好落在两条射线之间被漏掉，这是 2D 雷达的物理极限，膨胀半径设大一点就是在补偿它

二、建图：gmapping 还是 cartographer

gmapping：RBPF 粒子滤波

gmapping 的核心是带权重的粒子群：每个粒子携带一条机器人轨迹假设 + 对应的栅格地图，里程计预测粒子分布、激光匹配更新权重、重采样淘汰低权重粒子。三个工程结论：

  1. gmapping 的好坏上限 = 里程计的好坏。粒子滤波的提议分布依赖里程计，里程计漂了地图就糊。麦轮底盘打滑（见运动学篇）建图必糊——建图前先确认里程计（推车走直线，rostopic echo /odom 看位置是否线性增长）
  2. 粒子数 = 精度 vs 算力。默认 30 个粒子，长廊/对称环境加到 80~100 才不飘；但粒子越多 CPU 越满（建图时雷达帧率会掉）。调法：地图出现”重影/错层”就加倍粒子数
  3. 退化场景：长廊（激光前后对称，匹配多解）、空旷场地（无特征，粒子权重全平）——gmapping 在这两类场景必翻车，不是参数问题，是算法本质

cartographer：子图 + 图优化 + 回环

cartographer 用局部子图（submap）+ 全局图优化：前端把连续帧匹配进当前子图（scan-to-submap，带畸变补偿），后端在检测到回环时把子图之间的位姿约束丢进稀疏图优化（数学本质见《SLAM 与视觉基础》篇的图优化部分）一次性拉平漂移。为什么大场景选它：

  • 误差不是纯累积的：子图内误差被持续优化，回环闭合后全局误差被拉回
  • 长廊也能建（图优化 + 子图间约束比纯粒子滤波稳），但对称场景回环仍会误闭合——需要 --config 里调小回环阈值或建图时避免走重复对称路径

建图质量怎么评估

不看”图漂不漂亮”，看三个量化指标：

  1. 回环闭合：绕一圈回到起点，地图上起点位置的特征（墙角、门框）与轨迹起点重合，偏差 < 10cm 合格
  2. 对称度：房间的墙应该是直的——把地图导到图片里看墙角是不是 90°、门框宽度和实测是否一致
  3. 重影检查：同一面墙在图上出现两条平行线 = 轨迹漂移（粒子/回环没兜住）
1
2
3
# 建图命令速查（gmapping）
roslaunch my_pkg gmapping.launch
rosrun map_server map_saver -f ~/maps/office   # 保存 map.pgm + map.yaml

保存的 map.yaml 里 resolution: 0.05（5cm/像素）、origin 是地图左下角在世界系的位置——origin 错了导航会整体偏移，加载后先在 RViz 里对比激光与地图是否对齐再放机器人。

三、定位：AMCL 粒子滤波

AMCL（自适应蒙特卡洛定位）与 gmapping 同源——粒子滤波，但地图已知，只估计位姿：

  1. 初始：粒子在初始位姿附近撒开（或全图均匀撒——initial_pose 不给就是全局定位）
  2. 预测：按里程计运动模型移动粒子
  3. 更新：每个粒子按”该位姿下激光与地图的匹配程度”加权
  4. 重采样：低权重粒子被高权重粒子替换，粒子群收敛到真实位姿

工程要点：

  • initial_pose 必须给（RViz 2D Pose Estimate 点一下）：全局定位要几秒到几十秒收敛，比赛/产线等不起；给了初始位姿秒级收敛
  • **kidnapped robot problem**：机器人被搬走（被抬起来换地方），粒子群还在原地——AMCL 的”自适应”体现在粒子数会根据定位质量自动增减，但搬走这种极端情况要手动重新初始化（RViz 重新给初始位姿）
  • 参数三件套：min_particles/max_particles（默认 100/5000，算力够就调大）、update_min_d/update_min_a（里程计走多远才做一次激光匹配，调大省 CPU 但响应慢）、laser_max_range（超过该距离的激光点丢弃——空旷场地用它避免”远处墙的匹配噪声”）
1
2
3
4
5
6
7
amcl:
  min_particles: 500
  max_particles: 3000
  update_min_d: 0.2
  update_min_a: 0.2
  laser_max_range: 8.0
  transform_tolerance: 0.3

定位质量的量化检查：RViz 里开 ParticleCloud，粒子云应收敛成一团贴在地图上；导航时粒子云若发散成几团 = 定位丢失，先别导航，查地图/里程计。

四、代价地图：三层结构与代价衰减公式

三层各司其职

层 数据来源 作用
static_layer map_server 静态地图 已知障碍（墙）
obstacle_layer 实时传感器（LaserScan） 动态障碍 + 清障（raytrace）
inflation_layer 前两层结果 把障碍膨胀出”禁区”（cost 梯度）

marking 与 clearing 的语义（obstacle_layer 的灵魂）：

  • marking: true：激光打到的点（obstacle_range 内）标记为障碍
  • clearing: true：激光穿过的区域（从雷达到障碍点之间，raytrace_range 内）清除为自由——这保证移动障碍消失后，代价地图能自动把残留的障碍擦掉
  • **必须 raytrace_range > obstacle_range**：只标记不清除，车走了障碍”影子”留在图上，导航会绕不存在的墙。典型配置：obstacle 2.5m、raytrace 3.0m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
# costmap_common_params.yaml
footprint: [[-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125]]
transform_tolerance: 0.2

obstacle_layer:
  enabled: true
  obstacle_range: 2.5
  raytrace_range: 3.0
  observation_sources: laser_scan_sensor
  laser_scan_sensor:
    data_type: LaserScan
    topic: /scan
    marking: true
    clearing: true

inflation_layer:
  enabled: true
  inflation_radius: 0.55
  cost_scaling_factor: 2.58

膨胀代价的衰减公式（inflation 的数学）

inflation 层对每个障碍栅格周围施加代价，随距离指数衰减：

$$\text{cost}(d) = 252 \cdot e^{-\alpha (d - d_{inscribed})}, \quad \alpha = \frac{\text{cost_scaling_factor}}{\text{inflation_radius} - d_{inscribed}}$$

  • 障碍栅格本身代价 254（致命 lethal）；footprint 内切圆半径（inscribed）以内代价 253（内切碰撞）；外切圆半径（circumscribed）以外代价 < 253，规划器可以”擦边过”
  • cost_scaling_factor 越小衰减越慢：2.58 是”温和推开”（路径平滑绕行），5.0 是”贴墙走”（窄通道能过但风险高），0.5 是”极远就躲”（空旷场景安全，窄门过不去）
  • inflation_radius 必须 ≥ 机器人内切半径，否则规划的路径会要求机器人穿过自己过不去的缝（贴着障碍走，实际会撞）。底盘 0.5m 半宽 → 膨胀半径至少 0.55~0.6

全局 vs 局部代价地图

global_costmap local_costmap
坐标系 map odom
更新 静态地图为主，导航前构建 滚动窗口，随机器人移动
数据 静态层 + 障碍 + 膨胀 障碍 + 膨胀（无静态层）
用途 全局路径规划 局部避障 + 速度约束

rolling_window: true + 小尺寸（5m×5m）让局部地图始终以机器人为中心，代价是窗口外无信息——所以局部规划器只负责”眼前几米”，全局路径由 global planner 负责。局部代价地图分辨率 0.05m 时 5m 窗口 = 100×100 栅格，20Hz 更新完全跑得动。

五、路径规划：A*、DWA、TEB 的算法本质

A*：启发函数必须 admissible

A* 在 Dijkstra 基础上加启发项：f(n) = g(n) + h(n)，其中 h(n) 是 n 到目标的估计代价。最优性条件：h(n) 不能高估真实代价（admissible）——2D 栅格里欧氏距离永远 ≤ 实际路径长度，所以 A* 保证最优；曼哈顿距离在允许斜走的 8 邻域里会高估，可能给出次优路径。工程结论：global_planner 用 AStar 时保持默认欧氏启发，别手改。

1
2
3
4
5
# global_planner 参数
GlobalPlanner:
  use_dijkstra: false     # true=Dijkstra false=A*
  use_quadratic: true     # 二次插值，路径更平滑（少折线）
  default_tolerance: 0.2  # 目标点被障碍占据时，允许停在附近多远

default_tolerance 是高频坑：目标点设在地图障碍上（或贴墙），A* 找不到路径直接失败报 “Failed to get a plan”——给 0.2~0.5m 容差让它停到目标附近可达点。

DWA：速度空间采样 + 轨迹评价

DWA（动态窗口法）的核心：在机器人当前可达的速度空间里采样（考虑加速度极限的动态窗口），对每个采样速度预测一段轨迹（运动学前推），用评价函数打分，选最高分执行：

$$J = \alpha \cdot \text{heading}(v,\omega) + \beta \cdot \text{dist}(v,\omega) + \gamma \cdot \text{vel}(v,\omega)$$

  • heading：轨迹末端朝向与目标方向的夹角（越小越好）
  • dist：轨迹上离最近障碍的距离（越远越好，撞障碍的直接淘汰）
  • vel：速度大小（鼓励快跑）
  • 三个权重就是行为风格：α 大 → 直奔目标（可能绕不过障碍）、β 大 → 保守绕行（可能绕远路）、γ 大 → 飙车
1
2
3
4
5
6
7
8
9
10
# base_local_planner (DWA)
DWAPlannerROS:
  max_vel_x: 0.5
  min_vel_x: -0.1
  max_vel_trans: 0.5
  max_vel_theta: 1.5
  acc_lim_x: 2.0        # 加速度极限：决定动态窗口大小
  acc_lim_theta: 3.0
  sim_time: 1.7         # 轨迹预测时长
  goal_tolerance: 0.2

运动学约束在这里体现：acc_lim 限制加速度（防打滑，呼应麦轮篇的 S 曲线）、max_vel_theta 限制转弯速率——这些值必须 ≤ 底盘实际能力，否则 DWA 预测的轨迹底盘执行不了，表现是”规划器一直重新规划、车走走停停”。

TEB：时间弹性带

TEB（Timed Elastic Band）把路径看成一条带时间戳的弹性带，优化目标是最小化”到目标时间”同时满足约束（避障、运动学、动力学）：

  • 相比 DWA 只规划 1.7 秒，TEB 规划整条局部路径，提前看到弯道减速，窄道通行能力更强
  • 参数核心：min_obstacle_dist（离障碍最近距离）、weight_obstacle（避障权重）、dt_ref（相邻位姿时间间隔，调小 = 轨迹更精细但更费算力）
  • 坑：TEB 参数多且相互耦合，调不好比 DWA 更容易”原地抖动”——先用 DWA 跑通，再用 TEB 优化，别一上来就 TEB

六、move_base 状态机与恢复行为

move_base 内部是一个状态机：全局规划 → 局部规划执行 → 失败 → 恢复行为 → 重试。三个默认恢复行为（recovery behaviors）：

  1. conservative_reset：清除代价地图上距离机器人较远的障碍（可能是传感器误报/动态障碍残留）
  2. rotate_recovery：原地旋转 360° 重新扫描，刷新局部代价地图（车在窄道里会原地转圈，就是这个）
  3. aggressive_reset：清除整个代价地图上的障碍层重新来过
1
2
3
4
5
6
7
8
# 三个恢复行为全关 = 失败直接放弃，别这么干
recovery_behaviors:
  - name: conservative_reset
    type: clear_costmap_recovery/ClearCostmapRecovery
  - name: rotate_recovery
    type: rotate_recovery/RotateRecovery
  - name: aggressive_reset
    type: clear_costmap_recovery/ClearCostmapRecovery

导航失败的标准排查路径（按此顺序，90% 的问题在前两层）：

1
2
3
4
5
6
1. TF 树对吗？        → view_frames：odom→base_link→laser 是否连续更新
2. 激光与地图对齐吗？  → RViz 对比 scan 与 map，错位 = 定位/外参问题
3. 代价地图合理吗？    → 障碍是不是多了一圈/少了一圈（inflation/marking 配置）
4. 全局路径出来了吗？  → 没有 = global planner 失败（目标被占/容差太小）
5. 局部路径在动吗？    → 不动 = DWA 采样全被障碍淘汰（β 权重/膨胀问题）
6. 执行了吗？         → cmd_vel 在发但车不动 = 底盘/电机问题（与导航无关！）

最后一条最坑：导航栈一切正常、/cmd_vel 频率正确，车就是不动——查底盘。导航调试一半的时间花在”导航没问题，是执行层的问题”上。

七、调参实战：顺序与量化评估

调参顺序（每步验证再进下一步）

  1. TF：树结构 + 更新频率（10Hz 以上）
  2. 静态地图：加载后激光与地图偏差 < 5cm
  3. AMCL 定位：粒子收敛、定位误差 < 10cm（推车转一圈看粒子云）
  4. 膨胀层：膨胀半径 ≥ 内切半径，窄道能过、贴墙不撞
  5. 全局规划：路径合理（不穿墙、不绕远、终点可达）
  6. 局部规划：跟踪平滑、不抖动、窄道不卡
  7. 速度限制：加减速匹配底盘能力，不打滑

量化评估指标

指标 定义 合格线
路径长度比 实际行驶距离 / 最短可行距离 < 1.3
到达时间 起点到目标耗时 与速度上限的理论下限比 < 2x
绕路率 明显绕行（如绕 3 倍远）的次数 0
成功率 同一任务重复 10 次成功次数 ≥ 9/10
贴墙距离 行驶中与障碍最近距离 > 膨胀半径的 80%

调参要有对照组：改一个参数跑同一组任务（固定起点终点），记录上面 5 个指标再对比。**”感觉顺畅了”不算数**——这是把导航从”能跑”做到”可靠”的唯一路径，与 PID 篇的 MAE/ITAE 量化评估是同一个方法论。

踩坑清单

  1. NaN/inf 当有效距离：ranges[i] 无效值会导致代价地图”脸上有墙”，先过滤再进 costmap
  2. 运动畸变不管：建图时快开快转，地图糊；控制车速 + 用带畸变补偿的 cartographer
  3. raytrace_range ≤ obstacle_range：障碍影子残留，导航绕不存在的墙；永远 raytrace > obstacle
  4. 膨胀半径小于内切半径：规划路径要求机器人钻自己过不去的缝
  5. 目标点在地图上（default_tolerance=0）：A* 无解报失败；给 0.2~0.5m 容差
  6. acc_lim 超过底盘能力：规划器预测轨迹执行不了，走走停停
  7. 导航不动先查底盘：cmd_vel 正常车不动 = 执行层问题，别在导航参数里瞎调
  8. 里程计差就建图：麦轮打滑/里程计漂移，gmapping 必糊；先修里程计
  9. AMCL 不给初始位姿：全局定位几十秒还可能收敛到错误位置；RViz 手动初始化
  10. 调参无对照组：改参数不记录指标 = 玄学调参；用路径长度比/成功率量化