CN112720493B - 一种机械臂与障碍物的双层次碰撞检测算法 - Google Patents

Abstract

本发明属于机器人控制和碰撞检测领域，具体涉及一种机械臂与障碍物的双层次碰撞检测算法，包括下列步骤：通过选取物体上最远两点的距离作为直径来获得障碍物球体模型；根据障碍物自身的几何条件生成紧凑长方体包围盒OBB；计算最大运动轨迹的长度；计算q_a、q_b状态时机械臂与障碍物球体模型的直线距离；对机械臂与障碍物球体模型的碰撞情况进行初次判断；对机械臂与障碍物球体模型的碰撞情况进行二次判断。本发明检测过程中无需附加任何传感器，对机器人的结构无需任何修改，适用于机器人的任何运行工况，对工况没有任何限制，成本低，且程序简单，执行效率高。本发明用于机械臂与障碍物碰撞的检查。

Description

一种机械臂与障碍物的双层次碰撞检测算法

技术领域

本发明属于机器人控制和碰撞检测领域，具体涉及一种机械臂与障碍物的双层次碰撞检测算法。

背景技术

目前常用的检测方法有传感器法和几何体模型法。传感器法的检测效果易受环境影响，应用成本高。Lumelsky.V.J等提出在机械臂上布置感知皮肤来检测碰撞，这种方式虽然能够检测出所有碰撞，但检测过程复杂，需要处理的数据很多，检测速度过度依赖于处理器的性能。比较而言，几何体模型法主要思想是计算机械臂运动轨迹插补点位置与障碍物几何体模型位置的距离来判断是否碰撞，检测结果更可靠。陈友东等提出将机械臂的关节和连杆等效成球体和胶囊体，将碰撞问题转换为距离计算问题，一定程度上简化了检测过程。这种方法的弊端在于若选择检测的插补点间隔数目过大会损失检测精度，遗漏碰撞情况；若待检插补点间隔数目过少，会增加碰撞检测的时间，特别是当机械臂与障碍物距离较远时，这种方法的效率很低。因此，连续碰撞检测的概念受到广泛关注。F.Schwarzer等提出了一种自适应动态碰撞检测方法，通过估算运动轨迹的长度判断机械臂与障碍物的碰撞情况；金明杰等将其加入到运动规划过程，提高了路径规划过程的完备性。这种方法对障碍物的特定模型进行判断，能够检测出连续运动轨迹是否与障碍物发生碰撞，虽然速度更快，但检测精度有待提高。

发明内容

针对上述常用的检测方法检测过程复杂、效率低、检测精度低的技术问题，本发明提供了一种成本低、效率高、检测精度高的机械臂与障碍物的双层次碰撞检测算法。

为了解决上述技术问题，本发明采用的技术方案为：

一种机械臂与障碍物的双层次碰撞检测算法，包括下列步骤：

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

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

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

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

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

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

所述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)的长度的方法为：

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

所述q_a,k、q_b,k代表第k个关节运动的始、末状态的角度参数，所述

是一个常数，所述

代表第k个关节连接的各机械臂长度之和。

所述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中对机械臂与障碍物球体模型的碰撞情况进行初次判断的方法为：

若λ_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中对机械臂与障碍物球体模型的碰撞情况进行二次判断的方法为：

障碍物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与障碍物碰撞，否则该点不会与障碍物碰撞；

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

本发明与现有技术相比，具有的有益效果是：

本发明所提出的一种机器人关节与几何体碰撞检测方法，在检测过程中无需附加任何传感器，对机器人的结构无需任何修改，适用于机器人的任何运行工况，对工况没有任何限制，成本低，且程序简单，执行效率高。本发明的方法不仅可以应用于机器人关节末端连接的几何体与各关节间的碰撞检测，也可应用于机器人关节与外界环境空间中固定几何体的碰撞检测。

附图说明

图1为本发明的轨迹点与OBB投影示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要理解的是，术语“中心”、“上”、“下”、“前”、“后”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述发明和简化描述，而不是指示或暗示所指的组合或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

一种机械臂与障碍物的双层次碰撞检测算法，包括下列步骤：

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

步骤2、根据障碍物自身的几何条件生成紧凑长方体包围盒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的三条边长。

步骤3、计算最大运动轨迹λ_i(q_q,q_b)的长度。

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

其中：q_a,k、q_b,k代表第k个关节运动的始、末状态的角度参数，

是一个常数，

代表第k个关节连接的各机械臂长度之和。

步骤4、计算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)的计算公式如下：

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

若λ_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)轨迹不会与障碍物发生碰撞。

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

障碍物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轴作为分离轴，如图1所示，将OBB模型与轨迹点分别向X轴、Y轴、Z轴投影，下式成立则判断轨迹点M与障碍物碰撞，否则该点不会与障碍物碰撞；

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

上面仅对本发明的较佳实施例作了详细说明，但是本发明并不限于上述实施例，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化，各种变化均应包含在本发明的保护范围之内。

Claims (1)

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与障碍物碰撞，否则该点不会与障碍物碰撞；

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

Images