0 引言

    随着以MEMS(Micro-electromechanical Systems)传感器为代表的微型化惯性器件的发展，基于捷联式惯导原理和MEMS传感器的惯性定位技术也日益受到重视，特别是在难以接收卫星信号的室内、地下、矿井、水下、战场等场合^[1]。针对上述问题往往采用利用电子罗盘对陀螺进行校正的方法，在室内、地下、矿井、水下等过程中磁强计更加容易受到干扰，造成方位更大的偏差。针对磁强计易受干扰和陀螺积分漂移的问题已经有众多融合的算法出现，比如卡尔曼滤波、无迹卡尔曼滤波（UKF）、扩展卡尔曼滤波（EKF）等^[2-4]，这些滤波方法需要建立准确的状态方程和观测方程。还有一种滤波算法是在互补滤波的基础上进行扩展，比如经典互补滤波、基于梯度下降法的互补滤波算法等^[3-6]，但是这种滤波算法适用的精度不高。面对这些问题，本文提出了一种Kalman滤波和互补滤波融合的惯性定位算法，该算法在设计Kalman滤波中，将加速度计和磁强计融合得出的四元数作为观测值，利用陀螺仪得出的四元数作为状态值，通过数据的融合进行滤波，完成四元数的第一次最优估计，针对陀螺漂移问题则利用所设计的互补滤波对陀螺漂移进行补偿，得到校正后的角速度，进而求得校正后不断更新的四元数，然后和第一次完成的最优估计四元数通过第二次Kalman滤波进行估计，进而输出高精度的姿态角。

1 算法总体设计

    Kalman滤波和互补滤波融合的惯性定位算法的总体思路如图1所示。

    首先，将磁强计和加速度计测量的磁场强度和加速度利用高斯牛顿迭代法解算出的四元数送入Kalman滤波中当作观测值，将陀螺仪测量的角速度值利用四阶-库塔法解算出的四元数作为Kalman滤波的状态值进而得到四元数的第一次最优估计值，在Kalman滤波过程中既可以去掉随机噪声，通过电子罗盘对陀螺仪进行校正，又可以避免由于磁场干扰带来的干扰数据。

    其次，利用载体坐标系和地理坐标系之间的转换矩阵，将地理坐标系下的重力和地磁分量转换到载体坐标系下，然后与加速度计和磁强计在载体坐标系下测量得到的加速度和磁场强度做向量积的运算，将二者向量积的和相加经过比例积分低通滤波，可以将电子罗盘测量姿态中的高频抖动信号滤出，然后和陀螺仪测量的角速度进行融合，得到对陀螺仪补偿后的角速度，利用四阶龙格库塔法得到校正后的四元数。

    最后，将互补滤波算法得到的校正四元数作为状态量，将第一次Kalman滤波得到的最优四元数作为观测量进行第二次Kalman滤波，完成四元数的第二次优化，然后通过四元数建立姿态矩阵求得更加精确的姿态角。

2 卡尔曼滤波设计

    卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。建立系统的状态方程和测量方程是卡尔曼滤波的主要工作。本次卡尔曼滤波设计中用四元数的不断更新量作为卡尔曼滤波中的状态量。

    下面建立测量方程，由重力加速度和三轴加速度计

式(4)中a_x、a_y、a_z表示重力加速度的测量值，重力加速度用g表示。

    由地磁场和磁强计测量信息之间的关系可得：

    由式(6)、式(7)并通过高斯牛顿迭代法求得四元数，作为观测量。观测量取为：

    式(8)中观测量方程Z(k)=H(k)X(k)+V(k)，其中H(k)为观测量的增益矩阵，V(k)为观测噪声，方差R(k)可以通过传感器的测量数据获得。由于第一次Kalman滤波和第二次Kalman滤波都是对四元数的最优估计，故上述滤波公式和建立的方程均保持不变。

3 互补滤波算法设计

    由于陀螺仪通过积分得到的方位角存在长时间误差积问题，但其动态响应好，不易受外界干扰，而与之对应的电子罗盘易受外界干扰，但它具有无误差累积的优点。互补滤波就是利用陀螺仪的高频特性和电子罗盘的低频特性进行融合，通过比例环节进行频率特性的调节，如图2所示。

    在图2中，a、m表示载体坐标系下加速度计和磁强计测量得到的加速度和磁场强度；将地理坐标系下的重力分量和地磁分量通过四元数构成的姿态转换矩阵变换为载体坐标系下的重力分量和地磁分量，分别用a_s和m_s表示。在载体坐标系下分别对a、as和m、ms做向量积的运算，得到加速度计和磁强计对陀螺仪的校正误差。其中俯仰角和横滚角的误差设为Δ_θ，γ，航偏角的误差设为即：

    将最后得到的角速度通过四阶龙哥库塔法进行姿态解算，得到最优姿态角。

4 实验验证和结果分析

    本文采用MPU9150模块进行试验验证，通过硬件I²C总线的方式进行数据的读取，传感器按100 Hz/s进行数据更新，以外部中断的方式进行数据采集，然后将采集的数据存放于TF中，通过MATLAB编程进行数据调用。为了验证此惯性定位融合算法的效果，主要在行走和磁场干扰严重的两种模式下进行测试，且两种方式都是在室内完成。图3为携带传感器人员(将传感器绑在脚上)在室内某一位置出发，转一圈回到起始位置，时长约5 min；图4为携带传感器人员在下电梯过程中保持站立姿态的测试。在图3和图4中，单个陀螺仪解算（无滤波算法）用图(a)代表，互补滤波算法用图(b)代表，本算法用图(c)表示。

    通过图3对比始末位置方位偏差的数据结果可知，互补滤波算法虽然比单个陀螺进行行人方位解算更加精确，但是利用互补滤波算法始末位置偏差在8°之内，而本算法在行走5 min左右始末位置偏差在 1.5°之内。由图3(c)证明本算法可以准确提供在大动态下(转圈)的方位。由图4可知，在电梯内磁强计受到强烈干扰的情况下，仅利用互补滤波算法会比单个陀螺进行解算出现更严重的方位偏差问题，而利用Kalman滤波和互补滤波相融合的惯性定位算法避免了此问题，确保姿态精度在2°以内，有效抑制了磁场强度的干扰，确保方位的高精度输出。

5 结束语

    本文针对在导航解算中陀螺仪的积分偏差和磁强计容易受到干扰的问题，提出了一种利用Kalman滤波和互补滤波相融合的惯性定位算法，利用Kalman滤波求最优四元数，利用互补滤波对陀螺仪的角速度进行校正，通过校正的角速度进行四元数的更新，将两次校正后的四元数通过Kalman滤波求最优四元数。通过实验结果可以看出，此方法不仅可以很好地解决陀螺漂移问题，还能有效抑制磁强计受到干扰的问题，能够准确地对方位进行校准，实现姿态角高精度的稳定输出。

参考文献

[1] 杜小菁，翟峻仪.基于MEMS的微型惯性导航技术综述[J].飞航导弹，2014(9)：77-81.

[2] 叶锃锋，冯恩信.基于四元数和卡尔曼滤波的两轮车姿态稳定方法[J].传感技术学报，2012，25(4)：524-528.

[3] MARINA H G，ESPINOSA F，SANTOS C.Adaptive UAV attitude estimation employing unscented Kalman filter，foam and low-cost MEMS sensors[J].Sensors，2012，12(7)：9566.

[4] 张彤，孙玉国.尔曼滤波在MEMS惯性姿态测量中的应用[J].光学仪器，2015，37(1)：28-30.

[5] 林伟捷，黄唯佳，蔡剑卿.基于四元数互补滤波和PID控制算法的两轮自平衡车系统设计[J].软件导刊，2016，15(6)：80-82.

[6] KUNDRA L，EKLER P，CHARAF H.Orientation estimation in modern wearables with visual feature tracking[J].Journal on Multimodal User Interfaces，2015，9(4)：313-322.

作者信息：

张梦影，曾  成，狄素素，王云瑞

(河北工业大学 电子信息工程学院，天津300400)