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

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

【技术实现步骤摘要】
一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法


[0001]本专利技术属于煤矿井下钻孔机器人精确开孔定位
，具体涉及一种煤矿井下钻孔机器人钻臂激光测距组合定位误差检测与补偿方法。

技术介绍

[0002]煤矿井下钻孔施工中，开孔精度影响成孔质量，成孔质量又会直接影响瓦斯抽采、探放水、注浆加固和工程加固的效果，因此精确调姿不但有利于瓦斯抽采效果，更是实现精准钻孔、精准快速治理的基础和前提。
[0003]钻孔机器人钻臂因受加工误差、装配误差和零部件磨损等影响，无法达到精确定位，进而影响瓦斯抽采、探放水、防突、防冲和工程加固的效果。而现有钻机位姿调节借助开孔定向仪和人工测量等，与手动、电控调姿配合，操作繁琐，并需要反复校准才能达到所需开孔位置，对工人操作要求高、位姿调节耗时、费力。即使少数钻机具有自动位姿控制功能，但采用开环控制，存在精度低、定位误差大、误差无法补偿的问题，难于实现钻孔快速自动精确定位，成为制约全自动施工的关键因素。
[0004]已有钻臂定位误差补偿方法标定工作量大，补偿过程复杂，费时费力，一次标定不能解决多运动状态变化补偿，且补偿不具备实时性，无法适应钻进机器人孔群施工中对定位误差和施工效率的要求，因此煤矿井下钻孔自动精确定位是实现钻孔机器人自主施工孔群的难点，也是实现少人化、无人化的关键技术。

技术实现思路

[0005]针对现有技术中存在的不足，本专利技术的目的在于，提供一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，针对亟待解决的钻孔机器人钻臂精确开孔定位问题，分析钻臂误差产生原因，针对经典误差补偿方法非实时性，提出在线钻臂位姿误差检测方法并建立误差补偿模型，实现钻臂的精确定位。
[0006]为了解决上述技术问题，本专利技术采用如下技术方案予以实现：一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，该钻孔机器人包括钻进车体、设在钻进车体上依次排布的动力头、自对中夹持器和扶正器；钻孔机器人钻臂定位误差检测通过误差检测装置实现；该误差检测与补偿方法包括以下步骤：步骤一、布设误差检测装置并调平钻进车体；所述误差检测装置包括靶板、激光测距传感器和高精度开孔定向仪；步骤二、将高精度开孔定向仪放置在倾角和方位角回转中心位置，利用钻孔机器人自带钻臂调姿模块调节倾角和方位角，利用高精度开孔定向仪反馈信号找到倾角和方位角的零位，读取激光测距传感器测得的距离，记作初始距离；步骤三、再通过钻孔机器人钻臂调姿模块多次改变倾角和方位角后，即有多组倾角和方位角的输入值，通过激光测距传感器对应测得多组距离值以计算倾角和方位角实际
计算值；步骤四、激光测距组合法测倾角误差计算模型：（1）上式中，L
c1
是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，L
d1
是激光测距传感器安装点在Y方向与转动中心O1的距离，L
z1
是倾角变化后测距传感器测定的距离，钻臂平移油缸平移距离为L
y
；步骤五、激光测距组合法测方位角误差计算模型：（6）上式中，激光测距传感器安装点在x方向与方位角水平线有h距离，
△
l是因方位角变化h增加段的距离，L
c2
是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，L
d2
是激光测距传感器安装点在x方向与方位角转动中心O的距离，L
z2
是方位角变化后测距传感器测定的距离，平移油缸平移距离为L
y
；步骤六：钻臂总误差补偿模型为：（9）；式（9）中，是相邻的第0个关节和第1个关节坐标系之间的齐次变换矩阵，是相邻的第2个关节和第1个关节坐标系之间的齐次变换矩阵，是相邻的第3个关节和第2个关节坐标系之间的齐次变换矩阵；Rot(z,Δθ1)是绕z轴的旋转Δθ1变换，Trans(Δz,0,0)是绕x轴的移动Δz变换；Rot(y,Δθ2)是绕y轴的旋转Δθ2变换；步骤七、利用RBF神经网络对总误差补偿矩阵进行逆解求解，得到的逆解即为误差补偿后的新倾角和方位角的控制输入量。
[0007]本专利技术还包括如下技术特征：具体的，所述步骤一包括：在钻臂钻孔位置前方放置一个表面平整且靶板面平行于车体的测试靶板，靶板能接收到并反射激光信号；在动力头后部，安装一个高精度激光测距传感器，确保其发射的激光能穿过动力头内孔、钻杆中心通孔、两个夹持器和扶正器中心，投射到靶板上，且靶板反射的激光也能投射到激光测距传感器的激光接收窗口上，将数据采集板与激光测距传感器信号连接，可实时获取激光测距传感器的距离值；将高精度开孔定向仪与数据采集板信号连接，高精度开孔定向仪可测试钻臂倾角和方位角；布设好误差检测装置后，利用钻孔机器人自带调平模块，对钻臂所在车体进行调平，再用高精度开孔定向仪在车体四个角进行复测，确保车体已经调平。
[0008]具体的，所述步骤四中，由实际倾角θ2’
可通过式（2）计算开孔位置x、y、z的具体坐标值，其中L
c1
，L
y
，L
z1
，L
d1
都是已知量，H0是钻臂举升初始距离，举升高度为
△
H1，当钻臂距离靶板某一位置时，实际倾角下钻孔开孔位置末端z坐标减去理论倾角z坐标值，就是倾角误
差在高度方向的偏移量；（2）当倾角改变时，末端开孔位置会产生一个方位上的偏角Δθ2，同时还会产生x轴的偏移量Δz，因此在引入虚关节，增加一个坐标变换矩阵补偿倾角关节引起的误差，对倾角的位姿误差进行补偿得式（3），引入虚拟关节后的倾角误差模型：（3）式（3）中，是相邻的第2个关节和第1个关节坐标系之间的齐次变换矩阵；Trans(Δz,0,0)是绕x轴的移动Δz变换；Rot(y,Δθ2)是绕y轴的旋转Δθ2变换。
[0009]具体的，所述步骤五中，由实际方位角θ1’
可通过式（7）计算开孔位置x、y、z的具体坐标值，其中L
c2
，L
d2
，L
y
，L
z2
，h，都是已知量，当钻机钻臂距离煤臂某一位置时，实际倾角下钻孔开孔位置末端x坐标减去理论倾角x坐标值，就是误差在x方向的偏移量
△
x，实际方位角下钻孔开孔位置末端θ1’
减去输入方位角θ1，即方位角偏移量
△
θ1：（7）方位角误差主要来自关节转动，末端开孔位置会产生一个方位上的偏角Δθ1，即此引入虚关节，绕Z轴旋转角度Δθ1的齐次变换，变换矩阵如下：（8）。
[0010]具体的，将步骤四和步骤五中倾角和方位角的误差补偿模型汇总得到步骤六中的钻臂总误差补偿模型。
[0011]具体的，所述步骤七包括：由式（1）和式（6）分别得到倾角和方位角误差变换量，利用式（9）求得末端开孔位姿，再由开孔位姿求倾角θ1和方位角θ2是求逆解问题，因此利用RBF神经网络法对式（9）的误差补偿矩阵进行逆解求解，得到的逆解即为误差补偿后的新倾角和方位角的控制量。
[00本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，其特征在于，该钻孔机器人包括钻进车体、设在钻进车体上依次排布的动力头、自对中夹持器和扶正器；钻孔机器人钻臂定位误差检测通过误差检测装置实现；该误差检测与补偿方法包括以下步骤：步骤一、布设误差检测装置并调平钻进车体；所述误差检测装置包括靶板、激光测距传感器和高精度开孔定向仪；步骤二、将高精度开孔定向仪放置在倾角和方位角回转中心位置，利用钻孔机器人自带钻臂调姿模块调节倾角和方位角，利用高精度开孔定向仪反馈信号找到倾角和方位角的零位，读取激光测距传感器测得的距离，记作初始距离；步骤三、再通过钻孔机器人钻臂调姿模块多次改变倾角和方位角后，即有多组倾角和方位角的输入值，通过激光测距传感器对应测得多组距离值以计算倾角和方位角实际计算值；步骤四、激光测距组合法测倾角误差计算模型：（1）上式中，L
c1
是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，L
d1
是激光测距传感器安装点在Y方向与转动中心O1的距离，L
z1
是倾角变化后测距传感器测定的距离，钻臂平移油缸平移距离为L
y
；步骤五、激光测距组合法测方位角误差计算模型：（6）上式中，激光测距传感器安装点在x方向与方位角水平线有h距离，
△
l是因方位角变化h增加段的距离，L
c2
是钻臂在经过高精度开孔定位仪寻找的初始水平点激光测距测定的距离，L
d2
是激光测距传感器安装点在x方向与方位角转动中心O的距离，L
z2
是方位角变化后测距传感器测定的距离，平移油缸平移距离为L
y
；步骤六：钻臂总误差补偿模型为：（9）；式（9）中，是相邻的第0个关节和第1个关节坐标系之间的齐次变换矩阵，是相邻的第2个关节和第1个关节坐标系之间的齐次变换矩阵，是相邻的第3个关节和第2个关节坐标系之间的齐次变换矩阵；Rot(z,Δθ1)是绕z轴的旋转Δθ1变换，Trans(Δz,0,0)是绕x轴的移动Δz变换；Rot(y,Δθ2)是绕y轴的旋转Δθ2变换；步骤七、利用RBF神经网络对总误差补偿矩阵进行逆解求解，得到的逆解即为误差补偿后的新倾角和方位角的控制输入量。2.如权利要求1所述的煤矿井下钻孔机器人钻臂定位误差检测与补偿方法，其特征在于，所述步骤一包括：在钻臂钻孔位置前方放置一个表面平整且靶板面平行于车体的测试
靶板，靶板能接收到并反射激光信号；在动力头后部，安装一个高精度激光测距传感器，确保其发射的激光能穿过动力头内孔、钻杆中心通孔、两个夹持器和扶正器中心，投射到靶板上，且靶板反射的激光也能投射到激光测距传感器的激光接收窗口上，将数据采集板与激光测距传感器信号连...

【专利技术属性】
技术研发人员：梁春苗，姚宁平，王小波，徐勇智，董洪波，李坤，彭光宇，李渊，
申请(专利权)人：中煤科工西安研究院集团有限公司，
类型：发明
国别省市：