IEEE ISIE 2005, June 20-23, 2005, Dubrovnik, Croatia The Application of Spline Functions and Bézier Curves to AGV Path Planning K. Petrinec, Z. Kova i University of Zagreb / Faculty of Electrical Engineering and Computing, Zagreb, Croatia kresimir.petrinec@fer.hr, zdenko.kovacic@fer.hr Abstract The paper describes a path planning method for an automated guided vehicle (AGV). The method consists of two steps, the calculation of a path by using Bézier curves and the calculation of a path trajectory by using a Ho and Cook spline-interpolation algorithm. The generation of parallel path curves for left-hand and right-hand AGV wheels is elaborated. The solution for the generation of sharp curves is derived. The proposed path planning method has been tested on tricycle-type AGVs with a front stabilization wheel and two rear powered wheels. I. INTRODUCTION Shop floor layouts usually contain a number of robotic manufacturing cells and a number of automated guided vehicles (AGVs). From the operational point of view they involve many repetitive operations. For example, an AGV travels from one node to another through determined paths and executes the sequence of actions like loading, unloading, parking or docking. A path following in such an environment can be accomplished by using a pure pursuit algorithm, whose main characteristics are marked simplicity and computational efficiency [1, 2]. A pure pursuit algorithm is an on-line method originally devised as a method for calculating the arc necessary to get a mobile robot back on a path. The role of a mission controller is to control the motion of AGVs along the paths and execute related operations determined during the shop floor layout design. The control involves a search for obstacle-free paths and in the same time, it cares about prevention of AGV collisions. In such circumstances, a Ho and Cook trajectory planning that uses cubic and fourth-degree spline-functions can be used [3]. The method was originally designed for off-line trajectory planning for industrial robots. In general, off-line methods have an advantage of allowing the knot points to be specified more closely in time, thus increasing the accuracy of a trajectory and allowing a shorter sampling period [4]. All the data for an application is calculated only once, before the robot runs the application for the first time. This data is stored and retrieved by a robot controller at run time. If a robot is executing some preplanned trajectory and an external stimulus is received, the controller may simply switch to the data for another preplanned trajectory. The method by Ho and Cook cannot be directly applied to AGVs path planning, as cubic and fourthdegree spline-functions cannot describe path segments with a required precision. In order to be able to apply the method, before the calculation of spline-functions a continuous curve through a desired path must be created. For this purpose a simple cubic equation developed by Pierre Bézier, usually called a Bézier curve, can be used [5]. In [6] an on-line piecewise cubic Bézier curves trajectory generation algorithm is used to create a smooth trajectory, but the method is based on the distance and not on the time. The paper is organized in the following way. First we describe the calculation of Bézier curves that define path knot points for a two wheel-driven AGV. Then we demonstrate a Ho and Cook trajectory generation method that uses these path knot points. An emphasis is given to the problem of planning of sharp turn paths. A method of reducing a number of inserted knot points related to the given path planning accuracy is explained and illustrated by simulation results. Comments and directives for the future work conclude the paper. II. PATH PLANNING The proposed path planning method for autonomous guided vehicles is a two-step method. The first step is generating the reference path points by using so-called Bézier curves and the second step is creating a trajectory through the reference points by applying a Ho and Cook trajectory planning method. A. The application of a Bézier curve As shown in Fig. 1, a Bézier curve is defined by four points: T 0 (x 0, y 0 ), T 1 (x 1, y 1 ), T 2 (x 2, y 2 ) and T 3 (x 3,y 3 ). Point T 0 is the origin endpoint, while point T 3 is the destination endpoint. Points T 1 and T 2 serve as control points. A cubic Bézier curve x, y can be created by using the following equations: 3 2 x axbxcx x0 3 2 y aybycy y where cx 3( x1 x0) bx 3( x2 x1) cx ax x3 x0 cx bx. cy 3( y1 y0) by 3( y2 y1) cy a y y c b y 3 0 y y 0, 0 1, (1) 0-7803-8738-4/05/$20.00 2005 IEEE 1453
Assuming that the mass center of the vehicle is moving along the Bézier curve [x( ), y( )], constructed curves parallel to, L [x L ( ), y L ( )]={T Li (x Li, y Li ), 0 i n} and R [x R ( ), y R ( )]={T Ri (x Ri, y Ri ), 0 i n}, describe the paths of the left-hand and right-hand wheels, respectively: vy xl x vc 2 vx yl y vc 2, (3) vy xr x vc 2 vx yr y v 2 c Figure 1. Bézier curve. Differentiation of (1) gives: dx 2 vx 3ax2bx cx d dy 2 vy 3ay2by c. (2) y d 2 2 v v v c x y In order to move a two wheel-driven vehicle along a Bézier curve, two parallel curves must be created, one for the left-hand wheel and another for the right-hand wheel. Except for the trivial cases of coincident parallel curves or straight lines, generally, it is impossible to derive a Bézier curve that is parallel to another Bézier curve. However, parallel Bézier curves can be constructed, as shown in Fig. 2. Points T L and T R lie on the normal to the tangent of the Bézier curve in point T. Points T L and T R are equidistant to point T. where w b is a wheel base length. Let us split -interval [0, 1] into n subintervals 1 i i 1, i 1, n, n 0 0, n 1, (4) Now, let us calculate n-1 points of the Bézier curve (1) and n+1 points of the left-hand and right-hand wheel paths (3). For n, calculated points create parallel curves H L, and H R. When n is small, curves H L, and H R can be approximated by piecewise linear segments which results in their quasi-parallelism (see Fig. 3). In other words, the piecewise linear approximation of path segments will cause an error in the calculation of the parallel curves. 6 5 T 3 4 y 3 T 1 2 1 T 0 T L T R 0 0 1 2 3 4 5 6 x T Figure 2. Construction of left-hand wheel and right-hand wheel paths parallel to the central Bézier curve. T 2 Figure 3. Bézier curve is quantized and parallel curves are constructed. The distances between two neighboring points of the left-hand and the right-hand parallel curves shown in Fig. 4 are calculated using the following equations: ( 1) ( 1) 2 2 d x x y y Lk Lk L k Lk L k ( 1) ( 1) 2 2 d x x y y Rk Rk R k Rk R k, 1 k n. (5) 1454
Equation (6) can be rewritten as: s s d Lk L( k 1) Lk s s d Rk R( k 1) Rk. (7) Figure 4. Distances between the neighboring points of the lefthand and the right-hand parallel curve. Besides the distance, it is necessary to find a motion direction of the wheels, i.e. the sign of d Lk and d Rk values. The motion direction can be detected by calculating the scalar product of vectors o 1, o 2 and o 3 defined by related neighboring points as shown in Fig. 5. Vector o 2 presents an orientation of a vehicle, while vectors o 1 and o 3 present oriented position changes of the left-hand and right-hand wheel. If o 1 o 2 < 0, d Lk must be taken with a negative sign. Similarly, if o 2 o 3 < 0, parameter d Rk has a negative sign. T L(k-1) T Lk o 1 o 2 B. The application of spline functions Now, the Ho and Cook trajectory planning method can be applied [3]. The method is based on the calculation of fourth-degree and third-degree polynomials that concatenated describe a continuous motion of an AGV between given knot points T Li (x Li, y Li ) and T Ri (x Ri, y Ri ), 0 i n. The developed path through n+1points contains n polynomials. The calculation of polynomials is an iterative process which involves intense operations with high-order matrices. Third-degree polynomials describe intermediate segments; while fourth-degree polynomials describe the first and last segments (we assume that velocity and acceleration at the start-points and end-points are equal to zero). In order to keep reference trajectories within physical limitations of AGV s wheel drives, the calculation of polynomial coefficients must account for actual AGV s position, velocity and acceleration constraints. Under assumption of the velocity constraint v m =1.2 m/s and the acceleration constraint a m =10 m/s 2, Fig. 7 shows simulation results of the AGV driving along the planned path. One can see that the planned path is continuous in position, velocity and acceleration, and planned velocities and accelerations are within physical limits. The traverse time from T 0 to T 3 is 6.4 s. The AGV is accurately positioned into the desired position, as shown in Fig. 8. T L(k-1) o 3 T lk Figure 5. Detection of the motion direction of the wheels. Figure 6. A reference path of the left-hand wheel and of the right-hand wheel. When the neighboring distances are calculated and the motion direction is detected, reference paths can be created (see Fig. 6). The reference path is a cumulative sum of the distances between points of the parallel curves: k s d, s d Lk Li Rk Ri i 1 i 1 k, 1 k n. (6) Figure 7. The planned AGV trajectory: v m =1.2m/s, a m =10 m/s 2. 1455
Figure 10. Distances between the neighboring points of the lefthand and the right-hand parallel curve (low density quantization is applied). Figure 8. The motion of the AGV driving along the planned trajectory. III. PLANNING OF PATHS WITH SHARP TURNS The Bézier curve-based planning of a path with sharp turns can result in a position error due to lower density of knot points in the turns (see Figs. 9 and 10). Lower density of points causes higher approximation errors of distances between two neighboring points lying on the parallel curves H L and H R. A problem can be solved by increasing the quantization of -interval and thus increasing a number of knot points (Figs. 11 and 12), but this can result in a longer traverse time and oscillations in the velocity and acceleration responses (see Fig. 13). As a compromise, the quantization of -interval could be higher at sharp turns, while it could remain lower on the straight parts of the path curves. Figure 11. Distances between the neighboring points of the lefthand and the right-hand parallel curve (high density quantization is applied). Figure 9. A position error due to low density quantization of the parallel curves. Figure 12. A position error due to high density quantization of the parallel curves. 1456
As shown in Figs. 15 and 16, the removing of excess points resulted in the significant reduction of a number of reference points. Comparing the numbers of reference points from Fig. 11 and Fig. 16, it can be seen that the described procedure reduced the number of points from k =100 to k =22. Besides on the density of the -interval quantization directed by the angle values of sharp turns, a final number of reference points primarily depends on the magnitude of a bounded deviation error parameter. In practice, the requirements on are not rigorous, which makes the choice of its acceptable value much easier. So obtained reference points for the paths with sharp turns become the knot points for the Ho and Cook trajectory planning procedure. As shown in Fig. 17, in spite of a reduced number of points, the positioning error remained the same, while the traverse time of the reference trajectory was reduced from 6.4 s to 4.9 s, which is more than 20% faster. Figure 13. The planned AGV trajectory: v m =1.2m/s, a m =10 m/s 2 (high density quantization is applied). Removing superfluous points from densely quantized curves H L and H R can be done by searching and removing those points whose distance from the line which connects neighboring points is smaller than a bounded deviation error. As can be seen in Fig. 14, the distance of point T Lk from the line T ( -1) T and the distance of point T Lk Lk ( +1) Rk from the line TRk ( -1) TRk ( +1) are less than. Therefore, points T Lk and T Rk can be removed from the list of the reference path points, but the lengths of the paths T ( -1) T and Lk Lk ( +1) T ( -1) T must be included in the Rk Rk ( +1) length of the reference path as d Lk +d L(k+1) and d Rk +d R(k+1). Figure 15. Reference paths obtained after dense quantization of parallel curves and after removing excess points. T L(k+ 1) T Lk d L(k 1) + T L(k-1) d Lk T Rk d R + (k 1) T R(k+ 1) d Rk T R(k-1) Figure 16. Distances between the adjacent points of the left-hand and the right-hand wheel paths after removing excess points. Figure 14. Removing superfluous points from densely quantized curves H L and H R. 1457
A problem arises when paths with sharp turns must be planned. Due to a lower density of knot points at the turns, higher approximation errors of distances between two neighboring points on the wheel paths occur. The problem was solved in two ways: first, by increasing the level of path quantization and thus getting a larger number of knot points at the turns; and second, by removing excess reference points using a bounded deviation error method. Reference path points obtained in this way become the knot points for the so-called Ho and Cook trajectory planning procedure, which calculates position, velocity and acceleration profiles for each AGV wheel using linked fourth-degree and third-degree polynomials (spline functions). The proposed path planning method has been tested by simulations on a tricycle-type AGV with a front stabilization wheel and two rear-powered wheels. The results of the planned motion indicate smooth changes of all wheel variables within given limits. REFERENCES Figure 17. The planned AGV trajectory: v m =1.2m/s, a m =10 m/s 2 (removing of excess points is applied). IV. CONCLUSIONS A two-step path planning method for automated guided vehicles (AGV) is described. The first step is the calculation of the geometric path through which the mass center of the vehicle should pass by using parameterdependent cubic Bézier curves. Since the case study deals with a tricycle type of an AGV equipped with a front stabilization wheel and two rear powered wheels, the generation of parallel path curves for left-hand and righthand AGV wheels is elaborated. [1] R.C. Coulter, Implementation of the Pure Pursuit Path Tracking Algorithm, The Robotics Institute Carnegie Mellon University Pittsburgh, Pennsylvania 15213, Jan. 1992. [2] K.Petrinec, Z.Kovacic and A.Marozin, "Simulator of Multi AGV Robotic Industrial Environments", The CD-ROM Proceedings of the IEEE Internatioanl Conference on Industrial Technology ICIT'03, Maribor, Slovenia, 10-12 December, 2003 [3] C.Y. Ho and C.C. Cook, The application of spline functions to trajectory generation for computer controlled manipulators, Digital Systems for Industrial Automation, 1 (4): 325-333, 1982. [4] P.G. Ranky and C.Y. Ho, Robot Modelling Control and Applications with Software, Springer-Verlag, IFS (Publications) Lts, UK, 1985. [5] J. Bloomenthal, R.E. Barnhill, B.A. Barsky, P. Bézier, R. Forrest,N. Max, D.M. Palyka, D.F. Rogers, H. Rushmeier, A.R. Smith, R. Stock, N.M. Thalmann and D. Thalmann, Graphics remembrances, Annals of the History of Computing, IEEE, vol. 20, issue 2, pp. 35-51, April-June 1998 [6] J.-H. Hwang, R.C. Arkin and D.-S. Kwon, "Mobile robots at your fingertip: Bézier curve on-line trajectory generation for supervisory control", Proc. IROS 2003, vol. 2, pp.1444-1449, Las Vegas, NV, 27-31 October 2003. 1458