第二章 惯性导航原理（上）：加速度计与陀螺仪的工作原理、比力方程与姿态更新算法
各位同学，欢迎来到第二章。这一章我们开始啃硬骨头——惯性导航的核心原理。
说实话，我刚入行那会儿，觉得惯导这东西特别玄乎。一个盒子装几个传感器，就能算出位置？后来亲手调过几套系统，踩过坑，才慢慢摸清了门道。今天咱们就把这层窗户纸捅破。
2.1 加速度计：它到底在测什么？
很多人以为加速度计测的是「运动加速度」。这是个常见的误解。
加速度计测的其实是比力（Specific Force）。说白了，就是物体受到的惯性力与重力的合力，再除以质量。单位是 m/s²，跟加速度一样。
数学上，加速度计的输出 f 可以写成：
f = a - g其中 a 是物体相对于惯性空间的运动加速度，g 是重力加速度矢量。
嗯，这里要注意：当物体静止在地面上时，a = 0，但加速度计输出的是 -g。也就是说，它测到了向上的支撑力。我当年第一次看到这个数据时愣了半天——明明没动，怎么有读数？
核心理解：加速度计测的是「除了重力以外的力」引起的加速度。想得到真实的运动加速度，必须把重力补偿掉。
2.2 陀螺仪：角速度的测量原理
陀螺仪测的是载体相对于惯性空间的角速度。目前 MEMS 陀螺仪主流用的是振动式陀螺效应。
原理不复杂：一个质量块在驱动方向上高速振动。当载体旋转时，科里奥利力会在检测方向上产生位移。测出这个位移，就能反推出角速度。
公式长这样：
F_c = -2m (ω × v)其中 ω 是输入角速度，v 是振动速度。科里奥利力与 ω 成正比。
我在项目里遇到过一个问题：陀螺仪对温度特别敏感。零偏会随着温度漂移，有时候开机半小时数据还在飘。后来我养成了一个习惯——每次上电后先做 2-3 分钟的静态对准，让温度稳定下来再开始导航。
个人经验：选型时别只看陀螺仪的零偏稳定性，还要看它的「角度随机游走」（ARW）。这个参数决定了你的姿态发散速度。我一般要求 ARW 小于 0.1°/√h。
2.3 比力方程：惯导的基石
比力方程是惯导解算的起点。它描述了载体加速度、比力和重力之间的关系。
在导航坐标系（比如 n 系）下，比力方程的形式是：
v̇^n = C_b^n · f^b - (2ω_ie^n + ω_en^n) × v^n + g^n看着复杂？我们拆开看：
- v̇^n：导航系下的速度变化率，也就是我们想要的加速度
- C_b^n · f^b：把加速度计的比力从载体系转到导航系
- (2ω_ie^n + ω_en^n) × v^n：哥里奥利加速度和向心加速度的补偿项
- g^n：重力加速度
你想想看，如果没有后面两项补偿，直接用加速度计数据积分，那误差会大得离谱。我见过有人做简易惯导，把这两项忽略了，结果跑 10 秒钟位置就偏了几百米。
避坑指南：我曾经在车载导航中忘记补偿哥里奥利项，结果车辆转弯时速度解算出现明显偏差。后来排查了整整两天才发现问题。记住：只要载体在运动，这两项就不能省。
2.4 姿态更新算法：等效旋转矢量法
姿态更新是惯导解算中最关键的一步。姿态算错了，后面速度和位置全完蛋。
常用的方法有：欧拉角法、四元数法、等效旋转矢量法。我个人最推荐的是等效旋转矢量法，因为它能有效抑制圆锥误差。
什么是圆锥误差？简单说，当载体同时存在两个正交方向的角振动时，常规的四元数更新会产生累积误差。等效旋转矢量法通过引入一个修正项，把这个误差给压下去。
更新流程是这样的：
- 读取陀螺仪角增量 Δθ
- 计算等效旋转矢量 Φ
- 由 Φ 计算对应的四元数 q
- 用 q 更新姿态四元数
核心公式：
Φ = Δθ + 1/12 (Δθ₁ × Δθ₂)其中 Δθ₁ 和 Δθ₂ 是相邻两个采样周期的角增量。这个叉乘项就是圆锥误差的补偿。
关键点：等效旋转矢量法要求陀螺仪输出的是「角增量」而不是「角速度」。如果你的陀螺仪只输出角速度，需要先积分成角增量再用这个方法。
2.5 速度与位置更新算法
姿态更新完了，接下来就是速度和位置。这部分相对直观一些。
速度更新：
v_k = v_{k-1} + [C_b^n · f^b - (2ω_ie^n + ω_en^n) × v_{k-1} + g^n] · Δt说白了就是：上一时刻的速度 + 加速度 × 时间。
位置更新：
p_k = p_{k-1} + v_k · Δt位置更新更简单，就是梯形积分。不过要注意，在大范围导航时，位置要用经纬高表示，更新公式会复杂一些，涉及地球曲率半径。
我习惯在代码里把速度更新和位置更新分开写，方便调试。曾经有一次位置发散，我通过单独打印速度中间值，发现是比力补偿项算错了。
调试技巧：做惯导解算时，建议把每一步的中间结果都记录下来。姿态四元数、速度、位置，每个时刻都存一份。出问题时，回放数据就能定位到是哪一步算错了。
2.6 本章知识体系总览
下面这张图总结了本章的核心逻辑：
从这张图可以看得很清楚：加速度计和陀螺仪提供原始数据，经过比力方程和姿态更新，最终得到速度和位置。每一步都有坑，每一步都需要仔细处理。
好了，这一章的内容就到这里。记住：惯导的核心不是公式有多复杂，而是你理解每个物理量到底代表什么。下一章我们会讲初始对准和误差模型，那才是真正考验工程能力的地方。
公众号:蓝海资料掘金营，微信deep3321