机器人控制技术逆运动学方程.ppt

Click to edit Master title style,Click to edit Master text styles,Second Level,Third Level,Fourth Level,Fifth Level,*,第四章,逆运动学方程,Chapter,Inverse Kinematic Equations,4.1 引言,4.2 逆运动学方程的解,4.3 斯坦福机械手的逆运动学解,4.4,欧拉变换的逆运动学解,4.5 RPY,变换的逆运动学解,4.6,球坐标变换的逆运动学解,4.,7,本章小结,4.1,引言,(Introduction),所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿（,pose）T,6,，,求出各节变量,n,or,d,n,T,6,=A,1,A,2,A,3,A,4,A,5,A,6,（,4.1,）,逆运动学方程解的步骤如下,：,（,1,）根据机械手关节坐标设置确定,A,n,A,n,为关节坐标的齐次坐标变换，由关节变量和参数确定关节变量和参数有：,a,n,连杆长度;,n,连杆扭转角;,d,n,相邻两连杆的距离;,n,相邻两连杆的夹角对于旋转关节,n,为关节变量，而对于滑动关节,d,n,为关节变量。

其余为连杆参数，由机械手的几何尺寸和组合形态决定2）,根据任务确定机械手的位姿,T,6,T,6,为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由任务确定，即式,（3.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,4.2 逆运动学方程的解（,Solving inverse,kinematic,equations,）,根据式,（4.1）,T,6,=A,1,A,2,A,3,A,4,A,5,A,6,分别用,A,n,（n1，2，5）,的逆左乘式（4.1）有,A,1,-1,T,6,=,1,T,6,(,1,T,6,=,A,2,A,3,A,4,A,5,A,6,),（,4.2,）,A,2,-1,A,1,-1,T,6,=,2,T,6,(,2,T,6,=A,3,A,4,A,5,A,6,),（,4.3,）,A,3,-1,A,2,-1,A,1,-1,T,6,=,3,T,6,(,3,T,6,=A,4,A,5,A,6,),（,4.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,),（,4.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,),（,4.6,）,根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可求出关节变量,n,或,d,n,。

4.3 斯坦福机械手的逆运动学解,（,Inverse solution of Stanford manipulator,）,在第三章我们推导出,Stanford Manipulator,的运动方程和各关节齐次变换式,下面应用式（4.2）（4.6）进行求解：,这里,f,11,=,C,1,x,S,1,y,(,4.10,),f,12,=,-,z,(,4.11,),f,13,=,-,S,1,x,C,1,y,(,4.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,由第三章得到的斯坦福机械手运动学方程式（,3.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,（4.13）,0 1,比较式（,4.9,）和式（,4.13,）矩阵中的第三行第四列元素相等得到,f,13,（,p,）,=,d,2,（,4.14,）,或,-,S,1,p,x,C,1,p,y,=d,2,（,4.15,）,令,p,x,=,r,cos,（,4.16,）,p,y,=,r sin,（,4.17,）,其中,（,4.18,）,（,4.19,）,将式（,4.16,）和式（,4.17,）代入式（,4.15,）有,sin,con,1,con,sin,1,d,2,/r,（,0,d,2,/r,1,）（,4.20,）,由式（,4.20,）可得,sin,(,1,),d,2,/r,（,0,1,）（,4.21,）,con,(,1,),（,4.22,）,这里,号表示机械手是右肩结构（）还是左肩结构（）。

由式（,4.21,）、（,4.22,）和（,4.18,）可得到第一个关节变量,1,的值,（,4.23,）,根据同样的方法，利用式（,4.9,）和式（,4.13,）矩阵元素相等建立的相关的方程,组，可得到其它各关节变量如下：,（,4.24,）,（,4.25,）,（,4.26,）,（,4.27,）,（,4.28,）,注意：,在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，此时应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关，因此当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以逆运动方程的解不是唯一的，我们应该根据机械手的组合形态和各关节的运动范围，经过多次反覆计算，从中选择一组合理解由此可见，求解机械手的逆运动方程是一个十分复杂的过程4.4,欧拉变换的逆运动学解 （,Inverse solution of,Euler Angles,）,由第三章知欧拉变换为,Euler(,),Rot(,z,)Rot(,y,)Rot(,z,),（,4.29,）,我们用,T,来表示欧拉变换的结果，即,T,Euler(,),（,4.30,）,或,T,Rot(,z,)Rot(,y,)Rot(,z,),（,4.31,）,其中,（,4.32,）,（,4.33,）,比较式（,4.32,）和式（,4.33,）有,（,4.34,）,（,4.35,）,（,4.36,）,（,4.37,）,（,4.38,）,（,4.39,）,（,4.40,）,（,4.41,）,（,4.42,）,由式（,4.42,）可解出,角,（,4.43,）,由式（,4.40,）和式（,4.43,）可解出,角,（,4.44,）,由式（,4.36,）和式（,4.43,）可解出,角,（,4.45,）,这里需要指出的是，在我们采用式（,4.43,）,式（,4.45,）来计算,、,、,时都是采用反余弦函数，而且式（,4.43,）和式（,4.45,）的分母为,sin,，这会带来如下问题：,1,）由于绝对值相同的正负角度的余弦相等，如,cos,cos,（,-,），因此不能确定反余弦的结果是在那个象限；,2,）当,sin,接近于,0,时，由式（,4.43,）和式（,4.45,）所求出的角度,和,是不精确的；,3,）当,0,或,180,时，式（,4.43,）和式（,4.45,）无数值解。

为此，我们必须寻求更为合理的求解方法由三角函数的知识我们知道，反正切函数,tan,1,（,x/y,）所在的象限空间可由自变量的分子和分母的符号确定（如图,4.1,所示），因此如果我们得到欧拉角的正切表达式，就不难确定欧拉角所在的象限为此，我们采用本章第二节的方法，用,Rot(,z,),1,左乘式（,4.31,）有,Rot,1,(,z,)T,Rot(,y,)Rot(,z,),（,4.46,）,y,x,y,y,x,y,x,x,y,x,图,4.1,正切函数所在象限,即,（,4.47,）,将上式写成如下形式,（,4.48,）,式中,（,4.49,）,（,4.50,）,（,4.51,）,同样，上面三个式子中的,x,、,y,、,z,分别表示,n,、,o,、,a,、,p,矢量的各个分量，如,（,4.52,）,比较式（,4.48,）等号两边矩阵的第,2,行第,3,列元素可知,（,4.63,）,即,（,4.54,）,由此可得到,（,4.55,）,或,（,4.56,）,结果得到,（,4.57,）,或,（,4.58,）,上述结果相差,180,，可根据实际系统的组合形态从中选择一个合理解如果,a,y,和,a,x,都为,0,，则式（,4.57,）和式（,4.58,）无定义，这是一种退化现象，此时,值可任意设置，如,0,。

由于角,已求出，比较式（,4.48,）等号两边矩阵第,1,行第,3,列和第,3,行第,3,列元素相等有,（,4.59,）,（,4.60,）,或,（,4.61,）,（,4.62,）,由此可得,（,4.63,）,同样比较式（,4.48,）等号两边矩阵的第,2,行第,1,列和第,2,行第,2,列元素可知,（,4.64,）,（,4.65,）,或,（,4.66,）,（,4.67,）,由此可得,（,4.68,）,至此，我们求出了欧拉变换的逆运动学解4.5 RPY,变换的逆运动学解,（,Inverse solution of,RPY,）,第三章介绍的摇摆、俯仰和偏转,(RPY),变换的表达式如下,T=RPY(,),Rot(,z,)Rot(,y,)Rot(,x,),（,4.69,）,用,Rot,1,(,z,),左乘上式得到,Rot,1,(,z,)T,Rot(,y,)Rot(,x,),（,4.70,）,将上式写成式（,4.48,）的形式,（,4.71,）,式中,（,4.72,）,（,4.73,）,（,4.74,）,由式（,4.71,）等号两边矩阵的第,2,行第,1,列元素相等有,（,4.75,）,由此得到,（,4.76,）,或,（,4.77,）,角,已求出，根据式（,4.71,）等号两边矩阵的第,3,行第,1,列和第,1,行第,1,列元素相等有,（,4.78,）,（,4.79,）,由此可得,（,4.80,）,进一步比较式（,4.71,）等号两边矩阵元素，由第,2,行第,3,列和第,2,行第,2,列元素相等有,（,4.81,）,（,4.82,）,由此可得,（,4.83,）,至此，我们求出了,RPY,的逆运动学解。

4.6,球坐标变换的逆运动学解,（,Inverse solution of,Spherical Coordinates,）,第三章介绍的球坐标变换的表达式如下,T=,Sph,(,)=Rot(,z,)Rot(,y,)Trans(0,0,),（,4.84,）,用,Rot,1,(,z,),左乘上式得到,Rot,1,(,z,)T=Rot(,y,)Trans(0,0,),（,4.85,）,将上列矩阵方程的第,4,列元素写出有,（,4.86,）,由上式第,2,行元素相等有,（,4.87,）,由式（,4.87,）可得到,（,4.88,）,或,（,4.89,）,由式（,4.86,）第,1,行和第,3,行元素相等有,（,4.90,）,（,4.91,）,由此可得,（,4.92,）,为了获得平移量,，我们用,Rot,1,(,y,),左乘式（,4.85,）,Rot,1,(,y,)Rot,1,(,z,)T=Trans(0,0,),（,4.93,）,上式第。