基于惯性传感器行人室内定位导航算法研究

刘昶

(合肥市测绘设计研究院，安徽 合肥 230000)

1 引 言

目前发展比较完善的全球定位系统(GPS)技术，在一些易受遮挡、GPS信号干扰、室内场合等环境下无法提供有效的定位服务，而基于微机电系统(micro-electro-mechanic system，MEMS)的惯性传感器的定位技术能很好解决这一问题，惯性系统是一种自主导航系统，它能够提供连续性好、短期精度高和稳定的导航信息(如位置、航向、姿态角数据)[1]。随着惯性传感器不断在人们生活中得到应用和发展，特别在室内的环境中，如通过加速度检测行人的步长和步幅，结合惯性传感器提供的航向信息，通过航位推算确定行人位置(pedestrian Dead Reckoning，PDR)[2～4]。Robertson等提出基于鞋底安装惯性传感器的贝叶斯定位算法框架，采用卡尔曼滤波估计行人位置[5]；John-OlofNilssion等提出了基于鞋绑式零速度修正(Zero Velocity Update，ZUPT)辅助的惯性导航系统，提供开源的代码，但算法参数设置繁多，对不同的环境定位应用带来不便[6]；Skog I，Händel P等研究ZUPTs在辅助惯性导航系统中局限性和传感器误差建模影响分析[7]。

本文以航向姿态参考系统(AHRS)为基础，IMU(Inertial Measurement Unit)的三轴陀螺仪和三轴加速度计追踪载体运动的旋转和平移，提出对三轴动态方向估计滤波融合修正，优化设计行人行走中零速度检测算法，通过对行人静止判断，进行加速度方向校准和行人速度漂移误差的剔除，最后对时刻速度积分获取精确的位置信息，从而提高系统定位的精度。

2 行人步态跟踪定位算法结构

当前MEMS IMU的应用精度相对较低，进行独立的惯性导航解算时，其解算的位置积累误差随着时间成三次方发散，从而失去惯性传感器导航定位的现实意义，因此采用零速修正(ZUPT)可有效解决惯性传感器的积累误差的三次方发散问题。行人行走过程如图1所示： 从运动可以看出， 行人行走的明显规律为“运动—静止—运动—静止……”的过程，脚底A与地面接触时会有短暂的静止时间，从时刻T=0.48 s到T=0.72 s的行人短暂△T时间段内为静止状态，此时行人速度VA=0，针对△T静止时间段内进行零速度修正，其修正效果如图2(a)，其中惯性导航系统(INS)是未经零速度修正的速度误差呈三次方发散。INS+ZUPT为经过零速度修正后误差发散情况，由此可得，经过零速度修正后的速度获取的位置信息，如图2(b)所示，位置误差与行人步数成一定的线性相关。

图1 行走动态阶段

图2 零速修正结果

本文将MEMS惯性传感组件安装于人体足部，结合人体足部行走特征，通过互补滤波融合的方向估计修正算法，及足部零速检测判断，构成基于方向和零速修正的惯性辅助行人跟踪定位系统。该系统主要为三部分组成：

①航向姿态更新算法。方向估计更新在AHRS系统上，采用方向四元数(Quaternion)算法解算，通过MEMS惯性传感器的三轴加速度计和三轴陀螺仪的互补滤波融合对载体的三轴姿态进行精确方向估计。与欧拉角算法相比，避免采用三角函数运算，既保证更新速率，计算时也不会出现奇异性

②零速度检测算法。零速度检测算法通过MEMS惯性传感器的三轴加速度值检测行人行走过程中足部与地面接触时间段内的零速度，本次实验设计广义似然比检测算法(GLRT)，通过设置零速检测阈值判断是否处于零速度状态，为零速更新提供必要条件。对于正常行走步态可实现很好的检测，但对于跳动、抖动的步态难以检测，目前本文只研究正常行走步态。

③修正算法。修正算法主要是削弱图2(a)和图2(b)中的速度积累误差和位置积累误差的发散问题，通过对方向修正及零速、非零速时漂移误差的剔除更新，获取用于导航计算最终人体速度。

3 滤波融合方向估计算法

AHRS系统，是一种主要采用惯性传感器数据测量空间姿态的传感系统，一般情况，AHRS系统能够确定载体的三轴姿态[10]。

方向是基于AHRS系统进行估计，采用惯性传感器的三轴加速度计和三轴陀螺仪，分别量测载体坐标系下三轴上的加速度和角速率，利用陀螺仪补偿角解算出量测误差的四元数的微分形式，从而估计出地理坐标下的方向四元数。

设三轴陀螺仪在载体坐标系下的角速度为wx，wy，wz，则角速度向量Ww：

Ww=[0wxwywz]

(1)

(2)

(3)

(4)

(5)

则设误差函数f：

(6)

通过迭代计算来估计方向四元数，即计算公式为：

(7)

(8)

4 零速度检测算法

零速检测的目的是检测行走过程中速度为零的时段，即对图1中△T的时间段内VA=0的检测，零速度检测是触发零速修正的前提，也是实现速度误差清零关键步骤。崔潇等文献采用一种多条件阈值复合检测算法(比力模值、比力方差、角速度模值)，只有三条件同时满足，对零速检测效果表现出更有效性[8]；Skog I，Händel P，Nilsson J O等通过与常用的加速度方差检测、加速度幅度值检测、角速度能量检测方法相比较，它们都存在一定缺陷，而广义似然比检测方法适用性更有效[9]。

(1)加速度值和角速度值向量表示。

(9)

(2)检测算法公式。

(10)

式(10)中g为重力加速度值。

(3)设置检测阈值γ：

(11)

当式(11)满足条件，认为速度为零(足部静止状态)，设置为1，非零速度设置为0(足部离开地面)。

对本文楼道采集的数据进行零速检测，截取部分时段数据如图3所示，Stationary为零速检测折线图，数值为1表示为行人静止状态，由Stationary折线图可得，零速检测对行走过程中静止时刻能有效检测。X、Y、Z分别为三轴加速度，X、Y轴加速度周期性的出现加速度为零(aA=0)，即对应行人速度为零的时刻(vA=0)。

图3 零速检测结果

5 基于方向和零速修正辅助定位算法

本章节修正算法包括对方向估计后三轴方向的修正以及零速检测后的速度修正，并对非零速状态下的速度漂移改正，来约束行人在运动过程中的方向漂移以及速度漂移，从而提高行人行走状态下的位置更新精度。

5.1 互补滤波融合方向修正算法

(12)

(13)

(14)

(15)

则式(14)中可简化为：

(16)

将式(2)、式(15)和式(16)带入式(12)，可简化为：

(17)

(18)

(19)

5.2 零速及速度漂移修正辅助定位

将MEMS惯性传感器固定在行人足部，按照文中5.1节的互补滤波融合算法的方向修正和第4节的零速检测的零速更新修正实现行人行走状态下导航定位。

①计算平移加速度。通过5.1节的方向四元数估计，将加速度计量测的三轴加速度通过旋转变换到地理坐标系下，得到精确地理坐标系下的三轴加速度a。即：

(20)

式中⊗表示将三轴加速度通过四元数乘积的旋转变换计算。

②计算平移速度，将①中转换得到的地理坐标系中实际三轴加速度a，通过对a数值积分获得速度v。

vt=vt-1+at△t

(21)

式(21)中，△t为采样周期。在对通过积分求取的速度，并通过零速更新修正，当判断在某时刻速度为零时，速度设置为零，并触发ZUPT更新机制，实现行人轨迹定位，同时本文考虑对非零时刻的速度，剔除非零速存在的整体漂移vD。

v=v-vD

(22)

③计算行人位置。对最终求得的速度v进行积分获取行人位置P。

Pt=Pt+vt△t

(23)

5.3 室内导航定位算法流程

基于以上算法结构，设计基于惯性传感器的行人室内导航定位算法系统的完整结构图，如图4所示。

图4 行人定位导航算法工作原理流程图

6 实验结果及验证

为了验证设计的行人步态导航定位系统的有效性，本次实验区为连续的矩形楼道走廊，其实验区示意图如图5所示，惯性传感器为X-IMU型号，固定于足部，如图6所示。实验为闭合路径，共四次拐弯，起点为西北处，沿楼道连续行走两圈，数据量共166.04×2 m。

图5 楼道示意图 图6 惯性传感器与足部固定位置示意

利用MATLAB软件编写上述算法，实现INS(未修正)、INS+ZUPT(修正，包括方向和速度修正)与实际楼道路径对比实验的方法。图7为实验行走路径对比结果。

图7 两圈矩形轨迹定位结果

表1为在矩形楼道中连续行走两圈，回到最初起点的路径统计结果，第一圈为各拐弯路径的行走结果，然后统计第一圈和第二圈的统计结果。

表1 行走距离结果

在图7中得：在实验中，蓝色路径为行走一圈路径，随时间推移已严重偏离实际路径，且位置误差积累不断增大，主要原因是未经方向误差补偿修正和速度修正导致。则红色路径是采用互补滤波融合方向修正计算，能有效补偿陀螺仪的漂移误差和滤除加速度计的高频错误，从而获取精确的方向，并结合零速修正，解决速度误差积累，且与实际路径相吻合，有效实现行人步态导航定位。在实验中，两圈矩形路径行走的最后闭合差约为 1.6 m，其定位精度均在1.1%以内。

7 结 语

基于MEMS的惯性传感器行人步态室内导航定位算法的实现，主要采用基于AHRS系统的互补滤波融合方向估计算法和零速修正技术完成行人跟踪定位的过程，本文重点论述了在AHRS系统上，融合MEMS惯性传感器的加速度计和陀螺仪的数据提出一种滤波融合方向估计算法，有效补偿陀螺仪漂移，并有效的计算出全局方向估计，结合足部惯性传感器的加速度和角速度特性设计零速检测算法(GLRT)，并触发ZUPT更新，完成跟踪定位。在对缺少GPS信号的室内和室外环境均可有效的较长时间进行跟踪定位，具有重要的意义。