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

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

Info

Publication number
CN114211498B
CN114211498B CN202111658153.1A CN202111658153A CN114211498B CN 114211498 B CN114211498 B CN 114211498B CN 202111658153 A CN202111658153 A CN 202111658153A CN 114211498 B CN114211498 B CN 114211498B
Authority
CN
China
Prior art keywords
bounding box
virtual direction
drilling frame
direction bounding
frame
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
CN202111658153.1A
Other languages
English (en)
Other versions
CN114211498A (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

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

基于方向包围盒的锚杆支护机器人碰撞检测方法及系统
技术领域
本发明属于锚杆支护机器人自动锚杆支护作业技术领域，特别是涉及一种基于方向包围盒的锚杆支护机器人碰撞检测方法及系统。
背景技术
锚杆支护机器人是煤矿矩形巷道支护的重要装备，对矩形巷道高效安全掘进具有巨大的意义。锚杆支护机器人碰撞检测技术是自动化锚杆支护的技术基础，锚杆支护机器人碰撞检测的主要任务是在锚杆支护机器人进行自动锚杆支护作业时检测锚杆支护机器人的钻架是否与机器人自身或者矩形巷道发生接触，进而避免锚杆支护机器人自动化作业时钻架与其他部件发生碰撞。
然而目前还没有应用于锚杆支护机器人碰撞检测的方法，这给锚杆支护机器人自动化作业带来了很大的安全隐患，为了保证锚杆支护机器人的安全工作，亟需一种锚杆支护机器人碰撞检测方法。
发明内容
本发明的目的是提供一种基于方向包围盒的锚杆支护机器人碰撞检测方法及系统，能够保证锚杆支护机器人自动作业时监测钻架是否与机器人自身发生碰撞，满足锚杆支护机器人安全高效作业的需求。
为实现上述目的，本发明提供了如下方案：
本发明提供了一种基于方向包围盒的锚杆支护机器人碰撞检测方法，所述方法包括：
分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系；
根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；
若是，则重新规划所述锚杆支护机器人的运动路径；
若否，则继续工作。
可选地，所述根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞，具体包括：
根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
可选地，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
所述左钻架的虚拟方向包围盒的尺寸信息为
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
所述右钻架的虚拟方向包围盒的尺寸信息为
所述转移矩阵为：
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，/>PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eL表示左钻架的虚拟方向包围盒的正半轴边长向量；/>表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，/>aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eR表示右钻架的虚拟方向包围盒的正半轴边长向量，/>为转移矩阵。
可选地，所述分离轴检测公式具体为：
|T·L|＞rL+rR；
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影。
可选地，所述将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞，具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和；
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
可选地，所述方法还包括：
获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
为实现上述目的，本发明还提供了一种基于方向包围盒的锚杆支护机器人碰撞检测系统，所述系统包括：
包围盒建立和信息确定模块，用于分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻边长的位置关系为垂直关系；
碰撞检测模块，用于根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；在左钻架和右钻架发生碰撞时，重新规划所述锚杆支护机器人的运动路径；在左钻架和右钻架未发生碰撞时，继续工作。
可选地，所述碰撞检测模块，具体包括：
转移矩阵和位移向量计算单元，用于根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
碰撞检测单元，用于将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
可选地，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
所述左钻架的虚拟方向包围盒的尺寸信息为
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
所述右钻架的虚拟方向包围盒的尺寸信息为
所述转移矩阵为：
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，/>PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eL表示左钻架的虚拟方向包围盒的正半轴边长向量；/>表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，/>aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eR表示右钻架的虚拟方向包围盒的正半轴边长向量，/>为转移矩阵；
所述分离轴检测公式具体为：
|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三维空间分离轴检测计算公式
鉴于以上内容，本发明提供了一种基于方向包围盒的锚杆支护机器人碰撞检测方法，如图3所示，包括以下步骤：
S1：分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系。锚杆支护机器人碰撞检测的方向包围盒示意图如图4和图5。
具体地，虚拟方向包围盒为矩形盒，使用矩形盒完全包围钻架，钻架的虚拟方向包围盒由中心点相对锚杆支护机器人的坐标、方向矩阵和三条互相垂直的棱边的半边长表示，即虚拟方向包围盒的中心位姿矩阵(12个浮点数)以及沿基向量表示正区间范围的3个实数(共15个浮点数表示方向包围盒)，描述矩形盒的位姿，得到钻架的虚拟方向包围盒。其中，虚拟方向包围盒的中心位姿矩阵由锚杆支护机器人的运动学模型计算得到。通过建立钻架的虚拟方向包围盒，能够通过检测虚拟方向包围盒相交来代替对真实物体碰撞检测，降低了碰撞检测的计算成本。
S2：根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞。图6为左右钻架碰撞检测示意图。
进一步地，S2具体包括：
S21：根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵。
S22：将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
其中，根据方向包围盒分离轴检测原理的计算规则要求，为了减少计算量，将右钻架虚拟方向包围盒位姿矩阵转换到左钻架虚拟方向包围盒坐标系下表示。使左钻架方向包围盒中心成为坐标系原点(零点)，省去了左钻架碰撞检测时的矩阵计算量，提高了计算效率。
具体地，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
所述左钻架的虚拟方向包围盒的尺寸信息为由于虚拟方向包围盒包围盒关于中心点对称，只用单位基向量正半轴即可表示整个方向包围盒姿态。和/>分别为左钻架虚拟方向包围盒的半边长。
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
所述右钻架的虚拟方向包围盒的尺寸信息为 和/>分别为右钻架虚拟方向包围盒的半边长。
所述转移矩阵为：
其中，旋转矩阵为：
中心位移向量为：
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL。
其中，表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，/>PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eL表示左钻架的虚拟方向包围盒的正半轴边长向量；/>表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，/>aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eR表示右钻架的虚拟方向包围盒的正半轴边长向量，/>为转移矩阵。
进一步地，将所述转移矩阵转换为绝对值形式，得到转移矩阵的绝对值。
对所述转移矩阵的绝对值增加ε项进行修正，得到修正后的转移矩阵。增加ε项进行修正，从而避免坐标轴平行时求叉积产生计算错误。
其中，提取旋转矩阵，旋转矩阵的绝对值形式为
更进一步地，所述分离轴检测公式具体为：
|T·L|＞rL+rR。
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影。
进一步地，步骤S22具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和；
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
进一步地，左右钻架的15条分离轴对应的检测公式如下所示：
(1)分离轴第一距离即分离轴投影为|T·L|＝|t0|；第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影
(2)分离轴第一距离即分离轴投影为|T·L|＝|t1|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影
(3)分离轴第一距离即分离轴投影|T·L|＝|t2|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影
(4)分离轴第一距离即分离轴投影|T·L|＝|t0r00+t1r10+t2r20|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(5)分离轴第一距离即分离轴投影|T·L|＝|t0r01+t1r11+t2r21|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(6)分离轴第一距离即分离轴投影|T·L|＝|t0r02+t1r12+t2r22|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(7)分离轴第一距离即分离轴投影|T·L|＝|t2r10-t1r20|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(8)分离轴第一距离即分离轴投影|T·L|＝|t2r11-t1r21|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(9)分离轴第一距离即分离轴投影|T·L|＝|t2r12-t1r22|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(10)分离轴第一距离即分离轴投影|T·L|＝|t0r20-t2r00|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(11)分离轴第一距离即分离轴投影|T·L|＝|t0r21-t2r01|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(12)分离轴第一距离即分离轴投影|T·L|＝|t0r22-t2r02|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(13)分离轴第一距离即分离轴投影|T·L|＝|t1r00-t0r10|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(14)分离轴第一距离即分离轴投影|T·L|＝|t1r01-t0r11|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
(15)分离轴第一距离即分离轴投影|T·L|＝|t1r02-t0r12|，第二距离为rL+rR，其中，左钻架虚拟方向包围盒的中心半径投影/>右钻架虚拟方向包围盒的中心半径投影/>
其中，分离轴和/>分别为左钻架虚拟方向包围盒的三个基向量对应的分离轴；分离轴/>和/>分别为右钻架虚拟方向包围盒的三个基向量对应的分离轴；分离轴和/>分别为左钻架虚拟方向包围盒与右钻架虚拟方向包围盒的基向量组合对应的分离轴。
S3：若是，则重新规划所述锚杆支护机器人的运动路径。
S4：若否，则继续工作。
进一步地，所述方法还包括：基于虚拟方向包围盒对锚杆支护机器人钻架与矩形巷道碰撞检测步骤，具体包括：
获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
所述分离轴检测公式为|T·L|＞rF+rT，根据分离轴检测公式结果判断钻架是否与矩形巷道发生碰撞。式中：rF为钻架虚拟方向包围盒的中心半径投影，rT为矩形巷道的中心半径投影。
进一步地，本发明基于方向包围盒的锚杆支护机器人碰撞检测系统，包括：包围盒建立和信息确定模块和碰撞检测模块。
包围盒建立和信息确定模块，用于分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系。
碰撞检测模块，用于根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；在左钻架和右钻架发生碰撞时，重新规划所述锚杆支护机器人的运动路径；在左钻架和右钻架未发生碰撞时，继续工作。
进一步地，所述碰撞检测模块，具体包括：
转移矩阵和位移向量计算单元，用于根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
碰撞检测单元，用于将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。
具体地，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
所述左钻架的虚拟方向包围盒的尺寸信息为
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
所述右钻架的虚拟方向包围盒的尺寸信息为
所述转移矩阵为：
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，/>PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eL表示左钻架的虚拟方向包围盒的正半轴边长向量；/>表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，/>aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eR表示右钻架的虚拟方向包围盒的正半轴边长向量，/>为转移矩阵。
所述分离轴检测公式具体为：
|T·L|＞rL+rR；
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影。
所述碰撞检测单元，具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和。
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
进一步地，所述系统还包括：
巷道碰撞检测模块，用于获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
下面结合具体实施例，对本发明方法做进一步详细说明。
实施例
根据锚杆支护机器人部件外轮廓，建立锚杆支护机器人部件的虚拟方向包围盒，如图4所示，1为锚杆支护机器人，2为虚拟方向包围盒；锚杆支护机器人钻架虚拟方向包围盒如图5所示，3为钻架。钻架虚拟方向包围盒正半轴边长向量为：
定义整车坐标系在左右工作臂基坐标系OL0和OR0中间，左钻架中心O7L相对锚杆支护机器人工作臂基坐标系的坐标为(xL7,yL7,zL7)，右钻架中心O7R相对锚杆支护机器人工作臂基坐标系的坐标为(xR7,yR7,zR7),根据矩阵变换，得到左、右钻架虚拟方向包围盒中心位姿矩阵和/>
令右钻架静止，即关节变量θ1＝θ2＝d3＝θ4＝θ5＝θ6＝0，左钻架旋转-30°，即关节变量θ1＝θ2＝d3＝θ4＝θ5＝0，θ6＝-30°，对应于左钻架从竖直向水平转换。
左钻架虚拟方向包围盒中心位姿矩阵为：
右钻架虚拟方向包围盒中心位姿矩阵为：
进而得到右钻架虚拟方向包围盒在左钻架虚拟方向包围盒坐标系下的转移矩阵为：
/>
取ε＝0.05，转移矩阵的绝对值为：
由此依次计算15个分离轴的检测公式，可得15个分离轴检测公式的结果都不满足|T·L|＞rL+rR，因此左右钻架相交，发生了碰撞。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (6)

1.一种基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述方法包括：
分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系；
根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；
若是，则重新规划所述锚杆支护机器人的运动路径；
若否，则继续工作；
所述根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞，具体包括：
根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞；
其中，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
所述左钻架的虚拟方向包围盒的尺寸信息为
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
所述右钻架的虚拟方向包围盒的尺寸信息为
所述转移矩阵为：
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，/>aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eL表示左钻架的虚拟方向包围盒的正半轴边长向量；/>表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，/>aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，/>PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，eR表示右钻架的虚拟方向包围盒的正半轴边长向量，/>为转移矩阵。
2.根据权利要求1所述的基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述分离轴检测公式具体为：
|T·L|＞rL+rR；
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影。
3.根据权利要求2所述的基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞，具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和；
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
4.根据权利要求1所述的基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述方法还包括：
获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
5.一种基于方向包围盒的锚杆支护机器人碰撞检测系统，其特征在于，所述系统包括：
包围盒建立和信息确定模块，用于分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系；
碰撞检测模块，用于根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；在左钻架和右钻架发生碰撞时，重新规划所述锚杆支护机器人的运动路径；在左钻架和右钻架未发生碰撞时，继续工作；
所述碰撞检测模块，具体包括：
转移矩阵和位移向量计算单元，用于根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；
碰撞检测单元，用于将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞；
其中，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：
所述左钻架的虚拟方向包围盒的尺寸信息为
所述右钻架的虚拟方向包围盒的中心位姿矩阵为：
所述右钻架的虚拟方向包围盒的尺寸信息为
所述转移矩阵为：
左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t0为：
t0＝PR-PL；
其中，表示左钻架的虚拟方向包围盒的中心位姿矩阵，nL表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oL表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，/>aL表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，PL表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，/>eL表示左钻架的虚拟方向包围盒的正半轴边长向量；/>表示右钻架的虚拟方向包围盒的中心位姿矩阵，nR表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，/>oR表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，/>aR表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，/>PR表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，eR表示右钻架的虚拟方向包围盒的正半轴边长向量，/>为转移矩阵；
所述分离轴检测公式具体为：
|T·L|＞rL+rR；
其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，rL为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，rR为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影；
所述碰撞检测单元，具体包括：
将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，计算第一距离和第二距离；所述第一距离为距离矢量T在分离轴L上的投影；所述第二距离为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影与右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影的和；
基于所述第一距离和所述第二距离，判断左钻架和右钻架是否发生碰撞；其中，当所述第一距离大于所述第二距离时，左钻架和右钻架未发生碰撞；当所述第一距离小于等于所述第二距离时，左钻架和右钻架发生碰撞。
6.根据权利要求5所述的基于方向包围盒的锚杆支护机器人碰撞检测系统，其特征在于，所述系统还包括：
巷道碰撞检测模块，用于获取锚杆支护机器人所在巷道的侧帮相对于锚杆支护机器人的坐标、巷道的顶板相对于锚杆支护机器人的坐标和巷道的底板相对于锚杆支护机器人的坐标，并将所述侧帮相对于锚杆支护机器人的坐标、顶板相对于锚杆支护机器人的坐标和底板相对于锚杆支护机器人的坐标代入所述分离轴检测公式，以判断钻架是否与所在巷道发生碰撞。
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 CN114211498A (zh) 2022-03-22
CN114211498B true 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)

Families Citing this family (2)

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

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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 中国铁建重工集团股份有限公司 一种隧道环境机械臂实时碰撞预警系统及方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4192976B2 (ja) * 2006-08-02 2008-12-10 ソニー株式会社 接触形状算出装置及び接触形状算出方法、並びにコンピュータ・プログラム
CN105512377B (zh) * 2015-11-30 2017-12-12 腾讯科技（深圳）有限公司 实时虚拟场景中圆柱碰撞体与凸体碰撞探测的方法和系统

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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 中国铁建重工集团股份有限公司 一种隧道环境机械臂实时碰撞预警系统及方法

Also Published As

Publication number Publication date
CN114211498A (zh) 2022-03-22

Similar Documents

Publication Publication Date Title
CN114211498B (zh) 基于方向包围盒的锚杆支护机器人碰撞检测方法及系统
CN108286949B (zh) 一种可移动式三维检测机器人系统
CN111360824B (zh) 一种双臂自碰撞检测方法和计算机可读存储介质
CN112223291A (zh) 一种基于三维任务空间约束的机械臂避障方法及装置
CN112405541B (zh) 激光3d精密切割双机器人协同作业方法
CN106272429B (zh) 一种龙门式吊装机器人工作单元中附加轴运动规划方法
CN105547166A (zh) 一种基于二维激光扫描的掘进机掘进窗口快速检测方法
CN111203916B (zh) 一种机器人关节与几何体碰撞的检测方法
CN106643673A (zh) 一种基于基线的地铁隧道水平位移变形监测方法
CN108481324A (zh) 一种八轴多功能机械臂的逆解工程及其碰撞检测算法
JP4262715B2 (ja) ロボットのティーチングデータ作成方法
CN113848892B (zh) 一种机器人清扫区域划分方法、路径规划方法及装置
CA2962670A1 (en) Position monitoring of a kinematic linkage
CN103077310A (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
CN108628184B (zh) 一种用于机器人仿真系统的碰撞检测方法
CN113772363B (zh) 一种刮板输送机位姿模型建立方法和系统
CN102513644A (zh) 带有结构光检测的三割炬机器人h型钢切割装置
JPS61127007A (ja) ロボツトとワ−クの干渉チエツク方式
CN114782526B (zh) H型钢的焊缝轨迹计算方法、装置、电子设备及存储介质
CN115847412B (zh) 一种基于分离框的异轨堆、取料机防碰撞运动规划方法
Guo et al. RESEARCH ON MODELING AND COMPENSATION METHOD FOR GROUND LEVEL ERROR BASED ON MOBILE MANIPULATOR
Mahboubkhah et al. Workspace analysis of 2-pr (pa) u-2-pr (pa) r new parallel mechanism
US20220194245A1 (en) Robust Docking of Robots with Imperfect Sensing

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