Roadheader combined positioning method based on strapdown inertial navigation and differential odometer
-
摘要: 掘进机自主精确定位是实现掘进工作面智能化乃至无人化的关键,捷联惯导与里程计组合是一种较理想的掘进机定位方案。针对捷联惯导定位误差随时间累计、单里程计位姿感知能力有限等问题,在捷联惯导定位的基础上引入差速里程计作为辅助定位,提出了一种基于捷联惯导与差速里程计的掘进机组合定位方法。该方法的实现由基于捷联惯导的位姿感知、基于差速里程计的航位推算、基于卡尔曼滤波的数据融合3个部分组成:通过捷联惯导获得导航坐标系下掘进机参考位移与姿态角;由2个分别安装于掘进机左右履带的里程计组成差速里程计,通过差速里程计对掘进机进行航位推算;根据捷联惯导与差速里程计的误差方程设计卡尔曼滤波器,并以捷联惯导与差速里程计得到的掘进机位姿数据之差作为卡尔曼滤波器输入,以卡尔曼滤波器输出值对捷联惯导数据进行校正与补偿。将里程计数据有效性判断融入组合定位方法中,避免了履带打滑对定位精度的影响。在模拟巷道进行了掘进机定位实验,结果表明:该组合定位方法测得的掘进机航向角误差能够控制在0.6°以内,位置误差能够控制在0.19 m以内,具有较高的定位精度。Abstract: Autonomous and accurate positioning of roadheader is the key to realize intelligent and unmanned heading face. The combination of strapdown inertial navigation and odometer is an ideal positioning scheme for roadheader. The positioning error of strapdown inertial navigation accumulates with time and the position and posture perception capability of single odometer is limited. In order to solve the above problems, a roadheader combined positioning method based on strapdown inertial navigation and differential odometer is proposed by introducing differential odometer as auxiliary positioning on the basis of strapdown inertial navigation. The realization of the method consists of three parts: position and posture perception based on strapdown inertial navigation, dead reckoning based on differential odometer, and data fusion based on Kalman filtering. The reference displacement and attitude angle of roadheader in navigation coordinate system are obtained by strapdown inertial navigation. The differential odometer is composed of two odometers installed on the left and right tracks of the roadheader, and the dead reckoning of the roadheader is calculated by the differential odometer. Kalman filter is designed according to the error equation of strapdown inertial navigation and differential odometer. The difference between the position and posture data of roadheader obtained by strapdown inertial navigation and differential odometer is used as the input of Kalman filter. The output value of Kalman filter is used to correct and compensate the strapdown inertial navigation data. The validity judgment of odometer data is integrated into the combined positioning method, so that the influence of track slipping on the positioning precision is avoided. The positioning experiment of roadheader is carried out in the simulated roadway. The results show that the heading angle error measured by the combined positioning method can be controlled within 0.6° and the position error can be controlled within 0.19 m. The method has high positioning precision.
-
表 1 捷联惯导参数
Table 1. Strapdown inertial navigation parameters
参数 CH110捷联惯导 FSON II捷联惯导 零偏稳定性/(°·h−1) $ \leqslant 3.5 $ $ \leqslant 0.005 $ 自寻北误差/(°) $ \leqslant 0.1 $ $ \leqslant 0.01 $ 水平对准精度/(°) $ 0.1 $ $ 0.01 $ 航向精度/(°) $ \leqslant 0.4 $ $ \leqslant 0.02 $ 表 2 里程计参数
Table 2. Odometer parameters
参数 左里程计 右里程计 允许最高转速/(r·min−1) 6 000 6 000 分辨率/(P·R−1) 1 000 1 000 刻度系数/(m·P−1) $ 2.904 \times {10^{ - 4}} $ $ 2.904 \times {10^{ - 4}} $ 俯仰误差角/(°) 0.46 0.50 航向误差角/(°) 0.38 0.72 表 3 掘进机初始位姿数据
Table 3. Initial position and posture data of roadheader
参数 值 航向角/(°) −74.090 9 横滚角/(°) 0.348 1 俯仰角/(°) −0.560 4 经度/(°) 118.602 0 纬度/(°) 37.745 0 高程/m 787.815 0 当地重力加速度/(m·s−2) −9.797 -
[1] 谢和平,吴立新,郑德志. 2025年中国能源消费及煤炭需求预测[J]. 煤炭学报,2019,44(7):1949-1960. doi: 10.13225/j.cnki.jccs.2019.0585XIE Heping,WU Lixin,ZHENG Dezhi. Prediction on the energy consumption and coal demand of China in 2025[J]. Journal of China Coal Society,2019,44(7):1949-1960. doi: 10.13225/j.cnki.jccs.2019.0585 [2] 黄刚. 我国区域煤炭消费动态变化分析−产业结构调整和新兴能源对煤炭消费的双重影响[J]. 中国煤炭,2017,43(5):22-26. doi: 10.3969/j.issn.1006-530X.2017.05.004HUANG Gang. Analysis on regional coal consumption dynamics:dual influences of industrial structure adjustment and new thriving energy on coal consumption[J]. China Coal,2017,43(5):22-26. doi: 10.3969/j.issn.1006-530X.2017.05.004 [3] 张旭辉,刘永伟,毛清华,等. 煤矿悬臂式掘进机智能控制技术研究及进展[J]. 重型机械,2018(2):22-27. doi: 10.3969/j.issn.1001-196X.2018.02.006ZHANG Xuhui,LIU Yongwei,MAO Qinghua,et al. Research and progress on intelligent control technology of boom-type roadheader in coal mine[J]. Heavy Machinery,2018(2):22-27. doi: 10.3969/j.issn.1001-196X.2018.02.006 [4] 田伟琴,田原,贾曲,等. 悬臂式掘进机导航技术研究现状及发展趋势[J]. 煤炭科学技术,2022,50(3):267-274. doi: 10.13199/j.cnki.cst.2020-1334TIAN Weiqin,TIAN Yuan,JIA Qu,et al. Research status and development trend of cantilever roadheader navigation technology[J]. Coal Science and Technology,2022,50(3):267-274. doi: 10.13199/j.cnki.cst.2020-1334 [5] 吴淼,沈阳,吉晓冬,等. 悬臂式掘进机行走轨迹及偏差感知方法[J]. 煤炭学报,2021,46(7):2046-2056. doi: 10.13225/j.cnki.jccs.jj21.0702WU Miao,SHEN Yang,JI Xiaodong,et al. Trajectory and deviation perception method of boom-type roadheader[J]. Journal of China Coal Society,2021,46(7):2046-2056. doi: 10.13225/j.cnki.jccs.jj21.0702 [6] 沈阳,王鹏江,吉晓冬,等. 二维里程辅助的掘进机自主导航方法研究[J]. 仪器仪表学报,2021,42(11):96-105. doi: 10.19650/j.cnki.cjsi.J2107815SHEN Yang,WANG Pengjiang,JI Xiaodong,et al. Research on autonomous navigation method of roadheader aided by two-dimensional mileage[J]. Chinese Journal of Scientific Instrument,2021,42(11):96-105. doi: 10.19650/j.cnki.cjsi.J2107815 [7] 刘豪. 捷联惯导与里程计组合的矿用掘进机自主导航定位系统[D]. 重庆: 重庆大学, 2020.LIU Hao. An autonomous navigation system of mining TBM based on the combination of SINS and OD[D]. Chongqing: Chongqing University, 2020. [8] 郭楠楠,申亮亮,邵会兵,等. 车载捷联惯导双里程计组合导航方法研究[J]. 导航定位与授时,2020,7(6):86-93. doi: 10.19306/j.cnki.2095-8110.2020.06.011GUO Nannan,SHEN Liangliang,SHAO Huibing,et al. Research on integrated navigation algorithm with SINS and double odometers[J]. Navigation Positioning and Timing,2020,7(6):86-93. doi: 10.19306/j.cnki.2095-8110.2020.06.011 [9] 刘鹏飞. 里程计辅助的高精度车载GNSS/INS组合导航系统[J]. 光学精密工程,2020,28(4):979-987.LIU Pengfei. High-precision vehicle GNSS/INS integrated navigation system aided by odometer[J]. Optics and Precision Engineering,2020,28(4):979-987. [10] 秦永元. 惯性导航[M]. 2版. 北京: 科学出版社, 2020: 290-318.QIN Yongyuan. Inertial navigation[M]. 2nd ed. Beijing: Science Press, 2020: 290-318. [11] 史凯,刘马宝. 捷联惯导四元数的四阶龙格库塔姿态算法[J]. 探测与控制学报,2019,41(3):61-65.SHI Kai,LIU Mabao. Strapdown inertial navigation quaternion fourth-order Runge-Kutta attitude algorithm[J]. Journal of Detection & Control,2019,41(3):61-65. [12] 严恭敏,李思锦,郭正东. 一种求解等效旋转矢量高阶误差补偿系数的新方法[J]. 中国惯性技术学报,2020,28(3):347-352,396. doi: 10.13695/j.cnki.12-1222/o3.2020.03.010YAN Gongmin,LI Sijin,GUO Zhengdong. A new method to obtain high-order error compensation coefficients for equivalent rotation vector[J]. Journal of Chinese Inertial Technology,2020,28(3):347-352,396. doi: 10.13695/j.cnki.12-1222/o3.2020.03.010 [13] WANG Tianmiao,WU Yao,LIANG Jianhong,et al. Analysis and experimental kinematics of a skid-steering wheeled robot based on a laser scanner sensor[J]. Sensors,2015,15(5):9681-9702. doi: 10.3390/s150509681 [14] HUANG Yulong,ZHANG Yonggang,WANG Xiaodong. Kalman-filtering-based in-motion coarse alignment for odometer-aided SINS[J]. IEEE Transactions on Instrumentation and Measurement,2017,66(12):3364-3377. doi: 10.1109/TIM.2017.2737840 [15] 秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M]. 2版. 西安: 西北工业大学出版社, 2012.QIN Yongyuan, ZHANG Hongyue, WANG Shuhua. Kalman filtering and integrated navigation principle[M]. 2nd ed. Xi'an: Northwestern Polytechnical University Press, 2012. [16] 周召发,赵芝谦,张志利,等. 考虑里程计截断误差的SINS/OD组合导航算法[J]. 中国惯性技术学报,2022,30(3):336-344.ZHOU Zhaofa,ZHAO Zhiqian,ZHANG Zhili,et al. SINS/OD integrated navigation algorithm considering odometer truncation error[J]. Journal of Chinese Inertial Technology,2022,30(3):336-344. [17] 赵洪松,缪玲娟,沈军. 捷联惯导/里程计高精度组合导航算法[J]. 兵工学报,2014,35(4):433-440. doi: 10.3969/j.issn.1000-1093.2014.04.001ZHAO Hongsong,MIAO Lingjuan,SHEN Jun. High accuracy algorithm for SINS/odometer integrated navigation system[J]. Acta Armamentarii,2014,35(4):433-440. doi: 10.3969/j.issn.1000-1093.2014.04.001 [18] LIU Xuhang,LIU Xiaoxiong,YANG Yue,et al. Robust variational Bayesian method-based SINS/GPS integrated system[J]. Measurement,2022,193:110893. doi: 10.1016/j.measurement.2022.110893 [19] 石静,缪玲娟,倪茂林. 一种抗野值自适应滤波算法及在MEMS−SINS/GPS中应用[J]. 宇航学报,2010,31(12):2711-2716. doi: 10.3873/j.issn.1000-1328.2010.12.014SHI Jing,MIAO Lingjuan,NI Maolin. An outlier rejecting and adaptive filter algorithm applied in MEMS-SINS/GPS[J]. Journal of Astronautics,2010,31(12):2711-2716. doi: 10.3873/j.issn.1000-1328.2010.12.014 [20] 马智渊,石志勇,王志伟. 捷联惯导/里程计组合导航技术[J]. 火力与指挥控制,2017,42(2):183-186.MA Zhiyuan,SHI Zhiyong,WANG Zhiwei. Strapdown inertia/odometer integrated navigation technology[J]. Fire Control & Command Control,2017,42(2):183-186.