Path tracking of mining boom road-header using SVD-Unscented Kalman Filtering
-
摘要: 针对井下悬臂式掘进机自主行进纠偏的实际需求,基于机身调度相对于规划路径的位姿偏差模型,设计并简化了机身调度位姿跟踪控制律,通过构建Lyapunov函数证明该控制律作用下位姿偏差的收敛性。以履带打滑率表征机身行进调度的主要执行误差,以位姿检测误差表征系统的主要观测误差,通过试验及验算获得误差的统计规律,并利用SVD-Unscented卡尔曼滤波估计修正控制指令,从而降低行进调度过程中误差带来的影响。仿真结果表明,提出的轨迹跟踪控制策略能高效实现机身在有限的调整周期内向目标轨迹调度,履带驱动轮参考转速数值连续变化稳定,位姿偏差收敛性良好,达到了纠偏的目的; 基于SVD-Unscented卡尔曼滤波修正控制指令有效削弱了机身调度中来自过程误差和观测误差的影响。调度过程简单高效,跟踪效果重复性好,能为井下有限空间内的掘进机自动纠偏提供参考。
-
关键词:
- 悬臂式掘进机 /
- 行进纠偏 /
- 位姿偏差 /
- 轨迹跟踪 /
- SVD-Unscented卡尔曼滤波
Abstract: Facing the demand of autonomous path correction of the road-header working underground, a control law was designed and simplified based on the position and orientation deviation model of the road-header against its aimed walking trajectory.Reasonable Lyapunov function is constructed to prove the convergence of the position and orientation deviation that resulted from the designed control law.Based on a certain real time position and orientation detection strategy for the road-header, the possible errors exist during posture adjustments are summarized and classified into two parts: one is the main execution error of the road-header and it is presented by the slip rate of tracks; the other is the main observation error and it is identified as the position and orientation measurement errors.This paper proposed to take advantages of the statistical characteristics of these errors, which is obtained by a certain number of experiments or deductions instead of rigorous theoretical analysis.The SVD-unscented Kalman Filtering is used to modify the commands that are given originally by the controller so as to deduce and reduce the influence of process error and observation error in the process of position and orientation adjusting.The simulation results show that the proposed path tracking control strategy can effectively realize the transition of the road-header to the target roadway in limited adjustment period, and the corresponding recommended speeds of the driving wheels vary continuously and stably in the whole process.The negative influence of process error and observation error is effectively weakened by commands modification based on SVD-unscented Kalman Filtering.The obtained moving trajectory of the road-header coincides with the planned path very well during the adjustment, which is of great repeatability.This proves that the proposed strategy is of great potential to be applied experimentally in underground real road heading scenario. -
表 1 仿真参数
Table 1. Data used in simulation
参数名称 取值 履带驱动轮半径r/m 0.5 履带中心距b/m 1 履带宽度B/mm 320 履带接触地面长度l/m 3 机身最大行走速度vmax/(m·s-1) 0.8 机身直线行走平均速度vav/(m·s-1) 0.6 系数kx 0.25 系数ky 0.8 系数kθ 0.1 履带驱动轮最大转速/(rad/s) 4 角加速度允许范围/(rad/s2) [0,0.2] 机身初始位姿 $\left(1, 0, \frac{\pi}{6}\right)$ -
[1] 魏景生, 吴淼. 中国现代煤矿掘进机[M]. 北京: 煤炭工业出版社, 2015. [2] 葛世荣. 智能化采煤装备的关键技术[J]. 煤炭科学技术, 2014, 42(9): 7-11. https://www.cnki.com.cn/Article/CJFDTOTAL-MTKJ201409002.htmGe Shirong. Key technology of intelligent coal mining equipment[J]. Coal Science and Technology, 2014, 42(9): 7-11. https://www.cnki.com.cn/Article/CJFDTOTAL-MTKJ201409002.htm [3] 王国法, 王虹, 任怀伟, 等. 智慧煤矿2025情景目标和发展路径[J]. 煤炭学报, 2018, 43(2): 295-305. https://www.cnki.com.cn/Article/CJFDTOTAL-MTXB201802001.htmWang Guofa, Wang Hong, Ren Huaiwei, et al. 2025 scenarios and development path of intelligent coal mine[J]. Journal of China Coal Society, 2018, 43(2): 295-305. https://www.cnki.com.cn/Article/CJFDTOTAL-MTXB201802001.htm [4] 杨健健, 张强, 王超, 等. 煤矿掘进机的机器人化研究现状与发展[J]. 煤炭学报, 2020, 45(8): 2995-3005. https://www.cnki.com.cn/Article/CJFDTOTAL-MTXB202008032.htmYang Jianjian, Zhang Qiang, Wang Chao, et al. Status and development of robotization research on roadheader for coal mines[J]. Journal of China Coal Society, 2020, 45(8): 2995-3005. https://www.cnki.com.cn/Article/CJFDTOTAL-MTXB202008032.htm [5] 李岩, 杨向东, 陈恳. 履带式移动机器人动力学模型及其反馈控制[J]. 清华大学学报: 自然科学版, 2006, 46(8): 1377-1380. doi: 10.3321/j.issn:1000-0054.2006.08.010Li Yan, Yang Xiangdong, Chen Ken. Dynamics model and feedback control of tracked robots[J]. Journal of Tsinghua University: Science and Technology, 2006, 46(8): 1377-1380. doi: 10.3321/j.issn:1000-0054.2006.08.010 [6] 李岩, 杨向东, 陈恳. 基于滑动操作模型的履带机器人驱动力设计[J]. 机械设计与制造, 2006(7): 119-121. doi: 10.3969/j.issn.1001-3997.2006.07.055Li Yan, Yang Xiangdong, Chen Ken. Tractive force design of tracked robot based on skip-steering model[J]. Machinery Design & Manufacture, 2006(7): 119-121. doi: 10.3969/j.issn.1001-3997.2006.07.055 [7] Le A T, Rye D C, Durrant-Whyte H F. Estimation of track-soil interactions for autonomous tracked vehicles[C]// Proceedings of International Conference on Robotics and Automation. April 25-25, 1997, Albuquerque, NM, USA. IEEE, 1997: 1388-1393. [8] Wang X, Taghia J, Katupitiya J. Robust model predictive control for path tracking of a tracked vehicle with a steerable trailer in the presence of slip[J]. IFAC-PapersOnLine, 2016, 49(16): 469-474. doi: 10.1016/j.ifacol.2016.10.085 [9] Lenain R, Thuilot B, Cariou C, et al. High accuracy path tracking for vehicles in presence of sliding: application to farm vehicle automatic guidance for agricultural tasks[J]. Autonomous Robots, 2006, 21(1): 79-97. doi: 10.1007/s10514-006-7806-4 [10] Burke M. Path-following control of a velocity constrained tracked vehicle incorporating adaptive slip estimation[C]//. 2012 IEEE International Conference on Robotics and Automation. Saint Paul, MN, USA. IEEE, 2012: 97-102. [11] 符世琛, 李一鸣, 杨健健, 等. 基于超宽带技术的掘进机自主定位定向方法研究[J]. 煤炭学报, 2015, 40(11): 2603-2610. https://www.cnki.com.cn/Article/CJFDTOTAL-MTXB201511016.htmFu Shichen, Li Yiming, Yang Jianjian, et al. Research on autonomous positioning and orientation method of roadheader based on Ultra Wide-Band technology[J]. Journal of China Coal Society, 2015, 40(11): 2603-2610. https://www.cnki.com.cn/Article/CJFDTOTAL-MTXB201511016.htm [12] 王福忠, 张利敏. 无人值守掘进机行走双模态模糊控制策略[J]. 工矿自动化, 2013, 39(11): 28-31. doi: 10.7526/j.issn.1671-251X.2013.11.008Wang Fuzhong, Zhang Limin. Dual-modality fuzzy control strategy of walking of unattended roadheader[J]. Industry and Mine Automation, 2013, 39(11): 28-31. doi: 10.7526/j.issn.1671-251X.2013.11.008 [13] 张敏骏, 蔡岫航, 吕馥言, 等. 受限巷道空间区域栅格化掘进机自主纠偏研究[J]. 仪器仪表学报, 2018, 39(3): 62-70. https://www.cnki.com.cn/Article/CJFDTOTAL-YQXB201803008.htmZhang Minjun, Cai Xiuhang, Lu Fuyan, et al. Research on roadheader auto rectification in limited roadway space based on regional grid[J]. Chinese Journal of Scientific Instrument, 2018, 39(3): 62-70. https://www.cnki.com.cn/Article/CJFDTOTAL-YQXB201803008.htm [14] 张旭辉, 赵建勋, 杨文娟, 等. 悬臂式掘进机视觉导航与定向掘进控制技术[J]. 煤炭学报, 2021, 46(7): 2186-2196. https://www.cnki.com.cn/Article/CJFDTOTAL-MTXB202107014.htmZhang Xuhui, Zhao Jianxun, Yang Wenjuan, et al. Vision-based navigation and directional heading control technologies of boom-type roadheader[J]. Journal of China Coal Society, 2021, 46(7): 2186-2196. https://www.cnki.com.cn/Article/CJFDTOTAL-MTXB202107014.htm [15] 薛光辉, 张云飞, 候称心, 等. 基于激光靶向跟踪的掘进机位姿测量方法[J]. 矿业科学学报, 2020, 5(4): 416-422. http://kykxxb.cumtb.edu.cn/article/id/306Xue Guanghui, Zhang Yunfei, Hou Chenxin, et al. Measurement of roadheader position and posture based on laser target tracking[J]. Journal of Mining Science and Technology, 2020, 5(4): 416-422. http://kykxxb.cumtb.edu.cn/article/id/306 [16] 符世琛, 李一鸣, 宗凯, 等. 面向掘进机的超宽带位姿检测系统精度分析[J]. 仪器仪表学报, 2017, 38(8): 1978-1987. doi: 10.3969/j.issn.0254-3087.2017.08.016Fu Shichen, Li Yiming, Zong Kai, et al. Accuracy analysis of UWB pose detection system for roadheader[J]. Chinese Journal of Scientific Instrument, 2017, 38(8): 1978-1987. doi: 10.3969/j.issn.0254-3087.2017.08.016 [17] 韩庆珏, 刘少军. 深海履带车的路径跟踪控制算法[J]. 中南大学学报: 自然科学版, 2015, 46(2): 472-478. https://www.cnki.com.cn/Article/CJFDTOTAL-ZNGD201502014.htmHan Qingjue, Liu Shaojun. Path tracking control algorithm of the deep sea tracked vehicle[J]. Journal of Central South University: Science and Technology, 2015, 46(2): 472-478. https://www.cnki.com.cn/Article/CJFDTOTAL-ZNGD201502014.htm [18] 吴卫国, 陈辉堂, 王月娟. 移动机器人的全局轨迹跟踪控制[J]. 自动化学报, 2001, 27(3): 326-331. https://www.cnki.com.cn/Article/CJFDTOTAL-MOTO200103007.htmWu Weiguo, Chen Huitang, Wang Yuejuan. Global trajectory tracking control of mobile robots[J]. Acta Automatica Sinica, 2001, 27(3): 326-331. https://www.cnki.com.cn/Article/CJFDTOTAL-MOTO200103007.htm [19] 瞿圆媛, 宋林珂, 吉晓冬, 等. 井下掘进机行进纠偏调度规划与控制研究[J]. 矿业科学学报, 2020, 5(2): 194-202. http://kykxxb.cumtb.edu.cn/article/id/280Qu Yuanyuan, Song Linke, Ji Xiaodong, et al. Study on path planning and tracking of the underground mining road-header[J]. Journal of Mining Science and Technology, 2020, 5(2): 194-202. http://kykxxb.cumtb.edu.cn/article/id/280 [20] 韩庆珏. 深海履带式集矿机打滑及路径跟踪控制问题研究[D]. 长沙: 中南大学, 2014. [21] Zhou B. Nonlinear estimation methods for autonomous tracked vehicle with slip[J]. Chinese Journal of Mechanical Engineering: English Edition, 2007, 20(4): 1-7. doi: 10.3901/CJME.2007.04.001