移动阅读
吴昊骏，龚 敏
(北京科技大学 土木与资源工程学院，北京 100083)
摘 要:车体定位是凿岩台车钻孔自动控制的先决工序，原有车体定位方法仅在理想无碴工作面可行，当有堆碴车体横向倾斜情况下，推进梁可绕激光束横向旋转造成角变量无法锁定，车体位姿存在无穷解，导致定位误差很大甚至无法使用。为解决这一问题提出利用双钻臂同时参与定位的新方法，从理论上证明了双钻臂法解算车体位姿的可行性，将求车体位姿惟一解的问题转化为求解固定边长三角形顶点位置的问题;基于D-H法及坐标系平移确定了车体-激光坐标系变换关系;引入集合条件找出车体基坐标系实际原点位置，通过MATLAB编程计算车体两侧角变量值，求解车体基坐标系在工作面坐标系下的位姿矩阵，实现车体定位。现场测试结果表明:双钻臂法充分适应有渣条件堆碴高度变化，并能快速求得车体实际位姿确定值;车体竖直方向定位误差在5%以内，不采用误差补偿手段条件下钻孔定位精度在12 cm内。通过对比两种车体定位方法对钻孔定位效果的影响发现:原方法存在定位误差向非定位侧钻臂传递的现象，全站仪测量准确的情况下，非定位侧炮孔平均定位误差较定位侧高出94%。双钻臂法定位则不存在这一问题，其双侧炮孔定位精度与原方法定位侧炮孔定位精度一致，对于误差补偿较为有利。双钻臂定位法已在井下试验得到成功验证，具有普遍性，可用于同类型凿岩台车车体定位。
关键词:凿岩台车;车体定位;双钻臂;位姿矩阵
中图分类号:TD421.2
文献标志码:A
文章编号:0253-9993(2019)02-0647-08
收稿日期:2018-02-24
修回日期:2018-06-10
责任编辑:毕永华
作者简介:吴昊骏(1990—)，男，安徽六安人，博士研究生。E-mail:whjcyyxzc@gmail.com
通讯作者:龚 敏(1963—)，男，重庆巫山人，教授，博士生导师。Tel:010-62332939,E-mail:gongmustb@163.com
移动阅读
吴昊骏，龚敏.基于双侧钻臂位姿协同约束的凿岩台车车体定位方法[J].煤炭学报,2019,44(2):647-654.doi:10.13225/j.cnki.jccs.2018.0238
WU Haojun，GONG Min.Carriage positioning method of drill rig based on mutual constraints of dual booms[J].Journal of China Coal Society,2019,44(2):647-654.doi:10.13225/j.cnki.jccs.2018.0238
WU Haojun，GONG Min
(School of Civil and Resource Engineering,University of Science and Technology Beijing,Beijing 100083,China)
Abstract:Carriage positioning is the prerequisite process for the automatic control of the drilling of the drill rig.The original method is only feasible in the ideal working face has no stone stacked.The carriage of drill rig is tilted laterally when there are stones stacked.The push beam can be rotated laterally around the laser beam.The angular variable cannot be locked,and there is an infinite solution to the pose of the carriage,resulting in a large positioning error or even unusable.In order to solve this problem,a new method of using the booms in dual sides to participate in positioning is proposed.It is theoretically proved the feasibility,and the problem of finding the unique solution of the carriage pose is solved to solve the vertex position of the triangle when the length are fixed;the transformation relationship of the carriage-laser coordinate system is determined based on the D-H method and the coordinate system translation;the set condition is introduced to find the actual origin position of the carriage coordinate system,and the angular values of the dual sides of the drill rig are calculated by MATLAB programming.Solve the pose matrix of the carriage coordinate system in the working face coordinate system to realize the carriage positioning.The field test results show that the new method fully adapts to the height change of the stone condition,and can quickly determine the actual position and pose of the carriage;the vertical positioning error of the carriage is within 5%.Drilling positioning accuracy is within 12 cm when the error compensation method is not used.By comparing the influence of two methods on the drilling positioning effect,it is found that the original method has the phenomenon that the positioning error is transmitted to the non-positioning side boom.When measured data of total station are accurate,the average positioning error of blast-holes in the non-positioning side is 94% higher than that in the positioning side.There is no such problem with the positioning of the new method.The positioning accuracy of the blast-holes in dual sides is consistent with that in the positioning side of the original method,which is advantageous for error compensation.The new method has been successfully verified in the roadway and is universal.It can be utilized to the positioning of the same type of drill rig.
Key words:drill rig;carriage positioning;dual booms;pose matrix
凿岩台车钻孔自动定位是掘进技术发展的重要方向和近年来研究热点[1-4]。钻孔自动定位首先需要解决车体定位(台车与巷道位置关系)问题，车体定位后计算机才能据此确定各钻臂移动参数和实施动作控制[5-6]，因此具有非常重要的作用。
由于产权保护原因国外车体定位文献很少，可供借鉴参考的资料有限。李军利等[7]研究了与台车类似的掘进机车体定位，通过坐标变换矩阵并基于闭环尺寸链进行定位，然而每次掘进前需用全站仪测量的方式较麻烦并影响施工;ELSRUD Rolf[8]介绍了阿特拉斯台车自动定位RCS控制系统，首次提出利用钻臂穿过巷道定向激光的车体定位方法，但文献没有阐述具体解算过程;对此何清华，谢习华等[9-10]详细推导了该方法的计算求解步骤，为我国首台隧道凿岩机器人研发奠定了重要基础，一些学者[11-13]据此进行了相关研究，这也是目前普遍认可的定位方法。但这一方法在实际应用时因堆碴高度变化(现场常见情况)误差很大，采用车体横向角度传感器作补偿，在现场应用时准确性较差。
为解决上述方法无法适应堆碴高度变化的难题，笔者提出双光束双钻臂车体定位的新方法，从理论上证明了车体位姿解的惟一性，给出了车体定位计算求解过程，并在绿水洞煤矿成功实现井下钻孔自动定位工业性试验。
车体定位即确定台车与巷道位置关系。双钻臂台车如图1所示，左右钻臂对称，各有5个转动轴和1个移动轴。台车停在工作面前适当位置，以设计姿态钻孔，根据台车移动特点建立两空间坐标系如图2所示，坐标系分别为:车体基坐标系{OC}和工作面坐标系{OG}。要求将钻具轴线向量stick在{OC}下的坐标表达实时转换到其在{OG}下的坐标表达，直到其与{OG}内炮孔设计向量hole共线，且当stick末端与hole始端重合时，钻孔自动定位过程结束，可以开始钻孔。图2中坐标系{O0L}和{O6L}，{O0R}和{O6R}分别为按照D-H法[14-15]确定的左侧钻臂首端和末端坐标系，右侧钻臂首端和末端坐标系。
图1 车体结构简图
Fig.1 Organization graph of drill rig
各工作循环停车位置是随机的，{OC}和{OG}的关系不固定。钻孔前应以简易办法快速确定本次{OC}和{OG}的变换关系，即车体定位。
针对原定位法存在的问题，本文提出双钻臂车体定位方法(以下简称双钻臂法):利用已有传感器，增加一路井下定向激光，以台车双侧钻臂同时穿过两束激光进行车体定位。该方法不存在车体双侧定位误差传递问题，且一定程度上可反映工作面左右不平整情况。
下面将分析双钻臂法能否得到车体定位的惟一解，是否可解决车体横向角度变化的问题。
双钻臂法原理如图3所示:提供两束平行激光，在工作面上的投影OL和OR分别作为左侧和右侧激光坐标系{OL}，{OR}的原点。定位过程中{OL}的YL轴和{O6L}的Y6L轴形成角变量θL，{OR}的YR轴和{O6R}的Y6R轴形成角变量θR。在θL和θR的值随机变动的过程中，{OC}的原点OC到激光的距离保持不变(点绕轴转动，点到轴的距离不变)。图3中h为OC到履带底板高度。
图2 车体定位的数学坐标系表述
Fig.2 Mathematical description of carriage positioning
图3 新车体定位方法示意
Fig.3 New carriage positioning method
此时两侧激光之间距离确定，OC到两侧激光的两个距离不变。则这3个距离将构成一个三边长度已知的稳定三角形。因此求解θL和θR值的问题转化为求解OC位置的问题，并进一步转化为已知三角形两顶点坐标和三条边长，求另一顶点坐标的问题。该问题具有惟一解如图4所示，到OL固定距离的点构成解集A，到OR固定距离的点构成解集B，同时满足两固定距离的解惟一确定为A∩B，因此OC，OL，OR构成一稳定三角形。
图4 真实解的惟一性
Fig.4 Uniqueness of true solution
双钻臂法计算流程如图5所示。
图5 计算流程
Fig.5 Process of calculation
以下将根据流程阐述详细计算过程。
以左侧钻臂为例，基于D-H法在各轴建系{OiL}(i=1,2,…,6)如图6所示，并在激光投影位置建立{OL}，{OC}与{O0L}在车体侧视图内重合。图6中a1,a2,d3,a4,d5,d6为对应位置钻臂的杆件尺寸。首先通过相邻轴坐标系之间的变换关系逐级推导出{O0L}和{O6L}的位姿矩阵。
图6 左侧钻臂连杆坐标系
Fig.6 Linkage coordinate system of left boom
沿各轴方向确定各坐标系Z轴，ZiL为{OiL}的Z轴，Z(i-1)L为{O(i-1)L}的Z轴。{O0L}的X0L轴方向人为设定，其余XiL=Z(i-1)LZiL(i=1,2,…,6)，XiL为{OiL}的X轴，按右手法则确定各坐标系Y轴。
按D-H法位姿矩阵通式Ai确定相邻两坐标系(两轴)间的位姿矩阵:
(1)
式中，ai-1，di为连杆长度参数;αi-1，θi为连杆间角度参数;(xiL,yiL,ziL)为点或向量在坐标系{OiL}下的坐标表达;(x(i-1)L,y(i-1)L,z(i-1)L)为点或向量在坐标系{O(i-1)L}下的坐标表达。
通过连续矩阵相乘，得到{O0L}和{O6L}的位姿矩阵
式中，A1L为左侧{O0L}和{O1L}的位姿矩阵，…，A6L为左侧{O5L}和{O6L}的位姿矩阵。
设{O6L}在{O0L}下的位姿矩阵:
(2)
式中，(nxL,nyL,nzL)，(oxL,oyL,ozL)，(axL,ayL,azL)，(pxL,pyL,pzL)分别为{O6L}的X6L轴，Y6L轴，Z6L轴，原点{O6L}在{O0L}下的坐标表达。
同理，{O6R}在{O0R}下的位姿矩阵:
(3)
将{O6L}，{O6R}到{O0L}，{O0R}的位姿矩阵
扩充为{OC}到{OL}，{OR}的位姿矩阵。
设{OC}位于{O0L}和{O0R}中间，三者仅有平移变换而无旋转变换。设平移距离为ε，则{O6L}，{O6R}在{OC}下的位姿矩阵:
(4)
式中，ε为双侧钻臂间距一半，为正数。
对
求逆，得到{OC}在{O6L}，{O6R}下的位姿矩阵:
(5)
其中左侧:
其中pL=(pxL,pyL+ε,pzL)，nL=(nxL,nyL,nzL)，oL=(oxL,oyL,ozL)，aL=(axL,ayL,azL)。右侧:
其中pR=(pxR,pyR-ε,pzR)，nR=(nxR,nyR,nzR)，oR=(oxR,oyR,ozR)，aR=(axR,ayR,azR)。
由式(5)得OC在{O6L}，{O6R}下的X，Y坐标为:
再考虑与两侧钻具轴线平行的激光。定位过程中为保证平行关系成立，两侧各特制了两枚激光靶，确保OL，OR在{O6L}，{O6R}下的X，Y坐标分别为:(εx,εy)，(-εx,εy)。
θR=0，θL=0时，可得到{OC}在{OL}，{OR}下的位姿矩阵:
(6)
设OC到两束激光距离分别为LL，LR。LL，LR不会随θL，θR取值变化而变化。由式(6)得:
(7)
因{OL}，{OR}的ZL轴，ZR轴与{OG}的ZG轴同向，在不涉及Z坐标时，可认为三坐标系的XOY平面共面。设OL，OR在{OG}的XOY平面内的坐标分别为(UL,VL)，(UR,VR)，利用MATLAB编程求解到OL点距离LL，同时到OR点距离LR的点的坐标(U,V)，作为OC在{OG}的XOY平面内的投影点坐标如图7所示(舍去V值较大的解)。
图7 求车体基坐标系原点坐标
Fig.7 Find the origin coordinates of the carriage coordinate system
由坐标(U,V)可得车体偏离巷道中线距离E，并根据h计算履带底板下方堆碴高度H。
(8)
式中，E为车体偏中距;H为履带底板下方堆碴高度。
再求θL，θR，以右侧为例，如图8所示。工作面坐标系下θR=0时，(U,V)到(UR,VR)的向量应为
而由坐标计算得到该向量实际为(U-UR,V-VR)。两向量不相等时，说明θR≠0，向量夹角即为θR值，左侧同理。θL，θR分别为
(9)
图8 求角变量
Fig.8 Calculate the rotation angle
将式(6)所得θR=0，θL=0时，{OC}在{OL}，{OR}下的位姿矩阵
左乘旋转矩阵Rot(ZL,θL)，Rot(ZR,θR)得新位姿矩阵
相当于将θR=0，θL=0条件下{OC}绕右侧(左侧)激光旋转θR(θL)，得到存在θR，θL时{OC}的真实解。
(10)
式中，Rot(ZL,θL)表示绕ZL轴旋转θL所对应矩阵;Rot(ZR,θR)表示绕ZR轴旋转θR所对应矩阵。
{OL}，{OR}和{OG}仅存在平移变换。得到{OC}在{OG}下的两侧表达形式
完成车体定位:
(11)
在实验厂房内验证双钻臂法。模拟巷道高3.6 m，宽4.8 m。台车停于工作面前，h=1 256 mm，H=420 mm，稳定后不再移动。分别使用全站仪测量方法和双钻臂法计算车体位姿，将2组结果进行对比。
用全站仪测量车体位姿，在车体四周合适位置设四控制点D,G,E,F如图9所示，线段DG⊥DE且DG⊥GF。DE距巷道中线1 645 mm，与左侧激光指向重合，GF距巷道中线1 580 mm，与右侧激光指向重合。
图9 全站仪测量布点
Fig.9 Measuring points of total station
在F点架设全站仪，设置向量FG方向为0角度方向。在车体左右两侧各找一点WL，WR，测得两点坐标为(1.481,2.157,0.969)，(1.490,1.093,0.978)，计算得到E=(2 157+1 093)/2-1 580=45 mm，车体左右翻转角-0.48°。在车体右侧竖直面板上找前后两点W1，W2，测量得到两点坐标为(2.710，1.071，0.873)，(1.587，1.061，0.884)，计算得到前后俯仰角-0.56°。
使用双钻臂法计算车体位姿，根据台车尺寸确定D-H法参数列于表1，尺寸变量单位为mm，角度变量单位为(°)。表1中di，θi两列中θ1～θ5以及d6为轴变量，其值由传感器实时确定。小括号内数据为各轴位于基准位置时变量取值如图10所示。
表1 简化机构连杆参数表(右侧)
Table 1 Link parameters of simplify mechanism(right side)
图10 各轴基准位置
Fig.10 Reference position of each axis
台车双侧钻臂宽2ε=530 mm;激光靶参数εx=60 mm，εy=120 mm。
车体两侧激光:OL位于{OG}左上方(1 645,1 875)位置，OR位于{OG}右上方(-1 580,1 840)位置。移动双侧钻臂使钻具轴线与激光平行。完成后，两侧各传感器读数为:(-25.840°，-9.624°，0.309°，9.185°，28.330°，1 183.680 mm)，(25.356°，-10.063°，0.369°，9.053°，-28.969°，1 097.002 mm)。
由式(1)～(6)确定
由式(7)得:LL=1 609.7，LR=1 639.1。
MATLAB编程求得OC在工作面上的投影坐标(U,V)=(49.4,1 662.4)，由式(8)得:E=U=49.4 mm，H=V-h=1 662.4-1 256=406.4 mm，与全站仪测量结果吻合。
由式(9)得:θL=3.384°;θR=-1.687°。
由式(10)，(11)得
根据位姿矩阵求得车体前后俯仰角，左右翻转角为(-0.76°,-0.43°)。与全站仪测量结果基本一致。
综上，采用双钻臂法能有效定位。注意到OC在
中有相同的X坐标
和Y坐标
但Z坐标不同
说明上个循环工作结束后，工作面左侧平均掘进深度比右侧多出105.9 mm。为维持工作面平整程度，本次循环右侧应适当增加钻孔深度。
在车体定位参数中，以E和H为例，进一步分析θR，θL变化时，引起的定位参数变化。基准平面设置为H=420 mm，基准偏中距设置为E=45 mm。
以左侧为例，首先计算θL取值不同造成车体位置计算结果与实际结果的差异如图11所示。
图11 角变量和车体位置参数关系
Fig.11 Relationship between angle variable and body position parameter
横坐标为θL，θL在0±10°内变化。图11中位置较高曲线表示θL与H的关系;位置较低的表示θL与E的关系。
以H=420 mm为基准面，θL=-10°时对应H=782 mm，二者相差86%;θL=10°时对应H=224 mm，二者相差47%;即使按照θL=0°计算该比例也达到19%。采用双钻臂法计算出θL=3.384°，误差比例为3.3%。
全站仪测得E=45 mm，θL=10°时对应E=85 mm，二者相差88%;而采用双钻臂法计算出θL=3.384°，误差比例为9.7%。
可见θL对车体位置计算结果的影响很大。其中H在后续计算中更为重要。通过双钻臂法将该值误差控制在3.3%，较为可靠。
下面讨论θL变化造成车体定位误差对钻孔定位的影响。假设右侧激光所指位置存在一待钻目标孔。车体精确定位后，按照右侧各传感器读数移动右侧钻臂，钻具末端仍能定位到该位置。当θL取不同值，系统仍按传感器读数进行钻孔定位后，该炮孔实际定位结果与设计孔位的偏离量如图12所示。
图12 角变量和炮孔位置关系
Fig.12 Relationship between angle variable and position of hole
横坐标仍为θL，纵坐标为炮孔定位位置与设计位置距离。θL=-10°时，钻孔定位误差达到75 cm。为控制钻孔定位精度在5 cm以内，θL计算误差应在1.5°内，这在井下清碴很难做到，因此不考虑车体横向转角将对最终钻孔定位精度影响较大，采用双钻臂法则可避免这种情况。
对比采用①全站仪+原方法(左侧定位)和②双钻臂法时的钻孔定位精度(均不进行定位误差补偿)如图13所示。图中2条黑线为①法左右两侧钻孔定位误差结果，红线为②法左右两侧结果，实线为左侧炮孔，虚线为右侧炮孔，因巷道中线上有3孔由左侧钻臂完成，归为左侧孔，故图中实线较长。横坐标代表孔序。
图13 2种方法钻孔定位结果对比
Fig.13 Comparison of the results of hole positioning between two methods
由图13可知:原方法两侧炮孔误差明显不同，对侧右侧(虚线)误差比定位侧左侧(实线)误差大，前者集中在12～20 cm，平均误差16.7 cm，后者集中在0～12 cm，平均误差9.3 cm。而双钻臂法任一侧误差均与原方法定位侧误差精度一致，平均误差8.6 cm。
原方法对对侧炮孔存在误差传递并放大的问题。全站仪测量准确的情况下，原方法定位侧炮孔误差精度与双钻臂法双侧炮孔误差精度一致。未补偿前精度均在0～12 cm内。原方法对侧平均误差比定位侧及双钻臂法任一侧平均误差高94%，补偿难度大。
(1)针对原有车体定位方法在工作面堆碴高度变化时定位误差很大甚至无法使用的问题，提出了双钻臂车体定位法，通过数学推导证明了解的惟一性，这一方法已在井下工业性试验得到成功验证，具有普遍性，可用于同类型凿岩台车车体定位。
(2)车体绕激光横向旋转的角变量是求解车体位置参数的关键因素。当横向偏转角为10°时，用过去定位法的车体竖向位置误差达86%。双钻臂法通过准确求解定位过程中的角度值，将竖方向位置误差控制在5%以内，准确度较高。
(3)采用原有方法，为将精度控制在5 cm以内需将车体横向角度控制1.5°以下，这对井下清碴要求很高。双钻臂法不仅从理论上对堆碴高度没有限制，而且将随机变化的堆碴条件转变为求解对象，从定位原理上避免了过去方法的缺陷。
(4)单钻臂定位存在误差向非定位侧钻臂传递的现象，非定位侧炮孔平均定位误差较定位侧高出94%。双钻臂法定位则不存在这一问题，工作面左右两侧钻孔定位精度均在12 cm内，有利于全断面炮孔高效率补偿。
参考文献
[1] WANG Y,FANG C,JIANG Q,et al.The automatic drilling system of 6R-2P mining drill jumbos[J].Advances in Mechanical Engineering,2015,7(2):504861-504861.
[2] 
E,LEWANDOWSKI T.New generation automated drilling machine for tunnelling and underground mining work[J].Automation in Construction,2008,17(3):224-231.
[3] DERLUKIEWICZ 
J.Static and dynamic analysis of telescopic boom of self-propelled tunnelling machine[J].Journal of Theoretical and Applied Mechanics,2012,50(1):47-59.
[4] 王恒升,何清华,邓春萍.凿岩台车自动化控制系统的发展、现状及展望[J].测控技术,2007,26(3):1-4.
WANG Hengsheng,HE Qinghua,DENG chunping,Development and prospect of automatic control system of drill rig[J].Measurement and Control Technology,2007,26(3):1-4.
[5] 谢习华,何清华,郭勇.隧道凿岩机器人钻臂移位跟踪与控制系统[J].凿岩机械气动工具,2000(2):31-34.
XIE Xihua,HE Qinghua,GUO Yong.The control system of booms moving of rock-drilling robot[J].Rock Drilling Machinery and Pneumatic Tools,2000(2):31-34.
[6] 周友行,何清华,徐海良,等.多关节凿岩机械手快速定位方法[J].中南工业大学学报,2001,32(6):622-625.
ZHOU Youhang,HE Qinghua,XU Hailiang,et al.The speediness orientation of the multi-joint rock-drilling manipulator[J].Journal of Central South University Science and Technology,2001,32(6):622-625.
[7] 李军利,廉自生,李元宗.机器人化掘进机的运动分析及车体定位[J].煤炭学报,2008,33(5):583-587.
LI Junli,LIAN Zisheng,LI Yuanzong.Kinernatics analysis and carriage positioning of roboticized road header[J].Journal of China Coal Society,2008,33(5):583-587.
[8] ELSRUD Rolf.Modern trends in drill rig navigation and system integration cases studies from boliden and Kemi underground mines[A].14th international symposium on mine planning and equipment selection,MPES 2005 and the 5th international conference on comprter applications in the minerals industries,CAMI[C].2005:292-310.
[9] 何清华,曾桂英.凿岩机器人的车体定位[J].中南大学学报(自然科学版),1999,29(3):299-302.
HE Qinghua,ZENG Guiying.Carriage positioning of rock-drilling robot[J].Journal of Central South University (Science and Technology),1999,29(3):299-302.
[10] 谢习华,何清华,周亮.隧道凿岩机器人的车体定位方法[J].同济大学学报(自然科学版),2001,29(9):1032-1035.
XIE Xihua,HE Qinghua,ZHOU Liang.Methods for carriage positioning of rock-drilling robot[J].Journal of Tongji University (Natural Science),2001,29(9):1032-1035.
[11] 李延龙,胡国斌,蒋先尧,等.凿岩台车钻孔定位技术及应用[J].黄金,2017,38(10):45-48.
LI Yanlong,HU Guobin,JIANG Xianyao,et al.Borehole-location technique with drill jumbo and its application[J].Gold,2017,38(10):45-48.
[12] 王宪伦,喻洋,王道全,等.凿岩机器人的建模与运动学分析[J].矿山机械,2016,44(1):90-93.
WANG Xianlun,YU Yang,WANG Daoquan,et al.Modeling and kinematics analysis of rock drilling robot[J].Mining and Processing Equipment,2016,44(1):90-93.
[13] 张付祥,史文军.双臂掘进钻车自动化钻孔方法研究[J].工矿自动化,2016,42(6):42-45.
ZHANG Fuxiang,SHI Wenjun.Research on an automatic drilling method for dual-arm drill rig[J].Indrstry and Mine Automation,2016,42(6):42-45.
[14] 何清华.隧道凿岩机器人[M].长沙:中南大学出版社,2005:43-66.
[15] 刘泽鑫.电脑凿岩台车钻臂动力学分析[D].成都:西南交通大学,2012:13-18.
LIU Zexin.The dynamic analysis of computer drill jumbo boom[D].Chengdu:Southwest Jiaotong University,2012:13-18.