基于误差四元数的单兵导航系统算法*

于 飞，白红美，叶 攀，高 伟，，赵 博

(1.哈尔滨工程大学自动化学院，黑龙江哈尔滨150001;2.哈尔滨工业大学鞍山研究院，辽宁 鞍山114000)

0 引言

单兵导航系统的一大难题在于如何获得准确的航向信息，为解决航向估计问题，减小单兵定位误差，文献［1］利用地图匹配辅助微型惯性测量单元(MIMU)的算法对单兵进行定位。此算法需针对已知环境，且计算量较大。文献［2］采用启发式偏移消除算法对单兵姿态进行修正，但要求单兵处于直行运动状态。

在磁干扰较弱的环境，考虑到MEMS陀螺和磁力计互补的性质，本文提出基于误差四元数的多传感器信息融合算法。通过磁力计标定技术补偿外界磁干扰，并以MEMS加速度计和磁力计输出为姿态基准，利用卡尔曼滤波器对误差四元数和陀螺漂移进行估计，以减小单兵定位误差。

1 磁力计标定

每次使用前利用标定实验对磁力计进行标定可有效补偿外界磁干扰的影响［3］。标定前后的磁场强度如图1。

图1 标定前后的磁场强度Fig 1 Magnetic field intensity before and after calibration

由图1可以看出，标定前后的磁力计输出存在明显差距，通过标定能够有效补偿外界磁场的影响。

2 多传感器数据融合算法

基于误差四元数的数据融合算法流程图如图2。

图2 数据融合算法流程图Fig 2 Flow chart of data fusion algorithm

首先利用MEMS陀螺获得更新四元数，磁力计和加速度计获得姿态基准;然后以误差四元数和速度误差为观测量实现滤波更新;最后利用估计结果修正导航参数，得到修正后输出。

2.1 状态模型分析

以误差四元数和陀螺漂移为状态向量

其中，δqi(i=1，2，3，4)为四元数元素，εj(j=x，y，z)为陀螺漂移。

非线性系统模型为

其中，F为状态转移矩阵，G为噪声驱动矩阵，v为系统噪声。

定义误差向量δx为

其中，x(t)为滤波估计值，xr(t)为向量的真实值。在真值处进行一阶泰勒展开［4］，可得到关于误差向量的模型为

四元数微分方程为

由式(4)和式(5)得到误差四元数的微分方程

对陀螺漂移进行一阶高斯马尔科夫建模

其中，εj为陀螺漂移，βj为相关时间的倒数，σ2j为陀螺信号的协方差，v为零均值高斯白噪声。

由式(6)和式(7)得到系统状态模型

其中，Fw和Fb分别为与w和βj相关的矩阵

2.2 量测模型分析

量测更新过程由MEMS加速度计、陀螺和磁力计共同完成。姿态基准测量由MEMS加速度计、磁力计获得。陀螺主要用于滤波更新阶段。由陀螺输出可得到旋转四元数［5］q(w)

其中，k为某一时刻，Δt为采样间隔。

在磁干扰较弱的地方，可由标定后的磁力计输出得到航向值［6］。将载体系的磁力计测量值Bbk投影到导航系

其中，ψ为航向角，Md为磁偏角。

加速度计主要用于获得水平姿态角［7］。利用三轴MEMS加速度输出计算得到横摇角θ和纵摇角γ

在得到欧拉角后，可根据式(15)求解捷联矩阵，再利用捷联矩阵和四元数的关系式(16)得到四元数q(ε)。

以q(w)与q(ε)之差为观测量，实现量测更新过程。量测模型为

3 场地实验

采用荷兰Xsens公司的MTi—G型IMU进行实验。场地为哈尔滨工程大学篮球场。首先将MIMU安装在鞋上，并通过USB与PC连接;然后对磁力计进行标定，并采集静态数据以获得导航参数的初值;最后选取行走轨迹，行走时间约15 min，路程约80 m。利用谷歌地图可得到运动轨迹如图3所示。其中，五角星标记的曲线代表运动轨迹，三角代表开始点。

图3 运动轨迹Fig 3 Trajectory

磁力计输出航向和滤波后的航向对比如图4。

图4 航向对比图Fig 4 Heading comparison diagram

由图4可以看出:磁力计输出航向与滤波后的航向存在较大差距。在130～160 s之间磁力计输出较为准确，由于外界磁干扰的影响，其他时间的磁力计航向波动较大。滤波后的航向更能够反映单兵真实运动轨迹。

基于误差四元数的数据融合算法得到的单兵运动轨迹如图5。

图5 单兵运动轨迹Fig 5 Personal trajectory

由图5和图3对比可以看出:图5中的运动轨迹与实验轨迹相符，此算法能够完成单兵定位功能。运动过程中的单兵位置误差如表1所示。

表1 单兵位置误差Tab 1 Personal location error

由表1可以看出:运动过程中的最大误差为2.194 m，约占总步行距离的2.74%;平均误差为1.269 m，约占总距离的1.58%;返回起始点时的误差约为2.184 m，约占总距离的2.73%，故其全局误差小于总步行距离的3%。

4 结论

本文所提的基于误差四元数的数据融合算法充分利用了MEMS传感器的信息，实验证明该算法能够对单兵进行准确定位。在无外界信号下，当步行距离为80 m时，该算法的全局误差不超过总距离的3%，可以满足单兵的自主定位要求。

［1］Aggarwal P，Thomas D，Ojeda L，et al.Map matching and heuristic elimination of gyro drift for personal navigation systems in GPS-denied conditions［J］.Measurement Science and Technology，2011，22(2):025205.

［2］Borenstein J，Ojeda L.Heuristic drift elimination for personnel tracking systems［J］.Journal of Navigation，2010，63(4):591-606.

［3］Renaudin V，Afzal M H，Lachapelle G.New method for magnetometers-based orientation estimation［C］∥Position Location and Navigation Symposium(PLANS)，2010 IEEE/ION，IEEE，2010:348-356.

［4］孙红星，李德仁.非线性系统中卡尔曼滤波的一种新线性化方法［J］.武汉大学学报:信息科学版，2004，29(4):346-348.

［5］田亚军，周 刚，胡军照，等.捷联惯导系统姿态算法实现及工程应用［J］.四川兵工学报，2011，32(1):10-12.

［6］Afzal M H，Renaudin V，Lachapelle G.Magnetic field-based heading estimation for pedestrian navigation environments［C］∥2011 International Conference on Indoor Positioning and Indoor Navigation(IPIN)，IEEE，2011:1-10.

［7］Luinge H J，Veltink P H.Measuring orientation of human body segments sing miniature gyroscopes and accelerometers［J］.Medical and Biological Engineering and Computing，2005，43(2):273-282.