Car-Like Robot Path Following in Large

Unstructured Environments
Shahram Rezaei, Jose Guivant, Eduardo M. Nebot
ARC Centre of Excellence in Autonomous Systems
School of Aerospace, Mechanical and Mechatronic Engineering
University of Sydney
NSW, Australia
Email: s.rezaei/jguivant/nebot@cas.edu.au
AbstractThis paper addresses the problem of on-line path
following for a car working in unstructured outdoor environ-
ments. The partially known map of the environment is updated
and expanded in real time by a Simultaneous Localization and
Mapping (SLAM) algorithm. This information is used to imple-
ment global path planning. A cost graph is initially constructed
followed by a search to nd the near-optimal path considering
uncertainty in both vehicle location and map. Selected points
in the global path are connected by continuous-curvature paths.
An improved feedback linearization technique is presented to
guide the car along the dened path. Experimental results are
presented to demonstrate the performance of the algorithms.
I. INTRODUCTION
During the past few years there has been signicant
progress in navigation applied to outdoor robots with several
industrial applications in well dened environments. At the
same time there is still a need of fundamental breakthroughs
in autonomous systems to make them reliable in much less
structured environments. These systems will have applications
in many areas such as search and rescue and re ghting.
Once a reasonable representation of the environment is
obtained, the vehicle needs to be controlled to perform a
certain path. Path following has three main stages: navigation,
path planning and guidance. The navigation module is usually
responsible for the localization of the vehicle within a given
map. The path planning module deals with dening global as
well as local paths and the guidance module is responsible
for keeping the car on the dened path within acceptable
errors.
Generally speaking, there are two categories for path
planners: deterministic and probabilistic. In the rst category
are cell decomposition, roadmap and articial potential eld.
In 1998, Yahja et. al. [1] proposed a Framed Quadtrees
concept which is basically a cell decomposition method. This
work presents experimental results in outdoor environments
but does not take into account uncertainty. Stentz improved
this algorithm with generalization to globally constrained
problems [2]. Incremental construction of Voronoi diagram
was addressed by Choset [3]. Then Nagatani et. al. [4]
proposed an optimization method to modify the original
Voronoi graph to obtain a smooth and traversable path for
car-like robots. However, as noted in the conclusions, the
algorithms to create the local path based on Bezier curves
are computationally expensive and highly dependent on the
tuning parameters. In general these algorithms are designed
for the case of working with known static maps and not for
cases where the map is built in real time.
The rst probabilistic motion planner was introduced by
Kavraki et. al. [5]. This planner, called Probabilistic RoadMap
(PRM), constructs a roadmap off-line during the learning
phase that consists of a set of nodes and local path. Then
during a query phase the path from a goal to a destination is
found based on the nodes obtained in the learning phase. The
algorithm is also extended and applied to Car-like robots but
still requires complete knowledge of the map [6]. Recently,
some new randomized motion planners were developed to
perform the same process on-line and considering the vehicle
dynamics [7] [8] [9]. The key assumption behind all these
planners is that a perfect navigation loop exists, that is
they assume a perfect knowledge of the environment, i.e no
uncertainty.
The real time operation of a car-like vehicle in a large
unknown / uncertain environment is still a very challenging
problem. So far, most applications consider the existence
of a map to design the path that a vehicle should follow to
perform a task based on a given optimization criterion. In an
unstructured partially known environment, only incremental
global path planners are useful since information about
the map is acquired incrementally. The use of global path
planner requires path re-planning from scratch every time new
information is incorporated. There are very few works that
considered uncertainty in path planning. In [10], [11], [12]
they considered some type of uncertainty in well structured
indoor environments. A near real time path follower was
introduced by Lambert [13]. This work can be considered
near real time, since it performs an off-line simulation in order
to generate sensors measurements and calculate uncertainty
in any possible vehicle pose. This work has recently been
generalized to car-like robots [14] but still requires a map
with the uncertainty representation.
In this paper the navigation module will be responsible
not only for the localization of the vehicle but for building
a map of the environment. This increases the complexity
of the problem since in addition to the standard issues of
computational cost, kinematics and dynamic constrains of the
car, we also add all the problems associated with uncertainties
in vehicle position and map. There is a signicant number
of papers describing SLAM algorithms using different
approaches [15], [16], [17], [18]. More recently some works
have appeared addressing the consistency aspect of SLAM
in very large areas [19], [20] presenting approaches that
use topological and features maps. In [21] a new approach
is presented to maximize the information that can be
incorporated into the maps. This approach is based on a
Hybrid Metric Map (HYMM) structure that combines feature
maps with any other metric representation such as occupancy
grid in a consistent manner. This algorithm splits the global
region into several local triangular regions (LTRs). The
advantage of this method is that the traversing cost of each
LTR can be evaluated in advance for path planning purposes.
In this paper we propose a new method to construct the cost
graph. The search over the map, is based on the D* search
algorithm presented in [22]. In this stage, uncertainty is incor-
porated in the cost function. Since continuity of path is crucial
for car type robots we choose continuous-curvature local paths,
proposed by Fraichard [23]. An improved feedback lineariza-
tion control algorithm is used to guide the car along this
computed reference path. The algorithm presented addresses
the singularity and instability problems in the original method
when only steering control is used [24]. Experimental results
are presented to demonstrate performance of the algorithms.
II. NAVIGATION AND MAP REPRESENTATION
Figure (1) shows a owchart of the tasks required to
implement real time path following in unknown environments.
The navigation box obtains data from sensors such as encoder,
laser, and GPS and provides path planners with vehicles
pose together with information about the surrounding area.
The SLAM algorithm used for navigation utilizes the Hy-
brid Metric Map (HYMM) structure for mapping [21]. This
approach can combine feature maps with any other metric
representation. When working with feature based maps, a set
of natural or articial landmarks are used to partition the
region covered by the map. These partitions are shown in
Figure (2) and consist of a set of connected local triangular
regions (LTR). Each LTR is dened by the position of three
landmarks called vertices. However, not all the landmarks are
needed as vertex points in the LTRs denition.
Any point that belongs to an LTR can be dened by a convex
linear combination of the three vertex points associated to that
sub-region. Similarly, any function of the global position can
also be locally dened as a function of its local representation.
These functions can be very accurate since the strong correla-
tion of the vertex points makes the position uncertainty of an
observer very low in this local frame. For instance, location of
Fig. 1. Flowchart of tasks in real time operation
Fig. 2. Local triangular regions and landmarks
an obstacle will be very accurate with respect to the relevant
LTR, although it might have a considerable error in the global
map. To take advantage of this fact all the properties required
for path planning such as obstacle presence, quality of the
ground, etc are represented in the local frame. Thus, a function
describing the cost of moving from one point to another can
then be dened based on local properties. This function will
remain constant if the local properties, shape and size of the
LTR do not change. This will be the case of a SLAM process,
where a set of close landmarks are strongly correlated and will
dene a triangle with shape and size well dened. Since the
LTRs are used in this work to implement the path planning
algorithm the uncertainty in map is inherently considered in
this process.
III. PATH PLANNING
Path planning involves the design and update of global and
local trajectories. The global path planner usually implements
an optimization algorithm to choose a set of consecutive
points in the global environment that lead the vehicle from
starting point to the destination point while avoiding obstacles.
The path is designed to be near-optimal according to a
certain criterion.
In this paper, we take into account uncertainty and
kinematics constrains of the vehicle. Uncertainty of the
70 60 50 40 30 20 10 0
70
60
50
40
30
20
10
0
10
East (metres)
N
o
r
t
h

(
m
e
t
r
e
s
)
Path Planner
Fig. 3. Quadtree representation in local triangular regions
vehicle pose at any possible location in the map is obtained
using an information map (IM) function. The information
map function species the amount of information that can
be obtained by positioning the vehicle at any location in the
map. The IM can be built with the information provided by
the SLAM algorithm. For most practical purposes the path
planner does not require an analytical function to represent
IM, but benets from having knowledge of the areas of good
information, that is a set of level sets, each representing areas
of at least a given information quality. The level sets will
increase their volumes in all directions of the space as the
SLAM map improves its quality and gains more information
from the environment [21].
In this paper we use the QuadTree concept in order to
discretize the map. Figure (3) shows a typical discretization
of the space. In this Figure the primary triangles represent the
LTRs provided by the SLAM algorithm. They are split into
four sub-triangles if there is obstacle inside. This process is
repeated until we get the minimum size triangle (maximum
grid resolution) that enclose part of the object. Finally the
sides of all the sub-triangles are split into small segments.
The segment points form nodes of the cost graph. We discard
those nodes in the list that are not on the perimeter of any
obstacle-embedded triangle.
Uncertainty in vehicles pose is included in the path planning
in the following form: The information distribution function
is used to select as uncertainty guides the middle point of
the triangles that are known to have good information. Then
the minimum normal and relative distance of the vehicle
to the guides are determined, Figure (4). Note that normal
distances to guides located behind or too far from the car
are not selected. If any of the distances is greater than a
corresponding threshold, then the path is penalized by an
augmentation factor. This is due to the fact that different
100 80 60 40 20 0 20 40 60 80 100
80
60
40
20
0
20
40
60
80
East (metres)
N
o
r
t
h

(
m
e
t
r
e
s
)
Path Planner
Relative Distance
Normal Distance
Guider
Uncertainty
Start
Destination
Fig. 4. An example of a path generated using QuadTrees representation and
A* search considering uncertainty
layers are independent from the probabilistic point of view. In
fact, the cost function is modied in the same manner for all
different property layers, where each layer represents value
of that property as a function of local frames components.
Kinematic constrains of the car is also incorporated in the
global path planning. Our experimental car can not reverse
autonomously and its turning angle is also bounded. In the
search for optimum neighbors only those neighbors that are
ahead of the line connecting previous pose to current pose
are considered.
The data structure built by the QuadTrees is searched by
D* algorithm to perform re-planning once new information is
acquired [22]. Results for a sample map in shown in Figure
(4) where the asterisks represent the uncertainty guides. From
this Figure it can be appreciated that the algorithm has found
a path that passes through information regions and at the same
time is near-shortest. In this case D* is equivalent to A* since
re-planning has not been implemented.
The waypoints dened by the global path planner are set as
local start and goal points. To connect these points, we use
the Continuous-Curvature path planner (CC-Steer) proposed
by Fraichard [23]. Since the algorithm is implemented in a
standard car, we have to consider the mechanical limit of steer-
ing angle as well as maximum possible rate of steering change.
The algorithm has two key parameters to take into account the
steering constrains. These parameters are: curvature K and
sharpness . They are related to steering angle and its rate by
K = tan/l ; =

K = (1 + tan
2
)

/(V l) (1)
where l is length of the car and V is velocity. Since the
only control input is steering angle, we chose a conservative
smaller value for the maximum steering angle to allow for
5 0 5 10 15 20 25
0
5
10
15
20
25
30
Start Point
XY Location
X (in meter)
Y

(
in

m
e
t
e
r
)
Fig. 5. Local path planner
unknown modeling errors.
According to the CC-Steer method, they are at most six
simple continuous-curvature paths between two congurations,
dened respectively as lsl, lsr, rsl, rsr, lrl and rlr. In this
case, s denotes a line segment and l or r denote a continuous-
curvature turn to the left or right. Figure (5) shows a path
generated using CC-Steer method for the following waypoints.
_
_
_
x
y

_
_
_
=
_
_
0 10 20
0 15 30
0 90 0
_
_
(2)
where x, y (in meters) and (in degrees).
The area that is swept by the car while moving from one
point to another should also be computed. This is essential in
order to avoid forbidden zones. We simplify these calculations
by evaluating the worst case conditions. Figure (6) shows
the sweeping area for two typical types of local paths used
to connect global waypoints. Part (a) consists of left turn,
straight line, and right turn (lsr) and part (b) includes the
sequence left, right and left turns (lrl). The vehicle starts from
point A and ends at point B with a different orientation. In
practice, the path will have some deviation from the straight
line AB. In this work we consider the rectangle parallel to AB
and circumference to the path as the sweeping area, which is
related to location of circles centers and maximum curvature.
Figure (7) shows the continuous-curvature path after tting
local paths between waypoints of Figure (4).
IV. GUIDANCE
We use Feedback Linearization theory for the guidance of
the car. The following change of coordinates is used [24]
x
1
= x
x
2
= tan /(l cos
3
)
x
3
= tan
x
4
= y
(3)
where, x and y represent the 2D position, is the steering
angle and is the vehicle orientation. This results in the fol-
Fig. 6. Sweeping area in a local path, (a)lsr, (b)lrl
100 80 60 40 20 0 20 40 60 80 100
80
60
40
20
0
20
40
60
80
East (metres)
N
o
r
t
h

(
m
e
t
r
e
s
)
Path Planner
Start
Destination
Fig. 7. The continuous-curvature path generated using QuadTrees represen-
tation and A* search considering uncertainty
lowing canonical form representation of the motion equations
x
1
= u
1
x
2
= u
2
x
3
= x
2
u
1
x
4
= x
3
u
1
(4)
when
u
1
= v
1
cos
u
2
= (v
2
+
3sin sin
2
u1
l cos
2

)/(l cos
3
cos
2
)
(5)
v
1
and v
2
are velocity and steering rate, respectively. In
path following, the only control input is steering angle since
velocity control is not implemented. Assuming the desired
states are
x
d
= (x
d1
, x
d2
, x
d3
, x
d4
) (6)
The control input command that linearize the model is
u
2
= k
1
|u
1
(t)| x
4
k
2
u
1
(t) x
3
k
3
|u
1
(t)| x
2
(7)
where
u
2
= u
d2
u
2
; x
i
= x
di
x
i
i = 2, 3, 4 (8)
The index d means desired states. The gains k
i
are chosen
to satisfy the Hurwitz stability criterion. In particular by
assigning three coincident eigenvalues at |u
1
| (with > 0),
we can solve for the three gains as [24]
k
1
=
3
, k
2
= 3
2
, k
3
= 3 (9)
The main problems of this algorithm is the singularity at
orientation of 90 degree and unstability in the of x component.
This is due to the fact that velocity is not a control input. To
address these two problems a new transformation is dened
y
1
= y
y
2
= tan /(l sin
3
)
y
3
= 1/ tan
y
4
= x
(10)
This transformation yields the following motion equations
y
1
= u

1
y
2
= u

2
y
3
= y
2
u

1
y
4
= y
3
u

1
(11)
where
u

1
= v
1
sin
u

2
= (v
2

3cos sin
2
u

1
l sin
2

)/(l sin
3
cos
2
)
(12)
In this case the control input is
u

2
= k
1
|u

1
(t)| y
4
k
2
u

1
(t) y
3
k
3
|u

1
(t)| y
2
(13)
The switching policy is designed is such a form that when
cos approaches to zero the Y model becomes active. The
error of the uncontrollable state can also be reduced with
appropriate selection of the switching time. The oating gains
technique is used to take into account the steering constrains.
This is implemented by adjusting the three gains k
1
to k
3
to
keep v
2
below its limit value.
V. EXPERIMENTAL RESULTS
The experimental tests are performed using a utility car
equipped with a Sick laser range and bearing sensor, GPS,
linear variable differential transformer (LVDT) sensor for the
steering and back wheel velocity encoder. A photo of the
vehicle is shown in Figure (8). More information regarding
modeling aspect and other experimental information is avail-
able in Ref. [25]. The actual steering limit of this vehicle is
35 degrees in both sides. However, we used a conservative
value of 20 degrees to account for other non-modeling errors.
Then the value of the sharpness parameter becomes 0.08
(deg/msec), being 0.2 (deg/msec) the maximum value.
Figure (9) presents the result for the path shown in Figure
Fig. 8. The utility car used for the experiments
0 2 4 6 8 10 12 14
1
0.5
0
0.5
1
1.5
2
Error Vs. Time
E
r
r
o
r

x
/
y

(
m
e
t
e
r
)
Time (sec.)
Error X
Error Y
X Based
Y Based
Fig. 9. Result for a path with initial offset
(5). It is assumed that at the beginning the vehicle position
has 1 and -2 meter error along x and y axes, respectively. The
value of the controller parameters are listed below
v = 3 m/s , , l = 2.83 m
K
max
= 0.128 1/m ,
max
= 0.0883 1/ms
(14)
Figure (10) shows the actual steering commands versus time
needed to follow the reference path while eliminating the
errors. The steering curve for the ideal path (without any
initial error) is also shown. As can be seen from the gure,
the proposed algorithm has no longer singularity problems.
Furthermore, it can eventually remove both x and y errors. In
this experiment, we use the X-Based system while orientation
is less than 45 degrees and the Y-Based system otherwise. The
error in the uncontrollable states (y if using Y model and x if
using X model) can be reduced adjusting the switching time
according the orientation of the vehicle and actual error.
0 2 4 6 8 10 12 14
30
20
10
0
10
20
30
40
Steering Vs. Time
S
t
e
e
r
i
n
g

(
d
e
g
.
)
Time (sec.)
X Based
Y Based
Reference
Fig. 10. Actual and reference steering commands
VI. CONCLUSION
In this paper we presented incremental path following
and guidance algorithms for a car-like robot while consid-
ering kinematics and dynamic constrains and uncertainties
in vehicle pose and map. The navigation algorithm uses an
HMM representation to exploit the benets of both, efcient
feature based localization and rich occupancy grid information.
The D* search algorithm was modied in order to consider
uncertainty in the cost function. This resulted in a global path
planner that nds the near-optimal path in terms of length,
path feasibility, and uncertainty. The points selected by the
global path planner are connected by the local continuous
curvature curves. Feedback linearization technique is used to
guide the car along the reference path. Two transformations are
presented to overcome the singularity and instability problems
for car type vehicles.
VII. ACKNOWLEDGMENTS
This work is supported by the ARC Centre of Excellence
programme, funded by the Australian Research Council (ARC)
and the New South Wales State Government. The real time
software was developed using QNX Momentix Integrated
development Environment.
REFERENCES
[1] A. Yahja, S. Singth and A. Stentz, Ef cient On-Line Path Planner for
Outdoor Mobile Robots, Robotics and Autonomous Systems, 32(2): pp.
129-143, 2000.
[2] A. Stentz, CD*: A Real-Time Resolution Optimal Re-Planner for Globally
Constrained Problems, American Association for Articial Intelligence
:http://www.frc.ri.cmu.edu/ axs/doc/aaai02.pdf, 2002.
[3] H. Choset and J. Burdick, Sensor-Based Exploration: The Hierarchical
Generalized Voronoi Graph, International Journal of Robotics Research,
19(2): pp. 96-125, 2000.
[4] K. Nagatani, Y. Iwai and Y. Tanaka, Sensor Based Navigation for Car-
Like Mobile Robots Using Genralized Voronoi Graph, IEEE Int. Conf. on
Intelligent Robots and Systems, Hawaii, USA : pp. 1017-1022, 2001.
[5] L. E. Kavraki, P. Svestka and M. H. Overmars, Probabilistic Roadmaps for
Path Planning in High-Dimensional Con guration Spaces, UU-CS (Ext.
r. no. 94-32), Utrecht, 1994.
[6] P. Svestka, A Probabilistic Approach to Motion Planning for Car-Like
Robots, RUU-CS (Ext. r. no. 93-18), Utrecht, 1993.
[7] S. LaValle and J. Kuffner, Randomized Kinodynamic Planning, Interna-
tional Journal of Robotics Research, 20(5): pp. 378-400, 2001.
[8] D. Hsu, R. Kindel, J. C. Latombe and S. Rock., Randomized Kinody-
namic Motion Planning with Moving Obstacles, International Journal of
Robotics Research, 21(3): pp. 233-255, 2002.
[9] E. Frazzoli, M. A. Dahleh and E. Feron., Real-Time Motion Planning
for Agile Autonomous Vehicles, Journal of Guidance, Control, and
Dynamics, 25(1): pp. 116-129, 2002.
[10] H. Takeda, C. Facchinetti and J. C. Latombe, Planning the Motions of
a Mobile Robot in a Sensory Uncertainty Field, IEEE Transactions on
PAMI, 16(10): pp. 1002-1017, 1994.
[11] N. Le Fort-Piat,I. Collin and D. Meizel, Planning Robust Displacement
Missions by Means of Robot-Tasks and Local Maps, Robotics and
Autonomous Systems, 20(1): pp. 99-114, 1997.
[12] B. Bouilly, T. Simeon and R. Alami, A Numerical Technique for
Planning Motion Stategies of a Mobile Robot in Presence of Uncertainty,
IEEE Int. Conf. on Robotics and Automation, Japan: pp. 1327-1332,
1995.
[13] A. Lambert and N. Le Fort-Piat, Safe Task Planning Integrating Uncer-
tainties and Local Maps Federations, Int. Journal of Robotics Research,
19(6): pp. 597-611, 2000.
[14] A. Lambert and Th. Fraichard, Landmark-Based Safe Path Planning for
Car-Like Robots, IEEE Int. Conf. on Robotics and Automation, San
Francisco, CA: pp. 2046-2051, 2000.
[15] J. Guivant and E. M. Nebot, Optimization of the Simultaneous Localiza-
tion and Map Building Algorithm for Real Time Implementations, IEEE
Trans. on Robotics and Automation, 17(3): pp. 242-257, 2001.
[16] J. Guivant and E. M. Nebot, Improved Computational and Memory
Requirements of Simultaneous Localization and Map Building, IEEE Int.
Conf. on Robotics and Automation, Zurich, Switzerland: pp. 2731-2736,
2002.
[17] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, Fastslam: A
Factored Solution to the Simultaneous Localization and Map Building,
American Association for Articial Intelligence, 2002.
[18] J. J. Leonard and H. J. S. Feder, A Computationally Ef cient Method for
Large-Scale Concurrent Mapping and Localization, Ninth International
Symposium on Robotics Research, Utah: pp. 316-321, 1999.
[19] M. Bosse, P. M. Newman, J. J. Leonard and S. Teller, An Atlas
Framework for Scalable Mapping, MIT Marine Robotics Laboratory
Technical memorandum 2002-04, http://oe.mit.edu/ jleonard/index.html.
[20] T. Bailey, Mobile Robot Localisation and Mapping in Extensive Outdoor
Environments, PhD Thesis, ACFR: University of Sydney, 2002.
[21] J. Guivant, J. Nieto, F. Masson and E. Nebot, Navigation and Mapping
in Large Unstructured Environments, Accepted Int. Journal of Robotics
Research,
[22] A. Stentz, Optimal and Ef cient Path Planning for Partially-Known
Environments, IEEE Int. Conf. on Robotics and Automation Munich,
Germany: pp. 3310-3317, 1994.
[23] Th. Fraichard, Continuous-Curvature Path Planning for Car-Like Vehi-
cles, IEEE Int. Conf. on Intelligent Robots and Systems, Grenoble,
France : pp. 997-1003, 1997.
[24] A. De Luca, G. Oriolo and C. Samson, Feedback Control of a Nonholo-
nomic Car-like Robot, Robot Motion Planning and Control, edited by
J.-P. Laumond: Springer-Verlag, 1998.
[25] E. Nebot, J. Guivant and J. Nieto, Experimental data set,
http://www.acfr.usyd.edu.au/homepages/academic/enebot/dataset.htm.