3、坐标系与变换：世界坐标系、机体坐标系、传感器坐标系，以及旋转矩阵、四元数、欧拉角的转换

在传感器融合与卡尔曼滤波的实际工程中，坐标系定义与姿态表达是决定算法能否收敛、精度能否达标的第一道门槛。许多开发者将精力集中在卡尔曼滤波公式推导上，却忽略了坐标系混乱带来的灾难性后果——例如将机体坐标系下的加速度数据直接当作世界坐标系下的重力向量使用，导致姿态估计发散。本章将系统梳理世界坐标系、机体坐标系、传感器坐标系之间的层级关系，深入讲解旋转矩阵、四元数、欧拉角三种姿态表达方式的数学本质与转换方法，并结合IMU（惯性测量单元）数据融合场景给出可落地的工程建议。

上图展示了本章的核心知识框架：世界坐标系作为惯性参考基准，机体坐标系描述载体姿态，传感器坐标系则反映物理芯片的实际安装方向。三者之间通过旋转矩阵或四元数建立数学联系。下面我们将逐一深入剖析。

3.1 三种坐标系的定义与工程意义

3.1.1 世界坐标系（World Frame）

世界坐标系是描述物体位置与姿态的参考基准，通常选择地球固连坐标系。在无人机、机器人、自动驾驶领域，最常用的是NED（北-东-地）坐标系或ENU（东-北-天）坐标系。NED坐标系中，X轴指向地理北，Y轴指向东，Z轴垂直向下指向地心；ENU坐标系则相反，Z轴指向天顶。选择哪种坐标系取决于应用场景：航空领域习惯使用NED，而机器人室内导航更常用ENU。

世界坐标系的关键特性在于其惯性属性——它不随载体运动而旋转，是牛顿力学定律成立的惯性参考系。在卡尔曼滤波中，世界坐标系下的重力向量、地磁向量等先验信息是姿态解算的重要观测量。例如，加速度计静止时测量到的比力向量，在世界坐标系下应等于重力加速度向量（约9.81 m/s²）的反方向。

3.1.2 机体坐标系（Body Frame）

机体坐标系固连于运动载体，原点通常位于载体质心。对于无人机，机体坐标系定义为：X轴指向机头前方，Y轴指向机体右侧，Z轴垂直机体向下（符合右手定则）。该坐标系随载体一起旋转和平移，是IMU直接测量的参考系。加速度计输出的比力、陀螺仪输出的角速度，本质上都是机体坐标系下的物理量。

理解机体坐标系的核心在于：它描述的是“载体自身的感觉”。当无人机俯仰抬头时，机体坐标系相对于世界坐标系发生了旋转，但传感器输出的数据仍然是在机体坐标系下表达的。因此，任何姿态解算算法都必须先将传感器数据从机体坐标系转换到世界坐标系，才能进行有物理意义的积分或融合。

3.1.3 传感器坐标系（Sensor Frame）

传感器坐标系是IMU芯片内部定义的坐标系，其原点位于MEMS芯片的封装中心，轴向由芯片的制造工艺决定。理想情况下，传感器坐标系与机体坐标系完全对齐，但实际工程中由于PCB焊接偏差、芯片安装方向、封装引脚定义等因素，两者之间存在固定的安装误差角。这个误差角通常用安装矩阵（Mounting Matrix）来描述。

例如，某款IMU芯片的数据手册规定其X轴指向芯片的1号引脚方向，Y轴指向2号引脚方向，Z轴垂直芯片表面向上。如果PCB设计时将芯片旋转了90°安装，那么传感器坐标系与机体坐标系之间就存在90°的偏航角偏差。忽略这个偏差，将导致姿态估计出现恒定偏置误差。

3.2 姿态表达的三种数学工具

3.2.1 欧拉角（Euler Angles）

欧拉角通过三个依次旋转的角度来描述刚体姿态，最常用的旋转顺序是Z-Y-X（先偏航Yaw，再俯仰Pitch，最后横滚Roll）。设世界坐标系为W，机体坐标系为B，则从W到B的旋转可分解为：先绕W的Z轴旋转偏航角ψ，再绕新Y轴旋转俯仰角θ，最后绕新X轴旋转横滚角φ。对应的旋转矩阵为：

R_WB = Rz(ψ) * Ry(θ) * Rx(φ)

其中各基本旋转矩阵为：

Rz(ψ) = [cosψ  -sinψ  0;  sinψ  cosψ  0;  0  0  1]
Ry(θ) = [cosθ  0  sinθ;  0  1  0;  -sinθ  0  cosθ]
Rx(φ) = [1  0  0;  0  cosφ  -sinφ;  0  sinφ  cosφ]

欧拉角的优点在于物理意义直观，便于人机交互和调试。但其致命缺陷是万向节死锁（Gimbal Lock）：当俯仰角θ接近±90°时，偏航和横滚的旋转轴重合，导致自由度丢失，姿态解算出现奇异。因此，欧拉角不适合作为卡尔曼滤波的状态量，仅适合用于结果显示和简单控制。

3.2.2 旋转矩阵（Rotation Matrix）

旋转矩阵是一个3×3的正交矩阵，满足R^T R = I且det(R)=1。它完整描述了坐标系之间的旋转关系，且不存在奇异问题。旋转矩阵的优点是计算坐标变换非常方便：若已知向量在机体坐标系下的坐标v_B，则其在世界坐标系下的坐标为v_W = R_WB * v_B。

然而，旋转矩阵有9个元素，但实际自由度只有3个，这意味着6个冗余约束。在卡尔曼滤波中直接使用旋转矩阵作为状态量，会导致协方差矩阵奇异，滤波发散。因此，旋转矩阵通常用于中间计算，而非滤波状态。

3.2.3 四元数（Quaternion）

四元数是一种超复数形式，由一个实部和三个虚部组成：q = q0 + q1·i + q2·j + q3·k，满足i²=j²=k²=ijk=-1。单位四元数（模长为1）可以表示三维空间旋转，其与旋转矩阵的转换关系为：

R(q) = [1-2(q2²+q3²)  2(q1q2-q0q3)  2(q1q3+q0q2);
        2(q1q2+q0q3)  1-2(q1²+q3²)  2(q2q3-q0q1);
        2(q1q3-q0q2)  2(q2q3+q0q1)  1-2(q1²+q2²)]

四元数的主要优势包括：无奇异问题、计算效率高（仅需4个参数）、便于插值（球面线性插值SLERP）。在卡尔曼滤波中，四元数被广泛用作状态量，但必须注意归一化约束——滤波更新后需重新归一化，否则误差会累积。

3.3 三种表达方式的转换方法

3.4 实战案例：IMU数据从传感器坐标系到世界坐标系的完整变换

假设某四旋翼无人机搭载BMI088 IMU，其传感器坐标系与机体坐标系存在固定安装误差：绕Z轴旋转5°（即芯片安装偏航角为5°）。无人机当前姿态为：偏航30°、俯仰-10°、横滚15°（欧拉角，Z-Y-X顺序）。加速度计在传感器坐标系下输出：a_s = [0.5, 0.2, 9.7] m/s²。我们需要计算该加速度在世界坐标系下的投影。

第一步：构建安装矩阵M

安装矩阵M表示从传感器坐标系到机体坐标系的旋转，由于仅绕Z轴旋转5°，则：

M = [cos5°  -sin5°  0;  sin5°  cos5°  0;  0  0  1]
   = [0.9962  -0.0872  0;  0.0872  0.9962  0;  0  0  1]

第二步：将传感器数据转换到机体坐标系

a_B = M * a_s = [0.9962*0.5 - 0.0872*0.2,  0.0872*0.5 + 0.9962*0.2,  9.7]
     = [0.4807,  0.2428,  9.7] m/s²

第三步：构建姿态旋转矩阵R_WB

根据欧拉角（ψ=30°, θ=-10°, φ=15°），计算R_WB = Rz(30°)·Ry(-10°)·Rx(15°)。代入数值后得到：

R_WB = [0.8524  -0.4924  0.1763;
        0.4924  0.8524  0.1763;
        -0.1763  0.1763  0.9684]

第四步：转换到世界坐标系

a_W = R_WB * a_B = [0.8524*0.4807 - 0.4924*0.2428 + 0.1763*9.7;
                    0.4924*0.4807 + 0.8524*0.2428 + 0.1763*9.7;
                    -0.1763*0.4807 + 0.1763*0.2428 + 0.9684*9.7]
     = [1.684,  1.932,  9.386] m/s²

从结果可见，由于无人机存在俯仰和横滚，重力加速度（约9.8 m/s²）被分解到了世界坐标系的X和Y轴分量上。如果忽略安装矩阵M，直接使用a_s进行变换，则结果会存在约0.05 m/s²的误差，在长时间积分后会导致位置漂移显著。

3.5 常见误区与工程建议

• 误区一：将加速度计数据直接用于世界坐标系下的重力补偿。正确做法是先用姿态矩阵将加速度转换到世界坐标系，再减去重力向量。
• 误区二：在卡尔曼滤波中使用欧拉角作为状态量。当俯仰角接近±90°时，滤波协方差矩阵奇异，导致发散。推荐使用四元数或修正罗德里格斯参数（MRP）。
• 误区三：忽略四元数归一化。滤波更新后四元数模长会偏离1，若不归一化，旋转矩阵不再正交，导致坐标变换失真。
• 误区四：旋转顺序混淆。不同领域（航空、航海、机器人）对欧拉角旋转顺序约定不同，跨团队协作时必须明确标注旋转顺序。

总结复盘：坐标系与姿态表达是传感器融合的基石。世界坐标系提供惯性参考，机体坐标系反映载体运动，传感器坐标系描述物理安装。旋转矩阵适合计算，四元数适合滤波，欧拉角适合显示。在实际工程中，建议采用“四元数状态+旋转矩阵观测+欧拉角输出”的混合架构，既保证滤波稳定性，又兼顾调试直观性。每次系统上电时，应执行坐标系一致性检查：将IMU静止放置，验证加速度计输出在转换到世界坐标系后是否接近[0,0,9.8] m/s²，陀螺仪输出是否接近零。这一简单测试能快速发现坐标系定义错误或安装矩阵错误。