捷联惯导初始对准与航向角解算：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 等级可缩放：

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 系列工程包的调用顺序比任何叙述都直观：

这里有个很实用的建议：不要把粗对准的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 工程，用这个方法十分钟就能完成自检，比对着公式调半天参数高效得多。

本文还有配套的精品资源，点击获取