CN111852456A - 基于因子图的鲁棒uwb井下锚杆钻孔定位方法 - Google Patents

Abstract

本发明公开了一种基于因子图的鲁棒UWB井下锚杆钻孔定位方法，可自动测量出锚杆打孔位置的三维坐标，且无需预先测量基站的位置。具体来说，在因子图的框架下，提出了改进的基于Turkey鲁棒核的UWB定位算法，将标签与基站坐标一起作为未知参数求解。该算法克服了最小二乘进行UWB定位对非高斯噪声的天生缺陷，也避免了鲁棒核函数(如Turkey)的过度优化的缺点。针对UWB数据由于遮挡一般大于真实值的特点，选择只对测距偏大的数据加入鲁棒核；而测距偏小的数据很可能由于定位错误引起，以其残差的平方作为优化目标函数进行优化求解。实验结果表明，改进的基于Turkey鲁棒核的UWB定位算法及定位设备，在非视距环境下，可以获得精度在2厘米以下的打孔位置精度。

Description

基于因子图的鲁棒UWB井下锚杆钻孔定位方法

技术领域

本发明涉及井下锚杆钻孔定位技术领域，具体涉及一种基于因子图的鲁棒UWB井下锚杆钻孔定位方法。

背景技术

在矿井下巷道的掘进施工中，顶板的支护有很大的意义。锚杆支护的采用是一项关键的技术，支护的稳定性直接决定井下巷道的安全与否，关乎井下工作者的人身安全，并决定着企业的施工效率、施工质量情况及经济效益。在巷道顶板上安装吊挂锚杆时，首先需要对锚杆的安装位置进行定位，打好钻孔，再安装锚杆。但是由于巷道顶板高度较高，在测量锚杆打孔位置时定位困难，往往需要两人登高协同作业，比如一人手持标尺定位，另一人手持标记器在顶板标记孔位，操作复杂，导致劳动强度大，耗时多。本发明提出一种基于多标签约束的UWB定位方法，可自动测量出打孔位置的三维坐标，打孔机器人利用该坐标位置，结合自身的定位信息，便可实现自动打孔，极大的提高了打孔的效率，降低了劳动强度。

UWB信号定位精度相对较高，在视距(Line-of-sight，LOS)环境下定位精度可以控制到0.3m左右。然而，UWB传感器在井下使用仍然面临一些困难，多径效应在狭窄巷道空间大大加强，而且信号有可能随时被井下工作人员或行进车辆遮挡，这时电磁波传播速度与其在空气中传播速度相差甚远，测量值将带有很强的非高斯噪声。另外由于UWB信号的测距范围有限，每次只能测量一定范围区域的打孔坐标。这就意味着UWB基站在测量完当前区域时，要移动到下一个待测区域。如何快速方便的解决每个区域UWB基站的临时坐标位置，也是我们要解决的问题。

发明内容

本发明拟通过因子图优化的方法对NLOS条件下的UWB数据进行定位，该方法通过构造代价函数并进行求解估计系统状态，特别的，算法中把UWB基站的坐标也作为待求的状态参数之一，这样就不需要每次变换测量区域再进行基站坐标的测量。因子图的构建基于测距目标的状态节点、基站节点及五种因子构成。因子图优化方法保留了系统之前的一系列状态，充分利用了状态之间的相互关系进行定位估计，实现了噪声较大的条件下的精确的状态估计。利用一系列预算设计的约束就能实现对定位结果很大程度的修正。同时，针对UWB测距值在NLOS条件下，一般测距值偏大的特点，只对实际测距值大于估计测距值的数据增加鲁棒核；而小于测距值的数据很可能由于定位误差引起，这不属于测距异常数据，故以其测距残差的平方作为优化目标函数。

具体技术方案如下：

一种基于因子图的鲁棒UWB井下锚杆钻孔定位方法，包括如下步骤：

步骤1：在待钻孔位置下方设置垂直的测量杆，测量杆上纵向设置若干个第一类定位标签，最上方第一类定位标签与待钻孔位置之间的距离已知，相邻第一类定位标签之间的距离已知；测量杆的周围设置至少三个高度相同的基站，每个基站的相邻位置设置一个第二类定位标签，基站与其相邻第二类定位标签的距离已知；设定其中一个基站的坐标为局部坐标(0,0,h)，h为基站的高度；

步骤2：测量各个第一类定位标签和第二类定位标签与各个基站的距离；

步骤3：估算出其余基站、第一类定位标签和第二类定位标签的初始坐标，然后利用改进的因子图鲁棒优化算法计算出各个基站、第一类定位标签和第二类定位标签相对于该已知坐标基站的精确相对位置；

步骤4：根据第一类定位标签的坐标，计算出待钻孔位置的坐标。

进一步的，步骤1中所述测量杆为顶部设有接触式传感器的可伸缩的测量杆。

进一步的，最上方第一类定位标签与待钻孔位置的距离为60厘米。

进一步的，步骤3中因子图优化算法的具体步骤包括：

步骤3.1：构建因子图；因子图包括若干个状态节点和表示状态节点间相互约束的若干个因子，若干个因子包括：(1)第一类定位标签的位置变化的误差函数；(2)第二类定位标签与各个基站的测距值的误差函数；(3)第一类定位标签与各个基站的测距值的误差函数；(4)基站与其相邻第二类定位标签之间距离的误差函数；(5)相邻第一类定位标签之间距离的误差函数；

步骤3.2：在每一次获得测距值时计算所有第一类定位标签、第二类定位标签和基站的坐标，并计算第一类定位标签、第二类定位标签和基站之间的估计测距值；

步骤3.3：当实际测距值大于估计测距值的实际测距值时误差函数增加鲁棒核，对于小于估计测距值的实际测距值，误差函数以其测距残差的平方作为优化目标函数；

步骤3.4：根据因子构建代价函数，通过求解代价函数的最小值得到因子图最优状态节点序列。

进一步的，基站有四个，第一定位标签有三个，第二定位标签有四个，所述误差函数(1)-(5)分别为：

(1)e_f1＝[x_t+2-x_t+1]-[x_t+1-x_t]＝c；x_t表示t时刻第一类定位标签的三维坐标；c为相邻第一类定位标签之间的距离；

(2)当实际测距值大于估计测距值的实际测距值时误差函数为e_f2＝tukey[r_tag1～4-norm(x_t,tag1～4,B_t,1～4)]；当实际测距值小于估计测距值的实际测距值时误差函数为e_f2＝[r_tag1～4-norm(x_t,tag1～4,B_t,1～4)]²；r_tag1～4表示t时刻第二类定位标签与基站的实际测距值，norm(x_t,tag1～4,B_t,1～4)表示t时刻第二类定位标签与基站的估计测距值，x_t,tag1～4表示t时刻第二类定位标签的三维坐标，B_t,1～4表示t时刻基站的三维坐标，tukey为鲁棒核函数，δ表示鲁棒核函数的门限值；

(3)当实际测距值大于估计测距值的实际测距值时误差函数为e_f3＝tukey[r_tagA～C-norm(x_t,tagA～C,B_t,1～4)]；当实际测距值小于估计测距值的实际测距值时误差函数为e_f3＝[r_tagA～C-norm(x_t,tagA～C,B_t,1～4)]²；r_tagA～C表示t时刻第一类定位标签与基站的实际测距值，norm(x_t,tagA～C,B_t,1～4)表示t时刻第一类定位标签与基站的估计测距值，x_t,tagA～C表示t时刻第一类定位标签的三维坐标；

(4)e_f4＝norm(x_tb,B_t)_tag1～4＝a；a表示基站与其相邻第二类定位标签的距离；

(5)e_f5＝norm(x_tc,x_tc′)_tagA～C＝b；b表示相邻第一类定位标签的距离。

有益效果：本发明自动测量出锚杆打孔位置的三维坐标，且无需预先测量基站的位置。具体来说，在因子图的框架下，提出了改进的基于Turkey鲁棒核的UWB定位算法，将标签与基站坐标一起作为未知参数求解。该算法克服了最小二乘进行UWB定位对非高斯噪声的天生缺陷，也避免了鲁棒核函数(如Turkey)的过度优化的缺点。针对UWB数据由于遮挡一般大于真实值的特点，选择只对测距偏大的数据加入鲁棒核；而测距偏小的数据很可能由于定位错误引起，以其残差的平方作为优化目标函数进行优化求解。实验结果表明，改进的基于Turkey鲁棒核的UWB定位算法及定位设备，在非视距环境下，可以获得精度在2厘米以下的打孔位置精度。

附图说明

图1为UWB定位试验场；

图2为基于UWB的井下定位因子图结构；

图3为Tukey损失函数在不同参数下的值；

图4为UWB数据样例；

图5为UWB定位结果。

具体实施方式

下面结合实施例和附图对本发明做进一步解释说明。

1、实验场景和设备说明

为了评估定位算法在NLOS环境下的精度，在中国矿业大学计算机一楼大厅模拟巷道进行试验，共采用了四个基站，如图1所示：

在巷道的待测量区域内的四个角放置4个三角支架。每个支架的高度为1.2米。在支架的上方是一个高度、宽度均为20厘米的U型支架，其上安装一个定位基站及定位标签。定义图中左下方的基站和标签分别为Anchor1/Tag1，其局部坐标为(0,0)。按顺时针方向，另外三个基站及标签分别定为Anchor2-4/Tag2-4。这样对于定位算法而言，相当于额外增加了两种约束，一种是同一三脚架上的定位基站与标签间的已知距离20厘米。另一种是每个标签与不同三脚架的其余三个基站的测距约束。

顶板打孔坐标的测量，由一个可自由移动、伸缩的测量杆来实现。测量时，移动测量杆于待测打孔位置的下面，然后控制测量杆向上升高。在伸缩杆的顶部，安装接触式传感器，接触式传感器的目的是让测量杆能够明确的探测到当前打孔坐标的最高处位置。即当测量杆上升到接近顶板的时候，停止上升。接触式传感器的精度可达到1毫秒，从安全及稳定性考虑，实测中设置为距顶板5毫秒时停止上升。

测量杆上还需安装三个定位标签。在顶部的下方间隔60厘米处，安装定位标签一个，定义为TagA。之所以在顶部下方60厘米处安装定位标签，是充分考虑了UWB信号的多径效应。如果标签的天线过于接近顶板，则大量的反射信号会造成测距的误差。在TagA的下方20厘米处，再安装一个定位标签，定义为TagB。在TagB的下方20厘米处，安装最后一个定位标签。之所以安装3个定位标签，是为了更好的限定打孔坐标的Z轴的位置。因为首先三个标签与打孔位置的X/Y坐标一致，在算法中可以做为一个约束条件去求解，即三个标签的X/Y坐标共在一条垂线上，而打孔位置的X/Y坐标也在这条垂线上。另一个约束条件是三个标签与顶板的Z轴坐标相对位置已知。这些已确定的约束条件会加速求解的收敛速度与精度。

由于井下能见度较低，每次测量基站的相对坐标位置比较困难。本发明通过基站与标签组合的方式，对定位的不同基站间进行相互测距，增加了算法的定位约束条件，从而能够动态的自动确定基站的相对位置，减少了每次转换定位区域就要重新测量基站位置的工作，提高了巷道顶板孔位的定位效率。

2、定位算法描述

1)因子图的构建

基于因子图优化算法在对系统状态进行计算时，是一种增量式的计算；相比于批量图优化每次计算并更新所有历史状态，增量图优化算法可以自适应的判断当前测量是否影响某个历史状态，从而判断是否要计算更新，从而减少不必要的计算。

在图优化问题中，定义系统状态序列：

X_t表示第t个节点中所有定位标签的的三维坐标，n表示状态节点数量。以本文实验方案的四个基站及三个测量标签为例，

由于基站和第二类标签的相对关系已知，故其坐标不在状态节点中计算。为了对系统状态进行求解，建立代价函数，通过求解代价函数的最小值得到最可能的系统状态。定义整体代价函数为：

式中，e_ij表示状态X_i和X_j之间的约束构成的误差函数，它的大小能够反应状态对约束的符合程度，越符合则该值越小。∑_ij表示该项的协方差矩阵，反映了这一约束的置信程度。因此我们可以定义最优的状态序列的估计值X^*为：

这一算法需要针对不同的约束条件构建不同的误差函数，误差函数的好坏直接关系到计算结果的优劣。在UWB井下打孔坐标测量的应用中，其因子图的构建示例如图2所示。

图中的节点有两种，圆形节点代表UWB基站的三维坐标，标记为Anchor1-4；矩形节点表示两类UWB标签的定位结果，其中上面一排矩形节点表示三脚架上的标签节点，用于UWB基站间的相互定位；下面一排矩形节点表示测量杆上的标签节点，用于约束定位打孔位置的三维坐标。这两排矩形节点的状态值均标记为X(t)～X(t+2)，表示连续三个定位时间点。

边(因子)定义了五种，第一种表示UWB定位标签的位置变化，由于在定位时，测量杆和基站三脚架均是静止放置，所以其误差函数定义为0。如下所示：

e_f1＝[x_t+2-x_t+1]-[x_t+1-x_t]＝0 (4)

第二种因子定义为三脚架上的标签(x_t,tag1～4)到四个基站(B_t,1～4)的测距值误差，其中测量值定义为r_tag1～4，norm函数是利用标签和基站的自身坐标估算出的两者间的距离。这里标签和基站的坐标均是上一次优化算法迭代计算出的临时解。误差函数定义为：

e_f2＝r_tag1～4-norm(x_t,tag1～4,B_t,1～4) (5)

第三种因子定义为测量杆上的标签(x_t,tagA～C)到四个基站的测距误差，其中测量值定义为r_tagA～C，其误差函数定义为：

e_f3＝r_tagA～C-norm(x_t,tagA～C,B_t,1～4) (6)

第四种因子是一种已知条件约束，即已知同一三脚架上的基站(B_t)和标签(x_tb)的距离为20厘米，其误差函数定义为：

e_f4＝norm(x_tb,B_t)_tag1～4＝0.2 (7)

第五种因子为另一种已知条件约束，即已知测量杆上的标签(x_tc)与其余标签(x_tc′)的间距均为20厘米，其误差函数定义为：

e_f5＝norm(x_tc,x_tc′)_tagA～C＝0.2 (8)

2)改进的UWB测距鲁棒核设计

在因子图的边的生成过程中，由于环境噪声影响，有可能产生误差很大的UWB测距值。比如井下工作人员对信号的遮挡及多径效应等。优化算法很可能把一条误差很大的边加进图中去了。然后优化算法试图调整这条边所连接的节点的估计值，使它们顺应这条边的错误要求。由于这个边的误差相对很大，往往会降低其他正确边的影响，使优化算法专注于调整一个错误的值。图优化中可以引入核函数，是因为其能保证每条边的误差不会过大，从而导致其他边约束效果的降低。我们在实验中采用的是Tukey核，其定义如下：

其中δ表示鲁棒核函数的门限值。由图3可见，不同的门限参数对于优化函数的影响不同。门限越小，对异常的容忍性越弱。针对UWB的测距特点，我们提出如下鲁棒核的改进：首先定义UWB的测距值为r_i，第i个基站坐标值为b_i，p为当前计算出的标签的位置。d_i＝‖p-b_i‖₂表示第i个基站与当前定位结果的距离。对于UWB的测距误差边e_i，我们以如下方式加入鲁棒核：

即只对r_i>d_i的情况加入鲁棒核。这是因为UWB测距值在收到遮挡或多径影响后，一般会降低信号的传播速度，从而使测距值偏大。尤其是遮挡严重的情况下，测距值会变的异常大。此时，即需要鲁棒函数的处理，降低异常值对定位结果的影响。而对于r_i<d_i的情况，很有可能是因为当前的定位结果本身有很大的偏差，这种偏差不是由测距异常引起的，所以不该再使用鲁棒核处理。我们将误差函数定义为(r_i-d_i)²。

由于对UWB测距误差加入了自定义的鲁棒核，需要推导出其相应的求导函数，以方便优化算法去调用。e_i的导数定义如下：

3.实验与具体操作

为了对比本发明的效果，基于单标签、简单UWB基站的定位结果标记为UWB，基于改进的Turkey鲁棒核的定位算法记为New_Robust_UWB，该算法使用三标签UWB及带测距功能基站。

实验中，某一次测量的四个基站的UWB数据如图4所示。横坐标代表时间，单位为秒；纵坐标单位为米，代表测距距离。小于0.0米的测距值表示当前轮次该UWB基站测距值缺失。可以很明显的发现大量的NLOS噪声，即有大量缺失值和异常值。实验中，这些测距异常点是由于人为的故意遮挡引起，这将对定位结果造成较大影响。

利用以上UWB数据进行定位，定位结果见图5-a显示，单一UWB标签的定位结果很明显的受到了NLOS噪声的影响，出现了较大偏差的异常点，且定位结果相对发散，坐标方差值较大。图5-b给出的是基于Turkey鲁棒核的UWB定位结果。实验中，鲁棒核阈值δ设置为1.2，没有出现5-a中的跳跃点，这是因为引入了Turkey鲁棒核后，降低了测距异常边对定位结果的影响，整体定位结果相较5-a有了很大幅度的提升。

另一方面，5-b的整体定位结果更为紧密，即X/Y/Z的三轴定位结果方差更小。原因有两点，一是测量杆上的已知相对位置的标签对因子图的求解起到了很大的约束作用，因为其三个标签的X/Y坐标约束在同一位置，而Z轴有固定的坐标偏移量。其次，改进的鲁棒核函数对连续的测距异常值引起的状态异常起到了纠正作用。因为在连续的误差很大的UWB观测值下，会导致系统状态发生很大误差，这会导致出现UWB的观测值反而小于估计值的情况。对于这种由于定位误差引起的问题，算法通过定义距离误差函数有针对性的去修正这种误差，而不是盲目加入鲁棒核去消弱这种误差。

表1给出了使用两种不同算法和设备，对三个不同的测量点A\B\C，各实验10次的统计结果。每个点的测量时间约3分钟，对测量结果求平均值，与事先测定好的坐标求差作为误差计算标准。对于UWB算法，由于无法过滤定位异常值，其定位精度平均值在A\B\C约为0.07、0.07、0.08米。基于Turkey鲁棒核的改进设备的定位误差的均值约为0.01、0.01、0.02米。总体来说，相较于普通UWB定位算法，New_Robust_UWB平均定位精度提高70％～90％。

表1.定位误差分析

4.小结

发明了一种井下锚杆钻孔定位中具有较高定位精度和稳定性的UWB定位方法和设备。首先对本文提出的New_Robust_UWB算法的理论基础进行了说明。通过实验表明，本文提出New_Robust_UWB算法相较于普通UWB定位算法具有较高的定位精度和鲁棒性。该方法能够很好地抑制非高斯噪声带来的定位精度的下降，具有一定的普适性。未来拟对更复杂工业场景的要求，探索基于深度学习的定位算法，通过训练学习定位问题中隐含的模型，用于提升定位结果精度和鲁棒性。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种基于因子图的鲁棒UWB井下锚杆钻孔定位方法，其特征在于，包括如下步骤：

步骤1：在待钻孔位置下方设置垂直的测量杆，测量杆上纵向设置若干个第一类定位标签，最上方第一类定位标签与待钻孔位置之间的距离已知，相邻第一类定位标签之间的距离已知；测量杆的周围设置至少三个高度相同的基站，每个基站的相邻位置设置一个第二类定位标签，基站与其相邻第二类定位标签的距离已知；设定其中一个基站的坐标为局部坐标(0,0,h)，h为基站的高度；

步骤2：测量各个第一类定位标签和第二类定位标签与各个基站的距离；

步骤3：估算出其余基站、第一类定位标签和第二类定位标签的初始坐标，然后利用改进的因子图鲁棒优化算法计算出各个基站、第一类定位标签和第二类定位标签相对于该已知坐标基站的精确相对位置；

步骤4：根据第一类定位标签的坐标，计算出待钻孔位置的坐标。

2.根据权利要求1所述的一种基于因子图的鲁棒UWB井下锚杆钻孔定位方法，其特征在于，步骤1中所述测量杆为顶部设有接触式传感器的可伸缩的测量杆。

3.根据权利要求1所述的一种基于因子图的鲁棒UWB井下锚杆钻孔定位方法，其特征在于，最上方第一类定位标签与待钻孔位置的距离为60厘米。

4.根据权利要求1所述的一种基于因子图的鲁棒UWB井下锚杆钻孔定位方法，其特征在于，步骤3中所述改进的因子图优化算法的具体步骤包括：

步骤3.1：构建因子图；因子图包括若干个状态节点和表示状态节点间相互约束的若干个因子，若干个因子包括：(1)第一类定位标签的位置变化的误差函数；(2)第二类定位标签与各个基站的测距值的误差函数；(3)第一类定位标签与各个基站的测距值的误差函数；(4)基站与其相邻第二类定位标签之间距离的误差函数；(5)相邻第一类定位标签之间距离的误差函数；

步骤3.2：在每一次获得测距值时计算所有第一类定位标签、第二类定位标签和基站的坐标，并计算第一类定位标签、第二类定位标签和基站之间的估计测距值；

步骤3.3：当实际测距值大于估计测距值的实际测距值时误差函数增加鲁棒核，当实际测距值小于估计测距值时误差函数以其测距残差的平方作为优化目标函数；

步骤3.4：根据因子构建代价函数，通过求解代价函数的最小值得到因子图最优状态节点序列。

5.根据权利要求4所述的一种基于因子图的鲁棒UWB井下锚杆钻孔定位方法，其特征在于，基站有四个，第一定位标签有三个，第二定位标签有四个，所述误差函数(1)-(5)分别为：

(1)e_f1＝[x_t+2-x_t+1]-[x_t+1-x_t]＝c；x_t表示t时刻第一类定位标签的三维坐标；c为相邻第一类定位标签之间的距离；

(2)当实际测距值大于估计测距值的实际测距值时误差函数为e_f2＝tukey[r_tag1～4-norm(x_t,tag1～4,B_t,1～4)]；当实际测距值小于估计测距值的实际测距值时误差函数为e_f2＝[r_tag1～4-norm(x_t,tag1～4,B_t,1～4)]²；r_tag1～4表示t时刻第二类定位标签与基站的实际测距值，norm(x_t,tag1～4,B_t,1～4)表示t时刻第二类定位标签与基站的估计测距值，x_t,tag1～4表示t时刻第二类定位标签的三维坐标，B_t,1～4表示t时刻基站的三维坐标，tukey为鲁棒核函数，δ表示鲁棒核函数的门限值；

(3)当实际测距值大于估计测距值的实际测距值时误差函数为e_f3＝tukey[r_tagA～C-norm(x_t,tagA～C,B_t,1～4)]；当实际测距值小于估计测距值的实际测距值时误差函数为e_f3＝[r_tagA～C-norm(x_t,tagA～C,B_t,1～4)]²；r_tagA～C表示t时刻第一类定位标签与基站的实际测距值，norm(x_t,tagA～C,B_t,1～4)表示t时刻第一类定位标签与基站的估计测距值，x_t,tagA～C表示t时刻第一类定位标签的三维坐标；

(4)e_f4＝norm(x_tb,B_t)_tag1～4＝a；a表示基站与其相邻第二类定位标签的距离；

(5)e_f5＝norm(x_tc,x_tc′)_tagA～C＝b；b表示相邻第一类定位标签的距离。

Images