第四章：初始对准——粗对准与精对准
各位同学，今天我们来聊聊惯性导航系统里一个绕不开的话题——初始对准。说白了，就是让系统在启动时知道自己“面朝哪、站多直”。我做了这么多年惯导，见过太多因为对准没做好，导致后续导航结果一塌糊涂的案例。嗯，咱们一步步来拆解。
4.1 粗对准：解析法与罗经法
粗对准，顾名思义，就是快速给系统一个大概的姿态。精度不高，但胜在快。我习惯把它比作“先找个方向，再慢慢调准”。
4.1.1 解析法
解析法，说白了就是利用重力矢量和地球自转角速度矢量来解算姿态。你想想看，重力方向是垂直向下的，地球自转轴是固定的，这两个矢量在载体坐标系下的投影，就能反推出姿态矩阵。
具体怎么做？我简单说一下：
- 首先，测量重力在载体坐标系下的分量（gx, gy, gz）。
- 然后，测量地球自转角速度在载体坐标系下的分量（ωx, ωy, ωz）。
- 最后，利用这两个矢量的叉积和点积，解算出姿态矩阵。
我在项目中遇到过一个问题：如果载体有轻微晃动，解析法的精度会急剧下降。所以，实际应用中通常会对测量值做一段时间的平均，来抑制噪声。
核心公式：
C_b^n = f(g^b, ω^b)其中，C_b^n 是姿态矩阵，g^b 是重力在载体坐标系下的投影，ω^b 是地球自转角速度在载体坐标系下的投影。
4.1.2 罗经法
罗经法，这个名字听起来有点古老，但原理很实用。它利用磁罗盘或陀螺罗盘来寻找北向。我早期做船用惯导时，经常用这个方法。
罗经法的核心思想是：
- 利用磁罗盘测量地磁场方向，确定磁北。
- 或者利用陀螺罗盘测量地球自转角速度的水平分量，确定真北。
这里有个坑，我曾经踩过：磁罗盘容易受周围铁磁物质干扰。有一次在钢铁结构的船上做实验，磁罗盘读数偏了十几度，导致对准失败。后来我学乖了，必须做硬磁和软磁补偿。
注意：罗经法在极地地区会失效，因为地磁场和地球自转轴的方向变化很大。这时候，解析法反而更可靠。
4.2 精对准：卡尔曼滤波
粗对准做完，我们有了一个大概的姿态。但精度还不够，通常误差在几度到十几度。接下来，就要靠精对准来“打磨”了。
精对准，我几乎只用卡尔曼滤波。为什么？因为它能融合多种信息，还能估计误差。你想想看，粗对准后的姿态误差、陀螺漂移、加速度计零偏，这些都可以通过卡尔曼滤波来估计和补偿。
卡尔曼滤波的精对准流程，我总结为三步：
- 建立状态方程：状态量通常包括姿态误差角、速度误差、位置误差、陀螺漂移、加速度计零偏等。
- 建立观测方程：观测值通常是速度误差（比如，静止时速度应为0，但系统测出来不为0，这个差值就是观测值）。
- 迭代更新：利用卡尔曼滤波的预测-更新循环，不断修正状态量。
我的经验：卡尔曼滤波的初始协方差矩阵设置很关键。设得太小，滤波收敛慢；设得太大，滤波容易发散。我一般会根据粗对准的精度来设置，比如姿态误差角初始协方差设为(1°)^2。
下面是一个简化的卡尔曼滤波代码示例，用于精对准中的姿态误差估计：
// 状态量：姿态误差角 (phi_E, phi_N, phi_U)
// 观测量：水平速度误差 (dV_E, dV_N)
// 预测步骤
x_pred = F * x_est; // F为状态转移矩阵
P_pred = F * P_est * F' + Q; // Q为过程噪声协方差
// 更新步骤
K = P_pred * H' * inv(H * P_pred * H' + R); // K为卡尔曼增益
x_est = x_pred + K * (z - H * x_pred); // z为观测值
P_est = (I - K * H) * P_pred;这段代码看起来简单，但实际工程中，F矩阵和H矩阵的推导非常繁琐。我记得有一次，因为F矩阵中一个符号写反了，导致滤波结果一直发散，排查了整整两天才找到问题。
4.3 动基座对准
前面讲的都是静基座对准，也就是载体不动的情况。但现实中，很多场景下载体是运动的，比如飞机在滑行、舰船在航行。这时候，静基座对准的方法就不适用了。
动基座对准，说白了就是在运动过程中完成对准。我做过的最复杂的动基座对准项目，是在一艘高速快艇上，船体晃动剧烈，GPS信号还经常丢失。那真是考验人的时候。
动基座对准的常用方法：
- 利用外部参考信息：比如GPS提供的速度和位置，作为卡尔曼滤波的观测值。
- 利用运动约束：比如车辆在平坦路面行驶时，垂直速度近似为0；舰船在海上航行时，横摇和纵摇有周期性规律。
- 利用多传感器融合：比如IMU+GPS+里程计的组合，可以互相弥补不足。
动基座对准的关键：一定要处理好运动加速度和重力加速度的分离。如果运动加速度被误认为是重力，对准误差会非常大。我常用的方法是，利用GPS速度微分得到加速度，然后从IMU测量值中减去这个运动加速度，剩下的就是重力分量。
这里有一个避坑指南，我曾经吃过亏：动基座对准时，如果载体做匀速直线运动，那还好办。但如果载体做加速或转弯运动，运动加速度会严重干扰对准。我的经验是，在转弯过程中，暂停对准更新，等直线运动时再继续。
4.4 知识体系总览
为了让大家更直观地理解本章的知识结构，我画了一张流程图：
这张图把本章的知识结构梳理清楚了。粗对准是基础，精对准是提升，动基座对准是实战。三者层层递进，缺一不可。
4.5 总结与经验
好了，关于初始对准的内容，我就讲到这里。最后，我分享几点个人经验：
- 粗对准别太粗：虽然叫粗对准，但也不能太随意。我一般要求粗对准误差在5°以内，否则精对准很难收敛。
- 卡尔曼滤波要调参：Q矩阵和R矩阵的设置，直接影响滤波效果。我习惯先做仿真，再在实际系统中微调。
- 动基座对准要谨慎：运动加速度是最大的干扰源。我建议在运动过程中，尽量保持匀速直线运动，或者利用外部传感器辅助。
嗯，这些经验都是我用时间和教训换来的。希望大家在实际项目中，能少走一些弯路。