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

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
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 )
RPY(ø，θ，ψ) =
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
构成与o和a正交的n
n o×a
（3.7）
在o和a形成的平面上旋转o，使得o与n和a正交
单位向量o是
o a×n
o o
|o|
（3.8） （3.9）
根据第二章给出的一般性的旋转矩阵Ｒot (k ,θ)，它把机械手末端的姿态 规定为绕k轴旋转θ角。
3.３欧拉角 ( Euler Angles )
姿态变更常用绕x,y或z轴的一系列旋转来确定。欧拉角描述方 法是：先绕z轴旋转ø，然后绕新的y(即y/)轴旋转θ，最后绕更新的 z(z//)轴旋转ψ（见图3.2）欧拉变换Euler(ø,θ,ψ)可以通过连乘三个旋 转矩阵来求得
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
3.11
Sph(α,β,γ)
3.26
RPY(ø,θ,ψ)
3.12
我们已经研究过的各种平移与旋转的式子，总结在表3.1中。如果我 们使用Cyl和Sph的非旋转的形式，那么矩阵积（3.29）仅仅是一个平移 变换。
3.9 各种A矩阵的确定 ( Specification of matrices A )
现在考虑方程(3.1)右边 各A矩阵的确定。串联杆型 机械手是由一系列通过连杆 与其活动关节连接在一起所 组成 。
Euler(ø,θ,ψ) ＝Ｒot(z,ø)Ｒot(y,θ)Ｒot(z,ψ) （3.10）
在一系列旋转中，旋转的次序是重要的。应注意，旋转序列 如果按相反的顺序进行，则是绕基坐标中的轴旋转：绕z轴旋转ψ ， 接着绕y轴旋转θ，最后再一次绕z轴旋转ø ，结果如图3.3所示，它 与图3.2是一致的。
z z’
连续变换的若干A矩阵的积称为T矩阵，对于一个六连杆（六自由度）机械手有
T6 = A1 A2 A3 A4 A5 A6
（3.1）
六连杆的机械手有六个自由度，其中三个自由度用来确定位置，三个自由度用来确 定方向。Ｔ６表示机械手在基坐标中的位置与方向。则变换矩阵Ｔ６有下列元素
T6 =
nx ox ax px ny oy ay py
ø
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 )
如图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 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）
sinα cosα 0 rsinα -sinα cosα 0 0
Cyl(z,α,r) = 0
0 1z
0
0 00
0
0 01
0
0 01
Cyl(z,α,r) =
1 0 0 r cosα 0 1 0 r sinα 001 z 000 1
（3.20） （3.21） （3.22）
上式表明平移矢量未变，旋转矩阵为单位阵，此时末端坐标的姿态未变， 而只是改变了它的空间位置。
摇摆、俯仰和偏转为另一种旋转。如图3.4所示，就像水中航行的一条小船一 样，绕着它前进的方向（z轴）旋转 ø称为摇摆，绕着它的横向中轴（y轴）旋转 θ 称为俯仰，绕着它甲板的垂直向上的方向（x轴）旋转ψ 称为偏转。借助于这
种旋转来描述机械手的末端执行器如图3.5所示。规定旋转的次序为
RPY(ø,θ,ψ)＝Ｒot(z,ø)Ｒot(y,θ)Ｒot(x,ψ)
棱形关节：关节变量为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轴向为公垂线向下一个关节延伸的方向。
转动关节：关节变量为θ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。
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 末端执行器的描述
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.1 引言 ( Introduction )
本章，我们采用齐次变换来描述在各种坐标系中机械手的位置与方向。首先介绍各 种正交坐标系的齐次变换。然后介绍在非正交关节坐标系中描述机械手末端的齐次变换。 注意，对任何数目关节的各种机械手均可以这样进行。
描述一个连杆与下一个连杆之间关系的齐次变换称A矩阵。A矩阵是描述连杆坐标 系之间的相对平移和旋转的齐次变换。
xn-1
图3.9 连杆参数
表3.2 连杆参数
连杆本身 连杆长度 的参数 连杆扭转角
an 连杆两个轴的公垂线距离（x方向） αn 连杆两个轴的夹角（x轴的扭转角）
连杆之间 连杆之间的距离 的参数 连杆之间的夹角
dn 相连两连杆公垂线距离（z方向平移距） θn 相连两连杆公垂线的夹角（z轴旋转角）
为了描述连杆之间的关系，我们对每个连杆赋一个坐标系。
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 )
3.2 姿态描述 ( Specification of Orientation )
对式（3.2）中16个元素一一赋值就可确定T6。假定机械手可以到达要求 的位置，而单位旋转矢量o和a正交，即
o·o ＝ 1 a·a Baidu Nhomakorabea 1 o·a ＝ 0
（3.3） （3.4） （3.5）
a形成单位向量
a a
|a|
（3.6）
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
如 图 3.8 所 示 ， 任 何 一 个 连杆都可以用两个量来描述： 一 个 是 公 共 垂 线 距 离 an ， 另 一 个 是 与 an 垂 直 的 平 面 上 两 个 轴 的 夹 角 αn ， 习 惯 上 称 an 为连杆长度，αn称为连杆的 扭转角。
图3.8 连杆的长度与扭转角
如图3.9所示，在每个关节轴上有两个连杆与之相连，即关节轴有两个公垂线 与之垂直，每一个连杆一个。两个相连的连杆的相对位置用dn和θn确定， dn是沿着 n关节轴两个垂线的距离， θn是在垂直这个关节轴的平面上两个被测垂线之间的夹 角， dn和θn分别称作连杆之间的距离及夹角。
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.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）
第三章 运动学方程
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 小结
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(ø,θ,ψ)