SLAM 建图与自主导航¶
在本项目中，你将让移动机器人具备**探索未知环境、构建地图并自主导航**到任意目标位姿的能力。你将实现三个算法层级：
| 层级 | 方法 | 传感器 | 核心包 |
|---|---|---|---|
| 传统方法 | 2D 激光 SLAM（gmapping） | 2D 激光雷达 | slam_gmapping |
| 中间层 | Cartographer / RTAB-Map | 2D 激光雷达 / RGB-D | cartographer_ros、rtabmap_ros |
| 现代方法 | 视觉 SLAM（ORB-SLAM3） | 单目 / 双目 / RGB-D 相机 | orb_slam3_ros |
三个层级均与 ROS 导航栈（move_base、代价地图、全局/局部规划器）集成，使机器人能够实际行驶到目标点。
学习目标¶
- 理解 SLAM 问题：同时定位与地图构建
- 实现基于粒子滤波的 2D SLAM（gmapping）
- 使用基于图的 SLAM（Cartographer、RTAB-Map）进行鲁棒建图
- 运行基于特征的视觉 SLAM（ORB-SLAM3）处理相机数据
- 配置 ROS 导航栈实现自主到达目标
- 比较不同 SLAM 方法的精度、鲁棒性和传感器需求
1. SLAM 问题¶
1.1 什么是 SLAM？¶
SLAM（Simultaneous Localization and Mapping，同时定位与地图构建）是在未知环境中**一边构建地图、一边在地图中定位机器人**的问题。这是一个"鸡与蛋"问题：
┌──────────────────────────────────────────────────────┐
│ SLAM 流水线 │
│ │
│ 传感器 ──► 特征/扫描 ──► 数据 ──► 地图 │
│ （激光雷达、 提取 关联 更新 │
│ 相机） │ │ │ │
│ │ │ │ │
│ ▼ ▼ │ │
│ 位姿估计 ◄──── 位姿更新 │ │
│ │ │ │
│ ▼ │ │
│ 定位（我在哪？）────────────┘ │
└──────────────────────────────────────────────────────┘
数学公式：在每个时间步 \(t\)，机器人接收里程计 \(u_t\) 和观测 \(z_t\)。SLAM 估计：
\[
p(x_{1:t}, m \mid z_{1:t}, u_{1:t})
\]
其中 \(x_{1:t}\) 是轨迹，\(m\) 是地图，\(z_{1:t}\) 是观测，\(u_{1:t}\) 是控制输入。
1.2 前端与后端¶
┌─────────────────────────────────────────────────┐
│ SLAM 系统 │
│ │
│ ┌──────────────┐ ┌──────────────┐ │
│ │ 前端 │ │ 后端 │ │
│ │ │ │ │ │
│ │ • 扫描匹配 │───────►│ • 位姿图 │ │
│ │ • 特征提取 │ 边 │ 优化 │ │
│ │ • 里程计 │ 节点 │ • 回环检测 │ │
│ │ │ │ │ │
│ └──────────────┘ └──────────────┘ │
│ 传感器数据 优化后的地图 │
└─────────────────────────────────────────────────┘
- 前端：将原始传感器数据处理为约束（边/节点）
- 后端：优化位姿图以生成一致的地图
2. 传统方法：基于 gmapping 的 2D 激光 SLAM¶
2.1 算法——Rao-Blackwellized 粒子滤波器¶
gmapping 使用 Rao-Blackwellized 粒子滤波器（RBPF），将后验概率表示为：
\[
p(x_{1:t}, m \mid z_{1:t}, u_{1:t}) = p(m \mid x_{1:t}, z_{1:t}) \cdot p(x_{1:t} \mid z_{1:t}, u_{1:t})
\]
每个粒子 \(i\) 携带：
- 一个机器人轨迹假设 \(x_{1:t}^{(i)}\)
- 一个以该轨迹为条件的地图 \(m^{(i)}\)
每次迭代的关键步骤：
- 运动更新 —— 从运动模型中采样新位姿：
\[
x_t^{(i)} \sim p(x_t \mid x_{t-1}^{(i)}, u_t)
\]
- 观测更新 —— 使用扫描匹配计算重要性权重：
\[
w_t^{(i)} = w_{t-1}^{(i)} \cdot p(z_t \mid x_t^{(i)}, m_{t-1}^{(i)})
\]
-
地图更新 —— 更新粒子 \(i\) 的占据栅格地图
-
重采样 —— 低方差重采样以避免粒子枯竭
自适应粒子数量：gmapping 根据估计的有效样本大小 \(N_{\text{eff}}\) 调整 \(N\)：
\[
N_{\text{eff}} = \frac{1}{\sum_{i=1}^{N} (w_t^{(i)})^2}
\]
当 \(N_{\text{eff}}\) 低于阈值时，执行重采样。
2.2 ROS 启动文件 —— gmapping¶
<!-- launch/gmapping.launch -->
<launch>
<!-- 参数 -->
<arg name="base_frame" default="base_link"/>
<arg name="odom_frame" default="odom"/>
<arg name="map_frame" default="map"/>
<arg name="scan_topic" default="/scan"/>
<!-- gmapping 节点 -->
<node pkg="slam_gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="base_frame" value="$(arg base_frame)"/>
<param name="odom_frame" value="$(arg odom_frame)"/>
<param name="map_frame" value="$(arg map_frame)"/>
<param name="delta" value="0.05"/> <!-- 5 厘米网格分辨率 -->
<param name="xmin" value="-25.0"/>
<param name="ymin" value="-25.0"/>
<param name="xmax" value="25.0"/>
<param name="ymax" value="25.0"/>
<param name="particles" value="30"/>
<param name="minimumScore" value="200"/>
<param name="linearUpdate" value="1.0"/> <!-- 每移动 1 米更新 -->
<param name="angularUpdate" value="0.5"/> <!-- 每转 0.5 弧度更新 -->
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="srr" value="0.01"/> <!-- 里程计噪声 -->
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="lstep" value="0.05"/> <!-- 搜索步长 -->
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
2.3 保存地图¶
建图完成后，使用 map_saver 保存地图：
YAML 文件内容：
# my_map.yaml
image: my_map.pgm
resolution: 0.050000
origin: [-25.000000, -25.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
3. 中间层：Cartographer 与 RTAB-Map¶
3.1 Google Cartographer¶
Cartographer 使用**子图（submap）和**位姿图优化。前端通过**相关性扫描匹配**构建局部子图；当检测到回环时，后端执行基于 Ceres 的优化。
扫描匹配——Ceres 代价函数：
\[
\min_{\xi} \sum_{k=1}^{K} \left( 1 - M_{\text{smooth}}\!\left( T_\xi \, h_k \right) \right)^2
\]
其中 \(\xi \in \mathfrak{se}(2)\) 是位姿，\(h_k\) 是第 \(k\) 个命中点，\(M_{\text{smooth}}\) 是双线性插值后的子图值。
位姿图——当一个子图完成后，在以下节点间添加约束（边）：
- 子图节点 ↔ 子图节点（子图间约束）
- 子图节点 ↔ 路标节点
优化目标为最小化：
\[
\min_{\mathbf{x}} \sum_{(i,j) \in \mathcal{E}} \left\| z_{ij} - \hat{z}_{ij}(\mathbf{x}_i, \mathbf{x}_j) \right\|^2_{\Omega_{ij}}
\]
其中 \(\mathbf{x}\) 是位姿，\(z_{ij}\) 是观测约束，\(\Omega_{ij}\) 是信息矩阵。
启动文件——2D Cartographer：
<!-- launch/cartographer_2d.launch -->
<launch>
<arg name="configuration_directory" default="$(find cartographer_ros)/configuration_files"/>
<arg name="configuration_basename" default="backpack_2d.lua"/>
<node pkg="cartographer_ros" type="cartographer_node" name="cartographer_node"
args="-configuration_directory $(arg configuration_directory)
-configuration_basename $(arg configuration_basename)"
output="screen">
<remap from="scan" to="/scan"/>
<remap from="odom" to="/odom"/>
</node>
<!-- 为导航栈生成占据栅格 -->
<node pkg="cartographer_ros" type="cartographer_occupancy_grid_node"
name="cartographer_occupancy_grid_node" output="screen">
<param name="resolution" value="0.05"/>
</node>
</launch>
Lua 配置文件（backpack_2d.lua）：
-- backpack_2d.lua
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = true,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range = 30.0
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.0
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 5.0
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.2
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(1.0)
POSE_GRAPH.optimization_problem.huber_scale = 1e1
POSE_GRAPH.optimize_every_n_nodes = 90
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
3.2 RTAB-Map（基于外观的实时建图）¶
RTAB-Map 使用**基于图的方法**融合**激光、RGB-D 和里程计**数据，并通过**内存管理**系统处理大规模环境。
核心特性：
- 基于视觉词袋（BoW）的外观回环检测
- 通过 g2o / GTSAM 进行图优化
- 多会话建图支持
- RGB-D 或纯激光模式
启动文件——RTAB-Map RGB-D：
<!-- launch/rtabmap_rgbd.launch -->
<launch>
<!-- 参数 -->
<arg name="frame_id" default="base_link"/>
<arg name="rgb_topic" default="/camera/rgb/image_raw"/>
<arg name="depth_topic" default="/camera/depth/image_raw"/>
<arg name="camera_info" default="/camera/rgb/camera_info"/>
<arg name="odom_topic" default="/odom"/>
<arg name="wait_for_transform" default="0.2"/>
<!-- RTAB-Map 节点 -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_rgb" type="bool" value="true"/>
<param name="subscribe_odom" type="bool" value="true"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="10"/>
<param name="RGBD/AngularUpdate" type="double" value="0.05"/>
<param name="RGBD/LinearUpdate" type="double" value="0.05"/>
<param name="RGBD/OptimizeFromGraphEnd" type="bool" value="false"/>
<param name="Kp/MaxFeatures" type="int" value="500"/>
<param name="Mem/RehearsalSimilarity" type="double" value="0.3"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info)"/>
<remap from="odom" to="$(arg odom_topic)"/>
</node>
<!-- 可视化 -->
<node name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen"/>
</launch>
4. 现代方法：ORB-SLAM3 —— 视觉 SLAM¶
4.1 系统架构¶
ORB-SLAM3 是一个**基于特征的视觉 SLAM** 系统，支持单目、双目和 RGB-D 相机，并可选融合 IMU。它使用 ORB 特征和共视图进行跟踪、建图和回环检测。
┌─────────────────────────────────────────────────────────────┐
│ ORB-SLAM3 流水线 │
│ │
│ ┌──────────┐ ┌───────────┐ ┌──────────┐ ┌────────┐ │
│ │ 跟踪 │──►│ 局部建图 │──►│ 回环检测 │──►│ 地图集 │ │
│ │ │ │ │ │ │ │ │ │
│ │ • ORB │ │ • 关键帧 │ │ • DBoW2 │ │ • 活跃 │ │
│ │ 提取 │ │ 插入 │ │ 检测 │ │ 地图 │ │
│ │ • 运动 │ │ • 光束法 │ │ • Sim(3) │ │ • 全局 │ │
│ │ 模型 │ │ 平差 │ │ 验证 │ │ 地图 │ │
│ │ • 帧匹配 │ │ • 地图点 │ │ • 图融合 │ │ │ │
│ │ │ │ 剔除 │ │ │ │ │ │
│ └──────────┘ └───────────┘ └──────────┘ └────────┘ │
│ ▲ │ │
│ │ 位姿输出 (Tcw) │ │
│ └──────────────────────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────┘
4.2 光束法平差（Bundle Adjustment）¶
后端求解一个**稀疏光束法平差**问题。给定 \(n\) 个相机位姿和 \(m\) 个 3D 地图点：
\[
\min_{\mathbf{T}_i, \mathbf{P}_j} \sum_{i=1}^{n} \sum_{j \in \mathcal{V}(i)} \rho\!\left( \left\| \mathbf{u}_{ij} - \pi(\mathbf{T}_i, \mathbf{P}_j) \right\|^2_{\Sigma} \right)
\]
其中：
- \(\mathbf{T}_i \in SE(3)\) —— 相机位姿 \(i\)
- \(\mathbf{P}_j \in \mathbb{R}^3\) —— 3D 点 \(j\)
- \(\pi(\cdot)\) —— 投影函数
- \(\mathbf{u}_{ij}\) —— 观测到的 2D 关键点
- \(\rho(\cdot)\) —— Huber 鲁棒核函数
- \(\Sigma\) —— 观测协方差
通过 Levenberg–Marquardt 算法结合 Schur 补技巧高效求解。
4.3 ROS 封装——ORB-SLAM3¶
<!-- launch/orb_slam3_stereo.launch -->
<launch>
<arg name="vocabulary_path"
default="$(find orb_slam3_ros)/vocabulary/ORBvoc.txt"/>
<arg name="settings_path"
default="$(find orb_slam3_ros)/config/stereo/TUM_RGBD.yaml"/>
<arg name="world_frame_id" default="map"/>
<node pkg="orb_slam3_ros" type="orb_slam3_ros_node" name="orb_slam3"
output="screen">
<param name="voc_path" value="$(arg vocabulary_path)"/>
<param name="settings_path" value="$(arg settings_path)"/>
<param name="world_frame_id" value="$(arg world_frame_id)"/>
<remap from="camera/left/image_raw" to="/camera/left/image_raw"/>
<remap from="camera/right/image_raw" to="/camera/right/image_raw"/>
</node>
</launch>
配置 YAML（示例）：
# TUM_RGBD.yaml —— 双目设置
Camera.type: "PinHole"
Camera.fx: 525.0
Camera.fy: 525.0
Camera.cx: 319.5
Camera.cy: 239.5
Camera.bf: 40.0 # 基线 × fx
Camera.width: 640
Camera.height: 480
Camera.fps: 30.0
ThDepth: 40.0
ORBextractor.nFeatures: 1200
ORBextractor.scaleFactor: 1.2
ORBextractor.nLevels: 8
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize: 2
5. ROS 导航栈集成¶
5.1 架构¶
┌─────────────────────────────────────────────────────────┐
│ ROS 导航栈 │
│ │
│ ┌───────────┐ ┌──────────────────────────────┐ │
│ │ move_base │ │ 地图 │ │
│ │ │ │ ┌────────────┐ ┌──────────┐ │ │
│ │ ┌───────┐ │ │ │ 全局代价 │ │ 局部代价 │ │ │
│ │ │全局 │ │◄───│ │ 地图 │ │ 地图 │ │ │
│ │ │规划器 │ │ │ │(静态, │ │(滚动 │ │ │
│ │ │(A*, │ │ │ │ 障碍物) │ │ 窗口) │ │ │
│ │ │Dijkstra│ │ │ └────────────┘ └──────────┘ │ │
│ │ │NavFn) │ │ └──────────────────────────────┘ │
│ │ └───────┘ │ ┌──────────────────────────────┐ │
│ │ ┌───────┐ │ │ 恢复行为 │ │
│ │ │局部 │ │ │ • 旋转恢复 │ │
│ │ │规划器 │ │ │ • 清除代价地图 │ │
│ │ │(DWA, │ │ │ • 保守重置 │ │
│ │ │TEB) │ │ └──────────────────────────────┘ │
│ │ └───────┘ │ │
│ └───────────┘ │
│ │ 目标 输出: cmd_vel │
│ ▼ │
│ 机器人硬件（base_controller） │
└─────────────────────────────────────────────────────────┘
5.2 move_base 启动文件¶
<!-- launch/move_base.launch -->
<launch>
<arg name="map_file" default="$(find my_robot)/maps/my_map.yaml"/>
<arg name="cmd_vel" default="/cmd_vel"/>
<!-- 地图服务器 -->
<node pkg="map_server" type="map_server" name="map_server"
args="$(arg map_file)" output="screen"/>
<!-- AMCL 定位 -->
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<param name="odom_frame_id" value="odom"/>
<param name="base_frame_id" value="base_link"/>
<param name="global_frame_id" value="map"/>
<param name="use_map_topic" value="true"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="laser_max_beams" value="30"/>
<param name="laser_max_range" value="12.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<remap from="scan" to="/scan"/>
</node>
<!-- move_base -->
<node pkg="move_base" type="move_base" name="move_base" output="screen">
<rosparam file="$(find my_robot)/config/costmap_common_params.yaml"
command="load" ns="global_costmap"/>
<rosparam file="$(find my_robot)/config/costmap_common_params.yaml"
command="load" ns="local_costmap"/>
<rosparam file="$(find my_robot)/config/local_costmap_params.yaml"
command="load"/>
<rosparam file="$(find my_robot)/config/global_costmap_params.yaml"
command="load"/>
<rosparam file="$(find my_robot)/config/base_local_planner_params.yaml"
command="load"/>
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>
<remap from="cmd_vel" to="$(arg cmd_vel)"/>
</node>
</launch>
5.3 代价地图配置¶
# config/costmap_common_params.yaml
robot_radius: 0.18
inflation_layer:
inflation_radius: 0.50
cost_scaling_factor: 10.0
obstacle_layer:
observation_sources: laser_scan
laser_scan:
sensor_frame: laser_link
data_type: LaserScan
topic: /scan
marking: true
clearing: true
max_obstacle_height: 2.0
min_obstacle_height: 0.0
# config/local_costmap_params.yaml
local_costmap:
global_frame: odom
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05
# config/global_costmap_params.yaml
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 1.0
publish_frequency: 0.5
static_map: true
rolling_window: false
# config/base_local_planner_params.yaml (DWA)
DWAPlannerROS:
max_vel_x: 0.5
min_vel_x: -0.1
max_vel_y: 0.0 # 非完整约束
min_vel_y: 0.0
max_vel_trans: 0.5
min_vel_trans: 0.1
max_vel_theta: 1.5
min_vel_theta: 0.3
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
xy_goal_tolerance: 0.15
yaw_goal_tolerance: 0.15
latch_xy_goal_tolerance: true
sim_time: 2.0
vx_samples: 10
vy_samples: 1
vtheta_samples: 20
path_distance_bias: 32.0
goal_distance_bias: 24.0
occdist_scale: 0.1
6. SLAM 方法对比¶
| 特性 | gmapping | Cartographer | RTAB-Map | ORB-SLAM3 |
|---|---|---|---|---|
| 类型 | 2D 占据栅格 | 2D/3D 子图 | 基于图（2D/3D） | 稀疏视觉 |
| 传感器 | 2D 激光雷达 | 2D/3D 激光雷达 | 激光雷达 / RGB-D / 双目 | 单目 / 双目 / RGB-D + IMU |
| 后端 | 粒子滤波 | Ceres（图优化） | g2o / GTSAM | g2o / GTSAM（BA） |
| 回环检测 | ❌ 无 | ✅ 基于相关性 | ✅ 视觉词袋 | ✅ DBoW2 |
| 3D 建图 | ❌ 否 | ✅ 是（3D 模式） | ✅ 是 | ✅ 是（稀疏） |
| 实时性 | ✅ 是 | ✅ 是 | ✅ 是 | ✅ 是 |
| 地图输出 | 占据栅格 | 栅格 / 点云 | 栅格 / 点云 | 稀疏 3D 点 + 位姿 |
| 内存 | 随粒子数增长 | 基于子图 | 内存管理 | 基于关键帧 |
| 鲁棒性 | 低（无回环） | 高 | 高 | 高（需要纹理） |
| 适用场景 | 简单 2D 建图 | 生产级机器人 | 多传感器融合 | 纯视觉或轻量化方案 |
| 复杂度 | ⭐⭐ | ⭐⭐⭐ | ⭐⭐⭐ | ⭐⭐⭐⭐ |
7. 完整系统启动文件¶
将 SLAM + 导航集成到一个启动文件中：
<!-- launch/slam_navigation_full.launch -->
<launch>
<!-- 1. 机器人描述 -->
<include file="$(find my_robot)/launch/urdf.launch"/>
<!-- 2. 传感器驱动 -->
<include file="$(find my_robot)/launch/sensors.launch"/>
<!-- 3. SLAM（选择其一） -->
<!-- 方案 A：gmapping -->
<include file="$(find my_robot)/launch/gmapping.launch">
<arg name="scan_topic" value="/scan"/>
</include>
<!-- 方案 B：Cartographer（取消注释以使用） -->
<!--
<include file="$(find my_robot)/launch/cartographer_2d.launch"/>
-->
<!-- 4. move_base（SLAM 运行时无需 map_server） -->
<node pkg="move_base" type="move_base" name="move_base" output="screen">
<rosparam file="$(find my_robot)/config/costmap_common_params.yaml"
command="load" ns="global_costmap"/>
<rosparam file="$(find my_robot)/config/costmap_common_params.yaml"
command="load" ns="local_costmap"/>
<rosparam file="$(find my_robot)/config/local_costmap_params.yaml"
command="load"/>
<rosparam file="$(find my_robot)/config/global_costmap_params.yaml"
command="load"/>
<rosparam file="$(find my_robot)/config/base_local_planner_params.yaml"
command="load"/>
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>
<remap from="cmd_vel" to="/cmd_vel"/>
</node>
<!-- 5. RViz -->
<node pkg="rviz" type="rviz" name="rviz"
args="-d $(find my_robot)/rviz/slam_nav.rviz"/>
</launch>
7.1 测试系统¶
# 终端 1：启动 Gazebo 仿真
roslaunch my_robot gazebo.launch
# 终端 2：启动 SLAM + 导航
roslaunch my_robot slam_navigation_full.launch
# 终端 3：发送导航目标（或在 RViz 中使用 "2D Nav Goal"）
rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \
"header:
frame_id: 'map'
pose:
position: {x: 3.0, y: 2.0, z: 0.0}
orientation: {w: 1.0}"
8. 实践练习¶
练习 1：用 gmapping 建图¶
- 在 Gazebo 中启动你的机器人（已知环境）
- 使用
teleop_twist_keyboard手动驾驶机器人 - 在 RViz 中观察地图逐步构建
- 保存地图并重新加载用于导航
练习 2：对比 SLAM 方法¶
- 录制包含激光雷达 + 相机数据的 rosbag
- 对同一数据分别运行 gmapping、Cartographer 和 RTAB-Map
- 比较地图质量、精度和计算时间
练习 3：视觉 SLAM 挑战¶
- 在 RGB-D 模式下对 TUM RGB-D 数据集运行 ORB-SLAM3
- 使用
rpg_trajectory_evaluation评估 ATE（绝对轨迹误差） - 尝试不同特征数量并比较结果
练习 4：完整导航流水线¶
- 用 Cartographer 构建地图
- 保存地图，启动 AMCL 定位 + move_base
- 用 Python 节点程序化发送导航目标
- 添加模拟行人的动态避障
参考资料¶
- gmapping ROS Wiki — gmapping 官方文档
- Grisetti et al., 2007 — Improved Techniques for Grid Mapping with RBPF — gmapping 算法论文
- Cartographer ROS 文档 — Cartographer ROS 官方文档
- Hess et al., 2016 — Real-Time Loop Closure in 2D LIDAR SLAM (Google) — Cartographer 算法论文
- RTAB-Map 文档 — RTAB-Map 官方文档
- Labbé & Michaud, 2019 — RTAB-Map Real-Time Appearance-Based Mapping — RTAB-Map 论文
- ORB-SLAM3 GitHub — ORB-SLAM3 官方仓库
- Campos et al., 2021 — ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM — ORB-SLAM3 论文
- ROS 导航栈 Wiki — ROS 导航栈官方文档
- DWA Local Planner — 动态窗口法规划器
- Thrun et al., 2005 — Probabilistic Robotics (书籍) — SLAM 与导航经典教材
