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

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

GNSS与IMU紧耦合组合导航系统的核心任务，是融合两类异构传感器的量测信息，以估计载体的位置、速度和姿态。而这一切数学运算的前提，是建立一套严格、统一且可相互转换的坐标系与姿态描述体系。若坐标系定义不清或姿态转换错误，即便滤波器设计再精妙，也会因“差之毫厘”而“谬以千里”。本章将系统梳理地心地固系（ECEF）、导航系（ENU）、载体系（b系）的定义，深入解析姿态角（欧拉角）、四元数与旋转矩阵三种姿态表示方法的数学本质、适用场景及相互转换关系，为后续章节的惯性导航解算与紧耦合滤波模型奠定坚实的数学基础。

上图清晰地展示了组合导航系统中三大坐标系（e系、n系、b系）之间的转换关系，以及三种姿态表示方法（欧拉角、四元数、旋转矩阵）在系统中所处的角色。下面我们将逐一深入剖析。

4.1 三大坐标系的定义与物理意义

核心原理：坐标系是描述载体运动状态的基准框架。在GNSS/INS紧耦合系统中，我们主要涉及三个坐标系：

地心地固系（ECEF，e系）：原点位于地球质心，Z轴指向协议地球极（CTP），X轴指向本初子午线与赤道的交点，Y轴按右手定则确定。该坐标系随地球自转，是GNSS卫星位置和接收机位置解算的天然参考系。其坐标通常表示为（X, Y, Z），单位为米。
导航系（n系）：通常采用当地水平坐标系（ENU），原点位于载体所在位置，X轴指向东（E），Y轴指向北（N），Z轴指向天顶（U）。导航系是惯性导航解算（速度、位置更新）和载体姿态描述的基准系。从ECEF到ENU的转换依赖于载体当前的经纬度。
载体系（b系）：原点位于IMU测量中心，X轴指向载体前方（前），Y轴指向载体右侧（右），Z轴指向载体下方（下），构成右手系（FRD）。IMU的加速度计和陀螺仪输出的原始数据就是相对于载体系的。

判断方法：在实际工程中，必须明确区分“向量在哪个坐标系下表达”和“坐标系之间的相对旋转”。例如，GNSS给出的速度通常是ECEF系下的速度，而惯导解算需要的是导航系（ENU）下的速度。因此，需要通过一个由经纬度决定的旋转矩阵 C_e^n 进行转换。

坐标系	原点	轴定义	主要用途
e系 (ECEF)	地球质心	X: 赤道与本初子午线交点; Z: 极轴; Y: 右手定则	GNSS卫星轨道、接收机位置
n系 (ENU)	载体位置	X: 东; Y: 北; Z: 天	惯导速度、位置解算，姿态参考
b系 (FRD)	IMU中心	X: 前; Y: 右; Z: 下	IMU原始测量输出

经验提醒：在撰写代码或文档时，务必在变量名中体现坐标系，例如 vel_e、vel_n、acc_b，避免因坐标系混淆导致滤波器发散。

4.2 姿态表示方法：欧拉角、四元数与旋转矩阵

核心原理：姿态描述的是载体系（b系）相对于导航系（n系）的角位置关系。三种主流表示方法各有优劣：

欧拉角（姿态角）：通过三次基本旋转（通常为Z-Y-X顺序）定义，分别对应航向角ψ（绕Z轴）、俯仰角θ（绕Y轴）、横滚角φ（绕X轴）。优点是物理意义直观，便于人机交互和显示。缺点是当俯仰角接近±90°时，航向与横滚运动耦合，产生万向节锁，导致姿态解算奇异。
四元数：由一个实部和三个虚部组成，q = q0 + q1·i + q2·j + q3·k，满足模长为1的约束。它无奇异问题，且微分方程是线性的，计算量小，是组合导航滤波器中姿态状态量的首选。
旋转矩阵（方向余弦矩阵，DCM）：一个3×3的正交矩阵 C_b^n，其列向量为载体系各轴在导航系中的投影。它可以直接用于向量坐标变换，但包含9个元素，存在冗余，且需满足正交性约束，不适合直接作为滤波状态量。

操作流程（转换关系）：在工程中，我们经常需要在这三种表示间切换。以下为关键转换公式：

欧拉角 → 四元数：按Z-Y-X顺序，q = q_yaw ⊗ q_pitch ⊗ q_roll，其中各分量为半角的正余弦组合。
四元数 → 旋转矩阵：由四元数元素直接构造矩阵元素，例如 C[0][0] = 1 - 2(q2² + q3²)。
旋转矩阵 → 欧拉角：从矩阵元素中提取，例如 θ = asin(-C[2][0])，φ = atan2(C[2][1], C[2][2])，ψ = atan2(C[1][0], C[0][0])。

// 四元数转旋转矩阵 (C_b^n) 示例 (C语言风格)
double C[3][3];
C[0][0] = 1 - 2*(q2*q2 + q3*q3);
C[0][1] = 2*(q1*q2 - q0*q3);
C[0][2] = 2*(q1*q3 + q0*q2);
C[1][0] = 2*(q1*q2 + q0*q3);
C[1][1] = 1 - 2*(q1*q1 + q3*q3);
C[1][2] = 2*(q2*q3 - q0*q1);
C[2][0] = 2*(q1*q3 - q0*q2);
C[2][1] = 2*(q2*q3 + q0*q1);
C[2][2] = 1 - 2*(q1*q1 + q2*q2);

常见误区：初学者常将欧拉角旋转顺序混淆，导致姿态解算错误。必须明确系统约定为Z-Y-X（航向-俯仰-横滚）内旋方式。此外，四元数必须定期归一化，以抑制数值误差导致的模长漂移。

4.3 姿态微分方程与更新

核心原理：IMU中的陀螺仪输出的是载体系相对于惯性系的角速度 ω_ib^b。为了更新姿态，我们需要求解姿态微分方程。对于四元数，其微分方程为：

q̇ = 0.5 * q ⊗ [0, ω_ib^b]

其中 ⊗ 表示四元数乘法。在实际解算中，还需扣除地球自转 ω_ie^n 和导航系旋转 ω_en^n 的影响，得到 ω_nb^b。

操作流程：离散化更新通常采用一阶或二阶龙格-库塔法。对于高动态载体，建议使用更高阶的积分方法或采用等效旋转矢量算法（如圆锥补偿），以抑制不可交换误差。

实战场景（小型案例）：某无人机在悬停时，陀螺仪测得 ω_ib^b = [0.01, 0.02, 0.5] rad/s（主要为Z轴偏航）。若采用简单的欧拉积分，步长0.01s，则四元数增量约为 Δq ≈ [1, 0.00005, 0.0001, 0.0025]。经过100秒后，若不进行归一化，四元数模长将偏离1约0.03%，导致姿态误差累积。通过每步归一化，可有效抑制该误差。

总结复盘：姿态更新是组合导航中最核心的环节之一。必须确保陀螺仪零偏已事先补偿，且姿态微分方程中的各项旋转速率（地球自转、导航系旋转）在长航时或高精度场景下不可忽略。

4.4 坐标系转换在紧耦合中的工程应用

核心原理：在紧耦合（深组合）架构中，GNSS接收机输出的原始多普勒频移或载波相位测量值，本质上反映了卫星与接收机之间的相对运动。为了构建量测方程，必须将IMU预测的载体位置、速度转换到ECEF系，并进一步投影到卫星视线方向（Line-of-Sight, LOS）。

操作流程：

将IMU解算的导航系位置（纬度L、经度λ、高度h）转换为ECEF坐标（X, Y, Z）。
将IMU解算的导航系速度 v_n 通过旋转矩阵 C_e^n 的转置转换到ECEF速度 v_e。
计算卫星与接收机之间的几何距离和LOS单位向量。
将ECEF速度投影到LOS方向，得到预测的多普勒频移，与GNSS原始测量值做差，形成量测残差。

案例拆解（排障过程）：某车载组合导航系统在测试中发现，车辆转弯时定位误差明显增大。初步分析怀疑是姿态不准导致速度投影错误。通过日志分析，发现IMU的安装角（载体系与车体坐标系之间的偏差）未标定。该偏差导致车辆在直线行驶时，IMU测得的加速度与车体实际运动方向存在约2°的夹角。在转弯时，该夹角被放大，导致速度估计错误，进而影响位置。解决方法是进行六位置法或动态标定，精确估计并补偿安装角。

步骤	输入	输出	关键公式/矩阵
1. 位置转换	L, λ, h	X, Y, Z	标准椭球变换公式
2. 速度转换	v_n	v_e	v_e = (C_e^n)^T · v_n
3. LOS投影	v_e, 卫星位置	多普勒预测	f_d = (v_e - v_sat) · u_los / λ

风险提示：在紧耦合滤波器中，若坐标系转换矩阵存在微小误差，会直接引入量测偏差，且该偏差与卫星几何分布相关，难以通过滤波估计消除。因此，必须对坐标系转换代码进行严格的单元测试，使用已知的经纬度和速度值验证转换结果的正确性。

总结复盘：坐标系与姿态表示是组合导航系统的“语法”。熟练掌握三者之间的转换，并理解其在GNSS量测方程中的应用，是设计鲁棒紧耦合算法的前提。建议在项目初期就建立统一的坐标变换库，并编写详尽的测试用例，避免后期排查问题的巨大成本。