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 ינואר ,ה"תשע טבת, חיפה

ניווט רכבים אוטונומיים עם אילוצי מהירות

גיל מנור