ROS2学习（建图与定位）

原创已于 2025-10-13 17:03:31 修改 · 740 阅读

21 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #python #机器人

于 2025-10-13 17:01:10 首次发布

ROS2 专栏收录该内容

2 篇文章

订阅专栏

建图和定位笔记

1️⃣ 获取环境数据

传感器类型
建图和定位最常用的数据来源是传感器，这些传感器可以提供环境的“感知信息”：

传感器	数据类型	优点	常用场景
LiDAR	激光点云（2D/3D）	精度高，受光照影响小	室内/室外地图建图、导航
RGB-D 摄像头	彩色图 + 深度图	可以获取丰富的纹理信息	室内小环境建图
IMU	加速度、角速度	提供姿态估计	辅助里程计、惯性导航
轮速计	轮子转速	估算位移	简单里程计（Odometry）

流程：

机器人启动后，传感器开始采集数据（比如 LiDAR 扫描周围环境）。
数据通过 ROS2 的 topic 发布，例如 /scan 对应 2D 激光扫描，或者 /velodyne_points 对应 3D 点云。

2️⃣ 数据处理

拿到原始传感器数据后，还需要做一些预处理才能用来建图和定位：

滤波（Filtering）：去掉噪声点，例如 LiDAR 会有飞点或者远处虚假反射。
下采样（Downsampling）：3D 点云通常很大，需要减少点数以加快算法速度。
坐标转换（TF）：把传感器坐标系的数据转换到机器人 base_link 坐标系下，保证不同传感器数据对齐。

3️⃣ 建图（Mapping）

建图的目标是把环境表示成地图，一般有几种地图类型：

地图类型	描述	数据结构
栅格地图（Occupancy Grid Map）	2D 网格，每个格子记录占用概率	`nav_msgs/OccupancyGrid`
点云地图（Point Cloud Map）	3D 点云，每个点是环境表面一个点	`sensor_msgs/PointCloud2`
八叉树（OctoMap）	3D 占据概率的体素地图	内存友好，支持动态更新

流程举例（点云地图 SLAM）：

传感器数据输入 → LiDAR 扫描一帧点云。
点云匹配 / 配准（Scan Matching） → 当前帧点云和历史帧或已有地图对齐，求得机器人相对运动（位姿）。
- 常用算法：ICP（Iterative Closest Point）、NDT（Normal Distributions Transform）
地图更新 → 将对齐后的点云累积到地图中，生成环境点云地图。
优化（可选） → 用图优化或闭环检测（Loop Closure）修正累计误差，生成全局一致地图。

这就是 SLAM（Simultaneous Localization and Mapping）同时完成建图和定位的核心原理。

4️⃣ 定位（Localization）

定位的目标是知道机器人在地图中的位置。方法可以分为：

里程计 / IMU 融合
- 通过轮速计 + IMU 估计运动，得到相对位姿（增量），容易漂移。
基于地图的定位（Map-based Localization）
- 有了地图后，用传感器实时扫描，和地图做匹配，校正位置。
- 常用算法：
  - AMCL（Adaptive Monte Carlo Localization）：粒子滤波定位。
  - ICP / NDT 点云匹配定位。
- 流程：
  1. 获取传感器最新扫描（点云/激光线）。
  2. 在已有地图上找最佳匹配，计算机器人位姿。
  3. 更新机器人状态，用于导航。

5️⃣ 从传感器数据到点云地图的总结

采集 → LiDAR/深度相机/IMU
预处理 → 坐标变换 + 滤波
位姿估计 → 当前帧与历史帧匹配
累积更新 → 添加到地图
优化 → 闭环检测修正偏差

最终得到的就是点云地图或栅格地图，可以用于导航和路径规划。

明白，你想看的是 建图（Mapping）和定位（Localization）在 ROS2 框架下的模块级协调，以及各模块之间是怎么调用的。我们可以从整体架构、数据流、ROS2 机制和调用方式来分析。

1️⃣ 模块架构概览

在 ROS2 的移动机器人系统中，建图和定位通常被拆成几个节点（Node），互相协作：

模块	功能	ROS2 实现示例
传感器节点（Sensor Node）	发布原始数据，如 LiDAR、IMU	`velodyne_driver`、`rplidar_node`
里程计节点（Odometry Node）	根据轮速计/IMU计算增量位姿	`robot_localization`、`diff_drive_controller`
建图节点（Mapping Node）	接收传感器 + 里程计数据，构建地图	`slam_toolbox`、`cartographer`
定位节点（Localization Node）	利用已有地图计算机器人全局位置	`amcl`、`ndt_localizer`
导航节点（Nav Node）	路径规划 + 运动控制	`nav2_controller`、`nav2_planner`
TF 树（tf2）	管理各个坐标系的关系	`tf2_ros`

2️⃣ 数据流和协调方式

以一个典型 2D LiDAR + 栅格地图的例子：

LiDAR -> /scan
            \
             -> Mapping Node (/slam_toolbox/map) -> OccupancyGrid
Odometry -> /odom ----/
                           \
                            -> Localization Node (/amcl/pose) -> 全局位姿 -> Nav Node

关键点：

消息通信（Topic）
- 建图节点订阅 /scan、/odom 等 topic，发布 /map。
- 定位节点订阅 /map 和 /scan，输出机器人位姿 /amcl_pose。
- 所有模块通过 ROS2 topic 异步通信，模块间不直接调用函数，而是通过消息流协调。
坐标转换（TF）
- 建图和定位节点都需要知道传感器坐标系和机器人 base_link 之间的变换。
- ROS2 的 tf2 会广播这些坐标系关系，例如：
```
base_link -> laser -> map
```
- 定位节点可以把激光点云投影到全局地图坐标系中做匹配。
服务调用（Service）
- 一些模块也提供服务接口，比如建图节点可以提供 save_map 服务，外部节点调用保存地图。
- ROS2 服务是同步请求/响应，topic 是异步数据流。
参数协调
- 建图和定位节点通过 YAML 文件配置参数，例如：
  - 建图：resolution, use_sim_time, loop_closure
  - 定位：initial_pose, particle_count, update_rate
- 参数在 launch 文件中加载，各节点按参数行为协作。

3️⃣ ROS2 中建图 & 定位协调细节

（1）建图阶段

机器人启动 SLAM 节点：

ros2 launch slam_toolbox online_async_launch.py

节点订阅：
- /scan → LiDAR 数据
- /odom → 里程计数据
节点发布：
- /map → 实时更新的地图
- /tf → 坐标变换
可选：
- /slam_toolbox/save_map → 保存地图

（2）定位阶段

加载已保存地图：

ros2 launch nav2_bringup localization_launch.py map:=<map.yaml>

节点订阅：
- /scan → 实时激光扫描
- /map → 已知地图
节点发布：
- /amcl_pose → 机器人全局位姿
- /tf → 坐标系关系
定位节点会不断把当前扫描匹配到地图上，输出修正位姿。

4️⃣ 模块协调关键总结

协调方式	说明
Topic 异步发布订阅	建图输出 `/map` → 定位订阅 `/map`，解耦模块
TF 坐标变换	各节点可以在统一坐标系下处理数据
参数文件 / Launch 文件	各节点初始化参数，保证行为一致
Service / Action	可触发保存地图、初始化定位等操作

总结一句话：建图节点负责生成地图（map），定位节点利用地图校正位姿（pose），通过 ROS2 topic 和 TF 交换信息，形成闭环协作，而不直接调用对方函数。

建图逻辑

[硬件层]
   |
   ├─ LiDAR（2D/3D）       ---> 点云 / LaserScan
   ├─ IMU                   ---> 加速度 / 角速度 / 姿态
   ├─ 轮速计 / Odometry      ---> 位移增量
   ├─ RGB-D 摄像头          ---> 彩色图 + 深度图
   └─ GNSS/GPS（室外）       ---> 经纬度 / 高度
   |
   v
[驱动节点 / 数据采集层]
   |
   ├─ LiDAR 驱动节点        ---> 发布 ROS2 topic: /scan 或 /points
   ├─ IMU 节点              ---> 发布 /imu/data
   ├─ Odometry 节点         ---> 发布 /odom
   └─ RGB-D 驱动节点         ---> 发布 /camera/depth/points
   |
   v
[ROS2 Topic / TF 层] （数据总线）
   |
   ├─ Topic 异步消息
   │    ├─ /scan           ---> LiDAR 扫描
   │    ├─ /points         ---> 3D 点云
   │    ├─ /odom           ---> 里程计位姿
   │    ├─ /imu/data       ---> IMU 信息
   │    └─ /camera/depth/points ---> 深度点云
   |
   └─ TF 坐标变换
        ├─ base_link -> laser
        ├─ base_link -> imu_link
        ├─ base_link -> camera_link
        └─ map -> odom -> base_link
   |
   v
[建图/定位模块层]
   |
   ├─ **建图（Mapping）节点**
   │    ├─ 订阅：/scan, /odom, /imu/data
   │    ├─ 输出：/map（OccupancyGrid 或 PointCloud2）
   │    ├─ 发布 /tf（map->odom）
   │    └─ 示例：`slam_toolbox`, `cartographer`
   |
   └─ **定位（Localization）节点**
        ├─ 订阅：/scan, /map, /odom
        ├─ 输出：/amcl_pose（机器人全局位姿）
        ├─ 发布 /tf（map->base_link）
        └─ 示例：`amcl`, `ndt_localizer`
   |
   v
[导航 & 控制层]
   |
   ├─ 路径规划节点 / Planner
   ├─ 控制器节点 / Controller
   └─ 小车底盘执行运动

核心逻辑

硬件产生数据 → 驱动节点转成 ROS2 topic → 发布到总线上
建图节点订阅数据 → 累积生成地图 → 发布 /map 和 TF
定位节点订阅 /map + 实时扫描 → 匹配生成全局位姿 → 发布 /amcl_pose
导航模块订阅位姿和地图 → 规划路径 → 控制底盘运动

关键是 topic 异步订阅（发布和订阅时间不同步）+ TF 坐标变换，建图和定位模块不直接调用对方函数，而是通过 ROS2 的消息和坐标体系协作

概念解析：TF树

1️⃣ TF 树到底是什么？

TF（Transform） 是 ROS2 用来管理不同坐标系之间关系的工具。

为什么需要 TF：
在机器人中，不同传感器的数据都有自己的坐标系：
- LiDAR：laser_link
- IMU：imu_link
- 轮式里程计：odom_link
- 机器人底盘中心：base_link
- 地图参考系：map

如果你想把 LiDAR 扫描投影到地图上，必须知道 map -> base_link -> laser_link 的关系。
TF 树就是保存这些关系的树状结构，每一条边都是一个位姿变换（平移 + 旋转）。

例子：

map
 └─ odom
     └─ base_link
         ├─ laser_link
         └─ imu_link

作用：
1. 建图节点用 TF 将传感器坐标转换到机器人坐标系，再累积到地图坐标系。
2. 定位节点用 TF 将当前扫描投影到地图坐标系，匹配地图，得到全局位姿。

2️⃣ 数据流总结（结合你描述的理解）

[硬件层]
   LiDAR / IMU / 轮速计
        |
[驱动节点]
   发布 topic (/scan, /odom, /imu/data)
        |
[ROS2 中枢]
   Topic 异步发布/订阅
   TF 树管理坐标系变换
        |
[建图节点]
   订阅: /scan, /odom, /imu/data
   坐标变换: laser_link -> base_link -> map
   输出: /map (点云或栅格)
        |
[定位节点]
   订阅: /map, /scan, /odom
   坐标变换: laser_link -> base_link -> map
   输出: /amcl_pose (机器人全局坐标)
        |
[导航节点]
   订阅: /amcl_pose, /map
   输出: 控制命令给底盘

3️⃣ 示例项目（建图 + 定位演示）

示意项目结构。

my_robot_slam_project/
├─ launch/
│   ├─ slam_launch.py          # 启动建图节点（slam_toolbox）
│   └─ localization_launch.py  # 启动定位节点（AMCL）
│
├─ config/
│   ├─ slam_params.yaml        # 建图参数
│   ├─ amcl_params.yaml        # 定位参数
│   └─ robot_tf.yaml           # TF 树静态坐标关系
│
├─ src/
│   ├─ nodes/
│   │   ├─ lidar_driver_node.py
│   │   ├─ imu_driver_node.py
│   │   └─ odom_node.py
│   ├─ mapping/
│   │   └─ slam_toolbox_node.py
│   └─ localization/
│       └─ amcl_node.py
│
├─ rviz/
│   └─ slam_rviz_config.rviz   # 用于可视化地图和机器人
└─ README.md

🔹 节点说明

节点	订阅	发布	说明
lidar_driver_node	无	/scan	将 LiDAR 原始数据发布为 ROS2 topic
imu_driver_node	无	/imu/data	发布 IMU 数据
odom_node	无	/odom	发布轮式里程计里程信息
slam_toolbox_node	/scan, /odom, /imu/data	/map, /tf	建图节点，生成地图
amcl_node	/scan, /map, /odom	/amcl_pose, /tf	定位节点，输出机器人全局位姿
nav2_node	/amcl_pose, /map	/cmd_vel	路径规划和运动控制

🔹 TF 树示意

map
 └─ odom
     └─ base_link
         ├─ laser_link
         └─ imu_link

当 LiDAR 扫描数据到达建图/定位节点时：
1. 用 TF 将 laser_link 转到 base_link
2. 再将 base_link 转到 map 坐标系
3. 这样才能把点云正确投影到地图上
定位节点类似，最终输出 /amcl_pose 在 map 坐标系下。

总结理解：

硬件采集数据 → 驱动节点发布 topic
建图节点订阅 topic + TF 坐标变换 → 输出地图
定位节点订阅地图 + topic + TF → 输出机器人全局位姿
导航节点订阅位姿 + 地图 → 控制底盘运动

TF 树是整个系统的坐标转换工具，保证所有传感器和地图在统一坐标系下工作。