足绑式PNS自适应滑动平均零速区间检测方法

陈克川，李保国，刘思庆，李淑影

(北京航空航天大学，北京 100191)

0　引言

现如今，为个人提供定位服务的导航设备在人们的生活中得到了广泛的应用，已成为人们日常生活中相当重要的一部分。该功能可以有效地指导人们在陌生环境下的活动，有的个人导航系统设备可以通过无线通讯传输给监控单元，实时监测徒步人员的位置变化[1]。目前，绝大多数个人导航产品都是基于卫星导航定位，这种导航方式对室内应用无能为力。

针对室内定位不同的应用场合和功能需求，人们提出了使用多种不同的定位模型和定位方法推算室内导航位置。个人导航系统(Personal Navigation System, PNS)，主要用于跟踪定位徒步行走人员的实时位置[2]。行人徒步自主导航，通过在足部[3]绑定一个惯性测量单元(Inertial Measurement Unit, IMU)和磁强计，构成一个足绑式个人导航系统。IMU和磁强计常采用低成本的微电子机械系统(Micro Electro Mechanical System, MEMS)传感器，其成本低、体积小、功耗低、穿戴方便，受到人们的欢迎。

但是市场上MEMS运动传感器精度低、噪声大，用于个人导航时误差随时间积累，发散十分快。因此，必须利用行人行走时落地零速的特点，对速度误差、位置误差和水平姿态角误差进行零速修正(Zero Velocity Update, ZUPT)，以提高定位导航的精度。

根据行人运动特点，当脚着地时，捷联在脚上的IMU速度为零。据此对姿态、速度和位置信息进行零速修正，可以有效抑制导航系统参数发散。零速修正的关键在于零速区间精确的检测，本文在借鉴前人研究工作的基础上，设计了基于自适应滑动平均算法的零速区间检测方法。经实验验证，该方法能准确、有效地检测出零速区间的范围，证明了该算法的有效性。

1　自适应滑动平均零速区间检测方法

行人在运动过程中，安装于脚部的MEMS惯性测量单元能够敏感脚部的比力及角速度变化，可将其用于行人运动或静止的分析与识别。在行走过程中，足部与地面接触的时间较短，一般情况下为0.2s～0.5s[4]，在跑步时触地时间更短。因此，基于行人导航系统精度的要求，需要非常准确地判断零速区间。

在足绑式PNS中，静止状态即零速状态的判定[5]，是零速修正的基础。大多数文献是基于陀螺或者加速度计或者二者兼有之的输出进行零速判定，部分文献介绍了利用压电、超声波[6]等辅助传感器进行零速判定。

为不增加额外传感器，同时更准确地进行零速区间判定，本文利用自适应滑动平均算法对三轴陀螺输出和三轴加速度计输出进行联合判定。

1.1　自适应滑动平均算法

自适应滑动平均算法广泛用于金融计算、雷达系统等，常用来解决寻找包含高噪声动态信号的锋利间断点。算法首先鉴别出间断点发生的时间段，然后在时间区间内通过自适应的调整滑动窗口大小，避免滑动平均算法造成的时间延迟。该算法是一种基于迭代滑动平均的自适应算法，算法根据滑动平均后数据的变化趋势，通过比较样本方差，改变滑动窗口的大小。因此，在数据变化的区间，窗口会自适应缩放，数据变化率越大，窗口越紧凑。

算法步骤：

1)取长度为N的采样数据作为一帧，并在区间[2q+1,N-2q-1]内应用滑动平均算法KZq,k[X(t)]处理。其中，k为迭代次数，q为滤波长度。

应用q点滑动平均算法做数据预处理，KZq,k是一个迭代的滑动平均算法：

(1)

其中，Xi为原始数据，yt为滤波后的数据。

2)预处理后的数据yt将用于滑动平均的自适应滤波算法。在自适应滤波器中，窗口大小将自适应调整。滤波器分别由帧头(qh)和帧尾(qt)构成，在数据变化率较大的区间，有效地调整其大小，而在间断点处帧头qh将减小。

3)根据滑动平均算法构造差分向量：

D(t)=|y(t+q)-y(t-q)|

(2)

4)计算导数：D′(t)=D(t+1)-D(t)。如果斜率为正，帧头将按窗口全尺度以比例f[D(t)]缩小，帧尾将扩大到全窗口长度；如果斜率为负，帧头将扩大到全窗口长度，帧尾将按窗口全尺度以比例f[D(t)]缩小。

(3)

即：

(4)

(5)

5)由此得到滤波器的输出Yt为：

(6)

6)将输出的qh和qt作为输入，进行上一步算法的迭代运算。

应用自适应滑动平均算法处理含有间断点数据，如图1所示。

行人走动时，脚部的运动存在明显的运动、停止、运动、停止……交替前进的特殊运动规律[7]。体现在惯性传感器输出上，则有明显的间断点。因此，用该算法可以有效地找到由动到静或者由静到动的间断点，即零速区间的起始点。

1.2　零速区间检测方法

零速区间检测过程如下：

(1)加速度零速检测方法

采用加速度的模进行判断是否为零速，K时刻加速度的模为：

(7)

如图2所示，在零速状态下，即在没有线性加速度干扰阶段，理论上几乎只有当地重力加速度g。因此，该阶段加速度计输出的模应在某一范围内，将加速度计模值减g后取绝对值，设定阈值后即可进行零速点的检测，本文设置阈值为thamin=-0.1g，thamax=0.1g。

加速度数据判定零速有效标志：

(2)角速度零速检测方法

同样采用角速度的模进行判断是否零速，K时刻角速度的模为：

(8)

如图3所示，在零速状态下，理论上角速度几乎为0，因此该阶段陀螺仪输出的模应在某一范围内。考虑到MEMS陀螺仪噪声大，且固定在脚上也无法真正做到静止，故本文设置阈值较大，为thg=20(°)/s。

角速度数据判定零速有效标志：

最终，将基于角速度模值检测的结果与基于加速度计检测的结果相与作为检测的结果，以便提高零速区间检测的准确性。

flag=flag_a&flag_g

(9)

2　PNS算法设计

PNS采用成熟的SINS算法。由于个人导航系统的导航范围有限，位置宜采用东-北-天导航坐标系，即地理坐标系。另外，行人行走速度较慢，计算过程中的小量可以忽略，而且MEMS陀螺仪精度比较差，地球自转角速度完全淹没在噪声之中，因此导航方程可以做相应的简化[8]。

2.1　PNS误差方程

根据行人运动特点，推导了适用于行人导航的SINS的姿态误差方程、速度误差方程以及位置误差方程。

(1)姿态误差方程

忽略地球转速以及行人相对地球角速度小量，SINS的姿态误差方程的矢量形式为：

(10)

(2)速度误差方程

忽略哥氏加速度，同时忽略2阶小量，可得速度误差方程：

(11)

其中，fb为加速度计的输出，aB为加速度计的零偏。

(3)位置误差方程

对位置方程求偏导，可得位置误差方程的矢量形式：

(12)

2.2　ZUPT算法

零速检测触发零速修正，在检测为零速的时间内将速度置为0。利用零速检测估计更多误差参数，根据导航误差模型设计零速修正Kalman滤波器[9]，状态量取为X=[φnTδVnTδPnTεbT▽bT]T。

量测量取为检测为零速时刻的解算速度误差δVn，所以量测矩阵为：

H=[03×3I3×303×303×303×3]T。

考虑到量测量δV只在检测为零速的时刻才能获取，且量测噪声与具体运动和干扰相关，事先并不能准确获知，因此Kalman滤波器需进行如下改良：Kalman滤波更新时刻tk，若零速检测为运动，则滤波器只做时间更新；反之，若零速检测为零速，则滤波器做完整更新，同时将估计结果反馈回系统。

ZUPT对于减小速度、位置以及水平姿态误差是一项非常有效的技术，不过该过程无法有效地估计与修正航向误差[10]。这样，随着导航时间的延长，方位误差的影响就会逐渐取代器件误差，成为MEMS个人导航系统的主要误差源。因此要引入磁强计，对航向进行修正。

磁强计可以得到绝对航向，且误差不随时间积累，但存在两个问题：一是磁强计易受外界干扰磁场影响；二是MEMS磁强计精度低，动态精度更差。因此，引入磁强计进行以下改进：1)只选用静态数据，即零速时磁强计输出的磁航向，与SINS的航向之差作为航向误差构造量测量；2)采用模值法对磁强计是否受到外界干扰做判断，同一地区磁场强度不变，即磁强计输出模值应为一常数。设定阈值范围，在此范围内则认为未受到外界干扰，磁航向可以使用。符合以上两个条件时，量测矩阵改为：

H=[diag(0,0,1)I3×303×303×303×3]T。

个人导航系统算法框图如图4所示。

3　实验验证

本实验选用应美盛(InvenSense)公司的MPU9250模块作为微惯性传感器，MPU9250为9轴组合传感器将6轴惯性测量单元(加速度计+陀螺仪)和3轴磁强计集成。利用ZigBee模块通过标准IIC通信协议与MPU9250通信，然后ZigBee将数据打包发送至上位机进行导航解算。ZigBee无线通信模块选用TI公司的CC2530单片机，本身带有射频的功能，外加一些简单电路即可实现无线通信功能。ZigBee+ MPU9250构成足绑式个人导航系统无线采集模块，如图5所示。

将无线采集模块固定在脚上，进行行走实验。

为对比滑动平均与自适应滑动平均算法的效果，行走实验为直线路线行走。行走方向为正北向，行走60步(包括一开始的磁强计罗差校正，原地转动两圈的16步)，距离为52.2m。实验进行了10次，10次的结果对比如表1所示。

表1　导航实验统计结果

从表1可以看出，两种零速检测方法都能准确地计算出步数。但在导航误差上，采用自适应滑动平均算法得到零速区间进行ZUPT，导航误差均小于采用自滑动平均算法。10次导航结果中，自适应滑动平均算法的导航误差平均值为2.51%，滑动平均的误差平均值为5.09%，效果改善了55.87%。

以第8次实验为例，当零速检测值由1变为0，对应着脚离地的动作；当零速检测值由0变为1，对应着脚着地的动作。根据检测值的变化可以得到一步经历的时间，进而计算步频和步数，如图6所示。

由图6可以看出，相对于滑动平均算法对零速的判断，自适应滑动平均算法得到的零速区间范围稍大一些。这是因为自适应算法减弱了传统滑动平均算法检测零速区间的延迟问题，从而得到了更加准确的零速区间判定。两种算法得到的零速区间带入导航解算中去，自适应算法的导航精度提高了57%，具体如图7所示。

4　结论

本文设计了自适应滑动平均算法检测零速修正区间，能及时跟踪模值变化趋势，且可以起到平滑抖动的作用。本算法能避免平滑算法延时问题，得到更准确的零速区间。考虑到现实生活中行人的运动形式多种多样，除了正常的行走外，还有跑、跳、跨、上下楼梯等形式，本文给出的算法还不能适应所有运动形式，针对这种情况还需要做进一步的研究。

[1]Krach B, Roberston P. Cascaded estimation architecture for integration of foot-mounted inertial sensors[C]. IEEE Position Location and Navigation Symposium, 2007: 112-119.

[2]Renaudin V, Yalak O, Tomé P, et al. Indoor navigation of emergency agents[J]. European Journal of Navigation, 2007,5(3): 36-45.

[3]Yun X P, Bachmann E R, Moore H, et al. Self-contained position tracking of human movement using small inertial/magnetic sensor modules [C]. IEEE International Conference on Robotics and Automation， 2007:2526-2533.

[4]Park S K, Suh Y S. A zero velocity detection algorithm sing inertial sensors for pedestrian navigation system[J].Sensors, 2010, 10(10): 9163-9178.

[5]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 & Indoor Navigation, 2010:1-6.

[6]Zhou C M, Downey J, Choi J, et al. A shoe-embedded RF sensor for motion detection [J]. IEEE Microwave and Wireless Components Letters, 2011,21(3):169-171.

[7]崔潇,秦永元,周琪,等. 鞋式个人导航系统算法和试验研究[J].测控技术, 2013, 32(3):138-142.

CUI Xiao, QIN Yong-yuan, ZHOU Qi, et al. Algorithm and test research on shoe-mounted personal navigation system[J]. Measurement & Control Technology, 2013, 32(3):138-142.

[8]殷红,郭杭,邓晓华.基于foot-mounted的IMU室内行人航位推算研究[J].测绘科学, 2014, 39(1):20-23.

YIN Hong, GUO Hang, DENG Xiao-hua. A research of

IMU indoor pedestrian dead reckoning based on foot-mounted[J]. Science of Surveying and Mapping, 2014, 39(1):20-23.

[9]秦永元,张洪钺,汪叔华.Kalman滤波与组合导航原理[M].西安:西北工业大学出版社, 1998.

QIN Yong-yuan, ZHANG Hong-yue, WANG Shu-hua. Kalman filter and integrated navigation principle[M]. Xi’an: Northwestern Polytechnical University Press, 1998.

[10]张新喜, 张嵘, 郭美凤, 等. 基于足绑式INS的行人导航三轴磁强计在线校准[J]. 清华大学学报(自然科学版), 2016, 56(2) :211-217.

ZHANG Xin-xi, ZHANG Rong, GUO Mei-feng, et al. Online three-axis magnetometer calibration for a pedestriannavigation system using a foot-mounted inertial navigation system[J]. Journal of Tsinghua University(Science and Technology), 2016, 56(2) :211-217.