4、坐标系与变换：世界坐标系、机器人坐标系、传感器坐标系、欧拉角与四元数、坐标变换矩阵
各位同学，今天我们来聊聊坐标系与变换。说实话，这是整个传感器融合里最基础、也最容易翻车的地方。我见过太多项目，算法写得天花乱坠，最后发现是坐标系没对齐——嗯，那种感觉就像你明明瞄准了靶心，子弹却飞到了隔壁靶子上。
4.1 为什么需要这么多坐标系？
你想想看，一个机器人身上有激光雷达、有IMU、有摄像头，每个传感器都有自己的“小世界”。激光雷达说“前方2米有障碍”，IMU说“我转了30度”，摄像头说“那个红点在第100个像素”。如果这些数据不统一到一个“共同语言”里，融合就是一句空话。
我个人习惯把坐标系分成三个层次：
- 世界坐标系：全局的、固定的参考系。比如地图上的经纬度，或者你设定的原点。
- 机器人坐标系：固定在机器人本体上，通常以机器人的中心为原点，朝前为X轴。
- 传感器坐标系：每个传感器自己的坐标系，比如激光雷达的扫描中心、IMU的安装位置。
说白了，我们做传感器融合，就是不停地在这些坐标系之间“翻译”数据。
核心原则：所有传感器数据最终都要转换到同一个坐标系下，才能进行融合。我个人建议统一到机器人坐标系，因为这样最直观，调试也方便。
4.2 欧拉角与四元数——旋转的两种表达
旋转怎么描述？最直观的是欧拉角。绕X轴转叫横滚（Roll），绕Y轴转叫俯仰（Pitch），绕Z轴转叫偏航（Yaw）。听起来很清晰对吧？
但我得提醒你一句：欧拉角有万向锁问题。什么意思？当俯仰角接近±90度时，横滚和偏航会变得无法区分。我曾经在一个无人机项目里踩过这个坑——飞控突然乱转，查了半天才发现是欧拉角在作怪。
所以，在实际工程中，我更推荐用四元数。它没有万向锁，而且插值平滑，计算效率也高。四元数长这样：
q = w + xi + yj + zk
其中 w^2 + x^2 + y^2 + z^2 = 1你可能会问：“这玩意儿怎么用？”别急，我们来看一个实际例子。
我的经验：在代码里，我通常用四元数做内部计算，只在可视化或人机交互时才转成欧拉角。这样既避免了万向锁，又方便调试。
4.3 坐标变换矩阵——平移+旋转
坐标系之间的变换，说白了就是两步：先旋转，再平移。用矩阵表示就是：
| x' | | R11 R12 R13 tx | | x |
| y' | = | R21 R22 R23 ty | | y |
| z' | | R31 R32 R33 tz | | z |
| 1 | | 0 0 0 1 | | 1 |这个4x4矩阵叫齐次变换矩阵。它把旋转矩阵R和平移向量t打包在一起，一次乘法就能完成坐标变换。
我记得有一次做多传感器标定，激光雷达和摄像头的数据总是对不上。后来发现是变换矩阵里的旋转顺序搞反了——先平移后旋转，和先旋转后平移，结果天差地别。
注意：变换矩阵的乘法不满足交换律。也就是说，A到B的变换乘以B到C的变换，不等于B到C乘以A到B。顺序搞反，你的机器人可能会“原地起飞”。
4.4 知识体系总览
下面这张图是我自己画的，帮你理清本章的核心逻辑：
4.5 实战中的坐标系管理
在实际项目中，我一般会这样做：
- 定义清晰：在代码开头就定义好所有坐标系，用注释写清楚每个传感器的安装位置和朝向。
- 统一接口：写一个坐标变换函数库，所有传感器数据都通过这个库转换。
- 可视化验证：把转换后的数据画出来，看看是不是在正确的位置上。我习惯用RViz或Matplotlib。
避坑指南：我曾经在一个项目里，因为IMU的安装方向标反了，导致机器人一直在原地转圈。后来花了整整两天才找到问题——其实就是坐标系定义里少了一个负号。所以，一定要做单元测试，用已知的旋转和平移验证你的变换矩阵。
4.6 小结
好了，这一章的内容就这些。记住三个坐标系、两种旋转表达、一个变换矩阵。这些东西看着简单，但用好了，你的传感器融合就成功了一半。
下次遇到数据对不上的问题，先别急着调算法参数——检查一下坐标系，说不定问题就出在那里。