做惯导的人迟早要碰一次传递对准。无论你是做弹载、机载还是车载组合导航只要载体在运动状态下需要启动一套新的惯导系统就绕不开这个问题新装上去的子惯导初始姿态未知又没法让载体停下来做静基座对准怎么办答案是借一套已经在正常工作的、精度更高的主惯导在运动中把子惯导的姿态误差和器件误差估计出来。这个动作就叫传递对准。我最早接触这个概念是在一个惯性导航系统的算法验证项目里当时最头疼的不是滤波公式本身而是缺一套能快速改参数、能复现、能对结果做定量评估的仿真平台。硬件转台不是随时都有真实飞行的数据更是难得。所以我在MATLAB里从零搭了一套传递对准仿真程序专门用来做算法预研、参数调优和可观测性分析。今天把整个程序的思路和关键实现完整写出来希望能给正在被传递对准折磨的人一点参考。这套程序能做什么先交代清楚生成一条带机动的载体运动轨迹输出真实的速度、姿态、位置模拟主惯导和子惯导的测量输出包括陀螺、加速度计的噪声和常值误差人为注入初始失准角和器件误差制造一个“已知真值的问题”跑速度匹配或姿态匹配的卡尔曼滤波估计失准角和惯性器件误差把估计值和真值画在一起直接看收敛情况。适合谁来参考正在做传递对准算法仿真、想重点验证卡尔曼滤波器设计、或者论文里需要传递对准对比结果的人。看完以后你自己就能搭起来不需要依赖任何商业工具箱只需要一个带基础MATLAB的环境。1. 传递对准要解决的问题和仿真目标1.1 为什么载体运动时不能简单做“静基座对准”捷联惯导启动之后第一步必须知道载体当前的姿态。静基座条件下可以用解析法粗对准也就是利用重力向量和地球自转角速度来确定水平姿态和航向。这个方法有一个隐含前提载体是静止的或者至少没有明显的线运动和角运动。但很多载体的工作环境不允许你停下来慢慢对准。军舰甲板上的飞机、行驶中的车辆、飞行过程中需要重新启动的惯导系统都处在持续运动状态。这时候主惯导已经正常工作精度很高子惯导刚上电姿态一无所知。如果把主惯导的导航参数当作基准让子惯导利用两者的速度和姿态差来反推自己的初始姿态误差这就是传递对准的核心思路。1.2 仿真的目标验证算法而不是等真机真实系统里验证传递对准算法需要高精度主惯导、转台、真实飞行任务周期长成本高。而在MATLAB里搭一套纯数字仿真可以在几分钟内换一组机动轨迹、换一组合噪声参数、换一种匹配方式把算法的性能边界摸清楚。我这套程序设定的典型任务是在60秒内将初始失准角[0.1°0.1°0.5°]的子惯导通过速度匹配卡尔曼滤波估计到0.01°量级以内。60秒只是一个参考窗口实际收敛时间取决于机动幅度和噪声水平。程序里所有注入误差都设为已知真值滤波器估计的结果可以直接和真值对比收敛与否一目了然。2. 建模前要把这三个模块的关系理清楚写代码之前第一件要搞清楚的事情是传递对准仿真里的“误差模型”到底包含哪些模块。如果你直接把论文里的矩阵抄过来开跑多半会翻车。我的建议是先理清楚下面三个模块再动手写代码。2.1 状态方程失准角和速度误差怎么互相影响传递对准的核心状态量是子惯导相对主惯导的失准角φ和速度误差δV。这两者不是独立的失准角会通过比力耦合进速度误差方程而速度误差和姿态误差又会随运动学关系传递。状态向量取15维X [φ_E, φ_N, φ_U, δV_E, δV_N, δV_U, ε_x, ε_y, ε_z, ∇_x, ∇_y, ∇_z]^T其中ε是陀螺漂移∇是加速度计零偏。在小失准角假设下误差方程可以写成如下线性形式φ̇ -ω_in^n × φ C_b^n ε_b δV̇ -(2ω_ie^n ω_en^n) × δV f^n × φ C_b^n ∇_b这就是滤波器的“状态传递”基础。第一个方程说的是失准角怎么随时间变化一部分来自陀螺漂移的积分一部分来自载体自身转动的耦合第二个方程说的是速度误差不仅受加速度计零偏影响还会因为失准角导致比力投影方向出错产生一个f^n × φ的误差项。这一项是最关键的正是它让卡尔曼滤波可以通过速度观测把失准角“反向”估计出来这也是传递对准的原理所在。在MATLAB里构建状态矩阵F时只需要把上面的方程转成矩阵形式F zeros(15, 15); F(1:3, 1:3) -skew(omega_ie omega_en); % 简化忽略位置误差耦合 F(1:3, 7:9) Cbn; % 陀螺漂移到失准角 F(4:6, 1:3) skew(fn); % f^n × φ F(4:6, 4:6) -skew(2*omega_ie omega_en); % 哥氏项 F(4:6, 10:12) Cbn; % 加速度计零偏到速度误差2.2 量测方程速度匹配和姿态匹配的差异量测方程决定了滤波器“看到”什么。工程上最常用的有两种速度匹配。量测取主子惯导速度之差Z V_sub - V_master δV vH矩阵就是把状态中的δV取出来H zeros(3, 15); H(1, 4) 1; H(2, 5) 1; H(3, 6) 1;姿态匹配。量测取主子惯导的姿态误差在小角度假设下近似等于失准角Z θ_sub - θ_master ≈ φ v程序里我建议做一个量测模式选择开关这样你可以非常方便地对比两种匹配方式的性能差异use_att_measurement false; % true姿态匹配 false速度匹配初始化滤波器时对应切换H矩阵。2.3 可观测性为什么必须有机动一开始我不理解为什么仿真里经常出现某个失准角估计始终不动。后来才意识到这不是滤波器写错了而是可观测性问题。简单说可观测性决定了“某个状态能不能从量测中恢复出来”。速度匹配模式下水平失准角φ_E、φ_N可以通过比力与重力的耦合被观测而航向失准角φ_U需要横向加速度的持续激励也就是载体必须转弯或者侧向机动。如果你设计了一条匀速直线轨迹航向失准角基本不可观测滤波器给出的估计只会停留在初值附近。所以轨迹设计里必须专门安排机动段。仿真结果好不好看很大程度上在你设计轨迹的时候就已经决定了。3. 轨迹生成和主子惯导数据模拟3.1 轨迹剖面设计原则传递对准仿真里轨迹生成不是随便让载体动起来就行它得照顾两个需求一是让需要观测的状态被激励出来二是贴近真实飞行包线。我常用的剖面是“直飞-转弯-直飞-转弯-直飞”三段式总时长60秒仿真步长0.01秒0~10s匀速直飞让滤波器进入稳定的传播阶段10~20s以3°/s的偏航角速度左转航向从30°转到60°20~30s直飞让滤波器消化转弯段带来的量测信息30~40s以3°/s右转航向转回30°40~60s直飞收尾观察估计是否能稳定保持。除此之外我会在三个姿态角上都叠一个幅度1°~2°的正弦小扰动频率0.3~0.5Hz。这样做的目的是模拟真实载体持续存在的低幅值抖动这种抖动对水平失准角的可观测性是有帮助的。注意不要把小扰动的幅度加太大否则会超出线性误差模型的小角度假设后面滤波容易出问题。3.2 从真值到量测数据的生成流程有了姿态角时间序列和速度剖面之后按照下面的顺序生成数据就很清晰。第一步把欧拉角转成姿态矩阵。用ZYX旋转顺序MATLAB里可以自己写euler2dcm函数也可以用方向余弦矩阵的递推更新。初始姿态设定为俯仰0°横滚0°航向30°。第二步生成速度真值。设地速恒定50m/s按航向角分解到北东地坐标系Vn_true [50*cos(yaw); 50*sin(yaw); zeros(1, N)];第三步由速度差分得到加速度再算出比力真值f^n。捷联惯导中加速度计感受的是比力而不是加速度。忽略小量时可以写成f^n ≈ V̇ (2ω_ie^n ω_en^n) × V - g^n这个公式如果你一时不记得记住物理含义就行比力就是载体受到的非引力加速度。在静态水平状态下加速度计输出的就是重力反方向的大小。第四步生成陀螺和加速度计测量值。给真值加上常值漂移和白噪声gyro_meas gyro_true gyro_bias randn(3, N) * sigma_gyro; acc_meas acc_true acc_bias randn(3, N) * sigma_acc;3.3 误差注入给子惯导人为制造初始失准角这是仿真程序里最巧妙的一步。要让结果能定量评估就必须“明知故犯”地注入已知误差然后看滤波器能不能把它还原出来。我给子惯导设的初始失准角是φ_0 [0.1°; 0.1°; 0.5°]水平两个方向各0.1°航向0.5°。这是很典型的传递对准任务场景航向误差相对大一些。同时注入陀螺零偏0.01°/h、加速度计零偏100μg。之后的做法是不直接去跑完整的捷联解算而是用误差方程把子惯导相对主惯导的误差随时间传播出来作为“真实误差”再叠加量测噪声形成量测Z。这样滤波器的估计结果可以直接和注入真值对比一眼判断收敛情况。代码如下X_true zeros(15, 1); X_true(1:3) phi0; X_true(4:6) [0.5; -0.3; 0.2]; % 初始速度误差 X_true(7:9) gyro_bias; X_true(10:12) acc_bias; for k 1:N-1 F buildF(fn(:, k), Cbn(:,:, k), omega_ie); X_true (eye(15) F * dt) * X_true; Z_true(:, k) H * X_true; % 之后的量测就是用 Z_true 噪声 end这就把“滤波器估计值 vs 注入真值”的对比变得非常简单。4. 卡尔曼滤波器从零搭建4.1 离散化与矩阵构建连续系统要先离散化才能编程。状态转移矩阵用二阶近似Phi eye(15) F * dt 0.5 * F^2 * dt^2; Qd Q * dt; % 简单近似的离散过程噪声如果你的系统模型需要更精细可以用矩阵指数函数但对传递对准仿真来说二阶近似已经足够而且不容易出现数值发散。真正需要花心思的是F矩阵的构建函数务必要保持单位一致。陀螺漂移用rad/s加速度计零偏用m/s^2不要混用度、角秒这些单位否则隐蔽性极强。4.2 初始化参数怎么定卡尔曼滤波最让人头疼的不是公式推导而是初始协方差P0和噪声阵Q、R的量级怎么给。我一开始把P0给得特别大结果前几秒估计值疯狂跳后来又把R给得很小结果滤波器直接发散。调了几轮之后我总结出一套比较稳的初值参数取值说明P0对角线失准角(1°)^2对0.5°以内的初始失准角足够P0对角线速度误差(0.5 m/s)^2与初始速度误差量级匹配P0对角线陀螺漂移(0.02°/h)^2别太大否则前期波动剧烈P0对角线加速度计零偏(200μg)^2和100μg的注入值同量级R速度量测diag(0.1^2)对应0.1m/s量测噪声Q过程噪声对角阵按传感器白噪声水平给见代码段一个实用技巧先把R设得特别大让滤波器几乎只做状态预测跑一次看状态会不会漂移然后再逐步缩小R观察收敛速度和噪声之间的平衡。如果R太小新息会被噪声主导状态估计会出现高频抖动。4.3 滤波主循环和状态估计输出滤波主循环其实很短核心就是时间更新和量测更新两件事for k 1:N-1 % 时间更新 X_pred Phi * X_est; P_pred Phi * P * Phi Qd; % 量测更新 Z vs_sub(:, k) - vs_master(:, k); S H * P_pred * H R; K P_pred * H / S; X_est X_pred K * (Z - H * X_pred); P (eye(15) - K * H) * P_pred; phi_his(:, k1) X_est(1:3); dV_his(:, k1) X_est(4:6); end程序跑完之后绘图部分直接把估计的失准角曲线叠到注入真值上。记得把单位转换成角度或者角分不然图上纵坐标的数字会非常难看。5. 仿真结果怎么看、怎么调5.1 收敛判断不能只看单点不要盯着某个时间点的估计值就直接说“收敛了”。我判断一套仿真是否正常主要看三个指标失准角估计误差是否进入稳态且稳态值在设定性能范围内新息序列Z - H*X_pred是否在零附近波动不出现明显偏置P矩阵对角线是否随着收敛逐步减小而不是振荡或发散。这三个指标同时满足才可以认为滤波器正常工作。5.2 典型结果对照转弯段决定航向失准角以速度匹配为例跑出来比较典型的规律是在0~10s直飞段水平失准角基本不动航向失准角更是完全原地踏步进入第一个转弯段之后水平失准角和航向失准角同时快速收敛大概10秒内就能收敛到0.01°量级后面的右转段和直飞段只是让估计值进一步稳定。如果你看到“直飞段航向失准角也在下降”这种结果先别高兴大概率是量测噪声设置得太小或者滤波器参数不当属于假收敛。真正可靠的做法是故意把转弯段去掉跑一遍这时候航向失准角应该明显不收敛这就能证明你的可观测性设计是生效的。5.3 发散排查链路仿真跑飞了怎么排查我比较推荐按下面的顺序一步步来第一先查F矩阵里f^n × φ的符号方向。这是最容易出错的点符号反了滤波器必发散 第二把量测噪声R加大看状态是否会趋于稳定。如果R加大以后反而不发散说明原来的R太小量测噪声被滤波当成真实信号用了 第三检查新息序列。如果新息一直有固定偏置说明注入误差的真实演化过程和滤波器的模型不一致。这时候重点检查状态转移矩阵和陀螺、加速度计零偏的单位换算 第四如果失准角估计收敛到某个偏置值下不来大概率是陀螺漂移不可观测或者注入的某轴漂移没有被激励起来。这种情况不是调参能解决的要从轨迹机动上找改善空间。6. 工程仿真中容易被忽略的细节6.1 杆臂效应会让速度量测产生系统性偏差主子惯导装在同一个载体上不代表它们感受的运动完全一样。只要安装位置不重合载体转动时两个位置的速度就不同这个速度差叫杆臂速度。速度匹配量测里如果不补偿就会给滤波器引入系统偏置最终表现为失准角估计有稳态误差。仿真里模拟杆臂效应很简单只需要在量测生成时加一项lever_arm [2; 0.2; 0.3]; % 子惯导相对主惯导的杆臂单位米 V_lever Cbn * cross(gyro_true, lever_arm); % 载体转动引起的杆臂速度真实系统里还需要补偿主惯导本身的运动速度差这里只是示意。至少要明白速度匹配不是简单的“两个速度相减”就完事。6.2 时间同步和数据率的影响比你想象的大仿真里通常会忽略时间同步问题因为所有数据都在同一时刻生成。真实系统中这往往是最大的坑主惯导和子惯导的输出频率不一致信号经过通讯链路还有延迟。姿态匹配对时间延迟尤其敏感因为角度随时间在快速变化延迟几十毫秒就可能引入明显误差。仿真中想验证这个影响可以把量测数据人为平移几个采样周期看滤波结果会不会变差。我试过仅仅延迟0.05秒航向失准角估计的稳态误差就能从0.01°恶化到0.1°量级。6.3 线性模型的适用范围要当成硬约束最后说一个很多人容易忽视的前提。这套程序基于小失准角线性误差模型要求初始失准角在几度以内。如果应用场景里初始失准角很大比如航向完全未知那线性卡尔曼是扛不住的你需要的是大失准角模型或非线性滤波。仿真中我的做法是给程序加一个前置检查如果注入的初始失准角超过5°直接提示算法适用性不足免得后面调参半天结果还是不对。我自己跑这套程序时最大的体会是传递对准仿真最珍贵的不是“能跑通”而是“能对比”。由于注入了真值每一次参数修改之后你都能明确看到收敛是变好了还是变坏了这对理解可观测性、调滤波器参数都有非常大的帮助。后面如果你在这个基础上做扩展可以尝试加入里程计或者多普勒雷达辅助的传递对准、考虑挠曲变形的动态传递对准或者把可观测度分析做成实时曲线都是很有价值的方向。