第2章姿态表示方法

第2章：姿态表示方法——欧拉角、方向余弦矩阵、四元数、旋转矢量

各位同学，大家好。我是你们这门课的讲师。今天咱们来聊聊姿态表示方法。说实话，这是捷联惯导里最基础、也最容易让人绕晕的一块。我当年刚入行时，就在这上面栽过跟头。你想想看，一个飞行器在天上转来转去，我们怎么用数学去描述它？

嗯，常用的方法有四种：欧拉角、方向余弦矩阵、四元数、旋转矢量。它们各有各的脾气，也各有各的坑。今天我就结合我自己的项目经验，把这四种方法掰开了揉碎了讲给你听。

2.1 欧拉角：直观但暗藏危机

欧拉角是最直观的。说白了，就是用三个角度来描述一个刚体的旋转。比如飞机，我们常说它的俯仰角、横滚角、航向角。你一看这三个数，脑子里就能大概想象出飞机是什么姿势。

但这里有个大坑——万向锁。我记得我第一次做飞行器仿真时，就遇到了这个问题。当俯仰角接近±90°时，横滚和航向就分不清了，系统直接失控。为什么会这样？因为欧拉角在数学上存在奇异性。

⚠️ 避坑指南： 我曾经在无人机项目中，因为用了欧拉角做全姿态解算，结果飞机在做大机动时姿态解算直接发散。后来我换成了四元数，问题才解决。所以，如果你要处理全姿态（尤其是俯仰角接近±90°），千万别用欧拉角。

欧拉角的另一个问题是，它的微分方程是非线性的，而且计算量不小。不过，在小角度情况下，比如平台调平、初始对准，欧拉角还是很好用的。因为这时候我们可以做线性化近似。

优点	缺点
直观，物理意义明确	存在万向锁（奇异性）
小角度下可线性化	微分方程非线性，计算复杂
适合人机交互	不适用于全姿态解算

2.2 方向余弦矩阵：稳定但笨重

方向余弦矩阵，简称DCM。它是一个3×3的正交矩阵，用来描述两个坐标系之间的旋转关系。说白了，矩阵里的每个元素，就是两个坐标系对应轴之间的夹角余弦。

DCM最大的好处是没有奇异性。你随便怎么转，它都能描述。而且，多个旋转可以简单地用矩阵乘法串联起来。这在做多级旋转链时特别方便。

但它的缺点也很明显——太占资源。9个元素，每次更新都要做大量的乘法和加法。我早期在嵌入式平台上做惯导时，CPU资源紧张，用DCM做姿态更新，帧率根本跑不上去。而且，DCM必须保持正交性，否则会引入误差。所以每次更新后，还得做一次正交化修正，这又是一笔开销。

💡 个人经验： 我个人习惯在初始对准阶段使用DCM。因为对准过程需要高精度，而且计算量不是瓶颈。但一旦进入导航解算阶段，我会立刻切换到四元数。

2.3 四元数：工程中的首选

四元数，这才是我们工程界的宠儿。它用四个数来描述旋转：一个标量加三个矢量。你想想看，比DCM少了5个元素，计算量直接降了一大截。

四元数最大的优点是无奇异性、计算高效、易于插值。它的微分方程是线性的，而且只有四个参数。在嵌入式平台上，用四元数做姿态更新，效率比DCM高出一大截。

我记得有一次，我在一个低成本的MEMS IMU项目上做姿态解算。CPU主频只有几十兆赫，内存也小得可怜。用DCM根本跑不动，换成四元数后，不仅跑起来了，还能留出余量做其他任务。

# 四元数姿态更新示例（简化版）
import numpy as np

def quaternion_update(q, gyro, dt):
    """
    q: 当前姿态四元数 [w, x, y, z]
    gyro: 角速度 [gx, gy, gz] (rad/s)
    dt: 时间间隔 (s)
    """
    w, x, y, z = q
    gx, gy, gz = gyro
    
    # 计算四元数导数
    q_dot = 0.5 * np.array([
        -x*gx - y*gy - z*gz,
         w*gx + y*gz - z*gy,
         w*gy - x*gz + z*gx,
         w*gz + x*gy - y*gx
    ])
    
    # 一阶积分
    q_new = q + q_dot * dt
    
    # 归一化（保持单位四元数）
    q_new = q_new / np.linalg.norm(q_new)
    
    return q_new

    🔑 核心要点： 四元数虽然不直观，但它是工程实现的最佳选择。我建议你把四元数作为姿态解算的核心，只在需要输出或显示时，才转换成欧拉角。

2.4 旋转矢量：高精度解算的利器

旋转矢量，也叫等效旋转矢量。它用一个三维矢量来描述旋转，矢量的方向是旋转轴，大小是旋转角度。说白了，它就是四元数的对数映射。

你可能会问，已经有了四元数，为什么还要用旋转矢量？嗯，这里有个关键问题——圆锥运动。在振动环境下，角速度测量会有高频噪声，直接用四元数积分会产生不可交换误差。而旋转矢量可以很好地补偿这个误差。

我在做高精度光纤陀螺惯导时，就深刻体会到了这一点。当时在振动台上做测试，用四元数直接积分，姿态误差一直在漂。后来换成了旋转矢量法，加上圆锥补偿，误差立马降了一个数量级。

旋转矢量的微分方程比较复杂，但好在有经典的毕卡-加里宁算法可以求解。实际工程中，我们通常用旋转矢量来做多子样算法，比如二子样、三子样，来抑制圆锥误差。

方法	参数个数	奇异性	计算量	适用场景
欧拉角	3	有	中	小角度、人机交互
方向余弦矩阵	9	无	高	初始对准、多级旋转链
四元数	4	无	低	通用姿态解算（首选）
旋转矢量	3	无	中高	高精度、圆锥运动补偿

2.5 如何选择？我的建议

好了，四种方法都讲完了。你可能会问，那我到底该用哪个？

我个人习惯是这样的：

日常导航解算：用四元数。效率高，无奇异性，够用。
初始对准/调平：用欧拉角或DCM。小角度下欧拉角很直观，DCM精度高。
高精度/振动环境：用旋转矢量。虽然复杂一点，但能有效抑制圆锥误差。
输出给用户看：转成欧拉角。毕竟人脑理解不了四元数。

💡 一个小技巧： 我经常在代码里维护一个四元数作为核心姿态，然后写几个转换函数，需要时再转成欧拉角或DCM。这样既保证了效率，又兼顾了灵活性。

嗯，这一章的内容就到这里。记住，没有最好的方法，只有最合适的方法。下一章，我们会深入讲解四元数的具体运算和更新算法，到时候再聊。

📚 课程目录