(翻譯)黃瓜機(jī)器人的運(yùn)動(dòng)規(guī)劃.pdf

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 Department of Greenhouse Engineering, Institute of Agricultural and Environmental Engineering (IMAG B.V.), P.O. Box 43, NL-6700 AA Wageningen, The Netherlands; e-mail of corresponding author: eldert.vanhentenwur.nl (Received 26 April 2002; accepted in revised form 8 July 2003; published online 29 August 2003) One of the most challenging aspects of the development, at the Institute of Agricultural and Environmental Engineering (IMAG B.V.), of an automatic harvesting machine for cucumbers was to achieve a fast and accurate eyehand co-ordination during the picking operation. This paper presents a procedure developed for the cucumber harvesting robot to pursue this objective. The procedure contains two main components. First of all acquisition of sensory information about the working environment of the robot and, secondly, a program to generate collision-free manipulator motions to direct the end-effector to and from the cucumber. This paper elaborates on the latter. Collision-free manipulator motions were generated with a so-called path search algorithm. In this research the A *-search algorithm was used. With some numerical examples the search procedure is illustrated and analysed in view of application to cucumber harvesting. It is concluded that collision-free motions can 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 solution or stops when a solution cannot be found. This favourable property, however, makes the algorithm prohibitively slow. The results showed that the algorithm does not include much intelligence in the search procedure. It is concluded that to meet the required 10s for a single harvest cycle, further research is needed to fi nd fast algorithms that produce solutions using as much information about the particular structure of the problem as possible and give a clear message if such a solution can not be found. # 2003 Silsoe Research Institute. All rights reserved Published by Elsevier Ltd 1. Introduction In 1996, the Institute of Agricultural and Environ- mental Engineering (IMAG B.V.) began research on the development of an autonomous cucumber harvesting robot supported by the Dutch Ministry of Agriculture, Food and Fishery (Gieling et al., 1996; Van Kollenburg- Crisan et al., 1997). The task of designing robots for agricultural applications raises issues not encountered in 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 damage and should be handled with care. The robot has to operate under adverse climatic conditions, such as high relative humidity and temperature as well as changing light conditions. Finally, to be cost effective, the robot needs to meet high performance characteristics 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- ment of an automatic harvesting machine is to achieve a fast and accurate eyehand co-ordination, i.e. to achieve an effective interplay between sensory information acquisition and robot motion control during the picking 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- mance characteristics. A task analysis revealed that, for economic feasibility, a single harvest operation may take ARTICLE IN PRESS 1537-5110/$30.00135# 2003 Silsoe Research Institute. All rights reserved Published by Elsevier Ltd up to 10s (Bontsema et al., 1999). Still, robot motions should be as fast as possible while preventing collisions of the manipulator, end-effector and harvested fruit with the crop, the greenhouse construction and the robot itself (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 imposed on the travelling speed and accelerations of the manipulator during various portions of the motion path. 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, the sensory system is based on computer vision, as reported by Meuleman et al. (2000). This paper focuses on the fast generation of collision-free motion trajectories for the manipulator of the picking machine. This issue has not received much attention in agricultur- al engineering research despite the fact that considerable research effort has been spent on automatic harvesting of vegetable fruits (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 220V power line; (h) pneumatic pump; (i) heating pipe Notation ctransition costs between two nodes fcost function gcost of the motion path from node S to the current node Ggoal node hoptimistic estimate of the cost to go to the goal from the current node nnode n0successor of node Sstart node E.J. VAN HENTEN ET AL.136 decided to pick the cucumber, the low-resolution images of the vehicle-mounted camera are used for positioning the end-effector in the neighbourhood of the cucumber. Once the end-effector has arrived in the neighbourhood of the cucumber, then, using the laser ranging system or camera system mounted on top of the end-effector, high- resolution information of the local environment of the cucumber is obtained for the fi nal accurate approach of the cucumber. The end-effector grips and cuts the stalk of the fruit. The gripper fi xes the detached fruit and fi nally the harvested fruit is moved to the storage crate. Obstacle avoidance motion planning will be used both for the initial approach of the cucumber as well as the return journey of the harvested cucumber to the crate, to assure that other objects in the work space such as the robot vehicle itself but also stems and, if present, leaves and parts of the greenhouse construction are not hit. Clearly, the harvested cucumber increases the size of the end-effector, which should be considered during the return motion of the manipulator to the storage. The average length of a cucumber is 300mm. 4. A collision-free motion planning algorithm Figure 3 illustrates the components of a program that 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)informationaboutthephysical structure of the robot as well as the workspace in which the robot has to operate. So, the fi rst step in collision- free robot motion planning is the 3D world description acquisition.Thisdescriptionisbasedonsensory information such as machine vision as well as a priori knowledgeabout,forinstance,the3Dkinematic structure of the harvesting robot, e.g. 3D models, contained in a database. With this information, during the task defi nition phase, the overall task of the robot is planned. It is decided which fi nal position and orienta- tion of the end-effector result in the best approach of the cucumber. Also specifi c position and orientation con- straints are defi ned during this phase. In the inverse kinematics phase the goal position and orientation of the end-effector, defi ned during task defi nition, are trans- lated into the goal confi guration of the manipulator. The goal confi guration is represented as a combination of a translation of the linear slide and six rotations of the joints of the seven-DOF manipulator. This informa- tion is used by the path planner. The path planner employs a search technique to fi nd a collision-free path from the start confi guration of the manipulator to its goal confi guration. Once the collision-free path planning 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 CUCUMBER PICKING ROBOT137 converts the collision-free path into a trajectory that can be executed by the manipulator. Typically, the path planning process is concerned only with collision-free confi gurations in space, but not with velocity, accelera- tion and smoothness of motion. The trajectory planner deals with these factors. The trajectory planner produces the motion commands for the servomechanisms of the robot.Thesecommandsareexecutedduringthe implementation phase. Some components of the motion planning system will 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 fi nally, using stereovision techni- ques, the camera vision system produces 3D maps of the workspace within the viewing angle of the camera. In this way the robot is able to deal with the variability in the working environment with which it is confronted. As stated above, also a priori knowledge about, for instance, the physical structure of the robot is needed for collision-free motion planning. As an example, Fig. 4 shows a 3D model of the six-DOF Mitsubishi RV-E2 manipulator implemented in MATLAB1. The 3D structure of the manipulator is represented by polygons constructed of rectangles and triangles. The model is used for evaluation of motion strategies during simula- tions and serves as a basis for the detection of collisions of the manipulator with structural components in the workspace of the robot during motion planning. 4.2. Inverse kinematics The inverse manipulator kinematics deal with the computation of the set of joint angles and translations that result in the desired position and orientation of the tool-centre-point (TCP) of the manipulator (Craig, 1989). The TCP is a pre-defi ned point on the end- effector. For the six-DOF Mitsubishi RV-E2 manip- ulator Van Dijk (1999) obtained an analytic solution of the inverse manipulator kinematics. For the seven-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 theinherentredundancyinthekinematicchain. 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. VAN HENTEN ET AL.138 inverse kinematics of this redundant manipulator was obtained (Schenk, 2000). Given the position of the ripe cucumber,thealgorithmproducesacollision-free harvest confi guration of the seven-DOF manipulator. Also, it assures that the joints have suffi cient freedom for fi ne-motioncontrolintheneighbourhoodofthe cucumber. 4.3. Path planner Algorithms for collision-free path planning have been 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 the search, the feasibility of each step in the search space 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. It is important to note that for most path planners the search space is the so-called confi guration space of the robot, which crucially differs from the 3D workspace of the robot. In case of the seven-DOF manipulator of the cucumber harvest machine, the confi guration space is a seven-dimensional space spanned by the combination of 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 pointthroughtheseven-dimensional confi guration space from the start confi guration to the goal confi g- uration. In this way problems with redundancy in the kinematic chain are easily circumvented. There is a one- to-one mapping of a point in the confi guration space to a position and orientation of the tool-centre-point in the workspace.However,formostmanipulators,the opposite does not hold. Then a single position and orientation of the tool-centre-point in the workspace can be reproduced by several confi gurations of the manip- ulator. Due to its unique representation a search in the confi guration space is preferred. For collision detection, however, one needs to describe the physical posture of the manipulator in relation with other objects in the 3D workspace. Because every confi guration represents a single posture of the manipulator in the 3D workspace, collisionscanbeeasily verifi ed.Then,giventhe particular kinematic structure of the robot, the work- space obstacles can be mapped into confi guration space obstacles as will be shown later. 4.3.1. The search algorithm A path search algorithm should be effi cient and fi nd a solution if one exists. The latter property is referred to as completeness (Pearl, 1984). Usually, algorithms that guarantee completeness are not computationally effi - cient. Computational effi ciency, 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 effi ciency. The main reason for that choice is that a complete algorithm will either fi nd the solution or stop using a clearly defi ned 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. In this research the so-called A *-search algorithm was used (Pearl, 1984; Kondo, 1991; Russell and an optimistic estimate of the cost from the current 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 confi guration space: S; start node; G; goal node CUCUMBER PICKING ROBOT139 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 confi guration coincide with grid nodes or that grid nodes in the close neighbourhood of these confi gurations can be selected. Then according to Pearl (1984), the A * algorithm manipulates the nodes in the grid as follows. (1) Put the start node S on OPEN. (2) If OPEN is empty then exit with failure, else remove from OPEN and place on CLOSED a node n for which f is a minimum. (3) If n is equal to the goal node G; exit successfully with 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 n0of n: (a) if n0is not already on OPEN or CLOSED, estimate hn0 (an optimistic estimate of the cost of the best path from n0to goal node G), and calculate fn0 gn0 hn0 where gn0 g? n cn;n0 with cn;n0 being the transition cost from node n to node n0and gS 0; (b) if n0is already on OPEN or CLOSED, direct its pointers along the path yielding the lowes