第1章：坐标系与姿态描述——惯导系统的“语言”

各位同学，大家好。我是你们这门课的老朋友。今天咱们开始聊《捷联惯导初始对准与动基座技术》的第一章。

说实话，每次带新人，我第一件事不是让他们看代码，而是先问一个问题：“你知道你的传感器在哪个坐标系里说话吗？”

嗯，这个问题很关键。惯导系统说白了，就是一台不断“猜”自己位置和姿态的机器。它怎么猜？靠测量。但测量值本身没有意义，你得知道它是在哪个“参考系”下测的。这就好比我说“我往前走了5米”，你得知道我的“前”是朝哪个方向，对吧？

所以，坐标系和姿态描述，就是惯导系统的“语言”。今天咱们就把这门语言学明白。

1.1 常用坐标系定义

我个人习惯，把坐标系分成四类：i系、e系、n系、b系。这四兄弟，你搞懂了，后面所有公式推导都顺了。

1. 惯性坐标系（i系）

i系，全称是地心惯性坐标系。它是个“绝对”的参考系。原点在地心，Z轴指向地球自转轴（北极方向），X轴和Y轴在赤道平面内，指向遥远的恒星。

为什么叫“惯性”？因为在这个坐标系里，牛顿定律成立。说白了，它不跟着地球转。你想想看，如果传感器跟着地球一起转，那它测到的加速度里就混进了地球自转的离心力。所以，我们得先有个“不转”的基准。

2. 地球坐标系（e系）

e系，也叫地心地固坐标系。它和地球固连，跟着地球一起转。原点还是地心，Z轴还是指向北极，但X轴指向本初子午线与赤道的交点。

这个坐标系有什么用？定位。你手机里的GPS给出的经纬度，本质上就是e系下的坐标。我当年做车载导航时，最头疼的就是把GPS的经纬度（e系）和惯导的加速度（b系）对齐。这一步错了，后面全白搭。

3. 导航坐标系（n系）

n系，通常指“东北天”坐标系。原点在载体所在位置，X轴指向东，Y轴指向北，Z轴指向天顶（垂直于当地水平面向上）。

为什么用n系？因为人习惯“东、北、天”这种直观的方向。你开车时，导航告诉你“前方500米右转”，这个“右转”就是相对于n系的。惯导解算出的姿态，最终也要转换到n系下，才能被人理解。

4. 载体坐标系（b系）

b系，就是传感器自己身上的坐标系。原点在载体质心，X轴指向载体前方（右），Y轴指向载体右侧，Z轴指向载体下方（符合右手定则）。

IMU（惯性测量单元）输出的加速度和角速度，默认就是在b系下测的。你想想看，陀螺仪测的是“绕b系各轴的旋转角速度”，加速度计测的是“沿b系各轴的比力”。所以，所有解算的第一步，就是把b系下的测量值，转换到n系下。

1.2 姿态描述方法

好了，坐标系定义清楚了。接下来，我们怎么描述一个坐标系相对于另一个坐标系的“朝向”？

说白了，就是怎么用数学语言，把b系“拧”到n系。常用的方法有四种：欧拉角、方向余弦矩阵、四元数、等效旋转矢量。我一个个说。

1. 欧拉角

欧拉角最直观。它用三个角度来描述旋转：航向角（ψ）、俯仰角（θ）、横滚角（γ）。

你可以想象成：先绕Z轴转ψ（航向），再绕Y轴转θ（俯仰），最后绕X轴转γ（横滚）。

但欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，航向和横滚会耦合，丢失一个自由度。我在做飞行器仿真时，就因为这个bug，导致姿态解算在俯冲时直接炸了。嗯，从那以后，我但凡涉及大角度机动，绝对不用欧拉角做内部运算。

2. 方向余弦矩阵（DCM）

DCM是一个3x3的矩阵，记作C_b^n。它把b系下的向量，直接映射到n系下。

比如，你在b系下测到一个加速度向量a_b，那么它在n系下的表示就是：a_n = C_b^n * a_b。

DCM的好处是：没有奇点，可以表示任意姿态。坏处是：9个元素，冗余大，而且必须满足正交性约束。每次更新后，还得重新正交化，否则误差会累积。

# 一个简单的DCM更新示例（角速度增量法）
import numpy as np

def update_dcm(C, omega, dt):
    """
    C: 当前时刻的DCM (3x3)
    omega: 角速度向量 (3x1), 单位 rad/s
    dt: 时间步长
    """
    # 构造角速度的反对称矩阵
    Omega = np.array([[0, -omega[2], omega[1]],
                      [omega[2], 0, -omega[0]],
                      [-omega[1], omega[0], 0]])
    # 一阶近似更新
    C_new = C @ (np.eye(3) + Omega * dt)
    # 别忘了正交化！我一般用SVD分解
    U, _, Vt = np.linalg.svd(C_new)
    C_new = U @ Vt
    return C_new

3. 四元数

四元数，我个人认为是工程上最优雅的姿态描述方法。它用四个数（一个标量+三个矢量）表示旋转，没有奇点，计算量小，而且容易插值。

四元数q = [q0, q1, q2, q3]^T，其中q0是标量部分，代表旋转角的余弦；[q1, q2, q3]是矢量部分，代表旋转轴的方向。

更新公式也很简洁：q_new = q ⊗ Δq，其中Δq是由角速度增量构造的旋转四元数。

# 四元数更新示例
def update_quat(q, omega, dt):
    """
    q: 当前四元数 [q0, q1, q2, q3]
    omega: 角速度 [wx, wy, wz]
    """
    # 计算角速度增量对应的旋转四元数
    norm = np.linalg.norm(omega)
    if norm < 1e-12:
        return q
    half_theta = 0.5 * norm * dt
    delta_q = np.array([np.cos(half_theta),
                        np.sin(half_theta) * omega[0] / norm,
                        np.sin(half_theta) * omega[1] / norm,
                        np.sin(half_theta) * omega[2] / norm])
    # 四元数乘法
    q_new = np.array([q[0]*delta_q[0] - q[1]*delta_q[1] - q[2]*delta_q[2] - q[3]*delta_q[3],
                      q[0]*delta_q[1] + q[1]*delta_q[0] + q[2]*delta_q[3] - q[3]*delta_q[2],
                      q[0]*delta_q[2] - q[1]*delta_q[3] + q[2]*delta_q[0] + q[3]*delta_q[1],
                      q[0]*delta_q[3] + q[1]*delta_q[2] - q[2]*delta_q[1] + q[3]*delta_q[0]])
    # 归一化
    q_new = q_new / np.linalg.norm(q_new)
    return q_new

4. 等效旋转矢量

等效旋转矢量，也叫罗德里格斯参数。它用一个三维向量φ来表示旋转，方向是旋转轴，大小是旋转角。

这个方法的优势在于：它可以精确处理“圆锥运动”等动态误差。在捷联惯导的高动态场景下，角速度变化剧烈，直接用四元数或DCM的一阶近似会引入不可交换性误差。而等效旋转矢量，通过求解Bortz方程，可以补偿这种误差。

说实话，这个知识点比较深。我当年在导弹制导项目中，才真正体会到它的价值。如果你只是做低动态的车辆导航，用四元数就够了。但如果你做无人机、导弹、或者高旋载体，等效旋转矢量是绕不开的。

1.3 知识体系总览

说了这么多，我画个图帮你理清思路。下面这张SVG图，展示了本章的核心逻辑：从坐标系定义，到姿态描述方法，再到它们之间的转换关系。

1.4 本章小结

好了，咱们捋一捋今天的内容：

• 坐标系：i系是绝对基准，e系是地球固连，n系是导航参考，b系是传感器本体。搞清这四者的关系，是惯导的入门第一课。
• 姿态描述：欧拉角直观但有奇点，DCM冗余但稳定，四元数工程首选，等效旋转矢量用于高动态补偿。
• 我的建议：初学者先从四元数入手，配合DCM做可视化。等遇到高动态场景，再研究等效旋转矢量。别一口吃成胖子。

说实话，这些概念看起来枯燥，但它们是整个捷联惯导的基石。你想想看，如果连坐标系都搞混了，后面所有公式推导、代码实现，都会像在沙滩上盖楼——一推就倒。

下一章，咱们会深入聊“姿态更新算法”，也就是怎么用陀螺仪的角速度，实时更新四元数。到时候，我会带你们手撕代码，看看实际工程中是怎么做的。

---