course material students)

8/7/2019 Course Material Students)

1/46

Universit deUniversit deUniversit deUniversit de SfaxSfaxSfaxSfax

Ecole NationaledIngnieurs de Sfax

National EngineeringSchool of Sfax

ROBOTIQUE

Document prpar

par

Professeur Slim Choura


2/46

1

OUTLINE OF ROBOT DESIGN

Consider a two-link robot as shown below:

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 21 and 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 coordinatesx andy can be determined. The problem

of inverse kinematics consists of determining the joint angles in terms ofx andy. Since the

forward kinematics equations are nonlinear there could be:

x

y

joint 1

joint 2

1

2

A

B

S


3/46

2

a- one solution

b- two or more solutions

c- no solution

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:

JR

==

y

x 1

2

=

whereJis known as the Jacobian of the manipulator. This implies:

RJ -1 =

The values 21 nd a at whichJis singular correspond to the singular configurations of the

manipulator.

Examples:

(a) (b)

02 = =2

position

position

Position

cannot be

reached


4/46

3

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 onLagrangian 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.


5/46

4

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 ( )1i and i are connected. The ith joint variable is denoted by iq . In the

case of revolute joint, iq is the joint angle of rotation and in the case of prismatic joint, iq 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 linki.

Illustration: elbow manipulator


6/46

5

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.

iiii ,xa,xd,z,zi

RotTranTranRot +++=A

=

1000

0cossin0

0sincos0

0001

1000

0100

0010

001

1000

100

0010

0001

1000

0100

00cossin

00sincos

ii

ii

i

i

ii

iia

d

1000

cossin0

sinsin-coscoscossin

cossinsincossincos

=

iii

iiiiiii

iiiiiii

d

a

a

(*)

where the four quantities iii da ,, and i are parameters of linki and joint i called

ia : length of the link

i : angle

i : twist angle

id : offset

Since iA 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 id for a prismatic joint, is the

joint variable.

iA can be rewritten as:

=

10

ii

i

pRA

iR : relative rotation of frame i with respect to frame ( )1i

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

( )1

i


7/46

6

Procedure of the D-H Convention

Step 1: Locate and label the joint axes ...,,, 120 nzzz .

Step 2: Establish the base frame. Set the origin anywhere on the -0z axis. The 0x and 0y axes

are chosen conveniently to form a right-hand frame.

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

Step 3: Locate the origin iO where the common normal to iz and 1iz intersects iz . If iz

intersects 1iz locate iO at this intersection. If iz and 1iz are parallel, locate iO at

joint i .

Step 4: Establish ix along the common normal between 1iz and iz through iO , or in the

direction normal to the 1iz - iz plane if 1iz and iz intersect.

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

Step 6: Establish the end-effector frame nnnn zyxO . Assuming the nth

joint is revolute, set

akn = along the direction 1nz . Establish the origin conveniently along nz , preferably

at the center of the gripper or at the tip of any tool that the manipulator may be

carrying. Set sjn = in the direction of the gripper closure and set nin = as as . If the

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

Step 7: Create a table of link parameters iii da ,, and i .

ia : distance along ix from iO to the intersection of ix and 1iz axes.


8/46

7

id : distance along 1iz from 1iO to the intersection of the ix and 1iz axes. id is a

variable if joint i is prismatic.

i : the angle between 1iz and iz measured about ix .

i : the angle between 1ix and ix measured about 1iz . i is a variable if joint i is

revolute.

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

parameters in (*).

Step 9: Form

nn ...AAAAT 3210 =

which gives the position and orientation of the tool frame expressed in base

coordinates.


9/46

8

Example 1: Planar elbow manipulator

0z and 1z are normal to the page.

Link ia i id i

1 1a 0 0 *1

2 2a 0 0*

2

=

=

1000

0100

sin0cossin

cos0sincos

1000

0100

sin0cossin

cos0sincos

2222

2222

21111

1111

1

a

a

a

a

AA

( ) ( ) ( )( ) ( ) ( )

1 2 1 2 1 1 2 1 2

1 2 1 2 1 2 1 1 2 1 20 1 0 1 2

0cos sin cos cos

0sin cos sin sin

0 0 1 0

0 0 0 1

a a

a a

+ + + + + + + +

= = =

T A T A A

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

origin 2O in the base frame:

( * ) variable


10/46

9

( )1 1 2 1 2cos cos x a a = + +

( )1 1 2 1 2sin sin y a a = + +

Example 2: Three-link cylindrical robot

Linkia i id i

1

2

3

=

=

=

1000

100

0010

0001

1000

01-0

0100

0001

1000

10000cossin

00sincos

3

3

2

2

1

11

11

1

dddAAA

1 1 3 1

3 1 1 3 10 1 2 3

1 2

0cos sin sin

0sin cos cos

0 1 0

0 0 0 1

d

d

d d

= = +

T A A A

0z 1z

2z

2z 3z

0y

1y

2y 3y

1x

2x 3x

0z

1z

O0

O1

O2 O3

1d

2d

3d


11/46

10

Example 3: Spherical wrist

Linkia i id i

4

5

6

=

=

=

1000

100

00cossin

00sincos

1000

0010

0cos0sin

0sin0cos

1000

0010

0cos0sin

0sin0cos

6

66

66

655

55

544

44

4

d

AAA

1000

ccsscs

ssssccscsscscsscsccssccssccc

10

6556565

654546465464654

654546465464654

63

63

65463

++

=

==

d

d

ddR

AAAT

where ii cosc = and ii sins =

4x

5z

Oa

3z

4z


12/46

11

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 to2

z and thus coincides with the axis3

z

of Example 2. Thus

=

++

+

==

1000

1000

ccsscs

ssssccscsscscs

scsccssccssccc

1000

010

cc0s

ss0c

333231

232221

131211

6556565

654546465464654

654546465464654

21

1311

1311

63

30

60

z

y

x

drrr

drrr

drrr

d

d

d

dd

d

d

TTT

651641654111 cssssccccc +=r 5154113 csscc =r

651641654121 cscssscccs +=r 5154123 ccscs +=r

6465431 scccs =r 5433 ss=r

651641654112 ssscscsccc =r 316516541 sdcsscc dddx =

651641654122 ssccsssccs +=r 316516541 cccscs ddddy ++=

6465432 ccscs=

r 21654ss ddddz++=

s

n

3d

1

2d 4

5

6

a


13/46

12

Example 5: Stanford manipulator

Linkia i id i

12

3

4

5

6

1000

100

0010

0001

1000

010

0c0s

0s0c

1000

0010

0c

0s

0s0c

3

3

2

22

22

211

11

1

=

=

=

ddAAA

=

=

=

1000

100

00cs

00sc

1000

0010

0c0s

0s0c

1000

0010

0c0s

0s0c

6

66

66

655

55

544

44

4

d

AAA

65432160 AAAAAAT =


14/46

13

Example 6: SCARA manipulator

Linkia i id i

1

2

3

4

1000

100

00cs

00sc

1000

100

0010

0001

1000

0100

s0cs

c0sc

1000

0000

s0cs

c0sc

4

44

44

4

3

32222

2222

21111

1111

1

=

=

=

=

dd

a

a

a

a

AAAA

+

+++

==

1000

100

ss0ccsssccs

cc0csscsscc

43

12211412412412412

12211412412412412

432140

dd

aa

aa

AAAAT


15/46

14

INVERSE KINEMATICS

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

Given a 44 homogeneous transformation matrix:

1 =

0R dH

with R is the rotation matrix, find (one or all) solutions of the equation

( )0 1 2 n, , ...,n

q q q =T H (1)

where

( )0 1 2 n 1 2 n, , ...,n q q q =T A A ... A

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

written as

( )1 2 n, , ...,ij ijq q q h=T 1, 2, 3i = 1, 2, 3, 4j =

Example: Stanford Manipulator (see previous chapter)

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

11 12 13

21 22 236

0

31 32 33

0 0 0 1

x

y

z

r r r d

r r r d

r r r d

=

T

To find the corresponding variables 654321 and,,,,, d we must solve the following

simultaneous set of nonlinear trigonometric equations:


16/46

15

( ) ( )11 1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6r c c c c c s s s s c s s c c c s = +

( ) ( )21 1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6r s c c c c s s s s c c s c c c s = + +

( )31 2 4 5 6 4 6 2 5 6r s c c c s s c s c=

( ) ( )12 1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6r c c c c s s s s s s s s c s c c = + + +

( ) ( )22 1 2 4 5 6 4 6 2 5 6 1 4 5 6 4 6r s c c c s s s s s s c s c s c c = + + + +

( )32 2 4 5 6 4 6 2 5 6r s c c s s c c s s= + +

( )13 1 2 4 5 2 5 1 4 5r c c c s s c s s s= +

( )23 1 2 4 5 2 5 1 4 5r s c c s s c c s s= + +

33 2 4 5 2 5r s c s c c= +

( )1 2 3 1 2 6 1 2 4 5 1 5 2 1 4 5xd c s d s d d c c c s c c s s s s= +

( )1 2 3 1 2 6 1 4 5 2 4 1 5 5 1 2yd s s d c d d c s s c c s s c s s= + + + +

( )2 3 6 2 5 4 2 5zd c d d c c c s s= +

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.

( )60 1 2 3 4 5 6, , , , ,q q q q q q =R R

( )60 1 2 3 4 5 6, , , , ,q q q q q q =d d

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

Assumption of a spherical wrist means that the axes 4z , 5z and 6z 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 ofO. In such a case, the position of the

wrist center is thus a function of only the first three joint variables.


17/46

16

Since the origin of the tool frame O6 in the frame 0000 zyxO is just

6 6d =O O Rk

=

1

0

0

k unit vector along the axis of rotation

Let cp 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 ( )ijr=R , it is necessary and sufficient that wrist center O be located the

point

6c d= p d Rk (2)

and let the orientation of frame 6666 zyxO with respect to the base be given by R . If the

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

by xd , yd and zd , and xp , yp and zp , then (2) becomes

+

=

=

32

21321

21321

336

236

136

dc

dcdssdsdsc

rddrddrdd

ppp

z

y

x

z

y

x

(3)

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

determine 30R . Then

O

d6

k

O6

6

0 =d d

0

c

c=d p

y0

z0

z4

z5

x0


18/46

17

=3 6

0 3R R R

( ) ( )1T

= =6 3 33 0 0R R R R R (4)

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

0 0c s c s0c s

0 0s c 0 1 0 s c

0s c0 0 1 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

=

= + +

6

3R

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 1q , 2q and 3q such that the wrist center cp is located at

6c d= p d Rk

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

Step 3: Find a set of Euler angles corresponding to the rotation matrix

( ) ( )1 T

= =6 3 3

3 0 0R R R R R

Inverse Position: A Geometric Approach

a- Spherical Manipulator:Consider the inverse position kinetics for a three degree of freedom spherical

manipulator.


19/46

18

It can be seen from geometry that

1arctan

y

x

p

p =

provided that xp and yp are not both zero. If 0=xp and 0=yp , the manipulator is at its

singular configuration

2 arctans

r =

where

2 2 2

x yr p p= + and 1zs p d=

We also have

( )2

2 23 2d a r s+ = +

( )22 2 2 23 2 1 2 x y zd r s a p p p d a= + = + +


20/46

19

b- SCARA Manipulator:

12 4 12 4 12 4 12 4 1 1 2 12

4 12 4 12 4 12 4 12 4 1 1 2 120

3 4

0c c s s c s s c c c

0s c c s s s c c s s

0 0 1 1

0 0 0 1

a a

a a

d d

+ + +

+ = =

0

R dT

00

0 0 1

c s

R s c

=

121 2 4

11

arctanr

r = + =

From the above figure,

r

r2

2

1arctan

=

2 2 2 2

1 2

1 22

x yd d a a

ra a

+ =

2 21

1 2 2

arctan arctany

x

d a sd a a c

= +

Then 124 1 2 1 2

11

arctanr

r = + = +

Finally

43 ddd z +=


21/46

20

VELOCITY KINEMATICS

Derivation of the Jacobian:

Consider an n-link manipulator with joint variables nqqq ...,, 21 . Let

( )( ) ( )0 0

0

1

n n

n

= 0

R q d qT q (1)

Denote the transformation from the end-effector frame to the base frame, where

( )1 2, , ...,T

nq q q=q

is the vector of joint variables. q , ( )0nR q and ( )0

nd q are all functions of time.

Objective: Relate the linear and angular velocity of the end-effector to the vector of joint

velocities ( )tq .

Let n0 denote the angular velocity of the end-effector, and let

= n n

0 0V d

denote the linear velocity of the end-effector. We seek expressions of the form

= n

0 VV J q

= n

0 J q

whereV

and

are n3 matrices. The above two equations can be rewritten as:

n

=

n

0

0n

0

Vq

where n

=

V

0J is called theManipulator Jacobian orJacobian for short. 0n is n6

matrix where n is the number if links.


22/46

21

Angular Velocity :

If the ith

joint is revolute, then the ith

joint variable iq equals i and the axis of rotation

is the 1iz -axis. Thus, the angular velocity of linki expressed in the frame i -1 is given by:

iq=

i

i -1 k

If the ith

joint is prismatic 0=i 1-i . Therefore, the overall angular velocity of the end-

effector, n0 in the base frame is determined as:

1 11 1 2 2 0 0 1

1

...

n

nn n i i i

i

q q q q

=

= + + + =

n0 k R k R k z

where 11 0

i

i

=z R k 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

0

1

= =

0z k

Thus,

1 1 2 2 .... n nz = J z z

Linear Velocity:

The linear velocity of the end-effector is just n0d . By chain rule of differentiation:

00

1

n nn

i

ii

q

q=

=

dd

Thus, the ith

column of VJ is justi

n

q

0d . We shall consider two cases:

Case 1: Joint i is prismatic:

10

jR is independent of ii dq = for allj and

iRkd iiiiii ad 11 +=

If all joints are fixed but the ith we have that


23/46

22

1 1

0 0 1 0 1

n i i i

i i i id d

= = = d R d R k z

Thus,

01

n

i

iq

=

dz

Case 2: Joint i is revolute:

Let ko denote the vector 0kd from the origin O0 to the origin Ok from any k, and write

1 1

0 0 0 1

n i i n

i

= +d d R d

or, in the new notation,

1

1 0 1i n

n i i

=O O R d

Note that both 10id and 10

iR are constant if only the ith

joint is actuated. Therefore,

1

0 0 1

n i n

i

= d R d

Since the motion of linki is a rotation iq about 1iz we have

1 1

n n

i i iq

= d k d

Thus,

( )10 0 1 n i n

i iq

= d R k d

1 1

0 0 1

i i n

i iq

= R k R d

( )1 1i i n iq = z O O


24/46

23

Thus,

( )0

1 1

n

i n iiq

=

d

z O O

and the upper half of the JacobianV

is given by

1 2 1

...n n

V V V V V

= J J J J

where the ith

columniV

is

( )1 1

1

(revolute joint)

(prismatic joint)

i n i

i

=

=

i

i

V

V

J z O O

J z

Finally

1 2 1...

n n = J J J J

( )1 1

1

1

(if is revolute)

(if is prismatic )

i n i

i

i

i

i

=

=

0

i

i

z O OJ

z

zJ


25/46

24

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:

0

1 2 3

0

n

n

=

VJ J q

ReplacenO by cO . Thus,

( )1 0 0c= z O O

( )2 1 1c= z O O

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

(ii) Two-link planar manipulator:


26/46

25

( )( ) ( ) ( )

( )0 2 0 1 2 1 0

0 1

= =

z O O z O O V O

q J q qz z

0

0

0

0

=

O

1 1

1 1 1

0

a c

a s

=

O

1 1 2 12

2 1 1 2 12

0

a c a c

a s a s

+

= +

O 0 1

0

0

1

=

z z

Thus,

( ) 2 121 1 2 12

1 1 2 12 2 12

0 0

0 00 0

1 1

a sa s a s

a c a c a c

+ + =

J

(iii) SCARA manipulator:

Since joints 1, 2 and 4 are revolute and joint 3 is prismatic, and since 34 oo is parallel to

3z then,

( ) ( ) 20 4 0 1 4 1

0 1 3

=

0

0i

zz O O z O OJ

z z z

1 1

1 1 1

0

a c

a s

=

O

1 1 2 12

2 1 1 2 12

0

a c a c

a s a s

+

= +

O

1 1 2 12

4 1 1 2 12

3 4

a c a c

a s a s

d d

+

= +

O

Similarly,

0 1= =z z k and 2 3= = z z k

( ) 2 121 1 2 12

1 1 2 12 2 12

0 0

0 0

0 0 1 0

0 0 0 0

0 0 0 0

1 1 0 1

a sa s a s

a c a c a c

+ +

=

J


27/46

26

Singularities:

The n6 Jacobian ( )qJ defines a mapping

( )= X J q q

between the vector q of joint velocities and the vector ( ),T

=X V of the end-effector

velocities. Infinitesimally, this defines a linear transformation

( )d d=X J q q

between the differentials qd and Xd . Singularities occur when the rank of ( )q

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

reasons:

1. Singularities represent configurations from which certain directions of motionmay be unattainable.

2. At singularities, bounded gripper velocities may correspond to unbounded jointvelocities.

3. At singularities, bounded gripper forces and torques may correspond tounbounded 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 kinematicsproblem. In such cases, there may be no solution or there may be infinitely many

solutions.

Example :

Consider the two-dimensional system of equations,


28/46

27

( )1 1

0 0

d d d

= =

X J q q q

that corresponds to the two equations:

1 2dx dq dq= +

0dy =

rank(J) = 1. Note that for any values of 1dq and 2dq , there is no change in the value of

dy . Thus any vector Xd having a nonzero second component represents an unattainable

direction of instantaneous motion.

Decoupling of Singularities :

Suppose that 6=n , 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 0=J q

Let us partition the Jacobian J into 33 blocks as:

11 12

21 22

P O

= =

J

J J J J

Since the final three joints are always revolute

( ) ( ) ( )3 6 3 4 6 4 5 6 5

3 4 5

O

=

z O O z O O z O OJ

z z z

Since the wrist axes intersect at a common point O, if we choose the coordinate frames so

that

3 4 5 6= = = =O O O O O

then 0J becomes

3 4 5

O

=

0 0 0J

z z z

and the ith column of iJ of PJ is


29/46

28

( )1 1

1

if joint is revolutei i

i

i

i

=

z O OJ

z

1 if joint is prismaticii

i

= 0

zJ

In this case, the Jacobian matrix has the form:

11

21 22

=

0JJ

J

11 22det det det= J J

11J and 22J are 33 matrices. 11J has an ith column ( )1 1i i z O O if joint i is

revolute and 1iz if joint i is prismatic, while

22 3 4 5 = z z z

Therefore, the set of singular configurations of the manipulator is the union of the set of

arm configuration satisfying 0det 11 =J and the set of wrist configurations satisfying

22det 0=

J .

(i) Wrist singularities :22 3 4 5

det det 0 = = J z z z

This implies that 543 and, zzz are linearly dependent. This happens when 53 andzz 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 53 andzz are

prevented from lining up.


30/46

29

(ii) Elbow manipulator singularities :

Consider the three link articulated manipulator with coordinate frames attached as shown

above. It can be shown that

2 1 2 3 1 23 2 2 1 3 23 1 3 1 23

11 2 1 2 3 1 23 2 2 1 3 1 23 3 1 23

2 2 3 23 3 23

s c s c s c s c c s

c c c c s s s s s s

0 c c s

a a a a a

a a a a a

a a a

= + +

J

( )11 2 3 3 2 2 3 23det a a s a c a c = +J

Singular configurations 3 3

2 2 3 23

0 0 or

0

s

a c a c

= =

+ =

First configuration : elbow is fully extended or fully retracted.


31/46

30

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

(iii) Spherical manipulator singularities :

This manipulator is in a singular configuration when the wrist center intersects0

z , as

shown below.


32/46

31

(iv) SCARA manipulator singularities :

=

100

0

0

42

31

11

J

1 1 1 2 12

2 1 1 2 12

2 1 12

4 1 12

a s a s

a c a c

a s

a c

=

= +

=

=

11 1 4 2 3det 0 0 = =J

It can be shown that this reduces to ,00 22 ==s

Inverse Velocity and Acceleration

Inverse velocity and acceleration relationships are conceptually simpler that inverse

position. Recall that

( )= X J q q (2)

This implies

( )1= q J q X

Differentiating (2) yields the acceleration equations

( ) ( )d

dt= +

X J q q J q q

This implies

( )1=q J q b ( )d

dt=

b X J q q

Provided that ( )det 0J q


33/46

32

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:

( ), ,B

m x y z dxdydz=

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

The kinetic energy of the object is given by:

( ) ( ) ( )

( ) ( )

T

T

1, , , , , ,

2

1, , , ,

2

B

B

K x y z x y z x y z dxdydz

x y z x y z dm

=

=

V V

V V

( ) ( )T1

, , , ,

2 B

x y z x y z dm=

V V

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

1C

B

dmm

= r r

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:

C= + V V r

which corresponds to the velocity with respect to an inertial frame expressed with respect

to an inertial frame.


34/46

33

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,

( ) ( ) ( )T T T TC C+ = + R V r R V R R 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:

( )C= +V V S r

where

( )3 2

3 1

2 1

0

0

0

=

S

is a skew-symmetric matrix

( ) ( )T

= S S

The kinetic energy becomes:

( ) ( )

( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( )

T

T T T T T T

T T T T T T

0 0

T T T

1

2

1 1 1 12 2 2 2

1 1 1 1

2 2 2 2

1 1

2 2

C C

B

C C C C

B B B B

C C C C

B B B

C C

B

K dm

dm dm dm dm

m dm dm dm

m dm

= + +

= + + +

= + + +

= +

V S r V S r

V V V S r r S V r S S r

V V V S r r S V r S S r

V V r S S r

for any two vectors a and b

T TTr=a b ab (*)


35/46

34

Using (*) in K

( ) ( )T T1 1

Tr2 2C C

K m = +

V V S JS

where J is a 33 matrix defined by

2

T 2

2

B

x dm xydm xzdm

dm xydm y dm yzdm

xzdm yzdm z dm

= =

J r r

It can be further shown that

T T1 1

2 2C C

K m= +V V I

where I is the 33 inertia matrix defined as:

( )

( )

( )

2 2

2 2

2 2

y z dm xydm xzdm

xydm x z dm yzdm

xzdm yzdm x y dm

+

= +

+

I

Now consider a manipulator consisting of n links. Since the joint variables are indeed

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

J and

( )i

we have that

( )= Ci

Ci VV J q q ( ) ( )Ti=

ii

R q J q q

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

frame attached to the link.


36/46

35

Suppose that the mass of link i is im 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

( ) ( ) ( ) ( ) ( ) ( )T T T T

1

1

2

n

i i i

i

K m I

=

= + Ci Ci i iiV V q J q J q J q R q R q J q q

In other words,

( )T1

2

K= q D q q

( )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

T T T

C

B B

V dm dm m= = = g r g r g r

Equations of Motion

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

( ) ( )T

1 1

1 1

2 2

n n

ij i j

i j

K d q q

= =

= = q 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:

( ) ( )1 1

1

2

n n

ij i j

i j

L K V d q q V

= =

= = q q

The equations of motion

( )1 1 1

1

2

n n n

kj ij

kj j i j k

i k k j i j

d d Vd q q q

q q q

= = =

+ =

q 1, ... ,k n=


37/46

36

It can also be written as:

( ) ( )1 1 1

n n n

kj j ijk i j k k

j i j

d q c q q

= = =

+ + = q q 1, ... ,k n=

where

1

2

kj ijkiijk

i j k

d ddc

q q q

= +

( )kk

V

q

=

q

or in matrix form:

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

where

( )1 1

1

2

n n

kj ijkikj ijk i i

i j ki i

d ddc c q q

q q q= =

= = +

q

Example: Two-link revolute joint arm:

1m

2m

2l

1l

2Cl

1Cl

x

y

1

2


38/46

37

11 =

CC V

V J q

1

1 1

1

1

0sin

0cos

0 0

C

C

l q

l q

=

CVJ

similarly,

22 =

CC V

V J q

( ) ( )

( ) ( )2 2

2 2 2

1 1 1 2 1 2

1 1 1 2 1 2

sin sin sin

cos cos cos

0 0

C C

C C

l q l q q l q q

l q l q q l q q

+ + = + + +

CVJ

Hence, the transnational part of the kinetic energy is

{ }1 1 2 2

T T T T T

1 1 1 2 2 2 1 2

1 1 1

2 2 2C C C C

m m m m+ = + C C C C V V V V

V V V V q J J J J q

It is clear that

1 1q= k ( )2 1 2q q= + k

Since i is aligned with ik the triple productT

i i i I reduces simply to ( )iI33 times the

square of the magnitude of the angular velocity. Let

( )33 iiI I=

Hence the rotational kinetic energy of the overall system becomes:

T

1 2

1 0 1 11

0 0 1 12I I

+

q q

Thus, ( )1 1 2 2

T T 1 2 2

1 2

2 2

I I I m m

I I

+= + +

C C C C V V V V

D q J J J J

It can be shown that

( )1 2 2

2 2 2

11 1 2 1 1 2 1 22 cosC C Cd m l m l l l l q I I = + + + + +


39/46

38

( )2 2

2

12 21 2 1 2 2cosC Cd d m l l l q I = = + +

2

2

22 2 2Cd m l I = +

11111

1

10

2

dc

q

= =

2

11121 211 2 1 2

2

1sin

2 Cd

c c m l l q hq

= = = =

12 22221

2 1

1

2

d dc h

q q

= =

21 11112

1 2

1

2

d dc h

q q

= =

22122 212

1

10

2

dc c

q

= = =

22222

2

10

2

dc

q

= =

Next, for the potential energy

( ) ( )1 2

1 2 1 2 1 1 2 1 2sin sin

C CV V V m l m l g q m l g q q= + = + + +

Hence,

( ) ( )1 2

1 1 2 1 1 2 1 2

1

cos cosC C

Vm l m l g q m l g q q

q

= = + +

( )2

2 2 1 2

2

cosC

Vm l g q q

q

= = +

The final equations of motion are:

2

11 1 12 2 121 1 2 211 1 2 221 2 1 1d q d q c q q c q q c q + + + + + =

2

21 1 22 2 112 1 2 2d q d q c q + + + =

In this case

2 1 2

10

hq hq hq

hq

+=

C


40/46

39

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 ( )qD is the nn inertia matrix and ( ), C q q and ( )q are defined as

( )1 1

1

2

n n

kj ijkikj ijk i i

i j ki i

d ddc c q q

q q q= =

= = +

q

k

k

V

q

=

The input vector is suggested to take the following form:

= P D K q K q (2)


41/46

40

where = dq q q is the error between the desired joint displacements dq and the actual

joint displacements q andP

K andD

K 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:

( )T T1 1

2 2P

V= q D q q q K q (3)

The first term in (3) is the kinetic energy of the robot and the second term accounts for

the proportional feedbackP

K q . Vrepresents the total kinetic energy that would result if

the joint actuators were to be replaced by springs with stiffnesses represented by PK and

with equilibrium positions at dq . Thus V is a positive function except at the goal

=d

q q and =q 0 , at which point Vis 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 dq is constant, the time derivative ofVis given by:

( ) ( )T T T1

2PV= +

q D q q q D q q q K q (4)

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

into (4) yields:

( )( ) ( )T T T1

,2

PV= + q C q q q q D q q q K q

( ) ( ) ( )T T1

2 ,2

P = +

q K q q D q C q q q

( )T P= q K q


42/46

41

where in the last equality we have used the fact that the 2 C is skew symmetric.

Substituting the PD control law for into the above yields

T 0D

V = q K q

The above analysis shows that Vis 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 dq q . To show that this cannot happen we use

LaSalles Theorem:

Given the system

( )f=x x (*)

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

trajectories

0V

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

0V =

is the null solution.

Now, for the manipulator is hands:

Suppose 0V , then (5) implies that q 0 and hence q 0 . From the equations of

motion with PD control

( ) ( ), P D+ =

q q C q q q K q K q

We must then have:

P= 0 K q

which implies that =q 0 and =q 0 . LaSalles theorem implies that the system is

asymptotically stable.


43/46

42

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,

( )( )T PV = q q K q

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

will satisfy:

( ) ( )dP =K q 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 ( )dP K q q sufficient to balance the

gravitational torque ( ) q .

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

( )= + P D K q K 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)


44/46

43

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:

0 1= +v K q K q r (13)

where 0K and 1K are diagonal matrices with diagonal elements consisting of position

and velocity gains, respectively. The closed loop system is then the linear system:

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

Now given a desired trajectory

( ) ( )( ),d dt t t q q

If one chooses the reference input ( )tr as:

( ) ( ) ( ) ( )1 0d d dt t t t = + + r q K q K q (15)

then the tracking error:

( ) dt = e q q satisfies (16)

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

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

{ }2 2 20 1 2diag , ,..., n =K


45/46

44

{ }1 1 1 2 2diag 2 ,2 , ... ,2 n n =K

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:

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

where M and h represent nominal or computed versions ofM and h , respectively. The

uncertainty or modeling error, represented by

( ) ( ) = :M M q M q

( ) ( ) , , = : 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:

+ = +M q h Mv h (19)


46/46

Thus q can be expressed by:

1 1 = + q M Mv M h

( )1 1 = + + q v M M I v M h (20)

where = h h h . Defining

1 = E M M I (21)

and setting

( ) ( )1 0d d d

= v q K q q K q q (22)

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

1 0+ + = e K e K e (23)

where , hereafter called the uncertainty, is given by the expression

1= + Ev M h

( )1

1 0

d = +

E q K e K e M 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.

course material students)

Documents