Journal of Intelligent and Robotic Systems 24: 347–366, 1999. © 1999 Kluwer Academic Publishers. Printed in the Netherlands.

347

A Shortest Path Based Path Planning Algorithm for Nonholonomic Mobile Robots KAICHUN JIANG? Manufacturing Engineering and Industrial Management, Department of Engineering, The University of Liverpool, Liverpool L69 3BX, UK; e-mail: [email protected]

LAKMAL D. SENEVIRATNE and S. W. E. EARLES Department of Mechanical Engineering, King’s College London, Strand, London WC2R 2LS, UK (Received: 25 May 1998; accepted: 5 October 1998) Abstract. A path planning algorithm for a mobile robot subject to nonholonomic constraints is presented. The algorithm employs a global-local strategy, and solves the problem in the 2D workspace of the robot, without generating the complex configuration space. Firstly, a visibility graph is constructed for finding a collision-free shortest path for a point. Secondly, the path for a point is evaluated to find whether it can be used as a reference to build up a feasible path for the mobile robot. If not, this path is discarded and the next shortest path is selected and evaluated until a right reference path is found. Thirdly, robot configurations are placed along the selected path in the way that the robot can move from one configuration to the next avoiding obstacles. Lemmas are introduced to ensure that the robot travels using direct, indirect or reversal manoeuvres. The algorithm is computationally efficient and runs in time O(nk + n log n) for k obstacles and n vertices. The path found is near optimal in terms of distance travelled. The algorithm is tested in computer simulations and test results are presented to demonstrate its versatility in complex environments. Key words: motion planning, nonholonomic robot, constraints, visibility graph, reversal manoeuvres.

1. Introduction A nonholonomic mobile robot usually has typical features of a car, such as a rectangular shaped body and a limited steering angle. Although vehicles with those features are used in numerous applications, research results in general motion planning for nonholonomic robots are hardly adopted into autonomous vehicles. Two facts have contributed to the complication of the problem. One is the rectangular shape of the robot. This shape rules out the possibility of using some algorithms which requires the robot to be circular, or triangular. The other fact is that the nonholonomic constraints on a mobile robot give strict requirement on the curvature of the path, which general path planners cannot satisfy. ? Corresponding author.

VTEX(P) PIPS No.: 193930 (jintkap:mathfam) v.1.15 JINT1466.tex; 12/02/1999; 16:23; p.1

348

KAICHUN JIANG ET AL.

Early research on mobile robot motion planning considered circular robots, and the features of nonholonomic robots were not fully investigated until relatively recently. Laumond’s work in this field started early. His methods in [1] and [2] generally presented the problem of path planning for a nonholonomic robot in a configuration space (C-space) [3]. The C-space for a nonholonomic robot moving among 2D polygonal obstacles is 3D, and consists of complex shaped obstacles. Constructing and searching the C-space for a nonholonomic robot is time consuming. A solution using cell decomposition is presented in [4], where the C-space is decomposed into an array of small rectangloids (box shaped elements) and a directed graph is constructed and searched. All the rectangloids are in the free configuration space and each pair of adjacent rectangloids contribute a node to the graph. The algorithm requires time O(mr m log m) and space O(r m ), where r is the size of the decomposition along each axis of the configuration space and m is the dimension of the configuration space. The approaches in [5], [6] and [7] also adopt decomposition methods; the first decomposing the environment into corridors for planning smooth paths, and the second decomposing the environment into lanes for a motion with minimum turns. Although decomposition is a commonly used method, a rough decomposition may lead to some possible paths being missed, and a detailed discretization requires a long computational time. Lafferiere and Sussmann [8] presented the first general planner for nonholonomic robots based on a constructive proof of controllability. Murray and Sastry [9] showed how to solve the problem for some canonical systems. However, both references do not address obstacle avoidance, as pointed out by Sussann and Liu [10]. Jacobs, Laumond and Taix [11] improved the situation by presenting a complete planner for mobile robots and showed that its strategy can be generalized. The algorithm works in three steps: (a) plan a path π for the corresponding holonomic system; (2) subdivide π until all endpoints can be linked by a minimal length collision-free feasible path; and (3) run an “optimization” routine to reduce the length of the path [12]. An example of car parking is given, running in time 3.7 seconds on a SUN Sparc 2 Workstation. This algorithm has been further tested [13] for a nonholonomic mobile robot moving in complex environments consisting of a number of polygonal obstacles. In one of the given examples, where five obstacles are distributed inside the workspace, the processing time on a SUN Sparc 2 is 38 seconds. Divebliss and Wen [14] investigated the possibility to solve the problem of finding a collision-free path using iterated calculations on equations formulated from constraints, initial and end configurations. The algorithm works in large number of cases. As its convergence is not guaranteed, the algorithm may fail in some cases. It may also produce a path other than an obvious better solution because of the lack of optimal control equations used in calculations. This paper presents a new motion planning algorithm for mobile robots subject to nonholonomic constraints. It employs a global analysis to find the shortest path for a point and then locally modifies the path to suit the nonholonomic robot. The algorithm processes in the following three stages:

JINT1466.tex; 12/02/1999; 16:23; p.2

A SHORTEST PATH BASED PATH PLANNING ALGORITHM

349

(i) Construct and search the global reduced visibility graph for a point. (ii) Select the shortest path for local free space evaluation. If a local free space along the selected path is not sufficient for the robot to pass, reject this path and find the next shortest path avoiding that local space. Repeat this until a path with sufficient local free spaces is found. (iii) Lay configurations sequentially along the selected path in the way that the robot can manoeuvre from one configuration to the next avoiding obstacles. For polygonal obstacles, the expensive computational stage is searching the visibility graph for the shortest path; this requires time O(nk + n log n), where n is the total number of vertices, including the two end points, and k is the number of convex parts of the obstacles. The path found is near optimal in terms of distance travelled and the algorithm is computationally efficient. The algorithm has a number of novel features compared with conventional methods. It develops the final path based on the feasible shortest path generated using visibility graph, which is one of the fastest methods for finding a path for a point. Obstacle edges and vertices at which the point path turns are used in evaluating the local environment before configurations for collision-free motion are laid out. Therefore, the process time of the algorithm is faster than those which can be found so far. Compared with C-space based methods, it works in 2D work space rather than the complex 3D C-space. Compared with the cell-decomposition methods, its computational complexity is better as mentioned above, thus faster. Compared with the approach in [13], the proposed algorithm does not need an optimization stage, as it is based on the shortest path for a point, while the optimization is necessary in [13] to generate a near optimal final path. This paper also presents the study of the constrained robot manoeuvres, and introduced three transfer techniques to make robot motion versatile. In the following sections, the visibility graph for producing the shortest path for a point is briefly introduced. Then the paper is concentrated at local free-space evaluation, motion techniques and laying configurations to construct a final path for the constrained robot. Examples are given for testing the algorithm in computer simulation, and results are satisfactory. 2. Global Shortest Path for a Point Let the workspace W contain a set of obstacles O = {Oi : i = 1, 2, . . . , n}. It is required to move a point from qinit to qgoal . The problem is to find the shortest collision-free path Pmin from qinit to qgoal . The solution to this problem is well established and the concept of a reduced visibility graph (RVG) [15] is employed. Visibility graph and RVG can be briefly described as follows. Let obstacle vertices and the two ends qinit , qgoal form a node set and a line connecting two nodes be a link. A visibility graph is constructed by connecting all the nodes visible from each other. In a RVG, tangential links are retained and nontangential links are removed, even if they connect visible nodes. It

JINT1466.tex; 12/02/1999; 16:23; p.3

350

KAICHUN JIANG ET AL.

can be shown that Pmin is given by searching the RVG using A∗ or other searching algorithms. The proposed algorithm first selects the global shortest path for a point and evaluates it to determine whether all the local spaces along the path are sufficient for modification into a safe path for a kinematically constrained robot. If not, this path is rejected and the next shortest path avoiding the identified narrow space can be selected from an array produced in RVG process. This may be repeated until a suitable path is selected for modification. 3. Local Evaluation of Free Space A local evaluation of the free space around the path for a point is carried out to determine whether the path for a point can be modified into a suitable safe path for a kinematically constrained mobile robot. The free space evaluation considers the kinematic constraints on the robot, the free space required by the nonholonomic robot and the free space available in the locality of the point path. 3.1.

MODELLING A NONHOLONOMIC CONSTRAINED MOBILE ROBOT

The distance that a robot has to keep away from the obstacles depends on its geometry, dimensions and kinematic constraints. A general nonholonomic mobile robot model is presented in this section. Let the robot be a rectangular rigid object R, moving around an instantaneous centre Oi . Let the robot be 2a long and 2b wide, with its wheels symmetrical about its long and short axes, as shown in Figure 1. A moving frame Fm is attached to R with its origin at the geometric centre of R. The instantaneous centre Oi can be expressed in the moving Cartesian frame as:    tan α + tan β l    x0 = − 2 tan α − tan β   tan α 6= tan β, (1)   l  y0 = tan α − tan β , where l is the distance between the two wheel axes, and α and β are steering angles of the front and back wheels respectively, measured in the counter-clockwise direction from the x-axis. It is noted that the sign of α will normally be opposite to that of β. A reference point F is set on the main axis of the robot, distance x0 from the geometric centre Fm . This reference point F has a velocity that is perpendicular to the radial vector of the instantaneous centre, FOi . Equation (1) is a general expression for different steering models. When β = 0, Equation (1) gives the position of the instantaneous centre of a front wheel steered vehicle; when α = 0, it gives that of a back wheel steered vehicle; and when

JINT1466.tex; 12/02/1999; 16:23; p.4

A SHORTEST PATH BASED PATH PLANNING ALGORITHM

351

Figure 1. The model of a moving nonholonomic robot.

α = −β = 90◦ , it gives that of a robot turning around its geometric centre. Most vehicles are steered either by their front wheels or by their rear wheels. A vehicle steered by both front and rear wheels can change its direction at a greater rate than when steered by only one pair of wheels, assuming the same steering angle limits. In planar motions, mobile robots are subject to nonholonomic kinematic constraints, which are expressed in nonintegrable differential forms. For example, since F has no radial component of velocity, −

dy dx sin θ + cos θ = 0. dt dt

(2)

This is a nonintegrable differential equation and hence a nonholonomic constraint. Another kinematic constraint is generally imposed by the construction of the vehicle, where mechanical stops in the steering gear constrain α and β such that, αmin < α < αmax ;

βmin < β < βmax .

Since the robot is a rigid body, every point on R moves around its instantaneous centre Oi . So the reference point F follows a curve whose curvature c is upper bounded by: cmax =

1 ρmin

,

where ρmin is the minimum turning radius of the reference point F , and from Equation (1), ρmin = y0min . The velocity v of F measured along the main axis of the robot can be written as,  2  2  2 dθ dθ dx dy 2 + − ρmin > 0. (3) |v| > ρmin or dt dt dt dt

JINT1466.tex; 12/02/1999; 16:23; p.5

352

KAICHUN JIANG ET AL.

This is constraint inequality of the robot. The nonholonomic constraints provided by (2) and (3) are imposed on vehicles whenever they are moving. Thus any proposed path for the nonholonomic robot must satisfy the constraints (2) and (3). 3.2.

WIDTH REQUIREMENT FOR COLLISION - FREE MOTION

The robot R will sweep over a certain area along its path when it moves, Figure 2. If the robot R runs along its longitude direction following a straight line, the swept width is ws = 2b. If it turns with steering angles, α and β, the swept width ws (α, β) is given by ws (α, β) = ρmax (α, β) − ρmin(α, β), where ρmax (α, β) =

s a+

l tan α + tan β 2 |tan α − tan β|

2

 + b+

l |tan α − tan β|

2

and ρmin (α, β) =

2l − b. tan α − tan β

Therefore, the minimum distance, Dmin , between obstacles for a robot to pass should satisfy: Dmin − ws (α, β) > 0; i.e. Dmin − 2b > 0 (for straight line motion)

(4)

Figure 2. Swept area of a robot R from qi to qi+1 .

JINT1466.tex; 12/02/1999; 16:23; p.6

A SHORTEST PATH BASED PATH PLANNING ALGORITHM

353

and s

  2  l tan α + tan β 2 l a+ + b+ + Dmin − 2 |tan α − tan β| |tan α − tan β| l − b > 0 (for turning motion). + tan α − tan β

(5)

Inequalities (4) and (5) give the space requirements for collision free movement. Further, (5) is used to help plan the orientation of the robot along its path by determining α and β. For conventional vehicles either α = 0 or β = 0. So the required non-zero angle can be determined with a given Dmin . In the case that α = −β, a suitable steering angle can also be found. Since a small turning radius ρ minimises the local path around a corner, (5) assists in finding a short and feasible path. 3.3.

LOCAL COLLISION - FREE SPACE EVALUATION

The path for a point touches obstacle vertices and needs to be modified to account for the size and kinematic constraints of the robot. It is first verified whether the path for a point can be modified for the robot by evaluating the free space local to the point path. The free space evaluation is carried out by computing the minimum distances between the path and the surrounding obstacles, and between the obstacles on either side of the path. There are a number of algorithms available for computing the minimum distance between two objects, most being based on linear or nonlinear programming [16]. These are applicable only to convex obstacles and generally require a time consuming recurring search between obstacles. The approach adopted here makes full use of the features of the problem and obtains the minimum distances in a relatively short time. Moreover, it is independent of the shape of the obstacles. Let m obstacles be located in a workspace W . The obstacle set O is divided into separate groups relative to the point path, as shown in Figure 3: (i) Circumferentially, obstacles located on the left (viewed from the initial point) of the selected path are denoted as O l , and those on the right as O r ; (ii) Longitudinally, all obstacles are further divided into radial groups Oil and Ojr (i, j = 1, 2, . . . , s; where i, j are the radial group numbers). The location of the geometric centre of an obstacle determines its group membership. The bounds of the radial groups are circles at the turn points of the point path, Figure 3. Thus, the relationships between these groups are: s s [ [ l l Oi ⊂ O ; Ojr ⊂ O r ; O l ∪ O r ⊂ O. i=1

j =1

The path planning algorithm for the rectangular robot requires only two computations of minimum distances within each radial group Oil ∪ Oir :

JINT1466.tex; 12/02/1999; 16:23; p.7

354

KAICHUN JIANG ET AL.

Figure 3. Obstacle groups divided by the path and the turn points of the path.

(a) Between the path segment Pi and each obstacle in Oi , where Oi = Oil ∪ Oir ,

i = 1, 2, . . . , s,

and (b) Between obstacles in the circumferential groups Oil and Oir . These distances can be obtained from minimum distance computations between a line li and a vertex vj : min kli vj k = (li , vj ) , where (li , vj ) is the inner product; and between two vertices vi and vj : p min kvi vj k = |(vi − vj , vi − vj )|.

(6)

Thus the computation is fast, as no recurring search is involved [17]. 3.4.

VERIFICATION OF THE PATH FOR A POINT AND LAYING NEW CONFIGURATIONS

It is now possible to determine whether the shortest path for a point could be modified to suit the robot. The verification is carried out at locations along the path using conditions (4) and (5). If the conditions are not satisfied, then this path for a point is rejected. The shortest path avoiding this narrow gate is selected as the reference path. If the conditions are satisfied, then a configuration of the robot is placed between the two obstacles such that the robot will pass through this place in the given configuration to avoid possible collision. Once a reference path is selected, the path needs to be modified to suit the motion of the robot. The path modification is carried out in two stages. First, safe

JINT1466.tex; 12/02/1999; 16:23; p.8

A SHORTEST PATH BASED PATH PLANNING ALGORITHM

355

Figure 4a. Locating a configuration beside a nearest point.

Figure 4b. Nearest points around a turning point of a selected path.

configurations are laid at potential collision locations. Then the final path is constructed passing through these configurations employing transfer techniques which are outlined in the next section. Here two types of cases are considered: Case (i). An obstacle is less than w/2 away from a straight line path segment. Case (ii). The path touches obstacles at all its turning points. Figure 4a shows the configuration laying method for case (i): A configuration is laid with reference point F keeping a distance w/2 away from the nearest point Ts on the obstacle. Considering case (ii) above, Figure 4b shows a case with two nearest points p1 and p2 close to Ts , a turning point of the selected path; p1 is on an edge of an obstacle and p2 is at a vertex of another obstacle. The edges of the nearest points are expressed as fi (x, y), fj 1 (x, y) and fj 2 (x, y). The edges of Ts are expressed as gk1 (x, y) and gk2 (x, y). These edges form an outline of the free space. Now in conjunction with (6), it can be determined if the path can be transformed to suit the robot. If it can be transformed, the safe configurations are laid as outlined in Figure 4b: on each line joining the turning point Ts , and a nearest point pi , place a reference point F a distance of at least (y0 − ρmin ) from Ts . The instantaneous

JINT1466.tex; 12/02/1999; 16:23; p.9

356

KAICHUN JIANG ET AL.

direction of motion of the robot is perpendicular to the line pi Ts , when the space is tight, or has other orientations when the gap is wider than Dmin . Thus the process of verification and laying configurations will generate a series of collision-free configurations for the robot, along the path for the point. The next stage determines transfer techniques between adjacent configurations. 4. Techniques for Transferring Between Adjacent Configurations Three motion transfer techniques, direct, indirect and reversal, are employed in moving the robot R from configuration qi to its adjacent configuration qi+1 . The present discussion is restricted, without loss of generality, to cases where qi+1 is located on the right of qi . A local co-ordinate system is set up with its origin at qi and its x axis coinciding with the direction of motion of qi . The transfer from qi to its left configuration qi+1 can be solved in the same way using symmetry. The transfer of the robot from qi to the next configuration can in general be expressed as in Figure 5, where qj , qk , ql , and qm are next possible configurations. A circle Cmin with minimum radius ρmin is drawn tangential to the configuration qi , and a local co-ordinate system x-y is set up, Figure 5. The transfer from qi to qj , qk , ql , or qm involves particular techniques, which are discussed here. In a Cartesian co-ordinate, a path segment p(x) is called a direct path segment if its second differential does not change sign along the segment, i.e. d2 p(x) > 0 or dx 2

d2 p(x) 6 0, dx 2

for a continuous range of x values.

A direct path segment may have a number of different curvatures, but keeps the same second differential sign in the given region. A direct transfer is the movement of a robot along a direct path segment. An indirect path segment describes a path p(x) which changes its second differential sign at least once. An indirect transfer is the movement of the robot along an indirect path segment. A reversal is a manoeuvre of a robot involving movements in opposite directions. A reversal path is the locus of a robot after a reversal movement. A reversal transfer is the movement of the robot along a reversal path segment. At a reversal point, the tangent to the path changes its direction by 180◦ . Hence, it produces a cusp where the robot changes its moving direction. A reversal path may have more than one directional change. A reversal transfer is used for manipulating a robot from one configuration to another under the conditions of limited steering angle and space. It is often used to move the robot sideways by moving backwards and forwards. Car parking is an example. In Figure 6 a robot can travel from the initial configuration q1 to the final configuration q2 , avoiding collisions, after two reversal manoeuvres. In order to avoid collisions during reversal manoeuvres, the robot’s movement is restricted to the inside of a rectangular area, which is selected to be a collision-free area.

JINT1466.tex; 12/02/1999; 16:23; p.10

A SHORTEST PATH BASED PATH PLANNING ALGORITHM

357

Figure 5. Transfer of configuration qi to next possible configurations qj , qk , ql , or qm .

Figure 6. Reversal car park problem.

A robot with limited turning radius, ρmin, may be able to move from a configuration qi to the next configuration qi+1 , by using any of the three types of transfers, i.e. direct, indirect and reversal. It can be shown that when a configuration qi+1 can be reached from qi by a direct transfer, then it can also be reached by either indirect or reversal transfers. When qi+1 can be reached by indirect transfers but not direct transfers, it can also be reached by reversal transfers. Thus the reversal transfers are the most flexible manoeuvres, and direct transfers the least flexible ones. However, a reversal transfer takes longer time, while a direct transfer is the most time efficient one. Therefore, in path planning, direct transfers have the highest priority to be used, with indirect transfers the next, and reversal transfers being the last option. Direct transfers are considered first and Lemma 1 is introduced in order to determine the conditions when a direct transfer is feasible. LEMMA 1. Given configurations qi (xi , yi , αi ), qi+1 (xi+1 , yi+1 , αi+1 ) and a limited turning radius ρmin of a robot R, a local co-ordinate system is set up with origin at qi , the x axis coinciding with the direction of movement of qi and the y axis on the side of the next configuration qi+1 . The robot can move from qi to qi+1 by direct transfer if: G1 (xi+1 , yi+1 , αi+1 ) > 0 and

G2 (xi+1 , yi+1 , αi+1 ) 6 0,

where G1 (x, y, α) = y − ρmin (1 − cos α)

JINT1466.tex; 12/02/1999; 16:23; p.11

358

KAICHUN JIANG ET AL.

and G2 (x, y, α) = y cos α − x sin α + ρmin (1 − cos α). The proof of the lemma is omitted here. Details can be found in [18]. Lemma 1 gives a means to decide whether a configuration can be reached by a direct transfer and also provides the procedures necessary to achieve the direct transfer. The specific steps for judging if a configuration qi+1 is directly reachable from qi can be summarised as: 1 (i) Given qi , qi+1 and ρmin , set up a local frame Oxy and draw a circle Cmin tangential to qi . 1 (ii) Determine line G2 parallel to qi+1 and tangential to Cmin at p, and the line G1 parallel to the x axis through point p. This forms the slope co-ordinates pξ ψ. (iii) Check if the configuration qi+1 is located in the first quadrant of pξ ψ. If it is, qi+1 is reachable from qi by direct transfer; otherwise, use either indirect or reversal transfers. 1 2 LEMMA 2. Draw a line π 1 π 2 connecting the centres of Cmin and Cmin . If kπ 1 π 2 k > 2ρmin , the configuration qi+1 can be reached by an indirect transfer; otherwise reversal transfer is necessary.

Proof is omitted here, referring to [18]. Lemmas 1 and 2 can be used to determine which transfer techniques are required under a given condition. The results can also be used in transfer design, such as determining how many reversals are required in a particular situation. In practice, if configurations qi , qi+1 and the minimum turning radius ρmin are given, and a reversal path is required, then “configuration interpolation” can be used, where configurations are inserted between qi and qi+1 to construct a feasible reversal path for a nonholonomic robot. 5. Constructing the Final Path The final path generated for the robot is a continuous collision free path described by a series of configurations. The proposed method generates sufficient configurations of the robot so that there is direct transfer between all adjacent configurations. If it is found that an indirect transfer is required, one or more configurations are added in to change the indirect transfer into a number of direct transfers. The techniques adopted for adding configurations for direct, indirect and reversal transfers are outlined below. A direct path segment can be constructed by drawing two circles tangential to the two configurations, and a tangent to the circles. Then the arc-tangent-arc chain will be the direct path segment, Figure 7a. The radii ρ1 and ρ2 of the two circles can vary within the range of ρ1 > ρmin and ρ2 > ρmin . However, ρ1 and ρ2 will affect the length of the path segment with ρ1 = ρ2 = ρmin giving the shortest route.

JINT1466.tex; 12/02/1999; 16:23; p.12

A SHORTEST PATH BASED PATH PLANNING ALGORITHM

359

Figure 7a. Constructing a direct path segment from qi to qi+1 using tangential circles with different radii ρ1 > ρmin and ρ2 > ρmin , and ρ1 = ρ2 = ρmin .

Figure 7b. Converting an indirect transfer from qi to qi+1 into two piecewise direct transfers by adding a configuration qa1 .

Figure 7c. Converting an indirect transfer from qi to qi+1 into three piecewise direct transfers by adding two configurations, qa1 and qa2 .

JINT1466.tex; 12/02/1999; 16:23; p.13

360

KAICHUN JIANG ET AL.

Figure 8. Constructing a reversal path segment from qi to qi+1 with piecewise indirect transfers.

This is also the case for constructing indirect paths. Figure 7b shows a case where a configuration qa1 is added between qi and qi+1 . qa1 is the touching point of the two circles C1 and C2 , tangential to qi and qi+1 and having radii ρ1 and ρ2 (ρ1 > ρmin , ρ2 > ρmin ), respectively. The path segment is formed by the arc of C1 from qi to qa1 , and the arc of C2 from qa1 to qi+1 . Figure 7c shows another case where two configurations qa1 and qa2 are assigned between qi and qi+1 . Two circles C1 and C2 , tangential to qi and qi+1 respectively, are drawn having the same radius ρmin . A line l is drawn tangential to both C1 and C2 , with the additional configurations qa1 and qa2 located at the end points. The path segment is formed by the arc of C1 from qi to qa1 , straight line l from qa1 to qa2 , and the arc of C2 from qa2 to qi+1 . It can be shown that the path segment proposed in Figure 7b is longer, and requires a larger change in the robot’s orientation, than that in Figure 7c, where the radii of the circles tangential to qi and qi+1 are ρmin . If reversal transfers are required for the robot to move from qi to qi+1 , then the path will also contain piecewise indirect and/or direct transfers. Figure 8 illustrates a path containing two reversal transfers. The robot moves by direct transfer along a circular arc from qi to qa1, which is parallel to qi+1 . A location for qa2 , in the same orientation as qi+1 , is found within the free space, and is accessible by an indirect transfer from qa1 . Configuration qa3 of the robot is at the same location as qa2 but in an opposite direction. Similarly, the configurations qa4 and qa5 can be determined. The path segment qi → qa1 → qa2 → qa3 → qa4 → qa5 → qi+1 contains two reversals, with adjacent configurations reached either by direct or indirect transfers. The number of reversal manoeuvres depends on the location of qi+1 relative to qi , the local free space and the steering capability of the robot.

JINT1466.tex; 12/02/1999; 16:23; p.14

A SHORTEST PATH BASED PATH PLANNING ALGORITHM

361

The reversal manoeuvres of a mobile robot should be limited to within a certain collision-free domain, as indicated by the dot-lined rectangle in Figure 8. If a local co-ordinate system ξ Oψ is set as in Figure 8, the width ω and length λ of the rectangle can be determined by the criteria 0 < ξ < ω and 0 < π < λ; where p(ξ, ψ) is a point of the obstacle set: p ⊆ O. 6. Computer Simulations The algorithm has been extensively tested using computer simulations. The examples have included a variety of obstacle sizes, shapes, locations and numbers. The tests also included varying the robot dimensions and kinematic constraints. The dimensions of the rectangular robot are: length: 2a = 1.5; width: 2b = 0.8; the distance between two axes: l = 1.3; the distance between the two wheels: d = 0.8. The same set of 6 polygonal obstacles are used in the first three examples (Figs 9–11). The front steering angle limit α and the obstacle locations are used as the variables. The back steering angle β is zero. Figure 9 represents a car with α = 40◦ . The solution path requires one reversal manoeuvre when the robot is approaching qgoal. This in fact is a reverse parking situation. The simulation was carried out on an Apollo 300 Workstation, and a processing time of 2.1 second was required including the graphical display of obstacles and the paths, and animation of the robot movement. In Figure 10, the mobile robot is the same, but the triangular obstacle has been moved upwards and the star-like obstacle moved to the right. This affects the motion of the robot in two ways. Initially it turns left then right to avoid the triangular obstacle. Further the shortest path for the point is no longer valid for the robot because of the narrow gap indicated by d. The path planner selects the shortest path that avoids this narrow gap and transform this to suit the robot. For this case, the processing time is 2.3 second. To make the problem more difficult, the steering angle of the front wheels is reduced to 12◦ . Under this condition, the path planner has to use more reverse transfers to reach the goal position. The case is shown in Figure 11, the robot starting with two reversals, and ending with three reversals. The processing time on an Apollo 300 for this example was 2.7 seconds. Another set of four obstacles are selected for next two examples. The robot parameters remain the same as before, but the steering limit is varied. In the example of Figure 12, the steering angle is set at 25◦ , and the robot needs one reversal manoeuvre along the shortest path for a point to reach the goal. The processing time is 1.8 seconds. When the steering limit is increased to 30◦ , a solution path with no reversal manoeuvres is given. In Figure 13, the obstacle positions are changed and the shortest path is through a gap with width less than that of the robot. Hence, the second shortest path for a point obstacle is selected for modification, and the final path produced is shown.

JINT1466.tex; 12/02/1999; 16:23; p.15

362

KAICHUN JIANG ET AL.

Figure 9. A feasible path planned for a front-wheel-steered vehicle with steering angle limits α = 40◦ and β = 0◦ .

Figure 10. When the shortest path is not valid (d < 2a), the planner selects the shortest path avoiding the gap d to construct the final path.

JINT1466.tex; 12/02/1999; 16:23; p.16

A SHORTEST PATH BASED PATH PLANNING ALGORITHM

363

Figure 11. The steering angle limit reduced to α = 12◦ .

Figure 12. The steering limit is α = 25, and the processing time is 1.8 sec.

The steering limit is set at 30◦ , and the robot needs a reversal before reaching the goal. The processing time is 2.0 seconds. The computer simulation results demonstrate that the algorithm is robust and versatile. It is applicable in cluttered environments requiring difficult reversal manoeuvres, and is computationally efficient.

JINT1466.tex; 12/02/1999; 16:23; p.17

364

KAICHUN JIANG ET AL.

Figure 13. The steering limit is α = 30, and the processing time is 2.0 sec.

7. Conclusions A new motion planning algorithm for a rectangular mobile robot subject to nonholonomic constraints is presented and tested in computer simulation. It is shown to be suitable for cluttered environments requiring difficult manoeuvres. The algorithm is also computationally efficient, with the computational complexity being O(nk +n log n) for n vertices and k obstacles. Computer simulations are employed to demonstrate that the algorithm works efficiently in difficult situations. The algorithm consists of three stages: (i) Solving the shortest path problem for a point using a Reduced Visibility Graph. (ii) Selecting the shortest path for a point, and evaluating the local free space along the path. If the path is not suitable for the given robot, discard the path and select the next shortest path avoiding the area where the mobile robot cannot pass through. Repeat it if necessary until a suitable path is selected. Then safe configurations for the nonholonomic robot are laid out along the selected path. (iii) Specifying transfer techniques between adjacent configurations and adding more configurations. The transfer techniques take into account the robot’s non-holonomic kinematic constraints and includes reversal manoeuvres. The proposed algorithm has the following distinctive features compared to previously published motion planning methods for nonholonomic constrained rectangular robots:

JINT1466.tex; 12/02/1999; 16:23; p.18

A SHORTEST PATH BASED PATH PLANNING ALGORITHM

365

(a) The motion planning is guided by the feasible global shortest path for a point. The free space is only evaluated locally to verify its suitability for the robot. Thus the algorithm combines global and local methods. This is in contrast to the C-space based cell decomposition methods which evaluate the free space globally and in 3D C-space. (b) Based on the study of the manoeuvres of a nonholonomic mobile robot, robot transfer techniques are introduced to suit different situations. This makes the constrained robot versatile in its motion. (c) The algorithm is computationally efficient, the overall complexity being of polynomial order. The algorithm works with polygonal obstacles at the moment, as local environment evaluation is carried out using straight obstacle edges. More work is needed to extend the algorithm for wider range of obstacles. Convex obstacles can be dealt with in less time than concave obstacles, as shortest path find among concave obstacles takes longer time. Further work of this algorithm can be carried out in using curves with better continuity property than circular arcs and straight lines in the final path. Also the possibility to reduce or eliminate some reversal manoeuvres can be studied.

References 1.

Laumond, J. P.: Feasible trajectories for mobile robots with kinematic and environment constraints, Preprints of the International Conference on Intelligent Autonomous Systems, Elsevier Science Publishers, B.V., Amsterdam, The Netherlands, 1986, pp. 346–354. 2. Laumond, J. P.: Finding collision-free smooth trajectories for a non-holonomic mobile robot, in: Proceedings of the 10th International Joint Conference on Artificial Intelligence, Milan, Italy, 1987, pp. 1120–1123. 3. Lozano-Pérez, T.: Spactial planning: A configuration space approach, IEEE Trans. Computers 1C-32(2) (1983), 108–120. 4. Arraquand, J. and Latombe, J. C.: On non-holonomic mobile robots and optimal maneuvering, Revue d’Intelligence Artificielle 13(2) (1989), 77–103. 5. Tournassoud, P.: Motion planning for a mobile robot with a kinematic constraint, in: J. D. Boissonnal and J. P. Laumond (eds), Geometry and Robotics, Lecture Notes in Computer Science, Vol. 391, Springer-Verlag, 1989, pp. 150–171. 6. Wilfong, G. T.: Motion planning for an autonomous vehicle, in: IEEE Int. Conf. in Robotics and Automation, 1988. 7. Pruski, A. and Rohmer, S.: Robust path planning for non-holonomic robots, J. Intelligent & Robotic Systems 18(4) (1997), 329–350. 8. Lafferriere, G. and Sussmann, H. J.: Motion planning for controllable systems without drift: A preliminary report, Technical Report SYCON-90-04, Rutgers University, June, 1990. 9. Murray, R. M. and Sastry, S. S.: Steering nonholonomic systems using sinusoids, 29th C.D.C., Honolulu, Dec. 1990. 10. Sussmann, H. J. and Lui, W.: Limits of highly oscillatory controls and the approximation of general paths by admissible trajectories, Report SYCON-91-02, Rutgers University, 1991. 11. Jacobs, P., Laumond, J. P., and Taix, M.: Efficient motion planners for nonholonomic mobile robots, in: Proceedings of 1991 IEEE IROS, Osaka, November 1991. 12. Li, Z. and Canny, J. F.: Nonholonomic Motion Planning, Kluwer Academic Publishers, 1993

JINT1466.tex; 12/02/1999; 16:23; p.19

366 13. 14. 15. 16. 17.

18.

KAICHUN JIANG ET AL.

Laumond, J. P., Jacobs, P. E., Taix, M., and Murry, R. M.: A motion planner for nonholonomic mobile robots, IEEE Transactions on Robotics and Automation 10(5) (1994). Divelbiss, A. W. and Wen, J. T.: A path space approach to nonholonomic motion planning in the presence of obstacles, IEEE Transactions on Robotics and Automation 13(3) (1997), 443–451. Rohnert, H.: Shortest paths in the plane with convex polygonal obstacles, Information Processing Letters 23 (1986), 71–76. Schwartz, J. T.: Finding the minimum distance between two convex polygons, Inform. Process Lett. 13 (1981), 168–170. Seneviratne, L. D., Jiang, K., and Earles, S. W. E.: A fast collision avoidance algorithm for a rectangular object, Proc. of 8th World Congress on The Theory of Machines and Mechanisms, Prague, August 1991. Jiang, K.: Operation planning for mobile robots and robot manipulators, PhD thesis, King’s College London, 1994.

JINT1466.tex; 12/02/1999; 16:23; p.20