基于零速修正辅助MIMU的行人导航算法研究

唐文杰　吕志伟　张伦东　丛佃伟

1　信息工程大学导航与空天目标工程学院，郑州市科学大道62号，450001 2　地理信息工程国家重点实验室,西安市雁塔路中段1号,710054



基于零速修正辅助MIMU的行人导航算法研究

唐文杰1,2吕志伟1张伦东1丛佃伟1,2

1信息工程大学导航与空天目标工程学院，郑州市科学大道62号，450001 2地理信息工程国家重点实验室,西安市雁塔路中段1号,710054

摘要：针对微惯性测量单元(MIMU)误差积累问题，根据行人行走过程中每一次迈步的落地阶段短时间内速度可保持为0的特性，在零速时刻利用系统输出对系统误差进行估计，从而提高导航精度。对比分析基于加速度、角速度、加速度方差和加速度与角速度组合的4种零速检测算法，结果表明，在步态平缓、行走距离较短的情况下，4种算法都能达到较高的精度，其中组合检测法精度最高。

关键词：MIMU；零速修正；零速检测；行人导航

传统导航模式需要接收机等导航设备与卫星之间保持通视。但是，在室内环境下信号易受遮挡，多路径效应增强，导致无法使用[1]。惯性导航具有自主性强且不受外界干扰等特点，能在复杂环境中提供连续的位置和姿态信息。 近年来，基于 MIMU的惯性导航技术发展迅速，相比于常规惯性器件，其具有体积小、成本低、可靠性强、易于集成和数字化等特点[1]，使其应用于行人导航成为可能。但MIMU精度比传统惯性器件低很多，导航积累误差很大，必须采取辅助手段修正误差。零速修正[2]利用行人行走过程中每次着地时短暂的静止状态，将 MIMU的输出作为系统误差的观测量，从而对系统误差进行估计，改善导航精度。本文主要介绍基于零速修正辅助 MIMU的行人导航算法，利用加速度信号和角速度信号的周期特性检测零速状态，在每个零速时刻对惯导误差进行修正，并利用4种零速检测方法进行实验。

1行人导航算法

1.1姿态角初始对准

设i系为惯性坐标系，e系为地球坐标系，b系为载体坐标系，三轴分别指向载体的前、右、下3个方向；n系为导航坐标系，采用当地地理坐标系，三轴方向分别指向北、东和当地铅垂方向。

(1)

(2)

(3)

式中，fx、fy、fz分别为加速度计在载体坐标系三轴上的分量。

航向角在系统方程中不可观[3]，不能直接通过滤波计算出来。因此在静态条件下，可以利用磁强计进行航向估计。设三轴磁强计在载体坐标系下的测量值为Mxb、Myb、Mzb,则三轴磁强计在导航坐标系下的坐标为：

(4)

在忽略磁偏角的情况下，航向角为：

(5)

1.2扩展卡尔曼滤波器

扩展卡尔曼滤波就是对非线性系统中的待估参数Xk进行线性化，得到线性化的模型，然后再利用标准Kalman滤波进行估计。尽管这种滤波是一种次优的状态估计方法[4]，但由于行人导航系统是非线性的，利用扩展卡尔曼滤波能够获得较好的精度。捷联惯导在当地地理坐标系下的系统误差模型为：

(6)

其中,

选取地理坐标系中姿态角误差、速度误差、位移误差、陀螺仪和加速度计常值零偏构成状态矢量，即系统状态x=(δφ,δv,δr,Δb,ηb)T，观测量z=δvobv，δvobv为系统速度误差，则系统方程和观测方程为：

(7)

z=Hx+V

(8)

式中,F是根据误差模型和状态矢量构成的15×15维系统矩阵， H=[03×3I3×303×303×303×3]是3×15维观测矩阵，W为15维系统随机过程噪声序列，V为三维系统随机观测噪声序列。将系统模型线性化后，利用标准Kalman滤波可实现对误差状态的估计。

1.3零速修正算法

零速修正的思想就是在零速情况下将系统输出的速度误差作为观测量对卡尔曼滤波器进行更新，得到状态的估计值，利用估计值对输出的导航信息进行修正，给出系统状态的最优估计[5]。

(9)

利用§1.2中介绍的扩展卡尔曼估计可以得到速度误差δvk和姿态误差δγk、δθk、δψk，则通过零速修正后的姿态为：

(10)

(11)

2零速检测算法

利用ZUPT进行误差修正的前提是能精确检测出行人的零速状态，现有的步态检测算法大多数都是基于加速度和角速度信号进行检测的。本文主要介绍4种零速检测数学模型。

假设fk=[fxkfykfzk]T、ωk=[ωxkωykωzk]T分别为k时刻加速度计和陀螺仪输出。实际上，零速检测的目的就是为了检测一段时间内MIMU处于零速状态的时刻。假设在这段时间内的采样值共有N个，即窗口的大小为N。判断MIMU是否处在零速状态可以看作是一个假设检验问题，即当下式成立时，认为MIMU为零速状态：

(12)

式中，T(fk,ωk)为零速检测的检验统计量，L为阈值。检验统计量形式的不同，会影响零速检测的准确率。这里总结了4种形式[6-8]。

1)加速度检测法：

(13)

2)角速度检测法：

(14)

3)加速度协方差检测法：

(15)

4)组合检测法：

(16)

式中，σf和σω是加速度计和陀螺仪随机噪声方差，‖·‖为二范数。

(17)

随机噪声起到了定权的作用。实际情况中，加速度计的随机误差要大于陀螺仪的观测误差，当σf/σω→时，角速度检测法是组合检测的一种特殊形式。

滑动窗口和阈值的大小与采样率和实际的行走速度有关，采样率越大，行走速度越慢，则滑动窗口取值越大，根据统计量的形式不同，阈值也会有相应的调整。

利用以上4种算法进行3min的行走实验,图1为不同算法检测20s的结果。

实验总共步行50步，利用上述4种算法分别正确检测出47、48、48、50个零速状态，从效率上来看，4种方法的效率差别不大，但组合检测法检测的正确率最高。不同检测方法得到的每次零速时间不同，通过图中柱体的粗细可以明显地区分出来。具体的检测精度可以通过实际行走实验在最终的平面精度中体现出来。

3实验精度分析

实测该数据由OPENSHOE提供,MIMU为ADIS公司生产，设备型号为ADIS16367。实验采样率为250Hz，通过串口输出原始数据，利用Matlab进行数据处理。实验路线是闭合的，行走时间约3min，行走距离为189m，速度约3km/h。起点和终点重合，通过计算起点和终点的坐标差验证实验精度。

利用不同检测方法对采集数据进行处理。设定σf=0.01m·s-2、σω=0.1*π/180rad·s-1,窗口大小和阈值见表1。

图2给出了利用组合检测法进行零速检测得到的行走轨迹。

不同零速检测方法的平面精度见表2。由表2可知，4种检测方法均可达到较高的精度，其中组合检测法精度最高。角速度检测法与组合检测法精度近似，这是因为加速度计随机误差较大，在最优组合法中角速度统计量所占的权重较大，而单纯依靠加速度进行零速检测误差较大。

为验证长时间行走时算法的可用性，进行时间长度为23 min的实验，其中包括30 s的静态数据采集，行走距离为750 m。实验中使用的仪器是荷兰Xsens公司生产的Mti-g-700型MIMU，采样频率为100 Hz，图3为行走轨迹。

由图3看出，起点和终点坐标差为24.718 m，误差百分比仅为3%。所以，利用零速修正对惯导误差进行修正，在长时间长距离的行走实验中也能达到很好的效果。

4结语

本文主要介绍基于零速修正的行人导航算法，将零速时刻的系统输出作为卡尔曼滤波观测量估计系统误差，改善导航精度。基于加速度计和陀螺仪输出的周期特性，总结并实现了4种零速检测算法，4种算法都在较大程度上提高了惯导精度，其中利用加速度和角速度组合的检测法精度最高。

参考文献

[1]马晓龙.基于MIMU的单兵室内导航算法研究[D].长沙:国防科技大学,2013(Ma Xiaolong. Research on the Navigation Algorithm of Individual-Soldier Indoor Positioning Based on MIMU[D].Changsha:National University of Defense Technology,2013)

[2]方靖,顾启泰,丁天怀.车载惯性导航的动态零速修正技术[J].中国惯性技术学报,2008,16(3):265-268(Fang Jing, Gu Qitai, Ding Tianhuai.Dynamic Zero Velocity Update for Vehicle Inertial Navigation System[J].Journal of Chinese Inertial Technology, 2008,16(3):265-268)

[3]El-Sheimy N. Inertial Techniques and INS/DGPS Integration[Z].ENGO 623-Course Notes, Department of Geomatics Engineering, University of Calgary, Canada, 2004

[4]Titterton D H,Weston J L.Strapdown Inertial Navigation Technology[M].Stevenage: The Institute of Electrical Engineers and the American Institute of Aeronautics and Astronautics, 2004

[5]赵小明,邓东黎,蒋志炜.车载单轴旋转激光捷联惯导抗晃动初始对准和零速修正方法[J].中国惯性技术学报,2013,21(3):302-307(Zhao Xiaoming, Deng Dongli, Jiang Zhiwei.Anti-Disturbing Alignment and ZUPT Methods for Land Single-Axis Rotation Laser SINS [J].Journal of Chinese Inertial Technology,2013,21(3):302-307)

[6]Skog I, Nilsson J O, Händel P.Evaluation of Zero-Velocity Detectors for Foot-Mounted Inertial Navigation Systems[C].International Conference on Indoor Positioning and Indoor Navigation (IPIN), 2010

[7]Skog I, Händel P, Nilsson J O, et al. Zero-Velocity Detection—An Algorithm Evaluation[J]. IEEE Transactions on Biomedical Engineering, 2010, 57(11): 2 657-2 666

[8]Nilsson J O, Skog I, Handel P, et al. Foot-Mounted INS for Everybody-An Open-Source Embedded Implementation[C].Position Location and Navigation Symposium (PLANS), IEEE/ION, 2012

Foundation support:Open Fund of State Key Laboartory of Geographic Information Engineering, No.SKLGIE2015-M-2-5.

About the first author:TANG Wenjie, postgraduate, majors in pedestrian navigation algorithm, E-mail:twjhao123@sina.com.

Research of Pedestrian Navigation Algorithm Based on Zero Velocity Update Aided MIMU

TANGWenjie1LÜZhiwei1ZHANGLundong1CONGDianwei1,2

1School of Navigation and Aerospace Engineering, Information Engineering University，62 Kexue Road, Zhengzhou 450001，China 2State Key Laboratory of Geo-information Engineering，1 Mid-Yanta Road, Xi’an 710054,China

Abstract:This paper studies a pedestrian navigation algorithm based on zero velocity update(ZUPT) aided MIMU. Considering the problem of the increasing error of MIMU over time,according to the assumption that the pedestrian’s velocity can maintain zero in a short time in every foot stance phase,the virtual noise observations are constructed to estimate system error. Noting that the zero-velocity detector is the key technology in ZUPT,we compare four detectors and evaluate their efficiency and accuracy.The four detectors are the acceleration moving variance detector, the acceleration detector, the angular rate detector, and a combined detector.The experimental results show that under the circumstance that the distance is short and step is slow,the four detectors can all reach high accuracy. However, the combined detector give the highest performance.

Key words:MIMU;ZUPT;zero-velocity detector;pedestrian navigation

收稿日期：2015-07-04

第一作者简介：唐文杰，硕士生，研究方向为行人导航算法，E-mail:twjhao123@sina.com。

DOI：10.14075/j.jgg.2016.06.018

文章编号：1671-5942(2016)06-0548-04

中图分类号：P227

文献标识码：A

项目来源：地理信息工程国家重点实验室开放基金(SKLGIE2015-M-2-5)。