基于多传感器组合的钻锚机器人机身定位方法研究
时间：2023-03-08 来源：《煤炭科学技术》 分享：基于多传感器组合的钻锚机器人机身定位方法研究
0 引 言
巷道支护是煤炭安全开采关键环节，随着对煤炭需求量的增加，开采和支护效率也必须相对应的提高[1]。钻锚机器人是专门用于煤矿井下和其他井巷工程中巷道安装锚杆的支护类设备，可以代替工人完成物料运输、钻孔、锚杆装配等作业，有效地解决工人劳动强度大等问题。但目前大多数钻锚作业都是通过人工驾驶钻锚机器人移动以及工作机构位姿调整完成钻锚机器人定位，由此可见其定位精度和效率无法保证[2-3]。因此，研究性能好、可靠性高、适应性强的钻锚机器人机身精确定位技术，提升自动定位水平，构建自主定位系统对提高煤矿钻锚机器人自动化具有重要意义和应用价值[4-7]。
钻锚机器人机身定位问题实际上是通过机器人所携带的传感器及时反馈机器人所处的环境信息以及机器人本身的位姿信息，以确定自身在煤矿巷道的坐标和方位角[8-11]。国外对于多传感器组合定位技术的研究起步相对较早，在煤矿机器人井下移动自身定位研究方面做了大量工作[12-13]。美国桑迪亚实验室新一代搜救机器人通过所携带的光电编码器将记录机器人行走的位移信息，从而判断出机器人在井下的位置[14-15]；HILARE机器人通过视觉传感器和雷达扫描按照加权平均进行信息融合并绘制环境地图，然后提取当前图像特征信息从而得出机器人位置[16]。中国科学院沈阳自动化研究所研制的CLIMBER移动机器人以及清华大学研制的智能移动机器人THMR-V采用红外、超声等测距传感器与惯性传感器进行信息融合，保证了移动机器人道路环境中的可靠定位要求[17-20]。
上述方法虽然能够获得机器人位置信息，但利用编码器定位时存在无法识别机器人打滑等问题；视觉传感器在光线暗时无法实现测量；利用惯性元件定位随时间增长会存在飘移现象；红外、超声等测距传感器随着测量距离增加，精度会有所降低，从而无法实现定位目标。因此，针对钻锚机器人机身定位困难等问题，笔者研究了基于激光测距传感器和激光雷达组合的钻锚机器人机身定位方法，建立适用于井下复杂环境的钻锚机器人机身定位系统，提高测量范围以及定位精度，消除光线暗时对测量的影响，对实现煤矿巷道钻锚机器人精确定位具有重大应用价值与研究意义。
1 钻锚机器人定位系统
笔者所研究的钻锚机器人本体结构由履带行走机构和龙门框架组成，传统掘进机可在龙门框架内部通过。进行钻锚作业时，钻锚机器人与掘进机会存在空间干涉情况，因此将机器人定位内容分为前移定位和后移定位：①前移定位时，钻锚机器人需在距掘进工作面指定位置停止并完成位于掘进机机身高度以上的所有钻锚作业；②后移定位时，钻锚机器人完全后退至掘进机机身长度以外停止并进行补打剩余锚杆，确保钻锚机器人能够精确停止在钻锚平面是巷道支护作业顺利高效完成的前提。
针对上述钻锚作业特点，分别建立基于激光测距传感器的前移定位模型和基于激光雷达的后移定位模型。前移定位是以掘进工作面为参考，利用激光测距传感器检测钻锚机器人与掘进工作面之间的距离(即机器人-工作面)，建立数学模型，实现前移定位的过程；后移定位是以后端首排顶部锚杆为参考，利用激光雷达扫描获得的点图信息(即机器人-锚杆)，建立数学模型，实现后移定位的过程。钻锚机器人定位系统结构如图1所示。
UART—通用异收发传输器
图1 钻锚机器人定位系统结构
Fig.1 Positioning system structure of drilling anchor robot
由图1可知，钻锚机器人定位系统主要由机载计算机、运动控制卡、前移定位模块和后移定位模块组成。
1)机载计算机。用于显示钻锚机器人在巷道中具体位姿信息，处理各传感器所采集的数据和算法分析，可远程干预钻锚机器人启停，将控制指令下发至运动控制卡并执行。
2)运动控制卡。用于接收各传感器所采集数据并进行A/D转换发送至机载计算机，同时向履带机构下发控制指令以驱动左右履带实现不同动作。
3)前移定位模块。由分布式激光测距传感器组成，其位于钻锚机器人顶梁结构处且呈左右对称分布。钻锚机器人向前移动时，利用分布式激光测距传感器测量钻锚机器人与掘进工作面的垂直距离，并根据左右传感器所采集的距离差建立“机器人-工作面”前移定位模型，求解机器人前移位姿信息。
4)后移定位模块。由激光雷达完成偏航夹角检测，其位于钻锚机器人后端顶部中线处。在钻锚机器人向后移动时，利用激光雷达所扫描到的点图信息，建立“机器人-锚杆”后移定位模型，求解机器人后移位姿信息。
最终将“机器人-工作面”模型解算的定位信息和“机器人-锚杆”解算的定位信息显示至机载计算机，实现人机交互等目标。
2 定位模型建立及分析
为了精确获取钻锚机器人在煤矿巷道中的位置信息，必须对传感器模块所采集的数据进行分析处理。根据各传感器的布置以及工作方式，从以下2个方面对机器人定位系统进行建模分析：“机器人-工作面”前移定位模型和“机器人-锚杆”后移定位模型，并通过所得出的机器人位姿解算公式求解其在巷道中的位置。
2.1 “机器人-工作面”前移定位模型分析
钻锚机器人在向前移动时，采用分布式激光测距传感器检测钻锚机器人前端与掘进工作面的距离信息，建立基于激光测距传感器的定位模型并进行分析，实现钻锚机器人前端绝对定位。激光测距传感器安装在钻锚机器人前端顶梁结构位置，传感器以机器人中心轴线为基准呈左右对称分布，以实现前后距离定位和偏航夹角计算，如图2所示。
图2 激光测距传感器布放示意
Fig.2 Layout of laser range finder
由于掘进工作面存在不平整、欠挖超挖等情况，可通过添加激光测距传感器计算距离平均值等方式来降低定位误差。笔者以左右2个测距传感器为例建立钻锚机器人前移定位模型。
钻锚机器人在前移时的位姿如图3所示，其中D为钻锚工作面与掘进工作面之间的距离，钻锚机器人需要在钻锚工作面上完成掘进机机身以上的钻锚作业。图3a表示机器人向前移动时未发生偏航，图3b表示存在偏航夹角，钻锚工作面没有平行于待钻孔巷道截面，因此需要通过建模分析偏航夹角与所测距离的几何关系，计算得出偏航夹角的大小并确定机器人所在位置，为位姿调整提供重要参数。
图3 前移时钻锚机器人位姿示意
Fig.3 Schematic of forward displacement posture of anchor drilling robot
假设钻锚机器人存在左偏航情况下的坐标关系如图4所示。以煤矿巷道左前端为坐标原点，沿巷道截面向右和煤壁向后分别为x轴和y轴正方向，建立二维平面坐标系。其中A、B表示左右顶梁两侧的激光测距传感器，其可通过云台设计使激光不受偏航夹角影响，始终正对掘进工作面；D1和D2分别为2个传感器所采集到的其与掘进工作面的距离大小；S为2个传感器之间的距离；直线L为S的垂直平分线；γ和β分别为机器人与y轴的偏航夹角和与x轴的偏航夹角，二者大小相等，因此，求解β大小即可得知钻锚机器人偏航夹角大小。
在△ABC中
sin β=AC/AB
(1)
由图4可知，AC=D1-D2，AB=S，代入式(1)可得
sin β=(D1-D2)/S
(2)
β=arcsin(D1-D2)/S
(3)
图4 钻锚机器人前移定位模型
Fig.4 Forward positioning model of anchor drilling robot
可根据β大小，自主或手动调节钻锚机器人位姿，同时观察钻锚机器人与两侧煤壁的距离，当β=0且D1=D2=D时，表明机器人位姿正确，即实现了钻锚机器人前移时的精确定位目标。
2.2 “机器人-锚杆”后移定位模型分析
钻锚机器人后移时，采用激光雷达检测钻锚机器人后端与锚杆之间的距离信息，建立基于激光雷达的定位模型并进行分析，实现钻锚机器人后端绝对定位。将激光雷达安装在钻锚机器人后端顶梁结构中间位置，激光雷达可以动态实时成像，还原当时所采集的锚杆图像及距离信息，以便数据提取和模型分析计算。
激光雷达布放如图5所示，为了较好地检测到锚杆位置，采用调整激光雷达安装角度的方式，以便于能够顺利检测到锚杆，最终通过水平投影计算抵消安装角度带来的误差即可，激光雷达测距原理如图6所示。P为钻锚机器人后端与锚杆所在平面之间的水平距离，φ为安装传感器时设置的角度，其大小可根据实际情况设定，结合激光雷达实际检测距离H可得水平投影距离Hsin φ。
图5 激光雷达布放示意
Fig.5 Layout of laser scanner
图6 激光雷达测距原理
Fig.6 Principle of lidar ranging
钻锚机器人在向后移动时的位姿如图7所示。其中，图7a为钻锚机器人向后移动时未发生偏航的位姿示意，图7b为存在偏航的位姿示意，钻机所在平面与钻锚工作面不重合，因此需要建立激光扫描定位模型确定偏航夹角，然后确定出钻锚机器人所在位置，以便于位姿调整。
图7 钻锚机器人后移位姿示意
Fig.7 Schematic of back displacement posture of anchor drilling robot
钻锚机器人后移定位模型如图8所示，在二维坐标系内，J点为激光雷达所在位置，坐标为x,y，箭头指向为钻锚机器人航向，锚杆之间距离为M，激光雷达采集到左锚杆距离水平投影为a，到右锚杆距离水平投影为b，a、b之间夹角为θ，航向与左锚杆之间的扫描夹角为θ1，与右锚杆之间的扫描夹角为θ2，偏航夹角为φ。
图8 钻锚机器人后移定位模型
Fig.8 Back positioning model of anchor drilling robot
以激光雷达所检测到左端锚杆为坐标原点，建立模型分析钻锚机器人向后移动时存在偏航夹角的运动数学模型。因此，以激光雷达所在坐标点为圆心可得几何圆方程为
(4)
由式(4)可得J点坐标为
(5)
在ΔJOM中，根据余弦定理可得
cos θ=(a2+b2-M2)/2ab
(6)
因此
(7)
将式(7)带入式(5)可得
(8)
由于钻锚机器人始终在所建立坐标正半轴，y>0，因此可得J点坐标值为
(9)
根据几何关系可得偏航夹角大小为
(10)
根据φ大小，可自主或手动调节钻锚机器人位姿，同时观察钻锚机器人与两侧煤壁的距离，当φ近似等于0，且a=b时，表明机器人位姿正确，即实现了钻锚机器人后移时的精确定位目标。
3 定位模型试验验证
利用实验室移动机器人平台验证钻锚机器人定位模型的可行性(图9)，定位系统由2个GJJ-5型激光测距传感器、URG-04LS-UG01激光雷达、锚杆、掘进工作面以及履带式移动机器人组成。
图9 定位系统试验平台搭建
Fig.9 Experimental platform construction of positioning system
试验过程中模拟钻锚机器人在巷道中的工作方式，利用标尺测量以及IMU传感器获得移动机器人位姿测量值与理论模型计算值进行对比。
定位试验场地的二维示意如图10所示，设定锚杆排距为800 mm，S为800 mm，M为1 200 mm，P为800 mm，与左端锚杆和右端锚杆的水平投影距离a、b为1 000 mm，将试验分为2个部分：机器人前移定位试验和机器人后移定位试验，2部分试验各进行20次，只列出5组试验数据。
图10 定位试验场地示意
Fig.10 Schematic of positioning experiment site
3.1 机器人前移定位试验
在进行机器人前移定位试验时，将真实测量得出的值与模型计算值进行对比，分别求解前移时的距离误差和偏航夹角误差，以机器人正前方为0°方向，左偏为正，右偏为负，试验结果见表1、如图11所示。
表1 钻锚机器人前移定位试验结果
Table 1 Forward positioning test results of drilling anchor robot
项目编号至掘进工作面距离/mm传感器1传感器2 偏航夹角/(°)模型计算值11 8181 7820.5621 7501 850-2.8631 8461 7542.1541 7651 835-3.0851 8611 7393.01测量值11 8091 7910.7521 7531 847-2.6931 8401 7602.4641 7591 841-2.9151 8661 7343.24
图11 机器人前移模型定位误差
Fig.11 Positioning error of robot forward model
由表1和图11可知，通过“机器人-工作面”前移定位模型得出的计算值与实际测量值进行对比，结果显示距离误差不超过10 mm，偏航夹角误差不超过1°，因此利用该方法可以实现钻锚机器人在向前移动时的位姿测量和精确定位。
3.2 机器人后移定位试验
进行机器人后移定位试验时，以2排10根锚杆为例，将测量值与模型计算值进行对比，分别求解后移时的距离误差和偏航夹角误差，以激光雷达所在点为探测中心，左偏航为正，右偏航为负，可得激光扫描的点图信息如图12所示。
图12 激光扫描锚杆定位效果
Fig.12 Laser scanning positioning results
在图12中，以激光雷达检测到两排锚杆为例，可得各个锚杆相对于激光雷达的坐标值如下：
1(-605,798)2(-288,800)3(-0,801)4(310,799)5(607,800)6(-601,1 602)7(-279,1 589)8(-1,1 602)9(305,1 597)10(629,1 601)
第1、2排锚杆在y方向上的差值较大，因此可根据差值确定出y值的阈值选取范围(801～1 589)，以消除第2排锚杆对测量结果的影响。
选用第1排锚杆中编号1和编号5的坐标信息解算出激光雷达与锚杆的距离信息和机器人航向与锚杆之间的扫描夹角θ1和θ2，最终根据模型公式可推导出机器人相对于锚杆1的坐标值和偏航夹角的大小，试验结果见表2、如图13所示。
图13 机器人后移模型定位误差
Fig.13 Positioning error of robot backward model
表2 钻锚机器人后移定位试验结果
Table 2 Experimental results of backward positioning of drilling anchor robot
项目编号至锚杆距离/mm左锚杆右锚杆机器人坐标(x,y)偏航夹角/(°)计算值11 018976(627,796)3.7529471 045(547,768)-5.4831 034982(640,837)5.9741 057956(637,830)6.135954998(636,769)-3.24测量值11 010968(637,781)4.3429571 039(538,795)-4.1731 046990(654,825)4.8641 065967(646,822)5.6859631 009(628,775)-3.58
由表2和图13可知，通过“机器人-锚杆”后移定位模型得出的计算值与实际测量值进行对比，结果显示距离误差不超过20 mm，并根据激光扫描结果可计算出偏航夹角的大小，误差不超过1.5°。因此利用该方法可以实现钻锚机器人在后移时的位姿测量和精确定位。
4 结 论
1)提出了基于激光测距传感器和激光雷达的组合传感器钻锚机器人机身定位方法：利用分布式激光测距传感器检测钻锚机器人到掘进工作面之间的距离；利用激光雷达扫面钻锚机器人与后端首排顶部锚杆的图像，最终分别得到钻锚机器人在前后移动时位姿信息。
2)根据钻锚机器人工艺特点，搭建了定位系统框架，并分别建立“机器人-工作面”前移定位模型和“机器人-锚杆”后移定位模型，得出了具体的位置解算公式，为实现机器人在不同状态下的精确定位提供了理论依据。
3)利用移动机器人平台模拟了钻锚机器人定位试验，依次对前移定位模型和后移定位模型进行了验证，并得出了机器人在前移时的位姿信息以及后移时的位姿信息。试验结果显示前移定位模型距离误差不超过10 mm，偏航夹角误差不超过1°；后移定位模型距离误差不超过20 mm，偏航夹角误差不超过1.5°。
参考文献(References):
[1] 刘跃东,林 健,杨建威,等.基于掘锚一体化特厚顶煤巷道快速掘进与支护技术[J].煤炭科学技术, 2017,45(10):60-65.
LIU Yuedong,LIN Jian,YANG Jianwei,et al. Rapid excavation and supporting technology of ultra-thick top coal roadway based on excavation and bolting integration [J].Coal Science and Technology,2017,45 (10): 60-65.
[2] 马宏伟,王成龙,尚东森,等.煤矿井下钻锚机器人布网单元设计与仿真[J].煤炭工程,2019,51(6):160-164.
MA Hongwei,WANG Chenglong,SHANG Dongsen,et al.Design and simulation of layout net unit for drill-anchor robot in underground coal mine [J].Coal Engineering,2019,51(6):160-164.
[3] 葛世荣.煤矿机器人现状及发展方向[J].中国煤炭,2019,45(7):18-27.
GE Shirong.Present situation and development direction of coal mine robots [J].China Coal,2019,45(7):18-27.
[4] 李泉新，刘 飞，方 俊.煤矿坑道智能化钻探技术发展框架分析[J].工矿自动化，2020,46(10)：9-13.
LI Quanxin,LIU Fei,FANG Jun.Analysis of development framework of intelligent coal mine underground drilling technology[J].Industry and Mine Automation，2020,46(10)：9-13.
[5] 马宏伟,张 璞,毛清华,等.基于捷联惯导和里程计的井下机器人定位方法研究[J].工矿自动化,2019,45(4):35-42.
MA Hongwei,ZHANG Pu,MAO Qinghua,et al.Research on positioning method of underground robot based on strapdown inertial navigation and odometer [J].Industrial and Mining Automation,2019,45(4):35-42.
[6] 陶云飞,杨健健,李嘉赓,等.基于惯性导航技术的掘进机位姿测量系统研究[J].煤炭技术,2017,36(1):235-237.
TAO Yunfei,YANG Jianjian,LI Jiageng,et al.Research on position and orientation measurement system of heading machine based on inertial navigation technology [J].Coal Technology,2017,36 (1):235-237.
[7] 岳俊梅，彭新光，李庆义，等．基于RSSI测距的煤矿井下定位研究[J].煤炭工程,2018,50(8)：122-125.
[8] 符世琛,李一鸣,杨健健,等.基于超宽带技术的掘进机自主定位定向方法研究[J].煤炭学报,2015,40(11):2603-2610.
FU Shichen,LI Yiming,YANG Jianjian,et al.Reseach on autonomous positioning and orientation method of roadheader based on Ultra Wide-Band technology [J].Journal of China Society,2015,40(11):2603-2610.
[9] 吴昊骏，龚 敏.基于双侧钻臂位姿协同约束的凿岩台车车体定位方法[J].煤炭学报,2019,44(2):647-654.
WU Haojun,GONG Min.Carriage positioning method of drill rig based on mutual constraints of dual booms[J].Journal of China Society,2019,44(2):647-654.
[10] 杨文娟,张旭辉,马宏伟,等.悬臂式掘进机机身及截割头位姿视觉测量系统研究[J].煤炭科学技术,2019,47(6):50-57.
YANG Wenjuan,ZHANG Xuhui,MA Hongwei,et al.Research on position and posture measurement system of body and cutting head for boom-type roadheader based on machine vision [J].Coal Science and Technology,2019,47 (6):50-57.
[11] 张 鹏.基于改进粒子滤波的煤矿机器人定位方法研究[J].煤炭工程,2016,48(3):113-115,118.
ZHANG Peng.Localization method for coal mining robot based on improved particle filter algorithm [J].Coal Engineering,2016,48(3):113-115,118.
[12] 田 原.基于四点式光靶的掘进机自动定位方法研究[J].煤炭科学技术,2018,46(12):35-40.
TIAN Yuan.Research and test on automatic positioning method of roadheader based on four point light target [J].Coal Science and Technology,2018,46(12):35-40.
[13] 尚 磊,王 杰,宋尊师,等.基于单目ORB-SLAM2算法的煤矿搜救机器人定位研究[J].机床与液压,2020,48(11):49-52.
SHANG Lei.Research on the localization of the coal mine search and rescue robot based in the monocular ORB-SLAM2 algorithm [J].Machine Tool and Hudraulics,2020,48(11):49-52.
[14] 马宏伟,姚 阳,赵 昊,等.矿山钻孔救援探测机器人设计[J].工矿自动化,2019,45(9):1-6.
MA Hongwei,YAO Yang,ZHAO Hao,et al.Design of mine drilling rescue and detection robot [J].Industrial and Mining Automation,2019,45(9):1-6.
[15] 蔡李花,方海峰,李允旺,等.矿井救灾机器人自主定位方法研究[J].工矿自动化,2015,41(7):62-67.
CAI Lihua,FANG Haifeng,LI Yunwang.Reseach of self-localization method of mine rescue robot [J].In dustrial and Mining Automation,2015,41(7):62-67.
[16] 吴 显.基于多传感器新消息融合的移动机器人定位方法研究[D].北京：北京交通大学.2016.
WU Xian.Research on localization method of mobile robots based on multi-sensor data fusion [D].Beijing:Beijing Jiaotong University2016.
[17] 陈 超,张伟伟,徐 军.一种基于三维视觉的移动机器人定位与建图方法[J].现代电子技术,2020,43(6):34-38,42.
CHEN Chao,ZHANG Weiwei,XU Jun.A method of mobile robot localization and mapping based on 3D vision [J].Modern Electromics Rechnique,2020,43(6):34-38,42.
[18] 程 健.煤矿巷道机器人管线视觉辅助定位与导航方法研究[J].煤炭科学技术,2020,48(7):226-232.
CHENG Jian.Study on pipeline vision-aided positioning and navigation method for coal mine tunnel robot[J].Coal Science and Technology,2020,48(7):226-232.
[19] 张 彬,王永雄,邵 翔.融合异质传感器信息的机器人精准室内定位[J].控制工程,2018,25(7)1335-1343.
ZHANG Bin,WANG yongxiong,SHAO Xiang.Precise robot indoor localization based on informationfusion of heterogeneous sensor [J].Control Engineering of China,2018,25 (7)1335-1343.
[20] 谭玉新,杨 维.一种基于UKF的井下机器人超声网络定位方法[J].煤炭学报,2016,41(9):2396-2404.
TAN Yuxin,YANG Wei.UKF-based ultrasomic network localization for a mine robot [J].Journal of China Coal Society,2016,41(9):2396-2404.
Research on body positioning method of drill-anchor robot based on multi-sensor combination
移动扫码阅读
- 相关推荐
