2. Autonomous Mobile Robot Navigation with Velocity
Constraints
Research thesis
In Partial Fulfillment of the
Requirements for the
Degree of Doctor of Philosophy
Gil Manor
Submitted to the Senate of the
Technion - Israel Institute of Technology
Tevet, 5775 Haifa January 2015
3. The Research Thesis Was Done Under the
Supervision of Prof. Elon Rimon
in the Faculty of Mechanical Engineering
ACKNOWLEDGMENT
I would like to express my sincere gratitude to Prof. Elon Rimon for his
support and dedicated guidance and assistance throughout this research.
The Generous Financial of The Technion and The Halevy Fellowship
Scholarship is Gratefully Acknowledged
4. List of Publications
Referred Papers in Professional Journals
• Gil Manor and Elon Rimon. VC-method: high-speed navigation of a uniformly
braking mobile robot using position-velocity configuration space. Autonomous
Robots (2013) 34:295-309.
• Gil Manor and Elon Rimon, The Speed Graph Method: Time Optimal Naviga-
tion Among Obstacles Subject to Safe Braking Constraint, Submitted to IEEE
Transactions on Robotics. Nov. 2014.
Referred Papers in Conference Proceedings (* - Presenter)
• *Gil Manor and Elon Rimon, High-Speed Navigation of a Uniformly Braking Mo-
bile Robot Using Position-Velocity Configuration Space, International Conference
of Robotic and Automation, St. Paul MN, May 2012.
• *Gil Manor and Elon Rimon, The Speed Graph Method: Time Optimal Naviga-
tion Among Obstacles Subject to Safe Braking Constraint, International Confer-
ence of Robotic and Automation, Hong Kong, June 2014.
Contributed Papers in Conferences and Workshops (* - Presenter)
• Gil Manor, Global Time Optimal Path Planning Using the Brachistochrone Curve,
Technion, Nov. 2012.
• *Gil Manor and Elon Rimon, The Speed Graph Method: Time Optimal Naviga-
tion Among Obstacles Subject to Safe Braking Constraint, ICR 2013, Tel Aviv,
Nov. 2013.
5. Contents
Abstract 1
List of Figures 3
List of Tables 6
Nomenclature 7
1 Introduction and Literature Review 11
1.1 Literature Review 12
1.1.1 Configuration Space 12
1.1.2 Two-steps Methods 13
1.1.3 Reactive Motion Planners 14
1.1.4 Discretized Search Spaces 16
1.1.5 Exact Optimal Paths Using Calculus of Variations 17
1.1.6 Optimal Control Based Methods 18
1.1.7 Potential Field Methods 18
1.2 Objectives and Significance of Work 19
1.3 Thesis Outline 20
2 High-Speed Navigation of a Uniformly Braking Mobile Robot Us-
ing Position-Velocity Configuration Space 22
2.1 Introduction 23
2.2 Problem Description and Preliminaries 24
2.3 Position Velocity Configuration Space 27
2.4 Cellular Decomposition of Position-Velocity Configuration Space 29
6. 2.4.1 The Critical Points of the Height Function 29
2.4.2 The Partition of F v
Into 3D Cells 33
2.5 The Cells Adjacency Graph 36
2.6 Practical Path Realization 38
2.7 Simulations 41
2.7.1 Influence of Obstacles Clearance on VC-Method Paths 41
2.7.2 Comparison of the VC-Method with the Dynamic Window Ap-
proach 43
2.8 Experiments 45
2.9 Conclusion 47
3 The Speed Graph Method: Time Optimal Navigation Among Ob-
stacles Subject to Safe Braking Constraint 49
3.1 Introduction 50
3.2 Problem Description and Preliminaries 51
3.3 The Safe Time Optimal Path Near a Single Obstacle Feature 53
3.3.1 The Travel Time Functional 53
3.3.2 The Safe Time Optimal Path Near a Wall Segment 54
3.3.3 The Safe Time Optimal Path Near a Point Obstacle 57
3.4 Basic Properties of the Safe Time Optimal Path in Polygonal Environ-
ments 60
3.4.1 The Safe Time Optimal Variational Problem in Polygonal Envi-
ronments 60
3.4.2 Basic Properties of the Safe Travel Time Functional 61
3.5 The Speed Graph 69
3.6 Speed Graph Based Computation of the Safe Time Optimal Path in
Polygonal Environments 74
3.6.1 Path Optimization Using Newton’s Method 77
3.7 Numerical Simulations and Experimental Setup 80
3.8 Conclusion 84
7. 4 Conclusion 86
4.1 Summary 87
4.2 Current Work 88
4.3 Future Extension 89
Appendices 91
A The Turning Radius Constraint 92
B The Time Optimal Path for a Disc Robot 96
C Computation of the Speed Graph Edges and the Safe Time Optimal
Path as Convex Optimization Problems 99
D The Turning Radius Constraint Along an Optimal Path 104
Bibliography 113
Hebrew Abstract I
8. Abstract
Navigation is an important aspect in mobile robotics. Recently, we see a trend in
robotic systems leaving laboratories and clean rooms and moving to real world envi-
ronments. At this point, it is critical to ensure the safety of the robotic system as
well as the environment in which it travels. One of the most common approaches to
this problem is modeling a configuration space of permitted positions and orientations
of the robot, then searching for a free collision path in this space. The method is
applicable both off-line, pre knowing all the surrounding, and also on-line, while re-
trieving the data with contact and distance sensors. Despite all this, the problem of
navigating a mobile robot between obstacles while taking into account the dynamic
characteristics of the robot while satisfying some quality measure of the path, remains
with no certain unique solution.
One quality measure for paths is their total travel time or in other words, the
time optimal path between two end points. For such a quality measure, high speeds
of motion are required to complete the task as quick as possible. Traveling with
high speed imposes two main safety concerns that should be taken into account when
planning the path. (1) A speed dependant safe braking constraint; (2) A turning
radius constraint that prevents sliding affects or tipping over. These safety constraints
involve the robot’s speed, denoted ν, as well as the coefficient of friction between the
robot’s wheels and the ground, denoted µ.
Common approaches for solving the high speed navigation problem first compute
a collision free path for the robot, then parameterize the speed along it such that
velocity constraints are met. In this research we synthesize the speed dependant safety
constraints and the geometric path selection into a single framework. This ensures that
the path taken by the robot, prematurely satisfies these safety constraints and therefore
allows to compute the globally time optimal path. Two subsequent approaches are
suggested.
The first approach, suggests modeling an augmented configuration space for the
1
9. robot with the parameters x, y and ν. The speed dependant safe braking constraint is
modeled in this space and represented by towering columns whose interior coordinates
are states of the robot which lead to an imminent collision with the surrounding
obstacles. The Morse Theory is then used to identify special events in this space where
the free space for navigation losses its connectivity. These special events determine a
cellular decomposition which in turn induces an adjacency graph. A standard graph
search algorithm on this adjacency graph yields an approximated time optimal path
in a highly efficient way in terms of computational time.
The second approach uses calculus of variations and applies convexity properties
of these paths to compute the globally time optimal path in the environment. First,
the calculus of variations is used to compute the time optimal path near a single point
or wall obstacles subject to the safe braking constraint. These optimal paths are then
composed into a “speed graph” whose edges are time optimal path segments connecting
each individual pair of nodes. A time optimal path candidate is then selected using
a standard graph search algorithm. Next, convexity properties of these paths are
used to smooth the optimal candidate and to derive the globally time optimal path
in the environment. For both methods, the computed paths satisfy the turning radius
constraint such that the robot does not slide or tip over. The results are illustrated
with examples and described as readily implementable procedures.
2
10. List of Figures
1.1 (a) An arrangement of polygonal obstacles in the plane. (b) Each obstacle
induces a c-obstacle in the robot’s c-space. 13
1.2 The velocity obstacle voB induced by a moving obstacle ˆB and a moving
robot ˆA. The entities’ velocities are vA and vB respectively. 15
2.1 The straight line braking path ends at the maximal Euclidean distance from
the robot’s initial position. 24
2.2 (a) The c-obstacle of a disc robot. (b) The vc-obstacle slice when the robot
travels with velocity magnitude ν. 27
2.3 The vc-obstacles resemble columns whose cross section expands monotoni-
cally along the ν coordinate. 29
2.4 (a) A Type I critical point is a saddle point of ϕ in Fv. (b) The vc-obstacles
slices in the vicinity of the saddle point. 32
2.5 A Type II critical point is a local maximum of ϕ in Fv. 33
2.6 The (x, y) projection of a stack of 3D cells. 34
2.7 A stack of 3D cells is generically surrounded by three vc-obstacle and three
vertical partitions. 35
2.8 The (x, y) projection of the 3D cells associated with a non-generic obstacle
arrangement. 35
2.9 The adjacency graph of the cellular decomposition induced by three polygo-
nal obstacles and an outer wall. 37
2.10 (a) The optimal path lies in vertical strips. (b) Path unfolding and vertical
edges realization. 38
3
11. 2.11 A switching curve (in red) for the double integrator problem along which
the point mass reaches the origin with zero speed. 40
2.12 (a) The optimal path passing through a node. (b) The smoothed path follows
the minimum safe turning radius arc. 40
2.13 A disc robot navigating with high speed in an office environment. 41
2.14 The speed of the robot for δ = 3.4. 42
2.15 The ratio t(vc-method)/t(lmin) of the vc-method as a function of δ. 43
2.16 The vc-method and DWA paths in (a)-(b) a rectangular room containing an
obstacle cluster, and (c) a urban-like environment. 44
2.17 The velocity profiles of the paths illustrated in Figures 2.16(a)-(b). 45
2.18 The experiment setup of three polygonal obstacles within a rectangular outer
boundary. 46
2.19 The velocity profiles of the three programmed paths. 46
3.1 (a) A circle rolling on the wall segment generates the cycloid curve. (b) The
safe time optimal path near a wall segment. 56
3.2 (a) Two symmetrically shaped time optimal paths from pS to pT . (b) The
safe time optimal path is a parabolic arc equidistant from the point obstacle
and the line l. 59
3.3 (a) A hypothetical time optimal path touching a convex obstacle vertex. (b)
The path can be moved away from the vertex, with a new segment of length
L < 2 . 63
3.4 (a) The Voronoi diagram, and (b) The speed graph of an environment popu-
lated by two polygonal obstacles (Voronoi arcs are solid curves, speed graph
edges are dashed curves). 69
3.5 (a) Two speed graph nodes, p1 and p2, are separated by three proximity cells
whose boundary lines are parameterized by (u1, u2). (b)T(γ(u1, u2)) is star
shaped, moreover its contours are convex in the (u1, u2) plane. 71
3.6 The Voronoi saturation problem. 73
4
12. 3.7 (a) The local smoothing directions of αo and αo point toward each other,
and (b) require a splitting of pi into two crossing points that will slide on the
neighboring Voronoi arcs ei1 and ei2. 75
3.8 (a) The piecewise time optimal paths connecting pS and pT while crossing
the Voronoi arcs e1 and e2 are parametrized by (u1, u2). (b) The contours of
T(γ(u1, u2)) are star shaped (and convex) with center at (u∗
1, u∗
2). 77
3.9 (a) The speed graph of the given environment, indicated with dashed curves.
(b) The initial speed graph path αo, and (c) the safe time optimal path α∗
connecting pS and pT in F. 79
3.10 The Newton-Raphson’s travel time convergence during five iterations on the
paths of Figure 3.9. 80
3.11 (a) The speed graph of the warehouse environment. (b) The safe time optimal
path α∗ selected by the speed graph, and the safe time optimal path ¯α∗
associated with the geometrically shortest path. 81
3.12 The speed profile along the paths α∗ and ¯α∗ in the warehouse environment. 81
3.13 The six best safe time optimal paths in the office floor environment. 82
4.1 A Formula One race car has to travel at high speed during turns without
sliding. 89
A.1 (a) Top view of a four-wheel mobile robot turning geometry. (b) Rear view
of a left turning mobile robot. 93
B.1 The safe time optimal path determined by the polygonal approximation, ¯α∗,
and the exact path α∗ associated with the disc obstacle. 97
C.1 The safe time optimal path connecting the speed graph nodes p1 and p2
crosses two proximity cell boundary lines, l1 and l2, at the corner points
p(u1) and p(u2). 99
C.2 (a) The safe time optimal path α∗ and a variant path αt. (b) The sector
spanned by −v(s−
i ) and v(s+
i ) locally contains the Voronoi arc tangent line
at p(ui) for i = 1, 2. 101
5
13. List of Tables
2.1 Running time in seconds and length in meters of the three simulations. 44
2.2 Experiment Results 47
3.1 The safe time optimal travel time in the six best homotopy classes in the
office floor environment. 83
4.1 Comparison between the VC method and the Speed Graph method. 88
6
14. Nomenclature
(r, θ) Polar coordinates used for paths near point and disc obstacles
(x, y) Cartesian coordinates in the plane
α Path connecting two endpoints in IR2
α The tangent of α
α∗
, β∗
Local time minimal paths
αo The most promising path selected by the speed graph
dst(α, Oi) The minimal distance between the path α and obstacle Oi
dst(q, Oi) The minimal Euclidean distance between point q and obstacle Oi
A desired solution accuracy
η A piecewise smooth function
F The obstacle-free portion of the environment
ˆv The robot’s instantaneous heading
int(COi) The interior of COi
int(COv
i ) The interior of COv
i
H The hessian of a scalar function of 2 variables
F The free configuration space of the robot
7
15. µ The static coefficient of friction with the ground at the contacts
ν The robot’s linear speed (velocity magnitude)
ω The robot’s front wheels average steering angle
IR The real numbers
IR2
The two dimensional infinite plane
IR3
The three dimensional infinite space
ρ The radius of a disc robot
ρc The radius of curvature of a path segment
ϕ Morse function
v The normalized tangent of α
ηi The normal to bdy(COv
i ) at p
T(α) The energy functional of T(α)
ξ The angle of a point on a rolling circle that creates the Cycloid
amax The robot’s maximal acceleration
amin The robot’s minimal acceleration
Ang(α1(pi) − α2(pi)) The polar angle between two path tangents α1, α2 at node pi
bdy(COv
i ) The surface of COv
i
C(1)
The division of paths with continuously varying tangent
COi A configuration obstacle induced by obstacle Oi
COv
i A velocity configuration obstacle induced by obstacle Oi
d(ν) The robot’s minimal braking distance
8
16. d1(α1, α2) The metric between two paths α1, α2
Ef The net work applied by the friction force
ei Voronoi arc i
Ek The kinetic energy of the robot
F The integrant of T(α)
fN The net normal force at the contacts
Fv The free position-velocity configuration space
g The gravitational acceleration
Gv The adjacency graph
H A horizontal plane in IR3
h The robot’s c.o.g. hight
l The robot’s wheels base
lj Proximity cell boundary line j
m The robot’s mass
n Total number of obstacle features in a polygonal environment
O0 The outer boundary of environment populated by obstacles
Oi An internal obstacle enumerated i
P A vertical plane in IR3
p A point in the velocity configuration space
p(ui) A corner point i of path α
pi A graph’s vertex i
9
17. pS Start position
pT Goal position
q The robot’s center position in IR2
R The robot
R(q) The set of points occupied by a robot R centered at q
rmin(ν) The robot’s minimal turning radius
s The robot’s path distance parameter
T(α) The total travel time along path α
ui The length parameter of corner point i
v The robot’s linear velocity
w The robot’s width
10
18. Chapter 1
Introduction and Literature Review
As autonomous mobile robots are deployed in ever more demanding tasks, they strive
to complete their tasks while minimizing overall travel time. This requirement is
achieved by maximizing the robot’s speed while maintaining velocity dependant safety
constraints throughout the navigation process. Examples are autonomous mobile
robots operating in sentry duty and surveillance [2, 3], where high speed is required to
complete the task without being detected by potential intruders [19]. Other examples
are self-driving cars [50] and traffic assistance systems [4] that will soon allow vehi-
cles to be piloted autonomously at speeds up to 50 km/h [63]. High speed systems
also include mobile robots carrying cargo in large warehouses [70, 79]. In all of these
applications collision safety along high speed paths depends on velocity constraints,
thus requiring a consideration of the robot’s full position-velocity state in order to
synthesize such paths.
There are two main safety concerns during execution of fast mobile robot tasks
in planar environment. The first is to ensure that the robot will always be able to
reach a full stop without colliding with any of the stationary obstacles or moving
agents [27, 57]. The second is to ensure that the robot will not slide or tip-over while
taking sharp turns at high velocity. These two limitations on robot’s speed are induced
by the robot’s geometry and the power delivered by its actuators as well as by the
friction between the wheels and the ground.
11
19. According to the classical path planning approach, one first determines a collision
free path using the robot’s configuration space, then selects a time parametrization
that attempts to minimize total travel time subject to velocity-dependent safety con-
straints [33, 35, 64]. The more general approach of planning a path in a space that in-
cludes the robots position as well as velocity is a relatively unexplored area in robotics.
In this work, the entire process of path planning takes place in the robot’s state space,
thus treating the geometric path planning and the speed profiling in a single frame-
work.
1.1 Literature Review
Next, we provide a survey of relevant works in the literature. The survey is divided
into parts according to the different approaches taken in this field.
1.1.1 Configuration Space
The classical approach for path planning considers planning a collision free path in a
configuration space [40, 46]. Given an arrangement of obstacles and the geometry of
a single rigid robot with no kinematic constraints (see Figure 1.1(a) for example), the
configuration space is computed. As depicted in Figure 1.1(b), for every orientation
of the robot, each obstacle induces a c-obstacle. The c-obstacle includes all the points
in the plane which bring the robot to penetrate the physical obstacle. Note that the
c-space depends on the placement of the robot’s coordinates system. When a 2D
environment of obstacles is considered while assuming a planar robot that can freely
translate and rotate, the induced c-space includes towering columns in IR3
. Each col-
umn, represents forbidden places and orientations that will result an imminent collision
between the robot and the physical obstacle. Generally, the c-space’s size depends on
the degrees of freedom of the robot as well as on the environment’s complexity.
The c-space method allows to compute a collision free path connecting the start
12
20. (a) (b)
physical environement c-space
c-obstacle
robot’s reference point
Figure 1.1: (a) An arrangement of polygonal obstacles in the plane. (b) Each obstacle
induces a c-obstacle in the robot’s c-space.
to target using various methods. These include exact methods such as cellular decom-
position where the free c-space is decomposed into simple regions whose connectivity
induces a undirected graph denoted a connectivity graph [17]. Other methods suggest
constructing a road map for the environment such as the Voronoi diagram or visibility
graph [76], then search for a collision free path on this graph.
Although very useful in known environment with unconstrained motion, adding
dynamic constraints, which is the main topic of this thesis, increases the size of the c-
space and hence makes it harder to implement most common exact methods. Moreover,
the addition of non-holonomic constraints on the robot’s motion adds non-integrable
equations involving the configuration parameters and their derivatives which limits the
possible motions of the robot but does not reduce the size of the c-space.
1.1.2 Two-steps Methods
The next approach is a two-steps method that first computes pure geometric collision
free path, then generates a velocity profile in such way that total travel time along the
path is minimized while meeting velocity and higher derivative safety constraints. One
of the original works in this subdivision was introduced by Bobrow et al. in [9]. In their
work, the speed parameterizations of the joints of a serial robot were determined using
13
21. a linear controller while following a desired path. These speed profiles are selected such
that actuators’ constraints are met. Their work was later extended by [65] to include
the dynamics of a mobile robot. Other works followed starting with lepetic et al. [45],
that proposed a method for high speed path planning while considering acceleration
limits induced by maximal ground friction forces. Their path is generated by a spline
curve computed over a set of “control via points”, whose positional optimization gives
an approximate time-minimal path.
Velenis and Tsiotras [72], propose a method to determine the robot’s speed in order
to minimize travel time along a pre-specified collision free path. The velocity profile
respects the mobile robot’s acceleration limits. Villagra et al. [73], first compute
collision free paths with continuous curvatures and curvatures’ derivatives, then apply
a speed profiler such that time minimal trajectories are obtained along the curvature-
based paths. Their time minimal trajectories satisfy additional comfort constraints
such as jerk limitations.
Wu et al. [74], first compute a geometric path by taking into account kinematic
constraints such as minimal turning radius. Then, a velocity profiler is applied to
maintain prescribed velocity constraints. All these methods plan the robot’s trajectory
in two separate steps. First a purely geometric collision free path is computed, then
the robot’s speed is selected along the path such that velocity safety concerns are met.
However, when truly time optimal paths are desired under velocity dependent safety
constraints, the geometric path taken by the robot and the speed profile along the
path must be synthesized within a single position-velocity space, which is the topic of
this research.
1.1.3 Reactive Motion Planners
An important class of high speed motion planners operate locally in discrete time steps.
Whereby at each time step the robot selects a local high-speed maneuver that avoids
nearby obstacles. The velocity obstacles method models the robot’s safe velocities as
14
23. efficient for on-line directional control, this method does not consider explicitly safety
criteria or other high level requirements such as total travel time.
Finally, let us discuss the work presented in [66] which considers static environments
populated by disc obstacles. Instead of computing at each time instant a local maneu-
ver which must avoid the obstacles, each obstacle is considered at a time. This reduces
the computational running time to linear in the number of obstacles. About each in-
dividual obstacle, an optimal bypassing maneuver is computed for a non-holonomic
point robot while assuming symmetrically bounded actuators and double integrator
dynamics. A global path is computed by concatenating a series of such paths. How-
ever, this method does not consider safety limitations which is the main topic of this
research.
This family of reactive motion planners is clearly useful in dynamically evolving
environments. In such environments the robot cannot posses full knowledge of the
obstacles and therefore cannot plan its complete path before travel. Therefore the
robot is forced to use a greedy strategy in which it computes its best and fasted path
with respect to its known surroundings.
1.1.4 Discretized Search Spaces
Other motion planners search for a high speed and safe path in the robot’s discretized
state space. First suggested by the kinodynamics method, which discretizes the robot’s
state space into a regular grid and searches the obstacle-free portion of the grid for
a minimum travel time path subject to velocity dependent safety constraints [21, 37,
42]. The kinodynamics approach requires a huge grid in complex environment [32, 43].
The method’s running time is proportional to the grid size and can be very high in
complex environments.
Karvaki [18] and LaValle and Kuffner [41, 44] introduced the RRT method, or
rapidly-exploring random trees, computed in the robot’s discretized state space. At
each time step until the target is reached, a randomly chosen point is connected to the
16
24. current tree with a feasible maneuver that obeys predefined velocity constraints and
minimizes a cost function such as path length or travel time. While the RRT method
was an important breakthrough, there are cases where the algorithm converges to a
non optimal path.
To over come this, Karman and Frazzoli, [36], introduced the RRT*. Beside linking
a randomized chosen point to the current tree with an optimal edge that satisfies the
velocity constraints, in each stage, the RRT* also updates the connections between
the nodes in the current tree. The connections between the nodes are updated in such
way that reduces the total cost of the path between the start to the current node. The
RRT* is proven of being complete and converges to the optimal path as number of
iterations reaches infinity. However, its rate of convergence can be excessively slow in
congested environment. Also note that this class of motion planers usually assumes
fully known environments.
1.1.5 Exact Optimal Paths Using Calculus of Variations
Using calculus of variations, Wein et al. [75] compute collision free paths whose geo-
metric shape is determined by a user specified “path quality” parameter δ. A value
of δ = 0 corresponds to minimal length paths that trace the visibility graph, while
δ = ∞ corresponds to maximal clearance paths that trace the Voronoi diagram of
the environment. Based on this path quality measure, they describe a scheme that
discretizes the environment’s Voronoi diagram into -length segments, then searches
the adjacency graph of these segments for optimal paths in O((Λ2
/ 2
)n5/2
log n) time,
where Λ is the total length of the environment’s Voronoi diagram, and n is the num-
ber of obstacle features in the environment. However, [75] does not explicitly consider
velocity dependent safety constraints or travel time minimization.
17
25. 1.1.6 Optimal Control Based Methods
The use of optimal control for time optimal path computation is well cited in the
literature. One of the earliest works in this field includes Dubins’s work which was
published in 1957 [22]. In his work, Dubins considered a non-holonomic motion of a car
like vehicle navigating between two end points in an obstacles-free environment while
assuming constant speed. He then proved that the time optimal path (with constant
speed!) between these two end points is constructed by path primitives consisting of
constant radius arcs and linear path segments. It is important to note that the initial
and terminal states of the car like vehicle are determined by its position as well as its
orientation. In [58], the car was also allowed with backward motion which created a
non smooth path with cusps.
Many works were inspired by Dubins’s work. In [31], for e.g., the authors compute
path primitives which minimize overall travel distance on the plane in heterogeneous
environments. In such environments the robot’s permitted speed is determined by its
position. Other works create the path using smooth path primitives such as clothoid
which allows to create smooth connectivity between adjacent segments [25, 77]. Com-
puting time optimal path primitives using optimal control theory was also suggested
in [16]. In this work, path primitives were computed for Dubins airplanes. For such
airplanes, the state space is 4D rather than 3D as in the planar Dubins car. However, non
of these works explicitly account the speed dependant constraints involving braking safety
as well as the turning radius of the robot while navigating with high speed between adjacent
obstacles. Although not fully presented in this thesis, this approach is currently being taken
by us in current work as briefly discussed in Section 4.2.
1.1.7 Potential Field Methods
Last, consider works were potential field methods, or PFMs, are used to navigate the robot.
For such methods each obstacle exerts a repulsive force on the robot while the target imposes
an attracting force. A summation of these forces propels the robot between the obstacles to
the target, for e.g. [55, 67, 28]. Although quiet efficient for global path planning and also
18
26. as a local planner, PFMs suffer from two main drawbacks as discussed by Koren et al in
[38]. (1) PFMs may lead the robot to a local minimum which will prevent the robot from
reaching its target. This drawback is well investigated in the literature and many solutions
were suggested, for e.g. [60, 59]. (2) PFMs may bring the robot to lose its stability when
implemented online in narrow corridors or near a long wall obstacle. This loss of stability
is characterized by rapidly growing oscillations, left and right, that bring the robot to lose
its grip with the ground and collide with adjacent obstacles. Koren et al. point out this
drawback of PFMs and suggest a stability criteria that has to be taken into account when
implementing such paths using PFMs.
Although this thesis utilizes ideas also used by PFM methods, the implementation draw-
back, discussed by Koren et al. is not in the scope of this research. In this research we
focus mainly on path and trajectory planning rather then trajectory tracking. These two
problems are complementary but can be isolated and dealt independently from each other.
Few examples for works that concern the tracking problem can be found in [6, 52, 15, 5, 80].
Despite all this, we find it important to mention Koren’s work which enlighten the navigation
problem from a different, rather important, point of view.
1.2 Objectives and Significance of Work
As presented in Section 1.1, the problem of finding a collision free path connecting two points
in occupied environments has no particular optimal solution. Moreover, when safety con-
straints such as braking distance and speed dependant turning radius are imposed, dynamic
constraints on robot’s motion are involved which makes the path planning a more complex
task.
As discussed, when dynamic constraints are involved, common numerical approaches
suggest discretizing the search space into a regular grid then implementing a numerical
solver which in some cases does converges to the optimal path (with respect to some quality
measure) in an infinite time.
This thesis considers a relatively simplified problem in which the robot’s model is a
planar disc navigating in the plane among stationary obstacles. The robot is subjected to
19
27. braking safety as well as turning radius constraint. The thesis synthesizes these dynamic
constraints into the geometrical path planning and allows to rigorously ensure safety as well
as optimality in analytical fundamental verified methods.
Our first approach taken for computing a safe high speed path is supported by the
Stratified Morse Theory (Chapter 2). Then a new approach is taken using the Theory of
Calculus of Variations (Chapter 3). Convexity properties of these paths allow to ensure
convergence to the time optimal path in an accuracy algorithm which is highly efficient for
path planning.
The tools developed in this research will allow extension to more complex scenarios where
dynamic constraints are involved and efficient computation process is required.
1.3 Thesis Outline
The thesis consists of two main parts excluding this introductory chapter and the concluding
one. The first part of the thesis (Chapter 2) consists of a simplified model where the robot’s
speed dependant safe braking constraint is considered uniformly in all directions. A pseudo
time optimal path is computed, one that not only avoids the surrounding obstacles by also
satisfies the safe braking constraint. This simplified model allows to model the safe braking
constraint in a 3D configurations space, denoted vc-space, whose axes are the robot’s position
in IR2
and it’s velocity magnitude or speed. A method for modeling the safe braking distance
is presented, then a cellular decomposition of the free portion of the vc-space is suggested.
Based on this cellular decomposition, an adjacency graph is constructed and a pseudo time
optimal path is found. This model enables us to find the pseudo-optimal, collision free path
for the robot while preserving the braking distance constraint. A post processing algorithm
ensures that the safe turning radius constraint that prevents sliding and tipping over is
satisfied.
The second part of the thesis (Chapter 3) consists of finding the time optimal path
subject to the uniform safe braking constraint, using the speed graph method. Using calculus
of variations, the speed graph method computes the global time optimal path of a mobile
robot navigating in an environment populated by stationary obstacles and subjected to a
20
28. uniform safe braking constraint. The classical Brachistochrone problem describes the time
optimal path between two points in an obstacle free environment subjected to a constant force
field. By encoding the braking safety constraint as a force field surrounding each obstacle, the
method generalizes the Brachistochrone problem into the time optimal navigation problem in
an environment populated by stationary obstacles. The time optimal safe path between any
start and target is first computed on a simplified problem, where the mobile robot navigates
in the vicinity of a single point or a wall segment obstacle. Then, the analysis is extended to
environment containing outer walls and multiple internal polygonal obstacles. First, a Speed
Graph for the environment in constructed, which consists of time optimal edges that connect
critical via points. The speed graph is then used to efficiently compute the homotopy class
of paths which most likely contains the global time optimal path1. In the second stage, the
exact time optimal path is computed within the chosen homotopy class based on convexity
properties of these paths.
The concluding chapter of this thesis, summarizes the results of this work and discusses
future extension beyond the scope of this research. Auxiliary material is also added in the
appendices.
1
Two continuous paths α : [a, b] → IRn
and β : [a, b] → IRn
connecting start and target denoted pS
and pT , belong to the same homotopy class if there is a continuous mapping, F : [a, b] × [0, 1] → IRn
,
such that F(t, 0) = α(t), F(t, 1) = β(t) for t ∈ [a, b] and F(a, s) = pS, F(b, s) = pT for s ∈ [0, 1].
21
30. 2.1 Introduction
This chapter considers planar environments populated by stationary obstacles whose location
is known to the robot. The assumption of stationary obstacles is a conservative approach,
suitable for environments where emergency events occur along obstacle edges or just beyond
corners obstructed by obstacles. For instance, an autonomous vehicle moving in an urban
environment may detect another vehicle suddenly emerging from its parking space. By
maintaining suitable safe velocity, the moving vehicle can brake and prevent collision with
the emerging vehicle. More generally, any environment in which emergency events can be
detected at a distance exceeding the robot’s braking distance can be modeled according to
the method proposed in this chapter and also published in [48, 47].
As presented in the abstract, there are two main safety concerns during execution of fast
mobile robot tasks. The first is to ensure that the robot will always be able to reach a full
stop without colliding with any of the stationary obstacles or moving agents [27, 57]. The
second is to ensure that the robot will not slide or tip-over while taking sharp turns at high
velocity. In this chapter we model the uniform braking constraint as vc-obstacles in position-
velocity configuration space. The vc-obstacles’ cross section increases monotonically along
the velocity coordinate. We characterize the critical points where two vc-obstacles meet
for the first time and locally disconnect the free position-velocity configuration space. In
the physical environment, these are critical events where the robot’s velocity becomes too
large to support safe passage between neighboring obstacles. Next, we describe a cellular
decomposition of the free position-velocity space which is based on these critical points. An
adjacency graph is constructed for the cellular decomposition, such that its edges project
onto the Voronoi diagram of the planar environment. A specialized search algorithm called
the vc-method finds an approximate time-minimal path along the adjacency graph. Finally,
the path’s vertices are smoothed to ensure safe turning at the prescribed path velocity. The
resulting path leads the robot from start to target with maximum velocity while guaranteeing
safe braking and turning along the path.
The chapter is structured as follows. Section 2.2 describes the main velocity depen-
dent safety constraints governing high-speed mobile robot motion. Section 2.3 introduces
the position-velocity space and the vc-obstacles induced by the safe braking constraint in
23
31. v
r
0v 0v v
d(v)
x
y 0v v
0v
0v v
wiggly path
stops at a shorter
distance from start
Figure 2.1: The straight line braking path ends at the maximal Euclidean distance from
the robot’s initial position.
this space. Section 2.4 defines a cellular decomposition of the free position-velocity space.
Section 2.5 constructs an adjacency graph for the cellular decomposition. A search along
this graph yields a pseudo time optimal path that maintains safe braking distance from the
obstacles. Practical path realization issues such as path turning safety are considered in
Section 2.6. Simulations and experimental results of the method are discussed in Sections
2.7 and 2.8 as well as a brief conclusion in the concluding section of this chapter. Axillary
material is also added in the Appendix A.
2.2 Problem Description and Preliminaries
This section describes the velocity dependent safety constraints governing fast mobile robot
motion. The robot, denoted R, is assumed to be a disc that can freely move in the (x, y)
plane (Figure 2.1). The robot’s position and linear velocity are denoted by q ∈ IR2
and
v ∈ IR2
. The robot is assumed to move with fixed orientation, so that its configuration
space, or c-space, is two dimensional.
The main velocity dependent safety constraints are the need to maintain safe braking
distance, and to prevent sliding as well as tipover during sharp turns. First consider the
safe braking constraint. When the robot executes fast motion between obstacles, it must be
able to brake and reach a full stop without hitting any obstacle during the braking process.
This requirement ensures that sudden events, such as a pedestrian emerging between parked
vehicles into the road, can be safely avoided by a fast moving mobile robot. The length of the
24
32. shortest braking path is dictated by the robot’s maximal deceleration, a system dependent
parameter which is part of the following definition.
Definition 1 (Linear Acceleration Constraint). The robot’s linear acceleration is con-
strained by system dependent parameters amin and amax, which bound the robot’s acceleration
by
amin ˙v · ˆv amax,
where ˆv is the robot’s linear velocity direction and amin 0 to represent deceleration.
The robot’s shortest braking path is defined as follows.
Definition 2 (Shortest Braking Path). The robot’s shortest braking path is any feasible
path along which the robot applies the maximal deceleration amin.
The distance from the robot’s initial position to the point where it reaches a full stop is
not constant. As the robot follows a wiggly but feasible path, this distance becomes shorter
as depicted in Figure 2.1. The robot’s minimal braking distance, denoted d, is the length
of any of its shortest braking paths along which the robot decelerate with amin. Stretching
these braking paths into a linear path of length d will also give a shortest braking path, but
the one with the largest distance measured between the robot’s initial position and the final
braking point. For safety, the robot must keep at least d away from any of the obstacles.
The minimal braking distance can be computed using energy balance as follows [27]. The
mobile robot is modeled as a cylindrical rigid body moving with fixed orientation. Its kinetic
energy is given by
Ek =
1
2
mν2
, (2.1)
where m is the robot’s mass and ν = ||v|| is the robot’s linear velocity magnitude.
Let α(s), s ∈ [0, 1] be a braking trajectory which brings the robot to a full stop along
any of the shortest braking paths. The net work applied by the static friction force, which
acts between the wheels and the ground in order to bring the robot to a complete stop is
given by
Ef = µ
α(s)
fN ds = µfN d = µmgd, (2.2)
where µ is the static coefficient of friction between the wheels and the ground, g is the
25
33. gravitational acceleration, and fN =mg is the net normal force at the contacts. The minimal
braking path, d, occurs when the frictional reaction forces between the wheels and the ground
are aligned with the friction-cone edge opposing the direction of motion. Note that during
this braking phase, the wheels are rolling without sliding at the contacts. A complete stop of
the mobile robot requires that all its initial kinetic energy be absorbed by the braking force,
Ek = Ef . (2.3)
Substituting for Ek and Ef gives 1
2mν2 = µmgd. The minimal braking distance is therefore
d(ν) =
1
2µg
ν2
= kν2
, (2.4)
where k = 1/2µg is a constant.
Note that d(ν) does not depend on the robot’s mass. This assumption is valid only under
an ideal rigid body model. In practice the robot’s wheels are rubber coated and experience
substantial deformations at the contacts. In these cases the friction coefficient µ (and hence
the braking distance d) is strongly influenced by the robot’s mass m [68][chap. 3].
Also note that the actual frictional work is accomplished by the braking pads of the robot
and not by the static friction between the wheels and the ground. However if we assume that
the braking pads can deliver a larger braking force than the friction force at the contacts
then the previous development sustains.
The uniform braking constraint: Uniform braking safety is ensured when the robot
is kept at least d(ν) away from the nearest obstacle. This conservative safety criterion is
illustrated in Figure 2.1. All paths that bring the robot to a full stop while applying the
maximal deceleration amin have the same length d. When the closest obstacle is located at
least d away from the robot’s initial position, the robot can reach a full stop without hitting
any obstacle while following any maximal deceleration path.
The second major velocity dependent constraint is the robot’s minimal turning radius.
While turning, centrifugal forces and lateral moments act on the robot’s body. A practical
means to prevent sliding or tipping-over is to lower bound the robot’s turning radius at any
given velocity, as specified in the following definition.
26
34. v
iCO
R
( )d v
x
R
y
iO
iCO
iO
(a) (b)
Figure 2.2: (a) The c-obstacle of a disc robot. (b) The vc-obstacle slice when the robot
travels with velocity magnitude ν.
Definition 3 (Turning Radius Constraint). The robot’s minimal turning radius is speci-
fied by a system dependent function rmin(ν), where ν is the robot’s current velocity magnitude.
An expression for the robot’s minimal turning radius, rmin(ν), is derived in Appendix A.
The high-speed navigation method proposed in this chapter generates a pseudo time optimal
path that maintains safe braking distance from the obstacles. The path is then locally
smoothed to ensure safe turning radius at the prescribed velocity along the path.
2.3 Position Velocity Configuration Space
This section defines the position-velocity configuration space and the “obstacles” that encode
the safe braking constraint in this space. Consider the mobile robot to be a disc, R, moving
in the (x, y) plane. Let R(q) be the set of points occupied by the disc robot when its center is
located at q=(x, y). The obstacles occupy stationary sets denoted O1, ..., Ok. Each obstacle
Oi induces a c-obstacle, denoted COi, given by
COi = {q∈IR2
: R(q) ∩ Oi = ∅} i = 1 . . . k. (2.5)
Each c-obstacle is obtained by uniformly expanding the physical obstacle by the robot’s
radius, as depicted in Figure 2.2(a). The free configuration space is the complement of the
c-obstacle interiors,
F = IR2
− ∪
i=1...k
int(COi), (2.6)
27
35. where int(COi) denotes the interior of COi.
Position-velocity configuration space generalizes the notion of configuration space by
adding the robot’s velocity magnitude, ν, as an additional coordinate.
Definition 4. The position velocity configuration space of a disc robot R is the three-
dimensional space IR3
with coordinates p = (x, y, ν), where q = (x, y) is the robot’s position
and ν is the robot’s linear velocity magnitude.
The safe braking constraint is next defined as forbidden regions in position-velocity con-
figuration space. Let dst(q, Oi) be the minimal Euclidean distance of a point q ∈ IR2
from
the obstacle Oi.
Definition 5. Given a robot R and an obstacle Oi, the vc-obstacle induced by Oi, denoted
COv
i , is given by
COv
i = (q, ν) ∈ IR3
: dst (q, Oi) ≤ d(ν) , (2.7)
where d(ν) is the minimal braking distance for a robot traveling with velocity magnitude ν.
The free position-velocity configuration space, denoted Fv , is the complement of the vc-
obstacle interiors,
Fv
= IR3
− ∪
i=1..k
int(COv
i ), (2.8)
where int(COv
i ) denotes the interior of COv
i . At any point p ∈ Fv, the robot initially
positioned at q and traveling with velocity magnitude ν can reach a full stop, without hitting
any of the obstacles O1, ..., Ok.
Each vc-obstacle forms a vertical column in position-velocity configuration space. The
ν = 0 slice of this column coincides with the c-obstacle COi. As ν increases, the column’s
cross section expands, since the braking distance increases quadratically with ν. Figure 2.2(b)
depicts a fixed-ν slice of a vc-obstacle, while Figures 2.4 and 2.5 sketch the full vc-obstacles.
The rate by which each vc-obstacle cross section expands is quadratic in ν, due to the relation
d(ν) = kν2.
Example: The vc-obstacles associated with three polygonal obstacles are shown in Fig-
ure 2.3. Their bottom cross sections are the c-space obstacles, drawn in the (x, y) plane
which corresponds to ν =0. As ν increases, the vc-obstacles expand outwards while the free
28
36.
x
y
Figure 2.3: The vc-obstacles resemble columns whose cross section expands monotonically
along the ν coordinate.
space Fv shrinks inward. The graph depicted in the (x, y) plane is the Voronoi diagram of
the c-space obstacles and an outer boundary which is not shown. The Voronoi diagram will
play an important role in Section 2.5. ◦
2.4 Cellular Decomposition of Position-Velocity Con-
figuration Space
This section describes a cellular decomposition of the free position-velocity configuration
space, Fv, such that each cell is associated with a range of velocities that can be safely
executed at a particular locality of the environment. Section 2.4.1 introduces the position-
velocity height function, ϕ, and analyzes its critical points. Section 2.4.2 partitions Fv into
3D cells based on the critical points of ϕ.
2.4.1 The Critical Points of the Height Function
The free position-velocity configuration space is a stratified set [30], consisting of the following
manifolds. The interior of Fv is a three-dimensional manifold. The portions of the vc-
obstacle surfaces on the boundary of Fv form two-dimensional manifolds. The intersection
curves of vc-obstacle surfaces form one-dimensional manifolds. The isolated points where
three vc-obstacle surfaces intersect form zero-dimensional manifolds.
29
37. The cellular decomposition of Fv will be determined by the critical points of the function
ϕ : Fv → IR , which measures the “height” of points p = (x, y, ν) along the ν coordinate in
position-velocity configuration space,
ϕ(x, y, ν) = ν. (2.9)
The critical points of ϕ in Fv are the union of the critical points of the restriction of ϕ
to the individual manifolds comprising Fv [30]. A point p ∈ Fv is a critical point of ϕ
when ϕ(p) is collinear with the generalized normal to the particular manifold containing
the point p. The generalized normal of the manifolds comprising Fv can be characterized as
follows [20]. In the interior three-dimensional manifold, the generalized normal is the zero
vector. At a point on a vc-obstacle surface, it is the usual surface normal pointing into Fv.
At a point on the intersection curve of two vc-obstacle surfaces, it is the convex combination
of the two surface normals meeting at this point. Last, every zero-dimensional manifold is
automatically a critical point of ϕ. The following theorem asserts that ϕ has two types of
critical points.
Theorem 1. The critical points of ϕ in Fv are located either on the intersection curve of
two vc-obstacle surfaces, or at the intersection point of three vc-obstacle surfaces.
Proof: First consider the interior of Fv. Since ϕ(p) = (0, 0, 1) is a non-zero vector, ϕ
cannot have any critical point in the interior of Fv. Next consider the vc-obstacle surfaces,
denoted bdy(COv
i ) for i = 1 . . . k. Denote by ηi = (ηx, ηy, ην) the normal to bdy(COv
i ) at p.
If p is a critical point of ϕ, ϕ must be collinear with ηi:
0
0
1
×
ηx
ηy
ηv
=
−ηy
ηx
0
=
0
0
0
⇒ η=
0
0
ηv
(2.10)
Thus, a necessary condition for p to be a critical point is that (ηx, ηy) = (0, 0) at this
point. The boundary of COv
i is the zero level-set of the scalar valued function: ψ(x, y, ν) =
dst((x, y), Oi) − kν2 = 0. Therefore ψ is collinear with the vc-obstacle normal ηi, and up
30
38. to a multiplicative factor:
ηi(x, y, ν) = ψ(x, y, ν) =
dst (q, Oi)
−2kν
.
The function dst(q, Oi) measures the minimal Euclidean distance between q=(x, y) and Oi.
This function is known to have a unit magnitude gradient, pointing from the neatest point on
Oi’s boundary to q [20][Proposition 2.5.4]1. Since dst(q, Oi) is a non-zero vector, ϕ cannot
have critical points on bdy(COv
i ) for i = 1 . . . k.
Next consider a point p on the intersection curve of adjacent vc-obstacle surfaces. The
generalized normal at p ∈ bdy(COv
i )∩bdy(COv
j ) is the convex combination of the vc-obstacle
normals, ηi and ηj,
η ∈ λ1ηi + λ2ηj 0 ≤ λ1, λ2 ≤ 1, λ1+λ2 =1.
A critical point p must therefore satisfy the condition:
ϕ(p) =
0
0
1
= λ1ηi + λ2ηj for some λ1, λ2 ≥ 0. (2.11)
It follows from (2.11) that ϕ has a critical point at p when the tangent to the intersection
curve at p is purely horizontal in (x, y, ν)-space. Such points occur at the lowest points
along the intersection curves. Finally, every isolated point where three vc-obstacle surfaces
intersect is automatically a critical point of ϕ.
The critical points of ϕ on the intersection curve of vc-obstacle pairs will be called Type
I points, while the critical points of ϕ at the intersection points of vc-obstacle triplets will
be called Type II points. The two types of critical points are either saddles or local maxima,
as stated in the following proposition.
Proposition 2.4.1. The function ϕ:Fv →IR has non-smooth saddles at the Type I critical
points, and non-smooth local maxima at the Type II critical points.
1
One must use the generalized normal of dst(q, Oi) when q has multiple nearest points on bdy(Oi).
31
39. P
v
jCOv
iCO
x
y
pv
F
Intersection curve
injn
pv v
pv v
pv v
(a) (b)
Figure 2.4: (a) A Type I critical point is a saddle point of ϕ in Fv. (b) The vc-obstacles
slices in the vicinity of the saddle point.
Proof Sketch: Let p be a Type I critical point of ϕ. Then p ∈ bdy(COv
i ) ∩ bdy(COv
j ),
such that the tangent to the curve bdy(COv
i ) ∩ bdy(COv
j ) is purely horizontal at p. Since
the vc-obstacles form columns whose cross-section expands monotonically along the ν axis,
the point p must be a local minimum of ϕ along the intersection curve. Let Vp be the plane
orthogonal to the curve bdy(COv
i ) ∩ bdy(COv
j ) at p. Let Up be a small three-dimensional
ball centered at p. The free space in Vp ∩ Up looks like an inverted V shape, with p located
at its upper vertex (see Figure 2.4(a)). It follows that ϕ has a local maximum within the set
Vp ∩ Up. Since ϕ has a local minimum and maximum along orthogonal directions at p, it has
a non-smooth saddle at this point.
When p is a Type II critical point of ϕ, three vc-obstacles intersect at this point. The
cross-section of each vc-obstacle expands monotonically along the ν axis. Hence at any fixed-
ν slice above p, the three vc-obstacles strictly penetrate each other. Since Fv is bounded
from above by the three vc-obstacles at p, the point p is a non-smooth local maximum of ϕ.
A Type I critical point p is thus a saddle of ϕ in Fv. Based on stratified Morse theory [30],
a locally connected fixed-ν slice of Fv just below p splits at the saddle and becomes two locally
disconnected sets in the slices just above p.
Example: Figure 2.4(a) shows a saddle point p where two vc-obstacles meet for the first
time along the ν axis. Note that ϕ = (0, 0, 1) is orthogonal to the intersection curve
32
40. P
v
jCOv
iCO
x
y
v
v
kCO
jn
kn
in
Figure 2.5: A Type II critical point is a local maximum of ϕ in Fv.
bdy(COv
i ) ∩ bdy(COv
j ) at p. Figure 2.4(b) depicts three fixed-ν slices: below p, through p,
and above p. ◦
The next example illustrates the free space Fv at a Type II critical point.
Example: Figure 2.5 illustrates a local maximum point p, where three vc-obstacles meet
for the first time along the ν axis. The point p is a zero-dimensional manifold and hence it
is a critical point of ϕ. ◦
2.4.2 The Partition of F v
Into 3D Cells
The free position-velocity configuration space will be partitioned by vertical planes called
vertical partitions, and by horizontal planes called horizontal partitions. Each of these par-
titions will pass through a critical point of ϕ. The vertical partitions are defined as follows.
Definition 6. Let p ∈ Fv be a saddle point of ϕ. Let P be the plane which passes through
p and spanned by the vc-obstacle normals at p. The vertical partition at p is the portion
of P ∩ Fv containing the point p.
Every vertical partition is bounded from below by the (x, y, 0) plane, from above by the
saddle point p, and from the “sides” by the vc-obstacles. The lower edges of the vertical
partitions decompose the (x, y) plane into two-dimensional regions, as shown in Figure 2.6.
To see that the vertical partitions are indeed vertically aligned, note that the tangent to the
intersection curve of two vc-obstacles is purely horizontal at p. At a critical point of ϕ, the
33
41. (x,y) projection of 3D cells
vertical partitions
bottom edges
saddle points projections
kO
jO
iO
Voronoi node
Figure 2.6: The (x, y) projection of a stack of 3D cells.
latter tangent is orthogonal to the vc-obstacle normals at p. Hence the plane spanned by
the vc-obstacle normals at p is vertically aligned in (x, y, ν) space. The horizontal partitions
are next defined.
Definition 7. Let p ∈ Fv be a saddle or a local maximum point of ϕ. Let H be a plane
parallel to the (x, y) plane passing through p. The horizontal partition at p is the portion
of H ∩ Fv containing the point p.
Each horizontal partition lies in a fixed-ν slice. When a horizontal partition passes
through a local maximum point, it consists of this single point. When the partition passes
through a saddle point, it forms a two-dimensional planar set bounded by nearby vc-obstacles.
The union of the horizontal and vertical partitions induces a cellular decomposition of the
free position-velocity space into 3D cells.
Definition 8. The cellular decomposition of the free position-velocity space, Fv, consists
of 3D cells bounded by the horizontal and vertical partitions passing through the critical points
of ϕ and the vc-obstacles.
The cellular decomposition of Fv consists of “stacks” of 3D cells separated by three
vertical partitions (in the generic case) and vc-obstacles. The cells within each stack are
separated by horizontal partitions, as illustrated in the following example.
Example: Figure 2.7 depicts three vc-obstacles together with their horizontal and vertical
partitions. The portion of Fv between the vc-obstacles forms a stack of four 3D cells,
separated by horizontal partitions passing through three saddles and one local maximum of
ϕ. ◦
A stack of 3D cells can have a more complex structure in non-generic situations. For
34
42. saddle
points
vertical
partitions
horizontal
partitions
3D cell
maximum point
3D cell
3D cell
3D cell
x
y
v
Figure 2.7: A stack of 3D cells is generically surrounded by three vc-obstacle and three
vertical partitions.
instance, when four identical obstacles are symmetrically arranged about a common center
point, their stack of 3D cells is surrounded by four vc-obstacles and four vertical parti-
tions. However, almost any local perturbation of the obstacle positions will give the generic
structure described above.
(x,y) projection of 3D cells
vertical patitions’
bottom edges
saddle points projections
jO
Voronoi node
kO
iO
lO
Figure 2.8: The (x, y) projection of the 3D cells associated with a non-generic obstacle
arrangement.
Example: Figure 2.8 depicts four symmetrically arranged obstacles in the plane. The
corresponding 3D cells are surrounded by four vc-obstacles and four vertical partitions whose
bottom edges can be seen in Figure 2.8. The portion of Fv between the vc-obstacles and the
vertical partitions consists of five rather than four 3D cells, and the cells’ local maximum
is the intersection point of four rather than three vc-obstacles. Note that almost any local
perturbation of the obstacle positions would give the generic structure described above. ◦
35
43. 2.5 The Cells Adjacency Graph
The vc-method constructs an adjacency graph, denoted Gv, for the cellular decomposition
of the free position-velocity space. Each vertex of Gv represents one 3D cell in the cellular
decomposition. The vertex, pi = (xi, yi, νi), is selected at the cell’s center as follows. Its
(xi, yi) coordinates are located at the point which maximizes the minimal distance from the
surrounding c-obstacles in the (x, y) plane (it is a node on the Voronoi diagram). Note
that the vertices in a stack of cells project to the same point in the (x, y) plane. The νi
coordinate is selected at the midpoint between the ν-values of the cell’s bottom and top
horizontal partitions (although it is possible to select any ν-value inside the cell). The edges
of Gv are of two types:
(i) Type A edge connects two vertices whose cells share a common vertical partition. The
(x, y) projection of the edge coincides with the Voronoi edge connecting the (x, y)
projections of the respective cell vertices.
(ii) Type B edge connects two vertices whose cells share a common horizontal partition.
The edge forms a vertical line whose (x, y) projection coincides with the Voronoi node
underlying the cells’ vertices.
When the robot traces a Type A edge it moves along a Voronoi edge with a continuously
varying velocity. Tracing of a Type B edge requires that the robot change its velocity without
any positional change. While not physically realizable, the vertical edges will guide the search
for a pseudo time optimal chain of cells in the cellular decomposition of Fv.
Example: Figure 2.9 depicts three c-obstacles surrounded by a rectangular wall in the (x, y)
plane. The vc-obstacles shown in the figure induce a cellular decomposition of Fv, and the
cells adjacency graph is depicted in the figure. ◦
Path Search on Gv: The vc-method assigns positive weights to the Type A edges and
zero weights to the Type B edges (as the latter edges do not correspond to any positional
change of the robot). The weight of a Type A edge reflects the minimum travel time along
its underlying Voronoi edge. Let a Type A edge connect two adjacency graph vertices,
pi = (qi, νi) and pj = (qj, νj), and pass through a saddle point pij = (qij, νij). The points
lying on the upper boundary of Fv above pi and pj are local maximum points. Denote by
36
44. adjacency graph
Voronoi edges
x
y
v
Figure 2.9: The adjacency graph of the cellular decomposition induced by three polygonal
obstacles and an outer wall.
νmax(pi) and νmax(pj) the speeds at the two local maxima. The minimum travel time along
the underlying Voronoi edge consists of two motion modes. Starting at qi with maximal
speed νmax(pi), the robot traces the Voronoi edge with maximal deceleration amin, until
reaching the (x, y) projection of the saddle point pij. The robot next traces the remaining
portion of the Voronoi edge with maximal acceleration amax, until reaching the endpoint qj
with maximal speed νmax(pj). The Type A edge cost is given by
Type A edge cost =
νmax(pi) − νij
|amin|
+
νmax(pj) − νij
amax
where νij is the lowest speed at the saddle point pij =(qij, νij).
Any shortest path search algorithm on Gv gives a pseudo time optimal chain of 3D cells
connecting the start cell to the target cell. Note that the chain of cells does not necessarily
contains the geometrically shortest path, but rather an approximate minimum-time path that
guides the robot safely from start to target under a uniform braking distance constraint.
Computational complexity of the vc-method: The adjacency graph vertices project
to Voronoi nodes in the (x, y) plane, while the Type A edges project to Voronoi edges in
the (x, y) plane. In general, the Voronoi diagram of a polygonal environment containing n
vertices has O(n) nodes and edges [7, 62]. Each vertical stack in the cellular decomposition
of Fv consists of four 3D cells. Hence there are O(n) vertices in Gv. Each 3D cell is
37
45. start
(a)
61 2 3 4 5
targetstart (b)
s
saddle
saddle
target
x
y
amax
acceleration
amin
deceleration
7 8
Figure 2.10: (a) The optimal path lies in vertical strips. (b) Path unfolding and vertical
edges realization.
horizontally connected to at most three neighboring cells, and is vertically connected to at
most two cells. Hence there are O(n) edges in Gv. Standard shortest path search algorithms
run in O(E log E) steps, where E is the total number of graph edges. Since Gv has size
O(n), the vc-method constructs the adjacency graph in O(n) time and searches the graph
in O(n log n) time. While the method is highly efficient, it only provides an approximate
minimum-time path subject to a uniform braking distance constraint.
2.6 Practical Path Realization
There are two practical path realization concerns. First, the optimal path on Gv contains
purely vertical Type B edges that must be modified into physically realizable arcs. Second,
the Type A edges project to Voronoi edges in the (x, y) plane. The Voronoi edges meet
non-smoothly at the nodes, so they must be smoothed at the nodes according to the robot’s
minimal turning radius. These two practical considerations are next discussed.
Path realization: Let the Type A edges along the optimal path be momentarily represented
as horizontal edges in (x, y, ν) space, as illustrated in Figure 2.10(a). The vertical strips along
the optimal path can be unfolded into a common vertical plane, as shown in Figure 2.10(b).
Note that the horizontal coordinate within this plane is the distance traversed along the
path’s horizontal edges, denoted s in the figure. For any physical robot, it is reasonable
to assume that its maximal acceleration does not exceeds its maximal deceleration, amax ≤
|amin|. Let us assume here that amax =|amin| (the path realization procedure can be adapted
to the case where amax |amin|). A time optimal realization is obtained by shifting the
38
46. adjacency-graph path vertically upward in (x, y, ν) space, until it lies on the upper boundary
of Fv. The shifted path vertices lie at local maximum points. Each arc of the shifted path
traces a vc-obstacle downward with minimal slope amin until reaching a saddle point, then
traces a vc-obstacle upward with maximal slope amax until reaching the next local maximum
point (Figure 2.10(b)). The path realization procedure is summarized for start and target
located on vc-obstacle boundaries.
1. Accelerate from the start position with amax to the first local maximum point along
the path (arc 1 in Figure 2.10(b)).
2. Decelerate from a local maximum point with amin to the next saddle point along the
path (arcs 2, 4, and 6 in Figure 2.10(b)).
3. Accelerate from the saddle point with amax to the next local maximum point along
the path (arcs 3, 5, and 7 in Figure 2.10(b)).
4. Decelerate from the last local maximum point with amin along the path, until reaching
the target with zero velocity (arc 8 in Figure 2.10(b)).
The path realization procedure achieves minimum travel time among all possible re-
alizations of the adjacency graph paths. The acceleration profile along the realized path
is basically a Bang-Bang parametrization: it always selects the maximal deceleration and
maximal acceleration, amin or amax, while maintaining a safe braking distance from the
obstacles.
The problem of minimizing the travel time along a specific given path is also known in
the literature as the time optimal control problem of a double integrator. The objective in
this problem is to bring a point mass moving with initial speed vS at xS to rest at the origin
in minimum time using bounded acceleration. This can be easily extended to any terminal
speed vT . The dynamics of this system is given by
˙x1(t) = x2
˙x2(t) = u,
with |u| 1, where x1(t) is the object’s position and x2(t) is its speed. A solution for this
39
47. 1x
2x
Figure 2.11: A switching curve (in red) for the double integrator problem along which
the point mass reaches the origin with zero speed.
kB
jB
iB
kB
jB
iBminr
x
y
(a) (b)
1e
2e
1e
2e
Figure 2.12: (a) The optimal path passing through a node. (b) The smoothed path follows
the minimum safe turning radius arc.
problem is derived by a switching curve (see Figure 2.11) in the x1−x2 plane which determines
the exact time instant when u switches between its extreme values. See [14](Section 3.9) for
full details.
Path smoothing at the nodes: The Type A edges project to Voronoi edges that meet
non-smoothly at discrete nodes in the (x, y) plane. But the robot must respect a velocity
dependent lower bound on its turning radius, rmin(ν), specified in the appendix. When the
optimal path in Gv passes through a vertex pi = (qi, νi), the robot changes its direction of
motion along a circular arc of radius rmin(νi) which is tangent to the Voronoi edges meeting
at qi. By following this circular arc with velocity magnitude which varies linearly between
its values at the arc’s endpoints, the robot executes a smooth edge transition while satisfying
the minimum turning radius constraint. The path smoothing procedure is illustrated in the
following example.
Example: Figure 2.12(a) depicts three c-obstacles in the (x, y) plane, together with three
40
48. 0 50 100 150 200 250 300
0
50
100
150
200
250
300
target
start
Voronoi
edges
vc-method
lmin
uW
lW
Figure 2.13: A disc robot navigating with high speed in an office environment.
Voronoi edges meeting at a node. The (x, y) projection of the adjacency graph path follows
the edges e1 and e2 indicated in the figure. The path must pass through the non-smooth node
at a speed ν = νi prescribed by the (x, y, ν) path. Figure 2.12(b) schematically shows the
circle having the minimal turning radius rmin(νi) (specified in the appendix). The locally
smoothed path replaces the node and portions of e1 and e2 with a circular arc of radius
rmin(νi) tangent to the path’s Voronoi edges at the node. The robot executes a smooth
transition from e1 to e2 by following the circular arc with with speed that varies linearly
between its values at the arc’s endpoints. ◦
2.7 Simulations
This section describes two simulation studies. The first study considers the effect of the inter-
obstacle clearance on the vc-method paths. The second simulation compares the vc-method
with the classical dynamic window approach.
2.7.1 Influence of Obstacles Clearance on VC-Method Paths
Consider the simplified office environment depicted in Figure 2.13. The left space is an empty
room, while the right space contains a large table of variable length. The Voronoi diagram
41
49. (a)
s0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
minl
vc-method
v
Figure 2.14: The speed of the robot for δ = 3.4.
which guides the construction of the adjacency graph is also depicted in the figure. The
upper passage is assigned a variable width Wu, while the lower passage is kept at a fixed
width, Wl, which equals 1.5 times the robot’s diameter. The ratio between Wu and Wl is
defined as δ = Wu/Wl, where δ ≥ 1.
Using the start and target indicated in Figure 2.13, the geometrically shortest path from
start to target on the Voronoi diagram passes through the lower passage. In this particular
environment, as long as 1 ≤ δ ≤ 3 the vc-method travels with low speeds through the lower
passage. When δ 3, the geometrically longer path can be safely traced at a higher speed
and hence with a shorter travel time. Figure 2.14 depicts the velocity profiles of the two
paths for δ = 3.4. The vertical axis is the robot’s speed, ν, while the horizontal axis is the
robot’s normalized position , s the path. Each velocity profile includes segments where the
robot accelerates or decelerates with its maximal capabilities and segments where the robot
travels with constant speed.
The graph in Figure 2.15 plots the ratio between the vc-method travel time, t(vc-method),
and the geometrically shortest path safe travel time on the Voronoi diagram, t(lmin), as
a function of δ. Note that t(lmin) remains constant for all values of δ. When δ is small, the
robot cannot travel rapidly through the top passage due to the braking distance constraint.
Therefore it selects the geometrically shortest path through the bottom passage. As δ in-
creases, the robot can safely travel with high speed through the top passage, thus achieving
a sorter travel time compared with t(lmin). While the travel time improvement is modest,
congested obstacle scenarios would give substantial travel time savings.
42
50.
lower passage paths
upper passage paths
t(vc-method)/t(lmin)
3
Figure 2.15: The ratio t(vc-method)/t(lmin) of the vc-method as a function of δ.
2.7.2 Comparison of the VC-Method with the Dynamic Win-
dow Approach
The vc-method is next compared with the dynamic window approach [13, 26]. The dynamic
window approach, or DWA, chooses the path’s velocity within a user specified time-window.
Within each time-window the robot moves along a circular arc of radius r. The selection
of r and the speed ν along this arc is based on maximization of an objective function that
consists of three components:
1. Target heading: a function head(r, ν) measuring the inverse deviation of the robot
heading from the target direction.
2. Obstacle clearance: a function dst(r, ν) measuring the minimal distance to the sur-
rounding obstacles.
3. Forward velocity: a function vel(r, ν) specifying the robot’s normalized speed during
tracing of the current circular arc.
The objective function, denoted G(r, ν), is the weighted sum of the three elements:
G(r, ν) = α · head(r, ν) + β · dst(r, ν) + γ · vel(r, ν)
where α, β, and γ are user-specified weights. In each time-window, G(r, ν) is maximized
over all turning radii r and speeds ν that ensure collision-free trajectories as well as braking
43
51. (b)
goal
start
(a)
start
targettarget
start
vc-method
DWA
Voronoi edges Voronoi edges
vc-method
DWA
target
start
DWA
Voronoi edges
(c)
vc-method
Figure 2.16: The vc-method and DWA paths in (a)-(b) a rectangular room containing an
obstacle cluster, and (c) a urban-like environment.
VC-method time DWA time VC-method length DWA length
Example (i) 24 39 4.85 4.37
Example (ii) 31 52 6.25 2.63
Example (iii) 24 46 5.04 4.38
Table 2.1: Running time in seconds and length in meters of the three simulations.
safety. In our simulations, the dynamic window approach is implemented with unit weights,
α=β =γ =1, and time-window of one second.
In order to study the difference between the vc-method and the dynamic window ap-
proach, two scenarios are considered. The first scenario, depicted in Figure 2.16(a)-(b),
consists of an obstacle cluster in a rectangular room. In Example (i), the start and target
lie at opposite sides of the obstacle cluster (Figure 2.16(a)). The DWA path in this example
passes between the obstacles through low-clearance passages. While the vc-method path
is geometrically longer, it passes through wide-clearance passages that allow higher speeds
subject to the uniform braking distance constraint. The robot’s speed profile is illustrated
in Figure 2.17(a). As indicated in Table 2.1, the vc-method travel time is almost one half of
the DWA travel time. In Example (ii), the target lies at the upper left corner of the room,
where it is directly visible from the start (Figure 2.16(b)). The DWA path moves through
the narrow passage along the geometrically shortest path to the target. The vc-method path
circumnavigates the obstacle cluster along a significantly longer path. However, this path
allows the robot to maximize its velocity while respecting the braking distance constraint
(Figure 2.17(b)). The vc-method travel time is again almost one half of the DWA travel
time (Table 2.1).
44
52. 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
35
(b)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
35
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
35
(a)
DWA
DWA
vc-method
vc-method
minl
v
s
s
s
v
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
vc-method
v
vc-method
(a)
Figure 2.17: The velocity profiles of the paths illustrated in Figures 2.16(a)-(b).
The second scenario, depicted in Figure 2.16(c), resembles urban city blocks surrounded
by a rectangular boundary. The start and target are located at opposite corners of the envi-
ronment. The vc-method path follows Voronoi edges that maintain wide clearance from the
obstacles. This path permits high speeds subject to the uniform braking distance constraint.
The DWA path passes through a low-clearance passage between adjacent city blocks. While
this is a shorter path, the narrow passage forces the robot to slow down, resulting in a path
whose travel time is almost twice longer than the vc-method travel time (Table 2.1).
The simulations indicate that when the motivation is to reduce travel time rather than
path length, wide clearance paths support higher speeds (and hence reduced travel time)
under the uniform braking constraint. By methodically searching the free cells of position-
velocity configuration space, significant travel time improvements can be obtained relative
to more traditional high-speed navigation methods.
2.8 Experiments
The vc-method was implemented on an Irobot Create 4400 platform. The robot’s mini-
mal deceleration was set to amin = −100 mm/sec2
, the maximal acceleration was set to
amax = 100 mm/sec2
, and the minimal turning radius was set to rmin = 300 mm. These
scaled values reflect those of autonomous high-speed platforms (e.g. [3, 2]). Figure 2.18 de-
picts the experimental setup. The environment contains three rectangular obstacles located
within a rectangular outer boundary. The Voronoi diagram which underlies the adjacency
graph construction is marked on the floor. The environment resembles the obstacle-cluster
environment described in Section 2.7.
45
53. 1
2
3
Figure 2.18: The experiment setup of three polygonal obstacles within a rectangular outer
boundary.
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
35
(b)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
35
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
DWA
vc-method
vc-method
DWA
vc-method
vc-method
minl
v
s
( / sec)v mm
(sec)T
s
v
0 5 10 15 20 25 30 35 40 45
0
50
100
150
200
250
300
1
2
3
Figure 2.19: The velocity profiles of the three programmed paths.
46
54. The robot was programmed to trace three trajectories along the Voronoi diagram edges
(Figure 2.18) and to implement different velocity profiles. The trajectories begin and end
at the same start and target points, but each has a different velocity profile dictated by the
uniform braking distance constraint. The experiment compares the geometrically shortest
path along the Voronoi diagram (trajectory 1), an intermediate length path (trajectory
2), and the vc-method path (trajectory 3). The experimental path length, travel time, and
average speed of the three trajectories are listed in Table 2.8. The velocity profiles of the three
programmed paths are depicted in Figure 2.19. The geometrically shortest path (trajectory
1) takes the longest time. The low-clearance passages along this path force low average speed
of 0.08 m/sec and travel time of 45 seconds. The intermediate length path (trajectory 2)
moves with average speed of 0.14 m/sec and travel time of 32 seconds. The vc-method path
(trajectory 3) moves through wide clearance passages with average speed of 0.26 m/sec and
the shortest travel time of 26 seconds.
Path Length (m) Duration (sec) Avg. speed (m/sec)
Trajectory 1 3.5 45 0.08
Trajectory 2 4.5 32 0.14
Trajectory 3 6.7 26 0.26
Table 2.2: Experiment Results
The experiment corroborates the vc-method usefulness for high-speed safe navigation in
congested environments. Controlling the Irobot position and speed proved to be difficult in
the experiments, due to its position sensor inaccuracy. This inaccuracy limited the robot
ability to precisely trace the Voronoi edges and implement the desired velocity profile. It
is interesting to note that on-line implementation of the method (which requires obstacle
detection sensors not available on the Irobot) will be able to compensate for such positional
inaccuracies based on local sensor readings.
2.9 Conclusion
Maintaining safe braking distance from the obstacles is a major concern during high-speed
mobile robot navigation. This chapter introduces position-velocity configuration space, where
the safe braking constraint is modeled as vc-obstacle. Based on the critical points of the
47
55. robot’s velocity magnitude function, ϕ(x, y, ν) = ν, the chapter described a cellular decom-
position of position-velocity configuration space into free cells. Each cell is associated with
a particular range of velocities that can be safely followed by the robot. An adjacency graph
constructed for the cellular decomposition allows a search for a pseudo time optimal path
from start to target which avoids the vc-obstacles. The path follows Voronoi edges which
are smoothed to ensure safe turning at the prescribed velocity magnitudes. The resulting
high-speed path leads the robot from start to target while maintaining safe braking distance
from the obstacles.
The vc-method treats the two velocity-dependent safety concerns, safe braking distance
and safe turning radius, at different stages of the planning process. The safe braking distance
is modeled by the vc-obstacles in position-velocity configuration space. The adjacency graph
path avoids the vc-obstacles and thus ensures the uniform braking distance constraint. The
(x, y) projection of the adjacency graph path traces Voronoi edges that meet non-smoothly at
discrete nodes. The turning radius safety constraint is enforces as a post-processing stage, by
locally smoothing the path at the Voronoi nodes. This division of the two velocity-dependent
safety concerns seems to offer practical advantages in terms of computational efficiency and
ease of implementation.
Based on simulations and experiments, the vc-method offers substantial improvement of
total travel time required for safe mobile robot navigation. The method computes an approx-
imate minimum-time safe path in O(n log n) time, where n is the total number of obstacle
vertices in the case of a polygonal environment. While the vc-method is highly efficient, it
does not provide an upper bound on the quality of approximation with respect to the exact
time-optimal safe path. Next, we propose the speed graph method which computes the time
optimal path of a mobile robot subjected to similar dynamic constraints as discussed here.
Although it first seems like an NP-hard problem, we will show that the time optimal path
computation can be reduced into a low polynomial complexity.
48
56. Chapter 3
The Speed Graph Method: Time
Optimal Navigation Among
Obstacles Subject to Safe Braking
Constraint
49
57. 3.1 Introduction
This chapter considers the time optimal paths of a mobile robot navigating in a polygonal en-
vironment subject to uniform braking safety constraints. Similarly to Chapter 2, stationary
obstacles are assumed as well as uniform braking safety. This enables to formulate to encode
the braking safety as a state (position and velocity) dependant constraint, which in turn
allows formulation of the safe time optimal navigation problem using calculus of variations.
The time optimal path minimizes a travel time functional, denoted T(α), defined over all
collision free paths α connecting the start and target. The work presented here, analyzes the
properties of T(α), then uses these properties to develop the speed graph method which com-
putes safe time optimal paths in environments populated by polygonal obstacles. The speed
graph consists of safe time optimal arcs connecting special via points located on the Voronoi
diagram of the environment. The speed graph is used to efficiently select the most promising
path homotopy class connecting the start and target in the environment1. Convexity prop-
erties of T(α) are then used to compute the safe time optimal path within the candidate
homotopy class as a convex optimization problem in O(n3 log(1/ )) time, where is the de-
sired solution accuracy. Importantly, the entire process takes place in the robot’s state space,
thus treating the geometric path planning and the speed profiling in a single framework.
The chapter is structured as follows. Section 3.2 formulates the safe time optimal navi-
gation problem under uniform braking safety constraints. Section 3.3 describes analytic so-
lutions for the safe time optimal path near a point obstacle and a wall segment. Section 3.4
formulates the safe time optimal navigation problem in polygonal environments, then de-
scribes basic properties of the safe travel time functional in such environments. Section 3.5
describes the speed graph whose construction uses the point obstacle and wall segment time
optimal solutions. Section 3.6 describes a convex optimization scheme for computing the
exact time optimal path in each path homotopy class suggested by the speed graph. Sec-
tion 3.7 discusses simulations and preliminary experiments. The concluding section discusses
extensions of the safe time optimal navigation problem to more demanding scenarios. Two
appendices contain auxiliary material.
1
The notion of path homotopy is reviewed later in the chapter.
50
58. 3.2 Problem Description and Preliminaries
This chapter considers the following problem, first formulated by Fraichard [27]. A mobile
robot modeled as point or a disc has to navigate with high speed in a planar environment pop-
ulated by static polygonal obstacles. The robot is assumed to move freely in IR2
and its state,
denoted (q, ν) ∈ IR2
× IR+
, consists of its position, q = (x, y), and its speed ν = ||v|| = || ˙q||.
While the robot navigates with high speed in the given environment, emergency events may
occur at any time along obstacle boundaries. For instance, a vehicle driving autonomously
along an urban road must limit its speed to a level allowing the vehicle to brake and reach a
full stop when another car suddenly emerges from its parking spot, or a pedestrian suddenly
emerges between two parked vehicles.
To ensure motion safety, the robot must brake and reach a full stop without hitting any
surrounding obstacles whenever such an event is detected by the robot. Braking safety is
ensured when the robot maintains the following safe braking distance constraint. Note that
some notations where already defined in Section 2.2, however for ease of reading we present
them again.
Definition 9. The robot’s safe braking distance, denoted d, is the length of the shortest
kinematically feasible braking path leading from the robot’s current state (q, ν) to a full stop
at ν = 0.
The shortest braking path may be any kinematically feasible path along which the robot
applies maximal deceleration (a system dependent parameter), eventually reaching a full
stop without hitting any obstacle. By maintaining a safe braking distance with respect to
the surrounding obstacles, the robot can handle emergency events that typically occur along
obstacle boundaries. Note that mobile robots are deployed in environments containing static
as well as dynamic obstacles. The current work assumes static obstacles, with the intention
of incorporating dynamic obstacles in future work.
To derive an expression for the safe braking distance as a function of the robot’s current
speed, first consider the robot’s maximal deceleration, denoted amin. Braking on the verge
of sliding determines the robot’s maximal deceleration. The net frictional reaction force
acting at the robot’s wheels during maximal deceleration is given by µmg, where µ is the
coefficient of friction at the ground contacts, m is the robot’s mass, and g is the gravitational
51
59. acceleration. The robot’s dynamics during maximal deceleration is given by the equation
mamin = µmg, and this equation gives the maximal deceleration:
amin = µg. (3.1)
While amin does not depend on the robot’s mass, the coefficient of friction µ usually
depends on m due to local deformations at the robot’s wheels [68][p. 15]. The safe braking
distance is computed using energy balance. When the robot moves along a kinematically
feasible braking path, the frictional reaction forces at the contacts are aligned with the
friction cone edge opposing the direction of motion (note that the wheels are rolling without
sliding during this braking phase). A complete stop of the robot requires that all its initial
kinetic energy be absorbed by the braking force. This energy balance leads to the expression
(see e.g. [27]),
d(ν) =
1
2amin
ν2
(3.2)
where amin is the robot’s maximal deceleration, and ν is the robot’s initial speed. The robot
is required to maintain a uniform braking safety distance from all surrounding obstacles, as
stated in the following definition.
Definition 10. When the mobile robot travels with speed ν, uniform braking safety is
ensured when the robot is kept at least d(ν) away from the nearest obstacle at every state
(q, ν).
Uniform braking safety is clearly a conservative safety criterion for autonomous robots.
To satisfy this requirement, the robot must maintain a circular safety zone in order to allow
safe deceleration to a full stop without hitting any obstacle. Since d(ν) is monotonically
increasing in ν, the circular safety zone increases in size when the robot navigates with
higher speeds, thus limiting the robot’s maximal speed at every point in the environment.
In particular, when the start or target are located at an obstacle boundary point, the robot
must start or reach the respective point with zero velocity.
Note that the actual frictional work is accomplished by the braking pads of the robot and
not by the static friction between the wheels and the ground. However if we assume that the
52
60. braking pads can deliver a larger braking force than the friction force at the contacts then
the previous development sustains.
3.3 The Safe Time Optimal Path Near a Single Ob-
stacle Feature
This section formulates the safe time optimal path variational problem, then describes an-
alytic solutions for the safe time optimal path near a single obstacle feature which can be
a wall segment or a point obstacle. These solutions will later be used to compute the safe
time optimal path in polygonal environment.
3.3.1 The Travel Time Functional
The robot’s path can be any piecewise smooth curve α(s) : [0, 1] → IR2
with endpoints
pS = α(0) and pT = α(1). When the geometric parameter s is parameterized by time, s(t),
the robot’s speed along α is given by
ν(s(t)) = d
dt α(s(t)) = α (s)
ds(t)
dt
where α (s) = d
ds α(s).
Solving for dt gives:
dt =
α (s)
ν(s)
ds, (3.3)
Integrating both sides of (3.3) gives the travel time functional:
T(α) =
1
0
α (s)
ν(s)
ds, (3.4)
and its minimization over all safe paths α defines the following variational problem.
Definition 11. The travel time variational problem seeks to minimize T(α) over all
piecewise smooth paths α(s) : [0, 1] → IR2
connecting pS and pT , where the speed ν along α
satisfies the uniform braking constraint d(ν) = ν2/2amin.
53
61. Note that T(α) is a path dependent function. The collection of piecewise smooth paths
in IR2
forms a metric space under the d1 metric:
d1(α1, α2) = max
s∈[0,1]
{ α1(s)−α2(s) }+ max
s∈[0,1]
{ α1(s)−α2(s) },
where α1 and α2 are two piecewise smooth paths connecting the same end points.
Moreover, T(α) is a continuous function of α with respect to this metric [29]. The inte-
grant of T(α) in (3.4), denoted F, is given by
F α(s), α (s), s =
α (s)
ν(s)
.
Based on calculus of variations [29], when F is differentiable with respect to its arguments,
any extremal path of T(α) over all piecewise smooth paths connecting pS and pT must satisfy
the Euler-Lagrange equation:
∂F
∂α
−
∂
∂s
∂F
∂α
= 0, (3.5)
where α and α are to be treated as formal variables. The safe time optimal path near a wall
segment and a point obstacle is next computed.
3.3.2 The Safe Time Optimal Path Near a Wall Segment
First consider the case of a point robot traveling near a wall segment. This case is a direct
extension of the classical Brachistochrone problem. The Brachistochrone problem considers
a mass particle sliding under the influence of gravity on a wire, and computes the wire’s
geometric shape that would give minimum travel time between two fixed endpoints [11]. In
our case the robot’s maximal deceleration, amin, replaces the gravitational acceleration as
follows.
Consider a wall segment aligned with the x axis as shown in Figure 3.1(a). Denote by
pS = (xS, yS) and pT = (xT , yT ) the start and target, which are assumed to lie in the upper
54
62. half of the infinite strip spanned by the wall segment. To encode the safe braking constraint,
note that the robot’s minimal distance from the wall is given by its y coordinate. Along
a safe time optimal path, the robot must keep a distance of at least d(ν) away from the wall
segment:
y ≥ d(ν).
Substituting d(ν) = ν2/2amin while requiring the robot’s speed to be maximized for travel
time minimization gives:
ν(y) = 2amin · y y ≥ 0. (3.6)
In the case of a wall segment, let us use the horizontal x coordinate as the path param-
eter s. The robot’s path is thus parameterized by α(x) = (x, y(x)) for x ∈ [xS, xT ]. In this
parametrization α (x) dx = 1 + (y (x))2dx, and the safe travel time functional (3.4) is
given by
T(α) =
xT
xS
F y(x), y (x) dx
where
F y, y =
1 + y 2
2amin · y
y ≥ 0, (3.7)
with the end conditions y(xS) = yS and y(xT ) = yT .
Any extremal path of T(α) must satisfy the Euler-Lagrange equation (3.5). Substitut-
ing for F(y, y ) in this equation, while considering the fact that ∂G(y , y, x)/∂x = 0, then
integrating with respect to x, gives the implicit scalar differential equation:
1 = c2
1 + y (x)
2
y(x) (3.8)
where c is yet to be determined. Note that the robot’s maximal deceleration, amin, does
not appear in (3.8). Thus, while the extremal path’s total travel time depends on amin,
55
63. 0 5 10 15 20 25 30 35
0
5
10
15
20
25
30
y
x
5 10 15 20 25 30 35 40
0
5
10
15
20
25
30
y
x
wall segment
(b)(a)
time optimal path
is a cycloid
Sp
Tp
rolling circle
wall segment
Figure 3.1: (a) A circle rolling on the wall segment generates the cycloid curve. (b) The
safe time optimal path near a wall segment.
its geometric shape is invariant with respect to amin. The extremal path solving (3.8) is
a cycloid, α(ξ) = (x(ξ), y(ξ)), where ξ represents the angle that a point fixed on a rolling
circle spans with respect to the vertical direction (Figure 3.1(a)). The formula for α(ξ) is
specified in the following lemma.
Lemma 3.3.1. The safe extremal path of a point robot moving in a planar environment
near a wall segment aligned with the x axis is the cycloid curve:
α(ξ) =
x0
0
+
1
2c2
ξ − sin ξ
1 − cos ξ
ξ ∈ [ξS, ξT ] (3.9)
where ξS, ξT , x0, and c are determined by the path’s endpoints pS and pT . Moreover, α(ξ) is
a local minimum of T(α).
While a formal proof of the lemma is omitted (see e.g. [29]), the Cycloid curve is known of
being the time optimal path connecting two points under a constant gravity field. Since the
problem formulated in this section is identical to the Brachistochrone problem, this Cycloid
curve is time minimal and unique. Note that the time optimal path is uniquely determined
by its endpoints pS and pT . Also note that the time optimal path reaches the x axis along
the normal direction to this axis (Figure 3.1(a)). Moreover, in addition to keeping a safe
braking distance, this path segment also maintains the safe turning radius constraint that
prevents sliding events (see Appendix D).
Example: The time optimal path of a point robot traveling near a wall segment from pS to
56
64. pT is shown Figure 3.1(b). The path is a cycloid segment that bends with convex curvature
away from the wall segment. ◦
To complete the wall segment case, consider a scenario where the robot is a disc of radius
ρ. The time optimal path of the robot’s center is given by the formula:
α(ξ) =
x0
ρ
+
1
2c2
ξ − sin ξ
1 − cos ξ
ξ ∈ [ξS, ξT ]
where ξS, ξT , x0, and c are determined by the endpoints pS and pT .
3.3.3 The Safe Time Optimal Path Near a Point Obstacle
Next consider the case of a point robot traveling near a point obstacle located at the origin
of the (x, y) plane. Using polar coordinates centered at the origin, (r, θ), the robot’s position
is given by (x, y) = (r cos θ, r sin θ). The robot’s distance from the point obstacle is given by
its r coordinate. Braking safety requires that the robot keep a distance of at least d(ν) away
from the point obstacle:
r ≥ d(ν).
Substituting d(ν) = ν2/2amin while maximizing the robot’s speed for travel time minimiza-
tion gives:
ν(r) =
√
2amin · r r ≥ 0.
Let the robot’s path be parameterized by θ, so that α(θ) = (r(θ) cos θ, r(θ) sin θ) for
θ ∈ [θS, θT ]. Under this parametrization α (θ) dθ = r2(θ) + (r (θ))2dθ, and the travel
time functional is given by
T(α) =
θT
θS
F r(θ), r (θ) dθ
57
