基于双侧钻臂位姿协同约束的凿岩台车车体定位方法.pdf

第4 4 卷第2 期 2 0 1 9 年2 月 煤炭学报 J O U R N A LO FC H I N AC O A LS O C I E T Y V 0 1 ．4 4N o ．2 F e b ．2 0 1 9 移动阅读 吴昊骏，龚敏．基于双侧钻臂位姿协同约束的凿岩台车车体定位方法[ J ] ．煤炭学报，2 0 1 9 ，4 4 2 6 4 7 - 6 5 4 ． d o i 1 0 ．1 3 2 2 5 ／j ．c n k i ．j C C S ．2 0 1 8 ．0 2 3 8 W UH a o j u n ，G O N GM i n ．C a r r i a g ep o s i t i o n i n gm e t h o do fd r i l lr i gb a s e do nm u t u a lc o n s t r a i n t so fd u a lb o o m s [ J ] ．J o u r n a l o fC h i n aC o a lS o c i e t y ，2 0 1 9 ，4 4 2 6 4 7 6 5 4 ．d o i 1 0 ．1 3 2 2 5 ／j ．c n k i ．j e c s ．2 0 1 8 ．0 2 3 8 基于双侧钻臂位姿协同约束的凿岩台车车体定位方法 吴昊骏，龚敏 北京科技大学土木与资源工程学院，北京1 0 0 0 8 3 摘要车体定位是凿岩台车钻孔自动控制的先决工序，原有车体定位方法仅在理想无碴工作面可 行，当有堆碴车体横向倾斜情况下，推进梁可绕激光束横向旋转造成角变量无法锁定，车体位姿存 在无穷解，导致定位误差很大甚至无法使用。为解决这一问题提出利用双钻臂同时参与定位的新 方法，从理论上证明了双钻臂法解算车体位姿的可行性，将求车体位姿惟一解的问题转化为求解固 定边长三角形顶点位置的问题；基于D - H 法及坐标系平移确定了车体一激光坐标系变换关系；引 入集合条件找出车体基坐标系实际原点位置，通过M A T L A B 编程计算车体两侧角变量值，求解车 体基坐标系在工作面坐标系下的位姿矩阵，实现车体定位。现场测试结果表明双钻臂法充分适应 有渣条件堆碴高度变化，并能快速求得车体实际位姿确定值；车体竖直方向定位误差在5 ％以内， 不采用误差补偿手段条件下钻孔定位精度在1 2c m 内。通过对比两种车体定位方法对钻孔定位效 果的影响发现原方法存在定位误差向非定位侧钻臂传递的现象，全站仪测量准确的情况下，非定 位侧炮孔平均定位误差较定位侧高出9 4 ％。双钻臂法定位则不存在这一问题，其双侧炮孔定位精 度与原方法定位侧炮孔定位精度一致，对于误差补偿较为有利。双钻臂定位法已在井下试验得到 成功验证，具有普遍性，可用于同类型凿岩台车车体定位。 关键词凿岩台车；车体定位；双钻臂；位姿矩阵 中图分类号T D 4 2 1 ．2文献标志码A文章编号0 2 5 3 9 9 9 3 2 0 1 9 0 2 - 0 6 4 7 - 0 8 C a r r i a g ep o s i t i o n i n gm e t h o do fd r i l lr i gb a s e do nm u t u a lc o n s t r a i n t s o fd u a lb o o m s W UH a o j u n ．G O N GM i n S c h o o lo f C i v i la n dR e s o u r c eE n g i n e e r i n g ，U n i v e r s i t yo f S c i e n c ea n dT e c h n o l o g yB e i j i n g ，B e i j i n g1 0 0 0 8 3 ，C h i n a A b s t r a c t C a r r i a g ep o s i t i o n i n gi st h ep r e r e q u i s i t ep r o c e s sf o rt h ea u t o m a t i cc o n t r o lo ft h ed r i l l i n go ft h ed r i l lr i g ．T h e o r i g i n a lm e t h o di so n l yf e a s i b l ei nt h ei d e a lw o r k i n gf a c eh a sn os t o n es t a c k e d ．T h ec a r r i a g eo fd r i l lr 培i st i l t e dl a t e r a l - l yw h e nt h e r ea r es t o n e ss t a c k e d ．T h ep u s hb e a mc a nb er o t a t e dl a t e r a l l ya r o u n dt h el a s e rb e a m ．T h ea n g u l a rv a r i a b l e c a n n o tb el o c k e d ，a n dt h e r ei sa ni n f i n i t es o l u t i o nt ot h ep o s eo ft h ec a r r i a g e ，r e s u l t i n gi nal a r g ep o s i t i o n i n ge r r o ro r e v e nu n u s a b l e ．I no r d e rt os o l v et h i sp r o b l e m ，an e wm e t h o do fu s i n gt h eb o o m si nd u a ls i d e st op a r t i c i p a t ei np o s i t i o n i n gi sp r o p o s e d ．I ti st h e o r e t i c a l l yp r o v e dt h ef e a s i b i l i t y ，a n dt h ep r o b l e mo ff i n d i n gt h eu n i q u es o l u t i o no ft h ec a r r i a g e p o s ei s s o l v e dt os o l v et h ev e r t e x p o s i t i o no ft h et r i a n g l ew h e nt h el e n g t ha r ef i x e d ；t h et r a n s f o r m a t i o nr e l a t i o n s h i po f t h ec a r r i a g e l a s e rc o o r d i n a t es y s t e mi sd e t e r m i n e db a s e do nt h eD Hm e t h o da n dt h ec o o r d i n a t es y s t e mt r a n s l a t i o n ；t h e s e tc o n d i t i o ni si n t r o d u c e dt of i n dt h ea c t u a lo r i g i np o s i t i o no ft h ec a r r i a g ec o o r d i n a t es y s t e m ，a n dt h ea n g u l a rv a l u e so f 收稿日期2 0 1 8 - 0 2 2 4修回日期2 0 1 8 - 0 6 - 1 0 责任编辑毕永华 作者简介吴吴骏 1 9 9 0 一 ，男，安徽六安人，博士研究生。E m a i l w h j c y y x z c g m a i l ．c o r n 通讯作者龚敏 1 9 6 3 一 ，男，重庆巫山人，教授，博士生导师。T e l 0 1 0 - 6 2 3 3 2 9 3 9 ，E m a i l g o n g m u s t b 1 6 3 ．c o r n 万方数据 煤炭学报 2 0 1 9 年第4 4 卷 t h ed u a ls i d e so ft h ed r i l lr i ga r ec a l c u l a t e db yM A T L A Bp r o g r a m m i n g ．S o l v et h ep o s em a t r i xo ft h ec a r r i a g ec o o r d i n a t e s y s t e mi nt h ew o r k i n gf a c ec o o r d i n a t es y s t e mt or e a l i z et h ec a r r i a g ep o s i t i o n i n g ．T h ef i e l dt e s tr e s u l t ss h o wt h a tt h en e w m e t h o df u l l ya d a p t st ot h eh e i g h tc h a n g eo ft h es t o n ec o n d i t i o n ，a n dc a nq u i c k l yd e t e r m i n et h ea c t u a lp o s i t i o na n dp o s e o ft h ec a r r i a g e ；t h ev e r t i c a lp o s i t i o n i n ge r r o ro ft h ec a r r i a g ei sw i t h i n5 ％．D r i l l i n gp o s i t i o n i n ga c c u r a c yi sw i t h i n1 2c m w h e nt h ee r r o rc o m p e n s a t i o nm e t h o di sn o tu s e d ．B yc o m p a r i n gt h ei n f l u e n c eo ft w om e t h o d so nt h ed r i l l i n gp o s i t i o n i n g e f f e c t ，i ti sf o u n dt h a tt h eo r i g i n a lm e t h o dh a st h ep h e n o m e n o nt h a tt h ep o s i t i o n i n ge r r o ri st r a n s m i t t e dt ot h en o n - p o s i - t i o n i n gs i d eb o o m ．W h e nm e a s u r e dd a t ao ft o t a ls t a t i o na r ea c c u r a t e ，t h ea v e r a g ep o s i t i o n i n ge r r o ro fb l a s t h o l e si nt h e n o n - p o s i t i o n i n gs i d ei s9 4 ％h i g h e rt h a nt h a ti nt h ep o s i t i o n i n gs i d e ．T h e r ei sn os u c hp r o b l e mw i t ht h ep o s i t i o n i n go f t h en e wm e t h o d ．T h ep o s i t i o n i n ga c c u r a c yo ft h eb l a s t h o l e si nd u a ls i d e si sc o n s i s t e n tw i t ht h a ti nt h ep o s i t i o n i n gs i d e o ft h eo r i g i n a lm e t h o d ，w h i c hi sa d v a n t a g e o u sf o re r r o rc o m p e n s a t i o n ．T h en e wm e t h o dh a sb e e ns u c c e s s f u l l yv e r i f i e di n t h er o a d w a ya n di su n i v e r s a l ．I tc a nb eu t i l i z e dt ot h ep o s i t i o n i n go ft h es a m et y p eo fd r i l lr i g ． K e yw o r d s d r i l lr i g ；c a r r i a g ep o s i t i o n i n g ；d u a lb o o m s ；p o s em a t r i x 凿岩台车钻孔自动定位是掘进技术发展的重要 方向和近年来研究热点‘1 叫1 。钻孔自动定位首先需 要解决车体定位 台车与巷道位置关系 问题，车体 定位后计算机才能据此确定各钻臂移动参数和实施 动作控制[ 5 - 6 ] ，因此具有非常重要的作用。 由于产权保护原因国外车体定位文献很少，可供 借鉴参考的资料有限。李军利等“ 1 研究了与台车类 似的掘进机车体定位，通过坐标变换矩阵并基于闭环 尺寸链进行定位，然而每次掘进前需用全站仪测量的 方式较麻烦并影响施工；E L S R U DR o l l 1 介绍了阿特 拉斯台车自动定位R C S 控制系统，首次提出利用钻 臂穿过巷道定向激光的车体定位方法，但文献没有阐 述具体解算过程；对此何清华，谢习华等∽。1 刨详细推 导了该方法的计算求解步骤，为我国首台隧道凿岩机 器人研发奠定了重要基础，一些学者⋯。1 副据此进行 了相关研究，这也是目前普遍认可的定位方法。但这 一方法在实际应用时因堆碴高度变化 现场常见情 况 误差很大，采用车体横向角度传感器作补偿，在 现场应用时准确性较差。 为解决上述方法无法适应堆碴高度变化的难题， 笔者提出双光束双钻臂车体定位的新方法，从理论上 证明了车体位姿解的惟一性，给出了车体定位计算求 解过程，并在绿水洞煤矿成功实现井下钻孑L 自动定位 工、f k 性试验。 1 双钻臂法与车体位姿惟一性证明 1 ．1 车体定位问题的理论表述 车体定位即确定台车与巷道位置关系。双钻臂 台车如图1 所示，左右钻臂对称，各有5 个转动轴和 1 个移动轴。台车停在工作面前适当位置，以设计姿 态钻孔，根据台车移动特点建立两空间坐标系如图2 所示，坐标系分别为车体基坐标系{ 0 ，} 和工作面坐 标系{ 0 。} 。要求将钻具轴线向量s t i c k 在{ 0 。} 下的 坐标表达实时转换到其在{ 0 。} 下的坐标表达，直到 其与{ 0 。} 内炮孔设计向量h o l e 共线，且当s t i c k 末端 与h o l e 始端重合时，钻孔自动定位过程结束，可以开 始钻孔。图2 中坐标系{ 0 。。，} 和{ 0 乩} ，{ 0 。。} 和 { 0 。。} 分别为按照D - H 法4 。1 纠确定的左侧钻臂首端 和末端坐标系，右侧钻臂首端和末端坐标系。 图1 车体结构简图 F i g ．1O r g a n i z a t i o ng r a p ho fd r i l lr i g 各工作循环停车位置是随机的，{ 0 。} 和{ 0 。} 的 关系不固定。钻孔前应以简易办法快速确定本次 { 0 。} 和{ 0 。} 的变换关系，即车体定位。 1 ．2 双钻臂法与位姿解惟一性证明 针对原定位法存在的问题，本文提出双钻臂车体 定位方法 以下简称双钻臂法 利用已有传感器，增 加一路井下定向激光，以台车双侧钻臂同时穿过两束 激光进行车体定位。该方法不存在车体双侧定位误 差传递问题，且一定程度上可反映工作面左右不平整 情况。 下面将分析双钻臂法能否得到车体定位的惟一 解，是否可解决车体横向角度变化的问题。 双钻臂法原理如图3 所示提供两束平行激光， 在工作面上的投影0 。，和0 。分别作为左侧和右侧激 万方数据 第2 期吴吴骏等基于双侧钻臂位姿协同约束的凿岩台车车体定位方法 6 4 9 图2 车体定位的数学坐标系表述 F i g ．2 M a t h e m a t i c a ld e s c r i p t i o no fc a r r i a g ep o s i t i o n i n g 光坐标系{ O ．} ，{ O 。} 的原点。定位过程中{ O 。．} 的 y L 轴和{ O 。。} 的y 6 。轴形成角变量0 。，{ O 。} 的y R 轴 和{ O 。。} 的y 6 。轴形成角变量0 。。在0 。和0 。的值随 机变动的过程中，{ O 。} 的原点O 。到激光的距离保持 不变 点绕轴转动，点到轴的距离不变 。图3 中h 为O ，到履带底板高度。 图3 新车体定位方法不意 F i g ．3 N e wc a r r i a g ep o s i t i o n i n gm e t h o d 此时两侧激光之间距离确定，O 。到两侧激光的 两个距离不变。则这3 个距离将构成一个三边长度 已知的稳定三角形。因此求解0 。．和0 。值的问题转 化为求解O 。位置的问题，并进一步转化为已知三角 形两顶点坐标和三条边长，求另一顶点坐标的问题。 该问题具有惟一解如图4 所示，到O 。固定距离的点 构成解集4 ，到O 。固定距离的点构成解集B ，同时满 足两固定距离的解惟一确定为A n B ，因此O 。，O O 。 构成一稳定三角形。 2 双钻臂法公式推导 双钻臂法计算流程如图5 所示。 以下将根据流程阐述详细计算过程。 2 ．1 钻臂基坐标系和末端坐标系位姿矩阵 以左侧钻臂为例，基于D H 法在各轴建系 { O 儿} i 1 ，2 ，⋯，6 如图6 所示，并在激光投影位置 建立{ O 。．} ，{ O 。} 与{ O 。。．} 在车体侧视图内重合。图6 图4 真实解的惟一性 F i g ．4U n i q u e n e s so ft r u es o l u t i o n l 建主r { 0 0 。} 和{ 0 6 。} 变换矩阵 建立{ D o 。 和{ 0 6 。 变换矩阵 J』 l 扩充建立{ D 。} 和{ D 。 扩充建立{ D c } 和{ D 。 变换矩阵 p 。 o 时 变换矩阵 日f 0 时 I 求{ D 。 原点位置 I 求转角变量0 。，0 。 I 求车体位姿矩阵，完成车体定位 图5 计算流程 F i g ．5 P r o c e s so fc a l c u l a t i o n 中o 。，o ，d 。，o 。，d ，，d 。为对应位置钻臂的杆件尺寸。 首先通过相邻轴坐标系之间的变换关系逐级推导出 { O m } 和{ O m } 的位姿矩阵。 图6 左侧钻臂连杆坐标系 F i g ．6L i n k a g ec o o r d i n a t es y s t e mo fl e f tb o o m Z L 万方数据 6 5 0 煤炭学报 2 0 1 9 年第4 4 卷 沿各轴方向确定各坐标系z 轴，z 儿为{ 0 儿} 的z 轴，z H 。为{ 0 H 。} 的z 轴。{ 0 。。} 的X o 。轴方向人 为设定，其余X 沮 Z m l 。Z 。L i 1 ，2 ，⋯，6 ，x 儿为 A i C O S0 i s i n0 I C O Sa i 一1 s i nO i s i nO t i 一1 0 { 0 儿} 的x 轴，按右手法则确定各坐标系y 轴。 按D H 法位姿矩阵通式A i 确定相邻两坐标 系 两轴 间的位姿矩阵 一s i n0 i C O S0 ￡C O SO t 。一l C O SO i s i nd i 一1 O 戈 i - 1 1 L y i 一1 L 名 i - 1 L 1 式中，o H ，d 。为连杆长度参数；O ／H ，0 。为连杆间角度 参数； z m Y ∞彳也 为点或向量在坐标系{ 0 儿} 下的坐 标表达； z 伊1 L ，Y “- 1 L ’彳伊1 L 为点或向量在坐标系 { 0 m 。、。} 下的坐标表达。 通过连续矩阵相乘，得到{ 0 。。} 和{ 0 。。} 的位姿 矩阵。 X O L Y O L Z O L 1 A 1 L A 2 L A 3 L A 4 L A 5 L A 6 L X 6 L y 6 L Z - 6 L 1 - - - m 磁 X 6 L y 6 L Z 6 L 1 式中，A 。。为左侧{ 0 。。} 和{ 0 。。} 的位姿矩阵，⋯，A a 。 为左侧{ 0 ，。} 和{ 0 。。} 的位姿矩阵。 设{ 0 。。} 在{ 0 。。} 下的位姿矩阵 瑶 o x Lp x L o 止P y L o 正p L 0l 2 司￡中， n 。L ，n y L ，n 扎 ， o 。L ，o 儿，o 止 ， o 。L ，o y L ， o ；。 ， p ∽p ∽P 。 分别为{ 0 。。} 的x 。。轴，y 6 。轴，邑。 轴，原点{ 0 。。} 在{ 0 。。} 下的坐标表达。 同理，{ 0 。。} 在{ 0 。R } 下的位姿矩阵 砭 3 2 ．2 车体基坐标系和激光坐标系位姿矩阵 将{ 0 乩} ，{ 0 。。} 到{ 0 。。} ，{ 0 。。} 的位姿矩阵， 贮扩充为{ 0 。} 到{ 0 。} ，{ 0 。} 的位姿矩阵。 设{ 0 。} 位于{ 0 。。} 和{ 0 。。} 中间，三者仅有平移 变换而无旋转变换。设平移距离为8 ，则{ 0 。。} ， { 0 。。} 在{ 0 。} 下的位姿矩阵 A 。 0 一S i nd L l c o so ／i 一1 0 戈儿 y 。L 彳B L 1 焉 略 o i 一1 一d 。s i nO t 。一1 d 。C O SO ／i l 1 p 。L P ，L s p L 1 P 。R P 。R 一8 P R l 4 式中，s 为双侧钻臂间距一半，为正数。 对焉，吃求逆，得到{ 0 。} 在{ 0 。。} ，{ 0 。。} 下的 位姿矩阵 辟 赡 ～ 砰 焉 ～ 5 其中左1 贝0 p 一n L p L ；p ，- L 1 一o L p L ；p - L 1 一a L P L ，其中 P L P 。L ，P 儿 s ，P 正 ，n L 凡。L ，n y L ，1 7 , 正 ，o L 0 x L ，o y L ， o L ，o L n 儿，1 7 , v L ，n L 。右倾4 p 1 R 一n 印R ；p v - R 1 一o R p R ；p - R 1 一a R p R ，其中P R P 。R ，P ，R s ， P ；R ，n R n ，R ，n y R ，n R ，o R o 。R ，o y R ，o 。R ，o R o ，R ，o ，R ，o ；R 。 由式 5 得0 。在{ 0 。。} ，{ 0 。。} 下的x ，y 坐标 为 p ，p ， p 矗，p 蒜 。 再考虑与两侧钻具轴线平行的激光。定位过程 中为保证平行关系成立，两侧各特制了两枚激光靶， 确保0 。，0 。在{ 0 。。} ，{ 0 。。} 下的x ，l ，坐标分别 为 蹦，∥ ， 一蹦，彰 。 哪 咿岫。 咿 咿 咿o Ⅳ 咿M o 咿 咿咿o 。儿一扎。止●。k一弧。‰， 以．n ．见， p p p 似 哪 渺。 邶 彬 鲫。 缈 缈 缈。 咿 叩 咿。 缈 咐 缈。 邶 咿 咿。 万方数据 第2 期吴昊骏等基于双侧钻臂位姿协同约束的凿岩台车车体定位方法 6 5 l 0 。 0 ，0 。 0 时，可得到{ 0 。} 在{ 0 。} ，{ 0 。} 下的 位姿矩阵 r E r S 一I p 扎一s z P ，L e y 一1 p ；L 1 p 。矗 8 戈 一】 P 。R e y 一1 P R 1 6 设0 。到两束激光距离分别为L 。，L 。。L 。，L 。不 会随0 0 。取值变化而变化。由式 6 得 L L √ p 。- L 1 一s 戈 2 p 一e y 2 L R √ p 。- R I s 戈 2 p 一e y 2 7 2 ．3 解车体基坐标系原点位置 因{ 0 。} ，{ 0 。} 的乙轴，Z 。轴与{ 0 。} 的z 。轴同 向，在不涉及Z 坐标时，可认为三坐标系的X O Y 平面 共面。设0 。，0 。在{ 0 。} 的X O Y 平面内的坐标分别 为 巩，K ， U 。，K ，利用M A T L A B 编程求解到0 。 点距离￡。，同时到0 。点距离L 。的点的坐标 u ，y ， 作为0 。在{ 0 。} 的X O Y 平面内的投影点坐标如图7 所示 舍去y 值较大的解 。 图7 求车体基坐标系原点坐标 F i g ．7 F i n dt h eo r i g i nc o o r d i n a t e so ft h ec a r r i a g e c o o r d i n a t es y s t e m 由坐标 u ，y 可得车体偏离巷道中线距离E ，并 根据h 计算履带底板下方堆碴高度H 。 盛孑一危 ㈩ 式中，E 为车体偏中距；H 为履带底板下方堆碴高度。 2 ．4 求角变量和车体位姿矩阵 再求0 。，0 。，以右侧为例，如图8 所示。工作面坐 标系下0 。 0 时， u ，y 到 u 。，‰ 的向量应为 p 。- 。1 蹦，p ，- 。1 一e y 。而由坐标计算得到该向量实际为 u 一 ‰，卜％ 。两向量不相等时，说明0 。≠0 ，向量夹角 即为0 。值，左侧同理。0 。，0 。分别为 n u u L ，y V L p 。- L I 一8 z ，p l 一￡y 叽一K ∞8 瓦r 一 ”a r c c 。s 生型鼍学堂 9 图8 求角变量 F i g ．8 C a l c u l a t et h er o t a t i o na n g l e 将式 6 所得0 。 0 ，0 。 0 时，{ 0 。} 在{ 0 。} ， { 0 。} 下的位姿矩阵畦，左乘旋转矩阵R o t 况， 0 。 ，R o t Z 。，0 。 得新位姿矩阵T 。。。，T 皂。。。相当于 将0 。 O ，0 。 0 条件下{ 0 。} 绕右侧 左侧 激光旋转 0 。 0 。 ，得到存在0 。，0 。时{ 0 。} 的真实解。 T L 。。。 R o t 磊，O L T L 吃。。 R o t Z 。，O R 砭 00 00 l0 01 0 0 O 0 10 Ol 1 0 式中，R o t Z 。，0 。 表示绕z 。轴旋转0 。所对应矩阵； R o t Z 。，0 。 表示绕z 。轴旋转0 。所对应矩阵。 { 0 。} ，{ 0 。} 和{ 0 。} 仅存在平移变换。得到 { 0 。} 在{ 0 。} 下的两侧表达形式。，8 ，完成车体 定位 砭。 霹8 T 。。。 砣⋯ 3 车体定位实例 在实验厂房内验证双钻臂法。模拟巷道高 T剖上吐 舢 岫 舢。 似 邶 鲫o L L L R R R 缈 咿 咿。 咿 咿 咿。 蛳 灿 灿。 咿 咿 咿。 矾一钆 旧艮 渤 吣 o o 豳 l 呈 o o C 一 吼吼 9 9●， 刚 o o l 宝 协o o “ ．虬 c s 乩K o ●‰K O ， 0 O 1 0 O O l 0 0 l 0 0 0 l 0 0 1 O 0 0 1 0 0 O 万方数据 6 5 2 煤炭 学报 2 0 1 9 年第4 4 卷 3 ．6I l l ，宽4 ．8m 。台车停于工作面前，h 12 5 6m m ， H 4 2 0m m ，稳定后不再移动。分别使用全站仪测量 方法和双钻臂法计算车体位姿，将2 组结果进行对 比。 3 ．1 全站仪测量车体实际位姿 用全站仪测量车体位姿，在车体四周合适位置设 四控制点D ，G ，E ，F 如图9 所示，线段D G 上D E 且D G 上G F 。D E 距巷道中线16 4 5m m ，与左侧激光指向重 合，G F 距巷道中线15 8 0m m ，与右侧激光指向重合。 模 拟 工 作 面 钢结构框架 图9 全站仪测量布点 F i g ．9M e a s u r i n gp o i n t so ft o t a ls t a t i o n 在F 点架设全站仪，设置向量F G 方向为0 角度 方向。在车体左右两侧各找一点睨，形。，测得两点 坐标为 1 ．4 8 1 ，2 ．1 5 7 ，0 ．9 6 9 ， 1 ．4 9 0 ，1 ．0 9 3 ， 0 ．9 7 8 ，计算得到E 21 5 7 10 9 3 ／2 15 8 0 4 5m m ，车体左右翻转角一0 ．4 8 。。在车体右侧竖直面 板上找前后两点暇，贩，测量得到两点坐标为 2 ．7 1 0 ，1 ．0 7 1 ，0 ．8 7 3 ， 1 ．5 8 7 ，1 ．0 6 1 ，0 ．8 8 4 ，计算 得到前后俯仰角一0 ．5 6 0 。 3 ．2 建立双钻臂法的位姿矩阵 使用双钻臂法计算车体位姿，根据台车尺寸确定 D - H 法参数列于表l ，尺寸变量单位为m m ，角度变 量单位为 。 。表1 中d 。，0 ；两列中0 ．～0 5 以及d 6 为轴变量，其值由传感器实时确定。小括号内数据为 各轴位于基准位置时变量取值如图1 0 所示。 表1 简化机构连杆参数表 右侧 T a b l e1 L i n kp a r a m e t e r so fs i m p l i f ym e c h a n i s m r i g h ts i d e 台车双侧钻臂宽2 e 5 3 0m m ；激光靶参数黜 6 0m m ，形 1 2 0m m 。 车体两侧激光0 。位于{ 0 。} 左上方 16 4 5 ， l8 7 5 位置，0 R 位于{ 0 G } 右上方 一15 8 0 ，18 4 0 位 基准位置 图1 0 各轴基准位置 F i g ．1 0 R e f e r e n c ep o s i t i o no fe a c ha x i s 置。移动双侧钻臂使钻具轴线与激光平行。完成后， 两侧各传感器读数为 一2 5 ．8 4 0 0 ，- 9 ．6 2 4 。，0 ．3 0 9 。， 9 ．1 8 5 0 ，2 8 ．3 3 0 0 ，11 8 3 ．6 8 0m m ， 2 5 ．3 5 6 0 ，一1 0 ．0 6 3 0 ， 0 ．3 6 9 0 ，9 ．0 5 3 0 ，- 2 8 ．9 6 9 0 ，l0 9 7 ． 1 0 2m m 。 由式 1 ～ 6 确定砖，砩 0 ．0 4 320 ．9 9 900 ．0 0 1016 0 5 ．3l 夺三i 0 0 92 ．。．- O ．哗0 0 1 三．。．1 ．0 0 。。0 9 0 3 ⋯- 1 1 3 8 蚍．0 I L 0001J 『- 也0 6 0 70 ．9 9 8o0 ．0 1 4l 1 6 3 3 8 ] 碎l 帅1 3 2 也0 1 33Q9 9 98 。1 龇l。 0 ．9 9 800 ．0 6 200 ．0 1 2453 3 3 ．3I L00 01 J 严0 4 2 6n9 9 74。0 加5 8o 坝4 ] 1 r G L l0 ．0 1 1 80 ．0 5 750 ．9 9 8316 6 2 ．4 J rl 。 l0 ．9 9 900 ．0 4 430 ．0 0 9354 3 9 ．2 l L 0001 J 『- 也0 6 0 3Q9 9 720 ．0 4 35 4 9 ．4 ] k l 一。Q 0 。6 2 0 ’。0 k 0 1 24 。5 3 1 3 3 3 J 00 J 万方数据 第2 期吴昊骏等基于双侧钻臂位姿协同约束的凿岩台车车体定位方法 z 坐标不同 凭。 3 ，4 一54 3 9 2 ，“ 3 ，4 一53 3 3 ．3 。说明上个循环工作结束后，工作面左侧 平均掘进深度比右侧多出1 0 5 ．9m m 。为维持工作面 平整程度，本次循环右侧应适当增加钻孔深度。 3 ．42 种定位方法对定位效果影响的讨论 在车体定位参数中，以E 和日为例，进一步分析 0 。帆0 变化时，引起的定位参数变化。基准平面设置 为H 4 2 0m m ，基准偏中距设置为E 4 5m m 。 以左侧为例，首先计算0 。取值不同造成车体位 置计算结果与实际结果的差异如图1l 所示。 萋 螽 篓 卅 一1 0 ，7 8 2 车体中心偏离巷道中线水平距离E 、．、．． 履带底板中线与巷道地板高度差Ⅳ 8 6 ％＼ o ，5 0 0基准面高H 4 2 0 m m ‘≮ 一k i 基准偏中距D 4 5m m 9 。- o 。；9 L 3 3 8 4 。 』．2 1 ％ i 一一 l O一5051 0 转角变量／ 。 图1 1 角变量和车体位置参数关系 F i g ．11R e l a t i o n s h i pb e t w e e na n g l ev a r i a b l ea n db o d y p o s i t i o np a r a m e t e r 横坐标为0 L 0 I ，在0 1 0 。内变化。图1 1 中位置 较高曲线表示0 。与Ⅳ的关系；位置较低的表示0 。与 E 的关系。 以H 4 2 0m m 为基准面，0 I ． 一1 0 。时对应H 7 8 2m m ，二者相差8 6 ％；9 I ． 1 0 0 时对应H 2 2 4m m ， 二者相差4 7 ％；即使按照0 。 0 0 计算该比例也达到 1 9 ％。采用双钻臂法计算出0 。． 3 ．3 8 4 0 ，误差比例为 3 ．3 ％。 全站仪测得E 4 5m m ，0 I ． 1 0 0 时对应E 8 5m m ，二者相差8 8 ％；而采用双钻臂法计算出0 ．． 3 ．3 8 4 0 ，误差比例为9 ．7 ％。 可见0 。．对车体