[发明专利]一种多机械臂协同的快速自碰撞检测方法

权利要求书

1.一种多机械臂协同的快速自碰撞检测方法，其特征在于，其步骤如下：

步骤一：将多条机械臂各运动关节以及与之固连的结构件放大后等效为最大包络的长方体，并分别建立关节坐标系和长方体坐标系；

步骤二：根据机械臂运动关节的位置及关节坐标系之间、关节坐标系与长方体坐标系之间的关系，计算各长方体的位姿；

步骤三：根据步骤二中长方体的位姿，确定长方体在另一个长方体坐标系的顶点，根据顶点坐标和两个长方体在不同方向的投影是否重叠判断两个没有直接相连的长方体是否发生碰撞；

所述根据顶点坐标判断两个没有直接相连的长方体是否发生碰撞的方法为：

步骤S3.1：设长方体m的中心点上的坐标系为长方体坐标系a，长方体n的中心点上的坐标系为长方体坐标系b；则根据长方体m沿三条轴线方向的长度计算得到长方体m的8个顶点在长方体坐标系a中的第一坐标；

步骤S3.2：根据长方体坐标系a与长方体坐标系b之间的齐次变换矩阵和第一坐标，计算长方体m的8个顶点在长方体坐标系b中的第二坐标；

步骤S3.3：若第二坐标中的三坐标的任一最大值小于相应的第一坐标或第二坐标的三坐标的任一最小值大于相应的第一坐标，则长方体m与长方体n不发生碰撞；

步骤S3.4：交换长方体m和长方体n，重复执行步骤S3.31-S3.3；

步骤四：重复步骤三直到某两个长方体发生碰撞，否则多机械臂自身不碰撞。

2.根据权利要求1所述的多机械臂协同的快速自碰撞检测方法，其特征在于，所述关节坐标系为第i条机械臂的第j个关节的关节坐标系(i,j)；与关节坐标系(i,j)相对应的长方体坐标系为长方体坐标系(i,j′)，长方体坐标系(i,j′)为第i条机械臂的第j个关节的最大包络的长方体的中心点上的坐标系。

3.根据权利要求2所述的多机械臂协同的快速自碰撞检测方法，其特征在于，所述长方体坐标系(i,j′)的坐标轴方向与最大包络的长方体的棱线平行。

4.根据权利要求3所述的多机械臂协同的快速自碰撞检测方法，其特征在于，所述长方体坐标系(i,j′)在关节坐标系(i,j)中的位姿用齐次变换矩阵表示，最大包络的长方体沿长方体坐标系(i,j′)的三条轴线方向的长度表示为^i,j′L_x、^i,j′L_y和^i,j′L_z，其中，^i,j′L_x为沿x轴方向的长度，^i,j′L_y为沿y轴方向的长度，^i,j′L_z为沿z轴方向的长度。

5.根据权利要求4所述的多机械臂协同的快速自碰撞检测方法，其特征在于，所述步骤二中计算各长方体的位姿的方法是：基于第i条机械臂上当前各关节的位置，计算第i条机械臂的第j个关节的关节坐标系(i,j)在上一个关节坐标系(i,j-1)中的齐次变换矩阵为设第i条机械臂的基座在世界坐标系w中的齐次变换矩阵为则第i条机械臂的第j个关节的最大包络的长方体的长方体坐标系在世界坐标系w中的齐次变换矩阵为

6.根据权利要求1或5所述的多机械臂协同的快速自碰撞检测方法，其特征在于，所述步骤三中确定长方体在另一个长方体坐标系的顶点即计算步骤S3.2中第二坐标的方法为：其中，^bP_k为长方体m的第k个顶点在坐标系b中的第二坐标，^aP_k为长方体m的第k个顶点在长方体坐标系a中的第一坐标，k＝1,2,...8；为长方体坐标系a在长方体坐标系b中的齐次变换矩阵，且其中，为长方体坐标系b在世界坐标系w中的齐次变换矩阵，齐次变换矩阵为齐次变换矩阵的逆，为长方体坐标系a在世界坐标系w中的齐次变换矩阵，r₁₁-r₃₃为与旋转变换相关的元素，p_x、p_y、p_z为与平移变换相关的元素。