>

TR-ICCC-21

PK9600227

DYNAMIC MODELLING AND SIMULATION FORCONTROL OF A CYLINDRICAL

ROBOTIC MANIPULATOR

By

ASIF IQBAL and S. M. ATHAR

Approved By:.

Dr. M. N.Project Director

INFORMATICS COMPLEX (ICCC)P. O. BOX 2191, ISLAMABAD

March 1995

V

ABSTRACT

In this report a dynamic model for the three degrees-of-freedom

cylindrical manipulator, INFOMATE has been developed. Although

the robot dynamics are highly coupled and non-linear, the

developed model is relatively straightforward and compact for

control engineering and simulation applications. The model has

been simulated using the graphical simulation package

SIMULINK. Different aspects of INFOMATE associated with

forward dynamics, inverse dynamics and control have been

investigated by performing various simulation experiments. These

simulation experiments confirm the accuracy and applicability of

the dynamic robot model.

TABLE OF CONTENTS

ABSTRACT i

TABLE OF CONTENTS ii

1. INTRODUCTION 1

2. TECHNIQUES TO DERIVE THE MANIPULATOR DYNAMICS 2

2.1 LAGRANGIAN EQUATIONS 2

2.2 NEWTON - EULER EQUATIONS 2

2.3 KANE'S EQUATIONS 3

3. LAGRANGE'S EQUATIONS 4

4. THE DYNAMIC MODEL OF INFOMATE 5

5. BUILDING A SIMULINK MODEL FOR INFOMATE 10

5.1. THE FORWARD DYNAMIC MODEL 10

5.2. THE INVERSE DYNAMIC MODEL 12

5.3. THE FORWARD DYNAMIC MODEL WITH FEED-BACK CONTROL 15

6. SIMULATION EXPERIMENTS 18

6.1 SIMULATION RESULTS OF THE FORWARD DYNAMIC MODEL 18

6.2 SIMULATION RESULTS OF THE INVERSE DYNAMIC MODEL 24

6.3 SIMULATION RESULTS OF THE FORWARD DYNAMIC MODEL

WITH PROPORTIONAL GAIN CONTROLLER 24

6.4 SIMULATION RESULTS OF THE FORWARD DYNAMIC MODEL

WITH PD-CONTROLLER 24

7. DISCUSSION : 35

8. CONCLUSIONS AND FUTURE ISSUES 37

APPENDLX1 39

A l l SALIENT FEATURES OF SIMULINK 39

A. 1.2 BRIEF DESCRIPTION OF SIMULINK BLOCKS 40

A. 1.3 BLOCKS USED IN THE SIMULATION 41

APPENDED 2 43

A.2.1 EQUATIONS OF MOTION OF INFOMATE INCLUDING THE DRIVES 43

A.2.2 CALCULATIONS OF INERTIAS OF THE DRIVE SYSTEMS 44

REFERENCES 47

1. INTRODUCTION

The project CYLROM1 aims to develop a robotic manipulator "INFOMATE" and is

a part of the efforts of Regulation, Protection and System Support Group, ICCC

for developing the autonomous robotic systems for operation and maintenance.

The manipulator will be a six degrees-of-freedom cylindrical robotic arm with a

revolute base, two prismatic links and a wrist with role, pitch and yaw movements.

In this report the dynamic model for the three degrees-of-freedom INFOMATE

(consisting of revolute base and two prismatic links) is introduced. The dynamic

model is a set of mathematical equations describing the dynamic behavior of the

manipulator. Such equations are necessary for devising a suitable control law using

computer simulations subsequent to the design of physical structure of a

robotic arm.

The report is organized as follows. In section 2, the three main formalisms

generally used for constructing robot arm dynamics are briefly outlined. The

Lagrange's method that has been used by the authors for developing the model is

reviewed in section 3. In section 4, the dynamic model of INFOMATE is developed

and characteristics and properties of the model are discussed. Section 5 describes

how the developed model is simulated using SIMULINK2. In section 6, various

simulation experiments are detailed that are performed to study the model for the

forward dynamics in open loop and inverse dynamics. A complete set of

experiments is also included to investigate the effect of applying independent

PD-controllers at the three joints. Various important findings of the study are

briefly discussed in section 7. Finally, in section 8 conclusions are drawn and the

future directions of this ongoing research are proposed. In this report the

appendices and references are given at the end.

1 CVLindrical RObotic Manipulator2 A graphical simulation package.

2. TECHNIQUES TO DERIVE THE MANIPULATORDYNAMICS

The three main formalisms that are generally used to model the manipulator

dynamics are described below.

2.1 Lagrangian Equations

The Lagrangian equation is [1]:

where L = K- P, K is kinetic energy and P is the potential energy of the system

under consideration and qi are the generalized co-ordinates. Ft denotes the

generalized forces or torques.

The equations governing the motion of a complex mechanical system, such as

robot manipulator, can be expressed very efficiently through the use of the method

developed by Lagrange. Lagrange's equations are particularly useful in that they

automatically include the constraint that exist by virtue of the different parts of a

system being connected to each other, and thereby eliminate the need for

substituting one set of equations into another to eliminate forces and torques of

constraints Since they deal with scalar quantities due to which the need of

complicated vector diagrams that are required to define and resolve vector

quantities in the proper co-ordinate system is eliminated [2]. The drawback in

Lagrange's formalism is the computational complexity [3]. The Lagrange's

equations are computationally very expensive and thus are inefficient for real time

control applications unless they could be simplified.

2.2 Newton - Euler Equations

The Newton-Euler Equations are written as [1]:

l=»>l ( 2 )w, =/,©,+, (3)

Here, ji is the net force on link /, mt is its mass, and ru is the acceleration of the

center of gravity. /, is the inertia,co; is the angular velocity and «• is the net torque.

Newton-Euler equations provide an alternate approach that isolates each link in

sequence as an independent rigid body to determine the dynamic model for t/iat

link. The resulting equations of motion represent a recursive model for a link

involving variables of the adjacent link. These equations describe in detail the

translational and rotational dynamics of the link in terms of inertial forces and

torques. Such transparency of relation do not appear in Lagrange's formulation.

Consequently, a developed model helps the designer to understand the dyiamic

behavior of each link separately, and particularly propagation of the forces and

torques through the joints and their interaction [4].

2.3 Kane's Equations

The analysis, based either on the Lagrange formulation or on the Newtoi-Euler

technique, become very complicated when applied to multi-link mechanisns. The

Lagrange's formulation gives a closed-form set of equations but a huge amount of

algebraic manipulation is required to introduce and subsequently elimintte the

Lagrange multipliers. Similarly, with the Newton-Euler approach, to bring in and

then eliminate non-contributing inter-body constraint forces and torques Kane

described [5] an efficient computational scheme for formulating explicit eqiations

of motion for open-loop mechanisms. The Kane's equations are [1]:

' 8 i. 8(o;

In Kane's equations ft is the net force on link /, ft is the velocity of the center of

gravity, /7, is the net torque, oo. is the angular velocity.

The ii(lv;ml;igo of this scheme over Ihe Lagrange and Ncwton-Luler iormulaiions is

that it automatically eliminates non-contributing constraint forces without requiring

the calculation of lengthy differentials [3]. However, the recursive equations smash

the structure of the dynamic model, which is quite useful in providing insight for

designing the controller. For controller analysis, one would like to obtain an

explicit set of closed form differential equations that describe the dynamic behavior

of manipulator. In addition, the interaction and coupling reaction forces in the

equation should be easily identified so that an appropriate controller is designed to

compensate for their effects. The equations resulting from Lagrange's approach

have these characteristics. The method of Lagrange is used by the authors to model

the dynamics of INFOMATE. The basic idea of Lagrange's method will be described

in the next section.

3. LAGRANGE'S EQUATIONS

The fundamental approach of Lagrange's method is to represent the system by

using a set of generalized co-ordinates

Equation (6) represents r number of second-order differential equations. Therefore,

a dynamic system with r degrees-of-freedom will be represented by r second-order

differential equations. If one state variable is assigned to each generalized

co-ordinate and another to the corresponding derivative, it ends up with

2r equations. Thus a system with /' degrees-of-freedom is of order 2r.

4. THE DYNAMIC MODEL OF INFOMATE

The derivation of the dynamic model of manipulator based on the Lagrange's

equation for manipulator is simple and systematic. Assuming rigid body motion,

the resulting equations of motion, excluding the dynamics of electronic devices,

backlash and gear friction, are a set of second order coupled non-linear differential

equations.

The simplified design of INFOMATE schematically shown in Fig. 1 consists of three

degrees-of-freedom, i.e. rotation 0, vertical translation z and radial translation /•.

Consequently, the position of the end-effector can be described by the independent

joint co-ordinates r, 0, z. The structural parameters of INFOMATE are assigned as:

mo Mass of the radial arm

/ Length of the radial arm

mp Mass of the payload

mh Mass of the hub

m = ma-r nip Mass of the radial assembly

mv Vertically translated mass (sum of mo, mp and mh)

Fr Fe> Fz Forces and torques that derive the r, #and

z-degree-of-freedom respectively

j(r) Variable inertia of the radial link

J Constant inertia of the vertical column

r, 0, z Velocities of r, 6 and z directions

g Acceleration due to gravity

mo 11 Mass per unit length of the radial arm

m,

, r

m.

(a)

(b)

Fig. 1. Simplified design of INFOMATE, (a): side view (b): top view.

The mass per unit length of prismatic radial link is constant and its length changes

when it slides through the hub. Another translational joint determines the height of

the arm on the horizontal plane. Following steps are performed to determine the

dynamic model for the motion of the manipulator using Lagrange's approach.

1. Evaluation of the total kinetic energy of the system.

2. Evaluation of the total potential energy of the system.

3. Forming the Lagrangian function.

4. Deriving the equations of motion.

The kinetic energy of system consists of two types namely rotational kinetic energy

and translational kinetic energy.

Kinetic energy(traiislational) = — (mass)(linear velocity)2

Kinetic energy(rotational) = —{moment of wetia){angular velocity)2

(8)

The variable inertia of the radial \\rk,j(r) is written as:

or

(9)

Since J is the constant inertia of the vertical column, rotational kinetic energy of

the system can be written as:

Kinetic energy(rofa(ional) = — [ J + _/(;•)] 92 (10)

The translational kinetic energies in the r and z directions can be written as:

Kinetic energy{radia]) = —mf2 (11)

Kinetic energy{vertica1) = — mv i2 (12)

Hence, the total kinetic energy, K of the system is:

+ ̂ mf2+±nKz2 (13)

The total potential energy, P of the system is simply:

P = mvgz (14)

Using equations (13) and (14), the Lagrangian function is described as:

• 2 , • 2 / i f \

In order to write the equations of motion, the following partial derivatives are

determined.

dL

dr

8L86

8L

86

8L

dz

dLdz

= mr

-[J+J(r)]0

= 0

= mv 2

(16)

Using the above relationships in equation (6), the equations of motion for

INFOMATE are:

2ml 4/ p I m(17)

(18)

m..(19)

These three equations govern the motion of the cylindrical manipulator, INFOMATE

end-effector in the cylindrical co-ordinate system.

In the matrix form:

\f

e0

0

0

L-gJ

m

+ 0

0

1 II0

0 —

(20)

In equation (20), the first term on right-hand-side represents the Coriolis and

centrifugal generalized forces, the second term is the gravitational force and the

last term signifies the applied generalized forces at respective joints.

The dynamic model of the INFOMATE in equation (20) indicates that the vertical

motion is characterized by an uncoupled double integrator, with the rotational and

radial motions being coupled and non-linear. The inertia matrix

I(r) = Diagonal[m J + j(r) mv] of the INFOMATE is diagonal and depends

explicitly upon the radial displacement. The radial dependence of the inertia matrix

leads to the Coriolis torque {d j I dr)6 f in equation (18) and the centrifugal

force {d j Idr)62 in equation (17). So the manipulator is a variable inertia

mechanical system. The co-ordinate dependent inertia matrix ](r) gives rise to the

coupling (centrifugal and Coriolis) forces, and thus the acceleration is not

constant [6].

Although the developed model seems to be relatively uncomplicated, it maintains

coupling and non-linear characteristics inherent in the robot dynamics. The vertical

motion can possibly be controlled by a conventional servomechanism, but the

coupled rotational and radial degrees-of-freedom introduce complex control

problems [6].

5. BUILDING A SIMULINK MODEL FOR INFOMATE

The following three models are implemented using SIMULINK.

1. The forward dynamic model.

2. The inverse dynamic model.

3. The forward dynamic model with PD-control.

5.1. The Forward Dynamic Model

The forward model is a set of differential equations representing the manipulator

dynamics The dynamic equations are used to solve for the joint variables (/', 6, z)

give the generalized torques and forces as inputs [4].

10

The forward model to be simulated is given by equations (17) (18) and (19). The

equations (17) and (18) are coupled, so both are simulated as a single system. The

radial and angular direction equations are rewritten as:

For implementing the simulation on SIMULINK, the equations are decomposed

into various subsystems. In the simulation diagram, each subsystem is represented

by a SIMULINK block3. A simulink block may be a generic library function

(e.g. an integrator) or a user defined subsystem consisting, for example, of a group

of parameters or a group of many generic simulink blocks. The following blocks

are built for angular and radial direction equations consisting of constants,

parameters and variables.

(21)

= 2mpr (22)

7 T (24)

< 2 5 )

3 A brief introduction to SIMULINK is given in the Appendix 1.

11

01 = (7^» (26>

02 = e,Fe (27)

Using equations (21) to (27) the relationships for the radial and angular directions

take the form:

(28)

(29)

Subsystems (21) to (27) are constructed and their SIMULINK diagrams are shown

in Appendix 1. These subsystems are connected using equations (28) and (29) and

the SIMULINK model is developed for forward dynamics.

Due to the generic nature of the manipulator, the equation of motion in vertical

direction is uncoupled and hence could easily be implemented. The complete

SIMULINK diagrams of the forward model are shown in Fig. 2.

5.2. The Inverse Dynamic Model

The inverse dynamic model can be obtained from the forward dynamic model. In

the inverse dynamics, the desired joint variables and their first two time derivatives

are given and the generalized forces are computed. By algebraic manipulation of

equations (28), (29) and (19), the inverse dynamic model can be obtained as:

(30)

9 (31)

F,=mvz- + mvg (32)

The SIMULINK diagrams of these models are shown in Fig 3.

12

1 « HTo Workjp*ce7

(—

v* r-To Worlspie*

hJ Vi\

—M « 1ToWorX

Sum3

4-1

Ep»C»6

S

4 - ii

5f2

•4-

• -»-

5-C3

•

4-.Producti

1

•

4 -

Product3

— * • Vi

Integrator lrXegriio

StepFcn Integrator Integrator!

* L jProducti ma.mp To Workspace

Sum

LMHlLjTo Workspace3

Product3

JLJ+

To Wo»V.jp*ct4

To Workspace5

Sum3

StepFcn!

Product2

Pioduct4

Sum1

11

m

4

S-To Workspace2

Product

C2

^

^

_EL

It2

Fen

VsIntegrators Int*grator2

J.j(r)

3-Cl

vti

dti

(a)

Tz To Workspace

^ M Product

sumi

Preducrt

LD

9.8 m v

^ 1

tnn

(b)

Fig. 3. The SIMULINK diagrams of the inverse dynamic model, (a): for the radial

and angular directions, (b): for the vertical direction.

14

5.3. The Forward Dynamic Model with Feed-Back Control

Given the dynamics of a manipulator, the aim of robot arm control is to maintain

the dynamic response according to some prespecified performance basis. The

control problem is complicated by non-linearities, inertial effects, coupling forces

and gravity loading on the links of a manipulator [3]. In classical control, the

synthesis of a suitable controller is an iterative process that is generally performed

in simulation. A dynamic model of the system under investigation is required to

perform such iterative design procedure. Using the model, control laws or

strategies are designed to achieve the desired system response and performance.

The full rigid body dynamics of an n degrees-of-freedom manipulator can be

written as []]:

r = 7 (0 )0 + V(0,0) + G(0) (33)

where 0 and x are generalized co-ordinates and forces vectors respectively. 7(0) is

symmetric (nxii) inertia matrix of the manipulator, V(0,0) is //-dimensional vector

representing the Coriolis and centrifugal terms and n-dimensional vector, G(0)

involves the gravity effects. The simplest and most commonly used form of robot

control is independent joint PD-control, described by:

x = Kv(Od - 0 ) + Kp(ed - 0 )

Where K^ and Kv are position and velocity diagonal gain matrices respectively.

Qd and Qd are desired position and velocity vectors respectively. Two of the

control strategies used for high speed movements are [7].

1. PD control with position reference only:

x = - K v 0 + ̂ ( 0 , - 0 ) (34)

2. PD-control with position and velocity reference:

T = K v ( 0 < / - 0 ) + K p ( 0 r f - 0 ) (35)

15

The second option i.e. PD-control with position and velocity references has been

used in this report for its generality. Accordingly, equations (28), (29) and (19),

take the form:

(36)

(37).

z=-g + L '"' " '• " ' " J ( 3 8 )

Where 0, , Ch C2, Rj and R2 are SIMULINK blocks as defined in section S.\.Kpr,

Kpe and Kpz are positional gains and Kw, Kvg and Ky~ are velocity gains for r, 0 and

z co-ordinates. To simulate the equations (36) and (37), two additional subsystems

are defined as PD(r direction) and PD(0 direction), where:

PD(r_direction) = Kpr(rd -/•) + K.,(fd-f) (39)

PD{0 direction) = Kp0(0d-0) + KVe{0d-0) (40)

Equations (36) and (37) are now simplified to:

r i ., PD(r direction)r = C,[C2Rx+R2]0

2+ ^ - }- (41)m

0 = - 0 , [C2Rt + R2 }r0 + 0 , PD(0 direction) (42)

Equations (41) and (42) are simulated as a single system, while equation (38) is

simulated separately. The SIMULINK diagrams of these models are shown in

Fig. 4.

16

To WorXspace3

Integrator Integrator]

Product

(a)

Fen•CD

Sum9_r

Kv mSum11 step Fcn5

(b)

Fig. 4. The SIMULINK diagrams of the forward dynamic model with feedback

control, (a): for the radial and angular directions, (b): for the vertical direction.

17

6. SIMULATION EXPERIMENTS

Various simulation experiments concerning the forward dynamics, inverse

dynamics and control of INFOMATE have been performed and are described in this

section. Since the z degree-of-freedom of the manipulator is completely uncoupled

(equation (19)) rendering a fairly trivial solution, no vertical motion is allowed in

any of the simulation experiments. In all experiments length of radial arm, mass of

radial arm and mass of hub are kept equal to 0 6 m, 6 kg and 3 kg respectively.

The inertia of the vertical column and payload are varied as specified. In the

simulations of forward and inverse dynamic model, zero initial conditions are

assumed. The integration method used in all simulations is Runge-Kutta 5 with

step size equal to 10 millisecond. The simulation time is kept 3 second.

This section is subdivided into four parts. In the first part experiments on the

forward model in the open loop are described. The simulation results of the inverse

dynamic model are presented in second part. To see the effect on responses by

applying proportional gain controllers alone is described in third part. In the fourth

part a complete set of experiments is included to investigate the effect of applying

independent PD-controllers at the three joints of the manipulator.

6.1 Simulation Results of the Forward Dynamic Model

In this subsection, the effect of drive system dynamics and that of variation in

payload are studied. The implications of change in payload are analyzed by

applying a constant force to the radial link and a constant torque to the angular

joint of the manipulator. In the first experiment, the payload is increased from 1 kg

to 5 kg and plots of the acceleration, velocity and displacement are compared for

both radial and angular directions These plots are shown in Figs 5 and 6.

To study the absence of coupling when one of the radial link or the angular joint

are actuated, a zero torque is applied in angular direction and plots of the radial

acceleration, velocity and displacement are obtained and results are shown in

Fig. 7.

18

1.5

u 1

0.5

0.25 •E

I °21 0 15

i 01" 0.05

0.5

0.5

0.5

1.5time(s)

1.5time(s)

1.5time(s)

2.5

' p

S*^ mp=1kg

mp=5kg

2.5

f 0.4Ea>o

•5.0-2CO

•a

n2.5

Fig. 5. Plots of input force, acceleration, velocity and displacement in the radial

direction with payloads mp = 5kg and lkg, (J= 5kg-m:).

19

1.5

o0.5

0.5 1.5time(s)

2.5

5,0.2o

1 0.1QJO

CD

£0.5

0.5

0.5

0.5

1.5time(s)

1.5time(s)

1.5time(s)

2.5

mp=1kgmp=5kg

2.5

' • -

I1

1

•

mp=1kg* r n p = 5 k g

2.5

Fig. 6. Plots of input torque, acceleration, velocity and displacement in the angular

direction with payloads mp = 5kg and lkg, (J = 5kg-m2).

20

oL

$ 0.25

"E" 0.2o

I 0.15g 0.1CO

0.05

0.4 -

I 0.2>

§ 0 - 4Ea>

4 0.2

0.5

0.5

0.5

1.5time(s)

1.5time(s)

1.5time(s)

2.5

Fe = ONm

0.5 1 1.5 2time(s)

2.5

_ - — • — " *

2.5

-r-•—•—'

-

In another set of experiments, the effect of drive system inertias is determined for

the radial and angular directions. The dynamic model' of the manipulator including

the drives system inertias can be written as []]:

where 1(0) is inertia matrix of the manipulator, J denotes the inertia matrix of the

driving system. V ( 0 , 0 ) is the vector of Coriolis and centrifugal terms, G(0)

accommodates the gravity terms and T is the control torque vector generated by

the actuators. The inertia matrix4 of the actual drive systems employed in the

design of INFOMATE is:

J = diagonaf[\.37 0.11 12.00] (44)

Considering these values of inertias, the profiles of acceleration, velocity and

displacement are obtained both, for the radial and angular directions. Due to the

addition of drive system dynamics, the resulting cross-coupling effects between the

radial and angular directions (as reflected back to the motor shaft) become

relatively insignificant. Thus the resulting acceleration appears to be relatively

uniform. In order to further investigate the effect of drive system inertias, the

experiment is repeated for an assumed strongly weighted inertia matrix of the drive

system, given by:

J = diagonal[5.00 10.00 12.00] (45)

The plots with these three configurations are shown in Fig 8. In this case, the

effect of drive system inertias is readily discernible.

3 The details of the proof arc given in the Appendix 2.A The entries of diagonal matrix J of order 3 are computed as given in Appendix 2.

22

1.5

Q)O

0.5

0.25

is ° 1 5o 01o u '

005

. .

0 0.5 1 1.5 2 2.5 3time(s)

' — • _

i

•

i

^ ^ -nr-

without gear

— ->^ar inertias

h.gh gear met-a;

0.5 1 1.5 2time(s)

2.5

E

3

1.5 •

0.5 -

CO

1.0.2o

1 0.1ore

0.5

0.5

1.5time(s)

vrthoul pi

2.5

1.5time(s)

2.5

Fig. 8. Plots of input force and torque, with corresponding radial and angular

accelerations, for three values of gear inertias, {wp = 5kg and J = 5kg-m:).

23

6.2 Simulation Results of the Inverse Dynamic Model

In the inverse dynamic model, profiles of actuator forces are obtained for

acceleration as constant input. The experiment is performed for payload of 1 kg

and 5 kg. The plots of required actuator forces in radial and angular directions for

payloads of 1 kg and 5 kg are shown in Fig. 9.

6.3 Simulation Results of the Forward Dynamic Modelwith Proportional Gain Controller

In the forward dynamic model, proportional gain controllers are employed both in

radial and angular directions. This is accomplished by setting the gains, Kve and Kvr

equal to zero. The results are shown in Fig. 10.

6.4 Simulation Results of the Forward Dynamic Modelwith PD-Controller

In this subsection behavior of the manipulator is investigated when a PD-controller

is employed. To see the mutual dependence of radial and angular links, the

proportional gains of radial and angular directions are varied. The effect on

responses are shown in Figs. 11 and 12.

The perturbation in tuned controller responses due to varying payload at

end-effector both in the radial and angular directions is shown in Fig 13. To realize

the need of gain scheduling, the manipulator is actuated at two different initial

configurations both in the radial and angular directions. The outcome is shown

in Fig. 14.

To further analyze the behavior of manipulator, tracking performance of radial and

angular links to sinusoidal trajectories of frequencies 2rads/sec and lOrads/sec are

obtained. The results are shown in Figs. 15 and 16. The two most commonly used

tracking trajectories used in path planning are cubic polynomial and linear

segments with parabolic blends (LSPB). The tracking responses of the radial and

angular links to such trajectories with corresponding velocity reference are

obtained The results are shown in Figs. 17 and 18.

24

$ 0.4 -

"H-Q.3O

2 0.2

§0.1

3.5

CD

- 2 . 5

0.04

C

1§ 0.02

0)OO(0

1.5

0.5

0.5

0.5

0.5

0.5

1.5time(s)

2.5

1 1mp=5kg

mp=1kg ;

1.5time(s)

1.5time(s)

1.5time(s)

2.5

2.5

1

1 t 1

y

2.5

Fig. 9. Plots of input accelerations and corresponding required force and torque of

the radial and angular directions for two values of payloads, (J = 5kg-m-).

25

1.5

1 -,

I 0.5o_roDL nv> U-a

-0.5

-v

r _ — -

y' 2 N

1

- "

(1)Kpi=1(2)Kpi=10P) Kpr =100

• - - "

- S

u- -2>

-4

1-1.51-

I 1

f 05

10

5 • - -

O 0

-5

0.5

0.5

0.5

0.5

1 1.5 2time(s)

1.5time(s)

1.5time(s)

1.5time(s)

2.5

^

, — I . I X\ I—'2.5

/

" i

/

/

\

(1)Kpe=15(2)Kpe=15

V^JKpe = 150

2.5

/ ^^ - '' '\ \—. \

y2.5

Fig. 10. Effect on the radial and angular responses when proportional gain

controller is employed, (Aw = 0, Kvo = 0, mP =5kg, J = 5kg-mJ, = desired,

= actual).

26

0.6

•50 .25.to

-

(1) Kpr =100(2) Kpr =150(3) Kpr =200(4) Kpr =250

1.5

3f 1

!£ 0.5o

? 0

-0.5

1.5

re

it" 1a>Ea>u« 0.5 •a.in

o

-1

0.5

0.5

0.5

1.5time(s)

1.5time(s)

1.5time(s)

0.5 1 1.5 2time(s)

2.5

2.5

25

1

2.5

Fig. 11. Effect on the radial and angular responses due to variation of positional

gain, Kpr, (A"vr= 50, KPe= 150, Kvo= 50, mP =5kg, J= 5kg-m2, = desired,

= actual).

27

0.6

Ea>o•5.0.2w

s*

0.6

I 0.4 -asE

f 0.2CO

. . . . . _ L . . . . . . .

(i)mpsOIKo(2) mp = 1 .Okg

1.5

I '•T 0.5u§; o

-0.50

1.5

TO

H 1O)

E

S* 05 -Sin

42 2T3

-1

0.5

0.5

0.5

1.5time(s)

1.5time(s)

0.5 1 1.5 2time(s)

1.5time(s)

2.5

k

— i » — . i

2.5

yL L . . _ _ _ „

2.5

J- . . . . - _\7

1i

2.5

Fig. 13. Plots of displacement and velocity of the radial and angular directions for

three values of payloads, (Kpr = 250, KPo = 150, Kvr = 50, Kvo = 50, J = 5kg- m!,

= desired, = actual).

29

0 15

• I 0.05V)

•1 r" — i •

1.5"5*

t 1Q)

e01

toT3

4.552¥ 4CD

ea>« 3.5to

T3

0.5

0.5

0.5

1.5time(s)

1.5time(s)

1.5time(s)

2.5

/2.5

2.5

Fig. 14. Effects on responses due to change of initial configuration of the radial and

angular directions, (KPr = 250, KPe, - 150, Kvr = 50, A'v# = 50, mP = 5kg,

J = 5kg-m:, = desired, = actual).

30

0.5

ca>Ea>u

s

-0.50.5 1.5

time(s)2.5

0.5

.¥ 0uo

-0.5

-1

^ .

\ /

0.5 1.5time(s)

2.5

I1. ^ 0oo

-2

•£ Y «.

' ~ 1

7 \ ,/ \

/ ^

0.5 1 1.5 2time(s)

2.5

Fig. 15. Tracking responses of the radial link to sinusoidal trajectories of frequency

2rads/sec and lOrads/sec, (A> = 250, KPe = 150, AV = 50, K\-e = 50, mP = 5kg,

J= 5kg- mJ, = desired, = actual).

31

£ 0.5

0Eo

S--0.5

-1

2T3CO

Oo

-1

-2

T3

I- 0-5CD

I outo

-1

r

2

oo

-4

0.5 1.5time(s)

25

y/1 \

/

0.5 1.5time(s)

2.5

L./.A.L...M

0.5 1.5time(s)

2.5

\ /

cr\ /

\ /\ / \

^^

/ \

I \0.5 1.5

time(s)2.5

Fig. 16. Tracking responses of the angular joint to sinusoidal trajectories of

frequency 2rads/sec and lOrads/sec, (A> = 250, KPo= 150, A'v, = 50, Kvo = 50,

nip = 5kg, J = 5kg- m\ = desired, = actual).

32

I 0.4a>

4 0.2

— - -

0.2

1 0 . 1

0.5

0.5

1.5time(s)

1.5time(s)

2.5

//

/

. -. - - - — . L • . .

\ \

2.5

Fig. 17. Tracking responses of the radial and angular links to LSPB trajectory with

corresponding velocity reference, (Kpr =100, A!>̂ = 150, Afvr = 200, Kve = 75,

nip = 5kg, J= 5kg- m2, = desired, = actual).

33

I 0-4Ea>o

1 0 . 2CO

—H

" g O . 2 -

•§0.1-g

1 3TO

¥2Eo

CO

1.5

ofj.5

0.5

0.5

0.5

0.5

1.5 2time(s)

1.5time(s)

1.5time(s)

1.5time(s)

2.5

. .. r r -

2.5

1 —

, '

2.5

2.5

Fig. 18. Tracking responses of the radial and angular links to cubic polynomial

trajectory with corresponding velocity reference, (KPr = 100, KPo= 150, Kvr = 200,

Kvo= 75, nip = 5kg, J= 5kg- m2, = desired, = actual).

34

7. DISCUSSION

In this section, results of various simulation experiments are discussed and efficacy

O f t h e d e r i v e d m o d e l o f t h e m a t l i p u l . l f o l i l y i K l i l i i f s k c \ ; m i i i i r . l w i l h \ . i i i . < i r . i n | M i l ' .

and expected responses.

Figs. 5 and 6 show the acceleration, velocity and displacement profiles for the

radial and angular directions in response to constant generalized forces. The results

are obtained for two values of payload i.e. 1 kg and 5 kg. The profiles clearly show

a smaller acceleration in the case of 5 kg payload. This trend is justified by the

Newton's second law of motion (a=F/m). The effect of the change in payload on

acceleration profiles is more prominent for the radial direction than for the angular

direction. This is due to the fact that for the angular joint, structure of the

manipulator is symmetrical about r-axis while in the case of radial link the

symmetry is violated.

Fig. 7 shows the acceleration, velocity and the displacement profiles for the radial

direction when applied torque in the angular direction is kept equal to zero. In this

case, as might be expected, a constant acceleration is produced and no coupling

effects are observed.

The equation (20) shows that the inertia matrix is co-ordinate dependent for

INFOMATE. This co-ordinate dependency in the inertia matrix generates coupling

forces and consequently the accelerations are not uniform for constant applied

forces [6]. However, when geared motors of sufficient transmission ratios are

employed, drive system inertias usually become dominant and thus overshadow the

cross-coupling effects among the joints [5]. This results in comparatively uniform

acceleration profiles for constant applied forces. Therefore, when geared motors

are used, the coupling effects are suppressed and each joint can be controlled

independently treating any remnant coupling as disturbance. Model based control is

no longer required in such cases Fig 8 shows response profiles for INFOMATE

with and without drive systems inertias respectively. It appears that although the

35

selected actuator systems for INFOMATE do not completely eliminate the coupling,

remaining effects are not substantial and could be ignored when the manipulator is

used in a constrained work envelope.

Fig. 9 shows an application of the inverse dynamic model. The model is used to

obtain actuator force and torque profiles for payloads of 1 kg and 5 kg for constant

acceleration inputs. As expected, larger forces appear for the 5 kg payload.

The main objective of studying and investigating the dynamic model of the

manipulator is to device a suitable control mechanism. As discussed earlier,

independent joint control is reasonably justifiable with the selected actuators.

Therefore, independent controllers at each of the three joints are used. First as

shown in Fig. 10 the manipulator is tested with proportional controller for various

gains. Results depict that the system could not be stabilized with proportional gain

controller. System exhibits instabilities even at very small gains. This situation

necessitates the use of some damping. Therefore PD-controller is employed as the

derivative type controller adds damping to the system and also improves the steady

state error by permitting the use of high proportional gains [10]. The step

responses for the radial and angular joints for various proportional gains are given

in Figs. 11 and 12 which show that the system is now easily stabilized. By

employing the PD-controller, system becomes more sensitive to changes and thus a

quick response is achieved. As earlier stated system exhibits oscillations even at

lower proportional gains hence Pi-controller cannot be used as it will intensify the

oscillations. It is also observed that by varying the KPr in the range 100 to 250, no

significant coupling effect is observed in the response of the angular joint. Similar

results are obtained for the radial link when KP0 is varied from 100 to 250.

Therefore, in this range of gains no strong coupling exists between the links. The

controllers are tuned by hit-and-trial method.

The effect of change in payload for fixed gains is shown in Fig. 13. Again the effect

is more apparent for the radial link because of the reason stated earlier. It can be

seen that for radial link oscillations are elevated for larger payloads because of

36

more inertia. Furthermore, the rise time for the larger payload is greater. This is

because controller takes more time to approach the desired step for larger value of

payload. This clearly shows that the radial controller needs to be tuned (or gain

scheduled) for various payloads to achieve the required performance.

The need of gain scheduling for the radial controller is further emphasized if step

input response profiles for different initial conditions given in Fig. 14 are

compared. However, for the angular joint, the gain scheduling is unnecessary as

shown in the figure. This is because the angular joint is symmetric about z-axis.

The tracking performance of the manipulator to sinusoidal trajectories is shown in

Figs. 15 and 16. It appears that the manipulator follows the desired trajectory of

2 rads/sec with a small discrepancy. However, for the desired sinusoidal trajectory

of 10 rads/sec, lags in the phase and amplitude become quite enormous. This

shows that for fixed gains, the manipulator can be made to manoeuvre only with

limited speed. Thus, the controllers need to be re-tuned if the rate of change in the

desired trajectory is to be altered excessively.

In tracking applications of manipulators, a velocity reference is often used. Figs. 17

and 18 show tracking responses of the radial and angular links to LSPB and cubic

polynomial trajectories when corresponding velocity reference is also given.

Tracking responses of radial and angular links to LSPB trajectory and cubic

polynomial trajectory show that the tracking errors in both cases are insignificant.

8. CONCLUSIONS AND FUTURE ISSUES

The Lagrange's method provides explicit state equations for robot dynamics. These

equations can be used to analyze and devise advanced control strategies.

Moreover, in order to simulate and study the behavior of a robotic manipulator in

the design phase, such models are inevitable.

37

It has been shown in section 7 that the dynamic coupling effects of manipulator

joints are very small compared to the inertia of the drive systems. This enables each

drive system to be treated separately as if it were moving under constant load.

Most of commercial robots, in fact, are of this type and that is why independent

PD-controller at each joint for robot control is a common practice in the

industry [7]. In the case of direct drive robots, the effects like backlash, friction,

and compliance incorporated due to the gears are eliminated. However, the

non-linear coupling among the links is now significant and the dynamics of the

motors become more complex. In that case model based control techniques are

necessary that require an inverse dynamic model of the manipulator [8]. The

inverse dynamic model is used in variety of position control strategies. Two such

approaches are computed-torque and feed-forward control [4]. It has been also

shown in section 7 that need of gain scheduling for radial link is indispensable

regarding to initial configuration and payload variation.

Future research efforts will be focused mainly on the control applications and

improvement of the developed model. Significant tasks include.

development of force control because the position control is only suitable when

a manipulator is following a trajectory through the space. When any contact is

made between the end-effector and the manipulator's surroundings, position

control may not be enough [9];

design of self-tuning adaptive controller;

improvement of model by adding the complete modelling of the selected drives

in the model and

simulating the model in state space.

38

APPENDIX 1

A.I.I Salient Features of SIMULINK

SIMULINK is a program for simulation of dynamic systems. Being an extension to

MATLAB, SIMULINK annexes a number of features required for the simulation.

In SIMULINK, an available system formulation is implemented using blocks in the

provided libraries. The time response of an implemented model is obtained and

analyses are performed. The model can then be improved iteratively to achieve the

desired response.

Models are created and edited by a mouse. After model definition, analyses can be

performed either by choosing options from the SIMULINK menus or by entering

commands in MATLAB window. The time response can be viewed while the

simulation is running and many of the model parameters could be modified on-line.

The final results can be made available in the MATLAB workspace after a

simulation terminates. The following steps are performed to develop a SIMULINK

model.

1. Start MATLAB.

2. Invoke SIMULINK by entering a simulink at the MATLAB prompt.

3. Make the block library the active window and choose New... from the File on

the menu bar to bring up a new SIMULINK model window.

4. Double-click on the libraries in the standard block library to open them up.

Drag the necessary blocks into the new model window.

5. Use the mouse to connect the blocks.

6. Double-click on the blocks to open their dialogue boxes, and enter the correct

parameters values.

7. Choose Group from the Options to group blocks into subsystems.

8. Save the model to a file.

39

A.1.2 Brief Description of SBMULINK Blocks

|~ \ The Step function block provides a single transition between two values occurring

at a specified time.

> The Constant block injects a constant value into a system.

The Scope block display the input and output signals.

The Graph scope block plots to the MATLAB graph window and can be used as

an improved version of the Scope block.

The Sum block produces as output a summation of a given number of inputs. The

number of inputs, as well as their polarity, is specified by a string of signs [+,-].

The string ++-+, for example, specifies a four-input system and an output equal to

U1+U2- U3+U4.

The Gain block multiplies its input by a scalar gain, which produces its output.

The gain may be entered as any legal MATLAB expression. If the input to the

block is a vector, the gain may be entered as either a scalar (in which case, it is

specified to each input) or as a vector (in which case, each element of the input

vector is multiplied by the corresponding element of the gain vector, producing a

vector of outputs).

The Integrator block produces as output the integral of its output over time. An

initial condition can be specified.

The Product block produces as output the product of its inputs. The number of

inputs is controlled by its single parameter.

s The Sine wave block accepts three entries: amplitude, frequency(rads/sec),andphase(rads). Its output is amplitudexsin(frequency x time + phase).

40

yout

The Fen. block performs a user-specified operation. The operation is defined by

any well formed mathematical expression which can contain numerical constants,

MATLAB variables, the input symbolized by u, binary operators ( + - • / ) , unary

operators (+ -), relational operators ( = < > < = >= !=), functions (sin, cos, acos,

hypot etc.), and parentheses.

The To Workspace block defines a variable to which the oulpul of the simulation

is sent. The signal entering this block is stored in a matrix in the workspace. This

variable is not available from MATLAB until the simulation has ended. The

maximum size of this variable is limited to the size specified for memory

considerations. If n samples are specified, only the last n samples are stored.

A.1.3 Blocks Used in the Simulation

"71m*

*

Sum

Block Ci

Fen

Block G

Block Cs

1 u'u

Fen

1

I SumFcni

Block R,

E h - •fTTrg m i).12 ^,

•

•->

"» m sun,LJJ

Fen—>

•

out 1

Product

-H3out 1

Suml

41

Block 0.

Q

Product

Block'

J

42

APPENDIX 2

A.2.1 Equations of Motion of INFOMATE Including the Drives

Equation (20) can be written as

V(0,0

where

(A.2.1)

1(0) =m 0 00 J + j(r) 00 0 w..

(A.2.2)

V(0,0) =

0

(A.2.3)

G(0) = 0 (A.2.4)

Dynamics of the drive system are given by [1]:

(A.2.5)

(A.2.6)

43

where x - (Tj,T2,T3)r is the control torque generated by the ith actuator. And

J -y, o oo J2 oo o J3

(A.2.7)

B,*oi ° °0 bm 0yO20 0 b03

(A.2.8)

Here J, and ba denote the inertia and the damping coefficient of the ith driving

system. If damping is ignored then equation (A.2.6) can be written as

-f+T (A.2.9)

Since the robot dynamics is directly connected to the dynamics of the drive system

through the reduction gears. So using equations (A.2.1) and (A.2.9)

(1(0) + J ) 0 + V(0 ,0 ) + G(0) - T

which is the required result.

A.2.2 Calculations of Inertias of the Drive Systems

DC motors have been used in the design of the manipulator. Rotor inertias of the

three motors are [12]:

Rotor inertia of radial drive = 1.16xlO"7kg-m2

Rotor inertia of angular drive = 1.16xlO"7kg- m2

Rotor inertia of vertical drive = 1.86 x 10"*kg - m2

The link of manipulator is driven by motors through a gear reduction, a; in other

words the link velocity is I/a times the actuator velocity. The motor inertia

44

reflected through the gear reduction is increased by a2. So inertias of the drive

systems after the gear reduction are

J r = ( 1 7 . 2 )2 x l . l 6 x l ( r 7 = 3.43xlO"5kg-m2

^ = (989) 2xl . l6xl0" 7 = 0.11 k g - m 2

y , = ( 1 4 ) 2 x 1.86x10"* = 0.36 xlO"3 k g - m 2

In the radial direction the translational motion is achieved with the help of lead

screw. Therefore inertia term of radial drive should be converted into mass. The

torque is the product of the force, Fand the mean radius, rm of the lead screw [11].

r=Frm (A.2.10)

The translational and angular velocity of the lead screw are related as

where/? is the pitch of the lead screw. Also

v = /w6 (A.2.12)

The Newton's second laws for the rotational and translational motions are

x = M (A.2.13)

F = mv (A.2.14)

where J and m are moment of inertia and mass of the body, and F and x are applied

forcie and torque respectively.

Comparing equations (A.2.10) and (A.2.13)

CO

45

Using equation (A.2.14) the above equation can be written as

mvrm

0)(A.2.15)

Now using equation (A.2.12)

or

(A.2.16)

(A.2.17)

The pitch and the mean radius of the radial lead screw are

p = 2.50xl0"3m

rm = 0.01 m

Therefore for the radial direction mass is given by using equation (A.2.17)

mr = 1.37 kg

For vertical direction lead screw pitch and mean radius is

/> = 3xlO-3mrm = 0.01 m

Thus the mass for vertical direction is

m w = 12.00 kg

Thus the matrix (A.2.7) now become

J =

1.37 0 0

0 0.11 0

0 0 12.00

46

REFERENCES

[I] M. Brady (Ed.), Robotic Science, The MIT Press, Cambridge, 1989.

[2] B. Friedland, Control System Design, McGraw-Hill Book Company, New York,

1987.

[3] Fu, K. S., Gonzalez, R. C. and Lee, C.S.G., Robotics: Control Sensing, Vision and

intelligence, McGraw-Hill, St. Louis, 1987.

[4] A. J. Koivo, Fundamental for Control of Robotic Manipulators, John Wiley and

Sons, Singapore, 1989.

[5] M. A. Syed, Modelling and Control of a Parallel-Actuated Robot Manipulator,

PhD. Thesis, University of Wales, UK, 1991.

[6] C. P. Neuman and V. D. Tourassis, "Discrete dynamic robot models", IEEE Trans.

System, Man and Cybernetics, vol. SMC-15, no. 6, pp.193-204, 1985.

[7] C. H. An, C. G. Atkeson and J. M. Hollerbach, Model-Based Control of a Robot

Manipulator, The MIT Press, Massachusetts, 1988.

[8] K. Yousuf-Toumi, "Design and control of direct drive robots - A survey", in

Oussama Khatib, John J. Crag and Thomas Lozan-Pwrez (Eds.), The Robotic

Review-\,Mn Press, Cambridge, 1989.

[9] J. Craig, Introduction to Robotics, 2nd Edition, Addison-Wesley Publishing

Company, New York 1989.

[10] K. Ogata, Modern Control Engineering, Prentice Hall International Inc., 1990.

[II] J. E. Shigley, Mechanical Engineering Design, 5th Edition, McGraw-Hill Book

Company, New York, 1989.

[12] Motor Catalogue of MTNIMOTOR SA, CH-6981, Croglio, Switzerland.

47

Distribution:

0 Chairman, PAEC

0 Senior Member

0 Member Power

0 Director PINSTECH

0 Project Director, ICCC

0 Director, CNS

0 Director, DCC

0 Project Director, CIAL (Karachi)

0 Director, KINPOE (Karachi)

0 Head SID, (PINSTECH)

0 Director, CTC

0 Head,RPSS

0 Library, ICCC

0 Authors