3. 惯性导航原理（下）：捷联惯导系统（SINS）的初始对准与误差方程
各位同学，欢迎来到第三章的下半部分。上一节我们聊了陀螺和加速度计怎么用，这一节我们聊聊更核心的东西——系统怎么“醒过来”，以及它为什么会“跑偏”。
说白了，捷联惯导系统（SINS）刚上电的时候，它就是个“睁眼瞎”。它不知道自己的脑袋朝哪，也不知道自己站在哪。初始对准，就是帮它找回“方向感”的过程。而误差方程，则是我们用来分析它为什么会“迷路”的数学工具。
我在项目里遇到过好几次，对准没做好，后面整个导航结果都是错的。嗯，这节内容，你值得认真看。
3.1 初始对准：粗对准 + 精对准
初始对准，我习惯把它分成两步：粗对准和精对准。粗对准是快速给个大概方向，精对准是慢慢把误差磨掉。
3.4.1 粗对准（Coarse Alignment）
粗对准的原理其实很简单。你想想看，在地球上，重力方向是确定的（指向地心），地球自转角速度方向也是确定的（指向北极）。我们利用这两个已知量，就能反推出载体的姿态。
具体怎么做？
- 利用加速度计测重力：加速度计在静止时测量的是重力加速度g。如果载体水平，那么水平方向的加速度计输出为0，垂直方向输出为g。如果载体倾斜，三个轴的分量就能告诉我们倾斜角。
- 利用陀螺测地球自转：陀螺在静止时能敏感到地球自转角速度ωie（约15°/h）。在北半球，这个角速度的水平分量指向北。通过这个分量，我们就能确定航向。
粗对准的公式，我直接给出来：
// 粗对准：利用加速度计和陀螺计算初始姿态矩阵
// 假设载体静止，忽略噪声
// 1. 利用加速度计计算俯仰角θ和横滚角γ
θ = atan2(-f_x, sqrt(f_y^2 + f_z^2));
γ = atan2(f_y, f_z);
// 2. 利用陀螺计算航向角ψ
// 先计算地球自转角速度在导航系下的投影
ω_n = [ωie*cos(L), 0, -ωie*sin(L)]; // L为当地纬度
// 再计算载体坐标系下的投影
ω_b = C_n^b * ω_n;
// 通过ω_b的x、y分量反算航向
ψ = atan2(ω_b_x, ω_b_y);
3.4.2 精对准（Fine Alignment）
粗对准只是给了个“大概齐”，精对准才是真正的重头戏。精对准的核心思想是：利用卡尔曼滤波，把粗对准的残差估计出来并补偿掉。
精对准通常采用“速度匹配”的方式。什么意思呢？
- 我们知道，载体静止时，真实速度是0。
- 但惯导系统解算出来的速度，因为姿态误差的存在，会有一个“虚假速度”。
- 卡尔曼滤波把这个虚假速度作为观测量，反过来估计姿态误差角。
精对准的状态量通常包括：
- 姿态误差角（φ_E, φ_N, φ_U）—— 三个方向
- 速度误差（δV_E, δV_N, δV_U）—— 三个方向
- 陀螺零偏（ε_x, ε_y, ε_z）—— 三个轴
- 加速度计零偏（∇_x, ∇_y, ∇_z）—— 三个轴
一共12个状态。观测量就是速度误差（δV_E, δV_N, δV_U）。
3.2 SINS的误差方程推导
误差方程，说白了就是描述“误差是怎么传播的”。你想想看，陀螺有零偏，加速度计有零偏，初始对准有残差，这些误差会怎么影响后面的速度、位置和姿态？
这部分推导比较繁琐，我尽量用直观的方式讲。记住一个核心思想：误差方程是线性化的，它假设真实值 = 标称值 + 小误差。
3.2.1 姿态误差方程
姿态误差方程描述的是：陀螺的测量误差，如何导致姿态计算误差。
公式长这样：
φ_dot = -ω_in^n × φ + δω_in^n - C_b^n * ε^b
其中：
φ = [φ_E, φ_N, φ_U]^T —— 姿态误差角向量
ω_in^n —— 导航系相对于惯性系的角速度
δω_in^n —— ω_in^n的计算误差
C_b^n —— 姿态矩阵
ε^b —— 陀螺零偏（载体坐标系下）
这个公式怎么理解？
- 第一项 -ω_in^n × φ：姿态误差的“自耦合”。说白了，一个方向的姿态误差会耦合到另一个方向。
- 第二项 δω_in^n：地球自转和载体运动引起的角速度计算误差。
- 第三项 -C_b^n * ε^b：陀螺零偏直接注入到姿态误差中。
3.2.2 速度误差方程
速度误差方程描述的是：加速度计误差和姿态误差，如何导致速度计算误差。
公式：
δV_dot = f^n × φ + C_b^n * ∇^b - (2ω_ie^n + ω_en^n) × δV + δg^n
其中：
δV = [δV_E, δV_N, δV_U]^T —— 速度误差
f^n —— 比力在导航系下的投影
∇^b —— 加速度计零偏（载体坐标系下）
ω_ie^n —— 地球自转角速度在导航系下的投影
ω_en^n —— 载体运动引起的角速度
δg^n —— 重力误差
这个公式里，第一项 f^n × φ 特别重要。它表示：姿态误差会把一部分重力加速度“泄漏”到水平方向。这就是为什么姿态不准，速度会很快发散。
我记得有一次做车载测试，初始对准的俯仰角差了0.1度，结果10分钟后水平速度误差达到了0.5m/s。嗯，这就是姿态误差对速度的“放大效应”。
3.2.3 位置误差方程
位置误差方程最简单，就是速度误差的积分：
δL_dot = δV_N / (R_M + h)
δλ_dot = δV_E / ((R_N + h) * cos(L))
δh_dot = δV_U
其中：
δL —— 纬度误差
δλ —— 经度误差
δh —— 高度误差
R_M —— 子午圈曲率半径
R_N —— 卯酉圈曲率半径
位置误差说白了就是速度误差的“累积”。速度误差是常数，位置误差就会随时间线性增长；速度误差是斜坡，位置误差就会随时间平方增长。
3.3 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
这张图把本章的核心逻辑串起来了：初始对准给出初始姿态，误差方程告诉我们这些初始误差和传感器误差会如何传播，最终影响导航解算的结果。
3.4 小结
这一节我们聊了：
- 初始对准的粗对准和精对准方法，以及各自的适用场景
- 姿态、速度、位置三个误差方程的推导和物理含义
- 误差传播的规律：姿态误差导致速度误差，速度误差导致位置误差
说实话，误差方程这部分，我第一次学的时候也觉得头大。但后来做项目多了，发现这些公式其实就是“误差的交通规则”——知道它们怎么走，你才能知道在哪设卡、在哪疏导。
嗯，今天就到这里。下一节我们聊聊组合导航的核心——卡尔曼滤波，到时候你会发现，今天学的误差方程，就是卡尔曼滤波的“骨架”。
公众号:蓝海资料掘金营，微信deep3321