Robotics and Autonomous Systems 60 (2012) 252–265

Contents lists available at SciVerse ScienceDirect

Robotics and Autonomous Systems

journal homepage: www.elsevier.com/locate/robot

Smooth path and speed planning for an automated public transport vehicle
Jorge Villagra ∗ , Vicente Milanés, Joshué Pérez, Jorge Godoy
AUTOPIA Program, Centre for Automation and Robotics (CAR, UPM-CSIC), Carretera de Campo Real, km 0.200, 28500 La Poveda, Arganda del Rey (Madrid), Spain

article info abstract

Article history: This paper presents a path and speed planner for automated public transport vehicles in unstructured
Received 17 May 2011 environments. Since efficiency and comfort are two of the key issues in promoting this kind of
Received in revised form transportation system, they are dealt with explicitly in the proposed planning algorithm. To that end,
26 October 2011
a global path planner has been designed with bounded continuous curvature and bounded curvature
Accepted 4 November 2011
Available online 20 November 2011
derivative to ensure smooth driving. This will allow the public transport system to know a priori which is
the shortest path within a selected area that guarantees lateral accelerations and steering wheel speeds
Keywords:
below given pre-set thresholds. A closed-form speed profiler uses semantic information provided by the
Path planning path planner to set a continuous velocity reference that takes into account not only bounds on lateral and
Speed planning longitudinal accelerations consistent with comfort, but also a bound on longitudinal jerk. The suitability
Automated vehicles of the above two features was compared to manual driving in a real instrumented public transport vehicle
on a test track.
© 2011 Elsevier B.V. All rights reserved.

1. Introduction However, although motion planning for autonomous vehicles

has been studied in some detail over the last few decades, the
The growth of private car use involves increased carbon dioxide requirement of producing trajectories with bounded lateral and
emissions and frequency and severity of traffic jams,1 especially in longitudinal accelerations and jerk has received little attention in
urban environments. Indeed, only 21% of EU27 citizens use public the literature. The approach to solving this problem taken here is
transport as their main mode of transport, with the irregularity twofold: we describe on the one hand a suboptimal path planner
of the schedules being the main reason for its not being a more that is capable of providing the most efficient route consistent
popular option in towns (cf. [2]). with given comfort and safety requirements, and on the other,
Given this context, the EU decided in 2009 to adopt an action an automatic speed profile generator that takes into account
plan for urban mobility in which public transport, and specifically the previously planned path and some additional drivability
the use of electric vehicles, is the main key to improving urban constraints.
mobility [3] because of their efficiency, lack of CO2 emission,
safety, and comfort. In this connection, fully automated passenger 1.1. State of the art
vehicles are expected to soon be extensively used in urban areas,
airport terminals, hospital complexes, university campuses, and
Path planning for autonomous robots or vehicles has been ex-
pedestrian zones. While these electric public transport vehicles
tensively studied in recent years to meet a variety of kinematic,
will be given traffic priority to encourage citizens to use them,
dynamic, and environmental constraints. General techniques to
this will not be sufficient to ensure the success of these systems.
obtain optimal trajectories can be grouped into two categories: in-
It will also be necessary to appropriately select the route for the
direct and direct. Indirect techniques discretize the state/control
vehicle to cover to improve efficiency and reduce users’ waiting
variables, and convert the trajectory problem into one of parame-
and travel times. To this end, both an accurate and robust control
system and a time-optimal path selection algorithm are essential ter optimization which is solved via nonlinear programming [4,5]
for full advantage to be taken of these transportation systems. or by stochastic techniques [6,7]. The latter use Pontryagin’s max-
imum principle (PMP) and re-express the optimality conditions as
a boundary value problem [8].
The complex and incomplete nature of most indirect techniques
∗ Corresponding author. has motivated the development of optimal control techniques
E-mail addresses: jorge.villagra@car.upm-csic.es, jorge.villagra@csic.es
(J. Villagra), vicente.milanes@car.upm-csic.es (V. Milanés),
using PMP solutions. These have been based on Dubins’ pioneering
joshue.perez@car.upm-csic.es (J. Pérez), jorge.godoy@car.upm-csic.es (J. Godoy). work [9] which presented the first set of paths (with straight
1 According to the EU green paper for urban mobility [1], the EU economy lost line segments and arcs of circles) constrained to go from point
around 100 billion Euros due to delays in traffic jams. to point with given initial and final orientations in a minimal
0921-8890/$ – see front matter © 2011 Elsevier B.V. All rights reserved.
doi:10.1016/j.robot.2011.11.001
 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265 253

time. In this context of path planning in a free environment, Finally, since one is not only looking for an adapted path
there have been several extensions of Dubins’ result. Reeds and planner but also a speed profile generator, a link between the
Shepp [10] generalized it to a forward/backward moving car, and two tasks would be desirable. In this connection, a semantic
other approaches have been proposed to solve time-optimal path- interpretation of the path can be useful both to help the driver
planning problems with linear/angular velocity bounds using line or co-pilot4 understand the path to be followed and to reduce the
segments and circles (cf. e.g., [11–15]). computational cost of calculating the best speed for each situation.
A first analysis of these PMP results suggests that the resulting As noted above, smoothness requirements impose constraints
maneuvers are ‘‘bang–bang’’ trajectories, and therefore that, when on both the path and the speed profile. The speed reference
implemented on real wheeled robots, there would be at least is commonly assumed to be continuously differentiable, and is
one discontinuity in the path’s curvature profile at circle–segment often designed by optimizing an appropriate performance index
transitions. To avoid this problem, several studies have been aimed (minimum time is the commonest criterion, but minimum jerk has
at obtaining ‘‘nearly time-optimal paths’’ which correspond to also been used [32]).
smoother trajectories than those provided by Dubins’ curves. Even though several interesting solutions have been presented
Kanayama and Hartman [16] showed that using cost functions in the literature (e.g. [33–35]), they all suffer from the same
problem. Since a topological semantic map is not used in any
such as the integral over the square of either the curvature itself or
case, iterative or optimization processes are required to satisfy a
the curvature’s derivative leads to concatenations of clothoids2 or
certain number of driving comfort constraints — maximum speed,
of cubic spirals, respectively.
longitudinal and lateral acceleration, and jerk.
In this connection, Fraichard and Scheuer [17] uses the set
In this connection, smooth trajectory generation has been one
of optimal curves described in Sussman’s paper (straight line
of the main concerns for accurate robot control. Thus, the studies
segments, arcs of circles, and clothoids) to implement an algorithm
of [36–38] present closed-form solutions — via different families
based on families of Dubins’ curves, modifying simple turns into
of polynomials — to plan a speed profile guaranteeing upper limits
continuous-curvature turns (with the aid of the clothoids). With on accelerations and jerks.
this strategy, the curvature profile along the path is continuous and The present work describes a global solution both to the
trapezoidal in shape. However, the path presents discontinuities smooth path planning problem for autonomous vehicles in
in the steering wheel angular velocity in each transition between unstructured environments,5 and to the generation of an optimal
circular arc and straight line segment generated by clothoids. speed profile for the resulting path. The solution to the former
Various workers interested in path planning for very high speed problem is based on the combination of the optimal continuous
vehicles have therefore proposed alternative fundamental curves, curvature path planner proposed by Fraichard and Scheuer [17]
namely: to deal with obstacle-free environments, and the probabilistic
• Curves whose coordinates have a closed expression: B-splines roadmap for path planning introduced by Kavraki et al. [30]
that is responsible for efficiently finding the minimum number
[18], quintic polynomials [19], polar splines [20], cardioids [21],
of landmarks necessary to cover an obstacle-free space. The
G2 -splines [22,23], η3 -splines [24], and Bézier curves [25].
speed profile is determined from the information provided by the
• Curves whose curvature is a function of their arc length:
path planner and an adaptation of the polynomial-based smooth
clothoids [26], Kanayama & Hartmann curves, and intrinsic
trajectory generation of [37].
splines [27].
Even though some of these approaches provide greater smooth- 1.2. Contribution
ness than the solution given by Fraichard and Scheuer [17], most
make use of an optimization algorithm whose completeness and With the above premises, the main contributions can be sum-
topological admissibility is not always guaranteed. Moreover, since marized as follows:
their main concern is smoothness, the resulting paths are some- • A global path planner with continuous bounded curvature and
times far from time optimal. curvature derivative aimed at providing smooth driving. This
One of the interesting features of [17] is that not only is the planner will allow the public transport system to know a
curvature bounded but also the curvature derivative is constrained priori which is the shortest path within a selected area that is
as pre-set by the user. This allows one to compensate the derivative consistent with a maximum lateral acceleration and steering
continuity limitation for vehicles running at a moderate speed wheel speed. Moreover, once the global planning learning
such as in urban public transport systems. In this connection, phase has been completed, any pair of start and end points of
this path planner may explicitly adapt the vehicle’s technological the selected circuit will be quasi-instantaneously connectable
(maximum steering wheel speed) and comfort (maximum lateral by the path planner.
acceleration) constraints to its design parameters. • A closed-form speed profiler, intelligently combined with the
Furthermore, there has as yet been little attention paid to path planner, will provide a continuous velocity reference that
combining smooth path planning with obstacle avoidance. Work takes into account comfort constraints not only on lateral and
which has addressed this issue includes ‘‘elastic band’’ [28] and longitudinal accelerations, but also on longitudinal jerk.
‘‘rapidly-exploring random tree’’ [29] techniques in successfully • The suitability of the above two features was evaluated with a
generating paths for mobile robots or vehicles with obstacle real instrumented public transport vehicle on a test track. To
avoidance and dynamic planning. However, these techniques share that end, several manual driving tests were compared with the
two main drawbacks: the shortest path (or a solution close to it) planning algorithm in terms of time and comfort.
cannot always be guaranteed, and the computational burden is
not re-usable to search for time-optimal paths within the selected
area.3 4 The term co-pilot has been introduced into the AUTOPIA Architecture for
Automatic Driving [31] to refer to the intelligent decision system that chooses from
among the set of available maneuvers.
5 A structured environment in this sense is an area for which a geometrical
2 A clothoid is a curve whose curvature is a linear function of its arc length s such
road network description is available, while for unstructured environments such
that κ = σ sr , where σ is a real constant called the sharpness of the clothoid. as parking lots, loading zones, and off-road areas, no such semantic description is
3 Only graph-based methods (e.g. [30]) can be used for this purpose. available.
254 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265

Fig. 1. Configuration of the obstacle-free space used on the AUTOPIA private circuit. (a) Circuit C1 ; (b) Circuit C2 . (For interpretation of the references to colour in this figure
legend, the reader is referred to the web version of this article.)

The remainder of the paper is structured as follows. Section 2 represented by circuit C1 , in blue in Fig. 1. In order to generalize the
presents the formalism of the problem that is to be tackled, and results for such a problem, a more complete circuit C2 (red path in
gives an overview of the solution adopted which is, as mentioned Fig. 1) has also been evaluated to reproduce a real urban scenario
above, based on a global planner that makes use of a continuous faced by public transportation systems. As can be observed, circuit
and bounded curvature metric. These characteristics of the local C2 combines the unstructured zone with the roundabout and
path planner will be highlighted in Section 3, and the global several U-turns.
planning algorithm will be described in 4. Some of the results will
be compared with real driving tests and discussed in Section 6. 2.1. Proposed solution
Finally, Section 7 presents some concluding remarks.
In the path planning literature, it is usual to solve the problem
2. Statement of the problem at hand by decomposing it into two hierarchical stages: a local
algorithm computes a path between two configurations in the
Given a start and end point configuration, the specific path and absence of obstacles. From that result, a global motion planning
speed planning problem we face is to find a path Γ and a speed scheme (e.g., [30] or [39]) is used to deal with the obstacles and
profile Σ such that: solve the full problem. In these schemes, the local planner is
used together with a collision checker to connect pairs of selected
1. Γ satisfies the extreme point conditions of Γ0 and Γe . configurations.
2. Γ respects local path planner constraints, i.e., kinematic and Fig. 2 depicts how the local and global algorithms co-operate to
dynamic vehicle constraints, as will be detailed in the local path find a solution to the aforementioned problem. The family of paths
planner section. used in the present work was based on the results of [17] which
3. Γ is obstacle free, i.e., it is entirely included in the free configu- employed a straight line segment, circular arc, clothoid metric
ration space which will be assumed static. (upper block of Fig. 2). If the resulting path is not obstacle-free
4. Σ respects dynamic comfort constraints (maximum speed, ac- and the initial and final configurations are still unconnected — see
celeration, and jerk). configurations 1–3 of the bottom block in Fig. 2 — then a global
5. Σ and Γ provide the time-optimal trip solution — if one exists
planner inspired by the Probabilistic Path Planner (PPP) [30] is
— connecting the extreme points, while respecting the above
used to intelligently select a new intermediate configuration. One
constraints. of the main features of the proposed planner is that this phase of
With these premises, a test-bed circuit was selected on the learning the obstacle-free configuration space will be available for
facilities of the Centre for Automation and Robotics to evaluate any future query within the same area.
the proposed algorithm. There are two types of test track in Finally, the semantic map provided by the local planner will
these facilities (see Fig. 1): a typical urban area with straight be exploited to simplify the generation of the optimal speed
stretches, U-turns, and a roundabout that has a very precise and profile. To this end, we adapted work by Liu [37] to our specific
semantically interpretable geometry; and a more generic and trajectory planning — see the block on the right of Fig. 2 — imposing
not easily classifiable (i.e., an unstructured) circuit. The present the longitudinal maxima of speed, acceleration, and jerk on the
work focuses on path and speed planning for the latter,6 which is vehicle’s motion for stretches in which there are no turning lateral
dynamics.

6 Note that a white rectangle appears on the right side of the red-framed aerial 3. Local planner
view. This is a bus that was left in the same position throughout the trials. The
external circuit used for the path planning problem — on the right of Fig. 1 — was The car-like mobile robot considered has a body frame attached
retraced to avoid any potential collision with this obstacle. to a reference point with coordinates ξ = (x, y)T in a global
 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265 255

Fig. 2. Path and speed planning scheme.

dκ
coordinate frame, and orientation θ with respect to the global Consider κmax the maximum curvature and σmax = ds |max
r
frame. The generalized coordinates of this wheeled mobile robot the maximum curvature derivative with respect to the arc length.
are therefore (x, y, θ). The vehicle is equipped with a front- From (2) and (3), it is straightforward to obtain
centered steerable wheel (whose angle is represented by φ ) and
fixed parallel rear wheels that move with velocity v . With the tan φmax
tan φ(t ) = Lκ(t ) ⇒ κmax = (4)
assumption of small velocities,7 the dynamics of a car having a L
wheelbase of length L can be described by dκ dκ dsr φ̇


= ⇒ σmax = 2
 . (5)
v cos θ dt dsr dt s˙ L cos φ 
   
ẋ 0 0 r max
   
 ẏ   v sin θ  0 0
  v   
 θ̇  =  tan φ  + 0 ω + 0 γx (1) Proposition 1. A path Γ in the {x, y} plane is generated by
   
φ̇   L  1
 0 model (1) with input ω(·) ∈ C 1 , ω ∈ [−ωmax , ωmax ], and V (·) ∈
0
v̇ 0 0 1 C 1 V ∈ [0, Vmax ], ∀t ≥ 0, if and only if its curvature κ ∈ C 1 , κ ∈
[0, κmax ] and its curvature derivative σ ∈ [−σmax , σmax ].
where ω is the angular velocity, and γx is the longitudinal accel-
eration. Note that the steering angle, steering speed, longitudinal A sketch of the proof follows. Given any C 2 -curve p(u) with u ∈
1
velocity, and acceleration are subject to the constraints |φ| ⩽ φmax , [u0 , u1 ], the inverse arc length function s−
r is defined, and is, by
|ω| ⩽ ωmax , v ⩽ Vmax , |γx | ⩽ γxmax , respectively. As shown in [40], definition, a continuous function. Moreover, the scalar curvature
the kinematic model of the wheeled mobile robot given by the κ(u) is also continuous over [u0 , u1 ] because p(u) is a C 2 -curve.
three first equations of (1) is flat with flat outputs {x, y}. As a con- Consider the curvature bound to be such that the steering angle is
sequence of this, any state or input variable can be expressed in |φ| ⩽ φmax . At the initial time t0 , consider the state of model (1)
terms of the {x, y} and their derivatives: given by [x(u0 ) y(u0 ) θ(u0 )]T . Then, applying the continuous input
1
ω(t ) = v(t )κ(s−
r (v(t − t0 ))) (6)
 
ẏ(t )
θ(t ) = arctan
ẋ(t ) s(u1 )
the vehicle’s motion from t0 to t0 + v exactly matches the path
of the given curve if |ω| ⩽ ωmax , v ⩽ Vmax , |γx | ⩽ γxmax . Moreover,

v(t ) = ẋ2 (t ) + ẏ2 (t )
  if κ(u) ∈ C 1 and v(u) ∈ C 1 , then ω(u) ∈ C 1 .
L(ẋ(t )ÿ(t ) − ẍ(t )ẏ(t ))
φ(t ) = arctan . (2)
(ẋ(t ) + ẏ(t ))3/2 Remark 1. Although model (1) satisfactorily represents a wheeled
mobile robot’s behavior, public transport vehicles driving in urban
We shall use that the arc length sr of a planar curve {x, y} can
areas exhibit nonlinear dynamics and are affected by many
be expressed as
disturbances, such as turning and static friction, or variations in the
f : [u0 , u1 ] → [0, f (u)], vehicle’s weight. Since these poorly known effects are difficult to
∫ u  model and include in the path planner, a robust, non-model-based,
u → sr = ẋ(ξ )2 + ẏ(ξ )2 dξ control system (e.g. [41] or [42]) should be used to precisely track
u0 the resulting path.
and that the scalar curvature is, according to the Frenet formulas,
a function of {x, y} and their derivatives Remark 2. Note that the lateral acceleration γy of a planar non-
slipping vehicle is equal to v θ̇ (cf. [43]), which combined with (2)
ẋ(t )ÿ(t ) − ẍ(t )ẏ(t ) and (3), and considering small turning radius yields γy = v 2 κ .
κ(t ) = . (3)
(ẋ(t )2 + ẏ(t )2 )3/2 As mentioned above, Fraichard and Scheuer [17] propose an
efficient solution that generates suboptimal paths with an upper
bounded continuous curvature and an upper bounded curvature
derivative. The resulting path will consist of straight line segments,
7 When small velocities are considered, the assumption of rolling without circular arcs, and clothoids, which will be automatically combined
slipping can naturally be taken to hold. to connect configurations with null curvature. Even if the algorithm
256 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265

Fig. 3. (a) Path between starting configuration A and final configuration B with κmax = 0.8 and σmax = 0.3; (b) curvature versus arc length.

does not strictly compute minimal length paths, it can be shown The exploration of the environment is done by successively
(cf. [17]) that it provides paths whose length is as close as adding to the graph a random configuration pi — a data set
desired to the optimal ‘‘Dubins’’ car paths. In fact, when the consisting of xi , yi , θi — and trying to directionally connect this
curvature derivative bound tends to infinity, the paths computed configuration to a maximum number of nodes of the graph with
by Fraichard and Scheuer [17] tend to the optimal Dubins paths. the local path planner.
If the sub-optimal path exists, the local planner will provide the As the aim of the planner is to find the shortest possible path,
set of parameters that defines that path (i.e., circle radius centers, the graph weight J will be the Fourier transform of the path
length, and deflection,8 and if necessary, segment lengths). The curvature — in other words, a measure of the frequency of the
basic steps of the algorithm are the following. steering angle motion. This weighting variable was chosen instead
of the classical path length criterion because the configuration
Algorithm 1 Local path planner algorithm. space is too narrow to allow any considerable reduction in path
1: Compute all 6 Dubins [9] optimal length path possibilities: lrl, length so it seems more reasonable to maximize the passengers’
rlr, lsr, rsl, lsl, rsr, where left turns (l), right turns (r), and/or comfort.
straight line segments (s) are concatenated. We modified the original path planner [30] slightly in order to
2: Transform each turn in the above paths into a clothoid/circular- achieve better graph connectivity over complicated access zones.
arc/clothoid set or, in the case of a degenerate turn, into a To that end, two main refinements were introduced into the initial
clothoid–clothoid concatenation — when the turn deflection is algorithm:
very small. • Increase the probability of generating intermediate points in
3: Evaluate the length of each of the resulting continuous sensitive areas by assigning priorities related to the search
curvature paths and take the shortest solution. zones geometric characteristics, i.e., Pz ∈ [0, 1], with 1 being a
4: Verify that the selected path is obstacle-free, i.e., that it is critical zone and 0 a very easily connectable zone. To automat-
entirely included in the test-bed’s free configuration space ically set the value of Pz , a fuzzy decision system, based on the
(a classical algorithm will be used to detect whether the curvature, the surrounding curvature and configuration space
trajectory lies inside a motion polygon [17]). width of the node, has been implemented. Fig. 4 shows how
from a curvature κ , curvature integral 1κ and width W map-
Fig. 3 shows the path between two given configurations pings of the road, a fuzzy function Pz = f (κ, 1κ, W ) ∈ [0, 1] is
using the local planner. Note that, as required, the curvature is inferred for the whole selected circuit.
continuous, and there are upper bounds to the curvature (κmax = • Decrease the probability of obtaining intermediate points in a
0.8) and curvature derivative (σmax = 0.3). The path generated is given area – characterized by a ball of center pi and radius r
a concatenation of clothoids, straight line segments, and circular Br (pi ) — when many configurations pi = [xi , yi , θi ]T have al-
arcs. ready been tried in this area, and the connectivity rate is suffi-
It is important to remark that this path planner is complete ciently high. To that end, the following function Pd ∈ [0, 1] is
(i.e., it can connect any pair of configurations) and feasible for a used
mobile robot modelled as in (1).
 
n
− kd
Pd = max 1 −  ,0 ,
4. Global planner j=1 (xi − xj )2 + (yi − yj )2
p1 . . . pn ∈ Br (pi ).
The global planning algorithm proceeds in two steps: the
learning phase and the query phase. A third step, the expansion Thus, for a new node pi , the local planner tries to connect it
phase, could be inserted between the two. with each node belonging to the graph whose weight J is less
In the first phase, a probabilistic technique is used to construct than a pre-set value Jmax . However, if the resulting associated
a directional weighted graph whose nodes correspond to obstacle- value Pt = Pz + Pd is lower than the most adapted heuristic
free configurations and whose edges correspond to feasible paths value Pc = 1, the new node c is not included in the graph, and
between configurations (only path lengths will be stored). This a new node is generated until it satisfies the previous condition.
graph is represented by an n × n adjacency matrix Aadjn , where n This process continues until a graph connectivity condition has
is the number of nodes, and a vector ACoordn of the coordinates of been fulfilled (for instance, to obtain at least one solution to a
those nodes. specific query) and a fixed number of nodes has been reached.
This selective process leads to a significant reduction in the
number of intermediate points needed to attain a given degree
8 Difference in the orientations of the start and end points of a turn. of connectivity (Fig. 5 and Table 1). Note in the figure that,
 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265 257

Fig. 4. Automatic selection of the probability of generating intermediate points in sensitive areas. (a) Road curvature map; (b) road curvature derivative map (c) road width
map; (d) Pz map.

Fig. 5. Refinements for the intermediate node selection: (a) graph with nmax = 40 purely random nodes; (b) graph with nmax = 40 prioritized nodes.

Table 1 nodes whose Pz values is lower than 0.5 — easy access zones — and
Learning phase refinements. higher than 0.5 — complicated access zones. Note that the refined
GDPz ⩾0.5 GDPz <0.5 UNRPz ⩾0.5 UNRPz <0.5 algorithm not only provides better results in terms of density, but it
C1 C2 C1 C2 C1 C2 C1 C2 also significantly increases the UNR values over complicated zones,
Random 0.051 0.042 0.108 0.083 0.566 0.672 0.433 0.328 which allows the algorithm to improve connectivity with relaxed
Refined 0.085 0.069 0.327 0.267 0.825 0.765 0.175 0.235 curvature constraints.
As can be appreciated in Algorithm 2, if both of the above
conditions are fulfilled, the learning phase ends. Otherwise, a
while with purely random configurations there are two large zones
connectivity refinement process is initiated. To this end, forward
that remained unconnected, the prioritized algorithm provides and backward unconnected nodes are automatically detected, and
much better connectivity with the same number of nodes. The connections of the latter with the closest nodes in configuration
results for this specific example can be quantified and generalized, space are sought by iteratively using different curvature and
as shown in Table 1, with the mean results obtained after 10 curvature derivative bounds.
different learning phases for each one of the two circuits -C1 As mentioned in the introduction, one of the interesting
and C2 - presented in Section 2. To quantify the enhancements features of this solution is that this learning phase can be done off-
provided by the proposed refinements, two different metric have line, because it only depends on the environment, not on the query
been introduced: the Graph Density9 (GD) and the Unconnected configurations.
Nodes Ratio (UNR).10 Both indexes are separately evaluated for In the query phase, given start and final configuration are
connected to the two closest nodes of the graph using the local
planner, and then a graph search is performed between these
9 The density of a graph is the ratio of the number of edges and the number of two nodes. This step is carried out with the aid of the Dijkstra
possible edges. algorithm [44] which finds the least-cost (the smoothest in the
10 UNR is defined as the number of unconnected nodes in a particular region with present case) path from a single source node to all other nodes in
respect to the overall number of unconnected nodes in the graph. a weighted and directed graph.
258 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265

Fig. 6. Global path planner refinements: (a) plot of nodes and links representing the adjacency matrix; (b) feasible paths; (c) curvature profile of the feasible paths;
(d) Selected curvature profile. (For interpretation of the references to colour in this figure legend, the reader is referred to the web version of this article.)

Algorithm 2 Learning algorithm of the global path planner. solved using an overall path made up of several elementary paths
1: while c = 0 | i < nmax do (generated by the local planner) will be solved, provided that
2: while Pt (pi ) <= Pc do the exploration is carried out for a sufficient amount of time.
3: (pi , Pt (pi )) = random configuration Moreover, [45] proved that under certain geometric assumptions
4: end while on the free configuration space — which are fulfilled for this work,
5: for j = 1; j <= length(Aadji ) do a link between the expected running time and the size of the
6: (Aadji , ACoordi ) = local path planner(pj , pi , κ0 , σ0 ) graph using PPP can be made. Specifically, the expected size of a
7: (Aadji , ACoordi ) = local path planner(pi , pj , κ0 , σ0 ) probabilistic roadmap required for solving this problem grows only
8: end for logarithmically in the complexity of the problem. Moreover, the
9: i:= i+1 number of nodes required to be generated, in order for the planner
10: end while to succeed with probability at least 1 − α is logarithmic in α1 and
11: while c = 0 & i = nmax do polynomial in the inverse of the clearance — the minimal distance
12: (κk , σk ) = new curvature constraints between the path and the obstacles configuration space.
13: pl = find unconnected nodes(Aadjn , ACoordn ), k = 1 . . . L In the case that a path with bounded curvature and curvature
14: for l = 1; l <= L do derivative cannot be found, the algorithm was designed to enhance
15: for m = 1; m <= length(Aadjn ) do the graph’s connectivity by looking for intermediate paths with
16: (Aadjn , ACoordn ) = local path planner(pl , pm , κk , σk ) more permissive constraints.
17: end for Fig. 6 shows the effects of these refinements in a case when
18: end for the curvature constraints are too demanding for the obstacle-free
19: end while configuration space. In the upper left plot, the graph with its nodes
and edges is represented in the x–y configuration space. Note
that, with the initial curvature constraints, there is an inaccessible
Note that, according to its authors [30], this planner is proba- part at the bottom of the graph — remark that if the red nodes
bilistic complete. This means that any problem which can be were removed, the remaining blue nodes, which represent the
 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265 259

Fig. 7. Speed planning scheme: curvature, speed, longitudinal acceleration, and jerk. (For interpretation of the references to colour in this figure legend, the reader is referred
to the web version of this article.)

paths with κmax = 0.08, would not permit us to complete the are necessary in the corresponding path. Otherwise, a straight
circuit. However, the second part of the Algorithm 2 (lines 11–19) line segment will be introduced between two transition speed
provides much better connectivity by relaxing the maximum curves whose length will be determined by the turn deflection.
constraint bound for the unconnected nodes — red nodes. This
Fig. 7 shows the concatenation of curvature profiles between
is also observed in the upper right and lower plots of the figure
nodes whose positions are represented by red dots, and the asso-
in which the feasible trajectories and the associated curvature
ciated speeds, accelerations, and jerks.
profiles are shown in blue or red depending on whether the
In order to obtain closed-form expressions for the second
maximum curvature is the initial value — κmax = 0.08 — or has
type of curve, various smooth trajectory generation techniques
been relaxed — the curvature bound is a fixed value between 0.08 were evaluated. Of those in the literature on robot manipulators,
and the inverse of minimum curvature radius of the vehicle, which that of [37] not only provides a deterministic optimal speed
is around 0.2. fulfilling the length and comfort constraints — bounds on speed,
acceleration, and jerk — but also allows the speed profile to be
5. Speed profile generation regenerated on-line in case a dynamic obstacle is detected so that
the path planner can take it into account.
As mentioned above, the concatenation of continuous curvature The speed trajectory is divided into seven intervals [ti−1 , ti ], i =
paths provides a semantic interpretation of the overall trajectory. 1 . . . 7 (see Fig. 8). This piecewise function can be expressed in
In other words, any path in an unstructured environment can terms of the arc length sr as follows
be decomposed, with the help of the proposed path planning ...
algorithm, into a succession of turns — composed of clothoids and ...
s rmax , t ∈ [t0 , t1 ] ∨ t ∈ [t6 , t7 ]
arcs of circles — and straight lines. This result is extremely useful in s r (t ) = 0,... t ∈ [t1 , t2 ] ∨ t ∈ [t3 , t4 ] ∨ t ∈ [t5 , t6 ]
finding closed-form optimal speed profiles because both straight − s rmax , t ∈ [t2 , t3 ] ∨ t ∈ [t4 , t5 ]
line segments and circle arcs can be associated with constant ...
s̈r (t ) = s̈r (ti−1 ) + s r (t )(t − ti−1 )
speeds. More precisely, when a turn is initiated the maximum
1 ... (7)
velocity will be constrained by the comfort lateral acceleration ṡr (t ) = ṡr (ti−1 ) + s̈r (ti−1 )(t − ti−1 ) + s r (t )(t − ti−1 )2
threshold, and when a straight segment is being tracked, the 2
maximum longitudinal speed, acceleration, and jerk will be the sr (t ) = sr (ti−1 ) + ṡr (ti−1 )(t − ti−1 )
limits imposed on the reference speed. 1 1 ...
With these premises, the overall speed profile will consist of + s̈r (ti−1 )(t − ti−1 )2 s r (t )(t − ti−1 )3 .
2! 3!
three families of curves:
The speeds, accelerations, and jerks in each stretch depend
1. Constant speed curves at a minimum value Vmin when the upon the initial conditions (s̈r (t0 ) = s̈r0 , ṡr (t0 ) = ṡr0 , sr (t0 ) = sr0 ),
curvature profile is a circular arc or its preceding clothoid. The the final conditions (s̈r...(t7 ) = s̈r...
e , ṡr (t7 ) = ṡre , sr (t7 ) = sre ), and the
minimal speed is fixed by the curvature bound for  the curve comfort constraints (| s r (t )| ⩽ s rmax , |s̈r (t )| ⩽ s̈rmax , |ṡr (t )| ⩽ ṡrmax ).
κmax , i.e., by combining (1) and (3) to yield Vmin = (γy /κmax ), The analysis given in [37] establishes four different cases
where γy is the maximum lateral acceleration. depending on the boundary conditions, but the present work will
2. A smooth transition from the minimum value Vmin to a maxi- consider only the most general case: when all seven intervals
mum allowed speed Vmax and back again to Vmin that fulfills the plotted in Fig. 8 exist, i.e., the maximum speed, acceleration, and
acceleration and jerk constraints. jerk are attained.
3. A set of one or two smooth transition curves (of type 2 above) In particular, the arc length will go from the initial point of
that go from zero to the maximum speed, and vice versa. the closing clothoid in a turn (sr0 ) to the final point of a straight
This passage will be performed with a single curve if no turns line segment (sre ), the initial and final speeds (ṡr0 , ṡre ) will be set
260 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265

Fig. 8. Intervals on a generic smoothed trajectory.

by a minimum speed Vmin , and the initial and final accelerations vehicle has four arrays of four 12 V batteries each, and a 5 kW
... ...
(s̈r0 , s̈re ) and jerks ( s r0 , s re ) will both be equal to zero. Concerning electric motor that allows it to reach 10 m s−1 with a maximum
the comfort constraints, the maximum speed ṡrmax will be Vmax ∗
longitudinal acceleration in first gear of around 2 m s−1 . The
and the maximum acceleration will be determined by the design steering wheel has a maximum angle of 37° and a maximum speed
parameters γmax and Jmax . of around 70° s−1 .
Note that the value of Vmax ∗
corresponds to the Vmax previously The vehicle has been instrumented with a Differential Global
defined if there is enough distance to reach the target. If the Positioning System (DGPS) installed at the vehicle’s rear-center,
available arc length is less than some critical value, the maximum which allows the vehicle to sense its position and velocity at a
speed will be set equal to the initial speed V0 resulting in the 10 Hz rate. An inertial measurement unit (IMU) is located as close
generation of a constant speed profile. Otherwise, a maximum as possible to the center of gravity of the minibus to provide, on the
speed between V0 and Vmax will be computed. The closed form one hand, back-up positioning in case of GPS receiver failures, and,
polynomial expression of Eq. (7) permits the maximum speed to on the other, data on the longitudinal and lateral acceleration of the
be computed as follows: vehicle. The architecture’s backbone is an On-board Control Unit
which consists of a solid-state industrial computer for automotive
applications. The solid state components allow the computer to
 
Vmax , if ∆s ⩾ (Vmax + V0 ) Vmax − V0
work normally under driving conditions, avoiding the commonly


Jmax

encountered hard-disk failures due to vibrations.



 
V − V

 + (Vmax + Vmin ) max min



∗
Vmax = Jmax (8) 6.2. Path planning in a real scenario
V0 , if ∆s < V0 /2 ∗ (V0 /γmax + γmax /Jmax )


1

2 Note that the obstacle-free configuration space used in the

− (γmax


2J

learning phase can be significantly reduced in this specific case.



 max
2
4 2 γ
+ 8Jmax 2 2
max ∆s + 4Jmax V0 − 4γmax Jmax V0 ), else Thus, the search over xi and yi will follow three basic steps [46].

− γmax
First, the boundaries of the polygonal obstacles are approximated
where ∆s = sre − sr0 . The interested reader can obtain further by subdividing each side of the original polygon into small
details in [37] about the generation of (8) for the specific case when segments. Second, the Voronoi diagram is computed for the
ṡre = 0, that has been generalized for any final speed ṡre . resulting points. And third, the Voronoi edges which have one or
Since this is a rather conservative approach, an alternative both end-points lying within any of the obstacles are eliminated.
algorithm was implemented to reduce the overall time needed to Once the Cartesian coordinates of the reduced obstacle-free
cover the planned path by slightly compromising the passengers’ points have been determined, their orientations θi are found using
comfort. Instead of reducing the speed to Vmin at each turn, only the y −y
a non-slipping hypothesis (θi = arctan( xi −xic )), where xic and yic
non-degenerate turns are taken into account for this purpose. Thus, i ic

if the curvature profile presents a concatenation of symmetric are the coordinates of the closest position in the reduced search
clothoids, the speed planner will not interpret this as a turn. space.
Therefore, the speed planner will provide greater speeds in zones In general, a reasonably good approximation can be made for
in which some degenerate turn is present in the overall path the bounds on the curvature and curvature derivative. Substituting
than would have been given by the algorithm presented above. the comfort constraint γy = Vx2 κ ⩽ γymax and the maximum
However, the bounds on the lateral acceleration will be briefly curvature expression (5) in (4), the maximum value of the
exceeded in these degenerate turns. To quantify these effects, both curvature derivative can be estimated as
speed planning approaches have been tested in circuits C1 and C2
φ̇(tan φ)1/2
and a discussion about the obtained quantitative comfort results is σmax = . (9)
1/2
presented in Section 6. γymax L3/2 cos2 φ
From the bus turning constraints, Eq. (9), and a maximum lateral
6. Experimental results acceleration of γymax = 1 m s−2 , one finds for the maximum
curvature and curvature derivative in this specific case κmax =
6.1. Instrumented public transport vehicle 0.189 and σmax = 0.207, respectively.
In order to obtain the parameters κmax , σmax , and nmax best
Molinero is a model EGK6152K electric minibus (Fig. 9) with adapted to our problem, a set of planning tests with Algorithm 2
capacity for 14 passengers. It is manufactured by Con+Auto. This was applied to the above configuration space.
 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265 261

Fig. 9. Electric minibus used in the experimental phase.

Fig. 10. (a) Influence of the number of nodes on the cost function; (b) influence of the curvature and curvature derivative on the cost function.

We first looked for a reasonable compromise between opti- derivative constraints on the cost function — the curvature Fourier
mality and computational cost by running the path planner algo- transform median.
rithm with increasing number of nodes nmax ∈ [50, 190]. Since
the global path planner obtains the intermediate points probabilis- Remark 3. FFT is an an efficient algorithm to compute the Discrete
tically, each fixed-size graph was computed five times to obtain Fourier Transform (DFT) F
meaningful results. One observes in the box plot of Fig. 10(a) that, N −1
− 2π N
while for nmax = 50, 70 there was at least one run which did not Kj = F (κj ) = κj e(− ji
)
, j = 0...N − 1
obtain a solution, for the interval nmax ∈ [90, 190] the cost function i=0
box plots varied only slightly. The zoomed-in inset shows the influ-
where κj is the curvature at sample j and N is the length of the
ence of the number of nodes on the total arc length of the planned
curvature signal. It is well known that DFT allows us to analyze the
paths, and is intended to highlight that in this latter interval the arc
frequency spectrum of a sampled signal, and as a consequence, its
length is hardly any different (only by some 2%) between the worst
sharpness. A statistical indicator will be taken from the curvature:
and the best cases of 90 and 190 nodes. It thus seems reasonable
the median κ̃ of sequence Kj
to consider nmax = 90 as the most efficient size of the adjacency
matrix for our purposes. 1 1
In order to evaluate the influence of each parameter on the
P (Kj ⩽ κ̃) ⩾ ∧ P (Kj ⩾ κ̃) ⩾
2 2
path planning process, an exhaustive set of tests was performed
to have a good indicator of the overall curvature evolution while
with κmax ∈ [0.08, 0.2], σmax ∈ [0.05, 0.2]. Note that the upper
giving reduced importance to outliers.
limits of these intervals are approximated from the previously
computed values, while the lower limits are from the empirical Fig. 10(b) confirms that, for a given curvature bound, the chosen
results on the configuration space considered. The surface plotted criterion is clearly related to the maximum curvature derivative.
in Fig. 10(b) shows the influence of the curvature and curvature Thus, the maximum curvature derivative will be σmax = 0.05, and,
262 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265

Fig. 11. Comparison of human drivers’ behavior with the theoretical dynamics in circuit C1 : (a) paths; (b) speeds; (c) total acceleration. (For interpretation of the references
to colour in this figure legend, the reader is referred to the web version of this article.)

since there is no solution to the problem for κmax ≤ 0.1, this value speeds recommended by the algorithm detailed in Section 5 if an
is the constraint chosen for the curvature. appropriate control algorithm – e.g. [42] — is used. Consequently,
the resulting speeds and accelerations can be compared with
6.3. Manual driving versus path and speed planning
those of a human driver trying to respect the maximum speed
Several trials were performed with the public transport vehicle and comfort acceleration constraints. In other words, even if
described above driven manually. The start and end points (Γ0 = longitudinal and lateral controllers are not still implemented on
Γe ) were the same as in our path planning problem, and the the vehicle, it seems reasonable to assume that the resulting path
different drivers were requested to choose the route such that and speed profile can be accurately tracked, and therefore can be
turns, accelerations, and braking provided a sensation of comfort compared with a human in the loop driving.
to the vehicle’s occupants while minimizing trip time. With the Table 2 and Figs. 11–12 present the most significant variables
aim of covering a large spectrum of driving attitudes, 15 people and indicators with which to evaluate the proposed automatic path
have been selected for such a task, who can be classified in three and speed planning algorithms against manual driving in circuits
different categories: Slow Drivers (SD), Medium Drivers (MD) C1 and C2 , respectively.
and Aggressive Drivers (AD). In the following, these three driver Following ISO 2631-1 standard [47], comfort can be quantified
typologies will be compared with the two variants of our Path and by computing the whole acceleration on the human body
Speed Planning (PSP) algorithm. 
γt = γx2 + γy2 + γz2
Remark 4. From Proposition 1, a vehicle that can be modeled
by Eq. (1) is able to track the path generated by Algorithm 2 at where γz2 is the vertical acceleration, which is neglected for our
 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265 263

Fig. 12. Comparison of human drivers’ behavior with the theoretical dynamics in circuit C2 : (a) paths; (b) speeds; (c) total acceleration.

To quantify the comfort all through the circuit, two different

indicators are taken into consideration:
Table 2
Path and speed planning versus manual driving.
• the integral squared difference between the total acceleration
of the vehicle and the maximum allowed acceleration
PSP1 PSP2 SD MD AD
T
1
∫
C1 C2 C1 C2 C1 C2 C1 C2 C1 C2
Iγ = (γt − γtmax )2
Mean 0 0 0.07 0.03 0.09 0.05 0.61 0.39 1.72 1.94 T 0
Iγ Max – – – – 0.12 0.07 0.73 0.46 2.05 1.99 which measures the mean trip comfort
Min – – – – 0.10 0.04 0.55 0.36 1.47 1.82
• the maximum difference between the total acceleration at any
Mean 0.22 0.05 3.81 3.64 1.23 1.12 1.42 2.34 3.74 4.17
Mγ Max – – – – 1.31 1.18 1.53 2.54 4.02 4.30 instant of the trial and the maximum allowed acceleration
Min – – – – 1.18 1.08 1.10 2.21 3.48 3.96 Mγ = max |γt − γtmax |
Mean 74.9 127.6 64.4 115.2 87.3 159.9 73.1 116.1 53.2 89.6
T Max – – – – 91.5 165.2 76.8 125.8 55.1 93.9 which quantifies the most abrupt turn that the vehicle’s occu-
Min – – – – 79.8 145.4 69.8 107.5 49.6 87.2 pants feel during the trip.
Table 2 summarizes the mean, maximum and minimum values
of these two comfort estimators and of the trip time in each one of
specific purposes. Since [47] sets the limit to a fairly uncomfortable the two considered circuits C1 and C2 . Note that the blue figures
motion at 1 m s−1 , the maximum overall acceleration will be set at highlight the maximum values, while the red ones emphasize
γt = 1 m s−1 . those with the worst result.
264 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265

In Fig. 11(a) a representative path of each driver group lie almost C03 and by the Spanish Ministry of Development through Research
on top of each other, while the result of the path planner plotted Grant GUIADE P9/08.
in red deviates somewhat from them. In particular, one notes that
the planned path is in general similar to those taken by human
References
drivers, especially on the left of the circuit, although there appear
some differences at the top right and the bottom of the circuit.
[1] Green paper: towards a new culture for urban mobility, Com(2007) 551,
These divergences reflect the imposition of a maximum curvature European Commission, 2007.
of κmax = 0.1 on the algorithm, while the driver intuitively looks [2] Attitudes on issues related to eu transport policy — analitical report, Tech. Rep.,
for the shortest comfortable path. In Fig. 12(a), the planned path European Commission, 2007.
for circuit C2 is also pretty similar to those adopted by most of [3] Action plan on urban mobility, Com(2009) 490, European Commission, 2009.
drivers. [4] G. Liu, C. Wu, A discrete method for time-optimal motion planning of a class of
mobile robots, Journal of Intelligent and Robotic Systems 32 (1) (2001) 75–92.
We were interested of course in contrasting the planned speeds [5] D. Dolgov, S. Thrun, M. Montemerlo, J. Diebel, Path planning for autonomous
provided by the two alternatives detailed in Section 5 with the vehicles in unknown semi-structured environments, The International Journal
three manual driving groups speed profiles. The former were of Robotics Research 29 (5) (2010) 485.
obtained with a maximum speed of Vmax = 10 m s−1 , a maximum [6] M. Yamamoto, M. Iwamura, A. Mohri, Quasi-time-optimal motion planning of
mobile platforms in the presence of obstacles, in: Proc. of IEEE International
longitudinal acceleration of γmax = 1 m s−2 , and a maximum jerk Conference on Robotics and Automation, vol. 4, 2002, pp. 2958–2963.
of Jmax = 1 m s−3 . [7] M. Haddad, T. Chettibi, S. Hanchi, H. Lehtihet, A random-profile approach for
One notes in Fig. 11(c) that all the manual driving tests exceeded trajectory planning of wheeled mobile robots, European Journal of Mechanics-
A/Solids 26 (3) (2007) 519–540.
the critical value γtmax = 1 m s−2 in the interval sr ∈ [170, 230]
[8] H. Sussmann, The markov-dubins problem with angular acceleration control,
(corresponding to the sharp turn at the bottom part of the circuit), in: Proc. of the 36th IEEE Conference on Decision and Control, San Diego, CA,
while the PSP approaches respect this critical value to a far US, 1997, pp. 2639–2643.
greater extent — especially PSP1 . In Fig. 12(c) this phenomenon is [9] L. Dubins, On curves of minimal length with a constraint on average curvature,
generalized at each significant turn of circuit C2 . and with prescribed initial and terminal positions and tangents, American
Journal of Mathematics 79 (1957) 497–516.
One observes from Table 2 that PSP1 clearly provides the best Iγ [10] J. Reeds, L. Shepp, Optimal paths for a car that goes both forward and
and Mγ results while obtaining a trip time approximatively equal backwards, Pacific Journal of Mathematics 2 (145) (1990) 367–393.
or shorter than that of all the human trials except that driven [11] P. Jacobs, J. Canny, Planning smooth path for mobile robots, in: Proc. of the IEEE
aggressively. The PSP2 strategy gains 10 s in trip time for circuit Int. Conf. on Robotics and Automation, vol. 1, Scottsdale, AZ, US, 1989, pp. 2–7.
[12] T. Simeon, S. Leroy, J. Laumond, Computing good holonomic collision-free
C1 — around 12 in circuit C2 by allowing some discontinuities in paths to steer nonholonomic mobile robots, in: Proc. of the IEEE-RSJ Int. Conf.
the acceleration profile. These short duration peaks reflect small on Intelligent Robots and Systems, vol. 2, Grenoble, FR, 1997, pp. 1004–1009.
deflection turns that should not have any great significance in an [13] B. Mirtich, Using skeletons for nonholonomic motion planning, Research
automated driving context. In any case, its value of Iγ is still at the report esrc 92-16/ramp 92-6, Department of Electrical Engineering and
Computer Science, University of California, Berkeley, June 1992.
same level as the medium driver trials.
[14] K. Jiang, L. Seneviratne, S. Earles, Time-optimal smooth-path motion planning
It is evident from Table 2 — in which the minimum and for a mobile robot with kinematic constraints, Robotica 15 (5) (1997) 553.
maximum values are given in blue and red, respectively — that [15] D. Balkcom, M. Mason, Extremal trajectories for bounded velocity mobile
PSP provides very interesting path and speed profiles in the sense robots, in: Proc. of IEEE International Conference on Robotics and Automation,
2002, pp. 1747–1752.
that they represent better combinations of trip time and comfort
[16] Y. Kanayama, B. Hartman, Smooth local path planning for autonomous vehicle,
than the human drivers were able to achieve in this unstructured in: Proc. of the IEEE-RSJ Int. Conf. on Intelligent Robots and Systems, vol. 3,
environment. Scottsdale, AZ, US, 1989, pp. 1265–1270.
[17] T. Fraichard, A. Scheuer, From Reeds and Shepp’s to continuous-curvature
paths, IEEE Transactions on Robotics 20 (6) (2004) 1025–1035.
7. Concluding remarks
[18] K. Komoriya, K. Tanie, Trajectory design and control of a wheel-type mobile
robot using B-spline curve, in: Proc. of the IEEE-RSJ Int. Conf. Robots and
A new approach to planning smooth path and speed profiles for Systems, Tsukuba, JP, 1989, pp. 398–405.
automated vehicles in unstructured environments has been pre- [19] A. Takahashi, T. Hongo, Y. Ninomiya, Local path planning and control for agv in
sented. It comprises three layers: (i) an optimal local continuous positionning, in: Proc. of the IEEE-RSJ Int. Conf. Robots and Systems, Tsukuba,
JP, 1989, pp. 392–397.
curvature planner for obstacle-free situations; (ii) a global plan- [20] W. Nelson, Continuous curvature paths for autonomous vehicles, in: Proc. of
ner that finds intermediate points to connect the configuration the IEEE Int. Conf. on Robotics and Automation, vol. 3, Scottsdale, AZ, US, 1989,
space to the desired degree, taking obstacles into account; and (iii) pp. 1260–1264.
a speed planner that uses the set of curves of the previous layer [21] M. Vendittelli, J. Laumond, C. Nissoux, Obstacle distance for car-like robots,
IEEE Transactions on Robotics and Automation 15 (4) (2002) 678–691.
to compute analytically a comfort-constrained profile of velocities [22] A. Piazzi, L. Bianco, M. Bertozzi, A. Fascioli, A. Broggi, Quintic G⟨sup⟩2⟨/sup⟩-
and accelerations. The results of the algorithms were satisfactorily splines for the iterative steering of vision-based autonomous vehicles, IEEE
contrasted in an automated public transport vehicle with real driv- Transactions on Intelligent Transportation Systems 3 (1) (2002) 27–36.
ing maneuvers performed by human drivers. [23] J. Villagra, H. Mounier, Obstacle-avoiding path planning for high velocity
wheeled mobile robots, in: IFAC World Congress, 2005.
The next natural stage of our work is, once the bus has been
[24] G. Bianco, Generation of paths with minimum curvature derivative with η3 -
fully automated, to include the path planner in an overall control splines, IEEE Transactions on Automation Science and Engineering 7 (2) (2010)
scheme in which an adapted robust control algorithm [41,42] will 249–256.
be used to track as closely as possible both the planned path and the [25] K. Yang, S. Sukkarieh, An analytical continuous-curvature path-smoothing
algorithm, IEEE Transactions on Robotics 26 (3) (2010) 561–568.
planned speed. We are also interested in further investigating the
[26] R. Liscano, D. Green, Design and implementation of a trajectory generator for
application of the results reported by Yang and Sukkarieh [25] to an indoor mobile robot, in: Proc. of the IEEE-RSJ Int. Conf. on Intelligent Robots
the present algorithms in order to improve the smoothness of the and Systems, Tsukuba, JP, 1989, pp. 380–385.
curvature profile, and hence reduce the trip time and the breadth [27] H. Delingette, M. Hébert, K. Ikeuchi, Trajectory generation with with curvature
constraint based on energy minimization, in: Proc. of the IEEE-RSJ Int. Conf. on
of the range of operating speeds. Intelligent Robots and Systems, vol. 1, Osaka, JP, 1991, pp. 206–211.
[28] T. Brandt, T. Sattel, J. Wallaschek, Towards vehicle trajectory planning for
Acknowledgments collision avoidance based on elastic bands, International Journal of Vehicle
Autonomous Systems 5 (1/2) (2007) 28–46.
[29] Y. Kuwata, S. Karaman, J. Teo, E. Frazzoli, J. How, G. Fiore, Real-time motion
This work was supported by the Spanish Ministry of Science and planning with applications to autonomous urban driving, IEEE Transactions
Innovation through Research Grant TRANSITO TRA2008-06602- on Control Systems Technology 17 (5) (2009) 1105–1118.
 J. Villagra et al. / Robotics and Autonomous Systems 60 (2012) 252–265 265

Jorge Villagra graduated in Electrical Engineering at the

[30] L. Kavraki, P. Svestka, J. Latombe, M. Overmars, Probabilistic roadmaps for
Universidad Politécnica of Madrid (Spain) in 2002. He
path planning in high-dimensional configuration spaces, IEEE Transactions on
received his Ph.D. Degree in Real-Time Computer Science,
Robotics and Automation 12 (4) (1996) 566–580.
Robotics and Automatic Control at the École des Mines
[31] J. Naranjo, C. Gonzalez, T. de Pedro, R. Garcia, J. Alonso, M. Sotelo, D. Fernandez,
de Paris (France) in 2006. From 2006 to 2009 he was
Autopia architecture for automatic driving and maneuvering, in: Intelligent
consecutively a research assistant at INRIA-Roquencourt
Transportation Systems Conference, 2006. ITSC’06, IEEE, 2006, pp. 1220–1225.
(France) and a Visiting Professor at the University Carlos
[32] C. Guarino Lo Bianco, M. Romano, Optimal velocity planning for autonomous
III, Madrid (Spain). He is currently a research fellow at
vehicles considering curvature constraints, in: IEEE International Conference
the Program AUTOPÍA in the Center for Automation and
on Robotics and Automation, 2007, pp. 2706–2711.
Robotics UPM-CSIC (Spain). His research interests include
[33] L. Labakhua, U. Nunes, R. Rodrigues, F. Leite, Smooth trajectory planning
on the one hand, nonlinear and optimal control and
for fully automated passengers vehicles: spline and clothoid based methods
nonlinear estimation for intelligent transportation systems, and on the other hand,
and its simulation, Informatics in Control Automation and Robotics (2008)
robust control systems for humanoid and mobile robotic applications.
169–182.
[34] R. Solea, U. Nunes, Trajectory planning with velocity planner for fully-
automated passenger vehicles, in: Intelligent Transportation Systems Confer-
Vicente Milanes was born in Badajoz, Spain, in 1980. He
ence, 2006. ITSC’06, IEEE, IEEE, 2006, pp. 474–480.
received the B.E. and M.E. Degrees in electronic engineer-
[35] M. Lepetic, G. Klancar, I. Skrjanc, D. Matko, B. Potocnik, Time optimal path
ing from the Extremadura University in 2002 and 2006,
planning considering acceleration limits, Robotics and Autonomous Systems
respectively and the Ph.D. Degree in electronic engineer-
45 (3–4) (2003) 199–210.
ing from the Alcalá University (UAH), Madrid, Spain, in
[36] F. Gravot, Y. Hirano, S. Yoshizawa, Generation of optimal speed profile for
2010. Since 2006, he has been with the Spanish National
motion planning, in: IEEE/RSJ International Conference on Intelligent Robots
Research Council, currently at the Center for Automation
and Systems, 2007, pp. 4071–4076.
and Robotics, Madrid, Spain. His research interests include
[37] S. Liu, An on-line reference-trajectory generator for smooth motion of
autonomous vehicles, fuzzy-logic control, intelligent traf-
impulse-controlled industrial manipulators, in: 7th International Workshop
fic and transport infrastructures, vehicle-infrastructure
onAdvanced Motion Control, 2002, pp. 365–370.
cooperation and intelligent transportation systems (ITS).
[38] X. Broquère, D. Sidobre, I. Herrera-Aguilar, Soft motion trajectory planner for
service manipulator robot, in: IEEE/RSJ International Conference on Intelligent
Robots and Systems, 2008, pp. 2808–2813.
[39] J. Laumond, P. Jacobs, M. Taix, R. Murray, A motion planner for nonholonomic Joshué Perez was born in Coro, Venezuela, in 1984. He
mobile robots, IEEE Transactions on Robotics and Automation 10 (5) (1994) received B.E. Degree in electronic engineering from the
577–593. Simón Bolivar University, Venezuela, in 2007, and the
[40] M. Fliess, J. Lévine, P. Martin, P. Rouchon, Flatness and defect of non-linear M.E. Degree in system engineering and automatic control
systems: introductory theory and examples, International Journal of Control from the University Complutense of Madrid, Spain, in
61 (6) (1995) 1327–1361. 2009. He is currently working toward the Ph.D. Degree
[41] J. Villagra, D. Herrero-Perez, M. Abderrahim, Robust flatness-based control in the Center for Automation and Robotics, Madrid. His
of an AGV under varying load and friction conditions, in: IEEE International research interests includes fuzzy logic, modeling, control
Conference on Control and Automation, 2009, pp. 1621–1628. and cooperative maneuvers among autonomous vehicles.
[42] J. Villagra, D. Herrero-Perez, A comparison of control techniques for robust
docking maneuvers of an AGV, IEEE Transactions on Control Systems
Technology (2011) doi:10.1109/TCST.2011.2159794.
[43] U. Kiencke, L. Nielsen, Automotive Control Systems, Springer Verlag, 2000. Jorge Godoy was born in Maracay, Venezuela, in 1986.
[44] E. Dijkstra, A note on two problems in connecting with graphs, Numerische He received the M.E. Degree in electronic engineering
Mathmatik 1 (5) (1959) 269–271. from the Universidad Simón Bolívar, Caracas, Venezuela, in
[45] P. Svestka, On probabilistic completeness and expected complexity for 2008. He is currently working toward the Ph.D. Degree in
probabilistic path planning, UU-CS, 1996-20. automation and robotics with the Universidad Politécnica
[46] M. Foskey, M. Garber, M. Lin, D. Manocha, A Voronoi-based hybrid motion de Madrid, Madrid, Spain. Since 2009, he has been with
planner, in: Proc. of IEEE/RSJ International Conference on Intelligent Robots the Center for Automation and Robotics, Universidad
and Systems, vol. 1, 2001, pp. 55–60. Politécnica de Madrid-Consejo Superior de Investigaciones
[47] Mechanical vibration and shock-evaluation of human exposure to whole-body Científicas, Madrid, researching autonomous vehicles.
vibration. Part 1: general requirements, Tech. Rep. ISO 2631-1, International His research interests include fuzzy-logic control and
Organization for Standardization, 1997. intelligent transportation systems.