行人导航系统中的MEMS误差在线修正技术

李清华，于文昭，谢阳光, 黄志威，李新年

(1.哈尔滨工业大学空间控制与惯性技术研究中心，哈尔滨 150001;2.飞行器控制一体化技术国防科技重点实验室，航空工业自控所，西安 710065)

0 引言

行人导航系统(Pedestrian Navigation System，PNS)，又名单兵导航系统，是导航中的一个重要组成部分，近年来受到越来越多的关注。由于其不受卫星信号的约束，故能被应用于商场等室内场景。惯性行人导航系统通常采用微机电系统(Micro-Electro-Mechanical System, MEMS)，具有成本低、体积小、携带方便[1]等优点。但目前市面上的MEMS精度相对较低，长时间的工作会导致误差积累，从而影响定位精度[2]。传统的标定方法大多在实验室中进行，需要将器件从设备或载体上拆下来，用实验室的转台等设备进行标定。这种方法的过程复杂，且耗时较长[3]。另外，经过一段时间，MEMS的标定参数也会发生变化，影响使用性能。

为了减少随着时间推移累计误差的影响，提高行人导航系统的精度，Foxlin[4]提出了在静止区间使用零速更新，辅助卡尔曼滤波的方法对误差进行修正,但精度有待提升。Fischer C[5]提出了利用无迹卡尔曼滤波器(Unscented Kalman Filter，UKF)进行信息融合。时伟等提出了用2个微惯性测量单元(Miniature Inertial Measurement Unit，MIMU)分别固联在单兵的双脚上，利用基于假设检验和极大似然估计的零速检测器进行零速检测及修正[6]，但工作量大，操作复杂。Borestein J等提出了启发式漂移消除算法(Heuristic Drift Elimination，HDE)[7]，将方向分成4个或8个主方向，对角速度信息进行修正，从而修正航向。A. R. Jiménez 等在此基础上进一步做了改进，提出了改进型启发式漂移消除算法[8](Improved Heuristic Drift Elimination，iHDE),将姿态角和主方向信息加入扩展卡尔曼滤波(Extended Kalman Filter, EKF)，降低航向误差。在器件在线标定方面，王海亮等[9]提出了一种弹载捷联惯导简易在线标定技术，利用车体做常规运动下的横滚角和俯仰角对惯导误差的激励作用实现在线标定，但是是面向弹体等可以控制固定轨迹来进行实验标定的对象，不适用于行人导航。由于MEMS的零偏和刻度系数受环境影响较大，而安装误差受环境影响小、稳定性好，何昆鹏等[10]提出了一种野外标定MEMS器件参数的方法，对微组合导航系统的3个轴分别进行位置摆放，通过方程标定零偏和标度因数，实现组合导航系统的野外在线标定，但因行人导航系统中，器件始终固定在脚上，因此也不适用于行人导航系统。

本文在分析了上述问题的基础上，通过研究行人导航和微惯性器件的特点，提出了一种基于MEMS器件模型的在线修正算法。该方法基于微惯性器件的误差模型，对陀螺仪和加速度计的零偏进行修正。初始时，利用误差模型进行初步修正，从而达到修正初始姿态角的目的；在行走过程中，搜索判定行走过程中的修正区间，进行在线修正，并对修正之后的数据进行主航向角反馈修正，综合提高了行人导航系统长时间定位性能。

1 基于微惯性的导航系统误差模型

1.1 微惯性器件误差模型

考虑陀螺仪和加速度计的刻度因数、安装误差和零偏误差，误差模型如下

(1)

(2)

其中，Bx、By、Bz为陀螺仪三轴向的零偏误差，Dx、Dy、Dz为加速度计三轴向的零偏误差；δkgx、δkgy、δkgz为陀螺仪三轴向的刻度因数误差，δkax、δkay、δkaz为加速度计三轴向的刻度因数误差；δLij(i=x,y,z;j=x,y,z)和δL′ij(i=x,y,z;j=x,y,z)分别为陀螺仪及加速度计三轴之间的安装误差变化；ωx、ωy、ωz、fx、fy、fz分别为陀螺仪和加速度计的理想输出。

1.2 误差模型的简易处理

由于陀螺仪和加速度计集成在一个芯片上，安装误差的长期稳定性较好，因此可将其安装误差看作常值，使用事先标定好的参数加入捷联惯导系统中，进而达到简化误差模型的目的，简化后的模型如下[11]

(3)

(4)

2 基于误差模型的MEMS器件在线修正算法

为了实现陀螺仪和加速度计的在线修正，首先需要判断出行人步行的零速区间，即零速检测。零速检测[12]指当行人行走脚落地过程或者行人静止状态下，绑有惯性测量单元(Inertial Measurement Unit，IMU)的脚在落地期间，理论上，其角速率和加速度均为零。通过检测落脚瞬间，可以人为消除陀螺仪和加速度计输出值的累积误差。在此基础上，选择可修正区间，利用在线修正算法修正陀螺仪和加速度计的参数，进而优化导航解算的位置信息。

2.1 在线修正区间的判定

为了判定在线修正区间，在四条件法零速检测的基础上，进行长度检测与识别。取数据窗口的长度为L，半窗口的长度为s，L=2s+1。

加速度阈值条件为

(5)

|ak|即为判断窗口内的加速度的均值，若处于给定的阈值区间，则判断此时脚部处于静止状态。用C1来表示人行走的状态，C1为1时表示零速状态，C1为0时表示非零速状态，即

(6)

角速度阈值条件为

(7)

|ωk|即为判断窗口内的角速度的均值，若处于给定的阈值区间，则判断此时脚部处于静止状态。用C2来表示人行走的状态，C2为1时表示零速状态，C2为0时表示非零速状态，即

(8)

加速度方差条件为

(9)

(10)

(11)

角速度方差条件为

(12)

(13)

(14)

当上述4个条件都满足时，即C=C1&C2&C3&C4=1时，表示为零速时刻。

取thcor为判断零速区间长度阈值，Lk-zero为第k个零速区间的长度，在零速区间检测之后，利用公式进行判定，判断关系式如下

(15)

2.2 基于模型的在线修正算法

(16)

图1 导航系-载体系坐标变换示意图Fig.1 Coordinate transformation between navigation and body frame

根据角度转换关系，可得n系下的比力(0,0,fg)′与b系下的比力(fx,fy,fz)′之间的转换关系为

(17)

根据式(16)和式(17)，可以计算求得初始的俯仰角和滚转角分别为

(18)

(19)

(20)

(21)

初步修正之后，根据2.1节提到的方法进行可修正区间的判别，选择出符合条件的几个可修正区间。取可修正区间内窗口长度为S,根据

(22)

(23)

可将后续加速度和角速度的值进行进一步优化。优化后的加速度值通过式(23)即可重新计算该区间内俯仰角和滚转角。

3 主航向反馈修正算法

在足绑式行人导航系统中，俯仰角和横滚角可以通过加速度计输出计算得到，但是航向角的误差存在不可观性，所以会导致航向角的漂移和误差累积[13]。为了实现修正航向角的目的，提出了主航向角误差反馈修正算法，将行走过程中的航向角变化进行在线计算，判断直行与转弯两种状态。将航向角误差作为观测量反馈到EKF中。

3.1 航向估计

对于足绑式行人导航系统，由于IMU器件绑在脚面上，且脚面运动比较复杂，所以其航向角的变化很快。规定行人行走时的状态为直线和转弯两种，那么在行走两步之间的航向角变化为

(24)

(25)

由此可知，在第k个直线行走的路径中，经过一段时间，在行走第j步时，对应的主航向角变化值为

(26)

设定一定的角度阈值thturn，判断是否处于转弯状态

(27)

引入权重系数μ，对行人行走的状态进行进一步的描述，即

(28)

3.2 主航向反馈滤波修正

为了实现预测行走过程中零速时刻的误差并进行修正，使用EKF，状态方程和量测方程如式(29)所示。

Xk=φk|k-1Xk-1+Wk-1
Zk=HδXk|k+Vk

(29)

φk|k-1=

(30)

在量测更新部分，取主航向角误差量δψ和速度变化向量δvk为观测量，即

Zk=[δψ,δvk]

(31)

对应地

(32)

设置量测噪声Rk中对应主航向角误差的部分为R′k-head，对应速度变化的部分为Rk-v，即

(33)

R′k-head=μ2Rk-head

(34)

计算当前的卡尔曼增益，并把速度信息作为系统的量测值，通过状态方程对信息进行时间更新，再通过量测更新得到更新后修正的速度、位置和姿态角等信息。

4 实验验证与结果分析

针对本文提出的在线修正与主航向反馈修正算法，分别在学校建筑物走廊和学校操场设计进行了直线实验及环形实验。实验使用的微惯性测量单元为荷兰Xsens公司的MTi-G-710。该元件内部包括三轴加速度计、三轴陀螺仪和磁力计。加速度计量程为±50m/s2，零偏稳定性40μg，零偏重复性0.03m/s2，陀螺仪量程为±450(°)/s，零偏稳定性10(°)/h，零偏重复性0.2(°)/s，采样频率为100Hz，固定于脚面，如图2所示。

直线实验部分在学校建筑物走廊进行，路程为40m，开始时在原地静止3s，在前进到26m处左右时暂停5s，再走到终点。环形实验部分在学校操场进行，路程为300m，开始时在原地静止3s，在每前进60m左右时暂停5s，直到走到终点。使用本文提到的在线修正技术及主航向反馈修正算法前后的轨迹分别如图3和图4所示，实验结果如表1所示。从中可以看出，直线行走实验误差从11.08%降低至2.01%。环形行走实验的误差从14.79%降低至1.65%，误差有了很大程度降低。

图2 MEMS器件及穿戴方式Fig.2 MEMS devices and wearing methods

图3 直线行走轨迹Fig.3 Linear walking trajectory

图4 操场环形实验轨迹Fig.4 Circular walking trajectory on the playground

表1 实验验证结果

Tab.1 Experiments results

实验类别起点位置(x/m, y/m)实际终点位置(x/m, y/m)解算出的终点位置(x/m, y/m)误差/%室内-在线修正前(0, 0)(0, 40)(4.25, 38.75)11.08室内-在线修正后(0, 0)(0, 40)(0.77, 40.23)2.01室外-在线修正前(0, 0)(0, 0)(-43.97, -5.86)14.79室外-在线修正后(0, 0)(0, 0)(-0.92,-4.87)1.65

5 结论

本文提出了一种基于误差模型的MEMS器件参数在线修正技术，对陀螺仪和加速度计的零偏进行在线修正。根据行人行走的特点，检测并区分行走过程中的可修正区间，使用逆向解算算法进行数据的解算和修正补偿，并提出了主航向角反馈修正算法，以提高行人导航系统长时间定位性能。从实验验证的结果可以看出，直线实验的误差降低了9.07%，环形实验的误差降低了13.14%，效果很好。该方法提高了行人导航系统的定位精度，其思路和方法对于行人自主导航系统的优化具有很好的应用价值。