Sie sind auf Seite 1von 39

# Kinematics of

Robot
Manipulator
1
Examples of Kinematics
Calculations
Forward kinematics

Given joint variables

End-effector position and orientation,
-Formula?
) , , , , , , (
6 5 4 3 2 1 n
q q q q q q q q =
) , , , , , ( T A O z y x Y =
x
y
z
Examples of Inverse Kinematics
Inverse kinematics
End effector position
and orientation

Joint variables -Formula?
3
) , , , , , , (
6 5 4 3 2 1 n
q q q q q q q q =
) , , , , , ( T A O z y x
x
y
z
Example 1: one rotational
4
0
x
0
y
1
x
1
y
) / ( cos
kinematics Inverse
sin
cos
kinematics Forward
0
1
0
0
l x
l y
l x

=
=
=
u
u
u
u
l
Robot Reference Frames
World frame
Joint frame
Tool frame
5
x
y
z
x
z
y
W
R
P
T
Coordinate Transformation
Reference coordinate frame OXYZ
Body-attached frame Ouvw
w v u
k j i
w v u uvw
p p p P + + =

6
z y x
k j i
z y x xyz
p p p P + + =

x
y
z
P
u
v
w
O, O
Point represented in OXYZ:
z w y v x u
p p p p p p = = =
T
z y x xyz
p p p P ] , , [ =
Point represented in Ouvw:
Two frames coincide ==>
Properties: Dot Product
Mutually perpendicular

Unit vectors
7
Properties of orthonormal coordinate frame
0
0
0
=
=
=
j k
k i
j i

1 | |
1 | |
1 | |
=
=
=
k
j
i

Let and be arbitrary vectors in and be
the angle from to , then

3
R
u
u cos y x y x =
x
y
x
y
Coordinate Transformation
Coordinate Transformation
Rotation only
w v u
k j i
w v u uvw
p p p P + + =

8
x
y
z
P
z y x
k j i
z y x xyz
p p p P + + =

uvw xyz
RP P =
u
v
w
How to relate the coordinate in these two frames?
Basic Rotation
Basic Rotation
, , and represent the projections of onto
OX, OY, OZ axes, respectively

Since

9
x
p
P
y
p
z
p
w v u x
p p p P p
w x v x u x x
k i j i i i i + + = =
w v u y
p p p P p
w y v y u y y
k j j j i j j + + = =
w v u z
p p p P p
w z v z u z z
k k j k i k k + + = =
w v u
k j i
w v u
p p p P + + =
Basic Rotation Matrix
Basic Rotation Matrix

10
(
(
(

(
(
(

=
(
(
(

w
v
u
z
y
x
p
p
p
p
p
p
w z v z u z
w y v y u y
w x v x u x
k k j k i k
k j j j i j
k i j i i i
x
z
y
v
w
P
u
u
(
(
(

=
u u
u u u
C S
S C x Rot
0
0
0 0 1
) , (
u
Is it True? Can we check?

11
u u
u u
u u
u u
cos sin
sin cos
cos sin 0
sin cos 0
0 0 1
w v z
w v y
u x
w
v
u
z
y
x
p p p
p p p
p p
p
p
p
p
p
p
+ =
=
=
(
(
(

(
(
(

=
(
(
(

x
z
y
v
w
P
u
u
u
Basic Rotation Matrices

12
uvw xyz
RP P =
(
(
(

=
u u
u u u
C S
S C x Rot
0
0
0 0 1
) , (

0
0 1 0
0
) , (
(
(
(

=
u u
u u
u
C S
S C
y Rot
(
(
(

=
1 0 0
0
0
) , ( u u
u u
u C S
S C
z Rot
u
u
u
Matrix notation for rotations
Basic Rotation Matrix

Obtain the coordinate of from the coordinate of
(
(
(

(
(
(

=
(
(
(

z
y
x
w
v
u
p
p
p
p
p
p
z w y w x w
z v y v x v
z u y u x u
k k j k i k
k j j j i j
k i j i i i
13
uvw xyz
RP P =
(
(
(

=
w z v z u z
w y v y u y
w x v x u x
k k j k i k
k j j j i j
k i j i i i
R
xyz uvw
QP P =
T
R R Q = =
1
3
1
I R R R R QR
T
= = =

uvw
P
xyz
P
<== 3X3 identity matrix
Dot products are commutative!
Example 2: rotation of a point in a rotating
frame
A point is attached to a rotating frame, the
frame rotates 60 degree about the OZ axis of the
reference frame.

Find the coordinates of the point relative to the
reference frame after the rotation.

14
) 2 , 3 , 4 ( =
uvw
a
(
(
(

=
(
(
(

(
(
(

=
=
2
964 . 4
598 . 0
2
3
4
1 0 0
0 5 . 0 866 . 0
0 866 . 0 5 . 0
) 60 , (
uvw xyz
a z Rot a
Example 3: find a point of rotated system
for point in coordinate system
A point is the coordinate w.r.t. the
reference coordinate system, find the
corresponding point w.r.t. the rotated OU-V-
W coordinate system if it has been rotated 60
) 2 , 3 , 4 ( =
xyz
a
uvw
a
(
(
(

=
(
(
(

(
(
(

=
=
2
964 . 1
598 . 4
2
3
4
1 0 0
0 5 . 0 866 . 0
0 866 . 0 5 . 0
) 60 , (
xyz
T
uvw
a z Rot a
Reference coordinate frame OXYZ
Body-attached frame Ouvw
Composite Rotation Matrix
A sequence of finite rotations
matrix multiplications do not commute
rules:
if rotating coordinate O-U-V-W is rotating about
principal axis of OXYZ frame, then Pre-multiply the
previous (resultant) rotation matrix with an
appropriate basic rotation matrix

if rotating coordinate OUVW is rotating about its own
principal axes, then post-multiply the previous
(resultant) rotation matrix with an appropriate basic
rotation matrix
16
Example 4: finding a rotation matrix
Find the rotation matrix for the following
operations:

17
Post-multiply if rotate about the OUVW axes
Pre-multiply if rotate about the OXYZ axes
...
o
u
|
(
(
(

+

+
=
(
(
(

(
(
(

(
(
(

=
=
o u | o | o | o u | u |
o u o u u
o | o u | o u | o | u |
o o
o o u u
u u
| |
| |
o u |
S S S C C S C C S S C S
S C C C S
C S S S C C S C S S C C
C S
S C C S
S C
u Rot w Rot I y Rot R
0
0
0 0 1
1 0 0
0
0
C 0 S -
0 1 0
S 0 C
) , ( ) , ( ) , (
3
Reference coordinate frame OXYZ
Body-attached frame Ouvw
Coordinate Transformations
18
position vector of P
in {B} is transformed
to position vector of P
in {A}

description of {B} as
seen from an observer
in {A}

Rotation of {B} with respect to {A}
Translation of the origin of {B} with respect to origin of {A}
Coordinate Transformations
Two Special Cases

1. Translation only
Axes of {B} and {A} are
parallel

2. Rotation only
Origins of {B} and {A} are
coincident

19
1 =
B
A
R
' o A P B
B
A P A
r r R r + =
0
'
=
o A
r
Homogeneous Representation
20
Coordinate transformation from {B} to {A}

Homogeneous transformation matrix
' o A P B
B
A P A
r r R r + =
(

=
(

1 1 0 1
3 1
' P B o A
B
A P A
r r R r
(

=
(

1 0
1 0
1 3 3 3
3 1
'
P R
r R
T
o A
B
A
B
A
Position
vector
Rotation
matrix
Scaling
Homogeneous Transformation
Special cases
1. Translation

2. Rotation
21
(

1 0
0
3 1
1 3 B
A
B
A
R
T
(

1 0
3 1
'
3 3
o A
B
A
r I
T
Example 5: translation
Translation along Z-axis with h:

22
(
(
(
(

=
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
) , (
h
h z Trans
(
(
(
(

+
=
(
(
(
(

(
(
(
(

=
(
(
(
(

1 1 1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1
h p
p
p
p
p
p
h z
y
x
w
v
u
w
v
u
x
y
z
P
u
v
w
O, O
h
x
y
z
P
u
v
w
O, O
Example 6: rotation

(
(
(
(

(
(
(
(

=
(
(
(
(

1 1 0 0 0
0 0
0 0
0 0 0 1
1
w
v
u
p
p
p
C S
S C
z
y
x
u u
u u
23
(
(
(
(

=
1 0 0 0
0 0
0 0
0 0 0 1
) , (
u u
u u
u
C S
S C
x Rot
x
z
y
v
w
P
u
Homogeneous Transformation
Composite Homogeneous Transformation
Matrix

Rules:
Transformation (rotation/translation) w.r.t (X,Y,Z)
(OLD FRAME), using pre-multiplication

Transformation (rotation/translation) w.r.t (U,V,W)
(NEW FRAME), using post-multiplication

24
Example 7: homogeneous
transformation
Find the homogeneous transformation matrix (T)
for the following operations:

4 4 , , , ,
= I T T T T T
x a x d z z o u
:
axis OZ along d of n Translatio
axis OX along a of n Translatio
u
o
(
(
(
(

(
(
(
(

(
(
(
(

(
(
(
(

=
1 0 0 0
0 0
0 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0 1 0
0 0 1
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0
0 0
o o
o o u u
u u
C S
S C
a
d
C S
S C
Homogeneous Representation
A frame in space (Geometric
Interpretation)

x
y
z
) , , (
z y x
p p p P
(
(
(
(

=
1 0 0 0
z z z z
y y y y
x x x x
p a s n
p a s n
p a s n
F
n
s
a
(

=

1 0
1 3 3 3
P R
F
Principal axis n w.r.t. the reference coordinate system
(X)
(y)
(z)
Homogeneous Transformation
Translation
y
z
n
s
a
n
s
a
(
(
(
(

+
+
+
=
(
(
(
(

(
(
(
(

=
1 0 0 0

1 0 0 0 1 0 0 0
1 0 0
0 1 0
0 0 1
z z z z z
y y y y y
x x x x x
z z z z
y y y y
x x x x
z
y
x
new
d p a s n
d p a s n
d p a s n
p a s n
p a s n
p a s n
d
d
d
F
old z y x new
F d d d Trans F = ) , , (
Homogeneous Transformation
2
1
1
0
2
0
A A A =
i
i
A
1
28
Composite Homogeneous Transformation Matrix
0
x
0
z
0
y
1
0
A
2
1
A
1
x
1
z
1
y
2
x
2
z
2
y
?
Transformation matrix for
Chain product of successive
coordinate transformation matrices
Example 8: homogeneous transformation based on
geometry
For the figure shown below, find the 4x4 homogeneous transformation matrices
and for i=1, 2, 3, 4, 5
(
(
(
(

=
1 0 0 0
z z z z
y y y y
x x x x
p a s n
p a s n
p a s n
F
(
(
(
(

=
1 0 0 0
0 1 0
1 0 0
0 0 0 1
1
0
d a
c e
A
i
i
A
1
i
A
0
0
x
0
y
0
z
a
b
c
d
e
1
x
1
y
1
z
2
z
2
x
2
y
3
y
3
x
3
z
4
z
4
y
4
x
5
x
5
y
5
z
(
(
(
(

+

=
1 0 0 0
0 1 0 0
0 0 1
0 1 0
2
0
c e
b
A
(
(
(
(

=
1 0 0 0
0 0 0 1
1 0 0
0 1 0
2
1
d a
b
A
Can you find the answer by observation
based on the geometric interpretation of
homogeneous transformation matrix?
Orientation Representation
Rotation matrix representation needs 9 elements
to completely describe the orientation of a rotating
rigid body.
Any easy way?
(

=

1 0
1 3 3 3
P R
F
Euler Angles Representation
Orientation Representation
Euler Angles Representation ( , , )
Many different types
Description of Euler angle representations

| u

Euler Angle I Euler Angle II Roll-Pitch-Yaw

| |
|
u u u

## Euler Angle I, Animated

x
y
z
u'
v'
|
u
v "
w"
w'=
=u"

v'"

u'"
w'"=

Orientation Representation
Euler Angle I
|
|
|
.
|

\
|

=
|
|
|
.
|

\
|
=
|
|
|
.
|

\
|

=
1 0 0
0 cos sin
0 sin cos
,
cos sin 0
sin cos 0
0 0 1
,
1 0 0
0 cos sin
0 sin cos
' '
'

u u
u u | |
| |

u |
w
u z
R
R R
Euler Angle I

|
|
|
|
|
|
|
|
|
.
|

\
|

=
u u u
u |
u |
|
u |
|
u
u |
|
u |
|
u |
cos sin cos sin sin
sin cos
cos cos cos
sin sin
cos sin cos
cos sin
sin sin
cos cos sin
sin cos
cos sin sin
cos cos
' ' ' w u z
R R R R
Resultant eulerian rotation matrix:
Euler Angle II, Animated
35
x
y
z
u'
v'
|
u
=v"
w"
w'=
u"

v"'

u"'
w"'=
Note the opposite
(clockwise) sense of the
third rotation, |.
Orientation Representation
Matrix with Euler Angle II
36
|
|
|
|
|
|
|
|
|
.
|

\
|

u
u u
u
u |
|
u |
|
u |
u |
|
u |
|
cos
sin sin sin cos
sin sin
cos cos sin
cos cos
cos cos sin
sin cos
sin cos
cos cos sin
cos sin
cos cos cos
sin sin
Quiz: How to get this matrix ?
Orientation Representation
Description of Roll Pitch Yaw
X
Y
Z

u
|
Quiz: How to get rotation matrix ?
The City College of New York

38
The City College of New York
Kinematics of Robot Manipulator
Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
39
Introduction to ROBOTICS