一种用于多臂凿岩机器人的机械臂碰撞检测方法
阅读说明：本技术 一种用于多臂凿岩机器人的机械臂碰撞检测方法 (Mechanical arm collision detection method for multi-arm rock drilling robot ) 是由 徐巧玉 方梦娟 李坤鹏 张正 王军委 刘阳 于 2021-08-05 设计创作，主要内容包括：一种用于多臂凿岩机器人的机械臂碰撞检测方法,包括如下步骤：S1、将凿岩机器人的机械臂转化为若干个圆柱体包络盒的集合,将隧道顶壁转换为圆弧线段并且基于圆弧线段构建扇形检测面；S2、在凿岩机器人动作过程中实时检测任意两个圆柱体包络盒之间的最小距离值判断机械臂之间是否发生碰撞；S3、基于圆柱体包络盒在检测面上的投影判断机械臂与隧道顶壁是否发生碰撞。本发明提供一种用于多臂凿岩机器人的机械臂碰撞检测方法,能够实时准确且全面地检测凿岩机器人的碰撞情况,使系统及时预警从而避免碰撞干涉事故发生,实时性强,效率更高。(A mechanical arm collision detection method for a multi-arm rock drilling robot comprises the following steps: s1, converting a mechanical arm of the rock drilling robot into a set of a plurality of cylindrical enveloping boxes, converting the top wall of the tunnel into an arc line segment and constructing a fan-shaped detection surface based on the arc line segment; s2, detecting the minimum distance value between any two cylinder enveloping boxes in real time in the action process of the rock drilling robot to judge whether the mechanical arms collide; and S3, judging whether the mechanical arm collides with the top wall of the tunnel or not based on the projection of the cylindrical envelope box on the detection surface. The invention provides a mechanical arm collision detection method for a multi-arm rock drilling robot, which can accurately and comprehensively detect the collision condition of the rock drilling robot in real time, so that the system can give early warning in time to avoid collision interference accidents, and has strong real-time performance and higher efficiency.)
技术领域
本发明涉及凿岩机器人领域，具体的说是一种用于多臂凿岩机器人的机械臂碰撞检测方法。
背景技术
目前，多臂凿岩机器人广泛应用于煤矿、隧道开挖等工作中。由于凿岩机器人属于工业液压重载型机械臂，具有多臂协作、结构复杂等特点，因此机械臂在隧道中作业时极易发生碰撞干涉现象，严重时会造成极大的施工人员伤亡和经济损失，因此多臂凿岩机器人的碰撞检测问题亟需解决。
迄今为止，众多学者在凿岩机器人的碰撞检测方面进行了研究，主要分为基于外部传感器的碰撞检测方法、基于视觉的碰撞检测方法和通过建模实现的碰撞检测方法。对于工业液压重载机器人来讲，由于使用建模实现的碰撞检测方法易于实现、处理数据效率高，所以目前依赖建模实现的碰撞方法应用广泛。该方法通常将机器人进行模型简化，通过计算空间几何模型距离实现机器人的碰撞检测，该方法主要包括如下几种类型。
第一种是采用球体和胶囊体包围盒简化机械臂模型，能够实现有效的碰撞检测，但因机械臂关节机械结构的不规则性，使用球体包络会对检测精度造成一定的影响。
第三种是使用空间凸多面体模型对复杂物体进行建模，能够准确实现碰撞检测，但凿岩机器人所处隧道壁相对规则，用该种方法会降低机器人与隧道的碰撞检测效率。依赖建模展开的凿岩机器人碰撞检测研究多应用于凿岩机器人多个机械臂之间，因此凿岩机器人机械臂与隧道的碰撞检测研究较少，仍待进一步深入研究。同时在依赖建模实现的碰撞检测方法中，复杂的建模方法和计算方法会降低检测算法的效率和精度。
第三种是采用圆柱体包围盒简化机械臂模型，基于空间向量运算实现碰撞检测，该方法相对于第一中方法的精度更高，相对于第二种方法更加简单，但是现有的此类方法实现方法仍然非常复杂，实施效率较低。
发明内容
为了解决现有技术中的不足，本发明提供一种用于多臂凿岩机器人的机械臂碰撞检测方法，能够实时准确且全面地检测凿岩机器人的碰撞情况，使系统及时预警从而避免碰撞干涉事故发生，实时性强，效率更高。
为了实现上述目的，本发明采用的具体方案为：一种用于多臂凿岩机器人的机械臂碰撞检测方法，包括如下步骤：
S1、将凿岩机器人的机械臂转化为若干个圆柱体包络盒的集合，将隧道顶壁转换为圆弧线段并且基于圆弧线段构建扇形检测面；
S2、在凿岩机器人动作过程中实时检测任意两个圆柱体包络盒之间的最小距离值判断机械臂之间是否发生碰撞；
S3、基于圆柱体包络盒在检测面上的投影判断机械臂与隧道顶壁是否发生碰撞。
作为上述用于多臂凿岩机器人的机械臂碰撞检测方法的进一步优化：S1中，机械臂包括工作臂和吊篮臂，工作臂和吊篮臂均包括两个及以上的动作部件，将所有动作部件均转换为圆柱体包络盒；
将圆弧线段的两个端点分别记为T1和T2，将圆弧线段的圆心记为O，则检测面为扇形 T1OT2，半径为R,并且以O为极点，OT2为极轴构建极坐标系。
作为上述用于多臂凿岩机器人的机械臂碰撞检测方法的进一步优化：S2中，参与检测的两个圆柱体包络盒的轴线之间的最小距离为d，则两个圆柱体包络盒之间的最小距离为 dmin＝d-(r1+r2)，其中r1和r2分别为两个圆柱体包络盒的半径，若dmin＞0则两个圆柱体包络盒对应的动作部件未发生碰撞，否则两个圆柱体包络盒对应的动作部件发生碰撞。
作为上述用于多臂凿岩机器人的机械臂碰撞检测方法的进一步优化：S2中，计算两圆柱体轴线间最小距离d的方法为：
S21、记两个圆柱体包络盒的轴线段为l1和l2，l1的两个端点坐标分别为P1(x1,y1,z1)和 P2(x2,y2,z2)，l2的两个端点坐标分别为Q1(x3,y3,z3)和Q2(x4,y4,z4)，则l1和l2上任意一点的坐标可以表示为：
其中向量向量λ1和λ2为系数；
S22、将最小距离d的求解过程转化为求系数λ1和λ2的最优解过程，
S23、基于极小值条件对S22中的公式进行简化得到
S24、当0≤λ1,λ2≤1时d2＝f(λ1,λ2)，否则执行S25；
S25、当或时，分别计算P1到l2的距离d1、P2到l2的距离d2、Q1到l1的距离 d3和Q2到l2的距离d4，并且有d＝{d1,d2,d3,d4}min。
作为上述用于多臂凿岩机器人的机械臂碰撞检测方法的进一步优化：S3的具体方法为：
S31、将圆柱体包络盒在检测面上的投影简化为矩形，并且将圆柱体包络盒轴线投影的两端分别记为K1和K2，在极坐标下将K1、K2、T1和T2描述为 和并且有β2＝0；
S32、当K1和K2中最大极角小于T1和T2中的最小极角或K1和K2中最小极角大于T1和T2中的最大极角时，矩形投影全部落在扇形T1OT2范围外，矩形投影与圆弧线段无干涉发生，矩形投影对应的动作部件未与隧道顶壁发生碰撞；
当T1和T2的最大极角介于K1和K2中最小和最大极角之间且T1和T2的最小极角小于K1和K2中的最小极角时，矩形投影轴线与扇形边缘的交点处为K，若 则矩形投影与圆弧线段无干涉发生并且矩形投影对应的动作部件未与隧道顶壁发生碰撞，否则矩形投影与圆弧线段发生干涉并且矩形投影对应的动作部件与隧道顶壁发生碰撞；
当T1和T2的最小极角介于K1和K2中最小和最大极角之间且T1和T2的最大极角大于K1和K2中的最大极角时，矩形投影轴线与扇形边缘的交点处为K，若 则矩形投影与圆弧线段未发生干涉并且矩形投影对应的动作部件未与隧道顶壁发生碰撞，否则矩形投影与圆弧线段发生干涉并且矩形投影对应的动作部件与隧道顶壁发生碰撞；
当K1和K2中最大极角和最小极角均介于T1和T2中的最小极角和最大极角之间时，若则矩形投影与圆弧线段无干涉发生并且矩形投影对应的动作部件未与隧道顶壁发生碰撞，否则矩形投影与圆弧线段发生干涉并且矩形投影对应的动作部件与隧道顶壁发生碰撞。
作为上述用于多臂凿岩机器人的机械臂碰撞检测方法的进一步优化：S3中，记OK的长度为d1，则计算d1的方法为：
计算
由矢量点积公式可得
由三角余弦定理可得
有益效果：本文首先建立凿岩机器人的碰撞检测模型，针对机器人的结构特点将机械臂的杆件简化为圆柱体包络盒，通过时刻计算空间中各个圆柱体包络盒的距离实现凿岩机器人单条钻臂自身、三条钻臂之间、三条钻臂分别与吊篮臂的碰撞检测，通过分析计算圆柱体包络盒在隧道壁面上的矩形投影与隧道壁面的干涉情况快速实现凿岩机器人与隧道的碰撞检测，能够实时准确且全面地检测凿岩机器人的碰撞情况，使系统及时预警从而避免碰撞干涉事故发生，实时性强，并且效率更高。
附图说明
图1是圆柱体包络盒的构建方式示意图；
图2是S2中判断机械臂之间是否发生碰撞的示意图；
图3是检测面的示意图；
图4是隧道简化模型的示意图；
图5是矩形投影与检测面的相对关系示意图A；
图6是矩形投影与检测面的相对关系示意图B。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
一种用于多臂凿岩机器人的机械臂碰撞检测方法，包括如下步骤：
S1、将凿岩机器人的机械臂转化为若干个圆柱体包络盒的集合，将隧道顶壁转换为圆弧线段并且基于圆弧线段构建扇形检测面。S1中，机械臂包括工作臂和吊篮臂，工作臂和吊篮臂均包括两个及以上的动作部件，将所有动作部件均转换为圆柱体包络盒。如图3所示，将圆弧线段的两个端点分别记为T1和T2，将圆弧线段的圆心记为O，则检测面为扇形T1OT2，半径为R,并且以O为极点，OT2为极轴构建极坐标系。
凿岩机器人的结构较为复杂，以中铁DJ3E多臂凿岩机器人为例，其包括了三条钻臂和一条吊篮臂，其中三条钻臂的结构相同，在工作过程中，钻臂与钻臂之间、钻臂与吊篮臂之间以及钻臂或者吊篮臂与隧道壁之间都有可能出现碰撞，如果对每条工作臂都精确建模的话工程量过大，并且实施效率比较低，因此本发明利用圆柱体包络盒对工作臂进行建模，每个工作臂可以包括多个圆柱体包络盒，如图1所示，钻臂可以包括四个圆柱体包络盒CS1、CS2、CS3和CS4，吊篮臂可以包括两个圆柱体包络盒CS5和CS6，利用圆柱体包络盒对工作臂进行建模，可以有效降低模型复杂度，进而降低方法实现难度，提升运行效率。
S2、在凿岩机器人动作过程中实时检测任意两个圆柱体包络盒之间的最小距离值判断机械臂之间是否发生碰撞。如图2所示，S2中，参与检测的两个圆柱体包络盒的轴线之间的距离为d，则两个圆柱体包络盒之间的最小距离为dmin＝d-(r1+r2)，其中r1和r2分别为两个圆柱体包络盒的半径，若dmin＞0则两个圆柱体包络盒对应的动作部件未发生碰撞，否则两个圆柱体包络盒对应的动作部件发生碰撞。
计算最小距离dmin的方法为S21至S25。
S21、记两个圆柱体包络盒的轴线段为l1和l2，l1的两个端点坐标分别为P1(x1,y1,z1)和 P2(x2,y2,z2)，l2的两个端点坐标分别为Q1(x3,y3,z3)和Q2(x4,y4,z4)，则l1和l2上任意一点的坐标可以表示为：
其中向量向量λ1和λ2为系数。
S22、将最小距离dmin的求解过程转化为系数λ1和λ2的求最优解过程，
S23、基于极小值条件对S22中的公式进行简化得到
S24、当0≤λ1,λ2≤1时d2 min＝f(λ1,λ2)，否则执行S25。
S25、当或时，分别计算P1到l2的距离d1、P2到l2的距离d2、Q1到l1的距离d3和Q2到l2的距离d4，并且有dmin＝{d1,d2,d3,d4}min。
S3、基于圆柱体包络盒在检测面上的投影判断机械臂与隧道顶壁是否发生碰撞。首先如图4所示，基于隧道内情况建立三轴坐标系，直线段T1T3、T2T4分别为隧道壁面的左右轮廓线，直线段T3T4为地平面，O点为圆弧线段T1T2的圆心，T5为圆弧线段T1T2的中点，A 点为直线段T1T2的中点，从施工图中可以获取隧道轮廓线上各点在掌子面坐标系下的坐标值及圆弧线段的拱高H，至此可以计算圆弧线段的半径R及圆心O的坐标值：
O(x5,0,z5-R)；
其中x5,z5分别为T5的X、Z轴坐标值。
S3的具体方法为S31至S32。
S31、将圆柱体包络盒在检测面上的投影简化为矩形，并且将圆柱体包络盒轴线投影的两端分别记为K1和K2，在极坐标下将K1、K2、T1和T2描述为 和并且有β2＝0。
S32、如图5中a部分所示，当K1和K2中最大极角小于T1和T2中的最小极角或K1和 K2中最小极角大于T1和T2中的最大极角时，矩形投影全部落在扇形T1OT2范围外，矩形投影与圆弧线段无干涉发生，矩形投影对应的动作部件未与隧道顶壁发生碰撞。
如图5中b部分所示，当T1和T2的最大极角介于K1和K2中最小和最大极角之间且 T1和T2的最小极角小于K1和K2中的最小极角时，矩形投影轴线与扇形边缘的交点处为K，若则矩形投影与圆弧线段无干涉发生并且矩形投影对应的动作部件未与隧道顶壁发生碰撞，否则矩形投影与圆弧线段发生干涉并且矩形投影对应的动作部件与隧道顶壁发生碰撞。
如图5中c部分所示，当T1和T2的最小极角介于K1和K2中最小和最大极角之间且T1和T2的最大极角大于K1和K2中的最大极角时，矩形投影轴线与扇形边缘的交点处为K，若则矩形投影与圆弧线段未发生干涉并且矩形投影对应的动作部件未与隧道顶壁发生碰撞，否则矩形投影与圆弧线段发生干涉并且矩形投影对应的动作部件与隧道顶壁发生碰撞。
如图5中d部分所示，当K1和K2中最大极角和最小极角均介于T1和T2中的最小极角和最大极角之间时，若则矩形投影与圆弧线段无干涉发生并且矩形投影对应的动作部件未与隧道顶壁发生碰撞，否则矩形投影与圆弧线段发生干涉并且矩形投影对应的动作部件与隧道顶壁发生碰撞。
如图6所示，S3中，记OK的长度为d1，则计算d1的方法为：
计算
由矢量点积公式可得
由三角余弦定理可得
对于机械臂是否与隧道垂直侧壁之间是否发生碰撞，也可以基于圆柱体包络盒在掌子面上的投影判断，并且基于圆柱体包络盒与隧道壁面的左右轮廓线T1T3、T2T4之间的距离进行判断，该距离的计算方法与两个圆柱体包络盒之间的最小距离的计算方法相同，当该距离大于0时机械臂未与隧道垂直侧壁发生碰撞。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种不停水复原治理涌水孔转换装备