机器人运动学

(1)

第三章第三章

机器人运动学

(2)

第三章第三章机器人运动学机器人运动学

3.1 引言

3.2 位姿表示和齐次变换

3.3 机器人的正逆运动学方程

3.4 机器人的微分运动和雅可比矩阵

(3)

3.1 3.1 引言引言

问题一问题一：已知杆件几何参数和关节：已知杆件几何参数和关节角矢量求机器人

角矢量求机器人

末端相对于参考坐

标系

标系的位置和姿态？的位置和姿态？

问题二问题二：给定机器人末端相对于参：给定机器人末端相对于参考坐标系的期望位置和姿态，机器考坐标系的期望位置和姿态，机器人人

能否、如何使其末端达到

能否、如何使其末端达到这个位这个位姿？姿？

－－实际应用问题

(4)

^{机器人运动学（} ^{机器人运动学（} Kinematics）主要是对机器人相对于参考坐标系的 Kinematics ）主要是对机器人相对于参考坐标系的运动运动进行分析研究，而不考虑引起这些运动

进行分析研究，而不考虑引起这些运动力和力矩力和力矩

正运动学正运动学 (direct kinematics),逆 (direct kinematics), 逆运动学运动学 (inverse kinematics) (inverse kinematics)

机械手型机器人，特别是最有代表性的关节坐标型机器人是由一系列关机械手型机器人，特别是最有代表性的关节坐标型机器人是由一系列关节连接而成的空间连杆

节连接而成的空间连杆开环链式机构开环链式机构

(5)

两自由度机器人运动学问题两自由度机器人运动学问题

关节变量关节变量－→－→手爪位置手爪位置（正（正））

1 1 2 1 2

cos cos( )

sin sin( )

x L L

y L L

θ θ θ

= + +

矢量表示矢量表示 ¹

2

, ( ) x y f

θ θ

⎡ ⎤ ⎡ ⎤

= ⎢ ⎥ = ⎢ ⎥

⎣ ⎦ ⎣ ⎦

= r r

θ θ

y y

x x

P P

θ θ

22

θ θ

11

L L

11

L L

22

r r

O O

(6)

手爪位置－手爪位置－→→关节变量关节变量（逆（逆））

非惟一解：非惟一解：

微分：微分：速度、加速度速度、加速度

2

2 2

1

1 2 2

arctan( ) arctan( sin ) cos L

y

x L L

θ π α θ θ

θ

= −

+

2 2 2 2

1 2

( )

arccos[ ]

2 x y L L

α = ⁻ ⁺ L L ⁺ ⁺

1

( ) f

⁻

= r

θ

1

′

θ θ

₂

^′

LL22sinsin

θ θ

22

y y

x x

P P

θ θ

22

θ θ

11

L L

11

L L

22

r r

O O

LL22coscos

θ θ

22

1

′ θ

2

′ θ

α ^′

α

(7)

3.2 3.2 位姿表示和齐次变换位姿表示和齐次变换

1.齐次坐标和位姿的矩阵表示 1. 齐次坐标和位姿的矩阵表示

一、空间点的表示一、空间点的表示

^一个点 ^一个点 P P 可用三个坐标表示可用三个坐标表示

式中式中分别为分别为和和轴上的单位向量轴上的单位向量二、空间向量的表示

二、空间向量的表示

一个向量一个向量可用三个起始和终止坐标表示可用三个起始和终止坐标表示

起始于原点，则有起始于原点，则有

矩阵形式矩阵形式

P = + + ai bj ck

k j

i , , x, y z

( ) ( ) ( )

P

AB

= Bx − Ax i + By − Ay j + Bz − Az k

P = + + ai bj ck

a

P b

c

⎡ ⎤ ⎢ ⎥

= ⎢ ⎥

⎢ ⎥ ⎣ ⎦

(8)

三、齐次坐标三、齐次坐标

一个点矢量也可用齐次坐标表示为列矩阵一个点矢量也可用齐次坐标表示为列矩阵式中式中为比例系数，为比例系数，

齐次坐标不是单值确定的，向量齐次坐标不是单值确定的，向量可表示为可表示为

或或。坐标原点可表示为。坐标原点可表示为

^分别表示 ^分别表示 ^轴的无 ^轴的无

穷远点，即表示直角坐标轴的方向穷远点，即表示直角坐标轴的方向。。矢量矢量没有意义没有意义

x P y

z

ω

⎡ ⎤ ⎢ ⎥

= ⎢ ⎥

⎢ ⎥ ⎢ ⎥ w ^a = ^x ^w ^, ⎣ ⎦

w z c

w y

b = , =

k j

i 4 5 3 + +

[ ³ ^, ⁴ ^, ⁵ ^, ¹ ] [

^T

^, ⁶ ^, ⁸ ^, ¹⁰ ^, ² ]

^T

[ ⁻ ¹² ^, ⁻ ¹⁶ ^, ⁻ ²⁰ ^, ⁻ ⁴ ]

^T

[ ⁰ ^, ⁰ ^, ⁰ ^, ⁿ ]

^T

[ ⁿ ^, ⁰ ^, ⁰ ^, ⁰ ] [

^T

^, ⁰ ^, ⁿ ^, ⁰ ^, ⁰ ] [

^T

^, ⁰ ^, ⁰ ^, ⁿ ^, ⁰ ]

^T

[

⁰ ^,⁰ ^,⁰ ^,⁰

]

^T

oz oy

ox , ,

(9)

^例 ^例 1：图中所示的矢量用齐次坐标表示 1 ：图中所示的矢量用齐次坐标表示

u：cos α = ₀ ， _cos β = _0.866 ， _cos γ = _0.5 u = [0 0.866 0.5 0]T

v：cos α = _0.866 ， _cos β = ₀ ， _cos γ = _0.5 v = [0.866 0 0.5 0]T

w：cos α = _0.866 ， _cos β = _0.5 ， _cos γ = ₀

w = [0.866 0.5 0 0]T

(10)

^{参考坐标系} ^{参考坐标系}

全局参考坐标系全局参考坐标系（（ Base Reference Base Reference Coodinate）：通用坐标系， Coodinate ）：通用坐标系，

其位置和方位不随机器人各构件运动而变化，用来定义机器人相对其位置和方位不随机器人各构件运动而变化，用来定义机器人相对

于其他物体的运动。也称为：

于其他物体的运动。也称为：惯性坐标系、固定坐标系惯性坐标系、固定坐标系（相对（

相对）

）

构件坐标系构件坐标系（（ Frame Frame Coodinate）：为固联在机器人各构件上的坐 Coodinate ）：为固联在机器人各构件上的坐标系，它随构件在空间的运动而运动（旋转或平移），用来描述独标系，它随构件在空间的运动而运动（旋转或平移），用来描述独

立关节的运动。也称为：

立关节的运动。也称为：运动坐标系运动坐标系

全局参考坐标系全局参考坐标系

构件坐标系构件坐标系

(11)

^向量计算 ^向量计算

设设则有则有

其中其中

( )

⎪ ⎪

⎩

⎪ ⎪

⎨

⎧

=

cont

r r r r R

b b b b B

a a a a A

T z

y x

T z

y x

T z

y x

α

ω ω

ω

, , ,

( )

ω ω

ω

α α

a a a

a A

r r r r B

A R

b a b

a b

a B

A

b b a

a b

b a

a b

b a

B a A

a a a a A

z y

x

T z

y x

z z y

y x

x

T z

y z x y

x

T z

y x

2 2

2

, , ,

1 , ,

, , , ,

+ +

=

×

=

+ +

=

• ⎟⎟ ⎠

⎜⎜ ⎞

⎝

⎛ + + +

= +

⎟ ⎠

⎜ ⎞

⎝ ⎛

=

ω ω ω

a b r

b a b

a r

b a b

a r

b a b

a r

x y y

x z

z x x

z y

y z z

y x

=

−

=

−

=

−

=

x y z

i j k

a a a

b b b

(12)

四、刚体的位姿表示四、刚体的位姿表示

移动坐标系在固定参考坐标系中的表示移动坐标系在固定参考坐标系中的表示

令令 n（ n （ normal） normal ）、、 o（ o （ orientation） orientation ）、、 a（ a （ approach） approach ）分别为移动坐标系分别为移动坐标系 X X ′ ′ 、、 _Y _Y _′ _′ 、、 _Z _Z _{′坐标轴的单位矢量} _′ 坐标轴的单位矢量，以齐次坐标形式分别表示为，以齐次坐标形式分别表示为

[ ]

0 0 0 1

x x x x

y y y y

z z z z

F

n o a P

=

⎡ ⎤

⎢ ⎥

= ⎢ ⎥

⎢ ⎥

⎣ ⎦

n o a P

(13)

^{刚体位姿的表示} ^刚体位姿 ^的表示

通过在刚体上固连一个移动坐标系，将固连坐标系在空间表示出来通过在刚体上固连一个移动坐标系，将固连坐标系在空间表示出来

刚体的运动可看作是刚体随其上任一点的平动及刚体绕该点的转动的合成刚体的运动可看作是刚体随其上任一点的平动及刚体绕该点的转动的合成

约束条件→约束条件→

6个自由度 6

个自由度

0 0 0 1

x x x x

y y y y

object

z z z z

n o a P

F n o a P

⎡ ⎤

⎢ ⎥

= ⎢ ⎥

⎢ ⎥

⎣ ⎦

1 1 1

=

• =

• n n

a a

o

a o

o

n = ×

(14)

例2：图中固连于连杆的坐标系{B}位于O

_B

点，X

_B

= 2，Y

_B

= 1，

Z

_B

= 0。在XOY平面内，坐标系{B}相对固定坐标系{A}有一个30°的偏转，试写出表示连杆位姿的坐标系{B}的齐次坐标矩阵表达式

[ ^{cos 30} ^{cos 60} ^{cos 90} ⁰ ]

^T

= ° ° °

n

[

^0.866 ^0.500 ^0.000 ⁰

]

^T

=

[ ^cos120 ^{cos 30} ^{cos 90} ⁰ ]

^T

= ° ° °

o

[

^0.500 ^0.866 ^0.000 ⁰

]

^T

= −

[ ^0.000 0.000 1.000 0 ]

^T

a =

[ ^{2 1 0 1} ]

^T

= P

0.866 0.500 0.000 2.0 0.500 0.866 0.000 1.0 0.000 0.000 1.000 0.0

0 0 0 1

−

=

⎡ ⎤

⎢ ⎥

⎣ ⎦

T

(15)

课堂练习：图中固连于连杆的坐标系{O’} 的原点位于ZOY平面内，坐标值x = 0，y = 3，z = 5。，坐标系{O’}相对固定坐标系z、x、y轴依次有90° 、90°和30°的偏转，写出连杆坐标系的齐次坐标矩阵

0.500 0.866 0 0

0 0 1 3

0.866 0.500 0 5

0 0 0 1

− −

⎡ ⎤

⎢ ⎥

⎢ − ⎥

⎢ ⎥

⎣ ⎦

(16)

2. 齐次坐标 2. 齐次坐标变换变换

变换定义为空间的一个运动，可以用坐标系来表示变换定义为空间的一个运动，可以用坐标系来表示一、齐次变换

一、齐次变换

若已知一直角坐标系

若已知一直角坐标系中一点的坐标，那么该点在另一直角坐标中一点的坐标，那么该点在另一直角坐标系系中的坐标可由下列变换公式求得中的坐标可由下列变换公式求得

其中，其中，为坐标系为坐标系的原点在坐标系的原点在坐标系中的坐标，中的坐标，

是坐标系是坐标系各轴相对于坐标系各轴相对于坐标系的方向余弦的方向余弦引入齐次坐标变换，

引入齐次坐标变换，表示齐次坐标，表示齐次坐标，

则有则有

z y x

o ′ ′ ′ ′ oxyz

⎪ ⎩

⎪ ⎨

⎧

′ +

=

′ +

=

′ +

=

3 3

2 1

2 3

2 1

1 3

2 1

r z c y c x c z

r z b y b x b y

r z a y a x a x

3 2 1

, r , r

r o ′ x ′ y ′ z ′

3

oxyz

1

c

a … _o _′ _x _′ _y _′ _z _′

oxyz

( ^x

₁

^, ^x

₂

^, ^x

₃

^, ^ω ) ^and ( ^x

₁

^′ ^, ^x ^′

₂

^, ^x ^′

₃

^, ^ω ^′ )

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

′

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

=

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

⇒

⎪ ⎪

⎩

⎪ ⎪

⎨

⎧

= ′

+ ′ + ′

+ ′

= ′

+ ′ + ′

+ ′

= ′

+ ′ + ′

+ ′

= ′

ω ω ω

ω

ω ω ω

3 2 1

3 3

2 1

2 3

2 1

1 3

2 1

3 2 1

3 3

3 2

2 1

1 3

2 3

3 2

2 1

1 2

1 3

3 2

2 1

1 1

1 0

0 0

x x x

r c

c c

r b

b b

r a

a a

x x x

r x

c x

r x

b x

r x

a x

a

x

(17)

二、平移二、平移齐次变换齐次变换

(18)

点点 A A 平移到平移到 B（向量相加） B （向量相加）

为平移齐次变换矩阵为平移齐次变换矩阵（算子）（算子）

( )

1 0 0 0 1 0

( , , ) 0 0 1

1 0 0 0 1

1 0 0 0 1 0

, , 0 0 1

0 0 0 1

A B A B

BO

a x x a a x

b y y b b y

P P P Trans a b c P

c z z c c z

a H Trans a b c b

c

ω ω

+ +

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ + ⎥ ⎢ + ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥

= + = = = =

⎢ + ⎥ ⎢ + ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦

⎡ ⎤

⎢ ⎥

= =

⎢ ⎥

⎣ ⎦

XX

YY ZZ

AA

BB

OO aa

bb cc

(19)

三、旋转三、旋转齐次变换齐次变换

(20)

绕绕 x x 轴旋转的直角坐标变换轴旋转的直角坐标变换

易得绕的易得绕的 X,Y,Z X,Y,Z 轴旋转轴旋转 θ θ 角的相应旋转齐次变换是角的相应旋转齐次变换是

( )

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡ −

=

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

= −

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

= −

1 0 0

0

0 1 0

0

0 0 cos

sin

0 0 sin

cos ,

1 0

0 0

0 cos

0 sin

0 0

1 0

0 sin

0 cos

,

1 0

0 0

0 cos

sin 0

0 sin

cos 0

0 0

0 1

,

θ θ

θ

θ θ

θ

θ θ

θ θ θ

Z Rot

Y Rot

X Rot

1 0 0

0 cos sin 0 sin cos

x n

y o

z a

P P

θ θ

⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢= − ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦ ⎣ ⎦

YY ZZ

OO aa

oo PP

PPaa

PPoo

PPzz

PPyy

ll33

ll⁴⁴

ll11

ll22

1 2

3 4

cos sin sin cos

x n

y o a

z o a

P P

P l l P P

θ θ

=

= − = −

= + = +

(21)

例：已知一个点

例：已知一个点 ,绕 , 绕轴旋转轴旋转，试求该向量，试求该向量

因为因为，故，故

k j

i

u = 7 + 3 + 2 _Z 90 °

0 90

cos cos

, 1 90

sin

sin θ = ° = θ = ° =

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡−

=

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡ −

=

1 2 7

3 1 2 3 7

1 0 0

0 0 1 0

0 0 0 0

1 0 0 1 0

v

(22)

四、复合（旋转加平移）四、复合（旋转加平移）变换变换

(23)

将两个旋转变换和平移变换结合起来，矩阵表达式为：将两个旋转变换和平移变换结合起来，矩阵表达式为：

绕绕

z z

轴旋转90轴旋转90°°；；

接着绕接着绕

y y

接着平移接着平移[4,[4,--3,7]3,7]

点点经变换后变成经变换后变成

( ^{4, 3, 7} ) ( ^{, 90} ) ( ^{, 90} )

1 0 0 4 0 0 1 0 0 1 0 0

0 1 0 3 0 1 0 0 1 0 0 0

0 0 1 7 1 0 0 0 0 0 1 0

0 0 0 1 0 0 0 1 0 0 0 1

0 0 1 4

1 0 0 3

0 1 0 7

0 0 0 1

H

=

Trans

−

Rot Y Rot Z

⎡ ⎤ ⎡ ⎤ ⎡ − ⎤

⎢ − ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

= ⎢ ⎥ ⎢ − ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦ ⎣ ⎦

⎡ ⎤

⎢ − ⎥

⎢ ⎥

= ⎢ ⎥

⎢ ⎥

⎣ ⎦

7 3 2

P = + i j + k

( ^{4, 3, 7} ) ( ^{, 90} ) ( ^{, 90} ) [ ^{6, 4,10,1} ]

^T

xyz noa

P = Trans − ⋅ Rot Y ⋅ Rot Z P =

(24)

改变变换顺序改变变换顺序

绕绕

z z

接着接着平移平移[4,-[4,-3,7] 3,7] ；；

接着接着绕绕

y y

轴旋转90轴旋转90°°

( ^{, 90} ) ( ^{4, 3, 7} ) ( ^{, 90} ) [ ^{9, 4, 1,1} ]

^T

xyz noa

P = Rot Y ⋅ Trans − ⋅ Rot Z P = − (

^{, 90}

) (

^{4, 3, 7}

) (

^{, 90}

)

H = Rot Y Trans − Rot Z

0 0 1 9 1 0 0 4

0 1 0 1

0 0 0 1

⎡ ⎤

⎢ ⎥

⎢ − ⎥

⎢ ⎥

⎣ ⎦

0 0 1 4

1 0 0 3

0 1 0 7

0 0 0 1

⎡ ⎤

⎢ − ⎥

⎢ ⎥

⎣ ⎦

(25)

3. 齐次 3. 齐次变换的几何意义变换的几何意义

一、变换矩阵的

一、变换矩阵的列向量列向量分解及其几何解释分解及其几何解释 1、当 1 、当为单位阵时，即为单位阵时，即

说明构件坐标系与参考坐标系完全重合，即相当于没有进行变换。从左说明构件坐标系与参考坐标系完全重合，即相当于没有进行变换。从左至右的列向量表示两个坐标系的各对应轴

至右的列向量表示两个坐标系的各对应轴和原点和原点 2、当 2 、当为为

表示构件坐标系

表示构件坐标系相对于参考坐标系相对于参考坐标系先绕先绕轴旋轴旋转转，，即即；再绕；再绕轴旋转轴旋转，即，即。旋转后，使得。旋转后，使得轴与轴与轴重合，轴重合，轴与轴与轴重合，轴重合，轴与轴与轴重合轴重合

H

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

=

1 0 0 0

0 1 0 0

0 0 1 0

0 0 0 1

H

( ) ( ) ( ) ^X ^Y ^Y ^Z ^Z

X ′ 、 ′ 、 ′

H

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

=

1 0 0 0

0 0 1 0

0 0 0 1

0 1 0 0 H

( O ′ − X ′ Y ′ Z ′ ) ( ^O − ^XYZ ) Z

°

90 ^Rot ( ^Z ^, ⁹⁰ ) _Y Rot ( Y , 90 )

X ′ Y Y ′ Z Z ′ X

°

90

(26)

3、当 3 、当为为

表示进行上述两次旋转变换后，再将构件坐标系

表示进行上述两次旋转变换后，再将构件坐标系在参在参考坐标系考坐标系中进行平移中进行平移

4、当进行如下变换时，即 4 、当进行如下变换时，即

表示对构件坐标系各轴上的单位向量，进行表示对构件坐标系各轴上的单位向量，进行

变换后，构件坐标系变换后，构件坐标系各轴上单位向量顶点和坐标原点，在各轴上单位向量顶点和坐标原点，在参考坐标系

参考坐标系上有了新的坐标值上有了新的坐标值 H

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

= −

1 0

0 0

7 0

1 0

3 0

0 1

4 1

0 0 H

( O ′ − X ′ Y ′ Z ′ ) ( O − XYZ ) ^Trans ( ⁴ ^, ⁻ ³ ^, ⁷ )

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

−

= −

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

= −

1 1

7 7

8 7

3 3

3 2

4 5

4 4

1 1 1 1

0 1 0 0

0 0 1 0

0 0 0 1

1 0 0 0

7 0 1 0

3 0

0 1

4 1 0 0

H

( ⁴ ^, ³ ^, ⁷ ) ( ^Rot ^Y ^, ⁹⁰ ) ( ^Rot ^Z ^, ⁹⁰ )

Trans −

( ^O ^′ ⁻ ^X ^′ ^Y ^′ ^Z ^′ )

( ^O − ^XYZ )

(27)

二、以构件坐标系二、以构件坐标系进行变换（区分于以进行变换（区分于以固定坐标系固定坐标系进行变换）进行变换）

对于变换过程对于变换过程

若变换过程若变换过程从左向右从左向右进行可做如下解释：构件坐标系进行可做如下解释：构件坐标系首先在参考坐标系

首先在参考坐标系中平移，然后绕当前构件坐标系的中平移，然后绕当前构件坐标系的轴轴旋转旋转，最后再绕当前的构件坐标系，最后再绕当前的构件坐标系轴旋转轴旋转，如图所示，如图所示

1 1 、如果用一个描述平移和（或）旋转的变换、如果用一个描述平移和（或）旋转的变换左乘一个坐标系变换左乘一个坐标系变换，，那么产生的平移和（或）旋转是相对于

那么产生的平移和（或）旋转是相对于

参考坐标系

参考坐标系进行的。进行的。

2 2 、、如果用一个描述平移和（或）旋转的变换如果用一个描述平移和（或）旋转的变换右乘一个代表坐右乘一个代表坐标系的变标系的变换换，那么产生的平移和（或）旋转是相对于，那么产生的平移和（或）旋转是相对于

变换的坐标系（构件坐标

系）

轴

轴进行的进行的。。

( ) ( ) ( )

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

= −

−

1 0 0 0

7 0 1 0

3 0

0 1

4 1 0 0 90

, 90

, 7

, 3 ,

4

Rot Y Rot Z Trans

( ^O

^′⁻

^X

^′

^Y

^′

^Z

^′

)

( ^O ⁻ ^XYZ ) _{Y ′}

°

90 Z ′ 90 ^°

C C T

T

C

(28)

例如：一个坐标系

例如：一个坐标系 C C 和一个变换和一个变换 T T ，，相当于绕相当于绕 Z Z 轴旋转轴旋转 90° 90 °，，且在且在 X轴 X 轴方向平移方向平移 10 10 个单位。个单位。

当变换过程是相对于参考坐标系产生时，得到新的构件坐标系当变换过程是相对于参考坐标系产生时，得到新的构件坐标系

^{当变换过程是相对于} ^{当变换过程是相对于} C C 坐标系的轴产生，坐标系的轴产生，

得到新的构件坐标系得到新的构件坐标系

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

=

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

−

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡ −

=

1 0 0 0

0 0 1 0

20 0 0 1

0 1 0 0

1 0 0 0

0 0 1 0

10 1 0 0

20 0

0 1

1 0 0 0

0 1 0 0

0 0 0 1

10 0 1 0 TC A

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

−

=

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡ −

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

= −

=

1 0 0 0

0 0 0 1

20 1 0

0

30 0 1 0

1 0 0 0

0 1 0 0

0 0 0 1

10 0 1 0

1 0 0 0

0 0 1 0

10 1 0 0

20 0 0 1 CT B

(29)

三、变换过程的可逆性

三、变换过程的可逆性 ——逆变换 —— 逆变换

逆变换使构件坐标系返回到原来的参考坐标系逆变换使构件坐标系返回到原来的参考坐标系一般说来，已知变换

一般说来，已知变换 T T 为为

那么，其逆变换为那么，其逆变换为

旋转变换矩阵是酉矩阵旋转变换矩阵是酉矩阵

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

=

1 0

0 0

z z

y y

x x

p a

o n

p a

o n

p a

o n

T

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

• −

=

−

1 0

0 0

1

a P a

a a

o P o

o o

n P n

n n T

z y x

( )

¹

( )

1

, ,

^T

T

Rot x Rot x

RR RR I

θ

⁻

θ

−

=

= =

(30)

作业作业：：

1.2、 1.2 、 1.4、 1.4 、 1.5、 1.5 、 1.6 1.6

(31)

1. 1. 机器人运动方程的表示机器人运动方程的表示

一、变换方程的建立一、变换方程的建立

一个机器人的末端是执行器的中心点可用两种变换来描述：

一种是通过机座、机械接口到该点的变换一种是通过机座、机械接口到该点的变换

一种是通过物体到该点的变换一种是通过物体到该点的变换

由于两个变换终点相同，故有

由于两个变换终点相同，故有由该方程易得由该方程易得如果机器人运动使得手部正好定位在物体上面，就可从上述

如果机器人运动使得手部正好定位在物体上面，就可从上述方程方程求出求出

B的变换，即 B

的变换，即

( ^Z ⁻ ^T

₆

⁻ ^E ) ( B − G )

BG E

ZT

₆

= T

₆

= Z BGE

⁻¹ ⁻¹

1 6

= ZT EG

−

B

3.3 3.3 机器人的正逆运动学方程机器人的正逆运动学方程

(32)

二、机器人的变换矩阵的建立二、机器人的变换矩阵的建立

机器人机构是由一系列连杆结构组成，而每一个连杆机构可以看作是一个机器人机构是由一系列连杆结构组成，而每一个连杆机构可以看作是一个构件坐标系，引入齐次变换可以描述这些坐标系之间的

构件坐标系，引入齐次变换可以描述这些坐标系之间的相对位置和方向相对位置和方向

设描述一个连杆与下一个连杆之间关系的齐次变换阵为设描述一个连杆与下一个连杆之间关系的齐次变换阵为矩阵，可以描述矩阵，可以描述连杆构件坐标系之间相对平移和旋转的齐次变换。

连杆构件坐标系之间相对平移和旋转的齐次变换。

描述第一个连杆相对于某个坐标系（如机身）的位姿描述第一个连杆相对于某个坐标系（如机身）的位姿

描述第二个构件坐标系相对于第一个连杆构件坐标系的位姿描述第二个构件坐标系相对于第一个连杆构件坐标系的位姿可以给出第二个连杆在基坐标系里的位姿

可以给出第二个连杆在基坐标系里的位姿

如此类推，对于一个六连杆机器人，有如此类推，对于一个六连杆机器人，有

机器人最后一个构件（手部）有三个自由度用来确定其位置，三个自由度机器人最后一个构件（手部）有三个自由度用来确定其位置，三个自由度确定其方向。用表示机械手的位置和姿态，这样六连杆

确定其方向。用表示机械手的位置和姿态，这样六连杆机器人在它的活动机器人在它的活动范围内可以任意定位和定向

范围内可以任意定位和定向

A

1

A

2

2 1

2

A A

T

=

6 5 4 3 2 1

6

A A A A A A

T =

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

=

1 0 0 0

6

z z z z

y y y y

x x x x

p a o n

p a o n T

Z0

Y₀

ⁿ

o

^P

^a

^o

X₀

(33)

对于图示的机器人手部，可以把描述其位置和方向的坐标系原点定在两个手对于图示的机器人手部，可以把描述其位置和方向的坐标系原点定在两个手指的中点，用一个向量

指的中点，用一个向量

P P

描述该原点，用三个单位向量描述该原点，用三个单位向量

n n 、、 o和 _o

和

a a

描述机器描述机器人的姿态：

人的姿态：

向量向量

Z Z

指向夹手接近物体的方向上，其单位指向夹手接近物体的方向上，其单位向量向量

a a

称为接近向量称为接近向量；向量；向量

Y Y

的方向的方向从一个指从一个指尖指向另一个指尖，处于规定夹手方向上，称尖指向另一个指尖，处于规定夹手方向上，称为方向向量为方向向量

o o

；向量；向量

n n

称为法线向量称为法线向量，它与向，它与向量量

o和 o

和

a 一起构成右手矢量积 a

一起构成右手矢量积

机器人的结构确定以后，变换矩阵的形式也就确定机器人的结构确定以后，变换矩阵的形式也就确定，，就成为关节坐标向量就成为关节坐标向量的函数，若给定关节坐标向量

的函数，若给定关节坐标向量，由变换矩阵可很容易地求出手臂端点的位，由变换矩阵可很容易地求出手臂端点的位置和指向置和指向。。

a o n = ×

T

6

θ θ

(34)

2.机器人运动学方程的建立 2. 机器人运动学方程的建立

机器人机构运动学方程的建立方法，就是建立变换矩阵的方法机器人机构运动学方程的建立方法，就是建立变换矩阵的方法

对于变换矩阵，可以分解为

其中其中为综合平移变换为综合平移变换

为综合旋转变换为综合旋转变换

向量向量

P P

没有限制，向量没有限制，向量必须满足必须满足即向量即向量

o和 o

和

a必须是单位向量且正交 a

必须是单位向量且正交

变换可以是纯平移变换、纯旋转变换或既包括平移变换又包括旋转变换可以是纯平移变换、纯旋转变换或既包括平移变换又包括旋转变换的混合变换

变换的混合变换。。

6

0 1 0 0 0 0 1 0 0 0 0 1

0 0 0 1 0 0 0 1 0 0 0 1

x x x x x x x x

y y y y y y y y

z z z z z z z z

n o a p n o a p

T n o a p n o a p

⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

= =

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎣ ⎦ ⎣ ⎦ ⎣ ⎦

( )

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

=

1 0 0 0

1 0 0

0 1 0

0 0 1

z y x

p p p P

Trans

( )

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

=

1 0 0 0

0 0 0 ,

,

z z z

y y y

x x x

a o n

a o n a

o n Rot

( ⁿ ^o ^a )

n a

o

、、 = ×

1 1 1

=

• =

• n n

a a

o

(35)

（（ 1 ） 1 ）、欧拉角（、欧拉角（ Euler） Euler ）变换变换 ——纯旋转变换 —— 纯旋转变换

欧拉角：先绕

欧拉角：先绕轴旋转轴旋转角，然后绕新的角，然后绕新的（即（即）轴旋转）轴旋转角，最后绕新的角，最后绕新的（即（即））轴旋转轴旋转角，则变换矩阵为：角，则变换矩阵为：

Z

φ

_Y _{Y ′}

θ

Z

Z ′′

ψ

Z、Z′ Z、Z′ ψ

Z′′、Z ′′′ φ Z ′′ φ Y ′′′ Y ′′′ Z ′′′ θ Y ′′

ψ θ φ φ

ψ θ Y′Y′′ θ Y ′ o φ Y o ψ Y

θ θ

φ ψ X ψ X X ′ θ φ X ′ X ′′ X ′′′ X ′′ X ′′′

欧拉角在构件坐标系中的描述欧拉角在基准坐标系中的描述

( ) ( ) ( ) ( )

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

−

+

− +

−

=

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡ −

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

⎥ −

⎥⎥

⎥

⎦

⎤

⎢⎢

⎣

⎡ −

=

1 0 0

0

0 0 0

1 0 0

0

0 1 0

0

0 0 cos

sin

0 0 sin

cos

1 0 0 0

0 cos 0 sin

0 0 1 0

0 sin 0 cos

1 0 0

0

0 1 0

0

0 0 cos

sin

0 0 sin cos

θ ψ

θ

θ φ ψ φ ψ θ φ ψ

φ ψ θ φ

θ φ ψ φ ψ θ φ ψ

φ ψ θ φ

ψ ψ

θ θ

φ φ

ψ θ

φ ψ

θ φ

c s

s c

s

s s c c s c s s

c c c s

s c c s s c c s

s c c c

Z Rot Y

Rot Z

Rot

Euler ，，，，，

(36)

（（ 2 ）、横滚、俯仰和偏转 2 ）、横滚、俯仰和偏转( ( RPY RPY )变换 ) 变换（（ Roll- Roll -Pitch Pitch- -Yaw Yaw 的缩写）的缩写）

从构件的角度看，横滚相当于绕

从构件的角度看，横滚相当于绕轴旋转轴旋转，俯仰相当于绕，俯仰相当于绕轴旋转轴旋转，偏，偏转相当于绕

转相当于绕轴旋转轴旋转。变换矩阵为：。变换矩阵为：

X Z

φ Z φ

θ

ψ o Y

θ ψ o

Y X

横滚φ、俯仰θ和偏转ψ 机械手的横滚、俯仰和偏转坐标

Z φ _Y θ

X ψ

( ) ( ) ( ) ( )

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

−

− +

+

−

=

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

−

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

⎥ −

⎥⎥

⎥

⎦

⎤

⎢⎢

⎣

⎡ −

=

1 0

0 0

0 0 0

1 0

0 0

0 cos

sin 0

0 sin

cos 0

0 0

0 1

1 0 0 0

0 cos 0

sin

0 0 1 0

0 sin 0 cos

1 0 0

0

0 1 0

0

0 0 cos

sin

0 0 sin cos

ψ θ ψ

θ θ

ψ φ ψ θ φ ψ φ ψ θ φ θ φ

ψ ψ

θ θ

φ φ

ψ θ

φ ψ

θ φ

c c s

c s

s c c s s c c s s s c s

s s c s c c s s s c c c

X Rot Y

Rot Z

Rot

RPY ，，，，，

(37)

(3)、圆柱坐标（ (3) 、圆柱坐标（ Cyl Cyl ）变换）变换

圆柱坐标式机器人确定其手部的位置和方位的变换包括平移和

圆柱坐标式机器人确定其手部的位置和方位的变换包括平移和旋转两种旋转两种从基础坐标系出发变换的顺序为：沿

从基础坐标系出发变换的顺序为：沿轴平移轴平移，接着绕，接着绕轴旋转轴旋转，最沿，最沿轴平移轴平移

相对于参考坐标系的变换，

相对于参考坐标系的变换，位置和姿态都有变化位置和姿态都有变化，变换矩阵为：，变换矩阵为：

X ψ _Y θ ^Z φ

r Z α

Z z

Z

a

o n Y o

r z α X

圆柱坐标变换

( ) ( ) ( ) ( )

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡ −

=

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡ −

⎥ ⎥

⎦

⎤

⎢ ⎢

⎣

⎡

=

1 0

0 0

1 0

0 sin 0

cos sin

cos 0

sin cos

1 0 0 0

0 1 0 0

0 0 1 0

0 0 1

1 0 0

0 0 1 0

0 0 0 cos

sin

0 0 sin

cos

1 0 0 0

1 0 0

0 0 1 0

0 0 0 1

0 , 0 , ,

, 0 , 0 ,

,

z r r r

z

r Trans Z

Rot z Trans

r z Cyl

α α

α

α α

α

α α

X

(38)

（4 （ 4）、球坐标（）、球坐标（ Sph Sph ）变换）变换

球坐标机器人确定其构件在空间的位置和姿态的变换可描述为：在基坐球坐标机器人确定其构件在空间的位置和姿态的变换可描述为：在基坐

标系中先沿

标系中先沿轴平移轴平移，然后绕，然后绕轴旋转轴旋转，最后绕，最后绕轴旋转轴旋转

Z −α

r Y

Z β Z α

Z

a

o β 绕 Y 轴旋转后 r n

Y o

α X

球坐标变换

( ) ( ) ( ) ( )

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

−

=

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

⎥⎥

⎦

⎤

⎢⎢

⎣

⎡

⎥ −

⎥⎥

⎥

⎦

⎤

⎢⎢

⎣

⎡ −

=

1 0

0 0

cos cos

0 sin

sin sin sin

sin cos

cos sin

sin cos sin

cos sin

cos cos

1 0 0 0

1 0 0

0 0 1 0

0 0 0 1

1 0

0 0

0 cos

0 sin

0 0

1 0

0 sin

0 cos

1 0 0

0

0 1 0

0

0 0 cos

sin

0 0 sin cos

, 0 , 0 ,

, ,

,

β β

β

β α β

α α

β α

β α β

α α

β α

β β

α α

β α

α

r r r

r r

Trans Y

Rot Z

Rot r

z Cyl

机器人运动学

第三章 第三章