3. 惯性导航系统仿真基础
惯性导航系统（INS）依靠陀螺仪测量角速度、加速度计测量比力，通过机械编排递推得到姿态、速度和位置。仿真一套惯导系统，核心不是把传感器理想化，而是把误差源、编排算法和初始条件三者串起来，观察误差如何随时间传播。本节围绕误差模型、机械编排、初始对准和误差传播四条线索展开，并给出可复现的仿真实现思路。
一、坐标系与符号约定
为避免方向混乱，先固定约定：导航坐标系取当地东北天（ENU），即 x 轴指东、y 轴指北、z 轴指天；机体坐标系取右前上（RFU）。姿态用四元数 q 表示，从机体系到导航系的旋转记为 q_b^n。陀螺输出为机体系角速度 ω_ib^b，加速度计输出为机体系比力 f^b。重力矢量在 ENU 下为 g^n = [0, 0, −g]，g 取 9.793 m/s²（示例值）。全文与图示均遵循该约定。
二、陀螺仪与加速度计误差模型
MEMS 陀螺和加速度计的误差可归纳为确定性误差与随机误差两类。确定性误差包括零偏、标度因数误差和安装失准角；随机误差包括角度/速度随机游走和零偏不稳定性。工程仿真中常用如下模型：
ω_meas = (I + S_g + M_g) · ω_true + b_g + n_g
f_meas = (I + S_a + M_a) · f_true + b_a + n_a
其中：
S_g, S_a 标度因数误差对角阵（示例：陀螺 300 ppm，加计 500 ppm）
M_g, M_a 安装失准角引起的交叉耦合矩阵（示例：0.02° 量级）
b_g, b_a 零偏，建模为一阶马尔可夫过程
n_g, n_a 白噪声，由随机游走系数决定零偏的一阶马尔可夫模型为 ḃ = −b/τ + w，τ 为相关时间（陀螺示例取 1000 s，加计取 500 s），w 为驱动白噪声。白噪声标准差由随机游走系数换算：若陀螺角度随机游走为 0.2 °/√h，采样周期 Δt = 0.01 s，则单步角度噪声标准差约为 0.2/60×√0.01 ≈ 3.3×10⁻⁴ °。这些参数直接决定误差传播速度，是仿真输入的关键。
三、惯导机械编排算法
机械编排是惯导的“心脏”，按采样周期递推。以 ENU 导航系为例，单步流程如下：
- 姿态更新：由陀螺输出构造增量旋转矢量 Δθ = ω_ib^b·Δt，用四元数乘法更新 q_b^n。为抑制圆锥误差，采用双子样或三子样旋转矢量算法，例如双子样补偿项为 Δθ₁×Δθ₂ 的 2/3。
- 比力坐标变换：将机体系比力旋转到导航系，f^n = C_b^n · f^b，C_b^n 由 q_b^n 生成。
- 速度更新：v̇^n = f^n + g^n − (2ω_ie^n + ω_en^n) × v^n。仿真中若忽略地球自转和曲率，可简化为 v̇^n = f^n + g^n，但长航时仿真必须保留哥氏项和向心项。
- 位置更新：ṗ^n = v^n，用梯形积分或龙格-库塔法提高精度。
以 100 Hz 采样、飞行 60 s 为例，若陀螺零偏为 0.05 °/h，则 60 s 内姿态漂移约 0.05/3600×60 ≈ 8.3×10⁻⁴ °，对应水平位置误差约 0.5×g×t²×sin(θ_err)，量级在米级。这说明短时仿真中零偏影响有限，但标度因数和失准角在机动条件下会显著放大误差。
四、初始对准仿真
初始对准分粗对准和精对准。粗对准利用重力矢量确定水平姿态，利用地磁或已知航向确定方位。在静止条件下，加速度计输出 f^b ≈ −g^b，可解算俯仰角和横滚角：
θ_pitch = arcsin(f_y / |g|)
γ_roll = arctan2(−f_x, −f_z)
ψ_yaw = 由磁力计或外部航向给定精对准采用卡尔曼滤波，状态取姿态误差、速度误差、陀螺零偏和加计零偏，量测为速度误差（静止时速度真值为零）。滤波收敛后，姿态误差可降至角分级。仿真中可设置初始失准角为 [0.5°, 0.5°, 1.0°]，观察滤波收敛曲线：通常 30～60 s 内水平失准角收敛到 0.05° 以内，方位失准角收敛较慢，约 100～200 s。
五、误差传播特性
惯导误差传播具有明显的振荡特征。水平通道存在舒勒振荡，周期 T_s = 2π√(R/g) ≈ 84.4 min，使姿态和速度误差呈现周期性摆动而非单调发散。方位误差则与陀螺零偏和东向速度耦合，产生随时间增长的航向漂移。仿真中可通过以下方式观察：
- 固定陀螺零偏，改变加计零偏，观察位置误差斜率变化；
- 固定传感器误差，改变初始失准角，观察误差初值对传播的影响；
- 加入机动（如转弯、加速），观察标度因数和失准角引起的误差跳变。
综合来看，惯导仿真质量取决于三件事：误差模型是否覆盖主要误差源、机械编排是否数值稳定、初始对准是否给出合理初值。三者共同决定误差传播曲线的形状和量级，也是后续组合导航仿真与性能验证的基础。