逆运动学方程

1、逆运动学方程第1页，共31页，2022年，5月20日，19点35分，星期三上节知识点回顾：空间姿态的描述：1.欧拉角： Euler(,) ot(z,)ot(y,)ot(z,) 2.RPY角：RPY(,)ot(z,)ot(y,)ot(x,)空间位置的描述：1.圆柱坐标： Cyl(z, ,r) = Trans(0,0,z)Rot(z, ) Trans(r,0,0)2.球坐标： Sph(,) = Rot(z,) Rot(y,) Trans(0,0,)如果机械手用变换矩阵Z与参考坐标系相联系，机械手末端执行器用E来描述，末端执行器的位置和方向相对参考坐标系用X来 描述，有 X = Z T6 E 第2页

2、，共31页，2022年，5月20日，19点35分，星期三第四章 逆运动学方程Chapter Inverse Kinematic Equations4.1 引言4.2 逆运动学方程的解4.3 斯坦福机械手的逆运动学解4.4 欧拉变换的逆运动学解4.5 RPY变换的逆运动学解4.6 球坐标变换的逆运动学解4.7 本章小结 第3页，共31页，2022年，5月20日，19点35分，星期三4.1 引言 (Introduction) 所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿（pose）T6，求出各节变量n or dn 。 T6 = A1 A2 A3 A4 A5 A6 （4.1）逆运动学方程解

3、的步骤如下：（1）根据机械手关节坐标设置确定An An为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量和参数有：an连杆长度; n连杆扭转角;dn相邻两连杆的距离; n相邻两连杆的夹角。 对于旋转关节n为关节变量，而对于滑动关节dn为关节变量。其余为连杆参数，由机械手的几何尺寸和组合形态决定。第4页，共31页，2022年，5月20日，19点35分，星期三（2） 根据任务确定机械手的位姿T6 T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由任务确定，即式（ 3.37 ）给出的表达式T6 = Z-1 X E-1确定。它是由三个平移分量构成的平移矢量P（确定空间位置）和三个旋转矢

4、量n，o，a（确定姿态）组成的齐次变换矩阵描述。（3）由T6和An（n1，2，6）和式（4.1）求出相应的关节变量n 或 dn。 第5页，共31页，2022年，5月20日，19点35分，星期三4.2 逆运动学方程的解（Solving inverse kinematic equations）根据式（4.1）T6 = A1 A2 A3 A4 A5 A6分别用An（n1，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

5、 A1-1 T6 = 3T6 ( 3T6 = A4 A5 A6 ) （4.4） A4-1 A3-1A2-1 A1-1 T6 = 4T6 ( 4T6 = A5 A6 ) （4.5） A5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6 ( 5T6 = A6 ) （4.6） 根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可求出关节变量n或 dn。第6页，共31页，2022年，5月20日，19点35分，星期三 4.3 斯坦福机械手的逆运动学解 （ Inverse solution of Stanford manipulator） 在第三章我们推导出 Stanford M

6、anipulator 的运动方程和各关节齐次变换式。下面应用式（4.2）（4.6）进行求解：第7页，共31页，2022年，5月20日，19点35分，星期三这里 f11 = C1 xS1 y (4.10) f12 = - z (4.11) f13 = - S1 xC1 y (4.12)其中 x = nx ox 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 - S4

7、S6 ) + 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第8页，共31页，2022年，5月20日，19点35分，星期三比较式（4.9）和式（4.13）矩阵中的第三行第四列元素相等得到 f13（p）= d2 （4.14）或 - S1 pxC1 py = d2 （4.15）令 px = r cos （4.16） py = r sin （4.17）其中 （4.18） （4.19）将

8、式（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）这里号表示机械手是右肩结构（）还是左肩结构（）。第9页，共31页，2022年，5月20日，19点35分，星期三由式（4.21）、（4.22）和（4.18）可得到第一个关节变量1的值 （4.23） 根据同样的方法，利用式（4.9）和式（4.13）矩阵元素相等建立的相关的方程组，可得到其它各关节变量如下： （4.24）（4.25）（4.26）（4.27）（4.28）第10

9、页，共31页，2022年，5月20日，19点35分，星期三注意：在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，此时应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止。同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关，因此当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以逆运动方程的解不是唯一的，我们应该根据机械手的组合形态和各关节的运动范围，经过多次反覆计算，从中选择一组合理解。由此可见，求解机械手的逆运动方程是

10、一个十分复杂的过程。第11页，共31页，2022年，5月20日，19点35分，星期三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）第12页，共31页，2022年，5月20日，19点35分，星期三（4.33）第13页，共31页，2022年，5月20日，19点35分，星

11、期三比较式（4.32）和式（4.33）有 （4.34） （4.35） （4.36） （4.37） （4.38） （4.39） （4.40） （4.41） （4.42）第14页，共31页，2022年，5月20日，19点35分，星期三由式（4.42）可解出角 （4.43）由式（4.40）和式（4.43）可解出角 （4.44）由式（4.36）和式（4.43）可解出角 （4.45）第15页，共31页，2022年，5月20日，19点35分，星期三 这里需要指出的是，在我们采用式（4.43）式（4.45）来计算、时都是采用反余弦函数，而且式（4.44）和式（4.45）的分母为sin，这会带来如下问题： 1

12、）由于绝对值相同的正负角度的余弦相等，如coscos（-），因此不能确定反余弦的结果是在那个象限； 2）当sin接近于0时，由式（4.43）和式（4.45）所求出的角度和是不精确的； 3）当0或180时，式（4.43）和式（4.45）无数值解。 为此，我们必须寻求更为合理的求解方法。 由三角函数的知识我们知道，反正切函数tan1（x / y）所在的象限空间可由自变量的分子和分母的符号确定（如图4.1所示），因此如果我们得到欧拉角的正切表达式，就不难确定欧拉角所在的象限。 为此，我们采用本章第二节的方法，用Rot (z, )1左乘式（4.31）有Rot1(z,) T Rot (y, ) Rot

13、(z, ) （4.46）yxyyxyxxyx图4.1 正切函数所在象限第16页，共31页，2022年，5月20日，19点35分，星期三即（4.47）将上式写成如下形式（4.48）式中 （4.49） （4.50） （4.51）同样，上面三个式子中的x、y、z分别表示n、o、a、p矢量的各个分量，如 （4.52）第17页，共31页，2022年，5月20日，19点35分，星期三比较式（4.48）等号两边矩阵的第2行第3列元素可知 （4.63）即 （4.54）由此可得到 （4.55）或 （4.56）结果得到 （4.57）或 （4.58）第18页，共31页，2022年，5月20日，19点35分，星期三

14、上述结果相差180，可根据实际系统的组合形态从中选择一个合理解。如果ay和ax都为0，则式（4.57）和式（4.58）无定义，这是一种退化现象，此时值可任意设置，如0。 由于角已求出，比较式（4.48）等号两边矩阵第1行第3列和第3行第3列元素相等有 （4.59） （4.60） 或 （4.61） （4.62） 由此可得 （4.63）第19页，共31页，2022年，5月20日，19点35分，星期三同样比较式（4.48）等号两边矩阵的第2行第1列和第2行第2列元素可知 （4.64） （4.65）或 （4.66） （4.67）由此可得 （4.68）至此，我们求出了欧拉变换的逆运动学解。第20页，共3

15、1页，2022年，5月20日，19点35分，星期三例：给定一个直角坐标欧拉角型机器人的最终期望姿态，求相应的欧拉角。第21页，共31页，2022年，5月20日，19点35分，星期三解：由式（4-57） 得到 由式（4-63） 得到 由式（4-68） 得到 第22页，共31页，2022年，5月20日，19点35分，星期三4.5 RPY变换的逆运动学解（Inverse solution of RPY）第三章介绍的摇摆、俯仰和偏转( RPY )变换的表达式如下T = RPY ( , ,) Rot ( z, ) Rot ( y, ) Rot ( x, ) （4.69）用Rot1( z, )左乘上式得到

16、Rot1( z, ) T Rot ( y, ) Rot ( x, ) （4.70）将上式写成式（4.48）的形式 （4.71）式中 （4.72） （4.73） （4.74）第23页，共31页，2022年，5月20日，19点35分，星期三由式（4.71）等号两边矩阵的第2行第1列元素相等有 （4.75）由此得到 （4.76）或 （4.77）角已求出，根据式（4.71）等号两边矩阵的第3行第1列和第1行第1列元素相等有 （4.78） （4.79）由此可得 （4.80）第24页，共31页，2022年，5月20日，19点35分，星期三 进一步比较式（4.71）等号两边矩阵元素，由第2行第3列和第2行第

17、2列元素相等有 （4.81） （4.82） 由此可得 （4.83） 至此，我们求出了RPY的逆运动学解。第25页，共31页，2022年，5月20日，19点35分，星期三例：下面给出一个RPY机器人手所吸取的最终位姿，求滚动角、俯仰角和位移。第26页，共31页，2022年，5月20日，19点35分，星期三解：由式（4-76） 得：由式（4-80） ，得：由式（4-83） ，得：第27页，共31页，2022年，5月20日，19点35分，星期三4.6 球坐标变换的逆运动学解 （Inverse solution of Spherical Coordinates ）第三章介绍的球坐标变换的表达式如下T

18、= 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.87）第28页，共31页，2022年，5月20日，19点35分，星期三由式（4.87）可得到 （4.88）或 （4.89）由式（4.86）第1行和第3行元素相等有 （4.90） （4.91）由此可得 （4.92）第29页，共31页，2022年，5月20日，19点35分，星期三为了获得平移量，我们用Rot1( y, )左乘式（4.85）Rot1( y, ) Rot1( z, ) T = Trans ( 0, 0, ) （4.9