INS与GPS组合导航技术：EKF算法实现与优化

1. 项目概述：INS与卫星组合导航技术解析

在导航定位领域，惯性导航系统(INS)和全球卫星导航系统(GNSS)是两种最常用的定位技术。INS通过加速度计和陀螺仪测量载体运动，具有自主性强、短期精度高的特点，但存在误差累积问题；GNSS提供绝对位置参考，长期稳定性好，但易受遮挡影响且更新频率低。将两者优势互补的组合导航技术，已成为无人机、自动驾驶等领域的标配方案。

本项目基于MATLAB平台，实现了INS(MPU6050)与GPS(ATGM332D)的松耦合组合导航系统，核心算法采用扩展卡尔曼滤波(EKF)和标准卡尔曼滤波(KF)进行对比验证。通过融合惯性测量单元的高频动态数据和GPS的低频绝对位置信息，有效抑制了INS的误差累积，提升了导航参数的估计精度。实测数据显示，经EKF融合后的位置误差较纯INS积分结果降低约60%，速度误差减少45%。

2. 硬件系统与数据预处理

2.1 传感器选型与数据特性

本系统采用MPU6050作为惯性测量单元，ATGM332D作为GPS接收模块，两者通过串口输出数据到上位机。硬件配置及参数如下表所示：

实际应用中需注意：MPU6050需预热5分钟以上使零偏稳定，ATGM332D在开阔天空视图下才能达到标称精度。城市峡谷环境中，GPS误差可能增大至10米以上。

2.2 数据预处理流程

原始数据需经过以下关键处理步骤：

1. 时间对齐：由于IMU(100Hz)和GPS(10Hz)采样率不同，采用线性插值法将GPS数据上采样到100Hz，确保时间戳严格同步。核心MATLAB代码片段：

matlab复制gps_time = linspace(0, duration, length(gpsx));
imu_time = linspace(0, duration, length(accx));
gpsx_interp = interp1(gps_time, gpsx, imu_time, 'linear', 'extrap');

2. 坐标系统一：
   • 将GPS经纬度(gpsx/y)从WGS84坐标系转换为局部ENU(东-北-天)坐标系，以第一个GPS点为原点
   • IMU输出的加速度需通过姿态矩阵R_b从机体坐标系转换到导航坐标系：

matlab复制R_b = [cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+cos(yaw)*sin(pitch)*cos(roll);
       sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(yaw)*sin(pitch)*sin(roll), -cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)*cos(roll);
       -sin(pitch), cos(pitch)*sin(roll), cos(pitch)*cos(roll)];
acc_nav = R_b * [accx; accy; accz];

3. 异常值剔除：采用滑动窗口法(窗口大小=10)检测GPS速度突变，当相邻点速度差超过3σ时视为异常值，用前一点数据替代。

3. 卡尔曼滤波算法实现

3.1 标准卡尔曼滤波(KF)设计

KF采用6维状态向量和4维观测向量，适用于线性运动模型：

状态方程：

code复制x_k = F * x_{k-1} + w_k  
z_k = H * x_k + v_k

其中：

• 状态向量x = [pos_x, pos_y, pos_z, vel_x, vel_y, vel_z]^T
• 状态转移矩阵F包含位置-速度的积分关系：

matlab复制F = [eye(3), dt*eye(3);
     zeros(3), eye(3)];

• 观测矩阵H选取位置和速度作为观测量：

matlab复制H = [eye(3), zeros(3);
     zeros(3), eye(3)];

噪声协方差设置：

• 过程噪声Q主要反映IMU误差，根据Allan方差分析结果设为：

matlab复制Q = diag([0.01^2, 0.01^2, 0.01^2, 0.1^2, 0.1^2, 0.1^2]);

• 观测噪声R根据GPS性能指标确定：

matlab复制R = diag([6.25, 6.25, 12.25, 0.01, 0.01, 0.04]);

3.2 扩展卡尔曼滤波(EKF)设计

EKF针对INS/GPS组合导航的非线性特性，采用15维状态向量：

状态向量：

code复制x = [δpos, δvel, δθ, δω, δacc]^T

包含位置误差、速度误差、姿态角误差、角速度误差和加速度计零偏

非线性状态方程：

code复制δx_k = f(δx_{k-1}, u_k) + w_k

其中f(·)包含姿态矩阵更新的非线性运算

线性化处理：
在每个时间步计算雅可比矩阵F_k：

matlab复制F_k = eye(15) + ...
      [zeros(3), eye(3), zeros(3,9);
       zeros(3), zeros(3), -R_b*skew(acc), zeros(3), -R_b;
       zeros(3), zeros(3), -skew(omega), -eye(3), zeros(3);
       zeros(6,15)];

观测更新：
观测残差计算INS与GPS的位置、速度差：

matlab复制z = [pos_ins - pos_gps; vel_ins - vel_gps];

4. 算法实现与性能优化

4.1 MATLAB实现架构

项目代码采用模块化设计，主要包含以下功能模块：

1. 数据加载模块：读取并解析ceshi.txt中的原始数据
2. 预处理模块：完成时间同步、坐标转换和异常值处理
3. KF/EKF核心模块：实现两种滤波算法
4. 可视化模块：生成位移、速度估计曲线和误差分析图

关键实现技巧：

• 使用MATLAB的tic/toc函数对滤波循环进行耗时分析
• 预分配结果存储矩阵（如pos_ekf = zeros(3,N)）提升运行效率
• 将EKF的雅可比矩阵计算封装为独立函数便于维护

4.2 实时性优化策略

针对嵌入式平台部署需求，可采用以下优化方法：

1. 矩阵运算简化：

   • 利用对称性减少协方差矩阵计算量（如P = (I-K*H)*P只需计算下三角）
   • 固定维数矩阵预计算其逆矩阵

2. 降维处理：

   • 当高度变化较小时，可简化为二维平面模型（状态维数从15降至8）
   • 对于短时应用，忽略角速度和加速度零偏估计（状态维数降至9）

3. 代码生成：
   使用MATLAB Coder将算法转换为C代码，在STM32等MCU上实时运行：

matlab复制cfg = coder.config('lib');
codegen('ekf_filter.m', '-config', cfg);

5. 实验结果与分析

5.1 静态测试结果

在静止状态下(IMU固定放置)，分析算法对传感器零偏的抑制效果：

5.2 动态测试结果

车载实验轨迹长约2km，包含直线、转弯和加减速工况：

1. 位置估计对比：

   • INS积分产生明显漂移，终点误差达35.2m
   • KF将误差降至8.7m
   • EKF进一步将误差控制在4.3m

2. 速度估计对比：

   • 急加速阶段，EKF的速度估计延迟比KF减少30%
   • 横向速度估计误差从0.25m/s(KF)降至0.12m/s(EKF)

3. 收敛特性：

   • KF在GPS信号丢失30s后误差开始显著增长
   • EKF可维持60s以上的可靠导航

5.3 典型问题排查

在实际调试中遇到的典型问题及解决方案：

1. 滤波器发散：

   • 现象：估计误差随时间不断增大
   • 原因：过程噪声Q设置过小，无法覆盖实际IMU误差
   • 解决：通过Allan方差分析重新标定Q矩阵

2. 更新异常：

   • 现象：GPS更新后状态估计出现跳变
   • 原因：时间未严格同步导致观测残差计算错误
   • 解决：增加插值同步算法，确保时间对齐

3. 数值不稳定：

   • 现象：协方差矩阵失去正定性
   • 原因：浮点运算累积误差
   • 解决：采用Joseph形式协方差更新公式：

matlab复制P = (I-K*H)*P*(I-K*H)' + K*R*K';

6. 工程应用建议

6.1 参数调优方法

1. Q矩阵标定：
   • 静态采集IMU数据2小时
   • 计算Allan方差确定各噪声参数
   • 示例代码：

matlab复制[tau, adev] = allanvar(gyro_data, 'octave', fs);
Q_gyro = adev(find(tau==1,1))^2;

2. R矩阵确定：

   • 动态测试中记录GPS位置标准差σ_p和速度标准差σ_v
   • 设置R = diag([σ_p^2, σ_v^2])

3. 自适应滤波：
   根据GPS信号质量动态调整R矩阵：

matlab复制if gps_hdop > 2.0
    R(1:3,1:3) = R(1:3,1:3) * (gps_hdop/1.0)^2;
end

6.2 不同场景下的算法选择

6.3 扩展改进方向

1. 多传感器融合：

   • 增加磁力计补偿航向角漂移
   • 融合轮速计抑制水平面速度误差

2. 算法升级：

   • 改用无迹卡尔曼滤波(UKF)避免线性化误差
   • 实现因子图优化进行全局轨迹优化

3. 深度学习辅助：

   • 使用LSTM网络建模IMU误差特性
   • CNN识别GPS异常测量值

在实际部署中发现，对于消费级IMU，EKF在计算资源和精度的平衡上表现最佳。一个实用技巧是在GPS信号良好时存储IMU误差特性，当GPS丢失时基于历史数据进行补偿，可延长自主导航时间约40%。