[发明专利]一种机械臂与障碍物的双层次碰撞检测算法

权利要求书

1.一种机械臂与障碍物的双层次碰撞检测算法，其特征在于：包括下列步骤：

S1、通过选取物体上最远两点的距离作为直径来获得障碍物球体模型；

S2、根据障碍物自身的几何条件生成紧凑长方体包围盒OBB；

所述S2中生成紧凑长方体包围盒OBB的方法为：

包围盒OBB是一个任意方向的长方体，用一个中心点、一个三阶方向矩阵和三个1/2边长表示，所述三阶方向矩阵表示包围盒OBB三条轴的方向，通过计算包围盒OBB内的全部三角形顶点的协方差矩阵C以及矩阵C三个特征向量，得到OBB的三条轴的方向，

假设模型中包含n个三角形，第i个三角形的顶点矢量分别用p_i、q_i、r_i表示，则用下面公式得到这n个三角形顶点的分布均值u，即模型的质心：

然后由u得到协方差矩阵C：

其中，1≤j，k≤3，

通过上式得到的协方差矩阵C为一个3*3的对称矩阵，将协方差矩阵C的三个特征向量正规化之后得到的基即为包围盒OBB的三条轴的方向，最后计算包围盒OBB内的三角形的顶点在这三条轴上投影的最大值和最小值即可确定包围盒OBB的三条边长；

S3、计算最大运动轨迹λ_i(q_q,q_b)的长度；

所述S3中计算最大运动轨迹λ_i(q_q,q_b)的长度的方法为：

当机械臂从q_a状态运动到q_b状态时，第i轴运动轨迹的最大长度为：

所述q_a,k、q_b,k代表第k个关节运动的始、末状态的角度参数，所述是一个常数，所述代表第k个关节连接的各机械臂长度之和；

S4、计算q_a、q_b状态时机械臂与障碍物球体模型的直线距离η_ij(q_a)、η_ij(q_b)；

所述S4中计算q_a、q_b状态时机械臂与障碍物球体模型的直线距离η_ij(q_a)、η_ij(q_b)的方法为：

获取物体上最远两点的空间坐标，以两点连线的中心点作为球心，以两点连线的距离作为直径生成障碍物球体模型；

设q_a、q_b状态时的末端坐标分别为(x_a,y_a,z_a)，(x_b,y_b,z_b)，障碍物球体模型的半径为R、球心坐标为(x₀,y₀,z₀)，则η_ij(q_a)、η_ij(q_b)的计算公式如下：

S5、对机械臂与障碍物球体模型的碰撞情况进行初次判断；

所述S5中对机械臂与障碍物球体模型的碰撞情况进行初次判断的方法为：

若λ_i(q_a,q_b)≥η_ij(q_a)+η_ij(q_b)，判断：

若q_a到q_b中的轨迹插补点数目大于2，则选取q_a与q_b之间的中间点q_mid，对(q_a,q_mid)、(q_mid,q_b)按上述步骤重复判断；若q_a到q_b中的轨迹插补点数目小于等于2，(q_a,q_b)之间可能与障碍物发生碰撞，进行二次判断；

若λ_i(q_a,q_b)η_ij(q_a)+η_ij(q_b)，则(q_a,q_b)轨迹不会与障碍物发生碰撞；

S6、对机械臂与障碍物球体模型的碰撞情况进行二次判断；

所述S6中对机械臂与障碍物球体模型的碰撞情况进行二次判断的方法为：

障碍物OBB模型的长度和宽度均为2d，障碍物OBB模型的高度为h，障碍物OBB模型上、下端面的中心点A、B的坐标分别为(x_A,y_A,z_A)，(x_B,y_B,z_B)，轨迹点M坐标为(x₁,y₁,z₁)，以世界坐标系中的X轴、Y轴、Z轴作为分离轴，将OBB模型与轨迹点分别向X轴、Y轴、Z轴投影，下式成立则判断轨迹点M与障碍物碰撞，否则该点不会与障碍物碰撞；

若待检轨迹段内任一轨迹点检测碰撞，得出机械臂会与障碍物碰撞的结论；否则该轨迹段不会与障碍物发生碰撞，当所有可疑轨迹段都判断为不碰撞情况时，得出最终结论：机械臂的规划轨迹不会与障碍物发生碰撞。