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

权利要求书

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

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

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

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

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

步骤3中改进的因子图鲁棒优化算法的具体步骤包括：

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

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

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

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

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

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

4.根据权利要求1所述的一种基于因子图的鲁棒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表示相邻第一类定位标签的距离。