3. 惯性导航系统（INS）基础：加速度计与陀螺仪原理、捷联惯导算法、INS误差特性
各位好，咱们今天聊聊惯性导航系统。说实话，INS这东西在列车定位里是个“硬骨头”——它不依赖外部信号，全靠自己算。但算得准不准，就看你对加速度计和陀螺仪的理解深不深了。
3.1 加速度计与陀螺仪：INS的“眼睛”和“耳朵”
先说说加速度计。它的原理其实不复杂——测量比力。什么叫比力？就是物体受到的惯性力与重力的合力。我习惯把它想象成一个弹簧秤：你把它放在桌上，它测到的是重力；你推它一下，它测到的是推力加重力。
具体到MEMS加速度计，内部有个微小的质量块。当列车加速时，质量块会偏移，电容值就变了。嗯，这里要注意：加速度计测的是“比力”，不是纯运动加速度。所以你得把重力分量去掉，才能得到真正的运动加速度。
关键点：加速度计输出 = 运动加速度 + 重力分量。搞混了这两者，定位误差会大得离谱。
陀螺仪呢？它测的是角速度。原理上，传统的机械陀螺靠高速旋转的转子保持方向稳定。但咱们列车定位里用的多是光纤陀螺或MEMS陀螺。光纤陀螺利用萨格纳克效应——两束光沿相反方向绕光纤环传播，旋转时会产生相位差。我当年第一次调试光纤陀螺时，被它的温漂搞得头大，后来才发现是安装基座的热膨胀没考虑进去。
MEMS陀螺仪更小巧，它靠科里奥利效应工作。一个振动的质量块，当系统旋转时，会产生垂直于振动方向的力。说白了，就是“转起来就有感觉”。
| 传感器类型 | 测量物理量 | 常见原理 | 典型精度 |
|---|---|---|---|
| 加速度计 | 比力（加速度+重力） | 电容式、压阻式 | 0.1~10 mg |
| 陀螺仪 | 角速度 | 光纤、MEMS、激光 | 0.01~10 °/h |
个人经验：选型时别只看精度指标。我曾经在项目中选了一款高精度加速度计，结果它的带宽太低，列车过道岔时的振动信号全被滤掉了。后来换了一款带宽更高的，虽然噪声大一点，但配合滤波算法反而效果更好。
3.2 捷联惯导算法：从原始数据到位置
有了加速度计和陀螺仪的数据，怎么算出位置？这就是捷联惯导算法的事了。捷联，意思是传感器“绑”在载体上，不像平台式惯导那样有物理稳定平台。
算法核心分三步：姿态更新、速度更新、位置更新。我建议你把这个流程记牢，因为所有INS的误差分析都绕不开它。
3.2.1 姿态更新
陀螺仪输出角速度，我们用它来更新姿态矩阵。姿态矩阵描述了载体坐标系和导航坐标系之间的旋转关系。常用的方法有四元数法、方向余弦矩阵法、欧拉角法。
我个人偏爱四元数法。为什么？因为它没有奇点问题，计算量也小。欧拉角在俯仰角接近90度时会“万向锁”，你想想看，列车虽然不会翻，但万一遇到大坡度呢？
// 四元数更新示例（简化版）
// 输入：陀螺仪角速度 wx, wy, wz (rad/s)
// 输入：采样周期 dt
// 输出：更新后的四元数 q0, q1, q2, q3
double norm = sqrt(wx*wx + wy*wy + wz*wz);
double half_angle = norm * dt * 0.5;
// 如果角速度很小，直接用一阶近似
if (norm < 1e-10) {
q0_new = q0 - (q1*wx + q2*wy + q3*wz)*dt*0.5;
q1_new = q1 + (q0*wx - q3*wy + q2*wz)*dt*0.5;
q2_new = q2 + (q3*wx + q0*wy - q1*wz)*dt*0.5;
q3_new = q3 - (q2*wx - q1*wy - q0*wz)*dt*0.5;
} else {
double sin_half = sin(half_angle) / norm;
double cos_half = cos(half_angle);
q0_new = q0*cos_half - (q1*wx + q2*wy + q3*wz)*sin_half;
q1_new = q1*cos_half + (q0*wx - q3*wy + q2*wz)*sin_half;
q2_new = q2*cos_half + (q3*wx + q0*wy - q1*wz)*sin_half;
q3_new = q3*cos_half - (q2*wx - q1*wy - q0*wz)*sin_half;
}
// 别忘了归一化
norm = sqrt(q0_new*q0_new + q1_new*q1_new + q2_new*q2_new + q3_new*q3_new);
q0_new /= norm; q1_new /= norm; q2_new /= norm; q3_new /= norm;
注意：四元数必须定期归一化。我见过有人忘了这步，结果姿态矩阵慢慢“漂移”，最后定位误差大到不可接受。归一化不是可选项，是必须项。
3.2.2 速度与位置更新
姿态更新完后，把加速度计测得的比力转换到导航坐标系，减去重力，再积分得到速度。速度再积分得到位置。说起来简单，做起来全是坑。
举个例子：加速度计有零偏，比如0.1 mg。这个零偏积分一次变成速度误差，再积分一次变成位置误差。位置误差和时间平方成正比。你想想看，列车跑10分钟，误差能有多大？
// 速度更新（简化版）
// 输入：加速度计输出 ax, ay, az (m/s^2)
// 输入：姿态矩阵 Cbn (3x3)
// 输入：重力向量 g (m/s^2)
// 输出：更新后的速度 vx, vy, vz
// 比力转换到导航坐标系
double fx_nav = Cbn[0][0]*ax + Cbn[0][1]*ay + Cbn[0][2]*az;
double fy_nav = Cbn[1][0]*ax + Cbn[1][1]*ay + Cbn[1][2]*az;
double fz_nav = Cbn[2][0]*ax + Cbn[2][1]*ay + Cbn[2][2]*az;
// 减去重力，积分
vx += (fx_nav - 0) * dt; // 假设水平方向无重力分量
vy += (fy_nav - 0) * dt;
vz += (fz_nav - g) * dt; // 垂直方向减去重力
// 位置更新
px += vx * dt;
py += vy * dt;
pz += vz * dt;
避坑指南：我曾经在项目中直接用这个简化版本，结果列车静止时速度一直在漂。后来发现是重力补偿没做准——地球重力不是精确的9.8，不同纬度有差异。对于高精度定位，你得用WGS84模型算当地重力。
3.3 INS误差特性：为什么它不能单独用？
说到INS的误差特性，我直接给你结论：误差随时间累积，且无界。这就是为什么列车定位里INS必须和其他传感器融合。
具体来说，INS误差分三类：
- 初始对准误差：启动时姿态、位置没对准。比如列车停在坡道上，你以为是水平，实际上有倾角。这个误差会一直存在。
- 传感器误差：零偏、刻度因子、噪声。零偏是最头疼的，它导致位置误差随时间二次方增长。
- 算法误差：离散化误差、截断误差。采样率不够高时，积分精度会下降。
我做个简单的误差分析：假设加速度计零偏为0.1 mg（约0.001 m/s²），列车运行100秒。速度误差 = 0.001 × 100 = 0.1 m/s。位置误差 = 0.5 × 0.001 × 100² = 5米。才100秒就漂了5米，跑10分钟呢？你自己算算。
| 误差源 | 对速度的影响 | 对位置的影响 | 时间特性 |
|---|---|---|---|
| 加速度计零偏 | 线性增长 | 二次方增长 | ∝ t² |
| 陀螺仪零偏 | 通过姿态间接影响 | 三次方增长 | ∝ t³ |
| 初始姿态误差 | 线性增长 | 二次方增长 | ∝ t² |
核心结论：INS的短时精度很好（毫秒到秒级），但长时精度很差。在列车定位中，我们通常用它来填补GNSS信号丢失时的空白，或者与轮速传感器、信标等组合使用。
嗯，说到这里，我想起一个项目。当时我们做地铁的定位系统，隧道里GNSS完全失效，全靠INS+轮速传感器。结果有一次列车在站台停了3分钟，INS的位置漂了将近10米。后来我们加了零速修正算法——检测到列车静止时，强制把速度置零，同时估计加速度计零偏。效果立竿见影。
所以，理解INS的误差特性，不是为了抛弃它，而是为了知道怎么“管住”它。多传感器融合，说白了就是拿其他传感器的长处补INS的短处。
下一章咱们聊聊轮速传感器和信标，看看它们怎么和INS配合。到时候我会分享一个实际融合案例，保证让你有收获。