2. 导航系统坐标系与运动学建模

导航系统仿真的第一步不是写代码，而是把“位置、姿态、速度”这些量放进一个明确的数学参照系里。坐标系定义不清，后续的六自由度方程、误差传播和滤波器设计都会失去意义。本节围绕三件事展开：常用坐标系的定义与转换、飞行器六自由度运动方程、导航状态量选取与误差定义。

一、常用坐标系及其转换

在飞行器导航中，最常用的坐标系有以下几类，它们各自承担不同的表达任务：

• 地心惯性系（ECI，i 系）：原点在地心，坐标轴相对恒星固定，不随地球自转。它是牛顿力学定律成立的基准系，适合描述轨道运动和陀螺仪的理想输出。
• 地心地固系（ECEF，e 系）：原点在地心，随地球一起自转，z 轴指向北极，x 轴指向本初子午线与赤道交点。它适合描述地面站、卫星星下点和地球相关的位置。
• 地理坐标系（n 系，NED）：原点在载体所在点，N 指北、E 指东、D 指地。它是导航解算最直观的局部水平系，位置常用纬度 φ、经度 λ、高度 h 表示。
• 机体坐标系（b 系）：原点在飞行器质心，x 轴指向机头，y 轴指向右翼，z 轴指向机腹。姿态角（滚转 φ、俯仰 θ、偏航 ψ）就是 b 系相对 n 系的转动关系。

坐标系之间的转换靠旋转矩阵或四元数完成。以 n 系到 b 系为例，按“偏航—俯仰—滚转”的 3-2-1 顺序旋转，方向余弦矩阵为：

C_n^b =
[ cosθcosψ,                cosθsinψ,               -sinθ;
  sinφsinθcosψ-cosφsinψ,   sinφsinθsinψ+cosφcosψ,  sinφcosθ;
  cosφsinθcosψ+sinφsinψ,   cosφsinθsinψ-sinφcosψ,  cosφcosθ ]

需要注意：旋转矩阵是正交矩阵，满足 C_n^b = (C_b^n)^T，且行列式为 +1。实际编程中若用四元数 q = [q0, q1, q2, q3] 表示姿态，可避免欧拉角在俯仰 ±90° 附近的万向节死锁问题。四元数到方向余弦矩阵的转换关系为：

C_b^n =
[ 1-2(q2²+q3²),  2(q1q2-q0q3),  2(q1q3+q0q2);
  2(q1q2+q0q3),  1-2(q1²+q3²),  2(q2q3-q0q1);
  2(q1q3-q0q2),  2(q2q3+q0q1),  1-2(q1²+q2²) ]

ECI 与 ECEF 之间只差一个地球自转角。设地球自转角速度 ω_ie = 7.292115×10⁻⁵ rad/s，则 t 时刻的转换矩阵为绕 z 轴旋转 ω_ie·t 角。ECEF 到 n 系的转换则依赖当地纬度 φ 和经度 λ，常用形式为：

C_e^n =
[ -sinφcosλ,  -sinφsinλ,   cosφ;
  -sinλ,        cosλ,        0;
  -cosφcosλ,   -cosφsinλ,  -sinφ ]

下面这张图把上述坐标系及其转换关系组织成一张关系图，便于对照记忆。

二、飞行器六自由度运动方程

六自由度指三个平动自由度（位置）和三个转动自由度（姿态）。在 n 系下，质心平动方程可写为：

ṗ_n = v_n
v̇_n = C_b^n · f_b + g_n - (2ω_ie^n + ω_en^n) × v_n

其中 p_n = [φ, λ, h] 为位置，v_n = [v_N, v_E, v_D] 为地速，f_b 为加速度计测得的比力，g_n 为当地重力矢量，括号项为科里奥利与向心加速度修正。姿态方程用四元数表示为：

q̇ = 0.5 · q ⊗ [0, ω_nb^b]
ω_nb^b = ω_ib^b - C_n^b · (ω_ie^n + ω_en^n)

这里 ω_ib^b 是陀螺仪输出的机体角速度，ω_ie^n 是地球自转在 n 系下的投影，ω_en^n 是载体相对地球运动引起的转移角速度。仿真中通常用四阶龙格-库塔法以固定步长（如 0.01 s）积分上述微分方程。以某小型无人机为例，设初始位置 φ=39.9°、λ=116.4°、h=100 m，初始速度 v_N=30 m/s、v_E=0、v_D=0，初始姿态 φ=0°、θ=5°、ψ=90°，陀螺零偏 0.01°/s，加速度计零偏 1 mg，积分 60 s 后可观察位置漂移量级，用于评估纯惯导的发散速度。

三、导航状态量选取与误差定义

导航滤波器的状态量选取直接决定可观测性和收敛速度。对于惯导/卫星组合导航，常用 15 维误差状态：

误差定义必须与坐标系约定一致。姿态误差通常定义为计算 n 系与真实 n 系之间的小角度旋转矢量，速度误差定义为计算地速与真实地速之差。若采用“计算值减真实值”的定义，则卡尔曼滤波的量测方程 H 矩阵符号也要相应统一，否则会出现滤波器发散或修正方向相反的问题。

综上，坐标系是导航仿真的“语言”，六自由度方程是“语法”，状态量与误差定义是“词汇表”。三者一致，后续的卡尔曼滤波、组合导航和性能评估才有可靠基础。