[发明专利]基于因子图的鲁棒UWB井下锚杆钻孔定位方法

说明书

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

技术领域

本发明涉及井下锚杆钻孔定位技术领域，具体涉及一种基于因子图的鲁棒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：根据第一类定位标签的坐标，计算出待钻孔位置的坐标。