本发明涉及数据处理技术领域，尤其涉及一种基于包围盒算法的高精度机械臂碰撞检测方法。
背景技术：
在工业机械臂运动过程中，为了确保工件以及机械臂自身在运动中的安全性，避免碰撞是必要条件，否则易导致机械臂损坏和工件表面划伤，并可能对工作人员的安全构成威胁，因此碰撞检测是机械臂在避障运动规划中的重要内容。
碰撞检测问题的本质是求物体之间的最短距离，在距离小于或等于零时则认为物体之间接触或发生碰撞，所以计算空间中物体的距离是必要的。而多数情况下物体形状不规则，目前的解决方案主要包括空间分解法(spacepartitioning)包围盒法(boundingvolume)和层次包围盒法(boundingvolumehierarchy)，其中层次包围盒法在复杂环境的碰撞检测效果显著，更适用于机械臂的碰撞检测问题。
包围盒的基本思想是通过简化复杂物体轮廓，用一个简单的立体几何将物体包裹住，转化为不同包围盒之间距离的计算，从而达到碰撞检测的目的，常见的包围盒轴向包围盒(axis-alignedboundingboxes:aabb)、有向包围盒(orientedboundingbox:obb)、离散方向多面体(discreteorientationpolytopes:k-dop)等。虽然利用包围盒简化物体轮廓，但是检测精度不能保证。目前的包围盒碰撞检测技术存在检测精度不高的情况，特别在形状不规则物体之间的检测存在误差较大的情况。
技术实现要素：
本发明的目的在于提供一种基于包围盒算法的高精度机械臂碰撞检测方法，提高检测精度，降低检测误差。
为实现上述目的，本发明提供了一种基于包围盒算法的高精度机械臂碰撞检测方法，包括：
获取机械臂和工件的包围盒，并进行相交测试；
获取所述机械臂和工件的包围盒相交的交点和重叠区域，得到线段解；
根据获取的参考边进行工件识别并确定旋转射线；
将所述旋转射线进行合并和迭代，完成包围盒的分离。
其中，所述在获取机械臂和工件的包围盒，并进行相交测试前，所述方法还包括：
按照设定测试要求对机械臂自身进行碰撞测试，若所述机械臂连杆有互相接触，则结束碰撞检测，若无互相接触，则进行机械臂与工件的相交检测。
其中，所述获取机械臂和工件的包围盒，并进行相交测试，包括：
分别对所述机械臂和工件进行建模以及模型简化，并根据计算得到的机械臂和工件的包围盒进行相交测试，若不相交则结束检测，若相交则计算出线段解。
其中，获取所述机械臂和工件的包围盒相交的交点和重叠区域，得到线段解，包括：
获取所述机械臂和工件的包围盒相交的两个交点和所述交点形成的矩形重叠区域，并在所述矩形重叠区域内连接所述交点，计算得到线段解。
其中，获取所述机械臂和工件的包围盒相交的交点和重叠区域，得到线段解，还包括：
将基于设定条件得到起点和终点进行连接产生超平面，若产生的所述超平面与所述机械臂和所述工件均不相交，则得到唯一线段解，若产生的所述超平面与所述机械臂和所述工件任意一个相交，则得到的所述线段解无效。
其中，所述根据获取的参考边进行工件识别并确定旋转射线，包括：
根据获取的参考边得到识别物体的交点序列组成的第一射线，同时得到与所述交点序列依次相交的第一物体和第二物体，并在所述矩形重叠区域内，由所述起点处进行投射或按照设定角度进行旋转，得到第一条与所述第一物体不相交的第二射线，所述第二射线为旋转射线。
其中，将所述旋转射线进行合并和迭代，完成包围盒的分离，包括：
基于不同的工件的几何形状，判断所述旋转射线与所述线段解是否发生接触，进而进行所述旋转射线的合并和迭代，得到多超平面解，完成所述机械臂和所述工件的相交包围盒的分离。
其中，判断所述旋转射线与所述线段解是否发生接触，进而进行所述旋转射线的合并和迭代，包括：
若所述旋转射线位于前一个所述起点和目前所述起点之间，且是所述线段解的一部分，则重新获得所述线段解并确定所述旋转射线，然后搜索每个新的所述线段解，直到当前的所述起点和所述终点重合，并进行所述旋转射线的合并；
若所述旋转射线与所述线段解发生接触，则终止当前线段的搜索，无法分离包围盒；
若所述旋转射线与所述第一物体相交，且相交线不在所述矩形重叠区域的任何边上，则构建一个与原所述参考边平行并经过所述起点的新参考边，并将所述第一物体和所述第二物体的位置互相交换，反转所述旋转射线的方向，进行旋转射线的迭代。
其中，在反转所述旋转射线的方向，进行旋转射线的迭代中，所述方法还包括：
若在反转时未找到所述旋转射线，则将所述第一射线代替所述旋转射线，并纳入多超平面解中，并保持所述参考边在下一次迭代中保持不变；
若所述旋转射线与所述矩形重叠区域相交，则将相交边替换为所述参考边，然后进行所述旋转射线的迭代。
本发明的一种基于包围盒算法的高精度机械臂碰撞检测方法，按照设定测试要求对机械臂自身进行碰撞测试，在无互相接触时，分别对所述机械臂和工件进行建模以及模型简化，并根据计算得到的机械臂和工件的包围盒进行相交测试，当包围盒相交时，获取所述机械臂和工件的包围盒相交的两个交点和所述交点形成的矩形重叠区域，并在所述矩形重叠区域内连接所述交点，计算得到线段解，并当所述线段解无效时，根据获取的参考边进行工件识别并确定旋转射线，基于不同的工件的几何形状，判断所述旋转射线与所述线段解是否发生接触，进而进行所述旋转射线的合并和迭代，得到多超平面解，完成所述机械臂和所述工件的相交包围盒的分离，提高检测精度，降低检测误差。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明提供的一种基于包围盒算法的高精度机械臂碰撞检测方法的步骤示意图。
图2是本发明提供的矩形重叠区域的划分示意图。
图3是本发明提供的线段解的确认示意图。
图4是本发明提供的区分有无线段解的包围盒示意图。
图5是本发明提供的分离包围盒的示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
请参阅图1，本发明提供一种基于包围盒算法的高精度机械臂碰撞检测方法，包括：
s101、获取机械臂和工件的包围盒，并进行相交测试。
具体的，首先，按照设定测试要求对机械臂自身进行碰撞测试，若所述机械臂连杆有互相接触，则结束碰撞检测，若无互相接触，则进行机械臂与工件的相交检测，在机械臂避障研究中，首先需要分别对机械臂和障碍物进行建模以及模型简化，分别计算出机械臂和工件的包围盒。根据得到的包围盒对机械臂和工件进行相交测试，若不相交则结束检测，若相交则计算出线段解。
s102、获取所述机械臂和工件的包围盒相交的交点和重叠区域，得到线段解。
具体的，当包围盒不相交时，则认为物体不发生接触。当包围盒相交时，需要判断机械臂与工件是否发生接触的情况。如果机械臂和工件各自的包围盒重叠，则将彼此分割成两部分，并生成两个交点，这两个交点形成一个矩形重叠区域r，交点会出现在区域的同一边，如图2提供的矩形重叠区域的划分示意图a所示；或在对角的一边，如图2提供的矩形重叠区域的划分示意图b所示。由于这个重叠区域实际并不造成机械臂与工件接触，所以存在由一个或多个相互连接的超平面组成的解，在区域内连接两个交点并且将两个包围盒分割。实际上其中会存在多种解，本发明提出的算法将保证得到一个线段解s。基于旋转光线的思想来得到线段解s，具体算法描述如下：
(1)选择起点。以邻边至少与一个包围盒相交为原则选取一个交点，记作pstart，如图3提供的线段解的确认示意图a所示。若两个点都满足要求，则选择其中一个作为起点。若两个交点都不满足要求，则在下一步中直接得到一个线段解s,如图3提供的线段解的确认示意图b所示。
(2)提前终止的情况。在线段解s产生之前，有两种情况可能直接产生解，导致算法提前终止，在此提前处理这种情况：
i.如图3提供的线段解的确认示意图b所示，通过连接pstart和pend产生一个超平面，可以确保直接得到一个线段解s。
ii.如果起点终点在矩形重叠区域r的对角线上，且连接的超平面不与物体相交，则有线段解s，如图4提供的区分有无线段解的包围盒示意图a所示。若该超平面与任一物体相交，如图4提供的区分有无线段解的包围盒示意图b所示，则该解无效，然后进行求取多超平面解。
s103、根据获取的参考边进行工件识别并确定旋转射线。
具体的，根据获取的参考边er得到识别物体的交点序列组成的第一射线rtrial，同时得到与所述交点序列依次相交的第一物体os和第二物体oε，并在所述矩形重叠区域内，由所述起点pstart处进行投射或按照设定角度进行旋转，得到第一条与所述第一物体os不相交的第二射线，所述第二射线为旋转射线rfirst-collision，其中，所述参考边er为矩形重叠区域r的一个边，且这条边从起点pstart开始，记作er，它最初是与接触物体相交的那一条边，然而在后面的步骤中，矩形重叠区域r中的其他边也可被定义为新的er，若起点的两个邻边都符合er，则可以任意选择；如图5提供的分离包围盒的示意图a所示，第一段射线表示初始选择的矩形重叠区域r的左边缘，给出了一种沿着er的第一射线rtrial来识别有界物体的交点序列，第一个相交的物体(机械臂或工件)记为os，则另一个物体记为oε，os和oε定义了线段搜索的条件。并根据os和oε，则将起点pstart处投射的碰撞测试射线定义为旋转射线rfirst-collision，即第一条不与os碰撞的射线，这是通过将射线围绕pstart旋转一个间隔角度θ来实现的，射线投射从er方向开始，并向使射线保持在矩形重叠区域r的范围内旋转。根据参考边er和矩形重叠区域r的相对位置，射线旋转可以是顺时针的，也可以是逆时针。
s104、将所述旋转射线进行合并和迭代，完成包围盒的分离。
具体的，根据不同的工件几何形状，判断所述旋转射线与所述线段解是否发生接触，进而进行所述旋转射线的合并和迭代，得到多超平面解，完成所述机械臂和所述工件的相交包围盒的分离，而在查找旋转射线rfirst-collision时会出现三种情况，如下所示：若旋转射线rfirst-collision位于前一个pnew和目前的pnew之间，且是解的一部分。则重新获得所述线段解并确定所述旋转射线，然后搜索每个新的所述线段解，直到当前的所述起点和所述终点重合，并进行所述旋转射线rfirst-collision的合并。最后的线段解s由每条线段组成。
其中，射线的旋转间隔角最大为180度。如果旋转射线rfirst-collision与之前找到的任何解段发生接触，则终止当前线段的搜索过程，因为得到的解将无法分离包围盒。下面介绍另一种可能发生的情况，如下所示：
i.如果旋转射线rfirst-collision与第一物体os相交，但交线不在矩形重叠区域r的任何边上，则构建一个与原参考边er平行并经过新起点pnew的新参考边er。然后将第一物体os和第二物体oε的位置互相交换，在下一次迭代过程中，反转所述旋转射线的方向，进行旋转射线的迭代。
ii.如果在反转过程中找不到旋转射线rfirst-collision，则使用第一射线rtrial代替旋转射线rfirst-collision，并将其纳入多超平面解中，在这种情况下，参考边er在下一次迭代中保持不变。
iii.如果旋转射线rfirst-collision与矩形重叠区域r的一条边相交，则这条相交的边被记为新的参考边er，然后进行所述旋转射线rfirst-collision的迭代。
如图5提供的分离包围盒的示意图a所示，根据以上步骤分离两个接触包围盒，线段解s由每一段射线表示的超平面组成。如图5提供的分离包围盒的示意图b所示，线段解s与原始的包围盒一起形成两个新的多边形，完成两物体相交包围盒的分离，改善了传统包围盒算法存在较大误差的问题，提高了检测效率，适用于对精度要求较高的机械臂工作场合。
本发明的一种基于包围盒算法的高精度机械臂碰撞检测方法，按照设定测试要求对机械臂自身进行碰撞测试，在无互相接触时，分别对所述机械臂和工件进行建模以及模型简化，并根据计算得到的机械臂和工件的包围盒进行相交测试，当包围盒相交时，获取所述机械臂和工件的包围盒相交的两个交点和所述交点形成的矩形重叠区域，并在所述矩形重叠区域内连接所述交点，计算得到线段解，并当所述线段解无效时，根据获取的参考边进行工件识别并确定旋转射线，基于不同的工件的几何形状，判断所述旋转射线与所述线段解是否发生接触，进而进行所述旋转射线的合并和迭代，得到多超平面解，完成所述机械臂和所述工件的相交包围盒的分离，提高检测精度，降低检测误差。
以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。
1.一种基于包围盒算法的高精度机械臂碰撞检测方法，其特征在于，包括：
获取机械臂和工件的包围盒，并进行相交测试；
获取所述机械臂和工件的包围盒相交的交点和重叠区域，得到线段解；
根据获取的参考边进行工件识别并确定旋转射线；
将所述旋转射线进行合并和迭代，完成包围盒的分离。
2.如权利要求1所述的一种基于包围盒算法的高精度机械臂碰撞检测方法，其特征在于，所述在获取机械臂和工件的包围盒，并进行相交测试前，所述方法还包括：
按照设定测试要求对机械臂自身进行碰撞测试，若所述机械臂连杆有互相接触，则结束碰撞检测，若无互相接触，则进行机械臂与工件的相交检测。
3.如权利要求2所述的一种基于包围盒算法的高精度机械臂碰撞检测方法，其特征在于，所述获取机械臂和工件的包围盒，并进行相交测试，包括：
分别对所述机械臂和工件进行建模以及模型简化，并根据计算得到的机械臂和工件的包围盒进行相交测试，若不相交则结束检测，若相交则计算出线段解。
4.如权利要求3所述的一种基于包围盒算法的高精度机械臂碰撞检测方法，其特征在于，获取所述机械臂和工件的包围盒相交的交点和重叠区域，得到线段解，包括：
获取所述机械臂和工件的包围盒相交的两个交点和所述交点形成的矩形重叠区域，并在所述矩形重叠区域内连接所述交点，计算得到线段解。
5.如权利要求4所述的一种基于包围盒算法的高精度机械臂碰撞检测方法，其特征在于，获取所述机械臂和工件的包围盒相交的交点和重叠区域，得到线段解，还包括：
将基于设定条件得到起点和终点进行连接产生超平面，若产生的所述超平面与所述机械臂和所述工件均不相交，则得到唯一线段解，若产生的所述超平面与所述机械臂和所述工件任意一个相交，则得到的所述线段解无效。
6.如权利要求5所述的一种基于包围盒算法的高精度机械臂碰撞检测方法，其特征在于，所述根据获取的参考边进行工件识别并确定旋转射线，包括：
根据获取的参考边得到识别物体的交点序列组成的第一射线，同时得到与所述交点序列依次相交的第一物体和第二物体，并在所述矩形重叠区域内，由所述起点处进行投射或按照设定角度进行旋转，得到第一条与所述第一物体不相交的第二射线，所述第二射线为旋转射线。
7.如权利要求6所述的一种基于包围盒算法的高精度机械臂碰撞检测方法，其特征在于，将所述旋转射线进行合并和迭代，完成包围盒的分离，包括：
基于不同的工件的几何形状，判断所述旋转射线与所述线段解是否发生接触，进而进行所述旋转射线的合并和迭代，得到多超平面解，完成所述机械臂和所述工件的相交包围盒的分离。
8.如权利要求7所述的一种基于包围盒算法的高精度机械臂碰撞检测方法，其特征在于，判断所述旋转射线与所述线段解是否发生接触，进而进行所述旋转射线的合并和迭代，包括：
若所述旋转射线位于前一个所述起点和目前所述起点之间，且是所述线段解的一部分，则重新获得所述线段解并确定所述旋转射线，然后搜索每个新的所述线段解，直到当前的所述起点和所述终点重合，并进行所述旋转射线的合并；
若所述旋转射线与所述线段解发生接触，则终止当前线段的搜索，无法分离包围盒；
若所述旋转射线与所述第一物体相交，且相交线不在所述矩形重叠区域的任何边上，则构建一个与原所述参考边平行并经过所述起点的新参考边，并将所述第一物体和所述第二物体的位置互相交换，反转所述旋转射线的方向，进行旋转射线的迭代。
9.如权利要求8所述的一种基于包围盒算法的高精度机械臂碰撞检测方法，其特征在于，在反转所述旋转射线的方向，进行旋转射线的迭代中，所述方法还包括：
若在反转时未找到所述旋转射线，则将所述第一射线代替所述旋转射线，并纳入多超平面解中，并保持所述参考边在下一次迭代中保持不变；
若所述旋转射线与所述矩形重叠区域相交，则将相交边替换为所述参考边，然后进行所述旋转射线的迭代。
技术总结