第4章 机器人逆运动学

ching

2019/09/02 发布于 教育 分类

文字内容
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组解。由于实际 系统关节运动范围的限制,其中一些解需要舍去,在余下的 有效解中,通常选取与当前机械臂关节角位置最近的解。