1

1
Advanced Modeling of Robots
AMORO (EMARO)
MOROB (ARIA 2)
2012/2013
Wisama KHALIL
wisama.khalil@irccyn.ec-nantes.fr
Bureau 403 Bat. S
ECTS 4: (M2 emaro and ARIA)
2
Contents
2- Geometric and kinematic models of complex
chain robots
3- Geometric and kinematic modeling of parallel
robots
1- Dynamic modeling of serial robots
4- Dynamic Models of robots with complex
structure
2
Programme M2
.
3
Programme S3
4
3
5
Prerequisite (MOCOM-M1)
Terminology and general definitions
Transformation matrices
Direct geometric model of serial robots
Inverse geometric model of robots
Direct kinematic model
Inverse Kinematic model
Dynamic modeling
Trajectory generation
Control of Robots
MOCOM
Part 1(M1)
6
References
Course notes (W.Khalil)
W. Khalil, E. Dombre, Modeling, identification and control of robots, Hermes
Penton, London, 2002.
Further readings:
C.Canudas, B. Siciliano, G.Bastin (editors), Theory of Robot Control,
Springer-Verlag, 1996.
J . Angeles, Fundamentals of Robotic Mechanical Systems, Springer-Verlag,
New York, 2002.
Springer Handbook of Robotics, Siciliano, O.Khatib,edt, Verlag 2008.
4
7
Recall- MOCOM (M1)
Terminology and general definitions
Transformation matrices
Direct and Inverse geometric model of serial robots
Direct and inverse kinematic model
8
Chapter 1
Terminology and general definitions
Robot:
is a mechanical device possessing capacities of
perception, action, decision, learning, communication
and interaction with its environment, to realize tasks on
the place of the man or in interaction with him.
Robotics:
Robotics is a multidisciplinary science treating the
design and use of robots.
5
9
Types of Robots
The term robot covers a wide variety of mechanical
systems ranging from wheeled vehicles, legged
(walking) robots, flying or submarines, humanoid robots,
via articulated systems as the robot manipulators or
exoskeletons.
We will distinguish in our master:
Manipulators (serial, complexe, parallel),
Mobile Robots,
Humanoid Robots,
Bio-inspired Robots.
10
Types of robots
Industrial Manipulators
Mobile Robot
Mobile Manipulator
Drone
Walking (legged) Robot Underwater Robots
6
11
Legged (Walking) robots
Monopod (jumping) Humanoid (Honda)
SDR3X Sony:
2 processors RISC 64 bytes
24 dof
accelero 2 axes and gyro 2 axes
distance sensors IR
8 contact detectors
NAO- Aldebaran
12
7
13
Mechanical components of a robot
Manipulator
The mechanism of a robot manipulator consists of two subsystems;
the end-effector and an articulated mechanical structure:
End-effector (Tool), is a device intended to manipulate objects
(magnetic, electric or pneumatic grippers) or to transform them
(tools, welding torches, painting guns, etc.). It constitutes the
interface with which the robot interacts with its environment.
The articulated mechanical structure whose role is to place the end-
effector at a given location (position and orientation) (pose). The
mechanical structure is composed of a kinematic chain of articulated
rigid links. One end of the chain is fixed and is called the base. The
end-effector is fixed to the free extremity of the chain.
14
The Kinematic chain may be simple open chain
(serial) or complex (tree structured or closed).
8
15
Serial ROBOTS (Robots srie)
16
Closed loop Robots
9
17
Parallel Robots
Gough-Stewart
18
Type of joints (Articulation):
Revolute-rotational (rotode) joint ( R): limiting the
motion between two links (bodies) to a rotation about
a common axis. The relative location between the two
links is given by the angle about this axis.
Prismatic joint (P) (Prismatique): limiting the motion
between two links to a translation along a common
axis
-Complex joints(universal, cylindrical, spherical) with
more than one mobility are represented by a combination
of R and P joints
10
19
J oint variables (variables articulaires)
Revolute joint q=u Prismatic joint q=r
20
Definitions
Degrees of freedom of a body:
Is the number of independent components of its instantaneous
velocity (or elementary displacement) s 6.
J oint space (espace articulaire) (or configuration space-espace des
configurations) E(q):
The space in which the location of all the links of a robot are
represented.
q=[q1 q2 qN]
N gives the number of dof of the robot=number of independent
joints=number of joints for serial or tree structure robot.

11
21
Task space (operational space) E(X) (espace
oprationnel):
The space in which the location (situation) of
the end effector is represented: Cartesian
coordinates are used to specify the position in
93 and the rotation group SO(3) in case of 3D
motion.
X =[X1 X2 XM]
Ms 6 and Ms N
Transformation between E(q) and E(X)
Direct Geometric Model; X=DGM(q)
Inverse Geometric Model; q=IGM(X)
Direct and Inverse Kinematic models.
22
Redundancy (Redondance)
A robot is classified as redundant when the number of
degrees of freedom of its task space is less than the
number of degrees of freedom of its joint space M<N .
The following examples constitute redundant (redondant)
serial chain:
having more than six joints;
having more than three revolute joints whose axes
intersect at a point;
having more than three revolute joints with parallel
axes;
having more than three prismatic joints;
having prismatic joints with parallel axes;
having revolute joints with collinear axes.
12
23
Singular configurations (configurations
singulires)
at these configurations the number of degrees of
freedom of the end-effector becomes less than
the dimension of the task space. For example,
this may occur when:
the axes of two prismatic joints become
parallel;
the axes of two revolute joints become
collinear;
24
1.4. Number of degrees of freedom of a
robot
To place the end-effector (tool) arbitrarily in the
3D space 6 dof are required n=6.
To place the end-effector in a plane 3 dof are
required n=3, possibilty one more to release.
To place an axis in space 5 dof are sufficient
(welding torch)
To be chosen in terms of the task
Increasing n more expensive but easy adaptable
for other task.
13
25
1.5. Architectures of robot manipulators
Considering: revolute or prismatic joints whose
consecutive axes are either parallel or perpendicular
gives more than 3000 possible structures.
In general the structure is decomposed into shoulder
(porteur) (first three joints and links) and wrist (Poignet)
the other joints (one, two or three).
Shoulder Architectures (porteurs): For each joint either R
or P and the angle between consecutive axes are either
0 or 90 after canceling redundant structure with M<3,
we obtain 12 distinct (mathematically different)
architectures with three degrees of freedom.
26
.
RRR RRP
RPR
RPP PRR PPR PPP
Notes: -the three RRR structures can de detailed as: R-p/2-R-0-R, R-p/2-R-t/2, and R-0-R-t/2-R
- Taking into account if revolute consecutive perpendicular axes are intersecting or not will increase
the number of structures. For instance RRR with intersection axis defines a spherical joint (wrist),
whereas if the consecutive joint axes are not intersecting gives (orthogonal shoulder)
14
27
Industrial structures
Only five structures are manufactured:
- anthropomorphic (anthropomorphes) shoulder represented by the first
RRR structure, like PUMA from Unimation, Acma SR400, Stabli
(RX series), etc.;
spherical shoulder RRP: "Stanford manipulator" and Unimation
robots (Series 1000, 2000, 4000);
The RPR structure (also PRR-RRP) with parallel axes. Adding a
wrist with one revolute degree of freedom of rotation gives the
SCARA (Selective Compliance Assembly Robot Arm). It has been
manufactured by many companies: IBM, Bosch, Adept, etc.;
cylindrical shoulder RPP;
Cartesian shoulder PPP.
28
Anthropomorphic Robots
Stubli RX90 RRR Structure
15
29
Spherical Robot
Unimate (1961)
(First Industrial robot)
RRP Structure
30
Robot SCARA
(Selective Compliance Assembly Robot Arm)
Yamaha YK700X Structure RRRP / RRPR or RPRR
16
31
Cylindrical Robot
RPP shoulder Acma TH8 Robot
32
Cartesian Robot
PPP shoulder
17
33
The wrist (Poignet)
.
One-axiswrist
Twointersecting-axiswrist
Twononintersecting-axiswrist
Threeintersecting-axiswrist (spherical wrist)
Threenonintersecting-axiswrist
34
Classical 6 dof robots
(Shoulder and wrist of 6 dof robots)
.
P
shoulder wrist
- The first three dof place the center of the wrist in space, then the
wrist axes are determined in terms of the desired orientation.
- IGM can be obtained analytically.
18
Orthogonal Shoulder (RRR)
It is a RRR structure with orthogonal and non
intersecting axes.
35
36
Chapter 2
Transformation matrix between vectors,
frames
(Matrices de transformation
homognes)
19
37
2.1. Introduction
compute the location, position and orientation of
robot links relative to each other;
define the position and orientation of objects;
specify the trajectory and velocity of the end-
effector of a robot for a desired task;
describe and control the forces when the robot
interacts with its environment;
implement sensory-based control using
information provided by various sensors, each
having its own reference frame.
38
Homogenoues coordinates (coordonnes):
The homogeneous coordinates of a point with
respect to frame are defined by
w.Px, w.Py, w.Pz, w,
where w is a scaling factor.
Px, Py, Pz are the Cartesian coordinates.
In robotics we take w=1.
2.2. Homogeneous coordinates
20
39
2.2. Homogeneous coordinates
2.2.1. Representation of a point
w.r.t. a frame R
i
=(O
i
,x
i
, y
i,
z
i,
)
x
y
z
P
P
i i
i
P
i
i
( )
i
1
(
(
(
= =
(
(
(

P O P
z
i
x
i
y
i
P
P
x
P
z
P
y
R
i
40
2.2.2. Representation of a direction (free vector)
Let the unit vector along the direction be u
x
y
z
u
u
i
u
i
i
i
0
(
(
(
=
(
(
(

u

u

z
i
x
i
y
i

R
i
The same relation is used for a vector joining two points P
1
P
2
21
41
2.3. Homogeneous transformations
The transf., trans. and/or rot., of a frame Ri into frame Rj
is represented by the (4x4) hom. Trans. matrix
i
T
j
:
j j
j j j j
i
j
i i
i i i i
0 0 0 1
(
(
= =
(
(

(

R P
s n a P
T
i
T
j
x
i
x
j
y
j
z
j
R
i
R
j
z
i
y
i
j j j j
i i i i
0 0 0 1
(
(
(

s n a P
=
=
42
The matrix
i
T
j
represents the transformation from
frame R
i
to frame R
j
;
The matrix
i
T
j
can be interpreted as representing
the frame Rj (three orthogonal axes and an
origin) with respect to frame R
i
.
22
43
2.3.2. Transformation of vectors
A point or direction vector
j
V expressed in frame
i, can be expressed in frame j by the relation:

j
V =
j
T
i
i
V
For a point vecor:

i
(O
i
P) =
i
P
j
+
i
(O
j
P)
=
i
P
j
+
i
R
j
j
(O
j
P)
=
i
T
j
j
P
x
i
y
j
z
j
R
i
R
j
z
i
y
i
i
T
j
P
x
j
O
i
44
Example 2.1.
Find
i
T
j
and
j
T
i
from Figure.
i
j
0 0 1 3
0 1 0 12
1 0 0 6
0 0 0 1
(
(
(
=
(
(


T
j
i
0 0 1 6
0 1 0 12
1 0 0 3
0 0 0 1
(
(

(
=
(
(

T
6
3
12
z
i
x
j
x
i
z
j
y
i
y
j
23
45
2.3.4. Transformation matrix of a pure
translation
Trans(a, b, c) =
Trans(a,b,c)=Trans(x,a)Trans(y,b)Trans(z,c)
1 0 0 a
0 1 0 b
0 0 1 c
0 0 0 1
(
(
(
(
(
(

c
a
y
j
z
i
y
i
x
j
x
i
b
z
j
46
2.3.5. Transformation matrices of a rotation about the
principle axes
.
x
i
y
i
x
j
s
j
n
j
u
u
u
y
j
z
j
z
i
a
j i
j
1 0 0 0
0 C S 0
(x, ) =
0 S C 0
0 0 0 1
(
(

(
=
(
(

T Rot
u u
u
u u
0
( , ) 0
0
0 0 0 1
(
(
(
(
(
(

rot x u
24
47
.
i
j
C 0 S 0 0
0 1 0 0 ( , ) 0
= (y, ) = =
S 0 C 0 0
0 0 0 1 0 0 0 1
u u
u
u
u u
( (
( (
( (
( (
( (

rot y
T Rot
i
j
C S 0 0 0
S C 0 0 ( , ) 0
= (z, )= =
0 0 1 0 0
0 0 0 1 0 0 0 1
u u
u u u
u
( (
( (
( (
( (
( (

rot z
T Rot
Similarly:
48
2.3.6. Properties of homogeneous transformation
matrices
a) General form:
R represents the rotation, with three independent elements,
P represents the translation.
b) The matrix R is orthogonal,
R
-1
= R
T
c) The inverse of a matrix
i
T
j
defines the matrix
j
T
i
.
i
T
j
-1
=
j
T
i
x x x x
y y y y
z z z z
s n a P
s n a P
=
0 0 0 1 s n a P
0 0 0 1
(
(
(
(
=
(
(

(
(

R P
T
25
49
d) We verify that:
Rot
-1
(u, u) = Rot(u, -u) = Rot(u, u)
Trans
-1
(u, d) = Trans(u, d) = Trans(u, d)
e) The inverse of a transformation matrix:
T
T T T T
-1
T
=
0 0 0 1
0 0 0 1
(

(
( (
=
( (
(
(
(

s P
R n P R R P
T
a P
1 2 1 2 1
0 0 0 1
+ (
(

R R R P P
1 1 2 2
1 2
=
0 0 0 1 0 0 0 1
( (
( (

R P R P
T T
=
f)
T
1
T
2
= T
2
T
1
50
g) If a frame R
0
is subjected to k consecutive
transformations (i =1, , k), defined with
respect to the current frame R
i-1
, then:
0
T
k
=
0
T
1
1
T
2
2
T
3

k-1
T
k
1
T
2
z
1
z
0
k-1
T
k
x
0
x
1
y
1
x
2 y
2
z
2
x
k-1
z
k-1
y
k
z
k
0
T
1
0
T
k
R
0
R
1
R
2
R
k
y
0
y
k-1
x
k
26
51
h) If a frame R
j
, defined by
i
T
j
, undergoes a
transformation T defined relative to frame R
i
,
then R
j
will be transformed into R
j
' with
i
T
j
' =T
i
T
j
x
i
y
i
z
i
x
j
y
j
z
j
T
i
T
j
=
i
T
j'
R
i
R
j
z
j'
y
j'
x
j'
R
i'
z
i'
y
i'
x
i'
R
j'
T
i'
T
j'
=
i
T
j
i
T
j
g and h are the property of multiplication on the right or on the left.
52
i) Consecutive transformations about the same
axis:
Rot(u, u1) Rot(u, u2) = Rot(u, (u1 +u2))
Rot(u, u) Trans(u, d) = Trans(u, d) Rot(u, u)
j) Decomposition of a transformation matrix.
Note the order
3
=
0 0 0 1 0 0 0 1 0 0 0 1
( ( (
=
( ( (

R P I P R 0
T
27
53
2.3.7. Transformation matrix of a rotation
about a general axis located at the origin
Rot (u, u)=
z y
z x
y x
0 u u
u 0 u
u u 0
(
(
=
(
(

u
54
2.3.8. Equivalent angle and axis for a
rotation
Solving the equation:
x x x
y y y
z z z
s n a 0
s n a 0
s n a 0
0 0 0 1
(
(
(
=
(
(
(

T
Rot(u,u) =
From the diagonal elements we obtain:
Cu =(1/2 )(sx +ny +az 1)
From the off-diagonal elements we obtain:
x z y
y x z
z y x
2u S n a
2u S a s
2u S s n

u
u
u
2 2 2
z y x z y x
1
S (n a ) (a s ) (s n )
2
u = + +
Thus
28
55
When Su is small, the elements u
x
, u
y
and u
z
cannot be determined with good accuracy by
this equation. However, if Cu <0, we obtain
them more accurately using the diagonal terms
of Rot(u, u) as follows:
y
x z
x y z
n C
s C a C
u ,u ,u
1 C 1 C 1 C
u
u u
u u u

= = =

x
x z y
y
y x z
z
z y x
s C
u sign(n a )
1 C
n C
u sign(a s )
1 C
a C
u sign(s n )
1 C

u
u
u
u
u
u
56
Chapter 3
Direct geometric model of serial robots
(Modle gomtrique direct)
29
57
Direct geometric model of 2R robot:
Px =L1.cos (q1)+L2 cos(q1+q2)
Py =L1.sin (q1)+L2 sin(q1+q2)
The aim of this chapter is to present a general method
for more complicated structures.
Modified Denavit and Hartenberg method
[Khalil&Kleinfinger 86]
Exemple
x
y
E
L1
L2
58
3.2. Description of the geometry of serial
robots
.
n joints,
n+1 links (body): Link 0, , link n,
J oint j connects link j-1 with link j,
Link 0 is the fixed base, the tool is fixed on link n
rigid links, ideal revolute or prismatic joints (no backlash,
no elasticity),
Link
1
Link 0
Link
2
Link
3
Link
n
30
59
we assign a frame Rj to each link j, such that:
- zj axis is along the axis of joint j;
- xj is along the common normal between zj and
zj+1:
- yj axis is formed by the right-hand rule.
- Frame Rj is defined w.r.t R
j-1
using 4 parameters:
- dj, oj, rj, uj.
z
j-1
x
j-1
z
j
x
j
d
j
r
j
u
j
o
j
O
j
O
j-1
60
The variable of joint j is:
oj =0 if joint j is revolute; oj =1 if j is prismatic;
j-1
T
j
= Rot(x, oj) Trans(x, dj) Rot(z, uj) Trans(z, rj)
j j j j j
q = + r o u o
1
j j
= o o
j j j
j j j j j j j
j j j j j j j
C S 0 d
C S C C S r S
S S S C C r C
0 0 0 1
u u (
(
o u o u o o
(
=
(
o u o u o o
(
(

j-1
R
j
= rot(x, o
j
) rot(z, u
j
)
31
61

j
T
j-1
=Trans(z,rj) Rot(z,uj)Trans(x,dj)Rot(x ,oj)
T
j
j j
j j
j
d C
j 1 d S
r
0 0 0 1
(
(

(
=
(
(
(
(

R
u
u
j
R
j-1
= rot(z, -u
j
) rot(x, -o
j
)
62
Notes:
- Definition of frame R
0:
R0 is chosen to be aligned with frame R
1
when q
1
=0:
this makes o
1
=0, d
1
=0 and (r
1
or u
1
=0);
Definition of frame R
n:
choosing x
n
aligned with x
n-1
when q
n
=0 makes
(r
n
or un =0);
if joint j is prismatic, the zj axis must be taken to be parallel to the
joint axis but can have any position. We place it such that d
j
=0 or
dj+1 =0;
assuming that each joint is driven by an independent actuator, the
vector of joint variables q can be obtained from the encoder
readings qc:
q = K qc +q0
where K is an (nn) constant matrix and q0 is an offset representing
the robot configuration q when qc =0;
32
63
If z
j+1
is parallel to z
j

j-1
T
j+1
=
j-1
T
j
j
T
j+1
= Rot(x, o
j
) Trans(x, d
j
) Rot(z, uj) Trans(z, r
j
)
Trans(x, d
j+1
) Rot(z, u
j+1
) Trans(z, r
j+1
)
j
j j 1 j j 1 j j 1 j
j j j 1 j j j 1 j 1 j j j j 1 j
j j j 1 j j j 1 j j 1 j j j j 1 j
C( ) S( ) 0 d d C
C S( ) C C( ) S d C S (r r )S
S S( ) S C( ) C d S S (r r )C
0 0 0 1
+ + +
+ + + +
+ + + +
u + u u + u + u (
(
o u + u o u + u o o u + o
(
=
(
o u + u o u + u o o u + + o
(
(

This relation can be applied recursively if there is more
parallel axis, Example find
j-1
T
j+2
64
Example 3.1. Geometric description of the
Stubli RX-90 robot
x
3
x
4
, x
5
, x
6
z
6
z
3
z
2
z
0
, z
1
z
5
x
0
, x
1
, x
2
D3
RL4
z
4
33
65
Stubli RX-90 robot
66
We can add a column q giving the joint values at the current
configuration. In the previous figure all qj =0.
34
67
Example 3.2. Geometric description of a SCARA robot
It has four parallel axes:
z
0
, z
1
x
0
, x
1
z
2
z
3
, z
4
x
2
x
3
, x
4
D2
D3
68
3.3. Direct geometric model
The Direct Geometric Model (DGM) gives the location of
the end-effector of the robot in terms of joint coordinates.
It is represented by the transformation matrix
0
T
n
as:

0
T
n
=
0
T
1
(q
1
)
1
T
2
(q
2
)
n-1
T
n
(q
n
)
It can also be represented by the relation:
X = f(q) , or X = DGM(q)
The joint position values
q = [q
1
q
2
q
n
]
T
The position and orientation of the terminal link
X = [x
1
x
2
x
m
]
T
35
69
There are several possibilities of defining X :
- elements of
0
T
n
X=[Px Py Pz sx sy sz nx ny nz ax ay az]
T
Taking into account that s =n x a, we can also
take:
X = [Px Py Pz nx ny nz ax ay az]
T
In general for the position coordinaates all the
robot manufacturers take Px, Py, Pz. Whereas
different solutions are taken for the orientation
coordinates.
70
Example 3.3. Direct geometric model of the
Stubli RX-90 robot
From the geometric parameters table, row by
row, we write the elementary transformation
matrices
j-1
T
j
as:
0
1
C1 S1 0 0
S1 C1 0 0
0 0 1 0
0 0 0 1
(
(
(
=
(
(

T
1
2
C2 S2 0 0
0 0 1 0
S2 C2 0 0
0 0 0 1
(
(

(
=
(
(

T
3
C3 S3 0 D3
S3 C3 0 0
0 0 1 0
0 0 0 1
(
(
(
=
(
(

2
T
1
3
C23 S23 0 C2D3
0 0 1 0
T =
S23 C23 0 S2D3
0 0 0 1
(
(

(
(
(

Axis 2 and 3 are parallel:
C23=cos(q2+q3)
36
71
Similarly:
When miltiplying the elementary matrices, in
order to compute
0
T
6
, it is better to multiply
themfrom right to left:
the intermediate matrices
j
T
6
, denoted as Uj
will be used in the IGM
- this reduces the number of operations
(additions and multiplications) of the model.
3
4
C4 S4 0 0
0 0 1 RL4
=
S4 C4 0 0
0 0 0 1
(
(
(
(
(

T
4
5
C5 S5 0 0
0 0 1 0
=
S5 C5 0 0
0 0 0 1
(
(

(
(
(

T
5
6
C6 S6 0 0
0 0 1 0
=
S6 C6 0 0
0 0 0 1
(
(
(
(
(

T
72
.
0 0
0 6 1 1
= = U T T U
sx =C1(C23(C4C5C6 S4S6) S23S5C6) S1(S4C5C6 +C4S6)
sy =S1(C23(C4C5C6 S4S6) S23S5C6) +C1(S4C5C6 +C4S6)
sz =S23(C4C5C6 S4S6) +C23S5C6
nx =C1( C23 (C4C5S6 +S4C6) +S23S5S6) +S1(S4C5S6 C4C6)
ny =S1( C23 (C4C5S6 +S4C6) +S23S5S6) C1(S4C5S6 C4C6)
nz = S23(C4C5S6 +S4C6) C23S5S6
ax = C1(C23C4S5 +S23C5) +S1S4S5
ay = S1(C23C4S5 +S23C5) C1S4S5
az = S23C4S5 +C23C5
Px = C1(S23 RL4 C2D3)
Py = S1(S23 RL4 C2D3)
Pz =C23 RL4 +S2D3
37
73
3.5. Transformation matrix of the end-
effector in the world frame
Let the matrix Z =
f
T
0
, E =
n
T
E
R
n
R
E
R
0
R
f

0
T
n
0
T
E

E
Z
f
T
E
= Z
0
T
n
(q) E
In most programming languages, the user can specify Z (base)
and E (Tool).
74
3.6. Specification of the orientation
Cosinus directors (rotation matrix) (9
parameters):
n n n
0
n
= 0 0 0
(

s n a
R
Other methods:
3.6.1. Euler angles (3 parameters)
3.6.2. Roll-Pitch-Yaw angles (Rollis-Tangage- Lacet)(3 par.)
3.6.3. Quaternions (4 parameters)
38
75
3.6.1. Euler angles (3 parameters: |, u, )
u

u
O
y
n
|
N
x
0
x
n
y
0
z
0
z
n
|

|: angle between x0 and ON about z0;

u : angle between z0 and zn about ON;
: angle between ON and xn about z0;
0
R
n
= rot(z, |) rot(x, u) rot(z, )
C C S C S C S S C C S S
S C C C S S S C C C C S
S S S C C
| | u | | u | u (
(
= | + | u | + | u | u
(
( u u u

76
Inverse solution of E-A
rot(z, |)
0
R
n
= rot(x, u)rot(z,)
x y x y x y
x y x y x y
z z z
C s S s C n S n C a S a
C S 0
S s C s S n C n S a C a C S C C S
S S S C C
s n a
| + | | + | | + | (
(
(
(
| + | | + | | + | = u u u
(
(
(
( u u u


Using (1,3) elements:
1 x y
2 x y 1
atan2( a ,a )
atan2(a , a )
| =

| = = | + t

when ax =0, ay =0, az =1 | is undefined (description singularity).

Using the (2, 3) and (3, 3) elements
u = atan2(S| a
x
C| a
y
, a
z
)
Then
= atan2( C| nx S| ny, C| sx +S| sy)
39
77
3.6.2. Roll-Pitch-Yaw angles (3 parameters: |, u, )
0
R
n
=rot(z, |) rot(y, u) rot(x, )
C C C S S S C C S C S S
S C S S S C C S S C C S
S C S C C
| u | u | | u + | (
(
= | u | u + | | u |
(
( u u u

u

|
x
0
x
0
'
x
0
'', x
n
y
0
y
0
', y
0
''
y
n
z
0
, z
0
'
z
0
''
z
n
u

|
u
|

78
Inverse problem of R-P-Y:
rot(z, |)
0
R
n
= rot(y, u)rot(x,)
x y x y x y
x y x y x y
z z z
C s S s C n S n C a S a
C S S S C
S s C s S n C n S a C a 0 C S
S C S C C
s n a
+ | | | + | | + | (
u u u (
(
(
| + | | + | | + | =
(
(
(
( u u u


1 y x
2 y x 1
atan2(s ,s )
atan2( s , s )
| =

| = = | + t

u = atan2( sz, C| sx+S| s)

= atan2(S|ax C| ay, S| nx +C| ny)

Two solutions, description with singularity when sx=sy=0,

axis xn along =z0
40
79
3.6.3. Quaternions
1
2 x
3 y
4 z
Q C( / 2)
Q u S( / 2)
Q u S( / 2)
Q u S( / 2)
= u

= u

= u

= u

x
n
x
0
y
0
y
n
u
z
0
z
n
u
2 2 2 2
1 2 3 4
Q +Q +Q +Q = 1
2 2
1 2 2 3 1 4 2 4 1 3
0 2 2
n 2 3 1 4 1 3 3 4 1 2
2 2
2 4 1 3 3 4 1 2 1 4
2(Q Q ) 1 2(Q Q Q Q ) 2(Q Q Q Q )
= 2(Q Q Q Q ) 2(Q Q ) 1 2(Q Q Q Q )
2(Q Q Q Q ) 2(Q Q Q Q ) 2(Q Q ) 1
(
+ +
(
(
+ +
(
( + +

R
80
Quaterion inverse problem
1 x y z
1
Q s n a 1
2
= + + +
2 z y x y z
1
Q = sign (n a ) s n a 1
2
+
3 x z x y z
1
Q = sign (a s ) s n a 1
2
+ +
4 y x x y z
1
Q = sign (s n ) s n a 1
2
+ +
No description singularity, simple transformation relations,
One solution.
41
Reprsentation by a vector-angle
Vector-angle: A vector U with four components:
Three for components of u,the fourth is u.
See Matlab function:
U=vrrotmat2vec(R)
81