引言

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

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

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

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

1.1 为什么需要坐标系？

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

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

1.2 标准坐标系命名规范

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

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_publisher000.2000base_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）订阅两个输入：

2.3 EKF配置实战

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

ekf_filter_node:ros__parameters:# 运行频率：100Hz（每秒计算100次）frequency:100.0# 坐标系定义map_frame:mapodom_frame:odombase_link_frame:base_linkworld_frame:odom# EKF输出参考系# 里程计输入：融合线速度（vx）odom0:/wheel_odomodom0_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_rawimu0_config:[false,false,false,# x, y, z 位置（不融合）false,false,true,# roll, pitch, yaw 角度（只融合yaw）false,false,false,# 线速度（不融合）false,false,false]# 角速度（不融合）

2.4 EKF的输出

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

当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（代价地图）来表示环境中的障碍物信息。它采用五层架构：

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

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

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-upose-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，设置超时并循环重试：

defget_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))returntransformexceptExceptionase:self.get_logger().warn(f"Transform lookup failed:{e}")returnNone

5.4 经验总结

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

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

六、总结

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

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

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

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