CN115030706A - 基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法 - Google Patents

基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法 Download PDF

Info

Publication number
CN115030706A
CN115030706A CN202210675702.4A CN202210675702A CN115030706A CN 115030706 A CN115030706 A CN 115030706A CN 202210675702 A CN202210675702 A CN 202210675702A CN 115030706 A CN115030706 A CN 115030706A
Authority
CN
China
Prior art keywords
drilling
drill
anchor
boom
anchoring
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.)
Pending
Application number
CN202210675702.4A
Other languages
English (en)
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.)
Xian University of Science and Technology
Original Assignee
Xian University of Science and Technology
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 Xian University of Science and Technology filed Critical Xian University of Science and Technology
Priority to CN202210675702.4A priority Critical patent/CN115030706A/zh
Publication of CN115030706A publication Critical patent/CN115030706A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • E21B44/00—Automatic control systems specially adapted for drilling operations, i.e. self-operating systems which function to carry out or modify a drilling operation without intervention of a human operator, e.g. computer-controlled drilling systems; Systems specially adapted for monitoring a plurality of drilling variables or conditions
    • 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
    • E21D—SHAFTS; TUNNELS; GALLERIES; LARGE UNDERGROUND CHAMBERS
    • E21D20/00—Setting anchoring-bolts
    • E21D20/003—Machines for drilling anchor holes and setting anchor bolts
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Mining & Mineral Resources (AREA)
  • Geology (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Data Mining & Analysis (AREA)
  • General Physics & Mathematics (AREA)
  • Geochemistry & Mineralogy (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Theoretical Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Fluid Mechanics (AREA)
  • Mathematical Physics (AREA)
  • Environmental & Geological Engineering (AREA)
  • Evolutionary Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Probability & Statistics with Applications (AREA)
  • Operations Research (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Mechanical Engineering (AREA)
  • Structural Engineering (AREA)
  • Earth Drilling (AREA)

Abstract

本发明涉及一种基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法，属于钻锚机器双钻臂协同控制技术领域，锚钻孔信息采集模块采集支护钢带上所有锚钻孔的图像，钻机位姿检测模块用于检测钻机初始空间位姿，防爆计算机构建钻臂运动学模型，分析运动学正解和逆解，求取钻臂运动空间，通过孔序规划策略完成左右钻臂锚钻作业任务分配及锚钻顺序，并基于五次多项式插值方法完成轨迹规划，钻机快速、平稳到达锚钻孔，完成锚固作业任务，依此循环，完成一条钢带上锚钻作业任务。本发明可有效提高双钻臂钻锚机器人锚固作业效率，避免左右钻臂工作过程发生干涉，避免安全事故，提升了锚固作业的安全性。

Description

基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法
技术领域
本发明涉及钻锚机器双钻臂协同控制技术领域，特别是涉及一种基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法。
背景技术
由于煤矿巷道支护工艺流程复杂且环境恶劣，主要依靠人工操作单体式锚杆钻机完成支护任务，导致劳动强度大，支护效率低，安全性差。近年来，多钻臂锚杆台车逐渐应用到巷道支护作业任务中，但是由于巷道空间狭小，工艺流程复杂，多钻臂工作过程发生干涉现象时有发生，支护效率并没有得到有效提高。因此，为提高巷道支护效率，改善巷道综掘工作面自动化程度，多钻臂协同控制系统及技术的研究意义重大。
发明内容
本发明的目的是提供一种基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法，以有效提高双钻臂钻锚机器人锚固作业效率。
为实现上述目的，本发明提供了如下方案：
一种基于锚固工艺的钻锚机器人双钻臂协同控制系统，所述协同控制系统包括：钻锚机器人、控制模块、防爆计算机、两个锚钻孔信息采集模块和两个钻机位姿检测模块；
钻锚机器人的两个钻臂的末端均设置有钻机；两个锚钻孔信息采集模块分别一一对应地设置在两个钻机上；两个钻机位姿检测模块分别一一对应地设置在两个钻臂上；
两个锚钻孔信息采集模块均与防爆计算机连接，两个所述锚钻孔信息采集模块用于采集支护钢带上所有锚钻孔的图像，并将所有锚钻孔的图像传输至防爆计算机；
两个钻机位姿检测模块均与防爆计算机连接，所述钻机位姿检测模块用于检测钻机初始空间位姿，并将钻机初始空间位姿传输至防爆计算机；
防爆计算机与控制模块连接，所述防爆计算机用于根据所有锚钻孔的图像确定支护钢带上每个锚钻孔的位置，并根据所有锚钻孔的位置分配两个钻臂的锚钻任务和锚钻顺序，进而根据锚钻任务、锚钻顺序和钻机初始空间位姿规划每个钻臂完成锚钻任务的轨迹，同时将每个钻臂的轨迹、锚钻任务和锚钻顺序下发至控制模块；
控制模块与锚钻机器人连接，所述控制模块用于根据每个钻臂的轨迹、锚钻任务和锚钻顺序控制两个钻臂执行锚钻作业，完成锚钻任务。
可选的，所述钻臂为六自由度机械臂；
所述锚钻孔信息采集模块为双目相机，双目相机安装在钻机前端，双目相机的光轴方向与钻臂轴线方向平行，且面向钻臂顶端方向。
可选的，所述钻机位姿检测模块包括：多个倾角传感器和多个拉绳传感器；
多个倾角传感器分别一一对应地设置在钻臂的多个旋转关节上，多个拉绳传感器分别一一对应地设置在多个移动关节上；
多个倾角传感器和多个拉绳传感器均与防爆计算机连接，所述倾角传感器用于测量旋转关节的转动角度，并将转动角度传输至防爆计算机；所述拉绳传感器用于测量移动关节的伸缩量，并将伸缩量传输至防爆计算机；
所述防爆计算机用于根据转动角度和伸缩量确定钻机空间位姿。
一种基于锚固工艺的钻锚机器人双钻臂协同控制方法，所述协同控制方法应用于前述的协同控制系统，所述协同控制方法包括：
构建钻臂运动学模型；
根据所述钻臂运动学模型，采用蒙特卡洛方法确定钻臂运动空间；
获取支护钢带上所有锚钻孔的图像，并识别所有锚钻孔的图像上每个锚钻孔的位置；
获取每个钻臂中各个关节的转动角度和伸缩量，并将钻臂各个关节的转动角度和伸缩量带入所述钻臂运动学模型，确定两个钻机的初始空间位姿；
根据所有锚钻孔的位置和钻臂运动空间，采用孔序规划策略分配两个钻臂的锚钻任务和锚钻顺序；
根据两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，基于所述钻臂运动学模型，利用钻臂运动学分析计算锚钻任务中每个钻臂的各个关节运动量；
根据锚钻孔的位置、两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，采用五次多项式插值方法规划每个钻臂完成锚钻任务的轨迹；
控制两个钻臂按照各自规划的轨迹和锚钻任务中每个钻臂的各个关节运动量执行锚钻作业，完成锚钻任务。
可选的，所述构建钻臂运动学模型，具体包括：
基于改进的DH坐标系构建方法建立钻臂各个关节坐标系，同时确定钻臂中各个关节的DH参数；
根据钻臂各个关节坐标系，建立钻臂中相邻连杆的坐标系转换关系为
Figure BDA0003694501400000031
其中，
Figure BDA0003694501400000032
表示连杆坐标系{i}和连杆坐标系{i-1}的变换矩阵，θi表示关节i的关节变量，αi-1表示关节i-1与关节i之间的连杆扭转角，ai-1表示关节i-1与关节i之间的连杆长度，di表示关节i-1与关节i之间的连杆偏移，i＝1,2,3,4,5,6；
根据钻臂中相邻连杆的坐标系转换关系和钻臂中各个关节的DH参数，确定钻臂运动学模型为
Figure BDA0003694501400000033
其中，
Figure BDA0003694501400000034
表示钻机坐标系相对于钻臂基坐标系的变换矩阵，(nx,ny,nz)、(ox,oy,oz)和(ax,ay,az)分别表示描述钻机相对于钻臂基坐标系的姿态的第一、第二、第三三维单位正交向量，(px,py,pz)表示描述钻机相对于钻臂基坐标系的三维位置；
nx＝cθ1c(θ2+θ4)sθ6+sθ1cθ5cθ6-cθ1s(θ2+θ4)sθ5cθ6
ny＝-sθ1s(θ2+θ4)sθ5cθ6-cθ1cθ5cθ6+sθ1c(θ2+θ4)sθ6
nz＝-c(θ2+θ4)sθ5cθ6-s(θ2+θ4)sθ6
ox＝cθ1s(θ2+θ4)sθ5sθ6-sθ1cθ5sθ6+cθ1c(θ2+θ4)cθ6
oy＝sθ1s(θ2+θ4)sθ5sθ6+cθ1cθ5sθ6+sθ1c(θ2+θ4)cθ6
oz＝c(θ2+θ4)sθ5sθ6-s(θ2+θ4)cθ6
ax＝cθ1s(θ2+θ4)cθ5+sθ1sθ5
ay＝sθ1s(θ2+θ4)cθ5-cθ1sθ5
az＝c(θ2+θ4)cθ5
px＝-sθ1d4+cθ1cθ2(1000+d3)
py＝cθ1d4+sθ1cθ2(1000+d3)
pz＝sθ2(1000+d3)
式中，c表示cos符号，s表示sin符号。
可选的，所述改进的DH坐标系构建方法，具体包括：
沿着关节i的方向为轴Zi的方向，沿公垂线的指向为Xi轴；
按照右手定则确定Yi轴的方向；
以关节i和i+1的交点或公垂线与关节i的交点作为连杆坐标系{i}的原点。
可选的，根据所述钻臂运动学模型，采用蒙特卡洛方法确定钻臂运动空间，具体包括：
预设钻臂各个关节变量的取值范围为[θimin,θimax]，并设定随机次数为N；其中，θimin表示最小关节变量，θimax表示最大关节变量；
初始化当前循环次数k＝1；
利用随机函数生成n个0-1的随机点，利用公式θi＝θimin+(θimax-θimin)*rand，计算各个关节变量的随机值；其中，rand表示随机点的数值；
根据各个关节变量的随机值，利用所述钻臂运动学模型，确定当前循环次数的钻机末端空间坐标；
绘制当前循环次数的钻机末端空间坐标的空间点；
判断k是否大于或等于N，获得第一判断结果；
若所述第一判断结果表示否，则令k的数值增加1，并返回步骤“利用随机函数生成n个0-1的随机点，利用公式θi＝θimin+(θimax-θimin)*rand，计算各个关节变量的随机值”；
若所述第一判断结果表示是，则将所有空间点构成钻臂运动空间。
可选的，所述根据所有锚钻孔的位置和钻臂运动空间，采用孔序规划策略分配两个钻臂的锚钻任务和锚钻顺序，具体包括：
根据巷道空间、所有锚钻孔的位置和钻臂运动空间，构建双钻臂协同控制模型；所述锚钻孔包括顶板锚固点、左侧帮锚固点和右侧帮锚固点；
根据双钻臂协同控制模型进行顶板支护时，当顶板锚固点为2j+1个时，判断钻机工作半径R是否大于相邻锚固点之间距离d0，获得第二判断结果；其中，j为整数；
若第二判断结果表示否，则左侧钻臂从锚固点j开始向锚固点j-1方向完成锚固作业，同时右侧钻臂从锚固点j+2开始向锚固点j+3方向完成锚固作业，最后由左侧钻臂完成顶板锚固点j+1的锚固作业任务；
若第二判断结果表示是，则左侧钻臂从锚固点j开始向锚固点j-1方向完成锚固作业，同时右侧钻臂从锚固点J开始向锚固点j+1方向完成锚固作业，之后继续从锚固点J+1向锚固点2j+1完成顶板锚固作业任务；其中，锚固点J到锚固点j的距离大于或等于2R；
根据双钻臂协同控制模型进行顶板支护时，当顶板锚固点为2j个时，判断钻机工作半径R是否大于d0/2，获得第三判断结果；
若第三判断结果表示否，则左侧钻臂从锚固点j开始向锚固点j-1方向完成锚固作业，同时右侧钻臂从锚固点j+1开始向锚固点j+2方向完成锚固作业；
若第三判断结果表示是，则左侧钻臂从锚固点j开始向锚固点j-1方向完成锚固作业，同时右侧钻臂从锚固点J开始向锚固点j+1方向完成锚固作业，之后继续从锚固点J+1向锚固点2j完成顶板锚固作业任务；其中，锚固点J到锚固点j的距离大于或等于2R；
顶板支护完成后，控制左侧钻臂单独完成左侧帮锚固作业任务，右侧钻臂单独完成右侧帮锚固作业任务。
可选的，所述根据两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，基于所述钻臂运动学模型，利用钻臂运动学分析计算锚钻任务中每个钻臂的各个关节运动量，具体包括：
变换所述钻臂运动学模型，获得钻臂各个关节的关节运动量计算公式为：
Figure BDA0003694501400000061
Figure BDA0003694501400000062
Figure BDA0003694501400000063
θ4＝arctan2(b,a)
Figure BDA0003694501400000064
θ6＝arctan2(s1ox+c1oy,-(s1nx+c1ny))
其中，s1表示sinθ1，c1表示cosθ1，b、a分别表示第一、第二参数，a＝s2c2ax+s2s1ay+c2az，b＝c2c1ax+c2s1ay-s2az，s2表示sinθ2，c2表示cosθ2；
根据两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，利用钻臂各个关节的关节运动量计算公式，计算每个钻臂到达各自分配的锚钻孔位置时的各个关节运动量。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明公开一种基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法，锚钻孔信息采集模块采集支护钢带上所有锚钻孔的图像，钻机位姿检测模块用于检测钻机初始空间位姿，防爆计算机构建钻臂运动学模型，分析运动学正解和逆解，求取钻臂运动空间，通过孔序规划策略完成左右钻臂锚钻作业任务分配及锚钻顺序，并基于五次多项式插值方法完成轨迹规划，钻机快速、平稳到达锚钻孔，完成锚固作业任务，依此循环，完成一条钢带上锚钻作业任务。本发明可有效提高双钻臂钻锚机器人锚固作业效率，避免左右钻臂工作过程发生干涉，避免安全事故，提升了锚固作业的安全性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的基于锚固工艺的钻锚机器人双钻臂协同控制系统的信息传输原理图；
图2为本发明实施例提供的基于锚固工艺的钻锚机器人双钻臂协同控制系统的结构图；
图3为本发明实施例提供的基于锚固工艺的钻锚机器人双钻臂协同控制方法的流程图；
图4为本发明实施例提供的基于锚固工艺的钻锚机器人双钻臂协同控制方法的原理图；
图5为本发明实施例提供的基于改进DH方法的机械臂运动坐标系的示意图；
图6为本发明实施例提供的蒙特卡洛方法的原理示意图；
图7为本发明实施例提供的双钻臂协同控制数学模型的示意图；
图8为本发明实施例提供的顶板锚固点为2i+1个时的孔序规划示意图；
图9为本发明实施例提供的顶板锚固点为2i个时的孔序规划示意图。
符号说明：1-控制模块，2-钻锚机器人，3-钻臂，4-双目相机，5-顶板钢带，6-锚钻孔。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法，以有效提高双钻臂钻锚机器人锚固作业效率。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
为了解决现有多钻臂锚杆钻车在狭小空间锚固作业过程中易干涉、效率低和安全性差的问题，本发明提供了一种基于锚固工艺的钻锚机器人双钻臂协同控制系统，如图1所示，协同控制系统包括：钻锚机器人2、控制模块1、防爆计算机、两个锚钻孔信息采集模块和两个钻机位姿检测模块。
钻锚机器人2的两个钻臂3的末端均设置有钻机；两个锚钻孔信息采集模块分别一一对应地设置在两个钻机上；两个钻机位姿检测模块分别一一对应地设置在两个钻臂3上。两个锚钻孔信息采集模块通过以太网均与防爆计算机连接，两个锚钻孔信息采集模块用于采集支护钢带上所有锚钻孔6的图像，并将所有锚钻孔6的图像传输至防爆计算机。两个钻机位姿检测模块均与防爆计算机连接，钻机位姿检测模块用于检测钻机初始空间位姿，并将钻机初始空间位姿传输至防爆计算机。
防爆计算机与控制模块1连接，防爆计算机用于根据所有锚钻孔6的图像确定支护钢带上每个锚钻孔6的位置，并根据所有锚钻孔6的位置分配两个钻臂3的锚钻任务和锚钻顺序，进而根据锚钻任务、锚钻顺序和钻机初始空间位姿规划每个钻臂3完成锚钻任务的轨迹，同时将每个钻臂3的轨迹、锚钻任务和锚钻顺序下发至控制模块1。控制模块1与锚钻机器人连接，控制模块1用于根据每个钻臂3的轨迹、锚钻任务和锚钻顺序控制两个钻臂3执行锚钻作业，完成锚钻任务。
其中，防爆计算机接收锚钻孔6信息获取模块采集图像信息并处理，获取一条钢带上所有锚钻孔6空间坐标信息及相对位置关系等信息；通过孔序规划策略完成任务分配，确定左右钻臂3相应的锚钻任务及锚钻顺序；基于五次多项式完成钻臂轨迹规划，确定关节变量，下发控制信号至控制模块1。控制单元模块主要接收来自防爆计算机的控制信号，控制钻臂3各关节按照设定运动，快速、稳定到达锚钻作业任务点，完成锚钻作业任务。孔序规划为根据锚固工艺要求(先顶板后侧帮，从中间向两边)、锚钻孔空间信息、左右钻臂运动空间等，对锚钻任务进行分配并确定锚钻孔作业顺序，避免左右钻臂3无序工作，发生干涉。轨迹规划为根据钻臂运动学模型及锚钻孔目标位置，求解钻臂3各个关节变化量，在传感器检测钻机起始位置的基础上，基于五次多项式插值方法完成轨迹规划，使钻机平稳、快速到达目标锚钻孔位置，实现钻锚作业任务。钻机完成锚钻孔作业任务，根据孔序规划结果，进行下一锚钻孔轨迹规划并运动控制，循环往复直至完成该钻臂锚钻作业任务。
参照图2，支护钢带上设置有若干圆形孔洞(锚钻孔6)，通过螺栓将锚网、钢带和顶板锚固在一起，钢带放置在钻锚机器人2临时支护装置上，顶在巷道顶板。
在一个示例中，钻锚机器人2由悬臂式掘进机集成两个六自由度机械臂组合而成，两个六自由度机械臂对称分布在钻锚机器人2机身两侧，钻机作为钻臂3的末端执行器用于完成巷道顶板和侧帮锚固作业任务。
锚钻孔信息采集模块为双目相机4，双目相机4安装在钻机前端，双目相机4的光轴方向与钻臂3轴线方向平行，且面向钻臂3顶端方向。双目相机4采集的一条钢带上所有锚钻孔6图像信息通过以太网传输至防爆计算机。若防爆计算机得到锚钻孔6个数小于钢带上固有锚钻孔6，则调整钻臂3位置，重新采集钢带图像并传输至防爆计算机处理获取锚钻孔6空间位置等相关信息，直至得到钢带上固有锚钻孔6个数的相关信息，结束信息采集。
钻机位姿检测模块包括：多个倾角传感器和多个拉绳传感器。多个倾角传感器分别一一对应地设置在钻臂3的多个旋转关节上，多个拉绳传感器分别一一对应地设置在多个移动关节上。多个倾角传感器和多个拉绳传感器均与防爆计算机连接，倾角传感器用于测量旋转关节的转动角度，并将转动角度传输至防爆计算机。拉绳传感器用于测量移动关节的伸缩量，并将伸缩量传输至防爆计算机；防爆计算机用于根据转动角度和伸缩量确定钻机空间位姿。
系统工作时，钻锚机器人钻臂3按照设定靠近钢带，双目视觉系统(双目相机4)采集顶板钢带图像并传输至防爆计算机，实时解算钢带上锚钻孔圆心空间坐标，直至获取一条钢带上所有锚钻孔空间坐标及相对位置等信息；基于孔序规划策略完成锚钻任务分配，钻臂位姿检测模块实现钻机初始位姿检测，根据锚钻作业任务确定钻机目标位姿，基于五次多项式完成轨迹规划，防爆计算机将此信息下发至控制单元模块，该模块下发控制指令控制钻臂3运动，使钻机运动至锚钻孔6下方，实现锚钻作业任务。
本发明实施例还提供了一种基于锚固工艺的钻锚机器人双钻臂协同控制方法，如图3-4所示，协同控制方法应用于前述的协同控制系统，协同控制方法包括：
步骤S1，构建钻臂运动学模型。
根据钻锚机器人结构和参数完成运动学建模，分析运动学正解；
具体的，根据钻锚机器人钻臂结构和参数，依据改进的DH坐标系构建法则对此六自由度钻臂进行分析，首先找出六个关节轴，沿着关节轴i的方向为轴Zi的方向，以关节轴i和i+1的交点或公垂线与关节轴i的交点作为连杆坐标系{i}的原点；规定Xi轴沿公垂线的指向，若关节轴i和i+1相交，则Xi轴垂直于关节轴i和i+1所在的平面；最后按照右手定则确定Yi轴的方向。基于此原则，建立钻臂各个关节坐标系，如图5所示，其中{0}坐标系与{1}坐标系重合，固定在基座上。
根据建立的DH坐标系，得到六自由度钻臂的DH参数表如表1所示，其中ai-1为连杆长度，
Figure BDA0003694501400000101
为连杆扭转角，转动关节θi关节变量，di为连杆偏移。
表1六自由度钻臂DH参数表
Figure BDA0003694501400000102
具体的，基于改进的DH坐标系构建方法建立的参数表及钻臂相应连杆的参数，通过相邻连杆的坐标系转换可得到钻臂末端执行器(钻机)相对于钻臂基坐标系的转换关系，即为钻臂正运动学模型，得到钻机目标位置相对钻臂基坐标系的空间位姿。坐标系{i}和坐标系{i-1}的转换关系可以用变换矩阵
Figure BDA0003694501400000111
表示：
Figure BDA0003694501400000112
因此，
Figure BDA0003694501400000113
的一般表达式为：
Figure BDA0003694501400000114
将钻锚机器人钻臂DH参数表及对应连杆参数代入上式，可得：
Figure BDA0003694501400000115
Figure BDA0003694501400000116
Figure BDA0003694501400000117
Figure BDA0003694501400000118
Figure BDA0003694501400000119
Figure BDA00036945014000001110
经解算，末端执行器(钻机)坐标系相对于基坐标系转换关系为：
Figure BDA0003694501400000121
其中：
nx＝cθ1c(θ2+θ4)sθ6+sθ1cθ5cθ6-cθ1s(θ2+θ4)sθ5cθ6
ox＝cθ1s(θ2+θ4)sθ5sθ6-sθ1cθ5sθ6+cθ1c(θ2+θ4)cθ6
ax＝cθ1s(θ2+θ4)cθ5+sθ1sθ5
px＝-sθ1d4+cθ1cθ2(1000+d3)
ny＝-sθ1s(θ2+θ4)sθ5cθ6-cθ1cθ5cθ6+sθ1c(θ2+θ4)sθ6
oy＝sθ1s(θ2+θ4)sθ5sθ6+cθ1cθ5sθ6+sθ1c(θ2+θ4)cθ6
ay＝sθ1s(θ2+θ4)cθ5-cθ1sθ5
py＝cθ1d4+sθ1cθ2(1000+d3)
nz＝-c(θ2+θ4)sθ5cθ6-s(θ2+θ4)sθ6
oz＝c(θ2+θ4)sθ5sθ6-s(θ2+θ4)cθ6
az＝c(θ2+θ4)cθ5
pz＝sθ2(1000+d3)
式中，ci、si分别表示cosθi和sinθi，单位正交向量n、o和a描述了钻臂末端执行器(钻机)相对于钻臂基坐标系的姿态，p描述了钻臂末端执行器(钻机)相对于钻臂基坐标系的位置，θ1、θ2、d3、θ4、θ5、θ6分别表示钻臂五个旋转变量和一个平移变量。
步骤S2，根据钻臂运动学模型，采用蒙特卡洛方法确定钻臂运动空间。
具体的，工作空间是指机械臂末端执行器能到达空间点的集合，即为钻机所能到达的空间位置。本案基于蒙特卡洛方法利用matlab机器人工具箱解算该掘锚机器人双钻臂运动空间，随机选取大量的采样点，尽可能构建出钻臂完整的运动空间。如图6所示为蒙特卡洛方法求解钻臂运动空间方案，其实现步骤为：
(1)首先构建钻臂运动模型，机械臂各个关节变量θi(di)取值范围分别为[θimin,θimax]，设定随机次数为N；
(2)利用随机函数rand生成n个0-1的随机点，以(θimax-θimin)*rand为随机步长，求解各个关节变量的随机值，即
θi＝θimin+(θimax-θimin)*rand (4)
(3)将式(4)所得关节变量随机值代入式(3)即可求得钻机末端空间坐标，并绘制该坐标的空间点；
(4)将步骤(2)和(3)重复N次，即可得到双钻臂运动空间轮廓，其中，N取值越大，运动空间轮廓越接近真实情况。
步骤S3，获取支护钢带上所有锚钻孔的图像，并识别所有锚钻孔的图像上每个锚钻孔的位置。
双目视觉系统采集钢带图像并传输至防爆计算机，防爆计算机完成钢带上锚钻孔的识别和锚钻孔空间位姿信息的获取，若获得空间位姿信息的锚钻孔个数少于钢带上锚钻孔个数，则调整钻机姿态(即调整双目视觉系统姿态)，重新采集图像并传输至防爆计算机进行处理，重复以上步骤，直至获取到钢带上所有锚钻孔的空间位姿信息及相对位置信息等。
基于前述的机械臂正运动学分析，将各个关节变量值带入式(3)，可得到钻臂末端执行器(钻机)相对于钻臂基坐标系的空间位姿，通过坐标系转换可得到其相对于钻锚机器人机身坐标系空间位姿。
步骤S4，获取每个钻臂中各个关节的转动角度和伸缩量，并将钻臂各个关节的转动角度和伸缩量带入钻臂运动学模型，确定两个钻机的初始空间位姿。
利用钻臂各个关节上布置的倾角传感器和拉绳传感器采集各关节转动角度和伸缩量。
步骤S5，根据所有锚钻孔的位置和钻臂运动空间，采用孔序规划策略分配两个钻臂的锚钻任务和锚钻顺序。
具体的，所述步骤S5中孔序规划策略主要包括确定锚钻孔在左右钻臂运动空间、任务分配、孔序规划。根据巷道空间构建双钻臂协同控制数学模型，如图7所示，XOY为煤矿巷道横截面绝对坐标系，Li、Ri分别表示左右侧帮第i个钻孔位置，Di表示巷道顶板第i个钻孔位置，两钻臂工作空间分别为SL和SR。钻臂在完成锚固作业任务时，可以分别独立完成各自工作空间范围内目标点的锚固作业，若锚固点为奇数，则中间锚固点最后由左侧钻臂完成锚固作业。因此为了高效快速完成锚固作业任务，钻机需要协同作业。由于钻机自身结构，为避免两钻臂作业过程产生干涉，相互影响，需要考虑两钻臂协同控制问题，避免钻臂干涉，提高锚固效率。由于两钻臂对称分布在掘锚机器人机身两侧，因此，两侧帮锚固作业任务分别由对应侧钻臂单独完成，协同控制模型主要针对顶板锚固作业任务，需要对左右钻臂完成顶板锚固作业点个数和作业顺序进行规划，使得两钻臂能够协同作业，高效完成巷道支护作业任务。
按照规范要求，巷道支护需要先完成顶板支护再进行侧帮支护，其中顶板支护要求从中间向两帮进行顺序施工。因此，本文所提掘锚机器人两钻臂在锚固作业时，在顶板中间位置锚固作业时最容易发生干涉，相互影响，随着顶板锚固作业任务从中间向两帮进行，两钻臂距离越来越远，不会发生干涉。因此，主要研究巷道顶板中间位置附近锚固点协同作业任务问题，通过左右钻臂锚固点欧氏距离判断是否可能发生干涉。
如图8-9所示为机械臂作业次序，主要研究中间位置锚固点作业次序及分配原则，图8表示顶板锚固点为奇数，图9表示顶板锚固点为偶数。假设相邻锚固点之间距离为d0，钻机工作半径为R，为保证两钻臂锚固作业过程不发生干涉，有：
(m+1)d0≥2R (5)
式中，m表示左右钻机锚钻位置中间的锚钻孔个数。
(1)顶板锚固点为2i+1个
当顶板锚固点为2i+1时，若2R≤2d0，即R≤d0，则左侧钻臂从锚固点i开始向锚固点i-1方向完成锚固作业，同时右侧钻臂从锚固点i+2开始向锚固点i+3方向完成锚固作业，最后由左侧钻臂完成顶板锚固点i+1的锚固作业任务；
若2d0≤2R≤3d0，即d0≤R≤3/2d0，则左侧钻臂从锚固点i开始向锚固点i-1方向完成锚固作业，同时右侧钻臂从锚固点i+3开始向锚固点i+1方向完成锚固作业，之后继续从锚固点i+4向锚固点2i+1完成顶板锚固作业任务；
若2R≥3d0，则依此类推完成顶板锚固作业任务。
(2)顶板锚固点为2i个
当顶板锚固点为2i时，若2R≤d0，即R≤1/2d0，则左侧钻臂从锚固点i开始向锚固点i-1方向完成锚固作业，同时右侧钻臂从锚固点i+1开始向锚固点i+2方向完成锚固作业；
若d0≤2R≤2d0，即1/2d0≤R≤d0，则左侧钻臂从锚固点i开始向锚固点i-1方向完成锚固作业，同时右侧钻臂从锚固点i+2开始向锚固点i+1方向完成锚固作业，之后继续从锚固点i+3向锚固点2i完成顶板锚固作业任务；
若2d0≤2R≤3d0，即d0≤R≤3/2d0，则左侧钻臂从锚固点i开始向锚固点i-1方向完成锚固作业，同时右侧钻臂从锚固点i+3开始向锚固点i+1方向完成锚固作业，之后继续从锚固点i+4向锚固点2i完成顶板锚固作业任务；
若2R≥3d0，则依此类推完成顶板锚固作业任务。
在掘进工作面进行巷道掘进时，锚索与锚杆支护会交错进行，对于锚索的工作任务，其策略与锚杆一致，服从锚杆的最优匹配策略。
步骤S6，根据两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，基于钻臂运动学模型，利用钻臂运动学分析计算锚钻任务中每个钻臂的各个关节运动量。
具体的，逆运动学分析与正运动学分析正好相反，主要实现在钻臂末端执行器在空间坐标系位姿已知条件下求解各个关节运动量。采用逆变换的方法求运动学逆解，已知钻臂末端执行器相对于钻臂基坐标系的转换矩阵为06T,矩阵中各个数值已知，即可求得钻臂各个关节要到达目标位置关节变量。
由机械臂正运动学可知，
Figure BDA0003694501400000161
将含有θ1的部分移到方程左边，则有：
Figure BDA0003694501400000162
其中：
Figure BDA0003694501400000163
令等式(4)左右两边元素(2,4)分别相等，则有：
-s1px+c1py＝d4 (8)
利用三角恒等变换对上式进行处理可得：
Figure BDA0003694501400000164
同理，令等式(4)两边元素(1,4)和(3,4)分别相等，则有：
c1px+s1py＝c2(1000+d3) (9)
-s1px+c1py＝d4 (10)
对上式进行整理可得：
Figure BDA0003694501400000165
同理，对式(8)利用三角恒等变换处理可得：
Figure BDA0003694501400000166
将等式(3)含有θ1、θ2和d3的部分移到方程左边，则有：
Figure BDA0003694501400000167
Figure BDA0003694501400000171
令等式(12)两边元素(2,3)分别相等，则有：
s1ax+c1ay＝s5 (13)
同理可得：若
Figure BDA0003694501400000172
则有
Figure BDA0003694501400000173
令等式(12)两边元素(1,3)和(3,3)分别相等，则有：
s2c1ax+s2s1ay+c1az＝c4c5 (15)
c2c1ax+c2s1ay-s2az＝s4c5 (16)
若
Figure BDA0003694501400000174
则有
θ4＝arctan2(b,a) (17)
其中，a＝s2c2ax+s2s1ay+c2az，b＝c2c1ax+c2s1ay-s2az。
令等式(12)两边元素(2,1)和(2,2)分别相等，则有：
s1nx+c1ny＝c5c6 (18)
s1ox+c1oy＝-c5c6 (19)
若
Figure BDA0003694501400000175
则有
θ6＝arctan2(s1ox+c1oy,-(s1nx+c1ny)) (20)
分析可得双钻臂到达运动空间位置时各个关节变化量，但是到达空间一点各个关节变化量可能会存在多组解。
步骤S7，根据锚钻孔的位置、两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，采用五次多项式插值方法规划每个钻臂完成锚钻任务的轨迹。
具体的，采用五次多项式插值方法进行钻臂轨迹规划，五次多项式插值方法解决了关节角速度变化不平滑且加速度存在跳变的情况。两钻臂对称布置在掘锚机器人机身两侧，因此，设定左右钻臂分别完成左右各部分顶板和侧帮锚固作业任务。本文以右侧钻臂为研究对象，研究钻臂轨迹规划问题。
设钻臂关节关于时间的运动函数为
s(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5 (21)
其角速度和角加速度函数表达式分别为：
Figure BDA0003694501400000183
s(t0)、s(tf)分别表示关节的初始位置和目标位置，因此对初始位置和目标位置、角速度和角加速度带入公式有：
Figure BDA0003694501400000181
将上式写成矩阵的形式，则有：
Figure BDA0003694501400000182
将上式记为AB＝P，则B＝A-1C，求解上式可得五次多项式各个系数。
步骤S8，控制两个钻臂按照各自规划的轨迹和锚钻任务中每个钻臂的各个关节运动量执行锚钻作业，完成锚钻任务。
依照规划顺序逐一重复该步骤，实现一排钢带上锚钻孔锚固作业任务。
与现有技术相比，本发明用于双臂钻锚机器人顶板锚固过程双臂协同控制。系统工作时，双目视觉系统采集钢带图像，获取钢带上所有锚钻孔位姿信息；构建钻臂运动学模型，分析运动学正解和逆解，求取钻臂运动空间；通过布置在钻臂上的倾角传感器和拉绳传感器采集钻臂初始位姿；通过孔序规划策略完成左右钻臂锚钻作业任务分配及锚钻顺序，基于五次多项式插值方法完成轨迹规划，钻机快速、平稳到达锚钻孔，完成锚固作业任务，依此循环，完成一条钢带上锚钻作业任务。本发明可有效提高双钻臂钻锚机器人锚固作业效率，避免左右钻臂工作过程发生干涉，避免安全事故，提升了锚固作业的安全性。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (9)

1.一种基于锚固工艺的钻锚机器人双钻臂协同控制系统，其特征在于，所述协同控制系统包括：钻锚机器人、控制模块、防爆计算机、两个锚钻孔信息采集模块和两个钻机位姿检测模块；
钻锚机器人的两个钻臂的末端均设置有钻机；两个锚钻孔信息采集模块分别一一对应地设置在两个钻机上；两个钻机位姿检测模块分别一一对应地设置在两个钻臂上；
两个锚钻孔信息采集模块均与防爆计算机连接，两个所述锚钻孔信息采集模块用于采集支护钢带上所有锚钻孔的图像，并将所有锚钻孔的图像传输至防爆计算机；
两个钻机位姿检测模块均与防爆计算机连接，所述钻机位姿检测模块用于检测钻机初始空间位姿，并将钻机初始空间位姿传输至防爆计算机；
防爆计算机与控制模块连接，所述防爆计算机用于根据所有锚钻孔的图像确定支护钢带上每个锚钻孔的位置，并根据所有锚钻孔的位置分配两个钻臂的锚钻任务和锚钻顺序，进而根据锚钻任务、锚钻顺序和钻机初始空间位姿规划每个钻臂完成锚钻任务的轨迹，同时将每个钻臂的轨迹、锚钻任务和锚钻顺序下发至控制模块；
控制模块与锚钻机器人连接，所述控制模块用于根据每个钻臂的轨迹、锚钻任务和锚钻顺序控制两个钻臂执行锚钻作业，完成锚钻任务。
2.根据权利要求1所述的基于锚固工艺的钻锚机器人双钻臂协同控制系统，其特征在于，所述钻臂为六自由度机械臂；
所述锚钻孔信息采集模块为双目相机，双目相机安装在钻机前端，双目相机的光轴方向与钻臂轴线方向平行，且面向钻臂顶端方向。
3.根据权利要求1所述的基于锚固工艺的钻锚机器人双钻臂协同控制系统，其特征在于，所述钻机位姿检测模块包括：多个倾角传感器和多个拉绳传感器；
多个倾角传感器分别一一对应地设置在钻臂的多个旋转关节上，多个拉绳传感器分别一一对应地设置在多个移动关节上；
多个倾角传感器和多个拉绳传感器均与防爆计算机连接，所述倾角传感器用于测量旋转关节的转动角度，并将转动角度传输至防爆计算机；所述拉绳传感器用于测量移动关节的伸缩量，并将伸缩量传输至防爆计算机；
所述防爆计算机用于根据转动角度和伸缩量确定钻机空间位姿。
4.一种基于锚固工艺的钻锚机器人双钻臂协同控制方法，其特征在于，所述协同控制方法应用于权利要求1-3任一项所述的协同控制系统，所述协同控制方法包括：
构建钻臂运动学模型；
根据所述钻臂运动学模型，采用蒙特卡洛方法确定钻臂运动空间；
获取支护钢带上所有锚钻孔的图像，并识别所有锚钻孔的图像上每个锚钻孔的位置；
获取每个钻臂中各个关节的转动角度和伸缩量，并将钻臂各个关节的转动角度和伸缩量带入所述钻臂运动学模型，确定两个钻机的初始空间位姿；
根据所有锚钻孔的位置和钻臂运动空间，采用孔序规划策略分配两个钻臂的锚钻任务和锚钻顺序；
根据两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，基于所述钻臂运动学模型，利用钻臂运动学分析计算锚钻任务中每个钻臂的各个关节运动量；
根据锚钻孔的位置、两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，采用五次多项式插值方法规划每个钻臂完成锚钻任务的轨迹；
控制两个钻臂按照各自规划的轨迹和锚钻任务中每个钻臂的各个关节运动量执行锚钻作业，完成锚钻任务。
5.根据权利要求4所述的基于锚固工艺的钻锚机器人双钻臂协同控制方法，其特征在于，所述构建钻臂运动学模型，具体包括：
基于改进的DH坐标系构建方法建立钻臂各个关节坐标系，同时确定钻臂中各个关节的DH参数；
根据钻臂各个关节坐标系，建立钻臂中相邻连杆的坐标系转换关系为
Figure FDA0003694501390000031
其中，
Figure FDA0003694501390000032
表示连杆坐标系{i}和连杆坐标系{i-1}的变换矩阵，θi表示关节i的关节变量，αi-1表示关节i-1与关节i之间的连杆扭转角，ai-1表示关节i-1与关节i之间的连杆长度，di表示关节i-1与关节i之间的连杆偏移，i＝1,2,3,4,5,6；
根据钻臂中相邻连杆的坐标系转换关系和钻臂中各个关节的DH参数，确定钻臂运动学模型为
Figure FDA0003694501390000033
其中，
Figure FDA0003694501390000034
表示钻机坐标系相对于钻臂基坐标系的变换矩阵，(nx,ny,nz)、(ox,oy,oz)和(ax,ay,az)分别表示描述钻机相对于钻臂基坐标系的姿态的第一、第二、第三三维单位正交向量，(px,py,pz)表示描述钻机相对于钻臂基坐标系的三维位置；
nx＝cθ1c(θ2+θ4)sθ6+sθ1cθ5cθ6-cθ1s(θ2+θ4)sθ5cθ6
ny＝-sθ1s(θ2+θ4)sθ5cθ6-cθ1cθ5cθ6+sθ1c(θ2+θ4)sθ6
nz＝-c(θ2+θ4)sθ5cθ6-s(θ2+θ4)sθ6
ox＝cθ1s(θ2+θ4)sθ5sθ6-sθ1cθ5sθ6+cθ1c(θ2+θ4)cθ6
oy＝sθ1s(θ2+θ4)sθ5sθ6+cθ1cθ5sθ6+sθ1c(θ2+θ4)cθ6
oz＝c(θ2+θ4)sθ5sθ6-s(θ2+θ4)cθ6
ax＝cθ1s(θ2+θ4)cθ5+sθ1sθ5
ay＝sθ1s(θ2+θ4)cθ5-cθ1sθ5
az＝c(θ2+θ4)cθ5
px＝-sθ1d4+cθ1cθ2(1000+d3)
py＝cθ1d4+sθ1cθ2(1000+d3)
pz＝sθ2(1000+d3)
式中，c表示cos符号，s表示sin符号。
6.根据权利要求5所述的基于锚固工艺的钻锚机器人双钻臂协同控制方法，其特征在于，所述改进的DH坐标系构建方法，具体包括：
沿着关节i的方向为轴Zi的方向，沿公垂线的指向为Xi轴；
按照右手定则确定Yi轴的方向；
以关节i和i+1的交点或公垂线与关节i的交点作为连杆坐标系{i}的原点。
7.根据权利要求5所述的基于锚固工艺的钻锚机器人双钻臂协同控制方法，其特征在于，根据所述钻臂运动学模型，采用蒙特卡洛方法确定钻臂运动空间，具体包括：
预设钻臂各个关节变量的取值范围为[θimin,θimax]，并设定随机次数为N；其中，θimin表示最小关节变量，θimax表示最大关节变量；
初始化当前循环次数k＝1；
利用随机函数生成n个0-1的随机点，利用公式θi＝θimin+(θimax-θimin)*rand，计算各个关节变量的随机值；其中，rand表示随机点的数值；
根据各个关节变量的随机值，利用所述钻臂运动学模型，确定当前循环次数的钻机末端空间坐标；
绘制当前循环次数的钻机末端空间坐标的空间点；
判断k是否大于或等于N，获得第一判断结果；
若所述第一判断结果表示否，则令k的数值增加1，并返回步骤“利用随机函数生成n个0-1的随机点，利用公式θi＝θimin+(θimax-θimin)*rand，计算各个关节变量的随机值”；
若所述第一判断结果表示是，则将所有空间点构成钻臂运动空间。
8.根据权利要求4所述的基于锚固工艺的钻锚机器人双钻臂协同控制方法，其特征在于，所述根据所有锚钻孔的位置和钻臂运动空间，采用孔序规划策略分配两个钻臂的锚钻任务和锚钻顺序，具体包括：
根据巷道空间、所有锚钻孔的位置和钻臂运动空间，构建双钻臂协同控制模型；所述锚钻孔包括顶板锚固点、左侧帮锚固点和右侧帮锚固点；
根据双钻臂协同控制模型进行顶板支护时，当顶板锚固点为2j+1个时，判断钻机工作半径R是否大于相邻锚固点之间距离d0，获得第二判断结果；其中，j为整数；
若第二判断结果表示否，则左侧钻臂从锚固点j开始向锚固点j-1方向完成锚固作业，同时右侧钻臂从锚固点j+2开始向锚固点j+3方向完成锚固作业，最后由左侧钻臂完成顶板锚固点j+1的锚固作业任务；
若第二判断结果表示是，则左侧钻臂从锚固点j开始向锚固点j-1方向完成锚固作业，同时右侧钻臂从锚固点J开始向锚固点j+1方向完成锚固作业，之后继续从锚固点J+1向锚固点2j+1完成顶板锚固作业任务；其中，锚固点J到锚固点j的距离大于或等于2R；
根据双钻臂协同控制模型进行顶板支护时，当顶板锚固点为2j个时，判断钻机工作半径R是否大于d0/2，获得第三判断结果；
若第三判断结果表示否，则左侧钻臂从锚固点j开始向锚固点j-1方向完成锚固作业，同时右侧钻臂从锚固点j+1开始向锚固点j+2方向完成锚固作业；
若第三判断结果表示是，则左侧钻臂从锚固点j开始向锚固点j-1方向完成锚固作业，同时右侧钻臂从锚固点J开始向锚固点j+1方向完成锚固作业，之后继续从锚固点J+1向锚固点2j完成顶板锚固作业任务；其中，锚固点J到锚固点j的距离大于或等于2R；
顶板支护完成后，控制左侧钻臂单独完成左侧帮锚固作业任务，右侧钻臂单独完成右侧帮锚固作业任务。
9.根据权利要求5所述的基于锚固工艺的钻锚机器人双钻臂协同控制方法，其特征在于，所述根据两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，基于所述钻臂运动学模型，利用钻臂运动学分析计算锚钻任务中每个钻臂的各个关节运动量，具体包括：
变换所述钻臂运动学模型，获得钻臂各个关节的关节运动量计算公式为：
Figure FDA0003694501390000051
Figure FDA0003694501390000061
Figure FDA0003694501390000062
θ4＝arctan2(b,a)
Figure FDA0003694501390000063
θ6＝arctan2(s1ox+c1oy,-(s1nx+c1ny))
其中，s1表示sinθ1，c1表示cosθ1，b、a分别表示第一、第二参数，a＝s2c2ax+s2s1ay+c2az，b＝c2c1ax+c2s1ay-s2az，s2表示sinθ2，c2表示cosθ2；
根据两个钻臂的锚钻任务、锚钻顺序和两个钻机的初始空间位姿，利用钻臂各个关节的关节运动量计算公式，计算每个钻臂到达各自分配的锚钻孔位置时的各个关节运动量。
CN202210675702.4A 2022-06-15 2022-06-15 基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法 Pending CN115030706A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210675702.4A CN115030706A (zh) 2022-06-15 2022-06-15 基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210675702.4A CN115030706A (zh) 2022-06-15 2022-06-15 基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法

Publications (1)

Publication Number Publication Date
CN115030706A true CN115030706A (zh) 2022-09-09

Family

ID=83125560

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210675702.4A Pending CN115030706A (zh) 2022-06-15 2022-06-15 基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法

Country Status (1)

Country Link
CN (1) CN115030706A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115807685A (zh) * 2023-01-20 2023-03-17 太原理工大学 矿用锚护装备钻锚作业的自动控制系统

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115807685A (zh) * 2023-01-20 2023-03-17 太原理工大学 矿用锚护装备钻锚作业的自动控制系统

Similar Documents

Publication Publication Date Title
CN108286949B (zh) 一种可移动式三维检测机器人系统
CN110815180B (zh) 六自由度并联机器人运动分析建模及快速求解方法
US5781705A (en) Method and apparatus for controlling the motion of a redundancy manipulator
CN104476550A (zh) 全液压自主移动机械臂及其动作方法
CN103240739A (zh) 一种移动机械臂分散集中自主切换控制方法
CN111300384B (zh) 一种基于标识卡运动的机器人增强现实示教的注册系统及方法
CN115030706A (zh) 基于锚固工艺的钻锚机器人双钻臂协同控制系统及方法
CN110561419B (zh) 臂型线约束柔性机器人轨迹规划方法及装置
Marwan et al. Calibration method for articulated industrial robots
Wang et al. A control method for hydraulic manipulators in automatic emulsion filling
CN108356820B (zh) 一种多关节机械臂手动操控的逆运动学求解方法
CN114378827B (zh) 一种基于移动机械臂整体控制的动态目标跟踪抓取方法
CN114505862A (zh) 一种建筑3d打印移动机械臂站位规划方法及系统
Subramanian et al. Real-time obstacle avoidance for an underactuated flat-fish type autonomous underwater vehicle in 3D space
Liu et al. Automatic spraying motion planning of a shotcrete manipulator
Nazarzehi et al. Decentralized three dimensional formation building algorithms for a team of nonholonomic mobile agents
CN115284293A (zh) 适应复杂任务的空间站机械臂多模式路径规划系统和方法
WO2012153958A2 (ko) 고소작업대의 직선이동제어 방법 및 장치
RU2424893C2 (ru) Адаптивный мобильный пространственный робот-манипулятор и способ организации движений и контроля физико-механических свойств и геометрической формы контактируемой поверхности и траектории перемещения с его помощью
CN111075489B (zh) 一种液压支架与刮板输送机浮动连接机构姿态描述方法
KR102517598B1 (ko) 로봇의 직접 교시 장치 및 직접 교시 방법
Chang et al. Mobile robot navigation and control with monocular surveillance cameras
CN117801831A (zh) 一种基于双臂协作的砌筑机器及砌筑方法、系统
Chang et al. Map-based navigation and control of mobile robots with surveillance cameras
Boccolato et al. 3D kinematics of a tentacle robot

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