基于足间距信息辅助的行人三维惯性定位算法

路永乐,苏 胜,杨 杰,罗 毅,粟 萍,惠嘉威

(重庆邮电大学智能传感技术与微系统重庆市高校工程研究中心,重庆 400065)

0 引言

导航系统指为行人提供位置信息服务的系统,通常利用卫星定位系统实现室外导航定位[1],但人类大部分的活动都发生在室内环境中,卫星信号会受建筑物墙壁遮挡,导致信号衰弱严重,进而无法提供可靠的室内导航服务[2]。而惯性导航系统(inertial navigation system, INS)以惯性测量单元(inertial measurement unit, IMU)提供的加速度和角速度信息为基础,利用捷联惯导算法解算得到位姿信息,实现导航定位[3]。由于INS不需要外界信息辅助,具有自主性强、抗干扰能力强等优势,在单兵作战、火场救援及室内导航等领域成为了主要研究方向[4-5]。

但是IMU存在不可避免的误差,导致INS定位结果出现严重漂移,无法提供准确的定位服务[6]。为抑制行人INS累积误差,Foxlin等[7]采用零速修正(zero velocity update, ZUPT)算法,在静止区间内利用零速条件修正系统误差。但ZUPT算法无法解决航向角误差漂移的问题,长航时定位结果仍会偏离真实情况[8]。

基于以上问题,研究人员对行人INS误差修正进行了以下三方面的研究:1)利用多传感器融合抑制航向漂移。孙猛等[9]利用磁力计测量磁场信息,并利用扩展卡尔曼滤波构建基于磁定位和行人航迹推算(pedestrian dead reckoning,PDR)的融合系统,实现室内定位;何成阳等[10]利用超宽带(ultra wide band,UWB)无线通信技术辅助IMU定位,并利用联邦导数容积卡尔曼滤波器对航向误差进行修正。2)部署多IMU约束误差。李兴等[11]利用IMU阵列实现位置解算,并根据漂移误差情况自适应融合阵列数据,提高行人导航精度;李晓东等[12]在行人双腿关节处部署6个IMU,利用人体关节信息构建双足约束模型来抑制航向误差。3)利用外界信息实现高度解算。陆传华等[13]利用气压计更新高度信息,并利用地图信息限制高度更新的区域,最终实现行人高度解算;俞敏杰等[14]在大楼内部署信号源,根据接收来自各楼层信号强度差值的分布情况,确定行人所在楼层。

上述方法可以有效修正系统误差,但引入其他传感器增加了系统的复杂性和成本,并且导航系统只能在特定场景内工作,不具有普遍适用性。基于以上问题,本文提出了一种基于足间距离信息辅助的行人惯性定位算法。该算法利用随机森林算法对行人的动作模式进行识别,并采用基于时间因子修正的自适应零速检测算法,实现了对零速区间的高精度划分;在平面运动中,通过利用超声测距的足间距约束模型修正导航误差,有效限制了航向误差的发散;而在楼梯运动情况下,采用基于台阶高度和足间距信息的高度解算算法,实现了对高度信息的准确解算,有效避免了高度的发散。在保证系统自主性、便携性和普遍适用性的同时,可以有效提高双足IMU导航系统的定位精度。

1 基于足间距信息的位置解算

1.1 基于超声测距的足间距约束模型

INS根据IMU采集的行人运动数据,建立“东北天”导航坐标系(n系)和“右前上”载体坐标系(b系),利用捷联惯导解算获得行人导航信息,导航信息解算方程如下[15]

(1)

将根据式(1)得到的任意时刻行人左右脚导航状态表示为

(2)

(3)

图1 行人单次迈步足部示意图

(4)

(5)

(6)

(7)

当任意时刻左右脚位置关系不满足足间距约束模型时,利用式(7)可以得到双足位置误差为

(8)

考虑到行人运动的每个步态周期中都存在一只静止脚与一只摆动脚,利用零速修正算法修正静止脚所处的导航子系统,并将静止脚作为参考基准,利用足间距约束模型对摆动脚所处的导航子系统进行修正。在行人行走过程中双足交替进行摆动,双足IMU导航系统将不断地对行人双脚导航子系统交替进行零速修正和足间距约束修正,最终提高双足IMU导航系统的定位精度。基于卡尔曼滤波的双足导航子系统的误差修正模型如下

(9)

(10)

(11)

其中,an表示n系下的加速度。

(12)

(13)

1.2 基于台阶高度与足间距的高度解算算法

图2 行人上下楼梯运动示意图

在已知台阶高度信息的情况下,利用单次迈步中初始位置与终点位置间的足间距信息,求取单次迈步跨越台阶数,便可以实现行人的高度解算。设单次迈步的起点和终点平面位置坐标分别如式(14)所示

(14)

由行人单次迈步的前后零速区间平面位置,可以计算出单次迈步跨越台阶数为

(15)

其中,N为单次迈步跨越台阶数;thstair为跨越台阶数判断阈值;floor(·)为向下取整函数。当行人处于上楼运动时,N值为式(16)计算值;当行人处于下楼运动时,N值为式(16)计算值的相反数,具体表达式为

(16)

利用单次迈步跨越台阶数、台阶高度信息以及迈步总数便可以得到行人的高度信息

(17)

其中,hk为行人当前高度信息;i为迈步序号;n为总迈步数;Ni为第i次迈步跨越的台阶数。

2 足间距辅助IMU行人定位系统

2.1 总体框架

本研究在行人双脚脚跟位置安装IMU与超声测距模块,采集行人运动数据,利用捷联惯导算法解算行人导航信息。由于行人在建筑物内存在平面运动与上下楼运动,因此首先利用随机森林算法对行人运动模式进行识别,当行人处于平面运动时,根据超声测距模块测得的双足相对距离信息,建立足间距约束模型,并结合零速修正算法,利用卡尔曼滤波修正双足导航误差;当行人处于上下楼运动时,利用台阶高度信息进行高度解算,最终实现三维导航定位。超声测距辅助双足IMU行人定位系统的总体框架如图3所示,其中γ为零速修正阈值,ΔT为时间因子。

图3 总体框架

2.2 零速检测

如图4所示,行人行走过程中足部特征由运动区间和静止区间交替构成,在静止区间中,足部完全接触地面,此时足部理论速度为零,直到再次进入运动区间[16]。

图4 足部运动特征

零速修正算法将静止时捷联惯导解算速度作为误差观测量,利用卡尔曼滤波修正导航误差[17]。而使用零速修正算法的前提是正确地划分足部运动状态,本文采用广义似然比检测法(generalized likelihood ratio test, GLRT)对足部运动状态进行划分[18]

(18)

(19)

本文提出基于时间因子修正的零速检测算法,用来消除初步划分中的误判情况。具体步骤为:

1)根据运动模式选择γ阈值和时间因子ΔT,利用GLRT零速检测算法对行人足部状态进行初步划分。将运动区间和静止区间分别定义为Cmove,i和Cstatic,i,其中Cmove,i表示第i个运动区间内包含的数据数量,Cstatic,i表示第i个静止区间内包含的数据数量。

2)将各区间的数据数量与时间因子ΔT进行比较。若区间的数据数量均大于时间因子,则表示区间划分正确,反之,若某一区间的数据数量小于时间因子,则表明该区间划分错误。

3)划分错误区间进行状态翻转,即将伪静止区间转换为运动区间,伪运动区间转换为静止区间。对行人各步态区间状态都进行时间因子检测,减小误判和漏判出现的次数,提高零速区间检测精度。

2.3 运动模式识别

行人在室内运动时,主要运动模式包括静止、行走、跑步及上下楼等。为实现不同运动场景算法以及零速检测阈值的动态调整,引入随机森林算法对行人运动模式进行识别。随机森林算法是以决策树为基础的集成学习模型,其原理为利用boots-trap技术获得多个样本子集,并使用不同样本子集构建决策树,当输入待分类数据时,各决策树共同投票实现随机森林分类的算法[19]。分类示意图如图5所示,图中数字为各动作模式的类别标签。

图5 运动模式分类示意图

本文使用的IMU数据输出频率为200 Hz,采集的原始数据表达式为

(20)

其中,Sorig表示原始运动数据;ax,ay,az分别为x轴,y轴,z轴的加速度数据;ωx,ωy,ωz分别为x轴,y轴,z轴的角速度数据。

在采集完测试人员的运动数据后,采用加窗法对数据进行分割,窗口宽度为140,重叠率为50%,并提取三轴加速度和三轴角速度的数据特征值,降低识别和分类的复杂性,提高效率,由此得到数据集D,数据集D的表达式为

D=(F,Y)={(fi,yi)},i=1,2,…,m

(21)

其中,m为样本个数;Y表示样本对应的运动类型,yi表示样本i的类型;F表示运动数据的特征矩阵,具体表示为

(22)

其中,F为样本的特征向量,共7组运动数据时域特征;max,min分别为最大值和最小值;μ,σ分别为均值和方差;kurt,skew分别为峰度和偏度;IQR为四分位距。利用样本的特征向量对随机森林模型进行训练,实现行人运动模式分类。

根据运动模式分类情况,选取相应的算法进行导航解算。针对平面运动,采用1.1节提出的基于超声测距的足间距约束模型,并结合零速修正算法,抑制INS误差;针对上下楼运动,采用1.2节提出的基于台阶高度与足间距的高度解算算法,计算行人高度信息,最终实现高精度行人三维惯性定位系统。

3 实验验证

3.1 实验平台搭建

本文选择大楼室内平面和楼梯走道作为算法验证环境,实验人员装备本文搭建的超声辅助双足IMU定位系统,按照规划路线行进,双足IMU定位系统示意图如图6所示。

图6 双足IMU定位系统

系统中IMU为实验室自研的惯性测量单元,输出频率为200 Hz,主要性能参数如表1所示;超声测距模块为逐飞科技公司的“有来有去”超声模块,对于短距测距的误差情况如表2所示。

表1 IMU主要性能参数

表2 超声测距误差结果

双足IMU和超声测距传感器利用AT32F4单片机实现数据采集与存储。超声波测距模块进行短距测量时,平均测量误差为6.4 mm,测距精度误差为2.3%,该误差能够满足本文搭建系统的精度需求。

图7 足间距离测量结果

3.2 实验验证与分析

设定实验人员所处的起点坐标为(0,0),行走路线的起点与终点坐标重合,使用闭环误差(起点和终点的欧式距离)和闭环误差占比(闭环误差占总路程的百分比)作为衡量算法效果的依据,实验场地如图8所示。

图8 实验路线

实验1:平面闭环行走实验。为验证本文超声波足间距约束算法的有效性,选择折返路线作为实验的基准路线,以星形点作为起点,以棱形点为折返点,返回起点,行进距离总计184.00 m,实验结果如图9所示,轨迹误差如表3所示。

表3 平面定位误差比较

图9 实验1运动轨迹

图9中浅蓝色线与绿色线为零速修正后的左脚与右脚轨迹,在终点位置双脚平均闭环误差为2.66 m,误差占比为1.44%。蓝色线与红色线为经本文提出的超声波足间距约束算法修正后得到的左脚与右脚轨迹,在终点位置双脚平均闭环误差为1.17 m,误差占比为0.64%,导航误差与零速修正算法相比下降了55.56%。

实验2:上下楼实验。为验证基于台阶高度与足间距的高度解算算法的有效性,选择实验1中的折返路线作为基准路线,实验人员以楼梯A圆形点作为起点,经过楼梯A从二层到达四层,沿路线到达楼梯B,经过楼梯B从四层达到二层折返点,最后沿路线回到起点,该路线二层到四层总高度为7.52 m。高度解算结果如图10所示,高度误差结果如表4所示,三维轨迹如图11所示,三维定位误差如表5所示。

表4 高度解算误差比较

表5 三维定位误差比较

图10 高度解算结果

图11 实验2三维运动轨迹

图10中红色线为本文的高度解算算法得到的高度结果,其终点高度为0.07 m,高度解算平均绝对误差为0.06 m,每层楼的高度解算误差比较稳定;蓝色线为利用零速修正解算算法得到的高度结果,由于加速度计数据积分存在累计误差,导致高度漂移,从第三层到达第四层解算高度误差达到最大,由第四层到达第三层时,累计误差正负抵消,高度误差达到最小,其终点高度为0.43 m,高度解算平均绝对误差为0.41 m。绿色线为利用气压计相对高度信息和零速修正进行解算得到的高度结果,由于气压计易受到外界环境因素影响,导致高度解算出现误差,每层楼的高度解算误差相差较大,其终点高度为0.35 m,高度解算平均绝对误差为0.17 m。基于台阶高度与足间距的高度解算算法相较于零速修正和零速修正与气压计联合算法高度解算平均绝对误差分别下降了85.37%和64.70%。

图11中红色轨迹为本文算法得到的三维定位结果,三维定位误差占总路程的0.39%,相较于零速修正与气压计联合算法,误差下降了77.06%,行人在大楼内的运动轨迹,航向稳定,轨迹高度误差小,表明该算法可以有效修正航向误差,提高行人在大楼内的高度计算精度,实现高精度行人室内三维定位。

4 结论

本文针对基于纯惯性器件的行人导航系统误差随时间累积致使定位精度严重下降的问题,提出基于足间距信息辅助的行人三维惯性定位算法。在平面运动和上下楼运动分别进行对比实验,实验数据显示:

1)行人围绕平面闭环路径行走,本文算法解算的双足导航系统闭环误差占比平均值为0.64%,闭环误差占比与零速修正算法相比下降55. 56%,证明足间约束模型能有效减小航向误差。

2)在上下楼高度实验中,本文算法解算的高度信息绝对平均误差为0.06 m,与零速修正算法和零速修正与气压计联合算法解算高度信息相比,平均绝对误差分别下降了85.37%和64.70%,证明通过台阶高度与足间距信息解算高度能有效减少高度漂移。

3)本文提出的算法能在保证系统自主性与适用性的同时,有效提升足绑式IMU三维INS定位精度,能够实现导航误差在总路程的0.50%以内的三维定位,具有良好的工程应用价值。