基于改进SHKF 算法的UWB/IMU 组合定位方法

黄卫华，梅宇恒，章 政，赵广营，刘思贤

（武汉科技大学 信息科学与工程学院，武汉 430081）

超宽带（Ultra-Wide Band,UWB）定位技术因其定位精度相对较高、信号功耗相对低且抗多径效应较好等特性，被广泛应用于室内定位、体育运动、智能制造等领域[1-3]。惯性测量单元（Inertial Measurement Unit,IMU）仅使用内部惯性信息，具有不易受外部环境影响、可提供即时相对位置信息等特点[4-6]。在复杂的环境下，UWB/IMU 组合定位方法既可利用IMU 不易受环境干扰的特点，修正易受复杂环境干扰的UWB 信号；也可利用UWB 的绝对位置信息消除IMU 由于随机游走与温度漂移造成的累积误差[7,8]。同时，UWB 定位系统不可避免地会遇到非视距（Non-line of Sight,NLOS）场景，且存在因信号波动或扰动而产生的随机误差，使得UWB/IMU 定位精度下降[9]。因此，研究鉴别NLOS 信号，以及确定UWB/IMU 传感器数据的组合定位方法具有重要意义。

目前，许多学者采用机器学习的方法对UWB 采集的测距信号进行分类鉴别。文献[10]同时分析UWB信号总功率与首径功率，构建深度学习模型，以鉴别出NLOS 环境下的UWB 基站测距数据。文献[11]以UWB 的信号强度和距离估算值作为输入特征，分别比较几种不同机器学习算法对NLOS 信号鉴别的效果。文献[12]以NLOS 环境下的误差序列作为输入，通过反向传播神经网络建立误差改进模型，改正预测得到标签的误差值。文献[13]以UWB 波形信息作为特征，基于 python 的 Scikit-learn 库，使用支持向量机（Support Vector Machine,SVM）、随机森林与神经网络三种机器学习鉴别NLOS 信号。在实际场景中，UWB 测距信号与室内障碍物的分布信息密切相关，测距信号的特征与标签映射关系的不确定性会影响机器学习的效率。此外，在UWB 测距信号采集过程中，不可避免会出现噪声，直接对UWB 测距信号进行鉴别，会影响鉴别NLOS 信号的准确性。

Sage-Husa 卡尔曼滤波（Sage-Husa Kalman Filter,SHKF）算法是一种用于状态估计的滤波算法，具有应用灵活性高、估计结果准确性强等优点。文献[14]通过陀螺仪信息建立状态方程，通过加速度计与磁力计的信息建立量测方程，采用SHKF 算法将陀螺仪、加速度计与磁力计组合以获取无人机的航姿信息。文献[15]以SHKF 为基础，采用渐消因子进行动态调整，将捷联惯导系统与多普勒计程仪组合，提升了多普勒计程仪量测信息异常情况下的导航精度。对于UWB/IMU组合定位而言，传感器自身缺陷会引起如随机误差、温度漂移等噪声类型与幅值多变的问题，导致UWB/IMU 定位的新息误差产生随机性的异常值，从而SHKF 算法对UWB 与IMU 噪声协方差估计值的准确性降低。由于历史噪声的影响，当两者均产生较大误差时，SHKF 算法无法将UWB/IMU 组合定位系统的定位估计值与实际值的偏差及时消除，甚至滤波器会逐渐失去估计作用。

鉴于上述情况，本文在实现基于概率密度提升树的NLOS 信号鉴别方法的基础上，提出了一种改进SHKF 的 UWB/IMU 组合定位方法。首先，将UWB/IMU 特征数据的概率密度函数引入提升树的损失函数中，对NLOS 信号进行鉴别。然后，为了降低历史噪声对当前定位的影响，在SHKF 算法中引入渐消因子，修改临近量测数据的权值，并定义表示新息变化趋势的自适应因子，自动调整用于补偿新息误差所采用的策略，从而提升SHKF 算法的准确性和稳定性。

1 基于概率密度提升树的NLOS 信号鉴别方法

提升树算法是以决策树为基学习器，采用加法模型和前向分布算法的提升方法，具有模型简便、训练数据量少的优点。在复杂环境下，UWB 测距存在的NLOS 问题影响UWB/IMU 定位精度，采用提升树对UWB 信号进行鉴别，可以提升UWB 传感器定位数据的质量。同时，考虑到提升树中训练数据特征属性与训练效率的关联性，为了细化UWB/IMU 特征数据属性，提高提升树的训练效率，将UWB/IMU 采集的特征数据进行统计分析，把所得特征数据的分布密度引入提升树中。基于概率密度提升树的NLOS 信号鉴别方法如图1 所示。

图1 基于概率密度提升树的NLOS 信号鉴别方法Fig.1 NLOS signal identification method based on probability density boosting tree

1.1 UWB/IMU 数据特征选取

一般而言，可由UWB 的信道信息与量测信息鉴别出NLOS 信号。当出现NLOS 情况时，UWB 信号功率会产生显著变化，因此选取信号功率估计偏差作为特征一；此外，由于IMU 定位属于无信标定位，不易受NLOS 环境干扰，解算UWB 标签与UWB 基站的距离以及IMU 传感器与UWB 基站的距离，通过比较这两者的差值判断UWB 测距数据的情况，并将其作为特征二。

特征一T1与特征二T2分别表示为：

其中，RxPower表示UWB 信号接收估计功率；FpPower表示UWB 首径信号计算估计功率；DisIU表示IMU传感器与UWB 基站的距离；DisUU表示UWB 标签与UWB 基站的距离。

定义特征向量T=[T1T2]T，则含有N组训练数据的集合Sd为：

其中，Ti、Bi分别表示第i组数据集的特征向量与其标签；Sdi表示第i组数据。设置标签为：

1.2 基于概率密度提升树的构建

1）融合概率密度的损失函数定义

当UWB 传感器采集的数据出现噪声时，特征值方差大的数据难以与标签映射，导致采用提升树对UWB 信号进行特征分类时，难以鉴别出NLOS 信号。通过在损失函数中融入UWB/IMU 特征数据的分布密度，进而细化特征数据属性，提高提升树的学习效率。

假设前m次迭代产生的决策树所构建的提升树模型Trm为：

其中，σTik表示每个特征点的分离程度；σ1/N表示特征集的方差表示以特征值为切分点时，信号鉴别错误的情况；表示特征集的平均值。

第m次迭代的累积概率密度函数为：

将式(6)引入提升树的损失函数，则有：

2）决策树的构建

每一轮迭代都以求解损失函数的极小值为目标产生决策树，进而不断提升模型的准确性，即解算出与αm：

联立式(4)以及式(6)～式(8)，则有：

考虑到UWB 信号鉴别可以表示为鉴别正确情况与错误情况的组合形式，则可将式(9)极小化的损失函数重新整理为：

根据极值判别式，式(11)对αm求一阶导并令其等于0，则可以解算出αm：

为了方便算法迭代，采用分类误差率代替损失函数来选择切分点，分类误差率的定义为：

选择分类误差率最低的切分点Ti，得到第m次迭代产生的决策树为：

将UWB 测距数据经过基于概率密度提升树分类后，将鉴别为NLOS 的信号剔除，并利用鉴别为LOS的信号构建UWB 定位模型，进而用于UWB/IMU 组合定位。

2 基于改进SHKF 的UWB/IMU 组合定位算法

UWB/IMU 组合定位时，SHKF 算法通过指数衰减法修正UWB 的噪声协方差矩阵，弥补模型估计结果与实际定位结果的误差，但这一效果会随时间逐渐减弱。考虑到传感器受到的噪声类型与幅值多样且多变，导致SHKF 算法易产生滤波发散的现象，本文设计了一种改进的SHKF 算法。为了防止历史UWB 和IMU 数据噪声对当前新息误差的修正产生负面影响，将新息误差分成两部分：其一采用指数衰减法，降低当前量测数据对噪声协方差的影响；其二采用变窗口长度的加权平均法，解算出渐消因子以增强临近量测对预测协方差矩阵的影响。同时，设计表征新息误差变化趋势的自适应因子，实时调整采用指数衰减法与加权平均法对新息误差调整的占比，达到跟随噪声变化的目的。

2.1 UWB/IMU 定位模型

UWB 定位模型根据环境建立坐标系，采用多个基站与单个标签得到UWB 标签与基站的绝对位置信息，称为全局定位[16]；IMU 定位模型根据自身运动建立坐标系，基于加速度计与陀螺仪的数据得到IMU 相对于自动导航车（Automated Guided Vehicle,AGV）的位置信息，称为局部定位。

令搭载UWB 与IMU 定位模块的AGV 面向x方向行驶，其法线方向为y方向。设k时刻系统的状态向量为：

对于UWB 定位系统，采用双向双边测距法得到UWB 三个基站与标签的距离在此基础上，通过三边定位法，求解以UWB 基站为圆心、三边距离为半径的定位方程组funUWBp为：

根据式(14)解算出k时刻的UWB 定位坐标，并依据k-1 时刻UWB 的坐标信息解算出速度信息，UWB的定位模型f1为：

其中，solve(·) 表示对函数求解表示k时刻UWB 的定位方程dt为相邻时刻的时间间隔。

得到IMU 定位模型f2为：

2.2 改进的SHKF 算法设计

1）SHKF 算法

将UWB 量测信息作为量测更新、IMU 量测信息作为预测更新，根据UWB 定位模型f1和IMU 定位模型f2，设k时刻 UWB 定位建模方程为IMU 定位建模方程为其中，rk与qk分别为k时刻UWB、IMU 传感器的噪声，表达式为：

其中，σdis为UWB 的定位误差；σa为加速度计在x、y方向上偏移的矢量和；σω为偏航角角速度偏移；rk与qk的协方差分别为

由此可得系统的状态方程与观测方程分别为：

其中，Zk为k时刻系统的量测向量。

卡尔曼滤波器的预测更新与量测更新为：

定义k时刻的新息误差与新息误差协方差分别为：

由式(25)可知，若能准确估计新息协方差Ck，就能推算出量测噪声协方差Rk。SHKF 算法通过指数衰减法估计当前的新息误差为：

根据式(25)和式(26)，可得SHKF 算法UWB 的噪声协方差矩阵为：

2）渐消因子的解算

为了提升SHKF 算法的稳定性，且为了解算方便，将渐消因子λk引入式(20)，通过λk修改临近量测数据在位置估计中的占比，即：

当λk=1 时，将SHKF 算法的新息协方差与噪声协方差分别记为其协方差记为则有：

其中，tr(·)表示求矩阵的迹。

分别联立式(25)、式(28)和式(29)，则λk为:

为了简化渐消因子的计算，令λk为：

通过实时改变窗口长度，达到适应复杂环境下UWB/IMU 传感器噪声变化的目的。设为k时刻可变窗口长度，并令其与k-1时刻的新息成正比，考虑到不能过短，定义为：

其中，N0为常数，表示初始窗口长度；KN代表窗口的比值常数；表示定位误差，为k时刻的定位估计值与量测定位值的误差。

当新息误差大时，适当加宽窗口，削弱当前噪声的影响；当新息误差小时，适当减小窗口，降低SHKF算法的计算量。

3）自适应因子的设计

为了同时保证SHKF 算法的精度与稳定性，根据相邻时刻新息误差变化趋势，将噪声的突变程度划分为缓慢、中等、急速三种状态。根据不同的状态选择不同的新息误差修正策略，定义自适应因子βk为：

结合式(25)与式(33)，将新息误差分成自适应的两部分进行补偿，即新息协方差矩阵估计由组成，改进SHKF 算法的新息误差协方差矩阵为：

3 实测实验及结果分析

本文以载有UWB/IMU 传感器的AGV 为运动载体，搭建了室内环境下UWB/IMU 定位的实验平台，验证本文所设计组合定位方法的可行性和有效性。实验场地尺寸为10 m × 8 m × 3.5 m，放置三个UWB 基站对UWB 标签测距，分别通过DW1000、MPU6050 模块采集UWB、IMU 量测数据，采样频率分别为30 HZ与150 HZ。分别经过到达时间补偿（DW1000）、零偏修正（IMU6050）与滑动均值滤波后，IMU 与UWB传感器参数如表1 所示。

表1 传感器参数Tab.1 Sensor parameters

3.1 NLOS 鉴别实验对比

将AGV 放置在距离UWB 基站5 m 处，以无障碍物遮挡的情况模拟LOS 场景，如图2 所示。在距离UWB 基站2.5 m 处放置障碍物木板模拟NLOS 场景，如图3 所示。

图2 LOS 场景模拟Fig.2 LOS scenario simulation

图3 NLOS 场景模拟Fig.3 NLOS scenario simulation

分别在上述NLOS 与LOS 场景下各采样60 s，采集数据特征分布如图4 所示。随机选取75%的数据作为训练数据，剩下25%的数据作为测试数据。分别采用提升树与本文所设计的基于概率密度的提升树对UWB 信号进行鉴别，设置迭代次数为50 次，两种算法的鉴别结果如图5 和图6 所示。

图4 采集数据特征分布图Fig.4 Collect data feature distribution map

图5 提升树信号鉴别结果Fig.5 Result of signal identification by boosting tree

图5 和图6 中蓝色圆圈区域表示特征值交界处。由蓝色圆圈内信号的鉴别结果可知，提升树算法在NLOS 与LOS 数据特征值交界处的数据难以鉴别，而基于概率密度的提升树算法提高了这部分数据鉴别的准确率。采用NLOS 信号鉴别的准确率PNN、误识别率PNL作为性能指标，鉴别效果如表2 所示。

表2 NLOS 信号鉴别效果Tab.2 Performance of NLOS signal identification

3.2 UWB/IMU 组合定位实验

以UWB 基站1 为原点建立坐标系，坐标系单位为cm，UWB 基站2 与基站3 的坐标分别为(700,0)、(0,500)。在(150,250)处，通过放置障碍物模拟NLOS环境，实验现场实际环境如图7 所示。

图7 UWB/IMU 组合定位实验现场Fig.7 Site of UWB/IMU combined positioning experiment

AGV 从(50,50)处出发，以0.5 m/s 的速度匀速绕类矩形路线行驶一周。人为去除15 s～17 s之间的UWB量测数据，模拟两个以上UWB 基站的测距数据被鉴别为NLOS 信号的情况。UWB 与IMU 采集的数据独立解算后，AGV 的定位轨迹如图8 所示。

图8 UWB 与IMU 独立的AGV 定位轨迹Fig.8 Independent AGV positioning trajectory of the UWB and IMU

图8 中，粉色区域分别表示1 s～7 s、25 s～30 s、33.5 s～35.5 s 的AGV 行驶区间，在此区间障碍物挡住了一个UWB 基站的测距路线，导致UWB 的定位随机误差增大；蓝色区域分别表示从15 s 和32 s 开始IMU 定位数据误差增大；黄色区域表示从15 s 至17 s出现UWB 数据缺失情况。

采用改进的SHKF 算法分别与βk≡ 0时的算法（SHKF0 算法）以及βk≡ 1时的算法（SHKF1 算法）对比。上述算法的遗忘因子b均设为0.99，改进SHKF算法的初始窗口长度N0为10，比值常数KN为10，自适应因子βk中阈值c0、c1分别为1 和3.5。三种算法的AGV 定位轨迹对比如图9 和图10 所示，对应的实时定位误差结果如图11 所示，定位均方根误差（Root Mean Square Error,RMSE）的统计结果见表3。

表3 定位RMSE 对比Tab.3 Comparison of RMSE of positioning

图9 改进SHKF 与SHKF0 算法的UWB/IMU 组合定位轨迹对比Fig.9 UWB/IMU combined location trajectory comparison between improved SHKF and SHKF0

图10 改进SHKF 与SHKF1 算法的UWB/IMU 组合定位轨迹对比Fig.10 UWB/IMU combined location trajectory comparison between improved SHKF and SHKF1

图11 SHKF0、SHKF1 与改进SHKF 算法的实时定位误差Fig.11 Real-time location error of SHKF0,SHKF1 and improved SHKF algorithm

对应图8中UWB与IMU独立的AGV定位轨迹，从图9 可知，粉色区域处由于UWB 与IMU 两个传感器的定位误差同时增大，通过SHKF0 算法所估算出AGV 的位置出现了较大偏差，且短时间内无法修正，而改进的SHKF 算法引入了渐消因子，并通过自适应因子提升了渐消因子的作用，因此迅速修正了当前噪声，提高了AGV 的定位精度。从图11 可知，SHKF1算法引入渐消因子虽然提升了算法的稳定性，但自适应因子并未对渐消因子实时调整，所以SHKF1 算法的定位轨迹平滑度较差，定位精度相对较低。

由表3 可得，与SHKF0 和SHKF1 算法相比，本文算法的x轴定位RMSE 分别提升了5.97%、50.62%，y轴定位RMSE 分别提升了26.49%、39.48%，定位RMSE 分别提升了18.25%、44.58%。

4 总结

针对复杂环境下UWB 的NLOS 信号鉴别以及随机误差问题，本文提出一种基于改进SHKF 算法的UWB/IMU 组合定位方法。基于室内环境下UWB/IMU组合定位的AGV 实验平台开展实验验证，实验结果表明：本文算法对NLOS 信号鉴别的准确率为99.12%，定位均方根误差为4.30 cm。本文的主要结论如下：

1）将UWB/IMU 特征数据的分布密度融入提升树的损失函数中，细化了特征值的属性，提升了NLOS信号鉴别的准确率。

2）根据新息误差的变化趋势提出一种具有自适应因子的改进SHKF 算法，自适应调整渐消因子与权重系数对新息误差的修正，跟踪噪声类型与幅值的变化，进而提升UWB/IMU 组合定位的精度。