Part 4: Algorithms · The robot's brain
Chapter 10
Nav2 - 机器人的“高德地图 + 自动驾驶”
你打开手机高德地图，输入目的地，它做了三件事：第一，在全城地图上给你画了一条从 A 到 B 的推荐路线；第二，在你开车的过程中实时微调 - “前方 200 米右转”、“重新规划路线”；第三，当你走错路或遇到封路时，它有一套应急策略 - 掉头、绕行、等待。
Nav2 就是机器人版的高德地图 + 自动驾驶。它假设你已经有了一张地图（SLAM 建的），也知道自己在地图上的位置（定位模块给的），然后解决那个看起来简单但实际上极其复杂的问题：怎么从 A 点走到 B 点，不撞东西。
为什么说“看起来简单”？因为你自己走路从来不觉得这是个问题 - 你眼睛看到障碍物，大脑自动绕开，脚自动调整步伐。但把这件事交给一台只有激光雷达和轮子的机器人，你会发现“不撞东西地走到目标”需要同时解决路径搜索、实时避障、运动控制、异常恢复这四个互相耦合的问题。Nav2 是 ROS 2 生态里解决这个问题的标准答案。
Nav2 的三层架构
Nav2 把导航拆成了三层，每层干一件事，互不干扰又紧密配合。这个拆法本身就是它最精妙的设计。
全局规划器（Planner Server） - 在地图上画路线
全局规划器的工作最像高德地图的路线推荐：拿到起点和终点，在已知地图上搜出一条最优路径。它不关心实时发生的事情（比如前面突然出现一个人），它只看静态地图。
Nav2 里最常用的两个全局规划器：
NavFn 是经典选择，底层用的是 Dijkstra 或 A* 算法。如果你写过 LeetCode 的最短路径题，你已经懂了它的核心原理。NavFn 把地图看成一个网格，从起点开始向外“扩散”，每扩到一个格子就算一下到起点的代价，直到扩散到终点，然后回溯出一条最低代价路径。它快、稳、好调试，大部分场景下够用。
SMAC Planner 是更现代的选择。NavFn 有个隐含假设 - 机器人是个圆形的点，可以原地转向。但真实世界里很多机器人不是圆的（比如长方形的送货机器人），也不能原地转（比如阿克曼转向的车型底盘）。SMAC Planner 支持 Hybrid-A*，能在规划路径时就考虑机器人的运动学约束 - 它规划出来的路径，机器人真的能走，而不是规划了一个需要原地旋转 90 度的弯但机器人根本转不了。
选择建议：差动驱动（两轮）的圆形机器人用 NavFn 就行，非圆形或者阿克曼转向的用 SMAC。
局部控制器（Controller Server） - 实时跟车导航
全局规划器给了一条“理想路径”，但机器人在实际执行时需要持续微调：路径上突然出现了一个椅子怎么办？当前速度太快拐不过来怎么办？局部控制器就是那个坐在副驾驶位上不断喊“方向盘左打一点、减速、现在加速”的人。
它的输入是全局路径 + 实时传感器数据，输出是具体的速度指令（线速度 + 角速度），直接发给电机。
三个主流选择：
DWB（Dynamic Window Based） 是经典局部控制器。原理是：在当前速度附近采样一堆可能的速度组合，对每个组合模拟未来一小段时间的轨迹，然后用打分函数评估哪条轨迹最好（离路径近、离障碍物远、速度合适），选最高分的执行。它稳定、可预测，但调参数比较痛苦 - 打分函数有十几个权重，而且这些权重之间有复杂的交互效应。
MPPI（Model Predictive Path Integral） 是目前社区最推荐的控制器。它的思路和 DWB 类似 - 也是采样大量轨迹然后评估 - 但采样量大得多（通常上千条），而且跑在 GPU 上所以速度不是问题。更关键的是，MPPI 的评估函数更灵活，能很自然地处理“远离障碍物”和“跟踪路径”之间的平衡。我个人建议新项目直接上 MPPI，DWB 的时代基本过去了。
Regulated Pure Pursuit 是最简单的选择。它就是追着路径上前方一个点走 - 像驴追胡萝卜。没有复杂的优化，没有大量采样。适合宽敞环境下的简单场景，比如空旷仓库里的 AGV。在拥挤或复杂环境里就不够用了。
行为服务器（Behavior Server） - 卡住了怎么办
这是很多人忽略但极其重要的一层。机器人在真实环境里一定会卡住 - 被椅子腿困住、被墙角卡住、前方路径被彻底堵死。这时候全局规划器和局部控制器都帮不了你，因为它们的设计假设是“存在一条可行路径”。
行为服务器提供了几个“急救动作”：
- Spin - 原地旋转。机器人转一圈，让传感器重新扫描周围环境，有时候只是因为 costmap 里有“幽灵障碍”（传感器噪声留下的假障碍），转一圈就清掉了
- BackUp - 后退。往后退一段距离，给自己腾出空间重新规划
- Wait - 原地等待。前面有人挡路？等一会儿看他是不是走开了
这三个动作看起来简单，但它们的编排逻辑 - 先等一下，等不行就后退，后退不行就旋转，旋转不行就重新全局规划 - 决定了机器人在异常场景下的表现。后面会讲到，这个编排逻辑是用行为树实现的。
Costmap 详解 - Nav2 的“增强现实地图”
SLAM 建出来的地图是一张黑白灰的占用栅格图 - 黑色是墙，白色是空地，灰色是未知区域。但导航不能直接在这张地图上算，因为它缺少一个关键信息：危险程度。
一面墙旁边 1 厘米的位置和旁边 1 米的位置，在占用栅格图上都是“白色可通行”，但对机器人来说风险完全不同。Costmap（代价地图）就是在原始地图上叠加了一层“危险程度渐变”，把世界从非黑即白变成了连续的光谱。
每个格子的代价值从 0 到 254：0 表示完全安全，254 表示致命（撞上了），中间值表示“越靠近障碍物越危险”。路径规划在 costmap 上做，所以规划出来的路径会自然地远离墙壁和障碍物 - 因为靠近墙的格子“代价高”，规划器会避开。
Global Costmap vs Local Costmap
Nav2 维护两张 costmap，各有分工：
Global Costmap 覆盖整张地图。它基于 SLAM 建的静态地图生成，主要告诉全局规划器“哪些地方走不了”。更新频率低（因为静态地图本身不怎么变），计算量也可控。
Local Costmap 只覆盖机器人周围几米的范围。它基于实时传感器数据（LiDAR、深度相机）持续更新，是局部控制器做实时避障的依据。一个人走过来，local costmap 会立刻在那个位置标上高代价值；人走了，代价值又清掉。
两张 costmap 的分工很清晰：global 负责“大局观”（这条走廊能不能走），local 负责“近身反应”（前面 3 米有个人，绕开）。
Inflation Layer - 障碍物周围的“危险气泡”
Inflation Layer 是 costmap 里最重要也最容易调崩的一层。它做的事情很直观：围绕每个障碍物向外“膨胀”一圈代价值，形成一个渐变的危险区域。离障碍物越近代价越高，到了一定距离代价降为零。
两个关键参数：
cost_scaling_factor 控制代价衰减的速度 - 值越大，代价在离开障碍物后下降得越快，机器人愿意走得更靠近障碍物。
inflation_radius 控制膨胀的范围 - 这是每个 Nav2 开发者的噩梦参数。设太大，两面墙之间的走廊在 costmap 上就“合并”了，机器人觉得根本过不去（明明物理上过得去的走廊在 costmap 上变成了一片致命区域）。设太小，机器人贴着墙走，一个小的定位抖动就可能让它蹭到墙。
这个参数没有万能值。它取决于机器人的物理尺寸、定位精度、环境的通道宽度。你要做的是：量出环境里最窄的通道宽度，确保 inflation_radius 不大于 (通道宽度 - 机器人宽度) / 2，然后在这个上限附近找一个让机器人既能通过又不蹭墙的值。
定位（Localization） - 建完图之后，实时知道“我在哪”
SLAM 是边建图边定位 - 地图和位置同时从零开始算。但地图建好之后呢？机器人每次开机不需要重新建图，它需要的是拿着现有地图，实时推断“我在地图上的哪个位置”。这就是纯定位问题。
Nav2 默认使用的定位方案是 AMCL（Adaptive Monte Carlo Localization），基于粒子滤波。
粒子滤波的直觉是这样的：想象你把机器人放到地图上，但不告诉它在哪。AMCL 在地图上随机撒一把“粒子” - 每个粒子代表一个猜测的位置和朝向。然后机器人动了一步，传感器扫了一圈。AMCL 做两件事：
第一，根据运动指令，移动所有粒子（“我往前走了 1 米，所以每个粒子也往前 1 米，加一点随机噪声”）。
第二，对比每个粒子位置“应该看到的传感器数据”和“实际看到的传感器数据”。如果一个粒子站在墙前面，理论上前方 0.5 米就该有障碍物回波，但实际传感器说前方 3 米才有回波 - 这个粒子猜错了，权重降低。相反，如果一个粒子的理论读数和实际高度吻合，权重升高。
经过几轮运动和观测，大部分粒子死掉了（权重太低被淘汰），少数粒子聚集到了正确位置附近。这一簇聚集的粒子就是 AMCL 对机器人位置的最佳估计。
AMCL 的 “Adaptive” 是指粒子数量会自适应调整：定位不确定时多撒粒子（扩大搜索），定位收敛后减少粒子（省算力）。
一个常见的坑：机器人刚开机时，如果初始位置给得不对（比如你在 RViz 里手动设的初始位姿偏了），AMCL 可能收敛到错误位置。症状是机器人在 RViz 里的显示位置跟实际位置差了一两米，然后它就开始撞墙 - 因为它以为自己在走廊中间，实际上已经贴着墙了。解决方法：让机器人原地旋转一圈，给 AMCL 更多观测数据来纠正位置估计。
Behavior Tree Navigator - 用行为树编排一切
前面讲的全局规划器、局部控制器、行为服务器、costmap、定位 - 这些都是独立的模块。谁来决定“先干什么后干什么”？谁来决定“规划失败了该调用哪个恢复行为”？
Nav2 的答案是 Behavior Tree（行为树）。
Nav2 自带一棵默认的导航行为树，它编排了整个导航流程：收到目标 → 全局规划 → 局部控制跟踪 → 如果局部控制报错 → 清除 costmap → 重新全局规划 → 如果还是不行 → 调用 Spin → 如果还不行 → 调用 BackUp → 如果彻底不行 → 任务失败。
这棵树是一个 XML 文件，你可以用 Groot 可视化编辑器打开它、修改它。Nav2 的默认行为树对大部分场景够用，但在实际部署中，你几乎一定会需要改它 - 增加更多的恢复策略、调整重试次数、加入业务特有的逻辑（比如“如果电量低于 20% 就放弃任务去充电”）。
行为树的详细原理会在第 13 章展开，这里只需要知道一点：Nav2 的灵活性很大程度上来自于行为树的可编辑性。你不喜欢它的恢复策略？改 XML。你想在导航过程中加一步“到达目标后拍张照”？在行为树末尾加一个 Action 节点。
开发者踩坑指南
狭窄通道过不去
这是 Nav2 部署中排名第一的问题。症状是：物理上机器人明明能通过的通道，Nav2 就是不规划路径过去，或者规划了但执行到一半放弃。
原因几乎都是 inflation_radius 太大。两面墙的膨胀区域在中间重叠，costmap 上这段通道变成了一片高代价甚至致命区域。全局规划器一看 - 此路不通，绕行。
排查方法：在 RViz 里打开 costmap 的可视化。如果你看到通道中间是红色或紫色（高代价），而不是蓝色或无色（低代价），那就是 inflation 的问题。缩小 inflation_radius，或者增大 cost_scaling_factor 让代价衰减更快。
动态障碍物处理
一个人突然走到机器人前面。理想情况下，local costmap 应该立刻标记那个人的位置为障碍，局部控制器重新计算轨迹绕过去。但实际上常见的问题是：
- 人走了之后 costmap 没及时清除障碍标记 - 机器人还在绕一个“幽灵”。这通常是 costmap 的 observation_sources 配置问题，传感器数据的清除逻辑没配对。
- 人走得太快，从 LiDAR 盲区（比如太近或太高）穿过。这是传感器物理局限，解法是增加传感器覆盖（比如同时用 LiDAR + 深度相机）。
目标点不可达
你给了一个目标点坐标，但那个点恰好在 costmap 上是障碍区域 - 比如太靠近墙壁，或者在一张过时的地图上那个位置确实有障碍物但实际已经搬走了。
Nav2 会直接返回“规划失败”。解决思路有两个：一是在发送目标前先检查目标点在 costmap 上是否可达（Nav2 提供了相关的服务接口）；二是给目标点一个“容差范围”，让规划器寻找目标点附近最近的可达点。
恢复行为死循环
这是最让人抓狂的场景：机器人卡住了 → Spin → 还是卡住 → BackUp → 往后退了一点但还是出不去 → Spin → BackUp → Spin → BackUp... 无限循环。
根本原因是默认行为树没有“放弃”的逻辑 - 它会一直重试。在实际部署中你必须给恢复行为加上重试上限和超时。比如：尝试恢复 3 次，如果还是失败，就放弃当前任务，退回到上一个安全位置，然后通知上层系统“我需要人工帮助”。
另一个常见的变种是 costmap 清除后机器人以为路通了，冲上去又发现不通，清除，再冲，再不通。这时候需要检查的不是行为树逻辑，而是环境本身 - 也许那个位置真的过不去，地图需要更新了。
一个综合性的调参心得
Nav2 的参数非常多，新手容易陷入“这个参数是什么意思 → 改一下试试 → 好像更差了 → 改回来 → 改另一个”的随机游走。我的建议是按优先级调：
- 先确保 costmap 正确 - 打开 RViz 看 costmap 可视化，确认障碍物标记准确、膨胀合理
- 再调全局规划器 - 确认它能规划出合理路径
- 最后调局部控制器 - 确认跟踪路径时速度、加速度、转弯半径合理
- 恢复行为留到最后 - 先把正常情况跑顺，再处理异常
每次只改一个参数，观察效果，记录下来。Nav2 的参数之间有复杂的交互关系，同时改两个以上你就不知道哪个起了作用。