基于惯性导航的步行者零速检测算法*

2019-03-05李刘颂徐向波

传感器与微系统 2019年3期

李刘颂，徐向波

(北京林业大学工学院，北京 100083)

0 引言

步行者导航广泛应用于矿井定位、消防员定位等步行者的步行特征没有规律。基于惯性导航系统的步行者导航主要有两种解算算法：一种是行人航迹推算(pedestrian dead-reckoning，PDR)算法[1,2]，包括航向、步频和步长的估计，这种算法主要适用于有规律的步行者导航的情况，但由于样本的差异性和周围环境的复杂度，有一定的局限性，由于传感器自身精度较低，会产生较大的累积误差，无法满足要求[3，4]；另一种是基于零速检测(zero velocity detection，ZVD)[5]的惯性导航系统推算算法，这种方法通过零速检测将步行状态分为静止和运动两个状态，运动状态用惯性导航进行步长估计，在静止状态进行零速检测，根据零速的速度误差、位置误差进行修正，以提高导航精度。通过零速修正现在常用的几种零速检测算法对比[6,7]，本文提出一种基于滑动平均的极大似然估计算法，经实验验证，该方法能够有效检测零速区间的范围，证明了该方法的有效性。

1 步态分析

人体行走的典型步态，根据传感器的运动特征，步态可分为两个阶段。分析步态特性便于对行人行进过程中的零速状态进行检测。结合步态周期，本文将步态周期静态相位中行人运动脚脚后跟着地、站立阶段定义为行人步态中的零速区间，静止阶段大约持续时间为0.1～0.3 s[8]，加速度的理论输出值是重力加速度，陀螺理论输出值为零。

2 定位算法设计

本文提出的基于零速检测的惯导导航算法主要包括零速区间的检测算法、步长估计算法、零速修正算法、初始对准算法以及滤波算法。如图1所示，通过加速度计以及陀螺仪的输出值进行惯性导航系统解算得到速度、姿态以及位置，同时步态区间检测算法判断行人处于何种步态，进行零速更新，结合零速更新算法对速度、姿态、位置进行修正，提高行人定位系统的精度。

图1 算法结构

3 零速检测算法

根据加速度计和陀螺仪的输出，设计相应的阈值，可以判断步行状态。行人的行走步态可分为迈步阶段和静止阶段，理论上行人处于静止状态，角速度值和水平加速度值应为零以及竖直方向的加速度值应等于重力常量。多数的零速检测算法是基于对传感器输出的加速度和角速度的模值、方差以及幅值和已设定的阈值进行比较，提取出步态中的零速点[9]，但行人在行走过程中，足部与地面的接触时间特别短，这类算法易误判零速区间导致丢失零速点，导致行人的速度、位置误差增加。本文算法是通过对行人处于站立相、摆动相进行概率分配，根据广义似然函数法，确定出行人的站立相[10]。

设行人处于摆动相的状态值为H0，处于站立相的状态值为H1。假设H1为真值时，此时行人处于摆动状态的概率表达式为:PFA=Pr{H1|H0}，以及假设H0为真值时，行人处于摆动状态的概率表达式为PD=Pr{H1|H1}

(1)

式(1)为当前时刻是处于行走摆动状态，其中，γ为检测的阈值，L为对于每一个Zn的值的似然比。

采用极大似然估计法对θ进行估计，其中，θ为加速度和角速度值

(2)

(3)

(4)

(5)

若设未知数θ在H0,H1中极大似然估计值分别为0,1，则得到

(6)

由式(6)进行相应的计算可以得到

(7)

式(7)两边取对数可得

(8)

(9)

式中ZVD为检测结果，1为零速，0为迈步状态。

4 零速修正算法

在零速检测区间进行零速修正[11]，采用卡尔曼滤波算法。在检测为零速区间时，将速度置为0，利用零速修正姿态误差、速度误差和位置误差，设计卡尔曼滤波器。

状态变量为X=[PVφaω]，其中,P=[PxPyPz]为位置误差，V=[VxVyVz]为速度误差，φ=[φxφyφz]为姿态误差，a和ω分别为加速度计和陀螺仪的零偏误差。

观测变量选择零速区间解算出的速度误差V，所以，观测矩阵为H=[O3×3I3×3O3×3O3×3O3×3],在零速区间进行时间更新和信息更新，达到零速修正的效果。

5 实验验证

5.1 实验平台

选用惯性元器件(MPU6050)和磁强计(HMC5983)组合的方式，传感器的性能参数如表1所示。

表1 MEMS传感器性能参数

实验场地选在北京林业大学图书馆前广场，经测量广场每块瓷砖的长度为0.6 m，将传感器固定在步行者脚上，采集数据如图2所示。

图2 实验数据采集

5.2 零速区间数检测

通过对加速度和角速度的数据值，选定合适的阈值γ′，本实验中γ′为108，如图3所示判断静态区间，结果如图4所示。分别计算行人在不同行走速度下的零速区间的个数，并进行误差分析，由表2可以看出，该零速检测算法具有较高的精确性。

图3 T值

图4 零速区间判断

试验类型零速区间实际值计算值误差/%慢速行走2550255000常速行走2550255000快速行走2550255000

5.3 步长估计

实验采集数据，步行者在图书馆门口沿一块瓷砖东向行走一瓷砖的距离，图5中x轴、y轴、z轴分别代表地理坐标系的东北天方向，从图5可以得到表3中的数据，可以看出对于步长的估计推算相对误差为2.7 %。单步误差比较大，主要原因是实验人员不能准确跨出单步距离。

图5 单步三轴位移

测试项x/my/mz/m理论值0.5930.0920实测值0.6110.0780.018误差0.0180.0140.018

5.4 行人轨迹推算

实验采集数据，步行者在图书馆门口沿瓷砖西向行走12块瓷砖，大概12 m的距离，通过实验得到图6虚线为计算轨迹，实线为实际轨迹，方块标记为起始位置，由图看出：两个轨迹基本重合。经计算得到表4，步行12 m的相对误差为1.6 %。图7为基于零速检测和基于惯性导航推算，结果表明，零速检测能够有效解决惯性导航长时间误差漂移、步行者轨迹推算精度低的缺点，且能够检测脚步的运动特征。