Embed Size (px)
Citation preview
Development of a Hydraulic Robot for Tunnel Drilling - Manipulator Kinematics and Tracking Control
A Thesis
Submitted to the Faculty of Graduate Studies
in Partial Fulfillment of the Requirements
for the Degree of
Doctor of Philosophy
in the Department of Mechanical Engineering
University of Saskatchewan
by
George Guozhen Zhong
Saskatoon, Saskatchewan
July, I995
The author claims copyright. Use shall not be made of the material contained herein
without proper acknowledgment, as indicated on the following page.
National Library 191 of Canada Bibliothgque nationale du Canada
Acquisitions and Acquisitions et Bibliographic Services services bibliographiques
395 Wellington Street 395, rue Wellington Ottawa ON KIA ON4 OttawaON K l A O N 4 Canada Canada
The author has granted a non- exclusive licence allowing the National Library of Canada to reproduce, loan, distribute or sell copies of this thesis in microform, paper or electronic formats.
The author retains ownership of the copyright in this thesis. Neither the thesis nor substantial extracts fkom it may be printed or otheMise reproduced without the author's permission.
Your rYe Vorre rrifemna
Our tile Norre rdfBrencB
L'auteur a accorde m e licence non exclusive permettant a la Bibliotheque nationale du Canada de reproduire, preter, distnbuer ou vendre des copies de cette these sous la forme de microfiche/film7 de reproduction sur papier ou sur format electronique .
L'auteur conserve la propriete du droit d7auteur qui protege cette these. Ni la these ni des extraits substantiels de celle-ci ne doivent &re imprimes ou autrement reproduits sans son autorisation.
UNIVERSITY OF SASKATCHEWAN
College of Graduate Studies and Research
SUMMARY OF DISSERTATION
Submitted in partial fblfillment
of the requiremenm for the
DEGREE OF DOCTOR OF PHILOSOPHY
by
George Guozhen Zhong
Department of Mechanical Engineering
University of Saskatchewan
Spring of I996
Examining Committee:
Dr. E. Salt C e a n ' s Designate, Chair ColIege of Graduate Studies and Research
Dr. P. N. Nikiforuk Co-supervisor, Department of Mechanical Engineering
Dr. P. R. Ukrainetz Co-supervisor, Department of Mechanical Engineering
Dr. J. N. Wilson Department of Mechanical Engineering
Dr. G. J. Schoenau Department of Mechanical Engineering
Dr. R. T. Burton Department of Mechanical Engineering
Dr. D. I. Norum Department of Agricultural and Bioresource Engineering
External Examiner:
Dr. N. Sepheri Department of Mechanical and Industrial Engineering University of Manitoba 15 Gillson Street Winnipeg, MB R3T 5V6
Development of a Hydraulic Robot for Tunnel Drilling
- Manipulator Kinematics and Tracking Control
The purpose of the research described in this thesis was to contribute to the
analytical and experimental development of a new hydraulic tunnel drilling robot which
could be used to replace the present tedious, but highly skilled manual operations, which
must be carried out in dusty, damp, noisy and often dangerous conditions. In this research,
three separate but related investigations were carried out by the author to improve the
precision and speed of positioning the hydraulic robot and to reduce its cost. The first
investigation was the determination of the kinematics of a new positioning mechanism
which has a tripod arrangement of hydraulic cylinders to improve the positioning stiffness
of the robot manipulator. The second investigation was the developrnent and
implementation of an optimal tracking control algorithm to improve the precision of the
manipulator tracking. The third investigation was the design of a tracking control
hydraulic system using a low cost stepping motor driven proportional valve incorporating
a pressure compensator to stabilize the flow gain of the valve and automaticalIy
compensate for the load disturbances. The design considerations. theoretical analysis,
(including the derivation and solution of the inverse kinematics problem) and experimental
testing which were related to these three investigations are presented in this thesis.
A kinematics model of the drilling robot manipulator was first established by
deriving the homogeneous transformation matrices for describing the relationship between
the links of the robot manipulator and its work space. A combination of analytical and
numerical methods were then used to solve the inverse kinematics problem of the drilling
robot manipulator. A three dimensional simulation was then developed to verifL the
validity of the solution of the inverse kinematics problem.
To implement an optimal tracking control algorithm, the dynamics of an
experimental hydraulic robot were analyzed and a multiple input fifth order discrete state
space model was established for the pitch control hydraulic system of the robot. An
optimal tracking control algorithm was then derived and experimentally implemented to
improve the tracking precision and positioning speed of the pitch control system of the
hydraulic robot. The design of the controller was based on the dynamic model of the
hydraulic robot and the optimal tracking control algorithm. A Kalman filter was designed
for o b s e ~ n g the state variables of the system. System identification was carried out using
a triangle pulse input method that was developed to estimate the parameters of the
tracking control system. This method was faster and caused less disturbance to the
positioning mechanism than the sine wave and the random signal methods. It also had the
advantage of being able to estimate the parameters of the system for both directions of
motion of the actuator.
A hydraulic circuit, which used a stepping motor driven valve incorporating a
pressure compensator to stabilize the flow gain of the valve and which automatically
compensated for the load disturbances, was designed and tested in the pitch optimal
tracking control system. The comparison of this hydraulic circuit with one using a
conventional proportional valve showed that the former improved the tracking
performance significantly under large load disturbances and could successfblly be used in
the robot optimal tracking control system for tracking the given trajectories of
displacement, velocity, and acceleration.
The author has agreed that the library, University of Saskatchewan may make this
thesis tieely available for inspection. Moreover, the author has agreed that permission for
extensive copying of this thesis for scholarly purposes may be granted by the professors
who supervised the thesis work recorded herein or, in their absence, by the Head of the
Department or Dean of the College in which this thesis work was done. It is understood
that due recognition will be given to the author of this thesis and the University of
Saskatchewan in any use of the material in this thesis. Copying o r publication o r any other
use of the material in this thesis for financid gain without approval by the University of
Saskatchewan and the author's written permission is prohibited.
Requests for permission to copy or to make any other use of the material in this
thesis in whole o r in part should be addressed to:
Head of the Department of Mechanical Engineering
University of Saskatchewan
Saskatoon, Saskatchewan
Canada S7N OW0
ACKNOWLEDGMENT
The author wishes to express his gratitude to his co-supervisors Dr. P.N. Nikiforuk
and Dr. P.R. Ukrainetz for their invaluable guidance, advice and encouragement
throughout his graduate studies and research, the development of this project, and the
preparation of this thesis. The technical assistance of Mr. A. Dixon and Mr. D.V. Bitner is
also gratefully acknowledged.
The author also wishes to express his gratitude to Dr. R.T. Burton, Dr. G.J.
Schoenau and Dr. D.I. Norum, members of his advisory committee. for their valuable
comments and suggestions throughout the research and the development of this project.
The author would like to take this opportunity to thank the University of
Saskatchewan and the Department of Mechanical Engineering for supporting him through
scholarships which assisted him in completing his Ph.D. studies and research.
The author would also likc to thank his former colleagues, Dr. Yang Xiangbi. Dr.
Xia Jishun. Sr. Engineer Chcn Zenan. Mr. He Qinghua. Engineer Yan Xiancai and Lab
Assistant Liu Shuncheng. in the Hydraulic Drilling Equipment Group. Mechanical
Research Center of the Central-South University of Technology. P.R. China, for their
valuable collaborations during his mrly design and research work in the 1980's on
hydraulic drilling robots.
... 111
ABSTRACT
The purpose of the research described in this thesis was to contribute to the
analytical and experimental development of a new hydraulic tunnel drilling robot which
could be used to replace the present tedious, but highly skilled manual operations, which
must be carried out in dusty, damp, noisy and often dangerous conditions. In this research,
three separate but related investigations were carried out by the author to improve the
precision and speed of positioning the hydraulic robot and to reduce its cost. The first
investigation was the determination of the kinematics of a new positioning mechanism
which has a tripod arrangement of hydraulic cylinders to improve the positioning stiffness
of the robot manipulator. The second investigation was the development and
implementation of an optimal tracking control algorithm to improve the precision of the
manipulator tracking. The third investigation was the design of a tracking control
hydraulic system using a low cost stepping motor driven proportional valve incorporating
a pressure compensator to stabilize the flow gain of the valve and automatically
compensate for the load disturbances. The design considerations, theoretical analysis,
(including the derivation and solution of the inverse kinematics problem) and experimental
testing which were related to these three investigations are presented in this thesis.
A kinematics model of the drilling robot manipulator was first established by
deriving the homogeneous transformation matrices for describing the relationship between
the links of the robot manipulator and its work space. A combination of analytical and
numerical methods were then used to solve the inverse kinematics problem of the drilling
robot manipulator. A three dimensional simulation was then developed to verify the
validity of the solution of the inverse kinematics problem.
T o implement an optimal tracking control algorithm, the dynamics of an
experimental hydraulic robot were analyzed and a multiple input fifth order discrete state
space model was established for the pitch control hydraulic system of the robot. An
optimal tracking control algorithm was then derived and experimentally implemented t o
improve the tracking precision and positioning speed of the pitch control system of the
hydraulic robot. The design of the controller was based on the dynamic model of the
hydraulic robot and the optimal tracking control algorithm. A KaIman filter was designed
for observing the state variables of the system. System identification was carried out using
a triangle pulse input method that was developed to estimate the parameters of the
tracking control system. This method was faster and caused less disturbance to the
positioning mechanism than the sine wave and the random signal methods. It also had the
advantage of being able to estimate the parameters of the system for both directions o f
motion o f the actuator.
A hydraulic circuit, which used a stepping motor driven valve incorporating a
pressure compensator to stabilize the flow gain of the valve and which automatically
compensated for the load disturbances, was designed and tested in the pitch optimal
tracking control system. The comparison o f this hydraulic circuit with one using a
conventional proportional vdve showed that the former improved the tracking
performance significantly under large load disturbances and could successfblly be used in
the robot optimal tracking control system for tracking the given trajectories o f
displacement, velocity, and acceleration.
Key words :
hydraulic rock drilling rig, robotic kinematics, optimal tracking control, system identification.
v
TABLE OF CONTENTS
Page . . .................................................................................. ACKNOWLEDGMENT 11
TABLE OF CONTENTS ................................................................... v
LIST OF FIGURES .......................................................................................... ix
NOMENCLATURE ....................................................................................... xiv
.............................................................................. 1 . INTRODUCTION 1
1 . 1 Tunnelling and Drilling ............................................................................. 1
........................................................ . I 2 Development o f Tunnel Drilling Rigs 3
1.3 Purpose and Main Scope of the Study ...................................................... 9
.............. MANIPULATOR KINEMATICS ...................................... 12
2.1 Introduction ........................................................................................... 12
2.2 Notations ............................................................................................... 13
2.3 Coordinate Frames ................................................................................. 13
2.3.1 Link Coordinate Frames (0) . (9) .............................................. 13
................... ....................... 2.3.2 Work Face Coordinate Frame (S) ... 13
........................................ 2.3 -3 Base Reference Coordinate Frames ( B ) 15
2.3.4 Tool Coordinate Frame { T) .................................................... 16
......................... 2.3.5 Goal Coordinate Frame {G) ....................... .... 16
2.4 Kinematics Modeling ........................................................................... 17
vii
.............................................................................. 3.6 Conclusions Remarks 87
4 . PARAMETER ESTIMATION .................................................................. 94
................................................................................................... 4.1 General 94
4.2 Triangle-Pulse Response of the System ................................................... 99
................. 4.3 Non-Linear Least-Squares Method for Parameter Estimation 102
4.4 Experimental Testing and Data Processing ............................................ 107
......... 5 . DISCUSSION. CONCLUSIONS AND RECOMMENDATIONS 116
.................................................................. 5.1 Summaries and Conclusions 116
................................................................ 5.1.1 Manipulator Kinematics 117
............................................................. 5.1.2 Optimal Tracking Control 118
5.1.3 Hydraulic System and Experimental Testing ................................. 119
.................................................................. 5.1.4 Parameter Estimation 1 2 1
............................................................... 5.2 Suggestions for Future Work -121
LIST OF REFERENCES ............................................................................. 125
APPENDIX A DERIVATION OF TRANSFORMATION
MATRICES ......................................................................... A1
APPENDIX B CODE OF SIMULATION PROGRAM FOR
APPENDIX B CODE OF SIMULATION PROGRAM FOR
OPTIMAL TRACKING CONTROL ......-..- ......... ..., ..... . B 1
APPENDIX C CODE OF PROGRAM FOR SYSTEM
TIDENTIFICATXON *- ...... ....*..*....*......*...............*..*-..... ..*..... Cl
LIST OF FIGURES
Figures
......................................................................................... A drilling pattern 2
A contemporary drilling rig in the 1930's ..................................................... 4
................................................................... A 3-boom hydraulic drilling rig 6
............................................................ Drilling boom in tripod arrangement 7
................. Link coordinate frames assigned to the hydraulic drilling boom 14
Hydraulic drilling boom and its motion space ........................... ... ....... 15
......................................................................... Tunnelling in goal frame 1 34
........................................................................ Tunnelling in god Frame 3 35
Turnerling in god frame 5 ......................................................................... 36
................................. Tunnelling in goal Frame 7 ................................... .... 37
Roof drilling in goal frame 1 ...................................................................... 38
. . . ...................................................................... Roof dnllmg In god frame 2 39
Roof drilling in goal h e 3 ...................................................................... 40
2.10 Roof drilling in goal frame 8 ...................................................................... 41
........................... Schematic of the mechanisms of the experimental robot 48
.......................................................... Pitch angle hydraulic control system 49
Schematic of operation of the pitch angle control .............................. ..................................... actuator and valve for u > 0 ... 5 5
Block diagram of the valve ...................................................................... 56
....... ...................... The block diagram of the tracking control system ... 58
3.6a The equivalent block diagram of the tracking control system .................... .. 59
3.6b Simplified block diagram of the tracking control system ............................ 60
3.7 Block diagram of the optimal tracking control system ............................... 65
3.8 Displacement simulation curves with the flow gain, Kv, of the v d v e varying S O % f?om the base values .......... .................... .... .-. . . 72
3.9 Velocity simulation curves with the flow gain, Kv, of the valve varying k20% from the base values ................................. ... .-.. . . . . . . . 72
3.10 Acceleration simulation curves with the flow gain, Kv. of the valve varying e 0 % fiom the base values .................................. . . . . . . . . . . 73
3.1 1 Displacement simulation curves with the time constant, T,. of the valve varying &20% fiom the base values ............................................... 73
3.12 Velocity simulation curves with the time constant. T, . of the valve varying f l 0 % fiom the base values ........................ ................ . 74
3.13 Acceleration simulation curves with the time constant. c. of the valve varying -0% fiom the base values . . ... ...................... . . . ... . . . . . . . . . . . ..... 74
3.14 Displacement simulation curves with the natural frequency, o . of the valve varying f20% from the base values ............................... ... .. . . .. . . . . . . . 75
3.15 Velocity simulation curves with the natural frequency, o . of the valve varying G O % fiom the base values .................................................. 75
3.16 Acceleration simulation curves with the natural frequency, o . of the valve varying f 2 O % fiom the base values .. ................ ..... ........ . ... . . . . . . 76
3.17 Displacement simulation curves with the damping coefficient, 6. of the valve varying +20% from the base values . . .. .. . . . . . .. .. .. . . . .. ... .. .. .. . . . . . . . . . . . . . 76
3.18 Velocity simulation curves with the damping coefficient, 6. of the valve varying +20% from the base values . . .. .. . ... . .. . .. .... ..... . .. . .. . . . . . .. . . . . . . . . 77
3.19 Acceleration simulation curves with the damping coefficient, 6. of the valve varying f20% from the base values . ................. ............-...- ... ..- ......... 77
............................................................................. 3 -20 The robotic test system 79
.............................. 3.2 1 Experimental hydraulic circuit for pitch piston control 80
........................................................................... Displacement test curves 82
................................................................................... Velocity test curves 83
............................................................................ Acceleration test curves 83
Pressure test curves ................................................................................. 84
Displacement test curves when accumulator was shut off .......................... 84
Velocity test curves when accumulator was shut off ............................... ... 85
Acceleration test curves when accumulator was shut off ............................ 85
Pressure test curves when accumulator was shut off .................................. 86
.................. Displacement test curves for analog propcrtional valve system 88
Velocity test curves for analog proportional valve system .......................... 88
Acceleration test curves for analog proportional valve system ................... 89
Pressure test curves for analog proportional valve system ....................... ... 89
Test results of proportional control when proportional
gain = 10 and when the robot boom was raised ....................................... 90
Test results of proportional control when proportional
gain = 10 and when the robot boom was lowered ...................................... 91
Test results of proportional control when proportional
........................................ gain = 5 and when the robot boom was lowered 92
The signal transformation through the digital control system incortlorating; a ste~oing motor driven ~ro~ortional valve ......................... 100
4.2 The triangle pulse response of the system when the robot boom had a minimum length and was raised ...................................................... -109
4.3 The triangle pulse response of the system when the robot boom had a minimum length and was lowered .................................................... 110
4.4 The triangle pulse response of the system when the robot boom had a maximum length and was raised ...................................................... 111
4.5 The triangle pulse response of the system when the robot boom had a maximum length and was lowered ................................................... 112
Slll
LIST OF TABLES
Tables Page
Parameters of work space ( T,, and Tm ) for fonvard drilling . . . . . . . . . . . . . . . . . . .30
.................................. Parameters of given goal frames for forward drilling 31
Joint coordinates corresponding to the given goal frames for forward drilling ............................................................................... 31
Parameters of work space ( T,, and Tm ) for roof drilling ......................... 32
Parameters of given goal frames for roof drilling ....................................... 3 2
Joint coordinates corresponding to the given goal fiames ......................................................................................... for roof drilling 33
......... Matrices TosG and ToSG corresponding to Pose 7 for forward drilling 33
The base values of the parameters ............................................................. 71
Program run when the robot boom had a minimum length ......................................................................................... and was raised - 1 13
Program run when the robot boom had a minimum length and was lowered ...................................................................................... 1 13
Program run when the robot boom had a maximum length and was raised ......................................................................................... - 1 14
Program run when the robot boom had a maximum length and was lowered ...................................................................................... 1 14
The results of parameter estimation .......................................................... 1 15
NOMENCLATURE
Notations for Chapter 2 (Manipulator Kinematics):
ROTw (or :ROT)
Si
Subscript k €[O,n]
Superscript T
Tw (or 7)
coordinate fiames assigned to the links of the boom base reference fiame goal frame work face fiame tool fiame
i [I, 2, ... 71, rotation angles ofjoints (All of the angles are measured according to the right hand rule with respect to the corresponding coordinate axes.)
denote X-Y-Z Euler angles
denotes cos(Ai)
denotes an axis of the coordinate frame u, where K E [X, Y, Z]
denotes distance of a parallel translation dong Ku
denotes distance from the origin of (u) to the origin of (v)
denotes a rotation transformation about K, by A , where A represents an angle
denotes a rotation transformation fiom {u) to {v)
denotes sin(Ai )
refers to sampling sequence number
refers to transpose of matrix
4 by 4 matrix, denotes a homogeneous transformation from (u) to (v), also denotes the position and orientation of {vJ with respect t o w
4 by 4 matrix, denotes a homogeneous transformation from {u) to
( ~ 1 to (4
denotes a parallel translation along K, for LKu
u denotes a coordinate frame, u E [O, 1,...9, B, S, T, GI
uv (or UVW) denotes two (or three) coordinate fiarnes being considered, u orv orw E [O, 1 ,... 9, B, S, T, GI
~ Y , z ] denote Cartesian coordinates (coordinate components)
Notations for the other Chapters except Chapter 2:
area of piston area of piston on rod side viscous fiiction force coefficient z E [I, 2ldriving force of ith actuator effective load of pitch piston gravitational acceleration unit matrix i E [0, n - I] performance index i E [l, 21 inertia momentum of links kinematic energy flow gain of valve overall flow gain of valve flow coefficients of actuator exhaust port i E [ I , 31 geometric dimensions i E [l, 31, length of I, at operating point geometric dimensions length of I, at operating point i E [I, 31 masses of links and load effective mass of pitch piston symmetric positive semidefinite matrix gravitational potential energy supply pressure pressure in the chamber of actuator pressure in return line symmetric positive semidefinite matrix flow rate out fkom the actuator flow rate out fiom the valve reference input vector symmetric positive semedefinite matrix, /R, I > 0 for all k Laplace operator
sampling time time constant of the valve control signal volume of P; chamber of actuator volume of tube vector of state space variables i tz [ 1,5] state space variables displacement of pitch piston
displacement of pitch piston at operating point displacement of extension piston displacement of extension piston at an operating point
estimated vector of state space variables output vector effective bulk modulus of oil in the actuator effective bulk modulus of oil in the tube damping coefficient angular displacement of the link geometric dimensions effective natural frequency of the actuator
1 INTRODUCTION
1.1 Tunnelling and Drilling
Turnelling is involved in a number of engineering applications, such as mining, hydro
power, oil storage, waste disposal, and road and railway construction in mountainous
areas. One of the most efficient methods for driving tunnels in hard rock is by drilling and
blasting. A typical work cycle includes the following five processes [I]:
1) Drilling holes into the rock face according to given patterns.
2) Placing explosives in the holes and blasting.
3) Ventilation.
4) Mucking (removing and transporting rock from the face to outside of the tunne
5) Ground support (concrete lining or timbering).
Drilling rigs (or jumbos) have been widely used for driving tunnels. The basic requirement
of a drilling rig is to move the drills to the work face, to hold the drills in given positions
and orientations according to a predetermined drilling pattern, and to drill the holes. The
drilling patterns specify the positions and orientations of the holes in the face so as to
obtain the desired contour of the tunnel (assuming that the explosives are utilized
efficiently). Depending on the size of the tunnel and the properties of the rock and the
explosives, the drilling pattern may specify the positions and orientations of dozens or
even hundreds of holes in the face. Figure 1.1 shows an example of a drilling pattern. The
top of the Figure shows the position of holes on a work face ( a cross section of a tunnel).
The bottom shows the depth and the orientation of the holes. After the holes are drilled
into the face, the explosives are placed in the holes for blasting so as to break the rock.
Usually, each cycle advances the tunnel 3 to 5 meters and the cross section of the tunnel
after blasting is larger than the desired contour. The blasting which breaks the rock outside
of the specified contour is called overbreak. It is obvious that an excessive overbreak is
not desired because it will increase the amount of mucking and ground support needed.
Figure 1.1 A drilling pattern.
The numbers on the face show blasting sequence of the holes.
Two of the most important factors which concern tunnelling engineers are cost and
time. It would be of great advantage if the drilling was precise because this would result in
effective blasting, thus reducing overbreak of rock [2] and the cost of the ground support
(concrete lining or timbering the roof and walls of the tunnel to prevent rock fall). The
cost is especially significantly reduced by improving the precision of drilling in those
situations where concrete Iining is necessary because reducing the overbreak of rock
means reducing the quantity of concrete for the filling. Since the drilling process takes up
most of the cycle, it is evident why reducing the time to position the drill and the drilling
process is important in tunnel construction.
1.2 Development of Tunnel Drilling Rigs
Drilling rigs have been used in tunnel construction in the USA since the 1930s. Figure
1.2 shows a tunnel drilling rig used in the early days [I]. The drilling rigs were mounted o n
a flat car running on the muck track. Folding scaffolding at convenient levels provided
platforms for reaching the face. The drills were mounted on bars or columns permanently
attached to the front of the rig. Some of these bars or columns were fitted with jacks in
their outer ends, and when the car was run up to the face, it was held in position by
jacking against the walls, roof, and floor.
The first significant change to affect drilling rigs was the introduction of the hydraulic
percussion hammer in the early 1970s. This hammer started to replace the old pneumatic
drills. These hydraulic drills had significant higher penetration rates than their pneumatic
counterparts and consumed less energy per meter drilled [3]. As well, they greatly
improved the environmental conditions at the rock face. However, because these hydraulic
systems were prone to fail if dust got into the system, special precautions had to be taken,
including the use of dust-free workshops for maintenance. Although hydraulic drills cost
Figure 1.2 A drilling rig in the 1930s, carrying 32 drills, mounted on a
truck chassis at a Boulder Dam diversion tunnel.
more than pneumatic drills, experience showed that the former were more economical in
the long run.
Hydraulic drilling rigs and drills experienced rapid development and found a variety
of applications in the 1980s. Figure 1.3 shows a contemporary 3-boom hydraulic drilling
rig manufactured in the mid 1980s by a Danish company called TAMROCK- Compared to
the older drilling rigs, it was more compact and flexible, required fewer operators, and the
work environment of the operators was greatly improved by providing a sealed operator
compartment. Almost at the same time a Swedish company, Atlas-Copco, developed new
drilling rigs equipped with BUT 30 series drilling booms. The improved features of this
type of drilling boom were due to the tripod arrangement of the cylinders [4] (the
actuators of the boom positioning mechanisms), as shown in Figure 1.4, which made it
more compact than any other arrangement (such as the arrangement shown in Figure 1.3)
and provided improved positioning stiffness and stability which are necessary for
improving the drilling precision.
Although hydraulic drilling rips are superior to their pneumatic counterparts, skilled
operators have to manipulate up to 8 valves to determine the position and orientation of
the drilling rods to complete a drilling task. Usually. each boom needs one operator to
manipulate it and an additional worker is needed at the front end of the drilling rod to
indicate the position of the drill h a d (using gestures) to those who operate the control
valves for the hydraulic actuators of the drilling booms. This is especially necessary for
contour holes since the precision of the drilling has great influence on the overbreak of the
tunnel rock and so on the cost of the tunnel lining.
The development ol computer controlled drilling rigs started in the early 1970s.
almost at the same time when hydraulic drills were introduced. However, this development
Figure 1.3 A contemporary j-boom hydraulic drilling rig.
rock drill drill rod feed cradle feed extra-tilt cylinder
/ feed
/ feed extension cylinder
roll-over mechanism
boom lift and swing cylinders
feed tilt and swing cylinders
boom
Figure 1.4 Drilling boom in tripod arrangement.
was much slower than that of the conventional hydraulic rigs because of high initial cost
and because of difficulties due to the severe environment in which the computer control
systems had to operate.
In the early 1980s experimental field test results of prototypes of computer controlled
tunnel drilling rigs were reported by Kristen Dahl [ 5 ] and Tohru Mashimo [6] . In these
articles the authors summarized the motivations for the development of computer
controlled drilling rigs and presented experimental test results of prototypes which had
been developed in Norway and Japan.
The reasons for developing the computer controlled automatic drilling rigs have been
summarized as follows [5, 61. First, although a detailed drilling and blasting plan may be
provided, the operators often make their own drilling patterns which they believe are even
better, although they may be quite different from the official one and also different in the
position and number of holes from gang to gang. Second, contour holes have often been
placed at random whereby they, together with overloading, have unnecessarily increased
the overbreak. The direct cost of overbreak is limited, but its effect on ground stability can
result in high extra costs. If the tunnel has to be concrete lined, for example, the total
overbreak volume has to be replaced with concrete. Third, on multi-boom rigs, one of the
booms is often idle while the operator attends to some other problem. As well, the time
spent moving from one hole to the next depends very much on the skill of the operator.
This greatly influences the total drilling time. Fourth, the penetration rate of the drilling
hammers depends very much on the material being penetrated and on the adjustment of
parameters such as percussive energy, feed pressure, rotation speed, etc. For optimum
penetration, the hammer parameters must be under constant surveillance and modification,
which is almost impossible to do manually. Hammers fiequently work, therefore, far from
their optimum conditions.
The performance of the prototypes reported by Dahl and Mashimo was satisfactory
in that the equipment worked reliably in the adverse environmental conditions
encountered in tunnelling. Improvements in operational performance and drilling precision
were demonstrated. Maintenance of the booms and associated parts was greatly reduced
due to the smoother action of the computer controlled drilling rigs. In particular, the
results reported by Mashimo showed that the average boom positioning time was about 10
to 15 seconds for the automatic drilling rigs compared to about 1 minute for manual ones.
In addition, the positioning precision was such that the maximum error of the hole position
was less than 20 mm which is equal to the lowest values in ordinary tunnelling [Z].
1.3 Purpose and Main Scope of the Study
As stated in the preceding section, satisfactory field test results of computer
controlled prototype drilling rigs were reported by Dahl [ 5 ] and Mashimo [6 ] , but the
development of the tunnel drilling robots is only beginning. Two areas of research which
would result in improved performance of the dri!ling robot were identified by the author of
this thesis. First, rapid developments in computational technology have made it possible to
use low cost high speed computers to construct control systems for implementing more
sophisticated control algorithms. If employed, this would improve the tracking precision,
reduce the processing time, and minimize the need for human operation. Second, although
a new arrangement of the drilling boom (the tripod arrangement) has been developed by
Atlas-Copco and proved in field tests to be more stiff and stable for boom positioning [4],
a kinematics model must be derived before it can be properly controlled by a computer
system. Studies related to these two research areas (the optimal tracking control and the
kinematics modeling) were considered by the author to be essential to the development of
a new advanced drilling robot. The main scope of the research reported in this thesis
includes the studies of the manipulator kinematics and the optimal tracking control of the
hydraulic robot.
The research on manipulator kinematics dealt with the kinematics modeling of an
eight-degrees-of-freedom drilling robot which has a tripod arrangement. The objective of
this part of study was to obtain a kinematics model for this type of drilling robot and to
solve the inverse kinematics problem so that for a given motion trajectory of the drilling
tool the corresponding motion trajectories of every joints of the drilling robot can be
uniquely determined. The research focused on developing a method for solving the
problem of the inverse kinematics and developing a graphic simulation method for
verifLing the correctness of the solutions of the inverse kinematics problem. This is one of
the most important and essential work in development of the new drilling robot because
the solutions of the inverse problem provide data which are necessary for trajectory
generation, motion planning, and the tracking control of the drilling robot.
The research on robot tracking control focused on the design and experimental
testing of an electro-hydraulic system employing a stepping motor driven proportional
valve which incorporated a pressure compensator to stabilize the flow gain of the valve
and automatically compensated for the toad disturbance caused by the compIex link
dynamic coupling of the hydraulic robot. The objectives of this part of the study were:
1) To investigate the feasibility of using this type of less expensive and more rugged
vdve instead of more expensive and more fragile alternatives, such as servo-valves, for
this application;
2) To investigate the feasibility of implementing an optimal tracking algorithm in this
electro-hydraulic system for the control of an experimental hydraulic robot to track given
trajectories of displacement, velocity, and acceleration. The techniques associated with the
implementations of an optimal tracking contro1 algorithm and a system identification
algorithm for this particular application were to be developed.
The research on robot tracking control included the following main topics:
1) Design of the electro-hydraulic circuit which included stepping motor driven
proportional valves for the robot tracking control system.
2) Modeling the dynamics of the tracking control system.
3) Computer simulation and analysis of the tracking control system to determine the
influence of variations in the parameters on the performance of the tracking control
system.
4) Analyzing the effect of the link dynamic coupling and load disturbances on the
performance of the tracking control system.
5) Determining the system impulse response and implementing the nonlinear least-
square method for the system parameter estimation.
6) Investigating the feasibility of different control algorithms which include optimal
tracking control, proportional control and adaptive control.
2 MANIPULATOR KINEMATICS
2.1 Introduction
Advanced drilling robots require stable positioning mechanisms to improve
positioning stiffness and accuracy of the drilling robot manipulator, of the type shown in
Figure 1.4. This chilling boom has eight degrees-of-freedom, a tripod arrangement of
hydraulic cylinders, and thus a higher positioning stiffness and greater stability under heavy
loads than other arrangements [4]. In order to use this type of drilling boom arrangement
to improve the performance of a new advanced drilling robot. its manipulator kinematics
must be determined to provide information for computer aided design and control of the
drilling robot. The solution of the inverse kinematics (discussed in Section 2.5 of thls
Chapter) then provides the tools and data which are essential necessary for motion
planning, trajectory generation, and the tracking control of the drilling robot.
In this research, the coordinate frames (or coordinate systems) and the homogeneous
transformation matrices were established to describe the positions and orientations of the
drilling boom and its links in the motion space. A combination of an analytical and a
numerical algorithm was then used to solve the inverse kinematics problem. Usually, the
solution of the inverse kinematics probIem is not unique [7, 81, but using this algorithm
and the spatial coordinate constraints for this particular application, a unique solution (a
set of the joint coordinates) corresponding to an arbitrary given position and orientation of
the end effector (drilling tool) were determined [9]. The motion trajectories of the
actuators (hydraulic pistons) can be generated based on the sets of the joint coordinates,
(determined as the solution of the inverse kinematics problem,) with respect to a desired
motion path of the end effector.
2 -2 Notations
In developing a kinematics model, various notations have been used for describing
the coordinate fiames, transformations, and structural parameters of the drilling boom, and
are given in the NOMENCLATURE of this thesis.
2 -3 Coordinate Frames
As shown in Figures 2.1 and 2.2, a set of coordinate frames was established and
assigned to the drilling boom, using the Denavit-Hartenberg representation [lo, 111, to
describe the position and orientation of the drilling boom in the motion space. A detailed
description of the coordinate frames follows.
2.3.1 Link Coordinate Frames (0) - (9)
Frames (0) - (9) were assigned to the links of the drilling boom. The first, (01, was
assigned to the base (the boom support plate) and fixed when the drilling rig was located
in fiont of the work face. The fiames ( I ) - (9) were assigned to the links with the axes
Zi coinciding with the axes of the rotational joints between the adjacent links. The
dimensions and geometrical points of each Iink were initially described in the link's own
coordinate fiame. When needed, these parameters were then transformed into other
coordinate frames. For exarnpIe, the coordinates of the gravitational center of each link
were initially described in the link's own coordinate frame, but they were transferred to
base fiame (0) to compute the resultant gravitational forces on the base.
2.3.2 Work Face Coordinate Frame IS)
All of the holes to be drilled were described in fiarne (S). In this study the X, axis
was chosen as the normal to the work face, and Zs or Ys was chosen to make the
description of the coordinates of the holes as simple as possible. For example, if the work
Figure 2.1 Link coordinate frames assigned to the hydraulic drilling boom.
Figure 2.2 Hydraulic drilling boom and its motion space.
face has a symmetrical shape, it would be better to choose Z, to be colinear with the
symmetrical axis of the work face. By doing this, the description of the coordinates of the
holes in the work face can be simplified because the other half has the same absolute
values, and only different signs.
2.3.3 Base Reference Coordinate Frames {B )
The base reference coordinate frame, {B), was necessary because it was needed as a
datum point (or line or plane) for measurement. As the drilling process of a tunnel
proceeds, the work faces are advanced and their normal direction varied for the turning or
sloping of the tunnel. Meanwhile, the rig is moved fiom place to place. Since the drilling
boom is mounted on the rig, the orientations of the link frames will be varied if the ground
is not level. All of these variations can be measured and described in i i l is fixed base
reference frame by using transfer matrices defined as T, and %. The position and
orientation of the base reference frame can be chosen accordingly to make the
measurement, description, and computation of the coordinates simpler. For example, 2,
can be chosen to be parallel with the direction of the gravitational vectors of the links.
This ensures that all of the initial given gravitational vectors have only Zi components
while the other components of the gravitational vectors are equal to zero.
2.3.4 Tool Coordinate Frame (T)
The position and orientation of the drilling tool (or drilling rod) are represented by
the tool coordinate frame {T). The origin of this fiame was located at the front end of the
drilling tool and its X, axis coincided with the axis of the drilling rod.
2.3.5 Goal Coordinate Frame {G)
Usually, dozens (even more than one hundred holes) are drilled into one work face.
These holes are described using a goal frame {G) with respect to the work face
coordinate frame {S). The goal frame represents the goal position and orientation of the
tool frame. This means that the task of positioning the drilling boom is to control all of the
joint coordinates of the drilling boom in order to make the frame {T) coincide with frame
(G). The frame (GI consists of five parameters. Three of these parameters are Cartesian
coordinates used for describing the origin of the frame, and the other two are Euler
angles1 for the orientation.
All of the angles are measured according to the right hand rule with respect to their corresponding coordinate axes.
2 -4 Kinematics Modeling
As shown in Figures 2.1 and 2.2, a set of coordinate frames was established to
describe the position and orientation of the drill boom in the work space. The 4 by 4
homogeneous transformation matrices used for describing the relationships between the
coordinate fi-ames are as follows: (In this discussion, "ROT" is a rotation transformation,
and " Trans" is a translation transformation; C, , S, , etc., are defined in the Nomenclature.
The derivation of the homogeneous transformation matrices is presented in Appendix A.)
The homogeneous transformation related frames (0) and ( 1 ) is given by
The homogeneous transformation related frames { 1 ) and ( 2 ) is determined by
T,, = ROT(Z,, A,) =
T,, = ROT(X, , A , ) = 0 c, -s, 0
0 s, c, 0
where
:ROT = ROT(XB7 Amx)ROT(YB, ABOY)ROT(ZB, Am*)
s ROT 'P,,
1 1 where (A8 = A,,, A, = AsGI- for simpliwng the writing of the matrices)
where 0
ROT(Xs,Ag)= 0 sin(A,) cos(A,)
cos(A,) 0 sin(A,)
ROT(Y,, A,) = 0 1
- sin(A,) 0 cos(A,) o 1 T
Gpc, = [Lscx. &,¶ Lscz l
0 = [O 0 01
2.5 Inverse Kinematics
In order to avoid collisions between the boom and the rock face, the drilling robot
manipulator must be controlled to track given trajectories while moving the drilling tool
fiom one hole to another. Because of the digital nature of the robot control system, the
trajectories are generated in a discrete manner; that is, the trajectories are specified by a
series of goal coordinate frames {G} which describe the position and orientation of the
drilling tool at each discrete time interval of a motion time period. As stated in Section
2.3, the holes which are to be drilled into the rock face and the trajectories (or paths)
along which the drilling tool (drilling rod) is to be moved fiom one hole to another were
specified by goal coordinate frame (GI with respect to work face coordinate frame {S) .
In order to control the drilling tool to track the given trajectories, the links or actuators of
the robot manipulator must be controlled to track their corresponding trajectories which
are specified by joint coordinates in their link coordinate frames. Therefore, for a given
goal coordinate frame, ( G ) , the coordinates of every joint of the robot manipulator must
be uniquely determined. In the preceding section the homogeneous transformation
matrices have been derived to describe the relationship between the tool coordinate e r n e
(T} and the link coordinate frames (0) - (9) of the manipulator. This section deals with
the problem of determining the coordinates of every links (or joints) of the manipulator
given the position and orientation of the drilling tool. This problem is called inverse
kinematics problem. Solving the inverse kinematics problem is an important and essential
task because the solution provides information needed for the robot motion trajectory
design in which the time histories of position, orientation, velocity, and acceleration of
links of the robot manipulator are concerned.
The inverse kinematics problem of the hydraulic drilling boom is described below.
Find a set of joint coordinates Al, A2, A3, A4, A5, A6, LX2 and LZg , to
make tool coordinate frame (T) coincide with an arbitrary given goal
coordinate frame {G).
This inverse problem can be mathematically expressed as flows :
Let
where&, represents the position and orientation of {T) corresponding to ( 0 ) ; &,
contains all of the positioning parameters, that is, the joint coordinates;
= %s* ?&3
where G, represents the position and orientation of ( G ) corresponding to (0) ;
and determine a set ofjoint coordinates Al, A2, A3, A4, A5, A6, LX2 and LZg to make
TOT = TOG (2- 1 )
T,, and T, are measurable; T, is arbitrarily given.
To solve the inverse problem an equivalent equation is derived from Equation (2.1).
Let
that is
Let (T) coincide with ( G ) ', then
By Equation (2.2), it follows that
that is
Equation (2.3) is an equivalent of Equation (2.1) where contains the unknown
positioning parameters Al, A2, A3, A4 and f&, TG5 contains the unknown positioning
parameters A5, A6, and 4, and T O B , TBS, and T, are given or measurable as previously
mentioned.
2.6 Solution of the Inverse Kinematics Problem
As mentioned in the previous Section, the problem of the inverse kinematics is to find
a set of joint coordinates Al, A2, A3, A4, A5, A6, L& and LZ,, to make tool coordinate
frame {T) coincide with an arbitrary given goal coordinate frame (G). Generally
speaking, there are two approaches to solve the inverse kinematics problem [7]; one uses
analytical method and the other uses numerical method. The analytical method solves the
inverse kinematics problem in the form of a set of formulas which describe the joint
1 As stated at the beginning of this Section, the inverse problem is to find the joint angles or displacements of the links to make the drilling tools positioned in the goal position and orientation.
coordinates for the given tool position while the numerical method solves the inverse
kinematics problem in a step by step manner in which the joint coordinates of the
manipulator for a given tool position are determined by recursive computations which
makes the solution approach to more accurate values step by step. The former is more
favorable than the latter because once the solution formulas are obtained, the computation
of the solution using the analytical method is much faster than that using numerical
method. However, the complexity of the analytical method increases dramatically when
the degrees of freedom of the robot manipulator increases. Since the drilling robot had
eight degrees of freedom, it was very difficult to use analytical method to solve the inverse
kinematics problem presented in the preceding section. To bypass this difficulty, the
combination of an analytical method and a numerical method was used by the author to
solve the inverse kinematics problem. The tactical approach to the solution of the problem
was as folIows :
1) In order to obtain a unique solution, the distance ( L s S ) tiom the origin of frame (5)
(refer to Figure 2.1) to the work face (refer to Figure 2.2) was initially chosen. This
means that the origin of frame { 5 ) was confined to a given plane which is parallel to
the work face and L,, away from it.
2) Two joint coordinates, A5 and A6, were initially chosen where A5=A8, A6=A9.
3) The joint coordinates, Al, A2, A3, A4, and L&, were evaluated to make the fiame
(5) located in the given plane and the X, axis of the frame { 5 ) parallel to the X, axis
ofthe h e (S).
4) The h e (T) was controlled to coincide with the frame {G) by modifying the joint
coordinates A5 and A6. Thus
A5= Am +AM
A6=AsGr
where A, is the error of the X Euler angle between the two frames.
5) Al, A2, A3, A4 and L& were re-evaluated according to the new values of
A5 and A6.
This iteration starts fiom (4) and continues until AM become smaller than a specified
value. The selection of the initial values of A5 and A6 were based on the following
theorem.
Given two parallel lines L1 and L2, and two parallel axes Z , and Z2. Let L l rotate
about Z, for to obtain Ll', let L2 rotate about Z2 for O2 to obtain L2'; if 8, =
4, then L 1' will be parallel to L2'.
In fact, A5 and A6 can be assumed to be X-Y Euler angles of the frame {T) with
respect to frame ( 5 ) and Ascx and ASGY to be X-Y Euler angles of {G) with respect to
{S). Since the X5 axis of (5) has been controlled to be parallel with the X, axis of {S) in
step 3, the only difference of the orientation between (5) and (S) is the X Euler angle AM
(i-e. if ( 5 ) is rotated about its X axis by A M , { 5 j will have the same orientation as (S)).
Based on the analysis above, it is not difficult to prove that if {T) has the X-Y-Z
Euler angles AS+AM, A6, and 0 with respect to ( 5 1 , and {G) has the X-Y-Z Euler angles
A,, A,,, and 0 with respect to ( S J, and if AsGx=AS+AM and Asm=A6, then
{T) and (GI will have the same orientation.
The derivation of the equation of the inverse kinematics problem is as follows :
By the symbolic equation below
GG= & 6 ' & 7 ' % ' & 9 * G G
it follows that
&(I, 4) = L& cos(%)+ LX, + LZ6 -t L& sin(&)sin(A7)
Let (for forward drilling)
G ~ ( l 3 4 ) = &S
that is,
LX9 COS(&) + LXS + L& + LZg sin(&)sin(A7) = LSs = -Lss
It follows that
where cos(& ) # 0 , that is, 4 # 90".
While drilling in the tunnel roof, A6 can be equal to 90" and Equation (2.4) can not
be used, hence, I& must be simply chosen as a constant.
Evaluating Tos5 yields
The matrix GS5 represents the given position and orientation o f the frame ( 5 ) with
respect to the h e ( 0 ) . On the other hand, the position and orientation of the frame { 5 )
with respect to the frame (0) can also be expressed by the symbolic transformation matrix
To5 as follows :
then
C1(T,,(3.3).W(,+C2-LX,+U,)=7,5(1.4)
Equating T5 (2.4) = T,, (2.4) yields
S*(&(3.3)- L X q +C2 . u2 +MI) = T o ~ ~ ( 2 . 4 )
Equating I& (3.4) = (3.4) yields
(S2C3 + C2S3 ) . W(, + S2 - LX2 = Tm (3.4)
Dividing Equation (2.7) by Equation (2.6) yields
A, = ~ ~ ( G s s ( 2 , 4 ) / G~s(1.4))
Since
S2C3 + C2S3 = sin(A2 + Ag )
it follows from Equation (2.8) that
S2-& = T,,,(3,4)- LX4 -sin(A2 + A 3 )
From Equation (2.6), it follows that
c24& = T,,(1,4)lC1 -Tws(3.3)-W(, -u, Dividing Equation (2.1 1 ) by Equation (2.12) yields
Since s2C3 + C2S3 = sin(A2 + A3)a.nd C2C3 -SzS3 = cos(A2 +A3).
then equating T,, (1, 1) = 7& (1,l) yields
C,C4 cos(A2 + A,) - S1S4 = Tw5 (I, 1)
Equating G5 (2.1) = GS5 (2.1) yields
S1C4 cos(A2 + A3 ) + CJ4 = Toss (2. 1)
Equating G5 (3.1) = T,, (3.1) yields
C4 sin(& + 4) = 7&5(3,1)
From Equation (2.14), it folIows that
~ o s ( ~ 2 + A 3 ) = (&5(l.I)+SlS4 )I (ClC4)
Substituting Equation (2.17) into Equation (2.15) yields
( ( ~ S S ( ~ , ~ ) + S ~ S ~ ) I ( C , ~ ~ ))&c4 +CIS4 = TmS(2.1)
i.e.
From Equation (2. I6), it follows that
sin(A2 + A3 ) = TOS5 (3, I) / CJ
Substituting Equation (2.19) into Equation (2.13) yields
From Equation (2.19). it follows that
A, = arcsin(T,,, (3.1) I C, ) - A, (2.2 1)
From Equation (2.12), it follows that
LX, =(Toss(l.4)lC1-Toss(3,3).LX4 -LX,)ICz (2.22)
(Note : From Equation (2.9). GS5 (I. 4) # 0 ; and from Equation (2.13), cos(A, ) # 0,
hence Al # 90° .)
The angleAY can be evaluated as follows :
A, = -arccos(Gs5(2, 3))
where
Gss = TG' - Gss
Equations (2.9), (2.13), (2.18), (2.20), (2.2 I), and (2.22) are genetic formulas for
evaluation of parameters Al, A2, A3, A4, and LX2 . These formulas were used in step 5 of
the computation for solving the inverse kinematics problem of the drilling robot.
2.7 Computational Results and 3-D Graphic Simulation
In order verify the feasibility of the algorithm presented in the preceding section for
solving the inverse kinematics problem, programs for the kinematics computations and
three dimensional graphic simulations were developed. Obviously, it is too expensive to
build a real multi-degrees-of-freedom drilling robot and implement physical experimental
testing to verify the solution of the inverse kinematics problem at this design stage.
Therefore, a computer graphic simulation, because it is a less expensive and feasible
approach, was performed for this purpose.
The 3-D graphic simulation for two basic tasks, tunnelling (or drifting) and roof
drilling (or drilling for bolting), were performed by the author. The input data (mm for
length and degrees for angle) are listed in Table 2.1, which includes the parameters
describing the work space for tunnelling, and Table 2.2, which includes eight sets of the
parameters describing the given positions and orientations of the eight goal frames in a
circular path. The output data of the kinematics program are listed in Table 2.3, which
includes eight sets of the joint coordinates determined as the solution of the inverse
kinematics problem. The 3D graphic simulation results for tunnelling. corresponding to set
numbers 1, 3, 5, and 7 in Table 2.3 are shown in Figures 2.3 to 2.6. respectively. Tables
2.4 and 2.5 give the data describing the work space and the parameters of the given
tiames for roof driIling. The output data of the kinematics program for roof drilling are
listed in Table 2.6. The graphics simulation results for roof drilling, corresponding to set
numbers 1, 3, 5, and 7 of Table 2.6, are shown in Figures 2.7 to 2.10, respectively. The
simulation results for the other goal fiames, described in Table 2.3 and Table 2.6 for
tunnelling and roof drilling, are omitted.
Figures 2.3 to 2.10 show the 3-D views (or poses) of the drilling robot for the
different given positions and orientations of the tool frame. These 3-D views provided an
intuitive tool for inspection of the correctness of the solution of the inverse kinematics
problem of the drilling robot. It can be seen from these Figures that the drilling tool is
located exactly at the given positions and orientations, and there is no mechanical
interference between the mechanisms of the drilling robot. The correctness of the solution
of the inverse kinematics problem can also be verified by inspection of Table 2.7, which
shows two matrices: GSG (representing the given position and orientation of the drilling
tool) and T,,, (representing the position and orientation of the actual tool frame of pose 7,
obtained from the solution of the inverse kinematics problem for forward drilling).
Comparing these two matrices, it is clear that the actual position and orientation of the
drilling tool match with the given position and orientation.
Table 2.1 Parameters of work space ( T,, and T,) for forward drilling.
Table 2.2 Parameters of given goal frames for forward drilling.
Table 2.3 Joint coordinates corresponding to the given goal frames for forward drilling.
Table 2.4 Parameters of work space ( T,, and T,) for roof drilling.
Table 2.5 Parameters of given goal frames for roof drilling.
33
Table 2.6 Joint coordinates corresponding to the given goal frames for roof drilling.
Table 2.7 Matrices TosG and To5, corresponding to Pose 7 for forward drilling.
TOSG
Col. 1 Col. 2 Col. 3 Col. 4
ROW 1 0.99194 0.12184 -0.03493 4689.6 19
ROW 2 -0.04916 0.623 85 0.77999 -46.174
ROW 3 0.11682 -0.77 199 0.6248 1 1464.926
Row 4 0.00000 0.00000 0.00000 1 .OOO
To% Col. 1 Col. 2 CoI. 3 Col. 4
ROW 1 0.99193 0.12184 -0.03496 4689.599
ROW 2 -0.049 17 0.62404 0.77984 -46.2 18
Row 3 0.1 1683 -0.77 1 83 0.62500 1465. 126
Row 4 0.00000 0.00000 0.00000 1 .OOO
Figure 2.3 Tunnelling in god fiame 1.
Figure 2.4 Tunnelling in goal frame 3.
Figure 2.5 Tunnelling in goal fhne 5.
Figure 2.6 Tunnelling in goal frame 7.
Figure 2.7 Roof drilling in goal frame 1.
Figure 2.8 Roof drilling in goal fiame 2.
Figure 2.9 Roof drilling in goal frame 3.
Figure 2.10 Roof drilling in goal frame 8.
2.8 Remarks
The coordinate frames representing the positions and orientations of the links of the
tunnel drilling robot and its work space were established. Four by four homogeneous
transformation matrices were modeled for describing the relationships between the frames.
The equations of the inverse kinematics problem of the robot manipulator were derived
and the combination of' an analytical and a numerical method was presented for solving
these equations. The unique solution for the inverse kinematics problem of this particular
application was found by providing the additional constraints, that is, confining the origin
of frame ( 5 ) of the robot in a given plane which is parallel with, and is positioned in a
constant distance fiom, the work face.
The graphic simulation provided an intuitive tool for inspecting the mechanical
interference of the drilling robot and for verifjing the correctness of the solution of the
inverse kinematics problem of the drilling robot. The results of the 3D graphic simulations
for two basic drilling tasks (tunnelling and roof drilling) showed that the solutions to the
inverse kinematics problem pertaining to the drilling robot manipulator are feasible and
satisfactory.
43
3 TRACKING CONTROL
3.1 General
In the preceding Chapter, the kinematics of the drilling robot manipulator were
established and the inverse kinematics problem was solved so that for a given trajectory of
the drilling tool, the unique corresponding trajectory of each link of the robot manipulator
can be determined. This unique solution then allows each actuator of the links of the robot
manipulator to track the corresponding trajectory specified in its own link frame. The
reverse is also true: if each actuator of the links of the drilling robot manipdator is
controlled to track its corresponding trajectory, the drilling tool (end effector) will be
subsequently controlled to track thc given tricctory. The design of the tracking control
SySkm was focused, thcrcfcl~. on thc tracking control of a single link (or actuator) of the
robot manipulator; that is, bascd on a dc-coupled control scheme [ I I], while the effects of
dynamics of the other links wcrc considcrcd as load disturbances to the link to be
controlled. Following this consideration, an malytical and experimental robot manipulator
with only two links (two-dcgrccs-of-ticcdom) was designed to implement the tracking
control hydraulic system, to vcrily ~ h c optimal tracking control algorithm, and to
investigate the influence ol' thc load disturbances on the performance of the tracking
control system.
The implemented hydraulic control system included stepping motor driven 3-way
proportional valves incorporating prcssurc compensators for stabilizing the flow gain of
the valves and automatically compnsating for the load disturbances. This eased the
requirement for complcx on-line computations of the link dynamics of the robot. A
method is presented for mdciing thc stepping motor driven valve in the digital control
system. The controller design was hascd on a firth-order discrete state space multiple-
input model with an optimal tracking algorithm. A Kalman filter was designed for
estimating the state variables of the tracking control system. The design of the tracking
control system enabled a single joint of the analytical hydraulic robot to continuously track
for given multiple inputs of displacement, velocity and acceleration. Computer simulations
were used for investigating the influences of various parameters (the flow gain and time
constant of the valve and the natural fkquency and damping coefficient of the actuator) on
the performance of the tracking control system. Experimental testing was carried out to
investigate the effects of the load disturbances (a result of the complex link dynamics of
the robot) on the performance of the tracking control system. A comparison of the
tracking control system using the stepping motor driven proportional vdve and a
conventional analog proportional valve was made using experimental testing. In addition,
a comparison of the tracking control system using an optimal tracking control algorithm
and an ordinaq proportional control algorithm was made.
The design concept, system modeling, analysis and experimental test results are
presented in this Chapter.
3.2 Design Consideration and System Configuration
3.2.1 Work Requirement
A major requirement of a tunnel drilling robot is to move drills and hold them in
given positions and orientations to complete the drilling task. In order to avoid
mechanical collisions between different booms, or between the booms and the work face,
and to obtain fast and smooth motion, the robot booms have to be controlled to track the
trajectories specified by points and their corresponding velocities and accelerations along a
given path. Another requirement is reducing the boom positioning time. The time saved
would be significant if it could be reduced from 15 seconds (the average positioning
time of the early drilling robots) to one second for each hob-to-hole shifr The positioning
stiffness of the robot boom is also of great concern because of the requirement for
achieving high drilling precision under large impact loads during the drilling process.
In addition to these basic requirements, some other factors which must be taken into
account in the design of the robotic control system for this particular application arc: as
follows:
1) Since the drilling robot has multi-degrees of freedom, and its control system is a
multi-valve elecuo-hydraulic control system, the cost reduction would be si@icant if less
expensive proportional valves were used instead of expensive servo valves. This could
make the application feasible and acceptable-to the customers.
2) Since a de-coupled control scheme was used for the tracking control, the hydraulic
system should provide mechanisms to automatically compensate for the influence of the
load disturbances caused by the dynamic coupling of the links.
3) Since the operating environment of a drilling robot is severe (dust, dampness, and
vibration) the ~I iabi l i ty of the system is important. In particular, the contaminant
sensitivity of the hydraulic components must be low.
Based on these consideration, the use of a servo-valve control system in this
application was rejected because of its high cost and need for strict maintenance. Instead,
stepping motor driven proportional valves incorporating pressure compensators, which are
more rugged and more economic. were selected for this application. (Usually. a servo
valve has a strict tolerance requirement for the widths of the porting lands and the
corresponding matched widths of the slr=c=ve. In addition, close tolerances must held on the
radial dimensions between spool and sleeve and on the squareness of the land edge. These
five dimensions are held to tolerances of +. 0.0001 inches or higher in high performance
valves [12]. On the contrary, the stepping-motor-driven 3-way proportional valve used in
this study has a tolerance up to 0.0004 inches for the radial dimensions between spool and
sleeve and there is no strict requirement on the matched lands.)
3 -2.2 Control Scheme
The difficulties involved in the design of robotic tracking control systems are due to
the complex link dynamics, nonlinearities, time varying and uncertain parameters, load
and friction. For this type of hydraulic robot, the effects of temperature variation and air
entrapment on the system performance must also be considered. One approach to solving
these problems is adaptive control. Numerous research papers exist on robotic adaptive
control [13], but relatively few of these papers deal with hydraulically powered robots
[ 141. When testing the effectiveness of the adaptive control algorithms, most researchers
used only ideal signals such as square wave input which could meet the requirements for
persistent excitation [ I 5, 161. However, the true effectiveness o f these design theories can
only be determined by actual implementation and experimental evaluation of each
particular application [ I 71.
In this study a practical approach to the design of the trajectory control system for
the drilling robot was implemented. The influences of variations in the parameters and the
load on the performance of the robot tracking control system were first investigated by
analysis and computer simulation. The initial studies showed that the flow gain of the
valve has a significant influence on the performance of the tracking control system. The
other variable parameters, including the time constant of the valve, and the natural
Frequency and damping coefficient of the piston, also influence the performance of the
tracking control system. Since the flow gain is a function of the load pressure which varies
as a complex function of the link dynamics and fiiction forces when the robot is in motion,
a pressure compensator was designed and built into the flow and directional control valve
to compensate for the load disturbances and to stabilize the flow gain. It should be noted
that the design of the tracking control system was based on flow control instead of force
control. The leakage of the piston and the valve selected for the experimental testing were
negligibly small during the tracking control. The load pressure compensation and the small
leakage enhanced the high load s tmess of the hydraulic system, substantially reduced the
influence of the load disturbances, and made the system insensitive to parameter variations
so as to ease the requirement for complex on-line computations of link dynamics and
parameter updating. These would have had to be conducted more frequently if there was
no pressure compensator, or if the tracking system was based on force control scheme.
This simulation study and analysis of the control system resulted in the design,
construction, and experimental testing of a 3-way stepping motor driven valve
incorporating a pressure compensator. SuccessfUl test results had been obtained earlier
and presented at the University of Bath by the author of this thesis [18]. The present work
involved using this type of valve to construct an optimal trajectory control system for the
drilling robot, testing the associated tracking control algorithms and software programs,
and investigating the influence of the parameter variations and load disturbances on the
performance of the system. In addition, a comparison of the tracking control systems using
the stepping motor driven proportional valve and using a conventional analog proportional
valve was made during the experimental testing.
3.2.3 System Configuration
A two-degree-of-freedom hydraulic robot boom was designed for experimental
testing purposes. The boom included two positioning mechanisms for controlling the
boom pitch angle and extension, as shown in Figure 3. I. The maximum boom length was
1.8 meters and the maximum payload 2000 N. The control of the pitch angle was the
major concern of this study, and the extension mechanism was used to enhance the load
disturbance of this experimental test system.
The pitch angle hydraulic control system incorporating the stepping motor driven
valves is shown in Figure 3.2. The arrows in this figure show the flow path when the
piston is driven in the extension direction. The cylinder is controlled by two 3-way valves:
Figure 3.1 Schematic of the mechanisms of the experimetal robot.
1 - base; 2 - pitch actuator; 3 - extension actuator;
4 - boom; 5 - extension boom; 6 - load.
Figure 3.2 Pitch angle hydraulic control system.
1, 5 - rotary spools driven by stepping motor;
2, 4 - pressure compensators;
3 - pitch actuator
one is connected to the pressure supply port of the cylinder, which consists of rotary spool
1 and pressure compensator 2; the other is connected to the exhaust port of the cylinder,
which consists of rotary spool 5 and pressure compensator 4. Compared to a tracking
control system using conventional proportional directional valves, three features of this
proposed hydraulic system can be identified: (Other comparisons of the performance of
the tracking control systems using these two types of proportional valves will be made
later in the experimental testing section.)
1) The advantages of a stepping motor driven valve, compared to a conventional
analog proportional valve, include its simplicity and digital nature. The stepping motor
itself is driven by pulses in an open loop mode. An analog proportional valve, however,
needs a transducer and feedback control for spool positioning and therefore is more
complex, more costly, and more sensitive to electric noise than stepping motor driven
valves.
2) The built-in pressure compensator reduces the influence of the load disturbances
which are generated by the dynamic coupling of the links when the robot is in motion. It
can also provide a more stable flow gain which is necessary for improving tracking
performance.
3) Using two 3-way proponional valves independently operated on the inlet and
outlet of the actuator improves the positioning stiffness when the closure of the valve on
the pressure supply port of the piston [19] is delayed. In addition, it is possible to add a
pilot check valve into the system for safety reasons. This would usually cause a problem
for a control system using a conventional proportional valve because when the spool is
operated in the vicinity of the null position, the pilot control pressure may not be large
enough to keep the check valve open, especially in the case where the piston has a large
negative load force.
One of the essential features of the system under consideration is that the two 3-way
valves connected to the pressure supply port and the exhaust port of a cylinder can be
operated independently. The valve on the exhaust port of the cylinder is used to regulate
flow rate while the flow gain of its orifice is stabilized by the pressure compensator. Under
normal operating conditions, the valve on the pressure supply port is blly opened before
the valve on the exhaust port of the cylinder is operated. This insures that the former does
not regulate flow rate. Once a target piston position is reached, the flow metering orifice
of the valve on the exhaust port of the cylinder closes. After a delay, the valve on the
pressure supply port closes so that the pressure in the cylinder is maintained after both
vaIves have been closed. The increased pressure in the cylinder will increase the
positioning stiffness of the piston. As well, the vacuum in the cylinder, which may be
caused by improper closure of the valve on the pressure supply port while the piston is still
in motion, will be avoided by the delay-closure of the valve on the pressure supply port. It
is also possible to use one 4-way valve instead of two 3 way valves to implement the
tracking control system and to control the pre-open and delay closure of the pressure
supply port. This type of valve would only require one stepping motor for driving the
spool of the valve. However, the matched orifices of the 4-way valve have to be specially
designed for this purpose and it is more difficult to manufacture it than a 3-way valve.
Therefore, a prototype 3-way valve was designed and used in this study instead of the
4-way valve.
3.3 System Modeling
3.3.1 Link Dynamics of the Experimental Robot
The derivation of the dynamics equations for the experimental robot was based on
Lagrange's equations [20]. The following assumptions were made for simplicity:
1) The link was a rigid body.
2) The masses of the actuators were omitted.
3) The pitch control actuator was operated in the vicinity of an operating point
(XP
4) The gravitational potential energy of the system was zero when the two actuators
were completely contracted.
From Figure 3.1 the kinematic energy of the system is given by
The gravitational potential energy of the system is given by
where 1, ,1,, 1,, 9, , and pc are geometric constants shown in Figure 3.1 . The variables
used in the derivation can be found in the Nomendature.
As is seen in Equation (3.3), the pitch angle q, is a nonlinear function of piston
displacement X,. At an operating pointX, = Xpo, this nonlinear function can be
linearized by
where
Using the Lagrange's equations
and
where F, and F, are the driving forces on the pitch and extension control actuators,
respectively.
Equations (3.5) and (3.6) are the nonlinear coupled differential equations which
describe the motion of the two-degrees-of-freedom robot boom given the external driving
forces Fp and 6.
3.3 -2 Nonlinear Model of the Pitch Control Hydraulic System
To simplify the modeling, the following assumptions were made:
1) The two 3-way valves can be operated independently. Therefore, it was possible
to pre-open the valve on the pressure supply side and to keep it hlly open during
tracking control. Thus, Ps was assumed to be constant.
2) The leakages of the cylinder and the valve were very small, and could be
neglected, therefore, in the equations describing the flow characteristics of the
vaive and cylinder.
3) The flow gain of the control valve was stabilized by using a pressure
compensator. Therefore, it was assumed to be constant.
Figure 3.3 shows the operation of the pitch angle control actuator and meter-out
valve when u > 0. The simplified equation describing the dynamics of the piston is given
by El21
where the equivalent mass mp and load F; can be determined by comparing Equations
(3 -7) and (3 -5) and are given as follows:
F;=2(m2f2 +m,l,)kf k , ~ ~ + g ( m , I ~ +m24 +m,l,)k, sin(q, +q, +p)
Using the mass flow continuity equation at chamber 1 yields
The flow equation for the exhaust port of the cylinder is given by
Figure 3.3 Schematic o f operation of the pitch angle control
actuator and valve for u > 0.
Applying the continuity equation to the tube on the exhaust port of the cylinder yields
The modeling of the stepping motor driven valve was an interesting problem because in
this control system, it consisted of digital and analog components. The modeling was
based on the observation of the experimental test results of the flow response of the
stepping motor driven valve [21]. The test results indicated that the valve could be
simplified by a transfer fbnction which included an integration factor and a first-order
factor as shown in Figure 3.4.
Figure 3.4 Block diagram of the valve
Figure 3.4 shows the simplified block diagram of the stepping motor driven valve, where u
is the input pulse rate of the stepping motor driven valve, X, is the displacement of the
rotary spool, and Qr is the output flow rate of the valve. The stepping motor of the valve
was modeled by the integration element (where K, is the gain of the stepping motor) due
to the fact that for a constant stepping rate input (u), the displacement (X, ) of the spool
is proportional to the integration of u. The dynamics of the valve were modeled by the
first-order factor, where T, is the time constant and K, is the flow gain of the valve. A
tentative approach to modeling of the stepping motor driven valve is therefore given by
where K, is the overall flow gain of the valve, given by
K,, = K,K,
As can be seen by Equation (3.13), the valve was modeled by a continuous equation
although its input is the pulse rate. In this digital control system, the pulse rate in each
sample interval was assumed to be constant.
3.3 -3 Linearized Model of the Pitch Control Hydraulic System
For a given operating point, the Linearized model was obtained by using the Laplace
transformation of Equations (3.7), (3.1 O), (3.1 1), (3.12), and (3.13). The linearized model
is given by
mPs2 X p + BpsXp = -A, P, - I;,
where K, is flow coefficient of the exhaust port.
Equation (3.14) to Equation (3.18) can be used to construct the block diagram of the
tracking control system, as shown in Figure 3.5. The equivalent block diagram of Figure
3.5 can be determined based on the rules of block diagram transformation, as shown in
Figure 3.6q where
Figure 3.5 Block diagram of the tracking control system.
1 L( VK - & + & I 1 S f - -
A, KIPIS, PI P I
Figure 3.6a The equivalent block diagram of the tracking control system,
derived fiom Figure 3.5.
Assuming that the flow resistance of the exhaust port o f the actuator is very small; that is.
the flow coefficient K, is very large, The coefficients of C,. c, and C3 can be simplified
Let
and
Figure 3.6a is therefore simplified as shown in Figure 3.6b.
Figure 3.6b Simplified block diagram of the tracking control system.
The linearized model can be determined fiom Figure 3.6b by
Equations (3.21) and (3.22) describe the dynamics of the actuator and the control valve,
respectively.
In order to implement optimal tracking control algorithm, it is more convenient to use
state space representation of the model. To obtain the state space model, the state space
variables are defined as follows:
61
The continuous state space equations are given by
Equation (3.32) is derived from Equation (3.2 I ) by omitting the SF, (the derivative of the
load force) term since it is much smaller than the Qr term since P, and P, are very large.
When ~i < 0, the piston rod is driven in the opposite direction. A similar set of
equations exist except that the piston areas are different, so are the variable volumes
between the actuator and the valve.
The matrix representation of Equations (3.28) to (3.32) is given by
where
A, =
0 L O O 0
0 -- l o 0 0 T,
0 0 0 1 0
0 0 0 0 1 0' - 0 0-0'-260 A,
These equations can now be used to predict the states of the actuator-valve system
for a given input (u) to the valve. It is noted that the external load disturbance force ( 5 )
is not included in these equations, but its influence will be investigated and discussed
through experimental testing in Section 3 -5 .
3.4 Optimal Control Scheme
3.4.1 Optimal Tracking Control Algorithm
To facilitate the implementation of computer control, the state space equations of the
system derived in the previous Section were transformed into the discrete form [22].
These discrete state space equations are given by
x,,, = Ax, + Bu,
Y k = e x ,
where
The objective of the optimal control was to control the piston to track the given
trajectories of displacement, velocity and acceleration, and to minimize the tracking errors.
The performance index was the quadratic fhction
defined over the time interval of interest [ i , ~ ] . It was assumed that P , Q, and R are
symmetric positive semidefinite matrices, and in addition that I R ~ t 0 for all k . The
performance index given by Equation (3.37) can be considered as the summation of the
tracking errors of displacement, velocity, and acceleration of every sampling intervals
between the given time interval [i, n 1.
The problem of optimal control was to find the control u, in the interval [ i , n ] that
drives the system described by Equations (3.35) and (3.36) along a trajectory X , so that
the performance index (3.37) is minimized. The optimal tracking control algorithm now
follows. (It should be noted that two approaches, the variational approach and dynamic
programming, can be used for the derivation of the optimal control algorithm. The
derivation of the optimal control algorithm can be found in [23] and is omitted in this
thesis.)
The closed-loop plant under the influence of this type of control is the non
homogeneous time-varying system
x,+, = (A-BK,)x, +BK;V,+,
The control algorithm consists of an afiine state feedback; that is, a term linear in Xk
plus a term (Vk+,) independent of X k . The gains of these two terms depend on the
solution of the Riccati Equation (3.40) and the second term depends on an auxiliary
sequence Vk derived from reference input r, by the auxiliary difference Equation (3.41).
The optimal tracking control algorithm can make the dynamic system track a desired
trajectory over the entire time interval by using the closed loop control law.
The complete block diagram of the optimal tracking control system is given in Figure
3.7, where r, represents the multiple inputs of displacement, velocity and acceleration of
the piston, v represents the noise in the measurement signal and w represents the noise in
the input which includes the modeling errors (such as the omitted load forces F, ) , and D
is the disturbance transition matrix. The KaIrnan equations [24, 251 were designed to
obtain the estimation of the state variables and provide feedback signals required by the
optimal tracking controller. This configuration of the control system was based on the
separation principle of the optimal control which states that the control and the observer
can be determined separately although they are constructed in series 1261.
Figure 3.7 Block diagram of the optimal tracking control system
3 -4.2 Formulas of the Kalman Filter
The optimal tracking control algorithm needs the feedback signals of the state
variables of the system. Although transducers can be used for measuring these variables
and providing the feedback signals, the high cost and complicated wiring deterred the
implementation of this approach. (In some applications not alI of the state variables are
measurable.) Instead, the Kalman filter [27, 281 was used for observing or estimating the
state variables of the tracking control system and providing the required feedback signals.
As is seen in Figure 3.7, the inputs of the discrete control system include the noise and
disturbances which make it impossible to obtain the true values of the state variables of the
system. The task of the Kalman filter is to obtain an optimal estimation of the sequence of
the state vector {x(k)) based on the measurement sequence {y (k ) ) in the presence of
noise and disturbances. The Kalman fiIter is actually a data processing method which can
be implemented on a digital computer. The Kalman filter uses a recursive algorithm; a new
estimation can be obtained based on each new measurement and the previous estimation.
Therefore, the Kalman filter requires less memory space and has a faster processing speed
than other optimal estimation algorithms which need to record data in an entire time
interval, and process them at the same time [28]. This filter is found in a variety of
applications, especially in real time control applications.
In this application, the designed tracking control system is observable, so only the
displacement of the piston needs to be measured in order to obtain information on all of
the state variables of the system. The Kalman filter has several formats [26, 291 and the
one implemented in this study is given in [29].
Assuming that the statistic characteristics of x(0) (which is a random vector
representing the initial value of the state variables of the system), input noise {w(k)), and
measurement noise {v(k)) are known; that is, {w(k) J is a white noise sequence which has
a normal distribution and a zero mean value:
E { w ( k ) ) = 0
E { w ( k ) w ( l ) ' 1 = R, ( W ,
where
1 when k = I
0 when k # I
(v(k)} is a white noise sequence which has a normal distribution and a zero mean value:
E { v ( k ) } = 0
E{v(k)v(l)' 1 = R2 ( k ) 6 ,
x(0) has a normal distribution, a mean value f (0) , and a covariance P(0); that is,
E[x(O)] = E(0)
E[x(O) - E(O)] [x(O) - E(O)] ' = P(0)
In addition, {w ( k ) } , ( v ( k ) } , and x(0) are assumed to be independent.
The formulas of the Kalman filter are as follows:
x(k + I ) = A(k)s (k ) + B(k)u(k) + D(k)w
~ ( k ) = C(k)x(k) + v ( k )
K(k + 1 ) = P(k + 1 / k)CT (k + I)[C(k + 1)P(k + 11 k ) c T ( k + 1) + R2 (k) ] - '
P ( k + l / k + l ) = P ( k + l / k ) - K ( k + l ) C ( k + l ) P ( k + l / k )
P(k + 1 / k ) = A(k)P(k / k ) ~ ' ( k ) + D(k)R, ( k ) ~ ' ( k )
Remarks:
1 ) The estimation of the state variables is a linear fbnction of the value of the
measurement sequence, { y ( k ) ) .
2) The variance of the error between the estimated value and the actuaI value of the
state variables is minimized.
3) The estimation is unbiased, and the mathematical expectation of the estimation
has the same value as its true value.
3.4.3 Simulation and Analysis
Computer simulation is a usefbl tool for the design of the optimal tracking control
system of a hydraulic drilling robot. In this study the computer simulation and analysis of
the optimal tracking control system were carried out to determine the effects of a number
of parameters and variables of the system. These parameters and variables were:
1) the sampling rate
2) the parameters of the weighting matrices of the optimal control algorithm
3) the maximum acceleration of the reference input trajectory
4) the time delay and nonlinear elements of the system
5) the flow gain and time constant of the valve, and the frequency and damping
coefficient of the system
The simulation results show that for the optimal tracking control system, the higher
the sampling rate, the better the performance if there is no delay element in the system.
However, the complexity of the optimal control algorithm and the computational power of
the computer place a limit on the highest sampling rate. On the other hand, if a time delay
element exits in the digital control system, an excessive high sampling rate may cause an
undesired effect [22]. The simulation results showed that fairly good tracking performance
can be obtained by using a 5 millisecond sampling time; that is, a sampling rate of 200
samples per second.
The determination of the parameters of the weighting matrices of the performance
index of the optimal tracking control system were based on a trial and error method. The
relative values of the weighting matrices P, Q, and R were determined based on the
emphases of the tracking control strategy. The simulation results showed that increasing
the values of P reduced the final position errors of displacement, velocity, and
acceleration; Increasing the values of the Q matrix decreased the tracking error of
displacement, velocity, and acceleration. In these two cases, as the values of P and Q
increased, the value of control input u also increased. The simulation results also showed
that a large acceleration of the given trajectory would result in a large value of control
input u. Since the actual system has a limitation on the maximum value of control input u,
saturation may occur if the values of P and Q, or the value of acceleration of the given
trajectory, are selected too large. On the contrary, increasing the value of R reduced
control input u, but increased the tracking error of the system. This meant that increasing
the constraint on control input u would reduce the capability of the system to track the
trajectory with a high acceleration value and would result in large tracking errors.
Since the maximum acceleration of the piston was proportional to the derivative of
the flow rate of the stepping motor driven valve, it can be seen from Equation (3.22) that
the maximum acceleration of the piston is directly related to the maximum stepping rate of
the stepping motor. A higher maximum acceleration of the given trajectory wouId result
in a higher maximum stepping rate of the stepping motor. This indicates that saturation
could occur if the maximum acceleration of the given trajectory was too high, or the
maximum stepping rate of the stepping motor too low.
Time delay and nonlinear elements also existed in the optimal tracking control
system. These elements normally have important influences on the performance of the
system, but require considerable time to study their effects. For this reason, in this study,
the system was assumed to be linear and the influence of time delay was ignored.
The determination of the effects of the parameters of the flow gain and time constant
of the valve, and the frequency and damping coefficient of the actuator, are of special
interest in this study. The simulation and analysis for determining the influences of these
parameters are now introduced.
The reference inputs of the simulated optimal tracking control system are: discrete
values of displacement, velocity, and acceleration. These were obtained by sampling a fifth
order polynomial which describes the desired trajectory of displacement of the actuator
where the initial and final values of velocity and acceleration are equal to zero.
The optimal control algorithm included two computation procedures:
1 ) off-line computation;
2) on-line computation.
The off-line computations were based on Equations (3.38) to (3 -4 1) where the auxiliary
sequences, V, , were determined. The on-line computation, on the other hand, based on
Equation (3.42) where the control signal u was determined using the feedback state
variables and the auxiliary sequences.
To investigate the influence of the overall flow gain, K,, of the valve on the
performance of the tracking control system, the simulation curves of the optimal tracking
control system with the actual values of flow gain which varies S O % fiom the base value
(given in Table 3.1 ) are shown in Figures 3.8 to 3.10. Figures 3.1 1 to 3. I 3 show the
simulation curves with the actual values of the valve time constant varying +20% fiom the
base value. Figures 3.14 to 3.16 show the simulation curves with actual values of the
natural frequency of the actuator varying S O % Born the base value. Figures 3.17 to 3.19
show the simulation curves with the damping coefficient of the actuator varying S O %
fiom the base value.
Comparing the different simulation curves, shown in Figures 3.8 to 3.10, it can be
seen that the variation of the flow gain of the proportional valve has a significant influence
on tracking performance. The variation of other parameters, which include the time
constant of the valve and the natural fiequency and damping coefficient of the piston, have
very small influences on tracking performance. Their influences can be seen in Figures
3.1 1 to 3.19.
Table 3.1 The base values of the parameters.
parameters
base values
actual value 1
actual value 2
flow gain time constant natural fiequency damping coeff
K, r, W S
( cu.cm/sec/step ) (set) (radhec)
4.1 0.02 40 0.25
4.9 0.024 48 0.30
3 -3 0.016 32 0.20
Figure 3.8
0.25 0.5 0.75
Time (sec)
Displacement simulation curves with the flow gain, varying -0% fkom the base values.
. . . - - . . . . .
....... -. - . . . . . . . - . - - - - . .
. . . . . . - -. -. . . - -. -. -. . .
. . . . . .a - ..... .- . . . . . - ..
.. .- ........ ...... -... . . . . . . . . . * -.
. . . . . . . . . . - . -. - . .. . . . . . - ... ..
0 0.25 0.5 0.75
Time (sec)
+Actual 1 -Actual 2 + Given
K, , of the valve
Figure 3.9 Velocity simulation curves with the flow gain, K,, of the valve varying S O % fiom the base values.
-30 1 I I I I L 1
I I I I I I I i I
0 0.25 0 .5 0.75
Time (sec)
Figure 3.10 Acceleration simulation curves with the flow gain, K,, of the valve varying S O % fiom the base values.
0.25 0.5 0.75
Time (sec)
/ t Actual I
=;5"1
Figure 3.1 1 Displacement simulation curves with the time constant, valve varying &20% from the base values.
T,, of the
0.25 0 .5 0.75
Time (sec)
Figure 3.12 Velocity simulation curves with the time constant, T, , of the valve varying S O % fiom the base values.
Figure
0 0.25 0.5 0.75
Time (sec)
3.13 Acceleration simulation curves with the time constant, valve varying e0% fiom the base values.
T, , of the
+Actual 1
&Actual 2 , + Given
0 0.25 0.5 0.75
Time (sec)
Figure 3.14 Displacement simulation curves with the natural frequency, o , of the actuator varying f20% &om the base values.
0 0.25 0.5 0.75
Time (sec)
Figure 3.15 Velocity simulation curves with the natural frequency, o , of the actuator varying -0% fiorn the base values.
0.25 0.5 0.75
Time (sec)
Figure 3.16 Acceleration simulation curves with the natural actuator varying E20% from the base values.
frequency, w , of the
0.25 0.5
Time (sec)
Figure 3.17 Displacement simulation curves with the damping coefficient, 6, of the actuator varying -0% from the base values.
0 0.25 0.5 0.75
Time (sec)
Figure 3.18 Velocity simulation curves with the damping coefficient, 6, of the actuator varying S O % fiom the base values.
...... ..... ... i . . . . . . ..... . . ....
-. - , - -..
. ....... ... - ..... - .- ...
0 0.25 0.5 0.75
Time (sec)
Figure 3.19 Acceleration simulation curves with the damping coefficient, S, of the actuator varying S O % fiom the base values.
3.5 Experimental Test Results and Analysis
3.5.1 Robot Test System
In addition to the computer simulation, experimental testing was carried out by the
author to verify the feasibility of the designed hydraulic system and the optimal tracking
control algorithm. Also, additional experimental tests were carried out to investigate the
influence of load disturbances on the tracking performance of the system. For this
purpose, a prototype robot was designed (by the author) and manufactured by the
Engineering Shops of the College of Engineering. The test equipment, which included
computers, sensors, and various instrumentation, are shown in Figure 3 -20. The computer
control system included two 80486 computers - a 66 MHz master computer and a 33
MHz slave computer - which communicated through a general purpose interface bus
(GPIB) having a maximum data transmission rate of 1 mbytedsec. The master computer
was used for trajectory generation, real time monitoring, and data processing (which
included parameter estimation); and the slave computer was used to control two actuators
and receive and send sampling data to the master computer for real time monitoring and
data processing. The slave computer could be operated off-line or controlled by the
master computer. The monitored signals included the displacement, velocity, and
acceleration of the pistons, and the pressure in the chambers of the actuators. Two LVDT
transducers were used for measuring the displacements of the pitch piston and the
extension piston. The velocity and acceleration were generated by the Kalman filter. The
real-time software for the robotic experimental test system was developed by using
assembly language and C/C++ for Windows in the multiprocessor and multitasking
environment.
The experimental hydraulic circuit for the pitch piston control of the robot is given in
Figure 3.21. Since only one 3-way stepping motor driven proportional valve was made,
the solenoid 4-way directional valve was used to control the directions of the piston,
Figure 3.20 The robotic test system
and the stepping motor driven valve incorporating a pressure compensator was used to
regulate flow rate in the meter-out configuration so as to control the speed of the piston
Two pressure transducers were installed, one in the inlet and the other in outlet of the
cylinder. The LVDT transducer was used to measure the displacement of thr piston -The
accumulator was installed in the pressure supply pon to stabilize the supply pressure ot'
the hydraulic system.
3.5 .l Experimental Test Results and Analysis
Three sets of experimental test results were obtained for investigating the intluence ot'
a load pressure disturbance on the optimal tracking performance of the control sysreni
[30] . Two types of valves were compared in the pitch control system where the samc
Figure 3.21 Experimental hydraulic circuit for pitch piston control.
I - Pump, 2- safety valve, 3- pressure gage, 4- oil filter, 5- needle valve,
6- accumulator, 7- solenoid directional valve, 8- pressure transducer,
9- pressure transducer, 10- pitch piston, 11- LVDT transducer,
12- stepping motor driven valve, 13- thermal meter, 14- tank.
optimal tracking control algorithm was used. One was a stepping motor driven
proportional valve, incorporating a pressure compensator, and the other was a
commercially available analog proportional valve without pressure compensation. (The
proportional valve was modeled by a first-order factor; the input to the valve was voltage
and the output was flow ratc. The other part of the analog proportiond valve control
system was similar to the stepping motor driven valve control system shown in Figure
3.5.) During experimental testing, the cxtension actuator was controlled to travel a
displacement of LO cm in 1 swond while the pitch actuator was controlled to track a given
trajectory specified by a tifth order polynomial with zero velocity and acceleration at the
initial and final points. Thc multipic given inputs tbr the pitch actuator control were
displacement, velocity, and acceleration of the piston.
Figures 3.22 to 3.25 show' thc test results of the stepping motor driven valve.
Figures 3.26 to 3.29 show the tcst results of the stepping motor driven valve when the
accumulator, (refer to Figurc 3.21) connected to the output of the pump to stabilize the
supply pressure Ps. was shut off to enhance the disturhance. Dramatic variations in
pressures Ps and P1, at thc inlct and oullct of the pitch actuator respectively, can be
observed. It can also he seen that rht: large load disturbance had very little influence on
tracking performance. In fact. the maximum displacement and velocity tracking errors
were less than 4% and the maximum acceleration tracking error was less than 8%. The
relative error was defined by
actual value - givcn value Relative Error =
magnitude of given value
' It is to be noted hat the plols for ihc givcn and actual signals arc almost identical in Fig. 3.22 as well as in olher -es.
"here the magniludc of thc givcn value ncvcr approaches zero.
In addition, a fast and very smooth motion control of the large inertial load was achieved.
When the stepping motor driven proportional valve was replaced by an analog
proportional valve without pressure compensation (purchased from Vickers), a significant
influence of the load disturbance on the performance of the system was observed. A set of
test results for the analog proportional valve system are shown in Figures 3.30 to 3.33.
AIthough good performance was obtained when the displacement was small and only the
pitch actuator was operated, a signilicant displacement tracking error was observed as
shown in Figure 3.30 when both the pitch and extension actuators were operated. This
large tracking error was altrihutcd to thc load disturbances with the following two effects:
1) Input of I;;.
2) Variation of the flow gain of thc proportional valve.
The frrst effect did not signilicantly affect the system performance because the leakage in
the tracking control system, which included thc valve and the piston, was very small and
T i m e (sec)
Figure 3.22 Displacement test curves.
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time (sec)
Figure 3.23 Velocity test curves.
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time (sec)
Figure 3.24 Acceleration test curves.
0 0.3 0.6 0.9 1.2
Time (sec)
Figure 3.25 Pressure test curves.
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time (sec)
Figure 3.26 Displacement test curves when accumulator was shut off
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time (sec)
Figure 3.27 Velocity test curves when accumulator was shut off.
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Time (sec)
Figure 3.28 Acceleration test curves when accumulator was shut off.
0 0.3 0.6 0.9 1.2 1.5
Time (sec)
Figure 3.29 Pressure test curves when accumulator was shut off.
the dynamic stifiess was very large. The second effect was significant because the flow
gain of the control valve was determined by the pressure difference across the control
orifice. This pressure difference is directly related to the load.
For the case where a pressure compensator was built into the stepping motor driven
valve, the flow gain of the valve was stabilized and found to be almost independent of the
pressure difference across the actuator. It can be deduced from this that the load
disturbance had very little influence on the tracking control system using the stepping
motor driven valve incorporating the pressure compensator.
Additional experimental testing was carried out for comparing the performance of the
hydraulic robot control system using the optimal tracking control scheme and the
performance using the conventional proportional control scheme. The experimental test
results of the control system using the conventional proportional valve and the
proportional control scheme showed that the tracking error of the pitch angle trajectory
control could be reduced to an acceptable level by a proper selection of the proportional
gain of the system when the boom was raised, as shown in Figure 3.34. However,
shuddering (oscillation) was observed whcn the boom was lowered. shown in Figure 3.35,
because of the negative load effect due to the gravitational forces on the boom. (This
phenomenon was not observed in the experimental testing of the optimal tracking control
where there was no significant diff'erencc in test results either when the boom was raised
or lowered.) When the proportional gain was reduced, the oscillation was also reduced,
but the tracking error could not k maintained at an adequate level, as shown in Figure
3.36. This experimental tcst result showed that good performance of the tracking control
system could not he achievcd by simply adjusting the proportional gain of the system, thus
showing the need for a prcssurc compcnsator to stabilize the flow gain of the valve.
Comparison of the test results ol' the proportional control scheme and the results of the
optimal tracking control scheme shows that the advantage and feasibility of the latter are
obvious.
3.6 Concluding Remarks
In order to implement an optimal tracking control algorithm to improve the
tracking control performance ot' ~ h c new advanced drilling rohot, the dynamics of an two-
degrees-of-freedom expcrimcntal robot was tirst analyzed. The simulation study was
carried out to investigate thc cffccts of the variations of the parameters, which were flow
gain and time constant of thc proportional valvc, and the natural frequency and damping
coefficient of the pitch control actuator. i t was found that the flow gain of the
proportional valve had a signilicant cffcct on the tracking performance of the robot. This
indicated the necasity of using a pressurt: compensator to stabilize the flow gain of the
valve in the tracking control hydraulic system. A hydraulic tracking control system using il
stepping motor drivcn proportional valvc incorporating a pressure compensator was then
designed and mdclcd. An optimal tracking control algorithm was implemented for a
Time (sec)
Figure 3.30 Displacement test curves for analog proportional valve system.
0 0.1 0.2 0.3 0.4 0.5 0.6
Time (sec)
Figure 3.31 Velocity test proportional
curves for analog valve system.
Time (sec)
Figure 3.32 Acceleration test curves for analog ~ro~ortional valve svstem.
Time (sec)
Figure 3.33 Pressure test curves for analog proportional valve system.
0.5 0.75 1
Time (sec)
Displacement of the piston.
Actual
I Given
0.5 0.75
Time (sec)
Pressures measured
1 1.25
at the inlet and outlet ports.
Figure 3.34 Test results of proportional control when proportional
gain = 10 and when the robot boom was raised.
0.5 0.75 1
Time (sec)
(a) Displacement of the piston.
Time (sec)
(b) Pressures measured at the inlet and outlet ports.
Figure 3.35 Test results of proportional control when the proportional
gain = 10 and when the robot boom was lowered.
0.5 0.75 1
Time (sec)
Displacement of the piston.
- Actual
0.5 0.75 1
Time (sec)
@) Pressures measured at the inlet and outlet ports.
Figure 3.36 Test results of proportional control when the proportional
gain = 5 and when the robot boom was lowered.
single joint (pitch control actuator) of the robot, which resulted in the improved tracking
in the case of multiple inputs of displacement, velocity. and acceleration of the pitch
control piston. The effect of the load disturbance caused by the link dynamics was
investigated through experimental testing. The experimental test results showed that the
pressure compensator built into the designed valve was able to stabilize the flow gain and
automatically compensate for load disturbances so as to simplify the complex iink
dynamics computations and allow the system to be controlled in a de-coupled form.
Although the model of the robot manipulator was based on the assumption that there
were small variations in the variables and parameters about their initial operation points,
the experimental test results showed that the designed system was insensitive to the
parameter variations. On the contrary. the. optimal tracking control system incorporated
the conventional proportional valve without pressure compensation was more sensitive to
load disturbances and parameter variations. Therefore, more sophisticated control
algorithms, such as using a software to compensate the load disturbances and on-line
parameter estimation, are necessary to reduce the tracking errors.
The experimental test results in this Chapter showed that the designed stepping
motor driven valve, the optimal control algorithm, and the associated software programs
could be successfully used in a robotic tracking control system for tracking the given
inputs of displacement, velocity, and acceleration.
94
4 PARAMETER ESTIMATION
4.1 General
The first step in the design of a control system is to develop a mathematical model of
the system because it is impossible to determine a control algorithm without knowing the
mathematical model of the control system. There are two approaches to modeling: one
approach is through theoretical analysis and the other is through experimental testing and
data processing. Using the first approach, a designer must have sound knowledge of the
process and the laws of physics, mechanics, thermodynamics, etc., which govern the
behavior of the dynamic system. However, there are some processes or phenomena which
are too complicated to model theoretically and the parameters of the processes have to be
determined experimentally. In these cases, system identification methodology can provide
the tools that are needed to solve the modeling problems. Even when a mathematical
model of system can be derived through theoretical analysis, the system identification
method can still play an important role in verifjing the correctness of the mathematical
model or formulas which determine the parameters of the mathematical model.
In Chapter 3, the mathematical model of the tracking control system was established
through theoretical analysis. Formulae were derived to calculate the natural fiequency and
the damping coefficient of the system. The natural frequency and damping coefficient of
the actuator were formulated using Equations (3.19) and (3.20) where these twc
coefficients are functions of the bulk modulus of the hydraulic oil. However, the effective
bulk modulus of the hydraulic oil cannot be precisely determined through theoretical
analysis because of the complexity of the hydraulic lines and the uncertainty as to the
amount of air entrapped in the lines. A number of assumptions were made, therefore, to
simplifL the derivation of the mathematical model which otherwise would have been too
complicated to solve. One of the assumptions made was that the transmission time of the
pressure wave to propagate through the hydraulic lines was zero. The correctness o f the
assumptions and the theoretical calculations to determine the value of the effective bulk
modulus of the hydraulic oil can be verified by the system identification method.
Based on the amount of a priori knowledge of the system to be identified, the
problem can be divided into two categories. The first is called the black case problem, or
the complete identscation problem, where the characteristics of the system to be
identified are completely unknown. This is the most difficult problem in system
identification. The second is called the gray case problem, or the incomplete identification
problem. In this problem, the characteristics, such as the order of the dynamic equation or
the bandwidth of the response of the system to be identified, are only partially known.
Like most identification problems encountered in engineering practice, the identification
problem described in this Chapter belongs to the second group.
Compared to other algorithms such as the adaptive control algorithm and the
conventional PID control algorithm, the optimal tracking control algorithm needs more
precise system parameter values t o achieve good tracking performance. Although some
parameters such as the natural frequency and damping coefficient of the system can be
approximated by analytical methods, some parameters such as the time constant o f the
valve cannot be determined using analytical methods. Instead, experimental methods are
more efficient and more reliable for determining the values of these parameters. However,
since the parameters vary when the robot manipulator is in motion, the estimation of
parameters should be updated when the robot manipulator moves to a new position. For
this particular application, the displacement and the parameter variation of the robot
manipulator, which correspond to a hole-to-hole shift, were made to be relatively small so
that parameter estimation could be implemented off-line at each drilling position. (The
maximum displacement of the actuators corresponding to the hole-to-hole shift of the
robot manipulator is usually less than one tenth of the stroke length of the actuators.)
During the process of system identification, signals are fed into the system to be
tested and the output of the system is sampled and recorded. The general identification
problem is to determine a complete description of relationships between the input states of
the system and its output states. There are a number of input signals that can be used for
dynamic system identification. These include signals such as a sine wave signal, a random
signal, an impulse signal, or any other signal which can sufficiently excite the system to be
identified. Selection of the input signals for system identification is highly dependent on
the application. A brief review of methods of system identification using these different
type of input signals are given in the following paragraph. Based on this discussion the
triangle pulse input method was selected by the author in this study.
Sine wave signals are widely used for system identification when the dynamic system
is to be described in the frequency response domain. During system identification, sine
wave signals are fed into the system. If the system is linear, its output is also in the form of
a sine wave, but its magnitude and phase are different from that of the input sine wave.
Although sine wave signals are one of the most convenient input signals available for
testing the frequency response of a system, the fiequency response test has to be
conducted at several frequencies over the whole fiequency spectrum of interest in order to
obtain the complete frequency response characteristics of the dynamic system. In addition,
several stable wave cycles of the output response of the system have to be sampled and
recorded at each frequency of interest to obtain good parameter estimation. This process
could be very time consuming if the natural frequency of the system to be identified is very
low. (The lowest natural frequency of actuators of a drilling robot was estimated to be less
than 3 Hz. To record a stable sine wave response at a single frequency point of the whole
frequency spectrum may need several seconds.)
The advantage of using a random signal input for system identification is that it has
the potential of being implemented in an on-line manner. However, this method was
rejected for this application because of the following two reasons:
1) The random signal input may cause vibration in the robot manipulator for a
sustained period of time. This is undesirable because the vibration could generate a large
inertial load on the positioning mechanism and accelerate mechanical wear in the link
bearings.
2) The random signal input may cause the piston to drift a large amount because the
actuator is a single rod end type.
Adaptive control provides tools for on-line parameter identification when the system
operates under normal conditions. The adaptive control algorithm is not only able to
estimate the parameters of the system, but also able to adjust the controller to obtain the
desired control performance. However, when a normal input signal cannot sufficiently
excite the system, an additional dither signal is needed to provide the necessary source of
excitation for the system. The potential of implementing adaptive control in this
application will be discussed in Chapter 5 of this thesis. The actual implementation of the
adaptive control algorithm is outside of the scope of this thesis.
For a theoretical system, an impulse signal can be used for system identification.
Once the transfer knction of the system has been determined, the impulse input signal can
be used to determine the parameters of the system. However in an actual system, an ideal
impulse signal cannot be generated because there is no a physical system which can
generate the ideal pulse with infinitely small width and infinite large magnitude. Instead, a
triangle pulse signal can be easily generated and used for system identification, and a
convolution integration method used for determining the system response. This was the
method adopted in this thesis for the implementation on the actual system.
The advantages of using a triangle pulse input for system identification over the other
methods previously stated are as follows:
1 ) With the impIementation of parameter estimation using a triangle pulse input, the
response test time is minimized and the iterative estimation of parameters can be
implemented in parallel with the operation of the drilling process. The test time of the
impulse response is less than 1 second for each operation. This test time is small compared
to a period of about 3 minutes for a drilling cycle.
2) The parameter estimation method can be implemented for both directions in
which the piston moves. This feature is usehl for this application because the pistons used
in the positioning mechanisms of the system were of the single rod end type and parameter
estim>i;i.*,r in both directions of movement are required. This would be impossible if the
input signals were random or sinusoidal.
3) Because the test time of the triangle pulse response is very short, there is less
disturbance to the positioning mechanism than if other methods were used.
4) Parameter estimation using a triangle pulse input is easier to implement than the
on-line parameter estimating method.
4.2 Triangle Pulse Response of the System
One of the difficulties involved in the parameter estimation of the system using
proportional valves was due to its high order transfer hnction and because of its slow
response speed compared to servo valves. A servo valve control system can usually be
modeled by a third order transfer function in which the servo valve is simply modeled by a
constant gain because the time constant of the valve is much smaller than that of the
actuator, or the natural fkequency of the servo valve is much higher than that of the
actuator. However, the time constant of a proportional valve control system usually
cannot be neglected because its d u e is close to that of the actuator.
In Chapter 3, the dynamic system of the tracking control system using a stepping
motor driven valve was modeled by a fifth order mathematical model. The input to the
system was the pulse rate u, and the output of the system was the displacement of the
piston. The high order of the system made it difficult to determine the parameters of the
system through system identification. To simplie this problem, the author introduced an
ideal rotary spool displacement, which is equal to the integration of the input pulse rate, to
facilitate data processing. The prospective signal transformation through the stepping
motor driven valve is schematically illustrated in Figure 4.1. As seen from this Figure, the
input signal or stepping rate was fed into the ideal sampler and became a discrete pulse
rate (approximately) which passed through the data hold and the integral element of the
system. The output of the integration element was a triangle pulse shape and could be
considered to be the ideal rotary spool displacement, which did not include the dynamics
of the valve or the stepping motor. The system transfer function which does not include
the integration element of the stepping motor is given, therefore, by
Y(s) - - Kv I A, - - ~ , w ' 1 (A, T, ) s' 2s s(s + 1 l T,)(s' + 26rrrs + 02)
s(T"s+I)(-+-s+l) W' 0
where xv is the ideal displacement of the rotary spool of the valve.
Figure 4.1 The signal transformation through the digital control system incorporating a
stepping motor driven proportional valve.
The system impulse response is determined by using the inverse Laplace transformation:
Y *
U
2'
where Xv (s) = 1 .
It follows that
W ) = f ( t ,Kv ,T , ,w9
where
K , p 2 / (A , -T,) s(s+1/T,)(s'+26m+o2)
1 - e-sT
s
I .r, - s )
The system response y( t ) to an arbitrary input r( t ) can be determined by using the
convolution integration:
For the time interval [O,tb] the convolution integration can be approximately determined
where
Equation (4.4) was used for determining the triangle pulse respulse of the system.
4.3 Non-Linear Least-Squares Method for System Parameter Estimation
Since the triangle pulse response o f the system was o f non-linear form, the non-linear
least-squares method was used for the system parameter estimation. The implementation
of the non-linear least-squares method [3 1, 321 for iterative parameter estimation is
introduced as follows.
The response of the tracking control system is given by:
Y = f ( f , 8 ) = f ( t , K v , T v . ~ , 6 ) (4.5)
where
@ = LO,, e,, 61, 821' =[& Lo, 61 (4-6)
Assuming that the observed value of y at tk is yk. k=1,2,3,. . .N. A set of equations can be
obtained based on the observed values:
The fitting error is defined by
en = y k - f ( lk ,8) , k=1 ,2 ,..., N(24)
and the index fimction is defined by
The principle of the nonlinear least-squares method is to linearize f ( t , ,8) and to
determine the values of vector8 to make J(8) to be minimum.
where
do) is the initial estimated parameter vector which is composed by Kv, T,, qand S . as
q a s g shown in Equation (4.6). The partial derivatives of -, - - . and - ae, ae, ' a, %e 24
determined by
where
where
where
Substituting Equation (4.10) into Equation (4.9) yields
To make J(@ + min . A should satisfy
where
i = l , 2 ,... 4. and j = l , 2 ,... 4.
From (4.13) it follows that
The nonlinear least-squares problem in Equation (4.7) was converted to the linearized
form given in Equation (4.14). As long as A is non-singular, A can be determined by
A=A- 'V
Therefore, 0 can be determined by
8= go) + A
For practical implementation, the initial ~ a l u e 0 ' ~ ) may be far from the true value of 0 and
the error caused by linearization cannot be neglected. Therefore, t3'0' + A is not equal to
8, but is usually closer to the true value of 0 . Using 8"' + A as a new initial value of 0
for the next run of the iterative computation, a new A can be determined. This iterative
computation continues until Inequality (4.1 8) is satisfied. One of the limitations of the
Guassian-Newton method is that if the selection of 8'0' is too far fiom the true value of
8, the assumption made for Equation (4.10) cannot be satisfied and the iterative
computation may not converge. This limitation can be extended by introducing a damping
coefficient 2 as shown in Equation (4.19). This method is called the damped Guassian-
Newton method [32] and was implemented by the author of this thesis for the parameter
estimation of the tracking control system.
4.4 Experimental Testing and Data Processing
Experimental testing was carried out to venfL the feasibility of the scheme to test the
system response using a triangle pulse input and implementing the nonlinear least-squares
method for system identification. During testing, SO pulses were fed into the valve at a rate
o f 500 steps per second and the direction of the stepping motor was switched at the 25
step spool displacement location. The output of the system, the displacement of the pitch
control piston, was sampled at a rate of 1000 Hz and recorded to determine the response
of the system to the triangle pulse input. The triangle pulse response of the system was
investigated in four different cases where the robot boom had different lengths and
different directions of motion. The test results are shown in Figures 4.2 (a) to 4.5 (a). In
these figures it can be seen that the noise had a significant influence on the test results. In
order to minimize the influence of the noise, the signals were processed through:
1) rejection of abnormal signals.
2) moving average.
A simple algorithm used for the rejection of the abnormal signals is:
if I A , - A , I > d ,
then let A, = A, .
Here d is a positive value which specifies the maximum difference of the magnitude of two
adjacent sample data. The value of d can be determined by inspection of the curves of the
sampled data points or based on the standard deviation of the noise which can be
determined fiom a statistics study of the noise signal. The former method was used by the
author of this thesis. A, and A, are magnitudes of the signals at adjacent sample time r ,
and t , . Figures 4.2 @) to 4.5 (b) show the responses of the system after the signals were
processed using the two steps stated above.
In order to estimate the values of the parameters of the system a software program
implementing the nonlinear least-squares method was developed. The code of this
program is presented in Appendix C. The processed data of the triangle pulse responses
were used as the input data to the program. The program runs of the iterative estimation
of the parameters for four different cases (different boom length and different piston
motion directions) are given in Tables 4.1 to 4.4. It can be seen from these program runs
that the iterative parameter estimations converge to the stable values shown in Table 4.5.
Each of these program runs took only a few seconds on an IBM-PC/DX2 66 MHz micro-
computer. Table 4.5 reveals that the values of the parameters vary not only when the
boom length of the robot change, but also when the direction of motion of the boom
changes. The following conclusions can be drawn from observing the results in Table 4.5:
(I) The flow gain has the most insignificant variation ( about 15%) of the four
cases compared to the variations in other parameters.
(2) The natural frequency of the system has a larger value, and the damping
coeficient has a smaller value, when the boom of the robot is lowered than
when it is raised,
(3) When the boom length increases, the natural frequency of the system increases,
but the damping coefficient has no significant change.
(4) The time constant of the valve increases when the boom length increases. The
time constant has a smaller value when the boom is lowered than when it is
raised.
Time (sec)
(a) The test curve of the triangle pulse response
Time (sec)
@) The curve o f the response after the sampling data were processed
using the moving average method.
Figure 4.2 The triangIe pulse response of the system when the robot
boom had a minimum length and was raised
Time (sec)
(a) The test curve of the triangle pulse response.
Time (sec)
(b) The curve of the response after the sampling data were processed
using the moving average method.
Figure 4.3 The triangle pulse response o f the system when the robot
boom had a minimum length and was lowered.
Time (sec)
(a) The test curve of the triangle pulse response.
Time (sec)
@) The curve of the response after the sampling data were processed
using the moving average method.
Figure 4.4 The triangle pulse response of the system when the robot
boom had a maximum length and was raised.
Time (sec)
(a) The test curve of the triangle pulse response
Time (sec)
@) The curve of the response after the sampling data were processed
using the moving average method.
Figure 4.5 The triangle pulse response of the system when the robot
boom had a maximum length and was lowered.
Table 4.1 Program run when the robot boom had a minimum length and was raised.
Sample Data File: puprninl .dat ***** Input Data (IDEN-DAT) *****
n npr nprl N dt a1 ad 500 10 1 10 0.001 15.20 0.0050
Initial Estimation: kvO td0 omega0 delta0
4-26 I 0.0290 33.0000 0.2100
***** Output of IDEN-CPP ***** Parameter Estimations:
kv td omega delta 4.26 1 0.0290 33 .OOOO 0.2 100 2.348 0.0269 34.582 1 0.2478 2.3 11 0.0268 34.8473 0.2352 2.307 0.0272 31.7642 0.2248 2.3 12 0.0270 35.00 19 0,2272 2.307 0.0272 34.7627 0.2 199 2.3 14 0.0270 35.070 1 0.2267 2.306 0.0273 34.7 lo8 0.2 179 2.3 15 0.0269 35.134 1 0.2279 2.304 0.0274 34.6287 0.2 159
Table 4.2 Program run when the robot boom had a minimum length and was lowered.
Sample Data File: pdmin M a t ***** Input Data (IDEN.DAT) *****
n npr nprl nr dt a 1 ad 500 10 1 10 0.001 20.26 0.0050
Initial Estimation: kv0 tdO omega0 del taO 4.261 0.0290 33.0000 0.2100
***** Output of IDEN-CPP ***** Parameter Estimations:
kv Ld omega 4-26 1 0.0290 33.0000 2-29 1 0.0222 35.8260 2.334 0.0104 46.7014 2.255 0.0 172 39.852 1 2.293 0.0212 41.83 10 2.302 0.0 165 44.1935 2.276 0.0 180 43.49 13 2.29 1 0.0 175 44.2870 2.276 0.0 178 43.7000 2.289 0.0 177 41.2 142
delta 0.2 100 0.3 137 0.54 14 0.048 1 0.1229 0.1892 0.1651 0.1749 0.163 1 0.1697
Table 4.3 Program run when the robot boom had a maximum length and was raised.
Sample Data File: pupmx 1 da t ***** Input Data (IDEN-DAT) *****
n npr nprl nr dl a1 ad 500 10 1 10 0.001 15.20 0.0050
Initial Estimation: kvO tdO omega0 delta0 4.26 1 0.0290 33.0000 0.2 100
***** Output of IDEN.CPP ***** Parameter Estimations:
kv t d omega 4.261 0.0290 33.0000 2.283 0.0358 29.3862 2.383 0.0407 28.5047 2.394 0.0380 29.1600 2.376 0.0395 28.1372 2.396 0.0381 29.1222 2.376 0.0394 28.4308 2.396 0.038 1 29.1177 2.376 0.0394 28.4275 2.396 0.038 1 29.1 189
delta 0.2 100 0.2042 0.1985 0.2288 0.2 1 18 0.233 I 0.2138 0.2338 0.2140 0.2339
Table 4.4 Program run when the robot boom had a maximum length and was lowered.
Sampling Data File: pdmas l .dat ***** Input Data (IDEN.DAT) *****
n npr nprl nr dt a1 ad 500 I0 1 10 0.001 20.26 0.0050
Initial Estimation: kvo tdO omega0 delta0 4.26 1 0.0290 33.0000 0.2 100
***** Output of IDEN-CPP ***** Parameter Estimations:
lw td omega delta 4.261 0.0290 33.0000 0.2 100 2.707 0.0248 36.0450 0.2770 2.647 0.0255 36.2505 0.2 122 2.656 0.0256 36.8023 0.2017 2.645 0.0260 36.4872 0.1857 2.658 0.0258 36.9 127 0.1907 2.645 0.026 1 36.5722 0.1822 2.656 0.0259 36.8995 0.1880 2.637 0.026 1 36.62 18 0.1822 2.656 0.0259 36.87 18 0,1870
Table 4.5 The results of parameter estimation.
K, r, W S
( cm3 I sec /step) (set) (radsec)
rnin. and raised 2.304 0 -0274 34.6 0.216
rnin. and lowered 2.289 0.0 177 44.2 0.170
max. and raised 2.396 0.038 1 29.1 0.234
max. and lowered 2.656 0.0259 36.9 0.187
5 DISCUSSION, CONCLUSIONS AND RECOMMENDATIONS
5.1 Discussion and Conclusions
During the recent past, work has been done on using computer controlled automatic
drilling rigs to replace the tedious and highly skilled manual operations of tunnelling in
adverse environments. Although the early research work and field prototype tests of these
automatic drilling rigs ( m d e in Japan and Norway) demonstrated improved dlilling
precision and cost d u c t i o n s in tunnel construction compared to manually operated
hydraulic drilling rigs, the performance (precision and speed to position the drilling boom)
of the early automatic drilling rigs needes improvement. On the other hand, the high initial
investment is still one of the major reasons that deter hydraulic robots from being widely
used in tunneling applications. Performance improvements and cost reductions of the
hydraulic drilling robot were. therefore, two of the important issues in the development of
a new hydraulic drilling rohot and were considered in this thesis.
In order to contribute to the develop the new hydraulic drilling robot, the studies
were carried out on three different hut related subjects. The first was the kinematics study
of an eight-degrees-of-freedom hydraulic drilling robot which has a tripod arrangement in
hydraulic cylinders to improve positioning stiffness of the robot manipulator. The second
was the implementation of an optimal tracking control algorithm to improve the tracking
precision and to increase the positioning s p d of a two-degrees-of-freedom experimental
hydraulic robot. The third was he design of the tracking control hydraulic system using a
low cost stepping motor driven proportional valve which incorporated a pressure
compensator to stabilize the flow gain of the valve and automatically compensate for load
disturbances. The design considerations, theoretical studies, and experimental test results
related to these three approaches which have been presented in this thesis will now be
reviewed and discussed in the following sections.
5.1.1 Manipulator Kinematics
The drilling boom, which has a tripod arrangement, improved the positioning stiffness
and stability under large Ioad disturbances in tunneling applications, compared to drilling
booms using other arrangements [4]. The kinematics studies on this type of manipulator
provided the foundation for further studies on the dynamics analysis and the optimal
tracking control of the robot manipulator.
Research on the manipulator kinematics dealt with the kinematics modeling of an
eight-degrees-of-freedom drilling robot with a tripod arrangement. The research focused
mainly on developing a method for solving the inverse kinematics problem and using a
graphic simulation method for verifLing the correctness of the solutions of this problem.
These same methods can potentially be used for real-time monitoring of the hydraulic
robot manipulator in a tunnelling operation.
Two parts of the kinematics problem, the direct and the inverse problem, were
discussed in Chapter 2. For the first part, the coordinate frame ( or coordinate system) and
the homogeneous transformation matrices were established to describe the positions and
orientations of the links of the robot manipulator in their motion space. For the second
part, the equations were established for the inverse kinematics problem. From the solution
of this inverse kinematics problem, a set of joint coordinates, corresponding to an arbitrary
given position and orientation of the drilling tool of the robot, could be found. In general,
the solution of the inverse kinematics problem is complex and not unique. Efforts were
made by the author to restrict the problem to one feasible solution by providing the
additional positioning constraint (confining the origin of frame 5 in the given plane which
is parallel with the work face) for this particular application. A combination of numeric
and analytical (algebraic) methods was developed for solving the inverse kinematics
problem. Graphic simulation results were used for verifLing the sotution (visualizing the
position of the drilling tool and checking the mechanical interference of the positioning
mechanisms) while the drilling robot boom was positioned to a set of joint coordinates
determined by the solutions to the inverse problem.
5.1.2 Optimal Tracking Control
The optimal tracking control algorithm was designed and experimentally implemented
to improve tracking precision and increase the positioning speed of the experimental
hydraulic robot manipulator. Because of the rapid development of computer technology
in the past decade, the cost of computers has decreased dramatically and as a result, more
sophisticated control algorithms, such as optimal control algorithm, can be implemented at
a fairly low cost today.
The design of the controller was based on a multiple-input fifth-order discrete state
space model and an optimal tracking control algorithm. The Kalman filter was designed
for observing the state variables of the digital control system and for providing the
feedback signals of all of the state variables. The control scheme can make the dynamic
system track the given trajectories of displacement, velocity, and acceleration over the
entire time interval by using the closed-loop control law.
The model of the stepping motor driven valve was an interesting problem because the
valve was made up of a combination of digital and analog components driven by a digital
control system. A tentative approach to modeling the stepping motor drive valve was
presented in Chapter 3. This approach is considered to be suitable for similar digitally
controlled valves whose flow rate is proportional to the number of input pulses.
For implementing the optimal tracking control algorithm, feedback signals of
displacement, velocity, and acceleration of the piston were needed. If the transducers
would have been used to generate these signals, the complicated wiring and
instrumentation would have made the system very costly and unreliable. The Kalman filter
was designed, therefore, to observe the state variables of the digital control system and to
provide all of the feedback signals which were needed by the optimal controller to
synthesiie the control signals.
Computer simulations were camed out to investigate the influences of parmeters o n
the performance of the tracking control system. The simulation results showed that the
flow gain of the valve had the most significant influence of the performance of the optimal
tracking control system. The other parameters (the time constant of the valve, the natural
frequency and the damping coefficient of the actuator) had relatively small influences. This
indicated that the successfL1 implementation of the optimal tracking control algorithm
depends mainly on the precise determination of the flow gain of the control valve.
5.1 -3 Hydraulic System and Experimental Testing
The optimal tracking control system was experimentalIy tested in a hydraulic circuit
using stepping motor driven valves incorporating pressure compensators. Since the
performance and the cost of the hydraulic robot are largely dependent on the hydraulic
system, especially on the control valves of the hydraulic system, a great deal of efforts was
put into the development of the new hydraulic valve and the rest of the system to meet
the requirements of this application.
The difficuhies involved in the design of the optimal tracking control system were
due to non-linearities, time varying parameters, and operational uncertainties in the
hydraulic robot. Of particular importance is that, the flow gain of the control valve varied
as the operating point varied. (The variation in the load pressure also caused the variation
in the flow gain.) These made it difficult for a conventional PID control system to achieve
good performance (small tracking error, fast response, and stability).
In this thesis then, a practical approach to the design of the control system of the
hydraulic robot was implemented. The hydraulic control system included a stepping motor
driven proportional valve incorporating a pressure compensator for stabilizing the flow
gain of the valve and automatically compensating for load disturbances which are
comprised of complex combination of inertial, centrifugal, Coriolis, and gravitational
effects in addition to fiction forces when the robot is in motion. In this research, the
feasibility of using such a stepping motor driven proportional valve to implement the
optimal tracking control system of the hydraulic robot was carried out. The tracking
control system with this valve arrangement is considered to be feasible because the
stepping motor driven valve is more rugged and less expensive than servo valves or
conventional andog proportional valves. As well, fairly good tracking control
performance of the system could be achieved with these types of valves.
Comparison between two optimal tracking control systems, one using a stepping
motor driven proportional valve incorporating a pressure compensator and the other using
a conventional analog proportional valve, was made though experimental testing. The
experimental test results showed that the pressure compensator built into the designed
valve was able to stabilize the flow gain of the valve and automatically compensate for
load disturbances so as to make the designed system insensitive to parameter variations.
Therefore, the parameter updating of the control system was eased and could be
implemented less frequently than a system using a conventional analog proportional valve
which necessitates on-line parameter updating and requires more sophisticated control
algorithms. The experimental test results also showed that the designed hydraulic system
and the optimal tracking control algorithm could be successfully used in the robot control
system for tracking the given trajectories of displacement, velocity. and acceleration.
The experimental test results showed that the designed hydraulic system and the
optimal tracking algorithm were feasible for this application.
5.1.4 Parameter Estimation
Since the optimal tracking control system relied on the knowledge of the parameters
values of the control system, parameter estimation using the triangle pulse input and the
nonlinear least squares method was implemented in Chapter 4 for determining these
values.
The triangle pulse response of the system was obtained using the convolution
integration method. and the parameter estimations of the tracking control system were
obtained using the non-linear least-squares method. This method is considered to be
feasible for this application because it is fast, it causes small disturbance, and it can
separately identify the parameters of the system when the piston moves in either direction.
The test results showed that this method could be successfully applied to the hydraulic
robot system.
5.2 Suggestions for Future Work
1) Although good tracking control performance of the hydraulic robot manipulator
was achieved, the tracking error of the experimental tests was larger than the tracking
error in the simulation results. The major reason for this difference is that the flow rate of
the proportional valve was nonlinear. In addition, there was a time delay in the flow
response of the valve. The nonlinearity of the valve (or the system) was not accounted for
in the optimal tracking control algorithm used in this study. The simulation results also
showed that the variation in the flow gain of the proportional valve had a relatively
si-cant effect on the performance of the tracking control system compared to other
parameters (damping coefficient, natural frequency. and time constant). The performance
of the optimal tracking control system could be improved if the nonlinearity problem of
the proportional valve was rectified. The nonlinear characteristic observed through
experimental testing was saturation. The characteristics of the proportional valve could be
improved in two ways. One is the hardware approach; that is, redesigning the shape of the
flow metering orifice and edarging the flow passages of the valve. Perhaps a more
convenient method is the software approach or the look-up-table method. Specifically, the
nonlinear relationship of the flow rate and the input to the valve could be calibrated and
the data stored in the memory of the computer. This information could then be used to
determine the desired output command signals to the digital controller to obtain the flow
rate equivalence of an ideal valve.
2) Using 3-way valves in the construction of the tracking control system has the
advantages discussed in Chapter 3; however, two valves are needed for each cylinder. An
alternative approach is to use a 4-way valve instead of the 3-way valve. If the flow
metering orifice of the 4-way valve was properly designed, the feature of pre-open and
delay closure of the pressure supply port of the valve could still be obtained. Although the
4-way valve is more difficult to manufacture than the 3-way valve, only one valve would
be needed to control each actuator. Therefore, the system could be simplified using this
type of valve.
3) Theoretically, adaptive control is superior to optimal control because the former
has the ability to perform on-line parameter identification and to automatically adjust the
controller to adapt to parameter variations due to changes in the operating point of the
system, or parameter variations due to unknown disturbances [16]. The latter does not
have such an ability. To successfully implement the optimal linear control system, the
parameters of the dynamic system would have to be known, and the parameter variations
from the values at the operating point would have to be small when the robot manipulator
shifted from one hole to another.
The complexity of on-line computations of an adaptive control system is much
greater than that of the optimal tracking control system, especially for a fifth order system
such as the robot manipulator control system. Most computations of the optimal tracking
control system were implemented off-line so that the on-line computation was much
simpler to implement than that used in adaptive control. An investigation should be
carried out to determine whether or not the system could be modeled by a lower order
system because the on-line computations of a frfth order system may be too complicated
to implement if an adaptive control algorithm is used.
For this application, where operating point of the tracking control system changes
from hole to hole, the convergence speed to determine dynamic system identification and
parameter estimation must be fast enough for the adaptive control algorithm to be
implemented successfully. More experimental testing is needed to investigate whether or
not the convergent speed is adequate, or whether an additionai dither signal must be
applied to the system to increase the convergence speed so that improved performance of
the adaptive control system can be achieved.
4) The experimental test results for system identification showed that time delay
elements do exit in the tracking control system. The computer simulation results showed
that the time delays reduce the precision of the tracking control system. A large time
delay may even result in an unstable system. It was also found that parameter variations
had a larger influence on the performance of the tracking control system in which the time
delay was present than for the ideal system which had no time delay. An investigation into.
and then, an elimination of the sources of time delay, or an improvement in the control
algorithm which takes the time delay element into account, would result in better
performance of the tracking control system.
5) Other topics, such as using computer control to optimize the hammer work and
using computer vision techniques to assist the drilling bit collaring on rock faces which
have ragged shapes, may also be important areas of research related to the development of
the new advanced rock drilling robot.
1 25
LIST OF REFERENCES
H.W. Richardson, R.S. Mayo, Practical Tunnel Driving, McGraw-Hill Book
Company, Inc. New York, 1941.
U. Langefors, B. Kihlstrom, The Modern Techniaue of Rock Blasting, John Wiley
& Sons, Inc., New York, 1963.
Tunnels and Tunnelling, Dec. 1979 pp. 5 and pp. 14.
E.V. Kirnber, A.L. Lundstrom, "Drill Boom Arrangement", U.S. Patent,
4232849, Nov., 1 1, 1980.
K. Dahi, "Computer Control Comes to Hard Rock Drilling", Tunnels & Tunnelling,
May, 1981, pp. 12-15.
Tohru Mashimo, "Fully Automatic Hydraulic Drilling Rig", Construction Machinery
(a Japanese journal), Dec., 198 1, pp. 49-55.
M.Vukobratovic, M.Kircanski, Kinematics and Trajectory Synthesis of
Manipulation Robots, Series : Scientific-Fundamentals of Robotics 3, Springer-
Verlag, 1986, pp. 138--155.
R. Manseur, K.L. Dity, "A Robot Manipulator with 16 Real Inverse Kinematic
Solution Sets", The International Journal of Robotics Research, Vol. 8, No. 5,
October 1989, pp. 75-78.
G. Zhong, P.N.Nikiforuk, P.R.Ukrainetz, "Kinematics of a Robot Manipulator - A
Hydraulic Drilling Boom", Proceedings of the First International Svmposium on
Fluid Power Transmission and Control, Beijing, P.R.China, Oct., 199 1, pp. 556-559.
J. Denavit, R.S. Hartenberg, "A Kinematic Notation for Lower-Pair Mechanisms
126
Based on Matrices", Journal of Applied Mechanics, June 1955, pp. 2 15-22 1.
[ 1 11 J.J. Craig, Introduction to Robotics, Addison-Wesley, 1986, pp. 100- 103.
El21 H.E. Memtt, Hydraulic Control Svsterns, John Wiley & Sons, New York, 1967.
[13] I.D. Landau, "Adaptive Control Techniques for Robotic Manipulators - The Status
of the Art", Proceedings of the 1st IFAC S-ymposium, Barcelona, Spain, 1985,
pp. 17-25.
[I41 S. Ananthakrishnan, R.M. Fullmer, "Experimental Adaptive Control of a Four-
Axis Hydraulicdly Actuated Robot", International Journal of Robotics and
Automation, Vol. 6, No. 4, 199 1, pp. 2 10-2 19.
[ 1 51 K.S. Narendra, A.M. Annaswamy, Stable Adaptive Svstem, Prentice Hall,
Englewood Cliffs, N. J. 1989.
S. Sastry, M. Bodson, Adaptive Control - Stability. Convergence. and Robustness,
Prentice Hall, Englewood Cliffs, N.J. 1989.
T.C. Hsia, "Adaptive Control of Robot Manipulator - a Review", Proceedings of
1986 E E E International Conference on Robotics and Automation, April, 1986,
Vol. 1, pp. 183- 1%.
G. Zhong, P.R. Ukrainetz, P.N. Nikiforuk, "Development of a Digitally Controlled
Proportional Valve", Proceedings of the Fifth Bath International Fluid Power
Workshop on Circuit. Component & S~sterns Design, Bath, UK, Sept., 1992,
pp. 2 1 1-224.
C.W. Chuen, J. Leahy, "Microprocessor Based Positional Control of a Mining
Boom Ripper", Proceedings I-Mech E, Aug. 1984, pp. 29-36.
[20] R.P. Paul, Robotic Manipulators: Mathematics. Proerarnming: and Control, MIT
Press, Cambridge, Mass., 198 1.
[2 11 G. Zhong, Computer Aided Design of a Di~itallv Controlled Proportional VaIve,
Thesis (M.Sc.), University of Saskatchewan, 1992.
C.L. Phillips, H.T. Nagle, Digital Control Svstem Analysis and Design, Prentice
Hall, Englewood Cliffs, N. J. 1990.
F.L. Lewis, Optimal Control, John Wiley & Sons, New York, 1986.
R.E. Kalman, "A New Approach to Linear Filter and Prediction Problems," Trans.
ASME, Series D, Vol. 82, pp. 35-45, 1960.
R.E. Kalman, R.S. Bucy, "New Results in Linear Filter and Predictions", Trans.
ASME, Series D, Vol. 83, pp. 95-108, 1961.
J.S. Meditch, Stochastic Optimal Linear Estimation and Control, McGraw-Hill Book
Company, New York, 1969.
M. Noton, Modem Control Theory, Pergarnon Press Inc. New York, 1972.
Edited by Nanjing Aeronautic Institute, Northwest Industrial University, and Beijing
Aeronautic Institute, Principles of Automatic Control, National Defense Industrial
Publisher, 1982. (Chinese Version)
Han Zhenj in, Ada~tive Control Svstems, Mechanical Industrial Publisher, 1983.
(Chinese Version)
G. Zhong, P.R.Ukrainetz, P.N.Nikiforuk, "Optimal Tracking Control of a Hydraulic
Robot", Proceedings of Fluid Power West, Anaheim, USA, March 1994.
W.D.T. Davies, Svstem Identification for Self-Adaptive Control, Wiley-Interscience,
a division of John Wiley & Sons Ltd. New York, 197 1 .
[32] Xu Ningshou, System Identification, Mechanical Industrial Publisher, (Chinese
version,) 1986.
APPENDIX A DEWATION OF TRANSFORMATION MATRICES
In Chapter 2 of this thesis a set of coordinate fiames was established by the author to
describe the position and orientation of the drill boom in the work space, and the 4 by 4
homogeneous transformation matrices used for describing the relationships between the
coordinate fiames were also presented. These transformation matrices are now derived in
more detail as follows:
As can be seen from Figure 2.1, the homogeneous transformation relating fi-arne { 1 )
and h e ( 0 ) is determined by rotating frame (1) about 2, axis by A , . Thus the
transformation matrix 1;, is given by
The homogeneous transformation relating h e ( 1 ) and frame {2) is determined by
performing the following successive transformations: frame (2) first is translated along the
X, axis by LX, , then rotated about the X, axis by 90a, finally rotated about the 2, axis
by A2 . Thus the overall transformation T,? is given by
& = Tmns(XI, U 1 ) R O T ( X l , 90°)ROT(ZLA2) =
where
The determinations of the transformation matrices 27, , T,, , T,, , T,, , T,, , T,, , T,, ,
T,, , T,, and T, are straight forward very much like the procedures for determining T,,
and q2, and, therefore, the description of the derivations is omitted here. The results are
given as follows:
T,, = ROT(Z,, A,) = 1; 2 y ;I
where
ROT(Z,, A,) =
where
Trans(X, , LX, ) =
where
T,, = ROT(X, , A , ) =
where
where
B ~ O ~ = ROT(ZB,Aw)ROT(YB, A m ) R O T ( Z B , A m ) S
i cos(A,) -sin(A,) 0
ROT(Z , . A,) = sin(A,) COS(A,) 0
0 0 1 1
where (A8 = ASCY, A9 = AsM for simplivng the writing of the matrices)
APPENDIX B CODE OF SIMULATION PROGRAM FOR OPTIMAL TRACKING CONTROL
The simulation program for optimal tracking control was coded in FORTRAN
language. The simulation runs were carried out on a VAXNMS computer system of the
College of Engineering. A math library, called IMSL and installed on the computer
system, was used to facilitate the programming. The following external calls to the
hnctions in this library are illustrated below:
CRGRG:
MURRV:
MRRRR:
MXTYF:
TRNRR:
LINRG:
Copy a real general matrix.
Multiply a real rectangular matrix by a vector.
Multiply two real rectangular matrices, A*B.
Multiply the transpose of matrix A by matrix B, trans(A)*B.
Transpose a rectangular matrix.
Invert a matrix.
The computation steps of the simulation program are illustrated as follows:
I. Read program execution data.
2. Read parameters of the control system.
3. Read the given trajectories of displacement, velocity, and acceleration.
4. Determine the coefficients of matrices A, B, and C. (refer to Equations (3.35) and
(3 -36))
5. Off-line computations to determine time sequences: K, , Ki, S, , and V,
where k = 1, 2 , 3 .. .N- 1. (refer to Equations (3.3 8) to (3.4 1 ))
6. Off-line computation to determine sequence K(k + 1) of the Kalman filter.
(refer to Equation (3 -46))
7. Read the actual parameters of the control system.
8. Start on-line computation loop.
9. Determine control signal u. (refer to Equation (3 -42))
10. Simulation to determine the state variables (X, ) of the control system
1 1. Measure the state variable X1(3), i.e. the displacement of the piston.
12. Estimate the other state variables using the Kaiman filter.
13. k=k+ 1 ; if k < N- 1, go to step 8; otherwise print the computation results.
The code of the simulation program for the optimal tracking control system is listed
as follows for reference:
C PROGRAM SIMU3 .FOR C OPTIMAL CONTROL OF A ROBOT MANIPULATOR C USING ESTIMATED STATE VARIABLE FEEDBACK C INPUT DATA : SIMU3.DAT C OUTPUT DATA : SIMU30.DAT C N=stage number, N2=output interval count NP==printing interval count C N3=NS=(steps/sec)/(per step)
PARAMETER (N 1 = 100 1) INTEGER N,N2,NP,NS,MM 1 ,I,J REAL Z(5,5),Z 1(5,5),Z2(5,5),COEF( 10) REAL T,TD,OMEGA,DELTkKVA,A 1 ,A,B,N3 REAL TAC,TDAC,OMEGAAC.DELTAAC,AlAC,KVAAC REAL X1(5),X2(5),X(5,Nl),y(3),Y I(Nl),R(3),XD(3,Nl),U,U l(Nl),U2 REAL AD(5,5),BD(5, 1),ADA(S75),BDA(S, l),C(3.5),C l(l,5),B 1(5,5) REAL P(3,3),Q(3,3),RW,S(5,5),S 1(5,5),W5),V25,N 1) REAL K(1,5),KV(1,5),Kl(l75,Nl),KV1(1,5,IUl) REAL T1(1,5),T2(1,1),T3,T4(1,5),T5(5,5),T6(5,5),T7(5,5),T8(5,5) REAL T9(5,3),T 10(5,5),T11(5.5),TL2(5,S),T13(5),T14(5,3),T15(5) REAL T16(1),T17(1),T18(5),T19(5) REAL XE 1(5),XE2(5),XE3(5,Nl) REAL KE(S,l),KE l(S,Nl),P l(5,S),P2(5,5),Rl ,RZG(5,1) REAL T20(5,1),T2 1(1,5),T22(1,1),T23,T24(5,5),T25(5,5),T26(5.5) REAL T27(5,5),T28(5,5),T29(5,5),T30(5),T3 1(5),T32(1.5) REAL T33(1),T3J(l,l).T35.RIX,R2X.P IX
EXTERNAL CRGRG.MURRV,MRRRRTRNRRLR\CRG-MXTYF,POLRG DATA FMT44/'(3F12.5.1F12.3)'1
DO 1 1=1,3 DO 2 .J=1,3 P(I.JI=O. Q(I,JI=o- CONTINUE CON-lxwE
OPEN(1 ,FILE='SIMU3 .DAT,STATUS='OLD') READ( 1 ,*) N2,NP,T,TD,OMEGA,DELTAAl ,KVA READ( I,*) P( l,l),P(2,2),P(3,3) READ( 1 ,*) Q(l,l),Q(2,2),4(3,3) R W is weighted factor for control signal u READ(I,*) RW READ(I,*) RlX,R2XPlX CLOSE( 1) Input the data of the given path OPEN(2,FILE='WAVE. D AT, STATUS='OLD') READ(2,*) N DO 3 I=l.N READ(&*) XD( l,I),XD(2,1),XD(3,I) CONTINUE CLOSE(2)
T= SAMPLING TIME, TD= DELAY TIME OF THE VALVE DELTA= DAMPING COEFFICIENT, OMEG= HYDRAULIC FREQUENCY A= i ./TD B=2.*DELTA*OMEGA DO 22 I=1,5 DO 23 .l=1.5 Z(I.J)=O. CONTINUE CON-mWE Z(1,2)=1. Z(2,2)=- 1 .m Z(3,4)= 1. Z(4.5)= 1. Z(5,l )=OMEGA*OMEGA/A 1 Z(5,4)=-OMEGA*OMEGA Z(5.5)~-2. *DELTA*OMEGA COEF( 1 )=I. COEF(2)=T COEF(3 )=T*T/2. COEF(4)=T**3/(3. *2.) COEF(5)=T**4/(4.*3. *2.) COEF(6)=T**5/(5. *4. *3. *2.) COEF(7)=T**6/(6,*5.*4.*3 .*2.) COEF(8)=T**7/(7.*6.*5.*4.*3.*2.) COEF(9)=T**8/(8.*7.*6.*5.*4.*3.*2.) COEF(lO)=T**9/(9.*8.*7.*6.*5.*4.*3.*2.)
CALL POLRG(5.2.5, lO.COEF,Z 13) DO 26 I=1,5 DO 27 J=1,5 ~ ( L J l = Z 1 (Ln c o m CONTrNuE
CALL POLRG(S,Z,S, lO,COEF,Z2,5) BD(l,1)=22(1,2)*KVA~TD BD(2,1)=22(2,2)*KVA/TD BD(3, I)=Z2(3,2)*KVA/TD BD(1,1)=22(3,2)*KVA/TD BD(5.1)=22(5,2)*KVA/TD DO 3 1 I=1,5 =(***I (AW,J)* J = W CONTINUE DO 32 I=1,3 DO 33 I=1,5 c(I,l)=o. c0NTrNu-E CONTINUE C( 1.3)= I. C(2,4)= I. C(3,5)= 1.
CP=CA(T)*P CALL MXTYF(3,5,C,3, 3,3.P13, 5,3,CP15) S 1 (N)=CP*C CALL MRRRR(S13,CP,5, 3,5.C13, 5,5,S1,5)
CP=CA(l-)*P CALL MXTYF(3,5,C.3, 3,3,P,3, 5,3,CP.5) V2(N)=CP*R(N) R(I) (I=1..3) is reference input vector DO 13 I=1,3 R(T)=XW,N) CONTINUE CALL MURRV(5.3 ,CP,5,3,R 1,5,V2) DO 15 I=1,5 V(1, N)=V2 (I) CONTINUE
* OFF-LR\IE COMPUTATION OF K, KV, S, V * ****************************************
DO 10 M=l, N-l Renew R(1) reference input vector DO 5 I=1,3 R(I)=XD(I, N-M) CONTINUE ****************************************************************** K(K)=(BDA(T) *S(K+ I ) *BD+RW)"(- 1 )*BDA(T)*Sw+l)*AD. S(N)=CA(T))*P*C TI=BDA(T)*Sl CALL MXTYF(S,l,BD.S, 5,5,S1,5, 1,5,Tl,l) T2=Tl *BD CALL MRRRR(l,5,Tl,I. 5,1,BD,5, 1,1,T2,1) RW is weighted factor for control signal u when R W increases, the optimal control signal decreases, T3= 1 ./(T2( l,l)+RW) T4=BDA(T)*S 1 *AD=Tl *AD CALL MRRRR(1,5,Tl,l, 5,5,AD,5, 1,5,T4,l) K(1. I)=TJ(l, l)*T3 K(l,2)=T4(l92)*T3 K(1,3)=T4(193)*T3 K(1,4)=T4(1,4)*T3 K(1,5)=TS( 1,5)*T3 ***************************************** KV(K)=(BDA(T)*S(K+ 1)*BD+RW6(- i )*BDA(T) KV=T3 *BDA(T) DO 90 I=1,5 KV(l,I)=T3*BD(l, 1) COI'rmwE ******************************************* S(K)=ADA(T)*S(K+l)*(AD-BD*K(K))+CA(T)*Q*C T5=ADA(T)*S I CALL MXTYF(5,5,AD,S, 5,S7S1,5.5,5,T5,5) TG=BD*K CALL MRRRR(5,l ,BD.5, 1.5.EC. 1.5.5 ,T6,5) T7=AD-T6 DO 20 1=1,5 DO 30 J=1,5 mI,r)=AD(~,s)-T6(I,J) c o r n CONTINUE T8=T5*T7 CALL MRRRR(S,S,T5,5, 5,5,T7,5, 5,5,T8,5) T9=CA(T)*Q CALL MXTYF(3,5,C,3,3,3,Q,3,5,3,T9,5) T lO=T9*C
CALL MRRRR(5,3,T9,5,3,5,C,3,5.5,TlO75) S=T8+T 10 DO 40 I=1,5 DO 50 J= 1.5 S(I,I)=T8(I,J)+T lO(1.J') C O N T r n
CONTINUE ************************************************************* V(K)=(AD-BD*K(K))"(T)*V(K+I)+CA(T)*Q*R(EQ V(N)=CA(T)*P*R(N) T1 l=BD*K CALL MRRRR(5,1,BD,5,1,5,Yl,5,5,T11,5) T 12=AD-T 1 1 DO 60 I=1,5 DO 70 J=1,5 T 120, J)=AD(I,J)-T 1 l(1J COI'JTmuE CONTINUE T 12T=T 1293') CALL TRNRR(5,5,T12,5, 5,5,T12T,5) T13=T12*(T)*V2 CALL MURRV(5,5,T12,5, 5,V2, 2, 5,T13) T14=CA(T)*Q CALL MXTYF(3,5.C.3, 3,3,Q,3, 5,3.T14,5) TlS=TlJ*R CALL MURRV(5,3,T14,5,3,R 1,5,T15) Vl=T13+TlS DO 80 I=1,5 V 1 O=T 13 (I)+T l5(I) CONTINUE
COPY S TO S1 CALL CRGRG(S,S,S, S1,5) COPY v1 TO v2 DO 95 1=1,5 V2(r)=Vl(I) CONTINUE STORE K, KV. AND V for On Line Control DO 110 I=1,5 Kl(1,I.N-M)=K(l,I) KVl(l,I,N-M)=KV(l ,I) V(1,N-M)=V 1 (I) CONTINUE CONTINUE
******************************************* * KALMAN FILTER OFF-LINE COMPUTATION * ******************************************* DO 232 1=1,5 G(I,1)=0. C l(l,I)=O. CONTINUE G(l,L)=l. C1(1,3)=1. Rl=RIX R2=R2X
CONTINUE
KE=P l*C 1 "T*(C 1 *P l*C lAT+R2)"(-1) CALL MXI(TF(5,5,P1,5,1,5.C1,1.5,1.T20,5) CALL MRRRR(1,5,CI,1,5,5~1,5,1,5,T21,1) CALL MXYTF(I,5,T21.1,1,5,Cl,l.1,1,T22,1) T23=l ./(T22(1,1)+R2) DO 240 1=1,5 KE(I,l)=T20(I, 1)*T23 KE 1 ( I , W = W I , 1) c o r n
P2=P1-P1*ClAT*(Cl*P1*C1AT+R2)"(-1)*C1*Pl=P1-KE*C1*P1 CALL MRRRR(S,l,KE,5,1,5,C1,1,5,5,T24,5) CALL -(5,5,T23,5,5,5,P1,5,5,5,T25,5) DO 260 J= 1,s DO 250 I=1,5 P2(I,J)=P l(1,J)-T25(I,J) CONTINUE CONTINUE
P 1=AD*P2*ADAT+G*Rl*WT CALL MRRRR(5,5,AD,5,5,5,P2,5,5,5,T26,5) CALL MXYTF(5,5,T26,5,5,5,AD,5,5,5,T27,5) CALL MXYTF(5,1,G,5,5,1,G,5,S75,T28,5) DO 265 I=1,5 DO 266 J=1,5 T29(T,J)=T28(1,J)*Rl CONTINUE c0NTrNu-E
DO 270 I= 1,5 DO 280 J= 1,5 P 1 (I,J)=T27(I,J)+T29(I,J) COhlTRKE CONTINUE CONTrNuE
*****************************************a**
* ON-LINE COMPUTATION DETERMINING U * .......................................... U(K)=K(K)*XE(K)+KV(K)*V(K+l) OR U=-K*XE l+KV*V
DO 3 10 I=1,5 xE10=0. Xl(I)=o. X(I*l)=Xl(I) CONTINUE u=O. M1=0 u24.
N3=U/(N2*T*U)= (STEPS/SEC)/(PER STEP) NS=INT((l ./(T*FLOAT(N2))* 10.+5.)/10.) N3=FLOAT(NS)
*********************************************** READ OLD PARAMETERS (ACTUAL PARAMETERS) OPEN(1 ,FILE='ADBD.DAT,STATUS='OLD') READ(l,'(A)') '* AD(5.5)' DO 23 1 I=1,5 READ( 1 ,*) (ADA(I,.J).J= 1 3 ) c o m READ( 1 ,'(A)') '* BD(5,l)' READ( I,*) (BDA(1, 1),I= 1.5) READ(1 ,'(A)') ' T, TD, OMEGA. DELTA, A 1, KVA' READ(1,200) TAC,TDAC.OMEGAAC,DELTAAC,A 1 AC,KVAAC CLOSE( 1)
DO 120 M=I,N-1 Tl6=K*XE 1 DO 130 I=1.5 W,O=KW,I ,W c o r n CALL MURRV(l,S,K.l, 5,XE1, 1, 1,T16) DO 140 I=1,5 KV( l,f)=KVl (I ,I,M) CONTINLTE DO 150 I=1,5 Vl(I)=V(I,M+l) CONTINUE
T17=KV*V1 CALL MURRV(l,S,KV,l. 5.V1, 1, 1,T17)
................................ * SIMULATION DETERh4NING X * ................................ X2=AD*XI+BD*U T18=AD*X1 YlCM)=X1(3) CALL MURRV(5,5,ADA,S, 5,X1, 1, 5.T18) T19=BD*U DO 160 I=1,5 T 19(I)=BDA(I, l)*U c o r n DO 170 I=1.5 E ( I ) = T 18(I)+T l9(I) CONTrNUE DO 180 I=1.5 X(I.M+ 1 )=XZ(I)
* KALMAN ESTTMATOR ON-LINE COMPUTATION DETARMINING XE * *****************************************************************
CALL MURRV(5,S,AD,5,5,XE19 1,5,T30) DO 3 11 I=1,5 T3 l(I)=BD(I, I)*UlO CONTINUE CALL MRRRR(I,S,C1,1,5,5,AD,5,1,5,T32,1) CALL MURRV(1,5,T32,l75.XE1, 1, 1.T33) CALL MRRRR(l,5,CI,1,5,l.BD,5.1,1.T34,1) T35=T34( 1, l)*U 1 0 DO 320 I=1,5 XE2(I)=T30(I)+T3 1 (I)+KE 1 (I,M)*(Y I(M)-T3 3( 1)-T35) XE3 (I,M)=XE 1(r) XE 1 (x)=XE2(1) c o m CONTMUE
************ OUTPUT *
************ OPEN(9,FILE='SIMU3O.DAT,STATUS='NE W')
WRITE(9,'(A)') I* OUTPUT DATA OF SIMU3 .FOR1 W - m 9 , * )
WRITE(9,'(A)') ' T TD OMEGA DELTA KVA' WRITE(9,200) T,TD,OMEGA.DELTA,A I ,KVA FORMAT(lX, 6F10.3) -(9,*) WRITE(g,'(A)') ' P(1,l) P(2,2) P(3,3) I WTE(9.205) P( Ll), P(2,2), P(3,3) -(97*) WTE(9,'(Nt) ' Q( 1,l) QW) Q(3,3 ' -(9,205) Q( 1-1 ),Q(2,2).4(3,3) -(9, *) W-(g,'(A)') ' RW N N2 NS' WlUTE(9,206) RW,N,N2,NS -(9,*) FORMAT(IX,2E12.1.F12.1) FORMAT(lXFlO.l,3IlO)
WRITE(9,220) (X(I,I), 1=3,5) 210 CONTINUE 220 FORMAT(IX.15, F8.0,2F10.2, 3F10.1)
-(9**) WRITE(g,'(A)') '* AD(5,5)' =(9,*) DO 237 1=1,5 -(9,*) (AW,JlT),J=1,5)
237 CONTINUE -(9,*) NWTE(9,'(A)') '* BD(5,l)' WRITE(9,*) (BW, 1),1=1,5) W ( 9 ? * ) WRITE(9,'(A)') '* ACTUAL DATA : ' WRITE(g,'(A)') ' T TD OMEGA DELTA A1
# KVA' WRITE(9,200) TAC,TD AC,OMEG AAC,DELTMC, A 1 AC. K V M C r n ( 9 , * ) WRITE(g,'(A)') '* ADA(5,5)' -(9**) DO 233 1=1.5 WRI.E(9,*) (ADA(I,J), J= 1.5)
233 CONTINUE -(9,*) WRITE(g,'(A)') '* BD(3,I)' WRITE(9,*) (BDA(1, 1),I= 1,s) r n ( 9 . * ) CLOSE(9)
m ( 2 , *) WRITE(2,'(A)') ' T TD OMEGA DELTA A1 KVA' WRITE(2,200) T,TD,OMEG&DELTA, A 1 .KVA CLOSE(2) STOP END
APPENDIX C CODE OF THE PROGRAM FOR PARAMETER ESTIMATION
The program for parameter estimation of the tracking control system was coded in
Borland C+t- The parameters of the control system to be estimated were the flow gain
( Kv ) and the time constant ( T, ) of the proportional valve, and the natural frequency ( o )
and damping coefficient (6) of the actuator. The variable names of the program for these
parameters are: kv = K,, , td = T, , omega = o , and delta = 6. The computation steps of
the program are illustrated as follows:
1. Read program execution parameters.
2. Read parameters of the control system.
3. Read and print initial estimated values of the system parameters.
4. Generate a triangle pulse.
5. Read sampling data obtained from experimental testing.
6. Determine time sequences of the partial derivatives. The variables of the propram
are ilIustrated as follows: (refer to Equation (4.10))
T;kO T k O , fdelta[i] = - b [ i ] = 6, fid[i] = - gk 0 , and fomega[i] = -
X V fl" a
where i and k = 1, 2, 3...n.
7. Determine coefficients of Equation 4.19:
a[i, j] = L(i, j) where i and j = 1, 2, 3, 4.
8. Computing impulse response of the system, using convoIution integration method.
(refer to Equation (4.4))
9. Determine the variables of Equation (4.19):
b[i] = v, where I = 1, 2, 3, 4.
10. Solve Equation (4.19) and obtain A, where i = 1, 2, 3, 4.
1 I. Compute and print the new estimated values of the system parameters.
(refer to Equation (4.1 7))
12. m=m+ 1 ; if m < nr, go to step 7; otherwise print the computation results.
The code of the program is listed as follows:
11 PROGRAM IDEN-CPP for system identification and parameter estimation I1 estimated parameters: kv, td,omega.del ta #include <stdio. h> #include <fistream. h> #include <strstrea. h> #include <iomanip. h> #include <string. h> #include <math. h> #include " ftoc. h"
const int n 1 =5OO; float kv,t~omeg~del~etdedo,4dl.st&fdphase.a l.dty[n 1 1 ; e~qern "C" void idensub(float*a. int coln, int n, float*b,
int* pivot, int *info); void resp(int nf, int nXfloat dtf.,float kvffloat tdf,float omegaf, float deltafifloat a If); void prockv(int nif, int nf); void proctd(int nif,int nf); void procomega(int nif,int nf); void procdelta(int nif,int nf);
void main(void) (
int ij,kkl.n,npr,nprl,~,m,nO,ni,np.ndt; float t,gain; float yl[nl].yO[n1].fl50] ; float a[4][4],b[4I7x[4],ad; float term 1 ,term2,tenn3,kp ; float kvO,tdO,ornegaO,deltaO,iwa,tda,omegaa,dela ; float £kv[n 1 ],ftd[n l],fomega[nl],fdelta[nl] ; float kvs[30],tds[30],omegas[30],deItas[30];
ifstream is("iden.datl*); I1 fill input buffer
ip> n>>npr>>npr l>>nr>>dt>>a 1 >>ad:
// read actual parameters of the system iY>kva>> tda>>ornega.a>>deItaa;
/I read and print initially estimated values of the parameters i~>kvO>>tdO>>ornegaO>>deltaO; printf("%f %f O/of Y&", kYO,tdO,ornegaO,dellaO );
// generating a triangle pulse f(i) : /I pulse width=np*ndt*dt
is>>np>>ndt>>gain; for (i=O;i<np;i++)( iU>f[i]; 1 is.close();
for ( i 4 ; icnp; i*) { f( i )qi] *gain; 1
N read the sample data ifstream is 1 ("testa-dat");
/I is.getline(string,S 1); //fill input buffer
for (i=O;i<n;ite){ is 1 >>y 1 [i]; 1 is 1 .close();
I1 repeative estimations for (m==O;m<nr,m*) {
std=sqrt( 1. delta*del ta) ; fd=omega*std ; d=omega*omega-2.*delta*omega/td+ I ./(td*td); dl= 1 ./tddelta*omega ; $(dl = 0.) ( phase=atan(std/(delta))+3. I4 15912.;
1 else ( phase=atan(std/(-delta))+atan(fd/d 1 );
1
/I determine the time sequence of partid derivatives: N fkv[i], Ad[i], fomega[i] and fdela[i], i=0,1,2 ..A.
prockv(ni,n) ; for (i=ni; i<n; i*) (
fwi l=~[ i l *fill ; I
procomega(ni,n); for (i=ni;i<n;i++) {
fomega[i]y[i]*fi 1 1; 1
procdelta(ni,n); for (i=ni;i<n;i++) {
fdelta[i]=y[i]*fl I]; 1
proctd(ni,n); for (i=ni; i<n;itt) (
fid[i]=ftd[i]+y[i] *fCII; 1
procomega(ni,n); for(i=ni;i<n;i*) (
fomega [i] =fomega [i] +y [i] *flj] ; I
procdel ta(ni,n); for (i=ni;i<n;i++) ( fdelta[i]=fdelta[i]+y[i] *fli] ;
1
I/ determine a(i j)= a (i j ) refere to Eq. (4.13a)
for (i=O;i<n;i++) ( b[O]=fkv[i]*(yl [i]-yO[i])+b[O]; b[l]4d[i]*0fl[i]-yO[i])+b[l]; b[2 ]=fomega(i]*(y 1 [i]-yO[i])+b[2]; b[3]=fidelta[i] *(y I [i]-yO[i])+b[3];
1
int pivot [4], info; for(i=O;i4;i++) { x[i]=b[i];
1
N calling e.xtemal subroutine to solve linear equations (4.14) and (1.19), N returning results in x [i]= A [I].
idensub(&a[O] [O], J,J,.upivot&info);
N improve the initial estimation. refere to Eq. (4.17) kv=lw+x[O J; td=td+x[ 11; omega=omega+x [2] ; delta=delta+x[3]; k= k+ 1 ; if(lc=nprl) ( printf("%f %f %f %f \nW. kv,td,omega,delta ); kl=kl+l ; h s [ k l]=kv; tds[k 1 j=td; omegas F 1 ]=omega; deltasF1 ]=delta; k=O;
1
/I output the computation data FILE *out; o~Hopen(~resp.dat" , "w"); fprintf(out, "***** Input Data (IDEN-DAT) *****\nW); Qrintf(out," n npr nprl N dt a1 ad \nW); fprintf(out,"% %d %d O/od %lO.3f %lO.3f %lO.4fW'. n, npr. nprl, N. dt, al. ad ); fprind(out, "Initial Estimation:\n "); fprintf(ouf " kvO tdO omega0 delta0 \n "); fprintfi(out,"%12.4f %l2.4f %l2.Qf %12.4fWU, kvO,tdO,ornegaO,deltaO ); fprintf(out. "***** Output of IDEN.FOR *****\nV); Qrintfiiout, "Parameter Estirnations:\n"); fprintf(out, " n kv td omega delta h"); i 4 ; fprintf(out, "O/od % l U f %l2.4f %l2.4f %l2.lf\n", i,kvO,tdO,omegaO,deltaO ); for (i=l:i<kl;i*) { fprintf(ouf "%d %l2.Jf %l2.Jf %l2.-lf %t2.UW1. i,kvs[i],tds[i~,omegas[i].deltas[i]);
1
fpfintf(out, "\n\n"); fprintfiouf "***** Output Data of RESP *****hM); @rintf(out, " t yO(t) y l(t)\nW ); for(i= 1 ;icn;i+=npr) {
t=(i- 1 )*dt; fprintf(ou~"%l0.5f %lOSf %lOSf\n", t@[i],yl[iJ);
1 fclose(out);
1
11 subroutine RESP for determining the impulse response of the system, // using convolution method, results in y[i].
void resp(int nf,int nif, float dtf,float kvf, float tdf, float omegaf. float deltaf,float a lf) { int i ; fIoat tf, phase; float tml,tm2,tm3,tm4,~,
tm 1 =Wornegaf*omegaEl(a 1Ptd.f); tm2=tdE'(omegaf*omegaf); -rnegafYomegaf-2. *deltaf*omegafkE+ 1 ./(tdf*tdf) ; if ((l./tdf-deltaf*ornegaf) = 0.) { phase--atan(sqrt( 1 .deltaf*delta£)/(-deltat))+3.14 lS9/2. : ) else ( phase=atan(sqrt( 1 .deltaf*deltaf)/(-deItaf))+ a tan(omegaf*sqrt( 1. d e l tafcdeltaf)/( 1 ./tdfdeltaPomegaf)) ; 1
for (i=nif,icnf;itt) ( tHoat(i-nif)*dtf; tm3=tdf*e.~p(-Wtdf)/df; tm4=e.q(-deltaf*omegaf*tf)*sin(omegaftsqrt(1 .deltaf*deltaf)*tf-phase)/ (omegaf*omegaf*sqrt(l .deltaf*deltaf)*sqrt(df)); y [i]=trn 1 *(trn2-tm3+tm4);
1 return ;
1
I/ subroutine determining time sequence kv, results in y[i]. void prockv(int nif, int nf) {
int i; float etd 1 ,cdo 1 ,kv,t 1 ; for(i=nif-,icnf;i++) {
t l+oat(i-nif)*dt ; etd l=exp(- 1 Ad* t 1); d o l=exp(delta*omega*t 1); y[i]=omega*omega/(a 1 *td)*(td/(omega*omega)-td*etd l/d+ edo 1 *sin(fd*t 1 -phase)/(ornega* fd*sqrt(d))) ;
1 return;
1
I1 subroutine determining time sequence td. results in y[i]. void proctd(int nif,int nf) ( int i; float etdI,edol,tl; float teml,term2,tem3,phtdomega2; for (i-rllr,i<nf;i++) {
t 1 =float(i-nif)*dt; etdl=exp(-l./td*t 1); omega2wmega*omega; I/ or pow(omega,2.); edo I =exp(delta*omega* t 1 ); terml=-kv*omega2/(td*td*al)*(td/(ornega2)- td*etd l/d+edo 1 *sin(fd* t 1 -phase)/(omega*fd*sqrt(d))); term2= 1 ./(omega2)-((etd I +etd 1 *t l/td)*d- etd 1 *(2. *delra*omega/td-2./(td*td)))/(d*d); phtd=(fdl(td* td))/d ; term3=(edo 1 *cos(fd*t 1-phase)*(-phtd)*d-edo 1 *sin(fd*t I-phase)* (1 ./(td*td)*delta*omega- 1 ./pow(td,3. )))/(omega* fd*pow(d, 1.5)); y[i]=terml+h*omega2/(a 1 *td)*(term2+te&);
1 return;
1
I/ subroutine determining time sequence omega, results in y[i]. void procomega(int nif,int nf) { int i; float etd1,edo 1 ,t I; float term 1 ,term2,term3,tm3 l,trn32,trn33,phomega,omega2; omega2=omega*omega; for (i=nif-,i<nf;i+t) {
t 1 =float(i-nif)*dt; etdl=exp(-1 ./td*t I); edo 1 =e.xp(delta*omega*t I ); term 1 =2. *kv*omega/(a 1 *td)*(td/omega2-td*etd lldi-edo 1 *sin(fd*t 1 -phase) /(omega* fd*sqrt(d))); term2=2.*td/pow(omega,3. )+td*etd I *(2. *omega-2. *deltaftd)/(d*d): phomega=(stdltd)/d;
tm3 l=edol*(delta*t 1) ; tm32=(tm3 l+edo I *cos(fd*t I -phase)*(std*t 1 -phomega))*omega2*d-edo 1 * sin(fd*t 1 -phase)*(2. *omega*d+omega2*(omegadelta/td)); tm33=(std*pow(ome~4)*pow(d, 1 -5)); term3 =lm3 2/tm3 3; y[i]=tenn l+kv*omega2/(a 1 *td)*(term2+term3);
I return;
1
/I subroutine determining time sequence delta, results in y[i]. void procdelta(int nif,int nf) ( int i; float etd I , d o I ,t 1; float term 1 ,tcrm2,term2 1.tcnn22,tm2 1.tm22: float ddeIta.phdelta.omega2.della2:
ornega2=omega*omega; delta2=delta*delta; for (i=nif; i<&,i++) (
t l=float(i-nif)*dt ; etd l=exp(- 1 ./td*t 1); edo 1 =exp(-delta*omega* t 1); ddelta=(-delta*omega2+3. *delta2*omega/td-delta/(td* td)+mega/td) /(std*wt(d)); phdeltadel W/std+std+((-del ta*omega/td+delta2 *ome@)/std+ omega* fd)/d; term 1 =td*etd 1 *(-2. *omega/td)/(d*d); tm2 1=edo l*(*mega*tI)*sin(fd*tl-phase); term2 1 =((tm2 l+edo 1 *cos(fd*t 1 -phase)*(deIta*ornega*t l/std-phdelta))* std*sqrt(d)-edo 1 *sin(fd* t I -phase)*ddelta) ; tenn22=(omega2*( 1 .delta2)*d); term2=tenn2 Menn22; y[i]=kv*omega2/(a 1 *td)*(terrn 1 +term2);
i return:
1
