Miomir Vukobratovic

Dragan Stokic

Applied Control of
Manipulation Robots
Analysis, Synthesis and Exercises

With 100 Figures

Springer-Verlag Berlin Heidelberg NewYork

London Paris Tokyo Hong Kong
Professor Miomir Vukobratovic, Ph. D., D. Sc.
Corr. member of Serbian Academy of Sciences and Arts
Foreign member of Soviet Academy of Sciences
Assoc. professor Dragan Stokic, Ph. D.
Institute Mihailo Pupin
Volgina 15
P. O. Box 15
11000 Beograd
Yugoslavia

Based on the original Upravljanje Manipulacionim Robotima

published by NIRO "Tehnicka Knjiga", Beograd, Yugoslavia

ISBN-13 :978-3-642-83871-2 e-ISBN-13 :978-3-642-83869-9

DOl: 10.1007/978-3-642-83869-9
This work is subject to copyright. All rights are reserved, whether the whole orpartofthe material is
concerned, specifically the rights of translation, reprinting, re-use of illustrations, recitation, broad-
casting, reproduction on microfilms or in other ways, and storage in data banks. Duplication of this
publication or parts thereof is only permitted under the provision of the German Copyright Law of
September9, 1965, in its version ofJune 24, 1985,and a copyright fee must always be paid. Violations fall
under the prosecution act of the German Copyright Law.
© Springer·Verlag Berlin Heidelberg 1989
Softcover reprint of the hardcover 1st edition 1989
The use of registered names, trademarks,etc. in this publication does notimplY,even in the absence of a
specific statement, that such names are exempt from the relevant protective laws and regulations and
therefore free for general use.

216113020543210 - Printed on acid-free paper

Preface

The first book of the new, textbook series, entitled Applied Dynamics
of Manipulation Robots: Modelling, Analysis and Examples, by M.
Vukobratovic, published by Springer-Verlag (1989) was devoted to the
problems of dynamic models and dynamic analysis of robots. The present
book, the second in the series, is concerned with the problems of the
robot control.

In conceiving this textbook, several dillemas arouse. The main issue

was the question on what should be incorporated in a textbook on such
a complex subject. Namely, the robot control comprises a wide range of
topics related to various aspects of robotics, starting from the syn-
thesis of the lowest, executive, control level, through the synthesis
of trajectories (which is mainly related to kinematic models of robots)
and various algorithms for solving the problem of task and robot moti-
on planning (including the solving of the problems by the methods of
artificial intelligence) to the aspects of processing the data obtai-
ned from sensors. The robot control is closely related to the robot pro-
gramming (i.e. the development of highly-specialized programming lan-
guages for robot programming). Besides, numerous aspects of the con-
trol realization should be included here. It is obvious that all these
aspects of control cannot be treated in detail in the frame of a text-
book. Therefore, we decided to confine ourselves only to the synthesis
of control at the executive level, while the other aspects of the con-
trol mentioned above, should be treated in the coming volumes of the
series. To facilitate the understanding of the control synthesis at
the executive level, the first two chapters of this book deal with the
problems concerning higher control levels and fundamental aspects of
the robot control realization.

After deciding to restrict our attention only to the synthesis of con-

trol at the executive level, we posed the following question: whatsho-
uld be the necessary background knowledge of the user of this textbook?
It is known that the control synthesis at the executive level is, on
the one hand, closely related to theory of automatic control (i.e. the-
ory of large-scale technical systems) and on the other, to dynamiCS of
VI

active mechanisms. We assume the user of this book has already read
volume one of this series, so that he is acquainted with the basic
knowledge on dynamic models of robots. Therefore, in this book (Chap-
ter 3) we only briefly consider the dynamic model of a robot without
repeating many of the concepts related to the robot mechanics. However,
the problem of the relation to systems theory is much more complex. Ma-
ny of the approaches and solutions developed in the general controlsy-
stems theory have also been applied, after more or less adaptation, to
the synthesis of robot control. Someone who wants to study robot con-
trol should be familiar with some basic concepts of theory of automa-
tic control and theory of large-scale systems. For this reason it was
necessary to "repeat" in this book some of the approaches of both clas-
sical theory of automatic control and theory of control of large-scale
systems (i.e. at least those approaches that found a wider applicati-
on in robotics). In order to avoid the unnecessary broadening of the
book by including detailed considerations of the concepts and approac-
hes that can be found in the related literature on automatic control
and systems theory we assume the user of this textbook is familiar with
basic concepts and techniques of automatic control (such as the models
of linear systems in s-domain, models in state space, classical methods
of control synthesis, etc.). However, as we have endeavoured to write
a book which should be as much as possible a self-contained unit, we
included in it a number of notions of theory of automatic control (e.g.
the elements of position servo systems, fundamental concepts of the me-
thods of pole-placement, the synthesis of the optimal regulator, and
the like). In doing this we avoid all theoretical corroborations and
all those details which are not necessary for the understanding of ro-
bot control (and which the reader, if need be, can find in the cited
literature). We hope this approach will enable a wider circle of rea-
ders to study robot control, with no need to consult additional lite-
rature.

In planning this textbook, the next dilemma was: which approaches to

the robot control synthesis should be described and how extensively it
should be don.e? Presently, a great number of various approaches to ro-
bot control are being developed. Many of them have not been verified
in pract~ce, as they are still at the stage of theoretical and experi-
mental studies. It is difficult to decide whether these approaches sho-
uld be included in a textbook of this kind or not. We decided first to
consider thoroughly the classical approach via the servo systems around
particula;r- jOints, Which is involved in the majority of the present-day
cOmme;r-cial ;r-obots. We think that, from an educational viewpoint, this
 VII

approach provides a most convenient way to understanding the dynamic

behaviour of the robot as a whole. For this reason we consider in de-
tail the methods of the analysis of robot's behaviour. From our experi-
ence, it is most advisable to choose those approaches which, starting
from the decentralized control, introduce the correctional signals to
compensate for the effect of the robot's dynamics. For this reason we
consider dynamic control via the global one. In this way, the reader
learns gradually, starting from the most simple servo control and going
to the complex dynamic control. Thus, an important educational aim is
achieved: the user of the textbook becomes aware of the crucial impor-
tance of choosing the simplest control law in the control synthesis and
also, that it is equally important to check if such control satisfies
the robotic system in different dynamic regimes of its operation, Le. if
a satisfactory accuracy and desired operation speed of the system can
be achieved. Furthermore, we think it is worth to emphasize the impor-
tance of the problems related to variations of the robot parameters,
and indicate the circumstances when it is necessary to introduce adap-
tive control. As in the case of non-adaptive dynamic control, we think
it necessary for the reader to develop the criterion for introducing
adaptive control. However, we thought that reviewing of various sche-
mes of adaptive control would burden the book very much and that a ba-
sic coursebook on robot control should not be stuffed with schemes of
ada.ptive control which are, mainly, at the stage of the laboratory ap-
plication. We thougnt it also necessary to include in the book some
representative approaches to the dynamic robot control, such as the
"co!llPuted to;r;-que method," and tne classical approach to systems theory
v;ia the 1;ip,ea;I) optimal regulator. I:n addition, we decided to present
the Ca,;t:tesian control which, though still being at the experimental
stage, is ye;r;-y impo;t:tant for educational purposes as it provides a go-
Od insight ip,to certain crucial aspects of robot control. Finally, we
thought ;it necessary to pay a special attention to the robot control
with constrained motion of the gripper a.nd to so-called hybrid positi-
op./;f'orce cont,;t:ol" as t.hese are specific problems in the field of robo-
t.ics which. attract a great deal of attention of a number of researchers.

Haying ma.de such a cnoice of the subject matter to be included in our

textbook, we divided it into seven chapters.

Chapter 1 presents the fundamental control principles of the current

robotic systems. Without repeating basic concepts of robotic mechanisms
(whicn can be foun.d in the first book of this series), the chapter bri-
efly d,esc;r;-ibes the classification of robots according to the control
VIII

and then deals with the usual control hierarchy of automatic robots.
Furthermore, i t gives a concise discussion of some characteristic ro-
bot tasks and points out that the different requirements are imposed
before the robot, what should be borne in mind in the control synthe-
sis.

The aim of Chapter 2 is to provide the reader with some basic concepts
of the kinematic control level and thus enable him to follow the chap-
ters to come. Here, we are not concerned with the methods of forming
kinematic models of robots (which can be found in the appropriate li-
terature) but we endeavour to indicate the problems related to solving
the inverse kinematic problem.

Chapter 3, which occupies the central place in the book, is devoted to

the synthesis of servo systems around particular robot jOints. As we
mentioned above, the servo system approach is prevailingly found with
the present-day commercial robots. We think it extremely important in
the teaching to emphasize the specificity of the synthesis of servo
systems for robots. First part of the chapter presents in short the
robot models: the model of the mechanical part and the model of the
actuators, and their assemblying into a unique model. Then, the model
of the behaviour of one robot jOint is derived under the condition that
a.ll oth.er jOints are kept locked. Such a joint model is the basis for
the servo systems synthesis. Section 3.3. describes the synthesis of
the local servo systems in two ways: in s-domain and by pole-placement
method. Throughout the chapter, the servo systems are consistently re-
presented via the models in s-domain and via the models in state space.
The elements of servo systems and their transfer functions are presen-
ted in detail, although their more precise definitions can be found in
the l.iterature on automatic control. We wanted the reader to refresh
his knOWledge on classical servo system and thus be prepared, without
consulting additional literature, to follow the further consideration
f'or WhiCh a full understanding of the concepts of a position servo sy-
stem is needed. The synthesis in s-domain is given in detail and then
the sYnthesis is generalized using the method of pole-placement. We
Oilndeavoured, to simplify this method as much as possible because it is
he:r:e aPl?l;i.ed on the simple, low-orde;r subsystems. The synthesis of the
optiJilal l.ocal regulator, as one of alte;rnative approaches to the syn-
thesis of se:r:vo systems, is given in Appendix 3.A. without entering
detail.ed, theoretical. considerations. Section 3.3.4. explains in detail
the sPOilci:f;icity of the robot's servo systems: the effect of variation
of the !1IechanisJII ' s inertiality and gravitational moments. The next
 IX

sections d.eal with the nonlinear effects d.ue to the constraints on the
actuators, friction, and the like, as well as the application of the
PID regulator. Finally, Section 3.5. is devoted to the problems of trac-
king of trajectories at the servo system level, and to the precompen-
sator synthesis. We thought it advisable to explain the problems rela-
ted to the delay compensation on a simple subsystem model (one joint
and one actuator).

Chapter ~ analyzes the behaviour of the robot system as a whole when

all its joints move simultaneously and the local servo systems are ap-
plied. As first, a qualitative analysis of the effects of particular
d.ynamic forces is given, and then, the robot stability is analyzed us-
ing a linearized. model. This analysis is carried out in a gradual way:
first, the robot's behaViour during the position control (around a gi-
Ven position) is analyzed, then, the change of the given position is
considered, and finally, the trajectory tracking is described. At this
pOint, nO!l\in.al centralized control is introduced with the aim to expla-
in the phenomena of dynamic coupling between the joints, and not as a
representative approach to solving the problem of dynamic control. Mo-
reove;r, a:n th.e shortcomings of this approach to robot control are cle-
a;rly stated,. special attention is paid to the effect of the robot's
d,ynamics when. the local servo systems are used to track trajectories,
as this approach is most frequent in practice. At that, we wanted to
make a clear d.istinct;ion between the case when such simple control can
serve the purpose and. the case when it is necessary to introduce dyna-
mic control.

An analys;is of stability of a nonlinear model during the position con-

t;rol and. trac~ing of trajectories, is the subject of Appendix 4.A. The
stapility an.alys;is of the nonlinear robot model requires the knowledge
of theory of large-scale systems. A method of analysis of complex systems
WhiCh, to Ou;r;' opinion, can be w;t.th success applied On robotic systems,
is p;r;'esented, .in Append,ix 4.A. in a. most simple way so that it may be
understoOd Py the peade;r;'s which did, not have opportunity to become ac-
quainted, with the lite;r;'atu;r;'e in this field,. However, this appendix is
intended fOr those ;r;'eaders whiCh would like to study more deeply this
aspect of th,e con,trol synthesis an,d .is not necessary for the understan-
ding th,e bas.ic app;r;'oach,es treated in the subsequent chapters.

chapter 5 treats dynamic global robot control via two main forms: the
fO;r;'ce feedPac~ and th,e on-lin.e calculation of robot dynamics. The
x
application of various approximate models of robot dynamics for dyna-
mic global control received special attention. At this point, we tho-
ught it advisable to exemplify the computer-aided synthesis of control
by presenting a characteristic programming package'. In Appendix 5 .A. ,
the analysis of robot stability is extended to cover the case when,
apart from the local servo systems and nominal control, the global dy-
namic control is introduced. However, as in the case of Appendix 4.A.
this appendi~ is not necessary for the understanding of the rest of the
book. A substantial part of this chapter is devoted to the approach via
the "computed. torque method" as the one of the most popular approaches
to robot control. Woe clearly indicate the e~istence of different vari-
ants of this approach, as well as some of its shortcomings. As we have
already mentioned, this chapter presents the possibility of Cartesian
robot control. The centralized quadratic regulator is described in Ap-
pendix 5.B. (again without repeating the known theoretical statements
which can be found in the literature) as an example of the possible
application of the control systems theory on robots. Besides, we indi-
cate all the shortcomings of this approach which are the reasons why
such control has not been applied in practice.

Chapter 6 is devoted to the problems of the variation of robot's para-

meters and to the robustness of the control laws considered. To save
space in the book we confined ourselves to only a qualitative analysis
of the robustness w-ithout pX'oviding rigorous definitions and analyses.
We want to help the reader to develop a critical attitude to possible
in,troduction of adaptive control.

Chapter- 7 dealS with the most delicate tasks involving the constrained
g'ripper motion, and especially in the assembly processes. Following the
p,t:ip,ciple "from the simpler tc:> the !!lore complex", which has been imple-
mented. in the whole book, we first describe various "contact situati-
ons" occurring in the characteristic "peg-in-hole" task. Then, we deal
with, the conceptual scheme of the control synthesis for such a task,
and finally, we treat the problem of the synthesis of hybrid position/
(for'ce coptrol in the tasks involving the constrained gripper motion.

;rp' a sepaX'ate <I.,P,Pen,di;X; at the end of the book we give a short version
of the prog'ramme package for the synthesis of robot control. We have
chosen SOme characteristic ,P;t'ogramrnes which can be used. both for the
syp.thesis of local seX'vo systems and analysis of robot's behaviour for
di:i;:i;eX'ep.,t cont,t:o); :Laws. :Because of the limited space, we included only
 XI

the analys~s of the linea~ize~ fflodel. It is o~r opinion that these pro-
graIl\llles, tho~gh being I?resente~ in a re~~ce~ fo~, mak.e an independent
package which can serve in the ed~cational practice. The main purpose
of this appendix is to enable the rea~er to master' the computer appro-
ach to the robot oontrol sYnthesis highly important f(!)r the education
of both the future robot designers and robot userS. Moreover, we hope
these programmes might inspire the reader to develop his own programmes
which could. yield the synthesis of SOffle other control laws, as well as
the adaptation of these programmes to the robot types not included in
this package.

Practically each section of the book contains a numerical example il-

lustrating the theoretical results given in the text. We endeavoured
to present all the e~amples for the one and the same robot (with the
exception of some sections where for the obvious reasons this was not
possible). We chose a robot having a simple structure (one prismatic
and two revolute jOints), but which enables the demonstration of the
~ajority of characteristic phenomena of both the non-dynamic and dyna-

mic robot control (such as the mechanism's inertiality, coupling of

jo;t.p,ts Via the c;J::"oss - inertia terms, and the like, as well as the exam-
ple of a jOint totally decoupled from the others in a dynamic sense).

The majority of the sections end with a set of numerical problems. The-
se probleffls are especially important for the use of this textbook. Our
inte!1tio!1, was to stifflulate the reader to use actively the results pre-
:;;e!1ted in the preceeding :;;ection. and thus increase his knowledge thro-
ugll an, independ,ent act;i,vity. l!'or this purpose we composed a number of
exerc;i,ses of :;;everal type:;; which can be conditionally divided into
tllree group:;;:

a) Tile purpose of the first group of problems is to illustrate the par-

tiCular subject that has been described in the preceeding section.
Most often they are numerical exercises and for their solving the
read,er sh(!)uld use the presented res~lts. These problems can be sol-
ved analytically (by hand) and they are usually similar to the wor-
k,ed-ou.t e~amples, but for a!1other robotic structure. In this way the
peade;I;' ind,epetidently learnS how the presented theoretical statements
cap, be applied on different robot tYl?es, and thus gain experience
i!1, dealip,g with differen.t structures of robotic mechanisms, which
to a great extent, affect the control synthesis. In addition, we
wa!1ted to efflj?hasize the ifflj?ortance of making a proper choice of ac-
tuators i!1 tne control synthesis. As in the text itself, we exclu-
XII

sively dealt with DC electrO-)1lOt.ors, it is left to the reader to apply,

by solving the exercises, the same approaches, to hydraulic actuators.

b) The aim of the second group of exercises is to provoke the reader to

think about the approaches presented and find, in an independent way,
the explanations of certain statements which have been given in the text
wi thout proving or detailed explanation. Often, these problems illustra-
te the given statements (e.g. the statement on the complexity of imple-
mentation of the presented control law is illustrated by the exercises in
which the user has to determine the number of numerical operations needed
to calculate the given control, as well as to estimate the "microproces-
sor architecture" needed to implement the control law, and the like).

c) The problems of the third group (marked with the asterisk) assume
the read.er has a. sound knowled.ge of the related scientific fields.
These problems imply writing the programmes to synthesize or implement
a control. law using a microcomputer. Some problems of this group as-
sume the previous knowledge of systems theory, and their aim is to
encourage the reader to delve more deeply into the literature and
thus prepare himself for independent work. For such a reader, sol-
ving these problems should be compulsory.

we think the thus structured textbook will enable the reader to gain a
sufficient knowledge for his further work on the problems of control of
robotic systems and. for implementation of theoretical approaches into
practice. We want the reader to develop an engineer's approach to the
subject and. to direct him to use computer approach to the synthesis of
robot control, as this approach enables efficient linking of mathema-
tiCal models and the practical requirements to be realized by current
robots. »ow well we have succeeded it remains to be judged on the ba-
sis of the use of this book as a textbook in teaching practice as well
as in the research-and-development units for applied robotics.

Authors eiKpress their thanks to Mr. Dj. Lekovie B.Sc., for his help in
preparing an educational version of the software for the synthesis of
control of manipulation robots. Also, they are indebted to Miss. V.
¢osie for her careful and excelent typing of the whole text. Finally,
authors extend. their thanks to Prof. L. Bjelica for his high profes-
sional contribution to the book translation into English.

March 1989 Aut h 0 r s

B e 0 g r a d
Contents

Chapter!
Concepts of Manipulation Robot Control ................................. .

1 . 1. Introduction .••.........................•..•..........•

1.2. Automatic manipulation robots . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.3. Classification of automatic robots ....................• 9

1.4. Characteristic tasks and applications of robots in

industry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . • 10

1.5. Implementation of control of manipulation robots .•.•... 16

References 26

Chapter 2
Kinematic Control Level 27

2.1. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

2.2. Direct and inverse kinematic problem-determination of

robot position . . . . . . . . . . . . . . . . . . . . . . • . . . . . . . . . . . . . . . • . . 27

2.2.1. Analytic solution to inverse kinematic problem .. 34

Exercises .....•.................................•.••... 38

2.2.2. Numerical solutions to inverse kinematic problem 39

Exercises •.•................................•..••.•... 44

2.3. Trajectory synthesis for manipulation robots •......... 44

Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . • . . . . . . . . . . . . . 50

References 50

Chapter 3
Synthesis of Servo Systems for Robot Control 52

3.1. Introduction ..•.•...•..•.....................•..•..... 52

3.2. Dynamic model of robot ..•.............•............... 52

XIV

3.2.1. Mathematic model of mechanical part of system ... 53

Exercises . . . . . • . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

3.2.2. Models of actuators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

Exercises· .•.............•.........•••••....•...•....••. 62

3.2.3. Total model of robotic system . . . . . . . . . . . . . . . . . . . 63

Exercises 69

3.3. Synthesis of local servo system . . . . • . . . . . . . . . . . . . . . . . . . 71

Exercises . . . . . . • . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . • . . . . 75

3.3.1. Elements of local servo system . . . . . . . . . . . . . . . . . . 76

Exercises . . . . . . . • . . . . . . . . . . . . . . . . . • . . . . . . . . . . . . . . . . . . . . 86

3.3.2. Synthesis of servo system in s-domain . . . . . . . . . . . 88

Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

3.3.3. Synthesis of servo system by pole-placement

method.......................................... 97

Exercises . • • • . . . . . . . . . . . • . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

3.3.4. Effects of inertia variation and gravity moment

of the mechanism on the behaviour of servo system 106

Exercises 115

3.3.5. Nonlinear effects in local servo system ......... 118

Exercises 124

3.4. Synthesis of local PID controller . . . . . . . . . . . . . . . . . . . . . . 126

Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . • . . . . . . . . . . . . . 133

3.5. Synthesis of local servo system for trajectory tracking 135

Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144

References 146

Appendix 3.A
Local Optimal Regulator 147

References 151

Chapter 4
Control of Simultaneous Motions of Robot Joints . . . . . . . . . . . . . . . . . . . . . . . . . . . 152

4.1. Introduction . . . . . . . . . . . . . . . . . . . . . . . . • . . . . . . . . . . . . . . . . . 152

4.2. Coupling between jOints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153

 xv
Exercises .••••••••••••••••..••••••.•••..•••••••••.••••.•••• 158

4.3. Analysis of linearized model of robot ••••.•••.•••••.•. 159

4.3.1. Linear model of robot 160

Exercises •..•••••.•••.••••••••••••••.•••.••••••••••••• 168

4.3.2. Analysis of stability of linear model of robot

with position control 168

Exercises 176

4.4. Synthesis of decentralized control for simultaneous

motions of robot joints •••••••••.•••.•••••••.•••.••.•• 177

Exercises •.•..•..•.•.••..••...•.•.•..•..••••.....••..•.•••. 185

4.4.1. Synthesis of nominal programmed control .••••••. 188

Exercises •..••....•.•....••••.••......•....•.•.•..•••. 201

References 202

Appendix 4.A
Stability Analysis of Nonlinear Model of Robot 204

4.A.1. Analysis of asymptotic stability of robot position

control •.••••.••••••••••••.•••••••.•••••••••••.•••.• 205

4.A.2. Analysis of asymptotic stability of robot trajectory

control •.••••.•••.••.•••.•••••••.•.•••••••••••.••••• 224

Chapter 5
Synthesis of Robot Dynamic Control . • • • . • • • • • • • . • • . . • . • • • • • • • • • • • . • • . • • 233

5.1. Introduction 233

5.2. Synthesis of global control........................... 234

5.2.1. Force feedback as global control ••••••••••••.•• 242

5.2.2. On-line computation of dynamic forces for global

control 246

Exercises .•.••••••••••.•••••••••.••••.••••.••••••••••• 252

5.3. Computer-aided synthesis of robot control............. 254

Exercise •••••••.••••.••.••••••••.•.•.••.....••.•....•.•••.• 263

5.4. Computed torque method for robot control synthesis •••• 263

Exercises ••••••.•••••••••••••••••••••••••.••.••.••••••••••• 270

5.5. Cartesian based control of robot ••••.••••.•••••••••••• 272

XVI

Exercises 277

References 279

Appendix S.A
Stability Analysis of Robot with Global Control 281

Appendix S.B
Centralized Optimal Regulator 287

References 294

Chapter 6
Variable Parameters and Concept of Adaptive Robot Control 295

6.1. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295

6.2. Robustness of control to variations of robot parameters 296

Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 305

6.3. The concept of adaptive robot control . . . . . . . . . . . . . . . . . . 306

Exercises . . . . . . • . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 311

References 313

Chapter 7
Control of Constrained Motion of Robot 314

7.1. Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314

7.2. An analysis of assembly process by robots . . . . . . . . . . . . . . 315

Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 324

7.3. Robot control in the stage of parts mating . . . . . . . . . . . . . 325

Exercises . . . . . . • . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 337

7.4. Hybrid position/force control of robots . . . . . . . . . . . . . . . . 338

Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . • . . . . . . . . . . . . 351

7.5. Stiffness and impendance control of robots . . . . . . . . . . . . . 353

Exercises . . . . . . • . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . • • . 358

References 360

Appendix
Software Package for Synthesis of Robot Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 363

Subject Index . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 468

Chapterl
Concepts of Manipulation Robot Control

1.1 Introduction

The tasks that are nowdays assigned to robots are becoming more varied
and more complex. More and more often robotic units are becoming parts
of flexible technological cells, lines and intelligent technological
systems. In view of these facts, the organization of robot control
should be based upon the principle of control hierarchy.

Hierarchical levels in control systems have appeared in modern develop-

ment of the control techniques to meet requirements of more complex
functioning of some technical systems and their control systems. Robotic
systems often posses a certain degree of adaptability with respect to
both changed parameters of the workpiece and the changing environment
in which the processes take place.

The control systems of manipulation robots may be of diverse nature, as

can be seen from their classification presented in Table 1.1.

This introductory discussion shall give a more detailed description

only of those automatic systems of robotic control that are of practi-
cal importance for industrial application. Reference [1] was served in
some hand as the conceptual basis for writing Sections 1.1 and 1.2 of
this chapter. More informations about all types of manipulation robots
and classification of robots and their control systems, in a broader
sense more may be found in several books of which here will be mentio-
ned only two [2, 3]. To give some basic information about the "non-in-
dustrial" robots, in this chapter we shall present briefly the bio-
technical and interactive robotic manipulators.

Bioteehnieal robots are called the systems requiring permanent invol-

vement of an operator to control the process of manipulator motion.
Unlike the automatic systems, biotechnical systems do not possess the
ability of generating autonomously a desired motion or attaining a cer-
tain goal. A manipulation system moves only when the operator acts upon
it. The simplest kind of these systems is the system of direct control
- by pressing a push-button or a lever the operator effectuates parti-
2

cular degrees of freedom. By releasing the push-button the motion is

stopped at the desired moment. The positioning accuracy in this case
in not high because of the lack of a position feedback. In a strict
sense these systems cannot be called robots but manipulators with com-
manding control.

Another kind of biotechnical controlling systems are various master-

-slave systems. The conventional scheme of a copying system contains an
assigning device which is in a kinematical sense quite similar to the
robot's arm. The sensors placed at the joints of this device supply
signals to the servo systems of the corresponding manipulator joint. In
some cases, the operator's hand is part of the assigning device. Howe-
ver, this device is usually supplied with a joystick by means of which
the operator controls the whole system and brings its tip to a desired
point in space. The robot end-effector is automatically brought up to
the corresponding point in the workspace.

T YP E V A R I ANT

I AUTOMATIC 1.1. Programmable (I generation)

1.2. Adaptive (II generation)
I. 3. Intelligent (robots with elements of
artificial intelligence) (III generation)

II BIOTECHNICAL 11.1. Direct controlled (control of individual

degrees of freedom)
II. 2. Master-slave (of one and two-side action)
I I. 3. Semi-automatic (joystick and computer)

III INTERACTIVE 111.1. Automated (combination of automatic and

biotechnical regimes)
IIL2. Supervised (automatic with functional
commands of goal)
111.3. Dialogue (higher forms of interaction
with operator)

Table 1.1. Classification of manipulation robots

Interactive control systems diminish the permanent operator's involve-

ment in the control process of biotechnical systems. The primary inter-
active systems are the automated systems in which operations are partly
automated and partly controlled by the operator. Another kind of inter-
 3

active systems uniting the human role and the automatic regime is the
system of supervisory aontroZ. On the basis of the information from the
monitor screen about the situation in the workspace the operator sends
(through a control computer) the commands to the robot, e.g. to bring
the gripper (tool) to a certain position. The robot carries out the
operation in automatic regime. When the operation is over, the operator
instructs the robot (by the aid of the computer) to carry out the next
operation, and the procedure is repeated.

1.2 Automatic Manipulation Robots

Basically, four hierarchical levels of the automatic robot control can

be established, each of them involving a computer processing of infor-
mation. On the basis of the results obtained, the computer forms the
appropriate control signals. These control signals are then transfer-
red from the upper to the lower control levels until they reach the
realization of motion (executive system).

The highest level of the control system of a manipulation robot con-

tains elements of artificial intelligence. We think here about those
elements of artificial intellect that enable processing of the sensory
information (visual, tactile, proximity, etc.) along with recognition
the situation in which the robot's arms are acting, and modelling the
scene in one of the appropriate forms. The information are compared to
those stored in the memory as part of a global plan of the robot task,
after which this (highest) level adopts the solutions that are neces-
sary for execution of the required operation in the recognized situa-
tion. At that, the previous working experience of the robot is automa-
tically taken into account, i.e. the robot possesses certain self-te-
aching abilities.

The highest control level receives the relevant information from all
the lower levels and" from various sensors controlling the motion of
robot's arms and the state of the workspace. In this way the feedback
loops are established that form the many-fold contures of a complete
control system.

The next, lower level, is the strategia ZeveZ at which the global task
is divided in accordance with the solution generated at the upper le-
vel, into the elementary operations. The notion of an elementary
4

operation is not strictly defined as it depends on the particular task

and system. For example, elementary operations may be as follows: to
find an object of given characteristics, to pick it up, to bring it to
a point of given coordinates, or to bring it into the contact with
another defined object or tool, to put the object down, to fix it, or
to subject it to certain processing, etc. An elementary operation may
be even simpler (more elementary). For example, the object searching
can be split into several more elementary operations of the type: to
reach a certain point in the workspace, to carry out searching accom-
panied by the appropriate surveying or measuring, to determine the
smallest cross-sectional dimension of the object and to orient appro-
priately the manipulator gripper. The operation of transferring the
object (if the mode of transfer or its trajectory are important too)
can also be divided into several more elementary operations, for exam-
ple: to lift the object to a predetermined height, to carry it along a
circular trajectory, to place it on a surface of defined slope and
with the defined spatial orientation of the gripper. The operation of
joinning two objects can be split as follows: to put the objects on
pins, to take a set of nuts, to fix the nuts according to a certain
sequence, etc. The last operation can be divided into several even more
elementary operations, e.g. after establishing the sequence of screw-
ing, or after ascribing a slow screwing of all the nuts in the begin-
ing, carry out the final tightening according to a certain sequence.

At the strategic level, the degree of splitting the task obtained from
the higher level determines the working algorithm of the strategic le-
vel. The modes of automatic planning at strategic level satisfy certain
criteria of speed, quality and accuracy of execution of the task as a
whole. However, apart from the information from the higher level, the
strategic level uses also the feedbacks to get the information about
all lower levels, as well as of the executive level and the workspace.

At the strategic level are most frequently planned the elementary ac-
tions to be realized by the robot's gripper (e.g. determination of the
position in the workspace the gripper should be brought in order to
pick the object, determination of the trajectory to be followed by the
gripper carrying the object, etc.). The coordinates of the robot's
gripper with respect to the absolute coordinate frame attached to the
robot's base, are termed the externaZ coordinates (a precise definiti-
on will be given in Chapter 2). Therefore, at the strategic level are
planned the trajectories of external coordinates of the robot. However,
the robot's motion is realized via the movements of its particular
 5

joints; most often, each robot's joint is powered by an appropriate

actuator. Therefore, to realize the gripper trajectories planned at the
strategic level, it is necessary to determine how the robot joints are
to be driven.

At the lower control level, tactical level, the external gripper coor-
dinates are mapped to the robot's joints coordinates (the so-called
internal coordinates of the robot), i.e. the required motion is distri-
buted to the particular degrees of freedom (d.o.f) (joints) of the
robot. In other words, at the tactical level is determined the motion
of each manipulator d.o.f. in such a way that their overall motion
realizes all the elementary operations defined at the strategic level.

This would be the basic function of the tactical level. However, as

already pointed out, the elementary operations can be defined in a more
general way, e.g. to find the object of given characteristics (with no
detailed searching and the accompanying surveying), or, to transfer an
object to a certain point in space with no prescribing the trajectori-
es of motion. In this case, a detailed task splitting is carried outat
the tactical level.

The strategic level can be avoided provided the elements of artificial

intelligence give the solution with a sufficiently deatiled sequence of
operations. In such a case, the highest level renders the signals di-
rectly to the tactical level. In practice, depending on the nature of
the task, the functions of levels are generally intertwined. Thus, the
hierarchy should be understood rather in a conditional sense.

Like the higher levels, the tactical level too makes use of not only
the signals supplied by the higher levels, but also all the necessary
feedback information of the lower (executive) level, concerning the
executive mechanisms and the workspace.

Finally, when the necessary movements of particular robot jOints are

determined at the tactical level, they are transferred to the lowest,
executive level, for realization. The task of the executive level is
to realize the given motion of joints in order to perform the desired
functional movements of the robot mechanism, defined at the strategic
level. In this way the executive level realizes all operations requi-
red (ordered) by the higher levels. The executive level realizes the
prescribed movements of joints on the basis of the information sup-
plied by sensors about the instantaneous positions, velocities,
6

accelerations, moments (forces) at the robot joints, i.e., at this le-

vel are realized the feedbacks with respect the position, velocity and
forces at joints. To achieve a high precision in performing the desi-
red movements of joints, and thus to ensure an adequate realization
of the task given by the higher levels, the executive level should take
care of the dynamic characteristics of the robot, i.e. about the inter-
action of the movements of particular robot jOints. From the point of
view of the executive control level, a robot is a complex (multidimen-
sional) system having more inputs and outputs, the system in which
generally exist strong interactions between particular d.o.f. Because
of that, the synthesis of control laws which would guarantee a precise
realization of the motion defined at the tactical level is a very com-
plex task. In this book we shall deal with the problems which are pri-
marily concerned with the synthesis of the executive control level. We
shall consider different control laws ensuring at the executive level,
the realization of operations prescribed by the higher control levels.

The hierarchical control of automatic robots is schematically presen-

ted in Fig. 1.1.

It should be noted that a sub-level is usually inserted between the

strategic and the tactical level. This is the sub-level of the robotia
language. Nowadays, the control and programming of robots is carried
out most frequently through specialized robotic languages. In the ro-
botic language are given all the elementary operations to be performed
by the robot. The introduction of robotic languages is dictated by the
need of simplifying the procedure of giving instructions, which in it-
self may be a complex task. Robotic languages are also included into
the robot controllers. Because of that, the strategic control level is
often connected to the robot through the language, i.e. this level
should give the robot instructions (desired operations) in the form of
a predetermined set of instructions in the given robotic language. Ho-
wever, the sub-level of robotic language has also the task of checking
the consistency of the given elementary operations. In principle, this
sub-level can be avoided, i.e. it is possible to couple directly the
strategic level to the tactical control level. In such a case, however,
it is necessary to transfer some of the functions of this sub-level to
the strategic level.

Since the functions of the highest and strategic level are extremely
complex, they often cannot be realized in the course of robots motion
and task execution (on-line), but should be carried out before the
 7

robot begin the work (off-line regime), as is indicated in Fig. 1.1.

This means that the highest control level defines the task, and the stra-
tegic control level, on the basis of the sensors' information, plans
particular robot operations and, by means of the robotic language,
gives the appropriate instructions. When the instruction on the desi-
red motion is given (in the robotic language) to the tactical level,
the task itself is then realized in on-line regime. However, such a
design does not allow adaptation of the robot functioning to the chan-
ged conditions during the task execution. Because of that it is pre-
sently endeveured to realize the strategic level in on-line regime too,
i. e.· to plan robot's operations during the task execution itself and in
accordance with the instantaneous situation in the work space. Since even
very powerful computers are hardly able to achieve a sufficiently fast.

r--
off-line regime
Analysis of situation (environment)
Defining the task and choice of the
solution

Strategic control level:

Planning trajectories

L J
I Tactical control level:
Transforming the external into ......- - - - - - - ,
l
I I
the internal coordinates

Executive control level:

I Realization of internal
trajectories (servo systems) I
I I
I SENSORS
I
J
Fig. 1.1. Schematic diagram of hierarchical control of robotic systems
8

planning of elementary robot operations, only some simpler functions of

the strategic level can be realized in on-line regime. Because of that
the more complex operations are defined in the robotic language, whereas
the strategic level on the basis of the sensors information about the
actual situation in the workspace plans the elementary movements (e.g.
the motion of the robot gripper to avoid collision with some movable,
i.e. changeable obstacles in the work-space). The corresponding hie-
rarchy of robot control is presented in Fig. 1.2.

Robotic language

Strategic control level:

determining the trajectories

Trajectories (Obstacles modelling)

in external
coordinates

Tactical control level:

transforming the trajectories
from the external into the
i nterna 1 ones

Trajectories
of joints

Executive level:
servo systems ensuring the
realization of desired
trajectories

signals

Robot I S state

Fig. 1.2. Hierarchical structure of the robot control

It should be also noticed that the programmable robots (first-genera-

tion robots) or the automatically programmable manipulators have no
higher control levels, but only the tactical and executive level.
 9

Adaptive robots (robots of second generation) too, have no the highest

control level, i.e. the level of artificial intelligence; they possess
the other three control levels.

With the adaptive robots, the strategic level receives the task from
the operator in the robotic language, as shown in Fig. 1.2., and the
robot controller plans automatically the gripper's trajectories. For
the first-generation robots, the operator has to prescribe the trajec-
tories, i.e. positions of the gripper in a direct way and determine the
trajectories (for avoiding the obstacles, picking up and transferring
the object) through the appropriate programming language, or through a
teaching box (see Sect. 1.5).

1.3 Classification of Automatic Robots

There are four types of automatic manipulation robots: with fixed

(rigid) programmes, programmable, adaptive, and "intelligent".

The term "generation" is often used instead of "type". As manipulators

with fixed programmes in fact are not, robots, they can be considered
as the "zero" ("pre-robotic") generation. Hence, the programmable ro-
bots are considered as the first, the adaptive as the second, and the
intelligent ones as the third generation.

Manipulators with fixed programmes have a control system which is not

programmable. These are simply mechanical arms. They are firmly linked
to the technological equipment, subdueing themselves to a certain tech-
nological process as a whole. Their application is particularly charac-
teristic for substituting manual work in mass production, e.g. on as-
sembly lines of watch mechanisms, and similar. However, we are not
going to consider the problems concerning design and control of these
manipulators.

Programmable robots (first generation robots) have the controlled dri-

ves at all joints and their control system is easily adapted to vari-
ous manual operations. However, after each adjustment these robots re-
peat one and the same fixed programme under strictly defined conditi-
ons and with fixed arrangement of objects. The majority of contempora-
ry industrial robots is of such type and they are applied for perfor-
ming some auxiliary operations in pressing, welding, casting, machine
tools servising and similar. These robots demand technological arrange-
ment in the work environment and position of parts.
10

Adaptive robots (second generation robots) are such robots that can, to
a higher or less degree, orient themselves independently in the envi-
ronment which is not fully determined, and to which they can adapt.
They are equiped with sensors reacting to the situation and with an
information data processing system aimed at generating adaptive data
signals, i.e. flexible changes in the manipulator motion programme ac-
cording to the real situation. In their modern versions, compact micro-
processor systems are widely used. Adaptive industrial robots are ne-
eded in all cases when it is difficult to ensure a strictly defined si-
tuation, when avoiding obstacles, working with parts on conveyers in
assembly operations, in arc welding, painting, applying protective la-
yers, and other operations.

Intelligent robots (third generation robots) are equiped with a more

varied range of sensors with the microcomputer processing of informa-
tion, recognition of situations, automatic generation of the solutions
for further actions by the robot itself, aimed at performing the nece-
ssary technological operations in an undetermined environment. These
robots possess elements of artificial intelligence.

Transducers (sensors) are used with the robots of second and third ge-
neration. With the second generation robots, the force transducers,
tactile, proximity (ultrasonic) and similar sensors can be used. The
third generation robots are characterized by the presence of a complex
of sensory devices, including technical vision, which, together with
the advanced microcomputer data processing, forms an artificial intel-
lect itself, i.e. the behaviour of the robot is more self-contained,
and to a certain degree corresponds to the rational human behaviour in
the process of working activity. Besides, the complex of sensory devi-
ces may include the equipment for controlling quality of products and
properties of the environment, in case this is demanded by the automa-
tic regulation of the working regime.

1.4 Characteristic Tasks and Applications of Robots in Industry

There is hardly any industrial production in which application of in-

dustrial robots has not been, at least, attempted. Robots are increa-
singly used in the following operations: arc welding, spot welding,
pressure casting, spray painting, tool machine servicing, material
 11

handling and paletizing, treatment of castings, applying glue and seal

layers, assembling, etc.

It is estimated that presently in the U.S.A., for example, 50-60% of

robotic technology belongs to the car industry, mainly the jobs of wel-
ding and spray painting. A similar situation is also in the West Euro-
pe. In Japan, in the car industry is engaged below 30% of the inbuilt
robotic technology, as robots are prevailingly used in the electronic
and similar industries. A forcast for the future is a sharp increase in
the number of robots engaged in the assembly and production control
jobs. This prediction particularly holds for the electronic, car and
some other industries.

To get an insight into some characteristic applications of industrial

robots we shall briefly review some of pertinent technological proces-
ses.

1. Pressure casting

This procedure of producing high-quality parts from molten non-ferrous

metals and alloys, consists in injecting the necessary amount of metal
melt into a special mould, usually made of two steel parts.

Generally, these are two basic procedures of pressure cassting: casting

from a hot chamber and cold-chamber casting. The former procedure is
faster since it saves the time needed to transport the melt from the
melting pot to the mould. Hence, this more recent casting method is
more interesting for robotization.

However, the procedure of pressure casting involving no human may have

certain drawbacks. As first, it is the need for manufacturing the
moulds of better quality in order to minimize the possibility of ad-
hesion of castings.

For economy reasons, one industrial robot and one cooling equipment
usually serve two horizontal pressure casting machines. The Japanese
robotic industry has produced several types of simple industrial robots
that are directly installed onto the casting machines. Of the several
millions of working hours of the robotized machines employed in pres-
sure casting in the U.S.A., less than 3% has been lost because of the
mis-functioning of the robots.
12

2. Spot welding

The technique of electric spot welding is presently widely used in the

technology of joining parts of metal sheets, especially in car indus-
try for connecting the body parts, in the industry of electrical house-
hold appliences, etc.

The application of industrial robots for this purpose began in 1969,

when General Motors introduced a line of 26 UNIMATE robots to do the
job of car body welding. Only a year after, a line of robots was in-
stalled for the same purpose in the Mercedes factory of Daimler Benz.
Today, more than 4000 robots is engaged in the jobs of spot welding in
car industry which has reamined the main field of this application.
The highest efficiency that has been achieved on such a line is about
100 car bodies per hour. Some of car manufacturers, e.g. volkswagen
and Renault, have developed special industrial robots only for this
purpose. The most modern lines for the car body production, including
their painting, are ROBOGATE with Fiat, and VOLVO in Geteburg, with a
capacity up to 60 cars an hour.

2. Arc welding

The proces of arc welding is especially unhealthy to workers becauseof

the presence of a strong and harmful ultraviolet radiation and evolving
smokes that are rather poisonous. This is a typical example of a job
where the replacement of man by robot represents a genuine humanizati-
on of labour. To get a welding seam of a high quality, it is necessary
to ensure not only a precise positioning of the tool tip along the
seam, but also an appropriate speed. To ensure the weld gun follows
the seam and in each time instant has the right position with respect
to it,S active d.o.f. at least, are needed. In order to obtain a good
sem it is necessary to use also the external sensors which should en-
sure good welding parameters. The advanced robots for arc welding pos-
sess an increasing adaptability, i.e. the control systems of higher
performances. In the U.S.A. there are even today several hundred thou-
sand jobs where arc welding is done by hand. Robots are increasingly
used in a similar technological process of oxyacetylene cutting.
 13

4. Forging

The present-day use .of industrial rebets fer this technelegical epera-
tien is relatively medest. A very interesting example .of such applica-
tien is the preductien .of punched links chains. Te everceme the limi-
tatiens .of rebets applicatien fer ferging eperatiens caused by diffi-
culties in centrelling quality .of fergings, the attempts have been
recently made te intreduce suitable sensery devices inte the rebet
centrel system. Presently, use is widely made .of pheteelectric and
thermal sensers whese task is te ensure the centinuity .of the techne-
legical precess itself, and increase its reliability and safety. These
sensers are mest .often in-built in the apprepriate place .of the rebet
gripper.

5. Spray painting

The preblem .of spraying a paint frem its selutien te ferm a layer which
will dry fast and thus prevent the .occurrence .of a preductien bettle-
-neck due te the shertage .of man-pewer, is especially present in car
industry. When the parts .of mere cemplicated shape are invelved, asfer
example, the car bedies .of different type (processed on the same pre-
ductien line), the use .of autematic cabins .or tunnels weuld be very
cemplicated and expensive. Hence, the use .of industrial rebets with
their high capacity .of adapting themselves te different kind .of pre-
ducts, is the legical selutien te the preblem. The werld leader in this
field is the Nervegian cempany TRALLFA Niles Underhaug; the number .of
specialized rebetic units installed by this cempany is higher than in
any ether car cempany. Since spray painting is an attractive area .of the
applied industrial rebetics, a number .of cempanies, apart frem TRALLFA,
is engaged in develeping rebets fer this purpese. In additien, the le-
ading car facteries and ether similar cempanies are invelved in the
prejects .of develeping the fully-autemated and rebetized lines fer
spray painting.

6. Application of robots for tool machines servicing

This is .one .of the mest impertant applicatien fields .of industrial re-
bets. They are mainly empleyed with these universal teel machines that
are used fer preductien .of larger series, but which are net yet large
eneugh te justify the investment inte buying fully autemated machines
fer preducing the same parts. Hewever, the applicatien .of industrial
rebets in manufacturing .of smaller series preducts can be justified by
14

the possibility of quality standardization and increased capacity of

the production. In such a case use is made of the properties of high
flexibility of the robotic control system. Thus different production
programmes covering a wide assortment of products, can be easily stored
and used when needed.

In recent years, the industrial robots for small-series production have

been more often integrated into the groups of numerically controlled
(NC) machines, making thus a fLexibLe production celL or, wider, afle-
xible technoLogical line.

7. Application of robots in thermaL treatment processes

Thermal treatment is an almost unavoidable phase in the modern techno-

logy of metallic products. It includes hardening, improving, tempering,
browning, and all other treatment procedures involving the exposure of
the work-piece either to an abrupt change of temperature or keeping it
at a fixed temperature for a prolonged time. The application of indus-
trial robots in these operations is expediential and is already today
rather widespread. However, a serious problem arises as how to protect
the robot's gripper from the frequent action of high temperatures of
the work-piece. This is achieved either by using special materials, or
by cooling the gripper parts with water or some other cooling agent.

8. Application of robots in glass industry

Glass making, and especially production of sheet glass, is a technolo-

gical process which is rather hazardous to workman. The fact that glass
in some production stages is at very high temperature makes its hand-
ling very difficult. In the case of the sheet shattering, small sharp
pieces of glass scattered around can cause severe, and even mortal,
injuries to people in the working environment. In the production of
cathode tubes (TV screens), the products are heavy, hot and very ex-
pensive. Thus, apart from a genuine humanization of the labour, the
application of robots can substantially decrease wast due to breakage.

Recently, industrial robots have found some unconventional applicati-

ons. Thus, due to their high accuracy in positioning and especially due
to the accuracy of the robot arm position sensors (encoders, resolvers,
measuring laths) robots can be used instead of the so-called "measu-
ring machines", which are the extremely expensive, motor-driven
 15

rectangular measuring systems of large dimensions, used to control

large-size products (aircrafts, ships, vehicles, etc.). Fig. 1.3. shows
a sketch of such a machine for measuring the airplane body dimensions
by the aid of two measuring robots which are "fingering" the object at
certain characteristic points; the measurement data are converted into
dimensions of the working object in its own coordinate frame and stored
in the memory of the control system and finally protocolled in a prin-
ted form. Another, quite different, unconventional use of industrial
robots is illustrated in Fig. 1.4: an industrial robot ("PUMA" type)
mounted onto a movable robot ("ROBOCAR") with the purpose of servicing
several working positions in the so-called "clean" rooms for electro-
nics and other highly sofisticated technologies.

Fig. 1.3. A measuring robot

Fig. 1.4. A mobile robot with manipulator

16

1.5 Implementation of Control of Manipulation Robots

There are a number of factors which should be considered when the robot
control synthesis is concerned, viz. the nature of tasks to be carried
out, structural and dynamic characteristics of the robot, the equipment
available for control realization (sensors and microcomputers), etc.
In the preceding sections we presented a variety of tasks of diffe-
rent complexity that can be carried out by robots. The robot design,
the choice of its geometrical characteristics (number and type of
joints, mutual position of joint axes, link length, etc), and the cho-
ice of appropriate actuators are all determined by the class of desig-
nated task. The class of the task determines to a great measure the
complexity of the robot control system, i.e. the choice and solutionof
the control. For example, if the robot is to perform simple operations
(such as the transfer of an object from one place to another in an ob-
stacle-free work-space and with the object position prescribed in ad-
vance) a simple robotic structure may be adopted, and the control sys-
tem reduced to the executive level only. Thus, the desired robot posi-
tions can be given directly to the internal robot's angles, i.e. the
positions to be attained by the particular robot jOints in order to
bring the robot to the desired position are prescribed in a direct way.
However, in the case of more complex tasks (e.g. the transfer of work-
-pieces in the work-space with obstacles, etc) the robot control should
be hierarchical, as it has been described above.

On the other hand, the chosen robotic structure and its dynamic cha-
racteristics determine to a great extent the complexity of laws of the
robot control. In the chapters to follow we shall see that in case when
the realization of a precise positioning of a robot and/or tracking of
the desired gripper trajectories in the work-space is concerned, it is
necessary for some robotic structures to take into account the dyna-
mics of the manipulation robot, which substantially complicates the
control system of the robot.

However, in the synthesis of the robot control special attention should

be paid to the possibilities of implementation of a chosen control law.
Obviously, the more complex control law is chosen, the more complex
will be its implementation. When choosing a suitable robot control one
should adopt the one which is as simple as possible from the viewpoint
of its implementation, but which will, on the other hand, satisfy the
given requirements. However, this book is not specially concerned with
 17

the problems of robot control implementation [3]. We shall only briefly

point out some of the aspects of realization of robotic control units
which should be taken into account when the choice of appropriate con-
trol laws is concerned.

As with other systems, the robot control may be realized by analogue,

hybride or microprocessor technique. The analogue technique of robot
control is characteristic of the manipulators of the "zeroth" genera-
tion whose control system is reduced to the executive level only, i.e.
to the programming units controlling mechanical arms. Such a control is
practically abandoned in the present-day robots, so that we can say
that a microprocessor is an obligatory part of any robot control sys-
tem. The use of microcomputers has enabled an easy changing of the task
to be performed (the so-called, reprogramming of robots), and this is
an important advantage of robots over classical manipulation mechanisms.
In order to be able to perform various complex tasks mentioned above,
the robot control systems should comprise both strategic and tactical
level. These control levels can be practically realized only by the aid
of computers.

The advanced robot control system should enable communication with

other computers, with the operator (through both the terminal and the
robot teaching unit), with the sensors in the work space, and with the
robot itself (Fig. 1.5).
HOST Cm1PUTER
SENSORS

~----~
Y-------
0000

11111111
~----~~
0000

CONTROL
SYSTEM OF
ROBOT

MANIPULATOR

TERMINAL PRINTER TEACHING-BOX

Fig. 1.5. Robot control system
18

The communication with another computer (host computer) should ensure

the coordination and synchronization of the robot's work with its tech-
nological surroundings (other robots, NC machines, moving tracks, etc).
Such a communication is of special importance for incorporating robots
into the flexible technological systems in which one central computer
is controlling several other computers and NC machines, conveyers, all
being involved in a flexible technological cell or line. In some cases
it is necessary to realize the communication between the robot control
system and another computer for processing sensors data (e.g. signals
obtained from the camera), since the information supplied by the sen-
sors are necessary for solving the task at the strategic control level.

The communication between the robot control unit and the user is reali-
zed either through a terminal or a robot teching box. In communicating
through a terminal the operator uses various high-level languages to
programme the task the robot is to carry out (see Sect. 1.2) . An easier
way of instructing the robot is by using a teaching box. This unit
enables an easy programming of simple tasks, and by combining them
(using a robotic language) it is possible to handle more complex tasks.
The teaching box usually has a certain number of keys for giving the
robot proper instructions, e.g. to move the gripper to the desired di-
rection, to move particular robot jOints, etc. Often, the task assigne-
ment can be realized by means of the so-called "pilot-robot" which has
the same kinematic structure as the robot itself and is supplied with
the sensors for measuring the jOint coordinates, but having no actua-
tors. The operator drives the pilot along the certain paths that should
be followed by the robot when executing the given task; the control
system read out the sensors data about joints positions and memorizes
them. When the pilot-robot motion is recorded, the robot can reproduce
the given motion on the basis of the data stored in the memory.

The control system receives the information from the sensors in the
work space and they are used in planning the robot trajectories at the
strategic control level. If the sensor information are such that no
substantial treatment is needed, they are introduced directly to the
microcomputer of the robot control unit. On the other hand, if the
sensors information needs further processing, this is done, as mentio-
ned above, by means of separate computers. Often, the robot control
unit has to follow the sensors information during the robot motion and
on the basis of such information "change" both the motion and operati-
on under execution (see Fig. 1.2).
 19

Hence, an advanced robot control system should ensure all the above
communications and, on the basis of the assignement received from the
operator (through the programming language, teaching box, pilot) or
from the higher control level (i.e. central computer), as well as on
the basis of the sensory system information, plan the actions to be
carried out by the robot. As we have explained above, the motion of the
robot gripper is usually planned at the strategic level, i.e. the tra-
jectories to be followed by the gripper are determined at this level
and then transmitted to the tactical level.

At the tactical control level, the assigned gripper motion is mapped

into the robot's jOints trajectories (see Chapter 2). The execution of
such mapping generally requires a lot of numerical calculation (addi-
tions, square roots, multiplications, calculating of trigonometric
functions, etc). In order the robot could precisely realize the desi-
red motion, the mapping of desired positions (and velocities) of the
robot gripper into the corresponding positions (and velocities) of the
robot jOints should be carried out every 15 - 30 [ms] *), i.e. every
15 30 [ms] the gripper coordinates on the desired gripper trajecto-
ry should be mapped into the joints coordinates. This means that the
microcomputer should complete each transformation within 15-30 [ms],
i.e. it should be capable to carry out the necessary numerical calcu-
lations with a speed which allows the completion of this mapping during
this time interval.

When the desired positions and veloci ties are determined, they are realized
at the executive level. The executive control level should permanently
generate the signals to the inputs of the actuators whose task is to
drive the robot joints into the desired positions, i.e. to effectuate
the joints to move with desired velocities and accelerations. The sig-
nals at the executive level are generated on the basis of both the tra-
jectories received from the tactical level and the information obtained
from the inner sensors (i.e. the robot's sensors supplying the infor-
motion about the actual positions of joints, their velocities and ac-
celerations) .

The executive control level can be realized either by the analogue

technique or by the aid of microprocessor. In the former case, the
robot control unit is provided with a computer for implementation of

*) This is an average "speed" (sampling rate) needed to carry out the

mapping at the tactical control level.
20

higher control levels and with the analogue servo systems with actua-
tors to move the robot joints. Every 15-30 [ms] the tactical level in
the microcomputer calculates all the necessary positions and velociti-
es of robot jOints, and, by a D/A converter sends them to the analogu-
es control device for realization (Fig. 1.6). The analogue servo sys-
tems realize the prescribed positions (trajectories) of joints on the
basis of the information from the robot sensors. This means that,
starting from the prescribed trajectories (signals from the D/A con-
verters) and signals from the sensors, the analogue controller gene-
rates the appropriate signals for the actuators at the robot jOints.
However, because of the existence of strong dynamic interactions bet-
ween the individual d.o.f., mentioned above, the control law at the
executive level has to be very complex for certain robot types and the
tasks imposing rigorous requirements with respect to the accuracy and
speed of their realization. The analogue technology is not suitable
for implementation of complex control laws. In the case the robot con-
trol system is realized by the aid of simple servo systems which inde-
pendently control the individual jOint actuators (see Chapters 3 and 4)
these laws are realizable by analogue technique. However, if the con-
trol has to include the compensation of complex dynamic interactions,
occurring between the motion of particular jOints, the realization of
an analogue control device is much less suitable if compared to the
implementation of microprocessors. On the other hand, the digital
technique has substantial advantages as for the possibility of alte-
ring and adjusting the control laws, as well as from the viewpoint of
maintenance, reliability, and robustness under variable external con-
ditions [4).

For these reasons, microcomputers are increasingly used in the imple-

mentation of executive control level (Fig. 1.7). Thus the microcompu-
ter appears as a site for implementation of all control levels. The
microcomputer receives the information about the instantaneous states
of robot joints from the sensors through A/D converters, if the sen-
sors used do not supply the information in a digital form. On the ba-
sis of these information and the robot trajectories determined at the
higher levels, the microcomputer computes the appropriate control sig-
nals, which are through D/A converters fed into the inputs of the ac-
tuators*). Calculation of the signals for actuators is carried out

*) It should be noticed that between the D/A converter and actuators

are inserted the appropriate amplifiers, not shown in Fig. 1.7
Central Computer
(or Opera tor)

ANALOGUE
NICROCOMPUTER
(STRATEGIC AND SERVO SYSW~S
TACTICAL LEVEL)
(EXECUTIVE LEVEL)

Fig. 1.6. The robot control unit with analogue implementation

of the executive control level

mCROCOMPUTER ACTUATOR
(STRATEGIC,
TACTI CAL AND
EXECUTIVE LEVEL) ACTUATOR

Fig. 1.7. The robot control unit with digital implementation

of the executive control level

~
22

according to the selected control law and selected parameters, using a

suitable programme module. As advanced microcomputers possess the trans-
lators for high-level programming languages, the programming on a micro-
computer is relatively simple, which enables an easy change of the
control law. The designer of the control unit should only change (in
high-level language) the program module of "executive level". On the
basis of the desired values for coordinates in a given time instant
(calculated in the corresponding programme module which implements the
tactical level) and the values of the actual coordinates (i.e. angles
and velocities) as well as the moments (forces) at the manipulator
joints (obtained through an A/D converter), this module calculates the
signals to be supplied (through a D/A converter) to the actuators in-
puts. In this way, one and the same hardware (microcomputer) can be
used for the implementation of different control laws. Moreover, such
direct digital control of robots enables the development of a general
purpose controller which would be (after minor alterations, mainly of
the software) applicable for robots of different types and structure
[51, which is practically unattainable by the analogue technique.

However, the realization of a digital robot control system is associa-

ted with certain problems. Namely, microprocessors require some period
of time for execution of each mathematical operation. Thus, the con-
trol microcomputer needs certain amount of time to compute (on the ba-
sis of a given control law) the control signals for the actuators. The
time interval between the moment the microcomputer had read the values
for coordinates from the sensors (i.e. from the A/D converters) and the
moment it computed the signals and sent them over (through D/A con-
verters) to the actuators, is (conditionally) called the sampling peri-
od*). During this interval one and the same values of signals are sent
by the microcomputer to the D/A converters, i.e. the actuators. To di-
minish the influence this microcomputer delay has on the robot's beha-
viour (i.e. on the precision of realization of the prescribed trajec-
tories), the sampling period should be as short as possible in compa-
rison to the time constants of the robot dynamics. It appeared that
the sampling period with the majority of modern robotic systems should
be below 10 [ms1. It means that in, let say 5 [ms1, the microcomputer
should (on the basis of the instantaneous values for robot coordinates
and in accordance with the chosen control law) calculate the control
signals and send them (through D/A converters) to the actuators.

*) A more precise definition of the sampling period can be found in

reference [4].
 23

Different control laws require different number of numerical operations

to be executed to compute the appropriate control signals. If more com-
plex control laws are involved, the number of operations to be executed
by the microcomputer during the sampling period is normally higher.
Certain simpler control laws require about 30-40 numerical operations
(additions and multiplications), whereas some others may require seve-
ral hundred operations per sampling period. The microprocessor must be
sufficiently fast to execute the necessary number of operations in 5-10
[msl. The 8-bit processors can hardly achieve the calculation speed
needed for the control of advanced robots*). Because of that the 16-bit
microprocessors are used in advanced robotics, together with numerical
co-processors which enable an accelerated execution of different nume-
rical operations (addition, multiplication, square root, etc). Numeri-
cal co-processors ensure the realization of arithmetics with floating
point which is necessary for calculating more complex control laws.

However, even the advanced 16-bit microprocessors with numerical co-

-processors, capable of doing multiplications in 20 to 30 [psl, cannot
calculate more complex control laws during the allocated sampling pe-
riodof5-10[msl. Besides, it should be borne in mind that the micro-
processor has to realize not only the calculation of control signals
according to the chosen law, but also to carry out the appropriate
processing of the information obtained from sensors (their scaling,
etc) as well as processing of the calculated control signals which are
sent to the D/A converter, in addition to different checkouts, con-
straints, and so on. Apart from the executive level, the microproces-
sor should also implement the tactical level which includes, as we have
already mentioned, the mapping of the external coordinates into the
joint robot coordinates, which has to be done every 15-30 [msl. This
mapping may require several hundred numerical operations, so that the
microprocessor should also accomplish these calculations in the allo-
cated time. If the control unit includes the strategic control level
(or, at least some of its elementary functions that has to be realized
on-line, during the robot's motion), then, the microprocessor should
ensure the execution of the necessary operations for this level too,
and all that during the assigned time interval. The present-day com-
mercial microprocessors cannot perform all the necessary calculations
during the sampling period imposed by the robot dynamics (less than 10 [msl).

*) The 8-bit processors are even today used with manipulators for some
simple tasks; the executive level is realized either by the analo-
gue technique or by the aid of a number of such microprocessors in
parallel (see below) .
24

For this reason, use is made of several processors working in a pa~al­

lel, but which can exchange data between each other through a common
memory. Usually, one of the processors is dedicated to the strategic
control level which communicates with the user and the higher control
level (in the host computer). The tactical control level is imple-
mented by second processor, while the third one is dedicated to the
executive control level. Thus, the robot control is realized by three
processors. However, this number of processors is often insufficient.
Hence, more complex control laws have to be realized at the executive
level by means of several processors. Similarly, for the realization
of complex algorithms and treatment of sensory data at the strategic
control level, several processor should also be employed. The use of
a large number of processors in parallel may substantially complicate
the realization of the control unit and, on the other hand, affect the
reliability of the work of the whole system. Therefore, the present-
-day 16-bit processors (together with numerical co-processors) are not
yet capable of implementing some more complex algorithms at the stra-
tegic control level; most often one ought to be satisfied with some
simpler solutions that enable performing certain specific tasks. Fur-
ther advances in microprocessor technique and the enhancement of cal-
culation speed will bring the progress in general problem solving at
the strategic level.

On the other hand, the control law chosen at the executive level should
be as simple as possible (requiring the smallest number of operations),
but which can satisfy the desired requirements, in order to avoid the
application of a large number of processors in parallel. However, im-
plementation of many of the complex control laws requiring the calcu-
lation of robot's dynamic forces, demands application of several 16-
bit processors in parallel. Such a realization is rather complicated,
so that such control laws are not used with the majority of commerci-
all available robots. Therefore, one of the important criteria in cho-
osing the robots control laws, imposed by the control realization, is
the requirement for the lowest possible number of numerical operations
to be carried out in calculating control signals during the sampling
period.

In the realization of robot control unit, the requirements concerning

the memory of the microcomputer should be also taken into account. The
standard RAM (Random Access Memory) capacity of advanced robotic con-
trollers is between 64 KB (of the 8-bit words) and 1 MB, which depends
 25

on the complexity of the algorithm for the control calculation and the
amount of data to be stored. It should be pointed out that certain
control laws may require a large memory capacity for storing the data
calculated (off-line) in advance and which are used during the on-line
robot control (see Chapter 4). Those robots which should perform com-
plex motion for a prolonged time demand the storing of a large amount
of data about the coordinates that are to be realized. The application
of these robots is characterized by the use of peripheral magnetic me-
mories, such as floppy and hard disc. The function of these peripheral
memories is to store (either in the appropriate robotic language, or
in the form of a set of coordinates which the robot has to realize du-
ring its motion) the programmes for various tasks to be assigned to
the robot for execution when needed. Thus, various tasks can be pro-
gran~ed in advance, the programmes stored in the peripheral memory
units, and used afterwards in the course of realization of the indus-
trial process.

All these factors concerning the implementation of the robot control

should be taken into account in the robot control synthesis. It should
be mentioned that in the implementation of robot control by micropro-
cessors, special attention should be paid to the accuracy of calcula-
tions. The data from the sensory system reaching the microcomputer,
either directly in digital form or through an AID converter, are most
often 12 - or 16-bit. As we have already mentioned, calculation of the
control is also carried out by 16-bit processors. In this way, a suf-
ficient accuracy can be ensured for the majority of the present'day
robots and their applications (Le. the microcomputer truncation does not
significantly affect the robot's behaviour). However, for some speci-
fic tasks and some robots, a higher calculation precision may be re-
quired. Besides, in the robot control synthesis, care should be taken
of the reliability and robustness of the chosen control, i.e., it
should be ensured the control is implemented in a reliable manner
which is not sensitive to possible disturbances that might occur in
the external conditions.
26

References

[1] Medvedov V.S., Leskov A.G., Juschenko, Control Systems of Manipu-

lation Robots (in Russian), Nauka, Moscow, 1978.

[2] Vukobratovic M., Applied Dvnamics of Manipulation Robots: Model-

linq, Analysis and ExaMPles, Sprinaer-Verlaa, 1989.

[3] Vukobratovic M., (Ed.), Introduction to Robotics, Springer-Verlag,

1989.

[4] Cadzow A.J., Martens H.R., Discrete-Time and Computer Control Sys-
tems, Prentice-Hall, Englewood Cliffs, New Jersey, 1970.

[5] Vukobratovic M., Stokic D., Kircanski N., Non-Adaptive and Adapti-
ve Control of Manipulation Robots, Scientific Fundamentals of Ro-
botics 5, Springer-Verlag, 1985.

[6] Popov E.P., Vereschagin F.A., Zenkevich S.L., Manipulation Robots:

Dynamics and Algorithms, Series: Scientific Fundamentals of Robo-
tics, (in Russian), "Nauka" , Moscow, 1978.

[7] Albus J.S., McCain H.G., Lumia R., NASA/NBS Standard Reference Mo-
del for Telerobot Control System Architecture (NASREM), NBS Tech-
nical Note 1235, 1987.

[8] Engelberger J.F., Robotics in Practice, Kogan Page, 1980.

Chapter 2
Kinematic Control Level

2.1 Introduction

As pointed out in Section 1.2, the tasks for which robots are applied
in industry and other fields are of very diverse complexity. The more
complex the task a robot has to perform and the more strict the requi-
rements for its performing, the more complex should be the control sy-
stem of the robot. The complexity of the robot control also depends,
as it will be shown in Chapter 4, on the robot's mechanical structure,
i.e. on the extent and mode the motion of one joint (mechanical degree
of freedom) of the robot influences the other joint. Because of that,
different "types" of control systems appear in practice, which in dif-
ferent ways solve the problems at both tactical and executive level
and enable accomplishment of tasks of different class. As will be shown
below, the "types" of control are most often related to different clas-
ses of tasks in robotics, which, on the other hand, have different re-
quirements toward the executive control level. This chapter deals with
the problems concerning the tactical control level, while the subject
of the coming chapters will be the synthesis of control at the execu-
tive level.

2.2 Direct and Inverse Kinematic Problem-Determination

of Robot Position

The most elementary task that appears in robot control is to bring the
robot to the desired position of the workspace. If we consider the
tasks encountered in robotics (Section 1.4) we realize that one of the
simplest tasks is the transfer of an object from one position to ano-
ther (Figure 2.1). What is needed here is to bring first the robot
from the initial position (A) to the workpiece, at such a distance
from where the gripper can take hold of it (B). To execute the task,
the gripper has to be oriented in a proper way as to catch the work-
piece. The position of the manipulator is, therefore, defined by the
desired position and orientation of the gripper in the workspace and
these are determined by the position and orientation of the workpiece
28

itself. The next step is to grip the workpiece *). After that, the work-
piece should be transferred to a new position. This new desired posi-
tion of the workpiece determines the new position of the gripper, and
thus, requires a new position of the manipulator (C). If the work spa-
ce is not containing obstacles, and if there are no limitations upon
the mode of transferring the workpiece (e.g. if the orientation of the
workpiece is not important, in contrast to the case when a vessel con-
taining liquid is to be transferred, etc), the manipulator can move
from B to C in an arbitrary way. When the workpiece is brought to po-
sition C, the gripper opens, and the workpiece (having acquired the
desired position in the workspace) is released. The robot is then po-
sitioned again at the initial position A, or it goes directly to posi-
tion B.

Fig. 2.1. The task of transferring a workpiece

from position B to position C

We have described this simple task to show that the only problem of
practical importance in its execution is the accurate positioning of
the robot and the workpiece which it is carring, 1. e. the brin<;Jin<;J of the
robot gripper to a desired position and acquiring the adequite orien-
tation in the workspace. Such robot positioning is involved in many
tasks, described in Section 1.4, either as the only task, or as a sub-
task in a complex assignement. For example, spot welding requires a
precise robot positioning in regard the positions needed to carry out
the operation. In parts assemblying, the problem of accurate position-
ing arises at the state of bringing the workpiece to the site of parts
mateing, though the operation of assemblying is more complicated

*) In a general case, the gripping phase can be complex too (in the
case the shape of the workpiece is not defined in advance), but
these problems are not going to be dealt with here.
 29

(as it will be shown in Chapter 7). Thus, all robots have to ensure
the possibility of versatile positioning, and they differ with respect
to the mode the positioning is programmed and executed, and the acou-
racy the desired positioning is accomplished*). It should be noticed
that mechanical arms with open feedback have also the possibility of
positioning but their capability of reprogramming the desired positi-
ons and assigning new positions is very limited, if compared to robots
(as it was explained in Section 1.3).

The primary question arising in robot positioning is, how to assign a

desired position to the robot. In principle, there are two ways of as-
signing the robot's coordinates:

a) through the so-called internal coordinates of the robot (or the

robot joints coordinates), or

b) through the so-called external robot's coordinates.

In the first book of this textbook series [1] we have explained some
basic concepts concerning the kinematics and dynamics of robots, such
as mechanical configuration of the manipulator, link, kinematic pair,
kinematic chain, etc. Internal robot coordinates are defined by the
scalar values describing relative position of the one link with res-
pect to another link of the same kinematic pair. The internal coordi-
nate for a revolute joint is the deflection angle at the joint, where-
as, for a translational kinematic pair, it is the linear displacement
along the joint axis. In Fig. 2.2. the internal coordinates are deno-
ted by qi, and the vector of internal coordinates of a robot having n
simple joints (degrees of freedom) is q = (q1, q2, ••. ,qn)T. (The defi-
nitions of a revolute and a linear jOint are given in [1]).

The external robot coordinates are the Cartesian coordinates of a ter-

minal robot link (gripper) with respect to the absolute coordinate
frame attached to the manipulator base, or, to any other point in the
workspace, as well as the gripper orientation with respect to the ab-
solute coordinate frame (i.e. the angles between the terminal robot
link and the axes of the absolute coordinate frame). Therefore, the

*) It should be mentioned that the task of robot positioning is the

most elementary one, though, as it will be shown later, the requi-
rements put before modern robots may be much more complicated (to
follow a space trajectory, etc).
30

vector of external coordinates s consists of the coordinates xc' Yc'

Zc of a point on the gripper (e.g. its inertia centre, or the tip) with
respect to the absolute coordinate frame and the angles e, ~, ~, which
the axes of the coordinate frame, attached to the gripper, form with
the axes of the absolute coordinate frame (Fig. 2.2).

z
z" Z=ZI

./::r
ylll

q'=y"
G

x
y

x
Fig. 2.2. Cartesian coordinates of a manipulator gripper

The yaw angle ~ corresponds to the rotation about axis z of the fixed
coordinate frame, the peatch angle e corresponds to the rotation about
the newly formed axis y (after the rotation for angle ~), whereas the
roll angle corresponds to the rotation about the new axis x. Thus, the
external coordinates describe the gripper position with respect to the
fixed coordinate frame.

The vector of external coordinates s have, in a general case, m coor-

dinates, where m is the number of coordinates needed to describe the
gripper position in a particular class of manipulation tasks. Most of-
ten, it is accepted that m=6, i.e. the position and orientation of the
gripper with respect to a fixed frame are fully described by the vec-
tor of external coordinates:

s = ( xc' Yc' z c' "',

~
8., ~)
~ T

If a certain class of tasks can be described by a smaller number of

external coordinates, such a vector of external coordinates is adopted
which does not include these coordinates that are not of interest. For
example, if the gripper positioning is only to be considered, the vec-
tor of external coordinates is of the form x = (xc' Yc' Zc)T.
 31

It should be noticed that robot external coordinates can be defined in

different ways. For example, the gripper position can be described by
cylindrical or spherical coordinates, and the gripper orientation can
be described by angles adopted in some other way.

As is known [2J, robots are powered by means of actuators of various

types. Most often, one manipulator joint is powered by one actuator.
The motion of the actuator is transformed into the motion (either, ro-
tation or translation) of the corresponding joint. As will be shown in
the next chapter, each actuator is supplied with its own servo system
which drives the actuator and brings it to the desired position. The-
refore, to each actuator it should be assigned the desired position to
be attained. This means that the executive level needs the robot posi-
tion be defined in terms of internal coordinates. For each joints, it
is necessary to define the angle (or displacement) qOi to be attained,
and this angle is the input to the corresponding servo system; the
servosystem activates the actuator, which drives the joint until the
desired joint position qOi, is attained. In this way, the robot is
brought to a desired position in the workspace.

If the manipulator position is assigned through the internal coordina-

tes, then the robot positioning is reduced to assigning these coordi-
nates at the executive control level, whose servo systems should rea-
lize the given joint positions. Therefore, the tactical control level
is now trivial: the joints coordinates, assigned by higher levels (or
by the operator), are transferred directly to the executive level. Ho-
wever, the question can be raised on how the joint coordinates qOi
are determined. In the task considered above (Fig. 2.1), the desired
robot position is defined by the position of the workpiece, i.e. by
the gripper position that should be attained, or in other words, by
the Cartesian coordinates. One of possible ways of doing it is by te-
aching the robot: the operator, by means of either a joystick, a con-
trol panel, or by guiding the robot gripper manually (the robot is
"loosened", so that the operator can easily guide its gripper) *), or
in some other way, brings the robot gripper to the desired position
in the workspace; the joint coordinates qOi corresponding to the de-
sired gripper position are stored in the control system, and, when the

*) The "teaching" method has been considered in [2J.

32

robot works in an automatic mode, these coordinates are assigned to

the executive level for realization.

However, such a mode of teaching is not always acceptable. In some ca-

ses, it is not possible to realize a manual guiding of the robot beca-
use of its weight (size) and type of task (if the robot works in a
workspace inaccessible to the operator). On the other hand, the robot
teaching through the internal coordinates, i.e. by bringing the robot
gripper to a desired position, by assigning the joint coordinates
(either, via a terminal, a joystick, or a teching unit), can be a te-
dious and time consuming job. Because of that, modern robots should
have the possibility of assigning position in terms of external coor-
dinates. This way of position assigning is incomparably more conveni-
ent from the point of view of the operator and the higher control le-
vels (at which the task of planning trajectories is usually solved in
terms of Cartesian coordinates).

However, because the executive level (servo systems) requires that de-
sired position is assigned in terms of internal coordinates, it is ne-
cessary to ensure conversion of the external coordinates into the cor-
responding internal coordinates. As we explained in Chapter 1, this
problem is solved at the tactical control level, whose task is to con-
vert the given values of Cartesian coordinates SO into the correspon-
·
d lng va I f0 "]Oln t coor d'lna t es q 0 = (01
ues q , q 02 , ... ,q on) T .

It is obvious that there is a correspondence between the internal (q)

and external (s) robot coordinates. To each value of the vector of
internal coordinates q corresponds a value of the vector of external
coordinates s, that is:

s = f(q) (2.2.1)

where f represents the functional transformation of the vector of in-

ternal coordinates into the vector of external coordinates.

This functional transforming is called "direct kinematic problem" and

it involves the determination of external coordinates vector s for the
given internal coordinates (vector q). As the relevant vector of ex-
ternal coordinates is generally of dimension m = 6, and the vector of
internal coordinates is of dimension n, expression (2.2.1) represents
the transformation of the vector q of dimension n into the vector s of
dimension m.
 33

Direct kinematic problem can be solved in different ways. There are

several procedures enabling the correspondence between internal and
external coordinates to be determined for different robot structures.
The primary aim in all of these procedures is to develop an algo-
rithm for systematic determination of functional relationship between
the internal and external coordinates for an arbitrary robotic struc-
ture. As the robot control is presently realized by the aid of compu-
ters, a number of algorithms has been developed, enabling an easy cal-
culation of external coordinates as a function of internal coordinates
by using computers (microcomputers). The procedures differ in respect
to a number of mathematical operations that are needed to calculate
the external coordinates for the given values of internal coordinates,
in respect of their generality (the applicability to different robotic
structures), etc. These procedures have been reviewed in [3, 4J. It is
clear that the transformation f represents a nonlinear trigonometric
function, and its determination for complex robotic structures, having
a large number of joints (n ~ 6), requires a large number of mathema-
tical operations. The procedures for solving direct kinematic problem
are generally reduced to the determination of relationship between the
coordinate frames attached to the particular links. Thus, the relation-
ship between the coordinate frame attached to the terminal robot link
(gripper) and the fixed coordinate frame, represents the solution to
direct kinematic problem. It should be emphasized that the functional
transformation (2.2.1) can always be determined in analytic form, i.e.
it is always possible to determine external robot coordinates as a
function of internal coordinates in the form of analytic expressions.

However, as we mentioned above, the task is most often given in terms

of external coordinates, i.e. the desired robot position is defined by
the values of external coordinates. Because of that, one of the major
problems in robotics is to determine the values of internal coordina-
tes qO, corresponding to the given external coordinates so. Determina-
tion of internal coordinates qO (which are assigned to the executive
level - the servo systems at joints - where they are realized) for the
given external coordinates SO is carried out by inverse transformation:

(2.2.2)

Determination of the inverse function f- 1 is called "inverse kinematic

problem". To solve this problem is incomparably more difficult than to
34

solve direct kinematic problem. Determination of f- 1 depends also on

the dimensions of vectors sand q. In relation to this problem, three
cases are possible:

a) m=n, when the number of external coordinates is equal to the number

of internal coordinates, it is possible to determine a unique q
corresponding to the given s (under the condition that the manipu-
lator is not in the special, so-called "singular" position for which
several q values correspond to one s)*);

b) m>n, then, it is not possible (except in some special cases) to de-

termine q satisfying s;

c) m<n, there are more solutions q satisfying the given s; this is the
case of redundant manipulators (for details see ref. [3]).

In a general case, for non-redundant manipulators too, the solution of

inverse problem is not unique, but there is a set of solutions for in-
ternal coordinates corresponding to one and the same position and ori-
entation of the gripper.

As the function f, connecting the external and internal coordinates, is

a nonlinear trigonometric function, solving the inverse problem is equi-
valent to search solution to a set of nonlinear equations. Two general
approaches are known. In the one of them, the vector function f- 1 (so)
is obtained in an analytic form for each particular manipulator, while
in the other approach, the solution is sought by one of the known me-
thods of numerical analysis. Both approaches have certain advantages
and disadvantages.

2.2.1. Analytic solution to inverse kinematic problem

As the vector function f in (2.2.1), is a complex nonlinear (trigono-

metric) function of n variables, the analytic determination of the in-
verse function f- 1 is a complex problem which cannot be solved gene-
rally, i. e. for an arbitrary robotic structure. For some robotic struc-

*)
The problem of singular pOints will be treated briefly in Section
2.2.2; a detailed consideration of the problem can be found in [3].
 35

tures, however, it is possible to obtain an analytic solution of the

inverse kinematic problem, while for some others, this is not possible.

Thus, it has been shown, that for robotic structures having three d.o.f.,
two intersecting and two parallel joint axes (Fig. 2.3) is possible to
obtain an analytic solution to the inverse kinematic problem, i.e. to
determine explicitly the joint angles qi as a function of external co-
ordinates. However, this is not always possible, even for the robots
with three d.o.f. In [5], it was shown that solving the inverse prob-
lem for robots having three d.o.f. can be, in a general case, reduced
to finding the zeros of a polynom of the fourth power.

Fig. 2.3. A robot having three jOints with two intersecting and
two parallel axes (the robot for which is possible to
solve the inverse problem)

To obtain an inverse solution in analytic form, for a manipulator ha-

ving six d.o.f., it is necessary to split the problem into two inde-
pendent subproblems: the determination of the solution for the first
three joints q1, q2, q3, and determination of the remaining three co-
ordinates q4, q5, and q6. The first three jOints form the so-called
minimal aonfiguration, and the remaining three joints are usually as-
sociated with the robot gripper*). An inverse problem can be divided
in two subproblems (for the minimal configuration and the gripper) ,
provided the terminal three d.o.f. make a spheric joint, i.e. all

*) It should be noticed that this holds for robots with n=6 d.o.f.,
so-called non-redundant robots.
36

three axes intersect at one point (Fig. 2.4), which in other words,
means the gripper consists of one link only. In that case a simple
calculation enables the position of the tip of the minimal configura-
tion to Pe determined (on the basis of the given position of the grip-
per tip and the given orientation of the terminal link - gripper). Ha-
ving the position of minimal configuration determined, it is possible
to find an analytic solution to the inverse kinematic problem for the
minimal configuration and thus, determine the internal coordinates q1,
q2, q3. Then, for the known values q1, q2, q3, the orientation of the
terminal link of the minimal configuration is determined. This enables
the determination of internal coordinates q4, q5 and q6 of the spheri-
cal gripper joint that satisfy the required gripper orientation.

Fig. 2.4. The robot with a spherical joint (wrist)

Certain types of industrial robots possess such mechanical structure

for which there is an analytic inverse solution. However, with some
robotic structures, as well as with redundant manipulators, it is not
possible to determine an analytic solution to the inverse kinematic
problem, so that a numerical approach has to be used.

Example 2.2.1. We shall show an example of robotic structure for which

the inverse function (2.2.2) can be obtained in an analytic form. For
a cilindrical type of robot (Fig. 2.5) with n=4 d.o.f. the relation
 37

between the external and internal coordinates (2.1.1) (the vector of

external coordinates is chosen as s (x c , y c , ZC , 'f'~)T, where x C' y c
=

and Zc are the coordinates of the gripper gravity centre, and 1> is the
Euler angle shown in Fig. 2.5), is given as

x
C
(2~+2~+q3)sin q 1 , 003
yc = (2 3 +2 4 +q )cos
q1
(2.2.3)
2 0 4
Z q + 22 , 1> q
c
1
q
Q

Fig. 2.5. A robot having cylindrical structure (with 4 d.o.f.)

where 2~ are the lengths of robot links (Fig. 2.5). The inverse func-
~

tion (2.2.2) can be now defined as:

q1 2 Z _2 0
arctg(xc/Yc) , q
c 2
(2.2.4)
q
3 fx2:"7 0
x c +Yc- 2 4- 2 3'
0
q
4
1>

Therefore, for such a manipulation robot, it is possible to determine

the inverse kinematic model in an analytic form, i.e. to determine in
an analytic way the coordinates q corresponding to the desired exter-
nal coordinates s. However, as was explained above, this is not pos-
sible to achieve for all robotic structures.
38

E x ere 1 ses
2.1. For the "spherical" manipulator having 3 d.o.f., presented in
Fig. 2.6., determine the relation between the internal and ex-
ternal coordinates and the inverse kinematic model.

2.2. For the "anthropomorphic" manipulator having 3 d.o.f., presented

in Fig. 2.7, determine the relation between the internal and ex-
ternal coordinates. Is it possible to determine the inverse kine-
matic model in an analytic form?

Fig. 2.6. A robot of spherical structure having 3 d.o.f.

1
~q

Fig. 2.7. A robot of anthropomorphic structure having 3 d.o.f.

 39

2.2.2. Numerical solutions to inverse

kinematic problem

In essence, an inverse kinematic problem represents the solving of a

set of nonlinear trigonometric equations with respect to internal co-
ordinates q. A set of nonlinear equations can be solved by different
numerical procedures. Because of that, for those robots for which is
not possible to determine an inverse kinematic solution in analytic
form, this problem is solved by one of standard numerical methods.

We shall consider here a numerical method for solving inverse kinema-

tic problem which is often used in practice. Let find the derivations
with respect to time for the functional relationship between the ex-
ternal and internal robot coordinates (2.2.1). Thus, we obtain:

(2.2.5)

where J(q) = af/aq denotes the Jacobian matrix of partial derivatives

of the function f(q), having the dimension mxn.

Equation (2.2.5) represents the relationship between the vector of

Cartesian velocities 5 and the vector of joint velocities q. If the
velocities of Cartesian coordinates SO are given, the joint coordina-
tes velocities should be determined from (2.2.5). Here, three cases
may arise:

a) m=n, in that case the Jacobian matrix is quadratic, and its inverse
matrix can be determined:

-1 •
q J (q) s (2.2.6)

However, an inverse matrix exists if the matrix determinant J(q) is

not equal to ~ero; the manipulator positions for which det(J(q» 0
represent the so-called singular points for which inverse Jacobian
cannot be determined, i.e. the internal coordinates velocities cannot
be determined from (2.2.6) (Various procedures for solving the problem
of singular pOints have been described in the literature).

b) m>n, it is not possible to determine a q corresponding to the given

5 (except in some special cases);
40

c) m<n, this is the case of redundant manipulators, for which solution

is not unique, and some additional criteria should be introduced in
order to determine the unique q corresponding to the given s; one
of possibilities is to adopt the so-called minimal inverse solution

q (2.2.7)

which gives a q which is, according to the criterion of minimum

quadratic error, closest to the "exact" solution.

In this way, the vector of internal coordinates velocities qO can be

determined for the SO values, assigned by the operator or by higher
control levels. The tactical level should determine the Jacobian J(q)
for an instantaneous value of internal coordinates q, carry out the
inversion of the matrix J(q), and, on the basis of (2.2.6), calculate
the qO for a given so. The calculated qO should be realized by the exe-
cutive control level. This can be done in two ways: either to assign
the desired internal coordinates velocities qO directly to the execu-
tive level (which means that the so-called speed controllers used at
the executive level can realize the given angular (translational) ve-
locities of particular joints), or, to calculate the internal coordi-
nates qO and then assign them to the executive level (which now con-
sists of position servo systems, as in the case of positioning control).
In the latter case, it is necessary to determine the internal angles
qO corresponding to the calculated velocity qO, i.e. to find an inte-
gral of velocity:

t+lIt
qO(t+lIt) = qO(t) + f qO(T)dT (2.2.8)
t

The velocity integration may be carried out in different ways, depen-

dending on the mode of control realization. After the integration of
(2.2.8), the internal coordinates obtained should be assigned to the
executive level.

However, it has been supposed that the velocities of external coordi-

nates SO have been given. As the desired robot positions, assigned to
the tactical level, are often given in terms of external coordinates,
it is necessary to solve the inverse kinematic problem for the given
position so. An inverse Jacobian can be used to solve numerically the
inverse kinematic problem with respect to position, in the following
way. Let be given external coordinates SO and let q(k) be an approxi-
 41

mate (assumed) solution to the inverse kinematic problem, that is

(2.2.9)

The difference between the exact values of external coordinates SO and

the values corresponding to the assumed solution f(q(k)) may be appro-
ximately expressed in the following way:

6S (k) = sO_f (q (k)) "" J (q (k)) (q (k+1) _q (k)) (2.2.10)

where q(k+1) is a more accurate solution to the inverse problem, cor-

responding to so. On the basis of (2.2.10), it is obtained that

q (k+1) "" q (k) +J-1 (q (k)) 6S (k) (2.2.11)

Expression (2.2.11) can be used as an iterative procedure to determine

solution to the inverse problem. The procedure is simple: for a given
so, a solution to the inverse problem q(o) is assumed and correspon-
ding values of external coordinates f(q(o)) and error 6S 0 determined;
then, the Jacobian matrix at point q(o) is determined and also its in-
verse matrix (provided that the matrix is not a singular one); further,
on the basis of (2.2.11), next approximate solution q(1), closer to
the exact solution, is determined. The procedure is then repeated for
the new q(1) and the repeatition continued until the error for exter-
nal coordinates satisfies the condition 16s(k) 1<2, where 2 is a small
positive constant defining the desired accuracy of the solution. This
is, in fact, the classical Newton method for solving a set of nonline-
ar equations. The method is characterized by a quadratic convergence,
which means that it has a high convergence rate toward the exact solu-
tion. Obviously, the Newton method gives only one solution to the in-
verse kinematic problem, and this is the one closest to the initial
guess, q (0) •

It is clear that equation (2.2.11) corresponds to equation (2.2.5) (if

the velocities are replaced with finite increments, i.e. by the finite
difference between two iterative solutions). Thus, the inverse Jacobi-
an can be used for solving the inverse kinematic problem and for de-
termining the internal coordinates for the given external coordinates
(positions) .
42

In practice, however, the desired gripper velocity (i.e. the external

coordinates velocities) is often assigned in a direct way. If this is
done by the "teaching" method, the operator should guide the manipula-
tor to the desired positions, and it is obvious that it is easier for
the operator to guide the robot by assigning the external than by as-
signing the internal coordinates (for this reason, the tactical con-
trol level is introduced). However, it appears inconvenient to the
operator, guiding the robot by the aid of a joystick, to assign the
external coordinates in a direct way. It is more convenient to the
operator using a joystick to assign the velocity of the robot tip
(gripper), i.e. the velocities of external robot coordinates. On the
other hand, it appears (see the next section) that even when the desi-
red external coordinates are determined (either by "teaching", or by
direct assignement of the desired values by the operator or the higher
control levels) a direct assignement of terminal values of the desired
coordinates is not suitable; instead, it is necessary to ensure the
robot "guiding" to the desired position. The robot guiding by assig-
ning coordinates velocities is more suitable than by assigning the co-
ordinates themselves (in time). For this reason, the desired velocity
of the robot gripper is assigned in a direct way, and from (2.2.6) the
velocities of internal coordinates are determined.

All that has been said above, reffers to the manipulation tasks requi-
ring only the position control, i.e. bringing the robot to various po-
sitions in the workspace. Even nowdays, such tasks are most common in
industry. However, a number of tasks appear in practice for which accu-
rate robot positioning is not sufficient, but it is necessary to ensu-
re that the robot moving from one to another position in the workspace,
follows the more or less precisely defined paths. For example, in many
manipulation tasks in the industrial practice, the robot moves in a
workspace which is not empty, but contains different obstacles; in
such a workspace the robot cannot move freely (i.e. along an arbitrary
trajectory): hence, its trajectory should be defined in such a way the
robot avoids collisions with the obstacles. In addition, in some tasks,
it is necessary to realize the robot's tip moving along a particular
path: such a task appears in paint spraying, where the robot has to
spray paint uniformly over a particular surface. In some cases, a re-
quirement may arise for a precise maintaining of the robot tip veloci-
ty along a given trajectory. A typical task of this kind is arc wel-
ding, where the robot tip should move along a defined seam at a stric-
tly defined speed. In a great number of tasks, it is not required an
explicit realization of a given velocity. Instead, a fixed motion time
 43

is required, so that the robot's velocity is defined in an indirect

way.

In practice, the robot control is frequently accomplished through ve-

locity: the gripper velocities are assigned in a direct way. When the
jOints velocities are determined by the aid of an inverse Jacobian,
they are (as we mentioned above) sent directly to the executive level
for realization, or integrated numerically according to (2.2.5) to de-
termine the internal coordinates to be assigned to the executive level.

It can be seen that in the case of velocity control, the problem sol-
ving is generally simpler than in the position control: the Jacobian
can always be determined and its inversion carried out if n=m (except
at singular points). However, from the point of view of realization,
a number of problems may arise. In a general case, a Jacobian is a
complex function of all internal coordinates; the inversion of a Jaco-
bian is not a simple task because it requires a relatively large num-
ber of mathematical operations (most often the matrix is of dimension
6 x 6, so that its inversion is a numerically complex problem); for this
reason a powerful microcomputer (from the point of view of a speed of
performing numerical operations) is needed in order to achieve a suf-
ficiently fast calculation of velocities qO; besides, the problem of
singular pOints and the problem of redundancy should be additionally
solved. Similar conclusions hold also for numerical determination of
internal coordinates by inversion of the appropriate Jacobian.

Here, we are not going to consider the problem of assigning external

coordinates velocities so, needed to guide the robot to a given posi-
tion; the problem of calculating the trajectories to be followed by
the robot in order to perform a given task, will be considered in the
next section. The problems concerning the tactical control level (cal-
culation and inversion of Jacobians, the problem of redundancy, etc)
have been cons idered in [3].

Example 2.2.2. For the cylindrical manipulator shown in Fig. 2.5. the
Jacobian is obtained by differentiating the right-hand sides of equa-
tions (2.2.3), that is
44

003 1 1
(Jl 3 +Jl 4 +q )cosq 0 sinq 0

003 . 1 1
-(Jl 3 +Jl 4 +q )slnq 0 cosq 0
J (2.2.12)
0 0 0

0 0 0

EX ere 1 s e s

2.3. For the robot in Fig. 2.5. determine (in a analytic form) the in-
verse of (2.2.12), and check if this robot has singular points.

2.4. Draw a flow-chart of the algorithm for solving equation (2.2.2)

by the Newton procedure.

2.5. For the manipulator in Fig. 2.6, determine the Jacobian and in-
verse Jacobian. Are there singular pOints?

2.6. Do the same, as in the previous exercise for the robot in Fig.
2.7.

2.7.* Write a programme (in one of high-level programming languages)

for soiving equation (2.2.2) for the manipulator in Fig. 2.7. by
the Newton procedure.

2.8. Determine the number of numerical operations (multiplying and

adding) to be carried out in order to determine an inverse matrix
of the nxn matrix. Assume n=6.

2.3 Trajectory Synthesis for Manipulation Robots

In the preceeding two sections we showed how the problem of control is

solved at the tacticai level, i.e. how the jOint coordinates or their
veloci ties are determined on the basis of the given Cartesian coordina-
tes or their velocities. Besides, we saw that in some tasks of practi-
cal importance, there is a need that the robot tip (gripper) follows a
predetermined path at a given velocity (and acceleration). Here, we
shall show, in short, how the trajectories are synthesized in order to
 45

achieve desired motion of the gripper. This problem has been dealt
with in detail in r3j. To facilitate understanding of the further con-
sideration of control at the executive level that will be presented in
the chapters to come, we shall discuss only some elementary notions of
trajectory synthesis.

A robot gripper trajectory may be assigned in different ways: one of

them is the robot teaching in which the operator guides the robot
along a desired trajectory and the control system stores the set of
pOints through which the robot has passed. However, this method of as-
signing trajectories is not convenient for several reasons. It is of-
ten required that the operator assign a desired trajectory numerical-
ly, through the terminal and in a suitable programming language (or,
the trajectory is assigned by the higher, strategic control level).
It is necessary to ensure that the assignement of a desired gripper
trajectory is realized in the simplest possible way, so that the ope-
rator (or the higher control level) supplies a minimal number of tra-
jectory parameters; in this way the operator's job is made easier,and,
at the same time, the amount of data to be stored is minimized. There-
fore, it should be determined the minimal and most suitable set of da-
ta enabling the assignement of a desired trajectory. For example, if
the motion is performed between the two given positions along a path
which represents a geometric figure, it is necessary to define the
initial and final position, the parameters describing the figure (for
example, if a circle is assumed between the two points, it is necessa-
ry to determine its centre, or the radius and the plane the circle
lies in), as well as the velocity profile, i.e. the change of the grip-
per velocity along the given path. On the basis of these parameters,
the control system should generate the gripper trajectory that should
be realized at the tactical and executive level.

Consider the simplest problem of synthesizing a trajectory between the

two given robot positions, defined by the vectors of external coordi-
nates sA and sB' where the robot tip, when moving from one position to
the other, has to follow a straight line. As a straight line is defi-
ned by two pOints, no additional parameters are required to define the
gripper path. However, it is necessary to define the change of gripper
velocity along this straight line. If we suppose that the gripper ve-
locity in the initial and final position is equal to zero, a triangu-
lar (Fig. 2.8), a trapezoidal (Fig. 2.9), a parabolic, or some other
kind of velocity profile, may be adopted. Let adopt a triangular velo-
46

city profile and let the time needed for robot passing from point A to
point B be T. In that case, the equations describing the change of ex-
ternal coordinates (the Cartesian coordinates of the gripper tip) are:

(2.3.1 )

for T2:.t>T/2

and the analogous for the other two coordinates, Yc and zc.

Besides, it can be required that the (Eulerian) angles of the gripper

change according to a triangular velocity profile, from the initial
values eA , ~A' ~A to the final values eB , ~B' ~B· In that case, the
change of external angles in time has the form:

for

(2.3.2)
for

and the analogous for the other two angles, ~ and ~.

t o T t

Fig. 2.8. Triangular velocity Fig. 2.9. Trapezoidal velocity

profile for the robot profile for the robot
gripper gripper

In this way are obtained the trajectories of external coordinates (the

gripper coordinates and angles) which should be then realized. It is
easy to show that the external velocities are in this case defined as
 47

B A
4(si- s i)
t for O,.:t":T/2

{
2
T
·0
si (2.3.3)
4(s~-s~)
]. ].
(T-t) for T/2<t":T
2
T
000 0 Too 0 0 0 0 T
for i = 1,2, ••• ,6, s = (s1' s2,···,s6) = (xc' yc' zc' e ,1jJ , cfJ ) ,
where s~ denotes the coordinate corresponding to position A and s~cor­
responds to position B. Similarly, the accelerations are:

{ 4(,,-,,'
B A

2
for O,.:t":T/2
T
.. 0
s.]. (2.3.4)
B A
-4 (si-si)
for T/2<t":T
2
T

Therefore, the control system, in this case, generates easily the tra-
jectories, as well as velocities and accelerations, in terms of exter-
nal coordinates. The trajectories thus generated are converted at the
tactical level into the trajectories of robot joints. This conversion
can be accomplished in one of the following ways:

a) By a direct calculation of internal coordinates from the external

coordinates on the basis of (2.2.2) (provided an inverse transfor-
mation, either analytic or numeric, is possible); in that case the
joint trajectories qOi(t) are obtained, whose differentiation yields
the velocities qOi(t) and accelerations qOi(t) along the trajecto-
ries of robot jOints;

b) By calculating joint velocities on the basis of relation (2.2.6),

·oi ·0
so that the values q (t) are obtained (on the basis of the silt)
from (2.3.3», whose integration yields the joint trajectories
qOi(t) and differentiation yields the accelerations qOi(t) along
the trajectories;

c) By calculating the joints accelerations qOi(t) along the trajectory

on the basis of external accelerations sO(t), given by (2.3.4); the
integration of accelerations qOi (t) yields the velocity qOi (t) along
the jOint trajectory and the integration of the velocity provides
the joint trajectory qOi(t). (It is assumed that there are no sin-
gular points along the chosen trajectory). When the accelerations
48

of external coordinates SO are given, the joint accelerations are

obtained on the basis of the relations between the accelerations of
the external and internal coordinates. This relation is obtained by
differentiating relation (2.2.5) with respect to time

oJ ·2
Jq + oq q (2.3.5)

On the bubis of this relation, we can determine the joint accelera-

tion corresponding to the acceleration of external coordinates so:

(2.3.6)

In this way, the joint accelerations along a given trajectory are

obtained, and then, the internal coordinates qO(t) are generated.

Generation of trajectories in terms of external and internal coordina-

tes may be realized either off-line (at the stage of preparation, ro-
bot teaching, so that the trajectories are then stored), or on-line,
during the robot motion along a desired trajectory, which depends on
the type of task and the capabilities of the control system.

It can be seen that in this most elementary case, the trajectory gene-
ration is simple. However, if some more complex paths are involved,
the problem becomes much more complex. Often, the operator, or the
higher control levels, generate a set of points in space, through which
the robot gripper should pass (without stopping), so that the gripper,
moving from one given point to another, follows a straight line, as
above, or a regular geometric figure*). In this case the problem of
trajectories generation is much more complex, anditis especially com-
plex if the motion time is not given, but it should be minimized [6].
The problem of minimization of the time of motion along a given path
and the problem of optimal distribution of the robot tip veloci ty (from
the point of view of time or energy), may be dealt with on the basis
of a complete dynamic model of the robot; the problem is extremly com-
plex, and the "optimal" solutions are difficult to realize in practice
[7].

*) This is a frequent case with the robots moving in the presence of

obstacles, which are recognized by higher control levels, and which
assign then a set of points through which the robot should pass in
order to avoid obstacles.
 49

Here, we are not goint to consider the problems concerning the synthe-
sis of robot trajectories, because they have been dealt with in detail
in [3]. For a better understanding of the further text it is essential
to bear in mind that the tactical level generates either the jOint po-
sitions qOi to be realized, or the joint velocities to be realized, or
the joint trajectories qOi(t) with corresponding velocities qOi(t) and
accelerations qOi(t), and the executive level should ensure the trac-
king of these trajectories.

Example 2.3. Consider the transfer of the manipulator shown in Fig.

2.5. from position A, defined by sA = (-0.064, 0.443, 0.624, O)T to
position B, defined by sB = (0.2, 0.65, 0.68, 0.3)T. Assume the grip-
per moves along a straight line with the triangular velocity profile
presented in Fig. 2.8, and let the required time of the movement be
1.5 [s]. The trajectories of external coordinates, based on relations
(2.3.1) and (2.3.2) are presented in Fig. 2.10. Since the inverse
transformation of the external into internal coordinates may be deter-
mined in the analytic form (2.2.4), it is easy to obtain the corres-
ponding joint trajectories qO(t). These jOint trajectories are presen-
ted in Fig. 2.11.

[m] Zc e [rad]
o. 6 b---::"---::::::::::=
0,3
0.4
0.2 0,2

0.5 1.0 1. 5 t [s 10.1

-0.2

0.5 1.0 1.5 t[s]

Fig. 2.10. Trajectories of external coordinates

q 1 ,q4 [radl
0.3 0.3
0.2
0.2
0.1
O~~~ ____ ~ ______ 0.1
-0.1
-0.2
-0.3 -0.1

Fig. 2.11. Trajectories of joint coordinates

50

E x ere 1 5 e 5

2.9. Write the functions of the change of the manipulator gripper co-
ordinates in time for the case the manipulator, while moving from
pOint sA to point sB' follows the trapezoidal velocity profile
(Fig. 2.9), for the time T, and acceleration (i.e. deceleration)
time is 0.2 T.

2.10. Repeat the same as in Example 2.3. for the robot in Fig. 2.5, but
assuming a trapezoidal velocity profile (as in Exercise 2.9), in-
stead of the triangular one.

2.11. Write the functions of the change of the manipulator gripper co-
ordinates as a function of time for the gripper moving from the
A A A T B B B T
point sA = (xc' Yc' zc) to sB = (xc' Yc' zc) and followsing a
circle line whose centre is at the point So = (x o , yO, zO). Assu-
me a triangular velocity profile and a motion time T.

2.12.* Write a programme for generating the joint trajectories for the
manipulator shown in Fig. 2.7. such that the robot tip moving
from one point to another tracks a straight line and a triangular
velocity profile for defined time T. Use relation (2.2.6) and the
programme from Exercise 2.7.

2.13.* Repeat the task in Exercise 2.12 using relation (2.3.6) instead
of (2.2.6), i.e. calculate accelerations q directly on the basis
of s. Compare the results. Which of the two variants of trajecto-
ry generation is more suitable?

References

[1] Vukobratovic M., Applied Dynamics of Manioulation Robots: Model-

linq, AnalYSis and Examples, Sprinqer-Verlaq, 1989.

[2] Vukobratovic M., (Ed.), Introduction to Robotics, Springer-Verlag,

1988.

[3] Vukobratovic M., Kircanski M., Kinematics and Trajectory Synthesis

of 14anipulation Robots, Monograph Series, Vol. 3, Springer-Verlag,
1985.

[4] Craig J.J., Introduction to Robotics, Mechanics and Control, Addi-

son-Wesley Publ. Company, 1986.
 51

[5] Pieper D.L., Roth B., "The Kinematics of Manipulators under Com-
puter Control", Proc. II Intern. Congress on Theory of Machines
and Mechanisms, Vol. 2, Zakopane, 1969.

[6] Kahn M.E., Roth B., "The Near Minimum Time Control of Open Loop
Articulated Kinematic Chains", Trans. of the ASME, Journal of
Dyn. Systems, Measurement and Control, Sept., 164-172, 1971.

[7] Vukobratovic M., Kircanski M.,"A Method for Optimal Synthesis of

Manipulation Robot Trajectories", Trans. of ASME, Journal of Dyn.
Systems, Measurement and Control, Vol. 104, No 2, 1982.
Chapter 3
Synthesis of Servo Systems for Robot Control

3.1 Introduction

In the previous chapter we have considered problems concerning the

synthesis of tactical control level. In the text to follow we shall
consider the synthesis of the executive control level. As we have al-
ready explained the executive control level has to ensure implementa-
tion of trajectories (or, only positions) of joint coordinates of a
robot. These trajectories are computed at the tactical control level.
The implementation of the trajectories (positions) directly involves
dynamic behaviour of the robot. Due to this, first we shall briefly
present dynamic model of the robot system. The dynamic model of the
robot "consists" of dynamic model of the mechanism and models of actu-
ators which drive the joints of the mechanism. Next, we shall consider
synthesis of servo system around each jOint (actuator) of the robot.
In practice, the executive control level is often implemented in the
form of independent controllers for each joint of robot. As we shall
explain, robot is a complex system with stronq interactions between
the motions of its joints. However, the independent control of jOints
is the simplest control law, and so it is the most appropriate appro-
ach from the standpoint of control implementation. In this chapter we
shall present various methods for synthesis of a local controller around
each jOint, and in the next chapters we shall discuss certain disad-
vantages of this approach to control of robots. We shall also consider
the methods for improvement of this control in order to meet the requ-
irements which are imposed before the robots in industry.

3.2 Dynamic Model of Robot

The robotic systems "consists" of mechanism (mechanical part of the

system), actuators which drive the joints of the mechanism and control
system (which includes various sensors and other equipment). The mathe-
matical model of dynamics of the mechanical part of the system and the
models of actuators have been considered in detail in [1]. Here we
shall briefly present the mathematical model of robotic manipulators
 53

only in a degree which is necessary to understand the synthesis ofcon-

trol at the executive control level.

3.2.1. Mathematical model of mechanical part of system

The mechanical part of the robotic system is a complex mechanism which

consists of a number of bodies-links. The links are connected to each
other by jOints. This type of mechanism is called open kinematic
chain*). We shall assume that each link of the robot mechanism is a
rigid body, i.e. we shall neglect elasticity of the robot links. The
validity of this assumption will be considered latter on. The joints
between two neighboring links might be rotational or linear (and each
joint might have one or more degrees of freedom)**). Let us consider
the robotic system with n joints and n degrees of freedom (d.o.f.).
Dynamic model of such mechanism might be extremely complex. Let us as-
sume that each joint is driven by separate actuator which produce ac-
tive force (moment) around the corresponding jOint. The movement of
the i-th joint is described by the corresponding internal (joint) co-
ordinate qi which represents angle between the two neighboring links
(if rotational pair of links are considered) or, linear displacement
between two neighboring links (if we consider linear joint). The i-th
jOint movement is also described by joint velocity qi and acceleration
i
q (see Fig. 3.1. [1). The active (driving) moment Pi of the actuator
around the i-th joint causes the movement of this jOint. The accelera-
tion of the jOint qi is proportional to driving torque P .. The factor
. 1
of proportionality between ql and Pi represents the moment of inertia
of the complete moving part of the mechanism around the i-th joint.
This moment of inertia is a complex function of the positions (coordi-
nates) of all jOints of the mechanism qj which are in the kinematic
chain "behind" the i-th jOint, i.e. for j>i. However, besides the eigen
moment of inertia of the mechanism around the i-th joint, the movement
of the i-th joint is affected by the so-called cross-inertia terms
which represent the influence of the accelerations of the rest of the
joints in the mechanism qj, j*i upon the i-th jOint. Namely, if the

*) If the gripper of the robot is in a contact with some fixed object

(with large mass) in the robot working environment, the robot be-
comes so-called closed kinematic chain (see Chapter 7) •

**)
As it is explained in [1), the complex joints which have more than
one degree of freedom can be considered as a set of several simple
jOints (the so-called pairs of the fifth class) with one degree of
freedom each. Thus, in the text to follow we shall assume that all
the jOints of the mechanism are simple.
54

j-th joint is accelerated by qj the dynamic moment around the i-th

jOint appear which is proportional to qj. The factor of proportionali-
ty represents this cross-inertia member. The cross-inertia members are
also complex functions of the positions (coordinates) of the jOints of
the mechanism qj. Next, the moments due to jOints velocities also in-
fluence the movement of the i-th joint. These moments are called Cori-
olis and centrifugal moments (forces) and they are proportional to
product of the joints velocities. At last, gravity forces (moments)
also affect the movement around the i-th joint, i.e. they also produce
moments around the i-th joint axis. The gravity force is also complex
function of all coordinates (positions of joints) qj of the mechanism.

In accordance to the above considerations, the equation of equilibrium

of the dynamic moments which act around the i-th joint might be writ-
ten in the following form:

n
I (3.2.1)
j =1
jH

where H .. denotes the moment of inertia of the mechanism around the

II
i-th jOint which is the function of all coordinates of the mechanism q
(save for the coordinate of the i-th jOint), Hij denote cross-inertia
terms which also are the functions of all coordinates of the mechanism
i
q, C jk represent Coriolis and centrifugal effects, gi represents gra-
vity moment around the axis of the i-th joint and it is also function
of all the coordinates of the mechanism q. The equation (3.2.1) des-
cribes the movement of the i-th jOint for both rotational and linear
jOint. (In the case of linear jOint, Pi is the driving force of the
actuator, Hij are masses, gi is the gravity force, while in the case of
rotational joints we operate with driving torque of the actuator (Pi)'
the moments of inertia (H ij ) and gravity moments (gi».

If we write down the moment equations (3.2.1) for all n joints of the
mechanism we obtain the complete model of dynamics of whole robotic
mechanism. The model of dynamics of the mechanism can be written in
the matrix form as:

P H(q)q + h(q, q) (3.2.2)

where P=(P 1 , P 2 , ... ,PdT represents the nx1 vector of driving torques
(forces), H (q) = [H .. (q) 1 is nxn inertia matrix which is the function
of the vector of c~;rdinates of the mechanism q = (q1, q2, ... ,qn)T,
 55

h(q, q) = gTC(q)q + g(q) is the vector of dimension nx1, C(q) =

[C~k(q) J is three dimensional nxnxn matrix which is called matrix of
Coriolis and centrifugal effects, or "C-matrix", g (q) is the nx 1 vector
of gravity moments (forces) which is also the function of q.

As it can be seen, the dynamic model of the mechanism (3.2.2) represents

the set of n nonlinear differential equations of the second order (the
total order of this system is obviously 2n). It can also be seen that
the movement of each jOint is strongly interconnected with the movement
of all the other joints and that the driving torques of actuators af-
fects all the joints in the mechanism (i.e. the driving torque of the
i-th actuator affects the movement all other jOints in the system). We
must emphasize that the matrix H and the vector h might be very complex
nonlinear functions of all the coordinates of the mechanism, which ma-
kes the setting of differential equations (the mathematical model
(3.2.2» very difficult, in general case. The complexity of the model
depends on the structure of the particular robot mechanism. Due to this
complexi ty of robot dynamic model, there was developed a large number
of various methods for automatic setting of the mathematical model of
the mechanism at digital computer. These methods enable to generate
automatically the mathematical model of the mechanism of arbitrary
structure and of arbitrary number of d.o.f. The user has to impose just
basic data about the mechanism (the mechanism structure, geometric data
on the links lengths and positions of the centres of gravity of each
link, masses, and moments of inertia of the links, etc.), and the com-
puter automatically generates the equations (3.2.2). The generation of
dynamic model of robot mechanism might be accomplished by various al-
gorithms. These algorithms differs in the law of mechanics which is
applied to form motion equations (3.2.1) [1J. Thus, the following me-
thods are well known: the method for forming differential equations of
the robot dynamics based on Lagrange's equations, methods based on
Newton-Euler's dynamic equations, methods based on Appel's equations.
Here we shall not present various methods for automatic generation of
dynamic models of open kinematic chains since they can be found else-
where [2J. It should be pOinted out that the methods for generation of
mathematical models of robots differs in number of multiplications and
additions that they require to compute driving torque once the coordi-
nates, velocities and acceleration of the jOints are given (or, to
compute inertia matrix H and vector h). This is a crucial point from
the standpoint of application of this methods for computation of dyna-
mic models required for control of robotic system. However, this fact
56

will be discussed latter on when we shall consider dynamic control of

robot (see Chapter 5).

EXAMPLE 3.2.1. The mathematical model of dynamics of the robot in Fig.

3.2. with n=3 d.o.f. might be written in the form (3.2.2) where the
matrix H(q) and the vector h(q, q) are given by:

H(q) o

h(q, q) (3.2.3)

It can be seen that there exist inertial and centrifugal coupling bet-
ween the first and the second joint of this robot, while the third
joint is not dynamically coupled to the first two jOints.

Fig. 3.1. Rotational and linear joint

->- -7
Note: Vectors r ii and r i +, ,i from the i-th
and the (i+1)-st joint to the centre
of mass of the i-th link are defi-
ned by:
->-
r 11 *
(£1 ' 0, 0) T
I
~
qL
I
Y
-7
r 21 ."
((£'-£1)' 0, O)T
->- * 0, £Z)T
(£3'
r33 3

Fig. 3.2. Robot with three d.o.f. (two rotational and one linear joint)
 57

E x ere 1 s e s

3.1. Write the mathematical model of the dynamics of the robot in Fig.
2.5. Are there interconnections between the motions of the jOints
of this particular robot (see [1])?

3.2. Repeat the Exercise 3.1. for the robot in Fig. 2.6.

3.3. Write the dynamic model for the two-joint robot presented in Fig.
3.3. What are interconnections between the jOints?

Fig. 3.3. Robot with two rotational joints

3.4. Determine the minimal number of additions and multiplications and

special functions (sinus and cosines) which is required to compu-
te matrix H(q) and vector h(q, q) if values of vectors q and q
are given, for robot presented in Example 3.2.1. (Fig. 3.2), and
for robots in previous three exercises.

3.5.* Write the programme, in some high progran~ing language, for com-
putation of matrix H(q) and vector h(q, q) if values of vectors q
and q are given (q and q are input variables for the programme,
and Hand h are output variables) for the robot in Fig. 3.2. Re-
peat this for the robots in Figs. 2.5, 2.6. and 3.3. Try to mini-
mize the number of additions and multiplications in each program-
me.

3.6.* Estimate the minimal sampling period (i.e. the time period requi-
red) for computation of matrix H and vector h (for given q and q)
if we implement programme written in Exercise 3.5. at:

a) Microprocessor INTEL-80-80 (which requires 0.8 [ms] for one

floating-point addition and1.S [ms] for one floating-point
multiplication)
58

b) Microprocessor INTEL-80-87 (which requires 35 [vs] for one

floating-point addition and 65 [vs] for one floating-point
multiplication) .

Assume that the processor time is consumed just for additions and
multiplications.

3.2.2. Models of actuators

We assume that each joint of the robot is driven by the separate actu-
ator. Majority of the robots at the market today are driven by D.C.
permanent-magnet electro-motors. There are a lot of robots which are
powered by electrohydraulical actuators, A.C. electro-motors and even
electropneumatical actuators*). The mathematical models of these actu-
ators have been considered in detail in [1]. Here, we shall briefly
present just the model of D.C. electro-motors, since in this book we
shall consider just the control of robots driven by D.C. motors. Howe-
ver, the reader can easily apply all considerations to other types of
actuators (see exercises). We shall pay a special attention to direct-
-drive actuators, i.e. electrical actuator without reducer.

Let us assume that the i-th jOint of the robot is driven by permanent mag-
net D.C. electro-motor with reducer. The scheme of such D.C. motor is
given in Fig. 3.4. [1]. The differential equations which describe the
behaviour of such D.C. motor, are:

- the equation of mechanical equilibrium of the driving torque and the

equivalent moment of load at the output shaft of the reducer:

(3.2.4)

- the equation of electrical equilibrium in the rotor circuit:

*) Pneumatic actuators have been applied for so-called mechanical

hands, while with robots they were not used due to their disability
to track continuous trajectories. However, appearance of pneumatic
servo actuators announced their possible application in robotics.
All considerations of control synthesis in the text to follow, could
be extended to pneumatic actuators.
 59

i
u (3.2.5)

In the above equations the following notations ~re used: i~ is rotor

i 1
current [Al, u is rotor control voltage ~Vl, r R is rotor resistance
[~l'L~ is rotor winding indu:tance [Hl, 8 1 is output angle of motor
shaft after reducer [radl, C~ is torque constant (the ratio between the
torque at the motor shaft and the current in the rotor circuit) [Nm/Al,
C~ is electromotor force constant (the ratio between the voltage in the
rotor circuit caused by the rotation of the rotor in the magnetic field
and the rotational velocity) [V/rad/sl, Fi is viscous damping constant
. v
redu;ed ~o output shaft [Nm/rad/sl, J~ is rotor moment of iner~ia,
[kgm-], J~ is ro~or moment of inertia reduced to output shaft J~ =
JiNiN i [kgm 2 ],N 1 is speed reduction ratio (the ratio between the rota-
Mv m v
tional velocity at the input shaft and the velocity of the output shaft
of the reducer), Ni is torque reduction ratio (the ratio between the
m *
output and input torque at the reducer), Mi is load torque at the re-
ducer output shaft [Nm]. The equation (3.2.4) and (3.2.5) represent
mathematical model of dynamics of the D.C. motor. The order of the mo-
del is obviously n i = 3. The index i denotes that the actuator is dri-
ving the i-th joint of the robot mechanism, and i might be i = 1,~, ...
... Tn.

i
1 :N V

i~=CO:S; ~ f---1 1 :N~

~?ill
.1
8

Fig. 3.4. Scheme of the permanent-magnet D.C. electro-motor

The model of the D.C. motor can be written in the state space. Let us
adopt the state vector of the actuator model in the following form:
xi= (8 i , e
• i , 1R
. i) T were
h th ex i .1S th en x 1 vector. Th en, we can wr1te
i
.
model of the D.C. motor (3.2.4), (3.2.5) in the state space as:

(3.2.6)
60

where Ai is matrix of the system, b i is (n i x 1) input dis-

(n i xn i )
tribution vector, and f i is (n x 1) load distribution vector. The matrix
Ai and the vectors b i and fi are given by:

o o o o
Fi
o - - - v- -
NiNiJ i
o
- NiNiJ i
v m M v m M

CiN i
o E v o
i
LR

(3.2.7)

In (3.2.7) u i represents the scalar input to the system (i.e. the rotor
circuit voltage). However, we have to take into account the fact that
the input signal (voltage) is constrained by amplitude, i.e. the ampli-
tude of the input must be below some maximal value u i . This nonlineari-
m
ty of the amplitude saturation type we have to introduce in the model
(3.2.6) of D.C. motor, so it becomes:

.i
x = Aixi + biN(u i ) + fiM~ (3.2.8)
l

where N (u) is defined by:

i i i
-u for u < -u
m m
i i i
N (u i ) u for -u < ui < u (3.2.9)
m m
i i i
u for u > u
m m

This nonlinearity is presented in Fig. 3.5.

N\u i)

Fig. 3.5. Amplitude saturation constraint upon the actuator input

 61

The model of D.C. motor is obtained as a system of linear differential

equations of the third order with the nonlinear amplitude constraint
upon the system input. However, the inductance of the rotor circuit can
be often neglected, and thus, the model of the actuator becomes system
of differential equations of the second order (but its form in the sta-
te space is given by (3.2.8), too).

The model of electrohydraulic actuator can be obtained in analogous

way. We obtain model in the form (3.2.8), too, and its order is n i = 3.
However, the model of the electrohydraulic actuator can be written as
a higher order system, or, as the second order system (see [1)). Inthe
text to follow, we shall consider the model of the actuator in the form
(3.2.8.) and the elements in matrices Ai, b i , fi are distributed as in
(3.2.7). Since the distribution of the elements in these matrices for
electrohydraulic actuators is the same as in (3.2.7), all considerati-
ons to follow are valid for both D.C. motors and electrohydraulic ac-
tuators. The only difference between the models of D.C. motors and
electrohydraulic actuators is in the physical meaning of the elements
in corresponding matrices. We must underline that for the electrohydra-
ulic actuators these linear models are very approximate, since in the
hydraulic actuators nonlinear effects might be significant. Neverthe-
less, for the sake of symplicity first we shall consider linear models
of actuators and in Section 3.3.5. we shall consider the influence of
the nonlinear effects in actuators upon the servo system control.

EXAMPLE 3.2.2. For the permanent-magnet D.C. motor of the type IG2315-
-P20 the data taken from the data-sheets are given in Table 3.1.

Based on these data we can easily obtain the matrices Ai, b i , fi of the
actuator model acc. to (3.2.7) (for the first two joints of the robot
in Fig. 3.2., data are given in the first two rows of Table 3.1):

[:
-0.201 5~.03 ]
-621.74 -695.65

(3.2.10)
62

ACTUATOR 1 2 3

i
CE [ra~/s ] 0.0459 0.0459 0.0459

i
CM [~] 0.0481 0.0481 0.0481

Ji [kgm 2 ] 0.00003 0.00003 0.00003

M

Nvi [ -] 31. 31. 2616.

i
Nm [-] 31. 31. 2616.

ri [n] 1.6 1.6 1.6

R
Fi m
v [raNd / 5] 0.0058 0.0058 0.0154

Li [H] 0.0023 0.0023 0.0023

R

Table 3.1. Data on actuators for the robot in Fig. 3.2.

E X ere s e s

3.7. Show that if we assume that LR~O, the model of the D.C. permanent-
-magnet electro-motor might be obtained in the form (3.2.8) butas
the second order model n i 2, where the matrices are now given

l l
by:

7 ~
i
0
N i : ri
vMR
, fi =
- Ni~iJi
0

vmM
j
(3.2.11 )

3.8. Compute the elements of the matrices Ai, b i , fi in (3.2.11) based

on data given in Table 3.1, for all three joints of the robot in
Fig. 3.2.

3.9.* See Appendix 6. in [1] for models of electrohydraulic actuators.

Wri te the state space equations and matrices of this model
 63

for electrohydraulic actuato~ for two cases: if we adopt the

second order model (n i =2) and for the third order model (n i =3).

3.10.* See Appendix 6 in [1] for models of elect~opneumatic actuators,

A.C. electro-motors, and direct-drive actuators.

3.2.3. Total model of the robotic system

As we have already explained, the robotic system consists of mechani-

cal part of the system and actuators which drive the robot joints. The
model of mechanical part of the system is given by (3.2.2), and the
model of actuators is given by (3.2.8). Our next task is to combine
these models into unique model of the robotic system [1].

The relation between the model of actuators and the model of the mec-
hanism is by coordinates and by moments (loads). The movement of the
actuator (output shaft of the reducer,if we consider D.C. motors with
reducers) is the movement of the corresponding joint (rotational or
linear displacement of the joint). Thus, the rotation of the actuator
8 i is transformed into the motion of the jOint qi. In the simplest
cape, the movement of the actuator shaft is equal to the movement of
the corresponding joint, i.e.:

(3.2.12)

However, in general case, the relation between e i and qi might be more

complex. For example, if the linear displacement of the piston of the
hydraulic cylinder is transformed into rotation of the jOint (see Fig.
3.6), then the relation between the actuator coordinate e i and corres-
ponding coordinate of the jOint qi is given by cosines formula:

(3.2.13)

In general case, the relation between the coordinate of actuator ei

and coordinate of the corresponding jOint qi is given by:

or, (3.2.14)

We can establish relation between the load Mi* upon the actuator (out-
put shaft of D.C. motor (reducer), or, at the piston of hydraulic ac-
tuator) and driving torque Pi which acts around corresponding joint of
64

the mechanism, in the similar way. In the simplest case {which corres-
ponds to the relation between the coordinates (3.2.12», the load mo-
* is equal to the driving torque Pi (force) around
ment upon actuator Mi
the jOint axis:

M.* P. (3. 2.15)

1 1

Fig. 3.6. Relation between actuator coordinate and jOint coordinate

However, if the relation between the coordinate of the actuator and the
coordinate of the joint is not linear, then the relation between Mi* and
Pi might also be complex. For example, in the case presented in Fig.
3.6, the relation between the load upon the piston of the hydraulic
cylinder and the driving torque which is produced around the joint axis
is given by:

(3. 2 .16)
a.coss i
1

We may assume that the relation between the load moment upon the actu-
ator Mi* and the moment around joint axis Pi is given in the form:

M.* (3. 2 .17)

1

However, for the sake of simplicity we shall consider the case when the
relation between e i and qi is given by (3.2.12) and the relation bet-
ween Mi* and Pi is given by (3.2.15).

The total model of the robotic system can be obtained by combining the
 65

actuators models (3.2.8) and the model of the mechanism dynamics

(3.2.2). Since the relations between the actuator coordinates and the
coordinates of the mechanism are given by (3.2.12), we may adopt the
vector of the state of the total system in the form:

(3.2.18)

The state vector of the system is obviously of the order N which is

given by:

n
N I n. (3.2.19 )
i=l J.

Thus, the order of the total robotic system is N. Based on (3.2.12) we

can write:

.i
q (3.2.20)

where Ti is the 1 x n i vector. (For example, if xi is of the order n i =2,

Ti is given by Ti=(O, 1), and if n i =3 then Ti is given by Ti=(O, 1, O)~
Based on (3.2.18) and (3.2.20) we may write:

q Tx (3.2.21 )

where T is the matrix of dimensions nxN given by T

The model of the mechanical part of the system (3.2.2) might be expres-
sed by the state vector of the total system in the following way:

P H(x)T~ + h(x) (3.2.22)

We can combine the models of all actuators (3.2.8) of the robot in a

unique system of differential equations in the matrix form:

x = Ax + BN(u) + FP (3.2.23)

where we have used the relation (3.2.15) and where A is the NxN matrix
given by A=diag(A i ), Band F are the Nxn matrices given by B=diag(b i ),
66

F = diag(f i ), while N(u) denotes n x1 vector given by:

N (u) (3.2.24 )

By u we have denoted the nx1 vector of inputs to the actuator system,

which is defined by (the order of the system input is evidently n):

u = (3.2.25)

If we substitute x from (3.2.23) into (3.2.22), and solve the system by

P we get:

P = (In -H (x) TF) -1 [H (x) T (Ax+BN (u) ) +h (x) 1 (3.2.26 )

where In is the nxn unit matrix. Evidently the inverse matrix in

(3.2.26) always exist, i.e. the corresponding matrix is regular. If
we substitute P from (3.2.26) into (3.2.23) we obtain the total model
of the robotic system in the following form:

a(x) + B(x)N(u) (3.2.27)

where by a(x) is denoted the Nx1 vector:

a(x) [A+F(I -H(X)TF)-1 H (x)TAlx+F(I -H(X)TF)-1 h (x)

n n

and by B(x) is denoted the Nxn matrix:

B(X) B+F(I -H(X)TF)-1 H (x)TB

n

In this way we obtain the total model of the robotic system in the
state space as a system of N nonlinear differential equations. This
form of model of the robotic system is called the centralized model
while the model expressed by the models of actuators (3.2.8) and model
of the mechanism dynamics might be called decentralized form of the
model. We shall mostly use the decentralized form of the model of the
robotic system. In this form of model, the robot is described by a set
of n models of actuators. The actuators might be considered as subsys-
tems which are interconnected by the mechanical part of the system (by
rotiOt mechanism). The "scheme" of the model of the robot is presented
in Fig. 3.7.
 67

r-------- ..,
I 0.. I

: 0-
X

Fig. 3.7. "Scheme" of the model of the robot: mechanical part of the
system and n actuators (in each joint one actuator); the
vectors T.
.1
are of dimensions 1xn. and they are defined by
1
A 1
Tix .
68

EXAMPLE 3.2.3. The model for the mechanical part of the robot in Fig.
3.2. is given by (3.2.3). The actuators for this robots are D.C. motors,
the model of which are given by (3.2.8) and of the order n.
. . ~
=2 for all three
joints. We assume that the relation between e~ and q~ is given by (3.2.12),
and that the relation between M and P is given by (3.2.15). The model
of the total system can be written in the form (3.2.27), where the
state vector x is given by x = (q1, q1, q2, q2, q3, q3)T, and the input
vector u is given by u = (u 1 , u 2 , u 3 )T, the order of the total system
is N=6, the order of the system input is 3, and the vector a(x) is
given by:

·2
q
a(x) (3.2.28)

·3
q

The matrix SIx) in this particular case is given by:

o o o

o o o
SIx) (3.2.29)
o

o o o

o o
 69

where a.i ii
k , b., f. are elements of the corresponding matrices and vec-
J J J
i
tors A ~ [a. k ],i
i i i
b ~ [b.], fi [f j ], H' k are elements of the matrix
J J 2 .1 .2 J
H (and they are functions of q , q , q ) while d 1 , d 2 , DET denote:

E X ere s e s

3.11. Data on parameters of the robot in Fig. 3.2. are given in Table
3.2. The joints of the robot are driven by D.C. motors the values
of parameters of which are given in Table 3.1. Calculate the nu-
merical values of the vector a(x) and matrix B(x) which are given
by (3.2.28), (3.2.29), for the following values of the state vec-
tors: x ~ (0, 0, 0, 0, 0, O)T, x~ (1.57,0.,1.57,0.,0.2, O.)T
and x ~ (1.57, 0.5, 1.57, 0.5, 0.2, a.1)T.

LINK 1 2 3
MASS [kg] 7. - 4.
r xii [m] 0.3 O. O.

r;i [m] O. O. 0.2

z
r i i [m] O. 0.2 0.3
x
r i+1 • i [m ] -0.3 O. O.
[m] O. O. 0.2
r; +1. i
z O. O.
r i+1 • i [m] -0.3
Moment of inertia around 0.3 - 'V O.
the z axis [kgm 2]
Table 3.2. Data on robot fromFig. 3.2. (i?ii, i?i+1 i are vectors from the
i-th and i+1-st joint to the centre of mass of the i-th link)
70

3.12. For the robot in Fig. 3.3. write the total model in the form
(3.2.27) assuming that each joint is driven by D.C. motor the
model of which is given by (3.2.8) (adopt the second order mo-
dels of actuators). The link masses are m1 =2. [kg], m2 =1. [kg],
moments of inertia J 1 = 0.02 [kgm 2 ], J 2 = 0.01 [kgm 2 ], the
lengths of links are ~1=O.3 [ml, ~2=0.15 [ml and the centers of
masses are at the middle-points of the links. Data on D.C. mo-
tors are given in the first two rows of the Table 3.1. Calculate
the values of the vector a(x) and the matrix B(X) for this par-
ticular robot and for the following particular values of the sta-
te vector x = (q1, ~1, q2, ~2)T = (0., 0., 0., O.)T and x =
(0.3, 0.3, 0.2, 0.1)T.

3.13. Show that the vector ~(x) and matrix S(X) might be written in
the form:

where IN is the NxN unit matrix. Which of the two forms (this or
(3.2.27)) of matrices is numerically more convenient? (Instruc-
tion: substitute P from (3.2.22) into (3.2.23) and determine x).
3.14. Show that if we adopt that the order of the model of actuator is
n i =3 (for all actuators in robotic system) and if models of ac-
tuators are given by (3.2.7), (3.2.8), then the matrix B is not
function of system state i.e. B(X) B.

3.15. Show that if the relation between 8 i and qi is given by (3.2.14)

and relation between Mi* and Pi is given by (3.2.17) (for all jo-
ints of the robot), then the vector a(x) and matrix B(X) get the
following forms:

- -1 T
a(x) [IN-FZ(G(X)).H.G(x)] [Ax+Fz(G(x)) (G(x)x .x+h)]
A

-1
B(x) = [IN-FZ(G(X)).H.G(x)] B
A -

where z(q) = diag(zi(qi)-1), q = G(x) (g1 (x 1 ), g2(x 2 ) , ...

... ,gn(xn))T and q = G(x)x + G(x)XT.x.
 71

3.16~ Write the programme (using some high programming language) to

compute the vector a(x) and matrix B(x) for an arbitrary robot
with n d.o.f. for which the matrix H and vector h are computed
in separate programme. Robot is powered by D.C. motors. The mo-
dels of actuators are given by (3.2.8). The input to the program-
me is value of the vector of the system state x, the values of
the corresponding matrix H and vector h and the values of the
!llat):;";i.G~S Ai, b i and fi. Combining this programme with
actua.to):;"
the programme written in Exercise 3.5. compute the total model
matrices for the robots in Figs. 3.2. and 3.3. for some particu-
lar values of the state vector.

3.3 Synthesis of Local Servo System

In the previous sections we have presented the model of dynamics of the

robotic system. In the text to follow we shall consider synthesis of
control at the executive control level. As we have explained in the
previous chapter, the tactical control level generates desired positi-
ons and/or trajectories of the so-called internal coordinates (or jOint
coordinates) and sends them to the executive control level, which has to
realize them. Thus, the task of the executive control level is to en-
sure driving of the robot joints to the the desired positions, or their
driving along prescribed trajectories. The executive control level has
to ensure desired positions qO of joint coordinates, or to ensure tra-
cking of desired joint trajectories qO(t), velocities qO(t) and acce-
lerations gO(t).

In this chapter we shall consider synthesis of servo system for each

individual joint (and corresponding actuator) of the robot. We shall
consider one joint of the robot and we shall synthesize servo system
which has to realize any desired position qOi, or desired trajectory
qOi(t) of that joint. Such a control law, which is synthesized for each
joint of the robot independently, is called decentralized control of
robot*). This control, which "neglects" dynamics of the total robotic
system, cannot be applied for all the tasks that are imposed to the
robots in industry and in other applications. This fact will be consi-
dered in the next chapter. However, this control law is the simplest

*) In literature this control law is often called "naive" decentralized

control since in synthesis of servo systems for each joint indepen-
dently we do not take care of dynamics of the total systems, nor of
interconnections between the joints.
72

one from the pOint of view of implementation, and this is the reason
why it is most frequently applied in robot controllers in practice.

We shall consider the i-th jOint of the robot and its corresponding
actuator (see Fig. 3.8). Let us assume that all the other jOints are
locked in some positions, i.e. that they keep constant values of their
angles (or linear displacements) qj* = const., for j*i, so that just
the i-th jOint might be moved. Thus, the actuator in the i-th jOint
moves that jOint and the part of the mechanism which is behind the
i-th jOint in the kinematic chain. The moment of inertia of the moving
part of the mechanism reduced to the axis of the i-th joint (for the
fixed values of the other joints qj*) will be denoted by Hii . This va-
lue is, obviously, fixed since the jOints behind the i-th joint are
locked and H .. does not depend on qi. The mechanism can move just around
11
the i-th joint and the load moment around the axis of the i-th jOint
includes two components only: inertia moment due to rotational or line-
ar acceleration qi and gravity moment of the mechanism around the i-th
. . i * * * 1 * 2* i-1 * i+1 *
]Olnt g. (q , q ). Here q denotes vector q = (q , q , ... , q , q ,
n*l T
.•. ,q ) • Thus, we can write that the moment around the i-th joint is:

(3.3.1)

Gravity moment is in general case, nonlinear function of all joint co-

ordinates qj, j=1,2, ••• ,n. It is also function of qi. First we shall
neglect this gravity term, but latter on we shall take it into account.

i+1 *

qi-1 *

Fig. 3.8. Actuator in the i-th jOint of the robot

(all the other jOints are locked)

The actuator drives the i-th jOint and the load Mi* is acting upon that
actuator. This load is equal to moment Pi which is determined by
 73

(3.3.1). Thus, the model of the considered system (if just the i-th
joint is moving and all the other joints are locked) can be easily
obtained by combining model of the actuator driving the mechanism
around the i-th jOint (3.2.8) and the model of the load moment (3.3.1):

·i iii i. i - .. i i * (3.3.2)
x = A x +b N(u )+f (Hiiq +gi(q , q ))

If we apply relation between qi and xi given by (3.2.20) we obtain

x· i (3.3.3)

where Ini denotes the nixn i unit matrix.

If we neglect gravity term in (3.3.3), we obtain model of the actuator

and the load for this specific case in the following form:

x·i (3.3.4)

where Ai is n.xn. matrix given by:

l. l.

and bi is n.x1 vector given by:

l.

In this way we obtain the model describing the behaviour of the indi-
vidual actuator in the i-th joint if all the other jOints are locked.
Our task is to synthesize the control law which will control the actu-
ator and the mechanism modelled by (3.3.4). We have to synthesize con-
trol which will ensure that the i-th joint will be driven towards the
desired position qOi imposed by the higher control level, or which
will ensure tracking of desired trajectory qOi(t). First, we shall
consider the case when the control system has to ensure just positio-
ning of the joint, and afterwards the tracking problem will be addres-
sed. First we shall neglect gravity term, and latter on we shall con-
sider its effect upon the control of the actuator. We shall synthesize
the so-called static controlZer first, and afterwards we shall consi-
der dynamic controller (i.e. controller which includes feedback loop
by integral of position error so that the order of the system model
increases - see Section 3.4).
74

EXAMPLE 3.3. Consider the first joint of the robot in Fig. 3.2. Let us
assume that the second and the third joints are locked in positions
q 2* O. and q 3 * O. Then, the moment of the inertia of the mechanism
around the axis of the first joint is given by:

(3.3.5)

There is no gravity moment around the first joint. Thus, the model of
the actuator and the mechanism in this case is given by (3.3.4) where
the matrices Ai and b i are (if we adopt the model of the actuator in
the form (3.2.8) with matrices given by (3.2.7»:

o o o

o o (3.3.6)

zr
1
o
R

If we apply the D.C. electro-motor the parameters of which are given in

Table 3.1. (first row) and if the parameters of the robot mechanism are
given in Table 3.2, then the matrices (3.3.6) get the following nume-
rical values:

-0.00165 o ] ,
0.42 (3.3.7)
-621.74 -695.65

If we compare (3.2.10) and (3.3.7), then it is clear that the model of

actuator with the mechanism rotating around the axis of the i-th jOint
(if all other jOints are locked) has the equal form as the model of
the actuator itself (i.e. they are both linear time invariant system,
their orders are equal and the distribution of the elements in matri-
ces are equal), but the numerical values of the elements of the matri-
ces are different (due to the fact that the moment of inertia of the
mechanism is added to the actuator model). Namely, the only diffe-
rence between these two models is in the moment of inertia: in the mo-
del (3.3.6) the equivalent moment of inertia of the motor rotor is in-
creased by the moment of inertia of the mechanism Hii .
 75

E x ere 1 s e s

3.17. Calculate the elements of the matrices Ai, bi in (3.3.6) for the
first joint of the robot in Fig. 3.2. (Example 3.3) if the second
joint is locked in position q2* = 1.573 [rad] (data on parameters
are given in Tables 3.1. and 3.2). How much have changed the nu-
merical values of the elements of the matrices with respect to
(3.3.7) ?

3.18. Write the expressions for the elements of the matrices Ai and bi
in (3.3.4) for the first joint of the robot in Fig. 3.2, if we
adopt the second order model of the actuator n i =2 and if the
matrices of actuator are given by (3.2.11). Calculate these mat-
rices for data given in Tables 3.1. and 3.2. for the following
two cases: if the second jOint is locked (a) in the position
q2* = 0, and (b) in the position q2* = 1.573 [rad).

3.19. Determine the matrices Ai and hi in (3.3.4) for the second and
the third joints of the robot in Fig. 3.2, if we apply D.C.
electro-motors which models are given by (3.2.8) and (3.2.7).
Compute these matrices for data given in Tables 3.1. and 3.2.

3.20. Determine matrices Ai and bi in (3.3.4) for the three jOints of

the robot in Fig. 2.5, if D.C. electro-motors are applied which
models are given by (3.2.7) and (3.2.8). Data on parameters of
the applied D.C. electro-motors are given in Table 3.3. and data
on parameters of the robot mechanism are given in Table 3.4.

ACTUATOR 1 2 3
V 0.0459 0.0459
CE [ratus) 0.0459
CM [Nm)
T 0.0480 0.0480 0.0480
JM [ kgm 2] 0.00003 0.00003 0.00003

Ny [-] 31.17 2616 1570

Nm [-] 31.17 2616 1570
r R [>1] 1.6 1.6 1.6
Nm
Fy[rad/s) 0.0058 0.0154 0.000923
LR [H] 0.0023 0.0023 0.0023

Table3.3. Data on parameters of the actuators for the robot in Fig. 2.5.
76

LINK 1 2 3
Mass [kg] 10.0 7. 4.15
Link length [m] 0.213 0.026 0.036*
Moment of inertia
around the vertical 0.0294 0.055 0.318
axis [kgm 2]

Table 3.4. Data on mechanism of the robot in Fig. 2.5.

(*link length is for qi=O, centre of mass
of the third link is at distance of O.036[m])

3.3.1. Elements of local servo system

Now we shall consider synthesis of control for the actuator in the

i-th jOint under assumption that all the other joints are locked. We
have adopted the model of such system in a form (3.3.4). Thus, the
system to be controlled is described by linear time-invariant model
(system of linear differential equations of the order n i ). The ~ontrol­
ler has to ensure that for each imposed value of joint angle q01 the
actuator drives the joint to this desired position. This task can be
accomplished by classical position servo system. We shall consider
synthesis of such position servo system around the actuator in thei-th
jOint. The synthesis of servo system is well known from classical con-
trol theory. Here, we shall briefly repeat some of its basic characte-
ristics, since the servo systems are essential for understanding robot
control.

The servo system can be implemented either by analogue technology, or

by microprocessors. The elements of the servo systems are basically the
same in both cases, and only their implementation differs. First, we
shall consider implementation of servo system by analogue technology,
and afterwards we shall present its microprocessor implementation. The
block-diagram of the servo system is presented in Fig. 3.9. [3].

Let us consider the elements of the servo system. For the sake of sim-
plicity we shall first consider their transfer functions in s-domain,
and afterwards we shall consider the state space model of the servo
system. The elements of servo systems are: detector of the error sig-
nal, amplifiers, D.C. electro-motor with its mechanical load (actuator
and the moving part of the robot mechanism), sensor of velocity (e.g.
 i 9 i (S)
-Kr.,
r--
(Tim5+1)5
.

D.E.S. Position amplifier Electro-motor Paten ti ometer

Vi-Ki oi 1-- - -, Vi Ki Vi
- oq 1+ 1 2 Ki I ~ o 1
1 1 P q' -I ,; 1
~- (T~5 + 1) 5 ro'
I I
L __ --.J Vi"
Vi
1
Velocity amplifier Tacho-genera tor
i I
dJ
"II
I i
'! 1
Vi 5qi
Ki I 3
'--- V
K"v
t--

Fig. 3.9. The structural block-diagram of the position servomechanism

(electro-mechanic servo system) with D.C. electro-motor
(the model of the electro-motor is of the second order - the
electrical constant is neglected)

-..J
-..J
78

tacho-generator) and sensor of position of the joint (e.g. potentiome-

ter). We assume that the voltage corresponding to desired position of
the i-th jOint qOi is fed at the input Vi of the servo system. If we
implement the servo system by analogue technology and the tactical con-
trol level is implemented by microprocessor, then the desired position
is fed to the servo system input through D/A converter. The voltage
signal from D/A converter is proportional to the desired position, i.e.
the voltage at the servo system input is Vi = K~qOi, where K~ is the
proportionality coefficient.

Detectol' of the el'l'ol' signal. At the output of this element the vol-
tage,which is proportional to the error of the servo system output po-
sition, is produced. The error in positioning of servo system is given
as a difference between the actual position of the joint qi and the de-
sired (imposed) position qOi. This element has two inputs: the first
input is the input voltage vi to the servo system which is proportio-
nal to desired position qOi, and the second input is the voltage v~
which is proportional to actual position and which is obtained from the
position sensor placed in the i-th joint axis. Voltage from the posi-
tion sensor (e:g. poten~iome~e:) v~ is proportional to actual position
of the jOint ql, i.e. v~ = K~ql. (For the sake of simplicity we shall
assume that the coefficient of proportionality is the same as for the
input voltage Vi and the desired position qOi). The voltage at the
output of the detector of the error signal is equal to difference bet-
ween these two input voltages:

(3.3.8)

i.e. the voltage is directly proportional to the error in positioning

~qi. This element does not introduce any nonlinearity in the servo
system. Its transfer function (including the position sensor) can be
obtain from (3.3.8) as:

V~(S)
--i-- (3.3.9)
~q (s)

Amplifiel's. The role of the amplifier of the position error in the

servo system is to amplify the error signal. The amplifier in the ve-
locity feedback loop has to amplify the feedback signal by velocity,
which is obtained from the tacho-generator (as a sensor of velocity).
In implementation of the servo system by analogue technology these two
 79

amplifiers are implemented by electronic amplifiers whose time con-

stants are considerably lower than the time constants of the electro-
-mechanic components in the servosystem (i.e. of the actuator and the
moving part of the robot mechanism). Due to this, these amplifiers
might be considered as non-dynamic elements. The transfer function of
the amplifier of the position error is given by:

Vi' (s) • I
K~
p (3.3.10)
V~(s)
where Ki'
p
is the gain of the amplifier.

The amplifier in the velocity feedback loop has the following transfer
function:

Vi" (s) . I
K~ (3.3.11)
v
V;(S)

where Ki' is the amplifier gain,

v in
v; is the output voltage of the tacho-
-generator, and V is the output voltage of the amplifier.

D.C. electro-motor and mechanical part of the robot. This element of

the servo system consists of the actuator (which might be either elec-
tro-hydraulic actuator, or electro-pneumatic, or D.C. electro-motor
etc.) and the mechanical part of the robot which can rotate around the
axis of the i-th joint of the robot while the other joints are locked.
This element has been already considered in the previous section. The
model of this element in the state space is given by (3.3.4). Its
transfer function in the s-domain is given by (for the actuator model
of the third order n.=3 and matrices Ai and
~
b i given by (3.3.6)*»:

(3.3.12)

where Ti is the electric time constant given by:

R
Li
Ti R
(3.3.13)
R ir
R

while Ti is the electro-mechanic time constant given by:

m

*) Here, we have neglected the nonlinear amplitude constraint upon the

actuator input. This nonlinearity we shall consider in Sect. 3.3.5.
80

(3.3.14)

Ki and Fi are the constants given by:

D D

(3.3.15 )

We can easily obtain the transfer function (3.3.12) if we remember that

the state space model (3.3.4) has been formed on the basis of equations
of mechanical and electrical equilibrium in the D.C. electro-motor
( 3 . 2. 4) and (3. 2. 5) .

However, let us remind that we have neglected the gravity moment around
the i-th jOint. This gravity moment acts as an external load around the
output shaft of the reducer and it might be treated as an external dis-
turbance upon the servo system. The transfer function from the external
load moment g. to the servo system output qi is given by (if we remember
1
the equation of the moment equilibrium around the output shaft of the
reducer (3.2.4)):

(3.3.16 )

where

In the case when the electrical time constant of the D.C. motor.T~ is
significantly lower than the electro-mechanical time constant Tl, then
m
we can neglect T~ (which actually means that we adopt the second order
model of the actuator n i =2), the transfer functions (3.3.12) and
(3.3.16) get the following forms:

(3.3.17)

(3.3.18)
(T i S+1)s
m

The input voltage to the actuator u i is formed combining the output

 81

signals of the amplifiers, i.e.:

Vi" (3.3.19 )

Tacho-generator. Tacho-generator is the sensor of the velocity. Its

i
output voltage V3 is proportional to the velocity of the movement of
the joint. As the sensor of the jOint velocity usually serves the ge-
nerator which is placed at the output shaft of the D.C. motor (before
the reducer). This generator generates the voltage in its windings
which is proportional to the rotational velocity of the output shaftof
the motor. Since the rotational velocity of the motor is directly pro-
portional to the angular (or, linear) velocity of the corresponding
jOint of the robot, the voltage v~ is generated at the output of the
tacho-generator which is directly proportional to the angular velocity
of the joint qi. Thus, the transfer function of the tacho-generatorcan
be adopted in the form:

i
V3 (s) Kin
(3.3.20)
V
qi(s)

Sensor of position. Various sensors can be used to measure the actual po-
sition of the joint. One of the most usually applied sensor is simple
potentiometer. On its output the potentiometer gives the voltage V~
which is proportional to the joint position (angle or linear displace-
ment) qi. Thus, the transfer function of this element is in the form:

(3.3.21)

Obviously, we might assume that all elements in the servo system are
linear. The total transfer function of the servo system from the input
qOi(s) to its output (joint position) qi(s) can be obtained by simple
combining of the transfer functions of the elements according to their
arrangement in the servo system, as presented in Fig. 3.9. So, in the
case when the third order transfer function of the actuator and the
mechanical part of the robot is adopted (3.3.12), we obtain the trans-
fer functions from the input of the servo qOi(s) to its output qi(s),
and from the load disturbance gi(s) to the servo system output, in the
following forms:
82

(3.3.22)

i
where by Kp we denote the total gain of the position feedback loop (we
shall call this gain position gain), from the joint coordinate - posi-
error 6qi to the input voltage of the actuator vi'

Vi I (s)
(3.3.23)
6qi (s)

while by Ki we denote the total gain of the velocity feedback loop (we
v
shall call this gain velocity gain) from the jOint velocity qi to the
input voltage of the actuator vi"

(3.3.24 )

In the case when we adopt the second order transfer function (3.3.17)
of the actuator and the mechanical part of the robot (i.e. when ni~2) ,
we obtain also the second order transfer functions of the complete
servo:

K~K~+K~K~S+(T!S+1)S
(3.3.25)
_K i
M

In this way we obtain the transfer functions in the s-domain for the
classical servo system with feedback loops by position and by velocity.

Let us consider briefly the microprocessor implementation of the posi-

tion servo system. The direct digital servo contains all the elements
as the servo implemented by analogue technology, but some of these
elements in digital version are implemented by the microprocessor.
Several versions of the microprocessor implementation of servo systems
are encountered in practice. One of these possible microprocessor im-
plementations of the servo is presented in Fig. 3.10. The following
elements are implemented by microprocessor: the detector of the error
 83

signal, amplifications in the position and in the velocity feedback

loops. The other elements are implemented in analogue technology. As
it can be seen in Fig. 3.10, the signals from the potentiometer and
tacho-generator are feedback to microprocessor through AID converters *) .
The microprocessor computes (at the tactical control level) the desired
position of the jOint qOi. The microprocessor computes control (input
for the actuator u i ) in the following way: it multiplies the error in
positioning qi - qOi by the position gain K~, ~nd to the computed value
it adds the value of the actual joint velocity ql (obtained from the
tacho generator and through the AID converter) multiplied by velocity
gain Ki. The resulting numerical value of the input signal for the ac-
v
tuator is by DIA converter converted to voltage signal as the actuator
input u i . The second possible microprocessor implementation of servo
system is presented in Fig. 3.11. In this version the velocity feedback
loop is implemented in analogue technology, so the microprocessor just
has to multiply the error in positioning by the position gain. Here, we
shall not consider in detail the microprocessor implementation of servo
system. The more details on these problems can be found in literature
[4]. We just wanted to point out that the scheme of the digital servo
system is essentially similar to the implementation of the servo system
by analogue technology.

EXAMPLE 3.3.1. For the first joint of the robot in Fig. 3.2. the D.C.
electro-motor is applied, the data on which are given in Table 3.1.
The data on mechanical part of the robot are given in Table 3.2. In
Example 3.3. we have calculated the matrices of the model (3.3.6) for
this joint (if the second and the third jOints are locked in positions
q2* O. and q3* 0.). Now, we shall determine the transfer function
in the s-domain for this actuator with the mechanism in which just the
first joint is moving. If we adopt the third order model of the actua-
tor n 1 = 3, then the transfer function in the s-domain is obtained as:

1
~ 185.33
(3.3.26)
u 1 (s) (s2+695.65s+266.17)s

If we neglect the electrical time constant T~ (i.e. if we adopt the

second order model of actuator n 1 =2) we obtain the transfer function:

*) Instead the potentiometer it is often applied shaft encoder which

generates directly digital information on the actual position of the
jOint, so there is no need to apply AID converter to convert analo-
gue voltage signal from the sensor to digital number which is used
by microprocessor.
 00
....

COMPUTATION/GENERATION OF THE DESIRED

POSITION qOi
LOAD

-------------------------------------------- 9i

COMPUTATION OF THE INPUT FOR THE ACTUATOR ui qi qi

D/A ELECTRO-MOTOR
CONVERTER J
ACCORDING TO

TACHO-GENERATOR POTENT OMETE R

ui = Ki(qoi_qi)_Kiqi
P v Vi'
A/D 3
Ki
0
CONVERTER K"V
SCALING OF THE SIGNALS FROM A/D
CONVERTERS. (COMPUTATION OF qi AND qi Vi
1
FROM V~ AND vj') AND SCALING OF THE A/D
OUTPUT TO THE D/A CONVERTER CONVERTER

Fig. 3.10. Global scheme of the microprocessor implementation of the position servo system
 COMPUTATION/GENERATION OF
DESIRED POSITION qOi
-------------------------------- -

COMPUTATION OF POSITION 9i
SERVO LOOP
t
Vi '= _Ki(qi_qOi) D/A Vi' qi qi
p f-- I-- D.C. ELECTRO-MOTOR f
CONVERTEF r- + 2-
SCALING OF SIGNALS FROM A/D
TACHO-GENERATOR POTENT OMET R
CONVERTERS (COMPUTATION OF
Vi" Vi'
qi FRDr4 V1) AND SCALI NG Ki' 3 Ki" Ki
'---
OUTPUT FOR D/A CONVERTER v v
°
DIAGNOSTICS AND EXCEPTION Vi
HANDLING A/D 1
I--
CONVERTER
~

Fig. 3.11. Global scheme of the microprocessor implementation of the position servo system
with velocity feedback loop implemented by analogue technology

00
0'1
86

q 1 (s) 0.26642
-1- (3.3.27 )
u (s) (s+0.382)s

If we establish servo system around these actuator and jOint, then we

can obtain the transfer function of the servo as a function of positi-
on and velocity gains (this transfer function corresponds to the third
order transfer function of the actuator and the moving part of mecha-
nism (3.3.26)):

1 185.33 Ki
~= P
(3.3.28 )
q01 (s) 2 i i
(s +695.65s+266.17)s+185.33(K v S+K p )

E X ere s e s

3.21. Starting from the model of the actuator and the moving part of
the robot mechanism in the state space (3.3.4) and the corres-
ponding matrices for D.C. electro-motor, show that the transfer
functions in the s-domain for this system are given by (3.3.12)
and (3. 3 . 1 6) .

3.22. Repeat the previous exercise but for the second order model of
the actuator (n i =2), so that the corresponding transfer functions
are given by (3.3.17) and (3.3.18).

3.23. Show that the transfer function of the servo system around the
first joint of the robot in Fig. 3.2. as a function of position
and velocity gain is given by (3.3.28), if the transfer function
of the actuator and the moving part of the robot is given by
(3.3.26). Determine also the transfer function of this servo
from the external load g1(s) to the servo system output (the
jOint angle q1) as a function of position and velocity gains
(for the third order model of the actuator this transfer functi-
on generaly is given by (3.3.22)).

3.24. Repeat the previous exercise but if the second joint of the ro-
bot in Fig. 3.2. is locked in the position q2* = 1.573 [radl.
(Instruction: the matrices Ai and bi have been already determi-
ned in the Exercise 3.17, determine the transfer functions for
the actuator and the moving part of the robot).
 87

3.25. Determine the transfer functions of the servos as functions of

the position and the velocity gains for the actuators and "loads"
in Exercise 3.19.

3.26. Determine the transfer functions of the servos (as functions of the po-
sition and the velocity gains) around the first three jOints of
the robot in Fig. 2.5. if we apply D.C. electro-motors, the data
of which are given in Table 3.3. (the matrices of the actuators
and "loads" models have been already determined in Exercise 3.20).

3.27.* Determine the transfer functions in the s-domain for the hydrau-
lic actuator in the i-th joint of robot and for the moving part
of the mechanism of robot if we aSSUI!le that only the i-th jOint is
moving and all the other joints are locked. Determine also the
transfer functions of the servo around this actuator. (Instruc-
tion: the matrices of the electro-hydraulic actuator model in
the state space have been determined in Exercise 3.9. for two
cases n i =2 and n i =3; the procedure is completely identical as
for D.C. electro-motors). Calculate the transfer functions of the
hydraulic servo system (for the second order model) as functions
of the position and velocity gains, if data on the electro-hy-
draulic actuator are given in Table 3.5. and if this actuator is
applied in the first jOint of the robot in Fig. 3.2. (assume that
e 1 =0.12 q1, where e 1 is obtained in [m],if q1 is in [rad], and
P1=0.12 M1* where P 1 is in [Nm], if M1* is in [N]). Calculate these
transfer functions for two positions in which the second jointof
2* 2*
the robot is locked: for q =0. and for q =1.573 [rad], (the
leakage in hydraulic system is neglected) .

Mass of piston [kg] 2.65

Piston area [cmZ] 12.6
Flow/pressure coefficient of servovalve
0.00075
[cm3 / S ]
NIr7
Coefficient of proportionality of oil [m3/s] 0.0000833
flow and the current of servova 1ve IiiJ\
Coefficient of the viscous friction'r~1 30.
Compressibtl tty coefficient [N/m 3 ] 1.7'10 7

Cylinder volume [cm 3 ] 756.

Table 3.5. Data on electro-hydraulic actuator
88

3.3.2. Synthesis of servo system in s-domain

In the previous section we have determined the transfer function in the

s-domain for the position servo system around the i-th joint of the
robot. As we have seen, the transfer function of the servo is obtained
by combining the transfer functions of the elements in servo. If the
actuator, sensors and detector of the error signal have been already
selected, then the transfer function of the servo system depends just
on the selection of the gains in the position and velocity feedback
loops.

The role of the position servo system is to ensure positioning of the

jOint qi into the desired position qOi. As it is well known [3], the
transfer function of the system describes the behaviour of the system:
it shows accuracy of achieving of the goal position of the joint (in
the steady state regime) and it shows the characteristics of the tran-
sient process when the output variable is approaching to the desired
value (i.e. to the value of the input variable qOi). Obviously, the
behaviour of the system can be affected by the selection of the posi-
tion K~ and the velocity K; gains. Thus, our task in synthesis of the
servo system for the i-th jOint of the robot is practically reduced to
the selection of the position and velocity gains. We have to select
feedback gains in such a way to satisfy certain requirements regarding
the "quality" of the joint positioning.

Let us consider the transfer function of the position servo system. For
the sake of simplicity we shall consider the second order transfer
function, i.e. we shall neglect electrical time constant T~ of the ac-
tuator. Let us consider the transfer function (3.3.25) from the servo
system input qOi to the output qi. Let us write this transfer function
in the following form:

(3.3.29)

where s1 and s2 are the roots of the characteristic polynome (i.e. the
roots of the polynome in the denominator of the transfer function
(3.3.25». These roots might be written in the following form:

(3.3.30)
 89

where wi is called the aharaateristia frequenay of the system or the

~nda~ed natural freguency and si is the damping ratio or, damping faa-
tor [5). If we determine the roots s1 and s2 from (3.3.25), then we can
see that the wi and si are functions of the actuator parameters.and the
m?ment of inertia of the mechanism, and of the feedback gains K~ and
K1. Since the actuator and mechanism parameters are fixed values, we
v . .
can change w. and s. by changing Kp1 and K1.
1 1 V

It is well known that the servo system performance is defined by the

characteristic frequency and the damping factor. Let us consider the
response of the servo system to the step input function (the amplitude
of the step function corresponds to desired angle of the jOint qOi).
If si<1, then the response of the servo system to the step function in-
put is given by:

where c~ and c~ are constants. In this case the response of the servo
system is oscillatory around the desired position qOi. The response is
presented in Fig. 3.12. If si<1 the servo system is underdamped or
underaritiaaZZy damped. In this case the response is fast, it consists
of a sinusoidal function superimposed over an exponential function.
The output of the servo exponentially approaches desired input positi-
on, but it overshoots the desired angle.

If s. > 1 the servo system is overdamped or overari tiaa Uy damped. In this

1 .
case the response of the servo to step input signal q01 is given by:

i oi Ci -(Siwi+wi/S~-1)t cie-(SiWi-wi~t-1)t
q -q 3e + 4 (3.3.32)

where cj and c~ are constants.

The response of the system is also presented in Fig. 3.12. In this case
the response of the servo is slower, but it is non oscillatory and
there is no overshoot of the final position.

At last, if si=1 the servo system is aritiaaZZy damped. The response

of the servo to step function input is given by:

(3.3.33)

where Ci is constant.
o
90

undercritically damped

Fig. 3.12. Response of the servo system to step function input

The response of the servo is presented in Fig. 3.12. In this case the
response of the servo is fast but non oscillatory, and without over-
shoot of the desired position.

Now, we have to establish criteria for selection of servo feedback

gains Ki and Ki. Actually, we have to set requirements upon the "qua-
p v
Ii ty" of the servo system response, which we have to satisfy by appro-
priate selection of feedback gains.

First requirement concerns appearance of the overshoots in the servo

system response. It is well known, that with the robots the overshoots
of the desired position must not be allowed under any circumstances
[5]. For instance, if the desired position of the robot link is near
some obstacle in the work space and if the joint overshoots the desi-
red position, the link would be driven into the obstacle (Le. the robot
link would hit the obstacle). This is the reason why for robot it is
always required not to be undercritically damped, i.e. it must not be
allowed to be ~i<1. On the other hand, it is required that the servo
response be as fast as possible, in order to achieve fast positioning
of the robot (and ensure fast execution of the set task). So, the most
appropriate response of the robot servo is achieved if ~i=1, i.e. if
the servo is critically damped. Since the damping factor ~i is direct
function of the feedback gains we have to select the feedback gains to
achieve that ~i=1.

The next requirement which has to be met in selection of the feedback

gains concerns the influence of the external load upon the servo out-
put. In the previous section we have noted that the external load is
acting upon servo. In the considered case when all jOints are locked
 91

and just the i-th jOint is moving the external load upon the servo is
gravity moment gi. The transfer function (3.3.22), or (3.3.25), shows
the influence of this load upon the servo output (the joint position
qi). Let us consider the steady state regime when the robot jOint
stops, i.e. after the transient process is finished (theoretically it
occurs after infinite time t+ oo , practically, a·fter 3~5 time constants
of the robotic system, i.e. after 3t5 (l/wi~i». Based on tAe trans-
fer function (3.3.25), we can determine the steady state error between
the actual position of the joint qi and the desired set position qOi
if the constant gravity moment is loading the servo. Namely, since the
gravity moment is function only of the position of the i-th joint (the
other joints are locked), when the i-th jOint stops, we might consider
the gravity moment as constant (i.e. as a step external load). Thus,
the steady state error is given by:

(3.3.34)

where by gi(oo) is denoted steady state gravity moment upon the i-th
joint. It can be seen that the steady state error of the servo is in-
versely proportional to position gain K;. It is obvious that the stea-
dy state error represents the accuracy of the servo positioning.
Thus, we want to keep this error as low as possible. This error also
inform us about the robot stiffness. If we apply some external load
upon robot, the robot hand deflects due to finite stiffness of the servo.
This deflection can be also determined by (3.3.34), where the applied
external load (multiplied by the quadratic of the effective lever arm
from the i-th joint to the robot hand) should be substituted for gi(oo).
It is required that the stiffness of the robot be as high as possible.
Obviously, if we want to keep steady state error low we must select
the position gain to be as high as possible. Even more, if we calcu-
late the gravity moment gi(oo) and if we define allowable steady state
error ~qi(oo), i.e. if the allowable tolerance is set (or, if a desired
stiffness of the robot is imposed), then using (3.3.34) we can deter-
mine position gain K; which satisfies the set requirements.

Based on (3.3.34) it follows that to ensure high precision of the robot

posi tioning, it is required to select the position gain as high as pos-
sible. However, too high position gain involve certain drawbacks. For
example, too high feedback gains amplify also the noise in the system
and, thus, their influence might become too strong.
92

It is obvious that the position feedback gain directly affects the

characteristic frequency of the servo. Based on (3.3.25) and (3.3.30)
it is easy to show that the characteristic frequency wi and the damping
factor ~i are given by:

/NiCiKi
m M P
(3.3.35)

(3.3.36)

The characteristic frequency shows the frequency of oscillations of the

servo system output around the final position during the transient process.
The robot mechanism has its own resonant frequency with which the struc-
ture oscillates due to structural stiffness of the robot links. This
frequency is called structural resonant frequency or, simply structu-
ral frequency. We have shown above that the feedback gains should be
selected to ensure the servo system to be (over) critically damped.
However, we have to keep in mind the following facts: the damping ratio
~i is function of several parameters, and the considered linear model
of the actuator is just an approximation of the actual (nonlinear) sy-
stem behaviour. Thus, although we select feedback gains so that ~i>1,

the oscillations of the servo system output could appear. The servo sy-
stem would oscillate with frequency close to characteristic frequency
wi. If the characteristic frequency of the servo wi is close to the
structural frequency wo ' the resonant oscillations of the robot struc-
ture will appear, i.e. the oscillations of the servo will excite struc-
tural oscillations. It is obvious that such oscillations are not ac-
ceptable. Thus, we must require that the characteristic frequency of
the servo is under the structural frequency woo However, the structu-
ral frequency is difficult to determine theoretically (using some mo-
del). Usually it is determined experimentally. Due to unreliability of
determination of the structural frequency wo ' it is often required that
the characteristic frequency of the servo wi satisfies [5]:

< 0.5 (3.3.37)

If the characteristic frequency of the servo satisfies (3.3.37), where

Wo is the guess value of the structural frequency of the robot mecha-
 93

nism, then the servo will not excite the oscillating modes of the ro-
bot, i.e. it will not cause resonant structural oscillations.

If the expression for the characteristic frequency (3.3.35) is intro-

duced in (3.3.37), we obtain the upper limit upon the maximum allowable
value of the position gain, i.e.:

(3.3.38)

In this way we have determined the lower (3.3.34) and the upper limit
(3.3.38) upon the servo position gain. We have to select the feedback
gains to meet all the listed requirements. The following procedure in
selection of the feedback gains can be adopted. First, the position
gain K~ is selected according to (3.3.34) to satisfy allowable steady
state error. If the obtained value of position gain satisfy the requi-
rement (3.3.38), then this value of position gain can be accepted.
Otherwise, if the condition (3.3.38) is not satisfied, then we must
compute position gain from (3.3.38) as a maximum allowable value. In
that case the required accuracy around the desired set point will not
be ensured, but we can reduce the steady state error by some other me-
thod (see Section 3.4).

Once the position gain K~ has been selected, we must determine the ve-
locity gain Ki so as to ensure that the damping factor is equal to
v
one, i.e. to ensure that the servo is critically damped. Based on
(3.3.36) we obtain the velocity gain as:

(3.3.39)

In this way we select both position and velocity gains for the servo
controlling the actuator in the i-th jOint of the robot, assuming that
all the other joints are locked. However, the validity of this soluti-
on when all the jointso! the robot are moving simultaneously, will be
discussed latter on.

We have considered the second order transfer function of the servo sy-
stem and we have presented the synthesis of the feedback gains. In the
similar way the synthesis of the feedback gains can be carried on if
the third order transfer function is considered.
94

EXAMPLE 3.3.2. The transfer function of the servo in the first jointof
the robot presented in Fig. 3.2. is given by (3.3.28). The guess value
of the structural resonant frequency for this joint is w~ = 12 [rad/sl.
The feedback gains can be calculated based on (3.3.38) and (3.3.39),
and their values are:

K~ < 135.13
[r~d]
(3.3.40)
K1
v
43.6
[ra~/s]
where in calculating K; we have taken the upper bound for position gain
i.e. K~ = 135.13 [V/radl. In this case the steady state error is zero
since no gravity moment is acting upon the first jOint (rotation around
the vertical z-axis).

EX ere 1 s e s

3.28. Calculate the position and velocity gains for the servo in the
first joint of the robot in Fig. 3.2. if w1 = 12 [rad/sl, using
o
the third order transfer function determined in Exercise 3.23.
(for n i = 3), in such a way to satisfy conditions (3.3.38) and
(3.3.39) .

3.29. Repeat the previous exercise but if the second joint of the ro-
bot is locked in the position q2* = 1.573 [radl and w1 ~ 14
o
[rad/sl. (Instruction: use the transfer function obtained in
Exercise 3.24). How much the feedback gains have changed in res-
pect to the previous case when q2* = o.? In order to prevent the
servo to become underdamped in any position of the robot (i.e.
to ensure that ~i is always equal or greater than one,si ~ 1),
which of these two sets of feedback gains should be selected?

3.30. Repeat the previous two exercises for the same jOint of the robot
in Fig. 3.2, but if we apply as an actuator the D.C. electro-mo-
tor which parameters are given in Table 3.6. Compute (using the
second order transfer functions of the servo) the servo feedback
gains for two different positions of the second joint (q2* = o.
and q2* 1.573 [radl) and determine the relative variation of
 95

the feedback gains. Explain why in this case the relative varia-
tion of the feedback gains is less than for the previous D.C.
electro-motor (in Table 3.1)?

CE rlrad/s
V ] 0.62

CM [~] 0.61

J M [kgm 2] O. 035

Nv [-] 190.

Nm [-l 154.

rR [nl 0.3

Fv [ra~7s] 25.13

LR [Hl ",0.01

Table 3.6. Data on D.C. electro-motor (Exercise 3.30)

3.31. For the servos which transfer functions have been determined in
Exercise 3.25, determine the feedback gains if w2 = 50 [rad/s],
3 0
Wo = 50 [rad/s], the allowable steady state error in the third
jOint is Illq3(oo) I = 0.01 [ml. The conditions (3.3.34), (3.3.38)
and (3.3.39) have to be satisfied.

3.32. For the servos which transfer functions have been determined in
Exercise 3.26 (for the robot in Fig. 2.5) calculate the feedback
gains if w1 = 12 [rad/sl, w2 = 50 [rad/s], w3 = 50 [rad/sl and
o 0 0
allowable steady state error in the second joint is Illq2(oo) I =
0.01 [m]. The conditions (3.3.34), (3.3.38) and (3.3.39) have to
be satisfied.

3.33. For the robot in Fig. 3.13. (so-called Stanford manipulator) com-
pute the servo feedback gains if the data on the moments of
inertia of the mechanism Hii , data on D.C. electro-motors and data
on structural resonant frequencies of the links are given in Table
3.7. [5]. The conditions (3.3.38) and (3.3.39) have to be satis-
fied(the steady state errors are not pre-specified).
96

Fig. 3.13. The Stanford manipulator

LIN K 1 2 3

Moment of inertia of 0.5 2.8 6.2

- 2
mechanism Hii[kgm ]

JMNyNm [kgm 2 ] 0.953 2.193 0.782

rNm]
CMN m lA 3.415 8.61 3.41

CEN y [ ra~/s] 4.043 10.6 4.043

r R [Il] 0.9 0.8 0.9

Fy [ rad/s
Nm ] 0.043 0.21 0.043

fo = W/21T [Hz] 4. 6. 20.

Table 3.7. Data on actuators, moments of inertia and structural

frequencies for Stanford manipulator (for the first
three joints) [5]
 97

3. 34 ~ Write the programme (in some high programming language) for compu-
tation of servo feedback gains if the input values for the pro-
gramme are: data on actuator parameters (D.C. electro-motor), data
on the moment of inertia of the mechanism Hii , data on structu-
ral frequency w of the mechanism, data on maximal allowable ste-
o .
ady state error ~qL(oo), and data on maximal gravity moment in
the joint. Check by computer the results obtained in the previ-
ous exercise.

3.35.* Calculate the servo feedback gains for the hydraulic servo which
transfer function has been determined in Exercise 3.27. (for two
positions of the second joint q2* = 0., and q2* 1.573 [radl)
and which is applied in the first joint of the robot in Fig. 3.2.
The guess value for the structural frequency is w1 = 12. [rad/sl
2* 1 2*
for q O. and Wo ~ 14. [rad/sl for q = 1.573 0 [radl. How
much have the feedback gains varied for various positions of the
second joint? Compare these variations with the results obtained
in Exercise 3.29.

3.3.3. Synthesis of servo system by

pole-placement method

In the previous section we have presented the synthesis in thes-domain

of the position servo system for the i-th joint of the robot assuming
that all other joints are locked*). NOw, we shall present the synthe-
sis of feedback gains using the state space model of the system.

The state space model of the actuator and the mechanism (if just the
i-th joint is moving) is given by (3.3.4). As we have explained in the
previous sections, the servo system is realized by introducing feed-
back loop with respect to the position error of the robot joint (using
the sensor of position, the detector of the position error signal, and
amplifier) and feedback by velocity (using the sensor of the actual
joint velocity and amplifier). The block diagram of the servo in s-do-
main is given in Fig. 3.9. and it has been explained in Section 3.3.1.
Practically, the servo feedback loops mean that the input voltage

*)
Here, we shall not present various classical methods for servo sys-
tem synthesis (such as for example, Bode's method in the frequency
domain, Niquist method and similar). However, our recommendation is
that the reader should remind of these methods from the literature
[3l.
98

signal u i is given by (according to (3.3.19)):

(3.3.41)

Since the position and velocity of the joint are the variables in the
system which are measured by sensors they are the system output va-
riables. The system output is given, in this case, by:

i ·i T
Yi = (q , q ) (3.3.42)

In the general case the output vector is of the order k~ 1 (here,

]. ->
obviously, k~ = 2). The output variables are related to the coordina-
tes of the state vector xi. In general case the relation between the
output vector Yi and the state vector xi is given by*):

(3.3.43)

where C.]. is the output matrix of dimensions k~xn

].
.. In the particular
].

case,when the output of the system is given by (3.3.42) while the sta-
i i'i T
te vector is given by x = (e , e) for n i =2, and if we assume that
(3.2.12) holds, then the matrix Ci is the unit matrix Ini' If we adopt
i ·i .i T
the third order model of the system n i =3, x=(e , e , ].R) , then the
matrix Ci is given by:

C.].
[: :] 0
1
0
(3.3.44)

Now, we can write that the input signal of the actuator (3.3.41) is
equal to:

(3.3.45)

where by k i is denoted the vector of the feedback gains

k.]. (3.3.46)

*) In general case, the relation between the system output and the
system state might be nonlinear, but we shall consider just the
linear case.
 99

The model of the complete servo system in the state space (i.e. the
model of the actuator and the mechanism (3.3.4) together with the fe-
edback loops (3.3.45» can be written in the following form:

·i
x (3.3.47)

The model (3.3.47) represents the closed-loop model of the system. The
task of the control system is to ensure that the system output Yi ap-
proaches the desired output y~ = (qOi, O)T. In order to ensure posi-
~ .
tioning of the jOint in the desired set point qO~, we must ensure the
stability of the system (3.3.47) around the desired state x Oi = (qoi,
0, O)T, i.e. we must ensure that the eigen-values of the closed-loop
matrix of the system (Ai_bik~C.) are placed in the desired positions
~ ~ .
in the complex plane. The eigen-values of the closed-loop matrix (A~
Ai T
b kiC i ) are the poles of the transfer function of the servo system
(3.3.29). It is well known [3] that the system is stable if the eigen-
-values of the closed-loop matrix are in the left part of the complex
plane (i.e. they must be on the left from the imaginary axis). Besides,
the poles of the transfer function (3.3.30) must satisfy conditions
that the damping factor is ~i>1, and that the characteristic frequency
is less than 0.5 woo Thus, in the servo system synthesis (i.e. in the
selection of feedback gains), we may specify the desired positions of
the system poles in the complex plane. Once the positions of the poles
are specified, the vector of the feedback gains k i can be computed
which will ensure desired positions of the eigen-values of the closed-
-loop matrix of the system. The gain vector is computed from the fol-
lowing condition:
n.
n
~

K (s<:'-s) (3.3.48)
j =1 J

where det( ) denotes the determinant of the corresponding matrix, K is

the proportionality coefficient, and sj, j=1,2, ... ,ni denote the desired
positions of eigen-values of the closed-loop matrix in the s-plane.
Thus, the synthesis of the servo reduces to determination of the gain
vector k i which has to satisfy the algebraic equation (3.3.48). As it
is well known, this procedure for control synthesis is called pole
placement method [6].

There are numerous problems related to control synthesis by this method

if the general case of the multi-input and multi-output system is con-
sidered. Here, we shall not treat these problems, since we consider
100

just the synthesis of the feedback gains for local servo systems for
individual joints of the robot. Evidently, the local servos are with
single input u i and their output vector is usually given by (3.3.42).
In this particular case the application of the pole-placement method
for synthesis of servo feedback gains is simple. Nevertheless, we shall
briefly mention problem related to the order of the system n i , the or-
der of the output k~ and the number of the eigen-values of the closed-
1
-loop system matrix which might be specified.

In the previous section we have considered the second order model of

the servo system (for n i =2). If the servo output is given by (3.3.42),
then the positions of both eigen-values of the closed-loop matrix can
be specified, and based on (3.3.48) we can determine the position and
velocity feedback gains to ensure desired places of the eigen-values.
This has been practically done in the previous section (but by consi-
dering transfer functions), since we have defined poles in order to
satisfy certain conditions.

However, if we consider the third order model of the actuator (for

n i =3), and if the system output is given by (3.3.42) (i.e. if kl=2),
then we may specify positions of just two eigen-values, while the po-
sition of the third eigen-value is free, i.e. it cannot be defined in
advance. Nevertheless, for the considered servo systems it is always
possible to ensure that all three eigen-values are in the left part of
the complex plane (i.e. it can be ensured that the closed-loop system
is stable). However, there is a constraint upon the allowable positi-
ons of the eigen-values in the complex plane, if k~<n .. The eigen-values
1 1
cannot be placed so that the absolute values of their real parts are
arbitrary large. Let us show this fact by a particular example. As we
have seen in the Section 3.2.2. the matrices of the actuator (3.2.7)
have the following form (see Exercise 3.9. for the electro-hydraulic
actuators) :

(3.3.49)

If we assume that the matrix Ci is in the form (3.3.44) and if we spe-

cify two complex conjugate eigen-values s~,2 -~~w~±jw~/1-~~2, then
we can determine the feedback gains based on (3.3.48) as:
 101

(3.3.50)

while the third eigen-value is placed at:

(3.3.51)

Based on (3.3.51), we can see that the real part of the third eigen-
o
-value depends on the specified eigen-values s1 ,2' and if

(3.3.52)

then the system is not stable, i.e. the third eigen-value is not in
the left part of the s-plane. Actually, (3.3.52) determines the upper
bound upon the absolute values of the real parts of the specified
eigen-values: one must not require that the eigen-values of the clo-
sed-loop system have the real parts ~~w~ greater (by absolute value)
~ ~

than the limit set by (3.3.52). The real-parts of the eigen-values of

the system determine the speed by ~hich the system output Yi approac-
hes to the desired output y~ = (qO~, O)T, i.e. the velocity by which
~ .
the joint approaches to the desired position qO~. If we want to incre-
ase the absolute values of the real parts of the eigen-values of the
closed-loop system matrix, then we must introduce the feedback by the
third state coordinate. In the case of D.C. electro-motors the third
state coordinate is the current of the rotor circuit which also might
be measured (a resistor might be used as a sensor, but the obtained
signal must be filtered using appropriate filters)*). In this case the
output matrix Ci becomes unit matrix and the vector of the feedback
gains is given by:

(3.3.53)

where Ki is the gain in the feedback loop by the robot current (the
third state coordinate). Now, all three eigen-values of the closed-
-loop system matrix might be specified. By pole placement method we
can compute the feedback gains (3.3.53) to place the poles of the sys-
tem in the desired locations in complex plane.

*)
In the case of the electro-hydraulic actuators the third coordinate
of the state vector is the difference of pressure in the cylinder
which also can be, relatively simpl~measured.
102

The scheme of the servo system is presented in Fig. 3.14. in the case
when the feedback loop by the robot current is introduced.

We should note that this method for control synthesis can be also
used to determine the feedback gain if only feedback loop by position
error is introduced (i.e. if k~
1.
= 1, y.
1.
= qi). Namely, the servo might
be realized only with the position feedback loop, but the performance
of such servo system is unsatisfactory regarding the requirements which
are encountered in robotics. Actually, such a servo cannot satisfy
both requirements regarding the servo damping and the steady state er-
ror.

EXAMPLE 3.3.3. The data of the D.C. electro-motor applied in the first
joint of the robot in Fig. 3.2, are given in Table 3.1. If the inertia
of the mechanism is taken into account, the matrices of the model of
the actuator and mechanism (3.3.4) have been calculated in Example 3.3.
For this actuator and mechanism we have to determine the servo feed-
back gains using the pole placement method. Let us assume that the fe-
edback loops are introduced by all three state coordinates, i.e. by
joint position, by joint velocity and by rotor current. Let us assume
that it is required to place the poles of the closed-loop servo in the
following positions:

o
s1 ,2 -6. , -800.

By the pole-placement method we can calculate the servo feedback gains

as:

K~ = 155.43 [r~d]' K~ = 50.568 [ra~/s]'

If the feedback loop by the rotor current is not introduced, we may
define just two poles of the closed-loop system. Let us define two
real poles:

-5. , -6.

Based on (3.3.50) we calculate the feedback gains as:

110.85 [r~d]' 39.372 [_V_]

rad/s
 i~ feedback by current
9 i (1 Dad)

t I
fi
L
qOi + + I cji qi
r
!
r
J
-
i
a32
L -,~

qi feedback by velocity

feedback by position

Fig. 3.14. Scheme of servo system with the feedback loop by the rotor current (according
to the model of actuator in the state space (3.2.7), (3.3.49»

8
E x ere 1 ses
3.36. The second order model of the actuator and the mechanism (n i =2)
has the matrices in the following form:

[~] (3.3.54)

Determine the expressions for the feedback gains if the poles of

the closed-loop system are prescribed, in two cases:

a) if feedback loops by position and by velocity are introduced

k~ = 2 (two poles are prescribed; examine both cases: when
1
the pair of complex conjugate poles are prescribed and when
the pair of real poles is prescribed),

b) if feedback loop by position error is introduced only,k~ = 1

1
(one pole is prescribed; examine all three cases: if the pres-
cribed pole is real, if the real part of the pair of complex
conjugate poles is prescribed, and if the imaginary part of
the pair is prescribed); determine also the second pole which
is not prescribed. Is it always possible to stabilize the sy-
stem by introducing the position feedback only?

3.37. The third order model of actuator and the mechanism (n i =3) has
the matrices in the form (3.3.49). If we introduce the feedback
by position only, determine the expressions for the position fe-
edback gain so that the closed-loop servo system has:

a) real pole in the prescribed position s~ or

1

b) real part of the pair of complex poles in the prescribed po-

000
sition Re(s1,2) = -~iwi' or

of complex poles in the given po-

(the complex conjugate pair is

In all three cases determine the expressions for the rest two
poles of the closed-loop system. What can you conclude con-
 105

cerning the possibility to stabilize the system by introducing

the position feedback loop error only?

3.38.* Write the programme (in some high programming language) for deter-
mination of the feedback gains if the input data for the programme
are: data on actuator, data on the moment of inertia of the mo-
ving part of the mechanism HI..I , prescribed poles of the closed-
-loop servo system. The programme should include various informa-
tion feedback structure (if the feedback by position error is
introduced only, or if the feedback loops by position and velo-
city are introduced, or if the feedback loops by all three state
coordinates are introduced). (Instruction: use the expressions
given in the text and in Exercise 3.37). Using the programme,
check the results in Example 3.3.3.

3.39.* Write the programme (in some high programming language) for simu-
lation of the closed-loop servo system if initial state xi(O)
and desired position qOi are given. The input data for the pro-
gramme are: data on actuator, data on moment of inertia of the mo-
ving part of the mechanism HI..I , the vector of feedback gains,
.
the output matrix C., the initial state xl(O) and desired posi-
. l
tion qOl. (Instruction: use the state space model of the servo
system (3.3.47) to compute the first derivative of the system
state vector xi, and then apply some of available numerical methods
for integration, e.g. use the simplest Euler method for inte-
gration, see Section 5.3.; the integration interval should be
selected to be less than 1 [ms]). Simulate the positioning of
the first jOint of the robot in Fig. 3.2. around desired position
qol = 0.5 [rad], if initial state is xl (0) = (0,0, O)T. Data
on the model of servo are given in Example 3.3; use two sets of
the feedback gains determined in Exercises 3.28. and 3.29. Com-
pare the results of simulation for these two sets of the feed-
back gains.

3.40.* The data on electro-hydraulic servo system are given in Table

3.5, for n i =3 (see Exercise 3.27). Determine the feedback gains
which should place two poles of the closed-loop system in the
positions s~,2 = -6. Explain how the feedback loop by the third
state coordinate can be implemented in the case of electro-hy-
draulic servo system?
106

3.3.4. Effects of inertia variation and gravity

moment of the mechanism on the behaviour
of servo system

In the previous sections we have considered the synthesis of local sta-

tic servo systems for the individual joints of the robot mechanism. In
doing this we have considered each joint and its actuator independent-
ly from the other joints and actuators of the robot. Namely, we have
considered the i-th joint of the robot and its actuator, and we have as-
sumed that all the other joints are locked. The model of the i-th joint
and actuator is given by (3.3.4). This model includes the moment of
inertia of the moving part of the mechanism -Hii(q * ) around the axis of
the i-th jOint. This moment of inertia depends on the positions of all the
other joints, i.e. it depends on the positions q * in which the other
joints are locked. Up to now, we have considered the behaviour of the
system for just one set of positions of all the other joints q * . For the
given values of the vector q * we have determined the servo feedback
gains. Obviously, if the positions qj* (for j>i) of the other joints
change, the moment of inertia of the mechanism around the i-th jOint
-H ..
(q * ) will also change. Then the feedback gains calculated for the
l.l. * *)
previous position q need not be valid any more . The variation of the
positions of the other jOints of the robot causes also the change of
the gravity moment of the mechanism around the i-th joint. We have
neglected the gravity moment in the model (3.3.4), but we have consi-
dered its effect upon the servo system positioning (steady state error) .
The variation of positions of the other joints will reflect the steady
state error in the i-th joint.

First, let us consider the effects of the variation of the moment of

inertia of the mechanism around the axis of the i-th jOint Hii upon
the performance of the local servo system in the i-th joint. Let us
consider what happens if the joints change their positions q with res-
pect to previous positions q * for which we have calculated the servo
feedback gains. The variation of the positions of the jOints qj*qj*
(j*i) causes the variation of the moment of inertia of the mechanism
-
around the i-th jOint Hii(q)*Hii(q * ). Let us consider the second order
model of the servo system and the synthesis of the servo feedback
gains in s-domain (Sect. 3.3.2). We have seen that the damping factor
of the servo system is given by (3.3.36). We have also shown that it
is required to ensure that the servo is always critically or overcri ti-

*) See Exercises 3.28.- 3.30.

 107

cally damped. Let us assume that the feedback gains are selected to
ensure that the servo is critically damped for the position of the ro-
bot defined by vector q * (i.e. if the moment of inertia of the mecha-
nism is H .. (q*». In this case the velocity gain Ki is given by
~~ v
(3.3.39). If the other joints of the robot are moved to the new posi-
. .*
tionsqJ*qJ the damping factor of the servo in the i-th joint becomes:

~i (3.3.55)

where Hii (q) is the moment of inertia of the mechanism for the new po-
sitions of the joints q.

- -
We can distinguish two cases. If Hii(q)<Hii(q * ), then ~i>l, i.e. the
servo is overcritically damped in the new position, which means that
the selected feedback gains ensure acceptable response of the servo in
the new position of the robot. However, if -Hii(q»Hii(q
- * ), then ~i<l,

i.e. the servo is undercritically damped, which cannot be accepted.

- -
This means that we must not allow the situation in which Hii(q»Hii(q *)
to appear. To prevent this, we must determine max H .. (q'), i.e. we must
q' ~~
determine the maximum possible value of the moment of inertia of the
mechanism around the i-th joint (maximum over all allowable positions
of joints). For this position of the robot we must calculate the velo-
city feedback gain using the expression (3.3.39). By this, we ensure
that for all possible positions of the robot joints the servo system
in the i-th joint is overcritically,or critically damped.

If we have selected the velocity gain in this way, then we can again
distinguish two cases:

a) If
iii
JMNvNm»m~x(Hii(q
- * )-Hii(q'», then the variation of the damping
q
factor (3.3.55) for various positions of the robot joints is rela-
tively small. In other words, the response of the servo system is
nea~ly uniform independently on the positions of the other joints of
the robot. The servo system is nearly critically damped for all po-
sitions of the other joints of the robot, and the satisfactory po-
sitioning of the i-th jOint might be expected. This case appears
if a relatively powerful actuator and large gear reducer with high
reduction ratio are applied. Such actuator and reducer have anequi-
valent moment of inertia of the rotor JMiNiN i relatively high with
v m
108

respect to the masses and moments of inertia of the robot links. In

practice it is often the case that relatively powerful actuator and
large reducer are applied and by this the effects of the variation
of the moment of inertia of the mechanism are reduced. Namely, the
large equivalent moment of inertia of the actuator rotor tends to
mask the variation of the moment of inertia of the mechanism Hii .
However, the drawback of such a solution lies in the fact that too
large actuators and reducers are applied, which is undesirable from
the standpoint of the energy consumption and the prise of the robo-
tic system. On the other hand, large reducers often introduce large
backlash and friction in the system which certainly reflects the
accuracy of positioning of the servo system. That is why large ef-
forts are directed towards introducing the so-called direct drive
robots, i.e. the robots with actuators having no reducers. For such
robots the problems of backlash and friction are minimized.

b) If
iii -
JMNvNm«m~x(Hii(q
* )-Hii(q'», then the damping factor might
q
significantly vary depending on the position of the robot (acc. to
(3.3.55». Thus, for the robot configuration q * for which Hii(q
- *)
max H .. (q') the critical damping of the servo is obtained (which
q' 11
means fast response of the servo), while for the robot configurati-
on q for which H11
.. (q) = min
q'
H .. (q') the damping of the servo beco-
11

mes very overcritical (~i»l), which causes slow response of the

system. Thus, the servo system has a variable performance depending
on the positions of the other jOints of the robot (although the re-
sponse is never oscillatory, nor the overshoots of desired positi-
ons appear). To ensure critical damping of the servo for all posi-
tions of the other jOints, in this case, (and to ensure uniform per-
formance of the robot around all its possible configurations), we
must introduce compensation for the effects of the variable moment
of inertia of the mechanism (by introducing so-called global con-
trol), or we must implement variable gains which change depending
on the configuration of the robot (i.e. depending on the actual va-
lue of the moment of inertia of the mechanism). These problems will
be addressed in Chapters 4. - 6. We must underline that the deter-
mination of the variable moment of inertia of the mechanism in ge-
neral case, is very difficult, since the payload carried by the ro-
bot hand might be of variable and unknown parameters (with variable
mass, dimensions, shape, moments of inertia and so on). If these
parameters are not known in advance, the problem of their identifi-
cation arises and the adaptation of the servo system to the variation
of the payload parameters has to be considered (see Chapter 6).
 109

We have shown in Section 3.3.2. that the position gain can be selected
based on Expression (3.3.38), i.e. this expression gives the upper
bound of the allowable value of the position gain. As it can be seen
from (3.3.38), the variable moment of inertia of the mechanism Hii(q * )
also affects the upper bound of the position gain. However, we have
to keep in mind that the resonant structural frequency of the robot
also depends on the moment of inertia of the mechanism. This frequency
decreases if the moment of inertia of the mechanism increases. Wemight
assume that, in the first approximation, the structural frequency is in-
versely proportional to the square root of the moment of inertia of
mechanism [5].

If Wo(H ii ) is the resonant structural frequency for the robot configu-

ration q for which the value of moment of inertia of the mechanism
around the i-th joint is Hii , then we can write that the structural
frequency wo(H ii ) for some other configuration of the robot mechanism
(for which the value of moment of inertia of the mechanism is Hii ) is
given by:

k WO(Hii)/J!N!N;+Hii
(3.3.56)
l,.iNiN i + H ..
M v m 1.1.

where k is the proportionality coefficient.

We have assumed that the position servo gain K~ is computed based on

(3.3.38) for the robot configuration for which the moment of inertia
of the mechanism is -Hii (q * ). If the other joints move in some new
(locked) positions q to which corresponds Hii(q), then the characte-
ristic frequency of the servo in the i-th joint becomes, according to
(3.3.35) and (3.3.38):

(3.3.57)

It is obvious that the characteristic frequency of the servo satisfies

the following inequality:

(3.3.58)
110

This means that regardless of the variation of the moment of inertia of

the mechanism, the condition (3.3.37) is always satisfied, if the position
servo gain is selected to satisfy condition (3.3.38). In other words,
the constraint upon the maximum value of the position servo gain does
not depend on the moment of inertia of the robot mechanism, and we can
select the position servo gain just based on (3.3.38). Such position
gain will be valid for any configuration of the robot. By so selected
position servo gain we ensure that the characteristic frequency of the
system is sufficiently below the resonant structural frequency of the
system. However, as we have explained above, to ensure that the servo
is always (over)critically damped we must select the feedback gains
taking into account the maximum possible moment of inertia of the mecha-
nism (or, we might apply variable velocity servo gain).

Based on the above consideration it follows that the effects of the

variable moment of inertia of the mechanism upon the servo system per-
formance can be eliminated by selection of powerful actuators and lar-
ge gear reducers with high reduction ratio, or by applying constant
position servo gain and variable velocity servo gain, or by global
control which compensates for the effects of the variation of the mo-
ment of inertia of the robot mechanism.

The second factor which effects the response of the servo if the con-
figuration of the robot change, is the gravity moment around the axis
of the i-th joint gi(q i , q * ). In synthesis of local servo system for
the i-th joint we have neglected the gravity moment. However, in Sec-
tion 3.3.2. we have considered the effects of gravity moment as exter-
nal load upon the performance of the servo system. We have shown that
steady state error due to external load is given by (3.3.34). It is
obvious that the steady state error is the consequence of the gravity
moment (if we assume that all the other joints are locked). When the
i-th joint stops, this gravity moment becomes constant which depends
on the configuration of the mechanism q * (but it depends also on the
final position of the i-th joint). The error caused by this gravity
moment can be calculated by (3.3.34) as the steady state error of the
servo.

From (3.3.34) it follows that the error due to gravity moment is in-
versely proportional to the position servo gain. However, we have seen
that the position gain is limited by the condition (3.3.38). Based on
(3.3.34) and (3.3.38) we can determine the minimal steady state error
 111

which might be achieved:

(3.3.59)

Obviously, this "minimal" steady state error depends on the gravity

. *
moment g. (ql, q ), i.e. it depends on the positions of all the joints
l
of the robot. If we determine the maximal possible gravity moment around
the i-th joint (for all possible configurations q * ) by (3.3.59) we may
determine the steady state error which can appear in the positioning
of the i-th joint. Obviously, this error determines the accuracy of
the servo in the i-th jOint. If this error is not within the allowable
tolerances around the desired position of the joint we must compensate
for the effects of the gravity moment, i.e. we must el~m~nate t~~s un-
acceptable steady state error.

On the other hand, due to variation of the gravity moment for various
configurations of the robot (i.e. for various positions of the locked
joints and for various desired positions of the i-th joint), the steady
state error of the servo also varies for various configurations of the
robot. This means that the performance of the servo in not uniform for
all desired set positions, and this is an unacceptable drawback of
the servo system (from the standpoint of the robot application and pro-
gramming). This is also the reason to consider compensation for the gra-
vity moment.

The compensation for the gravity moment might be achieved in several

ways. One of the solutions isto directly compensate for gravity momen~

and the other is by application of the feedback by integral of the po-

sition error. Here, we shall consider the first approach, and the se-
cond will be addressed in Section 3.4. It should be noted that in some
applications the steady state error due to gravity moment can be eli-
minated by brakes in the jOints, but this solution is not applicable
for elimination of the errors in tracking the desired trajectories of
the joints.

Since the gravity moment around the i-th joint is well known function
of the joint coordinates (positions), this moment can be computed on-
-line in the control microprocessor. Based on the computed value of the
gravity moment, the microprocessor calculates the voltage signal which
corresponds to this moment and generates the control signal for the
112

actuator. This additional control signal produces the driving torque,

which compensates for the gravity moment around the joint. The control
scheme which includes this direct compensation for the gravity moment
is presented in Fig. 3.15. For the second order model of the servo sy-
stem, the control signal including direct compensation for the gravity
moment is computed in the following way (if D.C. electro-motor is ap-
plied) *)

(3.3.60)

where by gi* is denoted on-line c~mputed gravity moment around the i-th
jOint in the desired position qOl. Here, it is assumed that the angles
or displacements of all the other joints are measured by appropriate
sensors and the signals are sent to microprocessor which (based on the
* If the computedgra-
obtained valu;s of joint coordinates) computes gi'
vity moment gi perfectly coincides with the actual gravity moment gi'

COMPUTATION ri
R
OP 9i* CiN i
qOi Mm

q*
KM 9j
(T's+1 )s
m

+
(T~S+1)S
t

Fig. 3.15. Position servo system with direct

compensation for gravity moment

*) The problem of the direct compensation of the gravity term in the

case of the third order model of the actuator will be considered
in Chapter 5.
 113

then the control signal (3.3.60) will produce the torque which will
competely compensate for the gravity torque and the steady state error
due to the gravity moment will be eliminated. The positioning of the
servo in the desired position qOi in this case will be very accurate.
However, all parameters of the robot mechanism which are required to
compute the gravity moment gi* (link lengths, positions of the centers
of the masses of the links, masses of the links) are usually not known
accurately. Thus, in the general case, the gravity moments cannot be
ideally accurately calculated, nor they can be ideally compensated for.
Nevertheless, in this way we can achieve significant decrease of the
positioning error.

In implementing of the presented compensation for the gravity moment, a

problem of on-line computation of the gravity moment arises, i.e. a
problem of number of additions and multiplications necessary to compu-
te gravity moment. This problem will be addressed in Chapter 5.

EXAMPLE 3.3.4. For the first jOint of the robot in Fig. 3.2. the posi-
tion and velocity servo gains have been computed in Example 3.3.2. for
the value of moment of inertia of the robot mechanism which corresponds to
the locked position of the second joint q2* =0. (the corresponding trans-
fer function of the servo is given by (3.3.28)). If the second jOint
moves to another locked position, the moment of inertia of the mecha-
nism changes, which causes the variation of the servo damping factor
according to (3.3.55). The variation of the damping factor when the an-
gle q2* varies is given in Table 3.8. It can be seen that the damping
factor varies significantly if the q2 varies, but it is always ~1>1
since the velocity servo gain has been calculated for q2* = 0., when
the moment of inertia of the mechanism around the first jOint is maxi-
mal. If we decide to introduce variable velocity feedback gain, then
its values for various q2* are also given in Table 3.8. These variable
velocity gains are calculated to maintain the damping factor equal to
one for all positions of the second joint. Applying such variable fe-
edback gains we maintain the uniform performance of the servo in the
first jOint independently on the position of the second joint. Obvi-
ously, the implementation of the servo system with variable velocity
gain is much more complex than if the fixed feedback gains are applied.

The performance of the servo system in the first joint of the robot in
Fig. 3.2. with fixed servo gains has been simulated by computer. The
responses of the servo for various (fixed) positions of the second
114

jOint q2* are presented in Fig. 3.16. under assumption that the selec-
2*
ted gains ensure that ~1 = 1 for q = O.

Position of the ~1 for fixed Velocity gain

2*
second joint q gain for ~1 =1 - [V/rad/s]

0° 1. 43.6

30° 1.003 "'43.5

60° 1.056 41.2

90° 1.15 37.7

120° 1.277 33.8

150° 1.39 ",31.

Table 3.8. Variation of the damping factor and

variable velocity servo gain

0.5 ------------- ----- ...-

:.,:.-.=,.: :.-

/
/
.. '
../.' q
2*
o.
0.3 / .. '
/.'
/ .. '
q
2* 1. 57[rad]
/ .. '
/ ....
0.1 " .' q
2*
3.141 [rad]
~<.. ,..
/ '

~~~~_+__~~~~~__t---- -----ll~~~~_t_~_~

0.2 0.5 0.8 1. t [5]

Fig. 3.16. Response of the servo system in the first joint

of the robot in Fig. 3.2. for various (locked)
positions of the second jOint

Let us consider the second joint of the robot in Fig. 2.6. and let us
determine the effects of gravity moment upon the servo system in this
joint. Let us assume that the D.C. electro-motor is applied, the data
of which are given in the second column of the Table 3.3., while the
mass of the second link is m2 = 9. [kg], the distance between the se-
cond joint and the centre of mass of the second link is £2* = 0.2 [m],
the distance between the second and the third joint is £2 = 0.5 [m],
the mass of the third link is m3 = 10. [kg] and the distance between
 115

*
the third joint and the centre of mass of the third link is £3=0.2 [ml
(for q3=0.). If we assume that the structural frequency for the second
joint is w2 = 45 [rad/sl, then we calculate the position servo gain to
o
satisfy the condition (3.3.38) and obtain the value K; = 1414. [V/rad).
The steady state error of the servo due to gravity moment can be cal-
culated based on (3.3.59). For various positions of the second and the
third joint various values of the gravity moment are obtained resul-
ting in various steady state errors in the second joint. The variation
of the steady state error in the second joint for various positions of
the second and the third joint is given in Table 3.9.

qo2 q*3 6q2(oo) 92

[radl [radl [rad) [Nm)

o. o. o. o.
1.57 o. 0.000783 86.3
1.57 0.4 0.00110 126.
1.57 0.7 0.0014 154.
1.0 0.7 0.00117 130.

Table 3.9. Steady state errors and gravity moments in the

second jOint of the robot in Fig. 2.6.

E X ere s e s

3.41. For the first jOint of the robot in Fig. 2.5. determine the servo
feedback gains (data on the robot and actuators are given inTab-
les 3.3. and 3.4). If the resonant structural frequency is esti-
mated to be w1 = 12 [rad/s) (for the moment of inertia of the
o
mechanism around the first joint which corresponds to the posi-
tion of the third joint of q3* 0.6 [m) determine the servo
gains to satisfy the following requirement:

a) the critical damping has to be maintained for the following

positions of the third joint q3*=0., 0.20, 0.40, 0.60 [ml
(Instruction: for each position of the third jOint q3* deter-
mine the velocity servo gain so that the servo is critically
damped ~1=1), or
116

b) the fixed servo gains have to ensure that the system is over-
critically damped for all positions of the third jOint.

3.42.* Let us assume that the electro-hydraulic actuator is applied in

the first joint of the robot in Fig. 3.2. Data on this actuator
are given in Exercise 3.27. (Table 3.5). The servo gains are se-
lected to satisfy usual requirements, for the position of the se-
cond joint q2* O. and for the estimated value of the structu-
ral frequency of w112. [rad/s]. Determine the variation of the
o
damping factor of the servo for various positions of the second
joint q2* given in Table 3.8. Also determine the values of the
velocity servo gain which should be applied to maintain the cri-
tical damping for all positions of the second joint q2* (see
Exercise 3.35).

3.43. a) Calculate the errors in positioning of the robot end pOint due
to steady state errors of the servo system in the second jOint
of the robot in Fig. 2.6. The steady state errors of the servo
are given in Table 3.9. (assume that the distance between the
centre of mass of the third link and the manipulator endpoint
is 0.3 [m]).

b) Determine the reduction of the end point position error, if

the direct compensation for the gravity moment around the se-
cond joint is applied, but instead of actual value of the mass
of the third link (m 3 = 4. [kg]) an estimated value of m3~3.

[kg] is used for computation of gravity moment.

3.44.* Starting from the open-loop model of the servo system (3.3.4) and
the closed-loop model (3.3.47) determine the model of sensitivi-
ty to variation of the moment of inertia of the mechanism Hii and
calculate the matrix of the sensitivity model for the servo sys-
tem given in Exercise 3.41. Determine the eigen-values of the
matrix of the sensitivity model for various positions of the
third joint q3* of the robot which have been considered in Exer-
cise 3.41.

3.45. Repeat Exercise 3.41 but now assuming that at the end point of
the minimal configuration of the robot there is a gripper with
a payload of a mass mp 1.5 [kg]. If the mass of the payload is
not known in advance and if the values of the servo gains are
 117

taken as in Exercise 3.41. b), determine the minimal damping

factor which can occur when the payload is present.

3.46. Repeat Exercise 3.43 but now assuming that at the end point of
the minimal configuration of the robot there is a payload of a
mass mp = 1.5 [kg]. Assume that the direct compensation of the
gravity moment is introduced, but in on-line computation of the
gravity moment we take an estimated value for mp~O. (since the
actual value of the mass of the payload is assumed to be unknown).
Determine the steady state errors of the servo in the second
joint, and the errors of the positioning of the end point of the
manipulator.

3.47. Determine the deflection of the end point of the Stanford mani-
pulator (Fig. 3.13), the data of which are given in Tables 3.7.
and 3.10., due to steady state errors of the servos in the robot
jOints which are caused by gravity moments around the joints.
The gravity moments around the robot joints are given in Table
3.10. (the servo gains are computed according to data given in
Table 3.7) [5].

JOINT r [m] 9i [Nm]

1 0.54 O.
2 0.50 69.3
3 - 81.73
4 0.25 5.54
5 0.25 5.54
6 0.25 O.

Table 3.10. Gravity moments for Stanford manipulator

(r is effective lever arm from the jOint
to the robot end point, gi is gravity moment
around the jOint axis; for joints 4 and
iii
5 adopt KDKp/KM = 220 [Nm/rad]) [5]
118

3.48~ Extend the programme written in Exercise 3.39. to include simula-

tion of the servo system if the constant gravity moment is ac-
ting upon the servo system. Apply this programme to simulate res-
ponse of the robot in Fig. 2.6. (data on servo are given in
Example 3.3.4) if the constant gravity moment is acting around
the second jOint and the servo has to drive the joint from ini-
tial state x 2 (0) = (0., 0., O)T into desired position qo2 = 1.573
[radl. Simulate the responses of the servo if the constant gra-
vity moment takes various values from Table -3.9.

3.3.5. Nonlinear effects in local servo system

We have applied the linear model of the actuator and moving part of the
mechanism (3.3.4) for synthesis of local servo system. As we have al-
ready noted, the linear model of the actuator is an approximation in
which certain nonlinear effects are neglected. Here, we shall briefly
consider the effects of these nonlinearities upon the performance of
the servo system,since we have not taken them into account in the syn-
thesis of the servo feedback gains.

(a) Effect of the actuator input amplitude constraint

In the model of actuator and the mechanism (3.3.4), we have denoted

that the amplitude of the input signal for actuator u i is limited, i.e.
the nonlinearity N(u) given by (3.2.9) has to be taken into account.
However, the synthesis of the local servo system has been performed
neglecting this nonlinearity of the amplitude constraint type.

Let us consider the second order model of the actuator and the mecha-
nism and let us assume that the synthesized servo control is in the
form (3.3.41). If we apply the nonlinearity of the amplitude constraint
type upon this control, we obtain:

(3.3.61)

Based on (3.3.61) we can define the region in the state space xi

(qi, qi)T for which the control signal does not reach the amplitude
constraint (the upper bound of the control signal). This region in the
state space is presented in Fig. 3.17. For the points within the regi-
on the nonlinearities (3.3.61) are satisfied, which means that the
 119

input signal does not reach upper bound of its amplitude. Thus, for all
states within this region the amplitude constraint upon the actuator
input has no effects, and the servo system behaves as it has been des-
cribed in the previous sections.

However, for the states out of this region the nonlinearities (3.3.61)
are not satisfied, which means that the computed control requires grea-
ter signal at the input of the actuator than it is actually allowed for
the particular actuator. Thus, the performance of the servo system for

Fig. 3.17. Region of linear behaviour of servo system (for qOi=O.)

these states cannot be linear, as we have assumed in the synthesis of

the servo gains. It has been shown [7] that for the states out of the
region denoted in Fig. 3.17., the closed-loop servo system behaves as
its poles were moved to the right in the complex plane. We may say that
higher is the required control signal relative to the maximum allowed value
u i , the poles of the closed-loop system move more to the right. Theo-
m
retically, if the state were as far from the region denoted in Fig.
3.17. that it would require an infinite control signal, then the poles
of the closed-loop system would coincide with the poles of the open-
-loop system. This means, that for the states out of the region pre-
sented in Fig. 3.17. the stability of the closed-loop servo system is
guaranteed (since the poles of the open-loop system in this particular
case of servo-actuator are in the left half of the complex plane, or
at the imaginary axis). However, we cannot guarantee the desired spe-
ed of the transient process (i.e. the time by which the joint appro-
aches to the desired position qOi). Thus, the region denoted in Fig.
3.17. represents the set of states for which we can expect that the
120

closed-loop servo system will behave as predicted in the synthesis of

servo gains. If the deviation of the system state around the desired
set position qOi are within this region, then the servo system will
ensure desired positioning of the robot jOint. Otherwise, the positio-
ning of the jOint will be slower depending on how far is the initial
position of the joint qi(O) from the imposed desired position qOi.

(b) Effect of static friction

In the actuator and on the shaft of the joint there always appears (to
a certain degree) Coulomb friction. This frictional effect has not
been taken into account in the model (3.3.4). The static friction op-
poses the movement of the joint when it starts to move. Once the joint
is in motion and when it reaches a certain velocity, the static fric-
tion drops to zero and dynamic friction appears which opposes the mo-
tion. The dynamic friction is usually less than static friction. The
Coulomb friction directly affects repeatability of the servo, and,
obviously, repeatability of the robot end point positioning. Due to
this friction the position error of the servo might appear, since the
servo requires an additional signal to overcome the friction moment.
If the robot is well designed and the reducer gears are well fabricated,
then the Coulomb friction might be reduced to minimum and it can be
neglected in the control synthesis. However, if this friction is signi-
ficant, we can include an additional term in the servo-control. We can
add an impulse signal at the actuator input whenever the jOint starts
to move. The servo system with such additional impulse torque for com-
pensation for static friction is presented in Fig. 3.18. The most ap-
propriate way to determine the amplitude of this impulse signal (i.e.
the magnitude of the additional torque) is by experiment.

(c) Effects of dynamic friction

Once the jOint is in motion the dynamic friction appears instead of

the static friction. This friction also can be reduced to a minimum by
a well design of the robot jOint and reducer and by lubrication of
contact surfaces between the moving parts in the robot. However, if
this friction is significant, then it must be compensated for by addi-
tional control signal. We may adopt the following approximative expres-
sion for dynamic friction moment, i.e. we may write that the moment
due to dynamic friction around the joint is given by:
 121

pit dynamic "" Kdq

i·i + POi dynaml'c sgn(q·.i)

Thus, we can compensate for dynamic friction moment by additional

control signal generated as presented in Fig. 3.18. In this case again
the coefficients Kdi (the dynamic friction coefficient) and pi d '
o ynamlc
might be determined experimentally (i.e. by identification at the ac-
tual robotic system). It should be kept in mind that the actual fric-
tion moment is nonlinear function of the joint velocity, but it has
been verified experimentally that using the compensation presented in
Fig. 3.18. satisfactory results can be achieved [8].

' - - - - - - - - - - - - 1 Ki
v
r~
Compensation of CiN i
[., m
static friction ....._ _ _...J

dynamic

Fig. 3.18. Servo system with friction compensation

(d) Effect of backlash

The backlash in the gear reducer and other parts of the robot system
might have a significant effects upon the servo repeatability and ro-
bot end point positioning. For example, the backlash in the gear redu-
cer in Fig. 3.19. directly reflects the error in the positioning of
the robot end pOint. If the effective lever arm from the jOint to the
robot end point is relatively long, then very small backlash in the
gear reducer causes the significant error in the robot positioning and
accuracy of the robot is highly reduced. The repeatability of the ro-
bot positioning is also poor if there are backlash in the robot jOints
122

since it is hard to keep the joint position always at the same side of
the backlash. The backlash can be hardly compensated for by the con-
trol system, since its modelling is very complex and unreliable. Thus,
this effect must be reduced to a minimum by proper design and fabrica-
tion of all "critical parts" in the robotic system. Since the gears
are most critical parts from the standpoint of backlash, their design
and production must be carefully done, if the high accuracy and re-
peatability of the robot are required. As we have already mention,
these reasons motivated the research towards introducing of so-called
direct-drive robots, i.e. robots which actuators are with no reducers.

I _________ _
----------

ACTUATOR

Fig. 3.19. Effect of backlash in gear reducer upon

robot end point positioning

(e) Other nonlinear effects

Besides above listed nonlinear effects in the robot servo system there
appear some other nonlinearities which might reflect the servo per-
formance. Their modelling is usually not simple, but they can often be
neglected relative to the effects which have been taken into account
in the linear model of the servo. In other words, we may assume that
the control synthesized on the basis of the linear model is robust
enough to overcome these nonlinear effects, so they do not reflect
significantly the servo system performance. Still, in the process of the
 123

robot design and the servo system synthesis and implementation, for
each particular robotic system, we must pay attention of these effects
[8] •

We must underline that the linear model of the servo is rough approxi-
mation if electro-hydraulic actuators are considered. Namely, the pa-
rameters of the linear models of the hydraulic actuators are functions
of the actuator state (or, "the work regime" of the actuator). This
problem can be solved if we introduce variable servo gains (depending
"on work regime"), or if we select the fixed servo gains but which make
the servo system robust enough to ensure desired performance of the
servo system regardless of the variation of the actuator parameters.

It should be also noted that if the servo system is to be implemented

by microprocessor, then extra nonlinearities due to truncations in the
microprocessors and AID converters have to be taken into account. If
the analogue information from the sensors is introduced in microcompu-
ter through AID converter, then converter obviously truncates the in-
formation since it gives a finite number of digits at its output. This
truncation also might reflect the accuracy of the servo system and
the repeatability of the robot. Thus, the selection of microprocessor
and converters must be carefully done.

EXAMPLE 3.3.5. Let us assume that the amplitude constraint upon the
input of the actuator u 1 for the servo system synthesized in the
Example 3.3.2. is given by u~ = 27 [V]. In this case the finite region
in the state space for which the inequalities (3.3.61) are satisfied,
is given in Fig. 3.20.

Let us consider some state out of this region (e.g. the state q1 = 0.3
[rad], q1 0.). For this state the control law would require the con-
trol signal of u 1 = 40.539 [V] which is not allowed. Due to this, the
servo behaves as it had the following gains instead the synthesized
servo gains (see Example 3.3.2) :

Ui.Ki
Ki '
P
m P 27
135.13 "" 90 r
V ]
lrad
IKi.qi+Kiqil 40.539

..
P v
ui.Ki
m v 27
Kl.
v IKi i+Ki.i l
P q vq 40.539
43.6 29.04
[ra~/s]
124

Fig. 3.20. Region of linear performance of

servo system in Example 3.3.5.

E X ere 1 s e s

3.49. Consider the third order model of the servo system (3.3.4), the
state vector of which is given by xi = (qi,.i .i)T ' were
q , 1R h .i
1R
is the current of the rotor circuit. If the feedback loops by
all three state coordinates are introduced (i.e. if the control
law is given by u i = _k~xi), if the amplitude of the input sig-
1
nal of the actuator is constraint by (3.3.61),and if the ampli-
tude of the rotor current is constrained by I i~ I < i~ax' then wri-
te the expressions for the boundaries of the finite region in
the state space for which the servo behaves linearly. Draw this
finite region in the 3-D state space.

3.50. For the servo system in Example 3.3.5, draw in the complex plane
the displacement of the poles of the closed-loop servo, if the
state of the system moves along lines qi = 0., 6qi<-0.189, and
qi = 0., 6qi>-O.189 [rad]. (Instruction: calculate the values of
the poles of the closed-loop servo for several values of 6qi sa-
tisfying 6qi<-O.189, and for several values satisfying 6qi>O.198;
for each state first calculate servo gains as presented in Example
3.3.5., and then calculate corresponding poles). To which posi-
tions are approaching these poles if 6qi+oo?
 125

3.51. Moments due to dynamic friction pio d ynamlc

. in the joints of Stanford
manipulator (Fig. 3.13) have been measured experimentally. Their
values are presented in Table 3.11. In table are also given po-
sition gains of individual servos in the robot joints. Determine
the errors in the positioning of the manipulator end point (i.e.
repeatability) which are caused by the dynamic friction in the
joints (in this, assume that the dynamic friction moments act as
external loads upon the servos) [5].

r [m] Dynamic friction CiNiKi/ri [~]

JOINT moment [Nm] M m P R rad

1 0.54 1.91 790.

2 0.50 3.18 1780.
3 - 12.0 27600.
4 0.25 0.565 220.
5 0.25 0.635 220.
6 0.25 0.424 1480.

Table 3.11. Moments due to dynamic friction for Stanfort manipulator

[5] (r is effective level arm from the jOint to the
manipulator end point)

3.52.* For electro-hydraulic servo system considered in Exercise 3.35,

assume that flow/pressure coefficient of the servovalve varies
within the limits Ki = (0.0006, 0.0009), while all the other pa-
c
rameters are fixed, as they are given in Table 3.5.

a) Calculate the position and velocity servo gains to satisfy

requirements given in Exer~ise 3.35 (w o =12. [rad/s], ~i=1),
for the following values Kl = 0.0006, 0.0007, 0.0008, 0.0009
3 2 c
[cm /s/N/m ].

b) If we adopt servo gains calculated for Ki = 0.0007 determine

c
the variation of the poles of the closed-loop servo if Kic
ta-
kes values Ki = 0.0006, 0.0008, 0.0009. Is it acceptable to
c
adopt fixed servo gains, or is it necessary to implement
variable servo gains, as calculated under a)?
126

3.S3. The sketch of the robot is presented in Fig. 3.19. Assume that
the backlash appears at the points A and B. If the geometric pa-
rameters are as follows: d 1 = O.OS [ml, d 2 = 0.60 [ml and r =
2.S [ml, calculate the repeatability of robot end pOint positio-
ning due to backlash of 0.0001 [radlat point A. Next, calculate
the robot repeatability due to the backlash at point B of value
O.OOOOS [radl. Repeat this calculations, but if the diametar of
the larger gear change to d 2 = 0.9 [ml, and the backlash atpoint
A increases to 0.0002 [rad] (the backlash at point B is the same
as in previous case). Comment the results obtained in these two
cases: how the increase of the gear reduction ratio affects the
repeatability of the robot?

3.S4. For the robot in Fig. 3.19. calculate the repeatability of the
robot end point positioning due to truncation of the A/D conver-
ter for the servo in the first joint. The A/D converter is con-
nected to the position sensor. The position sensor is located at
the output shaft of the actuator, before the gear reducer. The
geometric parameters are given in previous exercise. The A/D
converter is of 12 bits (i.e. it truncates all angles bellow
360 0 /4096). Calculate the repeatability for both values of the
gear radius d 2 •

3.4 Synthesis of Local PID Controller

Up to now, we have considered so-called static servo systems for indi-

vidual joints of the robot. We have considered the movement of thei-th
jOint while the other joints are kept locked solid. We have shown that
the servo system ensures positioning of the i-th jOint, but the steady
state error appears which is caused by external load - gravity moment
of the mechanism around the i-th joint. The gravity moment can be com-
pensated by on-line computation of this moment in control microproces-
sor (Fig. 3.1S). This compensation requires computation of the gravity
moment, which might produce problems due to numerical complexity of
the expressions for this moment which depends on the robot structure.
On the other hand, uncertainty in determining the parameters necessa-
ry to compute gravity moment, might cause poor compensation of steady
state errors. The second approach to eliminate the steady state error
 127

due to gravity moment is by introducing of so-called dynamic servo sy-

stems [9].

If we allow the other joints to move simultaneously with the i-th

joint, then an external moment, due to dynamic coupling, will act upon
the i-th jOint (see Chapter 4). This dynamic moment will also cause an
error in positioning of the i-th jOint. The effects of this moment,
Which is cOmpLex function of alL joints coordinates, their velocities
and accelerations, might also be partially compensated by dynamic servo
system.

Here, we shall briefly consider application of dynamic controllers in

control of the robot. In the dynamic servo system the feedback loops
by the integrals of the state coordinates of the system (or, of thesy-
stem outputs) are introduced. We shall consider the simplest form of
the dynamic controller - so-called PID controller in which the feed-
back loop by the integral of the position error is introduced.

Namely, it is possible to introduce integrator which integrates the er-

ror between the actual position and desired (imposed) position of the
jOint, and the computed integral of the position error is added to the
actuator input signal. In this way PID controller is obtained which is
presented in Fig. 3.21. This controller, thus, besides position (P),
velocity or, differential (D), includes so-called integral (I) feed-
back loop [3].

It is obvious that the introduction of the integrator in the control-

ler increases the order of the system model. If the second order model
of the actuator and the moving part of the mechanism is considered
(n i =2), then by applying of the integral feedback loop we obtain the
third order model of the complete system (controller and actuator).
The transfer function of the system (3.3.25) now becomes:

(3.4.1)

i
where KIN denotes gain in the feedback loop by integral of the positi-
on error. Based on (3.4.1) we may determine the position error as:
i S 2 (Ti S 1) 2] oi()
iK V i ()
, - [K D + m + s q s -KMsg l, S
'ql (s) = (3.4.2)
~ i i + i i i i 2+ i + 2
KDKIN KDKps+KDKv S (Tms 1)s
128

91 (load)

Fig. 3.21. PID (proportional - integral - differential) controller

(for the second order model of actuator, KiNdenotes
the gain in the integral feedback loop)

If we assume the step external load acting upon the i-th servo (i.e.
we assume constant load moment in the i-th joint), then we obtain ste-
ady state error as:

~qi(oo) = lim s~qi(s) o (3.4.3)

s~O

Thus, we see that the integral feedback loop eliminates the steady
state error (for step input moment). If all other joints are locked,
then only the gravity moment is acting around the i-th joint when it
stops. This moment in steady state conditions might be assumed to be
constant. Thus, by integral feedback loop we eliminate the positioning
error of the servo caused by gravity moment around the i-th joint. The
PID controller ensures more accurate positioning of the robot joint
than the static controller considered in the previous sections.

Let us consider briefly the problems related to synthesis of PID con-

troller, i.e. let us consider selection of the feedback gains. We may
use various procedures for synthesis of feedback gains: synthesis in
frequency domain (Bode's method), root locus methods, etc. We also may
apply procedures for control synthesis related to state space model of
the system: pole-placement method, optimal regulator approach etc.

Let us consider synthesis of PID controller by pole-placement method,

which we have already considered in Section 3.3.3. The model of the
actuator and the moving part of the mechanism with the introduced
 129

integrator by position error in the state space has the following form
(if we neglect the effects of the external disturbances):

!i
x (3.4.4 )

where Ai is (n i +1)x (n i +1) matrix, bi , ai, are (ni+1)x1 vectors given

by:

(3.4.5)

and xi is (n.+1)x1 augmented state vector of the system, which is given

. iT~ i T i
by x~ = (x , z ) . Here, z is the new state coordinate which repre-
sents the integral of the position error:

t . .
zi(t) = f(q~-qO~)dt (3.4.6)
o

_In (3.4.4) the control signal (input signal for actuator) is denoted
by u i • For the PID controller presented in Fig. 3.21 (n i =2) the control
signal is generated as:

(3.4.7)

where by k i is denoted (ni+1)x1 feedback gain vector which is given by:

k. ~
(3.4.8)

The model of the closed-loop system is obtained by combining (3.4.4)

and (3. 4 . 7) :

-i oi
d q (3.4.9)

The closed-loop system matrix is given by (Ai_bik~). As described in

~

Section 3.3.3, the feedback gains k. have to be selected in such a way

~

that the eigen-values of the closed-loop system matrix are placed in

the desired locations in the s-plane. In the case of the PID control-
ler, since the order of the system is (n i +1), we may specify (n i +1)
eigen-values (if feedback loops by all the state coordinates are im-
plemented). Let us assume that we have specified (n i +1) eigen-values
130

so that all s~ are in the left half of the s-plane. In that case the
J
feedback gains k i can be obtained from:

n i +1
K n
j=1
(s~-s)
J
(3.4.10)

The feedback gains k.l. which satisfy the relation (3.4.10) can be easily
determined.

Here, we have assumed that the feedback loops by all state coordinates
xi are implemented. However, it is not always necessary to introduce
feedback loops by all state coordinates xi. As we have explained in
Section 3.3.3, it is possible to introduce feedback loops just by the
system output Yi which is given by (analogously to (3.3.43)):

(3.4.11 )

where y.l. is k~x1 output vector, and C i is k~x(n.+1) output matrix.

l. l. l.
Therefore, the control is introduced as feedback by the system output:

(3.4.12)

where now k. is a k~x1 vector of feedback gains.

l. l.

If this control is introduced in the model (3.4.4),we obtain the clo-

sed-loop matrix of the system as (Ai_bik:C i ). Now, we may specify k~
l. l.
eigen-values sj of the closed-loop system matrix, while the rest (n i +1)-
-k~ eigen-values are free (for k~<n.+1):
l. l. l.

kY
i
n. +1
n n
l.
K (s~-s). (Sj-s) (3.4.13)
j=1 J
j=k~+1
l.

Based on (3.4.13) we can determine k~ gains in feedback loops by system

l.
output. We can examine the conditions under which the unspecified ei-
gen-values Sj are also in the left part of the s-plane (i.e. under
which they are on the left from the specified eigen-values s~).
J

Thus, the problem of synthesis of PID controller is reduced to speci-

fication of the desired eigen-values sj in the complex plane. The se-
lection of these eigen-values might be performed in various ways. We
may require that the eigen-values of the closed-loop system matrix are
 131

on the left from the line Re(s) = -Si' where Si is positive number. By
this we ensure that the system has sufficiently fast response to step
input (i.e. that the position of the jOint approaches to desired posi-
tion qOi faster than ~ exp(-Sit».

The feedback loop by integral of position error eliminates the steady

state error in the controller response to step ~ravit~moment. However,
if the time variable external moment (disturbance) is acting upon the
system, then this feedback loop by integral of position error cannot
compensate for the error in the positioning of the jOint. Letus assume
that the ramp moment disturbance is acting upon the system, i.e. let
us assume that the external moment Pl.' (t) satisfies dP, (t)/dt=p~=const.
l. l.
In this case the Laplace transform of the external disturbance moment
is given by Pi(s)=p~/s2. If we replace this moment in the expression
for the positioning error in s-domain (3.4.2),we obtain:

(3.4.14 )

Based on this expression we obtain the steady state error as:

(3.4.15)

which means that the steady state error of the system response to ramp
moment disturbance is inversely proportional to the integral feedback
gain.

If we want to reduce the steady state error due to ramp moment distur-
bance (3.4.15), we should select high integral feedback gain K~N. Ho-
wever, to ensure that the pair of the dominant poles of the system is
as close as possible to the real axis of the s-coordinate system, and
to ensure fast response of the system (i.e. to ensure that the real
parts of all the system poles are sufficiently high), we must apply
relatively high position and velocity feedback gains. Since the high
feedback gains reflect the characteristic frequency Wi of the system,
which is constrained by (3.3.37), and since the high feedback gains
are undesirable regarding the effects of the noise upon the system
performance, we must make a tradeoff between these opposite require-
ments. Namely, in specifying of the desired eigen-values of the closed-
-loop system matrix s~ we must take care of the following requirements:
J
132

to ensure low steady state error (3.4.15), sufficient damping of the

system, fast response, but also the condition (3.3.37) must be satis-
fied.

We have explained that if all the joints of the robot are locked, save
for the i-th joint, then just the gravity moment is acting upon the
i-th joint as external disturbance. This moment is constant in steady
state conditions. However, if the other joints are also moving, then
upon the i-th actuator is acting variable moment (load) due to dynamic
interconnections between the joints movements. This dynamic moment is
complex function of actual joints coordinates, velocities and accele-
rations (see Chapter 4). Therefore, we have to ensure that the error
due to variable external moment disturbance is as small as possible.
Obviously, by PID controller we cannot eliminate this error. It is pos-
sible to introduce dynamic controller of higher order, i.e. with feed-
back loops by higher integrals of the position error. However, such
controller is too complex and its response might be too slow, so they
are not often applied in robotics.

EXAMPLE 3.4. In Example 3.3.4 the PD servo system for the second joint
of the robot in Fig. 2.6. has been synthesized. The data on the D.C.
electro-motor are given in Table 3.3. Let us synthesize the PID con-
troller for the same joint and actuator by pole-placement method. If
the second order model of the actuator is considered (n i =2), then we
can specify n i +1=3 eigen-values of the closed-loop system matrix. Let
us select the following places of the eigen-values:

0 0
s1 ,2 -5.45 ± j2.8, s3 -44.6

Based on (3.4.10) we compute the feedback gains as:

K2 V K2 V K2 V
1446. [rad] , 34.36 [rad/s], 4691.2 [rad/s]
P v I

The steady state errors due to constant gravity moment are given in
Table 3.9. for various positions of the second and the third jOint of
the robot if PD servo system is applied. It is clear that if the PID
controller is applied these steady state error are reduced to zero. In
Fig. 3.22. are presented the responses of the PD and PID controllers
to step input qo2=0. 02+q2 (0) [rad] when the gravity moment is acting aro\.lnd
the joint axis. (We assume that the gravity moment is not compensated
by on-line computation as in (3.3.61». From Fig. 3.22. we see that
 133

the PID controller eliminates the steady state error, but the oversho-
ot appears.
q2 [rad]

1.573 ---- - -.--=------- --

/--.,....------------
V
V _ _ pro
___ po
1.563
q3* = 0.7 [ml

1.553L-------~------r_------+_------~--~
0.5 1.5 2 t[sl
Fig. 3.22. Responses of PD and PID controllers to step
input and step moment disturbance

E X ere 1 s e s

7
3.55. If the order of the model of the a tuator.and joint is n i =3, and
if the structure of the matrices Al and b1 is given by (3.3.49)
and (3.4.5), then write the expressions for feedback gains of
the PID controller which ensure placement of the eigen-values of
the closed-loop matrix of the system in desired positions. Assu-
me that the feedback loops are introduced as follows:

a) feedbacks by position, velocity and integral of the position

error; determine the expression for unspecified eigen-value,

b) feedbacks by all the state coordinates of the system (by po-

sition, velocity, current/pressure, and integral of position
error),

c) feedbacks by position and integral of the position error,

only; determine the expressions for the rest two unspecified
eigen-values and determine conditions to guarantee that these
eigen-values are on the left from the specified eigen-values.
1~

3.56. The data on the D.C. electro-motor applied in the second jOint
of the robot in Fig. 2.5. are given in Table 3.3. Determine the
matrices of the model of actuator and jOint for n i =3 (data on
the robot mechanism are given in Table 3.4). Determine the feed-
back gains of the prD controller to satisfy the following condi-
tions: all eigen-values of the closed-loop system matrix should
be on the left from the line Re(s) = -3., the dominant pair of
eigen-values has to be as close as possible to real axis, the
steady state error to ramp moment disturbance (3.4.15) for P~=l
l
[Nm/sl should satisfy l~q2(oo) 1<0.01 [ml. Determine the feedback
gains under assumption:

a) that the feedback loops by all state coordinates are introdu-

ced,

b) that feedback loops by position, velocity and integral of the

position error are introduced.

3.57.* How can be prD controller implemented by microprocessor? Deter-

mine the expression for numerical integration of the position
error by Euler's method for numerical integration. Write the pro-
gramme (using some higher programming language) for computation
of the control signal for prD controller, assuming that the in-
put variables for the programme are: desired position of the
joint gOi, actual values of the jOint position gi and velocity
qi (which are obtained from the sensors through A/D converters),
and the selected feedback gains. Minimize the number of additi-
ons and multiplications required for computation of u i according
to (3.4.7).

3.58. Compare the compensations of the steady state error due to gra-
vity moment: by prD controller and by PD servo system with the
on-line computation of gravity moment. Compare these compensati-
ons from the standpoints of:

a) the number of numerical operations required to compute the

control signal (compare for example (3.3.61) and (3.4.7) ta-
king into account the numerical integration in PID control-
ler),

b) the robustness to variations of the payload parameters,

 135

c) the settling time (i.e. the time required for the joint
to settle at the desired position).

3.59.* Extend the programme for computation of the feedback gains on

the basis of the pole-placement method (required in Exercise
3.38) to include the integral feedback loop. (Instruction: use
expressions obtained in Exercise 3.55).

3.5 Synthesis of Local Servo System for Tnijectory Tracking

Up to now we have considered local servo system for individual joints

of the robot. The control task of these servo systems is to position
the robot joints in the desired positions. The synthesized servo sys-
tem for the i-th joint ensures its positioning under assumption that
all the other joints are locked.

As we have shown in Chapter 1. the tasks which are imposed in modern

robotics require not only the accurate positioning of the robot, but
also the tracking of desired trajectories. This means that the input
for the local servo system is not constant (desired) position qOi of
the corresponding jOint (the step function), but the desired trajecto-
ry qOi(t), i.e. the time variable signal. Since the set input varies
by time, the controller has to ensure that the system output (joint
coordinate qi) varies with time in the same way, i.e. the controller
output qi has to track imposed trajectory qOi(t). In other words, the
task of the controller is to minimize the error between the actual
trajectory of the robot joint qi(t) and the imposed trajectory qOi(t)
at each moment of time.

If the input signal for the servo system synthesized in the previous
sections is time variable function qOi(t) (trajectory), the servo sys-
tem output will not track accurately this trajectory. The error between
the actual trajectory and desired trajectory will appear. Let us assu-
me that the imposed trajectory is ramp function, i.e. that it requires
constant velocity of the jOint qOi(t) = Q~t. The Laplace transform of
. 2 l
this input function is qOl(S) = Q~/s . Let us consider the error of
l
a static servo system whose transfer function in the s-domain is given
by (3.3.25), assuming that the order of the actuator model is n i =2. If
we replace the expression for the ramp function input in (3.3.25), then
we obtain the Laplace transform of posi tion error between the actual
136

joint trajectory and nominal (desired) trajectory as:

(3.5.1)

The steady state position error in tracking of the desired (input)

trajectory is:

lim Stlqi (s) (3.5.2)

s~o

Depending on the velocity Q~ of the input trajectory and on the para-

l
meters of the servo system, the significant error in tracking of the
imposed trajectory might appear.

If we set nominal trajectory which requires more"complex variation"of

the joint position by time, the error in servo system tracking might
be even higher. For instance, if the nominal trajectory requiring con-
stant acceleration of the joint is imposed (qOi(t)=a~t2), then the La-
l
place transform of the position error in tracking of this trajectory
can be obtained as (taking into account that the Laplace transform of
the input trajectory is qOi(s) = a~/s3):

(3.5.3)

The steady state error in this case is (under assumption that the de-
lay due to velocity terms are compensated for - see the text to fol-
low) :

(3.5.4)

In this case the error might be high depending on the required accele-
ration a~ and on the parameters of the servo. It is obvious that these
l
errors in tracking of desired trajectories are consequence of the de-
lays in the servo system, so that the system output cannot follow the
imposed variation of the input signal. However, this delay can be com-
pensated for by introducing of so-called "pre-compensator" (or, feed-
forward term).
 Ki
M

0; ( s)
q + sqi
-;-
Tm s+ 1

Fig. 3.23. Servo system with feedforward term to compensate fo~ velocity and
acceleration variations along nominal trajectory gOl(t)

w
......
138

The feedforward term is introduced in the following way: the input signal
for servo is not desired trajectory qOi(t), but some modified signal
which takes into account the delay in the servo. Namely, the signal
which has to "accelerate" (or, "decelerate") the servo proportionally
to desired acceleration qOi(t), is added to the original input signal
(corresponding to desired trajectory qOi(t)). This additional signal
has to reduce the error between the imposed input trajectory qOi(t)
and actual trajectory qi(t) of the joint (t.e. the servo output).

First, let us consider the introduction of feedforward term in the s-

-domain. It can be easily shown that if the feedforward term is intro-
duced as in Fig. 3.23, then the steady state error in tracking the tra-
jectory qOi (s) =a'?/s3 is reduced to zero. This means that the delay due to
1-
both velocity term and acceleration term is compensated for by the fe-
edforward in Fig. 3.23. Similarly, if the input trajectory is given by
qOi(s) = n'?/s2, the steady state error is also reduced to zero by the
1-
feed forward term added to servo as in Fig. 3.23. Therefore, by the pre-
-compensator we can ensure that the servo tracks the input time-variab-
le trajectory qOi(t).

Let us consider introduction of the feedforward term using the state-

-space model of the servo. Let us consider the model of actuator and
the moving part of the robot mechanism in the form (3.3.4). We have to
ensure tracking of a desired nominal trajectory qOi(t). We consider the
second order model of the system n.=2, so that the state vector xi is
i i.i T 1-
given by x = (q , q ) . We have to ensure that the actual system sta-
te xi tracks the desired trajectory of the state vector xOi(t) =
(qOi(t), qOi(t))T. Therefore, we have to ensure that the system state
xi(t) moves along desired nominal trajectory XOi(t). To ensure this,
we introduce so-called local nominal programmed control U~i(t) which
has to satisfy:

(3.5.5)

The explanation of the physical meaning of the nominal control term is

obvious: this is the programmed input signal (so-called open-loop con-
trol) which ensures that the system state moves along desired nominal
trajectory if the following conditions are met [10]:

(a) if the initial state of the system xi(O) coincides with the nominal
initial state xOi(O), i.e. if xi(O) = xOi(O), and
 139

(b) if the model of the servo (3.3.4) is ideally accurate and no exter-
nal disturbance is acting upon it.

Obviously if xi(O) = xOi(O) and if the model of the system were perfect
(which means that all the parameters of the system were perfectly accu-
ratelyidentifie~, and if the signal U~i(t) computed to satisfy (3.5.5)
is fed to the input of the servo, then the actual state of the system
xi(t) would be driven along nominal trajectory xOi(t). However, it
is obvious that in the general case none of the above assumptions are
satisfied. Due to this, we have to consider the model of the deviation
of the system state xi(t) from the imposed nominal trajectory xOi(t).
Based on the model (3.3.4) and (3.5.5) we can obtain the model of the
state deviation in the following form*):

(3.5.6)

where l\xi (t) xi(t)_xOi(t) is n x1 vector of the deviation of the sy-

o

l 0 0 0 0

stem state from the nominal trajectory xOl(t), and l\ul(t)=Ul(t)_UOl(t)

is scalar deviation of the servo input ui(t) from the nominal program-
med control uOi(t). Now, we have to determine the additional control
signal l\Ui(t). The role of this signal is to reduce the deviation of
the robot state l\Xi(t) from the nominal trajectory, i.e. to ensure that
l\xi(t)+O. In this way, the problem of synthesis of control for tracking
of the nominal trajectory for (3.3.4) reduces to the problem of syn-
thesis of servo for the model of deviation (3.5.6). Actually, the pro-
blem reduces to synthesis of a regulator which has to ensure the sta-
bility of the model of deviation around the equilibrium pOint l\xi=O.
This problem has been already considered in Section 3.3. We have to
synthesize servo which will drive the system state to the desired sta-
te of the model of deviation l\Xi=O. Since the matrices of the model of
deviation (3.5.6) Ai and bi are identical to the matrices of the basic
model of the system (3.3.4), the servo system synthesized in the pre-
vious sections can be directly applied for stabilization of the model
of deviation around the nominal trajectory and the nominal control.
Thus, the problem of trajectory tracking is reduced to synthesis of
positional servo system to which local nominal programmed control is
added. This nominal control signal has to compensate for the variati-
ons of the velocity and accelerations along the imposed (nominal)

*) Here, we have not taken into account the amplitude constraint upon
the servo input.
140

trajectory. Obviously, the control signal is calculated as*):

(3.5.7)

where k i is the feedback gain vector, C i is the kixni output matrix

(see Section 3.3.3). The feedback gains can be synthesized by one of
the previously considered methods: by synthesis in s-domain (Section
3.3.2), or by pole-placement (Section 3.3.3), or by some other method.
The scheme of the control is presented in Fig. 3.24. There are twodif-
ferences between this scheme and the scheme of simple position servo:
the scheme in Fig. 3.24. includes computation of local nominal control
as feed forward signal, and velocity feedback gain k i amplifies the er-
v
ror between the actual velocity of the jOint and nominal velocity of
the joint ~qi(t) = qi(t)_qOi(t), instead to amplify just the actual
velocity signal qi(t) (as in the scheme in Fig. 3.14).

Computation of
nominal control
uOi
L

Fig. 3.24. Scheme of servo system with local nominal

programmed control (the scheme is given
according to the state space model of
actuator)

computation of the local nominal control which satisfies (3.5.6) is

very simple. Let us assume that the model of the actuator is of the
second order n i =2. Let us assume that the trajectory qOi(t) is given,

*)
Here, the general case is considered when the feedback loops only by
the system output Yi are introducedi if the feedback loops are in-
troduced by all state coordinates X1, then the matrix C. is the
unit matrix. 1
 141

and that by differentation we get variation of the jOint veloci ty qOi (t)
and acceleration qOi(t) along the desired trajectory. By this the no-
minal trajectory of the state xOi(t) and the first derivative of the
state xOi(t) along the nominal trajectory are defined. Assuming that
the matrices Ai and £i are in the form (3.3.54), on the basis of
(3.5.6) we can compute the local nominal control as:

(3.5.8)

where bY.X~i is ~enote~ the second coordinate of the state vector (i.e.
oi ·01 ·01 -01
x2 q and x 2 = q ). Thus, the nominal control can be easily com-
puted based on (3.5.8) and this is applied in the control scheme in
Fig. 3.24. In this, it should be kept in mind that the actuator input
amplitude is constrained. Based on (3.2.9), the following inequality
has to be satisfied:

l uOi(t)
L I -<
i
um (3 •5• 9)

If the condition (3.5.9) is satisfied, and if the two above listed con-
ditions were also met, then the nominal control (3.5.8) would drive the
the joint along the desired (nominal) trajectory qOi(t). However, if
the condition (3.5.9) is not satisfied, the nominal control cannot dri-
ve the system along desired trajectory. This means that the actuator
cannot realize the desired nominal trajectory. The required velocity or
acceleration of the jOint are too high, and they cannot be realized by
the applied actuator in the robot joint. The desired trajectory should
be slow down. In the process of the robot design, the selection of the
actuators must be properly done. This means, that the actuators in the
robot joints must be selected to ensure realizations of desired velo-
cities and accelerations of all the robot joints (so that the desired
speed of the robot is guaranteed). Namely, the robot designer must es-
timate the velocities and accelerations of the robot jOints that will
be required in the tasks for which the robot is intended. When he se-
lects actuators, he must check the condition (3.5.9) for each jOint and
for some representative (test) trajectories. If the condition (3.5.9)
is not satisfied, then the actuator in the corresponding robot jOint
must be re-selected. Otherwise, the robot will not be able to accomplish
the desired task*).
*)
It should be notices that this consideration concerns the case when
each joint of the robot is moved independently, i.e. when the robot
joints are moved successively, one by one. If the simultaneous moti-
on of all robot joints are required, then the dynamic moments due to co-
upling between the jOints must be taken into account (see Chapter 4).
142

Analogously to (3.5.9), the nominal programmed control can be determi-

ned also in the case when the model of the actuator is of the third
order n i =3 (see Exercise 3.60).

Since this programmed control takes into account only dynamics of the
local actuator and single joint, it is called local nominal control
[10, 111. It differs from the so-called centralized nominal control
which takes into account dynamics of the complete robot mechanism and
which is computed on the basis of the complete model of the robot dy-
namics when all the jOints are moving simultaneously (see Chapter 4).

The model (3.3.4) takes into account the moment of inertia of the me-
chanism around the i-th joint (if all the other joints are kept loc-
ked). The value of this moment of inertia varies if the other joints
change their positions. We have shown (Section 3.3.4) that for thesyn-
thesis of the servo feedback gains it is required to consider the ma-
ximum value of the moment of inertia of the mechanism around the i-th
joint. However, for the synthesis of the local nominal control we must
consider the minimum possible value of the moment of inertia of the
mechanism around the corresponding joint. If we calculate local nomi-
nal control taking into account (in (3.5.8» some greater moment of
inertia, then when the value of the moment of inertia is less, the jOint
angle would overshoot the desired trajectory. Since the overshoot of
the desired input trajectory must not be allowed in any case, this me-
ans that we must calculate local nominal control taking into account
the minimum possible value of the moment of inertia of the mechanism
around the i-th joint (see Exercise 3.62).

EXAMPLE 3.5. For the first joint of the robot in Fig. 3.2. the model
of the actuator and the moment of inertia of the mechanism around the
jOint axis, for q2*=0., is given by (3.3.4) and the elements of the mat-
rices Ai and bi are given by (see Exercise 3.18):

The corresponding transfer function in s-domain is given by (3.3.27).

We want to synthesize the servo which has to ensure tracking of nomi-
nal trajectory qOl (t) given by:
 143

°t2
_a___ 0 < t < -
T
2' - 2
01 (t) - { (3.5.10)
q - 02 0 0
~ _~(t_~)2+~(t_~) "2
T
< t < T
8 2 2 2 2'

This trajectory is presented in Fig. 3.25 for a O

[rad]

0.5
0.4

0.3

0.2

0.1

o t[s]

Fig. 3.25. Nominal trajectory for the first joint

of robot in Fig. 3.2.

Compensation of the error in tracking of the trajectory due to varia-

tion of the velocity and acceleration can be realized by control sche-
me given in Fig. 3.23, or by scheme in Fig. 3.24. In this case the lo-
cal nominal control is given by:

.{
(ao+0.38aOt)/0.27, 0 < t 2- ~
2
u 01 (t) (3.5.11)
T
[-a o +0.38ao(T-t)]/0.27, < t < T
"2

while the servo gains can be selected as in Example 3.3.2. (i.e. by

(3.3.40)). This programmed control compensates for the variation of
the velocity and acceleration along the nominal trajectory, and so the
accurate tracking of this trajectory is ensured. In Fig. 3.26. the re-
sults of the simulation by digital computer of the tracking of nominal
trajectory (3.5.10) are given. Two cases are presented: if the nominal
programmed control is not introduced and if the local nominal control
(3.5.11) is included in the control scheme. It can be seen that the
tracking in the latter case is much better, since in the former case
144

the servo does not compensate for the variation of velocity and acce-
leration along the nominal trajectory. We assume that the initial error
between the actual position of the joint ql (0) and the nominal positi-
on qOl (0) is liql (0) = ql (0) - qOl (0) = 0.2 [radl. The Fig. 3.26. shows
how this error liql (t) decreases along the trajectory in the two above
mentioned cases. The digital simulation of the dynamics of the single
jOint and complete robot will be addressed in Section 5.3.

ql
[radl

-0.1 with nominal

local control
wi thout nomi na 1
local control
-0.2

Fig. 3.26. Tracking of the nominal trajectory with and without

nominal programmed control for the first joint of
robot in Fig. 3.2.

E X ere 1 s e s

3.60. For the third order model of the servo (n i =3) in the form (3.3.4)
where the matrices of system are in the form (3.3.49), show that
the local nominal control is calculated according to the follow-
ing equations (assuming that qOi(t), qOi(t), qOi(t) are given
i i·i.i T iii T
and that x = (q, q , l.R) = (xl' x 2 ' x 3 ) ):

Draw the corresponding control scheme (analogous to the scheme

in Fig. 3.23. - lit is short sampling interval).
 145

3.61. For the stanford manipulator shown in Fig. 3.13. the data on
which are given in Table 3.7, the servo feedback gains (for the
static servo systems) for all joints have been computed in Exer-
cise 3.33. Determine on the basis of (3.5.2) and (3.5.4), the
steady state errors for:

a) the input trajectory given by qOi(t) 0.2 t (in the s-domain

qOi(s) = 0.2/s2),

b) the input trajectory given by qOi(t) = 4. t 2 (in the s-domain

qOi(s) = 4./s 3 ), and determine the errors in positioning of
the manipulator tip point due to these steady state errors,
if the effective lever arms r between the joints and the ro-
bot tip are given in Table 3.11. [5l.

3.62. The nominal programmed control for the first jOint of the robot
in Fig. 3.2. is given by (3.5.11) for the nominal trajectory
(3.5.10). The matrices of the servo model have been computed for
the positicn of the second joint q2*=0. Calculate these matrices
for the position of the second joint q2* = 1.573 [radl. Show that
if the nominal control (3.5.11) is applied in the first joint
when the second jOint is in the position q2* 1.573 [radl, than
there can occur overshoot (assume that initial error is zero
~ql (0) = 0.). Determine the nominal programmed control using the
value of the moment of inertia of the mechanism which corresponds
to the position of the second jOint q2* = 1.573 [radl. Which no-
minal control has to be applied if the position of the second
jOint q2* is not known?

3.63.* Write the programme (in some high programming language) for compu-
tation of nominal local programmed control for the case when the
model of the servo is of the second order (3.5.8) and when the
third order model of the servo is considered (see Exercise 3.60).
Minimize the number of additions and multiplications for these
computations. Next,introcucc the check of the condition (3.5.9).
By combining this programme with the programme written in Exer-
cise 3.57 write the programme for microprocessor implementation
of the servo which ensures tracking of the desired nominal tra-
jectory (the inputs for this programme are nominal trajectory
qOi(t), actual jOint position gi (t), veloci ty qi(t), and rotor current
i~(t), and the parameters of the actuator, the moment of inertia
146

of the mechanism around the jOint axis Hii' the servo gains, and
the output of the programme is the computed input signal for the
actuator ui(t».

3.64.* Extend the programme written in Exercise 3.39. for simulation of

the servo system, to include the simulation of tracking of the
input nominal trajectory with and without local nominal control.
Assume that the desired nominal trajectory is given by (3.5.10).
Check the results of simulations in Example 3.5. for the first
joint of the robot in Fig. 3.2.

References

[1] Vukobratovic M., ApDlied Dynamics of ManiDulation Robots: Model-

ling, Analysis and ExamDles, SDringer-Verlag, 1989.

[2] Vukobratovic M., Potkonj ak V., Dynamics of Manipulation Robots:

Theory and Application, Monograph, Series: Scientific Fundamentals
of Robotics 1, Springer-Verlag, 1982.

[3] Chestnut H., Mayer R.W., Servomechanisms and Regulation System De-
sign, John Wiley and Sons, Inc., New York, 1963.

[4] Cadzow A.J., l~artens R.H., Discrete-Time and Computer Control Sys-
tems, Prentice-Hall, Englewood Cliffs, New Jersey, 1970.

[5] Paul P., Robot Manipulators: Mathematics, Programming, and Control,

The MIT Press, 1981.

[6] Chen C.T., Introduction to Linear System Theory, Hort, Rinehard,

and Winston, New York, 1970.

[7] Stojic M.R., Lazarevic S., "Stability Properties of a Regulator

System with Control Upper Magnitude Constraints", Proc. Third
International Symposium on Network Theory, Split, 1975.

[8] Kubo T., Anwar G., Tomizuka N., "Application of Nonlinear Friction
ComDensation to Robot Arm Control", Proc. of IEEE Conference on
Robotics and Automation, 722-277, 1986.

[9] Johnson D.C., "A,ccomodation of External Disturbances in Linear Re-

gulator and Servomechanism Problems", IEEE Trans. AC-16, 552-554,
1971 .

[10] Vukobratovic' M., Stokic D., Kircanski N., Non-Adaptive and Adaptive
Control of Manipulation Robots, Monograph Series: Scientific Fun-
damentals of Robotics 5, Springer-Verlag, 1985.

[11] Vukobratovic M., (Ed.), Introduction to Robotics, Springer-Verlag,

1988.
 147

Appendix 3.A
Local Optimal Regulator

In Chapter 3. we have considered synthesis of local servo systems for

individual robot joints (and actuators) using the transfer function of
the system and by pole-placement method (using the state space model of
the system). Other methods for synthesis of simple linear local servo
system for control of robot joints will not be considered in this book.
Here, we shall briefly present synthesis of local controller by mini-
mization of standard quadratic criterion, i.e. we shall consider syn-
thesis of local optimal controller for one single joint and actuatorof
the robot (when all the other joints are locked). The model of the con-
sidered system is again given by (3.3.4). We shall consider synthesis
of optimal regulator for positioning of the individual joint in the
desired position qOi

In the servo system synthesis, considered in Chapter 3, the designerof

the control system has to specify the desired locations of the closed-
-loop system poles in the s-plane, and by this he speciffies desired
response of the system to some typical input signals (e.g. to step in-
put signal and so on). In doing this, the designer does not take into
account the magnitudes of the input signals for actuators that will be
generated by the controller, nor the energy required by actuators to
realize desired positioning of the joint (or, tracking of desired no-
minal trajectory). To satisfy requirements regarding the system res-
ponse and, at the same time, to minimize the energy consumption, it is
demanded to synthesize control by minimization of some criterion. This
criterion should include both desired response of the system and requ-
irements regarding magnitudes of the actuator input signals. Various
criteria for control synthesis have been established. One of thesecri-
teria is so-called standard quadratic criterion which includes both
above mentioned requirements. The standard quadratic criterion is de-
fined in the following form (assuming that the model of the system is
in the form (3. 3 .4)) [1, 2, 3]:

(3.A.l )
148

where ~xi is n .• 1 vector of the state deviation from the desired nomi-
nal state x Oi ~xOi is defined as x Oi = (qOi, 0, O)T for n.=3, and x oi =
. T 1
(qOl, 0) , for n.=2, since the control problem is to ensure positioning
1 . . . ,
of the joint in the desired position qOl) , ~Xl=Xl_X01, Q. is n.xn. po-
111
sitive definite matrix, ri>O is positive number. For the system descri-
bed by the state space model (3.3.4), we have to synthesize control law
which will minimize the criterion (3 .A.1) • The minimization of the first
member in the criterion ~xiQ.~xi represents requirement to minimize the
1 .
error between the actual state of the system Xl and the desired (nomi-
nal) state xOi. By this, it is required that the control law ensures
desired performance of the system, i.e. to ensure that the joint is
driven to desired position qOi. The second member in the criterion
uir i u i represents minimiza.tion of the control signal amplitudes, by
which it is (indirectly) taken into account energy consumption of the
actuator during the positioning of the joint.

Therefore, the following problem is set: to synthesize the control of

the linear system (actuator+joint), the model of which is given by
(3.3.4), in such a way as to minimize criterion (3.A.1). This optimi-
zation problem can be solved analytically. Here, we shall not present
the proof of the solution of the standard quadratic regulator, since
it can be found in literature [1, 2, 31. It has been shown that the
control which minimizes criterion (3.A.1) is obtained in the form which
is linear by deviation of state ~xi:

(3 .A. 2)

Here Ki denotes nixn i positive definite matrix which is the solution

of the matrix algebraic equation of Ricatti type:

(3.A.3)

Therefore, the control law is obtained as a linear feedback with res-

pect to deviation of the system state from the nominal (imposed) state
x Oi The feedback gains are given by:

-1-iT
k~1 r i b Ki (3.A.4)

where k i is n i x 1 vector of feedback gains.

The control scheme practically represents servo system with feedback

 149

loops by all state coordinates. The scheme is identical to the one

presented in Fig. 3.14. (or, to the one in Fig. 3.9), the only diffe-
rence being the algorithm by which the feedback gains are determined.
In the schemes in Figs. 3.9. and 3.14, the feedback gains are obtai-
ned by selection of the positions of the poles of the closed-loop sys-
tem in the s-plane. In the case of the optimal quadratic regulator the
feedback gains are computed by minimization of the quadratic criterion
(3.A.1), which reduces to solving of the equation (3.A.3).

However, in the synthesis of the optimal regulator, the problem of se-

lection of the weighting matrices Qi and r i arises. It is obvious that
the selection of these matrices represents the tradeoff between the
requirements concerning the system response (i.e. the speed by which
the desired nominal state is reached), and the requirements which con-
cern the energy consumption, i.e. the amplitudes of the actuator input
signals. If r i is low relative to the members of the matrix Qij then
this means that the fast positioning of the joint is required regard-
less to energy consumption, i.e. regardless how high amplitudes of the
actuator input signals are demanded. Otherwise, if r i is high
relative to the members of Qi , then too high actuator input signals u i
are not allowed, but this might reflect the time required for joint po-
sitioning in the desired position qOi. The relation between the selec-
tion of the weighting matrices and the system performance might be
complex, in general case.

It can be shown, that the poles of the closed-loop system (with the
optimal quadratic regulator) satisfy the following inequality [2, 3]:

< - (3 .A. 5)

Ai -l AiT
where Wi is nixn i matrix given by Wi = Qi+Ki b r i b Ki , Am denotes the
minimal eigen-value of the corresponding matrix, and AM denotes the
maximum eigen-value of the corresponding matrix, A( ••• ) denotes eigen-
-values of the matrix in the brackets. Based on (3.A.S), we can get an
estimate of posi tions of the poles of the system if the optimal quadra-
tic regulator is applied.

To ensure desired performance of the system, when the optimal regula-

tor is applied (i.e. to ensure sufficiently fast response of the sys-
tem) instead criterion (3.A.1) we may consider criterion
150

(3.A.G)

where f\ is desired stability degree of the closed-loop system (Si>O

is positive real number). By minimization of criterion (3.A.G) we ob-
tain the standard quadratic regulator with prescribed stability degree.
It can be shown that the control of the system (3.3.4) which minimizes
(3.A.G) is again obtained in the form (3.A.2), but, now, Ki is nixn i
positive definite matrix which is the solution of the matrix equation
[2, 31:

(3.A.7)

where Ini is the nixni unit matrix. It can be also shown that, in this
case, the poles of the closed-loop system satisfy the following ine-
quality:

(3 .A. 8)

i.e. the poles are on the left side from the line Re(s) = -Si (see Fig.
3.A.1) in the s-plane. Therefore, by the selection of Si (the prescri-
bed stability degree) we can specify posi tions of the poles of closed-loop
system, i.e. we can ensure that the joint approaches the desired posi-
tion qOi faster than 'Vexp (- Sit). By this regulator we ensure the stabi-
lity degree of the closed-loop system, and the speed of approaching to
the desired system state is guaranteed.

Obviously, the optimal regulator requires feedback loops by all thesta-

te coordinates xi. If all the state coordinates are not measurable
(i.e. if we do not want to introduce feedback loop by current
(pressure), or by velocity), then we may apply so-called output regu-
lator [31, or we may apply observer to reconstruct the system state on
the basis of measured outputs [41. In the above discussion, we have
restricted ourselves to synthesis of optimal regulator for positioning
of the single jOint of the robot (when all the other joints are locked). Ho-
wever, the optimal regulator can be synthesized to ensure tracking of
the desired trajectory [1-31. These problems will not be considered in
this book.
 151

Im(s)

o
Re( s)

Fig. 3.A.1. Region in the s-plane which is on the left

from the line Re(s) -Si

EXAMPLE 3.A.1. For the first joint of the robot in Fig. 3.2, a D.C. elec-
tro-motor is applied, data on which are given in Table 3.1. If we take
into account the moment of inertia of the robot mechanism, the matri-
ces of the model of the actuator and the joint (3.3.7) have been cal-
culated in Example 3.3. For this system we have to synthesize the opti-
mal linear regulator. If we select the following weighting matrices:

1.0 O.
O. ]
Qi = [ O. 0.1 O. 0.1, 3.
O. O. 0.01

then the feedback gains are obtained according to (3.A.4):

k. = (129.B, 43.314, 0.057)T

1

The poles of the closed-loop system are

s = -5.92, s2 = -5.72, s3 = -70B.B

and obviously, they satisfy condition (3.A.B).

References

[1] Kalman R.E., "On the General Theory of Control Systems", Proc. of
IFAC, Butterworth Scientific Publications, London, 1960.

[2] Athans M., Falb L.D., Optimal Control, UcGraw-Hill, New York, 1966.,

[3] Anderson O.D.B., Moore J.B., Linear Optimal Control, Englewood

Cliffs, New Jersey, Prentice-Hall, 1971.

[4] Luenberger D.G., "An Introduction to Observers", IEEE Transactions

of Automatic Control, Vol. AC-16, No.6, December 1971
Chapter 4
Control of Simultaneous Motions of Robot Joints

4.1 Introduction

In the previous chapter we have considered the synthesis of local servo

system for one single jOint of the robot. We have assumed that only the
considered joint can move, while all the other joints of the robot are
kept locked. The robots of the first generation often have implemented
such a solution of the control system. In this case the joints of the
robot are moving successively one by one until the goal position of the
robot hand is reached. During the movement of one joint (until it re-
aches its desired position) the rest of the joints are kept locked
(i.e. they are fixed). The local servos, synthesized in Chapter 3, sa-
tisfy such robotic tasks which require the successive movements of the
robot jOints.

However, such solution of the control system is not satisfactory for

the tasks which are imposed to modern robotic systems. The reasons for
this are numerous. Consider the task requiring only positioning of ro-
bot hand in various positions in work space (assume that tracking of
the nominal traj ectory in space is not required). The successive movements
of the robot jOints are obviously slow. The time required by the robot
hand to reach its goal position is equal to sum of the time periods
required for each single joint to reach its goal position. If we rea-
lize simultaneous motion of all jOints towards their goal positions
(corresponding to the desired position of the robot hand), then the
time required to reach desired position of the hand is equal to the
time period necessary to position the jOint for which duration of mo-
vement, from the initial to the goal position, is the longest one. It is
obvious that in the latter case the positioning of the robot hand is
considerably faster. This fact is very important regarding the appli-
cation of robots in industry, where it is often required to execute
each task as fast as possible. On the other hand, many robotic tasks
require that the robot hand tracks some desired specified path in the
working space. Also it is often required that the robot hand keeps
certain orientation in space (or, changes the orientation in some de-
sired manner) during the robot motion. Such tasks cannot be realized
 153

by successive movements of the robot joints. These tasks requires

simultaneous movements of all jOints of the robot.

If the joints of the robot are moving simultaneously, their movements

are dynamically coupled: the movement of each joint affects the move-
ments of the other joints. The load moment acting upon each jOint is
variable and depends on the movements of all joints of the robot. The
following problem arises: whether the local servo systems synthesized
independently for each "isolated"joint, are capable to ensure simulta-
neous movements of all joints, or not? If we assign desired positions at
the inputs of the local servos and allow the jOints to move simultane-
ously, would they reach the goal positions in desired way (without
overshoots, with minimal steady-state errors, etc.)? Similarly, if we
set desired trajectories of the joints at the inputs of local servos,
would the servo systems ensure satisfactory tracking of these trajec-
tories assuming that the joints are moving simultaneously? In other
words, the question arises whether the coupling between the jOints can
"destabilize" the local servos and disturbs the desired movements of
the robot joints to such extend that the performance of the robot be-
comes unacceptable? To answer these questions it is necessary to ana-
lyze dynamic interconnections between the joints and to analyze the
stability of the robotic system. In this chapter we shall consider the
performances of the robotic system if all the jOints are moving simul-
taneously and if only the local servo systems are applied to control
the robot joints. On the basis of such analysis we shall determine
whether we can accept the local servo systems (which control each joint
independently), or we must introduce additional feedback loops to com-
pensate for the effects of coupling between the joints, in order to en-
sure simultaneous movements of all robot joints.

4.2 Coupliug Between Joints

Let us consider simultaneous positioning of several joints of robotic

system. Performance of the robot is described by the mathematical model
of the dynamiCS of the mechanism (3.2.2) and the mathematical models
of the actuators (3.2.8), or by mathematical model of the entire sys-
tem (3.2.27). The movement of a single jOint (if all the other jOints
of the robot are locked) is described by the model (3.3.4), which in-
cludes moment of inertia of the mechanism around the axis of the con-
sidered jOint. Let us assume that joints are controlled by local servo
systems. These local servos have been synthesized assuming that only
154

the corresponding joint is moving while the other joints are locked.
The scheme of the control system of the entire robotic system (if only
local servo systems are applied) is presented in Fig. 4.1. When the
other jOints start to move the dynamic forces and moments, described
by (3.2.2), appear. These dynamic forces act as external load upon the
servo systems around the robot joints. Let us consider how these for-
ces affect the performances of local servo systems.

(a) Variable moment of inertia. We have already shown (Sect. 3.3.4)

that the moment of inertia of the robot mechanism around the axis
of the i-th jOint varies if the other joints change their angles
(or linear displacements). This means ~hat Hii in the model (3.2.2)
is function of all joint coordinates qJ for j>i. We have already
shown that in order to prevent overshoots in the servo system, we
must determine the maximal value of the moment of inertia of the
mechanism Hii(q). Then, for this maximum value of the moment of
inertia we have to determine the feedback gains so as to ensure
the servo system to be critically damped. If this procedure is
followed, then when the jOints coordinates change their values and
the moment of inertia becomes Hii <maxH ii (g), it is ensured that the
q
servo system around the i-th joint is overcritically damped (and,
thus, the overshoots cannot appear). However, if all the joints of
the mechanism are moving simultaneously, during the movement of
the i-th joint, the moment of inertia of the mechanism Hii around
this jOint is varying, which might cause an uneven behaviour of
the servo system. The local servo is always critically or overcri-
tically damped (i.e. the overshoots of the set goal position can-
not appear), but still the performance of the servo might be un-
satisfactory. The compensation for the variable moment of inertia
might be achieved by variable feedback gains, or by global control
which will be considered in Chapter 5.

(b) Cross-inertia members. If we consider the matrix of inertia H(g) ,

we can see that besides the diagonal elements Hii(q) (which varia-
tion we have considered above) there are also off-diagonal ele-
ments Hij(q) which represent so-called cross-inertia members. The-
se members represent the effects of accelerations of the other
.. j
joints upon the load moment around the i-th joint Hij(q)q . With
some robot structures the values of these members might be close
to the values of the eigen-inertia terms Hii (i.e. Hij~Hii)' and
therefore their effects might be significant. This inertia coupling
'"'J
.....
lQ

..".

(J
o
:oJ
rT
Ii
o
t-' q1
q·1
til
o
::r P,
~
(i)

o KP- MECHANICAL PART OF HOBOT

o
:oJ
til
..... P = Hq + h
til
rT
.....
:oJ Pn
lQ

o
t-h

t-'
o qn
o q·n
PI
t-'
til
(i)
Ii
<:
o
til
'<:
til
rT
~til Mn
ACTUATOR n
L _____ _ _ _ .-J
01
01
156

between the robot joints represents external load upon the i-th
joint. vlhen the j-th jOint is accelerated or deceleration the
inertia coupling causes the external load upon the servo in
the i-th jOint (the moment is given by H .. (q)qj). As we have alre-
1J
ady explained, the external load upon the shaft of the actuator
(D.C. motor) causes errors in the positioning of the jOint, or in
the tracking of trajectories. However, these moments are signifi-
cant if the accelerations are relatively high. When the accelera-
tions drop to zero (when the robot stops), these moments also va-
nish and they do not affect the positioning of the joints, i.e.
they do not cause steady state errors in robot positioning. These
moments can cause errors in tracking trajectories, if high accele-
rations qj are demanded. If accurat~ tracking of fact trajectories
is essential for implementation of a given robotic task, then the-
se moments have to be compensated for. As we have shown in previ-
ous chapter the constant external load can be compensated in vari-
ous ways. However, these moments due to cross-inertia interconnec-
tions are variable and they depend on the positions of the robot
joints qj and on the accelerations qj of the joints. Therefore
these moments cannot be efficiently compensated by integral feed-
back loop. The compensation of these dynamic moments by global
control will be considered in Chapter 5.

(c) Gravity moments. The effects of gravity moments upon the local
servos have already been considered in Section 3.3.4. Here, we
shall only note that in simultaneous movements of several jOints
gravity moments become variable, causing errors both in pOSi-
tioning and in tracking of traj ectories. Thus, the gravity mo-
ments are not any more constant load upon the servo. Therefore,
they may produce, if not compensated by global control, uneven
tracking of desired trajectories, and uneven positioning of the
robot jOints.

(d) Centrifugal and Corio lis forces (torques). The vector h(q, q) in
the model of the mechanism dynamics (3.2.2) includes centrifugal
and Coriolis forces which depend on the joint velocities q. Since
these forces are directly proportional to squares of the jOint
velocities, they are significant only if the joints are moving at
high velocities. When the robot is starting to move or is stopping
these forces are negligible, which means they do not affect the
positioning of the robot in any desired positions and do not cause
 157

steady state errors. However, these forces cause errors in the

tracking of fast trajectories. If the robot joints are moving at
high velocities, these forces act as external loads upon the servo
systems around the jOints. If the precise tracking of fast trajec-
tories is not required the effects of these forces can be ignored.
If the tracking of fast trajectories is essential, we must take
into account centrifugal and Coriolis forces in the synthesis of
control. Since the requirements upon the modern robots in industry
are growing (regarding the accuracy and speed of the desired tra-
jectories and so on), it is often required to compensate for these
dynamic forces.

Example 4.2. Let us consider the robot in Fig. 3.2. For the perVo
around the first Joint of this robo~we have synthesized the servo
gains in Example 3.3.2. assuming that all the other joints of the ma-
nipulator are locked (we have aspumed that the joints are ~ocked in
the positions in which moment of inertia of the mechanism around
the first joint H11 gets its maximal value). Let us cons~d~r the be-
haviour of this servo if all the other joints are ll\OVirg. Let u,!,\ con-
sider the positioning of the first joint. pesired position of the jo-
int is q01=0.5 [rad] and the initial position of the joint is q1 (0)=0.
[rad]. If all the other Joints are locked, then the positioning of the
servo systems is as shown in Fig. 4.2. (bold line). The results are
obtained by digital simulation using the complete robot dynamic model.
If the second joint is moving in such a way that it is accelerated at
q2=1 [rad/s 2 ] (while the third jOint is fixed), then upon the first
joint is acting the moment due to cross-inertia term H12 (q)q2. The po-
sitioning differs from the previous case.
D.6 0.8

0.2 0.4
.. '
1 .0 t [5]
- 0. 1
~

n.
•• L
- 0•2 q
.. 2
1[~]
- D. 3 --- q
52

- 0.4 ......... q2 5[~]

5

- 0•5

l
Fig. 4.2. Positioning of the first joint of the robot in Fig. 3.2.
for various accelerations of the second jOint
158

The results of digital simulation are presented in Fig. 4.2, too (das-
hed line). It can be seen that the effects of cross-inertia term is
relatively weak. However, if the acceleration of the second is extre-
mely high q2=5. [rad/sl, the effects of these forces will be conside-
rably stronger, which can be verified by the simulation of the posi-
tioning of the first jOint (also presented in Fig. 4.2. - dotted line).

E X ere 1 s e s

4.1. For the robot of cylindrical structure, presented in Fig. 2.5,

(the data on which are given in Table 3.4), the servo feedbacK
gains for the third joint have been synthesized in Exercise 3.32.
Determine the "steady state error" due to Coriolis forces when
the first jOint is moving at velocity q1=2 [rad/sl, and the third
joint is moving at velocity q·3 =1 [m/s].

4.2. For the manipulator in Fig. 3.2. and for the servo system in its
first joint, servo gains have been synthesized in Example 3.3.2. De-
termine the steady state error in the first joint (when the se-
cond jOint is in q2=o.) due to inertial coupling between the first
and the second jOint if the second jOint is moving at accelera-
tion of q2=5. [rad/s 2 1. Check whether this result complies with
the results of simulation in Fig. 4.2. Explain the differences in
these results.

4.3.* If the actuator for the first joint of the robot in Fig. 3.2. is
hydraulic actuator, for which the servo has been synthesized in
Exercise 3.35., determine the steady state error due to inertial
coupling between the first and the second joint, if the second
jOint is moving at acceleration of q2=5 [rad/s 2 1. Compare the
obtained results with the results in Exercise 4.2.

4.4.* Hri te the programme (in one of high-level programming language) for
simulation of robot performance. The simulation should include the
complete dynamic model of the robot. Nrite this programme for par-
ticular robot in Fig. 3.2. assuming that the robot joints are
driven by D.C. actuators (data on actuators are given in Table
3.1). Each jOint is controlled by its servo system, and the feed-
back gains are synthesized as presented in the previous chapter.
The input data for the programme are: data on actuators (for
 159

computing the matrices of the models of actuators (3.2.7», data

on robot (for computing matrix H and vector h acc. to (3.2.3»,
data on servo gains (for computation of actuators inputs acc. to
(3.3.41», data on desired positions of the joints qOi, data on
initial state of the robot x(0)=(x 1 (0), x 2 (0), x 3 (0»T, xi(O)=
(qi(O), qi(O», and data on integration interval. Simulate the si-
multaneous positioning of all three joints from the initial state
x(O)=(O, 0, 0, 0, 0, O)T towards desired positions q01=0.2 lrad],
qo2=0.1 [rad] and qo3=0.05 lm]. (Instruction: At each sampling in-
terval for given x(t) compute matrix H and vector h (using pro-
gramme written in Exercise 3.5), compute inputs for actuators acc.
to (3.3.41), then compute the first derivative of the state x(t)
acc. to (3.2.27) and apply some method for numerical integration-
for example simple Euler's method, see Chapter 5. and Exercise 3.39) .

4.3 Analysis of Linearized Model of Robot

In the previous section we have considered the effects of various com-

ponents of the dynamic moments (3.2.2) on the performance of the local
servo systems. It is obvious that the effects of dynamic coupling bet-
ween the jOints can be significant. Therefore, it is necessary to exa-
mine these effects more precisely. Actually, we have to examine wheth-
er the local servo systems can be successfully applied for control of
simultaneous movements of several robot joints (see Fig. 4.1). The co-
upling between robot joints are dynamic moments around joints which
are described by the dynamic model of the mechanism (3.2.2). These dy-
namic forces are in general case, complex functions of angles, veloci-
ties and accelerations of joints. Therefore, the analysis of the ef-
fects of these forces upon the servo system performances is not simple,
specially if the trajectory tracking problem is addressed. To do this
we have to apply various methods for analysis of complex systems which
have been developed within the control system theory.

One of the ways to analyze the performances of complex nonlinear sys-

tems is by their linearization. The nonlinear model is linearized,
and then the linear model is analyzed. The basic motive for such an
approach lies in the fact that many methods for analysis of complex
linear systems have been developed. However, the results of analysis
of linear model cannot guarantee the equal performance of the original
nonlinear system, since the linear model is just an approximation of
160

the actual system*). Namely, if the stability of the nonlinear model

of the system is analyzed by testing the stability of the linear model
of the system, then we obtain just necessary, and not sufficient,condi-
tions for stability of the nonlinear model of the system. The meaning
of this is as follows: if linear model of the system is not stable, then
we can be sure that the nonlinear model also is not stable; however,
if the linear model is stable, then the nonlinear model is not neces-
serily stable. The stability of the linear system guarantees the stabi-
lity of the nonlinear model only in some region around the point, in
the state space, around which the linearization has been performed.
However, the linear analysis is often applied for analysis of the non-
linear systems, due to its simplicity and since i t offers an insight
in the system performance in the close region around the pOint in the
state space around which the nonlinear model has been linearized. Lat-
ter on we shall explain some other reasons which justifies the appli-
cation of the linear analysis for robotic systems (see Chapter 5).

Here, we shall briefly describe how we can obtain the linear model of
the robot, and we shall consider application of some methods for stabi-
lity analysis of the linear systems to the robotic systems. We assume
that just the local servos are applied for control of robot joints [1,
2, 3].

4.3.1. Linear model of robot

Let us consider the entire model of the robot in a so-called centrali-

zed form (3.2.27)

a (x) + 13 (x) N (u) (4.3.1)

where the vector function a(x) and matrix SIx) are given in (3.2.27).
The vector function a(x) and matrix SIx) are complex nonlinear functi-
ons of the system state x. Let us consider the system in the surroun-
dings of the point x O in the state space. Let us assume that the "no-
minal" state x O corresponds to desired positions of the robot jOints
qOi, i.e. X O (x o1 , xo2, ... ,xon)T, x Oi = (qOi, O)T for n i =2. Let us
assume that we can determine the input signals u O=(u 01 , u o2 , •.. ,u on )T

*)
Nonlinear model of the system is also an approximation of the actu-
al system but, obviously, much better approximation, i.e. it is
much closer to the actual system performance.
 161

such that the system is in equilibrium in the point xO, i.e. that it
is satisfied:

o (4.3.2)

o
The model of state deviation around the point x , UO can be written in
the following form, based on (4.3.1) and (4.3.2):

(4.3.3)

where a is Nx1 vector function given by:

Here, denotes Nx1 vector of state deviation from the equilibrium

~x

o
point x , i.e. x(t) = x(t)-xo, ~u is nx1 vector of deviation of the
=

!
input signals from the "nominal" signals uo, i.e. ~u(t) u(t) - uo,
N(uo, ~u) denotes the nonlinearity of the amplitude saturation type
according to (3.2.9) *)

for ~ui<_ui_uoi
-u, -urn m
oi i
N(u oi , ~ui) ~Ul for _ui_uoi<~ui<u i -u oi (4.3.4)
m m
oi i ~ui>u i -u oi
-u +u for
m -m

The model of deviation of the state coordinates from "nominal" point

x o (4.3.3) has the same form as the original model (4.3.1). The model
of deviation is the system of differential nonlinear equations.

We want to linearize the model (4.3.3) in order to obtain linear ap-

proximation of the robotic system which will serve for analysis of the
stability of the entire robotic system. The linear model is obtained
in the form:

(4.3.5)

where AL is NxN matrix and BL is Nxn matrix. Matrices AL and BL are

obtained according to:

*) Here it is assumed that UO satisfies JuoJ<u;.

162

A = aa (x o , llx) I , 0
B(x , 0) (4.3.6)
L a~x for 6X=O'

o
Obviously, the matrices AL and BL depend on x , i.e. on the pOint in
the state space around which the model is linearized, and this we shall
consider latter on. Based on (4.3.3) and (4.3.6) we obtain:

A = ~I aB I (4.3.7)
L allxl llx=O + allx llx=O

If we introduce a and B as defined in (3.2.27) we obtain:

A+F(I -HTF)-1
n

+ F (I -HTF) -1 [~(TAXo+TBUo) +HTA + "a~ 1 (4.3.8)

n ollx ouX

where aH/allx and ah/allx are matrices of adequate dimensions.

To obtain matrices AL and BL of the linear model of the system it is

necessary to determine the linear model of the mechanical part of the
robotic system, i.e. it is necessary to determine the following mat-
rices:

~
aq
- matrix of dimensions nxnxn

(4.3.9)

- both of dimensions nxn

These matrices represent the first derivatives of the matrices H(q)

and h(q, q) by q and q, for q = qO and q = qo.

Thus, the problem how to determine the linear model of the robot dy-
namics reduces to determination of matrices (4.3.9), i.e. to lineari-
zation of the dynamic model of the mechanical part of the robotic sy-
stem. The matrices (4.3.9) can be determined in several ways:

(a) The matrices of the linear model (4.3.9) can be obtained analyti-
cally. This means that we can write the original model of dynamics
of the mechanical part of the robot in analytical form (3.2.2),
and we can analytically determine the first derivatives of the
elements of the matrix H(q) and of the vector h(q, q) by the joint
angles (displacements) q and their (rotational or linear) veloci-
 163

ties q. By this we obtain the matrices (4.3.9). However, since the

elements of matrices H(q) and h(q, g) are complex nonlinear func-
tions on q and q, determination of their derivatives analytically might
be very tedious and complex task (which might be subject to many
errors). This is especially the case with robots with many rota-
tional joints which models are often very complex functions of q
and g. These were the reasons which motivated development of me-
thods for automatic computer-aided determination of the linear mo-
del of mechanical part of the robot by analytical differentiation
of the matrix H and of the vector h [1].

(b) The matrices (4.3.9) might be also determined applying various

numerical procedures, which might be set at a digital computer.
These procedures are completely analogous to numerical procedures
for computation of nonlinear dynamic models of robots (for compu-
tation of matrix H(q) and vector h(q, q)). As we have already ex-
plained, several algorithms have been developed for numerical com-
putation of the matrices Hand h of the mathematical model of dy-
namics of the robot mechanism, and these procedures are based upon
recursive relations between the velocities of the robot links,
between their accelerations, between their forces and moments. Pa-
rallel to these procedures for determination of the matrices Hand
h, the algorithms for determination of matrices (4.3.9) have been
developed. These algorithms are based upon the same principles as
the algorithms for determination of the basic matrices Hand h.
The algorithm based on D'alembert's principle for numerical deter-
mination of the matrices (4.3.9) of the linear model of the dyna-
mics of open kinematic chains, is presented in detail in [1]. This
algorithm set at the digital computer enables computation of numerical
values of the matrices (4.3.9) for given qO, and gO, for arbitrary
type and structure of robot (i.e. for robots with arbitrary number
and types of joints, etc.). In this way, using such program for
digi tal computer, we can simply determine the matrices of the linear
model of the robot, but just numerically (Le. for the given data
on qO and gO we determine the numerical values of the elements of
the matrices (4.3.9)).

(c) The matrices of the linear model of the robot (4.3.9) can be de-
termined by various procedures for identification. We assume that
the nonlinear dynamic model of the robot is available and based
on this model we can numerically identify the linear model which
'64

describes the performance of the actual system. The aim is, obvious-
ly, to determine the linear model whose response to various inputs
is as close as possible to the response of the actual system.

We shall briefly describe a simple procedure for numerical identifica-

tion of the linear model of the robotic system. Let us consider the
"exact" nonlinear model of the state deviation of the robot (4.3.3)
around the point xo. Let us assume that certain signals eu(t) are fed
at the system inputs. For such control signals we may integrate nume-
rically the system of differential equations (4.3.3) and obtain the
trajectories of the state vector exIt) which represent the response of
the robotic system to the given inputs eu(t) (around the nominal point
xo, uO), for some selected initial state extOl:

ox(t) - ° ' ° °
a(x, ex) + B(x, ex)N(u, eu(t)) (4.3.' 0)

Let us assume that we have selected the input signal to satisfy the
amplitude constraint (4.3.4), i.e. such that N(uo, eu(t)) = eu(t). The
input signal might belong to various classes of functions (of time).
Let us observe the response of the nonlinear model (4.3.'0) in m, dis-
crete time instants t i , i=,,2, ... ,m,. The matrices of the linear model
(4.3.5) have to be determined in such a way that they satisfy the fol-
lowing equations:

where [ex] is Nxm, matrix given by [ex] [ex(t,), ex(t 2 ), ..• ,ex(tm,)]
and [ox/eu] is (N+n)xm, matrix given by:

ex (t, ) ex(t ,)
m
[----:---,
eu(t, )
---------]
eu (t ,)
m

Based on (4.3.") it is possible to determine the pair of matrices

AL(Xo), BL(Xo) via generalized inversion of matrix [ex/eu], i.e. using
the following equations:

(4.3.'2)

Here, it is assumed that the matrix in { } is non-singular. In this

way we can determine the matrices AL(Xo), BL(Xo) of the linear model
of the robotic system around the point x O , uO.
 165

The procedure for numerical identification of the linear model (4.3.5)

on the basis of nonlinear model (4.3.3) is simple. We have to simulate
behaviour of the nonlinear model of the state deviation around the point
xO, u O (4.3.5), if the selected input signals Qu(t) are applied. The
values of the state c:oordinates QX (E.) and of the first derivatives of
~

the state coordinates ax(t i ), i=1,2, ••. ,m 1 (i.e. the response of the
nonlinear model) are memorized for m1 time instants during the simula-
tion. In this,we have to select the sufficiently large number m1 • Nex~
we have to compute the matrices AL(X o ), BL(Xo) according to (4.3.12),
using the memorized values of Qx(t.) and ex(t.).
~ ~

In computing the matrices ~(xo), BL(Xo) we have to take large number

of data Qx(t i ) (i.e. to simulate the system performance sufficiently
long and to memorize many "instants" m1 ), in order to cover the sur-
roundings of the point x O and take into account as much information on
the system behaviour as possible. By this we ensure that the response
of the linear model "sufficiently" well approximates the response of
the original nonlinear model in the surroundings of the pOint xO, u o .
Actually, the response of the linear model of the system has to "fit"
the response of the original nonlinear model of the system. However,
if the number of data to be memorized increases, then numerical prob-
lems in solving of the equation (4.3.11) also increase. Namely, nume-
rical errors in matrix inversion and matrix product which are required
in (4.3.12) increase if the dimensions of the matrices increase. Gene-
rally speaking, this procedure for identification of the linear model
of the robotic system is subject to numerous possible numerical errors,
Which might even lead to unreliable and inadequate linear model. It
is necessary to ensure that the input signal Qu(t) is "sufficiently
complex" to excite all significant modes of the robotic system (Le.
its frequency bandwidth must be sufficiently wide). The above pre-
sented procedure is efficient for identification of linear model if
the original nonlinear model (4.3.3) is stable. Otherwise, if we apply
some arbitrary input signal Qu(t), then it might happen that Qx(t)+oo,
and then the identification becomes invalid due to large numerical er-
rors. Therefore, first the system (4.3.3) is stabilized introducing
some feedback loops (for example, simple local servos around the robot
jOints might be applied to "approximately" stabilize entire robot) .
Then, the linear model of the stabilized robot is identified. At last,
we omit from the linear model the introduced feedback loops and by
this we obtain open-loop linear model of the robotic system. However,
often it is not easy to "approximately" stabilize robotic system.
'66

Due to this, it must be underlined that the numerical aspects highly

limit the applicability of this procedure for identification of the
linear model of the robot. Therefore, the more complex and more sophi-
sticated procedures for identification of the linear model of the sys-
tem might be applied. In these procedures the numerical problems due
to operations 'with large amount of data are overcome in various ways.

In the above described identification procedure we have used the non-

linear model of the robot to determine the linear model. However, we
have to notice that the linear model of the robot might be identified
also by recording the responses of the actual robotic system to vari-
ous input signals eu(t), i.e. the linear model might be identified ex-
perimentally. Based on records of the responses of the actual robot
exIt) to various input signals eu,it is possible to identify linear
model which approximatively describes the performance of the robot dy-
namics around the nominal point x O , u O •

Here, we have presented how we can directly identify the linear model
of the entire robotic system which includes the models of robot actua-
tors. Since the models of actuators are linear (or, we may say that
the linear models are good approximation of their actual performances),
we may identify only the matrices of the linear model of the mechani-
cal part of the system (4.3.9). If these matrices are identified,it is
easy to determine the matrices of the linear model of the entire robo-
tic system based on (4.3.8). The identification of the linear model of
the mechanical part of the system can be carried out applying the pro-
cedure described above.

Example 4.3.'. The model of dynamics of the mechanical part of the ro-
botic system presented in Fig. 3.2. is given by differential equations
(3.2.3). Since this model is relatively simple, it is easy to determi-
ne the corresponding matrices (4.3.9):

~
aH

u :J 0
0
0

r '.
:l
* 2
-2m 3 l'"l',3 s l. nq -m3l'"l',3sinq
2
aH *. 2
-m 3 l',1l',3 s l. nq 0
aq2
0 0
 167

aH
aq3 [: 0
0
0 :] (4.3.13 )

l
*·2·2·1 2
0 -m 3 £1£3 q (q +2q )cosq
* ·1 2 2

:1
ah 0 m3 £1£3(q) cosq
3q
0 0

l
*·2. 2 * ·2 '1 2
-2m 3 £1 £3 q sl.nq -2m 3 £1£3(q +q )sinq
3h *·1 . 2
2m 3 £1£3 q sl.nq 0
oq
0 0

If the matrices of the actuators models (D.C. electro-motors) are given

by (3.2.11), then, using the data on the robot and the applied actua-
tors given in Tables 3.1. and 3.2., we compute the matrices of the li-
near model AL , BL around the point in the state space q02=0.5 [radl,
q01=0., q02=0. (the values of other state coordinates might be selec-
ted arbitrary, they have no effects upon these matrices). We compute
these matrices according to (4.3.8):

0 0 0 0 0

0 -0.832 0 2.56 0 0

0 O. 0 0 0
AL
0 2.56 0 -15. 0 0

0 0 0 0 0

0 0 0 0 0 -45.

(4.3.14 )

0 0 0

0.58 -1 .75 0

0 0 0
BL
-1.78 10.43 0

0 0 0

0 0 0.361

This way of determination of the linear model of the robot might be

easily applied in the cases of simple robot structures. However,
168

in the cases of complex robot structures with many degrees of freedom,

this analytic differentiation is very tedious job, so in these cases
it is much more convenient to apply numerical algorithms set at the
digital computers to obtain linear model of such robots. As already
mentioned, the algorithms for automatic computer-aided generation of
linear models of the robot with arbitrary structure in analytical forms
have been developed. Therefore, the matrices (4.3.13) can be generated
automatically by such software packages [4J.

E X ere 1 s e s

4.5. Show that the matrices of the linear model of the robot are obtai-
ned in the form (4.3.8) if we start from the nonlinear model of
robot (3. 2 . 27), ( 4 . 3 . 6) and (4. 3 . 7) .

4.6. For the robot presented in Fig. 2.5., the dynamic model of which
has been determined in Exercise 3.1, determine analytically the
matrices (4.3.9) and the matrices of the linear model (4.3.8), if
D.C. electro-motors are applied (and the models of actuators are
linear and of the second order) .

4.7. If data on the manipulator in Fig. 2.5. are given in Table 3.4.,
determine the values of the matrices (4.3.9) (computed in Exer-
cise 4.6) for the following nominal state coordinates: a) X o
(0, 0, 0, 0, 0, O)T, b) X o = (0, 0, 0, 0, 0.5, O)T, c) X o = (0,
1., 0.2, 0., 0.5, 1.)T. If data on actuators are given in Table
3.3. compute the values of the elements of the matrices (4.3.6)
for these three cases.

4.3.2. Analysis of stability of linear model of robot

with position control

The linear model of the robot, obtained by one of the procedures des-
cribed in the previous section, represents the time-invariant system
of N linear differential equations. To examine the performance of this
system we may apply various well-known methods for analysis of linear
multi-input mUlti-output systems. In this particular case, we shall
address the problem of stability of such system. We want to examine
whether the robotic system is stable around the nominal point XO (i.e.
around the desired goal positions of the robot joints) if we apply only
 169

local controllers and the joints are moving simultaneously. Here,

for the sake of simplicity, under the notion of stability we shall as-
sume asymptotic stability [5].

To examine stability of a complex linear system we may apply various

procedures for stability analysis. One 0f the most often applied is to
determine eigen-values of the matrix of the linear model. If the open-
-loop system is considered, we may analyze the stability of the system
by examining the eigen-values of the matrix AL :

Ka n
j =1
(s'?-s)
J
(4.3.15)

where IN is the NxN unit matrix, Ka is the coefficient of proportiona-

lity, sj j=1,2, ... ,N are the eigen-values of the matrix AL • Byanaly-
sis of the eigen-values s'? we can easily conclude on stability of the
J
open-loop linear model of the system.

Now, let us consider the stability of the closed-loop robotic system

if the local servo systems are applied (Fig. 4.1). In this case the
control law is given by:

(4.3.16)

where K is nxKc matrix of feedback gains, C is the output matrix of

dimensi,?ns KcxN. The matrix of feed~ack gains K includes the position
gains K~, velocity feedback gains K~ and the gains in the feedback lo-
p . v
ops by the robots currents K~ (if such feedback loops are applied) ,
and it is given by:

K1 K' K1
p v I

K2 K2 K2 0
p v I

o
170

It is obvious that the matrix of feedback gains is diagonal and given

by K = diag(ki). The servo feedback gains k i are synthesized as des-
cribed in the previous chapter. The output matrix C is also diagonal
matrix, given by C = diag(C i ). In the case when local feedback loops
are applied by all state coordinates xi, then the output matrix C is
the NxN unit matrix. Here, by Kc is denoted the order of the system
output which is given by

n
I k~
i=l 1

If the control (4.3.16) is applied, i.e., if the local servo feedback

loops are closed, the matrix of the closed-loop linear model of the
system is given by AL - KC. To analyze the stability of the r~bot with
local servo systems around the desired positions of joints qOl, we ha-
ve to determine eigen-values of the closed-loop system matrix:

where Ka is the proportionality coefficient and sj, j=1,2, ... ,n are

the eigen-values of the matrix AL-BLKC.

Next, we have to analyze positions of the eigen-values s~ of the clo-

J
sed-loop system matrix in the complex plane. As it is well known, if
all eigen-values of s~ of the system matrix lie in the left part of
J
the complex plane, then the linear model of the robot is stable. If
any of the eigen-values lies in the right part of the complex plane
(i.e. if any eigen-value has positive real part), then the system is
unstable. Thus, we have to test if

Re(s~) < 0 for j=1,2, ... ,N (4.3.18)

J

However, to ensure satisfactory performance of the robot, i.e. to en-

sure sufficiently fast response of the system, it is required that all
the eigen-values have to lie at the left-hand side of the line Re(s)=
-a, where a is prescribed stability degree of the particular robot.
As it has been already explained in Sect. 3.3.3. this requirement me-
anS that the linear model of the system has all time constants less
than 1./a, i.e. the response of the linear model of the system to the
step inputs (positions) has to approach to the desired positions faster
than ~exp(-at). Therefore, instead to examine the condition (4.3.18)
we have to test condition:
 171

Re(s<:') < -a, for j=1,2, ••• ,N (4.3.19 )

J

If the condition (4.3.19) is satisfied we may be sure that the linear

model of the robotic system approaches the desired state x O faster
than exp(-at) even when all jOints of the robot are moving simultane-
ously towards their goal positions qOi (which correspond to x O) .

However, to ensure satisfactory performance of the entire robotic sys-

tem, it is not only required that the system has sufficiently fast re-
sponse (and short settling time), but it is also necessary to prevent
overshoots of the goal positions (i.e. in the position control it is
required that all jOints do not overshoot their set goal positions).
In Sect. 3.3. we have explained the reasons why the overshoots of
desired positions of the robot jOints are not acceptable. The local
servo systems have been synthesized to prevent the appearance of the
overshoots of the goal positions, if each jOint is moving independent-
ly. However, if simultaneous motions of several joints are permitted the
overshoots in some joints might appear due to dynamic coupling between
the joints motions. Actually, it is quite obvious that the eigen-valu-
es of the linear model of the entire robotic system differs from the
eigen-values of the models of the decoupled subsystems (The decoupled
subsystems are independent joints with their local servo systems). We
have to examine whether the eigen-values of the closed-loop linear mo-
del of the robot (with local servo systems applied) satisfy the condi-
tion:

1m (s<:') o for j=1,2, •.. ,N (4.3.20)

J

Actually, we require that all eigen-values of the linear model of the

robotic system are real. However, this condition is too restrictive.
The system performance is mainly affected by the so-called dominant
eigen-values which are the closest to the imaginary axis (i.e. which
are with the greatest real parts), while the eigen-values which are
far from the imaginary axis in the left half of the complex plane have
considerably less effects (since they are related to shorter time con-
stants of the system). 'rherefore, we can relax the requirement (4.3.20) ,
so that we require that the dominant eigen-values of the linear model
matrix have to be real (or very close to the real axis), while the
other eigen-values (far in the left half of the complex plane) might
be complex. However, for all eigen-values it is generally required
that they should be as close as possible to the real axis.
172

This means that the analysis of the stability of the linear model of
the robot with the closed local servo feedback loops is relatively
simple: we have to determine the eigen-values of the matrix of the
closed-loop linear model, and then,we have to examine whether these
eigen-values satisfy the conditions (4.3.19) and (4.3.20) (this latter
condition might be partially fulfilled, as explained above). If these
conditions are met we may state that the linear model of the robotic
system is exponentially stable around the point x O , u O [3] with expo-
nential stability degree a. We can also state that the linear model
has no overshoots. However, by this the exponential stability of the
entire nonlinear model of the system is not proved, which means that
we cannot guarantee that the actual system will have the satisfactory
performance if several joints are moving simultaneously*). If the con-
dition (4.3.19) is not met (even for a=O.) we may state that the robot
system is not stable. Namely, the stability of linear model of the ro-
bot gives necessary (but not sufficient) conditions for stability of
the nonlinear model of the robotic system. Therefore, if condition
(4.3.19) is not met we may be sure that the synthesized local servo
systems do not ensure satisfactory performance of the robot. In that
case there is no need for further analysis of the stability of the non-
linear model of the robot, but we have to re-select the local control-
lers until we stabilize the linear model of the system. When we deter-
mine the local servos which stabilize the linear model of the system,
we have to examine the stability of the nonlinear model of the system
(see Appendix 4.A).

In the analysis of stability of the linear model of the robot in the

above described way, the main problem lies in determination of eigen-
-values of the system matrix since this matrix might be of relatively
high order. If we consider a robot with n=6 degrees of freedom (6 jo-
ints) and if all its actuators are modelled by simple linear models of
orders n i =3, the entire system is of order n.n i =18. Computation of the
eigen-values of such high order matrix is related to numerical prob-
lems. However, there are numerous algorithms (and corresponding com-

*) It has been shown [6] that the robotic system in general can be
asymptotically stabilized around the desired positions of the
jOints if we close local servo feedback loops by position error and
by velocity. However, the exponential stability of the nonlinear
model of the rObot is not gUaranteed if the linear model is expo-
nentially stable. Also we cannot guarantee that nonlinear model
will have no overshoots. The tracking of input trajectories by lo-
cal servos if several jOints are moving simultaneously must be exa-
mined for each particular robot separately (see Appendix 4.A).
 173

mercially available computer programmes) for computation of the eigen-

-values of the high order matrices. rherefore, the problem of computa-
tion of eigen-values of linear model of robot will not be addressed
here.

We have presented above how we can form the linear model matrices of
the closed-loop robotic system if only local static servo systems are
applied. It is easy to extend this procedure to the case when local
dynamic controllers are applied. It is quite obvious that in that ca-
se the order of the entire model of the system becomes higher (for
example if in n=6 jOints we apply PID controllers, and if we consider
the models of actuators of the orders n i =3, the matrix of the linear
model of the system is of the order nx(ni+1)=24 - see Exercise 4.9).

Up to now we have considered the linear model of the robot around one
point x O in the state space (and around the corresponding nominal con-
trol signals u O which satisfiy (4.3.2». As we can see in equations
(4.3.8), (4.3.9) the values of the elements of the matrices of the
linear model of robot depend on the point x O around which the nonline-
ar model is linearized. (As we already explained the inertia matrix H
depends on the jOints positions qO and the vector h depends on the
joints positions qO and velocities qo). Therefore, in general case,
for various pOints in the state space X O we obtain various linear
models of the system. If we consider eigen-values of the open-loop
linear model matrix, we see that they move in the complex plane as it
is moving the point X O in the state space around which the system is
linearized. If we select the local feedback gains, the matrix of the
closed-loop linear model of the entire robot varies depending on the
point xc, and therefore its eigen-values also vary. According to this
considerations, if we want to examine the performance of the robot
when the local controllers are applied and when the several joints are
moVing simultaneously, we have to examine the linear model of the ro-
bot around. several points in the state space. Theoretically, we should
eXa!l\ine the system in the infinite number of points xO, but practical-
ly this can be redllced to examination several "characteristic" points
in t.h.e state space. It cannot be precisely defined around which and
around. how !l\any points in the state space we have to determine the
linear models of the robot and to analyze the system stability. For
each particular robotic system we have to analyze variations of the
eigen-values of the system depending on the pOint in the state space arollnd.
which the system is linearized. In this way, we have to examine whether
174

the conditions (4.3.19) and (4.3.20) are met for "all" points in the
state space (i.e. for all possible positions and velocities of the ro-
bot joints), and to examine whether the local controllers stabilize
the robotic system in the "entire" state space*). Namely, the goal is
to determine such local controllers which will ensure stability of the
robotic system around "all" points in the state space. Since the line-
ar model of the robot varies as varies the point around which the sys-
tem is considered, to keep the eigen-values of the linear model in
approximately fixed positions in the complex plane (i.e. to ensure
uniform performance of the robot independently on the specified goal
positions of the robot) we should have to apply variable local gains.
Actually, we should have to apply gains which vary in dependence on
the actual positions (and velocities) of the robot joints. However,
the implementation of the variable feedback gains is not simple, so
we prefer to apply such unique local feedback gains, which could ensu-
re that the robot performance is (approximatively) uniform in all po-
ints in the state space. Therefore, the aim is to determine such fe-
edback gains which ensure that the positions of the dominant eigen-
-values in the complex plane of the linear model of the robot move mi-
nimally if the point X O around which the system is linearized is "mo-
ving" in the state space. Selection of such uniform local controllers,
might be realized by various iterative procedures set at a digital
computer (see Chapter 5).

Example 4.3.2. For the robot in Fig. 3.2. the matrices of the open-loop
linear model have been determined in Example 4.3.1. For all three jo-
ints the local static controllers have been synthesized (see Example
3.3.2). The feedback loops by position and velocity of the jOint have
been introduced in local servo. Data on actuators have been given in
Table 3.1. and data on the robot mechanism have been given in Table
3.2. In Table 4.1. the local feedback gains are given and eigen-values
of the local decoupled subsystems (i.e. if each joint with its actua-
tor is considered independently on the rest of the system). Based on
(4.3.17) the eigen-values of the matrix of the linear model of the en-
tire closed-loop robotic system are determined. These eigen-values are
given in Table 4.1, too. The eigen-values of the open-loop linear

"')
It is obvious that the state space of each particular robotic sys-
tem is limited by the kinematic constraints (allowable rotations or
displacements of the robot jOints) and by the actuators capabiliti-
es (in regard to the allowable velocities and accelerations of the
jOints) .
 175

Subsystem Local servo Eigen-values Eigen-values Eigen-values

joint gains of decoupled of open-loop of closed-loop
subsystem 1inear model 1inear model

Kp1=135.13 -6. o. -3.45

1
<= 43.6 -6. o. -14.7

K~=126 -25. o. -22.3

2
K2= 8.65 -25. -0.38 -25.
v

K3p=1674.4 -25. -15.44 -25.

3
K3= 13.44 -25. -44.98 -90.
v

Table 4.1. Local servo gains, eigen-values of decoupled subsystems,

eigen-values of the open-loop and closed-loop linear
model of the robot in Fig. 3.2.

Re(s)=-3
o - decoupled servo systems
• - matrix of the open-loop
linear model of robot
x - matrix of the closed-loop
linear model of robot

Re(s)
-90 -45 -24 -22 -14 -10 -6 -4

Fig. 4.3. Eigen-values of decoupled subsystems and of matrices of

.~open-loop and closed-loop linear model of the robot in
the complex plane

model of the entire robot are also given in Table 4.1. The eigen-va-
lues in all three cases (eigen-values of the local decoupled servos,
eigen-values of the open-loop and closed-loop linear model of the ro-
bot) are presented in Fig. 4.3. in the complex plane. We see that the
176

eigen-values of the closed-loop system matrix are at the left - hand

side of the line Re(s) = -3. which means that the linear model of the
robot is exponentially stable with the stability degree a = 3. Since
all the eigen-values of the closed-loop system matrix are real, this
means that the condition (4.3.20) is also satisfied. However, this
does not guarantee that the nonlinear model of the robot (which is,
obviously, much closer to the actual robotic system) is exponentially
stable and that it has satisfactory performance when several joints
are moving simultaneously.

E X ere t s e s
4.8. For the robot in Fig. 3.2. (which has been considered in Example
3.2, local serVO gains are given in Table 4.1, data on the robot
are given in Tables 3.1. and 3.2.) write the matrix of the linear
model with the closed local servo loops and check the eigen-valu-
es given in Table 4.1.

4.9. Starting from the matrices of the open-loop linear model (4.3.8),
write the expressions for the matrices of system with the closed
local servo loops, if in all n joints PID controllers are applied
(i.e. if local feedback loops by position error, velocity and in-
tegral of position error are introduced around each jOint). For
the robot in Fig. 3.2. (for which the matrices of the open-loop
linear model are given by (4.3.14)) write the expression for the
matrix of the linear model if in all three jOints are applied PID
controllers (write this matrix as a function of local servo gains) .

4.10. For the robot in Fig. 2.5. the matrices of the open-loop linear
mod.el have been determined in Exercise 4.6. Determine the matrix
of the closed-loop linear model in the following cases: a) if in
all joints just local position feedback loops are introduced,
b) if in all joints local position and velocity feedback loops
are introduced, c) if in all joints local PID controllers are ap-
plied.

4.11. In Exercise 4.7. the matrices of linear model of the robot inFig.
2.5. are determined around three different "nominal" states. Lo-
cal serVO systems have been synthesized and the obtained servo
feedback loops are given in Table 4.2. Determine the matrices of
the linear model of the robot if these local controllers are ap-
 177

plied. Determine these matrices for the same three "nominal" sta-
tes as in Exercise 4.7.

~
Position Velocity
feedback feedback gain
Joint gain [Nm/rad] [Nm/rad/s]
[N/m] [N/m/s]

1 40.2 11.9

2 1723.8 17.
3 1036.3 10.4

Table 4.2. Local servo gains for robot in Fig. 2.5. (data on robot
are given in Tables 3.3. and 3.4)

4.4 Synthesis of Decentralized Control for Simultaneous

Motions of Robot Joints

In previous sections we have considered the stability of the robotic

system with position control if all joints are moving simultaneously.
We have analyzed the stability of the robot using linear model of the
system, and this is obviously an approximative procedure. In doing
this we have restricted ourselves to consider just the case when local
serVO systems are applied around robot joints. Certainly, the stabili-
ty analysis using linear model of robot can be applied in the cases
when any other control law is applied for control of the robot (see
Chapter 5). However, decentralized control of robots (by local servo
systems synthesized for each joint independently) is still most frequ-
ently en.countered in practice, and therefore we shall pay special at-
tenti.on to this control law.

As we have seen in the previous section, in position control of robot

in local servos we may apply nominal control signal UO which has to
compensate for gravity moments in the desired goal position (see Fig.
4.1). This additional control signal U O need not to be introduced,
since we can compensate for the effects of gravity moments applying
some other methods. We have synthesized local servo systems in Ch. 3.
We have seen that in simultaneous positioning of several jOints of ro-
bO~dynamic coupling between the joints appears and this coupling is
not compensated py local servos. If the effects of dynamic coupling
178

during positioning of the jOints are "strong", the local servos might
become inefficient for simultaneous position control. By analysis of
stability of robot around the set goal positions of the joints we may
check whether the local servos, synthesized for each joint indepen-
dently, are sufficient to ensure satisfactory simultaneous positioning
of the robot. Instead approximative linear analysis presented in previ-
ous section, we have to analyze the entire nonlinear model of the ro-
bot (see Appendix 4.A). If the analysis shows that the robot positio-
ning is not satisfactory (regarding the speed of transient process etc.)
when selected local servos are applied, we have to re-select local
servos. Local servos can usually ensure efficient simultaneous positi-
oning of robot jOints.

The problem is much more complex if the tracking of desired path of

a robot endeffector in workspace is required. Aswe have already expla-
ined, in many robot tasks it is required that the robot hand tracks
some desired path in space sO(t), and, even more, the velocity and acce-
lerations of the robot hand have to vary along this path in some pres-
cribed way. To achieve this, the control system has to determine, at
the tactical control level (see Chapter 2), trajectories of all robot
jOintsqOi(t) which correspond to the desired path of the hand sO(t).
The robot jOints must simultaneously realize these trajectories in or-
der to ensure that the robot hand moves along desired path (and change
its velocity and acceleration in prescribed manner). The requirements
regarding the dynamic performance of the robot might be very strong in
such control tasks. Often it is required to ensure not only accurate
tracking of the joints angles trajectories qOi(t) but to ensure trac-
king of desired variations of the joints velocities and accelerations
(in order to synchronize the joint motions and realize desired varia-
tions of the robot hand velocities and accelerations). If desired nomi-
nal trajectories of joints angles are with high velocities and accele-
rations, then the dynamic moments (coupling between the joints) might
be very high and they might have strong effects upon the system beha-
viour. Here, we shall consider implementation of such robot tasks by
simple local servo systems.

Let assume that desired path of the robot hand is assigned by specifica-
tion of external coordinates of the robot as a function of time sO(t)
(by this also the desired velocities and accelerations of the robot
hand are given). At the tactical control level trajectories of the
jOints are computed qOi(t) to correspond to the set desired path
 179

of the endeffector sO (t) . The computed trajectories qOi (t) of the joints
are forwarded to local servos which have to ensure their tracking, i.e.
they have to ensure that the actual joint angles are as close as pos-
sible to qOi(t) at each moment t during the task execution (and by
this to achieve that actual joint velocities q(t) are as close to de-
sired trajectories gO(t), and the same holds for accelerations). The
problem is whether the local servos systems can adequately track input
trajectories qOi(t) if all joints of the robot are moving simultane-
ously. Actually, our problem is to examine whether the local servo sys-
tems can ensure tracking of jOints trajectories and realization of the
desired gripper movement.

The problem of trajectory tracking by local controller has been alrea-

dy addressed in Sect. 3.5. We have shown that if we introduce feed-
forward term in local servo we can ensure satisfactory tracking of de-
sired input trajectory qOi(t), assuming that all the other joints are
locked.

The feedforward term is synthesized in the form of local nominal con-

trol which has to satisfy (3.5.5). Actually, if only the i-th joint is
moving and all the other joints are kept locked,the dynamics of the
i-th joint and its actuator is described by

i=1 ,2, ... ,n (4.4.1)

where notations have been explained in Sect. 3.3. Local nominal con-
trol is synthesized to satisfy the following equations*):

·oi ~i oi ~i oi
x A x + b uL (4.4.2)

where xOi (t) is the nominal trajectory of the i-th subsystem. (Under
subsystem we assume actuator and joint if all the other joints are
locked, and the subsystem model is given by (4.4.1)). Calculation of
the local nominal contpol u~i(t) has been presented in Sect. 3.5. This
local nominal control ensures implementation of nominal trajectories
for decoupled subsystems (4.4.1) assuming that no perturbation is ac-
ting upon the system, that the model is perfect and that xi(O)=xoi(O).

*) For the sake of simplicity we shall assume that the local nominal
control is synthesized using the same H.. as for the synthesis of
the local servo gains, although this is~~not appropriate approach
(see Sect. 3.5).
180

If these conditions are not fulfilled, the state of the subsystem

(4.4.1) deviates from the nominal trajectory xOi(t). The model of
deviation of the subsystem from its nominal trajectory is given by:

·i A i Ai i
~x = A~x + b ~u (4.4.3)

where we have neglected the amplitude constraint upon the actuator

input u i . As we have shown in Sect. 3.5. the control ~ui which stabi-
lizes the model of deviation of the decoupled subsystem (4.4.3) redu-
ces to local servo system (which has been synthesized in Sect. 3.3).
Therefore, the control which stabilizes the individual joint around
the nominal trajectory qOi(t) (i.e. which stabilizes the decoupled
subsystem (4.4.1) around xOi(t)) is in the form:

(4.4.4)

assuming that only the position and velocity feedback loops are intro-
duced. Obviously, such control is very simple: each joint is
controlled independently from the rest of the system. Each local con-
troller has only information on the actual state of the corresponding
joint (position and velocity). Therefore, such local controllers re-
present decentralized control of robot.

We may apply this control (4.4.4) for tracking of nominal trajectories

of the jOints qOi (t) also in the case when all jOints are moving simul-
taneously, i.e. we may try to ensure simultaneous tracking of nominal
trajectories of all jOints of the robot by such decentralized control
law. However, this control has been synthesized neglecting the actual
coupling between the joints which appears when all joints are simulta-
neously moving. The simultaneous motion of all joints is described by
the model:

(4.4.5)

where Pi is driving torque around the i-th joint given by model

(3.2.2) •

If we compare the model of entire robot when all joints are moving
simultaneously (4.4.5), with the model of decoupled subsystems (4.4.1),
we can see that the coupling between the joints has not been taken
into account in control law (4.4.4). The coupling between the actuators
 181

is given by fi p .• Actually, upon the actuators are acting dynamic mo-

~

ments Pi which are complex functions of the jOints positions, veloci-

ties and accelerations (3.2.2). In synthesis of (4.4.4) we have taken
into accounts just "decoupled dynamics" of the robot mechanism, i.e.
we have assumed that upon the actuator is acting driving torque Pi gi-
ven by:

(4.4.6)

where Hii is an estimate of the moment of inertia of the mechanism

around the i-th joint (see Sect. 3.3). Therefore, in synthesis of con-
trol (4.4.4) difference between actual coupling fi p . and couplingfip.:
~ ~

(4.4.7)

has not been taken into account, which means that effects of dynamic
interconnections between the actuators are not compensated by the lo-
cal controllers (4.4.4). We have to examine the influence of these
factors (4.4.7) upon the stability of the entire robot around the no-
minal trajectories. In other words, we have to examine whether the
dynamics of the robotic system (4.4.7) which has not been taken in the
synthesis of the control (4.4.4) can "spoil" tracking of the desired
nominal trajectories. To answer these questions we have to analyze so-
-called practical stability of the system around the nominal trajecto-
ry x O (t)=(x 01 (t), x 02 (t), ... ,x on (t)). lfowever, this analysis is o~t of
the scope of this book and it can be found elsewhere [7, 8]. Byanaly-
sis of the practical stability of the robot around the nominal trajec-
tory we get an answer to the question whether the control law (4.4.4)
is able to overcome the effects of the factor (4.4.7) upon the stabi-
lity of the robot. If the influence of coupling is relatively weak,
the decentralized control (4.4.4) might overcome it, and the implemen-
tation of the imposed nominal trajectories of the jointsqOi(t) can be
ensured, regardless the fact that in the control synthesis we have not
taken into account the actual coupling between the jOints. In such a
case, the control (4.4.4) would be sufficient to realize simultaneous
tracking of all nominal trajectories of the joints, and by this to
realize desired nominal path of the robot hand.

The scheme of the control (4.4.4) is presented in Fig. 4.4. The con-
trol represents n local controllers around the robot jOints. The de-
centralized structure of this control is quite obvious. The main
182

a.. <=
a..

f-
0:: <=
CT <
a.. ..c::
CT

f-
-' 0 +
<
U
co
0
:0-
...... 0:: :r:
Z
'0- <
::c
lL.
<=
u
C)
a.. 'CT
w
::E:

1
I
I
I
0:: 0:: I
C)
f-
<
I;:: I
=> I~ I
f-
u It; I
< 1<
L_,......-:-~ _________ --l L __ _
-~
I
.....

LOCal
......----'""-... nominal
control

Fig. 4.4. Control scheme: local servo systems and

local nominal control
 183

advantage of this control law relative to other control laws (which

will be considered in the next chapters) lies in its simplicity. Name-
ly, the control (4.4.4) includes only local servo loops and the local
nominal control. The local nominal control does not include computati-
on of nominal driving torques Pi and, thus, it does not require compu-
tation of the dynamic model of the mechanical part of the robot. The-
refore,the local nominal control which is computed via (3.5.5) can be
easily computed on-line using cheap microprocessors.

However, we must underline once again that the control (4.4.4) does
not compensate for the dynamic coupling between joints, and, there-
fore, the tracking of fast trajectories by such control might be poor.
If relatively slow trajectories are to be realized (so that the dyna-
mic coupling between jOints is relatively weak) and if high accuracy
in trajectory tracking is not required, then the local controllers are
usually sufficient. This is specially the case if powerful actuators
and reducers with high reduction ratio are applied. In that case the
constant value of the equivalent moment of inertia of the actuator is
high relative to the variable values of the moment of inertia of the
mechanism, and, as we have seen in Section 3.4.4, the equivalent moment
of inertia of the actuator masks the effects of the variable moment of
inertia of the mechanism. Powerful actuators and reducers might reduce
to a high extend relative effects of coupling upon the behaviour of
local servos. If the powerful actuators are applied, then relatively
high local gains might be applied and local servos are less sensitive
to effects of an external load (dynamic coupling) upon them.

However, in modern robotics it is required to ensure accurate tracking

of fast trajectories. On the other hand, it is quite obvious a tenden-
cy to apply cheaper actuators requiring less power and to apply smal-
ler reducer gears in order to reduce their weight and to reduce the
backlash and friction. As we have already mentioned in Chapter 3, the
number of installed robots with direct-drive actuators is increa-
sing. The equivalent moment of inertia of such actuators is relatively
small. In these cases the application of only local controllers (4.4.4)
usually is not acceptable, but we have to apply a control which takes
into account dynamic coupling between joints r9]. Such dynamic control
which compensates for dynamic coupling between joints will be conside-
red in the next sections.

It should be noticed that the decentralized control (4.4.4) is still

applied to control many current robots on the market.
184

The reasons for these are extreme simplicity of the implementation of

this control, and the fact that majority of robots in industrial prac-
tice are not indented for precise tracking of fast trajectories, but
they just ensure position control and tracking of relatively slow no-
minal motions.

It should be also noticed that the linear analysis, considered in the

previous section, can be used for approximative analysis of the robot
performance if the decentralized control (4.4.4) is applied for simul-
taneous tracking of the nominal trajectories of all joints. Around
"each" point on the nominal trajectory xO(t) in the state space the
linear model of the entire robotic system is varying. Generally spea-
king, we should determine the linear models of the robot around an
infinite number of points at the nominal trajectory xO(t), and examine
their stability when local controllers (4.4.4) are applied. In this way
we can get an answer whether the robot is stable in the surroundings
of the nominal trajectory xO(t). Actually, as we have already explai-
ned, we get only necessary condition for nonlinear system stabili ty, but
not sufficient conditions. However, in practice, instead an infinite
number of points it is sufficient to examine the system stability
around several "characteristic" pOints at the nominal trajectoryxo(t)
For these "characteristic" points, the linear models have to be deter-
mined and their stability has to be checked. Since the linear models
vary along the nominal trajectory, the eigen-values of the linear mo-
del matrix also vary. To keep uniform performance of the robot along
the desired nominal trajectory, we should introduce variable local fe-
edback gains. As we have already explained, implementation of variable
feedback gains is not simple. Therefore, we should try to select con-
stant local servo gains which ensure approximately uniform behaviour
of the robot along the nominal trajectory. However, for each particu-
lar robot we should examine whether we can determine such unique local
servo gains so that the robot system behaves uniformly in the entire
working space and for all allowable working regimes (velocities and
accelerations) .

Example 4.4. For the robot in Fig. 3.2. we have synthesized local con-
trollers (4.4.4) around its joints in Examples 3.3.2, 3.5. and 4.3.2.
(local nominal control and local servo feedback gains). The nominal
trajectories of the robot joints are selected in such a way that the
joints move from the point A defined by coordinates (0.0 [radl, 0.4
[radl, 0.0 [ml) towards the point B with coordinates (0.5 [radl,
 '85

0.6 [radl, 0.08 [ml). The joints velocities have triangular profiles.
The movement should be accomplished in T = , [sl. The desired nominal
trajectories of the joints are given in Fig. 4.5. The digital simula-
tion of simultaneous tracking of these nominal trajectories by the
synthesized local controllers are presented in Fig. 4.6. (for all
three jOints).

--
-- ..----
0.6

0.5

0.4 ----- ---

q02
- _----

......
.. '
.. '
............ .

0.3
qOl ..•..••.....
0.2
.........
0.1
........
.... .. '

0.1 0.2 0.4 0.6 0.8

Fig. 4.5. Nominal trajectories of jOints of the robot in Fig. 3.2.

In the figure also is presented the tracking of the nominal trajecto-

ries if local nominal control (feedforward) is not introduced, but
only the local servo feedback loops are applied (dashed line). It can
be seen that in this latter case the tracking is relatively poor, sin-
ce the controllers do not compensate for the variation of the accele-
rations along the nominal trajectories even at the subsystem level.
The simulation in both cases assumes that the initial errors of the
jOints coordinates in respect the nominal trajectories qOi(O) are
lIq'(O) = -0.1 [rad], lIq2(0) = -0.05 [rad], lIq3(0) = -0.01 [ml, l\C~i(O)=O
for i = 1, 2, 3.

E X ere 1 s e s

4.12. Draw detail scheme of control considered in Example 4.4 fo~ the
robot in Fig. 3.2. The control includes local nominal control
and local (static) servo systems in all three joints (feedback
loops by position and by velocity as in (4.4.4)). Which is the
total number of feedback loops in this control scheme?
186

0 ;-
--
0.2 0.4 0.6
0 t[S]

loca 1 nominal
/ control
-0.05 I
/ --- without local

-- -- ...- ./
nomi na 1
control

-0.1

Lql[rad]

,...-
;-

0.2 0.4 0.8 1.0 t[S]

0.01

0.02

0.03

C.04

0.05
Lq2[rad]

0 ..
.2 0.4 0.6 0.8 y.~ t[s]
I

/ - .... , /
I
/

-0.005 /
""- /
/

I
I
I
"- ,, /
/
/

"
I /
~
-0.01 .... ---..".,/ /
Lq3[m]

Fig. 4.6. Simulation of tracking of nominal trajectories for

robot presented in Fig. 3.2. with local servo sys-
tems with or without local nominal control
 187

4.13. Compare the simulation of nominal trajectory tracking for the

first jOint of the robot in Fig. 3.2. in two cases: when all the
other joints are locked (Example 3.5. - the results of simulati-
on are given in Fig. 3.26) and when all the jOints simultaneous-
ly track their nominal trajectories (Example 4.4. - the results
of simulation are given in Fig. 4.6). Explain the reasons which
cause differences in trajectory tracking in these two cases.
Would these differences appear in the tracking of trajectory of
the third jOint of this robot?

4.14. Determine the minimal number of real operations (multiples and

adds) that has to be performed to compute nominal local control
and local servo control according to (4.4.4) in all three joints of
the robot in Fig. 3.2. (the control scheme has been considered
in Exercise 4.12). Assume that the second order models of actua-
tors are used to compute local nominal control and assume that
local servo loops are introduced by position and velocity (assu-
me that nominal qOi, qOi and actual qi, qi are given). Determine
the minimal number of microprocessors which have to be implemen-
ted in parallel in order to compute input signals for all three
actuators acc. to (4.4.4) at each 3 [ms] (sampling period whith-
in which the control computer has to compute new values of con-
trol signals for actual qi, qi and nominal qOi, qOi), if we ap-
ply microprocessors:

a) INTEL-80-80 (assume that one floating point add takes about

0.8 [ms], and one floating-point multiply takes about 1.5
[ms]), or

b) INTEL-80-87 (addition takes about 35 [~s], and multiply takes

65 [~s]).

Assume that computer time is consumed only for additions and

multiplications.

4.15. Explain in detail the statement presented in section 4.4. that

with the direct-drive actuators application of only local servos
for control of simultaneous motion of joints might be poor (take
into account equivalent moment of inertia of such actuator and
allowable local feedback gains) .
188

4.16.* Extend the programme written in Exercise 4.4. for simulation of

simultaneous motion of all joints to include local controllers
with local nominal control (use also the program written in
Exercise 3.64). Check the results of simulation in Example 4.4.
(Fig. 4.6).

4.4.1. Synthesis of nominal programmed control

In the previous text we have explained that the simple local control-
lers cannot always ensure simultaneous motions of the robot jOints
along desire nominal trajectories. It is often required to apply some
control law which takes into account dynamic coupling between the mo-
tions of the joints. A possible extension of decentralized control is
to introduce so-called centralized nominal programed control.

In Section 4.3. we have considered simultaneous position control of

robot joints and we have considered analysis of linear model of the
robot around the desired positions of the robot joints. In that case
the control system has to ensure that the robot state will be driven
toward the desired nominal state x O (which corresponds to the desired posi-
tions qOi of the jOints). We have assumed that the nominal control sig-
nals UOi , determined based on (4.3.2), are introduced at the actuators
inputs. These nominal signals ensure that the robot is in equilibrium
in the point x O , i.e. that x
= O. for x = xO. However, if the robot
hand has to move along some defined path sO(t) in workspace (with de-
sired variations of velocities and accelerations), then the system
state x(t) has to move along nominal trajectory xO(t) which corres-
ponds to nominal trajectories of the jOints qoi(t). It is obvious that
the local nominal control u~i~t) synthesized in Section 3.5. ensures
tracking of the trajectory qOl(t) of the i-th jOint, only if all the
other jOints are kept locked. If all joints are moving simultaneously,
u~i (t) does not ensur':l moving of the actual subsystem state xi (t) along
desired trajectory X 01 (t), since the influence of the other jOints
(dynamic moment Pi) has not been accounted in computation of U~i(t).
Therefore, in order to compute feed forward which will take into acco-
unt the coupling between the joints along the nominal trajectories
qOi(t), we have to consider the entire model of the system (4.3.1).

We have to determine programmed control UOi(t) which satisfies (analo-

gously to (4.3.2)) [2,6]:
 189

tE (0, T) (4.4.8)

Control UOi(t) represents a set of time-variable signals uO(t)=(uo1 (t),

u0 2 (t) , ... ,uon(t»T which have to satisfy (4.4.8) along the desired
nominal trajectories. If these signals are fed at the inputs of actua-
tors of the robot, they will realize the movement of the robot state
along the nominal trajectory xOi(t) assuming that the following condi-
tions are fulfilled (analogously as in Section 3.5):

(a) the actual initial state of the system x(O) has to coincide with
the nominal initial state x(O) = xO(O),

(b) the model of the entire system (4.4.8) must be perfect (all para-
meters of the system must be perfectly identified, etc.),

(c) no disturbance is acting upon the system.

If all these conditions are satisfied, the signals uOi(t) would drive
the robot joints so that their angles vary in ti~e as prescribed
qOi(t). Thus, if the control satisfying (4.4.8) is applied, the system
is in equilibrium along the trajectory XOi(t).

The model of deviation of the system state around the nominal trajec-
tory xOi(t) and the nominal control uOi(t) can be observed. The model
of deviation is obtained in the form which is equivalent to the model
of deviation around the point xO, u O (4.3.3):

(4.4.9)

We have to keep in mind that the Nxl vector a and the Nxn matrix Bare
time variable functions of 6x(t) since they depend on nominal trajec-
tory xO(t). Similarly, the constraint upon the amplitude of the input
signals u i becomes time dependent N(uoi(t), 6U i ) since it depends on
uOi(t). The model of deviation is in equilibrium for 6x = O. (since if
6u = 0, then it follows 6x = 0.).

Analogously to the linearization of the model of the state deviation

around the point xO, UO which is obtained in the form (4.3.5), we can
obtain linear model of deviation of the robot state around the nominal
trajectory xO(t) and control uO(t):
190

(4.4.10)

where the matrices AL(NXN) and BL(Nxn) are, now, time dependent, since
they depend on nominal trajectory. Now, we should analyze stability of
time-variable linear model. However, it is sufficient to consider li-
near model of the robot in a several "characteristic" points at the
nominal trajectory and we have to examine eigen-values of the matrices
of these linear models.

The nominal control uOi(t) which satisfies (4.4.8) represents program-

med control, since it is a function just on time and it does not de-
pend on the actual state of the system. Since it is computed using the
total centralized model of the robot (4.4.8) it is called centralized
nominal programmed control (to differ it from the local nominal con-
trol which is computed using the models of local (decoupled) subsys-
tems (3.5.5) without taking into account the coupling between the sub-
systems). The centralized nominal control represents feedforward along
the trajectory which compensates not only for the delays in the system
caused by the variations of the joints velocities and accelerations
along the nominal trajectory, but also compensates for the dynamic co-
upling between the joints of the robot caused by their simultaneous
motions. This centralized nominal control compensates for the effects
of nominal coupling between the joints and therefore it reduces the
effects of the interconnections between the joints.

Let us briefly explain the above statement that the nominal centrali-
zed control uO(t) reduces the effects of coupling between the jOints.
Let us consider the model of the robot in the form (4.4.5). The centra-
lized nominal control satisfies (analogously to (4.4.8)):

xOi(t) (4.4.11 )

where p?(t) is the so-called nominal driving torque in the i-th joints
1.
which must satisfy:

p?(t) (4.4.12)
1.

The model of the robot actual performance is given by (4.4.5). From

(4.4.5) and (4.4.11) we can obtain the model of deviation of the sys-
tem state from the nominal trajectory xO(t) and the nominal control
u O (t) (analogously to (4.4.9)):
 191

i=1,2, ••• ,n (4.4.13)

where /lXi XOi(t) is the vector of deviation of the state of the

i-th actuator (subsystem) from its nominal trajectory, /lP i = Pi-P~(t)
is the deviation of the moment in the i-th joint from the nominal mo-
ment P~(t), /lui = u i - uOi(t) is the deviation of the input signal of
1 ci
the i-th actuator from the nominal control u (t). For the sake of
simplicity, in (4.4.13) we have neglected the constraint upon the am-
plitude of the actuator input signal. The control /lUi has to be syn-
thesized to stabilize the model of deviation of the state (4.4.13)
around the nominal trajectory and control. The synthesis of the con-
trol /lui is performed in the following way. Instead the actual model
of the state deviation (4.4.13) we consider approximative model in the
following form:

i=1,2, .•. ,n (4.4.14)

where /lP i is the deviation of the moment from the nominal moment, but
for approximative model of the robot dynamics (4.4.6). Therefore, /lP i
is given by

(4.4.15)

where H11
.. is the estimated constant value of the moment of inertia of
the mechanism around the i-th joint. It is obvious that the model
(4.4.14) represents the set of models of the individual jOints and
their actuators considered independently one from another. Now, the
control /lUi, which stabilizes the model (4.4.14), can be obtained as
a set of local servo systems around the robot joints. The local servos
can be synthesized for each joint independently from the other jOints.
The synthesis of such independent local servos has been considered in
Chapter 3. The local servo systems stabilize the approximative model
of deviation (4.4.14). The local servo systems are synthesized to sta-
bilize each joint decoupled from the other joints. If just position
and velocity servo loops are introduced in each local servo, then the
control signal for each actuator is obtained as

(4.4.16)

The scheme of the robot control, if the centralized nominal control

and local servo systems around robot joints are applied, is presented
192

in Fig. 4.7. Actually, the nominal programmed control signals calculated

by the centralized model of the robot, are added to signals from simple
local servo systems.

If we apply the control presented in Fig. 4.7, we may guarantee that the
approximative model of the robot dynamics (4.4.14), (4.4.15) is stabi-
lized around the nominal trajectory xOi(t). However, it is qUite obvi-
ous that this control does not guarantee the stabilization of the "ac-
tual" model of the robot (4.4.13). The actual coupling between the jo-
ints of the robot is not described by (4.4.15) (as assumed in synthe-
sis of local servos), but actual coupling are total dynamic moment in
the joints:

(4.4.17)

The total dynamic moment in the joint Pi is described by the dynamic

model of the mechanical part of the system (3.2.2).

The local servo systems stabilize the decoupled models of the joints
(and actuators) in which the actual coupling between the jOints 6P i is
neglected. Therefore, the actual coupling between the joints 6P i (i.e.
the deviation of the actual coupling from the nominal coupling P~(t))
l
has not been compensated neither by local servo systems, nor by nominal
centralized control. Actually, the difference:

(4.4.18 )

has not been taken into account in the synthesis of the control (4.4.16).
Therefore, we have to examine the effects of these factors upon the
stability of the robot system. This analysis is presented in Appendix
4.A.2. If the effects of these "factors" upon the stability of the en-
tire system is relatively "weak", then the control (4.4.16) is suffi-
cient to stabilize the robot and to ensure acceptable tracking of the
nominal trajectory.

By comparison of the equation (4.4.7) (representing the uncompensated

coupling when local controller (4.4.4) are applied) and the equation
(4.4.18), we can see that if the centralized nominal control (4.4.16)
is applied, then the uncompensated coupling is decreased by the value
 193

r l
I
I
I
c
<I- I
I I
I I
I :CT
I I I
I I I
I-
I I I
I I
I I
I I I
I I I
I I I
I I I
I 1

I
I
I c
I 16 1

I I!;;:
=>
I
I It; I
- - - - - - - -~ I_~ ____ ~:o_ _____ J

o c
:0 o
:0
+
NOMINAL
CENTRALIZED
CONTROL

'0
x
Fig. 4.7. Control scheme: local servo systems and centralized
nominal control
194

of nominal coupling p?(t). Namely, local controllers (4.4.4) do not

~

compensate for nominal dynamic torques P~(t). Therefore, the effects of

~

coupling (4.4.7) upon the system performance (when just local control-
lers (4.4.4) are applied), might be considerably stronger than the ef-
fects of the difference between the actual and nominal coupling (4.4.18)
(which is uncompensated in the case when the centralized nominal con-
trol is added to local servos). In other words, the stabilization of
the robot around the nominal trajectory is much efficient if centrali-
zed nominal control is applied than if just local controllers with lo-
cal nominal control are applied.

However, the application of the centralized nominal control suffers

from certain drawbacks. Let us consider how we can compute centralized
nominal control. In Section 3.5. we have shown how we can compute lo-
cal nominal programmed control. Let us assume that the nominal trajec-
tory xO(t) of the system state is given, i.e. let us assume that the
nominal trajectories xOi(t) of all coordinates of the states vectors of
all subsystems (actuators) are given. Let us assume that all models of
the actuators are of the second orders n.=2 and that the matrices of
~

models (3.2.6) are in the form:

(4.4.19 )

The state vector of the system in this case is given by x(t)=(x 1T (t),
x 2T (t), ... ,x nT (t))T = (ql, ~1, q2, ~2, ... ,qn, ~n)T. If the trajectori-
es of the jOint angles (linear displacements) qOi(t) and of the velo-
cities ~Oi(t) are given, by differentation we can get the desired va-
riation of the joint accelerations qOi(t) along the nominal trajecto-
ries. On the basis of the dynamic model of the mechanical part of the
system (3.2.2), nominal driving torques P~(t) can be computed accor-
~

ding to (4.4.12). These nominal driving torques have to be realized

around the robot joints in order to ensure that the robot joints chan-
ge their angles in time according to the desired functions qOi(t). If
all three above listed conditions were fulfilled, the realization of
nominal driving torques P~(t) would cause desired movement of all ro-
~

bot jOints. To realize the nominal driving torques, we have to realize

the nominal programmed signals uO(t) = (U 01 (t), u 02 (t) , ... ,uon(t))T at
the inputs of actuators. These nominal signals have to satisfy (4.4.11)
and, therefore, they are computed as (taking into account the models of
 195

actuators (3.2.6) and (4.4.19»:

(4.4.20)

where by X~i(t) is denoted,the second coordinate of the state ~ector of

the i-th actuator (i.e. ,X~l(t) denotes the nominal velocity gOl(t) of
the i-th joint), and X~l(t) denotes the acceleration of the i-th joint
qOi(t). Based on (4.4.20) we can compute the centralized nominal pro-
grammed control uOi(t). It can be seen that expression (4.4.20) for
the centralized nominal control differs from expression (3.5.8) forthe
local nominal control, due to the term fip~(t) which represents the ef-
1
fects of nominal driving torques, i.e. of the nominal coupling caused
by simultaneous motions of all jOints. When uOi(t) is computed by
(4.4.20), we have to test whether the nominal control is within the
permitted amplitude limits:

(4.4.21)

If (4.4.21) is not fulfilled for any joint, that means that the given
nominal trajectory cannot be realized by the particular robot and its
actuators. In that case we have either to slow down the desired motion
of the robot, or, if the robot is under design, we may re-select actu-
ators (see Sect. 3.5).

It is obvious that the centralized nominal control requires computati-

on of the nominal driving torques P~(t). The nominal driving torques
1
are computed on the basis of total dynamic model of the mechanical part
of the robot. As we have already explained, the dynamic model of the
robot might be extremely complex nonlinear equations. Therefore, com-
putation of nominal driving torques for the given nominal angles, ve-
locities and accelerations might require the control computer to per-
form a large number of adds and multiplies in a short sampling in-
terval. If the centralized nominal control has to be computed
on-line (during the execution of motions of the robot), then we have
to ensure that the control computer computes the values of the nominal
driving torques and the values of the nominal programmed control ac-
cording to (4.4.20), at rate of 5-10 [msl. This means that within 5-10
[msl the microcomputer has to calculate once P~(t) for given qO(t),
, 1
gO(t), gO(t) and to calculate U01(t) for all jOints. Therefore, the
microcomputer which is capable to achieve desired rate of computation
must be relatively powerful and expensive (or, several microprocessor
196

might be applied in parallel, but this certainly complicates implemen-

tation of control system).

However, the nominal driving torques and centralized nominal program-

med control are exclusively functions of nominal (imposed) trajectories
of the jOints angles, velocities and accelerations, and they do not de-
pend on actual (realized) coordinates of the robot. If the desired no-
minal trajectories of the joints are known in advance, then the nominal
driving torques and the nominal programmed control might be computed
off-line (in advance) and memorized in the control microcomputer. In some
industrial applications the process to be realized by the robot is per-
formed in strictly pre-defined conditions, in defined environment and
in precisely pre-specified manner. In such cases the nominal trajecto-
ries of the robot hand (and corresponding nominal trajectories of the
joints) might be defined in advance. Then, we can compute nominal con-
trol off-line in the phase of robot teaching to perform the set task.
In this way we avoid computation of the nominal programmed control in
real time, during the task execution. The control computer may compute
nominal driving torques and control relatively slowly. During the exe-
cution of the movements of the robot, the control computer has just to
take, from its memory, prepared, computed values of the nominal control
signals and to send them at the actuators inputs (obviously, the con-
trol signals of local servo systems are added to these nominal control
signal according to the scheme in Fig. 4.7). In this way requirements,
regarding the necessary speed of computation, that are imposed upon the
control microcomputer are reduced.* )

However, such a solution has certain drawbacks. In modern industry

(specially if we think on flexible manufacturing systems) the robot
tasks are often executed in variable conditions, which cannot be strict-
ly pre-defined, and therefore it is not possible to compute in advance
nominal trajectories of the robot. On the other hand, many tasks (mo-
vements), which robots have to realize, might be very complex, consis-
ting of a large number of various elementary movements, so that the
ammount of data on nominal trajectories which have to be memorized
might be enormous (specially if we take into account that we must me-
morize nominal trajectories and off-line computed nominal programmed
control for n joints). To memorize so large ammount of data on nominal

*)
The nominal control might be computed and memorized with lower sa.m-
pIing rate then 10-15 [ms], and then, in on-line control the compu-
ter can determine the nominal control signals by interpolation bet-
ween the memorized values.
 197

trajectories and on nominal control, large capacity of the computer

memory is required, which also increases the price of the control mi-
crocomputer.

Due to these reasons, the application of off-line computed and memori-

zed nominal programmed control is limited to only a narrow set of ro-
bots tasks in which all conditions are pre-defined and which require
only a few short movements of the robot to be repeated for many times.
However, the nominal programmed control suffers from some other draw-
backs, too. The centralized nominal programmed control compensates just
for so-called nominal coupling between the robot joints along the no-
minal trajectory. The actual coupling between the joints which appears
when the joints coordinates deviates from the nominal trajectories is
not compensated for by nominal control. This means that the effects of
the centralized nominal control are limited. As we have explained abo-
ve, application of the centralized nominal control together with the
local servos (Fig. 4.7) does not always guarantee that the robot is
stabilized around the nominal trajectory (see Appendix 4.A). It might
be necessary to introduce, besides the centralized nominal control, an
additional global control which has to compensate for the actual coup-
ling between the jOints (see Chapter 5).

The computation of the centralized nominal control is based on the to-

tal dynamic model of the robot. This means that the nominal control is
efficient only if all parameters of the robot (geometric data, masses
and moments of inertia, friction coefficients, data on actuators, etc.)
are identified very accurately. This requirement cannot be often sa-
tisfied, and therefore the efficiency of the centralized nominal pro-
grammed control, computed on the basis of the total dynamic model of
the robot, might be even unefficient regarding the compensation of the
nominal coupling between the joints. Namely, nominal programmed con-
trol is not robust (see Chapter 6).

In modern industry it is usually required that the robot plans its mo-
vements in real time (using information, obtained from cameras or other
sensors, on actual state in its workspace or using information from
other subsystems, conveyers, other robots cooperating in the same pro-
cess and so on). In such tasks many parameters are unknown in advanced
and they might vary during tasks executions. In such tasks application
of nominal centralized control is not efficient. The decentralized con-
trol (4.4.4) which includes local nominal control is much simpler for
198

application. The decentralized controller (4.4.4) can be easily imple-

mented on-line and it is much more appropriate for the tasks which re-
quire on-line planning of the robot paths and on-line generation of the
jOints trajectories. However, in comparison to the centralized nominal
control, local controllers are less efficient in reducing the effects
of coupling, since they do not even compensate for the nominal coupling.

Example 4.4.1. For the robot in Fig. 3.2. the nominal trajectories of
the joints have been given in Fig. 4.5. Based on these trajectories
qOi(t) we can easely obtain the velocities of the joints qOi(t) and
corresponding accelerations qOi(t). Based on dynamic model of the mec-
hanical part of the robot (3.2.3) and corresponding data (given in
Table 3.2), we calculate the nominal driving torques which are presen-
ted in Fig. 4.8. Based on the models of actuators (D.C. electro-motors
- data on which are given in Table 3.1) we compute centralized nominal
programmed control for all three joints (eq. (4.4.20)). The centrali-
zed nominal control is presented in Fig. 4.9.

1' 2 pO [N]
7 3
--P1 o t-----+---+----+--+-~f----_____<_
p~
0.2 0.4 0.6 0.8 1.0 t[s]
4 L

I -20
o
0.2 0.4 G.6 0.8 1 t[ s]
-40

-4

-7

Fig. 4.8. Nominal driving torques for trajectories in Fig. 4.5.

The linearized model of this robot has been presented in Example 4.3.1.
The matrices of the linearized model are given by (4.3.13). The varia-
tions of the elements of the linearized model of the robot along the
nominal trajectories in Fig. 4.5. are presented in Fig. 4.10 (AL =
[a .. ], BL = [b .. ]). The eigen-values a of the matrices of the open-
1J 11
-loop linearized model of the robot AL also very along the nominal
trajectories as presented in Fig. 4.11. Let us assume that at the
 199

20

10

o
.0 t[sl

02-
u
-18

Fig. 4.9. Nominal programmed control (centralized)

for robot in Fig. 3.2.

--
0.5
a 21
02 0.5 C.8 1. t[s]

0.2 0.4 0.6 0.8 t[ sl

"-
i'-- a 23
-0.5
............... a
42

-1. !---
............... a
22

Fig. 4.10. Variations of elements of linearized model matrix of

robot along nominal trajectory (robot from Fig. 3.2)

inputs of the actuators are fed the nominal centralized control sig-
nals (from Fig. 4.9) and that the local servos with constant feed-
back gains are applied. If the local servo feedback loops are closed
with the feedback gains given in Table 4.1 (see Example 4.3.2), then
the eigen-values of the matrix of the linearized model of the robot
also varies along nominal trajectories as presented in Fig. 4.12.
However, based on Fig. 4.12 we can conclude that the eigen-values of
the matrix of the closed-loop system vary slightly and that they stay
200

at the left hand side from the line Re(s)=-3 along the entire nominal
trajectory. This means that the linearized model of the system around
the nominal trajectory is exponentially stable with stability degree
of a=3. Since all eigen-values of the closed-loop system matrix are
real along the nominal trajectory, this means that the local servos
together with the nominal programmed control UOi(t) are sufficiently
robust to ensure satisfactory performances of the linearized model of
the robot along the nominal trajectory. However, the problem is whe-
ther such control can satisfy nonlinear model of the robot, or we
must introduce variable local gains, or additional global control
has to be applied.
Re(a)

0.05

0.2 0.5 0.8 t[s]

-0.05f~----------------------------------------

-16.+----------------------------------
=
-45.+----------------------------------

Fig. 4.11. Variation of eigen-values of matrix of open-loop

linearized model of robot along nominal trajectory

-1 0.2 0.4 0.6 0.8 t[s]

-3

-14. ~-------------------------------------

-24.+-______________________________________
-25.+---------------------------------------
-96.r---------------------------------------
Re(a)
Fig. 4.12. Variation of eigen-values of matrix of closed-loop
linearized model of robot along nominal trajectory
 201

Exercises
4.17. For the robot in Fig. 2.5. the trajectories of the joints are
given by the following time functions:

o < t .:: 0.5[s]

2
- -;- - t 2 ) 0.5<t'::1[s], where a 1 0.5 [ra2d],
s

3 0.4 [m2 ],
a T = 1 [s]
s
Compute the nominal driving torques if all data on the mecha-
nism are given in Table 3.4. and compute the centralized nomi-
nal control if the models of actuators are of the second order
and if data on D.C. electro-motors are given in Table 3.3.

4.18. For the robot considered in the previous exercise compute the
local nominal control and compare it with the centralized no-
minal control.

4.19. For the robot in Example 4.4.1. check whether the nominal dri-
ving torques are corectly computed (Fig. 4.8) in the following
time instants t=O., t=T/2, and t=T. Also check the nominal con-
trol (Fig. 4.9).

4.20. For the third order models of actuators n i =3 in the form

(3.2.6) where the matrices are in the form (3.2.7) show that
the centralized nominal control is computed according to the
following equations (assuming that qOi, qOi, qOi are imposed
i i·i.i T iii T
and that x = (q , q , lR) = (x 1 , x 2 , x 3 ) ):

(I'It - small),

i=1 ,2, •.. ,n

202

4.21. Calculate the nominal programmed control for the robot in Ex-
ample 4.4.1. if all models of actuators are of the third orders,
using the expressions given in Exercise 4.20. Compare these re-
sults with the nominal control presented in Fig. 4.9.

4.22. Explain why in expressions (4.4.20) for the centralized nominal

control (or in expressions in Exercise 4.20) we use matrices of
actuators without taking into account the moments of inertia of
the mechanism, while in the expressions for the local nominal
control (3.5.8) (or, in Exercise 3.60) we use the matrices which
include the moments of inertia of the mechanism around the cor-
responding jOints axes.

4.23. Explain why in Example 4.4.1. the eigen-values of the open-loop

linearized model of the robot (Fig. 4.11) vary much more along
the nominal trajectory than the eigen-values of the matrix of
the system with the closed local servo loops (Fig. 4.12)?

4.24.* Write in one of high-level programming language the programme

for computation of the nominal driving torques (for the robot
in Fig. 3.2) and for computation of the centralized nominal con-
trol (if the models of actuators are either of the second, or of
the third order). Try to minimize the number of adds and multi-
plies required for these computations. The programme inputs are
the nominal trajectories of the joints qOi(t), velocities qOi(t),
and accelerations qOi(t), and the outputs are the nominal dri-
ving torques P~(t) and the centralized nominal control uOi(t),
1
i=1, 2, 3. Combining this programme with the programme written
in Exercise 3.57 write the programme which will implement (by
micro-computer) the control law presented in Fig. 4.7 (for par-
ticular robot in Fig. 3.2).

References

[1] Vukobratovic M., Applied Dvnamics of Manipulation Robots: Model-

ling, Analysis and Examples, Springer-Verlag, 1989.

[2] Vukobratovic M., Stokic D., Control of Manipulation Robots: Theory

and Application, Series: Scientific Fundamentals of Robotics Mono-
graph, Springer-Verlag, Berlin, 1982.
 203

[3] ~edvedov S., Leskov A., Juschenko A., Control Systems of Manipu-
lation Robots, (in Russian), Monograph, "Nauka", Moscow, 1978.

[4) Vukobratovi6 M., Kir6anski N., "Computer-Oriented Method for Li-

nearization of Dynamic Models of Active Spatial Mechanisms", Me-
chanism and Machine Theory, Vol. 17, No.1, 1982.

[5] Chen C.-T., Linear System Theory and Design, Holt, Rinehart and
Winston, New York, 1984.

[6] Takigaki M., Arimoto S., "A New Feedback Method for Dynamic Con-
trol of Manipulators", Trans. of the ASME, Journal of Dynamic
Systems, Measurement a,nd Control, Vol. 103, No 2, 1981.

[7) Michel N.A., "Stability, Transient Behaviour and Trajectory

Bounds of Interconnected Systems", Int. Journal of Control, Vol.
11, No.4, pp. 703-715, 1970.

[8) Vukobratovi6 M., Stoki6 D., Kir6anski N., Non-Adaptive and Adap-
tive Control of Manipulation Robots, Monograph, Series: Scienti-
fic Fundamentals of Robotics 5., Springer-Verla,g, Berlin, 1985.

[9) Asada H., Youcef-Toumi K., Direct-Drive Robotics: Theory and

Practice, The MIT Press, 1987.
204

Appendix 4.A
Stability Analysis of Nonlinear Model of Robot

In Chapter 4. we have shown how we can analyze the linearized model of

robot in the case when the nominal centralized control is applied to-
gether with the local controllers (local servo systems) and the simul-
taneous movements of the robot joints are required. As we have already
underlined, the analysis of linearized model gives necessary, but not
sufficient conditions of stability. Nonlinear model of the robot should
be analyzed. Therefore, here we shall consider the analysis of the non-
linear model of robot. Due to high nonlinearity of the dynamic model
of robot, in general case, it is not possible to solve analytically
the system of differential equations (which represents the model of
the robotic system). Therefore, we have to apply methods for stability
analysis which do not require explicit solution of the nonlinear model
of the system. There is a number of methods for analysis of stability
of large-scale nonlinear systems. These methods mightbegeneraly clas-
sified into two groups: the input-output stability methods and the me-
thods via Liapuno~s stability [1-4]. Here, we shall consider the lat-
ter approach for stability analysis.

We shall analyze the stability of the nonlinear model of robot when the
local servo systems are applied and when all joints might move simul-
taneously. First we shall consider the stability of the robot when it
has to be positioned in various positions in work space. In doing this
we assume that the control signals which compensate for gravi ty moments
(forces) in the goal position, are implemented. Therefore, the imposed
goal positions might be regarded as equilibrium points in the state
space, and we may analyze the asymptotic (or exponential) stability of
the robot around these imposed goal positions.

Next, we shall analyze the stability of the system around the nominal
trajectory, i.e. we shall examine the robot capability to realize the
desired trajectories. We shall assume that besides the local servo sy-
stems, the nominal centralized control is also applied. The applicati-
on of the centralized nominal control (as we have explained in Section
4.4.1) ensures that the robotic system is in equilibrium along the nomi-
 205

nal trajectory. Therefore in this case the asymptotic (or exponential)

stability of the robot can be analyzed, too. If the nominal centrali-
zed control is not applied, but just the local nominal control and lo-
cal servos are implemented, then the robotic is not in equilibrium along
the nominal trajectory. Thus, in this case we cannot analyze asympto-
tic stability of the system around the equilibrium point, but we have
to analyze the so-called practical stability of the system.

In presentation of the method for the stability analysis we shall not

give rigorous proofs for each step of the procedure, but we shall try
to present the method in a simple way. The more rigorous formal treat-
ment of the subject might be found in the literature [1-9].

4.A.1 Analysis of asymptotic stability of robot position control

First, we shall consider the problem of stability of the nonlinear mo-

del of the robot in the position control. Let us assume that desired
position of the robot is imposed qOi, i=1,2, .•• ,n, (i.e. the goal posi-
tions of all joints). The state vector x O corresponds to the imposed
position qO (assuming that all joints velocities have to be zero in
the goal positions qO = 0). We shall assume that control U O is deter-
mined which satisfies (4.3.2), i.e. this control compensates the gra-
vity moments in the imposed positions qO (since when the robot stops,
just the gravity moments are acting around the joints axes). If the
control signals satisfying (4.3.2) are applied at the actuator inputs,
then when the robot reaches the desired state X O the first derivative
of the state vector must be equal to zero iO=o, which means that the
system is in the equilibrium state. Since the state x O is the equilib-
rium state of the robotic system, we may analyze the asymptotic stabi-
lity of the robot around this state. We have to examine whether the
control ensures that for any initial state x(O) the system is driven
to the imposed position qO. As it is well known [2], the equilibrium
state of the system XO is asymptoticaly stable if for each number £>0,
there exists a number 8>0, such that (here I I· I I denotes the norm of
the vector)

II fix (0) II <£ (4.A.1l

where flx(t) = x(t)-x o is the deviation of the system state from the
equilibrium state xO, implies
206

II1Ix(t) II < 15 for t 2. 0 (4.A.2)

and that there exists a number ~>O such that I 11Ix(o) I I<~ implies

lim x (t) (4.A.3)

t-+-oo

Since we are considering stationary system it is obvious that the ini-

tial moment might be any time instant (and therefore this is also uni-
form asymptotia stability of the system). It is obvious that if the
robot is asymptotically stable around the imposed positions qO, then
the control system ensures positioning of the robot in the desired
(goal) position. However, from (4.A.3) if follows that the robot might
reach the desired position (state) in theoretically infinite time. The
asymptotic stability does not say anything about the speed by which
the robot approaches the desired position. Since it is necessary to
ensure sufficiently fast positioning of the robot, we have to examine
exponential stability of the system. It is well known [21 that the equ-
ilibrium state of the system X O is exponentially stable, if there exist
two numbers n>O and n>O which are independent from the initial state
x(O) and which satisfy

(4.A.4)

This practically means that the state XO is exponentially stable if

each motion of the system (starting from any initial state x(O» con-
verges towards x O faster than the exponential function exp(-nt). The-
refore, by examination of the exponential stability of the robotic sy-
stem we estimate the rate of its Positioning*).

The analysis of the asymptotic (exponential) stability of the robotic

system around the imposed position can be realized by application of
the Liapunov's direct method [31. The basic idea of the Liapunov's di-
rect method is to analyze a nonlinear system of arbitrary high order
by one scalar (positive) function of the system state. If it is possi-
ble to select such a continuously differentiable positive definite
scalar function of the system state v(lIx), that its first derivative
by time v(lIx) is negative definite function (except for v(O)=O.), then

*) We have to note that we have already used these concepts in Chapter

3 concerning the local control synthesis. We also note that under
the notion of the robot stability (asymptotic, exponential, practi-
cal) we actually assume the stability of the robot state, or tra-
jectory.
 207

the considered system is asymptotically stable around the equilibrium

state xO. This means that for any initial state of the system x(O) the
selected function v(6x) must continually decrease along the solution
of the model of the system x(t) until it reaches its minimum vIOl, (sin-
ce v(0) = 0), which means that the system will reach its equilibrium
state xO. Therefore, the method enables analysis of the stability of
the system without solving the differential equations which describe
the system behaviour. (Actually, the nonlinear differential equations
representing the model of the system cannot be solved analytically in
general case). Here, we shall not prove the validity of this method.
It should be underlined that the basic problem in application of this
method lies in the selection of the Liapunov's function for the consi-
dered system. There is no general procedure for unifold selection of
the Liapunov's function which will guarantee the fulfillment of the
stability conditions. In other words, the Liapunov's method gives just
sufficient but not necessary conditions of the system stability: if
the selected Liapunov's function satisfies that its derivative along
the system solution is negative definite, then we may guarantee that
the system is asymptotically stable. However, if this condition is not
fulfilled, we cannot say anything about the system stability. The sy-
stem might be stable, although the selected Liapunov's function does
not fulfill the above mentioned conditions. Since the procedures for
selection of the Liapunov's function are missing, the stability analy-
sis by Liapunov's method might be conservative. If we do not select
an adequate Liapunov's function we may get negative results of the
stability test although the system is actually stable. This means that
we would require the stronger conditions to ensure system stability
than they are actually necessary. (In other words, we would require
higher feedback gains than they are actually necessary to ensure the
system stability). Therefore, it is necessary to pay special attention
to selection of the Liapunov's function in order to minimize the con-
servatism of the stability test.

Here, we have presented just a few general notes on the direct Liapu-
nov's method for the stability analysis. The basic problem in applica-
tion of this method to analysis of stability of large scale nonlinear
systems lies in the selection of the adequate Liapunov's function: the
complex high order nonlinear system (which means the system with high
number of the state coordinates) is substituted by a scalar function.
To minimize the conservatism of this method, a number of aggregate-
-decomposition methods for stability analysis of large-scale systems
208

has been developed. The basic idea of all these methods is to ensure
the least conservative tests of the system stability by an insight in
the physical structure of the system. Actually, the idea is to "use"
the system structure in the stability analysis. In this it is assurnrned
that the majority of the large-scale systems might be decomposed to a
number of subsystems of the lower order. In essence all aggregate-de-
composition methods for stability analysis follows the same procedure
[1] :

a) The system is decomposed to a number of subsystems of lower order

(in doing this it is the most important to observe the physical
structure of the system and to decompose the system according to
its physical features; in this way the subsystems are determined
which physical interconections are weak - under assumption that such
subsystems can be identified at all) .

b) Each local subsystem is analyzed independently from the rest of the

system (decoupled subsystems) and the "measure" of their local sta-
bility is estimated.

c) The quantitive estimates of the interconnections between the subsys-

tems are determined.

c) The conditions for the stability of the entire (complex) system are
determined on the basis of the quantitative estimates of the stabi-
lity of the local subsystems and on the basis of the quantitative
estimates of the interconnections between the subsystems.

Itis clear that if there is no interconnections between the subsystems

(i.e. if the subsystems are completely decoupled), then the stability
of the entire large-scale system is guaranteed by the local stability
of the subsystems. If the coupling between the stable subsystems is
relatively weak, the entire system will be stable. Therefore, the me-
thods for stability analysis are better if stronger coupling between
the subsystems is allowed but yet they can prove the stability of the
complete system. The majority of methods assume that the subsystems
are the stabilizing elements in the system while the coupling between
the subsystems is "the source" of instability. Concerning the applica-
tion of the Liapunov's direct method, two approaches can be recognized:
by application of scalar Liapunov's function [4] and by concept of Lia-
punov's vector function [1, 5, 6]. The concept of the Liapunov'svector
function assumes that the stability of each subsystem is represented
 209

by one scalar Liapunov's function, which can be used as a component of

the Liapunov's vector function. In this wayan aggregate model of the
system is obtained as a vector differential inequalit~ the order of
which is equal to the number of subsystems. The stability of the sub-
systems and of the aggregate model ensures the stability of the entire
system. Here, we shall not consider various methods for analysis of
the large-scale systems. There is a number of papers which elaborate
these methods and also a number of very good survey papers on these
topics [1, 4].

We shall restrict ourselves to only one method which can be efficient-

ly applied for the stability analysis of robotic systems. The robotic
systems in general meets the assumption that it can be considered as a
set of subsystems and these subsystems represent stabilizing elements
in the system. In principle, it is possible to decompose the robotic
system to subsystems in various ways. The most adequate decomposition
from the point of view of physical features of the system, is to con-
sider each joint of the robot and its actuator as a local subsystem.
Namely, as a local subsystem we may adopt one joint with its actuator
as it has been considered in the previous chapter. The coupling between
such subsystems is represented by dynamic moments (forces) which are
produced due to simultaneous motions of several joints. Such decompo-
sition is justified also from the point of view of the control synthe-
sis. The local controllers have been synthesized based on these sub-
systems (see Chapter 3).

Let us consider the model of the robotic system in the form of the mo-
dels of actuators (3.2.6) and the model of the mechanical part of the
system (3.2.2). Let us consider the model of deviation of the system
state around the desired (nominal) state X O (i.e. around the desired
position qO). Let us assume that the nominal control u O , which satis-
fies (4.3.2), is applied. Therefore it is fulfilled that xo=O. If the
control u O satisfies (4.3.2), then it must also satisfies the models
of actuators:

i=1 ,2, ... ,n (4 .A. 5)

where p~ is the moment around the i-th jOint when the robot is in the
state x
6 (x o1T , x o2T , ... ,x onT )T:

(4 .A. 6)
210

The model of deviation of the system state and control of the i-th ac-
tuator from the desired state x O and the corresponding nominal control
u O can be written in the following form:

lIx· i i=1,2, ... ,n (4 .A. 7)

where lIx i is the deviation of the state vector of the i-th actuator
from the nominal state (position) lIx i = xi_xOi, lIU i is the deviation
i i oi
of the i-th input from the nominal signal lIu = u -u , lIP i is the de-
viation of the driving torque from its nominal value, liP. = P.-P~, and
111
this deviation of the driving torque is described by the model of the
mechanical part of the system:

o .. 0 •
lIP i = Hi(q, lIq)lIq+h i (q, lIq, lIq), i=l, 2, ... ,n (4.A.8)

i oi
q -q

In the previous chapter we have considered the independent motion of

each joint of the robot and we have synthesized local controller for
each isolated actuator and joint. Each actuator and joint might be
considered as a subsystem. The model of the i-th joint motion is given
by (3.3.1). The model of the i-th actuator and the jOint is given by (3.3.4).
The model of deviation of the actuator and jOint around the nominal
point xOi, u Oi can be written, analogously to (4.A.7), in the form:

·i
lIx i=1,2, ... ,n (4 .A. 9)

The model (4.A.9) describes the motion of the i-th actuator and the
i-th joint when all the other jOint are kept locked. When the joints
are moving simultaneously the model of the state deviation around the
nominal is given by (4.A.7) and (4.A.8). By combining (4.A.9), (4.A.7)
and (4.A.8) the model of the deviation of the state of the robotic sy-
stem from the nominal can be written in the following form:

·i
lIx (4.A.10)

liP. (4. A.11)

1

i=1,2, ... ,n

where Hii has been already explained in relation to (3.3.4).

 211

It is very simple to show that the models (4 .A. 7), (4 .A. 8) and (4 .A.1 0) ,
(4.A.11) are equivalent.

The robotic system might be "decomposed" to subsystems which corres-

pond to "decoupled" joints and actuators. In other words, the system
might be observed as a set of n subsystems (4.A.9) which are intercon-
nected by f i 8P. where 8P. is given by (4.A.11). In this way we satisfy
1 1
the assumption that the robotic system can be decomposed to subsystems,
and therefore we may apply the above described procedure for stability
analysis of the large-scale systems. As we shall show in the text to
follow, it is also fulfilled the assumption that the local subsystems
(4.A.9) are stabilizing elements in the system, while the coupling
~i8P.1 is the destabilizing factor. However, on the contrary from some
other large-scale systems (for example power systems, economic systems
etc.) the subsystems of the robotic system might be "strongly" coupled.

Here, we shall present a method for analysis of asymptotic stabilityof

the robotic systems [7, 8] at the finite regions in the state space,
i.e. the method for estimation of regions of asymptotic stability of
system,which has been developed for general large-scale systems by
Weisenberger [9]. Namely, instead to examine the stability conditions
in the entire state space, which in general case is very complex due
to system nonlinearity, the system is considered at the finite regions
in the state space. In other words, the robotic system is considered
for the limited variations of the joints angles, velocities and the
rotor currents (i.e. for limited variations of the state coordinates).
Due to constraint upon the amplitude of the actuators inputs (4.3.4)
the system can be stabilized just in some bounded regions in the state
space (see Section 3.3.5).

We have assumed that the robot has to be positioned in the position qO,
i.e. we want to stabilize the robot around the state x o = (xo 1T , x 02T ,
... ,xonT)T, where x Oi = (qOi, O)T*). In this we shall assume that we
want to ensure positioning of the robot for some limited (allowed)vari-
ation of the joints angles, i.e. we shall assume that the variation of
the angle of the i-th joint must satisfy

*) If we consider the third order model of the actuator n.=3 the nomi-
nal state of the actuator is defined by xOi = (Oi
q , 0 ,1R
1.Oi)T where
i~i is the nominal value of the current in the rotor curcuit for
the given nominal positions qO of the robot joints.
212

(4.A.12)

where 6Q!ax is the maximal allow~d deviation of the angle of the i-th
joint from the given position qOl. This also means that the initial
position of the joint qi(O) must satisfy (4.A.12). Let us assume that
we have synthesized local controller around the i-th joint in the form
(static controller - see Section 3.3).

oi Kvi 6·q i (4.A.13)

u

Due to amplitude constraint upon the input (4.3.4) and limited varia-
tions of the joint angle (displacement) (4.A.12), on the basis of

(4 .A.14)

we may determine the contraint upon the velocity of the i-th joint

(4.A.15)

Constraints (4.A.12) and (4.A.15) define the finite set, in the state
space of the i-th subsystem, for which we have to ensure the robot po-
sitioning. In other words, we have to ensure stability of the robot
around the nominal point x Oi for all states in the finite region (in
the i-th subsystem state space) which is defined by (4.A.12) and (4.A.15).
This finite set of states (finite region) is presented in Fig. 4.A.1.
(similar to Fig. 3.17).

Let us denote by Xi the set of states given by:

X.
l
(4.A.16)

If we define such sets Xi for all n jOints of the robot, then for the
entire robot is defined the set X which represents the product of the
sets Xi corresponding to subsystems, i.e. in the state space of the
entire system, the region X is defined by:

(4.A.17)

The region (4.A.17) in the state space of the robot (the dimension of
this space is N) can be written as:
 213

II 1 IU 01 _K 1 l1ql_K 1 l1ql I 1
X {x: IlIq 1 I < qmax' P v
< u
m'

2 02 2 2 2·2 2
IlIq21 < lIqmax' lu -Kpllq -Kv llq I < u rn I •••

... ,llIqnl < II n IUon_K~lIqn_K~lIqnl < un} (4.A.18)

qmax' - m

Our task is to examine whether the robot is asymptotically (exponenti-

ally) stable in the region X around the imposed state XO if just the
local controllers are applied.

·i
i\q

i oi
[= urn +U

Fig. 4.A.l. Finite region in the state space of

the i-th local subsystem

We have adopted the robot decomposition into n-subsystems given by

hi -
(4.A.9) which are interconnected by the factors f lIPi. By this we have
performed the first step in the stability analysis of the entire non-
linear model of the robot. The second step is to examine stability of
the local subsystems if the interconnections between them are neglec-
ted. We have to examine whether or not the subsystem (4.A.9) is asym-
i
ptotically stable around the point lIX = 0 in the region Xi if the 10-
cal feedback loops are closed*) :

lIX i = AillXi-bik~lIXi = (Ai_bik~)lIXi (4.A.19)

l l

*) Here, we shall restrict ourselves to consider the case when ni=2and

when iust the static controllers are applied. The cases when n' =3 and
when the dynamic controllers are introduced are considered in 7, 8]. t
214

where as before k i = (K~, K;)T is the vector of the feedback gains. In

(4.A.19) we have ommitted the nonlinear constraint upon the amplitude
of the input due to assumption that the subsystem is observed at the
region Xi in which (4.A.16), or (4.A.14), is fulfilled.

In the previous chapter we have synthesized the local feedback gains

in such a way to ensure exponential stabilityof the subsystem (4.A.19).
Namely, the local controller ensures that all the poles of the closed-
-loop subsystem are on the left from the line Re(s) = -Si (in the s-
-plane). Therefore, it is clear that the subsystem is exponentially
stable with the exponential stability degree of Si' However, to analy-
ze the stability of the entire robotic system, we have to examine sta-
bility of the subsystem by introducing the subsystems Liapunov's func-
tions.

Let us select as a candidate for the Liapunov's function of the i-th

subsystem (4.A.19) the following function:

V. (4.A.20)
1

where H. is the n.xn. positive definite matrix. Let us consider the

1 1 1
derivative of this function along the solution of the closed-loop sub-
system (4.A.19):

T ·i
vi(along solution of (4.A.19»=(gradv i ) 6x

(4.A.21l

It can be shown that for the stability analysis of the entire system
it is the most convinient if the subsystems Liapunov's functions are
selected in such a way that it is satisfied*):

vi(along solution of (4.A.19» ~ -Sivi (4.A.22)

Therefore, we have to select the matrix Hi in (4.A.20) in such a way

that the derivative of the function vi along the solution of (4.A.19),
which is given by (4.A.21), satisfies the inequality (4.A.22). To ful-
fill this requirement we introduce a non-singular transformation nixni
matrix Ti such that

*) The proof of this statement can be found in [1].

 215

(4.A.23)

where Ai is the diagonal matrix given by

or A.
1
=t-o~ w~l-W 1
i
-0 1
i
(4 .A. 24)

i i i .
where -0 1 , -0 2 or -0 1 ± w~ are the eigen-values of the matrix.of.the

closed-loop subsystem, i.e. the eigen-values of the matrix (Al_blk~).

Since the closed-loop subsystem matrix (Ai_bik~)
1
is stable (i.e. a~l
its eigen-values are in the left part of the s-plane), it is possible
to determine the matrix T. which satisfies (4.A.23). For example, if
the eigen-values of the m~trix (Ai_bik~)
1
are real and not equal,
.
it
can be shown that the transformation matrix Ti satisfying (4.A.23) is
given by:

(4.A.25)

wh~re.sl' s2 are the eigen-vectors (of dimensions nix~) ?f the m~trix

(Al_blk~). The eigen-vectors satisfy the equations (Al-blk~)s.-o~s.=O,
1 A - 1 J JJ
j=l,2. Now, we can select the matrix Hi in the following way:

H. (4.A.26)
1

It is obvious that if we select the matrix Hi by (4.A.26), then it

holds:

2A.H.
1 1
(4.A.27)

If we substitute (4.A.27) into the expression for the first derivative

of the Liapunov's function, along the solution of the subsystem, (4.A.21)
we get

.
vi(along solution of (4.A.19)) I1x
iT
A.H.l1x
1
A

1-
i
Iv.1 <
-

< -minlo~lv.
J 1
<
-
-S.v.
1- 1
(4 .A. 28)

since it holds Si ~ min 10~1. Therefore, if we select the matrix Hi by

j=l,2 J
(4.A.26), we can ensure that the first derivative of the Liapunov's
function satisfies (4.A.22). This means that such selection of the
216

Liapunov's function ensures exact estimate of the subsystem stability.

Namely, since the solution of the differential inequality (4.A.22) is
given by V.<V. (0) exp(-S.t), based at (4.A.22), we may state that,
l l l 1/2'
(taking into account obvious inequalities Am (H.) IIt.xl(t) II < v. (t)
1/2 hi l l
and vi (0) < AM (Hi) IIt.x (0) II):

(4.A.29)

where AM is the maximal, and Am is the minimal eigen-value of the mat-

rix Rl.• The inequality (4.A.29) has the following meaning: the subsys-
tern is exponentially stable with the exponential stability degree Si'
This means that we have determined the Liapunov's function by which
the exponential stability degree of the subsystem is accurately esti-
mated.

Since we have assumed that the subsystem state vector t.x i belongs to
region (finite set) Xi (and therefore in (4.)1..19) and (4.A.21) we have
not taken into account the constraints upon the input amplitude), we
may state that the differential inequality (4.A.22) is satisfied for
the re9ion Xi' In other words, if the initial conditions are such that
(4.A.16) is satisfied, then the local subsystem is exponentially stab-
le with the stability degree 6 i .

Next, we have to estimate the region Xi by the Liapunov's function. To

do this, let us introduce the region (set) in the subsystem state spa-
ce X.l which is defined by:

(4 .A. 30)

where v io > 0 is the constant which has to be determined. In Fig. 4.A.2.

the regions X.l
are presented for various values of v . . The region X.
lO l
represents an estimate of the region X. if all points of the region X.
l l
belongs also to the region Xi' Therefore, all regions Xi which are in-
scribed in the region Xi (Fig. 4.A.2) might be considered as estimates
of the region Xi' The best estimate is the region which covers the
largest "area" (but, still, is inscribed in the region Xi)' Therefore,
we have to determine the largest number v io for which the region Xi
given by (4.A.30) completely belongs to the region Xi'
 217

I
I
I
I
I
I ;
I Vl~
10
>Vl! >V.
10 10

Fig. 4.A.2. Estimates of the stability regions

of the local decoupled subsystems

NOw, let us proceed to the third step in the stability analysis of the
entire robotic system. The selected Liapunov's functions candidates vi
guarantee stability of the local sUbsystems if the coupling between
them is completely ignored. (If the subsystems are actually decoupled
this will guarantee the stability of the entire system). We have to
estimate quantitatively the nonlinear coupling between the subsystems.
As we have already said, the coupling between the subsystems is given
... i - -
by f ~Pi where ~Pi is given by (4.A.11). According to the model of the
mechanical part of the system (4.A.11), the driving torques Pi are the
complex nonlinear functions of system state. Therefore to estimate the
coupling might be very complex job. It is obvious that:

lim ~Pi ~ 0 (4.A.31)

~x~O

since ~x ~ 0 means that ~qi ~ 0, ~qi ~ 0 and ~qi ~ 0 for all joints.
Therefore, we may determine the numbers ~ij which satisfy the inequa-
lities:

T~i - ~
(gradv<) f ~P< < L ~ .. v., i=1,2, ... ,n (4.A.32)
~ ~ j=1 ~J J

for all values of the state vector ~x which belong to the region X
(4.A.18). ~ve have to determine such number Sij which ensures that the
inequalities (4.A.32) are fulfilled for all possible values of the
state vector of the robotic system x belonging to the region X. In
this, the numbers Sij must satisfy:
218

~ ..
~J
>
-
0 for i '* j (4.A.33)

Determination of the numbers ~ij in general case is not simple due to

the fact that ~Pi is the complex nonlinear function of all state co-
ordinates of the robotic system. It will be shown in the text to fol-
low, that it is necessary to determine the least numbers ~ij which
fulfilltheinequalities (4.A.32). In the cases of simple robot struc-
tures the numbers ~ij might be estimated analytically. However, in the
general case the determination of these numbers requires implementati-
on of the digital computers. By application of computer programmes it
- *)
is possible to examine the values of ~Pi for "all" points in the re-
gion X in the state space of the system. In this way we can determine
the numbers ~ij which fulfill inequalities (4.A.32). Various numerical
procedures for searching along the region X and for determination of
the minimal numbers ~ij which satisfy (4.A.32), might be applied.

The numbers ~ij represent the quantitative estimates of the coupling

between the subsystems (i.e. between the robot joints motions).

At last, we proceed to the fourth step in the stability analysis - and

that is establishment of the sufficient conditions for the stability
of the entire robotic system using the estimates of the subsystems
stability and quantitative estimates of coupling. To examine the sta-
bility of the entire system, let us select the Liapunov's function can-
didate for the entire (interconnected) system (4.A.7) in the following
form [9]:

v max (v./v.) (4.A.34)

i=1,2, ... ,n ~ ~o

As we have explained before, the entire system (4.A.7) (or, (4.A.10),

(4.A.11» is asymptotically stable, if the first derivative of the se-
lected Liapunov's function candidate is negative along the solution of
the model of the system (i.e. for all states in the considered region).
Let us consider the first derivative of our Liapunov' s function candidate:

v max (v./v.) (4.A.35)

i=1,2, ... ,n ~ ~o

*) It is obvious that the exam~n~ng of ~Pi can be performed for a fi-

nite number of points in the region X, but since system is smooth
such approach is quite acceptable.
 219

where max denotes max vi of all vi' i=1,2, ••. ,n (and not the maximum
of the derivative vi)'

The first derivative of the Liapunov's function for the i-th subsystem
must satisfy:

T ·i
vi(along solution of (4.A.10» = (gradvi ) ~x

T' i 'i oi i T'i -

(gradv i ) [A~x +b N(u , ~u ) 1 + (gradv i ) f ~Pi (4.A.36)

Taking into account that the local control has been introduced in the
form (4.A.13) and that we consider only solutions of the system which
completely belong to the finite region X given by (4.A.18), we may
write:

vi(along solution of (4.A.10»

(4 .A. 37)

Based on (4.A.28) and (4.A.32) we may write:

n
vi(along solution of (4.A.10» ~ -Sivi + L sl.J
j =1
.. v J' (4.A.38)

Let us note that the expressions (4.A.38) are valid for all subsystems
(for each i=1,2, ..• ,n) if the system state belongs to the region X.
Let us restrict our consideration to the case when the system state
belongs to the region X given by

(4.A.39)

As we have already shown, the regions Xi are estimates of the finite

regions Xi' and therefore the region X is an estimate of the finite
region X. All states belonging to the region X must also belong to the
region X (but, vice versa does not hold). The region Xi is defined by
(4.A.30). Therefore, if the state belongs to the region X it must be
fulfilled:

vi < v io for i=1,2, •.. ,n (4.A.40)

220

It can be proved that if the following inequalities are fulfilled:

n
-a.v.
1 10
+ I E·
j=1 1J JO
.v. ~ 0 for each i=1 , ... ,n (4.A.41)

then it must hold:

for vi/v iO = max(v./v. ) (4.A.42)

j J JO

This means that if the conditions (4.A.41) are fulfilled, then the
following relation is then the following relation is also fulfilled
(taking into account (4.A.35) and (4.A.38»:

max (~./v.) < 0 (4.A.43)

v(along solution of (4.A.10» i=1,2, ... ,n 1 10 -

In other words, if the condition (4.A.41) is satisfied, then the first

derivative of the selected Liapunov's candidate (4.A.34) along the so-
lution of the model of the entire robotic system is negative for all
states belonging to the finite region X. Therefore, if the conditions
(4.A.41) are fulfilled the nonlinear model of the robotic system is
asymptotically stable around the imposed position qO, for all states
whithin the frame of the finite region X. The finite region X repre-
sents an estimate of the asymptotic stability region of the robotic
system.

The conditions (4.A.41) might be written in a more convinient form.

Let us introduce a matrix G of dimensions nxn whose elements are given
by:

(4.A.44)

where <5.. is the Kronecker's symbol (<5 •• = 0 for i*j, <5 •. = 1 for i=j). Let
1J 1J 1J T
us introduce an nx1 vector Vo which is given by v 0 = (v 10' v 20'···'v no) . Now,
the conditions (4.A.41) might be written in the following form:

Gv o < 0 (4.A.45)

The matrix inequality (4.A.45) represents the sufficient condition for

the robotic system to be asymptotically stable in the region X defined
by (4.A.30), (4.A.39), or, we may state that (4.A.45) is the suffici-
ent condition for the region X to represent an estimate of the asymp-
totic stability region of the system. If the condition (4.A.45) is
fulfilled we may guarantee that the model of the robotic system is as-
asymptotically stable around the desired set position qO for all ini-
tial states belonging to the region X. This means that all solutions
 221

of the nonlinear model of the robot starting from any state which be-
longs to the finite region X must terminate in the desired nominal
state x O , assuming that just the local controllers (4.A.13) are applied.

Even more, based on the above analysis we may estimate the speed by
which the robot positioning will be realized (i.e. how fast will the
robot be driven towards the desired position qO). Based on (4.A.35),
if follows:

v (x ( t» < v (x ( 0) ) exp ( - 11 t ) (4 . A. 46 )

where 11 is the degree of the exponential "shrinkage" of the region to

which the system state must belong during the transient process (during
positioning) and it is given by:

n
min 1- S . v. + I ~ .. v. I /v. (4 .A. 47)
i=l,2, ... ,n l lO j=l lJ JO lO

Based on (4.A.46) we may estimate that the robot state approaches the
imposed state X O by the speed which is higher than exp(-11t), where 11
is given by (4.A.47). Based on (4.A.46) it follows:

1/2 A
max AM (H.)
IllIx(t) II < i=l,2: ... ,n 1/2 Al IllIx(O) Ilexp(-11t) (4 .A. 48)
mln A (H.)
i=l,2, ... ,n m l

which means that the system is exponentially stable around the imposed
desired position with the exponential stability degree which is higher
or equal to 11 defined by (4.A.47). Now, we have just to check whether
it is fulfilled that 11>a, where a is desired exponential stability de-
gree.

Using the described procedure we are able to investigate whether the

selected local control ensures positioning of the robot in the desired
position when all joints are moving simultaneously. It is obvious that
we may use this method to synthesize the decentralized control (i.e.
for the synthesis of the local controllers) which will ensure the accu-
rate positioning of the robot when all the joints are moving simulta-
neously. This procedure for control synthesis will be considered in
Chapter 5.

If we consider the sufficient conditions (4.A.45) for the asymptotic

stability of the robotic system we may conclude the following: the
222

stability tests will be "relaxed"if the exponential stability degrees

of the local subsystems 6 i are greater (by absolute values), and if
the numbers ~ .. , which estimate the coupling between the subsystems
1J
(joints), are low. If we introduce the local controllers only, then we
cannot affect the coupling between the subsystems, but we can increase
the exponential stability degrees of the isolated subsystems. However,
the increase of the exponential stability degrees require increase of
the local feedback gains. The high feedback gains are unconvinientsin-
ce they might excite resonant structural oscillations of the mechanism
and since they amplify the noise in the system (see Chapter 3).

Regarding the presented method for the stability analysis, it is obvi-

ous that it is less conservative if we could obtain better (more pre-
cise) estimates 6 i and ~ij (i.e. if we may determine 6 i as highas pos-
sible and numbers ~ij as low as possible) .

It should be noticed that the numbers ~ij which fulfill (4.A.32) have
to be determined for the estimated regions X (4.A.39) and not for the
regions X (4.A.18), since the regions X are "inscribed" in the regions
X. By this we can obtain lower numbers ~ij' and the stability is any-
way proved just in the estimated regions X.

The asymptotic stability of the position control of robots, if just

local controllers are applied, might be proved in simpler way by ap-
plication of the Liapunov's method. Namely, starting from the dynamic
model of the robot it is possible to directly determine the Liapunov's
function candidate for the entire system which satisfy that its first
derivative is negative along the solution of the system, if just the
local controllers are applied for the robot positioning [10, 11]. Ho-
wever, this approach doesnot work when tracking of the nominal trajec-
tories is in question. Therefore, we presented the more complex agreg-
gate-decomposition method for the stability analysis also in the case
of position control of robots, since it can be easely extended to the
traj ectory servoing problem, as will be presented in the text to follow.

Let us briefly explain how we may directly prove asymptotic stability

of the position control of the robot when only local servos are appli-
ed. Let us consider the dynamic model of the mechanical part of the
robot (3.2.2) in the form:
 223

Let us consider the second order models of the actuators which might
be written in the following form:

i 1,2, ... ,n

All symbols have been described in Section 3.2.

Let us assume that local servo systems are applied which include per-
fect on-line compensation of gravity term, i.e. the actuator inputsig-
nals are computed as:

If we combine the model of the mechanical part of the system, models

of actuators and applied control we get model of the entire system in
the following form:

where J is the nxn matrix defined by J diag(N;N;J~), D is the nxn

matrix defined by:

Kv is the nxn matrix:

K
v

Kp is the nxn matrix:

We have to analyze asymptotic stability of the system described by

this model around the point qO, i.e. around the point 6q = O. Actually
since gO 0., qO = O. (position control is considered), we may write
the model of the system deviation around the nominal (desired) positi-
on as:
224

In order to analyze stability of this system let us select the Liapu-

nov function candidate for the entire system in the following form
[10, 11]:

Since both matrices Kp and [H(g)+J] are positive definite, the Liapu-
nov's function candidate is positive v > 0 for all 6q and 6q, except
for 6g = 0., 6q = O. The first deviative of this function along the
solution of the system model is given by:

+ 6g·T ( H (g) +J )..

6g + 21 6g·T··
H6g

At this point we shall exploit the relation which is well known from
theory of robot dynamics:

Using this relation, we get for the derivative of the Liapunov's func-
tion candidate:

This means that the first derivative of the Liapunov's function of the
entire system is negative for all system states (save for 6q = 0, but
6q * 0 for g * gO).

By this it is proved that the system is asymptotically stable if only

local controllers are applied. This explaines why the robot work well
whith the simple servo control. However, as already mentioned above,
if trajectory control is in question this analysis cannot be applied.

4.A.2 Analysis of asymptotic stability of trajecotry control

We have considered the problem of stability of the entire nonlinear

model of the robotic system around the prescribed position, i.e. in
the position control of robot. Now, we shall consider the problem of
 225

trajectory control of robotic systems, i.e. the problem of the system

stability around the specified trajectory. We shall assume that the
robot is controlled by local controllers synthesized for each jOint
independently, as it has been explained in the previous chapter. Howe-
ver, there are two possible variants regarding the feedforward term
which has to compensate for delay along the nominal trajectory. The
first solution is to introduce the nominal programmed control synthe-
sized on the basis of complete centralized dynamic model of the robot
(Fig. 4.7), as it has been explained in Section 4.4.1. The second solu-
tion it to apply local feedforward terms in the form of the local no-
minal programmed control synthesized on the basis of the models of iso-
lated actuator and joint (i.e. the model of the local subsystem), as
it has been presented in Section 4.4. (Fig. 4.4).

Here, we shall consider the case when the centralized nominal program-
med control is applied.

Let us define the stability conditions of the system around the impo-
sed nominal trajectory. Here, we shall consider so-called praatiaal
stabi lity of the robot around the specified nominal traj ectory. We shall
not present in detail various definitions of the practical stability
of the system, since they can be found in the literature [12J. We shall
adopt a definition which is adequate for requirements appearing in ro-
botics. Let us assume that the nominal trajectory of the state vector
of the robot xO(t), tE (0, ,) is given. liVe assume that the trajectory
xO(t) is continual with respect to all coordinates of the state vector
x. Let us assume that the maximal deviation of the actual initial sta-
te x(O) from the nominal initial state xO(O) is defined by the positi-
ve number Xl such that

-I
Ilx(O) - xO(O) II < X (4.A.49)

The robotic system is practically stable around the specified nominal

trajectory xO(t) if for each x(O) which fulfilles (4.A.49) the actual
trajectory x(t) fulfilles the following condition

Ilx(t) - xO(t)11 2. xtexp(-at) , tE(O, T) (4.A.50)

where Xt>XI and a are positive numbers independent of Xl. This defini-
tion of the practical stability of the system in essence represents
the exponential stability of the system around the nominal trajectory
at the finite time interval and at the finite region in the state
226

space. Our task is to examine if the synthesized control ensures that

for all initial states of the system which fulfill (4.A.49) the actual
trajectory of the system satisfies (4.A.50).

Let us consider the case when the nominal control uO(t), tE(O, T) is
applied, which is synthesized based at the centralized model of the
robot, i.e. let us apply the control uO(t) which satisfies (4.4.11).
The task is to examine the stability of the model of deviation (4.4.13).
The practical stability of the robot around the specified trajectory,
when the nominal centralized control is applied, might be considered
as the problem of the exponential stability of the deviation model. Na-
mely, for the model of the state deviation (4.4.13), the followingsta-
tement holds: 6x=O and 6u=O implies 6X=O. This means that 6x=O is the
equilibrium point of the model of the state deviation. The conditions
of the practical stability (4.A.49), (4.A.50) of the robotic system around
the nominal trajectory xO(t) correspond to the following conditions of
the exponential stability of the model of deviations. If the model of
deviations (4.4.13) is exponentially stable around the point 6x=O with
the exponential stability degree greater or equal to a for all initial
conditions which satisfy

I 16X (0) I I .::. xl (4 .A. 51)

then the actual trajectory of the robot meet the condition (4.A.50) for
all initial states fulfilling (4.A.49), and therefore the robot is prac-
tically stable around the specified nominal trajectory. This means that
if we prove that the model of the state deviations around the nominal
(4.4.13) is exponentially stable around the point 6x=O, then we may
state that the robot is practically stable around the imposed nominal
trajectory in the sense of conditions (4.A.49), (4.A.50). We have to
note that, in the general case, the exponential stability of the model
of deviations is stronger condition than the practical stability of the
robot around the specified trajectory. However, it is simpler to exa-
mine the exponential stability of the model of the state deviations,
and, therefore, we shall apply this procedure: we shall examine the ex-
ponential stability of the model of the state deviations (4.3.22) and
by this we shall check if the robot is practically stable around the
set nominal trajectory.

Therefore, we have to examine the exponential stability (with the pre-

scribed. stability degree a) of the model of the state deviation around
 227

the point ~x=O for all initial conditions which belong to the finite
region defined by (4.A.51). We shall examine the system stability if
the nominal centralized programmed control and the local servo systems,
synthesized for each actuator and joint independently (see Chapter 3) ,
are applied:

(4 .A. 52)

Let us consider the case when the static local servos are applied. The
stability analysis of the deviation model can be carried on analogous-
ly to the procedure presented in Appendix 4.A.1. for the case of robot
position control. The application of the agreggation-decomposition me-
thod for stability analysis of the complex nonlinear system is perfor-
med according to the steps described in the previous section.

First, we shall consider the model of deviation (4.4.13) in an approxi-

mative form as a set of decoupled subsystems each corresponding to one
jOint and its actuator, in the form (4.A.9), where, now, ~xi represents
the deviation of the subsystem state vector from the nominal trajecto-
ry ~xi(t) = xi(t)_xoi(t), and uOi(t) denotes the nominal programmed
control synthesized at the basis of the complete model of the system
(4.A. 21):

(4 .A. 53)

Let us examine the stability of the decoupled subsystems (4.A.53) (in

which couplings are ignored) if the static controllers are applied

(4.A.54)

As in the previous case, the nonlinearity of the amplitude saturation

type upon the actuator input N(u Oi , ~ui) bounds the region in the sta-
te space in which the system can be stabilized. The amplitude constra-
int upon the input ~ui defines the set of points in the state space
(the region):

(4.A.55)

The bounderies of this region are obviously time dependent. Instead

"the time-varaying region" defined by (4.A.55) we shall adopt the re-
gion which is inscribed in (4.A.55) and which is defined as
228

X. max u Oi (t) } (4.A.56)

1.
tE (0, T)

However, it should be remembered that we want to examine exponential

stability of the system for the finite region of the initial states
which is defined by (4.A.51). The condition (4.A.51) bounds the region
by all state coordinates of the robotic system. We may assume that by
the condition (4.A.51) the finite region of the subsystem (4.A.53) is
also defined, so that the subsystem is considered at the region given
by:

X~1. i=1, 2, ... ,n (4.A.57)

-I -I
where the constraint Xi might be adopted as Xi

Similarly we may assume that the region (4.A.50) might be presented in

the "decoupled" form, i.e. by the regions in the subsystems state spa-
ce:

Ilxi (t) _x oi (t) II < x~exp (-at) }

1.

where xt are the numbers which might be calculated as x~

1. 1.

The set of states X~ represents the region of the initial conditions

1.
for which the subsystem stability should be guaranteed. Let us assume
that X~ is the subset of X. given by (4.A.56). In other words, let us
1. 1.
assume that for all initial states of the subsystems for which we want
to examine the system stability, the control u i (4.A.54) does no vio-
late the constraint upon the actuator input amplitude *). This means that
for all initial states belonging to the region (4.A.57) we may assume
that the subsystem behaves as linear and that the nonlinearity
N(uOi(t), 6U i ) might be ignored:

(4.A.58)

The stability of the subsystem (4.A.58) will be examined as presented

in the previous section. The Liapunov's function candidate for the
subsystem (4.A.58) is adopted in the form:

v. (4 .A. 59)
1.

*) The case when the set of states (4.A.57) is not the subset of
(4.A.56) we shall not consider. The solution to this case can be
found in [8].
 229

where the matrix H.l. can be computed according to (4.A.27). In this ca-
se the first derivative of the Liapunov's function for the decoupled
subsystem (4.A.58) satisfies the condition:

(4 .A. 60)
vi(along the solution of (4.A.58)) ~ -Sivi

where Si is the exponential stability degree of the decoupled subsys-

tem (i.e. the eigen-value of the closed-loop subsystem matrix (Ai -
bik~) which real part has the least absolute value. The espression
l.
(4.A.60) is valid for all initial states of the subsystem which belong
to the finite set (4.A.57).

The set of initial states of the subsystems X~ might be estimated by

l.
the subsystems Liapunov's functions, as described in the previous sec-
tion. The region Xi which estimates the region xi is given by (4.A.20),
where the highest numbers v io have to be determined, such that all po-
ints in the region Xi belong also to the region xi (i.e. the region
I
Xi must be "inscribed" into the region Xi) .

The next step in the stability analysis is to examine the coupling be-
tween the sUbsystems (4.A.53). The coupling between the subsystems is
given by ti~Pi where ~Pi is given by (4.A.11), i.e.

(4 .A. 61)

It is obvious that it holds:

lim ~P.+O (4 .A. 62)

l.
~x+O

The condition (4.A.62) is fulfilled since we assume that the applied

nominal centralized control uOi(t) "perfectly" compensates for the no-
minal moments P~, and therefore the coupling between the subsystem
(4.A.53) represents the deviation of the actual driving torques from
the nominal moments. When the actual state of the robot approaches the
nominal trajectory xO(t) (i.e. when Ilx+O), then the actual moments Pi
must convergate to the values of the nominal moments P~(t), and, the-
l.
refore, the coupling LIP. must convergates towards zero. Due to (4.A.62),
l.
we can determine the numbers ~ .. which satisfy the following inequali-
l.J
ties:

for tE(O, T), i=1,2, ... ,n (4.A.63)

230

This means that the numbers ~ij have to be determined such that the
inequalities (4.A.63) are fulfilled for all deviations from the nomi-
nal trajectory xO(t) which satisfy the conditions (4.A.30). In deter-
mining the numbers ~ij we have to examine the deviation of the moment
Pi from P~(t) for "all" pOints along the nominal trajectory xO(t), i.e.
not just for the initial state xo(O), but along the "whole" trajectory,
for "each" tE(O, T).

We must keep in mind that the practical stability of the robot requi-
res that the system state belongs to the region given by (4.A.50),
(i.e. the region to which the actual state of the robot must belong is
exponentially "shrinking" around the nominal trajectory). Therefore,
the inequalities (4.A.63) must be fulfilled for all states around the
nominal trajectory which belong to the region (4.A.50). This region
might be estimated by the following region (expressed by the subsys-
tems Liapunov's functions):

(4.A.64)
X (t) = X1 (t) xX 2 (t) x ... xXn (t)

It is obvious that X(t) (4.A.64) is an estimate of the region (4.A.50),

since the numbers v. are determined in (4.A.30) so that all pOints of
X. must belong to X~~
on the other hand, it must be satisfied that
-t -I ~ -t ~I -
X >X , and therefore Xi>X i ' and Xi(t) is the subset of Xi(t).

It is clear that if ~ij are determined numerically by digital computer,

it is possible to examine the deviation of P. from P~(xo(t» for a fi-
~ ~

nite number of points at the nominal trajectory. In other words, in-

stead to determine the numbers ~ij by examination of the moments in
the robot jOints around the specified position, as we did in the pre-
vious case, here we have to examine the deviations of the moment from
the nominal moments along the nominal trajectory. This might be perfor-
med by determination of the coupling 6Pi(6X) for "the sufficient num-
ber of states" 6X (fulfilling the condition (4.A.64», and around "the
sufficient n.umber of points" at the nominal trajectory x O (0), x O (t 1 ) ,
... ,xo(t~). It is possible to establish an algorithm which will search
for the least numbers ~ij which satisfy the inequalities (4.A.63), by
determination of the values of the coupling 6Pi(6X).
 231

Once the numbers Bi , estimating the exponential stability degrees of

the decoupled subsystems, and the numbers ~ .. , estimating the actual
1)
coupling between the subsystems, are determined, we may apply the same
procedure as before for the stability analysis. It is easy to show that
if the following condition is fulfilled

(4.A.65)

where the nxn matrix G is given by:

(4.A.66)

and the n x 1 vector Vo is given by Vo = (v 10 ' v 20 , ... ,v no )T, then the

model of the deviations (4.4.13) is asymptotically stable around the
point ~x=O for all states belonging to the finite region X. Even more,
if the test (4.A.65) is fulfilled then we may say that the model
(4.4.13) is exponentially stable around the point ~x=O for all states
in the region X, and the exponential stability degree can be estimated
by:

n
n . min I-Siv io + L~' .v. I/v. (4.A.67)
1=1,2, .•. ,n j::1 1J)0 10

If n ~ a is fulfilled we may state that the model of deviation is ex-

ponentially stable around the point ~x=O with the prescribed exponen-
tial stability degree in the finite region X. As we have presented
above, the exponential stability of the model of deviation is suffici-
ent condition for the robotic system to be practically stable around
the specified nominal trajectory. This means that the analysis of the
practical stability of the robot around the nominal trajectory has been
red.uced. to testing of the condition (4.A.65) and determination of the
number n which fulfilles (4.A.67). If the test (4.A.65) is satisfied
and if n ~ a, then it can be guaranteed that the model of the robot is
practically stable around the nominal trajectory, but just in the es-
timated. regions (4.A.64). The procedure for the stability analysis
!IIight be used. to establish an algorithm for iterative estimation of the
region X in which the system is practically stable. The numbers v io in
(4.A.64) a.nd. ~ij in (4.A.63) have to be iteratively determined until
"the largest" region in which the robot is stable is determined. If the
estimated region of the practical stability X(O) completely "covers"
the region (4.A.51) we may guarantee the practical stability of the
nonlinear model of the robot.
232

We have to note once again that the basic drawback of the presented
procedure for the stability analysis of the robot lies in its conser-
vativity. The condition (4.11..65) might be too conservative. If the test
(4.11..65) is not fulfilled, the robot still might be practically stable
around the specified trajectory. Therefore, in analyzing the robotsta-
bility by the presented procedure we must carefully interpret the re-
sults of the test. In estimating of the stability degrees of the local
subsystems (the numbers Si) and in estimating of the coupling between
the subsystems (the numbers ~,,) we must try to get the best possible
1.J
estimates (i.e. to determine the largest numbers Si satisfying
(4.11..60), and the least numbers ~" satisfying (4.11..63».
1.J

References

[1] Biljak D.D., Large Scale Dynamic Systems, North Holland, 1978.

[2] Chen C.T., Linear System Theory and Design, Holt, Rinehart and
Winston, New York, 1984.

[3] Liapunov 11..1., Selected Papers, (in Russian), edition of Academy

of Science SU, editor V.I. Smirnov, 1948.

[4] Voronov A.A., "The State of the Art and Problems of the Theory of
Stability", (in Russian), Automatica and Remote Control, no. 1,
1983.

[5] Bellman R., "Vector Liapunov Functions", SIAM Journal of Control,

1, 32-34, 1962.

[6] Bailey F.N., "The Application of Liapunov's Second 11ethod to

Interconnected Systems", SIAM Journal of Control, 3, 443-461
1966.

[7] Vukobratovic ~1., Stokic D., "Contribution to the Decoupled Con-

trol of Large-Scale Hechanical Systems", Automatica, No.1, 1980.

[8] Vukobratovic M., Stokic D., Kircanski N., Non-adaptive and Adap-
tive Control of Hanipulation Robots, Honograph, Series: Scienti-
fic Fundamentals of Robotics 5, Springer-Verlag, Berlin, 1985.

[9] ~'leissenberger S., "Stability Regions of Large-Scale Systems",

Automatica, Vol. 9, 653-663, 1973.

[10] Takegaki M., Arimoto 5., "A New Feedback Hethod for Dynamic Con-
trol of Manipulators", Trans. of the ASME, Journal of Dynamic
Systems, Heasurement and Control, Vol. 102, No.2, 1981.

[11] Arimoto S., Miyazaki F., "Stability and Robustness of PID Feed-
back Control of Robot Hanipulators of Sensory Capability", The
First Intern. Symposium on Robotics Research, 1983.

[12] Michel N.A., "Stability, Transient Behaviour and Trajectory Bo-

unds of Interconnected Systems", Int. Journal of Control, Vol.
11, No.4, pp. 703-715, 1970.
ChapterS
Synthesis of Robot Dynamic Control

5.1 Introduction

As we have explained in previous chapter, during simultaneous motions

of robot joints there appear dynamic forces (torques) which act around
the joints axes and affect the performances of local servo systems. To
ensure accurate tracking of nominal trajectories (imposed by the hig-
her tactical control level) we have to compensate for the effects of
these dynamic forces. If the executive control level has to ensure only
positioning of the robot hand in various pOSitions in work space, or to
ensure tracking of "slow" trajectories of the robot hand, then the ef-
fects of these dynamic forces might be relatively weak and they might
be ignored. Therefore, the local servos might accomplish such control
tasks. In the previous chapter we have shown how we can examine whether
or not the local servos could ensure accurate tracking of desired tra-
jectories. If local servo systems can not "overcome" the effects of dy-
namic forces, we must introduce additional control loops which take
into account these dynamic forces. Such control law, which accounts
for dynamic characteristics of robotic systems is called dynamic con-
tro'l of robots.

In the previous chapter we have already considered one possibility to

introduce dynamic control law. We have introduced centralized nominal
control which takes into account dynamics of rObot, but only at the no-
minal level. We have considered adventages and drawbacks of this type
of dynamic control.

One of the problems in introducing of dynamic control lies in comple-

xity of dynamic model of robot, and therefore, implementation of con-
trol which has to compensate for dynamic effects, might be complex,
too. Due to this, our aim is to synthesize dynamic control which is
the Simplest possible, but which meets imposed requirements.

In this chapter we shall consider several forms of dynamic control

laws. First, we shall consider synthesis of global dynamic control
which has to compensate directly for the effects of dynamic forces
upon performance of local servos. We shall consider various forms of
234

such dynamic control. However, the synthesis of dynamic control is com-

plex due to complexity of dynamic model of robot, and, therefore, it is
difficult to realize this synthesis without the aid of a digital compu-
ter. The computer-aided synthesis of control for robot is often applied
today. These algorithms, set at a computer, have to help a robot deSig-
ner to select the simplest control law which still satisfies the requi-
rements regarding the accuracy of trajectory tracking. In this chapter
we shall consider such a computer-aided procedure for synthesis of dy-
namic control of robots.

In this chapter we shall consider yet another approach to dynamiC con-

trol of manipulation robots. We shall consider well-known procedure for
control synthesis using so-called "computed torque method". We shall also
consider Cartesian based control of robots. In Appendix we shall brief-
ly present a well-known procedure for synthesis of control for complex
systems by centralized optimal regulator which is interesting more from
the theoretical standpoint than for its real applicabili ty in robotics.

5.2 Synthesis of Global Control

In the previous chapter we have presented how, by stability analysis,

we can test whether local servo systems can meet the requirements re-
garding the accuracy of tracking of imposed nominal trajectory. If lo-
cal servos can meet the imposed requirements, there is no need to in-
troduce any additional control loops. However, if local servos do not
guarantee sufficiently accurate positioning,or tracking of trajectori-
es we must either re-select local servo systems, or we must introduce
additional global control. The re-selection of local servos ussualy
means increasing of the stability degrees of the local subsystems. To
meet requirements regarding the stability of the over-all system, it
is required to increase the stability degrees of subsystems Si. Howe-
ver, increase of the stability degrees Si means that the servo feed-
back gains are to be increased. The high feedback gains are not accep-
table since they might excite resonant structural oscillations and due
to effects of noice in servo systems (see Section 3.2.2). Therefore,
this solution (by increasing of Si) might be applied up to certain li-
mit. If we cannot increase any more the local feedback gains, and the
required accuracy in tracking of nominal trajectories is not achieved
yet, we must introduce additional global feedback loops.
 235

Local servo systems are supplied with information (feedback loops) on

the state (position, velocity) of the corresponding local subsystem
(joint) only. Dynamic forces (moments) which act upon the servo are
functions, in general case,of all state coordinates (angles, velociti-
es and accelerations of all jOints). To compensate for these forces we
have to introduce not only the local feedback loops, but also global
cross-feedback loops. The purpose of these cross-feedback loops (from
one subsystem (joint) to another) is to compensate for dynamic forces
and ensure satisfactory tracking of the nominal trajectories of all
robot jOints.

First, let us consider the case when centralized nominal control, syn-
thesized using the complete model of robot (Fig. 4.7), it introduced.
The control already compensates for the nominal dynamic moments pO(t)
*) 1
(4.4.12) .

However, during the realization of the imposed trajectories qOi(t) the-

re appear deviations of the actual trajectories of joints from nominal,
and dynamic moments also deviates from nominal driving torques P~(t).
1
As we have already explained, the model of deviation of the dynamics of
the mechanical part of the system from "the nominal dynamics" might be
written in the form:

(5.2.1)

The model of deviation of the i-th actuator from the nominal dynamics
(i.e. around the nominal trajectory xO(t), and nominal centralized con-
trol U O (4.4.11» might be written in the form:

·i
t,x i=1,2, ... ,n (5.2.2)

where t,x i xi_xoi is the deviation of the actual state vector of the
i-th actuator around the nominal trajectory xOi(t), t,u i = ui_u oi is
deviation of the i-th input signal from the nominal control signal
uOi(t), and t,P. = P.-P~ is deviation of the i-th driving torque from
111
the nominal driving torque.

In chapters 3 and 4 we have synthesized local controllers in the form

of local servo systems around the individual joints which stabilize

*) We shall consider tracking of the nominal trajectories, and it can

be easely reduced to the case of position control of robots.
236

subsystems (actuator and joint) given by (3.3.4). The models of devia-

tions around the nominal trajectory and control xOi(t), uOi(t) for such
decoupled subsystems (in which actual coupling has been neglected) are
given by:

(5.2.3)

Local controller has been selected in the form of linear feedback con-
trol (in the case of static controllers):

(5.2.4)

where k i is the vector of feedback gains in the i-th servo. Local servos
(5.2.4) ensure stabilization of local decoupled subsystems (5.2.3),
i.e. they guarantee that the closed-loop local subsystems:

(5.2.5)

are exponentially stable around the nominal trajectory xOi(t), uOi(t).

(In Eq. (5.2.5) the amplitude constraint upon the actuator input has
been ignored).

Thus, the local servos guarantee stability of subsystems (5.2.5) if they

were decoupled from each other. However, it is obvious that the actual
subsystems are not decoupled. The models (5.2.3) do not represent ac-
curate model of the complete actual system, but just very approximati-
ve model. The model of the actual system (5.2.2) might be written in
the form

(5.2.6)

where ~Pi is given by

(5.2.7)
- ··i
In (5.2.7) the dynamics of the joint (represented by Hii~q ) has been
"submitted" from ~Pi since it has been included in the model of sub-
system (5.2.3) and compensated by local control. The difference betwe-
en the accurate model (5.2.6) and approximative model formed of decoupled
subsystems (5.2.3), is in the term fi~Pi. Obviously, the nominal pro-
 237

grammed control (4.4.11) and the local control (5.2.4) do not compen-
sate for this term. This term represents interconnection (coupling)
between the subsystems (5.2.3) which has not been taken into account
neighter in nominal programmed control synthesis, (4.4.11), nor in
synthesis of local control (5.2.4). If the effects of this term are
strong, we have to introduce additional global control in order to com-
pensate for these effects.

Therefore, we introduce dynamic control in the following form:

(5.2.8)

where 6U? is the scalar global control which has to compensate for co-
l
upling between subsystems (5.2.3), i.e. to compensate for the effects
of the term f i 6P,. This term is given by (5.2.7) which obviously re-
1
presents dynamic forces which act upon the i-th joint due to the move-
ments of the other joints of the robots. Actually, this term represents
deviation of the dynamic forces from their nominal values. Since these
dynamic forces 6Pi enter linearly into the model (5.2.6), the global
control can be introduced in the following way. Let us assume that 6Pi*
represents some scalar function, measured or calculated, which corres-
ponds to deviation of the dynamic forces from nominal forces 6P i , i.e.
which satisfies the following inequality, (for all pOints in the state
space around the nominal trajectory xO(t»:

16P~1 (x) I-< 16P,1 (x) I,

(5.2.9)

where sign() denotes the sign of the value in brackets. Thus, the glo-
bal control in the i-th joint (servo system) might be introduced in the
form [1]:

(5.2.10)

-G i
7
where by Ki (6x ) is d noted the scalar function of the state vector of
the i-th subsystem 6Xl. The function K?(6x i ) might be selected in va-
l
rious ways.

The aim is to ensure compensation for the coupling, i.e. for the dyna-
mic forces which act upon the i-th joint. If this global control is
238

applied in the i-th controller, then the model of deviation of the sy-
stem state from the nominal becomes:

(5.2.11 )

where, for the sake of simplicity, the amplitude constraint upon the
input signal has been ignored. It is obvious that K~(~xi) has to be
selected in such a way as to minimize the term

Let us assume that the model of the i-th actuator is of the second or-
der (n i =2). Then, the vectors fi and hi are in the following forms:

(5.2.12)

If we consider the model of the i-th sUbsystem (5.2.11) with the ap-
Ai -
plied global control, it is obvious that the effects of coupling f ~Pi
is compensated for, if the global control is introduced in the follow-
ing form:

(5.2.13 )

where K~>O is scalar global feedback gain. The selection of this glo-
~

bal gain will be adressed in the text to follow. NOw, the system model
becomes

(5.2.14 )

where amplitude constraint upon the input signal has been again igno-
red. The effect of coupling fi~p, has been reduced to fi(~p._K~~P~),
~ * ~ ~ ~
taking into account that the function ~Pi fulfilles (5.2.9). The sche-
me of such control (5.2.8), (5.2.13) is presented in Fig. 5.1.

However, such global control directly compensates for the coupling be-
tween the subsystems (i.e. the dynamic forces acting upon the i-th jo-
int), only if we adopt the second order models of actuators as suffi-
ciently accurate. The second order model of actuator is approximative
since it ignores the delay between the actuator input and the driving
torque (force produced by the actuator). If this delay cannot be igno-
red, the introduced global control cannot efficiently compensate for
 '"'l
.....
LQ

l.n K~f1 /6 1

(l(')
o 0
::s ::s
rTrT
t1 t1
00
~~
. III COMPUTATION P1
~(l

(l (1) OF NOMINAL MECHANICAL

~
0""EI
~(1) CENTRALIZED PART OF
(l .....
o ::s
PROGRAMMED ROBOT
--
::s (l xon
rT~ CONTROL
t1 ~ Pn
o 0..
~ .....
~::s
(1)LQ
t1
III ::s
o
~ EI
::s .....
o..::s
~
LQ~
~
o (l
tf(1)
~ ::s
~rT
t1
(l ~
o ~
::s .....
rTN
t1 (1)
00..
~

~
240

dynamic forces. Therefore, in the synthesis of global control we have

to take into account this delay between the input signal u i and the
driving torque p~). One of methods to solve this problem is considered
in Appendix 5.A.

In the synthesis of the global control we have to select the scalar

gain K~ in (5.2.13). This is the global feedback gain. It is obvious
that the compensation for the coupling between the subsystems (i.e. the
compensation for the dynamic forces acting upon the i-th subsystem),
depends on the selection of K~. It is clear that if we select higher K~
1 1
then the global control will compensate for "larger portion" of coup-
Ai -
ling f ~Pi. However, if we assume that we have realized the function
6P *i in such a way that it satisfies 6P * -
i = 6P i , then it is obvious that
the global gain K~ must be less than 1 (i.e. K? < 1), otherwise an
1 1 -
"overcompensation" will appear. If we select K~ to be equal 1 (K~=1),
1 1
the global control would compensate for total coupling (in ideal case,
if the delay in actuator can be ignored and if it is fulfilled 6P *
i =
6P i ). However, since we can never ensure such an ideal compensation that
6P~ = 6P., the global control might cause oscillatory behaviour of the
1 1
system (specially in close suroundings of the nominal trajectory when
6P i is very small). On the other hand, too high feedback gains are not
acceptable, due to reasons already explained in Chapter 3. Therefore,
we must select the global gain to be as low as possible,but to ensure
the stabilization of the robot. In the global control synthesis we have
to analyze the stability of the robot with the selected global control
(see Appendix 5.A), and determine the minimal gains K~ which ensure de-
1
sired stability of the robot.

Up to this point, we have considered the case when the global control
is applied together with local servo systems and the centralized nomi-
nal control. We have explained that the centralized nominal control
suffers from certain drawbacks. Thus, instead the centralized nominal
control we may apply local nominal control together with local servos,
which is much simpler for implementation (Fig. 4.4). However, the lo-
cal nominal control does not compensate even for the nominal dynamic
forces. Therefore,the effects of these forces upon the servo systems
performances are considerably stronger than in the previous case. This

*) In D.C. electro motors this delay is defined by the electrical con-

i
stant TR of the rotor curcuit. This constant is often very small
but it cannot be always ignored, specially in the case of large
D.C. motors having massive windings with high inductances.
 241

means that it is much"harder"to ensure accurate tracking of nominal

trajectories (or positioning), if just local nominal control is appli-
ed. Further, it is necessary to introduce global control which has to
compensate not only for the difference between the actual dynamic co-
upling and the nominal coupling, but for the complete dynamic forces
(moments) which load the local servo systems.

The global control might be introduced in an analogous way to the pre-

vious case. The only difference is that instead of function * which
~Pi

corresponds to the deviation of the dynamic forces from the nominal

forces ~Pi
- (5.2.7), here we must introduce a function Pi* which corres-
ponds to total actual dynamic forces Pi (3.2.2). We select the functi-
on Pi* to satisfy the following inequality for all states around the no-
minal trajectory:

(5.2.15)
sign(P *i (x» sign (P i (x) )

*
If such function Pi(x) is introduced, the global control might be re-
presented in the form (5.2.10):

~u~ (5.2.16)
l

The global control might get particular form which is analogous to

(5.2.13) :

~u~l (5.2.17)

Here K~>O again denotes scalar global gain. Next, we have to examine
directly the practical stability of the system if the following control
is applied:

(5.2.18)

i.e. if together with local nominal control and local servo feedback
loops we apply the global control (5.2.17). As we have already said,
the analYpis of the practical stability of the robotic system is out of
the scope of this book, and it can be found elsewhere [2J.

So f~r we have not explained yet how we can realize the functions *
~Pi

or Pi' which play the central roles in the global control law. The
242

realization of these functions represents the basic problem in global

control synthesis. We shall consider two possible realizations of the-
se functions, i.e. of the global control: via force feedback and by
on-line computation of dynamic couplings. We shall consider the case
when the local servo systems, the feedforward terms in tne form of the
local nominal control and the global control in the form (5.2.16) are
applied, since such control is much more applicable for industrial
robots. However, all considerations can be easely extended to the con-
trol law which includes the feedforward terms in the form of the cen-
tralized nominal control and the global control in the form (5.2.10).

5.2.1. Force feedback as global control

.,
The function Pi has to correspond to dynamic forces Pi' which act upon
the servo system as coupling from other jOints. Dynamic force (moment)
Pi which acts upon the i-th jOint can be directly measured using force
transducers. If we implement force transducer in the joint we may ob-
tain direct information on the effects of coupling upon this joint. In
.,
that case the function Pi can be made equal to the actual ~ynamic mo-
ment (force) P., taking into account "joint dynamics"
~
ii ~~
.. q~

P.
~

In other words, the global control directly compensates for dynamic

forces, which are measured by sensors. If we introduce global control
in the form (5.2.17) and if we assume tnat the actuator is of the se-
cond order ni~2, and if we adopt K~=1, then by force feedback we would
obtain the total compensation of coupling, i.e. the system would be
ideally deaoupZed to subsystems (local servos). By introducing for-
ce feedback we achieve that the system behaves as a set of local sub-
systems among which there are no interconnections (i.e. the subsystems
would behave as we have considered in Chapter 3. when we have assumed
that, except the i-th jOint, all the other jOints are locked). Actually,
even force feedback cannot completely decouple robotic system: due to an
amplitude constraint upon the actuator inputs, the global control by
force feedback is also constrained, and therefore for large deviations
from the nominal trajectories it cannot completely compensate for the
actions of dynamic moments. On the other hand, information from sensors
are not perfectly equal to actual dynamic moments since they always
include noice.
 243

The control scheme which includes, becides local nominal control and
local servo systems, the global control in the form of force feedback
is presented in Fig. 5.2. The force feedback, as global control, has
certain adventages:

a) Simp~e controZ structure. By introducing of force feedback we do not

complicate significantly the control structure in respect to decen-
traZi:zed' structure of control (Fig. 4.4). Practically in the joint
for which the global control is required, we introduce only one new
feedback loop-a feedback by force measured by force transducers.

b) Minimal computation. Force feedback does not require additional com-

putation since information from the force transducers gives total
information on coupling. It is sufficient to multiply information
from force transducer by global gain. (It is also required to sub-
mitte from measured moment P. the value
1
H,11,qi). This means that such
global control does not require additional computation time in a
control microcomputer.

c) Robustness to parameter variations. Since force transducers measure

the total dynamic forces, this information does not depend on the
parameters of the robot mechanism and actuators. If some parameters
of the mechanism var~ this global control ensures compensation for
dynamic forces regardles to parameters variations. In other words,
the force feedback guarantees robustness of the control system to
variations of parameters of the mechanism (for example, to varia tion of
payload parameters, see Chapter 6), and to uncertaini tes of robot model-
ling.
However, the application of force feedback also suffers from certain
drawbacks:

a) Technical problems of force transducers implementation. To measure

directly the forces acting in the joint, we have to build the force
transducers directly in a shaft of actuator (reducer), or at some
other place on the mechanism. Low stiffness of the force transducers
might produce serious problems since it reduces the structural stif-
fness of the mechanism but it has been shown that these problems ca,n
be successfully solved [3, 4].
244

to
Ie..
~

e..

I
s:- I r--;::-

~
lil
to
Qj
II>
I
I
I
C
0
II)

Qj
II)
Qj
u
s-
I Qj
u
I s-
-
0

I
~
~

....J
<C ~
U 0 I-
~

Z 0
<C I- <Xl
:r 0:: 0

,•
<C 0::
w e..
U
::E

• .. e..c
.. e..
~

-'--
---
c

~~Q ......
'..0

C~O
•..0

---
'<0-
....,
c
'<0-
<.!lC

""
~
<.!l~

""
--
Qj Qj

a....,s- as-
~ ~ ~

...
Vl ~
Vl
-roo- o

C
s-
o x s- .....
.....
3::::: I-- '-.3 8'= r--
c~ c~

e.. '"
....,
::l
e.. '" ....,
::l
u
<C '";;; u '";;; ....,
~

u <C u ~

.::!- '"0 X
0
....
0
~r-
.:;;! ....,

::l
'" ::l

<.!l~
::l

~-Q9--

<.!l
::l
~
C
~~

Fig. 5.2. Control scheme including global control in

the form of force feedback (trajectory
generation level and "joint dynamics"
ij, ,qi are omitted for simplicity)
11
 245

b) Price of force transducers. The high-quality force sensors, which

can precisely measure the forces might be relatively expencive. In-
stead to apply faster (and expencive) microprocessors, in this case
we use force transducers the price of which might be also high and
increase the total cost of the control system. Therefore, the robot
designer has to consider which of these two options is from the
techno-economical aspects more acceptable: force feedback, or on-
-line computation of coupling (see Section 5.2.2).

c) Noice at force transducers. The force sensors, as well as all other

sensors, in their output information always include certain noice.
Most often this noice is negligable and can be ignored, specially
if high-quality force transducers are used. However, the problem
lies in the fact that these transducers are built directly in the
mechanisms shaft which might vibrate due to their elastic modes. These
vibrations are transfered to the sensors outputs (force informati-
on), and then they are amplified in the control system. This might
lead to resonant vibrations of the mechanism. Therefore the global
gain K~ must be limited, and filters have to be applied to filter
1
the signals from the force sensors [3, 4l.

Example 5.2.1. For the robot in Fig. 3.2. we shall show the effects of
application of global control in a form of force feedback. Let us assume
that we have to ensure tracking of the nominal traj ectories presented in
Fig. 4.5. If we apply just local programmed control and local feedback
loop!> (local feedback gains are given in Table 4.1. - Example 4.3.2),
then tracking of the trajectories is quite unsatisfactory (Example
4.4). Therefore, it is required to apply global control which compen-
sates for effects of coupling. Obviously, it is sufficient to apply
global control just in jOint 1 and 2. Let us apply force feedback in
these joints. If we put force transducers in these joints, then it can
be shown that, to stabilize the robot, we have to apply global gains
which are at lea.st:

0.5; 0.5

Thus, the global control is applied in the form (5.2.17):

llU~ 0.5. (0.28/0.26) (P1-3.49.q1)

llu G
2
··2 )
0.5· (5.28/4.96) (P 2 -O.16.q
246

where Pi are the dynamic moments in the joints which are measured by
force sensors built in the actuator shafts. In Fig. 5.3 the simulation
of tracking of nominal trajectories from Fig. 4.5. is presented, if
the local nominal control, local servos and the global control in the
form of force feedback are applied.

0.8 1.0 t[sl

-0.02

-0.05

-0.08

-0.1
tqi[rctdl, [ml

Fig. 5.3. Trajectory tracking for robot in Fig. 3.2. with

force feedback as global control

5.2.2. On-line computation of dynamic forces

for global control

The second possible implementation of function Pi* for global control is

by on-line computation of dynamic moments in joints, i.e. of the coupling
between joints. By potentiometers in the jOints (or by some other sen-
sors) we get information on the actual positions of all jOints qi. By
tachogenera.tors we obtain information on the joints velocities qi. In
this we get information required to compute actual values of matrix of
inertia H(q) and of vector of centrifugal, Coriolis and gravity moments
. i .i
h(q, q). If we forward the information on q , q from sensors to a control
microcomputer, it can compute these elements of the dynamic model of mecha-
nism Of robot (3.2.2). On the basis of models of actuators (3.2.6) and
of the mechanisFt (3.2.2), we can determine accelerations of the joints
.:.ii, Or they might be computed by numerical differentation of the jOints
veloc:t,t±es q;l, Or they can be measured by accelerometers.

In this way a microcomputer computes values of the driving torques Pi (x) ,

or the coupling between the servo systems p, (x). Therefore, a microcom-
* 1
PI! ter generat,es function Pi (x) which is, theoretically, equal to actual
coupling.
 247

However, this solution has certain drawbacks. The basic problem rela-
ted to on-line computation of the coupling lies in complexity of dynamic
model of robot. In general case the model of the mechanism dynamics
(3.2.2) might be very complex and it requires a large number of adds
and multiplies to be performed by a microcomputer in order to compute
matrix H(q) and vector h(q, q). Various computer-oriented methods for
computation of dynamic models of robots have been developed. These
methods differ in their efficiencies from the standpoint of the amount
of computation necessary to compute matrix H and vector h. Here, we
shall not consider various methods for computation of dynamic model of
robot [5, 61. Let us mention that the methods for computer generation
of dynamic models of robots in symbolic forms are the most efficient
for computation of dynamic moments Pi [7].

If the robot is moving fastly, the dynamic moments Pi' i.e. matrix H
and vector h, also might vary fastly. Therefore, the microcomputer must
compute their values at each 10-20 [ms1. This means that a control
microcomputer has to be fast enough to compute new values of matrix H
and vector h, (i. e. of driving torque Pi (x», at rate of 1 0 - 20 [ms].
Since for execution of each computational operation a microprocessor
requires some period of time, this means that to compute driving tor-
que Pi a microprocessor might require much longer time than 10-20 [ms1.
To achieve samp 7;i'ng period' which is compatible with dynamic characte-
ristics of the robot, a control microprocessor should be very fast, and that
meanS tha.t it might be relatively expencive. For certain robot structu-
res a number of computational (real) operations, to be performed at
each sampling period in order to compute coupling Pi(x), might be so
high that even the current microprocessors hadrly can achieve it [61.
In such Case we may apply several microprocessors in parallel, which
mea.ns that the required computation load has to be distributed over two
or more microprocessors. Such solution, obviously causes some new techni-
cal problems (for example, how to distribute computations over micro-
processors, how to exchange data between microprocessors, how to sync-
hronized microprocessors execution, etc.).

However, it is not always necessary to compute complete (total) dyna-

m.tc jIlodels of ;robots, i.e. it is not always necessary to compute all
components of dynamic moments (forces). As we have explained in Sect.
4.2, the effects of all components of dynamic moments upon performan-
ces of servos is not, equal. Therefo;re, it is not necessary to compute
all components of dynamic coupling, but in the function Pi* we may
248

include only those components whose effects upon the system performan-
ce is significiant. In other words, the global control might use just
approximative models of dynamics of robot. Let us consider some appro-
ximative models of robots which might be used for the global control [8]:

a) If relatively slow motions of robot are considered, whith which ve-

locity and inertia terms are not significiant, the global control
might include only gravity moments, so that:

(5.2.19 )

computation of gravity moments usually is simple and if we compen-

sate for these moments, we may eliminate static error in the robot
positioning as well the error during trajectory tracking (see Sect.
3.3.5) .

b) In Sections 3.3.5 and 4.2. we have considered the effects of vari-

able inert.ia of the mechanism around each jOint Hii (q). These eigen-
-inertia terms may be compensated for by the global control, if on-line
computation of eigen-moments of inertia of mechanism and of corres-
ponding accelerations is implemented at control microcomputer:

(5.2.20)

In this way we may achieve more uniform performance of local servo

systems since the effects of variations of moments of inertia of
mechanism are compensated for.

c) If we consider fast motions of the robot, with high accelerations and

deacceler,a tions, cross- inertia terms produce signif iciant dynamic
load upon the servo systems and, by this, errors in trajectory trac-
King appear. If we introduce real-time computation of these cross-
-inertia terms, then we may achieve compensation of their effects.
In th.a,t case app;t:oXimative model of the robot dynamics is given by:

* , n , ,
Pi. = f1ii (q)gl + ,I
J=1
HiJ' (g)ejJ + gi (q) - Hii ql (5.2.21 )

j*i

computation of cross-inertia terms often requires a large number of

rea.l operations and therefore it should be carrefully studied whe-
ther all cross-inertia terms are necessary to be computed.
 249

d) At last, if very fast motions of robot are considered, with which

it is necessary to achieve high accuracy in tracking of trajecto-
ries (along entire trajectory, not only when the robot reaches its
final position at the end of the motion), then we have to compen-
sate even for Coriolis and centrifugal forces, which depend on jo-
int velocities. Often the computation of these dynamic forces is
the most complex, and, therefore, we have to pay special attention
to analysis whether it is justified to compute them on-line. If we
include Coriolis and centrifugal forces in the model (5.2.21) we
obtain complete "exact" model of the robot mechanism dynamics:

n .
I H ... (q)qJ + h.(q, q) (5.2.22)
j=1 IJ I

As it follows from the above considerations, we have to carrefullystu-

dy which components of dynamic moments we have to compensatefordepen-
ding on specific robot and specific task. The adventage of such appro-
ach to synthesis of global control lies in the fact that it enables
compensation for only those components of moments which are essential
in particular case. In this way the global control can be simplified
and unenecessary computations ina control microcomputer are avoided. A
decision which approximative model is acceptable for particular robot
and task, might be made by analysis of the robot stability, as explai-
ned ip Appendix 5.A. In doing this, for the function Pi* we should use
various approximative models starting from the simplest one (5.2.19) up
to the complete model (5.2.22).

It should be also poted, that all components of dynamic moments do not

require the same computational rate. Some of them vary faster then
others, SO it is not necessary to compute them at the same rate. Gene-
;r:a.l:ly spea,k;tng, ;it ;is often sufficient to compute dynamic components at
the rate which are from 10 to 20 [ms], and this is for several time less
than sepV'o rate which for industrial robots must be between 1 and 5 [ms] .
(i.e. the rate at which local servo control must be computed). It is
also necessary to analyze which is the lowest rate at which each com-
ponent of dynamic moment might be computed.

However, on-line computation of dynamic forces suffers from some other

drawbacks, too. This form of global control assumes that all parame-
ters of the robot are precisely identified and that they do not vary,
which is not true in general case. In industrial applications of ro-
bots, this assumption is often valid, since the processes are oft.en
250

known in advance. However, as we shall consider in Chapter 6, with

current robots parameters of a payload are often not known in advance.
Therefore, the compensation for the effects of payloads upon the robot
dynamics is not simple if we apply on-line computation of dynamic mo-
ments. Actually, it is necessary to implement on-line identification
of unknown parameters of a payload. In other words, the global control
by on-line computation of dynamic coupling is not robust to parameters
variations, and to uncertainities of the robot modelling.

The control scheme which includes on-line computation of dynamic mo-

ments is presented in Fig. 5.4. It can be seen that the structure of
this control scheme is much more complex than in the case of force fe-
edback. This considerably complicates the implementation of control
and its maintaince, and reduces the reliability of the system.

Example 5.2.2. In Example 5.2.1. we have synthesized global control in

the form of force feedback for the robot presented in Fig. 3.2. NOw,
we shall present global control for this robot which applies on-line
computation of the robot dynamics. We shall use approximative model of
the robot dynamics. As in the previous case, we have to ensure accura-
te tracking of trajectories in Fig. 4.5. We shall apply local servo
systems and l,ocal nominal control. By analysis of practical stability
of the robot, the simplest approximative model of the robot dynamics
is d,etermined which can be u.sed for computation of couplings in the
robot jO,ints, and to ensure accurate tracking of the nominal trajecto-
ries. It can be shown that the global control might be introduced by on-
~line computat,ion of inertia moments in the first and in the second
joint. (:f;or the third joint there is no need to appl,y global control).
'rAe minimal global :f;eedback gains whicA ensure robot stabilization in
this Case a:re ~~ = 0.8 and :K~ = 0.8, so that the global control gets
t.he following form:

The tracking of trajectories in Fig. 4.5. with such global control

(together with the local servos and the local nominal control) is pre-
sented in Fig. 5.5.
 >Yj
f-'.
'"1

U1 P*1

"'" K~f1/61
""'10n
o 0 ~ 0
'1 tr' I ~
Of-'ci"
00 ci" f-'. '1
f-'. ~ 0
3-CDf-'
'dci"
f-''10OO
f-'. Il! 0 ° ON-LINE COMPUTATION P1
° u. S tr' x1
f-'. CD 'd CD
ci"0>::!3 OF DYNArHC MOMENTS
'<: ci" ci" CD MECHANICAL
-Oil!
'1 ci" f-'. ACCORDING TO
'<: f-'. ~ PART OF
o
'"1~f-' ° SELECTED xn Pn
(i) >::
~Oo. APPROXIMATIVE ROBOT
CD .." f-'.
'1 ~ MODEL
Il! 0.'"1
ci"'<:
f-'.~ '"1
Oll!f-' Pn*
~ S 0
f-'. tr'
f-'Oll!
CD f-'
<: S
CD 0
f-'SO °
(i) ~
f-'.~ ci"
OOci"'i
00 0
o f-'
S 0
f-' . .." <:
ci" f-'.
rT Il!
CD
0.

~
(.f1
252

0.6 0.8 1.0 t[s]

-0.02

-0.05

-0.08

-0.01

Fig. 5.5. Tracking of nominal trajectories for robot in Fig. 3.2.

with global control in form of on-line computation of
dynamic moments

E X ere 1 s e s

5.1. Why the global control (5.2.13) is approximative? Explain the sta-
tement that such control ignores delay in actuator.

5.2. Draw the control scheme for the robot in Fig. 3.2. if the control
considered in Example 5.2.1. is applied. (Local programmed con-
trol, local servo feedback loops, global control in the form of
force feedback). Determine the minimal number of real operations
(adds and multiples) that have to be performed at each sampling
interval in order to compute this control (the local nominal con-
trol is computed as in Example 3.5).

5.3. Estimate nmnbe;r- of microproce$$ors that have to be applied in pa-

;r-allel in order to implement the control considered in Exercise
5.2, and t.o achieve a servo rate of 5 [ms] (for computation of 10-
ca.l $ervos, l,ocal nOlllinal cont;r-ol and global control) if we use:

a) microp;r-oces$ors of type INTEL-80-80 (which takes for one floa-

ting-point mUltiply 1.5 [ms], and for one floating-point add
0.8 rmsJ).

p) microprocessors of type INTEL-80-87 (which takes for one floa-

ting-pOint, mul,t,iply 65 [j.lsl, and for one floating-point add
 253

35 [~sl). (Instruction: According to the number of multiplica-

tions and additions determined in previous exercise, determine
the processor time required for computation of control at one
sampling interval, assuming that the processor time is taken
only for multiplies and adds (overhead is neglected), and then
determine the number of processors required to achieve sampling
rate of 5 emsl).

5.4. For the robot in Fig. 3.2. determine approximative models in forms
(5.2.19)-(5.2.22). For each approximative robot determine the num-
ber of real operations necessary to compute dynamic forces. Try to
minimize these numbers. Assume that accelerations of the joints
are given (that they are obtained by differentation of velocities).
Assume that for computation of sinus and cosinus it is required
1 multiple and 2 adds.

5.5. Using results obtained in Exercise 5.4. determine the total num-
ber of real operations which have to be performed at each sampling
interval in order to compute local nominal control, local servos
(see Examples 5.2.1. and 5.2.2) and global control in the form
(5.2.17) if various approximative models (5.2.19)-(5.2.22) are
used. Estimate the number of microprocessors that have to be ap-
plied in parallel, for each of four control versions, in order to
achieve sampling rate for local servos and local nominal control
of 5 [msl and for global control of 10 [msl. Assume that we use
the same microprocessors as in Exercise 5.3. Compare the results
to those in Exercise 5.3.

'"
5.6. In one of high-level programming language write the programme for
implementation of global control for the robot in Fig. 3.2. The
global control is in a form: a) of force feedback as in Example
5.2.1 (assuming that the measured values of forces Pi are inputs
for programme), or b) of on-line computation of dynamic moments as
in Example 5.2.2. - using approximative dynamic models. Try to
minimize the number of multiples and adds. (Inputs for programme
are actual values of joint angles qi, velocities qi and accelera-
tions qi, and outputs are global control signals).

5.7.* For the robot in Fig. 3.2. determine expressions for calculation
of joints accelerations qi as function of qj, qj, j=1,2,3, using
the second order models of actuators (n i =2) and approximative
254

dynamic models (5.2.19)-(5.2.22) - see Exercise 5.4. If we use

this procedure for determination of joints accelerations, then
determine for how much increases the number of real operations
necessary to on-line compute dynamic moments by approximative mo-
dels (5.2.19)-(5.2.22), in respect to the previous case (considered in
Exercise 5.4) in which we have assumed that accelerations are gi-
ven. (Given accelerations means that they are either directly mea-
sured by accelerometers, or they are computed by numerical diffe-
rentation of velocities qi according to equationqi "" (qi(t+Tn ) -
qi(t»/T n - where Tn is the sampling interval). In which way we
can reduce the number of real operations necessary to compute ap-
proximative models, if the accelerations are obtained by models of
actuators and mechanism. Solve the above problem (determination of
jOints accelerations on the basis of actual joints angles and ve-
locities using the second order models of actuators and approxi-
mative models of dynamics of robot mechanism) in general case, for
a robot of an arbitrary structure with n jOints.

5.3 Computer-Aided Synthesis of Robot Control

From our previous discussion we may conclude that the selection of con-
trol law at the executive control level is strongly dependent on type
and structure of a robot and on a task which is assigned to it. If we
consider simple control tasks which can be reduced to position control
or to tracking of slow trajectories (requiring no high accuracy), then
we may adopt only local servo systems (eventually, we may also intro-
duce local feedforward terms in a form of local nominal control). In
such cases effects of dynamics of the entire robotic system are weak
and therefore, we may ignore them. This approach has been applied in
control of many robots at the market. However, if very accurate trac-
king of fast trajectories is required, then the effects of dynamic for-
ces upon the performances of servo systems might be strong, and then we
have to apply dynamic control which compensates for these forces. We
have seen that the dynamic control might be introduced in various ways:
by nominal centralized control, by global control in the form of force
feedback, or via on-line computation of dynamic forces using various
approximative models of robot dynamics. In dependance on complexity of
th,e robot structure and, on reqUirements which are imposed before the
robot,we may introduce dynamic control of various degree of complexity.
For example, the global control need not to be applied in all joints.
 255

In some joints just gravity moments have to be compensated for, in the

other inertia moment require to be compensated for, etc. The aim is to
determine the simplest dynamic control which still satisfy requirements
imposed before the robot in particular tasks. However, this problem is
not simple since it demands analysis of the robot dynamic performances.
In Chapter 4 we have shown how we can analyze the stability of the ro-
bot and in Appendix S.A. we shall present how we can synthesize global
control by robot stability analysis. Obviously, such analysis is rela-
ted to large amount of computations, which means that we have to use
digital computer. Therefore, the computer-aided synthesis of control
for manipulation robot is needed.

Application of computers for control synthesis of large-scale dynamic

system is broadly encountered, today. Here, we shall briefly describe
a software package for computer-aided synthesis of control for manipu-
lation robots, which is based upon the above considered methods for con-
trol synthesis*). Since in the previous text we have explained in de-
tail theoretical background for analysis of the robot performance and
for synthesis of dynamic control, here we shall just briefly describe
basic structure (flow-chart) of this software package [9].

The software package enables synthesis of the executive control level

for robots of arbitrarY structure with various number of jOints and
actuators. The user of the package has to impose data on robot and the
package helps him to synthesize the control. Namely, the control syn-
thesis is interactive. Instead of completely automatic synthesis of
control, here interactive synthesis of control is applied in which the
computer only helps the designer of the robotic system to select the
most appropriate control law for the specific robot and task. The
software package might be regarded as an assistive device which enab-
les the designer to use his experience in the control system design
[10].

The package is modular, i.e. it consists of several modules which the

user can easely combine. We shall briefly describe the basic modules.
The flow-chart of the package is presented in Fig. 5.6.

*)
The package has been developed in Institute Mihailo Pupin, Beograd.
256

Module for input data on robot mechanism

structure, geometry, dynami c parameters-
- setting of dynamic model of robot

Module for input data on actuators:

type of actuators and data on actuators
- setting of complete model of robot

Module for imposing the desired trajectory;

positioning or trajectory tracking;
required accuracy of tracking;
solving of inverse kinematic problem

Synthesis Optimal Bode's PID regulator

in s-domain regulator method by pole - placement

local nominal of centralized

contro 1 nominal control
2
Fig. 5.6. Flow-chart of software package for control synthesis of robots
 257

Module for linearization of dynamic model

of robot - analysis of linearized model

YES

NO

Porce feedback

>''''
-I-' -I-'
.~ e
, '" '"
Co,....
<IJ -I-' e
~
'''''''e
Vl 0-,- -i-J
'" -I-'
><IJ m'-<IJ O'-<IJ
E '- OJ E
'" E .-
Weo
Q)
ueo
, ,
'- 0
<!lE .~ E E.~

Fig. 5.6. (Continue 1)

258

Analysis of nonlinear model of robot

YES NO

YES

YES
3k-------<

Module for control synthesis in time-discrete

domain - se1 ection of microprocessor for
implementation of selected control law

MOdule for simulation of robot dynamics

with selected control law

Module for graphical representation of

robot structure and of results of simulation

Fig. 5.6. (Continue 2)

 259

1. Mod~le fo~ t~p~t data on ~obot mechanism. This module enables the
~se~ to tmpose data about geomet~y and st~uctu~e of the ~obot
mechanism fo~ which he wants to synthesize the cont~ol. The use~
has to impose data on lengths of the links, o~ientation of the jo-
int axes, types of the jOints (rotational or linea~). The use~ also
has to specify data on masses and moments of ine~tia of the links.
Based on these data, the software package automatically sets kine-
matic and dynamic models of ~obot, which are used in the synthesis
of cont~ol. The use~ need not to take care di~ectly on these mo-
dels, but the package uses them automatically in cont~ol synthesis
wheneve~ they a~e needed. In this pa~ticular package, the procedu-
~e for automatic setting of dynamic models of robots based on
D'Alambert's p~inciple has been used [11].

2. Module for input data on actuato~s. In this module the user has to
select type of actuato~ which he wants to apply for his ~obot (or,
which has been applied to drive al~eady designed robot), and the
basic data on actuato~s (moment of inertia of the ~otor, moment and
electromoto~ constants of the moto~, elect~ical resistance of the
rotor circuit, if D.C. motors are applied as actuators). Based
on these data the package automatically sets the models of actua-
tors. Package includes D.C. electro-motors and hydraulic actuators.

3. Module for imposing the desired trajectory. The user has to speci-
fy the task which the ~obot has to implement, i.e. the user has to
choose whether the robot has just to be positioned in various posi-
tions in its work space, or whether it should move "point-to-point",
or if it should track the desi~ed trajectory. The user has to impo-
se data on the desired positions, or trajectories of the gripper of
the robot. The t~ajectories might be imposed in va~ious ways. Ba-
sed on these data, the algorithm, using the kinematic model of the
robot, dete~ines the cor~esponding positions or trajectories of
the robot jOints, i.e. it solves the so-called inverse kinematic
problem (see Chapter 2). In this way the desired (goal) positions,
or trajectories of the robot joints are defined, and the executive
cont~ol level has to implement them.

4. Module fOr sYnthesis of local servo systems. The package can syn-
thesize the local se~vo systems using various methods in accordan-
ce with the use~ option. The package selects servo feedback gains
fo~ all joints of the ~obot.
260

5. Mod-ule for nOll\inal control synthesis. The package enables automatic

computation of the nominal driving torques which correspond to the
imposed. paths of the robot gripper, and synthesis of the nominal pro-
grammed. control based on the complete dynamic model of the robot.
The package also includ,es computation of the local nominal control on
the basis of local subsystems models.

6. Module for synthesis of g.lobal control. The user is able (if he wis-
hes so) to synthesize the global control using this software package.
The global control has to compensate for dynamic coupling between
the joints and, by this, to increase accuracy of tracking of de-
sired trajectories. The global control can be introduced either as
force feedback, or as on-line computation of dynamic moments. In
the latter case the package helps the user to select the approxima-
tive model of the robot dynamics which can be used for computation
of the global control. Namely, the package enables iterative selection
of the sill\plest approximative model by which imposed requirements
can be satisfied [10].

7. Mod.ule for analysis of robot performance. This module analyzes the

the stability of the robot with the selected control law. First the
stability of the linearized model of the system is analyzed, and
then, the stability of the entire nonlinear model of the robot is
verified. The software package automatically linearizes the dynamic
ll\od,el of robot and by testing eigen-values of the linearized model
with the selected control, it examines whether the linearized model
Of the system is stable or not. If the linearized model is stable,
then the nonlinear model is examined. The analysis of the stability
of the nonlinear model of the robot is performed by aggregation-de-
composition methods for analysis of stability of large-scale sys-
tems (Appendix 4.A). However, the analysis is performed automati-
cally, and, thus, the user need not to learn much about these ra-
ther complex algorithms. For the user it is quite sufficient to obta-
in an answer whether the robotic system is stable with the selected
control law, or not. If the answer is positive, the selected con-
tr·ol can be accepted. Qtherwise, he has to re-select either local
se;t'yos, op global cOntrol (see flow-chart in Fig. 5.6).

8. Mod.ule far simulation of robot dynamics. The module for simuZation

Of ;t'obot dynamics enables verification of r.obot behaviour with va-
rious control laws. Using this modUle, the designer of the robot
obtains precise intight into the performance, quality and speed of
the robot which, h,elps h,im to make a final decision in selecting the
 261

most appropriate control law. The package also permits simulation of

only local isolated subsystems (joints) in order to get better in-
sight into effects of dynamic interactions behveen the joints of
the robot.

9. Module for synthesis of control in time-discrete domain. This modu-

le enables synthesis of control in time-discrete domain. Such con-
trol synthesis is necessary since the control of current robotic
systems is implemented exclusively by microprocessors. This means
that the feedback gains have to be synthesized taking into account
a sampling period, i.e. the period which is taken by microprocessor
to compute new values of control signals according to the selected
control law. This module also helps the user to select microproces-
sor achitecture which is required for implementation of the synthe-
sized control (taking into account computational capability of
microprocessors). The control synthesis in time-discrete domain and
microprocessor implementation of various control laws are out of
the scope of this book.

Besides above listed modules, the package includes some auxiliary mo-
dules, for example module for graphic presentation of a robot structu-
re and graphic representation of results of trajectory synthesis, no-
minal control synthesis, simulation etc.

At the end of this brief description of the package we have to make

two notes. As we have explained before, in stability analysis of the
robot with the selected control, first the linearized model of the ro-
bot is analyzed, and then the nonlinear model is analyzed. One of the
reasons for such approach is that analysis of the linearized model inclu-
ding automatic linea.rization of dynamic model of the robot consumes
much less computation time than the analysis of the nonlinear model.
The analysis of the stability of the nonlinear model (due to estimati-
on of dynamic coupling between joints in defined surroundings of nomi-
nal trajectorY - see Appendix 4.A) might, in general case, require re-
latively long computation time. In order to increase speed of computer-ai-
ded analysis of the robot performances, first the fast analysis of the
linearized model is performed. The analysis of the linearized models
gives sufficient, but not necessary conditions of the system stabili-
ty. Th.is means, that if the linearized model is stable (according to
adopted definition of stability), then we should proceed to stability
analYsis of the nonlinear model. If the analysis of the stability of
the nonlinear model of the robot gives positive answer, than we can
262

guarantee that the robot is stabilized by the selected control law.

However, if the linearized model is not stable, there is no need to
analyze the nonlinear model, since, in that case, the robot is obviously
unstable. In that case, we have to re-select control (or, feedback
gains) and repeat analysis of the linearized model. This procedure
should be repeated until (desired) stability of the linearized model
is achieved. After that we have to proceed to the analysis of nonline-
ar model. We have to keep in mind that the analysis of the stability
of the nonlinear model gives sufficient but not necessary conditions
for the system stability (see Appendix 4.A), and if the answer of the
computer is that the stability tests are not fulfilled, we must not
state that the robot is not stable. In other words, the user has to combine
the results of stability analysis of the linearized and of the nonlinear
models, using his own experience, and in that way to get an answer
whether or not the robot is stabilized by the selected control, or re-
-selection of control law and parameters has to be done.

The second note concerns the simulation of the robot dynamics. The
simulation of the robot reduces to numerical integration of dynamic
model of robot (i.e. of the set of nonlinear differential equations
(3.2.27)). For the numerical integration we may use various algorithm~

which can be found in literature. The most simple procedure, (but also
the least accurate method) is EuZer's method, which reduces to compu-
tation. of the next system state (the state of the system at the next
sampling interval t+6T) on the basis of the state at the instant t,
according to simple equation:

X(t+6T)=x(t)+6T·i(t)=x(t)+6T·[i(x(t»+§(x(t»N(u(t»] (5.3.1)

where AT is the integration intervaZ (i.e. the time interval at which

we compute a new state). For the given instant state x(t) we have, in
accordance to a selected control law, to compute values of the control
signals, i.e. th,e values of the elements of the input vector u(t) .
'I:n,en, we h.ave to compute expre!3 s ion at tn,e right-hand side of the equ-
ation (3.2.27) , and, then, based on expression (5.3.1) to compute the
!3ta,te ;i.n the next sampling interval x (t+6T) . Then, the procedure is
repea,ted at the next interval. ObViously, the user must impose the
in.itia.l state x(O), a,nd tn,e integration interval 6T (which must be
!3ufficiently sn,ort to enSllre that the numerical integration (5.3.1)
gives sta,te trajectory whicn, is close to "actual" trajectory of the
system). Tn,is mO!3t simpl,e procedllre for numerical integration of the
differential equations (tn,e model of the robot dynamics) suffers from
 263

drawbacks regarding numerical accuracy, and therefore it is often

necessary to apply more complex (and less approximative) procedures
for numerical integration.

E X ere 1 s e
5.8~ Write in a high-level programming language a programme for simu-
lation of the dynamics of the robot in Fig. 3.2. if local nominal
Gontrol, local servo systems and global control in the form of on-
-line computation of inertia moments in the first and in the se-
cond joints are applied (as in Example 5.2.2). Repeat simulation
results presented in Example 5.2.2. Use the programmes written in
Exercises 4.16 and 5.6.

5.4 Computed Torque Method for Robot Control Synthesis

To this point we have considered the dynamic robot control based upon
the decentralized approach to control [121. In this approach, the lo-
cal servo systems at particular robot jOints are synthesized first. Such
control has a decentralized structure: each of the local servo systems
possesses information only about the state of its own subsystem (actu-
ator and jOint) which it controls. As we have already seen, to ensure
the accurate positioning and tracking of fast trajectories, such decen-
tralized control is not always sufficient, so that it is necessary to
introduce the control which takes into account the dynamics of the sy-
stem as a whole. For this purpose, certain control signals have been
"added" to the decentralized control structure to compensate the ef-
fect of the coupling between the subsystems. Such control, called the
nominal programmed control is calculated on the basis of the complete,
centralized robot model. The global control using either force feed-
backs, Or the on-line calculated moments has also the role to compen-
sate the dynamic forces acting as the coupling between the subsystems.
Both kinds of dynamic control are the deviation from the decentralized
structure, because the control signals for the particular actuators
are calculated as a function of states of all subsystems (not only of
the corresponding subsystem). Therefore, such control possesses a cen-
tralized structure. However, the main aim in this approach is to re-
tain, to the most possible extent, the decentralized structure, and
264

introduce the global, cross feedback loops only t):1.ere where it is neCeS~

sary. In doing so, as we have already shown (Section 5.3) the starting
point is always a decentralized structure whose main features are sim-
plicity and reliability, and the centralized control components are
introduced later on.

Here, we shall consider an opposite approach: starting from the global

system model, the control is synthesized with the centralized structu-
re and then, the unnecessary feedbacks are eliminated. Such a centra-
lized approach is theoretically consistent but is much more complicated
and might lead to complex control laws.

It should be pointed out that the majority of the present day commer-
cial robots possess a decentralized structure: each joint is control-
led by a separate local servosystem. Centralized control schemes are
rarely used with robots because the majority of the commercial robots
have not been intended for solving dynamic tasks (accurate tracking of
fast trajectories), for which the centralized structure is justified.

The "computed torque method" or "inverse dynamics" method is one of the

centralized approaches to the robot control synthesis. The idea is to
include, in a direct way, the mathematical model of the robot dynamics
into the control law [13-15].

For the simplicity sake, let us consider first the model of the dyna-
mics of the mechanical robot part, while the models of the actuators
will be introduced afterwards. The dynamics of the mechanical part of
the robot is described by the model in the form of (3.2.2)*):

p fI(q)q T h(q, q) (5.4.1)

To ensure tracking of the given trajectories of the robot joints given

by qOi(t), let suppose the control system generates the driving torqu-
es by calculating them from the following control law:

PIt) = Hs (q)[qo(t)+K 1 (q(t)~qo(t»TK2(q(t)-qo(t»]Ths(q, q)

(5.4.2)

*) For an explanation of the notations see Section 3.2.1.

 265

where Hs (q) is the inertia matrix of dimensions nxn, calculated on the

basis of the actual (measured) values of the jOints angles q, hs(q, q)
the vector of Coriolis', centrifugal and gravitational moments, calcu-
lat.ed on the basis 0f the actual values of angles q, and velocities q,
K1 is the matrix of the position feedback gains, of order nxn, and K2
is the matrix of velocity feedback gains, (nxn). The control system
(computer), on the basis of sensory information about positions q and
velocities q of the joints, should calculate Hs' hs and P from (5.4.2).
When the torques thus calculated are realized at the robot joints, it
is obtained (by introducing (5.4.2) into (5.4.1)):

+hs (q, q) = H(q)q+h(q, q) (5.4.3)

If the calculation of the matrix Hs and vector hs is perfectly accura-

te, then

H(q), hs (q, q) = h(q, q) (5.4.4)

On the basis of (5.4.3) and (5.4.4) we obtain:

(5.4.5)

If we introduce the vector of deviation of the actual robot coordina-

tes q(t) from the nominal trajectory qO(t), ~q(t)=q(t)-qo(t), then
(5.4.5) can be written as

(5.4.6)

If the gains K1 and K2 have been chosen such to ensure the solution of
the system of differential equations (5.4.6) is asymptotically stable,
the actual robot coordinates q(t) will asymptotically approach the no-
minal trajectories qO (t). In other words, if K1 and K2 are chosen to
be such that the linear model (5.4.6) is asymptotically stable, itwill
al,so gua.ra.ntee an asymptotic sta.bility of the nonlinear robot model
around the nOll\inal, trajectorY qO(t) •

In order to ensure the linear model (5.4.6) is asymptotically stable,

the gains K1 and K2 can be selected in various ways. One of the possi-
ble solutions is to select these gain matrices in the diagonal forms,
266

K1 diag(K~i), K2 = diag(K~i). Then, the model (5.4.6) splits into a

set of n independent second-order differential equations:

i=1,2, ... ,n (5.4.7)

The choice of gains K~L and K~i is now simple. It should be ensured the
roots of the equation (in the s-domain)

0, i=1 ,2, ... ,n (5.4.8)

are in the left half of the complex plane*). Instead of the asympto-
tic stability of the model (5.4.6), (i.e. of the decoupled equations
(5.4.7», an exponential stability may be required: it suffices the
solutions of equations (5.4.8) be in the complex plane on the left to
the straight line Re(s) = -a, where a is the required degree of expo-
nential stability. In that case, the nonlinear robot model is also ex-
ponentially stable around the nominal trajectory. It is easy to show
that, in this way, the practical robot stability can also be guaranteed.

The control scheme corresponding to the control law (5.4.2) is presen-

ted in Fig. 5.7. It is quite obvious that this scheme represents a com-
bination of the control system with closed feedback loops (with res-
pect to q and q) and nonlinear control signals calculated on the basis
of the nonlinear robot model. In this scheme, the compensation of the
variable gravitational, centrifugal and Coriolis forces is ensured.
The feed.back gains are directly adjusted in accord with the changes in
matrix R(q), i.e. in accord with the change of the moments of inertia
of the mechanism; a precompensating signal is introduced with respect
to the nominal acceleration qO(t), to compensate for the delay along
the nominal trajectory.

The main problem in the implementation of the dynamic control (5.3.2)

is in the requirement that the complete model of the robot dynamics is
calculated on-line, which, as we explained in Section 5.2.2, is gene-
rally very difficult to attain, even by emploing very fast micropro-
cessors. Here, similar to global control, instead of an exact calcula-
tion of the matrix Hs and vector h s ' different approximative models
can be used. All the approximate models of the robot dynamics (5.2.19)

*)
Obviously, it is desirable to require the roots are real, so that
the damping ratio would be ~ > 1.
 267

- (5.2.22) that have been mentioned in relation to the on-line calcu-

lation of robot dynamics in the global control can also be applied he-
re. For example, if the approximate dynamic model (5.2.20) is adopted,
which includes only the diagonal elements of the matrix Hs(q) (the own
inertia moments) and gravitational moments, then, the calculation of
the driving moments is reduced to:

(5.4.9)

In this way, the calculation can be substantially simplified, though,

for certain robot types, it may be still complex. On the other hand,
in each specific case (for the specific robot type and its structure,
as well as the specific class of task) it should be examined if such
simplified control can satisfy the required stability conditions (as
relations (5.4.4) do not, obviously, hold any more) .

In order to avoid too complicate calculation of the matrix Hs and vec-

tor hs in real time, the elements of matrices Hand C, and gravitatio-
nal moments gi can be calculated in advance (off-line) for the diffe-
rent values of angles q, and stored in a computer [16]. Here, C repre-
sents the matrix of the centrifugal and Coriolis effects C = Cijk - see
the model (3.2.1). During the robot's motion, for the actual instanta-
neous va.lues of joints angles q, the corresponding values for Hs' C
and gi are taken from the memory and, on the basis of the measured
velocities q, vector hs is calculated:

(5.4.10)

where g (q) =[gi (q)] is the vector of gravitational moments (n x 1). In

this way, the amount of calculation in real time is substantially re-
duced (it is only necessary to calculate (5.4.10) and multiply the
stored matrices Hs by the term in the square bracket in (5.4.2». This
approach has all the shortcomings of the off-line calculation of nomi-
nal control. First of all, it reqUires a large memory capacity. The
scheme for realization of such control is shown in Fig. 5.8.

Apart from the problems related to large amount of calculation, the

computed torque method suffers also from other shortcomings. This is
related to the problem of the robustness of such control to variations
268

Calculation of h(q, q)

q
Mechanical part of robot
q

Fig. 5.7. Scheme of control by computed torque method (ace. [13])

H(q) q

q(t)
Mechanical part of robot
q(t)

Fig. 5.8. Control scheme (ace. [16])

 269

of the robot (and payload) parameters. The assumption in this control

synthesis has been that all the robot parameters were ideally known in
advance - see (5.4.4). If these parameters are changed it is necessary
to examine robustness of such a solution. The robustness investigation
is complicated by the complex centralized structure of the control. It
has been shown that this control scheme is ratner sensitive to both pa-
rameter variations and to model uncertainities. Namely, it is evident
that the control scheme is developed under assumption of perfect model-
ling of the robot and actuators dynamics. Due to presence of unmodel-
led high frequency modes of the systems (i.e. flexibility of the links,
joints actuators and reducers which are not included in the rigid body
model of the mechanism and of the actuators) the presented control
scheme is v-alid only if the feedback gains are properly adjusted to
prevent excitation of these high frequency modes. Therefore, the feed-
back gains must not be too high, but this might cause that the system
becomes sensitive to parameter variations (since robustness to parame-
ter uncertainties is generally achieved by high feedback gains, i.e.
by high control bandwidth). This method can be applied for the synthe-
sis of adaptive control, but its realization is complex [17].

The main problem in this approach is in the complex, a priori chosen,

centralized control structure which is making difficult the synthesis
and choice of a simplest acceptable solution. Because of that, in the
centralized control, it is much more difficult to develop a synthesis
procedure which would utilize the robot designer's experience (see
Section 5.3).

Finally, let us note that in considering the "computed torque" method

we have neglected the dynamics of the actuators. If the models of the
actuators have been chosen in the form of (3.2.11), (second-order ac-
tuators models) it is easy to show that the input signals to the actu-
ators to realize the driving torques (5.4.2) are (J~ = J~N~N!):

CiCiNiN i .
MEv m)ql} (5.4.11)
i
r R

where H! ~nd I~ are the i-th rows of matrix Hs' i.e. of the unit mat-
rix In' h~ the i-th element of vector h s ' while the other symbols,
270

related to the D.C. electro-motors, have been explained for equations

( 3 • 2 . 4) and ( 3 . 2 . 5) •

It should be noticed that the problem of variable and unknown parame-

ters in the "computed torque" method can be overcome by using the ro-
bust control [18] which represents a combination of the computed tor-
que approach and the so-called "sliding mode" [19]. However, this at-
tempt has not been developed sufficiently to be realized in practice,
and it is still at the stage of theoretical investigations.

Example 5.4. For the manipulator in Fig. 3.2, we shall write the ex-
pression for the driving torques in the "inverse dynamics" method
(5.4.2). The gains K~i and K~i have been chosen such that the roots of
(5.4.8) are both -5 (for all three joints, i=1,2,3). The obtained fe-
edback gains are K~i = -25, K~i = -10, i=1,2,3. The driving torques
should be calculated (using the model of the robot dynamics (3.2.3»
from the following expressions:

m3 [ q-03 (t)-256q 3 (t)-106q. 3

(t)] -m 3 g

E X ere i s e s

5.9. Prove that expression (5.4.11) represents the voltages at the in-
puts of D.C. motors which would generate the driving torques
(5.4.2) if the models of the motors had been given in the form
(3.2.6), (3.2.11).
 271

5.10. For the manipulator in Fig. 3.2, the driving torques using the
"computed torque" method are given as in Example 5.4. Hrite the
expressions for the input signals to the actuators (5.4.11) cor-
responding to these driving torques for particular robot. If the
approximative models (5.2.19) - (5.2.22) are used instead of the
exact model of the robot dynamics, write the expressions for the
driving to;r-ques (Le. inputs to the actuators) using the "compu-
ted torque" method. Minimize the number of numerical operations
which should be carried out.

5.11. For each of the set of voltage inputs to the actuators (5.4.11)
written in the preceeding exercise for the robot in Fig. 3.2.
determine, using the approximate models (5.2.19) - (5.2.22), the
number of operations (multiplies and adds) needed for their cal-
culation (for the given q and q). Determine the minimal number
of microprocessors which should work in parallel to calculate
these input signals (for all three actuators) every 10 [ms], if
the microprocessor used is:

a) INTEL-80-80 (one addition operation lasts 0.8 [ms], and one

multiplication 1.5 [ms]),

b) INTEL-80-87 (addition 35 [~sJ, multiplication 65 [~s]). Sup-

pose the processor time is used only for the operations of ad-
dition and multiplication (the calculation of a sine or cosi-
ne function is equivalent to two adds and one multiply opera-
tion) .

5.12. For the control laws in Exercise 5.10, determined on the basis
of the approximate models (5.2.15) - (5.2.22) for the robot in
Fig. 3.2, draw the control scheme taking into account the model
of the actuators.

5.13.* write in a high programming language the programme for the on-
-line calculation of the input signals (5.4.11) to the actuators
of the robot in Fig. 3.2. which would correspond to the driving
torques in Example 5.4. (using the "computed torque"method). The
programme input data are the robot actual and desired cordinates
qi, qOi and velocities qi, qOi and desired accelera tions qOi, and
the output data are the input signals to the actuators u i , i=l,
2,3 (the data about the robot are also inputs).
272

5.14~ Explain what is the similarity a,nd th.e difference between the
control law (5.4.11) according to the computed torque method and
the control law presented in Section 5.2.2. which is composed of
the local nominal control, local servo systems feedbacks, and
global control in the form of on-line calculation of the comple-
te model of the robot dynamics.

5.15. If in Example 5.7. the poles of equation (5.4.8) are set (both)
at the position -20, instead of -5, what would be the gains K~i
and K~i, i=1,2,3. What are the real gains in the feedback loops
with respect to the position and velocity (the gains from the
error 6qi and 6qi to the actuators inputs u i ). Are these gains
acceptable, and how the poles of equation (5.4.8) should be cho-
sen to constraint these gains?

5.16. Explain what would be the role of variation of the gains in the
feedback loops with respect to the position and velocity as a
function of the inertia of the mechanism. What would be achieved
in respect of the uniformity of functioning of the system?

5.5 Cartesian Based Control of Robot

As we have already pOinted out (Chapter 1), the task assigned to the
robot is usually described in the so-called external (Cartesian) coor-
dinates. In other words, the task is defined by assigning the path (or
position) to be realized by the robot hand together with the workpiece.
If the task is defined by the operator, by the direct lead-through me-
thod, or using the corresponding programming (robotic) language, it is
much Simpler to define the path, i.e. the action to be performed by
the robot hand. Similarly, if the task is assigned by higher control
levels, it is usually defined "what the gripper should do with the
workpiece". On the other hand, the robot's joints are powered by the
a.ctuators, and they should be controlled to realize the desired posi-
tions or trajectories of the robot hand. To this point we have assumed
the hierarchical robot control: at the higher, tactical level, thetra-
jectories of the external (Cartesian) coordinates are transformed into
the trajectories (positions) of the robot joints, while at the lower,
executive level, the calculated joints coordinates are realized. In
the previous chapters we considered the problems of realization of the
given joints trajectories (positions), assuming they had been calcula-
 273

ted on the basis of the gripper trajectories (Chapter 2). A global

scheme of such control is shown in Fig. 5.9.

Such hierarchical solution to the robot control is practically used

with all present day commercial robots. As we have seen, to realize
the obtained joints coordinates, different control laws can be appli-
ed. Such control is called joint-based control [20J. It is obvious that
this control requires the solving of the inverse kinematic problem,
which is usually reduced to the inversion of the Jacobi matrix, and
this may require a great number of numerical operations (i.e. a power-
ful microprocessor is needed to attain the sufficiently short sampling
period). It is intuitively clear that another solution to the control
problem is possible. As the task is usually defined in terms of exter-
nal (hand) coordinates it would be possible to control the robot using
directly these coordinates. The joints positions (and velocities), me-
asured by the corresponding sensors (potentiometers, shaft-encoders,
technogenerators, and the like) are transformed into the external co-
ordinates (the hand coordinates) sIt) and compared with the desired
(nominal) hand trajectories sO(t), so that the errors are also obtai-
ned in terms of external coordinates. On the basis of the error in the
hand coordinates, the control signals to the robot actuators are gene-
rated. A block-scheme of such control is presented in Fig. 5.10. This
control is called the Cartesian coordinates based control (or the con-
trol in the task-space coordinates [20, 21J, because the control is
based on the errors in the coordinate frame in which the task is as-
signed, e.g. the frame related to tool, or workpiece, etc.).

After the microprocessor has calculated the error in the hand coordi-
nates, it is possible to apply different control laws to generate the
appropriate control signals. It should be borne in mind that the job
to be done is to generate the inputs to the actuators, by which parti-
cular robot joints are powered. One of the possible solutions is to
transform the error (using the inverse Jacobian), calculated in terms
of Cartesian coordinates 6s = sIt) - sO(t) into the error of the in-
ternal coordinates 6q, and then, to generate the inputs to the actua-
tors according to one of the control laws considered (e.g. for the
l~cal servo systems, the control is generated acc. to u i = -K~6qi -
K~q, and the like). In this way, it is possible to calculate not only
the error with respect to the position but also the errors with res-
pect to the joints velocities. However, this solution reqUires the ap-
plication of the inverse Jacobian whose calculation usually requires a
274

large amount of calculation so that it has no substantial advantage

over the solution presented in Fig. 5.9. On the other hand, the in-
verse transformation from the external into the internal coordinates
is in this case carried out within the control loop, so that it may be
required that the inverse Jacobian is calculated during a much shorter
sampling period than in the scheme in Fig. 5.10.

Another solution to the Cartesian based control is, starting from the
error in the hand coordinates ~s, to calculate the forces (moments)
which should be produced on the hand so that the hand would follow the
desired trajectory sO(t) (i.e. the given position so). Let denote by F
the mxl vector which is composed on the force components and corres-
ponding moments acting at the gripper mass centre (i.e. around the hand
mass centre)*). The components of the force and moment at the hand re-
present their projections to the axes of the Cartesian (or task speci-
fic) coordinate frame with respect to which the external coordinates
have been defined. Therefore, the control law in terms of hand coordi-
nates defines the link between the vector F and the error in Cartesian
coordinates (as well as, the error in the Cartesian coordinates velo-
cities) :

F (5.5.1 )

The Cartesian forces and moments thus obtained should be applied to

the robot hand. In order to realize these forces and moments, the cor-
responding torques (forces) to be produced by the actuators about the
jOints axes, should be determined. The link between these tor~ues abo-
ut the joints axes P and the vector of hand forces F is (in the static
case) defined as:

P = JT(q)F (5.5.2)

where J(q) is the Jacobi matrix (mxn), defined in Chapter 2. The tor-
ques to be realized about the joints are given by (5.5.2). It is easy
to determine the voltage signals to be applied to the actuators inputs
to realize these torques. The global Cartesian control thus obtained
is schematically represented in Fig. 5.11. The main advantage of this

* ) If the task is assigned with respect to the mass centre or the tip
of the workpiece, and the external coordinates are refferred to
this point, then the vector F is also defined with respect to this
point. Actually, F should be reffered to the same coordinate frame
to which s is reffered.
 275

-
sO(t)
Inverse kinematics
qO(t)
Joints based
control law -u(t) Robot and
actua tors
q
I--

Fig. 5.9. conceptual scheme of joint based control

sO(t) IIS(t) Cartesian control law u(t) Robot and actuators -q( t)

s(t) Direct kinematics

Fig. 5.10. Conceptual scheme of Cartesian based control

t sOt t)
F P
~~ Calculation of
F=f(IIS, 6S, so)
f--- } f---- Robot and actuators I--

+ -

s(t) q(t)
Direct kinematics

Fig. 5.11. Conceptual scheme of Cartesian based control

(for simplicity, velocity feedbacks and cal-
culation of signals to actuators omitted)

control structure over the scheme in Fig. 5.10. is in that the calcu-
lation of the inverse kinematics is avoided. Instead of the inverse
Jacobian, a transposed Jacobian is used, and the direct robot kinema-
tics is calculated (i.e. the hand coordinates are calculated on the
basis of the joint coordinates). These calculations require an incom-
parably smaller number of numerical operations.
276

The control law expressed in external (hand) coordinates (5.5.1) might

be of different nature. Practically, all the control laws expressed in
the jOint coordinates, considered in this chapter and the preceeding
ones, might be, (after cartain modifications) also applied for the Car-
tesian control. Thus, for example, the decentralized control might al-
so be applied in the external coordinates [21J. In that case, each com-
ponent of the vector F (the Cartesian force and moment) is obtained
only through the errors in corresponding external coordinates;

i ·i
K s (5.5.3)
ve

where Fi denotes the i-th component of vector F, to which corresponds

the i-th component (si) of the vector s, Ki Ki are the position and
pe' ve
velocity gains in feedback loops by hand coordinates. It is obvious
that (5.5.3) represents the control law which should ensure the robot
positioning at the desired position so, while for the case of tracking
the gripper trajectory sO(t) a precompensating term, and the like (ana-
logous to the solution explained in Section 3.5), should be introdu-
ced. Similarly, it is possible to adopt the control law in the Carte-
sian coordinates which would correspond to the optimal regulator (Ap-
pendix 5.B), or which would include the complete dynamic model of the
robot, i.e. which would represent the computed torque method applied
in Cartesian coordinates [22J.

Apart from the obvious advantages of such control schemes in respect

of the reduction of the number of numerical operations to be carried
out by the microprocessor to generate the control signals (due to the
avoidance of the Jacobian inversion), the Cartesian control has also
certain disadvantages. The feedback gains synthesis in these schemes
is extremely complicated, because the analysis of stability in exter-
nal coordinates is made difficult. It appears that, in order to achie-
ve satisfactory positioning (or tracking of trajectories) using the
decentralized control in external coordinates (5.5.3), it is not pos-
sible to adopt the fixed gains Ki , Ki , but these have to vary in de-
pe ve
pendence of the robot position. It should be borne in mind that the
calculation of hand coordinates on the basis of the measured jOint co-
ordinates q and the transposed Jacobian in the scheme in Fig. 5.12.
are within the control loop, so that it is possible to require their
calculation much more frequently that in the case of the scheme shown
in Fig. 5.9. (where the calculation of the inverse Jacobian is requi-
red, but outside the control loop). It would be possible to avoid the
calculation of the hand coordinates on the basis of the jOint coordi-
 277

nates if the hand coordinates were measured directly, which would re-
quire the use of the corresponding sensors.

It should be noted that the Cartesian robot control is still at the

stage of theoretical and laboratory investigations so that the present
day commercial robots are controlled solely in the joint coordinates.

E X ere 1 s e s

5.17. Explain how the position and velocity gains in (5.5.3) should be
chosen in the decentralized control on the basis of the Cartesi-
an coordinates, to ensure the robot exhibits no overshoots, nor
the oscillatory motion, and the characteristic frequency is not
close to the resonant frequency of the structure.

5.18.* Draw the control scheme for the robot in Fig. 3.2. which corres-
ponds to the computed torque method in hand coordinates [22l. For
this purpose write the dynamic model of the robot in the Carte-
sian coordinates, assuming the external robot coordinates are given
T
by s=(x c ' Yc' zc) ,where xc' yc' zc are the robot tip coordina-
tes with respect to the coordinate frame fixed to the robot base
(at the first jOint). Determine the minimal number of numerical
operations (adds and multiplies) to be carried out on each sam-
pling interval to calculate input signals to the actuators using
the computed torque method in external coordinates for the given
robot (use expressions (5.5.1>, (5.4.2) (5.5.2), (5.4.11». As-
sume the adopted mOdels of actuators are of second order.

5.19.* Repeat the same as in the preceeding problem for the robot in
Fig. 3.3, assuming the robot is in the horizontal plane, so that
the external robot coordinates are given by s = (xc' Yc)T, xc'
Yc are the coordinates of the robot tip with respect to the co-
ordinate frame fixed at the first robot jOint. Determine the
number of operations needed to control this robot (to calculate
inputs to the actuators on each sampling interval) using the com-
puted torque method in the jOint coordinates, assuming the nomi-
nal trajectory is given in terms of Cartesian coordinates; these
Cartesian coordinates are first transformed (using the inverse
kinematic model) into the jOint coordinates, and then, the con-
trol signals are calculated according to (5.4.2) and (5.4.11>.
278

Compare the obtained number of operations to that needed to cal-

culate the control (input) signals to the actuators using the
computed torque method on the basis of the Cartesian coordina-
tes. (Assume that actuators are D.C. motors whose models are of
the second order, so that (5.4.11) holds).

5.20.* It has been shown that the dynamics of the robot hand might be
expressed by the model written in Cartesian (hand) coordinates
[22]. The dynamic model of the robot hand in Cartesian space
might be written as:

F A(S)s + Il(s, s) + p(s)

where F is 6x1 Cartesian force-moment vector, s is 6x1 vector of

Cartesian coordinates of the robot hand (positions and orienta-
tions), A(s) is the 6x6 inertia matrix in Cartesian space, Il(s,
s) is the 6x1 vector of the hand centrifugal and Coriolis moments,
and p(s) is the 6.1 vector of gravity forces. Compare this model
to the model (3.2.2) in joint coordinates and show that the fol-
lowing relations hold:

p(s) = J-T(q)g(q)

P = JT(q)F

(Instruction: Use the fact that

·T • • • 1·T oH •
q C(q)q = H(q)q - 2 q oq q)

Starting from the above written model in the Cartesian space,

show that the Cartesian control on the basis of computed torque
method might be written as:
 279

where sO(t) is desired hand path, Kpe is matrix of position fe-

edback gains, Kve matrix of velocity feedback gains. Try to re-
arange last expression to obtain more convinient form for compu-
tation, using above relations between models in jOint space and
in Cartesian space. Draw the corresponding control scheme.

References

[1] Vukobratovi6 M., Stokic D., Control of Manipulation Robots: Theo-

ry and Application, Series: Scientific Fundamentals of Robotics
2., Monograph, Springer-Verlag, 1982.

[2] Stokic D., Vukobratovic M., "Practical Stabilization of Robotics

Systems by Decentralized Control", Automatica, Vol. 20, No.3,
1984.

[3] Luh Y.S.J., Fisher D.W., Paul C.P.R., "Joint Torque Control by a
Direct Feedback for Industrial Robots", IEEE Trans. on Automatic
Control, Vol. AC-28, No.2, 1983.

[4] Tanie K., Yokoi K., Kaneko M., Fukuda T.: "A Pos i tion Sensor Ba-
sed Torque Control Method for a DC Motor with Reduction Gears",
IEEE Int. Conference on Robotics and Automation, pp. 1867-1880,
1988

[5] Hollerbach M.J., "A Recursive Langrangian Formulation of Manipula-

tor Dynamics and a Cooperative Study of Dynamics Formulation Com-
plexity", IEEE Trans. on Systems, Man and CybernetiCS, Vol. SMC-
-10, 730-736, November, 1980.

[6] Bejczy K.A., Paul P.R., "Simplified Robot Arm Dynamics for Con-
trol", Proc. of IEEE Conf. on Automatic Control, 261-262, 1981.

[7] Kir6anski M., Vukobratovi6 M., Kircanski N., Timcenko A., "A New
Program package for the Generation of Efficient Manipulator Kine-
matic and Dynamic Equations in Symbolic Form", Robotica, July,
1988.

[8] Vukobratovie M., Stokie D., "Contribution to Suboptimal Control of

Manipulation Robots", IEEE Trans. on Automatic Control, June, '1983.

[9] Vukobratovi6 M., Stokic D., "A Procedure for Interactive Dynamic
Control Synthesis of Manipulators", Trans. on Systems, Man, and
CybernetiCS, Sept./Oct. issue, 1982.

[10] Vukobratovi6 M., Stokic D., "Is Dynamic Control Needed in Robotic
Systems, and if so, to What Extent?", International Journal of Ro-
botic Research, Vol. 102, Juni, 1982.

[11] Vukobratovic M., Aoolied Dynamics of Manipulation Robots: Model-

ling, Analysis and Examnles, S9rinaer-Verlag, 1989.

[12] Vukobratovi6 K.M., Stoki6 M.D., Kircanski M.N., Non-adaptive and

Adaptive Control of Manipulation Robots, Monograph, Series: Scien-
tific Fundamentals of Robotics 5., Springer-Verlag, 1985.
280

[13] Paul R.C., Modelling, Trajectory Calculation and Servoing of a

Computer Controlled Arm, Ali. Hemo 177, Stanford Artificial In-
telligence Laboratory, Stanford University, Septembar, 1972.

[14] Bejczy K.A., Robot Arm Dynamics and Control, Technical Hemorandum
33-669, Jet Propulsion Laboratory, February, 1974.

[15] Saridis N.G., Lee G.S.C., "An Approximation Theory of Optimal

Control for Trianable Manipulators", IEEE Trans. on Systems, Man,
and Cybernetics, Vol. SMC-9, No.3, March, 1979.

[16] Raibert H.M., Horn P.K.B., "Manipulator Control using the Confi-
guration Space Method", The Industrial Robot, Vol. 5, No.2, 69-
-73, Juny, 1978.

[17] Timofeev, A.V., Ekalo, Yu.V., "Stability and Stabilization of the

Programmed Motions of Robot", (in Russian), Automatic and Remote
Control, No. 20, 1976.

[18] Asada H., Slotine E.J., Robot Analysis and Control, John Wiley
and Sons, New York, 1986.

[19] Young K.K.D., "Controller Design for a Manipulator Using Theory

of Variable Structure Systems", IEEE Trans. on Systems, Man, and
Cybernetics, Vol. SMC-8, No.2, 1978.

[20] Craig J.J., Introduction to Robotics: Mechanics and Control, Ad-

dison-Hesley Publishing Company, 1986.

[21] Takegaki M., Arimoto S., "A New Feedback Method for Dynamic Con-
trol of Hanipulators", Trans. of the ASME, Journal of Dynamic
Systems, Heasurement and Control, Vol. 103, No.2, 119-125, 1981.

[22] Khatib 0., "A Unified Approach for Motion and Force Control of
Robot Manipulators: The Operational Space Formulation", IEEE
Journal of Robotics and Automation, Vol. RA-3, No.1, February,
1987.
 281

Appendix S.A
Stability Analysis of Robot with Global Control

The analysis of asymptotic (and exponential) stability of the robot

around the nominal trajectory xO(t) if just the nominal centralized
control and decentralized controller (local servos around individual
joints) are applied, has been presented in Appendix 4.A. Here,we shall
briefly consider stability analysis when global control is also applied.

Let us consider the case when the centralized nominal control and loca.l
servos are applied. In Section 5.2. we have introduced global control
in the form (5.2.13). However, as we have explained, this global con-
trol directly compensates for coupling between the joints (subsystems)
only if we may ignore delay between actuator input and driving torque
produced by the actuator. If this delay can not be neglected, this dy-
namic control cannot completely compensate for dynamic moments (which
act as an external load upon the actuator). Therefore, in global con-
trol synthesis we have to take into account this delay between the in-
put signal u i and the driving torque Pi'

In stability analysis of the nonlinear model of the system we have in-

troduced Liapunov function of subsystem vi(~xi) as a scalar function
of the subsystem state ~xi (see Appendix 4.A.1). It can be shown that, in
order to directly compensate for the effects of coupling upon the sys-
tem stabilit~ the global control can be introduced in the following
form:

(5.A.1l

where K~ is scalar gZobaZ gain (constant), while gradv 1. denotes the

1 .
derivative of function Vi(~Xl) by the coordinates of the vector ~xi.

However, the Eq. (S.A.1) is defined only if (gradv.)TSi",O. Since v. is

. 1 1
the function of the subsystem state AXI, then in some pOints in the
subsystem state space (gradvi)TSi might be equal to ° (or, it might
get values close to 0). In these pOints, the global control signals
(5 .A.1) would become extremely high (theoretically infinite) • However,
282

the amplitude of the total actuator input signal is constrained N(u i ),

and, therefore, in the paints in which (gradv.)Tbi+ O, the global con-
~

trol is not realized according to (S.A.1), but some limited input sig-
nal is realized. We may introduce a number Ei>O which satisfies the
following inequality:

(S.A.2)

for all points I'.x in the surrounding of the nominal trajectory xO(t), as-
suming that it is already fulfilled that luOi(t)_k~l'.xil<ui. Now, the
~ - m
global control might be introduced in the form:

-K~r (gradv.) TBi]-1 (gradv.) Tfil'.p~

G ~ ~ ~ ~
I'.u. = {
~ G -1 T'i T'i *
-K.E. sign[(gradv.) b ] (gradv.) f I'.P.
~ ~ ~ ~ ~

(5 .A. 3)

The global control (S.A.3) is defined for all points in the state spa-
ce of the i-th subsystem. The scheme of this global control is presen-
ted in Fig. S.A.1. In (S.A.3) I'.P i* denotes function which satisfies
(S.A.9) and which can be implemented in one of two ways described in
Section 5.2: either by the force feedback, or by on-line computation
of dynamic moments (using approximative models of robot dynamics) .

Thus, the robot control is given by (5.2.8) and (S.A.3). It is obvious

that the global control (S.A.3) directly reduces the effects of dy-
namic coupling upon the asymptotic (exponential) stability of robot.
Namel~ if we apply control (5.2.8), (S.A.3) the first derivative (by
time) of Liapunov function vi (I'.x i ) of the i-th subsystem gets the fol-
lowing form:

vi(along solution of (5.2.11), (S.A.3))

(S.A.4)

NOW, the analysis of the stability of the nonlinear model of the robot
is performed in completely analogous way to that one presented in Ap-
pendix 4.A.
 'rj
f-'-
LQ

U1

[(gradv/S 1 ]-1
f-'- H1 ()
en 0 0
'i !:l
PJ EI rt-
'0 'i
'0 - 0
1-'U11-'
f-'- •
ro~en 01
p,. 0
W::r
~ro

EI PI
I ro ~ CO~lPUTATION r,lECHAN I CAL
f-'- ~
H1::r OF NOMINAL PART OF
f-'- ---.
rt-O
::r::r CENTRAL! ZED -1 ROBOT Pn
ro
f-'- CONTROL
o !:l
ro 0 ~
!:l I-'
rt-s:;
[( gradvn)Tbn ]-1
tiP,
xon
PJ ro
I-'en
f-'- Pn
NLQ
ro I-'
P,O
0-
!:l PJ
01-'
EI
f-'- 0
!:l 0
PJ !:l
I-'rt-
'i
00
o I-'
!:l
rt- f-'-
'i !:l
o
I-'rt-
::r
ro
I'..l
!!l
284

The only difference is that now we have to take into account the glo-
bal control (5.A.3) when we estimate the coupling between the subsys-
tems (joints).

Instead of numbers ~ij' now we introduce numbers *

~ij which fulfill
the following inequalities:

n
2 ..~
* v., for
TA·
< I (gradv.)
1
b 1 1 >E:.1
j = 1 1J J

n *
< I ~ .. v., for (5.A.5)
j=1 1) J

* estimate not only the coupling 6F i between the subsys-

The numbers Sij
tems, but they also estimate the effects of the global control upon
the system stability. The inequalities (5.A.5) must be satisfied for
all pOints in the state space around the nominal trajectory xO(t) which
fulfill (4.A.30). Since the global control is selected to compensate
*
for coupling between the subsystems, then the numbers Sijmust satisfy:

for i,j = 1,2, •.. ,n (5.A.6)

Now, the analysis of the exponential stability of the model of the

state deviation around the nominal trajectory (5.2.11) can be perfor-
med by testing the following inequalities:

G *vo < 0 (5.A.7)

where the elements of the nxn matrix G * are given by:

G *.. = -1\6 ij + * (5.A.8)

1J Sij

If the conditions (5.A.6) are fulfilled, then, obviously, the tests of

the exponential stability (5.A.7) will be "easier" fulfilled than the
tests (4.A.65), which are valid when the global control is not applied.
 285

In other words, by the application of the global control, the effects

of coupling are reduced and therefore the fulfillment of the exponen-
tial stability tests is relaxed. As we have explained in Appendix 4.A,
the fulfillments of conditions (4.A.65) and of n>a (where n is given
by (4.A.67), and a is the demanded degree of the shrinkage of the re-
gion around the nominal trajectory to which the state of the system
has to belong during the trajectory tracking) guarantee that the ro-
bot is practically stable around the trajectory. When we introduce the
global control, then the degree of shrinkage of the region, around the
nominal trajectory, to which the state of the robot belongs can be es-
timated by (instead by (4.A.67)):

n *
n* . min I-Siv io + I I; .. v. I/v. (5.A.9)
1=1,2, ... ,n j=1 1J JO 10

Obviously, the conditions (5.A.6) imply that n * >n, i.e. if the global
control is introduced, then the region, to which the system state be-
longs, must have greater degree of shrinkage than if just the nominal
control and local controllers are applied.

We can see that the global control (S.A.3) can stabilize the robot by
compensating for the effects of dynamic moments. The function K~(~xi)
1
in this case is selected in such a way that the global control direct-
ly compensates for the effects of coupling upon the system stability.
However, to implement the global control (5.A.3) we have to realize
T~i -1 T~i
on-line computation of the expression [(gradv i ) b 1 (gradv i ) f which
demands a few float,ing-point mul tipl ies and adds for each subsystem in
which we introduce the global control~)

The simpler solution for the function

G
Ki(~x
i
) in (5.2.10) is in the
form (5.2.13). The stability analysis with the global control (5.2.13)
can be performed in analogous way as in the case of (5.A.3). Now, the
numbers ~ij estimating both the coupling and the global control have
to satisfy inequalities:

(5.A.10)

for all states in the region defined by (4.A.30). If the second order
model of the actuator (subsystem) is considered (n i =2), then we get:

*) Precisely, we have to realize (2ni+1) multiplies, (2ni-2) adds and

one floating-point division for each control signal.
286

T'i - G *
(gradv].,) f (6P].,-K].,6P].,) ~ En *
t, ,v, (5 .A. 11)
j = 1 ].J J

However, for n i =3 the last inequalities do not hold. In that case, the
global control (5.2.13) does not compensate directly for the coupling,
but we may adopt it as an approximative form (which, in essence, igno-
res the delay in the actuator). Once we determine the numbers t *ij which
satisfy (S.A.l0), the procedure for stability analysis is reduced to
examination of the tests (S.A.7) and determination of n according to
(S.A. 9).

We can test stability conditions (S.A.7) for various forms of global

control. We can use various approximative dynamic models of the robot
for on-line computation of 6P * * acc. to (S.A.S) and
i and determine Sij
check for which of these models the stability tests (S.A.7) are fulfil-
led. The same holds if we implement force feedback as the global con-
trol. In this way we can iteratively determine in which joint we must
apply global control and which approximative model might be used for
it. So, we Can determine the simplest form of the global control which
ensures fulfillment of the conditions (S.A.7), and this procedure has
been applied in the software package for computer-aided synthesis of
control for manipulation robots (see Section 5.3).

At last, let us mention that if the local nominal control and local
servo systems are applied, then we may also introduce the global control
which is analOgOUs to the form (S.A.3). The global control (5.2.16) is,
now, introduced in the form:

for I (gradv,) Tb i I >E,

]. ].

(S.A.12)

The only difference between global control (S.A.3) and (S.A.12) is in

*
implementation of the function Pi(x) instead of the function 6P i* (X)
since in this case the global control has to compensate for the total
dynamic moments Pi(x). However, if just the local nominal control, the
local cOntrollers and the global control (S.A.12) are applied we can-
not analyze robot for its asymptotic (or exponential) stability, but
we have to analyze practical stability of the system and that is out
of the scope of this book.
 287

Appendix S.B
Centralized Optimal Regulator

We shall consider the problem of synthesis of the centralized robot

control for the positioning and tracking of trajectories, which is ba-
sed on an optimization procedure. As with other complex systems, with
the robot control synthesis too, a dilemma arises of whether to carry
out the so-called optimal or suboptimal synthesis. In the optimal syn-
thesis the control should be such to minimize a certain numerical cri-
terion. In the given task we want to ensure the robot is tracking the
given trajectories with a defined accuracy (or, it is positioned with
certain accuracy), the control is as simple (and cheap) as possible
from the standpoint of implementation and maintainance, robust to va-
riations of the parameters, reliable, etc. It is difficult, however,
to express all these requirements in terms of preCise numerical crite-
ria. On the other hand, when the criteria are defined, the optimal con-
trol synthesis is associated with serious numerical problems yielding
usually extremly complex and unacceptable control laws [11.

Nevertheless, there have been a number of attempts to realize the ro-

bot control synthesis by minimizing a certain criterion. For example,
the synthesis has been carried out by minimizing the time necessary to
position the robot at a certain point [21, or by minimizing the acce-
lerations at the joints [31. However, many of the problems in these
optimizations require significant simplifications, resulting in the
unacceptable solutions.

The optimal quadratic regulator represents an analytic solution to con-

trol, which minimizes the standard quadratic criterion, when applied
onto a linear system. This approach can also be used for the synthesis
of robot control.

Let the trajectories of all the coordinates of the robot state vectors
xO(t) be given (by the higher control level). The task is to ensure the
tracking of the trajectories. The robot model in the centralized form
288

is given by (3.2.27)*):

a(x) + B(x) N (u) (5.B.1)

Let us suppose the nominal centralized control uO(t), satisfying

(4.4.8), has been introduced:

tE (0, .r) (5.B.2)

The model of the robot state deviation around the nominal trajectory
xO(t) and nominal control uO(t) is given by (4.4.9):

(5.B.3)

It should be ensured, that the model of deviation is being stabili-

zed around 6x =0 (i.e. around the nominal trajectory xO(t)). The so-
-called standard quadratic criterion is introduced in the form:

(5.B.4)

where Q(t) and QT are the positive semidefinite matrices of dimensions

NxN, and ~(t) is a nxn positive matrix. The quadratic criterion (5.B.4)
includes two requirements: the first and the third term express the
minimization of states deviations from the nominals (which is required
in the task) and the second term represents the minimization of the
input signals (i.e., indirectly, the energy) - see the explanation in
Appendix 3.A.

If we would endeavour to minimize the criterion (5.B.4), using the non-

linear robot model (5.B.3), we would encounter a lot of problems, for,
only a numeric solution to control could be found, and not a solution
in analytic form - as a function of state coordinates. This means, we
would obtain an open loop control of the robot (for each initial
state, a different numerical solution is obtained, which is extremly
difficult to realize). To solve this optimization problem in an analy-
tic form, an approximate, linearized model of the robot should be con-
sidered [4].

*) For an explanation of the notation see Chapters 3 and 4.

 289

In Section 4.4. we showed how, starting from the nonlinear model of

deviations (5.B.3), we can obtain the linearized time-varying robot
model in the :l;orm of (4.4.10)

(5.B.5)

It is known [4] that the optimal control minimizing the critetion

(5.B.4) for the linearized model (5.B.5), is obtained in the form of
the centralized regulator:

,',u(t) -D(t),',x(t) (5.B. G)

where K(t) is the positive definite symmetric matrix, NxN, which is

the solution of the Riccati type differential matrix equation:

-i< (t)

(5.B.7)

Matrix D(t) is the NxN matrix of feedback gains. The solution (5.B.G)
holds under the assumption that the matrix pair AL' BL is controllable
on the interval (0, T), that all the system states coordinates 4x are
measured by sensors, and the constraint on the input amplitude N(u o ,
,',u) is neglected.

It is obvious that the control (5.B.G) is linear with respect to the

robot state coordinates ,',x(t) , i.e. it represents a set of linear fe-
edbacks with respect to the coordinates ,',x(t) to the inputs ,',u(t) with
the gains equal to the elements of matrix D(t). However, these gains
are time-v'arying, which makes the realization much more difficult and
requires a large memory capacity to store the time-varying gains, cal-
culated, in advance. The on-line solving of differential equation
(5.B.?) is hardly attainable.

To simplify the realization of the control law, instead of the linear

time-varying model (5.B.5), we use the linearized time-invariant robot
model which is obtained by "averaging with respect to time" of the mo-
del (5.B.5). Thus we obtain an approximate model in the form:

(5.B.B)
290

where AL is a constant NxN matrix, while BL is a constant matrix of

dimensions Nxn. Instead of the criterion (5.B.4), we introduce:

(5.B.9)

where Q is a positive semidefinite NxN matrix, and R is a positive de-

finite nxn matrix. It should be noted that the criterion is defined on
an infinite time interval (around the nominal trajectory), but, since
the linearized model (5.B.8) is time-invariant, this makes no problem.
It is known that the control minimizing the criterion (5.B.9) for the
model (5.B.8) is given as the linear regulator with constant gains:

lIu(t) (5.B.l0)

where K is a positive definite NxN matrix which is the solution of the

algebraic matrix equation of Riccati type:

(5.B.l1)

where D represents the nxN matrix of constant feedback gains. Such

linear optimal regulator is represented schematically in Fig. 5.B.l.

p1

---
/

/
/ MECHANICAL
/ PART OF
ROBOT
"- '\ SM
'\ xn Pn

Fig. 5.B.1. Scheme of linear optimal regulator

The linear optimal regulator has the following shortcomings:

(a) The control structure of the optimal regulator is centralized and

complex, and requires a large number (Nxn) of feedback loops, which
complicates the implementation of the control system.
 291

(b) The optimal regulator guarantees the stability of a linearized ro-

bot model. As the robot model is, generally, highly nonlinear, it
is questionable if such linear control can also stabilize actual
model. Of course, it is possible to analyze stability of the non-
linear robot model when the linear regulator has been applied.

(c) Robustness of the linear regulator should also be analyzed, beca-

use the regulator has been synthesized for the linearized robot
model and for certain fixed values of its parameters.

(d) The "optimality" of the linear regulator is also guestionable,both

because of the approximate nature of the model and because of an
arbitrary choice of the weighting matrices Q and ~, on which, to a
great extent, depends behaviour of the system when the linear re-
gulator is applied. Various methods have been developed for selec-
tion of the weighting matrices to satisfy certain requirements [1],
but such an approach complicates substantially the synthesis.

In order to overcome the problems arising from the application of the

approximate linearized model (S.B.8), instead of the exact nonlinear
model, it is possible to introduce the additional nonlinear control.
Let write the nonlinear robot model (S.B.3) in the form:

(S .B.12)

where f is the vector function of the order N, given as:

- 0
f(lIx(t» a(x (t), lIx(t) )-ALlIX(t) (S.B.13)

Here we assumed that BL = B(Xo(t), x(t». It can be proved that the

control minimizing the criterion (S.B.9) for the model (S.B.12) is gi-
ven in the form [S]

lIu(t) (S.B.14)

In other words, the control is in the form:

(S .B.1S)

where lIU lin is the linear, and 1I~ nonlinear part of the control law.
292

The block-scheme of such control is presented in Fig. S.B.2. An essen-

tial difference between the scheme in Fig. S.B.l. and the one in Fig.
S.B.2. is in the introducing the nonlinear control component which, in
the latter case, plays the role of the global control for the compen-
sation of nonlinear effects in the robot model. Obviously, the robot
control (S.B.14) is extremely complex: besides of the complex structu-
re of the optimal regulator (6u lin ) the on-line calculation of the
complete robot model is introduced, which brings about all the prob-
lems related to the implementation and robustness (see Section S.2.2).

It should be noticed that we have assumed that the nominal programmed

control uO(t) had been introduced. However, the optimal regulator can
also be applied in the case when the nominal control was not being in-
troduced, but it is necessary, then, to examine the practical stabili-
ty of the robot around the nominal trajectory.

x(t)
ROB 0 T s Y S T ~ M
+

M(t)
+
+ +

Calculation of 6X(t)
flM(t»

Fig. S.B.2. Scheme of linear optimal regulator

with additional nonlinear control
which compensates for the model
nonlinearities (acc. [S])
 293

Example 5.B. For the robot in Fig. 3.2, the linearized time-varying
system model around the nominal trajectories, presented in Fig. 4.5., is
given in Example 4.3.4. After time-averaging, the linear time-invariant
model is obtained whose matrices are:

0 0 0 0 0

0 -0.132 0.254 2.77 0 0

0 0 0 1. 0 0
AL
0 -1.54 -1.14 -15.6 0 0

0 0 C 0 0

0 0 0 0 0 -45.

0 0 0

0.58 -1.75 0

0 0 0
BL =
-1.78 10.43 0

0 0 0

0 0 0.361

If the weighting matrices in the criterion (5.B.9) are chosen as

Q diag ( 0 • 1, O. 1, O. 1, O. 1, O. 1, o. 1 ) ,
~ diag(5., 0.5, 0.05)

solving the matrix equation of Riccatti type gives:

13180. 1730. 2379. 313. O. O.

1730. 706. 233. 123. O. O.

2379. 233. 452. 43. o. O.

K
313. 123. 43. 21. O. O.

O. O. O. O. 5810. 135.

O. O. O. O. 135. 3.

The matrix of feedback gains is:

294

89. 38. 11. 5 6.6 o.

D =[ 362. 51.9 77 .1 10.2 O.
o.
O.
]
O. O. O. O. 964.2 22.2

Tracking of the nominal trajectories using such optimal regulator with

and without nominal centralized control introduced, is shown in Fig.
5.B.3. As can be seen, in the latter case, tracking is

0.2 0.4 t[s] t[s]

-0.02 -0.01

-0.04 -0.02
-0.06
-0.03
wi th nomi na 1
-0.08 centra 1 i zed -0.04
control
-0.1 without -0.05
nominal
control
Ilql[rad] Ilq2[rad]

Fig. 5.B.3. Tracking of nominal trajectories using centralized

optimal regulator with and without nominal control
(for the robot in Fig. 3.2)

References

[1] Vukobratovic M., Stokic D., Control of Manipulation Robots: Theory

and Application, Series: Scientific Fundamentals of Robotics 2.,
11onograph, Springer-Verlag, Berlin, 1982.

[2] Kahn t1.E., Roth B., "The Near Minimum Time Control of Open Loop
Articulated Kinematic Chains", Trans. of the ASME, Journal of Dy-
namic Systems, !4easurement and Control, September, 164-172, 1971.

[3] Young K.K.D., "Control and Optimization of Robot Arm Trajectories",

Proc. of IEEE Milwaukee Symp. on Automatic Computation and Control,
175-178, April, 1976.

[4] Popov E.P., VereschaginA.F., Ivkin A.M., Leskov A.S., Medvedov V.

S., "Synthesis of Control System for Robots Using Dynamic Models
of Manipulation r,lechanisms", Proc. of the Sixth Symp. on Automatic
Control in Space, Erevan, USSR, 1974.

[5] Popov, E.P., Vereschagin, A.F., Filaretov, F.V., "Synthesis of

Quasi-optimal Nonlinear Control System of Manipulators", (in Rus-
sian), Technical Cybernetics, No.6., pp. 91-101, 1982.
Chapter 6
Variable Parameters and Concept of
Adaptive Robot Control

6.1 Introduction

In the proceeding considerations we assumed the robot models and their

parameters are being known and determined in advance. Such an assumpti-
on holds for the majority of robot parameters, such as masses and iner-
tia moments, link lengths, positions of jOints axes, inertia moments of
motors, etc. However, some robot parameters change during the work, and
they are not always known in advance: these parameters are for example
some of the motors parameters, coefficients of viscous and static fric-
tion, etc. A common feature of all these parameters is that they change
very slowly, so that they can be considered to be quasi-stationary. Very
often, their determination is rather difficult. The values of these pa-
rameters can be identified when the robot starts to work, and they sho-
uld be checked from time to time and updated appropriately. Most of-
ten, these parameters do not affect significantly the functioning of
the control system, and they may be considered as being known. A spe-
cial group form the parameters which can undergo fast and drastic chan-
ges during the robot's work, and which cannot be always known in advan-
ce. To this group belong, primarily, the parameters of the working ob-
ject.

In the course of task execution, the robot can move with an empty grip-
per, grasp the payload and transfer it from one place to another in its
workspace (or do some operation on it). In practice, the payload parame-
ters (mass, in.e;r-tia mOments, shape and dimensions) are often known in
advance. However, in some cases in. the industry, the working obj ect is
not defined tn advance, bu,t it can have different parameters in depen-
dence of the conditions and other circumstances under which the task is
being executed at a given moment. This case appears in modern industry
mOx;e and more often, especially in those situations when the robot ;is
part of a fle;x:i,'b~e technological system, in which the tasks are frequ-
ently changed, so that many of task elements cannot be determined in
advance, a,nd the cont;r-ol system has to make decisions on the basis of
the information obtained f;r-om the sensors, or other subsystems. Thus,
the assumption on the known (determined in advance) parameters of the
robot system becomes then untenable.
296

This chapter is devoted to the problems concerning the variation of the

robot parameters, and especially of the payload parameters. We shall
consider first the problems of control robustness to the variation of
payload parameters and then we shall tackle briefly the problem of im-
plementation of adaptive robot control.

6.2 Robustness of Control to Variations of Robot Parameters

If the robot (workpiece) parameters change during the work, the control
has to ensure the reliable and smooth functioning of the robot irres-
pective of the parameters variations. In other words, the robot control
should be robust to the changes of the workpiece parameters. The theory
of control robustness has been fully developed, so that a precise defi-
nition of the notion of robustness and different theoretical aspects of
this problem will not be the subject of our concern here. Instead, we
are going to outline several practical problems related to the variati-
on of workpiece parameters.

rt is obvi(!)u.s that the variations in workpiece parameters may substan-

tially influence the behaviour of the robot control system (the quality
of trajectories tracking, etc.). If the control has been synthesized
under the assumption that the workpiece parameters d have a value do
(where d i.s a vector of the workpiece parameters) and the changed pa-
rameters are do+AQ (Ad is the parameter change), it is obvious that the
robot's behaviour will not be the same as supposed in the control syn-
thesi.s. The changes of the workpiece parameters, such as the shape and
dimensions, influence the operation of grasping the object by the ro-
bot gripper. The problem of grasping, however, is beyond the scope of
th;is book. Most often, this problem is solved at higher control levels
(stJ:;'ateg;ic and tactical), and this is done on the basis of the infor-
mation obtained from various senSOrs (tactile sensors, proximity sen-
sors, cameras, etc.) (1]. The mass of the workpiece, its inertia moments
and dimensions (position of the mass centre) influence substantially
the robot dynamics, i.e. these parameters are included in the dynamic
model of the robot mechanism:

a(q, d)q + h(q, q, d) (6.2.1)

where, as we have already sa.id, d is a vector of the workpiece parame-

ters of the dimension 4xl. d = (mp , Jpx , J py , J p z
)T ' where mp is the
 297

workpiece mass, and J px ' J py ' J pz are the inertia moments about main
inertia axes (variations in the distance of the workpiece mass centre
from the gripper might be usually neglected). The variation in the pa-
rameters d, from the value do to do+~d causes a variation in the dyna-
mic behaviour of the robot. If the variation of parameters Ad is known
in advance, the control system can be prepared in advance to compen-
sate for this change. However, as we explained it above, this variati-
on is not, generally, known in advance, so that the control system
should be robust enough to overcome this variation, i.e. it should en-
sure that the change of parameters does not cause the robot's malfunc-
tioning.

Let consider briefly the robustness of particular components of con-

trol law, synthesized in the preceeding chapters, to the variation of
the workpiece parameters.

The local servo system at the i-th joint was synthesized under the as-
sumption that only the i-th joint moves, while all others are kept lo-
cked (Chapter 3). In Section 3.3.4. we discussed the influence of the
var;iations of the moment of inertia and gravitational moments on the
behaviour of the system. The workpiece parameters influence both the
inertia. I\lol\lent of the I\lechan.ism about the axis of the i-th joint and
grav,;tta.tiona,l, 1\l0men.t of the mechanism. It can be easily shown that the
moment of inertia about the i-th joint is a linear function of the work-
piece parameters d,:

(6.2.2)

0'
where Rii(d) is the value of the moment of inertia of the mechanism
about the i-th jOint for the case of the no-load gripper, H~. (q) >0 is
1.1.
a 4x1 vector whose all the elements are positive. Suppose the velocity
gain ot:· the servo system ~ ~as been ch,osen on the basis of (3.3.39),
for th,e mechanism position q and t:or the parameters values d , for
_ * 0
which the moment of inertia about th,e i-th jOint Hii(q , do) is:

(6.2.3)

If the mechanism is shifted to another position q, and the workp;iece

parameters becol\le do +Ad, the moment of inertia a.bout the i-th joint
will be:
(6.2.4)
298

On the basis of (3.3.55), the servo system damping at the i-th joint
for the joints position q and the parameters value do+Ad is:

(6.2.5)

where Ji
R

Now, an analysis can be carried out as in Section 3.3.4. As was shown,

in order to ensure the servo system is always (over)critically dam-
ped, i.e. that si~1, the chosen velocity gain should be such to cor-
respond to a maximal moment of inertia of the mechanism. Furthermore,
it should be ensured that -Hii(q * , do) ~ Hii(q, do+Ad), to satisfy
S. > 1. It is easy to show that this requirement can be fulfilled if such
1- *
mechanism position q is chosen, for which

-0
Hii (q )
* o
max Hii(q), (6.2.6)
q

and if do = max d, so that all the elements of the Ad have to be nega-

tive. Therefore, the servo system gains should be synthesized for the
largest allowed values of the workpiece parameters.

As we have explained above, it is not necessary the workpiece parame-

ters be known in advance, but for each robot in the stage of its de-
signing it is necessary to determine the range of variations of these
parameters, i.e. their maximum allowed values (the so-called, maximal
robot load - the largest workpiece mass which can be carried by the ro-
bot). If the servo system gains are calculated for the maximum values
of the payload parameters, the servo system will be (over)critically dam-
ped. However, if the allowed variation of robot parameters is large,
the servo system damping will change from the critical (for d=d o ) to
the highly overcritical (for d=O):

(6.2.7)

i T d *
This means, if J R + doHii (q ) + Hii (q ) »
-0 * .
J~ + H~i (q), then si (d=O)>>1,
i.e. for the no-load gripper, the servo system would be highly over-
damped, which indicates the robot's beh.aviour would be extremely
 299

nonuniform. As this nonuniformity is undesirable, such a solution is

not satisfactory.

As can be seen, the influence of workpiece parameters depends substan-

tially on the parameters of the robot mechanism itself, H~. (q*) and on
i i 110
the moment of inertia of the servomotor rotor JR' If J R + Hii (q ) »
*
d T H~. (q *), then the damping of the "no-load" robot, cannot be high,
o 11
i.e. the mass and the moment of inertia of the workpiece exhibit no
great effect. In other words, if the moment of inertia of the motor
rotor J~ and of the mechanism itself are significantly larger then the
T d
moment of inertia produced by the workpiece doHii (q * ), the influence
of the workpiece on the servo system's behaviour is not significant.
This means that in the case of the relatively large motors and heavy
mechanism links, compared to the nominal robot payload, the influence
T d *
of the payload is not significant. Contrary, if doHii(q )>> JR+Hii(q),
i -0 *
i.e., if the mechanism (and the motor) are relatively light in compa-
rison with the planned workpiece mass, the influence of the workpiece
may be great.

A similar analysis may also be carried out for the gravitational mo-
ments. The workpiece mass influences the gravitational moment about the
i-th jOint axis and contributes to the servo system's steady state er-
ror. The steady state error can be eliminated in different ways, as we
demonstrated it in Section 3.3.4. One of the ways is through the on-
-line calculation of gravitational moments. However, as the workpiece
parameters are not known in advance, compensation of the gravitational
moments, caused by the presence of the workpiece, cannot be achieved
in this way. How large will be the steady state error which the work-
piece causes at the i-th servo system, it depends on the gripper pOSi-
tion with respect to the i-th joint axis, as well as on the allowed
mass of the workpiece. If the servo system would always be compensated
with respect to the maximum allowed mass of the workpiece, then, in
the case of no-load gripper, it would cause a steady state error of the
opposite sign.

When all the robot joints move simultaneously, the workpiece parameters
affect the dynamic forces (moments) (6.2.1) that are loading the servo
systems. The workpiece parameters influence all the force components
that had been considered in Section 4.2. These forces may substantial-
ly change in dependence of the mass and the moments of inertia of the
workpiece carried by the robot (or, if the gripper is not loaded). The
300

dynamic control, whose task is to compensate dynamic force, must also

take into account the dynamics of the workpiece.

If the nominal centralized control is applied, it should also "include"

the working object. If the workpiece parameters (and their variation)
are known in advance, i.e. if it is known whether the gripper at a par-
ticular moment is carrying the workpiece (and what are the parameters
characterizing it), or the gripper is "empty", the nominal programmed
control can be calculated from the overall model (6.2.1) (including
also the workpiece). However, if the workpiece parameters are not known,
the nominal control has to be calculated for the absence of the work-
piece, which means that the workpiece dynamics will not be compensated,
not even at the nominal level (along the nominal trajectory), which
causes a delay in tracking of the trajectory. If, however, the nominal
centralized control was synthesized under the assumption of the maxi-
max payload parameters d, it might happen that the given trajectory is
"overshooted" which is, as we have explained before, unacceptable. If
the mass and moment of inertia of the workpiece are small in compari-
son with the mass and inertia moments of the mechanism links, the de-
lay in the trajectory tracking may be negligible.

A similar situation also arises when the local nominal control is ap-
plied: the control has to be calculated under the assumption of a mi-
nimal moment of inertia of the mechanism about the jOint axis, which
means that the no-load gripper should be assumed in order to avoid the
overshoots of the trajectory. Rowever, if such a control is involved,
the dyn,amics of the m,echanism and of workpiece become more pronounced,
beca~se the nominal con,trol does not compensate for the nominal mecha-

nism's dyp.amics.

Finally, if a,ll the requirements concerning the accuracy of fast tra-

jectories tracking a.re such that the global control has to be introdu-
ced this cop.t'rol should also cOmPensate the effect of workpiece dyna-
mics. If the global cOntrol introduced is in the form of force feed-
back, then the robot will be robust to the variation of workpiece pa-
rameters, because the force sensOrS measure the total dynamic forces,
irrespective of whether they come from the mechanism, or from the
workpiece (Section 5.2.1). Such control can compensate fOr the effect
of )::loth th,e mechani,sm dynamics and the workpiece dynamics, irrespecti-
ve of their para,meters. AS we explained a,bove, the major sho;r:tcoming
of this solution is the direct cop.nection between the sep.sors and the
 301

inpu,ts to the a,ctuators, which may cause the system to oscillate, a,s a
result of elastic effects directly "transmitted" to the control system.

If dynamic forces are calculated on-line using one of the approximate

models (Section 5.2.2), the workpiece parameters may be taken into ac-
count, provided they are known. If, however, they are not known, the
global control cannot compensate for the change of the workpiece para-
meters. The same holds for computed torque method (Section 5.4).

As can be seen from all the above, the extent to which the variation
in workpiece parameters will influence the robot's behaviour depends
mostly on the relative ratio of the allowed variations of the workpie-
ce (the robot's "capacity") and the mechanism (links) parameters. If
the maximum workpiece mass assumed is small in comparison to the mas-
ses of links, the workpiece effect may be considered a,s wea,k, a,nd the
local nominal a,nd globa,l control is sufficiently robust to overcome
this effect. If, on the other ha,nd, the maximum workpiece mass assumed
is of the same order of magnitude as (or larger than) the mass of ro-
bot links, then, the workpiece effect becomes significant, and the qu-
estion arises of whether the control synthesized is sufficiently ro-
bust to ensure a uniform a,ndaccurate tra,cking of tra,jectories (i.e.
posi tioning) when, the workpiece is cha,nged. Furthermore, if the links
ma,sses a,re sma,ll, or they are of the same order of magnitude, as the
ma,ss of the working object, some additional problems usually arise:
t)::le linkS )::la,ving small masses are relatively thin, so, when loaded
with "heavy" objects, the resulting etas tic effects are greater than
in the ca,se of "heavy" and "rigid" links. A ")::leavy" working object ca-
uses elastic bending of the links, which generates a new problem, either
fro!1l the point of view of the accuracy of positioning and the robot's
trajectories tracking, or the appearance of oscillations (especially
in the force feedbacks). The need to overcome the effects of these
elastic modes by the robot control system makes the synthesis and rea-
lization of the control much more complicated [2]. However, the novel
"elastic" robots that have appeared recently (suitable from the point
of view of material expenditure, energy consumption and reduction of
actuators power) have a relatively great allowed load and a more com-
plex control system capable of overcoming both the changes in workpiece
and the elastic effects. In all the above considerations, we have as-
sumed the robot links to be rigid; the control of elastic manipulators
is beyond the scope of this book.
302

It should be emphasized that the effect of the unknown workpiece mass

(as well as of other unknown and variable mechanism parameters) is
very important for the direct-drive robot. For this reason, the adap-
tive control has to be used with these robots, which will be conside-
red in the following section.

Example 6.2. Using the manipulator shown in Fig. 3.2, we shall demon-
strate how the workpiece parameters influence the manipulator's beha-
viour. We shall assume that a workpiece of maximal mass mp = 5 [kg]
might be placed at the mass centre of the third link, and the corres-
2 2
ponding inertia moments are: J px = 0.01 [kgm ], J py = 0.01 [kgm ], and
Jpz = 0.02 [kgm2]. Let assume first the servo systems at all three jo-
ints be synthesized without taking into account the workpiece (i.e.,
for mp = 0). The corresponding servo systems gains are given in Table
4.1. (see Example 4.3.2). The positioning of the first link for the
case of no-load and the case the robot is carrying workpieces of dif-
ferent masses is illustrated in Fig. 6.1. As we can see, the overshoot
of trajectories appears when the robot is loaded by a workpiece, beca-
use the system is undercritically damped. For this reason, the servo
systems gains hav€ to be synthesized by taking into account the maxi-
mum workpiece mass allowed. The gains synthesized are presented in
Table 6.1. and the corresponding simulation of positioning (Fig. 6.2)
shows the servo system is always overcri tically damped. It is also evi-
dent that the positioning time in the two cases is not the same.

O.5+-------------------------------~~~~~~,~ ---
/'
0.3 /'
/
/
/
/
0.1 ./

0.2 0.4 0.6 0.8 1.0 t[s]

Fig. 6.1. Positioning of the first robot joint for different

workpiece masses (the robot shown in Fig. 3.2. and
the gains in Table 4.1)
 303

q 1 [radl
0.5 ---
0.25

0.1

0.2 0.5 0.8 1.0 t[sl

Fig. 6.2. Positioning of the first robot jOint for different

workpiece masses (the robot shown in Fig. 3.2. and
the gains in Table 6.1)

JOINT 1 2 3
GAINS

V 132.5 128.6 1714.

Kp [raa l

V
Kv [raars l 60.2 13.1 166.

Table 6.1. Servo systems gains for the case of workpiece

mass m = 5 [kgl (the robot shown in Fig. 3.2)
P

If it is necessary to ensure the tracking of trajectories presented in

Fig. 4.5, then the nominal centralized programming control, calculated
under the assumption of the no-load gripper, should be introduced. The
tracking of trajectories using such nominal control for different work-
piece masses is illustrated in Fig. 6.3. It can be seen that the grea-
ter is the workpiece mass, the greater is the delay. The trajectory
tracking illustrated in Fig. 6.4. corresponds to the centralized con-
trol that has been calculated assuming the maximum workpiece mass. It
is evident that the tracking of trajectories by applying such control
for the no-load robot is not satisfactory.

Finally, if the tracking of trajectories shown in Fig. 4.5. is reali-

zed by using the local nOminal control, local feedbacks and global con-
trol in the form of force feedbacks (Example 5.2.1), the tracking of
trajectories is, practically, of the same quality, irrespective of the
workpiece parameters (Fig. 6.5). This illustrates the robustness of
such control to the variation of workpiece parameters.
304

Aq 1[rad]
0.02 _________ mp=S[kg] 0.02
••••.••••••• mp=2.S[kg]
0.01 0.01

·~ •.•?4 0.6 0.~.•••1/· t[s]

-0.01
,,~..... ::;/ -0.01
-0.02 for mp=O ---
-0.02
Aq 1-.n 2
><>V. Aq ",0.

Fig. 6.3. Tracking of trajectories using nominal centralized

control and local feedbacks, for different workpiece
masses (nominal control calculated for mp = 0)

Aql [rad]

0.02 0.02
0.01
0.01

0.6 0.8 1. t[s] t[s]

-0.01 -mp=O -0.01
••••••••• mp=2.S[kg]
-0.02 -0.02
for mp=S[kg] Aq 1,>J() Aq 2...o
Fig. 6.4. Tracking of trajectories using nominal centralized
control and local feedbacks, for different workpiece
masses (nominal control calculated for mp = 5 [kg])

0.03 Aq2[rad]
0.03
0.02 0.02

0.01 0.01

. t[s] 1. t[s]
-0.01

-0.02 -0.0

-0.03 -0.03

Fig. 6.5. Tracking of trajectories using local nominal control, local

feedbacks and global control in the form of force feedbacks
 305

E x ere 1 s e s
6.1. Prove the expression (6.2.2) is correct.

6.2. Explain the statement that the local programmed control should be
synthesized under the assumption of the "no-load" robot gripper
(in the case when the workpiece parameters are not known). How
can the workpiece parameters influence the tracking of trajecto-
ries by the local nominal control?

6.3. a) For the second joint of the manipulator presented in Fig. 2.6.
and the servo system gains in Example 3.3.4, calculate the
steady state error at the second joint, caused by the gravita-
tional moment at the position qo2 = 1.573 [rad], qo3 = 0.7 [m].
Use the data on the mechanism and the actuator at the second
joint from Example 3.3.4. Assume the workpiece masses at the
end of the third link are mp = 0 [kg], mp 1.5 [kg], m =3[kgl.
p

b) If the compensation of gravitational moment is introduced by

its on-line calculation (Fig. 3.15, equation (3.3.60)) find
out when a greater steady state error is made: (1) when the
compensating moment is calculated neglecting the workpiece
mass, and the workpiece mass involved is mp = 3 [kg], or, (2)
when the compensating moment is calculated for the workpiece
mass m = 3 [kg], and the positioning has been done for the
p
"no-load" robot. Calculate what will be the error in position-
ing of the robot hand caused by these steady-state errors.

6.4. Repeat the previous exercise a), but assuming now the third link
mass is m3 = 2 rkg], and the other data are the same as above.
Why the relative increase in steady state error when the robot is
carrying the heaviest object, in comparison to the "no-load" ro-
bot, is now much larger than in the preceeding case. Assuming the
tip of the third link is elastically bent for 0.02 [roml under the
action of a moment of 1 [Nm], calculate the error in the manipula-
or tip positioning, caused by the steady state error at the se-
cond joint and the elastic deformation of the link, for different
masses of the workpiece.

6.5.* Explain why the procedure for the analysis of practical robotsta-
bility through the analysis of the exponential stability (Section
306

4.A.2), when the centralized nominal control is applied, cannot

be used when the workpiece parameters has been changed with res-
pect to their "nominal" values (for which the nominal control has
been calculated).

6.3 The Concept of Adaptive Robot Control

Up to now we have considered the various control laws in which all the
control parameters (gains) are constant and which do not vary in de-
pendence of either the robot working regime, or the variation in the
robot's parameters. Such non-adaptive control can be, as we have seen,
sufficiently robust, so that the changes in workpiece parameters (as
well as in other variable parameters of the system) do not influence
the robot's behaviour. The control robustness depends on the chosen
law, the information included in that law, the choice of gains, as well
as on the ratio of the variable parameters and the known constant pa-
rameters of the system. A direct analysis of the practical robot sta-
bility can serve to investigate the robot stability for the variable
workpiece parameters, and thus, to test the robustness of the non-adap-
tive control to the variation of these parameters. Thus, it is possib-
le to determine the range of variation of the workpiece parameters for
which the non-adaptive control synthesized, can guarantee the robot
stability [3, 4]. Although the workpiece parameters are not known, it
is always possible to estimate the range of variation in workpiece pa-
rameters that is to be expected in a particular task. If the analysis
of practical stability shows the non-adaptive control chosen is suffi-
ciently robust to encompass the predicted range of parameters change,
such control can be considered as satisfactory. The majority of com-
mercial robots use exactly this solution: they make use of the control
with fixed gains, which assumes a certain allowable variation in the
workpiece parameters.

However, if the predicted variation in the workpiece parameters would

exceed the capability of the robust control to "overcome" it, the con-
trol with variable gains should be introduced. Such control can adapt
to the variation in workpiece parameters. For example, if a variable
velocity gain is realized at the local servo systems, it is possible
to achieve the servo systems have always the same damping, and thus
work uniformly, irrespective of the variation in workpiece parameters
(Section 3.3.4). The servo systems gains should change then in depen-
 307

dence of the robot position and workpiece parameters (i.e. in depen-

cence of the moments of inertia of the mechanism and the workpiece
about the joint axis). The adaptive control thus realized can guarantee
a satisfactory behaviour of the robot for a wide range of variations
in the workpiece parameters.

Adaptive robot control can be realized in different ways [5, 6]. In

the majority of the approaches, the control parameters and gains are
adjustedin dependence of the instantaneous values of the variable sys-
tem parameters. Thus in the above example, it is the velocity gains
that should adapt to the change of workpiece parameters. If the work-
piece parameters are known (defined at higher control levels, or via
the robot programming language), the executive level should only adjus-
ed the servo systems gains in dependence of the parameters values. In
other words, in the stage of task planning at the strategic level, or
in the course of the robot programming, the workpiece parameters should
be defined and servo systems gains determined in accordance with the
instantaneous values of workpiece parameters. The gains can be calcu-
lated on-line according to the procedure described in Section 3.3.2,
and they are determined as a function of moments of inertia of the me-
chanism and workpiece about the joint axis. Alternatively, the gains
can be calculated in advance for different moments of inertia (work-
piece parameters), stored in the computer memory, and, when the infor-
mation about the workpiece parameters is obtained, those gains values
are taken from the memory that correspond to the actual values of the
payload parameters.

However, if the payload parameters are not known in advance, it is ne-

cessary to ensure their id~ntification in the course of the robot wor~

The on-line identification of these parameters can be realized in dif-

ferent ways. The information about the error of tracking of a given
trajectory may be the basis for the identification of variations in
the system's parameters. This identification is also possible to rea-
lize via the functions of sensitivity of the robot model to the para-
meters variations. However, such an identification usually requires a
large amount of calculation, which is obvious if we bear in mind the
complexity and nonlinearity of the robot model. Because of the need to
determine robot parameters in on-line regime (in the course of the mo-
vement), in order to ensure a fast adaptation of control parameters,
the identification can be implemented only with the aid of a fast and
powerful microcomputer. The identification can be considerably accele-
rated and simplified by introducing force sensors to measure either
30B

moments at robot joints, or the forces at the points of contact of the

payload and the gripper (see Section 7.3). On the basis of the infor-
mation about forces (moments), the parameters of the working object
can be determined in a relatively simple way (with much less calcula-
tion involved). In this way, the transition process which lasts while
the identification algorithm is carrying the determination of workpie-
ce parameters and while the adjustment of servo gains is completed, is
substantially shorthened.

The structure of such adaptive control is illustrated in Fig. 6.6. This

approach to adaptive control is called the indirectly decentralized
adaptive control, because it retains a decentralized structure with
respect to the local servo systems [6]. The advantage of such control
is in its relative simplicity: it does not require a great deal of cal-
culation and neither substantially complicates the control structure.

Certainly, this is only one of the possible variants of adaptive robot

control. Various algorithms have been developed for centralized [7, B]
and decentralized [9,10] adaptive control, suchasaretheseZf-tuning
(decentralized or local) PID controllers, the approach using the model
referenced adaptive control etc.

The main problem with all these algorithms for adaptive control is their
numerical complexity, which makes their implementation difficult and
expensive. For this reason, it is always advisable to examine if the
implementation of such control is necessary, or the problem can be sol-
ved using the robust non-adaptive control, whose implementation is usu-
ally much simpler.

Example 6.3. For the manipulator presented in Fig. 3.2. we have synthe-
sized the non-adaptive control in the form of local nominal control
and local servo system feedbacks whose gains are given in Table 4.1
(Example 6.2). The tracking of nominal trajectories (Fig. 4.5) fordif-
ferent workpiece masses is illustrated in Fig. 6.7. As can be seen,
the tracking for the workpiece masses m = 5 [kg] is much less satis-
p
factory than in the case of the no-load robot. For this reason, the
adaptive control is included (Fig. 6.6) in which the identification of
workpiece mass is realized on the basis of the force measurements at
the points of contact of the gripper and the workpiece. On the basis
of the identified mass (and moments of inertia) of the workpiece, the
mechanism moments of inertia and velocity servo gains are calculated
 P1~
I x1
Actua tor (1) I
~
Force sensor I
Local controller
'--- (1) with r--
variable gains I--
P1
Identification
of workpiece • dIN xo1 (t) L.,
x1 t1echani ca 1
parameters xi
accordi ng to x2 -------~
1--'----- ------- part
measured xn
forces and of the robot
robot state t- ,.. Pn
Pn
t
Actuator (n)
I xn Force sensor I
H
Local controller
L--- (n) with
variable gains ---xon(t)
--=-
, dIN

Fig. 6.6. Structure of indirect decentralized adaptive control with identification

of workpiece parameters via force measurement and robot states
w
o
<0
310

on-line, which should be such to ensure the damping of all servo sys-
tems is permanently critical.

The tracking of trajectories for different workpiece !'lasses is illustra-

ted in Fig. 6.8. It can be seen that during the transition process,
while the workpiece para!'leters are identified and the gains adjusted,
the tracking is poor, but it improves from the moment of completion of
the transition process. As the transition period is short, the adapti-
ve control ensures a satisfactory tracking of trajectories for a wide
range of changes in workpiece parameters.

l.Iq 1 [radj

0.02 0.02

0.01 0.01

-0.01

Fig. 6.7. Tracking of nominal trajectories with non-adaptive

decentralized control for different workpiece masses

l.Iq 1 [rad]
0.02 0.02

0.01 0.01

-0.01

-0.02 -mp=o -0.02

- - - mp =5[kg]

Fig. 6.8. Tracking of nominal trajectories with adaptive

decentralized control for different workpiece
masses
 311

E x ere 1 s e s
6.6. For the manipulator presented in Fig. 3.2. determine the velocity
gains at all three servo systems, so that all of them are criti-
cally damped for the position qol = 0, q02 = 0, go3 0, if the
workpiece parameters are: a) mp = 2.0 [kg], J = J = J pz =
2 px py
0.005 [kgm ]; b) mp = 5 [kg], J
px
= J
py
= J
pz
o. 01 [kgm 2]; c)
mp = 10 [kg], J px = J py = J pz = 0.02 [kgm 2 ]. The manipulator data
are given in Table 3.1. and Table 3.2., while the resonant frequ-
encies of the structure about the particular joints for mp=O are
given in Exercise 3.28. and Exercise 3.31.

6.7. Determine the number of operations (adds and multiplies) to be

carried out for the given workpiece parameters (m , J , J , J )
. P px py pz
and the given mechanism position gOl to determine inertia moments
of the mechanism about all three jOint axes of the manipulator
shown in Fig. 3.2., and on the basis of them, calculate the velo-
ci ty servo gains such that the servo systems are critically damped
(see the preceeding exercise). Try to minimize the number of ope-
rations.

6.S. If the force feedbacks are used to identify the workpiece parame-
ters for the robot shown in Fig. 3.2., the algorithm for one ite-
ration to calculate the mass and inertia moment of the workpiece
requires ns = 30 adds and n M = 25 multiplies. Taking into account
the result of the preceeding problem, determine the number of
microprocessors to be used in parallel to identify the workpiece
parameters, calculate inertia moments of the mechanism about all
three jOints, and calculate the velocity gains at all three ser-
vos (as in the preceeding problem) every 50 [ms], using the micro-
processor:

a) INTEL-80-S0 (one add operation lasts O.S [ms], and one multi-
ply 1.5 [ms]), or

b) INTEL-80-S7 (add 35 [~s], multiply 65 [~s]).

Assume the machine time is used only for addition and multiplica-
tion (calculation of a sine or a cosine function is equivalent to
one multiply plus two adds).
312

6.9~ Write in a high programming language a programme for on-line cal-

culation of velocity gains for all three joints of the robot shown
in Fig. 3.2. as a function of workpiece parameters (m p ' J px ' J py '
J ) which should be considered as the input variables to the
pz
programme. Determine the velocity gains such that all three servo
systems are critically damped for all three values of workpiece
parameters and all mechanism positions qOi. The instantaneous po-
sitions of the mechanism joints qOi should also be considered as
input data. (For the input data qOi and mp ' J px ' J py ' J pz ' de-
termine the outputs K~, i = 1,2,3). Try to minimize the number of
operations.

6.10.* Assume the velocity gains for the servo systems of the robot pre-
sented in Fig. 3.2. have been calculated in advance for 6 diffe-
rent values of workpiece mass mp = 0, 1, 2,3, 4, 5 [kg] and then
stored. When the identification algorithm has determined the ac-
tual workpiece mass, the velocity gain is obtained by the linear
interpolation in between the stored values. Write in a high pro-
gramming language a programme which will, for the input value of
the workpiece mass mp ' calculate the velocity gains for the servo
systems by linear interpolation, using the stored values. How
much this programme is simpler than the programme in 6.9? As the
velocity gain in this case changes only as a function of work-
piece mass, what are the robot's jOints positions for which the
velocity gains should be calculated and stored? Explain what are
benefi ts and what is lost by assuming the velocity gains vary only
in dependence of the workpiece mass, and not of the robot posi-
tion?

6.11.* Explain what are the advantages and what are disadvantages of in-
direct adaptive control via the force measurement in comparison
to global force feedback (Section 5.2.1). (Suggestion: Take into
account elastic effects of robot links and their "transmission"
to the control system, on the one hand, and the complexity of
calculation, on the other).
 313

References

[1] Vukobratovic M., (Ed.), Introduction to Robotics, Springer-Verlag,

1988.

[2] Book J.W., Maizza Neto 0., Whitney E.D., "Feedback Control of Two
Beam, Two Joint Systems with Distributed Flexibility", Journal of
Dynamic Systems, Measurement and Control, Trans. of the ASME, Vol.
97, 424-431, 1975.

[3] Vukobratovic M., Stokic D., Kircanski N., "Towards Non-Adaptive

and Adaptive Control of Manipulation Robots", IEEE Trans. on Auto-
matic Control, Vol. AC-29, No 9, 841-844, 1984.

[4] Stoki6 D., Vukobratovi6 M., "Is Adaptive Control Necessary for
Manipulation Robots, and if so, to what Extent?", Proc. of Symp.
Third ICAR, Versailles, 1987.

[5] Vukobratovi6 M., Stoki6 D., Kir6anski N., Non-Adaptive and Adap-
tive Control of Manipulation Robots, Monograph Series: Scientific
Fundamentals of Robotics 5, Springer-Verlag, 1985.

[6] Vukobratovi6 M., Kircanski N., "An Approach to Adaptive Control of

Robotic Manipulators", Automatica, Vol. 21, No 6, 1985.

[7] Dubowsky S., "Application of Model Referenced Adaptive Control to

Robotic Manipulators", Journal of Dynamic Systems, Measurement,
and Control, Trans. of the ASME, Vol. 101, 1979.

[8] Timofeev A.V., Ekalo YU.V., "Stability and Stabilization of Pro-

grammed Motions of Robots", (in Russian), Automatica and Remote
Control, No 20, 1976.

[9] Kiovo A.J., Guo T.R., "Adaptive Linear Controller for Robotic Ma-
nipulators", IEEE Trans. on Automatic Control, Vol. 28, No 20, 1983.

[10] Landau Y.D., Adaptive Control, M. Dekker, New York, 1979.

Chapter 7
Control of Constrained Motion of Robot

7.1 Introduction

In all the tasks we have considered up to now, the robot does not come
into contact with the objects in the workspace, apart from those that
are being transferred. However, this does not hold for the one of the
most important industrial application of robots, namely, for the as-
sembly of machine parts. In this case, the robot comes into contact
with the objects in its environment and experiences actions of the
external environmental forces. Similarly, in the processes like cut-
ting, grinding, polishing and forging, the robot gripper has to act
upon the given object by certain forces. These external forces acting
on the robot gripper make the robot control much more complex. Hence,
this chapter will be devoted to the synthesis of control for the ro-
bots involved in the realization of the tasks of this type. First our
attention will be focused on the assembly process, as one of the most
important and most delicate tasks in which the action of external for-
ces is encountered. However, some general approaches to control of
constrained motion of robots will be also presented.

If the robot comes into contact with the objects in the workspace, the
reaction forces acting upon the robot are the functions of both the mo-
ments in the joints and coordinates and velocities of all the joints.
When a robot moves in free space. it represents an open kinematic chain;
when it comes into contact with the external objects, the robot beco-
mes a closed kinematic chain.

Dynamic models of closed kinematic chains are more complex than the
models of open kinematic chains, which makes the synthesis of the ro-
bot control more complex.

First, we shall consider briefly the problems encountered in the robo-

tic assembly processes, from the point of view of both modelling and
control synthesis. Then, we shall present some of the solutions to these
problems that are nowdays successfully used in the industrial robotics,
and then we shall consider some schemes for solving constrained motion
control of robots in general case.
 315

7.2 An Analysis of Assembly Process by Robots

Assembly of parts is one of the most sophisticated tasks of the indus-

trial robotics. Therefore, the solving of this problem is of greatest
importance, especially if we bear in mind the application of robots in
the flexible technological lines. As is known, the industry automation
is achieved by application of either specialized machines-automates,
or the robots for automatic assembly of parts. The application of highly-
-specialized automates is justified only in the mass production of the
one and the same products in large series (let say, at least one mil-
lion copies a year, and for a number of years). In the case of smaller
series, the development and application of the highly specialized ma-
chines is not justified any more, because these machines usually can-
not be used when some, even minor, changes in the shape and dimension
of the product are to be introduced. As the products of modern indus-
try are manufactured in the relatively small series, which are in ad-
dition, frequently changed, the introduction of flexible technological
systems is the preferable choice for the realization of automated pro-
duction. The central role in these flexible system is played by robots,
which enable an easy reprogramming and switching from the production
of the one product to another. Therefore, it is essential that the pro-
cess of assembly by robots is effectively solved, not only from the
point of view of the efficiency, accuracy, and speed of work, but al-
so from the point of view of an easy change of the working task and
maximal flexibility (though the robot flexibility is inevitably limi-
ted by its mechanical characteristics and adaptability to the varia-
tion in task conditions).

Assembly processes may vary substantially in dependence of the type of

elements involved, the mode of their joining, etc, but in all cases,
the following stages can be distinguished: the stage of approaching
the object, the stage of its grasping, the stage of transferring the
object to the sp0t of assembling, the stage of its joining to another
element, etc. It is clear that the robot in realizing this complex pro-
cess has to perform three kinds of motion: the so-called "gross motion"
related to the robot movement in the obstacle-free space (e.g. the
transfer of the working object from one place to another, etc), "fine
motion", related to the robot motion in an environment containing ob-
jects (for example, the stage of grasping the workpiece, the stage of
mating, etc), and "interface motion", representing all transition
kind of motion between gross motion and fine motion (for example, the
316

stage of approaching the workpiece, or, the stage just before mating
the objects, etc). In the preceeding chapters we have considered the
gross motion: the robot moves with a high speed along the given tra-
jectories (or, between the given positions), and, as we have seen, the
degree of accuracy required for the realization of this motion deter-
mined whether dynamic or nondynamic control had to be synthesized
(Chapters 4-6). The interface motion, which, according to some analy-
ses [1], takes the greatest amount of the assembly time, can be redu-
ced substantially by ensuring a high precision of the gross motion (in
the robot stopping and in approaching the workpiece). In other words,
the control synthesis for the interface motion can be carried out in a
way quite analogous to that used for the gross motion.

Here we shall focus our attention on the realization of the fine moti-
on in case the robot gripper comes into contact with the objects in
its environment. More precisely, we shall focus our attention on the
very stage of mating two elements, which represents the most sophisti-
cated stage in an assembly process. The stage of parts mating may be
realized in various ways, depending on the nature of the elements in-
volved; one of the possibilities is to use two robots, each of them
holding one of the two elements that are to be assembled. In princip-
le, the mating stage can be reduced to the problem of inserting the
workpiece (peg) held by the robot into a fixed hole, as shown in Fig. 7.1.
It is obvious that the assembly process involving bilateral manipula-
tion (two manipulators-robots) may also be reduced to this case.

The problem of controlling the robot during the assembly of machine

elements (insertion of the peg into a fixed hole) is, in principle,
the problem of accura te posi tioning. If all geometrical parameters of
the peg and of the hole and their positions in space were ideally known,
and if the robot positioning (tracking of the given trajectories) was
ideally accurate, then, it would be possible to insert the peg into the
hole using only the robot control that has been considered in the pre-
vious chapters. However, as none of the conditions could ever be ful-
filled, the peg insertion into the hole is inevitably accompanied by
the occurrence of contact between the peg and the hole, which results
in the appearance of the reaction forces. The effects of these forces
are of crucial importance for a successful assembly operation. To pre-
vent potential demaging of the assemblying objects, it is obvious that
such robot control should be ensured which would minimize these forces.
However, it should be noticed that in some tasks the robot itself has
to exert certain force on the elements, in order to realize the assem-
 317

bly operation. Hence, the appearance of the reaction forces between

the elements at the stage of their mating is practically unavoidable.
Therefore, to synthesize the control which would take into account the
effects of these forces, it is necessary to carry out a detailed ana-
lysis of all the diverse phenomena appearing in this process. First of
all, it is necessary to analyze the static and dynamic forces acting
via the workpiece upon the robot gripper.

Fig. 7.1. Mating phase of the "peg-in-hole" task

The analysis of the assembly process may be approximately reduced to

an analysis of the static forces appearing between the workpiece (ro-
bot gripper) and the hole. Of course, a complete and precise notion of
all the aspects of this process can be obtained by a dynamic analysis,
i.e. by setting out a complete dynamic model of the robot experiencing
the reaction force of the hole [2]. Now we shall analyze briefly the
forces appearing during the insertion operation and the situations
which may arise in the stage of the insertion.

Consider the insertion of a cylindrical object (peg) whose base diame-

ter is 2rm into a cylindrical hole of the diameter 2Rm' Itis straight-
318

forward that the quantity 2(R -r ) represents the clearance gap bet-
m m
ween the object and the hole. Obviously, the larger this gap is, the
simpler is the insertion process. For this reason, the assembling ca-
pability of a robot is judged from the minimal value of the gap betwe-
en the elements that can be assembled by the robot without causing any
damage to the objects, while ensuring fully reliable assembling of
the elements. To insert the peg into a hole, it is necessary to ensure
the peg is accurately positioned at the mouth of the hole, which is usu-
ally achieved on the basis of either precise information about the dis-
position (arrangement) of the objects in the workspace (known in ad-
vance), or the sensory information (visual feedback, and similar). The
task of precise initial positioning is of the greatest importance for
a successful assembly operation.

Let us analyze briefly the forces which can arise due to the peg-hole
interaction when the peg is brought to the mouth of the hole [2, 31.

The purpose of this analysis is to examine the effect of friction and

the problem of potential jamming, as well as the possibility of using
force feedbacks for controlling the manipulator with the aim of avoi-
ding the undesired effects of hole reaction forces on the insertion.
As we have already pointed out, regardless of how well the manipulator
has been positioned, contact between the peg and the hole may occur,
so that reaction forces result.

In this force analysis none of the aspects of dynamics is neglected.

It is assumed that the dynamic coefficient of friction between the peg
and the hole surface is the same everywhere and that it equals the sta-
tic coefficient of friction. This assumption has been introduced for
simplicity sake, but may easily be removed. Furthermore, it is assumed
that both the peg and hole walls are completely rigid, so that, in our
considerations deformation is not considered as this would make the
model much more complex.

Obviously, the given peg and hole geometry ensures that, upon inserti-
on, contact occurs simultaneously at most two points. (This does not
hold for special cases of coaxiality of the peg and cylindrical hole,
when contact may occur at an infinite number of points along the joint
generatrix). One of the two contact paints has to be between the hole
edge and the peg cylindrical surface, and the other on the edge of the
peg cylinder base and hole cylindrical surface. It is assumed that the
 319

peg has already entered the hole, i.e. no jamming has occured at the
hole edge itself.

In analyzing the forces arising during the insertion, four possible

situations are considered (under the above assumption that the peg has
already passed the hole base) [2].

1. No contact occurs between the hole and the object.

2. One contact point exists.

3. Two contact points exist.

4. Contact is realized along the joint generatrix.

Let us consider each of these situations, respectively.

Case 1, when there is no contact between the hole and peg, represents
free manipulator movement without reaction forces from the hole, so
this situation does not differ dynamically from the task of transfer-
ring the workpiece in free space along a desired trajectory and with a
desired orientation. The manipulator dynamics is described by the ma-
thematical model of open chain dynamics.

Case 2 introduces the problem of the unknown reaction force acting upon
the manipulator and affecting its dynamics. The problem of determining
the reaction force at the hole-object contact points arises. Fig. 7.2.
illustrates the case of contact between the hole edge and the cylindri-
cal surface of the peg, while Fig. 7.3. shows another possibility of
single contact, namely, contact between the edge of the cylindrical
base of the peg and the cylindrical surface of the hole. Both figures
show the hole and peg section along the plane determined by the con-
tact point and the symmetry axis of the hole cylinder. The reaction
forces at contact points are marked in the figures. Three components
of the reaction force at the point Kl or K2 are to be determined in
the direction of the coordinate frame axes fixed at the contact point.
In fact, the perpendicular component of the reaction force NKl (per-
pendicular to the peg cylinder generatrix on which the point Kl is si-
tuated, i.e. perpendicular to the hole cylinder generatrix on which K2
is situated) is to be determined, as well as the tangential component
TKl (or TK2 ) in the direction of slipping (if it occurs) of the peg
along the hole edge (Fig. 7.2) or hole surface (Fig. 7.3).
320

Fig. 7.2. Case of one contact point Fig. 7.3. Case of one contact point
in assembly process - con- in assembly process - con-
tact between hole edge and tact between edge of peg
peg surface and hole cylinder surface

Two cases are possible. Case 2.a), when friction is such that slipping
exists in the peg-hole contact, and 2.b), when no slipping occurs, so
that the peg has no tangential linear velocity. Assuming that the coef-
ficient of friction ~ between the peg material and hole material is
known, the following condition should be investigated

i 1,2 (7.2.1)

In case 2.a) the peg has a linear velocity at the contact point in the
direction of TKi and the reaction force has to satisfy the slipping
condition

i 1 ,2 (7.2.2)

In case 2.b) the reaction force at the momentary contact point satisfies
the condition (7.2.1). In case 2.a) the contact point changes, and the
tangential linear velocity of the object at the point Ki has to be in
the direction TKi . In case 2.b) the peg can have only rotational velo-
city about the point Ki and the contact point does not change. This
imposes certain constraints on the peg acceleration, namely, the peg
as a rigid body can have three degrees of freedom, i.e. three rotation
accelerations about the contact point Ki, while the three degrees of
freedom of translation with respect to the contact point are constrai-
ned (linear accelerations in case 2.b) at the point Ki are equal to
zero). It is quite obvious that in case 2.a) the peg's linear velocity
at the point Ki can have a tangential component only, while the velo-
city component in the same direction of but in the sense opposite to
NKi must be equal to zero. (If this velocity component has the same
direction and sense as NKi' contact is only fictitious) .
 321

In case 2 the forces acting upon the peg are: the inertia force of the
peg itself Fo , the peg gravitational force G0 = mp g, the forces by
which the manipulator (i.e. the gripper) acts upon the peg R~ (~ = 1,
2, ..• ,L, where L denotes the number of contact pOints between the peg
and gripper at which forces R~ act on the peg), and the reaction force
RK1. due to contact at the point K .. The dynamic equilibrium equation
1
of forces acting upon the peg (according to D'Alembert's principle) is

(7.2.3)

The moments acting on the peg round the centre of mass are: the moment
due to the inertial forces of the object itself Mo' the moment from to
the manipulator via the gripper, i.e. via forces at the gripper-peg
contact points, and the moment due to reaction forces at the object-
-hole contact points. Equilibrium of the moments about the centre of
mass of the peg (according to D'Alembert's principle) is

L
+ + + \ + +
Mo + r K · x RK . + L r 0 xR = 0 (7.2.4)
10 1 ~=1 ,,0 ~

-+
where r Kio is the position vector from the centre of mass of the peg 0
to the contact point Ki, ~£O (~ 1, 2, ... ,L) is the position vector
from the peg's centre of mass 0 to the peg-gripper contact pOint.

Equations (7.2.3) and (7.2.4) determine the peg dynamics in the case
of a single point of contact between the object and hole.

The manipulator dynamics represents the dynamics of a closed kinematic

chain.

Case 3, when two points of contact exist, is illustrated in Fig. 7.4.

Contact points K1 and K2 are the same as in Case 2. Depending on the
character of the reaction forces at contact points, the following ca-
ses may occur: Case 3.a), slipping occurs at both contact points; case
3.b) no slipping occurs at any contact point - the peg is jammed; 3.c)
it may happen that slipping occurs at one contact point and not at the
other - in that case the peg rotates about the point without slipping
and loses contact at the other point (if this rotation is permitted
by the geometry). However, it may also happen that slipping cannot oc-
cur at one contact point (e.g. K2) but can occur at the other (K1),
the peg would then rotate about K2, but this is not possible on account
of the geometry. In this case, the object becomes jammed.
322

Which of these three cases will occur it depends on the nature of the
reaction forces at the points K1 and K2, i.e. the following conditions
should be investigated.

(7.2.5)

If the condition (7.2.5) is satisfied, there is no slipping at any con-

tact point and case 3.b) occurs. In case 3.a) the reaction forces sa-
tisfy the following conditions

(7.2.6)

In case 3.c) a combination of these conditions occurs, namely,

(7.2.7)

However, in this case, it should be checked whether the peg rotation,

which would result on account of the condition (7.2.7) is permitted by
the hole geometry. If it is not, jamming which is dynamically undeter-
mined will occur, a peg deformation may result, which would require
the introduction of new degrees of freedom (peg and hole flexibility
and the like). The dynamics of peg jamming will not be considered here
since the elements are assumed to be absolutely rigid bodies and mani-
pulator control should ensure that peg jamming and deformations are
avoided. It will suffice to note that no jamming occurs during assem-
bling, i.e. that manipulator control is synthesized in such a way that
jamming is avoided. Force feedback must ensure, therefore, that the
peg always gets from the manipulator a moment contrary to the external
forces moment. Accordingly, case 3.b) should be avoided by applying
appropriate control. In case 3.b), since there is no slipping at con-
tact pOints, both the rotational and linear peg acceleration are con-
strained to zero, which gives 6 conditions for determining the reacti-
on forces that are to satisfy the condition (7.2.5).

In case 3.a) the peg may have only tangential linear velocity at con-
tact points (if geometry permits them), while linear velocity and ac-
celeration at point Ki in the same direction of but opposite sense to
NKi are forbidden, which, together with the condition (7.2.6) provides
the necessary conditions for determining the reaction forces at points
Ki.
 323

In case 3.c) conditions (7.2.7) and those constraining peg acceleration

to exist in the same direction of but opposite sense to NKi yield the
conditions necessary to determine the reaction forces and peg accele-
rations; the realizability of these conditions will depend on the geo-
metry.

Fig. 7.4. Case of two contact Fig. 7.5. Case of contact along
points joint generatrix

The equations of dynamic equilibrium of forces and moments acting on

the peg in this case are of the form

(7.2.8)

o (7.2.9)

Case 4 can be reduced to case 2. The reaction force due to the hole
acts on the peg along the joint generatrix of the peg's cylindrical
surface and hole cylinder (Fig. 7.5). As the reaction force acts uni-
formly at all contact pOints along the generatrix, the reaction forces
->-
may be represented by one resultant force RK acting at the mid-point
of the line of contact between the peg and hole. This special case may
be treated like the case with one contact point. Equations (7.2.1) -
(7.2.4) hold, so that either slipping or jamming may occur (i. e.
the peg cannot slip along the generatrix), but in the last case rota-
tion separating the peg from the hole wall is possible and case 2.
occurs (if rotation is permitted by the geometry) .

Let us construct the mathematical model of the robot during the inser-
tion process. Starting from the model of the robot dynamics (3.2.2)
for the robot moving in free space (open kinematic chain), the mathe-
matical model of the robot dynamics during the insertion of the peg
into the hole can be written in the form [3]

p (7.2.10)
324

where the notation used is the same as in (3.2.2), J K1 , J K2 are the

(nx3) matrices, determined by the position vectors of the peg-hole con-
tact points K1 and K2 with respect to the joints axes, RK1 , RK2 are the
(3 x 1) vectors of the reaction forces of the hole, while 0 1 , O2 are Kro-
necker's symbols which can assume the following values:

i f there is contact at point K1

{~ i f there is no contact at K1

f:
i f there is contact at point K2

i f there is no contact at K2

The reaction forces RK1 and RK2 are complex functions of the angles,
velocities and accelerations of the joints (i.e. of the driving moments
P at the mechanism joints). Here, we shall not be concerned with the
problems of expressing these forces as explicit functions of q, q and
q but we shall retain the model in the form of (7.2.10).

However, the model (7.2.10) is not smooth. At the instants of the ob-
ject-hole contacts (when 6 1 and O2 change their values), theoretically
"instantaneous jumps" of the system state vector (q, q)
occur as a con-
sequence of the impact of the elements. Modelling of the impact pro-
cess is extremly complex so that the model of the robot dynamics too
is difficult to compose, to simulate it in an exact way, and thus ana-
lyze it. It is quite clear that at the moments of the impact, after
instantaneous changes in the velocites (and positions) of particular
joints, the model of the robot dynamics changes its structure: from an
open kinematic chain (3.2.2) it becomes a closed kinematic chain (7.2.10).

By combining the model of the mechanical part of the robot (7.2.10)

and the models of the actuators at robot jOints, the global dynamic
model is obtained describing the robot in the assembly process.

EX ere 1 ses
7.1. For the manipulator presented in Fig. 7.6, determine the matrix
J K1 in the model (7.2.10) if the peg-hole contact is as shown in
the figure. Write the matrix according to the notation used in
the figure (the robot has n=4 jOints). Show that J K1 represents
Jacobian matrix for the contact point.
 325

\111/
q

Fig. 7.6. Cylindrical robot (with 4 d.o.f.) in assembly process

7.2. If the models of the actuators at the robot jOints are given by
(3.2.4) (second order models), write the global robot model in
centralized form (analogous to (3.2.27» if the peg insertion
process proceeds with reaction forces acting on the working ob-
ject. The model of the mechanism dynamics is given by (7.2.10).
->- ->-
(In the total model, the reaction forces RK1 and RK2 should be
retained) .

7.3 Robot Control in the Stage of Parts Mating

Control of the robot during the assembly process involves solving two
major problems: planning of such trajectories which will provide the
elements are brought to a desired mutual position and ensure their ma-
ting, and reducing the (undesired) reaction forces acting between the
assembled elements. These two problems are interrelated: the proper
planning of trajectories diminish the error in mutual positioning of
the elements, and thus, reduce reaction forces. On the other hand, re-
action forces provide the basis for planning the trajectories (to mo-
dify the given trajectories) yielding the parts mating.

In order to reduce the effect of reaction forces acting between the

elements it is advisable to measure these forces so that the informa-
tion obtained can serve as the basis for realization of suitable con-
326

trol. Thus a visual feedback (camera) or proximity sensors enable plan-

ning of the robot trajectories to the very stage of parts mating, i.e.
to the moment just before the insertion of the peg into the hole. Ho-
wever, during the insertion itself, the measurement of reaction forces
provides the best information about the mutual position of the elements
being mated. Because of that, the robotic assembly process is often
solved with the aid of force sensors, introduced to measure the reac-
tion forces acting between the peg and the hole.

As for the disposition of force sensors, different solutions are pos-

sible. They can be placed either on the robot itself, or mounted on
the support of the assembled object (at the hole). Obviously, the for-
mer solution is more convenient from the pOint of view of flexibility
and reprogrammability, and it does not require changes in the robot
environment. Sensors can be placed at different points on the robot.
The closer the sensors are to the contact point of the object and hole,
the more accurate information about reaction forces is obtained on the
basis of force measurement. One of the possible solutions is to mount
the force sensors on the gripper, just at the contact points of the
workpiece and the gripper (Fig. 7.7) [4]. The force sensors at contact
pOints measure the object-hole reaction forces (if contacts exist) and
the dynamic forces of the object itself. Namely, the forces at the con-
tact points of the object and the gripper, Ri' have to satisfy relati-
+
ons (7.2.8) and (7.2.9). In order the measurement of R ~ would supply an
accurate information about the reaction forces RK1 and RK2 , it is ne-
cessary to calculate (on the basis of the known robot angles q, velo-
cities q and accelerations q) both the inertia force of the object Fo
and moment ito due to the inertia forces of the obj ect. Obviously, for
relatively low speeds of the gripper motion and small mass and inertia
moment of the object, these forces can be neglected. A shorthcoming of
such a solution to sensors disposition is that, because of the need to
measure relatively small forces, sensors of high precision and sensi-
tivity have to be used. On the other hand, from the point of view of
design, it might be difficult to install sensors at the contact points.

A solution which is used most often is to mount force sensors at the

gripper (wrist) jOint (Fig. 7.8). This is usually realized in the form
of the so-called Maltese cross by which the forces and moments at the
wrist joint are measured [5, 6]. The sensors are disposed in all four
cross branches in order to obtain direct information about all three
force components and all three moment components appearing at the grip-
per jOint. However, the measured components of forces and moments do
 327

not represent the direct information about reaction forces acting at

the working object, because, in these forces are also included the dy-
namic forces of the gripper and working object. Since the gripper mass
is usually much larger than, or equal to, the workpiece mass, the mea-
sured forces provide "worse" information about reaction forces if com-
pared with the above solution of force sensors disposition (Fig. 7.7).
On the other hand, to measure force in the wrist joint is technically
much simpler. Besides, as the gripper velocities and accelerations du-
ring the assembly process are small, dynamic forces forces and moments
of the gripper and workpiece can be neglected. (It should be borne in
mind that the acceleration of the workpiece and gripper in the direc-
tion of the hole reaction forces is equal to zero).

Fig. 7.7. Experimental gripper with force sensors at

the object-gripper contact points [4]

Certainly, there are some other technical solutions for sensors pla-
cing on the robot (for example, force sensors can be placed on the ro-
bot base, or in the jOints, as considered in Section 5.2, and the like).
It is obvious that the "further" from the workpiece forces sensors are
placed, the "more fouled" information about the reaction forces is ob-
tained, so that the dynamic robot model has to be used to calculate,
on the basis of the measured forces, the actual reaction forces acting
upon the workpiece.
328

equipped with
strain gauges

Fig. 7.8. Typical device with force sensors at

the wrist jOint [5, 6J

Regardless of where they are situated, force sensors provide informa-

tion about the reaction forces acting between the objects in the as-
sembly process (i. e. about reaction forces of the hole on the workpie-
ce held by the gripper). After these forces have been measured, it is
possible to establish a feedback loop from the sensor (i.e. from the
calculated momentary values of reaction forces) to the inputs of the
actuators at the jOints. In other words, some additional signals (de-
pendent of the reaction forces measured) should be fed to the actua-
tors whose task is to realize the compensating movements of the robot
joints and thus, minimize the reaction forces. Therefore, such compen-
sating control should be realized (via the reaction force feedbacks)
which will minimize the workpiece-hole reaction forces and achieve
compliant motion of the robot and workpiece. Obviously, it is necessa-
ry to realize the motion of the gripper and workpiece in the same di-
rection of but in the opoosite sense to the action of the reaction
force.

Force feedbacks can be realized in various ways [7J. Sometimes, itsuf-

fices to introduce feedbacks at the gripper joints. The control law
may be either linear or nonlinear. A simple form of force feedback
might be realized when force sensors are installed at the points of
contact of the workpiece and the robot hand.
 329

Let us suppose the sensors ~t the workpiece-gripper contact points mea-

sure the reaction forces acting between the workpiece and gripper, R£,
->-
on the basis of which the reaction forces of the peg-hole RKi can be
determined. The torques at robot joints which should be applied in or-
der to minimize reaction forces, and thus, realize the insertion of the
peg into the hole may be represented in the following way:

6P~1 (7.3.1)

where rk1i and r K2i are the position vectors from the axis centre of
the i-th joint to the contact point K1 and K2 respectively (see Figs.
7.2 - 7.5) and ~, is the unit vector of the i-th joint axis. Let us
1
->- ->-
suppose the inertia forces of the object Fo' gravitati~n force Go and
the moment due to inertia forces of the object itself Mo' can be neg-
lected (which is, as we have already said, a realistic assumption in
regard to small veloci tes and accelerations of the workpiece in the sta-
ge of parts mating). On the basis of relations (7.2.8) and (7.2.9) for
the case of the occurrence of two contact points (i.e. on the basis of
(7.2.3) and (7.2.4) for the case of one contact point between the ob-
ject and hole), expression (7.3.1) can be written (taking into account
+ + + + + +
that r K1i = rK10+roi' r K2i rK20+roi):

L L
6P~1
+
(r ,x I +
R + I +
r, xR,)e,
+ +
(7.3.2)
01 £ =1 £ £ =1 ",0 N 1

where r ,
01
is the position vector from the centre of axis of the i-th
joint to the mass centre of the workpiece. If we suppose the workpiece
parameters are known, expression (7.3.2) can be calculated on the basis
of the force sensors information, i.e. from the measured forces RJ>.. On
the basis of the values obtained for the compensating torques 6P~, it
1
is possible to determine the compensating signals to the actuators in-
puts which should realize these torques. If we assume the actuators
models are of second order (i.e. if the delay in the rotor circuit can
be neglected), the compensating signals can be calculated in the form:

(7.3.3)

where K~>O is the gain in the force feedback loop, and fi and hi are
1
defined by (5.2.12). On the basis of (7.3.2) and (7.3.3) a feedback
loop can be established from the force sensors (which measure reaction
->-
forces between the object and gripper R£) to the actuators inputs. The
block-scheme of such control is shown in Fig. 7.9. It is not necessary
330

to introduce force feedbacks into all joints, i.e. the compensating

moments ~p~ need not to be realized by all the robot joints. The cho-
~

ice of the control structure depends on the mechanical robot structure

because the compensating moments at the gripper tip should be realized
via the torques at jOints. This means that the position vector of the
gripper (i.e. of the workpiece) with respect to particular jOints r oi '
as well as the directions of the particular joints (i.e. the orts ei )
determine what are the joints to which force feedbacks should be intro-
duced and what are the joints at which these feedbacks might be neg-
lected.

A special problem in the implementation of the laws (7.3.2) and (7.3.3)

represents the choice of gains K~. It should be borne in mind that the
~

control laws (7.3.2) and (7.3.3) include the robot parameters which
need not to be known precisely. On the other hand, the force sensors,
however precise they may be, introduce always certain noise in the for-
ces measured. Therefore, should the compensating torque be realized
exactly according to (7.3.2), it could result in an oscillatory beha-
viour of the robot,. The role of feedback gains K~ is to prevent such oscil-
~

latory behaviour. However, it is difficult to determine these gains at

the stage of robot design (since the modelling of these high frequency
modes of the system is very complex) but they are best determined ex-
perimentally. Obviously, the appearance of oscillations when force fe-
edbacks are applied in the assembly process may also be prevented in
some other ways (e.g. by introducing a damping feedback loops, and the
like) •

We have considered here a simple scheme of robot control for the as-
sembly process which involves force feedbacks. It should be borne in
mind that this control scheme cannot solve the assembly problem in a
general way. Moreover, even in the simplest case, this scheme cannot
guarantee that peg jamming will be avoided. It is necessary to develop
a special strategy (Le. to plan the gripper motion) so to prevent jam-
ming.

As can be seen,a force feedback can be applied in combination with the

posi tion,veloci ty, and (possibly) current feedbacks. Namely, in the sche-
me in Fig. 7.9. the feedbacks employed in the position robot control
have been retained, to which force feedbacks have been superimposed. The in-
teraction of these two parts of control scheme has not been analyzed.
However, a case may arise when the robot positioning feedbacks and
force feedbacks are in a "collision". This problem will be considered
in the following section.
 331

r--- -
'"
12--
Qllo-
a:'"
Qllo-
a: U O
lo-VI
U O
lo-VI
0<= 0<=
LLQl LLQl
VI VI

-
r--

[t
I
•I
I I L.-

I I
tI
I
I
·~X I
<=
x x

-- ~~ -- """,L-

?
~
~
<= ~

~
~
~

lo- lo-
lo- lo-
~~ ...,0
~Ql

....0 O~
"'~
go
...,'"
~ --'~
.... ...,'"
~ --'lo-
....
U <= U <=
0:( 0 0:( 0
U U

- .... L- ....
LL .~
- .... L... .....
U ~ U
<= 0 <J 0
~ <= ~ ~~

tI
~ ~ ~

~
fl'C:i)' +
LL~C~
:
<=
x
+
1 ~
LL<=
~
<l

....

~
0
<=
.~

~
U
'"
en

.... ....
0

'" '"'" .... '"

VI

Ql
a.
VI

'"
Ql
'" '"
'" "C
en

lo-
0
"C

N
M
<=
en
....'"
.~

VI

Ql
a.
VI
';;;
<=
en
'"
.~

"C
0
l0-
~

M
M
-
E E en
~ U
....: E U
......
U '" 0
U
0
E
U

'"
~
0
U VI
U

'"
~

Fig. 7.9. Block-scheme of control with feedbacks by forces measured

by sensors at contact points between the object and
gripper
332

Several examples of successful robotic assembly using force feedbacks

have been realized under laboratory conditions and some of them are
described in Ref. [8]. However, the force feedback has not found yet a
wider use with the commercially available robots. The reason is in un-
resolved problems related to the force feedback application [7].

Much better performances in the robotic assembly have been achieved

using the specially constructed grippers which enable a passive (with
no actuators involved) addaptation of the gripper during the assembly
task. Different types of the so-called passive compZiance have been de-
veloped, the best known being the "Remote Centre Compliance" (RCC) de-
veloped in the Charles Stark Draper Laboratory [9]. In this solution,
a special system of springs is situated between the end-effector and a
wrist of the robot which enables easy movements of the workpiece both
in the plane perpendicular to the hole axis and in the plane of the
rotation about the corresponding axes (Fig. 7.10). In this way the ro-
bot (gripper) acqUires certain additional degrees of freedom which enab-
le a better compZiance of the peg position to the hole axis, facilli-
tating thus the peg insertion into the hole. Actually, these passive
devices exploit the fact that to achieve easy insertion of the peg into
the hole it is necessary to ensure low lateral and rotational stiffness
of the gripper. Such low stiffness gripper allow the peg to "comply
~ith the hole", i.e. compliance between the peg and the hole is obtai-
ned. RCC owes its name to the fact that this device places the compli-
ance center at the tip of the peg. A compliance center is a point at
mechanical system such that a force applied at that point causes only
a translation, while a torque applied around this point causes only
rotation around the point. Therefore, if compliance center exists or
is artificially created, as RCC does, at the right position on the peg,
it allows efficient insertion of the peg into the hole. The advantage
of this solution is in that the control system is not complicated ei-
ther by special feedbacks or additional calculation. In this way, the
speed of assembling is increased significantly. However, a disadvan-
tage of these devices is that one passive compliance cannot be applied
for different pegs and holes (differing in the dimension, shape, and
the like), but usually for each peg-hole combination a proper mechani-
cal adaptor has to be used. As the major characteristic of a robot is
its reprogrammabiZity (i.e. the ability to switch from one production
task to another, that is from one working place to another, what is
achieved in a simple and quick way by reprogramming the robot), the
 333

application of these passive compliances diminishes robot's capabili-

ties and reduces the robot to the level of a classical machine capable
of doing a narrow set of tasks. A frequent solution is that a set of
different adaptors is developed for a robot, and when the task is beinq
changed, the adaptor is changed accordingly.

connection
with robot

RCC

gri pper

workpiece

Fig. 7.10. Scheme of a passive compliance (RCC)

Further development of RCC has led to the instrumented RCC (IRCC) whose
purpose is also to realize passive adaptation but which includes mea-
surement of contact forces and moments, so that this information can
be used in the feedback [10].

Example 7.3. We shall briefly describe an experimental study of the

force feedback in the robotic insertion of the workpiece into the hole
[4 ].

For the purpose to study experimentally force feedback loops and their ap-
plication in robotics a force sensor which could measure all the three
components of the force acting between the robot gripper and workpiece
has been used. Besides, a special gripper (Fig. 7.11) has been develo-
ped and attached to the tip of the minimal configuration of the mani-
pulation robot UKS-2.

Both jaws sides of the experimental gripper were supplied with three-
-component force sensors. The gripper was designed in such a way that
t):J,e jil,ws sides bearing force sensors move parallel with each other.

Eac):J, force sensor contained three one-component sensors to measure the

forces acting between the payload and gripper. The sensors used were
334

from the WAZAU company (Be rlin), and their measuring range was 20 N.
The force sensors are semiconductors of the strain gauge type of ave-
rage sensitiv ity of 1.505 mV/N and have exhibited v ery good reproduci-
bility. The gripper was covered with rubber to ensure uniform pressure
and friction needed for the efficient gripping of the payload. The
screws with nuts and springs enabled mechanical adjustment of "zero"
in each of the three measuring directions.

Fig. 7.11. Experimental gripper with force sensors to measure

reaction forc e s between the gripper and workpiece

The experimental gripper with the force sensors was attached to the
tip of the minimal configuration of the industrial manipulator UMS-2
shown in Fig. 7.6. The robot possesses n=4 d.o.f. (three for the mini-
mal configuration and one for the gripper rotation). The robot control
was r e alized using a PDP-11-03 microcomputer and either the analogue
or direct digital servo systems.

The force sensors mount e d on the gripper enabled direct measurement of

+ T
the forces R£ = (R x £ ' Ry £ ' Rz £ ) . The values of forces obtained from
the force sensors are fed via AID converter to the microcomputer. The
 335

additional compensating torques which are to be realized by the actua-

tors can be calculated in a direct way as:

2
lIP~
1
= I -; . xR 1,
1,=1 1,1

In the particular case of the UMS-2 robot, the compensating Moments

can be calculated in the following way:

(7.3.4)

where d denotes the half-width of the gripper jaws (i.e. the distance
from the left to the right force sensor; this distance is measured by
means of a corresponding linear potentiometer and, via AID converter
led to the microcomputer) .

The aim of the experiment was to investigate the force feedbacks in

the process of "fine motion". The manipulator task was to insert a
prismatic object into a hole. The clearance gap was relatively large
- up to 0.5 mm, so that the insertion could be carried out in a rela-
tively simple way and with no contact occurring between the object and
hole. However, to examine the effect of the force feedback, we have po-
sitioned the manipulator in such a way that the object and hole come
into contact and the resulting reaction forces were measured by the
force sensors on the gripper. It was possible to arrange that the re-
action force had only one (x) component in the absolute coordinate fra-
me. This component could be compensated by the movement of the first
(revolute) manipulator joint. This means that we have established the
force feedback to the first jOint and the global force gain for the
same joint has been varied.

The time-history of the forces appearing at the sensor (i.e. the modu-
le of the force vector) during the object insertion are presented in
Fig. 7.12. If the global force feedback is opened (by putting the gain
K~=O) these forces will reach their maximum. If the control also in-
cludes the force feedback (and through it additional driving torques
calculated by the microcomputer are applied) these forces become smal-
ler (see the result for K~;:1 in Fig. 7.12). However, if the global gain
336

is too high, the robot may start to oscillate and reaction forces may
even increase (see the case when K;=2). Fig. 7.13. shows the maximum
amplitude of the force Fx measured by the force sensor on the gripper
(actually, the sensor measures the reaction force RK acting between
the payload and hole, because the payload dynamics forces can be neg-
lected*)) as a function of the global gain applied in the force feed-
back K;. It is observed that the maximum reaction force decreases

3 \, \~ I" 6 t[s]

-I
,-It"
'.
I: ,. I
,
,

- K~=O. ~,..• .: II
~.i' I

-3
•••• _ ••• K~=I. "."
\J
---- K~=2.

Fig. 7.12. Experimental reaction forces measured in the "peg-in-hole"

task for different force feedback gains

0.5 2

Fig. 7.13. Dependence of maximum reaction force on the force

feedback gain in the "peg-in-hole" task

*) We have chosen a relatively light object (m =O.3 kg) in order to

p
diminish the effect of its dynamics on the insertion process.
 337

for K~ 2 1 and increases again for K~ > 1. This means that force gain
should be carefully selected in order to avoid oscillatory motion of
the robot when the force feedback gain is too high.

E X ere 1 s e s
7.3. write the expressions for the compensating torques in the robot
joints (7.3.2) for the case the peg and hole are in contact (Figs.
+
7.2 - 7.5), if the Rg, forces are measured at the points of contact
of the workpiece and gripper (as in Fig. 7.11) and if the assump-
+ +
tion on neglecting the inertia forces F o ' gravitational force Go
and the moment due to the inertial forces of the workpiece itself
Mo , is not valid (i. e. it is necessary to introduce these for-
ces and the moment into (7.3.2)). Determine these forces and mo-
ments via coordinates and velocities of the robot joints for the
robot UMS-2 in Fig. 7.6. Was such correction necessary?

7.4. In the experiment described in Example 7.3, the control of the ro-
bot shown in Fig. 7.6. is realized by means of a microprocessor
which on-line calculates the nominal centralized control accord-
ing to (4.4.20), local servo system (both the position and velo-
city) feedbacks according to (4.4.16) and additional compensating
moments (on the basis of the forces measured at the gripper-object
contact points) according to expressions (7.3.4), (the correspon-
ding signals for the actuators are calculated using (7.3.3)). Wri-
te down a dynamic model for the robot UMS-2 in Fig. 7.6 and the
models of the actuators (D.C. servomotors - second order models) .
Determine what is the minimal number of numerical operations (ad-
ditions and multiplications) needed to calculate the control sig-
nals (i. e. the inputs to actuators u i ), on the basis of the given nomi-
nal trajectories qOi, qOi on the actual coordinates qi and joint ve-
locities qi (i = 1, 2, 3, 4) and on the basis of forces measured
T T
by force sensors (Rx1 ' Ry1 ' Rz1 ) and (Rx2 ' Ry2 ' Rz2 ) ,as well
as of the gripper jaws d at each sampling period. Assume the no-
minal joint acceleration qOi is obtained by numerical differenti-
ation of the nominal velocity qOi ,., (qo(t+lIt)-qo(tll/llt, where lit
is the sampling period. Take that the calculation of a sine or
cosine function requires one multiply and two add operations.

7.5. On the basis of the results of the preceeding exercise, determine

what is the shortest sampling period that can be attained (i.e.
338

what is the shortest time needed to calculate input signals for

the actuators), if the control is realized using:

a) PDP-11-03 microprocessor (one add operation lasts 100 [~s],

and one multiply operation 250 [~s]),

b) a microprocessor INTEL-SO-SO (one add O.S [ms] one multiply

1.5 [ms]) ,

c) a microprocessor INTEL-SO-S7 (one add 35 [~s], one multiply

65 [~s]).

Assume the processor time is used only for the operations of ad-
dition and multiplication.

7.6. Draw a detailed scheme for the control of the UMS-2 robot accor-
ding to Example 7.3 and Exercise 7.4, so that all the necessary
AID and DIA converters, sensors and actuators are marked.

7.7.* Write in a high-level programming language a programme to calcu-

late the control (inputs to the actuators) for the robot UMS-2
according to Example 7.3. and Exercise 7.4. during one sampling
period. Assume the programme input variables are: the nominal jo-
ints coordinates qOi(t) and velocites qOi(t), actual jOints coor-
dinates qi (t) and velocities qi (t) , sensory forces (R 1 (t) , R 1 (t) ,
T T x Y
Rz1 (t)) and (R x2 (t), Ry2 (t), Rz2 (t)) and the information on the
gripper jaws d(t); whereas the programme output variables are:
the input signals to the actuators ui(t) at the time instant t
(i.e. at the current sampling period). Assume all the necessary
data about the mechanism, actuators and feedback gains are given
(and represent either input data or constants in the programme) .

7.4 Hybrid Position/Force Control of Robots

To the moment, we have considered in this chapter special task in ro-

botics, namely, the robotic assembly of machine parts and elements. As
we have seen, this task can be divided into a series of subtasks (sta-
ges), the most sophisticated of them being the stage of parts mating
itself. The task of the robotic assembly belongs to a wider class of
tasks in which the robot comes into contact with the objects in the
 339

workspace, so that these objects "act" upon the robot by reaction for-
ces and thus constrain the robot motion in certain directions. In other
words, in this class of tasks, the robot motion in the particular di-
rections is constrained by its contact with one or more surfaces. For
example, when the robot comes into contact with a surface in the work-
space, the robot's motion in the direction perpendicular to that sur-
face is constrained, but the robot can act upon that surface by a cer-
tain force (Fig. 7.14). It is obvious that the task of part assembl-
ing (i. e. of inserting a peg into a hole, Fig. 7.1) belongs to the class
of tasks with partialy constrained motion. As we have already mentioned,
to this class of tasks also belong the tasks of robotic cutting, for-
ging, grinding, deburring and many others.

In this section we shall consider some basic problems concerning the

control in this class of tasks. It is quite obvious that, in these
tasks, it should be ensured the robot position in certain directions
(in which its motion is not constrained) is controlled (its position-
ing is realized in accord with some definite requirements, i.e. the
robot should track a desired trajectory), while in some other directi-
ons (in which the robot motion is constrained) the force or moment ap-
plied by the robot is to be controlled. Therefore, in these tasks in-
volving partial constraint to the robot motion, some of the robot's
d.o.f. are controlled by position, and other by force. Because of that
this type of control is named hybrid position/force .control [11, 121.
The robot control in the stage of parts mating, considered in Section
7.3, belongs to this type of control.

However, we have not considered yet the general relationship between

position control and force control, but we have only described the for-
ce feedback control in a special case of inserting the workpiece into
a hole. In this sectioh we shall broaden the notion of position con-
trol and force control to encompass the whole class of tasks involving
partially constrained motion of the robot.

When the robot comes into contact with an object in the workspace, then,
in dependence of the specific mechanical and geometrical characteris-
tics of the given contact, various constraints are imposed to the ro-
bot gripper motion and to forces the hand can realized. A set of such
constraints which are a na,tural consequence of the par.ticular relation-
ship between the robot and environment (i.e. of the nature of the con-
tact between them) is called natural constraints. For example, with
the robot shown in Fig. 7.14. the robot gripper motion in the direction
340

perpendicular to the contact surface is naturally constrained, i.e.

there is a natural constraint with respect to position. If the fricti-
on is neglected, the gripper cannot realize an arbitrary force in the
direction tangential to the surface so that there is a natural con-
straint with respect to force (the force is zero).

To facilitate determination of these constraints to the hand position

or to the force by which the hand acts on its surroundings, for each
particular task, an appropriate coordinate frame of (or constpaint
fpame) is adopted and placed at the position which suits best the par-
ticular task. Two characteristic manipulation tasks together with the
corresponding constraints are illustrated in Fig. 7.15 [12, 13]. As
can be seen, the coordinate frame with respect to which restrictions
are defined may be either fixed or movable.

The constraints in Fig. 7.1S.a) have been defined under the assumption
that the friction between the gripper and crank handle ensures reliab-
le grasping and that the handle can rotate with respect to the crank
arm. The robot gripper position constraints are defined by the gripper
velocity components with respect to the chosen coordinate frame, which
is often more convenient than to define position constraints in a di-
rect way. Similarly, the force constraints are specified via prescri-
bing the values to components of the vector force and moment at the
gripper with respect to the same coordinate frame. It is obvious that
the gripper position constraint comprises the constraints with respect
to position and/or orientation of the gripper, while force constraints
include the constraints with respect to forces and/or moments acting
upon the gripper.

The constraints in Fig. 7.15.b) have been derived under the assumption
that the force in the direction y is limited to zero, since the slot
in the screw head allows the screw driver to slip in this direction.
All these natural constraints are determined by the nature of the con-
tact itself, not by the desired manipulator motion.

When the desired robot motion and/or the forces to be realized by the
robot gripper are prescribed, the so-called aptificial constpaints are
thus defined. To define a task to be realized by the robot, is to de-
fine the artificial constraints to the gripper motion and to the for-
ces to be realized by the gripper. In Fig. 7.15. apart from the natu-
ral constraints, we have also presented the artificial constraints
 341

Fig. 7.14. Robot contact with a surface: constraints to robot motion

in the direction perpendicular to the surface

Natural constraints Artificial constraints

x O. Fz O. z
c
O. F O.
c x

Yc O. F O.
Y
0
Wx O. M O. wY W M
x
O.
Y Y
Wz O. M
z
O.

(al Turning crack

Artificial constraints Natural constraints

Yc O. Fx O. x
c
O. F O.
Y
0
z pw F O.
X y c z z
0
w
Z
W M
x
O. W
x
O. M
z
O.
Z

M O. W O.
Y Y
(bl Turning screwdriver

Fig. 7.15. Natural and artificial constraints on robot for

two tasks [12]
342

which are determined by the production task given by the operator or

by a higher control level (i.e. the desired robot motion and forces it
has to realize). For example, in the task illustrated in Fig. 7.15.b)
it is required the gripper moves about axis z with an angular velocity
wO,but it does not move in the direction y, while in the direction z
z
it has to move with speed pw o and not to exert any force on the screw
z
head in the direction x and neither produces moments about axes x and
y (here, p denotes the step of the screw spiral groove); all these re-
quirements directly define the artificial constraints. It is obvious
that when there is a natural constraint on the gripper motion in acer-
tain direction (i.e. with respect to a certain d.o.f.) in the coordi-
nate frame of constraints, then it is necessary to define the artifi-
cial constraint with respect to force in that direction, and vice ver-
sa. In every task, each gripper d.o.f. in the constraint frame should
be controlled in such a way to satisfy either a position constraint,
or a force constraint.

Therefore, the hybrid robot control should ensure the position control
in the direction (in the constraint coordinate frame) in which there
is a natural force constraint, and to ensure control of the gripper
force in the direction in which there is a natural constraint to the
gripper position. Such control should be realized that it might be ap-
plied for different combinations of these constraints with respect to
an arbitrary (suitable) constraint coordinate frame [13].

We have already pointed out that the constraint coordinate frame can
be defined in different ways, which depends on the nature of the par-
ticular task. This frame may be fixed to the tip of the robot gripper
or to either a mobile or fixed part of the objects in the workspace
(e.g. on a mobile tool, at the hole into which the peg is being inser-
ted, etc.). Actually, the selection of this frame depends on specific
production task. Therefore this coordinate frame is usually reffered
as task-specific frame. However, the constraints defined with respect
to this coordinate frame may easily be transformed into the co.nstraints
on the gripper position and constraints on the force acting on the grip-
per with respect to the coordinate frame used to define the external
robot coordinates s, and vector forces at the gripper F (Cartesian
frame - see Section 5.5). For simplicity sake we shall assume that the
position constraints have been defined with respect to the coordinate
frame for which the external coordinates s and external forces F were
defined, so that the natural and artificial constraints are defined
 343

with respect to the coordinates of the vector s and vector F. Actually,

it should be borne in mind that it is necessary, using the correspon-
ding transformation matrices, to transform the position constraints
and force constraints from the task-specific coordinate frame into the
coordinate frame of the external coordinates s and forces F.

Let us suppose that in the i-th direction, in which a natural constraint

occurs on the i-th component of vector F, is defined a desired trajec-
tory (position) of the corresponding i-th component of vector s which
defines the artificial constraint on the gripper position in the i-th
direction s?(t). Of course, it is assumed that the component of the
l
external (Cartesian) coordinates vector si is defined with respect to
the same direction as the i-th component of F., i.e. s = (x , y , z ,
T T l C C C
S, <p)
ljJ, and F = (F x ' F y ' F z ' Mz ' My' Mx) , where xc' Yc' and zc are
the coordinates of a fixed point at the gripper with respect to the
corresponding coordinate frame and ljJ, S, <p are the Euler angles of the
gripper (see Section 2.2), whereas Fx' Fy and F z are the projections
of the external resultant forces acting on the gripper, onto the axes
of the same coordinate frame, and Mz ' My" Mx ' are the components of
the external moments at the gripper with respect to the axes the Euler
angles are being defined. Similarly, we can suppose that in the j-th
direction, in which there is a natural constraint on the j-th compo-
nent of vector s, is given a desired "trajectory" (value) of the cor-
responding component of vector F which defines the artificial constraint
o
on the hand force in the j-th direction, Fj(t).

The task of hybrid control is to realize simultaneously the prescribed

nominal trajectories (positions) of the gripper coordinates s?(t) and
l
prescribed nominal trajectories of the force components of the gripper
F~(t). Hybrid control is based upon the position and force feedbacks,
J
i.e. upon the assumption that the control system possesses the infor-
mation about the actual position of the robot (hand) and the informa-
tion on the actual forces acting on the hand. Thus, the control is re-
alized on the basis of the error in pos.i tion and error in the realized
force.

For simplicity sake, matrix S has been introduced with the purpose of
choosing the gripper's d.o.f. that are position controlled or force
controlled [12, 13]. SeZection matrix S is of diagonal form, the i-th
element on the diagonal has the value if the i-th gripper d.o.f. is
force controlled (i.e. if the artificial constriant is prescribed with
respect to the i-th component of the vector F), and the value 0, if
344

the i-th gripper d.o.f. is position controlled (i.e. if s~(t) has been
prescribed). It is obvious that the dimensions of matrix S are mxm
(where m is the order of the external coordinates vector s and of the
force vector F; m<6 - see Section 2.2). For different tasks and dif-
ferent conditions of the robot-environment contacts the elements of
matrix S change during the task execution (because the natural constra-
ints change) .

On the basis of the information about actual joint positions q(t) ob-
tained from the position sensors at joints, actual values of the ex-
ternal (Cartesian) coordinates s(t) are calculated using expression
(2.2.1). Thus, the control system calculates the error with respect to
the hand coordinates (i.e. the difference between the actual coordina-
tes and the prescribed nominal trajectories sO(t)):

LIS(t) = s(t)-so(t) = f(q(t))-so(t) (7.4.1)

However, as we have said above, only certain components of the vector

s are "constrained" by the nominal trajectories (positions) sO(t), and
these are those components which have a natural constraint on the for-
ce component. Because of that, only the "reduced" vector of error in
the gripper position ~se(t) is relevant to the control, and it is:

(7.4.2)

By measuring forces R£ (by means of the force sensors mounted on the

robot or on the objects in the workspace - see Section 7.3) the infor-
mation is obtained about the forces by which the environment acts upon
-7
the robot. From the measured R£, using the corresponding transformati-
on matrices, the actual instantaneous value of the vector F(t) is cal-
culated as

F(t) (7.4.3)

where L is the number of sensors (force transducers), TF £ is the (mx3)

transformation matrix of the information from the £-th sensor to the
vector F. In this way, the error in the vector F components in the
control system can be obtained as the difference between the actual
forces acting on the gripper F(t) and the desired nominal trajectories
of the force (and moment) components FO(t):
 345

lIF(t) (7.4.4)

As with the position s, for force control too (for the specific task
and contact situation) only certain components of F are relevant, i.e.
it is relevant the error vector with respect to the gripper force liFe'
defined as

(7.4.5)

Thus, for the specific contact situation, i.e. for a defined S, the
error in the control system with respect to the gripper posi tion (7.4.2)
and the error with respect to the force on the gripper (7.4.5) are ob-
tained. These errors are "divided" into the hand d.o.f. controlled by
position (orientation) and the hand d.o.f. controlled by force (moment) .
On the basis of these errors the controller should generate input sig-
nals to the actuators at the robot joints. In this way, different con-
trol laws, connecting the signals to the actuators inputs and the er-
rors with respect to the position lise and the errors with respect to
force liFe' can be applied.

Fig. 7.16. represents a general scheme of hybrid control [12]. The con-
trol consists of two complementary sets of feedbacks loops (the upper
with respect to position, the lower ones with respect to force) each
of them including its own control law and the both controlling one and
the same object - the robot. The control laws have to include the trans-
formation of the external (hand) coordinates into the joint torques
(i.e. input signals to the actuators). It is obvious that both sets of
the feedbacks control each robot jOint in a "cooperative" manner, though
each of the hand d.o.f. is controlled only by one of the feedback sets.
The control can be realized in external coordinates, and this is done
according to the solutions presented in Section 5.5. (Fig. 5.11). If
the decentralized control in external (Cartesian) coordinates is adop-
ted, the joints torques can be calculated (in the static case) from
the relations (using (5.5.2) and (5.5.3)):

LIP

[S(t)-SO(t)]-K (I -S)s} (7.4.6)

ve m

where KF denotes a matrix of dimensions mxm which can be adopted in

the form KF = diag(K~), Kpe and Kve are the matrices of dimensions mxm,
346

K diag(K i ), K diag(K i ). Here Ki denote~ the force feedback

pe pe ve. ve F
gain, whereas Ki and Kl are the position and velocity feedback gains
pe ve
in Cartesian coordinates (see Section 5.5). The hybrid control thus
obtained is decentralized with respect to external coordinates. This
control law is completely realized at the level of hand coordinates;
the error calculated in terms of hand coordinates and hand forces is
multiplied first with the feedback gains, and then, the transformation
is carried out into the joints torques to be realized by the corres-
ponding actuators. (The input signals to the actuators that realize
the calculated torques LIP (7.4.6), can be determined from (5.4.11».

Another possible solution is to calculate the position and force er-

rors in the hand coordinates and to transform them into the errors of
internal (joints) coordinates and of jOints torques. First, it is cal-
culated

-1
J (q) lise (t) (7.4.7)

(7.4.8)

and then, the control is realized in the joint coordinates (i.e. joints
torques are calculated):

(7.4.9)

Here, lIP p (t) is the vector of correctional torques due to the position
control which can be realized via the PIO control law:

(7.4.10)

where Kpp' Kpv' KpI are the respective nxn matrices of the position,
velocity and integral feedback gains (in joint po~ition control).

In (7.4.9) lIP F (t) denotes the vector of correctional moments due to

the force control. This control can be realized as the PI control law:

(7.4.111

where KFP and KFI are the (nxn) matrices of the proportional and inte-
gral gains in the force feedbacks loops. If the matrices Kpp' Kpv'
KpI ' KFP and KFI are adopted in diagonal form, the control is dec en-
 347

tralized with respect to both position and force (decentralization at

the jOints level). However, it is obvious that the control about each
jOint "participates" in both the position control and force control.
A block-scheme of such a control is shown in Fig. 7.17 [12].

It is evident that such control represents a combination of the con-

trol in Cartesian coordinates and the control in joint coordinates:
the difference between the given position sO(t) and the actual posi-
tion s(t), as well as the difference between the given force FO(t) and
the actual force at the gripper F(t) are calculated at the level of
Cartesian coordinates and the feedback gains are introduced at the
joints level. Hybrid control can also be realized in the joints coor-
dinates: the given position sO(t) and force FO(t) are transformed from
the Cartesian coordinates (into gO(t) and pO(t)) and then, the control
is applied which realizes the desired gO(t) and P~(t). To realize the
given joints positions (trajectories), different control laws can be
applied, which have been considered in the previous chapters. The given
o
forces (moments) PF(t) are realized by means of force feedbacks, by
+
transforming the R£ forces measured by the force sensors into the for-
ces at (moments about) the robot joints (see Section 7.3).

However, irrespective of the concrete law adopted for the realization

of the position control and force control, the essence of hybrid con-
trol is in the cooperative realization (by all the joints actuators)
of both the given positions (I -S)so(t) and the given force at the
m
gripper SFo(t), and a consistent position/force control is ensured by
choosing the appropriate S values, i.e. the artificial constraints on
the positions and forces are assigned (in a complementary way) .

In should be pointed out that the hybrid control is still at the stage
of laboratory investigations and only some partial solutions of such
control have been used with commercial robots, for some special purpo-
ses (e.g. for assembly). As we have already said in the preceeding
chapter, these solutions involve either force feedbacks or some pas-
sive compliances.

One of the basic problems in hybrid control is the automatic determi-

nation of the natural and artificial constraints on the positions and
forces in different tasks. As we have already mentioned, the natural
and artificial constraints are dependent of the contact situation and
specific task conditions, and these may change in the course of the
348

s{t) DIRECT
,..------~---I KI NEMATI CS
f{q{t))

POSITION CONTROL
AND COORDINATE
TRANSFOR~1A TI ON

S------~

FORCE CONTROL
AND FORCE
TRANSFORMATION

TRANSFOR~,1ATI ON
(7.4.3)

Fig. 7.16. General scheme of hybrid control [12]

J
q

Fig. 7.17. Block-scheme of hybrid control [12]

 349

task execution (depending on whet~er, and in what a way, the robot

comes into contact with the objects in its environment). When a task
has been assigned either by the operator or by the higher control le-
vel, these constraints should be defined for all the situations which
may appear during the task execution. Defining constraints may be a
very complex job, so that the task assignement may be really painsta-
king and may require complex programming (in a high robotic language).
Because of that, it has been endeavoured to develop the automatic plan-
ners [14] which would automatically determine the set of constraints
for all the possible situations that may arise in a particular task.
Thus the user (the operator of higher control level) would assign in a
global way, a desired production task result (e.g. do assembly of two
elements, and the like) and the automatic planner would generate a set
of artificial constraints which would ensure the task realization. The
strategic control level should define the set of matrices S for every
possible situation, as well as the set of artificial constraints with
respect to positions and forces (in dependence of the given situation
in the task execution). Moreover, the automatic planner should also
predict (with the aid of sensors and the like) the way in which the
robot controller can recognize the situational changes (the robot-ob-
ject relationship in the workspace) and thus determine what are the
artificial constraints (strategy) which should be assigned at a given
moment. Methods of automatic planning are still at the experimental
stage, because the general solutions are extremely complicated, which
is understandable if we have in mind all the variety and great comple-
xity of possible situations [14].

On the other hand, the hybrid control requires that the Same actuators
realize both the position control and force control. The problem of
choosing a control law and feedback gains has not been generally sol-
ved. The choice of the force feedback gains is hindered by the lack of
a reliable model of the robot-environment contact, as well as by the
insufficiently known relationship between the position control and
force control. It appears difficult to realize the robust hybrid con-
trol which would (using a unique control law and unique feedback gain~

satisfy different contact situations, i.e. for the different natural

and artificial constraints [15]. One of the fundamental problems is
how to ensure a stable robot work with the force feedbacks for diffe-
rent strategies.

In the control laws considered above, (7.4.6) and (7.4.7) - (7.4.11),

only some of the necessary terms have been introduced. To improve
350

the position control, some additional elements can be introduced into

these control laws. For example, it is possible to introduce into the
position control either a feedforward (local or centralized nominal
control), global control, or to apply computed torque method (Section
5.4). A feedforward element [121, and the like, can be also introduced
into the force control. Such hybrid control which takes into account
the robot dynamics is called dynamic hybrid control [15, 161.

However, instead to consider dynamic control of robot applying the mo-

del of the robot's dynamics expressed in joint coordinates, as we did
in Sections 5.2. and 5.4, we may apply the model expressed in external
(hand) coordinates. Since in hybrid control we intend to control di-
rectly the position of the hand and forces applied by the hand upon the
environmental objects, it is natural to express the complete robot dy-
namics in the hand coordinates [161. Actually, as the natural and arti-
ficial constraints upon the gripper positions and forces are defined
with respect to suitable constraint frame which depends on the speci-
fic task (task frame), it is convinient to express the robot dynamics
in such task-space frame. It can be shown [16] that the dynamic model
of the robot, when expressed in such task-space coordinates, is obtai-
ned in the form similar to that one expressed in the jOint coordinates
(3.2.2). Such dynamic model expressed in task-space coordinates might
be used for dynamic hybrid control, i.e. for the control scheme which
controls both position of the gripper and the forces applied by the
gripper and accounts for the entire dynamics of the robotic system. By
such dynamic control, all forces in the system (inertial, centrifugal,
Coriolis, gravity) are compensated for. However, the computation of
the "inertia matrix" and the vectors corresponding to Coriolis, cen-
trifugal and gravity moments in the dynamic model expressed in task-
-space coordinates (or hand coordinates) is even more complex than com-
putation of the matrix H and the vector h of the model (3.2.2). It has
been shown [17] that the current microprocessor might achieve adequate
sampling rate even if the computation of the entire dynamic model in
the hand coordinates is included in the control law. However, the main
problems with this approach are its sensitivity to variation of the
contact situation and to parameters variation, and the problem of se-
lection of appropriate feedback gains for position part of the control
scheme and for the force part of the control scheme [18, 191. The dy-
namic hybrid control (similarly to the "computed torque method" consi-
dered in Section 5.4) appears to be sensitive to unmodelled high fre-
quency modes of the system (flexibilities and nonlinearities in the
 351

manipulator links anq joints, in actuators and specially in actuators

transmission systems, contact situation between the robot hand and en-
vironment, flexibility of force sensors, etc. ~17J). This is not supri-
sing, since. dynamic hybrid control might be regarded as an extension
of "computed torque method" for position/force control. This sensiti-
vity is even more present in dynamics expressed in hand coordinates
when force control has to be included, since force sensors and contact
with the environment contribute to the model uncertainities.

The interaction between the position control part and force control
part has to be clarified. It is the question how a robotic system pri-
marily designed for position control might be used for both position
and force control. Therefore, a lot of both theoretical and practical
problems have to be solved before the hybrid position/force control
will be applied within industrial robotics [20].

It should be noted that, in the above considerations, we have tacitly

adopted several approximations. For example, we have neglected the fric-
tion forces which complicate the assignement of the artificial constra-
ints with respect to positions and forces [14]. Besides, we have neg-
lected the effect of robot dynamics in the relations (7.4.3) and
(7.4.8) .

E X ere 1 s e s

7.8. Determine natural constraints on the position and force for the
gripper which is not in contact with any surface in the workspace
(open kinematic chain).

7.9. Determine natural constraints on the positions and forces of the

gripper for the contact situations shown in Figs. 7.2 - 7.5. Cho-
ose the appropriate coordinate frames to define the constraints,
while neglecting the friction between the peg and hole. Determine
the artificial constraints which would allow the peg insertion
into the hole. How can the control system "recognize" the contact
situations presented? How does the friction influence the reali-
zation of this task?

7.10. If the force sensors are mounted on the gripper at the points of
contact of the gripper and workpiece as in Fig. 7.7, determine
the transformation matrices TFe from (7.4.3), if the vector of
352

force (and moment) F(t) at the gripper has been defined with res-
pect to the coordinate frame whose origin is at the workpiece ~ass

centre and the axes are along the main inertia axes of the work-
piece. If the workpiece dynamics is taken into account, how will
relation (7.4.3) change?

7.11. Repeat the preceeding task but assuming the force sensors have
been situated at the gripper joints as in Fig. 7.8, and the grip-
per possesses a spheric joint as in Fig. 2.4. Consider how the
dynamics of the gripper and of the workpiece can influence the
relationship between the forces (and moments) at the gripper F(t)
....
and the forces R t measured at the gripper joint.

7.12. Draw a scheme of the hybrid position/force control, if the con-

trol is realized in the joint coordinates: the given position
sO(t) and the given force FO(t) are transformed from the Cartesi-
an into the joint coordinates (qo(t) and P~(t)) and the nominals
qO(t) and P~(t) are realized by the local controllers about joints
with respect to the position and force (the local position con-
trollers are as in Fig. 4.1).

7.13. In the preceeding problem, define the hierarchical levels of the

robot control: strategic, tactical and executive and determine
their functions.

7.14.* Draw a scheme of the hybrid position/force control in the case the
position control is realized via the computed torque method (in
joint coordinates), whereas the force control is realized by a
decentralized force controller (in the Cartesian force coordina-
tes) .

7.15.* Starting from the model of the robot dynamics written in external
(Cartesian) coordinates (see Exercises 5.18 - 5.20) [16] draw a
control scheme of the dynamic hybrid control which includes com-
pensation of the total robot dynamics by "computed torque method"
in Cartesian coordinates. Assume that the position control is re-
alized by simple PD controllers in Cartesian coordinates (5.5.3)
and the force control is realized by PD controller of Cartesian
forces. This means that the commanded Cartesian force FD is com-
puted analogously to (7.4.6), but the inertia matrix A(S) and
centrifugal moments vIs) and gravity moments pes) - all in Carte-
 353

sian space - have to be introduced (see Exercise 5.20):

P = KpS[PM(t)-pO(t)]+A(S){(I -S)[SO(t)-K (S(t)-SO(t»-

D m pe

- K (s(t)-so(t»]} + \1(s, s)+p(s)

ve

where S is the force selection matrix and PM(t) is measured force vec-
tor. Then, the jOint torques have to be determined (according to
(5.5.2» which will realize this commanded Cartesian force PD. Try to
optimize computation of joint torques taking into account relation be-
tween the inertia matrix A(S), Centrifugal term \1(s, s) and gravity
term p(s) in Cartesian space and these terms in joint space (see Exer-
cise 5.20).

7.5 Stiffness and Impendance Control of Robots

As we explained in the previous section, hybrid position/force control,

although promising to solve efficiently the problems related to control
of contrained motion of robots, still cannot be effectively applied in
practice. On the other hand, we have mentioned that the passive com-
pliance devices (such as RCC) are very efficient for control of some
specific tasks involving control of constrained motion of robots. The-
se devices actually introduce compliance between the robot and the en-
vironment and enable compliance motion of robots. The basic idea of an
alternative approach to control of constrained motion of robot, so-cal-
led stiffness control, is to introduce active compliance between the
robot and the environment (in the directions where it is needed) rat-
her than to achieve compliance in a passive way [21].

In order to accurately control the position (or trajectory) of the ro-

bot gripper it is required to apply very stiff servo controllers around
the joints of the robot. As explained in Chapter 3, the high stiffness
of the controller ensures that the servo will reject all disturbances
which might act upon i t (from the dynamic forces of the robot itself,
of from the external forces acting upon the robot, etc.). As it has
been shown the stiffness of the servo is determined by the selected
feedback gain. Although position control requires high stiffness of
the servo, it is limited by the condition that the servo must not ex-
cite unmodelled structural modes of the system. Therefore, a finite
stiffness of the servo can be ensured, leading to finite stiffness of
the robot hand.
354

On the other hand, the force control requires that the system stiffness
be as low as possible. Ideally the zero stiffness of the servo will
allow to achieve desired force regardless of position disturbances.
This means that in order to realize force control we should decrease
the stiffness of the servo. If the stiffness of the joints servo sys-
tems is decreased the robot gripper will also behave as having low
stiffness which will enable appropriate force control. It is well-known
from some practical experiments (see Sect. 7.3) that the robot is much
more successful in inserting a peg into a hole if the stiffness of the
servos is decreased. (However, such solution leads to a poor position-
ing of the robot when it has to be used as positioning device) .

If we have to realize such control tasks which require that the robot
hand position has to be controlled in some directions and the force
applied by the hand must be controlled in other directions, then we
need the robot hand to exibit maximal stiffness in some directions
(those in which its position should be controlled) and minimal stif-
fness in other (in those in which force applied by the hand has to be
controlled). The idea of the stiffness control is to try to control
the gripper stiffness in various directions depending on the specific
task. Since the gripper stiffness depends on the joint stiffness, we
can compute the stiffness of the joint servos necessary to achieve a
desired gripper stiffness. Therefore, we have to adjust stiffness of
the servos in the robot jOints (by adjusting the feedback gains) in
order to obtain desired stiffness of the gripper.

Let us consider one possible solution to this problem [21J. Let us try
to make that the hand of the robot behaves as a general spring with
six degrees of freedom. The small displacements of the hand as (spring)
should cause the force F of the hand which can be expressed by (we shall
write the expression in hand coordinates, but actually task-space co-
ordinates should be used to express the hand stiffness characteristics) :

F -K 8s (7.5.1)
pe

where Kpe is a diagonal 6x6 matrix representing the general stiffness

matrix of the robot hand. Remember that 6s includes both linear dis-
placements and rotational displacements of the hand, while F is the
6x1 vector which includes both hand forces and hand moments. Therefore
 355
Kpe consists of three linear stiffness and three rotational stiffness.
Now, we want the hand to exibit different stiffness in various direc-
tions depending on the specific task: the stiffness has to be low in
those directions in which the force have to be controlled, and they
must be high in the directions where the hand position has to be ensu-
red. Therefore, we have to select the matrix Kpe according to the spe-
cific task being executed by the robot.

We have to determine the torques in the jOints that would produce the
force (7.5.1) at the robot hand. We recall the static relation between
the hand force F and the joint torques P (5.5.3). Therefore, we have
to produce the joints torques

(7.5.2)

in order to achieve the force F (7.5.1). On the other hand, the rela-
tion between the small displacements of the hand oS and the small dis-
placements of the joints oq is given by (according to (2.2.5)):

OS J(q)&q (7.5.3)

Combining (7.5.1)-(7.5.3) we obtain:

p (7.5.4)

Thus, we obtain the relation between the small joint displacements and
the joint torques which ensures that the robot hand behavesasa spring
with six degrees of freedom (7.5.1) and with desired stiffness in va-
rious directions. Actually, we have to apply such control law which
will ensure the relation between joint torques P and the joints displa-
cements oq given by (7.5.4), and by this the desired behaviour of the
robot hand (7.5.1) will be achieved.

In [21 J the following control law is proposed:

p (7.5.5)

where now ~q is the vector of deviation of the joints angles from the
desired position qO, i.e. ~q=q_qO, and K is a diagonal matrix Kv =
. v
diag(K~). By selecting stiffness matrix Kpe according to a specific
task and by applying control law (7.5.5), we might achieve that the
356

robot hand behaves as desired in a specific task: in some directions

it will have high stiffness, while in the other the stiffness will be
low. Thus, by Jacobian matrix J(q) we have mapped the hand stiffness
into the joint stiffness. The role of the damping factor Kv6q is to
keep the damping ratio of the system to be (over)critical (and there-
fore velocity feedback gains Ki might be also adjusted). It should be
v
noted the stiffness matrix JTK J between the joint torques and joints
pe
displacement is not diagonal, which means that the coordination of the
joints must be ensured in order to achieve desired stiffness of the
hand.

The stiffness control might be realized also via Cartesian control

(5.5.2), (5.5.3) in the following way [15] (gravity moments might be
directly compensated for):

p (7.5.6)

where K is now 6x6 diagonal matrix Kve = diag(K i ), Ki is the gain

ve ve ve
in the feedback loop by the i-th Cartesian (hand) velocity s,
and ~s
is the 6x1 vector of the deviation of the hand coordinates from the de-
sired position so, ~s = s-so. Here, we directly specify the desired
stiffness Kpe and desired damping Kve of the robot hand. The desired
stiffness is specified according to the specific task requirements, as
explained above, and the velocity feedback gains are selected to keep
the damping of the system in each direction to be (over) critical. This
means that in directions in which the stiffness is decreased, the ve-
locity gain Ki also must be decreased, and vice versa.
ve

It should be noted that in the stiffness control we have not to speci-

fy just the desired stiffness Kpe of the robot hand (and damping Kve) ,
but the control system also has to define desired position sO (or, qO).
If pure position control is considered this is the actual desired goal
position of the robot hand. However, in constrained motion control in
directions in which the forces applied by the robot gripper have to be
controlled, the desired position x O has to be specified in such a way
that the desired force F O is achieved. The determination of the sO which
corresponds to desired F O is not simple since the contact between the
robot and environment (i.e. the actual stiffness of the environment)
considerably affects the relation between SO and F O . Namely, the envi-
ronment itself exibits certain stiffness: under the action of hand for-
ces upon the environmental surface, a deformation of this surface ap-
 357

pears so that the reaction force FE of the environment (equal to ap-

plied hand force F) is given by:

where KE is an effective stiffness of the environment and sE is con-

tact point between the robot and environment. Therefore, the stiffness
of the environment plays main role regarding efficiency of th,is control.

If the assembly process is considered we have to ensure desired move-

ment of the robot hand in some directions (e.g. in direction in which
a peg is inserted into a hole) and in others we have to ensure that
the reaction forces are zero. Therefore, by specifying SO in an assem-
bly process we specify artificial compliance center in similar way that
the passive compliance (RCC) does, but in this case the compliance cen-
ter might be reprogrammed.

It also should be noted that stiffness control does not require expli-
cit application of force sensors (and therefore it belongs to a class
of so-called implicit foY'ce control). However, in practice, applicati-
on of force sensors to measure forces at the robot hand are required
in order to maintain the desired forces. If we assume that we measure
the actual forces at the robot hand F, than we can add a term in the
control law (7.5.6) which will take into account the error between the
desired force Fa and actual force F:

p (7.5.7)

where KF is the 6x6 diagonal matrix of the force feedback gains (the
force feedback is implemented in Cartesian space coordinates).

It is qUite obvious that the stiffness control of robots is quite si-

milar to simple position control which we have considered in Chapters
3. and 4. Therefore, it is qUite understandable that i t shows similar
characteristics. The stiffness control, besides being simple, is ro-
bust to parameter variations and uncertainities. Therefore, a few in-
dustrial robots already use some kind of stiffness control for some
specific tasks which include constrained motion of the robot. However,
the stiffness control is efficient only at low speeds. If the high
speeds in the constrained motion control is required, the dynamics of
the robot system has to be taken into account.
358

One possible extension of the stiffness control is by so-called imp en-

dance control; [22]. The impend.ance control besides controlling the ro-
bot hand stiffness, attempts to take into account its dynamic charac-
teristics. Actually, the impendance control attempts to make the robot
hand behaves as a system of appropriate inertia characteristics i.e.
as a system with desired mass. Thus, this approach intends to con-
trol mechanical impedance of the hand against environment and therefo-
re it gets its name: impendance control. This approach instead. to con-
trol directly forces or positions of the robot hand, attempts to con-
trol entire dynamic reZation between forces (applied by the hand upon
the environment) and the hand positions.

Numerous problems concerning impendance control still have to be sol-

ved. Stability of impendance controller has been studied, as well ex-
perimental tests have been carried on which show that this control also
might be sensitive to unmodelled high frequency dynamics in the robot
system and to contact situation, i.e. stiffness of the environments
[23, 24]. The application of force sensors both in joints and/or at the
robot gripper still has to be elaborated.

In should be mentioned that both stiffness and impendance control might

be regarded as a form of realization of hybrid pOSition/force control.
However, under hybrid position/force control it is usually assumed con-
trol scheme which includes explicit force feedback [12], while stif-
fness and impendance control represent implicit force control (although
the explicit force feedback loops as in (7.5.7) might be included).

The control of constrained motion of robots is a challenging research

area, whose successful solution will considerably affect further ap-
plication of robots in industry and increase their efficiency and pro-
ductivity.

E X ere 1 s e s

7.16. Assume a six degrees of freedom robot which has to apply a force
in z-direction (vertical) upon a frictionless horizontal plane,
and to move along the plane. Select the stiffness matrix Kpe in
the stiffness control (7.5.6) for this specific task.

7.17. Assume that the task from the Exercise 7.16. has to be performed
by four degrees of freedom robot presented at Fig. 7.6. Determine
 359

the Jacobian matrix ~o~ this robot and write explicitely both
control laws (7.5.5) and (7.5.6) (or (7.5.7). Introduce selection
matrix S. Compare this control laws to that one applied in Sec-
tion 7.3. Determine the number o~ operations (adds and multiples)
that has to be performed (at each sampling interval) by control
microprocessors to apply these control laws.

7.18. Consider the contact situations presented in Figs. 7.2 - 7.5.

Determine how the stiffness matrix of the robot hand Kpe has to
be selected in order to ensure insertion of a peg into a hole.
(Neglect the friction between a peg and a hole).

7.19~ Consider one-degree of freedom robot in a contact with environ-

ment. The system is presented in Fig. 7.18. The system consists
of a mass m and the environment and a force sensor. The environ-
ment and a force sensor are modelled as a spring with stiffness
kE [24]. Assume that the force F acting upon the mass can be di-
rectly controlled. Hrite the dynamic model of this system (fric-
tion is neglected). Assume that the force sensor gives direct in-
formation on force F = kEx.

sensor + environment

Fig. 7.18. Simple model of the one-link robot in a

contact with the environment [24]

a) Write the stiffness control law for this system (according to

(7.5.7)) .

b) Write impendance control law for this system [22], assuming

that the system has to behave as having apparent mass md , i.e.
we want that the system behaves as:

where md is desired apparent mass, Kp is desired stiffness

and Kv is velocity feedback gain.
360

c) Compare control laws determined under a) and b).

d) The mass is m = 10 [kg], and assumed environment stiffness kE

is 10 3 [N/m]. Determine the velocity feedback gain if we want
to make system (over)critically damped and the desired stif-
fness is Kp = 100 [N/m] in both control laws (assume that the
desired mass in the impendance control is md = 50 [kg]). What
will happen if the actual environment stiffness is kE = 10 6
[N/m] 7

e) Consider opposite case. Assume that the environment stiffness

kE is large 10 6 [N/m] and compute the adequate velocity feed-
back gain. How will the system behave if the actual environ-
ment stiffness is low (i.e. kE = 10 3 [N/m])7

7.20.* If in the system considered in the previous exercise, friction

force is taken into account, will the system exibit better sta-
bility characteristics or not [25]7

References

[1] Nevins J., D. Whitney et al., Exploratory Research in Industrial

Modular Assembly, Report R-996, The Charless Starc Draper Labora-
tory, Cambridge, Mass., 1976.

[2] Simunovic S., "Force Information in Assembly Processes", Proc. V

International Symposium on Industrial Robots, Chicago, 1975.

[3] Stokic M.D., Vukobratovic K.M., "Simulation and Control Synthesis

of Manipulator in Assembling Technical Parts", Journal of Dynamic
Systems, Measurement and Control, 1979.

[4] Stokic M.D., Vukobratovic K.M., Hristic S.D., "Implementation of

Force Feedback in Manipulation Robots", International Journal of
Robotic Research, No.4, 1985.

[5] Scheinman V., "Design of a Computer Controlled Manipulator", M.S.

Th., Mech. Engineering Dept., Stanford University, 1969.

[6] Nevins L., Whitney D., "The Force Vector Assembler Concept", Proc.
First International Conference on Robots and Manipulators, Udine,
Italy, September, 1973.

[7] Whitney E.D., "Histbrical Perspective and State of the Art in Ro-
bot Force Control", The International Journal of Robotics Research,
Vol. 6., No.1., 1987.

[8] Nevins J., \'lhitney D., "Assembly Research", Automatica, Vol. 16,
pp. 595-613, 1980.
 361

[9] Drake H., "The Use of Compliance in a Robot Assembly System",

IFAC Symp. on Information and Control Problems in Manufacturing
Technology, Tokyo, 1977.

[10] DeFazio T., Seltzer D., Whitney D., "The IRCC Instrumented Remote
Centre Compliance", Industrial Robot, 1984.

[11 ) Mason M., "Compliance and Force Control for Computer Controlled
Manipulators", MIT Artificial Intelligence Laboratory Memo 515,
April, 1979.

[12) Raibert M., Craig J., "Hybrid Position/Force Control of Manipula-

tors", Trans. of the ASME, Journal of Dynamic Systems, Measure-
ment and Control, Vol. 103, No.2, Juni, 1981.

[13] Craig J., Introduction to Robotics: Mechanics and Control, Addi-

son-Wesley Publishing Company, 1986.

[14 J Lozano-Perez T., Mason M., Taylor R., "Automatic Synthesis of

Fine Motion Strategies for Robots", Proc. 1st International Sym-
posium of Robotics Research, Bretton Woods, New Hampshire, August,
1983.

[15) Asada H., Slotine J., Robot Analysis and Control, John Wiley and
Sons, New York, 1986.

[16) Khatib 0., "A Unified Approach for Motion and Force Control of
Robot Manipulators: The Operational Space Formulation", IEEE Jo-
urnal of Robotics and Automation, Vol. RA-3, No.1, 1987.

[17) Khatib 0., Burdick J., "Motion and Force Control of Robot Manipu-
lators", Proc. of IEEE International Conference on Robotics and
Automation, 1986.

[18) Eppinger S. D., Seering W. P., "Understanding Bandwidth Limi tat'ions

in Robot Force Control", Proe. of IEEE International Conference
on Robotics and Automation, pp. 904-909, 1987.

[19) Yabuta T., Chona A., Beni G., "On the Asymptotic Stability of the
Hybrid Position/Force Control Scheme for Robot Manipulators",
Proc. of IEEE International Conference on Robotics and Automation,
pp. 338-343, 1988.

[20) Paul R.P., "Problems and Research Associated with the Hybrid Con-
trol of Force and Displacement", IEEE International Conference on
Robotics and Automation, 1987.

[21) Salisbury J.K., "Active Stiffness Control of a Manipulator in a

Cartesian Coordinates", Proc. of IEEE Conference on Decision and
Control, Albuquerque, 1980.

[22) Hogan N., "Impendance Control: An Approach to Manipulation: Part

I Theory, Part II - Implementation, Part III - Applications",
Trans. of the ASME, Journal of Dynamic Systems, Measurement, and
Control, Vol. 107, pp. 1-24, 1985.

[23) Kazerooni H., Houpt P.K., Sheridan T.B., "Robust Compliant Motion
for Manipulators: Part I - The Fundamental Concepts of Compliant
Motion, Part II - Design Methods", IEEE Journal of Robotics and
Automation, RA-2, pp. 83-105, 1986.
3~

[24] An C.H., Hollerbach J.M., "Dynamic Stability Issues in Force Con-

trol of Manipulators", Proc. of IEEE International Conference on
Robotics and Automation, pp. 890-896, 1987.

[25] Wedel D.L., Saridis G.N., "An Experiment in Hybrid Position/Force

Control of a Six Dof Revolute Manipulator", Proc. of IEEE Inter-
national Conference on Robotics and Automation, pp. 1638-1642,
1988.
Appendix
Software Package for Synthesis of Robot Control

In this Appendix listings of subroutines of a very reduced version of

the software package for computer-aided synthesis of control of robo-
tic systems are given. The package enables synthesis of the decentra-
lized control for the robot of arbitrary structure (with up to 6 de-
grees of freedom - up to 6 simple jOints). The package includes the
following functions:

- Read data on a robot for which the user wants to synthesize control
(structure and geometry of the robot, kinematic and dynamic parame-
ters of the robot, data on actuators); these data have to be defined
by the user in corresponding files-see description of input data fi-
les.

- Form dynamic model of the robot which consists of the model of mec-
hanical part of the robot and of the models of actuators (the model
of mechanical part is formed by programmes which listings are given
in the book: Dynamics of Manipulation Robots: Modelling, Analysis
and Examples, by M. Vukobratovic).

- Determine the maximum moments of inertia of the mechanism around the

axes of joints and form the models of the subsystems (actuator +
moment of inertia of mechanism).

compute the local servo systems feedback gains for individual joints
of the robot and actuators, using one of two available methods (ac-
cording to the user choice):

- by imposing of resonant structural frequence around the given jOint

and by requiring that the damping ratio must be equal to 1, or

- by optimal quadratic regulator for individual joint (the user has

to specify the weighting matrices and the prescribed (desired) de-
gree of exponential stability, and the user might, if he wants so,
introduce an tntegral feedback loop (FID regulator)).
364

- Determine the transfer functions of the open and closed-loop subsys-

tems (actuator + jOint).

- Compute trajectories of the joints for the specified initial and fi-
nal positions with both triangular and trapezoid velocity profile
(with specified movement time and with specified time of accelerati-
on/deceleration).

- Compute nominal driving torques in the joints for the specified tra-
jectories and compute nominal centralized or local programmed con-
trol.

- Form linearized model of the robot in the specified points on the

nominal trajectory, determine eigen-values of the open-loop and clo-
sed-loop matrices of the linearized model (with local servo feedback
loops) and check stability in accordance to the user requirements,
and re-compute the local control if the conditions of stability of
the linearized model are not fulfilled.

- Simulate the robot dynamics around the specified nominal trajec-

tory for the given initial conditions and various control laws (ac-
cording to the user wishes) :

- with the selected local servo systems,

- with or without the nominal centralized control or with the local

nominal control,

- with or without global control, and for each joint the user might
specify:

- the global control in the form of force feedback, or,

- the global control by on-line computation of driving torques

which uses approximative models of robot which has to pe selec-
ted by the user (as an approximative model computation of only
inertia matrix of the robot might be introduced, or on.ly gravity
moments, or centrifugal and Coriolis moments might be calculated,
or the liser might include computation of complete dynamic model
of the robot all the global control) ,
 365

selected form of the global gain (5.2.13) or (5.A.3) (the latter

form is allowed only i f the se;r:vo is synthes;L<led as loc"l opti-
mal quadratic regulator) .

In this Appendix the description of the package structure is given,

and the listings of all subroutines, with brief description of each
subroutine. The guide for forming of the task is also given. The pro-
grammes are written for VAX FORTRAN V.4.1 and they have been tested
at the VAx-750 computer un.der operating system VIiS 4.7, but they
can beeasely used at other computers which are supplied with FORTRAN
compiler.

The user has to prepare corresponding data files for hiS selected ro-
bot, according to guideline given in the text to follow.

In the Appendix is also given an example of application of the package

for control computation for a robot with three joints presented in Fig.
3.2. The data on the robot are given in Tables 3.3.1. and 3.3.2. The
example includes synthesis of the local servos according to the impo-
sed resonant frequencies of structure, computation of the trajectories
of the joints with the triangular velocity profile, analysis of stabi-
Ii ty of the linearized mod.el of the robot in two pOints at the nominal
trajectory, an.d simulation of tracking of the nominal trajectory with
t.he nominal centralized control and the local servo systems. The lis-
ting of ,,11 demanded input data files for this specific robot are gi-
ven (these da.ta files must be provided by the user), and listing of
the interaction between the user and the package also is given. The
pa.cKage fo;r:ms seve;r:al output data files which include simUlation re-
sults and, ;r:esul,ts of control synthesis. These data files are also des-
cribed. Re):,e, only t/1,e listing of the output data file which contains
computed local servo gain for the specific example is given, but the
other output d.ata files are not given due to lack of space.

Note: Note t.h.at in t/1,e presented package many options are not intro-
duced in order to condense the programmes and due to lack of
space. The u.ser can simply extend many functions and easely
in.t,rodl1ce various options which will increase the applicability
of this pacKage (;for example, the user might introduce computa-
tion of the local feedback gains by pole-placement method - see
Section 3.3.3, introduce specification and generation of the
nominal trajectories in external (Cartesian) coordinates - see
Chapter 2, introduce simulation of only individual (decoupled)
366

subsystems etc.). In lot of subroutines which read data from the

input data files and which communicate with the user, the neces-
sary verifications and testing of the data, specified by the
user, and also protection functions to prevent nonregular hand-
ling of the system are ommi tted, and this might produce certain prob-
lems in the application of this package. We recommand the user
to extend this package by himself in the above described sence,
and to add necessary protections (some of them are already in-
cluded in several subroutines in order to instruct the user how
this can be realized wherever it is necessary). We have also to
note that in writting these programmes the attempt has been to
simplify them as much as possible, in order to make them simple
for an user having no experience in software programming. Howe-
ver, in a few subroutines we have used some more sophisticated
programming procedures for educational purposes.

To use this package user needs programmes for modelling of mec-

hanical part of the robot. As mentioned before, the programme
support for modelling of robot dynamics is given in the listed
reference. However, the user may write his own programme for
modelling dynamics of his particular robot. He has to write pro-
gramme which for given joint angles q and velocities q computes
the inertia matrix H(q) and the vector h(q, q) (see Exercise
3.5). The programme should be called MODEL and it should inclu-
de the following two commons: COMMON/UG/ SI(6), SIDOT(6), and
COMMON/DINAM/ HH(6,6), H1 (6), where SI(6) is vector containing
values of joint angles, SIDOT(6) corresponds to joint velociti-
es, HH(6,6) is inertia matrix, H1 (6) is vector h.
 367

DESCRIPTION OF SUBROUTINES
Task KG includes the following subroutines:
KG - the main routine which calls all other subroutines
SMINIC - reads the initial values of variable, data
on auxiliary parameters and so on.
INPUT KIN - reads data on the kinematics of the
- selected robot
MODEL - the main subroutine for computation of the
inertia matrix H and the vector of centrjf.
Coriolis and gravity moments h - this sub-
routine and all subroutines called are
given in ref. M. vukobratovic: Applied
Dynamics of Robots, Springer-Verlag,Berlin,
1989.
COLIAS - the subroutine for the mechanims assem-
bling in the special case
MODCHK - testing of succesfull assembling of the
mechanism according to the user's data
INPUT DIN - reads the data on the dynamic parameters
of the robot
SM MAX IN - computes the maximal and minimal values
- - of the moments of inertia of the mecha-
nism around the joints axes
INPUT ACT - reads data on the actuators in the robot
joints and forms the models of actuators
and joints
INPUTL - r~ads data on the actuators
SMOPRG - synthesizes the local optimal regulators with
or without the integral feedback loop
SMOREG - solves the algebraic equation of Riccati
type
SMEIG3 - auxiliary subroutine for determination
of the eigen-values and eigen- vectors
of a given quadratic matrix
SMLOC E - synthesizes the local control feedback gains
(synthesis in S - domain)
TRAJEK - computes the nominal trajectories
(in the joints coordinates)
SMNOMP - computes the nominal moments and the nominal
(centralized or local) programmed control
LINANA - subroutine for analysis of the linearized
(total) model of the robot via determination
of the eigen-values of the system matrix with
or without the local controllers
SMLINM - forms matrices of the open-loop lineari-
zed model of the robot
SMLGAD - forms matrices of the closed-loop linea-
rized model of the robot
SMEIGN - auxiliary subroutine for computation of the
eigen-values and eigen-vectors of a given
quadratic matrix
SMSIMD - simulates the total dynamic model of the robot
SMOPTN - subroutine for imposing the control law
which will be simulated
SMINGL - for imposing the form and the gain of the
global control in simulation
SMCONT - computes the local control during the
simulation
SMCONG - computes the selected global gains during
the simulation
368

LIAPlS - computes subsystem Liapunov's function

GRADlS - computes gradient of the subsystem
Liapunov's function
SMMINV - the auxiliary subroutine for matrix
inversion
The auxiliary SSP subroutines which are used in the task:
GMPRD - for matrix multiplication
MINV - invert a given quadratic matrix
EIGENP,COMPVE,REALVE,HESQR,SCALE - subroutines
for computation of eigen-values and eigen-
vectors of a given quadratic matrix
The above listed subroutines uses the INCLUDE-files with
the COMMON-areas:
MODELM.MOD - COMMON-s for the subroutines related
to the MODEL of the mechanism
dynamics - see above given reference
CONFIG.MOD - "
INl:SMCOM.COM - COMMON-s with variables whose
description is given in the
corresponding subroutines
The subroutine SMLOC E uses the auxiliary data-file
INl:CONF.DAT for the-sake of better communication
with the user.
Note: for description of INl see the text to follow
 369

HIERARCHICAL STRUCTURE OF THE TASK

LEVEL: I II III IV V
KG
SMINIC
INPUT KIN
MODEL
COLIAS
MODCHK
INPUT DIN
SM MAX IN
MODEL
INPUT ACT
INPUTL
SMOPRG
SMOREG
SMEIG3
EIGENP(SCALE ... )
GMPRD
MINV
EIGENP
SCALE
COMPVE
REALVE
HESQR
SMLOC E
TRAJEK
SMNOMP
MODEL
LINANA
SMLINM
MODEL
MINV
SMLGAD
SMEIGN
EIGENP(SCALE ... )
SMSIMD
SMOPTN
SMINGL
MODEL
SMCONT
SMCONG
LIAPIS
GMPRD
GRADIS
GMPRD
SMMINV
MINV
370

INSTRUCTION HOW TO FORM THE TASK:

l.SET AT THE DIRECTORY WHERE ALL SUBROUTINES ARE
LOCATED AND ASSIGN THE FOLLOWING LOGICAL NAMES
ASS [directory name1 INl: - the directory where data-files
are located
ASS [directory name] IN2:
ASS [directory name] KRO: - directory where source codes
are located
2.Type the following commands for compilation of the subrou-
tines:
[email protected] [ret]
Form the library kro:MODEL.OLB which includes all subrouti-
nes necessary to form the model of the robot dynamics (the
subroutines are listed in the above given reference).
Form the library kro:EIGENP.OLB with all SSP subroutines
which are listed above.
3.Type the following command for linking of the task:
[email protected]
4.The task is executed by the following command:
$ R kro:KG
Before the running of the task the user has to check whether
all necessary input data-files are formed, containing the
data for the given robot (the data-files have the name of
the specific robot selected by the robot) - see instruc-
tions for forming of the data-files.

$ ! COMMAND FILE FOR SUBROUTINES COMPILATION

$ for kro:input kin, input din, input act,inputl,colias,modchk
$ for kro:smloc-e - -
$ for kro:domp
$ for kro:sm max in
$ for kro:trajek-
$ for kro:sminic
$ for kro:smnomp
$ for kro:linana,smlinm,smlgad,smeign,smoptn
$ for kro:smoprg,smoreg,smeig3,smorel,smeigl,smore2,smeig2
$ for kro:smsimd,smingl,smcong,smminv,smcont,liapls,gradls
$ lib/create kro:kg
$ lib kro:kg kro:*.obj
$ delete kro:*.obji*
$ for kro:kg

$ ! COMMAND FILE FOR LINKING THE TASK: KG

$ LINK kro:KG,KG/LIB,MODEL/LIB,EIGENP/LIB
 371

********************************************************************
* DESCRIPTION OF INPUT/OUTPUT DATA FILES *
********************************************************************

The package enables the synthesis of control, linear

analysis and simulation of any robot (with up to 6
degrees of freedom). The user has to prepare the data-
files which contains data on the robot for which he
wants to synthesize the control. The package as its
output generates the output data-file and/or prints
output data at the terminal.
All input/output data-files get their names according
to the following rule:
ROBOT NAME. EXT
where ROBOT NAME is a string of five characters which
contains the name of the robot selected by the user,
EXT denotes the type of data which is put in the cor-
responding data-file (see the text to follow). When
the user decides for which robot he wants to synthesi-
ze the control, he has to prepare the data-files with
data on his robot and in doing this he has to to obey
te above given rule on the files names.
The package requires the following input data-files:
ROBOT NAME.DAT - default values of certain parameters and
initial defining of the actuator matrices
for the sake of software reliability
ROBOT NAME.CNF - data on geometry, structure and kinematics
of the robot
ROBOT NAME.DNM - data on dynamic parameters of the robot
ROBOT NAME.ACT - data on the robot actuators
ROBOT NAME.TRA - data on specified joint trajectories
ROBOT NAME.INT - data on required exponential stability
degrees of the robot and on time instants
at the nominal trajectory in which the
user wants to analyze the stability of the
linearized model of the robot
As an output the package generates the following data-files
(names are given according to the above mentioned rule):
ROBOT NAME.LOC - data on synthesized local feedback gains
ROBOT NAME.ANG - the nominal joint trajectories as functions
of time
ROBOT NAME.DIN - the nominal joint trajectories, the nominal
driving torques, the nominal programmed
control as functions of time
ROBOT NAME.SIM - simulation results: deviations of the state
coordinates from the nominal, actual con-
trol signals, actual driving torques,
actual power of actuators and actual
energy consumptions as functions of time
372

*******************************************************************
* DESCRIPTION OF DATA-FILE ***.DAT *
*******************************************************************

The data-file ROBOT NAME.DAT contains default data on

actuators and parameters of control laws. The data on
actuators are given in this file only for the sake of
increasing the system reliability, although the actual
data on actuators are set in the data-file ROBOT NAME .
. ACT and then the models of actuators are formed. The-
refore, in this data-file the user might copy data on
the actuators matrices for any robot (it is recommended
to overtake data from the given example for the robot
UMSPR). The actual data for the selected actuators for
the specific robot, the user has to impose in the data-
-file ROBOT NAME.ACT (see description of this data-
file) .

File ROBOT NAME.DAT has to be formed according to the

following sequence:
1) N - the maximal number of degrees of freedom of the
robot (the actual value is given in the file
ROBOT NAME.CNF) - format 14
2) NI(I) - the order of the actuator in the i-th
joint - format 14
3) A(ni(i),ni(i),i), B(ni(i),i), F(ni(i),i) - the
matrices of the model of the i-th actuator -
- format 3E15.5

data 2)-3) are repeated in the file N-times

(it is recommended to copy data 1)-3) from the
file UMSPR.DAT - see Example)

4)Q(NI(j),NI(j)) - weighting matrix by the state in the

standard quadratic criterion for the local
optimal regulator - it must be positive
semidefinite and of dimensions NI(j)xNI(j)
where NI(j)=max(NI(i)) - format 3E15.5.
5) R(i) - the weighting elements by the control signals
in the standard quadratic criteria for the
local optimal regulators - must be N positive
numbers - format 3E15.5
6) QI(i) - the weighting elements by the "integral"
state coordinates in the standard quadratic
criteria for the local optimal regulators -
- must be N positive numbers - format 3E15.5
7) IOPTOR(i),IOPTV(i),IOPTI(i) - indicators whether in the
i-th local controller is applied or not:
- optimal quadratic regulator
- velocity feedback
- feedback by rotor current (pressure)
indicatiors are 1 if yes, 0 if not - format 14
Data under 7) have to repeat N times (for N local
controllers).
 373

*********************************************************************
* DESCRIPTION OF DATA-FILE ***.CNF
*********************************************************************
*
The data-file includes data on the robot structure. In the text
to follow brief descriptions of the variables for which data have
to be set are given and the corresponding formats are denoted.
The detailed description of the variables can be found in the
previously mentioned reference.

1) Number of degrees of freedom of the robot (format lX,Il) -

number of simple joints.

2) Indicator of the type of the first joint (0 - rotational, 1-

linear) (format lX,F15.6)

3) X coordinate of the unit vector of the first joint with respect

to the coordinate frame attached to the first link ell
(format lX,F15.6)

4) Y coordinate of the unit vector of the first joint with respect

to the coordinate frame attached to the first link ell
(format lX,F15.6)

5) z coordinate of the unit vector of the first joint with respect

to the coordinate frame attached to the first link ell
(format lX,F15.6)

6) X coordinate of the unit vector of the second joint with respect

to the coordinate frame attached to the first link e12
(format lX,F15.6)

7) Y coordinate of the unit vector of the second joint with respect

to the coordinate frame attached to the first link e12
(format lX,F15.6)

8) Z coordinate of the unit vector of the second joint with respect

to the coordinate frame attached to the first link e12
(format lX,F15.6)

9) X coordinate of the vector from the first joint to the mass cen-
ter of the first link with respect to the coordinate frame
attached to the first link rll 1m] (format lX,F15.6)

10) Y coordinate of the vector from the first joint to the mass cen-
ter of the first link with respect to the coordinate frame
attached to the first link rll 1m] (format lX,F15.6)

11) z coordinate of the vector from the first joint to the mass cen-
ter of the first link with respect to the coordinate frame
attached to the first link rll 1m] (format lX,F15.6)

12) X coordinate of the vector from the second joint to the mass cen-
ter of the first link with respect to the coordinate frame
attached to the first link r12 1m] (format lX,F15.6)

13) Y coordinate of the vector from the second joint to the mass cen-
ter of the first link with respect to the coordinate frame
attached to the first link r12 [m] (format lX,F15.6)
374

14) z coordinate of the vector from the second joint to the mass cen-
ter of the first link with respect to the coordinate frame
attached to the first link r12 1m) (format lX,FI5.6)
If the joint is linear, then at this place in the file ***.CNF
the following data on the robot structure have to be set:
15) The special vector for the linear joint "uii" X coordinate
(format lX,F15.6)
16) The special vector for the linear joint "uiill Y coordinate
(format 1X,F15.6)
17) The special vector for the linear joint "uii" Z coordinate
(format lX,FlS.6)
18) The special vector for the linear joint "ui,i+l" X coordinate
(format lx,FlS.6)
19) The special vector for the linear joint "ui,i+l" Y coordinate
(format lX,F1S.6)
20) The special vector for the linear joint "ui,i+l" Z coordinate
(format lX,FI5.6)
Description of the futher data to be set are identical as these
given for the first joint and they repeat n times for all joints
of the robot (n - number of joints)
At the very end of the file the data on the unit vector of the
axis of the first joint with respect to the absolute coodinate
frame are given:
x coordinate of the unit vector of the axis of the first joint
with respect to absolute coordinate frame "eO"(format 1X,FlS.6)
Y coordinate of the unit vector of the axis of the first joint
with respect to absolute coordinate frame "eO"(format 1X,F1S.6)
z coordinate of the unit vector of the axis of the first joint
with respect to absolute coordinate frame "eO"(format 1X,F1S.6)
 375

*****************************************************************
* DESCRIPTION OF DATA FILE ***.ACT *
*****************************************************************

Data file ***.ACT includes data on actuators which have been

adopted for the specific robot. In the text to follow the
description of the variables whose values have to be given in
this file, as well as formats of these values are listed.
1) The first line in the file represents the types of the
actuators in the robot joints. Each actuator is denoted by
two character symbol:
i) for D.C. electro-motors the symbol is DC
ii) for hydraulic actauator the symbol is HD
The format of data on the actuators types is 6(lX,A2).
Depending on the selected type of the actuator, in the next
lines of the data-file the following data have to be set (for
the actuator in the first joint):
- for D.C. electro-motors:
2) The second line contains a data on the order of the
actuator model; the vaule migh be: 2 or 3 - format lX,FIS.6
3) The third line contains a value of the mechanical constant of
the actuator Kern multiplied by the moment reduction ration of
the gear reducer [Nm/A] - format IX,FIS.6
4) The electromotor constant Kme multiplied by the speed reduction
ratio of the gear reducer [V/rad/s] - format Ix,FIS.6
S) The moment of inertia of the rotor [kg*m**2] - format IX,FIS.6
6) The coefficient of the viscous friction multiplied by the speed
reduction ratio and the moment reduction ratio of the gear redu-
cer [Nm/rad/s] - format IX,FIS.6.
7) The resistance of the rotor curcuit [Om] - format IX,FIS.6
8) The absolute value of the maximal allowed input voltage signal
(for positive values of signals) for D.C. motor [V) - format
IX,FIS.6.
9) The absolute value of the maximal allowed input voltage signal
(for negative values of signals) for D.C. motor [V) - format
IX,FIS.6.
10) The speed reduction ratio of the gear reducer-format IX,FIS.6.
11) The moment reduction ratio of the gear reducer-format lX,FlS.6.
12) The inductivity of the rotor curcuit [H] - format lx,FlS.6.
13) The maximal driving torque of the motor [Nm) - format lX,F15.6.
14) The motor maximal power [WI - format lX,FlS.6.
376

15) This values is of no use for D.C. motor model and therefore
any number might be put - format lX,F15.6

16)

- for hydraulic actuator:

2) The coefficient of viscous friction [Nm/m/s]; lx,F15.6.

3) The piston area [m**2]; lX,F15.6.

4) The cylinder volume [m**3]; lX,F15.6.

5) The mass of piston [kg]; lX,F15.6.

6) The coefficient of the oil compressibility [N/m**2];lX,FlS.6.

7) The gradient of the flow-preassure characteristics of the servo-

-valve [m**3/s/N/m**2]; lX,F15.6.

8) The coeficient of the proportionality between the current in the

servo-valve coil and the oil flow [m**3/s/mA]; lx,FlS.6.

9) The absolute value of the maximum allowed input current for the
servo-valve (for positive values of current) [mAl; lX,FlS.6.

10) The absolute value of the maximum allowed input current for the
servo-valve (for negative values of current) [mAl; lX,F15.6.

The next lines of this file contain values of the same parameters
of the actuators in the next joints of the robot, and therefore the
descriptions of the lines 2-15 repeat for the next sets of values
(16-29, 30-43, etc.). The file contains the sets of data on n actua-
tors in the n joints of robots.
 377

*********************************************************************
* DESCRIPTION OF DATA-FILE ***.DNM *
*********************************************************************

The data-file ***.DNM includes data on dynamic parameters of the

robot. In the text to follow brief descriptions of the variables
whose values have to be set in this data-file are given as well
as formats in which the values must be set in the file.

1) The link type (1- link of the cane type, 0 - not cane)
(format 1x,F15.6)

2) Link mass [kg] (format lX,F15.6)

3) Moment of inertia of link "Jxx" or "Js" depending on the link

type [kg*m**2] (format 1X,F15.6)

4) Moment of inertia of link "Jyy" or "In" depending on the link

type [kg*m**2] (format 1X,F15.6)

5) Moment of inertia of link "Jzz" (if link type 1) or any number

(if link type 0) [kg*m**2] (format 1X,F15.6)

6) Maximal allowable angle (displacement) of joint [rad or m]

(format 1X,F15.6)

7) Minimal allowable angle (displacement) of joint [rad or m]

(format 1X,F15.6)

Next data are repeated for all links of the robot

according to the same sequence.

*********************************************************************
* DESCRIPTION OF DATA FILE ***.TRA *
*********************************************************************

The data file includes data for the given robot which are used in
trajectory generation. The description of the values for which
the data have to be set are presented according to the sequence
in which they are set in the file.

1) QO(I) - the vector of the initial positions of the joints given

in: [m] - if the joint is linear
[rad] - if the joint is rotational
(format 1x,6FIO.5)
2) QF(I) - the vector of the terminal positions of the joints given
in: [m] -if the joint is linear
[rad] -if the joint is translational
(format 1X,6FIO.5)
3) H sampling period at which the values of joints coordinates
along the trajectory are computed in [s]
(format lX,6FlO.5)
4) T - time duration of the nominal movement (between the initial
and terminal positions) in [5]
(format lX,6FlO.5)
378

*******************************************************************
* DESCRIPTION OF DATA-FILE ***.INT *
*******************************************************************

The data-file ROBOT NAME.INT contains data on the time

instants along the nominal trajectory in which the ana-
lysis of the linearized model stability is required. The
exponential stability of the robot linearized model around
the nominal trajectory is tested. The desired exponential
stability degrees have also to be imposed. The user has to
prepare this file according to the following structure:
- the number of points along the nominal trajectory of
the robot in which the linearized model has to be
analyzed - format 14
time instant at the nominal trajectory in which
the analysis of stability of the linearized model
is required - format E15.5 (the user must
carefully specify the instant which is within
the adopted duration of tpe nominal movement
- this duration is specified in the file
***.TRA)
- the exponential stability degree of the linea-
rized model arond the nominal trajectory which
the user wants to achieve - format E15.5 (this
value must be positive and usually there is no
sense to impose values greater than 20. due
to numerical problems, save for some special
cases)

The last two data have to be specifeid as many times

as is the number of the points at the nominal trajecto-
ry around which the user wants to analyze the robot
stability (this number is specified in the first line
of the file).
 379

**********************************************************************
* DESCRIPTION OF DATA-FILE ***.LOC *
**********************************************************************

The output data-file ROBOT NAME.LOC contains the data

on the computed feedback gains of the local controllers.
The data-file has the following form:
- position feedback gain of the local controller
around the first jOint [V/rad) or [Vim) if D.C.
electro-motor is applied, [mA/rad) or [mAIm) if
hydraulic actuator is applied - format F12.S
- velocity feedback gain of the local controller
around the first joint [V/rad/s),[V/m/s) or
[mA/rad/s), [mA/m/s) - format F12.S
- gain in the feedback loop by the rotor current
if D.C. motor is applied [VIA), or by pressure
if hydraulic actuator is applied in the first
joint [mA/N/m**2) - format F12.5
- integral feedback gain in the local controller
around the first joint [v/rads), [v/ms) or
[mA/rads),[mA/ms) - format F12.S
The data on the feedback gains in the local
controllers in the other joints are repeated according
to the same sequence - the data are given for all
joints of the specific robot.

~***************************************************** *********
* DESCRIPTION OF DATA-FILE ***.ANG *
***************************************************************

The output data-file ROBOT NAME.ANG contains data on

the nominal trajectory. The file is the output of the
subroutine TRAJEK. The structure of the file is as
follows:
TE - time instant [s) - format E13.5
0(6) - nominal trajectories of the robot joints
the values of the joint coordinates in the
instant TE at the nominal trajectory
[rad) or [m) - format 6E13.S
In the line there are N data (N the number of
the joints of the specific robot)
00(6) - nominal trajectories of the joint velocities
[rad/s) or [m/s) - format 6E13.5
In the line there are N data.
000(6) - nominal trajectories of the joint accelera-
tions [rad/s**2) or [m/s**2)-format 6E13.5
In the line there are N values.
The listed data are repeated (at each time interval of
0.01 [s) m times, where m is an integer number obtai-
ned if the time duration of the nominal movement (defi-
ned in the file ***.TRA) is devided by 0.01.
300

**********************************************************************
* DESCRIPTION OF DATA-FILE ***.DIN *
**********************************************************************

The output data-file ROBOT NAME.DIN contains the data on

the nominal trajectories, the nominal driving torques,
the nominal programmed control, the nominal power and
energy. The file is the output of the subroutine SMNOMP.
The structure of the file is as following:
TIME - the time instant [s] - format E13.5
0(6) - nominal trajectories of the robot joints -
values of the joint coordinates at the nomi-
nal trajectory at the time instant TIME
[rad] or [m] - format 6E13.5
In the line there are N data (N the number of
joints of the user's specific robot).
00(6) - nominal trajectories of the joint velocities -
[rad/s] or [m/s] - format 6E13.5
There are N data in the line.
000(6) - nominal trajectories of the joint accelera-
tions [rad/s**2] or [m/s**2] - format 6E13.5
There are N data in the line.
PO(6) - nominal driving torques around the joints of
the robot (in the time instant TIME [Nm] or
[N] - format 6El3.5.
There are N data in the line.
UO(6) - the nominal programmed control for the actua-
tors of the joints [V] or [mAl-format 6El3.5
There are N data in the line.
POWER(6) - the required nominal power in the robot
joints [W] - format 6E13.5.
There are N data in the line.
ENERG(6) - the demanded nominal energy in the joints
of the robot [Ws] - format 6E13.5.
There are N data in the line.
These data are repeated (at each time interval of 0.01 [s])
m times, where m is the integer number which is obtained when
the time duration of the nominal movement (specified in the
file ***.TRA) is devided by 0.01.
 381

***************************************************************
* DESCRIPTION OF DATA-FILE ***.SIM *
***************************************************************

The output data-file ROBOT NAME.SIM contains data on

simulated (actual) trajectory, driving torques, actual
control signals, power and energy consumptions. The
file is an output of the subroutine SMSIMD and repre-
sents the results of the simulation of the robot dy-
namics in tracking of the nominal trajectory with
the selected control law.
The structure of the file is as follows:
TIME - time instant [s) - format E13.5
Y(18) - the simulated deviations of the actual
state coordinates from the nominal trajectory
(from the file ***.ANG)- the sequence of da-
ta is: the deviation of the joint angle from
the nominal [rad) or [m), the deviation
of the joint velocity from the nominal [rad/s)
or [m/s), deviation of the currenl (pressure)
of the actuator from the nominal values (if the
third order modelis applied) [A) or [bar) -
the data are repeated for N joints [N is the
number of the user's specific robot) -
- format 6E13.5.
P(6) - simulated (actual) driving torque around the robot
joints (in the instant TIME) [Nm) or [N) -
- format 6E13.5. - torques deeloped during the trac-
king of the nominal trajectory
In the line there are N data.
UO(6) - simulated (actual) control signals for the
actuators [V) or [mA) - format 6E13.5.
In the line tere are N data.
POWER(6) - simulated (required) actual power in each joint of
the robot [W) - format 6E13.5.
In the file there are N data.
ENERG(6) - demanded energy in the robot joints [ws)
- format 6E13.5.
In the line there are N data.
These data are repeated (at each time interval of 0.01 [s))
m times, where m is the integer number which is obtained when
the time duration of the nominal movement (specified in the
file ***.TRA) is devided by 0.01.
382

C---------------------------------------------------------------------
C ROUTINE: KG
C---------------------------------------------------------------------
C FUNCTION: THE MAIN ROUTINE OF THE TASK-
C IT CALLS THE FOLLOWING SUBROUTINES:
C SMINIC - reads the initial values of variable, data
C on auxiliary parameters and so on.
C INPUT KIN - reads data on the kinematics of the
C selected robot
C INPUT DIN - reads the data on the dynamic parameters
C of the robot
C INPUT ACT - reads data on the actuators in the robot
C joints and forms the models of actuators
C and joints
C SMOPRG - synthesizes the local optimal regulators with
C or without the integral feedback loop
C SMLOC E - synthesizes the local control feedback gains
C (synthesis in S - domain)
C TRAJEK - computes the nominal trajectories
C (in the joints coordinates)
C SMNOMP - computes the nominal moments and the nominal
C (centralized or local) programmed control
C LINANA - subroutine for analysis of the lineari-
C zed (total) model of the robot via determi-
C nation of the eigen-values of the system
C matrix with or without the local control-
C lers
C SMSIMD - simulates the total dynamic model of the
C robot
C------------------------------------------------------------------
C INPUT VARIABLES:
C FILE(S) - name of the robot
C N - number of joints of user's robot
C ALFAI(6) - prescribed degrees of the exponential
C stability of local subystems
C ICNVLN - indicator whether the linearized
C model of the system is stable or not
C-------------------------------------------------------------------
C OUTPUT VARIABLES:
C IOPTOR(6) - indicators whether the local optimal
C regulator is applied in the joint
C IOPTIN(6) - indicators whether the local integal
C feedback loop is applied in the local
C controler, or not
C-------------------------------------------------------------------
include 'in2:smcom.com'
C
COMMON/OPTPR/ ICNVLN
COMMON/file/ filetS)
COMMON/rbsl/ i
CHARACTER*1 file,filep*S,a$
C--------------------------------------------------------------------
C
C READ NAME OF THE ROBOT (up to S characters)
C All input/output data-file on robot have the
C given name of the robot (by convention ROBOT NAME.ext,
C where ext points to the type of data in the data-file;
C for example ROBOT NAME.CNF contains data on the robot
C structure and geometry)
C
 383

WRITE (6,2)
READ (5,3) filep
DO ipom~1,5
file(ipom)=filep(ipom:ipom)
END DO
C
C raed auxiliary data on the robot
C
CALL sminic
C
C read data on the robot
C
CALL input kin
CALL input-din
CALL input-act
C-----------------=------------------------------------------------
C
C computation of the local controllers around all n joints
C of the robot (n - number of joints)
C
TYPE 111
9 DO i=1,n
C
C user's decision if he wants to apply local optimal
C controller or not, and if he wants to introduce
C the integral feedback loop in the i-th joint
C
TYPE 100,i
READ(S,101)yes
IF(yes.eq.'Y')then
ioptor(i)=1
ioptin(i)=O
type 104,i
read(S,101)yes
if(yes.eq.'Y')ioptin(i)=l
C
C set the prescribed exponential stability degree for the
C i-th subsystem (joint)
C
10 TYPE 10S,i
READ(5,106)alfai(i)
IF(alfai(i).lt.1 .• or.alfai(i).gt.20.)go to 10
C
C if the local optimal regulator is seleceted
C
CALL smoprg(i)
else
C
C if the synthesis in the S-domain is selected
C
CALL smloc e
ioptor(i)=O
end if
C
end DO
C
C data on local feedack gains are written in the
C output data-file ***.LOC (if the user wants so)
C
TYPE 107
READ(S,10l)yes
384

IF(yes.eq.'Y') then
OPEN(unit=2,name=filepll'.LOC',type='NEW')
DO 11 i=l,n
WRITE(2,106)pkl(i)
WRITE(2,106)pk2(i)
WRITE(2,106)pk3(i)
11 WRITE(2,106)pk4(i)
CLOSE(2)
end IF
C-----------------------------------------------------------------
C
C computation of the nominal trajectories (in the joints
C coordinates)
C
CALL trajek
C
C computation of the nominal programmed control
C
CALL smnomp
C-----------------------------------------------------------------
C
C linear analysis - linearization of the entire model
C of the robot and computation of the eigen-values of
C the open-loop and the closed-loop system matrix
C (when the local controllers are applied)
C
TYPE 102
READ(5,101)yes
IF(yes.ne.'Y')GO TO 18
CALL linana
C
IF(icnvln.EQ.O)THEN
TYPE *,' The linearized model is stabilized with'
TYPE *,' the prescribed exponential stability degree'
ELSE
TYPE *,' The linearized model is not adequatly'
TYPE *,' stabilized by the selected local controllers'
TYPE 108
READ(5,101)yes
IF(yes.EQ.'Y')GO TO 9
end IF
C--------------------------------------------------------------------
C
C simulation of the entire dynamic nonlinear model of robot
C
18 CALL smsimd
TYPE 112
READ(5,101)yes
IF(yes.EQ.'Y') GO TO 18
C--------------------------------------------------------------------
C
2 format ($,lx,'THE ROBOT NAME [max. length 5 characters]:')
3 format (as)
5 format (il)
100 format(' Do you want synthesis of local optimal regulator',I,
1 $,' for the',I3,'th joint [YIN]?:')
102 format(II,$,' Do you want linear analysis [YIN]?:')
101 format(a1)
104 format($,' Want to apply integral feedback loop for the',i3,
I 'th joint [YIN]?:')
105 format($,' Imose the exponential stability degree for the',
 385

1 i3,'th jOint [1.-20.]:')

106 format(f12.S)
107 format($,' want to write the feedback gains in the file [YIN]
1 ?:' )
108 format($,' want to re-synthesize the local controllers
1 [YIN]?:' )
110 format(II,' Simulation of nonlinear dynamic model of
1 the robot' ,II)
111 format(/I,' Synthesis of local controllers around robot
1 joints',//)
112 format(//,$, , want another simulation of robot [YIN]?:')
C
end

C----------------------------------------------------------------
C SUBROUTINE: SMINIC
C----------------------------------------------------------------
C FUNCTION: READ DATA REQUIRED FOR THE CONTROL
C SYNTHESIS - READ DATA FROM THE FILE
C ROBOT NAME. DAT
C----------------------------------------------------------------
SUBROUTINE SMINIC
C
INCLUDE 'IN2:SMCOM.COM'
C
byte FTOT(14)
byte FILE(S)
COMMON/FILE/ FILE
C
DATA FTOT(1)/'I'/,FTOT(2)/'N'/,FTOT(3)/'1'/,FTOT(4)/':'/,
1 FTOT(10)/'.'I,FTOT(11)/'D'/,FTOT(12)/'A'I,
2 FTOT(13)/'T'I,FTOT(14)/0/
C---------------------------------------------------------------
C
DO 1=l,S
FTOT(I+4)=FILE(I)
END DO
C
OPEN(UNIT~2,NAME=FTOT,TYPE='OLD',ERR=3000)
C
NUK=O
C
READ(2,100)N
100 FORMAT(I4)
101 FORMAT(3E15.5)
C
C default data on actuators
C
DO 1 I=l,N
C
READ(2,100)NI(I)
READ(2,101)((A(J,K,I),K=1,NI(I»,J=1,NI(I»
READ(2,101)(B(J,I),J=1,N1(1»
READ(2,101)(F(J,1),J=1,N1(1»
C
C computation of the total order of the robotic system
C
386

NUK-NUK+NI(I)
1 CONTINUE
C
READ(2,101)«QQ(I,J,1),I=1,NI(1»,J=1,NI(1»
C
DO 10 II=2,N
DO 10 1=1,3
DO 10 J=1,3
10 QQ(I,J,II)=QQ(I,J,l)
READ(2,101)(R1(I),I=1,N)
READ(2,101)(QI(I),I=1,N)
C
DO 11 I=l,N
READ(2,100)IOPTOR(I),IOPTV(I),IOPTI(I)
11 CONTINUE
C
CALL CLOSE(2)
RETURN
C
3000 TYPE 3001,FILE
3001 FORMAT(' DIAG*** File ',5A1,'.DAT cannot be found')
RETURN
END

C---------------------------------------------------------------------
C SUBROUTINE: INPUT KIN
C ••••••••••••••••••••••• • -:- ••••••••••••••••••••••••••••••••••••••••••••
C FUNCTION: THE MAIN SUBROUTINE FOR INPUT DATA ON
C KINEMATIC PARAMETERS OF THE USER'S SPECIFIC ROBOT
C •••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••
C INPUT: N - number of the joints (NJ = N)
C 10 - the order number of the joint
C OUTPUT: Kinematic parameters of the user's robot
C •••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••
SUBROUTINE INPUT KIN
C
INCLUDE 'CONFIG.MOD'
INCLUDE 'MODELM.MOD'
COMMON /MODSW/KOD
COMMON/LINSPE/ UD(6,3),UG(6,3)
COMMON/ORDGLB/ NUK,NJ
COMMON/FILE/ FILE(5)
BYTE file
C
COMMON /INTR/ SIMB(19,6)
DIMENSION EPOC1(3)
CHARACTER*2 OPC1
CHARACTEr*4 POM1
CHARACTER*9 FILEP,FILEP2,FILEP4
C
FILEP=CHAR(FILE(1»//CHAR(FILE(2»//CHAR(FILE(3»//
> CHAR(FILE(4»//CHAR(FILE(5»//'.CNF'
c .................................................................. .
C read data from the file ***.CNF
C
INEW=O ! Indicator of new file 1:o1d; O:new
DESNI=l ! initialization of the coordinate frame
C DESNI=l - right, DESNI=O - left frame
OPEN (55,FILE=FILEP,STATUS='OLD',ERR=11)
INEW=l
 387
WRITE (6,14) FILEP
14 FORMAT (/,lX,' DIAG*** Fi1e:',a10,' is found',/)
read (55,'(lX,i1)') n
nj=n
DO IPOM=l,n
DO JPOM=1,19
READ (55,'(lX,F15.6)') SIMB(JPOM,IPOM)
END DO
END DO
DO IPOM=1,3
READ (55,'(lX,F15.6)') EPOC(IPOM)
END DO
CLOSE (55)
if (inew.eq.1) GO TO 12
11 WRITE (6,13) FILEP
13 FORMAT (/,lX,'DIAG*** File ',a10,' cannot be found')
STOP
C
C variables substitutions
C
12 do ipom=l,n
ksi2(ipom)=simb(1,ipom)
do kpom=1,3
eu(ipom,kpom)=simb(kpom+l,ipom)
eul(ipom,kpom)=simb(kpom+4,ipom)
rOu(ipom,kpom)=simb(kpom+7,ipom)
if (ipom.eq.n) then
rt(kpom)=simb(kpom+10,ipom)
else
rOu(ipom+n,kpom)=simb(kpom+lO,ipom)
end if
end do
end do
C
C examine the values of the variables
C
do ipom=l,n
C
C the vector ei must be the unit vector
C
eum=sqrt(eu(ipom,l)*eu(ipom,l)+
> eu(ipom,2)*eu(ipom,2)+eu(ipom,3)*eu(ipom,3) )
if (eum.gt.1.01.0R.eum.lt.O.99) then
write (6,'("+DIAG*** vector ei" ,i2,
> is not unit vector")') ipom
stop
end if
end do
do ipom=l,n
C
C the vector ei,i+1 must be unit vector
C but there is no need to examine the vector
C ei,i+1 in the terminal joint of the robot
C
if (ipom.lt.n) then
eu1m=sqrt(eu1(ipom,1)*eul(ipom,1)+
> eu1(ipom,2)*eul(ipom,2)+eul(ipom,3)*
> eu1(ipom,3»
if (eu1m.gt.1.01.0R.eulm.lt.O.99) then
write (6,'("+DIAG*** The vector ei+1" ,i2,
> " is not unit vector")') ipom
388

stop
end if
end if
end do
c
c examine whethel' there is at least one linear joint
c
do ipom=l,n
if (ksi2(ipom).eq.l) iflO=l
end do
if (ifIO.eq.l) then !IFLO=ljthere is at least
one linear joint
do ipom=l,n !=Ojthere is no linear joints
if (ksi2(ipom).eq.l) then
do kpom=l,3
ud(ipom,kpom)=simb(kpom+l3,ipom)
ug(ipom,kpom)=simb(kpom+l6,ipom)
end do
else
do kpom=l,3
ud(ipom,kpom)=O
ug(ipom,kpom)=O
end do
end if
end do
c
C examine the values of input data
C
do ipom=l,n
C
C test whether the vectors uii,ui,i+l are unit vectors or not
C the vector uii must be unit vector
C
if (ksi2(ipom).eq.l) then
udm=sqrt(ud(ipom,l)*ud(ipom,l)+
> ud(ipom,2)*ud(ipom,2)+
> ud(ipom,3)*ud(ipom,3))
if (udm.gt.l.Ol.or.udm.lt.O.99) then
write (6,'(" DIAG*** vector uii" ,i2,
> "is not unit vector" )') ipom
stop
end if
end if
end do
c
C the vector ui,i+l must be unit vector
C
do ipom=l,n
if (ksi2(ipom).eq.l) then
ugm=sqrt(ug(ipom,l)*ug(ipom,l)+
> ug(ipom,2)*ug(ipom,2)+
> ug(ipom,3)*ug(ipom,3))
if (ugm.gt.l.Ol.or.ugm.lt.O.99) then
write (6,'(" DIAG*** Vector ui,i+l" ,i2,
> " is not unit vector" )') ipom
stop
end if
end if
end do
 389

else
epocm=sqrt(epoc(1)*epoc(1)+epoc(2)*epoc(2)+
> epoc(3)*epoc(3»
if (epocm.lt .. 99.or.epocm.gt.l.Ol) then
stop
end i f
end i f
C
C test collinearity of the joints axes and the link
C axes and assembling of the kinetic chain
C
do ipom=l,n
call colias(ipom)
end do
kod=l
call model
call modchk(imod)
if (imod.lt.O) then
write (6,'(/," FATAL*** ASSEMBLING OF THE MECHANISM:
> UNSUCCESSFUL")')
return
else
write (6,'(/," DIAG*** Assembling of the mechanism:
> SUCCESSFUL")')
end if
return
END

C---------------------------------------------------------------------
C SUBROUTINE: COLIAS
C---------------------------------------------------------------------
C FUNCTION: GENERATING OF THE VECTORS NECESSARY FOR
C THE MACHANISM (KINEMATIC CHAIN) ASSEMBLY
C IF THE VECTORS (Rii,ei) or (Ri,i+l,ei+l) ARE
C COLINEAR
C---------------------------------------------------------------------
C INPUT VARIABLES:
C KSI2 joint type
C EPOC the unit vector of the axis of the first joint
C with respect to absolute coordinate frame
C UD special unit vector for the i-l-st link if
C the i-th joint is linear
C UG special unit vector for the t-th link if the
C joint is linear
C ROU link vectors (Rii and Ri,i+l)
C EU the unit vector of the axis of the i-th joint
C EUl the unit vector of the axis of the i+l-st
C joint
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C IASl indicator whteher the i-th link is special
C since (Ei 1 1Ri i): IAS1=0 the link is not
C "special", IAS1=1 the link is "special"
C IAS2 indicator whether the i-th link is "special"
C since (Ei, i+lll Ri, i+l) :
C =0 the link is not "special"
C =1 the link is "special"
C RAS the vectors for the mechanism assembling if
C the i-th link is "special"
C--------------------------------------------------------------------
390

SUBROUTINE COLIAS(I)
C
INCLUDE 'IN2:CONFIG.MOD'
COMMON/LINSPE/ UD(6,3),UG(6,3)
REAL IDEL(3)
DIMENSION IAS(6)
C
IAS1( I)-O
IAS(I)-O
IAS2(I)-0
C
IF(I.EQ.1.AND.KSI2(I).EQ.1)GO TO 567
ROPOC(1)--EPOC(3)
ROPOC(2)=-EPOC(1)
ROPOC(3)=-EPOC(2)
GO TO 4000
567 DO 403 K=1,3
ROPOC(K)=UD(I,K)
403 CONTINUE
4000 IND=O
M=I
IF(KSI2(I).EQ.1)GO TO 444
DO 15 K-1,3
IMUL1=0
IF(ROU(M,K).EQ.0.)IMUL1=1
IMUL2-0
IF(EU(I,K).EQ.0.)IMUL2=1
IF(IMUL1.NE.IMUL2)GO TO 200
IF(IMUL1.EQ.0.AND.IMUL2.EQ.0)GO TO 56
IDEL(K)=O. .
GO TO 15
56 IDEL(K)=ROU(M,K)/EU(I,K)
15 CONTINUE
C
300 IF(IDEL(l).EQ.O.)GO TO 101
IF(IDEL(2).EQ.0.)GO TO 102
IF(IDEL(3).EQ.0.)GO TO 103
IF(IDEL(1).EQ.IDEL(2).AND.IDEL(2).EQ.IDEL(3))GO TO 104
IAS(I)=O
GO TO 200
104 IAS(I)=l
GO TO 201
103 IF(IDEL(1).EQ.IDEL(2))GO TO 104
IAS(I)=O
GO TO 200
102 IF(IDEL(3).EQ.0.)GO TO 105
IF(IDEL(1).EQ.IDEL(3))GO TO 104
IAS(I)-O
GO TO 200
105 IAS(I)=l
GO TO 202
101 IF(IDEL(2).EQ.0.)GO TO 106
IF(IDEL(3).EQ.0.)GO TO 105
IF(IDEL(2).EQ.IDEL(3))GO TO 104
IAS(I)=O
GO TO 200
106 IF(IDEL(3).EQ.0.)GO TO 107
GO TO 105
107 TYPE 1016
1016 FORMAT(' FATAL***** The unit vectors of the joint axis and of
 391

*the link are zero-vectors*******')

STOP
200 RAS(M,l)-O.
RAS(M,2)-0.
RAS(M,3)~0.
GO TO 204
201 RAS(M,l)-l.
RAS(M,2)~0.
RAS(M,3)-0.
GO TO 204
202 IF(ROU(M,l).EQ.O.)GO TO 599
RAS(M,l)=O.
RAS(M,2)=1.
IF(IND.EQ.l)RAS(M,2)=-1.
RAS(M,3)=0.
GO TO 204
599 RAS(M,l)=l.
IF(IND.EQ.l)RAS(M,l)--l.
RAS(M,2)-0.
RAS(M,3)-0.
GO TO 204
444 IASl(I)=l
DO 446 K=l,3
RAS(M,K)=UG(I,K)
446 CONTINUE
GO TO 666
445 IAS2(I)=l
DO 447 K=l,3
RAS(M,K)=-UD(I+l,K)
447 CONTINUE
GO TO 900
204 IF(IND.EQ.l)GO TO 500
IASl(I)=IAS(I)
666 M=I+N
IF(KSI2(I+l).EQ.l)GO TO 445
IF(I.EQ.N)GO TO 505
DO 25 K-l,3
IMULl=O
IF(ROU(M,K).EQ.O.)IMULl-l
IMUL2=0
IF(EUl(I,K).EQ.0.)IMUL2=l
IF(IMULl.NE.IMUL2)GO TO 600
IF(IMULl.EQ.0.AND.IMUL2.EQ.0)GO TO 79
IDEL(K)-O.
GO TO 25
79 IDEL(K)-ROU(M,K)/EUl(I,K)
25 CONTINUE
IND-l
GO TO 300
500 IAS2(I)=IAS(I)
900 RETURN
505 IAS2(I)-0
RETURN
600 IAS2(I)=0
RAS(M,K)-O.
RAS(M,K)-O.
RAS(M,K)=O.
RETURN
END
392

C---------------------------------------------------------------------
C SUBROUTINE: MODCHK
C---------------------------------------------------------------------
C FUNCTION: EXAMINES IF THE MECHANISM (KINEMATIC CHAIN) CAN BE
C SUCCESSFULLY ASSEMBLED; PRINTS THE TRANSFORMATION
C MATRIX AND CHARACTERISTIC KINEMATIC PARAMETRS
C --------------------------------------------------------------------
C INPUT VARIABLES:
C Q (3,3) Transformation matrix of the terminal
C link (after mechanism assembling) - An
C lAX indication of the terminal link orientation
C SI joint coordinates
C E the unit vectors of the joints axes with
C respect to absolute coordinate frame
C RP
C RT position vectors with respect to abs. frame
C RTT
C OUTPUT VARIABLES:
C ICHK indicator of assembling of the mechanism
C ICHK=O successful assembling
C ICHK=-l unsuccessfull assembling - data on
C the mechanism are not consistent
C and the user must change data in the
C file ***.CNF
C
SUBROUTINE MODCHK(ICHK)
C
INCLUDE 'IN2:CONFIG.MOD'
INCLUDE 'IN2:MODELM.MOD'
DIMENSION RPl~6,6,3),RP2(6,6,3),RP3(6,6,3),RP4(6,6,3),
> RP5(6,6,3)
C
ICHK=O
DO 1 1-1,3
LL=O
DO 2 K=l,3
IF(Q(I,K).NE.O) LL=LL+l
2 CONTINUE
IF(LL.EQ.O) ICHK=-l
1 CONTINUE
TYPE *,' CHARACTERISTIC KINEMATIC VARIABLES'
TYPE *,' ----------------------------------,
TYPE *,' Joint coordinates: '
TYPE 1099,(SI(I),I=l,N)
1099 FORMAT(6(E13.5))
TYPE *
TYPE *,' Transformation matrix of the terminal link An:'
TYPE 100,«Q(I,J),J-1,3),I=l,3)
100 FORMAT(lX,3F10.5)
TYPE *
TYPE *,' Joint axes with respect to absolute coord. frame'
DO 202 I=l,N
TYPE 333,I,(E(I,K),K=l,3)
333 FORMAT(/,' E(',I1,')- ',3(F10.5))
202 CONTINUE
TYPE *
TYPE *,' position vectors of the centers of masses and of
> joints'
DO 303 I=l,N
IF(I.EQ.N) GO TO 505
 393

J-I+l
IF(I.EQ.3) GO TO 997
IF(I.EQ.4) GO TO 996
DO 339 K-l,3
RP4(I,J,K)_RP(I,I,K)+RP(J,J,K)-RP(J,I,K)
339 CONTINUE
TYPE 444,I,I,(RP(I,I,K),K=1,3),I,J,(RP4(I,J,K),K=1,3)
444 FORMAT(/,' R(' ,11,',' ,II,' )-' ,x,3FB.3,
, R(',Il,',',Il,')-',X,3FB.3)
* GO TO 303
997 DO BB7 K=1,3
RPl(I,J,K)=RP(I,I,K)-RP(J,I,K)
887 CONTINUE
TYPE 444,I,I,(RP(I,I,K),K=1,3),I,J,(RPl(I,J,K),K=1,3)
GO TO 303
996 DO 774 K=1,3
RP2(I,I,K)=RTT(I,I,K)
RP3(I,J,K)=RTT(I,I,K)+RP(J,J,K)-RP(J,I,K)
774 CONTINUE
TYPE 444,I,I,(RP2(I,I,K),K=1,3),I,J,(RP3(I,J,K),K-l,3)
GO TO 303
505 DO 677 K=1,3
677 RP5(I,I,K)=RP(I,I,K)+RTO(K)
TYPE 666,I,I,(RP5(I,I,K),K=1,3),(RTO(K),K=1,3)
666 FORMAT(/,' R(',Il,',',Il,')=',x,3FB.3,' RTO- ',3FB.3)
303 CONTINUE
RETURN
END

C---------------------------------------------------------------------
C SUBROUTINE: INPUT DIN
C ••••••••••••••••••••••• • -:- •••••••••••••••••••••••••••••••••••••••••••
C FUNCTION: THE MAIN SUBROUTINE FOR INPUT DYNAMIC
C PARAMETRS OF THE ROBOT MECHANISM
C---------------------------------------------------------------------
C INPUT VARIABLES:
C NJ - number of joints of the user's specific robot
C 10 - the order number of the joint
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C DYNAMIC PARAMETERS FOR THE USER'S SPECIFIC ROBOT
C---------------------------------------------------------------------
SUBROUTINE INPUT DIN
C
REAL 1(6) ,lz(6) ,mm(6) ,js(6) ,jn(6) ,jx(6) ,jy(6) ,jz(6)
COMMON/tipl/ nx,ksil(6),ksi2(6),ksi3(6)
COMMON/MIN/ MM,JS,JN,JX,JY,JZ
COMMON/SPMOM/ GM(6,3),G(6)
COMMON/ORDGLB/ NUK,NJ
COMMON/FILE/ FILE(5)
BYTE file
*
CHARACTER opcl*l
DIMENSION SIMB(17,6)
COMMON/HMAX/ HMAX(6)
COMMON/HMIN/ HMIN(6)
COMMON/MOD MAX MIN/ QMAX(6),QMIN(6)
CHARACTER*9 FILEP,FILEP2
394

C •••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••
14 format (/,lX,' DIAG*** File:',aIO,' has been found',/)
13 format (/,lX,' DIAG*** File ',aIO,' cannot be found',/)
2 format ('+',a6,':',F15.6)
4573 format (lx,'want to compute maximal moments of inertia of the
> mechanism',/,lx,' (otherwise you have to directly specify
> them) [Y/N]:'$)
4575 format (a2)
4572 format (/,lx,' DIAG*** Computation of maximal moments of
> inertia',/)
4574 format (lx,' Specify the maximal moment of inertia around the'
> ,i2,' joint:',$)
C
C read data from the data-file ***.DNM
C
FILEP=CHAR(FILE(1))//CHAR(FILE(2))//CHAR(FILE(3))//
> CHAR(FILE(4))//CHAR(FILE(5))//'.DNM'
OPEN (55,FILE=FILEP,STATUS='OLD',ERR=II)
INEW=l
WRITE (6,14) FILEP
DO IPOM=l,NJ
DO JPOM=1,7
READ (55,'(lx,FI5.6)') SIMB(JPOM,IPOM)
END DO
END DO
close (55)
if (inew.eq.1) GO TO 12
11 inew=O
WRITE (6,13) FILEP
CLOSE (55)
STOP
C
C check parameter KSI1: if it is not 0 or 1 it is put to 0
C
12 do ipom=l,nj
if (simb(1,ipom).ne.O.and.simb(1,ipom).ne.1) then
simb(l,ipom)=O
end if
end do
C
C substitution of the variables
C
do ipom=l,nj
ksi1(ipom)=simb(1,ipom)
mm(ipom)=simb(2,ipom)
g(ipom) =mm(ipom)*9.81
qmax(ipom)=simb(6,ipom)
qmin(ipom)=simb(7,ipom)
if (ksi1(ipom).eq.O) then
jx(ipom)=simb(3,ipom)
jy(ipom)=simb(4,ipom)
jz(ipom)=simb(5,ipom)
else if (ksi1(ipom).eq.1) then
js(ipom)=simb(3,ipom)
jn(ipom)=simb(4,ipom)
end if
end do
write (6,4573)
read (5,4575) opc1
if (opc1.eq.'Y') then
write (6,4572)
 395

C
C computation of the maximal moments of inertia of the mechanism
C around the joint axes
C
call sm max in

do ipom=l,nj
write (6,'("Max. mom.inert. H",il,il,"max-",f15.6
> )' )ipom,ipom,hmax(ipom)
write (6,'("Min. mom.inert. H",il,il,"min=",f15.6
> )')ipom,ipom,hmin(ipom)
wr i te (6,' ( " ..................... ")')
end do
else
C
C the user wants to specify the maximal moments of inertia
C
do ipom=l,nj
write (6,4574) ipom
read (5,*) hmax(ipom)
end do
end if
RETURN
END

C*********************************************************************
C SUBROUTINE: SM MAX IN
C----------------------=---=------------------------------------------
C FUNCTION: COMPUTES THE MAXIMAL AND THE MINIMAL VALUES OF
C THE MOMENTS OF INERTIA OF THE MECHANISM AROUND
C THE ROBOT JOINTS FOR ALL ALLOWABLE VALUES OF
C THE JOINTS ANGLES (LINEAR DISPLACEMENTS) - THE
C SUBROUTINE CALLS SUBROUTINE MODEL TO COMPUTE
C THE MOMENTS OF INERTIA (inerta matrix HH) FOR
C VARIOUS JOINTS ANGLES AND SEARCHES FOR THE MA-
C XIMAL AND MINIMAL VALUES OF MOMENTS OF INERTIA
C AROUND THE ROBOT JOINTS (diagonal elements in
C the inertia matrix HH(i,i))
C--------------------------------------------------------------------
C INPUT VARIABLES:
C NS - number of joints of the user's specific robot
C QMI(6) - the vector of the minimal allowable values of the
C joints angles (displacements)
C QMA(6) - the vector of the maximal allowable values of the
C joints angles (displacements)
C HH(6,6) - inertia matrix of the robot for given joints angles
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C HMAX(6) - vector of the computed maximal values of the
C moments of inertia of the mechanism around the
C robot joints
C HMIN(6) - vector of the computed minimal values of the
C moments of inertia of the mechanism around the
C robot joints
C---------------------------------------------------------------------
C SUBROUTINE REQUIRED:
C MODEL - computates the inertia matrix for the given
C values of the joints angles (computes HH)
C---------------------------------------------------------------------
396

SUBROUTINE SM MAX IN
C
byte file (5)
common/file/ file
INCLUDE 'MODELM.MOD'
COMMON /HMAX/ HMAX(6)
COMMON /HMIN/ HMIN(6)
COMMON /MOD MAX MIN/ QMA(6),QMI(6)
DIMENSION DELTA(6)
COMMON/ORDGLB/ NUK,NS
C
C initialization
C
DO I=1,NS
HMAX(I)=-1.
HMIN(I)=1.E+06
SI(I)=QMI(I)
C
C determination of increment of the joint angles at which
C the moments of inertia of the mehanism are computed
C
DELTA(I)=(QMA(I)-QMI(I»/3.
END DO
C
C branching according to the number of joints of the robot
C
GO TO (1,2,3,4,5,6) NS
C
C if the robot has one joint only
C
1 CALL MODEL
DO I=1,NS
IF (HH(I,I).GT.HMAX(I» HMAX(I)=HH(I,I)
IF (HH(I,I).LT.HMIN(I» HMIN(I)=HH(I,I)
END DO
RETURN
C
C if the robot has two joints
C
2 SI(2)=QMI(2)
DO J2=1,3
CALL MODEL
DO I=l,NS
IF (HH(I,I).GT.HMAX(I» HMAX(I)=HH(I,I)
IF (HH(I,I).LT.HMIN(I» HMIN(I)=HH(I,I)
END DO
SI(2)=SI(2)+DELTA(2)
END DO
RETURN
C
C if the robot has three joints
C
3 SI(2)=QMI(2)
DO J2=1,3
SI(3)=QMI(3)
DO J3=1,3
CALL MODEL
DO I=1,NS
IF (HH(I,I).GT.HMAX(I» HMAX(I)=HH(I,I)
IF (HH(I,I).LT.HMIN(I» HMIN(I)=HH(I,I)
END DO
SI(3)=SI(3)+DELTA(3)
 397

END DO
SI(2)=SI(2)+DELTA(2)
END DO
RETURN
C
C if the robot has four joints
C
4 SI(2)=QMI(2)
DO J2=1,3
SI(3)=QMI(3)
DO J3=1,3
SI(4)=QMI(4)
DO J4=1,3
CALL MODEL
DO I=l,NS
IF (HH(I,I).GT.HMAX(I)) HMAX(I)=HH(I,I)
IF (HH(I,I).LT.HMIN(I)) HMIN(I)=HH(I,I)
END DO
SI(4)=SI(4)+DELTA(4)
END DO
SI(3)=SI(3)+DELTA(3)
END DO
SI(2)=SI(2)+DELTA(2)
END DO
RETURN
C
C if the robot has five joints
C
5 SI(2)=QMI(2)
DO J2=1,3
SI(3)=QMI(3)
DO J3=1,3
SI(4)=QMI(4)
DO J4=1,3
SI(5)=QMI(5)
DO JS=1,3
CALL MODEL
DO I=l,NS
IF (HH(I,I).GT.HMAX(I)) HMAX(I)=HH(I,I)
IF (HH(I,I).LT.HMIN(I)) HMIN(I)=HH(I,I)
END DO
SI(S)=SI(S)+DELTA(S)
END DO
SI(4)=SI(4)+DELTA(4)
END DO
SI(3)=SI(3)+DELTA(3)
END DO
SI(2)=SI(2)+DELTA(2)
END DO
RETURN
C
C if the robot has six joints
C
6 SI(2)=QMI(2)
DO J2=1,3
SI(3)=QMI(3)
DO J3=1,3
SI(4)=QMI(4)
DO J4=1,3
SI(S)=QMI(S)
DO J5=1,3
398

SI(6)-QMI(6)
DO J6=l,3
CALL MODEL
DO I=l,NS
IF (HH(I,I).GT.HMAX(I)) HMAX(I)-HH(I,I)
IF (HH(I,I).LT.HMIN(I)) HMIN(I)-HH(I,I)
END DO
SI(6)-SI(6)+DELTA(6)
END DO
SI(S)=SI(S)+DELTA(S)
END DO
SI(4)=SI(4)+DELTA(4)
END DO
SI(3)=SI(3)+DELTA(3)
END DO
SI(2)=SI(2)+DELTA(2)
END DO
RETURN
end

C---------------------------------------------------------------------
C SUBROTINE: INPUT ACT
C------------------------=--------------------------------------------
C FUNCTION: COMPUTES THE MODELS OF THE SUBSYSTEMS -
C ACTUATOR MODELS INCLUDING "MAXIMAL" MOMENT
C OF INERTIA OF THE MECHANISM AROUND THE JOINT
C AXIS
C---------------------------------------------------------------------
C INPUT VARIABLES:
C ACTUATOR PARAMETERS WHICH ARE READ IN THE
C SUBROUTINE INPUL
C HMAX(6) - maximal moments of inertia of the mechanism
C around the joints axes (computed in SM MAX IN)
C OPCl - user's option: to print the computed matrices-or not
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C THE MATRICES OF THE ACTUATOR'S STATE MODEL
C dx/dt = A * x + b * u + f * P
C (here: x is the state vector of the i-th actuator model
C u is the input, P is the load - driving torque)
C NS - number of joints of the user's specific robot
C NI(6)- vector contaning the selected orders of the actuator
C models
C A(3,3,6) - three dimensional matrix which includes A matrices
C of the actuators models for all NS actuators
C B(3,6) - two dimensional matrix including control distribution
C b vectors of the actuators models
C F(3,6) - two dimensional matrix including load distribution
C f vectors of the actuators models
C a$(6) - character vector including information on actuators
C types
C AHM,BHM,FHM - matrices analogue to A, B, F but which
C include the maximal moments of inertia of
C the mechanism HMAX - matrices of the subsystems'
C state models
 399

C---------------------------------------------------------------------
C SUBROUTINE REQUIRED:
C INPUTL - reads data on the selected actuators for the robot
C and computes the matrices A,B,F of actuators'models
C---------------------------------------------------------------------
SUBROUTINE INPUT ACT
C
COMMON/SUBSYS/ A(3,3,6),B(3,6),F(3,6)
COMMON/SUBSHM/ AHM(3,3,6),BHM(3,6),FHM(3,6)
COMMON/ACTORD/ NI(6),KI(6)
COMMON/ORDGLB/ NUK,NS
C
COMMON/TIP ACT/ A$(6)
COMMON/HMAX/ HMAX(6)
DIMENSION RMEL(6)
CHARACTER*2 A$,OPC(15),B$,OPC1*1
C ••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••
2 FORMAT (A1)
3 FORMAT (lX,F16.5)
30 FORMAT (lX,'--------------------------------------------------

1453
*--------------')
FORMAT (lX,A19)
1256 FORMAT (/,lX,A11,I2)
1387 FORMAT (lX,'Do you want to print the matrices of subsystems
* models',/,' (which include the maximal mom. of inertia)
* [ YIN 1: ' , $ )
C---------------------------------------------------------------------
C
C call subroutine to read data on actuators and computes
C the matrices A,B,F of the actuators' models
C

CALL INPUTL
* ........................................................................................................................................ .
WRITE (6,1387)
READ (5,2) OPC1
DO I=l,NS
OPC(l)~A$(I)
RMEL(I)-F(2,I)
C--------------------------------------------------------------------.
C
C computation of the matrices AHM, BHM, FHM
C
C~-l./RMEL(i)/(-l./RMEL(i)+Hmax(I)
FHM(2,i)=F(2,i)*C
AHM(1,2,I)=1.
IF (NI(i).EQ.2) THEN
AHM(2,2,i)=A(2,2,i)*C
BHM(2,i)=B(2,i)*C
RMEL(i)=RMEL(i)*C
ELSE
AHM(2,2,i)=A(2,2,i)*C
AHM(2,3,i)=A(2,3,i)*C
* AHM(3,2,I)=A(3,2,I)
AHM(3,3,I)=A(3,3,I)
BHM(3,I)aB(3,I)
RMEL(i)-RMEL(i)*C
END IF
400

C-------------------------------------------------------------------
C
C print matrices
C
IF (OPC1.EQ.'Y') THEN
WRITE (6,1256) , Subsystem:',I
WRITE (6,1453) 'Subsystem matrix A:'
WRITE (6,'«NI(I»F16.5)') ((AHM(J,K,I),
> K=1 , NI ( I ) ) , J=1 , NI ( I ) )
WRITE (6,1453) 'Subsystem vector b:'
WRITE (6,3) (BHM(J,I),J=1,NI(I))
WRITE (6,1453) 'Subsystem vector f:'
WRITE (6,3) (FHM(J,I),J=1,NI(I))
WRITE (6,30)
END IF
END DO
END

C---------------------------------------------------------------------
C SUBROUTINE: INPUTL
C---------------------------------------------------------------------
C FUNCTION: READS DATA ON THE ACTUATORS PARAMETERS SPECI-
C FlED BY THE USER IN THE FILE *** .ACT AND
C COMPUTES THE ACTUATORS MODELS IN THE STATE
C SPACE
C---------------------------------------------------------------------
C INPUT VARIABLES:
C PARAMETRS OF THE ACTUATORS SPECIFIED IN THE FILE
C ***.ACT
C NS - number of joints of the user's specific robot
C A$ - charactet vector including information on the
C actuators types (DC - D.C. electro-motor, HD -
C hydraulic actuator)
C NI(6) - vector including ordrers of the actuators
C models as selected by the user
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C A,B,F - matrices of the actuators' models in the
C state space - see explanation in the subroutine
C INPUT ACT
C---------------------------=-----------------------------------------
C NOTE: Due to lack of space in this subroutine all necessary
C tests of the input data are omitted. The user is
C adviced to add himself this protective tests in
C order to protect his package from invalid values of
C input parameters
C---------------------------------------------------------------------
SUBROUTINE INPUTL
C
COMMON/SUBSYS/ A(3,3,6),B(3,6),F(3,6)
COMMON/SUBSHM/ AHM(3,3,6),BHM(3,6),FHM(3,6)
COMMON/ACTORD/ NI(6),KI(6)
COMMON/ORDGLB/ NUK,NS
BYTE FILE
COMMON/FILE/FILE(5)
c
CHARACTER*2 A$
CHARACTER*4 DC$(15),HD$(15)
CHARACTER*1 POM,PP,A1*80
 401

DIMENSION DC(15,6),HD(15,6)
COMMON/TIP ACT/ A$(6)
COMMON/UMAX/ UMAX(2,6),UMAXl(6)
CHARACTER*9 FILEP,FILEP2
DATA A$/'DC','DC','DC','DC','DC','DC'/
C---------------------------------------------------------------------
14 FORMAT (/,lX,' WARNING*** File:',alO,' is found',/)
13 FORMAT (/,lX,' DIAG*** File ',alO,' cannot be found')
21 format (' DIAG*** The specified order of the actuator ',iI' is
> not allowed')
C---------------------------------------------------------------------
C
C read data from the file ***.ACT
C
FILEP=CHAR(FILE(1»//CHAR(FILE(2»//CHAR(FILE(3»//
> CHAR(FILE(4»//CHAR(FILE(5»//'.ACT'
INEW=O
OPEN (55,FILE=FILEP,STATUS='OLD',ERR=11)
INEW=l
WRITE (6,14) FILEP
READ (55,'(6(lX,A2»') (A$(IPOM),IPOM=l,NS)
DO IPOM=l,NS
IF (A$(IPOM).EQ.'DC') THEN
DO JPOM=1,15
READ (55,'(lX,F15.6)') DC(JPOM,IPOM)
END DO
ELSE IF (A$(IPOM).EQ.'HD') THEN
DO JPOM=1,15
READ (55,'(lX,F15.6)') HD(JPOM,IPOM)
END DO
END IF
END DO
CLOSE (55)
IF (INEW.EQ.1) GO TO 12
11 INEW=O
WRITE (6,13) FILEP
RETURN
C.
C
C if the secected actuator is D.C. electro-motor (DC)
C
12 do i22=1,ns
i f (a$(i22).eq.'DC') then
umax(1,i22)=dc(7,i22)
umax(2,i22)=dc(8,i22)
n=dc(1,i22)
ni(i22)=dc(1,i22)
a(1,1,i22)=O.
a(1,2,i22)=1.
a(2,1,i22)=O.
a(1,3,i22)=O.
b(1,i22)=O.
f(1,i22)=O.
f(3,i22)=O.
f(2,i22)=-1./dc(9,i22)/dc(lO,i22)/dc(4,i22)
if(n.lt.2.or.n.gt.3) then
write (6,21) i22
stop
402

else if (n.eq.2) then

a(2,2,i22)=-(dc(2,i22)*dc(3,i22)/dc(6,i22)+
> dc(5,i22))/dc(4,i22)/DC(9,122)/DC(10,122)
b(2,i22)=dc(2,i22)/dc(4,i22)/dc(9,i22)/
> dc(6,i22)/DC(9,122)
a(2,3,i22)=0.
else if (n.eq.3) then
a(2,2,i22)=-dc(5,i22)/dc(4,i22)/DC(9,122)/
> DC(10,122)
a(2,3,i22)=dc(2,i22)/dc(9,i22)/dc(4,i22)/
> DC(10,122)
a(3,1,i22)=0.
a(3,2,i22)=-dc(3,i22)*dc(9,i22)/dc(11,i22)/
> DC(9,122)
a(3,3,i22)=-dc(6,i22)/dc(11,i22)
b(2,i22)=0.
b(3,i22)=1./dc(11,i22)
end if
C.
C
C if selected actuator is hydraulic (HD)
C
else if (a$(i22).eq.'HD') then
umax(1,i22)=hd(7,i22)
umax(2,i22)=hd(8,i22)
n=hd(1,i22)
ni(i22)=hd(1,i22)
a(1,1,i22)=0.
a(1,2,i22)=1.
a(2,1,i22)=0.
a(1,3,i22)=0.
b(1,i22)=0.
f(1,i22)=0.
f(3,i22)=0.
f(2,i22)=-1./hd(4,i22)
if(n.lt.2.or.n.gt.3) then
write (6,21) i22
stop
else if (n.eq.2) then
a(2,2,i22)=-hd(5,i22)/hd(4,i22)-.01*hd(2,i22)*
> hd(2,i22)/(hd(4,i22)*hd(3,i22))
b(2,i22)=100.*hd(10,i22)*hd(2,i22)/(hd(4,i22)*
> hd(3,i22))
else if (n.eq.3) then
pomp=400.*hd(9,i22)/hd(6,i22)
a(2,2,i22)=-hd(5,i22)/hd(4,i22)
a(2,3,i22)=.OOOl*hd(2,i22)*hd(11,i22)/hd(4,i22)
a(3,1,i22)=0.
a(3,2,i22)=-hd(2,i22)*pomp/hd(11,i22)
a(3,3,i22)=-hd(3,i22)*pomp*.Ol
b(2,i22)=0.
b(3,i22)=pomp*hd(lO,i22)*10000./hd(11,i22)
f(3,i22)=0.
end if
end if
end do
C •••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••••
C
C print actuators models
 403

do iOzz=1,ns
write (G,'(/," Actuator model in the joint:" ,12)' )IOZZ
write (G,*) , Matrix A:'
IO=IOZZ
write (G,'(1x,<ni(io»f15.G)') «a(jp,jl,io),jl=1,ni(io)
* ),jp=1,ni(io))
write (G,*) , Vector b:'
write (G,'(lx,f15.G)') (b(jp,io),jp=1,ni(io))
write (G,*) , Vector f:'
write (G,'(lx,f15.G),//') (f(jp,io),jp=l,ni(io))
end do
RETURN
END

C---------------------------------------------------------------------
C SUBROUTINE: SMOPRG
C---------------------------------------------------------------------
C FUNCTION: COMPUTES LOCAL OPTIMAL REGULATOR FOR THE
C SUBSYSTEMS WHICH MIGHT BE EITHER OF THE
C SECOND OR OF THE THIRD ORDER, WITH OR
C WITHOUT INTEGRAL FEEDBACK LOOP
C---------------------------------------------------------------------
C INPUT VARIABLES:
C I - The order number of the local subsystem for
C which we want to synthesize the local controller
C NI(I) - the order of the i-th subsystem
C ASHM(NI(I),NI(I),I) - matrix of the i-th subsystem
C (which includes the value of
C the maximal moment of inertia
C of the mechanism around the
C axis of the i-th joint)
C BSHM(NI(I),I) - vector of the input distribution
C of the i-th subsystem
C ALFAI(I) - the presribed exponential stability
C degree for the i-th subsystem
C IOPTOR(I) - user's option:
C =0 the user does not want optimal
C regulator around the i-th joint
C =1 the user wants optimal regulator
C IOPTV(I) =0 without the velocity feedback
C =1 with the velocity feedback
C IOPTI(I) =0 without the feedback loop by the
C rotor current
C (if D.C. motor is applied)
C (or, by the pressure if hydraulic
C actuator is applied)
C =1 with the feedback loop by current
C IOPTIN(I) - option of introduction of the integral
C feedback loop:
C =0 no
C =1 yes
C QQ(NI(I),NI(I),I) - weighting matrix by the subsystem state
C in the standard quadratic criterion for
C the i-th subsystem
C Rl(I) weighting element by the control Signal in
C the local standard quadratic criterion
C QI(I) - weighting element by the integral coordi-
404

C nate (integral of the position error in

C the PID regulator) in the local standard
C quadratic criterion for the i-th subsys.
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C PKI(I) - position feedback gain for the i-th subsystem
C PK2(I) - velocity feedback gain
C PK3(I) - feedback gain by current
C PK4(I) - integral feedback gain
C---------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C SMOREG - computes gains of the local linear optimal
C regulator (same holds for SMOREl and
C SMORE2 but for different subsystem orders
C different matrices dimensions; these two
C subroutines might be ommitted)
C---------------------------------------------------------------------
SUBROUTINE SMOPRG(i)
C
INTEGER*2 I
C
COMMON/GAINS/ PKl(6),PK2(6),PK3(6),PKG(6),PK4(6)
COMMON/SUBSHM/ ASHM(3,3,6),BSHM(3,6)
COMMON/ACTORD/ NI(6),KI(6)
COMMON/OPTION/ IOPTOR(6),IOPTV(6),
1 IOPTI(6),IOPTG(6),IPOM(l8),IOPTIN(6)
COMMON/SUBCRT/ QQ(3,3,6),Rl(6),QI(6)
COMMON/STDGIM/ ALFAI(6)
C
DIMENSION QS(3,3),AS(3,3),BS(3,l),PK(3)
DIMENSION QSS(4,4),ASS(4,4),BSS(4,l),PKS(4)
DIMENSION QS2(2,2),AS2(2,2),BS2(2,l),PKS2(2)
C---------------------------------------------------------------
C
C preparation of the auxiliary matrices ASS,AS, BSS,BS,
C AS2,BS2,QS,QSS,QS2 - for the various orders of the
C subsystems
C
DO 1 Il=l,NI(I)
BSS(Il,l)=BSHM(Il,I)
BS(Il,l)=BSHM(Il,I)
C
DO 1 J=l,NI(I)
ASS(Il,J)=ASHM(Il,J,I)
1 AS(Il,J)=ASHM(Il,J,I)
C
NS=NI(I)
C
C option: is the integral feedback loop introduced or
C not- if yes the subsystem order is increased by 1
C
IF(IOPTIN(I).EQ.l)NS=NS+l
IF(IOPTIN(I).EQ.O)GO TO 3
C
DO 2 J=l,NI(I)
QSS(NS,J)=O.
QSS(J,NS)=O.
ASS(NS,J)=O.
ASS(J,NS)=O.
2 CONTINUE
 405

C
BSS(NS,l)=O.
ASS(NS,l)=l.
QSS(NS,NS)=QI(I)
GO TO 50
C
C preparation of auxiliary matrices if the subsystem order is 2
C
3 IF(NI(I).NE.2)GO TO 50
C
DO 51 Il=l ,NI (I)
BS2(Il,1)=BSHM(Il,I)
DO 51 J=l,NI(I)
51 AS2(Il,J)=ASHM(Il,J,I)
50 CONTINUE
C
C preparation of auxiliary matrix QSS and the prescribed expo-
C nential stability degree
C
DO 21 Il=1,NI (I)
DO 21 J=1,NI(I)
QSS(I1,J)=QQ(I1,J,I)
21 QS(I1,J)=QQ(I1,J,I)
RS=R1(I)
ALFA=ALFAI(I)
C
IF(IOPTIN(I).EQ.1)GO TO 4
IF(NS.EQ.2)GO TO 4453
C
C computation of optimal regulator if the subsystem
C order is 3 and integral feedback loop is not introduced
C
CALL SMOREG(AS,BS,QS,RS,ALFA,NS,PK,I)
GO TO 5599
C
C preparation of the auxiliary matrix QS2
C
4453 DO 54 Il=1,NI(I)
DO 54 J=1,NI(I)
54 QS2(I1,J)=QQ(I1,J,I)
C
C computation of optimal regulator if the subsystem order
C is 2 and integral feedback loop is not introduced
C
CALL SMORE2(AS2,BS2,QS2,RS,ALFA,NS,PKS2,I)
C
DO 55 J=1,NI(I)
55 PKS(J)=PKS2(J)
PKS(3)=0.
PKS(4)=0.
GO TO 5
C
5599 DO 10 J=1,NI(I)
10 PKS(J)=PK(J)
PKS(4)=0.
GO TO 5
C
C regulators with the introduced integral feedback loops
C
4 IF(NS.EQ.4)GO TO 56
406

C
C if the subsystem order is 2 prepare the auxiliary matrices
C
DO 57 J=I,NI(I)
AS(J,NS)=O.
57 AS(NS,J)=O.
BS(NS,l)=O.
AS(NS,l)=l.
QS(NS,NS)=QI(I)
C
C computation of optimal regulator if the subsystem order
C is 2 and integral feedaback loop is introduced
C
CALL SMOREG(AS,BS,QS,RS,ALFA,NS,PK,I)
DO 58 J=l,NS
58 PKS(J)=PK(J)
PKS(3)=0.
PKS(4)=PK(3)
GO TO 5
C
C computation of optimal regulator if the susbsystem order is
C 3 and integral feedback loop is introduced
C
56 CALL SMOREl(ASS,BSS,QSS,RS,ALFA,NS,PKS,I)
C---------------------------------------------------------------------
C
C print the computed feedback gains
C
5 PKl(I)=PKS(l)
PK2(I)=PKS(2)*IOPTV(I)
PK3(I)=PKS(3)*IOPTI(I)
PK4(I)=PKS(4)*IOPTIN(I)
c
WRITE(5,2000)I
2000 FORMAT(3X,'Local servo feedback gains in the ',I3,'-th joint',
1 I)
WRITE(5,200l)PKl(I),PK2(I),PK3(I),PK4(I)
2001 FORMAT(3X,'Position feedback gain',G12.5,1,
1 3X,'Velocity feedack gain',G12.5,1,
2 3X,'Gain in feedback loop by current/pressure',G12.5,1,
3 3X,'Integral feedback gain',G12.5,/)
C
IF(IOPTOR(I).EQ.l)WRITE(5,2002)ALFA
2002 FORMAT(3X,'Exponential stability degree of local subsystem'
1 ,GlO.5,/)
RETURN
END
 407

C--------------------------------------------------------------------
C SUBROUTINE: SMOREG
C--------------------------------------------------------------------
C FUNCTION: COMPUTATION OF THE LINEAR OPTIMAL REGULATOR
C FOR THE GIVEN LINEAR SYSTEM
C (THIS IS WELL-KNOWN PROCEDURE FOR SOLVING OF
C THE ALGEBRAIC EQUATION OF THE RICCATI TYPE -
C see ref. e.g. Vukobratovic M,
C Stokic D., Control of Manipulation
C Robots:Theory and Application, springer-
C -Verlag,1982.
C---------------------------------------------------------------------
C INPUT VARIABLES:
C KK - The order number of the subsystem for which the
C local optimal regulator is synthesized
C N - the order of the subsystem
C A(N,N) - the subsystem matrix
C B(N) - the input distribution matrix of the
C subsystem
C Q(N,N) weighting matrix by the state vector in the
C local standard quadratic criterion
C R - the weighting matrix by the control signal
C ALFA - the prescribed exponential stability degree
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C PKK(N) - optimal regulator feedback gains
C RK(4,4,KK) - the matrix - solution of the algebraic
C equation of Riccati type for the i-th
C subsystem
C---------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C EIGENP - computes the eigen-values of the given
C quadratic matrix (SSP-subroutine)
C GMPRD - matrix multiplication (SSP-subroutines)
C MINV - matrix inversion (SSP-subroutines)
C SMEIG3 - auxiliary subroutine for computation of the
C eigen-values of the given quadratic matrix
C--------------------------------------------------------------------
SUBROUTINE SMOREG(A,B,Q,R,ALFA,N,PKK,KK)
C
COMMON/SUBLIA/ RK(4,4,6)
C
DOUBLE PRECISION AA,EVRA,EVRIA,VECRA,VECIA
DIMENSION A(N,N),B(N,1),BT(1,3),Q(N,N),PKK(1,N)
DIMENSION C(1,3),INDIC(6),CPOM(3,3),POM(6,3),LL(3),MM(3)
DIMENSION PP(3,3),PPP(3,3),P(3,3)
DIMENSION REALEV(3)
DIMENSION AA(6,6),EVRA(6),EVRIA(6),VECRA(6,6),VECIA(6,6)
DOUBLE PRECISION DA(3,3)
C-----------------------------------------------------------------
C
C The order of the subsystem input is equal to 1 (the input
C is scalar) - in general case the input order is M
C
M=l
C
C transpose the matrix B and compute B*(R**-l)*BT
C
DO I I=l,N
408

DO 1 J=1,M
BT(J,1)=B(1,J)
1 C(J,1)=BT(J,1)/R
C
CALL GMPRD(B,C,CPOM,N,M,N)
C
C form expanded matrix of the susbsystem
C
DO 2 1=1,N
DO 2 J=1,N
AA( I, J) =A( I, J)
DA(1,J)=A(1,J)
AA(I+N,J)=-Q(I,J)
AA(I,J+N)=-CPOM(I,J)
AA(I+N,J+N)=-A(J,I)
2 CONTINUE
C
C eigen-va1ues of the open-loop subsystem matrix
C
TYPE 500,KK
500 FORMAT(/,3X,' Eigen-values of the open-loop matrix of the' ,I,
1 3X,1l,'.th subsystem (actuator+joint)')
CALL SMEIG3(N,N,DA,REALEV)
C
DO 3 I=1,N
II=1+N
AA(II,II)=AA(II,II)-ALFA
3 AA(I,I)=AA(I,I)+ALFA
C
C computation of the eigen-values and the eigen-vectors of the
C expanded matrix of the subsystem
C
NN=N+N
C
CALL E1GENP(NN,NN,AA,EVRA,EVRIA,VECRA,VECIA,IND1C)

C
C form of the solution of the algebraic equation of Riccati type
C according to the procedure presented in above reference
C
M4=0
1=1
C
19 IF(I.GT.NN)GO TO 20
IF(EVRA(I).GT.O)GO TO 7
M4=M4+1
C
IF(EVRIA(I).NE.O)GO TO 17
DO 5 J=l,NN
POM(J,M4)=VECRA(J,I)
5 CONTINUE
GO TO 7
C
17 1=I+1
DO 16 J=1,NN
POM(J,M4)=VECRA(J,I)
16 POM(J,M4+1)=VECIA(J,I)
C
M4=M4+1
7 CONTINUE
 409

C
1=1+1
GO TO 19
20 CONTINUE
C
DO 21 I=l,N
DO 21 J=l,N
PP(I,J)=POM(I,J)
21 P(I,J)=POM(I+N,J)
C
C inverse of the matrix PP
C
CALL MINV(PP,N,DET,LL,MM)
C
C calculate (PP**-l)*P
C
CALL GMPRD(P,PP,PPP,N,N,N)
C
C memorize the solution PPP of the Riccati's algebraic equation
C in the matrix RK - necessary for computation of the subsystem
C Liapunov's function which is required for computation of the
C gain of global control (see subroutines LIAP1S and GRAD1S)
C
DO 657 I=l,N
DO 657 J=l,N
657 RK(I,J,KK)=PPP(I,J)
C
C computation of the feedback gains of the optimal regulator
C
CALL GMPRD(C,PPP,PKK,M,N,N)
C
WRITE(3,1008)
1008 FORMAT(/,3X,' Feedback gains of the optimal regulator' ,/)
WRITE(3,200)«PKK(I,J),J=1,N),I=1,M)
200 FORMAT(3X,3E15.5)
C
C computation of the closed-loop subsystem matrix
C
CALL GMPRD(CPOM,PPP,P,N,N,N)
C
DO 12 I=1,N
DO 12 J=1,N
12 DA(I,J)=A(I,J)-P(I,J)
C
C computation of the eigen-values of the closed-loop
C subsystem matrix
C
WRITE(5,1009)
1009 FORMAT(/,3X,'Eigen-values of the closed-loop subsystem
1 matrix' ,/)
C
CALL SMEIG3(N,N,DA,REALEV)
RETURN
END
410

C---------------------------------------------------------------------
C SUBROUTINE: SMEIG3
C---------------------------------------------------------------------
C FUNCTION: AUXILIARY SUBROUTINE TO COMPUTE EIGEN-
C -VALUES OF THE GIVEN QUADRATIC MATRIX
C---------------------------------------------------------------------
C INPUT VARIABLES:
C NN - Matrix order
C n
C DA(NN,NN) - Quadratic matrix
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C REALEV(NN) - Vector of the real parts of the eigen-
C values
C--------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C EIGENP - SSP-subroutine to compute the eigen-values
C of the given quadratic matrix
C REALVE,COMPVE,SCALE,HESQR - Auxiliary subroutines
C called by EIGENP
C----------------------------------------------------- ---------------
SUBROUTINE SMEIG3(NN,NM,DA,REALEV)
C
DOUBLE PRECISION DA(NN,NN),EVR(3),EVI(3),VECR(3,3)
DOUBLE PRECISION VECI(3,3)
DIMENSION REALEV(NN)
DIMENSION INDIC(3)
c-------------------------------------------------------------------
C
C call subroutine EIGENP to compute the eigen-values of
C the input matrix DA
C
CALL EIGENP(NN,NM,DA,EVR,EVI,VECR,VECI,INDIC)
C
DO 10 I=1,NN
10 REALEV(I)=EVR(I)
C
WRITE(5,102)
DO 6 I=1,NN
6 WR1TE(5,103)I,EVR(I),EVI(1),IND1C(I)
102 FORMAT(//,3X,'Eigen-values ',/,3X,'No. Real part
1 1 m . part indic',/)
103 FORMAT(3X,I4,2E15.5,I4)
RETURN
END
 411

C---------------------------------------------------------------------
C SUBROUTINE: SMLOC E
C------------------------------=--------------------------------------
C FUNCTION: COMPUTES LOCAL FEEDBACK GAINS FOR THE ISO-
C LATED SUBSYSTEMS (ACTUATOR + JOINT ASSUMING
C THAT ALL THE OTHER JOINTS ARE KEPT LOCKED);
C CONTROL IS SYNTHESIZED TO KEEP THE DAMPING
C RATIO CLOSE TO 1 AND TO KEEP CHARACTERISTIC
C FREQUENCY OF THE SERVO (NATURAL UNDAMPED
C FREQUENCY) BELLOW ONE HALF OF THE RESONANT
C STRUCTURAL FREQUENCY OF THE MECHANISM
C (SPECIFIED BY THE USER)
C---------------------------------------------------------------------
C INPUT VARIABLES:
C I - the order number of the subsystem for which the servo
C gains are to be synthesized
C NI(i) - the order of the i-th subsystem
C AHM, BHM - matrices of the subsystem's model (see subrou-
C tine INPUT ACT)
C the user's options: -
C uO - resonant structural frequency of the mechanism around
C the i-th joint [rad/s]
C OPC - the user wants feedback loop by current/press. (Y/N)
C ZETA - damping factor of the non-dominant pair of poles
C OMEGA N - natural undapmed frequency of the non-dominant
C - pair of poles - for the third order subsystem
C only
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C PK1(i} - position feedback gain [v/rad],[v/m],[mA/rad],
C [mAIm]
C PK2(i} - velocity feedback gain [v/rad/s] etc.
C PK3(i} - gain in feedback loop by current/pressure [VIA]
C---------------------------------------------------------------------
SUBROUTINE SMLOC E
C
COMMON/SUBSYS/ A(3,3,6),B(3,6),F(3,6)
COMMON/SUBSHM/ AHM(3,3,6),BHM(3,6},FHM(3,6}
COMMON/ACTORD/ NI(6),KI(6)
COMMON/GAINS/ PK1(6},PK2(6),PK3(6},PK4(6),PK5(6},PK6(6}
BYTE FILE
COMMON/FILE/FILE(5)
CHARACTER*9 FILEP
COMMON/HMAX/ HMAX(6)
COMMON/RBSL/ I
COMMON/TIP ACT/ .a.$ (6)
CHARACTER*I OPC
CHARACTER*30 1$pom*60
* .................................................................... .
2 FORMAT (A1)
3 FORMAT (lX,F16.5)
4 FORMAT (lX,A27,I1,A2,E12.5)
6 FORMAT (' Want to introduce feedback loop by current/pressure
> [Y/N]?:',$)
7 FORMAT (lX,'Specify the resonant structural frequency',/,' of
> the mechanism [rad/s]')
10 FORMAT (lX,F12.5,35X,A30,I1)
25 FORMAT (lX,'W(S)=',F12.5,'/(S**2+',F12.5,'*S)',/)
26 FORMAT (lX,'W(S)=',F18.5,'/(S**3+',F18.5,'*S**2+',F18.5,
> ' *S )' ,I)
412

30 FORMAT (lX,'----------------------------------------------- __ _
>-----------------',/)
35 FORMAT (lX,'G(S)=' ,F12.5,'/(S**2+',F12.5,'*S+',F12.5,')',/)
36 FORMAT (lX,'G(S)=',F18.5,'/(','S**3+',F18.5,'*S**2+',
> F18.5,'*S+',F18.5,')',/)
* ...................................................................................................................................
WRITE (6,7)
WRITE (6,37) I
37 FORMAT (lX,'for the servo in ',i2,' -th joint [rad/s1:' ,$)
READ (5,*) uO uO
c
C the specified frequency is modified to determine the gains for
e the case of two equal dominant poles of the closed-loop sub-
e system;this modification does not affect the computed feedback
C gains but the algorithms for their computation requires it;
C the frequency is not modified if there is just one dominant
C pole
C
U02=UO
UO=UO*2.5
C
IF (NI(I).EQ.3) THEN
WRITE (6,6)
READ (5,2) ope opc - feedback loop by
END IF current
WRITE (6,30)
e
if(ni(i).eq.2)then
ifl2=l
ierrind=l
go to 100
end if
e
C present the containts of the auxiliary file conf.dat
C in order to enable the user to select the poles of the closed-
e -loop subsystem (in the case the order of the subsystem is 3)
C
open (23,file='conf',status='old')
do ipom=l,lO
read (23,'(a60)') l$pom
write (6,'(lx,a60)') l$pom
end do
close(23)
1 write (6,'(lx," If you want to calculate feedback gains" ,/
> that the pole placement be as in Fig.l, then" ,/
> " specify 1, otherwise the gains will be computed",/
> " to place the poles as presented in Fig.2.:",$)')
ierrind=l
read (5,'(il)',err=1500) ifl2
c---------------------------------------------------------------------
C
C computation of the feedback gains
C
100 IF (ifl2.eq.l) THEN
C
C the pair of dominant poles is complex (or real and equal) and
C non-dominant pole is real (Fig.l.)
C
 413

dpot-uO/lO.
pot=2.
IF (ni(i).eq.2) THEN
pkl(i)=(uO/5 .)**2/bhm (2,i)
pk2(i)=(2.* uO /5.+ahm(2,2,i))/bhm(2,i)
ELSE
IF (opc.EQ.'N') THEN
alf=-ahm(2,2,I)-ahm(3,3,I)
pk3(I)=0
bet=alf*2.*uO/5.-3.*(uO/5.)**2
gam=alf*(u O/5.)**2-2.*( uO /5.)**3
pkl(I)=gam/ahm(2,3,I)/bhm(3,I)
pk2(I)=(bet-ahm(2,2,I)*ahm(3,3,I)+ahm(2,3,I)
*ahm(3,2,I))/ahm(2,3,I)/bhm(3,I)
>
ELSE
alf=(pot+1.)*uO
pk3(I)=(alf+ahm(2,2,I)+ahm(3,3,I))/bhm(3,I)
bet=alf*2.*uO/5.-3.*(uO/5.)**2
gam=alf*( uO/5.)**2-2.*(uO/5.)**3
pkl(I)=gam/ahm(2,3,I)/bhm(3,I)
pk2(I)=(bet-ahm(2,2,I)*ahm(3,3,I)+ahm(2,3,I)
> *ahm(3,2,I)+ahm(2,2,I)*pk3(I))/ahm(2,3,I)
> Ibhm(3,I)
DO WHILE (pk2(i).LT.0.OR.pk3(i).LT.0)
pot=pot+dpot
alf=(pot+1.)*uO
pk3(I)=(alf+ahm(2,2,I)+ahm(3,3,I))/bhm(3,I)
bet=alf*2.*uO/5.-3.*(uO/5.)**2
gam=alf*(uO/5.)**2-2.*(uO/5.)**3
pkl(I)=ga m/ahm(2,3,I)/bhm(3,I)
pk2(I)=(bet-ahm(2,2,I)*ahm(3,3,I)+ahm(2,3,I)
> *ahm(3,2,I)+ahm(2,2,I)*pk3(I))
> lahm(2,3,I)/bhm(3,I)
END DO
END IF
s3=2.*uO/2.-alf
WRITE (6,'(11" Non-dominant pole is (S3):",ElO.3)')S3
END IF
ELSE
IF (ni(i).eq.3) THEN
c
C the dominant pole is real and the nondominant pair of poles
c are complex (or real) - Fig.2.
C the third order subsystem
c
5 WRITE (6,'(" The damping coefficient (zeta[O.-l.]):",
> I," of the non-dominant pair of poles:",$)')
ierrind=2
read (5,*,err=1500) zeta
888 WRITE (6,'(" Natural undamped frequency (omega n)"
> ,I" of the non-dominant pair of poles:",$)')
ierrind=3
READ (5,*,err=1500) omega n
IF (opc.eq.'Y') THEN -
omegad=omega n/10.
DO WHILE (pk2(i).LE.O.OR.pk3(i).LE.O)
omega n=omega n+omegad
alfa=2.*zeta*omega n+u02/2.
pk3(i)=(alfa+ahm(2~2,i)+ahm(3,3,i))/b(3,i)
beta=omega n*omega n+zeta*omega n*u02
pk2(i)=(beta+ahm(2~3,i)*ahm(3,2~i)
414

> +ahrn(2,2,i)*pk3(i)*b(3,i)
> -ahrn(2,2,i)*ahrn(3,3,i»/ahrn(2,3,i)/b(3,i)
garna=.5*u02*ornega_n*ornega_n
pkl(i)=garna/ahrn(2,3,i)/b(3,i)
END DO
ELSE
ornega n=(-ahrn(2,2,i)-ahrn(3,3,i)-u02/2.)/2./zeta
garna=~5*u02*ornega_n*ornega_n
pkl(i)=garna/ahrn(2,3,i)/b(3,i)
beta=ornega n*ornega n+zeta*ornega n*u02
pk2(i)=(beta+ahrn(2~3,i)*ahrn(3,2~i)
> +ahrn(2,2,i)*pk3(i)*b(3,i)
> -ahrn(2,2,i)*ahrn(3,3,i»/ahm(2,3,i)/b(3,i)
END IF
ELSE
C
C the second order subsystem
C
s2=u02*2.
s2d=u02/10.
DO WHILE (pk2(i).LE.0.l)
s2=s2+s2d
pkl(i)-u02/2.*s2/bhm(2,i)
pk2(i)=(u02/2.+s2+ahm(2,2,i»/bhm(2,i)
END DO
END IF
END IF
C
C print computed gains and open-loop and clsed-loop subsystem
C transfer functions
C
WRITE (6,'(lX,"Local feedback servo gains:",/)')
WRITE (6,4) 'position servo gain KP(',I,')=',PK1(i)
WRITE (6,4) 'Velocity servo gain KV(',I,')=',PK2(i)
IF (NI(I).EQ.3) THEN
WRITE (6,4) 'Curr./pr. servo gain KS(',I,')=',PK3(i)
END IF
WRITE (6,30)
WRITE(6,'(lX,"Open-loop transfer function:",/)')
IF (NI(I).EQ.2) THEN
WRITE (6,25) bhm(2,i),-ahm(2,2,i)
ELSE
WRITE (6,26) ahm(2,3,i)*bhm(3,i),-(ahm(2,2,i)+ahm(3,3,i»,
> ahm(2,2,i)*ahm(3,3,i)-ahm(2,3,i)*ahm(3,2,i)
END IF
WRITE (6,30)
WRITE (6,'(lX,"Closed-loop transfer function:",/)')
IF (NI(I).EQ.2) THEN
WRITE (6,35) bhm(2,i)*pkl(i),-ahm(2,2,i)+bhm(2,i)*pk2(i),
> bhm(2,i)*pkl(i)
ELSE
bpom2=pk3(i)*bhm(3,i)-ahm(3,3,i)-ahrn(2,2,i)
bpoml=ahm(2,3,i)*(pk2(i)*bhm(3,i)-ahm(3,2,i»-ahm(2,2,i)*
> (pk3(i)*bhm(3,i)-ahm(3,3,i»-ahm(2,1,i)
bpomO=pkl(i)*bhm(3,i)*ahm(2,3,i)-ahm(2,1,i)*(pk3(i)*
> bhm(3,i)-ahm(3,3,i»
WRITE (6,36) bhm(3,i)*ahm(2,3,i)*pkl(i),bporn2,bpoml,bpomO
END IF
 415

WRITE (6,30)
RETURN
1500 WRITE (6,'(" DIAG**· Input conversion error")')
go to (1,5,888) ierrind
END

C---------------------------------------------------------------------
C SUBROUTINE: TRAJEK
C---------------------------------------------------------------------
C FUNCTION: GENERATES NOMINAL TRAJECTORIES OF THE
C ROBOT JOINTS - BETWEEN TWO SPECIFIED
C TERMINAL POSITIONS WITH TRIANGULAR OR
C TRAPEZOID VELOCITY DISTRIBUTION
C---------------------------------------------------------------------
C INPUT VARIABLES:
C FILE(5) robot name
C N number of joints of the user's specific robot
C QO(I) initial values of the joint coordinates
C QF(I) values of the terminal point of the joint tra-
c jectories (from the input file ***.TRA)
c H sampling interval at which the trajectories
C are computed
C T time duration of the nominal movement (traj.)
C BETA parameter defining the trapezoidal profile of
C velocity distribution
C A DLMB
C (trajectory)
C
C
C +
C 1
C 1
C 1
C I·
C
C
----1----+--------------+------+--------------->
o Tl T2 T TE(time)
C
C TI-T*BETA
C T2=T*(1.-BETA)
C
C PROFIL character variable for the user's selection
C of the desired profile of the velocity dis-
C tribution along the nominal trajectory
C KSI2(6) indicator of the joint type (1- linear,
C 0 - rotational)
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C TE time instant
C Q(6) positions of the joints angles at the nominal
C trajectory at the moment TE
C DQ(6) velocities "
C DDQ(6) accelerations
C these values are written in the output file ***.ANG
C---------------------------------------------------------------------
416

SUBROUTINE TRAJEK
C
CHARACTER *2 PROFIL,FILEP*9,STAMPA*1,FILET*9
CHARACTER *4 IND(6)
BYTE FILE(5)
COMMON/FILE/FILE
COMMON/DINT TR/ H
COMMON/ORDGLB/ NUK,N
COMMON/TIPL/NPOM,KSIl(6),KSI2(6),KSI3(6)
DIMENSION QO(6),QF(6),Q(6),DQ(6),DDQ(6),DLTQ(6)
REAL LMB
C---------------------------------------------------------------------
C
C read data from the input file ***.TRA
C
FILET=CHAR(FILE(1))//CHAR(FILE(2))//CHAR(FILE(3))//
* CHAR(FILE(4))//CHAR(FILE(5))//'.TRA'
open(unit=7,file=filet,status='old')
read(7,2l0)(qO(i),i=1,n)
read(7,2l0)(qf(i),i=1,n)
read(7,2l0)h,t
200 format(lx,i3)
210 format(lx,6flO.5)
close(unit=7)
C
22 FORMAT(6(4X,A5,4X))
100 FORMAT(6E13.5)
C
TE=O.
M=T/H
C
C indication of the joint type (TRA/ROT)
C
DO I=l,N
IND(I)=' rad '
IF(KSI2(I).EQ.l)THEN
IND(I)=' [m] ,
END IF
END DO
C
type *
type *,' Synthesis of the joints nominal trajectories'
TYPE 40
40 FORMAT(/,$,' Want to ~rint the nominal trajectories [YIN]?:')
READ(5,45)STAMPA
45 FORMAT(Al)
C
TYPE 10
10 FORMAT($,' Select the velocity profile-triangular or trapezoid
* [TA/TP]:')
READ(5,20)PROFIL
20 FORMAT(A2)
C
FILEP=CHAR(FILE(1))//CHAR(FILE(2))//CHAR(FILE(3))//
* CHAR(FILE(4))//CHAR(FILE(5))//'.ANG'
OPEN(UNIT=4,FILE=FILEP,STATUS='NEW')
C
IF(PROFIL.EQ.'TA')GO TO 70
C------------------------------------------------------------------
 417

C
C trapezoid velocity profile
C
25 TYPE 30
30 FORMAT(' Specify the trapezoid profile -
* by defining parameter BETA',/,$,' (duration of the
* acceleration/deacceleration phase as a part of',/,
* , total trajectory duration) [0.0-0.5]:')
READ (5,35) BETA
35 FORMAT (FlO.5)
IF((BETA.LT.0.0).OR.(BETA.GE.0.5)) GO TO 25
C
C
Tl=BETA*T
T2= (1. -BETA) *T
C
C Computation of the scalar parameters of the velocity
C profile LMB and DDLMB and CONST
C

P=1./((BETA*T**2)*(l.-BETA))
Pl=l./(T*(l.-BETA))
CONST=0.5*BETA/(l.-BETA)
C
50 CONTINUE
IF(TE.GT.T)GO TO 65
IF(TE.LT.Tl) THEN
LMB=0.5*p*TE**2
DLMB=P*TE
DDLMB=P
ELSE IF(TE.LT.T2)THEN
LMB=Pl*TE-CONST
DLMB=pl
DDLMB=O.
ELSE IF(TE.LE.T) THEN
LMB=-0.5*p*(T-TE)**2+l.
DLMB=P*(T-TE)
DDLMB=-P
END IF
C
C computation of the nominal positions, velocities and
C accelerations
C
DO I=l,N
DLTQ(I)=QF(I)-QO(I)
Q(I)=QO(I)+LMB*DLTQ(I)
DQ(I)=DLMB*DLTQ(I)
DDQ(I)=DDLMB*DLTQ(I)
END DO
C
C write the trajectories in the file ***.ANG
C
WRITE(4,lOO)TE
WRITE(4,lOO)(Q(I),I=l,N)
WRITE(4,lOO)(DQ(I),I=l,N)
WRITE(4,lOO)(DDQ(I),I=l,N)
C
C print at the screen
C
418

IF(STAMPA.EQ.'N')GO TO 110
WRITE(6,22)(IND(I),I=1,N)
WRITE(6,100)TE
WRITE(6,100)(Q(I),I=1,N)
WRITE(6,100)(DQ(I),I=1,N)
WRITE(6,100)(DDQ(I),I=1,N)
C
110 TE=TE+H
GO TO 50
65 CONTINUE
C
CLOSE(UNIT=4)
RETURN
C--------------------------------------------------------------------
C
C triangular velocity profile
C
70 CONTINUE
T1=T/2.
C
C computation of parameters
C
P=4./(T**2)
150 CONTINUE
IF(TE.GT.T) GO TO 160
C
IF(TE.LT.T1)THEN
LMB=0.5*p*TE**2
DLMB=P*TE
DDLMB=P
ELSE
LMB=-0.5*P*(T-TE)**2+1.
DLMB=P*(T-TE)
DDLMB=-P
END IF
C
DO I=l,N
DLTQ(I)=QF(I)-QO(I)
Q(I)=QO(I)+LMB*DLTQ(I)
DQ(I)=DLMB*DLTQ(I)
DDQ(I)=DDLMB*DLTQ(I)
END DO
C
C write in the output fIe ***.ANG
C
WRITE(4,100)TE
WRITE(4,100)(Q(I),I=1,N)
WRITE(4,100)(DQ(I),I=1,N)
WRITE(4,100)(DDQ(I),I=1,N)
C
C print at the screen
C
IF(STAMPA.EQ.'N')GO TO 90
WRITE(6,22)(IND(I),I=1,N)
WRITE(6,100)TE
WRITE(6,100)(Q(I),I=1,N)
WRITE(6,100)(DQ(I),I=1,N)
WRITE(6,100)(DDQ(I),I=1,N)
C
 419

90 TE=TE+H
GO TO 150
160 CONTINUE
C
CLOSE(UNIT=4)
RETURN
END

C---------------------------------------------------------------------
C SUBROUTINE: SMNOMP
C---------------------------------------------------------------------
C FUNCTION: COMPU~ES THE NOMINAL DYNAMICS OF THE ROBOT
C - NOMINAL DRIVING TORQUES AND NOMINAL CEN-
C TRALIZED OR LOCAL CONTROL
C---------------------------------------------------------------------
C INPUT VARIABLES:
C FILE(5) - robot name
C QOl(6) - nominal angles (displacements) of the joints
C QT01(6) -nominal velocities of the joints
C QUO(6) - nominal accelerations of the joints
C the data on nominal trajectory are read from
C the file ***.ANG
C NI(I) - the order of the i-th subsystem
C N - number of joints of the user's specific robot
C A,B,F - matrices of the actuators' models (see
C explanations in subroutine INPUTL)
C UMAX(2,6) - contraints upon the actuator inputs
C HMIN - minimal values of the moments of inertia of
C the mechanism around the joints axes (see ex-
C planation in subroutine SM MAX IN)
C HH(6,6) - inertia matrix of the robot for given joints
C angles(SI) and velocities (SIDOT) - computed
C in the subroutine MODEL
C Hl(6) - vector of centrifugal, Coriolis and gravity
C moments for given SI and SIDOT - subrout.MODEL
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C pO(6) - nominal driving torques
C UO(6) - nominal programmmed control -feedforward
C (centralized or local)
C POWER(6)- nominal power at each actuator
C SENERG(6) - nominal power consumption of each actuator
C DXO(I) - nominal currents/pressures
C---------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C MODEL - computes the dynamic model of the robot
C mechanism (computes inertia matrix Hand
C centrifugal, Coriolis, gravity moments h
C for the given nominal trajectories)
C-----------------------------------------------------------------
SUBROUTINE SMNOMP
C
420

DIMENSION XPOM(6)
DIMENSION QUO(6)
DIMENSION SENERG(6),POWER(6)
C
COMMON/NPOM1/ QOO(6),QTOO(6),QUOO(6),POO(6)
COMMON/NPOM2/ Q01(6),QT01(6),QU01(6),P01(6)
COMMON/SUBSYS/ A(3,3,6),B(3,6),F(3,6)
COMMON/NKOORD/ PO(6),XO(18),UO(6)
COMMON/ACTORD/ NI(6)
COMMON/UMAX/ UMAX(2,6)
COMMON/HMIN/ HMIN(6)
COMMON/SKOORD/ DXO(18),PXO(6)
COMMON/ORDGLB/NUK,NN
COMMON/FILE/FILE(S)
BYTE FILE
C
INCLUDE 'IN2:MODELM.MOD'
COMMON/MODOPC/ MOD,ITIP
CHARACTER*9 FILEP
C----------------------_______________________________ __________ _
C
TYPE *
TYPE *,' Computation of the nominal dynamics of the robot'
TYPE *,' - nominal driving torques and nominal'
TYPE *,' programmed control -'
type 399
399 FORMAT(/,$,, want centralized or local nominal programmed
1 control? [C/L]:')
read(S,401)YES
C
C set indicator ILOC: ILOC=O - local nominal control,
C ILOC=1 - centralized nominal control
C
ILOC=1
IF(YES.EQ.'L')ILOC=O
TYPE 400
400 FORMAT($,' want to print the nominal dynamics [YIN]?:')
READ(S,401)YES
401 FORMAT(A1)
C
MOD=O
ITIP=O
N=NN
C
C openning of the files ***.ANG to read nominal trajectories
C and ***.DIN to write the computed nominal control and
C nominal driving torques
C
FILEP=CHAR(FILE(1))//CHAR(FILE(2))//CHAR(FILE(3))//
1 CHAR(FILE(4))//CHAR(FILE(S))//'.ANG'
OPEN(UNIT=1,NAME=FILEP,STATUS='OLD',ERR=1110)
C
FILEP=CHAR(FILE(1))//CHAR(FILE(2))//CHAR(FILE(3))//
1 CHAR(FILE(4))//CHAR(FILE(S))//'.DIN'
OPEN(UNIT=2,NAME=FILEP,STATUS='NEW')
C
C read nominal trajectories from the file ***.ANG
C VR2 is the time instant along the nominal trajectory
C
 421

READ(1,100)VR2
READ(1,100)Q01
READ(1,100)QT01
READ(1,100)QUO
100 FORMAT(6E13.5)
C
C the auxi1aiar~ vectors SI and SIDOT - through them the
C values of the joint angles and velocities are sent to the
C subroutine MODEL to compute matrix HH and vector H1
C
DO 1 I=l,N
SENERG(I)=O.
SI(I)=Q01(I)
1 SIDOT(I)=QT01(I)
C
C call subroutine MODEL to compute HH and HI for given joints
C angles and velocities (nominal trajectory)
C
CALL MODEL
C
C computation of the nominal driving torque pxO in the initial
C point of the nominal trajectory and computation of the third
C actuator state coordinate (if its order is 3)
C
DO 2 I=1,N
PXO(I)=Hl(I)
DO 77 J=l,N
PXO(I)=PXO(I)+HH(I,J)*QUO(J)
77 CONTINUE
C
IF(NI(I).EQ.2)GO TO 2
DXO(I)=QUO(I)-A(2,2,I)*QT01(I)
C
C if the centralized nominal control is computed then the total
C nominal driving torque is taken into account
C if the local nominal control is computed then just the
C estimation of the mechanism moment of inertia around the
C joint axis is taken into account
C
IF(ILOC.EQ.I)THEN
DXO(I)=DXO(I)-F(2,I)*PXO(I)
ELSE
DXO(I)=DXO(I)-F(2,I)*HMIN(I)*QUO(I)
END IF
DXO(I)=DXO(I)/A(2,3,I)
2 CONTINUE
999 FORMAT(3X,6El2.3)
C---------------------------------------------------------------------
C
C memorize the nominal torques PXO in pO
C and the nominal trajectory in QOO,QTOO,QUOO (the previous
C point) and the time instant in VR1
C
3 VR1=VR2
DO 4 I=l,N
PO(I)=PXO(I)
QOO(I)=QOl(I)
QTOO(I)=QT01(I)
QUOO(I)=QUO(I)
4 XO(I)=DXO(I)
422

C
C read next point at the nominal trajectory from the file
C
READ(l,100,END=977)VR2
READ(l,100)Q01
READ(l,100)QT01
READ(l,100)QUO
C
POM=VR2-VR1
C
C compute QUO(6) - nominal accelerations
C and send nominal joints angles and velocities through the
C auxiliary vectors 51 and SIDOT into subroutine MODEL
C
5 DO 6 I=l,N
5I(I)=Q01(I)
5IDOT(I)-QT01(I)
6 CONTINUE
C
C call MODEL to compute HH and H1 for the nominal trajectory
C
CALL MODEL
C
C computation of the nominal driving torques, power and energy
C consumption, nominal state vectors and programmed control
C
DO 7 I=l,N
PXO(I)=H1(I)
DO 88 J=l,N
PXO(I)=PXO(I)+HH(I,J)kQUO(J)
88 CONTINUE
C
POWER(I)=ABS(PO(I)*QTOO(I))
SENERG(I)-5ENERG(I)+POWER(I)*POM
C
IF(NI(I).EQ.2)GO TO 89
C
C computation of the third state coordinate of the actuator
C model (if its order is 3)
C
DXO(I)=QUO(I)-A(2,2,I)*QT01(I)
C
IF(ILOC.EQ.1)THEN
DXO(I)=DXO(I)-F(2,I)*PXO(I)
EL5E
DXO(I)=DXO(I)-F(2,I)*HMIN(I)*QUO(I)
END IF
C
DXO(I)=DXO(I)/A(2,3,I)
C
C computation of the derivative of the third state coordinate
C
XPOM(I)=(DXO(I)-XO(I))/POM
C
C computation of the nominal control if the model order is 3
C
UO(I)-(XPOM(I)-A(3,2,l)*QTOO(I)-A(3,3,I)*XO(I))/B(3,I)
GO TO 90
C
 423

C if the order of the actuator state model is 2, directly

C compute the nominal control
C
89 UO(I)-QUOO(I)-A(2,2,I)*QTOO(I)
C
IF(ILOC.EQ.l)THEN
UO(I)=(UO(I)-F(2,I)*PO(I))/B(2,I)
ELSE
UO(I)=(UO(I)-F(2,I)*HMIN(I)*QUOO(I))/B(2,I)
END IF
C
C amplitude contraint upon the actuators inputs
C
90 IF(UO(I).LT.UMAX(1,I).AND.UO(I).GT.(-UMAX(2,I)))GO TO 7
TYPE 101,VRl,I,UO(I)
7 CONTINUE
C
C set computed values to 0 if they are less than 0.0001
C
DO 7722 I-l,N
IF(ABS(QOO(I)).LT.O.OOOOl)QOO(I)=O.
IF(ABS(QTOO(I)).LT.O.OOOl)QTOO(I)=O.
IF(ABS(QUOO(I)).LT.O.OOOl)QUOO(I)=O.
IF(ABS(PO(I)).LT.O.OOOl)PO(I)=O.
IF(ABS(UO(I)).LT.O.OOOl)UO(I)=O.
IF(ABS(POWER(I)).LT.O.OOOl)POWER(I)=O.
IF(ABS(SENERG(I)).LT.O.OOOl)SENERG(I)=O.
7722 CONTINUE
C
C write nominal dynamics in the output file ***.DIN
C
WRITE(2,100)VRl
WRITE(2,100)(QOO(I),I=1,N)
WRITE(2,100)(QTOO(I),I=1,N)
WRITE(2,100)(QUOO(I),I=1,N)
WRITE(2,100)(PO(I),I=1,N)
WRITE(2,100)(UO(I),I=1,N)
WRITE(2,100)(POWER(I),I=1,N)
WRITE(2,100)(SENERG(I),I=1,N)
C
C print nominal dynamics at the terminal screen
C
IF(YES.NE.'Y')GO TO 3
TYPE 200,VRl
200 FORMAT(' Time:',F12.s,' [s)')
TYPE 20l,(I,I-l,N)
201 FORMAT(' Joint numbers ',6(4X,Il,sx))
TYPE 202,(QOO(I),I-l,N)
202 FORMAT(' Angles [rad) or [m):',6ElO.3)
TYPE 203,(QTOO(I),I-l,N)
203 FORMAT(' Velocit.[rad/s-m/s):',6ElO.3)
TYPE 204,(QUOO(I),I=1,N)
204 FORMAT(' Acceler. [rad/s**2):',6EIO.3)
TYPE 20s,(PO(I),I=1,N)
205 FORMAT(' Driving torques[Nm):',6ElO.3)
TYPE 206,(UO(I),I=1,N)
206 FORMAT(' Control sign.[V-mA):',6ElO.3)
TYPE 207,(POWER(I),I=1,N)
207 FORMAT(' Power [WI: ',6EIO.3)
424

TYPE 208,(SENERG(I),I=1,N)
208 FORMAT(' Energy [WsJ: , ,6E10.3)
GO TO 3
C
C
101 FORMAT(' DIAG*** Nominal control signal are greater than the' ,I,
1 , allowable amplitude constraints at the actuators inputs' ,I,
2 , (change nominal kinematics, or change actuators)',/,3X,
3 , time =', F1S.S, ' in the joint No. ',12,' the required nominal
4 signal =', G1S.S)
C
977 CONTINUE
C
C write the terminal point at the trajectory into output file
C
WRITE(2,100)VR1
WRITE(2,100)(QOO(I),I=1,N)
WRITE(2,100)(QTOO(I),I=1,N)
WRITE(2,100)(QUOO(I),I=1,N)
WRITE(2,100)(PO(I),I=1,N)
WRITE(2,100)(UO(I),I=1,N)
WRITE(2,100)(POWER(I),I=1,N)
WRITE(2,100)(SENERG(I),I=1,N)
C
CLOSE (2)
C
6680 CONTINUE
CLOSE (1)
RETURN
1110 TYPE 1111, FILEP
1111 FORMAT (lX,'DIAG*** File' ,a9,lx,' cannot be found')
CLOSE (1)
STOP
END

C---------------------------------------------------------------------
C SUBROUTINE: LINANA
C---------------------------------------------------------------------
C FUNCTION: THE SUBROUTINE FOR EXPONENTIAL STABILITY
C ANALYSIS OF THE LINEARIZED MODEL OF THE
C ROBOT; CALLS SUBROUTINES FOR FORMING OF
C THE LINEARIZED MODEL OF THE ROBOT AND FOR
C COMPUTATION OF THE EIGEN-VALUES OF THE
C LINEAR MODEL; THE STABILITY OF THE SYSTEM
C IS TESTED IF ONLY LOCAL CONTROLLERS ARE
C APPLIED
C---------------------------------------------------------------------
C INPUT VARIABLES:
C FILE(S)-name of the user's robot
C N - number of joints of the user's robot
C TO - time instant at the nominal trajectory
C YO(12)- nominal joints angles and velocities at the
C time instant TO (the nominal state vector of
C the mechanical part of the system)
C DXO(6)- Nominal accelerations of the joint coord.
C PO(6) - Nominal driving torques in the time instant TO
 425

C TIMIN(*) - vector of time instants in which the analy-

C sis of the system stability is required -
C this is specifified by the user in ***.INT
C ITMIN - number of points at the nominal trajectory in
C which we have to analyze the robot stability
C ALFAIM - desired exponential stability degree of the
C linearized model of the robot
C IOPTIN(6) - option: integral feedback loop(l-yes,O-no)
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C REALEV(NUK) - Real parts of the eigen-values of the
C linearized global system
C RIMAG(NUK) Complex parts "
C ICNVLN - Indication whether the linearized model
C of the system is stabilized or not
C---------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C SMLINM - Forming of the matrices of linearized mo-
C del of the robotic system (open-loop system)
C SMLGAD - Introduce local feedback loops in the mat-
C rices of the linearized model of the system
C SMEIGN - Auxiliary subroutine for computation of the
C eigen-values of the given quadratic matrix
C----------------------------------------------------------------------
SUBROUTINE LINANA
C
COMMON/ORDGLB/ NUK,N
COMMON/STDGIM/ ALFAI(6),ALFAIM
COMMON/ACTORD/NI(6)
COMMON/OPTION/IPOMl(42),IOPTIN(6)
COMMON/OPTPR/ ICNVLN
COMMON/AUXIL/ TO,PO(6),YO(12),DXO(6),UO(6)
COMMON/CTIMIN/ TIMIN(60),ITMIN
C
BYTE FILE(5)
COMMON/FILE/ FILE
COMMON/CAUXIL/ ITERC,REALPS
COMMON/INTGAI/ NUKI
C
DOUBLE PRECISION AL(24,24)
DIMENSION AO(24,24),BL(24,6)
DIMENSION REALEV(24),RIMAG(24)
C
DIMENSION HU(6,6)
C
BYTE FTOT(14),FTOT1(14)
DATA FTOT(1)/'I'/,FTOT(2)/'N'/,FTOT(3)/'1'/,FTOT(4)/':'/,
1 FTOT(lO)/'.'/,FTOT(ll)/'D'/,FTOT(12)/'I'/,FTOT(l3)/'N'/
2 FTOT(14)/O/ '
DATA FTOT1(1)/'I'/,FTOT1(2)/'N'/,FTOT1(3)/'l'/,FTOT1(4)/':'/,
1 FTOT1(10)/'.'/,FTOTl(11)/'I'/,FTOT1(12)/'N'/,
C-------------------------------____________________________________ _
2 FTOT1(13)/'T'/,FTOT1(l4)/O/
C
TYPE *
TYPE *,' Stability analysis'
TYPE *
C
DO 199 1=1,5
FTOT1(I+4)=FILE(I)
426

199 FTOT(I+4)=FILE(I)
C
C computation of NUKI - the order of the total systems with
C local integral feedback loops
C
NUKI=O
DO 9000 I=l,N
9000 NUKI=NUKI+NI(I)+IOPTIN(I)
C
C open file containing nominal dynamics computed in SMNOMP
C
OPEN(UNIT=1,NAME=FTOT,TYPE='OLD',ERR=3100)
C
C open file ***.INT specified by the user which contains the
C desired stability degrees and the moments at the nominal
C trajectories "in which" the user wants to analyze the
C stability of the linearized model
C
OPEN(UNIT=2,NAME=FTOT1,TYPE='OLD',ERR=3101)
C
READ(2,102)ITMIN
102 FORMAT(I4)
GO TO 3102
C
3100 TYPE 3103, FILE
3103 FORMAT(' DIAG*** File ',5A1,'.DIN cannot be found')
RETURN
C
3101 TYPE 3104,FILE
3104 FORMAT(' DIAG*** File ',5A1,'.INT cannot be found')
RETURN
C------------------------------------------------------------------
C
C reset the counter ITERC of points at the nominal
C trajectory in which analysis has to be performed
C
3102 ITERC=l
C
C read data on nominal dynamics from ***.DIN
C
1 READ(1,100,END=3201)TO
READ(1,100)(YO(I),I=1,N)
READ(1,100)(YO(I),I=N+1,N+N)
READ(1,100)(DXO(I),I=1,N)
READ(1,100)(PO(I),I=1,N)
READ(1,100)(UO(I),I=1,N)
C
C auxiliary read of power and energy - not necessary in
C this routine
C
READ(1,100)(UO(I),I=1,N)
READ(1,100)(UO(I),I=1,N)
100 FORMAT(6E13.5)
GO TO 3202
C
3201 CLOSE(UNIT=l)
GO TO 3203
C
3202 CONTINUE
 427

C
C test if the next point at the nomonal trajectory around which
C the stability of the system has to be analyzed is reached or
C not; if it is reached - go to stability analysis
C otherwise - read the next data at the nominal trajectory
C
IF(TO.LT.TIMIN(ITERC)-O.OOOl)GO TO 1
C
C read data on desired stability degree
C
3203 READ(2,101)TIMIN(ITERC)
READ(2,101)ALFAIM
101 FORMAT(E15.5)
C---------------------------------------------------------------------
C
C compute matrices of the linearized model of the system in the
C user's selected point at the nominal trajectory (without local
C feedback loops)
C AO(NUKI,NUKI),BL(NUKI,N)- Matrices of the open-loop linearized
C model of the robot
C
10 CALL SMLINM(AO,BL,HU)
C
C put AO into matrix AL to get double-precision computation
C
NN=NUKI
NM=NUKI
DO 7773 KKK=1,24
DO 7773 JJJ=1,24
7773 AL(KKK,JJJ)=AO(KKK,JJJ)
C
WRITE(6,412) TIMIN(ITERC)
412 FORMAT(/,' Eigen-values of the open-loop matrix of',/,
1 , linearized model of the robot in the time instant' ,F12.5,'
2 [s]' ,/)
C
C computation of eigen-values of the open-loop system matrix
C
CALL SMEIGN(NN,NM,AL,REALEV,RlMAG)
C
C introducing of the local feedback gains in the matrix of the
C open-loop linearized model of the system AO
C the matrix AL s obtained: the matrix of the closed-loop linea-
C rized model of the robotic system
C
11 CALL SMLGAD(AO,AL,BL)
C
C computation of the eigen-values of the linearized model of
C the system in the selected point at the nominal tajectory
C
WRITE(6,411)TIMIN(ITERC)
411 FORMAT(/,3X,' Eigen-values of the closed-loop matrix of',
1 /,' linearized model of the robot in the time instant
2 ',F12.5,' [s]',/)
CALL SMEIGN(NN,NM,AL,REALEV,RlMAG)
C
C search for the greatest eigen-value of the system matrix
C (the eigen-value with the greatest value of the
428

C real part)
C
PALFA-REALEV(l)
DO 15 I=2,NUKI
15 IF(REALEV(I).GT.PALFA)PALFA=REALEV(I)
C
WRITE(6,4422)PALFA
4422 FORMAT(/,3X,' Achieved stability degree of the robot',F10.5,/)
C
IF(PALFA.GT.O)TYPE *,' WARNING*** The robot is not stabilized'
C
C Check if the exponential stability degree of the global system
C is greater than the desired stability degree
C
IF«PALFA).GT.(-ALFAIM))GO TO 50
C
C increment of the counter of points at the nominal trajectory
C
ITERC=ITERC+l
C
C check if the system stability is tested in all desired points
C at the nominal trajectory or not: if yes - terminate analysis
C otherwise go to read the next point at the nominal trajectory
C
IF(ITERC.LE.ITMIN)GO TO 1
C
C message whether the linearized model of the robot is stable
C in desired way if just local controllers are applied
C
ICNVLN=O
C
GO TO 111
C
50 ICNVLN-1
C
111 RETURN
END

C---------------------------------------------------------------------
C SUBROUTINE: SMLINM
C---------------------------------------------------------------------
C FUNCTION: COMPUTES THE MATRICES OF THE LINEARIZED
C MODEL OF THE ROBOT; COMPUTES THE OPEN-
C -LOOP MATRICES OF THE TOTAL SYSTEM BY
C ADDING THE MATRICES OF THE ACTUATORS
C MODELS TO THE MATRICES OF THE LINEARIZED
C MODEL OF THE ROBOT MECHANICAL PART
C---------------------------------------------------------------------
C INPUT VARIABLES:
C N - Number of joints of the user's robot
C NUK - system order
C NUKI - order of the system with local integral feed-
r back loops
 429

C NI(N) - orders of the subsystems (actuators) models

C A(NI(I),NI(I),I)-subsystem matrix of the i-th actuator
C B(NI(I),N) - input distribution vector of the i-th
C actuator
C F(NI(I),N) - load distribution vector of the i-th
C actuator
C YO(2*N) -nominal trajectory of the joint angles and
C velocities (at the given time instant)
C PO(N) - nominal driving torques
C DXO(N) - nominal accelerations of joint angles
C OPTIONS:
C IOPTIN(N)- Integral feedback loop in the local con-
C troller (l-yes,O-no)
C INPUT VARIABLES FROM THE CALLED SUBROUTINES:
C H(N,N) - inertial matrix H of the mechanical part of
C the system for the given state
C HO(N) - vector of gravity, centrifugal and Coriolis
C moments (forces) h for the given state
C DH(N,N,N)- the first derivative of the matrix H by the
C joint angles
C DHO(N,N) - the first derivative of the vector h by the
C joint angles
C DHODOT(N,N) - the first derivative of the vector h by
C the joint velocities
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C AO(NUKI,NUKI) - system matrix of the open-loop
C linearized model of the robot
C BL(NUKI,N) input distribution matrix of the
C linearized model of the robot
C MODLIN - indicator for computation of the linea-
C rized model of mechanical part of the
C robot in subroutine MODEL (I-compute,
C O-not)
C---------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C MODEL - computes the nonlinear and the linearized
C model of the mechanical part of the robot
C for the given joint angles and velocities
C (computes matrices H,HO,DH,DHO,DHODOT)
C MINV - SSP subroutine - inverts the given quad-
C ratic matrix
C---------------------------------------------------------------------
SUBROUTINE SMLINM(AO,BL,HU)
C
COMMON/DHCOM/ DH(6,6,6),DHO(6,6),DHODOT(6,6)
COMMON/DINAM/ H(6,6),HO(6)
COMMON/MODOPC/ MOD,ITIP,MODLIN
COMMON/SUBSYS/ A(3,3,6),B(3,6),F(3,6)
COMMON/OPTION/IPOMI(42),IOPTIN(6)
COMMON/ACTORD/ NI(6),KI(6)
COMMON/ORDGLB/ NUK,N
COMMON/AUXIL/ TO,PO(6),YO(12),DXO(6),UO(6)
COMMON/UG/ SI(6),SIDOT(6)
C
COMMON/INTGAI/ NUKI

DIMENSION
.
AO(NUKI,NUKI),BL(NUKI,N)
DIMENSION HP(6,6),HV(6,6),HU(N,N)
DIMENSION LL(6),MM(6)
DIMENSION Y(12),DXU(6),YI(12)
430

C---------------------------------------------------------------------
C
C computation of the model of the mechanical part: nonlinear
C and the linearized model matrices for the given nominal state
C
C put nominal state in the auxiliary vectors SI and SIDOT
C to be transferred into subroutine MODEL
C
DO 2 I=l,N
SI(I)-YO(I)
2 SIDOT( I )=YO( I+N)
C
C set indicator to 1 - to compute the matrices of the
C linearized model
C
MOD=O
MODLIN=l
C
CALL MODEL
C
MODLIN=O
C
C form matrices: HU (matrix by the accelerations)
C HP (matrix by the joints angles) and HV (matrix by the joints
C velocities) in the linearized model of the robot
C
DO 3 I=l,N
II=2-NI (l)
C
DO 3 J=1,N
II=II+NI (J)
C
HP(I,J)=DHO(I,J)
C
HV(I,J)-DHODOT(I,J)
C
HU(I,J)=H(I,J)
C
DO 3 JJ=l,N
3 HP(I,J)=HP(I,J)+DH(I,JJ,J)*DXO(JJ)
C
C introducing of the matrix F
C
DO 4 I=l,N
DO 4 J=l,N
C
HU(I,J)=-F(2,I)*HU(I,J)
C
HP(I,J)=F(2,I)*HP(I,J)
C
4 HV(I,J)=F(2,I)*HV(I,J)
C
C adding of the unit matrix to matrix HU
C
DO 5 I=1,N
HU(I,I)=HU(I,I)+1.
C
C set initially matrix BL to zero
C
 431

DO 5 J-l,NUKI
5 BL(J,I)=O.
C
C invert the matrix HU
C
NN=N
CALL MINV(HU,NN,D,LL,MM)
C
C form matrices AO and BL
C
DO 6 I=I,NUKI
DO 6 J=I,NUKI
C
6 AO(I,J)=O.
C
JJ=O
11=1
DO 11 I=I,N
C
III=II+l
C
IF(NI(I).EQ.2)GO TO 15
BL(II+2,I)=B(3,I)
C
15 DO 7 J=I,NI(I)
JJ=JJ+l
C
AO(II,JJ)=A(I,J,I)
C
IF(NI(I).EQ.2)GO TO 77
AO(II+2,JJ)=A(3,J,I)
IF(IOPTIN(I).EQ.O)GO TO 7
AO(II+3,JJ)=0.
GO TO 7
C
77 IF(IOPTIN(I).EQ.O)GO TO 7
C
AO(II+2,JJ)=0.
C
7 CONTINUE
C
IF(IOPTIN(I).EQ.O)GO TO 78
JJ=JJ+l
C
DO 79 J=I,NI(I)
79 AO(II+J-l,JJ)=O.
C
C introducing of "integral state coordinates"
C
AO(II+NI(I),JJ-NI(I»=1.
C
78 12=1
C
DO 10 I1=I,N
13=12+1
C
AO(III,I2)-HU(I,Il)*A(2,1,Il)
C
AO(III,I3)=HU(I,Il)*A(2,2,Il)
C
IF(NI(Il).EQ.2)GO TO 8
432

C
AO(III,I2+2)=HU(I,Il)*A(2,3,Il)
C
8 BL(II+l,Il)=HU(I,Il)*B(2,Il)
C
DO 9 IJ=I,N
AO(III,I2)=AO(III,I2)+HU(I,IJ)*HP(IJ,Il)
9 AO(III,I3)=AO(III,I3)+HU(I,IJ)*HV(IJ,Il)
C
I2=I2+NI(Il)
C
IF(IOPTIN(Il).EQ.O)GO TO 10
12=12+1
C
10 CONTINUE
C
II=II+NI(I)
IF(IOPTIN(I).EQ.l)II=II+l
C
11 CONTINUE
RETURN
END

C---------------------------------------------------------------------
C SUBROUTINE: SMLGAD
C---------------------------------------------------------------------
C FUNCTION: ADDS LOCAL FEEDBACK GAINS INTO THE
C MATRICES OF THE LINEARIZED MODEL OF THE
C ROBOT AND BY THIS FORMS THE MATRIX OF THE
C CLOSED-LOOP LINEARIZED MODEL OF THE SYSTEM
C (JUST LOCAL SERVO LOOPS ARE ADDED)
C---------------------------------------------------------------------
C INPUT VARIABLES:
C NUK - system order
C N - number of joints
C NUKI - order of the total system if integral feed-
C back loops are introduced
C AO(NUKI,NUKI) - system matrix of the open-loop linea-
C rized model (computed in SMLINM)
C BL(NUKI,N) - input distribution matrix of the linea-
C rized model of the robot (SMLINM)
C NI(N) - orders of the actuators models
C PKl(N) position feedback gain (computed in SMLOC E
C or in SMOPRG)
C PK2(N) - velocity feedback gain
C PK3(N) current (pressure) feedback gain
C PK4(N) - integral feedback gain
C OPTION:
C IOPTIN(N) - local controller with integral feedback
C loop (1 -yes, O-no)
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C AL(NUKI,NUKI) - matrix of the closed-loop linearized
C model of the robot - local feedback
C gains included
C---------------------------------------------------------------------
 433

SUBROUTINE SMLGAD(AO,AL,BL)
C
COMMON/GAINS/ PKl(6),PK2(6),PK3(6),PKG(6),PK4(6)
COMMON/INTGAI/ NUKI
COMMON/OPTION/ IPOM(42),IOPTIN(6)
COMMON/ACTORD/ NI(6)
COMMON/ORDGLB/ NUK,N
C
DIMENSION AO(NUKI,NUKI),BL(NUKI,N)
DOUBLE PRECISION AL(NUKI,NUKI)
C----------------------------------------------------------------------
C
C add local gains to the open-loop system matrix
C
DO 1 I=l,NUKI
DO 1 J=l,NUKI
1 AL(I,J)=AO(I,J)
C
11=0
JJ=l
C
DO 2 I=l,N
II=II+NI(I)
C
AL(II,JJ)=AL(II,JJ)-BL(II,I)*PKl(I)
AL(II,JJ+l)=AL(II,JJ+l)-BL(II,I)*PK2(I)
C
IF(NI(I).EQ.2)GO TO 3
C
AL(II,JJ+2)=AL(II,JJ+2)-BL(II,I)*PK3(I)
AL(II,JJ+3)=AL(II,JJ+3)-BL(II,I)*PK4(I)
C
GO TO 4
C
3 AL(II,JJ+2)=AL(II,JJ+2)-BL(II,I)*PK4(I)
C
4 JJ=JJ+NI(I)
C
IF(IOPTIN(I).EQ.l)II=II+l
C
IF(IOPTIN(I).EQ.l)JJ=JJ+l
C
2 CONTINUE
RETURN
END
434

C---------------------------------------------------------------------
C SUBROUTINE: SMEIGN
C---------------------------------------------------------------------
C FUNCTION: COMPUTATION OF EIGEN-VALUES OF THE
C GIVEN QUADRATIC MATRIX
C---------------------------------------------------------------------
C INPUT VARIABLES:
C NN - matrix order
C NM
C AL(NN,NN) - quadratic matrix
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C REALEV(NN) - vector of the real parts of the eigen-
C -values of the given matrix
C---------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C EIGENP - SSP subroutine to compute eigen-values of the
C given quadratic matrix
C REALVE,COMPVE,HESQR - auxiliary subroutines called by
C subroutine EIGENP
C---------------------------------------------------------------------
SUBROUTINE SMEIGN(NN,NM,AL,REALEV,RlMAG)
C
DOUBLE PRECISION AL(NM,1),EVR(24),EVI(24),VECR(24,24)
DOUBLE PRECISION VECI(24,24)
C
DIMENSION REALEV(NN),RIMAG(NN)
DIMENSION INDIC(24)
C---------------------------------------------------------------------
C
C call subroutine EIGENP to compute eigen-values of AL
C
CALL EIGENP(NN,NM,AL,EVR,EVI,VECR,VECI,INDIC)
C
DO 10 I=1,NN
RIMAG(I)=EVI(I)
10 REALEV(I)=EVR(I)
C
WRITE(6,102)
DO 6 I=1,NN
6 WRITE(6,103)I,EVR(I),EVI(I),INDIC(I)
102 FORMAT(3X,' Eigen-values: ',/,3X,'NO. Real part
1 Complex part Indic.',/)
103 FORMAT(3X,I4,2E15.5,I4)
RETURN
END
 435

C---------------------------------------------------------------------
C SUBROUTINE: SMSIMD
C---------------------------------------------------------------------
C FUNCTION: SIMULATES THE ROBOT DYNAMICS WITH VARIOUS
C USER SELECTED CONTROL LAWS
C THE SIMULATION IS PERFORMED BY NUMERICAL INTEGRA-
C TION OF THE NONLINEAR DYNAMIC MODEL OF THE ROBOT;
C FOR NUMERICAL INTEGRATION THE SIMPLE EULER'S MET-
C HOD IS APPLIED:
C X(next)=X(current)+DINT*XT(current)
C WHERE X IS THE STATE VECTOR OF THE SYSTEM, XT IS
C THE FIRST DERIVATIVE OF THE STATE VECTOR, DINT
C IS THE INTEGRATION INTERVAL DEFINED BY:
C next instant- current instant +DINT
C SUBROUTINE COMPUTES THE FIRST DERIVATIVE OF THE
C STATE VECTOR (AT THE CURRENT INSTANT - ONCE AT
C EACH INTEGRATION INTERVAL) BY COMPUTATION OF
C THE RIGHT SIDE OF THE DIFFERENTIAL EQUATIONS OF
C THE ROBOT MODEL IN THE STATE SPACE USING THE
C CURRENT VALUE OF THE STATE VECTOR
C NOTE: This is the simplest method for numerical inte-
C gration. It is easy to extend the programme to
C include other more reliable and more precise
C methods for numerical integration
C---------------------------------------------------------------------
C INPUT VARIABLES:
C FILE(5) - Robot's name
C N - number of joints for the user's robot
C NUK - the order of the entire system
C YSINT(18)-initial error of the system state vector
C (deviation of the state vector from the
C nominal state at the initial moment)
C YSOI(12)- nominal trajectory - values in the previous
C point: (1-6) joints anles (displacements)
C (7-12) joints velocities
C YS02(12)- nominal trajectory - next point
C QU01 - nominal accelerations at the previos point
C QU02 at the next point
C VR1,VR2 - time instants at the nominal trajectory in
C which the nominal trajectory is calculated
C and memorized in the file ***.ANG
C NI(6) - the orders of the actuators models
C A(3,3,6)- matrices of the actuators models
C AHM(3,3,6) subsystems models-see INPUT ACT
C (include moment of inertia of mechanism)
C B(3,6) - input distribution vectors of actuators
C models
C BHM(3,6)- of subsystems
C models
C (include moment of inertia of mechanism)
C F(3,6) - load distribution vectors of actuators
C models
C FHM(3,6)- of subsystems
C models
C (include moment of inertia of mechanism)
C UMAX(2,6) amplitude constraints upon the actuators
C inputs
C HMIN(6) - minimal moments of inertia of mechanism
C (see SM_MAX_IN)
436

C HH(6,6) - inertia matrix of the mechansm for the

C given value of joints angles
C Hl(6) - vector of centrifugal,Coriolis and gra-
C vity moments (forces) for given joints
C angles and velocities
C OPTIONS: (for each joint)
C IOPCNN(6) apply nominal control (1), or not (0)
C IOPCNM(6) apply centralized nominal control(l),or
C apply decentralized nominal control(O)
C IOPTGG(6) global control is applied in the form of
C on-line computation of gravity, centrifu-
C gal and Coriolis moments around the i-th
C joint (O-no,l-yes)
C IOPTGH(6) global control is applied in the form of
C on-line computation of the inertia moments
C around the i-th joint (O-no, I-yes)
C IOPTG(6) =0 global control is not applied in
C the i-th joint
C =1 global control gain is in the form
C PKG(I)*(gradv*F)/(gradv*B)
C =2 global control gain is in the form
C PKG(I)*F(2,I)/B(3,I)
C TINTS - time interval for printings at terminal
C display
C DINT - integration interval
C PKl,PK2,PK3,PK4 - local feedback gains
C (see SMLOC E)
C PKG(6) - global feedback gaIns
c--------------------------------------------------------------------
C OUTPUT VARIABLES:
C DY(18) - Deviation of the state vector from the
C nominal trajectory (at the current
C sampling interval)
C Pl(6) actual (simulated) driving torques in the
C joints developed during traking of the
C nominal trajectory
C U(6) - actual (simulated) control (actuator
C input) signals
C TIME time moment along the simulation
C SPOWER(6) -actual required power in each joint
C SENERG(6) -actual (simulated) energy consumption
C---------------------------------------------------------------------
C LOCAL VARIABLES:
C YSP(18) -actual state vector at the current inte-
C gration interval
C YSOP(18) -nominal state vector at the current interval
C YSO at the next interval
C POP(6) -nominal driving torques at the current
C interval
C PO(6) at the next interval
C YO(12) -nominal joints angles and velocities at the
C current integration interval
C QUO(6) - robot accelerations
C DYS(18) - the first derivative of the actual state
C vector at the current integration interval
C---------------------------------------------------------------------
C f~JBROUTINESREQUIRED:
C MODEL - computes the inertia matrix H and the vector h
C of gravity, centrifugal and Coriolis moments
C for the given joints angles SI and velocities
 437

C SIDOT
C SMOPTN - enables selection of a control law which will
C be simulated
C SMCONT - computes local control signals for the given
C deviations of the state coordinates from
C the nominal trajectory DX(3)
C SMCONG - computes the global gains for the given
C deviations of the state vector DX(3)
C---------------------------------------------------------------------
SUBROUTINE SMSIMD
C
BYTE FILE(S)
COMMON/FILE/FILE
C
INCLUDE 'IN2:SMCOM.COM'
C
INCLUDE 'IN2:MODELM.MOD'
C
COMMON/MODOPCI MOD,ITIP
COMMON/NOVLON/AHMN(2,2,6),BHMN(2,6)
COMMON/OPTNOM/IOPCNM(6),IOPCNN(6)
COMMON/NKOORDI YSOP(18),PO(6),UO(6)
COMMON/SIMTIMI TIME
COMMON/POMINTI POMINT(6)
C
DIMENSION YSP(18),DY(18),DYS(18),POP(6),P1(6)
DIMENSION DX(3)
DIMENSION YSO(18)
DIMENSION U(6),PKGP(6)
DIMENSION YS01(12),YS02(12)
DIMENSION YOPOM(12)
DIMENSION SENERG(6),SPOWER(6)
DIMENSION DX1(3),DXP(6)
DIMENSION QU01(6),QU02(6),QUO(6)
DIMENSION H11(6),H10(6)
DIMENSION YO(12),HP(6),HPOM(6,6),HPOM3(6,6)
DIMENSION LL(6),MM(6)
C
BYTE FTOT(14),FTOT1(14),FTOT2(14)
DATA FTOT( 1)/' 1'1, FTOT( 2 )/'N' I, FTOT( 3 )/' l' I, FTOT( 4 )/':' 1
1 FTOT(10)/'.'I,FTOT(11)/'A'I,FTOT(12)/'N'I,
2 FTOT(13)/'G'I,FTOT(14)/OI
DATA FTOT1(1)/'I'I,FTOT1(2)/'N'I,FTOT1(3)/'1'1,
1 FTOT1(4)/':'I,FTOT1(10)/'.'I,FTOT1(11)/'S'1
2 FTOT1(12)/'I'I,FTOT1(13)/'M'I,FTOT1(14)/OI
DATA FTOT2(1)/'I'I,FTOT2(2)/'N'I,FTOT2(3)/'1'1,
1 FTOT2(4)/':'I,FTOT2(10)/'.'I,FTOT2(11)/'L'I,
2 FTOT2(12)/'O'I,FTOT2(13)/'C'I,FTOT2(14)/OI
C---------------------------------------------------------------------
C
DO 1 1=1,5
FTOT(I+4)=FILE(I)
FTOT2(I+4)=FILE(I)
1 FTOT1(I+4)=FILE(I)
TYPE 6699
6699 FORMAT(/,' Simulation of tracking of the nominal trajectory')
TYPE *,('-',1-1,54)
C
C open file ***.ANG containing data on the nominal
C joint trajectories
C
438

OPEN(UNIT=1,NAME=FTOT.TYPE='OLD',ERR=999)
GO TO 55
C
999 TYPE 998,(FILE(I),I=1,5)
998 FORMAT(3X,'DIAG*** FILE:' ,5Al,' .ANG cannot be found')
RETURN
C
C Options: the integration interval is set to 0.0001 [s]; user
C might introduce variable integration interval and impose
C it in the file ***.DAT
C
55 DINT=O.OOOI
C
C test whether the file with simulation results already exists
C
OPEN(UNIT=2,NAME=FTOTl,TYPE='OLD',ERR=6691)
TYPE 6697,FILE
6697 FORMAT(/,' WARNING*** File ',SAl,'.SIM already exists')
C
TYPE 6696
6696 FORMAT($,, want to form a file with new simulation [YIN]?:')
ACCEPT 6695,YES
6695 FORMAT(Al)
IF(YES.NE.'Y')GO TO 6694
C
CLOSE(UNIT=2)
6691 CALL ASSIGN(2,FTOTl,14)
C
GO TO 6698
C
C if the user doesnot want new simulation-exit from the routine
C
6694 CLOSE(UNIT=2)
CLOSE(UNIT=l)
RETURN
C--------------------------------------------------------------------
C
C open file ***.LOC containing synthesized local feedback gains
C
6698 OPEN(UNIT=3,NAME=FTOT2,TYPE='OLD',ERR=2000)
C
DO 2002 I=l,N
READ(3,2003)PKl(I)
READ(3,2003)PK2(I)
READ(3,2003)PK3(I)
2002 READ(3,2003)PK4(I)
2003 FORMAT(F12.5)
C
CLOSE(UNIT=3)
GO TO 2001
C
C if there is no file ***.LOC with local feedback gains - exit
C from the routine
C
2000 TYPE *,'DIAG*** File ***.LOC cannot be found'
CLOSE(UNIT=l)
RETURN
C--------------------------------------------------------------------
C
C call subroutine to select control law which will be simulated
C
 439

2001 CALL SMOPTN

C
2112 NN=N+N
10001 FORMAT($,' want printings at the display during simulation
> [YIN)?:' )
5597 TYPE 10001
ACCEPT 6695,YES
IF(YES.NE.'Y')TINTS=O.
IF(YES.NE.'Y')GO TO 10005
TYPE 10002
10002 FORMAT($,' How oten [s - FlO.51?:')
READ(5,10003,ERR=5597)TINTS
10003 FORMAT(FlO.5)
IF(TINTS.GE.O.Ol)GO TO 10005
TYPE *,' DIAG*** The values less than 0.01 are not allowed'
GO TO 5597
c
c Intialize time counter for printings
C
10005 TSTAMP=O.
C
c------------------------------------------------------------------
c
C initialization of the variables for simulation
C
DO 9 I=l,N
U(I)=O.
SENERG(I)=O.
POMINT(I)=O.
9 SPOWER(I)=O.
C
C read initial point at the nominal trajectory
C
READ(l,lOO)VRl
TIME=VRl
READ(l,lOO)(YSOl(I),I=l,N)
READ(l,lOO)(YSOl(I),I=N+l,NN)
READ(l,lOO)QUOl(I)
100 FORMAT(6E13.5)
C
C put nominal angles and velocities in the auxiliary vectors
C SI and SIDOT to transfer data to the subroutine MODEL
C
DO 40 I=l,N
SI(I)=YS01(I)
40 SIDOT(I)=YSOl(I+N)
C
ITIP=O
MOD=O
IPS=l
C
C call subroutine MODEL to calculate inertia matrix H and vector
C h for the nominal joints angles and velocities (initial point)
C
CALL MODEL
C
C in auxiliary vector HlO the nominal vector h is memorized
C calculate the nominal driving torques (initail point)
C
2010 DO 41 I=l,N
POP(I)=Hl(I)
440

H10(I)=H1(I)
DO 41 J=l,N
41 POP(I)=POP(I)+HH(I,J)*QU01(J)
C
11=2
C
DO 43 1=l,N
YSOP(11)=YS01(1+N)
YSOP(11-1)=YS01(I)
1F(NI(1).EQ.2)GO TO 44
C
C if the actuator model is of order 3 then compute the nominal
C value of the third state coordinate of the i-th actuator
C
YSOP(II+1)=(QU01(I)-A(2,2,I)*YSOP(II)-F(2,1)*POP(I))/
1 A( 2, 3, I)
C
C test whether centralized or local nominal control is to be ap-
C plied
C
44 IF(10PCNM(I).EQ.1)GO TO 43
C
C computation of the subsystem matrices to include minimal va-
C lues of the moments cf inertia of the mechanim around the
C joints axes (necessary for computation of nominal local
C control)
C
FHM(2,1)=1./(HM1N(1)+1./F(2,1))
POM=(1./F(2,1))*FH"(2,I)
AHM(2,2,1)=A(2,2,I)*POM
IF(NI(I).EQ.2)GO TO 43
AHM(2,3,1)=A(2,3,1)*POM
C
YSOP(1I+1)=(QUO(I)-AHM(2,2,1)*YSOP(11))/AHM(2,3,1)
43 11=1I+N1(I)
C
C computation of the actial initial state coordinates (since
C initial errors of state coordinates are imposed in SMOPTN)
C
DO 10 I=l,NUK
10 YSP(I)=YSOP(I)+YSINT(I)
C********************************************************************
C Start cycle for simulation
C
2 CONTINUE
C
C compute deviation (errors) between actual state coordinates
C (YSP) and the nominal state coordinates (YSOP)
C
DO 77 I=l,NUK
77 DY(I)=YSP(I)-YSOP(I)
C
C write data on simulated robot performance in file ***.S1M
C
WRITE(2,100)TIME
WR1TE(2,100)(DY(1),I=1,NUK)
WRITE(2,100)(P1(I),I=1,N)
WR1TE(2,100)(U(I),1=1,N)
WR1TE(2,100)(SPOWER(I),1=1,N)
WRITE(2,100)(SENERG(I),I=1,N)
 441

C
C print at terminal display
C
IF(TSTAMP.GT.TIME+O.OOl.OR.TINTS.EQ.O.)GO TO 119
TSTAMP=TSTAMP+TINTS
TYPE 101,TIME
101 FORMAT(/,' Time:',F10.5,'[s]')
TYPE 10006,(I,I=1,N)
10006 FORMAT(' No. of joint ',6(4X,I1,5X»
TYPE 10007
10007 FORMAT(' Deviations of the actuator state coordinates')
TYPE 10008
10008 FORMAT(' Angles [rad] or [m]:' ,$)
II=l
DO 4097 I=l,N
TYPE 4098 ,DY( II)
4097 II=I I+NI ( I)
4098 FORMAT('+',$,E10.3)
TYPE 4049
4049 FORMAT( '+',' ,)
II=2
TYPE 10009
DO 4095 I=l,N
TYPE 4098,DY(II)
4095 II=I I+NI (I)
10009 FORMAT(' Ve1ocit.[rad/s-m/s]:',$)
TYPE 4049
TYPE 10010
II=3
DO 4099 I=l,N
IF(NI(I).EQ.2)GO TO 4096
TYPE 4098,DY(II)
GO TO 4099
4096 TYPE 4094
4099 II=II+NI ( I)
4094 FORMAT('+',$,10X)
10010 FORMAT(' Current/pressure[A]:' ,$)
TYPE 4049
TYPE 103,(P1(I),I=l,N)
103 FORMAT(' Driving torques[Nm]:',6E10.3)
TYPE l04,(U(I),I=l,N)
104 FORMAT(' Control signals [Vj:',6E10.3)
119 CONTINUE
C--------------------------------------------------------------------
C
C read data on nominal trajectory from the file ***.ANG -
C - at the instant VR2
C
READ(1,100,END=9000)VR2
READ(1,100)(YS02(I),I=1,N)
READ(1,100)(YS02(I),I=N+1,NN)
READ(1,100)(QU02(I),I=1,N)
C
POM1=VR2-VRl
C---------------------------------------------------------------------
C
C computation of nominal centralized or decentralized programmed
C control; incement simulation time for DINT
C
91 TIME=TIME+DINT
C
442

C interpolation along the nominal trajectory at the integration

C intervals between the moments VR2 in which the nominal
C trajectory has been computed and memorized in the file ***.ANG
C
POM=(TIME-VRI)/POMI
C
DO 7777 I=l,NUK
7777 DY(I)=YSP(I)-YSOP(I)
C
DO 3 I=l,NN
3 YO(I)=(YS02(I)-YSOl(I))*POM+YSOl(I)
C
C call MODEL to calculate matrix H and vector h for the
C nominal values of joints angles and velocities
C
DO 4 I=l,N
QUO(I)=(QU02(I)-QUOl(I))*POM+QUOl(I)
SI(I)=YO(I)
4 SIDOT( I) =YO (I+N)
C
CALL MODEL
C
C computation of the nominal driving torques
C and the nominal control
C
DO 5 I=l,N
PO(I)=Hl(I)
Hll ( I ) =Hl ( I )
DO 5 J=l,N
5 PO(I)=PO(I)+HH(I,J)*QUO(J)
C
II=2
DO 6 I=l,N
YSO(II-!)=YO(I)
YSO( II )=YO( I+N)
IF(NI(I).EQ.2)GO TO 67
III=II+l
C
C if the actuator order is 3
C
IF(IOPCNM(I).EQ.O)GO TO 349
C
C computation of the nominal value of the third state coordinate
C of the i-th actuator-if the computation of the nominal driving
C torque is included in the computation of the nominal control
C
YSO(III)=(QUO(I)-A(2,2,I)*YSO(II)-F(2,I)*PO(I))
1 2,3, I)
/A(
C
C computation of the centralized nominal control
C
UO(I)=((YSO(III)-YSOP(III))/DINT -A(3,2,I)*YSOP(II)-
1 A(3,3,I)*YSOP(III))/B(3,I)
GO TO 6
C
C if the nominal driving torque is not introduced in the nominal
C control (if local nominal control is required)
C
349 YSO(III)=(QUO(I)-AHM(2,2,I)*YSO(II))/AHM(2,3,I)
C
C computation of the local nominal control
 443

C
UO(I)=((YSO(III)-YSOP(III))/DINT-AHM(3,2,I)*YSOP(II)-
1 AHM(3,3,I)*YSOP(III))/B(3,I)
GO TO 6
C
C if the order of the i-th actuator model is 2
C
67 UO(I)=(QUO(I)-AHM(2,2,I)*YSOP(II)-IOPCNM(I)*FHM(2,I)*(POP(I)-
1 HMIN(I)*QUO(I)))/BHM(2,I)
6 II=II+NI(I)
C-------------------------------------------------------------------
C
C SIMULATION - computation of the right hand sides of the diff.
C equations of the stat0 nonlinear model of the robot
C
11=1
DO 7 I=l,N
SI(I)=YSP(II)
SIDOT(I)=YSP(II+1)
7 II=II+NI(I)
C
C call MODEL to compute matrix H and vector h for the actual
C joints angles and velocities
C
CALL MODEL
C-------------------------------------------------------------------
C
C computation of control signals for all actuators according
C to the selected control law (DX(3)-deviation of state coord.
C of the i-th actuator from the nominal trajectories)
C
11=1
111=2
11=3
DO 8 I=l,N
DX(l)=DY(II)
DX(2)=DY(III)
IF(NI(I).EQ.3)DX(3)=DY(Il)
C
C computation of local control signals
C
CALL SMCONT(I,U1,DX)
DYS(II)=A(1,2,I)*YSP(III)
C
C computation of the selected global feedback gains
C
CALL SMCONG(I,PKG1,U1,DX)
U1=U1+IOPCNN(I)*UO(I)
C
C PKGP(6) - vector of the global feedback gains computed
C for the actual state of the actuator (susbsystem)
C
U(I)=U1
PKGP(I)=PKG1
C
C HP - auxiliary vector for computation of the right sides
C of differential equations of the robot model-the second
C equations in the models of actuators in the state space
C
HP(I)=A(2,2,I)*YSP(III)+F(2,I)*H1(I)+A(2,3,I)*YSP(I1)
IF(NI(I).EQ.3)GO TO 68
444

C
C if the order of actuator model is 2
C
IF(IOPTG(I).EQ.O) GO TO 69
C
C computation of the global control according to the
C selected option
C
Ul-Ul+PKGl*(IOPTGH(I)*(HlO(I)-POP(I))*IOPCNM(I)+
* IOPTGG(I)*(Hl(I)-IOPCNM(I)*HlO(I)))
C
C amplitude constraints upon the actuator inputs
C
69 IF(Ul.GT.UMAX(l,I))Ul-UMAX(l,I)
IF(U1.LT.-UMAX(2,I))Ul=-UMAX(2,I)
C
U(I)=Ul
HP(I)=HP(I)+B(2,I)*Ul
68 II=II+NI(I)
III=II+l
I1=III+1
8 CONTINUE
C
C end of control computation
C (at current integration interval)
C----------------------------------------------------------------
C
C computation of the auxiliary matrix HPOM
C
DO 70 I=l,N
DO 70 J=l,N
70 HPOM(I,J)=-HH(I,J)*(F(2,I)+B(2,I)*PKGP(I)*IOPTGH(I))
DO 71 I=l,N
71 HPOM(I,I)=HPOM(I,I)+l.
C
C matrix inversion
C
CALL SMMINV(HPOM,HPOM3)
C
C multip1iaction by the velocity dependent moments
C
II=2
DO 72 I=l,N
DYS(II)=O.
DO 73 J=l,N
73 DYS(II)=DYS(II)+HPOM(I,J)*HP(J)
72 II=II+NI(I)
C
C computation of the actual joints torques
C
DO 74 I=l,N
II=2
Pl(I)-Hl(I)
DO 74 J=l,N
Pl(I)=Pl(I)+HH(I,J)*DY5(II)
74 II-II+NI(J)
C
11=0
DO 75 I=l,N
II=II+NI (I)
C
 445

C computation of the actual power and energy consumptions

C
SPOWER(I)=ABS(Pl(I)*YO(I+N»
SENERG(I)=SENERG(I)+SPOWER(I)*DINT
IF(NI(I).EQ.2)GO TO 75
C
C if the order of the actuator is 3 -add the global control
C
IF(IOPTGH(I).EQ.O) GO TO 76
U(I)=U(I)+PKGP(I)*(IOPTGH(I)*(Pl(I)-Hl(I)-
1 IOPCNM(I)*(POP(I)-HlO(I»)+IOPTGG(I)*
2 (Hl(I)-HlO(I)*IOPCNM(I»)
C
C amplitude constraints upon the actuator inputs if the
C order of the actuator model is 3
C
76 IF(U(I).GT.UMAX(l,I»U(I)=UMAX(l,I)
IF(U(I).LT.-UMAX(2,I»U(I)=-UMAX(2,I)
C
C computation of the first derivative of the third state
C coordinate of the i-th actuator model (if its order is 3)
C
DYS(II)=A(3,2,I)*YSP(II-1)+A(3,3,I)*YSP(II)+B(3,I)*U(I)
75 CONTINUE
C----------------------------------------------------------------
C
DO 90 I=l,NUK
C
C memorize the nominal state vector at the previous interval
C
YSOP(I)-YSO(I)
c
C numerical integration by Euler's method
C
90 YSP(I)=YSP(I)+DYS(I)*DINT
C-----------------------------------------------------------------
C
C memorize nominal torques from the previous integration
C interval
C
DO 92 I=l,N
H10(I)=H11(I)
POP(I)=PO(I)
92 CONTINUE
C
C memorize the values of the nominal trajectory at the
C previous point if the current time has reached the next
C time instant in which nominal trajectory has been
C imposed in file ***.ANG
C
IF(TIME.LT.(VR2-0.5*DINT»GO TO 91
DO 93 I=l,N
QU01(I)=QU02(I)
II=I+N
YS01(I)=YS02(I)
93 YS01(II)=YS02(II)
VRl-VR2
C
GO TO 2
C
C end of simulation cycle
C*******************·w.***************************************
446

C
9000 CONTINUE
CLOSE(UNIT=l)
CLOSE(UNIT=2)
TYPE *,' WARNING*** End of simulation'
RETURN
END

C----------------------------------------------------- ----------------
C SUBROUTINE: SMOPTN
C---------------------------------------------------------------------
C FUNCTION: INTERACTIVE SPECIFICATION OF THE CONTROL LAW
C OPTIONS FOR SIMULATION OF THE ROBOT DYNAMICS
C---------------------------------------------------------------------
C INPUT VARIABLES:
C N - number of joints of the user's robot
C----------------------------------------------------- ----------------
C OUTPUT VARIABLES:
C YSINT - deviation of the initial state from
c the nominal trajectory
C OPTIONS:
C IOPCNN(I)- nominal control in the i-th joint
C to be applied or not (1-yes,0-no)
C IOPCNM(i)- type of nominal control in the
C i-th joint (1-centralized,0-local)
C---------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C SMINGL - enables specification of global
C control
C---------------------------------------------------------------------
SUBROUTINE SMOPTN
C
INCLUDE 'IN2:SMCOM.COM'
C
COMMON/OPTNOM/IOPCNM(6),IOPCNN(6)
C---------------------------------------------------------------------
C
2000 FORMAT(A1)
TYPE 101
101 FORMAT(' Want simulation with the already adopted control law'
1 ,/,$,' (otherwise you have to specify it) [YIN]?:')
ACCEPT 2000,YES
IF(YES.EQ.'Y')GO TO 2
TYPE 102
102 FORMAT(' Want to apply nominal control in any joint'
1 ,/,$,' (either centralized or decentralized) [YIN]?:')
ACCEPT 2000,YES
IF(YES.EQ.'Y' )GO TO 1
DO 33 I=1,N
33 IOPCNN(I)=O
GO TO 5
1 DO 34 I=l,N
TYPE 103,1
103 FORMAT($,' Want to apply nominal control in the ',13,
1 '-th joint [YIN]?:')
 447

ACCEPT 2000,YES
IF(YES.NE.'Y')GO TO 3
IOPCNN(I)=l
TYPE 104,1
104 FORMAT($,' Want centralized nominal control in the',I3,'-th
1 joint [YIN]?:')
ACCEPT 2000,YES
IOPCNM(I)=O
IF(YES.EQ.'Y')IOPCNM(I)=l
GO TO 34
3 IOPCNN(I)=O
34 CONTINUE
C
5 TYPE 105
105 FORMAT($,' want to apply global control [YIN]?:')
ACCEPT 2000,YES
IF(YES.EQ.'Y')CALL SMINGL
GO TO 6
C

2 DO 35 I=l,N
IOPCNN(I)=l
35 IOPCNM(I)=l
C
6 TYPE 106
106 FORMAT($,' want to specify some specific initial conditions
1 [YIN]?:')
ACCEPT 2000,YES
IF(YES.NE. 'Y' ) RETURN
TYPE 107
107 FORMAT(' Specify initial conditions (for joint angles) [rad]
1 or [m]:')
DO 37 I s 1,NUK
37 YSINT(I)=O.
C
II=l
DO 36 I=l,N
TYPE 108,1
108 FORMAT($,' Joint',I3,'.')
ACCEPT 2001,YSINT(II)
II=II+NI (I)
2001 FORMAT(F10.5)
36 CONTINUE
RETURN
END
448

C---------------------------------------------------------------------
C SUBROUTINE: SMINGL
C---------------------------------------------------------------------
C FUNCTION: INTERACTIVE SPECIFICATION OF THE GLOBAL
C CONTROL LAW FOR SIMULATION - THE USER HAS
C TO SPECIFY A FORM OF THE GLOBAL CONTROL
C AND THE GLOBAL FEEDBACK GAIN
C---------------------------------------------------------------------
C INPUT VARIABLE:
C N number of joints of the user's robot
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C PKG(I) - attenuation of the global control in the
C i-th joint
C OPTIONS: (for each joint)
C IOPTG(i) =0 global control is not to be applied in
C the i-th joint
C =1 the global gain is in the form:
C PKG(i)*(gradv*F)/(gradv*B)
C =2 the global gain is in the form:
C PKG(i)*F(2)/B(2)
C IOPTGF(i) =0 force feedback is not to be applied
C in the i-th joint
C =1 force feedback is applied
C IOPTGG(i) - the global control is in the form of
C "on line" computation of gravity, centri-
C fugal, and Coriolis moments around the
C i-th joint (0 -no, 1 -yes)
C IOPTGH(i) - the global control is in the form of
C "on-line" computation of inertial moments
C around the i-th joint (0 - no, 1 -yes)
C---------------------------------------------------------------------
SUBROUTINE SMINGL
C
COMMON/OPTION/IOPTOR(6),IOPTV(6),IOPTI(6),
1 IOPTG(6),IOPTGF(6),IOPTGG(6),IOPTGH(6)
COMMON/GAINS/ PK1(6),~K2(6),PK3(6),PKG(6)
COMMON/ORDGLB/ NUK,N
C---------------------------------------------------------------------
C
DO 666 I=1,N
IOPTG(I)=O
IOPTGF(I)=O
IOPTGG(I)=O
666 IOPTGH(I)=O
C
TYPE 901
901 FORMAT(' Want to apply equal global control law in all joints'
1 ,/,$,' [Y/N]?:')
ACCEPT 2000,YES
2000 FORMAT(A1)
IF(YES.EQ.'Y')GO TO 100
C
DO 10 I=l,N
TYPE 9000,1
9000 FORMAT($,' Want global control in the ',13,
1 '-th joint [YIN]?:')
ACCEPT 2000,YES
IF(YES.EQ.'Y')IOPTG(I)=1
 449

C
IF(IOPTG(I).EQ.O)GO TO 10
TYPE 902,1
902 FORMAT($, , want force feedback in the ',13,
1 '-th joint [YIN]?:')
ACCEPT 2000,YES
IF(YES.EQ.'Y')IOPTGF(I)=l
IF(YES.EQ.'Y')GO TO 10
C
TYPE 903,1
903 FORMAT($,' want "on-line" computation of gravity,',I,$
1 'centrifugal and Corio1is moments in the ',13,
2 '-th joint [YIN]?:')
ACCEPT 2000,YES
1F(YES.EQ.'Y' )10PTGG(I)=l
1F(YES.EQ.'Y')GO TO 10
C
TYPE 9004,1
9004 FORMAT(' Want "on-line" computation of inertial moments',I,
1 $,' in the ',I3,'-th joint [YIN]?:')
ACCEPT 2000,YES
1F(YES.EQ.'Y')10PTGH(1)=l
10 CONTINUE
GO TO 500
C
100 TYPE 9006
9006 FORMAT($,' want force feedback in all joints [YIN]?:')
ACCEPT 2000,YES
1F(YES.NE.'Y')GO TO 200
DO 20 1=l,N
10PTG(1)=l
20 10PTGF(I)=l
GO TO 500
C
200 DO 25 1=l,N
25 10PTGF(I)=O
TYPE 9016
9016 FORMAT(' Want "on-line" computation of gravity,
1 centrifugal and coriolis moments' ,I,
2 $,' in all joints [YIN]?:')
ACCEPT 2000,YES
IF(YES.NE.'Y')GO TO 300
DO 30 I=l,N
IOPTG(I)=l
30 10PTGG(I)=l
GO TO 500
C
300 DO 35 I=l,N
35 10PTGG(I)=O
C
TYPE 9007
9007 FORMAT(' Want "on-line" computation of inertial moments',
1 1,$,' in all joints [YIN]?:')
ACCEPT 2000,YES
1F(YES.NE.'Y')GO TO 700
DO 40 I=l,N
10PTG(I)=l
40 10PTGH(1)=l
C
450

500 DO 501 I=l,N

IF(IOPTGF(I).EQ.O) GO TO 501
IOPTGG(I)=l
501 IOPTGH(I)=l
TYPE 9008
9008 FORMAT($,' want equal attenuation of global control in
1 all joints [YIN]?:')
ACCEPT 2000,YES
IF(YES.EQ.'Y')GO TO 600
DO 60 I=l,N
TYPE 9009,1
9009 FORMAT($,' Specify global attenuation for the ',13,
1 '-th joint [0.-1.]:')
60 ACCEPT 3000,PKG(I)
3000 FORMAT(F10.5)
GO TO 61
C
600 TYPE 9010
9010 FORMAT($,' Specify global attenuation for all joints
1 [0.-1.]:' )
ACCEPT 3000,POM
DO 70 I=l,N
70 PKG(I)=POM
GO TO 61
700 DO 80 I=l,N
IOPTGH(I)=O
80 IOPTG(I)=O
C
61 TYPE 800
800 FORMAT($,' want simplified form of the global control
1 gain [YIN]?:')
ACCEPT 2000,YES
IF(YES.NE.'Y')GO TO 6633
DO 90 I=l,N
90 IOPTG(I)=2
C
6633 DO 6644 I=l,N
IF(IOPTOR(I).EQ.O) THEN
IOPTG(I)=2
TYPE 400, I
END IF
IF(PKG(I).NE.O)GO TO 6644
IOPTG(I)=O
IOPTGF(I)=O
IOPTGG(I)=O
IOPTGH(I)=O
6644 CONTINUE
C
400 FORMAT (' DIAG*** Since in the' ,I2,'-th joint optimal',
> I,' regulator has not ben synthesized you must',
> I,' select simplified form of global gain')
RETURN
END
 451

C---------------------------------------------------------------------
C SUBROUTINE: SMCONT
C---------------------------------------------------------------------
C FUNCTION: COMPUTES THE LOCAL CONTROL SIGNAL FOR THE
C I-TH ACTUATOR AND FOR GIVEN STATE VECTOR
C---------------------------------------------------------------------
C INPUT VARIABLES:
C I the order number of the actuator(subsystem)
C OX deviation of the actual state vector (during
C simulation) from the nominal trajectory for
C the i-th subsystem (DX(l) - position error,
C DX(2)-velocity error, DX(3)- error in rotor
C current, or pressure)
C PKl(I) position local feedback gain
C PK2(I) velocity "
C PK3(I) gain in feedback loop by current/pressure
C PK4(I) integral feedback gain
C DINT - integration interval
C--------------------------------------------------------------------
C OUTPUT VARIABLE:
C Ul local control signal for the i-th subsystem
C for the given OX
C--------------------------------------------------------------------
SUBROUTINE SMCONT(I,Ul,DX)
C
COMMON/GAINS/ PKl(6),PK2(6),PK3(6),PKG(6),PK4(6)
COMMON/ORDGLB/ NUK,N
C
DIMENSION DX(3)
COMMON/ACTORD/ NI(6)
COMMON/INTI NT/ DINT
COMMON/POMINT/ POMINT(6)
C
C POMINT(I)-auxiliary vector for the numerical integration
C necessary to implement feedback loop by integral
C of the position error DX(l)
C
C--------------------------------------------------------------------
C
Ul=-PKl(I)*DX(l)-
1 PK2(I)*DX(2)
2 -PK3(I)*DX(3)
3 -PK4(I)*POMINT(I)
C
POMINT(I)aPOMINT(I)+DX(l)*DINT
RETURN
END
452

C---------------------------------------------------------------------
C SUBROUTINE: SMCONG
C---------------------------------------------------------------------
C FUNCTION: COMPUTES GLOBAL CONTROL GAIN FOR THE I-TH
C JOINT - ACTUATOR
C---------------------------------------------------------------------
C INPUT VARIABLES:
C I - the order number of the actuator/joint
C (subsystem)
C DX(3) - deviation of the i-th subsystem state
C from the nominal trajectory
C ul - local control signal for the i-th subsystem
C F(3,I)- load distribution vector in the i-th actu-
C ator model
C B(3,I)- input distribution vector in the state model
C of the i-th actuator
C NI(I) - order of the i-th subsystem
C V - Liapunov's function value for the i-th sub-
C system for the given state vector DX(3)
C GRADV(3) - gradient of the Liapunov's function for
C the i-th subsystem for the given DX
C IOPTG(I) - type of the global gain for the i-th
C subsystem (see subroutine SMINGL)
C PKG(I) - global control attenuation in the i-th
C joint - user's selection (see SMINGL)
C--------------------------------------------------------------------
C OUTPUT VARIABLE:
C PKGl - global gain for the i-th subsystem
C--------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C LIAPIS - computes the value of the subsystem
C Liapunov's function for the given DX
C GRADIS - computes the gradient of the subsystem
C Liapunov's function for the given DX
C--------------------------------------------------------------------
SUBROUTINE SMCONG(I,PKGl,Ul,DX)
C
INCLUDE 'IN2:SMCOM.COM'
C
DIMENSION DX(3),GRADV(3)
C--------------------------------------------------------------------
C
C if IOPTG(i) is equal to 0 - the global control is not
C applied in the i-th joint
C
IF(IOPTG(I).NE.O)GO TO 1
PKGl=O
RETURN
C
1 IF(IOPTG(I).EQ.2)GO TO 6
C
C call subroutines to compute Liapunov's function and
C its derivative for the i-th joint and given DX
C
CALL LIAPlS(DX,V,I)
CALL GRADlS(DX,GRADV,I,V)
C
C computation of GRADV*F and GRADV*B
C
 453

FG=O.
BG=O.
DO 2 J=l,NI(I)
FG=FG+GRADV(J)*F(J,I)
2 BG=BG+GRADV(J)*B(J,I)
C
C if GRADV*B is smaller than 0.001- it is constrained to 0.001
C
IF(ABS(BG).GT.0.001)GO TO 50
BG=SIGN(O.OOl,BG)
C
C computation of the global gain according to:
C GRADV*F/(GRADV*B) multiplied by attenuation PKG(I)
C specified by the user
C
50 PKG1=-PKG(I)*FG/BG
RETURN
C
C if the user has selected the simplified form of
C global gain IOPTG(I)=2
C
6 IF(NI(I).EQ.2)PKG1=-PKG(I)*F(2,I)/B(2,I)
IF(NI(I).EQ.3)PKG1=-PKG(I)*F(2,I)/B(3,I)
RETURN
END

C---------------------------------------------------------------------
C SUBROUTINE: LIAP1S
C---------------------------------------------------------------------
C FUNCTION: COMPUTES THE LIAPUNOV'S FUNCTION FOR THE
C I-th SUBSYSTEM FOR THE GIVEN VALUE OF THE
C SUBSYSTEM STATE VECTOR
C---------------------------------------------------------------------
C INPUT VARIABLES:
C I - the order number of the subsystem
C DX - the state of the i-th susbsystem (deviation of
C the state vector from the nominal)
C NI(I) - the order of the subsystem model
C RK(4,4,I) - Liapunov's matrix for the i-th sub-
C system (solution of the Riccati's
C algebraic equation - see subroutine
C SMOREG)
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C V - the Liapunov's function for the i-th subsystem
C for the given state vactor value DX
C---------------------------------------------------------------------
C SUBROUTINES REQUIRED:
C GMPRD - SSP subroutine for matrix multiplication
C---------------------------------------------------------------------
SUBROUTINE LIAP1S(DX,V,I)
C
DIMENSION DX(3),HPOM(3,3)
DIMENSION DGX(3)
4~

C
COMMON/SUBLIAI RK(4,4,6)
COMMON/ACTORDI NI(6)
C---------------------------------------------------------------------
C
C HPOM - auxiliary matrix
C
DO 1 J=1,NI(I)
DO 1 JJ=1,NI(I)
1 HPOM(J,JJ)=RK(J,JJ,I)
NN=NI(I)
C
C multiplication of the Liapunov's matrix by the state OX
C
CALL GMPRD(HPOM,DX,DGX,NN,NN,1)
C
C multiplication by the transpose of OX
C
V=O.
DO 2 J=1,NI(I)
2 V=V+DX(J)*DGX(J)
C
IF(V.GE.O)GO TO 200
TYPE 201
201 FORMAT(' DIAG*** Local controller is not synthesized as',l,
1 local optimal regulator, and',l,
2 therefore simplified form of the global',l,
3 gain must be selected')
CALL EXIT
C
200 V=SQRT(V)
RETURN
END

C---------------------------------------------------------------------
C SUBROUTINE: GRAD1S
C---------------------------------------------------------------------
C FUNCTION: COMPUTES GRADIENT OF THE GIVEN LIAPUNOV
C FUNCTION OF THE ij-th SUBSYSTEM ANO
C FOR THE GIVEN SUBSYSTEM STATE VECTOR
C---------------------------------------------------------------------
C INPUT VARIABLES:
C IJ - the order number of the subsystem
C ox - the subsystem state vector
C V - value of the subsystem Liapunov'S function
C for the given state vector OX
C NI(ij) - order of the ij-th subsystem
C RK(3,3,IJ) -Liapunov's matrix for the ij-th
C subsystem
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C DGX - the gradient of the Liapunov's function
C of the ij-th subsystem for the given OX
C---------------------------------------------------------------------
C SUBROUTINE REQUIRED:
C GMPRO - SSP subroutine for matrix multiplication
 455

C---------------------------------------------------------------------
SUBROUTINE GRADlS(DX,DGX,IJ,V)
C
DIMENSION DX(3),DGX(3),HL(3,3)
COMMON/ACTORD/ NI(6)
COMMON/SUBLIA/ RK(4,4,6)
C---------------------------------------------------------------------
C
C put the Liapunov's matrix in the auxiliary matrix HL
C
NN=NI(IJ)
DO 1 I=l,NN
DO 1 J=l,NN
1 HL(I,J)=RK(I,J,IJ)
C
C multiplication of the Liapunov's matrix by the value
C of the subsystem state vector DX
C
CALL GMPRD(HL,DX,DGX,NN,NN,I)
C
C multiplication of DGX by l./V
C
IF(V.LT.O.OOOOOl) THEN
v=O.OOOOOl
END IF
DO 2 I=l,NN
2 DGX(I)=DGX(I)/V
RETURN
END

C---------------------------------------------------------------------
C SUBROUTINE: SMMINV
C---------------------------------------------------------------------
C FUNCTION: AUXILIARY SUBROUTINE WHICH CALLS THE
C SSP SUBROUTINE MINV FOR THE MATRIX
C INVERSION (for various matrix dimen-
C sions)
C Note: This subroutine is not obligatory
C it could be omitted if matrix H is dimen-
C sioned in different way - depending
C on the FORTRAN compiler
C---------------------------------------------------------------------
C INPUT VARIABLES:
C N - the matrix order
C HPOM - the matrix to be inverted
C---------------------------------------------------------------------
C OUTPUT VARIABLES:
C HPOM3 - inverse of the matrix HPOM
C---------------------------------------------------------------------
C SUBROUTINE REQUIRED:
C MINV - SSP subroutine for the matrix inversion
C---------------------------------------------------------------------
SUBROUTINE SMMINV(HPOM,HPOM3)
C
456

COMMON/ORDGLB/ NUK,N
DIMENSION HPOM3(N,N),HPOM(6,6)
DIMENSION LL(6),MM(6)
C
DO 1 I=l,N
DO 1 J=l,N
1 HPOM3(I,J)=HPOM(I,J)
C
CALL MINV(HPOM3,N,D,LL,MM)
C
DO 2 I=1,N
DO 2 J=1,N
2 HPOM(I,J)=HPOM3(I,J)
RETURN
END

C
C INCLUDE FILE
C 'DL1:[107,21]SMCOM.COM'
C
C Fail includes COMMON - areas
C
COMMON/SUBSYS/ A(3,3,6),B(3,6),F(3,6)
COMMON/ORDGLB/NUK,N
COMMON/ACTORD/ NI(6),KI(6)
C
COMMON/SUBSHM/ AHM(3,3,6),BHM(3,6),FHM(3,6)
COMMON/HMAX/ HMAX(6)
COMMON/HMIN/ HMIN(6)
C
COMMON/STDGIM/ ALFAI(6),ALFAIM
C
COMMON/INTINT/ DINT
C
COMMON/OPTION/IOPTOR(6),IOPTV(6),IOPTI(6),IOPTG(6)
COMMON/OPTION/IOPTGF(6),IOPTGG(6),10PTGH(6),10PT1N(6)
COMMON/SUBCRT/ QQ(3,3,6),R1(6),Q1(6)
COMMON/GAINS/ PK1(6),PK2(6),PK3(6),PKG(6),PK4(6)
COMMON/UMAX/ UMAX(2,6),UMAX1(6)
COMMON/UM/ UQMAX(2,6),UDMAX(2,6)
COMMON/SUBL1A/ RK(4,4,6)
COMMON/CTIM1N/ T1M1N(60),ITM1N
C
COMMON/STIN1T/ YSINT(18)

Fig. 1 1m Fig. 2 1m
I I
I I
I I
I I
I I
S3 I *S2 : I
-*-{j-------+-----*-----+--> --------{j-----+-----*----+-->
S1=S2 I Re *S3 :wO S1 I Re
I I

A. Auxiliary file CONF.DAT

 457

6 ; defauld data
3
.OOOOOE 00 .10000E 01 .OOOOOE 00 ;auxiliary data
.OOOOOE 00 -.11320E 02 .47S00E 02 ion actuators
.OOOOOE 00 -.11333E 04 -.66178E 02
.OOOOOE 00 .OOOOOE 00 .74900E 02
.OOOOOE 00 -.03140E 02 .OOOOOE 00
3
.OOOOOE 00 .10000E 01 .OOOOOE 00
.OOOOOE 00 -.11320E 02 .47S00E 02
.OOOOOE 00 -.11333E 04 -.66178E 02
.OOOOOE 00 .OOOOOE 00 .74900E 02
.OOOOOE 00 -.188S0E 01 .OOOOOE 00
3
.OOOOOE 01 .!OOOOE 01 .OOOOOE 00
.OOOOOE 00 -.97700E 01 .4l040E 02
.OOOOOE 00 -.8S000E 03 -.49629E 02
.OOOOOE 00 .OOOOOE 00 .S6100E 02
.OOOOOE 00 -.32S00E 00 .OOOOOE 00
3
.OOOOOE 00 .10000E 01 .OOOOOE 00
.OOOOOE 00 -.16600E 03 .BOOOOE 03
.OOOOOE 00 -.30000E 03 -.BOOOOE 02
.OOOOOE 00 .OOOOOE 00 .BOOOOE 02
.OOOOOE 00 -.33000E 03 .OOOOOE 00
3
.OOOOOE 00 .10000E 01 .OOOOOE 00
.OOOOOE 00 -.16600E 03 .BOOOOE 03
.OOOOOE 00 -.30000E 03 -.BOOOOE 02
.OOOOOE 00 .OOOOOE 00 .BOOOOE 02
.OOOOOE 00 -.33000E 03 .OOOOOE 00
3
.OOOOOE 00 .10000E 01 .OOOOOE 00
.OOOOOE 00 -.16600E 03 .BOOOOE 03
.OOOOOE 00 -.30000E 03 -.BOOOOE 02
.OOOOOE 00 .OOOOOE 00 .BOOOOE 02
.OOOOOE 03 -.33000E 03 .OOOOOE 00
.10000E 01 ;weighting matrix Q
.10000E 00 ;for optimal regulator
.10000E-01
.10000E 00 .SOOOOE 01 .SOOOOE 01 ;weighting elements
.SOOOOE 01 . SOOOOE 01 .SOOOOE 01 ;by control for reg .
.00000E-04 .00000E-02 .10000E-04 ;weighting elemnst
.10000E-04 . 10000E-04 .10000E-04 ;by integral coord .
1 ;options on feedback
1 ;loops
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
1
A· ~xample - input - file QMSPR.DAT
458

3
0.000000 itype of joint- KSI2 1
0.000000 i joint unit axis- ei (x) 1
0.000000 ijoint unit axis- ei (y) 1
1.000000 ijoint unit axis- ei (z) 1
0.000000 ii+1 joint unit axis- ei, +1 (x) 1
0.000000 ii+1 joint unit axis- ei, +1 (y) 1
1. 000000 ii+1 joint unit axis- ei, +1 (z) 1
0.300000 i1ink vectors- Rii (x) 1
0.000000 ilink vectors- Rii (y) 1
0.000000 ilink vectors- Rii (z) 1
-0.300000 ilink vectors- Ri,i+1 (x) 1
0.000000 ilink vectors- Ri,i+1 (y) 1
0.000000 ilink vectors- Ri,i+1 (z) 1
0.000000 ispec. vect. for lin. j.- ui (x) 1
0.000000 ispec. vect. for lin. j .- ui (y) 1
0.000000 ispec. vect. for lin. j .- ui (z) 1
0.000000 ispec. vect. for lin. j.- ui,i+1 (x) 1
0.000000 ispec. vect. for lin. j .- ui,i+1 (y) 1
0.000000 ispec. vect. for lin. j.- ui,i+1 (z) 1
0.000000 itype of joint- KSI2 2
0.000000 ijoint unit axis- eii (x) 2
0.000000 ijoint unit axis- eii (y) 2
1.000000 ijoint unit axis- eii (z) 2
0.000000 ii+1 joint unit axis- ei, +1 (x) 2
0.000000 ii+1 joint unit axis- ei, +1 (y) 2
1. 000000 ii+1 joint unit axis- ei, +1 (z) 2
0.000000 ilink vectors- Rii (x) 2
0.000010 i1ink vectors- Rii (y) 2
0.200000 ilink vectors- Rii (z) 2
0.000000 ilink vectors- Ri,i+1 (x) 2
0.000000 ilink vectors- Ri,i+1 (y) 2
-0.300000 ilink vectors- Ri,i+1 (z) 2
0.000000 ispec. vect. for lin. j .- uii (x) 2
0.000000 ispec. vect. for lin. j.- uii (y) 2
0.000000 ispec. vect. for lin. j.- uii (z) 2
0.000000 ispec. vect. for lin. j.- ui,i+1 (x) 2
0.000000 ispec. vect. for lin. j .- ui,i+1 (y) 2
0.000000 ispec. vect. for lin. j.- ui,i+1 (z) 2
1.000000 itype of joint- KSI2 3
0.000000 ijoint unit axis- eii (x) 3
0.000000 ijoint unit axis- eii (y) 3
1.000000 ijoint unit axis- eii (z) 3
0.000000 ii+1 joint unit axis- ei,i+1 (x) 3
0.000000 ii+1 joint unit axis- ei,i+1 (y) 3
0.000000 ii+1 joint unit axis- ei,i+1 (z) 3
0.000000 ilink vectors- Rii (x) 3
0.200000 ilink vectors- Rii (y) 3
0.300000 ilink vectors- Rii (z) 3
0.000000 ilink vectors- Ri,i+1 (x) 3
-0.200000 ilink vectors- Ri,i+1 (y) 3
0.000000 ilink vectors- Ri,i+1 (z) 3
1.000000 ispec. vect. for lin. j.- uii (x) 3
0.000000 ispec. vect. for lin. j .- uii (y) 3
0.000000 ispec. vect. for lin. j .- uii (z) 3
1. 000000 ispec. vect. for lin. i .- ui,i+1 (x) 3

A. Example - input - file UMSPR.CNF

 459

0.000000 ;spec. vect. for lin. j.- ui,i+1 (y) 3

0.000000 ;spec. vect. for lin. j.- ui,i+1 (z) 3
0.000000 First joint ax s-ext. coordinate(x)
0.000000 ; First joint ax s-ext. coordinately)
1.000000 ; First joint ax s-ext. coordinate(z)
A. Example - input - file UMSPR. CNF (cent.)

0.000000 ;type of link 1

7.000000 ;mass of link 1
0.000000 ;moment of inertia Jxx/Js 1
0.000000 ;moment of inertia Jyy/Jn 1
0.300000 ;moment of inertia Jzz 1
3.140000 ;qmax 1
-3.141000 ;qmin 1
0.000000 ;type of link 2
0.000010 ;mass of link 2
0.000000 ;moment of inertia Jxx/JS 2
0:000000 ;moment of inertia Jyy/Jn 2
0.000000 ;moment of inertia Jzz 2
3.141000 ;qmax 2
0.000000 ;qmin 2
0.000000 ; type of link 3
4.000000 ;mass of link 3
0.010000 ;moment of inertia Jxx/Js 3
0.010000 ;moment of inertia Jyy/Jn 3
0.000000 ;moment of inertia Jzz 3
0.500000 ;qmax 3
0.000000 ;qmin 3
A. Example - input - file UMSPR.DNM

DC DC DC
2.000000 ;Act.model order
1.500000 ;Mechanical constant
1.430000 ;E1ectromotor constant
0.000030 ;Rotor moment of inertia
0.005829 ;Viscous coefficient
1.600000 ;Rotor resistance
24.000000 ;Input amplitude constraint-upp. bound
24.000000 ;Input amplitude constraint-low bound
31.000000 ;Gear speed ratio
31.000000 ;Gear torque ratio
0.002300 ;Rotor inductivity
20.000000 ;Max. torque
10.000000 ;Motor power
0.000000 ;Auxiliary variable
0.000000 ;Auxiliary variable
2.000000 ;Act. model order
1.500000 ;Mechanical constant
1. 430000 ;Electromotor constant
0.000030 ;Rotor moment of inertia
0.005800 ;Viscous coeficient
1.600000 ;Rotor resistance
24.000000 ;Input amplitude constraint-upp. bound
24.000000 ;Input amplitude constraint-low bound
31.170000 ;Gear speed ratio
31.170000 ;Gear torque ratio

A. Example - input - file UMSPR.ACT

460

0.002300 ;Rotor inductivity

20.000000 ;Max. torque
100.000000 ;Motor power
0.000000 ;Auxi1iary variable
0.000000 ;Auxiliary variable
2.000000 ;Act. model order
125.000000 ;Mechanical constant
120.000000 ;Electromotor constant
0.000030 ;Rotor moment of inertia
40.000000 ;Viscous coeficient
1. 600000 ;Rotor resistance
24.000000 ;Input amplitude constraint-upp. bound
24.000000 ;Input amplitude constraint-low bound
2616.000000 ;Gear speed ratio
2616.000000 ;Gear torque ratio
0.002300 ;Rotor inductivity
20.000000 ;Max. torque
100.000000 ;Motor power
0.000000 ;Auxiliary variable
0.000000 ;Auxiliary variable

A. Example - input - file UMSPR.ACT (cant.)

0.0 0.4 O. init.p.

0.0 0.6 O. term. p.
0.01 0.6 samp. in.
durati.on

A. Example - input - file UMSPR.TRA

2 number of points for test

O.OOOOOE+OO instant at nom. traj.
3.00000E+00 prescribed stability degree
0.60000E+00 instant at nom. traj.
3.00000E+00 prescribed stability degree

A. Example - input - file UMSPR.INT

 461

THE ROBOT NAME [max. length 5 characters]: UMSPR

DIAG*·* File: UMSPR.CNF is found
CHARACTERISTIC KINEMATIC VARIABLES
Joint coordinates:
O.OOOOOE+OO O.OOOOOE+OO O.OOOOOE+OO
Transformation matrix of the terminal link An:
0.00000 1.00000 0.00000
-1.00000 0.00000 0.00000
0.00000 0.00000 1.00000
Joint axes with respect to absolute coord. frame
E(l)= 0.00000 0.00000 1.00000
E(2)= 0.00000 0.00000 1.00000
E(3)- 0.00000 0.00000 1.00000
Position vectors of the centers of masses and of joints
R(l,l}= 0.300 0.000 0.000 R(1,2)= -0.300 0.000 0.000
R(2,2)= 0.000 0.000 0.200 R(2,3)= 0.000 0.000 -0.300
R(3,3}= 0.200 0.000 0.300 RTO= -0.200 0.000 0.000
DIAG.** Assembling of the mechanism: SUCCESSFUL
DIAG •• * File: UMSPR.DNM has been found
want to compute maximal moments of inertia of the mechanism
(otherwise you have to directly specify them) [YIN]: Y
DIAG*** Computation of maximal moments of inertia
ax. mom. inert. H11max- 3.490068
in. mom. inert. H11min= 2.050324
ax. mom. inert. H22max- 0.160016
in. mom. inert. H22min- 0.160016

ax. mom. inert. H33max= 4.000003

in. mom. inert. H33min= 4.000001

A. Example - interaction between user and package

(bold letters are user's answers)
462

WARNING*** File: UMSPR.ACT is found

Actuator model in the jOint: 1

Matrix A:
0.000000 1.000000
0.000000 -46.703224
vector b:
0.000000
32.518211
vector f:
0.000000
-34.686089
Actuator model in the joint: 2
Matrix A:
0.000000 1.000000
0.000000 -46.194183
vector b:
0.000000
32.164471
vector f:
0.000000
-34.308769
Actuator model in the joint: 3
Matrix A:
0.000000 1.000000
0.000000 -45.858898
vector b:
0.000000
0.380534
vector f:
0.000000
-0.004871
Do you want to print the matrices of subsystems models
(which include the maximal mom. of inertia) [YIN): Y
Subsystem: 1
Subsystem matrix A:
0.00000 1.00000
0.00000 -0.38264
Subsystem vector b:
0.00000
0.26642
Subsystem vector f:
0.00000
-0.28418

Subsystem: 2
Subsystem matrix A:
0.00000 1.00000
0.00000 -7.11780
Subsystem vector b:
0.00000
4.95604

A. Example - interaction between user and package

 463

Subsystem vector f:
0.00000
-5.28644

Subsystem: 3
Subsystem matrix A:
0.00000 1.00000
0.00000 -44.98249
Subsystem vector b:
0.00000
0.37326
Subsystem vector f:
0.00000
-0.00478

Synthesis of local controllers around robot joints

Do you want synthesis of local optimal regulator

for the 1th joint [YIN)?: N
Specify the resonant structural frequency
of the mechanism [rad/s)
for the servo in 1 -th joint [rad/s): 12.

Local feedback servo gains:

Position servo gain KP(l)= 0.13513E+03
Velocity servo gain KV(1)= 0.43606E+02

Open-loop transfer function:

W(S)= 0.26642/(5**2+ 0.38264*S)

Closed-loop transfer function:

G(S)= 36.00000/(S**2+ 12.00000*S+ 36.00000)

Do you want synthesis of local optimal regulator

for the 2th joint [Y/N)?: N
Specify the resonant structural frequency
of the mechanism [rad/s)
for the servo in 2 -th joint [rad/s): 50.

Local feedback servo gains:

position servo gain KP(2)= 0.12611E+03
Velocity servo gain KV(2)= 0.86525E+01

A. Example - interaction between user and package

464

Open-loop transfer function:

W(S)= 4.95604/(S**2+ 7.11780*S)

Closed-loop transfer function:

G(S)- 625.00000/(S**2+ 50.00000*S+ 625.00000)

Do you want synthesis of local optimal regulator

for the 3th joint [YIN)?: N
Specify the resonant structural frequency
of the mechanism [rad/s)
for the servo in 3 -th joint [rad/s): 50.

Local feedback servo gains:

Position servo gain KP(3)= 0.16744E+04
Velocity servo gain KV(3)= 0.l3442E+02

Open-loop transfer function:

W(S)= 0.37326/(S**2+ 44.98249*S)

Closed-loop transfer function:

G(S)= 625.00000/(S**2+ 50.00000*S+ 625.00000)

want to write the feedback gains in the file [YIN) ?: Y

Synthesis of the joints nominal trajectories
want to print the nominal trajectories [YIN)?: N
Select the velocity profile-triangular or trapezoid [TA/TP): TA
Computation of the nominal dynamics of the robot
- nominal driving torques and nominal
programmed control -
want centralized or local nominal programmed control? [C/L): C
want to print the nominal dynamics [YIN)?: N

DO you want linear analysis [YIN)?: Y

Stability analysis

A. Example - interaction between user and package

 465

Eigen-values of the open-loop matrix of

linearized model of the robot in the time instant 0.00000 [sl
Eigen-values:
No. Real part Complex part Indic.
1 O.OOOOOE+OO O.OOOOOE+OO 2
2 -0.16505E+02 O.OOOOOE+OO 2
3 -0.43290E+00 O.OOOOOE+OO 2
4 -0.53679E-08 O.OOOOOE+OO 2
5 O.OOOOOE+OO O.OOOOOE+OO 2
6 -0.44982E+02 O.OOOOOE+OO 2

Eigen-values of the closed-loop matrix of

linearized model of the robot in the time instant 0.00000 [sl
Eigen-values:
No. Real part Complex part Indic.
1 -0.23935E+02 O.OOOOOE+OO 2
2 -0.34195E+01 O.OOOOOE+OO 2
3 -0.98590E+02 O.OOOOOE+OO 2
4 -O.14510E+02 O.OOOOOE+OO 2
5 -0.25000E+02 O.OOOOOE+OO 2
6 -0.25000E+02 O.OOOOOE+OO 2

Achieved stability degree of the robot -3.41955

Eigen-values of the open-loop matrix of

linearized model of the robot in the time instant 0.60000 [sl
Eigen-values:
No. Real part Complex part Indic.
1 -0.16519E+02 O.OOOOOE+OO 2
2 -0.43022E+00 O.OOOOOE+OO 2
3 0.16400E-10 O.OOOOOE+OO 2
4 0.272S1E-08 O.OOOOOE+OO 2
5 O.OOOOOE+OO O.OOOOOE+OO 2
6 -0.44982E+02 O.OOOOOE+OO 2

Eigen-values of the closed-loop matrix of

linearized model of the robot in the time instant 0.60000 [sl
Eigen-values:
No. Real part Complex part Indic.
1 -O.23929E+02 O.OOOOOE+OO 2
2 -0.3420SE+01 O.OOOOOE+OO 2
3 -0.98602E+02 O.OOOOOE+OO 2
4 -0.14S06E+02 O.OOOOOE+OO 2
5 -0.25000E+02 O.OOOOOE+OO 2
6 -0.25000E+02 O.OOOOOE+OO 2

Achieved stability degree of the robot -3.42049

A. Example - interaction between user and package

466

The linearized model is stabilized with

the prescribed exponential stability degree
Simulation of tracking of the nominal trajectory

WARNING*** File UMSPR.SIM already exists

want to form a file with new simulation [YIN)?: Y
Want simulation with the already adopted control law
(otherwise you have to specify it) [YIN)?: Y
want to specify some specific initial conditions [YIN)?: Y
Specify initial conditions (for joint angles) [rad) or [m): Y
Joint 1. -0.03
Joint 2. -0.03
Joint 3. -0.03
Want printings at the display during simulation [YIN)?: Y
How oten [s - FlO.5)?: 0.1
Time: O.OOOOO[s)
No. of joint 1 2 3
Deviations of the actuator state coordinates
Angles [rad) or [m):-0.300E-Ol-0.300E-Ol-0.300E-Ol
Ve1ocit.[rad/s-m/s): O.OOOE+OO O.OOOE+OO O.OOOE+OO
Current/pressure[A):
Driving torques[Nm): O.OOOE+OO O.OOOE+OO O.OOOE+OO
Control signals [V): O.OOOE+vv O.OOOE+OO O.OOOE+OO
Time: 0.10000[s)
No. of joint 1 2 3
Deviations of the actuator state coordinates
Angles [rad) or [m):-0.28lE-OI-0.l14E-OI-0.142E-01
Velocit.[rad/s-m/s): 0.611E-Ol 0.135E+00 0.195E+00
Current/pressure[A):
Driving torques[Nm): 0.229E+Ol 0.463E+00-0.429E+02
Control signals [V): 0.2S5E+Ol 0.104E+Ol 0.206E+02
Time: 0.20000[s)
No. of joint 1 2 3
Deviations of the actuator state coordinates
Angles [rad) or [m):-0.208E-OI-0.272E-02-0.246E-02
Ve1ocit.[rad/s-m/s): 0.766E-Ol 0.497E-Ol 0.48SE-Ol
Current/pressure[A):
Driving torques[Nm): 0.67SE+00 0.223E+00-0.428E+02
Control signals [V): 0.828E+00 0.999E+00 0.297E+Ol
Time: 0.30000[s)
No. of joint 1 2 3
Deviations of the actuator state coordinates
Angles [rad) or [m):-0.138E-Ol 0.134E-03-0.308E-03
Ve1ocit.[rad/s-m/s): 0.S99E-OI-0.194E-02 0.663E-02
Current/pressure[A):
Driving torques[Nm):-0.214E+01-0.391E+00-0.398E+02
Control signals [V):-0.221E+Ot 0.514E+00-0.7S1E-Ol

A. Example - interaction between user and package

 ~7

Time: 0.39998[s)
No. of joint 1 2 3
Deviations of the actuator state coordinates
Angles [rad) or [m]:-0.889E-02 0.672E-03-0.339E-04
Velocit.[rad/s-m/s): 0.404E-01 0.206E-02 0.761E-03
Current/pressure[A):
Driving torques[Nm]:-0.189E+01-0.454E+00-0.393E+02
Control signals [V):-0.196E+Ol 0.876E-OI-0.456E+00

Time: 0.49997[s]
No. of joint 1 2 3
Deviations of the actuator state coordinates
Angles [rad) or [m]:-0.558E-02 0.682E-03-0.349E-05
Velocit.[rad/s-m/s]: 0.264E-01-0.144E-02 0.801E-04
Current/pressure[A):
Driving torques[Nm]:-0.166E+01-0.423E+00-0.392E+02
Control signals [V]:-0.174E+01-0.202E+00-0.497E+00

Time: 0.59999[s]
No. of joint 1 2 3
Deviations of the actuator state coordinates
Angles [rad) or [m]:-0.346E-02 0.508E-03-0.344E-06
Velocit.[rad/s-m/s): 0.166E-01-0.195E-02 0.801E-05
Current/pressure[A):
Driving torques[Nm]:-0.150E+01-0.398E+00-0.392E+02
Control signals [V):-0.158E+01-0.495E+00-0.502E+00
WARNING*** End of simulation

want another simulation of robot [YIN]?: "

A. Example - interaction between user and package

135.12567
43.60567
0.00000
0.00000
126.10879
8.65252
0.00000
0.00000
1674.42932
13.44234
0.00000
0.00000

A. Example - output - file UMSPR.LQC

468

Subject Index

Active compliance, 353 Compliance center, 332

Actuators, 58, 259 Computed torque method, 264, 351
Adaptive control, 307 Computer-aided synthesis of control,
Adaptive robots, 2, 10 255, 363
Aggregation-decomposition method Coriolis forces, 54, 156
for stability analysis, 268 Critically damped, 89
Amplifier, 78 Cross-inertia members, 54, 155
Analogue implementation of Cylindrical structure of robot, 37
control, 17, 76
Analytical linearization of robot Damping factor, 89
model, 162 Decentralized adaptive control, 308
Analytic solution to inverse Decentralized Cartesian control,
kinematic problem, 34 276
Anthropomorphic structure of Decentralized control of robot, 71,
robot, 38 263
Approximate models of robot, 248 Decentralized form of model, 66
266 Decomposition, 208, 211
Arc welding, 12 Decoupling, 242
Artificial constraints, 340 Detector of error signal, 78
Assembly, 314 Dialogue robots, 2
Asymptotic stability, 170, 205,222 Direct compensation for gravity
Automatic planning, 349 moment, 112
Automatic robots, 2, 9 D.C. electro-motors, 58
Direct digital control, 22
Backlash, 121 Direct-drive actuators, 63
Biotechnical robots, Direct drive robots, 108, 183
Direct kinematic problem, 32
Cartesian (external) coordinates, 29 Dynamic control of robot, 233, 254
Cartesian based control, 273 Dynamic controller, 73, 126
Centralized adaptive control, 307 Dynamic hybrid control, 350
Centralized control structure, 263 Dynamic friction, 120
Centralized model, 66
Centralized nominal control, 190 Elastic effects, 301
Centralized optimal regulator, 287 Electro hydraulic, 58, 62, 88
Centrifugal effects, 54, 156 Estimate of stability region, 220
Characteristic frequence of servo, Executive control level, 5, 19
89 Explicit force control, 358
Closed kinematic chain, 214 Exponential stability, 172, 206
Compliance, 332, 353 External (Cartesian) coordinates, 4
 469

Feedforward, 136 Joint (internal) coordinates, 5, 29

Fine motion, 315
Flexible production cell, 14 Kinematic (tactical) control level,
Flexible technological line, 22, 27
14, 315
Flexible technological systems, Liapunov's direct method, 206
18, 295 Linearization of robot model, 159
Force control, 325 162
Force feedback for global control, Linear joint, 53
242 Linear model of robot, 160, 288
Force transducers, 243, 387 Local nominal programmed control,
Forging, 13 138, 179
Local optimal regulator, 147
Global control 237, 260, 281 Local servo system, 76, 259
Global feedback loops, 235
Gross motion, 315 Master-slave systems, 2
Mathematical model of actuators, 58
Hierarchical control, 3, 7, 8 79, 259
Hybrid implementation of control, Mathematical model of mechanical
17 part of robot, 53
Hybrid position/force control 339 Microprocessor implementation of
control, 17, 76, 84
Identification of parameters, Minimal configuration, 35
295, 307 Mobile robots, 15
Impendance control, 358 Model of state deviation, 139
Implicit force control, 357 Model-referenced adaptive control,
Indirect decentralized adaptive 308
control, 309
Interactive robot Natural constraint, 339
automated, 2 Nominal programmed control-centra-
supervised, 2 lized, 188, 259
dialogue, 2 Nominal programmed control-local,
Integration interval, 262 138, 179, 260
Intelligent robots, 2, 10 Nominal trajectory, 138
Interface motion, 317 Nonlinearities in servo, 118
Internal (joint) coordinates, 5 Nonlinear control, 291
Inverse dynamics method, 264 Numerical linearization of robot
Inverse kinematic problem, 33 model, 163
Numerical solution to inverse
Jacobian matrix, 39, 274 kinematic problem, 39
Joint based control, 273
470

On-line computation of dynamic Semi-automatic robots, 2

model for global control, 246 Self-tuning PID controller, 308
Off-line computation of nominal Sensor information processing, 18
control, 196 Servo control, 52, 79
Open kinematic chain, 314 Simulation, 260
Optimal control, 287 Spherical structure of robot, 38
Optimal regulator, 147, 287 Spherical wrist, 36
Overcritically damped servo, 89 Spot welding, 12
Overshoot, 89 Spray painting, 13
Stability analysis, 168, 204, 222
Parallel processing, 24, 247 Stanford manipulator, 96
Parts mating, 316 Static controller, 73
Passive compliance, 332 Static friction, 120
Path planning, 4 Strategic control level, 3
Payload parameters, 296 Structural resonant frequency, 92
Peg-in-hole task, 317 Steady state error, 91
Pneumatic actuators, 52 Stiffness, 91, 353
Pole placement method, 99 Stiffness control, 353
Position control, 168 Stiffness matrix, 356
PID controller, 127 Subsystem, 179
Position sensor, 81 Supervisory control, 3
Practical stability, 233, 181,225 Supervised robots, 2
Pre-compensator, 136
Pressure casting, 11 Tacho-generator, 81, 259
Programming language, 45 Tactical control level, 5, 19
Programmable robots, 2, 9 Task-space coordinate frame, 273
342
Real time control, 197 Teaching box, 18
Redundant manipulators, 34, 40 Trajectory control, 135, 177, 225
Repeatibility of servo, 120 Trajectory generation, 44
Reprogrammability, 17, 332 Trajectory tracking, 139
Robotic language, 6, 45
Robustness, 197, 243, 269, 291 Uncertainities of model, 269, 350
Rotational joint, 53 Undamped natural frequency, 89
Undercritically damped system,89
Sampling period, 22, 247, 260 Uniform asymptotic stability, 206
 M. Vukobratovic (Ed.)

Introduction
to Robotics
With contributions by M. Djurovic,
D. Hristic, B. Karan, M. Kircanski,
N. Kircanski, D. Stokic, D. Vuijic,
M. Vukobratovic
1989.228 figures. XIV, 301 pp.
Hardcover ISBN 3-540-17452-4

Contents: Preface. - General Introduction to

Robotics. - Manipulator Kinematic Model.
- Dynamics and Dynamic Analysis of Mani-
pulation Robots. - Hierarchical Control of
Robots. - Microprocessor Implementation of
Control Algorithms. - Industrial Robot
Programming Systems. - Sensors in Robot-
ics. - Elements, Structures and Application
ofIndustrial Robots. - Robotics and Flexible
Automation Systems. - Appendix.

This book provides a general introduction to

robot technology with an emphasis on robot
Springer-Verlag mechanisms and kinematics. It is conceived
Berlin Heidelberg New York
London Paris
as a reference book for students in the field
Tokyo Hong Kong of robotics.
 M. Vukobratovic

Applied Dynamics
of Manipulation Robots
Modelling, Analysis and Examples
1989. Approx. 495 pp. 176 figs. Hardcover
ISBN 3-540-51468-6

Contents: General About Robots. - Computer Form-

ing of Mathematical Model of Manipulation Robots
Dynamics. - Computer Method for Linearization and
Parameter Sensitivity of Manipulation Robots
Dynamic Models. - Connection Between the Moving
and Fixed System. - Manipulator Kinematical
Model. - Determining Velocities and Accelerations.
- Momentum of Rigid Body with Respect to a Fixed
Pole. - Specifities of Lever-Mechanisms Dynamics. -
Mathematical Models of Driving Units. - Automatic
Forming of Dynamic Models. - Dynamics of
"ASEA" Mechanism. - Programme Support for
Dynamics Modelling of Manipulation Robots.
- Subject Index.

This book is devoted to the study of manipulation

robot dynamics and its applications. It contains a
computational procedure for the automatic genera-
tion of mathematical models of robot dynamics,
comprising linearized models of robot dynamics and
parameter sensitivity models. The presentation is
complemented by a selection of problems and solu-
Springer-Verlag tions presenting mathematical models of different
Berlin Heidelberg New York
London Paris types of drives and examples of dynamic models of
Tokyo Hong Kong characteristic manipulation systems.