引言

在移动机器人的自主导航系统中，定位（Localization） 和 导航（Navigation） 是两大核心技术。简单来说，导航系统要解决三个核心问题：

1. “我在哪里？” —— 定位
2. “我要去哪里？” —— 规划
3. “我怎么去？” —— 控制

本文将从坐标系这个最基础的概念讲起，逐步深入ROS2的定位与导航实现原理，帮助读者理解整个技术栈的工作机制。

---

一、TF坐标变换：机器人的“骨骼系统”

1.1 为什么需要坐标系？

想象一台移动机器人，它身上装着激光雷达、IMU（惯性测量单元）、摄像头等多个传感器。每个传感器都有自己的“视角”——激光雷达看到的是以激光雷达为中心的障碍物距离，IMU测量的是以IMU为中心的加速度和角速度。

如果这些传感器各说各话，机器人就无法统一理解世界。TF2（Transform Framework 2） 就是让所有部件“说同一种位置语言”的翻译官。

1.2 标准坐标系命名规范

在ROS2中，有一套通用的坐标系命名规范：

坐标系	含义	特性
map	全局地图坐标系	固定不变，是整棵变换树的根节点
odom	里程计坐标系	局部动态，短距离精确但会累积漂移
base_link	机器人本体中心	机器人底盘的核心坐标系
base_footprint	机器人与地面接触点	base_link在地面的投影
laser_link	激光雷达坐标系	固定在机器人上的传感器坐标
imu_link	IMU坐标系	固定在机器人上的传感器坐标

1.3 变换树（Transform Tree）

所有坐标系构成一棵无环有向树（Transform Tree）。每个子坐标系只能有一个父坐标系，整棵树必须有根节点（通常是 map）。

下面是一个典型的机器人TF树：

这棵树告诉我们：

• map 是全局根节点
• odom 相对于 map 的位置在不断变化（由定位系统更新）
• base_link 相对于 odom 的位置在实时更新（由里程计推算）
• imu_link 和 laser_link 相对于 base_link 的位置是固定的（螺丝拧上去就不会动了）

1.4 静态变换与动态变换

TF2支持两种变换发布方式：

静态变换（Static Transform） ：描述两个坐标系之间固定不变的关系，如 base_link → laser_link。使用 static_transform_publisher 发布一次即可，数据会保存在 /tf_static 话题中。

ros2 run tf2_ros static_transform_publisher 0 0 0.2 0 0 0 base_link laser_link

动态变换（Dynamic Transform） ：描述随时间变化的坐标系关系，如 odom → base_link（机器人移动时不断变化）。使用 TransformBroadcaster 持续发布到 /tf 话题。

1.5 变换查询

有了TF树，任何节点都可以查询任意两个坐标系之间的变换关系：

ros2 run tf2_ros tf2_echo map base_link

这个命令会实时输出 base_link 在 map 坐标系中的位置和姿态。在我们的实际调试中，这条命令成功输出了类似这样的结果：

At time 1782371112.452141823
- Translation: [-0.383, 0.075, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.035, 0.999]
- Rotation: in RPY [0.000, -0.000, 4.056] degrees

这说明机器人的 base_link 在地图坐标系中的位置是 (-0.383m, 0.075m)，朝向约 4.056°。

---

二、传感器融合定位：EKF如何“算”出位置

2.1 单一传感器的局限

移动机器人的定位不能只靠一个传感器：

• 轮式里程计：通过车轮编码器计算行走距离。短距离很准，但地面打滑或轮胎磨损时会产生累积误差——走久了位置会越来越偏。
• IMU（惯性测量单元） ：测量加速度和角速度。短期姿态很准，但长期会漂移——积分的误差会越来越大。

2.2 EKF扩展卡尔曼滤波

扩展卡尔曼滤波（EKF） 的核心思想是：融合多个传感器的数据，取长补短。它能融合任意数量的传感器，包括多个IMU或多个里程计。

在我们的系统中，EKF节点（ekf_node）订阅两个输入：

输入主题	数据来源	作用
/wheel_odom	车轮编码器	提供线速度（vx），推算短距离位移
/imu_raw	IMU传感器	提供姿态角（yaw），修正方向偏差

2.3 EKF配置实战

以下是经过实测的EKF配置文件（ekf.yaml）关键参数：

ekf_filter_node:
  ros__parameters:
    # 运行频率：100Hz（每秒计算100次）
    frequency: 100.0
    
    # 坐标系定义
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom   # EKF输出参考系
    
    # 里程计输入：融合线速度（vx）
    odom0: /wheel_odom
    odom0_config: [false, false, false,   # x, y, z 位置（不融合）
                   false, false, false,   # roll, pitch, yaw 角度（不融合）
                   true, false, false,    # vx, vy, vz 线速度（只融合vx）
                   false, false, false]   # 角速度（不融合）
    
    # IMU输入：融合偏航角（yaw）
    imu0: /imu_raw
    imu0_config: [false, false, false,   # x, y, z 位置（不融合）
                  false, false, true,    # roll, pitch, yaw 角度（只融合yaw）
                  false, false, false,   # 线速度（不融合）
                  false, false, false]   # 角速度（不融合）

2.4 EKF的输出

EKF节点融合数据后，发布两个关键输出：

输出	内容	用途
/odometry/filtered	融合后的里程计数据（位置+速度+协方差）	供导航、控制节点使用
/tf	odom → base_link 动态变换	更新TF树，让所有节点知道机器人位置

当EKF正常运行时，/odometry/filtered 的频率可以达到 ~100Hz，输出类似：

position:
  x: 0.148
  y: 0.042
  z: 0.0
orientation:
  z: 0.031
  w: 0.999

---

三、AMCL：让机器人知道“我在哪张地图上”

3.1 为什么还需要AMCL？

EKF虽然能融合里程计和IMU，但它只能推算相对运动——它知道“我从刚才的位置走了多远”，但不知道“我在地图上的绝对位置”。

如果机器人被抱起来放到另一个位置，或者轮子打滑导致推算严重偏移，EKF就无能为力了。这时候需要AMCL（自适应蒙特卡洛定位） 来救场。

3.2 AMCL的工作原理

AMCL是一种概率定位算法，它的工作方式是：

1. 撒粒子：在地图上随机生成大量“假设位置”（粒子）
2. 匹配地图：用激光雷达扫描当前环境，和地图进行比对
3. 更新权重：扫描结果和地图匹配度越高的粒子，权重越大
4. 重采样：淘汰权重低的粒子，在权重高的粒子附近生成新粒子
5. 迭代：重复以上过程，粒子逐渐收敛到机器人的真实位置

3.3 AMCL在TF树中的角色

AMCL发布的是 map → odom 变换。它的作用可以这样理解：

• EKF说：“根据我的推算，机器人在odom坐标系中的位置是 (10, 5)。”
• AMCL说：“根据我扫描地图的结果，odom坐标系原点在地图坐标系中的位置是 (-10.3, -14.3)。”
• 两者结合，就能算出机器人在地图中的绝对位置。

这就是为什么TF树中需要 map → odom → base_link 这条完整链条：

map（地图） → odom（里程计原点修正） → base_link（机器人本体）

3.4 启动AMCL

AMCL通常由Nav2的launch文件启动，也可以单独运行：

ros2 run nav2_amcl amcl --ros-args --params-file /path/to/amcl_params.yaml

关键参数包括：

• min_particles / max_particles：粒子数量（通常500~2000）
• odom_model_type：里程计模型类型（差分/全向）
• laser_model_type：激光雷达模型（似然场/波束）

---

四、Nav2导航堆栈：从“我在哪”到“怎么去”

4.1 Nav2架构概览

Nav2（Navigation2）是ROS2的官方导航框架，解决了“在哪里、去哪里、怎么去”三大问题。其核心架构由三大支柱支撑：

1. 行为树（Behavior Tree） ：任务调度中枢，定义导航任务的执行逻辑
2. 生命周期节点（Lifecycle Nodes） ：支持组件的动态启停与重启
3. 插件化设计：支持全局规划器、局部控制器等模块的动态替换

4.2 Costmap 2D：机器人的“障碍物地图”

Nav2使用Costmap 2D（代价地图） 来表示环境中的障碍物信息。它采用五层架构：

层级	功能
Static Map Layer	SLAM构建的静态地图
Obstacle Layer	激光雷达/深度相机的实时障碍物数据
Inflation Layer	障碍物的膨胀区域（安全距离）
Voxel Layer	三维点云处理（检测悬空/地面障碍）
Social Layer	人群移动预测（可选）

这种分层设计的优势在于：

• 全局地图与局部地图分离，更新互不干扰
• 动态障碍物与静态障碍物使用不同代价层
• 支持多传感器融合

4.3 导航工作流程

Nav2完成一次导航任务的典型流程：

1. 接收目标点（NavigateToPose action）
2. 全局规划（Global Planner）：计算从当前位置到目标点的全局路径
   ↓
3. 局部规划（Local Planner / Controller）：根据全局路径和实时障碍物，生成局部轨迹
   ↓
4. 控制执行：将速度指令发布到 /cmd_vel，驱动机器人移动
   ↓
5. 到达检查：判断是否到达目标点
   ↓
6. 完成 / 失败（触发恢复行为）

4.4 行为树调度

Nav2使用行为树（Behavior Tree） 替代了ROS1的固定状态机。行为树的核心优势：

• 灵活性：可以动态修改任务流程
• 可复用性：子树可被多个任务共享
• 可调试性：可通过Groot等工具实时监控

一个典型的导航行为树结构：

NavigateToPose
  └─ Sequence（顺序执行）
       ├─ ComputePathToPose（计算路径）
       └─ FollowPath（跟随路径）
            └─ Fallback（失败时执行恢复）
                 ├─ 正常跟随
                 └─ 恢复行为（旋转、后退等）

---

五、实战案例：从TF缺失到定位恢复

在实际调试中，我们遇到了一个典型问题：pose 服务报错 Transform not available yet。

5.1 问题现象

journalctl -u pose -f
[WARN] [pose_publisher]: Transform not available yet

5.2 排查过程

第一步：检查TF树

ros2 run tf2_tools view_frames

生成的 frames.pdf 显示 map → odom → base_link 链条完整，说明TF发布正常。

第二步：测试具体变换

ros2 run tf2_ros tf2_echo map base_link

最初几秒报错 Invalid frame ID "map"，但几秒后开始正常输出。这说明 AMCL启动较慢，map 帧还未发布时 pose 就已经开始查询了。

第三步：检查服务启动顺序

cat /etc/systemd/system/pose.service

发现服务文件中只有 sleep 10 的延迟，而AMCL可能需要更长时间才能就绪。

5.3 解决方案

方案一：增加服务启动延迟
在服务文件中将 sleep 10 改为 sleep 30，给AMCL足够的时间加载地图和初始化。

方案二：在脚本中添加重试逻辑
在Python脚本中使用 waitForTransform 替代 lookupTransform，设置超时并循环重试：

def get_transform(self, target_frame, source_frame, timeout_sec=5.0):
    try:
        now = self.get_clock().now()
        transform = self.tf_buffer.lookup_transform(
            target_frame, source_frame, now, 
            timeout=rclpy.duration.Duration(seconds=timeout_sec)
        )
        return transform
    except Exception as e:
        self.get_logger().warn(f"Transform lookup failed: {e}")
        return None

5.4 经验总结

这类问题的根本原因是节点启动顺序和依赖就绪时间不匹配。在ROS2系统中，建议：

1. 使用生命周期节点管理依赖关系
2. 在代码中主动等待依赖的TF变换就绪
3. 使用 tf2_ros::Buffer::waitForTransform 而非直接 lookupTransform

---

六、总结

本文从坐标系开始，逐步讲解了ROS2机器人定位与导航的完整技术栈：

层级	技术组件	解决的问题
坐标框架	TF2	统一所有传感器的坐标系
传感器融合	robot_localization (EKF)	融合里程计+IMU，推算相对位置
全局定位	AMCL	匹配地图+激光，确定绝对位置
导航决策	Nav2	路径规划+运动控制+异常恢复

整套系统像一条流水线协同工作：

1. 硬件层：激光雷达、车轮编码器、IMU持续产生原始数据
2. EKF层：融合轮速和IMU，发布 odom → base_link 动态变换
3. AMCL层：匹配激光扫描与地图，发布 map → odom 变换
4. Nav2层：基于完整TF树进行路径规划，通过 /cmd_vel 控制机器人移动

当这条链路完整打通时，机器人就能回答那三个核心问题了：“我在哪？”、“去哪？”、“怎么去？”——这正是自主移动机器人的智慧所在。

老徐，2026/07/15，入伏，晴