3、坐标系统与变换：世界坐标系、机体坐标系、坐标系旋转与平移、四元数与欧拉角

各位同学，今天我们来聊聊传感器融合里最基础、也最容易翻车的一块——坐标系统与变换。

说实话，我刚开始做飞控那会儿，觉得坐标系不就是个参考系嘛，有啥好讲的？结果第一次把IMU数据往控制算法里怼的时候，飞机直接在天上画了个8字……嗯，从那以后我再也不敢小看坐标变换了。

先搞清楚两个最常用的坐标系：

世界坐标系（World Frame）：也叫全局坐标系。通常以地面为参考，X轴指向北（或东），Y轴指向东（或北），Z轴垂直向上。说白了，就是“大地”的坐标系。
机体坐标系（Body Frame）：固定在飞行器或机器人身上。X轴指向机头，Y轴指向右翼，Z轴指向下方（符合右手定则）。

我习惯把世界坐标系想象成“地图”，机体坐标系想象成“我自己的朝向”。你想想看，传感器装在机身上，测出来的数据都是相对于机体坐标系的。但你要导航、要定位，最终得换算到世界坐标系里去。

  核心思想：传感器数据在机体坐标系下采集，控制指令在世界坐标系下执行。中间的桥梁就是坐标变换。

坐标变换说白了就两件事：旋转和平移。

旋转矩阵长这样（绕Z轴旋转θ角）：

R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

绕X轴、Y轴同理。三个轴的旋转组合起来，就能描述任意姿态。

我的经验：旋转矩阵虽然直观，但计算量大，而且容易遇到“万向锁”问题。我在做机械臂控制时就被这个坑过——某个角度下，两个轴突然“锁死”了，自由度丢失。所以后来我基本只用四元数。

说到姿态表示，绕不开这两个东西。

欧拉角用三个角度表示旋转：偏航（Yaw）、俯仰（Pitch）、横滚（Roll）。直观，好理解。我调试飞控时，看欧拉角一眼就知道飞机歪没歪。

但欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，偏航和横滚会耦合，丢失一个自由度。你想想看，无人机做筋斗动作时，如果姿态解算用欧拉角，直接就炸了。

四元数用四个数表示旋转：q = w + xi + yj + zk。没有万向锁，计算效率高，插值平滑。我建议所有做传感器融合的朋友，姿态解算一律用四元数，只在显示和调试时转成欧拉角。

四元数转欧拉角的公式（我常用的版本）：

roll  = atan2(2*(w*x + y*z), 1 - 2*(x² + y²))
pitch = asin(2*(w*y - z*x))
yaw   = atan2(2*(w*z + x*y), 1 - 2*(y² + z²))

注意：四元数必须归一化！我曾经在项目中忘了归一化，结果姿态越算越飘，找了三天bug才发现是四元数模长从1变成了0.98。嗯，这种低级错误，犯一次就记住了。

下面这张图，是我自己梳理的坐标变换知识脉络。你看一眼，心里就有谱了。

最后，分享几个我踩过的坑：

坐标系定义不一致：不同传感器厂商对X/Y/Z轴的定义可能不同。我遇到过IMU的Z轴朝上，但算法里默认朝下，结果姿态直接反了。拿到传感器第一件事，先看数据手册里的坐标系定义。
四元数顺序：四元数乘法不满足交换律。我习惯用 JPL 约定（标量在前），但有些库用 Hamilton 约定（标量在后）。混用的话，旋转方向就反了。
单位问题：角度用弧度还是度？我建议所有内部计算统一用弧度，只在人机交互时转成度。否则一个 π/180 漏掉，整个系统就偏了。

我的习惯：每次做坐标变换前，先写一个简单的单元测试——给一个已知的旋转和平移，看输出对不对。花10分钟验证，能省下后面10小时的调试时间。

好了，坐标系统与变换就讲到这里。这东西看着基础，但真到了项目里，80%的传感器融合bug都出在这一步。你想想看，坐标系都没对齐，后面融合得再好也是白搭。

可可精品资料库