Available online at www.sciencedirect.com

ScienceDirect

Agriculture and Agricultural Science Procedia 7 (2015) 27 - 34

Farm Machinery and Processes Management in Sustainable Agriculture, 7th International

Scientific Symposium

Trajectory planning with obstacles on the example

of tomato harvest

Marek Borygaa*, Andrzej Grabosb, Pawel Kolodzieja, Krzysztof Golackia, Zbigniew Stropeka

a University of Life Sciences in Lublin, Department of Mechanical Engineering and Automation,

Gfyboka 28, 20-612 Lublin, Poland b ARKONA Laboratorium Farmakologii Stomatologicznej, Nasutów 99 C, 21-025 Niemce, Poland

Abstract

The paper presents the PR-APT method (Planning Rectilinear-Arc Polynomial Trajectory) for planning a trajectory of the manipulator end-effector. In the described method, a path consists of two rectilinear segments of intersecting directions combined with the blending arc of a set radius. The arc can make a fragment of an avoidable obstacle. he algorithm and the implemented method on the example of tomato harvest in the greenhouse was presented. The developed algorithm facilitates the trajectory planning in only few steps and therefore, it is supremely effective. Notably, the proposed algorithm ensures the continuity of displacement, velocity and tangential acceleration for the planned motion of the end-effector. The numerical example includes the computational results and the courses of displacement, speed and acceleration for the planned trajectory. © 2015 The Authors.Published by Elsevier B.V.This is an open access article under the CC BY-NC-ND license (http://creativecommons.org/licenses/by-nc-nd/4.0/).

Peer-review under responsibility of the Centre wallon de Recherches agronomiques (CRA-W) Keywords: greenhouses; harvest; obstacle avoidance; tomatoes; trajectory planning.

1. Introduction

Trajectory planning is the first and crucial stage of designing operational processes of robot manipulators. This problem has been addressed in a number of research papers available in literature. Gasparetto and Zanotto (2007) developed a novel method for smooth trajectory planning of robot manipulators. They formulated an objective

* Corresponding author. Tel.: +48 81 531 97 65 E-mail address: marek.boryga@up.lublin.pl

2210-7843 © 2015 The Authors. Published by Elsevier B.V. This is an open access article under the CC BY-NC-ND license (http://creativecommons.Org/licenses/by-nc-nd/4.0/).

Peer-review under responsibility of the Centre wallon de Recherches agronomiques (CRA-W) doi:10.1016/j.aaspro.2015.12.026

function containing a term proportional to the total execution time and a term proportional to the integral of the squared jerk and finally, obtained smooth trajectory. With respect to optimization techniques, they assumed minimum move time, minimum energy and minimum jerk. Dyllong and Visioli (2003) utilized various spline techniques to plan and modify a trajectory for robot manipulators. The authors analyzed and compared in detail algebraic and trigonometric splines, their combined use and the use of B-spline techniques. Erkorkmaz et al. (2006) presented a trajectory planning strategy for maintaining the tool positioning accuracy in high speed cornering applications. Two spline fitting strategies were developed for smoothening sharp corners. The over-corner test results indicated the possibility of improving the contouring accuracy at sharp corners. Red (2000) investigated a dynamic and adaptive trajectory generator using S-curves. Between the constant acceleration and deceleration periods, there were used constant jerk transitions. Optimization was defined to be the minimum time to transition from the current speed to the set speed for the move segment when jerk and acceleration are limited. The S-curve equations were adapted to instantaneous changes in speed setting and path length. An integrated motion planner determined allowable speeds and transitional profiles based on the remaining move distance. Boryga and Grabos (2009) used polynomials of higher degree to plan a trajectory of robot manipulator. The linear acceleration profiles of end-effector were planned as the polynomials of 9, 7 and 5 degrees. They presented time courses of displacements, velocities, accelerations and jerk for the rectilinear path of end-effector for a three degree of freedom manipulator. A characteristic of the proposed polynomials is a zero jerk value at the motion start and end that has positive effect on drive unit load, motion control and positioning accuracy. Valero et al. (2006), presented an algorithm capable of obtaining a sequence of feasible robot configurations between the initial and final point for the trajectory with obstacles. They divided the algorithm into two stages obtaining a discrete configuration space followed by obtaining an optimal and feasible trajectory. In the second stage, configurations are to be found that allow a minimum weighted free-collision path. For this sequence of configurations, trajectories in the joint space are generated that minimize the total required time and are dynamically compatible with the robot features. Tian and Collins (2004) proposed a novel trajectory planning method for a robot manipulator whose workspace includes several obstacles. They developed a genetic algorithm (GA) to search for valid and optimal solutions to the trajectory in task space. To approximate the time histories of the trajectory, they used a polynomial based on Hermite cubic interpolation. The authors demonstrated the effectiveness and capability of the proposed approach through simulation studies. Grabos and Boryga (2013) presented an algorithm PCM (Polynomial Cross Method) for planning motion of a manipulator end-effector, whose path was composed of two rectilinear segments. Boryga (2014) presented an algorithm for rectilinear-arc trajectory planning whose path is composed of two rectilinear segments connected with a loop-shaped arc. The algorithm can be used in solutions with high speed cornering applications.

Some research centers have conducted the testing of manipulators for agricultural product harvesting, spraying and carrying. In Wageningen, the field tests are performed on a cucumber harvesting manipulator use in a greenhouse. Van Henten et al. (2002, 2003, 2009) developed a manipulator design for fully autonomous cucumber picking robot including the autonomous vehicle, the manipulator, the end-effector, vision system and control system. Besides, Van Henten et al. (2006) discussed the preliminary results of field tests with a robot for removing leaves from a cucumber plant. The field trials have confirmed the robot suitability for automatic cucumber harvesting and removing leaves from a cucumber plant.

Among the research papers addressing the generation of trajectory motion, the one by Van Henten et al. (2003) deserves attention as it presents the algorithms used to calculate collision-free end-effector motions. Although the developed algorithm generated good solutions, it was found to be too slow for on-line operation. Van Willigenburg et al. (2004) searching for a novel faster algorithm used the trajectory of the bang-bang type and assumed it timeoptimal control for fruit harvesting. The literature review allowed to state that the further studies should focus especially on development of new fast algorithms for trajectory motion generation (shortening of working cycle time), consider avoidance of obstacles and accuracy of end-effector towards a picked fruit.

This work presents an innovative algorithm for planning the rectilinear-arc trajectories with 7-degree polynomial application. The developed algorithm allows for the trajectory planning in only a few steps and therefore, it is very effective. Additionally, the algorithm ensures the continuity of displacement, velocity and tangential acceleration for the planned end-effector move. Besides, the present paper proposes the concept of the tomato harvest system using

the novel algorithm for the generation of end-effector trajectory ensuring high accuracy of positioning, especially at the initial and final trajectory point.

2. Trajectory planning with blending arc

2.1. PR-APT method

The PR-APT method (Planning Rectilinear-Arc Polynomial Trajectory) is applicable for planning an end-effector move trajectory, whose path consists of two rectilinear segments of intersecting directions linked by the arc. On the rectilinear distances, acceleration profile was described by polynomial of 7-degree. It is the lowest-degree polynomial, for which the jerk at the initial and final moment of the motion and at the change of the run-up phase into the brake one is equal zero (Boryga and Grabos, 2009). It was assumed that acceleration on rectilinear segments B-T1 and T2-E does not exceed the preset maximal value smax and further, that at transition from the rectilinear

segment to the arc, the resultant speed value of the end-effector does not change. The motion over the blending arc proceeds at a constant speed. During the transition from the arc to the other rectilinear segment, the resultant speed value does not change either. Appropriate determination of acceleration profile for the rectilinear segments necessitates the construction of ancillary segments. The ancillary segments are the axis symmetry of the rectilinear move distances in relation to the points tangent to the arc (Fig. 1).

Ancillary segment

Ancillary segment

Tangent point

Obstacle

Acceleration protlle

Fig. 1. Acceleration profile on a) B-T1 segment and its ancillary one b) T2-E segment and its ancillary one

To describe acceleration of total segments (move segment and ancillary one), a 7-degree polynomial was applied

s(t) = - p • t2 ■(t - 0.5te)3 ■ (t - te)2

where: p - polynomial coefficient, te - time of motion.

For this polynomial form, tangency point is achieved in the half move time on the total segment. At the same time, acceleration is equal to zero in the point of tangency. At the time of motion over the blending arc, a value of determined speed does not change, hence tangent acceleration is equal to zero. However, normal acceleration appears as the effect of motion over the curvilinear trajectory.

2.2. Algorithm of PR-APT method

Calculation of motion path geometry

Step 1. Assumption:

• coordinates of the initial point B (x^.x^.x^ ), and final point E (xf.xf.xf ),

• coordinates of the arc center O(x°,x°,x° ) and arc radius R being a fragment of avoidable obstacle design. Step 2. Determination:

• coordinates of the tangency point T1(xf1 ,xf1 .x^1 ) of straight line running through point B and tangent to the arc,

• coordinates of the tangency point T2(x12,xl2X2 ) of straight line running through point E and tangent to the arc,

• angle ^subtended by the arc of radius R (Fig.2).

Step 3. Determination of path increments on rectilinear segments B-Tl and T2-E

AsBn=mx? - xf )2, - =Z(xf - XT 2 )

Step 4. Assumption of acceleration limit 'smaa on the rectilinear segments.

Fig. 2. Motion path with introduced coordinate systems

Step 5. Introduction of the local right-handed coordinate system with the starting point O, axis versor £ directed towards the point of intersection of lines tangent to the arc and orientation of axis versor intersecting the line B-T1.

Step 6. Determination of matrix transformation A between the local system of coordinates connected with

the trajectory and the base coordinate system xixx

cos( ¿;1 ,x1 ) cos( Ç2 ,x1 ) cos( ,x1 ) x°

cos( ¿;1 ,x2 ) cos( Ç2 ,x2 ) cos( ,x2 ) x°

cos( ¿;1 ,x3 ) cos( Ç2 ,x3 ) cos( c;3 ,x3 ) x°

0 0 0 1

Computations for the Rectilinear Segments (superscript BT1 and T2E) Step 1. Pre-establishing motion time for segment B-T1 and T2-E

6VT05i s^ V2I • ( 2As)r

2 • ( 49 • S„ax)

For segment B-TT: As = AsBTT, t = tBTT. For segment T2-E: As = AsT2E, t = tT2E. Step 2. Determination of velocity value in the points of tangency TT and T2

105 4s

For segment B-TT: As = AsBTT, t = tBTT and s = sBT . For segment T2-E: As = AsT2E, t = tT2E and s = $ Step 3. Determination of motion time on B-TT and T2-E segment.

If s™ > Ti , then velocity in the points of tangency and over the arc should be assumed to be sA = s™, and the

105 AsBT1

motion time on B-T1 segment is derived from the dependency tBT1 =--t2e~ • In the case when s™ < s^f, the

64 s^tti

velocity at the tangential points and on the arc should be posited as sA = s™ while the motion time on T2-E

T2E 105 AsT2E

established from the dependency t =

64 s^'

Step 4. Establishing polynomial coefficient: acceleration, speed and position for segment B-T1 and T2-E 315

p="857- (6)

For segment B-T1: As = AsBT1, t = tBT1 and p = pBT1. For segment T2-E: As = AsT2E, t = tT2E and p = pT2E

Computations for Arc (superscript A)

Step 1. Establishing time of motion over the arc

tA (7)

Step 2. Determination of dependence of angular displacement

n/1 \ P - /t *BT1 1 e tBT1 * , ^ ,BT1 . .A

p(t) =---(t -1 ) lor t < t < t +1 (8)

Step 3. Determination of coordinates of the position vectors X A, speed X A and acceleration X A of the motion over the arc at base coordinate system X1X2X3

XA = A ■ SA, XA = A ■ SA, XA = A ■ SA (9)

where: XA =[xA(t), xA(t), x"A(t), 1f, EA =[RcosJ3(t), RsinP(t), 0, 1]T Final Computations

Step 1. Determination of translation in the time on the T2-E segment

tb = tBT1 - tT 2E + tA (10)

Step 2. Performing translation in the time of polynomial depicting acceleration profile for T2-E segment by tb value

-BT1(t) = -pBT1 .(t)2-(t-tBT1) ■ (t-2tBT1 )2 (11) sT2E(t) = pT2E • (t-tb)2 • (t-tT2E -tj ■ (t-2tT2E -tb)2 (12)

Step 3. Determination of total motion time over the planned trajectory B-T1-T2-E

te = tBT1 + tA + tT 2E (13)

3. Application of PR-APT Method in Tomato Harvest in Greenhouse

The proposed PR-APT method was utilized for planning trajectory of end-effector during tomato picking operation in a greenhouse. Designing the motion path based on this method was preceded with measurements performed in the Experimental Farm, University of Life Sciences, Lublin where the Admiro F1 tomato cultivation is carried out.

To robotize the harvest tasks, the following assumptions were formulated (Fig.3):

• Staggered arrangement rows of tomato plants, plant spacing in row is h=0.5m. Spacing of I and III row as well as II and IV is h.

• Spacing of I and II row with manipulator locomotion between them is 2h.

• Tomato plant space was denoted as three zones:

Z1 - minimum damage risk r e (k1R,R (yellow ring in Fig.3),

Z2 - increased damage risk r e (k2R,k1R) ( orange ring in Fig.3), Z3 - harvest zone r e (0,k2R, (red circle in Fig.3).

where: ki and k2 - coefficients of the harvest zone size associated with a tomato variety. The minimum damage risk zones Z1 in the plants from I and II row facilitate safe end-effector motion to approach the plants in III and IV row.

• Consecutive stop positions Pn of manipulator will be located in the axis of each plant from a selected side. A Pn position of manipulator enables picking tomatoes in eight quarters of the Z3 denoted with red.

• The harvested tomatoes will be transported to one site of the pallet - point E that will need a special solution of palletizing system. The assumed coordinate X2 of point E was L=0.325m.

• During the harvest operation of tomatoes from I and II plant row and one plant from III row, end-effector motion will proceed over the rectilinear trajectories.

III I II IV

Fig. 3. Tomato cultivation system with end-effector paths.

• During the harvest operation of tomatoes from III and IV plant rows, end-effector motion will proceed over the trajectories with arc. The arc segment planned at the boundary of Z1 and Z2 zone facilitates avoidance of obstacle, i.e. the plant from I or II row (minimization of plant damage).

• The planned trajectories will be located in the horizontal plane X3=0.5 resulting from the location of ready to harvest tomatoes at nearly the same picking position - 0.5m above the ground. In response to potential differences in the height, the end-effector will perform local movements.

4. Simulation Test Results

The presented method PR-APT will be used for trajectory planning over the path B-T1-T2-E, it includes an arc segment and is the longest trajectory among those realized in the Pn position of manipulator. The base coordinate system X1X2X3 is associated with the Pn position of manipulator (Fig.3). The assumed radius was R=0.25m, while h=2R=0.5m. The zone size coefficient for Admiro F1 variety tomato assumed was ki=0.8. The coordinates of B and E points are B(-1, -0.25, 0.5), E(0, 0.325, 0.5). The arc center has the coordinates 0(-0.5, 0, 0.5). The calculated coordinates of the tangency points are T1 (-0.480, - 0.199,0.5), T2 (-0.341, - 0.121,0.5). Maximal acceleration

assumed was 'snaa =2 m/s2. The distance increments on the segments B-T1 and T2-E are equal AsBBT = 0.522m and

As12E = 0.561 m. The motion times resulting from the preset acceleration snax and established distance increments

are tBT1 = 0.969 s and tT2E = 1.004 s. The velocities at the tangency points reach s^1 = 0.885 m/s and s™ = 0.917 m/s. According to the algorithm, the velocity at the tangency points was assumed to be lower from the established sn1 = S^2E = sA = 0.885 m/s. The motion time over the T2-E segment for the decreased velocity is tT2E = 1.041 s. The polynomial coefficients on the segment B-T1 and T2-E are equal pBT1 = 27.418m/s9, pT2E = 15.430 m/s9, respectively. Total angular displacement along the arc T1-T2 is ¡3 = 0.821, while motion time over the arc is

t'A = 0.232 s. Translation in the time of motion start on the T2-E segment is tb = 0.159 s, whereas total motion time over the planned trajectory is te = 2.242 s.

fBTf tA tT2E

Res spet j Irani 'd s Speed x1 Y t ....

: s> oeed x2 V '. \ \

0.8 1.2 Time [s]

jBT1 ,T2E

langent acceleration s

Tangent acceleration x2 I''' S

acceleration xf/

O.i 1.2

Time [s]

fl!]1 tA ,T2E

Normal acceleration x2 . Resultant normal \acceleration S

N a armai zceleratioi *1 À

Time [s]

Fig. 4. End-effector profiles: a) position, b) speed, c) tangent acceleration, d) normal acceleration

The simulation tests provided the courses of displacements, velocities and accelerations of robot manipulator end-effector presented in Fig.4. Fig.4a presents the end-effector displacements on the coordinates X1 and X2, whose course is in accordance with the predefined trajectory and the total displacement of the end-effector from point B to

E is 1.288 m. Fig.4b presents the course of end-effector speed on X1, X2 directions and resultant speed. During the motion over the arc, speed components change in consistence with the trigonometric functions cosine and sine.

Resultant speed during the motion over the arc does not change and is s^ = s^f = sA = 0.885 m/s. The course of end-effector resultant speed is a continuous function. Fig.4c shows the course of tangential acceleration of the end-effector on directions X1, X2 and resultant tangent acceleration. Fig.4d shows the course of normal acceleration of end-effector on directions X1, X2 and resultant normal acceleration. In points B, T1, T2 and E, the course of tangential component of acceleration is tangent to the time axis and hence, jerk value in these points is equal to zero. During the motion over the arc, tangential component of acceleration is equal zero, though, normal component appears which is unavoidable during the motion over a curvilinear trajectory.

5. Conclusion

The proposed PR-APT method facilitates planning a trajectory of manipulator end-effector whose path comprises two rectilinear segments of intersecting directions connected with the arc. The method is applicable for trajectory planning with obstacles which are avoided over the arc of the set radius. This method efficiency, as compared to the classical approach of polynomial trajectory planning, results from a necessity of determining only one coefficient of polynomial depicting acceleration profile. The PR-APT method was employed to generate end-effector trajectory at tomato harvest in the greenhouse. The simulation tests showed that displacement, resultant speed and tangential acceleration are continuous functions. The course of acceleration profile is tangent to the time axis in the initial and end point of trajectory which should reduce vibrations and increase accuracy of positioning in relation to a picked fruit and pallet cell. The proposed PR-APT algorithm allows for the trajectory planning in only a few steps and therefore, it is very effective

The analysis of harvest and maintenance measures, as well as the stage of trajectory planning, will serve as the basis for structural synthesis of manipulator.

References

Boryga, M., 2014. Trajectory planning of an end-effector for path with loop. Strojniski vestnik - Journal of Mechanical Engineering 60(12), 804814.

Boryga, M., Grabos, A., 2009. Planning of manipulator motion trajectory with higher-degree polynomials use. Mechanism and Machine Theory 44(7), 1400-1419.

Dyllong, E., Visioli A., 2003. Planning and real-time modifications of a trajectory using spline techniques. Robotica 21(5), 475-482. Erkorkmaz, K., Yeung, C.-H., Altintas, Y., 2006. Virtual CNC system. Part II. High speed contouring application. International Journal of

Machine Tools & Manufacture 46(10), 1124-1138. Gasparetto, A., Zanotto, V., 2007. A new method for smooth trajectory planning of robot manipulators. Mechanism and Machine Theory 42(4), 455-471.

Grabos, A., Boryga M., 2013. Trajectory planning of end-effector with intermediate point. Eksploatacja i Niezawodnosc - Maintenance and Reliability 15(2), 182-187.

Red, E., 2000. A dynamic optimal trajectory generator for Cartesian Path following. Robotica 18(5), 451-458.

Tian, L., Collins. C., 2004. An effective robot trajectory planning method using a genetic algorithm. Mechatronics 14(5), 455-470.

Valero, F., Mata, V., Besa, A., 2006. Trajectory planning in workspaces with obstacles taking into account the dynamic robot behaviour.

Mechanism and Machine Theory 41(5), 525-536. Van Henten, E.J., Hemming, J., Van Tuijl, B.A.J., Kornet, J.G., Meuleman, J., Bontsema, J., Van Os, E.A., 2002. An Autonomous Robot for

Harvesting Cucumbers in Greenhouses. Autonomous Robots 13(3), 241-258. Van Henten, E.J., Hemming, J., Van Tuijl, B.A.J., Kornet, J.G., Bontsema, J., 2003. Collision-free Motion Planning for a Cucumber Picking

Robot. Biosystems Engineering 86(2), 135-144. Van Henten, E.J., Van Tuijl, B.A.J., Hemming, J., Kornet, J.G., Bontsema, J., Van Os, E.A., 2003. Field Test of an Autonomous Cucumber

Picking Robot. Biosystems Engineering 86(3), 305-313. Van Henten, E.J., Van Tuijl, B.A.J., Hoogakker, G.-J., Van Der Weerd, M.J., Hemming, J., Kornet, J.G., Bontsema, J., 2006. An Autonomous

Robot for De-leafing Cucumber Plants grown in a High-wire Cultivation System. Biosystems Engineering 94(3), 317-323. Van Henten, E.J., D.A. Van't Slot, Hol, C.W.J., Van Willigenburg, L.G., 2009. Optimal manipulator design for a cucumber harvesting robot.

Computers and electronics in agriculture 65(2), 247-257. Van Willigenburg, L.G., Hol, C.W.J., Van Henten, E.J., 2004. On-line near minimum-time path planning and control of an industrial robot for picking fruits. Computers and Electronics in Agriculture 44(3), 223-237.