基于視覺的移動機器人設計與分析【基于視覺導航的履帶式移動機器人】【足球機器人】,基于視覺導航的履帶式移動機器人,足球機器人,基于視覺的移動機器人設計與分析【基于視覺導航的履帶式移動機器人】【足球機器人】,基于,視覺,移動,挪動,機器人,設計,分析,導航,履帶式,足球 Biosystems Engineering (2003) 86(2), 135144 doi:10.1016/S1537-5110(03)00133-8 Available online at AEAutomation and Emerging Technologies Collision-free Motion Planning for a Cucumber Picking Robot E.J. Van Henten; J. Hemming; B.A.J. Van Tuijl; J.G. Kornet; J. Bontsema DepartmentofGreenhouseEngineering,InstituteofAgricultural andEnvironmental Engineering(IMAGB.V.),P.O. Box43,NL-6700AA Wageningen,TheNetherlands; e-mailofcorrespondingauthor:eldert.vanhentenwur.nl (Received 26 April 2002; accepted in revised form 8 July 2003; published online 29 August 2003) One of the most challenging aspectsof the development, at the Institute ofAgricultural and Environmental Engineering (IMAG B.V.), of an automatic harvesting machine for cucumbers was to achieve a fast and accurateeyehandco-ordinationduringthepickingoperation.Thispaperpresentsaproceduredevelopedfor thecucumberharvestingrobottopursuethisobjective.Theprocedurecontainstwomaincomponents.First of all acquisition of sensory information about the working environment of the robot and, secondly, a programtogeneratecollision-freemanipulatormotionstodirecttheend-effectortoandfromthecucumber. Thispaperelaboratesonthelatter.Collision-freemanipulatormotionsweregeneratedwithaso-calledpath searchalgorithm.InthisresearchtheA * -searchalgorithmwasused.Withsomenumericalexamplesthesearch procedure is illustrated and analysed in view of application to cucumber harvesting. It is concluded that collision-free motionscan be calculated for the seven degrees-of-freedom manipulator used in the cucumber picking device. The A * -search algorithm is easy to implement and robust. This algorithm either produces a solutionorstopswhenasolutioncannotbefound.Thisfavourableproperty,however,makesthealgorithm prohibitively slow. The results showed that the algorithm does not include much intelligence in the search procedure.Itisconcludedthattomeettherequired10sforasingleharvestcycle,furtherresearchisneededto nd fast algorithms that produce solutions using as much information about the particular structure of the problem as possible and give a clear message ifsucha solution can not be found. #2003SilsoeResearchInstitute.All rightsreserved Published byElsevierLtd 1. Introduction In 1996, the Institute of Agricultural and Environ- mentalEngineering(IMAGB.V.)beganresearchonthe development of an autonomous cucumber harvesting robot supported by the Dutch Ministry of Agriculture, FoodandFishery(Gieling et al.,1996;VanKollenburg- Crisan et al., 1997). The task of designing robots for agriculturalapplicationsraisesissuesnotencounteredin other industries. The robot has to operate in a highly unstructured environment in which no two scenes are the same. Both crop and fruit are prone to mechanical damageandshouldbehandledwithcare.Therobothas to operate under adverse climatic conditions, such as high relative humidity and temperature as well as changing light conditions. Finally, to be cost effective, therobotneedstomeethighperformancecharacteristics in terms of speed and success rate of the picking operation. In this project, these challenging issues have been tackled by an interdisciplinary approach in which mechanical engineering, sensor technology (computer vision), systems and control engineering, electronics, software engineering, logistics, and, last but not least, horticultural engineering partake (Van Kollenburg- Crisan et al., 1997; Bontsema et al., 1999; Meuleman et al., 2000). One of the most challenging aspects of the develop- mentofanautomaticharvestingmachineistoachievea fastandaccurateeyehandco-ordination, i.e. toachieve an effective interplay between sensory information acquisitionandrobotmotioncontrolduringthepicking operation, just like people do. In horticultural practice, a trained worker needs only 36s to pick and store a single fruit and that performance is hard to beat. Fortunately, in terms of picking speed a robotic harvester does not have to achieve such high perfor- mancecharacteristics.Ataskanalysisrevealedthat,for economicfeasibility,asingleharvestoperationmaytake ARTICLE IN PRESS 1537-5110/$30.00 135 #2003SilsoeResearch Institute.All rightsreserved PublishedbyElsevierLtd up to 10s (Bontsema et al., 1999). Still, robot motions should be as fast as possible while preventing collisions ofthemanipulator,end-effectorandharvestedfruitwith thecrop,thegreenhouseconstructionandtherobotitself (such as the vehicle and vision system). In a Dutch cucumber production facility, the robot operates in a very tight working environment. Finally, to assure the quality of the harvested fruit, constraints have to be imposedonthetravellingspeedandaccelerationsofthe manipulatorduringvariousportionsofthemotionpath. To achieve the desired eyehand co-ordination, one needs (real-time) acquisition of sensory information of the environment as well as algorithms to calculate collision-free motions for the manipulator. In this project,thesensorysystemisbasedoncomputervision, as reported by Meuleman et al. (2000). This paper focuses on the fast generation of collision-free motion trajectoriesforthemanipulatorofthepickingmachine. Thisissuehasnotreceivedmuchattentioninagricultur- alengineeringresearchdespitethefactthatconsiderable research effort has been spent on automatic harvesting ofvegetablefruits(see e.g. Kondo et al.,1996;Hayashi Arima (a) vehicle; (b) wide angle camera; (c) seven-degrees-of-freedom manipulator; (d) end-effector; (e) laser ranger and position of camera for local imaging; (f) computer and electronics; (g) reel with 220 V power line; (h) pneumatic pump; (i) heating pipe Notation c transitioncosts between two nodes f cost function g cost of the motion path from node S to the current node G goal node h optimistic estimate of the cost to go to the goal from the current node n node n 0 successorof node S start node E.J.VANHENTEN ET AL.136 decidedtopickthecucumber,thelow-resolutionimages ofthe vehicle-mounted camera areusedfor positioning theend-effectorintheneighbourhoodofthecucumber. Oncetheend-effectorhasarrivedintheneighbourhood ofthecucumber,then,usingthelaserrangingsystemor camerasystemmountedontopoftheend-effector,high- resolution information of the local environment of the cucumberisobtainedforthenalaccurateapproachof the cucumber. The end-effector grips and cuts the stalk of the fruit. The gripper xes the detached fruit and nallytheharvestedfruitismovedtothestoragecrate. Obstacleavoidancemotionplanningwillbeusedboth for the initial approach of the cucumber as well as the returnjourneyoftheharvestedcucumbertothecrate,to assure that other objects in the work space such as the robotvehicleitselfbutalsostemsand,ifpresent,leaves and parts of the greenhouse construction are not hit. Clearly, the harvested cucumber increases the size of theend-effector,whichshouldbeconsideredduringthe return motion of the manipulator to the storage. The average length ofa cucumber is 300mm. 4. A collision-free motion planning algorithm Figure 3 illustratesthecomponentsofaprogramthat automatically generates collision-free motions for the cucumber picking robot based on the work of Herman (1986). Collision-free motion planning relies on three- dimensional (3D) information about the physical structureoftherobotaswellastheworkspaceinwhich the robot has to operate. So, the rst step in collision- free robot motion planning is the 3D world description acquisition. This description is based on sensory information such as machine vision as well as a priori knowledge about, for instance, the 3D kinematic structure of the harvesting robot, e.g. 3D models, contained in a database. With this information, during the task definition phase,theoveralltaskoftherobotis planned. It is decided which nal position and orienta- tionoftheend-effectorresultinthebestapproachofthe cucumber. Also specic position and orientation con- straints are dened during this phase. In the inverse kinematics phasethegoalpositionandorientationofthe end-effector, dened during task definition, are trans- lated into the goal conguration of the manipulator. The goal conguration is represented as a combination of a translation of the linear slide and six rotations of thejointsoftheseven-DOFmanipulator.Thisinforma- tion is used by the path planner.Thepath planner employsasearch technique tond acollision-free path from the start conguration of the manipulator to its goalconguration.Oncethecollision-freepathplanning has been completed successfully, the trajectory planner ARTICLE IN PRESS Initialise all systems Move vehicle delta x further on the rail Take stereo images Image processing (fruit detection) Camera at end position? Path planning Move TCP to cutting point Grip fruit and cut stalk Move with fruit to the crate and release it Move TCP to home position Harvest completed Move vehicle to home position yes Move global camera to 1st position Move global camera one position further no yes no yes yes no yes no 3D localisation no Move TCP close to cutting point Mini camera stereo images Image processing (high res. 3D localisation) Fruit(s) detected? Vehicle at end of rail? Other fruit in global image? Fruit mature? Fig. 2. Task sequence of a single harvest operation: 3D, three dimensional; TCP, tool-centre-point CUCUMBERPICKINGROBOT 137 convertsthecollision-freepathintoatrajectorythatcan be executed by the manipulator. Typically, the path planning process is concerned only with collision-free congurations in space, but not with velocity, accelera- tion and smoothness of motion. The trajectory planner dealswiththesefactors.The trajectory planner produces the motion commands for the servomechanisms of the robot. These commands are executed during the implementation phase. Somecomponentsofthemotionplanningsystemwill be described hereafter in more detail. 4.1. World description (acquisition) The machine vision based world description acquisi- tion for the cucumber picking robot is described in a paper by Meuleman et al. (2000). The vision system is able to detect green cucumbers within a green canopy. Moreover, the vision system determines the ripeness of the cucumber. And nally, using stereovision techni- ques,thecameravisionsystemproduces3Dmapsofthe workspace within the viewing angle of the camera. In thiswaytherobotisabletodealwiththevariabilityin the working environment with which itis confronted. As stated above, also a priori knowledge about, for instance,thephysicalstructureoftherobotisneededfor collision-free motion planning. As an example, Fig. 4 shows a 3D model of the six-DOF Mitsubishi RV-E2 manipulator implemented in MATLAB 1 . The 3D structureofthemanipulatorisrepresentedbypolygons constructed of rectangles and triangles. The model is used for evaluation of motion strategies during simula- tionsandservesasabasisforthedetectionofcollisions of the manipulator with structural components in the workspace ofthe robot during motion planning. 4.2. Inverse kinematics The inverse manipulator kinematics deal with the computation of the set of joint angles and translations thatresultinthedesiredpositionandorientationofthe tool-centre-point (TCP) of the manipulator (Craig, 1989). The TCP is a pre-dened point on the end- effector. For the six-DOF Mitsubishi RV-E2 manip- ulatorVanDijk(1999)obtainedananalyticsolutionof theinversemanipulatorkinematics.Fortheseven-DOF manipulator, i.e. the Mitsubishi RV-E2 manipulator mounted on a linear slide, a straightforward analytic solution of the inverse kinematics does not exist due to the inherent redundancy in the kinematic chain. Recently a mixed analytic-numerical solution of the ARTICLE IN PRESS 1000 800 600 400 200 0 500 500 500 500 0 0 X plane, mm Y plane, mm Z plane, mm Fig. 4. A three-dimensional model of the six-degrees-of-freedom Mitsubishi RV-E2 manipulator Start World description acquisition Inverse kinematics Path planning Trajectory planning Implementation End Data baseTask definition Fig. 3. A program for automatic generation of collision-free motions E.J.VANHENTEN ET AL.138 inverse kinematics of this redundant manipulator was obtained (Schenk, 2000). Given the position of the ripe cucumber, the algorithm produces a collision-free harvest conguration of the seven-DOF manipulator. Also,itassuresthatthejointshavesufcientfreedomfor ne-motion control in the neighbourhood of the cucumber. 4.3. Path planner Algorithmsforcollision-freepathplanninghavebeen the object of much research. See e.g. Latombe (1991) and Hwang and Ahuja (1992) for an overview. A collision-free path planner essentially consists of two important components: a search algorithm and a collision detection algorithm. The search algorithm explores the search space for a feasible, i.e. collision- free, motion from a start point to a goal point. During thesearch,thefeasibilityofeachstepinthesearchspace is checked by the collision detection algorithm. This algorithm checks for collisions of the manipulator with other structural components in the workspace of the robot. Itisimportanttonotethatformostpathplannersthe search space is the so-called conguration space of the robot,whichcruciallydiffersfromthe3Dworkspaceof therobot.Incaseoftheseven-DOFmanipulatorofthe cucumber harvest machine, the congurationspaceisa seven-dimensionalspacespannedbythecombinationof one joint translation and six joint rotations. Then, the search for a collision-free motion from a start position and orientation of the tool-centre-point to the goal position and orientation in the 3D workspace boils down to a search for a collision-free motion of a single point through the seven-dimensional conguration space from the start conguration to the goal cong- uration. In this way problems with redundancy in the kinematicchainareeasilycircumvented.Thereisaone- to-onemappingofapointinthecongurationspaceto apositionandorientationofthetool-centre-pointinthe workspace. However, for most manipulators, the opposite does not hold. Then a single position and orientationofthetool-centre-pointintheworkspacecan be reproduced by several congurations of the manip- ulator. Due to its unique representation a search in the congurationspaceispreferred.Forcollisiondetection, however, one needs to describe the physical posture of themanipulatorinrelationwithotherobjectsinthe3D workspace. Because every conguration represents a singlepostureofthemanipulatorinthe3Dworkspace, collisions can be easily veried. Then, given the particular kinematic structure of the robot, the work- spaceobstaclescanbemappedintocongurationspace obstacles aswill beshown later. 4.3.1. The search algorithm Apathsearchalgorithmshouldbeefcientandnda solutionifoneexists.Thelatterpropertyisreferredtoas completeness (Pearl, 1984). Usually, algorithms that guarantee completeness are not computationally ef- cient. Computational efciency, however, is crucial when on-line application is required. To obtain insight into the various aspects of motion planning, in this research, completeness of the algorithm was favoured above computational efciency. The main reason for that choice is that a complete algorithm will either nd the solution or stop using a clearly dened stopping criterion if a solution cannot be found. This is not true for algorithms that do not assure completeness. They either supply a solution or get stuck without further notice.Inthisresearchtheso-calledA * -searchalgorithm wasused(Pearl,1984; Kondo, 1991; Russell andanoptimisticestimateofthecostfromthecurrent position to the goal h: In this research, the Euler norm was used as an optimistic estimate of the cost to go to the goal node. The A * algorithm is both complete and optimal. Optimality assures that the path obtained minimises the cost function used. ARTICLE IN PRESS S G Start configuration Goal configuration Translation or rotation of joint 2 Translation or rotation of joint 1 Fig. 5. Orthogonal node expansion in a discretised two-dimen- sional configuration space: S; start node; G; goal node CUCUMBERPICKINGROBOT 139 The A * algorithm uses two lists with grid nodes, the OPEN list and the CLOSED list. The OPEN list contains grid nodes of which the cost function has not yet been evaluated, whereas function values of the grid nodes on the CLOSED list have been evaluated. It is assumed that the start and goal conguration coincide with grid nodes or that grid nodes in the close neighbourhood ofthese congurations canbe selected. Then according to Pearl (1984), the A * algorithm manipulates the nodesin the grid asfollows. (1) Put the start node S on OPEN. (2) IfOPENisemptythenexitwithfailure,elseremove from OPEN and place on CLOSED a node n for which f is a minimum. (3) If n isequaltothegoalnode G; exitsuccessfullywith the solution obtained by tracing back the pointers from n to S: (4) Otherwise expand n; generating all its successors, and attach to them pointers back to n: For every successor n 0 of n: (a) if n 0 is not already on OPEN or CLOSED, estimate hn 0 (anoptimisticestimateofthecost of the best path from n 0 to goal node G), and calculate f n 0 gn 0 hn 0 where gn 0 gC2 ncn; n 0 with cn; n 0 being the transition cost from node n tonode n 0 and gS0; (b) if n 0 isalreadyonOPENorCLOSED,directits pointersalongthepathyieldingthelowest gn 0 ; and (c) if n 0 requiredpointeradjustmentandwasfound on CLOSED, reopen it. (5) Go to step 2. Grid expansion in step 4 can take various forms. In thisresearchaso-calledorthogonalexpansionwasused. This approach is illustrated in Fig. 5. Figure 5 also illustrates that the start and goal nodes do not have to coincide with the actual start and goal conguration of the manipulator. In such cases the nearest neighbouring nodesare selected. Inthisalgorithm,thestoppingcriterionisveryclearly dened. The algorithm stops if at step 3, the node removed from the OPEN list equals the goal node. Alternatively,thealgorithmwillstopatstep2ifallthe gridnodesareevaluatedandtheOPENlisthasbecome empty. Inthat case a solution is not found. Therearetwowaysofdealingwithcollisiondetection during the path search. First of all, the colliding congurations can be identied before the path search by scanning the whole discretised conguration space. Thiswillbecomputationallyexpensiveincaseofahigh- dimensional conguration space discretised with a high resolution grid. It will be more efcient to evaluate the feasibility of the grid nodes during the search process. That is to say, that during the node expansion step, step 4, a collision detection algorithm checks whether therobotcongurationassociatedwiththatnodeyields a collision with the environment or not. Since the A * algorithm usually evaluates only a small part of the conguration space, this will yield a considerable improvement in the efciency. A collision can be penalisedbyaddingalargepenaltytothecostfunction mentioned in step 4a. Alternatively, a grid point resulting in a collision can be omitted directly from the OPEN list during the grid expansion phase. In this research the latter approach was used. 4.3.2. The collision detection algorithm Forcollisiondetectionanalgorithmwasimplemented in MATLAB 1 based on the ideas reported by Boyse (1979). This algorithm evaluates the intersection of surfaces of the robot model with the surfaces of other structuralcomponentsintheworkspace.Calculatingthe intersection of two surfaces essentially boils down to determining the intersection of all the edges of one surface with the other surface which can be achieved usingstandardtoolsfromgeometry.Allinall,collision detectionisacomputationallyintensivetask.Therefore, collisiondetectioninareal-timeapplicationsuchasthe cucumberrobot,requiresatrade-offbetweenthedesired accuracy of the collision detection and the available calculation time. The accurate CAD-model of Fig. 4 contains 600 triangular and rectangular surfaces. A factor15reduction incalculation time wasachievedby replacing the accurate manipulator model by a less accurate model built from so-called oriented bounding boxes (OBB). This 3D OBB-model of the manipulator consistingofonly36movin