第 10 章 SLAM 与自主导航

SLAM（Simultaneous Localization and Mapping）让机器人在未知环境中边运动边建图，自主导航（Nav2）在已有地图上规划路径并避障行驶。两者串联构成移动机器人的核心自主能力。本章从里程计融合讲起，经 GNSS/RTK 绝对定位接入、slam_toolbox 建图、map_server 地图管理、Nav2 导航栈配置与最小配置示例，给出从传感器数据输入到机器人自主到达目标点的完整路径。

1. 里程计与传感器融合

差速驱动里程计（第 6 章 diff_drive_controller 发布的 ~/odom）通过轮子编码器推算位姿，存在两大误差源：轮子打滑引入的位移误差、轮距/半径参数不准引入的角度误差。误差随时间累积——纯里程计定位在数分钟后可能偏出数十厘米。

1.1 robot_localization 的 EKF 融合

robot_localization 包提供扩展/无迹卡尔曼滤波器（EKF/UKF），融合多路姿态源得到更准确的里程计输出。典型配置：差速里程计（/odom）+ IMU（/imu/data）→ 滤波后里程计（/odometry/filtered）。

## ekf.yaml
ekf_filter_node:
  ros__parameters:
    frequency: 30.0                # 融合输出频率 (Hz)
    two_d_mode: true               # 差速底盘约束 z/roll/pitch 为零
    publish_tf: true               # 融合结果发布为 odom → base_link TF

    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: /diff_drive/odometry
    odom0_config: [true, true, false,   # x, y, z 位置
                   false, false, false,  # roll, pitch, yaw
                   false, false, false,  # vx, vy, vz
                   false, false, false,  # vroll, vpitch, vyaw
                   false, false, false]  # ax, ay, az

    imu0: /imu/data
    imu0_config: [false, false, false,
                  true, true, true,     # 取姿态（roll/pitch/yaw）
                  false, false, false,
                  true, true, false,    # 取角速度（vroll, vpitch）
                  true, true, false]    # 取线性加速度
    imu0_differential: false
    imu0_relative: true               # 使用 IMU 增量而非绝对值

启动（robot_localization 为每个滤波器提供 ekf.launch.py / ukf.launch.py；launch 文件固定加载包内 params/ekf.yaml，没有暴露参数文件参数）：

## 把自己的配置放到 robot_localization 包的 params/ekf.yaml，
## 或自写 launch 直接给 ekf_node 的 parameters 传入文件路径
ros2 launch robot_localization ekf.launch.py

robot_localization 的 Humble 包为 ros-humble-robot-localization；ekf.launch.py 启动 ekf_node，节点名 /ekf_filter_node，输出话题 /odometry/filtered。

two_d_mode: true 将 z 轴、roll、pitch 状态强制为零（平面运动假设），避免非完整约束的数值发散。融合结果通过 TF 覆盖原始 odom → base_link——下游 SLAM 与导航直接使用滤波后的位姿。

坑：经纬度不能直接进 EKF

EKF 吃的是本地平面坐标；NavSatFix 要先经 navsat_transform_node 转成局部平面位姿并做原点（datum）对齐，否则滤波输入量纲都不对。

1.2 GNSS/RTK 观测与质量分档

轮式里程计与 IMU 都是相对观测——上面记下的累积漂移没有任何外部基准能拉回。GNSS（Global Navigation Satellite System，全球导航卫星系统）提供绝对位置：接收机输出的是 WGS 84 参考椭球上的经纬度与高程，ROS 侧的标准载体是 sensor_msgs/msg/NavSatFix（sensor_msgs 定义）：

字段	含义与约定
latitude / longitude / altitude	度、度、米（相对 WGS 84 椭球面，无值时为 NaN）
header.frame_id	天线所在的机体相对帧——天线到 base_link 的偏移（lever arm，杆臂）必须用静态 TF 表达，否则绝对位置整体带一个固定偏置
position_covariance	m² 的 3×3 协方差，定义在过该点的切平面上、按 ENU（East-North-Up，东-北-天） 行主序排列；消息注释明确该坐标系在极点奇异
position_covariance_type	UNKNOWN / APPROXIMATED（只有 DOP 时估算）/ DIAGONAL_KNOWN / KNOWN
status	NavSatStatus，给出所用系统（GPS / GLONASS / 北斗 / Galileo）与服务位

精度不是一个数，而是分档的质量。NMEA 0183（接收机默认输出的 ASCII 文本协议）的 GGA 语句第 6 字段——定位质量标识——是这一分档的原始出处，接收机据此自述当前解算状态：

质量码	状态	解算方式	精度量级
0	Invalid	无有效解	—
1	Autonomous	单点定位（码伪距）	米级
2	DGPS	伪距差分改正	亚米级
4	RTK Fixed	载波相位差分，整周模糊度已固定	厘米级
5	RTK Float	载波相位差分，模糊度未固定	分米级
6	Estimated / DR	推算（惯导兜底）	随时间发散

RTK（Real-Time Kinematic，实时动态载波相位差分）的两项前提：一是载波相位观测量（波长远小于码伪距的分辨率，代价是多了“整周数未知”的模糊度）；二是基准站同视卫星的观测值差分——星历误差、卫星钟差与大气延迟在短基线上相减抵消，剩下的就是可固定的模糊度。改正数以 RTCM（海事无线电技术委员会建议标准 10403.x，二进制）下发，链路走数传电台或 NTRIP（Networked Transport of RTCM via Internet Protocol，基于 TCP/IP 的挂载点订阅）。NMEA 与 RTCM 的分工是状态层与改正层：NMEA 告诉主机“这个解可不可信”，RTCM 才是让解变准的数据。

上表的精度量级与状态递进取自规范与接收机资料的表述；具体型号与收敛时间取决于实际接收机。RTK 的观测方程与模糊度搜索算法属测量学内容，不在本板块覆盖。

工程后果直接落到融合上：质量档跳变（4→5→1）意味着观测的统计特性突变，而 EKF 假设量测噪声平稳——把 RTK 观测当恒定精度喂进滤波器是常见错配置。

1.3 navsat_transform 与双 EKF

NavSatFix 不能直接进 EKF——滤波器状态是本地平面坐标，观测是椭球经纬度。robot_localization 的 navsat_transform_node 承担这一转换。按 robot_localization 文档，该节点需要三件输入才能定出“全局原点对应本地哪里、两框差多少转角”：

1. 世界参考的航向：IMU（sensor_msgs/Imu）、或置 use_odometry_yaw: true 用里程计航向、或 datum 服务给定；
2. 机器人在世界框下的位姿：通常来自状态估计节点的 nav_msgs/Odometry；
3. 经纬高：sensor_msgs/NavSatFix。

原点也可固定：datum 参数或 SetDatum 服务把某经纬度设为原点、X 轴朝东，wait_for_datum: true 让节点等原点而非自动学习。输出是世界框下的 odometry（即 GPS 转成的本地平面位姿）。

室外融合的推荐拓扑是双 EKF（该包自带 dual-ekf + navsat 示例）：

flowchart TD
    odom["diff_drive odom"] --> ekfa["EKF-A<br/>world_frame: odom"]
    imu["IMU"] --> ekfa
    ekfa -->|"/odometry/filtered<br/>odom → base_link"| ekfb["EKF-B<br/>world_frame: map"]
    gps["NavSatFix"] --> ns["navsat_transform_node"]
    ekfa -.->|"位姿用于原点对齐（datum）"| ns
    ns -->|"GPS 转本地平面位姿"| ekfb
    ekfb --> out["/odometry/filtered<br/>map → base_link"]

## ekf_map.yaml（EKF-B 片段：全局滤波器）
ekf_filter_node_map:
  ros__parameters:
    world_frame: map            # B 发 map → base_link；odom → base_link 归 A，两个 world_frame 不能相同
    odom0: /odometry/filtered   # A 的高频连续输出：只取速度，不取位置
    odom0_config: [false, false, false, false, false, false,
                   true, true, true, false, false, false,
                   false, false, false]

    odom1: /navsat_transform/odometry/gps   # GPS 转成的本地平面位姿
    odom1_config: [true, true, false,            # 只取东-北两轴位置
                   false, false, false,
                   false, false, false,
                   false, false, false,
                   false, false, false]
    odom1_differential: false   # 绝对位置用绝对值，不取差分

三条判据：

• 两个滤波器分工：A 吃连续源（odom + IMU）负责高频与短期精度，B 吃离散跳变的全局源负责拉回长期漂移。直接把 GPS 塞进单滤波器，一次多路径跳变会通过 TF 污染 odom → base_link，下游局部避障跟着抖。
• 量测噪声按质量档给：NavSatFix 自带 position_covariance，驱动应把 4 / 5 / 1 档映射成不同协方差（固定解给小、浮点与单点给大），B 侧信任度随档变化；写死常量要么过信要么浪费。
• world_frame 与 TF 唯一性：同一 parent → child 只能有一个 TF 发布者。A 发 odom → base_link、B 也发同一条边会让 TF 树出现重复源——B 置 world_frame: map 与 A 的 odom 正是按这一约束错开（TF 规则见第 5 章）。

室内（无天空视野）与室外是同一条链的两种运行状态：RTK 需要开阔天空、稳定的载波相位跟踪与良好卫星几何，楼间与窗边几乎不会看到质量码 4。质量码长期在 1 与 2 之间跳动，指向多路径干扰或天线安装问题——与建图侧的“重影”是两类不同的现象。

判据：双 EKF 的 world_frame 不能相同

EKF-A 管 odom → base_link（连续源），EKF-B 管 map → base_link（全局源）；两个滤波器写同一个 world_frame 会抢发同一段 TF。

2. slam_toolbox 建图

slam_toolbox（Humble 包 ros-humble-slam-toolbox，提供 online_async_launch.py 等 launch 入口）是 ROS 2 默认的 2D SLAM 实现，基于图优化 + 子地图匹配，支持在线与离线两种模式。

2.1 在线建图最小启动

ros2 launch slam_toolbox online_async_launch.py \
  slam_params_file:=my_slam.yaml \
  use_sim_time:=true

输入话题：/scan（LaserScan）、TF 中 odom → base_link（里程计提供）。slam_toolbox 内部维护扫描匹配 + 图优化，输出 map → odom 的 TF 修正与 /map（nav_msgs/OccupancyGrid）。

RViz2 中观察建图过程：Fixed Frame 设为 map，添加 Map + LaserScan + TF Display——机器人移动时栅格地图实时扩展。

2.2 关键参数

## my_slam.yaml
slam_toolbox:
  ros__parameters:
    solver_plugin: solver_plugins::CeresSolver
    resolution: 0.05              # 地图分辨率 (m/cell)
    max_range: 12.0               # 超过此距离的激光点不入库
    minimum_travel_distance: 0.3  # 移动 0.3m 或旋转 15° 时插入关键帧
    loop_match_minimum_response_fine: 0.45  # 回环检测阈值

回环检测（loop closure） 是 slam_toolbox 的核心能力——机器人回到之前经过的位置时，识别匹配并修正累积误差。未触发回环时地图会出现"重影"（同一面墙出现两层）。

机制上分两步：先在历史关键帧里找候选，再做粗/细两档扫描匹配确认——候选在搜索空间内产生（loop_search_space_dimension），确认要同时过链长度（loop_match_minimum_chain_size，默认 10 个连续关键帧）与响应阈值（loop_match_minimum_response_coarse / _fine，my_slam.yaml 里调的是 fine），过了才在图里加一条回环边交给优化器；loop_search_maximum_distance 限回环的搜索半径，do_loop_closing 可整体关闭。判据上误回环比漏回环更贵——把两个不同地点焊成一个节点会让整张图扭曲且难恢复，所以链长度与阈值宁严勿松；阈值放得过松的典型症状是长直走廊里两段相似墙体被误配。回环在"跟踪 / 建图 / 重定位"中的一般位置见 camera-note 第 3 章「特征在跟踪 / 建图 / 重定位中的分工」。

2.3 与 cartographer 对比

Google Cartographer（分支支持 ROS 2）提供 submapping + branch-and-bound 回环，在大规模场景下内存管理更精细，但配置复杂度远高于 slam_toolbox。对 2D 激光雷达 + 室内中小场景，slam_toolbox 开箱即用是首选；需要多传感器融合（2D LiDAR + IMU + 视觉）或子地图层级管理时评估 Cartographer。

3. 地图保存与加载

3.1 保存

建图完成后用 slam_toolbox 自带的序列化服务保存：

## 保存为 posegraph（可继续增量建图）
ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph \
  "{filename: 'my_building'}"
## 生成 my_building.posegraph + my_building.data

## 保存为栅格图（用于 map_server 加载；map 话题经 remap 指定，不是参数）
ros2 run nav2_map_server map_saver_cli -f my_map --ros-args -r map_topic:=/map
## 生成 my_map.pgm + my_map.yaml

3.2 加载与 map_server

map_server 将保存的栅格图发布为 /map 话题；map → odom 变换由 AMCL 或 SLAM 节点发布，map_server 本身不发布 TF：

ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=my_map.yaml
ros2 lifecycle set /map_server configure
ros2 lifecycle set /map_server activate

my_map.yaml 格式：

image: my_map.pgm
resolution: 0.05          # m/pixel
origin: [-10.0, -10.0, 0.0]  # 地图左下角在 map 坐标系中的位置
negate: 0                 # 不反色
occupied_thresh: 0.65     # 灰度 > 0.65 视为占用
free_thresh: 0.196        # 灰度 < 0.196 视为空闲

OccupancyGrid 消息的值域：-1（未知）、0（空闲）、100（占用）；map_server 从 PGM 灰度按阈值映射。

4. Nav2 导航栈

Nav2（Navigation2）是 ROS 2 的自主导航框架，由多个独立节点通过行为树（Behavior Tree）协调。

4.1 核心组件

组件	话题/接口	职责
BT Navigator	/navigate_to_pose (action)	接收目标、运行行为树调度各模块
Planner	/compute_path_to_pose	全局路径规划（NavFn / Smac Planner）
Controller (Follower)	/follow_path → /cmd_vel	局部路径跟随，输出速度命令
Costmap2D	/global_costmap, /local_costmap	多层代价地图（静态 + 传感器 + 膨胀）
AMCL	/amcl_pose + TF map → odom	Monte Carlo 定位（已知地图上的粒子滤波定位）
Behavior Server	恢复行为（旋转、后退、等待）	路径阻塞或跟踪失败时的应对策略

4.2 最小配置

完整 nav2_params.yaml 片段（点击展开）

## nav2_params.yaml（精简关键项；Humble 1.1.x 的到达目标 BT 参数为 default_nav_to_pose_bt_xml）
bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    default_nav_to_pose_bt_xml: "navigate_to_pose_w_replanning_and_recovery.xml"

controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      max_vel_x: 0.5           # 最大线速度 (m/s)
      max_vel_theta: 1.0       # 最大角速度 (rad/s)
      acc_lim_x: 0.5           # 加速度限制

global_costmap:
  global_costmap:
    ros__parameters:
      global_frame: map
      robot_base_frame: base_link
      resolution: 0.05
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

amcl:
  ros__parameters:
    global_frame_id: map
    base_frame_id: base_link
    odom_frame_id: odom
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    scan_topic: /scan

启动全链路（仿真环境；nav2_bringup 提供 navigation/localization/slam 的 launch 入口）：

ros2 launch nav2_bringup navigation_launch.py \
  params_file:=nav2_params.yaml use_sim_time:=true

ros2 launch nav2_bringup slam_launch.py       # 若用 SLAM 定位而非 AMCL
ros2 launch nav2_bringup localization_launch.py  # 若用已知地图 + AMCL

发送目标点（RViz2 中用 2D Goal Pose 工具，或命令行）：

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \
  "{pose: {header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 1.0}, orientation: {w: 1.0}}}}"

4.3 代价地图

• 全局代价地图（global_costmap）：静态地图（墙体）+ 传感器实时障碍 + 膨胀层（robot_radius 范围内的障碍标记为不可通行）。规划器在此图上搜索全局路径。
• 局部代价地图（local_costmap）：短程窗口（通常 3~5m 分辨率 0.05m），仅包含传感器实时数据。控制器在此图上做局部避障。

膨胀半径（inflation_radius）应设为机器人外接圆半径 + 安全余量。过小导致路径擦墙（跟踪振荡），过大导致窄通道无法通过。

4.4 行为树与 BehaviorTree.CPP

Nav2 的「决策层」是行为树，不是硬编码状态机：BT Navigator 用一个 XML 描述的分支结构调度 Planner/Controller/恢复行为，指定入口就是 4.2 里的 default_nav_to_pose_bt_xml。执行引擎是 BehaviorTree.CPP（Nav2 经 nav2_behavior_tree 封了一层动作/服务/话题节点，包名 behaviortree_cpp_v3）。

XML 就是一棵树：叶子是具体动作，内部分是控制流——Sequence（依次成功才成功）、Fallback（任一成功即成功，恢复序列靠它串）、Decorator（RateController/GoalUpdated/RecoveryNode 一类修饰）。节点间数据走 blackboard，XML 里 {goal}、{path} 这类变量在运行时绑定：

<!-- my_nav.xml：先规划再跟随；任一失败就转恢复序列 -->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Fallback name="root">
      <Sequence name="navigate">
        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
        <FollowPath path="{path}" controller_id="FollowPath"/>
      </Sequence>
      <Sequence name="recover">
        <Spin spin_dist="1.57"/>
        <Wait wait_duration="2"/>
      </Sequence>
    </Fallback>
  </BehaviorTree>
</root>

自写节点只在“现有动作不够用”时才写：一个同步动作只需继承 SyncActionNode 并实现 tick()（返回 SUCCESS/FAILURE/RUNNING 三态），端口用 providedPorts() 声明：

class SayHello : public BT::SyncActionNode {
 public:
  SayHello(const std::string& name, const BT::NodeConfiguration& cfg)
      : BT::SyncActionNode(name, cfg) {}
  static BT::PortsList providedPorts() { return {}; }
  BT::NodeStatus tick() override {
    std::cout << "hello" << std::endl;
    return BT::NodeStatus::SUCCESS;
  }
};
// 注册后 XML 里写 <SayHello/> 就能加载
factory.registerNodeType<SayHello>("SayHello");

在 Nav2 里用自写节点多一步注册：把节点编译成插件库，再把库名加进 plugin_lib_names 参数，否则 XML 加载时报找不到节点。两种改法对应两类需求：换编排不改代码——调恢复链顺序、加限速分支、失败时插入重试，全在 XML 里完成；加新动作才写插件。判据就一条：现有动作重新组合用 XML，要接一个新 action 服务才下沉 C++。

5. 重定位与初始化

AMCL（Adaptive Monte Carlo Localization）在已知地图上做粒子滤波定位。初始化方式：

• 初始位姿估计（Initial Pose）：RViz2 工具栏点击后发布 /initialpose，AMCL 在该区域撒粒子开始收敛。
• 自动恢复：/amcl_pose 的协方差过大（粒子发散）时，AMCL 的 recovery_alpha_slow / recovery_alpha_fast 参数控制随机重撒速率。

## 命令行发布初始位姿
ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
  "{header: {frame_id: 'map'}, pose: {pose: {position: {x: 0.0, y: 0.0}, orientation: {w: 1.0}}}}"

若完全丢失（机器人被搬动），需手动给初始位姿或用 global_localization service（/reinitialize_global_localization）在所有空闲位置均匀撒粒子重新搜索。

6. 感知结果进入导航

6.1 障碍与语义进 costmap

代价地图只默认消费静态地图与原始传感器（LaserScan / PointCloud2）；检测、分割与语义地图要进入导航，得接到 costmap 的层机制上。

接入点在层的装配列表。costmap 的 plugins 按顺序装配：static_layer（静态栅格）、obstacle_layer / voxel_layer（传感器实时障碍，后者带高度分层，能区分地面与桌面级障碍）、inflation_layer（膨胀）。传感器层以 observation_sources 声明观测源，每个源给 topic、data_type、sensor_frame、marking（命中写障碍）、clearing（沿射线清自由空间）与 max_obstacle_height。外部感知因此有两条路：把感知输出的障碍点云或代价话题接成一个观测源（最小改动）；或写自定义层插件（派生 nav2_costmap_2d::Layer 进 plugins 列表，自由度最大，代价是插件生命周期与参数管理）。

语义层两个形态。区域语义走 costmap 的过滤器插件：filters: ["keepout_filter"]，插件类为 nav2_costmap_2d::KeepoutFilter / SpeedFilter；掩膜由 nav2_map_server 的 filter_mask_server 发布（topic_name 在线或 yaml_filename 离线掩膜文件），costmap_filter_info_server 给出过滤类型与换算系数（type/base/multiplier）——keepout 禁止通行、speed filter 对区域限速，适合"施工区不许进、门口减速"这类人工规则。类别→代价的语义层（"人周围 0.8 m 高价、草地可过"）则要自定义层或由外部节点把检测/分割结果（camera-note 第 7 章「任务族与输出粒度」）翻译成代价话题再接入；这一层最容易写错的地方，是把像素级掩膜直接当栅格代价发进 costmap——投影、分辨率与 frame 对齐三件事少一件，代价图就和现实错位。

导航侧的契约。三条硬条件与抓取侧（manipulation-note 第 6 章「感知输出到抓取输入的接口形状」）互为镜像：sensor_frame 必须能在 TF 里查到并链到 costmap 的 global_frame；数据带时间戳，costmap 按 transform_tolerance 容忍传输与变换延迟、超容忍即丢帧；update_frequency 要匹配感知帧率（10 Hz 感知 + 5 Hz costmap 是常见配比，反过来配会让代价地图停留在旧图上）。QoS 同样要对齐：传感器类数据用 best-effort，感知节点若以 reliable 发布而层内按 best-effort 订阅，两端在发现阶段就会报 QoS 不兼容。

6.2 重定位与加速感知栈

感知触发重定位。视觉位置识别（camera-note 第 7 章「视觉位置识别与基础模型」）命中已知地点时给出候选位姿，发到 /initialpose 让 AMCL 收敛——这条路把"感知认为自己在哪"与"滤波器认为自己在哪"接起来。错配的代价是粒子撒到错误区域，因此工程形态是 VPR 只给候选（多帧一致或人工确认后才下发），不直接改写 map → odom 变换；反向的触发也存在：AMCL 协方差持续过大（粒子发散）可作为召回 VPR 复查的信号——两个方向的判据都落在协方差与置信度上，而不是"画面看起来像"。

GPU 加速感知栈（Isaac ROS）。需要稠密三维感知又装得起 GPU 时，前面两块（建图、障碍进 costmap）可以下沉到 NVIDIA 的 Isaac ROS 包集：nvblox 在 GPU 上维护 TSDF/ESDF 与占据栅格，输出可直接接成 costmap 层或 observation source；Visual SLAM（cuVSLAM） 做视觉惯性里程计，发布 odom → base_link 与回环后的位姿图；图像预处理（矫正、去畸变）也有加速版。形态上是 ROS 2 节点集，两条安装路径：

# 路径一：apt 单包装（Humble）
sudo apt install ros-humble-isaac-ros-nvblox
# 路径二：NGC 容器（推荐整体环境一致，含 NITROS 零拷贝栈）
docker run --gpus all -it --rm --net=host \
  nvcr.io/nvidia/isaac-ros/isaac_ros_nvblox:<tag>
# 运行示例：示例 bringup 包按传感器给 launch（Realsense 等），内部已接好话题与 TF
ros2 launch nvblox_examples_bringup realsense_example.launch.py

话题接口与普通 ROS 2 节点一致，所以 6.1 的契约原样适用：frame_id、时间戳、QoS 与 costmap 的更新频率依旧要对齐；nvblox 吐出的 ESDF/网格/切片话题接给 costmap 层或自定义语义层，路径与“自己写传感器层插件”完全同构，区别只在计算发生在 GPU 上。

判据：二维激光导航或算力受限的底盘用不到它；需要实时稠密建图 + 三维避障 + 视觉定位（室内服务机器人、无人机）且预算里有 Jetson/独显时，再把这块装上——它替代的是“自己写 GPU 建图”，不是 Nav2 本身。

7. 全链路衔接

本章涉及的模块与前序各章的依赖关系（衔接链）：

传感器驱动（第 7 章）→ /scan, /imu/data, /odom, /fix
                              ↓
ros2_control（第 6 章）→ /cmd_vel → 底盘硬件/仿真
                              ↓
navsat_transform → /fix 转本地平面位姿（室外）
                              ↓
robot_localization → /odometry/filtered + TF(odom→base_link) / TF(map→base_link)
                              ↓
slam_toolbox / AMCL → /map + TF(map→odom)
                              ↓
Nav2 → /cmd_vel → 回到 ros2_control
                              ↓
RViz2 / Foxglove（第 4 章）← 全链路话题可视化

仿真中接通全链路（第 9 章 gz sim）：所有节点设 use_sim_time := true、gz sim 的 DiffDrive 插件发布 odom、LiDAR 插件发布 scan——SLAM 与 Nav2 在仿真中接通后再迁移到真机，只需替换传感器话题名与 TF frame。

8. 自研还是用框架

场景	建议
2D 室内导航、非结构化工厂场景	Nav2 + slam_toolbox——社区成熟、调试工具完善
需要定制行为逻辑（多机协调、动态任务分配）	Nav2 行为树可替换节点，不必整体重写
3D 环境（多层、坡道、三维定位）	平面地图 + 2D 建图在任何场景都能先跑；确认障碍的高度信息影响通行时再上——只顾避障不影响路径时，voxel_layer 的三维体素投影回二维代价地图就够，不必整套 3D SLAM；确需三维定位时走 RTAB-Map（RGB-D/双目 + 图优化）或 LIO 类（激光与 IMU 紧耦合），在 ROS 2 里以外部节点形态接入：自己发布 map → odom 与点云/栅格，Nav2 规划与控制不变。算法细讲不在本板块
学术研究（新算法试验）	自写 planner/controller 节点，只复用 costmap + TF + Nav2 行为树骨架
嵌入式/资源受限	Nav2 全栈较重（节点多、内存大）；评估精简为 recovery + DWB + A* 的轻量组合

用框架不等于黑箱——Nav2 每个组件都暴露为独立节点与话题/服务接口，可逐个替换。自写的代价不是"从零造轮子"，而是"把框架内置的容错、热更新与社区检验重做一遍"。

9. 要点

• 纯里程计误差累积是 SLAM/定位存在的根本原因；robot_localization EKF 融合 odom + IMU 输出平滑位姿，是下游算法的稳定输入。
• GNSS 是唯一的全局绝对观测，但它的精度是质量档而非常量：RTK 固定/浮点/单点由 NMEA GGA 质量码自述，navsat_transform_node 负责把经纬度变成 EKF 能吃的本地平面位姿，双 EKF 把连续源与跳变源分开跑。
• slam_toolbox 在线建图 + 回环检测是 2D 场景的开箱方案；建图质量取决于回环触发频率——minimum_travel_distance 与场景尺度需匹配。
• map_server 发布的栅格图供 Nav2 costmap 与 AMCL 共同使用；PGM + YAML 是跨版本通用的地图归档格式。
• Nav2 组件化设计——BT Navigator 调度、Planner 出全局路径、Controller 转速度命令、Costmap 融合障碍、AMCL 定位——每个环节可独立替换。
• 感知结果进入导航走 costmap 的层：observation_sources 最小改动、自定义层最大自由度；区域语义用 keepout/speed filter，类别语义要自定义层或外部合成代价话题；感知触发重定位只给 /initialpose 候选，不改写 map → odom。
• 全链路先在仿真中打通（use_sim_time + gz sim 传感器）后再上真机；接口（话题名、frame_id、QoS）保持一致即可无缝切换。
• 框架提供的是"90% 场景的默认解"；剩余 10% 的定制（特殊运动学、多机协调、非标准传感器）在 Nav2 接口上扩展节点，而非重写整栈。

参考文献与延伸阅读

• Nav2. 文档入口. 文档
• slam_toolbox. 建图与定位仓库. 仓库
• ROS 2. Humble 文档. 文档