0 引言
基于UWB(Ultra Wide Band，超宽带)的精确定位技术具有定位精度高、发射功率低和抗多径效应能力强等特点，近年来成为井下精确定位技术的研究热门[1-3]。目前，采用UWB技术研发成功的近感检测装置可安装于机车上，实时探测附近携带标签卡的工人的位置[4]。若有标签卡进入沿车身周围预先划设的警告区或危险区，近感检测装置可触发声光报警，引起作业人员注意，使机车制动停机，从而有效防止因井下环境复杂导致的机车挤伤工人的事故，保护工人安全。
不同于普通的室内定位，井下存在多种影响定位精度的噪声，其中影响最大的是由遮挡造成的NLOS(Non-Line of Sight，非视距)误差。多标签多锚节点同时测距法[5]和多边定位法能较有效保证定位的实时性和稳定性，但在实际应用中，因为NLOS误差计算没有固定模型，在某些特殊的遮挡位置，个别锚节点的测量精度仍可能会受到较强干扰。而具有最小二乘思想的多边定位法认为各锚节点的权重相同，因此没有从根本上对测距结果波动较大的锚节点进行弱化处理，定位结果可能出现较大误差。在标签卡的运动轨迹上，这些误差会形成突刺点，突刺点可能会进入划设的警告区或危险区，引起误报警甚至机车制动或停机。
卡尔曼滤波器是一种高效率的递归滤波器，它利用隐含马尔科夫模型和线性代数建立理论模型，能够从一组包含噪声干扰的测量信息中提取有效信息，估计系统的动态变化，对测量信息进行优化修正。基于高斯牛顿法、Levenberg-Marquard(LM)法和加权最小二乘法的加权LM法[6-8]能解决井下复杂环境应用中多边定位方程组求解困难的问题，具有较高的鲁棒性。因此，本文提出了一种基于卡尔曼滤波和加权LM法的井下精确定位算法(简称卡尔曼滤波-加权LM法)，该方法不需要进行批处理或大量迭代运算，能有效提高UWB定位的准确性。
1 卡尔曼滤波-加权LM法设计
1.1 算法流程
卡尔曼滤波-加权LM法实现步骤如下：
(1) 设计卡尔曼预测过程模型，根据选取的系统状态参数向量
确定状态转移矩阵F、控制矩阵B、控制量u和预测过程协方差矩阵Q。通过预测过程模型对t时刻的及其协方差矩阵
进行先验估计，得到
和
(2) 根据定位坐标的先验估计结果和各锚节点的测距值得到锚节点的权值矩阵，根据权值矩阵、各锚节点的坐标和测距值，使用加权LM法得到新的定位结果，将其作为卡尔曼滤波更新过程中的测量值zt。
(3) 设计卡尔曼更新过程模型，根据测量矩阵H、测量值zt和测量过程协方差矩阵R得到卡尔曼增益K，然后得到卡尔曼滤波后验估计值
和
即最终的优化定位结果。
卡尔曼滤波为迭代过程，经过预测过程和更新过程得到的t时刻最优后验估计结果成为t+1时刻的迭代初值，算法流程如图1所示。
1.2 卡尔曼滤波
卡尔曼滤波算法是一种最优化自回归数据处理算法，能实时修正滤波增益矩阵，从包含噪声信息的测量数据中提取有效信息，修正状态参量[9-11]。该算法只需要当前的测量值和前一个采样周期的后验估计值就能够进行状态估计，不需要大量的存储空间，计算量小，非常适合嵌入式开发。在井下UWB精确定位算法中，根据标签卡的移动特征设计卡尔曼预测过程，然后在更新过程中结合利用加权LM法得到的测量值对定位结果进行修正，能有效解决在复杂环境中使用多边定位法时存在的定位结果不准确或无法收敛的问题。
图1 井下精确定位算法流程
Fig.1 Underground precise positioning algorithm flow
卡尔曼滤波主要包括预测和更新2个过程：
(1) 预测过程体现的是系统的动态特性，即系统的特征参数在前后时刻的对应关系，预测过程为
(1)
(2)
在具体定位中，为根据t-1时刻的迭代结果得到的定位参数向量的先验估计，
为t时刻的先验估计协方差矩阵，控制矩阵B用于描述控制量ut对预测结果的影响，状态转移矩阵F反映标签卡运动状态变化。
(2) 更新过程是结合先验状态估计和当前t时刻的测量信息对当前时刻的参量进行后验估计[12-14]，更新过程为
(3)
(4)
(5)
式中I为单位矩阵。
在具体定位中，卡尔曼增益Kt用于对系统状态和协方差矩阵进行优化修正，测量矩阵H反映预测参量与测量值之间的关系，
为卡尔曼滤波后的估计值，
为滤波后更新的协方差矩阵。
1.3 加权LM法
LM法是一种结合一阶收敛的最速下降法和二阶收敛的高斯牛顿法的迭代求解方法。加权LM法将锚节点的测距权值加入非线性方程求解中，能用较少的迭代次数实现快速收敛，且能得到较准确的定位结果[15]。
经卡尔曼滤波预测过程得到t时刻标签卡坐标的先验估计值
利用几何关系计算其与各锚节点的距离
并计算出测距差值：
(6)
式中ri为探测器的直接测距值。
根据Δri的大小分配各锚节点的测距权值：
(7)
求得各锚节点测距权值后，使用加权LM法求得迭代过程中标签卡的位移：
ΔD=(JTWJ+λI)-1JTWξ
(8)
式中：ΔD为标签卡位移向量；J为雅克比坐标变换矩阵；W为权值矩阵；λ为阻尼系数；ξ为残差向量。
设定迭代终止条件，根据已知的各锚节点坐标pd1—pd6和r1—r6，在用高斯牛顿法迭代求解非线性方程组的过程中求得J和ξ；然后结合权值w1—w6，根据式(8)求得每次迭代的位移向量ΔD并进行累加更新，直至迭代终止，求得标签卡的最终坐标。
2 测试分析
2.1 测试环境
采用应用于WXP100BL型铲板车的近感检测系统验证卡尔曼滤波-加权LM法的性能，系统工作时的实时探测演示画面如图2所示。图2中，机车位于4 000 cm×3 000 cm区域的中心，红色区域为危险区，黄色区域为警告区，165号标签卡以图标的形式实时动态显示。沿车身周围1.5 m划定测试路线，在标签卡和车身之间随机摆放影响定位精度的尺寸不等的金属障碍物，造成非视距误差；然后将标签卡沿着测试路线行走的测距数据和定位结果实时打印到记录文件中，每5 min保存一个记录文件，每个文件最多包含600条定位相关数据。在上位机中使用C#开发图形化数据分析软件，方便对各种测试数据进行观察比较。
2.2 参数设计
锚节点(通常为6个)分布于机车外侧，坐标已知。以t时刻的标签坐标(px,py)和x轴、y轴方向的速度分量vx,vy作为卡尔曼滤波系统状态变量参数，即
xt=(px,py,vx,vy)T
(9)
锚节点每0.5 s对标签卡进行一次测距，标签卡在相邻定位点之间的移动可认为是匀速直线运动，因此不存在控制增量，所以状态转移矩阵为
图2 近感检测系统实时探测演示画面
Fig.2 Real time detect demo screen of proximity detection system
(10)
测量值为t时刻的位置坐标，因此测量矩阵为
(11)
速度参量只与单位时间内标签卡的位移有关，与标签卡坐标无关，因此预测过程噪声协方差矩阵Q为4×4的对角阵，同理可得R为2×2的对角阵。
迭代初值可从测试数据集合的第2个点开始，利用前2个点的位置坐标p0(p0,x,p0,y)和p1(p1,x,p1,y)可得初始速度参量为
v0=((p1,x-p0,x)/0.5,(p1,y-p0,y)/0.5)T
(12)
2.3 测试结果分析
携带标签卡沿设定标准路线行走一周，分别用多边定位法和卡尔曼滤波-加权LM法对标签卡进行多次定位，顺次连接各定位点形成相应的移动轨迹，结果如图3所示，图中的0～5表示安装在不同位置的锚节点。
图3 标签卡移动轨迹对比
Fig.3 Comparison of moving tracks of tag card
从图3可看出，使用卡尔曼滤波-加权LM法定位得到的移动轨迹比多边定位法更加接近真实轨迹，说明卡尔曼滤波-加权LM法能有效抑制因噪声干扰导致的轨迹波动，减少因信号遮挡造成的区域误识别。2种方法的平均定位误差对比见表1。
表1 平均定位误差统计
Table 1 Average positioning error statistics
改变标准轨迹半径，再进行3次测试，统计卡尔曼滤波-加权LM法的耗时情况，结果见表2。
表2 卡尔曼滤波-加权LM法耗时统计
Table 2 Time-consuming statistics of Kalman filter and weighted LM algorithm
由表1和表2可知，卡尔曼滤波-加权LM法的平均误差比多边定位法减小了50%以上，且因为不需要冗长复杂的迭代求解，单次定位耗时远低于1 ms，完全满足2～3次/s定位的需求。
3 结论
(1) 针对目前基于UWB精确定位的近感检测装置定位结果易受噪声干扰的问题，提出了卡尔曼滤波-加权LM法，介绍了该定位方法的流程及具体实现。测试结果表明，与多边定位法相比，卡尔曼滤波-加权LM法可在不影响定位速度的前提下，将定位精度提高一倍以上。
(2) 随着井下高精度定位技术的发展，可在卡尔曼滤波模型中加入惯性导航信息(如位移加速度、角速度)，为卡尔曼滤波的预测过程提供更多参考量，提高滤波性能，从而得到更接近真实值的加权LM法输入量，进一步提高井下定位精度。
参考文献（Ｒｅｆｅｒｅｎｃｅｓ）：
[1] 隋心,杨广松,郝雨时,等.基于UWB TDOA测距的井下动态定位方法[J].导航定位学报,2016,4(3):10-14.
SUI Xin,YANG Guangsong,HAO Yushi,et al.Dynamic positioning method based on TDOA in underground mines using UWB ranging[J].Journal of Navigation and Positioning,2016,4(3):10-14.
[2] 刘韬,徐爱功,隋心.基于自适应抗差卡尔曼滤波的UWB室内定位[J].传感技术学报,2018,31(4):567-572.
LIU Tao, XU Aigong,SUI Xin.Adaptive robust Kalman filtering for UWB indoor positioning[J].Chinese Journal of Sensors and Actuators,2018,31(4):567-572.
[3] 王飞.基于TOF技术的煤矿井下精确定位系统设计与实现[J].煤炭技术,2015,34(10):262-264.
WANG Fei.Design and implementation of coal mine precise positioning system based on TOF technology[J].Coal Technology,2015,34(10):262-264.
[4] 包建军.煤矿井下装备接近探测方法研究[J].工矿自动化,2017,43(1):1-4.
BAO Jianjun.Research on proximity detection method for machines in coal mine[J].Industry and Mine Automation,2017,43(1):1-4.
[5] 陈康,王军,包建军,等.基于消息复用的TOF井下精确定位技术[J].工矿自动化,2019,45(2):1-5.
CHEN Kang,WANG Jun,BAO Jianjun,et al.TOF underground accurate positioning technology based on message multiplexing[J].Industry and Mine Automation,2019,45(2):1-5.
[6] 廖芹,姚丽.基于LM算法的相关模糊神经网络及其应用[J].科学技术与工程,2017,17(11):260-266.
LIAO Qin,YAO Li.LM algorithm based relevance fuzzy neural network and its application[J].Science Technology and Engineering,2017,17(11):260-266.
[7] 张元刚,刘坤,白猛,等.井下多传感器组合导航系统[J].工矿自动化,2019,45(7):10-16.
ZHANG Yuangang,LIU Kun,BAI Meng,et al.Underground multi-sensor integrated navigation system[J].Industry and Mine Automation,2019,45(7):10-16.
[8] 吴静然,崔冉,赵志凯,等.矿井人员融合定位系统[J].工矿自动化,2018,44(4):74-79.
WU Jingran,CUI Ran,ZHAO Zhikai,et al.Mine personnel fusion location system[J].Industry and Mine Automation,2018,44(4):74-79.
[9] 赵广辉,卓松,徐晓龙.基于卡尔曼滤波的多目标跟踪方法[J].计算机科学,2018,45(8):253-257.
ZHAO Guanghui,ZHUO Song,XU Xiaolong.Multi-object tracking algorithm based on Kalman filter[J].Computer Science,2018,45(8):253-257.
[10] 关伟,单伶燕,肖倩.卡尔曼滤波在道路动态响应数据处理中的应用研究[J].自动化与仪表,2019,34(1):53-57.
GUAN Wei,SHAN Lingyan,XIAO Qian.Application of Kalman filter in road dynamic response data processing[J].Automation & Instrumentation,2019,34(1):53-57.
[11] 高端阳,李安,傅军,等.基于鲁棒无迹卡尔曼滤波的无线室内定位算法[J].中国惯性技术学报,2018,26(6):768-772.
GAO Duanyang,LI An,FU Jun,et al.Wireless indoor location algorithm based on robust unscented Kalman filter[J].Journal of Chinese Inertial Technology,2018,26(6):768-772.
[12] 王妍,邓庆绪,刘赓浩,等.结合运动方程与卡尔曼滤波的动态目标追踪预测算法[J].计算机科学,2015,42(12):76-81.
WANG Yan,DENG Qingxu,LIU Genghao,et al.Dynamic target tracking and predicting algorithm based on combination of motion equation and Kalman filter[J].Computer Science,2015,42(12):76-81.
[13] 孙继平,李晨鑫.基于卡尔曼滤波和指纹定位的矿井TOA定位方法[J].中国矿业大学学报,2014,43(6):1127-1133.
SUN Jiping,LI Chenxin.Mine time of arrival positioning method based on Kalman filtering and fingerprint positioning[J].Journal of China University of Mining & Technology,2014,43(6):1127-1133.
[14] 周恭谦,杨露菁,刘忠.基于TDOA的自适应扩展卡尔曼滤波跟踪算法研究[J].舰船电子工程,2018,38(7):30-33.
ZHOU Gongqian,YANG Lujing,LIU Zhong.Adaptive interation extended Kalman filter tracing algorithm based on TDOA[J].Ship Electronic Engineering,2018,38(7):30-33.
[15] 陈康,包建军,王伟.一种鲁棒的矿井接近探测定位算法研究[J].工矿自动化,2018,44(6):11-15.
CHEN Kang,BAO Jianjun,WANG Wei.Research on a robust positioning algorithm for mine proximity detection[J].Industry and Mine Automation,2018,44(6):11-15.