Optimal control of robotic manipulators


Material Information

Optimal control of robotic manipulators
Physical Description:
vii, 218 leaves : ill. ; 28 cm.
Guez, Allon, 1952-
Publication Date:


Subjects / Keywords:
Robots, Industrial   ( lcsh )
Manipulators (Mechanism)   ( lcsh )
Electrical Engineering thesis Ph. D   ( lcsh )
Dissertations, Academic -- Electrical Engineering -- UF   ( lcsh )
bibliography   ( marcgt )
non-fiction   ( marcgt )


Thesis - University of Florida.
Bibliography: leaves 212-217.
Statement of Responsibility:
by Allon Guez.
General Note:
General Note:

Record Information

Source Institution:
University of Florida
Rights Management:
All applicable rights reserved by the source institution and holding location.
Resource Identifier:
aleph - 021901043
oclc - 10083644
System ID:

Full Text







To Ilana, Jonathan and David



I would like to express my gratitude to my advisor, Dr. Alexander

Meystel, for the guidance, encouragement and invaluable professional

support and for his approaches to problems which considerably influenced


Special thanks are also to the Center for Intelligent Machines and

Robotics (CIMAR) at the University of Florida and its director, Dr. D.

Tesar,for the help in selection, formulation and successful completion

of the work described in this dissertation.

I would like to thank Dr. R. L. Sullivan, Dr. E. Chenette, Dr.

P. P. Khargonekar, Dr. M. Rao and Mr. M.Thomas for the helpful dis-

cussions and positive contributions they have made.

I also thank the typist Mrs. Mary Good for the excellent cooperation.

Computing was done utilizing the facilities of the Northeast

Regional Data Center of the State University System of Florida located

on the campus of the University of Florida in Gainesville.

This work was supported by the Electrical Engineering Department

at the University of Florida. ,



ACKNOWLEDGEMENT .. . . . . .... . . . .... iii

ABSTRACT . . . . . . . . . . . .. vi


1.1 The Problem of Time Optimal Positioning with State
Variable Inequality Constraints . . . . . 5
1 .2 Background . . . . . . . . . . . 6
1 .3 Dissertation Organization and Main Results . . .. 14


2.1 Manipulator Dynamic Model . . . . . . .... 16
2.2 Actuators Dynamic Model . . . . . . .... 19
2.3 Augmented Model . . . . . . . . .... 22


3.1 The Decoupling and Linearizing Transformation ..... 27
3.2 FLDT for a Single Link Manipulator (Example 1) . . 38
3.3 FLDT for a Double Link Manipulator (Example 2) . .. 45


4.1 Mathematical Statement of the TOPC Problem . . .. 58
4.2 Statement of a Restricted TOPC Problem with FLDT . 61
4.3 TOPC with SVIC's for Triple Integrator System ..... 64
4.4 TOPC for a Single Link Manipulator (Example 1) . 81
4.5 TOPC for a Double Link Manipulator (Example 2) . . 87
4.6 Comparison with Approximated Bang-Bang Voltage Control 90

V REALIZATION OF THE FLDT . . . . . . . . .. 104

5.1 Sensitivity of Output Motion to Quantized Voltage. . 105
5.2 Asymptotic Stability for Imperfect FLDT . . .... 153


6.1 Controller Structures . . . . . . . ... 164
6.2 Conclusions and Future Work . . .. . . . ... . 173




III COMPUTER LISTINGS . . . . . . . . . . ... 183

REFERENCES . . . . . . . . . . . . . . 212

BIOGRAPHICAL SKETCH . . . . . . . . . . . ... 218

Abstract of Dissertation Presented to the Graduate Council
of the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Doctor of Philosophy



Allon Guez

April, 1983

Chairman: Dr. Alexander M. Meystel

Major Department: Electrical Engineering

This dissertation addresses the problem of finding the desired

optimal trajectories and the optimal control inputs for a rigid multi-

degrees-of-freedom robotic manipulator in a positioning mode. For a

general, highly coupled and nonlinear dynamic model an analytical control

law is derived and structures of controllers are suggested.

The equations of motion of a multilink mechanism and its actuators

are unified into a common dynamic model. A nonlinear feedback trans-

formation which globally linearizes, decouples and places the poles of

the closed loop system is derived. Each of the decoupled subsystems

represents a single degree of freedom of the manipulator motion. This

transformation is realized by on-line computations based on the measure-

ments of the manipulator's state variables and reference to a look-up

table, and requires no matrix inversion.

The set of uncoupled subsystems in a form of triple integrators

allows a solution of various optimal control problems for each degree

of freedom independently.

The time optimal positioning problem of a multi-degrees-of-freedom

robotic manipulator with bounded velocity, acceleration and jerk is

posed in a form which guarantees uniqueness of the control and state

trajectories. The solution is demonstrated via single and double link


The look-up table implementing the feedback transformation intro-

duces non-analytic quantizationn) errors. The time optimal trajectories

of a double link manipulator are shown to be rather insensitive to this

type of error. For a class of errors an existence theorem of the linear

feedback controller is proven which guarantees closed loop asymptotic

stability at the state space origin. Finally, outlines of various

controllers structures are discussed.

This dissertation is the first application of the following principles:

- Instead of optimizing the real system, a fictitous system should be

introduced which is more convenient for optimization and can be

obtained from the original system, for example by a state space


- The state space feedback transformation can be realized for de-

coupling, linearization and pole-placement simultaneously.

- The programmed open loop control for a deterministic system can be

devised with the assumption that deviations are compensated by

a separated subsystem for "control in-the-small."



This work deals with the time optimal positioning control of robotic

manipulators. These are general purpose,programmable machines possessing

certain anthropomorphic characteristics. The robot manipulator system

is defined as "a programmable, multifunction manipulator designed

to move material, parts, tools or specialized devices through variable

programmed motions for the performance of a variety of tasks." In the

literature the terms mechanical arm, artificial hand, robotic arm, open

loop articulated chain and manipulator are used interchangeably.

Although practical examples in this dissertation involve serial

mechanisms, much of the theory developed applies to other programmable,

multifunction chain structures, designed to provide purposeful mechanical


The robot manipulator system is composed of N rigid links, con-

nected in series by kinematic joints, into an open loop kinematic chain.

The joints are rotary or linear (sliding), with the first joint fastened

to a fixed support (base), whereas the N-th link functions as the hand,

or "end-effector," which usually has a gripper for performing a task.

Each joint is assumed to have only one degree of freedom (DOF); i.e., each

joint defines a joint axis around, or along which, a link rotates or

slides. The number of joints defines the number of degrees of freedom of

the robot manipulator. The case of multiple rotational or combined

sliding/rotating degrees of freedom joints can be modelled by an equiva-

lent chain of zero masses and lengths of single degree of freedom joints.

The standard dexterity or articulation requirement is six DOF, three

DOF for linear and three DOF for angular locating of the end effector

with respect to a fixed reference, in which case the robot manipulator is

called nonredundant. If N, the total number of DOF is greater than

six, the manipulator is redundant. The analysis attempted here is to be

equally related to redundant as well as nonredundant manipulators.

The description of the manipulator's state can be provided either as

a vector specifying the various joint angles or as a position and orienta-

tion of the end effector. The former representation is called configura-

tion and is said to be a representation in joint variables (coordinates)

space or joint space. The latter is a representation in cartesian space.

The locus of time dependent points which the manipulators' joints trace

is called a trajectory.

In the analysis of robotic manipulators, two main problems can be

formulated. In the first, the desired position and orientation of the

end effector are given. The joints' positions which will locate the hand

at the desired point in the cartesian space with the specified orientation

are to be determined. For six DOF manipulators (nonredundant) the number

of solutions to this problem is finite and may be obtained (except at

singular points, [S-5]) by inversion of the geometrical transformation

which relates the joint variables to the hand's position and orientation.

For a redundant spatial N-DOF manipulator there may be infinitely many

solutions. Throughout this work, however, we assume that an appropriate

solution for this problem exists and is feasible.

The second problem is one of manipulators dynamic control: find the

structure of the controller and the controls (inputs) which provide the

movement of the manipulator system from its present configuration to the

desired one, such that constraints on the hand's and joint's path, velocity,

acceleration and jerk as well as actuators losses and energy consumption

are met. If the motion execution is optimal with respect to some given

"goodness criteria" then it is called an optimal control problem.

The basic tasks performed by industrial manipulators may usually be

reduced to two types, regardless of the complexity of the manipulation task.

In the first, sometimes called point to point task, the cartesian

path of the manipulator's hand or joints is not specified and may fre-

quently be arbitrarily chosen. Typical point to point applications include

"pick and place" activities, machine loading and unloading, spot welding,

etc. The second type of tasks performed by manipulators is sometimes

called tracking or "contouring." This type includes applications such as

paint spraying, continuous welding processes, some metal -cutting operation

and grasping objects moving on a conveyor. In this mode the desirable

trajectory of the motion is preassigned and is an essential part of the

manipulator's task definition.

In the point to point task type, the trajectory leading to the

desired terminal point is frequently of no importance (except of obstacle

avoidance requirement), e.g., the motion of an empty gripper towards a

tool or object. In this type of task, optimization procedures may be

applied to the control synthesis: minimization of time to reach the

desired state, optimal velocity distribution along the path, minimization

of energy used and dissipated by actuators which impose constraints to

the control, etc. The present work is aimed mainly at this class of tasks.

An observation made by many researchers (e.g., [B-2], [B-5], [L-4], [T-2]

and [V-1]) is that in the point to point task the manipulator motion should

be partitioned into two modes: "large," and "small." The first, which is

referred to as slewingg mode" [V-1], "coarse motion" [B-5], "transport phase"

[B-2], or "gross motion" [L-4], is the main part of the motion made as the

manipulator transports objects or tools from one point to another in the

work space. At this part, fast motion for higher productivity is to be

emphasized whereas accuracy seems to be less important. The second mode

which is denoted as "fine motion" [L-4], [B-5], or "terminal phase" [B-2],

is the part of the task where the system is exerting a force, manipulating

or having contact with external objects, with a relatively slow motion (if

at all) of most parts, performing "rendezvous," "accurate-stop" or fine

insertions. Since, in this work, we are dealing primarily with time

optimization of the manipulator's motion, the slewing mode (or the coarse

positioning) is naturally our main concern, which actually implies the

possibility to base our research upon a deterministic model.

In terms of the complexity of the control analysis, the coarse and

fine modes of motion differ significantly. If the system is to be moved

slowly, or.by varying one joint at a time (as in the fine mode), then

coupling torques, such as the Coriolis and centrifugal terms, are minimal,

and a simple solution to the control problem for each of the joints can

be found independently. However, in the slewing mode the motion is fast

and the entire manipulator configuration changes significantly during a

single cycle. For productivity, all joints should be moved simultaneously

and the coupled structure of the manipulator dynamics makes this problem

highly nonlinear and difficult. The conventional positioning techniques

currently implemented in industrial robots are no longer applicable.

1.1 The Problem of Time Optimal Positioning
with State Variable Inequality Constraints

At this point we do not have the mathematical tools to present a

mathematical statement of the major problem discussed in this dissertation.

This will be done in Section 4.1, after the results of Chapter II, the

manipulators dynamic model, and Chapter III, the decoupling transforma-

tion have been obtained. Nevertheless, for clarity of the presentation,

we define in this section the control problem in general terms, as


Given a N-DOF manipulator system with its N actuators (DC motors

are assumed), where all the relevant parameters of the load and manipu-

lator are known, and where the initial and final manipulator configura-

tions are given, find the actuators inputs (voltages) to be applied, the

controller structure, and the manipulator state trajectories to change

the manipulator configuration from the initial to the final one, in

minimum time, without exceeding the preassigned maximum velocities,
accelerations and jerks for all joints. This will be called the time

optimal positioning control (TOPC) with state variables inequality

constraints (SVIC) problem.

It should be clear from the definitions above that we are dealing

with slewing mode or coarse positioning, of point to point tasks, since

only in this case can the path be freely chosen by the controller.

#Constraints of this type were shown to affect the higher harmonic
content of the (driving) command signals and reduce the vibratory transient
response in the flexible system [T-3], [G-1], [M-2] and [M-3].

It is our intention to solve the above stated problem using methods

which remain applicable to a larger class of problems, e.g., TOPC problem

with actuator losses and energy consumption constraints; time optimal

point to manifold control and so on.

1.2 Background

In this section we give a brief survey of previous works which are

related to the important aspects of the problem. The following topics

are discussed: mathematical modelling of multilink manipulators, global

linearization and decoupling of manipulatros including actuator dynamics,

control and optimal control of robotic manipulators and similar mechanical


Various approaches were used in deriving the equations of motion for

the type of mechanical linkages described above. The pioneering works by

Uicker [U-1] and later by Bejczy [B-1] and Hollerbach [H-3] were based

on the Lagrangian mechanics approach. Orin et al. [(0-1] gave a recursive

algorithm using the Newton-Euler formulation. Thomas and Tesar [T-4]

presented an alternate derivation using the kinematic influence coeffi-

cients. Horowitz and Tomizuka [H-4] derived the dynamic model using

the Gibbs-Appel equations of motion.

In all of those derivations, the resulting dynamical model (for

control purposes) of the N-DOF, serial manipulator, has the following

form, which is almost exclusively used in robotic control literature.

(See for example [T-4], [H-4], [B-3], [D-1], [G-5], [K-4], [C-2], [Y-2],

[V-2], [P-3], [G-4], [S-2] and many others.)

T = J(O) 8 + f(0, 0)

where 8 is the vector of joint coordinates, T is the vector of

applied torques, J(8) is the inertia-matrix anf f is the vector of

velocity related and gravity terms. Notice that the input control is

assumed to be the torque generated by the actuators as seen in the joints'

shafts. These torques may be generated by electrical, hydraulic or

pneumatic motors, in all cases however the torques cannot be isntan-

taneously assigned thus the above model is an approximation.

The N-DOF manipulator may be assumed to be affected substantially

by the dynamics of the N actuators located at its joints. The

actuator's dynamics must be accounted for if an optimal trajectory con-

trol is to be executed. This is especially the case when a fast motion

is desirable, where the actuators natural frequencies are significant

and, thus, the driving torques can no longer be assumed instantaneously


Very few works in manipulator control were found to include a

dynamic model for the manipulators' drives. Paul [P-l] assumes that the

manipulator driving torques are instantaneously controllable, but he

utilizes approximation curves to account for the loading effects and

friction of the actuators. Cvetkovic and Vukobratovic [C-2] and

Vukobratovic and Kircanski [V-2] assume a linear time invariant third

order model for electric motors which are the manipulators' drives.

As shown inChapter II the dynamic model for the manipulator/

actuators system is a multi-input, multi-output, highly coupled, non-

linear, and high order (18 state variables for six DOF) set of

differential equations. This made any attempt to derive analytical con-

trol laws for even the simplest tasks impossible. Moreover, even on-

line numerical integration techniques, which had to be repeated for each

control cycle, due to system and load variations, were shown to be an

excessive computational burden and to require a huge amount of memory.

Thus, the need for model simplification techniques such as decoupling

and linearization became essential for fast and efficient control of

robotic manipulators.

Various linearization methods were used by many authors in the

control of robotic manipulators. Kahn [K-1] derives a linear model for

a three DOF manipulator for finding a time suboptimal controller, by

deletion of all second and higher order dynamics in the full model. He

found, however, that an average value of the velocity related (nonlinear)

terms had to be added to the model in order to guarantee suboptimality.

This means that some apriori knowledge of the state trajectory of the

system had to be assumed.

Other linearization techniques (see Pieper [P-4], Whitney [W-I],

Paul [P-1], Vukobratovic [V-2], Yuan [Y-2], Takegaki [T-l] and many

others) assume that the nominal trajectories of the system are given in

advance. The dynamic model is linearized about those trajectories,

yielding independent joint reference commands for each actuator. In

practice, however, the manipulator system, due to the variations of

real working conditions, does not follow the nominal trajectories.

This degrades the control performance and may even induce unstable

responses. To overcome this problem Paul [P-l] utilized precomputed

compensations of nonlinear effects as additional inputs. Vukobratovic

[V-2] designed asymptotic regulator, feedback controllers, for each

actuator, to guarantee stability about the nominal paths, and Yuan [Y-3]

suggested a feed forward compensator to improve the performance of the

linear independent joint controllers.

Decoupling of the manipulator/actuators system to a set of lower

order subsystems, was also attempted by various authors (see for example

Hemami [H-2], Freund [F-2], Guez [G-4] and Horowitz [H-4]) as a method of

simplifying the manipulator control tasks.

The literature on decoupling of nonlinear systems is rich but not well

organized. Porter [P-5], found sufficient conditions for the existence

of a decoupling state feedback transformation. Later Majumdar and

Choudhury [M-1] developed necessary and sufficient conditions for decoupl-

ing and diagonalization by state feedback for nonlinear system, which are

linear in control input. Freund [F-3] extended the results obtained by

Porter, to the case of non-purely dynamic systems. (See [K-2] for summary

of additional results.)

For locomotion and robotic manipulator systems, however, the use

of nonlinear feedback transformation was first suggested for lineariza-

tion, Hemami [H-2], and later for decoupling, Freund (F-2], Guez [G-4],

Horowitz [H-4]. In these works, however, actuator dynamics is not

included in the system model (this reduces the model order by 33%) and

the decoupling transformation is derived while assuming that the joints

driving forces and torques may be instantaneously assigned. Moreover,

the nonlinear feedback transformation derived in (F-2] requires the on-

line inversion of a NXN state dependent matrix, where N is the number

of DOF of the manipulator. This is an extremely difficult task for the

control sampling rate which is generally suitable for manipulative tasks.

Also, in [H-4] the gravity and load related forces were deleted in the

derivation of the linearizing and decoupling transformation.

Other decoupling techniques, such as the linear feedback asymptotic

regulator by Vukobratovic [V-2] and the open loop, feedforward compen-

sator by Yuan [Y-3] were suggested. These methods, however, assumed

given nominal trajectories about which the model is linearized, and

therefore represent only local decoupling. Another, approximate

decoupling method is used by Koivo and Guo [K-3] where the first three

joints of a six DOF manipulator are associated with the translational

motion of the end effector, and the last three joints with its orienta-

tion. The coupling terms between the variables of these two groups are


As will be shown in Chapters II and III, for a large number of DOF,

the feedback decoupling may involve the evaluation of thousands of

additions, multiplications and trigonometric operations. On-line

computations of those may therefore take many minutes for a single

iteration and turn out to be impractical for real time control. This,

however, is not the only possibility for the feedback decoupling exe-

cution. Various combinations of evaluations of analytical expressions

and references to table look-up are possible as a method of computa-

tional burden reduction in each cycle.

In 1975 Albus [A-1] suggested the Cerebellar Model Articulation

Controller (CMAC) as a new approach to manipulator control. In his

work he describes how the various control signals for a multi-DOF

manipulator are obtained by referring to a table (in a computer's

memory) rather than by mathematical solution of simultaneous equations.

CMAC combines input commands and feedback variables into an input vector

which is used to address a memory where the appropriate output variables

are stored. In [A-1] and later in [A-2] Albus describes the CMAC memory

management algorithm which makes it possible to store the necessary data

in a physical memory of a practical size.

In fact, as was shown by Raibert ER-1], by appropriate paramateriza-

tion of the equations of motion for the manipulator, divergent procedures

for control involving analytical expressions and table look-up can be

examined. Each procedure represents a different point on a continuum

characterized by the indicator P, the number of parametric variables.

As P increases storage requirements increase, but computational com-

plexity decreases. As shown by Raibert [R-1], for a N-DOF manipulator,

with 3N variables (namely, the joint positions (N), velocities (N)

and accelerations (N)), of which P are held constants as parameters

whereas the other 3N-P variables are represented by R resolution cells

(3 )
each, about N arithmetic operations and RP storage cells are

required for evaluation of the equations of motion.

A variety of other control structures have also been developed.

Through the application of variable structure theory, control of a

simple manipulator was accomplished by discontinuous feedback using

sliding modes which produced a somewhat undesirable discontinuous con-

trol signal. In [D-2], a model reference adaptive controller is proposed

which forces each manipulator joint to behave as an underdamped, second

order system, with the actual equations of motion not being utilized

in the design.

In spite of the importance of time optimal (fast) motions of the

manipulator for increased productivity, which has been recognized by

many (see for example [P-3], [K-1], [L-4], [Z-1], [V-1], [G-3], [C-2]

and [L-3]), very few works are known to be performed in this field.

This is, of course, due to the highly nonlinear and highly coupled form

of the differential equations which govern the system. As a result, very

few attempts to apply optimal control methods in manipulator control

were reported. Kahn [K-1] presented a suboptimal numerical solution to

a minimum time point to point control for three DOF manipulator. The

dynamic model used by Kahn did not include the actuators dynamics, and

was linearized by neglecting second and higher order terms in the equa-

tions of motion, some velocity dependent terms however were approximated

by their estimated values. In spite of these simplifying assumptions

the resulting control law was still dependent on the boundary conditions.

Paul, et al., [P-3] present a linear programming algorithm for finding

the near minimum time path of the manipulator end effector. It deals

with path planning by designing a set of time intervals for the transi-

tions among a sequence of preassigned points in the cartesian space for

the end effector. The motion between each pair of points is assumed

along a straight line segment with constant velocity. The total traveling

time is minimized while observing velocity and acceleration inequality

constraints. The manipulator dynamics are not included and since the

path is assigned in advance it does not solve the time optimal posi-

tioning problem. Later this method was extended by Lin et al. [L-3]

to fitting of cubic polynomials with velocity, acceleration and jerk

constraints in planning a minimum traveling time trajectories with

given sequence of points. Lynch [L-4] derives a sequential mode (one

axis at a time) minimum time for two axis manipulator. In a sequential

mode coupling between the various axes is significantly reduced, the

execution time, however, is much larger than in the one obtained in

simultaneous motion of all axes. In [G-2] various numerical methods

are utilized to solve a three DOF mechanism model for a gymnast per-

forming a kip-up maneuver on a horizontal bar in minimum time. Again,

no actuator dynamics are involved in this case and the optimal control

has to be computed for each new set of boundary conditions.

Other works in the area of optimal control of robotic manipulators

deal with minimizations of quadratic functionals of the state variables

and the applied torques such as [T-5] and [Y-1]. However, only second

order linear models are used and the actuators dynamics are not accounted


A related problem, the optimal control of container cranes

(minimizing a swing of the load as it moves along its trajectory) is

described in [S-1], again the actuators are not modelled and the

solution is nonanalytic, i.e., has to be recomputed for each task. The

only reference found where the actuators dynamics are accounted for in

the minimization of the energy dissipated by them was [C-2]. It assumes,

however, no interaction between the manipulator joints and is therefore


In summary, it is found that due to the high order, highly non-

linear and coupled form of the manipulators equations of motion, the

direct application of control theory to the problems of optimal control

of manipulator is impractical. Therefore, methods of global lineari-

zation and decoupling of the dynamic model are essential for the

successful solution of the optimal control problem.

1.3 Dissertation Organization and Main Results

In Chapter II the dynamic models of the manipulator and actuators

are defined and augmented into a single state space model.

In Chapter III we derive the feedback decoupling and linearizing

transformation for the N-DOF manipulator system including its actuator.

This transform does not require the inversion of a matrix and it

accounts for all torque terms in the dynamic model, including load and

gravity. It is demonstrated for a single and double link manipulator.

In Chapter IV, the time optimal positioning control problem is

accurately stated and solved taking into account various task configura-

tions. The solution is analytic and need not be repeated for each task.

Again, it is simulated for a single and double link manipulator and

then compared with bang-bang voltage control.

In Chapter V the approach taken by our work is shown to be

appropriate to slewing mode positioning via a simulation of a quantized

decoupling transform, indicating a low sensitivity of the output motion

to this type of error. Additionally, an existence theorem is proved

regarding the asymptotic stabilizer for a manipulator system controlled

by imperfect decoupling feedback.

The dissertation is concluded with suggested controller structures,

conclusions and topics for future work, which are described in Chapter VI.



In this chapter the dynamic rigid and deterministic model for the

N-degrees of freedom manipulator and its actuators is selected. Section

2.1 describes the rigid and deterministic dynamic model for a serial

N-DOF manipulator and defines the mathematical properties and the

physical meaning of each term in that model. Section 2.2 describes a

linear third-order time invariant model for the N permanent magnet DC

motors which are assumed to serve as the manipulators' actuators. In

Section 2.3 both dynamic models are combined into a single model, for

which common state variable and differential equations' vectors are


The following major assumptions are made in this chapter and will

be used throughout this work.

- The manipulator is an open loop chain (serial) of N rigid bodies.

- All kinematic and dynamic parameters of the manipulator and its

actuators are known (given constants), thus, a deterministic model

is assumed.

- The manipulators' joints are either rotary or linear (sliding).

- The actuator's dynamic model is linear and time invariant.

- The external loads applied to the manipulator are known and can be

expressed as smooth differentiablee) functions of the joints dis-

placements, velocities and time.


2.1 Manipulator Dynamic Model

The robot manipulator system is composed of N rigid links,

connected in series by kinematic rotary or linear joints, into an open

loop kinematic chain. (See Figure 1.) As mentioned above the dynamic

model for the manipulator system is given in Equation (2.1.1) and may

be described as follows:

T = J(0, t) + f(e, W, t)


f(0, W, t) = (0, W, t) +

WT G (0) W

T 2
wT G (0) w

w) G (0) (S

(2.1 .2)

and where

S= (01, 92, . ) is the joints linear or angular

tions vector, 9eR ;

(I, W . ( N) = (81 2 . ) is the j

linear or angular velocity vector, weR ;
* T
= (w1 2, . N ) T is the joints linear or angular

accelerations vector, LeR ;

T= (T T2, . T ) is the vector of generalized dr

forces, (forces or torques acting at sliding or rot

Joints respectively) TRN
joints respectively) TsR





(2.1 .1 )


\ ,,"End E)
Joint \ e2 Effector"1 N

\ f

\ f>

\ i

- - - - - - - --__ _ _ _ .,

Figure 1. N-DOF Manipulator

J(8, t) is the NXN, "effective" inertia, symmetric matrix function

of the joints' coordinates vector 9, J( ): RN+ RNXN ;

S(8, w, t) is the vector of gravity and external loads related

torques/forces, and is smooth vectorial function of the

joints coordinates vector 8, the joints rates, w and

7N+1 N
time {(8, w, t):R -2N+ R ;

G i(), i = 1, 2, . N are the NXN symmetric matrices repre-

senting the velocity dependent torque weighting matrices and

are functions of the joint coordinates vector 9,

G (8): RN RN i = 1, 2, . N.

It should be noted that the components of the matrices
A Gi
J(8,t) and G (8) for i = 1, 2, . N are products of trigonometric

functions (sines and cosines) of the various joints' coordinates.

Also, since the external loads were assumed to be smooth functions of

8, w and the gravity and load related torques vector, 2(8, w, t) and

J(8) and Gi(8) are all differentiable with respect to all their

arguments. This will be exploited later in Chapter III in deriving the

feedback decoupling transformation.

Notice also that the so-called "effective inertia" matrix, J(8, t)

at any given time, t, is positive definite for all GER and all

possible combinations of sliding and rotary joints. This property of

J(8, t) is proved in [H-4], but is not surprising since the total

In general 2 is a function of the external loads (torques and
forces) and gravity. Since these loads were assumed to be known functions
of 8, w and t, it is always possible to obtain the form 2(8, w, t)
by substitution.

kinetic energy of the N-DOF manipulator, which is always a positive

quantity, is given by wT J(O, t) w, and is zero only for zero angular

velocity, i.e., J(9, t) is positive definite by definition. (See [T-4].)

The various components of the above described matrices and vectors

possess an important physical interpretation. The i-th diagonal entry
of J(8, t), j..(ii(, t), represents the effective inertia (including

the load, which may vary in time) at joint i; the off-diagonal entry
of J(O, t), j ik(,0 t), represents the coupling inertia between joints

i and k. Similarly, Gk(8) represents the centripetal force at

joint i due to velocity at joint k; Gi () represents the force
jk -
at joint i due to velocities at joints j and k; and f. (8, W, t)

represents the torque developed at joint i due to gravity and external


2.2 Actuators Dynamic Model

The N-DOF manipulator is assumed to be driven by N actuators

located at its joints.

Throughout this work, we assume (using an approach similar to the

one taken in [C-2] and [V-2]) that the actuators are permanent magnet,

DC motors, with armature current control. The i-th actuator is acting

Linear models of other types of actuators may be used without
loss of generality of the work that follows.

on the i-th joint through a gear reduction box and may be described as


A.. + B.w. + J.w. + T. = r. KT I.
1 1 i 1i 1 i T.i
i = 1, 2, . N (2.2.1)
L.I. + R.I. + riKV = V.
ii ii iV. 1 1


V. is the i-th motor input voltage;

T. is the i-th joint load torque as given in (2.1.1);

I. is the i-th actuator's armature current;

r. is the i-th gear-box reduction ratio (r. > 1);
1 1

J. is the i-th actuator rotor inertia referred to the output

shaft, (J. = J r.); (J is the moment of inertia of
1 M. i M.
1 1

the i-th motor.)

B. is the i-th actuator's viscous friction coefficient;

A. is the i-th actuator's compliance coefficient;

R. is the i-th armature ohmic resistance;

L. is the i-th armature inductance;

K is the i-th actuator torque constant;


K-. is the i-th actuator back e.m.f. constant.

In addition to the above given constant parameters, the rated torque

and speed, the maximum acceleration and the maximum speed are given.

Notice that although all the motors' parameters were assumed known

(given), they are in fact, the result of a design procedure which selects

the appropriate actuators for the given manipulator and the desired

volume of tasks.

The N equations (2.2.1) may be rewritten in a vector form as


J + T = C I A B w

_=F W + F 1 + L- V


T is the vector of joint loading torques as defined in Section


V is the vector of input voltages with components V.;

I is the vector of armature currents with components I.;

J is a diagonal matrix with J.. = J. > 0 for i = 1, 2, . N,
ii 1

and is therefore positive definite;

C is a diagonal matrix with C.. = r. KT. > 0 for
11 1 T.
i = 1, 2, . N, and is therefore positive definite;

A is a diagonal matrix with A.. = A. > 0 for i = 1, 2, . N;
B is a diagonal matrix with B 1 B > 0 for i = 1, 2, . N;

B is a diagonal matrix with B.. = B. > 0 for i = 1, 2, . N;
1i 1
L is a diagonal matrix with L.. =L. > 0 for i = 1, 2, ... N,
11 1

and is therefore positive definite;
r.1 KV
r i K
F is a diagonal matrix with F = 1L. <0 for
1 1 L i.
11 1

i = 1, 2, . N and is therefore negative definite;

F is a diagonal matrix with F = - <0 for i = 1, 2,... N
2 2.. L.
11 1
and is therefore negative definite;

and where a diagonal matrix M is defined as a matrix M with

m.. = 0 for i # j i, j = 1, 2, . N.

Equation (2.2.2) defines a combined dynamic model for the N

actuators used. In the following section we use Equations (2.1 .1 ),

(2.1 .2) and (2.2.2) to obtain an augmented model for the N-DOF manipu-

lator with its actuators.

2.3 Augmented Model

In this section we combine the equations of motion for the N-DOF

manipulator with the dynamic model of its actuators. This augmented

model is required for a clear problem formulation and efficient deriva-

tion of an analytical control law.

Using state space notation, let the state variables for the

augmented system be the joint coordinates, the joint velocities and the

actuators' armature currents. That is, define

X1 =e

X = (2.3.1)
:-2 --

x =J1
3 -


x (X: ,I) X2, X3 )

In terms of X X-
-1may be written as
may be written as

and X3, Equations (2.1 .1), (2.1 .2) and (2.2.2)

J(X I, t) i2 + f(X1, X2 t) = T

f_(x_11 x_2)

G1 (Xii x2

T 2
X G (X ) X2

2 G (1 ) X

-2 + T- =- 3 1- -2
X3 = FX2 + F2X3 + L V

Combining (2.3.4) and (2.3.2) we obtain

[J + J(X, t)] X2 + F (X, X 2, t) = CX AXI BX2

or in a state variable form

Xi =2

i2 = g(X, t)

i = F + F + L-1
X-3 =FIX-- +F2X-- +L V





t) = I_(XI, X2, t) +


g(X, t) = J(X_, t)-1 [CX3 f(X1, X 2, t) AX BX_2] (2.3.6)


J(Xi t) = J(X_1, t) + J (2.3.7)

Thus, Equations (2.3.5), (2.3.6) and (2.3.7) represent the complete

state space model for the N-DOF manipulator with its actuator.

It should be noted that since J and J(X1, t) are positive

definite, so is J(Xi t) and is therefore nonsingular and the state

space model exists for any configuration (V eR ) of the manipulator.

See Figure 2 for a block diagram of the augmented system.

Figure 2. Block Diagram of Augmented System



In the previous chapter it was shown that the dynamic model for

the manipulator/actuators system is a multi-input, multi-output, highly

coupled, nonlinear, and high order (18 state variables for six DOF)

set of differential equations. This made any attempt to derive analytical

control laws for even the simplest tasks impossible. Moreover, even on-

line numerical integration techniques, which had to be repeated for

each control cycle, due to system and load variations, were shown to be

an excessive computational burden, (see Section 1.2) and to require a

huge amount of memory. Thus, the need for model simplification techniques

such as decoupling and linearization is essential for fast and efficient

control of robotic manipulators.

In this chapter, a nonlinear state feedback transformation is

introduced to the full manipulator/actuator system via the actuators'

input controls. This transformation takes into account the actuator

dynamics, gravity and load related terms, requires no matrix inversion,

and provides global decoupling of the N-DOF system into N linear

subsystems. Each decoupled subsystem represents one DOF of motion

(link) of the manipulator with the position, rate and acceleration of

the DOF as its state variables which preserves physical insight to

the control problem and the link's jerk as its input. The dynamics

of each DOF of the manipulator may be arbitrarily (and independently

of the other DOFs) assigned by a proper choice of a linear feedback.

Furthermore, the parameters of the transformation, do not need to be

readjusted for different tasks (paths) as is the case in linear control

schemes and all axes may have the same dynamics for all configurations

and parts of the manipulator's state space.

We commence in Section 3.1 by the derivation of the feedback

linearizing and decoupling transformation (FLDT) based on the dynamic

model presented in Chapter II. Sections 3.2 and 3.3 demonstrate the

application of the FLDT for single and double link manipulators.

3.1 The Decoupling and Linearizing Transformation

In this section a nonlinear state transformation is derived such

that in terms of the new state variables the manipulator/actuator

system is linear and decoupled to N subsystems. Each subsystem repre-

sents a single link (DOF) in a form of a triple integrator dynamics.

The equations of motion of the augmented system were given in

Section 2.3 and are restated here for convenience.

2 = g: (, t) (3.1.1)

= XF + FX3 + L-1 I
-3 1z-12 2:-3-


g(X, t) J -1-(Xi, t) [C_3 A-1 E-2 w(x-1 x )

__Xi, X2,* t)]

T 1 T 2 (3.1.2)
W(x1, 2)= X G(X )x_ x G(X )X, ,

X2 G (X ) x2T

and where X = (X X2, X) with X, X and X as defined in
Z-1 :-2'.3 wit '- -3
Equation (2.3.1).

In order to obtain a decoupled set of N triple integrator sub-

systems, let the following nonlinear state transformation be introduced:

Y = +(X, t) (3.1 .3)


+(X, t) [X1, X2, g(X, t)]T (3.1.4)

and where

Equivalently, Equation (3.1 .3) may be written as follows:

yi = x

y2 = x = x (3.1 .5)

Y3 = g(X, t) = X2

i.e., Y i, Y2i and Y3i represent the position, rate and acceleration

of the i-th joint, respectively.

The equations of motion of the system, in terms of the new state vector,

Y, are found by applying Equations (3.1.1) and (3.1.2) to (3.1.5). We


#Y is a state variables vector since the mapping g(X) is invertible
with respect to X_3. (J (X ) and C- exist).

Y =2 = =12


-1 ~ -2

Similarly, for Y

-2 2 = g (X t)

(3.1 .6)

i-2 = Y3 (3.1 .7

Finally, for Y3 we obtain
= ~d -
-3 = g t) = -d [J-1 (X t)] (3 0AX BX2 W(X1 X2)

at ~-3 -1l -2 -1
-(1 'I X2' t)] + j1(x1', t) [CX3 2 W(X)

i(X, t)] (3.1 .8

Equation (3.1.8) is the only state variable equation which involves

the input vector, V (through X3 ). Thus, the state transformation may

be performed via state feedback by a proper choice of the input function.

Now, in order to obtain a triple integrator form, we must have

Y3 = z (3.1.9

where Z is the new control input vector, ZeR Thus Equations

(3.1.8) and (3.1.9) define the state feedback as follows:





z = d [J-1 t)] [CX- -EX W(X, )
-Z- dt -1 -1 -2 --1' -2

J -1 (_ t) [cX 1AX BX -W(X)

h(X, t)] ,

using Equations (3.1 .1) and (3.1 .2) again yield
d -
Z= -d[J- (X1, t)] [CX3 AXl BX2 W(X-1 X2)

-1 1/
(XI, X2, t)] + J (X1, t) [CFX2 + CF2X3 +CL V
1-2 23 -2

2 B g(X, t) W(X) !(X, t)] (3.

The feedback transform V( X,Z, t) is obtained from Equation (3.1.10)

as follows:

V(X, Z, t) = LC-1 J(x1, t) {Z -d[J1 t)] [-3 AX

-t WL -1 AL
-B_2 W(X1, X2) -_(X1, X2, t)]} + LC-I [AX2

+ B g(X, t) + W(X) + i(X, t)] -L[F lX + F X13] (3.

1 .10)


d -1
t-[J (Xi, t)] =

i(x, t) = X

N 6[j-1 (X,[ t) ]
z x2J
ax. .

a[J-1 (X_1,t)]
+ at

al (X x t)
:--X_ g(X, t)
a-2 j

+ (QL1' a2' t)
+ -- at


- f(I1 x2' t)] +

X- 2 L t)


(gx) Gi (X.) X
T ) G2 1x-1 ) -2

j ( T) G 2(1) 2

gTx) GN(x ) X2

OG1 (X1 )

aG2 (XI )

aG (X)

(3G N X1


T 1
X G (X )


w(X) =

T 2
X G (X ) g(X)

X G (X_ ) g(X)

2j -1

X2j] 2

x2j I-2

(3.1 .12)

T 'N
x2 j^

x j1

i2 IJ'

and where we define

A is the Jacobian matrix
= of the MX1 vector a with
respect to the NX1 vector

Matrix of the partial
derivatives-of a matrix
NXM with respect to a
scalar argument b.


O b N *





! (b lA




ab. '

ab. '


C l 1
6b. '

ab. '

ab. '



ab. '


Clearly, in the general case, the feedback transform, V(X, Z, t)

as defined in Equations (3.1.11) and (3.1.12), is a function of all

state variables, including X3, the armature currents. In the new

state space, however, (Y), the armature currents are not directly

represented. It is therefore desirable to represent the feedback

linearizing/decoupling transformation (FLDT) in terms of Z, t and

Y only. That is measurements of the joints' positions, rates and

accelerations only are required, and not the currents. This is done as

follows. From Equations (3.1.2) and (3.1.5)

CX = AX + EX + W(X x,) + 2(X X t) + J(X t) g(X, t)
-3 -1- :-2 -2- --1 :-2 :-1 -


CX-3 = AY + BY + W(Y, Y) + (Y', Y,2' t) + J(Y1, t) Y (3.1.13)
:-3 :-i Z-2 -7- -= l-2 --1 -3-

Notice that C and F are both NXN diagonal matrices and therefore

commute (i.e., CF = F2C), thus

CF X = F [AY + BY + W(Y 2 Y-) + Y2, t)
2::~-3 2 ~~1i -z-2 -~ 11' 72\ R- ( Z-1 2'

+ J(Y t) Y] (3.1 .14)

Also recall that for a nonsingular, square matrix, A(t), the

following relation holds:

d -1-1 d A-1
d- [A-1 (t)] = A (t) [A(t)] A~(t) (3.1.15)

By applying Equations (3.1.15), (3.1.14) and (3.1.13) in (3.1.11)

we obtain after simple manipulations the FLDT as a function of the new

state vector Y and time, t, as follows:

V(Y, Z, t) = LC- {J(Y, t) Z + W(Y) + i(_Y, t) Fw(_, _)
:-i 2--i~' 2
F 2(-YIl' 42' t) 12AYl + [A 5-C F-2 B-2

+ [B- F J(Y 1,' t) + J(Y., 2' t)] 3 }

J(YI Y,2' t) =

a(Yi, t) aj(Y- I t)
aI jj Y2 + a t

Y3 G (Y ) Y2

.3 G (_1) .2

GN(-1 ) -2

T [



a G1 *Y1 )
a y1 j

aG2 (Y-)
ayI j

N 8GN(Yi )
j=i sY-j

iY2 G (Y) Y3

I2 G -i ] 3


K 2

Y2j 1 2

Y2j1 Y2

(-I ) -3

1.(Y, t) a (, 2' t)
(- t) -42 +

a'(.-' -2' t) of( + y1, t)
8aY2 3 + "at (3.1 .17)


(3.1 .16)

w(Y) =


Equations (3.1.16) and (3.1.17) define the FLDT. Notice that in view

of the assumptions in Chapter II, C- exists and is readily available

(since C is diagonal, see (2.2.2)), and V-(Y Y2 t) was assumed

differentiable, thus V(Y, Z, t) in (3.1.16) exists for all t, and

all points in the state space YeR Moreover, the FLDT employs only

immediately available system matrices such as J(Y1, t), W(Y Y2) and

their time derivatives; no matrix inversion is required.

In the case that a stationary (time invariant) external load is

applied, the inertia matrix J and the load torque vector 9 become

time invariant. This simplifies the evaluation of the FLDT which

becomes time invariant as well.

To summarize, when the FLDT as given in Equations (3.1.16) and

(3.1.17) is applied to the system (Equation (3.1.1)) we obtain the

following system (which from now on will be denoted as the equivalent

open loop system, EOS)

Y Y (3.1 .18)
-2 -3
yz =z1 31*8

-3 -.

In a scalar form Equation (3.1.18) may be rewritten as

Y = Y
ii 2i

Y = Y
2i 3i (3.1.19)

Y3i = Z. i = 1, 2, . N ;

which, as required, represents N decoupled triple integrator sub-

systems with the i-th input, Z., representing the i-th joint's jerk.

Figure 3 describes the manipulator/actuators system, the FLDT and

the new equivalent, open loop system EOS.

Notice that in Equation (3.1.18) the EOS is unstable since its

characteristic polynomial may easily be shown to be S3N = 0. Never-

theless, the EOS is reachable since the 3NX3N matrix M, where

0 0 IN 0 0 0 1N 0 0 1N 0 0

M 0 0 0 IN 0 0 0 IN 0 0 IN 0
\N N N

I., 0 0 0 I, 0 0 0 0 0 0 IN
N N ,N

has full rank (3N) and, therefore, according to the pole placement

theorem the EOS may have arbitrarily assigned dynamics by a proper

choice of a linear time invariant feedback. In fact, due to the de-

coupled structure of the EOS, and as can be seen from Equation (3.1.19),

similar arguments hold for each i-th subsystem which is stabilizable

and by a proper choice of ai.., j = 1, 2, 3 in (3.1.20)

Z a Yi + ai2Y 2i + a3Y3i (3.1.20)
any characteristic polynomial may be selected for each i-th subsystem

as well as the EOS.

(a) I


ON- Y 31

| -------- - y21

-Y ~3N


( b 1 N

(b). S -


Figure 3. (a) FLDT; (b) Equivalent Open-Loop System

3.2 FLDT for a Single Link Manipulator (Example 1)

In this section we apply the FLDT developed in Section 3.1 to a

single DOF case, i.e., a DC motor with a link attached to its shaft.

(See Figure 4.) This example will, of course, demonstrate linearization

only since coupling is irrelevant in a single DOF case.

The equations of motion for the system are derived in Appendix I

and are given by (see Equation (A1-4))

Xl = X2

X = J- [CX3 (X )] (3.2.1)

X3 = f1X2 + f2X3 + L-1 V

where, as in Chapter II

X- is the link's angle measured with respect to the vertical

(downward) axis;

is the link's angular velocity;

X is the armature current;

V is the applied armature voltage;

J is the total moment of inertia reflected to the motor

2 2
output shaft (J = JM r + md );

m, d are the link's mass and length respectively;

- g is the gravity acceleration;

KT, KV, R, L are the motor's torque constant, back emf, armature

resistance and armature inductance respectively;

- r is the speed reduction ratio of the gear box;


Figure 4. A Single Link Manipulator

and where

R -K r
C = r KT; (X ) = mgd sin(X1 ); f = 2 ; f. =

Following the notations of Section 3.1 let

YI = Xl
Y1 =x1
Y = X (3.2.2)
A -1
Y = J [CX3 (X )]

and evaluate the FLDT as given in Equation (3.1.16) to this case, noting

that W, A, B and J vanish,

V(Y, Z) = LC- [JZ + mgd Y2 cos(Y ) f2 mgd sin(Y 1)

CfY2 f2 J Y3] (3.2.3)

Applying the FLDT given in Equation (3.2.3) to the System (3.2.1)

we obtain the EOS as follows:

Y = Y
1 2

Y2 = Y3 (3.2.4)


Y- d [J-1 (CX (X )] = J [CX _(X )] = J-1 [Cf Y
3 dt 3 1 3 1 1 2

+ f JY3 + JZ + I(Y ) CfY 2 f jy ;(Y)] = J1 [JZI (3.2.5)

i. e.,

Y3 = Z

Equations (3.2.4) and (3.2.5) describe (as expected) the EOS as a

triple integrator system, with Z(t) as the new input, which physically

represents the angular jerk of link.

It is interesting to notice that, although the EOS state variables

Y13 Y2 and Y (i.e., the angular position, rate and acceleration of

link) form a linear dynamical system (a triple integrator); the arma-

ture current, X3, which is not an EOS state variable is nonlinearly

dependent on Y and Y as follows (see Equation (3.2.2):

X = C- [JY + mgd sin(Y )] (3.2.6)

For this purpose the step response of the EOS, and the corresponding

FLDT (armature applied voltage) and armature current variation were

computed in Experiment 1 and shown in Figure 5 as follows.

Experiment 1. A FORTRAN program (EXP1) which simulates the system

given in Equation (3.2.1) was written (its listings are given in

Appendix III). The input voltage was evaluated via the FLDT (3.2.3)

for the DC motor PMI MC 19P with a jerk step of 40 rad/s and zero

initial conditions and the following parameter values:

m = 10, g = 10, d = 1, R = 0.6, L = .0015, r = 10, K = .25, K = .25.

Figure 5 describes the step responses of the armature voltage (V)

and current (I) and the links angular velocity, acceleration, and position


Figure 5. Step-Response of the Equivalent Open-Loop System


Figure- 5. Continued

Figure 5. Continued

respectively. It demonstrates that the angular velocity, acceleration

and position step responses are, in fact, those of a triple integrator

system, whereas, V and I illustrate the expected nonlinear behavior

due to gravity.

3.3 FLDT for a Double Link Manipulator (Example 2)

In this section we apply the FLDT derived in Section 3.1 to a

double link planar manipulator driven by two DC motors. This example

demonstrates linearizstion as well as decoupling of the general N-DOF

manipulator system since it contains all major dynamical components

such as gravity, Coriolis and centripetal terms.

Figure 6 describes the double link system configuration. The

equations of motion for this system are derived in Appendix II using the

Lagrange method. We reformulate them here using the notations of

Chapter II.

The equations of motion are given in Appendix II (Equations

(A2-6) and (A2-7)) as follows:

T =D en9 +D e^ +DO+2 +D^
T1 D 11e1 + D12e2 + D(O 2 + 201e2) + D1

T = D 8 + D22 2 D(e2) + D
2 12 1 22 2 1 2


2 2
D = (m + m d + m d + 2m d d cos(2)
11 1 2 1 2 2 2 12 2

D = m d2 + m d d cos(2)
122 = 2 212 2

D =md2
22 2 2


Figure 6. Double Link Manipulator

D = md1d sin(e2 )

D 1 (m1 + m2 ) gd1 sin( ) + m2gd2 sin(e1 + e 2)

D = m2gd2 sin(81 + 2 )

and where

and second

and length


g is gravity; TV, T2 are generalized torques at the first

joints respectively; and mi, d1 and m 2, d2 are the mass

of the first and second links respectively.

the notations in Chapter II, let

D FD11 D121
J(X ) =
S LD12 D22J

I G^x) X^ Wl
(X T= 1 X
:-1 1 I 2
xI GD(XJ ) x wm

0 D -D 0
G 1X) = G2 (X =

D 1
D .

X = e

X (eI

T (T
^_- 1'



and notice that D, D. and D. for i, j = 1, 2, are functions of
1 1)
X Equation (3.3.1) may be rewritten as follows:
T = J(X ) x + f(x x
-1 :-2 -:-1 :-2
(X1, x ) = 2 (X )L + W(x1, x2).
-i--^ -1- -.-1' -2

The DC motors are modelled in a method identical to that in Section

2.3, i.e.,

JX2 + AXL + EKL + T = C1

x3 = FX2 + FX3 +L' V

where X3 is the vector of armature currents (2X1), V is the vector

of applied armature voltages (2X1), and where J, A, B, C, FI F2 and

L-1 are as defined in Section 2.3. Following Section 2.4, the

augmented model for the two link with actuators is finally given by

i =x2

x2 = g(x) (3.3.3)

-3 = F12 + F23 + L-1V


g(X) = J-1 (X )[(3 W(X ,X) .(X_) AX BX- ]
:--1- :-3 --?1-' 2 --1 "11 L2

Using the transformation

Y =X
Z-1 :-1

Y =X
-2 = _x-2
Y g(X)

we obtain the FLDT by applying Equations (3.1 .16) and (3.1 .17) as



D (m1 + m )

L2(Y_ ) : 2
% 2 gd^2( Y21

2D[ Y22 Y32

-2D[Y21 Y31

gd1 Y21 cos (Y11 ) + m2gd2 (Y21 + Y22) '

+ Y22) cos(Y11 + Y12)

+ 3 22 +Y21 Y32] + [22 +2

] D[Y22 ]

= -m2 d d2Y22

D = -42d1d2

D 12

D11 =-2m2d1d2Y22 sin(Y12

D12 = -m2dd2Y22 sin(Y12)


cos(Y1 2 )

sin (YI) 2



and the FLDT voltages may be explicitly given as follows:

L R R( R,

+ 1 1 + 1 + TI B1 Y21 + [1 + L11 + (D11 + J 1 ) 1 31

+ [D12 + 1D12] Y32} (3.3.4)

L 2 D Z +R +L+2 +R +2AY
2 T2V2 R R2
+ + J +[ +A +-D +B I) D I

12 121 2 +3 12 + L 123 1

R R'

+ r D +[ 22()l (3.3.5)
12 2 2 222 L 2 22 + 212 Y3

where all the parameters used are as defined in Section 2.3, and where
Z and Z2 are the angular jerk input of the first and second link


When V1 and V2 as obtained in Equations (3.3.4) and (3.3.5)

are applied to the system (3.3.3) we obtain the EOS as two, decoupled

triple integrator systems in the form of Equation (3.1.19).

As in the previous section, an experiment was conducted to evaluate

the step responses of the EOSs (shown in Figures 7 and 8) as follows:

Experiment 2. A FORTRAN program (EXP2), which simulates a double link

manipulator system driven by two DC motors (PMI MC23S and PMI MC19P

for the first and second links respectively) (Equation (3.3.3)), was

written. (Its listing are given in Appendix III.) The input voltages

were computed via the FLDTs (3.3.4) and (3.3.5) for a jerk step of

20 rad/S and zero initial conditions for both links, with the

following parameters:

m = m2 = 10 [kg]

d = d = 1 [m]

KI = KT = 0.5 [V S/RAD], [mn/A]

= K = 0.25 [V S/RAD], [Nm/A]
2 2

R, = 1 [01

R2 = 0.6 [(]

L = 0.00025 [HI

L = 0.00015 [H]

r1 = r2 = 35

J = 0.002 [kg-m2 ]

J = 0.001 [kg-m2]

The step responses of the armature's voltage and current and the links'

angular velocity, acceleration and position for the first and second

links are shown in Figures 7 and 8 respectively.

As can be seen from Figures 7 and 8 the EOS has a step response of

two decoupled triple integrator system. Notice also that as in

Experiment 1, all nonlinearities are "absorbed" in the armature's

currents (Ii 12) and voltages (Vi V2). By the fact that 11 is

different from I1 and V is different from V2 whereas 0 = 2,

Wl = W 2, W1 = W2 we observe that all coupling effects are also

perceivable through the currents and voltages only.

First Link's Step Response

Figure 7.



Figure 7. Continued




0 ---4 .. -4 4 4 I-
---- ----- .... - --

0.00 0.10 0.20 0.30 0.40 0.50 o.o60 0.70 0.10 0.9 .0

Figure 7. Continued


, jf



S..... ........

0 9 '* ~- - -- -, - - 9..

Fu L R o
-l I* '


819 I

0 ; ; .. J

0.O0 0.30 0.20 0.30 O.130 0. 0.60 0.70

Figure 8. Second Link's Step Response


,0 O.60 r0. 70 Q. gO


Figure 8. Continued

Figure 8. Continued




Having obtained the form of the feedback linearizing and de-

coupling transformation in Chapter III for the system model we are now

ready for the solution of the time optimal positioning control problem.

Section 4.1 presents the mathematical statement of the time

optimal positioning control for the N-DOF manipulator with state

variable inequality constraints and the difficulties encountered in

the direct solution. In Section 4.2 the FLDT derived in Chapter III

is applied, then the optimal control problem is restated and discussed.

The solution to each one of the resulting N uncoupled subsystems is

presented in Section 4.3 for various cases. Finally in Sections 4.4

and 4.5 a single and double link manipulator examples are solved


4.1 Mathematical Statement of the TOPC Problem

The time optimal positioning control problem for the N-DOF manipu-

lator may be stated as follows.

Given the system defined in Equations (2.3.5), (2.3.6) and (2.3.7)

with the following initial conditions:

x(0) =x10

x2 (0) = o_ (4.1 .1 )

(0o) = c- [f(x10, o0, o) + AXI] ;

find the minimum time tf, and an input voltage vector V(t),

te[0, t] I to be applied to the system, such that

I(tf) = Xf

X2 (t f) = 0 (4.1 .2)
-2^ f --

X 3(tf) = C-1 [f_(X_1f, 0_, 0) + AX_ ]

and for all te[0, t f]

IX -< W (4.1.3)
x2i M.

1X2il <

d 2x2(t) < A. (4.1 .5)

dt 1

for i = 1, 2, . .N.

Notice that the manipulator is at rest (static) in both boundary

points (Equations (4.1.1) and (4.1.2)) since X2 and -- are both

zero at t = 0 and t = tf. Thus, the problem is to find the optimal

control for time optimal point to point positioning, while observing

inequality constraints on the velocity, acceleration and jerk of the

manipulator's motion.

Let us now demonstrate the various difficulties encountered when

a direct solution via the maximum principle is attempted.

Form the Hamiltonian, H (following [A-3])

H = 1 + AX X + X g(X, t) + T[F X + F X + L'1 VI (4.1 .6)
-1 :m-2 -2 -- -3 1:-2 2:-3 -

The adjoint variables X (t), 1 )-(t) E R must satisfy

d -i (H
dt i = 1, 2, 3 (4.1.7)

According to the maximum principle, the optimal control V (t),

the optimal trajectory X (t) and the optimal adjoint variable's
vector X (t) = (A_ 2, 3 ) T must satisfy the necessary conditions

(2.3.5), (4.1.7) and the relation (4.1 .8)

H(X (t), *(t), V (t), t) H(X *(t), A*(t), V(t), t) (4.1 .8)

for all V(t) ER and t e[0, t f] whenever the inequality constraints

(4.1 .3), (4.1 .4) and (4.1.5) are strictly satisfied. On a constrained

arc, however, (i.e.,when either one of the inequalities becomes an

equality) it is not at all clear how to replace relation (4.1.8).

#In certain cases, e.g, a scalar state inequality constraints,
necessary conditions on a constrained arc are given in [S-4] and
[M-4]. For the general case, however, no theory could be found.

At this point the following observations should be noted.

The number of constrained arcs and the time instances at which

we enter to and exit from constrained arcs are not known and no

theory is available for finding them analytically.

The set of necessary conditions to be satisfied by an extremal

trajectory on a constrained arc is not known (no theory).

Even on an unconstrained arc with given points of entry and

exit, the resulting two point boundary value problem is a high

order, (36 variables for a six DOF manipulator) nonlinear, highly

coupled problem.

There may well be many time optimal trajectories, i.e. uniqueness

is not at all guaranteed.

In view of the above, it is concluded that the direct attempt to

solve this problem is a formidable task, thus, in the next section we

introduce a new statement of the TOPC problem, utilizing the FLDT

derived in Chapter III, which enables the unique solution of a

restricted TOPC problem.

4.2 Statement of a Restricted TOPC Problem with FLDT

In this section we employ the results of Chapter III by assuming

that the N-DOF manipulator system was linearized and decoupled into N

triple integrator subsystems each representing a single DOF of the

manipulator by application of the FLDT. Then the TOPC problem is re-

stated and shown to be a restriction on the original TOPC problem

stated in Section 4.1.

As shown in Chapter III, when the FLDT given in Equation (3.1.16)

is applied to the system described in Equations (2.3.5), (2.3.6) and

(2.3.7), the set of N uncoupled, linear, triple integrator subsystems

given in Equation (3.1.19) becomes the equivalent open loop system.

Let us now state the following optimization problem, which will

be denoted as the restricted TOPC problem. For each one of the N

subsystems described by Equation (3.1.19), with the initial conditions

Y 1(0) : X10

Yi(0) = 0 (4.2.1)

Y 3(0) = 0 ,

find the minimum time tf. ,and the optimal jerk input Z (t) to
be applied to the system on te[0, t f.] such that

Y li (tf) = Xlfi

Y 2i(t) = 0 (4.2.2)

Y 3(t) = 0

and for all t e[0, t I

lY~il5 <-2
M2i wM (4.2.3)

IY 3i aM. (4.2.4)



for i = 1, 2, . N, where X and X are the i-th compo-

nents of X and X if in Section 4.1 respectively.

The above stated uncoupled set of optimization problems with

state variable inequality constraints (SVICs) is closely related to

our original TOPC problem stated in Section 4.1 as follows. Recall

from Chapter III that Y, = X is the position vector of the manipu-

lator joints, and that Y = X and Y = EM are the manipulator
-:-2 -:2 z-3 ) r hemnpuao
velocity and acceleration vectors of the manipulator joints where

g is given in Equation (2.3.6). Therefore, it is clear that the

initial condition (4.1.1) is identical to (4.2.1), that the terminal

condition (4.1.2) is identical to (4.2.2) and that the inequality con-

straints in Equations (4.1.3), (4.1.4) and (4.1.5) are identical to

those given in (4.2.3), (4.2.4) and (4.2.5) respectively.

In view of this similarity it is clear that if a solution to the

restricted TOPC problem is given in the form

-* -* -*T
Z (t) = (Z Z . Z ) (4.2.6)


< <
-* Z. 0 t -tf
Z. = (4.2.7)
0 tf. 1

then substitution of Z in the FLDT (3.1.16) yields the optimal input

voltage V (t) sought for in the TOPC problem, where t is given by


See for example the discussion of a similar case in [L-1]
pages 53 56.

tf = max[t ] ; i = 1, 2, . N (4.2.8)
1 1

In other words, we find that an optimal solution to the restricted

TOPC problem is automatically (via the FLDT) an optimal solution to the

TOPC problem. The opposite relation, however, does not necessarily

hold, since the optimal solution to the TOPC problem does not necessi-

tate that each and every one of the degrees of freedom of motion will

move in minimum time.

It is intuitively clear that the restrictions of this type, i.e.,

multiobjective optimization, enable us to obtain uniqueness which we

may not otherwise have had. In our case, due to the desirability of

an uncoupled control strategy for the various DOFs, this restriction

is recommended and from now on by TOPC problem we shall mean the

restricted TOPC problem stated in this section.

4.3 TOPC with SVIC's for Triple Integrator System

As a result of the restatement of the TOPC problem via the FLDT,

we now deal with N independent TOPC problems. In this section we

find the time optimal positioning control for various tasks depending

upon which of the SVICs is active.

We commence by defining a generic triple integrator system for

which we find the time optimal positioning control to the origin

without state variable inequality constraints. Then depending upon the

task at hand, and by exploiting a very useful property of the electro-

servomechanism system, we define a variety of special cases with regard

F_ __

to the "activity" of a state variable inequality constraint. For each

one of these cases the TOPC problem is then solved separately.

For each i-th subsystem, let

S=Xf -x 10 (4.3.1)
1 1

then the restricted TOPC problem, without SVICs for each subsystem,

may be stated as follows. (Without loss of generality we shall assume

that E > 0 for all subsystems throughout this work.)

Problem PI. Given the system Z

Xl = x2

Z: X2 = X3 (4.3.2)

x3 = z

and the initial conditions

X (0) =
x (0) = X (0) = 0

find the minimum time tf and the optimal jerk input Z (t), t e [O,tf]

such that

X 1(t) = X 2(t f) = X3 (t) = 0 (4.3.4)


IZ(t)I A Vt e[0. t f] (4.3.5)

The problem P1 is a special case of the well known time optimal

control to the origin of a triple integrator system, for which a solu-

tion is given in terms of a switching surface, F (see for example

[F-1] and [S-3])

3 3 2 3/2
F =X + X /3 + e XX + C[X /2 c X 3/2
1 3 2 3 3 2


e= sgn(X + X /2),

and where the optimal control is

Z = -A sgn(F)



In the case of P1, however, due to the zero boundary velocity and

acceleration conditions, it was possible to obtain a closed form repre-

sentation of the optimal control, and the state trajectories as follows.

Theorem 4.1. The optimal control for P1 is given as follows:


Z (t) = -A


0 < t< f

tf < 3tf
4 4

3tf< <
---t -t
4 f

t = 2(4E 1/3
f A




Proof. The system, Z, is fully controllable since

0 o

j 0 .0

01 0f 0

1 0 0

0[ -1 0

in addition there is a single input, Z, thus the system Z is

normal (in the sense of definition 4.15 in [A-3] page 218) and therefore

the solution to P1 exists and is unique [A-3]. It is therefore suffi-

cient to find one control function, which together with its resulting

state and adjoint variables trajectories satisfy the necessary condi-
tions for optimality.
When Z (t), as defined in (4.3.8) and (4.3.9), is applied to the

system Z then the acceleration X3, the velocity X2 and the position

X1 trajectories are found by simple integration as follows.

X (t) = A( t)
3 -

[A(t t

At f2tf
2 A6 f 2

Aft f
2- t)2

< tf
S 0 t < -
tf < 3tf
3tf < <
4 f

< tf
0 -t < t

tf < 3t f
4 tc < -i
4 4

3tf < <
4 t tf

#Satisfying the necessary conditions will only indicate extremality.
In our case however,it will be shown that it is a minimum.



(4.3.11 )


3 2
Atf Atf Atf 2 At3 tf
X(t) = + 192 16 t + -- t 6

26 3 At2f Atf 2 At3
9-2- A tf + -2-t-- +--;

Following [A-3], let the Hamiltonian function be d

H = 1 + 1XX + 2XX + 3 Z

then the adjoint variables X.(t) i = 1, 2, 3 m


*2 OH
2 7F2 X1

3 OH
3 X3 2

From (4.3.14) we find that

X1 (t) = 10

X2 (t) = It + X20

A3 (t) = 2- A20t + A30

where X = X.(0) ; i = 1, 2, 3 and t e[0, t

Applying the maximum principle [A-3] we find that

Z (t) = A sgn(X (t))

t f
0 t tf4



3tf < <

4 -t-tf

defined by



lust satisfy



1f] .

Z (t) must satisfy



but from (4.3.8) Z (t) has two switching times, at t = and
3tf 4
at t = -, thus

tf 3tf)
^ ^f
X (t) = K (t - ) (t --- (4.3.17)
3 4 4

Equating coefficients in (4.3.17) and (4.3.15) we find that

x10 = 2K

120 = K t (4.3.18)

3 2
30 = -16 K tf

It is also known ([A-3]) that H(t) must satisfy

H(t) = 0 VtEC[0, tf] (4.3.19)

in particular, for t = 0+, we obtain

H(0') =1 + X 0 + 0+ + X30 A= 0


S= (4.3.20)

In view of (4.3.18) we obtain

3K A tf2
1 -160


K (4.3.21)
3 Atf

To summarize, the canonical (adjoint plus state) variables which

correspond to the control Z (t) in (4.3.8) are given by (4.3.10)

through (4.3.12). Since they satisfy the Euler Lagrange equations

(4.3.2) and (4.3.14), the condition (4.3.19) and the maximum principle
(4.3.16), then Z (t) must be optimal control. Notice, that the

minimum time tf was found by letting X1 (tf) = 0 in Equation

(4.3.12) which yields

tf = 2(-) Q.E.D.
f A

The optimal control, state and adjoint variable's trajectories

for problem P1 are respectively described in Figure 9.

Thus far we only obtained the TOPC for the unconstrained state case.

In the presence of SVICs the previous method (maximum principle) does

not hold and the approach taken by [S-4], [M-2] and [L-2] will be used.

In these references, however, only scalar SVICs exist whereas, as

shown in Equations (4.2.3) through (4.2.4), our single DOF subsystem

must satisfy multiple SVICs simultaneously. This difficulty is removed

by the following important observation concerning the appearance of

velocity and acceleration constraints in positioning tasks of servo-

mechanisms. (In the sequel this observation is treated as an


No theory was found to exist for the vectorial SVIC case with
scalar input systems.

x 2



2Figure 9. Optimal Control, State and Adjoint Trajectories for Problem P

Figure 9. Optimal Control, State and Adjoint Trajectories for Problem P1

Assumption. When accelerating (decelerating) from rest, an electro-

servomechanism system will always reach its maximum (minimum) accelera-

tion before achieving its maximum (minimum) speed.

This assumption is based on the physical fact that the electrical

time constant of the actuator is usually about two order of magnitudes

smaller than the mechanical time constant of the loaded actuator,

therefore the actuator achieves its maximum current (related to torque

or acceleration) much before its maximum speed (which depend on the

load's and actuator's inertia) when the maximum voltage (related jerk)

is applied to it.

Based upon the above given assumption we now may obtain, depending

upon the task at hand (i.e., depending upon E) one of the following


Case 1 No SVICs are active and therefore the restricted TOPC problem

is identical to P1 for which a solution is obtained in Theorem 4.1.

(This case occurs if E < -- see explanation below.)
Case 2. Constrained acceleration only. This case occurs when the

following conditions are both satisfied.

> -5- (4.3.22)


4 2
M -> ( M 2- (4.3.23)


Condition (4.3.22) simply means that the maximum acceleration allowed

is smaller than thp highest acceleration that would be obtained for

the task E, if the unconstrained case (P1) is assumed. (See Figure 9.)

Condition (4.3.23) will be explained below.

The optimal jerk input Z (t) for Case 2 i.e., when the SVIC

VteC[0, tf I


is added to the statement of problem P1, is found by applying the

method described in [S-4] to this system. The resulting optimal tra-

jectories and the optimal control are given in Equations (4.3.25)

through (4.3.29) and are also described in Figure 10.



Z (t) = -A


< OM
0 t < -

SM < tf c M
2-- < -
A 2
t f M t f i
tf M < f M

S+ -A t < tf --
f L M 2 A t
M< < ,
tfj-- t -tf


Ix3 (t) I -< (M


0 < t c A

; A


- At

< < tf
- 2


AM <

tf M <
-- + - t < t

At At


- 2A+ Mt


t f

I -A


+ -T ]

-ci t + ac t -
M M if


t f


t -< tf

0 A

< < tf M
St A

A t -

M < <
+-- t -

tf +- A

f AM
tif -

t2 I
A -tf 2-]

I aM < <
tf -- -t -
if A


X3(t) =

X 2(t) =





X (t) =

A ,

< 01M
0 t <-

S At3

3 2
6A2 2A

At3 Atf
At 4
-~ + -
9 ,2

8 ,2 2

Mtf Mtf
8 + 4A

aM tf iM

-t <-

-2 A -t 2 + 2

a- 2 + [ m
~+2 [Mt

c M

6 A2

At3 Atf 2
6 2

- 6 +

m- ]t +
f 2A
+ 2t +

tf aM

' 2
tf M

t f aM < < M
; -2 + -A t tf -A

M < <
tf A t tf

a 2
cM OM2 )
tf = -- + /(-- ) + -2.
f A + A aM

As shown in Figure 10 the maximum velocity is obtained at t = -
and is given by (from Equation (4.3.27))

4 2
Sf aM am
S2 + E+C 2

t CM t2
t +-Tt

t2 + Mtf





A -- f (IM I f f aM -
zL 2 - I 2 + -A
-A ,;
Z(t) ___ --._______--- ---- --
m I----1 t -^ -f



X2 "--"" I
x 2


Figure 10. Optimal Control and State Trajectories for TOPC
Problem with Bounded Acceleration (Case 2)


In order not to exceed the maximum speed it is necessary for the

task E and the constraints a M ) A to satisfy

W M > X2

which is exactly the condition (4.3.23) which is necessary for having

Case 2.

Case 3. All SVICs are active. This case occurs when the task F is

such that condition (4.3.23) is not satisfied, i.e. when

< 4 2
4M < 2 2AM

Again, as in Case 2 the method for finding the optimal control on con-

strained arcs in [S-4] was used for the restricted TOPC problem (see

Section 4.2). The resulting optimal control and state trajectories

are given in Equations (4.3.30) through (4.3.34) and are also described

in Figure 11.

A 0 -t < t

0 t -< t < t2

-A t t < t
2 3

Z (t)= 0 t3 t (4.3.30)

-A t4 t < t5

0 t5 t
5 < 11
A t6 t tf

Z t2 t3 t t
-A 11


1 1


Figure 11. TOPC for Case 3

Figure 11. TOPC for Case 3

X 3(t) =

X2(t) =



-A(t t3


-A(t t4


A(t t f)


-2- + M"t tI
At 2

WM (t t32


w A (t t2
M 22 4t
At E
M 2 M

At t2f)

0 t < t







- t5)



- t


- t

- t

- t

0 -t < t

t -t < t2

t4 1 t < t2

t2 t < t3

S- t3 t 4 t4
tt t4 -t< 5

t5 -t t6

t6 -t -tif


< t2

< t3

< t4

< t5

< t6

< t
- t f



2 3
M t2 t +-7M
2 6 A ,2
A t3 A(M 'M 2
-A 3 +A (- +--M) t -
6 2 M A


Aw-- ) t

tM M


A 6M
6 3

< <
; 0 t t2

S2 <
t2 -

+ W M (t t3) E

t t < t
t3 t


t = -
1 A
t M
2 aM

t3 = t + t2


M x M
aM "M
_-- + --
- 1M A- M

t 4=t f-t3
4 = 3 -

t 5 f 2

t6 f 1

It is interesting to notice that the time optimal trajectory obtained
here (Equation (4.3.31)) is identical to the trapezoidal acceleration motion
described by Tesar and Matthew [T-3], Gnerek, Tesar andMatthew [G-1I], and
Matthew and Tesar [M-2], [M-3] for minimizing the transient amplitude of
vibrations (high speed content) which reduces the positioning accuracy.

X (t) =


2 aM

+ A)

Figure 12 describes the various speed trajectories obtained for

the above described cases with the path as a parameter.

Having obtained the solution to the restricted TOPC problem we

complete this chapter by describing two examples, single and double

link manipulators in Sections 4.4 and 4.5 respectively.

4.4 TOPC for a Single Link Manipulator (Example 1)

In this section we apply the results of Section 4.3 to a single

link manipulator system. A load mass m is attached through a rigid

link with length d to the shaft of a DC motor, as described in

Figure 4. The task is to rotate the load through an angle of E

radians in minimum time while observing constraints on the maximum

speed, acceleration and jerk of the motion as follows.

Experiment 3. The PMI MC19P DC motor was chosen with the following


R = 0.6 Q

L = 0.0015 H

K = 0.25 Ni/amp

K = 0.25 V/rad/sec

JM = 0.001 Kg m2

Maximum Current, I = 100 Amp.
Rated Voltage, VR = 85 V

Rated Speed, 0 = 340 rad

E4 > E3 > 2 > 1 > 0

TOPC Speed Trajectories for Various Paths with SVICs

Figure 12.

The load was

m = 10 Kg

d= 1 m

The gear reduction ratio was

r = 100

and the gravity acceleration was assumed to be

g = 10 --

The path to be traversed was

= 1 rad.

(All notations are consistent with those of Chapter II.)

The equations of motion for the system (3.2.2) were derived in

Appendix I and used in Experiment 1 of Chapter III. Those equations

with the FLDT given in Equation (3.2.3) were simulated in a FORTRAN

program called FIN1 (its listings are given in Appendix III) with the

optimal jerk control as given in Equations (4.3.30) and (4.3.34) for

the TOPC problem, Case 3. In addition, the maximum speed, acceleration

and jerk were found from the motor specifications as follows:

VR 85
"M KV r 0.25 100 = 3 rad/sec

from the equations of motion (3.2.2) we obtain


angular dX2 2 2
acceleration = [rK I mgd sin(X )]/(md + r J
at T 1M


dX2 2 2
max[-] = [rK I max(mgd sin(X ))]/(md + r J)
dt M T max 1 M


25.*100 -100 _-, 2
2 = 2- 00- 120 rad/sec 2
M 20

also the maximum jerk was estimated via the electrical time constant

(assuming that it takes about four electrical time constants to develop

the maximum acceleration) as follows.

A z M 120 = 120,000 rad/sec2 1
R 0.6 .
4 40.00015

With the above given parameters, the program FIN1 simulated the system

motion from X = -1 rad to the zero position. Figure 13 describes

the FLDT voltage, the motor's current, the velocity and position

trajectories respectively. As can be seen in Figure 13 the velocity

and position trajectories identcally match the expected curves of a

TOPC for a triple integrator system, indicating a perfect linearization,

the voltage and current of the actuator,however, demonstrate the non-

linear effects of the load as it changes configuration.


Figure 13. TOPC Trajectories in Experiment 3


C --" .. __ -_-_.--_...-... .....

g 1' q r, r ,) *, I . Q. .Q 0. 2 , :' "1 :" ,

ao ,
FCn sec.

gie C


Fiur 13 Continued

4.5 TOPC for a Double Link Manipulator (Example 2)

An experiment similar to the one described in Section 4.4 but for

a two-link manipulator is described in this section. The system is

described in Figure 6. The equations of motion for the system are

given in Appendix II and are also used in Experiment 2 of Chapter III.

The task is: positioning a loaded double link system, driven by two

DC motors, at its null position, starting from -1 radian in both joints,

in minimum time, while observing similar state variable inequality con-

straints as in Experiment 3.

Experiment 4. For the second joint we selected the same DC motor as

in Experiment 3 (PMI MC19P). For the first joint the motor was PMI

MC23S, with the following parameters.

R =1 1

L = 0.00025 H

K = 0.5 Nm/A

KI = 0.5 V/rad/sec
JM = 0.002 Kg -m2

Maximum current, I = 100 A

Rated Voltage, V = 170 V

Rated speed w = 340 rad/sec

The loads were

m = m2 = 10 Kg

a1 = O2 = 1 m

Equations (3.3.3), (3.3.4) and (3.3.5) were simulated by a FORTRAN

program called FIN2 (its listings are given in Appendix III). The

optimal jerk inputs Z and Z were programmed as in Experiment 3.

In addition the maximum speeds, accelerations and jerks for both links

were found from the actuator's specifications and the equations of

motion of the system (3.3.3) as follows.
VR 170
M R 1 0 3.4 rad/sec
K1 V

VR 85
w = -- = 3.4 rad/sec
M K r2 25

The maximum accelerations a M and a M were estiamted by the current/

acceleration equations (X_2 in Equation (3.3.3))

1 2
I1 = rKT {[JM r + D22 + Q3 + 2Q cos(2)] e

+ [D22 + Q cos(02)] e2 + Q2 sin(81 + 82)

+ Q sin(e ) Q sin(82) (42 + 2el 2)} (4.5.1)

I2 = r1K [D22 + Q cos(02)] 1 + [D22 + M r2 e2

+ sin(9 + 0 ) + Q sin(8 ) e2} (4.5.2)
+ 2 1 2 2 1


D22 = 22

Q = m2d1 2

S= (m1 + m )g d (4.5.3)

Q2 = m2d2g

3 = Q, d /g

and where 1 82 are the first and second joints' coordinates

respectively. It was found that in order for the currents, I1 and

I 2) not to exceed their maximum values, 100 A, the angular accelera-

tions a M and a had to be

= = 55 rad/sec2
M1 M2

for any 01, 92 e(0, 211'. Finally, the maximum jerks, were found in

the same way as in Experiment 3.

a M 55' 3 41 ^1>^'
a M _ _ _ _ = 5 5 0 0 0 r a d / s e c 3 .. .
1 _RI __ .
4 0.00025


2 55 3 C3 (iL
A -- = = 55,000 rad/sec
2 4 R 4 0.6
L 0.00015

With the above given parameters, the program FIN2 simulated the

system motion from 01 = 02 = -1 rad at t = 0 to the null position

01 (tf) = 02 (tf) = 0. Figures 14 and 15 describe the voltages, currents

and state trajectories that correspond to this motion for the first

and second link respectively. As can be seen in Figures 14 and 15

the velocity and position trajectories of both links, identically match

the expected curves of a TOPC for a triple integrator system, indi-

cating complete decoupling and linearization, the actual voltages and

currents of the two motors however, demonstrate the expected non-

linear and coupling effects through the motion.

In Figures 14 and 15 the actuators' currents trajectories may give

the wrong impression that we actually produce a "bang-bang" current

(torque) control. This, however, is not true. The misleading form of

the current is the result of the fact that both joints traverse the

same path with the same velocity and acceleration constraints. In

order to demonstrate this, the following experiment was performed.

Experiment 5. The program FIN2 was used to repeat Experiment 4

4 ith 0 1(0) = -1 radian and 8 2(0) = -.5 radian while all the other

parameters were the same as in Experiment 4. The resulting voltages,

currents, velocities and positions of both joints are described in

Figures 16 and 17 respectively. As shown in Figures 16 and 17, the

actuators currents no longer have the "bang-bang" shape of Experiment 4.

4.6 Comparison with Approximated Bang-Bang Voltage Control

In order to demonstrate the advantages of FLDT in the slewing mode

of TOPC, and to emphasize the effects of nonlinearities and coupling

terms in the dynamics of manipulator, (if they are not accounted for,)

the results of Experiment 4 were compared to those of the current

approach in bang-bang voltage control for slewing mode motion, as des-

cribed in Experiment 6.



f y ; ',. * - r

g_____ ____ ___
i I-

I.. ,I. -
0 -

8* O : '. \ O

I' I i | i i -1

g .. - - .. .. .... ... .... .. ...-.- ....- ... ..... ..... _ ... ; .. .._..... .i. . . :

000 0.tp 0.00 01, 2 O,010 0.20 0.-'i 0.2 0.l ,0.G 0

Figure 14. TOPC Trajectories of Ist Joint in Experiment 4


Figure 14. Continued

Figure 15.

TOPC Trajectories of 2nd Joint in Experiment 4


Full Text
xml version 1.0 standalone yes
PageID P71
ErrorID 4
ErrorText 90 CW: 0189-0217