基于扩展卡尔曼滤波的清仓机器人位姿识别方法

李贵虎; 高贵军; 李军霞; 贾雪峰

doi:10.13272/j.issn.1671-251x.2024020004

基于扩展卡尔曼滤波的清仓机器人位姿识别方法

doi: 10.13272/j.issn.1671-251x.2024020004

李贵虎^{1, 2, 3,},
高贵军^{1, 2, 3, ,},
李军霞^{1, 2, 3},
贾雪峰^{1, 2, 3}

1.
太原理工大学机械与运载工程学院，山西太原　030024
2.
太原理工大学山西省矿山流体控制工程实验室，山西太原　030024
3.
太原理工大学矿山流体控制国家地方联合工程实验室，山西太原　030024

基金项目: 山西省科技合作交流专项项目（202104041101005）；山西省重点研发计划项目（202102100401004）。

详细信息

作者简介:
李贵虎（1998—），男，山西原平人，硕士研究生，主要研究方向为煤矿机器人，E-mail：1304712930@qq.com

通讯作者:
高贵军（1973—），男，河北任县人，教授，博士，主要研究方向为矿山机械及其自动化，E-mail：gaogj161@163.com。

中图分类号: TD67
计量
- 文章访问数: 79
- HTML全文浏览量: 24
- PDF下载量: 13
- 被引次数: 0
出版历程
- 收稿日期: 2024-02-03
- 修回日期: 2024-05-15
- 网络出版日期: 2024-06-13

A pose recognition method for warehouse cleaning robots based on extended Kalman filtering

LI Guihu^{1, 2, 3
,},
GAO Guijun^{1, 2, 3
, ,},
LI Junxia^{1, 2, 3},
JIA Xuefeng^{1, 2, 3}

1.
College of Mechanical and Vehicle Engineering, Taiyuan University of Technology, Taiyuan 030024, China
2.
Shanxi Mine Fluid Control Engineering Laboratory, Taiyuan University of Technology, Taiyuan 030024, China
3.
National-local Joint Engineering Laboratory of Mining Fluid Control, Taiyuan University of Technology, Taiyuan 030024, China

摘要

摘要: 煤矿水仓巷道光照强度不均匀且结构化特征明显，传统基于视觉的机器人位姿识别方法识别不准确，而单一的机器人定位技术如自适应蒙特卡洛（AMCL）方法随着清仓机器人的长时间运行，输出的位姿信息存在较大累计误差，易出现煤泥清理不干净、与两侧巷道发生碰撞的情况。针对上述问题，提出了一种基于扩展卡尔曼滤波的多传感器融合清仓机器人位姿识别方法。首先搭建多传感器融合算法框架，建立里程计、惯性测量装置、激光雷达数据采集模型；其次基于扩展卡尔曼滤波原理，以惯性测量装置角度信息建立观测方程，结合里程计位姿信息，得到第1次融合的清仓机器人位姿矩阵，利用激光雷达的位置信息与之前的位姿矩阵进行迭代，得到第2次融合的清仓机器人位姿矩阵；最后采用互补滤波算法对融合后的清仓机器人位姿矩阵进行处理，输出最终的清仓机器人位姿矩阵。实验结果表明：在直线位姿识别中2次的最大位置误差为0.04 m，最大姿态角误差为0.05 rad；在模拟巷道实验中的最大位置误差为0.1 m，最大姿态角误差为0.085 rad；与AMCL方法相比，基于扩展卡尔曼滤波的清仓机器人位姿识别方法在减少清仓机器人运行过程中的累计误差方面表现出显著的有效性。
- 煤矿水仓清理 /
- 清仓机器人 /
- 机器人位姿识别 /
- 多传感器融合 /
- 扩展卡尔曼滤波 /
- 煤泥自主清理
Abstract: The lighting intensity of coal mine water storage roadways is uneven and the structured features are obvious. Traditional vision based robot pose recognition methods are not accurate. The single robot positioning techniques such as Adaptive Monte Carlo localization (AMCL) method have significant cumulative errors in the output pose information with the long-term operation of the cleaning robot. It is easy to encounter situations where the coal slurry is not cleaned thoroughly and collides with both sides of the roadway. In order to solve the above problem, a multi-sensor fusion clearance robot pose recognition method based on extended Kalman filtering is proposed. Firstly, the method builds a multi-sensor fusion algorithm framework and establishes models for odometer, inertial measurement devices, and LiDAR data acquisition. Secondly, based on the principle of extended Kalman filtering, an observation equation is established using the angle information of the inertial measurement device. Combined with the odometer pose information, the first fusion of the clearance robot pose matrix is obtained. Then, the position information of the lidar is iterated with the previous pose matrix to obtain the second fused clearance robot pose matrix. Finally, the complementary filtering algorithm is used to process the pose matrix of the clearance robot after two fusion and output the final pose matrix of the clearance robot. The experimental results show that the maximum position error in linear pose recognition is 0.04 m, and the maximum attitude angle error is 0.05 rad. The maximum position error in the simulated roadway experiment is 0.1 m, and the maximum attitude angle error is 0.085 rad. Compared with the AMCL method, the pose recognition method of the warehouse cleaning robot based on extended Kalman filtering shows significant effectiveness in reducing the cumulative error during the operation of the warehouse cleaning robot.
- coal mine water storage cleaning /
- warehouse clearaning robot /
- robot pose recognition /
- multi sensor fusion /
- extended Kalman filtering /
- autonomous cleaning of coal slurry

HTML全文

图 1 清仓机器人位姿识别系统结构

Figure 1. Structure of pose recognition system for warehouse clearance robot

下载: 全尺寸图片幻灯片

图 2 清仓机器人上的传感器布置

1−激光雷达；2−算法控制箱； 3−惯性测量装置；4−里程计； 5−控制主板。

Figure 2. Sensor layout of warehouse cleaning robot

下载: 全尺寸图片幻灯片

图 3 清仓机器人里程计运动模型

Figure 3. Odometer motion model of warehouse cleaning

下载: 全尺寸图片幻灯片

图 4 EKF基本原理

Figure 4. Basic principle of extended Kalman filtering(EKF)

下载: 全尺寸图片幻灯片

图 5 多传感器融合算法原理

Figure 5. Multi-sensor fusion algorithm principle

下载: 全尺寸图片幻灯片

图 6 实验平台

1−激光雷达； 2−惯性测量装置；3−供电系统；4−核心控制系统； 5−里程计。

Figure 6. Experimental platform

下载: 全尺寸图片幻灯片

图 7 实验环境平面图

Figure 7. Experimental environment plan

下载: 全尺寸图片幻灯片

图 8 全局一致性地图

Figure 8. Globally consistent map

下载: 全尺寸图片幻灯片

图 9 沿直线方向位姿识别结果

Figure 9. Result of pose recognition along straight line

下载: 全尺寸图片幻灯片

图 10 巷道预设轨迹

Figure 10. Roadway preset trajectory

下载: 全尺寸图片幻灯片

表 1 坐标点实际值与2种方法处理结果

Table 1. Actual values of coordinate points and processing results using two methods m

观测点编号	真实坐标	测量坐标
观测点编号	真实坐标	AMCL	本文方法
1	（0.0, 0.0）	（−0.04, −0.03）	（−0.015, −0.003）
2	（−0.3, − 0.7）	（−0.326, −0.796）	（−0.316, −0.726）
3	（15, −0.4）	（15.105, −0.427）	（15.034, −0.423）
4	（10.3, −2）	（10.435, −1.877）	（10.289, −1.893）
5	（5, −0.85）	（5.105, −0.691）	（5.043, −0.821）

下载: 导出CSV

表 2 姿态角实际值与2种方法处理结果

Table 2. Actual value of attitude angle and processing results using two methods rad

观测点编号	真实姿态角	测量姿态角
观测点编号	真实姿态角	AMCL	本文方法
1	1.45	1.428	1.463
2	−3.14	−3.054	−3.123
3	0.15	0.052	0.093
4	1.57	1.421	1.485
5	3.14	2.975	3.085

下载: 导出CSV

表 3 位置误差和姿态角误差对比分析

Table 3. Comparative analysis of pose error and attitude angle error

编号	位置误差/m		姿态角误差/rad
编号	AMCL	本文方法	AMCL	本文方法
1	0.05	0.02	0.022	0.013
2	0.10	0.03	0.086	0.017
3	0.11	0.04	0.098	0.057
4	0.18	0.10	0.149	0.085
5	0.19	0.05	0.165	0.055

下载: 导出CSV

参考文献(24)

[1]	贾建称,贾茜,桑向阳,等. 我国煤矿地质保障系统建设30年:回顾与展望[J]. 煤田地质与勘探,2023,51(1):86-106. JIA Jiancheng,JIA Qian,SANG Xiangyang,et al. Review and prospect of coal mine geological guarantee system in China during 30 years of construction[J]. Coal Geology & Exploration,2023,51(1):86-106.
[2]	曾一凡,武强,赵苏启,等. 我国煤矿水害事故特征、致因与防治对策[J]. 煤炭科学技术,2023,51(7):1-14. ZENG Yifan,WU Qiang,ZHAO Suqi,et al. Characteristics,causes,and prevention measures of coal mine water hazard accidents in China[J]. Coal Science and Technology,2023,51(7):1-14.
[3]	石军杰,高贵军,游青山,等. 煤矿井下水仓清理机器人系统设计与应用[J]. 煤炭工程,2022,54(11):205-208. SHI Junjie,GAO Guijun,YOU Qingshan,et al. Water bin cleaning robot system for underground coal mine[J]. Coal Engineering,2022,54(11):205-208.
[4]	郭培红,薛蛟生,朱建安,等. 全液压水仓煤泥清挖泵送一体机研制[J]. 煤矿机械,2015,36(1):148-150. GUO Peihong,XUE Jiaosheng,ZHU Jian'an,et al. Development of cleaning-pumping combined machine with full hydraulic control for coal slime in water sump[J]. Coal Mine Machinery,2015,36(1):148-150.
[5]	宋峰,蒲仁利,钟灵敏,等. 一种矿用水仓清淤系统:CN202210801796.5[P]. 2022-09-02. SONG Feng,PU Renli,ZHONG Lingmin,et al. A mine water bin dredging system:CN202210801796.5[P]. 2022-09-02.
[6]	姚贵英,曹梦媛,马婧. 煤矿水仓清理机器人研究与设计[J]. 煤矿机械,2020,41(2):4-6. YAO Guiying,CAO Mengyuan,MA Jing. Research and design of coal mine sump cleaning robot[J]. Coal Mine Machinery,2020,41(2):4-6.
[7]	姚立健,丁为民,张培培,等. 基于改进型广义Hough变换的茄子果实位姿识别方法[J]. 农业工程学报,2009,25(12):128-132. doi: 10.3969/j.issn.1002-6819.2009.12.023 YAO Lijian,DING Weimin,ZHANG Peipei,et al. Recognition method of position and attitude of eggplant fruits based on improved generalized Hough transforms[J]. Transactions of the Chinese Society of Agricultural Engineering,2009,25(12):128-132. doi: 10.3969/j.issn.1002-6819.2009.12.023
[8]	陈东旭,赵铁军,乔赫廷. 基于线结构光的焊缝位姿识别研究[J]. 机械工程与自动化,2021(4):38-40,43. CHEN Dongxu,ZHAO Tiejun,QIAO Heting. Research on weld position-posture recognition based on line structured light[J]. Mechanical Engineering & Automation,2021(4):38-40,43.
[9]	马斌,彭光宇. 基于单目视觉的钻杆位姿识别技术研究[J]. 煤田地质与勘探,2022,50(10):171-178. doi: 10.12363/issn.1001-1986.22.01.0036 MA Bin,PENG Guangyu. Research on drill pipe pose recognition technology based on monocular vision[J]. Coal Geology & Exploration,2022,50(10):171-178. doi: 10.12363/issn.1001-1986.22.01.0036
[10]	KEDONG W. A new algorithm for fine acquisition of GPS carrier frequency[J]. GPS Solutions,2014,18(4):581-592. doi: 10.1007/s10291-013-0356-2
[11]	黄西平,杨飞. 综采工作面巡检机器人自主定位方法[J]. 工矿自动化,2023,49(4):86-91. HUANG Xiping,YANG Fei. Autonomous positioning method for inspection robots in fully mechanized working face[J]. Journal of Mine Automation,2023,49(4):86-91.
[12]	陆一,魏东岩,纪新春,等. 地磁定位方法综述[J]. 导航定位与授时,2022,9(2):118-130. LU Yi,WEI Dongyan,JI Xinchun,et al. Review of geomagnetic positioning method[J]. Navigation Positioning and Timing,2022,9(2):118-130.
[13]	崔苗,喻鑫,李学易,等. 多载波无线携能通信的上下行链路联合资源分配[J]. 通信学报,2019,40(3):206-214. doi: 10.11959/j.issn.1000-436x.2019052 CUI Miao,YU Xin,LI Xueyi,et al. Joint downlink and uplink resource allocation for multi-carrier SWIPT system[J]. Journal on Communications,2019,40(3):206-214. doi: 10.11959/j.issn.1000-436x.2019052
[14]	SMITH R,SELF M,CHEESEMAN P. Estimating uncertain spatial relationships in robotics[J]. Machine Intelligence & Pattern Recognition,1988,5(5):435-461.
[15]	FOX D,BURGARD W,DELLAERT F,et al. Monte carlo localization:efficient position estimation for mobile robots[C]. Sixteenth National Conference on Artificial Intelligence,Orland,1999.
[16]	王宁,王坚,李丽华. 一种改进的AMCL机器人定位方法[J]. 导航定位学报,2019,7(3):31-37. WANG Ning,WANG Jian,LI Lihua. An improved adaptive monte carlo localization method for robot[J]. Journal of Navigation and Positioning,2019,7(3):31-37.
[17]	冯佳萌,裴东,邹勇,等. 基于机器人激光定位的一种改进AMCL算法[J]. 激光与光电子学进展,2021,58(20):479-487. FENG Jiameng,PEI Dong,ZOU Yong,et al. An improved AMCL algorithm based on robot laser localization[J]. Laser & Optoelectronics Progress,2021,58(20):479-487.
[18]	马先重. 基于多传感器融合的室内移动机器人定位及障碍物检测与测量研究[D]. 武汉:武汉科技大学,2021. MA Xianchong. Research on localization and obstacle detection and measurement of indoor mobile robot based on multi sensor fusion[D]. Wuhan:Wuhan University of Science and Technology,2021.
[19]	金书奎,寇子明,吴娟. 煤矿水泵房巡检机器人路径规划与跟踪算法的研究[J]. 煤炭科学技术,2022,50(5):253-262. JIN Shukui,KOU Ziming,WU Juan. Research on path planning and tracking algorithm of inspection robot in coal mine water[J]. Coal Science and Technology,2022,50(5):253-262.
[20]	宗意凯,苏淑靖,高瑜宏. 基于多源IMU和粒子滤波优化的姿态融合算法[J]. 仪表技术与传感器,2023(8):88-95. doi: 10.3969/j.issn.1002-1841.2023.08.015 ZONG Yikai,SU Shujing,GAO Yuhong. Attitude fusion algorithm based on multi-source IMU and particle filter optimization[J]. Instrument Technique and Sensor,2023(8):88-95. doi: 10.3969/j.issn.1002-1841.2023.08.015
[21]	沈斯杰,田昕,魏国亮,等. 基于2D激光雷达的SLAM算法研究综述[J]. 计算机技术与发展,2022,32(1):13-18,46. doi: 10.3969/j.issn.1673-629X.2022.01.003 SHEN Sijie,TIAN Xin,WEI Guoliang,et al. Review of SLAM algorithm based on 2D lidar[J]. Computer Technology and Development,2022,32(1):13-18,46. doi: 10.3969/j.issn.1673-629X.2022.01.003
[22]	PENG Gang,ZHENG Wei,LU Zezao,et al. An improved AMCL algorithm based on laser scanning match in a complex and unstructured environment[J]. Complexity,2018(5):1-11.
[23]	ARASARATNAM I,HAYKIN S,ELLIOTT R J. Discrete-time nonlinear filtering algorithms using gauss-hermite quadrature[J]. Proceedings of the IEEE,2007,95(5):953-977.
[24]	SU Zhifeng,ZHOU Jiehua,DAI Jiyang,et al. Optimization design and experimental study of gmapping algorithm[C]. Chinese Control and Decision Conference,Hefei,2020. DOI: 10.1109/CCDC49329.2020.9164603.