第4章　机器人逆运动学

2020-03-01 163浏览

  • 1.第4章 机器人逆运动学 第4章 机器人学简明教程 机器人逆运动学 4.1 逆运动学问题的可解性 4.2 欧拉变换解 4.3 PUMA560逆运动学
  • 2.第4章 机器人逆运动学 4.1 逆运动学问题的可解性 1.解的存在性 逆运动学问题解是否存在完全取决于机械臂的工作空间。 所谓工作空间是指机械臂末 端执行器所能达到的空间位姿的集合。一般来说，对于给 定的机械臂，其工作空间是固定的。而对于少于6个自由度的 机械臂，它在三维空间内不能达到全部位姿。所以通用工业机 器人一般都设计成6个自由度。当期望位姿位于机械臂的工作 空间之外时，逆运动学问题无解。如图4－1所示期望平面机械 臂末端达到B点，显然该逆运动学问题是无解的。
  • 3.第4章 机器人逆运动学 图4－1 期望机械臂末端达到B点
  • 4.第4章 机器人逆运动学 2.多解问题 逆运动学求解的另一个问题是多解问题。如图4－2所示 的平面机械臂有两个解，虚线表示另外一个解。逆运动学解 的个数取决于机械臂关节的数量，同时与连杆参数和关节运 动范围有关。PUMA560工业机器人一般存在8个解。
  • 5.第4章 机器人逆运动学 图4－2 平面机械臂有两个解
  • 6.第4章 机器人逆运动学 3.逆运动学问题解法 前面强调，从运动学方程中求解关节变量θ1,θ2,…,θn是 一个非线性方程组求解问题。而非线性方程组求解方法分为 封闭（解析）解法和数值解法两大类。数值解法随着计算机 技术的发展已经成为非线性方程组求解的基本方法。然而， 对于逆运动学问题，数值解法并不适用，一是机械臂操作需 要频繁求解逆运动学问题，数值解法计算量比较大；二是数 值解法不能保证求出全部解。所以逆运动学问题一般只采用 封闭（解析）解法。
  • 7.第4章 机器人逆运动学 机器人逆运动学问题涉及一个复杂的非线性方程组求解， 而从数学角度分析一般的非线性方程组经常没有封闭（解析） 解。不过对于机械臂逆运动学问题存在合适的解决方案，因 为机械臂是人造机构，只需将其设计成存在封闭解的结构即 可解决该问题。理论上已经证明，对于6自由度机械臂，存 在封闭解的充分条件是有相邻的三个关节轴相交于一点。因 此，已经设计出来的6自由度机械臂几乎都有三个相交的关 节轴，例如PUMA560的4、5、6轴交于一点。 对于平面机械臂的逆运动学问题，可以采用3.3节介绍 的几何方法进行求解。下面首先介绍欧拉变换的求解方法， 然后以PUMA560为例介绍6自由度机械臂的逆运动学问题求 解方法。
  • 8.第4章 机器人逆运动学 4.2 欧拉变换解 式（2－43）给出了采用欧拉角表示的坐标变换，其逆 问题是给定旋转矩阵Rzyz，确定对应的欧拉角。假设给定 的旋转矩阵如下: nx ox ax  Rzyz  ny oy ay  nz oz ax 
  • 9.第4章 机器人逆运动学 根据欧拉变换方程式（2－40）可得如下9个方程:  nx  c c c  s s  n  s c c  c s  y  nz   s c ox  c c s  s c  o y   s c s  c c oz  s s  a  c s  x  a y  s s  az  c (4－1)
  • 10.第4章 机器人逆运动学 1.双变量反正切函数 在三角函数求解时，通常采用双变量反正切函数 atan2(y,x)来确定角度。atan2提供两个自变量，即纵坐标和 横坐标，见图4－3。当-π≤θ≤π时，由atan2反求角度过程中， 同时检查y和x的符号来确定其所在象限。该函数也能检验什 么时候x或y为0，并反求出正确的角度。atan2的精确程度对 其整个定义域都是一样的。高级编程语言如C和Matlab等提 供标准库函数供编程者调用。
  • 11.第4章 机器人逆运动学 图4－3 双变量反正切函数
  • 12.第4章 机器人逆运动学 2.欧拉变换解 根据式（2－39）和式（2－40）可知: Rot ( z,  )1 Rzyz  Rot ( y, ) Rot ( z, ) 即  c   s   0 s c 0 0   nx 0  n y 1   nz ox oy oz a x   c c a y    s az    s c c s c s s s  0  c  (4－2) 式中，矩阵两边对应（2，3）元素相等得
  • 13.第4章 机器人逆运动学 所以得  的两个解 =atan2(ay,ax)，= + （4-3） 对应（1，3）和（3，3）元素相等得 s axc+ays，caz 所以有 =atan2(cax+say, az) （4-4） 对应（2，1）和（2，2）元素相等得 s -nxs+nyc，c-oxs+oyc 所以有 =atan2(-nxs+nyc, -oxs+oyc) （4-5） 因此，欧拉变换解计算过程并不复杂，根据以上计算过程我们知道该 问题一般存在两个解。应当指出，当 时，根据（4-1）知 ay=ax=0， 此时（4-3）将不能确定  的值。此时对应绕 Z 轴连续做两次旋转， 不能确定每次转的角度，属于欧拉角奇异情况。
  • 14.第4章 机器人逆运动学 4.3 PUMA560逆运动学 本节将研究PUMA560的逆运动学封闭解，一般的6自由 度工业机器人逆运动学问题可以参考该方法进行求解。已知 0 变换矩阵 6T ，计算各关节变量θ1,θ2,…,θ6。各连杆坐标系变 换关系如下: T  [ 01T (1 )][ 21T ( 2 )][ 23T (3 )][ 34T ( 4 )][ 45T (5 )][ 56T ( 6 )]  [ 01T (1 )]1[ 06T ]  61T 0 6
  • 15.第4章 机器人逆运动学 与欧拉角求解类似，根据第3章PUMA560运动学式（3 －21）和式（3－23）得  c1 s1 s c  1 1  0 0   0 0 0 0 1 0 0  r11 r12 0  r21 r22 0   r31 r32  1  0 0 r13 r23 r33 0 p x   1r11  p y   1r21  1 p z   r31   1  0 1 r12 1 r22 1 r32 0 1 r13 1 r23 1 r33 0 px   1 py  1 pz   1  1 (4－6)
  • 16.第4章 机器人逆运动学 式（3－22）的最后三个数如下:  1 px  a2 c2  a3c23  d 4 s23 1  p y  d3  1 pz   a3 s23  a2 s2  d 4 c23 (4－7) 令式（4－6）两边元素（2,4）相等，得到 -pxs1+pyc1=d3 （4－8） 为了求解式（4－8），做三角恒等变换 px=cos py=sin （4－9）
  • 17.第4章 式中， 机器人逆运动学    p2  p2  x y    atan2(p y , px ) 将式（4－9）代入式（4－8）得 c s1  s c1  所以 d3   sin(  1 )  cos(  1 )   因此 d3  1  d32 2  d3 1  d32   1  atan2  ,  2      
  • 18.第4章 机器人逆运动学 最后θ1的解可以写为 1  atan2(px , p y )  atan2(d3 ,  px2  py2  d32 ) (4－10) 式（4－10）的正负号表明θ1有两种解。现在θ1已知，因此式 （4－6）的左边均为已知。令式（4－6）两边的元素（1，4） 和（3，4）对应相等，得  pxc1  p ys1  a2c2  a3c23  d 4 s23 px  a3 s23  a2 s2  d 4c23 (4－11) 将式（4－8）和式（4－11）平方后相加，经复杂的运算得 a3c3  d4 s3  k (4－12)
  • 19.第4章 式中， 机器人逆运动学 k  ( px2  py2  pz2  a22  a32  d32  d42 ) /(2a2 ) 采用与解式（4－8）相同的方法可以得到θ3的两种解。 3  atan2(a3 ,d4 )  atan2(k ,  a32  d42  k 2 ) (4－13) 在θ1和θ3均已知，根据运动学关系可以得到下面等式 即 [ 03T ( 2 )]1[ 06T ]  36T  c1c23 s1c23 s23 a2c3  r11 r12 r13 c s s s c  r r r a 1 23 1 23 23 2 s3    21 22 23  s1 c1 0 d3  r31 r32 r33   0 0 0 1   0 0 0 px   c4c5c6  s4s6 c4c5s6  s4c6 c4s5 a3  py   s5c6 s5s6 c5 d4   pz  s4c5c6  c4s6 s4c5s6  c4c6 s4s5 0     1  0 0 0 1 (4－14)
  • 20.第4章 机器人逆运动学 令式（4－14）两端的元素（1，4）和（2，4）相等，得到 两个方程 c1c23 px  s1c23 p y  s23 pz  a2 c3  a3 c1s23 px  s1s23 p y  c23 pz  a2 s3  d 4 (4－15) 联立上述两个方程可以解出s23和c23，结果为 (  a3  a2 c3 ) pz  (c1 px  s1 p y )( a2 s3  d 4 ) s23  pz2  (c1 px  s1 p y ) 2 ( a2 s3  d 4 ) pz  ( a3  a2 c3 )(c1 px  s1 p y ) (4－16) s23  pz2  (c1 px  s1 p y ) 2
  • 21.第4章 机器人逆运动学 两式中分母相等，且为正值，因此可以得θ23的值: 23  atan2{(a3  a2c3) pz  (c1 px  s1 py )(a2s3  d4 ), (a2s3  d4 ) pz  (a3  a2c3)(c1 px  s1 py )} 因θ23=θ2+θ3,故可得θ2的值: 2=23-3 （4－17） 现在式（4－14）的左边均为已知。令式（4－14）两 边的元素（1，3）和（3，3）对应相等，得  r13c1c23  r23 s1c23  r33 s23  c4 s5  r13 s1  r23c1  s4 s5 (4－18)
  • 22.第4章 机器人逆运动学 若s5≠0，可以解出  4  atan2(  r13s1  r23c1 ,  r13c1c23  r23s1c23  r33s23 ) (4－19) 当θ5=0时，和欧拉角方程求解一样，属于奇异状态，可以任 意指定θ4的值。 [ 04T ]1[ 06T ]  46T (4－20) 令式（4－20）两边的元素（1，2）和元素（3，3）相等得  r13 (c1c23c4  s1s4 )  r23 ( s1c23c4  c1s4 )  r33 s23c4   s5  r13c1s23  r23 s1s23  r33c23  c5 可以确定θ5的值 (4－21) 5  atan2(s5 , c5 ) (4－22)
  • 23.第4章 机器人逆运动学 现在θ1~θ5均已知，根据运动学关系可以得到下面等式 [ 05T ]1[ 06T ]  56T ( 6 ) (4－23) 令式（4－23）两边元素（1，1）和（3，1）相等可以得到 θ6的解  6  atan2(s6 , c6 ) s6   r11 (c1c23 s4  s1c4 )  r21 ( s1c23 s4  c1c4 )  r31s23 s4 c6  r11[(c1c23c4  s1s4 )c5  c1s23 s5 ]  r21[( s1c23c4  c1s4 )c5  s1s23 s5 ]  r31 ( s23c4 c5  c23 s5 )
  • 24.第4章 机器人逆运动学 由于式（4－10）和式（4－13）的θ1和θ3各有两个解， 另外机械臂腕关节“翻转”可以得到θ4~θ6的另一组解  4'   4  180o  ' 5  5  6'   6  180o (4－26) 因此，PUMA560的逆运动学问题共有8组解。由于实际 系统关节运动范围的限制，其中一些解需要舍去，在余下的 有效解中，通常选取与当前机械臂关节角位置最近的解。