融合UWB和IMU数据的新型室内定位算法

1 引 言

在基于位置的服务及其应用中，位置信息起着至关重要的作用.在室外环境中，可以轻松地从全球定位系统(Global Positioning System，GPS)获得准确的位置信息.但是，在室内环境中，GPS信号受到建筑阻挡[1]，导致GPS信号变弱甚至无法被检测，因此GPS定位的结果无法满足人们室内定位的要求.随着技术的发展，目前已有许多技术可以代替GPS用于室内定位，如超宽带、超声波、蓝牙等.其中大部分定位技术主要基于以下方式：到达角度(Angle of Arrival，AOA)[2]、到达时间差(Time Difference of Arrival，TDOA)[3]、到达时间(Time of Arrival，TOA)[4]、接收信号强度指引(Receive Signal Strength Index，RSSI)[5，6].由于基于TOA方法的定位精度较高[7]，可用于实现室内环境下的高精度定位.

超宽带(Ultra-Wideband，UWB)具有许多理想的特性，包括低能耗、高分辨率、多径免疫和某些障碍物穿透能力等，因此，UWB定位被认为是用于高精度室内定位中最有前途的技术之一[8].但是，当环境受到NLOS(Non-line-of-sight)影响时，UWB测距误差增大，从而导致定位精度下降.因此，如何处理NLOS误差已成为UWB定位领域的热门研究课题[9].与UWB不同，INS使用惯性测量单元(Inertial Measurement Unit，IMU)在没有其他任何辅助信息的情况下以较高的更新率估算物体的位置，短时间内的误差较小，但具有累计误差，与UWB系统形成良好的互补，因此有必要融合两种系统达到更好的定位效果[10].

文献[11]中，EKF基于IMU预测值对测量距离误差进行检测，并结合超宽带的信道冲激响应来进一步提高检测精度，忽略检测到的不精确测量值.该方法不适用于复杂的室内环境且丢弃了许多有用信息.文献[12]使用高斯低通滤波算法和主成分分析法进行预处理，并提取位置特征信息.结合最小二乘支持向量机对样本集进行离线训练和在线分类，获取人员实时位置.文献[13]提出了一种能够确定特定的非视距信道的非视距识别与缓解方法，并根据识别出的信道信息进行定位估计，但保留了太多的离线数据，需要大量时间进行在线匹配筛选.为了能够在复杂的室内环境下快速准确地识别出NLOS，并有效缓解NLOS影响，考虑UWB和INS各自的优点，本文提出一种新的定位算法——利用PDR(Pedestrian Dead Reckoning)预测位置进行NLOS识别，并基于多信道NLOS误差概率分布函数进行NLOS修正，最后利用KF(Kalman Filtering)融合UWB和IMU获得准确的定位结果.主要贡献归纳如下：

1)分信道采集数据与真实值比较获得NLOS误差，根据不同信道的误差分布，拟合出对应于不同信道NLOS误差的概率曲线.

2)利用PDR预测的位置进行NLOS识别，减少系统的NLOS识别时间.通过概率判断NLOS误差所属的信道情况并对NLOS误差进行估计，从而获得更为准确的估计距离.

3)对UWB系统，利用改进的最小二乘法估计移动节点位置，提高UWB系统的定位精度.

2 系统模型

定位的过程主要分为两个阶段：离线阶段和在线阶段.

2.1 离线阶段

基于TOA测距方法的NLOS误差表达式如下[13]：

(1)

其中εnlos和attnlos分别表示与传播通道中障碍物相关的介电常数和衰减因子，L是路径的数量，ai和τi表示第i条路径的幅值和TOA，aDP和τDP表示LOS(Line of sight)下的幅值和TOA.

根据式(1)可以看出不同的障碍物对NLOS误差的影响是不同的.为了更加准确地估计NLOS误差，在离线阶段，用UWB对不同信道分别进行采样，并将在不同障碍物下的测量值与真实值进行比较获得准确的NLOS误差值.将每一个信道测得的误差值细分为w个区间.第k个区间上的中点和该区间上NLOS误差值出现的频率为ak和bk，将其代入正态分布曲线进行线性拟合，获得每一个信道下误差分布的概率曲线(假设每一信道下的NLOS误差服从正态分布).本文对m个不同的信道进行采样拟合：

(2)

其中：

拟牛顿法是求解非线性优化问题最有效的方法之一，只要求每一步迭代时知道目标函数的梯度.通过拟牛顿法对式(2)求解，可以获得不同信道下NLOS误差的概率分布曲线.为了便于后期的比较，对每一条曲线进行相应的缩放，使得每一条NLOS误差概率分布曲线的取值范围在0-1之间，获得的NLOS误差概率曲线如图1所示.可以看出木门和行人的NLOS误差分布较为接近，而金属障碍物铁门和混凝土墙的分布较为接近.为了更好地进行NLOS识别修正，将行人分别与铁门和混凝土墙的均值和标准差平均来表示铁门与行人的情况和混泥土墙与行人的情况.

图1 不同障碍物下的NLOS误差分布

Fig.1 NLOS error distribution under different obstacles

2.2 在线阶段

考虑在一个室内环境下进行二维平面的目标定位与跟踪.在该空间中的已知位置放置可以发送带有时间戳的UWB发送装置，目标携带有UWB无线接收装置和INS的移动设备.

2.2.1 INS系统

INS通过IMU来估计位置，IMU的主要优点是无需外部输入即可估计其位置，并且可以在室内环境或没有卫星信号的地方使用，目前已经应用于室内定位.通常，IMU包括陀螺仪和加速度计.陀螺仪输出物体的角速度，而加速度计输出线性加速度.物体的三轴角速度通过一次积分可得姿态角信息，即偏航角、俯仰角和翻滚角.根据从IMU获得的信息，可以将惯性参考坐标中的加速度转换为导航参考坐标.然后，根据牛顿运动定律，在两次积分后计算对象的位置[14].因此，IMU位置估计系统可以通过以下线性方程建模：

(3)

(4)

(5)

其中，Δt是IMU的采样周期，和分别对应姿态向量、角速度向量、加速度向量、速度向量和位置向量.ψΩ、ψV和ψX分别是和的测量误差.

2.2.2 UWB测距模型

本文中，使用了基于TOA的测距方法，要求接收端和发送端时间同步，通过获得信号传播的时间t间接获得锚节点和移动目标的距离[15]：

d=c×t

其中，c为信号传播的速度(c=3×108m/s)，d为测得的发送端与接收端的距离.

假设有n个锚节点，测得移动目标接收到第i个锚节点发出的信号在空气中的传播时间为ti，由于有噪声、障碍物等原因的存在，使得测量的距离存在测量误差，表达式如下：

(6)

其中，di为真实距离，为测量距离，εi和ηi分别为测量噪声和由于障碍物阻碍产生的NLOS误差，其中且满足，μlos=0，μnlos>0.

3 定位系统描述

3.1 步行者航迹推算

INS系统存在一个众所周知的问题，即累积误差[16]，位置估计误差会随着时间(迭代次数)而迅速增加.从式(3)-式(5)中可以看出，角速度测量中的误差将引入到估计的姿态中，而线性加速度测量中引入的误差将传送到估计的速度和估计物体的位置.同时下一时刻的位置是基于当前的估计位置来获得的，INS系统的当前估计位置的误差将被会被累加到下一时刻的位置估计中.因此，在几次位置更新后，累积的误差会逐渐增大.为了较好地解决上述问题，采用了步行者航迹推算的方法(PDR)[17]，该方法包括3个过程：步检测、步幅(SL)估计和方向估计.

3.1.1 步检测

本文采用峰值检测的方法[18，19]来判定是否发生行走行为.首先，需要定义一个峰值检测的参数，减少噪声的影响.在实验中发现，由于个人行走的习惯及噪声的影响，检测到的步数通常高于实际步数.考虑到常人的步频不会太快，因此对峰值检测设置相应的时间间隔，如果在一段时间内出现多个峰值，则当出现第一个峰值时即认为行走了一步，并忽略该时间段内对其他峰值的检测.如图2所示，当设置间隔时间为0.5s时，会出现偶然的误检测，而当时间阈值设为0.6s则不会出现误检测，可以看出设置合适的时间阈值可以有效提高步数检测的准确性.

图2 时间阈值对步检测的影响

Fig.2 Influence of time threshold on step detection

3.1.2 步幅估计

正常行走时行人的步长不会发生太大的变化，所以步长可以假设成一个固定值，也可以通过加速度信号来进行计算，本文应用文献[20]中的经验公式，通过获得的Z轴加速度信息对行人步长进行估计：

SL=K×(Azmax-Azmin)(1/4)

其中，Azmax和Azmin分别是在单步中Z轴方向上最大垂直加速度和最小垂直加速度，K是可以通过对步长训练得到的常数.

3.1.3 方向估计

通常有两种主要方法可估计行人在第t阶段的方向：陀螺仪方法和加速度计方法[1].而本文中首先使用偏航角θt+1表示行人的方向，初步估计行人位置，用于NLOS初步识别和修正.

根据以上3步过程，首先可以获得预测位置：

xt+1，pre=xt+SLt+1×cosθt+1

(7)

yt+1，pre=yt+SLt+1×sinθt+1

(8)

其中，xt，yt分别为第t时刻行人所在坐标系的横纵坐标，xt+1，pre，yt+1，pre分别为预测行人在第t+1时刻的横纵坐标.

3.2 UWB定位系统

通过式(7)和式(8)得到IMU的预测位置，根据距离公式计算预测位置与锚节点之间的距离：

(9)

将UWB距离测量值与式(9)获得的距离预测值做差来获得误差的估计值，如式(10)所示：

(10)

设置阈值φ进行NLOS识别判断，当：

Δdi≤φ

(11)

满足时，则认为对应测量处于LOS，不对其进行处理，否则认为其受到NLOS干扰，将式(10)获得的NLOS的估计误差 代入不同信道下的NLOS误差的概率分布函数，得到属于各个信道的概率ωj，j=1，2，…，m.

通过NLOS误差属于每一个信道的概率来判别该测量值所受NLOS的情况.由于难以表示所有障碍物对应的NLOS误差概率分布函数，因此设置阈值α以解决无法识别修正的情况.当：

ωj<α，∀j，j=1，2，…，m

(12)

满足时，则认为该测量值超出了该方法的识别修正能力，因此将该测量值丢弃不用于最终的UWB系统的最小二乘定位.此外设置一个大于α的阈值β，如果：

max(ωj)>β，j=1，2，…，m

(13)

则认为NLOS误差的分布对应其中概率最大值所对应信道的误差分布，假设最大概率对应信道的误差分布为N(μ，σ2)，所以该NLOS误差对应的大小及标准差为：

(14)

(15)

当式(12)和式(13)两式都不满足时，则将传播信道视为多个信道的组合，定义：

W={j|α<ωj<β，j=1，2，…，m}

则修正的NLOS误差及其对应的标准差分别为：

(16)

(17)

根据式(14)和式(16)从而获得UWB系统测量距离的修正值：

得到修正距离后，使用最小二乘定位法对移动目标位置进行估计，假设行人的坐标为(x，y).因此，第i个锚节点与移动节点之间的距离为：

假设修正值的误差服从同一分布εi，即：

两边取平方:

可以看出误差项其中εi相比较于di较小，所以可以忽略不计，而di的取值大小会影响误差项2diεi.为了减少最小二乘法中选择的参考节点对定位结果的影响，选取修正的距离值中的最小值所对应的锚节点作为参考节点即：

(18)

将式(18)写成矩阵形式：

HX=b

其中：

从而获得UWB系统最小二乘的解：

XLS，t+1=(HTH)-1HTb

(19)

在获得UWB系统的估计位置之后，计算t时刻的位置Xt与XLS，t+1之间的夹角更新偏航角

其中，λ在0和1之间，并利用更新后的偏航角对式(7)和式(8)进行更新，从而获得更新后的估计位置：

(20)

3.3 KF融合IMU和UWB

卡尔曼滤波器广泛用于数据融合和状态估计，并且提出了许多使用卡尔曼滤波器来解决不同的问题.这些不同形式的卡尔曼滤波器遵循相同的两个通用步骤：预测步骤和更新步骤[21，22].本节中，利用卡尔曼滤波融合上述UWB和INS系统的定位结果，实现更加准确的行人位置估计.

预测阶段：

(21)

Pk|k-1=Pk-1|k-1+Qk

(22)

其中，预测状态Xk|k-1由式(7)和式(8)获得，Pk|k-1是预测状态Xk|k-1协方差矩阵，Pk-1|k-1是k-1时刻估计状态Xk-1|k-1的协方差矩阵，Qk是当前预测值噪声分布的协方差矩阵，可以通过训练过程中的步长估计的方差表示其对角线元素.

更新阶段：

Kk=Pk|k-1C(CPk|k-1CT+Rk)-1

(23)

Xk|k=Xk|k-1+Kk(Z-CXk|k-1)

(24)

Pk|k=(I-KkC)Pk|k-1

(25)

其中：

Zk由INS和UWB两个系统的估计值组成，两个子系统的估计值由式(19)和式(20)获得，Kk是卡尔曼增益，Rk是测量误差的协方差矩阵.其中Rk元素的表达式如下：

Qk/PDR=Pk|k-1

其中M为用于最小二乘定位的锚节点索引的集合，I是一个二维的单位矩阵，为修正后的测量值，σk为对应测量值所受到的NLOS误差的标准差，同时也是修正值的标准差，通过式(15)和式(17)可以获得.

在获得更为准确的估计结果之后，重新计算更新后的NLOS误差值对距离进行更新，再次获得最小二乘的估计位置，更新偏航角，卡尔曼滤波融合两个系统的结果，反复执行以上过程，直至估计位置变化在一定的范围之内(微小的变化可能由测量所受的噪声引起)，结束循环，获得最终的估计位置，并以此结果作为INS下一时刻的起始位置，从而来降低INS的累计误差，减小UWB的NLOS误差，降低整个系统的估计位置的误差，获得好的定位结果.

4 仿真设计和结果分析

为了验证定位系统的性能，模拟了一个室内定位的场景.假设在60m×50m 的房间内，左下角为原点，在固定的地点放置5个参考节点，行人带有移动节点和惯性导航模块在5个参考节点的覆盖范围内，沿着以点(10，5)、(50，5)、(50，45)和(10，45)为顶点的矩形逆时针行走一圈.假设PDR算法的步长为0.8米，误差服从N(0，0.042)，共走了200步，角度噪声设为N(0，π/50).UWB测距系统在LOS下的测距误差服从N(0，0.0262).仿真中假设有5种不同的NLOS情况，它们的分布分别为N(0.72，0.452)、N(0.92，0.132)、N(0.36，0.082)、N(0.59，0.092)和N(0.45，0.112).

4.1 选择合适的参考节点

从上文分析可以看出选择合适的参考节点对定位精度有明显的影响，由于在NLOS的识别和缓解过程中，NLOS误差被修正，残留的NLOS的误差值很小，为了便于观察选择合适的锚节点作为参考节点对定位精度的影响，首先假设5个锚节点都处于LOS情况下，误差服从N(0，0.0262).仿真效果图如图3所示，可以看出相比较于LOS情况下始终选择同一个锚节点作为参考节点的情况，选择合适的锚节点作为参考节点能够有效减少定位误差，进行了50次仿真.同一个锚节点作为参考节点的仿真中，50次仿真的总误差286.8361米，而每一次定位过程中都选择合适的锚节点作为参考节点的总误差为246.8173米，定位精度提高了13.95%.

图3 LOS下参考节点选择对误差的影响

Fig.3 Influence of reference node selection on error under LOS

考虑到算法的识别修正能力是有限的，有可能存在未完全消除NLOS误差的情况，为进一步说明选择合适参考节点对定位精度的影响，因此假设有一个锚节点始终处于NLOS情况下未被识别和修正，并且该NLOS误差服从N(0.33，0.082).当两种方法都不将受NLOS影响的锚节点作为参考节点时，其定位效果和LOS下相似，定位精度提高了11.61%.而当受NLOS影响的锚节点始终作为参考节点时，实时选择合适的锚节点作为参考节点能够明显的提高定位精度，仿真效果如图4所示.选取同一个锚节点作为参考节点的50次仿真总误差为2.6297×103米，而每次定位都选择合适的锚节点作为参考节点的总误差仅为1.7259×103米，定位精度提高了34.37%.

图4 NLOS下参考节点选择对误差的影响

Fig.4 Influence of reference node selection on error under NLOS

4.2 NLOS误差均值的动态变化

室内环境的变化如温度、湿度等会导致NLOS误差的变化，为了验证该算法能够有效解决该类问题，在仿真中假设NLOS误差均值从0.40m-0.80m变化，同时方差不变.其定位误差(单位m)如表1所示，从表中可以看出，当由于室内环境的变化导致的NLOS误差变化时，使用本文提出的算法可以获得较为精准的定位结果.

表1 不同NLOS误差均值下的定位误差比较

Table 1 Comparison of positioning errors under different NLOS error means

4.3 NLOS下的定位结果

如图5所示，比较了不同的方法之间的定位误差，从图中可以看出不经过NLOS识别和修正直接融合UWB和IMU的定位误差和直接使用LS获得估计位置的定位误差比较接近.对于不分信道，只判别LOS和NLOS在短时间内的定位误差较小，但是，随着时间的增加，定位误差也逐渐增加.而本文提出的方法定位误差较小且比较稳定.

图5 不同定位方式的定位误差比较

Fig.5 Comparison of positioning errors under different positioning methods

5 实验设计与结果分析

为了进一步验证本文提出的定位算法的定位性能，自主搭建了融合INS和UWB的定位系统平台，该定位系统包括惯性导航模块(BWT901CL)和UWB模块(UA-100).实验中，惯性导航模块和UWB移动节点安装在行人脚部，UWB参考节点安装在已知位置，为了确保会出现不同的NLOS信道，将一个锚节点置于木门后面，另一个锚节点放在走廊与移动节点之间始终受墙阻挡.移动节点将采集到的锚节点的ID和对应的距离值传输给PC端，惯性导航模块也将数据传送给PC端，PC端将两个模块的数据进行融合.利用上述系统在一个6.4m×8m的空间内进行实验.行人携带测量装置绕桌椅沿着以3.6m和4.8m为边长的长方形逆时针行走一圈.

如图6所示，○表示LS估计为位置时选择固定的参考节点，将UWB和INS数据融合后的误差，×表示在LS估计中每次都选择合适的锚节点作为参考节点的定位误差，可以看出通过选择合适的参考节点能够有效的降低最终的定位误差，提高定位精度.

图6 LS与改进的LS的定位误差比较

Fig.6 Comparison of positioning error between LS and improved LS

从图7中可以看出PDR的方法起初可以获得较为准确的定位结果，而随着步数的增加定位误差越来越大.而不分信道识别修正NLOS误差，刚开始能够获得较为准确的定位结果，随着时间的推移误差有增大的趋势.然而本文所提出的算法对于不同的障碍物引起的NLOS有很好的识别修正能力，能够获得较好的定位结果，定位误差较小且稳定，该方法下的最大定位误差为0.25m.

图7 不同方法的定位误差比较

Fig.7 Comparison of positioning errors of different methods

6 结 论

本文研究了室内定位系统在非视距下的精确定位问题.首先，在离线阶段建立了不同障碍物下的NLOS误差分布.利用PDR的预测位置和NLOS误差的概率曲线进行非视距误差识别和缓解，利用改进的最小二乘法来提高UWB系统的定位精度，并对偏航角进行修正，最后利用KF融合UWB和IMU的估计位置提高定位精度.该方法充分利用了两个系统的优点，在不同的NLOS误差情况下也能取得较好的定位结果.