第二章：惯性导航系统原理
各位同学，今天我们来聊聊惯性导航。说实话，这是整个机载武器瞄准系统里最“硬核”的部分之一。我当年刚接触惯导时，也被那一堆坐标系转换搞得头晕。但后来发现，搞懂它，你就能理解导弹为什么能“记住”自己的位置。
2.1 捷联惯导：把陀螺仪“焊死”在弹体上
传统的平台式惯导，有个物理转台，陀螺仪始终指向地理坐标系。捷联惯导不一样——它直接把陀螺仪和加速度计固定在弹体上。说白了，就是“硬连接”。
这样做的好处很明显：体积小、重量轻、可靠性高。坏处呢？计算量爆炸。因为传感器测得的数据都是弹体坐标系下的，你得实时把它转换到导航坐标系。
我在项目中遇到过一个问题：某型导弹在高速旋转时，捷联解算直接发散。后来发现是采样频率不够，角速率变化太快，积分步长太大。嗯，这里要注意，捷联惯导对处理器性能要求很高。
2.2 姿态解算：从角速度到姿态角
姿态解算的核心，就是利用陀螺仪测得的角速度，实时更新载体的姿态。常用的方法有欧拉角法、方向余弦矩阵法、四元数法。
我个人习惯用四元数法。为什么？因为它没有奇点，计算量也适中。你想想看，欧拉角在俯仰角接近±90°时会出现“万向锁”，这在机载武器里是致命的——导弹可能正在做大机动。
2.2.1 四元数更新方程
四元数更新其实不复杂。核心公式就一个：
q_new = q_old + 0.5 * Ω * q_old * Δt其中Ω是由角速度构成的反对称矩阵。每次采样到新的角速度，就更新一次四元数。然后从四元数里提取出姿态角。
我曾经踩过一个坑：四元数更新后忘记归一化。结果姿态越算越偏，最后导弹直接“翻跟头”。所以，每次更新后一定要做归一化处理，这是铁律。
2.2.2 从四元数到欧拉角
转换公式如下：
俯仰角 θ = asin(2*(q0*q2 - q1*q3))
横滚角 φ = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1^2 + q2^2))
航向角 ψ = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2^2 + q3^2))注意，航向角的范围是-π到π，实际使用时需要做象限判断。我建议用atan2函数，它能自动处理象限问题。
2.3 卡尔曼滤波基础：把噪声“滤”出去
卡尔曼滤波，说白了就是一种最优估计算法。它能把传感器噪声和系统噪声的影响降到最低。在惯导系统里，我们用它来融合陀螺仪和加速度计的数据。
为什么需要融合？因为陀螺仪短期精度高，但长期会漂移；加速度计长期稳定，但短期噪声大。卡尔曼滤波就是取长补短。
2.3.1 卡尔曼滤波的五步法
我把它总结成五步，你记住就行：
- 状态预测： 根据上一时刻的状态，预测当前时刻的状态。
- 协方差预测： 预测状态的不确定性。
- 卡尔曼增益计算： 决定相信预测还是相信测量。
- 状态更新： 用测量值修正预测值。
- 协方差更新： 更新不确定性。
代码实现大概长这样：
// 预测
x_pred = A * x_est + B * u;
P_pred = A * P_est * A' + Q;
// 更新
K = P_pred * H' * inv(H * P_pred * H' + R);
x_est = x_pred + K * (z - H * x_pred);
P_est = (I - K * H) * P_pred;这里Q是过程噪声协方差，R是测量噪声协方差。这两个参数怎么调？我个人的经验是：先根据传感器手册给个初值，然后在实际飞行数据上反复试。调参是个手艺活，没有捷径。
2.3.2 一个简单的姿态卡尔曼滤波器
假设我们要估计俯仰角和横滚角。状态量取为：
x = [θ, φ, θ_bias, φ_bias]'其中θ_bias和φ_bias是陀螺仪的零偏。测量量来自加速度计：
z = [θ_acc, φ_acc]'状态转移矩阵A和测量矩阵H分别为：
A = [1, 0, -Δt, 0;
0, 1, 0, -Δt;
0, 0, 1, 0;
0, 0, 0, 1]
H = [1, 0, 0, 0;
0, 1, 0, 0]这样，卡尔曼滤波就能同时估计姿态角和陀螺零偏。我在某型制导炸弹上用过这个方案，效果不错。
2.4 实战中的几个坑
最后，分享几个我踩过的坑：
- 初始化问题： 捷联惯导需要初始对准。如果初始姿态不准，后面全白搭。我建议用加速度计和磁力计做粗对准，再用卡尔曼滤波做精对准。
- 时间同步： 陀螺仪和加速度计的采样时间必须严格同步。差一个毫秒，在高动态下误差就大了。
- 数值稳定性： 四元数更新时，Δt不能太大。我一般控制在1ms以内。如果处理器性能不够，可以考虑用二阶龙格-库塔法。
好了，这一章就到这里。下一章我们讲组合导航，把GPS和惯导结合起来。到时候你会发现，卡尔曼滤波的威力才真正发挥出来。