第4章坐标变换基础

第四章：坐标变换基础

做AMR路径规划，绕不开一个基础问题：机器人怎么知道自己在哪？

嗯，这听起来像哲学问题。但落实到代码层面，就是坐标变换。

我刚开始做机器人那会儿，总觉得坐标变换就是个数学工具，调个库就完事了。直到有一次，我的机器人在走廊里原地转圈，死活找不到路。排查了半天，发现是TF树里一个父坐标系挂错了。从那以后，我再也不敢小看这个“基础”了。

4.1 TF坐标变换原理

说白了，TF（Transform）就是描述两个坐标系之间的相对关系。你想想看，机器人身上有激光雷达、有轮子、有摄像头，每个传感器都有自己的坐标系。

激光雷达说“前方1米有障碍物”，这个“前方”是雷达自己的前方。但机器人要避障，得知道这个障碍物相对于机器人底盘的位置。这就需要坐标变换。

核心公式：

一个点P在坐标系A中的坐标，可以通过变换矩阵T_AB转换到坐标系B中：

P_B = T_AB · P_A

其中T_AB包含旋转和平移信息。

我个人习惯把TF理解成“坐标系之间的桥梁”。每个桥梁都有两个属性：

平移（Translation）：两个坐标系原点之间的位移，用(x, y, z)表示
旋转（Rotation）：两个坐标系之间的朝向差异，用四元数或欧拉角表示

我记得有一次调试，发现里程计数据总是漂移。查了半天，原来是四元数归一化出了问题。嗯，这里要注意：四元数必须归一化，否则变换矩阵会变形。

4.2 ROS中TF树的维护

ROS里，TF是以树形结构组织的。为什么是树？因为坐标系之间不能有循环依赖。你想想看，如果A依赖B，B又依赖A，那谁也找不到谁。

一个典型的AMR的TF树长这样：

这张图我画了很多遍。你看，map到odom是里程计TF，会随时间变化。odom到base_link也是动态的。但base_link到各个传感器，通常是静态的——装上去就不动了。

实战技巧：

在ROS中维护TF树，我建议用tf2_ros库。发布静态TF用StaticTransformBroadcaster，动态TF用TransformBroadcaster。

// 发布静态TF示例
#include <tf2_ros/static_transform_broadcaster.h>

geometry_msgs::TransformStamped t;
t.header.stamp = ros::Time::now();
t.header.frame_id = "base_link";
t.child_frame_id = "laser";
t.transform.translation.x = 0.2;
t.transform.translation.y = 0.0;
t.transform.translation.z = 0.1;
t.transform.rotation.w = 1.0;

static_broadcaster.sendTransform(t);

避坑指南：

我曾经犯过一个错误：在回调函数里频繁创建TransformBroadcaster对象。结果内存泄漏，机器人跑了半小时就挂了。

正确做法：全局只创建一个broadcaster，在循环里更新它的内容。

4.3 坐标变换在路径规划中的作用

路径规划说白了就是回答三个问题：

我在哪？ —— 定位，需要map到base_link的变换
周围有什么？ —— 感知，需要传感器数据转换到全局坐标系
怎么走？ —— 规划，需要把目标点从全局坐标系转换到机器人坐标系

我举个例子。假设全局规划器给了一个目标点，坐标是(5.0, 3.0)，单位是米，在map坐标系下。但机器人控制指令需要的是相对于base_link的坐标。这时候就需要：

// 伪代码：坐标变换在路径规划中的应用
// 1. 获取当前位姿
tf2::Transform map_to_base = lookupTransform("map", "base_link");

// 2. 将目标点从map转换到base_link
tf2::Vector3 goal_map(5.0, 3.0, 0.0);
tf2::Vector3 goal_base = map_to_base.inverse() * goal_map;

// 3. 计算控制指令
double dx = goal_base.x();
double dy = goal_base.y();
double angle = atan2(dy, dx);

你看，没有坐标变换，这一步根本做不了。

核心要点：

全局路径规划在map坐标系下进行
局部路径规划在odom或base_link坐标系下进行
传感器数据必须转换到统一坐标系才能融合
控制指令必须在机器人坐标系下发出

我记得有一次做多传感器融合，激光雷达和摄像头各自发布数据。激光说“前方1米有墙”，摄像头说“前方0.8米有墙”。到底信谁？

后来发现，是激光雷达的TF没更新。它装在机器人顶部，但安装位置变了，TF还是旧的。更新了静态TF后，两个传感器数据就对齐了。

嗯，这里要强调一点：TF的精度直接影响路径规划的效果。TF差1厘米，规划出来的路径可能就偏10厘米。尤其是在狭窄通道里，这点误差就够让机器人卡住了。

我的调试习惯：

每次启动机器人，我都会先跑tf2_monitor看看TF树的延迟和频率。如果某个TF的延迟超过100ms，那路径规划肯定出问题。

另外，tf2_echo也是个好工具，可以实时查看两个坐标系之间的变换关系。

总结一下今天的内容。坐标变换不是花架子，它是AMR路径规划的基石。从传感器数据融合，到路径规划，再到控制指令下发，每一步都离不开TF。你想想看，如果连自己在哪都不知道，还谈什么路径规划？

下一章，我们会深入讲地图表示。但在此之前，我建议你把TF树理清楚。画一张你自己的TF树，标清楚每个坐标系之间的变换关系。这个习惯，能帮你省下很多调试时间。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录

第四章：坐标变换基础

4.1 TF坐标变换原理

4.2 ROS中TF树的维护

4.3 坐标变换在路径规划中的作用