一种六自由度机械臂奇异位形的快速判断方法
【专利摘要】本发明属于机械臂奇异位形判断领域，具体涉及一种六自由度机械臂奇异位形的快速判断方法。本发明包括：从六自由度机械臂的角度编码器中读取六个关节的当前角度值；判断六个关节角是否都没超出自身的实际运动范围；判断第3个关节角；判断第2个关节角；判断第5个关节角。本发明提出的方法将平面几何法应用于机械臂奇异位形的判断中。理论上本发明提出的方法是没有误差的，这能够保证该类六自由度机械臂奇异位形的判断精度，而且该发明提出的判断过程相比于求取雅可比矩阵的奇异值要简单许多，这能够保证该类六自由度机械臂奇异位形的判断速度。
【专利说明】一种六自由度机械臂奇异位形的快速判断方法
【技术领域】
[0001] 本发明属于机械臂奇异位形判断领域，具体涉及一种六自由度机械臂奇异位形的 快速判断方法。
【背景技术】
[0002] 机械臂的运动学奇异是所有类型机械臂的固有特性，在奇异点附近，雅可比矩阵 的行列式会无限趋近于零，由于雅可比矩阵反映了机械臂末端工具速度与各个关节角速度 的关系，所以当雅可比矩阵的行列式趋近于零时，采用雅可比矩阵的逆求得的各个关节的 角速度会趋近于无穷，在实际的机械臂控制中，要尽量避免这种情况的发生，否则可能会对 机械臂关节的驱动电机造成不可逆转的损坏，因此机械臂奇异位形的分析问题一直是机械 臂领域的研究热点。
[0003] 机械臂的奇异位形判断有很多种方法，大部分方法都是通过求取机械臂雅可比矩 阵的奇异值来实现的，例如雅可比矩阵的SVD(奇异值）分解、实时估计雅可比矩阵的最小 奇异值等。上述方法理论上均可求得雅可比矩阵的奇异值，但运算量却往往较大，不利于工 业机械臂的实时规避奇异控制。
[0004] 本发明提出的方法针对一类典型的六自由度机械臂，即6个关节均为旋转关节、 且后面3个轴相交于一点的机械臂。工业机械臂大部分属于此类机械臂，因此对于这类机 械臂奇异位形的研究具有很大的意义。本发明提出的奇异位形判断过程对此类机械臂的奇 异位形判断具有借鉴意义。奇异位形判断的速度与准确度会直接影响机械臂的实时规避奇 异控制，因此对于在运动过程中要实时规避奇异的机械臂来说，奇异位形判断的速度与准 确度将直接决定机械臂规避奇异的能力。
【发明内容】
[0005] 本发明的目的是提出一种简化了机械臂奇异位形的判断过程，提高了奇异位形的 判断速度的六自由度机械臂奇异位形的快速判断方法。
[0006] 本发明的目的是这样实现的：
[0007] (1)从六自由度机械臂的角度编码器中读取六个关节的当前角度值Θ i，i = 1? 6 ；
[0008] (2)判断六个关节角Θ i是否都没超出自身的实际运动范围，如果都没有超出实际 运动范围则执行步骤（3)，如果有关节角超出实际运动范围则判定该关节为奇异点，机械臂 停止运动；
[0009] (3)判断第3个关节角Θ 3，若tan Θ 3 = 13/12，则第3个关节为奇异点，机械臂停 止运动；若tan Θ 3尹13/12，则执行步骤⑷；
[0010] (4)判断第 2 个关节角 Θ 2，若 I1Cos Θ 2+l3sin( θ 2- Θ 3)+l2cos( θ 2- Θ = 0,则 第2个关节为奇异点，机械臂停止运动；若I1Cos Θ 2+l3sin( θ 2- Θ 3)+l2c〇s(02- Θ J+d^O， 则执行步骤（5)，I1为六自由度机械臂2号坐标系原点与3号坐标系原点之间的距离，I 2为 3号坐标系原点与4号坐标系原点之间的距离，I3为4号坐标系与机械臂腕部点O之间的 距离，Cl1为1号坐标系z轴与基坐标系z轴之间的距离，Θ i表示机械臂第i个关节转过的 角度；
[0011] (5)判断第5个关节角θ5，若θ5= π/2,则第5个关节为奇异点，机械臂停止运 动；若Θ 5尹η /2,则判定关节不为奇异点，机械臂继续运动。
[0012] 本发明的有益效果在于：
[0013] 于该问题的其他发明大多数都是通过求取雅可比矩阵的奇异值来实现的，而本发 明提出的方法将平面几何法应用于机械臂奇异位形的判断中。理论上本发明提出的方法是 没有误差的，这能够保证该类六自由度机械臂奇异位形的判断精度，而且该发明提出的判 断过程相比于求取雅可比矩阵的奇异值要简单许多，这能够保证该类六自由度机械臂奇异 位形的判断速度。
【专利附图】
【附图说明】
[0014] 图1是在此类六自由度机械臂各个关节轴上所建立的坐标系的示意图；
[0015] 图2是此类六自由度机械臂的坚直平面投影图；
[0016] 图3是关于本发明中提到的一类六自由度机械臂的奇异位形的判断流程图。
【具体实施方式】
[0017] 下面将结合附图举例对本发明做更详细的描述：
[0018] 该方法提出的判断思路可以应用到同类型的六自由度机械臂上。该方法大大能够 满足工业机械臂实时规避奇异控制中对于快速性和准确性的要求。
[0019] 对于该问题的其他发明大多数都是通过求取雅可比矩阵的奇异值来实现的，而本 发明提出的方法将平面几何法应用于机械臂奇异位形的判断中。理论上本发明提出的方法 是没有误差的，这能够保证该类六自由度机械臂奇异位形的判断精度，而且该发明提出的 判断过程相比于求取雅可比矩阵的奇异值要简单许多，这能够保证该类六自由度机械臂奇 异位形的判断速度。有部分发明提出了利用将导致机械臂奇异的成分分离出来的方法来进 行机械臂奇异位形的判断，由此得到的结论与本发明是一致的，但与本发明中提出的判断 过程并不相同，本发明中提出的判断过程比其他同类发明中提出的判断过程更加简便，而 且适用范围更加广阔。
[0020] 本发明的目的是通过以下方案实现的：
[0021] 将机械臂雅可比矩阵行列式分为两部分：通过对两部分组成元素的分析，将机械 臂进行坚直平面投影，利用平面几何法确定这两部分分别为0时各个关节角所满足的关 系。
[0022] 本发明最后根据分析所得到的各个关节的角度关系总结了此类六自由度机械臂 奇异位形的判断过程，首先从六自由度机械臂各个关节的角度编码器中读出当前角度值， 根据机械臂的几何结构尺寸（IpUUd 1)得到机械臂发生奇异所满足的角度关系，之后根 据每个关节的实际运动范围判断各个关节角的当前值是否超出了运动范围，接下来依次判 断机械臂第三个关节角θ 3、第二个关节角θ2、第五个关节角O5是否满足所得到的角度关 系，根据结果判断机械臂当前位姿是否为奇异点并选择是否继续运动，整个判断过程最终 以流程图的形式展示出来。
[0023] 图1中的标号i代表第i个坐标系，根据图1可知六自由度机械臂上各个关节轴 的坐标系U}位置与机械臂腕部点0的位置，本算法首先将六自由度机械臂的雅可比矩阵 行列式IjI分为两部分，即IjI = Ij11J22I = Ij11I IJ22I,其中Ij11I由机械臂的前三个关节 角所决定，ζ?|;而Ij22I由机械臂的后三个关节角所决定， Ij22I =z4z5z6,7^表示机械臂腕部点〇相对于坐标系U}的位置矢量在基坐标系中的表示， 即^Zi为系{i}中z轴单位矢量在基坐标系下的表示。
[0024] 对I J111和I J221的组成元素分别进行分析，并将机械臂进行坚直平面投影，如图2 所示。坚直平面即指机械臂第二个关节与第三个关节的轴线所垂直的平面，首先对Ij 11I的 组成元素进行分析，Ij11I中第一个元素的方向为垂直于坚直平面，而Ij11I中第二 个元素 Z2X2W和第三个元素 Z3X3W的方向均在坚直平面内。由此可知，Ij11I为〇时对应 着两种情况：第一种情况是IJ11I中第一个元素为〇,当且仅当Zl与1M共线时，Ij 11I中第 一个元素才会为〇。由图2可以得到当Z1与1；^共线时前三个关节所满足的几何关系，该几 何关系为：
[0025] I1Cos Θ 2+l3sin ( θ Θ 3) +I2Cos ( θ Θ 3) +(I1 = 0?
[0026] 另一种情况是Ij11I中的第二个元素与第三个元素的方向平行，此时Ij11I也为〇， 由机械臂的坚直平面投影图可得，Z2与Z3的方向均垂直于坚直平面，因此当且仅当与 平行时，J111中第二个元素与第三个元素才会平行，由2户〗与平行可得到机械臂前三 个关节所满足的几何关系，该几何关系为：
[0027] tan Θ 3 = 13/12 或 tan(_ Θ 3-90。）= 12/13
[0028] 以上表达式中I1代表2号坐标系原点与3号坐标系原点之间的距离，I2代表3号 坐标系原点与4号坐标系原点之间的距离，I 3代表4号坐标系与机械臂腕部点0之间的距 离，Cl1代表1号坐标系z轴与基坐标系z轴之间的距离，Θ i表示机械臂第i个关节转过的 角度。通过以上分析可以确定由Ij11I =0造成机械臂奇异时前三个关节角所满足的关系， 即 I1Cos Θ 2+l3sin ( θ 2- Θ 3) +I2Cos ( θ 2- Θ 3) +(I1 = 0 或 tan Θ 3 = 13/12 或 tan (- Θ 3-90。） =12/13。
[0029] 接下来对I J221的组成元素进行分析，由图1可知，机械臂无论处于何种位姿状态 下，IJ221中第一个元素 Z4与第二个元素 Z5始终保持互相垂直，而IJ221中第二个元素 Z5与 第三个元素 Z6也始终是互相垂直的。因此当且仅当Ij22I中第一个元素 Z4与第三个元素 Z6平行时，IJ221才会为〇,由图2可得（假设机械臂第四个关节角04为〇,此处假设并不 影响结果），当机械臂的第五个关节角9 5为±90°时，Ij22I中的24与26平行，Ij 22I为〇。 通过以上分析可得到由Ij22I =〇造成机械臂奇异时后三个关节角所满足的关系，即^ = ±90°。
[0030] 本发明根据以上分析给出了此类六自由度机械臂奇异位形的判断流程，如图3所 /Jn 〇
【权利要求】
1. 一种六自由度机械臂奇异位形的快速判断方法，其特征在于： (1) 从六自由度机械臂的角度编码器中读取六个关节的当前角度值e i，i = 1?6 ; (2) 判断六个关节角0 i是否都没超出自身的实际运动范围，如果都没有超出实际运动 范围则执行步骤（3)，如果有关节角超出实际运动范围则判定该关节为奇异点，机械臂停止 运动； (3) 判断第3个关节角0 3，若tan 0 3 = 13/12，则第3个关节为奇异点，机械臂停止运 动；若tan 0 3尹13/12，则执行步骤⑷； (4) 判断第 2 个关节角 0 2，若 lfos 0 2+l3sin ( 0 2- 0 3) +l2cos ( 0 2_ 0 3) +屯=0,则第 2 个关节为奇异点，机械臂停止运动；若lpos 0 2+l3sin( 0 2- 0 3)+l2cos( 0 2_ 0 3)+(^关0,则 执行步骤（5)，为六自由度机械臂2号坐标系原点与3号坐标系原点之间的距离，12为3 号坐标系原点与4号坐标系原点之间的距离，13为4号坐标系与机械臂腕部点0之间的距 离，屯为1号坐标系z轴与基坐标系z轴之间的距离，0 ,表示机械臂第i个关节转过的角 度； (5) 判断第5个关节角05，若05= /2,则第5个关节为奇异点，机械臂停止运动； 若e5# ji/2,则判定关节不为奇异点，机械臂继续运动。
【文档编号】B25J13/08GK104385283SQ201410474913
【公开日】2015年3月4日 申请日期:2014年9月17日 优先权日:2014年7月3日
【发明者】朱齐丹, 王欣璐, 蔡成涛, 张智, 夏桂华, 林圣琳, 原新, 王立辉, 刘志林 申请人:哈尔滨工程大学