3. 惯性导航基础：捷联惯导系统（SINS）原理、姿态更新算法与速度位置更新方程

各位同学，今天我们进入惯性导航最核心的部分——捷联惯导系统。说实话，我刚入行时觉得“捷联”这个词特别高大上，后来才明白，说白了就是把IMU直接“绑”在载体上，用数学代替物理平台。嗯，咱们今天就把这层窗户纸捅破。

传统平台惯导，是用物理陀螺稳定一个平台，让加速度计始终保持在导航坐标系里。捷联惯导呢？它把IMU直接固定在载体上，用计算机来“虚拟”一个数学平台。

我个人习惯把SINS比作一个“数学魔术”：

我在项目中遇到过一个问题：某次跑车实验，IMU安装支架有微小形变，导致姿态发散。后来才意识到，捷联惯导对安装误差特别敏感——因为它是“硬连接”，任何结构变形都会直接反映在测量值里。

核心思想：捷联惯导用“数学平台”代替“物理平台”，代价是计算量剧增，但换来了体积、重量和成本的巨大优势。

姿态更新是SINS的灵魂。你想想看，如果姿态算错了，后面速度和位置全是错的。常用的姿态表示方法有三种：欧拉角、旋转矩阵、四元数。但在实际工程中，我几乎只用四元数——为什么？

四元数没有奇异性，计算效率高，适合实时系统。姿态四元数定义为：

q = [q0, q1, q2, q3]^T
其中：q0 = cos(θ/2)
      q1 = e_x * sin(θ/2)
      q2 = e_y * sin(θ/2)
      q3 = e_z * sin(θ/2)

姿态更新方程（离散化形式）：

q_{k+1} = q_k ⊗ exp(Δθ/2)

其中 Δθ = ω * Δt，ω是陀螺测量的角速度

我曾经踩过一个坑：直接用一阶近似更新四元数，结果在高动态场景下姿态发散。后来改用二阶龙格-库塔法，精度提升明显。嗯，这里要注意——四元数必须归一化，否则误差会累积。

避坑指南：我曾经在无人机飞控上发现姿态漂移，排查了两天才找到原因——四元数归一化放在循环外面了。每步更新后必须立即归一化，这是铁律。

旋转矩阵（方向余弦矩阵）用9个元素表示姿态，冗余度高，但物理意义直观。更新公式为：

C_{k+1} = C_k * (I + [ω×] * Δt)

其中 [ω×] 是角速度的反对称矩阵

说实话，旋转矩阵在工程中用得越来越少，主要是计算量大。但它在某些场景下有用——比如需要直接提取欧拉角时。

有了姿态，我们就可以把加速度转换到导航坐标系，然后积分得到速度和位置。说白了，就是牛顿第二定律的离散化实现。

在导航坐标系（通常用n系，即北东地坐标系）下，速度更新方程为：

v_{k+1} = v_k + [C^n_b * f^b - (2ω^n_ie + ω^n_en) × v_k + g^n] * Δt

其中：

我记得第一次写这段代码时，把哥里奥利项漏了，结果在长航时测试中位置误差越来越大。后来加上后，精度提升了一个数量级。

个人经验：在低精度MEMS IMU中，哥里奥利项和运输项可以忽略，因为陀螺噪声本身就很大。但在光纤陀螺或激光陀螺中，这两项必须精确补偿。

位置更新相对简单，直接用速度积分：

p_{k+1} = p_k + v_k * Δt + 0.5 * a_k * Δt^2

但在实际工程中，我建议用更精确的梯形积分：

p_{k+1} = p_k + (v_k + v_{k+1}) * Δt / 2

为什么？因为加速度计噪声经过一次积分后变成速度噪声，再积分一次变成位置噪声——噪声是平方增长的。梯形积分能稍微抑制这种增长。

下面这张图是我自己总结的SINS核心逻辑，你看完应该能理解整个流程：

最后，分享几个我在实际项目中总结的经验：

一句话总结：捷联惯导就是用数学平台代替物理平台，核心是姿态更新（四元数/旋转矩阵），然后通过比力分解和积分得到速度和位置。每一步都有坑，但掌握了原理，你就能避开它们。

好了，这一章的内容就到这里。记住，理论是基础，但真正的理解来自动手实践。下次跑数据时，试着把姿态、速度、位置每一步都打印出来看看——你会发现很多有趣的现象。