CN115958593A - 一种基于对偶向量的智能破拆机器人碰撞检测方法及系统、计算机存储介质 - Google Patents

Abstract

本发明公开了一种基于对偶向量的智能破拆机器人碰撞检测方法及系统、计算机存储介质，属于仿真技术领域。所述方法具体实施方式包括：将三维空间中的障碍物用空间几何模型进行建模；基于所建立的模型，将破拆机器人和障碍物简化为简单的几何结构；几何结构用多条线段来近似；将空间几何模型间的碰撞检测转换为线段与点或线段与线段间的碰撞检测。本碰撞检测方法分为粗略检测和精细检测两个阶段。优先进行粗略检测，若未发生碰撞，则检测结束；若发生碰撞，则进行精细检测，最终判断碰撞是否发生。由此，解决了现有技术中仅使用简单模型对于障碍物进行包络导致检测精度较低和仅使用精细模型进行检测导致计算负荷较大的技术问题，从而提高仿真中碰撞检测的实用性。

Description

一种基于对偶向量的智能破拆机器人碰撞检测方法及系统、计算机存储介质

技术领域

本发明涉及破拆机器人控制领域，尤其是一种基于对偶向量的智能破拆机器人碰撞检测方法及系统、计算机存储介质。

背景技术

智能破拆机器人实际的工作环境往往比较复杂，在复杂的环境中对目标物体完成破拆的同时需要防止机械臂与环境的物体发生碰撞。这就需要我们完成破拆头与目标物体的碰撞检测以及机械臂与环境的物体的碰撞检测。本节提出一种基于对偶向量的碰撞检测方法。

在碰撞检测方面国内外学者开展了一定相关的研究。碰撞检测算法可以分为两类，一类是静态碰撞检测算法，另一类是动态碰撞检测算法。静态碰撞检测算法旨在检测物体在静态状态下是否发生碰撞。例如，Uchiki提出了一种叫做空间占有法的碰撞检测算法。动态碰撞检测算法是针对物体是否在碰撞状态下发生位置变化。它可以分为基于物体空间的碰撞检测算法和基于图像空间的碰撞检测算法。基于物体空间的碰撞检测算法得到了广泛的应用，如空间分割法和层次包围盒。空间划分法的主要思想是将物体所在的空间划分为等体积的小空间，并利用这些小空间来近似物体。因此，空间物体间的碰撞检测可以转化为空间单元间的交叉检测。层次包围盒算法使用体积略大、几何特征简单的包围盒对复杂的物体进行近似描述。然后通过构建树形层次结构越来越接近物体的几何模型，直到几乎完全获得物体的几何特征。这样就只需对边界框的重叠部分进行进一步的交点测试。常用的边界框有球形包围盒、轴向包围盒和方向包围盒。球形包围盒的计算是最简单的，但精度较低。轴向包围盒和方向包围盒都提高了精确度，但仍难以满足更复杂障碍物的要求。Ketchel等人提出了一种圆柱形刚体碰撞检测方法。Kuan-chen等人使用球形和圆柱形近似来表示施工现场的施工机械和结构元素。

在以往的研究中，空间障碍物碰撞检测问题已得到一定的研究。然而，在这些三维空间障碍物建模和碰撞检测一般会采用简单的几何模型包络。这样将降低潜在碰撞的检测的精度而且安全工作空间被大大减小。相反，如果采用了相对精确的模型，则在障碍物回避过程中计算量总是很大，占用较多的计算机运算资源及设备的存储资源。此方法解决了现有技术中仅使用简单模型对于障碍物进行包络导致检测精度较低和仅使用精细模型进行检测导致计算负荷较大的技术问题，从而提高仿真中碰撞检测的实用性。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的是提供一种基于对偶向量的智能破拆机器人碰撞检测方法及系统、计算机存储介质，用于实现智能破拆机器人的碰撞检测。

本发明所采用的技术方案是：一种智能破拆机器人的碰撞检测方法，包括以下步骤：

将三维空间中的障碍物用空间几何模型进行建模；

基于所建立的模型，将破拆机器人和障碍物简化为简单的几何结构，几何结构用多条线段来近似；

将空间几何模型间的碰撞检测转换为线段与线段间的碰撞检测；

碰撞检测分为粗略检测和精细检测两个阶段；

粗略检测阶段将障碍物和智能破拆机器人简化为一条通过其几何中心线段，并将这条线段与较大的安全距离耦合；实时检测智能破拆机器人与障碍物所代表的线段的最短距离是否小于安全距离，若判断结果为否，则智能破拆机器人与障碍物未发生碰撞；

进一步的，若判断结果为是，则智能破拆机器人与障碍物可能发生碰撞；

精细检测阶段用多条线段组合的几何体来近似包络障碍物，用一带有安全距离线段代表智能破拆机器人；判断智能破拆机器人所简化的线段与障碍物所简化的空间几何模型之间的最短距离是否小于或等于安全距离，若判断结果为是，则智能破拆机器人与障碍物发生碰撞；

进一步的，若判断结果为否，则智能破拆机器人与障碍物发生碰撞未发生碰撞。

本发明所采用的另一技术方案是：一种智能破拆机器人的碰撞检测系统，包括

建建模单元，用于分别将空间中的智能破拆机器人和障碍物用空间几何模型进行建模；

模型转换单元，用于将破拆机器人和障碍物简化为简单的几何结构，几何结构用多条线段来近似；

粗略检测单元，实时检测智能破拆机器人与障碍物所代表线段间的最短距离是否小于安全距离，若判断结果为否，判断智能破拆机器人与障碍物未发生碰撞，检测结束；

进一步地，若判断结果为是，则智能破拆机器人与障碍物可能发生碰撞，执行精细检测；

精细检测单元，判断智能破拆机器人所简化的线段与障碍物所简化的空间几何模型之间的最短距离是否小于或等于安全距离，若判断结果为是，则智能破拆机器人与障碍物发生碰撞；若判断结果为否，则智能破拆机器人与障碍物发生碰撞未发生碰撞；

进一步地，若判断结果为否，判断智能破拆机器人与障碍物发生碰撞未发生碰撞。

本发明所采用的另一技术方案是：一种计算机存储介质，其上存储有计算机程序，所述程序被处理器执行时实现以下步骤：

将三维空间中的障碍物和破拆机器人用空间几何模型进行建模；

基于所建立的模型，将破拆机器人和障碍物简化为简单的几何结构，几何结构用多条线段来近似；

将空间几何模型间的碰撞检测转换为线段与线段间的碰撞检测；

碰撞检测分为粗略检测和精细检测两个阶段；

粗略检测阶段将障碍物和智能破拆机器人简化为一条通过其几何中心线段，并将这条线段与较大的安全距离耦合；实时检测智能破拆机器人与障碍物所代表线段间的最短距离是否小于安全距离，若判断结果为否，则智能破拆机器人与障碍物未发生碰撞；

进一步的，若判断结果为是，则智能破拆机器人与障碍物可能发生碰撞；

精细检测阶段用多条线段组合的几何体来近似包络障碍物，用一带有安全距离线段代表智能破拆机器人；判断智能破拆机器人所简化的线段与障碍物所简化的空间几何模型之间的最短距离是否小于或等于安全距离，若判断结果为是，则智能破拆机器人与障碍物发生碰撞；

进一步的，若判断结果为否，则智能破拆机器人与障碍物发生碰撞未发生碰撞。

本发明的有益效果是：

本发明一种能破拆机器人的碰撞检测方法及系统、计算机存储介质，将用于基于所建立的模型，将三维空间中的障碍物用空间几何模型进行建模；基于所建立的模型，将破拆机器人和障碍物简化为简单的几何结构；几何结构用多条线段来近似；将空间几何模型间的碰撞检测转换为线段与点或线段与线段间的碰撞检测。本碰撞检测方法分为粗略检测和精细检测两个阶段。优先进行粗略检测，若未发生碰撞，则检测结束；若发生碰撞，则进行精细检测，最终判断碰撞是否发生。使用多条线段包络障碍物，对于结构更加复杂的物体也可以使用多条线段完美包络。解决了现有技术中仅使用简单模型对于障碍物进行包络导致检测精度较低和仅使用精细模型进行检测导致计算负荷较大的技术问题，从而提高仿真中碰撞检测的实用性。

附图说明

后文将参照附图以示例性而非限制性的方式精细描述本发明的一些具体实施例。

附图中相同的附图标记标示了相同或类似的部件或部分。本领域技术人员应该理解，这些附图未必是按比例绘制的。附图中：

图1为本发明中提供的碰撞检测方法中计算点与线段间距离的示意图；

图2为本发明中提供的碰撞检测方法中计算线段与线段间距离的示意图；

图3为本发明中提供的碰撞检测方法中计算线段与平面几何体间距离的示意图；

图4为本发明中提供的碰撞检测方法中计算线段与曲面几何体间距离的示意图；

图5为本发明中提供的碰撞检测方法的流程示意图；

图6为本发明中提供的碰撞检测方法粗略检测的流程示意图；

图7为本发明中智能破拆机器人与障碍物进行碰撞检测的位置关系示意图。

具体实施方式

为使本发明的目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而非全部实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

针对智能破拆机器人在工作环境中的碰撞检测问题，提出基于对偶向量法碰撞检测策略，包括以下步骤：

将三维空间中的障碍物用空间几何模型进行建模；基于所建立的模型，将破拆机器人和障碍物简化为简单的几何结构；几何结构用多条线段来近似；将空间几何模型间的碰撞检测转换为线段与点或线段与线段间的碰撞检测。

参考图1为碰撞检测方法中计算点与线段间距离的示意图；用向量法确定点C到线段AB的最短距离，点A、B、和C的向量可记为x_A,x_B和x_C。C点相对于AB有三种可能的位置:C₁,C₂,和C₃。点C在线段AB上的投影是点D，我们通过确定向量AC在向量AB方向上的投影来区分这三种情况。

线段AB的方向向量可表示为:

C点的位置可以由ε确定，可描述为：

当ε≤0时，障碍物位于点C₁；当0＜ε＜1时，障碍物位于点C₂；当ε≥1时，障碍物位于点C₃；因此，根据距离ε的值，点与线段间最小距离为：

图2碰撞检测方法中计算线段与线段间距离的示意图；采用对偶向量法计算两段线段之间的最短距离，采用普朗克坐标和对偶向量来表示线段。对偶向量法非常适合于碰撞检测，因为它允许以一种相对简单的方式找到两个线段所在直线之间的公共垂直线的长度。

这两条线的相对位置如图2所示。直线S₁可以用点T₁和方向向量s表示，如下式所示：

本发明使用对偶向量表示线段，对偶向量代数中ε²＝0，定义如下方程：

线点乘表示为：

线叉乘表示为：

其中是和的公法线。上述操作提供了两行公法线的长度d₀。两条线在空间上的对偶角由实部和对偶部分组成；两条相交的线只有实部，而两条平行线只有虚部。因此，我们可以通过确定对偶角来确定这两条线的相对位姿。

首先，检查这两个线段，以确定它们是否平行。如果线段是平行的，则必须确定一个线段到另一个线段上的投影是以某种方式重叠还是完全不重叠。如果线段以某种方式重叠，则为它们之间的最短距离d₀；否则，最短的距离是最近的两个端点之间的距离。

我们首先找到公共法线与每个线段所在的线相交的位置。这两条线可以用线段的端点B₁,B₂和线段的向量来表示。因此，是两个线段的长度。公共垂N分别与S₁和S₂相交。公垂线在S₁,S₂上的点a₁,a₂的参数方程是方程为：

a₁＝B₁+t₁s (10)

a₁＝B₁+t₁s (11)

其中，

n＝s×w (14)

a的位置由t的大小决定。如果t≤0，则a位于线段的起点或之前；如果t≥1，则a位于线段的终点或之后，而0＜t＜1，则a位于线段上。线段之间的最短距离取决于垂直点的位置，因此确定如下。

如果a₁和a₂都在线段的两个端点内，则最短的距离为d_lmin＝d₀。

如果a₁和a₂不同时落在线段的两个端点内，则最短的距离是从线段的终点到另一个线段的最小距离

参考图3和图4为碰撞检测方法中计算线段与几何形状之间的最短距离的示意图；几何体是由面所包围的。最基本的几何形状属于以下两类之一：平面几何形状如棱锥体和棱柱体，二曲线几何形状，如球体、圆柱和圆锥。

参考图3计算线段与平面几何体间最短距离的示意图。首先是识别线段与几何体距离最短的平面。这个平面记为目标平面。

几何体外表面之间的交点线是几何形状的轮廓线，这些线段作为几何形状的关键线段，并用向量表示。计算这些轮廓线与目标线段NV之间的最短距离。距离目标线段NV距离最短的两个线段分别为AB和CD，如图3(b)所示，由它们确定目标平面。然后，通过线段AD、BC的中点形成新的线段IJ。之后分别计算线段AB、CD、IJ与目标线段NV之间的最短距离。这样依旧可以找到最短的两个线段，通过不断添加新的线段来减少目标平面的大小。

如果两个最短的线段相交，例如，图3中的线段CD和CF。由于他们的两个点重合，这个重叠点是一个新的线段的端点。例如，以DF的中点为k。在本例中，使用CK作为新检测到的线段。然后以同样的方式形成一个新的目标平面，以及一个新的线段CG。线段密度可以以上述相同的方式增加，直到达到所需的水平。最短的距离为d_gmin。立方体和其他类型的棱锥体也可以用这种方法来解决。

图4为计算曲面与线段之间的距离的示意图。然而，轮廓线不能用来表示几何曲面。球体和圆柱体与线段间距离的研究最广泛的。球通常用半径r_p的点表示，而圆柱体通常用半径r_l的线段表示。点与线段之间的最小距离d_p和线段之间的最小距离d_l的计算可以使用上文中的方法。

线段与球体之间的最短距离为：

d_gmin＝d_p-r_p (15)

线段与圆柱体之间的最短距离为：

d_gmin＝d_l-r_l (16)

在锥体表面，关键线段用四个均匀间隔的母线表示；在图4中，这些母线分别为AB、AC、AD和AE。计算了这四个线段与目标线段之间的最短距离。这表明，图4中最接近圆锥形障碍物的两个线段为AB和AC。然后，确定了圆弧BC的中点F。利用三个新的三个线段AB、AF和AH，可以找到距离最短的两个线段AF和AH，并找到新的目标面。如上所述，线密度可以不断增加，直到线段密度达到所需的水平。最终可以得到最短的距离是d_gmin。

参考图5为本发明中提供的碰撞检测方法的流程示意图；碰撞检测包括两个阶段：粗略检测阶段和精细检测阶段；粗糙检测在破拆机器人手臂远离障碍物或破拆机器人远离障碍物的情况下进行。障碍物通过其几何中心被简化为一个大安全距离r₀耦合的线段；实时检测智能破拆机器人与障碍物所代表的线段间最短距离是否小于安全距离，若判断结果为否，则智能破拆机器人与障碍物未发生碰撞；精细检测用多条线段组合的几何体来近似包络障碍物，用一带有安全距离线段代表智能破拆机器人；判断智能破拆机器人所简化的线段与障碍物所简化的空间几何模型之间的最短距离是否小于或等于安全距离，若判断结果为是，则智能破拆机器人与障碍物发生碰撞；

参考图6为本发明中提供的碰撞检测方法粗略检测的流程示意图。使用对偶向量的方法对于进行碰撞检测时非常有利的。它可以通过较为简单的方式求出两线段所在直线间的公垂线的距离以及他们之间的角度。如果得到的距离大于安全距离，那么不管两条线段的长度如何都将不会发生碰撞。如果直线间的距离小于安全距离。再判断两线段是否平行，若平行则必须确定一个线段到另一个线段上的投影是以某种方式重叠；如果线段以某种方式重叠，则必然发生碰撞；否则，碰撞没有发生。若不平行则通过公垂线在线段上的垂点a₁和a₂相对于线段的位置求出线段间的最短距离。若最短距离小于安全距离，则进行精细检测。这样，在不影响检测准确性的同时，可以尽量减少计算。

参考图7为本发明中智能破拆机器人与障碍物进行碰撞检测的位置关系示意图。图7(a)破拆机器人和障碍物的粗略检测，将障碍物和智能破拆机器人简化为一条通过其几何中心线段，并将这条线段与较大的安全距离r₀耦合；图7(b)破拆机器人和障碍物上轮廓线的精细检测，来确定下一步要检测的目标平面；图7(c)破拆机器人和障碍物上新生成线段的精细检测；破拆机器人不断与新生成的线段进行检测，直到符合精度要求。

基于上述方法，本发明还提供一种基于对偶向量的智能破拆机器人碰撞检测系统，包括：

建模单元，用于分别将空间中的智能破拆机器人和障碍物用空间几何模型进行建模；

模型转换单元，用于将破拆机器人和障碍物简化为简单的几何结构，几何结构用多条线段来近似；

粗略检测单元，实时检测智能破拆机器人与障碍物所代表线段间的最短距离是否小于安全距离，若判断结果为否，判断智能破拆机器人与障碍物未发生碰撞，检测结束；

进一步地，若判断结果为是，则智能破拆机器人与障碍物可能发生碰撞，执行精细检测；

精细检测单元，判断智能破拆机器人所简化的线段与障碍物所简化的空间几何模型之间的最短距离是否小于或等于安全距离，若判断结果为是，则智能破拆机器人与障碍物发生碰撞；若判断结果为否，则智能破拆机器人与障碍物发生碰撞未发生碰撞；

进一步地，若判断结果为否，判断智能破拆机器人与障碍物发生碰撞未发生碰撞。

一种基于对偶向量的智能破拆机器人碰撞检测系统的具体工作过程描述可参照上述一种基于对偶向量的智能破拆机器人碰撞检测方法的描述，在此不再赘述。

另外，一种计算机存储介质，其特征在于，其上存储有计算机程序，所述程序被处理器执行时实现以下步骤：

将三维空间中的障碍物用空间几何模型进行建模；

基于所建立的模型，将破拆机器人和障碍物简化为简单的几何结构，几何结构用多条线段来近似；

优先进行粗略检测，若未发生碰撞，则检测结束；

若发生碰撞，则进行精细检测，最终判断碰撞是否发生。

本发明是一种基于对偶向量的智能破拆机器人碰撞检测方法及系统、计算机存储介质。使用多条线段包络障碍物，对于结构更加复杂的物体也可以使用多条线段完美包络。解决了现有技术中仅使用简单模型对于障碍物进行包络导致检测精度较低和仅使用精细模型进行检测导致计算负荷较大的技术问题，从而提高仿真中碰撞检测的实用性。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (8)

1.一种基于对偶向量的智能破拆机器人碰撞检测方法，其特征在于，包括以下步骤：

将三维空间中的障碍物用空间几何模型进行建模；

基于所建立的模型，将破拆机器人和障碍物简化为简单的几何结构，几何结构用多条线段来近似；

将空间几何模型间的碰撞检测转换为线段与点或线段与线段间的碰撞检测；

碰撞检测分为粗略检测和精细检测两个阶段；

优先进行粗略检测，若未发生碰撞，则检测结束；若发生碰撞，则进行精细检测，最终判断碰撞是否发生。

2.根据权利要求1所述的智能破拆机器人的碰撞检测方法，其特征在于，所述碰撞检测分为粗略检测和精细检测两个阶段，包括：

针对所述虚拟仿真场景中的所有障碍物优先进行粗略检测，得到粗略检测的结果；针对粗略检测中可能发生碰撞的障碍物进行精细检测，得到精细检测的结果。

3.根据权利要求2所述方法，其特征在于，所述针对所述虚拟仿真场景中的所有障碍物优先进行粗略检测，得到粗略检测的结果，包括：

粗略检测阶段将障碍物和智能破拆机器人简化为一条通过其几何中心线段，并将这条线段与较大的安全距离耦合；实时检测智能破拆机器人与障碍物所代表的线段间最短距离是否小于安全距离，若判断结果为否，则智能破拆机器人与障碍物未发生碰撞；若判断结果为是，则智能破拆机器人与障碍物可能发生碰撞。

4.根据权利要求2所述方法，其特征在于，所述针对粗略检测中可能发生碰撞的障碍物进行精细检测，得到精细检测的结果，包括：

精细检测阶段用多条线段组合的几何体来近似包络障碍物，用一带有安全距离线段代表智能破拆机器人；判断智能破拆机器人所简化的线段与障碍物所简化的空间几何模型之间的最短距离是否小于或等于安全距离，若判断结果为是，则智能破拆机器人与障碍物发生碰撞；若判断结果为否，则智能破拆机器人与障碍物发生碰撞未发生碰撞。

5.根据权利要求1至4任一项所述的智能破拆机器人的碰撞检测方法，其特征在于，所述实时检测智能破拆机器人与障碍物所代表线段间的最短距离是否小于安全距离，包括：运用线几何和对偶向量代数来计算两条线短的最短距离，安全距离为所设定的距离。

6.根据权利要求4所述的智能破拆机器人的碰撞检测方法，所述判断智能破智能拆机器人所简化的线段与障碍物所简化的空间几何模型之间的最短距离，其特征在于，

将障碍物所简化的空间几何模型的轮廓线定为几何形状的关键线段；计算这些轮廓线与智能破拆机器人所简化的线段间的距离，距离最短的两条线段记为目标平面；通过这两条线段生成新的线段；计算这两条线段与新生成线段间的最短距离，这样依旧可以找到最短的两个线段并且生成新的目标平面，通过不断添加新的线段来减少目标平面的大小，直至达到精度要求。

7.一种基于对偶向量的智能破拆机器人碰撞检测系统，其特征在于，包括以下步骤：

建模单元，用于分别将三维空间中的智能破拆机器人和障碍物用空间几何模型进行建模；

模型转换单元，用于将破拆机器人和障碍物简化为简单的几何结构，几何结构用多条线段来近似；

粗略检测单元，实时检测智能破拆机器人与障碍物所代表线段间的最短距离是否小于安全距离，若判断结果为否，判断智能破拆机器人与障碍物未发生碰撞，检测结束；

进一步地，若判断结果为是，则智能破拆机器人与障碍物可能发生碰撞，执行精细检测；

精细检测单元，判断智能破拆机器人所简化的线段与障碍物所简化的空间几何模型之间的最短距离是否小于或等于安全距离，若判断结果为是，则智能破拆机器人与障碍物发生碰撞；若判断结果为否，则智能破拆机器人与障碍物发生碰撞未发生碰撞；

进一步地，若判断结果为否，判断智能破拆机器人与障碍物发生碰撞未发生碰撞。

8.一种计算机存储介质，其特征在于，其上存储有计算机程序，所述程序被处理器执行时实现以下步骤：

将三维空间中的障碍物用空间几何模型进行建模；

基于所建立的模型，将破拆机器人和障碍物简化为简单的几何结构，几何结构用多条线段来近似；

优先进行粗略检测，若未发生碰撞，则检测结束；

若发生碰撞，则进行精细检测，最终判断碰撞是否发生。

Images