基于自适应PDR补偿的UWB/MIMU组合行人定位算法

王 伟,马晞茗,王敬民,佘啸天,黄 平

(哈尔滨工程大学智能科学与工程学院,黑龙江哈尔滨 150001)

0 引言

目前基于超宽带(UWB)[1]和微型惯性测量单元(MIMU)器件[2]的组合方案在行人室内定位领域取得了较大进展。UWB可以修正MIMU的累计误差,MIMU则可以解决UWB在非视距环境下定位结果不可靠的问题。杨海等[3]在UWB/MIMU组合定位模型基础上引入决策树容错判断机制,进而构建出容错组合定位系统,能够较好检测出UWB定位系统出现的定位数据丢失,但尚未考虑到噪声先验信息错误对定位精度的影响。S.Zhang等[4]采用最小方差无偏有限脉冲响应算法,从噪声测量中获得准确的位置和速度估计,提高了噪声先验信息错误情况下的定位精度,但该算法在零速修正不准确时性能受到较大影响。李荣冰等[5]将整个系统分为零速修正、UWB/MIMU、MIMU/磁力计3个子滤波器模型,提高了系统的容错性,但是当磁力计失效时,整体定位效果受到的影响较大。Q.Tian等[6]提出一种基于行人相对于基站的行走方向角与阻塞程度关系的模型,一定程度上降低了对磁力计的依赖,但该算法对结构性强的室内环境依赖性较强,同时PDR计步准确性较差时定位精度受干扰较大。K.Yu等[7]针对航迹丢失问题,提出一种新的粒子重置方法,利用UWB的距离测量值计算粒子群的均方根误差,提高了UWB/MIMU行人组合导航系统的稳定性,但UWB信号仅存在于局部区域导致该算法存在一定的局限性。Y.Wang等[8]提出一种基于UWB和惯性测量单元(IMU)紧组合的定位算法,提高了定位精度,但算法计算量较大,且PDR计步精准度有待提升,从而导致定位延迟较大。柯熙政等[9]基于卡尔曼滤波算法将MIMU和磁力计进行航向估计,提高了PDR过程中的航向角精度,但是行人航向角在定位中后期容易偏离真实的航向。

由此可知,现有的PDR以及UWB/MIMU组合方案存在着定位不连续和定位误差累计的问题。针对该问题,本文提出了一种基于自适应PDR补偿的UWB/MIMU组合定位方案。首先,对传统PDR计步方案[10]进行改进,引入步行周期阈值条件和后验策略,提出一种结合阈值和计步周期的计步方案,降低了对阈值设置的准确性要求,并消除了伪波峰和伪波谷对计步结果的影响; 引入ZUPT和ZARU组合的方法辅助惯导来修正误差,一定程度上抑制了MIMU在行走过程中的速度位置发散;将UWB和MIMU获取的定位信息统一至同一坐标系下,实现了UWB和MIMU的速度位置组合,并利用组合后的定位结果对PDR计步过程中识别的行人步长进行训练。当UWB信号丢失时,则利用训练的步长进行PDR得到位置量测信息,然后与MIMU进行速度位置组合,计算得到组合系统在UWB信号丢失时的定位精度。最后,通过具体实验验证了本文提出的方案对室内环境下行人定位的精确度有了显著提高。

1 基于MIMU的多约束PDR计步方案

1.1 定位系统结构设计

当UWB信号接收正常时,利用UWB为MIMU提供位置参考信息,并采用卡尔曼滤波进行速度位置组合,利用组合后的定位结果进行行人PDR步长的在线训练;当UWB信号受干扰丢失时,则利用UWB信号丢失前训练的步长结果辅助MIMU进行PDR,然后将PDR的结果与MIMU进行速度与位置的组合,组合定位系统的整体结构如图1所示。

图1 组合定位系统整体结构

1.2 PDR计步方案设计

对于计步环节而言,当行人从静止状态转为行走状态时,行人第1次迈步过程中z轴加速度计数据往往会出现幅值较小的情况。此时如果计步方案所选阈值太大,则可能会出现漏计现象;若阈值太小,则后续由于伪波峰伪波谷的干扰,可能出现多计现象。本文设计了一种多约束的PDR计步方案,可以在设置阈值较小的情况下实现准确地计步。图2为第i-1步PDR计步流程图。其中约束条件包括:极值条件、峰值条件以及后验周期阈值条件。

图2 第i-1步PDR计步流程图

约束条件的具体含义为:

(1)极值条件:设置1个2N+1的滑动窗口,若第N个元素大于滑动窗口内的其他元素,则该元素为极大值;同理,若第N个元素小于滑动窗口内的其他元素,则该元素为极小值。

(2)峰值条件:当满足极值条件的点大于波峰阈值或小于波谷阈值才可能是真正的波峰或者波谷。

(3)后验周期阈值条件:满足极大值条件和峰值条件的点可能是伪波峰,此时需计算当前波峰与上一个波峰之间的时间差,若大于步长阈值,则上一个波峰是上周期内真正的波峰;同理,若满足极小值条件和波谷阈值,则计算当前点与上一个波谷之间的时间差,若大于步长阈值,则上一个波谷可确认是真正的波谷。

若当前状态为波谷,则需寻找符合极大值条件和峰值条件的波峰。若在寻找波峰的过程中检测到波谷,则比较该波谷与当前状态波谷峰值的大小关系。若目前的波谷值更小,则更新当前状态的波谷值,否则继续寻找波峰。找到波峰后,则计算当前检测到的波峰与上一个波峰之间的时间间隔,若时间间隔大于周期阈值,即可确认上一个波峰是上一步行周期内的实际波峰,并把系统切换为寻找波谷的状态。

若当前状态为波峰,则需要寻找符合极值条件和峰值条件的波谷。若寻找波谷过程中又检测到波峰,则将该波峰与当前状态的波峰进行比较,若当前的波峰峰值更大,则更新当前的波峰值,否则继续寻找波谷。找到波谷后,则计算当前检测到的波谷与上一个波谷之间的时间间隔,若时间间隔大于周期阈值,则可确认上一个波谷是上一步行周期内真正的波谷。至此,上一步行周期内真正的波峰和波谷都已被确认,则计步加1,并把系统状态重新更新为寻找波峰的状态,然后开始下一步寻找。

通过上述过程,可在波峰和波谷阈值设置较小时实现伪波峰和伪波谷的剔除,阈值的判断方法如式(1)所示:

(1)

本文通过长度为W的时间窗计算波峰阈值Tcre与波谷阈值Ttro。

(2)

式中:aN为窗内竖直加速度集合;maxloc(aN)与minloc(aN)分别为竖直方向线性加速度的极大值与极小值集合;mean和var分别为窗内竖直加速度集合中所有数值的平均值和方差;α为权重因子。

设置波峰阈值为1.5g,波谷阈值为0.5g。

以某次实测结果为例,实验过程为从静止开始,每次走20步则停顿静止一段时间,一共4次。图3为整体步态检测结果。其中纵坐标比力(fb)表示载体相对惯性空间的绝对加速度和引力加速度之差。根据图4可知,在由静止变为运动的第一步过程中,z轴的加速度计数据幅值明显小于后续步行过程中的伪波峰幅值,但仍能被正确识别为第一步的波峰;第4、5、6、7、9步过程中出现的伪波谷也能被正确剔除。整个实验过程中一共行走了80步,实验的计步结果也为80步。实验结果表明本方案可准确实现PDR计步。

图3 整体步态检测结果

图4 局部步态检测结果放大图

2 自适应PDR补偿的UWB/MIMU组合模型

2.1 ZUPT与ZARU修正误差

在零速检测过程中,由于MIMU惯性器件的误差较大,因此零速修正在行人导航过程中非常重要。本文通过ZARU将陀螺仪实际角速度测量值与零角速度观测量作差后的结果作为量测量,进而进行卡尔曼滤波的方法修正偏航角误差。

在着地静止阶段有式(3)与式(4)成立:

(3)

(4)

根据式(3)、式(4)以及SINS的误差方程[10-11]可知,基于ZUPT和ZARU组合的连续卡尔曼滤波的状态方程为:

(5)

量测方程为:

(6)

对上述连续时间卡尔曼滤波方差进行离散化处理。离散化方程如式(7)与式(8)所示:

Xk=Φk/k-1Xk-1+Γk-1Wk-1

(7)

Zk=HkXk+Vk

(8)

式中:Xk为k时刻的状态;Xk-1为k-1时刻的状态;Zk为k时刻的量测;Φk/k-1为k-1时刻到k时刻的系统状态转移矩阵;Γk-1为系统噪声驱动阵;Wk-1为方差为Qk的系统噪声;Hk为量测矩阵;Vk为方差为Rk量测噪声。

若零速检测结果为非静止,则只进行时间更新操作,利用式(9)与式(10)可求状态的一步预测以及协方差阵的一步预测:

(9)

(10)

若零速检测结果为静止,则需要在进行时间更新的基础上进一步量测更新,利用式(11)～式(13)对状态的一步预测进行加权,对状态变量的值以及协方差矩阵进行更新:

(11)

(12)

(13)

为避免校正过度及提高反馈校正后的平滑度,本文采用部分反馈策略,具体表示为:

(14)

(15)

2.2 室内定位信息坐标统一

由于传统的UWB/MIMU组合方案需要额外考虑UWB所在室内坐标系、东北天坐标系和MIMU载体系与东北天坐标系之间的关系,一定程度上影响了整个定位方案的实用性,因此本文通过将室内定位结果统一至室内局部坐标系,有效避免了对于东北天坐标系的讨论与分析,从而提高了整个室内定位系统的实用性。图5为系统坐标系示意图。

图5 坐标系示意图

在图5中,l系为用于室内定位的UWB所在的室内局部坐标系;N系为行人初始静止时刻MIMU载体坐标系重合的导航坐标系。首先利用UWB和MIMU单独进行定位,分别获取初始位置在l系下的坐标(x0,y0)和N系下的坐标(0,0),运动后的新位置在l系下的坐标(x1,y1)和N系下的坐标(Δx,Δy),则有:

(16)

将式(16)代入图5坐标系中则有如下方程成立:

(17)

解得:

(18)

式中:ψ为N系相对于l系的偏航角,以符合右手系的方向为正方向,根据上式即可确定ψ值;(x2,y2)为下一时刻的坐标;Δx、Δy为x坐标与y坐标的变化。

至此,可以将UWB和MIMU的定位结果在UWB室内局部坐标系l系与MIMU导航坐标系N系之间互相转换。

2.3 UWB/MIMU组合模型

在整个行人运动过程中会出现3种情况:仅存在位置参考信息的时刻、仅存在速度参考信息的时刻以及位置和速度参考信息同时存在的时刻。

当检测到没有任何量测信息时,仅做卡尔曼滤波时间更新;当检测到ZUPT速度参考信息时,进行速度组合的卡尔曼滤波量测更新;当检测到位置参考信息时,则继续进行位置组合的卡尔曼滤波量测更新,整个模型的执行流程如图6所示。

图6 UWB/MIMU组合模型执行流程

(19)

(20)

(21)

(22)

3 实验结果与分析

本文使用传感器的加速度计与陀螺仪部分参数如表1所示。

表1 加速度计与陀螺仪部分参数

在实验中,将MIMU模块安装在行人足部,且z轴指向天向,在后续实验中仅对z轴的加速度计数据进行分析。本文基于FIR滤波对z轴加速度计的原始数据进行预处理,设计了一个25阶的FIR低通滤波器,采样频率fs为100 Hz,截止频率fc为10 Hz。图7为FIR滤波后的z轴加速度计数据。

图7 加速度计三轴比力数据

根据图7和图8可知,z轴加速度计的数据经过FIR滤波后,在去除了高频扰动的同时较好地保留了原始数据的峰值特征和零速特征,进而便于后续PDR计步算法和零速检测算法的执行。

图8 FIR滤波后的z轴加速度计数据

3.1 MIMU数据修正

实验场景如图9所示,在无卫星信号且未使用磁力计数据的情况下,直接以MIMU初始时刻的载体坐标系作为导航坐标系进行行人导航解算,并利用ZUPT和ZARU进行修正。

图9 实验场景图

具体实验步骤为:基于初始静止时刻的MIMU载体坐标系,沿着长为134.52 m、宽为86.15 m的矩形行走一圈后回到起始点,接着沿着反方向继续行走,最终形成如图10所示的运动轨迹,其中横纵坐标分别表示实际实验中x和y方向的坐标值,其闭合误差为2.59 m,占总行走距离的0.29%。

图10 定位轨迹结果

经过ZUPT和ZARU的修正后,运动轨迹整体趋势正确。但在行走过程中,存在5～10 m的位置偏差,分别占各边长行走距离的5.80%和7.43%,这是由于MIMU器件的精度较低,长时间行走会累计较大的位置误差。图11(a)中φ表示航向角,图11(b)中θ表示俯仰角,图11(c)中γ表示横滚角。图12(a)～图12(c)中的vNx,vNy,vNz分别表示载体x,y,z方向的速度。根据图11以及图12可知,经过ZUPT修正后的俯仰角和横滚角更加平稳,经过ZARU修正后的偏航角平稳度也有所提升,较好地抑制了速度误差的发散。

(a)航向角结果对比

(b)俯仰角结果对比

(c)横滚角结果对比图11 姿态角对比结果

(a)x方向速度

(b)y方向速度

(c)z方向速度图12 速度计算结果

由此可知,当行人处于未知绝对方向的室内环境时,基于本文提出的方案,可以较为准确地得到连续的行人相对运动信息,从而降低了行人室内导航对磁力计的依赖。

3.2 PDR补偿UWB/MIMU组合定位实验

本实验中MIMU模块安装在行人足部,Y轴朝前,X轴朝右。UWB定位标签则安装在行人帽子处,采用的MIMU数据为100 Hz,包括电脑接收时间、片内时间、三轴加速度计数据和三轴陀螺仪数据。UWB数据为10 Hz,标签侧UWB数据通过串口传到计算机。采集完数据后统一将时间转换为时间戳,并将UWB数据与最近时间的MIMU数据进行匹配,以实现UWB与MIMU数据的软件同步,实验场景如图13所示,实验的参考点和参考轨迹如图14所示,行人沿逆时针方向运动直至遍历完整个大厅,形成的运动轨迹为矩形。

图13 实验场景图

图14 实验参考点和参考轨迹示意图

3.2.1 ZUPT和ZARU仿真验证

利用轨迹发生器模拟行人运动,并输出对应的位置、速度、姿态信息以及MIMU的角速度和比力数据。设置行人步长为0.7 m,步行周期为0.8 s。行人每一步的运动包括0.2 s匀加速运动、0.2 s匀减速运动以及0.4 s零速阶段,每次行人转向时则静止5 s,图15显示了行人运动的仿真轨迹。

图15 行人运动的仿真轨迹

图16 导航仿真轨迹对比图

(a)航向角误差对比

(b)俯仰角误差对比

(c)横滚角误差对比图17 姿态角误差对比图

(a)x方向速度误差对比

(b)y方向速度误差对比

(c)z方向速度误差对比图18 导航速度误差对比图

图17(a)中δΨ表示航向角误差,图17(b)中δθ表示俯仰角误差,图17(c)中δγ表示横滚角误差。带有圆圈的曲线为惯导解算结果,无圆圈曲线为ZUPT+ZARU得出结果。图18(a)～图18(c)中δve、δvn、δvu分别表示x、y、z方向的速度误差。结果表明,经过ZUPT和ZARU组合方法修正后,行人运动的轨迹与参考轨迹几乎重合;偏航角误差从4°下降到2°,较好地抑制了MIMU速度误差的发散。北向位置误差0.1 m,东向位置误差0.04 m,天向速度误差与SINS相比发散速度得到了明显的减缓。

3.2.2 PDR辅助下UWB/MIMU组合方案实验

为验证组合方案的定位性能,分别测试仅使用ZUPT、仅适用UWB以及组合方案情况下的定位轨迹,图19为室内环境下各方案的定位轨迹对比图。

图19 室内环境下各方案的定位轨迹对比图

根据图19可知,仅使用ZUPT的MIMU定位结果虽然形状正确,但是由于位置误差逐渐积累,定位结果慢慢偏离真实轨迹;仅适用UWB模块的定位结果长期稳定性较好,但是定位结果跳变较为严重。与仅使用MIMU模块的定位结果相比,基于UWB/MIMU的组合导航方案修正了MIMU的位置累计误差;与仅使用UWB模块的定位结果相比,基于UWB/MIMU的组合导航方案有效提高了定位频率,降低了UWB定位结果的跳变。实验结果表明,本文提出的方案是有效的。

整个模拟信号丢失过程持续的时间为6 s,图20显示了信号丢失前的计步结果和步长估计结果。

(a)PDR计步结果

(b)PDR步长估计结果图20 UWB信号丢失前MIMU计步结果和步长估计结果

在模拟UWB信号丢失前,行人共行走67步,图20(a)中计步结果为66步,漏记步数为1步。在图20(b)中,存在4种步长结果,可按步长大小分为:短步、单步、长步、双步。

利用线性步长模型对UWB信号丢失前的步长模型进行估计,线性步长模型为

SLi=A+B·Ti+C·Pi

(23)

式中:SLi为第i步行人行走的步长;Ti为从第i-1步到第i步所用的时间;Pi为加速度计幅值;A、B以及C为待估计的步长模型。

本文基于最小二乘法对UWB信号丢失前的步长模型进行估计,得到的线性步长模型为

SLi=0.832 6-0.319 2×Ti+0.033 0×Pi

(24)

结合MIMU模块输出的航向来推算行人航位,将航位推算得到的位置用于MIMU模块的速度位置组合。图21显示了仅使用UWB模块、加入PDR补偿的组合方案以及无补偿的组合方案的定位轨迹对比图。

图21 各方案定位轨迹对比图

根据图21可知,当UWB信号丢失后,仅使用MIMU模块进行导航得到的轨迹定位误差逐渐增大,偏离真实轨迹。在PDR辅助下的MIMU定位轨迹误差增大趋势得到了抑制,轨迹更接近真实轨迹,表明本文提出的组合方案是有效的。

4 结论

本文提出了一种基于自适应PDR补偿的UWB/MIMU组合行人定位方案,较好地解决了现有方案存在的定位不连续和定位误差累计问题。首先通过ZUPT和ZARU组合的方法进行零速修正,有效抑制了速度误差的发散,使得航向角误差降低了50%;然后引入一种自适应滤波模型,利用线性步长模型结合MIMU模块输出的航向进行行人航位推算,较好地抑制了行人轨迹的发散,提高了行人定位的连续性。在未来的工作中,将在所提方案的基础上,进一步利用UWB信息来修正航向角的误差,同时考虑量测噪声方差阵的变化,进一步提高UWB/MIMU组合导航系统的定位精度。