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 forNonholonomic Mobile Robots
KAICHUN JIANG
⋆
Manufacturing Engineering and Industrial Management, Department of Engineering,The University of Liverpool, Liverpool L69 3BX, UK; email: kaichun@liverpool.ac.uk
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 globallocal strategy, and solves the problem in the 2D workspaceof the robot, without generating the complex conﬁguration space. Firstly, a visibility graph is constructedfor ﬁndingacollisionfreeshortest path forapoint. Secondly, thepathfor apoint isevaluatedto ﬁnd 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 pathis found. Thirdly, robot conﬁgurations are placed along the selected path in the way that the robotcan move from one conﬁguration to the next avoiding obstacles. Lemmas are introduced to ensurethat the robot travels using direct, indirect or reversal manoeuvres. The algorithm is computationallyefﬁcient and runs in time O
(nk
+
n
log
n)
for
k
obstacles and
n
vertices. The path found is nearoptimal in termsof distance travelled.The algorithm istestedin computer simulations and test resultsare 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 thosefeatures are used in numerous applications, research results in general motion planning for nonholonomic robots are hardly adopted into autonomous vehicles. Twofacts have contributed to the complication of the problem. One is the rectangularshape of the robot. This shape rules out the possibility of using some algorithmswhich 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 curvatureof the path, which general path planners cannot satisfy.
⋆
Corresponding author.
348
KAICHUN JIANG ET AL.
Early research on mobile robot motion planning considered circular robots, andthe features of nonholonomic robots were not fully investigated until relativelyrecently. Laumond’s work in this ﬁeld started early. His methods in [1] and [2]generally presented the problem of path planning for a nonholonomic robot in aconﬁguration space (Cspace) [3]. The Cspace for a nonholonomic robot movingamong 2D polygonal obstacles is 3D, and consists of complex shaped obstacles.Constructing and searching the Cspace for a nonholonomic robot is time consuming. A solution using cell decomposition is presented in [4], where the Cspaceis decomposed into an array of small rectangloids (box shaped elements) and adirected graph is constructed and searched. All the rectangloids are in the freeconﬁguration space and each pair of adjacent rectangloids contribute a node to thegraph. The algorithm requires time O
(mr
m
log
m)
and space O
(r
m
)
, where
r
is thesize of the decomposition along each axis of the conﬁguration space and
m
is thedimension of the conﬁguration space. The approaches in [5], [6] and [7] also adoptdecomposition methods; the ﬁrst decomposing the environment into corridors forplanning smooth paths, and the second decomposing the environment into lanesfor a motion with minimum turns. Although decomposition is a commonly usedmethod, 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 ﬁrst general planner for nonholonomic robots based onaconstructive proof ofcontrollability. 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 completeplanner for mobile robots and showed that its strategy can be generalized. Thealgorithm works in three steps: (a) plan a path
π
for the corresponding holonomicsystem; (2) subdivide
π
until all endpoints can be linked by a minimal lengthcollisionfree feasible path; and (3) run an “optimization” routine to reduce thelength of the path [12]. An example of car parking is given, running in time 3.7seconds on a SUN Sparc 2 Workstation. This algorithm has been further tested[13] for a nonholonomic mobile robot moving in complex environments consistingof a number of polygonal obstacles. In one of the given examples, where ﬁve obstacles are distributed inside the workspace, the processing time on a SUN Sparc2 is 38 seconds. Divebliss and Wen [14] investigated the possibility to solve theproblem of ﬁnding a collisionfree path using iterated calculations on equationsformulated from constraints, initial and end conﬁgurations. The algorithm worksin large number of cases. As its convergence is not guaranteed, the algorithm mayfail in some cases. It may also produce a path other than an obvious better solutionbecause of the lack of optimal control equations used in calculations.This paper presents a new motion planning algorithm for mobile robots subjectto nonholonomic constraints. It employs a global analysis to ﬁnd the shortest pathfor a point and then locally modiﬁes the path to suit the nonholonomic robot. Thealgorithm processes in the following three stages:
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 spacealong the selected path is not sufﬁcient for the robot to pass, reject this pathand ﬁnd the next shortest path avoiding that local space. Repeat this until apath with sufﬁcient local free spaces is found.(iii) Lay conﬁgurations sequentially along the selected path in the way that therobot can manoeuvre from one conﬁguration to the next avoiding obstacles.For polygonal obstacles, the expensive computational stage is searching thevisibility 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 numberof convex parts of the obstacles. The path found is near optimal in terms of distancetravelled and the algorithm is computationally efﬁcient.The algorithm has a number of novel features compared with conventionalmethods. It develops the ﬁnal path based on the feasible shortest path generatedusing visibility graph, which is one of the fastest methods for ﬁnding a path fora point. Obstacle edges and vertices at which the point path turns are used inevaluating the local environment before conﬁgurations for collisionfree motionare laid out. Therefore, the process time of the algorithm is faster than those whichcan be found so far. Compared with Cspace based methods, it works in 2D work space rather than the complex 3D Cspace. Compared with the celldecompositionmethods, its computational complexity is better as mentioned above, thus faster.Compared with the approach in [13], the proposed algorithm does not need anoptimization stage, as it is based on the shortest path for a point, while the optimization is necessary in [13] to generate a near optimal ﬁnal path. This paperalso presents the study of the constrained robot manoeuvres, and introduced threetransfer techniques to make robot motion versatile.In the following sections, the visibility graph for producing the shortest pathfor a point is brieﬂy introduced. Then the paper is concentrated at local freespaceevaluation, motion techniques and laying conﬁgurations to construct a ﬁnal pathfor the constrained robot. Examples are given for testing the algorithm in computersimulation, and results are satisfactory.
2. Global Shortest Path for a Point
Let the workspace
W
contain a set of obstacles
O
= {
O
i
:
i
=
1
,
2
,...,n
}
. Itis required to move a point from
q
init
to
q
goal
. The problem is to ﬁnd the shortestcollisionfree path
P
min
from
q
init
to
q
goal
.The solution to this problem is well established and the concept of a reducedvisibility graph (RVG) [15] is employed. Visibility graph and RVG can be brieﬂydescribed as follows. Let obstacle vertices and the two ends
q
init
,q
goal
form a nodeset and a line connecting two nodes be a link. A visibility graph is constructed byconnecting all the nodes visible from each other. In a RVG, tangential links areretained and nontangential links are removed, even if they connect visible nodes. It
350
KAICHUN JIANG ET AL.
can be shown that
P
min
is given by searching the RVG using A
∗
or other searchingalgorithms.The proposed algorithm ﬁrst selects the global shortest path for a point andevaluates it to determine whether all the local spaces along the path are sufﬁcientfor modiﬁcation into a safe path for a kinematically constrained robot. If not, thispath is rejected and the next shortest path avoiding the identiﬁed narrow space canbe selected from an array produced in RVG process. This may be repeated until asuitable path is selected for modiﬁcation.
3. Local Evaluation of Free Space
A local evaluation of the free space around the path for a point is carried out todetermine whether the path for a point can be modiﬁed into a suitable safe path fora kinematically constrained mobile robot.The free space evaluation considers the kinematic constraints on the robot, thefree space required by the nonholonomic robot and the free space available in thelocality 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 robotmodel is presented in this section.Let the robot be a rectangular rigid object
R
, moving around an instantaneouscentre
O
i
. Let the robot be 2
a
long and 2
b
wide, with its wheels symmetrical aboutits long and short axes, as shown in Figure 1. A moving frame
F
m
is attached to
R
with its srcin at the geometric centre of
R
. The instantaneous centre
O
i
can beexpressed in the moving Cartesian frame as:
x
0
= −
l
2
tan
α
+
tan
β
tan
α
−
tan
β
y
0
=
l
tan
α
−
tan
β
,
tan
α
=
tan
β,
(1)where
l
is the distance between the two wheel axes, and
α
and
β
are steeringangles ofthe front andback wheels respectively, measured inthe counterclockwisedirection from the
x
axis. It is noted that the sign of
α
will normally be opposite tothat of
β
.A reference point
F
is set on the main axis of the robot, distance
x
0
from thegeometric centre
F
m
. This reference point
F
has a velocity that is perpendicular tothe radial vector of the instantaneous centre,
FO
i
.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 steeredvehicle; when
α
=
0, it gives that of a back wheel steered vehicle; and when
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. Mostvehicles are steered either by their front wheels or by their rear wheels. A vehiclesteered by both front and rear wheels can change its direction at a greater rate thanwhen 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,
−
d
x
d
t
sin
θ
+
d
y
d
t
cos
θ
=
0
.
(2)This is a nonintegrable differential equation and hence a nonholonomic constraint.Another kinematic constraint is generally imposed by the construction of thevehicle, 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 instantaneouscentre
O
i
. So the reference point
F
follows a curve whose curvature
c
is upperbounded by:
c
max
=
1
ρ
min
,
where
ρ
min
is the minimum turning radius of the reference point
F
, and fromEquation (1),
ρ
min
=
y
0min
.The velocity
v
of
F
measured along the main axis of the robot can be writtenas,

v

d
θ
d
t
ρ
min
or
d
x
d
t
2
+
d
y
d
t
2
−
ρ
2min
d
θ
d
t
2
0
.
(3)