Sie sind auf Seite 1von 27

Robotic Systems(4)

Dr Richard Crowder
School of Electronics and Computer Science
Revised Jan 2011
Kinematic Equations
Forward Kinematics: computation of the position,
orientation and velocity of the end effector, given the
displacements and joint angles.
Reverse Kinematics: computation of the joint
displacements and angles from the end effectors position
and velocity.
R
n
Joint space
n variables
R
6
Tool space
6 Variables
Forward
Reverse
Homogeneous Transformations
(
(
(
(

- -
- -
- -
=
(

(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
+ + =
1 0 0 0
1 0
namely ation, transform describe matrix to 4 4 a use we robotics In
1
: can write we , : space in point a Consider
3 , 3
2 , 2
1 , 1
z
y
x
P R
P R
P R
n Translatio Rotation
A
c
b
a
w
cw
bw
aw
w
z
y
x
v
ck bj ai v
3D Transformation Matrices - 1
Consider two frames F
1
and F
2
with origins P
1
and P
2.
The relative position of F
1
in F
2
is given by the coordinates
of P
2
in F
1
.
F
2
F
1
P
2
3D Transformation Matrices - 2
The relative orientation of the frames is expressed by the cosine of
NINE angles. The orientation of the x axis of F
2
in F
1
is given by:

If this is repeated for the y and z axes, we can write:



Only four cases are require, translation and rotation around the
respective x, y and z axes

z y x
l l l z y x , , , ,
2 2 2

(
(
(
(

=
1 0 0 0
z z z z
y y y y
x x x x
p n m l
p n m l
p n m l
H
3D Transformation Matrices - 3
Consider the rotation around the x axis
X
Y
1
Y
2
Z
1
Z
2
Y
2

1 0 0 0
0
0
1
] Rot[x,
0 hence , 90 z , y and x
1 hence , coinsident are x and x
1 1 2
1 2
(
(
(
(

- - -
- - -
- - -
=
= = =
=
u
z y
o
x
l l
l
3D Transformation Matrices - 4
Consider the rotation around the x axis
X
Y
1
Y
2
Z
1
Z
2
Y
2

1 0 0 0
sin 0
cos 0
0 1
] Rot[x,
sin hence , 90 z and y
cos hence , y and y
0 hence , 90 is x and y
1 2
1 2
o
1 2
(
(
(
(

- -
- -
- -
=
= =
= =
=
u
u
u
u u
u u
z
y
x
m
m
m

90 -

90 -
3D Transformation Matrices - 5
Consider the rotation around the x axis
X
Y
1
Y
2
Z
1
Z
2
Y
2

1 0 0 0
cos sin 0
sin cos 0
0 0 1
] Rot[x,
cos hence , z and z
sin - hence , 90 y and z
0 hence , 90 is x and z
1 2
1 2
o
1 2
(
(
(
(

-
-
-
=
= =
= + =
=
u u
u u
u
u u
u u
z
y
x
n
n
n

90 +
3D Transformation Matrices - 6
Consider the rotation around the x axis
X
Y
1
Y
2
Z
1
Z
2
Y
2

1 0 0 0
0 cos sin 0
0 sin cos 0
0 0 0 1
] Rot[x,
zero is on translati hence coinside, P and P
1 2
(
(
(
(

=
u u
u u
u
Other transformations
(
(
(
(
(

u u
u u
u
1 0 0 0
0 C 0 S -
0 0 1 0
0 S 0 C
= ] Rot[y,
(
(
(
(
(

u u
u u
u
1 0 0 0
0 1 0 0
0 0 C S
0 0 S - C
= ] Rot[z,
(
(
(
(
(

1 0 0 0
c 1 0 0
b 0 1 0
a 0 0 1
= c] b, Trans[a,
Consider
| |
| |
| | | | | | | |u y Rot z Rot u z Rot y Rot w
v y Rot w
u z Rot v
o o o o
o
o
90 , 90 , 90 , 90 ,
1
3
7
2
1
2
7
3
.
1 0 0 0
0 0 0 1
0 0 1 0
0 1 0 0
90 ,
1
2
7
3
1
2
3
7
.
1 0 0 0
0 1 0 0
0 0 0 1
0 0 1 0
90 ,
= =
(
(
(
(

=
(
(
(
(

(
(
(
(

= =
(
(
(
(

=
(
(
(
(

(
(
(
(


= =
Verification
X
Y
Z
| | | |u z Rot y Rot
o o
90 , 90 ,
| | | |u y Rot z Rot
o o
90 , 90 ,
Movement of co-ordinate frames - 1
Y
Z
X
Z
X
Y
Z
X
Y
(
(
(
(

=
(

1 0 0 0
7 0 1 0
3 0 0 1
4 1 0 0
2
z Rot
2
y Rot ] 7 3 4 [ Trans
t t
Movement of co-ordinate frames - 2
Origin of the original frame [0 0 0 1]
T
, transform to [4 -3 7 1]
T
x = [1 0 0 1]
T
transforms to [4 -2 7 1]
T
y = [0 1 0 1]
T
transforms to [4 -3 8 1]
T
z = [0 0 1 1]
T
transforms to [5 -3 7 1]
T
If the four points are plotted, the new frame results, also subtracting the origin
position from the new unit vectors, gives the orientation.
(
(
(
(

=
(
(
(
(

(
(
(
(

0
0
1
0
1
7
3
4
1
7
2
4
Origin
X
This indicates that the new X axis
is parallel with the original Y axis
Relative Transformations
Up to this point we have considered all movements to be
made relative to a fixed co-ordinate frame, consider

Frame rotated around the reference Z axis
Frame rotated around the reference Y axis
Frame translated within the reference frame

(

2
z Rot
2
y Rot ] 7 3 4 [ Trans
t t
Relative Transformations
It is also possible to interpret the same move as:
Frame translated
Rotation 90
o
about the current Y axis
Rotation of 90
o
about the current Z axis.

The order of multiplication of the transformations is crucial to
obtaining the correct final position.
If we postmultiply a transform representing a frame by a second
transform describing a translation or rotation, then that translation or
rotation is made with respect to the frame described by the first
transformation
If we premultiply a frame transformation by a second transform
describing a translation or rotation, then that translation or rotation is
made with respect to the base co-ordinate frame.
Example
Execute the transformations left to right with respect to the
reference frame postmultiply
Execute the transformations right to left with respect to the
reference frame premultiply
Consider Trans[a,0,0]Rot(z,45
o
]



X
X
Y Y
Kinematic Equations
A manipulator consists of a selection of links and joints, each with its
own co-ordinate frame, and described by four parameters.
It is possible to state: [T
2
]=[A
1
][A
2
]
[A1] is the position and orientation of the first link with reference to the
manipulator's origin.
[A2] is the position and orientation of the second link relative to the
first link.
Hence for a multi-jointed manipulator,
[T
n
] = [A
1
] [A
2
].......[A
n
]
Transform Graph
A
1
0
T
6
Z
E
0
T
6
E
Origin
X
X
X
Z
Origin
Orientation Transformations
The matrix [T] has 16 elements, only 12 of which have any
real meaning.
No limit on the value of p
x
p
y
p
z
The l, m and n vectors must satisfy:
m.m = 1, n.n = 1, m.n = 0.
End effectors, two approaches:
RPY: roll, pitch and yaw
Euler angles
Roll, Pitch, Yaw
Movement around the fixed frame (premultiply)
Rotation around the x axis is defined as yaw
Rotation around the y axis is defined as pitch
Rotation around the z axis is defined as roll

| | | | | | | |
(
(
(
(
(

= =
1 0 0 0
0 c c s c s -
0 s c - c s s c c + s s s c s
0 s s - c s c c s - s s c c c
x Rot y Rot z Rot RPY
u u u
| u | | u | u |
| u | | u | u |
u | u | , , , , ,
Z: Roll
Y: Pitch
X: Yaw
Euler Angles
Rotation around the moving frame (postmultiply)
Rotation around the z axis
Rotation around the y axis
Rotation around the z axis

| | | | | | | |
(
(
(
(
(

+

= =
1 0 0 0
0 c s s c s -
0 s s c c + s c s s c c c s
0 s c c s - s c c s s c c c
z Rot y Rot z Rot Euler
u u u
u | | u | | u |
u | | u | | u |
u | u | , , , , ,
Specification of the [A] matrix
Z
n-1
X
n
d
n
a
n

n
Z
n
Joint n
Joint n+1
Link n-1
Link n
X
n-1
[A] matrix
Move sequence
Rotate around Z
n-1
by
n
Translate along Z
n-1
by d
n
Translate along X
n-1
by a
n
Rotate around X
n
by
n

(
(
(
(
(

o o
u o u o u u
u o u o u u
o u
1 0 0 0
d c s 0
as s c - c c s
ac s s c s - c
= ] ]Rot[x ]Trans[a00 ]Trans[00d Rot[z = ]
A
[
n
Application to a Manipulator
S
1
S
2
S
3
S
4
Joint 1
Joint 4
Joint 2
Joint 5
Joint 3
i 1 2 3 4 5
d
i
S
1
0 0 0 S
4
u
i

1

5

a
i
0 S
2
S
3
0 0
o
i
270 0 0 90 0
Solution.
| |
(
(
(
(
(

1 0 0 0
d
0 1 - 0
0
C
0
S
0
S
- 0
C
= A
1
1 1
1 1
1
(
(
(
(
(

1 0 0 0
0 1 0 0
S a
0
C S
C a
0
S
-
C
=
A
2 2 2 1
2 2 2 2
2
] [
(
(
(
(
(

1 0 0 0
0 1 0 0
S a
0
C S
C a
0
S
-
C
=
A
3 3 3 3
3 3 3 3
3
] [
(
(
(
(
(

1 0 0 0
0 0 1 - 0
0
C
0
S
0
S
- 0
C
=
A
4 4
4 4
4
] [
(
(
(
(
(

1 0 0 0
d
1 0 0
0 0
C S
0 0
S
-
C
=
A
5
5 5
5 5
5
] [
.
(
(
(
(
(

1 0 0 0
C d
-
S a
-
S a
-
d C
-
S S C S
-
)
S d
-
C a
+
C a
(
S S S
-
C C
-
C S
-
C C
-
C S
)
S d
-
C a
+
C a
(
C S C
-
C S
+
S C C
-
S S
+
C C
= ]
T
[
234 5 23 3 2 2 1 234 5 234 5 234
234 5 23 3 2 2 1 234 1 5 1 234 1 5 1 234 1
234 5 23 3 2 2 1 234 1 5 1 5 234 1 5 1 234 1
5
Where C
ijk
= Cos(q
i
+ q
j
+ q
k
)