一种机械臂碰撞检测方法

【技术领域】

本发明涉及机械臂运动仿真领域，属于虚拟仿真场景中机械臂碰撞检测方法。

【背景技术】

21世纪以来，机器人技术被认为是对未来新兴产业发展具有重要意义的高新技术之一，国内外产业界对机器人技术研究给予极大的重视。由于机械臂的应用环境往往很复杂，提前对机械臂的运动进行仿真，能最大程度减少机械臂与环境碰撞或自身碰撞的情况发生。因此国内外学者对碰撞检测算法展开了大量研究，该类算法的主要任务是及时检测虚拟仿真环境中的物体是否发生了接触，并且能在虚拟仿真系统中提示发生接触的具体物体模型信息。由此可见，碰撞检测算法是机械臂运动仿真领域不可或缺的一个部分。

现有用于机械臂运动仿真中的碰撞检测算法种类较多，空间分割法和包围盒技术是现阶段应用较为广泛的算法，但是当仿真场景庞大且仿真对象较多时，构造场景的基本几何元素总数也会增加，仅使用单一的碰撞检测方法将很难同时满足检测的准确性和实时性。

【发明内容】

有鉴于此，本发明提供了一种机械臂碰撞检测方法，以提高机械臂运动仿真中碰撞检测算法的准确性和实时性。

本发明实施例提供了一种机械臂碰撞检测方法，包括：

利用表面栅格化技术分别对虚拟仿真场景模型和机械臂模型进行预处理，获得虚拟仿真场景模型对应的点云文件和机械臂模型对应的点云文件；

利用八叉树分割技术对所得点云文件分别进行空间分割，以确定每个点云所处的子空间位置，得到点云文件的八叉树空间；

利用球包围盒和方向包围盒对所得八叉树空间进行层次包裹，得到混合包围盒树；

利用深度优先搜索算法和双向广度搜索算法，设计机械臂单一构型碰撞检测算法，并基于机械臂单一构型碰撞检测算法，得到机械臂单一构型碰撞检测结果；

利用机械臂运动特性和动态规划算法，设计机械臂连续运动碰撞检测算法，并基于机械臂连续运动碰撞检测算法，得到机械臂连续运动时碰撞检测结果。

上述方法中，所述利用表面栅格化技术分别对虚拟仿真场景模型和机械臂模型进行预处理，包括：

利用OSG三维引擎的线段求交方法及预设的扫描精度，对虚拟仿真场景模型的表面进行散点采样，得到虚拟仿真场景模型对应的散点的点云文件。

利用OSG三维引擎的线段求交方法及预设的扫描精度，对机械臂模型的表面进行散点采样，得到机械臂模型对应的散点的点云文件。

上述方法中，所述利用八叉树分割技术对所得点云文件分别进行空间分割，已确定每个点云所处的子空间位置，得到点云文件的八叉树空间之前，还包括：

判断待分割空间的点云数量是否小于第一指定数量；

如果空间内点云数量小于第一指定数量，不再对该空间进行分割；

如果空间内点云数量不小于第一指定数量，执行利用八叉树分割技术对所得点云文件分别进行空间分割的步骤。

上述方法中，八叉树的根节点是点云文件的全部点云；所述利用八叉树分割技术对所得点云文件分别进行空间分割，已确定每个点云所处子空间位置，得到点云文件的八叉树空间，包括：

从八叉树根节点开始，递归向下进行平均分割，平均分割得到的八个子空间为根节点的子节点，所有子节点包含子空间内全部点云；

当节点满足继续分割条件，对节点进行空间分割，直到节点不满足分割条件为止。

上述方法中，所述利用球包围盒和方向包围盒对所得八叉树空间进行层次包裹，得到混合包围盒树包括：

使用球包围盒与方向包围盒两种包围盒技术，对八叉树空间进行包裹，从八叉树根节点开始向下构建混合包围盒树，混合包围盒树紧密包裹所有点云；

其中，使用球包围盒对八叉树根节点进行包裹，成为混合包围盒树的根节点，球包围盒的构建公式为：

R＝{(x,y,z)|(x-o

(o

其中，使用方向包围盒对八叉树空间所有子节点包含的点云进行包裹，成为混合包围盒树的子节点，方向包围盒的构建公式为：

M＝{C+ar

C点为包围盒的中心点，v

上述方法中，所述基于机械臂单一构型碰撞检测算法，得到机械臂单一构型碰撞检测结果，包括：

基于机械臂单一构型碰撞检测算法，搜索混合包围盒树，对混合包围盒树的方向包围盒进行相交计算，得到机械臂单一构型碰撞检测结果；其中，所述相交计算通过曼哈顿距离计算得到。

上述方法中，所述基于机械臂单一构型碰撞检测算法，搜索混合包围盒树，对混合包围盒树的方向包围盒进行相交计算，得到机械臂单一构型碰撞检测结果，包括：

在虚拟仿真场景中，对机械臂单一构型与仿真场景做单次碰撞检测时，搜索机械臂与仿真场景两棵混合包围盒树，判断两棵混合包围盒树中是否存在相交子节点；

当两棵混合包围盒树中除根节点外所有叶子节点数量之差大于第二指定值时，对两棵混合包围盒树中高度较小的包围盒树进行深度优先搜索，搜索是否存在相交子结点，得到机械臂单一构型碰撞检测结果；

当两棵混合包围盒树中除根节点外所有叶子节点数量之差小于第二指定值时，对两棵混合包围盒树进行双向广度搜索，搜索是否存在相交子结点，得到机械臂单一构型碰撞检测结果。

上述方法中，所述基于机械臂连续运动碰撞检测算法，得到机械臂连续运动时碰撞检测结果，包括：

当机械臂处于运动初始时刻时，对机械臂做机械臂单一构型碰撞检测，并保存此次碰撞检测结果，所述碰撞检测结果包括碰撞结果、相交点云索引、最小点云距离、碰撞深度；

当机械臂处于非运动初始时刻时，若上一时刻不发生碰撞且最小点云距离大于第三指定值时，判断出当前时刻不发生碰撞；

或者，当机械臂处于非运动初始时刻时，若上一时刻不发生碰撞且最小点云距离小于第三指定值时，判断出当前时刻可能发生碰撞，在当前状态下对机械臂做机械臂单一构型碰撞检测，得到当前时刻的碰撞结果；

或者，当机械臂处于非运动初始时刻时，若上一时刻发生碰撞且碰撞深度大于第四指定值时，判断出当前时刻发生碰撞；

或者，当机械臂处于非运动初始时刻时，若上一时刻发生碰撞且碰撞深度小于第四指定值时，判断出当前时刻可能发生碰撞，在当前状态下对机械臂做机械臂单一构型碰撞检测，得到当前时刻的碰撞结果；

或者，当机械臂运动过程中所有时刻都不发生碰撞时，判断出该次运动过程中无碰撞，反之，该次运动过程中发生了碰撞，获取发生碰撞的碰撞时刻。

其中，第三指定值大于第四指定值。

由以上技术方案可以看出，本发明实施例具有以下有益效果：

本发明实施例的技术方案中，利用深度优先搜索算法和双向广度搜索算法，设计机械臂单一构型碰撞检测算法，提升了对机械臂单一构型的碰撞检测计算的计算速度和计算准确性；利用机械臂运动特性和动态规划算法，设计机械臂连续运动碰撞检测算法，提升了对机械臂连续运动的碰撞检测计算的计算速度。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单介绍，显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性和劳动性的前提下，还可以根据这些附图获得其它附图。

图1是本发明实施例所提供的一种虚拟仿真场景中机械臂碰撞检测方法的流程示意图；

图2是本发明实施例中预处理的流程示意图；

图3是本发明实施例中八叉树空间分割的示意图；

图4是本发明实施例中八叉树空间分割的流程示意图；

图5是本发明实施例中机械臂单一构型碰撞检测的流程示意图；

【具体实施例】

为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本发明实施例给出一种虚拟仿真场景中机械臂碰撞检测方法，如图1所示，该方法包括以下步骤：

步骤101，利用表面栅格化技术分别对虚拟仿真场景模型和机械臂模型进行预处理，获得虚拟仿真场景模型对应的点云文件和机械臂模型对应的点云文件。

具体的，步骤101的具体实现方案可以如图2所示，包括以下步骤：

步骤201，将虚拟仿真场景模型和机械臂模型分别导入OSG三维引擎中；

步骤202，导入模型后，利用三维引擎中提供的获取表面轮廓方法osg::BoundingSphere::getBound()提取虚拟仿真场景模型和机械臂模型的表面轮廓；

步骤203，根据预设的扫描精度在表面轮廓均匀取点，利用三维引擎中提供的获取直线方法osgUtil::IntersectVisitor::hits()方法获得虚拟仿真场景模型和机械臂模型的表面散点；

步骤204，获得虚拟仿真场景模型和机械臂模型的表面散点后，将散点信息保存为二进制点云文件，为后续空间分割操作做准备。

步骤102，利用八叉树分割技术对所得点云文件分别进行空间分割，以确定每个点云所处的子空间位置，得到点云文件的八叉树空间。

具体的，步骤102的具体实现方案可以如图4所示，包括以下步骤：

步骤401，将点云文件所有点云设置为八叉树空间根节点，并把根节点设置为待分割节点；

步骤402，判断待分割空间的点云数量是否小于第一指定数量。如果空间内点云数量小于第一指定数量，不再对该空间进行分割，执行步骤405；如果空间内点云数量不小于第一指定数量，执行步骤403；

步骤403，对待分割空间进行空间分割，在空间中分别用平行于平面xoy、平面yoz和平面xoz的辅助面，平均分割当前待分割空间，得到八个子空间，编号从0到7，子空间包含内部全部点云；八个子空间的父节点为当前节点。空间分割示意图如图3所示；

步骤404，分别将八个子空间设置为待分割节点，并重新执行步骤402；

步骤405，得到点云文件的八叉树空间。

步骤103，利用球包围盒和方向包围盒对所得八叉树空间进行层次包裹，得到混合包围盒树。

使用球包围盒与方向包围盒两种包围盒技术，对八叉树空间进行包裹，从八叉树根节点开始向下构建混合包围盒树，混合包围盒树紧密包裹所有点云：

具体的，对八叉树空间根节点内部所有点云使用球包围盒构建公式构造球包围盒，使球包围盒包裹八叉树空间根节点，成为混合包围盒树的根节点；

对八叉树空间非根节点的所有节点内部点云使用方向包围盒构建公式构造方向包围盒，使方向包围盒包裹八叉树空间的所有非根节点，成为混合包围盒树的子节点；

步骤104，利用深度优先搜索算法和双向广度搜索算法，设计机械臂单一构型碰撞检测算法，并基于机械臂单一构型碰撞检测算法，得到机械臂单一构型碰撞检测结果。

具体的，步骤104的具体实现方案可以如图5所示，包括以下步骤：

步骤501，获取机械臂的混合包围盒树全部节点数量和虚拟仿真环境的混合包围盒树全部节点数量；

步骤502，计算两棵混合包围盒树节点数量之差。当两棵混合包围盒树中除根节点外所有叶子节点数量之差大于或等于第二指定值时，执行步骤503；当两棵混合包围盒树中除根节点外所有叶子节点数量之差小于第二指定值时，执行步骤504。

步骤503，对两者中高度较小的包围盒树进行深度优先搜索，尽可能深的搜索混合包围盒树的分支。当节点所在边都己被探寻过，搜索将回溯到发现节点条边的起始节点。这一过程一直进行到已发现从源节点可达的所有节点为止。如果还存在未被发现的节点，则选择其中一个作为源节点并重复以上过程，整个进程反复进行直到所有节点都被访问为止；

步骤504，对两棵包围盒树进行双向广度搜索，分别从两棵包围盒树的根节点开始搜索，当搜索完叶子节点或检测到有相交节点时停止搜索。

步骤505，使用曼哈顿距离对叶子节点的方向包围盒进行相交性计算。

具体如下：假设有两个包围盒A和B，它们的中心点分别为O

假设O

R

R

若L＞R

采用曼哈顿距离来表示两点之间的距离L：

L＝|x

可见计算两中心点之间的曼哈顿距离只需要做2次加法计算、3次减法计算和3次绝对值计算，与欧氏距离相比减少了较多计算开销。

步骤105，利用机械臂运动特性和动态规划算法，设计机械臂连续运动碰撞检测算法，并基于机械臂连续运动碰撞检测算法，得到机械臂连续运动时碰撞检测结果。

在虚拟仿真环境中，仿真对象的运动往往是无规则、无法预测的，这种情况下就无法得到对象的运动函数。但机械臂的运动状态是连续的，其在这一时刻的位置与上一时刻的位置是有关联的。假设虚拟仿真环境中有一个对象A，其在相邻两个时间点t

当机械臂处于运动初始时刻时，对机械臂构型执行步骤104，做机械臂单一构型碰撞检测，并保存此次碰撞检测结果，所述碰撞检测结果包括碰撞结果、相交点云索引、最小点云距离、碰撞深度；

当机械臂处于非运动初始时刻时，若上一时刻不发生碰撞且最小点云距离大于第三指定值时，判断出当前时刻不发生碰撞；

或者，当机械臂处于非运动初始时刻时，若上一时刻不发生碰撞且最小点云距离小于第三指定值时，判断出当前时刻可能发生碰撞，需在当前状态执行步骤104，对机械臂做机械臂单一构型碰撞检测，得到当前时刻的碰撞结果；

或者，当机械臂处于非运动初始时刻时，若上一时刻发生碰撞且碰撞深度大于第四指定值时，可以认为当前时刻发生碰撞；

或者，当机械臂处于非运动初始时刻时，若上一时刻发生碰撞且碰撞深度小于第四指定值时，判断出当前时刻可能发生碰撞，在当前状态下执行步骤104，对机械臂做机械臂单一构型碰撞检测，得到当前时刻的碰撞结果；

当机械臂运动过程中所有时刻都不发生碰撞时，判断出该次运动过程中无碰撞，否者该次运动过程中发生了碰撞，并得到具体碰撞时刻。

本发明实施例的技术方案具有以下有益效果：

本发明实施例的技术方案中，设计了一种机械臂碰撞检测方法，利用表面栅格化技术分别对虚拟仿真场景模型和机械臂模型进行预处理，获得相应的点云文件；利用八叉树分割技术对所得点云文件分别进行空间分割，以确定每个点云所处的子空间位置，得到点云文件的八叉树空间；利用球包围盒和方向包围盒对所得八叉树空间进行层次包裹，得到混合包围盒树；利用深度优先搜索算法和双向广度搜索算法，设计机械臂单一构型碰撞检测算法，并基于机械臂单一构型碰撞检测算法那，得到机械臂单一构型碰撞检测结果；利用机械臂运动特性和动态规划算法，设计机械臂连续运动碰撞检测算法，并基于机械臂连续运动碰撞检测算法，得到机械臂连续运动时碰撞检测结果。

本发明通过构建虚拟仿真场景和机械臂的混合包围盒八叉树，并根据机械臂运动特性设计搜索策略，实现了机械臂运动仿真时碰撞检测的准确性和实时性。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。