>
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