-A191 AUTOMATIC COMTOL OF ROBOT NOTION(U) NAVL … · distribution/ availability of abstract 21...

-A191 312 AUTOMATIC COMTOL OF ROBOT NOTION(U) NAVL POSTUOATE 1/~3SCHOOL MNTEREY CA G P KALOOIROS DEC 37

UN CLS IID F1 /O9 NL

JJWIO 28UhI'. I i.02

U0 12.0

% 8

IIII 4 Z~-OIIII~i~qij-%

.flI,=e v

q "

If'- FILE Co ,30

NAVAL POSTGRADUATE SCHOOLMonterey, California

DTE L-FE+,-

MAR 2 8 IY88

H

THESISAUTOMATIC CONTROL OF

ROBOT MOTION

by

George P. Kalogiros

December 1987

Thesis Advisor George J.Thaler

Approved for public release; distribution is unlimited.

3 2 .052

SECURITY CLASSIFICA TION 7717 7l' A

REPORT 0OCUMENTATION PAGEIs. REPORT SECURITY CLASSIFICATION l b. RESTRICTIVE MARKINGS

UNCLASS IFIED __________________

2a. SECURITY CLASSIFICATION AUTHORITY 3. DISTRIBUTION I AVAILABILITY OF REPORT

Zb.DECASSFIATIN IDOWGRDIN SC4EDLEApproved for public release;2b.0ECASIFIATIN DONGRDIG SHEDLEdistribution is unlimited.

4. PERFORMING ORGANIZATION REPORT NUMBER(S) 5. MONITORING ORGANIZATION REPORT NUMBER(S) '~

6a. NAME OF PERFORMING ORGANIZATION 6b. OFFICE SYMBOL 7a. NAME OF MONITORING ORGANIZATION

Naval Postgraduate School 62 Naval Postgraduate School

6c. ADDRESS (City, State, and ZIP Code) 7b. ADDRESS (City. State, and ZIP Cod.)

Monterey, California 93943-5000 Monterey, California 93943-5000

Ba. NAME OF FUNDINGi SPONSORING 8 b. OFFICE SYMBOL 9. PROCUREMENT INSTRUMENT IDENTIFICATION NUMBERORGANIZATION I (if appikabWe

BC. ADDRESS (City, State, and ZIP Code) 10. SOURCE OF FUNDING N4UMBERS

PROGRAM PROJECT TASK WORKJUNITELEMENT NO. NO. NO ACCESS ON NO.

11. TITLE (Include Secury Classification)

AUTOMATIC CONTROL OF ROBOT MOTION

12. PERSONAL AUTHOR(S)Kalogiros George, P.

FS7AT O~13a. TYPE OF REPORT 113b. TIME COVERED 14. DATE OF REPORT (Year, Month, Day) u.PG O~

Master's Thesis IFROM TO I 1987 December I 261 .

16. SUPPLEMENTARY NOTATION

17 COSATI CODES 18. SUBJECT TERMS (Continue on reverse if necessary and identify by block number) -

FIELD GROUP SUBI-GROUP Curve following velocity loop servoComputer adaptive model, Direcr-drive servo motorsRectangular and Revolute robot configurations

19. ABSTRACT (Continue on reverse it necessary and identify by block number)

2The feasibility of controlling the three link rectangular andrevolute robots with an adaptive computer simulation model, usinga curve following technique, is investigated.

Both configurations are tested for different load conditions,for rejection of random disturbances and for robustness in the 0case of servo motor parameter variations.

The interactive nonlinear dynamics of the revolute robot, suchas coupling inertia, actuator dynamics, centripetal and coriolis -

forces, are also investigated. First a gravity-free environment isassumed and then the robot arms are tested under gravitationaltorques. % %r

20. DISTRIBUTION/ AVAILABILITY OF ABSTRACT 21 ABSTRACT SECURITY CLASSIFICATION3 UNCLASSIFIED/UNLIMITED C3 SAME AS RPT C OTIC USERS UNCLASSIFIED

22a. NAME OF RESPONSIBLE INDIVIDUAL 22b. TELEPHONE (include Area Code) 22c. OFFICE SY IVMOL%

George 3. Thaler (408) 646-2134 62Tr NVU0 FORM 1473, a4 MAR 83 APR edition may be used until exhausted. SECURITY CLASSIFICATION OF THIS PAGE

All other editions are obsolete 2 U.L S. ~ ".~ P."tinq OffiMS 111141-404-6.'

1%

Approved for public release; distribution is unlimited.

Automatic Control ofRobot Motion

by

George P. KalogirosLieutenant, Hellenic Navy

B.S., Hellenic Naval Academy, 1979

Submitted in partial fulfillment of the

requirements for the degree of

MASTER OF SCIENCE IN ENGINEERING SCIENCE

from the

NAVAL POSTGRADUATE SCHOOLDecember 1987

Author.

Approved by:

<,Hrol A. Titus, Sec n Reader

"ohn P. Powers, Chairman,Department" Electrical and Computer Engineering

Gordon E. Schacher,Dean of Science and Engineering

2

%w% W .. ~ ~ * ~ ~ *.' ~ ~ -

ABSTRACT *

The feasibility of controlling the three link

rectangular and revolute robots with an adaptive computer

simulation model, using a curve following technique, is

investigated.

Both configurations are tested for different load

conditions, for rejection of random disturbances and for 0

robustness in the case of servo motor parameter variations.

The interactive nonlinear dynamics of the revolute

robot, such as coupling inertia, actuator dynamics,

centripetal and coriolis forces, are also investigated.

First a gravity-free environment is assumed and then the

robot arms are tested under gravitational torques.

TI? :T:,,," & , \,-'

: , , , .... ..

Nca

.Acession For , -.

NT 17 11

N t-ft. - j"p "..

TABLE OF CONTENTS

I. INTrRODUCTIION ...... ... .. . .. . .......... 18

II . FUNDAMENTALI ROBOTICS ................. 21

A. INTrRODUCTIION ...................21

B. DEFINITION OF ROBOT.............................. 21

C. THE ROBOT ARM!........................... .......~oo22

D. DIFFERENT ARM CONFIGURATIONS...................... 22

1. Cartesian (Rectangular) Coordinate Robot ... .24

2. Cylindrical Coordinate Robot........ ... o.... 24

3. Spherical (Polar) Coordinate Robot.........26

4. Revolute Coordinate (Jointed Arm) Robot .... 26

E. TYPE OF CONTROL .. o ............ o..o....... 26

1. Point-to-point Robots ............... 26

2. Continuous-Path Robots.....................o.28

3. Controlled Path (Computed Trajectory)Robots ....... ....................... o.... 28

4. Servo versus Non-Servo Robots................ 28

F. CATEGORIES OF SERVO SYSTEM OPERATION............o29

1. Sequential Joint Control...................... 29 1

2. Uncoordinated Joint Control ................. 29

*3. Terminally Coordinated Joint Control.........29

G. DRIVE MECHANISM.................................. 30

1. Linear Drives .... ... .. o... o........ o..........30

* 2. Rotary Drives . . .. o.o. . . ... . ... ... .. ... . .. .30

4

lop A'

III. DEVELOPMENT OF THE SIMULATION MODEL............... 31

A. INTRODUCTION . . .. ... .. .. .. . ... .. .. ......... .. .31

B. DESCRIPTION OF THE MODEL ............. 33

C. SIMULATION STUDIES OF THE MODEL ................ 37

IV. THE ADAPTIVE MODEL FOR THE THREE LINKR.ECTIANGULA.R ROBOT .................... 40

A.* INTRODUCT'ION ........... ...... ........... 40

B. SELECTION OF POSITIONING SERVO MOTORS............ 40

C. ALGORITHM TO UPDATE THE ADAPTIVE MODEL...........43

V. MODELLING THE THREE LINK RECTANGULAR ROBOT...........54

A. INTRODUCTION ........................... 54

B. MODEL DEVELOPMENT................................ 54

C. EQUATIONS OF MOTION.............................. 55

VI. SIMULATION OF THE THREE LINK RECTANGULAR ROBOT.......58

A. PLANNING THE SIMULATION STUDIES .......... 58

B. SIMULATION STUDIES OF THE ADAPTIVE SYSTEM........59

1. Gravity-free Environment..................... 59

a. Different Load Conditions................ 59

b. Disturbance Rejection.................... 77

c . Robustness ........................ 84

2. Gravitational Torques Included............... 89

VII. MODELLING THE THREE LINK REVOLUTE ROBOT............. 116L

A. INTRODUCTION..................................... 116

B. MODEL DEVELOPMENT............................... 117

C. EQUATIONS OF MOTION............................. 119

Ir5

VIII. THE ADAPTIVE MODEL FOR THE THREE LINKREVOLUT'E ROBOT . . ... ........................... 123

A.* INTIRODUCTION . . .... . .. ... . ............... . .. 123

B. SELECTION OF POSITIONING SERVO MOTORS........... 123

C. CALCULATION OF GAIN CONSTANTS (Kin).............. 124

IX. SIMULATION OF THE THREE LINK REVOLUTE ROBOT.........132

A. PLANNING THE SIMULATION STUDIES................. 132

B. SIMULATION STUDIES OF THE ADAPTIVE SYSTEM.......132

1. Gravity-free Environment.................... 132

a. Different Load Conditions............... 132

b. Disturbance Rejection................... 151

c. Robustness ......... .. ............. 158

2. Gravitational Torques Included.............. 163

a. Different Load Conditions .......... 178

b. Disturbance Rejection................... 191

c . Robustness.......................... 202

X. CONCLUSIONS/AREAS FOR FURTHER STUDY................. 205

APPENDIX A: DSL PROGRAM FOR THE SIMULATIONOF THE SECOND ORDER MODEL.................... 207

APPENDIX B: DERIVATION OF MATHEMATICAL MODELFOR THE THREE LINK RECTANGULAR ROBOT.........208

APPENDIX C: DSL PROGRAM FOR THE THREE LINK RECTANGULARROBOT UNDER DIFFERENT LOAD CONDITIONS(NO GRAVITY).................................. 210

APPENDIX D: DSL PROGRAM FOR THE THREE LINK RECTANGULARROBOT WITH DISTURBANCE (NO GRAVITY)..........215

6 .

APPENDIX E: DSL PROGRAM FOR THE THREE LINK RECTANGULARROBOT UNDER DIFFERENT LOAD CONDITIONS(GRAVITATIONAL TORQUES INCLUDED) ........... 220

APPENDIX F: DSL PROGRAM FOR THE THREE LINK RECTANGULARROBOT WITH DISTURBANCE (GRAVITATIONALTORQUES INCLUDED) ........................ 225

APPENDIX G: DERIVATION OF MATHEMATICAL MODELFOR THE THREE LINK REVOLUTE ROBOT .......... 230

APPENDIX H: DSL PROGRAM FOR THE THREE LINK REVOLUTEROBOT UNDER DIFFERENT LOAD CONDITIONS(NO GRAVITY) ............................... 236

APPENDIX I: DSL PROGRAM FOR THE THREE LINK REVOLUTEROBOT WITH DISTURBANCE (NO GRAVITY) ........ 242

APPENDIX J: DSL PROGRAM FOR THE THREE LINK REVOLUTEROBOT UNDER DIFFERENT LOAD CONDITIONS(GRAVITATIONAL TORQUES INCLUDED) ........... 247

APPENDIX K: DSL PROGRAM FOR THE THREE LINK REVOLUTE

ROBOT WITH DISTURBANCE (GRAVITATIONALTORQUES INCLUDED) .......................... 253

LIST OF REFERENCES .............................. ....... 258

BIBLIOGRAPHY ... ......................... 259

INITIAL DISTRIBUTION LIST ............................... 260

7

. ~~ ~ ~ .~ . .. .

LIST OF TABLRS

1. PARAMETRIC DATFORJOITSEV OTOS............... 41

.1ep

8I

LIST OF FIGURES

2.1 The Six Motions Required to Orient a Gripperin any Way at any Point in Space .................. 23

2.2 Cartesian (Rectangular) Coordinate Robot .......... 25

2.3 Cylindrical Coordinate Robot ...................... 25

2.4 Spherical (Polar) Coordinate Robot ................ 27

2.5 Revolute Coordinate (Jointed Arm) Robot ........... 27

3.1 Block Diagram of the Model ........................ 33

3.2 Phase Plane Trajectory of the Model ............... 38

3.3 Step Response of the Model ........................ 39

4.1 Rack-and-Pinion Gearing ........................... 42

4.2 Open Loop Bode Plot of the JOINTI Servo Motor ..... 44

4.3 Open Loop Bode Plot of the KmI/s2 Motor ........... 45

4.4 Open Loop Bode Plot of the JOINT2 Servo Motor ..... 46

4.5 Open Loop Bode Plot of the Km2/s 2 Motor ........... 47

4.6 Open Loop Bode Plot of the JOINT3 Servo Motor ..... 48

4.7 Open Loop Bode Plot of the Km3/s 2 Motor ........... 49

4.8 Block Diagram of the Adaptive Joint Drive System ..50

6.1 Phase Plane Trajectory for Unloaded Arm(No Gravity) ...................................... 60

6.2 Step Response for Unloaded Arm (No Gravity) ....... 61

6.3 Error Curves for Unloaded Arm (No Gravity) ........ 62

6.4 Phase Plane Trajectory for Loaded Arm(Load=0.8 - No Gravity) ........................... 64

6.5 Step Response for Loaded Arm(Load=0.83 - No Gravity) .......................... 65

9

.. A XF W.7 Ki i KIK W.-7 k7 9 %W A7 Kr 1VW

6.6 Error Curves for Loaded Arm(Load-0.83 - No Gravity) ........................ 66

6.7 Phase Plane Trajectory for Loaded Arm(Load=0.9 - No Gravity) ....................... 67


6.9 Error Curves for Loaded Arm(Load=0.9 - No Gravity) ...................... 69

6.10 Phase Plane Trajectory for Loaded Arm(Load=l.5 - No Gravity) ........................ 70

6.11 Step Response for Loaded Arm(Load=.5 - No Gravity) ........................... 71

6.12 Error Curves for Loaded Arm(Load=.5 - No Gravity) ........................... 72

6.13 Phase Plane Trajectory for Load Pick-upand Return (Load=0.3 - No Gravity) ................ 74

6.14 Time Response for Load Pick-upand Return (Load=0.3 - No Gravity) ................ 75

6.15 Error Curves for Load Pick-upand Return (Load=0.3 - No Gravity) ................ 76

6.16 Phase Plane Trajectory with DisturbanceApplied at T=10 msec (No Gravity) ................. 78

6.17 Step Response with DisturbanceApplied at T=10 msec (No Gravity) ................. 79





6.22 Phase Plane Trajectory for Unloaded Arm withKt and Kv Increased by 10% (No Gravity) ........... 85

10

6.23 Step Response for Unloaded Arm with Ktand Kv Increased by 10% (No Gravity) .............. 86

6.24 Phase Plane Trajectory for Unloaded Arm withKt and Kv Decreased by 10% (No Gravity) ........... 87

6.25 Step Response for Unloaded Arm with Ktand Kv Decreased by 10% (No Gravity) .............. 88

6.26 Phase Plane Trajectory for Unloaded Arm withR Decreased and L Increased by 10% (No Gravity) ... 90

6.27 Step Response for Unloaded Arm with R Decreasedand L Increased by 10 % (No Gravity) .............. 91

6.28 Phase Plane Trajectory for Loaded Arm withR Decreased and L Increased by 10%(Load=0.94 - No Gravity) .......................... 92 V

6.29 Step Response for Loaded Arm withR Decreased and L Increased by 10%(Load=0.94 - No Gravity)...........................93

6.30 Phase Plane Trajectory for Loaded Arm withR Decreased and L Increased by 10%(Load=0.98 - No Gravity) .......................... 94

6.31 Step Response for Loaded Arm withR Decreased and L Increased by 10%(load=0.98 - No Gravity) .......................... 95

6.32 Phase Plane Trajectory for Loaded Arm withR Decreased and Kt, Kv, L Increased by 10%(Load=l.08 - No Gravity) .......................... 96 I.

6.33 Step Response for Loaded Arm withR Decreased and Kt, Kv, L Increased by 10% .-(Load=1.08 - No Gravity) .......................... 97

6.34 Phase Plane Trajectory for Loaded Arm with LR Decreased and Kt, Kv, L Increased by 10% C.

(Load=1.12 - No Gravity) .......................... 98

6.35 Step Response for Loaded Arm withR Decreased and Kt, Kv, L Increased by 10%(Load=l.12 - No Gravity) .......................... 99

6.36 Phase Plane Trajectory for Loaded Arm withKt, Kv , R Decreased and L Increased by 10%(Load=0.80 - No Gravity) ......................... 100

ow.

11 1'p

6.37 Step Response for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.80 - No Gravity) ......................... 101

6.38 Phase Plane Trajectory for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.84 - No Gravity) ......................... 102

6.39 Step Response for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.84 - No Gravity) ......................... 103

6.40 Phase Plane Trajectory for Unloaded Arm(With Gravity).................................... 105

6.41 Step Response for Unloaded Arm(With Gravity) ................................... 106

6.42 Error Curves for Unloaded Arm(With Gravity) ................................... 107

6.43 Phase Plane Trajectory for Loaded Arm(Load=0.86 - With Gravity) ....................... 109

6.44 Step Response for Loaded Arm(Load=0.86 - With Gravity) ....................... 110

6.45 Error Curves for Loaded Arm(Load=0.86 - With Gravity) ....................... 111

6.46 Phase Plane Trajectory with DisturbanceApplied at T=50 msec (With Gravity) .............. 112

6.47 Step Response with DisturbanceApplied at T=50 msec (With Gravity) .............. 113

6.48 Phase Plane Trajectory for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.82 - With Gravity) ....................... 114

6.49 Step Response for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.82 - With Gravity) ....................... 115

7.1 Point Mass Representation of a Three

Link Revolute Robot .............................. 118

8.1 Open Loop Bode Plot of the JOINT1 Servo Motor .... 126

8.2 Open Loop Bode Plot of the Kml/s 2 Motor .......... 127

12

; , -. .- '.., ,, - ,'. .. :- -'-.' ... ': , ' ' i,. , < "<- - ';- -':':'" -<' ' I


8.4 Open Loop Bode Plot of the Km2/s2 Motor ..... 129


8.6 Open Loop Bode Plot of the Km3/s 2 Motor .......... 131

9.1 Phase Plane Trajectory for Unloaded Arm(No Gravity) ..................................... 134

9.2 Step Response for Unloaded Arm(No Gravity) ............. ...... ................. 135

9.3 Error Curves for Unloaded Arm(No Gravity) ..................................... 136

9.4 Phase Plane Trajectory for Loaded Arm(Load=0.04 - No Gravity) ......................... 138

9.5 Step Response for Loaded Arm(Load=0.04 - No Gravity) ......................... 139

9.6 Error Curves for Loaded Arm(Load=0.04 - No Gravity) ......................... 140

9.7 Phase Plane Trajectory for Loaded Arm(Load=0.1 - No Gravity) .......................... 141


9.9 Error Curves for Loaded Arm(load=0.1 - No Gravity) .......................... 143

9.10 Phase Plane Trajectory for Loaded Arm(Load=0.157 - No Gravity) .............. 145

9.11 Step Response for Loaded Arm(Load=0.157 - No Gravity) ........................ 146 0

9.12 Error Curves for Loaded Arm(Load=0.157 - No Gravity) ........................ 147 .

9.13 Phase Plane Trajectory for Loaded Arm(Load=0.2 - No Gravity) .......................... 1-,3

9.14 Step Response for Loaded Arm 4(Load=0.2 - No Gravity) .......................... 149

9.15 Error Curves for Loaded Arm(Load=0.2 - No Gravity) .......................... 150

13

IV..

9.16 Phase Plane Trajectory with DisturbanceApplied at T-90 msec (No Gravity) ................ 152

9.17 Step Response with DisturbanceApplied at T=90 msec (No Gravity) ................ 153

9.18 Phase Plane Trajectory with DisturbanceApplied at T=270 msec (No Gravity) ............... 154

9.19 Step Response with DisturbanceApplied at T=270 msec (No Gravity) ............... 155

9.20 Phase Plane Trajectory with DisturbanceApplied at T=370 msec (No Gravity) ............... 156

9.21 Step Response with DisturbanceApplied at T=370 msec (No Gravity) ............... 157

9.22 Phase Plane Trajectory for Unloaded Arm withKt and Kv Increased by 10% (No Gravity) .......... 159

9.23 Step Response for Unloaded Arm with Ktand Kv Increased by 10% (No Gravity) ............. 160

9.24 Phase Plane Trajectory for Unloaded Arm withKt and Kv Decreased by 10% (No Gravity) .......... 161

9.25 Step Response for Unloaded Arm with Ktand Kv Decreased by 10% (No Gravity) ............. 162

9.26 Phase Plane Trajectory for Unloaded Arm withR Decreased and L Increased by 10% (No Gravity) ..164

9.27 Step Response for Unloaded Arm with R Decreasedand L Increased by 10 % (No Gravity) ............. 165

9.28 Phase Plane Trajectory for Loaded Arm withR Decreased and L Increased by 10%(Load=0.18 - No Gravity) ......................... 166

9.29 Step Response for Loaded Arm withR Decreased and L Increased by 10%(Load=0.18 - No Gravity) ......................... 167

9.30 Phase Plane Trajectory for Loaded Arm withR Decreased and L Increased by 10%(Load=0.19 - No Gravity) ......................... 168

9.31 Step Response for Loaded Arm withR Decreased and L Increased by 10%(Load=0.19 -No Gravity) ......................... 169

14

*08

* hi m ~ ~ mu ~ . - -' '

-.' . - - . - -.

9.32 Phase Plane Trajectory for Loaded Arm withR Decreased and Kt, Kv, L Increased by 10%(Load-0.21 - No Gravity) .......... 170

9.33 Step Response for Loaded Arm withR Decreased and Kt, Kv, L Increased by 10%(Load0.21 - No Gravity) ......................... 171

9.34 Phase Plane Trajectory for Loaded Arm withR Decreased and Kt, Kv, L Increased by 10%(Load=0.22 - No Gravity) ........ 172

9.35 Step Response for Loaded Arm withR Decreased and Kt, Kv, L Increased by 10%(Load=0.22 - No Gravity) ......................... 173

9.36 Phase Plane Trajectory for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.155 - No Gravity) ........................ 174

9.37 Step Response for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.155 - No Gravity) ........................ 175

9.38 Phase Plane Trajectory for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.165 - No Gravity) ........................ 176

9.39 Step Response for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.165 - No Gravity) ........................ 177

9.40 Phase Plane Trajectory for Unloaded Arm(With Gravity) ................................... 179

9.41 Step Response for Unloaded Arm(With Gravity) .................................... 180

9.42 Error Curves for Unloaded Arm(With Gravity) ...... . . . ...... ......... 181

9.43 Phase Plane Trajectory for Loaded Arm(Load=0.04 - With Gravity) ...................... 183

9.44 Step Response for Loaded Arm(Load=0.04 - With Gravity) ...................... 184

9.45 Error Curves for Loaded Arm(Load=0.04 - With Gravity) ...................... 185

9.46 Phase Plane Trajectory for Loaded Arm(Load=0.195 - With Gravity) ..................... 186

15 p.

9.47 Step Response for Loaded Arm(Load-0.195 - With Gravity) ...................... 187

9.48 Error Curves for Loaded Arm(Load=0.195 - With Gravity) ...................... 188

9.49 Phase Plane Trajectory for Loaded ArmJOINT3 Servo Motor Input Delayed 150 msec(Load=0.195 - With Gravity) ...................... 189

9.50 Step Response for Loaded ArmJOINT3 Servo Motor Input Delayed 150 msec(Load-0.195 - With Gravity) ............. 190

9.51 Phase Plane Trajectory for Move #2(Load=0.055 - With Gravity) ...................... 192

9.52 Step Response for Move #2(load=0.055 - With Gravity) ...................... 193

9.53 Phase Plane Trajectory for Move #2(Load=0.065 - With Gravity) ...................... 194

9.54 Step Response for Move #2(load=0.065 - With Gravity) ...................... 195

9.55 Phase Plane Trajectory for Move #3(Load=0.06 - With Gravity) ....................... 196

9.56 Step Response for Move #3(load=0.06 - With Gravity) ....................... 197

9.57 Phase Plane Trajectory for Move #3

(Load=0.07 - With Gravity) ....................... 198

9.58 Step Response for Move #3(load=0.07 -With Gravity) ....................... 199

9.59 Phase Plane Trajectory with Disturbance

Applied at T=90 msec (With Gravity) .............. 201

9.60 Step Response with Disturbance

Applied at T=90 msec (With Gravity) .............. 202

9.61 Phase Plane Trajectory for Loaded Arm withKt, KV, R Decreased and L Increased by 10%(Load=0.155 - With Gravity) ...................... 203

9.62 Step Response for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%(Load=0.155 - With Gravity) ...................... 204

16

I., v

ACKNOWLEDGMENTS

I would like to express my thanks to my thesis advisor,

Distinguished Professor, George J. Thaler for his guidance

and assistance in this study.

I would like also to thank the Greek Navy and all the

Greek tax-payers for having paid for my course of studies.

Finally, I would like to dedicate this thesis and give

special thanks to my wife Katerina for her love, support,.5

understanding and patient help throughout my studies in NPS. .

17i

-A TV - 'V..

I. INTRODUCTION

Current industrial robot arms tend to have been

justified because of their untiring nature, predictability,

precision, reliability and ability to work in relatively

hostile environments. Besides, robots frequently increase

industrial productivity, improve the overall product

quality, allow replacement of human labor in monotonous and

mainly hazardous tasks and also may save on materials or

energy.

The dynamic motion of a robot arm is strongly affected

by mechanical design, physical properties of the arm and the

effects of gravity. Such factors as joint friction, coupling

inertia, centripetal forces, actuator dynamics and gravity

effects produce an interactive nonlinear dynamic system. In

order to cope with the rapidly changing dynamics, a robust

and flexible control algorithm is required.

The design of an adaptive algorithm consists basically

of combining a particular parameter estimation technique and

any feedback control law. In this study, the use of a curve

following technique to control a robot arm is investigated.

The very important feature of this control scheme is that

the amplifier is intentionally driven to saturation so full

advantage of the maximum available power can be taken while

18

TJWMV ~ ~ ~ i ~ p~~ W7 1.1 WX "F.Ww 1;_J.. _V _1d X-. _X -_ -. 9 -. -b .YY". F-I-I . -. W

in linear controllers driving the amplifier to saturation

must be avoided.

First the three link rectangular robot is used as the

simulation model and then the same tests are applied to the

three link revolute robot. The same control scheme is used

to drive all servo motors. An ideal second order model is

chosen to approximate the servo motor and the adaptive

algorithm updates the second order position, acceleration

and gain constant using the servo motor position output.

Each of these configurations is tested first for

different load conditions, then for random disturbance

rejection and finally for robustness in the case of slight

variation of the servo motor parameters. In all these teststhe interactive nonlinear dynamics of the system as coupling

inertia, centripetal forces and coriolis forces are also

investigated. Initially a gravity-free environment is

assumed and then all the tests are repeated assuming the

robot arm operating under gravitational torques.

Definitions and several basic ideas about robotics and

materials which are referred to in this study, are included

in Chapter II. The development of the computer simulation

model and the simulation studies of this model are discussed

in Chapter III. Chapter IV deals with the development of the

adaptive algorithm for the three link rectangular robot and

Chapter V contains the modelling as well as the equations of

motion this robot while the simulation results are studied

19

t

in Chapter VI. The modelling and the equations of motion of

the three link revolute robot are presented in Chapter VII

and the development of the adaptive model for the revolute

configuration is presented in Chapter VIII. Chapter IX

contains the simulation studies of the revolute robot and

Chapter X concludes the studies of both configurations,

indicating also some areas for further study. The detailed

derivation of the mathematical models for both rectangular

and revolute robots as well as the basic DSL/VS simulation

programs used in the course of this thesis research are

listed in Appendices A through K. It

'2V.

I,.

V.

20 'It.

I

II. FUDMWALRBTC

A. INTRODUCTION d

The word robotics was invented by the Isaac Asimov, one

of the best of the science fiction writers, to describe the

science of dealing with robots. In one of his stories called

'Runaround', which appeared in the March 1942 issue of

'Astounding Science Fiction', Asimov propounded the famous

Three Laws of Robotics.

1. A robot must not harm a human being or, throughinaction, allow human being to come to harm.

2. A robot must always obey human beings unless that isin conflict with the First Law.

3. A robot must protect itself from harm unless that isin conflict with the First or Second Law.

B. DEFINITION OF ROBOT

As can be imagined from the confused usage of the word

robot there is no international agreement about definitions.

Several definitions of robot exist depending on what a robot

actually consists of. Some of the most commonly used S

definitions are:

1. A robot is a mechanical positioning system.

2. A robot is a pick and place device.

3. The popular conception is of a mechanical man capableof carrying out tasks that a human might do anddisplaying some capability for intelligence.

21

,a A WPI!6

4. A robot is a reprogrammable, multifunctional manipula-tor designed to move material, parts, tools or specia-lized devices through variable programmed motions forthe performance of a variety of tasks.

C. THE ROBOT ARM

Although mobile robots one day should be common, for the

present the state to which the industrial robot has evolved

is best referred to as a robot arm. Most of them are

essentially a mechanical arm f ixed somewhere or to another

machine, f itted with a special end - effector whi ch can be

either a gripper or some sort of tool. The arm moves, by

means of hydraulic, electric or pneumatic actuators, in a

sequence of preprogrammed motions under the direction of a

controller which these days is almost always microprocessor

based and senses the position of the arm by monitoring

feedback devices on each joint. The range of positions

which the arm can reach is called work volume or work space

or work envelope.

*D. DIFFERENT ARM CONFIGURATIONS

The essential role of a robot arm is to move a gripper

or tool to given orientations at a given set of points.

Mathematically, to be able to orient an object in any way at

any point in space requires an arm with six degrees of

freedom, as depicted in Figure 2.1. Three translational

* (forward/back, right/left, up/down) to reach any point, and

three rotational (pitch, roll, yaw) to get any orientation.

22 '-

PI tch

(bend) P

Figue 2. TheSixMotins Rquied t OrintGriper i anyway t an Poit inSpac

231

The work envelope of an arm varies in shape depending on

the actual configuration chosen for the design of the arm.

One common structural classification of robot arms is

according to the coordinate system of the three major (the

translational) axes which provide the vertical lift motion,

the in/out reaching motion, and the rotational or traversing

motion about the vertical lift axis of the robot. Such a

classification can distinguish between four basic types.

1. Cartesian-(Rectacrular) Coordinate Robot

This type of robot, shown in Figure 2.2, is

configured with the mutually perpendicular traversing axes.

Each part of the robot's arm slides at right angles to theprevious part. So the robot can reach any part of volume

bounded by the length of the separate sections of the arm.

This configuration is clearly ideally suited for direct

usage of the mathematical cartesian (or rectangular)

coordinate system.

2. Cylindrical Coordinate Robot

In this type of robot, shown in Figure 2.3, the

horizontal arm can move in and out parallel to the base,

can move up and down the vertical column, and the whole base

can rotate around the vertical axis, so the end of the arm

sweeps out a work envelope which is a partial cylinder. This

configuration is clearly ideally suited for direct usage of

the mathematical cylindrical coordinate system.

24

N N.J.

* AL - S - t-b b -- ~ --------- *------------ - 5--

.5,'

-U

*5~~~

.5.

5-.S.

£

'p

-A-

5..

5.,.5-

.5-'A-

A).V5~5

.5,.

V

0

.5-

Figure 2.2 Cartesian (Rectangular) Coordinate Robot.5-p.55-pA

-5-.5-.

.5

e

5-S.V'5.

*55*~*~

5-* .A

SA,-,

A....

-A

5..

S

Figure 2.3 Cylindrical Coordinate Robot

25 'pp'pp

A - * 5-..?.: .5-

p. * A .5 .qJA% *~*%****~*.............- * .

j5-*5-~~~~5- .*.**.~ :.-.P'.P......

3. Spherical (Polar) Coordinate Robot '

The robot in Figure 2.4 has an arm capable of linear

motion in and out. It is supported by two other sections,

one that rotates around the base and one that rotates about

an axis perpendicular to the vertical through the base. This

arm can reach almost anywhere in a volume bounded by an

outer and an inner hemisphere. The radii of the two

hemispheres correspond to the maximum and minimum extensions

of the linear section, respectively. This corresponds to the

mathematical spherical (or polar) coordinate system.

4. Revolute Coordinate (Jointed Arm) Robot

An example of this fourth class of robot, sometimes

known as an anthropomorphic robot, is shown in Figure 2.5. .5

It consists of rotary joints called the 'shoulder' and the

'elbow' (corresponding to the human arm) all mounted on a

* 'waist' consisting of a rotary base which provides the third

degree of freedom. This revolute (or jointed arm)

configuration has the advantage of having a very large work

envelope for its size, so minimizing floor space

requirements.

E. TYPE OF CONTROL

1. Point-to-Point Robots

Point-to-point robots are able to move from one-

specified point to another but cannot stop at arbitrary

points not previously designated. They are the simplest and

26

op

Figure 2.4 Spherical (Polar) Coordinate Robot

Figure 2.5 Revolute Coordinate (Jointed Arm) Robot

27

least expensive type of robot. Stopping points are often

just mechanical stops that must be adjusted for each new

operation. Point-to-point robots driven by servos are often

controlled by potentiometers set to stop the robot arm at a

specified point.

2. Continuous-Path Robots

Continuous-path robots are able to stop at any-

specified number of points along a path. However, if no stop

is specified, they may not stay on a straight line or

constant curved path between specified points. Every point

must be stored separately in the memory of the robot.

3. Controlled-Path (Computed Tralectorv) Robots

Control equipment on controlled-path robots can

generate straight lines, circles, interpolated curves, and

other paths with high accuracy. Paths can be specified in

geometric or algebraic terms in some of these robots. only

the start and finish coordinates and the path definition are

required for control.

4. Servo versus Non-Servo Robots

Servo-controlled robots have some means for sensing

their position and "feeding back" the sensed position to the

means of control in such a way that the control can cause a -

particular path to be followed. Nonservo robots have no way

of determining whether or not they have reached a specified-

location.

28

LeS

F. CATEGORIES OF SERVO SYSTEM OPERATION

In servo control, each joint is controlled by an

independent position servo, with joints moving from position

to position independently. There are three possible

categories of motion.

1. Seauential Joint Control

Joints are activated one at a time while the other

axes are held fixed. This procedure simplifies control, but

the operating time is longer than it would be if all joints

operated at the same time. It provides better path control

because there is no interaction between joint movements.

2. Uncoordinated Joint Control

All joints are allowed to move together, so that

they follow a path determined by the relative speeds of each

joint movement. There is no coordination between joints.

Thus, prediction of the end-of-arm path is difficult or

impossible, since varying arm loads or varying friction

might change the joint velocities in a random way.

3. Terminally Coordinated Joint Control

This is the most useful type of point-to-point

control but requires more control equipment. Individual

joint motions are coordinated to reach their endpoints

simultaneously.

'.%

29

p.4 4i * - 5 | - -*.. .4 -* %% % .

G. DRIVE MECHANISM 99

1. Linear Drives

Examples of linear drives are the x, y and z drives

of rectangular coordinate system, the vertical and

horizontal positioner of cylindrical systems, and the radial

drive on spherical systems. Linear motion may be generated

directly by a hydraulic or pneumatic piston in a cylinder or

by conversion of rotary motion to linear motion through rack

and pinion gearing, lead screws, worm gears, or ball screws.

2. Rotary Drives

Most electric motors and servo drive motors generate

rotary motion directly, but often at a lower torque and

higher rotational velocity than is desired. It is necessary,

therefore, to use some kind of gear train, belt drive, or

other mechanism to convert the available high speed to lower

speed with greater torque. There are also some cases in

which linear hydraulic cylinders or pneu~matic cylinders are

used as a power source, so that the linear motion must be

converted to rotary motion. The main options available for

* consideration are gear trains, timing belt drives, harmonic

drives, rotary hydraulic motors and the recently developed

direct-drive torque motors.

30

III. DEVELOPMENT OF THE SIMULATION MODEL

A. INTRODUCTION

The simulation model chosen was a servo motor with a

curve following velocity loop, identical with the one used

in Ref. 1 and Ref. 2. The velocity curve following scheme is "

a non linear adaptive scheme that has been used mainly in

disk drive systems [Ref.l], but recent studies [Ref.2] have

justified that the same scheme can also be used to control a

rigid robot arm.p..

Although in linear controllers driving the amplifier to

saturation limits must be avoided, in the case of the curve

following scheme the amplifier is intentionally driven to

saturation. This is a very important feature because full

advantage of the motor's capabilities can be taken.

Furthermore this system is simple and easily implemented in

a microprocessor [Ref.3].

For a given step position command this model operates in

two modes: An initial full acceleration mode and a curve

following mode. The equivalent transfer function of the

servo motor is [Ref.4] :

e(s) I/Kv(3.1)

V(s) ( JR Lss + -- + -

KvKt R

31

where

e = Angular position of the shaft

V = Applied dc voltage

Kv = Back emf constant

Kt = Torque constant

J = Total inertia

R = Armature resistance

L = Armature inductance

When the inertia of the robot arm is added to the motor

inertia, the mechanical pole of the motor becomes very small

and since the ratio L/R is a small number, the electrical

pole of the motor can be neglected. With the above

approximations the transfer function of the robot arm and

motor becomes approximately:

e(s) Km- (3.2)

V(s) s 2

Since in the mass production of such systems there would

be substantial tolerances on the physical motors, a more

complete motor model is not justified.

The second order model with a curve following velocity

loop is shown in Figure 3.1. If the curve to be followed is

chosen to be the deceleration curve for the idealized motor,

the model will be a practical application of bang-bang

control (Ref.5]. ".4

32

i. -S

'K

p

Figure 3.1 Block Diagram of the Model

Id

B. DESCRIPTION OF THE MODEL

I I

When a step position command is applied to the model,

the error signal (E) will produce a velocity command input

(X). This signal saturates the amplifier and full forward

drive signal is applied to the motor (full acceleration

mode). As the error signal decreases, the velocity command

is reduced and when it becomes equal to the velocity

feedback signal (KC) the system changes into curve following

mode and follows the curve down until the desired position

is reached.

For the system to be able to follow the curve to the

desired position under different loading conditions a proper

curve must be chosen. Since a parabolic curve approximates

33

-- -.-- . . . . .. | II!

the deceleration curve of an ideal motor, it was chosen as

the curve to be followed. To enable the system to follow the

curve accurately the curve should be below the motor

deceleration curve. This is accomplished by proper selection

of the gain constant K1 which reshapes the motor curve. In

this case the selected value for the gain constant K1 was

0.6.

It has been shown [Ref.3] that the equation of the curve

was derived from the idealized motor equations as follows:

C = Km Vsat (3.3)

C C dt = Km Vsa t t + C(0), (C(O)=0) (3.4)

C [ C dt = l/2 (Km Vsat t2) + C(0) (3.5)

From equation 3.4

Ct =(3.6)

Km Vsat

and substituting into equation 3.5

C2

c :(3.7)2 Km Vsat

For deceleration from initial conditions with the input R=O

C = - E (3.8)

C = - E (3.9)

34

Subtittin o euaton3.8 and 3.9 into eutio 3.7 gives

j2E= (3.10)

2 Km Vsat

or

E 2 Kmsat E(3.11)

Letting

A 2 Km Vsat (3.12)

and

X= E (3.13)

equation 3.11 becomes

X=AVE = velocity command input (3.14)

Therefore, if the parameter A is initially calculated,

then the commanded velocity curve can be generated by

continuous multiplication of the calculated parameter A with

the square root of the error signal (E).

The servo motor gain constant Km is determined by the

transfer function of the motor that the simulation model

will control, considering the motor parameters and the

effective inertia seen by the motor.

For the three link rectangular robot the chosen values

of Km were (this choice will be justified in Chapter IV)

35.5

-,- - a a .4 &arfr .P.. S .W. .h * * a a V.A M V- .3

Kml = 59.29 rad/volt

Km2 = 90.25 rad/volt


and for the three link revolute robot the chosen values of

Km were (this choice will be justified in Chapter VII)Kml = 0.4225 tad/volt


Km3 = 4.0 rad/volt

The saturation limits (±Vsat) of the amplifiers are

determined by available power supply voltage, servo motor

parameters, mechanical design of the arm, working conditions

and curve constant K1. This limit was arbitrary chosen as

*' ±150 volts for all actuators, under different loading and

working conditions.

The value of the gain constant K2 must be chosen such

that saturation of the amplifier occurs for small signals,

allowing the amplifier to operate as a switch providing full

forward and reverse drive signals to the motor in the curve

following mode. To effectively saturate the amplifier, K2

was given the value of 10,000.

The gain of the velocity feedback channel (K) is chosen

such that X = KC when the simulation motor velocity (C) is

at the desired speed for a given step position input. From

Figure 3.1, for deceleration from initial conditions with5,

R=0, and because of equation 3.9

X=KlE -KC KE (3.15)

36

7.1I-S • %"\ . V I'].% ~ % ~

Thus, the gain of the velocity feedback channel (K)

should be equal to K1 . From simulation studies, the best

curve following was achieved when K was given the value of

unity (K = 1).

C. SIMULATION STUDIES OF THE MODEL

To demonstrate the curve following ability of this

scheme, the model was simulated using DSL/VS. Appendix A -

lists the DSL simulation program used for these studies. Kma

was set to 59.29, a value which was found for the parameter

Kml of the three link rectangular robot. All the other

parameters and variables used in the program are as

previously discussed in this chapter. The constant CF is a

factor which converts the rotary motion to linear motion.

The parameter RO represents the radius of the pinion used

(to be discussed lin chapter IV) and was chosen to be 0.5 in.

The phase plane trajectories (velocity 6X versus

position CX) for a step position command of 1 inch are shown

in Figure 3.2. The figure shows that the linear velocity of

the model increases until the commanded velocity ()is

reached. Then the model velocity follows the curve down to

the commanded position. The step response of the model,

depicted in Figure 3.3, shows good performance of the model.

Now since a simulation model has been found, use of this

model to control the servo motors of a three link robot arm

(rectangular and revolute) will be investigated.

37

46

Uri .. 5

48- .48

40 / 40

/2 32

Z4/ 24 .

16!-6~

0.0 0.1 0.2 0.3 0. 4 0. S 0.6 0.7 0.8 0.9 .

CX (INCHESIPHASE PLANE

XDCTX.COOTX VS CX

Figue 3. Phse PaneTrajctor ofthe ode

38S

K aw "r ir. 5... .. ~ ..-- . - p-

WAvo

X1 0.oft I T I-

a 10 9 30 40 s so o 'G

TIME (1 -3SECISTE.RSP0S

1.V TM

Figue 33 Sep Rspose f th Moel

1.39

IV. THE ADAPTIVE MODEL FOR THE THREE LINK RECTANGULAR ROBOT

A. INTRODUCTION

The second order model developed in Chapter III will be

operated open loop to control its output, C, and bring it to

the commanded position. But this is not the desired solution

because what is really wanted is to control the output

position, CR, of the real motor which must be driven to the

commanded location. Due to the different dynamics of the

ideal and the real motor there will be always a discrepancy

between their outputs C and CR. To overcome this undesired

situation, the model states and the gain constant must be -'

adapted in a such a way that the ideal motor imitates the

real one.

In this chapter, the selection of the servo motors is

first discussed and then the adaptive algorithm to update

the model states and gain constant is obtained.

B. SELECTION OF POSITIONING SERVO MOTORS

The selected real motor is a permanent magnet motor

drive, which is widely used in industrial robots, and its

data are listed in Table 1 (Ref.6]. This is a rotary motor

and since a linear motion is required for the rectangular

coordinate robot arm, the rotary motion is converted to

linear through rack and pinion gearing.

40

5,*

9%.

TABLE 1

PARAMETRIC DATA FOR JOINT SERVO MOTOR

Total Inertia Jm 0.033 oz-in-sec2/rad

Torque Constant Kt 14.4 oz-in/amp

Back emf Constant Kv 0.1012 volts-sec/rad

Damping Coefficient Bm 0.04297 oz-in-sec/rad

Ave. Terminal resistance R 0.91 ohms

Armature Inductance L 100.0 g-henries

A rack is a long metal bar that has gear teeth cut

across its width. A pinion gear is an auxiliary smaller,

round gear placed so that its teeth can mesh with the teeth

of the rack. Usually the rack is fixed in place. As the

pinion gear rotates, it moves an attached carriage linearly

along the rack. The rotary motion of the pinion gear is

converted to linear motion of the carriage, as shown in

Figure 4.1 [Ref.7]. Support for the carriage is provided by

a guide rod or guide way. For these studies a pinion gear of

radius 1.0 inch was chosen.

The same motor drive is used for the three arm joints

with the same power supply. To calculate the transfer

function of the servo motors, the effective joint inertias

must be first calculated as follows

41

-'I

Carriage

-'Pinion Guode

Rack

Figure 4.1 Rack-and-Pinion Gearing

JEF, = Mlr 2+j MI (4.1)

JEF2 = M2r2+j m2 (4.2)

JEF3 = M3r2 -j m3 (4.3)

Substitution of the parameters results in

JEF1 = 0.167 oz.in.sec2



Plugging these results and the parameters from Table 1 into

equation 3. 1, the transfer functions of the three servo

motors become

9.88Gl(s) =rad/volt (4.4)

s(s/9.589+l) (s/9100+l)

9.88G2(s) =~ /7221(/101 rad/volt (4.5)

42

I

9.88G3 (S) rad/volt (4.6)

s(s/16.014+1) (s/9100+1)

These results were used to determine the gains Km for

the model motors. Figures 4.2, 4.4 and 4.6 show the Open .

Loop Bode diagrams of the three real motors transfer

functions respectively. From these diagrams it can be

observed that the -40 db/decade slope intersects the 0 db

axis at . 1 =7.7 rad/sec for the JOINT1 motor, W2 =9.5 rad/sec

for the JOINT2 motor and W,3=8.P rad/sec for the JOINT3

motor. Using the above frequencies as gain crossover

frequencies for the second order ideal motors, we calculate

Kml = 7.72 = 59.29 rad/volt

Km2 = 9.52 = 90.29 rad/volt

Km3 = 8.82 = 77.44 rad/voltK--!The Open Loop Bode diagrams of the Km/s 2 ideal motors

are shown in Figures 4.3, 4.5 and 4.7.

r .-

C. ALGORITHM TO UPDATE THE ADAPTIVE MODEL C.

The adaptive model depicted in Figure 4.8 is used to

drive a joint servo motor. The same scheme is used for each V

of the three servo motors. The output of the saturating

amplifier (V) is common to both the model and the servo

motors. The output of the servo motor (CR) is measured, at

prespecified time intervals, and fed into the Adaptive

43

.. "

010 Was- WWt S0I- 9*81 owe- 0,. 0K- 2Mb

.. ............. .... .... .. ........ ... ........... .... ... .. ... .

C '4

-CD

.......-- Li)

tr C j .............. ... ....

a .... .............. .... -......... .... . .....

.. . . . . . . . . . . ... . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

L . ... ... .... .... .. . .. . .. . .. ..

C.D ........... ......

z 0 CD C0 . 00- 0m- 0B.. .. .. .. .. .. ... .. .(.. .. .! ... ... .... .... .... .... .N. .... ... ..

cc -,>

41

* ~ ~ ~~ Fiue42Oe opBd lo fteJI ev oo

-j 0 4.

-n C)

L'4 L'~ P j ~4 * K. I A . .. .... .... .. . ... .. . . . . . .. .. . .. . .. . . .... ......

3SWWoo"OZ rn-u.- a, u o ~

.... . ... .. .. ... .. .. . .......

.. ........ ....... ...... ................. ...

C)

cr..... . . .. .w C),

w C -- .. ... ... .. .. ..... .. .. .. ... ... ... .. ... .. ... . ... .. ... ... ... ..

M -j CaF-- Li

w C

.........................

a ... .. .. .. ...9-i

445

'%-%-

3SU~cS

.. ... . .. . . ... .. - F -W ............. ....... . . .

ZCD

Li Cl C3

or_ z C3 - ............. .... .-- Li

.. ..... ....LJ ... . . . ..... . .. . ... . ... ... .

.. ... . .. .. ....

=Ca- .... .......... ... .. ...

U I/a

U V C

C) ~b o'~aa a . ~ os.... ... ... .. .. ...L. .. .. .

.4Lj4

-j C3 0.

-IC

Figure.. 4. Open. Loop Bode .. Plo of .he.....Srv.Mto

L I .. - .... ... .

4 46

b - - a h ~ M.k a.M . -M - a

... .. ........., t

....... .. . . . ... .. . . -* ... .. .......

C)

Q:: C)

..... . ... :.

.... . . ... .... ... ....

C. 3 . . ... ... ... . . . ..

I--

I-- C

.................. .. . .

.. .. . . .... ........ ...... ... ....

0

zcc~~ .

... .. .. .. .. .. . .. . .... .... . .. ... ..

w C).z .... .... .. ..

.... ...... . . ....

............. ..... ..

48-

... ... ... .. . . .. .. . .. ... ... .. . .. . . .. .-.. . . . . . . . . .

.. .. .. . .. .. . .

C)

wC)

CC

-Li

Li

w- C.

-j 0l 09- .............. 0 .- .O

L ' i . . . . . . . . . . . .. .... . ..

..... .... .... .... ..... .. . .. .

Figure 4.7 Open Loop Bode Plot of the K.3/ s2 Motor

49

S :el

E- E--

E-I

LL

aL

Fiue48 BokDarmoph dpieJitDieSse

50-

VV1

Algorithm which updates the velocity (C) and position (C)

of the model, and the gain parameter (Kin). Km is not kept

constant during the arm's motion but is adjusted in such a

way as to account for the inertia reflected back from the

arm. For the above reasons an estimate of the servo motor

gain (Km) as well as the linear velocity (CR) of the servo

motor must be determined. Two main considerations that must

be taken into account in these calculations are: [Ref.l)

1. The calculations must be reasonably accurate to allowthe model states to approximate the trajectory of theservo motor. dl

2. Since the updating of the model states and gainconstant in minimum time is required, the calculationsmust be kept as simple as possible.

There are several different methods for these

calculations. Since the method described in Ref. 1 best

satisfies both of the above requirements, it is used for the

calculations.

From equation 3.5 solving for Km

Km = 2C(4.7)Vsat t 2

For discrete time intervals

2 C

K Vsat (NT)2 (48

where N is the number of samples and T is the sampling

interval.

51

Letting C = CR

2 CRKm = (4.9)

Vsat (NT) 4

Equation 4.9 is valid only for the full acceleration portion

of the move where the acceleration of the servo motor is

constant. Therefore, the model gain Km is updated until the

velocity of the servo motor reaches the velocity curve

following portion of the move.

The estimated velocity of the servo motor is [Ref.1]

2[CR(N)-CR(N-I)]CR(N) = - CR(N-1) (4.10)

T.4

This calculation gives an estimate of the velocity as soon

as the last position of the servo motor is known, and

requires the storage of the last position measurement [CR(N-

1)] and the last estimated velocity [CR(N-I)]. After 2

samples of position are known, the stored velocity CR(N-I)

can be recalculated as

CR(N) - CR(N-2)CR(N-l) = (4.11)

2T

If the model switches from full acceleration to full

deceleration (curve following mode) between samples of

position, the velocity given by equation 4.10 is not self-

correcting until two position samples can be obtained after

52

%V.• o" 4

w

switching. To remove this discrepancy, the algorithm detects

the switching time and stores the present value of the model

velocity as CR(N-l) to be used in the next calculation.

As has been discussed earlier in this chapter, generally

CR does not duplicate C, because of the different dynamics

between model and real motor. However, if C and CR start

with the same values, and if they have identical initial

derivatives C and CR, then it is expected that CR and CR

will not deviate substantially from C and C over a very

short time interval. If C and C are reset at the end of this

interval and given the actual values of CR and CR, then both

model and motor start the next interval with the same

55. states. Also the gain parameter, Km, is updated during the

full acceleration portion of the move. [Ref.8]

In the simulation of this model fixed step integration

is used as the easiest numerical integration method. The

integration step size parameter, DELT, is chosen to be 1/5

of the sampling interval so that 5 integration steps are

completed between two successive updates of the adaptive

model. The model velocity error signal, XE, is computed at

each integration step and is sent to the amplifier.

53

.%

V. MODELLING THE THREE LINK RECTANGUIAR ROBOT

A. INTRODUCTION

In this chapter the dynamic equations of the three link

rectangular robot are first developed and then solved in

order to obtain the equations of motion. Although using

Lagrangian mechanics the dynamic equations of any system can

be obtained in a relatively simple manner, the Newton-Euler

formulation of equations of motion is more convenient in the

case of the cartesian coordinate configuration and for this

reason is preferable over the Lagrangian method.

Since the motion of each link is independent of the

motion of the other two links, there is no inertial coupling

between the three joints. Besides, the only link affected by

the gravitational acceleration is the vertical one while the lop

other two links move in directions perpendicular to the

direction of gravity and therefore are not affected at all..

B. MODEL DEVELOPMENT

The three link rectangular robot to be tested is

illustrated in Figure 2.2. The rotary motion of the three

joint servo motors is converted, through rack and pinion

gearing of radius r, to linear motion, and the motions of

the three links are constrained to the x, y and z directions

respectively. The link moving along the y axis has a5I

54 ,

V ~ ~ %~'%~.* ~ 'V.. % ~S~* ~ ~-. -w

gripper, as end effector, which can carry a load. The mass

of the servo motors is denoted mm and the masses of the

three links ml, m2 , M3 respectively. The total masses seen

by each servo motor are denoted M1 , M2 , and M3 . F1 , F2 , F3

represent the forces acting on the three links in the

directions of motion, and T1 , T2 , T3 represent the external

driving torques to the joints.

All joints can move simultaneously, so the end-of-arm

follows a path determined by the relative speeds of each

joint velocity. However, prediction of this path is

difficult because varying arm loads or varying friction

change the joint velocities in a random way.

V

C. EQUATIONS OF MOTION

Applying the Newton-Euler method, the motion of a rigid

body can be decomposed into the translational motion of an

arbitrary point fixed to the rigid body, and the rotational

motion of the rigid body about that point. The dynamic

equations of a rigid body can also be represented by two

equations: one which describes the translational motion of

the centroid, while the other describes the rotational

motion about the centroid. The former is Newton's equation

of motion for a mass particle, and the latter is called

Euler's equation of motion.

In the specific case of rectangular coordinate robot

there is no rotational motion, therefore the dynamic

55

equations are obtained applying only Newton's equation. If

vci is the linear velocity of the centroid of the link i,

then the inertial force is given by -MiVci, where Mi is the

total mass seen by the servo motor i. Then, the equation of

motion for the link i is obtained by adding the inertial

force to the static balance of forces so that

Fi-l'i-Fi,i+l+Mig-Mi~ci = 0 , for i=1,2,3 (5.1)

where Fi.li and Fii+l are the coupling forces applied to

link i by links i-l and i+l, respectively, and g is the

acceleration of gravity.

In the case of rectangular coordinate robot due to

absence of any inertial coupling between the joints,

equation 5.1 becomes

Fi+Mig-MiVci = 0 , for i=1,2,3 (5.2)

where Fi is the force applied to the link i by the joint

motor i, in the direction of motion of the link i. The total

torque about the axis of rotation of the motor i is given

then by the equation

Ti = Fir+Jmiei (5.3)

where r is the radius of the pinion gearing, Jmi is the

inertia of motor i, and ei is the angular acceleration of

*. the motor's i shaft.

56S'.! .5

15

- 'n - t . , n nm -- • '

Solving equations 5.2 and 5.3 for the three links, the

following second-order nonlinear differential equations have

been derived and used as the basic mathematical model for

the simulation of the three link rectangular robot. The

derivation of these equations is shown in details in

Appendix B.

T1 = (Mlr 2 +Jml)el (5.4)

T2 = (M2 r2 +Jm 2 )e2 (5.5)

T3 = (M3 r2 +Jm 3 )e3 -M3 gr (5.6)

where

M1 = ml+m 2+m 3 +2mm+Load

M2 = m2+Load

M3 = m2+m 3 +mm+Load

ml = 0.082 oz/in/sec2

m2 = 0.041 oz/in/sec2

m3 = 0.041 oz/in/sec2

mm = 0.186 oz/in/sec2

r = 0.5 in

g = 386.4 in/sec2

In these equations, terms of the form Mir2 +mi are known

as the effective inertia at joint i.

57

p VI *SIMULATION OF THE THRE LINK RECTANGUaR ROBOT

A. PLANNING THE SIMULATION STUDIES

The already developed model for the three link robot is

tested for the Voltage Source Drive. First a gravity-free

* environment is assumed and the model is tested for different

load conditions, for rejection of a disturbance applied

before and after ste~ady state condition, and finally for

* robustness in the case of motor parameters variation. Then

the same tests are repeated but in this case gravitational

torques are taken into account.

The tested arm is a point-to-point arm and the unit step

input command is applied in all joints simultaneously for

comparison reasons. The motions of the three joints are

independent of each other, and the path that the end of the

gripper follows depends on the relative speeds of each joint

* movement. In this thesis a free work space is assumed for

the tested arm so that obstacle avoidance is not a

consideration.

For every simulation program three plots are obtained:

1. Phase plane

2. Step response

3. Error versus time

In all phase plane plots the model linear velocity, C, and

the actual system linear velocity, CR, are plotted as

58

ordinates versus the system linear position, CR. In step

response plots the linear positions of the model, C, and the

actual system, CR, are plotted as functions of time.

Although the linear position and velocity of the actual

system can not be observed in the above plots, they are

available in simulations and are used to check the validity

of the adaptive algorithm. Finally in the last group of

plots the error between the commanded and the actual

position of the system is plotted also as a function of

time.

B. SIMULATION STUDIES OF THE ADAPTIVE SYSTEM

1. Gravity-free Environment

The adaptive system in a gravity-free environment is

tested first for different load conditions. Then the

capability of the system to reject a disturbance, which is

applied in any time instant, is tested and finally the

behavior of the system is examined for a slight variation of

servo parameters.

a. Different Load Conditions

The adaptive system is tested first without

carrying any load and then with different load. The DSL/VS

simulation program used for these tests is listed in

Appendix C. Figures 6.1, 6.2 and 6.3 are the phase plane

plot, the step response and the error curve, respectively,

for the unloaded arm. I59

" • O ..O, .€ -' € , " " " ' % %q ,"% " ."%" "". ". %' ."'-.',2 - % .% .' % . i .% .' -- - - " ' .- "p

40 40.

F T I

25 205I

LnL

20 20.

C6RZ (INCHES)

P4PSEPLAN (COT.CRO-1.OOT.5.CR

Figue 6. Phse PaneTrajctoy fo Unoade Ar(No Gavity60V

IX 0 203.0 1.5

T.0- 1.03EC

0.5 0.5 U

30 i 40 so 6

1. 0- - - - - - - -

z .5.

0~. 0. L

10 2O 30 40 so 1.5o1. TIME I10,3SEC)

61'~

1." 1.0 1.0

",p 0.8 0.8 0.8

0.6- 0.6 0.6

0.4- 0.4 0.4

0.2- 0.2 0.2

- .. 0.0

-0.? , _-0.2 0O.z0 10 20 30 40 50 8

TIME (10 "3 SEC)

ERROR .VS. TIME

Figure 6.3 Error Curves for Unloaded Arm(No Gravity)

623 3=

3!

.,,... "%" , e~r+ m+ mvr + "V +" +,- J - -- +,, . t ,, _ _ : _ ,..F ",r ,," _ + :: + +. . .. . .

Figure 6.1 shows that the adaptive model and the

servo motor have good curve following characteristics

for the three joints. It shows also that the linear)O

acceleration of the three links is inversely proportional to

their masses. So the link moving along the y-axis, which is

the smallest one, moves faster than the other two links and

is the first one that reaches the commanded velocity curve,

while the link moving along the x-axis, which is the largest

one, is the last one that reaches the velocity curve. The

link moving along the z-axis moves with an intermediate

velocity. Finally all links track the commanded velocity IJ

curve down to the desired position.

The step response curves of Figure 6.2 show the

three model and servo motors tracking together to steady

state condition. The y-axis link reaches the commanded

position faster while the x-axis link, moving slower, is the

last one that reaches the desired position. The time

required for the movement of the arm to be completed is the

time required by the slowest (x-axis) link to complete its

movement. From Figure 6.2 this time is read 42 msec.

The error curves of Figure 6.3 show zero steady

state error for all links, but an accurate observation of

the simulation data reveals a steady state accuracy of the

order of 10O5 inches. .

The loaded arm under different load conditions

is then simulated in Figures 6.4 through 6.12. In these

63

Ilk

% % % %LAS ,

-- -- - - - - - - - - - - -

so

30- 30 Aa

01-3n -30

0.2 0.4 .16 0.8 1 .0 707 -... CRZ ( INCHES]

- -35 A

L&J X

64-a 02 0.4 0.5 a.8 1. a,, 60

* -"'----... .. CRY ( INCHES5)

:30- 30 ,

0- 0

-n -30

0.0 0.2 0. 0.6 0.8 0.ZCRX (INCHES]

PHASE FLMNE (COOT.CROOTXOCT.VS.CR1

Figure 6.4 Phase Plane Trajectory for Loaded Arm

(Load=0.83 - No Gravity)

64

*, h h . , h. - . . ' a ,..1, -b,. ,. ,U ' m '.. -U . - - - - - --,-,..v. ,, ., ' . . ,-7 ." "

1.1 7 47.7

- 1.0

jo.n 0.05

P14

0.0 0.0

T20I 4 0 60 lo 1.5

0..5.

1.5 20 400 .5

TIME C10'3SEC]1.1

0.5-.5

a 20 40~1:5PO.S 800s 100

Figure 6.5 Step Response for Loaded Arm(Load=O.83 - No Gravity)

~~65

a. 0.8 0.-

0. 0 6 0.5

0.2- -0.2 0.2 I

EO ST

0. No GravIi""

66=

__ _ __ _ _ __ _-__ _ _ __ _ .2 I 01.2 ,

TIME (10"3 SEC) "

ERROR .VS. TIME :-.

Figure 6.6 Error Curves for Loaded Arm(Load=0.83 - No Gravity)

66

6 -,

Wr 60

30.- 30

.30I -30'..

7 ---- 0.2 0.4 0.6 0.8 1.0 7C

Of 0

0.2 0.4 0.6 0.8 1.0 1

0--

30 .~

0.0 0.2 0.4 0.6 0.8 1.0 1.Z

Figure 6.7 Phase Plane Trajectory for Loaded ArmI(Load=O.9 -No Gravity)

67

V-1.0

flu.t1. 040 60 80 1 00. 1.5

~~ia TI ME L0-3 SEC-Ii. 0 k

0. z

0 0. 80 0 :u

0.- 204 0 0IrIME J 10-3SEC)

L-iq

68

0. 01 .01.

0. 0.4 Z. LTA

0.a=. Gaiy

- ~. o~o69

100

0

'30KJL 0. 2 0.4 0.6 0.8 1.0 1.2 7

0. 04 CRY (INCHES])7

60.0 0.2 0.4 0.6 0.8 1.0 1.2 1.CRY (INCHES)

30-d. 30 NoGrviy

.301 700. b . . .3 1 0 1. .

** ~ ~ ~ ~ ~ ~ R (INCHES) . ~ ~ - -

1.5-0 20 4 o B 0 24 .5 .

1.- 1.0

0.5-.5.-

1.. - - - - - - - - - - - - 1.s - 5'

-0.

o.a Jo~ ~

TIMIE 110 3 SEC )

0 20 40 60 80 100 120 14

. TIME (10SEC)

L.j.

_ z

Fiur 6.0. StpRsos frLae r

2.0 Z 40 60 80 100 1l9 1TIflE f10"' EC] .

0 .5" 0.5 0*

, 01.6- 60 120.140

-71Xx

STEP RESPONSE

F:igure 6.11 Step Response for L.oaded Arm(Load=1.5 - No Gravitry)

711

.5".

+'1

'4I

0.0.6 0.6

0.4 0.4 0.4

0.2- 0.2 0.2

0.D- 0.0 0.0

,.,. -0.2- - -0.Z -i 0.2 .-

x >-C.

U- 0.4 "0.4

a 20 40 60 s0 100 120 140TIME (10 3 SECI

ERROR VS. TIME'p


72 -a

',,,,,,, 2,. ' V ,*, , ,t...;'t ' " -. ".v ," "'.'v" '. ..... +V -,"',.--, p'-".._ **."". . ". ..' ,v.,',.'*"-.,-'.. -"-.+." v .

figures the effects of the added load can be observed.

Although the same load is added to all links, the link

mostly affected by this load is the y-axis link because this

w is the lightest one and its relative increment of the weight

(the ratio of added load to link weight) is larger than in

the other two links. Thus, the settling time for the y-axis

link is increased more than the other links and as more load

4 is added to the arm, the time difference between the

settling time of the three links tends to zero.

For loads up to 0.83 oz/in/sec2,. the critical

load, the system shows good curve following characteristics

for all joints as depicted in Figures 6.4 -6.6. But as the

load exceeds the value of the critical load, the velocities

of the three links are reduced and they can not track the

deceleration curves. Therefore, when they reach the

commanded positions, their velocities do not reach zero but

they still have some values which are inversely proportional

to the weights of the respective links as shown in Figure

6.10. As a result of this fact the three links overshoot and

undershoot before they reach steady-state, as shown in

a. Figure 6.11. Consequently, the error curves oscillate before

they reach zero, as depicted in Figure 6.12.

Then, with a slight modification of the same

simulation program, the unloaded arm is commanded to go to

the desired position, take a load and return to the initial

location. The results are shown in Figures 6.13 -6.15.

73

40 . 4

20- .20

0 0.2 .4 0.56 0. e 17CRZ (INCES)I

0 0.2 0.4 0.5 0.8 1CRY I INCHES)

0- O

0.0 0.2 0.4 0.a. 1.0CRX (INCHES)

PHASE PLANE (C00T.CRDOT.XDOT.VS.CR)

Figure 6.13 Phase Plane Trajectory for Load Pick-upand Return (Load=O.3 -No Gravity)

74 1

dS.

U 0.

1. 1. S

710

el 0. a-5

0. 1 .10 20I 30 40 50 60 70 So 90 10 1

TIME (10-3SEC) .

1.0

10 20 30 4 560 70 88 90 lo

755

0. _ F

0.0.

-0.4 0.8 -0.4 1

0.6 -0.6 -0.6

-.0.2 -0.e

* ~RO VS. TIME04

and /eur -0.6=. -. NoGaiy

764

Figure 6.13 shows that the second-order model

and servo motor have good curve following characteristics

for all joints. The step response of Figure 6.14 shows that

the fastest (y-axis) link reaches the desired position and

waits there till the other two links reach this position.

When the slowest (x-axis) link reaches this position, the

load is picked up by the robot and the three links start to -

move to their initial locations.

b. Disturbance Rejection

It is not rare that disturbances, although

undesired, are present in the robot operation. These

disturbances can be treated as random impulse inputs applied

at any time during the robot motion. They can be rejected

from a well designed robot, or they can cause failure to the

desired robot motion if the design is poor. Next the

capability of the system to reject these random disturbances

is tested using the DSL/VS simulation program listed in

Appendix D.

An impulse of 2 msec duration is selected to

represent the disturbance and it is applied to three links

in different times during their motion. First the -

disturbance is applied before any of the links has reached -

steady-state, then the same disturbance is applied just

before the first (y-axis) link reaches steady-state and

finally after all links have reached steady-state. The

results of the three cases are shown in Figures 6.16 -6.21.

77

1 1

I I

60- so

n- 30

OleI I I Ole

0.0 0.2 0.4 0..06

CRX (INC~iEs

Figu.e 0.66 0.as Pln1rjcoywt itrac

Appled a T=1 msc (N Graity

78p

I~IX

1. i.3

0 .5- 0.0 l1.5'- 1.0

0.0 ,

a 10 2O 30 40 50 60 70 s .10 20 TIME (10"3 SEC

0.5.05

1.TIME (10-'SECJ

STEP RESPONSE

Figure 6.17 Step Response with DisturbanceApplied at T=10 msec (No Gravity)

A79

w.

.30 . -30-:

0.2 0.4 0.6 0.8 1.0

70

30o- 30

I-3

0.0 0.z 0.4 . 0.8 1.0CRX (INC HESI

P"S--- POEN (COOT.CROOT.xOOT.VS.CR

Figure 6.18 Phase Plane Trajectory with DisturbanceApplied at T=20 msec (No Gravity)

80

J

1." 1.5

1.0

0.5. 0.5

0:O.0 .0.01,.'" 10 20 30 40 s 60 70 1.S

TIME (10"3SECT

1. -' 1.0

0.5 0.5

-a

0 10 20 3'0 40a 5'0 6a 710

TIME (10 3 SEC)

81.0

Opn 0.5

"., TIME (10O~ s 5EC)",p,

• o,.STEP RESPC1MSE

&., 'Figure 6.19 Step Response with Disturbance[- Applied at T=20 msec (No Gravity)

d

30. 30

-30 -30 _.700 -! 0 0 . CRZ 1A!$ 1 .fl . - - j

6111'80

35 o- 35

a-S

___________-___________________ - 30

0.2 0.4 0.5 0.8 1.0 7

II 1 I I I I

0.5.2 04 0.8 1.0 80 o. o. o.5o'e .

CRX (INCiES3

PHASE PLANE (COOTcRoor, XOOT.VS.CR1


82

-Jo

0.5 0.5

a. t 0.0

TIME 1.13EC

-- -

1 10 20 30 40 50l s0 70 84TIME 110O3SECI

1. TEP [RESPONSE

1.0A

..... . . . .

Although the same disturbance is applied to all

links, the link affected mostly is the y-axis link which is

the lightest one. Generally, the lighter the link the more

sensitive to any disturbance. But in all different cases the

results show that the applied disturbances are rejected in a

time period which is also inversely proportional to link's

weight. Finally, all links go to the commanded position.

c. Robustness

The robot control problem is to design a stable

and robust algorithm to enable the robot to follow a

specified trajectory. Therefore, robustness is one of the

main considerations in robot design. Next the adaptive

algorithm is tested for slight (10%) variations of the servo

motors parametric data because, although the same servo

motors are used for the three joints, there are always some

tolerances in their parametric values that may cause

significant changes in the robot behavior if the system is

not robust. For these tests the simulation program of

Appendix C is used again, with different parametric values

for each run.

The effects of changing the torque constant (Kt)

and the back emf constant (Ky) for the unloaded arm by 10%

are depicted in Figures 6.22 - 6.25. Figures 6.23 and 6.25

show a very small variation in settling time which is-

proportional to the links weights and has the same sign with

the variation of the two parameters. Figures 6.22 and 6.24

84

, , - "" 4

: , . .w so

40- 40

m 20

754o 0.2 0.4 ao.6 0.8 7.

550

," 25

0

- 0.0 0.2 0.4 0609sCRY (INCHES)

PHASE PORfE (CDOT.CROCT.XDCJ.VS.CR)

Figure 6.22 Phase Plane Trajectory for Unloaded Armwith Kt and Ky Increased by 10% (No Gravity)

85

-J -A 5 . . -N . *

0.5-0

10') I 20 30 40 so 1.xTIMlE (10-3SECI

0..

TIME c10'2SECI

0.0-0 0 0 30 40 SO so

TIME (101 SEC)

* STEP RESPONSE

Figure 6.23 Step Response for Unloaded Arm withKt and Kv Increased by 10% (No Gravity)

86

404

a4 a

CRZ (INCHES)

So (.0

2255

00. 0.1 0.5 0.8 110s

I I II F I

0.0 0.2 0.4 0. 0.8a 1.3CRX (INCHES) 4

* P HWSE PLANE (COOT.CR(IOT.XOOT.VS.CR)

Figure 6.24 Phase Plane Trajectory for Unloaded Armwith Kt and Kv. Decreased by 10% (No Gravity)

87 4

d

1." 1.5

, i.0 - - --- 1.0

-I-0.C 0.0

10 zo 30 40 so 1.51. TIMlE (10O3SECJ

.1 0-1.0

C8..S

O- 0.5w-I

0.010 ZO30 40l 50 1.5

TIME (10*3SEC]

1.0* - - - -- - - - - - -

p. 0. 0.S

il 10 ZO 30 40 5o0sp. TIME (i0'3SECI

STEP RESPONSE

Figure 6.25 Step Response for Unloaded Arm with

Kt and Kv Decreased by 10% (No Gravity)

88

-p, , - % , " . % " . %," . - pp

show very good curve following characteristics either forJ.

increase or decrease of the above two parametric values.

Then the effects of a movement of the mechanical 1

pole towards the origin are examined. For the pole to be

moved closer to the origin, the resistance (R) is decreased

by 10% and the inductance (L) is increased by the same

percentage amount. Figures 6.26 and 6.27, compared with

Figures 6.1 and 6.2, show no change in the behavior of the

unloaded arm. If load is placed on the gripper of the robot,

as Figures 6.28 - 6.31 show, no overshoot occurs for loads

up to 0.94 oz/in/sec2 . Comparison with Figures 6.4 - 6.8

reveals that the critical load is increased from 0.83 to ."

0.94 oz/in/sec2 .

With the mechanical pole moved towards theorigin, if the constants Kt and Kv are increased by 10%

Figures 6.32 - 35 show that the robot can carry load up to

1.08 oz/in/sec2 without overshooting. In the opposite case,

if Kt and Kv are decreased by the same amount, Figures 6.36

- 39 show that load up to 0.80 oz/in/sec2 can be carried

without overshooting.

2. Gravitational Torques Included

The adaptive system is tested again for different

load conditions, disturbance rejection and robustness in the

case of motor parameters variation, but this time

gravitational torques are included in the simulation.

Actually, the only link which senses gravitational torques

89

so

20 20

40

* 0.2 0.4 0.6 0.8 7

O 0.2 0.4 6. 0.8 sCRY (INCHES)

0.0 0.2 0.4 0.8 0.8 1.

PHASE PLANE (CDOT,CROOT.XOOT.VS.CR)

Figure 6.26 Phase Plane Trajectory for Unloaded Arm withR Decreased and L Increased by 10% (No Gravity)

90

1.03 1 l.S

1.- 1.0

0.5- O.S

1 .5' 10zo3., sTIME (10-3 SEC) .

0.-0.

10 T0 30 40 so1

~J 0. .5

TIME (10"'SEC]

STEP RESPONSE

Figure 6.27 Step Response for Unloaded Arm withDecreased and L Increased by 10% (No Gravity)

91

59ki . loop". 4 .~ ~. I

---- 5.. 60 -7

W30 5

0 0.2-... 0.4 0.6 0.8 1.0 6

30- 30

0 0.2 0.4 0.6 0.8 t.0CRY (ItOIESI

PSEPLONE (COOT.CROOT.XOOT.VS.CR)

Figure 6.28 Phase Plane Trajectory for Loaded Armwith R Decreased and L Increased by 10%


92

-N. I'll

1..'

1. 1.5

1.0.- - - - - - - - - - - - - - 1.0

0.5 0.5

3 .0. 0.01.0 1's 2 30 40 50x 1.5

TIME (10-SEC) 15

1.0-1.0

( 1.$- 0.5-- - - - - -

0.0 0.

0 l0 20 30 40 1.5TIME ('10 "35EC)

0.5- O.S

TIME (1O'3SEC)

STEP RESPONSE

Figure 6.29 Step Response for Loaded Arm withR Decreased and L Increased by 10%


9

93 ... . . ... .

er 60

- 30'

0

0 0.1-, 0.4 0.6 0.8 1.0 s

30- 30 AU

O0. 0.2 0.4 0.56 0.8 1. 0CRY (INCHES) s

-301

0.0 0.2 0.4 0.6 0.8 1.0CRX (INCHESi

4,Pd* PHW5E PO.NE (COCT.CRCQT.XOOT.VS.CR)

Figure 6.30 Phase Plane Trajectory for Loaded Azrn withR Decreased and L Increased by 10%


94

.1r'

1.S0. 1.5

~TIME {(O-3 SEC)

--- - 1.0

II

0.0.l 0.0

21.7 10 0 30 40 SC 1.5

1._10 TIME -10-3SECI

-1.0 ----------- - - 1.0

-0.5- 0.5a., . .

TM. T E (1'SEC)

STEP RESPONSE

Figure 6.31 Step Response for Loaded Arm withR Decreased and L Increased by 10%


V 95

.%.

• ,~ ~ * %* ~~. .-. .d ~~* ~ ~ * *

AD-A191 012 AUTOHATIC CONTROL OF ROBOT NOTION(U NAYM POSTORRUATE 2/?3SCOOL MONTEREY CR G P ICRLOOIROS DEC 67

UUM CL 7 S~jSI jF E DFIG12/S N

mhmhmhhhhhhhlm

I mmmmomhm.

2-i

11.00 1 M2i

1111.1K 5 11/

ilhI'i~~~ .2 1111. 'I .

r. or r-~ ~ ~ -~ -- - - --

60

-31 3 0 L

O .2-., 0.4 0.6 0.8 1.0 s

I I I I I I I INH S

a .Z 0.4 0.8 0.8 1.0

CRYI INCE I soI P

0.0 0.2 0.4 0.6 0.8 1.0CRX (INCHES)

PH19SE P'LANE (COOT. CROOT. XOOT. VS. CR)

Figure 6.32 Phase Plane Trajectory for Loaded Arm withR Decreased and Kt, Ky, L Increased by 10t

(Load=l.08 -No Gravity)

96

fo. 0.5

V.r

Lii0 a.0

1.0 10 20 30 40 5o Go1.5TIME (10-3SECJ

.0 ---- -- -- ----

0.. 0.5

00.

.53 10 20 30 40 so ax1.

V. * 1.0 --- -- -- -- -- --- - - - - -.

TIMlE (10'SEC)

STEP RESPONSE

N Figuare 6.33 Step Response for Loaded Arm withR Decreased and Kt, Kv, L Increased by 10%

(Load=1.08 -No Gravity)

97

-0- - .0.

. _ _ _ __ _ __n_ _ _ -301

so6 0 0.~-. 0.4 0.6 0.8 1.0 s

o 0.2 0.4 0.5 0.8 1.0 sCRY (INCHE5J

0.1E POANE k'COOT.CROOT.XCOT.VS.C:I

Figure 6.34 Phase Plane Trajectory for Loaded Arm withR Decreased and Kt, Kv, L Increased by 10%


98

4.5 0.5

0. 0.5

1.2 3 0 o 6 1.5

*~~TM 1.01----------

a 0.5

1.2 0 0 s 1.5

0.5 0.5

0. 0. .0 10 Z0 30 40 so so

STEP RESPONSE

.J.

Figure 6.35 Step Response for Loaded Arm withR Decreased and Kt, Ky, L Increased by 10%


99

"30 30 -

I I I I I I I I t

Ga 6 0.2-. 0.4 0.6 0.1 1.0 so~-CZ I INCHES)

30- 3A

30A

-30 -301I V I 1 1 I I 1 / 1

0 0.2 0.4 0.6 0.8 1.0 60

CRY ( INCHI" )

30 -O . I

N

Kt Iv R Derae an L I Inceae by10

0.0 0. 0.4 0.6 0.8 1.0CRX (INCHES) 4

Pt'~iSE PLONE (COOT.CROOT.XOT.VS.CRI

* Figure 6.36 Phase Plane Trajectory for Loaded Arm withKt, Ky, R Decreased and L Increased by 10%


100

'Iva.

.1.L NUM.. . . . . . . 1.5 M,.

440. 0.5

oo.. S~ .

O.020 30 40 s xTIME (10 "3SEC]

0.5 0.5

1."10 20 3 40 so6 1.TIME (10 "- SEC! ,

Cm,

0. (Li0 1.5 3 0 o s

,., o'o.

TMtE r10*SSEC)

STEP RESPONSE ,.

Figure 6.37 Step Response for Loaded Arm withKt, Kv, R Decreased and L Increased by 10% .a>


101 ..

'a,.

'- -'<""" "t'" *'%'- {'"i' 'l" "i "i " { '' :-' -' "--: '< " " " "" " " " ' " ''""' ' I

' "" - -"-a-

so

30 3 0 u

0

.30___ __1__T .301

.0 0. a-.. 0.4 0.6 0.8 1.0 s

.30'A

.0 0.2 0.4 0.6 0.8 1.0*CRY (INCHES) s

0- L

30 .3010.00. 0.8 . 1.0

Pt1A.E PLF'NE (COOT. CROOT. XDOT. VS.CR)

Figure 6.38 Phase Plane Trajectory for Loaded Arm with* Kt, Ky, R Decreased and L Increased by 10%

(Load=0.84 -No Gravity)

102

1 0.0

1. - - -- - - - - - - - - - - - - 1.0 s

0.5 0.5

Cr.'

0.0iIII 20 40 so0.

TIME (1O*3SEC)

* 1.0 - - - - - - - - - - .

0.5 0.5

0.0. C

1. 1.5 z.04.5

(Lo.=8 No ravty

103

is the vertical one because the other two links moving in a

direction perpendicular to the direction of gravity are not

affected at all.

The adaptive system is tested first without carrying

any load, using the DSL/VS simulation program listed in

Appendix E. Figures 6.40, 6.41 and 6.42 seem to be identical

to Figures 6.1, 6.2 and 6.3 which are the respective ones

for the robot operating in a no gravitational environment,

and especially the step response and the error curves show

that the three model and servo motors track together to the

commanded position with zero steady state error. An accurate

observation of the simulation data reveals a positive steady

state error of the order of 10-5 inches for the z-axis link

of the robot arm operating under gravitational torques. This

means that the tip of the z-axis link never reaches the

commanded position but stops 10-5 inches lower than the

desired position. The error curves of the other two links in

steady state condition oscillate with an amplitude of the

order of 10-5 inches, as they do in the no gravity case.

Therefore, the only difference observed in the behavior of

* the unloaded robot arm operating under gravitational torqiues .

is the steady state error of the vertical (z-axis) link.

Then the robot arm is tested under the same loaded

* conditions applied in the no gravity case. The simulation

* results show that the behavior of the robot arm under

gravitational torques is almost identical to the behavior of

104

soS

25 Z5.

4 40

01 0.2 1 0 1.4 1 01.6 0.18.1CRY (INCHES) so.

1

6 .O02 0.4 .6081.CRY 1 (ICHS)

Fi ur 6.4 Ph s l nLra et r o Ul a e r

(With ravity

105 5

&A - % %

o.0.0SD 10 20 30 40 so5x0.

TIME 11O*35SEC)

00. f :11.7 1 2 3 40i so a

TIME 110*5EC)1.5

1.0 - - - - - - - 1.0

apr.0.

0.010 2 31 40 50 sTIME C1OI3SEC)

STEP RESPONSE

Figure 6.41 Step Response for Unloaded Arm(With Gravity)

106

*a-. 1 F 1 ", .- .0.2 -a z - -

16 20 30 40 56 s

TIME (-n-35.

ERROR VS. TI-

Figue 6.2 Eror urvs fo Unoade Ar

(Wit Gr.0ity)

0.0

-Z Z

I = a . a. a a t a

the robot arm operating in a gravity free environment. The

only difference observed is a small (3%) change in the

capability of the tested arm to carry loads without

overshooting depending on the direction of the move. If the

arm is moving against gravity, the change is positive

(Figures 6.43, 6.44 and 6.45), but if it is moving with

gravity the change is negative.

Since the simulation results are very much the same

with the results of the robot arm under no gravity, most of

them are omitted in this section and just a few indicative

figures are included to illustrate the nearly identical

behavior of the three link rectangular robot operating under

gravitational and no gravitational torques.

After that, the robot arm is tested for disturbance

rejection applying the same disturbances mentioned in part

l.b of this chapter. Figures 6.46 and 6.47 are the results

obtained using the simulation program listed in Appendix F,

and show that the applied disturbances are rejected and all

links go to the commanded position.

Finally, the robot arm is tested for slight (10%)

variation of motor parameters. Simulation results show no

noteworthy change in the characteristics of the system which

behaves exactly as does under no gravity as depicted in

Figures 6.48 and 6.49.

108

!w 3 Lh

0. 04 0 .6 0.8 107

0 _ _ _ _ _ __ _ _ _ _ 0E

0 0.2 0.4 0.6 0.83 1.0 1CRY (INHS

so~~L.

A %.

30- *.301

0. . i.4 06 0.8 1'.0 1.2

PAEPLANE 1COOT.CRDOT.XDOT.YS.CR)

Figure 6.43 Phase Plane Trajectory for Loaded Arm(Load=0.86 -With Gravity)

109

l.& 1.0

0.0 0.5 ,..5.

TIME (10" 5EC]

0.0-0.0ZO7 2 40 so Be 1

TIME (10 SEC)

- .- - - - - -1.0

o.S- .S to

0.O ( 0.0ii00..0 2 400

1.TIMlE (10 SEC3 I.

STEP RESPONSE

Figure 6.44 Step Response for the Loaded Arm(Load=0.86 - With Gravity)

110 '~

1. 1.0 1.0

0.8 1.8 6.8

0.6- 0.6 0.6

0.4- 0.4 0.4

0.2- 0.2 0.2

-_ L

0.2 ___ ___ __ ___ ___ __ ___ ___ * 0. 2L .0

TIME (10"2SEC)

ERROR .VS. TIME

F'igure 6.45 Er'ror Curves for LoadedAr(L~oad=0.86 -With Gravit'y)

NAl

.:

- sou

0

7P-L 0.2 0.4 0.6 0.8a 1.0 7

35,- 35

0-

0 0.2 0.4 0.6 0.8 1.0 s

0.0 0. .4aaa .

CRX (INCHES)I

PHASE PLANE (COOT. CROOT. XOOT. VS. CR)

Figure 6.46 Phase Plane Trajectory with DisturbanceApplied at T=50 msec (With Gravity)

112

0. 1.5

-jx . 0.5 0.5

1 O 30 40 50 60x7 1.510 20 TIME 110' SECI

0.. 0.5 L

*o0.aa 0 30 40 so 50 70 1.5

10 20 TIME (10'3SECI

STEP RESPONSE

Figure 6.47 Step Response with DisturbanceApplied at T-50 msec (With Gravity)

113

Gr1

30 30

0- 0

0 0.301-60 .2- 0.4 0.6 0 .8 1.0 s

30-30 L

0--

o0 0.2 0.4 0.5 0.8 1.0 60s

0--

0'.0 0i.2 0. 4 0. 6 0.8 1.0CRX (INCHES)

PMfiSE PLRKN (COOT.CRCCT.XOOT.VS.CR)

Figure 6.48 Phase Plane Trajectory for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%

(Load=0.82 - With Gravity)

114

0..

* 1.0. -- - - - ---- - - - --I - - - -

0.0 4

10 20 30 40 50Q .

0 .5- 1.0

20 30 6 so

(Load0.82 ith Ga0ity

115~

I&- -. 1N I -V" W9. T-.~ t. W. a -0

VII. MODELLING THE THREE LINK REVOLUTE ROBOT

A. INTRODUCTION

In this chapter the mathematical model of the three link

revolute robot is developed, making use of the Lagrangian

mechanics as this method allows us to obtain the dynamic

equations for very complicated systems in a rather simple

manner. The Lagrangian formulation describes the behavior of

a dynamic system in terms of work and energy stored in the

system rather than forces and momentums of the individual

members involved.

The dynamic equations, which relate forces and torques

to positions, velocities, and accelerations, are first

obtained and then solved in order to obtain the equations of

motion of the robot. Given forces and torques as input these

equations specify the resulting motion of the robot. This

approach is computationally difficult but gives results that

provide insight into both the linear and nonlinear operation

of robot arms.

Using the Lagrangian method makes possible the

computation of the interaction of gravitational, loading,

and centripetal torques and forces dependent on the external

forces, the inertias of the various components of the robot

arm, and the structure of the system. Coriolis acceleration

is derived as part of the overall analysis.

116

• "• " ". ". ". " " "€" " r .. ", •" " g ""..2""" "" ". ". "- " 2 2. " .' 'g€ .'2 "2'. .'2 " 2 22.' " a'.'

B. MODEL DEVELOPMENT

The three link robot, chosen to be tested, is depicted

in Figure 7.1. There are three rotary joints connecting the

three links. A gripper is mounted at the end of LINK3 which

can carry a load. It is assumed that the three links are

massless and the equivalent masses, ml, m2 , m3 , are lumped

at the end of the respective links. The lengths of the three

links are denoted dl, d2 , d3 respectively, and T1 , T2 , T3

represent the external driving torques to the joints.

Direct-drive motors are used to drive the three joints.

"Direct-drive" refers to the fact that the links of the

robot are coupled directly to the respective motor shafts

without going through any gearing. The advantage of this

scheme is that one immediately eliminates the effects of

backlash, joint flexibility, etc., which arise from the use

of gears. However, there are two significant negative co-

sequences. First, the presence of large gear ratios

effectively decouples the dynamic of each motor from the

rest, which in turn simplifies the control problem [Ref.9].

When direct drive is used, this isolation effect is no

longer present, and one is forced to deal with more

complicated equations. The second effect concerns the

actuator dynamics. In a robot with gears, even a small

motion of a link results in several revolutions of the

corresponding motor shaft, so that the standard method of

modeling a motor as an inertia and a viscous damper is quite

117

WV

'k t- - - - - - - ALI 'Ji ~

z

z 3 3

X 3-

x

Fiur 7. PitMs Rer ena i ofaTreLn

a11

C. EQUATIONS OF MOTION

The Lagrangian L is defined as the difference between

the kinetic energy K and the potential energy V of the

system.

L = K - V (7.1)

Generally, the dynamic equations, in terms of the

coordinates used to express the kinetic and potential

energy, are obtained as

Fi = - i=l,...,n (7.2)dt q

where

qi = the coordinates in which the kinetic and potentialenergy are expressed

qi = the corresponding velocity

Fi = the corresponding force or torque, depending uponwhether qi is a linear or an angular coordinate.

n = the number of links (degrees of freedom)

These coordinates, forces, and torques are referred to as

generalized coordinates, forces, and torques.

In order to apply equation 7.2 to the three link

revolute robot, the rotation angles 91, e2, and 93 are

chosen as the generalized coordinates. Therefore, F1 , F2 ,

and F3 represent the generalized torques about the axes of

rotation of the three links and from now on are renamed as

T1, T2 , and T3 . Solving equation 7.2 for the three links,

119

AeN%%N

Z C Z

the following system of nonlinear, second-order differential

equations has been derived and use as the basic mathematical

model for the simulation of the three link revolute robot.

The detailed derivation is presented in Appendix G.

CDl1+JM1)81 = Tj+Dll261 62+Dj136l63 (7.3)

(D22+Jm2)e)2 = T2-D2 3e3+D223(e2 93+D23 3 )3 2-D2 11612 -G2 (7.4)

(D33+Jm3)e3 =T 3-D3262 -D3 11612-D322 62 2-G3 (7.5)

where --

D1= m3 (d2cose2+d3cos (e2-.e3) )2 +m2d2 2cos2e92

D2= (m2+m3) d2 2+2m3d2d3cosE83+m3d3 2

D23 = m3d2d3cose3+m3d3 2

D32 = m3d2d3cOse3+m3d32 a

D33 = m3d3 2

D12= 2[ (m2-Im3)d2 sine2cOsE)2+m3d2d3sin(2e2+e3)

+m3d3 sin(e2+E)3 )cOs(e2+e3)]

D113 =2[m 3d2d3sin(e2+e3 )cose2

+m3d3 2sin(e2+ 3)cOs( 2 +s-3))

D21 (m2+m3)d2 sin 2cose2im 3d2d3sin(2 2 +e3)

+m3d3 sin (82+63)COs (e2+e3)

D223 =2m 3ddsiO

D233 = m3d2d3sin93

D31= m3d2d3sin(e2+e3 )cose2

+m3d3 sin(e2+e3) cos(e 2+E)3)

D32= m3d2d3sine3

120

G3 = m3 gd 3cos(e 2+e 3 )

Jm1 = 0.033 = motor inertia for JOINT 1



d I = 15 inches

d 2 = 10 inches

d3 10 inches

mI = 0.268 oz/in/sec2

"M2 0v227 °z/in/sec 2

m3 = 0.041 oz/in/sec2

g = 386.4 in/sec 2

Equations 7.3, 7.4, and 7.5 can be written in the form

T= (Dll+Jml)gl (7.6)-D126162 - D1136163

T 2 = (D22+Jm2)92 + D2 39 3 (7.7)

+ D211 - D2 3 36 32

D2 2 3 e263

+ G32

T3 = (D33+Jm3 )e3 +ED32E2 (7.8)

+ D311 12 + D322622

+ G 3

In these equations, the coefficients of the form

Dii+Jmi (Dll+Jml, D22+Jm2, D33+Jm 3 ) are known as the

effective inertia at joints i. Acceleration at joint i

121

.2 2

causes torque known as as inertial torque. The coefficients

of the form Dij (D23 , D32 ) are known as the coupling inertia

between joints i and j, as an acceleration at joint i or j

causes a torque at joint j or i, respectively. The terms of

the form Dijj4j 2 (D2 1 16 12 , D2 3 3e 3

2, D3 1141

2 , D3 2 2822 )

represent the centripetal forces acting at joint i due to an

angular velocity at joint j. The terms of the form Diijie j

(D1126162 , D1 1341e 3 , D22362e 3 ) represent the Coriolis force

acting at joint i due to angular velocities at both joints i

and j. The terms of the form Gi represent the gravitational

-5

forces acting at joint i. [Ref.10] ,

122 -

%%

VIII. THE ADAPTIVE MODEL FOR THE THREE LINK REVOLUTE ROBOT

A. INTRODUCTION

The same second order model developed in Chapter III and

used in the rectangular robot, will be operated again, open

loop, to drive the servo motors of the joints of the

revolute robot and control its output, C. For the reasons 0

described in chapter IV the model states and gain constant

must be adapted in such a way that the ideal motors imitate

the real ones. Since the adaptive model of Figure 4.8, used

for the rectangular robot, is used for the revolute robot

too, the adaptive algorithm to update the model states and

gain constant is identical with the one obtained in Chapter

IV and its derivation is not repeated.

In this chapter, the selection of the servo motors is

first discussed and then the initial values of the model

gain constants (Km) are obtained.

B. SELECTION OF POSITIONING SERVO MOTORS

The same permanent magnet servo motor used for the

rectangular robot is also used for the revolute robot and

its parametric data are listed in Table 1 [Ref.6]. The links

of the robot are coupled directly to the respective motor

shafts without going through any gearing ("direct-drive"

123

motors). The same motor drive is used for the three joints

with the same power supply.

C. CALCULATION OF GAIN CONSTANTS (KM)

To calculate the transfer functions of the servo motors,

the effective joint inertias must be first obtained as

following

JEFl = Dli+JmI (8.1)

JEF2 = D22+Jm2 (8.2)

JEF3 = D33+Jm3 (8.3)

where D11 , D22 and D33 are given in Chapter VII.

Substitution of the parameters, for e2=0 and 93=0 gives

JEFf = 39.133 oz.in.sec2

3EF2 = 39.133 oz.in.sec2


Plugging these results and parameters from Table 1 into

equation 3.1, the transfer functions of the three servo

motors are obtained as

9.88Gl (s) = rad/volt (8.4)

s(s/0.041+l) (s/9100+1)

9.88G2 (s) = rad/volt (8.5)

s(s/0.041+1) (s/9100+1)

9.88 "4

G3 (s) = rad/volt (8.6)s(s/0.39+1) (s/9100+1)

124

.r V %W

Using these results the gain constants of the model

motors (Km) were determined. The Open Loop Bode plots of the

three servo motors are depicted in Figures 8.1, 8.3 and 8.5,

respectively. These plots show that the -40 db/decade slope

intersects the 0 db axis at (01=0.65 rad/sec for the JOINT1

motor, W32=0.65 rad/sec for the JOINT2 motor and W3=2 rad/sec

for the JOINT3 motor. Making use of the above frequencies as .

gain crossover frequencies for the second order ideal

motors, we obtain

Kmi = 0.652 = 0.4225 rad/volt

Km2 = 0.652 = 0.4225 rad/volt

Km3 = 22 = 4 rad/volt

The Open Loop Bode plots of the Km/s2 ideal motors are

depicted in Figures 8.2, 8.4 and 8.6.

125

.. '

... . . ..... .... . ..... ........ ... .. .

.........

. . ...... ... ... . .... ... ......

E--

CC

......... ._..._..._

LCi

C)O Oflw m

j _j ..... .. .....

0* 6 1 -IZI alof Glw 3. a.

.... .... .... .. .... . .... .. .. ... ... ... ... ... .. .... ... ... ... ... .... .... ..

...... ....... ....... ...... ... .......... ... .. .. ...... ...... .... . .... . .. ...

!- ...... .

.. ... ... ... .. . .... .

CD

m I.-

Lj CEJ *............. ....... ..... ..... . ..... ................ .. ..

cm .... ............... ............. ... .. ..... ......

009 - 0 'a-)tw ~

C a-

U _n C.

L i . .... ... .... .. . .. ... .

Figure 8.2 Open Loop Bode Plot of the Kml/s2 Motor

127

3SWW~i

su~~ ~ ..... .~i ...- ..~ .... ~.

cn I--.. 0.:C

ot C ) _j.... .............. .. .... ..

.... .... .... ... .. . ... ... ..

.. .... ....

C.5-Li L CL .. . ... ........a-_ Ln C

C-'

Nd"-J,*..

z.5Z

Figure 8.3 Open Loop Bode Plot of the JOINT2 Servo Motor

128

3S"S

Wab- ~wt- G~nt 0*09- WK- a

.... .... ... . ..... .... .. .... ... ...

... .. ....? ... .... .. .......... . ....

.... .... .... ... ... .... .... . .. .............. ..........

.... .... .. .... ... . .... . .. ..

.... .... ... .. . ... ... ... ... . .

CD.

m tme

a-u m ~ C) w- o~c oa ~ -W O - ! .. . . ...... ......... ..... ............ .... . -.

.. .. .. .. ........... ......... ..... L

i C ..... .... .... .... . ..... .. ... .... .... '.. .. . ..... ....

I-- ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ ~ - I-................ ... . .......

LaiI

LiJ

... ... .. .....

CQCDWar 06.19

(GI W IN.

Fic 2Moto.jur 8.4 Ope Loo Bod Plt ofthe m2/

129~

a- - 09 - 6,0- Go-.'K 'K

.. .. . .. . .. . . .. . .

,.) on (m2 P

w* CD -- ...... .......

UC

Z -

Li

...... *.... . ...

. .. . . .... . . . ..

-giiniN8

Figure 8.5 Open Loop Bode Plot of the JOINT3 Servo Motor

130

20 Z,

0* a- VIM- - -0*- - *W 00- a, -b -.

.... . ......... .... .. .... . ....... ... ... ... . ... ........ ... .. .. .... ... ......

... ... ... .... .. ... .. ... ....... ..... ...... ... I .... ... .. ....... ... ... ... ... .. .... ... ....... ......

........ .......

.... . ...... . .......... .... ... ...... ..... ....... ............. . ......

4 . .. .. 4.. .. . . . .. . ..

CC

C

Li -I-

.. .. . . .. .. .... ... .. . .

I-6

(9 I aS * 5WH

Figue 8. Opn Lop Boe Pot o theKm3s 2 oto

1315

IX. SIMULATION OF THE THREE LINK REVOLUTE ROBOT

A. PLANNING THE SIMULATION STUDIES

In this chapter, the model developed in Chapter VII is

tested for the Voltage Source Drive. First the model is L

tested in a gravity-free environment and then the same tests

are repeated with gravitational torques taken into account.

The tested arm is a point-to-point arm and for this

thesis a free work space is assumed. A unit step input

command is applied in all joints at the same time to assure

realistic interaction between the three links. For each

simulation run the phase plane plot, the step response and

the error curves are obtained in order to study the behavior

of the system.k!

B. SIMULATION STUDIES OF THE ADAPTIVE SYSTEM

1. Gravity-free Environment

The three link revolute robot in a gravity-free

environment is tested first under different load conditions,

then for disturbance rejection and finally the robustness of

the system is tested for a slight variation of the

parametric data of the servo motors.


The adaptive model is tested first without

carrying any load and then under different load conditions,

132

using the DSL/VS simulation program listed in Appendix H.

Figures 9.1, 9.2 and 9.3 illustrate the phase plane plot,

the step response and the error curves, respectively, for

the unloaded arm.

Figure 9.1 shows that the model and servo motors

have good curve following characteristics for all joints.

Observing the phase plane plots for JOINT1 and JOINT2, one

would expect JOINT2 to reach the commanded position faster

than JOINT1. Contradictorily, the step response of FigureD

9.2 shows that JOINT1 reaches steady state a little earlier

than JOINT2. The explanation is that the upward motion of

the LINK3 creates an interaction torque on JOINT2 servo

motor and it takes time (about 40 msec) for the JOINT2 to

overcome the reaction torque and start accelerating. The

time required for the movement of the arm to be completed is Z

the time required by the slowest link (LINK2) to complete

its movement. From Figure 9.2 this time is read 335 msec.

The error curves of Figure 9.3 show zero steady state error

for the three links, but a more accurate observation of the

simulation data reveals a steady state accuracy of the order

of 10- 5 inches.

The simulation results of the loaded arm under

different load conditions are depicted in Figures 9.4-

9.15. In these figures the effects of the added load can be

observed. The phase plane plot of Figure 9.4 shows that at

the beginning of the motion, JOINT2 servo motor develops a

133

-a

I

, .. 4.. . .... ,-. .. . .. -

24

IYG I I I I

4-

CR3 (RAO J

.13

-

- 1 0.1 0.3 0.5 07 0.9 1.1

"0.1 0.1 0.3 0. 7 0.9 1.1""

CR3 (RMO)

PHASE PLA NE IC0T. CR0T. X0T. V5,CRI "'

a.a..

Figure 9.1 Phase Plane Trajectory for Unloaded Arm(No Gravity)

134

.3 _ ____ ___ ____ ___ ____ ____ ___ __"_ .3134 I

. '.. 41.;-,,.;'T,; < ": '' ,"0.1 :'- -"3 0.5:< 0.7 0.9 1.1 <'3< ""

- Y• v c - +' .r. .-. % .*.-.-, -, . . , .- ., . . . ,,. , -. .d* -. r. % ,L- ,J' Lr R + M. s.,- .- '.i .'

1. 4 1.4

4t.4

1.-)0c 200 300 400 5 1.4%T IME (10*3 SEC]

1.0- -- -- 1.0

.0.2-

100 200 300 400 50l 1.4TIME (1*O3SEC)

1.0 -- --------- 1.0

0.6- 0.5

:j -A

cc.

I T

0 100 200 300 400 SaoTIME {103SEC]

STEP RESPONSE

1 . 1) -. 1 .

135

O .-- oJ.

"o-:- -~- -0.2'... p " ,t t . ,--

I

0%

O.-0.0 0.0 L

0.- L-

TIM 1.? 25EC

o ,o No Gravity)Q

136-

.............

-5.

small negative velocity (in the opposite direction of the N

desired move). This negative velocity is caused by the

torque created on the JOINT2 due to coupling inertia between

JOINT2 and JOINT3. This torque is proportional to the

acceleration of JOINT3 which is large at the beginning of

the move. As the acceleration of JOINT3 starts decreasing,

JOINT2 servo motor overcomes this torque and starts moving

in the direction of the commanded move. When it reaches the

commanded curve it can not follow the curve due to the

interactive torque of the still accelerating JOINT3 servo

motor. As soon as JOINT3 reaches the commanded position, it

starts decelerating and the JOINT2 servo motor catches the

commanded velocity curve and follows it down to the

commanded position.

The step response and the error curves of Figure

9.5 and 9.6 show the initial movement of JOINT2 in the

opposite direction. The other two joints move directly to

the commanded position. The joint mostly affected by the

load is JOINT2 because the added load increases the coupling

inertia between JOINT2 and JOINT3 and consequently increases

the torque applied on JOINT2.

If the added load increases Figure 9.7 shows

that the negative velocity of JOINT2 at the beginning of the

motion becomes greater and the settling time for JOINT2 and

JOINT3 increase, as depicted in Figure 9.8 and 9.9. But as

more load is added, the Coriolis force acting on JOINT1, due

137

I

24' 24

0.1 0.3 0.5 0.7 !0 1.CR3 (RAO)

6- 46

3A

I t. II I. II--. 0.1 0.3 0.5 0.7 1.1

CR2 I RFI)

6-6

3- -

00 N a

13-01 01 0.3 0.5S 0. 7 0.9S .

p I I (RFIO I

PMASE PLN (C0T.CRDTX00T.VS.CRI

L

Figure 9.4 Phase Plane Trajectory for Loaded Arm(Load=O.04 NMo Gravity)

138

)1'

-. 2- 0.2 -- S-

• 0.2

T. I M 0-3 S'E"

1.0 - - - - - -- -

0.6

0 .2- 0.2 .'+

0.0 2 .--. 5

1.-)Ic 200 300 400 1 1.

TIME (10- SEC"0.- 1.0

i °+ °..

. - -0.2 "00 200 300 400 5 0.0-

TIME (10*3SECj .A

STEP RESPONSE "

• .-S

% ,

Figure 9.5 Step Response for Loaded Arm "(Load=0.04 -No Gravity) ]

139 "

0 100 200 00 40 50TIME (1O.SEC

• " "" J q J+ I j III . M J J) milM jlk~iNW~~ j l m " N k I lP l lISTEP RESPONSElte' " %' ''' ""' ' " " '''

L.; +"Wlu- r 1 m_. _ qu.__ __ ~a~m-vm m__ l~l~w~w.W-. lquw mm q~wmq__~m m q .q q._! u ,q q * l~t *'t q ' e*'u q*"u5 _5

1.1.2 1.2

1.1.0 1.0

0.-.8 a.

0.6- 0.6 0.6

0.4- 0.4 0.4

0.2- 0.2 0.Z

\ IIT a. -0.2 L -0.2

0 00zo 300 400 500TIME 103'SEC)

ERROR .VS. TIME

Figure 9.6 Error Curves for Loaded Arm(Load=O.04 -No Gravity)

140I".. '.'-'-' ' ' o,-," " Figure'- .. " 9.6 Error Curves"" for"' Loaded A"-rm -r. .".-,'".,.' '" " -'/'' ''i.""' '''N

24 2' I

A.3

- 01 0.3 S. 0. '7 09 1.CR3 f RP431

rr 6

.31 .3.0..50. 0. 0.9 1.1a.

0.1 0.3 CR2 ROi

*La~~ NoGrviy

141-

' !p

1.4" 1.4

I -. I.

0.26 0.-

-0.? -0.?

I0a 200 300 400 50 1.TIME l0 'SEC,

-0.2

100 200 300 40 1.4TIME 110"3SEC4

".5

0. -- .

0.-0.6

r 0.2

0 100 200 300 400 S00TIME 1I0- SECI

STEP RESPONSE

Figure 9.8 Step Response for Loaded Arm(Load=O.l - No Gravity)

142

%j, °

o~o- h o e o.8

11.2\p

11.0~

0. 1.2 1.2

1 p..

a. 0.6 0.6

--

-0.

0 Ion 20 300 40a S00TIME (10'3 SFCI

ERROR VS. TImEi wN.,

t,.

Figure 9.9 Error Curves for Loaded Arm(Load=O.1 - No Gravity)

143

LIS& T

. . . . - Q w , - • . • - -• • . -" - o" ".'. -" -°% \ *"

' % -'. o. - - % ' %°

to the angular velocities at all joints, increases and the

increased torque at JOINT1 is the reason that the JOINT1

servo motor can not follow the commanded velocity curve as

soon as it reaches the curve. But as JOINT3 starts

decelerating, the torque at JOINT1 decreases and finally

JOINT1 catches and follows the commanded curve to the

desired position. Because of the increased angular velocity

of JOINT1 its settling time is reduced.

Figure 9.10 show that the critical load for this

robot configuration is 0.157 oz/in/sec2 because for this

value of load, JOINT1 catches the commanded curve just at

the commanded position. Also it can be observed from the

same figure that while the initial negative velocity of

JOINT2 is decreased, JOINT2 is capable to follow the

commanded curve as soon as it reaches it. Figures 9.11 and

9.12 are the step response and the error curves,

respectively, for this critical load.

For any load greater than the critical one,

JOINT1 catches the commanded curve after it reaches the

commanded position. So, when JOINT1 reaches the commanded

position its velocity is not zero but still has a value

which is proportional to the load. As a result of this fact,

JOINT1 overshoots before it reaches steady state condition.

As the added load is further increased (load=0.2

oz/in/sec2 ), JOINT2 starts overshooting too, as illustrated

in Figures 9.13 - 9.15.

144

24" 24

0-%

6-5

3-

-1 0.1 0.3 0.5 0.7 0.9 1.CR3 CRAOI

6

0-I

CR II RFAOJ

% %

Z---

I

S1.4" 104

1.0

0.2

0. ' 0.6

"0.2- 0.2

.1r

1.4-) ,o0 200 300 40o Sol .

TIME t'1O 35ECI "

0.6- 0.6

0.2- O.2.

0.2o'

10Ic 206 300 400 50a 1.

TIME IIo"3SECJ

S1.0' P. '

+II 0t+,, ,, TIRE fi0"35EC) "

STEP RESPONSE

Figure 9.11 Step Response for Loaded Arm(Load=0.157 - No Gravity)

146 *

+ ,, ,i,, h+,,,.-;a; ., n

1. 0-

0.6 0.8 0.6

0.0.46.

1 0.4 0.2

0.0 0.2 0.0

-020.-.

100 200 300 400 S00TIME (io*35EC)

ERROR .VS. TIME

Figure 9.12 Error Curves for Loaded Arm(Load=O.157 - No Gravity)

147

II

1 0.1 0.3 0. RO1.7 0.9 11

.31.

-. 1 0.1 0'.3 0'S 0.7? 0.9 1-1Ci2 (RPOI

148

% V.

1.41.1.

1.0- 1.0 ".

0...

-0.2 - eI I I

100 200 300 400 S00 So5 .TIME 10-3SEC)

.2- 0.2

-.2 e,

1 100 200 300 400 500 60 1.4TIME 110*3SECJ

0.0- 0.6 -

am 0.D B

-0. - 0.2 G

a 100 200 300 400 s00 600 'a

TIME C10*35EC)STEP RESPONSE

II

Figure 9.14 Step Response for Loaded Arm(load=0.2 - No Gravity)

149-a

a-

, :- - .% -j .k ,.-.-, ..., .:- a -"" """'" " "''"< r

* .u. * . -. ' -. -- - -k - -- . , ,.,, ,.., -. ... -,.

N"Ll

.0

0.0.8 0.6

, 0.4 0.4

0.2 0.? 0.2

.. 2 L 0.0"

0 100 200 30,0 400a S00 600TIMlE 1O03SEC]

ERROR .VS. TIME ~ ~

.l

I ;'.


150

From the study of the above results for the

three link revolute robot operating under different load

conditions, it becomes obvious that the greater the added

load the greater the initial negative velocity of JOINT2 and

the greater overshooting for the JOINT1 and JOINT2 servo

motors.


To test the capability of the three link

revolute robot to reject random disturbances, an impulse

with an amplitude of 50 rad and a duration of 10 msec is

applied to three joints in different times during their

motion. Making use of the DSL/VS program listed in Appendix N

I, first the disturbance is applied just before JOINT3

reaches steady state, then the same disturbance is applied

just before JOINT1 and JOINT2 reach steady state and finally

after all joints have reached steady state.

In the first case Figure 9.16 shows that the

applied disturbance causes JOINT3 to be unable to catch the V.

curve before it reaches the commanded position and therefore

JOINT3 overshoots as illustrated in Figure 9.17. In the

second case Figure 9.18 shows that due to the applied

disturbance JOINT1 and JOINT2 catch the curve just after

they pass the commanded position and the resulting small

overshooting of these joints is shown in Figure 9.19. In the

third case, Figure 9.20 shows that the robot is disturbed

151

|, < ,. ... ..... .,.. - .- . . . . .. ..... N * .'i ,a .. ., ,. . ... . .. - .. . ... .... ,,: ., ... - ,,,., . -

'is

24o 24

16 16

I [ I I I I I I I I I

1 0.1 0.3 0.5 0.7 0.9 1.1 -CR3 (RRO,

rr 6

4I-3

U,- 1 0.1 0.3 0.5 0.7 0.9 1.1CR2 (RAOGI

3- 3

I I -

.0.1 0.1 0.3 0.S 0.7 0.9 1.1CR rRO I

PHRSE PLNE ICDT,CROT.XOCT.VS.CRI

.

Figure 9.16 Phase Plane Trajectory with Disturbance

Applied at T=90 msec (No Gravity)

152

-,,.~ .~~U ~ - : * ~V. Y-~-AVUW 1~ ~.,

4

0.2- 1.0 ;

1O.4- 20a 30 00 s; R%4" TIME (10"3SECI .

0.6- 0.6

0.2- 0.2

__0.? "0.2 L.00 200 300 4 0 50 1.4TIME (10-3 SEC)

1.0

0.6

0.2- 0.2

5

TiME ( '*3 EC)0

STEP RESPONSE

Figure 9.17 Step Response with DisturbanceApplied at T=90 msec (No Gravity)

153

Mg P V 4 '! 4 ~ A f% . ~ rq . .%~...*

t4 24

16

0.1 0.3 0.5 0.7? a.

CR3 MRO)

6

.31.

I I I I I I I I I F I

O'1 0.1 0.3 0.5 0.7 0.9 1.1CR2 IRMO)

-0.1 0.1 0.3 O.s 0.7- 0.9 1'.1

P'HASE PLANE (CDT.CROT.XOOT.VS.CRJ .


154

1,4.

4.6 0.6

ar 0.2 0.2 -

*r - -0.214316200 300 400 S .

10 TIME (103 EC) .

0. rr0.5

0.2~

14)100 z00 300 400 Sl 1.4T IME (10*3SECI

-- - - - -- 1.0 4

.0.2 0.2

0 l0g 200 300 400 So0TIMlE flO-2SEC1

STEP RESPONSE

Figure 9.19 Step Response with Disturbance '

Applied at T=270 msec (No Gravity)

155

24

m I I s - . 4

0.1 0.3 0.5 0. 7 0 .9 .

dd

1P3RS.1 0.3 4 ST 0' 7 .. 1.1

0--

I (RAM

PHASE POANE ICOT.CROTXOO)T.VS.CRJ


156

.F

1.4.

10.4 1.

0.6- - 0.5

0.2- 0.2

_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ - 0 . 2 0 4

100 200 300 400 5015TIME (10'3SEC) 1.4

.0.50 0.2

10 2100 300 400 5oo

1.4%

157%

1. N.

after all joints have reach steady state condition and the

overshooting of all joints can be observed in Figure 9.21.Although the same disturbance is applied to all

joints, the joint mostly affected is JOINT3 which is the

lightest one. Finally in all different cases the simulation

results show that the applied disturbances are rejected and

all joints return to the commanded position. Therefore, the

tested robot is capable to reject any random disturbance.

c. Robustness

Next the adaptive algorithm is tested for slight

(10%) variations of the servo motors parametric data in

order to check if the designed system is robust. For these

tests the simulation program listed in Appendix H is used

again, changing some of the parametric values for each

different run.

First the torque constant (Kt) and the back emf

constant (Kv) are changed 10% and the results are shown in

Figures 9.22 - 9.25. The phase plane plots of Figures 9.22

and 9.24 show very good curve following characteristics for

positive or negative change of the above constants. The step

response of Figures 9.23 and 9.24 are nearly identical and

only an accurate observation of the simulation data reveals

a small variation in the settling time which has the same

sign with the variation of the two constants. .

Then the mechanical pole of the servo motor is

moved toward the origin and the effects of this change in

158

24 24 U

16

3- 33.1

8. I I I I 8p

1 0.1 0.3 0.5 0.7 0.9CR3 IRRAOI 9

..

F T I I I I,I 0.1 0.3 0.5 _.I 0.2 . ,

CR2 I I O

-.3 0._ _ 0. _ _ _._ __.7_o .3t.,

PHAS5E PLAINE (COT.CROT.XCOTVS.CRI

Figure 9.22 Phase Plane Trajectory for Unloaded Armwith K t and K v Increased by 10% (No Gravity)159

..

0.r 0.r. ..

.

0'S.

.

1.43 1.4

1..1.0 '

T i E 0 E °.0

0.28 C

-0.2 -0.2 -"

100 zoo 300 400 sag 1

TIME C10"3 SEC)

- -o .- - - .-0.

-

.0.2 -a.2 Z

10oezoo 306 400 Soo1

TiME (10-3SEC)

1.00 "-.

0 0020.0040 0TIME I10.SEC

STEP RESPONSE

- "

Figure 9.23 Step Response for Unloaded Arm withKt and Kv Increased by 10% (No Gravity) 0

160

I'

.

-- 7

24 24

0'- So'1 1o. . _.__ _ _ _ _ _ _ _.__ _ _ _ _ _ _ _ _ _ ._ _ -

1 0.1 0.3 0.5 0.7 O.

II II I T I I I-. 1 0.1 0.3 O.S 0.7 0.9 1.1CR IROI

i F':I'.-3 , 0...

PHRSE PLPNE (COT, CROT. XOT. VS. CRI

--'..Figure 9.24 Phase Plane Trajectory Foi Unloaded Arms. with Kt and Kv Decreased by 10% (No Gravity)

.1161

%".

1.41

Ic 00 30404o 1.4T I M (10- SEC

1.0

0.6

p -0.2

100 200 300 400 Sol

0.

00.2-

0 100 zo0 300 400 5Soo.TIME 1Q3SECI .

10.2

At

or2 r rP.W r%

:h:

the behavior of the system are observed. Maximum movement

of the pole is achieved when the resistance (R) is decreased

and the inductance (L) is increased with the same amount

(10%), simultaneously. In comparison with Figures 9.1 and

9.2, Figures 9.26 and 9.27 show that this movement does not

affect at all the behavior of the unloaded arm. But Figures

9.28 - 9.31 show that the loaded arm does not overshoot for

loads up to 0.185 oz/in/sec2. Therefore, as result of this

movement of the mechanical pole of the servo motor, the load

for which overshooting starts is increased from 0.157 to

0.185 oz/in/sec2 , which corresponds to 18% increase of the

critical load.

After the mechanical pole has been moved toward

the origin, the constants Kt and Kv are changed again and

the effects of this combination of changes are observed.

Figures 9.32 - 9.35 show that for 10% increased values of Kt

and Kv the critical load is further more increased to the

value of 0.215 oz/in/sec2 , while for decreased values of the

above constants by the same amount, Figures 9.36 - 9.39 show

that the critical load is reduced to 0.160 oz/in/sec2 .

From the results of the above tests, it becomes

clear that any small change in the parameters of the servo

motors, which is possible in practice, does not affect

dramatically the behavior of the system. Therefore, the

designed robot arm is robust.

163

,r

24 24

I 16

- 0

1 . 0.3 0'.5 0.7 a 9 "

I IRC

3 .I'.

.31?

"0.1 0.1 0.3 0.5 0.7 0.9 1.1

CRI [MO J

PHRISE PLOtNE (COT. CRODTXDOT. VS. CRI

Figure 9.26 Phase Plane Trajectory for Unloaded Arm withR Decreased and L Increased by 10% (No Gravity)

164

1.4" 1.4

1.0

.- 0.6

ar 2-O.cc.

143100 2010 300c 400 so 1.4TIME (10*3 SECJ

1.0-- - 1.0

!,

0-6- 0.6

0. 0.2

Ica 200 300 400 SaxTIME (103 SEC)

1.0-- - -------

L

0.5 0.2

0.2

S-0.20 100 200 300 400 Soo

TIME f OS SECJ

STEP RESPONSE

Figure 9.27 Step Response for Unloaded Arm withR Decreased and L Increased by 10% (No Gravity)

Nk

165

• . I , , 4 4' 4 4 ,, ,s4Jua. -.. j-, -,.-s- .. , , . , ". % . %" y %"•" . .v "*. %' '" ",' . '-

U 24

01

I I I I I a

-1 0.1 0.3 0.5 0.7 0.9 1.1CR3 IRF0J

3- 39

0--

§-,1 0.1 0.3 0.5 0.7 0.9CR2 (R190J9

.39

-0.- 0. .3 05 0. .

CR1 IRSh

0.' d0. 8 0. 0.o 0.9vity)

C166~3 9

1.1.0

0.6

cd 0.2-0.2 z

__ _ _ _ _ __0_ _ _ _ _ _ _ __ _ _ _ _ _ _ -0.2 t

1.-)a0 200 300 400 Sl 1.4

-. - - -- 1.0 lJ

0.6

cd 0.2-

10Ic 200 300 400 Soo 1.TIMlE (10-'SECI

(Load0.18 No Ga0.ty

.

1.67~J

-~~~~LW -J ?r 1.Y -- -.. ~-

24

- 1 = 0.1 0.3 0.5 0.7 0.9 1.1

I.31

1 0.1 0.3 0.5 0.7 0.9 1.1&R2 (RSOF

0. 0. .

Cil (RAO

PMS LN (0,RTX0TV.R

Fiue93 hs ln rjetr o oddAmwt

R erae n Icesdb 0

____________- __NoGravity)

4- 1 __ __ __ __ ___ __ __ ___168__

1. 1.4

1.0

0.. 0.6 ,J

-0..-

1.4) 100 200 300 400 50; 1.4

1..

cr.1 00.5

Lai .. 2 T 0.2~

1.0- Ica20 300 400 S0

--. 0-- -- - .

0.5 I0.

00.?

0100 200 300 400 500 .TIME 110*3SEC) l

STEP RESPONSE

Figure 9.31. Step Response for Loaded Arm withR Decreased and L Increased by 101


169

24 24

0 .1 0.3 0.5 0.7 0.9 1.1 9CR3 fRRCj

a 6

I I I I I I I I I

.1 0.1 0.3 0.5 0.7 0.9 1.1CRI rRFMI

PHASE PLANE (COT.CROT.XDCT.VS.CRJ

N Figure 9.32 Phase Plane Trajectory for Loaded Arm withR Decreased and Kt, Ky, L Increased by 10%

(Load=0.2. - No Gravity)

170

1.4" 1.4

1.0

-. 2

Cr

-0.2100 200 300 400 S .

TIME '10'3 5EC)

00.6-,

1.4- 1.0

100 200 300o 400 ,o

TIME (10"3 SECJ

1.0

0. 6-0a'

p.0

a 2o zoo 300a 400SoTIME 110*3SECi

STEP RESPONSE

Figure 9.33 Step Response for Loaded Arm withR Decreased and Kt, Kv , L Increased by 10%


171

4" " 24

, Is

. --

bp 000. .5 0.7 0.

?."I.

C3 (R0AO]

R Dcrasd ndKt K~L nceaedby10

,L-d

(Lod=.2-- o Gavty

01 0'.3 0'.5 0.7 09CR2 IRM) 9: +

PHASE POPNE (C0T CR0T X0 T. V5CR! ,

Figure 9.34 Phase Plane Trajectory for Loaded Arm with, R Decreased and Kt, Kv, L Increased by 10%

~(Load=0.22 - No Gravity)

172

, 'p

1.4~

1.100

0.5- 0.6

g.2

100 200 300 400 0 1.4

1. -------------------------------------- 1.0 4

0.6- 0.6

0.2 U

100 200 300 400 SolT I ME (10 3SEC 11.

0. 0.6

02

0 106 200 300 4 00 Sa0TirE rl0-SECi

STEP RESPONSE

Figure 9.35 Step Response for Loaded Arm withR Decreased and Kt, Kv L Increased by 10%


173

24" 24

I 0.1 0.3 0.5 07 09 1.1 9

6

1 0.1 0.3 0.5 0.7 0.9 '~

I.

CR1 tRROJ

PHASE PLANE (CDT. CRCT.*XCOT.VS. CR)



174

5,. 53~ % O %

1.4- 1.4

1.

0.2

1.-D00 200 300 4001.TIME !IO1ISEC)

-- - - - --. - T 1.0 '

dr 00.6

-0.2 1T.-0.

-T 10 TME (10 -3SEC) 40 1.4 i

C 0.-0.6

-0.21 0. 2

00 2100 30 0 4010se

TIME (1O*3SZCi 9

STEP RESPONSE

Figure 9.37 Step Response for Loaded Arm withKt, Kv, R Decreased and L Increased by 10in-.


175

2C 24

1 16

. 1= 0.1 0.3 0.5 0.7 0.9 .1 9

3- 3-

I-3

1 0.1 0.3 0.5 0.7 0.9 9CR2 IRAGJ

-01 0.1 03 0.5 0.S.CR1 (RAO)

"HSE POMNE (COTSCROT.XDOT.VS.CRJ



176 p

1.4-1.

Cr. 0.2-0.6

Tp -0.2

Ica10 200 30040so 1.

0.6- 0.6

Cr.U 0.2--U.2

1.r 100 no0 300 40 So 1.TIME 110-3SEC)

.Jp 0.5

TV -0.2 U

Ica0 200 300 0SaTIME (10S3ECJ

STEP RESPONSE

Figure 9.39 Step Response for Loaded Armi withKt, Kv, R Decreased and L Increased by 10%

(Load=0..165 - No Gravity)

177

V.,

2. Gravitational Torgues Included

The same tests are applied to the adaptive model

operating under gravitational torques. First a unit step

is used as input in all joints and then some combinations of

positive and negative unit step input commands are used for

JOINT2 and JOINT3, to test the system for the existence of

excess interactive torques.


The phase plane plot of the unloaded arm, which

is depicted in Figure 9.40, show that at the beginning of

the movement JOINT2 servo motor develops negative velocity, r

which has larger magnitude than the respective one when thein.

arm operates in a no gravitational environment because thegravitational torque is added to the torque developed on

JOINT2 due to coupling inertia between JOINT2 and JOINT3. As

the acceleration of JOINT3 starts decreasing, JOINT2 servomotor overcomes the disturbance torque and starts moving in

the commanded direction. When it reaches the curve, it

follows the curve down to the commanded position. The other

two joints show very good curve following characteristics.

The step response and error curves of Figures

9.41 and 9.42 show clearly the motion of JOINT2 in the

opposite direction, initially. Figure 9.41 shows also that

it takes about 100 msec for JOINT2 to overcome the reaction

torque. The time required for the movement to be completed

is read as 420 msec, which is 25% increased comparing to the

178

'S.

- S S -- - - - , . a- a S

t1 0.1 03 .5 .7 0.9 11.1C3(RAG I

1 -- 3. 0. . . . .

CR (F,

A, I I I I (RO I 1I

PHAE LR4 (OCR(RHOT.OTV.

Fiue94 hs ln rjcoyfrUlae r

(With ravity

517

F e'r

- 1 _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _Z

1.4

0. S

0.2 .

. .4

147 100 200 300 400 so.;~J

.- 1.0

0.- 0.6

-0.? "0.2

-0.2 .

10 200 3010 400 Sol

-- - - --. - .

-. "0.6

1.0 4

0.22

100 200 3 010 400 S6O.

Figure 9.41 Step Response for Unloaded Arm(With Gravity)

180

.-

,%, . ..,•+ .,, "_'.' " "2-"..' ,, . -.- . . -' .. 4 . . "" ". "* 4. 4' , 4i'i .** .i li l l l l l i i i

1.2 1.2

1.- 1.0 1.0 "-

0.8°

0. 0.6 0.6

0.4- 0.4 0.4

0.2- 0.2 O.z

o.0- 0-. . . .0 0.o0L IC

r~ -0.2. _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ -0.2I -.@ .tl0 10,0 zo0 300 400a 500

TIME (10-3SEC!

ERROR .VS. TIME V

Figure 9.42 Error Curves for Unloaded Arm(With Gravity) 0

181 %.1%

respective time for the no gravity case. The simulation data

reveal a steady state error of the order of 10-5 inches.

In the case of a loaded arm, a larger magnitude

of negative velocity is observed in the phase plane plots of

Figures 9.43 and 9.46, because of the greater gravitational

torques. The other two joints continue to show very good

curve following characteristics and all joints are capable

to follow the commanded curve immediately after they catch

it. The maximum load that the designed robot arm can carry

was found to be 0.195 oz/in/sec2. From the step responses of

Figures 9.44 and 9.47 it can be observed that the developed

reaction torque at the beginning of the movement results in

JOINT2 servo motor moving initially in the opposite

direction. The joint mostly affected by the load is JOINT2

* and the settling time for this joint increases proportional

*. to the added load. The error curves of Figure 9.45 and 9.48

show that the error of JOINT2 at the beginning of the

movement is also proportional to the carried load and the

effects of the added load in the settling time of JOINT2

servo motor can be observed clearly.

In some applications the initially developed

negative velocity of JOINT2 may cause problems. To avoid

this undesired situation, a step input command delayed by

150 msec is applied to JOINT3 servo motor. Figure 9.49 shows

that the initially developed negative velocity of JOINT2

servo motor is eliminated and a zig - zag shape in its phase

182

I

24

is , 'U. o. '. '. , ,

IG-s

0.1 0.3 0.5 0.7 0.9 1.1CR3 fRIOJ

3

CR2 (RROI ".

3- 3

31~~

-0. 0.1 0.3, 05 0.7 0., ,1...

&~I (lqMI*PHASE PLANE rCOT.CROT.XOOT.VS.CR)

Figure 9.43 Phase Plane Trajectory for Loaded Arm it(Load=0.04 - With Gravity) ,

183

1.4 1.4

1.0

0.26 0.2

0.0.T

100 206 300 400 50 1.4

1.0 - - - .0

.7 -0.25

100 200 306 400So .T IME (10'3SECJ I.

0.-0.6 f.e

atf

-o..,, 0.2

0 06 20o 306 40'0 Sa0TIME 110-3SEC)

STEP RESPONSE

Figure 9.44 Step Response for Loaded Arm(Load-O.04 - With Gravity)

184

I-

J

IX 1.2?.

1.1.0 1.0

0.0 0.8 0.8

. r0.6 0.6

0 0.4 .4

0. Z-0.2 - 0.2

0.- - - - -0.0 - 0.0~

0 100 200 300 400 500ITIME (10O3SEC)

ERROR .VS. TIME

Figure 9.45 Error Curves for Loaded Arm(Load=O.04 - With Gravity)

1851

r r r I~ c '", r - N' , 0

\ "

24" 24 1

1 t 16 .

10,4 -0.2 0.0 0.2 0.4 0.6 0.8a 1.0 8

4- 4

4 ,,

.44

-0.4 -0.2 0.0 0.2 0.4 0.5 0.8 1.0 1.2

CIRI RM

PHASE PLANE (COTCRDT.XDOT.VS.CRI

Figure 9.46 Phase Plane Trajectory for Loaded Arm(Load=O.195 - With Gravity)

186

- - .%.- V. - 7 7 7 -

1. 1.4

ada 0.2

0.

0.4 1.2 IM (E 1.6 1.5

- - - -- - - - - - .0

.0 0.4 0.8 1.2 1.6 2.0 2.4TIME (SEC)

ccadO9 0.- Wih.rviy

p-0..1 18

0. .420 .

~~~~~~~~~~~~~~T~ N (-SECI* ? *P~~/.J%~..r~-~ b ~ %~

p.

I'_

1.4 1.4 1.4

1.2- 1.2 1.2

.o 1.0 1.0

0. j 0.8 0.8

0..1 0.8

0.4- 0.4 0.4

0.0- 0.2 0.2

.--. o., I~ -o-p,0.0 0.00.

-0.2- TI. (E).2 0.0

0.0 0.4 0.8 1.2 1.5 2.0 2.4TIME (SEC)

ERROR .VS. TIME

Figure 9.48 Error Curves for Loaded Arm(Load=O.195 - With Gravity)

188

7""

I

S T" I I J I F I 1 .31- 0 0.3 0.5 0.7 0.9 1.1

o- V

,.,

S. 0 0.7

I~ ~ ~~& 1 RAO)I II*01 0.1 0.3 0.5 0.9 1.1CRI (RAO)

189,

._ O" "-

-5

'."

Y l | T ] ' I T4,

'p,

(0.o0. d 0. 0 5 07it 0ravity)

C1 9 .R

O

-- " " " " ' " " " ",' " " ' " '4'J'" 4' . . . . "" ' r ' "

"P'J " " " " "% . "

"4",r" ",r",r"w"%" "" "' "r "",,,-,

11,4

0. C.

1.4- c o 0 o 1.4

1.0- - - - - - - - - - - - - - - - 1.0

0. F

0. Z___0.2

14100 200 300 400 Sl 1.4

ata

.0.2 0.6

100 200 300 400 SoT iME (10* SEC2

190?

'0 plane plot is observed due to reaction torque produced by

this servo motor. The step response of Figure 9.50 shows

that the settling time for JOINT2 servo motor remains the

same while the movement of JOINT2 in the opposite direction

is eliminated.

Then some combinations of positive and negative

unit step input commands are applied to JOINT2 and JOINT3

servo motors to simulate the movements of LINK2 and LINK3 in

the direction of gravity. Figures 9.51 - 9.54 show that when

both LINK2 and LINK3 move in the direction of gravity (move

#2), JOINT3 overshoots for a relatively small load (0.06

oz/in/sec 2 ) . The phase plane plots of these figures show

that for a considerable part of their movement, JOINT1 and

JOINT2 move with velocity greater than the commanded one. In

the case that LINK2 moves in the direction of gravity and

LINK3 moves against gravity (move #3), Figures 9. 55 - 9. 58

show that JOINT2 overshoots for a slightly larger load but

JOINT1 and JOINT3 follows accurately the commanded curve.

The DSL/VS simulation program used to test the

three link revolute robot operating in a gravitational

environment under d'fferent load conditions is listed in

Appendix J.


Making use of the DSL/VS simulation drogram

listed in Appendix K, the same disturbances mentioned in

part l.b of this chapter are applied to the robot arm

191

fl1.2 NTOU IC ITLO UTNTO(U AA OTJ /St012 ATTCOO NONTEY OF G O P NOTOIR )A OS TRO E DEC I

WICLRSSIFIED F/U 12/9 ML

EEEEEEEEEEEEEEEEEEEEEE

EEEEEEEEE

3 --J1

.... w-ff6 1.12.0

".25 11111.416

%. .% %

m m • • ___ - .... 0

r r " -' '%--" -- %" V'.- '" ,,.%r,. 9, _% m' -- %- ,.% % -'h % , % %- , % % 9. t, ', , m "

.24 -241T - - I r 9 1 1

k2 *t.0 -0.8 -0.6 -0.4 -0.2 0.0 UZ2CR3 IRAG])

0

2- -

-0.2 0.0 0.2 0.4 0.6 0.9 1.0 1.2C1( O

PHASE PLANE (COT.CROT.XOOT.VS.CRI

Figure 9.51 Phase Plane Trajectory for Move #2(Load=O.055 - With Gravity)

192

D<~

-0.2 -0.2

0.6 0.2

0.

0100 200 300 400 So o z0. TIME (103SEC) I

Figu. 9.2Se0.5onefr oe#

(Lod.0-5 - Wit1.0vty

ii TitlE 193EC

I I I

-24 .241

12 -0 0.8 -0.6 -0.4 -0.2 0.02CR3 (RFIOJ

.2- .2

-1.0 -0.85 -0.6 -0.4 -0.2 a.o

-p4-

-2

-0.2 o00 0.2 0.4 0.6 0.8 1.0 1.2CR1 CRMO2

PHASE PLANE ICDT,CRD',.XOOT.VS.CR)

* Figure 9.53 Phase Plane Trajectory for Move #2(Load=O.065 - With Gravity)

194

* -0.2

-4-

cZ3 100 200 300 400 sox0 .0T TIME (10*3SEC)

*0. -0.2

1. - 1.

1.4, 100 ZOO 306 400 at1.4T IME f 10'3SEC)

-- - - --. - t.

0.0.5

cd= .2-0.2

0 100 2-00 30040SoTIME 110*1SECI

STEP RESPONSE

Figure 9.54 Step Response for Move #2(Load=O.065 - With Gravity)

195

16.1

e. .02 0.4 0.8 . 2CR3 I RAIO

Si.2

a-.4

-2 -o0-. .6 -0.4 -0.2 0.CR2 I ROiJ

a. 00 0.2 0.4 0.5 0.6 1.0 1.2CRI (RAG)

"t~E PL.ANE (COT.CRDTXOOT.vS.CRI

Figure 9.55 Phase Plane Trajectory for Move #3(Load=O.06 - With Gravity)

196

1. 1. 4

a~r 0.2

at-.2 Q -JL.1

I00 200 300 400 so; 0.2TIME (10'3SEC)

-0.2-*0. 2

0. -0.6

r -1.4 Jew100 200 300 40So 1.

TIME rto-3SEC)

--0 - - - - - - --- -1.

0. 22-

a.;'.

0 100 6 300 40Or 540TIME r 0IS5ECj

STEP RESPONSE

Figure 9.56 Step Response for Move #3(Load=O.06 - With Gravity)

197%

24' 24

2a 0.0 0.2 0.4 6 0'.8 1 .0

CR3 RA6O

.2.

.44

rr 6

-0.2 0.0 0.2 .0.4 0.19 0.8 0.0 01.2CR2 ("Of3

198~

1.4.

-0.2- 3.2

crr

-r CIA

T E(10,3 SEC I 05

1.0-- - - - - - - - - - - -- 1.0

0.6 .

0.2.

cr. 0.2

0 TIMIE f300 SC

STEP RSOS

Figue 9.8 Sep Rspone fr Moe #(Load0.07- Wih Grvity

199.

lb

operating under gravitational torques. Simulation results

illustrate the nearly identical behavior of the robot arm

operating either under gravitational torques or in a gravity

free environment. Figures 9.59 and 9.60 show that the

applied disturbances are rejected and all joints reach the

desired position.

c. Robustness

Finally, the revolute robot is tested again for

slight (10%) variation of the servo motors parameters.

Simulation results do not reveal any noteworthy change in

the behavior of the system which is almost identical to the

behavior of the system in the no gravity case, as depicted

in the indicative Figures 9.61 and 9.62.

20o

S .

200 ""S

S.'

24' 24

I I I Ir

I |

* 0.1 0.3 0.5 0.7 0.9 1.1CR3 (R801'

*2 -3- 3-

.I 3 0.9 1.1

6-

3- 3

U' 1 _ _ _ _ _ _.3__ _ _ _

T 1 I I 1 I

.0.1 0.1 0.3 0.5 0.7 0.9 1.1

PHASE PIANE tCOT.CROT.XOOT.VS.CRI

.,

Figure 9.59 Phase Plane Trajectory with DisturbanceApplied T=90 msec (With Gravity)

201

%'a'

-" " " j~r " V, 6:, , "4," 5.. -,)..*J. *p - .. P J -.- . r" -. -p .P - V -. Jw - V - P - . . -. V . . . . , .-.-.- " .": ,

1, t1.4

1.0

S.?

100 200 300 400 s;1.4TIME (IO3 SEC)

1. -

00.

0.2- 0. z

1.-)lo 200 300 400SoTIMlE (10*3 ECI1.

0.5 IOiz

T 0.2

0 0 200 300 400 S00TIME (10,3SEC)

STEP RESPONSE

Figures 9.60 Step Response with DisturbanceApplied at T=90 msec (With Gravity)

202 -

24

1 16

- 0.1 0.3 0.5 0.7 0.9 1.1CR3 fRROI

3-

* 1I 0.1 0.3 0.5 0.7 0.9 1.

I T

-01 0.1 0.3 0.5 0.7 0.9 1.1CR1 IRFOI

PMASE PLANE (COT.CROT.XOOT.VS.CRI

Figures 9.61 Phase Plane Trajectory for Loaded Arm withKt, Ky, R Decreased and L increased by 10%


203

r,--- .-- * . 7 . 1_,-V. 77 - 7-- - - r -- 777

1.4 1.4 !1.0.6

0.2

1o 200 300 400 s 1.4TiME (10 3tSE.Cj 14

1. - 1.0 -

0.0.6

0.2. F0.Z~ 1

3100 2 36 00SaSTIME (10 3 EC 1.4

1. 1.0

0 0.6 t

0.2

0 100 206 300 ,00 SooTIME I10",5EC) NO

STEP RESPONSE

Figure 9.62 Step Response for Loaded Arm with -Kt, Kv , R Decreased and L Increased by 10% '


204 1,I

S. S." .*''*%S. ,-. r. ,r S. S.- p

X. CONCLUSIONS / AREAS FOR FURTHER STUDY

As a result of the research conducted in this thesis,

the control of both the three link rectangular and revolute

robots, using the curve following technique with an adaptive

simulation model appears feasible.

In this study, direct-drive servo motors were used. The

use of these motors has several importanrt advantages -

including no backlash, small friction and high mechanical

stiffness, but the complicated dynamics resulting from

inertia, interactions and nonlinearites is also more

prominent than that of a robot with gears because in this

case the disturbance torques are reduced proportionally to

the gear ratio.

Both configurations were tested for different load

conditions, for random disturbance rejection and for

robustness in the case of servo motor parameter variations.

Initially, a gravity-free environment was assumed and then

the same tests were repeated with gravitational torques

included. In all cases actuator dynamics, coupling inertia,

centripetal and coriolis forces were included.

From simulation results it was observed that as the

added load was increased the robot arms started overshooting

which may cause problems in some applications. The

capability of the arms to carry larger loads may be improved

205 "A'

if the curve scaling constant, K1 , is designed to be

adaptive. Further thesis research in this case is required.

The randomly applied disturbances were rejected in all cases

and the arms were driven to the commanded position. The

slight variation of the servo motor parameters did not show

any noteworthy change in the behavior of both robot arms.

A delayed step input command was used to prevent

undesired motion of the revolute configuration, caused by

interactive torques between links. In order that this

technique to be effectively used, an algorithm must be

developed which will take into account the initial

orientation of the arm and the commanded direction of

motion, and will determine the magnitude of the input step

command and the optimum delay time.

For this study a rigid arm was assumed for both robot

configurations. In rigid arms the speed is greatly limited

by the weight of the arm. This weight also increases the

size of the required servo motors as well as the power

consumption. From this fact, an area for further study is to

model an arm which consists of either only flexible links or

both rigid and flexible links.

206

'p"%

DSL PROGRAM FOR THE SIMULATION OF THE SECOND ORDER MODEL

TITLE THIS PROGRAM INVESTIGATES THE ABILITY OF THE BASICTITLE MODEL TO FOLLOW THE CURVE WHEN VELOCITY CURVETITLE FOLLOWING TECHNIQUE IS USED.PARAM K=1.0,K1=0.6,K2=10000.0,KM=59.29,VSAT=150.0,RO=0.5PARAM R=1.0INITIAL

A=SQRT (2. *K*VSAT)CF=1./RO '

REF=R*CFXDOT=O.0

DYNAMICRTH=REF*STEP (0.0)E=RTH-CIF(E.LT.0.0)XDOT=-A*Kl*SQRT(ABS(E))IF (E.GE.0.0) XDOT=A*K1*SQRT (E)

DERIVATIVEKCDOT=K*CDOTXDOTE=XDOT-KCDOTV=LIMIT (VSAT,VSAT, K2*XDOTE)CDDOT=V*KMCDOT=INTGRL( 0.0, CDDOT) pC=INTGRL(0.0, CDOT)CX=C*1 ./CFXDOTX=XDOT*1 ./CFCDOTX=CDOT* 1./CF

TERMINALMETHOD RKSFXCONTRL FINTIM=0.1, DELT=0.00005SAVE (S1)0.001,CX,XDOTX,CDOTXSAVE (S2) 0.001, CX, RGRAPH(G1/S1,DE=TEK618,PO=1, .5) CX(LE=6.5,UN='INCHES') ... e

XDOTX(LE-8,NI=8,LO=0. ,UN-'INCHES/SEC' ,SC=$AR)..CDOTX(LE=8,NI=8, LO=0. ,UN=' INCHES/SEC' ,PO=6 .5,SC=$AR)

GRAPH(G2/S2,DE=TEK618,PO=1,1) TIME(LE=6,UN='SEC'),....CX(LE=8,NI=6,LO=0,UN='INCHES' ,SC=0.25),.R(LE=8, NI=6, LO=0 ,SC=0.25 ,AX=OMIT)

LABEL (Gi) PHASE PLANELABEL (Gi) XDOTX,CDOTX VS CXLABEL (G2) STEP RESPONSE ..

LABEL (G2) CX VS TIMEEND

aSTOP

207

APEDIX~

DERIVATION OF MATHEMATICAL MODEL FOR THE THREE LINKRECTANGULAR ROBOT

According to Newton's equation of motion, the force Fi

applied to the link i, by the joint motor i, in the

direction of the motion of the link i, is

Fi = Hi(&ri-g) (B.1)

The total torque about the axis of rotation of the motor i

is given by the equation

Ti = Fir+Jmiei (B.2)

where r is the radius of the pinion.

Therefore for the three links of the rectangular robot,

the equations of motion are

F1 = M1x (B.3)

F2 = M2Y (B.4)

F3 = M3 (i-g) (B.5)

and the corresponding total torques are

T= Mlxr+J.m1 el (B.6)

T= M2yr+Jm2e2 (B.7)L

T3 =M 3 (Z*-g)r+Jm 3*93 (B.8)

208 4

But the linear motion of the link i is related to the

angular motion of the motor i with

s1 - eir (B.9)

Substituting equation B.9 into equations B.6, B.7, and B.8

T= (Mlr 2+j mi)e9i (B.10)

= Mr+m 2)e2 (B. 11)

T= (M3r2+j m3)e3-M3gr (B.12)

N N

DSL PROGRAM FOR THE THREE LINK RECTANGULAR ROBOTUNDER DIFFERENT LOAD CONDITIONS (NO GRAVITY)

TITLE SIMULATION PROGRAM FOR THE RECTANGULAR COORDINATETITLE CONFIGURATION UNDER DIFFERENT LOAD CONDITIONSPARAM K=1.0,KI=0.6,K2=10000.0,KM1=59.29,KM2=90.25,KM3=77.44PARAM VSAT-150.0,MI=0.082,M2=0.041,M3=0.041,MM=0.186PARAM JI-0.033,J2=0.033,J3=0.033,RI=0.91,R2=0.91,R3=0.91PARAM KT1=14.4,KT2=14.4,KT3=14.4,LI=.0001,L2=.0001,L3=.0001PARAM BM1=0.04297,BM2=0.04297,BM3=0.04297,RO=0.5,LOAD=O.OPARAM KV1=0.1012,KV2=0.1012,KV3=0.1012,T=0.00025PARAM RX=1.00 ,RY=1.00 ,RZ=1.00INTGER SW1,SW2,SW3,N1,N2,N3* KI: THE CURVE SCALING CONSTANT* K2: THE AMPLIFIER GAIN* KM: THE INITIAL CONSTANT OF THE IDEAL (MODEL) MOTOR* VSAT: THE SATURATION LIMITS OF THE AMPLIFIER* K: THE VELOCITY LOOP FEEDBACK GAIN (OF THE MODEL)* RX,RY,RZ:THE COMMANDED TIP POSITION IN INCHES* RO: THE RADIUS OF THE PINION GEARING* T: THE SAMPLING INTERVALINITIAL

SwI=oSW2=0SW3=0N1-0N2=0N3=0XlDOT=O.X2DOT=0.X3DOT=O.TL1=0.TL2=0.TL3=0.CF=1./ROA1=SQRT(2. *KM1*VSAT)A2=SQRT (2. *KM2 *VSAT)A3=SQRT(2 . *KM3*VSAT)REF1=RX*CFREF2=RY*CFREF3=RZ*CF

DERIVATIVETM1=M1+M2+M3+2*MM+LOADTM2=M2+LOADTM3=M2+M3+MM+LOADJM1=TM1*RO

210

3142=TM2 *ROJM3=TM3 *ROJTOT1=J 1+JM1JTOT2 =J2 +312JTOT3-J3+JM3RTH1=REF1*STEP(O. 0)RTH2=REF2*STEP(O.0)RTH3=REF3*STEP(O. 0)E 1=RTHl-C 1E2=RTH2 -C2E3=RTH3 -C3ERX=RX-CRXERY=RY-CRYERZ=RZ -CRZ

NOSORTIF(El.LT.0.0) X1DOT=-A1*K1*SQRT(ABS(El))IF(E1.GE.O.0) X1DOT=A1*K1*SQRT(E1)IF(E2.LT.O.0) X2DOT=-A2*Kl*SQRT(ABS(E2))IF(E2.GE.O.O) X2DOT=A2*K1*SQRT(E2)IF(E3.LT.0.0) X3DOT=-A3*K1*SQRT(ABS(E3))IF(E3.GE.0.0) X3DOT=A3*K1*SQRT(E3)

SORT

KClDOT=K*C1DOTX1DOTE=X1DOT-KClDOTVi-LIMIT (-VSAT,VSAT, K2*XIDOTE)ClDDT=V1*KM1ClDOT-INTGRL( 0.0, C1DDT)Cl=INTGRL(O.0,ClDOT)CX=Cl* 1./CF

MP1=REALPL(0. 0,L1/R1,VM1/R1)NT1=KT1*MP1MT1E=MT1-BM1*CRlDOT-TL1CR1DDT=(1./JTOT1) *14flECR1DOT=INTGRL (0.0, CRlDDT)CR1=INTGRL (0.0, CRIDOT)CRX=CR1*1 ./CFXXDOT=X1DOT*1./CFXXDOTE=X1DOTE*1 ./CFCXDOT=C1DOT*1 ./CFCRXDOT=CR1DOT* 1./CF

KC2 DOT=K*C2 DOTX2 DOTE=X2 DOT-KC2 DOTV2=LIMIT (-VSAT, VSAT, K2 *X2 DOTE)C2DDT=V2*KM2

211

C2DO-INTRL(.0,CDDT

C2OTINTGRL( .0, C2DT)

CY=C2*1 ./CFVM2-V2 -KV2 *CR2 DOTMP2-REALPL(0. 0, L2/R2 ,VM2/R2)MT2-KT2 *MP2MT2E=MT2 -BM2 *CR2 DOT-TL2CR2DDT-(1./JTOT2) *M2ECR2DOT=-INTGRL(0.0, CR2DDT)CR2=INTGRL(0. 0,CR2DOT)CRY=CR2*1 ./CFXYDOT=-X2 DOT* 1./CFXYDOTE=X2DOTE*1 ./CF

CYDOT-C2DO*1./C

CYDOT=C2DOT*1./CF

KC3 DOT=K*C3 DOTX3 DOTE=X3 DOT-KC3 DOTV3-LIMIT (-VSAT, VSAT, K2 *X3 DOTE)C3 DDT=V3 *KG3C3 DOT=INTGRL (0. 0, C3 DDT)C3=INTGRL(0. 0,C3DOT)CZ=C3*1 ./CFVM3-V3-KV3-"CR3 DOTMP3=REALPL(0.0, L3/R3 ,VM3/R3)MT3-KT3 *MP3MT3E-MT3 -BM3 *CR3 DOT-TL3CR3DDT=(1./JTOT3) *M3ECR3DOT=INTGRL(0.0, CR3DDT)CR3-INTGRL(0. 0, CR3DOT)CRZ=CR3*1 ./CFXZDOT=-X3DOT*1 ./CFXZDOTE=X3DOTE*1 ./CFCZDOT=C3DOT*1 ./CFCRZDOT=CR3DOT*1. /CF

SAMPLENOSORT

IF (N1.EQ.0.O) GO TO 10IF (C1DOT.GT.X1DOT) SW1=1IF (SW1.EQ.1) GO TO 20KS1=ABS (2. *CR1)/ (((N1*T) **2) *V1)KM1=KS 1

20 CONTINUECl=CR1C1DOT=CR1DOT

10 Nl=Nl+l

212

Ike-

CONTINUEIF (N2.EQ.O.0) GO TO 30IF (C2DOT.GT.X2DOT) SW2=1IF (SW2.EQ.1) GO TO 40KS2=ABS(2.*CR2)/ ( ((N2*T) **2) *V2)KM2=KS2

40 CONTINUEC2=CR2C2DOT-CR2DOT

30 N2=N2+1CONTINUEIF (N3.EQ.0.0) GO TO 50IF (C3DOT.GT.X3DOT) SW3=1IF (SW3.EQ.1) GO TO 60KS3=ABS(2.*CR3)/( ((N3*T) **2) *V3)KM3=KS3

60 CONTINUE-C3=CR3C3DOT=CR3 DOT

50 N3=N3+1CONTINUE

SORT

TERMINALMETHOD RKSFXCONTRL FINTIM=0.06,DELT=0.00005,DELS=0.00025SAVE (Si) 0.0004,XXDOT,XYDOT,XZDOT,CXDOT,CYDOT,CZDOT,....

CRXDOT, CRYDOT, CRZDOT, CRX,CRYCRZSAVE (S2) 0. 001,CX,CRX,RX,CY,CRY,RY,CZ,CRZ,RZSAVE (S3) 0.001,ERX,ERY,ERZGRAPH (G1/Si,DE=TEK618,PO=1) CRX(LE=6.0,UN='INCHES'),..

CXDOT(NI=3,LO=0,SC=20,UN='INCHES/SEC'),....ICRXDOT(NI=3,LO=0,PO=6.0,SC=20,UN='INCHES/SECI) ....XXDOT (NI=3 ,LO=0 ,SC=20 ,AX=OMIT)

GRAPH (G2/Si,DE=TEK618,OV,PO=1,3.25)..CRY(LE=6.0,UN=IINCHESI),..CYDOT(NI=3,LO=0,SC=25,UN='INCHES/SEC'),..CRYDOT(NI=3,LO=0,PO=6.0,SC=25,UN='INCHES/SEC') ..XYDOT (NI=3 ,LO=0 ,SC=25, AX=OMIT)

GRAPH (G3/S1,DE=TEK618,OV,PO=1,6.5) ...CRZ(LE=6.0,UN='INCHES') ..CZDOT(NI=3,L0=0,SC=20,UN='INCHES/SEC'),....CRZDOT(NI=3,LO=0,PO=6.0,SC=20,UN='INCHES/SEC'),..XZDOT (NI=3, LO=0 ,SC=20, AX=OMIT)

GRAPH (G4/S2,DE=TEK618,PO=1) TIME(LE=6.0,NI=6,UN='SEC') ....CX(NI=3,LO=0.0,UN='INCHES',SC=0.5,PO=6.0), ...CRX(NI=3,LO=0.0,UN='INCHES',SC=0.5) ....RX(NI-3,LO=0.0,SC=0.5,AX=OMIT)

GRAPH (G5/S2,DE=TEK618,OV,PO=1,3.25) ...

213 .

TIME(LE=6.0,NI=6,UN='SEC') ....CY(NI=3,LO=O.0,UN'INCHES' ,SC=O.5,PO=6.O),..CRY(NI=3,LO=0.0,UN='INCHES' ,SC=0.5) ..RY(NI=3, LO=O. O,SC=O. 5,AX=OMIT)

GRAPH (G6/S2,DE=TEK618,OVPO=1,6.5) ...TIME(LE=6.O,NI=6,UN='SEC') ..CZ(NI=3,LO=0.O,UN='INCHES',SC=O.5,PO=6.O) ....CRZ (NI-3,L0=0. 0,UN= IINCHES'I , SC=O. 5)RZ (1*=3,10=0. 0,SC=0. 5AX=OMIT)

GRAPH (G7/S3,DE=-TEK61B,PO=1, .5) ...TIME(LE=5.O,NI=6,UN='SEC'),..ERX(LE-8,NI=1O,LO=O.O,UN='INCHES',SC=.1),...ERY(LE=8,NI=1O,LO=0.O,UN=IINCHESI,SC=.1,PO=5.O),.ERZ(LE=8,NI=10,LoO-O.O,UN-'INCHES' ,SC=.1,PO=6.2)

LABEL (Gi) PHASE PLANE (CDOT,CRDOT,XDOT.VS.CR)LABEL (G4) STEP RESPONSELABEL (G 7) ERROR .VS. TIMEEND- -

STOP

21

M -

APPENfDIX~ p

,.%

DSL PROGRAM FOR THE THREE LINK RECTANGULAR ROBOTWITH DISTURBANCE (NO GRAVITY)

TITLE SIMULATION PROGRAM FOR THE RECTANGULAR COORDINATETITLE CONFIGURATION WITH DISTURBANCE (NO GRAVITY)PARAM K1.0,KI=0.6,K2=10000.0, KM1=59.29,KM2=90.25,KM3=77.44PARAM VSAT=150.0,MI=0.082,M2=0.041,M3=0.041,MM=0.186PARAM Jl=0.033,J2=0.033,J3=0.033,R=0.91,R2=0.91,R3=0.91PARAM KT1=14.4,KT2=14.4,KT3=14.4,LI=.0001,L2=.0001,L3=.0001 ..PARAM BM1=0.04297,BM2=0.04297,BM3=0.04297,RO=0.5,LOAD=0.0PARAM KV1=0.1012,KV2=0.1012,KV3=0.1012,T=0.00025PARAM RX=1.00 ,RY=1.00 ,RZ=1.00

- - INTGER SWISW2,SW3,.II,NZ,N3* KI: THE CURVE SCALING CONSTANT* K2: THE AMPLIFIER GAIN* KM: THE INITIAL CONSTANT OF THE IDEAL (MODEL) MOTOR* VSAT: THE SATURATION LIMITS OF THE AMPLIFIER* K: THE VELOCITY LOOP FEEDBACK GAIN (OF THE MODEL)* RX,RY,RZ:THE COMMANDED TIP POSITION IN INCHES* RO: THE RADIUS OF THE PINION GEARING* T: THE SAMPLING INTERVALINITIAL

SWI=0SW2=0 .SW3=0N1-ON2=0 .N3=0XIDOT=O.X2DOT=O.X3DOT=O.TL1=0.TL2=0. "-UTL3=0.CF=1./ROA1=SQRT (2. *KM1*VSAT)A2=SQRT(2. *KM2*VSAT)A3=SQRT(2. *KM3*VSAT)REF1=RX*CFREF2=RY*CFREF3=RZ*CF

DERIVATIVETM1=M+M2+M3+2*MM+LOADTM2=M2+LOADTM3=M2+M3+MM+LOADJM1=TM1*RO

215

JM2=TM2 *RO I

JM3='rM3 *ROJTOTI=J1+JM1JTOT2-J2+JM2JTOT3=J3+JM3RTH1=REF1*STEP(O.O)+1O*(STEP(0.01O)-STEP(0.012))RTH2=REF2*STEP(0.0)+10*(STEP(0.O10)-STEP(0.012))RTH3=REF3*STEP(O.0)+1O*(STEP(O.O1O) -STEP(0.012))E1=RTHI-C1E2-RTH2 -C2E3 =RTH3 -C3

NOSORTIF(Ei.LT.O.0) X1DOT=-Al*K1*SQRT(ABS(E1))IF(E1.GE.0.0) XlDOT=A1*K1*SQRT(E1)IF(E2.LT.0.0) X2DOT=-A2*K1*SQRT(ABS(E2))IF(E2.GE.0.0) X2DOT=A2*K1*SQRT(E2)IF(E3.LT.0.0) X3DOT=-A3*Kl*SQRT(ABS(E3))IF(E3.GE.0.0) X3DOT=A3*K1*SQRT(E3)

SORT

KClDOT=K*C1DOTXlDOTE=X1 DOT-KC1DOTV1=LIMIT (-VSAT, VSAT, K2 *X1DOTE)C1DDT=-V1*KM1C1DOT=INTGRL( 0.0, C1DDT)C1=INTGRL(0.0, ClDOT)CX=C1*1 ./CFVMi=V1-KV1 *CR1DOT a

MPi=REALPL(0.0, Li/Ri ,VM1/R1) -

MTi=KT1 *MPiMTiE=MT1-BMi *C~DOT-TL1CR1DDT=(1./JTOT1) *MlECR1DOT=INTGRL (0.0, CR1DDT)CR1=INTGRL( 0.0, CRiDOT)

CRX=CRi*1./CFXXDOT=-X1DOT*1 ./CF

* CXDOT=ClDOT*1./CFCRXDOT=CR1DOT* 1./CF

KC2 DOT=K*C2 DOTX2 DOTE=X2 DOT-KC2 DOTV2=LIMIT (-VSAT,VSAT, K2*X2DOTE) 1C2 DDT=V2 *KMJ2C2DOT=INTGRL(0.0, C2DDT)

* C2=INTGRL(0.0,C2DOT)CY=C2*1 ./CFVM2=V2-KV2*CR2DOT

216 '

V~ ~ ~ ~~~~~~~~V. V- .1? VV V-U'W~VW v -- -~ - a.

MP2=REALPL (0.0, L2/R2 ,VM2/R2)MT2=KT2 *MP2MT2E=MT2-BM2 *CR2DOT-TL2CR2DDT= (1./JTOT2) *M42ECR2DOT--INTGRL(0. 0,CR2DDT)CR2=INTGRL(0,CR2DOT)CRY=CR2 *1./CFXYDOT=X2DOT*1 ./CF4CYDOT=C2DOT*1 ./CFCRY DOT=-CR2 DOT* 1./CF

KC3DOT=K*C3DOTX3 DOTE=X3 DOT-KC3 DOTV3=LIMIT (-VSAT, VSAT, K2 *X3 DOTE)C3DDT=V3 *K3C3DOT=INTGRL (0.0 ,C3DDT)C3=INTGRL(0. 0, C3DOT)CZ=C3 *1./CFVM3=V3 -KV3 *CR3 DOTMP3=REALPL (0.0, L3/R3, VM3/R3)M3=KT3*MP3MT3E=MT3-BM3 *CR3DOT-TL3CR3DDT=(1./JTOT3) *M3ECR3DOT=INTGRL(0. 0,CR3DDT)CR3=INTGRL(0. 0,CR3DOT)CRZ=CR3*1 ./CFXZDOT=X3DOT*1 ./CFCZDOT=C3DOT*1 ./CFCRZDOT=CR3DOT*1 ./CF

sAmrLENOSORT

IF (N1.EQ.0.0) GO TO 10IF (C1DOT.GT.X1DOT) SW1=1IF (SW1.EQ.1) GO TO 20KS1=ABS (2. *CR1)/ (( (N1*T) **2) *V1)KM1=KSI

20 CONTINUEC 1=CR1C1DOT=CR1DOT

10 N1=Nl+1CONTINUE

* IF (N2.EQ.0.O) GO TO 30IF (C2DOT.GT.X2DOT) SW2=1IF (SW2.EQ.1) GO TO 40 '

KS2=ABS(2.*CR2)/( ((N2*T) **2) *V2)KM2=KS2

217

2a

40 CONTINUEC2=CR2C2 DOT=-CR2 DOT

30 N2=N2+1CONTINUEIF (N3.EQ.0.O) GO TO 50IF (C3DOT.GT.X3DOT) SW3=1IF (SW3.EQ.1) GO TO 60KS3=ABS(2.*CR3)/( ((N3*T) **2) *V3)KM3=KS3

60 CONTINUEC3=CR3C3 DOT=CR3 DOT

50 N3=N3+1CONTINUE

SORT

TERMINALMETHOD RI(SFXCONTRL FINTIM=0.08,DELT=O.00005,DELS=O.00025SAVE (Si) 0.0001,XXDOT,XYDOT,XZDOT,CXDOT,CYDOT,CZDOT,...

CRXDOT, CRYDOT, CRZDOT, CRX,CRYCRZSAVE (S2) 0.0O1,CX,CRX,RX,CY,CRY,RY,CZ,CRZ,RZGRAPH (G1/S1,DE=TEK618,PO=1) ...

CRX(LE=6.0,NI=10,UN='INCHES'),..CXDOT(NI=3,LO=0,SC=30,UN='INCHES/SEC') ..CRXDOT(NI=3,LO=0,PO=6.O,SC=30,UN='INCHES/SEC') ....XXDOT (NI=3, LO=0 ,SC=30 ,AX=OMIT)

GRAPH (G2/S1,DE=TEK618,OV,PO=1,3.25) ...CRY(LE=6.0,NI=10,UN='INCHES'),...CYDOT(NI=3,LO=0,SC=30,UN='INCHES/SEC'),....CRYDOT(NI=3,LO=0,PO=6.O,SC=30,UN='INCHES/SEC'),....XYDOT (NI=3, LO=0 ,SC=30 ,AX=OMIT)

GRAPH (G3/Si,DE=TEK618,OV,PO=1,6.5)..CRZ(LE=6.0,NI=10,UN='INCHESI),..CZDOT(NI=3,LO=0,SC=30,UN='INCHES/SEC'),...CRZDOT(NI=3,LO=0,PO=6.0,SC=30,UN='INCHES/SEC'),....XZDOT (NI=3, LO= ,SC=30 ,AX=OMIT)

GRAPH (G4/S2,DE=TEK618,PO=1) TIME(LE=6.0,NI=8,UN='SEC'),....CX(NI=3,LO=0.0,UN='INCHES',SC=O.5,PO=6.0) ....CRX(NI=3,LO=0.0,UN='INCHES',SC=0.5) ....RX(NI=3,LO=0.0,SC=0.5,AX=OMIT)

GRAPH (G5/S2,DE=TEK618,OVPO=1,3.25) ...TIME(LE=6.0,NI=8,UN='SEC'),....CY(NI=3,LO=0.O,UN='INCHES',SC=O.5,PO=6.O), ...CRY(NI=3,LO=O.0,UN='INCHES' ,SC=0.5)I .RY(NI=3,LO=0. 0,SC=0.5,AX=OMIT)

GRAPH (G6/S2,DE=TEK618,OV,PO=1,6.5) ...TIME(LE=6.0,NI=8,UN='SEC'),.

218 V

CZ(NI-3,LiO=O.O,UN='INCHES',SC=O.5,Po=6.O) ....CRZ(NI=3,LO=O.O,UN='INCHES' ,SC=O.5),....RZ (NI=3,LO=O. O,SC-O.5,AX-OMIT)

LABEL (Gi) PHASE PLANE (CDOT,CRDOT,XDOT.VS.CR)LABEL (G4) STEP RESPONSE -

ENDSTOP

J

''p

219p

DSL PROGRAM FOR THE THRE LINK RECTANGULAR ROBOTUNDER DIFFERENT LOAD CONDITIONS(GRAVITATIONAL TORQUES INCLUDED)

TITLE SIMULATION PROGRAM FOR THE RECTANGULAR COORDINATETITLE CONFIGURATION UNDER DIFFERENT LOAD CONDITIONSTITLE (WITH GRAVITY)PARAM K=1.0,KI=0.6,K2=10000.0, KM1=59.29,KM2=90.25,KM3=77.44PARAM VSAT=150.0,MI=0.082 ,M2=0.041,M3=0.041,MM=0. 186PARAM Jl=0.033,J2=0.033,J3=0.033,RI=0.91,R2=0.91,R3=0.91PARAM KT1=14.4,KT2=14.4,KT3=14.4,LI=.0001,L2=.0001,L3=.0 0 0 1

PARAM BM1=0.04297,BM2=0.04297,BM3=0.04297,RO=0.5,LOAD=0.86PARAM KV1=0.1012,KV2=0.1012,KV3=0.1012,T=0.00025,G=386.4PARAM RX=1.00 ,RY=1.00 ,RZ=1.00INTGER SW1,SW2,SW3,N1,N2,N3* KI: THE CURVE SCALLING CONSTANT* K2: THE AMPLIFIER GAIN* KM: THE INITIAL CONSTANT OF THE IDEAL (MODEL) MOTOR* VSAT: THE SATURATION LIMITS OF THE AMPLIFIER* K: THE VELOCITY LOOP FEEDBACK GAIN (OF THE MODEL)* RX,RY,RZ: THE COMMANDED TIP POSITION IN INCHES* RO: THE RADIUS OF THE PINION GEARING* T: THE SAMPLING INTERVAL .

INITIALSWI=0SW2=0SW3 =0N1=0N2=0N3=0

XIDOT=O.X2DOT=O.X3DOT=O. 4'

TL1=0. 'TL2=0.TL3= (M2+M3+MM) *G*ROCF=1./ROAI=SQRT(2. *KM1*VSAT)A2=SQRT (2. *KM2*VSAT)A3=SQRT (2. *KM3 *VSAT)REF1=RX*CFREF2=RY*CFREF3=RZ*CF

DERIVATIVETM1=MI+M2+M3+2*MM+LOADTM2=M2+LOAD

220

i'= ,, _ 1__ , , = _-= , _' ,i ~ '1t ' J ' = _'t ,= 'l "t= =" I,' = -' = ,"= ",, J =' " % % % % % " " ' ' °

"," " ",P-=P *'=' ' F *4-4-

TM3=M2+M3+14H+LOADJMI=TM1 *ROJ3M2--T2*ROJM3=TM3 *ROJTOT1=Jl+JM1JTOT2=J2+JM2JTOT3=J3+JM3

* RTH1=REF1*STEP( 0.0)RTH2=R.EF2*STEP(O. 0)RTH3=REF3*STEP(0. 0)E1=RTHl-Cl

-~ E2-RTH2-C2E3=RTH3-C3ERX=RX-CRXERY=RY-CRYERZ=RZ -CRZ

NOSORTIF(E1.LT.0.0) X1DOT=-A1*K1*SQRT(ABS(E1))IF(E1.GE.0.0) X1DOT=A1*K1*SQRT(E1)IF(E2.LT.O.0) X2DOT=-A2*K1*SQRT(ABS(E2))IF(E2.GE.0.0) X2DOT=A2*K1*SQRT(E2)IF(E3.LT.0) X3DOT=-A3*K1*SQRT(ABS(E3))IF(E3.GE.0.0) X3DOT=A3*K1*SQRT(E3)

SORT

KC1DOT=K*ClDOTXl DOTE=X 1DOT-KC iDOTVl=LIMIT (-VSAT, VSAT, K2 *X1DOTE)ClDDT=Vl*KOflC1DOT=INTGRL (0.0, C1DDT)C1=INTGRL (0.0, CiDOT)CX=C1*1 ./CFVMl=Vl-KV1 *CR1DOTHP1=REALPL(0. 0,L1/R1,VM1/R1)MT1=KT1*MP1MT1E=MT1-BM1*CRlDOT-TLlCRlDDT=(1./JTOT1) *M~ECR1DOT=INTGRL(0.0, CR1DDT)

y CR1=INTGRL (0.0, CRiDOT)CRX=CR1* 1./CFXXDOT=XlDOT* 1./CFXXDOTE=XlDOTE* 1./CFCXDOT=C1DOT* 1./CFCRXDOT=CR1DOT*1 ./CF

KC2DOT=K*C2 DOTX2DOTE=X2DOT-KC2DOT

221

V2=LIMIT (-VSAT,VSAT, K2*X2DOTE)C2 DDT=-V2 *K(2C2DOT=INTGRL( 0.0, C2DDT)C2=INTGRL(O.0, C2DOT)CY=C2*1 O/CFVM2=V2 -KV2 *CR2 DOT14P2=REALPL(O.0, L2/R2 ,VM2/R2)MT2=KT2 *MP2MT2E=MT2 -BM2 *CR2 DOT-TL2CR2DDT(./JTOT2) *M2ECR2 DOT=INTGRL (0.0, CR2 DDT)CR2=INTGRL(0. 0,CR2DOT)CRY=CR2*1 ./CFXYDOT=X2DOT*1 ./CFXYDOTE=X2 DOTE* 1. / CFCYDOT=C2DOT*1 ./CFCRYDOT=CR2DOT*1 ./CF

KC3 DOT=K*C3 DOTX3 DOTE=X3 DOT-KC3 DOTV3=LIMIT (-VSAT ,VSAT, K2 *X3 DOTE)C3 DDT=V3 *KM3C3DOT=-INTGRL(0. 0,C3DDT)

* C3=INTGRL(0.0, C3DOT)CZ=C3*1 ./CFVH3=V3-KV3 *CR3 DOTMP3=REALPL (0.0, L3/R3, VM3/R3)MT3=T3 *MP3MT3E=MT3 -BM3 *CR3 DOT-TL3CR3DDT=(1./JTOT3) *M3ECR3DOT=INTGRL(0.0, CR3DDT)CR3=INTGRL(0. 0,CR3DOT)CRZ=CR3*1 ./CFXZDOT=X3DOT* 1./CFXZDOTE=X3DOTE*1 ./CFCZDOT=-C3DOT*1 ./CFCRZDOT=CR3DOT*1 ./CF

SAMPLENOSORT

IF (N1.EQ.0.0) GO TO 10IF (C1DOT.GT.X1DOT) SW1=1IF (SW1.EQ.1) GO TO 20KS1=ABS (2. *CR1)/ (((N1*T) **2) *V1)KM1=KS 1

20 CONTINUEC1=CR1

222

ClDOT=CRlDOT10 N1=Nl+1

CONTINUEIF (N2.EQ.0.0) GO TO 30IF (C2DOT.GT.X2DOT) SW2=1IF (SW2.EQ.1) GO TO 40KS2=ABS(2.*CR2)/(((N2*T)**2)*V2)KM2=KS2

40 CONTINUEC2=CR2C2 DOT=CR2 DOT

30 N2=N2+1CONTINUEIF (N3.EQ.0.0) GO TO 50IF (C3DOT.GT.X3DOT) SW3=1IF (SW3.EQ.1) GO TO 60KS3=ABS(2.*CR3)/( ((N3*T) **2) *V3)

* KM3=KS 3*60 CONTINUE

C3=CR3C3 DOT=CR3 DOT

50 N3=N3+1CONTINUE

SORT

TERMINALMETHOD RKSFXCONTRL FINTIM=O.1,DELT=O.00005,DELS=0.00025SAVE (Si) 0.0004,XXDOT,XYDOTXZDOT,CXDOT,CYDOT,CZDOT,....

CRXDOT, CRYDOT, CRZDOT, CRX, CRY, CRZP SAVE (S2) 0.001,CX,CRX,RX,CY,CRY,RY,CZ,CRZ,RZ

SAVE (S3) 0.001,ERX,ERY,ERZGRAPH (G1/S1,DE=TEK618,PO=1)..

CRX(LE=6.0,NI=6,UN='INCHES') ..CXDOT(NI=3,LO=-30,SC=30,UN='INCHES/SECI),..CRXDOT(NI=3,LO=-30,PO=6.0,SC=30,UN='INCHES/SEC'),..XXDOT(NI=3,LO=-30,SC=30,AX=OMIT)

GRAPH (G2/Sl,DE=TEK618,OV,PO=1, 3.25) ...CRY(LE=6.0,NI=6,UN='INCHES') ..CYDOT(NI=3,LO=-35,SC=35,UN='INCHES/SEC'),..CRYDOT(NI=3,LO=-35,PO=6.0,SC=35,UN='INCHES/SEC') ....XYDOT(NI=3 ,LO=-35,SC=35,AX=OMIT)

GRAPH (G3/S1,DE=TEK618,OV,PO=1,6.5) ...CRZ(LE=6.0,NI=6,UN='INCHES') ..CZDOT(NI=3,LO=-30,SC=30,UN='INCHES/SEC'),..CRZDOT(NI=3,LO=-30,PO=6.0,SC=30,UN='INCHES/SEC'),..XZDOT(NI=3,LO=-30,SC=30,AX=OMIT)

GRAPH (G4/S2,DE=TEK618,PO=1) TIME(LE=6.0,NI=5,UN='SEC'),....CX(NI=3,LO=0.0,UN='INCHES' ,SC=0.5,PO=6.0),..

223

Ww"Ayyr]La

CRX(NI-3,LO)=O.O,UN='INCHES',SC=O.5) ....RX(NI-3,LO=O. O,SC=O. 5,AX=OMIT)

GRAPH (G5/S2,DE-TEK618,OV,PO=1,3.25) ...TIME(LE=6.O,NI=5,UN='SEC'),..CY(NI=3,LO=O.O,UN=INCHES,SC=0.5,P0=6.O),...CRY(NI=3,LO-O.O,UN='INCHESI,SC=.5) ....

GPHRY (NI=3, LO=O * 0,SC=O.5 ,AX=OMIT)GAH(G6/S2, DE=TEK618 ,OV, P0=1, 6. 5) ...

TIME(LE=6.O,NI=5,UN='SEC'),--CZ(NI=3,LoO=0.O,UN='INCHES',SC=O.5,PO=6.O) ....CRZ(NI=3,LO=O.O,UN='INCHES' ,SC=O.5),..RZ (NI=3 ,LO=0.O,SCzO.5,AX=OMIT)

*GRAPH (G7/S3,DE=TEK618,PO=1,.5) ...TIME(LE=5.0,NI=5,UN='SEC') ..ERX(LE=8,NI-6,LO=-.2,UN='INCHES',SC=.2) ....ERY(LE=8,NI=6, LO=-. 2,UN=' INCHES' 'Sc-.2,PO=5.0),.ERZ(LE=8,NI=6,LO=-.2,UN='INCHES' ,SC=.2,PO=6.2)

LABEL (Gi) PHASE PLANE (CDOT,CRDOT,XDOT.VS.CR)LABEL (G4) STEP RESPONSELABEL (G7) ERROR .VS. TIMEEND

* STOP

* 224

DSL PROGRAM FOR THE THREE LINK RECTANGULAR ROBOT WITHDISTURBANCE (GRAVITATIONAL TORQUES INCIAWDED)

TITLE SIMULATION PROGRAM FOR THE RECTANGULAR COORDINATETITLE CONFIGURATION WITH DISTURBANCE (WITH GRAVITY)PARAM K=1.O,K1=O.6,K2=10000.OKM1=59.29,KM2=90.25,KM3=77.44PARAM VSAT=-150.O,Ml=O.082,M2=O.041,M3=O.041,MM=O. 186PARAM J1=O.033,J2=O.033,J3=O.033,R1=O.91,R2=O.91,R3=O.91PAR.AM KT1=14.4,KT2=14.4,KT3=14.4,L1=.OOO1,L2=.OOO1,L3=.OOO1

-. PARAM BM1=O.04297,BM2=O.04297,BM3=O.04297,RO=O.5,LOAD=O.OPARAM KV1=O.1012,KV2=O.1012,KV3=O.1012,T=O.00025,G=386.4PARAM RX=1.OO ,RY-1.00 ,RZ=1.00INTGER SW1,SW2,SW3,N1,N2,N3* Ki: THE CURVE SCALLING CONSTANT* K2: THE AMPLIFIER GAIN* KM: THE INITIAL CONSTANT OF THE IDEAL (MODEL) MOTOR* VSAT: THE SATURATION LIMITS OF THE AMPLIFIER* K: THE VELOCITY LOOP FEEDBACK GAIN (OF THE MODEL)* RX,RY,RZ:THE COMMANDED TIP POSITION IN INCHES* RO: THE RADIUS OF THE PINION GEARING

*a* T: THE SAMPLING INTERVALINITIAL

Sw1=oSW2=OSW3=O

.a N1=O'0 N2=0

N3=0X1DOT=O.X2DOT=O.X3DOT=O.TL1=O.TL2=O.

£ TL3= (M2+M3+MM) *G*ROCF=1./ROA1-SQRT (2. *KM1*VSAT)A2-SQRT(2 . *KQ2*VSAT)A3-SQRT(2. *K3*VSAT)RJEF1=RX*CFREF2-RY*CFREF3=RZ*CF

DERIVATIVETMI=M1+M2+M3+2 *MMJ+LOADTM2=M2+LOADTM3 =M2 +M3 +MM+ LOADJM 1=TM 1 *RO

225

JM2=-TM2 *ROJM3=TM3 *ROJTOTi=Ji+J'M1JTOT2=J2+JM2JTOT3=J3IJM3RTH1=REF1*STEP(0.0)+1O*(STEP(0.050)-STEP(O.052))RTH2=REF2*STEP(O.O)+iO*(STEP(O.050)-STEP(O.052))RTH3=REF3*STEP(O.O)+iO*(STEP(O.050)-STEP(0.052))E i=RTH I-ClE2=RTH2 -C2E3=RTH3-C3

NOSORTIF(El.LT.O.0) XlDOT=-Al*Kl*SQRT(ABS(E1))IF(El.GE.0.0) XlDOT=Al*Kl*SQRT(E1)IF(E2.LT.O.O) X2DOT--A2*K1*SQRT(ABS(E2))IF(E2.GE.0.0) X2DOT=A2*Kl*SQRT(E2)IF(E3.LT.0.0) X3DOT=-A3*Kl*SQRT(ABS(E3))IF(E3.GE.0.0) X3DOT=A3*K1*SQRT(E3)

SORT

KC1DOT=K*ClDOTXiDOTE=XlDOT-KClDOTV1=LIMIT (-VSAT, VSAT, K2 *X1DOTE)ClDDT=Vl*KMlCiDOT=INTGRL(0, CiDDT)C1=INTGRL(O. O,CiDOT)CX=Ci*i ./CFVMI=Vi-KVi *CR1DOTMPi=REALPL (0.0,R, VMi/Ri)MTi=KTl *MP1MT1E=MTi-BMi*CRiDOT-TLiCRiDDT=(i./JTOTi) *MiECRiDOT=INTGRL(0. 0, CR1DDT)CRl=INTGRL (0.0, CRiDOT)CRX=CRi*i ./CFXXDOT=XlDOT* 1./CFCXDOT=C1DOT*1 ./CFCRXDOT=CRiDOT*1 ./CF

KC2DOT=K*C2 DOTX2 DOTE=X2 DOT-KC2 DOTV2=LIMIT (-VSAT, VSAT, K2 *X2 DOTE)C2DDT=V2 *K2C2DOT=INTGRL(0.0, C2DDT)C2=INTGRL(0. 0,C2DOT)CY=C2*1 ./CFVM2=V2 -KV2 *CR2 DOT

226

KP2-REALPL (0 * 0,L2/R2,Vx2/R2)W3!2-KT2'MP2IT2E-XT2-EM2*CR21DOT-TL2CR2DDT- (1. /TOT2) *NT2EC2DOT-INTGRL( 0 0 CR2DDT)CR2-INTGRL(O. 0 CR2DOT)

CRY-CR*./CFXYDOT-X2DOT*1 ./CFCYDOT-C200T*1 ./CFcRtYDOT-ca2DOT' 1./CF

KC3 DOT-K*C3 DOTX3DOTE-X3 DOT-KC3 DOTV3-LIMT (-VSAT, VSAT, K2 X3D0TE)C3DDT-V3 'K13C3DOT-INTGRL(0. O,C3DDT)C3-INTGRL(O.O, C3DOT)CZ-C3'1 ./CF

RP3-1REALPL(O. O,L3/R3,VM3/R3)MT3-RT3 'MP3N3E NT(3 -BM3 'CR3 DOT-TL3CR3DDT-(l./JTOT3)*'NT3ECR3DOT-INTGRL (0.0 ,CR3DDT)CR3-INTGRL(0, CR3DOT)CRZ-CR3*1 ./CFXZDOT-X300T*1 ./CFCZDOT-C3DOT*1 ./CFCRZDOTm*2R3DOT* ./CF

SAMENOSORT

IF (m1.EQ.0.0) GO TO 10IF (ClDOT.GT.XlDOT) SWlmiIF (SW1.EQ.1) GO TO 20ES1-ABS (2.*'R1)/ (((N1'T)*"2) 'Vl)KI-KS 1

20 CONTINUECl1,CR1ClDOT-,CRlDOT

10 Nl-Nl+lCONTINUEIF (N2.EQ.0.0) GO TO 30IF (C2DOT.GT.X2D0T) SW2-1IF (SW2.EQ.1) GO TO 40IKS2-ABS(2.*CR2)/((C(N2*T)*"2) *V2)IKK2-KS2

227

40 CONTINUEC2=CR2C2 DOT-CR2 DOT

30 N2=N2+1CONTINUEIF (N3.EQ.0.0) GO TO 50IF (C3DOT.4GT.X3DOT) SW3=1IF (SW3.EQ.1) GO TO 60KS3=ABS(2.*CR3)/ ( ((N3*T) **2) *V3)K143=KS 3

60 CONTINUEC3=CR3C3DOT=CR3 DOT

50 N3=N3+1CONTINUE

SORT

TERMINALMETHOD RKSFXCONTRL FINTIM=0.08,DELT=0.00005,DELS=0.00025SAVE (Si) 0.0001,XXDOT,XYDOT,XZDOT,CXDOT,CYDOT,CZDOT...

CRXDOT, CRYDOT, CRZDOT, CRX, CRY, CRZSAVE (S2) 0.001,CX,CRX,RX,CY,CRY,RY,CZ,CRZ,RZG R APH (G 1/i1, DE = TE K6 1 8, P0 =1)CRX(LE=6.0,NI=11,UN='INCHES') ...

CXDOT(NI=3,LO=-30,SC=30,UN='INCHES/SEC'),..CRXDOT(NI=3,LO=-30,PO=6.0,SC=30,UN=IINCHES/SEC'),..XXDOT(NI=3,LO=-30,SC=30,AX=OMIT)

GRAPH (G2/S1,DE=TEK618,OV,PO=1,3.25)..CRY(LE=6.0,NI=11,UN='INCHES'),....CYDOT(NI=3,LO=-35,SC=35,UN='INCHES/SEC'),..CRYDOT(NI=3,LO=-35,PO=6.0,SC=35,UN=INCHES/SEC)...XYDOT (NI=3, LO=-3 5, SC=35 ,AX=OMIT)

GRAPH (G3/S1,DE=TEK618,OV,PO=1,6.5) ...CRZ(LE=6.0,NI=11,UN='INCHES'),..CZDOT(NI=3,LO=-30,SC=30,UN='INCHES/SEC'),..CRZDOT(NI=3,LO=-30,PO=6.0,SC=30,UN='INCHES/SEC'),..XZDOT(NI=3 ,LO=-30,SC=30,AX=OMIT)

GRAPH (G4/S2,DE=TEK618,PO=1) TIME(LE=6.0,NI=8,UN='SEC'),....CX(NI-3,LO=0.0,UN='INCHES',SC=0.5,PO=6.0) ....CRX(NI=3,LO=0.0,UN='INCHES',SC=0.5),....RX(NI=3,LO=0. 0,SC=0.5,AX=OMIT)

GRAPH (G5/S2,DE=TEK618,OV,PO=1,3.25) ...TIME(LE=6.0,NI=8,UN='SEC'),..CY(NI=3,LO=0.0,UN='INCHES',SC=0.5,PO=6.0),....CRY(NI=3,LO=0.0,UN='INCHES',SC=0.5). ...RY (NI=3 ,LO=0.0, SC=0.5, AX=OMIT)

GRAPH (G6/S2,DE=TEK618,OV,PO=1,6.5) ...4TIME(LE=6.0,NI=8,UN='SEC'),..

228

CZ(NI-3,LO=O.O,UN=IINCHES',SC=O.5,PO=6.O)I ...* CRZ(NI=3,LO=O.O,UN='INCHES',SC=O.5) ....

RZ (NI=3 ,LO=O. O,SC=O.5,AX=OMIT)LABEL (GI) PHASE PLANE (CDOT,CRDOT,XDOT.VS.CR)LABEL (G4) STEP RESPONSEENDSTOP

S229

DERIVATION OF MATHEMATICAL MODEL FOR THE THREE LINKREVOLUTE ROBOT ARM

First the kinetic energy of each link must be computed

as

Ki = i/2miui2 (G.1)

and then the potential energy, which is related to the

vertical height of the mass expressed by the z coordinate,

must be calculated as

Vi = migz i (G.2)

It is obvious that the kinetic energy of mass ml is

K1 = 0 (G.3)

and the potential energy

V1 = mlgdI (G.4)

In the case of masses m2 and m3 the expressions for the

velocity squared of the masses must be obtained in order to',

compute the kinetic energy. Thus, we will first write the

expressions for the Cartesian position coordinates and then

differentiate them to obtain the velocity.

For the mass m2 the Cartesian position coordinates are

230

Wa, Jr .. .r.. . .' ' .. d.. ...... .. . .. . '' l - ' . . ..

X2 d2cose2cose1 (G.5)Z

Y2 =d 2cose2sine1 (G.6)

Z2 dl+d 2sine2 (G.7)

and the Cartesian components of the velocity are then

k2 -d2sine2cose162-d2cose2sineje1 (G.8)

Y2 =-d 2sine2sine162+d2cose2cose1 41 (G.9)

i= d2cose2e2 (G.10)

The magnitude of the velocity squared is then

U 2 2 = X22''2

IZ

-d2 se 2e12 +62 ) (G.11)

and the kinetic energy is A4

K2 2 = /md 2 c e2 81 +e2 ) (G. 12)

The height of the mass in2 is expressed by equation G.7, and

the potential energy is then

V2 =m 2g(dl+d2sine2 ) (G.13)

For the mass mn3 the Cartesian position coordinates are

x3 [d2cose2+d3cos(e2+e3)3cose 1 (G.14)

Y= (d2cose2+d3cos(e2+e3)]sine1 (G.15)

z= dl+d 2sine2+d3sin(e2+e)3) (G.16)

and the Cartesian components of the velocity are then

231

i= -[d2cose2+d3cos(e2+e3 )]sineje1 (G.17)

-(d2sine2e2+d3sin(e2+03) (e2+e3) ]cose1

i3 = d2cose2+d3cos(62+e3)Jcose161 (G.18)

-[d2sine2e2+d3sin(e2+e3) (02+03) Jsine1

i3 d2cose262+d3cos(02+03) (02+03) (G.19)

The magnitude of the velocity squared is then

u 3 2 = X3 2 +Y3 2 +Z3

2

= d2 2 cos2 e26e12 +e22] (G.20)

4-2d 2d3 [cose2cos(e2+e3)81 +cOSe3 62 (62+e3)] U

+d3 2 [cos2 (82+e3 ) 612 +e22+262e3+e3 2]

and the kinetic energy is

K3 1 /2m3d2 [cOs2 92 1 +622

+m3d2d3[cOse2cos(e2+e3)e,2+cose3e 2 (42+e3) J (G.21)

+l/2m3d32 cs (e2+e3) 1 +62 +2 2 3 +4-3 ]

The height of the mass M3 is expressed by equation G.16 and

the potential energy is then

V3 = M3g[dl+d2sine2+d3sin(e2+e3)] (G.22)

The Langrangian, L = Ki -Vi, is then obtained from

equations G.3, G.4, G.12, G.13, G.21, and G.22 U

L =[1/2 (m2+m3)d2 Cos e2+m3d2d3cOse2cos(e2+e3)

+1/2m 3d3 2cos2 (92+e3)+1/2JM1 ]612 (G.23)

+[1/2(m 2+m3)d2 2+m3d2d3cOse3+1/2(m 3d3 2 )+1/2Jm2]622

232

+[l/2(M 3d3 2+JM3) J3 2

+M3 (d3 2+d2d3COSe3)e2e3

-[ (Ml+m 2+M3 )dl-(M2+M3)d2Sine2-M3d3sin(e2+e3) ]g

Taking partial derivatives of equation G.23 with respect to

el, e2 and e3

-o. (G.24)

= E(m 2+m3 )d2 2sine2cOse2+m3d2d3sin(2e2+e3 )A. e2 +m3d3 2sin(62+e3 )cos(e2+e3) ]612 (.5

- (m2+m3) gd2cose2-m3gd3cos (e2+e3)

ae -Em3d2d3sin(e2+e3)cOse2*+x 3d3 2sin(e2+e3)cos(e2+e3) ]412

#1-m 3d2d3sine362 2 (G.26)

-m3d2d3sine362 43

-m3gd3cos (e2+e3)

-~ Taking partial derivatives of equation G.23 with respect to

el, e2 and e3

= E(m 2+m3)d2 Cos e2+2m3d2d3cose2cos(e2+e3) (G.27)Bel +m3d3 2cos2 ( 2+e3 )+Jml]61

= (m2+m3)d2 +2m3d2d3cOse3+m3d3 +m 2 162 (G.28)

B2 +Em3d2d3cose3+m3d3 2 63

'4.. 233

ZL3

Taking derivatives of equations G.27, G.28, and G.29 with

respect to time

-t [in3 (d2cose2+d3cos(e2+e3) 2C

+m2d2 2 COS2 92+Jml J 0

-2 (m2+m3) d2 2sine2cose2

+m3d2d3sin(292+93) (G.30)

+m3d3 2sin(e2+e3) cos ( 2+e3) je162

-2 [m3d2d3sin (e2+e3) cose2

+m3d3 2sin( 2ie 3) cos(e2+e3) )e163

~ ) = rn::d2:~s32:053r3d3+i2e

-2m3d2d3sine3 62 63 (G. 31)

-m3d2d3sine3 63 2

= m3d3 2+Jm31e3d e)+Em 3d2d3cos93+m3d3 2)92 (G.32)

-M3d2d3sine3e2e3 -

The corresponding torques are given by

Ti for i=1,2,3 (G.33) 1.

234

Substituting equations G.24 - G.32 into equation G.33

T= (m3(d2coSe2+d3cos(e2+e3) )2

+m2d2 2cos2e2+Jmi) ei

-2 ((m2+m3) d2 2sine2cose2+m3d2d3sin (2e2+93)

+m3d3 2sin(e2+e3)cos(82+e3) j61e2 (G.34)

-2 (m3d2d3sin(e2+e3) cOse2

+m3d3 2sin (e2+e3) cOs ( 2+e3) 163

T (m2+m3)d2 2+2m3d2d3c~s 3+m13d3 2+Jm2]62

T 2 = 2

+ (m3d2d3cose3+m3d3 193

-2m3d2d3sin93e293-m3d2d3sinG363 2 (G.35)

+[ (m2+m3 )d2 sine2cose2+m 3dsinC2e2+93)

+m3d3 2sin(92+93)cOs(92+e3)]61

+ (m2+m3) gd2cose24-m3gd3cos ce2+e3)

T3 = m3d3 2+Jm3)63+(m3d2d3cose3+m3d3 2 62

+ [m3d2d3sin (e2+e3) cose2 (G.36)

+m3d3 2sin(e2+e3 ) cos ( 2+e3) ]12

+m3d2d3sine3 e2 2+m3gd3cos (62+e3)

* 235

% N.

J6 %.

DSL PROGRAM FOR THE THREE LINK REVOWUTE ROBOT UNDERDIFFERENT LOAD CONDITIONS (NO GRAVITY)

TITLE MODEL OF REVOLUTE 3-DEG OF FREEDOM ROBOT ARMPARAM K=1.O,K1=O.6,K2=10000.O,KM1=0.4225,KM2=O.4225,KM3=4. 0PARAM VSAT=150.0,M1=O.268,M2=O.227,M3=O.041PARAM D1=15.0,D2=1O.O,D3=10.0,G=-386.4PARAM J1=O.033 ,J2=0. 033,J3=O.033,R1=O.91,R2=0.91,R3=0.9.PARAM KT1=14.4,KT2=14.4,KT3=14.4,L1=.O0O1,L2=.0001,L3=. 0001PARAM BN1=O.04297,BM2=0.04297,BM3=0.04297,L#OAD=0.0PARAM KV1=O.1012,KV2=O.1012,KV3=0.1012,T=0.00025PARAM REF1=1.0 ,REF2=1.0 ,REF3=1.0INTGER SW1,SW2,5W3,N1,N2,N3 w* Ki: THE CURVE SCALING CONSTANT* K2: THE AMPLIFIER GAIN* KM: THE INITIAL CONSTANT OF THE IDEAL (MODEL) MOTOR* VSAT: THE SATURATION LIMITS OF THE AMPLIFIER* K: THE VELOCITY LOOP FEEDBACK GAIN (OF THE MODEL)* REF1,REF2,REF3:THE COMMANDED TIP POSITION IN RADS* T: THE SAMPLING INTERVALINITIAL

FLAG1=0FLAG2=OFLAG3=0N1=0N2=0N3=0XlDOT=O.X2DOT=0.X3DOT=0.Cl=o.C2=0.C3=0. 2C1DT=0.C2DT=0.C3DT=0.C1DDT=0.C2DDT=O.C3DDT=0.CR1=O.CR2=0.CR3=0.CRlDT=0.CR2DT-O.CR3DT=0.CR1DDT=0.

236

CR2DDT=O.CR3DDT=O.TL1-O.TL2=O.TL3=O.

MP2=O. mpMP3=O.MT1=O.MT2=O. J"

MT3=O.A1=SQRT(2. *KM1*VSAT)A2=SQRT(2.*KMJ2*VSAT) 4A3=SQRT (2. *KM3*VSAT)

DERIVATIVERR1=REF1*STEP(O. 0)RR2=REF2*STEP(O.O)RR3=REF3*STEP(O. 0)E1=RR1-ClE2=RR2 -C2E3=RR3 -C3

NOSORTD11 = M3*(D2*COS(CR2)+D3*COS(CR2+CR3))**2 ...

+M2*D2**2*(COS(CR2) )**2D112= 2*((M2+M3)*D2**2*COS(CR2)*SIN(CR2)..

+M3*D3**2*COS(CR2+CR3) *SIN(CR2+CR3)..+M3*D2*D3*SIN(2*CR2+CR3))

D113= 2*(M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3) ...

+M3*D2*D3*COS(CR2) *SIN(CR2+CR3))D22 = (M2+M3)*D2**2+M3*D3**2+2*M3*D2*D3*SIN(CR3)D23 = M3*D3**2+M3*D2*D3*SIN(CR3)D211= (M2+M3)*D2**2*COS(CR2)*SIN(CR2) ...

+M3*D3**2*COS(CR2+CR3) *SIN(CR2+CR3) ...+M3*D2*D3*SIN (2*CR2+CR3)

D223= 2*M3*D2*D3*SIN(CR3)D233= M3*D2*D3*SIN(CR3)D32 = H3*D3**2+M3*D2*D3*COS(CR3)D33 = M3*D3**2D311= M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3) ...

+M3*D2*D3*COS (CR2) *SIN (CR2+CR3)D322= M3*D2*D3*SIN(CR3)G2 = (M2+M3) *G*D2*c0S(cR2)+M3*G*D3*C0S(cR2+cR3) ~.G3 = M3*G*D3*COS(CR2+CR3)TLl = -Dl12*CR1DT*CR2DT.-D113*CR1DT*CR3DTTL2 = D23*CR3DDT+D211*CRlDT**2-D233*CR3DT**2 ...

-D223 *CR2DT*CR3DTTL3 = D32*CR2DDT+D311*CRlDT**2+D322*CR2DT**2 *JTOT1 = Dl1+JIJTOT2 = D22+J2JTOT3 = D33+J3IF(El.LT.O.O) X1DOT=-Al*YKl*SQRT(ABS(El))

237Y%.

IF(E1.GE.0.0) X1DOT=A1*K1*SQRT(E1)IF(E2.LT.0.0) X2DOT=-A2*Kl*SQRT(ABS(E2))IF(E2.GE.0.0) X2DOT=-A2*K1*SQRT(E2)IF(E3.LT.0.0) X3DOT=-A3*K1*SQRT(ABS(E3))IF(E3.GE.0.0) X3DOT=A3*K1*SQRT(E3)

SORT

KC1DOT=K*ClDTXlDOTE=XlDOT-KC1DOTV1=LIMIT (-VSAT ,VSAT, K2*X1DOTE)ClDDT=V1*KM1

-* NOSORTIF (FLAG1.EQ.1) GO TO 2IF (Vl.LT.VSAT.AND.TIME.GT.0.00005) FLAG1=1NSW1=N1

2 CONTINUESORT

C1DT=INTGRL( 0.0, C1DDT)C1=INTGRL (0.0, C1DT)VM1=V 1-KV1 *CR1 DTMP1=REALPL(0. 0, L1/R1,VM1/R1)MT1=KT1*MP1MT1E=MT1-BM1 *C~1DTTL1CR1DDT=(1./JTOT1) *M~ECR1DT=INTGRL(0.0, CRlDDT)CR1=INTGRL( 0.0, CR1DT)

KC2DOT=K*C2DTX2DOTE=X2 DOT-KC2 DOTV2=LIMIT (-VSAT,VSAT, K2*X2DOTE)

* NOSORTIF (FLAG2.EQ.1) GO TO 4IF (V2 .LT.VSAT.AND.TIME.GT. 0.00005) FLAG2=1NSW2=N2

4 CONTINUESORT

C2 DDT=V2 *K2C2DT=INTGRL(0.0,C2DDT)C2=INTGRL(0. 0, C2DT)VM2=V2-KV2*CR2DTMP2=REALPL(0. 0,L2/R2 ,VM2/R2)MT2=KT2 *MP2

5 ~ MT2E=MT2-BM2*CR2DT-TL2CR2DDT=(1./JTOT2) *M2ECR2DT=INTGRL(0. 0,CR2DDT)CR2=INTGRL( 0.0, CR2DT)

238

No

U KC3DOT--K*C3DTX3 DOTE-X3 DOT-KC3 DOTV3=LIMIT (-VSAT, VSAT, K2 *X3 DOTE)

NOSORTIF (FLAG3.EQ.1) GO TO 6IF (V3.LT.VSAT.AND.TIHE.GT.0.00005) FLAG3=1NSW3 =N3

6 CONTINUESORT

C3DDT=V3*KM3C3DT=INTGRL(0.0,C3DDT)

* C3=INTGRL(0.0,C3DT)

VM3=V3-KV3*CR3DTMP3=REALPL(0.0,L3/R3 ,VM3/R3)MT3=KT3 *MP3Wr3 E=MT3 -BM3 *CR3 DT-TL3CR3DDT=(1./JTOT3) *M3ECR3 DT=INTGRL (0.0, CR3 DDT)

S CR3=INTGRL(0. 0,CR3DT)

SAMPLENOSORT

IF (N3.EQ.0) GO TO 30* * IF (N2.EQ.O) GO TO 20

IF (N3.EQ.0) GO TO 10V. C3=CR3

C2=CR2C1=CR1KS3=ABS(2.*CR3)/(((N3*T)**2)*V3)KS2=ABS(2. *CR2)/( ((N2*T) **2) *V2)KS1=ABS(2.*CR1)/(((Nl*T)**2)*V1)IF (FLAG3.EQ.0) KM3=KS3IF (FLAG2.EQCO) KM2=KS2IF (FLAGI.EQ.0) KM1=KS1IF (N3.GE.2) CR3DTL=-(CR3-CR32L)/(2.*T)IF (N2.GE.2) CR2DTL=-(CR2-CR22L)/(2.*T)IF (N1.GE.2) CR1DTL-(CR1-CR12L)/(2.*T)IF(FLAG3.EQ.0) C3DT=(2.*((CR3-CR3LST)/T))-CR3DTLIF(FLAG2.EQ.0) C2DT=(2.*((CR2-CR2LST)/T))-CR2DTLIF(FLAG1.EQ.0) C1DT=(2.*((CR1-CR1LST)/T) )-CR1DTLIF (N3.EQ.NSW3.AND.FLAG3.EQ.1) GO TO 30IF (N2.EQ.NSW2.AND.FLAG2.EQ.1) GO TO 20IF (N1.EQ.NSW1.AND.FLAG1.EQ.1) GO TO 10IF (FLAG3.EQ.1) C3DT=(2.*((CR3-CR3LST)/T))-CR3DTLIF (FLAG2.EQ.1) C2DT=(2.*((CR2-CR2LST)/T))-CR2DTLIF (FLAG1.EQ.1) C1DT=(2.*((CR1-CRlLST)/T))-CR1DTL

30 N3=N3+1

239

'4.'--.- 0 P J

W .W-.W" - -.... wr

*20 N2=N2+110 N1=Nl+l

CR3 DTL-C3 DT I

CR2 DTL-C2 DTCR1DTL=-C DTCR3 2 L=-CR3 LST

* CR22L=-CR2LSTCR12 L=CRlLSTCR3 LST=CR3CR2 LST=CR2CRILST=CR1

SORT

TERMINALMETHOD RKSFXCONTRL FINTIM=O.5,DELT=0.00005,DELS=0.00025SAVE (Si) 0.0004,X1DOT,X2DOT,X3DOT,C1DT,C2DT,C3DT,...

CR1DT, CR2DT, CR3DT, CR1, CR2, CR3SAVE (52) 0.001,C1,CR1,RR1,C2,CR2,RR2,C3 ,CR3 ,RR3SAVE (S3) 0.001,E1,E2,E3PRINT 0.01, E1,E2,E3

*GRAPH (G1/Si,DE=TEK618,PO=1) ...

C1(LE=3.,NI=,LO=-,SC=UN='RAD/SEC) ....CRDT(LE=3,NI=4,LO=-3,P=6,=3,UN=6 RA/SEC),..

X1DOT(LE=3,NI=4,LO=-3 ,SC=3 ,AX=OMIT)GRAPH (G2/Sl,DE=TEK618,OV,PO=1,3.25) ...

CR2(LE=6.0,NI=13,LO=-.1,SC=.l,UN='RAD') ....C2DT(LE=3,NI=4,LO=-3,SC=3,UN='RAD/SEC'),....CR2DT(LE=3,NI=4,LO=-3,PO=6.0,SC=3,UN='RAD/SEC'),....eX2DOT(LE=3,NI=4, LO=-3,SC=3,AX=OMIT)

GRAPH (G3/Sl,DE=TEK618,OV,PO=1,6.5) ...CR3(LE=6.0,NI=13,LO=-.1,SC=.l,UN='RAD') ....C3DT(LE=3,NI=4,LO=-8,SC=8,UN='RAD/SECI),....CR3DT(LE=3,NI=4,LO=-8,PO=6.0,SC=8,UN='RAD/SEC') ....X3DOT(LE=3, NI=4, LO=-8 ,SC=8 ,AX=OMIT)

GRAPH (G4/S2,DE=TEK618,PO=1) TIME(LE=6.0,NI=5,UN='SEC') ....C1(LE=3,NI=4,LO=-.2,UN='RAD',SC=0.4,PO=6.0) ....CR1(LE=3,NI=4,LO=-.2,UN='RADI,SC=0.4) .... fRR1(LE=3,NI=4,LO=- .2,SC=0.4,AX=OMIT)

GRAPH (G5/S2,DE=TEK618,OV,PO=1,3.25) ...TIME(LE=6.0,NI=5,UN='SEC) ..C2(LE=3,NI=4,LO=-.2,UN=IRADI,SC=0.4,PO=6.0),....

CR2(LE=3,NI=4,LO=-.2,UN='RAD',SC=0.4)I ...RR2(LE=3,NI=4,LO=-.2,SC=0.4,AX=OMIT)

GRAPH (G6/S2,DE=TEK618,OV,PO=1,6.5) ...TIME(LE=6.0,NI=5,UN='SEC'),...

* C3(LE=3,NI=4,LO=-.2,UN='RAD',SC=0.4,PO=6.0) ....CR3(LE=3,NI=4,LO=-.2,UN='RAD',SC=0.4) ....

240

RR3(LE=3,NI-4,LO--.2,SC=O.4,AX=OMIT) pGRAPH (G7/S3,DE=TEK618,PO-1, .5) ...

TIME(LE=5.O,NI-5,UN='SEC') .... pE1(LE=8,NI=7,LO=-.2,UN='RAD' ,SC=.2),..E2(LE=8,NI=7,LO=-.2,UM='RAD',SC=.2,PO=5.O) .... SE3(LE=8,NI=7,LO=-.2,UN='RAD',SC=.2,PO=6.2)

LABEL (Gi) PHASE PLANE (CDT,CRDT,XDOT.VS.CR)LABEL (G4) STEP RESPONSELABEL (G7) ERROR .VS. TIMEEND -

STOP

1

241.

-PE D I r .'r ~ - -

DS PRORA FO TH TM LIN -R -. .. ROBOT

TITE MDEL PORMFRTETRELN REVOLUTE 3DGO REO ROBOT

PARAM K=1.O,K1=O.6,K2=10000.,KM1=O.4225,KM2=O.4225,KM3=4. 0PARAM VSAT=-15O.0,Ml=0.268,M2=O.227 ,M3=O.043.PARAM D1=15.O,D2=1O.O,D3=1O.O,G=386.4PARAM J1=O.033,J2=0.033,J3=O.033,R1=O.91,R2=O.91,R3=O.91PARAM KT1=14.4,KT2=14.4,KT3=14.4,L1=.0001,L2=.0O01,L3=.OOO1 .

PARAM BM1=O.04297,BM2=O.04297,BM3=O.04297,LOAD=-O.0PARAM KV1=O.1012,KV2=O.1012,KV3=O.1012,T=O.00025PARAM REF1=1.O ,REF2=1.0 REF3=1.0INTGER SW1,SW2,5W3,N1,N2,N3" 1(1: THE CURVE SCALLING CONSTANT" 1(2: THE AMPLIFIER GAIN" KM: THE INITIAL CONSTANT OF THE IDEAL (MODEL) MOTOR" VSAT: THE SATURATION LIMITS OF THE AMPLIFIER* K: THE VELOCITY LOOP FEEDBACK GAIN (OF THE MODEL)" REF1,REF2,REF3:THE COMMANDED TIP POSITION IN RADS" T: THE SAMPLING INTERVAL-INITIAL

FLAG1=0FLAG 2 =0FLAG3=ON1=ON2=0N3=0XlDOT=O.X2DOT=O.X3DOT=O.C1=0.C2=0.C3=0.C1DT=O.C2DT=O.C3DT-O.ClDDT=O.C2DDT=0. p

C3DDT=O.CR1=O.CR2r-0.CR3=0.CR1DT=0.CR2DT=O.CR3DT=O.CR1DDT=O.

lip

242

%~.N %N% le

* VA. - . - w( . X-

CR2DDT=O.CR3DDT=-O.TL1=O.TL2=O.TL3=O.

MP2=O.MP3=O.

MT3=O.A1=SQRT (2. *IQQ*VSAT) -

A2=SQRT(2 . *K2*VSAT) -

A3=SQRT (2. *K3*VSAT)DERIVATIVE

RR1=REF1*STEP(O.O)+50*(STEP(.090)-STEP(.100))RR2=REF2*STEP(O.O)+50*(STEP(.090)-STEP(.100))RR3=REF3*STEP(O.O)+5O*(STEP(.O9Oh-STEP(.1OO))El=RR1 -ClE2=RR2-C2 N

E3=RR3 -C3

NOOTD11 = M3*(D2*COS(CR2)+D3*COS(CR2+CR3) )**2 ...

+M2*D2**2*(COS(CR2) )**2D112= 2*((M2+M3)*D2**2*COS(CR2)*SIN(CR2)..

+M3*D3**2*COS (CR2+CR3) *SIN(CR2+CR3)..+M3*D2*D3*SIN (2*CR2+CR3)) o

D113= 2*(M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3) ...

+M3*D2*D3*COS(CR2) *SIN(CR2+CR3))D22 = (M2+M3)*D2**2+M3*D3**2+2*M3*D2*D3*SIN(CR3)D23 = M3*D3**2+M3*D2*D3*SIN(CR3)D211= (M2+M3)*D2**2*COS(CR2)*SIN(CR2) ...

+M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3) ...+M3*D2*D3*SIN (2*CR2+CR3)

D223= 2*M3*D2*D3*SIN(CR3) 1D233= M3*D2*D3*SIN(CR3)D32 = M3*D3**2+M3*D2*D3*COS(CR3)D33 = M3*D3**2D311= M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3) ...

+M3*D2*D3*COS (CR2) *SIN(CR2+CR3)D322= M3*D2*D3*SIN(CR3)G2 = (M2+M3) *G*D2*COS (CR2)+M3*G*D3*COS (CR2+CR3)G3 = M3*G*D3*COS(CR2+CR3)TLi = -Dll2*CRlDT*CR2DT-D113*CR1DT*CR3DTTL2 = D23*CR3DDT+D21l*CR1DT**2-D233*CR3DT**2 ...

-D223 *CR2DT*CR3DTTL3 = D32*CR2DDT+D311*CRlDT**2+D322*CR2DT**2JTOT1 = Dl1+J1JTOT2 = D22+J2JTOT3 = D33+J3IF(E1.LT.O.O) X1DOT=-.Al*Ki*SQRT(ABS(E1))

243

_N:

IF(E1.GE.0.0) X1DOT=A1*K1*SQRT(E1)IF(E2.LT.0.0) X2DOT=-A2*K1*SQRT(ABS(E2))IF(E2.GE.0.0) X2DOT=A2*K1*SQRT(E2)IF(E3.LT.0.0) X3DOT=-A3*K1*SQRT(ABS(E3))IF(E3.GE.0.0) X3DOT=A3*K1*SQRT(E3)

SORT

KCIDOT=K*ClDTX1DOTE=X1DOT-KC1 DOT k.V1=LIMIT (-VSAT, VSAT,K2 *X1DOTE)ClDDT=V1*KM1 I

NOSORTIF (FLAGI.EQ.1) GO TO 2IF (V1.LT.VSAT.AND.TIME.GT.0.00005) FLAG1=1NSW1=Nl

2 CONTINUESORT

C1DT=INTGRL(0. 0, C1DDT)C1=INTGRL (0.0 ,C1DT)VM1=V1-KV1*CR1DTMP1=REALPL(0. 0, L1/R1,VM1/R1)MT1=KT1*MP1MT1E=MT1-BM1 *CR1DT-TL1CR1DDT=(1./JTOT1) *M~ECR1DT=INTGRL(O. 0,CR1DDT)CR1=INTGRL( 0.0, CR1DT)

KC2DOT=K*C2DTX2 DOTE=X2 DOT-KC2 DOTV2=LIMIT (-VSAT, VSAT, 12 *X2 DOTE)

NOSORT .

IF (FLAG2.EQ.1) GO TO 4IF (V2.LT.VSAT.AND.TIME.GT.0.00005) FLAG2=1NSW2=N2

4 CONTINUESORT

C2DDT=V2 *M2C2DT=INTGRL( 0. 0, C2DDT)C2=INTGRL(0. 0,C2DT)VH2=V2-KV2*CR2DTMP2=REALPL(0. 0, L2/R2 ,VM2/R2)MT2 =KT2 *MP2MT2E=MT2-BM2 *CR2DT-TL2CR2DDT= (1./JTOT2) *M2ECR2DT=INTGRL(0. 0.CR2DDT)CR2=INTGRL(0. 0,CR2DT)

244

-x7 -'-P

KC3DOT=-K*C3DTX3DOTE=X3DOT-KC3DOTV3=LIMIT (-VSAT, VSAT, K2 *X3 DOTE)

NOSORTIF (FLAG3.EQ.1) GO TO 6IF (V3.LT.VSAT.AND.TIME.GT.0.00005) FLAG3=1NSW3=N3

6 CONTINUE* SORT

C3 DDT=V3 *KM3C3DT=INTGRL(0. 0, C3DDm )C3=INTGRL(0.0,C3DT)VM3=V3 -KX3 *CR3DTMP3=REALPL(0 .0, L3/R3 ,VM3/R3)MT3=K3 *MP3MT3 E=MT3 -BM3 *CR3 DT-TL3CR3DDT= (1./JTOT3) *1T3ECR3 DT=INTGRL (0.0 ,CR3DDT)CR3=INTGRL (0.0 ,CR3DT)

SAMPLENOSORT

IF (N3.EQ.0) GO TO 30IF (N2.EQ.0) GO TO 20IF (N3.EQ.0) GO TO 10

* C3=CR3C2=CR2

a, C1=CR1KS3=ABS(2.*CR3)/( ((N3*T) **2) *V3)KS2=ABS (2. *CR2)/ ( ((N2*T) **2) *V2)KSI=ABS (2 .*CR1)/ ( ((N1*T) **2) *V1)IF (FLAG3.EQ.0) KM3=K53IF (FLAG2.EQ.0) KM2=KS2IF (FIAG1.EQ.0) KM1=KS1IF (N3.GE.2) CR3DTL=-(CR3-CR32L)/(2.*T)IF (N2.GE.2) CR2DTL=-(CR2-CR22L)/(2.*T)IF (N1.GE.2) CR1DTL=-(CR1-CR12L)/(2.*T)IF(FLAG3.EQ.O) C3DT=(2.*((CR3-CR3LST)/T))-CR3DTLIF(FLAG2.EQ.0) C2DT=(2.*((CR2-CR2LST)/T))-CR2DTLIF(FLAG1.EQ.0) C1DT=(2.*((CR1-CRlLST)/T) )-CR1DTL

*IF (N3.EQ.NSW3.AND.FLAG3.EQ.1) GO TO 30IF (N2.EQ.NSW2.AND.FLAG2.EQ.1) GO TO 20IF (N1.EQ.NSW1.AND.FLAG1.EQ.1) GO TO 10IF (FLAG3.EQ.1) C3DT=(2.*((CR3-CR3LST)/T))-CR3DTL

* IF (FLAG2.EQ.1) C2DT=(2.*( (CR2-CR2LST)/T))-CR2DTL*IF (FLAG1.EQ.1) C1DT=(2.*((CR1-CR1LST)/T))-CRlDTL

* 30 N3=N3+.20 N2=N2+110 N1=Nl+1

245

5, A A

wV . aa a a - t a ~a , . -a

CR3DTL=-C3DT

CR321,L=CR3 1STCR22 L=CR2 1STCR12L=-CRlLSTCR3 LST=CR3CR2 LST=CR2CRl1ST=CR1

SORT

TERMINALMETHOD RXSFXCONTRL FINTIM=O0.5,DELT=O.00005,DELS=O.00025SAVE (Si) O.0004,XlDOT,X2DOT,X3DOT,C1DT,C2DT,C3DT,....

CR1DT, CR2DT, CR3DT, CR1, CR2, CR3SAVE (52) O.OO1,C1,CR1,RR1,C2,CR2,RR2,C3,CR3,RR3GRAPH (G1/S1,DE=TEK618,PO=1) ...

CR1(LE=6.O,NI=13,LO=-.1,SC=1,N='RAD'),....C1DT(LE=3,NI=4,LO=-3,SC=3,UN='RAD/SEC') ....CR1DT(LE=3,NI=4,LO=-3,PO=6.O,SC=3,UN='RAD/SEC') ....XlDOT (LE=3 ,NI=4, LO=-3 ,SC=3, AX=OMIT)

GRAPH (G2/S1,DE=TEK618,OV,PO=1,3.25) ...CR2(LE=6.O,NI=13,LO=-.1,SC=.l,UN='RAD'),....'IC2DT(LE=3,NI=4,LO=-3,SC=3,UN='RAD/SEC') ....CR2DT(LE=3,NI=4,LO=-3,PO=6.O,SC=3,UN='RAD/SEC'),....*X2DOT(LE=3 ,NI=4,LO=-3 ,SC=3 ,AX=OMIT)

GRAPH (G3/S1,DE=TEK6iB,OV,PO=1,6.5) ...CR3(LE=6.O,NI=13,LO=-.1,SC=.l,UN='RAD'),....C3DT(LE=3,NI=4,LO=-8,SC=8,UN='RAD/SEC'),...CR3DT(LE=3,NI=4,LO=-8,PO=6.O,SC=8,UN='RAD/SEC'),...X3DOT(LE=3 ,NI=4,LO=-8,SC=8,AX=OMIT)

GRAPH (G4/S2,DE=TEK618,PO=1) TIME(LE=6.O,NI=5,UN='SEC'),....Cl(LE=3,NI=4,LO=-.2,UN='RADI,SC=O.4,PO=6.O),....

CR1(LE=3,NI=4,LO=-.2,UN='RAD' ,SC=O.4),....RR1 (LE=3,NI=4,LO=-.2,SC=O.4,AX=OMIT)

GRAPH (G5/S2,DE=TEK618,OV,PO=1,3.25)..TIME(LE=6.O,NI=5,UN='SEC'),....C2(LE=3,NI=4,LO=-.2,UN='RADI,SC=O.4,PO=6.O),...CR2(LE=3,NI=4,LO=-.2,UN='RAD',SC=O.4) ....RR2(LE=3,NI=4,LO=-.2,SC=O.4,AX=OMIT)

GRAPH (G6/S2,DE=TEK618,OV,PO=1,6.5) ...TIME(LE=6.O,NI=5,UJN='SEC') ..C3(LE=3,NI=4,LO=-.2,UN='RAD',SC=O.4,PO=6.O) ....CR3(LE=3,NI=4,LO=-.2,UN='RAD',SC=O.4) ....RR3 (LE=3,NI=4,LO=-.2,SC=O.4,AX=OMIT)

LABEL (Gi) PHASE PLANE (CDT,CRDT,XDOT.VS.CR)LABEL (G4) STEP RESPONSEENDSTOP

246

ri

1% 0

0

APPENDIX J ..

DSL PROGRAM FOR THE THREE LINK REVOWATE ROBOTUNDER DIFFERENT LOAD CONDITIONS(GRAVITATIONAL TORQUES INCLUDED)

TITLE MODEL OF REVOLUTE 3-DEG OF FREEDOM ROBOT ARMPARAM K=1.0,K1=0.6,K2=10000.0,KM1=O.4225,KM2=0.4225,KM3=4 .0PARAM VSAT=15O. 0,M1=0.268 ,M2=0.227 ,M3=0.041PARAM D1=15. 0,D2=10. 0,D3=10. 0,G=386.4 .

PARAM J1=0.033,J2=0.033 ,J3=0.033 ,R1=O.91,R2=0.91,R3=0.91PARAM KT1=14.4,KT2=u14.4,KT3=14.4,L1=.0001,L2=.0001,L3=.0001 0PARAM BM1=0.04297,BM2=0.04297,BM3=0.04297,LOAD=0.0PARAM KV1=0.1012,KV2=O.1012,KV3=0.1012,T=O.00025PARAM REF1=1.0 ,REF2=1.0 ,REF3=1.0INTGER SW1,SW2,SW3,N1,N2,N3" Ki: THE CURVE SCALLING CONSTANT .

" K2: THE AMPLIFIER GAIN" KM: THE INITIAL CONSTANT OF THE IDEAL (MODEL) MOTOR" VSAT: THE SATURATION LIMITS OF THE AMPLIFIER" K: THE VELOCITY LOOP FEEDBACK GAIN (OF THE MODEL)" REF1,REF2,REF3:THE COMMANDED TIP POSITION IN RADS" T: THE SAMPLING INTERVALINITIAL

FLAG1=0FLAG 2=0FLAG3=0N1=0N2=0N3=0XlDOT=0.X2DOT=0.X3DOT=0. 'C1=0.C2=0.C3=0.ClDT=0.C2DT=0.C3DT=0.C1DDT=0.C2DDT=0.C3DDT=0.CR1=0.CR2=0.CR3=0.CR1DT=0.CR2DT=0.CR3 DT=0.

247

5 ~ ~~~ ~~~~~ N.5 %. ' , ' ~ ... % % . % ~ % 5 ~ ' %3

J*J.J5 ~ ~ '. . P .. .. '.~ .'. . ? 5

~ p'p A'p' 'p . ~ .

Ir a --p * a '.4..

CR1DDT=O.CR2DDT=-O.CR3DDT=O.TL1=O.TL2=O.TL3=O.

MP2=O.HP3=O.MT1=O.MT2=O.MT3=O.A1=SQRT (2. *KM1*VSAT)A2=SQRT (2. *KM2*VSAT)A3=SQRT(2 .*Kj(3*VSAT)

* DERIVATIVERR1=REF1*-c -2(0.0)RR2=REF2*STEP(O.0)RR3=REF3*STEP(O.O)E1=RR1 -ClE2=RR2 -C2E3=RR3-C3


+M2*D2**2*(COS (CR2) )**2D112= 2*((M2+M3)*D2**2*COS(CR2)*SIN(CR2) ...

+M3*D3**2*COS(CR2+CR3) *SIN(CR2+CR3) ...+M3*D2*D3*SIN(2*CR2+CR3))

D113- 2*(M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3)..* +M3*D2*D3*COS (CR2) *SIN(CR2+CR3))

D22 = (M2+M3)*D2**2+M3*D3**2+2*M3*D2*D3*SIN(CR3)D23 = M3*D3**2+M3*D2*D3*SIN(CR3)D211= (M2+M3)*D2**2*COS(CR2)*SIN(CR2)..

+M3*D3**2*COS (CR2+CR3) *SIN(CR2+CR3) ...+M3*D2*D3*SIN (2*CR2+CR3)

D223= 2*M3*D2*D3*SIN(CR3)D233= M3*D2*D3*SIN(CR3)D32 = M3*D3**2+M3*D2*D3*COS(CR3)D33 =M3*D3**2D311= M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3)..

+M3*D2*D3*COS (CR2) *SIN(CR2+CR3)D322- M3*D2*D3*SIN(CR3)G2 = (M2+M3) *G*D2*COS(cR2)+M3*G*D3*COS (CR2+CR3)G3 = M3*G*D3*COS(CR2+CR3)TL1 = -Dl12*CR1DT*CR2DT-Dll3*CRlDr*CR3DTTL2 = D23*CR3DDTID2i1*CRlDT**2-D233*CR3DT**2 ...

-D2 23 *CR2DT*CR3DT+G2TL3 = D32*CR2DDT+D311*CR1DT**2+D322 *CR2DT**2+G3JTOT1 = Dl1+JlJTOT2 = D22+J2JTOT3 = D33+J3

248

IF(E1.LT.O.0) XlDOT=--A1*K1*SQRT(ABS(E1))IF(E1.GE.0.0) X1DOT=Al*K1*SQRT(E1)IF(E2.LT.0.0) X2DOT=-A2*K1*SQRT(ABS(E2))IF(E2.GE.0.0) X2DOT=A2*Kl*SQRT(E2)IF(E3.LT.O.O) X3DOT=-A3*Kl*SQRT(ABS(E3))IF(E3.GE.O.O) X3DOT=A3*K1*SQRT(E3)

SORT

KClDOT=K*C1DTXIDOTE=XlDOT-KClDOTV1=LIMIT (-VSAT,VSAT, K2*X1DOTE)ClDDT=Vl*KM1

NOSORTIF (FLAG1.EQ.1) GO TO 2IF (V1.LT.VSAT.AND.TIME.GT.O.00005) FLAG1=.NSW1=Nl

2 CONTINUESORT

C1DT=INTGRL (0.0, C1DDT)C1=INTGRL(0.0, C1DT)VM1=Vl-KV1*CR1DTMP1=REALPL(0. 0,L1/R1,VM1/R1)MT1=KT1*MP1MTlE=MT1-BM1*CR1DT-TL1CR1DDT= (1./JTOT1) *MT1E

* CR1DT=INTGRL (0 *0, CR1DDT)CR1=INTGRL(0.0, CR1DT)

KC2DOT=K*C2DTX2 DOTE=X2 DOT-KC2 DOTV2=LIMIT (-VSAT,VSAT, K2*X2DOTE)

NOSORTIF (FLAG2.EQ.1) GO TO 4IF (V2.LT.VSAT.AND.TIME.GT.0.00005) FLAG2=1NSW2=N2

4 CONTINUESORT

C2 DDT=V2 *K2C2DT=INTGRL( 0.0, C2DDT)C2=INTGRL(0. 0,C2DT)VM2=V2 -KV2 *CR2 DTHP2=REALPL (0.0, L2/R2, VM2/R2)HT2=KT2 *MP2MT2E=MT2 -BM2 *CR2 DT-TL2CR2DDT=(./JTOT2) *M2ECR2DT=INTGRL(0.0,CR2DDT)CR2=INTGRL(0.0, CR2DT)

249

KC3DOT=K*C3 DTX3 DOTE=X3 DOT-KC3 DOTV3=LIMIT (-VSAT,VSAT, K2*X3DOTE)

NOSORT p

IF (FLAG3.EQ.1) GO TO 6IF (V3.LT.VSAT.AND.TIME.GT.0.00005) FLAG3=1NSW3 =N3

6 CONTINUESORT

C3DDT=V3*KM3C3DT=INTGRL(O.O, C3DDT)C3=INTGRL(0. 0, C3DT)VH3=V3-KV3*CR3DTMP3-REALPL (0.0 ,L3/R3, VM3/R3)MT3=T3 *MP3MT3E=MT3 -BM3 *CR3 DT-TL3CR3DDT=(l./JTOT3) *MT3ECR3DT=INTGRL(0. 0, CR3DDT)IlCR3=INTGRL(0.0, CR3DT)

SAMPLE .

NOSORTIF (N3.EQ.O) GO TO 30IF (N2.EQ.0) GO TO 20IF (N3.EQ.0) GO TO 10C3 =CR3C2=CR2C 1=CR1KS3=ABS (2. *CR3)/ ( ((N3*T) **2) *V3).KS2=ABS(2.**CR2)/( ((N2*T) **2) *V2)KS1=ABS(2.*CR1)/((C(N1*T) **2) *V1)IF (FLAG3.EQ.0) KM3=KS3IF (FLAG2.EQ.0) KM2=KS2IF (FLAG1.EQ.0) KM1=KS1IF (N3.GE.2) CR3DTL=-(CR3-CR32L)/(2.*T)IF (N2.GE.2) CR2DTL=-(CR2-CR22L)/(2.*T)IF (N1.GE.2) CR1DTL=-(CR1-CR12L)/(2.*T)IF(FLAG3.EQ.0) C3DT=(2.*((CR3-CR3LST)/T))-CR3DTLIF(FLAG2.EQ.0) C2DT=(2.*((CR2-CR2LST)/T) )-CR2DTLIF(FLAG1.EQ.0) C1DT=(2.*((CR1-CR1LST)/T) )-CR1DTLIF (N3.EQ.NSW3.AND.FLAG3.EQ.1) GO TO 30IF (N2.EQ.NSW2.AND.FLAG2.EQ.1) GO TO 20IF (N1.EQ.NSW1.AND.FLAG1.EQ.1) GO TO 10

IF (IAG.EQ.) CDT=(.*(CR3-R3LT)/T)-C3DTIF (FLAG3.EQ.1) C3DT=(2.*((CR2-CR3LST)/T))-.CR2DTL I

IF (FLAG2.EQ.1) C2DT=(2.*((CR2-CRlLST)/T))-CR2DTL

30 N3=N3+1

250 I

Ir f~r

. . .~~~~ .V. Vo I

20 N2=N2+1

10 Nl=Nl+l

CR3 DTL=-C3 DT

CR2DTL=-C2DT

CR1DTL=-ClDT

CR32 L-CR3 1ST

CR22 L=CR2 1ST

CR12L=-CR1LST

CR3LST=CR3

CR2LST=CR2

CR11ST=CR1

SORT

TERMINALMETHOD RIKSFXCONTRL FINTIM=0.5,DELT=0.00005,DELS=0.00025SAVE (Si) 0.0004,X1DOT,X2DOT,X3DOT,C1DT,C2DT,C3DT,...

CR1DT, CR2DT, CR3DT, CR1, CR2, CR3SAVE (S2) 0.001,Cl,CR1,R.R1,C2,CR2,RR2,C3,CR3 ,RR3SAVE (S3) O.0O1,E1,E2,E3PRINT 0.01, E1,E2,E3GRAPH (G1/S1,DE=TEI(618,PO=1) ... I

CR1(LE=6.0,NI=13,LO=-.1,SC=.l,UN='RAD') ....C1DT(LE=3,NI=4,LO=-3,SC=3,UN='RAD/SEC'),...CR1DT(LE=3,NI=4,LO=-3,PO=6.0,SC=3,UN='RAD/SEC'),....X1DOT(LE=3 ,NI=4 ,LO=-3 ,SC=3 ,AX=OMIT)

GRAPH (G2/Sl,DE=TEK618,OV,PO=1,3.25)..CR2(LE=6.0,NI=13,LO=-.1,SC=.l,UN='RAD'),...C2DT(LE=3,NI=4,LO=-3,SC=3,UN='RAD/SECI),....CR2DT(LE=3,NI=4,LO=-3,PO=6.O,SC=3,UN='RAD/SEC') ..X2DOT(LE=3 ,NI=4, LO- -3, SC=3 ,AX=OMIT)

GRAPH (G3/S1,DE=TEK618,0V,PO=1,6.5) ...CR3(LE=6.0,NI=1 O .=-.1,SC=.1,UN='RAD') ....C3DT(LE=3,NI=4<. -,SC=8,UN='RAD/SEC') ....CR3DT(LE=3,NIVJ.')-8,PO=6.0,SC=8,UN=RAD/SEC) ....X3DOT(LE=3 ,NI=4, ,,O=-8,SC=8,AX=OMIT)

GRAPH (G4/S2,DE=TEY.68,PO=1) TIME(LE=6.0,NI=5,UN='SEC'),..C1(LE=3,NI=4 2L0=-.2,UN='RAD',SC=0.4,PO=6.0),...CR1(LE=3,NI=4,LO=-.2,UN='RAD',SC=0.4),...RR1(LE=3,NI=4,LO=-.2,SC=0.4,AX=OMIT)

GRAPH (G5/S2,DE=TEK618,OV,PO=1,3.25) ...TIME(LE=6.0,NI=5,UN='SEC),.C2(LE=3,NI=4,LO=-.2,UN='RAD',SC=0.4,PO=6.0),....CR2(LE=3,NI=4,LO=-.2,UN='RAD',SC=0.4) ....RR2(LE=3,NI-4,LO=-.2,SC=0.4,AX=OMIT)

GRAPH (G6/S2,DE=TEK61B,OV,PO=1,6.5) ...TIME(LE=6.0,NI=5,UN='SECI),..C3(LE=3,NI=4,LO=-.2,UN='RAD',SC=0.4,PO=6.0)I ...CR3(LE=3,NI=4,LO=-.2,UN='RAD' ,SC=0.4) ... II

251

RR3 (LE=3,NI=4,LO=-.2,SC=O.4,AX=OMIT)GRAPH (G7/S3,DE=TEK618,PO=1, .5) ...

TIME(LE=5.O,NI=5,UN='SEC'),...El(LE=8,,NI=7,LO=-.2,UN='RAD' ,SC=.2),....E2(LE=8,NI=7,LO=-.2,UN='RAD',SC=.2,PO=5.O) ....E3(LE=8,NI=7,LO=-.2,UN='RAD' ,SC=.2,PO=6.2)

LABEL (Gi) PHASE PLANE (CDT,CRDT,XDOT.VS.CR)LABEL (G4) STEP RESPONSELABEL (G7) ERROR .VS. TIMEENDSTOP

252.

DSL PROGRAM FOR THE THREE LINK REVOUTE ROBOT WITHDISTURBANCE (GRAVITATIONAL TORQUES INCLUDED)

TITLE MODEL OF REVOLUTE 3-DEG OF FREEDOM ROBOT ARMPARAM K=1.0,K=O.6,K2=10000. 0,KM1=0.4225,KM2=0.4225,KM3=4.OPARAM VSAT=150.0,M1=0.268,M2=O.227,M3=0. 041PARAM D1=15.0,D2=10.0,D3=10.0,G=-386.4PARAM J1=0.033,J2=O.033 ,J3=0. 033 ,Rl=O.91,R2=0.91,R3=0.91

*PARAM KT1=14.4,KT2=14.4,KT3=14.4,L1=.O001,L2=.000.1,L3=. 0001PARAM BM1=0.04297,BM2=0.04297,BM3=0.04297,LOAD=0O.0PARAM KV1=0.1012,KV2=0.1012,KV3=O.1012,T=O.00025PARAM REF1=1.0 ,REF2=1.0 ,REF3=1.0INTGER SW1,SW2,SW3,N1,N2,N3" Ki: THE CURVE SCALLING CONSTANT" K2: THE AMPLIFIER GAIN" KM: THE INITIAL CONSTANT OF THE IDEAL (MODEL) MOTOR" VSAT: THE SATURATION LIMITS OF THE AMPLIFIER" K: THE V ,ELOCITY LOOP FEEDBACK GAIN (OF THE MODEL)" REF1,REF2,REF3:THE COMMANDED TIP POSITION IN RADS" T: THE SAMPLING INTERVALINITIAL

FLAG1=0 -

FLAG2=0FLAG 3=0N1=0N2=0N3=0XlDOT=0.X2DOT=0.X3DOT=0.

C2=0.C3=0.ClDT=0.C2DT=O.C3DT=0.C1DDT=0.C2DDT=0.C3DDT=0.CR1=0.CR2=0.CR3=0.CR1DT=0.CR2DT=0.CR3DT=0.CR1DDT=O.

253 9

CR2DDTO0.CR3 DDT=O.TL1=O.TL2=O.TL3=O.

MP2=O.MP3=O.MT1=O.MT2=O.MT3=O.A1=SQRT (2.* *MJ1*VSAT)A2=SQRT(2.* *Mj2*VSAT)A3=SQRT(2. *K3*VSAT)

DERIVATIVERR1=REF1*STEP(O.O)+50*(STEP(.090)-STEP(.100))RR2=REF2*STEP(O.O)+50*(STEP(.090)-STEP(.100))RR3=REF3*STEP(O.O)+50*(STEP(.090)-STEP(.100))El=RR1-C1E2=RR2 -C2E3=RR3-C3


+M2*D2**2*(COS(CR2) )**2*D112= 2*((M2+M3)*D2**2*COS(CR2)*SIN(CR2)..* +M3*D3**2*COS(CR2+CR3) *SIN(CR2+CR3)...* +M3*D2*D3*SIN(2*CR2+CR3))

*D113= 2*(M3*D3**2*COS(CR2+CR3) *SIN(CR2+CR3)..+M3*D2*D3*COS (CR2) *SIN(CR2+CR3))

D22 = (M2+M3)*D2**2+M3*D3**2+2*M3*D2*D3*SIN(CR3)D23 = M3*D3**2+M3*D2*D3*SIN(CR3)D211= (M2+M3)*D2**2*COS(CR2)*SIN(CR2)..

+H3*D3**2*COS(CR2+CR3) *SIN(CR2+CR3) ...+M3*D2*D3*SIN (2*CR2+CR3)

D223= 2*M3*D2*D3*SIN(CR3)D233= H3*D2*D3*SIN(CR3)D32 = M3*D3**2+M3*D2*D3*COS(CR3)D33 = H3*D3**2D311= M3*D3**2*COS(CR2+CR3)*SIN(CR2+CR3)..

+M3*D2*D3*COS(CR2) *SIN(CR2+CR3)D322= M3*D2*D3*SIN(CR3)G2 = (M2+M3)*G*D2*COS(CR2)+M3*G*D3*COS(CR2+CR3)G3 = M3*G*D3*COS(CR2+CR3)

*TLl = -DI12*CRlDT*CR2DT-Dll3*CRlDT*CR3DTTL2 = D23*CR3DDT+D211*CRlDT**2-D233*CR3DT**2 ... -

-D223 *CR2DT*CR3DT+G2TL3 = D- 2*CR2DDT+D3ll*CRlDT**2+D322*CR2DT**2+G3JTOTl = DiiA-Ji

*JTOT2 = D22+,32JTOT3 = D3~TIF(E1.LT.O.O) XIDOT=-Al*Kl*SQRT(ABS(El))

254

IF(E1.GE.O.O) X1DOT=A1*K1*SQRT(E1)IF(E2.LT.O.O) X2DOT=-A2*K1*SQRT(ABS(E2))IF(E2.GE.O.O) X2DOT=A2*K1*SQRT(E2)IF(E3.LT.O.O) X3DOT=-A3*K1*SQRT(ABS(E3))IF(E3.GE.O.O) X3DOT=A3*K1*SQRT(E3)

SORT

KClDOT=K*ClDTXlDOTE=XIDOT-KClDOTV1=LIMIT (-VSAT, VSAT, K2 *X1DOTE)ClDDT=Vl*KM1

NOSORTIF (FLAG1.EQ.1) GO TO 2IF (V1.LT.VSAT.AND.TIME.GT.O.00005) FLAG1=1NSW1=N1

2 CONTINUESORT

C1DT=INTGRL (0.0 ,C1DDT)C1=INTGRL(0. 0,C1DT)VMI=V1-KV1*CRlDTMP1=REALPL(0. 0,Ll/R1,VM1/R1)MT1=KT1*14P1MTlE=MT1-BM1*CR1DT-TL1CR1DDT= (1./JTOT1) *M~ECR1DT=INTGRL (0.0 ,CR1DDT)CR1=INTGRL(0. 0, CR1DT)

KC2 DOT=K*C2 DTX2 DOTE=X2 DOT-KC2 DOTV2=LIMIT (-VSAT, VSAT, K2 *X2 DOTE)

NOSORTIF (FLAG2.EQ.1) GO TO 4IF (V2.LT.VSAT.AND.TIME.GT.O.00005) FLAG2=1NSW2=N2

*4 CONTINUESORT

C2DDT=V2*KM2C2DT=INTGRL(O. 0, C2DDT)C2=INTGRL(0.0, C2DT)VH2=V2-KV2*CR2DTMP2=REALPL (0.0, L2/R2, VM2/R2)MT2=KT2 *MP2MT2E=MT2-BM2*CR2DT-TL2CR2DDT=(1./JTOT2) *M2ECR2DT=INTGRL(O.0, CR2 DDT)CR2=INTGRL(O. O,CR2DT)

255

KC3 DOT=K*C3 DTX3 DOTE=X3 DOT-KC3 DOT .

V3=LIMIT (-VSAT, VSAT, K2 *X3 DOTE) 5

NOSORTIF (FLAG3.EQ.1) GO TO 6IF (V3.LT.VSAT.AND.TIME.GT.00 00005) FLAG3=1-NSW3 =N3

6 CONTINUESORT

C3 DDT=V3 *KJ43C3 DT=INTGRL (0. 0, C3 DDTC3=INTGRL(0.0, C3DT)VM3=V3-KV3 *CR3DTMP3=REALPL (0.0 ,L3/R3, VM3/R3)MT'3K3 *M 3MT3E=MT3-BM3 *CR3DT-TL3CR3DDT=(1 ./JTOT3) *M3ECR3DT=INTGRL( 0.0, CR3DDT)CR3=INTGRL(0. 0, CR3DT)

SAMPLENOSORT -

IF (N3.EQ.0) GO TO 30IF (N2.EQ.0) GO TO 20IF (N3.EQ.O) GO TO 10C3=CR3C2=CR2C1=CR1

KS3=BS(2*CR)/((N3*)**2*V3

KS3=ABS(2.*CR2)/( ((N3*T) **2) *V3)

KS1=ABS(2.*CR1)/( ((N1*T) **2) *V1)IF (FLAG3.EQ.0) KM3=KS3IF (FLAG2.EQ.0) KM2=KS2IIF (FLAG1.EQ.0) KM1=KS1IF (N3.GE.2) CR3DTL=-(CR3-CR32L)/(2.*T) -

IF (N2.GE.2) CR2DTL=-(CR2-CR22L)/(2.*T)IF (N1.GE.2) CR1DTL=-(CR1-CR12L)/(2.*T)IF(FLAG3.EQ.0) C3DT=(2.*((CR3-CR3LST)/T))-CR3DTLIF(FLAG2.EQ.0) C2DT=(2.*((CR2-CR2LST)/T) )-CR2DTLIF(FLAG1.EQ.0) C1DT=(2.*((CR1-CR1LST)/T) )-CR1DTLIF (N3.EQ.NSW3.AND.FLAG3.EQ.1) GO TO 30IF (N2.EQ.NSW2.AND.FLAG2.EQ.1) GO TO 20IF (N1.EQ.NSW1.AND.FLAGI.EQ.1) GO TO 10IF (FLAG3.EQ.1) C3DT=(2.*((CR3-CR3LST)/T) )-CR3DTLIF (FLAG2.EQ.1) C2DT=(2.*((CR2-CR2LST)/T) )-CR2DTLIF (FLAG1.EQ.1) C1DT=(2.*((CR1-CR1LST)/T) )-CR1DTL

30 N3=N3+120 N2=N2+.10 N1=N1+l

256

CR3 DTL-C3 DTCR2DTL-C2DTCRIDTL=-C1DTCR3 2L=-CR3LSTCR2 2L=-CR2LSTCR12L=-CRILSTCR3 LST=CR3CR2 IST=CR2CRlLST=CR1

SORT

TERMINALMETHOD RKSFXCONTRL FINTIM=O.5,DELT=O.00005,DELS=O.00025SAVE (Si) O.0004,X1DOT,X2DOT,X3DOT,ClDT,C2DT,C3DT,....

v CRlDT,CR2DT,CR3DT,CR1,CR2,CR3SAVE (S2) O.OO1,C1,CR1,RRi,C2,CR2,RR2,C3,CR3,RR3GRAPH (G1/S1,DE=TEK618,PO=1)..

CR1(LE=6.O,NI=13,LO=-.1,SC=.1,UN='RAD') ....C1DT(LE=3,NI=4,LO=-3,SC=3,UN='RAD/SEC') ....CR1DT(LE=3,NI=4,LO=-3,PO=6.O,SC=3,UN='RAD/SEC') ....X1DOT (LE=3 ,NI=4, LO=-3 ,SC=3 ,AX=OMIT)

GRAPH (G2/S1,DE=TEK618,OV,PO=1,3.25)..CR2(LE=6.O,NI=13,LO=-.4,SC=.l,UN='RAD'),....C2DT(LE=3,NI=4,LO=-3,SC=3,TJN='RAD/SEC'),....CR2DT(LE=3,NI=4,LO=-3,PO=6.O,SC=3,UN='RAD/SEC'),....X2DOT(LE=3 ,NI=4 ,LO=-3 ,SC=3,AX=OMIT)

GRAPH (G3/Si,DE=TEK618,OV,PO=1,6.5) ...CR3(LE=6.O,NI=13,LO=-.1,SC=.l,UN='RAD'),....C3DT(LE=3,NI=4,LO=-8,SC=8,UN='RAD/SEC') ....CR3DT(LE=3,NI=4,LO=-8,PO=6.O,SC=8,UN='RAD/SEC'),...X3DOT(LE=3 ,NI=4 ,LO=-8 ,SC=B ,AX=OMIT)

GRAPH (G4/S2,DE=TEK618,PO=1) TIME(LE=6.O,NI=5,UN=ISEC'),..Cl(LE=3,NI=4,LO=-.2,UN='RAD',SC=O.4,PO=6.O) ....CR1(LE=3,NI=4,LO=-.2,UN='RAD' ,SC=O.4),..RR1(LE=3,NI=4,LO=-.2,SC=O.4,AX=OMIT)

GRAPH (G5/S2,DE=TEK618,OVPO=1,3.25) ...TIME(LE=6.O,NI=5,UN='SEC'),....C2(LE=3,NI=4,LO=-.2,UN='RAD',SC=O.4,PO=6.O),....CR2(LE=3,NI=4,LO=-.2,UN='RAD',SC=O.4) ....RR2 (LE=3,NI=4,LO=-.2,SC=O.4,AX=OMIT)

GRAPH (G6/S2,DE=TEK618,OV,PO=1,6.5) ...TIME(LE=6.0,NI=5,UN='SEC') ..C3(LE=3,NI=4,LO=-.2,UN='RAD',SC=O.4,PO=6.O),....CR3(LE=3,NI=4,LO=-.2,UN='RAD',SC=O.4) ....

Z4 RR3(LE=3,NI=4,LO=-.2,SC=O.4,AX=OMIT)LABEL (Gi) PHASE PLANE (CDT,CRDT,XDOT.VS.CR)LABEL (G4) STEP RESPONSE

* ENDSTOP

257

%

LIST OF REFERENCES

1. Wikstrom, R. K., An Adaptive Model Based Disc File HeadPositioning Servo System. Master's Thesis, NavalPostgraduate School, Monterey, California, September1985.

2. Ozaslan, K., The Near - Minimum Time Control of a RobotArm. Master's Thesis, Naval Postgraduate School,Monterey, California, December 1986.

3. Suza, R. P., Real Time Programming of a Robot, Master'sThesis, Naval Postgraduate School, Monterey, California,December 1986.

4. Thaler, G. J. and Stein, W. A, "Transfer Function andParameters Evaluation for D-C Servomotors", Applicationsand Industry, pp. 410-417, January 1956.

5. Thaler, G. J., Nonlinear Controls Theory, and Methodsfor Analysis and Design, unpublished notes for EC 4350, CNonlinear Controls, Naval Postgraduate School, Monterey,California, October 1987. I

6. Luh, J. Y., Fisher, W. D., and Paul, R. C.,"Joint Torque '-

Control by a Direct Feedback for Industrial Robots",IEEE Transactions on Automatic Control, V. AC-28, No.2,pp. 153-161, February 1983.

7. Critchlow, Arthur. I., Introduction to Robotics, pp. 63-64, Macmillan, NY, 1985.

8. Thaler, G. J. and Martin, H. Dost., Modelling andSimulation of Dynamic Systems, unpublished notes forEC 4370, Modelling and Simulation for Control Systems,Naval Postgraduate School, Monterey, California, July1987.

9. Sweet, L. M. and Good, M. C., "Redefinition of the RobotMotion-Control Problem", IEEE Contr. Syst. Mag., pp. 18-25, August 1985.

10. Paul, R. P., Robot Manipulators Mathematics, Program-ming, and Control, the MIT press, pp. 161-162, 1981.

258

' -

BIBLIOGRAPHY -

Asada, H and Slotine, J.- J. E., Robot Analysis and Control,Wiley, 1984.

Brady, M., Hollerbach, J. M., Johnson, T., Lozano-Perez, T.and Mason, M. T., Robot Motion, the MIT press, 1982.

Engelberger, J. F., Robotics in Practice, Ama, 1980.

Scott, P. B., The Robotics Revolution :The Complete Guide,NY, 1986.

Wolovich, W. A., Robotics : Basic Analysis and Design, Holt,Rinehart and Winston, 1985.

259-

INITIAL DISTRIBUTION LIST

'a No. Copies

1. Defense Technical Information Center 2Cameron StationAlexandria, Virginia 22304-6145

2. Library, Code 0142 2Naval Postgraduate SchoolMonterey, California 93943-5002

3. Department Chairman, Code 62 1Department of Electrical & Computer EngineeringNaval Postgraduate SchoolMonterey, California 93943

4. Professor G. J. Thaler, Code 62Tr 5

Department of Electrical & Computer Engineering rNaval Postgraduate SchoolMonterey, California 93943

5. Professor H. A. Titus, Code 62Ti 1Department of Electrical & Computer EngineeringNaval Postgraduate SchoolMonterey, California 93943

6. Hellenic Navy General Staff 42nd Branch, Education DepartmentStratopedon PapagouGR 155.61 - Holargos, GREECE

7. LT George Kalogiros H.N 5Tirtaiou 13 - A. KalamakiGR 174.56 - Athens, GREECE

8. Commander 1Naval Surface Weapons Center White OakSilver Springs, Maryland 20910

9. Commander 1Naval Weapons CenterChina Lake, California 93555

10. Chief of Naval Operations 1Attn: Code OP-03Washington, D.C. 20362

260

-'ft.

B

Alft~ft

-~ft

ftp, ft,'-

p~."ft

S

V

I~.

ft.,

a-

0p.

tftftft-"ft.

aft'

0.ftq

'I' ~

N."'

ft."'"ft'-ftp

-- p.*ftft-a

a-. ft~

S U S V w ~ 0~~fta ~ft*~ftft*~ ft~ '--'-ft..'- .-- '~ -- ~- -- ft... -* ft. ft ~. -ft -- ftp

* ft.'- '~ -a 'N. ~. -a -.... ~V% ft-ft~,~ft %~ ft~ft~%ft~

~ ~ ft~ft~ft1' ftf - ft~ft~ftftft*Jft~1

~ ftft ~ -

~ ~ P . *. J.\..\f'ftI' ~ '.? . dP,.p. ftftftft~ ~ ft ftl'ft

-A191 AUTOMATIC COMTOL OF ROBOT NOTION(U) NAVL … · distribution/ availability of abstract 21...

Documents

Transcript of -A191 AUTOMATIC COMTOL OF ROBOT NOTION(U) NAVL … · distribution/ availability of abstract 21...