在机器人运动规划中避免运动学奇异点的方法及装置与流程

本发明涉及一种在机器人运动规划中避免运动学奇异点的方法，具体而言，是一种在球形腕工业机器人运动规划中避免运动学奇异点的方法。本发明还涉及一种在机器人运动规划中避免运动学奇异点的装置，具体而言，是一种在球形腕工业机器人运动规划中避免运动学奇异点的装置。

背景技术：

随着劳动力成本的不断提高以及工业机器人控制、规划、识别方法的不断发展，用工业机器人代替人工进行自动化生产的程度越来越高。但目前工业中，常用的带有球形腕的工业机器人在执行复杂环境中的未知操作时，常常会遇到运动学奇异点，严重影响了其通用性和实用性。

机器人相邻的三个旋转关节的转轴交于同一点即为球形腕。对于带有球形腕的工业机器人，当球形腕第二转轴角度接近0度时，球形腕第一转轴和第三转轴接近重合，即形成腕部奇异点。奇异点会导致机器人在执行空间中指定路径的时候，关节旋转速度过大，导致性能降低、实用性降低，甚至可能发生危险。因此，简单高效的、用于避免运动学奇异点的规划算法对于工业机器人的实用性至关重要。

中国发明专利cn101549495b披露了一种避免奇异点发生的机器人控制方法，在该方法中，在试运行阶段检测和计算奇异点并将其存入数据库中，然后在正式运行阶段则将实际运行过程中的检测和计算结果与数据库中的数据相比较，修正机器人的运行轨迹而避免奇异点的发生。

中国发明专利申请cn103802114a披露了一种工业机器人奇异点处理方法及装置，在该方法中，建立机器人的运动学模型，获取机器人运动学的正反解，并据此获得工业机器人处于奇异点的条件值，根据所述条件值判断工业机器人是否处于奇异点，如果是，则控制工业机器人通过该奇异点。

中国发明专利cn105382835b披露了一种可穿越腕部奇异点的机器人路径规划方法，在该方法中，包括拖拽机器人的末端完成一次完整的作业的示教步骤。

中国发明专利申请cn105772905a披露了一种基于弧焊机器人系统的斜交偏置管轨迹规划方法，该方法中建立了相应的斜交偏置管数学模型并求取了空间焊接曲线方程，结合曲线论中弗莱纳－雪列三元空间矢量原理，求出了该空间焊缝路径上各离散点的位姿曲线，并对焊枪的姿态进行了规划，最后通过一种特殊的示教方式标定了在实际焊接时斜交偏置管坐标系与机器人基座标系之间的关系。

因此，仍然需要一种能够自动进行、无需事先示教的用于在球形腕工业机器人运动规划中避免运动学奇异点的方法。

技术实现要素：

本发明所要解决的技术问题是在带有球形腕的工业机器人的运动规划中避免该工业机器人由于球形腕引入的运动学奇异点。

这一技术问题由根据本发明的一种在球形腕工业机器人运动规划中避免运动学奇异点的方法加以解决。在球形腕工业机器人，每个所述球形腕由该工业机器人的转轴轴线相交于一点的三个相邻旋转关节构成。该方法包括如下步骤：建立所述工业机器人的运动学模型(步骤s1)；根据所述运动学模型求出预规划路径的路径点的运动学反解并根据所求出的运动学反解建立所述工业机器人的关节空间的运动学轨迹(步骤s2)；判断所述运动学轨迹是否经过球形腕的运动学奇异点(步骤s3)；当判定所述运动学轨迹不经过球形腕的任何运动学奇异点时，执行所述预规划路径(步骤s4)；当判定所述运动学轨迹经过所述球形腕运动学奇异点时，则将所述预规划路径的起始点的角度进行微小偏转而得到新的规划路径(步骤s5)，然后根据所述运动学模型求出新的规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的新的运动学轨迹(步骤s6)；判断所述新的运动学轨迹是否经过球形腕的运动学奇异点(步骤s7)；当判定新的运动学轨迹仍经过球形腕的运动学奇异点时，放弃执行上述述预规划路径(步骤s8)；当判定新的运动学轨迹不经过球形腕的任何运动学奇异点时，执行所述新的规划路径(步骤s9)。

根据本发明的在球形腕工业机器人运动规划中避免运动学奇异点的方法的一种优选实施形式，判断所述运动学轨迹是否经过球形腕的运动学奇异点的步骤s3包括：计算出所述预规划路径中每个路径点的运动学反解中每个关节的角度与上一个路径点的运动学反解中对应的关节的角度之差，由此得出该关节在运动到所述每个路径点时的转动速度，并进而判断该转动速度是否超过一个预定的速度阈值；若该关节的所述转动速度超过所述速度阈值，则判断球形腕的沿从该工业机器人基座至终端执行器的方向看的第二个关节的关节角度的绝对值是否小于零点阈值，若该关节角度的绝对值小于零点阈值，则判定该运动路径经过球形腕运动学奇异点。

根据本发明的在球形腕工业机器人运动规划中避免运动学奇异点的方法的一种优选实施形式，在建立所述工业机器人的运动学模型时，根据所述工业机器人的预规划路径中的起始点终端执行器姿态t1和结束点终端执行器姿态t2，通过插值方法计算预规划路径中的路径点。

具体而言，优选的是，在执行所述插值方法时，首先将所述起始点终端执行器姿态矩阵t1分解为起始点终端执行器姿态矩阵平移部分p1和起始点终端执行器姿态矩阵旋转部分q1，将所述结束点终端执行器姿态矩阵t2分解为结束点终端执行器姿态矩阵平移部分p2和结束点终端执行器姿态矩阵旋转部分q2；然后，分别对所述起始点终端执行器姿态矩阵平移部分p1和所述结束点终端执行器姿态矩阵平移部分p2进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵平移部分pn，对所述起始点终端执行器姿态矩阵旋转部分q1和所述结束点终端执行器姿态矩阵旋转部分q2进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分qn；接着，合成所述预规划路径中的路径点的终端执行器姿态矩阵平移部分pn和所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分qn而得到所述预规划路径中的各路径点的终端执行器姿态矩阵tn。

根据本发明的在球形腕工业机器人运动规划中避免运动学奇异点的方法的一种优选实施形式，所述起始点终端执行器姿态矩阵平移部分p1和所述结束点终端执行器姿态矩阵平移部分p2用三维向量的形式表示，所述起始点终端执行器姿态矩阵旋转部分q1和所述结束点终端执行器姿态矩阵旋转部分q2用四元数的形式表示。

根据本发明的在球形腕工业机器人运动规划中避免运动学奇异点的方法的一种优选实施形式，将所述预规划路径的起始点的角度进行微小偏转而得到新的规划路径的步骤s5包括：首先，生成表示旋转的第一旋转四元数(qbias1)和第二旋转四元数qbias2，其中，第一旋转四元数(qbias1)的旋转轴与所述起始点终端执行器姿态矩阵旋转部分q1的旋转轴相同，第二旋转四元数qbias2的旋转轴与所述结束点终端执行器姿态矩阵旋转部分q2的旋转轴相同；然后，以所述起始点终端执行器姿态矩阵旋转部分q1和所述第一旋转四元数(qbias1)的乘积q1×qbias1作为新的起始点终端执行器姿态矩阵旋转部分与所述起始点终端执行器姿态矩阵平移部分p1一起重新构建新的规划路径的起始点，以所述结束点终端执行器姿态矩阵旋转部分q2和所述第二旋转四元数(qbias2)的乘积q2×qbias2作为新的所述结束点终端执行器姿态矩阵旋转部分与所述起始点终端执行器姿态矩阵平移部分p2一起重新构建新的规划路径的结束点。

装置方面，这一技术问题通过根据本发明的一种在球形腕工业机器人运动规划中避免运动学奇异点的装置加以解决。该装置包括：运动学模型处理模块，该运动学模型处理模块被设计用于建立所述工业机器人的运动学模型，根据所述运动学模型求出预规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的运动学轨迹；奇异点判断模块，该奇异点判断模块被设计用于判断运动学轨迹是否经过球形腕的运动学奇异点；规避路径规划模块，该规避路径规划模块被设计用于将所述预规划路径的起始点的角度进行微小偏转而得到新的规划路径，然后根据所述运动学模型求出新的规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的新的运动学轨迹。

根据本发明的在球形腕工业机器人运动规划中避免运动学奇异点的装置的一种优选实施形式，所述奇异点判断模块包括：关节角度比较单元，该关节角度比较单元被设计用于计算出所述预规划路径中每个路径点的运动学反解中每个关节的角度与上一个路径点的运动学反解中对应的关节的角度之差，由此得出该关节在运动到所述每个路径点时的转动速度，并进而判断该转动速度是否超过一个预定的速度阈值；以及奇异点判定单元，该奇异点判定单元被设计用于判断球形腕的沿从该工业机器人基座至终端执行器的方向看的第二个关节的关节角度的绝对值是否小于零点阈值，若该关节角度的绝对值小于零点阈值，则判定该运动路径经过球形腕运动学奇异点。

根据本发明的在球形腕工业机器人运动规划中避免运动学奇异点的装置的一种优选实施形式，所述运动学模型处理模块包括：姿态矩阵分解单元，该矩阵分解单元被设计用于将所述起始点终端执行器姿态矩阵t1分解为起始点终端执行器姿态矩阵平移部分p1和起始点终端执行器姿态矩阵旋转部分q1，将所述结束点终端执行器姿态矩阵t2分解为结束点终端执行器姿态矩阵平移部分p2和结束点终端执行器姿态矩阵旋转部分q2；插值单元，该插值单元被设计用于分别对所述起始点终端执行器姿态矩阵平移部分p1和所述结束点终端执行器姿态矩阵平移部分p2进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵平移部分pn，对所述起始点终端执行器姿态矩阵旋转部分q1和所述结束点终端执行器姿态矩阵旋转部分q2进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分qn；以及姿态矩阵合成单元，该矩阵合成单元被设计用于合成所述预规划路径中的路径点的终端执行器姿态矩阵平移部分pn和所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分qn而得到所述预规划路径中的各路径点的终端执行器姿态矩阵tn。

根据本发明的在球形腕工业机器人运动规划中避免运动学奇异点的装置的一种优选实施形式，所述规避路径规划模块包括：旋转四元数生成单元，该旋转四元数生成单元被设计用于生成表示旋转的第一旋转四元数(qbias1)和第二旋转四元数qbias2，其中，第一旋转四元数(qbias1)的旋转轴与所述起始点终端执行器姿态矩阵旋转部分q1的旋转轴相同，第二旋转四元数qbias2的旋转轴与所述结束点终端执行器姿态矩阵旋转部分q2的旋转轴相同；姿态矩阵构建单元，该姿态矩阵构建单元被设计用于以所述起始点终端执行器姿态矩阵旋转部分q1和所述第一旋转四元数(qbias1)的乘积q1×qbias1作为新的起始点终端执行器姿态矩阵旋转部分与所述起始点终端执行器姿态矩阵平移部分p1一起重新构建新的规划路径的起始点，以所述结束点终端执行器姿态矩阵旋转部分q2和所述第二旋转四元数(qbias2)的乘积q2×qbias2作为新的所述结束点终端执行器姿态矩阵旋转部分与所述起始点终端执行器姿态矩阵平移部分p2一起重新构建新的规划路径的结束点。

附图说明

在下文中根据实施例结合附图详细描述本发明。在附图中：

图1是工业机器人的球形腕的示意图；

图2是带有球形腕的工业机器人常见的路径，也即预规划路径中机器人各关节角的变化曲线，该路径包括若干由于球形腕的运动学特点导致的运动学奇异点；

图3是基于所述球形腕工业机器人运动规划中避免奇异点的方法重新为机器人规划的路径，即新的运动路径中机器人各关节角的变化曲线，其中，机器人终端执行器在空间中的位置与原路径保持相同，角度有微小的变化，该路径中机器人各个关节的变化过程相对平滑均匀；

图4为所述球形腕工业机器人运动规划中避免奇异点的方法的流程示意图。

具体实施方式

下面结合具体实施方式对本发明的技术方案作进一步详细地说明。

图1为工业机器人的球形腕的示意图。该工业机器人的转轴轴线相交于一点的三个相邻旋转关节构成每个所述球形腕。当球形腕的沿从该工业机器人基座至终端执行器的方向看的第二个关节的关节角度的绝对值接近0度时，球形腕的第一个关节的转轴和第三个关节的转轴接近重合，即形成腕部奇异点。图2为带球形腕的工业机器人常见的通过运动学奇异点的运动路径，该运动路径的奇异性是由于球形腕的运动学特点导致的。图3为图2所示路径中机器人各关节角的变化曲线。可以看到其关节变化曲线在某些位置十分陡峭。

图4示意地示出了所述球形腕工业机器人运动规划中避免奇异点的方法的流程图。

为避免奇异点，在本发明的第一实施例中，首先建立所述工业机器人的运动学模型(步骤s1)，并根据所述运动学模型求出预规划路径的路径点的运动学反解并根据所求出的运动学反解建立所述工业机器人的关节空间的运动学轨迹(步骤s2)。在此，通过机器视觉模块等其他方法得到预规划路径的起始点和结束点处终端执行器姿态矩阵t1和t2，通过插值的方法计算预规划路径中各个路径点处的终端执行器姿态矩阵tn。具体步骤是，先将各个路径点处的终端执行器姿态矩阵t1和t2分解为其平移部分p1，p2(以三维向量的形式表示)和旋转部分q1，q2(以四元数的形式表示)，再分别对其平移部分和旋转部分进行插值而分别得到各个路径点处的终端执行器姿态矩阵平移部分pn和各个路径点处的终端执行器姿态矩阵旋转部分qn。对平移部分的插值采用线性插值的算法，对旋转部分，使用基于四元数的对数和指数运算的插值方法计算。完成插值后，对相应于各个路径点的插值结果进行合成，得到预规划路径上各个路径点的机器人终端执行器姿态矩阵tn。

然后，针对上述方法计算出的每一个路径点，使用解析的方法对其求运动学反解。对于运动学反解中存在多个解的情况，选择与上一个路径点的运动学反解最接近的解作为结果。记录路径中每一个路径点求运动学反解的结果，也即建立所述工业机器人的关节空间的运动学轨迹。

接着，判断所述运动学轨迹是否经过球形腕的运动学奇异点(步骤s3)。此时，先针对所述运动学反解求出的每一个路径点处的运动学反解qi，比较运动学反解中每一个关节的角度qi[j]和上一个路径点的运动学反解qi-1中对应关节的角度qi-1[j]。如果所得到的关节角度之差与前述插值过程中使用的时间步距δt之比大于某一速度阈值ωmax，则说明该关节的转动速度过快，提示机器人可能在该点附近遇到运动学奇异点。此时再检测球形腕的连续三个关节中第二个关节的关节角绝对值是否小于零点阈值θthresh，若该关节角度小于该零点阈值，说明机器人遇到了由球形腕结构导致的运动学奇异点。

此时，尝试将所述预规划路径的起始点的角度进行微小偏转而得到新的规划路径(步骤s5)，然后根据所述运动学模型求出新的规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的新的运动学轨迹(步骤s6)；由此可以判断所述新的运动学轨迹是否经过球形腕的运动学奇异点(步骤s7)。图4是根据本发明的在球形腕工业机器人运动规划中避免运动学奇异点的方法重新为机器人终端执行器规划的路径。在此路径中，机器人终端执行器在空间中的位置与原路径保持相同，而角度则有微小的变化。

对于遇到球形腕运动学奇异点的路径，首先将路径起点和终点处的终端执行器姿态矩阵t1和t2分解为其平移部分p1，p2(用三维向量的形式表示)和旋转部分q1，q2(用四元数的形式表示)，之后生成两个转动角度为θ的表示旋转的四元数qbias1和qbias2，其中，qbias1的旋转轴与q1相同，qbias2的旋转轴与q2相同。θ的选取应与实际实践中对机器人终端执行器的角度、机器人的构型有关，应为一个尽量小而又能有效解决机器人运动学奇异点问题的数值。通过实验，较小的偏转角度也可以很好地缓解运动路径的奇异性。例如，abbirb1200机器人在偏转角度为10度时，可以解决99％以上的运动路径的奇异点。

分别用q1×qbias1和p1、q2×qbias2和p2作为旋转部分和平移部分重新构建预规划路径的起点机器人终端执行器姿态矩阵t1'和终点机器人终端执行器姿态矩阵t2'，并如上述步骤s1至步骤s3所述地生成重新构建的路径以及所述检查运动轨迹是否通过奇异点。当判定新的运动学轨迹仍经过球形腕的运动学奇异点时，放弃执行上述述预规划路径(步骤s8)；当判定新的运动学轨迹不经过球形腕的任何运动学奇异点时，则说明路径中遇到的奇异点问题已经被修复，因而新生成的规划路径可以控制机器人顺利执行，此时就执行所述新的规划路径(步骤s9)。

图3为图4所示的路径中机器人各关节角的变化曲线。由于新生成的规划路径避免了机器人运动学奇异点，因而该路径中机器人各个关节的变化过程相对平滑均匀。

在本发明的第二实施例中，在执行所述新的规划路径的步骤s9之后，使用新的规划路径代替预规划路径，作为后续路径处理和机器人控制的输入(步骤10)。

在以上两个实施例中，如果判定所述运动学轨迹不经过球形腕的任何运动学奇异点，则直接执行所述预规划路径(步骤s4)。

在本发明的装置实施例中，提供了一种在机器人运动规划中避免运动学奇异点的装置，该装置包括：运动学模型处理模块、奇异点判断模块、规避路径规划模块。

运动学模型处理模块被设计用于建立所述工业机器人的运动学模型，根据所述运动学模型求出预规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的运动学轨迹。该运动学模型处理模块包括：姿态矩阵分解单元，该矩阵分解单元被设计用于将所述起始点终端执行器姿态矩阵t1分解为起始点终端执行器姿态矩阵平移部分p1和起始点终端执行器姿态矩阵旋转部分q1，将所述结束点终端执行器姿态矩阵t2分解为结束点终端执行器姿态矩阵平移部分p2和结束点终端执行器姿态矩阵旋转部分q2；插值单元，该插值单元被设计用于分别对所述起始点终端执行器姿态矩阵平移部分p1和所述结束点终端执行器姿态矩阵平移部分p2进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵平移部分pn，对所述起始点终端执行器姿态矩阵旋转部分q1和所述结束点终端执行器姿态矩阵旋转部分q2进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分qn；以及姿态矩阵合成单元，该矩阵合成单元被设计用于合成所述预规划路径中的路径点的终端执行器姿态矩阵平移部分pn和所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分qn而得到所述预规划路径中的各路径点的终端执行器姿态矩阵tn。

奇异点判断模块被设计用于判断运动学轨迹是否经过球形腕的运动学奇异点，包括：关节角度比较单元，该关节角度比较单元被设计用于计算出所述预规划路径中每个路径点的运动学反解中每个关节的角度与上一个路径点的运动学反解中对应的关节的角度之差，由此得出该关节在运动到所述每个路径点时的转动速度，并进而判断该转动速度是否超过一个预定的速度阈值；以及奇异点判定单元，该奇异点判定单元被设计用于判断球形腕的沿从该工业机器人基座至终端执行器的方向看的第二个关节的关节角度的绝对值是否小于零点阈值，若该关节角度的绝对值小于零点阈值，则判定该运动路径经过球形腕运动学奇异点。

规避路径规划模块被设计用于将所述预规划路径的起始点的角度进行微小偏转而得到新的规划路径，然后根据所述运动学模型求出新的规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的新的运动学轨迹。该规避路径规划模块包括：旋转四元数生成单元，该旋转四元数生成单元被设计用于生成表示旋转的第一旋转四元数qbias1和第二旋转四元数qbias2，其中，第一旋转四元数qbias1的旋转轴与所述起始点终端执行器姿态矩阵旋转部分q1的旋转轴相同，第二旋转四元数qbias2的旋转轴与所述结束点终端执行器姿态矩阵旋转部分q2的旋转轴相同；姿态矩阵构建单元，该姿态矩阵构建单元被设计用于以所述起始点终端执行器姿态矩阵旋转部分q1和所述第一旋转四元数qbias1的乘积q1×qbias1作为新的起始点终端执行器姿态矩阵旋转部分与所述起始点终端执行器姿态矩阵平移部分p1一起重新构建新的规划路径的起始点，以所述结束点终端执行器姿态矩阵旋转部分q2和所述第二旋转四元数qbias2的乘积q2×qbias2作为新的所述结束点终端执行器姿态矩阵旋转部分与所述起始点终端执行器姿态矩阵平移部分p2一起重新构建新的规划路径的结束点。

上文中对本发明的实施方式进行了详细说明，但是本发明并不仅限于上述实施方式。对于本领域技术人员显而易见的是，上述实施形式应被理解为是示例性的而非限定性的。本领域技术人员能够在不偏离本发明的实质精神的情况下做出本发明的多种变化实施形式。各个实施形式的特征和在说明书的发明内容部分中述及的特征能够彼此分别地以及相互地组合。