CN114742944A - 面向工业机器人路径规划的保守碰撞检测方法 - Google Patents

Abstract

本发明属于工业机器人碰撞检测领域，公开了一种面向工业机器人路径规划的保守碰撞检测方法，包括1：采用基于莫顿码的二分查找算法构建线性层级包围体；2：提出一种Gpu加速的SDF构建算法；3：提出一种基于粒子群的包络球生成算法；4：采用SDF和包络球实现保守的碰撞检测。本发明通过标记并细分模型表面区域的体素网格，仅计算该区域内的距离场函数值，减少了需要计算的体素数量从而提升计算效率，利用稀疏网格进行存储从而降低了内存开销；所有线程可独立的访问LBVH并计算最短距离，提高了计算的并行度；采用粒子群优化算法计算紧密的包络球代替机器人关节执行碰撞检测，提高了碰撞检测的效率。

Description

面向工业机器人路径规划的保守碰撞检测方法

技术领域

本发明属于工业机器人碰撞检测领域，尤其涉及一种面向工业机器人路径规划的保守碰撞检测方法。

背景技术

工业机器人仿真能够模拟现实作业中的每一项工作和流程，并与之实现各种交互，可以在工业产品实物化前就对设计思想、产品潜在性能、使用效能和适应性等诸多方面进行研究、模拟和评估。目前已经广泛地应用到工业的各个领域中，从产品设计与研发、产品培训与调研到产品销售与推广等。仿真技术的应用能有效提高企业开发效率，加强数据采集、分析、处理能力，减少决策失误并降低企业风险。

为避免机器人在实际生产作业中与周围设备发生碰撞而引发安全问题，必须在离线仿真中加入碰撞检测功能，对仿真过程中出现的碰撞干涉做出实时响应和反馈。在产线布局、工业机器人路径规划以及IO事件仿真等多个阶段都少不了碰撞检测的参与，它是离线仿真中不可或缺的技术模块。精确的碰撞检测需要对场景中所有模型的面片进行相交计算，检测的成本较高。在机器人路径规划任务中，由于需要在整个三维工作空间中搜索所有可达的路径点，需要做大量的碰撞检测，使用精确的碰撞检测算法会大大降低路径规划的效率。此外采用精确碰撞检测算法所得到的规划路径到产线模型表面之间的安全距离较短，导致路径的可靠性不足，容易在实际生产作业中引发误碰撞。

一些基于SDF的检测算法可用于低精度的碰撞检测任务，但SDF在Cpu上的计算速度很慢，而一般Gpu上的构建方案往往较为复杂，往往会牺牲准确性以提高构建效率。此外基于SDF的碰撞检测算法其检测精度受到SDF分辨率的限制，增加SDF分辨率在提高检测精度的同时也会显著增加存储开销。

发明内容

本发明的目的在于提供一种面向工业机器人路径规划的保守碰撞检测算法，以解决现有路径规划中采用精确碰撞检测算法导致规划时间太长，一般的基于SDF的近似碰撞检测算法由于SDF构建效率低、存储开销大导致难以满足大场景规划任务的需求的技术问题。

为解决上述技术问题，本发明的面向工业机器人路径规划的保守碰撞检测方法的具体技术方案如下：

一种面向工业机器人路径规划的保守碰撞检测方法，包括如下步骤：

步骤1：采用基于莫顿码的二分查找算法构建线性层级包围体；

步骤2：提出一种Gpu加速的SDF构建算法；

步骤3：提出一种基于粒子群的包络球生成算法；

步骤4：采用SDF和包络球实现保守的碰撞检测。

进一步地，所述步骤1包括计算机器人关节模型的轴对齐包围盒(AABB)，通过计算所有面片的莫顿码并对所有面片进行排序，采用二分查找的方式创建模型的线性层级包围体。

进一步地，所述步骤1包括如下具体步骤：

建立LBVH树结构：输入模型包含n个三角面片，首先计算所有面片整体的AABB，以AABB作为边界，将面片中心坐标转换至0到1范围内，生成莫顿码并对所有面片进行基数排序；每个图元创建一个叶节点；每个叶节点发起一个线程并行计算，通

过二分查找建立树的连接关系；

采用AABB作为LBVH树节点的包围盒类型，LBVH内部节点包围盒的构建方式采用自下而上的策略：从所有叶子节点出发，每个三角形分配一个线程，计算三角形的AABB，而后所有线程向上遍历父节点，保留第二个达到父节点的线程去计算父节点的AABB，计算方式为将两个子节点的AABB进行合并，第一次达到的线程提前退出，使得每次向上遍历一层后线程数量减半，如此向上遍历直到完成整个LBVH的包围盒构建。

进一步地，所述步骤2包括以模型尺寸的1/64-1/100的分辨率在模型AABB边界内构建均匀网格，将模型上所有三角形在平面内向外偏移距离r，然后在与三角形垂直的两个方向上分别拉伸距离r以构建凸多面体；并行计算该凸多面体在网格中覆盖的范围；将所有被模型三角面片覆盖的网格单元进行细分，生成高分辨率的网格，每个网格单元发起一个线程计算其到模型表面的最短距离；对细分后的网格单元，以网格单元的中心为起点，分别向x和y方向发射两条射线，通过其与模型表面的相交次数来判断该网格单元是否位于模型内部。

进一步地，所述步骤2SDF构建算法包括第一阶段，所述第一阶段采用较低的分辨率将SDF空间划分为均匀网格，网格中每一小块立方体区域称为cell，cell的尺寸通过下式计算：

其中l、w、h为SDF边界的长宽高，r为包络球的最大半径，计算出网格在三个方向的维度：

网格以一维数组形式平铺存储，在显存上分配两块大小为gridsize＝l'×w'×h'、数据类型为整型的内存：idBuffer用来存储cell在网格中的一维编号；overlapBuffer存储cell是否被覆盖，每个三角形发起一个线程，将三角形在三角形平面内向外偏移距离r，然后在与三角形垂直的两个方向上分别拉伸距离r以构建凸多面体；利用分离轴定理计算出该凸多面体覆盖的所有cell，并将这些cell对应的overlapBuffer中的值设为1，未被覆盖的标记为0；接下来对overlapBuffer并行计算前缀和，根据前缀和将idBuffer中被覆盖的cell编号集中排列在数组前端，统计被覆盖的cell总数为overlapNum。

进一步地，所述步骤2SDF构建算法包括第二阶段，所述第二阶段需要对第一阶段中发生覆盖的cell进行细分，对cell细分后的立方体区域称为subcell，其尺寸计算公式为：

以每个subcell发起一个线程，计算subcell立方体中心到模型表面的最短距离；该过程需要利用LBVH树结构加速查找，具体过程为：以subcell立方体中心为球心，构造初始半径为R的球，自上而下进行球和LBVH树的相交测试，若和内部节点相交，则向下查找子节点；当到达叶子节点时，判断球和三角形是否相交，若相交则计算球心到三角面片的最短距离，并取所有叶子节点中最短距离的最小值作为该subcell到模型表面的最短距离；若球与至少一个三角形相交，则计算完成，否则将球的半径增加一倍后继续与LBVH做相交测试，重复以上过程；

当subcell中心到模型表面的最短距离不为零时，还需判断距离值符号，利用射线法，即以subcell中心为射线起点，向任意方向发射射线，计算该射线与物体表面的相交次数，若相交奇数次则距离值符号为负，否则为正，选择x和y两个方向进行两次射线求交测试，只有在两个方向上相交次数都为奇数时才认为距离值符号为负，否则为正，计算结果最终从Gpu回传到Cpu并利用稀疏网格存储所有subcell的值，稀疏网格数据结构采用VDB。

进一步地，所述步骤3包括将Gpu计算后的网格数据回传到Cpu，利用稀疏网格数据结构进行存储，在机器人模型表面以均匀采样的方式得到点集S，以每个模型八个包络球的标准创建粒子群优化算法模型，通过计算模型的定向包围盒(OBB)确定粒子群算法的自变量优化区间，以Gpu加速的方式判断包络球是否能够完全包含所有采样点，并将未包含的点的数量作为惩罚函数加入优化模型，最后运行粒子群算法得到优化后的包络球，并删除掉无效包络球。

进一步地，所述步骤3包括如下具体步骤：

首先需要确定算法优化的自变量，规定每个关节的包络球的数量不超过8个，所有包络球组成的集合记为E，每个包络球包含球心坐标以及球半径四个参数(x,y,z,r)，共计32个优化参数，算法优化目标为所有包络球的体积之和最小，采用所有包络球半径的三次方之和作为优化目标函数，通过采样的方法，以均匀密度在模型表面采样大量的点记为集合S，利用Gpu并行计算判断采样点集S是否完全被集合E包含，设S中位于E之外的点的数量为k，将该变量作为惩罚项加入到优化函数中得到最终的优化函数如下：

其中λ表示惩罚系数，由用户定义；

优化函数中自变量的范围通过以下方法确定：首先计算关节模型的OBB，将OBB沿三个轴向均匀划分为8个子立方体，从OBB中心指向每个子立方体的中心的向量记为d_i，这些向量的模长均相等，将其长度作为包络球半径参数的最大值R_max，每个包络球的参数(x_i,y_i,z_i,r_i)通过下式计算：

式中O_i表示OBB的中心坐标，u_i,v_i,w_i表示与球心坐标三个分量相对应的参数，通过这样的方式，确定优化函数的32个参数为{u_i,v_i,w_i,r_i},0≤i＜8；

从机器人关节模型获取采样点的方式为均匀采样，设定采样间隔为w，采样输出点集为S，采样过程描述为：首先将模型上所有顶点加入S；然后遍历所有三角形{v₀,v₁,v₂}，采取逐行扫描的方式采样，扫描的第一个方向为d₀＝v₁-v₀，第二个方向为d₁＝d₀ ^⊥，d₀ ^⊥表示将向量d₀在三角形平面内延逆时针方向旋转90度，设扫描线为l，初始状态l设置为v₀v₁所在直线，算法开始，分别计算l与边v₀v₂和v₁v₂的交点记作v_beg和v_end，在线段v_begv_end上从点v_beg开始以w为间距均匀采点，将所有采样点加入S；然后将l延d₁方向平移距离w，重复上述过程直到l与边v₀v₂和v₁v₂不产生交点或两个交点重叠；通过Gpu并行计算判断点集S是否被集合E包含的过程为：设定线程负载参数m，选取参数m＝4，将点集S扩充至m的倍数，每m个点分配到一个线程中发起计算，一个线程组中的线程数设定为1024，在单个线程中遍历m个点统计没有被集合E包含的点的数量为n，利用Gpu上的共享内存在线程组内并行归约，计算出一个线程组中没有被集合E包含的点数n'，最后由每个线程组中的第一个线程通过原子求和运算得到所有点集中未被E包含的点数k；

基于粒子群算法优化得到包络球后将半径为零的球删除，最终得到机器人关节包络球。

进一步地，所述步骤4包括如下具体步骤：

SDF在物体运动时跟随物体一起运动，在碰撞测试前先将包络球变换到SDF所在坐标系中，根据包络球球心与SDF边界的相对位置分为三种情况：当包络球的球心位于SDF边界外时一定不发生碰撞；当包络球球心位于SDF边界内，但球心所在位置的SDF值不存在，说明该位置距离模型表面最近距离大于机器人最大包络球半径，因而未发生碰撞；当包络球球心位于SDF边界内，且球心所在位置的SDF值的绝对值小于等于包络球半径时认为发生碰撞，否则认为未碰撞。

本发明的面向工业机器人路径规划的保守碰撞检测方法具有以下优点：本发明提出一种双层的SDF并行构建算法，通过标记并细分模型表面区域的体素网格，仅计算该区域内的距离场函数值，减少了需要计算的体素数量从而提升计算效率，利用稀疏网格进行存储从而降低了内存开销；通过建立LBVH树结构，所有线程可独立的访问LBVH并计算最短距离，提高了计算的并行度；采用粒子群优化算法计算紧密的包络球代替机器人关节执行碰撞检测，单次检测最多仅需要查询八次距离场，提高了碰撞检测的效率。

附图说明

图1为本发明的方法总体流程图；

图2为本发明的LBVH并行构建示意图；

图3为本发明的SDF创建过程示意图；

图4为本发明的包络球优化参数范围示意图；

图5为本发明的包络球生成过程示意图；

图6为本发明SDF和包络球碰撞检测示意图。

具体实施方式

为了更好地了解本发明的目的、结构及功能，下面结合附图，对本发明一种面向工业机器人路径规划的保守碰撞检测方法做进一步详细的描述。

本发明采用符号距离场(SDF)和包络球实现保守的碰撞检测方案，对周边设备等物体预计算生成SDF并通过稀疏网格存储，对机器人采用少量的包络球代替关节模型参与碰撞检测。运行期间机器人和周边设备的碰撞检测可转换为包络球和符号距离场的相交测试，算法的精度由距离场的分辨率以及包络球的半径决定。

如图1所示，本发明的面向工业机器人路径规划的保守碰撞检测方法，包括如下步骤：

采用基于莫顿码的二分查找算法构建线性层级包围体。LBVH的构建的目的为加速后续SDF的并行计算。LBVH树结构的建立方式为：输入模型包含n个三角面片，首先计算所有面片整体的AABB，以AABB作为边界，将面片中心坐标转换至0到1范围内，生成莫顿码并对所有面片进行基数排序；每个图元创建一个叶节点；每个叶节点发起一个线程并行计算，通过二分查找建立树的连接关系，计算过程如图2所示。

采用AABB作为LBVH树节点的包围盒类型，LBVH内部节点包围盒的构建方式采用自下而上的策略：从所有叶子节点出发，每个三角形分配一个线程，计算三角形的AABB，而后所有线程向上遍历父节点，保留第二个达到父节点的线程去计算父节点的AABB，计算方式为将两个子节点的AABB进行合并，第一次达到的线程提前退出，使得每次向上遍历一层后线程数量减半，如此向上遍历直到完成整个LBVH的包围盒构建。

提出一种Gpu加速的SDF构建算法，SDF计算分为两个阶段进行，第一阶段采用以模型尺寸的1/64-1/100的分辨率将SDF空间划分为均匀网格，网格中每一小块立方体区域称为cell，cell的尺寸通过下式计算：

其中l、w、h为SDF边界的长宽高，r为包络球的最大半径，计算出网格在三个方向的维度：

网格以一维数组形式平铺存储，在显存上分配两块大小为gridsize＝l'×w'×h'、数据类型为整型的内存：idBuffer用来存储cell在网格中的一维编号；overlapBuffer存储cell是否被覆盖。每个三角形发起一个线程，将三角形在三角形平面内向外偏移距离r，然后在与三角形垂直的两个方向上分别拉伸距离r以构建凸多面体。利用分离轴定理计算出该凸多面体覆盖的所有cell，并将这些cell对应的overlapBuffer中的值设为1，未被覆盖的标记为0。接下来对overlapBuffer并行计算前缀和，根据前缀和将idBuffer中被覆盖的cell编号集中排列在数组前端，统计被覆盖的cell总数为overlapNum。

第二阶段需要对上一阶段中发生覆盖的cell进行细分，对cell细分后的立方体区域称为subcell，其尺寸计算公式为：

以每个subcell发起一个线程，计算subcell立方体中心到模型表面的最短距离。该过程需要利用LBVH树结构加速查找，具体过程为：以subcell立方体中心为球心，构造初始半径为R的球，自上而下进行球和LBVH树的相交测试，若和内部节点相交，则向下查找子节点。当到达叶子节点时，判断球和三角形是否相交，若相交则计算球心到三角面片的最短距离，并取所有叶子节点中最短距离的最小值作为该subcell到模型表面的最短距离。若球与至少一个三角形相交，则计算完成，否则将球的半径增加一倍后继续与LBVH做相交测试，重复以上过程。

当subcell中心到模型表面的最短距离不为零时，还需判断距离值符号。可利用射线法，即以subcell中心为射线起点，向任意方向发射射线，计算该射线与物体表面的相交次数。若相交奇数次则距离值符号为负，否则为正。由于浮点运算的舍入误差，仅在一个方向采用射线法某些情况下计算结果并不可靠，因此选择x和y两个方向进行两次射线求交测试，只有在两个方向上相交次数都为奇数时才认为距离值符号为负，否则为正。计算结果最终从Gpu回传到Cpu并利用稀疏网格存储所有subcell的值，稀疏网格数据结构采用VDB。SDF计算过程如图3所示。

提出一种基于粒子群的包络球生成算法，包络球是指在机器人关节模型表面生成的若干球体，这些球体能够完全包含模型上的所有图元，因此可以用这些包络球和SDF进行快速的相交判别，从而避免对模型上所有图元做复杂的相交检测。包络球的生成原则是用最少数量的球完全包络模型上的所有图元，该问题可利用粒子群优化算法求解。

首先需要确定算法优化的自变量，规定每个关节的包络球的数量不超过8个，所有包络球组成的集合记为E，每个包络球包含球心坐标以及球半径四个参数(x,y,z,r)，共计32个优化参数。算法优化目标为所有包络球的体积之和最小，可采用所有包络球半径的三次方之和作为优化目标函数。此外在优化过程中需要满足集合E始终能够完全包含关节模型，由于输入的机器人关节为三角网格模型，精确的包含测试需要对三角形和包络球之间球做布尔运算，这样的计算开销是无法接受的。因此可通过采样的方法，以均匀密度在模型表面采样大量的点记为集合S，利用Gpu并行计算判断采样点集S是否完全被集合E包含。设S中位于E之外的点的数量为k，将该变量作为惩罚项加入到优化函数中得到最终的优化函数如下：

其中λ表示惩罚系数，由用户定义。优化函数中自变量的范围可通过以下方法确定：首先计算关节模型的OBB，将OBB沿三个轴向均匀划分为8个子立方体，从OBB中心指向每个子立方体的中心的向量记为d_i，这些向量的模长均相等，将其长度作为包络球半径参数的最大值R_max。每个包络球的参数(x_i,y_i,z_i,r_i)可通过下式计算：

式中O_i表示OBB的中心坐标，u_i,v_i,w_i表示与球心坐标三个分量相对应的参数，通过这样的方式，确定了优化函数的32个参数为{u_i,v_i,w_i,r_i},0≤i＜8，图4为包络球参数范围平面示意图。

从机器人关节模型获取采样点的方式为均匀采样，设定采样间隔为w，采样输出点集为S。采样过程描述为：首先将模型上所有顶点加入S；然后遍历所有三角形{v₀,v₁,v₂}，采取逐行扫描的方式采样，扫描的第一个方向为d₀＝v₁-v₀，第二个方向为d₁＝d₁ ^⊥，d₀ ^⊥表示将向量d₀在三角形平面内延逆时针方向旋转90度。设扫描线为l，初始状态l设置为v₀v₁所在直线。算法开始，分别计算l与边v₀v₂和v₁v₂的交点记作v_beg和v_end，在线段v_begv_end上从点v_beg开始以w为间距均匀采点，将所有采样点加入S。然后将l延d₁方向平移距离w，重复上述过程直到l与边v₀v₂和v₁v₂不产生交点或两个交点重叠。

通过Gpu并行计算判断点集S是否被集合E包含的过程为：设定线程负载参数m，本文选取的参数m＝4，将点集S扩充至m的倍数，每m个点分配到一个线程中发起计算，一个线程组中的线程数设定为1024。在单个线程中遍历m个点统计没有被集合E包含的点的数量为n，利用Gpu上的共享内存在线程组内并行归约，计算出一个线程组中没有被集合E包含的点数n'，最后由每个线程组中的第一个线程通过原子求和运算得到所有点集中未被E包含的点数k。

基于粒子群算法优化得到包络球后将半径为零的球删除，最终得到机器人关节包络球构建效果如图5所示。

采用SDF和包络球实现保守的碰撞检测。SDF与包络球的碰撞检测较为简单，SDF在物体运动时跟随物体一起运动，在碰撞测试前先将包络球变换到SDF所在坐标系中，根据包络球球心与SDF边界的相对位置分为三种情况：由图6可知当包络球的球心位于SDF边界外时一定不发生碰撞；当包络球球心位于SDF边界内，但球心所在位置的SDF值不存在，说明该位置距离模型表面最近距离大于机器人最大包络球半径，因而未发生碰撞；当包络球球心位于SDF边界内，且球心所在位置的SDF值的绝对值小于等于包络球半径时认为发生碰撞，否则认为未碰撞。

可以理解，本发明是通过一些实施例进行描述的，本领域技术人员知悉的，在不脱离本发明的精神和范围的情况下，可以对这些特征和实施例进行各种改变或等效替换。另外，在本发明的教导下，可以对这些特征和实施例进行修改以适应具体的情况及材料而不会脱离本发明的精神和范围。因此，本发明不受此处所公开的具体实施例的限制，所有落入本申请的权利要求范围内的实施例都属于本发明所保护的范围内。

Claims (9)

1.一种面向工业机器人路径规划的保守碰撞检测方法，其特征在于，包括如下步骤：

步骤1：采用基于莫顿码的二分查找算法构建线性层级包围体；

步骤2：提出一种Gpu加速的SDF构建算法；

步骤3：提出一种基于粒子群的包络球生成算法；

步骤4：采用SDF和包络球实现保守的碰撞检测。

2.根据权利要求1所述的面向工业机器人路径规划的保守碰撞检测方法，其特征在于，所述步骤1包括计算机器人关节模型的轴对齐包围盒(AABB)，通过计算所有面片的莫顿码并对所有面片进行排序，采用二分查找的方式创建模型的线性层级包围体。

3.根据权利要求2所述的面向工业机器人路径规划的保守碰撞检测方法，其特征在于，所述步骤1包括如下具体步骤：

建立LBVH树结构：输入模型包含n个三角面片，首先计算所有面片整体的AABB，以AABB作为边界，将面片中心坐标转换至0到1范围内，生成莫顿码并对所有面片进行基数排序；每个图元创建一个叶节点；每个叶节点发起一个线程并行计算，通过二分查找建立树的连接关系；

采用AABB作为LBVH树节点的包围盒类型，LBVH内部节点包围盒的构建方式采用自下而上的策略：从所有叶子节点出发，每个三角形分配一个线程，计算三角形的AABB，而后所有线程向上遍历父节点，保留第二个达到父节点的线程去计算父节点的AABB，计算方式为将两个子节点的AABB进行合并，第一次达到的线程提前退出，使得每次向上遍历一层后线程数量减半，如此向上遍历直到完成整个LBVH的包围盒构建。

4.根据权利要求1所述的面向工业机器人路径规划的保守碰撞检测方法，其特征在于，所述步骤2包括以模型尺寸的1/64-1/100的分辨率在模型AABB边界内构建均匀网格，将模型上所有三角形在平面内向外偏移距离r，然后在与三角形垂直的两个方向上分别拉伸距离r以构建凸多面体；并行计算该凸多面体在网格中覆盖的范围；将所有被模型三角面片覆盖的网格单元进行细分，生成高分辨率的网格，每个网格单元发起一个线程计算其到模型表面的最短距离；对细分后的网格单元，以网格单元的中心为起点，分别向x和y方向发射两条射线，通过其与模型表面的相交次数来判断该网格单元是否位于模型内部。

5.根据权利要求4所述的面向工业机器人路径规划的保守碰撞检测方法，其特征在于，所述步骤2SDF构建算法包括第一阶段，所述第一阶段采用较低的分辨率将SDF空间划分为均匀网格，网格中每一小块立方体区域称为cell，cell的尺寸通过下式计算：

其中l、w、h为SDF边界的长宽高，r为包络球的最大半径，计算出网格在三个方向的维度：

网格以一维数组形式平铺存储，在显存上分配两块大小为gridsize＝l'×w'×h'、数据类型为整型的内存：idBuffer用来存储cell在网格中的一维编号；overlapBuffer存储cell是否被覆盖，每个三角形发起一个线程，将三角形在三角形平面内向外偏移距离r，然后在与三角形垂直的两个方向上分别拉伸距离r以构建凸多面体；利用分离轴定理计算出该凸多面体覆盖的所有cell，并将这些cell对应的overlapBuffer中的值设为1，未被覆盖的标记为0；接下来对overlapBuffer并行计算前缀和，根据前缀和将idBuffer中被覆盖的cell编号集中排列在数组前端，统计被覆盖的cell总数为overlapNum。

6.根据权利要求5所述的面向工业机器人路径规划的保守碰撞检测方法，其特征在于，所述步骤2SDF构建算法包括第二阶段，所述第二阶段需要对第一阶段中发生覆盖的cell进行细分，对cell细分后的立方体区域称为subcell，其尺寸计算公式为：

以每个subcell发起一个线程，计算subcell立方体中心到模型表面的最短距离；该过程利用LBVH树结构加速查找，具体过程为：以subcell立方体中心为球心，构造初始半径为R的球，自上而下进行球和LBVH树的相交测试，若和内部节点相交，则向下查找子节点；当到达叶子节点时，判断球和三角形是否相交，若相交则计算球心到三角面片的最短距离，并取所有叶子节点中最短距离的最小值作为该subcell到模型表面的最短距离；若球与至少一个三角形相交，则计算完成，否则将球的半径增加一倍后继续与LBVH做相交测试，重复以上过程；

当subcell中心到模型表面的最短距离不为零时，还需判断距离值符号，利用射线法，即以subcell中心为射线起点，向任意方向发射射线，计算该射线与物体表面的相交次数，若相交奇数次则距离值符号为负，否则为正，选择x和y两个方向进行两次射线求交测试，只有在两个方向上相交次数都为奇数时才认为距离值符号为负，否则为正，计算结果最终从Gpu回传到Cpu并利用稀疏网格存储所有subcell的值，稀疏网格数据结构采用VDB。

7.根据权利要求1所述的面向工业机器人路径规划的保守碰撞检测方法，其特征在于，所述步骤3包括将Gpu计算后的网格数据回传到Cpu，利用稀疏网格数据结构进行存储，在机器人模型表面以均匀采样的方式得到点集S，以每个模型八个包络球的标准创建粒子群优化算法模型，通过计算模型的定向包围盒(OBB)确定粒子群算法的自变量优化区间，以Gpu加速的方式判断包络球是否能够完全包含所有采样点，并将未包含的点的数量作为惩罚函数加入优化模型，最后运行粒子群算法得到优化后的包络球，并删除掉无效包络球。

8.根据权利要求7所述的面向工业机器人路径规划的保守碰撞检测方法，其特征在于，所述步骤3包括如下具体步骤：

首先需要确定算法优化的自变量，规定每个关节的包络球的数量不超过8个，所有包络球组成的集合记为E，每个包络球包含球心坐标以及球半径四个参数(x,y,z,r)，共计32个优化参数，算法优化目标为所有包络球的体积之和最小，采用所有包络球半径的三次方之和作为优化目标函数，通过采样的方法，以均匀密度在模型表面采样大量的点记为集合S，利用Gpu并行计算判断采样点集S是否完全被集合E包含，设S中位于E之外的点的数量为k，将该变量作为惩罚项加入到优化函数中得到最终的优化函数如下：

其中λ表示惩罚系数，由用户定义；

优化函数中自变量的范围通过以下方法确定：首先计算关节模型的OBB，将OBB沿三个轴向均匀划分为8个子立方体，从OBB中心指向每个子立方体的中心的向量记为d_i，这些向量的模长均相等，将其长度作为包络球半径参数的最大值R_max，每个包络球的参数(x_i,y_i,z_i,r_i)通过下式计算：

式中O_i表示OBB的中心坐标，u_i,v_i,w_i表示与球心坐标三个分量相对应的参数，通过这样的方式，确定优化函数的32个参数为{u_i,v_i,w_i,r_i},0≤i＜8；

从机器人关节模型获取采样点的方式为均匀采样，设定采样间隔为w，采样输出点集为S，采样过程为：首先将模型上所有顶点加入S；然后遍历所有三角形{v₀,v₁,v₂}，采取逐行扫描的方式采样，扫描的第一个方向为d₀＝v₁-v₀，第二个方向为d₁＝d₀ ^⊥，d₀ ^⊥表示将向量d₀在三角形平面内延逆时针方向旋转90度，设扫描线为l，初始状态l设置为v₀v₁所在直线，算法开始，分别计算l与边v₀v₂和v₁v₂的交点记作v_beg和v_end，在线段v_begv_end上从点v_beg开始以w为间距均匀采点，将所有采样点加入S；然后将l延d₁方向平移距离w，重复上述过程直到l与边v₀v₂和v₁v₂不产生交点或两个交点重叠；

通过Gpu并行计算判断点集S是否被集合E包含的过程为：设定线程负载参数m，选取参数m＝4，将点集S扩充至m的倍数，每m个点分配到一个线程中发起计算，一个线程组中的线程数设定为1024，在单个线程中遍历m个点统计没有被集合E包含的点的数量为n，利用Gpu上的共享内存在线程组内并行归约，计算出一个线程组中没有被集合E包含的点数n'，最后由每个线程组中的第一个线程通过原子求和运算得到所有点集中未被E包含的点数k；

基于粒子群算法优化得到包络球后将半径为零的球删除，最终得到机器人关节包络球。

9.根据权利要求6所述的面向工业机器人路径规划的保守碰撞检测方法，其特征在于，所述步骤4包括如下具体步骤：

SDF在物体运动时跟随物体一起运动，在碰撞测试前先将包络球变换到SDF所在坐标系中，根据包络球球心与SDF边界的相对位置分为三种情况：当包络球的球心位于SDF边界外时一定不发生碰撞；当包络球球心位于SDF边界内，但球心所在位置的SDF值不存在，说明该位置距离模型表面最近距离大于机器人最大包络球半径，因而未发生碰撞；当包络球球心位于SDF边界内，且球心所在位置的SDF值的绝对值小于等于包络球半径时认为发生碰撞，否则认为未碰撞。

Images