Trajectory Planning Method for Mobile Robots

\IOBILE ROBOTS 11

Copyright © IFAC Inform ation Control Problem s in Manufacturing T echnology. \ladrid . Spain I'lS'l

TRAJECTORY PLANNING METHOD FOR MOBILE ROBOTS P. Campoy, G. F. Bobadilla, R. Aracil, M. A. Salichs and L. Moreno Departamento de Automat/ea , E .T. S .l.llldllstr/airs (DISA.\f ), ,\fluir/d, Spa/ll

Ab,tract. This paper accounts for the development of an original trajectory generation method for a two-dimensional mobile robot (i.e. capable of planar movement) designed to operate in industrial environments. In the last stage, a graph is searched to find an optimal solution. The nodes of this graph consist of three parameters, which determine absolute position and orientation of the robot. A first issue is the generation of trajectories in collision-free space (union of consecutive nodes). Cubicspline interpolation with optimization of the reference system with respect to a give n cost functi o n is used, where this cost or evaluation function has real physical meaning. Very efficient and computationally inexpensive algorithms were developed to determine the optim al reference system and the cost function of a given piece of trajectory, A second issue dealt with is that of graph-expanding. An on-line collision detection method is crea ted, and subsequently, a method for generating intermediate nodes in the presence of a single obstacle. Finally, an A-type algorithm is used to perform an on-line graph search, in order to determine the lowest cost trajectory in a fixed environment with obstacles. The trajectories obtained offer an advantageous compromise between optimality and computa ti o nal cost: while not sacrificing much of the first, possibility of on-line implementati on is ass ured. Keywords: Mobile robots, trajectory planning, obstacle avoidance, search graph, heuristics.

INTRODUCTION

The very spread method for finding collision-free trajectories are based on a point representation of the mobile robot. When working with such model of the mobile robot, there are basically two ways of finding the collision-free trajectory, each one operates in a different work space, which are: 1) Working in the two-dimensional physical space where the robot moves. In this case the robot is enclosed in a suitable circle and the obstacles are enlarged accordingly.

o

2) Working in the three-dimensional configuration space (C-space), whose coordinates are the planar position and the orientation, which determinate the state of the mobile robot. In this case obstacles are represented as three-dimensional constrains in such space.

Fig. 1. Collision-free space in industrial environment

Both operating ways are inefficient in the case of mobile robots thought to work in industrial indoor environment.

The second possibility of representing through a point is by working in the space of configuration (C-space) and has the following main backdraws in the case of study:

The first method implies to enlarge the obstacles' representation in such a distance that disappears most of the limited free space of an industrial environment where the mobile robot can go through (see Fig. 1). This way of working means using for a general case the solution which is apt for a robot with a circular cross-section, or which is made to maintain fixed orientation. Obviously, by doing so one wastes completely the manoeuvreing capability of the robot.

a) Increases of calculus complexity due to the augmentation of the dimensionality of the working space and to the fact of translating the obstacles into constrains in the C-space.

487

488

P. Campoy et al. b) Forces to use fictitious cost functions in C-space which have not direct physical meaning in the movement of the mobile robot. c) Difficults the control of the resulting trajectories due to the fact that every state parameter should follow a given time-function, independently of the others, without considering the actual mechanical relationships between them.

For these reasons, a new model of the robot is proposed, in which the robot is represented by means of an orientated segment.

- Work-space is two-dimensional, while sacrificing only a small portion of collision-free space. This portion of "wasted area" is related to the difference between the apparent and real edge of the robot (see Fig. 2), and is only significant at the rear part, which is a negligible fact, if considering that the robot is allowed to move only forward, and with limitations in maximum swerving angles. - Possibility of considering cost functions with physical meaning (time, length, weighted lengths), which is not feasible in C-space. - Rich description of the interaction between the robot and its surroundings.

MOBILE ROBOT MODELLING Description of the proposed model Due to the drawbacks of the methods mentioned above a new method of generating trajectories is presented in this paper, whose main advantage is the fact of generating trajectories which profit very well the free space in an indoor industrial environment and have as well a physical meaning, without increasing the complexity of calculus. The proposed method is based on a segment representation of the mobile robot, which is adequate for most of the mechanical structures of industrial mobile robots, based on present AGVs (Automated Guided Vehicle) (Fig. 2). The chosen model is a segment with its origin at A (middle of the rear axis) and its "arrow" at B (middle of the front wheel). Note that this can be applied to any mobile robot whose plant section can be successfully enclosed in a "curved" rectangle as the one showed with little loss of extra space.

·.....---1IIIIIIIIiI. d

GENERATION OF TIlE TRAJECTORY OF UNION BETWEEN TWO NODES, (Trajectories in collisionfree space) Description of the method of interpolation The method used is cubic-spline interpolation of the successive states of the robot, which are given by the position and orientation of the representing vector. The position and orientation fixed at the "initial" and "final" states of the robot determine six constraints on the trajectory. In a given reference system, there is a unique cubic interpolant which satisfies these constraints. We use as an additional parameter the angle that the x-axis of the reference system forrns to the x-axis of a given particular reference system, so that a single-parameter family of possible trajectories is offered at this stage. Difficult cases (Le. great changes of slope, or even orientations opposing the direction in which a natural trajectory would be traversed) can be solved admitting readjusting of unreasonable orientations, or simply by using a suitable intermediate state. This also applies for either very "small" or very "large" values of the parameter d/L (distance between points divided by the characteristic length of the robot). The advantages of this method of interpolation are the following:

Fig. 2 Mechanical structure of an industrial mobile robot Characteristics of the chosen model The model presented here is specific for the studied case, where the bidimensional mobile robot has an structure similar to that in Fig. 2 and using the orientation manoeuvreing capability is required (e.g. as a transport unit in indoor environments). Accordingly this model doesn't intend to be a general solution in the trajectory generation of robots.

- Smoothness. Tangent is continuous in global trajectories, which are formed by juxtaposition of several interpolants. Also, with imposed slopes not very different from those of the line passing through the initial and final position, curvature along the trajectory is very small. - "Global" solution. The trajectory is known in its whole and no incremental decisions are required. - Very small computational cost: closed formula, with no iterative process.

The advantages of this is model versus the point-model in Cspace are the following:

- Last and very important is the fact of having very simple, explicit analytic expressions (for the position of a reference point) which completely determine the successive states of the robot. It must be noted that the simplicity of the algorithms presented below depends strongly on this fact.

- Good controllable feature of the resulting trajectories, where the robot orientation is not an independent variable, but it is always tangent to trajectories.

The main drawback is that the generation of trajectories is performed statically, without any preview to the way in which the trajectories are going to be executed. There is no reference to the problem of control of trajectories and the

Trajectory Planning Method

489

controller will have to cope by itself with the dynamical aspects of the traversing. A minor difficulty is that global trajectories do not have

continuous second-derivative. So, there are not any discontinuities in the position of the robot, but the profiles of the swerving angles needed in the driving wheels are discontinuous. This feature does not represent any problem if trajectories are followed by a closed-loop controller. In this method of interpolation, the cubic polynomial is somehow the generalization of the straight line used as trajectory between two points, which represent the initial and final states of a mobile robot in the fIXed-orientation case.

Cost Function For a robot with a structure similar to that sketched in Fig. 2, the cost function is defined as the length of the trajectory traversed by the front wheel, which is the only driving wheel. Assuming a constant velocity on the driving wheel, the chosen cost function represents the time needed for traversing the given trajectory. This cost function generalizes the distance between the initial and final states of a mobil robot, when this is represented by a point. The possibility of using efficiently this cost function is related to the method of interpolation, which allows for explicit analytic expressions for trajectories. If (x.,y,) are the coordinates of the center of the front wheel, and (x,f(x» the coordinates of the center of the rear axis (reference point), where X€[x..x,), the cost associated to a piece of trajectory is:

LONGD=JJ;.,,(X)'+Y',(X)' dx -j71+f(X)'+L'.!J!l' dx [1) X. X. (1 + f(x)'l A direct method of optimizing this function would be to use the Calculus of Variations to find the optimal function y=f(x). Here only the one parameter cubic interpolants y=f..(x) are used, where a is the angle which determines the reference system as explained below.

Exact computation or Cost Function: A Romberg numerical integration can be used to assure a maximum error bound. Integral expression of [1) is computed. Approximate Computation or cost function: Let us define the "intrinsic reference system" for two given consecutive states of the robot as that whose x-axis passes through the initial and final point, so that the origin is at the initial point, the final point has positive abscissa and the orientation of the (x,y) system is positive (counter clock-wise x-y turn), see Fig. 3. An a reference system is defined as one whose x-axis forms

d sinal

Fig. 3 Reference System After algebraic manipulations in [1) (first order binomial Taylor series; averages value of integral with a positive weight function) the cost function can be approximated by: LONGD m"" d[1+km 5/48 cos'a(Il1o'+m,'+2/5 ffioIIl,»)+ + 2L'/(dcosa) [1/cos'a+2/15 (1l1o'+m,'+1/2 ffioIIl,W/' [2) (1l1o'+m,'+ffioIIl,) where: a = angle which determines reference system. d = distance between points. Ill; = Ill; - tga , (Ill; is the slope in the a-system) i = 0: initial state i = 1: final state L = characteristics length of robot (length of model) K.. = constant depending of a and verifying 0 < K.. < 1, optimized empirically to adjust best to exact cost function. The line followed here to estimate the validity of this approximate expression has not been to investigate the mathematical error bounds associated with the approximations (which are generally not reached by far), but to make a statistical study of errors, in order to determine the ranges of validity and associated errors. The imposed bound was an absolute value of relative error less than 2,5 %; and the average of relative errors obtained was 0,5 %; thus ranges of a and slope relative to intrinsic reference system were determined. The approximate cost function using formula [2) involves computational cost which is 2/5 to 1/3 of exact computational cost (CPU: System Micro VAX under VMS), using Romberg integration with the same maximum error bound. Though not too large a difference, this is significant due to the great number of times that the cost function must be evaluated in the course of graph-search, as will be discuss below. Out of the evaluated rang for formula [2), the Romberg algorithm is used. Optimal Reference System

an angle a to the x-axis of the intrinsic reference system. Obviously, all a-reference systems produce the same trajectory as it is invariant under translations and does not depend on direction of traversing. However, two different angles a, and a, give rise to different cubic interpolants corresponding to the Q, and Q,-systems.

The problem is to find the value a, such that fa,(x) attains minimum cost (LONGD(a) is minimum at a,), given the initial and final nodes.

Let fa(x) be the cubic-spline interpolant in an a-reference system.

Exact Algorithm

In practice, the problem is that given 10, find a such that ILONGD( a )-LONGD(a,) I <10, instead of that I a-a, 1< 10.

A value of a as close as desired to a, can be computed, solving:

490

P. Campoy et al. It is needed to design a collision-detection algorithm, and,

d/dta LONGD(ta)=0

[3]

based upon it, an algorithm to generate the intermediate node.

(where ta=tang(a) for simplicity) The derivative of LONGD with respect to ta can be computed through Leibuitz's formula of derivation of an integral with respect to a parameter, and it leads to a very cumbersome expression, (Derive [1] with respect to tana, where f depends on a, and also x,). A secant algorithm, modified to avoid inflexion points, is used to determine the ta which minimizes LONGD. (Obviously a Newton method would require further derivation to compute d'/dta' LONGD and a much greater complexity).

However this exact algorithm is prohibitive in terms of computational cost, exceeding by far the simple computation of the exact cost function. Therefore it is used, only to test the heuristic method presented below. Heuristic Algorithm Let ID., m, be the imposed slopes with respect to the intrinsic reference system. We distinguish between two cases: a) sgn(m.) =sgn(m,). The criterium used is intuitive: to minimize the area between trajectory and segment, which can be easily computed. The value obtained is tana o =---{-- + --) 2 m. m, b) sgn(m.)=sgn(m,). The previous criterium is not valid anymore, because the area can be made to approach zero as desired, while the cost function grows due to unlimited growth of second derivative in the initial and final points. A simple closed formula has been developed, which interpolates optimal values computed exactly. The heuristic algorithm has been tested using the exact algorithm, and the results are: - Case a) is exact. The formula gives 0.00% error in a and LONGD for all cases. Reasonably, the conclusion is a theoretical one (minimum area implies minimum length of the trajectory of the front wheel). - In case b) the formula has been optimized, so that for an extremely valid range of parameters (max Im.I, d/L), the results are: Average of absolute value of relative error: 0,02% Maximum absolute value of relative error: 0,5% The computational cost of this algorithm which reduces to the use of either one simple closed formula or another, is negligible with respect to any other computations involved. The cubic interpolant finally obtained does not depend on any arbitrary reference system. GRAPH EXPANSION METHOD. GENERAnON OF INTERMEDlATE NODES. In the presence of an obstacle, the interpolation described in section III might lead to a collision. Therefore, it is necessary to generate an intermediate node (position and orientation of robot) so that the consecutive union of the three nodes brings about two collision-free cubic interpolants, each one with its own reference system.

This algorithms are to be executed on-line, and it is here that the simplicity of analytic expressions of trajectories pays back. Collision-detection aliorithm The inputs are of this algorithm are the trajectory of the reference point (point A in Fig. 1) which includes a cubic polynomial and limiting abscissa, and the obstacle modelled by a convex polygon. The output is a boolean variable which states if there is collision or not, of some of the successive positions of the segment (Le. the model of the robot) with the polygon. Basically, this involves numerical investigation of crossintersection between a curve and a segment (where it is necessary to consider points of the curve where the derivative is equal to the slope of the segment). Additionally some quick ckeckings can be performed, which may serve to decide immediately that there is no intersection. If it is not so the more time-consuming routines are executed. Let us define:

d. = minimum distance from the center of the polygon to the cubic spline. fmax = maximum radius of the polygon, which is computed off-line. M = bound of difference in y-coordinate between the cubic spline (trajectory of the reference point) and the trajectory of the front wheel. This quantities are easily computed. (For M, much better values than L can be found). Obviously, if the polygon does not intersect the cubic spline, and d. < rmax + M, there can be no intersection, saving the numeric checking of the section with the trajectory of the front wheel (which is more complex, as it is not a polynomial). Also, if the polygon is small, and once it is seen that no edge of it intersects the two trajectories mentioned, checks are made that it is not enveloped between them. It is possible to perform this numerical global checks due to

the fact of having analytic expressions for the trajectories. However, the task is largely simplified as the trajectory of the reference point is a cubic polynomial. Generation of intermediate nodes In the presence of an obstacle with which the direct interpolation collides, a suitable intermediate state near the vertex is chosen, which represents an intermediate node in the search-graph. This intermediate state is chosen along the bisectrix of the correspondent angle of the polygon. Good initial estimates can be made as to the position of the freecollision-trajectory-generating intermediate node. The slope is a weighted average between the one at right angles with the bisectrix and the one that would appear naturally using clamped spline interpolation for the three points involved. Though the algorithm is iterative, until no collision appears, two or three iterates at the most are normally needed. Two versions of this algorithm were simulated: 8) a simplified one, which used only one reference system, estimated as close to the optimum, for the two pieces of trajectory, and b) one slightly more complex, which optimized independent-

Trajectory Planning Method Iy the angles of the reference systems of both implied splines. In forced cases of clear intersection with the polygon and large relative slopes, the advantage of the second algorithm is definite (see Fig. 4). As the computing time for optimum reference systems is negligible, the second version has been chosen.

491

the present node (determined by all the previous nodes encountered, and calculated here as described in 11.2) and an estimation of a minimal cost needed to reach the final node. f= g + h'

f: evaluation function g : cost function to reach present node h': estimation of remaining cost The possible choices for h' are: a) The same function as for g, as consequently with the trajectory generation method. It does not fulfill A' admissibility condition, and so the path found is not necessarily optimal. Nevertheless, it has got great "heuristic power" (see ref. Nilsson 1985), and the search is quicker, as a limited number of nodes are expanded. b) Distance between points. Obviously fulfills the admissibility condition, leading to optimal solutions, but the "heuristic power" is small. Inefficient graph search. c) A weighted average between them, depending on the parameters p ("depth" of the node in the graph) and d../L. where d" is a characteristic dimension of the surroundings. Obviously, for "deep" nodes, plausibly nearer the solution, the average must practically equal the function mentioned in a), as the constraint of slopes, makes the non-zero size of the robot be markedly felt. On the other hand, the distance between points is the major factor for a "shallow" node, as the size of the model may be negligible.

The trajectories obtained are intuitively very close to the optimal solution one would sketch. Small computing times assure possibility of on-line implementation. Figures 5 and 6 represent two exaples of the proposed algorithm, using the same initial and final position, but whit different orientation. The improvement of this algorithm versus a search algorithm where the cost function is based upon the distance between points are clearly seen. Fig. 4 Results' comparation using one (above) or two (below) optimal reference systems RESULTS. GRAPH-SEARCH All previous results are combined with the use of graphsearch algorithms to find the optimal path. While the graph is constructed on-line simultaneously to the graph search, the processing of the model of the surroundings, with obstacles appropriately enlarged, is performed off-line. Evaluation function in the I:raph-search The evaluation function is defined as the estimation of the cost of an optimal path conditioned to passing by a given node, reached throw its present path in the search tree. It is computed as the addition of the cost needed to reach

Fig.5 Example 1

492

P. Campoy et al. canny, John. (1986) Collision Detection for Moving Polyhedra. IEEE Transactions on pattern analysis and rnachine intelligence. Vol, Pami-8. No. 2.1. Chatila. (1986) Mobile robot navegation: space modelling and decisional processes. Robotics Research 3. MIT Press.

o [

Chattergy, R (1985) Some Heuristics for the Navigation of a Robot. The International Journal of Robotics Research, Vo!. 4, No. 1, Spring. Ihikawa, Yoshiaki; Norihiko Ozaki. (1985) A Heuristic Planner and a Executive for Mobile Robot Control. IEEE Transactions on Systems, Man, and Cybernetics, Vo!. SMC-15, No. 4. Iari i Valentf (1987) Study of new heuristics to compute collision-free paths of rigid bodies in a 2D universe.

Doctoral Thesis.

Fig.6 Example 2

CONCLUSIONS An original method has been presented in trajectory generation for mobile robots, using cubic interpolation.

The model chosen for the robot, as opposed to a pointmodel, has proved to be a complete success, and a good practical compromise solution for mobile robots designed to work in industrial environments, between computational cost and profiting robot maneuverability. Efficient approximate algorithms have been presented that compute a chosen cost function and the reference system in which an optimal interpolant is obtained. Also, an on-line collision detection algorithm has been outlined, and the method of generation of intermediate nodes (method of expansion of the graph) to which it applies. This algorithms take considerable advantage of the exact simple analytical description of trajectories mentioned above. Finally, the solution of the global problem has been attained, combining this results with the classical graph-search algorithms. The graph is created and simultaneously explored for a collision-free trajectory, linking initial and final specified states (position and orientation) of the robot in an environment crowded with obstacles. The trajectories obtained fulfill the two basic desired objectives: nearby optimal solution, computational economy assuring on-line implementation).

REFERENCES Aracil, R; P. Campoy y M.A. Salichs (61987) Robots M6viles. Feria Rob6tica. Zaragoza. Brooks, Rodney A; Tomas Lozano-perez. (1985) A Subdivision Algorithm in Configuration Space for Findpath with Rotation. IEEE Transaction on Systems, Man, and Cybernetics, Vo!. SMC-15. No. 2. Brooks, Rodney A (1983) Solving the Find-Path Problem by Good Representation of Free Space. IEEE Transactions on Systens, Man, and Cybernetics, Vo!. SMC-13, No. 3.

Iyengar, S.S.; C.S. Jorgensen, S.V.N. Rao, C.R Weisbin. (1985) Robot Navigation Algorithms Using Learned Spatial Graphs. Research sponsored by u.s. Dept. of

Energy Office of Basic Energy Sciences. Kambhampati, Subbarao; Larry S. Dacis. (1986) Multiresolution Path Planning for Mobile Robots. IEEE Journal of Robotics and Automation, Vo!. RA-2. No.3. Kant, Kamal; Steven W. Zucker. (1986) Toward Efficient Trajectory Planning: The Path-Velocity Decomposition. The International Journal of Robotics Research, Vo!. 5, No. 3. Keirsey, D.; J. Mitchell, B. Bullock, T. Nussmeier and D.Y. Tseng. (1985) Autonomus Vehicle Control Using AI Techniques. IEEE. Kuan, Darwin T.; Rodney A Brooks, James C. Zamiska, Mongobinda Das. (1984) Automatic Path Planning for a Mobile Robot Using a Mixed Representation of Free Space. IEEE 1984. Lozano-Perez, Tomas; Michael A Wesley. Communications of the ACM. (1979) An Algorithm for Planning Collision-Free Paths Among Polyhedral Obstacles. IEEE Transations on Computer Vo!. 22, No. 10. Lozano-Perez, Tomas. (1983) Spatial Planning: A Configuration Space Approach. IEEE Transactions on Computer, Vo!. C-32, No.2. Nilsson, Nils J. (1987) Principios de inteligencia artificial. Ed. Diaz de Santos, S.A