UFDC Home  myUFDC Home  Help 
DARK ITEM  



Full Text  
PAGE 1 1 DESIGN AND ANALYSIS OF LINEAR AND NONLINEAR CONTROLLERS FOR POSITION CONTROL OF HYDRAULIC MANIPULATORS FOR CITRUS HARVESTIN G By ANIRUDH SUNDARARAJAN A THESIS PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF SCIENCE UNIVERSITY OF FLORIDA 2011 PAGE 2 2 2011 Anirudh Sundararajan PAGE 3 3 To my wonderful parents PAGE 4 4 ACKNOWLEDGMENTS I would like to thank my advisor Dr. Thomas F. Burks for his guidance and his constant source of encouragement through the entire research process. I am also grateful to Dr. Warren Dixon and Dr. Schueller for their guidance and assistance provided by being a part of my thesis committee. I would like to sincerely acknowledge and thank Mr. Mike Zingaro and Mr. Greg Pugh for their valuable technical assistance provided with the mechanical, electrical and software components used in the thesis. My presence here would not have been possible i f not for the constant support and encouragement provided by my parents and I sincerely extend my gratitude for them. And for the support when I needed it most and for the fun times Ive had, I express my sincere thanks to all my friends. PAGE 5 5 TABLE OF CONTE NTS page ACKNOWLEDGMENTS .................................................................................................. 4 LIST OF TABLES ............................................................................................................ 8 LIST OF FIGURES .......................................................................................................... 9 ABSTRACT ................................................................................................................... 12 CHAPTER 1 INTRODUCTION .................................................................................................... 14 Overview ................................................................................................................. 14 Citrus Industry Economics ...................................................................................... 14 Thesis Organization ................................................................................................ 17 2 OBJECTIVES ......................................................................................................... 18 3 LITERATURE REVIEW .......................................................................................... 19 Robotic Harvesting Manipulators Related Work .................................................. 19 Past Efforts i n Control System Development for Robot Manipulators a nd Hydraulic Systems ............................................................................................... 23 Manipulator Kinematics ........................................................................................... 26 Manipulator Dynamics ............................................................................................ 28 Nonlinear Control System Development ................................................................. 33 4 METHODS AND PROCEDURES ........................................................................... 45 Mechanic al Design of the Manipulator .................................................................... 45 Framework for Real Time Implementation of Position Controllers RT LAB ......... 46 Working w ith RT LAB ....................................................................................... 46 Building a Distributed Model for RT LAB .......................................................... 48 Linear a nd Nonlinear Controller Design for Position Control of the Manipulator ................................................................................................... 50 Implementing a Real Time Joint Level Closed Loop Position Control System .......................................................................................................... 51 5 PHYSICAL AND MATHEMATICAL MODEL DEVELOPMENT FOR THE MECHANICAL AND HYDRAULIC SUBSYSTEMS OF THE MANIPULATOR ........ 52 Mechanical Design of the Hydraulic Manipulator .................................................... 52 Rotary Hydraulic Actuation System Design ...................................................... 53 Valve Configuration .................................................................................... 55 PAGE 6 6 Dual Solenoid Amplifier for Valve Control .................................................. 56 Feedback Transducer ................................................................................ 58 Data Acquisi tion Boards ............................................................................. 58 Mathematical Model Development for the Hydraulic Manipulator ........................... 59 Forward Kinematics .......................................................................................... 59 Inverse Kinematics ........................................................................................... 61 Euler Lagrange Equations for the Manipulator Dynamics ................................ 64 6 DEVELOPMENT AND IMPLEMENTATION OF CONTROL SYSTEMS FOR THE HYDRAULIC MANIPULATOR ........................................................................ 73 Proportional Joint Space and CartesianSpace Joint Level Position Control .......... 73 Control System Algorithm ................................................................................. 74 Descript ion of Control System Implementation ................................................. 74 Development of a n Absolute Positioning System for Joint Control ................... 79 Description of the hardware system for j oint level control system implementation ....................................................................................... 80 Robot m anipulator j oint level p osition controller v erification ...................... 81 7 NONLINEAR CONTROL OF THE HYDRAULIC ACTUATOR SYSTEM ................ 94 Dynamic Behavioral Model of the Hydraulic Actuator System ................................ 95 Problem Statement ................................................................................................. 98 Integrator Backstepping Controller Design ............................................................. 98 Stability Analysis of the Designed Controller ........................................................ 104 8 NONLINEAR MODEL BASED TRACKING CONTROL OF THE MANIPULATOR 112 Dynamic Model of the Hydraulic Manipulator ........................................................ 113 Control Objective .................................................................................................. 116 Error System Development ................................................................................... 116 Controller Design .................................................................................................. 117 Stability Anal ysis ................................................................................................... 118 9 OVERALL SUMMARY, CONCLUSIONS AND FUTURE SCOPE ........................ 125 Summary And Conclusions ................................................................................... 125 Future Work .......................................................................................................... 128 APPENDIX A WORKING WITH RT LAB USER INTERFACE .................................................... 130 B MANUFACTURER SPECIFICATIONS FOR THE ELECTRO HYDRAULIC CONTROL SYSTEM COMPONENTS .................................................................. 135 C MATLAB PROGRAM FOR COMPUTING THE FORWARD KINEMATICS OF THE HYDRAULIC MANIPULATOR ...................................................................... 150 PAGE 7 7 D MATLAB PROGRAM FOR COMPUTING THE INVERSE KINEMATICS OF THE HYDRAULIC MANIPULATOR ...................................................................... 151 E MATLAB PROGRAM FOR COMPUTING THE EULERLAGRANGE EQUATIONS OF THE HYDRAULIC MANIPULATOR .......................................... 153 F MATLAB/ S IMULINK PROGRAM FOR THE REAL TIME JOINT LEVEL POSITION CONTROLLER ................................................................................... 159 G S IMULINK PROGRAM FOR ADAPTIVE INTEGRATOR BACKSTEPPING OF THE HYDRAULIC ACTUATOR SYSTEM ............................................................. 170 H SIMULINK PROGRAM FOR EXACT MODEL KNOWLEDGE CONTROLLER OF THE HYDRAULIC ROBOT ............................................................................. 173 LIST OF REFERENCES ............................................................................................. 179 BIOGRAPHICAL SKETCH .......................................................................................... 183 PAGE 8 8 LIST OF TABLES Table page 5 1 Deadband jump and maximum amplifier gain settings. ...................................... 72 5 2 Mechanism parameters for the four DOF manipulator. ....................................... 72 6 1 Voltage readings for the 4 joint potentiometers .................................................. 92 6 2 Experimental r esults from the j oint level cartesian p ositional controller .............. 93 PAGE 9 9 LIST OF FIGURES Figure page 3 1 MAGALI apple picking a rm ................................................................................. 37 3 2 CITRUS picking arm structures .......................................................................... 37 3 3 Citrus picking robot. ............................................................................................ 37 3 4 Kubota arm. ........................................................................................................ 38 3 5 Schemes for self guided gantry robot for picking fruit. ........................................ 38 3 6 Picking arm structure for Agribot. ....................................................................... 39 3 7 Picking arm structure of first prototype ............................................................... 39 3 8 Dual arm layout for the second prototype. .......................................................... 40 3 9 End effector for citrus harvesting. ....................................................................... 40 3 10 4 DOF hydraulic manipulator .............................................................................. 41 3 11 Describing the DH parameters ........................................................................... 42 3 12 Coordinate frame assignment and D H parameters ........................................... 42 3 13 One degree of freedom system constrained to move vertically .......................... 43 3 14 A general rigid body in space ............................................................................. 43 3 15 Uniform rectangular solid .................................................................................... 44 5 1 Manipulator joint link layout ................................................................................ 67 5 2 Assembled manipulator prototype ...................................................................... 67 5 3 Roll joint exploded assembly .............................................................................. 68 5 4 Pitch joint assembly ............................................................................................ 68 5 5 Hydraulic circuit schematic for valve controlled motor motion ............................ 69 5 6 Overlapped four way spool valve schematic. ..................................................... 69 5 7 Flow gain for a closedcenter valve. ................................................................... 70 5 8 Proportional solenoid valve current input to flow output performance ................ 70 PAGE 10 10 5 9 Input to output signal proportionality of the dual solenoid amplifier .................... 70 5 10 Coordinate frame assignment for joint link pairs according to DH conventions .. 71 5 11 Motion of the end effector due to revolute joint i ................................................. 71 6 1 Schematic layout of the j oint level position controller ......................................... 84 6 2 Master subsystem for the Simulink model of the j oint level p osition controller 85 6 3 Simulink model of the C onsole subsystem for the j oint level p osition controller ............................................................................................................. 86 6 4 Joint level C artesian position controller Simulink model ................................... 86 6 5 Master subsystem for the Simulink model of the absolute joint controller ......... 87 6 6 Console subsystem, RT Lab main w indow and Parameter selection dialog box ...................................................................................................................... 88 6 7 System layout for real time implementation of the j oint level p osition controller using Opal RTs RT Lab HIL software ................................................ 89 6 8 Desired coordinates of the end effector vs a ctual coordinates obtained through joint level Cartesian positional control ................................................... 90 6 9 Total (actual) error in Cartesian coordinates vs e rror in positional controller neglecting errors due to IK algorithm .................................................................. 91 7 1 Hydraulic system layout showing the valve spool dynamics subsystem and the actuator and load subsystem. ..................................................................... 108 7 2 Simulink model for the Integrator Adaptive Backstepping Controller for the h ydraulic system ............................................................................................... 109 7 3 Position tracking error of the joint angle of the actuator. ................................... 110 7 4 Rate of convergence of the parametric estimates ............................................ 111 8 1 Simulink model block diagram for the Exac t Model Knowledge controller for the hydraulic 4 DOF manipulator ...................................................................... 121 8 2 Actual vs d esired joint angular position of Joint 1 ............................................. 122 8 3 Actual vs d esired joint angular position of Joint 2 ............................................. 122 8 4 Actual vs d esired joint angular position of Joint 3 ............................................. 123 8 5 Actual vs d esired joint angular position of Joint 4 ............................................. 123 PAGE 11 11 8 6 Errors in the joint angles ................................................................................... 124 PAGE 12 12 Abstract o f Thesis Presented t o t he Graduate School o f t he University o f Florida i n Partial Fulfillment o f t he Requirements f or t he Degree o f Master of Science DESIGN AND ANALYSIS OF LINEAR AND NONLINEAR CONTROLLERS FOR POSITION CONTROL OF HYDRAULIC MANIPULATORS FOR CITRUS HARVESTING By Anirudh Sundararajan December 2011 Chair: Thomas F Burks Major: Agricultural and Biological Engineering The automation of fruit harvesting operations has become an important area of research in trying to improve the efficiency of the harvesting operation as well as to minimize the costs associated with manual labor. A need to improve safety, especially when it comes to harvesting fruits from hard to reach spaces has been another reason for increasing research in the area of robotics applied to harvesting operations. The automation solutions applied to citrus harvesting can be broadly categorized into Mass harvesters (canopy or trunk shakers) and Selective harvesters (robot manipulator arms). While several designs of mass harvesters have been developed, they cannot be employed in cases where the fruit is to be harvested with minimum damage, for ex ample, in the fresh fruit industry or where damage to the tree canopy or trunk is to be avoided. These issues necessitate the research of selective harvesting mechanisms, e.g. robotic manipulators to selectively harvest fruits with little or no damage to t he fruit and tree and to efficiently automate the harvesting process. Robotic manipulators have traditionally been driven by means of hydraulics, pneumatics, electrical motors or a combination of these and different control system strategies exist PAGE 13 13 for thes e implementations. The thesis aims to design linear and nonlinear controllers for a hydraulic robot and to lay out a framework for real time implementation of the controllers for position control in fruit harvesting operations. The mechanical, electrical and hydraulic components layout of a manipulator a 4 degree of freedom (DOF) robot, is studied for proper design of control system strategies to be employed. Owing to the inherent nonlinear behavior of hydraulic components, including the control valves and rotary actuators as well as the mechanical system comprising of the joints and links of the manipulator, a detailed study and characterization of the entire manipulator system is crucial for ensuring acceptable performance of the controllers. Based on the mathematical model of the electro hydromechanical system of the manipulator, linear and nonlinear control systems are designed for trajectory tracking and position control. A framework for implementing the control systems in real time is designed and t he hardware and software packages necessary for a hardwarein loop implementation of the control system are set up. Simulations and results from the real time implementation are then used to discuss the stability and performance metrics of the different c ontrol strategies. PAGE 14 14 CHAPTER 1 INTRODUCTION Overview A utomation of citrus fruit harvesting has become an important area of research in trying to improve the efficiency of the harvesting operation as well as to minimize the costs associated with manual labor. The harvesting can also prove to be hazardous, with harvesters having to climb ladders up to 5.5 meters high while carrying collection bags which weigh close to ninety pounds (Roka and Longworth, 2001). A need to improve safety when it comes to harvesting fruits from hard to reach spaces has been another reason for increasing research in the area of robotics as applied to harvesting operations. Several past efforts have been successfully made in designing and deploying automation solutions for harvesting fruits and vegetables. Mass harvesting solutions (canopy or trunk shakers) have been used widely for harvesting citrus for processing. These mechanisms however cannot be used in cases where the fruit is to be harvested with minimal damage to the fruit surface. Damage to the tree canopy or trunk is also a cause of concern when employing mass harvesting mechanisms. These issues necessitate the research of selective harvesting mechanisms, e.g. robotic manipulat ors to selectively harvest fruits and efficiently automate the harvesting process. Citrus Industry Economics The United States is among the world leaders in citrus production. The term citrus includes a wide variety of orange, tangerine, grapefruit, lime and lemon. According to the Florida Citrus Statistics report released by National Agricultural Statistics Service for the 20092010 s eason, Florida accounted for 65% of the total PAGE 15 15 United States citrus production. And it is about 15% of the total world production. The report also notes that citrus were among the highest consumed fresh fruits in the United States. Apart from Florida, the other major citrus growing states are California, Texas and Arizona. Internationally, China, Brazil, EU27 and Mexico ar e the major competitors (Florida Citrus Statistics, 2010). Several factors such as seasonal freezes threaten citrus production and the associated economic returns. The United States is challenged by its competitors, with the global market poised to increas e the demand for oranges (fresh and processed) in the future. Scarce availability of seasonal labor and the increased labor costs also pose additional challenges. The abovecited reasons and the availability of newer, robust technologies in agricultural mechanization make a compelling argument for research in the applications of automating the Florida citrus harvesting operations. For the growers to realize profit there is a need for an intelligent harvester that can emulate a human and perform the task of fruit picking. Nevertheless, it must be mentioned that the orchards might need additional preparations including ground leveling, hedging, topping and inrow spacing for the mechanical harvester to perform optimally as discussed in the article on horticult ural aspects of robotic fruit harvesting by Burks et al. (2005). Implementing a Robotic Citrus Harvester Tree fruit harvesting is an intricate process that requires proper detection of the fruits, gripping, detaching fruits with minimal damage to either fr uit or tree and safe deposition of fruits into a collection bin. Each operation must be carried out in the most efficient manner to obtain a viable harvesting solution. The primary areas of research for PAGE 16 16 the development of a robotic harvester as identified by Hannan and Burks (2004) are fruit detection and fruit removal. Fruit detection involves selection of suitable sensors (including machine vision) and appropriate algorithm development which exploit various fruit properties such as color and shape. Fruit removal involves the development of a suitable robotic manipulator(s) and an endeffector (tool connected to the end of robot arm/wrist to grasp the fruit). Finally, the robot manipulator must be integrated with the fruit detection system to obtain a fully operational system. R obotic manipulators have traditionally been driven by means of hydraulics, pneumatics, electrical motors or a combination of these. Designing different control system strategies for these manipulators forms an important area of research in developing robot manipulators. The area of control system development can be broadly classified into linear control systems and nonlinear control systems. Linear control systems are widely used in several areas because they are simple to formulate, easy to implement and do not require a detailed knowledge of the system being controlled. Linear control strategies such as PID controllers (proportional integral derivative controller) have been commonly used in controlling industrial and other manipulators. However, owing to their simplistic approach, they are not well suited for areas where precise control is required, as is the case in precise position control of manipulators for harvesting. In such situations, linear controllers often require costly ac tuators and high control efforts to meet the precision requirements. This can be attributed to the fact that a linear controller works by discarding the unmodelled nonlinear characteristics of the controlled system, which are present in every real world mechanism. PAGE 17 17 Thus, the study of nonlinear control strategy is crucial in increasing the efficiency as well as precision of robotic harvesters. This research aims at analyzing the nonlinearities associated with controlling a hydraulic manipulator and at developing control system strategies that address these nonlinearities. Thesis Organization Chapter 1 provided an overview of the thesis and a background on the economics of the citrus industry. Chapter 2 outlines the objectives of the research and expected outc omes. A review of relevant previous research efforts and literature is presented in Chapter 3, with discussions of both the design and implementation aspects of automation solutions as well as efforts in controller system developments. In Chapter 4, the ex perimental methods for implementing a robotic harvester are discussed. The mechanical design and the hydraulic and electrical circuit layouts of a hydraulic manipulator are presented. A framework for implementing trajectory tracking controllers for the manipulator in real time is then laid out. In Chapter 5, the physical and mathematical model developments for the mechanical and hydraulic subsystems of the manipulator are laid out. Chapter 6 details the implementation of a real time joint level Cartesian position controller for the hydraulic manipulator. Chapter 7 discusses the nonlinear controller development for the hydraulic subsystem, and in Chapter 8 the nonlinear controller development for the manipulator subsystem is presented. Chapter 9 provides conc lusions and suggestions for future work in the development of control systems and their implementation for real time harvesting of citrus. PAGE 18 18 CHAPTER 2 OBJECTIVES This research addresses the systematic design and development of control systems for a hydraulic robot manipulator for a fruit harvesting application and the real time implementation of the position controllers. The first objective of the research is to study the mechanical design of the manipulator and the hydraulic and electric circuit com ponents that are used to operate the manipulator. The nonlinear characteristics of the electrohydromechanical model are to be analyzed for the design of nonlinear controller for position tracking. The second objective is to design linear and nonlinear co ntrollers for position control of the manipulator during trajectory executions. Performance metrics are to be chosen to evaluate and compare the performance of controllers. The third objective of the research is to design and set up a framework for real ti me implementation of position controllers, in a hardwarein loop (HIL) environment. The final objective is to implement a real time joint level closed loop position control system with the use of feedback transducers, data acquisition boards and solenoid v alve drivers. PAGE 19 19 CHAPTER 3 LITERATURE REVIEW In the first section of this chapter, a few past efforts in agricultural harvesting manipulator development are reviewed. In the second section of the chapter, previous developments and implementations of severa l control system architectures for various robotic manipulators are reviewed. The third section provides an introduction to the topics of mathematical modeling of open chain serial link robot manipulators and manipulator control system framework. The chapt er concludes with a discussion on the mathematical fundamentals, theorems and lemmas used in the development and analysis of nonlinear control systems using the Lyapunov based method. Robotic Harvesting Manipulators Related Work Several designs of robotic manipulators have been put forth to address specific requirements for the fruit being harvested. Past research efforts on apple, tomato and citrus harvesting manipulators are discussed here owing to their similar plant structure and harvesting requirements. Several other research efforts have been invested in automating harvest ing of other fruits and vegetables including cucumber (Van Henten, 2002), grape (Kondo, 1995 and Monta et al., 1995) and watermelon (Sakai et al., 2002). Schertz and Brown (1968) laid out the basic principles and considerations in mechanizing citrus harves t in their 1968 review of mechanical citrus harvesting systems. Parrish and Goksel (1977) designed the earliest prototype of a robotic harvesting system for apple harvesting. The prototype used a simple threejoint arm with a camera on a pan and tilt mechanism and a touch sensor in place of an end effector to make contact with the modeled fruit in the laboratory. Due to the rudimentary nature of the mechanical system, this prototype was not used for field tests. Grand PAGE 20 20 DEsnon (1985) developed another protot ype for harvesting apples. Consisting of a telescopic arm actuated by a rack and pinion arrangement, the robot could move up and down in a vertical framework. An additional degree of freedom for the system was realized by mounting the arm on a barrel that could rotate about the horizontal axis, thus making it a cylindrical configuration. This research was followed by an enhanced prototype known as MAGALI (Grand DEsnon et al 1987) which was a spherical manipulator served by a camera set at the center of r otation of the base axes. The manipulator incorporated a vacuum grasper for fruit picking. Hydraulic cylinders were used as the actuation mechanism and closed loop joint control was achieved through feedback from potentiometers. The spherical manipulator i s shown in Figure 31. The French Spanish EUREKA project named CITRUS for a robotic harvester for citrus was initiated in 1987. Two arm prototypes were developed and tested in 1991 (Pellenc et al. (1990), Juste and Sevila (1991), Juste et al. (1992), and R abatel et al. (1995)). One prototype was designed similar to the MAGALI arm with a pantographic structure while the other had a telescopic joint. Both the arms were of the spherical configuration. Field test results from this effort show that the performance of the telescopic joint was inferior to the pantographic structure. The hydraulic actuators used in the initial prototypes were replaced with electric motors in the later progressions. The end effectors, sensing, and control techniques used in this proj ect were similar to the MAGALI arm. Figure 3 2 shows the CITRUS arm structures. A dead reckoning guidance was implemented using a fixed camera and generating a straight line trajectory to the fruit once it was detected. PAGE 21 21 An alternative approach for robotic harvesting of citrus through real time fruit detection was developed at the University of Florida by Harrell et al. (1988). During this research, a three (DOF) manipulator actuated with servohydraulic drives was designed, constructed and tested. This geo metry is characteristic of a spherical coordinate manipulator and is shown in Figure 33. Hayashi, Ueda, and Suzuki (1989) designed a mandarin orange harvesting robot for the orchard named Kubota. The Kubota robot had an articulated arm with four DOF, bu t acted as a spherical coordinate robot due to the joint actuation schemes. Figure 34 shows the articulated arm of this robot which utilized an end effector with rotating stem cutters that contained a color TV camera and a light source. In the review of r obotics for fruit harvesting, Sarig (1993) reported few manipulator development efforts and discussed some approaches to design and implement self guided gantry systems for mobilizing the fruit harvesting manipulators from tree to tree. Two such schemes ar e shown in Figure 35. Ceres et al. (1998) presented a manipulator design for an aided fruit harvesting robot (Agribot) that works under human guidance. The articulated manipulator structure was designed based on a kinematic, dynamic and geometric study which took into account the fruit distribution on the tree. Figure 36 shows the parallelogram structure of the Agribots picking arm with four DOF (all rotational), including the gripper. All of the joints were driven by electric motors. Fruit detection was done by a human operator using a laser telemeter and a joystick and fruit detachment was done through an end effector with a suction cup that pulled the fruit into a V shaped cutter. PAGE 22 22 Another development for a citrus harvesting manipulator in Italy was reported by Cavalieri and Plebe (1996), Fortuna et al. (1996), and Muscato et al. (2005). The first research prototype had two spherically configured picking arms mounted at the tool point of a four DOF positioning platform. The picking arms were driven by el ectric motors while the platform was driven by hydraulic actuators. Figure 37 shows the arm structure. Muscato et al. (2005) also presented a second prototype with two arms mounted on a 45 inclined platform carried on a caterpillar as shown in Figure 38 Both the arms were of the Cartesian type and were driven by electric motors, but the upper arm had a telescopic link in place of a prismatic link as in the lower arm due to space constraints. The research also presented a variety of end effector developm ents, a three finger pneumatic device that used a cutter to remove fruit from stem, a grasping device with a helix movement that brought the stalk into the cutter, and another pneumatic end effector with jaws to capture the fruit, a sliding tray to hold the fruit and clippers to cut the stalk. Control was achieved through feedback from camera and proximity sensor located on the end effector. Flood (2006) designed an end effector for robotic citrus harvesting. Several harvesting motion studies and analyses o f the physical properties of the citrus fruit were carried out to establish safe grasping limits for the end effector and to establish an optimum harvesting motion. Figure 39 shows the design of the end effector. Sivaraman (2006) described a systematic development of a robotic harvesting manipulator by studying various kinematic designs of serial manipulators and choosing an appropriate kinematic configuration for citrus harvesting by comparing their kinematic performance indices. The distal, reduced set of joints (4) from the chosen rrpRPRP PAGE 23 23 manipulator configuration was then designed and fabricated, using rotary actuators to control the joints. The actuators were selected based on torque requirement studies from dynamic simulations. A joint level manipul ator position control was achieved using potentiometer feedback, data acquisition boards and solenoid valve drivers. This setup formed the framework of this thesis, where further efforts in the control system development were the key area of research inter est. The distal joints and the electrohydraulic circuit layout are shown in Figure 310. Past Efforts i n Control System Development for Robot Manipulators a nd Hydraulic Systems The essential function of a robot manipulator is to precisely position the end effector at any desired point in 3dimensional space within the manipulators workspace. Thus, the control system development for the position control of a manipulator plays a crucial role for its effective use. As can be seen from the earlier section where several past efforts in the manipulator development were discussed, there exist several schemes for actuating a manipulator. Some common actuation mechanisms include electric DC motors, linear and rotary hydraulic actuators, and pneumatic actuators. The choice of actuation depends on the type of application, power requirements and the nature of the work environment. Payload capacity, desired size and weight of the actuators, accuracy and repeatability of positioning and the economics of the manipulator are other important factors that influence the choice of actuation. The advantages of using electric motors for actuation are good position and torque control, less complicated joint design, no fluid leaks and low maintenance requirements. However, hydraulic actuators are used in several applications where a high output power to weight or torqueto weight ratios are desirable. Hydraulic actuators also PAGE 24 24 eliminate the use of gear reducers to increase the torque output, as is the case with electric motors. Al so, for a robot to be functional in an agricultural setting, where suitable electrical power may not be available during harvesting operations, a hydraulic system proves to be a feasible solution. Owing to the above mentioned desirable characteristics of hydraulic actuation systems, the manipulator under consideration for this thesis was designed to be actuated by rotary hydraulic actuators. The following sections illustrate some past efforts in the control system development for position tracking control of hydraulic systems and robot manipulators. Control system development in general can be broadly classified into linear and nonlinear control systems. Several linear control approaches have been suggested for position control of hydraulic systems. It has been observed that the proportional integral derivative (PID) control technique generally leads to poor tracking performance owing to the highly nonlinear characteristics of hydraulic components. The issue is more pronounced in higher frequency applications. Bobrow (1995) applied linear control theory to design an adaptive controller using full state feedback for a hydraulic servovalve. The tracking controller was designed to address parametric uncertainties in the model of the system. A linearizing feedb ack control law for electrohydraulic systems was developed by Vossoughi (1995). The controller demonstrated uniform response across a wide range of operating conditions. A major disadvantage of a linear control system applied to nonlinear hydraulic system dynamics is the lack of a global stability proof. The nonlinear nature of hydraulic systems led to several efforts in designing controllers that consider the nonlinearities instead of an approximate linear model. PAGE 25 25 Several adaptive and robust controllers have been designed to address the nonlinear model using Lyapunov theorems. Chern et al (1992) developed a robust, variable structure controller (VSC) to address the nonlinear hydraulic system. However, the VSC based control strategy suffers from chattering in the control action owing to a highfrequency, highbandwidth nature of the resulting controller implementation. This makes the controller not implementable in an actual hydraulic system since no physical actuator is capable of handling infinite switching requirements. Lu et al (1993) and Laval et al (1996) designed robust controllers based on the linearized model of the hydraulic system. Many of the robust control strategies assume that the hydraulic valve is infinitely fast to accommodate for the highfrequency switching control signal. Alleyne (1996) developed a force tracking Lyapunov based adaptive controller to compensate for parametric uncertainty in a twostage servovalve model. Zheng (1998) developed an adaptive controller to compensate for th e deadzone characteristics and friction of a proportional valve. Sohl et al (1999) conducted experiments on the nonlinear control of hydraulic servo systems, with an integrator backstepping like approach to develop force tracking and position tracking co ntrollers. Bu et al (2000) developed a nonlinear Adaptive Robust Control (ARC) based control of hydraulic actuators to address the linearly parametrized uncertainties in the system as well as the effects of uncertain nonlinearities. For simplicity, only one degree of freedom of the manipulator was considered owing to manipulator model complexity. Sirouspour et al (2000) presented an integrator backstepping algorithm to design a Lyapunov based position tracking controller for a linear hydraulic servosyste m. Continuing the work on ARC based control, Bu et al (2001) developed an over parametrizing method to PAGE 26 26 address parametric uncertainty and to avoid acceleration feedback. Cunha et al (2005) developed an adaptive fixed cascade controller for mechanical and hydraulic subsystems with adaptive deadzone compensation. Manipulator Kinematics A robot manipulator is composed of a set of links connected together by various joints, which can be either revolute or prismatic. The action of each joint can be described by a single real number: the angle of rotation in the case of a revolute joint or the displacement in the case of a prismatic joint. The links are assumed to be rigid bodies that connect the consecutive joint axes and maintain a fixed relationship between these axes. The four parameters that can describe each link in a kinematic chain are: link length a link twist or twist angle link offset d and joint angle The method of using these four parameters to describe a mechanism is called the Denavit Har tenberg (D H) convention. Referring to Figure 311, the physical interpretation of each of these quantities can be given as follows. The parameter a is the distance between the axes z0 and z1, and is measured along the axis x1. The angle is the angle b etween the axes z0 and z1, measured in a plane normal to x1. The positive sense for is determined from z0 to z1 by the right hand rule. The parameter d is the distance between the origin o0 and the intersection of the x1 axis with z0 measured along the z0 axis. Finally, is the angle between x0 and x1 measured in a plane normal to z0. Once the mechanism parameters are determined, the problem of locating each link with respect to its neighboring links is handled by attaching a coordinate frame to each link and performing transformations between these frames. In order to develop the transformation matrices between consecutive frames, homogeneous coordinates are used. The homogeneous coordinates are used to PAGE 27 27 represent a vector in a four dimensional space wher e the fourth coordinate is a nonzero scaling factor and is selected to be unity in robot kinematics applications for convenience. This allows representation of a three dimensional vector as the first three homogeneous coordinates. The following procedure describes the construction of the transformation matrix Ti i 1 to relate frame {i} to fram e {i 1} as shown in Figure 312. start wi th frames {i} and {i 1} aligned; t ranslate frame {i} about Xi 1 by ai 1; t hen, rotate frame {i} about Xi 1 by i 1; t ranslate frame {i} about Zi by di; rotate frame {i} about Zi by i to complete the transformation. The translations and rotations are represented as 4x4 transformations with the upper left 3x3 submatrix representing the orientation and the upper r ight 3x1 submatrix representing the position. The orientations are defined using the basic rotation matrices derived from the Euler angle representation. The individual translation and rotation matrices are then concatenated to obtain the transformation m atrix. 1cossin0010001001000 sincos00010001000cossin0 001000100100sincos0 0001000100010001ii i ii ii i i i iia T d ( 31) 1cossincossinsincos sincoscoscossinsin 0sincos 0001iiiiiii iiiiiii i i iiia a T d ( 32) The above general transformation matrix between subsequent coordinate frames can be used to determine the unique location of the end effector for a specified set of PAGE 28 28 joint variables. This is termed as the forward kinematics (FK) for a serial robot manipulator. The problem of inverse kinematics (IK) where the objective is to find a set of joint val ues to satisfy a desired location of a tool is not as straight forward to solve. There are various methods of solution for the IK problem that can be broadly classified into analytical or closed form and iterative solution techniques. These include cyclic coordinate descent methods (Wang et al 1991), pseudoinverse methods (Whitney,1969), Jacobian transpose methods ( Wolovich, 1964 and Balestrino et al 1984). A detailed description of the forward and inverse kinematics of robot manipulators can be found in Craig (1989) and Spong et al (2006). The IK method used in this thesis is based on the pseudoinverse of the Jacobian method, as given by Corke (1996). Manipulator Dynamics The dynamic equations for a manipulator explicitly describe the relationship bet ween force and motion. The equations of motion are important to consider in the design of robots, as well as in simulation and animation, and in the design of control algorithms. The socalled Euler Lagrange equations describe the evolution of a mechanical system subject to holonomic constraints. The Euler Lagrange system of equations can be used to derive the dynamical equations in a straightforward manner if the kinetic and potential energy of the system are expressed in terms of a set of generalized coor dinates. This section aims to lay out a foundation of the dynamics of robot manipulators and a summary of the Euler Lagrange theory which was used in the development of the dynamics equations of the hydraulic manipulator considered in the thesis. The theor etical development below is adapted from Spong et al (2006). PAGE 29 29 The general form of the Euler Lagrange equation can be derived by computing the kinetic and potential energies of an object in motion. Consider a onedegree of freedom constrained to move vertic ally as shown in Figure 313. Using Newtons second law of motion, the equation of motion of the particle can be obtained as myfmg ( 33) where my can be written as 21 ()() 2 dd dK mymymy dtdtydty ( 34) Here, 21 2 Kmy ( 35) Is the kinetic energy of the system. In a similar fashion, using the gravitational term in Equation 3 3 we get () P mgmgy yy ( 36) Here, Pmgy ( 37) is the potential energy of the system. The Lagrangian of the system is now be defined as LKP ( 38) Using Equation 3 8 the equation of motion of the particle can be rewritten as dLL f dtyy ( 39) Equation 3 9 gives the general form of the Euler Lagrange equation. PAGE 30 30 Thus, the dynamic equations of a system can be derived by expressing the kinetic and potential energies of the system in terms of a set of gen eralized coordinates. In the context of dynamics of a rigid link manipulator, the Euler Lagrange equation can be derived by deriving the equations for the kinetic and potential energies for the links using the D H joint variables discussed earlier. Referr ing to Figure 314, where the origin of the coordinate frame attached to the rigid object in space is at the center of mass of the object, the kinetic energy of the system is given as the sum of the kinetic energies due to translational and rotational moti ons Thus, 11 22TTKmvvI ( 310) where m is the mass of the object, v and are the linear and angular velocities respectively and I is the inertia tensor of the object. The inertia tensor can be computed for the body attached frame as 22 22 22()(,,) ()(,,) ()(,,) (,,) (,,) (xxxyxz yxyyyz zxzyzz xx yy zz xyyx xzzx yzzyIII IIII III where Iyzxyzdxdydz Ixzxyzdxdydz Iyxxyzdxdydz and IIxyxyzdxdydz IIxzxyzdxdydz IIyz ,,) xyzdxdydz ( 311) PAGE 31 31 The diagonal elements of I are called the principal moments of inertia and the off diagonal elements are called the cross products of inertia For a body with uniform distribution of mass and the origin of the body attached coordinate frame is located at the center of mass, as is assumed in deriving the E L equations for the hydraulic manipulator, the cross products of inertia are identically zero. Assuming a rectangular cross section of links, as in the case of the hydraulic manipulator shown in Figure 310, the principal moments can be computed as 22 22 22() 12 () 12 () 12xx yy zzm Ibc m Iac m Iab ( 312) where a is the length, b is the width and c is the height of the solid rectangular object shown in Figure 315. The kinetic energ y of a rigid link robot composed of n such links can be obtained using the Jacobian matrix, which gives the relate the linear and angular velocities to the generalized joint variables as ()()iiivvJqqandJqq ( 313) Assuming the mass of each link is given by mi and the inertia tensor for each link with the body attached coordinate at the center of mass, Equation 3 10 and Equation 3 13 can be used to obtain 11 {()()()()()()} 2iii in TTTT ivv iii iKqmJqJqJqRqIRqJqq ( 314) PAGE 32 32 Writing 1{()()()()()()}iii in TTT ivv iii iDmJqJqJqRqIRqJq ( 315) 1 2TKqDq ( 316) The potential energy can be obtained as 1in T ic iPmgr ( 317) Where the gravity vector g gives the direction of gravity in the inertial frame and icr gives the coordinates of the center of mass of link i. Using the general form of the Euler Lagrange equation derived in Equation 3 9 and using the equations of the kinetic and potential energies of the n links of the manipulator, the equations of motion for an n axis manipulator can be derived to obtain the equation DqqCqqqgq ( 318) where D is an configuration dependent matrix called the inertia matrix, The term C in Equation 3 18 is called the Centripetal Coriolis matrix, involving the effects of the centripetal and centrifugal forces of motion. The (k,j)th element of Cqq is defined by Christoffel symbols (of the first kind) as follows: 11 : 2n kj ij ki kj i ijkdd d c qqq ( 319) where di,j are the entries of the nxn inertia matrix D(q). The gravity vector g(q) is given by PAGE 33 33 1()[(),......()]T ngqgqqq ( 320) is vector of generalized forces associated with the generalized coordinates q. The above formulations of the manipulator kinematics and dynamics are utilized in the development of position control strategies for the hydraulic manipulator. The DH parameters for the manipulator are derived and the coordinate systems are assigned to each of the 4 joints as described above. A closed form, iterati ve pseudoinverse of the Jacobian of the robot approach is used to obtain the inverse kinematics of the manipulator. The Euler Lagrange equations of motion are derived using inertial parameters of the manipulator obtained experimentally. Nonlinear Control System Development One approach to designing controllers for nonlinear systems is to linearize the dynamics of system about a local, nominal region of operation for nonlinear parameters using standard linearizing techniques. The linear model can then be used for designing a controller. However linear controllers applied to nonlinear systems suffer from the following limitations: 1. Since any linearization is an approximation in the neighborhood of an operating point, a linear controller can only predict the local behavior of the system. A linear controller cannot be used to analyze the stability of the system when operating at points far away from the nominal operating conditions and thus cannot be used for a global stability analysis 2. Linear controllers cannot be used to analyze the performance of systems with multiple equilibrium or rest points, since linear systems have only one equilibrium point. Nonlinear systems may have one, several or no equilibrium points. Thus, linearizing a nonlinear system with several equilibrium points may lead to a loss of information about all the equilibrium points, leading to inadequate analysis re sulting in the system tending towards instability. 3. A fully controllable model, for example, a unicycle, whose dynamics are nonlinear, may become uncontrollable due to linearizing its model about PAGE 34 34 particular states of the dynamic model. Linear models may suffer from potential loss of controllability and observability. 4. Dynamic system behaviors exclusive to nonlinear models like finite escape time, limit cycles and periodic oscillations cannot be described by linear control system theory. Improper application of linearization techniques to such models may end up destabilizing the system. It can thus be inferred that an effective control approach for a nonlinear system such as the hydraulic actuator system would be to incorporate the nonlinear characteristics of the system dynamics and to design nonlinear controllers that can adapt to modeling uncertainties. The following definitions, theorems and lemmas, obtained from various sources cited will be used in the design and stability analysis of the nonlin ear controllers implemented in this thesis. UNIFORMLY CONTINUOUS Let () ft be a function of time on [0,) The function () ft is uniformly continuous if for each positive number 0 there exists a positive number 0 such that 10 1010()()max{0,} ftftforttt w here t1 is a specific instant of time (de Queiroz et al., 2000) CONTINUOUSLY DIFFERENTIABLE A function () ft is said to be differentiable at x if the limit 0() ()() '()limhfx fxhfx fx xh exists. f(t) is continuously differentiable if 2 2f x exists and is bounded. (Khalil, 2002) NORMS OF A FUNCTION. Let () ft be a function of time on [0,) The 2norm of f(t) is defined as 2 2 0()() ftfd If 2() ft then we can say 2() ftL T he infinity norm of f(t) is defined as ()sup()tftft If () ft then we can say () ftL i.e. f(t) is bounded. (de Queiroz et al.) PAGE 35 35 DECRESCENT AND RADIALLY UNBOUNDED. A function (,) xt is decrescent if (,)0() xtasxt (,)() Radiallyunboundedifxtasxt LYAPUNOV FUNCTION. Consider a nonlinear system ()nxfx Let () Vx be a continuous function with continuous first partial derivatives in the neighborhood of the origin in n Suppose, V is positive definite i.e. (0)000 VandVforx Then, V is called a Lyapunov function candidate for the system. If 0 V ,then V is a Lyapunov function for the nonlinear system. (Spong, 2006) Concepts of stability of a nonautonomous system (Slotine, 1991) : The equilibrium point 0 x is stable at t0 if for any 0 R ,there exists a positive scalar r(R,t0) such that 0()()oxtrxtRtt Th e equilibrium point is asymptotically stable at t0 if 001) 2)()0()()0 itisstable rtsuchthatxtrxtast T he equilibrium point is exponentially stable if there exists two positive number and such that 0() 00()ttxtxett Lyapunov theorem for nonautonomous systems (Khalil, 2002) : Let 0 x be an equilibrium point of the system ()nxfx and n DR be a domain containing the equilibrium point. Let :[0,) VxDR be a continuously differentiable function such that 12()(,)() xVxtx where 12and are positive definite functions in D and PAGE 36 36 (,)0 VV fxt tx Then, x=0 is uniformly stable. i.e. If V(x,t) is positive definite and decrescent and 0 V then, the equilibrium point is stable. The equilibrium point is globally uniformly stable if V(x,t) is radially unbounded. Barbalats Lemma (Slotine, 1991) : If the differentiable function f(t) has a finite limit as t and if f (t) i s uniformly continuous, then ()0 ftast Corollary to Barbalats Lemma (Slotine, 1991) : If a scalar function V(x,t) satisfies the following conditions : 1. (,) Vxt is lower bounded 2. (,) Vxt is negative semi definite 3. (,) Vxt is uniformly continuous in time Then, (,)0 Vxtast PAGE 37 37 Figure 31. MAGALI apple picking arm (Grand DEsnon et al., 1987). Figure 32. CITRUS picking arm structures: (a) Pantographic structure showing motion pattern, (b) Telescopic (Rabatel et al., 1995). Figure 33. Citrus picking robot (Harrell et al., 1988). PAGE 38 38 Figure 34. Kubota arm (Sarig, 1993). Figure 35. Schemes for self guided gantry robot for picking fruit (a) over row of trees (b) betweenrow configuration (Sarig, 1993). PAGE 39 39 Figure 36. Picking arm structure for Agribot (Ceres et al., 1998). Figure 37. Picking arm structure of first prototype (Cavalieri and Plebe, 1996). PAGE 40 40 Figure 38. Dual arm layout for the second prototype (Muscato et al., 2005). Figure 39. End effector for citrus harvesting (Flood, 2006). PAGE 41 41 Figure 310. 4 DOF hydraulic manipulator (RPRP configuration) (Sivaraman, 2006). PAGE 42 42 Figure 3 11. Describing the DH parameters (Spong, 2006) Figure 31 2 Coordinate frame assignment and D H parameters (Craig, 1989) PAGE 43 43 Figure 313. One degree of freedom system constrained to move vertically (Spong, 2006) Figure 314. A general rigid body in spac e with translational and rotational degrees of freedom. The origin of the coordinate system attached to the body is at the center of mass of the object (Spong, 2006) PAGE 44 44 Figure 315. Uniform rectangular solid (Spong, 2006) PAGE 45 45 CHAPTER 4 METHODS AND PROCEDURES An overview of the methods and procedures used to accomplish the objectives of this research work are presented in this chapter. The primary objectives as stated in C hapter 2 are: 1. Study the mechanical design of the manipulator and the hydraulic and electr ic circuit components that are used to operate the manipulator. 2. Design and set up a framework for real time implementation of position controllers, in a hardwarein loop (HIL) environment. 3. Design linear and nonlinear controllers for position control of the manipulator. 4. Implement a real time joint level closed loop position control system with the use of feedback transducers, data acquisition boards and solenoid valve drivers. Mechanical Design of the Manipulator The mechanical system development of the hydraulic manipulator for which the control system algorithms are to be developed was due to the efforts of Sivaraman (2006). A 4 DOF manipulator was designed to emulate the last four joint link pairs of a redundant 7 DOF robot. The joints were actuated using hydraulic rotary actuators chosen due to their high power to weight ratio and the elimination of additional gear reductions, which are crucial for direct drive manipulators, as well as consideration of economic factors. The hollow, square link cross sections provided high stiffness and adequate space for actuator placement. The hydrauli c components of the hydraulic test circuit were selected from the design specifications for the robot manipulator. A fixed displacement pump with 7.570 L/min and 6895 kPa flow and pressure capacity, respectively, was used to power all four joint actuators. The actuators selected were of the rotary vane type with limited joint range. Proportional dual solenoid valves and solenoid drivers were used for the valve control of the actuators. The maximum PAGE 46 46 pressure setting for the circuit was used using a pump side pressure relief valve. The mechanical deadband of the valves were reduced through the current setting on the controllers. Rotary potentiometers were used for position feedback from the individual joints. Data acquisition (DAQ) boards were used to read the analog voltages from the potentiometers as well as to send appropriate voltage signals to control the operation of the solenoid valves. Framework for Real Time Implementation of Position Controllers RT LAB Previous effort in the control system development for the manipulator was implemented with C++ programming using Microsoft Foundation Classes (MFC). To facilitate a model based approach for control system development and to implement a real time controller for the manipulator, the current research focused on developing controllers using MATLAB / Simulink. A framework for real time implementation of controllers was realized by using a software called RT Lab from Opal RT Technologies. RT LAB is a real time simulation platform for highfidelity plant simulation, control system prototyping, and embedded data acquisition and control. RT LAB facilitates the design process of engineering systems by using dynamic models developed in Simulink in real time with hardwarein loop. The following sections di scuss the basic aspects of programming in Simulink in conjunction with RT LAB. The section is adapted from OPAL RTs RT LAB Manual. Working w ith RT LAB RT LAB software runs on a hardware configuration consisting of (1) Command station (2) Compilation no de, (3) Target nodes (4) I/O boards The Command Station is a PC workstation that operates typically under Windows 2000/XP and serves as the user interface. The Command Station enables the user to PAGE 47 47 e dit and modify models ; see model data; r un the original model under its simulation software ( Simulink, SystemBuild) ; g enerate, separate and distribute code; control the simulator's Go/Stop sequences. The Command Station and target node(s) communicate with each other using communication links and for hardwarein the loop simulations. Target Nodes may also communicate with other devices through I/O boards. The Target Nodes are real time processing and communication computers. The real time target nodes performs r eal time execution of the models simulation ; r eal time communication between the nodes and I/Os ; a cquisition of the models internal variables and external outputs through I/O modules ; i mplementation of user performed online parameters modification. The Compilation Node is used to compile generated C c ode. A discussion of RT LABs user interface and software layout can be found in the Appendix A For the real time control system implementation for the hydraulic manipulator, the components required for RT LAB were set up appropriately. The Command stati on used was a PC running Windows XP, with MATLAB/Simulink R2007b. The Command station formed the compilation station as well. The Target station used was a PC running real time QNX 6.3.2 OS. Data acquisition cards from National Instruments were connected to the target station for analog input/output operations. NI DAQCard 6024E 200 kS/s, 12Bit, 16 Analog Input Multifunction DAQ was used to read the analog input signals from the potentiometers for joint angles feedback. NI PCI 6713 12 Bit, 8 Channel Analog Output Board was used to output desired voltages to the solenoid driver for valve control. PAGE 48 48 Building a Distributed Model for RT LAB Any Simulink model can be implemented in RT LAB with some modifications made to distribute the model and transfer it into the simulation environment. RT LAB works by separating and distributing a complex model into smaller subsystems synchronized to run in parallel. T wo key steps define the modifications needed for a Simulink model to run with RT L AB: (1) regroup the model into calculation subsystems (2) i nsert OpComm communication blocks. OpComm communication blocks are part of a block library specifically defined for the RT LAB interface. They are used by RT LAB to identify parameters required for communication between the nodes in the hardware configuration. The subsystems represent each node in the real time network. There are two types of subsystems available in RT LAB: Console: The Console subsystem is the Command station where the user interacts with the system. It contains all the Simulink blocks related to acquiring and viewing data (scope, manual switch, etc.). Any of these blocks which are required by the user, whether during or after the execution of the real time model, should be included in the Console Subsystem. There can be only one Console per model. Following a naming convention, the console subsystems begin with the prefix SC_. Master: The Master c omputation subsystem is responsible for the models real time calculation and for the overall synchronization of the network. In a system containing Hardwarein the Loop (HIL), this subsystem is also responsible for I/O communication. The Master includes S imulink blocks that represent operations to be performed on signals or on I/O icons. There can be only one Master subsystem per model. Master subsystems are named beginning with the prefix SM_. PAGE 49 49 OpComm blocks : Once the model is grouped into Console and com putation subsystems, special blocks called OpComm must be inserted into the subsystems. These are simple feedthrough blocks that intercept all incoming signals before sending them to computation blocks within a given subsystem. OpComm blocks serve three p urposes: 1. When a simulation model runs in the RT LAB environment, all connections between the main subsystems (SC_, SM_) are replaced by hardware communication links. 2. OpComm blocks provide information to RT LAB concerning the type and size of the signals being sent from one subsystem to another. 3. OpComm blocks inserted into the Console Subsystem allow you to select the data acquisition group you want to use to acquire data from the model, and to specify acquisition parameters. Rules for inserting OpCom m blocks : 1. When inserting OpComm blocks into the Console (SC_) subsystem, there must be a maximum of one OpComm block for each Acquisition Group. The Acquisition Group number is specified in the OpComm mask. 2. When inserting OpComm blocks into a Master (SM_) subsystem, there are two kinds of communication associated with Target Node (real time and nonreal time) but these cannot be sent through the same OpComm block. There must be a maximum of one OpComm block for all real time communication (communicati on between Target Nodes), and a maximum of one OpComm block for all nonreal time communication (communication between the Console and a Target Node), for a total maximum of two OpComm blocks in any Master (SM_) or Slave (SS_) subsystem. Compiling and running the model : Once the model is set up as described above, the model can be compiled for real time execution. During compilation RT LAB fully automates the following processes: 1. Separates the Model into the SC_, SM_, and SS_ subsystems defined. The now gr ouped block diagram will be split into smaller diagrams, each associated with a PAGE 50 50 different processor. There will be one diagram generated for each SC_, SM_, or SS_ subsystem. 2. Automatically generate s code on the Master and Slave computers. The Simulink sub models are coded into C language by the MATLAB Real Time Workshop (RTW) module. RTW also creates a makefile according to a specified template. Since real time models are executed under the QNX environment, the template used is one designed for compil ing under the QNX operating system. During execution of the models real time simulation, the SC_ model can interact with the simulation, since C code is not generated for the Console. 3. A utomatically compiles the C code. The sub model files, now coded in C, are then compiled within the QNX environment. Files required for compiling are transferred by Ethernet link from your NT workstation to a workstation operating under QNX. This QNX station compiles all C files for the submodels, to generate files ready for execution on the Target Node. 4. A ut omatically applies the panel settings. After the code is generated and the model compiled, the user must set execution options before running the simulation. These options deal with assigning the model to physical CPU s in the network, and setting the configuration parameters through the Configuration panel. Linear a nd Nonlinear Controller Design for Position Control of the Manipulator In order to design linear and nonlinear controllers for the manipulator, a mathemat ical model for describing the kinematics and dynamics was developed as presented in Chapter 3 The DH parameters for the robot were identified and the forward and inverse kinematics algorithms were developed. The dynamics of the manipulator were also model ed using the Euler Lagrange equations. The nonlinear control system for the hydraulic manipulator was then designed. Given the high complexity and coupled nature of the equations resulting from the dynamics of the manipulator, a unified integrator backstepping to address the nonlinear control of the manipulator and the hydraulic actuation system simultaneously proved unfeasible. This was mainly due to the fact that the nonlinear model of the manipulator dynamics was PAGE 51 51 not be linearly parametrizable, as is the requirement for the adaptive backstepping control algorithm. Thus, the nonlinear control of the sys tem was split into two modules: 1. Exact model knowledge nonlinear control of the 4 DOF manipulator subjected to unknown disturbances 2. Adaptive Cascade control of the hydraulic actuation subsystem using Integrator backstepping technique. The nonlinear controllers were then compared to linear control algorithms to evaluate the improvement in performance of position tracking. Implementing a Real Time Joi nt Level Closed Loop Position Control System A joint space controller was developed in Simulink for the position tracking control of the manipulator and implemented in real time using the RT LAB framework. The joint angles were measured using rotary potentiometers and read via the DAQ system. Positional experiments were carried out to calibrate the potentiometers and to calculate the angle turned to voltage change ratio. Given a desired coordinate translation of the end effector, the required joint angles to appropriately position the end effector were found using the inverse kinematics of the manipulator. The vector of desired joint angles was then converted to a vector of desired voltages of the potentiometers. The joint space controller then used these desired voltages and real time potentiometer readings to drive the joints to the desired angles. In what follows, t he mathematical development for the control systems and their imple mentations are described, with stability analyses to discuss the expected performance of each algorithm implemented. PAGE 52 52 CHAPTER 5 PHYSICAL AND MATHEMA TICAL MO DEL DEVELOPMENT FOR THE MECHANICAL AND HYDRAULIC SUBSYS TEMS OF THE MANIPULA TOR This chapter provides a detailed description of the mechanical and hydraulic design of the 4 DOF manipulator. The characteristics of the hydraulic components described illustrate the nonlinearities inherent in the system and thus serve to highlight the need for the design of nonlinear controllers to achieve precise position control. Following the description of the physical design, the mathematical model of the manipulator is presented using techniques of forward and inverse kinematics and Euler Lagrange dynamics discussed in Chapter 3. The mathematical models of the kinematics and dynamics equations of the manipulator will then be used to synthesize various position controllers in Chapter 6 and Chapter 7. Mechanical Design o f t he Hydraulic Manipulator The hydraulic manipulator was initially designed as the distal joint link pairs of a redundant 7DOF manipulator, the layout for which was obtained after rigorous analysis of the manipulability and dexterity of various manipulator configurations ( Sivaraman, 2006). Figure 51 shows the joint link layout of the 4 DOF system and Figure 52 shows the construction of the manipulator. The primary objective for the mechanical design of the citrus harvesting manipulator was to achieve high stiffness in a small and light structure. This was accomplished in part by choosing aluminum (Al 6061 alloy) for the fabrication of the links and employing hollow square cross sections of the links to house the actuators. As ease of assembly was sought for the construction of the join t link pairs, thin walled aluminum plates were held together by fasteners to form the tubular structure. PAGE 53 53 The actuators were located inside a stationary joint link structure and the torque was transmitted to the next joint link structure through the actuat or shaft. To overcome the overhanging load moment on the actuator shaft, single double row angular contact ball bearings were used. The subsequent joint link structure was attached to a roll collar that is directly coupled to the actuator shaft through a k eyway cut on the inner diameter of the collar. The inner race of the angular contact bearing was slip fit onto the outer surface of the roll collar and the outer race of the bearing was held stationary within the bearing housing by a flange plate. The actu ator was attached on the opposite side of the bearing housing. The broken down view of the roll joint assembly without the cover plates is shown in Figure 53. In the case of the pitch joints, the actuators were mounted as in the case of roll joints and by using a pair of miter gears (bevel pair with a ratio of 1:1) to transmit motion across right angles. The driving gear was mounted directly on the output shaft of the hydraulic actuator while the driven pinion was assembled on a shaft whose axis is perpendicular to the driving gear and was suspended on a pair of single row deep groove ball bearings that were attached on a fork type machined part. Figure 54 shows an assembly of the pitch joint. Rotary Hydraulic Actuation System Design V alve controlled system s offer faster responses to valve and load inputs as compared to a variable displacement pumpcontrolled system s. Due to s maller contained volumes and the ability to feed several valve controlled systems from a single hydraulic power supply, the valv e controlled system was chosen for the citrus harvesting manipulator. C ommercially available servo valves for continuous control of fluid flow are expensive in multiple valve applications and therefore undesirable for the PAGE 54 54 citrus harvesting application wher e low cost is a necessity for acceptance in the agricultural industry. T he performances of the spool valves controlled by proportional solenoids have improved greatly over the years minimizing the use of costly servo valves. Therefore, appropriate proporti onal directional control valves were chosen for the hydraulic actuation system. Figure 55 shows the hydraulic circuit for the manipulator actuation. As can be seen from the circuit, all the actuators were powered by a single power supply. The power suppl y was retrofitted with a relief valve on the pump side to regulate system pressure that can be monitored by a pressure gauge. All the fluid connections in the circuit were achieved through hoses and end fittings to match the individual ports of the hydraul ic elements. The fluid flow from the pump was transferred via a distribution manifold that has four parallel circuits for the four actuators. Each actuator was controlled by an individual dual solenoid controlled proportional directional valve. The valves are closedcentered threeposition four way cartridge type valves mounted on aluminum ported bodies. The return path for the fluids from the actuators is combined at the return manifold and then routed back to the reservoir through a 10 micron filter to c lear away any impurities and trapped air. The reservoir has an inbuilt heat exchanger that cools the oil temperature that can be monitored by a temperature gauge attached on the power supply unit. The primary selection criteria for the valves were the flo w rate and pressure requirements. All of the proportional valves and the relief valve were obtained from Hydraforce (Lincolnshire, IL). The manifolds were obtained from McMaster Carr (Atlanta, GA) and the hoses and fittings were resourced from Bearings and Drives (Gainesville, FL). As the hose lengths were short, PAGE 55 55 the pressure drops through the hoses were insignificant as compared to the pressure drops at the various fittings. All of the ports in the hydraulic components except for the power supply were spec ified to be SAE with O rings to provide good sealing. Although the power supply had quick disconnect fittings at the supply and return connections, minimal leakage was detected during operation. Valve Configuration The spool valves are typically classified by the number of ways the fluid can enter and leave the valve, the number of lands, and the valves centering configuration when in neutral position. A schematic of the four way valves used for controlling the actuation of the manipulator is shown in Figur e 56. In the figure, the ports 1 and 2 were pressurized to the levels indicated by P1 and P2 and used for directing fluid flow to and away from the load. When the spool was moved in the positive X direction, flow was allowed into port 1 with a volumetric flow rate Q1 while port 2 was opened to the return line with a volumetric flow rate Q2. The flow direction was reversed when the spool was moved in the opposite direction. The supply and the return pressures and the corresponding flow rates are designated Ps and Pr, and Qs and Qr, respectively. As the valves were closedcentered, the land widths were greater than the port widths leading to an overlapped region. The flow gain for the valves is shown in Figure 57. The flow gain indicates the presence of a deadband Db due to the overlap region that required the valves to be moved a certain distance before the flow metered in. Thus, in the deadband region, an input signal did not cause a corresponding output signal thereby causing a nil response signal band and typically resulted in a steady state error. The mechanical deadband characteristic was compensated for using the electronic deadband jump setting available on the dual solenoid amplifier. PAGE 56 56 The linearity that exists between the valve displacement and the input current makes the output proportional to the input thus making these valves proportional directional control valves. Identical valves were used for all the joints and the valve input to output performance curve is shown in Figure 58. For 12 volts dire ct current (VDC) operational mode, the flow metered in at 0.20 A and full flow was realized at (0.50 to 0.55) A. Dual Solenoid Amplifier for Valve Control The dual solenoid amplifier used for valve control was obtained from Hydraforce (Lincolnshire, IL). T he multiple input and output electrical signal formats for the controller/amplifier allowed for flexibility in designing the control system. The user specified selections can be made on the controller through the Dual Inline Package (DIP) switch settings. The voltage input signal mode was chosen for all the joints with a range of 10 VDC. The supply voltage to the controller was matched to the solenoid valve coil rating of 24 VDC. The output mode used was analog proportional direct current signal with a range of (0 to 2) A. The valve controller was equipped with a high frequency (20 kHz to 30 kHz) PWM converter and a low pass filter to generate the output current signal with the pulse amplitude and duty cycle directly proportional to the input voltage. The c ontroller also has an electronic deadband jump setting to provide corrective signal for the mechanical deadband present in the spool valve. Although the overlap reduces leakage in the null position and provides greater safety during power failure or emergency stop situations, it was undesirable for continuous position control of the actuators. The deadband jump was accomplished through the selection of the corresponding DIP switch and by setting the minimum current signal needed for the PAGE 57 57 solenoid coils to al low fluid flow through spool movement. This will cause the valve spool to jump to this setting when power is applied. Also, the maximum current signal was used to limit the gain of the amplifier for the experiments ensuring that the manipulator joints were operated at a safe speed level by restricting the maximum valve opening for full input signal. The minimum and the maximum current settings for the four joints and the maximum joint speeds that were observed empirically are shown in Table 5 1. The spool v alve movements were also affected by stiction and hysteresis. Stiction causes the valve spool to not move for small changes in the signal input and hysteresis is the characteristic of the spool to shift differently depending on the direction of the control signal even for identical input values. These nonlinearities of the spool valve were reduced through the dither setting on the electronic controller. Dither is a small, rapid movement of the spool around the desired position. The constant movement helps a void stiction and averages out hysteresis. The low frequency dither signal was generated separately and superimposed on the high frequency PWM output signal. T he dither amplitude was about 1% of maximum current and the frequency was about 140 Hz for the solenoid valves used in the electro hydraulic control circuit. The other settings for the controllers were ramps for the valve coils that aid in the smooth transition of the abrupt input signal changes. The default value for the ramp setting was 0.01 seconds for acceleration and deceleration and was used for all joints to maintain the fast valve response times. The proportionality of the output current signal to the input voltage signal is presented in Figure 59. PAGE 58 58 Feedback Transducer The angular positions of the manipulator joints were sensed with rotary potentiometers that converted the mechanical rotations of the actuators to proportional electrical voltage signals through the movement of a wiper attached to the potentiometer shafts. The potentiometer shaft s were coupled to the back ends of the actuator shafts, while the bodies of the potentiometers were held stationary through the use of Lshaped brackets. A 12 VDC signal was input to the potentiometer terminals at opposite ends and the actuator shaft posit ions were determined by reading the output voltage from the middle terminal through the DAQ board and were used for the closed loop electrohydraulic control of the actuator positions. Two sets of potentiometers used were obtained from ETI systems (Carlsbad, CA) and were single turn with an electrical angle of 320 340 and had 10 k calculated to be 0.03073 V/deg and has a rotational life of 10 million turns. Data Acquisition Boards The data generation and handling between the various components was accomplished through the use of data acquisition cards. Four analog inputs to read the joint angles through the potentiometer feedback and four analog output signals to send the commanded voltage to the valve contr ollers/amplifiers were needed for the electrohydraulic control system. Two DAQ cards from National Instruments (Austin, TX) were used for this purpose. The analog input values from the potentiometers were sampled at 1 kHz and 10 samples at a time. The man ufacturer specifications for the electrohydraulic control system components that include the dual solenoid operated valves, dual solenoid amplifiers, PAGE 59 59 potentiometers for angular position feedback, and the data acquisition cards can be found in Appendix B. Mathematical Model Development f or t he Hydraulic Manipulator In order to develop control system algorithms for a manipulator, a detailed mathematical model of its kinematics and dynamics is essential. The following discussion presents the forward and inver se kinematic descrip tions of the 4 DOF manipulator, followed by the development of the Euler Lagrange dynamic equations. Forward Kinematics As seen in Chapter 3, the forward kinematics problem is concerned with the relationship between the individual joints of the manipulator and the position and orientation of the end effector. The forward kinematics for the hydraulic manipulator was solved for by first using the DH convention to identify the kinematic parameters. Figure 5 10 shows the coordinate frames attached to each joint link system of the manipulator. then obtained using conventional rules and are given in Table 52. As all the joints are of rotary type, the j Using the homogeneous transformation matrix Ai presented in Equation 32 the transformation from each (i)th link to the (i 1)th link can be obtained. The transformation matrix is presented here again f or clarity. 1 1cossincossinsincos sincoscoscossinsin 0sincos 0001iiiiiii iiiiiii i i ii i ia a T d ( 51) PAGE 60 60 Using the above equation and the DH parameters of the manipulator, the transformation matrices from each joint to the previous joint are calculated as given below. 11 11 0 1 1cos0sin0 sin0cos0 010 0001 T d ( 52) 22 22 1 2cos0sin0 sin0cos0 0100 0001 T ( 53) 33 33 2 3 3cos0sin0 sin0cos0 010 0001 T d ( 54) 44 44 4 3cossin00 sincos00 0010 0001 T ( 55) The transformation matrix from the end effector coordinate system to the base coordinate system is obtained by 00123 412341112131 2122232 3132333 0001 rrrp rrrp TTTTT rrrp ( 56) PAGE 61 61 where r11 = cos(tht4)*(sin(tht1)*sin(tht3) cos(tht1)*cos(tht2)*cos(tht3)) cos(tht1)*sin(tht2)*sin(tht4) r12 = sin(tht4)*(sin(tht1)*sin(tht3) cos(tht1)*cos(tht2)*cos(tht3)) cos(tht1)*cos(tht4)*sin(t ht2) 13cos(tht3)*sin(tht1) cos(tht1)*cos( tht2)*sin(tht3) 145.2*cos(tht1)*sin(tht2) r p r21 = cos(tht4)*(cos(tht1)*sin(tht3) + c os(tht2)*cos(tht3)*sin(tht1)) sin(tht1)*sin(tht2)*sin(tht4) r22 = sin(tht4)*(cos(tht1)*sin(tht3) + cos(tht2)*cos(tht3)*sin(tht1)) cos(tht4)*sin(tht1)*sin(t ht2) 23cos(tht1)*cos(tht3) cos(tht2)*sin(tht1)*sin(tht3) 245.2*sin(tht1)*sin(tht2) r p r31 = cos(tht2)*sin(tht4) cos(tht3)* cos(tht4)*sin(tht2) r32 = cos(tht3)*sin(tht2)*sin(tht4) cos(tht2)*cos(tht4) 33sin(tht2)*sin(tht3) 345.2*cos(tht2) + 50.8 r p The tool offset from the end effector coordinate system was found to be 8.42 cm. 40 8.420 1ToolToolOffsetP cm ( 57) 004 4 Tool ToolPTP ( 58) Using E quation 5 7 and Equation 5 8 the position of the end effector can be obtained with respect to the base coordinate system for a given set of joint angles. The forward kinematics algorithm was implemented in MATLAB and the MATLAB code is given in Appendix C. Inverse Kinematics The inverse kinematics (IK) problem is concerned with finding a set or sets of joint variables that can position and orient the end effector at a desired point in space. The problem of IK is in general more complex than the forward kinematics problem. While PAGE 62 62 t he forward kinematics problem always has a unique solution, the IK problem may or may not have a solution depending upon the configuration of the robot and may have multiple solutions that can position the manipulators end effector at the desired point. I f no solution can be determined for a particular manipulator pose, that configuration is said to be singular The singularity may be due to an alignment of axes reducing the effective degrees of freedom, or the desired point being out of reach. Several analytical and closedform techniques exist for solving the IK of a manipulator. For real time implementation of inverse kinematics, closedform solutions are preferred given the computational complexity of numerical solutions. One such method for iteratively solving the IK problem is by using the pseudoinverse of the Jacobian of the manipulator (Corke, 2008). The Jacobian of a robot can be given by the following expression, referring to Figure 511 (Spong, 2006): vJ J J ( 59) Where for a revolute joint, the terms Jv and J are given by 11 1[...] () [...]in i in ivvv vini iJJJ Jzoo JJJ Jz ( 510) In Equation 5 10 1 iz represents the z axis of the (i 1)th joint, no refers to the origin of the coordinate frame attached to the end effector given relative to the base frame and 1 io is the origin of the (i 1)th joints coordinate system, also expressed relative to the base frame. PAGE 63 63 For the hydraulic manipulator, using the DH parameters and conventions, the Jacobian was calculated using Equation 5 9 as 2245.2*s1*s2 45.2*c1*c2 0 0 45.2*c1*s2 45.2*c2*s1 0 0 0)00 0 s1 c1*s2c3*s1 c1*c2*s3 0 c1 s1*s2c1*c3 c2*s1*s3 1 0 c2 (45.2*2*1(45.2*2*1) s2*s3 scss ( 511) where s1, s2, s3 refer to sin ( 1), sin 2) sin 3) and c 1, c 2, c 3 refer to cos1), cos2), cos 3) respectively. The manipulator Jacobian matrix transforms velocities in joint space to velocities of the end effector in Cartesian space. For an n axis manipulator the endeffector Cartesian velocity is given by 0 nxJq ( 512) where x is the Cartesian velocity and q is the joint space velocity. Thus, from Equation 5 12, the inverse problem of finding the joint velocity given the desired Cartesian velocity can be obtained by finding the pseudoinverse of the Jacobian J from which the inverse position solution can be found. The IK routine available in the Robotics Toolbox for MATLAB by Corke (2008) was used to solve the IK problem for the hydraulic manipulator. Since the manipulator had less than six degrees of freedom to place the end effector arbitrarily in 3D space, the pseudoinverse routine was suitably modified to address the issue. Such a solution is completely general, though much less efficient than specific inverse kinematic PAGE 64 64 solutions derived symbolically. The pseudoinverse algorithm was chosen given the ease of implementation in a real time setting. The modifi ed code is provided in Appendix D. Euler Lagrange Equations for the Manipulator Dynamics The Euler Lagrange (E L) equations of motion are a general set of differential equations that describe the time evolution of mechanical systems subjected to holonomic constraints, when the constraint forces satisfy the principle of virtual work (Spong et al 2006). The method based on the principle of virtual work is used to derive the E L equations of the hydraulic manipulator. Equation 33 gives the general form of the E L equations for a manipulator. The equation is presented below: DqqCqqqgq ( 513) For a detailed discussion of the derivation of the above equation, the reader is referred to the works of Spong et al (2006). The terms in Equation 5 13 are briefly discussed below. The term D(q) called the inertia matrix is a nxn matrix, where n is the number of links of the manipulator. 1(){()()()()()()}iii in TTT ivv iiiw iDqmJqJqJqRqIRqJq ( 514) Here, im mass of link i. iI Inertia matrix of link I evaluated around a coordinate frame parallel to frame I but whose origin is at the center of mass of the link. (),()iivwJqJq are the components of the Jacobian matrix given in Equation 5 10 PAGE 65 65 ()iRq is the orientation transformation from the body attached frame and the inertial frame. The term C in Equation 5 13 is called the Centripetal Coriolis matrix, in volving the effects of the centripetal and centrifugal forces of motion. The (k,j)th element of Cqq is defined by Christoffel symbols (of the first kind) as follows: 11 : 2n kj ij ki kj i ijkdd d c qqq ( 515) where di,j are the entries of the nxn inertia matrix D(q). The gravity vector g(q) is given by 1()[(),......()]T ngqgqqq ( 516) where k kP g q ( 517) and Pi is the potential energy of the link i given by iT iicPmgr ( 518) A MATLAB code to symbolically compute the terms in the equations was developed and is given in Appendix E The inertia matrix, Cor iolis matrix and the gravity vector for the hydraulic manipulator are obtained in terms of the joint angles. The kinematic and dynamic equations obtained here lead to the nonlinear control system development It is to be noted that for subsequent control s ystem development, the complex and coupled dynamic equation are linear in certain parameters, such as link masses, moments of inertia etc. The Euler Lagrange equation given in Equation 513 can be written as PAGE 66 66 ()(,)()(,,) DqqCqqqgqqqq Y ( 519) where the function (,,) qqq Y which is a nxl matrix for a n link manipulator is called the regressor matrix and l is the parameter vector. However, in general, it is difficult to find a minimal set of such linearizable parameters that can parametrize the dynamic equations a challenge faced in the current work that prevented a control system development for the coupled hydraulic and robotic systems taken into consideration as a single system. The control strategy instead focused on developing nonlinear controller for the mani pulator subsystem and an adaptive nonlinear controller for the hydraulic subsystem separately. PAGE 67 67 Figure 51. Manipulator joint link layout (Sivaraman, 2006) Figure 5 2 Assembled manipulator prototype. (Sivaraman, 2006) PAGE 68 68 Figure 5 3 Roll joint exploded assembly. (Sivaraman, 2006) Figure 54. Pitch joint assembly. (Sivaraman, 2006) PAGE 69 69 M Prop. Dir. Control Valves Hydraulic Power Supply Relief Valve Supply Manifold Rotary Vane Actuators Reservoir Figure 55. Hydraulic circuit schematic for valve controlled motor motion. (Sivaraman, 2006) Figure 56. Overlapped four way spool valve schematic. PAGE 70 70 Figure 5 7 Flow gain for a closedcenter valve. Figure 5 8 Proportional solenoid valve current input to flow output performance Figure 59. Input to output signal proportionality of the dual solenoid amplifier Acceptable Range PAGE 71 71 Figure 510. Coordinate frame assignment for joint link pairs according to DH conventions Figure 51 1 Motion of the end effector due to revolute joint i. (Spong, 2006) PAGE 72 72 Table 51 Deadband jump and maximum amplifier gain setti ngs. Joints Pressure and Temperature for Experiments Amperage setting for Solenoid A, A Amperage setting for Solenoid B, A Approximate maximum speed rad/s J0 J1 J2 J3 Pressure = 5171 kPa Temperature = (26.67 to 37.7) C I min = 0.258 Imax = 0.306 Imin = 0.272 Imax = 0.324 Imin = 0.216 Imax = 0.234 Imin = 0.233 I max = 0.240 I min = 0.233 Imax = 0.306 Imin = 0.229 Imax = 0.266 Imin = 0.241 Imax = 0.251 Imin = 0.271 I max = 0.279 T able 5 2. Mechanism parameters for the four DOF manipulator. Link a i (cm) i (rad) d i (cm) i (rad) 1 0 50.8 1 (0 2 0 0 2 (0 3 0 45.2 3 (0 4 0 0 0 4 (0 PAGE 73 73 CHAPTER 6 DEVELOPMENT AND IMPL EMENTATION OF CONTRO L SYSTEMS FOR THE HYDRAULIC MANIPULATO R The main objective of the thesis was to formulate differe nt control system strategies for the task space and joint space position control of the hydraulic manipulator described in Chapter 5. One of the primary functions of any robot application is to accurately follow a specified path, given that all points in t he trajectory are in the reachable workspace of the manipulator. In this chapter, three different controllers a Proportional Joint Space and Cartesianspace position Controller, a Feedback Linearization Controller and an Integrator Backstepping Adaptive Controller are then proposed and their implementations are discussed. The stability analyses of the controllers are formulated. The results from the simulations and implementation of the controllers are provided. Proportional Joint Space and Cartesian Spa ce Joint Level Position Control In the previous effort in implementing a position controller for the hydraulic manipulator, a purely proportional closed loop control system was implemented through the electrohydraulic valves driven by dual solenoid driver s ( Sivaraman, 2006). The control system and a user interface were developed in Visual C++ using Microsoft Foundation Classes (MFC) on a Windows platform. Since only a basic positioning system was required to study the working of the manipulator, the controller was implemented without considering the kinematics of the manipulator. The algorithm relied on the user input of desired individual joint angles and used a closed loop error system of the joint angles to drive the joints to the desired angular positions. This limited the flexibility of usage of the controller as Cartesian translations could not be used in positioning the manipulator at a desired point in space. This led to the improvement of PAGE 74 74 controller design to incorporate manipulator kinematics in im plementing joint space and Cartesianspace positional control as part of this thesis. In order to implement a model based real time control of the manipulator, the mathematical models of the manipulator described in Chapter 5 were used to develop models in MATLAB / Simulink. The real time HIL software, RT Lab was used to implement a real time controller. The following section describes the implementation of a real time joint space position control using Simulink and RT Lab. Control System Algorithm The fram ework of the proportional joint space controller implemented can be broadly given by the following algorithm: 1. Obtain the current joint angles using the potentiometer readings 2. Use the current joint angles and calculate the coordinates of the tool point relative to the base coordinate frame by using the forward kinematic model of the manipulator. 3. When the user inputs a desired position or translation for the end effector, use the inverse kinematics of the manipulator to obtain the corresponding desired j oint angles. 4. Using a closed loop error system of the joint angles, drive the joints to the desired angular positions that set the end effector at the desired location. Description of Control System Implementation Implementing the algorithm consisted of developing a Simulink model i ncorporating RT Lab blocks according to the suggested guidelines as presented in Chapter 4. Figure 61 shows a schematic block diagram of the implementation of the controller. Figure 62 shows the Simulink block diagram of the master subsystem of the joint space positional controller. Figure 63 shows the Console subsystem and PAGE 75 75 Figure 64 shows the two subsystems connected together to form the main Simulink model The MATLAB codes for the various blocks are given in Appendix F This section aims to describe the implementation of the controller developed. RT Labs Analog In and Analog Out blocks were used as part of the software interface to read voltage signals from the potentiometer and send Pulse Width Modulated (PWM) voltag e signal to the solenoid drivers. The Analog In and Analog Out blocks are configured for the particular NI DAQ boards that were used. Low pass filters were implemented in the software for attenuating the noise from the potentiometer readings. The Detect Change blocks were used in conjunction with the IfAction blocks to trigger responses based on when the user provided the input to avoid conflicts during the execution of motion. The Detect Change blocks ensured that the user had control over when a partic ular trajectory was to be executed, after fully specifying the Cartesian translations desired. The potentiometer readings were converted to joint angles using the calibrated voltage per angle ratio and the joint angles were then used in the forward kinemat ics to obtain the instantaneous coordinates of end effector. The controller Simulink model shown in Figure 62 incorporated a Homing block. The home position of the manipulator was chosen as when the four joints lie along a straight horizontal line (i. e. the z axis of the base coordinate system). The potentiometers on each joint at this angular configuration were set to output approximately half the voltage of their operational range (i.e. ~ 5V given the full range of 0 10V). The voltages of the potenti ometers at each joint in the home position are given in Table 6 1. Any joint motion could thus be calculated relative to its home position. The angle to voltage block and the voltage to angle block incorporated this relative PAGE 76 76 motion measured with respec t to the home position by calculating the shift in potentiometer voltage reading from the home position voltage reading. To avoid any inaccuracies due to the inverse kinematics solution while trying to home the manipulator, the homing operation was achieved through a feedback closedloop error system of potentiometer voltages, driving the error between the instantaneous joint angle value and its home joint angle value to zero. The instantaneous end effector coordinates relative to the base coordinate frame was computed using the forward kinematics algorithm discussed in Chapter 5. The homogenous transformation matrix for obtaining the coordinate frame attached to end effector relative to the base coordinate frame was given in Equation 52 and is given below for clarity. 00123 412341112131 2122232 3132333 0001 rrrp rrrp TTTTT rrrp ( 6 1 ) where r11 = cos(tht4)*(sin(tht1)*sin(tht3) cos(tht1)*cos(tht2)*cos(tht3)) cos(tht1)*sin(tht2)*sin(tht4) r12 = sin(tht4)*(sin(tht1)*sin(tht3) c os(tht1)*cos(tht2)*cos(tht3)) cos(tht1)*cos(tht4)*sin(t ht2) 13cos(tht3)*sin(tht1) cos(tht1)*cos( tht2)*sin(tht3) 145.2*cos(tht1)*sin(tht2) r p PAGE 77 77 r21 = cos(tht4)*(cos(tht1)*sin(tht3) + c os(tht2)*cos(tht3)*sin(tht1)) sin(tht1)*sin(tht2)*sin(tht4) r22 = sin(tht4)*(cos(tht1)*sin(tht3) + cos(tht2)*cos(tht3)*sin(tht1)) cos(tht4)*sin(tht1)*sin(t ht2) 23cos(tht1)*cos(tht3) cos(tht2)*sin(tht1)*sin(tht3) 245.2*sin(tht1)*sin(tht2) r p r31 = cos(tht2)*sin(tht4) cos(tht3)* cos(tht4)*sin(tht2) r32 = cos(tht3)*sin(tht2)*sin(tht4) cos(tht2)*cos(tht4) 33sin(tht2)*sin(tht3) 345.2*cos(tht2) + 50.8 r p The tool offset from the end effector coordinate system was found to be 8.42 cm. 40 8.420 1ToolToolOffsetP cm ( 6 2 ) T he tool coordinates relative to the base coordinate system can then be obtained from 004 4 Tool ToolPTP ( 6 3 ) Using E quation 6 1 and Equation 6 3 the position of the end effector can be obtained with respect to the base coordinate system for a given set of joint angles. The user input consisted of a desired translation along the X, Y and Z axes. This translational input was used to obtain the desired set of joint angles using the inverse kinematics algorithm, which were then converted to des ired voltages on the potentiometer. The inverse kinematics algorithm to compute the desired joint angles given the desired coordinates of the end effector was based on the pseudoinverse of the Jacobian of the manipulator provided with the Robotics Toolbox for MATLAB (Corke, 2008). The ikine function given in the toolbox was suitably modified given that the hydraulic manipulator consisted of only 4 joints, whose Jacobian is less than full PAGE 78 78 rank. Such a solution is completely general, though much less effici ent than specific inverse kinematic solutions derived symbolically. The pseudoinverse algorithm was chosen given the ease of implementation in a real time setting. The modified code is provided in Appendix D This iterative solution relies on an initial guess of the joint angles. In the implementation of the IK algorithm in the joint level controller, the initial guess of the joint angles was taken to be the joint angle values obtained at the instant when the user entered the desired translation values. If the joint angles required to position the endeffector at a desired point deviated largely from the initial guess, the pseudoinverse of the Jacobian matrix led to errors due to inaccurate convergence of th e solution. Also, the method performs poorly at the singular positions of the manipulator and thus the accuracy of the method depends on the instantaneous configuration of the manipulator. These deficits posed constraints in the implementation of the contr oller, where a desired translation of more than 10 cm along any axis led to significant errors in the IK solution. However, the pseudoinverse of the Jacobian of the manipulator proved to be an effective algorithm to be use d in a real time implementation o f the controller. The valve controllers received a voltage signal from the DAQ system and sent a Pulse Width Modulated (PWM) current signal proportional to the flow rate to the valve solenoids. The angular position was sensed continuously and the generated error signal was used to proportionally control the actuator motion. The absolute angular positions of the actuators were sensed using rotary potentiometers that produce voltage signals proportional to the measured angles. The sensed signals were then fed back and compared with the command signals of the PAGE 79 79 desired angular pos itions. The error signal created from this operation was then scaled to generate the proportional voltage signal. The Solenoid Voltage Output block was then used to send out appropriately scaled voltages to the dual solenoid amplifier that amplified the signal to energize the valve solenoids through a pulse width modulated current signal. The valve remained in the spring centered position until one of the solenoids was energized. Energizing the solenoids caused the spool to move to a particular position and stay until the solenoids were deenergized. The valves were continuously adjusted to correct for errors in the detected angular positions thereby executing the closed loop control of hydraulic manipulator. The hydraulic system pressure was set at 5171 K Pa and the oil temperature was 26.67 C. The Console subsystem shown in Figure 62 was used as a Graphical User Interface to control the operation of the position controller. The console served two purposes : 1. The console was designed as a dashboard to di splay the various real time information related to the manipulator control, including the current potentiometer and tool position readings, desired tool positions, solenoid output voltages and errors in the inverse kinematics algorithm. 2. The console included a user input interface, modeled as sliders, to obtain the desired translation in the 3 axes. A slider switching module was also incorporated to switch the hydraulic circuit on and off for ensuring safe operation of the manipulator. Development of a n A bsolute Positioning System for Joint Control In addition to the above joint level controller using inverse kinematics, another absolute positioning controller using only real time voltage inputs for each joint was designed and implemented. This controller used real time voltage inputs, sent through RT Labs Parameter dialog box that enables an onthe fly change of inputs to the PAGE 80 80 model. By manually changing the voltage input for each joint ( 10 to +10V), the joint can be positioned at any desired angle. The Simulink model for the Master subsection consisted of the AnalogIn block for voltage inputs from the potentiometer and a forward kinematics block for calculating the instantaneous endeffector position. These blocks were modeled in the same method as described earlier for the joint level controller implementation. In this case, instead of a solenoidout block to automatically calculate the applied voltages for the solenoid drivers, 4 unit step response blocks for controlling each joint of the manipulator were used. The amplitude value of these blocks could be changed in real time using RT Labs Parameter dialog box. Thus the voltage output to the solenoid drivers could be manually adjusted to position a joint at a desired angle. Analog Out block configured similar to the method used in the joint level inverse kinematics controller was used to output the voltages to the solenoid driver via the NI DAQ. The Simulink model for the voltageinput positioning controller is shown in Figure 6 5 Figure 6 6 shows t he Simulink models console, RT Labs real time parameter selection dialog box and RT Labs main window. The model console shows the current value of each potentiometer reading as well as the instantaneous tool position relative to the base frame computed using forward kinematics. Description of the Hardware system for Joint level Control System I mplementation In order to use RT Lab with Simulink, the hardware was setup as given in Chapter 4. A Command station x86based PC with ~909 MHz processor and 768 megabytes of memory and running Windows XP and MATLAB R2007b was interfaced with a x86based PC running a real time QNX 6.3.2 OS. Crossover Ethe rnet connection was used to facilitate the real time communication between the two stations while PAGE 81 81 OpComm blocks were used to facilitate data transfer between the two sub systems of the Simulink model. The NI DAQs through which the voltage signals were sent to the solenoid drivers (NI DAQ 6713) and through which voltage signals were received from the potentiometers (NI DAQ 6024) were installed on the Command station. The DAQ boards were appropriately shielded to minimize the disturbance in the signals due t o noise from the electric and electromagnetic components in the system. A block diagram of the system layout is shown in Figure 6 7 Robot Manipulator Joint level Position Controller Verification To test the performance of the inverse kinematics based joint level position controller, the manipulator was translated to various obtainable end effector positions using the real time controller. Cartesian translation required along the three orthogonal axes of the manipulator was used as the input. For each tra nslation, the controller recorded the starting coordinates and the desired coordinates of the end effector. The actual coordinates of the end effector after the translation were obtained using the voltage input from the potentiometers and using forward kinematics to obtain the coordinates. The errors in the position control of the manipulator, i.e. the deviation of the actual coordinates of the end effector vs the desired coordinates can be due to two factors: 1. Due to errors in the hardware components of the hydraulic circuit. These include the errors in the feedback from the potentiometers, errors due to calibration of the potentiometers and errors in the hydraulic valves and actuators. 2. Due to the errors in the inverse kinematics algorithm. The IK algorithm as discussed earlier, suffers from inaccuracies due to errors in the convergence of the iterative solution and dependence on the configuration of the manipulator (i.e. closeness to singularities). PAGE 82 82 To calculate the error due to the IK algorithm, forwar d kinematics was used to determine the end effector coordinates given the solution of joint angles from the IK algorithm. Ten trials of Cartesian translation were implemented and the results are tabulated in Table 62. The results from the positioning experiments are shown in Figures 68, 6 9. Figure 68 shows the plot of the desired coordinates vs the actual coordinates along the three axes. Figure 69 shows the plot of the total error in position of the end effector vs the error due to the positioning controller neglecting errors due to the IK algorithm. From Figure 68 it can be observed that errors depend on the configuration of the robot, with increased values at trials 6 and 7, where the desired translations were close to a singular configuration of th e manipulator at its workspace limit. The workspace limit was due to the joint angles tending towards zeros, placing the joints along an almost horizontal line. Figure 69 compares the total errors in the positioning control of the end effector vs the errors neglecting the effects of the IK solution. This comparison serves to illustrate any inaccuracies in the hardware setup of the system. It can be seen that the errors in positioning, after neglecting the errors due to the IK solutions lie close to zero al ong all three axes Joint level Cartesian and joint space positioning controllers for the hydraulic manipulator were designed and implemented. RT Lab was used to aid in the real time control and implementation of the model based controller. The kinematics of the manipulator was studied and forward and inverse kinematics algorithms were used in the position control of the joints. The controller was modeled in Simulink, taking into consideration the guidelines for real time execution with RT Lab. A dashboar d console was designed to provide the user real time information of potentiometer readings, PAGE 83 83 solenoid driver voltage outputs and end effector coordinates. Desired task space Cartesian translations were input by the user to position the manipulators end ef fector at a desired point in space. Experiments conducted on the task space control of the end effector proved that the controller was able to position the manipulator within 2 cm when the manipulator was operation away from singular c onfigurations. Neglecting the errors due to approximate inverse kinematics, the positioning errors were found to be within 0.5 cm As the robot manipulator is anticipated to be positioned and oriented through continuous feedback from vis ion, proximity, and other sensors mounted in or near the end effector in addition to the absolute positioning algorithm while harvesting a fruit, it can be concluded the position controller offers adequate performance in the Cartesian control of the hydraulic manipulator in real time. PAGE 84 84 F igure 61 Schematic layout of the Joint level position controller PAGE 85 85 Figure 62. Master subsystem for the Simulink model of the Joint level Position Controller PAGE 86 86 Figure 63 Simulink model of the Console subsystem for the Joint level Position Controller (during controller operation) Figure 64 Joint level Cartesian position controller Simulink model on_off j2_h2 Pot1 Pot2 Pot3 Pot4 sol_out Tool_pnt_o Tool_pnt_d Current_tool_point coords SM_Master Pot_reading1 Pot_reading2 Pot_reading3 Pot_reading4 sol_out tool_pnt_o tool_pnt_d current_tool_point coords on_off j2_h SC_Console PAGE 87 87 Figure 65 Master subsystem for the Simulink model of t he absolute joint controller PAGE 88 88 Figure 66. Console subsystem, RT Lab main Window and Parameter selection dialog box PAGE 89 89 Figure 67 System layout for real time implementation of the Joint level Position controller using Opal RTs RT Lab HIL software PAGE 90 90 Figure 68 Desired coordinates of the end effector vs Actual coordinates obtained through joint level Cartesian positional control, along the x,y,z axes PAGE 91 91 Figure 69 Total (actual) error in Cartesian coordinates vs Error in positional controller neglecting err ors due to IK algorithm, along the x,y,z axes PAGE 92 92 Table 61 Voltage readings for the 4 joint potentiometers Joint Home position voltage reading (V) Voltage reading at Maximum joint angle (V) Voltage reading at Minimum joint angle (V) 1 3.97 6.823 1.30 2 4.275 7.419 1.00 3 4.639 7.559 2.00 4 4.49 7.446 1.561 PAGE 93 93 Table 62 Experimental r esults from the j oint level cartesian p ositional controller No Initial coordinates (cm) Desired Translation (cm) Desired final coordinates (cm) Actual coordinates (cm) Error due to Inverse Kinematics (IK) (cm) Total Error in positioning (DesiredActual) (cm) Error in positioning neglecting error due to IK (Desired Actual + Error due to IK (cm) 1 27.57 10 37.57 35.5 2.197 2.07 0.127 0.42 0 0.42 0.4 0 0.02 0.02 95.03 5 90.03 87 2.678 3.03 0.352 2 29.45 0 29.45 28.3 0.42 1.15 0.73 0 5 5 5.5 0 0.5 0.5 93.5 0 93.5 93.5 0 0 0 3 27.95 10 37.95 35 2.861 2.95 0.089 0 5 5 6.2 1.346 1.2 0.146 94.7 5 89.7 87 2.74 2.7 0.04 4 28.83 0 28.83 28.1 0.44 0.73 0.29 0 5 5 5.5 0.03 0.5 0.53 94.03 0 94.03 94.7 0 0.67 0.67 5 27.77 10 37.77 35.3 2.226 2.47 0.244 5.5 0 5.5 2.7 0.9747 2.8 1.8253 94.44 5 89.44 87.3 2.683 2.14 0.543 6 27.65 10 17.65 23.2 6.011 5.55 0.461 1.25 0 1.25 1.03 0.2 0.22 0.42 94.94 5 89.94 98.1 7.751 8.16 0.409 7 22.72 10 12.72 15.5 4.508 2.78 1.728 0.93 0 0.93 0.8 0 0.13 0.13 98.28 5 93.28 100.6 7.712 7.32 0.392 8 11.4 10 21.4 21.3 2.65 0.1 2.75 3.821 0 3.821 1.7 2.52 2.121 0.399 102.8 5 97.8 98.5 1.064 0.7 0.364 9 23.69 0 23.69 23.6 0.4 0.09 0.31 2.03 5 2.97 2.98 0.07 0.01 0.08 97.65 0 97.65 97.65 0.19 0 0.19 10 24.6 0 24.6 23.7 1.597 0.9 0.697 2.988 10 7.012 7.7 0.409 0.688 1.097 97.01 0 97.01 97.03 0.543 0.02 0.523 PAGE 94 94 CHAPTER 7 NONLINEAR CONTROL OF THE HYDRAULIC ACTUAT OR SYSTEM Electro hydraulic actuators are used in several applications. The characteristics and advantages of a using a hydraulic servo system in manipulator control were discussed in Chapter 3. One primary advantage of hydraulic systems over electrical and other me ans of actuation is the high deliverable torque to weight ratio while operating in a mobile environment. However, the hydraulic systems exhibit significant nonlinear behavior and thus, precise control of the system pose a challenging task. The nonlinear ch aracteristics are primarily due to pressure variations and flow characteristics within the actuator, variations in effective fluid volume, fluid compressibility due to temperature variations and inertial parameters of the load. Other factors such as flow f orce effect on valve spool position, frictional effects and transmission nonlinearities also contribute to the complex nonlinear behavior of the hydraulic system. One approach to designing controllers for nonlinear systems is to linearize the dynamics of the hydraulic system about a local, nominal region of operation for nonlinear parameters using standard linearizing techniques (Merritt, 1967, Manring, 2005). The linear model can then be used for designing a controller. However linear controllers applied to nonlinear systems suffer from limitations as described in Chapter 3. Thus, it is desired to develop control system algorithms for the hydraulic actuator system which demonstrates a highly nonlinear behavior. A n effective control approach for the hydraulic actuator system would be to incorporate the nonlinear characteristics of the system dynamics and to design nonlinear controllers that can adapt to modeling uncertainties. Several adaptive and nonadaptive nonlinear controller design techniques exist and have been applied for hydraulic servosystems and robot manipulators as PAGE 95 95 given in Chapter 3. One such design tool for an adaptive (i.e. capable of adapting to model uncertainties) control of hydraulic system described in the previous section is the Integr ator Backstepping control design, based on Lyapunov theory for nonlinear control design and stability analysis described in Chapter 3. Backstepping refers to a recursive procedure that interlaces the choice of a Lyapunov function with the design of feedbac k control (Khalil, 2002). Given that the full system to be controlled consists of several controllable and cascaded subsystems, the design problem for the overall system can be broken down to a sequence of design problems for each subsystem. The dynamics o f the hydraulic system form a socalled strict feedback or cascaded form, suitable for the recursive framework of the Integrator Backstepping controller design. The following section describes the hydraulic system dynamics and model development where behav ioral models for the electrohydraulic and mechanical components or subsystems of the hydraulic actuation system are constructed. Following this, the nonlinear controller design and stability analysis are presented. Results from the simulation of the nonli near controller are then provided. Dynamic Behavioral Model of the Hydraulic Actuator System The system dynamics which describe the behavior of the full hydraulic actuator system can be decoupled into two subsystems: (1) Rotary actuator and payload dynami cs, (2) Solenoid valve dynamics. Figure 7 1 shows the layout of the hydraulic system showing the coupled subsystems. Rotary actuator dynamics : The dynamics of the hydraulic rotary actuator are governed by both hydraulic and mechanical forces such that: 12()act DJqbqkqPPDT ( 71) PAGE 96 96 where and qqq represent the angular position, velocity and acceleration of the actuator vane respectively. J represents the mass attached to the actuator shaft that rotates with the actuator shaft and is to be positioned at a desired angle. and bk represent the damping coefficient and the spring constant of a spring damper sy stem that models the compliance effects of the system and the load. 1P and 2P represent the fluid pressures acting on the control side and the noncontrol side of the rotary actuator. act and D represent the volumetric efficiency of the actuator and the volumetric displacement per swept radian of the actuator and are specified by the manufacturer. DT represents the lumped, unknown dis turbances acting on the system which are assumed to be bounded. The fluid pressures 1P and 2P in Equation 71 are given by the following pressure rise rate equation. Assuming no leakage in the actuator: 11 11 11 2 2 2()() ()ee ePQVQDq VV PDqQ V ( 72) Where e is the effective bulk modulus of the fluid used, 1 2 and V V are the volumes of the control and noncontrol chambers of the actuators and 1 2 and Q Q represents the fluid supply flow rate to the control chamber and fluid return flow rate from the noncontrol chamber respectively. The term Dq represents the change of the actuator control chamber volume relative to the change in the angular position of the actuator vane. 1 2 m mVVV VVV ( 73) PAGE 97 97 where mV equals half the volume of the effective region of vane motion and 1 2and V V = Dq The fluid flow entering and exiting the actuator given in Equation 72 denoted by 1Q and 2Q are derived from the application of flow continuity between the cylinder and the directional valve as given in Merritt (1967) 21 1 1, 1 1r 2 2 2, 2r,0 **1 P,0 ,0 **2 P0sv qv v sv qv vPPz QkzPP Pz PPz QkzPP Pz ( 74) where sP is the supply pressure, rP is the tank pressure. vz is the spool valve position. 1 qk and 2 qk are the constant flow gain coefficients for the forward and return loop respectively. The proportional directional control valve exhibits significant deadband characteristics as modeled in the works of Bu et al (2000). Considering the effects of the deadband in the valve, the net actual valve opening can be given as 0, z vpdvpd nv ndvpd vndvpdzzzz z zz zzzz ( 75) where z,ndpdz are the actual deadbands for the negative and positive spool displacement respectively. The electroproportional spool valve included in the hydraulic system described in Chapter 5 is controlled by means of a Dual Solenoid driver with inbuilt deadband control switch. Using the deadband controller, the deadband characteristics of the valve can be significantly reduced, thus eliminating the need for the inclusion of the deadband characteristics in Equation 75 while designing the adaptive controller. PAGE 98 98 Problem Statement The tracking control problem for the hydraulic actuator system can be given as follows: Given a desired motion trajectory qd(t),synthesize a control input such that the output of the system, i.e. q tracks the desired trajectory as closely as possible, given model uncertainties and unmodeled external disturbances acting on the system. Integrator Backstepping Controller Design The logic behind the cascaded controller development can be seen from the fact that the load pressure defined by 12 LPPP ( 76) can be construed as an control input to Equation 7 6 where by controlling PL, the joint angle q can be controlled. The load pressure is in turn governed by the dynamic system given in Equation 7 2 The fluid flow rates Q1 and Q2 can then be used as control inputs to Equation 7 2 to drive the system to track a desired load pressure (PLd) that drives the error in the joint angles to zero. The fluid flow can be controlled by the spool valve using the dynamics given in Equation 7 4 Thus, by controlling the spool valve position, the cascaded, strict feedback system allows the control of the joint angle output of the actuator. What follows is the control system development for the adaptive integrator backstepping controller. Step 1: Defining the error system The control objective is to have the actuator track a time varying desired trajectory under external load and unmodeled external disturbances. As a means to quantify this objective, the error system e(t) can be defined as PAGE 99 99 deqq ( 77) where ()dqt and its first five time derivatives are assumed to be bounded. This ensures the smoothness of the desired trajectory as given under the section on Lyapunov analysis in Chapter 3. A filtered tracking error (r(t)) is proposed to facilitate the controller design and stability analysis as follows: ree ( 78) where is a positive gain constant. Step 2: Openloop error system for r(t) Taking the time derivative of Equation 7 8 we get ree ( 79) Multiplying Equation 79 by J, we get JrJeJe ( 710) Differentiating Equation 77 twice, we get deqq ( 711) dJrJqJqJe ( 712) Rearranging and substituting for q from Equation 7 1 we get 1 LtdJrCPbqkqTJqJe ( 713) Where 1C is a constant, defined as 1:actCD ( 714) In order to backstep on the PL dynamics, a virtual desired load pressure (PLd) can be added and subtracted to Equation 7 13 as: 1 11ddLtdLLJrCPbqkqTJqJeCPCP ( 715) PAGE 100 100 Here, PLd functions as a virtual control input to the openloop error system in Equation 715. Defining a backstepping error term ( ) to represent the error between PLd and PL, we get 11dLLCPCP ( 716) Thus, Equation 7 15 can now be written as 1dLtdJrCPbqkqJqJeT ( 717) The parametric uncertainties in Equation 7 17 considered in the control system development are the damping coefficient of the actuator and load dynamics ( b ), the spring constant ( kt) and the actuator load ( J). The parametric uncertainties can be collected in a linear parametrization (LP) form, given by (,,,)ddYqqqq as follows: 1, (), d tb qqeqJ k Y ( 718) Here, 1Y is a matrix called the linear regressor matrix which is a matrix of known and measurable signals. is a vector of unknown constants. Using Equation 7 18 Equation 7 17 can now be written as 1dLJrCP1Y ( 719) An adaptation law will be subsequently constructed to facilitate an online estimation of the uncertain parameters in Let represent an estimate of the vector Step 3: Design the virtual control input PLd The virtual control input in Equation 7 19 can now be designed. Looking ahead to the subsequent Lyapunov stability analysis provides a guideline for the design of the virtual control input. The discussion of Lyapunov theory in Chapter 3 dictates that the PAGE 101 101 Lyapunov function candidate must be positive definite and its first derivative negative definite or semi definite to prove the system is stable. The type of stability can be analyzed by the nature of the derivative of the Lyapunov function candidate. In order to facilitate further design of the controller, let a Lyapunov function candidate for the system be given by 121 2LVJr ( 720) Taking the time derivative of Equation 7 20 11()dL LVrJrrCP1Y ( 721) Equation 7 21 dictates a design of the virtual control input PLd, it can be observed that if PLd is designed as 11 ()dLPkr C 1Y ( 722) Substituting Equation 7 22 into Equation 7 21 we obtain 1 ()LVrkr1 1Y ( 723) Defining the instantaneous error in the estimates as ( 724) we get 11()LVrkrY ( 725) Step 4: Develop the open loop backstepping error system ( ) Taking the time derivative of Equation 7 6 and using the pressure dynamics from Equation 7 2 we get 12 12()()ee LPQDqDqQ VV ( 726) PAGE 102 102 Now, the openloop backstepping error system can be given from Equation 7 16 by 11dLLCPCP ( 727) 11 211 12 [()()]eeCQDqDqQkrYY VV ( 728) Where the time derivative of Equation 7 22 was used as 1111 1 1 () LdPkrYYC ( 729) 1 is called the A daptation or gradient update law The design of 1 is again dictated by further Lyapunov analysis that includes the new state Let 1 211 2T LLVV ( 730) where is a positive control gain that can be used to control the rate of adaptation. 21 211 22T LVJr ( 731) Taking the time derivative of Equation 7 31 1 1 2()T LVrkrY ( 732) From Equation 7 32, it is seen that if 1TYr ( 733) 1 (since is a constant and =0)TYr ( 734) Substituting for in Equation 7 32, PAGE 103 103 22 LVrkr ( 735) Substituting Equation 7 33 in the openloop backstepping error dynamics in Equation 7 28, we get 11 2 111 12 ()()T eeCQDqDqQkrYYrY VV ( 736) Define for the sake of simplicity 1122 12: and :eeUCUC VV to be substituted in Equation 7 36 Let 2 321 2LLVV ( 737) 2 3 LVrkr ( 738) 2 3 111222111 ()T LVrkrUQUDqUDqUQkrYYrY ( 739) In Equation 7 39, it can be seen that the fluid flow to the control chamber can be treated as a design controllable input. If Q1is designed such that 11 1 111 11 []T desQQUDqkrYYrYkr U ( 740) and substituting Equation 7 40 in Equation 7 39, we get 22 3 LVkrk ( 741) Equation 7 40 provides the final control input to the system to provide a tracking control of the actuator. As described in the beginning of the controller development, the controlled fluid flow obtained in Equation 7 40 provides a means to ensure the load pressure PL track s the virtual control input PLd, which in turns ensures that the output tracking error e given by Equation 77 PAGE 104 104 deqq converges to zero with guaranteed transient performance and tracking accuracy. Thus, it can be seen that by controlling the fluid flow to the actuators, the actuator and the load can be made to track a desired time varying trajectory in the presence of inherent system nonlinearities, parametric uncertainties and external disturbances. Given the desired spool position, a proportional voltage can be calculated given the spool valve design characteristics provided by the manufacturer. The Lyapunov stability analysis of the control system developed along with the proof of boundedness of the synthesized virtual and actual control inputs are given in below. Theoretically, the proposed controller guarantees a prescribed output tracking transient performance and final tracking accuracy while achieving asymptotic stabi lity in the presence of parametric uncertainties. Stability Analysis of the Designed Controller To analyze the stability of the proposed controller system given in Equation 7 40, Equation 736 and Equation 722, consider the Lyapunov function given by 212111 222TVJr ( 742) Taking the time derivative of Equation 7 42 we get 1 TVrJr ( 743) which was shown to be given as (Equation 7 41) 22 12Vkrk ( 744) The first step is to show that all signals derived are bounded and measurable. It can be seen that the Lyapunov function V is a positive definite function while its time PAGE 105 105 derivative is negative semi definite. Therefore, it can be concluded that V is a decreasing function with respect to time. Thus, V is bounded .. ieVL ( 745) V can be upper and lower bounded as 22 12V ( 746) Where 1 and 2 are positive constants and is given by []Tr ( 747) ,, VLrL rLeeL LL ,dLLLPPL Given that eeL qqL ,,dddqqq were assumed to be bounded. Given that the joint signals and their time derivatives are bounded, 1YL Given 1,dLPandYL rLeLqL Given 1, YrL LLL Given 1,,,,dqqqqeLYL Thus the control load 12, QQL .Thus, L .Thus, LPL The above signal chasing approach thus proves all the signals are bounded. The corollary to Barbalats Lemma can be given as : 1. (,) Vxt is lower bounded 2. (,) Vxt is negative semi definite 3. (,) Vxt is uniformly continuous in time PAGE 106 106 Then, (,)0 Vxtast All the three conditions were proved, with 1()TgtrJr Thus, from the corollary to Barbalats Lemma, it can be proved that the controller implemented leads to a Global Asymptotic Stability of the system. The simulation of the integrator backstepping control of a hydraulic system usi ng full state feedback was carried out on Simulink. The Simulink model is shown in Figure 72 and the model development is given in Appendix G. The desired trajectory to be tracked by the actuator was a sinusoidal reference trajectory with a frequency of 1Hz and amplitude of 2 units. The desired trajectory is thus a bounded function of time. Using this reference signal as an example, it can be proved mathematically that the controller implemented offers the same transient performance for all such signals which are bounded with respect to time. However, a change in the above chosen reference trajectory will dictate a need to tune the gains for achieving the same stability result. Figure 7 3 shows the position tracking results from the simulation. As seen from the figure, the controller implemented results in asymptotically stable tracking performance. Figure 7 4 shows the estimation rates of the parametric uncertainties given by tb J k It can be observed that the estimates closely match the actual values of the parameters. It was desired to compare the performance of the proposed controller with a linear PID controller where the controller takes the form 0()()() t pidd uKetKetdtKet dt ( 748) PAGE 107 107 where ,,pidKKK represent the Proportional (P), Integral (I) and Derivative(D) gains respectively. A PID controller was implemented with the control structure given in(7 48) Given the nonlinear system dynamics, the performance of tracking control using the PID controller proved to be highly unstable, with errors exponentially increasing to infinity. After several attempts to obtain a meaningful tracking control by adjusting the P I and D gains, it was observed that the nonlinear system clearly resulted in an improved tracking control performance. PAGE 108 108 M Prop. Dir. Control Valves Hydraulic Power Supply Relief Valve Supply Manifold Rotary Vane Actuators and load Reservoir Figure 71 Hydraulic system layout showing the valve spool dynamics subsystem and the actuator and load subsystem. PAGE 109 109 Figure 72 Simulink model for the Integrator Adaptive Backstepping Controller for the Hydraulic system PAGE 110 110 Figure 73 Position tracking error of the joint angle of the actuator. PAGE 111 111 Figure 74 Rate of convergence of the parametric estimates PAGE 112 112 CHAPTER 8 NONLINEAR MODEL BASED TRACKING CONTROL OF THE MANIP ULATOR In the joint space and Cartesianspace independent joint control implementation, the 4 joints of the manipulator were treated as decoupled from one another. The controller used the manipulator inverse kinematics to solve for the transformation of motion requirements from the Cartesian operational space to the joint space and a control scheme for the individual joints was developed without considering the dynamics of the manipulator system. While simple positioning tracking control algorithms might be suffic ient in applications requiring low accuracies, the use of the knowledge of manipulator dynamics in developing nonlinear forcetracking controllers allows a more precise tracking control. And such a nonlinear controller can be used to analyze the stability of the system under various initial conditions. This proves to be an advantage of using nonlinear controllers as compared to linear controllers which do not provide means for analyzing the global stability of the implemented controller. Several nonlinear control design techniques available for robot manipulators or mechanical systems in general exists as discussed in Chapter 3. In this section, a link position tracking controller for the hydraulic manipulator in the presence of external disturbances is dev eloped. The Euler Lagrange dynamic equation given in Equation 59 was used in the development of a bounded torque controller. In the implementation of the controller, the robot model parameters were assumed to be known, i.e. the controller developed was an exact model knowledge controller that yielded a Global Uniformly Ultimately Bounded (GUUB) stability result in the presence of unmodelled disturbances acting on the system, while ensuring a bounded torque input. The controller system was simulated using a model built in Simulink. The section PAGE 113 113 concludes with the results from the simulation and the proof of the resulting GUUB stability analysis. Dynamic Model of the Hydraulic Manipulator To develop a torque controlled position controller for an nlink manipu lator, the dynamics of the robot given by the Euler Lagrange system of equations is considered as seen in Chapter 5. Equation 59 which gives the Euler Lagrange equation is presented in Equation 8 1 The system model for a serial rigid link manipulator wit h n links in the presence of bounded disturbance is given by dDqqCqqqgq ( 81) Here, is the control torque, D is the inertia matrix, C represents the centripetal Coriolis matrix, and d is a bounded, unknown disturbance. The dynamic model given in Equation 8 1 represents a multivariable system with n joint torques (inputs) and n joint positions (outputs). Given that the joint torques and angles form a coupled system rather than n decoupled systems, a nonlinear centralized control law for bounded torque control will be sought to ensure high manipulator dynamic performance and trajectory tracking performance (Sciavicco, 1996). The Euler Lagrange system of equations was computed symbolically for the 4 DOF hydraulic manipulator in MATLAB and the program code is giv en in Appendix E For the control system development that follows, the inertial parameters of the robot, specifically the masses of the links, centers of gravity through which the moments of each link acts and the Coriolis and gravitational effects were as sumed to be exactly known. In general, it is acceptable to assume that upper and lower bounds of each term PAGE 114 114 in Equation 8 1 are known. The boundedness and important properties of the dynamics of the manipulators are described in the following properties: Pr operty 1: Boundedness of the Inertia Matrix (D(q)) The inertia matrix of the nlink manipulator (D(q)), with the joints being all revolute is known to be positive definite and symmetric. For a fixed value of the generalized coordinate q, let 10()...()nqq ( 82) denote the n Eigenvalues of D(q), which are all positive due to D(q) being positive definite. Then, it can be shown that 1()()()nxn nnxnqIDqqI ( 83) where nxnI denotes the nxn identity matrix. Given that all the joints are revolute, the inertia matrix contains only terms involving the sine and cosine functions of the joint angles. Thus, it can be shown that there exists constants min and max and the bounds on the inertia matrix can be given as min max()nxn nxnIDqI ( 84) Property 2: The Skew Symmetry property The difference in the matrices given by 1 ()(,) 2 DqCqq ( 85) is skew symmetric. The proof of Equation 8 5 is given in Spong et al (2006). Here, D is the element wise derivative of the inertia matrix with respect to time. The skew symmetry property can be rewritten as PAGE 115 115 1 ()(,)0 2TxDqCqqx ( 86) where n x is an arbitrary vector. Property 3: Boundedness of the Centripetal Coriolis matrix Using the definition for the () normL as: () ftftL an upper bound can be placed on the norm of the matrix (,) Cqq given by (,) Cqqq ( 87) where is a positive scalar constant. Property 4: Linearity in the parameters of the E L equation The complex and coupled dynamic equation are linear in certain parameters, such as link masses, moments of inertia etc. The Euler Lagrange equation can be written as ()(,)()(,,) DqqCqqqgqqqq Y where the function (,,) qqq Y which is a nxl matrix for a n link manipulator is called the regressor matrix and l is the parameter vector. However, in general, it is difficult to find a minimal set of such linearizable parameters that can parametrize the dynamic equations PAGE 116 116 Control Objective The control objective is to design a bounded torque input based controller for link position trajectory tracking, given the knowledge of the bounds of the system parameters in the presence of bounded disturbances. The controller is to force the joint angles to track a desired trajectory, dq .To en sure acceptable performance of the controller, the desired trajectory is designed to be smooth and continuous and it is assumed that the first three time derivatives of the desired trajectory (i.e. and dddqqq ) exist and are bounded. Error S ystem Development To quantify the error in the joint angles, a position tracking error is defined as fo llows eqqd ( 88) A filtered tracking error is defined to facilitate the design and stability analysis: ree ( 89) where is a positive diagonal gain matrix. Differentiating Equation 8 9 and multiplying both sides of the resulting equation by D(q), the inertia matrix, we get DrDeDe ( 810) where d deqq eqq ( 811) Substituting Equation 8 11 in Equation 810 we get dDrDqDqDe ( 812) PAGE 117 117 Substituting for Dq from Equation 8 1 we get ()(,)()()()ddDqrCqqqgqDqqDqe ( 813) Equation 8 11 gives dqeq ( 814) And rearranging the filtered tracking error Equation 8 9 we get ere ( 815) dqreq ( 816) Substituting Equation 8 16 in Equation 813 we get ()(,)(,)(,)()()()dddDqrCqqrCqqeCqqqgqDqqDqe ( 817) Controller Design Fr om the open loop error Equation 8 17 it is desired to design the control input vector of joint torques such that the error between the desired and the actual joint angles goes to zero with prescribed transient stability. Given that the parameters in the robot model are known, the controller can be designed to cancel out the nonlinearities that exist in the system. Thus, the control torque can be designed using a feedback control law given by (,)(,)()()()ddfCqqeCqqqgqDqqDqeKr ( 818) where Kf is a positive feedback control gain matrix. Substituting Equation 8 18 in Equation 817, we can obtain the following closed loop error system: ()(,)fdDqrCqqrKr ( 819) PAGE 118 118 Stability Analysis The reason behind the design of the control torque as given in Equation 818 can be understood by constructing a suitable Lyapunov function candidate for the manipulator dynamics system. Consider a continuously differentiable, positivedefinite Lyapunov function candidate given by 1 () 2TVrDqr ( 820) Differentiating Equation 8 20 with respect to time, we get 1 () () 2TTVrDqrrDqr ( 821) Substituting for () Dqr from Equation 8 19, we get 1 () ( (,)) 2f T d TVr CqDqr qrKr r ( 822) Using the skew symmetry property of the inertia and Coriolis matrices as given in Property 3, Equation 8 6 we get 1 ( ()0 2 )TTrDqrr Cqqr ( 823) Substituting Equation 8 23 in Equation 822 we get ()T fdVrKr ( 824) It can be seen in Equation 8 24 that the time derivative of the Lyapunov function is negative definite. Considering the manipulator operates under the presence of bounded disturbances, Equation 8 24 can be upper bounded as 2 fmVKrr ( 825) PAGE 119 119 where m gives the bound on the disturbance. For the sake of analysis, considering Equation 8 25 without any external disturbance, we get 2 fVKr ( 826) This equation has a closed form solution. max 2 {}fK VV D ( 827) where max{} D is the maximum Ei gen value of the inertia matrix () Dq given in Equation 84 Thus, the closed form solution of Equation 827 can be obtained as max2 () (0)fKt Vtexp D V ( 828) From Equation 828, it can be concluded that the system is globally exponentially stable. Thus, V(t) exponenti ally decays to 0 and is bounded. From Equation 8 20, s ince V and D(q) are bounded, it can be seen that r is bounded. Since r is a function of the error ee it can be concluded that e is bounded. Thus, it can be seen from Equation 8 18 for the designed control torque, all the elements are bounded and thus, the control torque can be concluded to be bounded. Considering again the presence of unmodelled, bound disturbances as given in Equation 8 25, it can be seen that m/frK ( 829) The presence of disturbances leads to a Globally Uniformly Ultimately Bounded (GUU B) stability of the system. PAGE 120 120 The simulation of the nonlinear feedback torque control for trajectory tracking for the 4 DOF manipulator was carried out on Simulink. The Simulink model block diagram is given in Figure 8 1 and t he Simulink model development and t he model parameters are given in Appendix H The desired trajectory to be tracked by the actuator was a sinusoidal reference trajectory for each joint Figure 8 2 8 5 show the position tracking results from the simulation. The err ors in the joint angles are given in Figure 86. As seen from the figure, the controller implemented results in a GUUB stability in the presence of unmodelled, bounded disturbances and the errors can be seen to be bounded. PAGE 121 121 Figure 81 Simulink model block diagram for the Exact Model Knowledge controller for the hydraulic 4 DOF manipulator PAGE 122 122 Figure 82 Actual vs Desired joint angular position of Joint 1 Figure 83. Actu al vs Desired joint angular position of Joint 2 PAGE 123 123 F igure 84 Actual vs Desired joint angular position of Joint 3 F igure 85 Actual vs Desired joint angular position of Joint 4 PAGE 124 124 Figure 86 Errors in the joint angles PAGE 125 125 CHAPTER 9 OVERALL SUMMARY, CONCLUSIONS AND FUTURE SCOPE Summary a nd Conclusions The United States is among the wor ld leaders in citrus production and Florida accounted for 65% of the total United States citrus production. Several factors such as seasonal freezes threaten citrus prod uction and the associated economic returns. With growing competition, s carce availability of seasonal labor and the increased labor costs newer, robust technologies in agricultural mechanization make a compelling argument for research in the applications of automating the Florida citrus harvesting operations. The current research as part of this thesis aimed at address ing the systematic design and development of control systems for a hydraulic robot manipulator for a fruit harvesting application and the r eal time implementation of the position controllers For the purpose of the study, a 4 DOF hydraulic manipulator designed and fabricated at the ARMg Laboratory at the Agricultural & Biological Engineering was considered. Towards meeting the overall goal o f designing and developing control systems for the manipulator first the kinematics and the nonlinear dynamics of the manipulator were studied. Forward and Inverse kinematics routines capable of a real time implementation were developed in MATLAB. The n onlinear dynamic model of the serial chain, rigid link manipulator was developed using Euler Lagrange system of equations. MATLAB was again used to derive the components of the E L equations, namely the inertia matrix, the vector of gravity effects and the matrix of Coriolis centripetal forces that act on the manipulator. The nonlinear dynamics of the hydraulic actuation system was also studied and a theoretical, nonlinear cascaded model of the PAGE 126 126 electroproportional spool valve and the rotary actuator sys tems, used in the positioning control of the manipulator, was developed. The kinematic and dynamic system models developed were then utilized in designing the control systems for the manipulator. The basic function of trajectory tracking control was consi dered to be the primary objective of the controllers. The real time, on the fly control of the manipulator using a model based approach was identified as one important area of research in control system implementation. As a means to achieve this, a hardw are in loop concept for real time control of the manipulator was explored. RT Lab software from Opal RT Technologies helped accomplish the task of real time model based control implementation. After redesigning the hardware and software layout to allow the use of RT Lab with MATLAB/ Simulink, two joint level position controllers were implemented. An absolute positioning controller used real time inputs of solenoid driver voltages to drive the joints of the manipulator to desired angular positions. A kinematics based, joint level Cartesian controller was developed which incorporated the forward and inverse kinematics of the manipulator initially derived and accomplished the requirement for a real time positioning of the manipulator. The user inputs of Cartesian translat ions were used to perform task space control of the manipulator. Results from the implementation of the Cartesian controller showed that positioning accuracies of 2 cm were achieved when the manipulator was operation away from singular configurations. Neglecting the errors due to approximate inverse kinematics, the positioning errors were found to be within 0.5 cm The inverse kinematics algorithm was implemented by using the pseudoinverse of the Jacobian matrix of t he manipulator The algorithm was chosen based on the ease of real time PAGE 127 127 implementation and assurance of at least one solution for the joint angles at any manipulator configuration. It was however observed that the inverse kinematics of the manipulator was prone to inaccuracies due to manipulator configurations and closeness of an initial guess of joint angles to the actual joint angles required. This posed a limit on the maximum translation accurately achievable at a single positioning operation. However, as the robot manipulator is anticipated to be positioned and oriented through continuous feedback from vision, proximity, and other sensors mounted in or near the end effector in addition to the absolute positioning algorithm while harvesting a fruit, it wa s concluded that position controller offers adequate performance in the Cartesian control of the hydraulic manipulator in real time. The final area of research interest was in the implementation of a nonlinear positioning controller for the manipulator. Better tracking performance could be achieved by considering the nonlinearities due to the hydraulic components in the actuating system and due to the dynamics of the manipulator itself. Initially, a holistic approach towards developing an integrator back stepping controller was attempted by considering the two subsystems together. However, the dynamics of the manipulator presented significant difficulties in obtaining a linearly parametrizable form of the dynamic equations which was required in designing t he holistic controller. To overcome this, the nonlinear hydraulic subsystem was considered to be decoupled from the rigidlink manipulator subsystem. This simplification led to the design of an adaptive integrator backstepping trajectory tracking control o f the hydraulic subsystem in the presence of nonlinear uncertainties and unmodelled disturbances. The Lyapunov theory for nonlinear control design and stability analysis was used to design the controller and PAGE 128 128 a globally asymptotically stable result was achi eved for the trajectory tracking problem. Following this, Lyapunov theory was again used to develop an Exact Model Knowledge controller for the manipulator and a globally uniformly ultimately bounded stability result was achieved in the presence of unmodel led disturbances. The bounded torque nature of the EMK controller and ease of real time implementation was found to be an advantage of using the controller, as opposed to certain robust and variable structure controllers, where infinite bandwidth of the ac tuator is necessary for tracking control. Future Work The nonlinear controller development laid a framework for future work in the area of trajectory tracking control of the hydraulic manipulator. The controllers require redesigning the current hydraulic system setup for their implementation. Specifically, given the nature of the torque based controllers, a method to implement the torque inputs is required. This can be addressed by installing pressure feedback transducers on each line of the actuators and by using the relation between the fluid pressure and torque delivered. For implementing an accurate valveforce tracking controller, spool position feedback is required. The electroproportional valves currently installed on the manipulator actuation assem bly do not provide a direct feedback of spool position. Though the manufacturer specification of the proportionality of the current available to the solenoid coil to the displacement force of the spool can be indirectly used for inferring spool position, nonlinearities in the valve due to hysteresis and stiction could lead to potential inaccuracies. Hydraulic solenoid valves with direct position feedback could be installed, reducing these inaccuracies. However, the major deciding factor in the redesigning w ill be the cost of the suggested parts to ensure the hydraulic manipulator meets its initial design and cost requirements. PAGE 129 129 It was observed that the joint level Cartesian controller was affected by the instantaneous configuration of the manipulator. This w as due to the nature of the inverse kinematics algorithm implemented. While the positioning requirements were met within acceptable accuracies in a general reachable workspace of the manipulator, further research into the kinematics of the manipulator coul d lead to better closedform inverse kinematics solutions for the manipulator. An attempt to implement such a closed form solution should take into consideration that the 4 DOF manipulator has inherent redundancy in its design of the second roll joint (joi nt 3 in the roll pitch roll pitch configuration). H igh dexterity of the manipulator is a key requirement to harvest fruits in an unstructured environment of a canopy and in the presence of obstacles due to tree parts. By improving the dexterity, allowing a point in space to be reachable from different orientations, the ability to avoid obstacles is improved. Rigid link robots are limited in their dexterity and reach because of their physical construction. The feasibility of incorporating a class of robot ma nipulators, termed continuum manipulators due to their ability to bend along a smooth curve with constant curvature was briefly studied during this research to enhance the harvestability of the manipulator. Further research efforts in this area are warranted to construct a unified, hybrid rigid link continuum section manipulator to utilize the advantage of the speed and accuracy of operation of a rigid link manipulator as well as the improved dexterity of the manipulator system due to a continuum manipu lator. PAGE 130 130 APPENDIX A WORKING WITH RT LAB USER INTERFACE RT LAB is a distributed real time platform that facilitates the design process for engineering systems by taking engineers from Simulink or SystemBuild dynamic models to real time with hardwarein the loop, in a very short time, at a low cost It is flexible enough to be applied to the most complex simulation and control problem, whether it is for real time hardwarein the loop applications or for speeding up model execution, control and test. Some of the key components of the programs user interface are discussed here. MetaController : The MetaController is the main RT LAB application running in the background. This program is started when the operating system starts. This application acts as a server and is required to open a new model. Main Control Panel : The RT LAB Main Control panel is the graphical user interface which enables you to easily execute the various functions and operations offered by RT LAB. RT LAB MainControl PAGE 131 131 Open Model : Enables selection of the Simulink/SystemBuild file for the model to be processed and executed from a File Open dialog box displayed on screen (the MetaController must be running). The model used must be the toplevel, original model grouped into subsystems and at a subsystem file with an SC_ SM_ or SS_ prefix. Connect : Enables you to connect to any opened simulat ion model. Disconnect : Closes the connection to the current simulation model, while allowing you to connect to it again later, without resetting its simulation values (as with the Reset button). Parameters: Starts the Parameters Control panel, which enables you to modify a models values while the simulation is running. Configuration: Starts the Configuration panel, which enables you to set various options which relate to compiling the model and executing its simulation, and also to the real time netw ork settings. Edit : Opens the original Simulink or SystemBuild model for modification. Compile : Enables you to select the appropriate compilation options and start the model separation, code generation, and compiling of the various subsystems of the distr ibuted model. Assign Nodes : Enables you to assign a physical target node to each of the models subsystems. Load: Loads the executable code onto the various nodes and sets the model's state to PAUSE. Reset : Initializes the system nodes and causes a complete stop and unloading of the system. PAGE 132 132 Execute : Starts the system and the execution of the distributed models simulation on the network. If the Use model console checkbox is checked, the model's console starts automatically the first time you execute the model. Pause : Puts the system in a waiting state, stopping calculation in the system without resetting the nodes. To continue execution, click on Execute. Parameter Control Panel : The Parameter Control Panel manages online modifications of a models parameters (model has been loaded on target) for: p arameters contained in Simulink blocks v ariables originally initialized in the MATLAB workspace All parameters can be changed without recompiling your model. This is helpful when tuning parameters such as controller gains. Simulink parameters are displayed within a tree called Parameters tree. Each branch of the tree represents a level in your Simulink block diagram. The parameters are located at the leaves of the tree. Following is a description of the parameters for Simulink and an explanation of how these can be modified. PAGE 133 133 Parameter Control Panel Model Path : Command Station path where the current model is located. Model Name : Name of the current model. Connect : Connects to any opened simulation model and displays the A ctive models panel, if there are any simulations running. Disconnect : Closes connection to the current simulation model while still enabling you to connect to it again later Number of variables : Total number of MATLAB variables in the Simulink model. Load: Loads parameters' values from a file and displays a parameter dialog box when changes are detected between files and model parameters. PAGE 134 134 Save : Saves the parameters' values to a file. Parameters tableAdd: Adds the selected parameter from the tree to the Parameter Modification Table Selecting a branch recursively adds all children to the table. Parameters tabl e Clear : Removes the selected parameter from the Parameter Modification Table Parameters tableClear All : Removes all parameters from the Parameter Modification Table Parameters tableApply : Applies all modifications made in the Parameter Modific ation Table (values displayed in red). To modify your Simulink models parameters: 1. Select the desired parameter by clicking on its name in the Parameter Tree. 2. Click Add to add the desired parameter to th e Parameter Modification Table. 3. Enter your new v alue(s) directly into the table. If the parameter is a vector or a matrix, click on Edit to access all of the parameters values. 4. Repeat steps 1 through 3 to modify other parameters as required. 5. Once you have set all the parameters you want to modify, click SimulinkApply to send all modified parameters from the table to the real time model. Parameters are modified simultaneously on all real time target nodes. PAGE 135 135 APPENDIX B MANUFACTURER SPE CIFICATIONS FOR THE ELECTRO HYDRAULIC CONTROL SYSTEM COMPONENT S 1. Dual Solenoid Amplifier Datasheet (Hydraforce, IL) PAGE 136 136 PAGE 137 137 PAGE 138 138 PAGE 139 139 PAGE 140 140 PAGE 141 141 PAGE 142 142 PAGE 143 143 2. Electro hydraulic proportional directional control valve (Hydraforce, IL) PAGE 144 144 PAGE 145 145 3. PCI Data Acquisition Card (National Instruments, TX) PAGE 146 146 PAGE 147 147 PAGE 148 148 PAGE 149 149 5. Single Turn Rotary Potentiometer (ETI Systems, TX) PAGE 150 150 APPENDIX C MATLAB PROGRAM FOR COMPUTIN G THE FORWARD KINEMA TICS OF THE HYDRAULIC MANIPULATO R %% Program to symbolically compute the forward kinematics of the 4DOF % hydraulic manipulator. The homogenous transformation matrix A_i_(i1) % gives the transformation from the ith coordinate to the %(i1)th coordinate system %% clear all;clc; % Known system parameters d1=50.8;d3=45.2; % From the DH model of the manipulator syms tht1 tht2 tht3 tht4 % Constructing the transformation matrices A_1_0 = [cos(tht1) 0 sin(tht1) 0 ; sin(tht1) 0 cos(tht1) 0 ; 0 1 0 d1; 0 0 0 1]; A_2_1 = [cos(tht2) 0 sin(tht2) 0 ; sin(tht2) 0 cos(tht2) 0 ; 0 1 0 0 ; 0 0 0 1]; A_3_2 = [cos(tht3) 0 sin(tht3) 0 ; sin(tht3) 0 cos(tht3) 0 ; 0 1 0 d3; 0 0 0 1]; A_4_3 = [cos(tht4) sin(tht4) 0 0; sin(tht4) cos(tht4) 0 0; 0 0 1 0; 0 0 0 1]; %% Transformation matrices from each coordinate system to base frame (Forward kinematics)%% A_4_0 = A_1_0 A_2_1 A_3_2 A_4_3; PAGE 151 151 APPENDIX D MATLAB PROGRAM FOR COMPUTIN G THE INVERSE KINEMA TICS OF THE HYDRAULIC MANIPULATO R %% Program to compute the Inverse Kinematics of the 4DOF % hydraulic manipulator. Given a desired transformation matrix, TR_D it is % desired to find a set of joint angles q=[q1 q2 q3 q4] that results in % desired the end effector translation %% clear all;clc; % Defining a mask matrix that represents the active joints of the 4DOF % manipulator. The mask matrix has to be specified as the robot has less % degrees of freedom than 6, which is required to arbitrarily place the end % effector anywhere in its reachable workspace m=[1 1 1 1 0 0]; % The program takes a vector of joint angles q=[q1 q2 q3 q4] as inputs tht1 = q(1);tht2 = q(2);tht3 = q(3);tht4 = q(4); d1=50.8;d3=45.2; % Known manipulator parameters %% Defining the transformation matrices for each joint A_1_0 = [cos(tht1) 0 sin(tht1) 0 ; sin(tht1) 0 cos(tht1) 0 ; 0 1 0 d1; 0 0 0 1]; A_2_1 = [cos(tht2) 0 sin(tht2) 0 ; sin(tht2) 0 cos(tht2) 0 ; 0 1 0 0 ; 0 0 0 1]; A_3_2 = [cos(tht3) 0 sin(tht3) 0 ; sin(tht3) 0 cos(tht3) 0 ; 0 1 0 d3; 0 0 0 1]; A_4_3 = [cos(tht4) sin(tht4) 0 0; sin(tht4) cos(tht4) 0 0; 0 0 1 0; 0 0 0 1]; %% Transformation matrices from each coordinate system to base frame (Forward kinematics)%% A_4_0 = A_1_0 A_2_1 A_3_2 A_4_3; A_3_0 = A_1_0 A_2_1 A_3_2; A_2_0 = A_1_0 A_2_1; PAGE 152 152 %% The zaxis and origins of each coordinate system w.r.t base frame %% z0 = [0;0;1]; o0 = [0;0;0]; z1 = A_1_0(1:3,3); o1 = A_1_0(1:3,4); z2 = A_2_0(1:3,3); o2 = A_2_0(1:3,4); z3 = A_3_0(1:3,3); o3 = A_3_0(1:3,4); z4 = A_4_0(1:3,3); o4 = A_4_0(1:3,4); %% Defining the linear and angular velocity components of the Jacobian %% Jv_1 = cross(z0,(o4 o0)); Jv_2 = cross(z1,(o4 o1)); Jv_3 = cross(z2,(o4 o2)); Jv_4 = cross(z3,(o4 o3)); Jw_1 = z0; Jw_2 = z1; Jw_3 = z2; Jw_4 = z3; Jv = [Jv_1 Jv_2 Jv_3 Jv_4]; Jw = [Jw_1 Jw_2 Jw_3 Jw_4]; J = [Jv;Jw] % Jacobian of the manipulator Ji = pinv(J) % Pseudo inverse of the Jacobian % Defining an arbitrary translation of 10 units along the z axis of the end % effector coordinate system TR_d = [A_4_0(:,1:3) [o4;1]+[0;0;10;0]]; % Solving the inverse kinematics problem % From the ikine function in % Robotics Toolbox for MATLAB by Peter Corke (2008) d = [ TR_d(1:3,4)A_4_0(1:3,4); 0.5*( cross(A_4_0(1:3,1), TR_d(1:3,1)) + ... cross(A_4_0(1:3,2), TR_d(1:3,2)) + ... cross(A_4_0(1:3,3), TR_d(1:3,3)) ... )]; e = d .* m'; dq = pinv(J) e; q = ([tht1 tht2 tht3 tht4])'; q = q + dq; PAGE 153 153 APPENDIX E MATLAB PROGRAM FOR COMPUTIN G THE EULER LAGRANGE EQUATIONS OF THE HYDRAULIC MANIPULATOR %% Program to symbolically compute the Euler Lagrange dynamics equations % of the 4DOF hydraulic manipulator. Equations 7.47 7.65 given in Chapter % 7 of Spong et al. (2006) are used in the following development clc; clear all; syms s1 s2 s3 s4 c1 c2 c3 c4 d1 d3; % To denote the sines and cosines of the joint angles syms tht1 tht2 tht3 tht4 tht1_d tht2_d tht3_d tht4_d; syms Ixx1 Ixx2 Ixx3 Ixx4 syms Iyy1 Iyy2 Iyy3 Iyy4 syms Izz1 Izz2 Izz3 Izz4 syms lc1 lc2 lc3 lc4 m1 m2 m3 m4 i j k ijk; %% Defining the rotation matrices, using DH parameters %% A_1_0 = [c1 0 s1 0 ; s1 0 c1 0 ; 0 1 0 d1; 0 0 0 1]; A_2_1 = [c2 0 s2 0 ; s2 0 c2 0 ; 0 1 0 0 ; 0 0 0 1]; A_3_2 = [c3 0 s3 0 ; s3 0 c3 0 ; 0 1 0 d3; 0 0 0 1]; A_4_3 = [c4 s4 0 0; s4 c4 0 0; 0 0 1 0; 0 0 0 1]; %% Transformation matrices from each coordinate system to base frame %% A_4_0 = A_1_0 A_2_1 A_3_2 A_4_3; A_3_0 = A_1_0 A_2_1 A_3_2; A_2_0 = A_1_0 A_2_1; %% The zaxis and origins of each coordinate system w.r.t base frame %% z0 = [0;0;1]; o0 = [0;0;0]; z1 = A_1_0(1:3,3); o1 = A_1_0(1:3,4); z2 = A_2_0(1:3,3); o2 = A_2_0(1:3,4); z3 = A_3_0(1:3,3); o3 = A_3_0(1:3,4); z4 = A_4_0(1:3,3); o4 = A_4_0(1:3,4); PAGE 154 154 %% Defining the linear and angular velocity components of the Jacobian %% Jv_1 = cross(z0,(o4 o0)); Jv_2 = cross(z1,(o4 o1)); Jv_3 = cross(z2,(o4 o2)); Jv_4 = cross(z3,(o4 o3)); Jw_1 = z0; Jw_2 = z1; Jw_3 = z2; Jw_4 = z3; Jv = [Jv_1 Jv_2 Jv_3 Jv_4]; Jw = [Jw_1 Jw_2 Jw_3 Jw_4]; J = [Jv;Jw]; % <Jacobian of the manipulator %% Linear velocity component of the Jacobian, considering C.O.M of links %% % Center of mass of each link i in the ith coordinate system % c1_1 = [0;lc1;0;1]; c2_2 = [0;0;lc2;1]; c3_3 = [0;lc3;0;1]; c4_4 = [0;lc4;0;1]; % Center of mass of each link i in the base coordinate system % c1_0 = A_1_0*c1_1; c1_0 = c1_0(1:3,1); c2_0 = A_2_0*c2_2; c2_0 = c2_0(1:3,1); c3_0 = A_3_0*c3_3; c3_0 = c3_0(1:3,1); c 4_0 = A_4_0*c4_4; c4_0 = c4_0(1:3,1); % Computing Jv_ci Jv_c1 = [cross(z0,(c1_0 o0)) zeros(3,3)]; Jv_c2 = [cross(z0,(c2_0 o0)) cross(z1,(c2_0 o1)) zeros(3,2)]; Jv_c3 = [cross(z0,(c3_0 o0)) cross(z1,(c3_0 o1)) cross(z2,(c3_0 o2)) zeros(3,1)]; Jv_c4 = [cross(z0,(c4_0 o0)) cross(z1,(c4_0 o1)) cross(z2,(c4_0 o2)) cross(z3,(c4_0 o3))]; trans_ke_1 = m1*transpose(Jv_c1)*Jv_c1; trans_ke_2 = m2*transpose(Jv_c2)*Jv_c2; trans_ke_3 = m3*transpose(Jv_c3)*Jv_c3; trans_ke_4 = m4*transpose(Jv_c4)*Jv_c4; % Substituting for angles and simplifying above expressions trans_ke_1 = subs(trans_ke_1,s1,sin(tht1)); trans_ke_1 = subs(trans_ke_1,s2,sin(tht2));trans_ke_1 = subs(trans_ke_1,s3,sin(tht3)); trans_ke_1 = subs(trans_ke_1,s4,sin(tht4)); trans_ke_1 = subs(trans_ke_1,c1,cos(tht1)); trans_ke_1 = subs(trans_ke_1,c2,cos(tht2));trans_ke_1 = subs(trans_ke_1,c3,cos(tht3)); trans_ke_1 = subs(trans_ke_1,c4,cos(tht4)); trans_ke_1 = simplify(trans_ke_1); PAGE 155 155 trans_ke_2 = subs(trans_ke_2,s1,sin(tht1)); trans_ke_2 = subs(trans_ke_2,s2,sin(tht2));trans_ke_2 = subs(trans_ke_2,s3,sin(tht3)); trans_ke_2 = subs(trans_ke_2,s4,sin(tht4)); trans_ke_2 = subs(trans_ke_2,c1,cos(tht1)); trans_ke_2 = subs(trans_ke_2,c2,cos(tht2));trans_ke_2 = subs(trans_ke_2,c3,cos(tht3)); trans_ke_2 = subs(trans_ke_2,c4,cos(tht4)); trans_ke_2 = simplify(trans_ke_2); trans_ke_3 = subs(trans_ke_3,s1,sin(tht1)); trans_ke_3 = subs(trans_ke_3,s2,sin(tht2));trans_ke_3 = subs(trans_ke_3,s3,sin(tht3)); trans_ke_3 = subs(trans_ke_3,s4,sin(tht4)); trans_ke_3 = subs(trans_ke_3,c1,cos(tht1)); trans_ke_3 = subs(trans_ke_3,c2,cos(tht2));trans_ke_3 = subs(trans_ke_3,c3,cos(tht3)); trans_ke_3 = subs(trans_ke_3,c4,cos(tht4)); trans_ke_3 = simplify(trans_ke_3); trans_ke_4 = subs(trans_ke_4,s1,sin(tht1)); trans_ke_4 = subs(trans_ke_4,s2,sin(tht2));trans_ke_4 = subs(trans_ke_4,s3,sin(tht3)); trans_ke_4 = subs(trans_ke_4,s4,sin(tht4)); trans_ke_4 = subs(trans_ke_4,c1,cos(tht1)); trans_ke_4 = subs(trans_ke_4,c2,cos(tht2));trans_ke_4 = subs(trans_ke_4,c3,cos(tht3)); trans_ke_4 = subs(trans_ke_4,c4,cos(tht4)); trans_ke_4 = simplify(trans_ke_4); %% Rotational kinetic energy Jw_c1 = [Jw_1 zeros(3,3)]; Jw_c2 = [Jw_1 Jw_2 zeros(3,2)]; Jw_c3 = [Jw_1 Jw_2 Jw_3 zeros(3,1)]; Jw_c4 = Jw; R1 = A_1_0(1:3,1:3); R2 = A_2_0(1:3,1:3); R3 = A_3_0(1:3,1:3); R4 = A_4_0(1:3,1:3); I1 = [Ixx1 0 0; 0 Iyy1 0 ;0 0 Izz1]; I2 = [Ixx2 0 0; 0 Iyy2 0 ;0 0 Izz2]; I3 = [Ixx3 0 0; 0 Iyy3 0 ;0 0 Izz3]; I4 = [Ixx4 0 0; 0 Iyy4 0 ;0 0 Izz4]; rot_ke_1 = transpose(Jw_c1) R1 I1 transpose(R1) Jw_c1; rot_ke_2 = transpose(Jw_c2) R2 I2* transpose(R2) Jw_c2; rot_ke_3 = transpose(Jw_c3) R3 I3 transpose(R3) Jw_c3; rot_ke_4 = transpose(Jw_c4) R4 I4 transpose(R4) Jw_c4; rot_ke_1 = subs(rot_ke_1,s1,sin(tht1)); rot_ke_1 = subs(rot_ke_1,s2,sin(tht2));rot_ke_1 = subs(rot_ke_1,s3,sin(tht3)); rot_ke_1 = subs(rot_ke_1,s4,sin(tht4)); rot_ke_1 = subs(rot_ke_1,c1,cos(tht1)); rot_ke_1 = subs(rot_ke_1,c2,cos(tht2));rot_ke_1 = subs(rot_ke_1,c3,cos(tht3)); rot_ke_1 = subs(rot_ke_1,c4,cos(tht4)); rot_ke_1 = simplify(rot_ke_1); rot_ke_2 = subs(rot_ke_2,s1,sin(tht1)); rot_ke_2 = subs(rot_ke_2,s2,sin(tht2));rot_ke_2 = subs(rot_ke_2,s3,sin(tht3)); rot_ke_2 = subs(rot_ke_2,s4,sin(tht4)); rot_ke_2 = subs(rot_ke_2,c1,cos(tht1)); rot_ke_2 = subs(rot_ke_2,c2,cos(tht2));rot_ke_2 = subs(rot_ke_2,c3,cos(tht3)); rot_ke_2 = subs(rot_ke_2,c4,cos(tht4)); PAGE 156 156 rot_ke_2 = simplify(rot_ke_2); rot_ke_3 = subs(rot_ke_3,s1,sin(tht1)); rot_ke_3 = subs(rot_ke_3,s2,sin(tht2));rot_ke_3 = subs(rot_ke_3,s3,sin(tht3)); rot_ke_3 = subs(rot_ke_3,s4,sin(tht4)); rot_ke_3 = subs(rot_ke_3,c1,cos(tht1)); rot_ke_3 = subs(rot_ke_3,c2,cos(tht2));rot_ke_3 = subs(rot_ke_3,c3,cos(tht3)); rot_ke_3 = subs(rot_ke_3,c4,cos(tht4)); rot_ke_3 = simplify(rot_ke_3); rot_ke_4 = subs(rot_ke_4,s1,sin(tht1)); rot_ke_4 = subs(rot_ke_4,s2,sin(tht2));rot_ke_4 = subs(rot_ke_4,s3,sin(tht3)); rot_ke_4 = subs(rot_ke_4,s4,sin(tht4)); rot_ke_4 = subs(rot_ke_4,c1,cos(tht1)); rot_ke_4 = subs(rot_ke_4,c2,cos(tht2));rot_ke_4 = subs(rot_ke_4,c3,cos(tht3)); rot_ke_4 = subs(rot_ke_4,c4,cos(tht4)); rot_ke_4 = simplify(rot_ke_4); %% Christoffel symbols: D1 = trans_ke_1 + rot_ke_1; D2 = trans_ke_2 + rot_ke_2; D3 = trans_ke_3 + rot_ke_3; D4 = trans_ke_4 + rot_ke_4; D = D1 + D2 + D3 + D4; % <4x4 Inertia matrix syms acc_g; tht = [tht1 tht2 tht3 tht4]; tht_d = [tht1_d tht2_d tht3_d tht4_d]; % Substituting for angles and simplifying expressions for Center of masses c1_0 = subs(c1_0,s1,sin(tht1)); c1_0 = subs(c1_0,s2,sin(tht2));c1_0 = subs(c1_0,s3,sin(tht3)); c1_0 = subs(c1_0,s4,sin(tht4)); c1_0 = subs(c1_0,c1,cos(tht1)); c1_0 = subs(c1_0,c2,cos(tht2));c1_0 = subs(c1_0,c3,cos(tht3)); c1_0 = subs(c1_0,c4,cos(tht4)); c1_0 = simplify(c1_0); c2_0 = subs(c2_0,s1,sin(tht1)); c2_0 = subs(c2_0,s2,sin(tht2));c2_0 = subs(c2_0,s3,sin(tht3)); c2_0 = subs(c2_0,s4,sin(tht4)); c2_0 = subs(c2_0,c1,cos(tht1)); c2_0 = subs(c2_0,c2,cos(tht2));c2_0 = subs(c2_0,c3,cos(tht3)); c2_0 = subs(c2_0,c4,cos(tht4)); c2_0 = simplify(c2_0); c3_0 = subs(c3_0,s1,sin(tht1)); c3_0 = subs(c3_0,s2,sin(tht2));c3_0 = subs(c3_0,s3,sin(tht3)); c3_0 = subs(c3_0,s4,sin(tht4)); c3_0 = subs(c3_0,c1,cos(tht1)); c3_0 = subs(c3_0,c2,cos(tht2));c3_0 = subs(c3_0,c3,cos(tht3)); c3_0 = subs(c3_0,c4,cos(tht4)); c3_0 = simplify(c3_0); c4_0 = subs(c4_0,s1,sin(tht1)); c4_0 = subs(c4_0,s2,sin(tht2));c4_0 = subs(c4_0,s3,sin(tht3)); c4_0 = subs(c4_0,s4,sin(tht4)); c4_0 = subs(c4_0,c1,cos(tht1)); c4_0 = subs(c4_0,c2,cos(tht2));c4_0 = subs(c4_0,c3,cos(tht3)); c4_0 = subs(c4_0,c4,cos(tht4)); c4_0 = simplify(c4_0); %% Forming the Christoffel symbols according to Eq 7.6 for k = 1:1:4 PAGE 157 157 for i = 1:1:4 for j = 1:1:4 c(i,j,k) = 1/2 *(diff(D(k,j),tht(i)) + diff(D(k,i),tht(j)) diff(D(i,j),tht(k))); end end end %% Building the Coriolis matrix for k = 1:1:4 temp = 0; for j = 1:1:4 temp = 0; for i = 1:1:4 temp = c(i,j,k)*tht_d(i) + temp; C(k,j) = temp; end end end %% Potential energy of the robot m = [m1 m2 m3 m4]; grav = [acc_g;0;0]; com = [c1_0 c2_0 c3_0 c4_0]; P=0; for i=1:1:4 P = P + m(i) transpose(grav) com(1:3,i); end for k = 1:1:4 g(k) = diff(P,tht(k)); end g = transpose(g); D = simplify(D) C = simplify(C) g = simplify(g) %% For the EMK controller, the following manipulator parameters are % considered to be known. Refering to Figure 510, the following parameters % are evaluated lc1= 9.91/2;lc2=10.79/2; lc3=6.86/2; lc4=3.25; %Centers for moments of inertia m1=5; m2=5; m3=2.5; m4=2.5; % Masses of each link in kgs acc_g=9.81; %m/s^2 d1=20;d3=17.78; % From the manipulator's DH model % The following values of ai,bi and ci are calculated for computing % the inertia tensor for each link. The links are assumed to be uniform % rectangular solids. ai,bi,ci are in units of inches a1=4.5;b1=7.6;c1=4.5; a2=3.5;b2=7.16;c2=3.5; PAGE 158 158 a3=3.5;b3=5.4;c3=3.5; a4=3;b4=3.19;c4=3; % Principal moments of inertia about link i's x axis Ixx1 = m1/12*(b1^2+c1^2); Ixx2 = m2/12*(b2^2+c2^2); Ixx3 = m3/12*(b3^2+c3^2); Ixx4 = m4/12*(b4^2+c4^2); % Principal moments of inertia about link i's y axis Iyy1 = m1/12*(a1^2+c1^2); Iyy2 = m2/12*(a2^2+c2^2); Iyy3 = m3/12*(a3^2+c3^2); Iyy4 = m4/12*(a4^2+c4^2); % Principal moments of inertia about link i's z axis Izz1 = m1/12*(a1^2+b1^2); Izz2 = m2/12*(a2^2+b2^2); Izz3 = m3/12*(a3^2+b3^2); Izz4 = m4/12*(a4^2+b4^2); g_n = subs(g); D_n = subs(D); C_n = subs(C) PAGE 159 159 APPENDIX F MATLAB/ SIMULINK PROGRAM FOR THE REAL TIME JOINT LEVEL POSITION CONTROLLER %Forward kinematics block% function tp = f_kine(vc1, vc2, vc3, vc4) d1=50.8;d3=45.2; % Robot joint offsets % Based on the home position voltage values for each joint tht1 = 33.9443*(vc1 3.97); tht2 = 32.5415*(vc2 4.275); tht3 = 32.9598*(4.639 vc3); tht4 = 32.9598*(4.49 vc4); tht1 = pi/180*tht1 ; tht2 = pi/180*tht2; % Deg to rad % tht3 = pi/180*tht3 ; tht4 = pi/180*tht4; % Deg to rad % A_1_0 = [cos(tht1) 0 sin(tht1) 0 ; sin(tht1) 0 cos(tht1) 0 ; 0 1 0 d1; 0 0 0 1]; A_2_1 = [cos(tht2) 0 sin(tht2) 0 ; sin(tht2) 0 cos(tht2) 0 ; 0 1 0 0 ; 0 0 0 1]; A_3_2 = [cos(tht3) 0 sin(tht3) 0 ; sin(tht3) 0 cos(tht3) 0 ; 0 1 0 d3; 0 0 0 1]; A_4_3 = [cos(tht4) sin(tht4) 0 0; sin(tht4) cos(tht4) 0 0; 0 0 1 0; 0 0 0 1]; %% Transformation matrices from each coordinate system to base frame (Forward kinematics)%% A_4_0 = A_1_0 A_2_1 A_3_2 A_4_3; tp = [A_4_0(1:2,4); A_4_0(3,4)+8.4138]; % Volt to angle block % % Function to convert from potentiometer voltage values to Joint angles function [tht1,tht2,tht3,tht4] = v_to_a(j2_h,vc1,vc2,vc3,vc4) tht1 = 33.9443*(vc1 3.97); tht2 = 32.5415*(vc2 4.275); tht3 = 32.9598*(4.639 vc3); tht4 = 32.9598*(4.49 vc4); PAGE 160 160 % fw_kine block % % Function to compute the desired transformation matrix and end effector %coordinates given the desired Cartesian translation required function [tool_pt_o,tool_pt_d,q,tr_d] = fw_kine(tht_o,coords) tht1 = pi/180*(tht_o(1));tht2 = pi/180*(tht_o(2)); % Deg to rad % tht3 = pi/180*(tht_o(3));tht4 = pi/180*(tht_o(4)); % Deg to rad % d1=50.8;d3=45.2; % Robot joint offset parameters % %% Desired translation: Units to be moved in each axis %% x = coords(1); y = coords(2); z = coords(3); %% Defining the rotation matrices, using DH parameters %% A_1_0 = [cos(tht1) 0 sin(tht1) 0 ; sin(tht1) 0 cos(tht1) 0 ; 0 1 0 d1; 0 0 0 1]; A_2_1 = [cos(tht2) 0 sin(tht2) 0 ; sin(tht2) 0 cos(tht2) 0 ; 0 1 0 0 ; 0 0 0 1]; A_3_2 = [cos(tht3) 0 sin(tht3) 0 ; sin(tht3) 0 cos(tht3) 0 ; 0 1 0 d3; 0 0 0 1]; A_4_3 = [cos(tht4) sin(tht4) 0 0; sin(tht4) cos(tht4) 0 0; 0 0 1 0; 0 0 0 1]; %% Transformation matrices from each coordinate system to base frame (Forward kinematics)%% A_4_0 = A_1_0 A_2_1 A_3_2 A_4_3; %% desired transformation matrix tr_d = [A_4_0(:,1:3) A_4_0(:,4)+[x;y;z;0]]; q = [tht1 tht2 tht3 tht4]'; tool_pt_o = [A_4_0(1:2,4); A_4_0(3,4)+8.4138]; tool_pt_d = [tr_d(1:2,4); tr_d(3,4)+8.4138]; PAGE 161 161 % inv_kin block % function [q_d, err_ik] = inv_kine(q,TR_d) %% m=[1 1 1 1 0 0]; %% Updating the rotation matrices with new joint angles%% tht1 = q(1);tht2 = q(2);tht3 = q(3);tht4 = q(4);d1=50.8;d3=45.2; A_1_0 = [cos(tht1) 0 sin(tht1) 0 ; sin(tht1) 0 cos(tht1) 0 ; 0 1 0 d1; 0 0 0 1]; A_2_1 = [cos(tht2) 0 sin(tht2) 0 ; sin(tht2) 0 cos(tht2) 0 ; 0 1 0 0 ; 0 0 0 1]; A_3_2 = [cos(tht3) 0 sin(tht3) 0 ; sin(tht3) 0 cos(tht3) 0 ; 0 1 0 d3; 0 0 0 1]; A_4_3 = [cos(tht4) sin(tht4) 0 0; sin(tht4) cos(tht4) 0 0; 0 0 1 0; 0 0 0 1]; %% Transformation matrices from each coordinate system to base frame (Forward kinematics)%% A_4_0 = A_1_0 A_2_1 A_3_2 A_4_3; A_3_0 = A_1_0 A_2_1 A_3_2; A_2_0 = A_1_0 A_2_1; %% The zaxis and origins of each coordinate system w.r.t base frame %% z0 = [0;0;1]; o0 = [0;0;0]; z1 = A_1_0(1:3,3); o1 = A_1_0(1:3,4); z2 = A_2_0(1:3,3); o2 = A_2_0(1:3,4); z3 = A_3_0(1:3,3); o3 = A_3_0(1:3,4); z4 = A_4_0(1:3,3); o4 = A_4_0(1:3,4); %% Defining the linear and angular velocity components of the Jacobian %% Jv_1 = cross(z0,(o4 o0)); Jv_2 = cross(z1,(o4 o1)); Jv_3 = cross(z2,(o4 o2)); Jv_4 = cross(z3,(o4 o3)); Jw_1 = z0; PAGE 162 162 Jw_2 = z1; Jw_3 = z2; Jw_4 = z3; Jv = [Jv_1 Jv_2 Jv_3 Jv_4]; Jw = [Jw_1 Jw_2 Jw_3 Jw_4]; J = [Jv;Jw]; % <Jacobian of the manipulator %% d = [ TR_d(1:3,4)A_4_0(1:3,4); 0.5*( cross(A_4_0(1:3,1), TR_d(1:3,1)) + ... cross(A_4_0(1:3,2), TR_d(1:3,2)) + ... cross(A_4_0(1:3,3), TR_d(1:3,3)) ... )]; e = d .* m'; dq = pinv(J) e; q = q + dq; q_d = q; %% Verifying the accuracy of the inverse kinematics thtn1 = q_d(1);thtn2 = q_d(2);thtn3 = q_d(3);thtn4 = q_d(4);d1=50.8;d3=45.2; A_1_0n = [cos(thtn1) 0 sin(thtn1) 0 ; sin(thtn1) 0 cos(thtn1) 0 ; 0 1 0 d1; 0 0 0 1]; A_2_1n = [cos(thtn2) 0 sin(thtn2) 0 ; sin(thtn2) 0 cos(thtn2) 0 ; 0 1 0 0 ; 0 0 0 1]; A_3_2n = [cos(thtn3) 0 sin(thtn3) 0 ; sin(thtn3) 0 cos(thtn3) 0 ; 0 1 0 d3; 0 0 0 1]; A_4_3n = [cos(thtn4) sin(thtn4) 0 0; sin(thtn4) cos(thtn4) 0 0; 0 0 1 0; 0 0 0 1]; %% Transformation matrices from each coordinate system to base frame (Forward kinematics)%% A_4_0n = A_1_0n A_2_1n A_3_2n A_4_3n; err_ik = A_4_0n(1:3,4)TR_d(1:3,4); % angle to voltage block % function [vd1,vd2,vd3,vd4] = a_to_v(q_d) PAGE 163 163 % Function to calculate desired potentiometer voltages corresponding to a given set of desired joint angles tht1_d = 180/pi*q_d(1);tht2_d = 180/pi*q_d(2); % Rad to deg % tht3_d = 180/pi*q_d(3);tht4_d = 180/pi*q_d(4); % Rad to deg % if floor(tht1_d) == 0 tht1_d = 0; end if floor(tht2_d) == 0 tht2_d = 0; end if floor(tht3_d) == 0 tht3_d = 0; end if floor(tht4_d) == 0 tht4_d = 0; end vd_1 = 0.02946*tht1_d; vd_2 = 0.03073*tht2_d; vd_3 = 0.03034*tht3_d; vd_4 = 0.03034*tht4_d; vd1 = (vd_1 + 3.970); vd2 = (vd_2 + 4.275); vd3 = (4.639 vd_3); vd4 = (4.49 vd_4); % Homing block % function [sol_out1,sol_out2,sol_out3,sol_out4] = homing(vc,on_off) %% Initialize sol_out1 = 0; sol_out2 = 0; sol_out3 = 0; sol_out4=0; %#ok PAGE 164 164 sol_out1 = 0; end % Joint 2 % if (abs(vc(2)4.275) < 0.1) sol_out2 = 0; elseif ((vc(2) > 1) && (vc(2) < 3.95)) sol_out2 = 5; % Allowable speeds at this particular position elseif ((vc(2) > 3.95) && (vc(2) < 4.175)) sol_out2 = 3.75; % Allowable speeds at this particular position elseif ((vc(2) > 4.375) && (vc(2) < 7.419)) sol_out2 = 2.5; elseif (vc(2) < 1) sol_out2 = 5.5; elseif (vc(2) > 7.419) sol_out2 = 3; else sol_out2 = 0; end % Joint 3 % if (abs(vc(3)4.639) < 0.1) sol_out3 = 0; elseif ((vc(3) > 2) && (vc(3) < 4.539)) sol_out3 = 1.5; elseif ((vc(3) > 4.739) && (vc(3) <7.559)) sol_out3 = 1.5; elseif (vc(3) > 7.559) sol_out3 = 5; elseif (vc(3) < 2) sol_out3 = 5; else sol_out3 = 0; end % Joint 4 % if (abs(vc(4)4.49) < 0.1) sol_out4 = 0; elseif ((vc(4) > 1.561) && (vc(4) < 4.39)) sol_out4 = 5; elseif ((vc(4) > 4.59) && (vc(4) < 7.446)) sol_out4 = 3; elseif (vc(4) > 7.446) sol_out4 = 3; elseif (vc(4) < 1.561) sol_out4 = 4; else sol_out4 = 0; end elseif( (on_off == 0.2)) % Going to the first fruit picking position % % % % This avoids errors in inverse kinematics due to joint angles at 0 % % % % % Joint 1 % PAGE 165 165 if (abs(vc(1)3.97) < 0.1) sol_out1 = 0; elseif((vc(1) > 1.3) && (vc(1) < 3.5)) sol_out1 = 7.5; % Allowable speeds at this particular position elseif ((vc(1) > 3.5) && (vc(1) < 3.87)) sol_out1 = 7; % elseif ((vc(1) < 6.823) && (vc(1) > 4.5)) sol_out1 = 8; % elseif ((vc(1) < 4.5) && (vc(1) > 4.07)) sol_out1 = 5.5; % elseif (vc(1) < 1.3) sol_out1 = 2; % Bringing the joint within limits elseif (vc(1) > 6.823) sol_out1 = 5; else sol_out1 = 0; end % Joint 2 % if (abs(vc(2)5.5) < 0.1) sol_out2 = 0; elseif ((vc(2) > 1) && (vc(2) < 5.2)) sol_out2 = 5; % Allowable speeds at this particular position elseif ((vc(2) > 5.2) && (vc(2) < 5.40)) sol_out2 = 3.75; % Allowable speeds at this particular position elseif ((vc(2) > 5.65) && (vc(2) < 7.419)) sol_out2 = 2.5; elseif (vc(2) < 1) sol_out2 = 5.5; elseif (vc(2) > 7.419) sol_out2 = 3; else sol_out2 = 0; end % Joint 3 % if (abs(vc(3)4.639) < 0.1) sol_out3 = 0; elseif ((vc(3) > 2) && (vc(3) < 4.539)) sol_out3 = 1.5; elseif ((vc(3) > 4.739) && (vc(3) <7.559)) sol_out3 = 1.5; elseif (vc(3) > 7.559) sol_out3 = 5; elseif (vc(3) < 2) sol_out3 = 5; else sol_out3 = 0; end % Joint 4 % if (abs(vc(4)5.5) < 0.1) sol_out4 = 0; elseif ((vc(4) > 1.561) && (vc(4) < 5.40)) sol_out4 = 3; elseif ((vc(4) > 5.6) && (vc(4) < 7.446)) PAGE 166 166 sol_out4 = 3; elseif (vc(4) > 7.446) sol_out4 = 3; elseif (vc(4) < 1.561) sol_out4 = 4; else sol_out4 = 0; end else sol_out1 = 0; sol_out2 = 0; sol_out3 = 0; sol_out4=0; end % Solenoid output block % function [sol_out1,sol_out2,sol_out3,sol_out4] = solenoid_out(vc,on_off,vd,coords) % Solenoid driver voltage output block %% Defining errors ve1 = vd(1) vc(1); ve2 = vd(2) vc(2); ve3 = vd(3) vc(3); ve4 = vd(4) vc(4); %% Initialize sol_out1 = 0; sol_out2 = 0; sol_out3 = 0; sol_out4=0; %#ok PAGE 167 167 sol_out2 = 5; % Allowable speeds at this particular position elseif ((sign(ve2)== 1) && (vc(2) > 7) && (vc(2) < 7.419)) sol_out2 = 5; elseif ((sign(ve2)== 1) && (vc(2) > 3) && (vc(2) < 7)) sol_out2 = 2.5; elseif ((sign(ve2)== 1) && (vc(2) > 1) && (vc(2) < 3)) sol_out2 = 6; elseif (vc(2) > 7.419) sol_out2 = 2; elseif (vc(2) < 1) sol_out2 = 4.5; else sol_out2 = 0; end % Joint 3 % if ((ve3>=0.1) && (ve3<=0.1)) sol_out3 = 0; elseif ((sign(ve3)== 1) && (vc(3) > 2) && (vc(3) < 6)) sol_out3 = 2; elseif ((sign(ve3)== 1) && (vc(3) > 6) && (vc(3) < 7.559)) sol_out3 = 2; elseif ((sign(ve3)== 1) && (vc(3) > 3) && (vc(3) < 7.559)) sol_out3 = 2; elseif ((sign(ve3)== 1) && (vc(3) > 2) && (vc(3) < 3)) sol_out3 = 2; elseif (vc(3) > 7.559) sol_out3 = 2.5; elseif (vc(3) < 2) sol_out3 = 3; else sol_out3 = 0; end % Joint 4 % if ((ve4>=0.1) && (ve4<=0.1)) sol_out4 = 0; elseif ((sign(ve4)== 1) &&(vc(4) > 1.561) && (vc(4) < 5)) sol_out4 = 3; elseif ((sign(ve4)== 1) &&(vc(4) > 5) && (vc(4) < 7.446)) sol_out4 = 3; elseif ((sign(ve4)== 1) && (vc(4) > 1.561) && (vc(4) < 7.446)) sol_out4 = 4; elseif (vc(4) > 7.446) sol_out4 = 5; elseif (vc(4) < 1.561) sol_out4 = 3; else sol_out4 = 0; end else sol_out1 = 0; sol_out2 = 0; sol_out3 = 0;sol_out4 = 0; end PAGE 168 168 PAGE 169 1 69 PAGE 170 170 APPENDIX G SIMULINK PROGRAM FOR ADAPTIVE INTEGRATOR BACKSTEPPING OF THE HYDRAULIC ACTUATOR S YSTEM PAGE 171 171 function [e, theta_hat_dot, eta_dot, P_L_dot,Q1]= Adaptive_controller(t, alf, K, q, q_dot,q_ddot,gamma, theta_hat,eta) nact = 0.95; D = 0.81;C1 = nact*D; r_act = 7;q0 = 2.3562;l=7; Be = 2.7158e8;%Pa% V1 = (r_act^2/2)*(q0+q)*l; V2 = (r_act^2/2)*(q0q)*l; % Desired trajectory: qd = sin(t); qd_dot = cos(t); qd_ddot = sin(t); %% Error definition for the Tracking controller e = q qd; e_dot = q_dot qd_dot; e_ddot = q_ddot qd_ddot; % Filter Tracking Error r = e_dot + alf*e; r_dot = e_ddot + alf*e_dot; %% Linear parametrization % Defining the terms in Y and theta A = q_dot; B = qd_ddot + alf*e_dot; C = q; D = 1; Y = [A B C]; Yt = Y'; %% Control Torque based on estimate plant model K1 = K(1); K2 = K(2); Ft1 = K1*r; theta_hat_dot = gamma*Yt*r; U1 = Be*C1/V1; eta_dot = K2*eta r; Q1 = 1/U1*(U1*D*q_dot K1*r_dot Y*gamma*Yt*r Y*theta_hat K2*eta r); Q2 = D*q_dot; P_L_dot = (Be/V1 (Q1 D*q_dot)) (Be/V1 (Q2 + D*q_dot)) ; PAGE 172 172 %% Adaptive controller with disturbances function [Torque,q_ddot] = Actuator_dynamics(P_L, Td, q, q_dot) %% This block represents the actuator dynamics. % For an EMK controller, all the parameters are known %% Parameter description: J = 12; %kg% b = 65; %Ns/m% k = 25; %N/m% nact = 0.95; D = 0.81; Torque = nact*P_L*D; %% Output for feedback "q_ddot" q_ddot = 1/J*(Torque b*q_dot k*q Td); % From the given equation of dynamics PAGE 173 173 APPENDIX H SIMULINK PROGRAM FOR EXACT MO DEL KNOWLEDGE CONTRO LLER OF THE HYDRAULIC ROBOT PAGE 174 174 function [Torque, e, qd]= EMK_controller(t, alf, K, q, q_dot) qd = [0.9+0.025*sin(pi/4*t); 0.025*sin(pi/4*t); 0.6+0.025*sin(pi/4*t); 0.9+0.025*sin(pi/4*t)]; qd_dot = [0.025*cos(pi/4*t); 0.025*cos(pi/4*t); 0.025*cos(pi/4*t); 0.025*cos(pi/4*t)]; qd_ddot = [0.025*sin(pi/4*t); 0.025*sin(pi/4*t); 0.025*sin(pi/4*t); 0.025*sin(pi/4*t)]; %% Error definition for the Tracking controller e = q qd; e_dot = q_dot qd_dot; % Filter Tracking Error r = e_dot + alf*e; %% Joint position and velocity signals: q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4);% Joint angles q1_dot = q_dot(1); q2_dot = q_dot(2); q3_dot = q_dot(3); q4_dot = q_dot(4);% Joint angular velocities %% Model description: Obtained from the EL program described in Appendix E M = [(655.2322*sin(q2)^2)+(8.6271*sin(q2)^2*(cos(q3)^2)) + (3.995*(sin(q2)^2)*(sin(q3)^2)) + (2.5*((17.78*cos(q1)*sin(q2)) 3.25*sin(q4)*(sin(q1)*sin(q3) cos(q1)*cos(q2)*cos(q3)) + (3.25*cos(q1)*cos(q4)*sin(q2)))^2) + (2.5*((17.78*sin(q1)*sin(q2))+ (3.25*sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1))) + (3.25*cos(q4)*sin(q1)*sin(q2)))^2)+ 3.995*(cos(q2)*sin(q4) + cos(q3)*cos(q4)*sin(q2))^2 + (3.75*(cos(q2)*cos(q4) cos(q3)*sin(q2)*sin(q4))^2) + (8.6271*sin(q2)^2*sin(q3)^2) + 48.44, sin(q3)*((0.245*cos(q3)*sin(q2)*cos(q4)^2) + (0.245*cos(q2)*sin(q4)*cos(q4))(0.245*cos(q3)*sin(q2))) (16.25*sin(q3)*((13*cos(q3)*sin(q2)*cos(q4)^2) + (3.25*cos(q2)*sin(q4)*cos(q4)) (3.25*cos(q3)*sin(q2)) + (17.78*cos(q2)*sin(q4))))/8, (26.6513*cos(q2)*cos(q4)^2) + (26.6513*cos(q3)*sin(q2)*sin(q4)*cos(q4)) + (35.51*cos(q2)) + (144.46*cos(q3)*sin(q2)*sin(q4)), sin(q2)*sin(q3)*((144.46*cos(q4)) + 30.40); sin(q3)*((0.245*cos(q3)*sin(q2)*cos(q4)^2)+(0.245*cos(q2)*sin(q4)*cos(q4))(0.245*cos(q3)*sin(q2))) (65*sin(q3)*((3.25*cos(q3)*sin(q2)*cos(q4)^2)+ (3.25*cos(q2)*sin(q4)*cos(q4))(3.25*cos(q3)*sin(q2))+(17.78*cos(q2)*sin(q4))))/8, (577.85*sin(q4/2)^2) (26.65*sin(q3)^2*sin(q4)^2)+1.7888e+003, (0.245*cos(q4)*sin(q3)*sin(q4))(65*sin(q3)*sin(q4)*((3.25*cos(q4)) + 17.78))/8, (3.995*cos(q3))+(65*cos(q3)*((17.78*cos(q4))+3.25))/8; (26.6513*cos(q2)*cos(q4)^2)+(26.6513*cos(q3)*sin(q2)*sin(q4)*cos(q4))+(35.51* cos(q2)) + (577.85*cos(q3)*sin(q2)*sin(q4)), PAGE 175 175 (0.245*cos(q4)*sin(q3)*sin(q4)) (65*sin(q3)*sin(q4)*((3.25*cos(q4)) + 17.78))/8, (26.65*sin(q4)^2)+ 8.8542, 0; sin(q2)*sin(q3)*((577.85*cos(q4))+30.4013),(3.995*cos(q3))+(65*cos(q3)*((17.7 8*cos(q4))+ 3.25))/8, 0,30.4013]; g_n = [(1052613*sin(q1)*sin(q2))/1000 (12753*sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)))/160 (12753*cos(q4)*sin(q1)*sin(q2))/160; (1052613*cos(q1)*cos(q2))/1000 + (12753*cos(q1)*cos(q2)*cos(q4))/160 (12753*cos(q1)*cos(q3)*sin(q2)*sin(q4))/160; (12753*sin(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3)))/160; (12753*cos(q4)*(sin(q1)*sin(q3) cos(q1)*cos(q2)*cos(q3)))/160 (12753*cos(q1)*sin(q2)*sin(q4))/160]; Vm = [q4_dot*((137934466712251*(cos(q2)*sin(q4) + cos(q3)*cos(q4)*sin(q2))*(cos(q2)*cos(q4) cos(q3)*sin(q2)*sin(q4)))/562949953421312 (65*(cos(q4)*(sin(q1)*sin(q3) cos(q1)*cos(q2)*cos(q3)) + cos(q1)*sin(q2)*sin(q4))*((889*cos(q1)*sin(q2))/50 (13*sin(q4)*(sin(q1)*sin(q3) cos(q1)*cos(q2)*cos(q3)))/4 + (13*cos(q1)*cos(q4)*sin(q2))/4))/8 + (65*(cos(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)) sin(q1)*sin(q2)*sin(q4))*((889*sin(q1)*sin(q2))/50 + (13*sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)))/4 + (13*cos(q4)*sin(q1)*sin(q2))/4))/8) + q2_dot*((5*((889*sin(q1)*sin(q2))/50 + (13*sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)))/4 + (13*cos(q4)*sin(q1)*sin(q2))/4)*((889*cos(q2)*sin(q1))/50 + (13*cos(q2)*cos(q4)*sin(q1))/4 (13*cos(q3)*sin(q1)*sin(q2)*sin(q4))/4))/2 + (15725573*cos(q2)*sin(q2))/24000 (2248996792042171*(cos(q2)*sin(q4) + cos(q3)*cos(q4)*sin(q2))*(sin(q2)*sin(q4) cos(q2)*cos(q3)*cos(q4)))/562949953421312 (15*(cos(q4)*sin(q2) + cos(q2)*cos(q3)*sin(q4))*(cos(q2)*cos(q4) cos(q3)*sin(q2)*sin(q4)))/4 + (5*((889*cos(q1)*sin(q2))/50 (13*sin(q4)*(sin(q1)*sin(q3) cos(q1)*cos(q2)*cos(q3)))/4 + (13*cos(q1)*cos(q4)*sin(q2))/4)*((889*cos(q1)*cos(q2))/50 + (13*cos(q1)*cos(q2)*cos(q4))/4 (13*cos(q1)*cos(q3)*sin(q2)*sin(q4))/4))/2 + (4141*cos(q2)*sin(q2)*sin(q3)^2)/480 (2248996792042171*cos(q2)*sin(q2)*(cos(q3)^2 1))/562949953421312 (4141*cos(q2)*sin(q2)*(sin(q3)^2 1))/480) q3_dot*((2248996792042171*cos(q3)*sin(q3)*(cos(q2)^2 1))/562949953421312 (65*sin(q4)*(cos(q1)*cos(q3) cos(q2)*sin(q1)*sin(q3))*((889*sin(q1)*sin(q2))/50 + (13*sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)))/4 + (13*cos(q4)*sin(q1)*sin(q2))/4))/8 + (65*sin(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3))*((889*cos(q1)*sin(q2))/50 (13*sin(q4)*(sin(q1)*sin(q3) cos(q1)*cos(q2)*cos(q3)))/4 + (13*cos(q1)*cos(q4)*sin(q2))/4))/8 + (2248996792042171*cos(q4)*sin(q2)*sin(q3)*(cos(q2)*sin(q4) + cos(q3)*cos(q4)*sin(q2)))/562949953421312 (15*sin(q2)*sin(q3)*sin(q4)*(cos(q2)*cos(q4) cos(q3)*sin(q2)*sin(q4)))/4), (34900327*q1_dot*cos(q2)*sin(q2))/24000 (52486674091608113*q3_dot*sin(q2))/1688849860263936 (11557*q1_dot*cos(q3)*sin(q4))/80 + (16058862836908731*q4_dot*cos(q2)*sin(q3))/562949953421312 + (15003331674243771*q3_dot*cos(q3)^2*sin(q2))/562949953421312 + (15003331674243771*q3_dot*cos(q4)^2*sin(q2))/562949953421312 (15003331674243771*q3_dot*cos(q3)^2*cos(q4)^2*sin(q2))/562949953421312 + PAGE 176 176 (11557*q1_dot*cos(q2)*cos(q4)*sin(q2))/40 + (15003331674243771*q2_dot*cos(q2)*cos(q3)*sin(q3))/562949953421312 (15003331674243771*q1_dot*cos(q3)*cos(q4)*sin(q4))/562949953421312 + (11557*q2_dot*sin(q2)*sin(q3)*sin(q4))/80 (15003331674243771*q1_dot*cos(q2)*cos(q3)^2*sin(q2))/562949953421312 + (15003331674243771*q1_dot*cos(q2)*cos(q4)^2*sin(q2))/562949953421312 + (11557*q1_dot*cos(q2)^2*cos(q3)*sin(q4))/40 (15003331674243771*q4_dot*cos(q2)*cos(q4)^2*sin(q3))/562949953421312 + (15003331674243771*q1_dot*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q2))/56294995342131 2 + (15003331674243771*q2_dot*cos(q4)*sin(q2)*sin(q3)*sin(q4))/562949953421312 + (15003331674243771*q1_dot*cos(q2)^2*cos(q3)*cos(q4)*sin(q4))/281474976710656 (15003331674243771*q2_dot*cos(q2)*cos(q3)*cos(q4)^2*sin(q3))/562949953421312 + (15003331674243771*q4_dot*cos(q3)*cos(q4)*sin(q2)*sin(q3)*sin(q4))/5629499534 21312, (15003331674243771*q1_dot*cos(q3)*sin(q3))/562949953421312 (52486674091608113*q2_dot*sin(q2))/1688849860263936 + (15*q4_dot*cos(q3)*sin(q2))/8 + (15003331674243771*q2_dot*cos(q3)^2*sin(q2))/562949953421312 + (15003331674243771*q2_dot*cos(q4)^2*sin(q2))/562949953421312 (15003331674243771*q2_dot*cos(q3)^2*cos(q4)^2*sin(q2))/562949953421312 + (11557*q4_dot*cos(q3)*cos(q4)*sin(q2))/80 + (15003331674243771*q4_dot*cos(q2)*cos(q4)*sin(q4))/562949953421312 (11557*q3_dot*sin(q2)*sin(q3)*sin(q4))/80 (15003331674243771*q1_dot*cos(q2)^2*cos(q3)*sin(q3))/562949953421312 (15003331674243771*q1_dot*cos(q3)*cos(q4)^2*sin(q3))/562949953421312 + (15003331674243771*q4_dot*cos(q3)*cos(q4)^2*sin(q2))/562949953421312 + (15003331674243771*q1_dot*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q3))/56294995342131 2 (11557*q1_dot*cos(q2)*sin(q2)*sin(q3)*sin(q4))/80 (15003331674243771*q3_dot*cos(q4)*sin(q2)*sin(q3)*sin(q4))/562949953421312 (15003331674243771*q1_dot*cos(q2)*cos(q4)*sin(q2)*sin(q3)*sin(q4))/5629499534 21312, (16058862836908731*q2_dot*cos(q2)*sin(q3))/562949953421312 (11557*q1_dot*sin(q4))/80 + (15*q3_dot*cos(q3)*sin(q2))/8 + (11557*q1_dot*cos(q2)^2*sin(q4))/80 (15003331674243771*q1_dot*cos(q2)*cos(q3)*sin(q2))/562949953421312 + (11557*q3_dot*cos(q3)*cos(q4)*sin(q2))/80 + (15003331674243771*q3_dot*cos(q2)*cos(q4)*sin(q4))/562949953421312 (11557*q4_dot*sin(q2)*sin(q3)*sin(q4))/80 + (15003331674243771*q1_dot*cos(q2)^2*cos(q4)*sin(q4))/562949953421312 (15003331674243771*q2_dot*cos(q2)*cos(q4)^2*sin(q3))/562949953421312 (15003331674243771*q1_dot*cos(q3)^2*cos(q4)*sin(q4))/562949953421312 + (15003331674243771*q3_dot*cos(q3)*cos(q4)^2*sin(q2))/562949953421312 + (15003331674243771*q1_dot*cos(q2)^2*cos(q3)^2*cos(q4)*sin(q4))/56294995342131 2 + (11557*q1_dot*cos(q2)*cos(q3)*cos(q4)*sin(q2))/80 + (15003331674243771*q1_dot*cos(q2)*cos(q3)*cos(q4)^2*sin(q2))/281474976710656 + (15003331674243771*q2_dot*cos(q3)*cos(q4)*sin(q2)*sin(q3)*sin(q4))/5629499534 21312; (425*q3_dot*sin(q2))/96 (34900327*q1_dot*cos(q2)*sin(q2))/24000 + (11557*q1_dot*cos(q3)*sin(q4))/80 (15*q4_dot*cos(q2)*sin(q3))/8 + (15003331674243771*q3_dot*cos(q3)^2*sin(q2))/562949953421312 (15003331674243771*q3_dot*cos(q3)^2*cos(q4)^2*sin(q2))/562949953421312 (11557*q1_dot*cos(q2)*cos(q4)*sin(q2))/40 + (15003331674243771*q1_dot*cos(q3)*cos(q4)*sin(q4))/562949953421312 (11557*q3_dot*cos(q2)*cos(q3)*sin(q4))/80 (11557*q4_dot*cos(q2)*cos(q4)*sin(q3))/80 + PAGE 177 177 (15003331674243771*q1_dot*cos(q2)*cos(q3)^2*sin(q2))/562949953421312 (15003331674243771*q1_dot*cos(q2)*cos(q4)^2*sin(q2))/562949953421312 (11557*q1_dot*cos(q2)^2*cos(q3)*sin(q4))/40 (15003331674243771*q4_dot*cos(q2)*cos(q4)^2*sin(q3))/562949953421312 (15003331674243771*q1_dot*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q2))/56294995342131 2 (15003331674243771*q3_dot*cos(q2)*cos(q3)*cos(q4)*sin(q4))/562949953421312 (15003331674243771*q1_dot*cos(q2)^2*cos(q3)*cos(q4)*sin(q4))/281474976710656 + (15003331674243771*q4_dot*cos(q3)*cos(q4)*sin(q2)*sin(q3)*sin(q4))/5629499534 21312, q4_dot*((15003331674243771*sin(2*q4)*sin(q3)^2)/1125899906842624 + (11557*sin(q4))/80) (15003331674243771*q3_dot*sin(2*q3)*sin(q4)^2)/1125899906842624, (425*q1_dot*sin(q2))/96 (15*q4_dot*sin(q3))/8 (15003331674243771*q2_dot*cos(q3)*sin(q3))/562949953421312 (11557*q3_dot*cos(q3)*sin(q4))/80 (11557*q4_dot*cos(q4)*sin(q3))/80 + (15003331674243771*q1_dot*cos(q3)^2*sin(q2))/562949953421312 (15003331674243771*q4_dot*cos(q4)^2*sin(q3))/562949953421312 (15003331674243771*q1_dot*cos(q3)^2*cos(q4)^2*sin(q2))/562949953421312 (11557*q1_dot*cos(q2)*cos(q3)*sin(q4))/80 (15003331674243771*q3_dot*cos(q3)*cos(q4)*sin(q4))/562949953421312 + (15003331674243771*q2_dot*cos(q3)*cos(q4)^2*sin(q3))/562949953421312 (15003331674243771*q1_dot*cos(q2)*cos(q3)*cos(q4)*sin(q4))/562949953421312, (15003331674243771*q2_dot*cos(q3)^2*cos(q4)*sin(q4))/562949953421312 (11557*q2_dot*cos(q4/2)*sin(q4/2))/40 (15*q1_dot*cos(q2)*sin(q3))/8 (15003331674243771*q2_dot*cos(q4)*sin(q4))/562949953421312 (11557*q3_dot*cos(q4)*sin(q3))/80 (11557*q4_dot*cos(q3)*sin(q4))/80 (15003331674243771*q3_dot*cos(q4)^2*sin(q3))/562949953421312 (11557*q1_dot*cos(q2)*cos(q4)*sin(q3))/80 (15003331674243771*q1_dot*cos(q2)*cos(q4)^2*sin(q3))/562949953421312 (15*q3_dot*sin(q3))/8 + (15003331674243771*q1_dot*cos(q3)*cos(q4)*sin(q2)*sin(q3)*sin(q4))/5629499534 21312; (15003331674243771*q2_dot*cos(q3)^2*cos(q4)^2*sin(q2))/562949953421312 (15003331674243771*q1_dot*cos(q3)*sin(q3))/562949953421312 (16058862836908731*q4_dot*cos(q3)*sin(q2))/562949953421312 (15003331674243771*q2_dot*cos(q3)^2*sin(q2))/562949953421312 (425*q2_dot*sin(q2))/96 + (11557*q2_dot*cos(q2)*cos(q3)*sin(q4))/80 + (15003331674243771*q4_dot*cos(q2)*cos(q4)*sin(q4))/562949953421312 + (15003331674243771*q1_dot*cos(q2)^2*cos(q3)*sin(q3))/562949953421312 + (15003331674243771*q1_dot*cos(q3)*cos(q4)^2*sin(q3))/562949953421312 + (15003331674243771*q4_dot*cos(q3)*cos(q4)^2*sin(q2))/562949953421312 (15003331674243771*q1_dot*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q3))/56294995342131 2 + (15003331674243771*q2_dot*cos(q2)*cos(q3)*cos(q4)*sin(q4))/562949953421312 + (11557*q1_dot*cos(q2)*sin(q2)*sin(q3)*sin(q4))/80 + (15003331674243771*q1_dot*cos(q2)*cos(q4)*sin(q2)*sin(q3)*sin(q4))/5629499534 21312, (16058862836908731*q4_dot*sin(q3))/562949953421312 (425*q1_dot*sin(q2))/96 + (15003331674243771*q2_dot*cos(q3)*sin(q3))/562949953421312 (15003331674243771*q1_dot*cos(q3)^2*sin(q2))/562949953421312 (15003331674243771*q4_dot*cos(q4)^2*sin(q3))/562949953421312 + (15003331674243771*q1_dot*cos(q3)^2*cos(q4)^2*sin(q2))/562949953421312 + (11557*q1_dot*cos(q2)*cos(q3)*sin(q4))/80 (15003331674243771*q2_dot*cos(q3)*cos(q4)^2*sin(q3))/562949953421312 + (15003331674243771*q1_dot*cos(q2)*cos(q3)*cos(q4)*sin(q4))/562949953421312, PAGE 178 178 (15003331674243771*q4_dot*sin(2*q4))/1125899906842624, (17114393999573691*q2_dot*sin(q3))/1125899906842624 (17114393999573691*q1_dot*cos(q3)*sin(q2))/1125899906842624 + (15003331674243771*q3_dot*cos(q4)*sin(q4))/562949953421312 (15003331674243771*q2_dot*cos(2*q4)*sin(q3))/1125899906842624 + (15003331674243771*q1_dot*cos(q2)*cos(q4)*sin(q4))/562949953421312 + (15003331674243771*q1_dot*cos(2*q4)*cos(q3)*sin(q2))/1125899906842624; (11557*q1_dot*sin(q4))/80 + (15*q2_dot*cos(q2)*sin(q3))/8 + (16058862836908731*q3_dot*cos(q3)*sin(q2))/562949953421312 (11557*q1_dot*cos(q2)^2*sin(q4))/80 + (15003331674243771*q1_dot*cos(q2)*cos(q3)*sin(q2))/562949953421312 + (11557*q2_dot*cos(q2)*cos(q4)*sin(q3))/80 (15003331674243771*q3_dot*cos(q2)*cos(q4)*sin(q4))/562949953421312 (15003331674243771*q1_dot*cos(q2)^2*cos(q4)*sin(q4))/562949953421312 + (15003331674243771*q2_dot*cos(q2)*cos(q4)^2*sin(q3))/562949953421312 + (15003331674243771*q1_dot*cos(q3)^2*cos(q4)*sin(q4))/562949953421312 (15003331674243771*q3_dot*cos(q3)*cos(q4)^2*sin(q2))/562949953421312 (15003331674243771*q1_dot*cos(q2)^2*cos(q3)^2*cos(q4)*sin(q4))/56294995342131 2 (11557*q1_dot*cos(q2)*cos(q3)*cos(q4)*sin(q2))/80 (15003331674243771*q1_dot*cos(q2)*cos(q3)*cos(q4)^2*sin(q2))/281474976710656 (15003331674243771*q2_dot*cos(q3)*cos(q4)*sin(q2)*sin(q3)*sin(q4))/5629499534 21312, q1_dot*((2248996792042171*cos(q2)*sin(q3))/1125899906842624 + (65*sin(q3)*((13*cos(q2)*cos(q4)^2)/4 (13*cos(q3)*sin(q2)*cos(q4)*sin(q4))/2 + (889*cos(q2)*cos(q4))/50 (13*cos(q2)*sin(q4)^2)/4))/16 (sin(q3)*((137934466712251*cos(q2)*cos(q4)^2)/562949953421312 + (137934466712251*cos(q3)*sin(q2)*cos(q4)*sin(q4))/281474976710656 + (137934466712251*cos(q2)*sin(q4)^2)/562949953421312))/2 + (65*cos(q2)*sin(q3)*((889*cos(q4))/50 + 13/4))/16) q3_dot*((2248996792042171*sin(q3))/1125899906842624 (137934466712251*cos(q4)^2*sin(q3))/1125899906842624 + (15003331674243771*sin(q3)*sin(q4)^2)/1125899906842624 + (65*sin(q3)*((889*cos(q4))/50 + 13/4))/16 (65*cos(q4)*sin(q3)*((13*cos(q4))/4 + 889/50))/16) + q2_dot*((15003331674243771*cos(q4)*sin(q4)*sin(q3)^2)/562949953421312 + (11557*cos(q4/2)*sin(q4/2))/40), (17114393999573691*q1_dot*cos(q3)*sin(q2))/1125899906842624 (17114393999573691*q2_dot*sin(q3))/1125899906842624 (15003331674243771*q3_dot*cos(q4)*sin(q4))/562949953421312 + (15003331674243771*q2_dot*cos(2*q4)*sin(q3))/1125899906842624 (15003331674243771*q1_dot*cos(q2)*cos(q4)*sin(q4))/562949953421312 (15003331674243771*q1_dot*cos(2*q4)*cos(q3)*sin(q2))/1125899906842624, 0]; %% Control Torque based on exact model knowledge Torque = Vm*qd_dot + g_n + M*qd_ddot Vm*alf*e alf*M*e_dot K*r; PAGE 179 179 LIST OF REFERENCES Alleyne, A. 1996. Nonlinear force control of an electrohydraulic actuator. Proc. of the Japan/USA symposium on Flexible Automation, vol. 1. pp. 193 200. New York: ASME Balestrino, A. De Maria, G. and Sciavicco, L. 1984. Robust control of Robotic manipulators in Proc .of the 9th IFAC World Congress. Vol. 5 pp. 24352440. Bobrow, J.E. and Lum,K. 1995. Adaptive, High Bandwidth Control of a Hydraulic Actuator. Proc. of the American Control Conference. Part 1. pp7175. Seattle, WA. Bu,F. and Yao,B. 2000. Nonlinear Adaptive Robust Control of Hydraulic Actuators Regulated by Proportional Directional Control Valves with Deadband and Nonlinear Flow Gain Coe fficients. Proc. of the American Control Conference. pp. 41294133. Chicago, IL Bu,F. and Yao,B. 2001. Nonlinear model based coordinated adaptive robust control of electrohydraulic arms via overparameterizing method. International Conference on Robotics a nd Automation. pp. 3459 3464 Burks, T. F., F. Villegas, M. Hannan, S. Flood, B. Sivaraman, V. Subramanian, and J. Sikes. 2005. Engineering and horticultural aspects of robotic fruit harvesting: Opportunities and constraints. HortTechnology 15(1): 7987. Cavalieri, S., and A. Plebe. 1996. Manipulator adaptive control by neural networks in an orange picking robot. Intl. J. Neural Systems 7(6): 735755. Ceres, R., J. L. Pons, A. R. Jimenez, J. M. Martin, and L. Calderon. 1998. Design and implementation of an aided fruit harvesting robot (Agribot). Ind. Robot 25(5): 337346. Chern, T. and Wu, Y. 1992. An optimal variable structure control with integral compensation for electrohydraulic servo control system. IEEE Trans. Ind. Electron. vol. 39. pp.460 463. Cor ke, P. I. 1996. A robotics toolbox for MATLAB. IEEE Robotics and Automation Magazine 3: 2432. Craig, J. J. 1989. Introduction to Robotics: Mechanics and Control. 2nd ed. Reading, MA.: Addison Wesley. Cunha, M. and Guenther, R. 2005. Adaptive cascade cont rol of a hydraulic actuator with an adaptive deadzone compensation. In Eighteenth congress of mechanical engineering. COBEM 2005. Ouro Preto. Brazil. de Queiroz, M., Dawson, D.M., Nagarkatti,S. and Zhang,F. 2000. Lyapunov Based Control of Mechanical Systems Cambridge, MA: Birkhauser. PAGE 180 180 Flood, S. 2006. Design of a Robotic Citrus Harvesting End Effector and Force Control Model Using Physical Properties and Harvesting Motion Tests. PhD Dissertation Florida Department Of Agriculture Aand Consumer Services. 2010. Florida Citrus Statistics. Tallahassee, Florida. Fortuna, L., G. Muscato, G. Nunnari, A. Pandolfo, and A. Plebe. 1996. Application of neural control in agriculture: An orange picking robot. Acta Horticulturae 406: 441450. Grand DEsnon, A. 1985. Robotic harvesting of apples. In Proc. Agri Mation, 1:210214. St. Joseph, MI.: ASAE, Dearborn, MI.: SME. Grand DEsnon, A., G. Rabatel, R. Pellenc, A. Journeau, and M. J. Aldon. 1987. MAGALI: A self propelled robot to pick apples. ASAE Paper No. 871037. St. Jose ph, MI. Hannan, M. W., and T. F. Burks. 2004. Current developments in automated citrus harvesting. ASAE/CSAE Meeting Paper No. 043087. St. Joseph, MI.: ASAE. Harrell, R. 1987. Economic analysis of robotic citrus harvesting in Florida. Trans. ASAE 30(2):298304. Harrell, R. C., P. D. Adsit, T. A. Pool, and R. Hoffman. 1988. The Florida robotic grove lab. ASAE Paper No. 881578. St. Joseph, MI. Juste, F., and F. Sevila. 1991. Citrus: An European project to study the robotic harvesting of oranges. In Proc. 2nd Workshop Robotics in Agriculture and the Food Industry, 187195. Arlington, VA: IARP. Juste. F., I. Fornes, F. Pla, and F. Sevila. 1992. An approach to robotic harvesting of citrus in Spain. In Proc. 7th Intl. Citrus Congress, 3: 10141018. Riverside, CA. : International Society of Citriculture. H.K. Khalil. 1996. Nonlinear systems(3rd ed.). PrenticeHall, Upper Saddle River, NJ. Kondo, N. 1995. Harvesting robot based on physical properties of grapevine. Japan Agricultural Research Quarterly (JARQ) 29(3): 171177. Laval, L., M'Siridi, N. K., and Cadiou, J. C. 1996. H Force Control of Hydraulic ServoActuator With Environmental Uncertainties. Proceedings of IEEE International Conference on Robotics and Automation. pp. 1566 1571. Minneapolis, MA. Lu, H. and Lin, W. 1993. Robust Controller With Disturbance Rejection for Hydraulic Servo Systems. IEEE Transaction on Industrial Electronics. 40. pp.157 162. PAGE 181 181 Manring, N. 2005. Hydraulic Control Systems. Wiley. New York. Merritt, H. E. 1967. Hydraulic Control System s. New York, NY: John Wiley & Sons. Monta, M., N. Kondo, and Y. Shibano. 1995. Agricultural robot in grape production system. In Proc. 1995 IEEE Intl. Conf. Robotics and Automation, 25042509. Piscataway, NJ: IEEE. Muscato, G., M. Prestifilippo, N. Abbate, and I. Rizzuto. 2005. A prototype of an orange picking robot: past history, the new robot, and experimental results. Ind. Robot 32(2): 128138. Opal RT Technologies Inc. 2005. RT LAB Version 8.1, User guide. Montreal, Quebec. Parrish, E. A., Jr., and A. K. Goksel. 1977. Pictorial pattern recognition applied to fruit harvesting. Trans. ASAE 20(5): 822827. Pellenc, R., Montoya, J. L., Grand DEsnon, A., and Rombaut, M. 1990. Automated machine for detection and grasping of objects. U.S. Patent No. 4975016. Rabatel, G., A. Bourely, F. Sevila, and F. Juste. 1995. Robotic Harvesting of Citrus: State of art and development of the French Spanish Eureka project. In Proc. Intl. Conf. Harvest and Post harvest Technologies for Fresh Fruits and Vegetables, 95: 232239 St. Joseph, MI.: ASAE. Roka, F., and S. Longworth. 2001. Labor requirements in Florida citrus. Extension Digital Information Source (EDIS) FE 304. Gainesville, FL.: Department of Food and Resource Economics, University of Florida. Sakai, S., M. Iida, and M. Umeda. 2002. Heavy material handling manipulator for agricultural robot. In Proc. 2002 IEEE Intl. Conf. Robotics and Automation, 10621068. Piscataway, NJ: IEEE. Sarig, Y. 1993. Robotics of Fruit Harvesting: A Stateof the art Review. J. Agric. Eng. Re search 54: 265280. Schertz, C. E., and G. K. Brown. 1968. Basic considerations in mechanizing citrus harvest. Trans. ASAE 11(2): 343348. Sciavicco, L. and Siciliano, B. 1996. Modeling and Control of Robot Manipulators. McGraw Hill Inc. Sirouspour, M. R. and Salcudean, S. E. 2001. Nonlinear Control of Hydraulic Robots. IEEE Trans. Rob. Auto. 17(2).pp 173 182. Sivaraman, B. 2006. Design and Development of a Robot Manipulator for Citrus Harvesting. PhD Dissertation PAGE 182 182 Slotine, J. and Li W 199 1. Applied nonli near control. Englewood Cliffs NJ: Prentice Hall. Sohl, G. and Bobrow,J.E. 1999. Experiments and Simulations on the Nonlinear Control of A Hydraulic Servosystem. IEEE Transactions on Control Systems Technology. Vol. 7,No. 2.pp. 238 247. Spong,M.W., Hutchi nson, S. and Vidyasagar,M. 2005. Robot modeling and control. Wiley. New York. Van Henten, E. J., J. Hemming, B. A. J. Van Tuijl, J. G. Kornet, J. Meuleman, J. Bontsema, and E. A. Van Os. 2002. An autonomous robot for harvesting cucumbers in greenhouses. Autonomous Robots 13: 241258. Vossoughi R. and Donath M 1995. Dynamic feedback linearization for electrohydraulically actuated control systems. ASME J. Dynamical Systems,Measurement and Control. vol. 117. no. 4. pp.468 477. Wang, L and Chen, C. 1991. A combined optimization method for solving the inverse kinematics problem of mechanical manipulators IEEE Transactions on R obotics and Automation. 7. pp. 489499. Whitney, D.E. 1969 Resolved motion rate control of manipulators and human prostheses IEEE Tran sactions on ManMachine Systems. 10 pp. 47 53. Wolovich W. A. and Elliot, H. 1984 A computational technique for inverse kinematics in Proc. 23rd IEEE Conference on Decision and Control pp. 13591363. Zheng, D., Havlicsek, H. and Alleyne, A. 1998. Nonlinear Adaptive Learning for Electrohydraulic Control Systems. Proc. of the ASME Dynamic Systems and Control Division. Vol.5. pp.8390. Anahein,CA. PAGE 183 183 BIOGRAPHICAL SKETCH Anirudh Sundararajan was born in 1984 in the state of Tamil N adu, India. He graduated with a b achelor s degree in Mechanical Engineering in 2006 from Anna University, Chennai, TN, India. He started his graduate studies at University of Florida as a m asters degree student in Mechanical and Aerospace Engineering in August 2006 and started working on concurrent m aster s degrees in the Mechanical and Aerospace Engineering and Agricultural and Biological Engineering Departments in August 2007. He joined the Agricultural Robotics and Mechatronics group (ARMg) in the Department of Agric ultural and Biological Engineering where he worked as a Research Assistant with Dr. Thomas F. Burks. He graduated with the concurrent masters degrees in December 2011. 