AGV学习系列--（4）navigation2导航

小车是怎么导航成功的？—— 初学者入门指南

navigation2导航出发

从你点击"去那里"的那一刻，到小车稳稳到达目标，中间到底发生了什么？
本文用最通俗的方式，把 Navigation2 + autopartol_robot_cpp 背后的导航全过程讲清楚。

目录

1. 一句话回答
2. 导航的 5 个关键角色
3. 完整导航流程（一步步看）
4. 用 autopartol_robot_cpp 做实验
5. 你可能会问的问题
6. 怎么自己动手验证

1. 一句话回答

小车导航 = 知道自己在哪 + 知道要去哪 + 算出怎么走 + 一步一步走过去，走的时候不断修正方向。

就像你去一个陌生的地方：

• 先打开地图定位自己在哪（定位）
• 搜一下目的地（目标点）
• 地图给你规划一条路线（路径规划）
• 你沿着路线走，边走边看路牌修正方向（运动控制）

小车做的事情，和你一模一样，只不过它用的是激光雷达、算法和代码。

2. 导航的 5 个关键角色

把导航想象成一个团队，每个人分工不同：

"队长"（行为树导航器 bt_navigator） │ 负责指挥调度：先规划，再走路，走不通就想办法 ┌────────┼─────────┐ ▼ ▼ ▼ "导航员" "驾驶员" "应急员" (planner) (controller) (behavior) 算路在哪 控制车轮转 卡住了后退/转圈 │ │ ▼ ▼ "地图"（costmap）— 哪里能走，哪里不能走 │ ▼ "定位员"（AMCL）— 报告自己现在在哪

🎯 角色 1：定位员 —— “我在哪？”

扮演者：AMCL（自适应蒙特卡洛定位）

• 手上有一张地图
• 拿着激光雷达扫一圈，看看周围的墙和障碍物
• 和地图比对，算出自己在地图上的精确位置
• 结果：map → odom坐标变换（告诉所有人"机器人在地图的这个位置"）

类比：你在商场里，看了一眼周围的店铺，对照商场导览图，就知道自己站在哪了。

🗺️ 角色 2：地图管理员 —— “哪里能走？”

扮演者：costmap_2d（代价地图）

• 静态地图是死的（墙、房间不会动）
• 但实际环境会变（有人走过、放了个箱子）
• costmap 把传感器实时看到的障碍物叠加到静态地图上
• 结果：一张"哪里危险、哪里安全"的动态地图

类比：你走路时，看到前面有个障碍物，你脑子里会自动更新"这里不能走了"。

🧭 角色 3：导航员 —— “走哪条路？”

扮演者：planner_server（全局规划器）

• 知道起点（定位员给的）和终点（用户给的）
• 在地图上找一条不撞墙的最优路线
• 常用算法：A*、Dijkstra、Hybrid A*
• 结果：一条由很多小点组成的路径（Path）

类比：你用高德地图搜路线，它给你推荐一条最快的路。

🚗 角色 4：驾驶员 —— “怎么一步步开过去？”

扮演者：controller_server（局部控制器）

• 拿到导航员给的路径
• 实时控制轮子的速度和方向
• 边走边看：偏了就修正，快到了就减速
• 常用算法：DWB（动态窗口法）、纯追踪、MPPI
• 结果：输出cmd_vel（线速度 + 角速度）给底盘

类比：你开车时，眼睛看着路，手转方向盘，脚踩油门——导航员给你一条路，你自己决定每一秒方向盘打多少。

🎬 角色 5：队长 —— “协调所有人”

扮演者：bt_navigator（行为树导航器）

• 接收用户的"去那里"指令
• 按顺序调用导航员、驾驶员
• 如果走不通了，调用应急员（后退、转圈、重新规划）
• 结果：把整个导航过程串起来

类比：队长发号施令——“先规划路线”“开始走”“卡住了？后退一下再试”。

3. 完整导航流程（一步步看）

假设你对小车说：“去 (3, 2) 那个位置，面朝北。”

下面是完整的 9 个步骤：

第 1 步：你发出指令

你通过autopartol_robot_cpp的partol_node发了一个导航目标。

用户 / 代码 → NavigateToPose Goal → bt_navigator

partol_node做的事很简单：把 x、y、yaw 打包成一个PoseStamped消息，发给 Nav2。

第 2 步：队长（bt_navigator）接到任务

队长一看：“要去那个位置啊，行，我安排一下。”

它首先调用导航员（planner_server）：

“喂，从当前位置到 (3,2)，给我算一条路。”

第 3 步：导航员算路

导航员拿到起点和终点，打开costmap（代价地图）：

• 把地图看成一个个小格子
• 墙是黑色（不能走），空地是白色（能走）
• 用 A* 算法从起点开始，一格一格"试探"，找到终点
• 算出一条不撞墙的路径

路径大概长这样： 起点 → (0.1,0.0) → (0.2,0.0) → ... → (3.0, 2.0) ← 终点

第 4 步：队长把路径交给驾驶员

队长拿到路径，递给驾驶员（controller_server）：

“路算好了，你沿着这条路开过去。”

第 5 步：驾驶员开车

驾驶员拿到路径，开始干活。它每 0.05 秒（20Hz）做一次决策：

1. 看看自己在哪（从 TF 拿当前位置）
2. 看看路径在哪（离路径偏了多少）
3. 算一下速度和方向（往左打一点还是往右打一点，开多快）
4. 发指令给底盘（cmd_vel：线速度 + 角速度）

就像你开车时，每隔一小会儿就要看一眼路、调一下方向盘。

第 6 步：边走边反馈

驾驶员会不断告诉队长：

“还剩 2.5 米…还剩 1.2 米…还剩 0.3 米…”

这就是Feedback（反馈）。partol_node里的feedback_callback就是用来接收这个的，你会看到日志里不断打印"剩余距离"。

第 7 步：遇到问题怎么办？

如果走着走着，前面突然出现一个障碍物——

1. 驾驶员发现走不通了，告诉队长"我卡住了"
2. 队长启动应急方案（恢复行为）：
   • 先试试后退一点
   • 再试试原地转个圈看看
   • 如果还是不行，重新规划一条新路
3. 规划成功 → 继续走
4. 反复试了几次还是不行 → 宣告导航失败

这就是为什么 Nav2 要用"行为树"——它能灵活处理各种异常情况，而不是一条路走到黑。

第 8 步：到达目标

驾驶员发现"离目标点很近了，方向也差不多了"，就停下来。

检查条件（goal_checker）：

• 位置误差 < 某个阈值（比如 0.25 米）
• 角度误差 < 某个阈值（比如 0.25 弧度）
• 速度接近 0

都满足 →导航成功！

第 9 步：队长报告结果

队长把结果告诉partol_node：“任务完成啦。”

result_callback被触发，日志打印 “导航成功！”

然后partol_node继续去下一个航点。

📊 一张图看懂整个流程

发指令 │ ▼ ┌──────────┐ │bt_navigator│ ← 队长（行为树） └─────┬────┘ │ ①算路 ▼ ┌──────────┐ │ planner │ ← 导航员（全局规划） │ (A*) │ └─────┬────┘ │ 路径 ▼ ┌──────────┐ │controller│ ← 驾驶员（局部控制） │ (DWB) │ └─────┬────┘ │ cmd_vel ▼ 底盘/车轮 │ │ 边走边看 ▼ ┌──────────┐ ┌──────────┐ │ costmap │◄────│ 传感器 │ ← 激光雷达 │ (障碍地图)│ └──────────┘ └─────┬────┘ │ ▼ ┌──────────┐ │ AMCL │ ← 定位员（我在哪？） └──────────┘

4. 用 autopartol_robot_cpp 做实验

autopartol_robot_cpp这个项目，就是上面这套流程的最简单用法。

4.1 它做了什么？

intmain(){autonode=std::make_shared<PartolNode>();node->run();// 开始巡逻}

run()函数里就干三件事：

voidPartolNode::run(){init_robot_pose();// ① 告诉机器人"你在这"（初始位姿）autopoints=get_target_points();// ② 拿到要去的航点列表for(每个航点){// ③ 一个一个走过去nav_to_pose(目标点);}}

4.2 nav_to_pose 是怎么工作的？

boolPartolNode::nav_to_pose(目标位姿){1.把目标打包成 Goal2.发给 Nav2 的 Action 服务器3.等...4.收到"成功"就返回true，失败就返回false}

注意：partol_node自己不做路径规划，也不做运动控制。它只是一个"发令员"——把目标告诉 Nav2，然后等着 Nav2 干完活。

真正干活的是 Nav2 里面的 planner、controller 那些模块。

4.3 你看到的日志对应了什么

5. 你可能会问的问题

Q1：为什么要先设置初始位姿？

AMCL 是"蒙着眼睛"开机的——它不知道自己在哪。它手里有地图，但不知道自己在地图的哪个位置。

你给它一个初始位姿，就像说：“你大概在这个位置，你自己再精确看看。” 然后 AMCL 用激光雷达扫一下周围，和地图比对，就能精确定位了。

类比：你刚到一个新城市，打开地图 APP 时，它会先问"是否使用当前位置"，这就是在设初始位姿。

Q2：为什么要有全局规划和局部控制两层？一层不行吗？

不行，因为：

• 全局规划看的是"整张地图"，算的是"哪条路最近"——但它反应慢，算一次要几十毫秒
• 局部控制看的是"周围几米"，反应快（每秒 20 次），能随时躲避突然出现的障碍物

就像你开车：

• 导航 APP 告诉你走哪条路（全局规划）
• 你自己握着方向盘，躲行人、调方向（局部控制）

两者缺一不可。

Q3：行为树是什么？为什么不用 if-else？

行为树（Behavior Tree）是一种把"做什么"和"怎么做"组织起来的方式。

想象一下导航的逻辑用 if-else 写：

if (还没到) { if (前面有路) { 往前走 } else { if (能后退) { 后退 if (退完后能重新规划) { 重新规划 } else { 失败 } } else { 失败 } } }

逻辑一复杂，if-else 就会嵌套得非常深，很难改。

行为树把这些逻辑画成一棵树，每个节点是一个动作或判断，改起来很方便。Nav2 把行为树放在 XML 文件里，你甚至可以不用改代码，直接改 XML 就能换导航策略。

Q4：costmap 和普通地图有什么区别？

普通地图（map_server 加载的）是静态的——墙在哪里、房间有多大，都是固定的。

costmap（代价地图）是动态的：

• 底层是静态地图
• 上面叠加传感器实时看到的东西
• 给每个格子打分：0 = 完全安全，254 = 在墙上，255 = 未知

规划器看的是 costmap，所以它能绕开实时出现的障碍物。

Q5：小车怎么知道自己走了多少？

靠两个东西：

1. 里程计（odom）：轮子上有编码器，轮子转了多少圈能算出来，进而算出走了多远、转了多少度。但里程计会飘——轮子打滑、地面不平整，误差会越积越大。

2. AMCL 定位：用激光雷达比对地图，算出精确位置。它能纠正里程计的漂移。

所以 TF 树是这样的：

map → odom → base_footprint ↑ ↑ AMCL 里程计

AMCL 发布map → odom的变换，用来"纠正"里程计的误差。

6. 怎么自己动手验证

想亲眼看一遍导航过程？按下面来：

第 1 步：启动仿真和导航

ros2 launch robot_sim robot_bringup.launch.py

你会看到 Gazebo（仿真环境）和 RViz（可视化界面）。

第 2 步：在 RViz 里看各个组件

打开 RViz 后，你可以看到：

• Map—— 静态地图（灰色 = 空地，黑色 = 墙）
• LaserScan—— 激光雷达扫到的红点
• Global Costmap—— 全局代价地图（膨胀层会显示一圈红色，就是不能太靠近墙）
• Local Costmap—— 局部代价地图（跟着机器人走的一个小窗口）
• Path—— 规划出来的路径（一条绿色的线）
• TF—— 各个坐标系的位置关系

第 3 步：用 2D Pose Estimate 设置初始位姿

在 RViz 顶部工具栏点2D Pose Estimate，然后在地图上小车的位置点一下，拖一下方向。

你会看到一堆红色的箭头（粒子云），一开始可能很散开，过一会儿就会聚成一团——这就是 AMCL 在定位。

第 4 步：用 2D Nav Goal 发一个目标

点2D Nav Goal，在地图上选个空旷的地方点一下。

然后你会看到：

1. 瞬间出现一条绿色的线 → 这是 planner 算出来的路径
2. 小车开始动 → controller 在控制
3. 路径可能会变 → 重规划了
4. 小车到目标位置停下 → 导航成功

第 5 步：运行 autopartol_robot_cpp

手动玩够了，试试自动巡逻：

ros2 run autopartol_robot_cpp partol_node\--ros-args --params-file\install/share/autopartol_robot_cpp/config/partol_config.yaml

看着日志，对比上面说的 9 个步骤，你就能把理论和实际对应起来了。

总结

小车导航成功，靠的是 5 个角色的配合：

autopartol_robot_cpp就是给 Nav2 当"传声筒"——把要去的地方告诉 Nav2，然后坐着等结果。真正的硬核算法都在 Nav2 里面。