📚 课程目录第3章坐标系与姿态表示

坐标系与姿态表示：载体坐标系、导航坐标系、欧拉角、四元数、旋转矩阵及其转换关系

IMU（惯性测量单元）输出的原始数据是加速度和角速度，但这些数据本身并不具备物理意义——它们只是相对于传感器自身安装方向的一组矢量。要让这些数据真正服务于导航、控制或测量，必须首先回答三个问题：数据是在哪个坐标系下测量的？目标运动是在哪个坐标系下描述的？如何将一个坐标系下的矢量转换到另一个坐标系？这三个问题构成了IMU标定与校准的理论基石。本章将系统梳理载体坐标系（b系）、导航坐标系（n系）的定义，并深入讲解欧拉角、四元数、旋转矩阵三种姿态表示方法及其相互转换关系，为后续的零偏标定、安装误差补偿和传感器融合打下坚实的数学基础。

上图展示了本章的核心知识框架：左侧为两个基础坐标系的定义，中间为三种姿态表示方法，右侧为它们之间的转换路径。在实际IMU标定中，我们通常先在载体坐标系下采集原始数据，然后通过姿态解算得到四元数，再转换为旋转矩阵或欧拉角，最终将加速度和角速度投影到导航坐标系中。下面逐一展开讲解。

3.1 载体坐标系与导航坐标系的定义

3.1.1 载体坐标系（Body Frame, b系）

载体坐标系是固定于IMU传感器或运载体上的直角坐标系，其原点通常位于IMU的测量中心（即加速度计和陀螺仪敏感轴的交叉点）。按照右手定则，常见的定义方式为：Xb轴指向载体前方（前向），Yb轴指向载体右侧（右向），Zb轴指向载体下方（下向）。这种“前-右-下”（FRD）定义在航空和地面车辆领域最为常见。另一种变体是“右-前-上”（RFU），但无论采用哪种定义，必须保证三个轴构成右手正交系。

在IMU标定中，载体坐标系是传感器数据输出的原始参考系。加速度计测量的比力（Specific Force）和陀螺仪测量的角速度都是在该坐标系下表达的。例如，当IMU水平静止放置时，加速度计输出应为 [0, 0, g]（假设Zb向下），其中g为当地重力加速度。如果安装存在倾斜，则输出会按姿态角分解到三个轴上。

3.1.2 导航坐标系（Navigation Frame, n系）

导航坐标系是描述载体运动状态的参考坐标系，其原点通常选在载体所在位置的地面某点，三轴指向根据导航需求确定。最常用的是“北-东-地”（NED）坐标系：Xn指向地理北极，Yn指向正东，Zn垂直向下指向地心。NED坐标系是右手系，与载体坐标系的FRD定义天然对应，因此在航空和地面导航中广泛使用。

另一种常见选择是“东-北-天”（ENU）坐标系，多用于大地测量和机器人领域。选择哪种导航坐标系取决于应用场景：飞行器通常使用NED，而室内机器人可能更习惯ENU。无论选择哪种，IMU标定过程中都需要明确记录坐标系定义，否则后续的姿态解算和坐标变换将产生系统性偏差。

  关键点：坐标系定义是IMU标定的“宪法”。在开始任何标定工作之前，必须书面确认b系和n系的具体指向，并检查IMU数据手册中的轴定义是否与硬件安装一致。一个常见的错误是：IMU数据手册标注的Z轴向上，但机械安装时Z轴向下，导致所有加速度计读数符号反转。

3.2 欧拉角：直观但存在奇异性的姿态描述

3.2.1 欧拉角的定义与旋转顺序

欧拉角通过三次绕不同坐标轴的旋转来描述载体相对于导航坐标系的姿态。按照Z-Y-X顺序（航向-俯仰-横滚），定义如下：

航向角ψ（Yaw）：绕Zn轴旋转，范围通常为[-π, π]或[0, 2π]，描述载体纵轴在水平面上的投影与北向的夹角。
俯仰角θ（Pitch）：绕Y轴旋转，范围[-π/2, π/2]，描述载体纵轴与水平面的夹角，抬头为正。
横滚角φ（Roll）：绕X轴旋转，范围[-π, π]，描述载体绕纵轴的转动，右侧下沉为正。

旋转矩阵由三次基本旋转矩阵连乘得到：

R = 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φ]

注意矩阵乘法的顺序：先绕X轴旋转φ，再绕Y轴旋转θ，最后绕Z轴旋转ψ。这个顺序是固定的，改变顺序将得到完全不同的姿态结果。

3.2.2 欧拉角的工程应用与局限性

欧拉角的最大优点是物理意义直观，工程师可以直接从数值上理解载体的朝向。在IMU标定中，欧拉角常用于初始对准和结果验证。例如，将IMU放置在已知角度的倾斜台上，通过读取欧拉角输出即可快速验证标定精度。

然而，欧拉角存在一个致命缺陷——万向节锁（Gimbal Lock）。当俯仰角θ接近±90°时，航向角和横滚角的旋转轴重合，导致自由度丢失，姿态解算出现奇异。在动态应用中，如果载体可能经历大角度俯仰（如无人机垂直爬升），使用欧拉角进行姿态更新将导致数值不稳定。

风险提示：在IMU标定过程中，如果使用欧拉角作为姿态反馈，务必确保标定轨迹不会经过θ=±90°附近。否则，标定算法可能因奇异值问题而发散。建议在标定算法内部使用四元数进行姿态传播，仅在输入输出接口处转换为欧拉角。

3.3 四元数：无奇异性的姿态表示

3.3.1 四元数的数学定义

四元数是一种超复数，由一个实部和三个虚部组成：q = q0 + q1·i + q2·j + q3·k，其中满足 i² = j² = k² = ijk = -1。在姿态表示中，四元数被约束为单位四元数（模长为1），其几何意义是：绕单位向量 [u1, u2, u3] 旋转角度α，对应的四元数为：

q = [cos(α/2), u1·sin(α/2), u2·sin(α/2), u3·sin(α/2)]

四元数表示姿态时，q和-q表示相同的旋转，这一点在工程实现中需要特别注意。四元数的乘法运算可以组合旋转，且不存在奇异问题，因此在IMU姿态解算中被广泛采用。

3.3.2 四元数的运算规则

四元数乘法（Hamilton积）定义为：

q1 ⊗ q2 = [q0_1·q0_2 - q1_1·q1_2 - q2_1·q2_2 - q3_1·q3_2,
            q0_1·q1_2 + q1_1·q0_2 + q2_1·q3_2 - q3_1·q2_2,
            q0_1·q2_2 - q1_1·q3_2 + q2_1·q0_2 + q3_1·q1_2,
            q0_1·q3_2 + q1_1·q2_2 - q2_1·q1_2 + q3_1·q0_2]

四元数的共轭 q* = [q0, -q1, -q2, -q3] 表示逆旋转。利用四元数可以将b系矢量 v_b 转换到n系：

v_n = q ⊗ v_b ⊗ q*

其中 v_b 和 v_n 需要扩展为纯四元数（实部为0）。这种运算避免了三角函数的计算，效率更高，且适合嵌入式实时实现。

3.4 旋转矩阵：坐标变换的桥梁

3.4.1 旋转矩阵的性质

旋转矩阵 R_b^n 是一个3×3的正交矩阵，满足 R^T = R⁻¹ 且 det(R) = 1。它将b系下的矢量映射到n系：v_n = R_b^n · v_b。旋转矩阵的每一列是b系基向量在n系中的投影，每一行是n系基向量在b系中的投影。

旋转矩阵的优点是计算稳定、无奇异，且可以直接用于矢量旋转。缺点是存储量大（9个元素），且每次更新需要复杂的矩阵运算。在IMU标定中，旋转矩阵常用于安装误差补偿：如果IMU的安装轴与载体轴存在小角度偏差，可以通过一个固定的旋转矩阵进行校正。

3.4.2 三种表示方法的对比

特性	欧拉角	四元数	旋转矩阵
存储量	3个标量	4个标量	9个标量
奇异性	有（θ=±90°）	无	无
物理直观性	高	低	中
计算效率	中（含三角函数）	高（仅乘加）	低（矩阵乘法）
插值友好性	差	好（球面线性插值）	差
典型应用	初始对准、结果显示	姿态解算、数据融合	坐标变换、误差补偿

3.5 三种表示方法的相互转换

3.5.1 欧拉角 → 旋转矩阵

按照Z-Y-X顺序，将三个基本旋转矩阵相乘即可得到完整的旋转矩阵。展开后为：

R = [cosθ·cosψ,  sinφ·sinθ·cosψ - cosφ·sinψ,  cosφ·sinθ·cosψ + sinφ·sinψ;
     cosθ·sinψ,  sinφ·sinθ·sinψ + cosφ·cosψ,  cosφ·sinθ·sinψ - sinφ·cosψ;
     -sinθ,      sinφ·cosθ,                     cosφ·cosθ]

这个矩阵在IMU标定中用于将已知的台架姿态角转换为旋转矩阵，进而生成理论加速度计输出，与实测值对比以估计标定参数。

3.5.2 旋转矩阵 → 四元数

从旋转矩阵 R 提取四元数时，首先计算矩阵的迹 tr(R) = R11 + R22 + R33。然后根据迹的大小选择不同的求解路径：

若 tr(R) > 0:
    S = sqrt(tr(R) + 1) * 2
    q0 = 0.25 * S
    q1 = (R32 - R23) / S
    q2 = (R13 - R31) / S
    q3 = (R21 - R12) / S
否则，根据R11、R22、R33中最大值选择对应轴进行求解（Shepperd方法）

求解后必须对四元数进行归一化处理，确保模长为1。工程中建议使用Shepperd方法，它在所有情况下都能保持数值稳定性。

3.5.3 四元数 → 欧拉角

从四元数提取欧拉角的公式为：

φ = atan2(2(q0·q1 + q2·q3), 1 - 2(q1² + q2²))
θ = asin(2(q0·q2 - q3·q1))
ψ = atan2(2(q0·q3 + q1·q2), 1 - 2(q2² + q3²))

注意 asin 函数的输入范围是[-1, 1]，当 2(q0·q2 - q3·q1) 超出该范围时（由于数值误差），需要将其截断到[-1, 1]再计算。当θ接近±90°时，φ和ψ的atan2计算会变得不稳定，此时应改用四元数或旋转矩阵进行姿态描述。

工程经验：在IMU标定软件中，建议内部统一使用四元数进行姿态传播和更新，仅在用户界面显示或数据记录时转换为欧拉角。这样可以避免在标定过程中因欧拉角奇异而导致的算法崩溃。

3.6 案例拆解：六位置法标定中的坐标系转换

六位置法是IMU加速度计标定的经典方法，其核心思想是将IMU分别放置在六个已知姿态的位置（X/Y/Z轴分别朝上和朝下），利用重力加速度作为参考激励，估计加速度计的零偏和标度因数。下面通过一个具体案例展示坐标系转换在其中的应用。

背景与约束：某无人机项目使用一款工业级MEMS IMU，加速度计量程为±8g，陀螺仪量程为±500°/s。IMU被安装在一个三轴转台上，转台可以精确控制姿态角（精度0.01°）。标定目标是将加速度计的零偏误差控制在±5mg以内，标度因数误差控制在±0.1%以内。

初始现象：将IMU水平放置（b系Z轴垂直向下）时，加速度计输出为 [0.012, -0.008, 9.812] m/s²。理想情况下应为 [0, 0, 9.80665] m/s²，可见X轴和Y轴存在明显的零偏，Z轴标度因数也可能存在偏差。

操作步骤：

定义坐标系：确认b系为FRD（前-右-下），n系为NED。转台零位时，b系与n系重合。
控制转台依次到达六个位置：Xb朝上、Xb朝下、Yb朝上、Yb朝下、Zb朝上、Zb朝下。每个位置静止采集30秒数据，取平均值作为该位置的加速度计输出。
对于每个位置，根据转台姿态角（欧拉角）计算理论加速度值。例如，当Xb朝上时，转台姿态为φ=180°, θ=0°, ψ=0°，对应的旋转矩阵为 R = diag(1, -1, -1)，理论加速度为 [-g, 0, 0]。
将六个位置的实测值与理论值组成超定方程组，利用最小二乘法求解零偏 b_a 和标度因数矩阵 K_a。
标定完成后，将补偿参数写入IMU驱动，重新进行验证测试。

结果对比：标定前，加速度计在水平位置的输出误差为X轴12mg、Y轴-8mg；标定后，X轴误差降至1.2mg，Y轴误差降至-0.8mg，Z轴误差从-5.4mg降至0.3mg，满足指标要求。

风险与代价：六位置法假设转台姿态角精确已知，但实际转台存在安装误差和角度控制误差。如果转台本身的姿态精度不足，标定结果会引入额外的系统误差。此外，该方法只估计零偏和标度因数，无法估计轴间非正交误差，需要更复杂的多位置法或速率法。

复盘经验：在标定过程中，坐标系转换的正确性是决定成败的关键。一个常见的错误是：在计算理论加速度时，将转台姿态角直接当作欧拉角使用，但未明确旋转顺序。如果转台控制软件采用Z-Y-X顺序，而标定程序默认Z-X-Y顺序，则所有理论值都会出错。因此，在编写标定程序前，务必确认转台的旋转顺序约定，并在代码中显式指定。

3.7 常见误区与总结

误区一：混淆旋转矩阵的转置与逆。旋转矩阵是正交矩阵，其转置等于逆。但在坐标变换中，必须明确是“b系到n系”还是“n系到b系”。R_b^n 和 R_n^b 互为转置，使用错误将导致姿态完全反转。

误区二：忽略四元数的双值性。q和-q表示相同的旋转，但在四元数插值或滤波中，如果两个四元数符号不一致，会导致插值路径异常。在IMU数据融合中，应确保相邻时刻的四元数点积为正，否则取反。

误区三：欧拉角旋转顺序不统一。不同领域可能采用不同的旋转顺序（如Z-Y-X、Z-X-Z、Y-X-Z等）。在阅读IMU数据手册或转台控制文档时，必须确认其采用的顺序，否则姿态解算结果将完全错误。

总结：坐标系与姿态表示是IMU标定的数学基础。载体坐标系定义了传感器的原始测量参考，导航坐标系定义了目标运动的描述参考，而欧拉角、四元数和旋转矩阵则提供了从b系到n系的转换工具。三种表示方法各有优劣：欧拉角直观但存在奇异，四元数无奇异且计算高效，旋转矩阵稳定但存储量大。在实际标定工程中，应遵循“内部用四元数、接口用欧拉角、变换用旋转矩阵”的原则，并始终明确坐标系的定义和旋转顺序。掌握这些基础概念，才能正确设计标定流程、解释标定结果，并为后续的传感器融合算法奠定坚实基础。