第4章机器人控制技术逆运动学方程ppt课件

1、第四章第四章 逆运动学方程逆运动学方程Chapter Inverse Kinematic Equations4.1 引言4.2 逆运动学方程的解4.3 斯坦福机械手的逆运动学解4.4 欧拉变换的逆运动学解4.5 RPY变换的逆运动学解4.6 球坐标变换的逆运动学解4.7 本章小结 4.1 4.1 引言引言 (Introduction) (Introduction) 所谓逆运动学方程的解，就是知机械手直角坐标空间的位姿poseT6，求出各节变量n or dn 。 T6 = A1 A2 A3 A4 A5 A6 4.1逆运动学方程解的步骤如下：1根据机械手关节坐标设置确定An An为关节坐标的齐次坐

2、标变换，由关节变量和参数确定。关节变量和参数有：an连杆长度; n连杆改动角;dn相邻两连杆的间隔 ; n相邻两连杆的夹角。 对于旋转关节n为关节变量，而对于滑动关节dn为关节变量。其他为连杆参数，由机械手的几何尺寸和组合形状决议。2 根据义务确定机械手的位姿T6 T6为机械手末端在直角坐标系参考坐标或基坐标中的位姿，由义务确定，即式 3.37 给出的表达式T6 = Z-1 X E-1确定。它是由三个平移分量构成的平移矢量P确定空间位置和三个旋转矢量n，o，a确定姿态组成的齐次变换矩阵描画。3由T6和Ann1，2，6和式4.1求出相应的关节变量n 或 dn。 10006zzzzyyyyxxxx

3、paonpaonpaonT4.2 逆运动学方程的解逆运动学方程的解Solving inverse kinematic equations根据式4.1T6 = A1 A2 A3 A4 A5 A6分别用Ann1，2，5的逆左乘式4.1有 A1-1 T6 = 1T6 ( 1T6 = A2 A3 A4 A5 A6 ) 4.2 A2-1 A1-1 T6 = 2T6 ( 2T6 = A3 A4 A5 A6 ) 4.3 A3-1A2-1 A1-1 T6 = 3T6 ( 3T6 = A4 A5 A6 ) 4.4 A4-1 A3-1A2-1 A1-1 T6 = 4T6 ( 4T6 = A5 A6 ) 4.5 A

4、5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6 ( 5T6 = A6 ) 4.6 根据上述五个矩阵方程对应元素相等，可得到假设干个可解的代数方程，便可求出关节变量n或 dn。 4.3 斯坦福机械手的逆运动学解 Inverse solution of Stanford manipulator 在第三章我们推导出 Stanford Manipulator 的运动方程和各关节齐次变换式。下面运用式4.24.6进展求解：NoImage这里 f11 = C1 xS1 y (4.10) f12 = - z (4.11) f13 = - S1 xC1 y (4.12)其中 x = nx ox

5、 ax px T， y = ny oy ay py T， z = nz oz az pz T由第三章得到的斯坦福机械手运动学方程式3.48为 C2( C4C5C6 - S4S6 ) - S2S5C6 -C2( C4C5S6 + S4C6 )+ S2S5S6 S2( C4C5C6 - S4S6 ) + C2S5C6 -S2( C4C5 S6+ S4C6 )- C2S5S6 1T6 = S4C5C6 + C4C6 -S4C5S6 + C4C6 0 0 C2C4S5 + S2C5 S2d3 S2C4S5 - C2C5 -C2d3 S4S5 d2 4.13 0 1比较式4.9和式4.13矩阵中的第三行

6、第四列元素相等得到 f13p= d2 4.14或 - S1 pxC1 py = d2 4.15令 px = r cos 4.16 py = r sin 4.17其中 4.18 4.19将式4.16和式4.17代入式4.15有 sincon1consin1 d2/r 0 d2/r 1 4.20由式4.20可得 sin(1) d2/r 0 1 4.21 con(1) 4.22这里号表示机械手是右肩构造还是左肩构造。22yxpprxypp1tan221rd由式4.21、4.22和4.18可得到第一个关节变量1的值 4.23 根据同样的方法，利用式4.9和式4.13矩阵元素相等建立的相关的方程组，可得

7、到其它各关节变量如下： 2222111tantandrdppxyzyxppSpC1112tan4.24zyxpCpSpCSd211234.25zyxyxaSaSaCCaCaS21121114tan4.26zyxyxzyxaCaSaCSaCaSSaSaSaCCC21121142112415tan4.27yxzyxzyxyxzyxoCoSCoSoSoCCSoCoSoCSSoCoSSoSoSoCCCC114211242112511421124516tan4.28留意：在求解关节变量过程中如出现反正切函数的分子和分母太小，那么计算结果误差会很大，此时应重新选择矩阵元素建立新的方程组再进展计算，直到获得

8、称心的结果为止。同样，假设计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关，因此当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以逆运动方程的解不是独一的，我们应该根据机械手的组合形状和各关节的运动范围，经过多次反覆计算，从中选择一组合了解。由此可见，求解机械手的逆运动方程是一个非常复杂的过程。4.4 4.4 欧拉变换的逆运动学解欧拉变换的逆运动学解 Inverse solution of Euler Angles Inverse solution of Euler

9、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.321000zzzzyyyyxxxxpaonpaonpaonT4.3310000sinsincossin0sinsincoscossincossinsincoscoscossin0sincoscossinsincoscossinsincoscoscos1000010000cossin00sincos10000cossin00

10、100sin0cos1000010000cossin00sincos),(),(),(conzRotyRotzRot比较式4.32和式4.33有 4.34 4.35 4.36 4.37 4.38 4.39 4.40 4.41 4.42sinsincoscoscosxnsincoscoscossinyncossinzncossinsincoscosxocoscossincossinyosinsinzosincosxasinsinyacosza由式4.42可解出角 4.43由式4.40和式4.43可解出角 4.44由式4.36和式4.43可解出角 4.45)(cos1zasincos1xasinc

11、os1zn 这里需求指出的是，在我们采用式4.43式4.45来计算、时都是采用反余弦函数，而且式4.43和式4.45的分母为sin，这会带来如下问题： 1由于绝对值一样的正负角度的余弦相等，如coscos-，因此不能确定反余弦的结果是在那个象限； 2当sin接近于0时，由式4.43和式4.45所求出的角度和是不准确的； 3当0或180时，式4.43和式4.45无数值解。 为此，我们必需寻求更为合理的求解方法。 由三角函数的知识我们知道，反正切函数tan1x / y所在的象限空间可由自变量的分子和分母的符号确定如图4.1所示，因此假设我们得到欧拉角的正切表达式，就不难确定欧拉角所在的象限。 为此

12、，我们采用本章第二节的方法，用Rot (z, )1左乘式4.31有Rot1(z,) T Rot (y, ) Rot (z, ) 4.46yxyyxyxxyx图4.1 正切函数所在象限即4.47将上式写成如下方式4.48式中 4.49 4.50 4.51同样，上面三个式子中的x、y、z分别表示n、o、a、p矢量的各个分量，如 4.5210000cossinsincossin00cossin0sinsincoscoscos10001000010000cossin00sincoszzzzyyyyxxxxpaonpaonpaon10000cossinsincossin00cossin0sinsinco

13、scoscos1000)()()()()()()()()()()()(131313131212121211111111pfafofnfpfafofnfpfafofnfyxfsincos11yxfcossin12zf13yxaaafcossin)(12比较式4.48等号两边矩阵的第2行第3列元素可知 4.63即 4.54由此可得到 4.55或 4.56结果得到 4.57或 4.580)(12af0cossinyxaaxyaacossintanxyaacossintanxyaa1tanxyaa1tan 上述结果相差180，可根据实践系统的组合形状从中选择一个合了解。假设ay和ax都为0，那么式4.

14、57和式4.58无定义，这是一种退化景象，此时值可恣意设置，如0。 由于角已求出，比较式4.48等号两边矩阵第1行第3列和第3行第3列元素相等有 4.59 4.60 或 4.61 4.62 由此可得 4.63)(sin11af)(cos13afyxaasincossinzacoszyxaaasincostan1同样比较式4.48等号两边矩阵的第2行第1列和第2行第2列元素可知 4.64 4.65或 4.66 4.67由此可得 4.68至此，我们求出了欧拉变换的逆运动学解。)(sin12nf)(cos12ofyxnncossinsinyxoocossincosyxyxoonncossincoss

15、intan14.5 RPY变换的逆运动学解变换的逆运动学解Inverse solution of RPY第三章引见的摇摆、俯仰和偏转( RPY )变换的表达式如下T = RPY ( , ,) Rot ( z, ) Rot ( y, ) Rot ( x, ) 4.69用Rot1( z, )左乘上式得到Rot1( z, ) T Rot ( y, ) Rot ( x, ) 4.70将上式写成式4.48的方式 4.71式中 4.72 4.73 4.7410000coscossincossin0sincos00cossinsinsincos10000)()()(0)()()(0)()()(1313131

16、21212111111afofnfafofnfafofnfyxfsincos11yxfcossin12zf13由式4.71等号两边矩阵的第2行第1列元素相等有 4.75由此得到 4.76或 4.77角已求出，根据式4.71等号两边矩阵的第3行第1列和第1行第1列元素相等有 4.78 4.79由此可得 4.800cossinyxnnxynn1tan0180znsinyxnnsincoscosyxznnnsincostan1 进一步比较式4.71等号两边矩阵元素，由第2行第3列和第2行第2列元素相等有 4.81 4.82 由此可得 4.83 至此，我们求出了RPY的逆运动学解。yxaacossin

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