基于UWB/PDR组合的室内行人定位

郭 崴，张轩雄

(上海理工大学 光电信息与计算机工程学院，上海 200093)

随着通信技术的发展与无线网络的普及，基于位置的定位服务(Location Based Services，LBS)受到广泛关注。室外定位技术以全球定位系统为基础，包括美国GPS定位系统、俄罗斯GLONASS定位系统、我国自主研制的北斗定位系统等。然而，卫星信号难以穿透墙体，且全球定位系统在复杂环境下精度较差，导致卫星定位系统难以应用于室内[1]。针对这一问题，各种室内定位技术应运而生。

室内定位技术在室内行人导航[2]、室内机器人定位[3]、室内无人机定位[4]等领域有着广泛的应用前景。由于室内环境的复杂性，室内定位技术也多种多样。用于室内定位的感测信号有WiFi[5]、ZigBee[6]、RFID[7]、超宽带(Ultra Wide Band，UWB)、蓝牙[8]、超声波、惯性导航等[9]。

UWB定位系统作为一种无线定位系统具有精度高、功耗低等优点[10]，能够提供厘米级的定位精度。但是，UWB定位系统定位性能常受到信号非视距(None Line of Sight，NLOS)的影响。严重的NLOS将导致定位精度下降，甚至丢失位置信息。为了抑制NLOS对UWB定位系统的影响，需要将其他定位信息进行融合。文献[11]使用UWB融合视觉里程计来提高定位精度，但该方法计算复杂，难以满足实时性需求。

基于微机电惯性导航单元(Micro-Electro-Mechanical-System Inertial Measurement Unit，MEMS-IMU)的行人航迹推算(Pedestrian Dead Reckoning，PDR)是一种自主定位导航系统，具有短期精度高的优点，但其长期定位精度存在较大的累积误差。PDR算法使用惯性传感器，通过对行人的步态进行检测来估计行人的步长，并可结合方向等信息对行人位置进行累加推算，且不受NLOS影响。文献[2]利用磁力计修正行人航向，并结合零速更新(Zero Velocity Update Procedure，ZUPT)来修正行人位置，但是其总体定位精度依然在米级，无法满足高精度定位要求。

室内单源定位导航系统无法满足复杂室内环境定位需求，因此多源信息组合定位逐渐成为当前研究的热点。文献[12]利用采集的室内环境地图信息，提出利用地标修正PDR，对定位精度有一定提升，但地图环境数据库的建立及维护工作量较大。文献[13]采用卡尔曼滤波对WiFi和IMU进行融合，但是由于两种方式的定位精度都在米级，整体定位精度仍不高。根据UWB和PDR系统的优缺点，采取适当的融合方式将两种定位方法进行融合，可以有效解决复杂室内场景下的定位问题。

为了提高定位系统的稳定性和精度，本文提出了基于行人航迹推算与UWB组合的室内定位方法。PDR算法使用加速度传感器数据进行步态检测以及步长估计，然后计算出行人的相对位置变化。UWB通过测量移动节点与参考节点之间的距离来计算行人的绝对位置。最后，本文利用卡尔曼滤波算法对多源信息作滤波修正。实验结果表明，所提方法的定位精度和系统稳定性都有明显改善。

1 UWB定位

UWB技术是一种无载波通信技术，具有穿透性好、功耗低、测距精度高等优点。UWB定位系统基于无线测距技术[14]，其定位过程可以分成两步：距离测量和位置解算。

本文采用基于到达时间(Time of Arrival，TOA)的测距方法，使用双边双向测距方法(Double-Sided Two-Way Ranging，DS-TWR)[15]，测距流程如图1所示。

图1 双边双向测距流程Figure 1. Double side two-way ranging

该方法能有效抑制设备时钟漂移引起的测距误差，且不需要设备间时钟同步，简化了系统实现过程。实现完整的1次DS-TWR测距需要进行3次通信：首先设备A向设备B发送测距数据包，发起1次测距请求；然后设备B接收到数据包后发送响应数据包；最后设备A发送结束数据包，完成1次完整的测距过程。所有数据包的收发时间戳会被设备内部的高精度时钟记录下来：troundA为设备A发送请求数据包到接收到响应数据包的时间；treplyB为设备B接收到请求数据包到发送响应数据包的时间；treplyA为设备A接收到响应数据包到发送结束数据包的时间；troundB为设备B发送响应数据包到收到结束数据包的时间；TOF为电磁波飞行时间(Time of Flight，TOF)。

根据图1，设备A与设备B之间电磁波飞行时间tTOF由式(1)计算得到

(1)

使用式(2)计算出设备A(移动节点)与设备B(参考节点)之间的距离d

d=c×tTOF

(2)

其中，c为电磁波在空气中的飞行速度。

(3)

(4)

将式(4)写成如下矩阵形式

HX=b

(5)

其中，

X为行人的估计位置。式(5)的最小二乘解为式(6)。

X=(HTH)-1HTb

(6)

2 行人航迹推算

室内环境中，高精度惯性器件体积大，不利于携带，而MEMS-IMU体积小，功耗低，方便集成，易于携带。但是，传统MEMS-IMU精度低，如果采用传统的基于积分的捷联惯导[16]定位方式，会导致定位结果迅速发散。行人航迹推算通过检测行人步态可以有效抑制累积误差迅速增大的问题。该方法基于确定的起始位置，利用步长和航向角推算下一时刻行人的相对位置。行人航迹推算算法计算行人相对位置包括3个方面：步态检测、步长估计以及航向推算。

步态检测方法包括基于角速度和基于加速度两种方法。基于角速度的步态检测使用安装在行人脚部或手部的角速度传感器，通过检测角速度周期变化来检测行人行走步数。基于加速度的步态检测使用加速度传感器，相较于角速度步态检测，传感器安装位置更加灵活。本文采用基于加速度的步态检测方法。此外，为了避免传感器安装位置对步态检测的影响，使用三轴加速度模值进行步态检测。加速度模值可以使用式(7)进行计算。

(7)

其中，axi、ayi、azi为各样点三轴加速度的值。

图2 基于加速度的步态检测Figure 2. Step detection based on acceleration

如图2所示，行人在正常行走时，加速度数据会呈现周期性变化，加速度峰值点为行人脚落地的时刻，即一步步伐。本文采用常用的峰值检测方法来检测行人状态，并通过设定适当的加速度以及周期阈值来提高检测精度。根据文献[17]，峰值检测方法的检测精度可以达到98%以上。

步长可以通过加速度数据进行估计，本文应用文献[18～19]中的Weinberg模型进行步长估计

(8)

其中，S(k)为第k步长；Amax(k)和Amin(k)是第k步中的最大和最小加速度值；K为常数，可以通过离线数据训练得到。

PDR中的航向可以使用下面的模型进行推算

ψ(k)=ψ(k-1)+ΔT×∑ω

(9)

其中，ψ(k)和ψ(k-1)分别为k与k-1时刻的航向，初始航向在初始时进行校准；ΔT为陀螺仪采样数据间隔；∑ω为k-1到k时刻的陀螺仪输出值之和。

根据上文得到的步长和航向信息，并结合当前已知的位置，可以出推算当前行人的位置为式(10)。

(10)

3 基于卡尔曼滤波的数据融合算法

本文提出的UWB/PDR组合室内定位系统框图如图3所示。定位系统由惯性器件和UWB模块组成，其中惯性器件包含三轴加速度计和三轴陀螺仪，UWB模块包含3个参考节点和1个移动节点。

图3 UWB/PDR组合定位系统框图Figure 3. Block diagram of the UWB/PDR integrated system

根据图3，PDR系统通过读取惯性器件数据预测行人每一步的步长和航向，而UWB系统通过TOA测距得到节点间距离，经过NLOS检测剔除异常测距值；然后，使用最小二乘法得到行人位置；最终，使用卡尔曼滤波进行融合，输出融合后的位置。

3.1 非视距检测

UWB系统的测距精度易受非视距影响，室内的各种障碍物都容易产生NLOS误差。这种误差不是随机噪声造成的，使用卡尔曼滤波时，可能导致滤波器发散。为了消除NLOS影响，本文提出一种非视距检测方法，在融合滤波前过滤掉受到NLOS干扰的UWB测距数据，以便提高系统稳定性。

通常行人行走速度有限，UWB系统可以以较高的速率进行位置更新。在UWB两次测量间隔时间(40 ms)内，行人移动距离较小。假设UWB测距值不会发生突变，如果满足如下条件，则认为存在NLOS影响

min(αi，αi+1，αi+2)>η

(11)

其中，αi=TOFi-(TOFi-1+TOFi-2+TOFi-3)/3为第i次测量值与其前3次测量均值的差。根据式(11)，如果检测到测量结果大于η则认为进入NLOS区域；如果满足式(12)，则认为离开NLOS域。

max(αi，αi+1，αi+2)<ρ

(12)

3.2 卡尔曼滤波

在排除NLOS带来的影响后，本文采用基于卡尔曼滤波的数据融合算法对UWB和PDR信息进行融合，以便得到更优的定位结果。根据卡尔曼滤波准则，定义系统的状态转移方程为

X(k)=AX(k-1)+ω(k-1)

(13)

(14)

系统的测量方程定义为

Z(k)=HX(k)+v(k)

(15)

卡尔曼滤波可以分为预测和更新两步。根据k-1步状态以及PDR系统信息，建立如下卡尔曼预测方程

(16)

其中，Xk|k-1为先验状态估计矩阵；Mk|k-1为先验协方差矩阵；Q为过程噪声矩阵。

基于UWB系统测量值，建立如下卡尔曼更新方程

(17)

其中，Xk|k为后验状态矩阵；Mk|k为后验协方差矩阵；Kk为卡尔曼增益。

4 实验结果与分析

为验证算法的定位性能，搭建了基于UWB与PDR组合的定位系统，该系统由1个移动节点与3个参考节点组成。其中移动节点由行人佩戴且集成有惯性导航模块(MPU9250)以及Decawave公司生产的DWM1000模块；参考节点仅集成DWM1000模块。实验中，移动节点安装在行人头顶，参考节点安装在室内固定位置且安装在同一高度(2 m)。行人沿着固定路径行走，通过USB或者无线将采集到的数据上传到PC端并进行位置解算。

4.1 PDR定位测试

为了验证PDR定位的性能，行人将惯性传感器佩戴在头顶并沿着图3中虚线路径行走3圈。

图4 行人航迹推算结果Figure 4.Results of pedestrian dead reckoning

从图4可以看出，消费级MEMS-IMU精度较差，使用PDR算法对行人步态进行识别能一定程度上弥补传统惯导算法累积误差迅速增大的缺陷。然而，PDR算法在步长估计和航向估计方面均存在误差，虽然该方法在短时间内精度较高，但是累积误差同样会随着时间越来越大。从图中可以看出，经过1圈行走过后，PDR算法得到的轨迹已偏离实际轨迹，因此单一PDR算法难以满足长时间、高精度定位的需求。

4.2 UWB定位测试

为了测试UWB定位系统的定位性能，行人头顶配戴UWB移动节点沿着图5中虚线路径行走1圈，实验中UWB系统位置更新速率约为25 Hz。从图中可以看出，UWB系统不存在累积误差，但是其测距易受NLOS及其本身随机误差的影响，导致定位结果波动较大。

图5 UWB定位结果Figure 5. Results of UWB positioning

根据UWB和PDR定位结果，两种定位方法各有优缺点，使用卡尔曼滤波可以有效融合两种定位系统的结果。

4.3 UWB/PDR组合定位测试

为了实现更加稳定、精度更高的定位，本文使用卡尔曼滤波进行融合。首先测试非视距影响下的定位性能，实验中，行人沿着图5中虚线路径行走1圈，且参考节点与行人之间没有障碍物阻挡。

图6 定位结果比较Figure 6. Comparison of positioning results

从图6可以看出，UWB/PDR组合定位算法能有效修正PDR系统定位产生的积累误差，并减轻UWB系统定位抖动的缺点，与真实行走路径更加接近。卡尔曼融合解算方法在一定程度上通过PDR方法中的距离约束剔除了精度较差的定位结果，减小了UWB定位结果的波动，提高了整体的定位精度。如表1 所示为3种定位方式的误差结果比较。

表1 定位误差比较Table 1. Position error comparison /m

为了验证非视距情况下定位系统的性能，在室内放置书架模拟非视距场景，得到如图7所示的定位轨迹图。实验中，在非NLOS影响下，UWB测距误差通常在10 cm左右。而在NLOS情况下，UWB信号由于障碍物遮挡发生反射、多径等，导致测距距离通常大于正常值。因此，根据实际实验，将式(11)、式(12)中的阈值η、ρ分别设为经验值20 cm和-15 cm。

根据图7，由于书架的遮挡，UWB测距误差增大，导致靠近书架处UWB计算的坐标偏离参考路径较大。使用本文的NLOS检测方法剔除异常测距值并与PDR进行融合后，得到的轨迹更加符合参考路径，表明定位系统在短暂NLOS影响下依然有较高的精度。

图7 NLOS定位结果Figure 7. Positioning results in NLOS scene

5 结束语

本文提出了一种UWB与PDR组合的室内行人定位算法。该算法通过卡尔曼滤波器将UWB定位系统与PDR系统相结合，解决了PDR累积误差大的问题，并改善了UWB易受NLOS影响的缺陷。为了验证组合定位系统的性能，本文搭建了实验平台。实验结果表明，本文提出的组合定位系统具有较好的精度和稳定性。下一步还需要研究在NLOS影响下，在不直接剔除误差较大值时对UWB测距误差进行修正的方法，以及利用定位结果对步长预测进行修正的方法，以便实现更高精度的定位。