机器人原理及控制技术第0304章 运动学方程与逆运动方程

Sph(α,β,γ) = Rot(z,α)Rot(y,β)Trans(0,0,γ) Rot(y,-β) Rot(z,-α) （3.27）
Sph(α,β,γ) =
1 0 0 γcosαsinβ

0 1 0 γsinαsinβ
0 0 1 γcosβ
000
1
（3.28）
3.7 T6的确定 ( Specification of T6 )
0 0 01 0 0 0 1
（3.25）
Sph(α,β,γ) =
cosαcosβ -sinα cosαsinβ γcosαsinβ
sinαcosβ cosα sinαsinβ γsinαsinβ
-sinβ
0
cosβ
γcosβ
0
0
0
1
（3.26）
同样，如果不希望改变末端坐标的姿态，而只是改变其空间位置，我们可 以用Rot(y,-β)和Rot(z, -α)右乘式（3.26）
3.1 引言 ( Introduction )
本章，我们采用齐次变换来描述在各种坐标系中机械手的位置与方向。首先介绍各 种正交坐标系的齐次变换。然后介绍在非正交关节坐标系中描述机械手末端的齐次变换。 注意，对任何数目关节的各种机械手均可以这样进行。
描述一个连杆与下一个连杆之间关系的齐次变换称A矩阵。A矩阵是描述连杆坐标 系之间的相对平移和旋转的齐次变换。
3.7 球坐标 ( Spherical Coordinates )
如图3.7所示，用球坐标来确定位置向量的方法是： 沿着z轴平移γ，然后绕y轴旋转β，最后绕z轴旋转α。
Sph(α,β,γ) = Rot(z,α) Rot(y,β) Trans(0,0,γ) （3.23）
z a o
γ β
n
cosβ 0 sinβ 0 1 0 0 0
第三章 运动学方程
Chapter Ⅲ Kinematic Equations
3.1 引言 3.2 姿态描述 3.3 欧拉角 3.4 摇摆、俯仰和偏转 3.5 位置的确定 3.6 圆柱坐标 3.7 球坐标
3.8 T6的说明 3.9 各种A矩阵的说明 3.10 根据A矩阵来确定T6 3.11 斯坦福机械手的运动方程 3.12 肘机械手的运动方程 3.13 小结
（3.12）
即，绕x轴旋转ψ，接着绕y轴旋转θ，最后绕z轴旋转ø，这个变换如下
RPY(ø,θ,ψ) = Ｒot(z,ø)
cosθ 0 sinθ 0 1 0 0 0 0 1 0 0 0 cosψ –sinψ 0
–sinθ 0 cosθ 0 0 sinψ cosψ 0
00 01 00 0 1
（3.13）
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
0 0 01
0
0
0
1
（3.14）
图3.4 摇摆、俯仰和偏 转角
图3.5 机械手的末端执行器 的摇摆、俯仰和偏 转
如图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
xn-1
图3.9 连杆参数
表3.2 连杆参数
连杆本身 连杆长度 的参数 连杆扭转角
an 连杆两个轴的公垂线距离（x方向） αn 连杆两个轴的夹角（x轴的扭转角）
连杆之间 连杆之间的距离 的参数 连杆之间的夹角
dn 相连两连杆公垂线距离（z方向平移距） θn 相连两连杆公垂线的夹角（z轴旋转角）
为了描述连杆之间的关系，我们对每个连杆赋一个坐标系。
3.2 姿态描述 ( Specification of Orientation )
对式（3.2）中16个元素一一赋值就可确定T6。假定机械手可以到达要求 的位置，而单位旋转矢量o和a正交，即
o·o ＝ 1 a·a ＝ 1 o·a ＝ 0
（3.3） （3.4） （3.5）
a形成单位向量
a a
|a|
（3.6）
连续变换的若干A矩阵的积称为T矩阵，对于一个六连杆（六自由度）机械手有
T6 = A1 A2 A3 A4 A5 A6
（3.1）
六连杆的机械手有六个自由度，其中三个自由度用来确定位置，三个自由度用来确 定方向。Ｔ６表示机械手在基坐标中的位置与方向。则变换矩阵Ｔ６有下列元素
T6 =
nx ox ax px ny oy ay py
nz oz az pz 0 001
（3.2）
如图3.1所示，机器人的末 端执行器（手爪）的姿态（方 向）由 n、o、a 三个旋转矢量 描述，其坐标位置由平移矢量 p 描述，这就构成了式（3.2） 中的变换矩阵 T。
由于 n、o、a 三个旋转矢 量是正交矢量，所以有
n = o×a
图3.1 末端执行器的描述
0
0
0
1
（3.15）
3.5 位置的确定 ( Specification of Position )
一旦方向被确定之后，用一个相应的p向量的位移变换可得 到机器人末端执行器在基坐标中的位置：
1 0 0 px
旋转
0 1 0 py
变换
T6 = 0 0 1 pz
矩阵
0001
（3.16）
3.6 圆柱坐标 ( Cylindrical Coordinates )
ø
z’’ z’’’ ψ θ
0
y’’’
ψ
y’y’’
øθ
y
ø
x
θψ
x’
x’’’
x’’
图3.2 欧拉角
z z’
ψ
z’’
θ z’’’ ø
0
ø
y’’’
ø y’’
θ y’ ψ
y
θ
ψ
x’
x
θ x’’ ø x’’’
图3.3 基于基坐标的欧拉角
3.4 摇摆、俯仰和偏转 ( Roll, Pitch and Yaw )
0 001
如用一个绕z轴旋转-α的变换矩阵右乘式（3.19）,结果如下
（3.19）
Cyl(z,α,r) =
cosα -sinα 0 rcosα
sinα cosα 0 rsinα
0
01z
0
011
cos(-α) -sin(-α) 0 0
sin(-α) cos(-α) 0 0
0
0 00
0
0 01
cosα -sinα 0 rcosα cosα sinα 0 0
0 0 01 0 0 0 1
（3.17）
Cyl(z, α,r) =
1000 0100
001z
cosα -sinα 0 rcosα sinα cosα 0 rsinα
0
0 10
0001 0
0 01
（3.18）
z a o
C
n
z
αr
y
A
x
B
图3.6 圆柱坐标
注意：圆柱坐标只能绕 z 轴旋转
cosα -sinα 0 rcosα sinα cosα 0 rsinα Cyl(z,α,r) = 0 0 1 z
棱形关节：关节变量为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轴向为公垂线向下一个关节延伸的方向。
T6可以用旋转和平移的方法来确定。 T6 =[平移][旋转]
表3.1 各种平移与旋转的表达式
（3.29）
[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(ø,θ,ψ)
Euler(ø,θ,ψ) ＝Ｒot(z,ø)Ｒot(y,θ)Ｒot(z,ψ) （3.10）
在一系列旋转中，旋转的次序是重要的。应注意，旋转序列 如果按相反的顺序进行，则是绕基坐标中的轴旋转：绕z轴旋转ψ ， 接着绕y轴旋转θ，最后再一次绕z轴旋转ø ，结果如图3.3所示，它 与图3.2是一致的。
z z’
y
0 1 0 0 1100
α
Sph(α,β,γ) = Rot(z,α) -sinβ 0 cosβ 0 0 0 1 γ
0 0 0 1 0001
x
（3.24）
图3.7 球坐标
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β
3.11
Sph(α,β,γ)
3.26
RPY(ø,θ,ψ)
3.12