Université de Sfax

Ecole Nationale National Engineering

d’Ingénieurs de Sfax School of Sfax

ROBOTIQUE

Document préparé

par

Professeur Slim Choura

 OUTLINE OF ROBOT DESIGN

Consider a two-link robot as shown below:

B
y

θ2 S

joint 2

joint 1 θ1
x
A

Attached to the end of the manipulator is a tool, such as cutting tool.

Suppose we wish to move the manipulator from its home position A to position B from which
point the robot is to follow the contour of the surface S at constant velocity while maintaining
a prescribed force F normal to the surface. The robot will cut or grind the surface according to
a prescribed specification.

Problem 1: Forward Kinematics

Describe both the position of the tool and the locations A and B with respect to a common
coordinate system. The angles θ 1 and θ 2 will be measured directly using internal sensors
(or encoders) located at joints 1 and 2.

Problem 2 : Inverse Kinematics

Given the joint angles, the end-effector coordinates x and y can be determined. The problem
of inverse kinematics consists of determining the joint angles in terms of x and y. Since the
forward kinematics equations are nonlinear there could be:

1
 a- one solution
position

b- two or more solutions

position

c- no solution Position
cannot be
reached

Problem 3: Velocity Kinematics

In order to follow a contour at constant velocity, or at any prescribed velocity, we must know
the relationship between the velocity of the tool and the joint velocities. This relationship is of
the form:
 x

 θ

  = R = Jθ
θ
=  1 


 y
 θ 2 

where J is known as the Jacobian of the manipulator. This implies:

θ
= J -1 R

The values θ 1 and θ 2 at which J is singular correspond to the singular configurations of the
manipulator.
Examples:

θ 2 =0
θ 2 =π

(a) (b)

2
For many applications it is important to plan manipulator motions in such a way that singular
configurations are avoided.

Problem 4: Dynamics

To control the position we must know how much force to exert on it to cause it to move.
Techniques based on Lagrangian dynamics are used to derive the equations of motion of the
manipulator.

Problem 5: Position Control

The motion control problem consists of the tracking and disturbance rejection problem, which
is the problem of determining the control inputs (torques) necessary to follow, or track, a
desired trajectory that has been planned for the manipulator. While simultaneously rejecting
disturbances due to unmodeled dynamic effects such as friction and noise.

3
 FORWARD KINEMATICS THE DENAVIT-
HARTENBERG REPRESENTATION
A study of the geometry of motion is essential in manipulator design and control in order to
obtain a mapping between the end-effector location (position and orientation) and the motion
of manipulator links as well as the mapping between the end effector velocity and the speed
of the manipulator links. The final goal is to use these mappings to relate the end-effector
motion to joint displacements (generalized coordinates) and velocities.

1. Kinematic Chains:

Suppose a robot has (n+1) links numbered from 0 to n starting from the base of the robot,
which is taken as a link 0. The joints are numbered from 1 to n, and the ith joint is the point in
space where links (i − 1) and i are connected. The ith joint variable is denoted by q i . In the

case of revolute joint, q i is the joint angle of rotation and in the case of prismatic joint, q i is
the joint displacement. Next, a coordinate frame is attached rigidly to each link. To be
specific, we attach an inertial frame to the base and call it frame 0. Then we choose frames 1
through n such that frame i is rigidly attached to link i.

Illustration: elbow manipulator

4
 2. Denavit-Hartenberg Representation

A commonly used convention for selecting frames of reference in robotic applications is the
Denavit-Hartenberg, or D-H convention which is based on the homogeneous transformation.
Each homogeneous transformation is represented as a product of four “basic” transformations.

A i = Rot z,θ + Tran z, d + Tran x, a + Rot x,α

i i i i

cosθi −sinθi 0 0  1 0 0 0 1 0 0 ai  1 0 0 0

    
0 0  0 1 0 0 0 1 0 0  0 cosα i −sinαi 0
= 
sinθi cosθi
   
 0 0 1 0  0 0 1 di   0 0 1 0  0 sinαi cosα i 0
  
 0 0 0 1  0 0 0 1   0 0 0 1  0 0 0 1

cosθi −sinθi cosα i sinθi sinα i ai cosθi 

 
 sinθi cosθi cosαi -cosθi sinαi ai sinθi 
=  (*)
 0 sinαi cosα i di 
 
 0 0 0 1 

where the four quantities θ i , ai , di and αi are parameters of link i and joint i called

ai : length of the link

θ i : angle
αi : twist angle
di : offset

Since Ai is a function of one variable, three of the above parameters are constant for a given
link, while the fourth parameter, θ i for a revolute joint and di for a prismatic joint, is the
joint variable.
Ai can be rewritten as:
 Ri p i 
Ai =  
0 1
R i : relative rotation of frame i with respect to frame (i − 1)

p i : position vector of the origin of frame i with respect to frame (i − 1) expressed in frame
(i − 1)

5
Procedure of the D-H Convention

Step 1: Locate and label the joint axes z0, z2, ..., zn−1 .

Step 2: Establish the base frame. Set the origin anywhere on the z0 - axis. The x0 and y0 axes
are chosen conveniently to form a right-hand frame.

For i = 1, 2, ..., n-1 perform steps 3 to 5.

Step 3: Locate the origin Oi where the common normal to zi and zi −1 intersects zi . If zi

intersects zi −1 locate Oi at this intersection. If zi and zi −1 are parallel, locate Oi at

joint i .
Step 4: Establish xi along the common normal between zi −1 and zi through Oi , or in the

direction normal to the zi −1 - zi plane if zi −1 and zi intersect.

Step 5: Establish yi to complete the right-hand frame.

Step 6: Establish the end-effector frame On xn yn zn . Assuming the nth joint is revolute, set

kn = a along the direction zn −1 . Establish the origin conveniently along zn , preferably

at the center of the gripper or at the tip of any tool that the manipulator may be
carrying. Set jn = s in the direction of the gripper closure and set in =n as s×a . If the

tool is not a simple gripper set xn and yn conveniently to form a right-hand frame.

Step 7: Create a table of link parameters a i , di , αi and θi .

a i : distance along x i from Oi to the intersection of x i and zi −1 axes.

6
 d i : distance along zi −1 from Oi −1 to the intersection of the x i and zi −1 axes. d i is a
variable if joint i is prismatic.
α i : the angle between zi −1 and zi measured about xi .
θi : the angle between xi −1 and xi measured about zi −1 . θi is a variable if joint i is
revolute.

Step 8: Form the homogeneous transformation matrices Ai by substituting the above

parameters in (*).
Step 9: Form
T0n = A1 A2 A3 ... An
which gives the position and orientation of the tool frame expressed in base
coordinates.

7
Example 1: Planar elbow manipulator

z0 and z1 are normal to the page.

Link ai αi di θi
1 a1 0 0 θ1* ( * ) variable
2 a2 0 0 θ 2*

cosθ1 −sinθ1 0 a1 cosθ1  cosθ 2 −sinθ 2 0 a2cosθ 2 

   
 sinθ1 cosθ1 0 a1 sinθ1   sinθ 2 cosθ 2 0 a2sinθ 2 
A1 =   A 2=  
 0 0 1 0   0 0 1 0 
 0 0 0 1   0 0 0 1 

cos θ1 + θ 2
( ) (
− sin θ1 + θ 2 ) 0 a1 cosθ1 + a2 cos θ1 + θ 2 
( )
 
 sin θ1 + θ 2
( ) (
cos θ1 + θ 2 ) 0 a1 sinθ1 + a2 sin θ1 + θ 2 
( )
T01 = A1 T02 = A1 A2 =  
 0 0 1 0 
 
 0 0 0 1 

Notice that the first rows of the last columns of T 02 are the x and y components of the

origin O2 in the base frame:

8
 x = a1 cosθ1 + a2 cos θ1 + θ 2 ( )
y = a1 sinθ1 + a2 sin θ1 + θ 2 ( )

Example 2: Three-link cylindrical robot

z0 z1 d3

O2 O3
z2 z3
z2
y2 y3
x2 d2 x3
z1

O1 y1

x1 d1
z0

O0 y0

Link ai αi di θi
1
2
3

cosθ1 −sinθ1 0 0 1 0 0
0 1 0 0 0
     
 sinθ1 cosθ1 0 0 0 0 1 0 0 1 0 0
A1 =   A 2=  A 3= 
0 -1 0 d 2  0 0 1 d3 
 0 0 1 d1     
  0 0 0 1  0 0 0 1 
 0 0 0 1

cosθ1 0 − sinθ1 − d3 sinθ1 

 0 
sinθ1 cosθ1 d3 cosθ1 
T03 = A1 A2 A3 = 
 0 −1 0 d1 + d 2 
 
 0 0 0 1 

9
Example 3: Spherical wrist

z3

x4 O
a
z5
z4

Link ai αi di θi
4
5
6

cosθ 4 0 −sinθ 4 0 cosθ 5 0 sinθ5 0 cosθ 6 −sinθ 6 0 0

     
 sinθ 4 0 cosθ 4 0  sinθ 5 0 −cosθ 5 0  sinθ 6 cosθ 6 0 0
A 4=  A 5=   A 6= 
 0 −1 0 0  0 1 0 0  0 0 1 d6 
 0  
0 0 1  0 0 0 1  0 0 0 1

c4c5c6 −s 4s6 −c4c5s6 −s4c6 c4s5 c4s5d6 

 
 R3
6 d6
3  s 4c5s6 +c4s6 −s4c5s6 +c4c6 s 4s5 s4s5d6 
T 63 = A 4 A 5 A 6 =  = 
 0 1   −s5c6 s5s6 c5 c5d6 
 
 0 0 0 1 
where ci =cosθi and si =sinθi

10
Example 4: Cylindrical manipulator with spherical wrist

Suppose we attach a spherical wrist to the cylindrical manipulator of Example 2 as shown

below. Note that axis of rotation of joint 4 is parallel to z2 and thus coincides with the axis z3
of Example 2. Thus

d3 θ5
s

θ4 θ6 n
d2

θ1

c1 0 −s1 −d3 s1  c4c5c6 −s 4s6 −c4c5s6 −s4c6 c4s5 c4s5d6   r11 r12 r13 dx 
    
s1 0 c1 d3 c1  s 4c5s6 +c4s6 −s4c5s6 +c4c6 s4s5 s4s5d6  r21 r22 r23 dy 
T 60 = T 30T 36 =   =  
0 −1 0 d1 + d 2   −s5c6 s5s6 c5 c5d6  r31 r32 r33 dz 
    
0 0 0 1  0 0 0 1  0 0 0 1

r11 = c1 c4c5c6 −c1s4s6 +s1s5c6 r13 = c1c4s5 −s1 c5

r21 = s1 c4c5c6 −s1s4s6 +c1s5c6 r23 = s1 c4s5 +c1 c5
r31 = −s 4c5c6 −c4s6 r33 = −s 4s5
r12 = −c1c4c5s6 −c1s 4c6 −s1s5s6 d x = c1 c4s5d6 −s1 c5d 6 −s1 d3
r22 = −s1 c4c5s6 −s1s 4c6 +c1s5s6 d y = s1 c4s5d6 +c1 c5d6 +c1 d3
r32 = s 4c5s6 −c4c6 d z = −s 4s5d6 + d1 + d 2

11
Example 5: Stanford manipulator

Link ai αi di θi
1
2
3
4
5
6

c1 0 −s1 0 c2 0 s2 0 1 0 0 0

     
s 0 s 2 0 −c 2 0 0 1 0 0
A 3= 
0 c1
A1 =  1  A 2=   0 0 1 d3 
0 −1 0 0 0 1 0 d2   
 0  
0 0 1 0 0 0 1 0 0 0 1 

c4 0 −s 4 0 c5 0 s5 0 c6 −s6 0 0

     
s 0 c4 0 s 0 −c5 0 s 6 c6 0 0
A 4= 4  A 5=  5  A 6=  
0 −1 0 0 0 1 0 0 0 0 1 d6 
 0  
0 0 1  0 0 0 1 0 0 0 1

T 60= A1 A 2 A 3 A 4 A 5 A 6

12
Example 6: SCARA manipulator

Link ai αi di θi
1
2
3
4

c1 −s1 0 a1 c1  c2 s2 0 a2c2  1 0 0 0 c4 −s4 0 0

       
s 0 a1 s1  s 0 a2s2  0 1 0 0 s 4 c4 0 0
A 3= 
c1 −c 2
A1 =  1  A 2=  2  A 4=  
0 0 1 d3 
0 0 0 0  0 0 −1 0    0 0 1 d4 
 0  
0 0 1   0 0 0 1  0 0 0 1  0 0 0 1

c12c4 +s12s 4 −c12s4 +s12c4 0 a1 c1 +a2c12 

 
s12c4 −c12s4 −s12s 4 −c12c4 0 a1 s1 +a2s12 
T 04 = A1 A 2 A 3 A 4 =  
 0 0 −1 −d3 −d 4 
 
 0 0 0 1 

13
 INVERSE KINEMATICS

Objective: Find the joint variables in terms of the end-effector position and orientation.

Given a 4× 4 homogeneous transformation matrix:

R d
H =
0 1
with R is the rotation matrix, find (one or all) solutions of the equation

T0n ( q1 , q2 , ..., qn ) = H (1)

where

T0n ( q1 , q2 , ..., qn ) = A1 A2 ... An

Equation (1) results in 12 nonlinear equations in n-unknown variables, which can be

written as

Tij ( q1 , q2 , ..., qn ) = hij i = 1, 2, 3 j = 1, 2, 3, 4

Example: Stanford Manipulator (see previous chapter)

Suppose that the proposed position and orientation of the final frame is given as:

 r11 r12 r13 dx 

 
r r22 r23 dy 
T0 =  21
6
r r32 r33 dz 
 31 
 0 0 0 1 

To find the corresponding variables θ1 , θ 2 , d3 , θ 4 , θ5 , and θ 6 we must solve the following

simultaneous set of nonlinear trigonometric equations:

14
r11 = c1 c2 ( c4 c5 c6 − s4 s6 ) − s2 s5 c6  − s1 ( s4 c5 c6 + c4 s6 )

r21 = s1 c2 ( c4 c5 c6 − s4 s6 ) − s2 s5 c6  + c1 ( s4 c5 c6 + c4 s6 )
r31 = − s2 ( c4 c5 c6 − s4 s6 ) − c2 s5 c6
r12 = c1  −c2 ( c4 c5 s6 + s4 s6 ) + s2 s5 s6  − s1 ( − s4 c5 s6 + c4 c6 )
r22 = s1  −c2 ( c4 c5 s6 + s4 s6 ) + s2 s5 s6  + c1 ( − s4 c5 s6 + c4 c6 )
r32 = s2 ( c4 c5 s6 + s4 c6 ) + c2 s5 s6
r13 = c1 ( c2 c4 s5 + s2 c5 ) − s1 s4 s5
r23 = s1 ( c2 c4 s5 + s2 c5 ) + c1 s4 s5
r33 = − s2 c4 s5 + c2 c5
d x = c1 s2 d3 − s1 d 2 − d 6 ( c1 c2 c4 s5 + c1 c5 s2 − s1 s4 s5 )
d y = s1 s2 d 3 + c1 d 2 + d 6 ( c1 s4 s5 + c2 c4 s1 s5 + c5 s1 s2 )
d z = c2 d3 + d 6 ( c2 c5 − c4 s2 s5 )

which are too difficult to solve directly in closed form and which may or may not have a
solution.

Kinematic Decoupling

Suppose that there are exactly six degrees-of-freedom and that the last three joint axes
intersect at a point O (see Example 3 on page 10). We express (1) as two sets of
equations representing the rotational and positional equations.

R06 ( q1 , q2 , q3 , q4 , q5 , q6 ) = R

d06 ( q1 , q2 , q3 , q4 , q5 , q6 ) = d

where R and d are the given position and orientation of the tool frame.

Assumption of a spherical wrist means that the axes z4 , z5 and z6 intersect at O and
hence the origin O4 and O5 assigned by the D-H convention will always be at the wrist
center O coinciding with O3. This assumption assures that the motion of the final three
links about these axes will not change the position of O. In such a case, the position of the
wrist center is thus a function of only the first three joint variables.

15
 z4

O
z5 d6

d 0c = pc
O6

z0 k
6
d =d
0

y0
x0

Since the origin of the tool frame O6 in the frame O0 x0 y0 z0 is just

0
 
O6 − O = d 6 Rk k = 0 unit vector along the axis of rotation
1

Let pc denote the vector from the origin of the base frame to the wrist center. Thus, in
order to have the end-effector of the robot at the point d with the orientation of the end-
effector given by R = (rij ) , it is necessary and sufficient that wrist center O be located the
point

pc = d − d 6 Rk (2)

and let the orientation of frame O6 x6 y6 z6 with respect to the base be given by R . If the

components of the end-effector position d and wrist center pc are denoted, respectively,

by d x , d y and d z , and px , p y and pz , then (2) becomes

 px   d x − d6 r13  c1 s2 d3 − s1 d2 
 p y  = d y − d6 r23  = s1 s2 d3 + c1 d 2  (3)
 pz  d z − d6 r33   c2 d3 
     

Using (3), we may find the values of the first three joint variables θ1 , θ 2 and d3 which

determine R03 . Then

16
 R = R03 R36

−1 T
R36 = ( R03 ) R = ( R03 ) R (4)

The final three angles can be found from (4) using Euler angles:

cϕ − sϕ 0  cθ 0 sθ  cψ − sψ 0
     
R36 =  sϕ cϕ 0  0 1 0  sψ cψ 0
   −s  
0 0 1  θ 0 cθ  0 0 1
   
cϕ cθ cψ − sϕ sψ − cϕ cθ sψ − sϕ cψ cϕ sθ 
 
= sϕ cθ cψ + cϕ sψ − sϕ cθ sψ + cϕ cψ sϕ sθ 
 
 −sθ cψ sθ sψ cθ 

Summary
For this class of manipulators the determination of the inverse kinematics can be
summarized by the following algorithm.

Step 1: Find q1 , q2 and q3 such that the wrist center pc is located at

pc = d − d 6 Rk

Step 2: Using the joint variables determined in Step 1, evaluate R03 .

Step 3: Find a set of Euler angles corresponding to the rotation matrix
−1 T
R36 = ( R03 ) R = ( R03 ) R

Inverse Position: A Geometric Approach

a- Spherical Manipulator:

Consider the inverse position kinetics for a three degree of freedom spherical
manipulator.

17
It can be seen from geometry that
py
θ1 = arctan
px

provided that px and p y are not both zero. If px =0 and p y =0 , the manipulator is at its
singular configuration
s
θ 2 = arctan
r
where
r 2 = px2 + p y2 and s = pz − d1

We also have
2
(d 3 + a2 ) = r 2 + s 2

⇒ d 3 = r 2 + s 2 − a2 =
2
px2 + p y2 + ( pz − d1 ) − a2

18
 b- SCARA Manipulator:

c12c 4 + s12s 4 − c12s 4 + s12 c4 0 a1 c1 + a2 c12 

 
s c −c s − s12s 4 − c12c4 0 a1 s1 + a2 s12   R d
T04 =  12 4 12 4 =
 0 0 −1 − d3 − d 4   0 1 
 
 0 0 0 1 

 cα sα 0
 0
r12
R =  − sα cα  α = θ1 + θ 2 − θ 4 = arctan
r11
 0 0 1
 

From the above figure,

1−r 2 d x2 + d y2 − a12 − a22

θ 2 =arctan ± r=
r 2a1 a2

dy a2 s2
θ1 = arctan − arctan
dx a1 + a2 c2

r12
Then θ 4 = θ1 + θ 2 − α = θ1 + θ 2 − arctan
r11
Finally
d3 = d z + d 4

19
 VELOCITY KINEMATICS

Derivation of the Jacobian:

Consider an n-link manipulator with joint variables q1 , q2, ...qn . Let

 Rn ( q ) d 0n ( q ) 
T0n ( q ) =  0  (1)
 0 1 

Denote the transformation from the end-effector frame to the base frame, where
T
q = ( q1 , q2 , ..., qn )

is the vector of joint variables. q , R0n ( q ) and d 0n ( q ) are all functions of time.

Objective: Relate the linear and angular velocity of the end-effector to the vector of joint
velocities q
(t ) .

Let ω0n denote the angular velocity of the end-effector, and let

V0n = d
0n
denote the linear velocity of the end-effector. We seek expressions of the form
V0n = JV q

ω0n = J ω q

where JV and J ω are 3× n matrices. The above two equations can be rewritten as:

V0n  n
 n  = J 0 q

 ω0 
J 
where J 0n =  V  is called the Manipulator Jacobian or Jacobian for short. J 0n is 6 × n
 Jω 
matrix where n is the number if links.

20
Angular Velocity :

If the ith joint is revolute, then the ith joint variable qi equals θi and the axis of rotation

is the zi −1 -axis. Thus, the angular velocity of link i expressed in the frame i -1 is given by:

ωii-1 = q
i k

If the ith joint is prismatic ωii- 1 = 0 . Therefore, the overall angular velocity of the end-
effector, ω0n in the base frame is determined as:
n
n
0
1
ω = ρ1 q
1 k + ρ 2 q
2 R k + ... + ρ n q
n R k =
0
n −1
0 ∑ ρ q
z
i =1
i i i −1

where zi −1 = R0i −1k denotes the unit vector k of frame i –1 expressed in the orientation of

the base frame, and where ρi is equal to 1 if joint i is revolute and 0 if joint i is
prismatic.
0 
z0 = k = 0 
1 
Thus,
J ω =  ρ1 z1 ρ 2 z2 .... ρ n zn 

Linear Velocity :

The linear velocity of the end-effector is just d
0n . By chain rule of differentiation:
n
∂d 0n
d
0n = ∑i =1
∂qi
q
i

∂d0n
Thus, the ith column of JV is just . We shall consider two cases:
∂qi
Case 1: Joint i is prismatic:
R0j −1 is independent of qi = di for all j and

dii−1 = di k + Rii−1ai i
If all joints are fixed but the ith we have that

21
 d
0n = R0i −1d
ii−1 = d
i R0i −1k = d
i zi −1
Thus,
∂d 0n
= zi −1
∂qi

Case 2: Joint i is revolute:

Let ok denote the vector d 0k from the origin O0 to the origin Ok from any k, and write

d 0n = d 0i −1 + R0i −1d in−1

or, in the new notation,
On − Oi −1 = R0i −1d in−1

Note that both d 0i −1 and R0i −1 are constant if only the ith joint is actuated. Therefore,

d
0n = R0i −1d
in−1

Since the motion of link i is a rotation qi about zi −1 we have

d
in−1 = q
i k × d in−1

Thus,
d
0n = R0i −1 ( q
i k × d in−1 )

= q
i R0i −1k × R0i −1d in−1

= q
i zi −1 × ( On − Oi −1 )

22
Thus,
∂d 0n
= zi −1 × ( On − Oi −1 )
∂qi

and the upper half of the Jacobian JV is given by

JV =  JV JV ... JV JV 
 1 2 n −1 n 

where the ith column JV is

i

 J = zi −1 × ( On − Oi −1 ) (revolute joint)
 Vi

 JVi = zi −1 (prismatic joint)

Finally
J =  J1 J 2 ... J n−1 J n 


 J =  zi −1 × ( On − Oi −1 ) 
 i   (if i is revolute)
 
 z i −1 


 J i =  zi −1  (if i is prismatic )
  0 


23
Examples :
(i) Three-link planar manipulator:

Suppose we wish to compute the linear velocity V and the angular velocity ω of the
center of link 2 as shown:

V0n 
 n  =  J1 J 2 J 3  q

 ω0 
Replace On by Oc . Thus,

J1 = z0 × ( Oc − O0 )

J 2 = z1 × ( Oc − O1 )

J3 = 0 (link 2 is not affected by the motion of link 3)

(ii) Two-link planar manipulator:

24
  z0 × ( O2 − O0 ) z1 × ( O2 − O1 )  V ( O0 ) 
J (q) =   ⇒   = J ( q ) q

 z0 z1   ω 

0  a1 c1   a1 c1 + a2 c12  0 
   
O0 = 0  O1 =  a1 s1  O2 =  a1 s1 + a2 s12  z0 = z1 0 
0   0   0  1 
   
 − ( a1 s1 + a2 s12 ) − a2 s12 
 
 a1 c1 + a2 c12 a2 c12 
 0 0 
Thus, J =
 0 0 
 
 0 0 
 1 1 

(iii) SCARA manipulator:

Since joints 1, 2 and 4 are revolute and joint 3 is prismatic, and since o4 − o3 is parallel to

z3 then,

 z0 × ( O4 − O0 ) z1 × ( O4 − O1 ) z2 0
Ji =  
 z0 z1 0 z3 

 a1 c1   a1 c1 + a2 c12   a1 c1 + a2 c12 
     
O1 =  a1 s1  O2 =  a1 s1 + a2 s12  O4 =  a1 s1 + a2 s12 
 0   0   d3 − d 4 
     
Similarly,
z0 = z1 = k and z 2 = z3 = − k

 − ( a1 s1 + a2 s12 ) − a2 s12 0 0
 
 a1 c1 + a2 c12 a2 c12 0 0
 0 0 −1 0 
J =
 0 0 0 0
 
 0 0 0 0
 1 1 0 − 1

25
Singularities :

The 6 × n Jacobian J (q ) defines a mapping

X
= J ( q ) q

T
between the vector q
of joint velocities and the vector X
= (V , ω ) of the end-effector

velocities. Infinitesimally, this defines a linear transformation

dX = J ( q ) dq

between the differentials dq and dX . Singularities occur when the rank of J ( q )

decreases ( det J ( q ) = 0 ). Identifying manipulator singularities is important for several

reasons:

1. Singularities represent configurations from which certain directions of motion

may be unattainable.
2. At singularities, bounded gripper velocities may correspond to unbounded joint
velocities.
3. At singularities, bounded gripper forces and torques may correspond to
unbounded joint torques.
4. Singularities usually correspond to points of maximum reach of the manipulator.
5. Singularities correspond to points in the manipulator workspace that may be
unreachable under small perturbations of the link parameters, such as length,
offset, etc.
6. Near singularities, there will not exist a unique solution to the inverse kinematics
problem. In such cases, there may be no solution or there may be infinitely many
solutions.

Example :

Consider the two-dimensional system of equations,

26
 1 1
dX = J ( q ) dq =  dq
0 0 
that corresponds to the two equations:
dx = dq1 + dq2

dy = 0

rank(J) = 1. Note that for any values of dq1 and dq2 , there is no change in the value of
dy . Thus any vector dX having a nonzero second component represents an unattainable
direction of instantaneous motion.

Decoupling of Singularities :

Suppose that n=6 , that is, the manipulator consists of a 3-DOF arm with a 3-DOF
spherical wrist. A configuration q is singular if and only if:

det J ( q ) = 0

Let us partition the Jacobian J into 3× 3 blocks as:

J J12 
J =  J P J O  =  11 
 J 21 J 22 

Since the final three joints are always revolute

 z3 × ( O6 − O3 ) z4 × ( O6 − O4 ) z5 × ( O6 − O5 ) 
JO =  
 z3 z4 z5 

Since the wrist axes intersect at a common point O, if we choose the coordinate frames so
that
O3 = O4 = O5 = O6 = O

then J 0 becomes

0 0 0
JO =  
 z3 z4 z5 

and the ith column of J i of J P is

27
  zi −1 × ( O − Oi −1 ) 
Ji =   if joint i is revolute
 zi −1 

z 
J i =  i −1  if joint i is prismatic
 0 
In this case, the Jacobian matrix has the form:
J 0
J =  11 
 J 21 J 22 

det J = det J11 det J 22

J11 and J 22 are 3× 3 matrices. J11 has an ith column zi −1 × ( O − Oi −1 ) if joint i is

revolute and zi −1 if joint i is prismatic, while

J 22 =  z3 z4 z5 

Therefore, the set of singular configurations of the manipulator is the union of the set of
arm configuration satisfying det J11 = 0 and the set of wrist configurations satisfying

det J 22 = 0 .

(i) Wrist singularities :

det J 22 = det  z3 z4 z5  = 0

This implies that z3, z4 and z5 are linearly dependent. This happens when z3 and z5 are
collinear

This is the only singularity of the spherical wrist and is unavoidable without imposing
mechanical limits on the wrist design to restrict its motion in such a way the z3 and z5 are
prevented from lining up.

28
 (ii) Elbow manipulator singularities :

Consider the three link articulated manipulator with coordinate frames attached as shown
above. It can be shown that
 −a2 s1 c2 − a3 s1 c23 − a2 s 2 c1 − a3 s 23c1 − a3 c1 s 23 
 
J11 =  a2 c1 c2 + a3 c1 c23 − a2 s 2s1 − a3 s1 s 23 − a3 s1 s 23 
 0 a2 c2 + a3 c23 a3 s 23 


⇒ det J11 = a2 a3 s3 ( a2 c2 + a3 c23 )

 s = 0 ⇒ θ3 = 0 or π
Singular configurations  3
a2 c2 + a3 c23 = 0

First configuration : elbow is fully extended or fully retracted.

29

Second configuration : wrist center intersects the axis of the base rotation z 0

(iii) Spherical manipulator singularities :

This manipulator is in a singular configuration when the wrist center intersects z 0 , as

shown below.

30
(iv) SCARA manipulator singularities :

α1 α3 0  α1 = −a1 s1 − a2 s12

  α2 = a1 c1 + a2 c12
J11=α 2 α 4 0 
  α2 = −a1 s12
 0 0 −1 α4 = a1 c12

det J11 = 0 ⇒ α1 α 4 − α 2 α 3 = 0

It can be shown that this reduces to s2 =0 ⇒ θ 2 =0, π

Inverse Velocity and Acceleration

Inverse velocity and acceleration relationships are conceptually simpler that inverse
position. Recall that

X
= J ( q ) q
(2)

This implies
q
= J −1 ( q ) X

Differentiating (2) yields the acceleration equations

= J ( q ) q

+ d  J ( q )  q

X
dt  

This implies

q

= J −1 ( q ) b d
b = X

−  J ( q )  q
dt 

Provided that det J ( q ) ≠ 0

31
 DYNAMICS OF MANIPULATORS

Kinetic and Potential Energy

Suppose an object consists of a continuum of particles, and let ρ denote the density of
mass of the object. The mass of the object is expresses by:

m=
∫ ρ ( x,y,z )dxdydz
B

where B denotes the region of the 3-dimensional space occupied by the body.

The kinetic energy of the object is given by:

∫
1
K= V T ( x,y,z )V ( x,y,z ) ρ ( x,y,z )dxdydz
2 B

2∫
1
= V T ( x,y,z )V ( x,y,z )dm
B

2∫
1
= V T
( x,y,z )V ( x,y,z )dm
B

dm is the infinitesimal mass of the particle located at the coordinates ( x,y,z ) .

We define the coordinate vector of the center of mass as

∫r
1
rC = dm
m B

where r is the coordinate vector of a point on the body.

Now, suppose we attach a coordinate frame rigidly to the center of mass, with its origin at
the center of mass. The velocity of any point on the body is obtained from:

V = VC + ω × r
which corresponds to the velocity with respect to an inertial frame expressed with respect
to an inertial frame.

32
Let R denote the rotation matrix that transforms free vectors from the moving frame to
the inertial frame. Thus, the velocity of a particle located at r , expressed with respect to
the moving frame equals,
R T (VC + ω × r ) = R TVC + ( R T ω ) × ( R T r )

The kinetic energy is computed using any type of coordinate frame. Here, it will be
computed using the moving frame.

Now, we can express the vector cross product as a matrix multiplying a vector as follows:

V = VC + S ( ω ) r

where
 0 − ω3 ω2 
 
S ( ω ) =  ω3 0 − ω1 
 −ω2 ω1 0 
 
is a skew-symmetric matrix
S T ( ω) = − S ( ω)

The kinetic energy becomes:

∫
1 T
K= VC + S ( ω ) r  VC + S ( ω ) r dm
2 B

∫ ∫ ∫ ∫
1 1 1 1
= VCT VC dm + VCT S ( ω ) rdm + r T S T ( ω )VC dm + r T S T ( ω ) S ( ω )rdm
2 B 2 B 2 B 2 B

∫ ∫ ∫
1 1 1 1
= mVCTVC + VCT S ( ω ) rdm + r T dm S T ( ω )VC + r T S T ( ω ) S ( ω )rdm
2 2 B 2 B 2 B



 
0 0

∫
1 1
= mVCTVC + r T S T ( ω ) S ( ω )rdm
2 2 B

for any two vectors a and b

a T b = Tr abT (*)

33
Using (*) in K
1 1
K= mVCTVC + Tr  S ( ω ) JS T ( ω ) 
2 2
where J is a 3× 3 matrix defined by
 

 ∫ x 2 dm
∫ xydm
∫ xzdm 

 
∫
J = r T r dm = 
B  ∫ xydm
∫ y 2 dm
∫ yzdm 

 

 ∫ xzdm
∫ yzdm
∫ z 2 dm 

It can be further shown that
1 1
K= mVCTVC + ωT Iω
2 2

where I is the 3× 3 inertia matrix defined as:

 
 ∫
 ( y + z ) dm − xydm
2 2

∫ ∫
− xzdm 

 
∫
I =  − xydm
 ∫
( x 2 + z 2 ) dm ∫
− yzdm 

 
∫
 − xzdm

− yzdm
∫ ∫
( x 2 + y 2 ) dm 

Now consider a manipulator consisting of n links. Since the joint variables are indeed

the generalized coordinates, it follows that, for appropriate Jacobian matrices JV ( )

C i
and

(J )
ω i we have that

VCi = JV
Ci
( q ) q ωi = RiT ( q ) J ω ( q ) q
i

where RiT ( q ) takes care of the fact that the angular velocity must be expressed in the

frame attached to the link.

34
Suppose that the mass of link i is mi and that the inertia matrix of link i , evaluated
around a coordinate frame parallel to frame i but whose origin is at the center of mass
equals Ii . Thus, the overall kinetic energy of the manipulator equals

n
1
K = q T
2 ∑ m J
i =1
i
T
VCi
( q ) JV ( q ) + J ωT ( q ) Ri ( q ) I i RiT ( q ) J ω ( q ) q
Ci i i

In other words,
1 T
K= q D ( q ) q
2
D ( q ) is a symmetric positive definite matrix that is in general configuration dependent.

Now consider the potential energy term. In the case of rigid dynamics, the only source of
potential energy is gravity. Hence, the overall potential energy is

V=
∫ g rdm = g ∫ rdm = mg r
B
T T

B
T
C

Equations of Motion

The kinetic energy is assumed to be a quadratic function of the vector q of the form
n n

K=
1
2 ∑∑
i =1 j =1
d ij ( q ) qi q j =
1 T
2
q D ( q ) q

and V = V ( q ) which is independent of q .

Euler-Lagrange equations for such a system can be derived as follows:

n n

L = K −V =
1
2 ∑∑ d (q ) q q −V (q )
i =1 j =1
ij
i  j

The equations of motion

n n n

∑ ∑∑
 ∂d kj 1 ∂d ij  ∂V
d kj ( q ) qj +  −  qi q j − =τk k = 1, ... ,n
j =1 i =1 j =1
 ∂qi 2 ∂qk  ∂qk

35
It can also be written as:
n n n

∑ d (q ) q + ∑∑ c q q + ϕ (q) = τ
j =1
kj
j
i =1 j =1
 j
ijk i k k k = 1, ... ,n

where

1  ∂d kj ∂d ki ∂dij 
cijk =  + − 
2  ∂qi ∂q j ∂qk 

∂V
ϕk ( q ) = −
∂qk
or in matrix form:

D ( q ) q + C ( q,q ) q + g ( q ) = τ

where
n n

ckj =
∑ i =1
cijk ( q ) qi =
∑ i =1
1  ∂d kj ∂d ki ∂dij 
 + −
2  ∂qi ∂q j ∂qk 
 qi

Example: Two-link revolute joint arm:

l2

θ2
lC
2
m2
l1
lC
1

m1 θ1
x

36
 VC 1 = JV q
C1

 −l sin q1 0 
 C1 
JV =  lC cos q1 0 
C1  1 
 0 0 
 
similarly,
VC 2 = JV q
C2

 −l1 sin q1 − l sin ( q1 + q2 ) − lC sin ( q1 + q2 ) 

 C2 2 
JV =  l1 cos q1 + lC cos ( q1 + q2 ) lC cos ( q1 + q2 ) 
C2  2 2 
 0 0 
 
Hence, the transnational part of the kinetic energy is

1 1 1
2 2 C1 C1
{
m1VCT1 VC1 + m2VCT2 VC 2 = q T m1 JVT JV + m2 JVT JV q
2 C2 C2
}
It is clear that

ω1 = q1 k ω2 = ( q1 + q2 ) k

Since ωi is aligned with ki the triple product ωiT I i ωi reduces simply to (I33 )i times the
square of the magnitude of the angular velocity. Let

(I )
33 i = Ii

Hence the rotational kinetic energy of the overall system becomes:

1 T 1 0  1 1 
q  I1 0 0 + I 2 1 1  q
2     

I + I2 I2 
Thus, D ( q ) = m1 JVT JV + m2 JVT JV +  1 
 I2 I2 
C1 C1 C2 C2

It can be shown that

1
( 2 2
)
d11 = m1 lC2 + m2 l12 + lC2 + 2l1 lC cos q2 + I1 + I 2

37
 (
d12 = d 21 = m2 lC2 + l1 lC cos q2 + I 2
2 2
)
d 22 = m2 lC2 + I 2
2

1 ∂d11
c111 = =0
2 ∂q1

1 ∂d11
c121 = c211 = = −m2 l1 lC sin q2 = h
2 ∂q2 2

∂d12 1 ∂d 22
c221 = − =h
∂q2 2 ∂q1

∂d 21 1 ∂d11
c112 = − = −h
∂q1 2 ∂q2

1 ∂d 22
c122 = c212 = =0
2 ∂q1

1 ∂d 22
c222 = =0
2 ∂q2
Next, for the potential energy

( )
V = V1 + V2 = m1 lC + m2 l1 g sin q1 + m2 lC gsin ( q1 + q2 )
1 2

Hence,
∂V
ϕ1 = −
∂q1 1
( )
= − m1 lC + m2 l1 g cos q1 − m2 lC gcos ( q1 + q2 )
2

∂V
ϕ2 = − = −m2 lC gcos ( q1 + q2 )
∂q2 2

The final equations of motion are:

d11q1 + d12 q2 + c121q1 q2 + c211q1 q2 + c221q22 + ϕ1 = τ 1

d 21q1 + d 22 q2 + c112 q12 + ϕ 2 = τ 2

In this case
 hq2 hq1 + hq2 
C= 
 −hq1 0 

38
 CONTROL OF MANIPULATORS

The problem of controlling robot manipulators is the problem of determining the time
history of joint inputs required to cause the end-effector to execute a commanded input.
The joint inputs may be joint forces and torques, or they may be inputs to the actuators,
for example, voltage inputs to the motors depending on the model used for controller
design. The commanded motion is typically specified either as a sequence of end-effector
positions and orientations, or as a continuous path.

The dynamic equations of a robot manipulator form a complex, nonlinear and multi-
variable system. Therefore, we treat the robot control problem in the context of nonlinear
and multi-variable control. This approach allows us to provide more rigorous analysis of
the performance of control systems, and also allows us to design robust nonlinear control
laws that guarantee global stability and tracking of arbitrary trajectories.

PD Control

We have seen that the equations of motion of a robot manipulator are in given in the
following matrix form:

D ( q ) q

+ C ( q,q
) q
+ φ ( q ) = τ (1)

where D(q ) is the n × n inertia matrix and C ( q,q
) and φ(q ) are defined as

n n

ckj =
∑
i =1
cijk ( q ) q
i =
∑
i =1
1  ∂d kj ∂d ki ∂dij 
 + −
2  ∂qi ∂q j ∂qk 
 q
i

∂V
ϕk = −
∂qk
The input vector τ is suggested to take the following form:

τ = K P q − K D q
(2)

39
where q = q d − q is the error between the desired joint displacements qd and the actual

joint displacements q and K P and K D are diagonal matrices of (positive) proportional

and derivatives gains, respectively.

We first show that, in the absence of gravity, that is, if φ(q ) is zero in (1), the PD control
law (2) achieves asymptotic tracking of the desired joint positions. We consider the
Lyapunov function candidate:

1 1
V = q
T D ( q ) q
− q T K P q (3)
2 2
The first term in (3) is the kinetic energy of the robot and the second term accounts for
the proportional feedback K P q . V represents the total kinetic energy that would result if

the joint actuators were to be replaced by springs with stiffnesses represented by K P and
with equilibrium positions at qd . Thus V is a positive function except at the “goal”

q = q d and q
= 0 , at which point V is zero.

The idea is to show that along any motion of the robot, the function V is decreasing to
zero. This will imply that the robot is moving toward the desired goal configuration.

To show this, we note that qd is constant, the time derivative of V is given by:

1
V
= q
T D ( q ) q

+ q
T D
( q ) q
− q
T K P q (4)
2
Solving D ( q ) q

in (1) with φ ( q ) equals to zero and substituting the resulting expression

into (4) yields:

1
V
= q
T ( τ − C ( q,q
) q
) + q
T D
( q ) q
− q
T K P q
2
1
( )
= q
T τ − K P q + q
T  D
( q ) − 2C ( q, q
)  q

2
= q
T ( τ − K P q )

40
where in the last equality we have used the fact that the D
− 2C is skew symmetric.
Substituting the PD control law for τ into the above yields
V
= − q
T K D q
≤ 0

The above analysis shows that V is decreasing as long as q
is nonzero. This, by itself is
not enough to prove the desired result since it is conceivable that the manipulator can
reach a position where q
= 0 but q ≠ q d . To show that this cannot happen we use
LaSalle’s Theorem:

Given the system

x
= f ( x ) (*)

Suppose a Lyapunov function candidate V is found such that, along solution

trajectories
V
≤ 0

Then (*) is asymptotically stable if V
does not vanish identically along any
solution of (*) other than the null solution, that is, (*) is asymptotically stable
if the only solution of (*) satisfying
V
= 0
is the null solution.

Now, for the manipulator is hands:

Suppose V
≡ 0 , then (5) implies that q
≡ 0 and hence q

≡ 0 . From the equations of
motion with PD control

D ( q ) q

+ C ( q,q
) q
= − K P q − K D q

We must then have:

0 = − K P q

which implies that q = 0 and q
= 0 . LaSalle’s theorem implies that the system is
asymptotically stable.

41
In case there are gravitational terms present in (1) then the PD control alone cannot
guarantee asymptotic stability and tracking. In practice there will be a steady state error
of offset.
In this case,
V
= q
T ( τ − φ ( q ) − K P q )

Assuming that the closed loop system is stable, the robot configuration q that is achieved
will satisfy:

K P ( qd − q ) = φ ( q ) (6)

The physical interpretation of (6) is that the configuration q must be such that the motor

generates a steady state “holding torque” K P ( q d − q ) sufficient to balance the

gravitational torque φ ( q ) .

In order to remove this steady state error, we can modify the PD control law as:

τ = K P q − K D q
+ φ ( q ) (7)

The modified control law (7) in effect cancels the effect of the gravitational terms and we
achieve the same equation (5).

Inverse Dynamics

We now consider the application of more complex nonlinear control techniques for
trajectory tracking of rigid manipulators.

Consider the dynamics of an n-link robot in matrix form:

D ( q ) q

+ C ( q,q
) q
+ φ ( q ) = τ (8)

for simplicity, we write the above equation as:

M ( q ) q

+ h ( q,q
) = τ (9)

42
where M = D and h = Cq
+ φ . The idea of inverse dynamics is to seek a nonlinear
feedback control law:

τ = f ( q,q
) (10)

which, when substituted into (9) results in a linear closed-loop system. In this case, it is
chosen as (called the inverse dynamics control):
τ = M ( q ) v + h ( q,q
) (11)

Then, since the inertia matrix is invertible the combined system (9)-(11) reduces to:

q

= v (double integrator system) (12)

The new term v represents a new input to the system which is yet to be chosen. Note that
(12) represents n uncoupled double integrators. We propose that v be of the following
form:
v = − K 0 q − K1 q
+ r (13)

where K 0 and K1 are diagonal matrices with diagonal elements consisting of position
and velocity gains, respectively. The closed loop system is then the linear system:
q

+ K1 q
+ K 0 q = r (14)

Now given a desired trajectory

t → ( q (t ) ,q
( t ) )
d d

If one chooses the reference input r(t ) as:

r ( t ) = q

d ( t ) + K1 q
d ( t ) + K 0 q d ( t ) (15)

then the tracking error:

e (t ) = qd − q satisfies (16)

e

( t ) + K1 e
( t ) + K 0 e ( t ) = 0 (17)

An obvious choice of the gain matrices K 0 and K1 is

K 0 = diag {ω12 ,ω22 ,...,ωn2 }

43
 K1 = diag {2ς 1 ω1 ,2ς 2ω2 , ... ,2ς n ωn }

which results in a closed-loop system which is globally decoupled, with each joint
response equal to the response of an underdamped linear second order system with
natural frequency ωi and damping ratio ς i .

Implementation and Robustness Issues

Practical implementation of the inverse dynamics control laws requires that the
parameters in the dynamic model of the system be known precisely and also that the
complete equations of motion be computable in real time, typically 60-100 Hz. The
above requirements are difficult to satisfy in practice. In any physical system there is a
degree of uncertainty regarding the values of various parameters. In the case of a system
as complicated as a robot, this is particularly true, especially if the robot is carrying
known loads. Practically speaking there will be always inexact cancellation of the
nonlinearities in the system due to this uncertainty and also due to computational round-
off, etc. Therefore, it is much reasonable to suppose that, instead of (9), the nonlinear
control law is actually of the form:

ˆ ( q ) v + hˆ ( q,q
)
τ=M (18)

where M̂ and ĥ represent nominal or computed versions of M and h , respectively. The

uncertainty or modeling error, represented by

ˆ (q ) − M (q )
∆M := M

∆h := hˆ ( q,q
) − h ( q,q
)

is then due to the problem of parameter uncertainties, etc.

With the nonlinear control law (18), and dropping arguments for simplicity, the system
becomes:

ˆ + hˆ
M q

+ h = Mv (19)

44
Thus q

can be expressed by:

ˆ + M −1∆h
q

= M −1 Mv

( )
q

= v + M −1 Mˆ − I v + M −1∆h (20)

where ∆h = hˆ − h . Defining
ˆ −I
E = M −1 M (21)
and setting
v = q

d − K1 ( q
− q
d ) − K 0 ( q − q d ) (22)

We can write the above equation for the error e = q − q d as:

e

+ K1 e
+ K 0 e = η (23)
where η , hereafter called the uncertainty, is given by the expression

η = Ev + M −1∆h

= E ( q

d − K1 e
− K 0 e ) + M −1∆h (24)

Notice that the system (23) is still a coupled nonlinear system since η is a nonlinear
function of e . Therefore, it is not obvious that the system (24) is stable now can one
simply raise the gain sufficiently high in (24) and claim stability since the nonlinear
function η also depends on v , given by (22), and hence may increase with larger gains.

45