路径规划（Path Planning）是机器人的“指挥部”，它负责在 SLAM 构建好的地图上，为机器人计算出一条从 A 点到 B 点的安全、最短且平滑的路线。

1. 路径规划的两个层次

在 ROS 和工业界，路径规划通常被拆分为两个相互配合的部分：

A. 全局路径规划 (Global Planner) —— “战略地图”

原理： 在已知地图上，忽略动态障碍物（如突然走动的人），计算出一条总体的“最优折线”。
常用算法： A* (最经典)、Dijkstra (最短路径)、PRM/RRT (基于采样，适合高维空间)。
涉及知识： 图论、启发式搜索、栅格地图（Occupancy Grid Map）的解析。

B. 局部路径规划 (Local Planner) —— “战术避障”

如果全局规划是“战略”，那么局部规划就是“战术执行”。它的任务是让机器人沿着全局路径走，同时实时躲避传感器（激光雷达、深度相机）刚探测到的障碍物。
原理： 在全局路径的指引下，根据传感器实时感知到的障碍物（如突然走的人、新搬来的箱子），结合机器人的动力学限制（速度、加速度），计算出下一秒的具体速度指令（v,ω）
常用算法： DWA (Dynamic Window Approach)，TEB (Timed Elastic Band)， MPC (Model Predictive Control)。
涉及知识： 机器人运动学（Kinematics）、最优控制理论、多传感器融合（LiDAR/Depth Camera）的实时处理。

2. SLAM 介绍

SLAM (Simultaneous Localization and Mapping，即时定位与地图构建）。

想象你闭着眼睛在房间里走，1. 你不知道房间长啥样（建图问题）2. 你不知道自己在哪房子的那里（定位问题）。你能用的只有手摸墙（传感器，比如雷达），和记住自己走了几步（运动信息）。

所以, 把上的问题抽象成两个概念，就是建图和定位。

  • 1. 建图
    世界长啥样？哪有墙，哪有障碍物，那里是路， 这就是地图。
    没有地图，这些传感器数据无法告诉你“绝对位置”。

  • 2. 定位
    我在哪？机器人每一时刻的位置, 使用三维坐标表示，就是(x, y, θ)。
    定位错一点，地图位置也就错了，越走越离谱。

对于一个新环境，到底是先定位， 还是先建图呢？ 如果先定位，你得知道世界地图是怎样的。 如果先建图，你得知道绝对位置才能建图。这两个是相互依赖的关系。就像到底是鸡生的蛋，还是蛋生的鸡，相互依赖了。

有人说，我设置起始点为原点，不就可以了么？前进10cm看到一个障碍物，后退10cm， 不就可以了么？ 咋听是对的，其实忽略了一个事实，就是你前进的距离，也是传感器估算的，不是实际距离，会有误差。 你后退了了10cm，实际可能是12cm（有点夸大），结果你发现实际的原点上是一堵墙。

SLAM 是怎么解决的？第一帧，SLAM 先信自己，建一小块地图，移动一点，再扫一帧，不用地图，直接比较两帧数据！上一帧：墙在这里，这一帧：墙好像往后移了，于是，推断我向前走了 10cm，便更新地图和位置，这一步叫扫描匹配。通过不断重复，扫描 → 对齐 → 更新 → 再扫描， SLAM正是用“相对运动”一点点逼近真实结果。

根据传感器类型和算法逻辑，主流方案可以分为以下几大类：

2.1 2D 激光 SLAM

  • 原理： 利用单线激光雷达扫描水平面的轮廓，构建二维占据网格地图（Occupancy Grid Map）。
  • 主流算法：
    • Gmapping： 经典的粒子滤波算法，对算力要求低，但大场景易回环失败。
    • Cartographer (Google 开源)： 目前 ROS 2 的首选。采用子图（Submap）匹配，回环检测极其强大，支持多传感器融合。
    • Karto SLAM： 基于图优化（Graph-based）的方案。
  • 特点： 成本低（单线雷达即可）、技术极其成熟，但无法感知桌腿以上或地面的障碍物。

2.2 3D 激光 SLAM

  • 原理： 使用多线雷达（16线/32线及以上）获取空间点云。
  • 主流算法：
    • LeGO-LOAM： 针对地面进行了优化，轻量化且高效。
    • LIO-SAM： 紧耦合了 IMU 和激光雷达，运动过程中的建图非常稳健。
    • Fast-LIO： 运算速度极快，适合嵌入式低功耗平台。
  • 特点： 能够构建高精度三维点云地图，适用于自动驾驶、室外复杂环境。

2.3 视觉 SLAM (Visual SLAM / vSLAM) —— 潜力巨大

通过摄像头（单目、双目或 RGB-D）获取图像，像人眼一样识环境。

  • 原理： 提取图像中的特征点（ORB、SIFT 等），通过三角测量计算深度并构建稀疏或稠密地图。
  • 主流算法：
    • ORB-SLAM3： 目前视觉领域的“天花板”，支持单目、双目、惯性导航融合。
    • RTAB-Map： ROS 中集成度最高的方案，支持手持、无人机、机器人，能直接生成彩色 3D 地图。
  • 特点： 信息量大（能识别颜色、物体），硬件成本低（摄像头比雷达便宜），但在光照剧烈变化或白墙环境下易“丢位置”。

2.4 多传感器融合 SLAM (Multi-sensor Fusion)

这是工业化产品的真实形态，不依赖单一传感器。

  • 典型组合： Lidar + IMU (惯性测量单元) + 编码器 (Odom)。

  • 意义： * IMU 解决快速旋转时的补偿。
    - 编码器 提供短距离的推算。
    - Lidar/视觉 进行全局校准和回环。

  • 涉及工具： robot_localization 包（扩展卡尔曼滤波 EKF）。

下面给出这几个方案的优缺点：

方案 传感器 优点 缺点 适用场景
2D 激光 单线激光雷达 算法成熟、实现简单、计算量小、测距精度高 只能获取 2D 平面信息，对高低障碍物感知能力弱，容易被遮挡 室内机器人、扫地机器人、酒店配送机器人
3D 激光 多线激光雷达 三维环境感知能力强、精度高、抗干扰能力强 设备价格昂贵、数据量大、计算资源需求高 自动驾驶、无人巡检车、室外园区机器人
视觉 深度相机 / 双目相机 成本较低、可获取丰富语义信息（识别物体、场景） 对光照环境敏感、算法复杂、计算量大 无人机、AR/VR、服务机器人

3. 使用gmapping 建图

3.1 编译与安装slam_gmapping

# 1. 克隆 ROS 2 版本的 gmapping 源码 (由社区维护的兼容版本)
git clone https://github.com/Project-MANAS/slam_gmapping.git -b humble-devel

# 2. 克隆底层算法库 openslam_gmapping
git clone https://github.com/ros-perception/openslam_gmapping.git

# 3. 编译
colcon build

3.2 关键参数配置

Gmapping 的性能高度依赖参数。在你的 src/slam_gmapping/gmapping/params 目录下（或者直接在启动时指定），有几个参数非常重要：

参数名 建议值 作用
maxUrange 8.0 ~ 15.0 雷达探测的最大有效距离，超过该距离的数据不会参与建图。
linearUpdate 0.5 机器人每移动 0.5 米更新一次地图，可减少计算频率和 CPU 消耗。
angularUpdate 0.2 机器人每转动 **约 0.2 rad（≈11°）**更新一次地图。
particles 30 核心参数。粒子数量越多定位越精确，但 CPU 开销越大。在 RK3588 上通常可以设置 50–80。
delta 0.05 地图分辨率（5 cm/像素）。数值越小地图越精细，但内存和计算负担更大。

3.3 启动建图

在运行之前，确保你的底盘驱动、雷达驱动和 static_tf（base_link 到 laser）已经正常运行。

# 实际建图命令
ros2 run slam_gmapping gmapping --ros-args -p use_sim_time:=false
# 使用launch 包含 底盘驱动、雷达驱动和 static_tf 等。
ros2 launch slam_gmapping slam_gmapping.launch.py

3.4 gmapping 的工作流程

Gmapping 的核心工作可以拆解为：输入处理、扫描匹配、粒子滤波更新、以及地图生成。

3.4.1 Gmapping 的输入（数据依赖）

在 Gmapping 启动后，它像一个“数据加工厂”，必须持续接收以下三种信息：

  • /scan (LaserScan)： 2D 激光雷达数据，提供周围障碍物的距离和角度。
  • /tf (里程计变换)： 必须存在 odom -> base_link 的变换。这是机器人的“盲走”数据。
  • /tf (静态变换)： 必须存在 base_link -> laser_frame 的变换（雷达装在机器人哪个位置）。

3.4.2 核心工作流程（四个阶段）

1.扫描匹配 (Scan Matching)

这是 Gmapping 的第一步。由于里程计（Odom）存在物理误差（如轮子打滑），Gmapping 不会完全相信里程计。

  • 动作： 它拿当前这一帧的雷达扫描结果，去和上一时刻的地图做比对。
  • 目的： 找到一个最可能的坐标 (x,y,θ)，使得当前的雷达点云能最完美地重叠在已有地图上。
2. 粒子滤波 (Improved Carbon Particle Filter)

这是 Gmapping 的灵魂。它不只维护一个位置，而是维护一群可能的位置（称为粒子）。
- 预测阶段： 根据里程计数据，让所有粒子都“走一段距离”。
- 权重更新： 每一个粒子都会根据它观察到的雷达数据与自己地图的匹配程度，获得一个“得分”（权重）。匹配度越高，得分越高。
- 选择性重采样： 这是一个关键优化。Gmapping 不会频繁地打乱粒子，只有当粒子之间的差距太大（权重分散）时，才会保留高分粒子，剔除低分粒子。

3. 地图更新与融合

每个粒子不仅记录位置，还维护着自己的轨迹。

  • 当机器人移动超过一定阈值（参数 linearUpdate 和 angularUpdate）时，Gmapping 会将当前的雷达数据“画”到地图上。
  • 它使用 占有栅格地图 (Occupancy Grid Map) 算法，将空间划分为：占据（有墙）、空闲（无障碍）、未知。

4. 坐标系发布 (TF Publishing)

这是你看到 map 坐标系的原因。

  • Gmapping 计算出“当前最强粒子”的位置。
  • 由于 odom -> base_link 已经存在，为了修正累积误差，Gmapping 会计算并发布一个 map -> odom 的变换。
  • 公式逻辑： map -> base_link (算法算的) = map -> odom (发布的补偿) + odom -> base_link (驱动报的)。

3.5 Rviz2 验证

  • 打开 Rviz2。
  • 将 Fixed Frame 改为 map。
  • 添加 Map 组件，订阅 /map 话题。
  • 添加 TF 组件，观察 map -> odom -> base_link 链条是否完整。

在这里插入图片描述

  • 黑色像素 (0)： 障碍物（Occupancy）。
    雷达扫到的墙壁、柱子或纸箱。
  • 白色像素 (255)： 空闲空间（Free Space）。
    机器人已经探索过且确认没有障碍物的地方，导航时可以规划路径。
  • 灰色像素 (205 左右)： 未知区域（Unknown）。
    雷达还没扫到的地方，机器人不知道那边是墙还是路。
  • 红色小点 (LaserScan)
    观察红色点是否和地图上的黑边重合。
    • 重合良好： 说明定位非常准。
    • 红点乱跳或偏离黑边： 说明里程计有累积误差，或者算法处理速度（CPU 负载）跟不上机器人的移动速度。
类别 指标 观察方式 好的表现 👍 异常表现 ⚠️ 可能原因
地图质量 墙壁直线度 (Line Straightness) 在 RViz 中观察地图 墙壁为笔直单线 墙壁重影、锯齿 里程计角速度误差、雷达频率与运动不匹配
地图质量 闭环对齐 (Loop Closure) 机器人绕一圈回起点 新旧墙体完全重合 墙角错位、开叉 SLAM 位姿修正失败
地图质量 噪点分布 (Noise Density) 查看地图空旷区域 空旷区域干净 有很多孤立黑点 雷达精度低、玻璃反射、驱动未过滤
自身评估 扫描匹配得分 (Scan Matching Score) 查看终端 log score 较高且稳定 score 持续偏低 地图漂移、扫描匹配失败

Scan Matching Score 的常见范围：

得分范围 状态评估 影响 / 现象
> 300 极佳 (Excellent) 机器人对自身位置非常确定；地图边缘清晰，墙体为单线条，几乎没有重影或漂移。
100 – 300 良好 (Good) 正常工作状态；在环境特征较少（如长走廊、空旷区域）时，得分可能暂时下降到这个区间。
50 – 100 一般 (Fair) 匹配开始变弱；地图可能出现轻微模糊或小范围漂移，需要关注传感器或运动参数。
< 50 糟糕 (Poor) 扫描匹配基本失败；机器人可能打滑、环境过于空旷、或者雷达被遮挡。
0 失败 (Failed) 严重漂移，定位完全丢失；SLAM 可能需要重新初始化。

Gmapping 就像一个打开的 Word 文档，你正在里面写字（建图），只要你没点“保存”按钮，也没关掉程序，数据就一直存在内存（RAM）里。

3.6 Gmapping 不建议使用静态TF 变换

对于静态坐标转换（static_transform_publisher）base_footprint -> base_link 的时间戳 = 0。

ros2 run tf2_ros static_transform_publisher \
    0 0 0 0 0 0 \
    base_footprint laser \
    --publish-period 0.05

–publish-period参数 让 static_transform_publisher 持续以指定周期重新发布静态 transform，且每次发布 transform 都会带上当前时间戳。

理解TF2 坐标转换查询的流程

robot 收到雷达数据， 根据当前数据的 /scan.header.stamp 的时间戳 去TF buffer 中查找其坐标变换，直到找到TF buffer 中， 最接近的时间戳， 返回该transform。

例如， 你的 /scan 消息：

header:
stamp:
sec: 1774591684
nanosec: 133205296
frame_id: laser

时间戳：1774591684 秒 → 现在（系统时间）
当 GMAPPING 收到这个 scan 时，它会做：

tf2_buffer->lookupTransform(base_frame, scan.header.frame_id, scan.header.stamp)

也就是：在 scan 的时间戳那一刻，找到 laser → base_footprint 的 transform.

对于静态坐标转换（static_transform_publisher），TF buffer 中只有一条 transform，base_footprint -> base_link 的时间戳 = 0
base_link -> laser 的时间戳 = 0 或者近似 0。tf2 查询时间 = 1774591684，而 transform 时间 = 0， 其差值 = 1774591684 秒 → 约 1200 秒，所以，tf2 返回：

# 也可以使用这个查询坐标转换
$ ros2 run tf2_ros tf2_monitor base_footprint laser
....
Average Delay = 当前 scan 时间 - transform 时间 ≈ 1200 秒

在 monitor 中就看到巨大的延迟, 对 GMAPPING 来说，这个 transform “离 scan 太远了”，无法可靠投影 → 灰色未知区域.

4. 使用slam_toolbox

slam_toolbox 是 ROS2 中非常常用的一套 2D 激光 SLAM（建图与定位）框架，相比早期的 slam_gmapping，它在 稳定性、地图优化、在线/离线模式、地图保存与重定位方面都更强。下面给你系统讲清楚它的原理、模式、核心参数和使用方式。

slam_toolbox 是一个 基于图优化（Graph-based SLAM） 的 2D 激光 SLAM 系统。
核心功能：

  • 激光建图（mapping）
  • 定位（localization）
  • 地图保存 / 加载
  • 回环检测（loop closure）
  • 地图优化（pose graph optimization）

4.1 slam_toolbox 工作原理

整体流程如下：

/scan (LaserScan)
        │
        ▼
scan matcher
        │
        ▼
pose graph (位姿图)
        │
        ▼
loop closure (回环检测)
        │
        ▼
graph optimization
        │
        ▼
/map (OccupancyGrid)

机器人每移动一步：
1️⃣ 用 scan matcher 估计当前位置
2️⃣ 生成一个 节点（pose）
3️⃣ 将节点加入 pose graph
4️⃣ 检测是否形成 回环
5️⃣ 如果检测到回环 → 进行 图优化

4.2 slam_toolbox 输入

Topic 类型 说明
/scan LaserScan 激光数据
/tf transform 机器人坐标
/tf_static transform 静态TF

4.3 slam_toolbox 输出

Topic 类型 说明
/map OccupancyGrid 地图
/tf transform map→odom
/pose PoseStamped 机器人位姿

4.4 安装

apt install ros-humble-slam-toolbox

4.5 运行

 launch_ros.actions.Node(
            package='slam_toolbox',
            executable='async_slam_toolbox_node',
            name='slam_toolbox',
            output='screen',
            parameters=[
        		get_package_share_directory("wheeltec_slam_toolbox") + '/config/mapper_params_online_async.yaml'
        	],
            remappings=[('odom','odom_combined')]
        )

启动rviz2 后看到这个状态图（建图后）：
在这里插入图片描述

这个.yaml 配置文件就是调教手册。其包含以下大致的参数：

4.5.1 坐标系与传感器 参数

  • odom_frame: odom_combined
    核心逻辑： 告诉 SLAM 你的里程计坐标系叫什么。既然你在 Launch 里做了 remap，这里必须匹配。
  • base_frame: base_footprint
    核心逻辑： 机器人的中心参考点。
  • scan_topic: /scan
    核心逻辑： 订阅激光雷达数据的接口。

4.5.2 运动触发阈值

  • minimum_travel_distance: 0.1 (米)
    机器人位移超过 10 厘米，触发一次地图写入。
  • minimum_travel_heading: 0.174 (弧度)
    机器人转动了0.174 (弧度)，才触发一次地图更新。

4.5.3 扫描匹配与处理

  • use_scan_matching: true
    强制开启激光比对。
  • minimum_score: 0.1
    表示 激光匹配（scan matching）的最低匹配评分阈值。只有当 激光扫描与当前地图的匹配分数 ≥ 该值 时，匹配结果才会被接受，否则会被认为匹配失败。
  • max_laser_range: 15.0
    SLAM 算法在建图和匹配时使用的激光雷达最大有效距离（单位：米）

4.5.4 内存与性能管理

  • mode: mapping
    当前处于“建图模式”。未来你可以切换到 localization（纯定位模式）。
  • map_update_interval: 1.0
    表示 地图更新（发布 /map 话题）的时间间隔，单位是 秒（s）。
  • scan_buffer_size: 10
    表示 SLAM 内部用于匹配的激光扫描缓存数量（buffer size）。

4.5.5 后端优化 (闭环检测)

这是 Gmapping 所没有的高级功能。

  • do_loop_closing: true
    当机器人回到去过的地方，它会自动“对齐”地图，消除累积误差。
  • loop_search_maximum_distance: 3.0
    在这个范围内寻找回环机会。

5. 2D导航

2D 导航主要用于 移动机器人在已知或部分已知环境中规划路径和避障，目标是：

  • 从机器人当前位置移动到目标位置。
  • 避免静态和动态障碍物。
  • 可以根据不同策略调整行为（如最快路径 vs 最安全路径）。
  • 提供导航状态和反馈接口给上层应用。

在 ROS 2 中，2D 导航通常依赖 地图 + 激光雷达/LiDAR/深度摄像头 + 运动控制器 来实现。

通俗的讲， 可以将其理解为机器人的“自动驾驶系统”。它的核心任务是解决以下三个问题：

  • 我在哪？（通过 SLAM 或定位算法，如 amcl 结合地图确定位置）。
  • 我要去哪？（接收目标点任务）。
  • 我怎么过去？（计算最优路径，并控制电机避障移动）。

5.1 ROS 2 Nav2（Navigation2）

其核心架构是 基于“行为树 (Behavior Trees)”的决策大脑。这是 Nav2 与传统简单逻辑最大的区别。

  • 非线性逻辑：传统的导航可能只是简单的 if-else。Nav2 使用行为树 (BT) 来管理逻辑。
  • 任务编排：行为树决定了机器人什么时候该“规划路径”、什么时候该“避障”、如果“卡住了（Recovery）”该怎么办（例如原地旋转或后退）。

你可以把它理解为一个高度模块化的任务调度器，它不硬死代码，而是通过加载一个 XML 文件来改变机器人的决策逻辑。

5.1.1 关键组件的底层逻辑

你可以把 Nav2 看作由几个相互协作的“后台服务（Nodes）”组成：

  • Map Server (地图服务器)：
    • 作用：读取并发布静态地图文件（通常是 .yaml 和 .pgm）。地图文件本质上是一个像素数组，0 代表空地，255 代表墙。
  • Planner (全局规划器 - “导航地图”)：
    • 作用：在已知地图上，计算从 A 到 B 的最优几何路径。它不考虑机器人当前的惯性或速度。
    • 算法：通常使用 A* 或 Dijkstra 算法。
  • Controller (局部控制器 - “实时驾驶员”)：
    • 作用：这是最核心的部分。它获取 Planner 给出的路径，结合实时传感器（Lidar/雷达）数据，计算出底盘电机需要的 线速度 (Linear) 和 角速度 (Angular)。这其实是一个闭环控制算法（如 DWB 或 TEB），它必须实时运行，以确保机器人不会撞上突然出现的障碍物。
  • Recovery Server (恢复行为)：
    • 作用：当 Controller 发现走不动了（比如被围住了），它会启动自救程序。

5.1.2 Nav2靠什么“感知”和“控制”？

  • 输入 (Input)
    • /tf (Transform Tree)：这是 Nav2 的骨架。它描述了机器人各部件（激光雷达、轮子、底盘中心）之间的相对坐标关系。
    • /scan 或 /pointcloud：Lidar 采集到的点云数据。
    • /odom (Odometry)：里程计数据，告诉系统机器人理论上走了多远（通常由电机编码器计算得出）。
  • 输出 (Output)：
  • /cmd_vel：Nav2 计算出的控制指令。这通常是一个包含 linear.x (前后) 和 angular.z (转弯) 的结构体。你需要写一个简单的驱动节点，把这个指令转成底盘电机的 PWM 或 CAN 总线控制码。

5.1.3 空间理解：代价地图 (Costmaps)

Nav2 不直接在原始地图上开车，它会在地图上覆盖一层代价地图 (Costmap)。
考虑到机器人的物理尺寸（半径），它会将地图上的像素点进行加权。靠近墙壁的点代价高，空地的代价低。这样在路径搜索时，机器人会自动保持与墙壁的安全距离。

Logo
全球具身智能开发者社区

立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。

更多推荐

cover

Mac 用户如何玩转 LeRobot？M3 芯片部署6 轴机械臂SoArm101经验总结 第3篇

avatar 全球具身智能开发者社区
cover

2026世界人工智能大会亮点多：AI产业走向现实交付，多款创新产品亮相！

avatar 全球具身智能开发者社区
cover

2026具身智能迈向场景落地期，库萨科技全栈技术能否打通城市服务机器人规模化之路？

avatar 全球具身智能开发者社区