一种料框与夹具快速碰撞检测的方法与流程

本发明涉及碰撞检测技术领域，更具体的，涉及一种料框与夹具快速碰撞检测的方法。

背景技术：

在立体抓取中，机械臂的抓取姿态不唯一，且不同工件的抓取位置和姿态都不一样，首先需要对抓取姿态的机械臂末端与料框的碰撞问题进行检测；由于视觉单次识别工件的数量较多，每个工件的抓取姿态也有多种，需要对每个工件的每个抓取姿态都进行一次碰撞检测，但是在工业现场存在生产节拍的时间限制，所以对碰撞检测方法的实时性要求很高，需达到准实时。

现有技术中，进行碰撞检测的方法的实时性都不够高，无法达到准实时，如冲压线搬运机器人防碰撞控制方法，公开号为cn104850699b，采用aabb包络模型进行分析，并在此基础上进行碰撞检测，但碰撞检测的实时性不够高，无法达到准实时。

技术实现要素：

本发明为克服现有的碰撞检测方法的实时性不够高的技术缺陷，提供一种料框与夹具快速碰撞检测的方法。

为解决上述技术问题，本发明的技术方案如下：

一种料框与夹具快速碰撞检测的方法，包括以下步骤：

s1：建立夹具与料框的模型；

s2：根据模型建立坐标系，并进行坐标系统一；

s3：进行底面碰撞检测；

若未发生碰撞，则执行步骤s4；

若发生碰撞，则执行步骤s7；

s4：将三维碰撞检测问题转化为二维碰撞检测问题；

s5：将二维碰撞检测问题转化为一维碰撞检测问题；

s6：检测料框侧面与夹具的碰撞情况；

s7：完成料框与夹具快速碰撞检测。

上述方案中，通过统一坐标系解决实际生产过程中料框的位置变化和旋转的问题，保证检测精度；同时，将三维碰撞问题简化为一维碰撞问题，提高了碰撞检测效率，达到了准实时的效果。

优选的，在步骤s1中，采用长方体包络面的方式对夹具进行建模，将夹具建模为一个长方体包络面；按料框每个面的实际大小对料框进行建模，将料框建模为由四个长方体面和一个底面围成的区域。

上述方案中，由于工业现场的夹具种类较多且形状各异，采用长方体包络面的方式对夹具进行建模，既可以简化碰撞检测算法又可以为碰撞检测留有一定的安全距离；按料框的实际大小对料框进行建模，包括料框的长度、宽度、高度和厚度，其中根据料框底面的长度和宽度对料框底面进行建模，根据料框侧面的长度、高度和厚度对料框侧面进行建模，从而将料框建模为由四个长方体面和一个底面围成的区域。

优选的，在步骤s2中，建立的坐标系包括机器人基坐标系o0，夹具坐标系o1和料框坐标系o2；其中，料框坐标系的原点建立在料框模型底面的一个顶点上，且其坐标轴沿着料框模型的棱。

上述方案中，机器人基坐标系由机器人本身决定；夹具坐标系以夹具模型底面的几何中心为原点，以夹具模型底面向外为z轴，以夹具开合方向为x轴，以右手定则决定y轴。

优选的，在步骤s2中，进行坐标系统一具体包括以下步骤：

s2.1：获取夹具坐标系到机器人基坐标系的转化矩阵⁰t1；

s2.2：获取料框坐标系到机器人基坐标系的转化矩阵⁰t2；

s2.3：通过⁰t1和⁰t2计算得到夹具坐标系到料框坐标系的转化矩阵；

s2.4：通过转化矩阵²t1将夹具模型的八个顶点坐标统一到料框坐标系下。

上述方案中，夹具相对于机器人基坐标系的位姿可以由机器人控制器实时获得，从而得到转化矩阵⁰t1；通过转化矩阵²t1可以将任意位姿的夹具模型的坐标投影到料框坐标系下，解决实际生产过程中料框的位置变化和旋转的问题，保证检测精度。

优选的，在步骤s2.2中，具体包括以下步骤：

s2.2.1：设初始时料框模型沿料框坐标系的x轴方向的顶点坐标为p（x，y，z），料框模型沿料框坐标系的x轴方向的长度为length；通过测量实时获取顶点坐标p（x1，y1，z1）和料框坐标系的原点相对于机械人基坐标系的平移量（x0，y0，z0）；

s2.2.2：通过计算得到料框模型沿料框坐标系的z轴的旋转角度，计算公式为：

s2.2.3：根据平移量和旋转角度得到⁰t2。

上述方案中，通过顶点坐标p的x坐标变化值计算出旋转角度，然后将平移量和旋转角度带入标准的转化矩阵从而得到料框坐标系到机械人坐标系的转化矩阵。

优选的，在步骤s3中，通过判断夹具模型的八个顶点在料框坐标系下的最小z坐标是否大于0，若大于0则认为夹具未与料框底面发生碰撞；否则认为夹具与料框底面发生碰撞。

上述方案中，先通过z坐标判断夹具与料框底面是否发生碰撞，如果未发生碰撞，则进一步判断夹具与料框侧面是否发生碰撞，从而判断出夹具与料框是否发生碰撞。

优选的，在步骤s4中，通过投影的方式，将三维碰撞检测问题转化为二维平面的矩形投影面与棱投影线段的碰撞检测问题，具体包括以下步骤：

s4.1：分别沿料框坐标系的xo2z平面和yo2z平面对料框模型的四个侧面进行投影，从而在xo2z平面和yo2z平面分别得到四个矩形投影面；

s4.2：分别沿料框坐标系的xo2z平面和yo2z平面对夹具模型的底面和侧面的棱进行投影，从而在xo2z平面和yo2z平面分别得到八条棱投影线段。

上述方案中，由于料框模型的四个侧面具有厚度，所以能够在xo2z平面和yo2z平面分别得到四个矩形投影面。若夹具模型与料框模型发生碰撞，则一定有夹具模型的棱与料框模型的侧面接触；所以分别判断矩形投影面与棱投影线段的碰撞情况，就可以对夹具与料框的碰撞进行检测，实现将三维碰撞检测问题转化为二维碰撞检测问题。

优选的，在步骤s5中，将二维碰撞检测问题转化为一维碰撞检测问题具体包括以下步骤：

s5.1：将矩形投影面投影到其所在平面的x轴或y轴上，得到面投影线段；

s5.2：根据矩形投影面对处于同一平面的棱投影线段进行处理后投影到该平面的x轴或y轴上，得到线投影线段。

上述方案中，通过比较面投影线段和线投影线段是否有重合部分，就可以确定矩形投影面与棱投影线段是否有交点，实现将二维碰撞检测问题转化为一维碰撞检测问题。

优选的，对棱投影线段进行处理具体包括以下步骤：

s5.2.1：设棱投影线段为bc，过bc作直线l；其中，b点的z轴坐标大于c点的z轴坐标；

s5.2.2：针对在xo2z平面的矩形投影面，离x轴较远的边作延长线与直线l交于a点；

针对在yo2z平面的矩形投影面，离y轴较远的边作延长线与直线l交于a点；

s5.2.3：针对在xo2z平面的矩形投影面，离x轴较近的边作延长线与直线l交于d点；

针对在yo2z平面的矩形投影面，离y轴较近的边作延长线与直线l交于d点；

s5.2.4：在a点和b点中取z轴坐标较小的点作为处理后的线段的一端端点，在c点和d点中取z轴坐标较大的点作为处理后的线段的另一端端点，完成对棱投影线段进行处理。

优选的，在步骤s6中，分别在xo2z平面和yo2z平面中，将面投影线段与线投影线段进行重合检测；

若至少有一条夹具模型的棱的线投影线段与一个料框模型的侧面的面投影线段在xo2z平面和yo2z平面中均有重合部分，则判断料框侧面与夹具有碰撞；否则，判断料框侧面与夹具没有碰撞。

上述方案中，将夹具模型的每条棱的线投影线段都与料框模型的各个侧面的面投影线段进行重合检测，若夹具模型的一条棱的线投影线段同时与料框模型的一个侧面的面投影线段在xo2z平面和yo2z平面中均有重合部分，则确定该棱与料框模型发生碰撞，进而判断夹具与料框侧面发生碰撞；若夹具模型的一条棱的线投影线段只与料框模型的一个侧面的面投影线段只在xo2z平面或yo2z平面中有重合部分，则确定该棱与料框模型不发生碰撞；如果夹具模型的所有棱都不与料框模型发生碰撞，则判断夹具与料框侧面不发生碰撞，确定该姿态安全可以进行工件抓取。

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

本发明提供了一种料框与夹具快速碰撞检测的方法，通过统一坐标系解决实际生产过程中料框的位置变化和旋转的问题，保证检测精度；同时，将三维碰撞问题简化为一维碰撞问题，提高了碰撞检测效率，达到了准实时的效果。

附图说明

图1为本发明的技术方案步骤实施流程图；

图2为本发明中机器人基坐标系，夹具坐标系和料框坐标系的关系示意图；

图3为本发明中将二维碰撞检测问题转化为一维碰撞检测问题的原理示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

如图1所示，一种料框与夹具快速碰撞检测的方法，包括以下步骤：

s1：建立夹具与料框的模型；

s2：根据模型建立坐标系，并进行坐标系统一；

s3：进行底面碰撞检测；

若未发生碰撞，则执行步骤s4；

若发生碰撞，则执行步骤s7；

s4：将三维碰撞检测问题转化为二维碰撞检测问题；

s5：将二维碰撞检测问题转化为一维碰撞检测问题；

s6：检测料框侧面与夹具的碰撞情况；

s7：完成料框与夹具快速碰撞检测。

在实施过程中，通过统一坐标系解决实际生产过程中料框的位置变化和旋转的问题，保证检测精度；同时，将三维碰撞问题简化为一维碰撞问题，提高了碰撞检测效率，达到了准实时的效果。

更具体的，在步骤s1中，采用长方体包络面的方式对夹具进行建模，将夹具建模为一个长方体包络面；按料框每个面的实际大小对料框进行建模，将料框建模为由四个长方体面和一个底面围成的区域。

在实施过程中，由于工业现场的夹具种类较多且形状各异，采用长方体包络面的方式对夹具进行建模，既可以简化碰撞检测算法又可以为碰撞检测留有一定的安全距离；按料框的实际大小对料框进行建模，包括料框的长度、宽度、高度和厚度，其中根据料框底面的长度和宽度对料框底面进行建模，根据料框侧面的长度、高度和厚度对料框侧面进行建模，从而将料框建模为由四个长方体面和一个底面围成的区域，在matlab中对整个抓取的场景进行建模。

更具体的，如图2所示，在步骤s2中，建立的坐标系包括机器人基坐标系o0，夹具坐标系o1和料框坐标系o2；其中，料框坐标系的原点建立在料框模型底面的一个顶点上，且其坐标轴沿着料框模型的棱。

在实施过程中，机器人基坐标系由机器人本身决定；夹具坐标系以夹具模型底面的几何中心为原点，以夹具模型底面向外为z轴，以夹具开合方向为x轴，以右手定则决定y轴。

更具体的，在步骤s2中，进行坐标系统一具体包括以下步骤：

s2.1：获取夹具坐标系到机器人基坐标系的转化矩阵⁰t1；

s2.2：获取料框坐标系到机器人基坐标系的转化矩阵⁰t2；

s2.3：通过⁰t1和⁰t2计算得到夹具坐标系到料框坐标系的转化矩阵；

s2.4：通过转化矩阵²t1将夹具模型的八个顶点坐标统一到料框坐标系下。

在实施过程中，夹具相对于机器人基坐标系的位姿可以由机器人控制器实时获得，从而得到转化矩阵⁰t1；通过转化矩阵²t1可以将任意位姿的夹具模型的坐标投影到料框坐标系下，解决实际生产过程中料框的位置变化和旋转的问题，保证检测精度。

更具体的，在步骤s2.2中，具体包括以下步骤：

s2.2.1：设初始时料框模型沿料框坐标系的x轴方向的顶点坐标为p（x，y，z），料框模型沿料框坐标系的x轴方向的长度为length；通过测量实时获取顶点坐标p（x1，y1，z1）和料框坐标系的原点相对于机械人基坐标系的平移量（x0，y0，z0）；

s2.2.2：通过计算得到料框模型沿料框坐标系的z轴的旋转角度，计算公式为：

s2.2.3：根据平移量和旋转角度得到⁰t2。

在实施过程中，通过顶点坐标p的x坐标变化值计算出旋转角度，然后将平移量和旋转角度带入标准的转化矩阵从而得到料框坐标系到机械人坐标系的转化矩阵。

更具体的，在步骤s3中，通过判断夹具模型的八个顶点在料框坐标系下的最小z坐标是否大于0，若大于0则认为夹具未与料框底面发生碰撞；否则认为夹具与料框底面发生碰撞。

在实施过程中，先通过z坐标判断夹具与料框底面是否发生碰撞，如果未发生碰撞，则进一步判断夹具与料框侧面是否发生碰撞，从而判断出夹具与料框是否发生碰撞。

更具体的，在步骤s4中，通过投影的方式，将三维碰撞检测问题转化为二维平面的矩形投影面与棱投影线段的碰撞检测问题，具体包括以下步骤：

s4.1：分别沿料框坐标系的xo2z平面和yo2z平面对料框模型的四个侧面进行投影，从而在xo2z平面和yo2z平面分别得到四个矩形投影面；

s4.2：分别沿料框坐标系的xo2z平面和yo2z平面对夹具模型的底面和侧面的棱进行投影，从而在xo2z平面和yo2z平面分别得到八条棱投影线段。

在实施过程中，由于料框模型的四个侧面具有厚度，所以能够在xo2z平面和yo2z平面分别得到四个矩形投影面。若夹具模型与料框模型发生碰撞，则一定有夹具模型的棱与料框模型的侧面接触；所以分别判断矩形投影面与棱投影线段的碰撞情况，就可以对夹具与料框的碰撞进行检测，实现将三维碰撞检测问题转化为二维碰撞检测问题。

更具体的，如图3所示，在步骤s5中，将二维碰撞检测问题转化为一维碰撞检测问题具体包括以下步骤：

s5.1：将矩形投影面投影到其所在平面的x轴或y轴上，得到面投影线段gh；

s5.2：根据矩形投影面对处于同一平面的棱投影线段进行处理后投影到该平面的x轴或y轴上，得到线投影线段ef。

在实施过程中，通过比较面投影线段gh和线投影线段ef是否有重合部分，就可以确定矩形投影面与棱投影线段是否有交点，实现将二维碰撞检测问题转化为一维碰撞检测问题。

更具体的，对棱投影线段进行处理具体包括以下步骤：

s5.2.1：设棱投影线段为bc，过bc作直线l；其中，b点的z轴坐标大于c点的z轴坐标；

s5.2.2：针对在xo2z平面的矩形投影面，离x轴较远的边作延长线与直线l交于a点；

针对在yo2z平面的矩形投影面，离y轴较远的边作延长线与直线l交于a点；

s5.2.3：针对在xo2z平面的矩形投影面，离x轴较近的边作延长线与直线l交于d点；

针对在yo2z平面的矩形投影面，离y轴较近的边作延长线与直线l交于d点；

s5.2.4：在a点和b点中取z轴坐标较小的点作为处理后的线段的一端端点，在c点和d点中取z轴坐标较大的点作为处理后的线段的另一端端点，完成对棱投影线段进行处理。

更具体的，在步骤s6中，分别在xo2z平面和yo2z平面中，将面投影线段与线投影线段进行重合检测；

若至少有一条夹具模型的棱的线投影线段与一个料框模型的侧面的面投影线段在xo2z平面和yo2z平面中均有重合部分，则判断料框侧面与夹具有碰撞；否则，判断料框侧面与夹具没有碰撞。

在实施过程中，将夹具模型的每条棱的线投影线段都与料框模型的各个侧面的面投影线段进行重合检测，若夹具模型的一条棱的线投影线段同时与料框模型的一个侧面的面投影线段在xo2z平面和yo2z平面中均有重合部分，则确定该棱与料框模型发生碰撞，进而判断夹具与料框侧面发生碰撞；若夹具模型的一条棱的线投影线段只与料框模型的一个侧面的面投影线段只在xo2z平面或yo2z平面中有重合部分，则确定该棱与料框模型不发生碰撞；如果夹具模型的所有棱都不与料框模型发生碰撞，则判断夹具与料框侧面不发生碰撞，确定该姿态安全可以进行工件抓取。

在本实施例中，根据步骤s1建立夹具模型和料框模型，其中，料框模型的参数设定为：长度=450，宽度=300，高度=150；夹具模型的参数设定为：长度=170，宽度=50，高度=350；经过步骤s2、步骤s3、步骤s4、步骤s5处理后，根据步骤s6检测出料框与夹具的碰撞情况；本实施例共进行了3100组实验，其中，碰撞检测所需时间最短为0.03ms，最长为0.2ms，平均所需时间为0.1ms。因此，本实施例与现有技术相比，碰撞检测所需时间短，实现了准实时的碰撞检测。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。