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

权利要求书

1.一种基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述方法包括：

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

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

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

若否，则继续工作；

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

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

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

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

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

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

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

所述转移矩阵为：

左钻架的虚拟方向包围盒的中心点与右钻架的虚拟方向包围盒的中心点之间的位移向量t⁰为：

t⁰＝P^R-P^L；

其中，表示左钻架的虚拟方向包围盒的中心位姿矩阵，n^L表示左钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，o^L表示左钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，a^L表示左钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，P^L表示左钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，e_L表示左钻架的虚拟方向包围盒的正半轴边长向量；表示右钻架的虚拟方向包围盒的中心位姿矩阵，n^R表示右钻架虚拟方向包围盒坐标系向量基n在0原点坐标系x、y、z坐标轴上的分量，o^R表示右钻架虚拟方向包围盒坐标系向量基o在0原点坐标系x、y、z坐标轴上的分量，a^R表示右钻架虚拟方向包围盒坐标系向量基a在0原点坐标系x、y、z坐标轴上的分量，P^R表示右钻架虚拟方向包围盒中心点P在0原点坐标系x、y、z坐标轴上的分量，e_R表示右钻架的虚拟方向包围盒的正半轴边长向量，为转移矩阵。

2.根据权利要求1所述的基于方向包围盒的锚杆支护机器人碰撞检测方法，其特征在于，所述分离轴检测公式具体为：

|T·L|＞r_L+r_R；

其中，T为左钻架的虚拟方向包围盒的中心坐标与右钻架的虚拟方向包围盒的中心坐标的距离矢量，L为分离轴方向矢量，|T·L|为距离矢量T在分离轴L上的投影，r_L为左钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影，r_R为右钻架的虚拟方向包围盒的中心半径在分离轴方向矢量L上的投影。