Precise positioning method of tunneling machine for inertial navigation and visual information fusion
-
摘要:
针对煤矿巷道复杂环境下掘进机精确定位难题，提出了一种惯导与视觉融合的掘进机精确定位方法，该方法采用惯导与“视觉+激光标靶”的定位方案。该方案将设计的四特征点大尺寸激光标靶固定于巷道顶板，相机固定于掘进机机身采集激光标靶图像，并运用圆拟合法定位光斑中心和基于四特征点的EPnP算法解算掘进机位置。为了验证“视觉+激光标靶”方法对掘进机位置检测效果，在模拟掘进工作面环境下开展了“视觉+激光标靶”位置检测试验，结果表明：在30 m内沿巷道宽度方向、掘进方向、高度方向最大误差不超过28.549 mm、78.868 mm、44.459 mm，实现了掘进机位置精确检测。针对惯导测量掘进机位姿误差随时间累积和掘进机振动对组合定位系统产生干扰导致位姿检测不准问题，提出改进Sage-Husa自适应滤波的惯导与视觉信息融合方法，该方法通过检测新息方差值修正量测误差来提高定位准确性。在模拟掘进工作面环境下开展了惯导与“视觉+激光标靶”组合定位实验，采用改进前后Sage-Husa自适应滤波算法融合惯导与视觉信息进行对比分析，结果表明：改进后Sage-Husa自适应滤波算法融合得到的定位误差更小，俯仰角、横滚角、航向角最大误差分别为0.029°、0.051°、0.011 3°，在距离激光标靶30 m内巷道宽度位置误差在0.033 m范围内，巷道掘进方向位置误差在0.062 m范围内。所提出的惯导与视觉融合定位方法能够满足巷道掘进定位精度要求。
-
关键词:
- 掘进机定位 /
- 惯导 /
- 机器视觉 /
- EPnP /
- Sage-Husa自适应滤波
Abstract:For the difficulty of pinpointing of tunneling machines in the complex environment of coal mine tunnels,a precise positioning method of roadheader based on inertial navigation and visual fusion is proposed, which uses a positioning scheme of inertial navigation plus "vision+laser target". With the designed four feature point large-sized laser target fixed on the roof of the tunnel and the camera on the tunneling machine to collect laser target images, the scheme uses the circular fitting method to locate the center of the light spot and the EPnP algorithm based on four feature points to calculate the position of the tunneling machine. A “vision+laser target” position detection experiment was conducted in simulated condition of heading face to verify the effectiveness of the method in detecting the position of tunneling machines. The results are as follows: within 30 meters tunnel, the maximum error in the width, position, and height are no more than 28.549, 78.868, and 44.459 mm, which shows accurate detection of the position of the tunneling machine. For the inaccurate pose detection of tunneling machines in inertial navigation measurement, induced by the accumulation of pose errors over time and the machine vibration on the combined positioning system, an improved Sage-Husa adaptive filtering method for inertial navigation and visual information fusion is proposed, which could correct measurement errors by detecting new variance values to improve positioning accuracy. A combined positioning experiment of inertial navigation plus “vision+laser target” was conducted in a simulated heading face environment, using improved Sage-Husa adaptive filtering algorithm to fuse inertial navigation and visual information for comparative analysis, which show that the improved Sage-Husa adaptive filtering algorithm fusion results in smaller positioning errors. The maximum errors of pitch angle, roll angle, and heading angle are 0.029°, 0.051°, and
0.0113 °, respectively. Within 30 meters, the position error of tunnel width is within the range of 0.033 m, and the position error of tunnel excavation direction is within the range of 0.062 m. Overall, the proposed inertial navigation plus visual fusion positioning method can meet the accuracy requirements of tunnel excavation positioning.-
Keywords:
- tunneling machine /
- inertial navigation /
- machine vision /
- EPnP /
- sage-husa filtering
-
-
![]()
图 1 “视觉+激光标靶”系统结构及坐标系示意
Figure 1. Schematic of the “Vision+Laser Target” system structure and coordinate system
![]()
图 4 EPnP算法的位置解算原理
Figure 4. Schematic diagram of EPnP algorithm for position calculation
![]()
图 10 距离标靶10 m时位置测量结果
Figure 10. Position measurement results at a distance of 10 m from the target
![]()
图 11 距离标靶20 m时位置测量结果
Figure 11. Position measurement results at a distance of 20 m from the target
![]()
图 12 距离标靶30 m时位置测量结果
Figure 12. Position measurement results at a distance of 30 m from the target
项目 参数 最高分辨率/（px×px） 1 600×1 200 像素尺寸/(μm×μm) 4.4×4.4 光学尺寸 1/1.8'' 有效感光面积/(mm×mm) 7×5.3 最大帧率/fps 20 输出颜色 黑白 传输距离/m 100 
下载: 导出CSV
表 2 镜头参数
Table 2 Lens Parameters
项目 参数 分辨率/m 5 焦距/mm 8 视场角/DHV 67.1°×56.3°×43.7° 畸变/(°) <1 光圈 F=1:1.4～16 聚焦/m 0.1～0.9 像面尺寸 2/3"
下载: 导出CSV
表 3 定位平均误差
Table 3 Average positioning error
平均误差 宽度(X)
方向/mm掘进(Y)
方向/mm高度(Z)
方向/mm掘进机距激光
标靶的距离/m5 2.706 5.224 3.282 10 5.055 14.258 8.657 15 6.564 25.871 12.213 20 11.848 31.899 13.102 25 12.564 38.431 21.874 30 15.064 44.087 21.346
下载: 导出CSV
表 4 定位最大误差
Table 4 Maximum positioning error
平均误差 宽度(X)
方向/mm掘进(Y)
方向/mm高度(Z)
方向/mm掘进机距激光
标靶的距离/m5 4.925 13.433 7.984 10 6.738 25.896 13.392 15 8.317 49.755 22.563 20 18.103 56.729 29.600 25 26.694 68.563 37.863 30 28.549 78.868 44.459
下载: 导出CSV
表 5 Sage-Husa自适应滤波改进前后定位结果
Table 5 Positioning results before and after Sage-Husa adaptive filtering improvement
Sage-Husa自适应 新息误差跟踪 X方向最大误差/m Y方向最大误差/m X方向RMSE Y方向RMSE 10 m 20 m 30 m 10 m 20 m 30 m 10 m 20 m 30 m 10 m 20 m 30 m √ 0.055 1 0.082 2 0.089 3 0.080 2 0.102 4 0.202 8 0.056 6 0.056 9 0.050 7 0.098 8 0.044 2 0.115 9 √ √ 0.011 2 0.021 5 0.033 2 0.020 5 0.041 3 0.062 3 0.017 6 0.013 0 0.018 2 0.031 1 0.021 1 0.035 8 注：√代表使用该方法。
下载: 导出CSV
表 6 各算法解算最大误差与均方根误差
Table 6 Maximum error and root mean square error of each algorithm
融合算法 X方向最大误差/m Y方向最大误差/m X方向RMSE Y方向RMSE Sage-Husa自适应 0.089 3 0.202 8 0.050 7 0.115 9 自适应UKF 0.051 2 0.101 3 0.031 2 0.062 8 自适应SRCKF 0.034 6 0.073 4 0.021 4 0.042 8 改进Sage-Husa自适应 0.033 2 0.062 3 0.018 2 0.035 8
下载: 导出CSV
-
[1] 王国法,王 虹,任怀伟,等. 智慧煤矿2025情景目标和发展路径[J]. 煤炭学报,2018,43(2):295−305. WANG Guofa,WANG Hong,Ren Huaiwei,et al. 2025 Scenario objectives and development path of smart coal mines[J]. Journal of Coal Industry,2018,43(2):295−305.
[2] 王国法. 煤矿智能化最新技术进展与问题探讨[J]. 煤炭科学技术,2022,50(1):1−27. WANG Guofa. New technological progress of coal mine intelligence and its problems[J]. Coal Science and Technology,2022,50(1):1−27
[3] 杨健健,张 强,王 超,等. 煤矿掘进机的机器人化研究现状与发展[J]. 煤炭学报,2020,45(8):2995−3005. YANG Jianjian,ZHANG Qiang,WANG Chao,et al. Research status and development of robotization of coal mine boring machines[J]. Journal of Coal Industry,2020,45(8):2995−3005.
[4] 马宏伟,王世斌,毛清华,等. 煤矿巷道智能掘进关键共性技术[J]. 煤炭学报,2021,46(1):310−320. MA Hongwei,WANG Shibin,MAO Qinghua,et al. Key common technologies for intelligent excavation of coalmine tunnels[J]. Journal of Coal Industry,2021,46(1):310−320
[5] 朱信平,李 睿,高 娟,等. 基于全站仪的掘进机机身位姿参数测量方法[J]. 煤炭工程,2011(6):113−115. doi: 10.3969/j.issn.1671-0959.2011.06.044 ZHU Xinping,LI Rui,GAO Juan,et al. Measurement method of position and attitude parameters of roadheader body based on total station[J]. Coal Engineering,2011(6):113−115 doi: 10.3969/j.issn.1671-0959.2011.06.044
[6] 张旭辉,沈奇峰,杨文娟,等. 基于三激光点标靶的掘进机机身视觉定位技术研究[J]. 电子测量与仪器报,2022,36(6):178−186. ZHANG Xuhui,SHEN Qifeng,YANG Wenjuan,et al. Research on visual positioning technology of tunneling machine body based on three laser point targets[J]. Journal of Electronic Measurement and Instrumentation,2022,36(6):178−186.
[7] 贾文浩,陶云飞,张敏骏,等. 基于iGPS的煤巷狭长空间中掘进机绝对定位精度研究[J]. 仪器仪表报,2016,37(8):1920−1926. JIA Wenhao,TAO Yunfei,ZHANG Minjun,et al. Research on the absolute positioning accuracy of tunneling machines in narrow and long spaces of coal tunnels based on iGPS[J]. Journal of Instrumentation,2016,37(8):1920−1926.
[8] 陶云飞,杨健健,李嘉赓,等. 基于惯性导航技术的掘进机位姿测量系统研究[J]. 煤炭技术,2017,36(1):235−237. TAO Yunfei,YANG Jianjian,LI Jiageng,et al. Research on the Position and Attitude Measurement System of Tunneling Machine Based on Inertial Navigation Technology[J]. Coal Technology,2017,36(1):235−237.
[9] 陈慎金,成 龙,王鹏江,等. 基于掘进机位姿测量系统的自主标定方法误差分析[J]. 煤炭学报,2018,43(9):285−290. CHEN Shenjin,JACKIE Chan,WANG Pengjiang,et al. Error analysis of autonomous calibration method based on heading machine pose measurement system[J]. Journal of China Coal Society,2018,43(9):285−290.
[10] WANG J,YU B,KANG H,et al. Key technologies and equipment for a fully mechanized top-coal caving operation with a large mining height at ultra-thick coal seams[J]. International Journal of Coal Science & Technology,2015,2(2):97−161.
[11] KHONZI H,FRANCOIS D P. Implementation of an autonomous underground localization system[C]. Robotics and MECHATRONICS Conference. IEEE,2013:1–34.
[12] 刘 超,符世琛,成 龙,等. 基于TSOA定位原理混合算法的掘进机位姿检测方法[J]. 煤炭学报,2019,44(4):1255−1264. LIU Chao,FU Shichen,Jackie CHAN,et al. A hybrid algorithm based on TSOA positioning principle for detecting the pose of tunneling machines[J]. Journal of China Coal Society,2019,44(4):1255−1264.
[13] 刘 宇,陈根林,刘永忠. 煤矿全断面掘进机捷联惯导曲线测量系统[J]. 工矿自动化,2019,45(8):65−69, 73. LIU Yu,CHEN Genlin,LIU Yongzhong. Strap down Inertial Navigation curve measurement system for coal mine full face tunneling machine[J]. Industrial and Mining Automation,2019,45(8):65−69, 73.
[14] 卢新明,闫长青,袁照平. 掘进机精准定位方法与掘进机器人系统[J]. 通信学报,2020,41(2):58−65. doi: 10.11959/j.issn.1000-436x.2020042 LU Xinming,YAN Changqing,YUAN Zhaoping. Precise positioning methods for tunneling machines and tunneling robot systems[J]. Journal of Communications,2020,41(2):58−65. doi: 10.11959/j.issn.1000-436x.2020042
[15] AUGER F,HILAIRET M,GUERRERO J M,et al. Industrial applications of the Kalman filter:A review[J]. IEEE Transactions on Industrial Electronics,2013,60(12):5458−5471. doi: 10.1109/TIE.2012.2236994
[16] LI M,MOURIKIS A I. High-precision,consistent EKF-based visualinertial odometry[J]. The International Journal of Robotics Research,2013,32(6):690−711. doi: 10.1177/0278364913481251
[17] 余 铎,王耀南,毛建旭,等. 基于视觉的移动机器人目标跟踪方法[J]. 仪器仪表学报,2019,40(1):227−235. YU Duo,WANG Yaonan,MAO Jianxu,et al. Visual based object tracking method for mobile robots[J]. Journal of Instrumentation,2019,40(1):227−235.
[18] 孟秀云,王语嫣. 一种SINS/GPS紧组合导航系统的改进自适应扩展卡尔曼滤波算法[J]. 北京理工大学学报,2018,38(6):625–630, 636. MENG Xiuyun,WANG Yuyan. An Improved Adaptive Extended Kalman Filter Algorithm for SINS/GPS Compact Integrated Navigation System [J]. Journal of Beijing University of Technology,2018,38(6):625–60, 636.
[19] NARASIMHAPPA M,MAHINDRAKAR A D,GUIZILINI V C,et al. MEMS based IMU drift minimization:Sage Husa adaptive robust Kalman filtering[J]. IEEE Sensors Journal,2019,20(1):250−260.
[20] 王天宇,董文博,王震宇. 基于单目视觉和固定靶标的位姿测量系统[J]. 红外与激光工程,2017,46(4):153−160. WANG Tianyu,DONG Wenbo,WANG Zhenyu. A pose measurement system based on monocular vision and fixed targets[J]. Infrared and Laser Engineering,2017,46(4):153−160.
[21] 王 平,周雪峰,安爱民,等. 一种鲁棒且线性的PnP问题求解方法[J]. 仪器仪表学报,2020,41(9):271−280. WANG Ping,ZHOU Xuefeng,AN Aimin,et al. A robust and linear method for solving PnP problems[J]. Journal of Instrumentation,2020,41(9):271−280.
[22] ZHENG Y,KUANG Y,SUGIMOTO S,et al. Revisiting the pnp problem:A fast,general and optimal solution[C]//Proceedings of the IEEE International Conference on Computer Vision,2013:2344–2351.
[23] 周道德,高豆豆,董登峰,等. 基于深度学习和PnP模型的激光跟踪仪自动姿态测量[J]. 光学精密程,2022,30(9):1047−1057. doi: 10.37188/OPE.20223009.1047 ZHOU Daode,GAO Doudou,DONG Dengfeng,et al. Laser tracker automatic attitude measurement based on deep learning and PnP model[J]. Optical Precision Engineering,2022,30(9):1047−1057. doi: 10.37188/OPE.20223009.1047
[24] 王中宇,李亚茹,郝仁杰,等. 基于点特征的单目视觉位姿测量算法[J]. 红外与激光工程,2019,48(5):169−176. WANG Zhongyu,LI Yaru,HAO Renjie,et al. A monocular vision pose measure mentalgorithm based on point features[J]. Infrared and Laser Engineering,2019,48(5):169−176.
[25] LEPETIT V,MORENO–NOGUER F,FUA P. EPnP:an accurate O(n) solution to the PnP problem[J]. International Journal of Computer Vision,2008,81(2):155−166.
计量
- 文章访问数: 159
- HTML全文浏览量: 30
- PDF下载量: 58
