第2章坐标系基础

2、坐标系基础：地球坐标系、导航坐标系、载体坐标系、坐标系转换原理、欧拉角与四元数

2.1 引言：为什么需要坐标系？

在捷联惯导系统中，惯性测量单元（IMU）直接测量的是相对于惯性空间的角速度和比力。然而，导航解算最终需要输出载体相对于地球的位置、速度和姿态。因此，我们必须定义一系列坐标系，并理解如何在这些坐标系之间进行转换。坐标系是描述物理量（如位置、速度、加速度）的数学基准，也是惯导解算的基石。

2.2 常用坐标系定义

2.2.1 地球坐标系（e系，ECEF）

定义：地球坐标系（Earth-Centered Earth-Fixed，ECEF）是一个原点位于地球质心，且与地球固连的直角坐标系。

原点：地球质心。
X轴：指向本初子午线与赤道的交点。
Z轴：指向地球北极（协议地球极，CTP）。
Y轴：与X、Z轴构成右手直角坐标系（指向东经90度方向）。

特点：该坐标系随地球自转，因此地面上的静止点在该系下的坐标是固定的。常用于GPS定位和全球位置表示。

2.2.2 导航坐标系（n系）

定义：导航坐标系是惯导解算时使用的参考坐标系。在捷联惯导中，通常采用“东北天”（ENU）地理坐标系。

原点：载体所在位置（通常为IMU中心）。
X轴（E轴）：指向当地东向。
Y轴（N轴）：指向当地北向。
Z轴（U轴）：指向当地天向（垂直于参考椭球面向上）。

特点：n系是解算速度、位置和姿态的“工作台”。IMU输出的比力需要转换到n系下进行积分，才能得到速度增量。

2.2.3 载体坐标系（b系）

定义：载体坐标系与运载体（如飞机、导弹、车辆）固连。

原点：载体质心（或IMU安装中心）。
X轴：指向载体前进方向（机头方向）。
Y轴：指向载体右侧（右翼方向）。
Z轴：指向载体下方（构成右手系，即“前-右-下”）。

特点：IMU的陀螺仪和加速度计直接测量的是b系下的角速度和比力。因此，所有原始测量数据都是在b系下表达的。

2.3 坐标系转换原理

坐标系转换的核心是方向余弦矩阵（Direction Cosine Matrix, DCM），也称为旋转矩阵。一个向量在不同坐标系下的投影可以通过一个正交矩阵相乘得到。

例如，将b系下的向量 v^b 转换到n系下：

v^n = C_b^n · v^b

其中 C_b^n 是从b系到n系的旋转矩阵。该矩阵的每一行是b系坐标轴在n系下的方向余弦。

重要性质：旋转矩阵是正交矩阵，其逆等于其转置：(C_b^n)^{-1} = C_n^b = (C_b^n)^T。

2.4 姿态表示方法：欧拉角与四元数

旋转矩阵虽然有9个元素，但只有3个自由度。为了更高效地表示和更新姿态，工程中常用欧拉角和四元数。

2.4.1 欧拉角

定义：欧拉角通过三次绕不同轴的旋转来描述b系相对于n系的姿态。常用的旋转顺序为：Z-Y-X（即航向-俯仰-横滚）。

航向角（ψ, Yaw）：绕Z轴旋转，范围通常为0°~360°或-180°~180°。
俯仰角（θ, Pitch）：绕Y轴旋转，范围-90°~+90°。
横滚角（φ, Roll）：绕X轴旋转，范围-180°~+180°。

优点：物理意义直观，易于理解。

缺点：存在“万向锁”问题（当俯仰角为±90°时，航向和横滚无法区分），且三角函数计算在实时系统中效率较低。

2.4.2 四元数

定义：四元数是一个超复数，由一个实部和三个虚部组成：q = q0 + q1·i + q2·j + q3·k。在姿态表示中，它代表绕一个单位向量旋转一定角度。

优点：

无奇点（无万向锁问题）。
计算效率高（仅需4个参数，且更新时只需简单的乘法和加法）。
易于插值。

缺点：物理意义不如欧拉角直观。

2.4.3 欧拉角与四元数的转换

在惯导解算中，我们通常用四元数进行姿态更新，但在输出给用户或进行可视化时，需要转换为欧拉角。

四元数 → 欧拉角（Z-Y-X顺序）：

import math

def quaternion_to_euler(q):
    """
    将四元数转换为欧拉角（弧度）
    q: [q0, q1, q2, q3]
    返回: [roll, pitch, yaw] 单位：弧度
    """
    q0, q1, q2, q3 = q

    # 计算俯仰角（防止万向锁）
    sin_pitch = 2.0 * (q0 * q2 - q3 * q1)
    if abs(sin_pitch) >= 1.0:
        pitch = math.copysign(math.pi / 2.0, sin_pitch)
    else:
        pitch = math.asin(sin_pitch)

    # 计算横滚角
    sin_roll_cos_pitch = 2.0 * (q0 * q1 + q2 * q3)
    cos_roll_cos_pitch = 1.0 - 2.0 * (q1 * q1 + q2 * q2)
    roll = math.atan2(sin_roll_cos_pitch, cos_roll_cos_pitch)

    # 计算航向角
    sin_yaw_cos_pitch = 2.0 * (q0 * q3 + q1 * q2)
    cos_yaw_cos_pitch = 1.0 - 2.0 * (q2 * q2 + q3 * q3)
    yaw = math.atan2(sin_yaw_cos_pitch, cos_yaw_cos_pitch)

    return [roll, pitch, yaw]

欧拉角 → 四元数（Z-Y-X顺序）：

def euler_to_quaternion(roll, pitch, yaw):
    """
    将欧拉角（弧度）转换为四元数
    返回: [q0, q1, q2, q3]
    """
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)
    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)

    q0 = cr * cp * cy + sr * sp * sy
    q1 = sr * cp * cy - cr * sp * sy
    q2 = cr * sp * cy + sr * cp * sy
    q3 = cr * cp * sy - sr * sp * cy

    return [q0, q1, q2, q3]

2.5 本章小结

坐标系	符号	原点	主要用途
地球坐标系	e系	地心	全球位置基准，GPS输出
导航坐标系	n系	载体位置	惯导解算（速度、位置积分）
载体坐标系	b系	载体质心	IMU原始测量

理解这三个坐标系以及它们之间的转换关系（通过旋转矩阵、欧拉角或四元数），是后续进行速度积分和位置解算的前提。在下一章中，我们将利用本章的坐标系知识，推导IMU测量值如何转换到导航系下进行积分。

📚 课程目录