CN104385283B - 一种六自由度机械臂奇异位形的快速判断方法 - Google Patents

Abstract

本发明属于机械臂奇异位形判断领域，具体涉及一种六自由度机械臂奇异位形的快速判断方法。本发明包括：从六自由度机械臂的角度编码器中读取六个关节的当前角度值；判断六个关节角是否都没超出自身的实际运动范围；判断第3个关节角；判断第2个关节角；判断第5个关节角。本发明提出的方法将平面几何法应用于机械臂奇异位形的判断中。理论上本发明提出的方法是没有误差的，这能够保证该类六自由度机械臂奇异位形的判断精度，而且该发明提出的判断过程相比于求取雅可比矩阵的奇异值要简单许多，这能够保证该类六自由度机械臂奇异位形的判断速度。

Description

一种六自由度机械臂奇异位形的快速判断方法

技术领域

本发明属于机械臂奇异位形判断领域，具体涉及一种六自由度机械臂奇异位形的快速判断方法。

背景技术

机械臂的运动学奇异是所有类型机械臂的固有特性，在奇异点附近，雅可比矩阵的行列式会无限趋近于零，由于雅可比矩阵反映了机械臂末端工具速度与各个关节角速度的关系，所以当雅可比矩阵的行列式趋近于零时，采用雅可比矩阵的逆求得的各个关节的角速度会趋近于无穷，在实际的机械臂控制中，要尽量避免这种情况的发生，否则可能会对机械臂关节的驱动电机造成不可逆转的损坏，因此机械臂奇异位形的分析问题一直是机械臂领域的研究热点。

机械臂的奇异位形判断有很多种方法，大部分方法都是通过求取机械臂雅可比矩阵的奇异值来实现的，例如雅可比矩阵的SVD(奇异值)分解、实时估计雅可比矩阵的最小奇异值等。上述方法理论上均可求得雅可比矩阵的奇异值，但运算量却往往较大，不利于工业机械臂的实时规避奇异控制。

本发明提出的方法针对一类典型的六自由度机械臂，即6个关节均为旋转关节、且后面3个轴相交于一点的机械臂。工业机械臂大部分属于此类机械臂，因此对于这类机械臂奇异位形的研究具有很大的意义。本发明提出的奇异位形判断过程对此类机械臂的奇异位形判断具有借鉴意义。奇异位形判断的速度与准确度会直接影响机械臂的实时规避奇异控制，因此对于在运动过程中要实时规避奇异的机械臂来说，奇异位形判断的速度与准确度将直接决定机械臂规避奇异的能力。

发明内容

本发明的目的是提出一种简化了机械臂奇异位形的判断过程，提高了奇异位形的判断速度的六自由度机械臂奇异位形的快速判断方法。

本发明的目的是这样实现的：

(1)从六自由度机械臂的角度编码器中读取六个关节的当前角度值θ_i，i＝1～6；

(2)判断六个关节角θ_i是否都没超出自身的实际运动范围，如果都没有超出实际运动范围则执行步骤(3)，如果有关节角超出实际运动范围则判定该关节为奇异点，机械臂停止运动；

(3)判断第3个关节角θ₃，若tanθ₃＝l₃/l₂，则第3个关节为奇异点，机械臂停止运动； 若tanθ₃≠l₃/l₂，则执行步骤(4)；

(4)判断第2个关节角θ₂，若l₁cosθ₂+l₃sin(θ₂-θ₃)+l₂cos(θ₂-θ₃)+d₁＝0，则第2个关节为奇异点，机械臂停止运动；若l₁cosθ₂+l₃sin(θ₂-θ₃)+l₂cos(_θ2-θ₃)+d₁≠0，则执行步骤(5)，l₁为六自由度机械臂2号坐标系原点与3号坐标系原点之间的距离，l₂为3号坐标系原点与4号坐标系原点之间的距离，l₃为4号坐标系与机械臂腕部点O之间的距离，d₁为1号坐标系z轴与基坐标系z轴之间的距离，θ_i表示机械臂第i个关节转过的角度；

(5)判断第5个关节角θ₅，若θ₅＝±π/2，则第5个关节为奇异点，机械臂停止运动；若θ₅≠±π/2，则判定关节不为奇异点，机械臂继续运动。

本发明的有益效果在于：

于该问题的其他发明大多数都是通过求取雅可比矩阵的奇异值来实现的，而本发明提出的方法将平面几何法应用于机械臂奇异位形的判断中。理论上本发明提出的方法是没有误差的，这能够保证该类六自由度机械臂奇异位形的判断精度，而且该发明提出的判断过程相比于求取雅可比矩阵的奇异值要简单许多，这能够保证该类六自由度机械臂奇异位形的判断速度。

附图说明

图1是在此类六自由度机械臂各个关节轴上所建立的坐标系的示意图；

图2是此类六自由度机械臂的竖直平面投影图；

图3是关于本发明中提到的一类六自由度机械臂的奇异位形的判断流程图。

具体实施方式

下面将结合附图举例对本发明做更详细的描述：

该方法提出的判断思路可以应用到同类型的六自由度机械臂上。该方法大大能够满足工业机械臂实时规避奇异控制中对于快速性和准确性的要求。

对于该问题的其他发明大多数都是通过求取雅可比矩阵的奇异值来实现的，而本发明提出的方法将平面几何法应用于机械臂奇异位形的判断中。理论上本发明提出的方法是没有误差的，这能够保证该类六自由度机械臂奇异位形的判断精度，而且该发明提出的判断过程相比于求取雅可比矩阵的奇异值要简单许多，这能够保证该类六自由度机械臂奇异位形的判断速度。有部分发明提出了利用将导致机械臂奇异的成分分离出来的方法来进行机械臂奇异位形的判断，由此得到的结论与本发明是一致的，但与本发明中提出的判断过程并不相同，本发明中提出的判断过程比其他同类发明中提出的判断过程更加简便，而且适用范围更加广阔。

本发明的目的是通过以下方案实现的：

将机械臂雅可比矩阵行列式分为两部分：通过对两部分组成元素的分析，将机械臂进行竖直平面投影，利用平面几何法确定这两部分分别为0时各个关节角所满足的关系。

本发明最后根据分析所得到的各个关节的角度关系总结了此类六自由度机械臂奇异位形的判断过程，首先从六自由度机械臂各个关节的角度编码器中读出当前角度值，根据机械臂的几何结构尺寸(l₁,l₂,l₃,d₁)得到机械臂发生奇异所满足的角度关系，之后根据每个关节的实际运动范围判断各个关节角的当前值是否超出了运动范围，接下来依次判断机械臂第三个关节角θ₃、第二个关节角θ₂、第五个关节角θ₅是否满足所得到的角度关系，根据结果判断机械臂当前位姿是否为奇异点并选择是否继续运动，整个判断过程最终以流程图的形式展示出来。

图1中的标号i代表第i个坐标系，根据图1可知六自由度机械臂上各个关节轴的坐标系{i}位置与机械臂腕部点O的位置，本算法首先将六自由度机械臂的雅可比矩阵行列式|J|分为两部分，即|J|＝|J₁₁J₂₂|＝|J₁₁||J₂₂|，其中|J₁₁|由机械臂的前三个关节角所决定，而|J₂₂|由机械臂的后三个关节角所决定，|J₂₂|＝z₄z₅z₆，表示机械臂腕部点O相对于坐标系{i}的位置矢量在基坐标系中的表示，z_i为系{i}中z轴单位矢量在基坐标系下的表示。

对|J₁₁|和|J₂₂|的组成元素分别进行分析，并将机械臂进行竖直平面投影，如图2所示。竖直平面即指机械臂第二个关节与第三个关节的轴线所垂直的平面，首先对|J₁₁|的组成元素进行分析，|J₁₁|中第一个元素的方向为垂直于竖直平面，而|J₁₁|中第二个元素和第三个元素的方向均在竖直平面内。由此可知，|J₁₁|为0时对应着两种情况：第一种情况是|J₁₁|中第一个元素为0，当且仅当z₁与共线时，|J₁₁|中第一个元素才会为0。由图2可以得到当z₁与共线时前三个关节所满足的几何关系，该几何关系为：

l₁cosθ₂+l₃sin(θ₂-θ₃)+l₂cos(θ₂-θ₃)+d₁＝0。

另一种情况是|J₁₁|中的第二个元素与第三个元素的方向平行，此时|J₁₁|也为0，由机械臂的竖直平面投影图可得，z₂与z₃的方向均垂直于竖直平面，因此当且仅当与平行时，J₁₁|中第二个元素与第三个元素才会平行，由与平行可得到机械臂前三个关节所满足 的几何关系，该几何关系为：

tanθ₃＝l₃/l₂或tan(-θ₃-90°)＝l₂/l₃

以上表达式中l₁代表2号坐标系原点与3号坐标系原点之间的距离，l₂代表3号坐标系原点与4号坐标系原点之间的距离，l₃代表4号坐标系与机械臂腕部点O之间的距离，d₁代表1号坐标系z轴与基坐标系z轴之间的距离，θ_i表示机械臂第i个关节转过的角度。通过以上分析可以确定由|J₁₁|＝0造成机械臂奇异时前三个关节角所满足的关系，即l₁cosθ₂+l₃sin(θ₂-θ₃)+l₂cos(θ₂-θ₃)+d₁＝0或tanθ₃＝l₃/l₂或tan(-θ₃-90°)＝l₂/l₃。

接下来对|J₂₂|的组成元素进行分析，由图1可知，机械臂无论处于何种位姿状态下，|J₂₂|中第一个元素z₄与第二个元素z₅始终保持互相垂直，而|J₂₂|中第二个元素z₅与第三个元素z₆也始终是互相垂直的。因此当且仅当|J₂₂|中第一个元素z₄与第三个元素z₆平行时，|J₂₂|才会为0，由图2可得(假设机械臂第四个关节角θ₄为0，此处假设并不影响结果)，当机械臂的第五个关节角θ₅为±90°时，|J₂₂|中的z₄与z₆平行，|J₂₂|为0。通过以上分析可得到由|J₂₂|＝0造成机械臂奇异时后三个关节角所满足的关系，即θ₅＝±90°。

本发明根据以上分析给出了此类六自由度机械臂奇异位形的判断流程，如图3所示。

Claims (1)

1.一种六自由度机械臂奇异位形的快速判断方法，其特征在于：

(1)从六自由度机械臂的角度编码器中读取六个关节的当前角度值θ_i，i＝1～6；

(2)判断六个关节角θ_i是否都没超出自身的实际运动范围，如果都没有超出实际运动范围则执行步骤(3)，如果有关节角超出实际运动范围则判定该关节为奇异点，机械臂停止运动；

(3)判断第3个关节角θ₃，若tanθ₃＝l₃/l₂，则第3个关节为奇异点，机械臂停止运动；若tanθ₃≠l₃/l₂，则执行步骤(4)；

(4)判断第2个关节角θ₂，若l₁cosθ₂+l₃sin(θ₂-θ₃)+l₂cos(θ₂-θ₃)+d₁＝0，则第2个关节为奇异点，机械臂停止运动；若l₁cosθ₂+l₃sin(θ₂-θ₃)+l₂cos(θ₂-θ₃)+d₁≠0，则执行步骤(5)，l₁为六自由度机械臂2号坐标系原点与3号坐标系原点之间的距离，l₂为3号坐标系原点与4号坐标系原点之间的距离，l₃为4号坐标系与机械臂腕部点O之间的距离，d₁为1号坐标系z轴与基坐标系z轴之间的距离，θ_i表示机械臂第i个关节转过的角度；

(5)判断第5个关节角θ₅，若θ₅＝±π/2，则第5个关节为奇异点，机械臂停止运动；若θ₅≠±π/2，则判定关节不为奇异点，机械臂继续运动。