1. 项目概述
在导航定位领域,如何将惯性导航系统(INS)与卫星导航系统(GNSS)的优势互补一直是个经典课题。INS具有短期精度高、自主性强、不受外界干扰的特点,但存在误差累积问题;GNSS虽然长期稳定性好,却容易受遮挡影响且更新频率较低。我最近在Matlab平台上实现了一套融合这两种导航方式的组合算法,核心采用了卡尔曼滤波(KF)和误差状态卡尔曼滤波(ESKF)的双层架构,实测效果相当不错。
这个方案特别适合无人机、自动驾驶车辆等需要高精度连续定位的场景。当卫星信号良好时,系统以GNSS为主进行校准;当进入隧道、城市峡谷等GNSS拒止环境时,则自动切换为INS主导模式,通过ESKF算法有效抑制惯性器件的误差漂移。下面我就从原理到实现细节,完整分享这套组合导航系统的开发过程。
2. 核心算法解析
2.1 卡尔曼滤波基础框架
卡尔曼滤波的本质是通过"预测-更新"两个步骤实现状态最优估计。对于我们的导航系统,状态向量通常包含位置、速度、姿态角以及惯性器件的零偏误差:
matlab复制% 状态向量定义示例
X = [p_x, p_y, p_z, v_x, v_y, v_z, roll, pitch, yaw, b_ax, b_ay, b_az, b_gx, b_gy, b_gz]';
预测阶段使用惯性测量单元(IMU)的加速度计和陀螺仪数据进行机械编排:
matlab复制% 简化的IMU机械编排代码片段
function X_pred = imuMechanical(X_prev, acc, gyro, dt)
% 姿态更新
C_nb = euler2dcm(X_prev(7:9)); % 姿态矩阵更新
omega = gyro - X_prev(13:15); % 陀螺零偏补偿
X_pred(7:9) = updateAttitude(X_prev(7:9), omega, dt);
% 速度更新
acc_body = acc - X_prev(10:12); % 加速度计零偏补偿
acc_nav = C_nb * acc_body - [0; 0; 9.8]; % 转换到导航系并去除重力
X_pred(4:6) = X_prev(4:6) + acc_nav * dt;
% 位置更新
X_pred(1:3) = X_prev(1:3) + X_prev(4:6) * dt + 0.5 * acc_nav * dt^2;
end
更新阶段则利用GNSS测量的位置、速度信息修正预测值。关键在于设计合理的观测矩阵H和噪声协方差矩阵R:
matlab复制H = [eye(6), zeros(6,9)]; % 仅观测位置和速度
R = diag([0.5, 0.5, 0.8, 0.3, 0.3, 0.3].^2); % GNSS测量噪声
2.2 ESKF误差状态建模
传统KF直接估计系统状态,而ESKF则专注于估计状态误差,这种设计带来了三个显著优势:
- 误差量通常是小量,线性化近似更准确
- 姿态误差用三维向量表示,避免四元数过参数化问题
- 零偏等误差项可直接建模为随机游走过程
误差状态动力学模型可表示为:
code复制δẊ = F·δX + G·w
其中F是误差状态转移矩阵,包含IMU误差传播关系;G是噪声驱动矩阵;w代表系统噪声。在Matlab中实现时需特别注意姿态误差的更新方式:
matlab复制% ESKF姿态误差更新示例
delta_theta = gyro - X_imu(10:12) - beta_g; % 角速度误差
F(7:9,7:9) = -skew(omega_hat); % 姿态误差转移项
F(7:9,13:15) = -eye(3); % 陀螺零偏影响
2.3 松耦合组合架构
我们采用松耦合方案,INS和GNSS各自独立解算后再进行数据融合。这种架构的优点是:
- 模块化程度高,便于单独调试
- 对GNSS输出频率要求低
- 兼容不同型号的GNSS接收机
具体实现流程如下:
- IMU数据通过机械编排实时更新INS状态
- GNSS数据到达时,计算位置/速度差值作为观测值
- ESKF处理观测值并输出误差估计
- 将误差补偿到INS状态后重置ESKF
mermaid复制graph TD
A[IMU数据] --> B(INS机械编排)
C[GNSS数据] --> D{数据同步}
B --> E[ESKF预测]
D --> F[ESKF更新]
E --> F
F --> G[误差补偿]
G --> B
3. Matlab实现细节
3.1 传感器数据处理
实际项目中需要处理IMU和GNSS的异步数据问题。我设计了一个数据缓冲区,采用插值方法实现时间对齐:
matlab复制% 数据同步处理函数
function [imu_sync, gnss_sync] = syncData(imu_buffer, gnss_buffer)
gnss_time = gnss_buffer(:,1);
imu_time = imu_buffer(:,1);
% 线性插值IMU数据到GNSS时间戳
for k = 1:length(gnss_time)
idx = find(imu_time <= gnss_time(k), 1, 'last');
alpha = (gnss_time(k) - imu_time(idx)) / (imu_time(idx+1)-imu_time(idx));
imu_sync(k,:) = (1-alpha)*imu_buffer(idx,2:end) + alpha*imu_buffer(idx+1,2:end);
gnss_sync(k,:) = gnss_buffer(k,2:end);
end
end
3.2 卡尔曼滤波核心代码
完整KF实现包含以下关键函数:
matlab复制function [X, P] = kalmanFilter(X_prev, P_prev, Z, Q, R, dt)
% 预测步骤
[F, G] = getJacobians(X_prev, dt);
X_pred = stateTransition(X_prev, dt);
P_pred = F * P_prev * F' + G * Q * G';
% 更新步骤
H = getObservationMatrix();
K = P_pred * H' / (H * P_pred * H' + R);
X = X_pred + K * (Z - H * X_pred);
P = (eye(size(P_pred)) - K * H) * P_pred;
end
对于ESKF,需要特别注意误差状态注入和重置的逻辑:
matlab复制function X_nom = injectError(X_nom, delta_X)
% 位置速度直接相加
X_nom(1:6) = X_nom(1:6) + delta_X(1:6);
% 姿态误差通过旋转矩阵组合
delta_theta = delta_X(7:9);
R = eye(3) - skew(delta_theta);
X_nom(7:9) = dcm2euler(R * euler2dcm(X_nom(7:9)));
% 零偏更新
X_nom(10:15) = X_nom(10:15) + delta_X(10:15);
end
3.3 可视化与性能评估
设计了一套直观的可视化界面展示导航结果:
matlab复制figure('Name','导航结果对比');
subplot(3,1,1);
plot(gt_time, gt_pos(:,1), 'k-', ins_time, ins_pos(:,1), 'r--', gnss_time, gnss_pos(:,1), 'b:');
legend('真实值','INS估计','GNSS测量');
title('东向位置'); xlabel('时间(s)'); ylabel('米');
subplot(3,1,2);
plot(gt_time, gt_vel(:,2), 'k-', ins_time, ins_vel(:,2), 'r--');
legend('真实值','组合导航');
title('北向速度'); xlabel('时间(s)'); ylabel('米/秒');
subplot(3,1,3);
plot(gt_time, gt_eul(:,3)*180/pi, 'k-', ins_time, ins_eul(:,3)*180/pi, 'r--');
legend('真实值','组合导航');
title('航向角'); xlabel('时间(s)'); ylabel('度');
评估指标通常包括:
- 位置误差RMS值
- 速度误差最大值
- 姿态角收敛时间
- GNSS拒止期间的误差增长斜率
4. 工程实践中的关键问题
4.1 IMU标定与噪声参数辨识
在实际应用中,IMU的噪声特性直接影响滤波效果。我总结的标定流程如下:
- 静态采集2小时数据计算零偏稳定性
- 三轴转台实验标定刻度因数误差
- 振动测试确定随机游走系数
- Allan方差分析确定噪声参数
matlab复制% Allan方差计算示例
function [tau, adev] = allanVariance(omega, fs)
maxM = floor(length(omega)/10);
tau = zeros(maxM,1);
adev = zeros(maxM,1);
for m = 1:maxM
omega_m = mean(reshape(omega(1:floor(length(omega)/m)*m), m, []), 1);
tau(m) = m/fs;
adev(m) = sqrt(0.5*mean(diff(omega_m).^2));
end
end
4.2 自适应滤波调参
固定噪声参数难以应对动态环境变化,我实现了基于新息序列的自适应调节:
matlab复制function R = adaptR(innov, R0, window_size)
persistent buffer;
if isempty(buffer)
buffer = repmat(innov, window_size, 1);
else
buffer = [buffer(2:end,:); innov];
end
R = diag(var(buffer) + diag(R0));
end
4.3 GNSS质量检测与抗差滤波
针对GNSS多路径效应等问题,设计了质量检测机制:
- 信噪比(SNR)阈值检查
- 伪距一致性检验
- 多普勒辅助验证
- 新息检验(Innovation Check)
matlab复制function isGood = checkGNSSQuality(gnss, innov, P)
% 新息检验
mahalanobis = sqrt(innov' / (H*P*H' + R) * innov);
isGood = gnss.snr > 35 && ...
gnss.hdop < 1.5 && ...
mahalanobis < 3;
end
5. 实测效果与优化方向
在无人机平台上实测表明,该组合导航系统在GNSS可用时水平定位误差<0.5米,GNSS中断60秒后误差增长约1.5米/分钟。相比纯INS导航,精度提升了一个数量级。
未来优化方向包括:
- 引入视觉/激光雷达多源融合
- 开发基于神经网络的误差补偿模型
- 实现嵌入式平台的代码移植
- 增加抗干扰算法应对欺骗攻击
整套Matlab代码已模块化封装,包含:
IMU_Simulator.m:IMU数据生成器GNSS_Simulator.m:GNSS信号模拟INS_Mechanical.m:机械编排算法ESKF_Filter.m:误差状态卡尔曼滤波Nav_Fusion.m:主融合算法Plot_Utility.m:可视化工具
对于想复现的读者,建议从仿真数据开始,逐步过渡到真实传感器数据。特别注意IMU安装偏差补偿和时延校准这两个最容易忽视的环节。
