2. 惯性导航基础：惯性导航原理（捷联式与平台式），牛顿力学与导航方程，IMU测量模型与误差源（零偏、噪声、尺度因子），坐标系定义（导航系、载体系、地心地固系）

各位同学，欢迎来到惯性导航的世界。说实话，这是整个视觉融合导航的基石。你想想看，如果连IMU怎么工作的、误差从哪来的都搞不清楚，后面做融合滤波基本就是盲人摸象。我个人习惯把这一章叫做“打地基”，地基不牢，楼盖得再高也得塌。

2.1 惯性导航原理：平台式 vs 捷联式

惯性导航，说白了就是“猜”你在哪。怎么猜？靠加速度和角速度积分。但积分这东西，一步错步步错。所以系统架构的选择就至关重要了。

平台式惯导，我刚开始接触时觉得它像个“老古董”。它用物理平台模拟导航坐标系，把加速度计和陀螺仪放在一个稳定平台上，平台通过伺服电机始终跟踪导航系（比如当地水平系）。好处是计算简单，因为测量值直接就是导航系下的。坏处嘛——又大又重又贵，机械结构复杂得要命。我在某次项目评审会上见过一个老专家，他当年做平台式惯导，光调平台的水平度就调了三天三夜。

捷联式惯导，这才是现代的主流。它把IMU直接“绑”在载体上，没有物理平台。那怎么得到导航系下的数据？靠计算机算！用陀螺仪测角速度，实时更新姿态矩阵，再把加速度计测的比力投影到导航系。说白了，就是用数学平台代替物理平台。

2.2 牛顿力学与导航方程

惯性导航的物理基础，其实就是牛顿第二定律：F = ma。但放到地球上，事情就复杂了。因为地球在转，重力也在变。

导航方程，我习惯把它写成这样：

在导航系（n系）下：
位置微分：  p_dot = v
速度微分：  v_dot = C_b^n * f^b - (2ω_ie^n + ω_en^n) × v + g^n
姿态微分：  C_b^n_dot = C_b^n * (ω_nb^b ×)

看着有点吓人？别怕，我拆开讲。

• C_b^n：从载体系到导航系的旋转矩阵，也就是姿态。这是核心中的核心。
• f^b：加速度计测的比力，单位是m/s²。注意，它测的是“比力”，不是纯加速度。比力 = 加速度 - 重力。
• 2ω_ie^n × v：科里奥利加速度。地球自转引起的，你往北走，它会把你往东推。我在做车载导航时，这个项如果不补偿，几分钟就能偏出去几十米。
• ω_en^n × v：运输项。载体在地球表面移动时，导航系相对地球在转。
• g^n：重力加速度。注意，不是9.8那么简单，它随纬度和高度变化。

2.3 IMU测量模型与误差源

IMU不是完美的。它测出来的数据，永远带着误差。你要是不管这些误差，积分出来的轨迹能飞到天上去。

先看测量模型：

加速度计：  f̃^b = f^b + b_a + s_a * f^b + n_a
陀螺仪：    ω̃^b = ω^b + b_g + s_g * ω^b + n_g

其中：

• b_a, b_g：零偏（bias）。这是最头疼的误差。它是个缓慢变化的量，每次开机都不一样。我习惯叫它“漂移元凶”。
• s_a, s_g：尺度因子（scale factor）。输入和输出不成比例。比如你转10度，它告诉你转了10.1度。
• n_a, n_g：白噪声。高频随机抖动，没法消除，只能靠滤波压制。

误差源我按“杀伤力”排个序：

误差类型	对位置的影响	我的处理建议
零偏（bias）	随时间平方增长，最致命	必须在线估计，用视觉或GPS辅助
尺度因子	随运动幅度线性增长	出厂标定，或在线估计
白噪声	随机游走，缓慢发散	滤波平滑，选高精度IMU
交轴耦合	各轴互相串扰	出厂标定矩阵补偿

2.4 坐标系定义

做导航，坐标系搞不清楚，后面全白搭。我见过太多人把坐标系搞混，结果融合滤波直接炸了。

三个核心坐标系：

• 导航系（n系）：通常用NED（北-东-地）。原点在载体当前位置，x轴指北，y轴指东，z轴指向地心。做地面导航，这个最常用。
• 载体系（b系）：原点在IMU中心，x轴指前，y轴指右，z轴指下（右前下）。注意，不同IMU定义可能不同，一定要看手册。
• 地心地固系（e系）：原点在地心，x轴指向本初子午线与赤道交点，z轴指向北极。做全球导航、卫星定位时用。

坐标系之间的转换，核心就是旋转矩阵。从b系到n系，我习惯用欧拉角（横滚、俯仰、航向）表示：

C_b^n = R_z(ψ) * R_y(θ) * R_x(φ)

其中：
φ = 横滚角（roll）
θ = 俯仰角（pitch）
ψ = 航向角（yaw）

嗯，这一章的内容就到这里。说白了，惯性导航就是“积分+补偿”。积分是数学，补偿是工程。把误差模型搞明白，坐标系理清楚，后面做视觉融合才能游刃有余。下一章我们讲IMU的标定与预处理，到时候我会分享一些实测数据的处理经验。