室内导航系统 09：Nav2 落地要把路线、路径、控制和安全分成四层
Nav2 的插件很多：planner、controller、smoother、behavior、costmap、route、docking、collision monitor。项目最常见的错误，是把所有问题都归到“路径规划没调好”。
实际上，室内导航至少有四种不同尺度的决策。
1. 路线层：先决定经过哪些通道
在大楼、仓库和多楼层场景中，拓扑路线先决定：
- 走哪条走廊；
- 经过哪扇门或电梯；
- 哪些边单向、限速或需要权限；
- 某条边封闭后如何绕行。
Nav2 Route Server 可以在预定义图上计算 route，并在边和节点触发开门、限速、事件与重路由。
路线层不关心机器人如何绕过眼前一把椅子。
2. 全局路径层：在地图上生成可行 path
Planner Server 根据全局 costmap 和机器人运动学生成从当前位置到局部/最终目标的路径。
常见选择：
- NavFn/Smac 2D：平面差速/全向；
- Smac Hybrid-A*：阿克曼或有最小转弯半径；
- Theta*/可见性类：更直接的栅格路径；
- 自定义 planner：特殊约束和地图表达。
如果底盘不能横移，就不能用允许任意方向的路径再期待控制器“自己修正”。
3. 局部控制层：把 path 变成可执行速度
Controller Server 在局部 costmap 和实时状态下输出速度。主流控制器思路不同：
- Regulated Pure Pursuit：跟踪清晰、参数相对直观，适合差速；
- DWB：在速度空间采样并打分；
- MPPI：对候选控制序列采样，能表达更丰富代价和动力学；
- Rotation Shim：先对齐路径方向，再进入跟踪。
控制器必须知道真实：
- 最大/最小线速度和角速度；
- 加速度、减速度和制动距离；
- 控制频率与延迟；
- footprint；
- 是否允许倒车和原地旋转；
- 低速死区和轮滑。
仿真中能输出的速度，不代表实车可以稳定执行。
4. 安全层：独立覆盖运动
Collision Monitor 直接读取传感器观测，根据 stop/slowdown/limit 区域限制 cmd_vel。它不替代功能安全认证，却可以形成比规划回路更短的碰撞保护。
推荐命令链：
controller cmd_vel
→ velocity smoother
→ collision monitor
→ base controller
急停与硬件安全仍位于更下层。
Costmap 是导航系统的公共语言
典型层：
static layer 永久几何
obstacle/voxel 实时障碍
inflation footprint 安全余量
keepout filter 禁行区
speed filter 区域限速
semantic/custom 门、玻璃、可通行性
全局 costmap 范围大、更新慢，服务全局路径；局部 costmap 使用 rolling window、更新快，服务避障和控制。
常见问题：
- observation source 的 frame 或时间戳错误；
- clearing ray 把真实墙清掉；
- obstacle 高度范围不覆盖桌板；
- inflation 太大导致窄门无路；
- footprint 只用圆形，忽略载荷和传感器外伸；
- 局部窗口太小，小于制动和绕障所需距离。
Behavior Tree 负责失败编排
BT Navigator 决定何时规划、跟踪、恢复和重试。恢复行为应针对失败原因：
- 全局无路：等待地图更新或换路线；
- 局部阻塞：等待、后退或局部清图；
- 控制不收敛：重新规划或换控制模式；
- 定位质量差：进入重定位；
- 安全层触发：保持停止，不应盲目 spin。
“清空 costmap 再试一次”不能成为所有失败的通用答案。
调试顺序
- 底盘手动控制稳定；
- TF 和时间戳正确；
- odom 连续、定位可信；
- costmap 障碍与 clearing 正确；
- 单独测试 planner；
- 单独测试 controller 跟踪固定 path；
- 最后调 BT 和恢复；
- 加入 route、docking 和多机器人。
一次只改变一层，并用 rosbag 重放同一场景。
生产指标
- 到点成功率与位置/角度误差；
- 全局规划与局部控制 P95 时延；
- 路径长度、任务时间与能耗；
- 最小人距和安全层触发；
- 重规划次数；
- stuck、oscillation、定位失败分类；
- 接管率；
- 不同载荷与地面下的跟踪误差。
Nav2 的价值是清楚的插件边界。把路线、路径、控制和安全各自调通，才有能力替换其中一个模块，而不把整套系统重新做一遍。