Upload
gil-manor
View
37
Download
3
Tags:
Embed Size (px)
Citation preview
Autonomous Mobile Robot
Navigation with Velocity
Constraints
Gil Manor
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
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
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.
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
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
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
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
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
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 F v. (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 F v. 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
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
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
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
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(COvi ) The interior of COv
i
H The hessian of a scalar function of 2 variables
F The free configuration space of the robot
7
µ 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(COvi ) 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(COvi ) The surface of COv
i
C(1) The division of paths with continuously varying tangent
COi A configuration obstacle induced by obstacle Oi
COvi A velocity configuration obstacle induced by obstacle Oi
d(ν) The robot’s minimal braking distance
8
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
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
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
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
(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 obstacleinduces 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
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
&
VA
-VB
CCA,B
VA,B
A
B^
VB
λf
λr
λAB
8$97f6RS;<L { .yc�J>L��sL�572ZPN9�E6L' yL�5:3Kd09:Pa[(� �/. ��24=>B(PNJ>L \ 365:579:T<9734= \ 36=>L�,,"�/. ���
L � RS9:EZ2457L0=rPN5:[4hzXr[�PN;<24=>T<572ZPN9:=>f�P<J>L�d�365:579:T<9734=�d�34=>L(,�,"�0. �«Xr[ �'��hW24T TUJ>3�£s=�9:=¥8$97f4R>;<L 3 ��c�J>L�Y��������� ��������F*�9�G���7������97T©P<J>L�=DB>L0�>=>L�B�24T .
���31+,�,"�0. ��� �'� � { �
BVB
VOB
VA
VB
A^
8/97f6R>;UL 3 .yc�J>L E6L0573Gd�9�Pa[�36X>TpPe24d057L���� ���Figure 1.2: The velocity obstacle voB induced by a moving obstacle B and a moving robotA. The entities’ velocities are vA and vB respectively.
an allowed velocity cone [24]. As depicted in Figure 1.2, each velocity cone determines
a local avoidance maneuver, and a series of such maneuvers is selected until the robot
reaches the target. The velocity obstacles method has been extended to local time
optimal maneuvers, and to motion in the presence of other moving agents such as cars
and pedestrians [39, 69, 78].
The dynamic window approach plans the robot’s path within a finite time horizon
whose size is determined by the robot’s current state [26]. At each time step, the
robot’s trajectory is chosen by maximizing a weighted sum of the robot’s distance
from the obstacles, the robot’s speed, and the current direction to the target. Some
dynamic window implementations explicitly account for safe braking constraints [13,
26]. Another approach is the method of inevitable collision states [27, 56, 57]. This
method encodes the surrounding obstacles as well as the robot’s braking capabilities
into first-order differential equations. Numerical integration of these equations gives
the inevitable collision states that can be incorporated into any fast mobile robot
navigation scheme.
Additionally, let us mention the vector field histogram approach [10, 71]. Using the
robot’s sensors, this method builds a histogram based on the distances to the obstacles
in discrete set of angles. The method then searches for gaps in this histogram that
represent collision-free headings of motion from the robot’s current position. Although
15
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
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
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
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
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
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.
1Two continuous paths α : [a, b]→ IRn and β : [a, b]→ IRn connecting start and target denoted pSand 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
Chapter 2
High-Speed Navigation of a
Uniformly Braking Mobile Robot
Using Position-Velocity
Configuration Space
22
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
vr
0v 0v v
d(v)x
y 0v v
0v
0v v
wiggly pathstops at a shorter distance from start
Figure 2.1: The straight line braking path ends at the maximal Euclidean distance fromthe 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
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
2mν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)
fNds = µfNd = µmgd, (2.2)
where µ is the static coefficient of friction between the wheels and the ground, g is the
25
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 12mν
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
v
iCO
R
( )d v
xR
y
iO
iCO
iO
(a) (b)
Figure 2.2: (a) The c-obstacle of a disc robot. (b) The vc-obstacle slice when the robottravels 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 6= ∅} 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
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
COvi , is given by
COvi ={
(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 F v , is the complement of the vc-
obstacle interiors,
F v = IR3 − ∪i=1..k
int(COvi ), (2.8)
where int(COvi ) denotes the interior of COvi . At any point p ∈ F v, 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
x
y
Figure 2.3: The vc-obstacles resemble columns whose cross section expands monotonicallyalong the ν coordinate.
space F v 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, F v, 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 F v 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 F v is a three-dimensional manifold. The portions of the vc-
obstacle surfaces on the boundary of F v 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
The cellular decomposition of F v will be determined by the critical points of the function
ϕ : F v → 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 F v are the union of the critical points of the restriction of ϕ
to the individual manifolds comprising F v [30]. A point p ∈ F v 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 F v 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 F v.
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 F v 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 F v. Since ∇ϕ(p) = (0, 0, 1) is a non-zero vector, ϕ
cannot have any critical point in the interior of F v. Next consider the vc-obstacle surfaces,
denoted bdy(COvi ) for i = 1 . . . k. Denote by ηi = (ηx, ηy, ην) the normal to bdy(COvi ) 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 COvi 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
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(COvi ) for i = 1 . . . k.
Next consider a point p on the intersection curve of adjacent vc-obstacle surfaces. The
generalized normal at p ∈ bdy(COvi )∩bdy(COvj ) 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 ϕ :F v→IR has non-smooth saddles at the Type I critical
points, and non-smooth local maxima at the Type II critical points.
1One must use the generalized normal of dst(q,Oi) when q has multiple nearest points on bdy(Oi).
31
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 F v. (b) The vc-obstaclesslices in the vicinity of the saddle point.
Proof Sketch: Let p be a Type I critical point of ϕ. Then p ∈ bdy(COvi ) ∩ bdy(COvj ),
such that the tangent to the curve bdy(COvi ) ∩ bdy(COvj ) 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(COvi ) ∩ bdy(COvj ) 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 F v 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 F v. Based on stratified Morse theory [30],
a locally connected fixed-ν slice of F v 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
P
v
jCOv
iCO
x
y
v
v
kCO
jnkn
in
Figure 2.5: A Type II critical point is a local maximum of ϕ in F v.
bdy(COvi ) ∩ bdy(COvj ) at p. Figure 2.4(b) depicts three fixed-ν slices: below p, through p,
and above p. ◦
The next example illustrates the free space F v 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 ∈ F v 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 ∩ F v 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
(x,y) projection of 3D cells
vertical partitions bottom edges
saddle points projections
kOjO
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 ∈ F v 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 ∩ F v 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, F v, 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 F v 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 F v 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
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 threevertical 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 cellsvertical 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 obstaclearrangement.
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 F v 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
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 F v.
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 F v, 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 F v above pi and pj are local maximum points. Denote by
36
adjacency graph
Voronoi edges
x
y
v
Figure 2.9: The adjacency graph of the cellular decomposition induced by three polygonalobstacles 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 F v consists of four 3D cells. Hence there are O(n) vertices in Gv. Each 3D cell is
37
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 verticaledges 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 logE) 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
adjacency-graph path vertically upward in (x, y, ν) space, until it lies on the upper boundary
of F v. 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
1x
2x
Figure 2.11: A switching curve (in red) for the double integrator problem along whichthe point mass reaches the origin with zero speed.
kBjB
iB
kBjB
iBminr
x
y
(a) (b)
1e
2e1e
2e
Figure 2.12: (a) The optimal path passing through a node. (b) The smoothed path followsthe 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
0 50 100 150 200 250 3000
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
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
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 10
5
10
15
20
25
30
35
(a)
DWA
vc-method
DWA
vc-method
v
s
sec
mmv
s
s
v
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
5
10
15
20
25
30
minl
vc-method
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
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
(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 anobstacle cluster, and (c) a urban-like environment.
VC-method time DWA time VC-method length DWA length
Example (i) 24 39 4.85 4.37Example (ii) 31 52 6.25 2.63Example (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
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
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 10
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 10
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 10
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
1
2
3
Figure 2.18: The experiment setup of three polygonal obstacles within a rectangular outerboundary.
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
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 10
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 10
5
10
15
20
25
30
DWA
vc-methodvc-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 450
50
100
150
200
250
300
1
2
3
Figure 2.19: The velocity profiles of the three programmed paths.
46
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.08Trajectory 2 4.5 32 0.14Trajectory 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
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
Chapter 3
The Speed Graph Method: Time
Optimal Navigation Among
Obstacles Subject to Safe Braking
Constraint
49
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.
1The notion of path homotopy is reviewed later in the chapter.
50
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
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
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)) = ‖ ddtα(s(t))‖ = ‖α′(s)‖ds(t)dt
where α′(s) = ddsα(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
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) = maxs∈[0,1]
{‖α1(s)−α2(s)‖}+ maxs∈[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
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 · yy ≥ 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
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 pathis a cycloid
Sp
Tp
rolling circle
wall segment
Figure 3.1: (a) A circle rolling on the wall segment generates the cycloid curve. (b) Thesafe 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
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
where
F (r, r′) =
√r2 + r′2
2amin · rr ≥ 0, (3.10)
with the end conditions r(θS) = rS and r(θT ) = rT . Substituting for F (r, r′) in the Euler-
Lagrange equation (3.5), then integrating with respect to θ, gives the implicit scalar differ-
ential equation:
c2(r2(θ) + (r′(θ))2
)= r3(θ) (3.11)
where c is yet to be determined. Much like the wall segment case, the travel time T (α)
depends on amin, but the geometric shape of the extremal path solving (3.11) is invariant
with respect to amin. The solution of (3.11) is a parabolic curve specified in the following
lemma.
Lemma 3.3.2. The safe extremal path of a point robot moving in a planar environment
near a point obstacle located at the origin is the parabolic curve:
α(θ) =(r(θ) cos θ, r(θ) sin θ
)θ ∈ [θS, θT ]
where r(θ) is given by
r(θ) = 2c2 1+sin(θ−θ0)
cos2(θ − θ0)θ ∈ [θS, θT ] (3.12)
such that θS, θT , c, and θ0 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, let us note that ∂2F (r, r′)/∂2r′ > 0
and that F (r, r′) is differentiable with respect to r and r′. This condition implies that the
extremal path is a local minimum of T (α) [29][Theorem 5.5]. In addition, since this problem
is known by having a global minimum (Lemma. 3.4.2), the path α(θ) which is the unique
extremal path must be that global and unique minimum.
The safe time optimal path is uniquely determined by pS and pT , except when the path’s
endpoints lie on opposite sides of the point obstacle. This special case is illustrated in
the following example. Note that this path segment also maintains the safe turning radius
58
point obstaclepoint obstacle
( )r 0s 1s
SpTp x
y
( )r
SpTp
y
x
(a) (b)
l
( )r
Sr
Tr
Figure 3.2: (a) Two symmetrically shaped time optimal paths from pS to pT . (b) The safetime optimal path is a parabolic arc equidistant from the point obstacle and the line l.
constraint that prevents sliding events (see Appendix D).
Example: Consider a point robot traveling near a point obstacle located at the origin.
When pS and pT are located on opposite sides of the obstacle as depicted in Figure 3.2(a),
there are two symmetric safe time optimal paths leading from pS and pT . Except for this
special case, all other locations of pS and pT in IR2 are connected by a unique time optimal
path. Note that the safe time optimal paths depicted in Figure 3.2(a) are convex arcs that
bend away from the point obstacle. ◦
The safe time optimal path can be graphically constructed as follows. Let (rS, θS) and
(rT , θT ) be the polar coordinates of pS and pT with respect to the point obstacle. Construct
the line l tangent to the circles centered at pS and pT with radii rS and rT , as shown in
Figure 3.2(b). The safe time optimal path is the parabolic arc equidistant from the point
obstacle and l. To complete the point obstacle case, the safe time optimal path of a disc
robot traveling near a point obstacle is considered in Appendix B.
59
3.4 Basic Properties of the Safe Time Optimal Path
in Polygonal Environments
This section formulates the safe time optimal variational problem in multiple obstacle envi-
ronments2, then discusses basic properties of the safe travel time functional in these envi-
ronments. These properties will guide the speed graph construction and the optimal path
computation.
3.4.1 The Safe Time Optimal Variational Problem in Polyg-
onal Environments
When a mobile robot is subjected to a uniform braking safety constraint, its maximal allowed
speed is governed by its distance to the nearest obstacles. Consider a polygonal environment
consisting of an outer boundary, O0, populated by internal obstacles O1, . . . , Ok. Let F
denote the obstacle-free portion of the environment together with the obstacles’ boundaries.
The robot’s path can be any piecewise smooth curve, α(s) : [0, 1]→ F , such that α(0) = pS
and α(1) = pT . Braking safety requires that as the robot moves along α(s), it must keep a
distance of at least d(ν) away from the nearest obstacles,
mini=0...k
{dst (α(s), Oi)} ≥ d(ν(s)) s ∈ [0, 1], (3.13)
where dst(α(s), Oi) is the minimal distance between the robot positioned at α(s) and the
obstacle Oi. Travel time minimization requires maximal speed. Substituting the robot’s
maximal allowed speed, d(ν) = ν2/2amin, into (3.13) gives the maximal safe speed:
ν(s) =√
2amin · mini=0...k
{dst (α(s), Oi)} s ∈ [0, 1].
The safe travel time functional to be minimized is thus given by
T (α) =
∫ 1
0F(α(s), α′(s)
)ds,
2The term variational problem refers to optimization of a path functional over a set of admissiblepaths.
60
where F (α, α′) is given by
F(α, α′
)=
‖α′‖√2amin ·mini=0...k{dst(α,Oi)}
. (3.14)
The minimization of T (α) over all piecewise smooth paths connecting pS with pT in
F defines the safe time optimal variational problem. Note that F (α′, α) is continuously
differentiable with respect to the variables α′ and α, except at special points which are
equidistant from two or more obstacle features in the environment. Next we define the
energy functional associated with T (α).
Definition 12. The energy functional associated with T (α) is the functional T (α) having
the squared integrant,
T (α) =
∫ 1
0F(α′(s), α(s)
)ds,
where
F(α, α′
)=
‖α′‖2
2amin ·mini=0...k{dst(α,Oi)}. (3.15)
Although the integrants of T (α) and T (α) are almost identical, there is no correspondence
between their extremal paths. This would have enabled us to project the convexity properties
of T (α) on T (α). Consequently, an alternate approach had been taken to prove monotonicity
of T (α) in the paths α as described in Appendix C.
3.4.2 Basic Properties of the Safe Travel Time Functional
This section describes basic properties of the safe time optimal paths in F . Let us first
review the notion of homotopic paths.
Definition 13. Two continuous paths with endpoints pS and pT , α(s) : [0, 1] → F and
β(s) : [0, 1]→ F, belong to the same homotopy class if there exists a continuous mapping,
f(s, t) : [0, 1] × [0, 1] → F, such that f(s, 0) = α(s), f(s, 1) = β(s) for s ∈ [0, 1], with
f(0, t) = pS, f(1, t) = pT for t ∈ [0, 1].
The following example illustrates the notion of path homotopy classes.
61
Example: Let the point obstacle depicted in Figure 3.2(a) represent a disc obstacle of small
radius. The two paths depicted in this figure belong to distinct homotopy classes, since one
path cannot be continuously deformed into the other without crossing the obstacle. ◦
Two paths belonging to the same path homotopy class can be thought of as “points”
connected by a continuous curve (the path homotopy), in the metric space of piecewise
smooth paths in IR2. Under this interpretation every path homotopy class forms a connected
set in the metric space. Before proceeding on with the properties of the time optimal path,
a fundamental argument about the continuity of T (α) is presented and summarized by the
next Lemma.
Lemma 3.4.1 (Continuity of T (α)). The travel time functional, T (α), is continuous
with respect to paths connecting pS and pT in the metric space induced by the d1 metric.
Proof. The travel time functional T (α) is given by:
T (α) =
∫ 1
0F(α, α′
)ds,
where,
F(α, α′
)=
||α′(s)||√2amin ·mini=0...k{dst (α(s), Oi)}
,
for s ∈ [0, 1]. Note that F is a ratio between two functions which are continuous in α′ and α.
Hence it is a continuous function in the pair (α, α′) at points where the denominator does
not vanish. Therefore, for every ε > 0 there exists a δ(ε) > 0 such that for any two paths α
and β that satisfy:
d1 (α(s), β(s)) = maxs∈[0,1]
{||α(s)− β(s)||}+ maxs∈[0,1]
{||α′(s)− β′(s)||} < δ,
the integrant F satisfies:
∣∣F (α, α′)− F (β, β′)∣∣ < ε, (3.16)
for every s ∈ [0, 1]. Next, examine the travel time difference for two neighboring paths,
62
-0.04 -0.03 -0.02 -0.01 0 0.01 0.02 0.03 0.04
-0.03
-0.02
-0.01
0
0.01
0.02
0.03
-30 -20 -10 0 10 20 30
-30
-20
-10
0
10
20
30
2L
zoom
( )d Tp
Sp
(a) (b)
iOiO
Figure 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ε.
T (α) and T (β), in the same homotopy class,
∣∣T (α)− T (β)∣∣ =
∣∣ ∫ 10 F (α, α′)ds−
∫ 10 F (β, β′)ds
∣∣ =∣∣ ∫ 1
0 F((α, α′)− F (β, β′)
)ds∣∣ ≤∫ 1
0
∣∣F (α, α′)− F (β, β′)∣∣ds.
Based on (3.16) we get:
∣∣T (α)− T (β)∣∣ ≤ ∫ 1
0
∣∣F (α, α′)− F (β, β′)∣∣ds ≤ ∫ 1
0 εds = ε,
which shows that for every ε > 0 there exists a δ(ε) > 0, such that if d1(α, β) < δ then
|T (α)− T (β)| < ε. This proves the continuity of T .
The following lemma establishes the existence of a safe time optimal path in every path
homotopy class connecting pS and pT in F .
Proposition 3.4.2 (Optimal Path Existence). Let F be a bounded polygonal environ-
ment populated by obstacles with non-empty interiors. The safe travel time functional T (α)
attains a global minimum in each path homotopy class connecting pS and pT in F . The
corresponding time optimal path lies in the interior of F, except at its endpoints that can lie
on obstacle boundaries.
It will be later shown that T (α) attains a unique minimum in each path homotopy class of
F .
Proof sketch: An argument explaining why T (α) attains a global minimum in each path
homotopy class of F is based on the continuity of T (α) (Lemma 3.4.1) as follows. LetM be
the union of all voronoi arcs, and all proximity cell boundary lines spanned within a distinct
63
path homotopy class of F . Denote by pi the crossing points between the path α with the
segments ci ∈ M, for i = 1...k, where k is the number of segments crossed by α. Note that
k ≤ m where m = size(M) is upper bounded in every path homotopy class. Next, let us
parameterize the position of every crossing point pi along its respective segment ci with the
parameter ui ∈ [0, 1]. When ui = 0, the crossing point pi is positioned on one end of the
segment ci. As ui grows, the crossing point pi is shifted continuously towards the other end
of ci.
Let α be the concatenation of time optimal path segments connected at the crossing
points. Then for every specific selection of the parameters u1, ..., uk the complete path is
uniquely determined and the travel time along it can be computed. The travel time T (α) can
now be written in terms of the parameters u1, ..., uk, as T (α(u1, ..., uk)), which is continuous
over the compact domain u1×...×uk ∈ [0, 1]×, ..., [0, 1], and thus possesses a global minimum
in this domain. Let us accept this global minimality as a fact, and proceed to show that
every local minimum of T (α) lies in the interior of F . Braking safety requires that the robot
must travel with zero speed along any obstacles’ boundary segment, thus giving T (α) = ∞
along any boundary segment. It follows that a local minimum path of T (α) can only touch
isolated obstacle boundary points otherwise T (α) =∞.
If such an isolated point lies in the interior of a polygon edge or at a concave polygon
vertex, the path can be locally shortened and moved away from the obstacle, resulting in
a reduced total travel time along the modified path. A local minimum path of T (α) can
therefore touch only isolated convex obstacle vertices. Suppose the path touches a convex
obstacle vertex as depicted in Figure 3.3(a). The path locally consists of two cycloids that
reach the vertex along the respective edge normals. Such a path can be moved away from
the vertex as depicted in Figure 3.3(b), resulting in a shorter path that maintains larger
clearance from the obstacle. The modified path has a reduced total travel time, implying
that any local minimum path of T (α) must lie in the interior of F . �
Calculus of variations is next used to establish the C(1) smoothness of the safe time
optimal path.
Proposition 3.4.3 (Optimal Path Smoothness). Every safe time optimal path connect-
ing pS and pT in F is a C(1) curve having a continuous tangent.
64
Proof: Every obstacle feature in the environment (a polygon vertex or an edge) defines
a proximity cell, consisting of all points in F which are closest to the given obstacle feature.
The proximity cells partition the free space F , and every path connecting pS and pT in F
consists of segments that lie inside individual proximity cells. Let α∗ be a safe time optimal
path connecting pS and pT in F . This path lies in the interior of F according to Prop 3.4.2.
The path satisfies the Euler-Lagrange equation in each proximity cell, and hence forms a C(1)
curve within each cell. Let two segments of α∗ meet at a corner point, p0 = α∗(s0), located
on the common boundary of two proximity cells in F . The term ∂F (α, α′)/∂α′ appearing
in the Euler-Lagrange equation is a continuous function of the path parameter s along any
extremal path of T (α) [29][Section 15].3 Using the expression for F (α, α′) specified in (3.15),
the derivative ∂F (α, α′)/∂α′ is given by
∂F (α, α′)
∂α′=
1
L(α) · ‖α′(s)‖α′(s),
where L(α) =√
2amin ·mini=0...k {dst(α,Oi)}. Continuity with respect to s at p0 gives:
1
L(α∗((s−0 ))) · ‖α′∗(s−0 )‖α′∗(s−0 ) =
1
L(α∗((s+0 ))) · ‖α′∗(s+
0 )‖α′∗(s+
0 ). (3.17)
Since L is a continuous function of s, L(α∗(s−0 )) = L(α∗(s+0 )), eq. (3.17) implies that
α′∗(s−0 ) = α′∗(s+0 ), and α∗ is therefore a C(1) curve. �
The next lemma establishes the minimality of the extremal paths of T (α).
Lemma 3.4.4 (Optimal Path Minimality). Every extremal path of the safe travel time
functional T (α) which lies in the interior of F is a local minimum of T (α).
Proof Sketch: We will use the environment’s x coordinate as the robot’s path parameter
s. The robot’s path is thus parameterized by α(x) = (x, y(x)) for x ∈ [xS, xT ].
Consider the travel time functional T (α),
T (α) =
∫ xT
xS
F(x, y(x), y′(x)
)dx.
3This property is known as Erdman’s corner condition in calculus of variations.
65
where
F =
√1 + y′2(x)
2amax ·mini=1...k{dst (α(x), Oi)}dx.
Sufficient conditions for a path α(x) = (x, y(x)) to be a local minimum of T (α) are:
1. The path α is an extremal of T (α).
2. Along the path α, the second derivative ∂2F∂α′2 (α, α′, s) = ∂2F
∂y′2 (y, y′, x) > 0.
3. The interval [xS, xT ] contains no points conjugate to xS.
See ([29](chapter 5)) for more details.
The first condition stated above is clearly satisfied since every extremal path under
consideration must be a solution of the Euler-Lagrange equation given by, ∂F∂α −
dds
∂F∂α′ = 0,
or in cartesian coordinates as follows:
∂F
∂y− d
dx
∂F
∂y′= 0.
Moreover, each extremal is C(1) since it satisfies the Erdmann corner condition as described
by Prop. 3.4.3.
Next, consider the functional F =
√1+y′2(x)
2amax·mini=1...k{dst(α(x),Oi)}dx. Its second derivative
with respect to y′ is given by,
Fy′y′ =1√
2amax ·mini=1...k{dst (α(x), Oi)}· 1
(1 + y′2)3/2> 0.
Since Fy′y′ > 0, the second condition stated above is met not only along an extremal path
but also along every other path. The condition is also known as the strengthened Legen-
dre Condition. Note that necessary conditions only require semi positiveness of Fy′y′ and
therefore, here the strengthened form is met.
Note that generally, the integrant F (y, y′) must be continuously differentiable with re-
spect to y(x) and y′(x). In our case, F is continuously differentiable with respect to y′ but
only piece-wise differentiable with respect to y. At points where two obstacles are equidistant
F loses its differentiability due the minimum function in its denominator. However, these
points coincide with the corner points of the path and hence in every interval between two
66
subsequent corner points the integrant F is continuously differentiable.
Last, we will omit a formal proof of the third condition, also known as the Jacobi condition
and assume that there are no points conjugate to xS along an extremal path. This is a
common assumption in the literature and in many applications, the absence of conjugate
points is validated by substituting the computed extremal solution in the Jacobi equation (For
e.g. [82](p. 361),[83](p.58),[29](p.112),[84](p.161)). This allows us to derive the minimality
of α. �
Note that although sufficient conditions for a minimum are not fully satisfied, neces-
sary conditions are met. Results appearing in the continuation of this thesis support this
minimality (For e.g. see Sec. 3.6.1).
The following theorem establishes the uniqueness of the safe time optimal path in each
path homotopy class connecting pS with pT in F .
Theorem 2 (Optimal Path Uniqueness). The safe travel time functional, T (α), pos-
sesses a unique time optimal path in each path homotopy class connecting pS with pT in
F .
Proof sketch: According to Prop 3.4.2, T (α) attains a global minimum in every path
homotopy class connecting pS with pT in F . Suppose one of these path homotopy classes
contains two local minimum paths, α∗ and β∗. Since α∗ and β∗ belong to the same path
homotopy class, the two paths can be joined by a path homotopy, f(s, t) : [0, 1]× [0, 1]→ F ,
such that f(s, 0) = α∗(s) and f(s, 1) = β∗(s) for s ∈ [0, 1]. The path homotopy f(s, t) can
be interpreted as a continuous path connecting α∗ with β∗ in the metric space of piecewise
smooth paths in IR2.
The functional T (α) is a continuous function of α. According to the mountain pass theo-
rem [53, 81], the collection of all continuous paths (each being a path homotopy) connecting
α∗ and β∗ contains a distinguished path along which the maximal value attained by T (α)
is minimized.4 Moreover, the maximal value is attained at an extremal path, γ, which is
necessarily a saddle point of T (α). The path γ must lie in the interior of F based on the
following argument. Braking safety implies that γ can only touch isolated obstacle boundary
4The mountain pass theorem requires that T (α) be defined on a closed and bounded set of pathsin the d1 metric space [53]. This condition is met on the set of piecewise optimal paths connectingpS with pT in F .
67
points. Suppose γ touches an obstacle vertex. Consider a path homotopy between α∗ and
β∗ obtained by locally pulling the path homotopy containing γ away from the vertex as
depicted in Figure 3.3. The travel time along the modified path homotopy is reduced, and
the maximal value attained by T (α) along the modified path homotopy is lower than T (γ).
Therefore γ must lie in the interior of F . This leads to a contradiction, since every extremal
path of T (α) in the interior of F must be a local minimum according to Lemma 3.4.4. It
follows that T (α) attains a unique local minimum in each path homotopy class connecting
pS with pT in F . �
The last Prop. considers cases where the time optimal path coincides with a Voronoi arc.
Proposition 3.4.5 (Voronoi edge optimality). Given start and target positioned along
an individual straight or parabolic Voronoi arc e, the time optimal path α∗ coincides with the
segment of e bounded by pS and pT .
Proof Sketch: First, assume that e is a Straight Voronoi arc. When pS and pT belong to
e, the shortest path connecting them is the portion of e bounded by pS and pT . Therefore
this portion of e must be the time optimal path connecting pS and pT since it maximizes the
robot’s clearance and speed and additionally ensures minimal travel distance.
Next consider a parabolic Voronoi arc spanned between a wall segment and a point
obstacle. Since this type of Voronoi arc is known of having a parabolic shape [7], its formula
can be readily parameterized by polar coordinates with the point obstacle being at the origin
as follows:
r(θ) = y01 + sin(θ − θ0)
cos2(θ − θ0), (3.18)
where y0 is the distance between the wall segment and the point obstacle, θ0 is the polar angle
in which the parabolic arc acquires its minimal clearance from the obstacles and θ ∈ [θS , θT ].
Note that (3.18) is identical to the time optimal path near a point obstacle (3.12) with
2c2 = y0 and hence it is optimal with respect to this obstacle feature.
Consider small perturbations of the path away from e towards the wall segment. Such
small perturbations will increase the total length of the path. Moreover since e is equidis-
tant from the wall segment and the point obstacle, any small perturbation of the path will
68
1o
(b)
speed graph edges
speed graph nodes
Sp
Tp
1e
2e
3e4e
2o1o
(a)
2o
Figure 3.4: (a) The Voronoi diagram, and (b) The speed graph of an environment populatedby two polygonal obstacles (Voronoi arcs are solid curves, speed graph edges are dashedcurves).
necessary reduce the permitted safe speed. Therefore e is optimal. �
3.5 The Speed Graph
This section describes the speed graph, which will allow selection of the path homotopy class
that most likely contains the global time optimal path connecting pS with pT in F . The
speed graph construction will use the following Voronoi diagram of F [8, 54].
Definition 14. The Voronoi diagram of a polygonal environment F is the collection of
points equidistant from at least two non-adjacent obstacle features. The equidistant points
form a network of Voronoi arcs that meet at Voronoi nodes.
The Voronoi diagram of F forms a connected network of curves which retains the path
homotopic structure of F . As shown in Figure 3.4(a), the Voronoi arcs reach every concave
corner in F and consequently partition the free space into distinct Voronoi cells. For instance,
the environment depicted in Figure 3.4(a) is partitioned into six Voronoi cells: two cells
surround the internal obstacles O1 and O2, and four cells are associated with the outer walls.
The Voronoi diagram of a polygonal environment with n obstacle features has O(n) Voronoi
arcs and can be computed in O(n log n) time [8]. This work assumes the Voronoi diagram of
F has been computed and is available for the following speed graph construction. The speed
graph consists of safe time optimal arcs connecting points located on the Voronoi diagram
as next described.
69
Definition 15. The speed graph of F is the weighted graph having the following structure:
1. The speed graph nodes are points on the Voronoi arcs where the obstacles’ equidis-
tance function attains its minimal value along the respective Voronoi arc (possibly at
its endpoint).
2. The speed graph edges are safe time optimal paths connecting every pair of nodes
that lie on the boundary of a common Voronoi cell of F .
3. The cost of each edge is the value of the safe travel time functional, T (α), along this edge.
There are O(n) Voronoi arcs in a polygonal environment having n obstacle features [8],
and each Voronoi arc contains a single speed graph node. The speed graph thus consists of
O(n) nodes and O(n2) edges.
Example: The environment depicted in Figure 3.4(b) is populated by two polygonal ob-
stacles O1 and O2. The figure shows the speed graph nodes, as well as the principle layout
of the speed graph edges whose construction is discussed below. Note that each Voronoi
arc contains a single speed graph node, and that every pair of adjacent Voronoi arcs is con-
nected by a speed graph edge. Since the Voronoi diagram retains the path homotopy classes
of F , the speed graph contains all the path homotopy classes of F . For instance, the path
homotopy class connecting pS to pT while passing between O1 and O2 in Figure 3.4(b) is
represented by the speed graph path along its edges e1, e2, e3, and e4. ◦
Construction of Speed Graph Edges: Consider two speed graph nodes, p1 and p2,
located on the boundary of a Voronoi cell associated with a polygonal obstacle Oi. As
illustrated in Figure 3.5(a), the edges and vertices of Oi define proximity cells, each consisting
of the points in F closest to the given obstacle feature. The two speed graph nodes are
separated by a finite number, say m + 1, of proximity cells. For instance, the nodes p1 and
p2 in Figure 3.5(a) are separated by three proximity cells associated with a vertex and two
incident edges of Oi. The boundary between adjacent proximity cells forms a line segment
denoted lj for j = 1, . . . ,m.
The computation will focus on the piecewise time optimal paths connecting p1 and p2,
consisting of time optimal path segments within each proximity cell and corner points that
can freely vary along the proximity cells’ boundary lines. The corner points are parameterized
70
10 10.5 11 11.5 12 12.5 13 13.5 1410
10.5
11
11.5
12
12.5
13
13.5
14
1u
2u
*1u
*2u
1p
*2u
*1u
(a) (b)
2p
proximity cell
boundary lines
speed
graph
edge
* *1 2( , )u u
iO
1u
2u
initial path
Figure 3.5: (a) Two speed graph nodes, p1 and p2, are separated by three proximity cellswhose boundary lines are parameterized by (u1, u2). (b)T (γ(u1, u2)) is star shaped, moreoverits contours are convex in the (u1, u2) plane.
by p(uj) : [0, 1] → lj for j = 1, . . . ,m. For instance, Figure 3.5(a) depicts two corner points
p(u1) and p(u2). Since each of the time optimal path segments is uniquely determined by
its two endpoints, the safe travel time functional T (α) becomes a function of u1, . . . , um. To
compute the time optimal path between p1 and p2 (and hence the speed graph edge), we will
use the following Monotonic property of the travel time functional T (α). The proof of the
following Lemma appears in Appendix C.
Lemma 3.5.1. Let γ(u1, . . . , um) parameterize the piecewise time optimal path segment con-
necting the nodes p1 and p2 in F while crossing the proximity cell boundary lines l1, . . . , lm.
Let u∗ = (u∗1, . . . , u∗m) be the parameter values of the optimal crossing points. The functional
T (γ(u1, . . . , um)) is monotonically increasing along any ray emanating from u∗ in the set
(u1, . . . , um) ∈ [0, 1]× · · · × [0, 1].
The monotonicity of T (γ) leads to the following observation. In general, the level set S of
a scalar valued function f(u) is star shaped when there exists a center point, u0 ∈ IRn, such
that every point in S lies on a unique ray emanating from u0. In particular, convex level
sets are always star shaped. Since T (γ(u)) is monotonically increasing along rays emanating
from u∗, its level sets form star shaped sets as illustrated in the following example.
Example: Consider the speed graph nodes p1 and p2 depicted in Figure 3.5(a). The two
nodes are separated by three proximity cells. The proximity cell boundary lines are param-
eterized by (u1, u2) ∈ [0, 1] × [0, 1], and the convex contours of T (γ(u1, u2)) are depicted in
Figure 3.5(b). Starting with an initial path α0, convex optimization of T (γ(u1, u2)) yields
71
the optimal parameter values (u∗1, u∗2) corresponding to the speed graph edge connecting p1
and p2. ◦
Based on Lemma 3.5.1, every speed graph edge can be computed as a convex optimiza-
tion of T (γ(u1, . . . , um)) on the set (u1, . . . , um) ∈ [0, 1] × · · · × [0, 1]. Convex optimization
algorithms such as the ellipsoid algorithm run in O(m2 log(1/ε)) steps [51], where m is the
number of corner points and ε is the desired solution accuracy. Convex optimization requires
evaluation of the gradient of T (γ(u1, . . . , um)), a computation that takes O(m) operations
and increases the total run time to O(m3 log(1/ε)). It is worth mentioning that our examples
were prepared with the more practical Newton-Raphson’s method. While lacking a global
run time bound, it is easier to implement and rapidly converges to an ε accurate solution;
see Section 3.6.1 for implementation details.
The Voronoi saturation problem:
In Section 3.3, analytic solutions for time optimal paths near a wall segment obstacle and
a single point obstacle were derived. In a real environment which includes multiple internal
obstacles, the time optimal path under the influence of one obstacle may emerge out of its
Voronoi cell and penetrate into a neighboring Voronoi cell, as illustrated in the next example.
Motivational example: Figure 3.6 depicts a rectangular environment containing three
point obstacles, O1, O2, O3. The points pS and pT are positioned within the same Voronoi
cell. The Voronoi edges passing between the obstacles are also depicted in the figure. Three
possible paths are sketched. The path γ is a time optimal path computed as if only obstacle
O1 is present. Note that γ crosses the Voronoi edge at points a and b and penetrates the
neighboring Voronoi cell. The path β includes three segments. The first and last segments,
are the portions of γ connecting pS with a and pT with b. The middle segment of β, coincides
with the portion of the Voronoi diagram between points a and b. The path α is the true
time optimal path connecting pS with pT in this environment. Note that although γ was
computed analytically, it is not time optimal since path β reduces γ’s total travel time. ◦
In order to find the true time optimal path, a saturation method is proposed:
(i) Compute the time optimal path as if only obstacle O1 exists (Path γ).
(ii) If the computed path does not exceed from the Voronoi cell of O1, it is the time optimal
72
voronoi arcs
oaob
a b
1p
2p
3o
2o
1ospeed graph node
speed graph node
Figure 3.6: The Voronoi saturation problem.
path connecting the two points. If it does, compute the points a and b where the path
γ crosses the Voronoi edge.
(iii) The time optimal path between a and b is the portion of the Voronoi edge between
these points (Prop. 3.4.5). Thus, connect points a and b by tracing the Voronoi edge.
This produces a new path, β, such that T (β) < T (γ).
(iv) The path β does not belong to C(1). Therefore, move the points a and b to ao and bo
until the complete path has a continuously varying tangent at these points. As stated
below (Lemma 3.5.2), this path, denoted α, is the time optimal path connecting points
pS and pT .
Note that when the original path, γ, penetrates into two neighboring cells, a very similar
technique can be applied.
Lemma 3.5.2 (Path saturation). A path α from pS to pT which includes several path
segments is the time optimal path if it satisfies the following conditions:
1. Each individual path segment is time optimal between its two endpoints.
2. At each join point of two path segments, the path has a continuously varying tangent
α′.
Proof. Any piecewise optimal path α, connecting pS with pT , is an extremal path if it has
smooth corners satisfying the Weierstrass Erdmann corner condition. According to Theorem
2 only one extremum exists in each homotopy class and hence, α must be that extremum.
Based on Lemma. 3.4.4, this extremal path is also the global time minimal path.
73
3.6 Speed Graph Based Computation of the Safe
Time Optimal Path in Polygonal Environments
This section describes a speed-graph based scheme for computing the safe time optimal path bet-
ween two points pS and pT in F . As a preliminary step, pS and pT are connected with safe
time optimal arcs to the speed graph nodes lying on the boundary of their Voronoi cells.
The latter arcs can be computed with the convex optimization scheme used to compute the
speed graph edges in Section 3.5. The minimum cost path from pS and pT is next computed
along the augmented speed graph using any standard graph search algorithm. As the speed
graph contains O(n2) edges, the minimum cost path can be computed in O(n2 log n) time,
where n is number of obstacle features in the environment. The resulting minimum cost
path, denoted αo, consists of safe time optimal arcs (the augmented speed graph edges),
meeting non-smoothly at the speed graph nodes.
The path αo indicates the most promising path homotopy class in terms of safe travel
time from pS and pS in F . It will next serve as the initial path for computing the exact
time optimal path in the chosen path homotopy class. The computation will focus on the
piecewise time optimal paths connecting pS and pT , consisting of safe time optimal path
segments in each Voronoi cell, and corner points that can freely slide along the Voronoi arcs
of F . Our objective would be to slide the corner points of αo along the Voronoi arcs, until
the entire path becomes a C(1) curve. In calculus of variations [29][Section 15], any piecewise
optimal path having continuous tangents at its corner points is automatically an extremal
path of the respective functional. Hence the resulting C(1) curve, denoted α∗, is an extremal
path of T (α). Based on Theorem 2, α∗ is the safe time optimal path connecting pS and pT
in the path homotopy class of αo in F .
The path smoothing scheme first identifies which Voronoi arcs of F , termed the optimal
Voronoi arcs, are crossed by α∗. Let p1, . . . , pm denote the Voronoi crossing points of αo. By
inspecting the acute angle spanned by the tangents of αo at these points, one can determine
in O(m) steps the identity of the optimal Voronoi arcs. This property is formally justified
in Appendix C (see proof of Proposition 3.6.1), and is the basis for the following procedure.
74
smoothing direction
smoothing direction
optimal voronoi arc
voronoi edge
new crossing points
smoothing direction
(a) (b)
Sp
Tp
ip
o
*
*
o
'oie
1ie
2ie
'o
Tp
Sp
ip
'i
p
'i
p
Figure 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 Voronoiarcs ei1 and ei2.
Optimal Voronoi arcs procedure:
Input: Voronoi crossing points of the initial path α0, pi for i = 1, . . . ,m.
Repeat for each Voronoi crossing point:
1. Set the local smoothing direction of αo at pi along the acute angle spanned by the tan-
gents to the segments of αo at pi (Figure 3.7(a)).
2. Determine the Voronoi node p′i towards which pi should move according to the local
smoothing direction.
3. Compute the piecewise time optimal path, α′o, passing through p′i (instead of pi) while
all other Voronoi crossing points are held fixed.
4. Set the local smoothing direction of α′o along the acute angle spanned by its tan-
gents at p′i.
5. Compare the local smoothing directions of αo and α′o:
5.1. If the smoothing directions at pi and p′i point toward each other, mark the Voronoi
arc containing these points as optimal (Figure 3.7(a)).
5.2. Otherwise, mark the two neighboring Voronoi arcs emanating from p′i as optimal.
Replace pi with two crossing points located on the neighboring Voronoi arcs at p′i
(Figure 3.7(b)).5
End of repeat loop.
5When the new crossing points slide along the neighboring Voronoi arcs, the time optimal segmentbounded by these points bends away from its obstacle feature, thus generating acute angles that pointback to p′i. Hence there will be no subsequent splitting of the two Voronoi crossing points.
75
Let e1, . . . , en for n ≥ m denote the optimal Voronoi arcs identified by the above proce-
dure. For instance, two such optimal Voronoi arcs, e1 and e2, are depicted in Figure 3.8(a).
The computation of α∗ will focus on the piecewise time optimal paths connecting pS with
pT while crossing the Voronoi arcs e1, . . . , en at points that freely slide along these arcs.
The Voronoi crossing points are parameterized by p(ui) : [0, 1] → ej for i = 1, . . . , n (Fig-
ure 3.8(a)). Since each of the piecewise time optimal paths connecting pS and pT in F is
uniquely determined by its Voronoi crossing points p(ui) for i = 1, . . . , n, the safe travel time
functional T (α) becomes a function of u1, . . . , un. The computation of the safe time optimal
path will use the following monotonicity property of T (α).
Proposition 3.6.1. Let α(u1, . . . , un) parameterize the piecewise time optimal paths con-
necting pS and pT in F while crossing the optimal Voronoi arcs e1, . . . , en. Let u∗ =
(u∗1, . . . , u∗n) be the parameter values of the optimal Voronoi crossing points. The functional
T (α(u1, . . . , un)) is monotonically increasing along any ray emanating from u∗ in the set
(u1, . . . , un) ∈ [0, 1]× · · · × [0, 1].
The proof of Proposition 3.6.1 appears in Appendix C. The monotonicity of T (α) leads to
the following observation. In general, the level set S of a scalar valued function f(u) is
star shaped when there exists a center point, u0 ∈ IRn, such that every point in S lies on
a unique ray emanating from u0. In particular, convex level sets are always star shaped.
Since T (γ(u)) is monotonically increasing along rays emanating from u∗, its level sets form
star shaped sets as illustrated in the following example.
Example: Consider the polygonal environment depicted in Figure 3.8(a). The safe time op-
timal path connecting pS and pT in this environment, α∗, crosses the Voronoi arcs e1 and e2.
The piecewise time optimal paths connecting pS and pT while crossing e1 and e2 are param-
eterized by their Voronoi crossing points p(u1) and p(u2), where (u1, u2) ∈ [0, 1]× [0, 1]. As
shown in Figure 3.8(b), the contours of T (γ(u1, u2)) in the (u1, u2) plane are star shaped
(and convex), with a center point at the optimal parameter values (u∗1, u∗2) corresponding to
α∗. ◦
Convex optimization algorithms require that the level sets of T (γ(u)) be convex. The
same algorithms can be applied when the level sets are star shaped with center point at the
optimal parameter values u∗. This observation is based on the relation∇T (γ(u))·(u−u∗) ≥ 0
76
9 10 11 12 13 149
10
11
12
13
14
15
1u
2u
*1u
*2u
Sp
Tp
*2u
*1u
(a) (b)
iO
*
1e
2e1u
2u
* *1 2( , )u u
Figure 3.8: (a) The piecewise time optimal paths connecting pS and pT while crossing theVoronoi arcs e1 and e2 are parametrized by (u1, u2). (b) The contours of T (γ(u1, u2)) arestar shaped (and convex) with center at (u∗1, u
∗2).
satisfied by star shaped level sets, which allows construction of a barrier function (or a bound-
ing ellipsoid) in each iteration of the convex optimization process. Standard convex opti-
mization algorithms run in O(n3 log(R/ε)) time, where R is the maximal pointwise distance
between the initial path αo and the exact time optimal path α∗, and ε is the desired solution
accuracy [51]. Note that the number of Voronoi crossing points, n, is upper bounded by
the number of obstacle features in the environment. Here, too, we used Newton-Raphson’s
method in the optimization of T (γ(u)) as next explained.
3.6.1 Path Optimization Using Newton’s Method
Recall that the time optimal path α∗ is known to have a continuously varying tangent
(Prop. 3.4.3). Thus, the computation of the optimal crossing points p1, ..., pn which lie on
their respective Voronoi arcs e1, . . . , en an be also reduced into a system of n equations in n
variables:
S(p1, ..., pn) =
Ang(α′1(p1)− α′2(p1))
...
Ang(α′n(pn)− α′n+1(pn))
= ~0, (3.19)
where pi ∈ ei for i = 1...n and Ang(α′i(pi) − α′i+1(pi)) is angle between the tangents of the
two path segments meeting at pi. The expression for α′i is derived by differentiating the time
77
optimal path given in Section 3.3 for point and wall segment obstacles. For a point obstacle,
the time optimal path is parameterized by polar coordinates, α(θ) = (r(θ) cos θ, r(θ) sin θ)
for θ ∈ [θS , θT ], and the derivative dr/dθ at pi is given by,
dr
dθ(pi) = ri
√ri
2ac2− 1, (3.20)
where ri is the distance measured from the point obstacle to the crossing point pi, and c
is determined by the path’s end conditions. For a wall segment obstacle, the time optimal
path is parameterized by cartesian coordinates, α(x) = (x, y(x)) for x ∈ [xS , xT ], and the
derivative dy(x)/dx at pi is given by,
dy
dx(pi) =
√1
2ac2piy− 1, (3.21)
where pi = (pix, piy) is the crossing point and c is determined by the path’s end condi-
tions. Note that (3.19) requires that (3.20) and (3.21) be expressed in a common system of
coordinates.
Based on the expressions for the path derivatives, the optimal crossing points, p∗1, ..., p∗n,
can be computed using any numerical method, such as Newton’s method. At the i’th iteration
of Newton’s method, a new set of crossing points, pi = (pi1, ..., pin), is computed. Let Ei =
maxj=1...n{|p∗j − pij |} be the i’th error measure and denote by J the n× n Jacobian matrix
of S(p1, ..., pn). When the initial guess, p01, ..., p
0n, is sufficiently close to the optimal solution,
quadratic asymptotic convergence is guaranteed, |Ei+1| < κ|Ei|2, where κ is a positive
constant defined by properties of S(p1, ..., pn) ([34], p.115). When J has a full rank and
(3.19) is known to have a unique solution, the method is robust with respect to the exact
position of the initial crossing points p01, ..., p
0n. Each iteration of Newton’s method requires
O(n3) steps, and in practice it suffices to run 5-10 iterations in order to generate a practically
accurate path. While lacking a global run time bound, it is easier to implement and rapidly
converges to the exact solution as indicated in the following example.
Example: Figure 3.9 depicts a disc robot navigating in a 10 × 10 meters room populated
by four point obstacles. The speed graph of the environment is depicted in Figure 3.9(a).
Next, let pS and pT be located at two speed graph nodes as indicated in Figure 3.9(a). The
78
0 20 40 60 80 1000
10
20
30
40
50
60
70
80
90
100
0 20 40 60 80 1000
10
20
30
40
50
60
70
80
90
100
1O
2O
3O
4O
(a)
Sp
Tp
1O
2O
3O
4O
(b)
Sp
Tp
1O
2O
3O
4O
*
(c)
o
1e
2e
3e
*
1u
*
2u*
3u
robot
robot
Sp
Tp
1u
2u
3u
Figure 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 pSand pT in F .
minimum cost path from pS to pT on the speed graph, αo, is indicated in Figure 3.9(b). Note
that αo is non-smooth at its Voronoi crossing points. These points are next used to identify
which Voronoi arcs are crossed by the exact solution α∗. These arcs are marked as e1, e2, and
e3 in Figure 3.9(b). Using (u1, u2, u3) to parameterize the optimal Voronoi arcs, Newton-
Raphson’s method was used to compute the exact safe time optimal path connecting pS and
pT in the chosen path homotopy class. Using maximal deceleration of amin = 1 m/sec2 for
the robot, the travel time along the initial speed graph path was T (αo) = 7.8 seconds, while
the travel time along the safe time optimal path was reduced to T (α∗) = 7.6 seconds. As
depicted in Figure 3.10, Newton-Raphson’s method converged to α∗ after only five iterations,
with travel time fluctuations of less than 0.00001 seconds in subsequent iterations. The safe
time optimal path connecting pS with pT in F is the C(1) curve α∗ depicted in Figure 3.9(c).
More elaborate simulations of the speed graph method are next described.
A remark on the k-homotopy classes: A planar environment populated by k internal
obstacles may contain up to 2k homotopy classes of non self-intersecting paths6. While the
speed graph usually suggests the most promising homotopy class in terms of total travel
time subject to safe braking constraint, a standard k-best algorithm can compute the k
best paths on the speed graph in order to suggest the k-best homotopy classes in terms
of total travel time. Computing the k best paths along the speed graph can be done in
O(e + vlog(v) + klog(k)) steps, where e is the total number of edges and v is the total
6The homotopy classes can be easily counted using the Voronoi diagram: each Voronoi node hasa degree of three, hence at any node the path can be split into two paths creating two new homotopyclasses of paths.
79
1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 65.77
5.775
5.78
5.785
5.79
5.795
5.8
5.805
5.81
5.815
5.82
Iteration
Tra
vel t
ime
1 2 3
travel time [sec]
Iteration1 1.5 2 2.5 3 3.5 4 4.5 5
7.6
7.7
7.8
7.9
8
8.1
8.2
8.3
*
0
54
Figure 3.10: The Newton-Raphson’s travel time convergence during five iterations on thepaths of Figure 3.9.
number of vertices on the speed graph [23]. The total number of edges, e, is upper bounded
by v(v−1)/2. Since each vertex of the speed graph is located on an individual Voronoi edge,
the total number of vertices, v, equals the number of Voronoi edges in the environment, which
is linear in the number of the polygonal obstacles’ features, denoted f [61]. Hence, the k-best
time optimal paths on the speed graph can be computed in O(f2 +klog(k)) steps. Note that
when computing the k-best paths on the speed graph, two paths on the graph from the same
homotopy class may be selected. Therefore, it is necessary to eliminate paths from repeating
homotopy classes.
3.7 Numerical Simulations and Experimental Setup
This section describes simulations of the speed graph method in environments that resemble
an industrial warehouse and an office floor. The section concludes with a brief mention of
initial experiments, highlighting certain weaknesses of current mobile robot platforms.
Warehouse environment: Figure 3.11 depicts a warehouse containing eight storage racks,
O1, . . . , O8, surrounded by corridors. The warehouse measures 30 × 32 meters, while each
storage rack measures 5×10 meters with one meter clearance between neighboring racks. The
outer corridors are four meters wide, except the rightmost corridor which is seven meters
wide. A cylindrical mobile robot is serving a delivery counter located at the warehouse’s
lower right corner. The robot has a diameter of 80 cm, and its maximal deceleration has
80
0 50 100 150 200 250 300 3500
50
100
150
200
250
300
(b)
0 50 100 150 200 250 300 3500
50
100
150
200
250
300
(a)
2o
Sp
Tp
*
*
1o
3o4o
5o 6o
7o8o
2o1o
3o4o
5o 6o
7o8o
Sp
Tp
delivery counter delivery counter
0
Figure 3.11: (a) The speed graph of the warehouse environment. (b) The safe time optimalpath α∗ selected by the speed graph, and the safe time optimal path α∗ associated with thegeometrically shortest path.
0 10 20 30 40 50 60 700
0.5
1
1.5
2
2.5
3 /secm
v
sect
*
*
42.931.20
Figure 3.12: The speed profile along the paths α∗ and α∗ in the warehouse environment.
been set to amin = 1 m/sec2, which is typical for autonomous robots operating in warehouse
environments.
To synthesize the speed graph for this environment, we located the speed graph nodes
on the Voronoi diagram of the environment, then computed the safe time optimal arcs
connecting neighboring nodes. The speed graph of the warehouse environment is depicted in
Figure 3.11(a). A start point pS was next selected at the delivery counter, while a target point
pT was selected at the lower corner of the storage rack O1. The minimum cost path connecting
pS to pT along the speed graph, αo, is depicted in Figure 3.11(a). Note that αo passes through
the outer corridors, as these corridors allow significantly higher speeds than the narrow inner
corridors. The path αo initialized the computation of the safe time optimal path within the
path homotopy class suggested by the speed graph. This computation generated the path
81
(a) (b)
Sp
*
Tp0 50 100 150 200 250 300 350 400 450 500
0
50
100
150
200
250
300
350
400
SpTp*
2 =
3 4
5 6
*
1=
Figure 3.13: The six best safe time optimal paths in the office floor environment.
α∗ depicted in Figure 3.11(b), which has a total travel time of 31.2 seconds.
To highlight the speed graph usefulness in suggesting the time optimal path homotopy
class, we also computed the safe time optimal path in the homotopy class of the geometrically
shortest path connecting pS to pT in the warehouse environment. The latter path homotopy
class starts at pS, passes between O4 and O6, then between O3 and O4, and finally reaches pT
while passing between O1 and O3. Again, we first computed the speed graph path connecting
pS to pT in the selected path homotopy class, which then initialized the computation of
the safe time optimal path connecting pS to pT in the chosen path homotopy class. This
computation generated the path α∗ depicted in Figure 3.11(b). While α∗ is shorter than α∗,
its total travel time of 42.9 seconds is 40% longer than T (α∗). Figure 3.12 depicts the speed
profile along the two paths, indicating the higher (but still safe) speeds taken by the robot
along the outer corridors of the environment.
Office Floor Environment: Figure 3.13 depicts an office floor populated by obstacles
resembling pieces of furniture as well as inner walls. The office floor measures 30×40 meters
and contains 25 internal obstacles, with a total of 60 vertices in its polygonal representation.
While the speed graph of this environment is not shown, it contains 94 nodes (one node per
Voronoi arc of the environment), and 422 edges. This number of edges is much lower than
the theoretical upper bound of O(n2) edges mentioned in Section 3.5. The cylindrical robot
size is 20 cm, and its maximal deceleration was set to amin = 1 m/sec2. The speed graph
was next used to rank several path homotopy classes connecting that start and target points,
with the objective of highlighting the speed graph’s usefulness in identifying the best path
82
Path α1 α2 α3 α4 α5 α6
T (α) [sec] 27.9 33.8 43.6 47.7 54.3 57.4
Table 3.1: The safe time optimal travel time in the six best homotopy classes in the of-fice floor environment.
homotopy class in terms of safe travel time.
A start point pS was selected at the lower left corner of the conference room, while a target
point pT was set at the lower chair in the neighboring office. The speed graph was computed
for this environment, and its minimum cost path initialized a computation of the safe time
optimal path α∗ depicted in Figure 3.13(a). Next, the six-best path homotopy classes in
terms of total travel time were computed along the speed graph, using a standard k-best
graph search algorithm [23]. For each of these path homotopy classes, the speed graph paths
initialized a computation of the safe time optimal path connecting pS to pT in the respective
path homotopy class. These paths are marked as α1, . . . , α6 in Figure 3.13(b), and their
travel times are listed in Table 3.1. The path homotopy class suggested by the speed graph
contains the time optimal path among all six homotopy classes, α1 = α∗, with total travel
time of 27.9 seconds. As a comparison, the next best path homotopy class happened to be
associated with the geometrically shortest path connecting pS to pT . The travel time along
the second best path, α2 = α∗, is 33.8 seconds. The speed graph thus captured the time
optimal path homotopy class, as well as correctly ranked the k-best path homotopy classes.
Initial Experiments: An experimental validation of the speed graph method is challenging
for several reasons. First, the experiments require a truly fast mobile robot in order to
demonstrate the practical advantage of traveling along safe time optimal paths. However,
readily available platforms such as the iRobot Create can only move with maximal speeds of
about 1 m/sec [1]. Second, the speed graph stored in the robot’s on-board memory requires
accurate positioning, a notoriously hard problem in congested indoor environments. For
instance, the iRobot sensors include a front bumper that detects obstacle collision, and an
IR sensor that detects obstacles in the robot’s close vicinity [1]. However, the iRobot has no
global position sensor, and its position is estimated by odometric sensors mounted on the
robot’s wheels.
As a preliminary demonstration, we placed an iRobot platform at a docking station in
the ground floor of our office building. The robot was charged with the task of serving
83
fire extinguishers to various spots located as far as 50 meters from the docking station.
The robot received as input the floor’s speed graph, and delivered the fire extinguishers to
simulated fire breakups along safe time optimal paths computed along the speed graph of
the environment. Measurements taken during these experiments show that the iRobot was
able to reach sites located 50 meters away from its start point within 60 seconds once a
fire alarm was activated. However, the iRobot platform is not designed for such high speed
navigation tasks, and its poor localization system had difficulties in accurately following the
time optimal paths. Nevertheless, the experiments highlight the potential usefulness of high
speed navigation for applications such as first responder robots.
3.8 Conclusion
In this chapter we introduced the speed graph method which synthesizes time optimal paths
for a mobile robot navigating among polygonal obstacles subject to uniform braking safety
constraints. The safe braking constraint was encoded as a speed dependant safety zone
surrounding the robot’s current position. To find the safe time optimal path, the safe travel
time functional T (α) was formulated. Basic properties of T (α) guided the construction of the
speed graph for the environment. The speed graph nodes are located on the Voronoi diagram
of the environment, while its edges are safe time optimal paths connecting neighboring
speed graph nodes. Individual speed graph edges lie within particular Voronoi cells of the
environment. The chapter established that T (α) is a convex function over the piecewise time
optimal paths within any given Voronoi cell, thus leading to a computation of the speed
graph edges based on standard convex optimization techniques.
Once the speed graph is constructed for a given environment, the minimum cost path
connecting the start and target along the speed graph suggests the most promising path ho-
motopy class connecting the start and target in the environment. However, the speed graph
path is only piecewise time optimal, and the task of smoothing this initial path into a C(1)
curve connecting the start and target was next discussed. The initial path corner points
located at the speed graph nodes were allowed to slide along their respective Voronoi arcs.
The thesis then established that T (α) is a monotonic function with respect to rays emanating
from the parameters definition the C(1) time optimal curve. This result lead to a formula-
84
tion of the path smoothing process as a convex optimization problem. As demonstrated in
simulated environments, the speed graph suggests the path homotopy class containing the
globally time optimal path, as well as provides an initial path whose positional optimization
yields the safe time optimal path within the chosen path homotopy class.
Note that although not directly addressed by the calculus of variations formulation, the
turning radius constraint that prevents sliding events is satisfied by these paths. This could
be easily validated by computing the paths’ curvatures as suggested in Appendix D, then
comparing them with the minimal safe turning radius that prevents sliding (Eq. A.2).
The speed graph method is only a first step toward practical safe high speed mobile robot
navigation. Let us point out two extensions to this work currently under investigation. The
first extension concerns environments containing static as well as dynamic obstacles. For
instance, multiple mobile robots operating autonomously in industrial warehouses must be
able to move with high speed while maintaining braking safety constraints with respect to
all group members.
A second extension concerns the generalization of the circular braking safety zone into
a safety zone whose shape and size depends on the robot’s speed as well as heading. For
instance, when a mobile robot travels with high speed in a long narrow corridor, a non-
circular safety zone aligned with the corridor’s axis will allow the robot significantly higher
safe speeds than the conservative circular safety zone. A preliminary work on mobile robots
navigating under non-uniform braking safety constraints has recently appeared in [49].
85
Chapter 4
Conclusion
86
4.1 Summary
Navigating with high speed while ensuring safety for the mobile robot as well as to its
surroundings is becoming more demanding in today’s robotics. As mobile robots are traveling
with increasing speed their dynamics characteristics must be taken into account and be
synthesized into the path construction.
In this thesis we considered two main safety constraints involving high speed navigation:
(1) A speed dependant uniform safe braking constraint; (2) A turning radius constraint
that prevents sliding affects and tipping over. These constraints cover two different groups
of safety constraints. The first group considers speed, environmentally dependant, safety
constraints such as the safe braking constraint which depends on the robot’s clearance from
the obstacles. The second group considers speed, path dependant, safety constraints such as
speed limitations due to tight curvature of the path.
Additionally, the paths analyzed in this thesis satisfy a quality measure - the total travel
time. This quality measure requires to maximize the robot’s speed to complete the task as
fast as possible. Two approaches were taken to rigorously ensure the safety of the robot
throughout its navigation process. These approaches are purely analytic and suggest highly
efficient methods for computing the approximated time optimal path and then the globally
time optimal between obstacles subject to safety constraints. These two methods synthesize
the safety constraints into the path planning process rather than first computing a geo-
metrically collision free path and then parameterizing the speed along it such that velocity
constraints are met.
The first method discussed, denoted vc-method, models the uniform braking constraint
as vc-obstacles in position-velocity configuration space. We have implemented the Morse
Theory to characterize critical events where two vc-obstacles meet for the first time and
locally disconnect the free position-velocity configuration space. A cellular decomposition of
the free position-velocity space is described and is based on these critical events. Based on the
cellular decomposition, an adjacency graph is constructed, then a special search algorithm
retracts an approximated time-minimal path along this graph in O(n log n). Finally, the
path’s vertices are smoothed to ensure safe turning at the prescribed path velocity.
The second method presented, denoted speed graph method, encodes the braking
87
safety as a state dependant constraint, which in turn allows formulation of the safe time
optimal navigation problem using calculus of variations. The work presented here, analyzes
the properties of the travel time functional, T (α), then uses these properties to develop the
speed graph method which computes 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 environment. Convexity properties 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 desired solution accuracy.
Importantly, the two methods take place in the robot’s state space, thus treating the
geometric path planning and the speed profiling in a single framework. Table 4.1 compares
the two methods and indicates the advantages of each one of them.
Comparison Parameter VC Method Speed Graph Method
Output Produces a pseudo time opti-mal path based on a connec-tivity 3D network
Computes the time optimalpath based on a planar graph
Complexity runningtime
O(n log n) O(n3 log(1/ε))
Safety constraints Guarantees safety in two steps Safety constraints are satis-fied by a single computationalscheme
Mathematical foun-dations
Morse theory and topology Calculus of variations andconvex optimization
Main advantage Low computational runningtime
Computes the time optimalpath
Table 4.1: Comparison between the VC method and the Speed Graph method.
Next, we briefly discuss our current work which do not appear in this thesis but extends
its ideas to more real scenarios.
4.2 Current Work
Piloting along a high speed and curved path, as car racing drivers do, without the need to
maintain uniform braking safety clearance (see Figure 4.1 for our high speed inspiration
[12]), often results in a very high speed path which enforces an active speed dependant
88
Figure 4.1: A Formula One race car has to travel at high speed during turns without sliding.
turning radius constraint to prevent sliding affects. As discussed in this thesis, the minimal
turning radius that prevents side-sliding affects equals the squared speed, ν2, divided by µg,
thus requiring very large turning radii and high clearance zones when navigating with high
speeds. Moreover, in any practical system, the robot’s maximal acceleration is bounded by
a fixed value which depends on robot’s actuators and physical parameters such as robot’s
mass. Navigating along such a high speed path while trying to minimize the travel time (as
a race driver) creates a tradeoff between the path’s curvature and its length. As the robot
navigates with higher speeds, the required safe radius of curvature increases thus creating a
total longer path for the car to follow.
In this extension for our research, we analyze the time optimal navigation problem and
wish to compute an explicit solution which takes into account the constraints on robot’s speed
and turning radius while minimizing overall travel time. We analyze this problem using the
theory of time optimal control and impose Pontryagin’s minimal principle to derive time
optimal path primitives which are then composed into a global time optimal path. The
suggested computational scheme will allow to extract the global time optimal path and to
be implemented by off-line as well as on-line planners for high speed motion.
4.3 Future Extension
The thesis raises several issues that should be investigated in future research. Let us point out
three extensions to this work. The first extension concerns environments containing static
as well as dynamic obstacles. For instance, multiple mobile robots operating autonomously
in industrial warehouses must be able to move with high speed while maintaining braking
safety constraints with respect to all group members.
89
A second extension concerns the generalization of the circular braking safety zone into
a safety zone whose shape and size depends on the robot’s speed as well as heading. For
instance, when a mobile robot travels with high speed in a long narrow corridor, a non-
circular safety zone aligned with the corridor’s axis will allow the robot significantly higher
safe speeds than the conservative circular safety zone. A preliminary work on mobile robots
navigating under non-uniform braking safety constraints has recently appeared in [49].
Despite all this, note that the uniform braking safety which is considered in this thesis
is useful when the robot has to navigate in close proximity to adjacent obstacles and is
required to keep a sufficiently low speed to avoid collision with other moving agents or other
unexpected hazards. In such scenarios requiring uniform braking safety will ensure that the
robot can avoid these hazards subject to its sensing capabilities.
A third extension concerns the generalization of the planar disc robot to other types of
moving agents such as motor vessels piloting autonomously in dense commercial harbours.
These vessels are subjected to different dynamic constraints such as hydrodynamic resistance
and vessel’s heel that limits the boat’s handling through the water. In order to be safely
piloted autonomously these constraints should be taken into account when planning the high
speed boat’s path. An efficient on-line algorithm which regorosly ensure safety should be
implemented using the convexity tools developed by this thesis.
90
Appendices
91
Appendix A
The Turning Radius Constraint
This appendix develops an expression for the robot’s minimal turning radius, rmin(ν), as
a function of the robot’s parameters. We shall assume the common case of a four-wheeled
robot. However, the expression for rmin(ν) can be easily adapted to other mobile robots.
The three main constraints affecting a mobile robot turning radius are as follows. First, the
turning radius must respect the robot’s maximal steering angles. Second, the centrifugal
force affecting the robot must not incur radial sliding of the robot wheels. Third, the lateral
moment affecting the robot must be sufficiently low as to prevent tip-over during turning
maneuvers. The three constraints are next discussed.
We will use the following notations. The average steering angle of the robot’s front wheels
is denoted ω (Figure A.1(a)). This angle can vary in the range [−ωmax, ωmax]. The robot’s
wheels are mounted at the vertices of a rectangular frame having length l and width w (Fig-
ure A.1(a)). The robot’s center of mass is located at a height h above ground (Figure A.1(b)).
The robot’s turning radius, denoted r, is the current radius of curvature of the curve traced
by the robot’s center point. The robot’s inner turning radius, denoted rin, is the current
radius of curvature of the curve followed by the robot’s innermost wheel (Figure A.1(a)).
The kinematic turning radius constraint: Based on simple trigonometry, the average
steering angle ω is related to l, w, and rin by the expression:
tanω =l
w2 + rin
.
92
inr
CMr
l
. . .c o m
mg
CFf
sf sf
tip
left turn
(b)(a)
h
w
w
Figure A.1: (a) Top view of a four-wheel mobile robot turning geometry. (b) Rear view ofa left turning mobile robot.
The center point turning radius is given by
r =
√l2
4+(w
2+ rin
)2.
Replacing w/2 + rin with l/ tanω gives the turning radius:
r =
√l2
4+
(l
tanω
)2
ω ∈ [−ωmax, ωmax].
Since r is monotonically decreasing in |ω|, the minimal feasible turning radius, denoted rkin,
is attained at ω = ±ωmax. Thus r ≥ rkin, where
rkin= l
√1
4+
1
tan2(ωmax). (A.1)
The sliding constraint: Sliding occurs when the centrifugal force acting on the robot ex-
ceeds the radial friction force acting between the wheels and the ground. The net centrifugal
force, denoted fr, is given by
fr =mν2
r,
where m is the robot’s mass, ν is the robot’s linear velocity magnitude, and r is the robot’s
turning radius. The radial friction force acting between the wheels and the ground, denoted
fs, is given by
fs = µfN = µmg,
93
where µ is the friction coefficient between the wheels and the ground, and fN = mg is the
gravitational force magnitude acting downward on the robot. Sliding is prevented as long
as fr ≤ fs. Substituting for fr and fs, the minimal turning radius that prevents sliding,
denoted rslide, is given by
rslide(ν) =ν2
µg, (A.2)
where ν is the robot’s linear velocity magnitude.
The tipping-over constraint: Denote by τtip the net moment affecting the robot about
a horizontal axis passing through its outer wheels contacts with the ground. When τtip is
positive, the robot will tip-over and loose ground contact at its inner wheels (Figure A.1(b)).
The lateral moment generated on the robot by the centrifugal force, denoted τr, is given by
τr = h · fr =mh
rν2,
where h is the robot’s center of mass height above ground. The stabilizing moment generated
on the robot by the gravitational force is given by τg = −mgw/2. Tipping over is prevented
when τtip=τr + τg is negative semi-definite,
τtip = τr + τg =mh
rν2 −mgw
2≤ 0.
Tipping-over is prevented when r ≥ rtip, where rtip is given by
rtip(ν) =2h
gwν2. (A.3)
The robot’s minimal turning radius, denoted rmin, must satisfy all three turning radius
constraints. Based on formulas A.1, A.2, and A.3, the minimal turning radius is specified as
follows.
Lemma A.1. The minimal turning radius of a four-wheel mobile robot moving with linear
speed ν is given by
rmin(ν) = max {rkin, rslide(ν), rtip(ν)} ,
where rkin = l√
1/4 + 1/ tan2(ωmax) is the minimal kinematically feasible turning radius,
94
rslide(ν)=ν2/µg is the minimal sliding turning radius, and rtip(ν)=(2h/gw)ν2 is the mini-
mal tip-over turning radius.
95
Appendix B
The Time Optimal Path for a Disc
Robot
This appendix considers a disc robot of radius ρ traveling near a point obstacle located
at the origin of the (x, y) plane. Using polar coordinates, the path traced by the robot’s
center is parameterized by α(θ) = (r(θ) cos θ, r(θ) sin θ) for θ ∈ [θS, θT ], with the endpoint
conditions r(θS) = rS and r(θT ) = rT . The distance between the disc robot and the point
obstacle, denoted dr(θ), is given by dr(θ) = r(θ) − ρ along α. Note that dr(θ) must be
non-negative for all θ ∈ [θS, θT ] in order to prevent collision. Braking safety requires that
dr(θ) ≥ d(v(θ)). Substituting d(ν) = ν2/2amin and using the maximal allowed speed for
travel time minimization gives the constraint:
ν(r) =√
2amin(r − ρ) r ≥ ρ. (B.1)
The travel time functional T (α) is thus given by
T (α) =
∫ θTθS
F(r(θ), r′(θ)
)dθ,
where
F (r, r′) =
√r2 + r′2
2amin(r − ρ). (B.2)
96
Substituting for F (r′, r) in the Euler-Lagrange equation, then integrating once with re-
spect to θ, gives the implicit scalar differential equation:
r4(θ) = c2(r(θ)− ρ
)(r2(θ) + (r′(θ))2
), (B.3)
where c is yet to be determined. The analytic solution of (B.3) is the sum of two elliptic in-
tegrals. Rather then use elliptic integrals, a practical approximation for the analytic solution
of (B.3) is next described.
-20 -15 -10 -5 0 5 10 15 20-25
-20
-15
-10
-5
0
5
10
15
20
25
polygonal approximation
Sp
Tp
* *
1u 2u
3u
4u
1( )p u2( )p u
3( )p u
4( )p u
disc
obstacle
Figure B.1: The safe time optimal path determined by the polygonal approximation, α∗,and the exact path α∗ associated with the disc obstacle.
To compute an approximate solution path, expand the point obstacle into a disc obstacle
of radius ρ, while simultaneously shrinking the robot to a point. Next replace the portion
of the disc obstacle between θS and θT by polygonal segments tangent to the disc obstacle
as depicted in Figure B.1. The computation of the safe time optimal path in the vicinity
of the polygonal obstacle is a convex optimization problem according to Lemma 3.5.1. The
convex optimization scheme focuses on the piecewise time optimal paths connecting pS to
pT , consisting of time optimal path segments within each proximity cell determined by the
obstacle features (i.e. the points closest to a given obstacle vertex or edge), and corner points
that freely vary along proximity cells boundary lines. For instance, in Figure B.1 there are
five proximity cells with corner points p(u1), . . . , p(u4) that vary along four proximity cells
boundary lines.
Each of the time optimal path segments is uniquely determined by its two endpoints,
and the safe travel time functional thus T (α) becomes a convex function of the corner point
97
parameters. For instance, convex optimization functional T (α(u1, . . . , u4)) gives the safe time
optimal path α∗ in Figure B.1. Note that α∗ closely approximates the exact time optimal
path α∗, which was computed with trial and error of the precise initial conditions at pS that
lead to pT . Moreover, the quality of approximation improves as the number of segments used
in the polygonal approximation increases.
98
Appendix C
Computation of the Speed Graph
Edges and the Safe Time Optimal
Path as Convex Optimization
Problems
This appendix describes a convex optimization scheme. The scheme is first used to compute
the speed graph edges considered in Section 3.5. Nest, the optimization scheme is used to
compute the safe time optimal path as discussed in Section 3.6.
proximity cell
boundary line
Voronoi edge 1( )p u2( )p u
speed graph
edge
1p
2p
iO
1lproximity cell
boundary line2l
Figure 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).
Computation of the speed graph edges: The following discussion appears in Section
3.5 and is summarized here for ease of reference. Consider two speed graph nodes, p1 and p2,
99
located on the boundary of a common Voronoi cell of a polygonal obstacle Oi (Figure C.1).
Each speed graph edge is the safe time optimal path connecting p1 and p2 in F . Suppose
m +1 proximity cells separate between p1 and p2 (three such cells separate p1 and p2 in
Figure C.1). The boundaries between adjacent proximity cells form line segments denoted lj
for j = 1, . . . ,m. The computation will focus on the piecewise time optimal paths connecting
p1 and p2, consisting of time optimal path segments within each proximity cell and corner
points that can freely vary along the proximity cells’ boundary lines. The corner points
are parameterized by p(uj) : [0, 1] → lj for j = 1, . . . ,m. As discussed in Section 3.5, the
safe travel time functional T (α) becomes a function of u1, . . . , um. To compute the time
optimal path between p1 and p2 (and hence the speed graph edge), we will use the following
monotonic property of the travel time functional T (α) .
Lemma 3.5.1. Let γ(u1, . . . , um) parameterize the piecewise time optimal path segment con-
necting the nodes p1 and p2 in F while crossing the proximity cell boundary lines l1, . . . , lm.
Let u∗ = (u∗1, . . . , u∗m) be the parameter values of the optimal crossing points. The functional
T (γ(u1, . . . , um)) is monotonically increasing along any ray emanating from u∗ in the set
(u1, . . . , um) ∈ [0, 1]× · · · × [0, 1].
The proof of Lemma 3.5.1 is similar to the proof of the Prop. 3.6.1 with minor changes
in notations. The path γ is replaced with α and the crossing points p(ui) for i = 1...m or n
move from the proximity cell boundary lines denoted lj onto the Voronoi arcs denoted ej .
Computation of the safe time optimal path: The speed graph method identifies a can-
didate path homotopy class connecting pS and pT in F , then determines which Voronoi arcs
must be crossed by the safe time optimal path in the chosen path homotopy class. Denote
by e1, . . . , en the latter Voronoi arcs (two such arcs, e1 and e2, appear in Figure C.2(a)). The
computation of the safe time optimal path will focus on the piecewise time optimal paths that
cross the Voronoi arcs e1, . . . , en at points that can freely vary along these arcs. The Voronoi
crossing points are parameterized by p(ui) : [0, 1] → ej for i = 1, . . . , n (Figure C.2(a)).
As discussed in Section 3.6, the safe travel time functional T (α) becomes a function of the
parameters u1, . . . , un. The computation of the safe time optimal path will use the following
monotonicity property of T (α).
Proposition 3.6.1. Let α(u1, . . . , un) parameterize the piecewise time optimal paths con-
100
1( )p u
*
1( )p u
2( )p u
Sp
Tp
Voronoi diagram
*t
Sp
Tp
1e
2e*
2( )p u
1e
2e
tangent to1e
tangent to2e
1( )sv
(a) (b)
( )t s
iOiO
bisector
bisector
1- ( )sv
2( )sv
2- ( )sv
Figure C.2: (a) The safe time optimal path α∗ and a variant path αt. (b) The sector spannedby −v(s−i ) and v(s+
i ) locally contains the Voronoi arc tangent line at p(ui) for i = 1, 2.
necting pS and pT in F while crossing the Voronoi arcs e1, . . . , en. Let u∗ = (u∗1, . . . , u∗n) be
the parameter values of the optimal Voronoi crossing points. The functional T (α(u1, . . . , un))
is monotonically increasing along any ray emanating from u∗ in the set (u1, . . . , un) ∈
[0, 1]× · · · × [0, 1].
Proof: The piecewise time optimal paths connecting pS and pT in F will be parameterized
by a fixed division of the unit interval into sub-intervals [0, s1], [s1, s2], . . . , [sn, 1], such that
the Voronoi crossing points of these paths will correspond to s = s1, . . . , sn. Let α∗ denote the
safe time optimal path connecting pS and pT in F while crossing the Voronoi arcs e1, . . . , en.
A Voronoi path variation of α∗ is a differentiable mapping H(s, t) : [0, 1]× [0, 1] → F , such
that H(s, 0) = α∗(s) for s ∈ [0, 1], with fixed endpoints H(0, t) = pS and H(1, t) = pT for
t ∈ [0, 1]. Each path in this variation, denoted αt, corresponds to a particular value of t
(Figure C.2(a)). As t increases from t = 0 in the unit interval, the Voronoi crossing points
of αt vary along the Voronoi arcs e1, . . . , en. When H(s, t) represents a ray emanating from
u∗, the Voronoi crossing points of αt move away from those of α∗ along the Voronoi arcs as
t increases in the unit interval.
When the travel time functional T (α) is evaluated along the paths of H(s, t) it becomes
a function of t, T (t) =∫ 1
0 F (αt(s), α′t(s)) ds. All paths of H(s, t) cross the Voronoi arcs at
s = s1, ..., sn, and T (t) can be equivalently written as the sum:
T (t) =
n∑i=0
∫ si+1
si
F(αt(s), α
′t(s))ds,
101
where s0 = 0 corresponds to pS and sn+1 = 1 corresponds to pT . Using Leibnitz rule, the
derivative of T (t) with respect to t is given by
d
dtT (t) =
n∑i=0
∫ si+1
si
(∂F (α,α′)
∂α − dds∂F (α,α′)
∂α′)· ∂H(s,t)
∂t ds+n∑i=1
(∂F (α,α′)
∂α′
∣∣∣s−i
− ∂F (α,α′)∂α′
∣∣∣s+i
)· ∂H(si,t)
∂t
(C.1)
where we used the fact that H(s, t) is a fixed endpoints variation. Each of the variant paths,
αt(s), consists of time optimal path segments satisfying the Euler-Lagrange equation (3.5).
Hence all the integrals in (C.1) vanish, and the derivative of T (t) with respect to t becomes:
d
dtT (t) =
n∑i=1
(∂F (α, α′)
∂α′
∣∣∣s−i
− ∂F (α, α′)
∂α′
∣∣∣s+i
)· ∂H(si, t)
∂t. (C.2)
According to (3.15), F (α, α′) = ‖α′(s)‖/ν(s), where ν(s) = (2amin·mini=0...k{dst(α(s), Oi)})1/2
is the robot’s maximal speed along α(s). The expression for ∂F (α, α′)/∂α′ is thus given by
∂
∂α′F (α, α′) =
1
ν(s)
1
‖α′(s)‖α′(s) =
1
ν(s)v(s),
where v = α′/‖α′‖ is the normalized tangent of α. Substituting for ∂F (α, α′)/∂α′ in (C.2)
and using the subscript t to denote terms associated with αt gives:
d
dtT (t) =
n∑i=1
1
νt(si)
(vt(s
−i )− vt(s+
i ))· ∂∂tH(si, t). (C.3)
The vector vt(s−i )−vt(s+
i ) is the bisector of the sector spanned by −vt(s−i ) and vt(s+i )
at αt(si) (Figure C.2(b)). The vector ∂H(si, t)/∂t is tangent to the Voronoi arc ei at αt(si),
since the Voronoi crossing point αt(si) varies along ei as a function of t. The vectors −vt(s−i )
and vt(s+i ) are tangent to the segments of αt that reach the crossing point αt(si) from opposite
sides of ei (Figure C.2(b)). Hence the tangent line to ei lies within the sector spanned by
−vt(s−i ) and vt(s+i ) at αt(si). It follows that the i’th summand in (C.3) is the product of
two vectors lying in a common sector. Since one of the two vectors is the bisector, the i’th
summand in (C.3) can vanish only when vt(s−i )− vt(s+
i ) = ~0. In this case the time optimal
path segments meeting at αt(si) form a single C(1) curve connecting the neighboring Voronoi
102
crossing points αt(si−1) and αt(si+1).
The Voronoi crossing points are located at (u∗1, . . . , u∗n) at t = 0, then move away from
their optimal locations along e1, . . . , en as t increases in the unit interval. Hence T (t) is
increasing at t = 0. Suppose T (t) ceases to increase at t = t1. In this case there must exists
0 < t0 < t1 at which one of the summands in (C.3) becomes zero and then negative for t > t0.
This particular summand of index i0 satisfies the condition vt0(s−i0) = vt0(s+i0
) at the Voronoi
crossing point αt0(si0). The time optimal path segments meeting at αt0(si0) form a single
extremal path of T (α) connecting the neighboring Voronoi crossing points. By assumption
T (α) attains a local maximum along a local variation of this path which varies only the ei0
crossing point while keeping the neighboring Voronoi crossing points fixed. However, any
extremal path of T (α) must be a local minimum according to Lemma 3.4.4. This leads to a
contradiction, and T (t) therefore remain monotonically increasing for all 0 ≤ t ≤ 1. �
103
Appendix D
The Turning Radius Constraint
Along an Optimal Path
As discussed in Appendix A, when the robot navigates along a high speed path, it may
be subjected to high centrifugal forces that may bring the robot to lose its grip and slide
sideways. Therefore it is necessary to ensure that the radius of curvature of the time optimal
path segments that where derived in Section 3.3 do not violate the turning radius constraint
given by:
r(ν) ≥ rslide(ν) =ν2
µg. (D.1)
Note that the development of (D.1) appeared in Appendix A and hence is omitted here.
A single wall segment obstacle: Consider a time optimal path segment located near a
single wall segment. This optimal path segment was developed in Section 3.3 and is given
by the Cycloid curve (3.9). An expression for the radius of curvature, written in cartesian
coordinates is given by:
ρc =
∣∣∣∣∣∣∣(
1 + (dy/dx)2)3/2
d2y/dx2
∣∣∣∣∣∣∣ . (D.2)
where y(x) is the y coordinate of the optimal path segment α(x) = (x, y(x)).
104
Further substitution of dy/dx and d2y/dx2 in (D.2) according to the optimal path’s
formula, then comparing with the safe turning radius, would suggest that for every y(x) ≤
1/(2aminc2) the turning radius constraint is kept and the robot can safely follow this optimal
path segment without the risk of sliding. The constant amin = µg is the robot’s maximal
deceleration and c is a parameter determined by the path’s end conditions.
Next, observe the expression for dy/dx for this path segment:
dy(x)
dx=
√1
2aminc2y(x)− 1. (D.3)
Equation D.3 implies that this optimal path segment is only valid when y(x) ≤ 1/(2aminc2)
and hence we can conclude that ρc ≥ rslide. Next, consider the optimal path segment located
near a single point obstacle.
A single point obstacle: Consider a time optimal path segment located near a single point
obstacle. This optimal path segment was developed in Section 3.3 and is given by a parabolic
curve formulated in polar coordinates (3.12). However, to show that this path segment
maintains the safe turning radius constraint we shale formulate it in cartesian coordinates
as follows:
y(x) =x2 + y2
0
2y0for x ∈ [xS , xT ],
where y0 is the minimal distance between the obstacle and the imaginary wall segment that
creates the parabolic optimal curve. Note that the x axis merges with that imaginary wall.
The safe speed along this optimal path segment is given by,
ν(x) =
õg(x2 + y2
0)
y0,
and therefore the turning radius that will evoke sliding while navigating with this speed is:
rslide(ν(x)) =ν2(x)
µg=x2 + y2
0
y0.
Requiring that the path’s curvature ρc, derived according to (D.2), would be equal or
105
larger than rslide for every x gives:
ρc = y0 ·(
1 +x2
y20
)3/2
> y0 ·(
1 +x2
y20
)= rslide,
which is always true and hence this time optimal path segment satisfies the turning radius
constraint thus preventing sliding events.
106
Bibliography
[1] The iRobot Create owners guide. www.irobot.com.
[2] GUARDIUM-UGV. G-NIUS Unmanned Ground Systems, accesssed Jan. 2012.
http://g-nius.co.il.
[3] GUSS, a ground unmanned support surrogate. The Naval Surface Warfare Center and
TORC Robotics, accesssed Jan. 2012. http://www.torcrobotics.com/ground-unmanned-
support-surrogate.
[4] Traffic jam assistance. Volvo Car Group,
accesssed May. 2013. https://www.media.volvocars.com/global/enhanced/en-
gb/media/preview.aspx?mediaid=46386.
[5] A.P. Aguiar and J.P. Hespanha. Trajectory-tracking and path-following of underactu-
ated autonomous vehicles with parametric modeling uncertainty. IEEE Transactions
on Automatic Control, 52:1362–1379, 2007.
[6] K.J. Astrom and R.M. Murray. Feedback systems: an introduction for scientists and
engineers. Princeton university press, 2010.
[7] F. Aurenhammer and R. Klein. Voronoi diagrams. In Handbook of Computational
Geometry, chapter 5, pages 201–290. Elsevier Science, 2000.
[8] F. Aurenhammer, R. Klein, and D.T. Lee. Voronoi Diagrams and Delaunay Triangula-
tions. World Scientific, 2013.
[9] J.E. Bobrow, S. Dubowsky, and J. Gibson. Time-optimal control of robotic manipulators
along specified paths. The International Journal of Robotics Research, 4:3–17, 1985.
107
[10] J. Borenstein and Y. Koren. The vector field histogram - fast obstacle avoidance for
mobile robots. IEEE Transactions on Robotics and Automation, 7:278–288, 1991.
[11] C.B. Boyer and U.C. Merzbach. A History of Mathematics. New York: Wiley, 1991.
[12] F. Braghin, F. Cheli, S. Melzi, and E. Sabbioni. Race driver model. Computers &
Structures, 86:1503–1516, 2008.
[13] O. Brock and O. Khatib. High-speed navigation using the global dynamic window
approach. In IEEE International Conference on Robotics and Automation, 341–346,
1999.
[14] A.E. Bryson. Applied optimal control: optimization, estimation and control. CRC Press,
1975.
[15] F. Bullo and R.M. Murray. Tracking for fully actuated mechanical systems: A geometric
framework. Automatica, 35:17–34, 1999.
[16] H. Chitsaz and S.M. LaValle. Time-optimal paths for a dubins airplane. In 46th IEEE
Conference on Decision and Control, 2379–2384, 2007.
[17] H. Choset. Coverage for robotics a survey of recent results. Annals of Mathematics
and Artificial Intelligence, 31:113–126, 2001.
[18] H. Choset, K.M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.E. Kavraki, and
S. Thrun. Principles of Robot Motion. MIT Press, Cambridge MA, 2005.
[19] T.H. Chung, G.A. Hollinger, and V. Isler. Search and pursuit-evasion in mobile robotics,
a survey. Autonomous Robots, 31:299–316, 2011.
[20] F.H. Clarke. Optimization and Nonsmooth Analysis. SIAM Publication, 1990.
[21] B. Donald, P. Xavier, J. Canny, and J. Reif. Kinodynamic motion planning. Journal of
The ACM, 40:1048–1066, 1993.
[22] L.E. Dubins. On curves of minimal length with a constraint on average curvature,
and with prescribed initial and terminal positions and tangents. American Journal of
Mathematics, 497–516, 1957.
108
[23] D. Eppstein. Finding the k shortest paths. SIAM Journal on Computing, 28:652–673,
1998.
[24] P. Fiorini and Z. Shiller. Motion planning in dynamic environments using velocity
obstacles. The International Journal of Robotics Research, 17:760–772, 1998.
[25] S. Fleury, P. Soueres, J.P. Laumond, and R. Chatila. Primitives for smoothing mobile
robot trajectories. IEEE Transactions on Robotics and Automation, 11:441–448, 1995.
[26] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach to collision avoid-
ance. IEEE Robotics Automation Magazine, 4:23–33, 1997.
[27] T. Fraichard. A short paper about motion safety. In IEEE International Conference on
Robotics and Automation, 1140–1145, 2007.
[28] S.S. Ge and Y.J. Cui. Dynamic motion planning for mobile robots using potential field
method. Autonomous Robots, 13:207–222, 2002.
[29] I. M. Gelfand and S.V. Formin. Calculus of Variations. Prentice-Hall, 1963.
[30] M. Goresky and R. MacPherson. Stratified Morse Theory. Springer Verlag, New York,
1980,.
[31] B. Herisse and R. Pepy. Shortest paths for the dubins’ vehicle in heterogeneous en-
vironments. In 52nd IEEE Annual Conference on Decision and Control, 4504–4509,
2013.
[32] D. Hsu, R. Kindel, J.C. Latombe, and S. Rock. Randomized kinodynamic motion plan-
ning with moving obstacles. The International Journal of Robotics Research, 21:233–255,
2002.
[33] K. Iagnemma, S. Shimoda, and Z. Shiller. Near-optimal navigation of high speed mobile
robots on uneven terrain. In IEEE/RSJ Int. Conference on Intelligent Robots and
Systems (IROS), 4098–4103, 2008.
[34] E. Isaacson and H.B. Bishop. Analysis of numerical methods. Dover, 1994.
[35] K. Kant and S.W. Zucker. Toward efficient trajectory planning: The path-velocity
decomposition. The International Journal of Robotics Research, 5:72–89, 1986.
109
[36] S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning.
International Journal of Robotics Research, 30:846–894, 2011.
[37] R. Kindel, D. Hsu, J.C. Latombe, and S. Rock. Kinodynamic motion planning amidst
moving obstacles. In IEEE International Conference on Robotics and Automation, 537–
543, 2000.
[38] Y. Koren and J. Borenstein. Potential field methods and their inherent limitations for
mobile robot navigation. In IEEE International Conference on Robotics and Automa-
tion, 2:1398–1404, 1991.
[39] F. Large, D. Vasquez, T. Fraichard, and C. Laugier. Avoiding cars and pedestrians
using v-obstacles and motion prediction. In The IEEE Intelligent Vehicle Symp., 2004.
[40] J.C. Latombe. Robot Motion Planning. Kluwer Academic Publishers, 1991.
[41] S.M. Lavalle. Rapidly-exploring random trees: A new tool for path planning. Technical
report, Dept. of Computer Science, Iowa State University, 1998.
[42] S.M. LaValle and J.J. Kuffner. Randomized kinodynamic planning. In IEEE Interna-
tional Conference on Robotics and Automation, 1:473–479, 1999.
[43] S.M. LaValle and J.J. Kuffner. Randomized kinodynamic planning. The International
Journal of Robotics Research, 20:378–400, 2001.
[44] S.M. LaValle and J.J. Kuffner. Rapidly-exploring random trees: progress and prospects.
In Algorithmic and Computational Robotics: New Directions, 293–308. Wellesley, MA,
2001.
[45] M. Lepetic, G. Klancar, I. Skrjanc, D. Matko, and B. Potocnik. Time optimal path
planning considering acceleration limits. Robotics and Autonomous Systems, 45:199–
210, 2003.
[46] T. Lozano-Perez. Spatial planning: A configuration space approach. IEEE Transactions
on Computers, C-32:108–120, 1983.
[47] G. Manor and E. Rimon. High-speed navigation of a uniformly braking mobile robot us-
ing position-velocity configuration space. In IEEE International Conference on Robotics
and Automation, 193–199, 2012.
110
[48] G. Manor and E. Rimon. Vc-method: high-speed navigation of a uniformly braking
mobile robot using position-velocity configuration space. Autonomous Robots, 34:295–
309, 2013.
[49] G. Manor and E. Rimon. The speed graph method: Time optimal navigation among ob-
stacles subject to safe braking constraint. In IEEE International Conference on Robotics
and Automation, 1155–1160, 2014.
[50] J. Markoff. Google cars drive themselves, in traffic. The New York Times, October 9th
2010.
[51] Y. E. Nesterov and A. S. Nemirovsky. Interior Point Polynomial Methods in Convex
Programming: Theory and Applications. Springer Verlag, New York, 1992.
[52] V. Nieuwstadt, J. Michael, and R.M. Murray. Real time trajectory generation for dif-
ferentially flat systems. California Institute of Technology, 1997.
[53] L. Nirenberg. Variational and topological methods in nonlinear problems. Bulletin of
the AMS, 4:267–302, 1981.
[54] C. O’Dunlaing and C. K. Yap. A retraction method for planning the motion of a disc.
Journal of Algorithms, 6:104–111, 1985.
[55] M.G. Park, J.H. Jeon, and M.C. Lee. Obstacle avoidance for mobile robots using arti-
ficial potential field approach with simulated annealing. In IEEE International Sympo-
sium on Industrial Electronics, 3:1530–1535, 2001.
[56] R. Parthasarathi and T. Fraichard. An inevitable collision state-checker for a car-like
vehicle. In IEEE Intenrational Conference on Robotics and Automation, 3068–3073,
2007.
[57] S. Petti, T. Fraichard, and I. Rocquencourt. Safe motion planning in dynamic envi-
ronments. In IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), 3726–3731, 2005.
[58] J. Reeds and L. Shepp. Optimal paths for a car that goes both forwards and backwards.
Pacific Journal of Mathematics, 145:367–393, 1990.
111
[59] E. Rimon. Exact Robot Navigation Using Artificial Potential Functions. PhD thesis,
Yale University, New Haven CT, Dec 1990.
[60] E. Rimon and D.E. Koditschek. Exact robot navigation using artificial potential func-
tions. IEEE Transactions on Robotics and Automation, 8:501–518, 1992.
[61] J.R. Sack and J. Urrutia, editors. Handbook of Computational Geometry. Elsevier, 1999.
Chapter 5.
[62] M. I. Shamos. Computational Geometry. PhD thesis, Yale University, 1978. Section
6.3.
[63] I. Sherr and M. Ramsey. Toyota and audi move closer to driverless cars, The Wall Street
Journal, Jan. 3 2013.
[64] Z. Shiller and Y. R. Gwo. Dynamic motion planning of autonomous vehicles. IEEE
Transactions on Robotics and Automation, 7:241–249, 1991.
[65] Z. Shiller and H.H. Lu. Computation of path constrained time optimal motions with
dynamic singularities. Journal of dynamic systems, measurement, and control, 114:34–
40, 1992.
[66] Z. Shiller, S. Sharma, I. Stern, and A. Stern. Online obstacle avoidance at high speeds.
The International Journal of Robotics Research, 32:1030–1047, 2013.
[67] S. Shimoda, Y. Kuroda, and K. Iagnemma. Potential field navigation of high speed
unmanned ground vehicles on uneven terrain. In IEEE International Conference on
Robotics and Automation, 2828–2833, 2005.
[68] R. H. Smith. Analyzing Friction in the Design of Rubber Products and Their Paired
Surfaces. CRC Press, 2008.
[69] J. Snape, J. van den Berg, and S. J. Guy. The hybrid reciprocal velocity obstacle. IEEE
Transactions on Robotics and Automation, 27:696–706, 2011.
[70] K.C. Teixeira, M. Becker, and G.A.P. Caurin. Automatic routing of forklift robots in
warehouse applications. In ABCM Symposium in Mechatronics, 4:335–344, 2010.
112
[71] I. Ulrich and J. Borenstein. Vfh+: Reliable obstacle avoidance for fast mobile robots.
In International Conference on Robotics and Automation, 2:1572–1577, 1998.
[72] E. Velenis and P. Tsiotras. Optimal velocity profile generation for given acceleration
limits: theoretical analysis. In The American Control Conference, 2:1478–1483, 2005.
[73] J. Villagra, V. Milanes, J. Perez, and J. Godoy. Smooth path and speed planning for
an automated public transport vehicle. Robotics and Autonomous Systems, 2012.
[74] W. Weigo, C. Huitang, and W. Peng-Yung. Time optimal path planning for a wheeled
mobile robot. Journal of Robotic Systems, 17:585–591, 2000.
[75] R. Wein, J. van den Berg, and D. Halperin. Planning high-quality paths and corridors
amidst obstacles. International Journal of Robotics Research, 27:1213–1231, 2008.
[76] E. Welzl. Constructing the visibility graph for n-line segments in o(n2) time. Information
Processing Letters, 20:167–171, 1985.
[77] D.K. Wilde. Computing clothoid segments for trajectory generation. In IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS), 2440–2445, 2009.
[78] D. Wilkie, J.V. den Berg, and D. Manocha. Generalized velocity obstacles. In IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS), 2009.
[79] P.R. Wurman, R. D’Andrea, and M. Mountz. Coordinating hundreds of cooperative,
autonomous vehicles in warehouses. AI Magazine, 29:9–19, 2008.
[80] J.M. Yang and J.H. Kim. Sliding mode control for trajectory tracking of nonholonomic
wheeled mobile robots. IEEE Transactions on Robotics and Automation, 1999.
[81] J. Youssef. The Mountain Pass Theorem, Variants, Generalizations and Some Applica-
tions. Cambridge University Press, 2003.
[82] M. Athans and P.L. Falb. Optimal Control. An Introduction to the Theory and Its
Applications. Dover Publications, 2007.
[83] G. Leitmann. The calculus of variations and optimal control. Springer, 1981.
[84] J.Z. Ben-Asher. Optimal control theory with aerospace applications. AIAA Educational
Series, 2010.
113
יתרה מכך תהליך בניית המסלולים איננו נומרי . מסלול אופטימלי אף קבלתולכן מאפשרים
. ומבטיח באופן ריגורוזי את בטיחות הרובוט לעצמו ולסביבה בה הוא נע
ת תלויי המהירות ממודלים צי הבטיחואילו, בגישה הראשונה. שות מוצגותשתי גי
המרחב החופשי מחולק , לאחר מכן. הרובוטשל במרחב קונפיגורציה תלוי מהירות ומיקום
חיפוש על גרף זה מניב מסלולים . נבנהלתאים חופשיים וגרף קישוריות בין תאים אלו
את יש לציין שמסלולים אלו מקיימים מראש . אופטימליים מקורבים באופן חישובי יעיל מיותר
ות לצורך חישוב יבחשבון ואריצראשית יםמשתמש ,גישה השניהב. אילוצי הבטיחות
בהמשך נבנה גרף חיפוש המבוסס על . המסלולים האופטימליים בסמוך למכשולים בדידים
, כזה המקיים את אילוצי הבטיחות, לאחר מציאת מסלול איכותי על הגרף. מסלולים אלו
פונקציונאל זמן התנועה במסלולים אלו והמסלול הגלובלי השיטה מנצלת תכונות קמירות של
שהינו האליפסואיד עבור פונקציות קמורות םבאלגורית חישוב זה נעזר. האופטימלי מחושב
מקיים , מהתנגשויות בל הינו מסלול החופשיהמסלול המתק .יעיל ביותר בחישובים מסוג זה
ההסברים מלווים , ור שתי הגישותעב .וממזער את זמן התנועהמראש את אילוצי הבטיחות
. כנות למימוש מיידיובדוגמאות רבות ומתוארים כשגרות המ
III
לוצי אי. מהירות כזו שתמנע את התהפכותו של הרובוט עקב מיקום מרכז מסה גבוה מידי
ם החיכוך בין צמיגי בגיאומטריית הרובוט ובמקד, בטיחות אלו תלויים במהירות הרובוט
הרובוט לקרקע ולכן דורשים התייחסות למרחב המצב המלא של הרובוט הכולל מיקום
.ומהירות
התיחסות לאילוצים הדינמיים תוך במהירות גבוההשיטות לפתרון בעייה הניווט
בקבוצה הראשונה . נחלקות לשלוש קבוצות עיקריות ,הבטיחותהמוכתבים כורח דרישות
הראשון מוגדר מסלול תנועה גיאומטרי לרובוט בשלב. שלביםלשני נחלקת בניית המסלול
השני שלבב. כגון אורך המסלול הכולל אשר חופשי מהתנגשויות ומקיים אמת מידה מסויימת
כך שאילוצי הבטיחות לא , לול הנתוןמהירות הרובוט לאורך המס מוכתבת, של תכנון התנועה
, ןוכ, בעלות חישובית נמוכה מאפשרת אומנם קבלת מסלול מהיר, שיטת תכנון זו .יופרו
אך איננה מאפשרת קבלת מסלולים , זה שלא מפר את אילוצי הבטיחות הדינאמייםמסלול כ
. יחדיוסינתזה של התכנון הגיאומטרי וקביעת המהירות יםדורש אלואופטימליים היות ו
. הקבוצה השנייה כוללת שיטות נומריות לתכנון מסלול עבור סביבות מכשולים ידועות
בשיטות אלו מרחב חיפוש המסלולים עובר דסקרזיציה באופן כזה שמאפשר חיפוש מסלול
שיטות אלו מתכנסות לרוב למסלול . מקיים את אילוצי הבטיחותההחופשי מהתנגשויות וכזה
חישוב איננו מוגבל אך אינן מנצלות תכונות מתמטיות של מסלולים האופטימלי כאשר זמן ה
הקבוצה השלישית שנפוצה ויעילה בעיקר . אלו לטובת שיפור הדיוק והפתחת זמן החישוב
צעד זמן מחשבת את מסלול התנועה ופרופיל המהירות עבור , בחישוב מסלולים בזמן אמת
בכל צעד זמן . ומית של הרובוטבו מתייחסים לתכנון המסלול רק בסביבה מק, מוגדר
באופן כזה שאמת מידה מסויימת לאורכו המסלול ופרופיל המהירות מחושבים, דסקרטי
שיטות אלו אינן מאפשרות התייחסות לכל . ממוקסמת תוך כדי שמירה על אילוצי הביטחות
.המכשולים בו בעת באופן יעיל
אנו מסתנזים את אילוצי הבטיחות הדינאמיים ואת תכנון המסלול במחקר זה
הגיאומטרי לשלד אחד הלוקח בחשבון את כל המרכיבים המשפיעים את מסלול ומהירות
ש כאלו המקיימים את אילוצי הבטיחות מרא, שילוב זה מאפשר לקבל מסלולי תנועה. הרובוט
II
תקציר
לאחרונה אנו עדים . הינו היבט חשוב מאוד בתחום הרובוטיקה םייאוטונומרובוטים ניווט
למגמה בה מערכות רובוטיות אוטונומיות יוצאות מהמעבדות הסטריליות ועוברות לנוע
חיוני להבטיח את בטיחות המערכת , בנקודה זו. ממשיותבסביבות חיצוניות ו ולפעול
אחת השיטות . נעהית רובוטהרובוטית יחד עם בטיחות הסביבה בתוכה אותה מערכת
מרחב זה כולל את כל . הינה למדל מרחב קונפיגורציות לרובוט, המקובלות לפתרון בעייה זו
מיקומי ראשית הצירים והאורינטציות בהן המצאותו של הרובוט איננה גורמת לפגיעה
לאחר בניית מרחב זה מתבצעת בנייה של מבנה נתונים כגון עץ חיפוש . במכשולים הסמוכים
לבסוף מתבצע חיפש עבור מסלול החופשי מהתנגשויות בין הרובוט . גרף תלת מימדי או
שיטה זו ישימה גם במצב בו סביבת המכשולים ידועה מלכתחילה וגם . והמכשולים האחרים
למרות כל . בניווט בזמן אמת בו המידע על המכשולים נרכש תוך כדי תנועה על ידי חיישנים
מובהק לבעיית הניווט בין מכשולים כאשר דינאמיקת הרובוט עד כה לא הוצע פתרון , זאת
דרישה לקבלת מסלול איכותי כדוגמאת מזעור זמן התנועה או , יתרה מכך. נלקחת בחשבון
מגדירה בעיית ניווט חדשה , יחד עם קיום האילוצים הדינאמיים, מיקסום המרווח מהמכשולים
.ומורכבת
כמובן תוך התייחסות , אמת מידה אחת למסלולים החופשיים מהתנגשויות
המסלול , או במילים אחרות, היא זמן התנועה הכולל של הרובוט, לדינאמיקת הרובוט
מהירות תנועה , לכזו אמת מידה. האופטימלי מבחינת זמן בין שתי נקודות קצה נתונות
תנועת רובוט . הכרחית וזאת כדי להשלים את המשימה כמה שיותר מהרגבוהה של הרובוט
עקריים אשר יש לקחת דינאמיים במהירות גבוה מאלצת התייחסות לשני אילוצי בטיחות
כזה , האילוץ הראשון הינו שמירה את מרחק בלימה. בחשבון בתהליך תכנון המסלול
האילוץ השני . י לפגוע במכשוליםשיאפשר לרובוט להגיב לאירועי פתע ולהספיק לעצור מבל
עליו לנוע , כאשר הרובוט נע על גבי קשתות שאינן ישרות. נוגע לתנועת הרובוט בעת פנייה
או לחילופין , במהירות כזו שתמנע החלקה לצדדים עקב חיכוך בלתי מספיק עם הקרקע
I
המחקר נעשה בהנחיית פרופסור אילון רימון
מהפקולטה להנדסת מכונות
ואן 'ש הלוי וכן לארווין וג"אני מודה לקרן עייקובס על התמיכה הכספית הנדיבה במהלך 'ג
התשלמותי
ניווט רכבים אוטונומיים עם אילוצי מהירות
חיבור על מחקר
לשם מילוי חלקי של הדרישות לקבלת התואר
דוקטור לפילוסופיה
גיל מנור
מכון טכנולוגי לישראל –הוגש לסנט הטכניון
2015 ינואר ,ה"תשע טבת, חיפה
ניווט רכבים אוטונומיים עם אילוצי מהירות
גיל מנור