CN114211498A - 基于方向包围盒的锚杆支护机器人碰撞检测方法及系统 - Google Patents

基于方向包围盒的锚杆支护机器人碰撞检测方法及系统 Download PDF

Info

Publication number
CN114211498A
CN114211498A CN202111658153.1A CN202111658153A CN114211498A CN 114211498 A CN114211498 A CN 114211498A CN 202111658153 A CN202111658153 A CN 202111658153A CN 114211498 A CN114211498 A CN 114211498A
Authority
CN
China
Prior art keywords
bounding box
virtual direction
drilling rig
virtual
vector
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.)
Granted
Application number
CN202111658153.1A
Other languages
English (en)
Other versions
CN114211498B (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.)
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery Co Ltd
Original Assignee
Taiyuan Institute of China Coal Technology and Engineering Group
Shanxi Tiandi Coal Mining Machinery Co Ltd
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 Taiyuan Institute of China Coal Technology and Engineering Group, Shanxi Tiandi Coal Mining Machinery Co Ltd filed Critical Taiyuan Institute of China Coal Technology and Engineering Group
Priority to CN202111658153.1A priority Critical patent/CN114211498B/zh
Publication of CN114211498A publication Critical patent/CN114211498A/zh
Application granted granted Critical
Publication of CN114211498B publication Critical patent/CN114211498B/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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/0095—Means or methods for testing manipulators

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Earth Drilling (AREA)

Abstract

本发明涉及一种基于方向包围盒的锚杆支护机器人碰撞检测方法及系统，该方法包括：分别建立锚杆支护机器人的左右钻架的虚拟方向包围盒，并确定左右钻架的虚拟方向包围盒的位姿信息和尺寸信息；根据左右钻架的虚拟方向包围盒的位姿信息和尺寸信息以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；若是，则重新规划所述锚杆支护机器人的运动路径；若否，则继续工作。通过检测虚拟方向包围盒相交来代替对真实物体碰撞检测，降低碰撞检测的计算成本，能够保证锚杆支护机器人自动作业时监测钻架是否与机器人自身发生碰撞，满足锚杆支护机器人安全高效作业的需求。

Description

基于方向包围盒的锚杆支护机器人碰撞检测方法及系统
技术领域
本发明属于锚杆支护机器人自动锚杆支护作业技术领域，特别是涉及一种基于方向包围盒的锚杆支护机器人碰撞检测方法及系统。
背景技术
锚杆支护机器人是煤矿矩形巷道支护的重要装备，对矩形巷道高效安全掘进具有巨大的意义。锚杆支护机器人碰撞检测技术是自动化锚杆支护的技术基础，锚杆支护机器人碰撞检测的主要任务是在锚杆支护机器人进行自动锚杆支护作业时检测锚杆支护机器人的钻架是否与机器人自身或者矩形巷道发生接触，进而避免锚杆支护机器人自动化作业时钻架与其他部件发生碰撞。
然而目前还没有应用于锚杆支护机器人碰撞检测的方法，这给锚杆支护机器人自动化作业带来了很大的安全隐患，为了保证锚杆支护机器人的安全工作，亟需一种锚杆支护机器人碰撞检测方法。
发明内容
本发明的目的是提供一种基于方向包围盒的锚杆支护机器人碰撞检测方法及系统，能够保证锚杆支护机器人自动作业时监测钻架是否与机器人自身发生碰撞，满足锚杆支护机器人安全高效作业的需求。
为实现上述目的，本发明提供了如下方案：
本发明提供了一种基于方向包围盒的锚杆支护机器人碰撞检测方法，所述方法包括：
分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系；
根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；
若是，则重新规划所述锚杆支护机器人的运动路径；
若否，则继续工作。
可选地，所述根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞，具体包括：
根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
可选地，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure BDA0003446279590000021
所述左钻架的虚拟方向包围盒的尺寸信息为
Figure BDA0003446279590000022
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure BDA0003446279590000023
所述右钻架的虚拟方向包围盒的尺寸信息为
Figure BDA0003446279590000024
所述转移矩阵为：
Figure BDA0003446279590000031
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，
Figure BDA0003446279590000032
表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000033
oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000034
aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000035
PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000036
eL表示左钻架的虚拟方向包围盒的正半轴边长向量；
Figure BDA0003446279590000037
表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000038
oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000039
aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA00034462795900000310
PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA00034462795900000311
eR表示右钻架的虚拟方向包围盒的正半轴边长向量，
Figure BDA00034462795900000312
为转移矩阵。
可选地，所述分离轴检测公式具体为：
|T·L|＞rL+rR；
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影。
可选地，所述将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞，具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和；
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
可选地，所述方法还包括：
获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
为实现上述目的，本发明还提供了一种基于方向包围盒的锚杆支护机器人碰撞检测系统，所述系统包括：
包围盒建立和信息确定模块，用于分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻边长的位置关系为垂直关系；
碰撞检测模块，用于根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；在左钻架和右钻架发生碰撞时，重新规划所述锚杆支护机器人的运动路径；在左钻架和右钻架未发生碰撞时，继续工作。
可选地，所述碰撞检测模块，具体包括：
转移矩阵和位移向量计算单元，用于根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
碰撞检测单元，用于将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
可选地，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure BDA0003446279590000051
所述左钻架的虚拟方向包围盒的尺寸信息为
Figure BDA0003446279590000052
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure BDA0003446279590000053
所述右钻架的虚拟方向包围盒的尺寸信息为
Figure BDA0003446279590000054
所述转移矩阵为：
Figure BDA0003446279590000061
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，
Figure BDA0003446279590000062
表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000063
oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000064
aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000065
PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000066
eL表示左钻架的虚拟方向包围盒的正半轴边长向量；
Figure BDA0003446279590000067
表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000068
oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000069
aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA00034462795900000610
PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA00034462795900000611
eR表示右钻架的虚拟方向包围盒的正半轴边长向量，
Figure BDA00034462795900000612
为转移矩阵；
所述分离轴检测公式具体为：
|T·L|＞rL+rR；
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影；
所述碰撞检测单元，具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和；
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
可选地，所述系统还包括：
巷道碰撞检测模块，用于获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明提供了一种基于方向包围盒的锚杆支护机器人碰撞检测方法及系统，该方法包括：分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻边长的位置关系为垂直关系；根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；若是，则重新规划所述锚杆支护机器人的运动路径；若否，则继续工作。通过检测虚拟方向包围盒相交来代替对真实物体碰撞检测，降低碰撞检测的成本，能够保证锚杆支护机器人自动作业时监测钻架是否与机器人自身发生碰撞，满足锚杆支护机器人安全高效作业的需求。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为二维分离轴检测示意图；
图2为三维分离轴检测示意图；
图3为本发明基于方向包围盒的锚杆支护机器人碰撞检测方法的流程图；
图4为锚杆支护机器人碰撞检测的方向包围盒示意图；
图5为锚杆支护机器人钻架的方向包围盒；
图6为左右钻架碰撞检测示意图。
符号说明：
锚杆支护机器人-1，虚拟方向包围盒-2，钻架-3。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于方向包围盒的锚杆支护机器人碰撞检测方法及系统，能够保证锚杆支护机器人自动作业时监测钻架是否与机器人自身发生碰撞，满足锚杆支护机器人安全高效作业的需求。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
方向包围盒碰撞检测采用了分离轴检测方法，分离轴检测基于分离超平面原理得出，分离超平面原理证明：给定两个凸集，二者要么相交，否则必然存在分离超平面将两个凸集间隔在两侧。分离轴定义为分离两个图形的超平面的垂线，将分离图形在分离轴上投影，会得到不重叠的线段。因为分离轴存在是分离超平面存在的充分必要条件，因此如果两个图形投影在分离轴上的线段不重叠，两个图形必然不想交。根据分离轴检测理论，对于两个方向包围盒碰撞检测，可能存在的分离轴包含：两个方向包围盒的基向量以及基向量的组合。
二维平面方向包围盒分离轴检测示意图，如图1所示。每个方向包围盒由中心点坐标加方向矩阵和两条边的半边长表示，即中心点座标、包围盒2个方向的基向量以及沿基向量表示正区间范围的2个实数(共8个浮点数表示方向包围盒)二维平面可能存在的分离轴：两个方向包围盒的基向量4条，基向量组合4条，共8条。
方向包围盒A在可能的分离轴L上投影半径为rA，方向包围盒B在可能的分离轴L上投影半径为rB。方向包围盒A和方向包围B的距离矢量T在可能的分离轴L上投影为|T·L|。若满足
|T·L|>rA+rB
则分离轴存在，二维平面图形A和B不相交，无平面碰撞发生。
同理，三维空间方向包围盒分离轴检测示意图，如图2所示。每个方向包围盒由中心点加方向矩阵和三条边的半边长表示，即中心点座标(3个)、包围盒3个方向的基向量(9个)以及沿基向量表示正区间范围的3个实数(共15个浮点数表示方向包围盒)。三维空间可能存在的分离轴：两个方向包围盒的基向量6条，基向量组合9条，共15条，详细分离轴的计算公式如表1所示：
表1三维空间分离轴检测计算公式
Figure BDA0003446279590000091
Figure BDA0003446279590000101
鉴于以上内容，本发明提供了一种基于方向包围盒的锚杆支护机器人碰撞检测方法，如图3所示，包括以下步骤：
S1：分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系。锚杆支护机器人碰撞检测的方向包围盒示意图如图4和图5。
具体地，虚拟方向包围盒为矩形盒，使用矩形盒完全包围钻架，钻架的虚拟方向包围盒由中心点相对锚杆支护机器人的坐标、方向矩阵和三条互相垂直的棱边的半边长表示，即虚拟方向包围盒的中心位姿矩阵(12个浮点数)以及沿基向量表示正区间范围的3个实数(共15个浮点数表示方向包围盒)，描述矩形盒的位姿，得到钻架的虚拟方向包围盒。其中，虚拟方向包围盒的中心位姿矩阵由锚杆支护机器人的运动学模型计算得到。通过建立钻架的虚拟方向包围盒，能够通过检测虚拟方向包围盒相交来代替对真实物体碰撞检测，降低了碰撞检测的计算成本。
S2：根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞。图6为左右钻架碰撞检测示意图。
进一步地，S2具体包括：
S21：根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵。
S22：将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
其中，根据方向包围盒分离轴检测原理的计算规则要求，为了减少计算量，将右钻架虚拟方向包围盒位姿矩阵转换到左钻架虚拟方向包围盒坐标系下表示。使左钻架方向包围盒中心成为坐标系原点(零点)，省去了左钻架碰撞检测时的矩阵计算量，提高了计算效率。
具体地，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure BDA0003446279590000111
所述左钻架的虚拟方向包围盒的尺寸信息为
Figure BDA0003446279590000112
由于虚拟方向包围盒包围盒关于中心点对称，只用单位基向量正半轴即可表示整个方向包围盒姿态。
Figure BDA0003446279590000113
和
Figure BDA0003446279590000114
分别为左钻架虚拟方向包围盒的半边长。
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure BDA0003446279590000115
所述右钻架的虚拟方向包围盒的尺寸信息为
Figure BDA0003446279590000116
Figure BDA0003446279590000117
和
Figure BDA0003446279590000118
分别为右钻架虚拟方向包围盒的半边长。
所述转移矩阵为：
Figure BDA0003446279590000121
其中，旋转矩阵为：
Figure BDA0003446279590000122
中心位移向量为：
Figure BDA0003446279590000123
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL。
其中，
Figure BDA0003446279590000124
表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000125
oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000126
aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000127
PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000128
eL表示左钻架的虚拟方向包围盒的正半轴边长向量；
Figure BDA0003446279590000129
表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA00034462795900001210
oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000131
aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000132
PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000133
eR表示右钻架的虚拟方向包围盒的正半轴边长向量，
Figure BDA0003446279590000134
为转移矩阵。
进一步地，将所述转移矩阵转换为绝对值形式，得到转移矩阵的绝对值。
对所述转移矩阵的绝对值增加ε项进行修正，得到修正后的转移矩阵。增加ε项进行修正，从而避免坐标轴平行时求叉积产生计算错误。
其中，提取旋转矩阵，旋转矩阵的绝对值形式为
Figure BDA0003446279590000135
更进一步地，所述分离轴检测公式具体为：
|T·L|＞rL+rR。
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影。
进一步地，步骤S22具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和；
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
进一步地，左右钻架的15条分离轴对应的检测公式如下所示：
(1)分离轴
Figure BDA0003446279590000141
第一距离即分离轴投影为|T·L|＝|t0|；第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000142
右钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000143
(2)分离轴
Figure BDA0003446279590000144
第一距离即分离轴投影为|T·L|＝|t1|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000145
右钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000146
(3)分离轴
Figure BDA0003446279590000147
第一距离即分离轴投影|T·L|＝|t2|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000148
右钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000149
(4)分离轴
Figure BDA00034462795900001410
第一距离即分离轴投影|T·L|＝|t0r00+t1r10+t2r20|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001411
右钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001412
(5)分离轴
Figure BDA00034462795900001413
第一距离即分离轴投影|T·L|＝|t0r01+t1r11+t2r21|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001414
右钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001415
(6)分离轴
Figure BDA00034462795900001416
第一距离即分离轴投影|T·L|＝|t0r02+t1r12+t2r22|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001417
右钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001418
(7)分离轴
Figure BDA00034462795900001419
第一距离即分离轴投影|T·L|＝|t2r10-t1r20|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001420
右钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001421
(8)分离轴
Figure BDA00034462795900001422
第一距离即分离轴投影|T·L|＝|t2r11-t1r21|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001423
右钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001424
(9)分离轴
Figure BDA00034462795900001425
第一距离即分离轴投影|T·L|＝|t2r12-t1r22|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001426
右钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001427
(10)分离轴
Figure BDA00034462795900001428
第一距离即分离轴投影|T·L|＝|t0r20-t2r00|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000151
右钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000152
(11)分离轴
Figure BDA0003446279590000153
第一距离即分离轴投影|T·L|＝|t0r21-t2r01|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000154
右钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000155
(12)分离轴
Figure BDA0003446279590000156
第一距离即分离轴投影|T·L|＝|t0r22-t2r02|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000157
右钻架虚拟方向包围盒的中心半径投影
Figure BDA0003446279590000158
(13)分离轴
Figure BDA0003446279590000159
第一距离即分离轴投影|T·L|＝|t1r00-t0r10|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001510
右钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001511
(14)分离轴
Figure BDA00034462795900001512
第一距离即分离轴投影|T·L|＝|t1r01-t0r11|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001513
右钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001514
(15)分离轴
Figure BDA00034462795900001515
第一距离即分离轴投影|T·L|＝|t1r02-t0r12|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001516
右钻架虚拟方向包围盒的中心半径投影
Figure BDA00034462795900001517
其中，分离轴
Figure BDA00034462795900001518
和
Figure BDA00034462795900001519
分别为左钻架虚拟方向包围盒的三个基向量对应的分离轴；分离轴
Figure BDA00034462795900001520
和
Figure BDA00034462795900001521
分别为右钻架虚拟方向包围盒的三个基向量对应的分离轴；分离轴
Figure BDA00034462795900001522
和
Figure BDA00034462795900001523
分别为左钻架虚拟方向包围盒与右钻架虚拟方向包围盒的基向量组合对应的分离轴。
S3：若是，则重新规划所述锚杆支护机器人的运动路径。
S4：若否，则继续工作。
进一步地，所述方法还包括：基于虚拟方向包围盒对锚杆支护机器人钻架与矩形巷道碰撞检测步骤，具体包括：
获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
所述分离轴检测公式为|T·L|＞rF+rT，根据分离轴检测公式结果判断钻架是否与矩形巷道发生碰撞。式中：rF为钻架虚拟方向包围盒的中心半径投影，rT为矩形巷道的中心半径投影。
进一步地，本发明基于方向包围盒的锚杆支护机器人碰撞检测系统，包括：包围盒建立和信息确定模块和碰撞检测模块。
包围盒建立和信息确定模块，用于分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系。
碰撞检测模块，用于根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；在左钻架和右钻架发生碰撞时，重新规划所述锚杆支护机器人的运动路径；在左钻架和右钻架未发生碰撞时，继续工作。
进一步地，所述碰撞检测模块，具体包括：
转移矩阵和位移向量计算单元，用于根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
碰撞检测单元，用于将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
具体地，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure BDA0003446279590000171
所述左钻架的虚拟方向包围盒的尺寸信息为
Figure BDA0003446279590000172
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure BDA0003446279590000173
所述右钻架的虚拟方向包围盒的尺寸信息为
Figure BDA0003446279590000174
所述转移矩阵为：
Figure BDA0003446279590000175
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，
Figure BDA0003446279590000176
表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000177
oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000178
aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000179
PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA00034462795900001710
eL表示左钻架的虚拟方向包围盒的正半轴边长向量；
Figure BDA00034462795900001711
表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000181
oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000182
aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000183
PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure BDA0003446279590000184
eR表示右钻架的虚拟方向包围盒的正半轴边长向量，
Figure BDA0003446279590000185
为转移矩阵。
所述分离轴检测公式具体为：
|T·L|＞rL+rR；
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影。
所述碰撞检测单元，具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和。
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
进一步地，所述系统还包括：
巷道碰撞检测模块，用于获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
下面结合具体实施例，对本发明方法做进一步详细说明。
实施例
根据锚杆支护机器人部件外轮廓，建立锚杆支护机器人部件的虚拟方向包围盒，如图4所示，1为锚杆支护机器人，2为虚拟方向包围盒；锚杆支护机器人钻架虚拟方向包围盒如图5所示，3为钻架。钻架虚拟方向包围盒正半轴边长向量为：
Figure BDA0003446279590000191
定义整车坐标系在左右工作臂基坐标系OL0和OR0中间，左钻架中心O7L相对锚杆支护机器人工作臂基坐标系的坐标为(xL7,yL7,zL7)，右钻架中心O7R相对锚杆支护机器人工作臂基坐标系的坐标为(xR7,yR7,zR7),根据矩阵变换，得到左、右钻架虚拟方向包围盒中心位姿矩阵
Figure BDA0003446279590000192
和
Figure BDA0003446279590000193
令右钻架静止，即关节变量θ1＝θ2＝d3＝θ4＝θ5＝θ6＝0，左钻架旋转-30°，即关节变量θ1＝θ2＝d3＝θ4＝θ5＝0，θ6＝-30°，对应于左钻架从竖直向水平转换。
左钻架虚拟方向包围盒中心位姿矩阵为：
Figure BDA0003446279590000194
右钻架虚拟方向包围盒中心位姿矩阵为：
Figure BDA0003446279590000195
进而得到右钻架虚拟方向包围盒在左钻架虚拟方向包围盒坐标系下的转移矩阵为：
Figure BDA0003446279590000196
取ε＝0.05，转移矩阵的绝对值为：
Figure BDA0003446279590000201
由此依次计算15个分离轴的检测公式，可得15个分离轴检测公式的结果都不满足|T·L|＞rL+rR，因此左右钻架相交，发生了碰撞。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述方法包括：
分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系；
根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；
若是，则重新规划所述锚杆支护机器人的运动路径；
若否，则继续工作。
2.根据权利要求1所述的基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞，具体包括：
根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
3.根据权利要求2所述的基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure FDA0003446279580000011
所述左钻架的虚拟方向包围盒的尺寸信息为
Figure FDA0003446279580000021
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure FDA0003446279580000022
所述右钻架的虚拟方向包围盒的尺寸信息为
Figure FDA0003446279580000023
所述转移矩阵为：
Figure FDA0003446279580000024
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，
Figure FDA0003446279580000025
表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000026
oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000027
aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000028
PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000029
eL表示左钻架的虚拟方向包围盒的正半轴边长向量；
Figure FDA00034462795800000210
表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA00034462795800000211
oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA00034462795800000212
aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000031
PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000032
eR表示右钻架的虚拟方向包围盒的正半轴边长向量，
Figure FDA0003446279580000033
为转移矩阵。
4.根据权利要求3所述的基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述分离轴检测公式具体为：
|T·L|＞rL+rR；
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影。
5.根据权利要求4所述的基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞，具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和；
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
6.根据权利要求1所述的基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述方法还包括：
获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
7.一种基于方向包围盒的锚杆支护机器人碰撞检测系统，其特征在于，所述系统包括：
包围盒建立和信息确定模块，用于分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系；
碰撞检测模块，用于根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；在左钻架和右钻架发生碰撞时，重新规划所述锚杆支护机器人的运动路径；在左钻架和右钻架未发生碰撞时，继续工作。
8.根据权利要求7所述的基于方向包围盒的锚杆支护机器人碰撞检测系统，其特征在于，所述碰撞检测模块，具体包括：
转移矩阵和位移向量计算单元，用于根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
碰撞检测单元，用于将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
9.根据权利要求8所述的基于方向包围盒的锚杆支护机器人碰撞检测系统，其特征在于，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure FDA0003446279580000051
所述左钻架的虚拟方向包围盒的尺寸信息为
Figure FDA0003446279580000052
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
Figure FDA0003446279580000053
所述右钻架的虚拟方向包围盒的尺寸信息为
Figure FDA0003446279580000054
所述转移矩阵为：
Figure FDA0003446279580000055
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，
Figure FDA0003446279580000056
表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000057
oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000058
aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000059
PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA00034462795800000510
eL表示左钻架的虚拟方向包围盒的正半轴边长向量；
Figure FDA00034462795800000511
表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000061
oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000062
aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000063
PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，
Figure FDA0003446279580000064
eR表示右钻架的虚拟方向包围盒的正半轴边长向量，
Figure FDA0003446279580000065
为转移矩阵；
所述分离轴检测公式具体为：
|T·L|＞rL+rR；
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影；
所述碰撞检测单元，具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和；
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
10.根据权利要求1所述的基于方向包围盒的锚杆支护机器人碰撞检测系统，其特征在于，所述系统还包括：
巷道碰撞检测模块，用于获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
CN202111658153.1A 2021-12-30 2021-12-30 基于方向包围盒的锚杆支护机器人碰撞检测方法及系统 Active CN114211498B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111658153.1A CN114211498B (zh) 2021-12-30 2021-12-30 基于方向包围盒的锚杆支护机器人碰撞检测方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111658153.1A CN114211498B (zh) 2021-12-30 2021-12-30 基于方向包围盒的锚杆支护机器人碰撞检测方法及系统

Publications (2)

Publication Number Publication Date
CN114211498A true CN114211498A (zh) 2022-03-22
CN114211498B CN114211498B (zh) 2023-09-08

Family

ID=80707160

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111658153.1A Active CN114211498B (zh) 2021-12-30 2021-12-30 基于方向包围盒的锚杆支护机器人碰撞检测方法及系统

Country Status (1)

Country Link
CN (1) CN114211498B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115272379A (zh) * 2022-08-03 2022-11-01 杭州新迪数字工程系统有限公司 一种基于投影的三维网格模型外轮廓提取方法及系统
CN117095161A (zh) * 2023-10-20 2023-11-21 云南联合视觉科技有限公司 一种向量化编码的旋转目标检测方法及装置

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080034023A1 (en) * 2006-08-02 2008-02-07 Masakuni Nagano Contact geometry calculation device, contact geometry calculation method, and computer program product
US20180040101A1 (en) * 2015-11-30 2018-02-08 Tencent Technology (Shenzhen) Company Limited Method for detecting collision between cylindrical collider and convex body in real-time virtual scenario, terminal, and storage medium
CN107953334A (zh) * 2017-12-25 2018-04-24 深圳禾思众成科技有限公司 一种基于a星算法的工业机械臂无碰撞路径规划方法
US9981383B1 (en) * 2016-08-02 2018-05-29 X Development Llc Real-time trajectory generation for actuators of a robot to reduce chance of collision with obstacle(s)
CN108356819A (zh) * 2018-01-17 2018-08-03 西安交通大学 基于改进a*算法的工业机械臂无碰撞路径规划方法
CN108536880A (zh) * 2017-03-06 2018-09-14 上海盟云移软网络科技股份有限公司 一种虚拟现实系统的随机碰撞检测算法
CN109741461A (zh) * 2018-12-18 2019-05-10 南京东巴电子科技有限公司 三维动态模拟施工方案过程中实现碰撞检测的方法
CN111360824A (zh) * 2020-02-27 2020-07-03 中科新松有限公司 一种双臂自碰撞检测方法和计算机可读存储介质
CN112017237A (zh) * 2020-08-31 2020-12-01 北京轩宇智能科技有限公司 一种基于视场拼接和三维重建的操作辅助装置及方法
CN112669434A (zh) * 2020-12-21 2021-04-16 山东华数智能科技有限公司 一种基于网格与包围盒的碰撞检测方法
CN113051730A (zh) * 2021-03-15 2021-06-29 杭州电子科技大学 一种基于凸包和预判断的混合碰撞检测方法
CN113147590A (zh) * 2021-04-08 2021-07-23 中国铁建重工集团股份有限公司 一种隧道环境机械臂实时碰撞预警系统及方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080034023A1 (en) * 2006-08-02 2008-02-07 Masakuni Nagano Contact geometry calculation device, contact geometry calculation method, and computer program product
US20180040101A1 (en) * 2015-11-30 2018-02-08 Tencent Technology (Shenzhen) Company Limited Method for detecting collision between cylindrical collider and convex body in real-time virtual scenario, terminal, and storage medium
US9981383B1 (en) * 2016-08-02 2018-05-29 X Development Llc Real-time trajectory generation for actuators of a robot to reduce chance of collision with obstacle(s)
CN108536880A (zh) * 2017-03-06 2018-09-14 上海盟云移软网络科技股份有限公司 一种虚拟现实系统的随机碰撞检测算法
CN107953334A (zh) * 2017-12-25 2018-04-24 深圳禾思众成科技有限公司 一种基于a星算法的工业机械臂无碰撞路径规划方法
CN108356819A (zh) * 2018-01-17 2018-08-03 西安交通大学 基于改进a*算法的工业机械臂无碰撞路径规划方法
CN109741461A (zh) * 2018-12-18 2019-05-10 南京东巴电子科技有限公司 三维动态模拟施工方案过程中实现碰撞检测的方法
CN111360824A (zh) * 2020-02-27 2020-07-03 中科新松有限公司 一种双臂自碰撞检测方法和计算机可读存储介质
CN112017237A (zh) * 2020-08-31 2020-12-01 北京轩宇智能科技有限公司 一种基于视场拼接和三维重建的操作辅助装置及方法
CN112669434A (zh) * 2020-12-21 2021-04-16 山东华数智能科技有限公司 一种基于网格与包围盒的碰撞检测方法
CN113051730A (zh) * 2021-03-15 2021-06-29 杭州电子科技大学 一种基于凸包和预判断的混合碰撞检测方法
CN113147590A (zh) * 2021-04-08 2021-07-23 中国铁建重工集团股份有限公司 一种隧道环境机械臂实时碰撞预警系统及方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115272379A (zh) * 2022-08-03 2022-11-01 杭州新迪数字工程系统有限公司 一种基于投影的三维网格模型外轮廓提取方法及系统
CN115272379B (zh) * 2022-08-03 2023-11-28 上海新迪数字技术有限公司 一种基于投影的三维网格模型外轮廓提取方法及系统
CN117095161A (zh) * 2023-10-20 2023-11-21 云南联合视觉科技有限公司 一种向量化编码的旋转目标检测方法及装置
CN117095161B (zh) * 2023-10-20 2023-12-22 云南联合视觉科技有限公司 一种向量化编码的旋转目标检测方法及装置

Also Published As

Publication number Publication date
CN114211498B (zh) 2023-09-08

Similar Documents

Publication Publication Date Title
CN114211498A (zh) 基于方向包围盒的锚杆支护机器人碰撞检测方法及系统
CN109579831B (zh) 矿用悬臂式掘进机可视化辅助导引方法和系统
Lee Robots in the shipbuilding industry
CN110209183B (zh) 一种基于双阈值法zmp的六足机器人失稳调节算法
CN106643673B (zh) 一种基于基线的地铁隧道水平位移变形监测方法
CN111441810B (zh) 一种确定四柱式液压支架工作状态的方法
CN109115173A (zh) 基于直线定位模型的掘进机机身位姿单目视觉测量方法
CN112405541B (zh) 激光3d精密切割双机器人协同作业方法
CN105547166A (zh) 一种基于二维激光扫描的掘进机掘进窗口快速检测方法
CN106272429B (zh) 一种龙门式吊装机器人工作单元中附加轴运动规划方法
Tian et al. Kinematic models and simulations for trajectory planning in the cutting of Spatially-Arbitrary crosssections by a robotic roadheader
Wang et al. A control method for hydraulic manipulators in automatic emulsion filling
CN109115172B (zh) 基于机器视觉的采煤机机身位姿检测方法
CN104609207B (zh) 一种料场堆取料机防撞控制方法
CN114012726A (zh) 一种航天机械臂碰撞检测方法
Cui et al. Spiral trajectory planning approach for underground cavity measurements based on laser scanning
Cheng et al. Position measurement technology of boom-type roadheader based on binocular vision
CN112030994A (zh) 一种基于dynamo的深基坑支撑拆除方法
Silver et al. Feature extraction for topological mine maps
CN106080968A (zh) 一种船舶上层建筑无余量合拢施工工艺
CN116704019A (zh) 一种基于锚杆网络的钻锚机器人单目视觉定位方法
CN115847412B (zh) 一种基于分离框的异轨堆、取料机防碰撞运动规划方法
CN114782526B (zh) H型钢的焊缝轨迹计算方法、装置、电子设备及存储介质
Wardeh et al. Kinematic analysis of an under-actuated, closed-loop front-end assembly of a dragline manipulator
CN117621053B (zh) 一种用于带式输送机拆卸机器人的建模方法

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