第四章运动学方程

如图4.1所示，机 器人的末端执行器（手 爪）的姿态（方向）由 n、o、a 三个旋转矢量 描述，其坐标位置由平 移矢量 p 描述，这就构 成了式（4.2）中的变换 矩阵 T。 由于 n、o、a 三 个旋转矢量是正交矢量， 所以有 n = o×a
图4.1 末端执行器的描述
4.2 姿态描述 ( Specification of Orientation )
棱形关节： 棱形关节：关节变量为dn。关节轴的方向就是关节的运动方向。与转动关节不同，轴的运动方向被确 定了，但在空间的位置并没有确定（见图2.10）。对于棱形关节，连杆长度an没有意义，所以被 设置为0。x轴（xn ）平行或逆平行棱形关节轴的方向（zn-1）与zn 的叉积。对于棱形关节，当 dn=0时，定义为0位置（即坐标原点）。棱形关节坐标原点与下一个关节坐标原点重合，关节长 度an-1为上一个关节的轴线与zn-1的公垂线长度，xn-1轴向为公垂线向下一个关节延伸的方向。
转动关节： 转动关节：关节变量为θ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。
第四章 运动学分析
4.1 4.2 4.3 4.4 4.5 4.7 引言 姿态描述 欧拉角 摇摆、 摇摆、俯仰和偏转 圆柱坐标 球坐标 4.8 4.9 4.10 4.11 4.12 4.13 4.14 各种A 各种A矩阵的说明 根据A矩阵来确定T6 根据A矩阵来确定T6 斯坦福机械手的运动方程 肘机械手的运动方程 逆运动学 斯坦福机械手的逆运动方程 小结
0 0 0 1
（4.15）
4.6 圆柱坐标 ( Cylindrical Coordinates )
如图4.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 （4.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 A x （4.18） cosα -sinα 0 rcosα Cyl(z,α,r) = sinα cosα 0 0 0 0 0 rsinα 1 z 0 1 （4.19）
4.３欧拉角 ( Euler Angles ) ３
姿态变更常用绕x,y或z轴的一系列旋转来确定。欧拉角描述方 法是：先绕z轴旋转ø，然后绕新的y(即y/)轴旋转θ，最后绕更新 的z(z//)轴旋转 ψ （见图4.2）欧拉变换Euler(ø,θ,ψ)可以通过 连乘三个旋转矩阵来求得 Euler(ø,θ,ψ) ＝Ｒot(z,ø)Ｒot(y,θ)Ｒot(z,ψ) （4.10）
z
0
A1
A2
A3
A4
A5
4T
A6
5T 6
E
X 0
6
3T 2T 1T 0T 6 6 6
6
图4.11 机械手的坐标变换图
给出 T6 = A1 A2 A3 A4 A5 A6 （4.35） 如果机械手用变换矩阵Z与参考坐标系相联系，机械手 末端执行器用E来描述，末端执行器的位置和方向相对参 考坐标系用X来 描述，如图4.11所示有 X = Z T6 E （4.36） 由此可以得到T6的表达式 T6 = Z-1 X E-1 （4.37）
（4.25）
SFra Baidu bibliotekh(α,β,γ) =
（4.26）
4.8 各种 矩阵的确定 ( Specification of matrices A ) 各种A矩阵的确定
Zn坐标轴是沿着n+1关节的坐标轴。 Xn轴是沿着Zn和Zn-1的公法线，指向离开Zn-1轴的方向。
图4.9 连杆参数
公法线长度an称为连杆长度。 xn-1轴和xn轴之间的夹角θn，以绕zn-1轴右旋为正，为连杆夹角。 两公法线an-1和an之间的距离称连杆距离dn。 zn-1轴与zn轴之间的夹角αn，以绕xn轴右旋为正，为扭转角。
4.1 引言 ( Introduction )
本章，我们采用齐次变换来描述在各种坐标系中机械手的位置与方向。首先介绍各种 正交坐标系的齐次变换。然后介绍在非正交关节坐标系中描述机械手末端的齐次变换。 注意，对任何数目关节的各种机械手均可以这样进行。 描述一个连杆与下一个连杆之间关系的齐次变换称A矩阵。A矩阵是描述连杆坐标系 之间的相对平移和旋转的齐次变换。 连续变换的若干A矩阵的积称为T矩阵，对于一个六连杆（六自由度）机械手有 T6 = A1 A2 A3 A4 A5 A6 （4.1） 六连杆的机械手有六个自由度，其中三个自由度用来确定位置，三个自由度用来确 定方向。Ｔ６表示机械手在基坐标中的位置与方向。则变换矩阵Ｔ６有下列元素 nx ox ax px ny oy ay py T6 = nz oz az pz 0 0 0 1 （4.2）
an-1
图4.10 棱型关节的连杆参数
根据上述模式用下列旋转和位移我们可以建立相邻的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,α) 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 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α 0 0 0 （4.30）
（4.31）
d 1
（4.32）
对于棱形关节，an = 0，则式（4.32）A矩阵简化为 cosθ -sinθcosα sinθsinα 0 sinθ cosθcosα -cosθsinα 0 An = 0 sinα cosα 0 0 0
d 1
（4.33）
一旦给机械手各连杆坐标系都赋了值，各种固定的连杆参数可以确 定：对于后面是旋转关节的连杆参数为d, a和α，对于后面是棱形关节的 连杆参数为θ和α。根据这些参数，α的正弦和余弦也可以求出。这样， θ α α 对于旋转关节，A矩阵变成了关节变量θ的函数。或在棱形关节的情况下， 变成了d的函数。一旦这些值给出，对于六个Ai 变换矩阵的值就可以确 定。
机械手的末端相对于基坐标系（用T6表示）用下式
4.10 斯坦福机械手的运动方程 (Kinematic Equations for the Stanford Manipulator)
对式（4.2）中16个元素一一赋值就可确定T6。假定机械手可以到达要求的位置，而 单位旋转矢量o和a正交，即 o·o = 1 （4.3） a·a = 1 （4.4） o·a = 0 （4.5） a形成单位向量 a a （4.6） |a| 构成与o和a正交的n o a n n o×a （4.7） 在o和a形成的平面上旋转o，使得o与n和a正交 o a×n （4.8） 单位向量o是 o o （4.9） |o| 根据第三章给出的一般性的旋转矩阵Ｒot (k ,θ)，它把机械手末端的姿态规定为 绕k轴旋转θ角。
z
C
a o n z
α
r
B
y
图4.6 圆柱坐标
注意：圆柱坐标只能绕 z 轴旋转
4.7 球坐标 ( Spherical Coordinates )
如图4.7所示，用球坐标来确定位置向量的方法是： 沿着z轴平移γ，然后绕y轴旋转β，最后绕z轴旋转α。 z Sph(α,β,γ) = Rot(z,α) Rot(y,β) Trans(0,0,γ) （4.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 （4.24） x 图4.7 球坐标 a o n
图4.4 摇摆、俯仰和偏 转角
图4.5 机械手的末端执行器 的摇摆、俯仰和偏 转
RPY(ø，θ，ψ) = cosø cosθ cosø sinθsinψ – sinø cosψ cosø sinθcosψ + sinø sinψ sinø cosθ sinø sinθsinψ + cosø cosψ sinø sinθcosψ–cosø sinψ -sinθ cosθsinψ cosθcosψ 0 0 0
在一系列旋转中，旋转的次序是重要的。应注意，旋转序列如 果按相反的顺序进行，则是绕基坐标中的轴旋转：
4.4 摇摆、俯仰和偏转 ( Roll, Pitch and Yaw ) 摇摆、
摇摆、俯仰和偏转为另一种旋转。如图4.4所示，就像水中航行的一条小船一样， 绕着它前进的方向（z轴）旋转 ø 称为摇摆，绕着它的横向中轴（y轴）旋转θ 称为 俯仰，绕着它甲板的垂直向上的方向（x轴）旋转ψ 称为偏转。借助于这种旋转来 描述机械手的末端执行器如图4.5所示。规定旋转的次序为 RPY(ø,θ,ψ)＝Ｒot(z,ø)Ｒot(y,θ)Ｒot(x,ψ) 即，绕x轴旋转ψ，接着绕y轴旋转θ，最后绕z轴旋转ø ，这个变换如下 cosθ 0 –sinθ 0 0 0 sinθ 0 1 0 0 0 1 0 0 0 cosψ –sinψ 0 cosθ 0 0 sinψ cosψ 0 （4.13） 0 0 1 0 0 0 1 （4.12）
图4.9 连杆参数
表4.2 连杆参数 连杆本身 的参数 连杆之间 的参数 连杆长度 连杆扭转角 连杆之间的距离 连杆之间的夹角 an αn dn θn 连杆两个轴的公垂线距离（x方向） 连杆两个轴的夹角（x轴的扭转角） 相连两连杆公垂线距离（z方向平移距） 相连两连杆公垂线的夹角（z轴旋转角）
为了描述连杆之间的关系，我们对每个连杆赋一个坐标系。
根据A矩阵来确定T6 4.9 根据A矩阵来确定T6 ( Specification of T6 in Terms of the A matrices ) 机械手的坐标变换图如图4.11所示，机械手的末端（即连杆坐标系6）相对 于连杆坐标系n-1的描述用n-1T6表示，即： n-1T = A A （4.34） 6 n n+1 • • • A6
RPY(ø,θ,ψ) = Ｒot(z,ø)
RPY(ø,θ,ψ) =
cosø –sinø sinø cosø 0 0 1 0 0
0 0 cosθ sinθsinψ sinθcosψ 0 0 0 0 cosψ –sinψ 0 -sinθ cosθsinψ cosθcosψ 0 0 1 0 0 0
0 （4.14） 1
y α
Sph(α,β,γ) =
cosα -sinα 0 0 cosβ 0 sinβ rsinβ sinα cosα 0 0 0 1 0 0 0 0 1 0 -sinβ 0 cosβ rcosβ 0 0 0 1 0 0 0 1 cosαcosβ -sinα cosαsinβ γcosαsinβ sinαcosβ cosα sinαsinβ γsinαsinβ -sinβ 0 cosβ γcosβ 0 0 0 1