📚 课程目录第4章坐标系与变换

4、坐标系与变换：机体坐标系、地面坐标系、导航坐标系及坐标变换矩阵的推导与代码实现

姿态控制的本质，是在不同坐标系之间精确描述“载体现在朝向哪里”以及“期望朝向哪里”。飞控源码中，从传感器原始数据到姿态四元数，再到控制律输出的期望力矩，每一步都隐含着坐标系变换。如果坐标系定义混乱或变换矩阵推导错误，轻则控制精度下降，重则飞行器直接翻转坠毁。本章将系统梳理机体坐标系（Body Frame）、地面坐标系（Earth Frame）与导航坐标系（Navigation Frame）的定义与关系，推导核心的坐标变换矩阵（方向余弦矩阵 DCM），并给出可直接嵌入飞控源码的 C 语言实现。

上图展示了三个坐标系之间的核心关系。地面坐标系是惯性参考的近似，导航坐标系是实际飞控解算时使用的参考系，而机体坐标系则固连于飞行器。从导航系到机体系的旋转矩阵 R 是整个姿态控制算法的数学桥梁。下面我们逐一深入。

4.1 三种坐标系的定义与工程约定

核心原理：在飞控源码中，坐标系定义不统一是导致算法移植困难的首要原因。PX4、ArduPilot 等开源飞控均采用 NED（北东地） 右手坐标系作为导航坐标系，机体坐标系则定义为 X 轴指向机头前方，Y 轴指向右翼，Z 轴指向下方。地面坐标系（Earth Frame）通常与导航坐标系重合，但严格来说地面系是固定于地球表面的惯性系，而导航系可以随飞行器移动（例如以飞行器当前位置为原点）。

工程判断方法：阅读飞控源码时，首先查找头文件中的坐标系定义宏或注释。例如 PX4 的 vehicle_attitude_setpoint.h 中明确标注了机体轴方向。若发现代码中加速度计输出正负号与预期不符，优先检查坐标系定义是否匹配。

坐标系	原点	X轴	Y轴	Z轴	典型用途
地面系 (g)	起飞点	北	东	地	GPS 定位、航迹规划
导航系 (n)	飞行器质心投影	北	东	地	姿态解算、控制律
机体系 (b)	飞行器质心	机头前方	右翼	机体下方	传感器测量、力矩控制

在大多数飞控实现中，地面系与导航系不做严格区分，因为飞行器活动范围相对地球半径极小，地球曲率可忽略。但需注意：当飞行器做长距离飞行时，导航系应随位置更新，否则位置积分会累积误差。

4.2 坐标变换矩阵的推导：从欧拉角到方向余弦矩阵

核心原理：任意姿态可由三次基本旋转组合得到。飞控中约定使用 Z-Y-X 内旋顺序（即先偏航 ψ，再俯仰 θ，最后横滚 φ），对应的旋转矩阵为：

R = Rz(ψ) · Ry(θ) · Rx(φ)

其中：
Rx(φ) = [1,    0,     0;
         0, cosφ, -sinφ;
         0, sinφ,  cosφ]

Ry(θ) = [ cosθ, 0, sinθ;
             0, 1,    0;
         -sinθ, 0, cosθ]

Rz(ψ) = [cosψ, -sinψ, 0;
         sinψ,  cosψ, 0;
            0,    0, 1]

将三个矩阵相乘，得到完整的 DCM：

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θ ]

操作流程：在飞控源码中，通常不直接使用欧拉角计算 DCM，而是使用四元数避免万向锁问题。但理解 DCM 的推导有助于调试。当需要从四元数提取欧拉角时，使用以下公式：

φ = 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²))

注意：当 θ 接近 ±90° 时，asin 函数输入可能超出 [-1,1] 范围，导致 NaN。工程上需做钳位处理，例如 if (val > 1.0f) val = 1.0f;。

4.3 代码实现：C语言下的高效变换

核心原理：飞控代码运行在实时嵌入式环境中，要求变换函数无动态内存分配、无浮点异常。以下给出一个完整的姿态变换模块实现：

// 姿态数据结构
typedef struct {
    float q0, q1, q2, q3;  // 四元数
} Quaternion;

typedef struct {
    float roll, pitch, yaw;  // 欧拉角，单位：弧度
} EulerAngles;

// 四元数归一化
void quat_normalize(Quaternion* q) {
    float norm = sqrtf(q->q0*q->q0 + q->q1*q->q1 +
                       q->q2*q->q2 + q->q3*q->q3);
    if (norm < 1e-8f) { q->q0 = 1.0f; q->q1 = q->q2 = q->q3 = 0.0f; return; }
    q->q0 /= norm; q->q1 /= norm; q->q2 /= norm; q->q3 /= norm;
}

// 四元数旋转向量：将导航系向量转到机体系
void rotate_ned_to_body(const Quaternion* q, const float v_n[3], float v_b[3]) {
    float q0 = q->q0, q1 = q->q1, q2 = q->q2, q3 = q->q3;
    // 使用公式 v_b = q ⊗ v_n ⊗ q* （四元数乘法）
    float t2 = q0*q1, t3 = q0*q2, t4 = q0*q3;
    float t5 = -q1*q1, t6 = q1*q2, t7 = q1*q3;
    float t8 = -q2*q2, t9 = q2*q3, t10 = -q3*q3;

    v_b[0] = 2*((t5+t8)*v_n[0] + (t6-t4)*v_n[1] + (t7+t3)*v_n[2]) + v_n[0];
    v_b[1] = 2*((t6+t4)*v_n[0] + (t5+t10)*v_n[1] + (t9-t2)*v_n[2]) + v_n[1];
    v_b[2] = 2*((t7-t3)*v_n[0] + (t9+t2)*v_n[1] + (t8+t10)*v_n[2]) + v_n[2];
}

// 从四元数提取欧拉角（带钳位）
EulerAngles quat_to_euler(const Quaternion* q) {
    EulerAngles e;
    float q0 = q->q0, q1 = q->q1, q2 = q->q2, q3 = q->q3;
    // 俯仰角钳位
    float sinp = 2*(q0*q2 - q3*q1);
    if (sinp > 1.0f) sinp = 1.0f;
    if (sinp < -1.0f) sinp = -1.0f;
    e.pitch = asinf(sinp);
    e.roll = atan2f(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2));
    e.yaw = atan2f(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3));
    return e;
}

实战场景：在姿态控制回路中，期望姿态通常以欧拉角给定（来自遥控器或任务规划），而实际姿态由四元数表示。控制误差计算需要先将期望欧拉角转为四元数，再求差：

// 欧拉角转四元数（Z-Y-X顺序）
Quaternion euler_to_quat(float roll, float pitch, float yaw) {
    Quaternion q;
    float cr = cosf(roll*0.5f), sr = sinf(roll*0.5f);
    float cp = cosf(pitch*0.5f), sp = sinf(pitch*0.5f);
    float cy = cosf(yaw*0.5f), sy = sinf(yaw*0.5f);
    q.q0 = cr*cp*cy + sr*sp*sy;
    q.q1 = sr*cp*cy - cr*sp*sy;
    q.q2 = cr*sp*cy + sr*cp*sy;
    q.q3 = cr*cp*sy - sr*sp*cy;
    return q;
}

4.4 案例拆解：加速度计辅助的姿态初始化

案例背景：某四旋翼飞控上电后，陀螺仪存在零偏，无法直接积分得到初始姿态。需要利用加速度计测量重力方向来初始化横滚角和俯仰角。

初始现象：飞控静止放置于桌面，加速度计输出为 ax=0.02g, ay=0.01g, az=0.99g（理想应为 0,0,1g）。

分析过程：由于机体静止，加速度计只感受重力。重力在导航系中为 [0,0,1]（NED 下方向向下）。通过旋转矩阵反变换，可得到机体系中的重力分量：

// 利用 DCM 第三行
ax = -sinθ
ay = sinφ·cosθ
az = cosφ·cosθ

// 解得初始姿态角
roll_0 = atan2f(ay, az);
pitch_0 = atan2f(-ax, sqrtf(ay*ay + az*az));

实际操作：将上述公式写入初始化代码，并在上电后延时 2 秒采集 100 次加速度计数据取平均，以抑制噪声。计算得到 roll_0 ≈ 0.57°，pitch_0 ≈ -1.15°。随后将欧拉角转为四元数作为初始姿态。

结果对比：使用该初始化方法后，飞行器起飞时无明显姿态跳变。若直接以单位四元数（0,0,0,1）初始化，起飞瞬间飞控会误以为飞行器倾斜，产生错误修正力矩。

风险与代价：加速度计初始化无法提供偏航角初值，需依赖磁力计或外部给定。若飞行器处于运动状态（如车载启动），加速度计会混入线加速度，导致初始化错误。此时应增加加速度幅值判断：| |a| - 1g | < 0.1g 才允许初始化。

经验：在飞控源码中，姿态初始化通常采用“互补滤波”思路——加速度计提供长期稳定参考，陀螺仪提供短期快速响应。初始化阶段只取加速度计结果，运行阶段再融合。

4.5 常见误区与总结复盘

误区一：混淆旋转矩阵的转置与逆。由于 R 是正交矩阵，R⁻¹ = Rᵀ。但若使用非正交矩阵（如含数值误差的 DCM），必须使用真正的逆矩阵。工程上应定期对 DCM 做正交化修正。

误区二：欧拉角旋转顺序不一致。有些文献使用 Y-X-Z 顺序，导致矩阵元素排列不同。飞控源码中必须统一使用 Z-Y-X 内旋顺序，否则姿态解算结果完全错误。

误区三：忽略坐标系轴向定义。部分传感器（如某些 IMU）的 Z 轴向上，与 NED 相反。接入飞控前需做符号翻转，否则横滚角会反号。

误区四：四元数未归一化。四元数在连续乘法后模长会漂移，导致旋转矩阵不再正交。每次更新后必须调用 quat_normalize()。

检查项	方法	通过标准
旋转矩阵正交性	计算 R·Rᵀ 与单位阵的差	最大元素误差 < 1e-6
四元数模长	计算 q0²+q1²+q2²+q3²	偏差 < 1e-5
欧拉角范围	检查 roll/pitch 是否超限	roll ∈ [-π,π], pitch ∈ [-π/2,π/2]
坐标系一致性	旋转已知向量验证	误差 < 0.01°

总结：坐标系变换是飞控算法的基石。掌握 NED 与机体坐标系的定义、DCM 的推导过程以及四元数与欧拉角的互转，是阅读和修改飞控源码的前提。建议在代码中封装独立的坐标变换模块，并编写单元测试验证旋转正确性。实际调试中，利用地面站的数据可视化工具观察姿态角变化，能快速定位坐标系配置错误。