3. 坐标系基础：世界坐标系、机器人坐标系、轨迹坐标系、坐标变换
做轨迹跟踪，第一个绕不开的就是坐标系。我见过不少新手工程师，算法写得挺漂亮，结果一跑就飞，最后发现是坐标系搞反了。说白了，坐标系就是你和机器人沟通的「共同语言」。语言不通，啥都白搭。
这一章，咱们就把世界坐标系、机器人坐标系、轨迹坐标系这三个核心概念掰开揉碎，再聊聊它们之间怎么互相转换。嗯，这部分搞明白了，后面的误差分析才有根基。
3.1 世界坐标系：参考系
世界坐标系，也叫全局坐标系。你可以把它想象成整个场景的「标尺」。不管机器人怎么动，轨迹怎么画，最终都要回到这个坐标系里来度量。
通常我们用 {W} 表示，原点一般设在场景的某个固定点，比如工位的角落、地面的中心。三个轴 XW、YW、ZW 遵循右手定则。
关键点：世界坐标系是静止的。所有其他坐标系都是相对于它定义的。你规划的目标轨迹，通常也是在世界坐标系下描述的。
我在项目里遇到过一个问题：一台AGV在仓库里跑，激光SLAM建图时用的坐标系原点在仓库门口。后来换了工位布局，原点没变，但所有路径点都要重新标定。这就是世界坐标系选得不合理带来的麻烦。我个人习惯，世界坐标系的原点尽量选在场景的对称中心或长期不变的基准点上。
3.2 机器人坐标系：随体参考系
机器人坐标系，也叫本体坐标系，用 {R} 表示。它牢牢地「长」在机器人身上。原点通常在机器人的质心或几何中心，XR 轴指向机器人前进方向，YR 轴指向左侧，ZR 轴垂直向上。
为什么要单独搞一个机器人坐标系？因为传感器（激光雷达、摄像头、IMU）都装在机器人上，它们测得的数据天然就是相对于机器人自身的。你想想看，激光雷达扫到一个障碍物，它报告的是「障碍物在我前方2米」，而不是「障碍物在世界坐标系的(10, 5)处」。
| 坐标系 | 符号 | 原点 | 特点 |
|---|---|---|---|
| 世界坐标系 | {W} | 场景固定点 | 静止，参考 |
| 机器人坐标系 | {R} | 机器人质心 | 随机器人移动、旋转 |
| 轨迹坐标系 | {T} | 轨迹上的当前点 | 沿轨迹移动，方向随路径变化 |
小技巧：调试时，我习惯在可视化工具里同时显示世界坐标系和机器人坐标系。如果机器人坐标系的原点没有跟着机器人模型走，那一定是坐标变换参数没设对。我曾经因为这个排查了整整一个下午，最后发现是TF树里少写了一帧。
3.3 轨迹坐标系：路径上的移动参考系
轨迹坐标系 {T} 是专门为轨迹跟踪引入的。它沿着目标轨迹移动，原点在轨迹上距离机器人最近的点（或者当前跟踪的目标点）。
它的定义方式很讲究：
- XT 轴：沿轨迹的切线方向，指向轨迹前进方向。
- YT 轴：垂直于切线，指向轨迹的左侧（法线方向）。
- ZT 轴：垂直于轨迹平面（对于平面轨迹，就是垂直向上）。
为什么要引入它？因为在这个坐标系下看误差，最直观。机器人在轨迹坐标系下的 YT 方向偏差，就是横向误差；XT 方向偏差，就是纵向误差（或前向误差）。
说白了，轨迹坐标系把「机器人偏离轨迹多远」这个问题，变成了一个简单的二维坐标读数。
3.4 坐标变换：连接三个世界的桥梁
三个坐标系之间需要互相转换。最常用的变换是齐次变换矩阵 T，它包含旋转矩阵 R 和平移向量 t。
一个点 P 在坐标系 {A} 下的坐标 AP，转换到坐标系 {B} 下的坐标 BP，公式是：
B_P = R_A^B * A_P + t_A^B用齐次形式写就是：
| B_P | = | R_A^B t_A^B | * | A_P |
| 1 | | 0 1 | | 1 |实际工程中，我们最关心三个变换：
- 世界 → 机器人：
TWR，把全局路径点转换到机器人局部视角。 - 世界 → 轨迹：
TWT，根据轨迹参数方程实时计算。 - 机器人 → 轨迹：
TRT，这是误差计算的核心，直接给出机器人在轨迹坐标系下的位姿偏差。
注意：坐标变换的顺序不可交换。先旋转再平移，和先平移再旋转，结果完全不同。我见过有人把 R * P + t 写成了 R * (P + t)，结果轨迹跟踪曲线像喝醉了酒一样。嗯，这种bug最难查。
3.5 知识体系总览
下面这张图，把三个坐标系和它们之间的变换关系串起来了。你可以把它当作本章的「地图」。
3.6 实战：从传感器数据到误差计算
咱们走一遍完整的流程。假设机器人上有个激光雷达，测得一个障碍物在雷达坐标系 {L} 下的位置是 LP = (1.0, 0.5, 0)。
第一步：雷达坐标 → 机器人坐标
雷达装在机器人前方0.3米处，向右偏移0.1米。那么变换矩阵 TLR 的平移部分是 (0.3, -0.1, 0)，旋转是单位矩阵（假设雷达和机器人朝向一致）。
R_P = T_L^R * L_P
= (1.0 + 0.3, 0.5 - 0.1, 0)
= (1.3, 0.4, 0)第二步：机器人坐标 → 世界坐标
假设机器人当前在世界坐标系下的位姿是 (x=5.0, y=3.0, θ=30°)。我们需要用 TRW 来转换。
W_P = R(30°) * R_P + t
= [cos30 -sin30 0] [1.3] [5.0]
[sin30 cos30 0] * [0.4] + [3.0]
[0 0 1] [0 ] [0 ]
= [0.866 -0.5 0] [1.3] [5.0]
[0.5 0.866 0] [0.4] + [3.0]
[0 0 1] [0 ] [0 ]
= [0.925, 0.996, 0] + [5.0, 3.0, 0]
= (5.925, 3.996, 0)第三步：世界坐标 → 轨迹坐标
找到轨迹上距离 WP 最近的点，计算该点的轨迹坐标系 {T}，然后做变换。这一步通常由路径跟踪库（如纯追踪、MPC）自动完成。最终输出就是机器人在轨迹坐标系下的横向误差和纵向误差。
我的经验：在实际代码中，我建议把坐标变换封装成独立的函数或类。不要到处写矩阵乘法。ROS里的TF库就是个好例子，你只需要发布坐标系之间的静态变换和动态变换，剩下的查询和转换交给库去处理。这样代码干净，也不容易出错。
好了，坐标系这块地基算是打牢了。你想想看，有了这三个坐标系和它们之间的变换关系，我们就能精确地描述「机器人到底偏了多少」。下一层，咱们就要用这些工具去定义和计算具体的跟踪误差指标了。