[发明专利]基于方向包围盒的锚杆支护机器人碰撞检测方法及系统

说明书

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

技术领域

本发明属于锚杆支护机器人自动锚杆支护作业技术领域，特别是涉及一种基于方向包围盒的锚杆支护机器人碰撞检测方法及系统。

背景技术

锚杆支护机器人是煤矿矩形巷道支护的重要装备，对矩形巷道高效安全掘进具有巨大的意义。锚杆支护机器人碰撞检测技术是自动化锚杆支护的技术基础，锚杆支护机器人碰撞检测的主要任务是在锚杆支护机器人进行自动锚杆支护作业时检测锚杆支护机器人的钻架是否与机器人自身或者矩形巷道发生接触，进而避免锚杆支护机器人自动化作业时钻架与其他部件发生碰撞。

然而目前还没有应用于锚杆支护机器人碰撞检测的方法，这给锚杆支护机器人自动化作业带来了很大的安全隐患，为了保证锚杆支护机器人的安全工作，亟需一种锚杆支护机器人碰撞检测方法。

发明内容

本发明的目的是提供一种基于方向包围盒的锚杆支护机器人碰撞检测方法及系统，能够保证锚杆支护机器人自动作业时监测钻架是否与机器人自身发生碰撞，满足锚杆支护机器人安全高效作业的需求。

为实现上述目的，本发明提供了如下方案：

本发明提供了一种基于方向包围盒的锚杆支护机器人碰撞检测方法，所述方法包括：

分别建立锚杆支护机器人的左钻架的虚拟方向包围盒和右钻架的虚拟方向包围盒，并确定左钻架的虚拟方向包围盒的位姿信息和尺寸信息，确定右钻架的虚拟方向包围盒的位姿信息和尺寸信息；所述位姿信息包括虚拟方向包围盒的中心位姿矩阵；所述尺寸信息包括虚拟方向包围盒的边长尺寸信息；相邻所述边长的位置关系为垂直关系；

根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞；

若是，则重新规划所述锚杆支护机器人的运动路径；

若否，则继续工作。

可选地，所述根据所述左钻架的虚拟方向包围盒的位姿信息和尺寸信息，所述右钻架的虚拟方向包围盒的位姿信息和尺寸信息，以及分离轴检测公式，判断左钻架和右钻架是否发生碰撞，具体包括：

根据左钻架的虚拟方向包围盒的中心位姿矩阵和右钻架的虚拟方向包围盒的中心位姿矩阵计算转移矩阵，以及计算左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量；所述转移矩阵为右钻架的虚拟方向包围盒在左钻架的虚拟方向包围盒坐标系下的位姿矩阵；

将所述转移矩阵、所述左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量、左钻架的虚拟方向包围盒的尺寸信息以及右钻架的虚拟方向包围盒的尺寸信息，代入分离轴检测公式，以判断左钻架和右钻架是否发生碰撞。

可选地，所述左钻架的虚拟方向包围盒的中心位姿矩阵为：

所述左钻架的虚拟方向包围盒的尺寸信息为

所述右钻架的虚拟方向包围盒的中心位姿矩阵为：

所述右钻架的虚拟方向包围盒的尺寸信息为

所述转移矩阵为：