-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.8 Step Response for Loaded Arm(Load=0.9 - No Gravity) .......................... 68
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.18 Phase Plane Trajectory with DisturbanceApplied at T=20 msec (No Gravity) ................. 80
6.19 Step Response with DisturbanceApplied at T=20 msec (No Gravity) ................. 81
6.20 Phase Plane Trajectory with DisturbanceApplied at T=50 msec (No Gravity) ................. 82
6.21 Step Response with DisturbanceApplied at T=50 msec (No Gravity) ................. 83
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.3 Open Loop Bode Plot of the JOINT2 Servo Motor .... 128
8.4 Open Loop Bode Plot of the Km2/s2 Motor ..... 129
8.5 Open Loop Bode Plot of the JOINT3 Servo Motor .... 130
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.8 Step Response for Loaded Arm(Load=0.1 - No Gravity) .......................... 142
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
Km3 = 77.44 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
Km2 = 0.4225 rad/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
JEF2 = 0.043 oz.in.sec2
JEF3 = 0.100 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
Figure 6.12 Error Curves for Loaded Arm(Load=1.5 - No Gravity)
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
Figure 6.20 Phase Plane Trajectory with DisturbanceApplied at T=50 msec (No Gravity)
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%
(Load=0.94 - No Gravity)
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%
(Load=0.94 - No Gravity)
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%
(Load=0.98 - No Gravity)
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%
(Load=0.98 - No Gravity)
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%
(Load=1.12 - No Gravity)
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%
(Load=1.12 - No Gravity)
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%
(Load=0.80 - No Gravity)
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>
(Load=0.80 - No Gravity)
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
Jm2 = 0.033 = motor inertia for JOINT 2
Jm3 = 0.033 = motor inertia for JOINT 3
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
JEF3 = 4.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.
a. Different Load Conditions
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 ;'.
Figure 9.15 Error Curves for Loaded Arm(Load=0.2 - No Gravity)
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.
b. Disturbance Rejection
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 .
Figure 9.18 Phase Plane Trajectory with DisturbanceApplied at T=270 msec (No Gravity)
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
Figure 9.20 Phase Plane Trajectory with DisturbanceApplied at T=370 msec (No Gravity)
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
(Load=0.19 - No Gravity)
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%
(Load=0.21 - No Gravity)
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%
(Load=0.22 - No Gravity)
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)
Figure 9.36 Phase Plane Trajectory for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%
(Load=0.155 - No Gravity)
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-.
(Load=0.155 - No Gravity)
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
Figure 9.38 Phase Plane Trajectory for Loaded Arm withKt, Kv, R Decreased and L Increased by 10%
(Load=0.165 - No Gravity)
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.
a. Different Load Conditions
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.
b. Disturbance Rejection
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%
(Load=0.155 - With Gravity)
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% '
(Load=0.155 - With Gravity)
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
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 = 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
NOSORTD11 = M3*(D2*COS(CR2)+D3*COS(CR2+CR3))**2 ...
+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.*ft- ft-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
Top Related