本文由 linuxROS 整理发布，首发于 linuxros.cn，转载请注明出处。

ROS2 SLAM与Navigation2：从建图到自主导航

> 讲解SLAM Toolbox建图、Navigation2导航架构、AMCL定位、代价地图、路径规划与行为树编排。基于Jazzy Jalisco LTS，所有命令可直接运行验证。

一、SLAM概述

SLAM（Simultaneous Localization and Mapping）解决一个鸡生蛋的问题：机器人需要地图来定位，又需要位姿来建图。SLAM同时求解两者。

输入与输出

输入：传感器数据（激光雷达 / 深度相机 / IMU）
输出：栅格地图（OccupancyGrid）+ 机器人位姿（Pose）

ROS2推荐方案

ROS1时代主流是GMapping和Cartographer，到了ROS2 Jazzy，官方推荐SLAM Toolbox——纯ROS2原生实现，支持在线离线建图、地图合并、重定位，配置比Cartographer简单得多。

SLAM算法对比

特性	SLAM Toolbox	Cartographer	RTAB-Map
传感器	2D激光	2D/3D激光+IMU	RGB-D + 2D激光
ROS2支持	原生	需编译	原生
建图模式	在线/离线	在线/离线	在线
地图合并	支持	不支持	支持
回环检测	支持	支持	支持（视觉）
配置复杂度	低	高	中
适用场景	室内2D导航	多层/大场景	视觉SLAM

二、SLAM Toolbox实战

安装

sudo apt install ros-jazzy-slam-toolbox

在线建图（online_sync模式）

online_sync是同步模式，每帧激光数据都处理，建图精度高但延迟略大。对大多数室内场景够用。

# 终端1：启动机器人底盘+激光雷达（以TurtleBot3为例）
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_bringup robot.launch.py

# 终端2：启动SLAM Toolbox
ros2 launch slam_toolbox online_sync_launch.py \
    slam_params_file:=/path/to/mapper_params_online_sync.yaml \
    use_sim_time:=false

保存地图

# 建图完成后保存
ros2 run nav2_map_server map_saver_cli -f ~/maps/my_map
# 生成两个文件：my_map.pgm（图像）+ my_map.yaml（元数据）

生成的YAML文件内容：

# my_map.yaml - 自动生成，一般不需要手动修改
image: my_map.pgm
resolution: 0.050000   # 每像素5cm
origin: [-10.0, -10.0, 0.0]  # 地图左下角在世界坐标的位姿
negate: 0
occupied_thresh: 0.65  # 占用阈值
free_thresh: 0.196     # 空闲阈值

加载地图定位

建好图后，机器人再次开机不需要重新建图，加载已有地图+AMCL定位即可。

# 启动地图服务器
ros2 run nav2_map_server map_server --ros-args \
    -p yaml_filename:=~/maps/my_map.yaml

核心参数配置

# mapper_params_online_sync.yaml
slam_toolbox:
  ros__parameters:
    # 求解器参数
    solver_plugin: solver_plugins::CeresSolver   # Ceres优化器，精度高
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY

    # 建图参数
    max_laser_range: 20.0       # 激光最大有效距离(m)
    minimum_time_interval: 0.5  # 最小处理间隔(s)
    transform_publish_period: 0.02  # TF发布频率50Hz

    # 地图更新
    resolution: 0.05            # 地图分辨率(m/像素)
    map_update_interval: 5.0    # 地图更新间隔(s)

    # 回环检测
    enable_interactive_mode: true
    range_max: 20.0
    minimum_travel_distance: 0.5   # 最小移动距离触发更新(m)
    minimum_travel_heading: 0.5    # 最小旋转角度触发更新(rad)

三、Navigation2架构

Nav2不是单个节点，是一组协作的服务+行为树编排器。

架构图

flowchart TB subgraph UI["用户交互层"] RVIZ["RViz2 2D Nav Goal"] API["Nav2 API 程序化调用"] end subgraph BT["行为树导航器"] BTN["BehaviorTree Navigator"] end subgraph CORE["规划与控制"] PLAN["PlannerServer 全局路径规划"] CTRL["ControllerServer 局部运动控制"] RECOV["RecoveryServer 恢复行为"] SMOOTH["SmootherServer 路径平滑"] end subgraph PER["感知与定位"] AMCL["AMCL 蒙特卡洛定位"] COST["Costmap2D 代价地图"] MAP["MapServer 地图服务"] end UI --> BTN BTN --> PLAN BTN --> CTRL BTN --> RECOV BTN --> SMOOTH PLAN --> COST CTRL --> COST COST --> AMCL AMCL --> MAP style UI fill:#E3F2FD,stroke:#1565C0 style BT fill:#FFF8E1,stroke:#F9A825 style CORE fill:#F3E5F5,stroke:#7B1FA2 style PER fill:#E8F5E9,stroke:#388E3C

各模块职责

模块	职责	关键话题
MapServer	加载/保存栅格地图	`/map`
AMCL	粒子滤波定位，输出map→odom变换	`/tf`
Costmap2D	融合静态地图+动态障碍物+膨胀层	`/global_costmap/costmap`
PlannerServer	全局路径搜索（A*/Dijkstra等）	`/plan`
ControllerServer	局部路径跟踪+避障	`/cmd_vel`
BehaviorTree	编排导航任务流程	无
RecoveryServer	卡住时执行恢复动作	无
SmootherServer	平滑全局路径	`/smoothed_path`

四、AMCL定位

AMCL（Adaptive Monte Carlo Localization）是粒子滤波定位算法。机器人不需要GPS，靠激光扫描和已知地图就能算出自己在哪。

粒子滤波原理

在地图上撒一堆粒子（每个粒子代表一个可能位姿）
用激光扫描匹配每个粒子的位置，计算权重
权重高的粒子附近多采样，权重低的淘汰
粒子收敛到真实位姿

关键参数

参数	默认值	说明
`min_particles`	200	最小粒子数
`max_particles`	5000	最大粒子数
`update_min_d`	0.25m	移动多远触发更新
`update_min_a`	0.2rad	旋转多大触发更新
`resample_interval`	1	每N次更新重采样一次
`alpha1`	0.2	旋转噪声（旋转方向）
`alpha2`	0.2	旋转噪声（平移方向）

命令行验证

# 查看AMCL发布的TF变换
ros2 run tf2_ros tf2_echo map base_link

# 查看粒子云
ros2 topic echo /particlecloud --once

# 手动设置初始位姿（RViz2 2D Pose Estimate等同步）
ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
"{
  header: {frame_id: 'map'},
  pose: {
    pose: {
      position: {x: 0.0, y: 0.0, z: 0.0},
      orientation: {w: 1.0}
    }
  }
}"

五、代价地图（Costmap2D）

代价地图是导航的核心数据结构，告诉规划器哪里能走、哪里不能走。

来自 linuxros.cn · linuxROS

全局 vs 局部

维度	全局代价地图	局部代价地图
用途	全局路径规划	局部避障控制
范围	整张地图	机器人周围几米
更新频率	较低（1~2Hz）	较高（5~20Hz）
障碍物来源	静态地图为主	实时传感器为主

代价地图层级

flowchart TB A["静态层 StaticLayer"] --> D["代价地图 Costmap2D"] B["障碍物层 ObstacleLayer"] --> D C["膨胀层 InflationLayer"] --> D A1["来源：地图服务器"] --> A B1["来源：激光/深度相机"] --> B C1["来源：膨胀半径计算"] --> C style A fill:#E3F2FD,stroke:#1565C0 style B fill:#FFEBEE,stroke:#D32F2F style C fill:#FFF8E1,stroke:#F9A825 style D fill:#E8F5E9,stroke:#388E3C

静态层：从MapServer加载的已知地图，不会变
障碍物层：传感器实时检测到的障碍物，会动态更新
膨胀层：在障碍物周围按指数衰减生成代价梯度，让机器人保持安全距离

关键参数

# nav2_params.yaml - 代价地图核心参数
global_costmap:
  ros__parameters:
    update_frequency: 1.0       # 全局地图更新频率(Hz)
    publish_frequency: 1.0      # 发布频率(Hz)
    resolution: 0.05            # 分辨率(m/像素)
    robot_radius: 0.22          # 圆形机器人半径(m)
    inflation_radius: 0.55      # 膨胀半径(m)
    cost_scaling_factor: 10.0   # 膨胀衰减系数，越大衰减越快
    # 层级配置
    plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

    obstacle_layer:
      plugin: "nav2_costmap_2d::ObstacleLayer"
      observation_sources: scan   # 数据源
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: true            # 用于清除障碍物
        marking: true             # 用于标记障碍物
    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      cost_scaling_factor: 10.0
      inflation_radius: 0.55

local_costmap:
  ros__parameters:
    update_frequency: 5.0        # 局部地图更新更频繁
    publish_frequency: 5.0
    resolution: 0.05
    robot_radius: 0.22
    inflation_radius: 0.55
    width: 3                     # 局部地图宽度(m)
    height: 3                    # 局部地图高度(m)

    plugins: ["obstacle_layer", "inflation_layer"]
    # 局部代价地图不需要静态层，靠传感器实时感知

六、路径规划与运动控制

全局规划器

规划器	算法	特点
NavfnPlanner	Dijkstra/A*	默认选择，稳定可靠
SmacPlanner2D	A* + 平滑	支持路径平滑，推荐替换
SmacPlannerHybrid	Hybrid A*	考虑运动学约束，适合非全向机器人

局部控制器

控制器	算法	特点
DWBLocalPlanner	Dynamic Window	Nav2默认，参数多但灵活
TEBLocalPlanner	Timed Elastic Band	考虑时序最优，适合非全向
MPPIController	Model Predictive Path Integral	Nav2新加入，效果好

参数配置

# nav2_params.yaml - 规划与控制参数
planner_server:
  ros__parameters:
    expected_planner_frequency: 5.0
    plugin: "nav2_navfn_planner::NavfnPlanner"
    tolerance: 0.5              # 目标点容差(m)
    use_astar: true             # 使用A*而非Dijkstra

controller_server:
  ros__parameters:
    controller_frequency: 10.0  # 控制频率(Hz)
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: true
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.26          # 最大线速度(m/s)
      max_vel_y: 0.0
      max_vel_theta: 1.0       # 最大角速度(rad/s)
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      acc_lim_x: 2.5           # 线加速度(m/s²)
      acc_lim_y: 0.0
      acc_lim_theta: 3.2       # 角加速度(rad/s²)
      decel_lim_x: -2.5
      decel_lim_theta: -3.2

      # 评价函数权重
      Critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      GoalAlign.scale: 24.0
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0

七、行为树

行为树（Behavior Tree）是Nav2的编排核心。它决定导航过程中先做什么、后做什么、失败了怎么办。

常用节点

节点	类型	作用
ComputePathToPose	Action	计算到目标的全局路径
FollowPath	Action	执行路径跟踪
Spin	Action	原地旋转扫描
BackUp	Action	后退脱困
Wait	Action	等待一段时间
ClearCostmap	Action	清除代价地图
ComputePathThroughPoses	Action	经过多路点的路径

默认行为树

Nav2默认行为树XML（navigate_to_pose_w_replanning_and_recovery.xml）的逻辑：

flowchart TB START(["导航开始"]) --> PIPELINE{"PipelineSequence"} PIPELINE --> GOALCHECK{"目标已到达?"} GOALCHECK -->|"是"| DONE(["导航完成"]) GOALCHECK -->|"否"| COMPUTE["ComputePathToPose 计算全局路径"] COMPUTE --> FOLLOW["FollowPath 跟踪路径"] FOLLOW --> GOALCHECK COMPUTE -->|"失败"| FALLBACK{"Recovery"} FOLLOW -->|"失败"| FALLBACK FALLBACK --> CLEAR["ClearCostmap 清除代价地图"] CLEAR --> SPIN["Spin 原地旋转"] SPIN --> BACKUP["BackUp 后退脱困"] BACKUP --> PIPELINE style START fill:#E8F5E9,stroke:#388E3C style DONE fill:#E8F5E9,stroke:#388E3C style FALLBACK fill:#FFEBEE,stroke:#D32F2F style PIPELINE fill:#E3F2FD,stroke:#1565C0

行为树的核心逻辑：不断重规划+跟踪路径，失败时依次尝试清除地图→旋转→后退，恢复后重试。

八、完整导航启动

安装

sudo apt install ros-jazzy-navigation2 ros-jazzy-nav2-bringup

启动导航

# 前提：机器人底盘+激光雷达已启动，地图已建好

# 启动Nav2完整导航栈
ros2 launch nav2_bringup bringup_launch.py \
    map:=$HOME/maps/my_map.yaml \
    use_sim_time:=false \
    params_file:=/path/to/nav2_params.yaml

nav2_params.yaml完整配置

# nav2_params.yaml - ROS2 Jazzy导航参数（精简关键参数）
amcl:
  ros__parameters:
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    min_particles: 200
    max_particles: 5000
    update_min_d: 0.25
    update_min_a: 0.2
    resample_interval: 1
    set_initial_pose: true
    initial_pose.x: 0.0
    initial_pose.y: 0.0
    initial_pose.yaw: 0.0

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    bt_loop_duration: 10
    default_server_timeout: 20
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_clear_costmap_service_bt_node

controller_server:
  ros__parameters:
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]

    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0

    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25      # 位置容差(m)
      yaw_goal_tolerance: 0.25     # 角度容差(rad)
      stateful: true

    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: true
      min_vel_x: 0.0
      max_vel_x: 0.26
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      acc_lim_x: 2.5
      acc_lim_theta: 3.2
      Critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      GoalAlign.scale: 24.0
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 5.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0
        inflation_radius: 0.55

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0
        inflation_radius: 0.55

planner_server:
  ros__parameters:
    expected_planner_frequency: 5.0
    plugin: "nav2_navfn_planner::NavfnPlanner"
    tolerance: 0.5
    use_astar: true

smoother_server:
  ros__parameters:
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: true

behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
    spin:
      plugin: "nav2_behaviors::Spin"
    backup:
      plugin: "nav2_behaviors::BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors::DriveOnHeading"
    wait:
      plugin: "nav2_behaviors::Wait"

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: true
      pause_duration: 200

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: false
    feedback: "OPEN_LOOP"
    max_velocity: [0.26, 0.0, 1.0]
    min_velocity: [-0.26, 0.0, -1.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

完整导航流程

flowchart TB A(["设置导航目标"]) --> B["AMCL定位 获取当前位姿"] B --> C["PlannerServer 全局路径规划"] C --> D["SmootherServer 路径平滑"] D --> E["ControllerServer 局部运动控制"] E --> F["发送cmd_vel 驱动底盘运动"] F --> G{"到达目标?"} G -->|"是"| H(["导航完成"]) G -->|"否"| E E -->|"卡住"| I["RecoveryServer 执行恢复行为"] I --> J["清除代价地图"] J --> K["原地旋转"] K --> L["后退脱困"] L --> C style A fill:#E3F2FD,stroke:#1565C0 style H fill:#E8F5E9,stroke:#388E3C style I fill:#FFEBEE,stroke:#D32F2F style G fill:#FFF8E1,stroke:#F9A825

RViz2中发送导航目标

# 启动RViz2
ros2 run rviz2 rviz2

# 在RViz2中：
# 1. 点击 "2D Pose Estimate" 设置初始位姿
# 2. 点击 "2D Nav Goal" 设置导航目标
# 3. 观察机器人自主导航

或用命令行发送目标：

ros2 topic pub --once /goal_pose geometry_msgs/msg/PoseStamped \
"{
  header: {frame_id: 'map'},
  pose: {
    position: {x: 2.0, y: 1.0, z: 0.0},
    orientation: {w: 1.0}
  }
}"

九、常见问题

Q1：机器人不移动？

检查控制器状态和cmd_vel话题。

# 检查控制器是否活跃
ros2 lifecycle list /controller_server

# 检查cmd_vel是否有数据
ros2 topic echo /cmd_vel --once

# 检查控制器输出频率
ros2 topic hz /cmd_vel

# 常见原因：
# 1. 控制器未激活
ros2 lifecycle set /controller_server activate
# 2. 速度限制太小 → 检查max_vel_x参数
# 3. 代价地图全满 → 检查传感器数据是否正常

Q2：路径规划失败？

检查代价地图和目标点可达性：

# 查看全局代价地图
ros2 topic echo /global_costmap/costmap --once

# 检查规划器状态
ros2 action list
ros2 action info /compute_path_to_pose

# 常见原因：
# 1. 目标点在障碍物内 → 换个目标点或增大tolerance
# 2. 代价地图膨胀太大 → 减小inflation_radius
# 3. 起点或终点未定位 → 检查AMCL定位是否收敛

Q3：导航卡住频繁触发恢复？

调整膨胀半径和控制器参数。

# 1. 减小膨胀半径，让机器人更敢于靠近障碍物
# inflation_radius: 0.55 → 0.35

# 2. 增大progress_checker的容忍度
# required_movement_radius: 0.5 → 0.3
# movement_time_allowance: 10.0 → 15.0

# 3. 降低控制器频率，给机器人更多反应时间
# controller_frequency: 10.0 → 8.0

# 4. 检查DWB评价函数权重，增大PathAlign让机器人更贴路径

十、总结

SLAM建图+Nav2导航是移动机器人的基础能力栈。SLAM Toolbox解决"我在哪、周围长什么样"，Navigation2解决"怎么到目标去"。核心流程：AMCL定位→代价地图感知→全局规划→局部控制→行为树编排→恢复兜底。

速查表

导航任务	核心组件	关键参数
建图	SLAM Toolbox	max_laser_range, resolution
定位	AMCL	min/max_particles, update_min_d
感知	Costmap2D	inflation_radius, update_frequency
全局规划	PlannerServer	tolerance, use_astar
局部控制	ControllerServer	max_vel_x, acc_lim_x
任务编排	BehaviorTree	bt_loop_duration
脱困恢复	RecoveryServer	spin, backup, clear_costmap

本文首发于linuxros.cn，转载请注明出处。