基于协同定位和自适应卡尔曼滤波的定位算法研究

庞明瑞 高振航 刘璐 臧悦程

摘  要：针对煤矿井下复杂环境中非视距误差对无线信号定位的干扰，文章设计了区域判定和校正策略减小标签和基站之间的无效通信，提高了定位效率;提出基于UWB的煤矿井下协同定位算法，移动的标签充当定位基站，减少定位盲区提高定位精度;提出改进的自适应卡尔曼滤波算法对协同定位结果进行优化，获取信息向量检验非视距环境误差，并对协方差矩阵进行修正。仿真验证结果表明：改进后的自适应卡尔曼滤波算法结合协同定位算法相比卡尔曼滤波算法结合协同定位算法定位误差更小，可以更好地应用于煤矿井下人员定位中。

关键词：超宽带;煤矿井下定位;区域判定和校正策略;协同定位算法;自适应卡尔曼滤波

中图分类号：TP18     文献标识码：A文章编号：2096-4706（2021）24-0074-07

Abstract： Aiming at the interference of non-line-of-sight errors on wireless signal positioning in complex environment of underground coal mines， this paper designs a regional determination and correction strategy to reduce the invalid communication between tags and base stations， and improves positioning efficiency; an underground coal mine cooperative positioning algorithm based on UWB is proposed， the mobile tag acts as a positioning base station， reducing the positioning blind area and improving the positioning accuracy; an improved adaptive Kalman filter algorithm is proposed to optimize the collaborative positioning result， obtain the information vector to check the non-line-of-sight environment error， and correct the covariance matrix. The simulation results show that the improved adaptive Kalman filter algorithm combined with cooperative positioning algorithm has less positioning error than Kalman filter algorithm combined with cooperative positioning algorithm， and can be better applied to personnel positioning in underground coal mine.

Keywords： ultra-wideband; underground coal mine positioning; area determination and correction strategy; cooperative positioning algorithm; adaptive Kalman filter

0  引  言

煤炭資源在我国能源中仍占主体地位，是我们国家经济社会发展、社会生产生活之中最为关键和紧要的原始材料和能量来源。近年来我国高度重视煤炭行业的安全生产工作，大力建设并完善煤矿井下人员定位系统。煤矿井下人员定位领域逐步追求达到厘米级的定位精度，相关定位技术和产品已经成为许多行业的研究热点。而UWB技术可以凭借较高的定位精度以及自身良好的工作性能，很好地适应于井下工作环境，逐渐成为近年来煤矿井下定位的首选方式。因此，本文基于UWB技术，对煤矿井下人员定位算法进行研究，实现井下人员实时跟踪与精确定位。

本文采用TOA测距的定位方法进行定位基于到达时间TOA测距的定位方法，它根据UWB信号在空气中的传播时间来获取定位标签和定位基站之间的距离，从而求解出定位标签的位置[1]。采用改进的非对称双边双程测距ADS-TWR算法在一定程度上进行测距上的优化，能够有效地消除节点间时钟未完全同步对测距精度的影响。之后采用协同定位及自适应卡尔曼滤波算法对定位结果进行优化，减小非视距误差对定位结果的影响，提高定位精度。

1  区域判定与校正策略

在实际煤矿井下人员定位时，定位标签的每一次定位都要向所有通信范围内的井下定位基站发送请求帧，进行ADS-TWR测距来实现信息交互。而完成定位并不需要这么多定位基站来通信，这样会造成大量无效通信，一方面消耗节点功率，另一方面增加获取数据的时间，增加系统复杂程度，严重影响定位的实时性和准确性。因此需要进行标签的区域判定与校正，自动判定所处的区域并且只与该区域内的基站进行通信[2]，来提高定位的效率。并且通过区域校正保证判定的标签处在准确的区域内。

1.1  定位基站分布模型

首先根据煤矿井下巷道的特点进行定位基站的分布设计。井下巷道狭窄细长，为了实现人员的精准定位，将巷道进行二维定位模型，定位标签由3个以上基站进行定位。本定位系统将定位基站部署在巷道墙壁两端呈等间距分布状，井下定位基站分布图如图1所示。A0，A1，…，A2n+4为对称分布在巷道两端的定位基站，每四个定位基站形成一个定位区域，将整个巷道分成n个区域。巷道同一侧每两个相邻的定位基站之间的距离为l米，巷道宽即区域宽为m。考虑基站测距的稳定性以及保证定位的精度，l选取3 m。

1.2  定位过程的回传帧机制

在采用ADS-TWR测距算法之后，不需要定位基站和定位标签的时钟同步，减小了定位标签和定位基站之间由时钟漂移造成的测距误差，提高了定位精度。而引入区域判定与区域校正机制后需要在测距后加入一个Resport帧，标签向基站发送Final帧后，基站向标签再发送一个回传帧Resport，其中含有标签基站测距得到的距离值，为标签的区域判定提供测距值。回传帧机制如图2所示：

1.3  区域判定模型

在确定井下巷道内基站的分布以及引入定位过程的回传帧机制后，根据定位基站的分布图来进行区域判定。区域判定主要包括定位基站、测距值以及定位区域三个要素。对着三个要素分别建立集合，分别用I、D、R表示，其中I集合是所有定位基站的编号集合，D集合是测量距离，是在单次定位过程中标签通信范围之内所有的定位基站发送给定位标签的Report回传帧所包含的测距值，R是所有的定位区域，面积为m×l。他们之间建立的关系模型如图3所示：

根据上图关系模型，得到标签的区域判定模型：首先，标签根据接收到所有通信距离内基站发来的回传帧所包含的距离值来判断出5个与自身距离最短的定位基站，并寻找4个编号为基数并且连续的基站，假设这4个基站编号为i1、i2、i3、i4，其中i1=2n+1、i2=2n+2、i3=2n+3、i4=2n+4，则这四个基站Ai1、Ai2、Ai3、Ai4就在同一个定位区域内，他们所围成的定位区域则为Rn。判定到定位区域后，标签则与这几个定位基站Ai1、Ai2、Ai3、Ai4进行通信，Ai1、Ai2、Ai3、Ai4与标签通过ADS-TWR测距算法分别测得距离di1、di2、di3、di4。之后标签根据测得的四个距离di1、di2、di3、di4计算出此时相对于区域Rn的坐标（x，y）。

其中，f（D）表示定位标签根据接收到所有通信距离内基站发来的回传帧所包含的距离值来判断出4个与自身距离最短的定位基站，判别准则为编号是奇数开头并且基站编号为4个连续的编号，编号即为f（D）所求值。

g（i1，i2，i3，i4）表示标签所处的区域编号的求解公式，根据f（D）确定的定位基站编号来确定标签所处的定位区域Rn，判别准则为找到4个连续编号基站中编号最小的基站，令其编号为2n+1，求得n的值即为区域的标号，Zn即为判定区域。

h（Rn）表示根据便签的位置判定区域，来求解对该标签进行定位的基站的编号。判断准则为在获得区域编号n后，求得2n+1、2n+2、2n+3、2n+4，即为基站的编号，并且标签只与这几个基站进行定位。

p（di1，di2，di3，di4，Rn）表示对标签所处区域的判定，利用标签与4个基站的回传帧所包含的测距值来判定变迁是否仍处于当前区域，若不处于则进行区域的更新。判定准则是根据标签在该定位区域的定位坐标来判断，若横坐标x<0，则在编号为n-1的定位区域内;若0l则在编号为n+1的下一区域内。

1.4  区域判定策略

根据区域判定模型来得到区域判定策略：

第一步：标签上电并向所有基站发送定位帧，测距结果通过回传帧Report发送给标签，利用判定模型中f（D）计算出基站（i1，i2，i3，i4）。

第二步：利用式g（i1，i2，i3，i4）计算出定位区域Rn。

第三步：利用式h（Rn）根据判定的四个基站，进行测距通信。

第四步：得到距离值di1，di2，di3，di4，最小二乘法定位得到标签在该定位区域内的坐标值（x，y），之后根据式p（di1，di2，di3，di4，Rn）判定区域，若x<0，则在编号为n-1的定位区域Rn-1内，返回第三步判断新的定位基站;若x>l则在编号为n+1的下一区域Rn+1内，返回第三步判断新的定位基站。

第五步：若标签检测异常，则返回第一步。

1.5  区域校正机制

煤矿井下环境复杂，电磁干扰、环境变化等都会影响标签和基站测距过程，造成一些误差，因此我们建立区域校正机制来针对这些数据异常进行处理校正。它的原理就是检测区域判定过程中的异常情况，一旦检测出数据异常，则进行初始化重新进行区域判定。异常情况包括以下3种：

（1）若第一步无法满足得到四个编号连续且最小的编号为奇数的定位基站，如编号不连续或不满足四個等情况，则无法进行区域的判定，需要进行区域校正。

（2）若没有得到4个距离值或无法结算出定位坐标，说明测距有误，定位基站或定位区域选择有误，则无法进行区域的判定，需要进行区域校正，重新判定。

（3）若基站接收不到通信帧或标签接收不到返回的回传帧，则无法进行区域的判定，说明基站或标签设备存在问题，需要进行区域校正。

综合以上定位基站分布模型、定位过程的回传帧机制、区域判定模型、区域判定过程以及区域校正策略可得，区域判定和校正机制可以解决标签无效通信的问题，降低时间损耗，提高定位精确性和准确度。采用区域判定与校正的系统测距定位总流程图如图4所示。

2  协同定位算法

为了减小煤矿井下非视距误差对人员定位造成的误差，本课题提出一种基于UWB的煤矿井下人员协同定位算法对定位结果进行优化。其基本原理是定位基站充当固定参考节点，定位标签充当移动的参考节点[3]，原理图如图5所示，A1、A2、A3、A4、A5、A6为基站，T1、T2、T3为标签。

目标节点的定位需要三个参考节点的位置信息。图中T1、T3标签节点均满足定位范围内3个定位基站的要求，而T2节点覆盖的通信范围中仅有A1、A5这两个定位基站，按照传统TOA定位方法，不满足三个基站不能够完成定位。但是如果采用文中提出的协同定位的定位方法，标签节点可以充当定位基站与T2进行通信，则可以把T2通信范围内的T1标签节点视为移动的定位参考节点来对T2的进行定位。这种标签当作基站的方式迭代，可以获得所有目标节点的位置。

将参考节点以三个为单位进行分组，假设第k组中参考节点坐标为Ai（xi，yi）、Aj（xj，yj）、Am（xm，ym），目标定位节点坐标（x，y），之间所对应距离为di、dj、dm，列出以下距离方程：

其中，Xt-1是标签节点在（t-1）时刻的坐标，Uth是目标节点的位移阈值上限。当某一时刻第k组参考节点存在异常的测距信息时，表示超出阈值，上式不成立，则该参考节点k组的定位结果不符，应当舍弃。统计所有参考节点组未超过阈值的定位结果，取其平均值作为应求得目标节点在t时刻的定位结果。

下文中将分视距环境和非视距环境两种情况分别进行仿真实验。设置四个参考节点作为定位基站，两个目标节点作为定位标签，验证其中一个少于三个参考节点的目标节点的定位效果，用均方根误差（Root Squared Error， RMSE）作为评价指标。RMSE的计算公式为：

其中定位标签的真实坐标位置为（x，y），由定位算法估计得出的位置坐标估计值为（，）。均方根误差值的大小和实验数据误差呈正相，值越小代表计算精度越高。

视距环境下定位得到x，y轴的误差对比如图6所示，计算出的均方根误差RMSE值如表1所示：

由以上仿真结果可得，在视距环境中，TOA定位算法以及协同定位算法都有较高的定位精度，在此基础上协同定位算法精度稍好于TOA定位算法，性能也有所提升。

在非视距环境中，遮挡住某些参考节点来增大其测距误差。则非视距环境下目标节点x，y轴定位误差对比图如7所示。

可以得出在非视距环境中，协同定位算法目标节点定位误差相对较小。因此，不论是视距环境还是非视距环境中，协同定位算法都比TOA定位算法有更好的定位精度，尤其在非视距环境中优势更明显。因此本文将利用协同定位算法来抑制非视距误差，提高井下工作人员的定位精度。

3  自适应卡尔曼滤波算法

卡尔曼滤波在视距环境中能够消除测量误差，但是在非视距环境中，噪声的分布概率会由于环境的影响出现变化，导致噪声的协方差矩阵难以确定[4]。本文根据协同定位算法解算出的定位结果，获取卡尔曼滤波器的新息向量[5]来检验非视距环境中的测距误差，之后根据检测结果修正协方差矩阵，同时构造观测噪声的协方差矩阵，来减少测距异常值，提高定位精度。算法的流程图如图8所示。

根据系统的状态方程，设置状态参数X=[x，y]，状态转移矩阵A=[1，0;0，1]，Wt为误差向量。假定有一个目标节点MS，参考节点BS数量不少于3个。t时刻第i个BS和MS之间的测距信息表示为：

令α=diag（α1，α2，…，αn），调整系统的观测噪声协方差矩阵，对卡尔曼滤波算法进行自适应修正，在一定程度上可以实现对测量信息的修正，从而提高系统的抗干扰能力，提升定位精度。

4  仿真验证

为了验证协同定位算法以及自适应滤波算法对定位精确度的影响，采用四个参考节点和两个目标节点进行仿真实验，目标节点T1在完成自身定位后用作T2的参考节点，对目标节点T2进行定位。仿真实验的仿真环境为：充当基站的参考节点A1（0，10），A2（10，10），A3（10，0），A4（15，7.5），以及充当标签的目标节点T1坐标位置（5，5），T2坐标（1.5，0.8），单位为米（m）。之后生成多个参考，获取目标节点T2在各参考节点定位下的测距信息，并且加入下如图10所示的目标节点T2的测距随机误差。实际情况中非视距环境测距误差约为50 mm～200 mm，在视为非视距情况。加入的随机测距噪声如图9所示。

将TOA定位算法、协同定位算法配合标准卡尔曼滤波算法（KF）以及协同定位算法配合改进后的自适应卡尔曼滤波算法（AKF）三种定位情况进行仿真实验验证。设置采样间隔1 s，得到目标节点T2的位置，和预设的200个坐标点进行定位坐标的比较。将改进后的自适应卡尔曼滤波算法（AKF）的误警概率设置为0.01。得到的轨迹如图10所示，TOA定位算法的误差如图11所示，KF和AKF定位算法的误差如图12所示：

则由数据可得三种定位算法RMSE值如表3所示。

通过如上仿真实验的验证可得，TOA定位算法的定位误差最大，卡尔曼滤波算法结合协同定位算法很大程度上降低了非视距误差，有很好的定位效果，改进后的自适应卡尔曼滤波算法结合协同定位算法调整协方差矩阵相比卡尔曼滤波算法结合协同定位算法定位误差更小，可以更好地应用到煤矿井下人员定位中。

5  结  论

本文利用TOA的定位方法进行煤矿井下人员定位，在ADS-TWR测距算法的基础上增加区域判定与校正机制减少无效通信，提高定位效率;之后采用协同定位算法将标签作为定位基站进行定位，分别在视距和非视距环境下进行仿真实验验证其对定位精度的提升;最后采用自适应卡尔曼滤波片算法获取信息向量检验非视距环境误差，并对协方差矩阵进行修正，降低误差，并进行仿真实验验证自适应卡尔曼滤波算法结合协同定位算法在非视距误差环境下相比卡尔曼滤波算法结合协同定位算法定位误差更小，可以更好地應用于煤矿井下人员定位中。

参考文献：

[1] GAO S C，ZHANG S J，WANG G，et al. Robust Second-Order Cone Relaxation for TW-TOA-Based Location With Clock Imperfection [J].IEEE Signal Processing Letters，2016，23（8）：1047-1051.

[2] 方文浩，陆阳，卫星.基于区域判定的超宽带井下高精度定位 [J].计算机应用，2018，38（7）：1989-1994.

[3] 蒋晗中.协同定位技术研究综述 [J].通信电源技术，2017，34（6）：48-50.

[4] 邓锴.基于NLOS识别和误差消除的无线定位算法研究 [D].成都：西南交通大学，2018.

[5] 刘韬，徐爱功，隋心.基于自适应抗差卡尔曼滤波的UWB室内定位 [J].传感技术学报，2018，31（4）：567-572.

作者简介：庞明瑞（1997—）女，汉族，山西大同人，硕士研究生在读，研究方向：嵌入式技术;高振航（1998—）男，汉族，河南开封人，硕士研究生在读，研究方向：嵌入式技术;刘璐（1997—）女，汉族，山东滨州人，硕士研究生在读，研究方向：嵌入式技术;臧悦程（1998—）男，汉族，吉林白山人，硕士研究生在读，研究方向：嵌入式技术。