第3章 机器人控制技术运动学方程

null第三章 运动学方程 Chapter Ⅲ Kinematic Equations 第三章 运动学方程 Chapter Ⅲ Kinematic Equations 3.1 引言 3.8 T6的说明 3.2　 姿态描述 3.9 各种A矩阵的说明 3.3　 欧拉角 3.10 根据A矩阵来确定T6 3.4 摇摆、俯仰和偏转 3.11 斯坦福机械手的运动方程 3.5 位置的确定 3.12 肘机械手的运动方程 3.6 圆柱坐标 3.13 小结 3.7 球坐标3.1 引言 ( Introduction )3.1 引言 ( Introduction ) 本章，我们采用齐次变换来描述在各种坐标系中机械手的位置与方向。首先介绍各种正交坐标系的齐次变换。然后介绍在非正交关节坐标系中描述机械手末端的齐次变换。注意，对任何数目关节的各种机械手均可以这样进行。 描述一个连杆与下一个连杆之间关系的齐次变换称A矩阵。A矩阵是描述连杆坐标系之间的相对平移和旋转的齐次变换。 连续变换的若干A矩阵的积称为T矩阵，对于一个六连杆（六自由度）机械手有 　　　　　　　 T6 = A1 A2 A3 A4 A5 A6 （3.1） 　　 六连杆的机械手有六个自由度，其中三个自由度用来确定位置，三个自由度用来确定方向。Ｔ６表示机械手在基坐标中的位置与方向。则变换矩阵Ｔ６有下列元素 　　　　　　　　　 　 nx ox ax px 　　　　　　　　　　 ny oy ay py T6 = nz oz az pz （3.2） 0 0 0 1null 如图3.1所示，机器人的末端执行器（手爪）的姿态（方向）由 n、o、a 三个旋转矢量描述，其坐标位置由平移矢量 p 描述，这就构成了式（3.2）中的变换矩阵 T。 由于 n、o、a 三个旋转矢量是正交矢量，所以有 n = o×a3.2 姿态描述 ( Specification of Orientation )3.2 姿态描述 ( Specification of Orientation ) 对式（3.2）中16个元素一一赋值就可确定T6。假定机械手可以到达要求的位置，而单位旋转矢量o和a正交，即 o·o ＝ 1 　 　　　　　 （3.3） a·a ＝ 1　 　 　 　　 （3.4） o·a ＝ 0 （3.5） a形成单位向量 　　　 a a （3.6） | a | 构成与o和a正交的n 　　　　 n o×a 　　 （3.7） 在o和a形成的平面上旋转o，使得o与n和a正交 o a×n （3.8） 单位向量o是　　　　 o o （3.9） | o | 根据第二章给出的一般性的旋转矩阵Ｒot (k ,θ)，它把机械手末端的姿态 规定 关于下班后关闭电源的规定党章中关于入党时间的规定公务员考核规定下载规定办法文件下载宁波关于闷顶的规定 为绕k轴旋转θ角。3.３欧拉角 ( Euler Angles )3.３欧拉角 ( Euler Angles ) 姿态变更常用绕x,y或z轴的一系列旋转来确定。欧拉角描述 方法 快递客服问题件处理详细方法山木方法pdf计算方法pdf华与华方法下载八字理论方法下载 是：先绕z轴旋转ø，然后绕新的y(即y/)轴旋转θ，最后绕更新的z(z//)轴旋转ψ（见图3.2）欧拉变换Euler(ø,θ,ψ)可以通过连乘三个旋转矩阵来求得 Euler(ø,θ,ψ) ＝Ｒot(z,ø)Ｒot(y,θ)Ｒot(z,ψ) （3.10） 在一系列旋转中，旋转的次序是重要的。应注意，旋转序列如果按相反的顺序进行，则是绕基坐标中的轴旋转：绕z轴旋转ψ ，接着绕y轴旋转θ，最后再一次绕z轴旋转ø ，结果如图3.3所示，它与图3.2是一致的。null3.4 摇摆、俯仰和偏转 ( Roll, Pitch and Yaw ) 3.4 摇摆、俯仰和偏转 ( Roll, Pitch and Yaw ) 摇摆、俯仰和偏转为另一种旋转。如图3.4所示，就像水中航行的一条小船一样，绕着它前进的方向（z轴）旋转 ø 称为摇摆，绕着它的横向中轴（y轴）旋转θ 称为俯仰，绕着它甲板的垂直向上的方向（x轴）旋转ψ 称为偏转。借助于这种旋转来描述机械手的末端执行器如图3.5所示。规定旋转的次序为 RPY(ø,θ,ψ)＝Ｒot(z,ø)Ｒot(y,θ)Ｒot(x,ψ) （3.12） 即，绕x轴旋转ψ，接着绕y轴旋转θ，最后绕z轴旋转ø ，这个变换如下 cosθ 0 sinθ 0 1 0 0 0 0 1 0 0 0 cosψ –sinψ 0 RPY(ø,θ,ψ) = Ｒot(z,ø) –sinθ 0 cosθ 0 0 sinψ cosψ 0 （3.13） 0 0 0 1 0 0 0 1 cosø –sinø 0 0 cosθ sinθsinψ sinθcosψ 0 sinø cosø 0 0 0 cosψ –sinψ 0 RPY(ø,θ,ψ) = 0 0 1 0 -sinθ cosθsinψ cosθcosψ 0 （3.14） 0 0 0 1 0 0 0 1 null图3.4 摇摆、俯仰和偏 转角图3.5 机械手的末端执行器 的摇摆、俯仰和偏 转nullRPY(ø，θ，ψ) = cosø cosθ cosø sinθsinψ – sinø cosψ cosø sinθcosψ + sinø sinψ 0 sinø cosθ sinø sinθsinψ + cosø cosψ sinø sinθcosψ–cosø sinψ 0 -sinθ cosθsinψ cosθcosψ 0 0                        0          0 1                             3.5 位置的确定 ( Specification of Position ) 3.5 位置的确定 ( Specification of Position ) 一旦方向被确定之后，用一个相应的p向量的位移变换可得到机器人末端执行器在基坐标中的位置： 1 0 0 px 0 1 0 py T6 = 0 0 1 pz （3.16） 0 0 0 1 旋转 变换 矩阵3.6 圆柱坐标 ( Cylindrical Coordinates ) 3.6 圆柱坐标 ( Cylindrical Coordinates ) 如图3.6所示，在圆柱坐标中确定机械手的位置是沿x轴 平移r，接着绕z轴旋转α，最后沿着z轴平移z。 Cyl(z, α,r) = Trans(0,0,z)Rot(z, α) Trans(r,0,0) cosα -sinα 0 0 1 0 0 r sinα cosα 0 0 0 1 0 0 Cyl(z, α,r) = Trans(0,0,z) 0 0 1 0 0 0 1 0 0 0 0 1 0 0 0 1 （3.17） 1 0 0 0 cosα -sinα 0 rcosα 0 1 0 0 sinα cosα 0 rsinα Cyl(z, α,r) = 0 0 1 z 0 0 1 0 0 0 0 1 0 0 0 1 （3.18）注意：圆柱坐标只能绕 z 轴旋转null cosα -sinα 0 rcosα sinα cosα 0 rsinα Cyl(z,α,r) = 0 0 1 z （3.19） 0 0 0 1 如用一个绕z轴旋转-α的变换矩阵右乘式（3.19）,结果如下 cosα -sinα 0 rcosα cos(-α) -sin(-α) 0 0 sinα cosα 0 rsinα sin(-α) cos(-α) 0 0 Cyl(z,α,r) = 0 0 1 z 0 0 0 0 （3.20） 0 0 1 1 0 0 0 1 cosα -sinα 0 rcosα cosα sinα 0 0 sinα cosα 0 rsinα -sinα cosα 0 0 Cyl(z,α,r) = 0 0 1 z 0 0 0 0 （3.21） 0 0 0 1 0 0 0 1 1 0 0 r cosα 0 1 0 r sinα Cyl(z,α,r) = 0 0 1 z （3.22） 0 0 0 1 上式表明平移矢量未变，旋转矩阵为单位阵，此时末端坐标的姿态未变，而只是改变了它的空间位置。3.7 球坐标 ( Spherical Coordinates ) 3.7 球坐标 ( Spherical Coordinates ) 如图3.7所示，用球坐标来确定位置向量的方法是： 沿着z轴平移γ，然后绕y轴旋转β，最后绕z轴旋转α。 Sph(α,β,γ) = Rot(z,α) Rot(y,β) Trans(0,0,γ) （3.23） cosβ 0 sinβ 0 1 0 0 0 0 1 0 0 1 1 0 0 Sph(α,β,γ) = Rot(z,α) -sinβ 0 cosβ 0 0 0 1 γ 0 0 0 1 0 0 0 1 （3.24）null cosα -sinα 0 0 cosβ 0 sinβ rsinβ sinα cosα 0 0 0 1 0 0 Sph(α,β,γ) = 0 0 1 0 -sinβ 0 cosβ rcosβ （3.25） 0 0 0 1 0 0 0 1   cosαcosβ -sinα cosαsinβ γcosαsinβ sinαcosβ cosα sinαsinβ γsinαsinβ Sph(α,β,γ) = -sinβ 0 cosβ γcosβ （3.26） 0 0 0 1 同样，如果不希望改变末端坐标的姿态，而只是改变其空间位置，我们可以用Rot(y,-β)和Rot(z, -α)右乘式（3.26） Sph(α,β,γ) = Rot(z,α)Rot(y,β)Trans(0,0,γ) Rot(y,-β) Rot(z,-α) （3.27） 1 0 0 γcosαsinβ 0 1 0 γsinαsinβ Sph(α,β,γ) = 0 0 1 γcosβ （3.28） 0 0 0 1 3.7 T6的确定 ( Specification of T6 ) 3.7 T6的确定 ( Specification of T6 ) T6可以用旋转和平移的方法来确定。 T6 =[平移][旋转] （3.29） 表3.1 各种平移与旋转的表达式 [Translation] Eqn [Rotation] Eqn   px, py ,pz ox o y oz ax a y a z Rot(k,θ) 2.32 Cyl( z, α, r ) 3.22 Euler(ø,θ,ψ) 3.11 Sph(α,β,γ) 3.26 RPY(ø,θ,ψ) 3.12   我们已经研究过的各种平移与旋转的式子， 总结 初级经济法重点总结下载党员个人总结TXt高中句型全总结.doc高中句型全总结.doc理论力学知识点总结pdf 在表3.1中。如果我们使用Cyl和Sph的非旋转的形式，那么矩阵积（3.29）仅仅是一个平移变换。 3.9 各种A矩阵的确定 ( Specification of matrices A ) 3.9 各种A矩阵的确定 ( Specification of matrices A ) 现在考虑方程(3.1)右边各A矩阵的确定。串联杆型机械手是由一系列通过连杆与其活动关节连接在一起所组成 。 如图3.8所示，任何一个连杆都可以用两个量来描述：一个是公共垂线距离an，另一个是与an垂直的平面上两个轴的夹角αn，习惯上称an为连杆长度，αn称为连杆的扭转角。null 如图3.9所示，在每个关节轴上有两个连杆与之相连，即关节轴有两个公垂线与之垂直，每一个连杆一个。两个相连的连杆的相对位置用dn和θn确定， dn是沿着n关节轴两个垂线的距离， θn是在垂直这个关节轴的平面上两个被测垂线之间的夹角， dn和θn分别称作连杆之间的距离及夹角。null 为了描述连杆之间的关系，我们对每个连杆赋一个坐标系。 转动关节：关节变量为θn。连杆n的坐标原点设在关节n和关节n+1轴之间的公共垂线与关节n+1轴的交点上。在关节轴相交的情况下（无公垂线），这个原点就在两个关节轴的相交点上（an＝0）。如果两个关节轴平行（有无数条公垂线），则原点的选择要使下一个连杆的关节距离为0（dn＝0），连杆n的z轴与n+1关节轴在一条直线上。x轴与任何存在的公共垂线成一条直线，并且沿着这条垂线从n关节指向n+1关节。在相交关节的情况下，x轴的方向平行或者逆平行zn-1×zn的向量叉积，应该注意，这个条件对于沿着关节n和n+1之间垂线的x轴同样满足。当xn-1和xn平行，且有相同的指向时，则对于第n个转动关节θn＝0。 表3.2 连杆参数null棱形关节：关节变量为dn。关节轴的方向就是关节的运动方向。与转动关节不同，轴的运动方向被确定了，但在空间的位置并没有确定（见图2.10）。对于棱形关节，连杆长度an没有意义，所以被设置为0。棱形关节坐标的z轴（zn）与下一个连杆的轴在一条直线上，x轴（xn）平行或逆平行棱形关节轴的方向（zn-1）与zn的叉积。对于棱形关节，当dn=0时，定义为0位置（即坐标原点）。因此棱形关节坐标原点与上一个关节（n-1）坐标原点重合，上一个关节的z轴（zn-1）与棱形关节的轴向相同，其关节长度an-1为上一个关节的轴线与zn-1的公垂线长度，xn-1轴向为公垂线向下一个关节延伸的方向。null 根据上述模式用下列旋转和位移我们可以建立相邻的n-1和n坐标系之间的关系： 绕 zn-1 旋转一个角度θn 沿 zn-1 位移一个距离 dn 沿着被旋转的 xn-1 即 xn 位移 an 绕 xn 旋转的扭转角为αn 这四个齐次变换的积为A矩阵，即 An= Rot(z,θ) Trans(0,0,d) Trans(a,0,0) Rot(x,α) （3.30） cosθ -sinθ 0 0 1 0 0 a 1 0 0 0 sinθ cosθ 0 0 0 1 0 0 0 cosα -sinα 0 An = 0 0 1 0 0 0 1 d 0 sinα cosα 0 （3.31） 0 0 0 1 0 0 0 1 0 0 0 1   cosθ -sinθcosα sinθsinα acosθ sinθ cosθcosα -cosθsinα asinθ An = 0 sinα cosα d （3.32） 0 0 0 1null 对于棱形关节，an = 0，则式（3.32）A矩阵简化为 cosθ -sinθcosα sinθsinα 0 sinθ cosθcosα -cosθsinα 0 An = 0 sinα cosα d （3.33） 0 0 0 1 一旦给机械手各连杆坐标系都赋了值，各种固定的连杆参数可以确定：对于后面是旋转关节的连杆参数为d, a和α，对于后面是棱形关节的连杆参数为θ和α。根据这些参数，α的正弦和余弦也可以求出。这样，对于旋转关节，A矩阵变成了关节变量θ的函数。或在棱形关节的情况下，变成了d的函数。一旦这些值给出，对于六个Ai变换矩阵的值就可以确定。3.10 根据A矩阵来确定T6 ( Specification of T6 in Terms of the A matrices ) 3.10 根据A矩阵来确定T6 ( Specification of T6 in Terms of the A matrices ) 机械手的坐标变换图如图3.11所示，机械手的末端（即连杆坐标系6）相对于连 杆坐标系n-1的描述用n-1T6表示，即： n-1T6 = An An+1 • • • A6 （3.34） null 机械手的末端相对于基坐标系（用T6表示）用下式给出 T6 = A1 A2 A3 A4 A5 A6 （3.35） 如果机械手用变换矩阵Z与参考坐标系相联系，机械手末端执 行器用E来描述，末端执行器的位置和方向相对参考坐标系用X来 描述，如图3.11所示有 X = Z T6 E （3.36） 由此可以得到T6的表达式 T6 = Z-1 X E-1 （3.37）3.11 斯坦福机械手的运动方程 (Kinematic Equations for the Stanford Manipulator) 3.11 斯坦福机械手的运动方程 (Kinematic Equations for the Stanford Manipulator) 斯坦福机械手及其各关节坐 标的设置如图3.12所示。 将角θ的正弦和余弦简化 sinθi = Si cosθi = Ci sin (θi+θj) = Sij cos (θi+θj) = Cij 注：将所有关节x轴的方向设置 一致，可简化坐标变换。 null表3.2 斯坦福机械手连杆参数   Link Variable α a d cosα sinα   1 θ1 -90° 0 0 0 -1 2 θ2 90° 0 d2 0 1 3 d3 0° 0 d3 1 0 4 θ4 -90° 0 0 0 -1 5 θ5 90° 0 0 0 1 6 θ6 0° 0 0 1 0 null斯坦福机械手的A变换如下： C1 0 -S1 0 S1 0 C1 0 A1 = 0 -1 0 0 （3.38） 0 0 0 1 C2 0 S2 0 S2 0 -C2 0 A2 = 0 1 0 d2 （3.39） 0 0 0 1   1 0 0 0 0 1 0 0 A3 = 0 0 1 d3 （3.40） 0 0 0 1  null C4 0 -S4 0 S4 0 C4 0 A4 = 0 -1 0 0 （3.41） 0 0 0 1     C5 0 S5 0 S5 0 -C5 0 A5 = 0 1 0 0 （3.42） 0 0 0 1   C6 -S6 0 0 S6 C6 0 0 A6 = 0 0 1 0 （3.43） 0 0 0 1 null斯坦福机械手A变换的积如下所示，这些是从连杆6开始，然后逐步回到基坐标。   C6 -S6 0 0 S6 C6 0 0 5T6 = 0 0 1 0 （3.44） 0 0 0 1   C5C6 -C5S6 S5 0 S5C6 -S5S6 -C5 0 4T6 = S6 C6 0 0 （3.45） 0 0 0 1   C4C5C6-S4S6 -C4C5S6-S4C6 C4S5 0 S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 0 3T6 = -S5C6 S5S6 C5 0 （3.46） 0 0 0 1 null C4C5C6-S4S6 -C4C5S6-S4C6 C4S5 0 S4C5C6+C4S6 -S4C5S6+C4C6 S4S5 0 2T6 = -S5C6 S5S6 C5 d3 （3.47） 0 0 0 1   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 （3.48） 0 1 null nx ox ax px ny oy ay py T6 = nz oz az pz （3.49） 0 0 0 1 其中 nx = C1 [ C2 ( C4C5C6 - S4S6 ) - S2S5C6 ] - S1( S4C5S6 + C4S6 ) ny = S1 [ C2 ( C4C5C6 - S4S6 ) - S2S5C6 ] + C1( S4C5S6+C4S6 ) nz = -S2 ( C4C5C6 - S4S6 ) - C2S5C6 ox = C1 [ -C2 ( C4C5S6 + S4C6 ) + S2S5C6 ] - S1( -S4C5S6 + C4S6 ) oy = S1 [ -C2 ( C4C5C6 + S4C6 ) + S2S5S6 ] + C1( -S4C5S6 + C4S6 ) oz = S2 ( C4C5C6 + S4C6 ) + C2S5S6 ax = C1 ( C2C4S5 + S2C5 ) – S1S4C5 ay = S1