UFDC Home  Search all Groups  UF Institutional Repository  UF Institutional Repository  UF Theses & Dissertations   Help 
Material Information
Thesis/Dissertation Information
Subjects
Notes
Record Information

Full Text 
PAGE 1 KINEMATICS AND DYNAMICS OF FRUIT PICKING ROBOTIC MANIPULATOR By JIAYING ZHANG A THESIS PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF S CIENCE UNIVERSITY OF FLORIDA 2014 PAGE 2 2014 Jiaying Zhang PAGE 3 To my Mother, for encouraging me and trusting my ability PAGE 4 4 ACKNOWLEDGMENTS I sincerely thank my advisor, Dr. John Schueller, for providing me great support during the process of writing the thesis Dr Schueller kindly accepted me as one of his student s and tutored me with much patience throughout the two semesters He is alw ays willing to listen to my opinions and lead s me to a right direction. I learnt a lot of academic knowledge from him and am glad to listen to his interesting stories. He is always not hesitating to give me the strongest support that he can provide. This thesis cannot be finished successfully without his kind support and careful check. I thank University of Florida for providing me a good opportunity to practice the knowledge I have learnt here. University of Florida is the first college where I study in the English environment Through the process of writing this thesis, my academic knowledge is strengthen ed and my English writing skills is well prac ticed The speed to read and understand relative literature becomes faster than before. Through the thesis, I am able to work close with my advisor and thus to know the high standard for publishing in a worldclass university. The study at Mechanical Engi neering Department is joyful and worthwhile throughout my life. I thank the committee member, Dr. Carl Crane III I am thankful to Dr. Crane for his course Robot Geometry which is quite useful as the background knowledge for doing the research. All the com mittee members are very kind to give me advice and support. I thank the peers who also write the master s theses, such as T ianyu Gu, Zhe Wang, and Jing Zhou. The competition among us is one of the best impetuses for me to finish the thesis on time. I thank them for accompanying me from the very beginning to the end of the defense. T he information they gave me, the process they shared with me and the methods they taught me are all very helpful to me. PAGE 5 5 Finally, I thank my parent s for their eternal supp ort and love. They support me both economically and mentally. They are patient to listen to my compl aint when I am in trouble, although they don t know what exactly I am doing. Their encourage ment provide s me much confidence, which leads me finish ing this thesis at the end. PAGE 6 6 TABLE OF CONTENTS page ACKNOWLEDGMENTS ...............................................................................................................4 LIST OF TABLES ...........................................................................................................................8 LIST OF FIGURES .........................................................................................................................9 ABSTRACT ...................................................................................................................................11 C HAPTER 1 INTRODUCTI ON ..................................................................................................................12 Background .............................................................................................................................12 Problem Statement ..................................................................................................................13 Objectives ...............................................................................................................................13 2 LITERATURE REVIEW .......................................................................................................15 Fruit Harvesting Manipulators ................................................................................................15 Manipulator Kinematics .........................................................................................................17 Manipulator Dynamics ...........................................................................................................20 Backward Recursion ........................................................................................................22 Forward Recursion ..........................................................................................................25 3 KINEMATICS FOR ROBOTIC MANIPULATOR TO PICK FRUIT .................................31 Puma Robot Manipulator ........................................................................................................31 M atlab Robotics Toolbox .......................................................................................................31 Forward Kinematics for Puma 560 .........................................................................................32 Inverse Kinematics for Puma 560 ...........................................................................................34 4 DYNAMICS FOR ROBOTIC MANIPULATOR TO PICK FRUIT .....................................43 Dynamic Parameters of the Puma 560 ....................................................................................43 Inverse Dynamics for Puma 560 .............................................................................................45 5 FRUIT PICKING ARM DESIGNER GUI .............................................................................53 Assumption .............................................................................................................................53 Graphical User Interface Development ..................................................................................55 Fruit Tree Test Cases ..............................................................................................................57 Test C ase O ne: P each T ree ..............................................................................................57 Te st C ase T wo: C itrus T ree .............................................................................................58 Result and Conclusion ............................................................................................................59 PAGE 7 7 6 SUMMARY AND DISCUSSION .........................................................................................70 Summary .................................................................................................................................70 Discussion ...............................................................................................................................71 LIST OF REFERENCES ...............................................................................................................73 BIOGRAPHICAL SKETCH .........................................................................................................75 PAGE 8 8 LIST OF TABLES Table page 31 D H parameters of Puma 560 ............................................................................................42 41 Link m ass of Puma 560 ......................................................................................................51 42 Center of gravity of Puma 560 ...........................................................................................51 43 Moments of i nertia about COG ..........................................................................................52 51 Test results for typical fruit trees .......................................................................................69 52 Joint torques for hedge harvesting .....................................................................................69 PAGE 9 9 LIST OF FIGURES F igure page 21 MAGALI apple picking manipulator developed by Grand DEsnon et al ........................27 22 Citrus picking robot developed by Harrell ........................................................................27 23 The prototype of citrus harvesting robot manipulator .......................................................28 24 The joint types of the manipulator .....................................................................................28 25 Kubota m andarin orange harvesting robot .........................................................................29 26 Tomato harvesting robot designed by Okayama University ..............................................29 27 Tomato harvester manipulator ...........................................................................................30 28 Position vectors between the base origin and the link origins and the center of mass ......30 29 T he notation used for inverse dynamics ............................................................................30 31 PUMA 560 r obot m anipulator ...........................................................................................37 32 Links and j oints of Puma 560 ............................................................................................37 33 Coordinate f rames of Puma 560 ........................................................................................38 34 Puma 560 ready pose and zero pose ..................................................................................38 35 Puma 560 forward kinematics joint angles ........................................................................39 36 Puma 560 forward kinematics simulation ..........................................................................39 37 Puma 560 forward kinematics endeffector trajectory .......................................................40 38 Puma 560 forward kinematics endeffector X, Y, Z coordinates ......................................40 39 Puma 560 inverse kinematics joint angles .........................................................................41 310 Puma 560 inverse kinematics endeffector trajectory ........................................................41 41 Link a ngular velocity and a cceleration ..............................................................................48 42 Link m otor t orque for the r obot m odel without f riction ....................................................48 43 Joint m otor t orque for the r obot m odel with f riction .........................................................49 44 T he comparisons between total load torque and gravity load torque ................................49 PAGE 10 10 51 Fruit harvesting robot concept ...........................................................................................62 52 Articulated arm concept .....................................................................................................62 53 The suspension model for rotational inertia measurement ................................................62 54 Fruit picking arm designer GUI .........................................................................................63 55 Typical fruit position with respect to the tree coordinate frame ........................................63 56 Fully extended arm pose ....................................................................................................64 57 Flow chart of the Fruit Picking Arm Designer GUI ..........................................................64 58 A typical mature peach tree at University of Florida .........................................................65 59 GUI test results of the peach tree .......................................................................................65 510 A typical citrus tree at University of Florida .....................................................................66 511 GUI test results of the citrus tree .......................................................................................66 512 A typical fruit tree sketch ...................................................................................................67 513 Joint maximum torques for different arm cases .................................................................67 514 A hedge with fruits .............................................................................................................68 515 Joint torque variation for hedge harvesting .......................................................................68 PAGE 11 11 Abstra ct of Thesis Presented to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Master of Science KINEMATICS AND DYNAMICS OF FRUIT PICKING ROBOTIC MANIPULATOR By Jiaying Zhang May 2014 Chair: John K. Schueller Major: Mechanical Engineering The fruit picking robot is ideally suited to fresh fruit harvesting. It not only prevents the fruit tree from damage caused by mass harvesting machines but also saves human labor forces. Through several previous fruit harvester prototypes, some common rules on designing the manipulators are generalized to a representative robot model. Operating kinematics and dynamics studies on the representative robotic manipulator can help to simulate and analyze the fruit picking performance. This thesis studies the joint angles, velocities, and torques for Puma 560 arm to pick fruit individually. The development of fruit picking manipulator depends largely on the tree size and the fruit distribution An arm designer graphic user interface (GUI) is introduced to create arms that meet the developers different requirements on fruit trees. Kinematic and dynamic analyses are integrated into the G UI, which helps the user to understand the picking performa nce of the generated arm. PAGE 12 12 CHAPTER 1 INTRODUCTION Background Fruit growers in many countries a re facing potential problems that could affect their future business. Two significant problem s among them are the lack of adequate labor supply and the increase in labor cost In most countries, fruits are still being picked and packaged by hands, and graded by human inspection. In some advanced countries, scientists already have the awareness of combining automation technology into fruit production system in order to make up the labor shortage. One important stage that has large room to improve the effectiveness is the fruit picking process. Many researchers are continuously making efforts on the harvest mechanization. B a sically t here are three modes of harvest mechanization: Labor aids L abor saving machines, Robotics and Automation Firstly, Labor aids are aimed at reducing the drudgery of farm labor by reducing the effort and endurance required for the fruit picking operation. Ladders are one of the most typical labor aids. As long as the picking is done manually, the potential for increasing productivity is limited S econdly Labor saving machines are designed with the concept of mass harvesting. T hese machines have the advantage of reducing overall harvesting time and human labor costs. For example, Oxbo Corp. has citrus harvester with continuous canopy shake and catch system M echanical mass harvesting does not suit to fresh market due to the damage of fruits and tree during the e xcessive mechanical processing. Thirdly, Robotic fruit harvesting aims to automate the fruit picking process by using a syste m that emulate the human picker. Conceptually, it should provide a n overall faster rate than human labor and a bet ter protection for fruit and tree than mass harvesting. For this reason, many studies have been done on r obotic fruit harvesting over the past decades. PAGE 13 13 A typical robotic fruit harvestin g consists of fruit a detecting system to find the position of target f ruit, an endeffector to grip and detach t he fruit, and a manipulator that carries the end effector to reach the fruit. A f ruit detection system normally has cameras with multiple sensors which needs complex supporting algorithm to find the right fruit. Endeffectors are well designed to operate subtle behaviors such as stem cutting or fruittwisting. Manipulator arms are core components in computing the workspace and motion trajec tory. A well designed manipulator can have verified dexterity, accuracy and static performance. It is necessary to analy ze the manipulator before a robotic harvest er is built as a real product. The most general analysis methods are kinematics and dynamics, which are widely used in many robot ic manipulators. A nalysis tools like Matlab Robotics Toolbox (Corke, 1996) and Robotect (Nayar, 2002) are very convenient to do robot simul ation analysis. Problem Statement As many types of robotic fruit harvesting were designed to pick different fruit, it is easy to find similarities behind their design concept. For most tree fruit, the manipulator is a typical part among the harvesting robot s If a series of typical analysis could be done to simulate the fruit picking process, it will be helpful to design similar fruit harvesting in the future. There exist s a need to analysis the fruit picking process for a typical robotic arm, so the analysis can be applied to the design of robotic fruit harvesting with different picking requirement s Objective s The first objective of this research is to establish kinematic analysis for a robot manipulator performing fruit picking process. The Puma 560 robotic arm is selected as an example for doing the analysis because of its popularity, data approachability and similarity in structure to previous robotic fruit harvest arms. The Matlab Robotics Toolbox is introduced to do PAGE 14 14 both forward and inverse kinematics. T he goal is to simulate t he geometric trajectory of the endeffector for reaching a specified fruit position The second objective is to establish dynamic analysis for a robot manipulator performing fruit picking process. Similar to kinematic analysis, I use Puma 560 as an example and use Matlab Robotics Toolbox as the tool. In this part, the research will give the variety of joint velocities and accelerations during a picking trip. The torques of the arm actuators will also come out as the result. The fin al objective is to design a graphical user interface (GUI) which helps the users to generate fruit picking arms that meet their requirements. The GUI will allow the users inputting general fruit positions which are greatly determined by fruit types. Arm le ngth as well as the base position will be generated according to the inputted fruit distribution. The GUI will integrate inverse kinematics and dynamics, and apply them to the created fruit picking arm. PAGE 15 15 CHAPTER 2 LITERATURE REVIEW Fruit Harvest ing Manipulator s N umerous research es on robotic fruit harvesting have been studied over the past decades Recently, some researchers, such as Burks (2005), Sivaraman (2006), Hannan (2009), have also presented further studies on the base of those developed ones. Studies on harvesting manipulators of apples citruses, oranges and tomatoes are reviewed in this chapter due to their similar tree structure An a pple harvesting prototype was first developed in France by Grand DEsnon et al. (1985) Its mechanical system consisted of a telescopic arm that can move in a vertical framework. The arm was mounted on a barrel that could rotate horizontally. Later, DEsnon et al. (1987) built a new prototype called MAGALI. It used a spherical manipulator with a camera set at the center of the base rotation axes and a vacuum grasper for fruit picking. Figure 2 1 shows the spherical manipulator that executed a pantographic prismatic movement along with two rotations. A robotic system for citrus harvesting was development at University of Florida by Harrell (1988). A three degree of freedom manipulator that was actuated by serv ohydraulic drives was designed. Figure 2 2 shows the geometry character of the manipulator Joint 0 and 1 were revolute and joint 2 was prismatic. The whole picking motion followed a spherical coordinate system. Harrell used a rotating lip mechanism as the endeffector to serve the stem between a stationary and rotating cup. The joint position and velocity control was achieved through pot entiometer and tachometer feedback signals. U ltrasonic range sensors and a color charge coupled device (CCD) camera were used to detect the citrus depth information. PAGE 16 16 Later, new research on citrus fruit harvesting was launched at University of Florida by S ivaraman et al. (2006) A robot manipulator was designed to harvest both surface and inner canopy fruits, and a prototype was built for lab testing. Figure 23 shows the prototype of the manipulator and Figure 2 4 shows the joint types in detail The manipulator had seven degrees of freedom in all and the last four rotational joints were us ed to study its dexterity. K inematics and control system work were carried out but dynamics studies were not included in their research The endeffector of the prototype was studied by Flood (2006). The end effector was designed to grasp the citrus with three fingers and to detach it by twist forces. A nother m andarin orange harvesting robot was developed and trialed by Kubota& Co., Ltd in Ja pan (Sarig, 1993) This robot consist ed of a mobile carriage, a boom, and an arm fixed at the end of the boom Figure 25 shows the picking arm with the two main links (shown as Arm 1 and Arm2) of the same length. The Kubota robot had an articulated arm wi th four degree of freedom, but acted as a spherical coordinate robot This is because its elbow joint (shown as B in Figure 25 ) and the wrist joint (show as C in Figure 2 5) are interlinked at a speed ratio of 2:1, and then the endeffector can perform th e transitional movement as a prismatic joint. This design had the advantage of preventing interference with trees behind the robot. The Kubota robot used an endeffector with rotating stem cutters that contained a color TV camera and a light source. A tomato harvesting robot was studied by Kondo et al. (1996) at Okayama University. Tomatoes are normal cultivated in greenhouses with vertical supports. Figure 26 shows the robot prototype and Figure 27 shows the joint and link information. It had five rotational joints and two prismatic joints. The seven degree of freedom was designed to reach fruit efficiently and to evad e the foliage The joints were powered by electric motors. A two finger end effector equipped with a suction pad to pull the fruit in to the end effector was used for normal sized PAGE 17 17 tomatoes and a modified end effector with a nipper to cut the peduncle at the fruit peduncle joint was used for cherry tomato harvesting (Kondo et al., 1996). A photoelectric sensor and a color camera w ere used for visual sensing in the trials. M anipulator Kinematics A typical robot manipulator mechanism is composed of a serial chain of links and joints O ne end of the link chain is fixed to the base or the ground, while the other end carries an end effector to perform specific tasks. The links are assumed to be rigid bodies that are connect ed by neighboring joint axes and each joint has one degree of freedom (DOF), either rotational or translational. The most common joint type is revolute joint and prismatic joint. A method called t he Denavit Hartenberg (D H) notation (1955) is used to describe a manipulator mechanism by using four parameters. These four joint and link parameters are link length a, li nk twist link offset d, and joint angle T he link length and link twist define the relative location of the two axes in space. F or the first and last links these two parameters are arbitrarily selected to be 0 because they are meaningless. The link offset is the distance between neighboring links along the joint axis. The joint angle is the rotation of neighboring link s with respect to the joint axis. We can number the links of a manipulator from 0 to n, with n joints connecting between one and anothe r. Link 0 represents the base of the manipulato r and link n carries the end effector. Joint i connects link i to link i 1. The link i is attached to a coordinate frame i Denavit and Hartenberg proposed a matrix method of systematically assigning coordinate systems to each link of an articulated chain. The axis of revolute joint i is aligned with The xi1 axis is directed along the normal from zi1 to zi. I f z axe s are intersecting xi1 axis is parallel to zi1 zi. The y axis follows the right hand rule which satisfies zi in the direction of xi yi. Therefore t he four parameters can be summarized as: PAGE 18 18 link le ngth ai the offset distance between the zi1 and zi axes along xi axis; link twist i the angle from the zi1 axis to the zi axis about the xi axis; link offset di the distance from the origin of frame i 1 to the xi axis along the zi1 axis; joint angle i the angle between the xi 1 and xi axes about the zi1 axis. For a revolute joint, the joint angle is variable, while the link offset d is constant F or a prismatic joint, the joint angle is constant, while the link offset d is variable The relationship between two neighboring coordinate fram es is expressed in Equation (21) = (2 1) is the homogeneous transformation representing the pos ition of coordinate frame i with respec t to the world coordinate frame 0. is the t ransformation matrix that transfers the link coordinate frame i 1 to the next coordinated frame i It is = cos sin cos sin sin cos sin cos cos cos sin sin 0 sin cos d0 0 0 1 (2 2) We can see that this transformation matrix i s a 4 homogeneous matrix. The entries of the matrix are composed of trigonometric functions of i, i and di. The upper left 3 3 submatrix of represents the rotational relationship from the i1th link frame to the ith link frame. The fourth column of descr ibes the transitional relationship from the origin of the i 1th link frame to the origin of the ith link frame. The rest entries are zeroes which make the matrix a homogeneous one. There are two ways to do kinematics analysis, one is forward kinematics and the other is inverse kinematics. A forward analysis of a serial manipulator determines the unique position and orientation of the last link with a specified set of joint variables. It is obtained by a sequence of multiplication of the transformation matrixes PAGE 19 19 = (2 3) is t he overall manipulator transform, that is the end frame with respect to the base frame. A typical robot manipulator normally has 6 joints because many end effector s need 6 degree of freedom to reach arbitrary position in the space. For example, the Puma 560 is a 6axis robot, so the overall transform can be written as or T6 for short An inverse kinematic analysis of a serial manipulator determines all possible sets of the joint variables for any specified end effector location. Each set of joint variables defines a particular arm pose for the given end effector location. The joint variables can be summarized as q= for a revolute joint dfor a prismatic joint (2 4) The inverse kinematic analysis is especially important when the end effector is scheduled to perform some specified task. For example, a fruit picking robot manipulator is built to reach a number of fruit locations and then collect the picked fruit by placing them in a container. In order to find out the joint variables, we can generalize the overall transform matrix as T= K (q ) (2 5) w here = [ ] is the variable we need to solve and Tn is known from the given task l ocation for the endeffector. q = K (T) (2 6) There are many methods to solve the inverse kinematic problem. Matlab Robotics Toolbox is a fast way to compute the solution. Dr. Crane (1998) also introduced a closed loop method to solve out kinematic analysis problem s in his book. In most cases, t he solution is not unique, and for some classes of manipulator no solution exists. N o solution cases may be due to an alignment of joint axes reducing the effective DO F or the target point being out of workspace. PAGE 20 20 If the manipulator has more than 6 joints it is said to be redundant and the solution for joint angles is under determined. Manipulator Dynamics Manipulator dynamics is the study of motion with regard to forces. Motion is described by the displaceme nt of endeffector, joint velocity and acceleration. Forces are exerted as actuators which generate torques to drive the motion. Dynamic analysis is vital for mechanical design, control and simulation. Similar to kinematics, there are two important ways to solve dynamics analysis problems, one is forward (or direct) dynamics and the other is inverse dynamics. Forward dynamics is to compute the motion of manipulator from the given actuation forces and torques. Inverse dynamics is to compute the joint actuati on forces or torques from the position of endeffector, velocity, and acceleration. For an naxis rigi d body manipulator, Armstrong (1986) derived a dynamic model which generalized the motion with equation A ( q ) q + B ( q )[ q q ] + C ( q ) [q ] + G ( q ) = (2 7) w here ( ) is the n n symmetric inertia matrix or kinetic energy matrix; ( ) is the n n(n 1)/2 matrix of Coriolis torques; ( ) is the n n matrix of centrifugal torques; ( ) is the n vector of gravity torques; is the generalized joint force vector. is the n vector of joint variables describing the pose of manipulator ; is the n vector of joint velocities; is the n vector of joint accelerations; PAGE 21 21 The symbols [ ] and [ ] are given by: [ ] = [ , , , ], [ ] = [ , ]. For forward dynamics, we are given the right hand s ide of the E quation ( 2 7) and aim at finding Whilst for inverse dynamics, we are given the left hand side of the E quation ( 27) and aim at finding The equation can be derived via a number of techniques, such as the Lagrange method, NewtonEuler (NE) methodology, and Kanes methodology ( 1983) These methods compute the matrixes A, B, C and G directly with complex expressions, a nd they are not recursive. The NE and Lagrange forms can be written in terms of the Denavit Hartenberg parameters. Kanes method can have lower computational cost for specific manipulator s but cannot be generalized by D H parameters. Luh (1980) provided a recursive formulation of the Newton Euler equations with linear and angular velocities referred to link coordinate frames. He suggested a time improvement from the Lagrangians formulation, and thus it became practical in the implement of computation. Com paring to the nonrecursive forms, the recursive forms are more efficient. The Matlab Robotics Toolbox uses the recursive NewtonEuler (RNE) formulation (198 0) to compute the inverse dynamics problems. There are two step s to perform the recursive Newton E uler techniques, first goes the backward recursion and then the forward recursion. The formulas derived by Hollerbach (1980) and Walker and Orin (1982) included two different coordinate systems which the expressions are based on, one is expressed in the base coordinate frame which is also the inertia frame, and the other is expressed in the links internal coordinate frame. PAGE 22 22 Backward Recursion The backwa rd recursion propagates angular velocities, angular accelerations, linear accelerations, total link forces, and total link torques from the base coordinate frame. Figure 28 indicates the position vectors from the base origin to the origins of link frames and the center of mass. The velocity and acceleration parameters are computed through the basic concept of two points fixed on a rigid body. For a rotational joint, the recursive equations represented in the base coordinate frame are: = + z q (2 8 ) = + z q + z q (2 9) p = p + p (2 10) p = p + p + ( p ) (2 11) w here is the angular velocity of link i represented in the base coordinate frame is the angular acceleration of link i represented in the base coordinate frame is the position vector from the base coordinate origin to the joint i coordinate origin p is the position vector from coordinate origin i 1 to coordinate origin i is the joint variable of joint i is the unit vector in Z direction of joint frame i represented in the base coordinate frame For effective computation, the Matlab Robotics Toolbox performs the recursive algorithm based on link's internal coordinate frames. Comparing with the base frame form, the local frame form describes the relevant link in its own coordinate frame, and identi fies the kinematic perimeters with left superscripts to show which frame it is represented with. In order PAGE 23 23 to connect different coordinate systems, the local frame recursive formulas combine the rotation matrixes to transfer the coordinate frames. The recur sive equations represented in the link's internal coordinate frames are: = R ( + z q ) (2 12) = R ( + z q + z q ) (2 13) v = p = R v + p (2 14) v = p = R v + p + p (2 15) where is the angular velocity of link i represented in frame i, is the angular acceleration of link i represented in frame i, v is the linear velocity of frame i is the linear acceleration of frame i is the local unit vector in Z direction, obviously = [ 0 0 1 ] is the position vector from coordinate origin i 1 to coordinate origin i with respect to frame i. = [ ] is the rotation matrix defining frame i orientation with res pect to frame i 1. It is the upper left 3 3 submatrix of the link transform matrix, which is R = cos sin cos sin sin sin cos cos cos sin 0 sin cos (2 16) As this rotation matrix is orthonormal, we have R = R = R (2 17) PAGE 24 24 For a translational joint, the recursive equations represented the link's internal coordinate frames are: = R (2 18) = R (2 19) v = p = R ( v + z q ) + p (2 20) v = p = R ( v + z q ) + p + p + 2 ( R z q ) (2 21) All terms have been defined previously. Boundary conditions are used to accomplish the algorithm. We introduce the effect of gravity as the acceleration of the base link and set the angular param eters as zero. = [ 0 0 0 ] = [ 0 0 0 ] = [ 0 0 0 ] = [ 0 0 ] The backward recursion also needs to propagate total link forces, and t otal link torques. Hollerbach (1980) have derived the formulas as below. All terms can be referred to the base coordinate frame or the internal coordinate frames. r = r + ( r ) + p (2 22) F= mr (2 23) N= I + ( I) (2 24) Where the undefined terms are is a position vector f rom coordinate origin i to the link i center of mass ri is a position vector from the base coordinate origin to the link i center of mass, PAGE 25 25 is the mass of link i Fi is the total external force at the center of mess of link i Ni is the total external torque at the center of mess on link i Ii is the inertia tensor of link i about its center of mass. I can be described as a 3 3matrix, in which the diagonal entries are the moments of inertia, and the off diagonals are products of inert ia. I = IIIIIIIII (2 25) Fo rward Recursi on The forward recursion propagates the forces and moments exerted on link i by link i 1 from the end link of the manipulator to the base. f= f + F (2 26) n= n + N+ ( p + r ) F+ p f (2 27) = z n z f if link i + 1 is rotational if link i + 1 is translational (2 28) Where the undefined terms are is the force exerted on link i by link i 1, is the moment exerted on link i by link i 1 is the input torque exerted by actuator at joint i. The implicit reference coordinate frame in these formulas is the base coordinate frame. Similar to the backward recursion, we can introduce rotation matrixes and internal coordinate systems to the formulas as: f = R f + F (2 29) n = R ( n + R p f ) + p + r F + N (2 30) PAGE 26 26 = n R z f R z if link i + 1 is rotational if link i + 1 is translational (2 31) All terms have been defined previously. After finishing the backward and forward recursion processes, the torques or forces from the actuators come out as the solution of inverse dynamics. The Matlab Robotics Toolbox uses Method 1 in Walker and Orin techniques (1982) for computing the forward dynamics. This method makes use of the recursive Newton Euler solution of inverse dynamics. PAGE 27 27 Figure 21. MAGALI apple picking manipulator developed by Grand DEsnon et al. (1987) Figure 22. Citrus picking robot developed by Harrell (1988) PAGE 28 28 Figure 23. The prototype of citrus harvesting robot manipulator ( Source: Babu Sivaraman. Design and Development of a Robot Manipulator for Citrus Harvesting. Dissertation for the degree of doctor of philosophy at University of Florida. 2006) Figure 24. The joint types of the manipulator (Sivaraman et al., 2006) PAGE 29 29 Figure 25. Kubota m andarin orange harvesting robot ( Source: Naoshi Kondo, Mitsuji Monta & Noboru Noguchi. Agricultural Robots Mechanisms and Practice Kyoto University Press. 2006) Figure 26. Tomato harvesting robot designed by Okayama University. Source: Reprinted with permission from ( Source: Naoshi Kondo, Mitsuji Monta & Noboru Noguchi. Agricultural Robots Mechanisms and Practice Kyoto University Press. 2006) PAGE 30 30 Figure 27. Tomato harvester manipulator ( Kon do et al., 1996) Figure 28. Position vectors between the base origin and the link origins and the center of mass (Kurfess, 2005) Figure 29. T he notation used for inverse dynamics (Corke, 2002) PAGE 31 31 CHAPTER 3 KINEMATICS FOR ROBOTIC MANIPULATOR TO PICK FRUIT Puma Robot Manipulator Puma robot is one of the numerous mature robots that are popular in many industries. It is easy to do kinematic and dynamic analysis for Puma because t he data are available in many robotics research studies. I n this research th e Puma 560 robotic arm is selected as a representative fruit picking manipulator because the arm structure covers the characters from many other fruit picking robots. The Puma Robot from the Unimation Company is a widely used model of electrically driven robots. The series 500 is designed for applications requiring high degrees of flexibility and reliability. The Puma 560 is a six degree s of freedom robot manipulator wi th 6 rotational joints shown in Figure 31. The end effector of the robot arm can reach a point within its workspace from any direction. The six degrees of freedom are controlled by six brushed DC servo motors The power requirement is 110130VAC, 5060Hz 1500Watts. Its load capacity is 2.5kg (5.5 lbs.), and the maximum straight line velocity is 0.5m/sec. The arm of Puma 560 weighs 54.5kg (120 lbs.) The robot can determine its global pos i tion from the given feedback information. The robot can be controlle d remotely through a networ k. M atlab Robotics Toolbox The M atlab Robotics Toolbox (Corke, 2002) can perform many useful functions for robotics analysis such as kinematics, dynamics, and trajectory generation. The creation of a serial link robot manipulato r is based on the D H parameters. The Toolbox provides functions for converting between vectors, homogeneous transformations matrixes and quaternions which are necessary to represent 3 dimensional position and orientation. The Toolbox is also capable of plotting the 3D graphical robot and allows users to drive the robot model. PAGE 32 32 Forward K inematic s for P uma 560 According to Denavit and Hartenberg notation, we can number each link and joint as show in Figure 32. T he location of the end effector is dominated by the angles of joint s 1, 2 and 3, while joints 4, 5 and 6 play important roles in changing the orientation of the endeffector. Generally speaking, the workspace of the end effector is determined mostly by t he lengths of l inks 1, 2 and 3. The standard coordinate frames and the D H parameters are showed in Figure 33. Frame 0 is attached to link 0 and z0 is aligned with the axis of joint 1. Frame 1 is attached to link 1 and z1 is aligned with the axis of joint 2, and so on. Table 31 lists the values of link twist, link length, link offset of each link and the joint angle is variable. The for ward kinematic analysis of Puma 560 is done by using Matlab Robotics Toolbox. Several functions are used to do the analysis convenientl y. First, create the six links and set the D H parameters as given in Table 3 1. The joint variable is set to be 0 at the beginning. The first three links have rotation limits, which are defined as link properties. As each link is well defined, use robot function to set up the puma model. Second, define the time step and generate a time vector. A few useful joint angle vectors are generated to define the starting position, ready position or reach position. We can get the trajectory between any of these two joint angle vectors with respect to time. After that, we can easily perform the forward kinematic analysis by using fkine function, which returns a homogeneous transformation for the final link of the manipulator. A 3dimensional matrix is returned the first two dimensions homogeneous transformation and the third dimension is ti me. Finally, a few figures are plotted to present the result. Matlab Robotics Toolbox allows users to see the movement of the robot in 3D space, which makes the trajectory visible. The whole function returns the coordinates of the endeffector if the user input the angle of every joint. Here is the source code in Matlab. PAGE 33 33 function [preach]= puma560ForwK(qreach) close all; %create links using DH parameters %L = link([alpha, a, theta, d], convention) L{1} = link([ pi/2 0 0 0 0], 'standard'); L{2} = link([ 0 .4318 0 .15005 0], 'standard'); L{3} = link([pi/2 .0203 0 0 0], 'standard'); L{4} = link([pi/2 0 0 .4318 0], 'standard'); L{5} = link([pi/2 0 0 0 0], 'standard'); L{6} = link([0 0 0 0 0], 'standard'); %assign joint rotation limit L{1}.qlim=[deg2rad(160) deg2rad(160)]; L{2}.qlim=[deg2rad(125) deg2rad(125)]; L{3}.qlim=[deg2rad(270) deg2rad(90)]; %build up the robot model puma560 = robot(L, 'Puma 560'); qzero=zeros(1,6); qready = [0 pi/4 pi/4 0 0 0]; % ready position % generate a time vector t=[0:0.056:2]; % compute the joint coordinate trajectory q = jtraj(qready, qreach, t); % forward kinematics for each joint coordinate T = fkine(puma560, q); preach=T(:,4,36); %figure plotting figure,title('Puma 560 Forward Kinematics'); plot(puma560_ready,qready,'noname','erase'), hold on plot(puma560,q,'noname','noerase'); figure,plot3(squeeze(T(1,4,:)),squeeze(T(2,4,:)), squeeze(T(3,4,:))); xlabel('X (m)'),ylabel('Y (m)'),zlabel('Z (m)'), title('Endeffector 3D trajectory'),grid; figure,plot(t,squeeze(T(1,4,:)),'',t,squeeze(T(2,4,:)),'',t, squeeze(T(3,4,:)),'.'), xlabel('Time(s)'), ylabel('Coordinate (m)'),grid, legend('X','Y','Z'), title('Endeffector Position'); figure,plot(t,rad2deg(q(:,1)),'',t,rad2deg(q(:,2)),'',t, rad2deg(q(:,3)),'.'), xlabel('Time(s)'), ylabel('Joint Angle (Deg)'),grid, legend('Joint 1','Joint 2','Joint 3'), title('Joint Angle Variation'); end PAGE 34 34 For example, if we input the joint angles as qreach=[1.0694 0.0637 0.9054 0.0000 0.8417 1.0694] which means the robot needs to reach a position where 1=1.0694, 2= 0.0637, 3= 0.9054, 4= 0 5= 0.8417, 6= 1.0694, the function returns the coordinate vector as ans = 0.5000 0.6000 0.3000 1.0000 w hich means the final position of the endeffector is [0.5 0.6 0.3] with respect to the origin of the base coordinated frame of Puma robot. Figure 35 to Figure 38 are the output s of Puma 560 f orward kinematics programming. Figur e 35 shows the variati on of the first three joint angles with respect to time. A 7th order polynomial is used to fit the values. Figure 3 6 is the 3D simulation of Puma 560, and it reflects the motion in the space of every link gradually. The initial pose is shown in blue and t he final pose in black. It takes 2 seconds for the Puma 560 to perform the motion from t he initial pose to the final through every time step. Figure 36 is the trajectory showing the space displacement of the endeffect which provides a visualized viewpoint on the performance. Figure 38 shows the change of X, Y, Z coordinates of the endeffector. In this case, the end effector moves farther along y direction than along x direction, and the z span is the shortest. Inverse K inematic s for P uma 560 The inverse kinematic analysis of Puma 560 is done by using Matlab Robotics Toolbox. As in forward kinematics, we create the six link robot model and a time vector first As the ready pose is given by joint angles, we can use for ward kinematics function to generate the transform matrix of the ready pose with respect to the base of the robot The goal position is given by coordinate value, we can use trans l function to create goal transform matrix. The ctraj PAGE 35 35 function is used to produce the trajectory from the two transform matrix in the Cartesian coordinate It returns a Cartesian trajectory with straight line motion from the starting point to the finishing point represented by homogeneous transforms. After that, we can easily p erform the inverse kinematic analysis by using i kine function. This function returns a serial of vectors and each line of vector represents the six joint angle values in a step of time. Finally, a few figures are plotted to present the result. The whole function returns the final angle of every joint if the user input the position of the endeffector Here is the source code in Matlab. The robot building up part is omitted because it is the same as forward analysis. function [qreach] = puma560InverK(preach) %create links using DH parameters %build the robot model t=[0:0.056:2]; qready = [0 pi/4 pi/4 0 0 0]; %form a homogeneous transformation matrix from the ready joint angles using forward kinematic analysis T0 = fkine(puma560, qready); %form a homogeneous transformation matrix from goal position T1= transl(preach); %compute a Cartesian path TR01 = ctraj(T0, T1, length(t)); % to reach the goal position TR10 = ctraj(T1, T0, length(t)); %return to the ready position %inverse kinematic analysis q=ikine(puma560, TR01); %reach the goal only qq=ikine(puma560,TR10); %return to initial pose qreach=q(36,:); %figure plotting end PAGE 36 36 For example, if we input the goal position as puma560InverK([0.414, 0.203, 0.597]) which means the robot needs to reach a position has the coordinate values x = 0.414, y= 0.203, z= 0.597. The program returns the joint angle vector as ans = 0.1244 0.3955 0.4354 0.0000 0.0399 0.1244 w hich means in order to reach the final position the 6 joint angles should have the values of 1= 0.1244, 2= 0.3955, 30.4354, 4=0 5= 0.0399, 6= 0.1244 From the output figure s we can observe the movement of robot manipulator. It starts from a ready position and then move to the goal position which is inputted by user. After reaching the goal position, it pauses for a second to allow the endeffector finish ing its task s Finally, the manipulator returns to its starting position and ready for another motion. T he program also produces joint angle values showing the change of each joint angle with respect to time. See Figure 3 9. In this case, the joints 2 and 3 rotate by larger angle s than joint 1 and all of the joints rotate within their joint limits. Figure 3 10 is the 3D trajectory of the endeffector from the invers kinematics. A straight line motion is created rather than a polynomial fit ting curve because the use of invers kinematics is to find a fastest path to reach the goal. PAGE 37 37 Figure 31. PUMA 560 r obot m anipulator ( Unimation, 1984) Figure 32. Links and j oints of Puma 560 (Benitez, et al., 2012) PAGE 38 38 Figure 33. Coordinate f rames of Puma 560 (Kurfess, 2005) Figure 34. Puma 560 ready pose and zero pose PAGE 39 39 Figure 35. Puma 560 forward kinematics joint angles Figure 36. Puma 560 forward kinematics simulation PAGE 40 40 Figure 37. Puma 560 forward kinematics endeffector trajectory Figure 38. Puma 560 forward kinematics endeffector X, Y, Z coordinates PAGE 41 41 Figure 39. Puma 560 inverse kinematics joint angles Figure 310. Puma 560 inverse kinematics endeffector trajectory PAGE 42 42 Table 3 1. D H parameters of Puma 560. Link No. L ink Twist (rad) Link Length a (m) Link Offset d (m) Joi nt Angle (rad) Joint Type Link 1 1.570796 0 0 Variable 1 R evolute Link 2 0 0.4318 0.15005 Variable 2 R evolute Link 3 1.570796 0.0203 0 Variable 3 R evolute Link 4 1.570796 0 0.4318 Variable 4 R evolute Link 5 1.570796 0 0 Variable 5 R evolute Link 6 0 0 0 Variable 6 R evolute PAGE 43 43 CHAPTER 4 DYNAMICS FOR ROBOTIC MANIPULATOR TO PICK FRUIT Dynamic Parameters of the Puma 560 Many studies have been working on the examination of the dynamic parameters for Puma robots. Some of the well known studies are from Armstrong (1986) Paul (1981) Lee (1983) and Tarn (1985) Corke (1994) have also did research on the comparison among those studies. The main dynamic parameters for a robot manipulator can be separated into two aspects, one is regarding the links inertia and the other is regarding the actuator or motor. For the links inert ia, we mostly consider: Link mass Link center of gravity Link moments of inertia Com paring to the other studies, P au l s (1981) research is more fundamental and follows the standard D H notation. The data I use in this thesis refers to P au ls experiment be cause it contains detail values of the first link of Puma 560. Moreover, P au l assumes the link mass with uniform distribution, which is useful for my rese arch in the next section. Table 41, Table 42, and Table 43 list the links dynamic parameters of Pu ma 560 For the actuators, the dynamic parameters we mostly consider are: Motor inertia Gear ratio Friction There are two types of Puma 560, one is built by Unimation and the other is by Kawasaki (Japan). The two types are similar in most respects but u se different servo motors. Typically, the motors used for the first 3 joints are larger than for the last 3 joints due to the purpose of the links. The motor inertia combining with the link inertia is used to learn the total rotational inertia at PAGE 44 44 each join t. The motor friction reported in previous studies is Coulomb and viscous friction. The motor data used in the following analysis is based on the Unimation type. As discussed, the code to build a Puma 560 robot using Matlab Robtics Toolbox can be defined a s below. %the dynamics parameters refer to Pauls study %motor data refers to the Unimation Puma 560 %define link mass L{1}.m = 4.43; L{2}.m = 10.2; L{3}.m = 4.8; L{4}.m = 1.18; L{5}.m = 0.32; L{6}.m = 0.13; %define center of gravity L{1}.r = [ 0 0 0.08]; L{2}.r = [ 0.216 0 0.026]; L{3}.r = [ 0 0 0.216]; L{4}.r = [ 0 0.02 0]; L{5}.r = [ 0 0 0]; L{6}.r = [ 0 0 0.01]; %define link inertial as a 6element vector %interpreted in the order of [Ixx Iyy Izz Ixy Iyz Ixz] L{1}.I = [ 0.195 0.195 0.026 0 0 0]; L{2}.I = [ 0.588 1.886 1.470 0 0 0]; L{3}.I = [ 0.324 0.324 0.017 0 0 0]; L{4}.I = [ 3.83e3 2.5e3 3.83e3 0 0 0]; L{5}.I = [ 0.216e3 0.216e3 0.348e3 0 0 0]; L{6}.I = [ 0.437e3 0.437e3 0.013e3 0 0 0]; %define motor inertia L{1}.Jm = 200e6; L{2}.Jm = 200e6; L{3}.Jm = 200e6; L{4}.Jm = 33e6; L{5}.Jm = 33e6; L{6}.Jm = 33e6; %define gear ratio L{1}.G = 62.6111; L{2}.G = 107.815; L{3}.G = 53.7063; L{4}.G = 76.0364; L{5}.G = 71.923; L{6}.G = 76.686; % motor viscous friction PAGE 45 45 L{1}.B = 1.48e3; L{2}.B = .817e3; L{3}.B = 1.38e3; L{4}.B = 71.2e6; L{5}.B = 82.6e6; L{6}.B = 36.7e6; % motor Coulomb friction L{1}.Tc = [ .395 .435]; L{2}.Tc = [ .126 .071]; L{3}.Tc = [ .132 .105]; L{4}.Tc = [ 11.2e3 16.9e3]; L{5}.Tc = [ 9.26e3 14.5e3]; L{6}.Tc = [ 3.96e3 10.5e3] %build the robot model puma560 = robot(L, 'Puma 560'); After defin ing the link mass, center of gravity, link inertial, motor inertia gear ratio and Coulomb friction for the six links, the Puma 560 model is successful ly built. The dynamic analysis will be performed on the base of this model. Inverse Dynamics for Puma 560 The first several steps to do inverse dynamic analysis of Puma 560 in Matlab are similar to kinematics. Create the robot model with dynamics parameters which have been listed above, and then set up the time vector and useful pose vectors. T o find a joint spac e trajectory between two joint posed, the Toolbox has a function jtraj It uses a 7 th order polynomial with default zero boundary conditions to compute the velocities and accelerations. It also allows the user to add the boundary conditions for velocity. The built in function rne is used to computer inverse dynamics via recursive Newton Euler formulation. This function returns the torque from the actuator of each link. Here is t he Matlab code to use these functions: function [taufmax,tau0fmax] = puma560InverD(preach) %create links using DH parameters %build the robot model qready = [0 pi/4 pi/4 0 0 0]; PAGE 46 46 %use inverse kinematics solution to computer the joint angle qreach=puma560InverK(preach); % create time vector t = [0:.056:2]; [q,qd,qdd]=jtraj(qready,qreach,t); % compute joint coordinate trajectory %compute inverse dynamics using recursive NewtonEuler algorithm tauf = rne(puma560, q, qd, qdd); % compute the joint torque with no motor friction tau0f=rne(nofriction(puma560), q, qd, qdd); % compute the joint torque from the gravity load only taug = gravload(puma560, q); taufmax=[max(abs(tauf(:,1))) max(abs(tauf(:,2))) max(abs(tauf(:,3)))]; tau0fmax=[max(abs(tau0f(:,1))) max(abs(tau0f(:,2))) max(abs(tau0f(:,3)))]; %figure plotting end To use this inverse dynamics function, I input a goal position for the endeffector as an example: puma560InverD([0.5, 0.6, 0.3]) A few figures are plotted to show the trend and values of joint velocities, accelerations and torques. Observing the Figure 41, the angular velocity of all the joints have similar trend. This is because the b uilt in curve fitting function to compute the velocity uses the same order of polynomial. The angular velocities increase from zero to the middle of time vector and then decrease gradually to zero for finishing. The curves are smooth with no singular point For angular accelerations, all joints have their curve cross the zero line which means a trend switch of velocity. The crossing point implies the peek time of absolute velocity. In this case, the joint 3 has comparatively larger absolute values of angula r velocity and acceleration among the joints. PAGE 47 47 Observing the Figure 42, the motor torque exerted on link 2 has the larger value than on link 1 and 3. This is because m uch of the torque on joints 2 is due to gravity. The torque on joint 2 keeps increasing w hile the ones on joint 1 and 3 switch direction during the motion. Joint 46 do not have much torque due to their type of motor. When the motors inner friction is added to analysis the model, we can find the result in Figure 43. The motor friction affects only the beginning and the end of the motion. It increases the absolute value of the motor torque in a rapid speed when the robot starts moving, and drops to the initial value fast when the robot reach the stop position. The torque trend within the process is just identical to the non friction analysis. Figure 44 shows the torques on joint 2 and 3 caused by the links, gravity loads. As we can see, the torque affected by gravity load keeps a large percentage of the total motion torque, while the tor que from the pure motion occupies a comparatively small portion. The i nitial torque on joint 2 is already very large before it starts to move. This fact implies some suggestion on reducing the weight when we need to design a robot. PAGE 48 48 Figure 41. Link a ngular velocity and a cceleration Figure 42. Link m otor t orque for the r obot m odel without f riction PAGE 49 49 Figure 43. Joint m otor t orque for the r obot m odel with f riction Figure 44. T he comparisons between total load torque and gravity load torque PAGE 50 50 Figure 45. Maximum joint torques for different f ruit picking duration PAGE 51 51 Table 41. Link m ass of Puma 560 Link Index Mass (kg) 1 4.43 2 10.20 3 4.8 4 1.18 5 0.32 6 0.13 Table 42. Center of gravity of Puma 560. Link Frame Direction Coordinate Value(m) 1 X 1 0 Y 1 0 Z 1 0.08 2 X 2 0.216 Y 2 0 Z 2 0.026 3 X 3 0 Y 3 0 Z 3 0.216 4 X 4 0 Y 4 0.02 Z 4 0 5 X 5 0 Y 5 0 Z 5 0 6 X 6 0 Y 6 0 Z 6 0.0 PAGE 52 52 Table 43. Moments of i nertia about COG Link Index Moments of Inertia Value (kg m 2 ) 1 I xx1 0.195 I yy1 0.195 I zz1 0.026 2 I xx2 0.588 I yy2 1.886 I zz2 1.470 3 I xx3 0.324 I yy3 0.324 I zz3 0.017 4 I xx4 3.83e 3 I yy4 2.53e 3 I zz4 3.83e 3 5 I xx5 0.216e 3 I yy5 0.216e 3 I zz5 0.348e 3 6 I xx6 0.437e 3 I yy6 0.437e 3 I zz6 0.013e 3 PAGE 53 53 CHAPTER 5 FRUIT PICKING ARM DESIGNER GUI Assumption In this chapter, a graphic user interface (GUI) is designed to generate fruit picking arms that satisf ies the target plant type. In C hapter 2, I have listed a few previous harvesting robots that can pick tree fruit like apple, citrus and tomato. Comparing with one another, most of them have wheeled vehicles that carry picking manipulators and fruit collecting baskets, manipulators that send endeffects to reach the fruit, and endeffectors that perform the picking and detaching. Figure 51 shows Kubota orange harvesting robot (Sarig, 1993) T his design concept can be used as an illustration and the GUI developed in this chapter is based on it as an assumption. If we inspect the manipulator aims, all of them have several degrees of freedom some dominate the picking workspace and others dominate the approaching angle of the endeffector. Normally, the workspace dominating links have actuators with larger torque r equirement s due to their longer link length, while the approaching angle dominating links have smaller mot ors with accurate controllers. Therefore, there exist some common concepts among the fruit picking arm developments as long as the plant structures ar e similar. These common concepts can be generalized in Figure 5 2. Three rotational joints and two relatively long links are the main body connecting the base and the endeffector. From the base, it first has a joint with vertical revolution axis, and then two revolute joints with horizontal axis carrying two dominant links. At the end of the second dominant link, there connects an end effector which might contain 2 to 3 degrees of freedom. This robotic manipulator framework is right what Puma 560 has. Therefore, I assume the fruit picking arm we want to develop is similar to the first three links of the Puma 560 manipulator The rest links of Puma 560 are of the endeffector whi ch will not be considered in this research. The two workspace dominated links are assumed to have equal length PAGE 54 54 Some more assumption is regarding with the dynamic parameters. As discussed in C hapter 4 the li nks inertia parameters that we mostly consider are l ink mass c enter of gravity and moments of inertia The dynamic analysis of the fruit picking arm generated in this chapter refers to Puma 560 parameters. The link masses are assumed to be propor tional to link lengths of Puma 560 with the same material density. Link center of gravities are also proportionally computed. The calculation of link moments of inertia is more complicated and refers to Armstrongs (1986) research on Puma 560. Armstrong pr esent ed a suspension model to measure the moments of inertia see Figure5 3. The rotational inertia was concluded as I = (5 1) where I is the inertia about the axis of rotation Mg is the weight of the link r is the distance from each suspension wire to the axis of rotation is the oscillation frequency in radians per second l is the length of the supporting wires Applying this inertia computation, I assume the oscillation frequency and the length of the support wires are fixed for the picking arm generated in GUI. Therefore, the moment of inertia will be proportional to the link mass and link length square. This assumption is only valid for limited link length due to the way it is established The motor internal frictions are not considered in the dynamic analysis here because the users have their own choices when designing a manipulator. Other assumptions in this chapter are regarding with the orchard and manipulator behaviors. I assume the orchards or the greenhouses have enough room to place the robotic manipulator. A specified tree type in the same orchard is assumed to have similar fruit PAGE 55 55 distribution. Extreme cases are not considered and analyzed. The manipulator does not need to pick every fruit on a tr ee during each working period but can reach almost every fruit within its designed workspace. The detailed behavior of the endeffector is not considered in this research. The fruit positions are assumed to be detected by reliable sensors. All the assumpt ions are valid because the GUI is developed for future researcher s to get an overall fruit picking scheme before they design and build the arm. The user s need to do further studies if they have specific limits and special conditions. Graphical User Interface Development The fruit picking arm designer GUI is created by using Matlab GUI due to its compatibility with Matlab Robotics Toolbox. Figure 54 shows the fruit picking arm designer GUI. It allows the user to input five typical fruit positions with respect to the tree coordinate frame The instruction for en tering the fruit coordinates is indicated in the upper left image in the window A lso see Figure 5 5. The five typical fruit position s are the highest, the lowest, the right most, the left most and the front most which might be the closest to the robot arm. The first four positions determine the manipulator workspace and the last one determines the manipulator base location. If the user enters these five fruit positions from a specified tree, th e generated picking arm is designed for this specified case. The user might collect the five typical fruit positions from the tree s in one orchard and then enter their average coordinate values in GUI In this way, the generated picking arm will be suitable to most trees in that orchard. In Figure 55, a coordinate frame is set to trunk of a fruit tree with its origin to the ground. The x vector of this coordinate frame points to the base of the manipulator and the z vector is pointing up vertically. For easy computation, I located the base of the manipulator on the positive x vector. The five typical fruit cases are highlighted in orange with numbers from 1 to 5. PAGE 56 56 In order to reach this five fruit positions with a shortest arm length, the base of the manip ulator need to be lifted to the middle of the height difference between the highest fruit and the lowest one. Let b represents the height of the base, then b = + z, where zi is the height of fruit i in Figure 5 5. T he Pythagorean Theorem is used to bui ld the relationship between t he arm length a and the horizontal offset from the tree to the base d. I f we assume the highest fruit is reached when the arm is fully extended the relationship could be ( 2a )= + Figure 56 shows the extended arm reaching the two typical fruit positions. In general, the arm length a, the base height b, and base distance d are the outcome of the GUI, representing the basi c character s of the fruit picking arm. These three characters identify the shortest arm length need ed to be in order to reach the typical fruit s and the relative position the base need to be located with respect to the fruit tree. The computation of t hes e three characters depends only on the inputted coordinates in green boxes in Figure 54. They are the height of the highest and the lowest fruit, the side offsets of the left most and the right most fruit, and the x coordinated of the front most fruit. After the arm model is created, the user can easily do in verse kinematic and dynamic analy sis on it. T he analysis compiles the program using Matlab Robotics Toolbox which has been presented in previous chapters. If the user click s the Inverse Kinematics button, a joint angle variation image will show up in the lower left corner of the window. The image will include the joint angle variations of joint 1 to 3 with respect to time for the arm to pick the typical fruit by sequence. If the user click s the Inverse Dynamics button, a motor torque variation image will show up in the lower right corn er of the window. The image will include the torque variations of joint 1 to 3 with respect to time for the arm to pick the typical fruit by sequence. Note that the arm is assumed to return to its initial pose for fruit collection before it PAGE 57 57 starting to mak e a new pick. Because the system only has limited target fruit positions, the kinematic and dynamic analysis cannot represent all the fruit picking cases. The typical fruit picking process is just analyzed as illustration to help the user understanding the arms performance or selecting the motor. Figure 57 generalizes the work process of the GUI in a flow char t Fruit Tree T est Cases To test the Fruit Picking Arm Designer, I use two special cases, one is a peach tree and the other is a citrus tree. Both of the fruit trees are cultivated in an orchard at University of Florida. Test C ase O ne: P each T ree Figure 58 is a photograph of a typical peach tree taken in 2013 summer. The tree is about 2 m in height and 3 m in width with a few mature peach es on it. The five peaches that the GUI require s to consider are highlighted in red circle s with index number s in order A coordinate frame is attached to the peach tree, and t he coordinate of the five specified peaches are measured as: 1. The highest peach: x1=0.374 m, y1= 0.104 m, z1=2.012 m 2. The lowest peach: x2=0.589 m, y2= 0.018 m, z2=0.356 m 3. The left most peach: x3=0.298 m, y3= 1.672 m, z3=1.123 m 4. The right most peach: x4=0.461 m, y4= 1.720 m, z4=1.505 m 5. The front most peach: x5=0.603 m, y5= 0.419 m, z5=1.812 m After entering these coordinate values in the input panel, the user just click the generate button, and then the character parameters of the fruit picking arm will be calculated and pre sent in the output panel. In this case, the designed arm has Arm length a =1.651 m Base height b=1.184 m Base distance d=1.770 m PAGE 58 58 Therefore, the fruit picking arm designed for this peach tree should be located at 1.77 0 m away from the tree root with the base lifted to 1.184m high. The manipulator is supposed to have revolute joints for the first three joints, and the two links connecting neighboring joints should have 1.651 m in length equally in order to reach the peach es within the workspace. Next is to analyze the picking performance of the designed arm. Figure 59 shows the outcome of this test case. The inverse kinematics function produces a figure of joint angle vs time. During 25 seconds, the arm picks the five peach es in sequence and each pick spends almost 5 seconds. For instance, in period 0 to 5, the arm first move s forward to reach the highest peach and then returns to its initial pose to drop the fruit for collection. We can see that joint 1 does not rotate m uch when picking the highest and the lowest peaches, while picking the left and the right peaches it rotates by 50 degree at most. Joint 2 and 3 angles have steps when the arm finishing picking the lowest peach. The steps are due to the joint angles range. From Figure 5 9, we can see that joint 2 rotates by larger angles for picking the highest and the lowest peaches than the others. The joint 3 rotates largely when picking the left and the right peach. The inverse dynamics function produces a figure of joint torque vs time. From Figure 5 9, we can find that the joint 2 has the largest torque with its maximum of around 600 Nm. This means when the user wants to select motors for the arm, the motor torque for joint 2 should be no less than 600 Nm. The maximum torques of joint 1 and 3 are around 200 Nm. Test C ase T wo: C itrus T ree Figure 510 is the side view of a citrus tree taken in 2013 summer. The tree is about 3.5 m in height and 2 m in width with some citrus on it. A coord inate frame is attached to the citrus tree, and the coordinate of the five target citruses are measured as: 1. The highest peach: x1=0.187 m, y1= 0.475 m, z1=2.642 m 2. The lowest peach: x2=0.305 m, y2= 0.268 m, z2=0.832 m 3. The left most peach: x3=0.267 m, y3= 0.758 m, z3=1.583 m PAGE 59 59 4. The right most peach: x4=0.393 m, y4= 0.697 m, z4=1.881 m 5. The front most peach: x5=0.684 m, y5= 0.099 m, z5= 2.077 m T he character parameters of the fruit picking arm computed in the output panel are: Arm length a=0.869 m Base height b=1.737 m Base distance d=1.298 m Therefore, the fruit pic king arm designed for this citrus tree should be located at 1.298 m away from the tree root with the base lifted to 1. 737 m high. The lengths of two main links are both need to be 0.869 m. Figure 511 shows t he outcome of this test case. The produced kinematics image indicates that the rotations of the three joints are quite smooth if we ignore the step during time period from 7 to 10 s. The dynamics image in this citrus case is similar to the one of peach tre e. The torque variation pattern is close to the peach case, but the absolute values of the joint torque are comparatively smaller. It is because the arms have shorter length in this case than in the first case. Joint 1 has still larger torques than that of joint 2 and 3, whose maximum value is around 200 Nm. Result and C onclusion For the two test cases of real fruit trees, the GUI works and the picking process can be analyzed. To make things more general, a sketch of a typical tree with four key fruits is shown in Figure 512. The tree is supposed to be in a standard sphere shape and t he four fruits are supposed to be c entrosymmetric and uniform distributed. Several fruit trees with different canopy diameters are tested by the GUI, and then the required arm lengths and joint torques for picking the sample fruits are obtained. T he results of these test cases can be concluded in Table 5 1. The joint torques are increasing when the arm length become longer. The torque values for joint 1 to pick the highest PAGE 60 60 and the lowest fruit are zeroes because the fruits are assumed to be in the vertical center line of the tree so that joint 1 has no rotations. The torque values for joint 2 are larger than those of joint 3 due to gravity forces. For each arm case, the maximum torque for joint 1 is from the picking process of the left or right most fruit. While the maximum torque for joint 3 is from the picking of the lowest fruit. The torques of joint 2 for picking the different fruits are close to each other, but when the arm length is larger than 0.7 m, it happens to the highest fruit. The italic values in Table 5 1 are marked as the maximum torques for each joint in each arm length case. Figure 513 shows the variation of joint maximum torques with respect to the arm length. With the arm length increasing from 0.5 to 2 m, the joint torques raises smoothly in a curve. Nowadays, in wellplanned orchards or greenhouses fruit plants are cultivated in lines for easy managemen t and harvesting. With the g rowth of fruit trees, it is easy for them to form hedges which make the robotic harvesting become more re alizable. Figure 5 14 is an example of a hedge. The fruits in the hedge tend to grow into a vertical surface plane, which is especially convenient for robots to detect and har vest the fruit. Under this situation, the robot vehicle can just slowly move parallel alone the hedge, letting the harvesting arm pick the nearest fruit continuously. In extreme cases, the fruit picking arm just needs to pick the fruit with in a narrow vertical strip in the hedge. For hedge fruit harvesting, therefore, it is more important to study arm joints with horizontal axes than those with vertical ax e s For the a rticulated arm pre sented in this research (Figure 52), we need to consider more on joints 2 and 3 than joint 1. For instance, a 3m high hedge has mature fruit with different heights from ground to top. An arm for this hedge can be created by the Fruit Picking Arm Designer GUI. The length of the arm is 1.44 m with its base 2.018 m away from the hedge and 1.5 m lifted from the ground. Table 5 2 and Figure 515 PAGE 61 61 shows the torques of joints 2 and 3 for this arm to pick the fruit with different height. The maximum torque of joint 2 happens to pick the highest fruit, while the maximum torque of joint 3 happens to pick the lowest one. This result agrees with the study of typical fruit tree picking. PAGE 62 62 Figure 51. Fruit harvesting robot concept (Kubota, 1993) Figure 52. Artic ulated arm concept Figure 53. The suspension model for rotational inertia measurement (Armstrong, 1986) PAGE 63 63 Figure 54. Fruit picking arm designer GUI Figure 55. Typical fruit position with respect to the tree coordinate frame PAGE 64 64 Figure 56. Fully extended arm pose Figure 57. Flow chart of the Fruit Picking Arm Designer GUI. PAGE 65 65 Figure 58. A typical mature peach tree at University of Florida Jiaying Zhang. May 6, 2013. Gainesville, FL. Figure 59. GUI t est results of the peach tree. PAGE 66 66 Figure 510. A typica l citrus t ree at University of Florida Jiaying Zhang. May 6, 2013. Gainesville, FL. Figure 511. GUI test results of the citr us tree. PAGE 67 67 Figure 512. A typical fruit tree sketch Figure 513. Joint maximum torques for different arm cases PAGE 68 68 Figure 514. A hedge with fruits Source: Reprinted with permission from E dible landscape design, http://www.edible landscape design.com/privacy hedges.html (Feb 2014) Figure 515. Joint torque variation for hedge harvesting. PAGE 69 69 Table 5 1. Test results for typical fruit trees Arm Length a (m) Base Distance d (m) Joint Torque (Nm) High est Fruit Lowest Fruit Side Most Fruit Joint 1 Joint 2 Joint 3 Joint 1 Joint 2 Joint 3 Joint 1 Joint 2 Joint 3 2.020 3.426 0 1214 188 0 1160 374 379 1132 302 1.920 3.157 0 1032 149 0 1002 338 312 1003 235 1.820 2.789 0 876 110 0 829 299 252 861 227 1.630 2.350 0 667 73 0 606 229 175 663 171 1.440 2.018 0 509 51 0 441 171 118 506 127 1.248 1.682 0 379 33 0 305 123 76 372 91 1.056 1.346 0 267 19 0 196 84 45 258 61 0.864 1.211 0 183 18 0 142 54 26 182 42 0.672 1.076 0 114 17 0 101 32 14 121 28 0.480 0.739 0 58 8 0 47 16 6 60 14 Table 5 2. Joint torques for hedge harvesting. Fruit Height(m) Joint 2 (Nm) Joint 3(Nm) 3 509 51 2.5 504 58 2 465 76 1.5 427 101 1 414 128 0.5 415 153 0 441 171 PAGE 70 70 CHAPTER 6 SUMMARY AND DISCUSSION Summary This thesis studies the manipulator of robotics fruit harvesting. After comparing some previous fruit picking manipulators, a Puma 560 robot is selected as a representative fruit picking arm for analysis. Puma 560 has 6 degrees of freedom with 6 revolut e joints, and its arm structure covers the characters from many previous fruit picking robots. The two aspects to analyze a manipulator are kinematics and dynamics. Kinematics emphasizes on end position with regard to joint angles, while the dynamics is a study of motion with regard to forces or torques It is convenient to these analyses by using the Matlab Robotics Toolbox. In C ha pter 3 both forwar d kinematics and inverse kinematics are established to analyze the performance of Puma 560 to pick fruits. The four parameters of Puma 560 are specified by applying D H notation. Some programming is done by using the Toolbox, and the outcomes are clear to predict the performance. 3D figures are plotted to simulate the geometric trajectory of the endeffector from ready pose to reaching the fruits For inverse kinematics, the joint angles are solved if given the target fruit position. While f or forward kinem atics, the reaching position is verify by the given joint angles. In Chapter 4, forward dynamics and inverse dynamics are studied for the actuator torques of Puma560 during the fruit picking process. The Toolbox uses NewtonEuler Recursive technique to solve dynamics problems. The dynamic parameters of Puma 560 refer to previous studies. One outcome of the programming function is the variation of joint velocities and accelerations during a picking trip. A few figures are plotted to show the joint torq ues information under different situations, such as nonfriction motor case or only considering gravity forces. PAGE 71 71 The outcomes of the kinematics and dynamics are helpful if using Puma 560 to pick fruits, but this robot will not be applicable to every fruit tree cases. For different fruit trees, fruit picking robot designer might need different arms that depend on the fruit distribution. If future researchers want to build fruit picking arms that have similar link structure to Puma robot, they will probably need the GUI created in Chapter 5. The fruit picking arm designer GUI generates an arm that satisfies the users requirements on the objective fruit tree. It allows the user to input five typical fruit positions with respect to the tree coordinate frame. T hese five positions are dominant in the arms workspace, and are used to compute the arm length. The user will get three parameters of the generated arm, which identify the character of the arm they want. Kinematics and dynamics for the arm to pick the fiv e given fruits are also included in the GUI. The user will have a clear view on the performance of the arm picking the required fruit s Two test cases are illustrated, and they have proved that the GUI is helpful for tree fruit arm design Discussion The robotic fruit harvestings are complex machines with mechanical and cont rol systems. The manipulator is only a small part of the machine, so more studies regarding the cooperation between fruit picking arms, endeffectors, detecting sensors and the control systems will need future efforts. The main problem on selective robotics fruit harvesting is how to improve the picking effectiveness. The research in this thesis is based on many assumptions that need to be proved. The Puma 560 is assumed to be able to a fruit harvesting, and reliable fruit detecting sensor s can provide true fruit position information. The endeffectors are assumed to grasp and detach the fruit successfully so that the arm wont waste time on duplicated motion trips. The kinematic and PAGE 72 72 dynamic analysis for every picking process is from a specified reach pose, but in some cases, returning to initial pose is not necessary. Therefore, more detailed different fruit picking cases will need to be studied in future. Comparing to kinematics, dynam ics is much more complex and needs more data. The dynamics study in this research is based on previous puma robot studies by other researchers. The reliability of the dynamics parameters such as link mass and center of gravity are supposed to be correct. M oreover, in the fruit picking arm designer GUI, an arm is generated to pick the given fruit. Dynamic analysis is available for the new arm, but the dynamics parameters are assuming to be proportional to puma 560. It is difficult to measure the moments of i nertia for every new arm due to limit experiment conditions. If the assumption cannot be proved to be valid, more experiment to measure the moments of inertial is necessary for future work. Motor internal factors are also important in dynamics computation. As the researchers often have different motor choices when they are designing a new manipulator, the motor internal frictions are not considered in this research. Even if a sample robotic arm is computed through the GUI, more detailed design works needs to be operated. The arm generated by GUI gives the designer a general concept and very basic level information of a fruit picking arm, so there still exist much room to improve the arm. Finally, Matlab Robotics Toolbox is one of the many tool s for analyzing robot manipulators. If some work from other analysis tools can be used to make a comparison, it will be a good way to check the results in this research. PAGE 73 73 LIST OF REFERENCES Armstrong, B. Khatib, O. & Burdick, J. (1986). The explicit dynamic model and inertial parameters of the Puma 560 arm Proc. IEEE Int. Conf. Robotics and Automation, Washingt on, USA (1) 51018. Benitez, A, Huitzil1, I., Casiano, A., Calleja, J. D. L. & Medina M. A. (2012). PUMA 560: Robot Prototype w ith Graphic Simulation Environment Advances in Mechanical Engineering 2, 1, 1522. Burks, T., Villegas, F., Hannan, M., Flood, S., Sivaraman, B., Subramanian, V. & Sikes, J. (2005) HorTechnology 15 (1), 7987. C rane C. D III and D uffy J. ( 1998). Kinematic analysis of robot manipulators ISBN 978 0521570633, Cambridge University Press Corke, P. I. ( 1996) A robotics toolbox for MATLAB. IEEE Robotics and Automation Magazine 3: 2432. Corke P. I. & Armstrong B ( 1994) A search for consensus among model parameters reported for the PUMA 560 robot Proc. IEEE Int. Conf. Robotics and Automation. Craig, J. J. ( 1989) Introduction to Robotics: Mechanics and Control. 2nd ed. Reading, MA.: AddisonWesley. E dible landscape design. Retrie ved Feb 2014. from http://www.edible landscapedesign.com/privacy hedges.html F lood, S. J. (2006). Design of a Robotic Citrus Harvesting End Effector and Force Control Model Using Physical Properties and Harvesting Motion Tests. Dissertation for the degree of doctor of philosophy at University of Florida. Grand DEsnon, A. (1985). Robotic harvesting of apples. Proc. Agri Mation, 1, 210 214. Grand DEsnon, A., Rabatel, G., Pellenc, R., Journe au A. and Aldon, M. J. ( 1987) MAGALI: A self propelled robot to pick apples. ASAE Paper No. 871037. St. Joseph, MI. Hannan, M. W. Burks, T. F. & Bulanon, D. M. (2009) A Machine Vision Algorithm for Orange Fruit Detection. Agricultural Engineering International: the CIGR Ejournal. Manuscript 1281. Vol. XI Harrell, R. (1987). Economic Analysis of Robtic Citrus Harvesting in Florida. American Society of Agricultural Engineers 30(2), 298 304. Hartenberg R. S. and Denavit, J. (1955). A kinematic notation for lower pair mechanisms based on matrices Journal of Applied Mechanics, 77, 215221. PAGE 74 74 Hollerbach J. (1980). A recursive Lagrangian formulation of manipulator dynamics and a comparative study of dynamics formulation complexi ty I EEE Trans. Syst. Man Cybern., vol. SMC 10, 730736. Hollerbach, J. (1982) Dynamics. Robot Motion Planning and Control (M. Brady, J. M. Hollerbach, T. L. Johnson, T. LozanoPer ez, and M. T. Mason, eds.), 51 71 Kurfess, T. R. (2005). Robotics and Automation Handbook CRC Press LLC. ISBN 0 849318041 (alk. paper) Kane, T. and Levinson, D. (1983). The use of Kanes dynamical equations in robotics. Int. J.Robot. Res 2: 321. Kondo, N., Monta, M. and Fujiura. T. (1996). Basic constitution of a robot for agricultural use. Advanced Robotics 10(4): 339353. Kondo, N., Monta, M. & Noguchi, N. (2006). Agricultural Robots Mechanisms and Practice Kyoto University Press. Lee, C. S. G., Lee, B., and Nigham, R. (1983) Development of the generalized DAlembert equations of motion for mechanical manipulators Proc. 22nd CDC (San Antonio, Texas), 12051210. Luh, J. Y. S., Walker, M. W. & Paul, R. P. C. (1980). Online computational scheme for mechanical manipulators ASME Journal of Dynamic Systems, Measurement and Control 102: 6976. Nayar, H. D. (2002). Robotect: serial link manipulator design software for modeling, visualization and performance analysis. Proc. 7th Intl. Conf. Control, Automation, Robotics and Vision, 3: 13591364. Piscataway, NJ: IEEE. Oxbo C itrus Harvesters Retrieved Dec 2013 from http://www.oxbocorp.com/Products/Citrus.aspx Paul, R. P. (1981). Robot Manipulators: Mathematics, Programming, and Control Cambridge, Massachusetts: MIT Press, 1981. Paul R and Zhang, H. (1986). Computationally efficient kinematics for manipulators with spherical wrists based on the homogeneous transformation representation The International Journal of Robotics Reaserch. 5, 2, 3244. Sarig, Y. (1993) Robotics of Fruit Harvesting: A State of the art Review. J. Agric. Eng.Research 54: 265280. Sivaraman, B. (2006). Design and Development of a Robot Manipulator for Citrus Harvesting. Dissertation for the degree of doctor of philosophy at University of F lorida. Unimate Puma Series 500 Industrial Robot (1084), Unimation Incorporated. PAGE 75 75 BIOGRAPHICAL SKETCH Jiaying Zhang was born in 1987, and was born and grew up in Shanghai, China. She was enrolled into Shanghai Jiaotong Universty (SJTU) in September 2006, and received a Bachelor of Science in Engineering degree with an industrial engineering major in Jul 2010. She took a job immediately after graduation from SJTU, and worked as a mechanical engineer in Si e mens Lt d., China until 2012. She came to the United States of America for graduate education in 2012 fall. She was admitted by the Department of Mechanical and Aerospace Engineering and anticipates obtaining a Master of Science degree in May 2014. 