0 引言
基于到达时间(Time of Arrival，TOA)的精确定位方法具有方便部署、定位解算计算量小等优势，被广泛用于井下精确定位中。文献[1]提出一种基于对称双边双向到达时间(Symmetric-Double-Sides Two-Way Time-of-Arrival，SDS-TW-TOA)的无线测距方法，并用于煤矿井下接近探测中。该方法结合各传感器的位置坐标及测距结果得到测距方程组，求解测距方程组得到待定标签坐标。但其先将非线性定位方程平方后相减，再利用最小二乘法来求解，得到的标签坐标会随测距误差的变化而抖动，定位稳定性不好。
煤矿井下接近探测定位与传统室内定位相比有以下难点[2-3]：① 传感器固定在矿车上，矿车作业会影响测距精度及稳定性。② 矿车多为钢铁外壳，对电磁波的透射能力很差，测量标签到某些传感器的距离时非视距(Non Line of Sight，NLOS)误差严重。③ 矿车形状复杂，矿井内地形复杂且会随时间变化，使得NLOS误差存在不确定性，难以消除。最常用的测距方程组直接求解法是Caffery提出的相交线模型[4-5]，该算法运算速度快，适用于受限环境，但当测距存在NLOS误差时，定位误差较大。两边定位法[6-7]可以减小NLOS误差，但当所取的两测距圆不相交时或准确测距的传感器小于一半时，定位误差较大。最小中值法[6,8-9]有很好的定位准确性及鲁棒性，但计算量较大，同时也需要多于一半的传感器测距准确，否则定位误差也较大。
在复杂环境中，用基于泰勒展开的高斯牛顿法求解非线性方程组通常有更好的精度。本文在高斯牛顿法的基础上，结合加权法求解非线性测距方程组，使用Levenberg-Marquardt法(以下简称LM法)解决能准确测距的传感器很少时存在的迭代不收敛问题，使测距方程组的迭代收敛过程更快、更稳定，鲁棒性更强。
1 高斯牛顿法
高斯牛顿法是一种利用泰勒展开并使用迭代求解非线性方程组的方法，这种迭代求解法具有二阶收敛性[10]。
1.1 非线性方程组
设待定位标签到第i个定位传感器的距离为di，二维平面中各定位传感器的位置坐标为(xi,yi)，待定位标签的位置坐标为(x,y)，定位传感器数量为n，则测距方程组可表示为
(1)
将式(1)写成函数形式，即
则方程组中每个方程可写为fi(p)=di。可以看出，fi(p)为非线性函数，要求解p，应当求解由fi(p)=di构成的非线性方程组。
1.2 方程组求解
将f(p)在pk处泰勒展开，忽略二阶及二阶以上展开项[10-11]，得
其中
为雅可比坐标变换矩阵J(pk)的一行，每次迭代中的待求未知量Δp=(x-xk,y-yk)。
fi(p)在pk处一阶泰勒展开后，测距方程组可重新表示为
J(pk)Δp=ε
(2)
式中ε为残差向量，ε=d-f(pk)，d为测得的距离向量。
井下定位过程中传感器通常都有冗余，因此，本文只讨论测距方程组为超定方程的情况。若测距方程组为超定方程，则Δp可用最小二乘法求解，即Δp=(JTJ)-1JTε。求得Δp后，用Δp更新pk+1，pk+1=pk+Δp，并将pk+1作为下次迭代计算的泰勒展开点，通过迭代逐步逼近标签实际位置。
1.3 迭代初值选取
若标签为第1次初始化，可以用直接求解法快速求得准确度较低的坐标，并将其作为迭代初值；若标签已初始化，则可用上次计算得到的坐标作为本次迭代计算的初值。当距离向量的采样间隔较小时，这种初值选取方法可以提高迭代效率，加快迭代收敛速度。
1.4 迭代终止条件
迭代终止条件：① 残差范数ε2小于残差最小值ε1，即
② 坐标的最大变化值小于ε2，即
③ 达到最大迭代次数kmax。ε1，ε2，kmax的选取需要权衡收敛精度及计算速度，可通过实验选取经验值。
2 加权最小二乘法
高斯牛顿法没有考虑测距的误差信息。加权最小二乘法[12-13]是最小二乘法的一种优化算法，可以求解存在异方差的模型，即可以对方程组中的方程分别设置权值，将测量误差信息加入方程组的求解过程中。将加权最小二乘法与高斯牛顿法每一步迭代中待求解的超定方程组相结合，可得到基于加权最小二乘法的非线性方程组求解方法。
2.1 加权最小二乘法方程组
对式(2)中的第i个迭代方程Ji(pk)Δp=εi两边同时乘以
得
为第i个传感器对应的测距方程的权值。设权重矩阵
则待求解的迭代方程组可表示为
(3)
式(3)的最小二乘解可表示为Δp=(JTWJ)-1JTWε。
2.2 权值选取
权重矩阵W为对角阵，理想情况下其元素wi为对应传感器测距误差的方差倒数。实际测量的准确度与测量方差成反比，即测距准确度越高，测量方差越小，wi越大。在煤矿井下接近探测定位中，测量距离越大，越可能受到了NLOS的干扰，对应wi越小；反之，测量距离越小，精确测量的可能性也越高，对应wi越大。基于上述分析，结合实验结果，可以构造如下wi计算式：
(3)
式中dref=D(n+1)/2，D为从小到大排列的测距序列。
在实际计算过程中，可以忽略di≥dref的测距，即wi≈0时对应的测距值，以简化计算。
3 加权LM法
3.1 加权LM法原理
高斯牛顿法迭代收敛的充分条件：①
② 雅可比矩阵J始终满秩[10]。在实际的井下定位过程中，测距可能存在较大误差，甚至可能因遮挡或测距传感器测距延时过大而导致没有测距结果，从而使得雅可比矩阵病态，不能满足迭代收敛条件。因此，在复杂情况下，用加权最小二乘法迭代求解测距方程组的收敛性不能得到保证。针对该问题，需要在迭代过程中加入阻尼以提高迭代求解的稳定性。
LM法是一种加入阻尼系数的迭代求解法，其本质是通过阻尼系数λ将一阶收敛的最速下降法与二阶收敛的高斯牛顿法相结合[10]，在不明显影响收敛速度的前提下提高迭代的收敛性和稳定性。使用LM法求解原始非线性测距方程组时，Δp=(JTJ+λI)-1JTε，其中阻尼系数λ为大于0的实数。在算法实现时，可将单位矩阵I替换为JTJ的对角线矩阵diag(JTJ)，以减小数值计算误差及对参数λ的依赖[11]，使数值计算更加稳定。
加权LM法将LM法与加权最小二乘法结合起来。加入权值后，Δp在迭代过程中的计算过程可写为Δp=(JTWJ+λI)-1JTWε。
3.2 阻尼系数的选取
阻尼系数λ控制着高斯牛顿法与最速下降法的结合比例及最速下降法的收敛速度。若λ选取过大，则每次迭代残差向量的模
的下降速度会很小，收敛较慢；若λ选取过小，依然会出现不收敛的情况。迭代的每一步，希望在保证残差下降的情况下使
尽可能大，以便更快收敛。λ的选取策略：在稳定下降时逐步减小λ；当上升时不断增大λ，直到残差下降时再逐步减小λ。
4 测试分析
4.1 测试1
在采煤机上部署4个测距传感器，测量各自到标签的距离，然后计算标签坐标。计算参数设置：ε1=100，ε2=4，kmax=15，λ初值为0.3。 使用加权LM法计算标签坐标，结果如图1所示，其带数字的黑点处为测距传感器在采煤机上的位置，圆形的半径为对应传感器测距，采煤机下方的小圆点为算得的标签坐标。标签的实际位置与标签计算位置一致。从图1可看出，1号传感器测距值比真实距离要大很多，同时1号传感器与标签的直线连接需要穿过矿车车身，可以认为这是由于机车遮挡造成NLOS所引起的。0号、2号、3号传感器的测距结果与传感器到标签的真实距离相差不大，可以认为是没有受到NLOS影响的正常测距值。从测试结果可以看出，使用加权LM法可以有效减弱1号传感器测量数据的影响，同时对2号及3号传感器测距结果给以加强，使计算结果更精确。
图1 采煤机与传感器的位置关系
Fig.1 Positional relationship between shearer and sensors
分析标签连续运动时，取同一标签连续1 676次传感器测距向量作为样本，用不同的定位算法求解标签的位置坐标，结果见表1。均方根的计算公式为
为标签的真实坐标，n=1 676；计算总时间指在I5-6200U平台上，单标签1 676次定位计算的用时。最小中值法的样本选取总数为
即选取4个样本。
表1 不同定位算法计算结果
Table 1 Calculation results of different
positioning algorithms
从表1可以看出，最小中值法与加权LM法有着相近的高计算精度，加权LM法比最小中值法计算效率高3倍以上。直接最小二乘法虽然计算速度最快，但是精度较差。非线性高斯牛顿法、加权最小二乘法及加权LM法虽然计算复杂度提升，计算用时上升，但计算精度却有大幅提高。单独使用加权最小二乘法求得解的稳定性不如加权LM法好，求得的坐标会存在某些不收敛的跳变点，使其均方根误差比加权LM法大。
用加权LM法计算标签坐标并将求得的坐标依次首尾相连，得到标签运动轨迹，如图2所示，标签运动轨迹平滑，与真实轨迹一致。
图2 定位效果及标签运动轨迹
Fig.2 Positioning effect and label movement trajectory
4.2 测试2
大型采煤机上需要安装6个测距传感器，由于设备较大，受NLOS影响的传感器数量更多。使用加权LM法计算标签坐标(参数设置与测试1中相同)，结果如图3所示，图中0号、1号、3号传感器受到了NLOS的影响，测距精度很低。从图3可知，加权LM法通过权值的选择有效减小了NLOS的影响，标签的计算坐标与真实坐标误差很小。
图3 含有6个传感器时的定位效果
Fig.3 Positioning effect with 6 sensors
用表1中的5种算法分别计算标签坐标，并记录计算坐标与真实坐标的距离误差，计算结果见表2，其中距离误差的计算式为
表2 不同定位算法的误差
Table 2 Error of different positioning algorithms
直接最小二乘法与非线性高斯牛顿法的误差很大，其解不能作为正确的定位结果使用；最小中值法由于正确测量的传感器数量小于测距传感器总数的一半，所以也没有取得较好的精度；加权最小二乘法及加权LM法由于选取的权值恰当，计算精度都比较高。
5 结语
将高斯牛顿法、加权最小二乘法及LM法相结合，介绍了一种鲁棒的矿井接近探测定位算法——加权LM法。该算法有较强的鲁棒性，在具有NLOS误差的复杂场景，甚至在只有2个传感器进行准确测距的情况下，都能得到较准确的标签坐标。同时该算法的时间及空间复杂度可满足在单片机上运行的条件。测试结果表明，加权LM法的定位效率和精度较高。在实际工作中，即使测距过程中某些传感器工作异常，不能返回测距结果，该算法也能正常运行。
参考文献(References)：
[1] ALARIFI A,ALSALMAN A M,ALSALEH M,et al.Ultra wideband indoor positioning technologies: analysis and recent advances[J].Sensors,2016,16(5):1-36.
[2] WYLIE M P,HOLTZMAN J.The non-line-of-sight problem in mobile location estimation[C]// IEEE International Conference on Universal Personal Communications,1996.
[3] 包建军.煤矿井下装备接近探测方法研究[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.
[4] CAFFERY J J.A new approach to the geometry of TOA location[C]//Vehicular Technology Conference,Boston,2000.
[5] CHENG G.Accurate TOA-based UWB localization system in coal mine based on WSN[J].Physics Procedia,2012,24(16):534-540.
[6] LI X,HUA B,SHANG Y,et al.A robust localization algorithm in wireless sensor networks[J].Frontiers of Computer Science in China,2008,2(4):438-450.
[7] NORRDINE A.An algebraic solution to the multilateration problem[C]//International Conference on Indoor Positioning and Indoor Navigation,2012.
[8] LI Z,TRAPPE W,ZHANG Y,et al.Robust statistical methods for securing wireless localization in sensor networks[C]//IEEE International Symposium on Information Processing in Sensor Networks,2005.
[9] ROUSSEEUW P J.Least median of squares regression[J].Publications of the American Statistical Association,1984,79(388):871-880.
[10] MADSEN K,NIELSEN H B,TINGLEFF O.Methods for nonlinear least squares problems[J].Society for Industrial & Applied Mathematics,2004,2012(1):1409-1415.
[11] 王磊,李鹏涛,贾宗璞.基于全质心-Taylor的UWB室内定位算法[J].传感器与微系统,2017,36(6):146-149.
WANG Lei,LI Pengtao,JIA Zongpu.UWB indoor localization algorithm based on full centroid and Taylor[J].Transducer and Microsystem Technologies,2017,36(6):146-149.
[12] 李春华,蔡成林,邓克群,等.一种北斗伪距单点定位的加权最小二乘(WLS)快速算法[J].重庆邮电大学学报(自然科学版),2014,26(4):466-472.
LI Chunhua,CAI Chenglin,DENG Kequn,et al.A fast pseudo-range single point positioningalgorithm for the Beidou navigation system based on weighted least squares[J].Journal of Chongqing University of Posts and Telecommunications(Natural Science Edition),2014,26(4):466-472.
[13] 潘旦光,高莉莉,李小翠.求解Rayleigh阻尼系数的加权最小二乘法[J].计算力学学报,2015,32(1):70-76.
PAN Danguang,GAO Lili,LI Xiaocui.Weighted least squares method for solving Rayleigh damping coefficients[J].Chinese Journal of Computational Mechanics,2015,32(1):70-76.