CN109834706B - 在机器人运动规划中避免运动学奇异点的方法及装置 - Google Patents
在机器人运动规划中避免运动学奇异点的方法及装置 Download PDFInfo
- Publication number
- CN109834706B CN109834706B CN201711196083.6A CN201711196083A CN109834706B CN 109834706 B CN109834706 B CN 109834706B CN 201711196083 A CN201711196083 A CN 201711196083A CN 109834706 B CN109834706 B CN 109834706B
- Authority
- CN
- China
- Prior art keywords
- kinematic
- path
- point
- end effector
- planned path
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 210000000707 wrist Anatomy 0.000 claims abstract description 78
- 239000011159 matrix material Substances 0.000 claims description 143
- 239000012636 effector Substances 0.000 claims description 86
- 238000013519 translation Methods 0.000 claims description 39
- 238000012545 processing Methods 0.000 claims description 9
- 230000015572 biosynthetic process Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 238000003786 synthesis reaction Methods 0.000 claims description 3
- 230000002194 synthesizing effect Effects 0.000 claims description 3
- 238000003466 welding Methods 0.000 description 5
- 238000004458 analytical method Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
本发明涉及一种在机器人运动规划中避免运动学奇异点的方法。该方法包括：建立工业机器人的运动学模型；根据运动学模型求出预规划路径的路径点的运动学反解并根据所求出的运动学反解建立工业机器人关节空间的运动学轨迹；当判定运动学轨迹经过球形腕运动学奇异点时，则将预规划路径的起始点的角度进行微小偏转而得到新的规划路径，然后根据新的运动学反解建立工业机器人关节空间的新的运动学轨迹；当判定新的运动学轨迹仍经过球形腕的运动学奇异点时，放弃执行上述述预规划路径；当判定新的运动学轨迹不经过球形腕的任何运动学奇异点时，执行新的规划路径。本发明还涉及一种在机器人运动规划中避免运动学奇异点的装置。
Description
技术领域
本发明涉及一种在机器人运动规划中避免运动学奇异点的方法，具体而言，是一种在球形腕工业机器人运动规划中避免运动学奇异点的方法。本发明还涉及一种在机器人运动规划中避免运动学奇异点的装置，具体而言，是一种在球形腕工业机器人运动规划中避免运动学奇异点的装置。
背景技术
随着劳动力成本的不断提高以及工业机器人控制、规划、识别方法的不断发展，用工业机器人代替人工进行自动化生产的程度越来越高。但目前工业中，常用的带有球形腕的工业机器人在执行复杂环境中的未知操作时，常常会遇到运动学奇异点，严重影响了其通用性和实用性。
机器人相邻的三个旋转关节的转轴交于同一点即为球形腕。对于带有球形腕的工业机器人，当球形腕第二转轴角度接近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相同。θ的选取应与实际实践中对机器人终端执行器的角度、机器人的构型有关，应为一个尽量小而又能有效解决机器人运动学奇异点问题的数值。通过实验，较小的偏转角度也可以很好地缓解运动路径的奇异性。例如，ABB IRB1200机器人在偏转角度为10度时，可以解决99％以上的运动路径的奇异点。
分别用q1×qbias1和p1、q2×qbias2和p2作为旋转部分和平移部分重新构建预规划路径的起点机器人终端执行器姿态矩阵T1'和终点机器人终端执行器姿态矩阵T2'，并如上述步骤S1至步骤S3所述地生成重新构建的路径以及所述检查运动轨迹是否通过奇异点。当判定新的运动学轨迹仍经过球形腕的运动学奇异点时，放弃执行上述述预规划路径(步骤S8)；当判定新的运动学轨迹不经过球形腕的任何运动学奇异点时，则说明路径中遇到的奇异点问题已经被修复，因而新生成的规划路径可以控制机器人顺利执行，此时就执行所述新的规划路径(步骤S9)。
由于新生成的规划路径避免了机器人运动学奇异点，因而该路径中机器人各个关节的变化过程相对平滑均匀。
在本发明的第二实施例中，在执行所述新的规划路径的步骤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一起重新构建新的规划路径的结束点。
上文中对本发明的实施方式进行了详细说明，但是本发明并不仅限于上述实施方式。对于本领域技术人员显而易见的是，上述实施形式应被理解为是示例性的而非限定性的。本领域技术人员能够在不偏离本发明的实质精神的情况下做出本发明的多种变化实施形式。各个实施形式的特征和在说明书的发明内容部分中述及的特征能够彼此分别地以及相互地组合。
Claims (12)
1.一种在机器人运动规划中避免运动学奇异点的方法，所述机器人是球形腕工业机器人，每个所述球形腕由该工业机器人的转轴轴线相交于一点的三个相邻旋转关节构成，其特征在于，该方法包括：
建立所述工业机器人的运动学模型(S1)；
根据所述运动学模型求出预规划路径的路径点的运动学反解并根据所求出的运动学反解建立所述工业机器人的关节空间的运动学轨迹(S2)；
判断所述运动学轨迹是否经过球形腕的运动学奇异点(S3)；
当判定所述运动学轨迹不经过球形腕的任何运动学奇异点时，执行所述预规划路径(S4)；
当判定所述运动学轨迹经过所述球形腕运动学奇异点时，则将所述预规划路径的起始点的角度进行微小偏转而得到新的规划路径(S5)，然后
根据所述运动学模型求出新的规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的新的运动学轨迹(S6)；
判断所述新的运动学轨迹是否经过球形腕的运动学奇异点(S7)；
当判定新的运动学轨迹仍经过球形腕的运动学奇异点时，放弃执行上述预规划路径(S8)；
当判定新的运动学轨迹不经过球形腕的任何运动学奇异点时，执行所述新的规划路径(S9)。
2.根据权利要求1所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，判断所述运动学轨迹是否经过球形腕的运动学奇异点(S3)的步骤包括：
计算出所述预规划路径中每个路径点的运动学反解中每个关节的角度与上一个路径点的运动学反解中对应的关节的角度之差，由此得出该关节在运动到所述每个路径点时的转动速度，并进而判断该转动速度是否超过一个预定的速度阈值；
若该关节的所述转动速度超过所述速度阈值，则判断球形腕的沿从该工业机器人基座至终端执行器的方向看的第二个关节的关节角度的绝对值是否小于零点阈值，若该关节角度的绝对值小于零点阈值，则判定该预规划路径经过球形腕运动学奇异点。
3.根据权利要求1所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，在建立所述工业机器人的运动学模型时，根据所述工业机器人的预规划路径中的起始点终端执行器姿态矩阵(T1)和结束点终端执行器姿态矩阵(T2)，通过插值方法计算预规划路径中的路径点。
4.根据权利要求3所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，在执行所述插值方法时，
首先将所述起始点终端执行器姿态矩阵(T1)分解为起始点终端执行器姿态矩阵平移部分(p1)和起始点终端执行器姿态矩阵旋转部分(q1)，将所述结束点终端执行器姿态矩阵(T2)分解为结束点终端执行器姿态矩阵平移部分(p2)和结束点终端执行器姿态矩阵旋转部分(q2)；
然后，分别对所述起始点终端执行器姿态矩阵平移部分(p1)和所述结束点终端执行器姿态矩阵平移部分(p2)进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵平移部分(pn)，对所述起始点终端执行器姿态矩阵旋转部分(q1)和所述结束点终端执行器姿态矩阵旋转部分(q2)进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分(qn)；
合成所述预规划路径中的路径点的终端执行器姿态矩阵平移部分(pn)和所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分(qn)而得到所述预规划路径中的各路径点的终端执行器姿态矩阵(Tn)。
5.根据权利要求4所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，
在执行所述新的规划路径(S9)的步骤之后，使用新的规划路径代替所述预规划路径(S10)。
6.根据权利要求4所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，
所述起始点终端执行器姿态矩阵平移部分(p1)和所述结束点终端执行器姿态矩阵平移部分(p2)用三维向量的形式表示，所述起始点终端执行器姿态矩阵旋转部分(q1)和所述结束点终端执行器姿态矩阵旋转部分(q2)用四元数的形式表示。
7.根据权利要求4至6中任一项所述的在机器人运动规划中避免运动学奇异点的方法，其特征在于，将所述预规划路径的起始点的角度进行微小偏转而得到新的规划路径(S5)的步骤包括：
生成表示旋转的第一旋转四元数(qbias1)和第二旋转四元数(qbias2)，其中，第一旋转四元数(qbias1)的旋转轴与所述起始点终端执行器姿态矩阵旋转部分(q1)的旋转轴相同，第二旋转四元数(qbias2)的旋转轴与所述结束点终端执行器姿态矩阵旋转部分(q2)的旋转轴相同；
以所述起始点终端执行器姿态矩阵旋转部分(q1)和所述第一旋转四元数(qbias1)的乘积(q1×qbias1)作为新的起始点终端执行器姿态矩阵旋转部分与所述起始点终端执行器姿态矩阵平移部分(p1)一起重新构建新的规划路径的起始点，以所述结束点终端执行器姿态矩阵旋转部分(q2)和所述第二旋转四元数(qbias2)的乘积(q2×qbias2)作为新的所述结束点终端执行器姿态矩阵旋转部分与所述结束点终端执行器姿态矩阵平移部分(p2)一起重新构建新的规划路径的结束点。
8.一种在机器人运动规划中避免运动学奇异点的装置，所述机器人是球形腕工业机器人，每个所述球形腕由该工业机器人的转轴轴线相交于一点的三个相邻旋转关节构成，其特征在于，该装置包括：
运动学模型处理模块，该运动学模型处理模块被设计用于建立所述工业机器人的运动学模型，根据所述运动学模型求出预规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的运动学轨迹；
奇异点判断模块，该奇异点判断模块被设计用于判断运动学轨迹是否经过球形腕的运动学奇异点；
规避路径规划模块，该规避路径规划模块被设计用于将所述预规划路径的起始点的角度进行微小偏转而得到新的规划路径，然后根据所述运动学模型求出新的规划路径的路径点的运动学反解，并根据所求出的运动学反解建立所述工业机器人的关节空间的新的运动学轨迹。
9.根据权利要求8所述的在机器人运动规划中避免运动学奇异点的装置，其特征在于，所述奇异点判断模块包括：
关节角度比较单元，该关节角度比较单元被设计用于计算出所述预规划路径中每个路径点的运动学反解中每个关节的角度与上一个路径点的运动学反解中对应的关节的角度之差，由此得出该关节在运动到所述每个路径点时的转动速度，并进而判断该转动速度是否超过一个预定的速度阈值；以及
奇异点判定单元，该奇异点判定单元被设计用于判断球形腕的沿从该工业机器人基座至终端执行器的方向看的第二个关节的关节角度的绝对值是否小于零点阈值，若该关节角度的绝对值小于零点阈值，则判定该预规划路径经过球形腕运动学奇异点。
10.根据权利要求8所述的在机器人运动规划中避免运动学奇异点的装置，其特征在于，在建立所述工业机器人的运动学模型时，该运动学模型处理模块根据所述工业机器人的预规划路径中的起始点终端执行器姿态矩阵(T1)和结束点终端执行器姿态矩阵(T2)，通过插值方法计算预规划路径中的路径点。
11.根据权利要求10所述的在机器人运动规划中避免运动学奇异点的装置，其特征在于，所述运动学模型处理模块包括：
姿态矩阵分解单元，该姿态矩阵分解单元被设计用于将所述起始点终端执行器姿态矩阵(T1)分解为起始点终端执行器姿态矩阵平移部分(p1)和起始点终端执行器姿态矩阵旋转部分(q1)，将所述结束点终端执行器姿态矩阵(T2)分解为结束点终端执行器姿态矩阵平移部分(p2)和结束点终端执行器姿态矩阵旋转部分(q2)；
插值单元，该插值单元被设计用于分别对所述起始点终端执行器姿态矩阵平移部分(p1)和所述结束点终端执行器姿态矩阵平移部分(p2)进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵平移部分(pn)，对所述起始点终端执行器姿态矩阵旋转部分(q1)和所述结束点终端执行器姿态矩阵旋转部分(q2)进行线性插值而得到所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分(qn)；
姿态矩阵合成单元，该姿态矩阵合成单元被设计用于合成所述预规划路径中的路径点的终端执行器姿态矩阵平移部分(pn)和所述预规划路径中的路径点的终端执行器姿态矩阵旋转部分(qn)而得到所述预规划路径中的各路径点的终端执行器姿态矩阵(Tn)。
12.根据权利要求11所述的在机器人运动规划中避免运动学奇异点的装置，其特征在于，所述规避路径规划模块包括：
旋转四元数生成单元，该旋转四元数生成单元被设计用于生成表示旋转的第一旋转四元数(qbias1)和第二旋转四元数(qbias2)，其中，第一旋转四元数(qbias1)的旋转轴与所述起始点终端执行器姿态矩阵旋转部分(q1)的旋转轴相同，第二旋转四元数(qbias2)的旋转轴与所述结束点终端执行器姿态矩阵旋转部分(q2)的旋转轴相同；
姿态矩阵构建单元，该姿态矩阵构建单元被设计用于以所述起始点终端执行器姿态矩阵旋转部分(q1)和所述第一旋转四元数(qbias1)的乘积(q1×qbias1)作为新的起始点终端执行器姿态矩阵旋转部分与所述起始点终端执行器姿态矩阵平移部分(p1)一起重新构建新的规划路径的起始点，以所述结束点终端执行器姿态矩阵旋转部分(q2)和所述第二旋转四元数(qbias2)的乘积(q2×qbias2)作为新的所述结束点终端执行器姿态矩阵旋转部分与所述结束点终端执行器姿态矩阵平移部分(p2)一起重新构建新的规划路径的结束点。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711196083.6A CN109834706B (zh) | 2017-11-25 | 2017-11-25 | 在机器人运动规划中避免运动学奇异点的方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711196083.6A CN109834706B (zh) | 2017-11-25 | 2017-11-25 | 在机器人运动规划中避免运动学奇异点的方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109834706A CN109834706A (zh) | 2019-06-04 |
| CN109834706B true CN109834706B (zh) | 2020-12-15 |
Family
ID=66878577
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711196083.6A Active CN109834706B (zh) | 2017-11-25 | 2017-11-25 | 在机器人运动规划中避免运动学奇异点的方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109834706B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110561426B (zh) * | 2019-08-21 | 2021-01-12 | 哈尔滨工业大学（深圳） | 一种超冗余机械臂的路径规划方法、装置及控制系统 |
| CN118058844A (zh) * | 2020-09-25 | 2024-05-24 | 武汉联影智融医疗科技有限公司 | 手术机器人控制方法、计算机设备及手术机器人系统 |
| CN112405525B (zh) * | 2020-10-21 | 2022-04-29 | 深圳市汇川技术股份有限公司 | 奇异位置规避方法、系统、设备及计算机可读存储介质 |
| CN112621713B (zh) * | 2020-11-30 | 2022-02-25 | 珠海格力电器股份有限公司 | 机器人示教器、控制器及其示教、控制方法、装置和介质 |
| CN114378830B (zh) * | 2022-02-18 | 2024-02-20 | 深圳市大族机器人有限公司 | 一种机器人腕关节奇异规避方法及系统 |
| CN116985144A (zh) * | 2023-09-26 | 2023-11-03 | 珞石（北京）科技有限公司 | 一种具有c2连续的机器人末端姿态规划方法 |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5159249A (en) * | 1989-05-16 | 1992-10-27 | Dalila Megherbi | Method and apparatus for controlling robot motion at and near singularities and for robot mechanical design |
| CN103802114A (zh) * | 2012-11-08 | 2014-05-21 | 沈阳新松机器人自动化股份有限公司 | 工业机器人奇异点处理方法及装置 |
| JP2015085427A (ja) * | 2013-10-30 | 2015-05-07 | 株式会社デンソーウェーブ | ６軸ロボットの各軸角度決定方法及び６軸ロボットの制御装置 |
| JP2015098076A (ja) * | 2013-11-20 | 2015-05-28 | キヤノン株式会社 | ロボットプログラム作成方法、ロボットプログラム作成装置、プログラム及び記録媒体 |
| CN103909522B (zh) * | 2014-03-19 | 2016-08-17 | 华南理工大学 | 一种六自由度工业机器人通过奇异域的方法 |
| CN105700527A (zh) * | 2016-01-26 | 2016-06-22 | 哈尔滨工业大学 | 一种平面冗余度机器人避障及避奇异的路径规划方法 |
| CN107116542B (zh) * | 2017-06-28 | 2019-11-12 | 华中科技大学 | 一种六关节工业机器人通过姿态奇点的控制方法及系统 |
-
2017
- 2017-11-25 CN CN201711196083.6A patent/CN109834706B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN109834706A (zh) | 2019-06-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109834706B (zh) | 在机器人运动规划中避免运动学奇异点的方法及装置 | |
| JP6351293B2 (ja) | ロボットシステム、および物品の製造方法 | |
| CN107943034B (zh) | 移动机器人沿给定路径的完备且最短时间轨迹规划方法 | |
| JP4513663B2 (ja) | 自動組立システムにおける組立機構の動作教示方法 | |
| CN105855672B (zh) | 基于示教机器人的空间圆弧插补焊接方法 | |
| Fang et al. | Orientation planning of robot end-effector using augmented reality | |
| CA2526459C (en) | Teaching data preparing method for articulated robot | |
| US20150314439A1 (en) | End effector controlling method | |
| Chen et al. | Synchronization of tool tip trajectory and attitude based on the surface characteristics of workpiece for 6-DOF robot manipulator | |
| CN115741695A (zh) | 一种六轴串联工业机器人末端的姿态轨迹过渡规划方法 | |
| CN113084792A (zh) | 关节奇异区域的确定方法、机器人及存储装置 | |
| CN114378830B (zh) | 一种机器人腕关节奇异规避方法及系统 | |
| JP5495915B2 (ja) | 作業マニピュレータのセンシング動作生成方法及びセンシング動作生成装置 | |
| Rayankula et al. | Fault tolerant control and reconfiguration of mobile manipulator | |
| Trinh et al. | A geometrical approach to the inverse kinematics of 6R serial robots with offset wrists | |
| CN105563482A (zh) | 用于工业机器人的末端执行器的旋转运动规划方法 | |
| CN112356032B (zh) | 一种姿态平滑过渡方法及系统 | |
| CN111225772B (zh) | 垂直多关节机器人的示教数据创建系统 | |
| Amar et al. | Trajectory reconstruction for robot programming by demonstration | |
| Larsen | Including a collaborative robot in digital twin manufacturing systems | |
| Wagner et al. | Reachability analysis for cooperative processing with industrial robots | |
| JP2021175595A (ja) | シミュレータ、ロボット教示装置、ロボットシステム、シミュレーション方法、プログラム及び記録媒体 | |
| Lin | Path generation for robot polishing system based on cutter location data | |
| CN110919651A (zh) | 一种减少机器人关节结构误差对其精度影响的方法 | |
| Wagner et al. | Cooperative processing with multi-robot systems |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |