CN115556116B - 一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法 - Google Patents

一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法 Download PDF

Info

Publication number
CN115556116B
CN115556116B CN202211561133.7A CN202211561133A CN115556116B CN 115556116 B CN115556116 B CN 115556116B CN 202211561133 A CN202211561133 A CN 202211561133A CN 115556116 B CN115556116 B CN 115556116B
Authority
CN
China
Prior art keywords
angle
drilling
error
distance
inclination angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202211561133.7A
Other languages
English (en)
Other versions
CN115556116A (zh
Inventor
梁春苗
姚宁平
王小波
徐勇智
董洪波
李坤
彭光宇
李渊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
XI'AN RESEARCH INSTITUTE OF CHINA COAL RESEARCH INSTITUTE
Original Assignee
XI'AN RESEARCH INSTITUTE OF CHINA COAL RESEARCH INSTITUTE
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by XI'AN RESEARCH INSTITUTE OF CHINA COAL RESEARCH INSTITUTE filed Critical XI'AN RESEARCH INSTITUTE OF CHINA COAL RESEARCH INSTITUTE
Priority to CN202211561133.7A priority Critical patent/CN115556116B/zh
Publication of CN115556116A publication Critical patent/CN115556116A/zh
Application granted granted Critical
Publication of CN115556116B publication Critical patent/CN115556116B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1653—Programme controls characterised by the control loop parameters identification, estimation, stiffness, accuracy, error analysis
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
    • E21B15/00—Supports for the drilling machine, e.g. derricks or masts
    • E21B15/04—Supports for the drilling machine, e.g. derricks or masts specially adapted for directional drilling, e.g. slant hole rigs
    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
    • E21B7/00—Special methods or apparatus for drilling
    • E21B7/02—Drilling rigs characterised by means for land transport with their own drive, e.g. skid mounting or wheel mounting
    • E21B7/022—Control of the drilling operation; Hydraulic or pneumatic means for activation or operation

Landscapes

  • Engineering & Computer Science (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Mining & Mineral Resources (AREA)
  • Mechanical Engineering (AREA)
  • Robotics (AREA)
  • Physics & Mathematics (AREA)
  • Environmental & Geological Engineering (AREA)
  • Fluid Mechanics (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geochemistry & Mineralogy (AREA)
  • Earth Drilling (AREA)

Abstract

本发明公开了一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，该检测方法采用靶板、激光测距传感器和开孔定向仪配合进行倾角和方位角误差检测；该补偿方法的倾角误差补偿基于倾角中偏角和偏移量变化引入的虚关节建立倾角误差补偿模型，该补偿方法的方位角误差补偿基于关节转动的偏角引入虚关节建立方位角的误差补偿模型，倾角和方位角的误差补偿模型总和即为钻臂总误差补偿模型。本发明可以实时检测钻臂定位中的误差值，并实时进行钻臂定位的补偿，实现钻臂的精确控制。

Description

一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法
技术领域
本发明属于煤矿井下钻孔机器人精确开孔定位技术领域，具体涉及一种煤矿井下钻孔机器人钻臂激光测距组合定位误差检测与补偿方法。
背景技术
煤矿井下钻孔施工中，开孔精度影响成孔质量，成孔质量又会直接影响瓦斯抽采、探放水、注浆加固和工程加固的效果，因此精确调姿不但有利于瓦斯抽采效果，更是实现精准钻孔、精准快速治理的基础和前提。
钻孔机器人钻臂因受加工误差、装配误差和零部件磨损等影响，无法达到精确定位，进而影响瓦斯抽采、探放水、防突、防冲和工程加固的效果。而现有钻机位姿调节借助开孔定向仪和人工测量等，与手动、电控调姿配合，操作繁琐，并需要反复校准才能达到所需开孔位置，对工人操作要求高、位姿调节耗时、费力。即使少数钻机具有自动位姿控制功能，但采用开环控制，存在精度低、定位误差大、误差无法补偿的问题，难于实现钻孔快速自动精确定位，成为制约全自动施工的关键因素。
已有钻臂定位误差补偿方法标定工作量大，补偿过程复杂，费时费力，一次标定不能解决多运动状态变化补偿，且补偿不具备实时性，无法适应钻进机器人孔群施工中对定位误差和施工效率的要求，因此煤矿井下钻孔自动精确定位是实现钻孔机器人自主施工孔群的难点，也是实现少人化、无人化的关键技术。
发明内容
针对现有技术中存在的不足，本发明的目的在于，提供一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，针对亟待解决的钻孔机器人钻臂精确开孔定位问题，分析钻臂误差产生原因，针对经典误差补偿方法非实时性，提出在线钻臂位姿误差检测方法并建立误差补偿模型，实现钻臂的精确定位。
为了解决上述技术问题，本发明采用如下技术方案予以实现：
一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，该钻孔机器人包括钻进车体、设在钻进车体上依次排布的动力头、自对中夹持器和扶正器；钻孔机器人钻臂定位误差检测通过误差检测装置实现；
该误差检测与补偿方法包括以下步骤：
步骤一、布设误差检测装置并调平钻进车体；所述误差检测装置包括靶板、激光测距传感器和高精度开孔定向仪；
步骤二、将高精度开孔定向仪放置在倾角和方位角回转中心位置，利用钻孔机器人自带钻臂调姿模块调节倾角和方位角，利用高精度开孔定向仪反馈信号找到倾角和方位角的零位，读取激光测距传感器测得的距离，记作初始距离；
步骤三、再通过钻孔机器人钻臂调姿模块多次改变倾角和方位角后，即有多组倾角和方位角的输入值，通过激光测距传感器对应测得多组距离值以计算倾角和方位角实际计算值；
步骤四、激光测距组合法测倾角误差计算模型：
Figure 271284DEST_PATH_IMAGE001
（1）
上式中，Lc1是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，Ld1是激光测距传感器安装点在Y方向与转动中心O1的距离，Lz1是倾角变化后测距传感器测定的距离，钻臂平移油缸平移距离为Ly；
步骤五、激光测距组合法测方位角误差计算模型：
Figure 859392DEST_PATH_IMAGE002
（6）
上式中，激光测距传感器安装点在x方向与方位角水平线有h距离，△l是因方位角变化h增加段的距离，Lc2是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，Ld2是激光测距传感器安装点在x方向与方位角转动中心O的距离，Lz2是方位角变化后测距传感器测定的距离，平移油缸平移距离为Ly；
步骤六：钻臂总误差补偿模型为：
Figure 580223DEST_PATH_IMAGE003
（9）；
式（9）中，
Figure 371724DEST_PATH_IMAGE004
是相邻的第0个关节和第1个关节坐标系之间的齐次变换矩阵，
Figure 734572DEST_PATH_IMAGE005
是相 邻的第2个关节和第1个关节坐标系之间的齐次变换矩阵，
Figure 177186DEST_PATH_IMAGE006
是相邻的第3个关节和第2个关 节坐标系之间的齐次变换矩阵；Rot(z,Δθ1)是绕z轴的旋转Δθ1变换，Trans(Δz,0,0)是绕 x轴的移动Δz变换；Rot(y,Δθ2)是绕y轴的旋转Δθ2变换；
步骤七、利用RBF神经网络对总误差补偿矩阵进行逆解求解，得到的逆解即为误差补偿后的新倾角和方位角的控制输入量。
本发明还包括如下技术特征：
具体的，所述步骤一包括：在钻臂钻孔位置前方放置一个表面平整且靶板面平行于车体的测试靶板，靶板能接收到并反射激光信号；在动力头后部，安装一个高精度激光测距传感器，确保其发射的激光能穿过动力头内孔、钻杆中心通孔、两个夹持器和扶正器中心，投射到靶板上，且靶板反射的激光也能投射到激光测距传感器的激光接收窗口上，将数据采集板与激光测距传感器信号连接，可实时获取激光测距传感器的距离值；将高精度开孔定向仪与数据采集板信号连接，高精度开孔定向仪可测试钻臂倾角和方位角；布设好误差检测装置后，利用钻孔机器人自带调平模块，对钻臂所在车体进行调平，再用高精度开孔定向仪在车体四个角进行复测，确保车体已经调平。
具体的，所述步骤四中，由实际倾角θ2’可通过式（2）计算开孔位置x、y、z的具体坐标值，其中Lc1，Ly，Lz1，Ld1都是已知量，H0是钻臂举升初始距离，举升高度为△H1，当钻臂距离靶板某一位置时，实际倾角下钻孔开孔位置末端z坐标减去理论倾角z坐标值，就是倾角误差在高度方向的偏移量；
Figure 68918DEST_PATH_IMAGE007
（2）
当倾角改变时，末端开孔位置会产生一个方位上的偏角Δθ2，同时还会产生x轴的偏移量Δz，因此在引入虚关节，增加一个坐标变换矩阵补偿倾角关节引起的误差，对倾角的位姿误差进行补偿得式（3），引入虚拟关节后的倾角误差模型：
Figure 111830DEST_PATH_IMAGE008
（3）
式（3）中，
Figure 747210DEST_PATH_IMAGE005
是相邻的第2个关节和第1个关节坐标系之间的齐次变换矩阵；Trans (Δz,0,0)是绕x轴的移动Δz变换；Rot(y,Δθ2)是绕y轴的旋转Δθ2变换。
具体的，所述步骤五中，由实际方位角θ1’可通过式（7）计算开孔位置x、y、z的具体坐标值，其中Lc2，Ld2，Ly，Lz2，h，都是已知量，当钻机钻臂距离煤臂某一位置时，实际倾角下钻孔开孔位置末端x坐标减去理论倾角x坐标值，就是误差在x方向的偏移量△x，实际方位角下钻孔开孔位置末端θ1’减去输入方位角θ1，即方位角偏移量△θ1：
Figure 168964DEST_PATH_IMAGE009
（7）
方位角误差主要来自关节转动，末端开孔位置会产生一个方位上的偏角Δθ1，即此引入虚关节，绕Z轴旋转角度Δθ1的齐次变换，变换矩阵如下：
Figure 106964DEST_PATH_IMAGE010
（8）。
具体的，将步骤四和步骤五中倾角和方位角的误差补偿模型汇总得到步骤六中的钻臂总误差补偿模型。
具体的，所述步骤七包括：由式（1）和式（6）分别得到倾角和方位角误差变换量，利用式（9）求得末端开孔位姿，再由开孔位姿求倾角θ1和方位角θ2是求逆解问题，因此利用RBF神经网络法对式（9）的误差补偿矩阵进行逆解求解，得到的逆解即为误差补偿后的新倾角和方位角的控制量。
本发明与现有技术相比，具有如下技术效果：
本发明激光测距组合定位误差检测与补偿方法可以实时检测钻臂定位中的误差值，并实时进行钻臂定位的补偿，实现钻臂的精确控制。
附图说明
图1是本发明煤矿井下钻孔机器人及误差检测装置示意图；
图2是本发明煤矿井下钻孔机器人及误差检测装置示意图；
图3是基于激光测距组合位姿误差检测计算实际倾角误差计算模型；
图4是倾角误差补偿模型坐标变换；
图5基于激光测距组合位姿误差检测计算实际方位角误差计算模型。
附图标记含义：
1.钻进车体，2.动力头，3.夹持器，4.扶正器，5.靶板，6.激光测距传感器，7.开孔定向仪，8.钻杆，9.激光线。
具体实施方式
本发明提供一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，该检测方法采用测试靶板、高精度激光测距传感器和高精度开孔定向仪配合进行倾角和方位角误差检测；该补偿方法将倾角误差补偿模型和方位角的误差补偿模型结合进行钻臂总误差补偿。该激光测距组合定位误差检测与补偿方法可以实时检测钻臂定位中的误差值，并实时进行钻臂定位的补偿，实现钻臂的精确控制。
误差检测方法具体实施为：在钻臂前方放置测试靶板，在动力头后部中心处安装一个高精度激光测距传感器，确保激光穿过钻杆和动力头内孔，且保证激光发射和接收信号工作正常；将开孔定向仪放置在倾角和方位角回转中心位置，调节倾角和方位角，找到方位和倾角的零位，并得到初始时的激光测距距离，再通过改变倾角和方位角后再次测得距离，采用该方法可获得数组倾角、方位角输入值和实际计算值之间的关系，即可得钻臂在位姿变化时的误差量。
误差补偿方法具体为：该补偿方法的倾角误差补偿基于倾角中偏角和偏移量变化引入的虚关节建立倾角误差补偿模型，该补偿方法的方位角误差补偿基于关节转动的偏角，引入虚关节建立方位角的误差补偿模型，倾角和方位角的误差补偿模型总和即为钻臂总误差补偿模型。
以下给出本发明的具体实施例，需要说明的是本发明并不局限于以下具体实施例，凡在本申请技术方案基础上做的等同变换均落入本发明的保护范围。
实施例1：
本实施例提供一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，如图1和2所示，该钻孔机器人包括钻进车体、设在钻进车体上依次排布的动力头、自对中夹持器和扶正器；钻孔机器人钻臂定位误差检测通过误差检测装置实现，该误差检测装置包括靶板、激光测距传感器和高精度开孔定向仪；
该误差检测与补偿方法包括以下步骤：
步骤一、布设误差检测装置并调平钻进车体：在钻臂钻孔位置前方固定距离放置一个表面平整且靶板面平行于车体的测试靶板，靶板尺寸根据测试开孔高度、开孔倾角和开孔方位角设定，靶板能接收到并反射激光信号；在动力头后部，安装一个高精度激光测距传感器，确保其发射的激光能穿过动力头内孔、钻杆中心通孔、两个夹持器和扶正器中心，投射到靶板上，且靶板反射的激光也能投射到激光测距传感器的激光接收窗口上，将数据采集板与激光测距传感器信号连接，可实时获取激光测距传感器的距离值；将高精度开孔定向仪与数据采集板信号连接，高精度开孔定向仪可测试钻臂倾角和方位角；布设好误差检测装置后，利用钻孔机器人自带调平模块，对钻臂所在车体进行调平，再用高精度开孔定向仪在车体四个角进行复测，确保车体已经调平，得到车体距离地面距离；
步骤二、将高精度开孔定向仪放置在倾角和方位角回转中心位置，利用钻孔机器人自带钻臂调姿模块调节倾角和方位角，利用高精度开孔定向仪反馈信号找到倾角和方位角的零位，读取激光测距传感器测得的距离，记作初始距离；
步骤三、再通过钻孔机器人钻臂调姿模块多次改变倾角和方位角后，即有多组倾角和方位角的输入值，通过激光测距传感器对应测得多组距离值以计算倾角和方位角实际计算值；
步骤四、激光测距组合法测倾角误差计算模型与误差补偿模型：图3是基于激光测距组合位姿误差检测计算实际倾角误差计算模型，O1和O2点是倾角转动中心，A1点是激光测距传感器安装点，该安装位置在Y方向与转动中心有Ld1距离，Lc1是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，H0是钻臂举升初始距离，举升高度为△H1，Hz是在倾角改变和平移油缸平移后，激光测距传感器投射点距离车体地面的高度，可作为计算角度的复测值。钻臂平移油缸平移距离为Ly，当在开孔定位软件界面的输入倾角θ2后，倾角的变化引起了高精度激光测距传感器得到的距离的变化，Lz1是倾角变化后测距传感器测定距离，实际倾角θ2 ’可由式（1）计算。计算实际倾角θ2’为：
Figure 246959DEST_PATH_IMAGE011
（1）
上式中，如图3所示，O 1 和O 2 点是倾角转动中心，A 1 点是激光测距传感器安装点，Lc1是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，Ld1是激光测距传感器安装点在Y方向与转动中心O1的距离，Lz1是倾角变化后测距传感器测定的距离，钻臂平移油缸平移距离为Ly；H 0 是钻臂举升初始距离，举升高度为△H 1 ，H z 是在倾角改变和平移油缸平移后，激光测距传感器投射点距离车体地面的高度，可作为计算角度的复测值。
由实际倾角θ2’可通过式（2）计算开孔位置x、y、z的具体坐标值，其中Lc1，Ly，Lz1，Ld1都是已知量，H0是钻臂举升初始距离，举升高度为△H1，当钻臂距离靶板某一位置时，实际倾角下钻孔开孔位置末端z坐标减去理论倾角z坐标值，就是倾角误差在高度方向的偏移量；
Figure 686030DEST_PATH_IMAGE012
（2）
当倾角改变时，末端开孔位置会产生一个方位上的偏角Δθ2，同时还会产生x轴的偏移量Δz，如图4所示，X2O2Y2倾角回转中心，当倾角和平移油缸平移距离改变时，末端X3O3Y3的中心O3开孔位置会产生一个方位上的偏角Δθ2，同时还会产生x轴的偏移量Δz，因此在O2之后引入虚关节，增加一个坐标变换矩阵补偿倾角关节引起的误差，对倾角的位姿误差进行补偿得式（3），引入虚拟关节后的倾角误差模型：
Figure 851039DEST_PATH_IMAGE013
（3）
式（3）中，
Figure 84574DEST_PATH_IMAGE005
是相邻的第2个关节和第1个关节坐标系之间的齐次变换矩阵；Trans (Δz,0,0)是绕x轴的移动Δz变换；Rot(y,Δθ2)是绕y轴的旋转Δθ2变换。
步骤五、激光测距组合法测方位角误差计算模型与误差补偿模型：图5是基于激光测距组合位姿误差姿误差检测计算实际方位角误差计算模型，O点是方位角转动中心，A2点是激光测距传感器安装点，该安装位置在x方向与转动中心有Ld2距离，在x方向与方位角水平线有h距离，Lc2是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，平移油缸平移距离为Ly，当在开孔定位软件界面的将输入方位角θ1变化后，Lz2是方位角变化后测距传感器测定的距离，△l是因方位角变化h增加段的距离。实际方位角θ1’由式（4~6）计算：
存在关系式：
Figure 587231DEST_PATH_IMAGE014
（4）
因此有：
Figure 564414DEST_PATH_IMAGE015
（5）
通过式（6）计算得到实际方位角θ1’：
Figure 960761DEST_PATH_IMAGE016
（6）
上式中，如图5所示，O点是方位角转动中心，A 2点是激光测距传感器安装点，激光测距传感器安装点在x方向与方位角水平线有h距离，△l是因方位角变化h增加段的距离，Lc2是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，Ld2是激光测距传感器安装点在x方向与方位角转动中心O的距离，Lz2是方位角变化后测距传感器测定的距离，平移油缸平移距离为Ly；
由实际方位角θ1’可通过式（7）计算开孔位置x、y、z的具体坐标值，其中Lc2，Ld2，Ly，Lz2，h，都是已知量，当钻机钻臂距离煤臂某一位置时，实际倾角下钻孔开孔位置末端x坐标减去理论倾角x坐标值，就是误差在x方向的偏移量△x，实际方位角下钻孔开孔位置末端θ1’减去输入方位角θ1，即方位角偏移量△θ1：
Figure 489831DEST_PATH_IMAGE017
（7）
由上述激光测距组合定位误差测试方法中发现方位角误差主要来自关节转动，末端开孔位置会产生一个方位上的偏角Δθ1，即此在O0和O1之间引入虚关节，绕Z轴旋转角度Δθ1的齐次变换，变换矩阵如下：
Figure 604417DEST_PATH_IMAGE018
（8）
步骤六、将倾角和方位角的误差汇总的误差补偿模型为：
Figure 260658DEST_PATH_IMAGE019
（9）；
式（9）中，
Figure 511511DEST_PATH_IMAGE004
是相邻的第0个关节和第1个关节坐标系之间的齐次变换矩阵，
Figure 352428DEST_PATH_IMAGE005
是相 邻的第2个关节和第1个关节坐标系之间的齐次变换矩阵，
Figure 314830DEST_PATH_IMAGE006
是相邻的第3个关节和第2个关 节坐标系之间的齐次变换矩阵；Rot(z,Δθ1)是绕z轴的旋转Δθ1变换，Trans(Δz,0,0)是绕 x轴的移动Δz变换；Rot(y,Δθ2)是绕y轴的旋转Δθ2变换；
步骤七、利用RBF神经网络对总误差补偿矩阵进行逆解求解，得到的逆解即为误差补偿后的新倾角和方位角的控制输入量：由式（1）式（6）分别得到倾角和方位角误差变换量，利用式（9）求得末端开孔位姿，再由开孔位姿求倾角θ1和方位角θ2是求逆解问题，因此利用RBF神经网络法对式（9）的误差补偿矩阵进行逆解求解，得到的逆解即为误差补偿后的新倾角和方位角的控制量。

Claims (5)

1.一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，其特征在于，该钻孔机器人包括钻进车体、设在钻进车体上依次排布的动力头、自对中夹持器和扶正器；钻孔机器人钻臂定位误差检测通过误差检测装置实现；
该误差检测与补偿方法包括以下步骤：
步骤一、布设误差检测装置并调平钻进车体；所述误差检测装置包括靶板、激光测距传感器和高精度开孔定向仪；
步骤二、将高精度开孔定向仪放置在倾角和方位角回转中心位置，利用钻孔机器人自带钻臂调姿模块调节倾角和方位角，利用高精度开孔定向仪反馈信号找到倾角和方位角的零位，读取激光测距传感器测得的距离，记作初始距离；
步骤三、再通过钻孔机器人钻臂调姿模块多次改变倾角和方位角后，即有多组倾角和方位角的输入值，通过激光测距传感器对应测得多组距离值以计算倾角和方位角实际计算值；
步骤四、激光测距组合法测倾角误差计算模型：
Figure QLYQS_1
（1）
上式中，Lc1是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，Ld1是激光测距传感器安装点在Y方向与转动中心O1的距离，Lz1是倾角变化后测距传感器测定的距离，钻臂平移油缸平移距离为Ly；
步骤五、激光测距组合法测方位角误差计算模型：
Figure QLYQS_2
（6）
上式中，激光测距传感器安装点在x方向与方位角水平线有h距离，△l是因方位角变化h增加段的距离，Lc2是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，Ld2是激光测距传感器安装点在x方向与方位角转动中心O的距离，Lz2是方位角变化后测距传感器测定的距离，平移油缸平移距离为Ly；
步骤六：钻臂总误差补偿模型为：
Figure QLYQS_3
（9）；
式（9）中，
Figure QLYQS_4
是相邻的第0个关节和第1个关节坐标系之间的齐次变换矩阵；
Figure QLYQS_5
是相邻的第2个关节和第1个关节坐标系之间的齐次变换矩阵；
Figure QLYQS_6
是相邻的第3个关节和第2个关节坐标系之间的齐次变换矩阵；
Rot(z,Δθ1)是绕z轴的旋转Δθ1变换，Trans(Δz,0,0)是绕x轴的移动Δz变换，Rot(y,Δθ2)是绕y轴的旋转Δθ2变换；
步骤七、利用RBF神经网络对总误差补偿矩阵进行逆解求解，得到的逆解即为误差补偿后的新倾角和方位角的控制输入量；
所述步骤一包括：在钻臂钻孔位置前方放置一个表面平整且靶板面平行于车体的测试靶板，靶板能接收到并反射激光信号；在动力头后部，安装一个高精度激光测距传感器，确保其发射的激光能穿过动力头内孔、钻杆中心通孔、两个夹持器和扶正器中心，投射到靶板上，且靶板反射的激光也能投射到激光测距传感器的激光接收窗口上，将数据采集板与激光测距传感器信号连接，可实时获取激光测距传感器的距离值；将高精度开孔定向仪与数据采集板信号连接，高精度开孔定向仪可测试钻臂倾角和方位角；布设好误差检测装置后，利用钻孔机器人自带调平模块，对钻臂所在车体进行调平，再用高精度开孔定向仪在车体四个角进行复测，确保车体已经调平。
2.如权利要求1所述的煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，其特征在于，所述步骤四中，由实际倾角θ2’可通过式（2）计算开孔位置x、y、z的具体坐标值，其中Lc1，Ly，Lz1，Ld1都是已知量，H0是钻臂举升初始距离，举升高度为△H1，当钻臂距离靶板某一位置时，实际倾角下钻孔开孔位置末端z坐标减去理论倾角z坐标值，就是倾角误差在高度方向的偏移量；
Figure QLYQS_7
（2）
当倾角改变时，末端开孔位置会产生一个方位上的偏角Δθ2，同时还会产生x轴的偏移量Δz，因此在引入虚关节，增加一个坐标变换矩阵补偿倾角关节引起的误差，对倾角的位姿误差进行补偿得式（3），引入虚拟关节后的倾角误差模型：
Figure QLYQS_8
（3）
式（3）中，
Figure QLYQS_9
是相邻的第2个关节和第1个关节坐标系之间的齐次变换矩阵；
Trans(Δz,0,0)是绕x轴的移动Δz变换；Rot(y,Δθ2)是绕y轴的旋转Δθ2变换。
3.如权利要求2所述的煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，其特征在于，所述步骤五中，由实际方位角θ1’可通过式（7）计算开孔位置x、y、z的具体坐标值，其中Lc2，Ld2，Ly，Lz2，h，都是已知量，当钻机钻臂距离煤臂某一位置时，实际倾角下钻孔开孔位置末端x坐标减去理论倾角x坐标值，就是误差在x方向的偏移量△x，实际方位角下钻孔开孔位置末端θ1’减去输入方位角θ1，即方位角偏移量△θ1：
Figure QLYQS_10
（7）
方位角误差主要来自关节转动，末端开孔位置会产生一个方位上的偏角Δθ1，即此引入虚关节，绕Z轴旋转角度Δθ1的齐次变换，变换矩阵如下：
Figure QLYQS_11
（8）。
4.如权利要求3所述的煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，其特征在于，将步骤四和步骤五中倾角和方位角的误差补偿模型汇总得到步骤六中的钻臂总误差补偿模型。
5.如权利要求1所述的煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，其特征在于，所述步骤七包括：由式（1）和式（6）分别得到倾角和方位角误差变换量，利用式（9）求得末端开孔位姿，再由开孔位姿求倾角θ1和方位角θ2是求逆解问题，因此利用RBF神经网络法对式（9）的误差补偿矩阵进行逆解求解，得到的逆解即为误差补偿后的新倾角和方位角的控制量。
CN202211561133.7A 2022-12-07 2022-12-07 一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法 Active CN115556116B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211561133.7A CN115556116B (zh) 2022-12-07 2022-12-07 一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211561133.7A CN115556116B (zh) 2022-12-07 2022-12-07 一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法

Publications (2)

Publication Number Publication Date
CN115556116A CN115556116A (zh) 2023-01-03
CN115556116B true CN115556116B (zh) 2023-03-10

Family

ID=84770413

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211561133.7A Active CN115556116B (zh) 2022-12-07 2022-12-07 一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法

Country Status (1)

Country Link
CN (1) CN115556116B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111546334A (zh) * 2020-04-28 2020-08-18 中国科学院沈阳自动化研究所 一种减小轮廓误差的工业机器人在线位姿误差补偿方法
CN112068543A (zh) * 2020-07-16 2020-12-11 西安科技大学 基于视觉校准的煤矿钻锚机器人钻孔精确定位方法
CN114161425A (zh) * 2021-12-28 2022-03-11 中国人民解放军战略支援部队信息工程大学 一种工业机器人的误差补偿方法
CN115042189A (zh) * 2022-07-22 2022-09-13 西安理工大学 机械臂参数误差识别及补偿方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100345150B1 (ko) * 2000-09-29 2002-07-24 현대자동차주식회사 레이저 측정기를 이용한 로봇 위치 보정 방법
EP2698498A1 (en) * 2012-08-17 2014-02-19 Sandvik Mining and Construction Oy Method, rock drilling rig and control apparatus
CN108301766B (zh) * 2018-02-08 2019-04-02 北京科技大学 一种钻孔自动定位凿岩台车的双钻臂车体定位方法
CN108839027B (zh) * 2018-08-31 2020-12-25 河南工程学院 基于激光测距传感器的机器人自动对准控制方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111546334A (zh) * 2020-04-28 2020-08-18 中国科学院沈阳自动化研究所 一种减小轮廓误差的工业机器人在线位姿误差补偿方法
CN112068543A (zh) * 2020-07-16 2020-12-11 西安科技大学 基于视觉校准的煤矿钻锚机器人钻孔精确定位方法
CN114161425A (zh) * 2021-12-28 2022-03-11 中国人民解放军战略支援部队信息工程大学 一种工业机器人的误差补偿方法
CN115042189A (zh) * 2022-07-22 2022-09-13 西安理工大学 机械臂参数误差识别及补偿方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
凿岩机器人末端定位误差补偿;贺继林等;《工程机械》;20200910(第09期);第55-60页 *

Also Published As

Publication number Publication date
CN115556116A (zh) 2023-01-03

Similar Documents

Publication Publication Date Title
US6095260A (en) System, arrangements and associated methods for tracking and/or guiding an underground boring tool
CN112378399B (zh) 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法
CN103556945A (zh) 一种高造斜率井眼轨迹控制方法
CN112049648B (zh) 一种顶管施工监测控制方法
CN107677207A (zh) 基于扩展卡尔曼滤波的激光测距传感器位置误差标定方法
CN111473803B (zh) 一种用于矿用激光靶的标定方法
JPS61254792A (ja) さく岩機のブ−ム位置決め装置
CN102749066A (zh) 一种电子激光靶及其测量装置
CN115556116B (zh) 一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法
JP3940619B2 (ja) トンネル掘削機の位置計測装置
CN112817019A (zh) 一种基于姿态信息的可控震源车工作点定位误差补偿方法
CN111197486A (zh) 一种基于机器视觉的顶管机导向方法
CN108592875B (zh) 一种新型接触式隧洞收敛监测系统收敛位移的计算方法
JPH01235793A (ja) トンネル掘進機の姿勢制御方法
CN111411939B (zh) 一种非开挖钻探系统的钻头深度的计算方法
CN202734811U (zh) 一种电子激光靶及其测量装置
CN115014304B (zh) 一种煤矿井下全站仪定位系统的建站、移站方法
JPH0336399B2 (zh)
CN215338359U (zh) 一种倾角传感器自动标定装置
CN112377097B (zh) 一种具备辅助钻孔定点放样功能的装置及其定点方法
CN114689012B (zh) 一种用于竖向掘进机姿态测量的垂直导向系统及导向方法
CN113865559B (zh) 用于超深竖井自动联系测量的方法
CN117075125B (zh) 一种基于激光测距仪的模型试验盾构机空间姿态的监测方法
CN115142855B (zh) 一种基于机器视觉的智能顶管导向方法及系统
JPH07113640A (ja) レーザ測距による掘削機の姿勢測定方法及びその装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant