捷联惯导初始对准与航向角解算：MATLAB实现指南

简介这份MATLAB程序包面向捷联惯导系统研究者与导航专业学生解决惯导初始对准与解算编程实现问题覆盖粗对准、精对准和纯惯导解算完整流程。对准部分基于前10分钟静止数据用前2分钟完成解析粗对准用后8分钟进行五状态Kalman滤波精对准为验证收敛效果可在粗对准航向角上人为叠加10度误差观察偏差消除过程。解算部分转入纯惯性导航采用双子样圆锥与划摇补偿每20ms输出三轴姿态、两个水平速度和两个水平位置并绘制姿态、速度、位置及位置误差随时间变化曲线。zip压缩包共12个文件全部为m脚本总大小仅6KB含主程序以及粗对准、Kalman校正、四元数/方向余弦变换、数据预处理等模块结构清晰便于复用。已有574人学习下载适合需要快速搭建惯导仿真、进行课程设计或论文验证的读者。1. 初始对准是惯导解算的第一步航向角错 1 度意味着 6 海里/小时的横向漂移初始对准在捷联惯导系统里是进入导航解算之前必须完成的“摆正位姿”过程。静止的惯性测量单元虽然不动但加速度计能测量重力矢量陀螺仪能测量地球自转角速度矢量这两个物理矢量足以在载体坐标系里恢复出相对当地地理坐标系的姿态初值。实际调试中我见过很多人在 exp2 这类课程包里直接对着align函数改参数调精对准阈值、改滤波器协方差航向角输出却始终差着几十度。真正的原因往往是粗对准负号搞反、陀螺仪量纲没转成 rad/s、或者纬度没有换算成弧度。这篇文章就把粗对准、精对准、惯导解算到航向角输出这条链路用 MATLAB 的代码串起来适合正在调 SINS 课程设计或毕业设计里的初始对准部分、手头有静态 IMU 数据但不知道怎么验证对准结果的工程师。下面给出的代码都是常见做法的最小实现不依赖转台静止采集一两分钟的 IMU 数据就能跑完整个流程。2. 粗对准用双矢量定姿把静止 IMU 测量变成初始姿态矩阵粗对准指用静止 IMU 的加速度计和陀螺仪测量直接求解初始姿态矩阵C_b_n机体坐标系到导航坐标系的转移矩阵。这个过程不求滤波精度只要把姿态估计到度级给后级精对准滤波器一个合理的初值。它的理论依据是双矢量定姿在导航系里已知重力矢量和地球自转角速度矢量的理论值在机体系里又能测到对应的测量值两个非线性共线的矢量足以唯一确定坐标变换。2.1 坐标系约定先统一东北天 n 系、右前上 b 系后面所有公式都以东北天ENU为导航系机体坐标系取“右前上”。IMU 静止时加速度计测量的是比力f a - g物体不动所以a 0比力等于-g^n在机体系下的投影陀螺仪则直接测得地球自转角速度ω_ie^n在机体系下的投影。注意加速度计输出的是比力向量不是重力向量这里差一个负号是粗对准最常见的翻车点。2.2 双矢量构造正交基求解姿态阵用-g^n和ω_ie^n在 n 系下构造一组基用静止段的f_b和ω_b在 b 系下构造对应基那么两套基之间的坐标变换就是C_b_n。下面这段 MATLAB 代码输入是三轴静止段数据输出是初始姿态矩阵function C_b_n coarse_align(acc_static, gyro_static, lat_deg) % 粗对准由静止 IMU 数据解算初始姿态矩阵 C_b_n % acc_static / gyro_static: Nx3 矩阵分别对应该段各时刻的比力和角速度 % 单位分别约定为 m/s^2 和 rad/s若原始数据是 g 和 deg/s 需先转换 f_b mean(acc_static, 1); % 比力均值静止时约等于 -g 在 b 系的投影 w_b mean(gyro_static, 1); % 地球自转角速度在 b 系的分量均值 g 9.7803; % 简单重力模型高精度时可查当地重力值 wie 7.2921151467e-5; % 地球自转角速度单位 rad/s L deg2rad(lat_deg); % 纬度转弧度北纬为正 g_n [0; 0; -g]; % 东北天 n 系下的重力矢量 w_n [0; wie*cos(L); wie*sin(L)]; % 地球自转角速度在 n 系的分量 % 在 n 系和 b 系分别用两组非共线矢量构造正交基 A_n [ -g_n, cross(-g_n, w_n), cross(cross(-g_n, w_n), -g_n) ]; A_b [ f_b, cross( f_b, w_b), cross(cross( f_b, w_b), f_b) ]; % 同一点的两套基满足 A_b C_n_b * A_n因此体到导航系的矩阵用右除得到 C_b_n A_n / A_b; % 等价于 A_n * inv(A_b)数值稳定性更好 end逻辑说明静止时加速度计输出比力f_b对应导航系的-g_n这里不是g_n陀螺仪输出w_b对应导航系的w_n。cross(-g_n, w_n)构造出与重力、地球自转角速度都不共线的中间矢量再叉乘一次恢复正交性。A_b C_n_b * A_n是坐标变换原始方程两边右乘inv(A_n)得到C_n_b所以C_b_n A_n / A_b。A_n / A_b在 MATLAB 里是矩阵右除比显式求逆更稳。参数说明lat_deg是站点纬度北纬为正。MEMS 级 IMU 在纬度 45° 处能测到约 5.16e-5 rad/s 的地球自转角速度东向分量加速度计和陀螺仪数据至少要平滑 10 秒以上再取均值否则航向初值噪声会很大。如果你的陀螺零偏重复性大于 0.1°/s解析粗对准的航向精度会明显恶化这时候要么延长静态采集时间要么用下一章的精对准靠滤波把方位失准角慢慢逼出来。2.3 初始航向角从哪里读粗对准得到的C_b_n里已经隐含航向角。有 Aerospace Toolbox 可以直接[yaw, pitch, roll] dcm2angle(C_b_n, ZYX);没有的话按姿态转移顺序 Z-Y-X 从矩阵元素提取pitch asin( C_b_n(3,1) ); % 俯仰角单位 rad roll atan2( -C_b_n(3,2), C_b_n(3,3) ); % 横滚角 yaw atan2( C_b_n(2,1), C_b_n(1,1) ); % 航向角北偏东为正 yaw_deg wrapTo180(rad2deg(yaw)); % 转成 -180~180 度区间这个提取公式把航向角定义为前向轴在水平面的投影与北向的夹角正好符合我们选定的右前上 b 系和东北天 n 系。wrapTo180需要 Mapping Toolbox没有的话用mod(yaw_deg 180, 360) - 180效果一致。静止支架上俯仰角通常不超过 ±30°asin在这个区间是单值的不需要额外分支处理。3. 精对准用卡尔曼滤波把失准角和传感器零偏估出来粗对准把姿态收敛到度级但航向角由于地球自转角速度的水平分量比陀螺噪声小得多粗对准精度一般只有 1°~10°看 IMU 等级。精对准的任务是在静止状态下用速度误差作为量测把残余失准角和陀螺、加速度计零偏的估计值反馈到姿态阵里航向角误差收到角分级。静态精对准的通用做法是卡尔曼滤波不是调阈值。3.1 状态向量与可观测性静态精对准取 12 个状态3 个速度误差δv_e, δv_n, δv_u、3 个失准角φ_e, φ_n, φ_u、3 个陀螺常值漂移ε_x, ε_y, ε_z、3 个加速度计零偏∇_x, ∇_y, ∇_z。静基座对准的可观测性分析告诉我们水平失准角由加速度计水平零偏耦合可快速估计方位失准角只能靠陀螺仪感知地球自转分量收敛速度由陀螺漂移决定通常要几分钟到十几分钟。这一点在 exp2 这类实验包里体现为“精对准时间至少设 300 秒”设 30 秒就停止会看到一个没收敛的偏航角。3.2 状态方程和离散化实现下面给一个 12 状态静态精对准的 MATLAB 实现骨架。它把惯导速度误差方程和姿态误差方程一起做时间更新用零速作为量测。att_update和vel_update在下一章展开这里先按公式直接构造矩阵% 12 态静基座精对准滤波器状态按以下顺序排列 % x [δve; δvn; δvu; φe; φn; φu; εx; εy; εz; ∇x; ∇y; ∇z] % 量测 z [ve; vn; vu] - 0即估算速度与零速的差 dt 0.01; % 典型 IMU 采样周期 100 Hz F zeros(12,12); % 速度误差方程 δv_dot -φ × f^n C_b_n * ∇静基座下 f^n ≈ [0; 0; -g] F(1,5) g; F(1,10:12) C_b_n(1,:); % 东向速度误差方程 F(2,4) -g; F(2,10:12) C_b_n(2,:); % 北向速度误差方程 F(3,10:12) C_b_n(3,:); % 天向速度误差方程 % 姿态误差方程 φ_dot -C_b_n * ε忽略静基座下的位置耦合项 F(4,7:9) -C_b_n(1,:); F(5,7:9) -C_b_n(2,:); F(6,7:9) -C_b_n(3,:); % 陀螺和加速度计零偏建模为慢变常值对应行全零实际可加一阶马尔可夫项 Phi eye(12) F * dt; % 一阶离散化状态转移矩阵 % 量测矩阵速度误差直接可测 H zeros(3,12); H(1,1) 1; H(2,2) 1; H(3,3) 1;逻辑说明速度误差方程中-φ × f^n展开后得到东向速度误差里出现g * φ_n、北向速度误差里出现-g * φ_e这就是加速度计测量水平失准角的基本机制姿态误差方程里-C_b_n * ε把陀螺漂移耦合进失准角。eye(12) F * dt是离散化的一阶近似采样周期在 0.01 秒量级时精度足够如果采样率低于 50 Hz建议改用矩阵指数expm(F * dt)。量测矩阵 H 中速度误差状态是直接可测的因为静止时理想速度为 0。主滤波循环中每步惯导解算后执行一次量测更新并用误差状态修正姿态阵和速度% 精对准主循环省略 IMU 数据读取和上一章的速度/姿态更新部分 P blkdiag(0.01*eye(3), 0.01*eye(3), 1e-6*eye(3), 1e-4*eye(3)); % 初始协方差 Q blkdiag(1e-6*eye(3), 1e-8*eye(3), 1e-12*eye(3), 1e-8*eye(3)); % 过程噪声 R 0.01 * eye(3); % 零速量测噪声单位 (m/s)^2 for k 1:Nsamples % ... 读取 acc_k, gyro_k调用第 4 章的 att_update / vel_update z v_est; % 实际速度为 0所以量测就是估算速度本身 P_pred Phi * P * Phi Q; K P_pred * H / (H * P_pred * H R); dx K * z; P (eye(12) - K * H) * P_pred; % 反馈修正失准角、速度误差、零偏 C_b_n (eye(3) - skew(dx(4:6))) * C_b_n; % 小角度修正严格应用四元数 v_est v_est - dx(1:3); gyro_bias gyro_bias dx(7:9); acc_bias acc_bias dx(10:12); % 修正后误差状态清零下一周期重新估计 dx zeros(12,1); end逻辑说明零速量测的物理含义是“惯性解算出来的速度应该为零”任何与零的偏离都来自姿态误差和传感器误差滤波算法把这些偏差反推成可修正的状态量。反馈修正用skew(dx(4:6))构造失准角对应的反对称矩阵在工程实现中更规范的做法是把姿态阵转回四元数用误差四元数做一次乘法修正避免长时间小角度累加失去正交性。代码里P的初值、过程噪声Q和量测噪声R的取值直接影响收敛速度下面给一组常见经验值。精对准参数表按 IMU 等级可缩放状态初始协方差 P过程噪声 Q说明速度误差(0.1 m/s)²(1e-3 m/s²)²Q 对应比力积分误差失准角(0.01 rad)² / (0.05 rad)²(1e-4 rad)²方位失准角初值更松陀螺零偏(1e-6 rad/s)²(1e-9 rad/s²)²对应零偏稳定性加计零偏(1e-4 m/s²)²(1e-6 m/s³)²对应零偏重复性3.3 精对准过程中的航向角收敛判断不能只看姿态角振荡下去就认为滤波发散。要看速度残差和状态协方差。经验判断标准水平失准角估计值 5 分钟内趋稳方位角需要 10 分钟以上才能看得出单调收敛。若航向估计前后波动仍超过 0.1°多半是量测 R 偏小、陀螺零偏初值偏大或是采集数据里混入了震动引起的角运动。4. 惯导解算从精对准姿态出发做速度、位置和航向角连续输出惯导解算就是精对准滤波器中att_update和vel_update这两个函数落到实处。姿态更新是陀螺输出的积分速度更新是把加速度计比力变换到导航系并加重力、哥氏补偿位置更新在速度基础上积分。整个链路的输出最终要落到航向角上。4.1 姿态更新与航向角的一体化设计姿态阵用四元数维护避免欧拉角的奇异性每步从陀螺角增量构造旋转四元数。写一个最小实现的 MATLAB 函数function q att_update(q, gyro, dt) % 姿态更新一阶毕卡解算角增量足够小时可用 % 高动态或转台场景建议改用 Bortz 方程构造等效旋转矢量 phi gyro * dt; % 角增量单位 rad norm_phi norm(phi); if norm_phi 1e-12 delta_q [1; 0; 0; 0]; else half norm_phi / 2; delta_q [cos(half); phi / norm_phi * sin(half)]; end q quatmul(q, delta_q); % 左乘b 系到 n 系的旋转增量 q q / norm(q); % 强制单位化防止数值漂移 end这个函数假设gyro已经是 rad/s且已减去零偏。量纲是最容易出问题的地方模块给的往往是 deg/s常值误差 0.01°/s 积分 100 秒就是 1° 航向漂移精对准做得再好也白费。常见做法是在数据读入后立刻统一量纲gyro_rad deg2rad(gyro_deg); % 先转 rad/s gyro_rad gyro_rad - gyro_bias; % 再减零偏零偏由静态段计算 acc_ms2 acc_g * 9.7803; % 加速度计同理g 转 m/s^2速度更新中比力方程离散化的常用形式function [v, pos] vel_update(v, pos, C_b_n, acc, g_n, dt) % 速度/位置更新比力转到导航系加重力补偿 f_n C_b_n * acc; % 比力变换到导航系 v v (f_n g_n) * dt; % 简化形式忽略哥氏项 pos(3) pos(3) v(3) * dt; % 高度积分长时间导航需补地球曲率 end这里的简化处理对静态对准和低速运动足够但长航时或高动态时必须补全(2*ω_ie ω_en) × v项。位置积分在东向和北向要做纬度余弦换算直接按平面坐标积分会在长时间导航后明显发散。4.2 四元数到航向角的提取姿态用四元数维护时输出航向角建议先转成姿态阵再提取。四元数转欧拉角有万向锁分支转成矩阵后用atan2反而最稳C quat2dcm(q); % 四元数转姿态矩阵q 为 [w; x; y; z] 约定的情况下直接用 psi atan2(C(2,1), C(1,1)); % 航向角与第 2 章提取公式保持一致 theta asin(C(3,1)); % 俯仰角 gamma atan2(-C(3,2), C(3,3)); % 横滚角连续运动中航向角输出会遇到 ±180° 跳变即航向从 179° 变到 -179° 的视觉跳变。离线处理用unwrap很方便yaw_unwrap unwrap(cumsum([0; diff(yaw_rad)])); % yaw_rad 为逐点弧度序列 yaw_deg_out rad2deg(yaw_unwrap); % 连续无跳变航向角输出若是实时系统unwrap只适用于后处理要在每次更新时手动维护整数补偿量当yaw_now - yaw_last pi则补偿量减 1小于-pi则补偿量加 1把补出来的2π加回去。4.3 初始对准和航向角输出的数据流整个 exp2 系列工程包的调用顺序比任何叙述都直观阶段输入输出常用时长静止采集IMU 原始数据零偏均值、加速度均值1~5 min粗对准静止段均值C_b_n 初值1 s精对准整段数据迭代修正后 C_b_n、零偏估计5~15 min导航解算IMU C_b_n速度、位置、航向角实时更新这里有个很实用的建议不要把粗对准的C_b_n直接送进导航解算再用精对准结果“覆盖”回去。正确做法是把精对准估计的失准角、陀螺零偏、加速度计零偏在进入解算前一次性修正之后不再回写。这样后续航向角是连续平稳的。5. 对准质量验证与常见误差源排查初始对准做得好不好航向角最可靠的验收办法是转台或已知方位角基准。没有转台可以用静止段的“回读策略”对准结束后保持 IMU 不动再导航解算 10 分钟输出航向角应当保持在 0.1°~0.3° 以内MEMS 器件通常 0.5°~1°。如果航向角继续单调漂移基本可以断定精对准没把陀螺零偏估出来而不是姿态更新算法的问题。排查时按下面的顺序看数据检查原始数据时间戳有无丢帧加速度和陀螺是否按同一采样率对齐用静态段前 30 秒均值离线算陀螺零偏精对准状态初值直接用该均值而不是从零开始重新估计看速度残差序列ve/vn若残差呈锯齿状且均值明显非零说明加速度计零偏补偿没进回路把滤波协方差 P 的对角元素画出来方位失准角方差不下降时检查norm(w_b)的量级。如果只有 1e-5 rad/s 而你的 IMU 零偏稳定性是 1e-3 rad/s精对准物理上不可行这时要加转位或多位置对准。最后给一个不用转台的验证技巧采集两组静止数据间距约几分钟但两次之间把 IMU 绕竖直轴转一个已知角度比如正好 90°。分别做粗对准并记录航向角差若两次输出的航向差落在 90°±1° 以内说明整条解算链路的方向约定、叉乘顺序、航向提取公式都没有整体符号错误。这个技巧不需要任何外部基准设备却能快速排除掉“负号或转置导致航向镜像”这类隐蔽 bug。数据里包含几个静态 IMU 数据段的 exp2 工程用这个方法十分钟就能完成自检比对着公式调半天参数高效得多。本文还有配套的精品资源点击获取