坐标系与空间变换：全局坐标系、局部坐标系、齐次变换矩阵、欧拉角与四元数、坐标变换在跟踪中的应用
各位同学，今天我们来聊聊坐标系与空间变换。说实话，这是整个目标跟踪与路径规划里最基础、也最容易出坑的地方。我见过不少项目，算法本身没问题，结果坐标系搞反了，机器人直接往墙上撞。嗯，咱们今天就把这块彻底理清楚。
1. 全局坐标系与局部坐标系
先问个问题：你站在操场上，说「前面10米有棵树」。这个「前面」是谁的前面？是你的前面，还是操场大门的前面？
这就是全局坐标系和局部坐标系的区别。
- 全局坐标系（世界坐标系）：固定在地面上的一个参考系。比如地图上的经纬度，或者工厂里某个墙角作为原点。所有物体都可以用这个坐标系来描述位置。
- 局部坐标系（机体坐标系）：固定在机器人身上的坐标系。原点在机器人中心，x轴朝前，y轴朝左，z轴朝上。机器人自己感知到的「前方1米有障碍物」，就是在局部坐标系下说的。
核心要点：传感器数据通常是在局部坐标系下获取的，而路径规划需要在全局坐标系下进行。所以，你必须在两者之间来回切换。
我在做AGV小车项目时遇到过一个问题：激光雷达扫描到的障碍物坐标，直接拿来用了，没做坐标变换。结果小车明明看到左边有墙，却往左边拐。后来才发现，激光雷达装在车头偏右的位置，它的「前方」和车体的「前方」差了30度。你说坑不坑？
2. 齐次变换矩阵
好了，坐标系之间怎么转换？最优雅的方式就是用齐次变换矩阵。
一个4x4的矩阵，把旋转和平移打包在一起。结构是这样的：
| R t |
| 0 1 |其中R是3x3的旋转矩阵，t是3x1的平移向量。最后一行是[0 0 0 1]，这是齐次坐标的固定格式。
为什么要用4x4？说白了，就是为了把旋转和平移统一成一个矩阵乘法。你想想看，如果分开算，先旋转再平移，公式写起来麻烦，代码也容易出错。用齐次变换矩阵，一个矩阵乘法搞定一切。
我的习惯：在代码里，我通常用Eigen库的Isometry3d或者tf2::Transform来管理变换矩阵。手写矩阵容易写错，尤其是符号方向，我吃过这个亏。
3. 欧拉角与四元数
旋转怎么表示？两种主流方式：欧拉角和四元数。
欧拉角：绕三个轴依次旋转，比如先绕z轴转yaw，再绕y轴转pitch，最后绕x轴转roll。直观，好理解。但有个致命问题——万向锁。当pitch接近90度时，yaw和roll的旋转轴会重合，丢失一个自由度。我在做无人机仿真时遇到过，飞机姿态突然卡住，怎么调都调不回来，就是万向锁在作怪。
四元数：用四个数表示旋转，没有万向锁问题。形式是q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。虽然不直观，但计算稳定，插值平滑。
| 特性 | 欧拉角 | 四元数 |
|---|---|---|
| 直观性 | 高 | 低 |
| 万向锁 | 有 | 无 |
| 插值 | 困难 | 容易（球面插值） |
| 存储 | 3个值 | 4个值 |
避坑指南：我曾经在ROS里直接用欧拉角做姿态控制，结果在某个特定角度下，机器人开始疯狂抖动。后来换成四元数，问题立刻消失。所以，凡是涉及连续旋转的场合，我建议你直接用四元数。
4. 坐标变换在跟踪中的应用
讲完理论，咱们看看实际怎么用。目标跟踪中，坐标变换无处不在。
举个例子：你有一个移动机器人，上面装了摄像头。摄像头检测到目标在图像中的位置，你要控制机器人去跟踪这个目标。
流程是这样的：
- 摄像头检测到目标在图像坐标系下的像素坐标
- 通过相机内参，将像素坐标转换到相机坐标系下的三维坐标
- 通过相机到机器人本体的变换矩阵，将坐标转换到机器人局部坐标系
- 通过机器人当前位姿（全局坐标系下的位置和朝向），将坐标转换到全局坐标系
- 在全局坐标系下规划路径，生成控制指令
你看，每一步都是一次坐标变换。任何一个环节出错，目标位置就偏了。
我的经验：调试这类系统时，我习惯先做「正向验证」。就是让目标站在已知位置，然后反向推算传感器应该看到什么。如果推算结果和实际测量一致，说明变换矩阵是对的。否则，逐级检查。
另外，变换矩阵的更新频率也很关键。机器人一直在动，变换矩阵必须实时更新。我一般用卡尔曼滤波或者IMU数据来估计当前位姿，然后更新变换矩阵。如果更新太慢，跟踪就会滞后；如果更新太快，噪声会被放大。嗯，这里需要调参，没有银弹。
5. 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
这张图把本章的核心内容串起来了。你从上往下看：全局和局部坐标系通过齐次变换矩阵连接，旋转用欧拉角或四元数表示，最终落地到目标跟踪的实际应用中。每个环节都环环相扣，缺一不可。
一个小技巧：写代码时，我习惯给每个坐标系起一个清晰的名字，比如map、base_link、camera_link。然后在代码里显式地做变换，而不是靠「默认坐标系」。这样即使过了半年回头看，也能一眼看懂。
好了，坐标系与空间变换就讲到这里。记住一句话：没有错误的坐标，只有错误的变换。下次调试跟踪系统时，如果发现目标位置不对，先检查你的变换矩阵。
可可精品资料库