第四章：导航功能实现

  在上一章的实操中，我们已经通过ActionGraph与ROS2的结合，打通了IsaacSim关节控制的基础链路：从单轮固定值控制起步，最终实现了外部终端通过ROS话题远程调速单个车轮。这套方案验证了「指令下发→关节驱动→物理求解」的完整流程，也让我们熟悉了ActionGraph与ROS2的通信对接方式。

  但在尝试用这套方案控制三轮全向底盘时，我们会立刻发现单关节分散控制的局限性：

1. 逆运动学解算繁琐：全向底盘的前进、横移、自转，需要将底盘中心的$`Vx/Vy/ω`$线速度/角速度，通过逆运动学公式换算为三个车轮的目标转速。如果采用单轮单独控制的方式，我们需要手动编写代码完成这一换算，不仅调试成本高，还容易因参数错误导致底盘运动偏移；
2. 控制链路碎片化：为了控制三个轮子，我们需要维护三条独立的ROS话题，下发三组转速指令，链路复杂且不利于后续维护；
3. 无标准化接口，无法对接上层应用：全向底盘控制的行业标准是/cmd_vel话题，但单轮控制的方案无法直接兼容这一标准，后续对接导航、路径规划等上层算法时，需要额外做适配层开发。

  为了解决这些问题，本章我们将分为两个核心阶段，完成从「分散单轮控制」到「整车一体化控制」再到「自主导航」的进阶：

1. 第一阶段：全向底盘一体化控制 我们将引入Isaac Sim内置的Holonomic Controller（全向底盘控制器），通过它接收标准/cmd_vel话题，自动完成逆运动学解算，将底盘速度指令直接转换为各车轮的目标转速，实现整车的一体化控制。
2. 第二阶段：Nav2自主导航对接 当底盘能够稳定响应/cmd_vel指令后，我们将直接接入ROS2生态中最主流的导航栈Nav2，利用其路径规划、避障与控制能力，实现全向底盘的自主导航仿真。

4.1 基于 Action Graph + Holonomic Controller 实现全向轮底盘控制 （实操）

4.1.1 holonomic Controller 组件介绍（理论，可选读）

  Holonomic Controller（全向驱动器）是 Isaac Sim 官方专为全向移动机器人封装的专用组件，也是本节的核心工具，广泛适配 NVIDIA Kaya 等三轮全向轮机器人、麦克纳姆轮底盘。

• 核心能力：内置全向轮逆运动学算法，无需开发者手动编写解算代码；
• 数据输入：接收传入的具体轮子参数数据以及目标速度指令，解析底盘线速度、角速度指令；
• 数据输出：根据底盘硬件参数，自动计算出每一个车轮的目标转速，并转发给底层 Articulation Controller；
• 底层关联：组件仅做运动学解算，最终关节驱动依旧依赖前文讲解的 Articulation Controller 与 PhysX 物理引擎，全程遵循「力矩驱动关节」的底层逻辑。

官方文档介绍：holonomic controller

4.1.2 全向轮底盘控制 actiongraph 设置 （实操）

  与 3.2.3 实操二：ROS2+ActionGraph实现外部动态控制单个关节 一样的思路，我们都是需要一个节拍器（on Playback tick）、一个接收 ros 数据的接收器，然后将接收到的速度指令进行拆分，并输入到 holonomic controller 中，并进行逆运算，最后将具体的关节速度指令通过 articulation controller 来控制，进而控制三个全向轮

   转化到 Action Graph 中的节点分别为：

节点名称	核心作用	应用场景
ROS2 Subscribe	订阅外部 ROS2 话题，接收 /cmd_vel 速度指令	外部指令接入入口，解析 Twist 格式消息
Break 3 Vector	将三维向量拆解为 X/Y/Z 三个独立分量	拆分 cmd_vel 的线速度、角速度数据
Usd Setup holonomic Robot	读取 articulation 中具有 isaacmecanumwheel 属性的 joint 的参数 通过这个参数来获取全向轮的尺寸参数	初始化全向轮控制器参数，配置控制器参数以便逆运算
Holonomic Controller	全向运动学解算，底盘速度转各轮目标转速	本节核心节点，替代手动运动学代码
Make 3 Vector	将多个分量重新组合为三维向量	整合单轮转速指令，适配控制器输入格式
Articulation Controller	底层关节控制器，执行车轮转速指令	衔接物理引擎，驱动关节实际转动

整体数据流总览

外部ROS2终端 → /cmd_vel(Twist) → ROS2 Subscribe → Break 3 Vector（数据拆分）
→ Holonomic Controller（运动学解算：底盘速度→单轮转速）
→ Make 3 Vector（数据重组）→ Articulation Controller → PhysX 力矩驱动 → 全向轮底盘运动

4.1.2.1 全向轮 actiongraph 构建的具体流程

0. 加载测试场景

   我们首先在 IsaacSim 容器 的终端中运行下面的命令，以此来自动加载我们的场景

cd ~/robotac_isaac_assets
./integrated_runtime/run_demo_scene.sh --world test_control_one_wheel

   具体操作如下图：

  

1. 新建/复用ActionGraph画布    可在上一节固定值控制工程基础上修改，或新建空白Action Graph编辑器工程。

或

2. 鼠标左键拖拽节点至编辑区    在节点检索框分别搜索 On Playback Tick、ROS2 Context、ROS2 Subscribe Twist、Break 3 Vector、Usd Setup holonomic Robot、Holonomic Controller、Make 3 Vector、Articulation Controller，拖拽至画布合适位置。   

   

3. 逐个配置节点参数

   • ROS2 Context：按需填写Domain Id，固定ROS通信域；

   • ROS2 Subscribe Joint State：

     • Topic Name：自定义ROS话题名称，示例：/cmd_vel（后续终端发布话题必须和该名称保持一致）；

   • Articulation Controller ：

     • Target Prim配置规则和上一节完全一致

   • Usd Setup holonomic robot：

     • comPrim：说明在哪棵关节树能够正常找到指定的全向轮，一般直接指定 base_link（articulation root）即可
     • robotPrim：这里要指定的是场景中的指定 robot，而这个属性一般是 articulation root 的父级直接继承，因此我们这里选着 base_link 的父级

   • holonomic controller：

     • lineargain：是底盘线速度指令整体缩放增益，作用于 /cmd_vel 消息里的 linear.x、linear.y 前后平移速度输入值。我们这里设置为 -1.25

  最终完整连线效果图：

5. 仿真验证与问题说明    保存Graph配置，启动IsaacSim仿真；并在 Ros2 容器 新开终端输入ROS2发布指令，即可使用 Ros2 进行关节运动：

  ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.75, y: 0.75, z: 0.0}, angular: {x: 0.0, y: 0.0, z: -0.2}}"

最后的仿真结果为：   

  到这里我们就已经知道要怎么去控制全向轮底盘机器人进行移动了，接下来我们来学习一下要怎么样才能让机器人能够像我们人一样自主的进行移动到达指定的位置

4.1 导航系统

4.1.1 什么是自主导航

  我们先来带入一个所有人都经历过的日常场景：你第一次来到一所全新的陌生校园，下课之后想去食堂吃饭，整片校园建筑、道路、绿植对你而言都是完全未知的环境，校门口、教学楼、花坛、路障都是陌生障碍物，想要顺利走到食堂，你的大脑会自动完成三步思考，缺一不可：

1. 认清当下位置：抬头看楼栋标识、路边路标、周边建筑，确定【我现在站在校园哪个位置】

2. 确定目的地：明确最终目标，也就是【我要去往校园食堂这个固定地点】

3. 规划行走路线：观察前方路人、石墩、树木障碍物，避开拥堵和死路，选出一条安全最短的路，一步步走到食堂

  简单来说：人在陌生环境自主抵达目的地，靠的就是 「感知环境+确定位置+锁定目标+避障行路」 整套逻辑。

  而我们本节课要学习的机器人自主导航，逻辑和人类找食堂完全一模一样——只不过把“人的眼睛、大脑、双脚”，替换成了机器人的传感器、算法控制器、移动底盘。

  对应我们找食堂的三步自问，机器人想要自主移动，同样必须解答三个终极问题，这也是自主导航领域永恒的三大核心问题：

• 🗺️ 我在哪里？（定位 Localization）对标：我在校园哪一个位置？

• 🎯 我要去哪里？（目标设定 Goal Setting）对标：我要去食堂这个目的地？

• 🛣️ 怎么去？（路径规划 Path Planning）对标：避开路边障碍，怎么走安全到达？

  基于以上三大核心问题，我们给出标准化专业概念：

  自主导航（Autonomous Navigation）是指机器人在未知或已知环境中，通过传感器感知环境、规划路径、避开障碍物，自主地从起点移动到目标点的能力。

   我们顺着刚才陌生校园去食堂的场景继续往下思考：如果校园完全陌生、道路繁杂，我们不会盲目走路找食堂，都会统一使用手机地图导航出行，大家日常手机导航去食堂，有着固定连贯的行为流程，全程通俗易懂：

1. 提前拥有校园电子地图，看清校园道路、墙体、花坛位置

2. 打开手机定位，让手机精准识别我当下站在校园哪个位置

3. 导航自动给墙体、石墩留出安全距离，不会贴着障碍物规划路线，防止走路磕碰

4. 手动选定食堂为终点，地图规划一条顺畅的主干道直达食堂

5. 步行途中遇到临时路人、堆放杂物，自主微调路线，就近避障

6. 跟随导航指引，平稳移动，最终到达食堂目的地

   而机器人自主导航，就是百分百复刻人类手机导航的全套流程。

   既然复刻手机导航，我们不妨先把刚才人类的操作流程，原封不动地翻译成机器人的“预备动作清单”：

人类手机导航步骤	机器人对应动作	关键差异点
1. 提前拥有校园电子地图	机器人需要提前获取环境地图	重点：手机地图是提前下载好的，机器人地图需要自己动手画
2. 打开手机GPS定位	启动定位算法，匹配地图中的坐标
3. 地图自动给墙墩留出安全距离	对地图障碍物进行“膨胀处理”
4. 手动选定食堂终点，规划主干道	设定目标点，生成全局路线（粗线条的大路）
5. 遇到临时路人，自主微调路线	感知局部动态障碍，生成局部轨迹（微调方向盘）
6. 跟随指引，平稳到达	底盘接收速度指令，平滑移动

   看到这里，细心的你一定会发现一个大问题：人类手机里的地图是提前装好的，但机器人第一次来这个教室，谁给它装地图？

   没错，机器人没有“预装地图”。因此，机器人的自主导航，实际上要拆解成两大阶段去实现，缺一不可：

   阶段一（画地图）：让机器人第一次“逛校园”，把环境画成一张电子地图。

   阶段二（用地图）：加载画好的地图，打开导航App（Nav2），实现定位与路径规划。

   接下来，我们严格遵循这个先后顺序，一步一步动手实现。

提示：接下来的具体使用到的模块工具（比如下面会讲的 Nav2）均为参考，选手可以自行使用自己喜欢的模块进行完成比赛的功能

4.2 导航系统实现

4.2.1 阶段一: 建图（让机器人“画”出校园地图）

4.2.1.1 建图理论（可选读）

为什么要先建图？（回到校园场景）

   回到我们去陌生校园找食堂的例子：你第一次来到新学校，首先需要熟悉校园布局——走一遍主干道，记住食堂在哪栋楼后面，围墙在哪条路边。没有这个“心中有地图”的过程，后面的定位和导航都是空中楼阁。

   机器人完全一样：在建图完成之前，Nav2导航框架是无法启动的，因为它没有数据可读。所以，本节课我们先解决“画地图”的问题。

地图长什么样？

   机器人画的地图不是我们肉眼看的风景照，而是一张“占据栅格地图”（Occupancy Grid Map），如下图。你可以把它想象成一张超大围棋棋盘铺在地面上：

• 黑色格子（占据）：这里有墙、有柱子，机器人绝对不能走。

• 白色格子（空闲）：这里地面平坦，机器人可以自由通过。

• 灰色格子（未知）：这里还没探索过，机器人不清楚能否通过。

我们使用什么方式去建图？

   市面上建图方案很多（比如实体车用SLAM Toolbox、Cartographer），但对于零基础实训，最怕硬件出问题（比如网口松了、雷达不转）。为了让你跳过硬件坑，直击导航核心，我们提供两条路，今天上课我们统一走方案一：

   方案一（主线·推荐）：NVIDIA IsaacSim 仿真一键出图。无需实体硬件，鼠标点几下，直接生成一张“零噪点、精度高”的完美栅格地图。（本节课主讲）

   方案二（支线·拓展）：SLAM Toolbox 实景扫图。如果你手里有实体机器人，可以用手柄推着它现场扫描。我们在课程最后会补充演示它的流程，原理与方案一完全互通。

为什么先讲仿真建图？

  因为仿真生成的 occupancy_map.pgm 文件，和实体SLAM生成的 map.pgm 文件格式完全一致。你在仿真里学会了加载这张图，换到实体车上，操作一模一样，没有任何学习成本。

---

4.2.1.2 建图实操

方案一：仿真一站式建图

1. 打开 isaacsim 场景：

   我们首先在 IsaacSim 容器 的终端中运行下面的命令，以此来自动加载我们的场景

cd ~/robotac_isaac_assets
./integrated_runtime/run_demo_scene.sh --world create_map

   具体操作示例如下：

  

2. 跟着图中的步骤进行操作

3. 将生成的 占据图 保存下来

   这样我们就使用了 isaacsim 的 Occupancy Map 扩展完成了一个完整的建图了，而接下来我们就能使用刚才保存下来的地图，来开始使用我们的地图导航程序了

方案二：实景扫图

前置理论（选读）

一、SLAM 到底是什么？ 在动手之前，我们先花3分钟搞懂 SLAM 的核心逻辑。你不需要背公式，只需要理解它“边猜边走边画”的思路。

SLAM 的全称是 Simultaneous Localization and Mapping——同步定位与建图。

拆开来看，它要同时解决两个问题：

问题	通俗解释	对标人类行为
定位（Localization）	机器人需要知道自己现在站在哪里	你闭着眼走路，靠脚步数和方向感猜自己在哪
建图（Mapping）	机器人需要知道周围环境长什么样	你边走边摸黑记住墙在哪、桌子在哪

而这两个问题互相依赖，形成了一个“先有鸡还是先有蛋”的死结：

要知道自己在哪，得先有地图做参考；

要画地图，得先知道自己在哪才能把数据拼对。

SLAM 的聪明之处就在于：它打破了这个死结。

它让机器人一边走、一边猜、一边画——先粗略估计自己的位置，把激光雷达扫到的数据贴上去；走几步之后再回头修正之前的猜测，把地图拼得更准。就像你蒙着眼在一个陌生房间里走，每走几步摸到一个桌子角，就回头修正之前对房间大小的判断。

SLAM 的典型工作流程（理解即可，不用背）：

传感器采集数据：激光雷达向四周发射激光束，通过反射时间计算障碍物的距离和角度；同时轮子上的编码器记录走了多远、转了多少弯。

扫描匹配：把当前扫到的一圈数据和上一圈数据进行比对，判断机器人移动了多少。

位姿估计：结合编码器数据，计算机器人最可能的位置和朝向。

地图更新：把新的障碍物信息贴到地图上。

回环检测：当机器人绕了一圈回到起点附近时，识别出“哎？这个地方我来过！”，然后把整张地图“拉一拉、拧一拧” ，消除之前累积的误差。

核心认知：SLAM 画出来的地图不是一次性画对的，而是边走边修正的。所以你会发现建图过程中地图在慢慢变清晰，这就是 SLAM 在不断“自我纠错”。

---

二、 我们选用哪种 SLAM 算法？ 市面上常见的 2D SLAM 算法有 Gmapping、Cartographer、Hector SLAM 等。本课程选用 SLAM Toolbox，原因有三：

对比维度	SLAM Toolbox	老一代 Gmapping
建图质量	图优化后端，大场景地图一致性更好	粒子滤波，大场景容易飘
运行模式	支持同步/异步建图、纯定位等多种模式	模式单一
ROS2 支持	原生支持，官方推荐	需额外移植
终身地图	支持在地图基础上继续优化更新	不支持

简单说：SLAM Toolbox 是 ROS2 官方推荐的 2D SLAM 方案，功能更强、适配更好。本节课我们就用它来实操。

实操

1. 在 IsaacSim 容器端 启动 IsaacSim 并点击 开始仿真

        cd ~/robotac_isaac_assets
        ./integrated_runtime/run_demo_scene.sh --world bobac_slam

具体操作如下：

   打开场景后可以在 IsaacSim WebRTC Streaming Client 中通过 鼠标右键 + 键盘的 WASD 调整镜头的位置

2. 在 ROS2 容器端 启动 SLAM Toolbox 建图

        cd ~/demo_ws
        source install/setup.bash
        # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
        export ROS_DOMAIN_ID=44

        ros2 launch nav2_demo_pkg slam_toolbox_online.launch.py

3. 在 宿主机端 启动 X11 服务用于查看 rviz2

        export DISPLAY=:1
        xhost +

4. 在 ROS2 容器端 启动 rviz2，并进行配置

        export DISPLAY=:1
        xhost +

        # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
        export ROS_DOMAIN_ID=44

        # 启动 rviz2
        rviz2

并配置 rviz2 显示具体的话题中的数据

如下图：

及

完整具体操作如下：

配置完毕后可以通过 摁住鼠标中键并移动鼠标 来调整地图的位置，滚动滚轮 来放大或缩小地图，方便看到完整地图

5. 在 ROS2 容器端 启动键盘控制程序，开始控制机器人进行建图

        # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
        export ROS_DOMAIN_ID=44

        ros2 run teleop_twist_keyboard teleop_twist_keyboard

启动过后将会启动主程序以及键盘控制程序，随后将会在终端中显示出下面的键盘控制日志

然后就能通过键盘的按键进行控制 bobac 的底盘了（注意：该程序是通过键盘的按键来进行组合 cmd_vel 话题的话题数据的，最终控制底盘的方式都是通过 cmd_vel 话题进行控制） 具体控制底盘的按键如下：

说明；可通过 w(提高)/x(降低)按键调整机器人底盘运动的线速度，通过e(提高)/c(降低)按键调整机器人底盘运动的角速度

建图过程展示：

6. 在 ROS2 容器端 保存地图到指定的位置

        ros2 run nav2_map_server map_saver_cli -f 指定要存放的地址

当出现下图的时候表示地图保存完毕

4.2.2 阶段二：导航

   刚才我们忙活半天画好了地图，现在我们要让机器人动起来。先不要管 Nav2 内部有多少个节点、多少种通信机制，我们先把它当成一个“傻瓜导航APP”来启动。

   操作目标：加载地图 -> 启动Nav2 -> 在Rviz（机器人可视化界面）里用鼠标点一个“食堂目标点”，看机器人自己走过去。

1. 在 IsaacSim 容器端 启动 IsaacSim 并点击 开始仿真

        cd ~/robotac_isaac_assets
        ./integrated_runtime/run_demo_scene.sh --world bobac_slam

具体操作如下：

   打开场景后可以在 IsaacSim WebRTC Streaming Client 中通过 鼠标右键 + 键盘的 WASD 调整镜头的位置

2. 在 ROS2 容器端 启动 导航程序

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44
    cd ~/demo_ws
    source install/setup.bash

    ros2 launch nav2_demo_pkg nav2_navigation.launch.py map:=刚才保存的地图的地址.yaml

   这里我们运行示例地图：

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44
    cd ~/demo_ws
    source install/setup.bash

    ros2 launch nav2_demo_pkg nav2_navigation.launch.py map:=/workspace/demo_ws/src/nav2_demo_pkg/maps/map.yaml

   现在，导航软件已经“打开”了，地图也加载好了，在宿主机页面我们可以如下图所示的rviz2页面。但此时机器人还不知道自己在哪里，也不知道要去哪里——它正等着你帮它回答前面我们提出的导航的三个问题。

回顾我们一开始提出的机器人导航三大核心问题：

① 我在哪？ → ② 要去哪？ → ③ 怎么去？

这三个问题的逻辑顺序是严格固定的：

如果不知道“我在哪”，就不可能知道“要去哪”该往哪个方向走；

如果不知道起点和终点，就不可能规划出“怎么去”的路线。

这个逻辑顺序，直接决定了你在Rviz里的操作顺序。

在Rviz顶部的工具栏中，有两个按钮专门用来回答前两个问题：

按钮	名称	回答的问题	必须点击的时机
绿色圆点+箭头	2D Pose Estimate	① 我在哪？	必须先点——不知道自己在哪，无法规划路线
橙色旗帜	Nav2 Goal	② 要去哪？	必须后点——只有定位完成后，目标点才有意义

因此接下来，我们实操使用 rviz2 的这两个功能来回到机器人这两个问题：

3. 初始化定位（2D Pose Estimate）

具体操作：

     1） 选择 2D Pose Estimate 按钮，然后在地图上的 点击并按住鼠标左键 指定初始化位置与初始化的机器人姿态朝向

具体操作演示如下：

为什么要做这一步？

机器人此时虽然身处环境中，但它还不知道自己站在地图上的哪个坐标格子里。你给它的这个粗略位置，相当于告诉它：“喂，你现在大概在这片区域的这个角落，面朝那边。”

如何判断定位成功了？（关键！）

点击之后，请等待 3~5 秒，仔细观察屏幕上的红色或绿色的激光雷达扫描点（这些点代表了机器人眼中障碍物的位置）。你会看到：

起初，这些扫描点可能乱飞，与地图上的黑色墙壁轮廓完全错位（像是透明的鬼影）。

几秒后，随着AMCL自适应定位算法不断将雷达数据与地图进行匹配，这些扫描点会像被磁铁吸住一样，精准地贴合到地图的黑色障碍物（墙壁、桌腿）边缘上。

新手判断标准：当雷达扫描点不再飘忽，并且紧紧贴附在地图的黑色线条上时，说明机器人已经完成了“GPS自校验”，定位系统确认稳定。此时，机器人的位置才算是真正在地图中“锁定”了。

4. 发送导航目标点（2D Pose Estimate）

具体操作：

     1） 选择 Nav2 Goal 按钮，然后在地图上的 点击并按住鼠标左键 指定机器人导航的目标点和目标朝向

具体操作演示如下：

此时，你会看到Nav2三大部门瞬间响应：

出现全局路径（红色长线）：Planner Server（规划部）立刻计算出从机器人当前位置到“食堂”的最短安全大路，一条红色的粗线会瞬间贯穿地图上的通道。

机器人开始移动：Controller Server（控制部）接手，机器人开始沿着红色线稳步前进。（并且由于是全向轮底盘以及局部路径规划算法的位置定位权重较高，因此并不会立马调转机器人方向朝向目标的朝向）

动态避障生效：在机器人行走途中，如果路线前方出现了临时的障碍物，你会在Rviz中看到一条不断变化的蓝色局部轨迹——机器人的路径实时微调，微微绕开障碍物，随后重新靠拢回紅色全局线，最终平稳抵达“食堂”目标点。

---

看到机器人动起来并且稳稳停下的那一刻，恭喜你，你已经亲手跑通了一套完整的自主导航系统！

这套“先点绿标定位置，再点橙标设目标”的流程，完美复刻了你手机导航时“先等GPS定位转圈结束，再输入目的地点击导航”的日常习惯。

---

接着，我们拆开“黑盒”看一看

机器人成功跑起来了，但你可能好奇：刚才点击 2D Nav Goal 之后，后台到底发生了什么？

这就引出了我们之前提到的那张 Nav2 内部架构图。但现在你有了实操体验，我们再来看这张图，就不会觉得抽象了。我们不去死记硬背那些复杂的 Action 通信和 Behavior Tree，新手只需要搞懂 Nav2 内部的各个模块具体在干什么即可

4.2.3 ROS2 Nav2 导航系统的组成 (理论可选读)

   相对于 机器人 来说 Nav2内部数据流架构如下图所示：

   相对于我们上面的 陌生校园去食堂 例子来看，Nav2 内部的数据流架构如下图所示：

结合 Nav2 的架构分工与手机导航的出行逻辑，机器人自主导航的执行流程环环相扣，顺序不可颠倒：

测绘制作地图（SLAM建图） → 开启定位找准位置（AMCL自定位） → 障碍物预留安全距离（地图膨胀） → 规划往返路线（双层路径规划） → 全模块联动运行（Nav2整合导航）

纵观整个流程，除去最前端的建图准备工作，导航运行时核心数据的流动顺序可以提炼为：定位 → 全局规划 → 局部控制。

这个顺序完美契合了我们自主导航时的思考链路：

• 先确认“我在哪”（对应定位）；

• 再规划“走哪条大路”（对应全局规划）；

• 最后决定“怎么迈出每一步”（对应局部控制）。

顺着这条数据流动的主线，我们也能清晰地看到 Nav2 在幕后被划分成了三个各司其职的核心部门：Planner（规划部）看得更远，负责修大路；Controller（控制部）看得更近，负责实时操控；而 Recovery（应急部）则在遇到死胡同时负责兜底。 正是这三大部门的无缝协作，才最终完成了从“出发地”到“目的地”的完整导航任务。

那么问题来了——这三个部门分别是怎么工作的？它们各自的“输入”是什么？“输出”又是什么？

接下来，我们逐个拆解Nav2的四大核心模块，把你刚才跑通Demo时看到的每一个现象，都找到它的“幕后推手”。

注意：后面为了区分 demo 模块与上面示例视频中的效果程序，接下来我们的 demo 程序将会通过 绿色路线为全局规划路线，蓝色为局部规划路线

4.2.3.1 Planner

4.2.3.1.1 自定位（AMCL）

在规划部（Planner Server）开始画绿色路线之前，它必须先知道一件事：机器人现在站在地图上的哪个位置？

你可能会想：“我不是在Rviz里点了一下 2D Pose Estimate 吗？那不是已经告诉它位置了吗？”

没错，你确实给了它一个位置——但那是粗略的，误差可能有半米到一米。打个比方：你告诉规划部“机器人大概在图书馆门口附近”——但这个“附近”到底是门左边3米还是右边2米？规划部需要的是厘米级精度，否则它画出来的绿色路线可能从“撞墙”开始。

所以需要一个算法，把你给的模糊猜测，校准成精确坐标。

（二）AMCL是怎么工作的？

AMCL的核心逻辑叫“撒粒子”：

• 你点击 2D Pose Estimate 后，AMCL会在你点的位置周围随机撒出几百个“虚拟机器人”（粒子），每个粒子代表“机器人可能在这里”。

• 每个虚拟机器人都做一次“虚拟激光扫描”，和真实的激光雷达数据比对——匹配度高的粒子得高分并复制，低的淘汰。

• 经过3~5轮迭代（就是你看到的那几秒等待时间），所有粒子会汇聚到机器人真实位置附近——定位完成。

• 你在Rviz中看到的激光雷达扫描点从乱飘到贴紧黑色轮廓，就是这个过程的直观表现。

对标人类行为：手机GPS一开始显示你在一栋楼附近（几十米误差），等几秒钟后定位圈缩小到楼门口（几米误差）。AMCL做的就是同样的事情——把“米级”变成“厘米级”。

定位完成后，Planner Server 拿到了精确的起点坐标，可以开始干活了。

（三） AMCL 配置文件解析（选读）

文件路径：

/workspace/demo_ws/src/nav2_demo_pkg/config/amcl_params.yaml

核心内容如下：

# AMCL 自定位配置：只包含定位节点需要的参数，不混入规划/控制参数。
amcl:
  ros__parameters:
    use_sim_time:  # 是否使用仿真时钟

    # === 坐标系与输入话题 ===
    global_frame_id:  # 全局地图坐标系，AMCL 最终输出机器人在 map 中的位置
    odom_frame_id:  # 里程计坐标系，由底盘或仿真连续发布
    base_frame_id:  # 机器人本体坐标系，必须与 URDF/IsaacSim 中一致
    scan_topic:  # 激光雷达 LaserScan 话题，AMCL 用它和地图轮廓匹配

    # === 机器人运动模型 ===
    robot_model_type:  # 机器人运动模型类型

    # === TF 输出 ===
    tf_broadcast:  # 让 AMCL 发布 map -> odom 变换，供 Nav2 其它节点使用
    transform_tolerance:  # TF 查询允许的时间容差，仿真中可略大以避免时间抖动

    # === 粒子滤波核心参数 ===
    min_particles:  # 最少粒子数；越小越省算力，但定位稳定性下降
    max_particles:  # 最多粒子数；定位不稳时可增大，CPU 占用也会增加
    update_min_d:  # 机器人平移超过设定阈值后更新粒子滤波
    update_min_a:  # 机器人旋转超过设定阈值后更新粒子滤波
    resample_interval:  # 粒子滤波重采样间隔

    # === 激光匹配模型 ===
    laser_model_type:  # 似然场模型，室内栅格地图中常用且计算较快
    max_beams:  # 每帧激光参与匹配的采样数量上限
    laser_max_range:  # 参与定位的激光最大有效距离
    laser_min_range:  # 参与定位的激光最小有效距离
    laser_likelihood_max_dist:  # 激光点到障碍物的似然评分距离范围
    z_hit:  # 激光命中地图障碍物的权重
    z_short:  # 激光比预期更短时的权重，常见于临时障碍物
    z_max:  # 激光达到最大量程时的权重
    z_rand:  # 随机噪声权重，环境噪声大时可适当增大
    sigma_hit:  # 命中模型标准差，用于描述激光测量可信度
    lambda_short:  # 短距离异常读数的指数衰减参数

    # === 里程计噪声模型 ===
    alpha1:  # 旋转运动导致的旋转误差
    alpha2:  # 平移运动导致的旋转误差
    alpha3:  # 平移运动导致的平移误差
    alpha4:  # 旋转运动导致的平移误差
    alpha5:  # 侧向运动误差；全向底盘更需要关注

    # === 初始位姿 ===
    set_initial_pose:  # 是否在启动时使用预设初始位姿
    initial_cov_xx:  # 初始 x 方向不确定性；越大粒子云越分散
    initial_cov_yy:  # 初始 y 方向不确定性
    initial_cov_aa:  # 初始朝向不确定性

参数含义速查表：

参数	作用	配置注意
global_frame_id	全局地图坐标系	需要与地图发布坐标系保持一致
odom_frame_id	里程计坐标系	需要与底盘或仿真 TF 保持一致
base_frame_id	机器人本体坐标系	需要与机器人模型根坐标一致
scan_topic	激光雷达输入话题	需要能收到 LaserScan 数据
min_particles	粒子数量下限	影响定位稳定性和计算量
max_particles	粒子数量上限	影响定位稳定性和计算量
max_beams	每帧激光参与匹配的采样数量	影响匹配精度和计算量
alpha1~alpha5	里程计噪声模型	需要根据底盘运动误差调整

经验法则：定位漂移明显时，先检查 /scan、/odom、/tf 是否正确，再根据定位稳定性和计算资源调整粒子数量。

（三）实操：AMCL 配置文件与调参实验

0. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_slam

在运行下面的 amcl 示例程序前，需要配置一下 amcl 的配置文件，以更好的适配我们的机器人

1. 在 宿主机端 开启 X11 服务

需要确保 宿主机已经开启 X11 服务，我们可以通过下面的操作开启 X11 服务，确保 RViz 的正常可视化（如果有的时候 RViz 没能可视化，请先检查程序是否有自动开启 RViz 服务后再检查是否有开启 X11 服务）

    export DISPLAY=:1
    xhost +

2. 在 ROS2 容器端运行 AMCL 示例程序

文件路径：

/workspace/demo_ws/src/nav2_demo_pkg/launch/amcl_localization.launch.py

该启动文件只用于观察定位效果，会启动 map_server、amcl、生命周期管理器和 RViz。启动前需要先在 Isaac Sim 中加载导航场景，并确认地图文件已经存在。

    cd /workspace/demo_ws
    colcon build --symlink-install --packages-select nav2_demo_pkg
    source install/setup.bash

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch nav2_demo_pkg amcl_localization.launch.py \
      map:=/workspace/demo_ws/src/nav2_demo_pkg/maps/map.yaml

启动后，在 RViz 中使用 2D Pose Estimate 给出机器人初始位姿，观察粒子云是否逐渐收敛、激光扫描点是否贴合地图轮廓。

具体的实操演示如下：

在上面的演示中，我们可以看到，雷达扫描点在越来越贴近障碍物边缘，同时黄色的箭头粒子也越来越少，这是因为 amcl 正在发挥作用，不断的预测自己的当前位置

4.2.3.1.2 膨胀地图

（一）为什么需要膨胀层？ 我们画好的地图上，黑色格子代表“墙”——机器人绝对不能走。但问题是：机器人是有物理尺寸的（不是数学上的一个点）。如果规划器只把黑色格子当障碍物，它画的路径可能会紧贴墙边——那机器人的轮子或外壳就会蹭到墙。

解决方案：在地图上给每个黑色障碍物套一层“隐形防撞服”——把障碍物向外扩展N圈，扩展出来的区域变成“禁区”，规划器画路径时不得进入这些区域。

对标人类行为：你走路时会下意识地和墙壁保持“一臂距离”，防止蹭脏衣服。膨胀半径就是机器人的“臂长”。

你在Rviz中看到的黑色障碍物周围一圈蓝色/紫粉色区域，就是膨胀层——它属于代价地图（Costmap）的一部分。如下图所示：

（二）什么是代价地图？

代价地图（Costmap） 在原始地图基础上，给每个栅格赋予"代价值"：

颜色	名称	代价值	含义	Rviz 中的显示效果
黑色+紫色（深紫色）	LETHAL_OBSTACLE（致命障碍物）	254	被障碍物直接占据，机器人不可通行	地图上的墙体和障碍物
浅蓝色	INSCRIBED_INFLATED_OBSTACLE（内切膨胀障碍物）	253	机器人中心到这里时轮廓可能碰到障碍物，机器人不能通行	障碍物边缘的高代价区域
粉蓝色渐变	膨胀区域（Inflation Zone）	1-252	障碍物向外膨胀产生的安全缓冲区	障碍物周围的渐变代价区域，越接近浅蓝色区域代价越高
白色/灰色	FREE_SPACE（自由空间）	0	无障碍、无代价区域	可通行区域

（三）代价地图的核心概念 代价地图（Costmap）是对原始地图的“加工版本”——它在原始黑白地图的基础上，叠加了膨胀层和其他传感器数据层，供规划器使用。

代价地图 = 静态地图层 + 膨胀层 + （可选）动态障碍物层

Nav2中有两种代价地图：

全局代价地图（Global Costmap）：基于静态地图生成，用于全局路径规划，一般是整章地图的区域进行生成

局部代价地图（Local Costmap）：实时更新，用于局部避障，并且局部代价地图都是机器人周围一个小区域，会高频实时的检测这个区域是否有障碍物，如果有就会改变 local planner 的路线以避开障碍物

注意：

本节只观察 Global Costmap（全局代价地图）。全局代价地图固定在 map 坐标系中，它需要同时满足两个条件：

一、 map_server 已经发布 /map

二、 AMCL 已经发布 map -> odom -> base_link 的 TF

因此本实验会启动 AMCL。否则 costmap 无法知道机器人在地图中的位置，就不会正常激活和发布膨胀地图。

（四）代价地图文件解析(选读)

文件路径：

/workspace/demo_ws/src/nav2_demo_pkg/config/costmap_params.yaml

本实验关注全局代价地图中的三层结构：

global_costmap:
  global_costmap:
    ros__parameters:
      global_frame:
      robot_base_frame:
      resolution:
      robot_radius:
      rolling_window:
      plugins:

      static_layer:
        plugin:
        map_subscribe_transient_local:

      obstacle_layer:
        plugin:
        observation_sources:
        scan:
          topic:
          sensor_frame:
          data_type:
          marking:
          clearing:

      inflation_layer:
        plugin:
        inflation_radius:
        cost_scaling_factor:

三层作用如下：

层	作用
static_layer	读取 /map，把静态墙体和障碍物放入 costmap
obstacle_layer	读取 /laser_scan_fuse，加入当前雷达看到的障碍物
inflation_layer	在障碍物周围扩展安全距离，形成膨胀区域

（四）实操：观察膨胀代价地图

1. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_slam

2. 在 宿主机端 开启 X11 服务

需要确保 宿主机已经开启 X11 服务，我们可以通过下面的操作开启 X11 服务，确保 RViz 的正常可视化（如果有的时候 RViz 没能可视化，请先检查程序是否有自动开启 RViz 服务后再检查是否有开启 X11 服务）

    export DISPLAY=:1
    xhost +

3. 膨胀地图启动

文件路径：

/workspace/demo_ws/src/nav2_demo_pkg/launch/costmap_only.launch.py

该启动文件会启动：

节点	作用
map_server	加载并发布静态地图 /map
amcl	根据地图、雷达和里程计发布定位 TF
planner_server	创建并发布 global_costmap
amcl_initializer	发布初始位姿，并在静止状态下触发 AMCL 更新
rviz2	显示地图、激光和全局膨胀代价地图

运行命令：

    export DISPLAY=:1
    xhost +
  
    cd /workspace/demo_ws
    colcon build --symlink-install --packages-select nav2_demo_pkg
    source install/setup.bash

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch nav2_demo_pkg costmap_only.launch.py \
      map:=/workspace/demo_ws/src/nav2_demo_pkg/maps/map.yaml

完整启动演示如下：

5. 观察现象

启动后 RViz 中主要观察三类内容：

显示项	作用
Map	原始静态地图
LaserScan	当前雷达扫描点
Global Costmap	加入膨胀层后的全局代价地图

如果只看到原始地图、看不到膨胀区域，优先检查终端中是否还有类似下面的 TF 报错：

Timed out waiting for transform from base_link to map

出现这个报错说明 AMCL 还没有完成初始化，global_costmap 无法获得机器人在 map 中的位置。

6. 调参实验：观察膨胀半径变化

修改：

nano /workspace/demo_ws/src/nav2_demo_pkg/config/costmap_params.yaml

对比不同膨胀半径：

inflation_radius:
inflation_radius:
inflation_radius:

每次修改后重新编译并启动，并且 reset 并重新开始仿真：

    cd /workspace/demo_ws
    colcon build --symlink-install --packages-select nav2_demo_pkg
    source install/setup.bash

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch nav2_demo_pkg costmap_only.launch.py \
      map:=/workspace/demo_ws/src/nav2_demo_pkg/maps/map.yaml

实验记录：

调整方向	现象
减小膨胀半径	膨胀区域变窄，路径可能更贴近障碍物
增大膨胀半径	膨胀区域变宽，狭窄通道可能更难通过

4.2.3.1.3 全局路径规划

（零）全局路径规划理论（选读）

做完膨胀预处理后，规划部开始画路。

画路算法：A（A星）——怎么找到那条绿色线？ （提示：Nav2中最常用的全局规划算法叫 A。）

3句话讲明白A：想象你把地图画在方格纸上。A算法从起点格子开始，像水波一样向四周扩散探索，但它有个聪明策略——优先探索那些离终点更近且没有障碍物的格子。最终，它找到一条从起点到终点的最短路径——就是你在Rviz中看到的那条绿色长线。

关键点：全局规划器依赖“地图 + 定位 + 代价地图”。因此本节实验开始把前两节配置组合起来使用：amcl_params.yaml 负责定位，costmap_params.yaml 负责膨胀地图，planner_params.yaml 只负责规划器本身。

全局规划配置文件介绍（选读）：

文件路径：

/workspace/demo_ws/src/nav2_demo_pkg/config/planner_params.yaml

核心内容如下：

# Planner Server 配置：只包含全局路径规划器参数。
planner_server:
  ros__parameters:
    use_sim_time:  # 使用仿真时钟，需与 IsaacSim/Nav2 其它节点一致
    expected_planner_frequency:  # 期望规划频率；低于该频率会在日志中提示性能问题
    planner_plugins:  # 注册全局规划器插件列表

    GridBased:
      plugin:  # 全局规划器插件类型
      tolerance:  # 目标点容差范围
      use_astar:  # 是否使用启发式搜索策略
      allow_unknown:  # 允许规划穿过未知区域；教学仿真中更容易跑通

参数讲解：

参数	作用	配置注意
planner_plugins	注册全局规划器插件列表	需要与下方插件配置名称对应
plugin	全局规划器插件类型	需要选择当前环境已安装的 Nav2 规划器插件
use_astar	是否使用启发式搜索策略	会影响路径搜索方式和计算表现
tolerance	目标点附近允许的规划容差	影响目标不可精确到达时的规划结果
allow_unknown	是否允许路径穿过未知区域	需要结合地图质量和任务安全性判断

一句话总结：先把 A* 吃透，后续无论切换到哪种全局规划算法，底层的“在地图上找最短路径”逻辑都是一样的——只是搜索策略和约束条件不同而已。

本节使用 planner_test.launch.py 单独验证全局路径规划链路。这个启动文件会组合启动：

节点	作用
map_server	加载并发布地图
amcl	提供机器人在地图中的定位
planner_server	根据地图、定位和代价地图生成全局路径
lifecycle_manager_planner_test	激活上述生命周期节点
rviz2	显示地图、粒子云、代价地图和全局路径

文件路径：

/workspace/demo_ws/src/nav2_demo_pkg/launch/planner_test.launch.py

（一）实操：全局路径规划 Demo

本节只观察“全局路径是怎么生成的”。此时机器人不需要真正移动，重点是看清楚：在地图、定位和代价地图都正常之后，Planner Server 如何生成一条从当前位置到目标点的全局路径。

0. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_slam

1. 在 宿主机端 开启 X11 服务

需要确保 宿主机已经开启 X11 服务，我们可以通过下面的操作开启 X11 服务，确保 RViz 的正常可视化（如果有的时候 RViz 没能可视化，请先检查程序是否有自动开启 RViz 服务后再检查是否有开启 X11 服务）

    export DISPLAY=:1
    xhost +

2. 启动全局路径规划程序

在 ROS2 容器端运行：

    export DISPLAY=:1
    xhost +

    cd /workspace/demo_ws
    colcon build --symlink-install --packages-select nav2_demo_pkg
    source install/setup.bash

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch nav2_demo_pkg planner_test.launch.py \
      map:=/workspace/demo_ws/src/nav2_demo_pkg/maps/map.yaml

启动后，先在 RViz 中使用 2D Pose Estimate 给出机器人初始位姿，等待粒子云收敛。只有定位完成后，全局规划器才知道“从哪里开始规划”。

3. 请求全局路径的两种方式

方式一：使用 RViz 的 Nav2 Goal

这是最直观的观察方式：

具体操作：

     1） 选择 Nav2 Goal 按钮，然后在地图上的 点击并按住鼠标左键 指定机器人导航的目标点和目标朝向

完整操作演示如下：

方式二：在 Ros容器终端中手动调用 ComputePathToPose Action (这种方式与上面这种方式调用的接口一致，都是向 Planner 模块发送规划请求)

这种方式适合理解底层接口。全局规划本质上是把“目标位姿”交给规划器，让它返回一条 Path。

先确认 Action Server 是否存在：

ros2 action list | grep compute_path

然后手动发送规划请求：

  ros2 action send_goal /compute_path_to_pose nav2_msgs/action/ComputePathToPose \
    "{goal: {header: {frame_id: 'map'}, pose: {position: {x: <目标x>, y: <目标y>, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: <目标朝向z>, w: <目标朝向w>}}}, planner_id: '<规划器ID>'}"

其中：

• <目标x>、<目标y>：地图中可通行区域内的目标点

• <目标朝向z>、<目标朝向w>：目标朝向对应的四元数

• <规划器ID>：规划器配置中注册的插件名称

  操作演示与上一种方式一致，不做演示

---

4.2.3.2 Controller

规划部画好了绿色路线，现在轮到控制部（Controller Server）登场——它的职责是让机器人实际走起来，并实时应对动态变化。

4.2.3.2.1 局部路径控制

为什么有了绿线还不够？绿线是基于静态地图画的——它不知道仿真环境里突然冒出来的移动小球或临时行人。如果控制部只傻傻地沿着绿线走，遇到突然出现的障碍物就会撞上去。

所以控制部需要一边走，一边实时感知前方几米内的新障碍物，随时微调方向。

Nav2中常用的局部控制器叫 DWB（Dynamic Window Based），它的思想来自 DWA（动态窗口法）。

3句话讲明白DWA：DWA每0.1秒就算一次“接下来该用什么速度走”。它会在所有可能的速度组合里模拟预测——如果以这个速度往前走，0.5秒后会撞到障碍物吗？它选出不撞障碍物且最接近绿线方向的那组速度，发送给底盘执行。

你在Rviz中看到的机器人偏离绿线绕行，绕过后又回到绿线上——这就是局部控制器不断重新计算的结果。

（一）局部控制配置文件解析（选读）

文件路径：

/workspace/demo_ws/src/nav2_demo_pkg/config/controller_params.yaml

核心内容如下：

# Controller Server 配置：只包含局部控制器参数。
controller_server:
  ros__parameters:
    use_sim_time:  # 使用仿真时钟
    controller_frequency:  # 控制循环频率
    min_x_velocity_threshold:  # x 方向速度死区阈值，用于过滤微小抖动
    min_y_velocity_threshold:  # y 方向速度阈值；差速底盘不使用横向速度
    min_theta_velocity_threshold:  # 角速度死区阈值
    failure_tolerance:  # 控制器短时失败容忍时间，超过后上报失败

    progress_checker_plugin:  # 进度检查器插件名称
    goal_checker_plugins:  # 目标到达检查器列表
    controller_plugins:  # 路径跟随控制器插件列表

    progress_checker:
      plugin:  # 检查机器人是否持续向前推进
      required_movement_radius:  # 判断机器人是否持续移动的距离阈值
      movement_time_allowance:  # 判断卡住的时间窗口，单位秒

    general_goal_checker:
      plugin:  # 简单目标检查器
      stateful:  # 接近目标后保持状态，避免反复进入/退出到达状态
      xy_goal_tolerance:  # 位置到达容差，单位 m
      yaw_goal_tolerance:  # 朝向到达容差，单位 rad

    FollowPath:
      plugin:  # 局部控制器插件类型
      debug_trajectory_details:  # 输出轨迹评分细节，便于调参

      # === 速度限制 ===
      min_vel_x:  # 最小前进速度限制
      min_vel_y:  # 最小横向速度限制
      max_vel_x:  # 最大前进速度，单位 m/s
      max_vel_y:  # 最大横向速度限制
      max_vel_theta:  # 最大旋转角速度，单位 rad/s
      min_speed_xy:  # 平面合速度下限
      max_speed_xy:  # 平面合速度上限，应与 max_vel_x 匹配
      min_speed_theta:  # 旋转速度下限

      # === 加速度/减速度限制 ===
      acc_lim_x:  # x 方向最大加速度，影响起步快慢
      acc_lim_y:  # y 方向加速度限制
      acc_lim_theta:  # 最大角加速度，影响转向响应
      decel_lim_x:  # x 方向最大减速度，负值表示刹车
      decel_lim_y:  # y 方向减速度限制
      decel_lim_theta:  # 最大角减速度

      # === 轨迹采样与预测 ===
      vx_samples:  # 前进速度采样数量；越大轨迹选择越细但计算更重
      vy_samples:  # 横向速度采样数量；差速底盘保留较小值即可
      vtheta_samples:  # 角速度采样数量
      sim_time:  # 向前模拟运动轨迹的时间窗口
      linear_granularity:  # 轨迹线性采样间隔，单位 m
      angular_granularity:  # 轨迹角度采样间隔，单位 rad
      transform_tolerance:  # TF 查询时间容差
      xy_goal_tolerance:  # 控制器内部位置到达容差
      trans_stopped_velocity:  # 低于该平移速度时可认为接近停止
      short_circuit_trajectory_evaluation:  # 某轨迹明显不合格时提前停止评分，节省计算
      stateful:  # 保持控制器内部状态，提高到达目标时稳定性

      # === 轨迹评价函数 ===
      critics:
      BaseObstacle.scale:  # 避障评分权重
      PathAlign.scale:  # 朝向全局路径的评分权重
      PathAlign.forward_point_distance:  # 用于判断路径对齐的前向参考点距离
      GoalAlign.scale:  # 朝向目标点的评分权重
      GoalAlign.forward_point_distance:  # 用前方点判断目标对齐
      PathDist.scale:  # 偏离全局路径距离的评分权重
      GoalDist.scale:  # 距离目标点远近的评分权重
      RotateToGoal.scale:  # 到达目标附近后原地对准朝向的权重
      RotateToGoal.slowing_factor:  # 接近目标时旋转减速系数
      RotateToGoal.lookahead_time:  # 旋转到目标朝向时的前瞻时间设置

参数含义速查表：

参数	作用	调大效果	调小效果
controller_frequency	控制器计算频率	控制更新更频繁，计算负担增加	控制响应变慢
max_vel_x	最大前进速度	移动更快，对控制稳定性要求更高	移动更慢，通常更稳
max_vel_theta	最大旋转角速度	转向更灵敏	转向更平缓
acc_lim_x	线加速度限制	启动和刹车更快	启停更柔和
xy_goal_tolerance	位置到达容差	更容易判定到达	对到达精度要求更高
vx_samples / vtheta_samples	速度采样数量	轨迹选择更充分，计算更重	计算更轻，轨迹选择更粗糙
sim_time	轨迹预测时间	预测更远，计算更重	更关注近距离避障

（二）实操：局部控制配置文件与调参实验

本节开始需要机器人真正运动，因此会组合前面三个模块：AMCL 定位、代价地图、全局规划

0. 在 isaacsim 容器 并点击 开始仿真

    cd ~/robotac_isaac_assets
    ./integrated_runtime/run_demo_scene.sh --world bobac_slam

1. 在 宿主机端 开启 X11 服务

需要确保 宿主机已经开启 X11 服务，我们可以通过下面的操作开启 X11 服务，确保 RViz 的正常可视化（如果有的时候 RViz 没能可视化，请先检查程序是否有自动开启 RViz 服务后再检查是否有开启 X11 服务）

    export DISPLAY=:1
    xhost +

2. 启动例程

    export DISPLAY=:1
    xhost +
    cd /workspace/demo_ws
    colcon build --symlink-install --packages-select nav2_demo_pkg
    source install/setup.bash

    # 设置指定的 ros 话题空间（DOMAIN_ID），默认 bobac 接收发送话题空间编号为 44，可自行设置别的空间，但需要更改资产的 actiongraph
    export ROS_DOMAIN_ID=44

    ros2 launch nav2_demo_pkg controller_test.launch.py \
    map:=/workspace/demo_ws/src/nav2_demo_pkg/maps/map.yaml

3. 请求局部路径规划控制的两种方式

方式一：使用 RViz 的 Nav2 Goal

这是最直观的观察方式：

具体操作：

     选择 Nav2 Goal 按钮，然后在地图上的 点击并按住鼠标左键 指定机器人导航的目标点和目标朝向

完整操作演示如下：

最后我们就能看到全局（绿色）与局部规划（蓝色）两条路线，其中局部规划路线会同时控制着底盘进行移动，如下图所示：

方式二：直接在 Ros 容器终端中 用 Demo 脚本发送导航目标：

cd /workspace/demo_ws
source install/setup.bash
python3 src/nav2_demo_pkg/scripts/send_goal.py --x <目标x> --y <目标y> --yaw <目标朝向>

该脚本会向 /navigate_to_pose action server 发送 nav2_msgs/action/NavigateToPose 目标点，适合后续做自动任务流程。

---

4.2.3.3 Recovery （选读）

最后我们来认识应急部。规划部和控制部正常工作时，应急部不干活，处于待命状态。但当机器人被卡在墙角、或者迷路转圈时——应急部会立即介入：

下达“后退1米”指令

下达“原地旋转180度”指令

通知规划部“重新规划一条新路线”

对标人类行为：你走错岔路了，手机导航说“正在重新规划路线”——这就是应急恢复。

（一）Recovery Server 的可配置性 在较新的 Nav2 版本中，课程里常说的 Recovery Server 对应 behavior_server。它不是一个只能被动响应的“黑盒”，而是可以通过 YAML 参数灵活定义的模块。你不需要编写 C++ 代码，只需要通过配置文件指定在异常情况下执行哪些行为插件。

恢复行为包括：原地旋转、后退、沿当前朝向移动、等待等。Nav2 启动时会加载这些插件，行为树检测到异常后再调用它们。

（二）如何配置 Behavior Server

本节新增独立配置文件 behavior_params.yaml。它只包含恢复行为参数，不包含 AMCL、代价地图、规划器、控制器参数。启动实验时再把前面模块组合起来。

① 恢复行为配置文件

文件路径：

/workspace/demo_ws/src/nav2_demo_pkg/config/behavior_params.yaml

核心内容：

# Behavior Server 配置：只包含恢复行为参数。
behavior_server:
  ros__parameters:
    use_sim_time:  # 使用仿真时钟
    costmap_topic:  # 恢复动作碰撞检测使用的局部代价地图
    footprint_topic:  # 机器人轮廓话题，用于判断动作是否安全
    cycle_frequency:  # 恢复行为控制循环频率，单位 Hz
    behavior_plugins:  # 可被 BT 调用的恢复动作

    spin:
      plugin:  # 原地旋转，用于重新观察周围环境或调整朝向
    backup:
      plugin:  # 后退一段距离，用于脱离近距离障碍物
    drive_on_heading:
      plugin:  # 沿指定朝向短距离移动
    wait:
      plugin:  # 原地等待，给动态障碍物离开的时间

    global_frame:  # 恢复动作使用 odom 坐标系，短时间内连续稳定
    robot_base_frame:  # 机器人本体坐标系
    transform_tolerance:  # TF 查询时间容差
    simulate_ahead_time:  # 执行恢复动作前的碰撞预测时间窗口
    max_rotational_vel:  # 原地旋转最大角速度，单位 rad/s
    min_rotational_vel:  # 原地旋转最小角速度，太小容易转不动
    rotational_acc_lim:  # 旋转加速度限制，影响旋转起停平滑度

参数讲解：

参数	作用	配置注意
behavior_plugins	注册可被行为树调用的恢复动作	需要与下方插件配置对应
costmap_topic	恢复动作执行前用于碰撞检查的局部代价地图	需要与局部代价地图发布话题一致
footprint_topic	机器人轮廓话题	用于判断恢复动作是否安全
simulate_ahead_time	执行动作前的碰撞预测时间窗口	影响恢复动作安全性和保守程度
max_rotational_vel	原地旋转最大角速度	影响恢复动作转向速度
min_rotational_vel	原地旋转最小角速度	影响恢复动作是否能稳定起转
rotational_acc_lim	旋转加速度限制	影响恢复动作起停平滑度

（四）Recovery Server 的行为树触发机制（选读） 有同学可能会问：“Recovery Server 怎么知道机器人被卡住了？”

这背后是行为树（Behavior Tree）在监控。Nav2 的导航行为树中有一个专门的恢复节点，它会持续检测：

机器人是否长时间没有前进（横向速度/角速度异常低）

规划器是否连续多次无法规划出有效路径

控制器是否反复报告“接近障碍物但无法避让”

当这些条件被触发时，行为树会调用 behavior_server，按照配置的 behavior_plugins 执行恢复动作。

---

4.2.4 Nav2 整体整合 - 实现完整自主导航

  前面已经分别验证了建图、定位、代价地图、路径规划和局部控制。最后这一节不是给出比赛可直接使用的完整导航策略，而是把这些基础模块放到同一个启动流程中，验证 Nav2 的基本链路是否能够联动起来。

  本节实验的目标很明确：选手自行实现完整的导航功能，完整的 demo 程序我们也已给出（文件路径：/workspace/demo_ws/src/nav2_demo_pkg/launch/nav2_navigation.launch.py）作为参考，后续联调功能都需自行实现

---

Nav2 联动流程

1. 启动地图与 AMCL
   ↓
2. 在 RViz 中手动设置 2D Pose Estimate
   ↓
3. AMCL 根据地图、激光和里程计完成定位
   ↓
4. 启动 planner / controller / behavior / bt_navigator
   ↓
5. 手动发送导航目标
   ↓
6. 观察全局路径、局部代价地图和 /cmd_vel 输出

注意注意注意：这个流程只验证导航系统的基础组成，不包含自动目标选择、多点任务调度、场景级最优调参和比赛策略，具体内容需要选手自行调试设计。

总结回顾：

1. 建图：让机器人获得环境地图
2. 定位：让机器人知道自己在地图中的位置
3. 代价地图：把障碍物转换成规划可用的安全区域
4. 路径规划：生成从当前位置到目标点的全局路径
5. 局部控制：根据路径和局部障碍物输出 /cmd_vel
6. Nav2 联动：把上述模块接到同一个导航流程中进行验证