工业机器人运动学逆运动学课件

,Click to edit Master title style,Click to edit Master text styles,Second Level,Third Level,Fourth Level,Fifth Level,*,第三章 工业机器人的运动学,-3,主要内容,数学基础,齐次坐标变换,机器人运动学方程的建立（正运动学）,机器人逆运动学分析（逆运动学）,三、逆运动学方程,（,Inverse Kinematic Equations,）,3.1,引言,3.2,逆运动学方程的解,3.3,斯坦福机械手的逆运动学解,3.4,欧拉变换的逆运动学解,3.5 RPY,变换的逆运动学解,3.6,球坐标变换的逆运动学解,3.7,本章小结,3.1,引言,(Introduction),所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿（,pose）T,6,，,求出各节变量,n,or,d,n,。,T,6,=A,1,A,2,A,3,A,4,A,5,A,6,（,3.1,）,逆运动学方程解的步骤如下,：,（,1,）根据机械手关节坐标设置确定,A,n,A,n,为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量和参数有：,a,n,连杆长度;,n,连杆扭转角;,d,n,相邻两连杆的距离;,n,相邻两连杆的夹角。,对于旋转关节,n,为关节变量，而对于滑动关节,d,n,为关节变量。其余为连杆参数，由机械手的几何尺寸和组合形态决定。,（2）,根据任务确定机械手的位姿,T,6,T,6,为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由任务确定，即式,（2.37）,给出的表达式,T,6,=Z,-1,X E,-1,确定。它是由三个平移分量构成的平移矢量,P（,确定空间位置）和三个旋转矢量,n，o，a（,确定姿态）组成的齐次变换矩阵描述。,（3）由,T,6,和,A,n,（n1，2，6）,和式（,4.1,）求出相应的关节变量,n,或,d,n,。,3.2,逆运动学方程的解（,Solving inverse kinematic equations,）,根据式,（3.1）,T,6,=A,1,A,2,A,3,A,4,A,5,A,6,分别用,A,n,（n1，2，5）,的逆左乘式（,3.1,）有,A,1,-1,T,6,=,1,T,6,(,1,T,6,=,A,2,A,3,A,4,A,5,A,6,),（,3.2,）,A,2,-1,A,1,-1,T,6,=,2,T,6,(,2,T,6,=A,3,A,4,A,5,A,6,),（,3.3,）,A,3,-1,A,2,-1,A,1,-1,T,6,=,3,T,6,(,3,T,6,=A,4,A,5,A,6,),（,3.4,）,A,4,-1,A,3,-1,A,2,-1,A,1,-1,T,6,=,4,T,6,(,4,T,6,=A,5,A,6,),（,3.5,）,A,5,-1,A,4,-1,A,3,-1,A,2,-1,A,1,-1,T,6,=,5,T,6,(,5,T,6,=A,6,),（,3.6,）,根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可求出关节变量,n,或,d,n,。,3.3,斯坦福机械手的逆运动学解,（,Inverse solution of Stanford manipulator,）,在第三章我们推导出,Stanford Manipulator,的运动方程和各关节齐次变换式,。,下面应用式（,3.2,）（,3.6,）进行求解：,这里,f,11,=,C,1,x,S,1,y,(,3.10,),f,12,=,-,z,(,3.11,),f,13,=,-,S,1,x,C,1,y,(,3.12,),其中,x,=,n,x,o,x,a,x,p,x,T,，,y,=,n,y,o,y,a,y,p,y,T,，,z,=,n,z,o,z,a,z,p,z,T,由前节得到的斯坦福机械手运动学方程式（,2.48,）为,C,2,(,C,4,C,5,C,6,-,S,4,S,6,)-,S,2,S,5,C,6,-,C,2,(,C,4,C,5,S,6,+S,4,C,6,),+S,2,S,5,S,6,S,2,(,C,4,C,5,C,6,-,S,4,S,6,),+C,2,S,5,C,6,-,S,2,(,C,4,C,5,S,6,+S,4,C,6,)-,C,2,S,5,S,6,1,T,6,=S,4,C,5,C,6,+C,4,C,6,-,S,4,C,5,S,6,+C,4,C,6,0 0,C,2,C,4,S,5,+S,2,C,5,S,2,d,3,S,2,C,4,S,5,-,C,2,C,5,-,C,2,d,3,S,4,S,5,d,2,（3.13）,0 1,比较式（,3.9,）和式（,3.13,）矩阵中的第三行第四列元素相等得到,f,13,（,p,）,=,d,2,（,3.14,）,或,-,S,1,p,x,C,1,p,y,=d,2,（,3.15,）,令,p,x,=,r cos,（,3.16,）,p,y,=,r sin,（,3.17,）,其中,（,3.18,）,（,3.19,）,将式（,3.16,）和式（,3.17,）代入式（,3.15,）有,sin,con,1,con,sin,1,d,2,/r,（,0,d,2,/r,1,）（,3.20,）,由式（,3.20,）可得,sin,(,1,),d,2,/r,（,0,1,）（,3.21,）,con,(,1,),（,3.22,）,这里,号表示机械手是右肩结构（）还是左肩结构（）。,由式（,3.21,）、（,3.22,）和（,3.18,）可得到第一个关节变量,1,的值,（,3.23,）,根据同样的方法，利用式（,3.9,）和式（,3.13,）矩阵元素相等建立的相关的方程,组，可得到其它各关节变量如下：,（,3.24,）,（,3.25,）,（,3.26,）,（,3.27,）,（,3.28,）,注意：,在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，此时应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止。同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。,由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关，因此当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以逆运动方程的解不是唯一的，我们应该根据机械手的组合形态和各关节的运动范围，经过多次反覆计算，从中选择一组合理解。由此可见，求解机械手的逆运动方程是一个十分复杂的过程。,3.4,欧拉变换的逆运动学解 （,Inverse solution of,Euler Angles,）,由前节知欧拉变换为,Euler(,),Rot(,z,)Rot(,y,)Rot(,z,),（,3.29,）,我们用,T,来表示欧拉变换的结果，即,T,Euler(,),（,3.30,）,或,T,Rot(,z,)Rot(,y,)Rot(,z,),（,3.31,）,其中,（,3.32,）,（,3.33,）,比较式（,3.32,）和式（,3.33,）有,（,3.34,）,（,3.35,）,（,3.36,）,（,3.37,）,（,3.38,）,（,3.39,）,（,3.40,）,（,3.41,）,（,3.42,）,由式（,3.42,）可解出,角,（,3.43,）,由式（,3.40,）和式（,3.43,）可解出,角,（,3.44,）,由式（,3.36,）和式（,3.43,）可解出,角,（,3.45,）,这里需要指出的是，在我们采用式（,3.43,）,式（,3.45,）来计算,、,、,时都是采用反余弦函数，而且式（,3.43,）和式（,3.45,）的分母为,sin,，这会带来如下问题：,1,）由于绝对值相同的正负角度的余弦相等，如,cos,cos,（,-,），因此不能确定反余弦的结果是在那个象限；,2,）当,sin,接近于,0,时，由式（,3.43,）和式（,3.45,）所求出的角度,和,是不精确的；,3,）当,0,或,180,时，式（,3.43,）和式（,3.45,）无数值解。,为此，我们必须寻求更为合理的求解方法。,由三角函数的知识我们知道，反正切函数,tan,1,（,x/y,）所在的象限空间可由自变量的分子和分母的符号确定（如图,3.1,所示），因此如果我们得到欧拉角的正切表达式，就不难确定欧拉角所在的象限。,为此，我们采用前节的方法，用,Rot(,z,),1,左乘式（,3.31,）有,Rot,1,(,z,)T,Rot(,y,)Rot(,z,),（,3.46,）,y,x,y,y,x,y,x,x,y,x,图,3.1,正切函数所在象限,即,（,3.47,）,将上式写成如下形式,（,3.48,）,式中,（,3.49,）,（,3.50,）,（,3.51,）,同样，上面三个式子中的,x,、,y,、,z,分别表示,n,、,o,、,a,、,p,矢量的各个分量，如,（,3.52,）,比较式（,3.48,）等号两边矩阵的第,2,行第,3,列元素可知,（,3.63,）,即,（,3.54,）,由此可得到,（,3.55,）,或,（,3.56,）,结果得到,（,3.57,）,或,（,3.58,）,上述结果相差,180,，可根据实际系统的组合形态从中选择一个合理解。如果,a,y,和,a,x,都为,0,，则式（,3.57,）和式（,3.58,）无定义，这是一种退化现象，此时,值可任意设置，如,0,。,由于角,已求出，比较式（,3.48,）等号两边矩阵第,1,行第,3,列和第,3,行第,3,列元素相等有,（,3.59,）,（,3.60,）,或,（,3.61,）,（,3.62,）,由此可得,（,3.63,）,同样比较式（,3.48,）等号两边矩阵的第,2,行第,1,列和第,2,行第,2,列元素可知,（,3.64,）,（,3.65,）,或,（,3.66,）,（,3.67,）,由此可得,（,3.68,）,至此，我们求出了欧拉变换的逆运动学解。,3.5 RPY,变换的逆运动学解,（,Inverse solution of,RPY,）,第三章介绍的摇摆、俯仰和偏转,(RPY),变换的表达式如下,T=RPY(,),Rot(,z,)Rot(,y,)Rot(,x,),（,3.69,）,用,Rot,1,(,z,),左乘上式得到,Rot,1,(,z,)T,Rot(,y,)Rot(,x,),（,3.70,）,将上式写成式（,3.48,）的形式,（,3.71,）,式中,（,3.72,）,（,3.73,）,（,3.74,）,由式（,3.71,）等号两边矩阵的第,2,行第,1,列元素相等有,（,3.75,）,由此得到,（,3.76,）,或,（,3.77,）,角,已求出，根据式（,3.71,）等号两边矩阵的第,3,行第,1,列和第,1,行第,1,列元素相等有,（,3.78,）,（,3.79,）,由此可得,（,3.80,）,进一步比较式（,3.71,）等号两边矩阵元素，由第,2,行第,3,列和第,2,行第,2,列元素相等有,（,3.81,）,（,3.82,）,由此可得,（,3.83,）,至此，我们求出了,RPY,的逆运动学解。,3.6,球坐标变换的逆运动学解,（,Inverse solution of,Spherical Coordinates,）,前节介绍的球坐标变换的表达式如下,T=Sph(,)=Rot(,z,)Rot(,y,)Trans(0,0,),（,3.84,）,用,Rot,1,(,z,),左乘上式得到,Rot,1,(,z,)T=Rot(,y,)Trans(0,0,),（,3.85,）,将上列矩阵方程的第,4,列元素写出有,（,3.86,）,由上式第,2,行元素相等有,（,3.87,）,由式（,3.87,）可得到,（,3.88,）,或,（,3.89,）,由式（,3.86,）第,1,行和第,3,行元素相等有,（,3.90,）,（,3.91,）,由此可得,（,3.92,）,为了获得平移量,，我们用,Rot,1,(,y,),左