逆运动学方程精选文档.ppt
逆运动学方程逆运动学方程本讲稿第一页，共三十一页上节知识点回顾：上节知识点回顾：n空间姿态的描述：1.欧拉角：Euler(,)ot(z,)ot(y,)ot(z,)2.RPY角：RPY(,)ot(z,)ot(y,)ot(x,)n空间位置的描述：1.圆柱坐标：Cyl(z,r)=Trans(0,0,z)Rot(z,)Trans(r,0,0)2.球坐标：Sph(,)=Rot(z,)Rot(y,)Trans(0,0,)n如果机械手用变换矩阵Z与参考坐标系相联系，机械手末端执行器用E来描述，末端执行器的位置和方向相对参考坐标系用X来 描述，有 X=Z T6 E 本讲稿第二页，共三十一页第四章第四章 逆运动学方程逆运动学方程Chapter Inverse Kinematic Equations4.1引言4.2逆运动学方程的解4.3斯坦福机械手的逆运动学解4.4欧拉变换的逆运动学解4.5RPY变换的逆运动学解4.6球坐标变换的逆运动学解4.7本章小结本讲稿第三页，共三十一页4.1 4.1 引言引言 (Introduction)所谓逆运动学方程的解，就是已知机械手直角坐标空间的位姿（pose）T6，求出各节变量nordn。T6=A1A2A3A4A5A6（4.1）逆运动学方程解的步骤如下：（1）根据机械手关节坐标设置确定AnAn为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量和参数有：an连杆长度;n连杆扭转角;dn相邻两连杆的距离;n相邻两连杆的夹角。对于旋转关节n为关节变量，而对于滑动关节dn为关节变量。其余为连杆参数，由机械手的几何尺寸和组合形态决定。本讲稿第四页，共三十一页（2）根据任务确定机械手的位姿T6T6为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由任务确定，即式（3.37）给出的表达式T6=Z-1XE-1确定。它是由三个平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a（确定姿态）组成的齐次变换矩阵描述。（3）由T6和An（n1，2，6）和式（4.1）求出相应的关节变量n或dn。本讲稿第五页，共三十一页4.2 逆运动学方程的解（逆运动学方程的解（Solving inverse kinematic equations）根据式（4.1）T6=A1A2A3A4A5A6分别用An（n1，2，5）的逆左乘式（4.1）有A1-1T6=1T6(1T6=A2A3A4A5A6)（4.2）A2-1A1-1T6=2T6(2T6=A3A4A5A6)（4.3）A3-1A2-1A1-1T6=3T6(3T6=A4A5A6)（4.4）A4-1A3-1A2-1A1-1T6=4T6(4T6=A5A6)（4.5）A5-1A4-1A3-1A2-1A1-1T6=5T6(5T6=A6)（4.6）根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可求出关节变量n或dn。本讲稿第六页，共三十一页4.3 斯坦福机械手的逆运动学解斯坦福机械手的逆运动学解 （Inverse solution of Stanford manipulator）在第三章我们推导出StanfordManipulator的运动方程和各关节齐次变换式。下面应用式（4.2）（4.6）进行求解：本讲稿第七页，共三十一页这里 f11=C1 xS1 y(4.10)f12=-z(4.11)f13=-S1 xC1 y(4.12)其中 x=nxoxaxpxT，y=nyoyaypyT，z=nzozazpzT由第三章得到的斯坦福机械手运动学方程式（3.48）为C2(C4C5C6-S4S6)-S2S5C6-C2(C4C5S6+S4C6)+S2S5S6S2(C4C5C6-S4S6)+C2S5C6-S2(C4C5S6+S4C6)-C2S5S61T6=S4C5C6+C4C6-S4C5S6+C4C600C2C4S5+S2C5S2d3S2C4S5-C2C5-C2d3S4S5d2（4.13）01本讲稿第八页，共三十一页比较式（4.9）和式（4.13）矩阵中的第三行第四列元素相等得到 f13（p）=d2（4.14）或 -S1 pxC1py=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）这里号表示机械手是右肩结构（）还是左肩结构（）。本讲稿第九页，共三十一页由式（4.21）、（4.22）和（4.18）可得到第一个关节变量1的值（4.23）根据同样的方法，利用式（4.9）和式（4.13）矩阵元素相等建立的相关的方程组，可得到其它各关节变量如下：（4.24）（4.25）（4.26）（4.27）（4.28）本讲稿第十页，共三十一页注意：n在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，此时应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止。同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。n由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关，因此当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以逆运动方程的解不是唯一的，我们应该根据机械手的组合形态和各关节的运动范围，经过多次反覆计算，从中选择一组合理解。由此可见，求解机械手的逆运动方程是一个十分复杂的过程。本讲稿第十一页，共三十一页4.4 4.4 欧拉变换的逆运动学解欧拉变换的逆运动学解 （Inverse solution of Inverse solution of Euler Angles ）由第三章知欧拉变换为Euler(,)Rot(z,)Rot(y,)Rot(z,)（4.29）我们用T来表示欧拉变换的结果，即TEuler(,)（4.30）或TRot(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.44）和式（4.45）的分母为sin，这会带来如下问题：1）由于绝对值相同的正负角度的余弦相等，如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,)TRot(y,)Rot(z,)（4.46）yxyyxyxxyx图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，可根据实际系统的组合形态从中选择一个合理解。如果ay和ax都为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）至此，我们求出了欧拉变换的逆运动学解。本讲稿第二十页，共三十一页例：给定一个直角坐标例：给定一个直角坐标欧拉角型机器人的最终期望姿态，求相欧拉角型机器人的最终期望姿态，求相应的欧拉角。应的欧拉角。本讲稿第二十一页，共三十一页n解：由式（4-57）得到 由式（4-63）得到 由式（4-68）得到 本讲稿第二十二页，共三十一页4.5 RPY变换的逆运动学解变换的逆运动学解（Inverse solution of Inverse solution of RPY）第三章介绍的摇摆、俯仰和偏转(RPY)变换的表达式如下T=RPY(,)Rot(z,)Rot(y,)Rot(x,)（4.69）用Rot1(z,)左乘上式得到Rot1(z,)TRot(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的逆运动学解。本讲稿第二十五页，共三十一页例：下面给出一个例：下面给出一个RPY机器人手所吸取的最终位姿，求滚动角、机器人手所吸取的最终位姿，求滚动角、俯仰角和位移。俯仰角和位移。本讲稿第二十六页，共三十一页n解：由式（4-76）得：由式（4-80），得：由式（4-83），得：本讲稿第二十七页，共三十一页4.6 球坐标变换的逆运动学解球坐标变换的逆运动学解 （Inverse solution of 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.87）本讲稿第二十八页，共三十一页由式（4.87）可得到（4.88）或（4.89）由式（4.86）第1行和第3行元素相等有（4.90）（4.91）由此可得（4.92）本讲稿第二十九页，共三十一页为了获得平移量，我们用Rot1(y,)左乘式（4.85）Rot1(y,)Rot1(z,)T=Trans(0,0,)（4.93）上式第4列元素是（4.94）由上式第3行元素相等得到（4.95）至此，我们求出了球坐标变换的逆运动学解。本讲稿第三十页，共三十一页4.7 4.7 本章小结（本章小结（SummarySummary）l解逆运动方程是应用齐次坐标变换原理，从机器人末端执行器的直角坐标空间到关节坐标的变换（T6 n、dn），它是求解正运动方程的逆过程（n、dn T6），是机器人运动学的重要内容，是机器人控制的依据。l要注意的是正运动方程的解是唯一解，而逆运动方程的解不是唯一解，因此选择合理解是解逆运动方程的一项重要内容。本讲稿第三十一页，共三十一页
