基于TSOA定位原理混合算法的掘进机位姿检测方法

Pose detection method based on hybrid algorithm of TSOA positioning principle for roadheader

为实现掘进机的位姿检测,提出了一种面向掘进机的混合算法的位姿检测方法。基于超宽带(Ultra-wideband)测距和(Time Summation of Arrival,TSOA)定位原理,系统地推导了混合算法的计算过程,将间接法得到的定位点初始坐标代入Taylor级数展开法,循环迭代,通过混合算法得到了掘进机的坐标值,将坐标值代入姿态角解算公式得到了掘进机的姿态角。实际实验中需要其他测量方式的标定才可验证真值,且任何测量方式均有误差,因此基于MATLAB进行仿真分析研究规律。绘制了混合算法程序流程图,仿真对比了间接法和混合算法在15 m和90 m的定位点空间分布状态、三轴误差,以及2种算法在10～100 m的测量范围内定位点均方根误差和三轴均方根误差变化情况,分析了混合算法在15 m和90 m的姿态角精度,仿真结果表明:混合算法的定位性能优于间接法,精度高于间接法,姿态角误差可控制在0. 008°以下。搭建了掘进机位姿检测系统实验平台,在模拟巷道中进行了实验验证,完整地采集了UWB测距的相关数据,在MATLAB中通过曲线拟合绘制了误差随距离变化的曲线图,得到了误差随测量距离变化的规律,实验结果表明:在3～94 m测量范围内,X轴误差可控制在4 cm以内,Y轴误差可达到毫米级,Z轴误差随测量距离增大而增大。为实现综掘装备的自主导控提供了理论基础。

In order to realize the pose detection of the roadheader,a pose detection method for the hybrid algorithm was proposed.According to the positioning principle of Time Summation of Arrival(TSOA),the calculation process of the hybrid algorithm was systematically derived.The initial coordinates of the positioning point obtained by the indirect method were brought into the Taylor series expansion,loop iteration,the coordinate values of the roadheader were obtained by the hybrid algorithm.Taking the coordinate value into the attitude angle solution formula,the attitude angle of the roadheader was obtained.The actual measurement required the calibration of other measurement methods to verify the true value.Any measurement method had errors,so the simulation analysis was carried out in MATLAB.The flow chart of the hybrid algorithm program was drawn.The simulation compared the spatial distribution and triaxial error of both indirect and hybrid algorithms at 15 m and 90 m.It compared the RMSE error of the positioning point and the three-axis RMSE error of the two algorithms in 10-100 m.The attitude angle accuracy of the hybrid algorithm was analyzed at 15 m and 90 m.The results show that the positioning performance of the hybrid algorithm is better than the indirect method.Hybrid algorithm is more accurate than indirect method.The attitude angle error can be controlled below 0.008°.The experimental platform of the roadheader pose detection system was built.Experiments were carried out in the simulated roadway,and the relevant data of ultra-wideband ranging was collected completely.The curve of error with distance was plotted by curve fitting in MATLAB.The law of error with measurement distance was obtained.The experiment results show that the X-axis error can be controlled within 4 cm and the Y-axis error can reach millimeter,Z-axis error increases as the measured distance increases within 3-94 m.This method provides a basis for the realization of the position detection of roadheader.