• TABLE OF CONTENTS
HIDE
 Title Page
 Acknowledgement
 Table of Contents
 List of Tables
 List of Figures
 Abstract
 Introduction
 Objectives of research
 Review of literature
 Design of the Florida citrus-picking...
 Robot kinematic model
 Robot open-loop dynamics
 Performance criteria
 Controller selection and imple...
 Results and discussion
 Summary and conclusions
 Reference
 Biographical sketch
 Copyright














Title: Motion control of a citrus-picking robot
CITATION THUMBNAILS PAGE IMAGE ZOOMABLE
Full Citation
STANDARD VIEW MARC VIEW
Permanent Link: http://ufdc.ufl.edu/UF00090200/00001
 Material Information
Title: Motion control of a citrus-picking robot
Series Title: Motion control of a citrus-picking robot
Physical Description: Book
Creator: Pool, Thomas Alan,
 Record Information
Bibliographic ID: UF00090200
Volume ID: VID00001
Source Institution: University of Florida
Holding Location: University of Florida
Rights Management: All rights reserved by the source institution and holding location.
Resource Identifier: alephbibnum - 001483561
oclc - 21085429

Table of Contents
    Title Page
        Page i
    Acknowledgement
        Page ii
    Table of Contents
        Page iii
        Page iv
        Page v
    List of Tables
        Page vi
    List of Figures
        Page vii
        Page viii
        Page ix
        Page x
        Page xi
        Page xii
    Abstract
        Page xiii
        Page xiv
    Introduction
        Page 1
        Page 2
        Page 3
        Page 4
        Page 5
    Objectives of research
        Page 6
        Page 7
        Page 8
    Review of literature
        Page 9
        Page 10
        Page 11
        Page 12
        Page 13
        Page 14
        Page 15
        Page 16
        Page 17
        Page 18
        Page 19
        Page 20
    Design of the Florida citrus-picking robot
        Page 21
        Page 22
        Page 23
        Page 24
        Page 25
        Page 26
        Page 27
        Page 28
        Page 29
        Page 30
        Page 31
        Page 32
        Page 33
        Page 34
        Page 35
        Page 36
        Page 37
        Page 38
        Page 39
        Page 40
        Page 41
        Page 42
        Page 43
        Page 44
    Robot kinematic model
        Page 45
        Page 46
        Page 47
        Page 48
        Page 49
        Page 50
        Page 51
        Page 52
        Page 53
        Page 54
    Robot open-loop dynamics
        Page 55
        Page 56
        Page 57
        Page 58
        Page 59
        Page 60
        Page 61
        Page 62
        Page 63
        Page 64
        Page 65
        Page 66
        Page 67
        Page 68
        Page 69
        Page 70
        Page 71
        Page 72
        Page 73
        Page 74
    Performance criteria
        Page 75
        Page 76
        Page 77
        Page 78
        Page 79
        Page 80
        Page 81
        Page 82
        Page 83
        Page 84
        Page 85
        Page 86
        Page 87
        Page 88
        Page 89
    Controller selection and implementation
        Page 90
        Page 91
        Page 92
        Page 93
        Page 94
        Page 95
        Page 96
        Page 97
        Page 98
        Page 99
        Page 100
        Page 101
        Page 102
        Page 103
        Page 104
        Page 105
        Page 106
        Page 107
        Page 108
        Page 109
        Page 110
        Page 111
        Page 112
        Page 113
        Page 114
        Page 115
        Page 116
        Page 117
        Page 118
        Page 119
    Results and discussion
        Page 120
        Page 121
        Page 122
        Page 123
        Page 124
        Page 125
        Page 126
        Page 127
        Page 128
        Page 129
        Page 130
        Page 131
        Page 132
        Page 133
        Page 134
        Page 135
        Page 136
        Page 137
        Page 138
        Page 139
        Page 140
        Page 141
        Page 142
        Page 143
        Page 144
        Page 145
        Page 146
        Page 147
        Page 148
        Page 149
    Summary and conclusions
        Page 150
        Page 151
        Page 152
        Page 153
        Page 154
    Reference
        Page 155
        Page 156
    Biographical sketch
        Page 157
        Page 158
        Page 159
        Page 160
    Copyright
        Copyright
Full Text











MOTION CONTROL OF A CITRUS-PICKING ROBOT


By

THOMAS ALAN POOL

















A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL
OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT
OF THE REQUIREMENTS FOR THE DEGREE OF
DOCTOR OF PHILOSOPHY

UNIVERSITY OF FLORIDA


1989















ACKNOWLEDGEMENTS


The author wishes to express sincere appreciation to Dr. Roy C. Harrell, his major

advisor and friend, for guidance and assistance provided during the graduate program. For the

enormous amounts of time Dr. Harrell invested reviewing this dissertation, the author is

especially grateful.

Thanks are also due to the professors who graciously gave of their time to serve on the

graduate committee: Dr. Keith Doty, Dr. John Staudhammer, Dr. Robert Peart, and Dr. Carl

Crane. These men provided necessary excitement and instruction for making the author capable

of completing this project.

Mr. Ralph Hoffman provided some very necessary assistance with the tests which are

described within this dissertation. He was always available with an encouraging word, never

even once refusing to assist with even the most mundane chores. For all of this, the author is

grateful.

Mr. Phil Adsit, a fellow graduate student, assisted with many of the programming

tasks which were undertaken in this work. He gave much-needed encouragement and advice as

he worked so closely with the author. Much appreciation is extended to Mr. P.

Financial assistance was awarded to the author in the form of a fellowship under the

USDA Food and Agricultural Sciences National Needs Graduate Fellowships Grants Program.

For this assistance, the author is also grateful.

Funding for the citrus picking robot was provided by Agricultural Industrial

Development, SpA, Catania, Italy.

Most importantly, the author expresses sincere thanks to his parents, Mr. and Mrs.

William H. Pool, for their encouragement and support during this project. Their leadership

and guidance proved invaluable to the completion of this work.

ii














TABLE OF CONTENTS

page

ACKNOW LEDGEM ENTS ............................................................ ........................... ii

LIST OF TABLES ....................................................................................................... vi

LIST OF FIGURES ..................................................................................................... vii

ABSTRACT ............................................................................................................... xiii

CHAPTERS

1 INTRODUCTION ........................................................... ........................... 1

2 OBJECTIVES OF RESEARCH .................................................. .................... 6

3 REVIEW OF LITERATURE ................................................................................. 9

Previous Robotics in Tree Fruit Harvest ........................................... ............. 9
Robot Kinem atics Background............................................ ....................... 14
Controls Background ......................................................... .......................... 16
Performance Specifications ................................................... ....................... 20

4 DESIGN OF THE FLORIDA CITRUS-PICKING ROBOT ................................... 21

M echanical Design .......................................................... ........................... 21
Control Computer ........................................................... ............................ 25
Hardware ............................................................... .............................. 25
Video system .......................................................... ........................ 26
Ultrasonics .......................................................... ............................ 27
Analog to digital converters .................................................... ............. 27
Parallel I/O signals .................................................. ...................... 28
Servo mechanisms .................................................... ...................... 28
Software ........................................................... .................................... 29
Picking M echanism ........................................................... ........................... 31
Construction ................................................................................................ 31
Fruit Sensor Package ..................................................................................... 33
CCD camera ......................................................... .......................... 33
Ultrasonic range sensor ................................................ .................... 34
Lights ............................................................. ................................ 35
Hydraulic Actuation ....................................................... ............................ 36
Position Sensors ......................................... ................ .................................. 37
Velocity Sensors ............................................................ ............................. 42





iii









page

5 ROBOT KINEM ATIC M ODEL ................................................ ..................... 45

M anipulator Kinem atics ............................................................................ 45
Im aging Kinematics ......................................................................................... 49
Vision-Servo Kinem atics .................................................................................. 50

6 ROBOT OPEN-LOOP DYNAM ICS ............................................. ................. 55

Background .......................................... ................. ...................................... 55
Param eter Estim ation ........................................................ .......................... 58
Results and Discussion.................................................................................... 64

7 PERFORM ANCE CRITERIA ..................................................... ................... 75

Background ................................................................................................... 75
Characteristics of Fruit M otion ................................................ .................... 76
Picking Envelope Definition .................................................. ...................... 78
Velocity Control Requirem ents ....................................................................... 82
Position Control Requirem ents ............................................................................. 84
Vision Control Requirements .................................................. ...................... 85
Summ ary of Performance Criteria ................................................................... .... 88

8 CONTROLLER SELECTION AND IMPLEMENTATION ................................... 90

Controller Selection ............................................................... 90
Control System Discretization .......................................................... ............ ...... 93
Controller Im plem entation .............................................................................. 97
Velocity Controllers ..................................................... ......................... 97
Position Controllers............................. ................. .................................. 99
Position Controller With Velocity Control Minor Loop ................................. 101
Vision Controllers ........................................................................................ 103
Controller Tuning ................................................................................................ 107
Velocity Controller Tuning ............................................................................ 110
Position Controller Tuning ............................................................................. 113
Position Controller with Velocity Control Minor Loop Tuning ........................ 115
Vision Controller Tuning................................................................................ 116
Sum mary ....................................................................................................... 118

9 RESULTS AND DISCUSSION ............................................................................ 120

Velocity Controller Performance ........................................................................ 120
Position Controller Performance .......................................................................... 127
Vision Controller Performance ............................................................................ 131
Dynam ic Perform ance................................................................................... 131
Static Perform ance................................................................................... 133
Overall Controller Perform ance....................... ............. ........................ 137
Perform ance in the Search Routine................................................................. 137
Performance During the Pick Cycle ............................................................... 140









page

10 SUM M ARY AND CONCLUSIONS ................................................................ 150

Sum m ary ....................................................................................................... 150
Conclusions ................................................................................... .... ............ 152

REFERENCES ........................................................................................................... 155

BIOGRAPHICAL SKETCH ........................................................................................ 157















LIST OF TABLES


Table page

4.1. Servo amplifier and servo valve characteristics. ........................................... 29

4.2. Conversion factors for changing A/D information to actual positions and
velocities of the robot's joints. .................................................................... 44

5.1. Link parameters for the orange-picking robot. .................................................... 48

5.2. Range of motion for the joint parameters of the orange-picking robot. ............... 49

6.1. Experimentally determined steady-state gains, damping ratios, and hydraulic
natural frequencies of joint 0 in relation to position of joint 2 and direction of
m option ............................................................................................................... 62

6.2. Experimentally determined steady-state gains, damping ratios, and hydraulic
natural frequencies of joint 1 in relation to position of joint 2 and direction of
m option ............................................................................................................... 64

6.3. Steady-state gains, hydraulic damping ratios, and hydraulic natural
frequencies as determined by analysis of the step test responses .......................... 65

7.1. Range of positions of a fruit's centroid to guarantee a successful pick ................... 82

7.2. Values read by vision system corresponding to the established picking
envelope. ...................................................................................................... 82

7.3. Summary of performance criteria for the velocity,position, and vision control
algorithm s. ................................................................................................... 89

8.1. Velocity control variables as used for the velocity controllers as shown in
Figure 8.6. ............................................................... ................................... 99

8.2. Position control variables as used in Figures 8.10, 8.11 and 8.12. ............................ 101

8.3. Position control variables as used in Figures 8.14 and 8.15. .................................... 103

8.4. Vision control variables as used in Figures 8.17 and 8.18. ...................................... 107

8.5. Final velocity controller parameters for joints 0, 1, and 2. ..................................... 112

8.6. Final position controller parameters for joints 0 and 1. ........................................ 114

8.7. Final vision controller parameters for joints 0 and 1. ........................................... 118

8.8. Final determined values for all controller parameters. ........................................ 119

vi















LIST OF FIGURES


Figure page

1.1. The Florida Laboratory. ............................................................................ 2

3.1. Kinematic link frames as attached to the links of a manipulator (Paul, 1981). ..... 15

4.1. The three degree-of-freedom orange-picking robot. ......................................... 22

4.2. Base support stand of the robot. ................................................................... 23

4.3. Outer link of the robot. ............................................................................... 24

4.4. The inner link ............................................................................................... 25

4.5. The sliding link .......................................................................................... .. 26

4.6. I/O hardware architecture of the control computer. ......................................... 27

4.7. Organization of the robot programming environment. ......................................... 30

4.8. Cutaway side view of the picking mechanism showing the position of the
ultrasonic transducer, the color CCD camera, and the lights. .............................. 32

4.9. Top view of the picking mechanism showing the drive linkage and the lever
arm for actuating the rotating lip mechanism. ................................... .......... 32

4.10. Picking mechanism actuation assembly. ......................................... ........... 33

4.11. Front view of the picking mechanism showing the color CCD camera, ultrasonic
transducer, and the four lights. .................................................................. 34

4.12. Horizontal field of view of the color CCD camera and lens combination in the
vertical plane. ........................................................... ................................ 35

4.13. Vertical field of view of the color CCD camera and lens combination in the
horizontal plane. ......................................................... .............................. 35

4.14. Circuit diagram of hydraulic power unit. ........................................ .......... 36

4.15. Circuit diagram of hydraulic actuators for joints 0 and 1 ..................................... 37

4.16 Circuit diagram of hydraulic motor actualtor for joint 2. .................................... 37

4.17. Circuit diagram of picking mechanism lip actuator. ........................................ 38

4.18. Circuit diagram of the position sensing potentiometers. ..................................... 39

vii









Figure page

4.19. Relationship between position of joint 0 and the A/D value of the position of
joint 0. ................................................................................................................. 40

4.20. Relationship between position of joint 1 and the A/D value of the position of
joint 1. ........................................................................................................... 41

4.21. Relationship between position of joint 2 and the A/D value of the position of
joint 2. ................................................................................................................ 42

4.22. Circuit diagram of the tachometers. ............................................................ 43

5.1. Kinematic frames of the three degree-of-freedom robot. .................................... 46

5.2. Kinematic frames as assigned to the links of the robot. ..................................... 47

5.3. Lens imaging geometry. ............................................................................... 50

5.4. Coordinate frame representation of the camera frame and the fruit position
with respect to the base frame. ................................................................... 52

6.1. Open-loop hydraulic servo system. ..................................................................... 56

6.2. Reduced block diagram of the open-loop system. .......................................... 56

6.3. Block diagram of the open-loop system with external torque load ...................... 58

6.4. Typical response of joint 0 of the orange-picking robot to a step input of 1200
D/A bits with joint 2 centered in the Hooke joint. ............................................... 61

6.5. Typical response joint 1 of the orange-picking robot to a step input of -500 D/A
bits with joint 2 centered in the Hooke Joint. .................................... ........... 63

6.6. Typical response of joint 2 of the orange-picking robot to a step input of 750 D/A
bits. ................................................................................................................... 65

6.7. Responses of joint 0 to a step input: (a) actual, (b) simulated.
h = 32.95 rad/sec, Sh = 0.30, Kv = 0.036 (deg/sec)/(D/A word) ........................... 69

6.8. Responses of joint 0 to a step input: (a) actual, (b) simulated.
h = 17.36 rad/sec, 0h = 0.16, Kv = 0.032 (deg/sec)/(D/A word) ........................... 70

6.9. Responses of joint 0 to a step input: (a) actual, (b) simulated.
h = 16.54 rad/sec, h = 0.31, Kv = 0.044 (deg/sec)/(D/A word) ............................. 71

6.10. Responses of joint 1 to a step input: (a) actual, (b) simulated.
= 31.87 rad/sec, 8h = 0.17, K, = 0.055 (deg/sec)/(D/A word) ........................... 72

6.11. Responses of joint 1 to a step input: (a) actual, (b) simulated.
h = 22.02 rad/sec, h = 0.31, Kv = 0.056 (deg/sec)/(D/A word) ........................... 73

6.12. Responses of joint 1 to a step input: (a) actual, (b) simulated.
(O = 21.47 rad/sec, 8h = 0.22, Kv = 0.056 (deg/sec)/(D/A word) ........................... 74

viii









Figure page

7.1. Typical observations of the motion of fruit swinging from the canopy of a tree
indicating the peak-to-peak magnitude of the fruit for various periods of
oscillation. ............................................................... .................................. 77

7.2. Apparatus for determining the picking envelope of the robot's end-effector. ......... 78

7.3. Picking range of the end-effector in the vertical plane located on the centerline
of axis 2. ........................................................................................................ 80

7.4. Picking range of the end-effector in the vertical plane located 1 cm from the
centerline of axis 2. ........................................................ ............................. 80

7.5. Picking range of the end-effector in the vertical plane located 2 cm from the
centerline of axis 2. ........................................................ ............................. 81

7.6. Relationship between the amplitude of a fruit and the amplitude of its image
on the camera image plane. ..................................... .................................... 86

7.7. Maximum allowable phase lag as a function of amplitude ratio, ar/af, as
described in equation 7-3. Maximum allowable error = 47 pixels (2 cm). Case I:
af = 50 cm (1077 pixels). Case II: af = 12.5 cm (269 pixels) ................................. 88

8.1. Bode diagram of the open-loop velocity control system. ..................................... 91

8.2. Bode diagram of the open-loop position control system. ..................................... 92

8.3. Simulated responses of continuous-time and discrete domain controllers and the
percent error differences.
Kc = -15.00, Td = 3.7, and ri = 1.0 Kc = 4.00, rd = 0.02, and i = 0.01
(a) continuous-time domain controller (d) continuous-time domain controller
(b) discrete controller (e) discrete controller
(c) percent error (f) percent error ................................ 96

8.4. Subroutine to calculate coefficients of the discretized controller from the
continuous domain parameters. .................................................................... 97

8.5. Block diagram of the velocity control loop. ........................................................ 98

8.6. Implementation of the Lag-Lead velocity controllers as found in the software
environment. Note: change O's in the variables to 1's or 2's for joint 1 or 2. ........... 98

8.9. Block diagram of the position control feedback loop for joints 0 and 1 .................. 100

8.10. Position error calculation for joints 0 and 1 from ERRORCAL:C subroutine ........... 100

8.11. Position control subroutine for joint 0. .................................................................. 100

8.12. Position control subroutine for joint 1. .................................................................. 100

8.13. Block diagram of the position control feedback loop with velocity feedback in a
m inor loop. ......................................................................................................... 102









Figure page

8.14. Position error calculation for joint 2 as accomplished in the ERRORCAL:C
routine. .............................................................................................................. 102

8.15. Major loop position control subroutine for joint 2. ................................................ 103

8.16. Block diagram of a vision control loop. .............................................................. 104

8.17. Vision gain implementation scheme. .................................................................. 105

8.18. Vision error calculations as performed in ERRORCAL:C routine. ........................ 106

8.19. Vision control subroutine. ...................... ............................................................... 106

8.20. Bode diagram of a controlled second-order system indicating the effects changes
in the lag tim e constant, Ti .................................................................................. 108

8.21. Bode diagram of a controlled second-order system indicating the effects changes
in the lead time constant, d. .............................................................. 108

8.22. Bode diagram of a controlled second-order system indicating the effects changes
in the controller gain, K ............................................. ..................................... 109

8.23. Generalized open-loop Bode diagram of a lag-lead compensated velocity
control system ............................................................................................... 111

8.24. Bode diagram of an open-loop compensated position control system. ................... 114

8.25. Open-loop Bode diagram for joint 0 vision system indicating the gain
requirements for picking worst case fruit motions. ............................................... 117

9.1. Typical closed-loop response of joint 0 to an alternating step input of
16.3 deg/sec, joint 2 extended. ............................................................................ 121

9.2. Typical closed-loop response of joint 0 to an alternating step input of
39.1 deg/sec, joint 2 positioned at its mid location. ........................................... 122

9.3. Typical closed-loop response of joint 0 to an alternating step input of
48.8 deg/sec, joint 2 retracted. ........................................................................... 122

9.4. Typical closed-loop response of joint 1 to an alternating step input of
32.6 deg/sec, joint 2 retracted. ........................................................................... 122

9.5. Typical closed-loop response of joint 1 to an alternating step input of
32.6 deg/sec, joint 2 extended. ............................................................................ 124

9.6. Typical closed-loop response of joint 1 to an alternating step input of
32.6 deg/sec, joint 2 centered in the support. ..................................................... 125

9.7. Typical closed-loop response of joint 2 to step inputs of 34.8 cm/sec .................... 126

9.8. Typical closed-loop response of joint 2 to an alternating step input of
104.4 cm /sec. ............................................................................................... 126

x









Figure page

9.9. Typical closed-loop response of joint 0 to a step change in the position setpoint
of +51.8 deg. .................................................................................................. 128

9.10. Worst case closed-loop response of joint 0 to a step change in the position
setpoint of -51.8 deg with joint 2 extended. ......................................................... 128

9.11. Typical closed-loop response of joint 1 to a step change in the position setpoint
of +23.8 deg. .................................................................................................. 129

9.12. Worst case closed-loop response of joint 1 to a step change in the position
setpoint of -23.8 deg with joint 2 extended. ......................................................... 130

9.13. Typical closed-loop response of joint 2 to a step change in the position setpoint
of 43.8 cm ......................................................................................... ........... 131

9.14. Worst case closed-loop response of joint 2 to a step change in the position
setpoint of 43.8 cm ......................................................................................... 131

9.15. Closed-loop magnitude and phase plots for joint 0 vision control system:
Kc = 4.0 (D/A word)/pixel Kp = 2.27 pixels/(D/A word)
Ti= 0.02 sec = 0.18
Td =0.01 sec h = 17 rad/sec ......................... 133

9.16. Closed-loop magnitude and phase plots for joint 1 vision control system:
Kc = 4.0 (D/A word)/pixel Kp = 3.85 pixels/(D/A word)
Ti =0.02 sec 8 = 0.18
d = 0.01 sec h = 22 rad/sec ......................... 134

9.17. Pickable fruit motions as determined from the closed-loop response of the
manipulator along with the fruit motions determined to exist in the grove........... 135

9.18. Closed-loop vision system response of joint 0 to a setpoint of 200 pixels with the
beginning fruit location of 50 pixels in the vertical direction in the image
plane ........................................................................................................... 136

9.19. Closed-loop vision system response of joint 1 to a setpoint of 192 pixels with a
beginning fruit location of 37 pixels in the horizontal direction in the image
plane. ................................................................................................................ 136

9.20. Plot of position and position setpoint of joint 0 during a typical fruit search
routine. .............................................................................................................. 138

9.21. Plot of velocity and velocity setpoint of joint 0 during a typical fruit search
routine ......................................................................................................... 138

9.22. Plot of position and position setpoint of joint 1 during a typical fruit search
routine. ............................................................................................................. 139

9.23. Plot of velocity and velocity setpoint of joint 0 during a typical fruit search
routine. .............................................................................................................. 139

9.24. Control modes and position and velocity of joint 2 during a typical pick cycle ....... 141

xi









Figure page

9.25. Position of joints 0 and 1 and locations of the furit during a typical pick. cycle....... 142

9.26. Controller responses to large fruit motions using the wait state. ........................... 144

9.27. Control modes and joint 0 and 1 positions during a pick cycle with large fruit
motion which required use of the wait state. ..................................................... 145

9.28. Pick cycle aborted because of a collision during the fruit approach routine. ........... 148















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

MOTION CONTROL OF A CITRUS-PICKING ROBOT

By

Thomas Alan Pool

May, 1989



Chairman: Dr. Roy C. Harrell
Major Department: Agricultural Engineering

This work focused on the design and implementation of motion control strategies for a

citrus-picking robot. These control strategies were used by an intelligence base for the robot

which required precise control of the individual joints based on information from velocity,

position, and vision sensors. The type of controller that was used at any instant was determined

by this intelligence base, which called the applicable controller and supplied it with

setpoints.

Initially, the design of the hydraulically actuated, three degree-of-freedom,

spherical coordinate arm was presented. The picking mechanism housed the vision and

ultrasonic sensors, which provided real-time end of the arm fruit position sensing and a rotating

lip for removing the fruit from the tree. From the known dimensions of the robot, a kinematic

model was derived. This model provided a basis upon which the position of the joints could be

related to the information detected by vision sensor. Dynamic models of the robot joints were

experimentally determined for use in designing and tuning the joint controllers. Lag-lead

compensators were chosen for their ability to improve a system's steady state performance

while improving the response rate by increasing the system bandwidth. These controllers were

discretized and programmed into the software environment of the robot.

xiii










The performance of the robot was assessed by its suitability for the specified task.

Thus, typical fruit motions were investigated. Also, a picking envelope was established which

defined the volume with respect to the end-effector in which a fruit must be located to be

picked. These characteristics were used to relate the task of picking citrus fruit to performance

requirements for each of the robot's joint motion controllers.

The controllers achieved acceptable robot performance during a normal pick cycle.

Tests of the velocity and position controllers for all of the joints showed that these controllers

met or exceeded the established requirements. The vision controllers achieved success when

subjected to fruit with slower motions. For large, fast fruit motions, however, the robot was

required to wait for the fruit to slow before a successful pick attempt was achieved.















CHAPTER 1
INTRODUCTION


Traditionally, the harvest of citrus fruit has been a labor-intensive operation. Much of

this manual labor in America has been provided in the form of seasonal farm workers who were

actually illegal aliens from Mexico and other Latin-American countries (Martin, 1983). Martin

also observed that even with the illegal work force the turnover was very high. In fact,

maintaining a work force of 20 workers has required that some farmers hire as many as 200

workers in a month's time. Another challenge that farmers have been faced with is the rising

cost of harvest. In the United States, Martin noted that the farmer paid 20 percent of the fruit

price for harvesting. In comparison with other countries, the U.S. farm worker wages are 5

times that of Greece and 10 times that of Mexico. Therefore, U.S. farmers start out behind in

the competition for the world market. After identifying these problems, researchers have

turned their attention toward the mechanization of the harvest process. Several mechanical

harvesters such as tree shakers and blowers have been designed and built, only to find that

these attempts caused damage to the fruit trees. These non-selective harvesters also proved

detrimental to future yields from the Valencia orange which is harvested after the immature

fruit for the following season have already formed (Coppock, 1984). Damage and removal of

the small, unripe fruit reduced the following year's production. The conclusions of many of the

researchers was that individual and selective harvest of the fruit would be most desirable.

In 1984, researchers at the University of Florida began an investigation of the physical

and economic feasibility of picking citrus fruit with a robotic mechanism. Initially, a

commercially available robot was acquired for this purpose. A spherical coordinate

manipulator was chosen which allowed for ease of vision-servoing-controlling the motion of

the robot joints based on fruit position as determined by a camera. This spherical coordinate










(RRP: revolute, revolute, prismatic) configuration consisted of two revolute or rotary joints with

axes of rotation intersecting at a right angle and a prismatic or sliding joint. These first two

joints allowed the distal end of the robot to be "pointed" while the third joint provided the

capability for extension. A computer was interfaced with the robot's controller and the

manipulator was used to demonstrate the vision-servoing concept by picking artificial fruit

from a simulated canopy. Successes with the laboratory robot prompted the construction of a

more portable field robot which could be tested in actual grove settings.

The ideas for the Florida Laboratory (Figure 1.1) sprung forth. The Florida Laboratory

was constructed as a trailer package which could be towed behind a truck to groves around the

state of Florida. Basic construction of the lab consisted of an enclosed front section with an open

flat bed to the rear. The enclosed section sheltered the environment-sensitive hardware, such

as the control computer and electrical connections, and provided a comfortable working area for

the operators. Two software development stations were provided in this control room. The


Figure 1.1. The Florida Laboratory.


r--~YB: A










hydraulically actuated manipulator was mounted on the rear, flatbed portion of the lab. A 15

kW electric generator furnished electricity for the hydraulic power unit, the control computer,

and other hardware used in the development of software for the grove model robot. Visual

access to the robot was furnished by three windows mounted on the rear and picking side of the

control room.

The robot was a three degree-of-freedom, spherical-coordinate geometry arm.

Actuation of the joints was accomplished through the use of servo-hydraulic drives consisting

of servo amplifiers, servo valves, and actuators. For the first two joints, rotary actuators were

used to generate the revolute motion about intersecting horizontal and vertical axes. The third

joint, a sliding joint, was actuated by a hydraulic motor through a rack and pinion drive. The

revolute motion of the first two joints provided the robot with the ability to point toward a

fruit, and the prismatic or sliding joint provided the ability to reach toward the targeted fruit.

A picking mechanism was attached to the end of the arm and enclosed the color CCD camera

and ultrasonic ranging transducer which were used for fruit detection. Also, a rotating lip was

attached to the picking mechanism which was used to grasp the fruit and remove it from the

tree. The motion of the joints was determined by an intelligence base which used the

information from the position and velocity sensors on the joints as well as the fruit sensors to

establish desired actions of the manipulator.

The intelligence for the robot was built around a concept of states or modes of operation

in which decisions were made based on the available information from the sensors. The

intelligence base was developed and programmed for the orange-picking robot by Adsit (1989).

The heart of the intelligence base was a state network which provided robot command

decisions through sensing, action, and reasoning agents. Sensing agents were used to quantify

the robot's work environment and the robot's status. Reasoning agents made decisions in regard

to the information from the sensing agents. Action agents caused motion of the robot to alter the

work environment or the sensor's perception of the work environment. These agents were linked

together by a common database with result fields, parameter fields, and activate fields. The










result fields held the results from an agent's operation. Parameter fields specified the criteria

for accomplishing a task. Activate fields contained flags which induced or terminated an

action. The information contained in these fields was compared to models which characterized

significant events which occurred during the picking operation. The results of this comparison

were used to cause the execution to move from one command state to another.

The execution of a pick cycle included stepping through the state network in a logical

manner which resulted in removing a fruit from a tree. Each pick cycle began with the robot in

its home configuration and concluded by dropping the fruit while in this same configuration.

Before any joint motion was specified, the vision sensors were checked for the presence of fruit.

If no fruit were found in the field of view, a search pattern was initialized. When a fruit was

detected, an approach state was activated which actuated the revolute joints to align the arm

with the fruit (vision-servoing). While this vision-servoing operation continued, the sliding

joint was activated to extend to the targeted fruit. When the picking mechanism was

positioned so that the fruit could be captured, robot motion was stopped, and the fruit was

girdled by the rotating lip of the picking mechanism. The sliding joint was then retracted to its

home position. Once the sliding joint reached its home position, both revolute joints were

returned to their home locations, and the fruit was released from the picking mechanism. This

pick cycle was repeated until no more fruit were detected by the fruit sensors.

This picking operation required control of the motion of the joints based on velocity,

position, and vision information. When the robot was requested to search the tree canopy for a
)
fruit, the revolute joints were instructed to move with constant velocity. When a fruit was

detected, the motion of these two joints was changed to cause the targeted fruit to be positioned

in the center of the image of the camera. Thus, the motion of these two joints was also to be

altered by the commands of the vision system. Since the actual distance between the end-

effector and the targeted fruit was not known, the slider was directed to proceed at a constant

velocity until proximity with the fruit was detected by the ultrasonic system. As the fruit was

being girdled by the rotating lip, all three of the joints were required to remain stationary at










their present positions. For the slider to be retracted from the canopy at a constant velocity,

the revolute joints were to hold their positions. The instruction to go to the home location was

provided in the form of positions for each of the joints.

Thus, three modes of control were necessary for the orange-picking robot. First, each of

the joints was to be moved at a constant velocity by a velocity controller. Second, to cause the

joints to go to specified positions, position controllers were needed. Third, vision controllers

were required to change the position of the revolute joints to point the end-effector toward a

targeted fruit. The development of these three motion control strategies was the overall goal

of this work. The accomplishment of this goal would provide controllers which would take

setpoints from the intelligence base and accomplish the desired result in a smooth and accurate

manner.















CHAPTER 2
OBJECTIVES OF RESEARCH


The main objective of this work was to develop motion control strategies for the orange-

picking robot. This objective was divided into the following specific areas:

1. develop a kinematic model for the orange-picking robot;

2. derive an open-loop dynamic model for each robot joint;

3. define performance criteria necessary for picking citrus fruit;

4. develop and implement control strategies to provide acceptable robot performance; and

5. test and evaluate the performance of the robot.

Algorithms were necessary for controlling the robot, based on desired positions and

velocities of the joints of the manipulator and based on fruit position related to the end of the

robot. These motion control algorithms were to be called from the robot intelligence base to

cause the manipulator to move in a specified manner. The control algorithms were to be links

between the software environment, the manipulator, and the manipulator's workspace, with

the ultimate task of removing fruit from the canopy of an orange tree. Because the algorithms

were to be used with a digital computer, they were required to operate in the discrete domain.

Only a minimum of computational complexity was to be permitted because real-time control

was necessary to enable the robot to pick moving fruit.

A kinematic model was necessary to describe the geometric relationship between the

joints and links of the robot. This model was to provide an understanding of the motion of the

manipulator links for specified changes in joint positions. Ultimately, the kinematic model

was to be used for determining the relationship between the position of the robot's end-effector

and the position of a targeted fruit. This relationship provided a basis upon which functions

for the vision control system gains could be derived.










An open-loop dynamic model for each robot joint was to be derived for use in selecting

control systems for the manipulator. These dynamic models would provide insight into the

response characteristics of the manipulator's joints, which could be used for tuning the

controllers and thus to achieve desired responses from the joints. The open-loop response of

each joint to different step input signals was to be evaluated for use in determining open-loop

dynamic models. The dynamic models were to be used as plants in closed-loop systems for

selecting and tuning the controllers. Once established, these dynamic models were to be

verified by comparison of the actual response of the joints with the simulated response of the

determined dynamic models.

Final performance of the robot was to be defined by critical performance criteria. These

performance criteria were to be determined from observations of fruit motion in the tree's

canopy and other pick-cycle motion requirements. Along with the robot's ability to track the

motion of fruit in the canopy of the tree, the robot would be required to position the picking

mechanism such that the rotating lip would be able to remove the located fruit from the tree.

The manipulator was also to be required to move through a fruit-locating search pattern with

rapid though smooth actions. These criteria were to be used for designing and evaluating the

strategies and algorithms which were developed for controlling the orange-picking robot.

Based on the information from the kinematic and dynamic models and the performance

criteria, position, velocity, and vision control strategies for the manipulator were to be

developed. These control strategies were to be the basis upon which robot-control algorithms

were to be developed. The algorithms were to be incorporated into a robot-programming

environment which contained intelligence for the orange-picking robot. From the programming

environment, the algorithms could be called when necessary to accomplish a desired motion of

the manipulator.

Once implemented, the performance of each of the motion-control strategies was to be

evaluated. Assessment of the controllers would involve designing tests which would

demonstrate the ability or lack of ability of the controllers to meet the specified performance






8


criteria. First, the controllers would be tested on an individual basis. Finally, the response of

the controllers while operating in the intelligence base would be evaluated for the ability to

control the joint motions of the manipulator during the actual picking operation.















CHAPTER 3
REVIEW OF LITERATURE


Previous Robotics in Tree Fruit Harvest

Researchers have long noticed the necessity for automated harvest of tree fruit. Many

of these researchers have investigated the use of mechanical components. The need for a

proficient citrus harvester that would individually remove the fruit from the tree was

perceived early (Schertz and Brown, 1968). Schertz and Brown pointed out that any method of

mechanical harvest should inflict only minimal damage to the fruit and tree. They observed

that leaf removal of over 25 percent have corresponding yield decreases for the subsequent year.

However, defoliation of up to 25 percent did not provide an indication of reducing the yield.

These men realized that an advantage of individual fruit harvest over mass fruit removal

could be a reduction in damage to the fruit and to the tree. This method would also have less

impact on the succeeding year's production. Although many researchers have indicated the

need for robotic fruit harvesters and presented their ideas, the technology for making it

achievable has developed only within the last decade.

Pejsa and Orrock (1984) studied potential applications of robotics to agricultural

operations. In their work, some of the sensing and control requirements for intelligent robot

systems in agriculture and especially in orange harvesting were presented. These sensing

requirements included the use of vision for detecting the presence of fruit and its location in two-

dimensions. Range and proximity sensing were deemed necessary for determining the third

dimension of the fruit's location and for collision avoidance. These researchers proposed the

use of force and tactile sensors for reporting the status of the end-effector's grasp of the fruit.

Pejsa and Orrock pointed out that a major effort would be necessary to integrate the sensing

functions with the mechanical functions required to pick tree fruit. They stated that most










industrial robots provided greater accuracy and more degrees of freedom than would be required

of a fruit harvester. Thus, a task-specific robot could be designed for the purpose of fruit

picking. A closed-loop system would be needed that could tie the sensing functions with the

arm motion controllers.

Some of the earliest work in the development of the technology was conducted by

Parrish and Goksel (1977). These men studied the feasibility of analyzing the work space of

the robot by use of machine vision. In their study, a black-and-white video camera was used to

detect the fruit, which were represented by blobs in the two-dimensional picture array. By

analyzing their perimeters, the larger blobs which contained overlapping fruit were

successfully divided into separate fruit. The location of the fruit centroids were determined in

the two-dimensional picture array. Trajectories were calculated from the camera lens center to

the positions of the detected fruit centroids. The mechanical arm was driven to follow these

trajectories toward the fruit. When contact with a fruit was detected by a touch sensor, the

motion of the robot was halted. As fruit removal was not in the scope of this work, a successful

picking attempt was recorded when contact with the fruit was detected. In this work, a scene

was analyzed and one trajectory was calculated for each fruit in the scene. The arm was driven

until all of the trajectories were exhausted. Then, a new scene was analyzed, and the operation

was repeated. Although this system was quite elementary, the concepts and the feasibility of

robotically harvesting tree fruit were inspired. These concepts have been used as a motivation

for many of the later attempts.

Grand d'Esnon (1984 and 1985) developed an apple picker which used similar ideas for

the detection of fruit. This manipulator was a cylindrical coordinate robot (PRP, prismatic in

the first joint, revolute in the second, and prismatic in the third). Using the vision system to

determine the fruit's position in two coordinates, the robot was pointed towards the fruit by

raising the arm along a vertical prismatic joint and then pivoting around a vertical axis. This

motion relied on the concept that the fruit did not move between the time that the image was

acquired and the picking tool reached the fruit. In a later attempt, Grand d'Esnon et al. (1987)










developed a fruit picker which consisted of five revolute joints. The first two joints provided

the robot with the pointing ability while the remaining three joints formed an elbow for

extending the end-effector towards a targeted fruit. Operation of the robot again involved

acquiring an image and then adjusting the joints of the robot to align the end-effector with the

camera's line of sight. After the arm was pointed toward the fruit, the joints were moved to

extend the end-effector toward the determined location of the fruit. Again, the robot had no

ability to change its trajectory with the motion of the targeted fruit. In both attempts, the

robot was directed to follow the vector from the camera lens center to the determined position of

the fruit. Forward motion of the robots was continued until contact with a fruit was detected.

Grand d'Esnon et al. reported the removal of 50 percent of the fruit in a hedge at a rate of one

fruit every 4 seconds.

Tutle (1984) developed an idea for an image-controlled robot for harvesting tree fruit.

For his imaginary robot, Tutle proposed the use of a photo-diode-based camera and the

appropriate filters and lighting for detecting the fruit in the canopy of the tree. After the

location of a fruit was determined, the camera was to be displaced so that the arm could be

moved to align with the optical sensor's line of sight towards a detected fruit. Once aligned

with the fruit, the severance module would be extended towards the fruit. The severance

module would contain a second optical sensor that Tutle called a 'seeker' sensor, which

consisted of a four-element photovoltaic detector. The idea behind use of this sensor included

oscillating the severance module until equal light, supposedly reflected from a fruit, was

detected by each of the four elements. This action was intended to align the severance module

with the fruit and adjust for any fruit motion that might have occurred before the robot reached

the fruit. Response from this seeker sensor would also be a signal to slow the forward motion of

the arm. Tactile sensors in the severance module would indicate that a fruit had been scooped

into the module and that forward motion of the arm should be stopped. The fruit would then be

grasped by the severance module, which would twist the fruit while the arm was retracted to

remove the fruit from the tree branch. Since an operational model of this idea was not










constructed, its successfulness could not be evaluated. Also, the control problems were only

assumed and never actually encountered.

The application of robotics in fruit harvesting has also been investigated by

researchers at the Laboratory of Agricultural Machinery in Kyoto, Japan (Kawamura et al.,

1985; Kawamura et al., 1986). This work involved the design of a five degree-of-freedom

manipulator for picking tomatoes and oranges. A global color vision system was employed for

the detection of fruit. The camera was mounted on a vertical support and could be moved up or

down the support. An image was acquired with the camera in one location and then the camera

was moved for acquisition of a second image. The two images were analyzed and the centroids

of the fruit in the image were determined. The three-dimensional positions of the fruit were

calculated from the two-dimensional images by triangulation. A trajectory to each of the fruits

and the robot joint angles required to follow the desired path to a fruit were calculated. As the

arm approached the calculated position of the fruit, the speed of the joints was slowed. When

the hand reached the calculated position of the fruit, the fruit was gripped and severed from

its supporting stem. Three-fingered flexible hands were built for use with the manipulator

(Kawamura et al., 1987). These hands provided gripping forces similar to those of a human

hand while picking tomatoes and oranges. Image processing and joint motion calculations were

rather extensive, causing the picking time for one fruit to be approximately 20 sec.

Harrell et al. (1985) examined the concept of real-time vision-servoing for controlling

the manipulator during the picking process. This work involved the use of a commercially

available spherical coordinate (RRP) robot. A small black-and-white camera was mounted in

the end-effector of the robot and aligned with the axis of the prismatic joint. With this

configuration, a fruit whose image was centered in the camera's image plane was directly in

front of the end-effector. Thus, the end-effector could be extended towards the fruit. Since the

vision-servoing was kept active during the picking process, any motions of the fruit could be

followed by making changes in the two revolute joints. In this first generation, signals from the

control computer were sent directly to the electric robot controller for serving the positions of










the joints. In the controlled environment of a laboratory, this vision-servoing method was

proven to have potential for control of a grove model robot.

Tests in the grove, however, indicated that the black-and-white camera could classify

light reflections from the soil as fruit. Therefore, the use of color vision was investigated

(Slaughter et al., 1986; Slaughter, 1987; Slaughter and Harrell, 1987). In his work, a scheme

was developed which segmented the image into fruit and background regions based on the color

of each pixel. From this image, the centroid of each fruit's image could be determined for use in

adjusting the joints of the robot to point towards the fruit. Since the image was processed in

real-time, the robot could be controlled to track the dynamic motion of the targeted fruit.

In a joint project between researchers at the University of Florida and AID of Italy, two

robots were designed and constructed as prototypes for future work aimed at a commercially

produced tree-fruit-picking robot (Harrell and Levi, 1988). The AID robot relied on electrical

servo controllers to position the joints to point the arm towards the fruit. A global vision system

was used to determine the location of the fruit throughout the picking sequence. The Florida

robot further developed the previous concepts while implementing the vision system of

Slaughter. A hydraulically actuated robot was designed and constructed with a spherical

coordinate architecture (RRP).

For this prototype, a controller was not available. Even if a controller were available,

the exclusive use of point-to-point joint positioning would not have provided the desired

flexibility. The robot motion was to be formulated based on velocity and vision requirements as

well. An intelligence base for the robot was designed and programmed (Adsit, 1989). This

intelligence base added a requirement for fast and accurate control of the joints of the robot

based on velocity, position, and vision setpoints. The design and implementation of these

controllers is the scope of this work.










Robot Kinematics Background

Craig (1986) describes kinematics as the science of motion, ignoring the forces which

cause the motion. Kinematics deals with the variable joint coordinates as they relate to the

position and orientation of the end-effector. Because the relationships between the joints of a

manipulator can be quite complex, the study of robot kinematics deals with the coordinate

frames that describe the kinematic relationships. Any robot manipulator is made up of links

and joints. Most joints are grouped into one of two categories with one degree-of-freedom:

revolute or hinged joints and prismatic or sliding joints. The links of the manipulator are

usually rigid and define the relationship between the joints. The common normal distance, an,

between the axes of two consecutive joints and the twist angle, an, characterize a robot link (see

Figure 3.1). The twist angle is defined as the angle between the consecutive axes in a plane

perpendicular to an. For this work, the Denavit-Hartenberg (1955) notation will be used as

applied by Paul (1981).

The relationships between coordinate frames are expressed as products of rotation and

translation transformations. Homogeneous transformations which represent rotations of one

coordinate frame about the x, y, and z axes of a reference coordinate frame by angles 0 are

represented by the following 4 x 4 matrices:



Rot(x,e) =
0 sin cose 0




Rot(y,) 0 1 0
sin 0 cos E 0
0 and (3-2)


Rot(, sine cos 0 0
Rot(z,6) =
0 0 1 0
S 0 0 1 (3-3)














Joint n-1

) 0-, Link n-1


Link n-2


Joint n
8n


Joint n+1

on.l


Link n


Figure 3.1. Kinematic link frames as attached to the links of a manipulator (Paul, 1981).


A translational homogeneous transformation of one coordinate frame with respect to another by

a vector, ai + bj + ck, is described by

S10 0 a
Trans(a,b,c) = 1 c

.0 0 0 1. (3-4)

The general relationship between two successive frames (the coordinate frame of link n

and the coordinate frame of link n-1, see Figure 3.1) is expressed as the product of two rotation

transformations and two translation transformations. This relationship is referred to as a

general A matrix (Paul, 1981)

"-'An = Rot(zn-l,On) Trans(0,0,dn) Trans(a,,0,0) Rot (xn,an) (3-5)

where n-An = relationship between frame n-1 and n,

Rot(zn.i,9) = rotation transformation about zn.-, an angle On,

Trans(0,0,dn) translation along zn-, a distance dn,










Trans(an,0,0) a translation along rotated Xn.1 = xn a length an, and

Rot(xn,a) = rotation about x,, the twist angle cn.

By multiplying these rotation and translation matrices, the general A matrix for any two

successive links is calculated as

cos sin 8 cos a sin 0 sin a a cos 8
n-1n sin cos cos a -cos sin a a sin
0 sina cos a d
0 0 0 1 (3-6)
(3-6)

In this work, the term frames refers to the coordinate frames assigned to the links of the

manipulator. Coordinate frames represent a three-dimensional space by three orthonormal

vectors; X, Y, and Z. The origin of each frame is located at the intersection of these vectors.

When referring to these vectors, a trailing subscript will be used to identify the frame in

concern. Links are the physical components which make up the manipulator. The motion of the

links is accomplished by the joints. A joint can be rotational revolutee) or translational

(prismatic). The coordinate frames are fixed to the links, and homogeneous transformations are

used to relate the position and orientation of the frames. A joint axis is the vector about which

joint motion takes place. Link coordinate frames are assigned such that the frame's Z axis

defines the vector about which joint motion occurs. Joint motion will be either around or along

the respective Z axis.


Controls Background

The use of a Bode plot, in which the frequency response of a system can be analyzed,

enables the control system designer to evaluate closed-loop system characteristics based on

knowledge of the open-loop system. A controller can then be designed to achieve the desired

system characteristics. Palm (1983) presented some of the basic considerations used in the

frequency-response methods of control system design. First, the cosed-loop system's steady-

state error can be minimized by maintaining a high, open-loop gain in the low-frequency range.

Second, a slope of -1 in the gain curve near the crossover frequency will help to provide an










adequate phase margin. The crossover frequency is the frequency at which the open-loop gain

of the system is unity (0 db). Third, a small gain at higher frequencies will help to attenuate

the effects of noise or mechanical vibrations. All of these characteristics proved to be

applicable to the controller design for the orange-picking robot. Thus, controllers were desired

which provided flexibility to shape the phase and gain curves to achieve the necessary phase

and gain margins.

Lag and lead compensators furnish this flexibility by providing the ability to adjust

the phase and gain in a wide variety of frequencies. When tuned properly, a lag-lead

compensator improves the steady-state performance while also improving the transient

response (Palm, 1983; Ogata, 1970). The lag-lead compensator is represented by the transfer

function

HP s) Kc rdS+ 1
Hp (S)=
Tis+l (3-7)

where s = Laplace operator (sec-1)

Kc = controller gain (appropriate units),

dra lead time constant (sec), and

zi lag time constant (sec).

The design of the controllers involves the placement of the controller gain and the pole and zero

of the controller so that an acceptable response is achieved while meeting the steady-state

requirements. The open-loop system gain can be increased by increasing the controller gain, Kc.

The use of the lag-lead compensator increases the order of the system by one in between the lag

and lead factors. In other words, the addition of the lag or lead compensator causes an

attenuation of the system response between the frequencies of the lag and lead factors (1/zi and

1/Td). Thus, the value of Ti determines the frequency at which this attenuation takes place

while the value of rd determines the frequency at which the attenuation is canceled. The slope

of the gain curve is decreased by 1 between the lag and lead factors.










The applications of control theory to hydraulic systems were presented by Merritt

(1967) and supported by Gibson and Tuteur (1958), Johnson (1977), and McCloy and Martin

(1980). Merritt presented the background necessary for determining the dynamic characteristics

of many hydraulic systems. He demonstrated the methods for deriving the characteristic

equations for each of the system components. Then, he pointed out that the closed-loop response

of a system is limited by the response of the slowest element. Thus, the selection of the

components should take into consideration the hydraulic natural frequency by choosing

actuators that are capable of the desired system response. By choosing system components with

fast response rates (i.e., large natural frequencies), the response rate of the actuator and load

combination is left as the performance-limiting factor of the system. Merritt noted that after

choosing fast control components a normal position control hydraulic servo can be reduced to a

second-order system with an integration. Thus, the position servo system is represented by

2
Gp(s) = Koh 2
s + 28h(hS + (Oh (3-8)

where s = Laplace operator (secr')

Kp = position system gain (appropriate units),

Oh = system natural frequency (rad/sec), and

Sh = system damping ratio (unitless).

A normal velocity control servo can be reduced to a second-order system as in the following

transfer function:

2
G, (s) Kv(Oh
s +28h(hS + (Oh (3-9)

where s = Laplace operator (sec-1)

K, = velocity system gain (appropriate units),

(Oh system natural frequency (rad/sec), and

Sh = system damping ratio (unitless).










Fast components should have a natural frequency of 10 times or greater than the natural

frequency of the actuator and load. Merritt also indicated that the stability of the servo

system can be determined by the position of the resonant peak which occurs near the hydraulic

natural frequency of the second-order system on the Bode magnitude plot. The loop gain at the

resonant peak must be less than unity for stability. Merritt also noted that the lag-lead

controller usually provides acceptable results for both position and velocity control servos. He

demonstrated that the use of velocity minor feedback loops could be used to reduce errors which

result from drift and load friction.

Merritt summarized hydraulic servo design as follows. A power element which will

meet the basic considerations of the load and response is selected. The other components of the

loop are sized to provide the best accuracy and response under the limitations of the power

element. A controller is selected to give the highest possible gain and crossover frequency

while maintaining stability. The only way to achieve a faster response from the system is to

choose a faster power element. These methods of hydraulic controller design along with the

frequency-response design techniques were used for the selection and design of tuners for this

work.

According to Merritt, all real systems are nonlinear to some extent. However, no general

nonlinear theory exists for design of controllers for these nonlinear systems. But, techniques are

available for this design based on the familiar linear techniques. The most widely used

method is to assume that the nonlinearities are small and that the system is linear. In the case

of the orange-picking robot, the nonlinearities that would cause limit cycles or instabilities

were negligible in the range of operation. The deadband caused by imperfect valve

characteristics and the backlash and hysteresis were minimized by dithering the output signal

from the servo amplifiers. The saturation possibilities which would also cause nonlinear

performance were eliminated by limiting the controller outputs. This nonlinearity was

handled within the intelligence base of the robot. Thus, the effects caused by the










nonlinearities of the hydraulic servo systems were considered negligible allowing the design of

controllers based on common linear techniques as previously described.


Performance Specifications

According to Lau et al. (1988), a robot's performance is measured by its ability to

perform a task. Performance measurements are those general operating characteristics that are

important for determining the robot's suitability for a given task. The performance for

industrial robots is usually specified in terms of accuracy, repeatability, positioning resolution,

and speed. These measurements were also defined by Ranky and Wodzinski (1988), who also

presented very precise methods for determining each of them.

These performance measurements prove sufficient for industrial robots which operate in

a structured environment. However, in the unpredictable environment found in agricultural

operations, these methods of performance evaluation have little significance. The ultimate

performance measurement is that of determining the robot's suitability for its intended

operation. Because the location of each fruit in the tree is different, the accuracy and

repeatability of the robot were not pertinent. Performance requirements and evaluation

methods for robots which operate in unstructured environments are much more difficult to

assess. Evaluation of the control strategies for the citrus-picking robot made evident the need

for performance criteria for this specific task.















CHAPTER 4
DESIGN OF THE FLORIDA CITRUS-PICKING ROBOT


An overview of the design of the orange-picking robot, developed by Harrell et al.

(1988), is presented in this chapter. The physical specifications of each part of the robot are

described in the first section along with the position and orientation of the kinematic

coordinate frames as they relate to each physical component. The second section contains a

description of the computer hardware and software designed to control the robot. In the third

section, an illustration of the picking mechanism is introduced showing the configuration of the

components of the fruit sensor package. Hydraulic actuation of the robot is described in the

fourth section of the chapter. Finally, sensing mechanisms for position and velocity of the robot

joints are described in the fifth and sixth sections.


Mechanical Design

A brief description of the physical components that make up the orange-picking robot

are presented in this section. The parts are described along with their relationship to one

another. Also, presented in this section are the axes about which the motion of the robot is

accomplished. These axes of rotation form the Z axes of three-dimensional kinematic frames

which will be presented in a later chapter. The position and orientation of the kinematic

frames as related to the physical components are presented here as an aid in understanding the

kinematics chapter which follows.

The orange-picking robot (Figure 4.1) was built as four major parts: base stand, outer

link, inner link, and sliding tube. The base of the robot was bolted to the flat bed of the

laboratory trailer and acted as a stand to support the other components (Figure 4.2). The first

coordinate frame, frame 0, of the kinematic model was assigned to the base link as shown in the

figure. On the base, the outer link was supported by flange bearings which allowed the outer

21














Outer Link
Joint 0
Servo Drive


Servo Drive Nylon Rack
Inner Link -
Iero rive 1 Base Support
Joint 1 : *J
Servo Drive


Figure 4.1. The three degree-of-freedom orange-picking robot.


link to rotate about a horizontal axis (Zo). Similarly, the inner link was mounted inside of the

outer frame and provided rotation about a vertical axis (Z2). The inner link included a bearing

assembly which supported the sliding tube and provided linear motion along the Z axis of the

inner link (Z2). The picking mechanism of the robot which held the sensor package was

mounted on the end of the sliding tube.

The outer link (Figure 4.3) was fabricated from rectangular steel stock and reinforced at

the corners for increased rigidity. Two solid-steel cylindrical-shaped shafts were mounted on

either side of the outer link. The outer link was mounted to the support stand by a pair of

bearings which connected between these mounting shafts and the vertical members of the base

stand. Coordinate frame 1 of the kinematic model was assigned to the outer link. The center

line of the two shafts were coincident with the Zo axis of coordinate frame 0 allowing the outer

link and coordinate frame 1 to rotate about Zo by an amount O1. Connected to these shafts were a

rotary actuator used for rotating the axis through its motion, a tachometer, and a








































Figure 4.2. Base support stand of the robot.


potentiometer. Motion of the outer link was mechanically limited by two chains to

approximately 39 degrees in either direction from vertical. These chains were connected

between the lower member of the outer link and the cross member of the supporting structure.

The inner link (Figure 4.4) was constructed in two pieces and mounted inside of the outer

link in much the same way as the outer link was mounted to the support. This inner link was

also constructed of rectangular steel with reinforcements at the covers for added rigidity.

Solid steel cylindrical mounting shafts were fastened to the top and bottom of the link allowing

the inner link to rotate about the vertical axis of the outer link, Z1. This link was actuated by a

rotary actuator mounted to the bottom of the outer link and connected to the bottom shaft of the

inner link. The motion of this link was measured by the joint variable 82. Coordinate frame 2














S, Mechanical
Stop

!, -, 0o I
S50 cm z
30 cm


7/ 10.1 cm

S50cm

18.25 cm

Figure 4.3. Outer link of the robot.


was assigned to the inner link and positioned so that its origin was coincident with the origin of

the first two coordinate frames. The vertical axis of this link was always at a right angle to

the horizontal axis of the outer link, resulting in a Hooke joint. Inside of the two part inner link

casing, a split steel pipe was welded as a bushing housing which supported the sliding link.

The two part construction of the inner link allowed shims to be added or removed from between

the two sections to adjust for wear in the nylon slider bushings. The motion of the inner link was

mechanically limited to approximately 30 degrees in either direction from center by

mechanical stops welded to the outer link.

The sliding joint of axis 2 (Figure 4.5) was constructed from a high grade aluminum tube.

Sliding joint motion was accomplished by a rack and pinion drive actuated by a hydraulic

motor. A 1.5 cm square nylon rack was attached to a flat surface machined on the lower side of

the aluminum tube. A 10 pitch, 3.81 cm diameter pinion gear was incorporated into the lower

section of the inner link assembly and meshed with the rack for actuation of joint 2. The Z2 axis

was coincident with the centerline of the tube and intersected with Zo and Z1 at their





















35.5


z1, Y2


z2

20.3 cm

Figure 4.4. The inner link.


intersection point. The motion of the sliding tube was mechanically limited by end caps which

were mounted on either end of the tube. One end cap was welded to the forward or distal end of

the tube and used for mounting the end-effector. The other end cap was bolted to the rear or

proximal end of the tube. These end caps and mechanical stops bolted to the inner link limited

the linear motion of the tube, the joint variable d3, to approximately 1.31 meters.


Control Computer

In this section, the hardware and software used for controlling the robot is introduced.

Components of the computer hardware are presented in systems as they relate to controlling the

robot. A very brief description of the elaborate software environment is also presented here.

Hardware

The control computer for the orange-picking robot was a VME-bussed system with a

Mizar 7120 CPU card. The Mizar card utilized a 12.5 MHZ Motorola MC68020 processor and 1










1 163.5 cm



12 cm T--- 11.2 cm


end-cap actuation end-cap
rack

Figure 4.5. The sliding link.


MB of RAM. The hardware architecture for the control computer is diagrammed in Figure 4.6.

Mass storage for the computer was provided by a 20 MB hard drive and a 1 MB floppy drive

which were connected to the VME bus. The computer was equipped with three serial ports. The

main system console was connected to the computer through one serial port, while a second port

was wired to a remote connector for connecting a terminal outside of the control room of the

trailer. An Apple Macintosh Plus system, with 2 MB of RAM, a 20 MB hard disk, and a printer,

was used as a printer/plotter station and was connected to the VME bus by way of the third

serial port.

Video system. The video system was composed of the CCD camera, the FOR-A color

decoder, three Datacube VVG-128 video frame grabbers, and a Sony RGB video monitor. The

NTSC output signal from the camera was converted into separate red, green, and blue video

signals by the color decoder. The three Datacube frame grabbers provided the ability to

digitize, store, display, and process full color video images in real-time. The red, green, and

blue signals from the decoder were acquired simultaneously by the frame grabbers at a 60 HZ

rate. The signal for each primary color was quantified with 5 bit resolution based on the

intensity of the signal at each pixel. These acquired video data, ranging in value from 0 to 31,

were stored in memory as three, 384 x 244 pixel arrays, one for each primary color signal. The

red frame grabber also generated a hardware interrupt on the VME bus with each vertical

blanking period. This interrupt was used to synchronize all of the real-time activities. Video



































- VMEBUS
EXTERNAL CONNECTIONS


Figure 4.6. I/O hardware architecture of the control computer.


output from all three frame grabbers was displayed on the RGB monitor in the control room of

the Florida Lab.

Ultrasonics. The electronic circuitry for the Ocean Motion ultrasonic ranging transducer

provided a TTL pulse with width proportional to the time required for the ultrasonic burst to

echo from an object. A counter-timer module was used to quantify the duration of the TTL pulses

generated by the ultrasonic ranging unit. Output from the counter-timer was a digital value

which represented the distance from the picking mechanism to the targeted fruit. The counter-

timer was mounted on a card that plugged directly to the VME bus.

Analog to digital converters. Seven analog-to-digital (A/D) converters were used in

the hardware to convert analog voltage signals from the position, velocity, and temperature

sensors to digital values. Six A/D converters were used for the position and velocity sensors as










described in the position and velocity sensing sections. The seventh A/D converter was

connected to a type "T" thermocouple panel meter for monitoring the temperature of the

hydraulic fluid during operation. All of the A/D converters represented an input voltage

ranging from -5 volts to +5 volts with a value ranging from 0 to 4095. The A/D converters were

plugged directly to the VME bus of the control computer.

Parallel I/O signals. Control of components which required a signal depicting either on

or off operation was accomplished by the addition of a parallel card and an Opto-22 switching

panel. The parallel port of the card was connected to the Opto-22 panel. The Opto-22 panel

allowed the addition of either input or output modules. The output modules acted as electronic

relays for switching other components based on the signal from the corresponding pin of the

parallel port. Output modules were used to switch 120 VAC for the picking mechanism lights,

the solenoids of the picking mechanism actuator, and the main hydraulic dump valve. Input

modules were added to the card to sense the status of the hydraulic dump valve and the servo

amplifiers. A 5 VDC input module was used to sense the position of push button safety switches

which were located at various strategic locations in the lab and connected in series to one

another. Signals from the input modules were routed to the appropriate pins of the parallel

card.

Servo mechanisms. Joint motion of the manipulator was accomplished by two

hydraulic rotary actuators and a hydraulic motor. Hydraulic fluid flow to these actuators was

controlled by servo valves. The rate of fluid flow through a servo valve was proportional to

the amount of current passing through the coil of the valve. Three Vickers EM-D-20 servo

amplifier modules were used to adapt the output voltage from the control computer to a coil

current usable by the servo valves. The output voltages from the computer were generated by

three 12-bit digital-to-analog (D/A) converters. Gains for the servo amplifiers were adjusted

to produce the maximum coil current to each of the valves at the maximum output voltage of

2.5 volts from the D/A converters as listed in Table 4.1. The bandwidth characteristics for the

servo amplifiers and servo valves are also presented in Table 4.1.











Table 4.1. Servo amplifier and servo valve characteristics.

D/A ---- Servo Amplifiers ------
Output Gain Bandwidth
Toint (volts) (ma/volt) (rad/sec) (HZ)
0 2.5 80 1885 300
1 2.5 80 1885 300
2 2.5 16.8 1885 300

--------------- Servo Valves -----------
Max. Coil
Current Bandwidth
Joint Type (ma) (rad/sec) (HZ)
0 Pegasus 200 377 60
1 Pegasus 200 377 60
2 Sundstrand 42 189 30
note: Gain = Max. Coil Current / (D/A Output)


Software

The software environment for controlling the robot was written to run in a multi-user

PDOS operating system. The control system was organized into groups of sensing, action, and

control agents as presented in Figure 4.7. These agents exchanged information through a

dynamic database by way of result, parameter, and activate fields. The result fields stored

data which resulted from an agent's operation. Instructions for accomplishing an agent's

desired task were indicated in parameter fields. The ability to activate or deactivate certain

agents was included in activate fields within the database.

Sensing agents were those devices through which the work environment of the robot

and the robot states were quantified. Data from the sensing agents were filtered by fourth order

Integral of Time-multiplied Absolute Error (ITAE) data filters before their storage in the

database. The work environment or the sensors' perception of the work environment were

affected by the action agents. Control agents were used to direct the action agents to cause the

robot to move to a desired state. A desired robot state was characterized by setpoints which

were stored in the database and used by the control agents. For decreased calculation time, the

setpoints were specified in units compatible with the result fields rather than physical

measurements. An error calculator was used to compute the difference between the actual robot











WORK ENVIRONMENT





ACTION SENSING
AGENTS AGENTS

MODELSk
MOD S .. SUPERVISOR

CONOL DATA BASE
AGENTS --USER
SSHELLS
STATE I
NETWORK D
ERROR LOGGER
CALCULATOR

Figure 4.7. Organization of the robot programming environment.


state as perceived by the sensing agents and the desired robot state as prescribed by the control

agents in the setpoint fields. Results from the error calculator were also stored in the database

for used by the control agents.

Models characterized significant features and events within the work environment

through functional relationships which mapped sensing agent results and other system

information to boolean results. These models characterized certain conditions based on

information from the database. The models then returned boolean values to the database to be

used by the state network for directing the intelligence of the robot. The intelligence base for

picking fruit was programmed in the state network. This intelligence gave the robot the

ability to recognize and respond to events that commonly occurred during picking. The state

network mapped elements from the modeled response set to robot reactions through states

linked together by logical exit conditions. Each state in the network defined a different

modeled response or robot reaction. The responsibility of the state network was to cause the










robot to take the appropriate action to achieve its goal. The supervisor was responsible for the

orderly execution of the various system agents based on the intelligence programmed into the

state network. The connection between the developer and the database was accomplished

through user shells. These user shells were also the path for retrieving important database

information which was saved by a data-logger at a 60 HZ rate. Explicit details of this robot

programming environment can be found in Adsit (1989).


Picking Mechanism

In this section, the physical design of the picking mechanism is discussed. The

technique for mounting the picking mechanism on the sliding tube is presented along with the

method of mounting its actuation cylinder. The arrangement of fruit sensors and lights

incorporated in the end-effector are presented.

Construction

A picking mechanism (Figures 4.8 and 4.9) was developed with a rotating lip for

removing the fruit from the tree. The rotating lip of the picking mechanism was actuated so

that it encircled the fruit, impinging the stem of the fruit between the upper surface of the end-

effector and the rotating lip. Once the fruit was collected, the stem was broken as the end-

effector was retracted from the canopy of the tree. The detached fruit was secured with an

elastic collection sock as the end-effector was retracted and then dropped onto the ground or

into a collection bin.

The picking mechanism was constructed from a hollow aluminum cylinder (13 cm OD, 12

cm ID, 45 cm long). To aid in the ability of the end-effector to penetrate the canopy of a citrus

tree, its front end was machined on the top and bottom at an angle of approximately 34 degrees

from the centerline. A plastic collection scoop was added to the top of the picking mechanism

to aid in collection of the fruit as the rotating lip girdled it. The rotating lip was mounted to

the end-effector by shafts on either end which rotated in brass bushings. The lip was

constructed from spring steel formed into a slight oval profile. On the outer end of the shafts












Picking
Mechanism


Lights


Rear Drive
Sprocket


Tube
Centerline

Lens
Center


Rotating
Lip


Lens
Optical
Center


Figure 4.8. Cutaway side view of the picking mechanism showing the position of the
ultrasonic transducer, the color CCD camera, and the lights.




Rotating Lip
(Extended Position) Collection Sock
Bushing Bushing


Front Drive
Sprocket Front Drive
Sprocket

Collection Sp Picking Mechanism
Housing

Drive Chain Drive Chain

Lever Arm -- Rear Shaft
Bushing Bushing


Rear Drive
Sprocket Rear Drive
Sprocket

Cotter Key Adjustable
ILL Linkage


Figure 4.9. Top view of the picking mechanism showing the drive linkage and the lever arm
for actuating the rotating lip mechanism.


I










were mounted sprockets which were driven by a 25 pitch chain and a larger set of sprockets

providing a 3:1 drive ratio. The rear drive shaft, holding the larger set of sprockets, was

turned by a 5 cm long lever arm which converted the linear motion of the actuating cylinder to

rotary motion of the drive sprockets (Figure 4.10). A square aluminum linkage was connected

between the actuating cylinder and the lever arm. On the rear end of the sliding tube, the end

cap was bolted to the tube and used for mounting the actuator for the rotating lip picking

mechanism. On the forward end of the tube, an end cap was welded for mounting the end-

effector. Kinematic coordinate frame 3 was assigned to the end-effector (see Figure 4.1).

Fruit Sensor Package

The sensor package for locating the fruit in the canopy of the tree was mounted inside of

the tube of the picking mechanism (Figure 4.11). The sensor package included a color CCD

camera, an ultrasonic ranging transducer, and four small light sources and provided the real-

time three-dimensional position sensing between the end of the arm and a fruit in the

manipulator's workspace.

CCD camera. A Sony DXC-101 color CCD video camera with an 8 mm fixed focal length

and automatic aperture control lens was employed for detecting the fruit in the canopy of the

tree. The camera was mounted in the picking mechanism so that the optical axis of its lens was

approximately 25 mm below and parallel to the Z axis of the tube. The sensing element of the

camera was an 8.8 mm x 6.6 mm Charge Coupled Device (CCD) array. The CCD array was


Picking
Mechanism


Figure 4.10. Picking mechanism actuation assembly.











Bulb Lights
Retainer Ultrasonic

Lip Drive Lip Drive
Sprocket Sprocket

Light
Light
Plexiglas
Protector Color Camera
Collection Sock
Shield RotatingLip

Figure 4.11. Front view of the picking mechanism showing the color CCD camera, ultrasonic
transducer, and the four lights.


composed of a usable array of 384 pixels horizontally and 488 lines vertically. Since each

image corresponded to an acquired field, virtual resolution was reduced to 244 rows, with each

image using alternating even and odd rows. The image in the even rows (0,2,4,...) was processed

while a new image was being acquired in the odd rows (1,3,5,...). Integrated with the 8 mm

fixed focal length of the lens, the CCD gave the camera a field of view of 22.4 vertically and

28.80 horizontally as shown in Figures 4.12 and 4.13. The opening in the iris of the lens was

controlled by the control computer and based on the brightness of the targeted fruit (see

Slaughter, 1987). The NTSC color video information from the camera was decoded into red-

green-blue (RGB) format by a FOR.A, DEC-100 RGB color decoder. The video information was

processed by the control computer to obtain two-dimensional fruit-position data in real time (60

HZ).

Ultrasonic range sensor. The third dimension of the fruit, the distance from the picking

mechanism to the targeted fruit, was obtained by an Ocean Motion ultrasonic ranging

transducer. This transducer was equipped with a focusing horn to minimize reflection of the

ultrasonic burst from objects other than the targeted fruit. The transducer was positioned in the

picking mechanism above the camera and angled 10 degrees below the optical axis of the

camera so that the axis of the camera and the axis of the transducer intersected approximately

30 cm in front of the picking mechanism. The electronics provided with the ultrasonic ranging

transducer produced a TTL pulse with width proportional to the distance of the fruit from the










Camera Lens


Image Array


Figure 4.12. Horizontal field of view of the color CCD camera and lens combination in the
vertical plane.





8.8 mm Camera Lens




28.8*

28.8. Z4



8 mm
Image Array


Figure 4.13. Vertical field of view of the color CCD camera and lens combination in the
horizontal plane.


picking mechanism. This signal was quantified by a counter-timer module in the control

computer.

Lights. Four small flashlights were used as supplemental light sources. These lights

were needed to illuminate a targeted fruit as the arm was extended to the fruit and the picking

mechanism shaded the ambient light from the fruit. A pair of AA size flashlights were

secured on either side of the camera and powered by a separate 5 volt power supply.










Hydraulic Actuation

Joint motion was accomplished by hydraulic actuators controlled by servo valves. A

pressure compensated hydraulic power unit (Figure 4.14) provided hydraulic power to the

actuators. A 1 liter accumulator, charged to 7000 kPa, was added to help maintain constant

hydraulic pressure. Bird-Johnson SS-2-100 rotary actuators were utilized for actuating joints 0

and 1 (Figure 4.15). These actuators had splined shafts which interlaced with the splines that

were machined into the steel spindles that served as mounts for the outer and inner links.

Control of the fluid flow to these two actuators was achieved by Pegasus 3.8 liter-per-minute

(1.0 GPM), 120 series servo valves. The servo valves were connected to the actuators with

hydraulic manifolds which provided cross port pressure relief. Motion of the prismatic joint 2

was performed by a Hartmann HT-5 hydraulic motor (Figure 4.16) which was mounted to the

inner link of the robot. This motor was connected to the drive shaft of the pinion gear by a

Lovejoy coupling. It was controlled by a 18.9 liter-per-minute (5.0 GPM) Sundstrand MCV 103A



Charged to 6900 kPa
Lip Actuator
Axis2
Axis 1
Axis 0

Pressure Manifold
Return Manifold Type 'T'
Thermocouple

0 |Axis2
5 micronT Preset at Axis 1
filter 17000 kPa A-- is 0

o10 micron filter

14000 kPa Weeage
Weepage
3.7 kW Motor drain lin.. To Hartmann
1750 rpm -- case drain
1750 rpm O SM -M---. I I case drain


Figure 4.14. Circuit diagram of hydraulic power unit.










Bird-Johnson
SS-2-100 rotary
actuator


Hartmann
HTF-5
Relief valves Manifold hydraulic
set at Block motor
21000 kPa Assembly









servo valve servo valve

SFrom Pressure
To Return From Pressure Manifold
Manifold Manifold ToReturn
Manifold

Figure 4.15. Circuit diagram of hydraulic Figure 4.16. Circuit diagram of hydraulic
actuators for joints 0 and 1. motor actuator for joint 2.


series servo valve. The Sundstrand valve was mounted to the motor by a manifold block that

also provided for a case drain to the motor. Actuation of the rotating-lip picking mechanism

(Figure 4.17) was accomplished with a double-acting hydraulic cylinder (Allenair, 3500 kPa,

2.22 cm bore x 5.08 cm stroke) which was controlled by a 3-way solenoid valve energized by 120

volt AC current. Two solid-state relays were utilized to control the AC current. A pressure

reducing valve was used to reduce the hydraulic pressure to 3500 kPa for the actuating cylinder.


Position Sensors

The kinematics of the robot manipulator was developed with the understanding that

the position of each of the joints would be known. Control of the manipulator was based on the

ability to calculate the difference in the actual position from the desired position of each joint.

Rotary potentiometers were used to sense the actual position of each joint at any given time

(Figure 4.18).

The position of each of the joints of the robot was sensed by 2k ohm potentiometers

directly connected to the actuators. For joints 0 and 1, Spectrol single turn potentiometers were











Allenair double acting
hydraulic cylinder
2.22 cm bore
5.08 cm stroke







3-way solenoid valve



STo Return
SManifold


Pressure reducing
valve set to 3400 kPa

L ---J



From Pressure
Manifold

Figure 4.17. Circuit diagram of picking mechanism lip actuator.


chosen for their high precision and linearity. In both cases, the potentiometer body was

mounted to a plate which was attached to the actuator. The shaft of the potentiometer was

connected directly to the back side of the moving shaft of the actuator. A voltage was applied

across each of the potentiometers. The voltage across the wiper of the potentiometer was

proportional to the angular position of the rotary actuator and thus the position of the axis.

This voltage drop was quantified by a 12-bit, 5 volts analog-to-digital (A/D) converter.

On joint 0, the potentiometer was required to relate a total motion of approximately 78

degrees to a voltage ranging between -5 volts and +5 volts. The potentiometer was supplied

with +5 volts to the positive side and -12 volts to the negative side creating a voltage

difference of 17 volts across the 360 degrees of the single turn potentiometer. The result was a

resolution of approximately 19 bits per degree of rotation of the potentiometer wiper shaft

(0.0520/bit). A summary of the range of motion for joint 0 and the A/D bits read for some critical

positions is presented in Figure 4.19.










Pot + Supply
Pot Supply
Pot + Return


Pot Return


Toint 0
variable:
potentiometer:
+ supply:
supply:
Toint 1
variable:
potentiometer:
+ supply:
supply:
Toint 2
variable:
potentiometer:
+ supply:
supply:


PO
Spectrol, 2K, Single turn, 132-0-0-202
+5 V
-12 V

P1
Spectrol, 2K, Single turn, 132-0-0-202
+12 V
-12 V

P2
Bourns, 2K, Ten turn, 536
+5 V
GND


Figure 4.18. Circuit diagram of the position sensing potentiometers.


The potentiometer on joint 1 converted a total motion of approximately 46 degrees to a

voltage that could be used by the A/D converter. Because of the smaller motion of joint 1, a

larger voltage drop across the potentiometer was needed to increase the resolution. The

positive side of the potentiometer was connected to a +12 volts supply while the negative side

was connected to a -12 volts supply. This 24 volt difference across the 360 degrees of the

potentiometer resulted in a change of approximately 3.15 volts through the joint's 46 degree

range of motion. For this joint, the resolution was approximately 27 bits per degree or 0.037

degree per bit. The summary of the range of motion and the critical positions for joint 1 is

presented in Figure 4.20.

For determining the position of the sliding joint of the robot, a Bourns 10 turn, 2k ohm

potentiometer was used. In order to move the full length of the tube, the sliding joint actuator

















A/D bits 3072 Position of
\-38.7 Joint 0




3819 down



Position of Computer
joint 0 A/D Value Description
+38.7' 2324 upper mechanical limit
+34.6' 2400 low
+29.3' 2500 min
-1.6' 3100 home
-32.7' 3700 max
-37.8' 3800 high
-38.7' 3819 lower mechanical limit


Figure 4.19. Relationship between position of joint 0 and the A/D value of the position of
joint 0.


was required to make almost 11 complete revolutions. For measuring the position of the tube

with the Bourns potentiometer, a gear reduction was necessary to keep the motion within the

allowable 10 turns. This potentiometer was connected to the shaft of the Hartmann motor by

way of a small set of miter gears with a drive ratio of 2/3 turn of the potentiometer per

revolution of the motor. The 2:3 gear reduction caused the potentiometer to turn 7.3 revolutions

over the entire length of the tube. The positive supply voltage of this potentiometer was

connected to a +5 volts source while the negative supply was tied to ground. Again, a 12-bit

A/D converter was used to convert the analog voltage to a digital value that could be used by

the control computer. This set-up yielded a resolution of approximately 11 bits per centimeter

























Position of Joint 1


Position of Computer
Toint 1 A/D Value Description
67.0' 2518 left mechanical limit
69.9" 2600 low
73.5' 2700 min
88.7' 3125 home
103.8' 3550 max
107.4" 3650 high
113.0' 3807 right mechanical limit


Figure 4.20. Relationship between position of joint 1 and the A/D value of the position of
joint 1.


of linear travel (0.088 cm per A/D bit). A range of motion summary for joint 2 is presented in

Figure 4.21.

Even though the calculations for the resolution of the position sensors show high

sensitivity, other components also effect the precision of the measured positions. Variations in

the power supply for the entire Florida Laboratory influence the output voltage from the

voltage supply to the potentiometers. These fluctuations along with noise in the connections to

and from the potentiometers add error to the signals from the position sensors. Due to these and

other factors effecting the accuracy of the measurements, the accuracy of the revolute joint

position sensors was assumed to be limited to 05 degree. Likewise, the assumed accuracy of

the prismatic joint was limited to 0.5 cm. These limitations seemed necessary but did not










Picking
Mechanism-
Picking r Pivot for Joint 0 Actuator
Mechanism -




I I i I
A B


---- Robot Support



Actual Position Computer
A (cm) B (cm) A/D Value Position
131.5 0 2321 fully extended (to stop)
129.0 2.5 2350 low
124.5 7.0 2400 min
54.5 77.0 3200 home
10.5 121.0 3700 max
2.0 129.5 3800 high
0 131.5 3821 fully retracted (to stop)


Figure 4.21. Relationship between position of joint 2 and the A/D value of the position of
joint 2.


significantly reduce the ability to control the position of the orange-picking manipulator. A

summary of the conversion factors for converting A/D values to actual positions of the joints is

presented in Table 4.2.


Velocity Sensors

Effective control of the robot required a direct reading of the velocity of each joint. The

velocity of the joints was monitored by the use of DC tachometers (Figure 4.22) mounted to the

actuator or linkage of each of the joints. The voltage output from each of the tachometers was

quantified by 12-bit A/D converters with an input range of 5 volts. This range gave a digital

resolution of 409.6 bits per input volt from each of the tachometers.











Tachometer +




Tachometer -


Toint 0
variable:
tachometer:
output:
drive ratio:
filter gain:
Toint 1
variable:
tachometer:
output:
drive ratio:
filter gain:
Toint 2
variable:
tachometer:
output:
drive ratio:
filter gain:


V0
Servo-Tek SU-780D-1
45 volts per 1000 RPM
1:1
10

V1
Servo-Tek SU-780D-1
45 volts per 1000 RPM
1:1
10

V2
Servo-Tek SA-740A-2
7.0 volts per 1000 RPM
1:1
5


Figure 4.22. Circuit diagram of the tachometers.


For joints 0 and 1, Servo-Tek D series tachometers were chosen (Figure 4.22). The shaft

of each tachometer was directly connected to the mounting shaft of the link so that any motion

of the link created a voltage across the tachometer. Using 12-bit A/D converters and a software

filter with a gain of 10, the resulting resolution of the tachometers was 30.7 bits per degree per

second (0.033 degrees per second per bit). The velocity of the sliding tube was determined by a

Servo-Tek model SA-740A-2 tachometer. The 740A tachometer was mounted on the back side of

the Hartmann motor but connected to the motor's shaft through the 1:1 miter gear. This

mounting arrangement allowed the linear velocity of the sliding joint to be measured by the

angular velocity of the tachometer. A 12-bit A/D converter and a software filter with a gain of

5 were used to quantify the tachometer voltage. The resolution of the velocity sensor for the







44


sliding tube was 71.9 bits per cm per second (0.014 cm per second per bit). A full summary of the

velocity conversion factors is presented in Table 4.2.


Table 4.2. Conversion factors for changing A/D information to actual positions and velocities
of the robot's joints.

Conversion Factors
Toint Position Velocity
0 0.052 deg/bit 0.033 (deg/sec) / bit
1 0.037 deg/bit 0.033 (deg/sec) / bit
2 0.088 cm/bit 0.014 (cm/sec) / bit















CHAPTER 5
ROBOT KINEMATIC MODEL


The kinematic relationships between the joints and links of the robot are presented in

this section. In the field of robotics, homogeneous transformations are used to describe the

position and orientation of one link coordinate system with respect to another one. By

describing the position and orientation of a coordinate frame which is assigned to a link of the

manipulator, the homogeneous transformation describes the position and orientation of the link

itself. According to Paul's method (1981), the product of these homogeneous transformations

(called a T matrix) will be used to calculate the position values of each joint necessary to place

the final coordinate frame (corresponding to the robot camera) at a given position and

orientation in the robot's workspace. The inverse of this T matrix will be useful in calculating

the position and orientation of the camera coordinate frame when given the joint variable

positions. A vector will be defined to relate the position of the centroid of a fruit in the camera

coordinate frame. The T matrix will then be used to relate the position of the fruit to the origin

of the base frame of the manipulator. A similar kinematic relationship will be used to relate

the position of a fruit in the robot's workspace to the position of the fruit's image in the

imaging array of the CCD camera. After establishing this relationship, the information will

be used to present the change in position of the fruit in the imaging array for small changes in

the joint angles of the robot. The vision-servoing gains will be established for adjusting the

gains of the vision control system based on the position of the fruit and position of the robot joint

angles.


Manipulator Kinematics

The citrus-picking robot consisted of three joints which were numbered 0, 1, and 2. Joints

0 and 1 were revolute with intersecting and perpendicular axes of rotation (Zo and Z1,

45










respectively). Joint 2 was prismatic which slid about its Z2 axis that passed through the

intersection of axes 0 and 1. A robot with this order and configuration of the joints and links is

referred to as a spherical-coordinate manipulator.

The order of assignment of the coordinate frames on the orange-picking robot is

presented in Figures 5.1 and 5.2. The first coordinate frame (frame 0, labeled Xo, Y and Z )

was assigned to the stationary base of the robot. Frame 1 was attached to the outer link and

positioned to rotate about Zo an angle 81. The third frame (frame 2) was designated to the inner

moving link (link 2) of the robot and allowed to rotate 82 about Z1. The final coordinate frame

(frame 3) was assigned to the sliding tube of the robot and fixed to have linear motion along

axis Z2 which was coincident with Z3. This final frame was positioned in the end-effector of

the robot so that its origin corresponded with the optical lens center of the robot camera. This

arrangement caused the final coordinate frame of the manipulator and the camera's coordinate

frame to be coincident.

For the orange-picking robot, the joint parameters were specified as variables for

rotation and translation about the axes of motion and as constants to establish the fixed



frame 0

Yo, Z, Y2





frameframe 2 frame 3,






camera frame
Xo/, X ^^





frame 2 frame 3,
camera frame


Figure 5.1. Kinematic frames of the three degree-of-freedom robot.










Yo, Z1,


Outer Link


Sliding Tube


Z2 X3, Xc

Z3, Z,

Base \ Z31 Zc
Y1 Camera
Frame



Inner Link

d Base


Figure 5.2. Kinematic frames as assigned to the links of the robot.


relationships between the successive coordinate frames. The joint variables and geometry

constants for the orange-picking robot are listed in Table 5.1. Based on these parameters, the

following A matrices were determined

C, 0 -S, 0
0A S1 0 C1 0
0 -1 0 0
0001
0 0 0 1 (5-1)


C2 0 S2 0
12 S2 0 -C2 0
A2
0 1 0 0
0 1001 and (5-2)
0001
,and (5-2)









Table 5.1. Link parameters for the orange-picking robot.

Axis of Joint
Link Motion Variable a d a a
1 Zo 81 81 0 0 -900
2 Z12 4 0 0 900
3 Z2 d3 0 d3 0 0



01000
1 0 0 0
2A3 0 1 0 0
0 0 1 da
0 0 0 1 (5-3)

where Si = sin Oi, Ci = cos Oi, and d3 is the distance along Z2 from the axis of rotation Z1 to the

origin of frame 3. Multiplying these A matrices, the position and orientation of the image

plane of the manipulator's vision system with respect to the base was established as T3:

C1 C2 -S, C1S2 C1S2d3
T 1 2 SlC2 C1 S1S2 S1S2d3
T3= A2 A3 -S2 0 C2 C2d3

0 0 0 1 (5-4)

The orientation of Z3 with respect to the robot base frame (frame 0) was determined by the 3 x 3

matrix in the upper left-hand corer of T3. The three column vectors of the 3 x 3 matrix, from

left to right, defined the direction of the X3, Y3, and Z3 axes, respectively. The fourth column

vector of the T3 matrix defined the position of the origin of the camera frame (frame 3). Due to

the configuration of the manipulator, this position vector, [ C1S2d3 S1S2d3 C2d3 1 ]T, was

always colinear with the Z3 = [ C1S2 S1S2 C2 1 T vector of the camera frame.

Due to the importance of these position vectors in establishing the imaging and vision-

servoing kinematics, it was also important to understand the configuration of the robot for the

possible values of the joint variables. First, let all of the joint variables be equal to 0. In this

case, all of the coordinate frames of the robot had a common point as their origins. Frames 0, 2,

and 3 were completely coincident with common X, Y, and Z vectors. Frame 1, on the other hand,

was positioned so that X1 and Xo, Y1 and -Zo, and Z1 and Yo were coincident. As a result, if all of










the variables could be set to 0, the robot would be configured so that the outer link would be

parallel with the supports of the base. The inner link would be positioned at a right angle to

the outer link so that the sliding tube would point through the actuated mounting shaft of the

outer link. The sliding tube would be retracted so that the axis of the inner link would pass

through the lens of the camera in the end-effector.

However, the action of the orange-picking robot was limited by the mechanical stops

on each of the joints. In addition, these mechanical stops restricted the boundaries of the

individual parameters which governed the motion of the coordinate frames. The range of

motion for each of the joint variables is shown in Table 5.2. These ranges of motion for each joint

defined the working envelope of the orange-picking robot.


Table 5.2. Range of motion for the joint parameters of the orange-picking robot.

Range
Parameter Minimum Maximum
91 -390 +390
92 +670 +1130
d3 +51.0 cm +182.5 cm


Imaging Kinematics

Vision-servoing of the robot required that the relationship between actual position of a

fruit and the position of the fruit's image in the imaging array of the camera be established

(see Figure 5.3). For this derivation, the position of the fruit was defined by a vector, pc = [ x y

z 1 ]T, from the origin of the camera coordinate frame to the centroid of the fruit. The position

of the fruit's image was defined by another vector, pi = [ xi i zi 1 ]T, from the camera coordinate

frame to the centroid of the fruit's image in the imaging array.

The CCD imaging array of the camera was parallel to the X-Y plane of the camera

frame and positioned in the negative Z direction from the origin of the frame the distance of

the focal length of the lens, zi = -f. The position of a fruit's projection in the image plane, pi,

was related to Pc, its actual position, by


Pi = Tp pc


(5-5)










YC

XC
^y 4 x,





SOC z Z

Lens




Figure 5.3. Lens imaging geometry.


where

-/ 0 0 0
'z
0 J/ 0 0
T= z
0 0 -f/
0001
(5-6)

pc =[ x y z 1 F,and

pi= x, yi zi 1 F.

Calculating xi and yi, the relationship between the actual position of the fruit and the position

of the fruit's projection on the image plane was established,

xi = x (-f/z), and

yi = y (-f/z)

A change in the fruit's position in the camera frame resulted in a position change of the image

in the image array scaled by the -f/z factor.


Vision-Servo Kinematics

In the end-effector of the robot, the camera was mounted in such a way that its optical

axis was coincident with Z3. The later addition of the ultrasonic ranging horn required that the










position of the camera be lowered by a small amount. This amount was very small and

considered negligible in the further derivation of the vision geometry. The camera was

assumed to be positioned so that the center of its lens was at the origin of frame 3. Therefore,

the coordinate frame of the camera was the same as frame 3.

The relationship between the position of the fruit in the camera frame, Pc, and the

position of its image on the imaging array, pi, was important in formulating the effects of

changes in the position of the manipulator's joints on the position of the image. To vision-servo

the robot, it was necessary to develop closed-form solutions that related changes in 81 and 82 to

changes in xi and yi. These relationships were referred to as vision gains and expressed as

K -dyi
=d8 and (5-7)

dxi
Kvx=
d22. (5-8)

Because the position of the fruit was assumed to remain the same regardless of the

configuration of the robot, it became necessary to define the position of the fruit with respect to

the stationary base frame (see Figure 5.4). Using the T3 matrix to specify the position of the

fruit in the coordinates of the base frame, po, was established as


Po= T3Pc. (5-9)

Utilizing this relationship, the position of the fruit in the camera coordinate frame was

determined to be

-1
Pc= T3 Po. (5-10)

Substituting pc into equation 5-5, the position of the fruit's projection in the image array, pi, was

found


-1
Pi= TT3 PO.


(5-11)




























Figure 5.4. Coordinate frame representation of the camera frame and the fruit position with
respect to the base frame.


where

C1C2 S1C2 -S2 0
-1 -S Ci 0 0
C1S2 S1S2 C2 -d3
0 0 0 and (5-12)

po- the position of the fruit in the base frame coordinates.

The derivation of the vision gains began by assuming the manipulator was initially

aligned with a fruit which was located at some position po in the base frame coordinates and

some distance z from the camera. The joints of the manipulator were located at some position

81, e2, and d3 when the fruit was aligned. This arrangement caused the fruit to lie along Zc at a

radial distance (d3 + z) from the origin of the base frame. Therefore, po was expressed as

p (d3+z)C1 S2
P (d3+z)S1S2
(d3+ z)C2
1 (5-13)


Substituting equation 5-13 into equation 5-11 and solving for pi,









-1

Pi=TpT 3o= .f

(5-14)

xi and yi were found to be 0 as expected for the aligned position of the fruit. Also as expected, zi

was found to be the negative value of the focal length of the camera and lens (zi = -f).

Assuming that the manipulator position was changed to some position (e1 + dO1, 82 + d%2, d3) by

rotating joints 1 and 2 by some very small increments de1 and d82, the position of the fruit in the

image array was offset from the center by incremental amounts dxi and dyi. Because of the

increments were very small, the second order terms of d81 and d@2 were assumed to be 0. Also,

for small dEi, sin(di) dOi and cos(d)i) = 1. Solving equation 5-11 with the new T3-1 (equation

5-12) for pi yielded

d f de2(d3+z)
z and (5-15)

f dOe S2(d3+z)
dyi=
z (5-16)

Solving for the vision gains resulted in

vxf + 1
K=f z ( and (5-17)


K Y=f S2 d3+1
f +1). (5-18)

The vision gains were approximately proportional to d3 and inversely proportional to z.

Therefore, during a pick cycle, as the picking mechanism was extended toward the targeted

fruit, smaller motions of the two revolute joints were required to compensate for misalignment of

the robot with the targeted fruit. Additionally, Kvy was proportional to the sine of 02. This

term indicated that smaller adjustments in e) were required to compensate for the misalignment

of the robot and the fruit as the position of 02 moved to greater distances from 90.

Another important indication from this kinematic analysis of the relationships among

the position of the joints of the robot, the position of the camera frame, and the position of the







54


fruit was that the vision-servo problem was geometrically decoupled. In other words, changes

in 81 did not affect xi, and changes in 02 did not affect yi. This decoupling was indicated by the

absence of a dO1 term in equation 5-15 and the absence of a de2 term in equation 5-16. The

geometry contributed to the decoupling effect allowing the two-dimensional vision control

problem to be treated as separate problems, greatly simplifying the development of the control

algorithms.















CHAPTER 6
ROBOT OPEN-LOOP DYNAMICS


In this chapter, the open-loop dynamic models of the robot are presented. The joint

components of a generic electrohydraulic servo system and their interaction are discussed first.

The mathematical form of each of the components is presented. The open-loop model is then

reduced to a representative model which approximates the dynamic response of a servo system

and can be used for selecting and tuning control algorithms. Open-loop step tests of the robot

joints are then presented and used to estimate model parameters. Simulated responses to step

inputs are compared to the responses of the actual joints to step inputs for model and parameter

verification.


Background

Merritt (1967) discusses the design of control systems for electrohydraulic servo

mechanisms. He derives a mathematical model for each of the components of the generic

electrohydraulic servo system. An open-loop servo system (Figure 6.1) includes servo amplifier

dynamics, servo valve dynamics, external torque load dynamics, and actuator and load

dynamics. In his presentation, Merritt points out that the servo amplifier, servo valve, and no-

load system all respond as second-order systems. Merritt discusses the selection of components

of the servo system stressing the fact that the bandwidth of the servo amplifiers and servo

valves should be higher than that of the selected actuators. By choosing fast servo amplifiers

and valves, their dynamics can be reduced to a single gain as in Figure 6.2. In this case, the

open-loop transfer function for the velocity control servo system of each joint of the orange-

picking robot is approximated as a second-order system:











inD/A C Servo Amp Servo Valve Load ac o et
converter I Dynamics I Dynamics |- Load Tach | Converte--1-

Figure 6.1. Open-loop hydraulic servo system.



D/A
word liter deg
t(bits) g-7-"v-"vtq Servo Amp/ [iec Actuator and _sec ----v-olts -bits
Signal Converter Sero ADnas aValve ad Converter| Signal

Figure 6.2. Reduced block diagram of the open-loop system.


2
Au(s) = Kv- -
2 2
s +28h(OhS + (Oh (6-1)


where Au(s) = open-loop transfer function ((deg/sec)/(D/A word)),

s s Laplacian operator (sec-),

Kv = open-loop gain of the system ((deg/sec)/(D/A word)),

(h open-loop hydraulic natural frequency (rad/sec), and

6h = open-loop damping ratio dimensionlesss).

In this system, the open-loop gain includes the gains of the D/A converter, the servo amp/servo

valve combination, and the actuator and load dynamics.

The hydraulic natural frequency and the damping ratio are functions of parameters of

the fluid and the mechanical components of the controlled system and can generally be

approximated as

4 eDm
Wh= V
VVt t (6-2)


8h e(6-3)Pe
Dm V V (6-3)










where: pe = bulk modulus of hydraulic fluid (N/m2),

Dm volumetric displacement of rotary actuator or motor (ml/rad),

Vt total compressed volume of fluid (ml),

Jt M total inertia of load and actuator (N m sec2), and

Kce a servo valve flow-pressure coefficient (ml/sec/N/m2).

Each of these parameters can vary during operation. However, by keeping the temperature of

the hydraulic fluid constant during operation of a servo hydraulic system, the bulk modulus (Pe)

of the fluid can be considered constant. As rotary actuators and motors rotate, the volume of

fluid in one chamber decreases while the volume in another chamber increases. Since the total

volume of fluid in the actuators and motors changes only very small amounts due to pressure

differential during operation, the volumetric displacement (Dm) can also considered to be

constant. Likewise, if the total compressed volume of fluid (Vi) in the system changes only very

small amounts if any during operation, it can be considered constant. Because the quantities, 3e,

Dm, and Vt, are fairly constant, they cause only minor changes in the values of oh and 8h during

operation of the servo hydraulic system.

Another important effect on the performance of an electrohydraulic servo system is the

external torque load on the output shaft of the actuator as shown in the block diagram of Figure

6.3. External loads on the system must be accounted for in the derivation of the open-loop

transfer function. In the case of the orange-picking robot, this external load was altered by the

position of the joints 0 and 2. When the sliding joint was moved from a balanced position, an

external torque was added to joint 0 actuator. This torque was proportional to the position of

joint 2 with positive and negative maximum values occurring when the tube was extended or

retracted, respectively. When joint 0 was moved from a horizontal position, an external load

was induced on the motor which actuated joint 2 by the pull of gravity on the sliding tube. This

load was greatest when joint 0 was positioned to its maximum or minimum rotation. Each of

these loads affected the operation of the robot.










External
Torque Load
TL External
Torque Load
N.m Dynamics

D/A
word liter deg
Signal CoG + Dynami Converter Signal

Figure 6.3. Block diagram of the open-loop system with external torque load.


During critical performance times for the citrus-picking robot, the tube was already

extended toward the fruit. During these periods, motion of the sliding tube was so small that

changes in the torque load caused by its changing motion were considered negligible. Therefore,

the dynamics of the external torque on joint 0 were also considered constant. Similarly, changes

in position of the sliding tube were required only during occasions in which motion of joint 0 was

very small or nonexistent. As the end-effector was extended toward a targeted fruit, the

position of joint 0 was controlled by the vision system to follow the small vertical motion of the

fruit. This vertical fruit motion was very small and resulted in very small and negligible

position changes in joint 0. While joint 2 was being retracted from the tree, joints 0 and 1 were

held practically motionless to prevent manipulator damage caused by collisions with large

limbs or other obstacles. Therefore, the external touque load on joint 2 due to changes in the joint

0 position was also constant during the motion of joint 2. In other words, the external torque

loads that affected joint 2 could be considered constant during any operation of the sliding joint.


Parameter Estimation

In the general case, the actuator and load dynamics is the limiting factor for the

response of a servo system. As Merritt points out, servo amplifiers and valves should have

faster response rates than the actuator and load combinations. In this case, the dynamic

response characteristics of the servo amplifiers and servo valves can be neglected. In the case of

the orange-picking robot, servo amplifiers and servo valves were chosen with this criteria in

mind. All three of the servo amplifiers had bandwidths of 1885 rad/sec (300 HZ) (Table 4.1).










Also, servo valves with bandwidths of 377 rad/sec (60 HZ) for joints 0 and 1 and 189 rad/sec (30

HZ) for joint 2 were used. Equation 6-2 was used to approximate the hydraulic natural

frequencies for the joints of the robot. For the revolute joints, the maximum natural frequency

was estimated to be approximately 28 rad/sec which was classified as slow when compared to

the response rate of the actuators. Assuming that the sliding tube was a simple mass for the

motor actuator, a natural frequency of 67 rad/sec was estimated. Not included in this

estimation was the decrease in the response of the slider which developed as a result of the

friction in the bearing. Therefore, the response of the chosen actuator for joint 2 was adequate to

be considered fast enough to be neglected in the further analysis.

Theoretically, the steady-state gains, hydraulic natural frequencies, and damping

ratios could have been calculated from the known characteristics of the manipulator.

However, for the orange-picking robot, such parameters as link mass and mass moment of

inertia as well as some hydraulic fluid characteristics which affected the actuator and load

dynamics were unknown. Also, flow characteristics of the servo valves were undeterminable.

Therefore, the steady-state gain, hydraulic natural frequency, and hydraulic damping ratio

for each joint were determined experimentally. The experimental tests consisted of actuating

each joint by stepping the reference voltage to the servo amplifier and measuring the joint

response. These open-loop tests were conducted on each joint individually. For each joint, the

computer was programmed to signal the servo amplifier to send a current to the servo valve

holding it opened for a given amount of time while the computer recorded the velocity and

position of the joint as it moved. A routine was added to the user interface of the robot control

program to obtain a description of the step from the user and send the appropriate control signal

to the D/A board. The step was maintained until the joint reached a constant velocity or the

position of the joint reached a specified maximum or minimum. The valve was then closed to

stop the action and bring the joint to rest.

Typical graphs for step test results are shown in Figures 6.4, 6.5, and 6.6 for joints 0, 1,

and 2, respectively. Information from the graphs was used to estimate the gain, the hydraulic










natural frequency and the hydraulic damping ratio for each joint. The steady-state gain of a

joint was the ratio of the achieved steady-state velocity to the input value. The gain for each

joint was determined as

K Vout
K,-
V*P (6-4)

where: Kv a steady-state gain ((deg/sec)/(D/A word)),

Vout steady-state output velocity (deg/sec), and

Vsp = velocity control signal (D/A word).

The damping ratio was approximated with (Palm, 1983):



In ln(os) )2
Sh= ( I
2
1+ (In (os))
-IC ) (6-5)

where: 5h hydraulic damping ratio dimensionlesss), and
V max-V
os overshoot (decimal value) Vmax
V sa

The hydraulic natural frequency was approximated with (Palm, 1983)



tPv l- (6-6)

where oh = hydraulic natural frequency (rad/sec),

kh = damping ratio dimensionlesss), and

tp = time to peak (sec).

A typical response of joint 0 of the orange-picking robot to a step input is shown in

Figure 6.4. The response signals from the A/D converters were converted from the bit values as

used by the control computer to deg/sec by the conversion factor 0.033 (deg/sec)/bit from Table

4.2. For a step input to the actuator of 1200 D/A bits, the joint started from a velocity of 0

deg/sec (0 A/D bits) and peaked at a velocity of 65 deg/sec (1980 A/D bits) before returning to

an average steady-state velocity of 46 deg/sec (1400 A/D bits). Thus, the steady-state gain for







61


80

60-

40-

I 20



-20-

40 l i i i --- i i .
0 15 30 45 60 75 90 105 120
Time (1/60 sec)

1500

10


so-


0 15 30 45 60 75 90 105 120
Time (1/60 sec)

Figure 6.4. Typical response of joint 0 of the orange-picking robot to a step input of 1200 D/A
bits with joint 2 centered in the Hooke joint.


this system was calculated using equation 6-4 as approximately 0.038 (deg/sec)/(D/A word).

These values represent an overshoot of 19 deg/sec (580 A/D bits) which corresponds to 41

percent. Using this measured overshoot in equation 6-5, a damping ratio of 0.27 was calculated.

The time to peak was determined by finding the number of counter ticks required for the

velocity of the joint to reach its maximum and dividing by the scale factor, 60 ticks per sec. In

this case, the joint reached its maximum velocity in 10 counter ticks or 0.17 sec. This value along

with the already determined damping ratio was used in equation 6-6 to find the hydraulic

natural frequency of the joint of 20 rad/sec or 3.1 HZ.

Open-loop step tests were conducted on joint 0 for different directions of motion and

different positions of joint 2. The tests were conducted with the tube extended, in the center of

its working range, and retracted. Repetitions of the tests included step inputs that caused the

end-effector to be moved down from a raised position and up from a lowered position, or in









response to positive and negative steps. The results of the repetitions were averaged. The

average steady-state gains, hydraulic natural frequencies, and damping ratios are presented in

Table 6.1.

A typical response of joint 1 of the robot to a step input of -500 D/A bits is shown in

Figure 6.5. This step caused the joint to move the end-effector of the robot from left to right.

The step input was initialized at 1/2 sec (30 counter timer ticks). The joint which began from a

velocity of approximately 0 deg/sec (0 A/D bits) responded to the input by reaching a peak

velocity of -38 deg/sec (-1150 A/D bits) before returning to an average steady-state velocity of

-28 deg/sec (-850 A/D bits). The values for this step test represent an overshoot of -10 deg/sec

(-300 A/D bits) or 35 percent of the steady-state velocity. Joint 1 reached its maximum or peak

velocity in 7 counter timer ticks or 0.12 sec. Using these values in equations 6-4, 6-5, and 6-6, the

hydraulic natural frequency and damping ratio of joint 1 were found to be 28 rad/sec (4.5 HZ)

and 0.32, respectively. The steady-state gain, Kv, for joint 1 was found to be

0.056 (deg/sec)/(D/A word).

For step test response of joint 1, the position of joint 0 was held constant while joint 1

was free to respond to the step input from the control computer. These tests included both

positive and negative step inputs from the computer resulting in horizontal motions of the end-

effector. The test was also conducted in repetition for joint 2 in the retracted, centered, and


Table 6.1. Experimentally determined steady-state gains, damping ratios, and hydraulic
natural frequencies of joint 0 in relation to position of joint 2 and direction of
motion.

steady-state
joint 0 gain hydraulic hydraulic natural
position direction (deg/sec) damping frequency
of joint 2 of motion (D/A word) ratio rad/sec HZ
retracted down 0.035 0.30 33 5.2
up 0.042 021 22 3.4
centered down 0.033 0.18 17 2.8
up 0.038 0.26 20 3.1
extended down 0.044 0.31 17 2.8
up 0.040 0.34 27 4.3

















-10

S-20-

-30

-40
0 15 30 45 60 75 90 105 120
Time (1/60 sec)



"a 0

| | -200-

400-


0 15 30 45 60 75 90 105 120
Time (1/60 sec)

Figure 6.5. Typical response joint 1 of the orange-picking robot to a step input of -500 D/A bits
with joint 2 centered in the Hooke Joint.


extended positions. The averaged results of the repetitions of the open-loop step test of joint 1

are presented in Table 6.2.

Step tests of joint 2 gave undesirable results. When the sliding joint was actuated with

a step input, the joint responded with a quick jolt and then settled to an increasing velocity as

the valve was held opened. The sluggish response of the joint did not reach a steady or peak

velocity before the full length of the tube was used at which time the step input was

terminated. A typical response of joint 2 to a step input is shown in Figure 6.6. From the

information presented in the figure, the defining parameters for the system were not

determinable. However, important observations were made which aided in developing a

controller for the sliding joint. Due to the static friction between the nylon bushing and the

aluminum tube, the input required to start the tube moving was much larger than that required

to keep it moving. Small control values would not cause a change in the joint's position. On the









Table 6.2. Experimentally determined steady-state gains, damping ratios, and hydraulic
natural frequencies of joint 1 in relation to position of joint 2 and direction of
motion.

steady-state
joint 1 gain hydraulic hydraulic natural
position direction (deg/sec) damping frequency
of joint 2 of motion (D/Aword) ratio rad/sec HZ
retracted negative 0.054 0.29 28 4.5
positive 0.055 0.18 32 5.1
centered negative 0.055 0.32 28 4.5
positive 0.055 022 28 45
extended negative 0.056 0.31 22 3.5
positive 0.058 0.26 22 3.5


other hand, the response of joint 2 to large control values would increase stiffness requirements

for positioning the joint.


Results and Discussion

Design of the control systems for the orange-picking robot required a knowledge of the

each joint's transfer function. Merritt (1967) suggests that the transfer function of most

electrohydraulic servo systems can be estimated as a second-order system. Open-loop step tests

were conducted on each of the robot's joints for verification of Merritt's assumptions and for

establishing the parameters which define the behavior of a second-order system. After

defining the systems' parameters, the second-order systems were simulated for comparison with

the actual systems and verification of the determined parameters.

Analysis of the open-loop step tests responses of joint 0 for various positions of the

sliding tube yielded a range of hydraulic natural frequencies and damping ratios. Repetitions

of the step tests suggested average hydraulic natural frequencies ranging from 17 to 33 rad/sec

(2.8 to 5.2 HZ) and average damping ratio ranging between 0.18 and 0.34 (Table 6.3). The open-

loop gain (Kv) for joint 0 was averaged to be approximately 0.039 (deg/sec)/(D/A word) as the

ratio of the output signal to the computer's D/A converter in deg/sec to the input signal from the

computer's A/D converter in deg/sec.


















S-200-

-300,

-400
0 15 30 45 60 75 90
Time (1/60 sec)


3 800.
M ^ 600-
| 400-
UL 200.
0-
-200 ., 1. .. . .
0 15 30 45 60 75 90
Time (1/60 sec)

Figure 6.6. Typical response of joint 2 of the orange-picking robot to a step input of 750 D/A
bits.


Table 6.3. Steady-state gains, hydraulic damping ratios, and hydraulic natural frequencies as
determined by analysis of the step test responses.

averaged averaged
steady-state averaged hydraulic natural
gain hydraulic damping frequency
(deg/sec) ratio (rad/sec (HZ))
joint (D/A word) nin max rin max
0 0.039 0.18 0.34 17 (2.8) 33 (5.2)
1 0.056 0.18 0.32 22 (3.5) 32 (5.1)


Similarly, step test responses of joint 1 produced information for estimating the

parameters of its transfer function. Once again, the tests were conducted in both directions with

different positions of joint 2 for establishing a range of the parameters which were valid for

the manipulator during operation. Also listed in Table 6.3, the system open-loop gain was

estimated as a constant value of 0.056 (deg/sec)/(D/A word), while the average hydraulic










natural frequency spanned a range from 22 to 32 rad/sec (35 to 5.1 HZ). The average damping

ratio ranged from 0.18 to 0.32.

The second-order open-loop dynamic transfer functions were simulated with the

parameters as determined by the step tests for joints 0 and 1. The results of these simulated

systems were plotted for comparison with the experimental results of the step tests.

Representative results from the experimental tests and the simulations are presented in Figures

6.7 through 6.12. These graphs represent cases for joints 0 and 1 spanning the determined

natural frequency and damping ratio ranges. A response pattern was evident across all of the

tests. In each case, the response of the actual joint showed a peak which compared directly to

the first peak of the simulated system. In some, but not all cases, the actual joint oscillated

slightly (usually one cycle) before settling to a steady-state value. The simulated systems, on

the other hand, generally oscillated more.

This discrepancy indicated nonlinearities in the systems which probably resulted from

nonlinear servo valve characteristics. Theoretically, a deadband in the valve would deliver a

much smaller oscillation than would occur for a valve that was purely linear. A region in the

response of a hydraulic valve in which small changes in the control signals have no effect on

the direction or amount of fluid flow through the valve is referred to as the deadband of the

valve. The responses of the servo valve controlled joints of the robot seem to indicate that a

deadband caused by either an overlap of the valve ports or coulomb friction exists in these

systems.

Because design of the robot's control systems was based on linear systems, a linear

approximation of the transfer functions for these joints was necessary. Noting that the step

responses of the systems were best approximated by second-order responses, second-order

systems were chosen to estimate the transfer function of the actual systems. Therefore,

Merritt's assumptions for reducing the electrohydraulic control system to a second-order system

were verified for the cases of joints 0 and 1. These second-order systems were later used to model









each of the joints and aid in the selection and implementation of the controllers of the orange-

picking robot.

The joints' responses to the step input signals gave important information to be used in

the design of the respective controllers. For joints 0 and 1, the ranges in the natural frequencies

and damping ratios were related to the position of joint 2 during the tests. Higher natural

frequencies were calculated for joint 0 when the weight of joint 2 was being lifted. For example,

the highest natural frequencies resulted when the tube was extended with the end-effector

being lifted or when the tube was retracted with the end-effector being lowered. Likewise, the

highest damping ratios for joint 0 were calculated for those responses in which the most weight

was being lifted. When this information was checked against the parameters of equations 6-2

and 6-3, the damping ratio follows the expected trend that increased load inertia would result

in increased damping. However, the natural frequency contradicts the inverse relationship

with the inertia load as indicated in equation 6-2. This contradiction indicated that the

position of the sliding joint influenced the system more as an external torque load than an

inertia load used in equation 6-2.

The step tests for joint 1 involved moving the end-effector in horizontal directions. For

this joint, a steady-state gain of 0.05 (deg/sec)/(D/A word) was calculated for every trial. In

this case, the second-order parameters varied with the position of joint 2 and the direction of

travel. The hydraulic natural frequencies were smallest when the sliding joint was extended

and the highest inertia was expected. This followed the relationship of equation 6-2. On the

other hand, the established damping ratios did not follow the relationship of equation 6-3.

The only evident pattern of the change in damping ratio was that the damping ratio for the

positive direction was smaller than that for the negative direction. This variance could have

been caused by improper alignment of the outer frame causing binding in the bearings or from

different friction values for the different directions of travel.

As previously discussed, the model of joint 2 could not be determined due to the high

static friction values in the bushing and its unpredictable behavior. The behavior of joint 2







68


indicated that the system had a high deadband which required very large signals to initialize

the motion of the joint and much smaller signals to keep it moving. For controller development,

it was considered to have a sluggish second-order response in which special considerations

would have to be made in controller development and implementation.






























0 10 20 30 40 50
Time (1/60 sec)

(a)


o
O ."




.


0 10 20 30 40 50
Time (1/60 sec)

(b)

Figure 6.7. Responses of joint 0 to a step input: (a) actual, (b) simulated.
q = 32.95 rad/sec, Sh = 0.30, Kv = 0.036 (deg/sec)/(D/A word).










0

-10-

-20

S-30

0 -40 -

-50

-60

-70 .
0 10 20 30 40 50 60
Time (1/60 sec)

(a)

0

-10

-20

-30

2] -40

S -50

-60-

-70
0 10 20 30 40 50 60
Time (1/60 sec)

(b)

Figure 6.8. Responses of joint 0 to a step input: (a) actual, (b) simulated.
h = 17.36 rad/sec, h = 0.16, K, = 0.032 (deg/sec)/(D/A word).










0

-10

-20

S -30 -

-40

-50

-60-

-70 ,
0 10 20 30 40 50 60
Time (1/60 sec)

(a)

0

-10-

-20

| -30 -

a. -40-

-50

-60

-70
0 10 20 30 40 50 60
Time (1/60 sec)

(b)

Figure 6.9. Responses of joint 0 to a step input: (a) actual, (b) simulated.
Oh = 16.54 rad/sec, 8 = 0.31, K, = 0.044 (deg/sec)/(D/A word).






























0 10 20 30 40 50
Time (1/60 sec)


u
oh
- U


0 10 20 30 40 50
Time (1/60 sec)

(b)

Figure 6.10. Responses of joint 1 to a step input: (a) actual, (b) simulated.
wh = 31.87 rad/sec, kh = 0.17, Kv = 0.055 (deg/sec)/(D/A word).










0


-10-


0*- -20

>-
c -30


-40


-50
0 10 20 30 40 50 60
Time (1/60 sec)

(a)

0


-10


0^ -20-


S -30


-40


-50
0 10 20 30 40 50 60
Time (1/60 sec)

(b)

Figure 6.11. Responses of joint 1 to a step input: (a) actual, (b) simulated.
h = 22.02 rad/sec, Sh = 0.31, Kv = 0.056 (deg/sec)/(D/A word).










50


40


30


S 20
1

10-



0 10 20 30 40 50 60
Time (1/60 sec)

(a)

50-


40


0- 30-


20


10


0- .
0 10 20 30 40 50 60
Time (1/60 sec)

(b)

Figure 6.12. Responses of joint 1 to a step input: (a) actual, (b) simulated.
h = 21.47 rad/sec, 8h = 0.22, K, = 0.056 (deg/sec)/(D/A word).















CHAPTER 7
PERFORMANCE CRITERIA


Desired performance criteria for the joint control systems of the orange-picking robot

are specified in this chapter. Because joint control was based on position, velocity, and vision

information, performance requirements were defined to give details upon which the control

systems would be designed and evaluated. In order to determine these requirements, the

interaction between the robot and its environment was evaluated. Initially, the motions of a

fruit hanging from the tree were examined. Information acquired from this investigation was

used to establish vision dynamic criteria for the robot's control. To determine the precision

with which the end-effector of the robot was required to be positioned, a picking envelope was

established. This picking envelope defined the volume with respect to the end-effector in

which a fruit must be located to be picked. In other words, the controllers were required to

utilize information from the fruit sensors to position the end-effector within reach of the

targeted fruit. From this definition of fruit motion and the pick envelope, controller

performance criteria for each joint were determined. These requirements make up the static and

dynamic performance criteria which were necessary for choosing and designing controllers to

realize a satisfactory robot response.


Background

All control systems are designed to perform a specific task. Performance specifications

are spelled out as requirements of the control system. These criteria include relative stability,

accuracy, and response rate. In most cases, these specifications are given in precise numerical

values. However, in some cases, the criteria are not as critical and can be specified as

qualitative statements providing leeway for personal judgement. Many times, the requirements

placed upon a system must be modified during the course of implementing and tuning the

75










controller due to the increased expense of the system or conflicting criteria. Sometimes, the

performance specifications simply can not be met. In any case, the performance criteria are

specifications of design goals which are used for choosing and implementing system controllers.

For the orange-picking robot, the main purpose of joint control was to cause the end-

effector to be positioned so that a fruit could be detached from the tree within the shortest

possible pick cycle time. A pick cycle was characterized by the steps required for the robot to

begin from a "home" location, search for and locate a target fruit, position the end-effector so

that the targeted fruit could be detached, actuate the picking lip to girdle the fruit, return to

the specified home position, and release the fruit. The most important requirement of all

control modes was that the system remain stable for any requested action. Second most

important was the accuracy with which the end-effector was positioned with respect to the

fruit. The relation between the end-effector and the fruit was controlled by the vision control

algorithm and the information from the fruit sensors for joints 0 and 1 and by both the position

and velocity control algorithms for joint 2. Therefore, the strictest performance criteria were

specified for the vision control algorithms in addition to the position and velocity control

modes for the sliding joint.


Characteristics of Fruit Motion

Because the environment of the fruit tree was not free from disturbances during the

picking operation, the robot was required to pick moving fruit. The fruit motion usually

resulted from environmental disturbances such as wind or from canopy interference as the

manipulator removed fruit. Regardless of the cause, the resulting fruit motions were

investigated for use in specifying the performance requirements of the robot.

Initially, reactions of the fruit to wind disturbances were examined. Then, the branches

of the tree were disturbed in a manner similar to those disturbances anticipated during robotic

picking. A tape measure was placed below the fruit for determining the magnitude of the fruit










motion. Using a video camera with a timing feature, fruit's reactions to these disturbances were

recorded and analyzed.

Results from these tests indicated that the fruit could swing with a peak-to-peak

magnitudes ranging from 25 cm to 100 cm with cycle times ranging from 0.9 sec to 2.6 sec. The

results of these tests are presented in Figure 7.1 with the shaded region of the plot indicating

the observed fruit motion. Extreme cases of fruit motion which could take place during picking

were noted as the worst case that the robot could encounter. These extreme cases lie along the

sloping line which limits the upper side of the graph's shaded region and is established by

motions of 100 cm peak-to-peak magnitudes at 0.5 HZ and 25 cm peak-to-peak magnitudes at 1.1

HZ. The robot's control algorithms were required to manipulate the robot to be able to track

and pick fruit with any motion. The best case from the picking standpoint would have the

targeted fruit hanging motionless from the branch. Based on the results of this fruit motion test,

the worst cases that the robot would encounter ranged from small magnitudes with small cycle

times (12.5 cm, 1.1 HZ) to large magnitudes with long cycle times (50 cm, 0.5 HZ) (Figure 7.1).



120-

100-
100 3 Fruit Motion

80 -




40 -
40

20
0.2 0.4 0.6 0.8 1.0 1.2

Frequency (HZ)


Figure 7.1. Typical observations of the motion of fruit swinging from the canopy of a tree
indicating the peak-to-peak magnitude of the fruit for various periods of
oscillation.










Picking Envelope Definition

A study was conducted to define the picking envelope of the end-effector by quantifying

the fruit position range, relative to the stationary end-effector, in which fruit capture could be

accomplished. An apparatus was designed and constructed for holding a fruit in front of the

picking mechanism (Figure 7.2). The apparatus consisted of two pieces of aluminum bent at

right angles and mounted on the end-effector of the robot. The pieces were scribed with a

pattern for establishing the location of the fruit which hung from the apparatus and held in

place by two large spring dips. For the testing, an artificial orange (approximately 8 cm in

diameter) was weighted with water and hung from the support by an elastic cord. The elastic

cord allowed the artificial orange to have motion similar to that of a real orange hanging from

a citrus tree.

The picking envelope was defined by measuring the location of the fruit's center with

respect to the picking mechanism. In order for a location to be included in the picking envelope,


Figure 7.2. Apparatus for determining the picking envelope of the robot's end-effector.









the region in which a successful pick could be guaranteed, the end-effector was required to

girdle the fruit successfully in seven of seven attempts. Positions at which the rotating lip

pushed the orange away or the orange was caught between the rotating lip and the body of the

picking mechanism were considered unsuccessful attempts. Initially, the fruit was positioned in

the center of the picking mechanism, touching the protective shield of the camera with its

center located on the Z axis of joint 2 of the robot (home position). Thus, the position of the fruit

was defined in terms of the camera coordinate frame with the home position located at x = 0 cm,

y = 0 cm, and z = 10.2 cm. The fruit was then moved out along the Z axis in 1 cm increments until

the mechanism was unable to successfully girdle the orange in all of seven attempts. From this

location, the orange was moved to a location 1 cm up from the home position along the Y axis

and back to a point which was easily picked by the mechanism. Again, the fruit was moved out

from the picking mechanism until it could no longer be picked. As the orange was moved to the

edge of the picking range of the robot, the increments were reduced to one half centimeter. Each

location was noted along with the number successful attempts. This operation was repeated

until the complete region in which the robot could possibly pick fruit was covered.

The results of the picking envelope tests are graphically presented in Figures 7.3, 7.4,

and 7.5. In each of the figures, the shaded, circular region represents the fruit positioned

against the protective shield of the camera and centered vertically in the tube of the picking

mechanism. The centroid of the fruit in this position was used as the origin of the picking

envelope. The outer ranges of the orange centroid for a successful pick are represented by the

bold lines in the figures. The range in which the picking mechanism could pick a fruit whose

center lay in a vertical plane that intersected the centerline of tube (Z2) is presented in Figure

7.3. The region in which the orange could be picked if its center point was in a vertical plane 1

cm to the right or left of the centerline of the tube is shown in Figure 7.4. Similarly, the

successful picking region in the vertical plane located 2 cm from the centerline is displayed in

Figure 7.5. In these tests, if the center of the orange was more than 2 cm away from the center of











protective cover for camera


y (cm)


Figure 7.3. Picking range of the end-effector in the vertical plane located on the centerline of
axis 2.


protective cover for camera
(1 cm thickness)


y (cm)


Figure 7.4. Picking range of the end-effector in the vertical plane located 1 cm from the
centerline of axis 2.










protective cover for camera
(1 cm thickness)
OC /z (cm)
0 6.2 10.2 14.2 18.2
\ 12.2 16.2 20.2



I I
4 ......... ........ ...... 6





0 Y (cm)







Figure 7.5. Picking range of the end-effector in the vertical plane located 2 cm from the
centerline of axis 2.


the tube, the rotating lip would push the orange out of reach resulting in an unsuccessful pick

attempt.

The required position relationship for successful picking between the robot end-effector

and the fruit center are given in Tables 7.1 and 7.2. In the Z direction, the robot could pick a

fruit whose center was located no farther from the home position than 16.2 cm. Vertically, a

fruit's centroid could be positioned within 5.0 cm above and 1.0 cm below the tube's centerline.

These ranges in the Y and Z directions were valid for all acceptable positions in the X direction.

Therefore, requirements in the horizontal or X direction specified that the fruit be within 2 cm

of the tube's centerline. These figures indicated the position relationship that guaranteed a

successful pick attempt and were used for establishing required performance criteria for the

controlling systems. A summary of these results is presented in Table 7.1. In Table 7.2, values

for the vision system test which correspond to the edges of the picking envelope are shown. The

xcent and ycent pixel values represent the x and y position of the centroid of the fruit's image in

the vision field.
the vision field.









Table 7.1. Range of positions of a fruit's centroid to guarantee a successful pick.

X Z Y
maximum minimum maximum
(cm) (aCMn) (cm) (cm)
0.0 16.2 -4.0 5.0
1.0 17.2 -3.0 5.0
+2.0 16.7 -1.0 5.0


Table 7.2. Values read by vision system corresponding to the established picking envelope.

X Y Z Vision
(cm) (cm) (cm) xoent ycent
0.0 -4.0 16.2 197 303
0.0 5.0 16.7 196 69
1.0 -3.0 17.2 173 239
1.0 5.0 17.2 177 70
-1.0 -3.0 17.2 221 239
-1.0 5.0 17.2 217 70
2.0 -1.0 16.7 150 206
2.0 5.0 16.7 155 75
-2.0 -1.0 16.7 244 206
-2.0 5.0 16.7 239 75


In summary, for the robot to remove a fruit from the tree, its end-effector had to be

positioned so that the vision system possessed values within the following ranges. The vision

system was required to return an xcent value between 150 and 244 (197 47) and a ycent value

between 75 and 206 (140 65). The xcent and ycent values 197 and 140 represent the location of

the center of the fruit's image when the fruit was in the center of the picking envelope. Also,

the fruit was required to be within the minimum Z distance (16.2 cm) from the origin of the

camera frame. These values represent the borders of a cubic volume in which the fruit was

required to be positioned in order to guarantee that it could be girdled by the rotating lip of the

picking mechanism. Robot control algorithms were necessary which could position the end-

effector within these boundaries.


Velocity Control Requirements

Velocity control algorithms for joints 0 and 1 were necessary only for use during times in

which the robot was searching for a fruit (Adsit, 1989). During this operation, constant

velocities were required of joints 0 and 1 to cause the end-effector to scan the tree in search of a










fruit. The desired motions were achieved by setting velocity set points necessary for scanning

and calling the velocity control routines for joints 0 and 1. This scanning technique was

terminated when a fruit was identified by the vision system. Therefore, performance criteria

for velocity control of joints 0 and 1 did not prove to be critical. By adjusting the set points

which determined position of the joint at which the control was switched from one mode to

another, the overshoot specifications for joint 0 and 1 velocity controllers could be relaxed or

tightened. Steady-state error requirements could also be relaxed by increasing the velocity set

points. Experience with the robot operation indicated that actual velocity of joints 0 and 1

could overshoot the set point velocity by as much as 45 percent without greatly affecting the

performance of the search procedure. Also, deviations of 40 percent from the desired velocity

did not adversely affect the picking performance of the manipulator.

Although the velocity requirements of joints 0 and 1 were used only during the fruit

search, the velocity control algorithm for joint 2 was used during the picking operation to

extend the end-effector toward a targeted fruit during the approach states. Therefore, its

performance proved to be more critical than that for joints 0 and 1. Most critical was the ability

of joint 2 to extend in a smooth, rapid manner and respond quickly to velocity set point changes.

Also, the detection of robot/limb collisions during extension and the detection of unremovable

fruit during retraction relied upon the ability to accurately control the joint 2 velocity. The

intelligence base detected these events by monitoring the joint 2 velocity control word which

was generated from the velocity error by the velocity controller. Large velocity control words

over a specified period of time were used to indicate that joint motion was restricted either by

an obstacle in the arm's path or by the inability of the arm to remove a fruit from its stem.

During the picking operation, overshoot values up to 25 percent of the desired joint 2 velocity

were regarded as acceptable and velocity steady-state errors of up to 10 percent were

determined tolerable based on experience with the robot.










Position Control Requirements

Position control algorithms were used primarily for two operations. First, the position

control algorithms were called to return the robot to its home position after a pick cycle was

completed. This motion was accomplished by setting the position set points to the specified

home location and calling the position control routines to move the joints to the required

position. A second use was to stop the joints and hold them stationary while other actions were

performed. For instance, during the detach phase of a pick cycle, joints 0 and 1 position

controllers were used to hold the arm stationary while the lip was actuated for girdling the

fruit.

During the picking phase, a large error in the joint positions would cause the end-

effector to fail in its attempt to girdle the targeted fruit. Therefore, the maximum allowable

error for each joint was the motion of the end-effector which would cause the fruit to be out of

the picking envelope when the picking mechanism was actuated. For joint 0, the maximum

steady-state error was determined to be 3 cm at the end-effector. Likewise, the maximum

steady-state error for joint 1 was 2 cm, and for joint 2, the maximum error was 3 cm. The position

errors for joints 0 and 1 were influenced by the position of joint 2. For a given angular motion of

joint 0 or 1, the maximum error at the end-effector was realized when the sliding tube was in

the extended position. Therefore, the angular position errors allowed for joints 0 and 1 were

calculated based on the maximum allowable extension of joint 2 (158.3 cm). These values

translated to maximum steady-state errors of 1.1 degrees and 0.7 degree for joints 0 and 1,

respectively, and 3 cm for joint 2.

Dynamic response of the position control algorithms was not as critical as steady-state

error criteria. These algorithms caused motion only to return the joints to their home positions.

* The home positions could be set to allow some overshoot of the desired position allowing a

faster response time. Again, experience with the fruit-picking robot suggested that position

overshoots as high as 15 percent could be allowed for each of the axes without causing any










problems with the control of the manipulator. By allowing the dynamic response to overshoot

the desired value, greater speeds were achievable and thus faster pick cycle times.


Vision Control Requirements

Of all the control systems, the most critical operations took place while the robot was

operating under the vision control. After the vision system identified a fruit while in the

search mode, the intelligence base switched control of joints 0 and 1 from velocity control to

vision control. Using vision control, joints 0 and 1 were aligned with the targeted fruit and joint

2 extended to position the end-effector near the fruit. The operations which used vision control

required that the motion of the fruit be tracked by the end-effector and ultimately that the

end-effector be positioned so that the fruit would lie within the picking envelope.

As previously pointed out, the two worst observed cases of fruit motion were: Case I:

swinging motions up to 50 cm in magnitude with 2 sec (0.5 HZ) cycle times and Case II: swinging

motions 12.5 cm in magnitude with cycle times of 0.9 sec (1.1 HZ). Also, the established picking

envelope indicated that the fruit position could be allowed to fluctuate by 2 cm (47 pixels) in

the x direction (joint 1) and 3 cm (65 pixels) in the y direction (joint 0) from the center of the

picking envelope. Controllers for the vision system of the robot were thus needed to cause the

end-effector to be able to track these fruit motions with errors not to exceed 3 cm for joint 0 and 2

cm for joint 1.

Because the units of the vision system were pixels, the amplitudes of the fruit and the

end-effector were converted to pixel units (Figure 7.6). As presented in the figure, the image

plane was assumed to be infinitely large and the fruit was assumed to be its maximum pickable

distance (16.2 cm) from the camera optical center, Oc. The trigonometric relations indicated

that the image of a fruit whose magnitude was 500 mm would have a magnitude of 1077 pixels

on the infinite image plane. Likewise, a fruit magnitude of 125 mm corresponded to an image

magnitude of 269 pixels.











a,
Case I: 500mm
CaseII: 125mm


fruit

01----------------

\






Camera Optical Center, 0O
/
/
/.


fruit

/
/

/
/ 162 mm

/




%\ 8mm

\ Infinite Image Plane


Case I: 1077 pixels
Case II: 269 pixels


Figure 7.6. Relationship between the amplitude of a fruit and the amplitude of its image on
the camera image plane.


To provide more information for determining the performance requirements for the

vision control system, the difference between the location of the targeted fruit and the end-

effector was calculated. First, the previously observed motion of a fruit was expressed as a

vector,

fm = af sin(ot)

where fm = fruit motion (pixels),

at = fruit motion amplitude (pixels),

o = frequency of fruit motion (deg/sec), and

t time (sec).

The predicted motion of the robot's end-effector was also described as a vector,

rm = ar sin(ct+o)

where rm = end-effector motion (pixels),

ar = end-effector motion amplitude (pixels), and

phase lag between fruit and end-effector motion (deg).


I


I




University of Florida Home Page
© 2004 - 2010 University of Florida George A. Smathers Libraries.
All rights reserved.

Acceptable Use, Copyright, and Disclaimer Statement
Last updated October 10, 2010 - - mvs