BIB-VERSION:: CS-TR-v2.0
ID:: CORNELLCS//TR92-1279
ENTRY:: 1993-10-14
ORGANIZATION:: Cornell University, Computer Science Department
LANGUAGE:: English
TITLE:: Provably-Good Approximation Algorithms for Optimal Kinodynamic 
        Robot Motion Plans
AUTHOR:: Xavier, Patrick G.
DATE:: April 1992
PAGES:: 224
COPYRIGHT:: Patrick Gordon Xavier 1992 - All Rights Reserved
ABSTRACT::
The kinodynamic planning problem is to synthesize a robot motion obeying 
simultaneous kinematic and dynamics constraints. To maximize robot 
performance we can consider optimal kinodynamic planning: for a given robot 
system, find a minimal-time trajectory that goes from a start state to a goal 
state, avoids obstacles by a speed-dependent safety margin, and respects the 
dynamics laws and dynamics bounds governing the system. In general, previous 
work on algorithmic motion planning does not address dynamics; furthermore, 
even in simple cases, finding exact globally-optimal solutions is 
$\cal NP$-hard. In response, we obtain provably-good, polynomial-time 
approximation algorithms that synthesize optimal kinodynamic trajectories. 
These algorithms forge new mathematical links between control theory and 
complexity theory, and our analysis investigates how discrete-control 
trajectories can approximate optimal solutions

We cast optimal kinodynamic planning into the form of an 
$\epsilon$-approximation problem in which $\epsilon > 0$ characterizes 
closeness to optimality in terms of trajectory time, observance of the safety 
margin and closeness to the start and goal states

If $T_{opt}$ is the time of an optimal trajectory, then an $\epsilon$-optimal 
trajectory takes at most (1 + $\epsilon$)$T_{opt}$ time. We present 
(pseudo)-polynomial-time $\epsilon$-approximation algorithms for a family of 
robot classes, including fully-controllable open kinematic chains. These 
algorithms run in time polynomial in $\frac{1}{\epsilon}$ and the geometric 
complexity of the constraints.

The basic idea behind the algorithms is to reduce the trajectory planning 
problem to a shortest-path problem on a polynomial-sized reachability graph 
embedded in the robot state space. These graphs are generated by control 
primitives and a timestep that the algorithm chooses to ensure $\epsilon$-
optimality. To obtain our complexity and approximation results, we introduce 
both continuous and combinatorial tools to analyze the robot's dynamical 
system. These include scaling-tracking proof methods that capture the key 
insight necessary for provably-good results, tracking lemmas on how 
closely we can approximate an optimal or time-rescaled optimal trajectory, 
constructive trajectory proofs, adversary game proofs and Time-Safety 
planning trade-offs. We also decribe an implementation and experiments in 
a restricted domain.
END:: CORNELLCS//TR92-1279
BODY::
Provably-Good Approximation Algorithms
for Optimal Kinodynamic
Robot Motion Plans
Patrick Gordon Xavier
Ph.D Thesis
92-1279
April 1992
Department of Computer Science
Cornell University
Ithaca, NY 14853-7501
PROVABLY-GOOD APPROXIMATION ALGORITlIMS
FOR OPTIMAL NINODYNAMIC ROBOT MOTION
PLANS
A Dissertation
Presented to the Faculty of the Graduate School
of Cornell University
in Partial Fulfillment of the Requirements for the Degree of
Doctor of Philosophy
by
Patrick Gordon Xavier
May 1992
Oc Patrick Gordon Xavier 1992
ALL RIGHTS RESERVED
PROVABLY-GOOD APPROXIMATION ALGORITllMS FOR OPTIMAL
KINODYNAMIC ROBOT MOTION PLANS
Patrick Gordon Xavier, Ph.D.
Cornell University 1992
The kinodynamic planning problem is to synthesize a robot motion obeying si-
multaneous kinematic and dynamics constraints. To maximize robot performance
we can consider optimal kinodynamic planning: for a given robot system, find
a minimal-time trajectory that goes from a start state to a goal state, avoids
obstacles by a speed-dependent safety margin, and respects the dynamics laws
and dynamics bounds governing the system. In general, previous work on algo-
rithmic motion planning does not address dynamics; furthermore, even in simple
cases, finding exact globally-optimal solutions is ??-hard. In response, we obtain
provably-good, polynomial-time approximation algorithms that synthesize optimal
kinodynamic trajectories. These algorithms forge new mathematical links between
control theory and complexity theory, and our analysis investigates how discrete-
control trajectories can approximate optimal solutions.
We cast optimal kinodynamic planning into the form of an ?-approximation
problem, in which ?> 0 characterizes closeness to optimality in terms of trajectory
time, observance of the safety margin, and closeness to the start and goal states.
If Topt is the time of an optimal trajectory, then an e-optimal trajectory takes
at most (1 + e)T0?t time. We present (pseudo)-polynomial-time &approximation
algorithms for a family of robot classes, including fully-controllable open kinematic
chains. These algorithms run in time polynomial in 1E and the geometric complexity
of the constraints.
The basic idea behind the algorithms is to reduce the trajectory planning prob-
lem to a shortest-path problem on a polynomial-sized reachability graph embedded
in the robot state space. These graphs are generated by control primitives and a
timestep that the algorithm chooses to ensure e-optimality. To obtain our com-
plexity and approximation results, we introduce both continuous and combinatorial
tools to analyze the robot's dynamical system. These include scaling-tracking proof
methods that capture the key insight necessary for provably-good results, tracking
lemmas on how closely we can approximate an optimal or time-rescaled optimal
trajectory, constructive trajectory proofs, adversary game proofs, and Time-Safety
planning trade-offs. We also describe an implementation and experiments in a
restricted domain.
Biographical Sketch
Patrick Gordon Xavier was born on November 17,1962, in Euclid, Ohio. It is only
natural that his thesis research has a geometrical flavor.
On the brink of adulthood, Patrick learned the art of cooking omelettes and
completed high school in 1981 at Choate Rosemary Hall in Wallingford, Connecti-
cut.
Patrick ventured into Physics, Chinese cooking, and Computer Science at Har-
vard University, where he was awarded the degree of Bachelor of Arts, magna ctm
lande, in Applied Mathematics in June 1985. Later that year he bought a heavy
steel wok and moved to Ithaca, New York, to pursue graduate studies at Cornell
University. Faced with an electric range in 1986, he became adept at preparing
many foods on the barbeque grill by the time Cornell awarded him a Master of Sci-
ence degree in Computer Science in August 1988. In the meantime, he discovered
a love for bicycling.
Over the next few years, Patrick experimentally determined that Pop-Tarts and
Fig Newtons are good foods to eat on a century ride, and he also figured out how
to use his wok on an electric range. He eventually wrote this dissertation, for which
Cornell University is awarding him a Ph.D. in Computer Science in May 1992.
iii
To my parents, Cecilia Marie Xavier and Vincent Louis Xavier, Jr.
iv
Acknowledgements
Many thanks to my patient committee members, Bruce Donald, Allen llatcher,
Prakash Panangaden, and Alberto Segre and to proxy member Dan Huttenlocher.
Bruce has been a fine advisor and friend, and I'm proud to have been his first
Ph.D. student. Prakash deserves special gratitude for his friendship and valuable
counsel, especially in my early years at Cornell. Alex Nicolau and Jon Solworth
also have my gratitude for their guidance during my pre-robotics days. Although
Dexter Kozen was never on my committee, he was supportive of my progress as a
graduate student beyond his duties as GFR.
Outside of the Cornell Computer Science faculty, Jeff Koechling and Dinesh
Pai deserve thanks for much information and advice about robotics and research.
When I had a computational geometry question I could always turn to Robert
Freimer and Christine Piatko. Bill Aitken and I had many valuable discussions on
research and its presentation, as well as many enjoyable conversations on various
and sundry topics.
Laura Sabel and Mike Hopcroft were ideal officemates during the last crucial
months. They remained congenial, if not downright friendly, even when my thesis-
related sprawl threatened to cover every inch of spare deskspace. Also, Laura's
tape and scissors have been invaluable in the preparation of certain pages of this
v
document as well as several papers.
While I was in Ithaca, I was fortunate to have many friends who helped make
my stay enjoyable and without whom keeping life in perspective would have been
a trying task. Most notable in deserving my thanks are former housemates Bob
Mihailovich and Harry Barshatzky, and Lynne Hsu, Bill Aitken, Mike DiMilia, Deb
Hild, Steve Rapkin, and Kim Williams. I'm also grateful to the Ithaca Hash House
Harriers for 26 fun-filled hash runs and associated activities, and to the Robotlab
Lunch Society for many entertaining meals.
Finally, thanks to Mom and Dad for sending me down the right track, from
teaching me the alphabet (which I still use today) to paying for college.
vi
Table of Contents
2
Introduction
1.1			An Approach . . . . . .			.			.			. .			. .			. .			.
1.1.1			Observations . . .			.			.
1.1.2			Approximation			Algorithms .			. .			.			.			.
1.1.3			Provably Good Approximation Algorithms			for Kinodynamic
Planning . . . . . . .			.			. .			. .			. .			.
1.2			Kinodynamic Planning
1.2.1			Overview . .			.			. .
1.2.2			Path Planning . . . .			.			.			. .
1.2.3			Trajectory Planning
1.2.4			Kinodynamic Planning .			.			.			. .
1.3			Techniques
1.3.1			Path Planning			Observations			. .
1.3.2			Graphs Embedded In State Space
1.3.3			Graphs With Guarantees			. .			. .
1.4			Previous and Related Work .			.			. .
1.4.1			Applied Control Theory			.			. .
1.4.2			Algorithms With Provably			Good			Bounds .			.
1.4.3			Other Related Work . .			.			.			. .			. .			. .			. .
1.5			Contributions and Thesis Outline			.
1.5.1			Algorithms.			. .			.			.
1.5.2			Mathematical			Contributions
1.5.3			Thesis Outline. . .			.			. .			.			.
1.5.4			Pubtication Note . . .			.			.			. .			. .			. .			.
Cartesian Kinodynamic Planning
2.1 Kinodynamic Planning For A Simple Robot
2.1.1 Problem Formulation
2.1.2 Statement of Results .
2.2 Algorithm and Analysis
2.2.1 The General Idea
2.2.2			The Algorithm			. . .
vii
2
2
4
6
7
7
9
10
16
23
23
24
28
29
30
32
34
35
35
36
38
41
42
43
43
46
47
47
49
Time-Rescaling and Safe Tracking
The Simple Tracking Lemma.
Basic Safety Checking
Proving the Main Theorem
3
4
2.2.3
2.2.4
2.2.5
2.2.6
2.3			Summary. .
Planning
Better Bounds for Cartesian Kinodynamic
3.1			Improved Techniques . .			. .
3.1.1			Introduction
3.1.2			Improved Results			. . .			.			. . .
3.2 Kinodynamic Planning with L? Dynamics Bounds
3.2.1 An Algorithm for Basic Kinodynamic Planning, Revisited
3.2.2 Improving Algorithm 1
3.3 Better Bounds for a Cartesian Robot with L? Dynamics Bounds
3.3.1			Overview
3.3.2			The Efficient Tracking Lemma			.			.			. .			. .
3.3.3			The Strong Tracking Lemma.
3.3.4			?-Safe ("Also Safe") Grid-Bang Trajectories and Deriving
Final Bounds . .
3.4			An Implementation for the L?			Cartesian			Robot			. .			. .
3.4.1			Brief Description . .			.			.
3.4.2			Simple Examples . .			.			.			. .			. .
3.4.3			Discussion
3.5			Summary . . . . . .			. .			.			. .			. .
Kinodynamic Planning With Coupled Dynamics Bounds
4.1 Kinodynamic Motion Planning for Robots With Coupled Dynamics
Bounds
4.1.1
4.1.2
4.1.3
4.1.4
4.2
4.3
A More General Kinodynamic Planning Problem
Safe and Near-Optimal Kinodynamic Plans			. .			. .
Statement of Results
Generalizing the Basic Algorithms to Robots with Coupled
Dynamics Bounds
Robots with Coupled Dynamics Bounds
4.2.1			The Algorithms . . . . . . . .			. .			. .
4.2.2			Key Concepts
Deriving Bounds. . . . . .			. .			. .
4.3.1			Proving the Key Lemmas
4.3.2			Search-Space Bounds for Cartesian Manipulators			. .
4.3.3			Applying the Coupled Tracking Lemma and Robots with
State-Dependent Dynamics			. . .			. .
4.3.4			Safety-Checking and Final Bounds
Summary . . . . . . . . . . . . . .			. . .			. .
52
54
59
61
62
64
64
64
65
67
67
68
72
72
74
89
94
98
98
99
99
104
106
107
107
109
109
112
113
113
121
132
132
142
147
151
155
4.4
viii
5 A "Bang-Bang" Planner and Time-Safety Trade-Offs
5.1			?-Optimal Bang-Bang Trajectories			.
5.1.1			"Minimalist" Control
5.1.2			Bang-Bang Results .			. .			. .
5.1.3			Minimalist Cartesian kinodynamic Planning
5.1.4			A More General Bang-Bang Algorithm
5.2			Time-Safety Trade-Offs
5.2.1			A Finer Approximation Model . . .
5.2.2			Algorithms That Allow Compromise
5.2.3			Bounds For Trade-off Algoritlims . . .
5.2.4			Complexity Surfaces and Equicomplexity Curves
5.2.5			Using Trade-offs with an Implementation
5.3			Summary .
6
A
Conclusion
6.1			Future Directions. .			. .
6.1.1 Towards Practical, Provably- Good Algorithms
6.1.2 Theoretical Extensions .
6.2			Closing Thoughts			. .			.
Kinodynamic Planning Lower Bounds
A.1			Canny's and Reif's Proof
A.1.1			A Reduction. . .			. .			. . .			. .			. . .
A.1.2			Correspondence Between Homotopy Classes and Truth-Value
A.1.3			Bit-Counting . .			. .			. .			. .			.
A.2			The Extension for I?inodynamic Lower			Bounds			. .
A.2.1			The Idea. . .			. .			.
A.2.2			Bit-Counting Again			. .			. .			. .
B Guide to Notation
157
158
159
162
163
165
173
173
175
177
180
181
194
195
196
196
199
200
201
201
201
202
202
204
204
204
206
C Computing Parameters for the True-Extremal and Near-Extremal
Algorithms for Open-Chain Manipulators			209
C.1			Introduction. . . .			. . . .			. . . 209
C.2			Bounding Changes in M?1(p) . . .			211
C.3			Acceleration Bounds and Perturbations . . .			. . . .			. . . 212
lx
Bibliography
217
List of Figures
1.1
1.2
1.3
1.4
1.5
1.6
1.7
1.8
1.9
1.10
1.11
1.12
1.13
1.14
1.15
2.1
2.2
2.3
2.4
3.1
3.2
3.3
3.4
A kinodynamic planning problem for a point mass robot. 3
A kinodynamic planning problem for a point robot, showing solutions. 5
A planar, three-degree of freedom "open-chain" manipulator. 7
Maps between force bounds and acceleration bounds. . . . . 8
The visibility graph for a shortest-path problem in two dimensions. 10
A simple trajectory planning problem			. .			13
Another path planning problem. . .			. . 			14
Difficulties with a naive approximation problem formulation			15
Some intuition behind a speed-dependent safety margin CO t ci(IIvII)			17
A ?-sa1e trajectory avoids contact with obstacles by a speed-de-
pendent margin ?18
Obstacles and safety margins for a planar point robot problem			19
An ?-optimal trajectory can be topologically different from the truly
optimal kinodynamic trajectory			21
An example of a p?vab1y good polynomial-time e-approximation
algorilhrn for optimal kinodynamic planning. . 			22
Growing a reachability graph in one-dimension, starting at the state
(0,0). . .			25
Vertices, or states, from the first three generations of a reachability
graph (shown in succession).			. . .			. .			.			27
(?a, r)-bangs for a planar robot.
An (nz,?v)-tube projected into phase-space for one dimension. .
Bounding position changes subject to velocity knotpoints.
Bounding position changes subject to velocity knotpoints, another
case. .			. . .			. . .			. . .			. .
48
50
56
58
69
Edge-states can allow better approximation.
First step in proving the Efficient Tracking and			Strong			Tracking
Lemmas. . .. .			75
Constructing bounding velocity functions. .			. . . . . .			.			78
Second step in proving the Efficient Tracking and			Strong			Tracking
Lemmas. .			. . . .			.			79
x
3.5
3.6
3.7
3.8
3.9
3.10
4.1
4.2
4.3
4.4
4.5
4.6
4.7
4.8
4.9
5.1
5.2
5.3
5.4
5.5
5.6
5.7
5.8
5.9
Third step in our proofs of the Efficient Tracking and Strong Track-
ing Lemmas
Proving Corollaries from this section. . 			. . .			.
Prepending a trajectory segment.
A trajectory found by our implementation running Algorithm 2.
Near-optimal trajectories that were found by two versions of our
implementation running Algorithm 3. . . .			.
Trajectories with commanded accelerations.
82
86
87
100
101
102
An extremal set of controls U and two TI-discretizations of U: U--H?
and			. . .			. . .			. . .			. . .			. .			115
Search pruning 			117
Sets of accelerations: Ax* C Ax cAxcAx. . . . . . 			119
Allowable accelerations for the L2 Cartesian robot at any state X			125
Sets in acceleration space. . . . . . . . 			129
Details from a game against an adversary			134
Intuition for proving the Coupled Tracking Lemma: a game in 1D			135
Intuition for proving the Robust Coupled Tracking Lemma: another
tracking game in 1D139
The Control Extremal Algorithm still has the desired completeness
when we use single-occupancy cells to bound the search.
143
Ranges control functions from four class. . . 			158
N r-bangs determine an average force to O( &) accuracy161
Reachability graph pruning and the Tracking Game with Multi-
Perturbations172
Example of a contour map of computational complexity as a func-
tion of ?s and ?T for the Hybrid Algorithm for 2D Cartesian			Nino-
dynamic Planning. .			. 182
Example of a contour map of computational complexity as a			func-
tion of ?s and ?T for the two-dimensional L2 Cartesian			Algorithm
with Trade-offs183
Example of a three-dimensional plot of computational			complexity
as a function of ?s and ?T for the Hybrid Algorithm for 2D			Cartesian
Kinodynamic Planning184
Example of three-dimensional plot of computational			complexity as
a function of ?s and ?T for the two-dimensional L2 Cartesian			Algo-
rithm with Trade-offs. .			. . .			. . . . 185
A trajectory found by our planner.			. . . .			.			. . . 186
An illustrative problem and a solution found by our planner, with
= 0.6.			. 188
Solutions found by our planner for the same problem, with ?s =
0.55			and ?T = 0.9, and ? = 0.7 and ?T = 0.4.			189
xi
5.10
5.11			Four solutions found by our planner for the same problem190
5.12 Different ?s and eT choices can result in homotopically different
trajectories. . . . . . . . . .			. .			.			. . .			191
5.13			Another trajectory found by our planner.			. .			.			192
5.14			A trajectory found by our planner for the same			problem, but with
= CT = 0.8, is homotopically different. .			. .			193
C.1 A planar 2DOF a??, with constant physical parameters mi, ?2,
1?, 12, li, 12, L1 and configuration parameters Oi and 02. . .			210
xii
Chapter 1
Introduction
To be useful, a robot must be capable of moving without colliding with objects in
its environment or damaging itself. If, furthermore, a robot is to be productive in
its function, its motions should take as little time as possible. Thus, in addition to
competence ("Can your robot do X?"), we must be concerned with performance
("How long does it take for your robot to do X?")
A basic problem is to plan motions for a robot in a known environment.
Minimum-time obstacle-avoiding motions are determined by the dynamical and
control characteristics of the robot as well as the shapes and locations of the ob-
stacles. Ideally, we would like to guarantee a robot's performance relative to its
intrinsic physical, control, and sensory capabilities. Since the computation time
required by a motion planner itself will limit how the robot can interact with the
world, a planner that quickly synthesizes minimum-time motions would be desir-
able.
Unfortunately, even finding a shortest path can be computationally intractable
in the Af?-hard sense [CR?S7]. Since it can be shown that finding a minimum-time
motion is at least as hard,1 approximation algorithms are a reasonable approach
to planning time-optimal motions.
Natural questions arise about any approximation algorithm for finding time-
optimal motions:
o+ How close to optimal is the solution?
o+ What is the running time (computational complexity) of the algorithm?
1See appendix A. Intuitively we might expect the minimal-time problem to be harder since
it takes into account dynamics, and the shortest-path problem does not
2
o+ Can the running time of the algorithm be related to the guaranteed solution
accuracy?
Approximation and heuristic techniques have been proposed. However, prior to the
results in this thesis, there existed no bounds on the goodness of the resulting so-
lutions, or on the time-complexity of the algorithms. Note that by "time-optimal",
we will always mean globally time-optimal, that is, time-optimal among all possible
trajectories.2
1.1 An Approach
1.1.1 Observations
There is a wide spectrum of situations in which it is important for robot motions
to be time-efficient. How long it takes to complete given tasks in a factory largely
determines how productive and profitable that factory will be. While minimizing
the time of obstacle-avoiding robot motions cari speed up an assembly process
only to a point, in certain settings say, warehouses, shipping rooms, and docks
__ these motions would be a major bottleneck. In other situations the benefits
of a robot being able to minimize its motion time would not just be monetary:
how quickly toxic, radioactive, explosive, or otherwise hazardous materiMs can be
contained or removed at the site of an accident or leak, and even how quickly the
site can be surveyed, is important in limiting the damage to life, property, and the
environment. In general, the less time it takes for robots to perform tasks, the
less often people will need to subject themselves to dangerous environments. For
example, in outer space, robots that are both competent and fast would enable
missions to be accomplished within time constraints with a minimum of human
extra-vehicular activity.
Dynamics laws, applied controls, and constraints determine the motion of a
robot.3 If a motion is to be reliably executable by a robot, that motion must obey
constraints on dynamical and control quantities, such as acceleration, velocity,
and applied and internal torques and forces. While these constraints may directly
2We strongly distinguish time-optimal from locallatime-optimal.
3While the effects of dynamics on a planned robot motion appear whenever inertia is high
compared to available torques and forces they are most dramatically illustrated when one con-
siders using the Remote Manipulator System on the NASA Space Shuttle to move an object
matching its payload rating nearly half the mass of the Shuttle. [LLz9o].
3
reflect physical and control limitations, we might also impose "tighter" constraints
to reduce wear or power requirements or to use a simpler physical model, for
example. Bounds on the velocities and forces that a payload or end-effector can
tolerate would also result in additional constraints.
Figure 1.1: A kinodynamic planning problem for a point mass robot, showing
the obstacles, the start S (s,s), the goal G = (g,g). The point is subject to a
maximum speed v and a maximum acceleration a. Solutions and a speed-dependent
safety margin are illustrated in figure 1.2.
Execution time, obstacle-avoidance, and dynamics are not the only aspects we
need to consider about robot motions. We must also take into account how much
time is spent planning them. if a motion task is to be repeated often enough, then
it might be profitable spend a long time to plan nearly time-optimal movements.
If a task will be executed just once, but as a part of a planned mission whose
total time we wish to minimize, we might allocate planning time to a motion in
proportion to how much its execution time will affect the total mission time. When
4
the the usefulness of a motion will be short-lived or when the robot environment
can change quickly, the motion planning problem is inherently "on-line", and it is
the time required to plan plus the time required execute the motion that should be
minimized. Thus, we would like to plan time-efficient robot motions as best we
can tn an amount of lime reasonable for the problem. We therefore would hope for
an analytical relationship between the running time bound of an approximation
algorithm and the guaranteed quality, compared to the theoretical optimal, of a
motion it plans.
1.1.2 Approximation Algorithms
Using techniques from several fields including mechanical engineering, control
theory, and computer science researchers have introduced algorithms to generate
robot motions that, to various degrees, are approximately time-optimal. Combi-
natorial optimization provides a general framework for analyzing the relationship
between the running time of an approximation algorithm for an optimization prob-
lem and the quality of the solutions it is guaranteed to find. In particular, for
optimization problems over discrete (i.e., integer) variables, there is the notion of
e-approximation algorithms [FSs2], which we will borrow and extend.
Consider an algorithm for some optimization problem in which a cost L is to be
minimized. If for each problem instance ki and approximation parameter e > 0 the
algorithm is guaranteed to find a solution with cost no more than (1 t ?)Lj, where
Lj is the cost of the optimum solution, then it is an e-approximalion algorithm.
The (time-) complexity of the algorithm is the worst-case bound on its running time
over all problem instances in terms of ?, the length of the encoding4 of the problem
instance, and the largest number in the encoding. Thus, the complexity of an
e-approximation algorithm describes how quickly its running time can grow as we
demand solutions guaranteed to be closer and closer to optimal in cost.
If the complexity of the algorithm is polynomial in ?,the length of the encoding,
and the largest number in the encoding, then the algorithm is called a (pseudo-)po-
lynomial-time approximation scheme. The algorithm is a flilly polynomial-time
approximation scheme if its complexity is polynomial in terms of both and the
length of the encoding.5
4By an encoding we simply mean a representation using a fixed finite alphabet.
5Le. O((IKit0?1?(1?)?0?1?) [Ps82].
5
Fr
G
II
s
F
Figure 1.2: A kinodynamic planning problem for a point robot, which obeys par
tide dynamics (f = ma). This figure shows the obstacles, the start S, the goal
G, and three illustrated trajectories: time-optimal 1', optimal (safe) kinodynamic
Fr, and approximately optimal kinodynamic Fq, which happens to be exact at the
start and goal. All three obey the velocity and acceleration bounds v and a. How-
ever, the time-optimal rnotio'ii I, leaves no margin for obstacle avoidance, whereas
Fr avoids obstacles by a margin ?, and Fq avoids them by a smaller distance
(1 --H
1.1.3
6
Provably Good Approximation Algorithms for
Kinodynamic Planning
Because of control error, sensing uncertainty, and errors in the geometric models
of robots and their environments, we cannot expect the motion executed by a
robot to be exactly the same as the planned motion. Thus, the optimality of
planned motions should be defined relative to these factors. In addition to including
robot dynamics and obstacles, our kinodynamic planning formulation6, proposed
with Canny, Donald, and Reif in [CDRX88], accounts for control, modeling, and
sensing error by demanding that a planned motion avoid all obstacles by a speed-
dependent "safety margin" From the kinodynamic planning standpoint an optimal
kinodynamic trajectory is therefore a tim&optimal motion plan. (See figures 1.1
and 1.2.)
This thesis presents provably good approximation algorithms for a set of op-
timal kinodynamic planning problems within an e-approximation framework. Es-
sentially, we will say that a planned motion is e-optimal if it (a) takes at most an
e-factor more time than an optimal motion and (b) avoids obstacles by a safety
margin that is at most an e-factor smaller than the desired margin. (See figure 1.2.)
For each class of robots considered, the results depend on four basic assumptions:
1. a bounded environment,
2. velocity bounds,
3. force or acceleration bounds,
4. a positive, possibly speed-dependent, safety-margin, and
5. polyhedral obstacles (which may overlap arbitrarily).
The algorithms described here run in time polynomial in both ?(i and the geo-
metric complexity of the problems. The most general class of robots we consider is
that of fully-controllable (actuated in every degree of freedom and able to overcome
gravity statically) d-link open kinematic chains.7 (See figure 1.3.) We believe that
the techniques developed to prove the correctuess and complexity of our provably-
good polynomial-time approximation algon?thms for kinodynamic planning can be
6For a formal definition, see Section 2.1.1. This formulation is extended in Section 4.1.
7Thus, their dynamical behavior obeys a Lagrangian dynamics equation for (an open-
kinematic chain of) rigid bodies with gravity. See equation (4.1).
7
further generalized, if not applied directly, to other motion planning algorithms
and other classes of robots.
Ji			J2
J3
Figure 1.3: A planar, three-degree of freedom open-kinematic-chain ("open-chain")
manipulator with two revolute joints (J1, J3) and one transiational joint (J2). An
open kinematic chain is a serial linkage of rigid bodies, i.e., it contains no loops.
The most general class of robots we consider have are three-dimensional (i.e.,
full-dimensional)
1.2 Kinodynamic Planning
1.2.1 Overview
A kinodynamic trajectory is a motion that obeys simultaneous kinodynamic con-
straints.8 These constraints include kineniatic constraints, which limit the config-
urations9 a robot may assume, and dynamics constraints, which include bounds
on velocity, acceleration, and other quantities that determine a robot's motion, as
well as the dynamics laws that apply. Dynamics constraints are independent of
8The term "kinodynamic" describes the constraints relevant to tim&optimal motion plans.
Thus, optimal kinodynamic planning, as we describe later, is a sp?cific constrained optimal
control problem, as distinguished from general constrained optimization or problems obtained by
"factoring" the time-optimal trajectory problem described in Section 1.2.3.
9A configuration is the generalized position of a robot, which specifies its location and pose.
8
(a)
I			+			I
a 1a
m
I			+			I
(b)			a--H?--Hm'f			+
+
II
I			+			I
(c)
a=xv
??open??hain?? ??I +
I--H
Generalized force			Acceleration
Figure 1.4: Maps between force bounds and acceleration bounds. R?esults cover
robots with state-independent dynamics under (a) L? and (b) L2 dynamics bounds
and (c) robots obeying open-kinematic-chain dynamics. See Section 4.1 and equa-
tion (4.7) for "open-chain" dynamics.
9
a robot's environment. Finally, strictly kinodynamic constraints are environment-
dependent constraints that apply to the robot's state (configuration and velocity)
and its time-derivatives but do not fall into either of the previous categories.
The kinodynamic planning framework encompasses several classes of motion
planning problems. Path planning problems require one to find motions that satisfy
kinematic constraints. In trajectory planning one attempts to find motions that
satisfy kinematic constraints and dynamics constraints. This informal introduction
to kinodynamic planning will review these problem classes first (in Sections 1.2.2
and 1.2.3), to provide a background for the kinodynamic planning problem that
corresponds to the problem of finding a time-optimal motion plan.
Although simple examples in two dimensions will be used to help motivate and
illustrate the kinodynamic planning framework, it generalizes to motion planning
in arbitrary degrees of freedom. A typical example is seen in figure 1.1, where a
point mass in ?2 must move from a start state (configuration and velocity) to a
goal state while avoiding contact with a set of polyhedral obstacles.
1.2.2 Path Planning
Path planning considers only the kinematic aspects of motion planning. Thus,
the path planning problem is to find an obstacle-avoiding path between two robot
configurations, or, for a point mass robot in the plane, between two positions.
Mathematically, a path is a continuous map
p: ?,Tf] H
where C is the space of robot configurations and Tf > 0. The kinematic constraints
of this problem are simply the obstacles, which the path'0 must not intersect.
1.2.2.1 Shortest Paths
An "efficient motion" in the context of path planning is a shortest path, i.e., a
minimal arc-length motion. In two dimensions, a shortest path between two points
amongst polygonal obstacles with geometric complexity (number of vertices) N can
be found in time 0(N2) by constructing a "visibility graph" and searching for a
shortest path in the graph [GM87]. (See figure 1.5.) Although [Can88aJ shows that
there is an algorithm for finding some path in time polynomial in the geometric
iOstrictly speaking in mathematical terms this is actually its image.
10
complexity and the algebraic complexity of the obstacles, and singly-exponential
in the degrees of freedom, the shortest-path problem for a point among polyhedra
in three dimensions is AF?-hard [CR871
5
- --
--H
--H -
--H--HI
- - -I
I			--H--H
g
Figure 1.5: The visibility graph for a shortest-path problem in two dimensions.
Obstacle edges are also graph edges, and all other graph edges are shown as dashed
lines.
1.2.2.2 e-Approximately Shortest Paths
Since the shortest-path problem for a point robot amongst polyhedra in ?? is
AT?-hard, one might consider a corresponding approximation problem. For a given
problem instance, a shortest path between the start and goal positions is an optimal
solution. If a path between the start and goal has arc length no more than an e
factor greater than the shortest, then it would be natural to consider it "within e"
of being optimal. Thus, the e-approximation problem for shortest-path planning is,
for given start and goal positions and a given set of obstacles, to find an obstacle-
avoiding path between the start and goal whose arc-length is at most (1 + e)Lopt,
where Lopt is the length of a minimal arc-length solution. A fully-polynomial-time
e-approximation algorithm for the point robot problem in ?? is given by [PapS5]
1.2.3 Trajectory Planning
A motion planning problem with dynamics constraints is a trajectory planning
problem; that is, to find a motion that is a function of time. Because the term
11
"time-optimal trajectory" has come to mean several things in the computer science,
robotics, and control communities, it is necessary to describe what is meant by that
term in this thesis.
1.2.3.1 Basic Trajectory Planning
Let us return to the path planning problem in ?2 and "add dynamics". First, we
change the endpoint conditions of the motion to include velocity, so the motion
must now go from a start state and a goat state.11 Then, we introduce a bound
on the speed of the motion and a bound on its acceleration. Since a point mass
obeys the elementary dynamics law f = rna, an acceleration bound is equivalent
to a bound on applied force.
For a point in the ?2, a ii'ajectory taking time Tf is given by a map
F: [O,Tfj			?2 ?
where F(t) = (p(t),p(t)) for some path p : [O,Tf] ?2 The dynamics bounds
require these maps to obey
IIpII?			? ?			and
Ili)			? a,
where the bounds v and a are given in the problem iiistance. The resulting prob-
lem is a trajeciory planning problem, and with appropriate modifications, such
as the addition of control forces and more complicated dynamics laws, it can be
generalized to more realistic robot models.
In this thesis, a basic trajectory planning problem instance will have four parts:
1. a start and goal state for the robot,
2. obstacles that the robot must avoid,
3. a dynamics law relating the robot's state, acceleration, and applied (control)
generalized forces
4. dynamics bounds, such as bounds on the robot's velocity, acceleration, and
the applied forces.
`1Recall that a state is a (configura?ion, veThcily) pair.
12
A solution to a problem instance is a trajectory from the start state to the goal
state that avoids the obstacles such that under the dynamics law the velocity, ac-
celeration, and force bounds are not violated. The dynamics law and the dynamics
bounds are the dynamics constraints.
1.2.3.2 Planning Time-Optimal Trajectories
The term "time-optimal trajectory" (with respect to given problem constraints)
has several meanings in the robotics and control literature.
1. It can mean a trajectory that is time-optimal among the trajectories that
trace out a specified path.
2. It can mean a trajectory that is time-optimal in a class of trajectories that
correspond to some parameterized set of functions.
3. It can mean a trajectory that is time-optimal in the set of all trajectories
(that obey the constraints).
It does not help matters that the word "optimal" is often used to mean "locally
optimal" by the robotics and control communities and "globally optimal" by the
computer science and operations research communities, even though each commu-
nity has a reasonable justification for its choice of meaning.
In this thesis, a time-optimal trajectory will always mean a trajectory that is
globally time-optimal in the set of all trajectories that obey a given set of constraints
and endpoint conditions. We call the problem of finding time-optimal trajectory
with respect to obstacles and dynamics constraints the time-optimal trajectory
planning problem, and it is the natural analogue of the shortest-path planning
problem. It should be noted that finding shortest paths does not generally help
one to find time-optimal trajectories. (See figures 1.6 and 1.7.) By choosing a
maximum acceleration sufficiently large compared to the maximum speed, we can
use the result and proof of [CR?S7] to show that this problem is )\f?-hard for a
point robot amongst polyhedral obstacles in three dimensions.12
It should be made clear that in this thesis does not consider the problem of
synthesizing an optimal control law, another important research area. However,
the kinodynamic planning problem formulation allows for control error in following
a planned trajectory.
12See Appendix A,
13
s
G
Figure 1.6: A simpte trajectory planning problem. Which way around the rectangle
is minimal-time will depend on the acceleration and velocity bounds.
1.2.3.3 Approximately Time-Optimal Trajectories
Since the time-optimal trajectory problem is generally intractable, we have pursued
approximation algorithms. As described in Section 1.4, certain approximation
algorithms for finding time-optimal robot trajectories in the presence of obstacles
existed previous to the results presented here. While some of these earlier methods
are useful for synthesizing robot trajectories, there are currently no proofs of how
close to optimal the solutions they find will be. In fact, it is doubtful that an
appropriate approximation problem had been formulated prior to [CDR?X88].
One obvious way to cast the time-optimal trajectory planning problem into an
optimization problem is to use the time of a trajectory as its cost, while demanding
that solutions avoid the obstacles and obey the endpoint conditions and dynamics
constraints. Thus, if a time-optimal trajectory takes time Topt, an ?-time-optimal
trajectory would take at most time (1 + ?)Topt.
While this is the direct analogue of the ?-optimal shortest-path problem, it
differs in a fundamental way. Suppose we have a shortest path Popt, pick any
point x on it, and then choose another point x' near x such that the line segment
between them intersects no obstacles. Then the shortest path between the start
and goal that passes through x' will have a length bounded by 2jjx --H x'II plus the
length of the original path Popt In a region of obstacle-free space, a shortest-path
solution Popt can always be subjected to "perturbations" to obtain solutions that
are infinitesimally longer than the shortest path. Thus, the ?-optimal shortest-path
problem is intuitively "reasonable" in the sense that whenever a solution exists,
?-optimal solutions that are not exactly optimal will also exist.
14
s
Figure 1.7: Another path planning problem. The shortest path through the
"zig-zag" is the shortest path from s to g under both the L2-norm and the
L?-norm. Now, consider the related trajectory-planning problem with zero start
and goal velocity, and with acceleration and velocity bounds a and v. If is large
enough compared to the width of the zig-zag, then the minimal-time trajectory
through the zig-zag will take longer than the minimal-time trajectory that goes
from s to g by taking the outside route. Thus, finding a shortest path might not
even help to find the homotopy class of the minimum-time motion.
15
The same would not be true for the obvious ?-time-optimal trajectory problem.
Consider the example in figure 1.8. The trajectory shown is the optimal trajectory.
It follows full negative x-acceleration for its duration, and its y-acceleration is full
negative, followed by full positive. A viable trajectory from S that deviates from
this trajectory anywhere, even infinitesimally, must take substantially longer to
reach the goal state; otherwise it either will fail to achieve the goal state exactly
or will crash into the obstacle.
G
s
Figure 1.8: Difficulties with a naive approximation problem formulation. This (il-
lustrated) optimal particle-dynamics trajectory follows full negative r-acceleration
for its duration. The y-acceleration is full negative, followed by full positive. A
viable trajectory from S that deviates a small distance from this either will fail to
achieve the goal state exactly, will crash into the obstacle, or will have to go around
it. In fact, all trajectories that enter a region of state space nearby a segment of
this trajectory are doomed to crash into the obstacle. A state where an arbitrarily
small deviation forces qualitative changes on trajectories otherwise beginning at it
is a crnYical point.
16
1.2.4 Kinodynamic Planning
This thesis focuses on one class of problems within the kinodynamic framework.
The problem formulation accounts for errors in control, sensing, and models of
robot and obstacle geometry by including a speed-dependent safety margin as a
problem parameter in addition to the kinematic and dynamics constraints allowed
in the problem classes discussed above. Thus, for a given robot, a motion that is
time-optimal with respect to these constraints will be time-optimal in the set of
trajectories that the robot could attempt to follow while being guaranteed not to
collide with obstacles in the environment.
1.2.4.1 Safety Punctjons and Kinodynamic Planning
Following a theoretically time-optimal trajectory closely enough to avoid obstacles
may require unrealizable precision in control or sensing. The exact time-optimal
trajectory may thus be unexecutable by a physical robot. If the trajectory is
planned using models of the robot and the environment that contain some error,
then the likelihood of the executed motion avoiding collisions is even less.
For these reasons, an optimal planned trajectory should avoid obstacles by a
safety margin ?. In the kinodynamic planning formulation here, this margin is
allowed to be a positive affine function of speed, with function coefficients chosen
a priori. Specifically,
?v(co,ct)(v(t)) = co + ctlv(t)I,
where the coefficients co > 0 and c1 > 0 are contained in the algorithm input. Thus,
a trajectory that is "?v(co, ci)-safe", or "??-safe" for short, will avoid all obstacles
by a constant margin plus a margin linear in speed. Since whether a robot state is
safe depends on the robot velocity as well as position and the presence of obstacles,
this characterization of safety is a strictly kinodynamic constraint, as opposed to
a pure kinematic or pure dynamics constraint.
This first-order choice roughly corresponds to how accurately and quickly a
robot senses its position and velocity, combined with how much time and space
it requires to "survive" velocity errors and correct for them. Figure 1.9 illus-
trates some motivation this. For additional motivation, one might consider a one-
dimensional system tracking a planned trajectory. R?ecall the relation between
energy and momentum: = mv, and suppose that the control system allows a
maximum velocity error of Av, and Fres force is available for correcting velocity
17
Figure 1.9: Some intuition behind a speed-dependent safety margin Co t ci(IIvII).
Suppose following the path at planned speed v demands maximum acceleration
a radially inward. Taking the derivative of 1? we see that velocity error of
magnitude Av can increase the curve's radius by 2(?a V) ? A change in "stopping
distance" due to velocity error would also be linear in speed. Errors in static
position sensing and geometric models are accounted for by CO.
The safety margin ensures the existence of a "tube" or family of trajectories
that are "nearby" a time-optimal %-safe trajectory and approximate it. The paths
traced out by such motions would lie inside a tube of diameter ?, or a 6v-saj?ty
tube centered about path traced out by an optimal planned motion. (See figures
1.10 and 1.11.)
For a given robot and environment, a ?v-safr kinodynamic trajectory avoids
obstacles by a ?-safety margin and obeys the robots dynamics constraints. The
?-safr kinodynamic planning problem is to find a ?-safe kinodynamic trajectory
, 7
errors. Then, the tracking error in position can grow by my8V before the velocity
error can be corrected. Concisely stated, ci characterizes how accurately the robot
can control its energy consumption.
1?
" ??
18
G
Figure 1.10: A ?-safe trajectory avoids contact with obstacles by a
speed-dependent margin ?.
19
(a)
Figure 1.11: Obstacles and safety margins for a planar point robot problem. The
concentric boundaries correspond to different robot speeds. (a) shows the original
safety margins. (b) shows a safety margin a factor of O?3 smaller than in the first.
20
from a start state to a goal state. Unless noted, the term "kinodynamic planning
problem" will refer to this problem.
An optimal kinodynamic traicctory is a time-optimal ?-safe kinodynamic tra-
jectory, and an optimal solution to a given kinodynamic planning problem is a
time-optimal ?-safe kinodynamic trajectory from the start state to the goal state.
The optimal kinodynamic planning problem is to find an optimal solution to a
kinodynamic planning problem.
1.2.4.2 Approximately Optimal Kinodynamic Plans
We will choose our definition of an e-optimal kinodynamic trajectory to meet two
criteria. First, it should be intuitive and have the flavor of a conventional e-
approximation formulation. An e-optimal trajectory must be feasible according
to the kinematic and dynamics constraints, and we must be able to specify a
trajectory "arbitrarily close to optimal" by choosing an adequately small e. Second,
it should be feasible to find trajectories that are provably e-optimal solutions to a
kinodynamic planning problem whenever an optimal solution exists. In particular,
it should be possible to give a procedure that will provably find e-optimal solutions
to kinodynamic planning problems having optimal solutions. ?Ve must therefore
guarantee that some finitely-parameterized trajectory will be e-optimal, even for
problems where no closed-form optimal solution is known to exist.
Toward the first criterion, if an optimal solution to an instance of the ?,-
kinodynamic planning problem takes time Topt, then we allow an e-optimal solution
can take up to (1 + e)Topt time. Towards the second, we will allow e-optimal tra-
jectories to avoid obstacles by a margin given by (1 --H e)?; this is sometimes called
"6?1-safety", i. e.,
(1 --H
This &factor of slack in the safety margin ensures that e-optimal trajectories will
not be forced to follow the same path as an optimal solution. Also for the purpose
of meeting the second criterion, we will allow e-optimal trajectories found by an
algorithm to be e-close to the specified start state and goal state,13 by which we
mean that its initial and final states approximate these states to within 0(e).
An algorithm guaranteed to find e-optimal solutions to instances of an opti-
mal kinodynamic planning problem is called a provably good (e-)approximation
algorithm for that problem. We must note that a trajectory planned by such an
13This was not discussed in [CDRx88], but included in [Dx89b?Dx9oa].
21
p
Fr
Figure 1.12: An &optimal trajectory can be topologically different from the
truly optimal kinodynamic trajectory. In this illustration, Fr obeys the original
safety-margin, and Fq obeys a margin a factor of ? smaller (with a fairly large
<1.
22
Input:
a (acceleration bd)
v (velocity bd)
1 (env. diam.)
Co > 0
Cl > 0 (safety)
0 (obstacles)
S = (s,s) (start )
G = (g,g) (goal)
Algorithm 2
Output:
Fq (trajectory)
Tq ? (1 + ?)Topt
(1 --H ?)?v(c0,ci)-safe
"?-close" to S, G
O(CdBN(?)3d)
Figure 1.13: Algorithm 2 (from Chapter 3) is a provably good polynomial-tirne
?-approximation algorithm for optimal kinodynamic planning. As input, the algo-
rithm is given an instance of the Optimal Basic Kinodynamic Planning Problem
(see Chapter 2) in d degrees of freedom and an approximation parameter ? > 0.
If a solution exists, the algorithm is guaranteed to find an ?-optimal solution I'q
in time polynomial in and N, the geometric complexity of the obstacles in the
problem instance. CB is a constant dependent on the robot's dynamics bounds
(maximum speed v and maximum acceleration ?a), the workspace diameter 1, and
the ?-safety parameters, Co and Cl.
23
algorithm is permitted to be quite different from an optimal solution, except at the
start and goal. The e-optimal kinodynamic planning problem permits this in two
ways. First, because an e-optimal trajectory can take more time than an exactly
optimal trajectory, it can may trace out a longer path. Second, because e-optimal
trajectories obey slightly smaller safety margins, they can "squeeze through" some
gaps between obstacles that optimal trajectories cannot, and they can squeeze
through others at higher speeds. This illustrated in figure 1.12.
1.3 Techniques
1.3.1 Path Planning Observations
Even for path planning,'4 an apparent barrier to finding an e-optimal motion is that
the number of non-looping path-homotopy classes can be at least exponential in
the number of the obstacles. R?esults in shortest-path planning originally motivated
the study of kinodynamic planning in e-approximation framework and our use of
a state-space graph.
1.3.1.1 2D Path Planning
Once again, consider the problem of a planning shortest path in the plane for a
point robot amongst polygonal obstacles with a N vertices. Any shortest path
for such a robot is decomposable into line segments between obstacle vertices and
the start and goal positions. For this reason, we can reduce two-dimensional path
planning to finding the shortest path on a a visibility graph (recall figure 1.5),
which will have at most N t2vertices and can be constructed in time 0(N2), as
shown by Ghosh and Mount [GMS7]. While the number of total paths through L
vertices in the graph can be exponential in L, a breadth-first exploration of the
visibility graph finds a shortest-path solution in 0(N2) time.
1.3.1.2 3D Path Planning
Now, consider shortest-path planning in three dimensions. Here, we can still de-
compose any shortest path into straight line segments. However, a corollary of the
AF?-hardness result in [CR87] is that finding these line segments is Af?-hard. On
`4Recall that there are no dynamics in path planning.
24
the other hand, each segment endpoint either lies on an obstacle edge or is the
start or goal.
Papadimtriou's fully-polynomial approximation algorithm [Pap85] is conceptu-
ally simple though involved in detail. It first divides up the obstacle edges into
a number of segments, polynomial in ??, having non-uniform lengths. Edges on
the segment-segment visibility graph'5 correspond to equivalence classes of lines
between points on obstacle edges, such that the lengths of any two lines in a class
differ by no more than an e-factor. Graph edge weights are segment-segment dis-
tances, and this graph is easily augmented to include the start and goal. Finding
the shortest path between the start and goal vertices in this graph yields a path
between the start and goal positions that is at most an e factor longer than opti-
mal. Since the number of vertices in the graph is polynomial in ??, so is the cost of
finding a shortest path between any two vertices in it.
1.3.2 Graphs Embedded In State Space
The basic idea behind the algorithms in this thesis is to reduce finding an e-optimal
solution to a kinodynamic planning problem to finding the shortest path in a
directed reachability graph dependent on the problem instance. The vertices of the
graph "discretize" the robot statespace,'6 and the edges of the graph correspond to
trajectory segments that each take the same amount of time. The graph will have
the provable property that if an optimal solution exists, then the shortest path in
the reachability graph will directly correspond to an e-optimal solution.
1.3.2.1 A Reachability Graph
To generate the reachability graph, we choose a state ?e-close" to the desired
start state. This will be the root vertex of the graph. An e-optimal trajectory
wilt correspond to a the shortest path (in the graph) from the root vertex to
a vertex or edge sufficiently close to the desired goal state. The degree of the
graph and the number of vertices will be bounded by a polynomial in 1?. Thus,
tnstead of having to search a set of trajectories possibly exponential (or worse) in
the number of obstacles, our algorithm searches a graph with size polynomial in
_ and proportional to the "vohime" of the statespace. Except for dynamics laws,
kinodynamic constraints are forbidden regions of this discretized statespace.
15Vertices in this graph correspond to obstacle-edge segments.
16The space of all robot states i. e. configuralions x vetocilies
25
-			-			-			-?
(a)			x
0			0			0			0			0			0			0			0			0
x
Figure 1.14: Growing a reachability graph in one-dimension, starting at the state
(0,0). Constant accelerations from the set f--H?a, 0,?(it applied for duration r gen-
erate the graph. (a) shows the first generation. (b) shows the first three. States
found will lie on an regular grid and "fill" much of the statespace region that obeys
the kinodynamic constraints.
 00--
--000---
.`..?. .0---?--.t0-
0			0			0			0			0			0			0			0			0
--0			0			0			0
__?0?:i?iij:::1j?1?%;i%???)___
`			:,			? :
00000-
-?000			4
0--- -----?--
0			0			0			0			0			0			0			0			0			0
A
ar'
0			0			0			0			0			0			0
26
For any finite set of control-primitives17 applied at the root vertex state for a
timestep 7, we obtain a corresponding set of trajectory segments by integrating
forward in time. If we prune this set of trajectory segments to eliminate those
that violate the kinodynamic constraints, we obtain a set of trajectory segments
that obey these constraints and that last for duration r. These are the edges
from the root vertex; they determine its neighbors in the graph. By applying the
the control primitives on the newly-found graph vertices and taking the transitive
closure, we generate a reachability graph. Thus, instead of being determined by
spatial proximity, graph edges are determined by trajectory segments generated by
control primitives, and each edge corresponds to T elapsed time. Note that since
the reachability graph is used to find e-optimal trajectories, we only require the
graph edges (trajectory segments) to obey the desired safety margin weakened by
a factor of e, i.e., a (1 --H e)? safety margin.
If the root vertex and control primitives are chosen carefully, then we might
expect that the reachability graph would "fill" the region of statespace legal under
the kinodynamic constraints. The intuition is that at each state corresponding
to a vertex, the control primitives "sample" the vector fields permitted by the
dynamics bounds. For example, let us consider an obstacle-free regions in one
dimension. Suppose that (0, 0) is the root vertex, and suppose that we chose
constant accelerations from the set f --Ha, 0, a) applied for duration r This generates
a reachability graph as shown in figure 1.14.
1.3.2.2 Graph `I?ajectories
We will call a trajectory that corresponds to a path in the reachability graph a
graph trajectory. The smaller the timestep T is, the better some graph trajectory
will approximate an arbitrary trajectory in this simple state space space. In figure
1.15 we show the vertices from the first three generations of the reachability graph
for a two-dimensional problem.
Limiting the set of control primitives to t--Ha,a) yields a similar graph. For
higher dimensions or more complicated dynamics bounds, the analogous set of
control primitives consists of discretized control extremals or "near-extremals"
17A control primitive is function from time to acceleration (or to applied forces).
27
Figure 1.15: Vertices, or states, from the first three generations of a reachabil-
ity graph (shown in succession). Dots show position, line-segments show velocity.
Graph-generating parameters are chosen to guarantee that the graph will (dis-
cretely) fill all relevant regions of state space.
28
1.3.3 Graphs With Guarantees
While reachability graphs have intuitive appeal, we need to analytically relate the
parameters that determine a reachability graph to the quality of the approximation
obtained by searching it for a shortest path. In particular, how can we guarantee
that some graph trajectory will be e-optimal if a kinodynamic planning problem is
solvable?
1.3.3.1 The Key Question: Tracking
A simple observation is that if a trajectory avoids obstacles by a ?? margin, then
there is an statespace region surrounding it in which all positions lie outside of all
obstacles. A trajectory that stays in this region therefore avoids obstacles, as in
figure 1.10. Furthermore, we can easily show that a trajectory that at all times
tracks, or mimics, a ?-safe trajectory to within certain tolerances ?? in position
and ?? in velocity will be (1 --H e)?-safe.'8
If a trajectory tracks an optimal kinodynamic trajectory closely enough, to
an e-safe tracking tolerance, not only will it be (1 --H e)?v-safe, but its time and
endpoints will satisfy the conditions for e-optimality. Ignoring reachability graph
size for the moment, it would appear that the crucial question is how, given a
tracking tolerance, to choose
1. a root vertex state,
2. an adequate set of control primitives, and
3. a timestep
that will generate a graph trajectory that tracks an optimal solution to this tol-
erance. It is not generally apparent whether a such a trajectory can be generated
without violating the dynamics constraints. Specifically, we might worry that if
the set of control primitives is finite, then any graph trajectory staying in the
(1 --H e)?-safe region around the optimal trajectory might fall flirther and further
behind it.
18This sort of tracking is depicted in figtire 2.2,
29
1.3.3.2 Easier Tracking and Bounding the Graph Size
Suppose an optimal kinodynamic trajectory taking time Topt is uniformly slowed
down19 by a factor of ?. Then the resulting "?-slowed" trajectory will take time
(ltc)Topt, be within O(?) of the start and goal,20 and avoid obstacles by a (1 --H?)?
safety margin. Thus it will be ?-optimal. Furthermore, it will obey dynamics
bounds that are tighter (i.c.smaller) than those obeyed by the optimal trajectory.
This guarantees that there will be a closed region of states surrounding the im-
age of the ?slowed trajectory, such that under the original dynamics bounds, a
trajectory beginning at any state in this region will be able to track the slowed tra-
jectory to within a constant tolerance that depends only on the timestep and the
dynamics bounds. In other words, under the original dynamics bounds, there will
be a dynamics advantage that eliminates "critical points" from a closed region of
states surrounding the e-slowed trajectory image, relative to the e-weakened safety
margin. (Refer to figure 1.8.) Without knowledge of the actual optimal kinody-
namic trajectory, we can exploit this advantage to choose the timestep, control
primitives, and a root vertex that generate a trajectory that "tracks" the e-slowed
optimal trajectory closely enough to meet the e-optimality criteria.
Finally, we must be able to bound the size of the graph seard? (for finding a
shortest path in it). Two techniques prove useful. First, if we d?oose a finite set of
constant accelerations as the control primitives, we can immediately compute the
maximum size of the set of velocities and positions possible for any vertex state.
Assuming a bounded world, this limits the size of the reachability graph itself.
More generally, for appropriate primitive controls it is possible to use a density
condition to prune the search to obtain polynomial complexity bounds, as we show
in Chapter 4,
1.4 Previous and Related Work
Robotics is an inherently multidisciplinary area, and researchers from several dif-
ferent fields have been actively studying aspects of what we call kinodynamic plan-
ning for many years. While the path planning aspect of kinodynamic planning
is computational-geometric by nature, finding an optimal kinodynamic trajectory
19That is, its velocity will be an &factor smaller. See equation (2.9) for mathematical
expression.
20We require that ? < 1, since we don't want to allow collisions.
30
obviously requires the solution of a constrained optimal control problem. We rec-
ognize the importance of the many previous results concerning the time-optimal
trajectory problem that came from the field of optimal control, in addition to re-
sults from computer science. We have noted that in recent years other researchers
have begun to explore the links between control theory, computational geometry,
and computational optimization.
For a general review of issues in robotics and algorithmic motion planning, see
[BllJ+82 ,Yap86,Bra89 ,BBD+90,Lat9l].
1.4.1 Applied Control Theory
1.4.1.1 Optimal Control
There exists a large body of work on optimal control in the control theory and
robotics literature.21 Much of this work attempts an analytic characterization of
time-optimal solutions. A major example is the well-known Pontryagin Maximum
Principle [PBGM62], which gives an extremality condition necessary for optimal
control and yields a corollary that can be used to solve a class of linear control
problems. More recently Shin and McKay [SM85b] showed that the time of some
geodesic trajectory will bound the minimum motion time and gave conditions under
which a geodesic trajectory will be optimal. Sontag and Sussman [5585,5586]
considered a class of dynamical systems and established sets of conditions under
which (a) at least one control must be bang-bang, and (b) singular controls must
be analytic. Schaettler [5ch87] considered a class of single-input linear control
systems in ?? and showed that every state has an open neighborhood in which a
bang-bang trajectory with more than seven switchings cannot be optimal.
While these results are only a few among many in a rich and mathematically
challenging field, the application of optimal control theory to motion planning has
been difficult. Reasons for this include: state constraints (as opposed to control
bounds), the generally non-linear dynamics of robot manipulators, and the absence
of analytical solutions. Research in time-optimal trajectory planning for robotic
manipulators dates back at least to the work of Kahn and Roth [Nah69,KR7i],
who linearized the equations of motion, otherwise approximated the dynamics, and
assumed no kinematic constraints. Unfortunately, such simplifying assumptions
concerning the dynamics are theoretically invalid [LWP8O]. Even at present, there
21For a background on optimal control theory, terminology, and methods, see Leitman [Lei66j,
Athans [AF66], or Kirk [Kir70j.
31
exist no exact algorithms for optimat kinodynamic planning with more than two
degrees of freedom.
Studying restricted versions of the optimal trajectory problem has yielded nu-
merical techniques that take into account the full dynamics of an open-chain manip-
ulators. Luh, Walker, and Lin [LW77,LL81] minimize the motion time over paths
consisting of straight line and circular arcs (in the space of Cartesian end-effector
positions. Later, Bobrow, et al [Bob82,BDG85], and Shin and McKay [SM85a]
provided numerical algorithms for finding the minimum-time trajectory that traces
a given input path with zero start and goal velocity. Shiller and Dubowsky [SD85]
extended the Bobrow result to include gripper and payload constraints. Hollerbach
[11o183] showed how to scale the velocity profile of a given trajectory to obtain a
trajectory that obeys dynamics bounds. Slotine and Yang [SYS9j later derived
characteristic switching points and limit curves that allow faster computation of
the minimum-time trajectory along a given path.
1.4.1.2 Near-Optimal Trajectories
Using some of these ideas, a number of authors have proposed (and often imple-
mented) heuristic or approximate algorithms for what is hoped to be near time-
optimal trajectory planning for robots with ffill dynamics and obstacle avoidance.
Rajan [R.aj85] optimized over two-dimensionj joint-space paths parameterized by
cubic splines and used the [Bob82,BDG85] algorithin to optimize the trajectory
along each path. Dubowsky, Norris, and Shiller [DNSS6] and Bobrow [Bob88] used
parameterized cubic splines in the three-dimensional Cartesian space of the end-
effector. Gilbert and Johnson [GJ85] also took a parameterization and optimization
approach.
Sahar and Hollerbach [511851 and Shiller and Dubowsky [5D88] both imple-
mented algorithms which employ a fixed-resolution configuration-space (or state-
space) grid on which path (or trajectory) knotpoints lie to compute near-minimal-
time trajectories for robots with several degrees of freedom. Both groups used
heuristic techniques to prune their searches, and they did not bound the goodness
of their approximation, nor the running time of their algorithms. However, these
grid methods take time which grows exponentially with the number of grid points
or the resolution.
Another approach is to assume that some bang-bang control will be optimal22
22This is true in the linear case without state constraints, but not known to be true in general.
32
or nearly optimal. Niv and Auslander [NA84] and [S?84] follow this line of attack
and use optimization techniques to search the space of actuator switching times
(between bangs and coasts). Brown [Bro84] makes a similar assumption, but allows
"coasts" (zero force) as well as bangs, chooses a time-interval (thus determining the
set of possible switching times), rounds each trajectory segment endpoint to the
nearest neighbor in a statespace grid, and searches the resulting network (graph).
Of previous algorithms, Brown's are the most closely related to the family we
present here. However, he makes no attempt to choose parameters that analytically
guarantee approximation goodness or closeness to optimality.
While some of the optimization results mentioned above appear to have practi-
cal application, they all fail to provide the provable bounds we desire. Attempts to
apply conventional optimization methods are prone to at least one of three short-
comings: 1) the guaranteed "optimality" of the solution is local to the parame-
terization, or even worse, local within the parameter space; 2) the cost-functional
being minimized is not time but some functional that includes time; or 3) there is
no analytical relationship between the size of the parameter-space over the which
the minimization and how close to globally time-optimal the trajectory found could
possibly be. For the specific instances of these "classical" optimization approaches,
we never find an analytical relationship between the computational complexity and
the accuracy of the solutions.
1.4.2 Algorithms With Provably Good Bounds
1.4.2.1 Path Planning
Canny and Reif [CR87] showed that shortest-path planning problem for a point
amongst polyhedral obstacles in three dimensions is XP-hard. The problem is
equivalent to a basic optimal kinodynamic planning problem with the accelera-
tion bound a set to infinity, and this observation may be used to extend [CIt87]
to show that in three dimensions, optimal kinodynamic planning is W?-hard.23
Papadimitiou [PapS5] gave an O(N4(B + log $)2((1)2) algorithm for this shortest-
path problem; here N is the number of features of the polyhedra and B is the
bit-complexity of the largest integer used in describing them.24 Clarkson [ClaS7]
described an alternative algorithm that is faster when N?3 is sufficiently large and
also presented an algorithm for the planar case that finds ?-shortest paths between
23See Appendix A.
24This is the algorithm we have previously described.
33
Table 1.1: Provably-good algoritlims for obstacle-avoiding motion planning before
the kinodynamic planning approach.
Problem			Provably-Good Algorithm
Robot class			Minimize			Exact			e-approximate
2DOF point			path length			yes			yes
3DOF point			path length			yes			yes
2DOF point			motion time			no			no
3DOF point			motion time			no			no
d-link open-chain			motion time			no			no
any two points in O((log N)/e) time with an O(N2 tN log ]v/e)-time preprocessing
of the environment description. The fastest exact algorithm for planar shortest-
path planning is 0(N2), by Ghosh and Mount [GMS7], as noted earlier. Sharir
and Schorr [5584] gave an exact 2 algorithm for the 3D shortest-path problem,
and Reif and Storer [R585] later lowered the bound with a 2N0?1? algorithm. This
was subsequently bested by Canny, who found an o(AJo(N)) algorithm [Can88b].
The problem of finding some path between two positions in a polyhedral en-
vironment for d-link robots ("the Generalized Mover's Problem", or UMP) has
been studied for a number of years; see Lozano-Pe'rez and Wesley [LPW79], Reif
[Rei79], or Schwartz and Sharir [5583,5582]. Reif [Rei79] showed that OMP is
?S?AC?-hard in the degrees of freedom. Lozano-P6rez [LPS3] pioneered the use
of configuration space, which has been subsequently used by many researchers.
(See [Lat9t] for a review.) Canny [CanS8a] gave an algorithm that is singly ex-
ponential in the degrees of freedom and polynomial in the number of kinematic
constraints; this is currently the lowest complexity algorithm for C;MP.
1.4.2.2 Kinodynarnic Planning
6,Diinlaing [687] provided an exact algorithm for one-dimensional pursuit game,
a kind of kinodynamic planning. His methods may extend to the 2- and 3-
dimensional cases as well. Ninodynamic planning in 2D is related to the problem
of planning minimum-path motions with non-holonomic constraints, as studied by
Fortune and Wilfong [FW88,Wil88], who give exact algorithms, and Jacobs and
Canny [JC89] who describe an approximation algorithm. In the general problem,
34
a robot with wheels and a bounded minimum turning radius must be moved. To
make the analogy clear, in our case, the minimum turning radius is i'ffv2
a
Early results from this thesis come from research done in cooperation with
Canny and Reif [CDRX88]. Specifically, we introduced the algorithmic approach
to kinodynamic planning, by formalizing a simple problem and providing the first
provably good approximation algorithm for 2- and 3-dimensional optimal kinody-
namic planning.
Research in kinodynamic planning proceeded on the foundations laid out in
[CDRX88]. Certain techniques presented here (and described in [DX90b,DX89a,
DX90a,DX91]) to handle state-dependent dynamics are similar to those used by
Jacobs, et al [JllCP89,HJCP90], who gave the first polynomial-time approxima-
tion algorithm for optimal kinodynamic planning for robots with open-chain dy-
namics. Their work introduces several techniques to the kinodynamic planning
literature, including (a) discretizing acceleration-space according to the problem
parameters and (b) reducing state-dependent dynamics to being locally constant.
Our techniques result in lower complexity bounds for our algorithms. In concurrent
independent work, Reif and Tate [RT9o] used a parameter-dependent acceleration-
space discretization implicitly to obtain a polynomial-time approximation algo-
rithm for robots with decoupled dynamics, L2 dynamics bounds, and polyhedral
c-space obstacles.25 Canny, Reif, and Rege [CRR90] use the first-order theory of
real closed fields26 to obtain an exact ?ST'ACS algorithm for a planar optimal
kinodynamic planning problem. At present, this is the only known algorithm for
an exact optimal kinodynamic planning problem in more than one dimension.
1.4.3 Other Related Work
A relatively young research area examines problems from control theory using
techniques from computational complexity theory; this has yielded results in both
fields. For example, see the work of Papadimitriou and Tsitsiklis [PT86]. Recently,
using the kinodynamic framework, Reif and Tate [RT9ij defined a generalization
of alternation (as in alternating Turing machines [CNS8l]) to continuous domains.
They consider a class of pursuit games in which a robot must not only obey kino-
dynamic constraints, but evade a moving adversary. For one such game in three
dimensions, they show that the decision problem is ?X?TiM?-hard, the first
25A configuration-space representation of obstacles.
26See, e.g., [Yap86]
35
provable intractability result27 for a robot motion planning problem with com-
plete information. A notion of e-approximation algorithms for these games is also
defined28, and approximation results are presented for robots that obey L? and
L2 dynamics bounds.
Robot motion planning is often a more strongly "online" problem than an e-
approximation formulation effectively characterizes. In particular, the time spent
on a planning problem can affect the usefulness of a solution or even the overall
utility of the robot, especially when computation time is a shared resource. To
address such issues, Dean and Boddy [DB88,BDS9] have proposed an expectation-
driven refinement framework for time-dependent planning, with an eye towards
applying decision theory (e.g., Horvitz, et al. [HBH8Sj) to robot planning. Their
framework depends on the existence of anytime algon;Thms for individual problems.
An anytime algorithm has two important characteristics:
1. it can return some solution whenever it is asked and
2.			the quality of its solution improves ?`nicely"			with time.
This definition does not require that the solution quality converge to the optimal as
the running time goes to infinity, so anytime algorithms generally cannot be used
to find e-approximations. At the same time, an e-approximation algorithm will not
necessarily be "convertible" to an anytime algorithm, except in the sense that the
original algorithm could be run over and over with progressively smaller choices
of e. While this thesis does not consider anytime algorithms for kinodynamic
planning, such algorithms (and an appropriate problem formulation) would be
valuable. Finally, although e-approximation algorithms and anytime algorithms are
intended for different uses, algorithms that qualify as both would be an interesting
research area.
1.5 Contributions and Thesis Outline
1.5.1 Algorithms
This thesis presents a series of provably-good polynomial time e-approximation
algorithms for optimal kinodynamic planning. The particular algorithms can be
27Here, we mean that the intractability is independent of whether ? #
28The measure of optimality concerns an evasion margin or a deadline time.
36
applied to robots characterized by three classes of dynamics laws and bounds: de-
coupled dynamics under L?-norm (i. e., independent) bounds, decoupled dynamics
under L2-norm bounds, and fully-controllable open-chain manipulator dynamics.29
Table 1.5.1 presents the asymptotic complexities of the algorithms in terms of the
geometric complexity of the problem N, the number of degrees of freedom of the
robot d, and the approximation parameter e > 0.
The algorithm from Chapter 2 has the distinction of being the first provably-
good approximation algorithm for planning time-optimal robot trajectories. The
two algorithms from Chapter 3, also for robots with decoupled dynamics and dyna-
mics bounds, have the lowest known complexity for their classes. The (non-bang-
bang) algorithms for robots with open-chain kinematics and dynamics, found in
Chapter 4, also have the lowest known complexity for approximation algorithms
in their class. The last algorithm listed (from Chapter 5) is the first provably-
good approximation algorithm for planning time-optimal open-chain manipulator
trajectories that generates bang-bang trajectories.30
1.5.2 Mathematical Contributions
The work in this thesis introduces several new concepts and techniques to the field
of algorithmic motion planning. We briefly list highlights here. They are described
in context in Section 1.5.3.
Problem Formulation. A constrained optimal trajectory planning problem is
formulated to match the requirements of a time-optimal planned trajectory.
In Appendix A it is shown that finding an exact optimal solution in 3D is
)V'?-hard. An e-approximation version of this problem is formulated with the
intention of allowing provably-good algorithms. A speed-dependent safety
margin is included to account for errors in sensing, control, and modeling,
and to ensure that the approximation problem is mathematically reasonable.
See Chapters 2 and 4.
29Dynamics laws are d?coupled if the map from forces to accelerations is constant and linear.
(See figure 1.4.) Dynamic bounds are independeni if there is a constant linear map to a coordinate
system where the choice of acceleration and velocity along each single axis never affects the choices
along the others. A fully-conirollable manipulator is actuated at each joint and is able to overcome
gravity at every possible pose.
30A trajectory and its controls are bang-bang if each control is fully-saturated (at a `?corner?' of
the force bounds --H see figure 1.4) at all but a finite number of points during the motion.
37
Table 1.2: Provably-good approximation algorithms for optimal kinodynamic plan-
fling in this thesis. Accuracy and asymptotic bounds shown. "Decpl" is an abbre-
viation for "decoupled". cA,...,cF are constants that depend on the robot and the
algorithm, but that are independent of N and ?.
Ch			Robot Dynamics &			Time			Safety			Algorithm Complexity
Dynamics Bounds
2			decpl, d < 3			(1 + ?)Topt			(1			--H ?)?v			O(cAdN(ci)6d)
L?-norm
3			decpl, L?-norm			(t + ?)Topt			(1			--H ?)&			o(dcd?N(1?)3d)
3			decpl, L?-norm			Topt			(1			--H ?)?v			o(dcdcN(lc)3d)
4			decpl, L2-norm			(1 + ?)Topt			(1			--H ?)?v			o(dcd?(1?)6d?1)
4			open-chain			(1 + ?)Topt			(1			--H ?)?			o(cd?N(d + log N)(?c1)6d?1)
5			open-chain			(1 + ?)Topt			(1			--H ?)?			o(cd?N(d + log N)(1()7d)
(bang-bang)
Scaling-Tracking Methods. All our proofs of algorithm accuracy and complex-
ity follow a canonical structure: we show, if an optimal kinodynamic tra-
jectory trajectory exists, how to choose parameters that ensure that some
graph trajectory will track a "copy" of this trajectory that has been slowed
down by a factor of ? closely enough to meet the conditions for ?-optimality.
(For robots under decoupled laws and bounds, the optimal trajectory can be
tracked.) Introduced in Chapter 2.
Tracking Lemi?as. A series of tracking lemmas relate a tracking tolerance and
dynamics constraints to control discretizations, root vertex choice, and time-
step that guarantee an arbitrary trajectory will be tracked to the desired
trajectory by a graph trajectory. The proof techniques for some of these
lemmas are quite difficult. Introduced in Chapter 2 and found in all other
technical chapters.
Constructive `I?ajectory Proofs. The proofs used to obtain the lowest known
complexity bounds for kinodynamic planning for robots with decoupled dy-
namics and L?-dynamics bounds use inductive definition. Appears in proofs
in Chapter 3 and following chapters.
38
Adversary Game Proofs. These are first used to prove tracking lemmas for
robots with coupled dynamics bounds. In a basic version, rules in simul-
taneously-played games enforce the condition of extremal control. Next, a
slightly harder game allows us to prune the search of an exponentially-sized
reachability graph with a spatial hashing technique, while preserving the
validity of the search. Appears first in Chapter 4.
Time-Safety-Planning Trade-offs. In Chapter 5 we re-formulate the approxi-
mation problem, splitting c into (a) an ?T that describes how close to time-
optimal a kinodynamic trajectory is and (b) an ?? that describes how well it
observes the safety margin.
We note that proofs somewhat similar in flavor to the constructive trajectory
proofs and adversary game proofs are found in recursion theory and game theory.
1.5.3 Thesis Outline
1.5.3.1 Basic Kinodynamic Planning
Chapter 2 presents the first probably-good approximation results for optimal kino-
dynamic planning in more than one dimension. The restricted situation of a point
mass robot amongst polyhedral obstacles in two or three dimensions is considered,
and L?-norm velocity and acceleration bounds are used. The optimal kinodyna-
mic planning problem and the approximation problem are formalized for this class
of robot and obstacles, and the approximation algorithm is described, The proof
of the algorithm's correctness and of an o(cdN(??1)6d) time-complexity is given.
The general structure of this proof sets the pattern for most provably-good
approximation algorithms for kinodynamic planning.
o+ First, a theorem shows how closely, for a given safety-margin ? and approx-
imation parameter ?, a trajectory needs to "track" a &-safe trajectory to
guarantee that it will be (1 --H ?)-safe itself.
o+ Second, it is verified that if a trajectory obeys the given dynamics bounds,
then slowing it down uniformly by a factor of ? results in a trajectory that
obeys dynamics bounds that are smaller in magnitude by a fl(?) factor.
o+ Third, a tracking lemma shows how to choose the reachability graph parame-
ters to so that in the absence of obstacles they will generate a trajectory that
39
mimics this &slowed, or e-time rescaled trajectory, to a desired tolerance. In
particular, the size of the timestep shrinks polynomially with the closeness
of the approximation.
For the simple problem discussed in Chapter 2, a sufficient set of primitive
controls is the set of constant accelerations whose components are members of the
set f--H?a,O,?aJ. Using the steps together we show that a sufficiently small timestep
is ?(e2). We also show that safety-checking can be done in time 0(N) for each
graph edge.
1.5.3.2 Better Techniques For Better Bounds
In Chapter 3 we refine the both the complexity and accuracy results of the previous
chapter. The results directly generalize to robots obeying decoupled dynamics and
decoupled dynamics bounds.
First, we use constructive proof techniques to improve the earlier tracking
lemma, showing that the size of the timestep will shrink iinearly with how closely
we require a graph trajectory to track an e-time rescaled trajectory. This results
in lowering the complexity bound of the algorithm to O(cdN(??1)3d)
Second, we show that for robots obeying L?-norm velocity and acceleration
bounds the following is true: any ?-safe trajectory obeying the original dynamics
bounds is tracked closely enough by some trajectory generated by the primitive
controls above and an Q(e) timestep, so that the tracking trajectory is (1 --H
safe and meets the endpoint approximation conditions. Furthermore, the tracking
trajectory will take the same amount of time as the original. This means that for
robots obeying decoupled dynamics bounds, there is an approximation algorithm
that generates kinodynamic trajectories that are not only e-optirnal, but that take
the same amount of time as the optimal kinodynamic trajectories.
These algorithms have been implemented for point mass robots in two dimen-
sions. We briefly describe an implementation and empirical results. Ours is the
first implementation of a provably-good approximation algorithm for planning time-
optimal motions in more than one dimension.
1.5.3.3 Kinodynamic Planning For Robots With Coupled Dynamics
Bounds
Chapter 4 describes provably-good approximation algoritlirris for robots obeying
coupled dynamics bounds, specifically for a class of robot systems that includes
40
(a)			d-link open-chain manipulators with revolute and prismatic joints as well as
(b)			Cartesian robots obeying L2-norm dynamics bounds. The algorithms find two
types of trajectories: ones that obey piecewise-constant extremal controls, and
ones that obey piecewise-constant near-extremal accelerations.
The keys to the correctness of these algorithms are two Cottpled Tracking Lem-
mas (4.2.1 and 4.2.2). A tracking lemma for robots obeying L? dynamics bounds
obviously follows from a lemma for one dimension; however, for robots obeying
coupled dynamics bounds the choice of one component of an acceleration (or ve-
locity) vector can restrict the choices of the other components. Analyses of two
games against an adversary are used to reduce the proofs to cases in one-dimension.
Generating an extremal or near-extremal trajectory that tracks an e-time-rescaled
trajectory in ddimensions31 directly corresponds to winning dsimultaneous games
against the adversary.
While it might appear that the state-dependence of the open-kinematic chain
dynamics equation (4.1) would make proving the correctness of the algorithm dif-
ficult, the appropriate tracking lemmas limit the difficulty to detail. With the ap-
propriate tracking lemmas, open-kinematic chain dynamics becomes conceptually
simple because perturbations of size Ap in configuration and Av in velocity perturb
the boundary of the set of viable accelerations by a magnitude of O(Ap + Av).
1.5.3.4 Purther Kinodynamic Planning
Chapter 5 contains two results in different directions of kinodynamic planning
research.
By extending the techniques in Chapter 4, we show the existence of a provably
good approximation algorithm for optimal kinodynamic planning that generates
trajectories that obey bang-bangcontrol. In proving the correctness and complexity
of the algorithm, we show that if an optimal kinodyanmic trajectory exists, then
there is an e-optimal bang-bang trajectory with o(e) time between switchings.
Finally, while we formulated the optimal kinodynamic planning problem to
obtain an analytic trade-off relationship between the closeness to time-optimality
of the motions found and the time used to plan them, we propose a practically-
motivated extension of the formulation. We believe that algorithms that allow one
to trade-off among time-optimality, safety, and running time would be desirable.
Towards this goal, instead of using a single parameter e to describe approxima-
31dis the dimension of the configura?ion space i.e. degrees of freedom of the robot
41
tion goodness, we introduce two parameters ET and ?s that (respectively) describe
closeness to optimality in execution time and in observance of the safety margin.
In addition to asymptotic results, we derive equicomplexity curves to show how our
algorithms allow one to trade-off between time and safety.
1.5.4 Publication Note
Portions of the this material have been presented, or announced in abbreviated
form, at conferences [CDRX88,DX89b,DX89a,DX90a,DX9l] Chapter 2 represents
joint work with John Canny and John Reif.
Chapter 2
Cartesian Kinodynamic Planning
Given a robot system, we wish to find a minimal?time motion from a start state
(position and velocity) to a goal state, while avoiding obstacles and respecting
dynamic constraints on velocity and acceleration. Finding an exact solution for
this problem is generally A(?-hard (in more than two dimensions), even for simple
geometries and dynamics. Therefore, we turn to approximation algorithms to plan
these motions. Ideally, we would like to find an algorithm for which one could
guarantee the solution accuracy to some tolerance ?. While one might expect such
an algorithm to run longer when a more accurate solution was required, a probably-
good polynomial-time approrimalion algorithin would have a running-time bound
polynomially dependent on
Because the problem is hard, we start with a simplified case. In this chapter, we
consider the restricted situation of particle dynamics in two and three dimensions.
This will also serve as a simple example of the kinodynamic planning framework.
First, we define an appropriate optimal kinodynamic planning problem for two-
and three-dimensional problems with polyhedral obstacles. This is the simplest
formulation of the Optimal Cartesian Kinodynamic Planning Problem.
We describe a simple but provably-good polynomial-time e-approximation al-
gorithm for this problem. A proof of the complexity bounds and accuracy of the
algorithm are presented. The high-level proof structure will be used and the prob-
lem formulation generalized in later chapters.
42
43
2.1 Kinodynamic Planning For A Simple
Robot
2.1.1 Problem Formulation
We consider the fotlowing restricted problem. (See figure 1.2.) A point mass ??
= 2,3) must be moved from a stateS = (s,s) to a goal state G = (g,g). In the
course of the motion, the point must avoid a set 0 of polyhedral obstacles, which,
with world boundaries, constitute the kinematic constraints of the problem.
We will denote the configuration space ?d by C, and its phase space by TC.
Phase space TC is the robot state space and is isomorphic to ?2d Thus, a point
in TC is a (position, velocity) pair such as 5 or G. By using a configuration
space [LP83] approach, this is readily extended to cover a rigid non-rotating robot
geometrically described by a union 1? of convex polyhedra.
Formally, a robot motion over a time interval [0, Tf] can be specified by a
twice-differentiable map p : [0, Tf] H C This map is the paU? of the motion. In
kinodynamic planning, the motion must obey dynamics and dynamics constraints,
and it is convenient to specify p explicitly. The trajectory of a robot motion taking
time Tf is the map F : [O,Tf] TC given by F(1) = (p(t),p(!)) We denote
the position and velocity components of a subscripted trajectory 1'r by Pr and
Pr, respectively, and its time by Tr. While a motion p can be given directly as a
function of time, two equivalent specifications are useful: (a) an initial position Po
and a velocity function v = p, and (b) an initial state (po, vo) and an acceleration
function a =
Movement is controlled by applying forces or commanding accelerations, which
are equivalent for a point mass. The motion must respect upper bounds on the
magnitudes of the acceleration and velocity. At all times t the acceleration p(1)
and the velocity p(t) must obey
IP(t) oo			and			(2.1)
i3(t)			? a.			(2.2)
Eqs. (2.1) and (2.2) are the dynamics bounds. The simple dynamics law f = ma
and the dynamics bounds comprise the dynamics constraints of the problem. We
call a robot obeying these simple constraints a Cartesian robot with L? dynamics
bounds, or a a Cartesian robot for short.
44
We assume that the obstacles 0 are represented by a set of convex, possibly
overlapping polyhedra. Suppose these convex polyhedra have a total of n faces
overall. We call n the combinatorial complexity of 0. Note that n is also the
number of bounding halfplanes of the obstacles. Free space is the complement of
these obstacles. We assume that the set of free configurations is bounded by a
d-cube of side length 1. A general kinodynamic planning problem, then, is a tuple
(0,S, G, 1, a, v).
An exact solution to the kinodynamic planning problem is a trajectory F such
that F(0) = S, F(Tf) = 0, and F obeys the kinodynamic constraints. That is,
the path p avoids all obstacles, the velocity p respects (2.1), and ? respects (2.2).
The time-optimal kinodynamic planning problem is to find a kinodynamic solution
that takes a globally minimal time, that is, the fastest trajectory in the set of all
kinodynamic solutions. A solution is represented as a suitable encoding of the start
state F(O) and the acceleration function a.
A theoretically time-optimal solution may require unrealizable precision in con-
trol or sensing and thus be unexecutable by a physical robot. For this reason, an
optimal solution should observe a safety margin; the margin we define is speed-
dependent. Furthermore, the safety margin ensures the existence of a family of
solutions "nearby" in time and in phase-space that "approximate" the optimal
safe solution. Safety margins and the existence of such a `?tube" of approximating
are not only practically motivated, but mathematically necessary for our approach.
A ?-safr kinodynamic solution avoids all obstacles by a safety margin &. In
this thesis, we define this safety margin to be an affine function of the trajectory
speed. Under a simple model, this corresponds to how accurately and quickly a
robot senses its position and velocity, combined with how quickly it can correct for
velocity errors. (See section 1.2.4.1.) Two positive scalars co and ci parameterize
this safety margin, which one can view as an obstacle-free tube centered about the
path, as in Figure 1.10.
Formally, a ?-safr kinodynamic solution has the property that for all times I
in [0, TfJ, there exists a ball about p(t) in free space of radius
?v(co,ci)(p(t)) = co +cillp(t)I .			(2.3)
We will drop the parameters co and ci in the discussion when confusion will not
arise. Note that 6?-safety is is an example of a kinodynamic constraint that is
neither a pure kinematic constraint nor a pure dynamics constraint. A 6v-safr
45
kinodynamic planning problem, then, is a tuple (0, S, G, a, v, 1, co, ci). We call a,
v, 1, co, and c1 the kinodynamic bounds.
For fixed co and ci, consider the class of all ?-safe kinodynamic solutions. We
define an optimal ?-safe kinodynamic solution to be a solution whose time is min-
imal in this class. We will henceforth employ the term optimal safe kinodynamic
solution or optimal kinodynamic soThtion since ?-safety is the only type of safety
we consider here, and we will only consider planning trajectories that respect a
safety margin. The problem of finding such a solution is called the optimal kinody-
namic planning problem. When the robot is a Cartesian robot (with L? dynamics
bounds), we call the problem the Optimal Cartesian Kinodynamic Planning Prob-
lem. Note that the definition of time-optimal found earlier does not include a safety
margin, while the definition of optimal here means time-optimal with respect to a
? safety margin.
We now specify what it means for a kinodynamic solution Fq to be e-optimal,
where a positive e < 1 parameterizes the closeness of the approximation. ?ecall
our notational convention: trajectory Fq = (pq, pq) and takes time Tq.
First of all, Fq must obey the safety margin
6v'(co,ci)(pq) = (1 --H e)?v(co,ci)(pq). (2.4)
Second, if an optimal safe trajectory takes time Topt, then we require that
Tq ? (1 t e)Topi.
Now, let us say that an approximating state (x', x?) is ?e-clos&' to a reference state
(x,x) if
Ix --H x'II			=			0(e),			and			(2.5)
x --H x'			=			0(e).			(2.6)
Our use of "e"-closeness will always occur in a context where the origin of the
constant omitted in the O() notation is obvious or is shown in the discussion, such
as in the analysis of an algorithm.
For the final criterion, we require that Fq(O) and Fq(Tq) be e-close to the desired
start and goal states S and G, respectively.
In order to obtain our result, we must assume four things: a velocity bound,
a workspace diameter bound, L?-norm acceleration, and safety. Each of these
assm?ptions can be motivated in physical terms. For example, robots exist in
46
the physical world, and of course any actual robot will have bounded maximum
velocity and a bounded workspace. However, the proofs in this thesis do not go
through if any of these assumptions is dropped. In later chapters, we relax the
L?-norm. Safety, as we shall see, proves to be a crucial assumption.
2.1.2 Statement of Results
In section 2.2.2 we describe a provably good approximation algorithm for the op-
timal safe kinodynamic planning problem. Concisely stated, we show:
Theorem 2.1.1 Let IC (O,S,G,?a,Thl,c0,c1) be an instance of the Optimal
Cartesian Kinodynamic Planning Problem in two or three dimensions. Let 0 <
e < 1. Let n be the combinatorial complexity of the obstacles 0.
Suppose there is a ?v(c0,c1)-safe trajectory that obeys the dynamics bounds a
and v and goes from S to G in time Topt Then the algorithm finds a %(co, c1 )-safe
trajectory that obeys the dynamics bounds, takes at most time (1 + e)Topt, and goes
from some S* to some G* such that S* is e-close to S and G* is e-close to G.
The running time of ll?e algorithm is
d=2,3, where
max ?a(?+1), --Ha(?+1),a
co			co			v
Thus, c? = in Table 1.5.1.
An Optimal Cartesian Kinodynamic Planning Problem has three complexity
components. The combinatorial complexity of K is the number n of bounding
halfplanes of the obstacles 0. The algebraic complexity of the geometry is the
number of bits necessary to encode the coordinates of the vertices of 0 and the
start and goal states. The complexity of the kinodynamic bounds is the number of
bits necessary to encode the kinodynamic bounds (?a, v, ci, co). In the language of
combinatorial optimization [PS], we show that our algorithm is an e-approximation
scheme that is fully polynomial in the combinatorial and algebraic complexity of
the geometry, and pseudo-polynomial in the kinodynamic bounds.
Note that we cannot claim that the approximately optimal safe solution is nec-
essarily near to a truly optimal safe solution in position. In this respect it is useful
47
to compare our result to Papadimitriou's fully polynomial approximation scheme
for 3D Enclidean shortest path [Pap85]. Specifically, neither method necessarily
finds a solution that is spatially close to the optimal path, but merely one that
has a length (time) that is not too much longer than the optimal length (time). In
fact, this must be so, because of the shortest path Af?-hardness result of [CR87].
These above results can be extended to a rigid, non-rotating robot whose geom-
etry is given by a union 7? of convex polyhedra. This configuration space transfor-
mation has been discussed extensively in the literature (see, e.9., [LP83,Don87]).
The algorithm of [LP83] could be used as a preprocess to reduce the planning prob-
lem for 7? amidst 0 to the point navigation problem we discuss. Since the dynamics
equations for such a robot are identical to those of a point robot, we only need to
map the problem to this configuration space and apply the algorithm. This sim-
ple problem is identical to that of a two- or three-degree-of-freedom (3DOF) robot
with constant, decoupled dynamics equations and decoupled dynamics bounds and
polyhedral configuration-space obstacles with complexity N.
2.2 Algorithm and Analysis
2.2.1 The General Idea
Our approach transforms the problem of finding an approximately minimal-time
trajectory to finding the shortest path in a directed graph. The vertices of the
graph "discretize" the statespace TC, and the edges of the graph correspond to
trajectory segments that each take time T, a parameter computed by the algorithm.
Given the acceleration bounds a, let A be the set of constant accelerations
whose (vector) components are members of the set f--Ha, 0, a). Let us choose a
timestep T such that velocity bound v is a multiple of a?. Applying a member of
A for duration T is called an (?a,r)-bang. (See figure 2.2.1.) `A7e also use this term
to refer to the resulting trajectory segment: we say that there is an (a, r)-bang
from state X to state Y if following an (?a, r)-bang control at state X results in
state Y.
Suppose S? (s*,s*) E TC such that ?* is a vector of multiples of aT. Suppose
that (x, x) is a state reachable from 5* by some sequence of (a, r)-bangs. Then for
each coordinate z,
pi			s*i + A%??a?2 and			(2.7)
pi			s:* + n7aT
48
v'
x
`a
??ar
?I			X
`?ar
v'
Figure 2.1: (a, r)-bangs for a planar robot. At left, accelerations whose compo-
nents are members of f--Ha, 0, a? generate (?a, r)-bangs. At right: velocity changes
corresponding to (a, r)-bangs.
for some integers m? and flj. Thus, all states reachable from S* under a sequence
of (?a, r)-bangs belong to a set of states that lie at the interstices of an underlying,
aT
regular grid embedded in TC. This grid has spacings of in position and 2 in
velocity, as shown in figure 1.14. We call this set of intersticial states the TC-gn'd,
and each of these states a TC-gridpoint. We call a trajectory that results from a
sequence of (a, r)-bangs between TC-gridpoints an (a, r)-grid-bang trajectory. Its
velocity function is called a grid-bang velocity function.
We say that state (x, *) obeys ?-safety if the ball of radius ?(IIxII) about x
lies in free space. (Recall equation (2.4).) S*, v, ?, ?, and (?a, r)-bangs determine
a reachability graph ?(V, ?) embedded in TC. The vertices v, C V are the TC-
The edges ej E g are1 ?-safe (a, r)-bangs between pairs of these
We say that r, S*, a, v, Co, ci, 0, and e induce the reachability graph
We call S* the root vertex state, or root vertex of the reachability graph
gridpoints.
vertices.
?(V, C').
?(V, ?).
The smaller r is,
grid-bang trajectory
obeys the kinodynamic constraints.
the finer the underlying TC-grid, and the better some (?a, r)-
will approximate an arbitrary trajectory that starts at S* and
If r is small enough and Vopt is an optimal
1Strictly speaking, they are identified with the trajectory segments, but we use the correspon-
dence strongly and so often that not to say "are" would be cumbersome
49
kinodynamic trajectory from a state S, adequately approximated by S*, to a state
G, then it is intuitively plausible that there will be an (?a, r)-grid-bang trajectory
Fq going from S* to a vertex state G* approximating G in time
Tq < (1 + e)T0pt.
It would follow that this would reduce the problem of finding an e-optimal trajec-
tory to the shortest path in ?(V, ?) from S* to a vertex approximating G, since
each (?a, ?)-bang takes time T. This is the basic idea behind the algorithm presented
in this chapter.
2.2.2 The Algorithm
To explain the algonthm, and later, to prove its correctuess and complexity, we
need two more definitions. These definitions will be used throughout this thesis.
First, we introduce terminology to describe how closely one state approximates
another.
Definition 2.2.1 Let X = (x,k) andY (y,Y) be two states, and let n and n?
be non-negative scalars. Suppose that
and
Ix--HYIoo ? 4v
Then we say that X is within (71, 71v) of Y, or that X approximates Y to tolerance
Second, we generalize this definition of approximation to trajectories, formalizing
the notion of how closely one trajectory might "track" another.
Definition 2.2.2 Consider two trajectories Fa, F6 : [0, T] TC Given two 71071-
negative scalars `1 and 71v, `we say that Fa approximately tracks F6 to tolerance
(?Jz,71v) in the L?-norm if for all times t,
IIpa(1) P6(t)IIp			<			71, and
IIpa(1) --H Pb(t)IIp			?			liv
We will use the L? norm except where otherwise noted.
what this might look like in the phase-space.
(2.8)
Figure 2.2 illustrates
50
Pq
x
x
Figure 2.2: An (?Ix, 71v)-tube projected into phase-space for one dimension. Trajec-
tory Fq approximately tracks F'r to tolerance (71', iiv)'
51
We now give a high-level description of a provably-good polynomial approxi-
mation algorithm for optimal Basic Kinodynamic Planning. A method for check-
ing the safety of (a, r)-bangs is given in section 2.2.5. Given a problem instance
(0, S, G,a,v, 1, co, ci) and an approximation parameter ?, the algorithm does the
following:
Algorithm 1
1. Chooses a timestep T as a function of a, v, ?, Co, and cl. Specifically, the
algorithm chooses the largest r such that ? ? ?ffijV? ar divides v, and
2cot			co?
13 min ?a(ci t 1)'a(? +1)
2. Next, the algorithm chooses the starting TC-gridpoint S* according to the
following: s* = s, and s* is the multiple of aT closest to
3. It then searches for the shortest path in the induced embedded graph ?(V, E),
described above, from S* to any state (vertex) that is within (af , <47) of
(g, ?). The algorithm explores the graph using breadth-first search, check-
ing the ?-safety of each (ti, T)-grid-bang it considers.
To show the correctness and complexity of the algorithm we show how to choose
T 50 that the following holds: if there exists a ?-safe trajectory from S to G taking
time T, then there also exists a ?-safe (?a, r)-grid-bang trajectory between states
&dose to S and G that takes time (1 + ?)T.
We first observe that if trajectory Fopt obeys dynamics bounds a and v, then
there is a time-rescaled [11ol83] trajectory F'0p1 that takes time (1+ ?)Topi and that
obeys dynamics bounds an(I (l$Vc) We then choose i/ and 71v that guarantee
that if Fq tracks 1"opt to tolerance (i??, ?v), then it will be ??`-safe. We then show
there is a T proportional to ?2 such that there exists an (a, r)-grid-bang trajectory
Fq that (a) approximately tracks `opt to this tolerance, and (b) is within (?af2, <47)
of F'0p1 when t = 0 and when I = (1 + ?)Topt. The latter implies ?-closeness.
Finally, we show that ?-safety-checking is 0(n) per (?a, r)-bang. R?ecalling that
TC-gridpoints have the form (2.7), we find that the number of vertices in the
induced statespace graph is 0 (?L73)d. The definition of (ti,?)-bang implies that
the maximal out-degree in ? is 3d, but since d < 3 we get the complexity bound
in Theorem 2.1.1
52
2.2.3 Time-Rescaling and Safe Tracking
We say that a path p is traversed by a trajectory F if the image of the position
component of F is equal to the image of p.
Lemma 2.2.1 If path p is traversed in time Tr by a trajectory Fr under acceler-
ation bound a and velocity bound v, then there exists some ??? that traverses p in
time Tr(1 + e) while obeying acceleration bound and velocity bound In
particular, this is true of rr = (P'r,P'r), `where
P'r(t)			=			Pr			99
P'r(t)			=			i+1?Pr (i+tc)
Follows from the results of [11ol83] or from direct computation using
Proof:
(2.9). [3
To prove the main theorem we need to note that (2.9) preserves 6?-safety:
Observation 2.2.2 Suppose Fr is a 6v(co, ci )-safe trajectory from S to G that
takes time Tr and obeys bounds v and a. Then `r as defined by (2.9) is ?v(co, ci)-
safe, obeys bounds i+V( and ?%??, and goes from S' = (s, i+S'c) to G' =
Note that if it is known that e is less than some constant C independent of problem
instance, then S' and G' are e-close to S and G.
Intuitively, we expect that if a trajectory Fq tracks F'r as defined in (2.9) closely
enough, then Fq will be (1 --H e)&?-safe. We have the ibllowing lemma, which is
independent of norm:
Lemma 2.2.3 (The Safe Tracking Lemma) Suppose that ?, is specified by Co
and ci and that Fr is a ?-safr trajectory. Let 0 ? e < 1, and let &` = (1 --H
Then there exists a tolerance (7ix, ?1v) such that for any trajectory Uq, the following
hold:
1. If Fq tracks 1'r to tolerance (?x Tiv) then Fq is ?`? -safe.
2. Furthermore, for any positive ?, the following choices suffice:
7/v			?			ci(i&0?)+P
Tir			?			P7/v
(2.10)
53
Proof: We find positive real numbers ? and 71v sud? that if Fq tracks Fr to
tolerance (ijz,?v), then the 6I?-tube induced by Fq lies entirely inside the ?-tube
induced by Fr. If Fr is ?-safe, this assures that Fq is ?`-safe. Henceforth, let
c0, = (1 --H e)co and C?1 = (1 --H e)ci
Suppose that x E C lies inside the ?-tube induced by Fq. Then for some
Ei [O,Trj,
x --H pq(tx)? ? % + e'i pq(tx)ii. (2.11)
Let B?(x) denote the bail of radius ? about x. If Fq tracks l'r to tolerance
(?z,?jv), then pq(tz) E B?(pr(tx)) and pq(lz) E B?(pr(tr)), Therefore
lix			--H pr(1x)			?			lix --H pq(tx)11 + ??, and			(2.12)
iipq(t)ii			<			iipr(t)ii + liv
By substituting into (2.11) and adding li? to the right-hand side,
lx			--H Pr (tx)ll ? Co' + lix + C'1 (llPr (tx)il + `N)			(2.13)
Now, suppose that (3 > 0 and that lix and liv satisfy (2.10). It follows that
lix + (1 --H e)Ciliv ? ceo,
and thus
But then,
Co + lix + C'iliv ? Co.			(2.14)
Co' + lix + c'i (I pr(tx)ll + li?) ? Co + Cl llPr(tx)ll			(2.15)
This implies that llx--Hpr(tx)ll ? co+C111pr(tx)ll via (2.13). Therefore, x lies inside
the ?-tube induced by Fr.
Since x is an arbitrary point inside the ?-tube induced by I'q? it follows that
the ?-tube induced by Vq lies entirely inside the 6v-tube induced by Fr. L]
For given 6v-safety parameters Co and Ci and an approximation parameter e,
we call the tolerances (lix, liv) that obey (2.10) e-safr-t??ekiny lolerances.
Because this lemma holds for any L?-norm, our results are easily extendible to
safety margins given in any such norm. As a direct corollary, we have
Corollary 2.2.4 Let &v be spee?fled by Co and Ci, and let 0 ? e ? 1. Let the
tracking tolerance (lix, liv) satisfy the condition
liv			?			and			(2.16)
lix			<			Cp(
--H			ci+i
54
Suppose that Fr is a ?? safe trajectory, and 1"r is obtained by e-time-rescahng Fr
(as in (?9)). Suppose that Fq tracks 1"r to tolerance (?x,7iv) Then the ??`-tube
induced by Fq lies within the ??-tube induced by Fr.
2.2.4 The Simple Tracking Lemma
The Simple Tracking Lemma relates a timestep size T to a tracking tolerance. In
particular, it tells how to choose T to assure that in the absence of obstacles, for
every F? that obeys dynamics bounds and there will exist an (?a, T)-grid-
bang trajectory Fq that tracks Fu to tolerance (7ir, 7?v)
2.2.4.1 Knotpoints
We first show that if v? obeys dynamics bounds and i$Vc? then at "knot-
points" separated in time by an adequately large multiple of r, some (T, ?)-grid-
bang trajectory Fq will be within (a9r , aT) of f'?. That is, for a large enough integer
N, Fq(kNT) will approximate Fu(kNr) to tolerance (af2 , ffi)T) for all relevant k.
Lemma 2.2.5 Let e > 0, and let Fa be a trajectory respecting dynamics bounds
and Let T < %v, and suppose that ar divides v
Suppose that
N > 6			(2.17)
e
Then if (pqo,pqo) is a TC-gridpoint such that I1PqO --H PU(O) I ? -a972 and 1IPqO --H
pu(0)II < mar there exists an (a, r)-grid-bang trajectory Fq such that Fq(O)
2'
(pqo,pqo) and that Fq(Nr) is within (af,%j? of Fn(Nr).
Proof:
Since we are using the L?-norm, it is sufficient to consider the case of a one-
dimensional configuration space C. We show that the lower bound on N given
by (2.17) is sufficiently large to guarantee that if F? meets the hypotheses of the
lemma then some (?a, r)-grid-bang trajectory can meet the endpoint conditions.
Let e > 0 r < ?QJv, and arlv. Let P?L(O), p?(O), and iiu(NT) be fixed, and
consider some F? that satisfies these endpoint conditions and the hypotheses of
the lemma. Let (pqo,pqo) be a TC-gridpoint within (?f?2, L%r) of Fa(O). To find a
sufficiently large N, we introduce variables b and Q. These variables depend on
Fa. Let b be an integer such that such that
11)qO t bar --H pn(Nr)I ?
--H9
55
Now, define Q to be the collection of all (a, r)-grid-bang trajectories of time
length Nr starting at (pqo,pqo) with net velocity change bar. The positions reached
by the different trajectories in Q at time Nr form a set of discrete points spaced ar2
apart. Call these positions the Q-positions. If the range of Q-positions spans the
range of possible F?(Nr), then for some trajectory Fq E Q, Ipq(Nr) --H pu(NT)I <
--H2
ar
2
We show how to choose N so that the maximum Q-position exceeds the max-
imum possible pu(Nr). A similar argument shows that the same N guarantees
that the minimum Q-position is less than the minimum possible pn(Nr). For the
remainder of this proof, we will refer to the two trajectories that achieve the max-
imum Q-position and the maximum possible pu(NT) as Fq and Fn, respectively.
The Fq that attains the maximum Q-position obeys either (a) full positive
acceleration, possibly follo'ved by zero acceleration for one timestep, followed by
full negative acceleration2, or (b) full positive acceleration until its velocity is v,
followed by zero acceleration, followed by ftill negative acceleration. Similarly, a
Fu that maximizes pu(Nr) obeys either (c) full positive acceleration followed by
full negative acceleration, or (d) full positive acceleration until its velocity is
followed by zero acceleration, followed by full negative acceleration.
Consider f)q and Pu and their role in determining pq(Nr) and pn(Nr). In the
worst case, PqO = Pn(O) --H ffi? and pq(Nr) = pu(Nr) --H L%J. If pq(ATr) is to be greater
than pn(Nr), then we can divide the interval [0, Nr] into three intervals during
which Fq "loses ground to" Fu, "gains on" Ftt, and "loses ground to" Fu. (See
figure 2.3.) In other words, there are times tc and tt, 0 ? ? < Nr, such that
pq(t)			Au(t)			if 0 ? t < tc;
pq(1)			i;u(t)			if Ic < I < I?; and
pq(I)			p?(I)			if Ii < I < Nr.
Now, when Fq is accelerating full-positive,
pq(1) --H j?u(!) > a 1 --H (1 t)2 > ya.
Similarly, when Fq is accelerating full-negative,
pq(I)?pu(I) ?
2The zero acceleration tin?estep in the first case occurs if N P?(N4?)?P?0 is odd.
56
llence, whenO ? I < 1c,p%(1)?P'u(1) > --H%f, and when Ij ? I ? NT, i;q(l) Pu(I) <
?(??. Then, because PqO > i)t(O) --H ffi? and p'q(Nr) > pu(Nr) --H Tha?r,
fotC(pu(I) --H 1)q(t))dl < --Ha?7, and
ftNir(pu(I) --H I)q(t))dl ?
Therefore, it is sufficient to choose an N that guarantees
ti.
--H i)n(t))dl > ar
ar
2
v
I I
I I
I I
I I
I I
I			I
Ic
Ic			ii
pn
i)q
0
(2.18)
Figure 2.3: Bounding position d?anges subject to velocity knotpoints. An example
of p? and i)q that achieve the maximum position subject to conditions at times 0
and Nr. In this case, Pq never reaches the maximum allowed velocity v. N must
be large enough so that the distance Fq gains over Fu during Ic and It makes up
for the distance Fq loses to Fu during [0, Ic] and [It, Nr].
Consider the behavior of pq(t)--Hpu(t) between times 1c and It. li'or now, suppose
that pq(I) < `v during this time. Then, for an interval of time I? beginning with
Ic, both i)q(t) and pq(I) --H pn(I) increase; for an interval it beginning at most
57
one timestep after Ic and ending with It, both pq(t) and pq(t) --H pu(t) decrease.
Furthermore,
pq(t) --H pu(t)			>			ka?C(t --H Ic)			during Ic, and
pq(1) --H pu(1)			>			D;(ti --H t)			during Ii.
Using some manipulation, we then see that if pq(t) ? v for all I Ei [tc, It], the
condition
ti?Ic> 2T +1			(2.19)
e
guarantees that (2.18) is true.
Now, suppose pq(t) = v for some interval Im C [tc, It]. (See figure 2.4.) Then,
for an interval of time Ic immediately preceding Irn and beginning with ic, both
Pq(1) and pq(1) --H pu(t) increase; for an interval I? immediately after Im and ending
with t?, both pq(t) and pq(t) pn(t) decrease. However, during Im, Pq(1) 15n(t) >
ar, since r < ?f?. It follows that (2.19) again guarantees that (2.18) is true.
We observe that
Ic ? r and
NT--HIt			?
Recalling (2.19), we see that the following choice of AT guarantees that the range
of Q-positions will be adequate:
N =4 [?e1l t 1
Using the fact that 0 < e ? 1, we obtain the sufficient condition (2.17). E
2.2.4.2 Proving The Simple Tracking Lernrna
By bounding the maximum tracking error between knotpoints, we can establish
the following.
Len?rna 2.2.6 (The Simple Tracking Len?n?a) Let e > 0. Let Fu be a trajec-
tory that respects dynamics bounds and i$Vr and takes time Tu. Let 71x and
? be positive. Let r ? ?ffijV
Suppose that
r? $?min( 2??ix,%v),			(2.20)
andariv. Furthermore, let N be given by (2.17), and suppose that T? > Nr Then
in the absence of obstacles there e.xKsL? an (a, ?)-grid-bang t??ectory Vq respecting
58
dynamics bounds v and a that approximately tracks Fu to tolerance (?1r, TIv) during
[0, T?] and obeys the following endpoint conditions:
pq(O)
IIpq(0) --H ptt(O)IIoo
JIpq(Tt) --H pa(Tn)IIoo
jIpq(Tu) --H pn(Tn)IIoo
pu(O),
ar
2'
?ar2
aT.
and			(2.21)
I			I			I			I			I
I			I			I			I			I
I			I			I			I			I
I			I			I			I			I
Vma			I
I			I
I			I
I			I
v
ar
2			I
I?III
I			I
tc			Ii
pn
0			I			NT
Figure 2.4: Bounding position changes subject to velocity knotpoints, another
case. Further towards proving Lemma 2.2.5. An example of Pit and pq that achieve
the maximum position subject to conditions at times 0 and NT. In this case, Pq
sustains the maximum allowed velocity v for the interval 1rn N must be large
enough so that the distance Fq gains over F? during 1c? 1m, and Jj makes up for
the distance Fq loses to F? during [0, Ic] and [11, Nr]. Note that in this figure, the
condition T cit is not met.
--H a
Proof:
We show that the the upper bound (2.20) is correct. Let the hypotheses of the
lemma be satisfied. Let N be given by (2.17). Then it follows from Lemma 2.2.5
that there is an (a, r)-grid-bang trajectory Fq such that for any positive integer k
59
satisfying kNT < Tu,
jlpq(kNr) --H p?(kNr)IIoc <--H
Ipq(kNT) --H pn(kNr)IIoo ?
and
2'
ar
-9.
(2.22)
This can be shown by induction on
Now, for all t, IIp?u(1) --H p'?q(t)11oo ? 2a. By considering the relative velocity in
the worst case, where along some axis
Ipq(kNT) --H pu(kN7)I
p)(kNT) --H Pu(kNT)I
Ipq((k + 1)N7) --H pu((k + 1)AT7)
Ip)((k + 1)NT) --H ptt((k + 1)N7)I
we conclude that for all t E [0, Tn],
--H2
ar
Th,
aT
2'
--H 2' and
aT
2'
11pq(t) --H pn(!)IIoo ? ?(i(N+1)272
2			(2.23)
To guarantee that the right-hand side of (2.23) is less than ??, it is sufficient
that
))			(N'+1)			(2.24)
Since 1pq(1) --H pu(t)IIoo ? a(N + 1)7 when 0 ? I ? T?, for the velocity case we
require that
7< WV (N1+1)
(2.25)
If Tn > Nr is not a i?ultiple of A?7, then for some natural numbers n < IV and
k, (kN + n)T is within of Tn. Substituting twice the value for IV from Lemma
2.2.5 and rounding to simplify yields the condition (2.20). E
2.2.5 Basic Safety Checking
We describe an 0(n) time to check whether an (a, ?)-bang violates the speed-
dependent safety-margin ?(co, ci). ?Ve review some basic computational geometry,
describe the special case when ci 0, and then extend the method to the gen-
eral case. (See [LP83] for some background on the basic computational-geometric
techniques we employ.)
As noted above, we assume that obstacles are the union of (possibly-overlap-
ping) convex polyhedra. For now, let the safety margin be a constant co > 0 (i.e.,
60
ci = 0), and define the Bc0 to be the L? ball with radius co. Staying c0-safe
relative to a convex polyhedron A is then equivalent to avoiding A = A ? Bc0,
where "e" denotes the Minkowski sum. Since Bc0 is a d-cube, A is also a convex
polyhedron and has O(Ifaces(A)I) faces, By taking the Minkowski sum of each of
the obstacles with Bc0 we obtain the expanded obstacles.
Suppose A has faces ? ...... ,Fm) lying on the boundary planes of the closed
half-spaces (H0,. . ,Hm). The boundary plane of each Hi is the kernel of an
affine function f?. If flj is a unit vector in the outward normal direction from the
boundary plane of Hi, and yj is any point on this boundary then
fj(x) = (n?, x) --H (n?, yi).
(2.26)
The polyhedron A is thus described by a set of functions ,$"` = ? ..... `, frn)'
A point x is on the boundary of A if and only if it lies on some closed face Fk of
A. Equivalently, fk(x) = 0, and for all fj that determine an edge of Fk, fj(x) ? 0.
Since for a convex polyhedron the numbers of edges and faces are Iinearly related
and an edge is common to two faces, determining whether x lies on the boundary
of any of the expanded of obstacles takes total time 0(n).
Without loss of generality, suppose that (ti, ?)-bang p begins at t = 0 and that
p(O) is c0-safe. We then can check the c0-safety of p(t) by determining whether
p(t) violates the boundary of an expanded obstacle. For a face Fk of A, we only
need to solve fk(p(1)) = 0, and for each solution t3 check whether fj(p(ts)) > 0
for some fi that determines an edge of Fk with fk
Now, consider speed-dependent safety function 6v p(t) is 6?-safe relative to
a convex polyhedron A if and only if it avoids the expanded obstacle A(p'(t)) =
A 0 B?v(P'(t)), where Bav(P'(t)) is the L? ball ?vith radius ?v(p'(1))' A(p'(t)) is
described similarly to A, by a set of ffinctions ? ....... , fm 1 For each fi E ?,
fj(p(t), p'(t)) = (n?, p(t)) --H (n?, yi $ q???p'(l)I ?), (2.27)
where q? is a constant vector that depends only on flj. To check whether p(t)
violates the faces of A, we use the fi ? fi the same way we use the fj E F above.
By the same argument as in checking whether a point is in on an expanded
obstacle boundary, we need to solve 0(n) equations and check 0(n) inequalities,
overall. It follows that the cost of safety-checking is 0(n) per (?a, r)-bang. Since
p(t) is quadratic, fk(p(1),p'(1)) = 0 has solutions of the fon? I = a + 7b. When
computing the inequalities we can square twice to eliminate the radical, and thus it
61
is adequate to compute square roots symbolically. This implies that safety checking
never requires numbers longer (in the number of bits) than a constant-multiple of
the length of the longest number in the input. Therefore, the complexity of safety-
checking is 0(n) under both the Real-RAM and bit-complexity models.
2.2.6 Proving the Main Theorem
We can now prove Theorem 2.1.1.
Proof of Theorem 2.1.1:
Let K = (0,5, G,a,?v, 1, co, ci) be an instance of the optimal safe kinodynamic
planning problem. Let 0 ? ? 1
Suppose Fopt is a 6),(co, cj )-safe trajectory that obeys the dynamics bounds a
and v and goes from 5 to G in time Topt. By Lemma 2.2.1 and Observation 2.2.2
that follows it, there is a trajectory Vo,pt that is &v(co, ci )-safe, obeys bounds
1+c
and M5c???takes time (1 + ?)Topt, and goes from 5' = (s, ?+S?) to G' =
Now suppose we run the algorithm described in Section 2.2,2. The choice of
T in the algorithm matches the conditions in Lemma 2.2.6 when the values of T]z
and 1?v from equation (2.16) in Corollary 2.2.4 are substituted into equation (2.20).
Furthermore, the algorithm's choice of root vertex state 5* obeys the condition on
(pqo, p'qo) in Lemma 2.2.5. Therefore, some (t, ")-grid-bang trajectory beginning
at 5* tracks F1opt closely enough to be ?v'(Co, Ci )-safe, to obey the dynamics bounds
a and v, and to take time (1 + ?) to reach a state G* within tolerance (af2 , --H%") of
Breadth-first search guarantees sud? a trajectory will be found if there is no
?(co,ci)-safe (--Ha,i')-grid-bang trajectory beginning at 5* that obeys the dynamics
bounds and comes adequately close to G' in less time. Thus, the algorithm will
find a trajectory meeting the conditions of the theorem.
To establish the time complexity of the algorithm. we now bound size of
the reachability graph ?. The number of vertices is bounded by the number
G?(?a,?,?v,1,d) of TC-gridpoints for a point robot with maximum (L?) speed
v and acceleration a in a d-dimensional free-space of diameter 1. Without loss off
generality, let us choose ?* to be the "zero" position. Recalling the canonical form
of a TC-gridpoint from (2.7), we conclude
&0oGa,?,Th1,d) = 0			ffl)?3
(2.28)
62
The out-degree of the graph is the number of (a, r)-bangs from a state, 3d The
cost of checking the safety of a bang is 0(n). Therefore, the total complexity of the
algoritlim can be obtained by substituting r from (2.20) into (2.2S). Since ? ? 1,
we can use
2co			co
--H min
13 a(?+1)'a(ci+1
instead of (2.20) to get the bound in the theorem. L]
2.3 Summary
This chapter formally introduced the kinodynamic planning formulation of opti-
mal trajectory planning. The specific problem, Optimal Cartesian K inodynami c
Planning, considers a point mass robot obeying L? dynamics bounds in an envi-
ronment of polyhedral obstacles. Although this problem appears simple, finding
an exactly optimal solution is M?-hard iii three dimensions. Furthermore, the
algorithmic result can be generalized to robots obeying decoupled dynamics and
dynamics bounds.
The simple algorithm presented was the first provably-good polynomial-time
approximation algorithm for planning time-optimal robot motions in more than one
dimension. It searches for a shortest path in a reachability graph generated by using
elementary control primitives, (a, ?)-bangs. The trajectory segments generated
by these primitives sufficiently "discretize1, the vector fields possible under the
kinodynamic constraints, to guarantee that a trajectory corresponding to a path
in the reachability graph will be ?-optimal if the problem has a solution.
The "Scaling-Tracking" method used in proof of the algorithm's correctuess
and complexity bounds is very important to later chapters of this thesis. This
proof structure is characterized by three main features:
A lemmarelates the safety margin and ? to Q(() ?-safe-trackiug parameters ??
and TIv ensuring that any trajectory tracking a &?-safe trajectory to tolerance
(?z,'iv) will be (1 --H ?)??-safe, or ??`-safe.
2. It is observed that if an optimal kinodynamic trajectory is uniformly slowed
down by a factor of ?, or ?-time-rescaled, then the resulting trajectory will
be &optimal, observe ?-safety, and obey tighter dynamics bounds.
63
3. Given a tracking tolerance (?iz, 7/v), where 7/z, 7/v > 0, a tracking lemma pro-
vides criteria for choosing reachability graph parameters guaranteeing that
in the absence of obstacles, any trajectory obeying these tighter bounds and
starting appropriately close to the root vertex will be tracked to tolerance
(7/z, 7/v) by some graph trajectory. The graph parameters will be polynomial
in 7/z and 7/v, and the graph size will be polynomial in --H? and
Chapter 3
Better Bounds for Cartesian
Kinodynamic Planning
3.1 Improved Techriiques
3.1.1 Introduction
In the previous chapter, we formalized the optimal and ?-optimal Basic Kinody-
namic Planning Problems. The provably-good polynomial time approximation
algoritlim described is simple, and the overall structure of the proof we give is
intuitive. Almost all complicated details are contained in the proof the Simple
Tracking Lemma (2.2.6), which includes the supporting Lemma 2.2.5.
Although the proof of the lemma may seem tortuous, the strategy behind it
is rather naive: it simply focuses on tracking trajectory position. The velocity
tracking falls out because the "knotpoints" are too close together for too much
velocity error to accumulate, provided that the knotpoint velocity condition (2.22)
and the acceleration bound are obeyed. As one might expect, improved complexity
and accuracy bounds can be obtained through an analysis that carefully considers
tracking position and velocity together. While our proof of the Simple Tracking
Lemma combines knotpoints and structural induction to prove the existence of
the necessary graph trajectories, we can prove stronger tracking lemmas by giv-
ing inductive definitions that simultaneously enforce both position and velocity
conditions. These lemmas enable us to improve the ?-approximation results for
Cartesian robots obeying L? dynamics bounds in both accuracy and complexity.
We modify Algorithm 1 and apply new analytical techniques to lower the com-
64
65
plexity of finding e-optimal kinodynamic trajectories. We halve the exponent in
the polynomial bounds from 6d to 3d, showing that this second algorithm, Algo-
rithm 2, has complexity 0 (dcd?N (?El)3d), where N is the geometric complexity
of the environment and cB is a robot-dependent constant. Further modifications
yield an algorithm that finds solutions more accurately approximating the start
and goal. While the previous algorithms are guaranteed to find solutions that take
at most (1 + e)Topt when an optimal solution taking time Topt exists, Algorithm 3 is
guaranteed to find a solution that will take no more than Topt lime. The complexity
of this algorithm is the same as the second with respect to the approximation pa-
rameter e and the geometric complexity N, but it is more sensitive to the dynamics
bounds.
The complexity bounds indicate that implementations of these theoretical algo-
rithms could be feasible. We briefly report on a Common LL?P implementation of
the improved algorithms. This implementation is used for the simple experiments
in Chapter 5.
3.1.2 Improved Results
We describe two provably good approximation algorithms for the optimal Basic
Kinodynamic Planning Problem (O,S,G,Tt,?v,l,co,ci) in d dimensions.
The first algorithm improves on the complexity bounds of Algorithm 1 from
Chapter 2, lowering the exponent of from 6d to 3d.
Theoren? 3.1.1 Let (0, S, G,t', v, 1, co, ci) be an opt??nai kinodynamic pThnning
problem for a point mass robot in cl dimensions obeying L? dynamics bounds. Let
O<e< 1
Suppose there is a 6?(co,ci)-safe trajectory from S to G taking time Topt. Then
Algorithm 2 finds a (1 --H e)??(co,ci)-safe trajectory going from some =
to some = (g*, g*) within 0(e) of S and G, respectively, and taking time at
most (1 + e)T0?t. The approximation error at S and G is independent of v and 1.
The running time of the algorithm is
0 c%N i7V1(c1?iA4moa)?
where N is the geometne complexity of the problem and c? is a constant of the
algorithm.
66
Thus, with obvious substitutions, the algorithm has an asymptotic time bound
of O(dcdBN(??1)3d)? as in Table 1.5.1. Note that the the constant c? is used to sim-
plify the expression that results from substituting in the timestep r from equation
(3.10) into the bound (3.16) on the number of reachability graph vertices and then
factoring in the branching factor of graph.
The second algorithm improves on both the accuracy and the complexity.
Theorem 3.1.2 Let (O,S,G,a,?v,l,c0,c1) be an optimal kinodynamic planning
problem for a point mass robot in d dimensions obeying L? dynamics bounds. Let
o < e < 1.
Suppose there is a 6v(co,ci)-safr trajectory from S to G taking time Topt. Then
Algorithm3finds a (1 --He)?v(c0,c1)-safe trajectory going from some S*
to some = (g*, g*) wiU?in 0(e) of S and G, respectively, and taking time at
most Topt. The approximation error at S and G is independent of?v and I
The running time of the algorithin is
d
?a2c03e3 1
0 dcAd,N ;7v(aci+ ?v)3
where N is the geometric complexity of the problem and c? is a constant of the
algorithm.
Thus, with obvious substitutions, the algorithm has an asymptotic time bound
of O(dcdcN(?l)3d)? as in Table 1.5.1 Note that Algorithm 3 will find a trajectory
that takes at most as long as the optimal trajectory but that is not guaranteed
to be as safe. c? is obtained similarly to cd?, but using equation (3.11) instead of
(3.10).
Theorems 3.1.1 and 3.1.2 are similar in flavor to Theorem 2.1.1. They ana-
lytically relate the running times of the algorithms to guaranteed error bounds,
and they state that the algorithms run in time polynomial in the geometric com-
plexity N and in the resolution ((`) The complexity bounds in both theorems
are a significant improvement over the o(N(i?)6d) result in Chapter 2. The dcd?
(or dccd) factor in the complexity comes from removing the restriction that d = 2
or 3. Because the results identically extend to robots with decoupled dynamics
and decoupled dynamics bounds when the c-space obstacles are unions of simple
convex polytopes, we use N instead of n to denote geometric complexity, i.e., IV
denotes the number of c-space obstacle surfaces.
67
Recall from Section 2.1.2 that an optimal kinodynamic planning problem K
has three components: the combinatorial complexity of the geometry, the alge-
braic complexity of the geometry, and the algebraic complexity of the kinodyna-
mic bounds. The two algorithms we present in this chapter are ?-appwximation
schemes fully polynomial in the combinatorial and algebraic complexity of the
geometry, and pseudo-polynomial in the complexity of the kinodynamic bounds.
Again, the approximately optimal solutions are not necessarily spatially near to
the (true) optimal safe solution, except at the endpoints.
3.2
3.2.1
Kinodynamic Planning with L? Dynamics
Bounds
An Algorithm for Basic Kinodynamic Planning,
Revisited
The basic idea behind our approach is again to reduce the problem of finding an
approximately minimal-time trajectory to finding the shortest path in a directed
graph. The vertices of the graph "discretize" the statespace TC, and the edges
of the graph correspond to trajectory segments that each take time T, which is a
parameter calculated by the algorithm. As in the previous chapter, the algorithm
will choose a root vertex state 5? (s*, ?*) and use &`?-safe (a, ?)-bangs to induce
a reachability graph ?(V, ?). Again, all the components of ?* will be divisible by
aT, so that the vertices Vj Ei V will lie at the interstices of an underlying grid in
TC. (See section 2.2.1.)
The smaller T is, the finer the underlying IC-grid, arid the better some (?a, T)-
grid-bang trajectory will approximate any arbitrary trajectory that starts at 5*
and obeys the kinodynamic constraints. Thus, it is intuitively plausible that if T
is small enough and Vopt is an optinial trajectory from a state 5 sufficiently near
5* to a state G, then there will be an (a, ?)-grid-bang trajectory Fq going from 5*
to a state G* near ?? in time
Tq ? (1 t ?)Topt.
A naive algorithm might therefore do the following. First it would choose a
time-step T as a function of?a, v, ?, cO, arid ci. Then, it would choose a start state
5? that approximates 5, with the restriction that aT divides .s*. Finally, it would
search for the shortest path in the induced reachability graph to any of a set of
68
vertices that approximate G, and return the trajectory corresponding to this path.
The closeness of the approximations to S and G would improve as T decreases.
This intuition is correct, and it matches Algorithm 1 from Section 2.2.2. Again,
the main burden in proving correctness is to show how to choose an adequately
small r that induces a reachability graph only polynomially large in 1c The dis-
advantage of the naive algorithm is that the proofs that lower its asymptotic com-
plexity to o(n(??l)3d) would only guarantee it to approximate the goal to O(?vr) in
position.
3.2.2 Improving Algorithm 1
Recall that edges in the reachability graph correspond to (a,r)-bangs between
vertices. A state in the image of one of these trajectory segments is called an edge-
state if it is not a vertex. Since an (ti, r)-bang trajectory might come closest to the
goal state at a time that is not a multiple of T, we might expect that an algorithm
that checks the closeness of edge-states to the goal will find a better approximation
than the naive algorithm would find. (See figure 3.1.) This intuition is correct, and
by considering edge-states as well as vertices, this simply modified algorithm can
find a trajectory that approximates the e-time-rescaled goal state G' (g, 1+1?g)
to within O(a?2) in position and O(ar) in velocity.
We define a graph trajectoryto be a trajectory that begins at the root S* of the
reachability graph q and is a sub-trajectory of some trajectory corresponding to a
path in the graph. Thus, the algorithm looks for a minimal-time graph trajectory
to an edge-state or vertex within the appropriate closeness of G'
If for some coordinate i, sj is riot a multiple of tiT, then it is hard to choose
the root vertex S* that approximates S and that is the best for finding an ap-
proximately optimal graph trajectory, without a cumbersome case analysis. We
attempt to simplify the description by using the following trick: whenever some
is not a multiple of aT, the algorithm chooses a root vertex S* so that S will be
approximated by Fq(T), for any graph trajectory Vq beginning at S*. If l'q is the
minimal-time graph trajectory to a state adequately close to G', the algorithm will
then return the sub-trajectory of Fq that begins at time 7.
3.2.2.1 An Improved Algorithm for Basic Kinodynamic Planning
Recall that S' = (s, 1+1?S) and G' = (g, 1+1?g) where S = (s,s) and G (g,g) are
the start and goal states, respectively. Algorithm 2 does the following:
69
o+			o+			o+			o+			o+
0			0			o+			o+			o+			o+			o+
Figure 3.1: Edge-states can allow better approximation. State G* on the trajectory
of the (a, r)-bang from X to Y lies muTh closer to G' than either X or Y do.
Algorithm 2
1. Chooses a timestep T as a function of a, v, ?, c0,and ci. Specifically, the
algorithm chooses the largest ? such that aT v and
T ?			--HE)??ci1?0?()2+
a (7ci(1			+
2. Next, it chooses the starting TC-gridpoint S* that serves as the root vertex
of the reachability graph. For each i:
the multiple of tir closest to 1+1?Sj
sj(O) --H ???(si(O) + s?(O)).
3. It then searches for a minimal-time graph trajectory from S* to any state
that is within (5?arn2, 7??T) of G'. This search is basically a breadth-first search
70
of the induced reachability graph ?. In the ?th generation of the search the
algorithm finds vertices n edges away from S*. However, as the algorithm
explores an edge out of a vertex, it checks whether the corresponding (a, T)-
bang comes adequately close to ?? If during a generation n more than
one such (a, r)-bang is are found, the algorithm chooses one that comes
adequately close to G' at the earliest time. The algorithm uses backpointers
to construct a minimal-time graph trajectory when its search succeeds.
4. Let Fq be the minimal-time graph trajectory found above. If S* S', then
the algorithm returns Fq; otherwise, the algorithm returns l'rq, defined by:
Fq7(t) = t%(i + T).			(3.1)
3.2.2.2 Analyzing the Algorithm
We claim that if an optimal ?-safe trajectory Vopt from S to G takes time Topt,
then Algorithm 2 will find a ?`?-safe trajectory taking at most time (1 + ?)Topt and
approximating ?? and G' to within tolerances that are O(?) and that do not grow
with 1 or v. This accuracy bound is similar to that in the previous chapter.
Applied in conjunction with Lemma 2.2.3 (the Safe Tracking Lemma), a key
Efficient Tracking Lemma, 3.3.1) shows how to choose a timestep T
such that the choice of S* above assures that there will be a ?,-safe
lemma (the
that is ?(?)
graph trajectory Fq taking time
Tq ? (1 + c)Topt
and going from S* to a state G* approximating G' to the appropriate tolerance
given in step 3 above. This shows that seard?ing for the shortest graph trajectory
will be adequate. The lemma also bounds IFq(T) --H S'? in the case when S* #
Note that the corresponding lemma (the Simple Tracking Lemma, 2.2.6) in the
previous chapter requires a timestep r that is o(?2). Algorithm 2's choice of an
?(?) timestep is responsible for the improvement in complexity.
As before, the overall complexity of the algorithm is the product of the size of
the reachability graph g and the cost of checking whether an (Tt, T )?bang (candidate
graph edge) is ?-safe. The maximal o'it-degree in ? is 3d, and the number of ver-
tices is bounded by the number of TC-gridpoints, which is proportional to (?v73)d
71
Safety-checking using simple computational-geometric techniques described in Sec-
tion 2.2.5 costs 0(N) per (a, r)-bang for polyhedral obstacles. In d dimensions,
for obstacles that are the unions of simple convex polytopes, safety-checking costs
O(dN) per (a, r)-bang, with the d factor coming from the cost of computing dot-
products. With minor modifications, the technique for checking safety with respect
to a single obstacle is also used to d?eck closeness to the goal. Absorbing constant
factors from the timestep choice into a robot dependent constant cB, we get the
complexity bound O(dcdBN(1()3d)
3.2.2.3 Improving Accuracy Bounds
The accuracy bounds of the algorithm can be improved while maintaining the
(1 --H e)? safety bounds. With easy changes to the choice of root vertex and
the search-termination condition, a modified algorithm caii find a trajectory that
approximates S and G to (O(?ar2), O(T?T)) tolerances and that takes no more time
than the optimal, exact solution.
The Strong Tracking Lemma (3.3.9) is crucial to this improvement in algorithm
accuracy. Specifically, whereas Tracking Lemmas 2.2.6 and 3.3.1 show how to track
an e-time-rescaled trajectory, the Strong Tracking Lemma provides parameters that
guarantee some graph trajectory can track an arbitrary trajectory to a desired
tolerance, provided that the latter trajectory obeys the dynamics bounds obeys a
simple start condition.
Very similar to Algorithm 2, Algorithm 3 does the following
Algorithrn 3
1. Chooses a timestep T as a function of a, v, ?, co, and cl. Specifically, the
algorithm chooses the largest T such that TlTjT and ? 2?ac1
2. Next, it chooses the root vertex state S* of the reachahility graph. E?or each
si
Sj
--H the multiple of a? closest to
si(O) --H %2(sj(O) + s?(O)).
3. It then searches for a minimal-time graph trajectory from S* to any state
that is within (5%f2, Tha4r? of G. The search is a modified breadth-first search,
just like step 3 in Algorithm 2. The algorithm uses backpointers to construct
a minimal-time graph trajectory when its search succeeds.
72
4. Let Fq be the minimal-time graph trajectory found above. If S* = 5, then
the algorithm returns Fq; otherwise, the algorithm returns Fq7, defined by:
Vq?(t) = Fq(t + r).			(3.1)
3.3 Better Bounds for a Cartesian Robot with
L? Dynamics Bounds
3.3.1 Overview
We now outline the arguments proving Theorem 3.1.1 and Theorem 3.1.2. We
will show that the parameters chosen by Algorithms 2 and :3 that guarantee the
approximation closeness and the complexity bounds in the theorerns.
The general idea of the proof of Theorem 3.1.1 similar to the proof of Theorem
2.1.1. The choice of timestep T and root vertex S? will guarantee that for every
trajectory obtained by ?-time-rescaling a &v-safe trajectory beginning at start state
5, there will be a graph trajectory that tracks tlie rescaled trajectory closely enough
to be (1 --H ?)?-safe. In the proof of Theorem 3.1.2 there is no ?-tim&rescaling:
the choice of timestep and root vertex guarantees that for every 6?-safe trajectory
beginning at start state 5, there will be a graph trajectory that tracks it closely
enough.
3.3.1.1 A Proof for Improved Complexity
To show Theorem 3.1.1, we will use the structure of the proof of Theorem 2.1.1
but substitute in a stronger tracking lemma. Suppose that Fr is a trajectory from
5 to G taking time Tr, and suppose furthermore that I'r obeys dynamics bounds
a and?v and is %(co,ci)-safe. Let0? ?? 1.
We first recall that the Safe Tracking Lemma (2.2.3) yields a one-parameter
family of ?-safe-tracMng tolerances (Tix, ??v) such that if trajectory Fa tracks a
?v(co, ci)-safe trajectory F6 to a tolerance in this family, then Va will be (1 --H
e)?v(co,ci)-safe. Again, letting F'r denote Vr e-tirne-rescaled as in e(juation (2.9),
if a trajectory Fq tracks V'r to any e-safe-tracking tolerance, then l'q will be
(1 --H e)?v(co,ci)-safe.
We then show how to choose a timestep T and a root vertex state 5* ?? that
in the absence of obstacles, (a) for any graph trajectory V beginning at 5* and all
t ? [0, T], F(1) is within (TIz, ?v) of 5, and (b) there will be some graph-trajectory Fq
73
such that Fq? tracks1 F'r to tolerance (7/x, 7/v) (Lemma 3.3.1, the Efficient Tracking
Lemma, from Section 3.3.2.) Fq and Fqr will therefore be (1 --H e)&v(co,ci)-safe even
in the presence of the obstacles.
Finally, we show how to choose a safe tracking tolerance (7/x, 7/v) so that there
will be a timestep r that both is ?(e) and that satisfies the Efficient Tracking
Lemma. It follows that the number of TC-gridpoints will be 0 (cd (l?)3d),for
some robot-dependent constant c. Since each TC-graph vertex has a maximum
out-degree of 3d, and checking the (1 --H e)?(co, ci )-safety of an (a, ?)-bang is O(Nd),
it follows that searching the TC-graph for a shortest path from S* to G* in the
TC-graph takes time 0 (dcd3dN (7i)3d) Letting cB 3c, we get the complexity
o (dcd? (7i)3d) The lemmas and theorem (3.3.16) that follow, together with the
terms in the algorithm, give us Theorem 3.1.1.
3.3.1.2 A Proof for Improved Accuracy
Although Theorem 3.1.2 states that if an exact optimal solution takes time Topt
then Algorithm 3 will find an e-optimal solution that take at most time Topt as
opposed to at most (1 t e)I?pt our proof of the theorem is structurally simpler
than our proof of Theorem 3.1.1. In particular, we show how to choose a timestep
r and a root vertex state S* that guarantee some graph trajectory will track an
optimal trajectory, not merely an optimal trajectory that has been e-time-rescaled,
closely enough to be (1 --H e)??(co, cl )-safe and e-close at the start and goal.
We again use the family of e-safe-tracking tolerances (7/x7/v) given by the Safe
Tracking Lemma. However, Lemma 3.3.9 (the Strong Tracking Lemma) provides
conditions for a timestep and a root vertex that guarantee (a) for any graph tra-
jectory F beginning at S* and all t E [0, Tj, V(t) is within (??, 7/v) of S, and (b)
there will be some graph-trajectory Fq such that Fq7 tracks Ur to tolerance (7/r, 7/v).
To obtain the final bounds, we show how to select (7/x, 7/v) that result in a timestep
that is ?(e). Except for details, the rest of the proof for Theorem 3.1.2 is the same
as for Theorem 3.1.1
`Recall that FqT(t + ?) = Fq(t) fi?oni equation (3.1).
74
3.3.2 The Efficient Tracking Lemma
3.3.2.1 Preliminary Discussion of the Lemma
Our tighter approximation and time-complexity bounds in Theorem 3.1.1 rely on
following.
Lemma 3.3.1 (The Efficient
acceleration bound
suppose that a? divides v.
Tracking Lemma) Suppose a trajectory Fr re-
and velocity bound v and takes time Tr. Further-
Then, in the absence of obstacles, the following
spects
more,
hold.
1. For any positive n?, `lv, there exists a timestep size T and a choice of root
vertex S* such that
(a) for any graph trajectory F and all t ? [0,7], 1'(t) is within (nx,iiv) of
Fr(O), andV(?) is within (t?72,Thy) ofVr(0);
(b) there is a graph-trajectory 1'q such that Vq(t + 7) is within tolerance
(nx,?iv) of Vr(t) for all t ? [O,Tr], and [q(Tr + 7) is within ?5a?2, 7????)
of Fr(Tr)
2. Moreover, r is only polynomial small in 71x 71v, and a1 Specifically, 7 can be
chosen as the largest 7 suHi that a? divides v and
? min( ??4e(2+e)q?			4Thv			(3.2)
?L(3+io?+5&?) 7?(L
As in the proof of Lemma 2.2.6, it will be sufficient to consider the one-
dimensional case to prove this lemma, since we are using the L?-norm for dy-
As in the previous chapter, let us call a function that describes
namics bounds.
the velocity of some grid-bang trajectory a grid-bang velocity function.
We consider an arbitrary trajectory Fr in the absence of obstacles that obeys
the velocity bound v and the acceleration bound a We initially assume that
Tr = Nr for some integer N, and that v, vr(O) and vr(Tr) are multiples of a?
where vr = Pr
The proof has four stages. First, we show that there exists a pair of grid-
bang velocity functions bounding Vr from above and below while staying within
a constant of vr. (See figure 3.2.) Second, we use these bounding trajectories to
show there exists a grid-bang trajectory f;q that tracks Vr to a tolerance that is
75
a function of the dynamics bounds and r. (See figure 3.4.) Third, we show that
there exists a grid-bang trajectory Fq that tracks Fr only slightly less closely than
Fq but that approximates Fr better at Tr. (See figure 3.5.) Finally, we relax our
assumptions about Fr, except for the condition that ar divide v. To relax the
assumptions, we show how to prepend and append trajectory segments of length
r or less to Fr to reduce the proof to the restricted case. (See Figures 3.6 and 3.7.)
v
Figure 3.2: First step in proving the Efficient Tracking and Strong Tracking Lem-
mas (3.3.1 and 3.3.9). Assume that trajectory l'r obeys the velocity and accelera-
tion bounds. In addition, for now assume that Tr = Nr for some integer N, and
that v, vr(O) and vr(Tr) are multiples of aT. Then v? is bounded above and below
by (?a, r)-bang velocity functions ?fa8t and v?t0?, which stay within 7a4r of Vr.
76
3.3.2.2 Bounding Velocity Functions
We now introduce a notation to describe grid-bang trajectories. Observe that a
grid-bang trajectory F lasting for time Nr is uniquely described by the initial state
(p(O),p(O)) and asequenceofN vectors fcO,...,cN?1J over (?j,0,11d Then we
have for for t Ei [n7, (n + 1)r):
v(t)			=			v(nr) + (t --H n?)aIc?
(3.3)
p(t)			=			p(nr) + f?17 v(s)ds,
where I is the identity matrix2.
The following lemma shows the existence of two "bounding" grid-bang velocity
functions for every one-dimensional velocity ffinction Vr that obeys bounds v and
a. (See figure 3.2.) For a velocity function Vr in d dimensions there will exist d
such pairs of functions. in the construction of an (?a, r)-bang trajectory that ap-
proximates Fr, we will use these bounding ff?nctions to guarantee velocity-tracking.
Lemma 3.3.2 Let ? be fixed suJ? that ?aTI?v. Let v? be a velocity fitnction obeying
dynamics bounds v and a, and let TITr. Suppose that a? divides vr(O) and v?(T?).
Then there exist two 9rid-bang velocity functions VsThw, ?fast : [0, .`Vrj
that satisfy the following five conditions:
1. vsmw(O) = vr(O) = Vfast(O),
2. Vsiow(Tr) = vr(Tr) = Vf(Lst(TT)
3. vsiow(t) < Vr(t) < vfast(t) foi all I e [0, Tr], and
? 1ar
4. vfast(t) --H vr(t) --H ? and vr(t) --H v?m?(t) < 1Tir for all t c [0, `Pr]
5. for all n, vfast(flT) --H v?(nr) < -3ar and v?(nr) --H v3t0?(nr) <
Proof: We first define a grid-bang velocity function v? that approximates vr.
(See figure 3.3.) Let us define vr(nr) to be the integral multiple of ar that most
closely approximates3 v?(nr). ?`e then inductively construct %`r:
o+ Initialization: vr(0) =
2If instead of L? bounds v and -a we have vectors of decoupled dynaii?ics hounds we replace
al with the diagonal n?atrix of acceleration bounds A, i.e., Aji is the acceleration bound in the
jth coordinate direction.
3In cases when Vr(r) v??a(nrr) aT ar the floor or ceiling n?ust be chosen consistently.
77
o+ Stage n: for t ? [n7, (n + 1)7)],
vr(t) = Vr(fl7) + (?r((n+1)7)--Ht)r(fl7))(t--HnT)
7
(3.4)
Thus, for all n, Ivr(n7) vr(n7)I ? ffi?7. It follows that Vr also obeys acceleration
bound a, or else Vr would have to violate a. Furthermore, because of the closeness
at multiples of r, IVr(t) --H vr(t) ? 3a" for all t E [0, Tri. It is obvious that Vr obeys
velocity bound v.
We now define:
1. viast(t) = min(Thvr(t) +a?).
2. v?tow(t) = max(--HTh vr(1) --H
It is straightforward to verify that these grid-bang velocity functions satisfy the
last three claims of the lemma.
Vfas? and Vstow are similar to ??fast and t?s1ow. First, we set
Vfast(O) = vstow(O) = %Thr(O),and
Vfast(i'r) = vstow(Tr) =
We then construct the frinctions forwards in time from I = 0 and backwards
from I = Tr until they match i'fast(1) (See figure 3.3.) For the Vfa3t case at the
I = 0 end, we set
until the minimal time when Vfast(I) =
Vfast = Vr. The other cases are similar.
d			d
??1(vfast(I)) = min(l, ?1vfast(I) + 1)
Vfast(I). If there is no such time, then
E
3.3.2.3 ?acking a Restricted Trajectory
Given a trajectory Ur that begins and ends with velocities that are multiples of
aT and that takes a multiple of r time, we show that there exists an (?a, 7)-bang
trajectory that matches it at tlie start, approximates it at the end, and tracks it to
a tolerance that is independent of trajectory time. Lemma 3.3.3 establishes that
the start state can be matU?ed and that the tracking tolerance can be obeyed for
an arbitrarily long trajectory. The trajectories Vstow and Vf(?st are used in the con-
struction as "markers" to bound velocity tracking error and enforce exact matches
at I = 0 and I = Tr. Lemma 3.3.4 states that with an increase in tracking error,
an (?a, r)-bang trajectory can also approximate Fr(7)') to within a?2 in position.
v
7S
0
0			I			If
Figure 3.3: Constructing bounding velocity functions for Len?ma :3.3.2. tThr matches
Vr at t = 0, Tf. Vfast and VsThw are visible as dashed lines where they differ from
Vfast and Vs?ow, respectively.
v
79
Figure 3.4: Second step in proving the Efficient Tracking and Strong Tracking
Lemmas (3.3.1 and 3.3.9). \Ve constructively show the existence of an ()?Tif?h2a?anr?
trajectory that tracks Fr. pq(()) pr(O), and Ipr(1) --H T3q(t) ? a?2 + 8c(2+c)
in the proof of the Lemma 3.3.1. 1?q is bounded above and below by (a, ?)-bang
velocity functions Vfast and V3j0??, which stay within 7T4ir of Vr.
so
Lemma 3.3.3 Let T be fixed such that ?ari?v, and let e > 0. Let Vr have velocity
function Vr obeying dynamics bounds v and and let T Tr. Suppose?ar divides
Vr(O) andvr(Tr).
Then there is an (?a, r)-bang trajectory Fq such that for all limes t ? [0, Tr],
vr(t)--Hvq(t)1 ? 7?ar			and			(3.5)
4
Pr (1) pq(t) ? ?ar2			9(1+e)2?a?2			(3.6)
2			Se(2 + e)
Proof:
We inductively define a (a, r)-bang trajectory f;q that tracks Fr to the tolerance
in the lemma. Our choice of f;q(O) serves as the root vertex of the reachability graph
and determines the position?space alignment of the TC?grid coordinates. Let v?,
Vfast and Vs?ow be defined as in the previous lemma.
o+ Initially, set p?(O) pr(0) and z%(0) t4(0). The origin of the grid is
implicitly chosen so that (pq(O), t?q(O)) is a gridpoint.
o+ Stage n (> 0): We choose c? ? [--H1,0,1), and set i?(t) vq(flT) + c?a(t --H
ni') for I E [nT, (n + l)r). c? is s?mply chosen to minimize
Ipr((n + i)T) --H pq((n + 1)i')I
subject to the condition that
vsmw(t) ? vq(t) ? vfast(t) for all I E [nr, (n + l)Tj.
We claim that the trajectory l'q thus defined will meet the conditions of the
lemma. vq obeys (3.6) by inspection. To see that Pq obeys (3.6), we consider how
large Pr (I) --H P?(1)I can grow.
First, we observe by inspection that if for integer k
Ipr(kT)--Hj)q(kT)I ? ar2			and
2
IPr((k+1)T)?P?((k+1)T)I ?
then for all I E [kr, (k + 1)?J, Ipr(1) --H i)q(t) _? ?a72
Now, suppose that IPr(kT)--HJ)q(k7) ? ar hut Ipr((k+l)T)--Hj)q((k+l )r)I > ???
Because of the second rule, we conclude that
sgn(pr(kT) --H Pq(kT))			sgn(vr(kr) --H i?(kr))
sgn(j)r((k + l)T) --H P%((k + l)T))
#0
81
Furthermore, since Fr obeys the acceleration bound
ae(2+e)
[sgn(pr((k + i)r) --H pq((k + 1)r))](aq(t) --H ar(t)) > (1 + e)2
from time kr until the least later Tw such that vq(Tw) = vr(Tw), i.e., the time
when Ipr(t) --H p?(1)I stops growing.
Since IVr(kr) --H Vq(kT) < Th? and Fr obeys the acceleration bound ??%??, it
follows that
and
Tw < 3(1+e)2T
--H 2e(2 + e)
Ipr(Tw) --H pq(Tw) ? ?ar2			9(1+e)2?a?2
2			Se(2 + e)
Using the trajectory Fq defined above, we show the existence an (?a, ?)-bang
trajectory Fq that track the respective Fr a little less closely in position but that is
within (ar2,ar) of Fr at time Tr. Specifically, we modify the velocity function Vq
to improve the approximation while stQying between ?`fast and Vstow. (See figure
3.5.)
Lemn?a 3.3.4 Let r be fixed such that aTIv. Let Vr have velocity function v?
obeying dynamics bounds v and and let rTr Suppose aT vr(O),vr(Tr).
Then, there exists a grid-bang Vq that tracks Vr to tolerance
3$72 + 9(1+e)2?ar2 7a
such that Fq(O) = Fr(O), vq(Tr) = vr(Tr), and Ipq(Tr) --H p?(Tr)I ? aT2.
Proof: Given Fr obeying the hypothesis of the lemma, let us define ?fast, Vstow,
and f;q as defined in the proof of Lemma 3.3.3. We show by construction how to
incrementally modify l'q into a Fq that satisfies the lemma. let y = pr(Tr)--Hpq(Tr)
o+ Initialization: Let _ --H			. Co to stage ().
o+ Stage n > 0: If pr(Tr) --H pq(fl)(T?) < aT2, then Lq = q(fl) Otherwise, we
obtain ?q(fl+l) by modifying ?q(fl?l) Let m be the least integer such that
sgn(y)(vr(mT)?vq(mT)) > 0
sgn(y)crn > 0			and
5gn(y)crn?1 ? 0.
Set crn--Hl = crn--Hl + sgn(y) arid cm = c7n --H sgn(y). Proceed to the next stage.
v
S2
Figure 3.5: Third step in our proofs of Lenimas 3.3.1 and 3.3.9 Vq (from Lemma
3.3.3 or 3.3.10 ) is modified to obtain t'q such that f0Tr vq(t)di is within ?ar2 of
?Tr Vr(t)dt.
83
For n > 0 we verify that at stage n we can always find the necessary integer
m. We describe the case where y > 0; the y ? 0 case is similar. If 4n) Vfast
uniformly, then pq(fl)(??) > pr(Tr). We show that as long as ?q(fl)(1) < Vfa3t(t) at
some time t, the stage can proceed. Suppose that we have followed the construction
through n --H 1 stages, reach stage n, and find that pr(%) --H pq(fl)(T?) > ?ar2 Then,
s?nce ?q(fl)(()) = vfast(0), there is a least integer k such that ?q(fl)(??) < Vfast(kT).
Either ck?1 = 0 or ck?1 = 1. Suppose that ck?1 = --H1. Then there must be a
least integer m > k such that Cm > 0 since vq(fl)(??) ? Vfast(kT) but 4n) (Tr) =
Vfast(Tr). Suppose that J--H' --H 0. If ck > 0 then m = k. Otherwise, by the
previous argument, there must be a least integer m > k such that Cm > 0.
By inspection, we can see that for each n and all I E [0,Tr],
pq(fl)(t) <p$t+1)(!) ? pq(fl)(1)+?a?2			and
Th(fl+l)(Tr) = pq(fl)(??) + a?2.
This assures that the construction reaches termination and the position-tracking
bounds are achieved at Tr. The procedure guarantees that for t ? [0, Tri,
--H pq(t)I ?
9(t+e)2
Se(1 + e) ar2
This and the properties of Vq assure that l'q meets the position-tracking conditions
of the lemma everywhere. The velocity-tracking bounds follow from the fact that
vsmtv(t) ? vq(t) < Vfast(t) for all t. [fi
The following corollary is immediate.
Corollary 3.3.5 Let T be fixed such that ?a?i?v. Let Vr have velocity function Vr
obeying dynamics bounds v and and let TITr. Suppose that Vr(Tr) is a
TC-gridpoint relative to 1'r(O), r, and the dynamics bounds.
Then, there exists a grid-bang trajectory l'q that tracks l'r to tolerance
3$T2 + 9(1+e)2a?2
4e(2 + e)
such that Fq(O) = Fr(O) and Vq(Tr) =
7?;r
Proof: Lemma 3.3.4 asserts all the claims except that pq(Tr) pr(Tr). The
corollary follows from the property that (?a, ?)-grid-bang trajectories with the same
start states and final velocities will have final positions that differ by a multiple of
?ar2 Ei
84
3.3.2.4 Removing Restrictions
We will now remove certain restrictions on the Fr. (See figures 3.6 and 3.7.) First,
we remove the restriction that Tr be a multiple of 7. The basic idea is that if we
extend the trajectory and obtain a trajectory that takes [T7r1 7 time, we can obtain
a trajectory that satisfies the hypotheses of Lemma 3,3.11. The acceleration bound
and the timestep then limit how much greater the tracking error is at time Tr than
at
We now present three lemmas relating to trajectories that obey dynamics
bounds and v, followed by corresponding lemmas relative to trajectories
obeying dynamics bounds a and v.
Corollary 3.3.6 Let r be fixed such that a?I?v. Let Fr have velocity function Vr
obeying dynamics bounds v and a such that ?a7Ivr(O) and ?a7Ivr(Tr).
Then, there exists a grid-bang Fq that tracks ?? to tolerance
3?a?2 9(1+e)2?a?2 7?a?
4e(2 + e) ` 4
such that Fq(O) = Fr(O), vq(Tr) --H v?(Tr)I <?a?, and pq(Tr) --H pr(Tr)I < 3t\f2
Proof: Suppose Fr is a trajectory obeying the hypotheses of the corollary.
Define Tr' = r. Then there is a trajectory ?`r such that:
F'r(t)			=			1'r(t)			if			I E [0, Jr],
vr'(t)			=			vr(Tr)			if			I c [7?r,Tr'1.
By Lemma 3.3.4, there is an (a, ?)-grid-bang trajectory Fq obeying v that tracks
F'r to toterance (3?a?r2 + 9(?14????$r2,			such that Fq(O) = F'r(O) vq(Tr') =
and pq(T?')?p'?(Tr') ? a72. Since Fr(Tr) = 14(Tr) and v?'(t) = vr(Tr) = vq(T?) for
all I E [Tr, Tr'], and since vq obeys acceleration bound a, it follows that I?)r(Tr) --H
pq(Tr)J < 3a12 and Ivr(Tr) --H vq(Tr)j ? a7. [z
Next, we remove the restriction that a7jvr(Tr). Again, we show how to modify
the "tail" of 1'? to get a trajectory that satisfies the hypotheses of Lemma 3.3.4.
Corollary 3.3.7 Let ? be fixed such that a? v. Let Fr have velocity function vr
obeying dynamics bounds v and m7 such that a7lvr(O).
Then, there exists a grid-bang Fq that tracks 1'? to tolerance
3a?2			9(1+e)2?a?2 7a?
t
2			4e(2+e)			4
85
such that Fq(O) = Fr(O), Ivq(Tr) --H Vr(Tr)I ? 7???, and Ipq(Tr) --H pr(Tr)i ? 5a?r2.
Proof: Suppose Fr is a trajectory obeying the hypotheses of the corollary.
Define Tr' = r, and T7 =7)'--H T. Define vo to be the multiple of ar closest to
vr(Tr) subject to the condition that Ivr(T7) --H vol < ar.
We then define a trajectory F'?:
F'r(t)			=			Fr(t)			if			I ? [o,T7],
Vr'(t)			=			vr'(T7) + (t?W')(vo --H vr'(T7))			if			I E [T7,7)'].
and
r? obeys a. It follows that pr(Tr) --H p'r(7))I ? aT2, since 7)" --H 7)11 = r. By
Lemma 3.3.4, there is an (a, r)-grid-bang trajectory Fq obeying v that tracks F'r
to tolerance (3%f2 + 9(?14??)??$?2, 7??7) such that Fq(O) FtT(O), vq(7),) =
and pq(7),) --H P'r(Tr')I ? ?ar2 It follows that pq(1) --H p'r(t)I ? 7?f2 for all t e
[T7, 7)]. Because vq(7)") --H vr(1?) ? and vq(7)') --H vr(fr') ? aT, it follows
that Ivq(t) --H vr(t) ? for all I E (7)11,7)] E
Finally, we remove the restriction that Vr(0) he a multiple of aT. We modify
Fr by prepending a trajectory segment that has an initial velocity that is a mul-
tiple of aT, ends at Fr(O), and takes time r. (This construction ?symmetrical" to
the construction in the proof of Corollary 3.3.6. See Figure 3.7.) The modified
trajectory satisfies the hypotheses of Corollary 3.3.7.
Corollary 3.3.8 Let r be ftxed such that ?arIv. Let Vr have veThcity function vr
obeyin? dynamics bounds v and
Suppose that
the niult4?le of?ar closest to vr(O)
5*			pr(O) --H ?2(vr(O) + s*)
Then, there exists a grid-bang l'q such that
1. Fq(O) =
2. Fq(r) approximates Vr(O) to tolerance (TiT2, Tha4T);
3. For all I c [0,7)], Fq(I + T) ivacks Fr(I) to tolerance (3t?r2 + 9(1+?)2?ar2 ThaT);
4c(2+c)
and
4. Fq(Tr + r) approximates Vr(Tr) to tolerance (5a?T2, Tha4T)
86
I			I			I			I
I			I			I			I
I			I			I			I
I			I			I
Ia			I			I
I			I			I			I
I			I
I			I
I			I
I			I
I			I
I			I			I
I			I
I			I
I			I			I
I			I			I
I			I			I
[$JT			Tr			([?)rJtl)?
Figure 3.6: Proving Corollaries 3.3.5 through 3.3.14 anci 3.3.5 through 3.3.14: if
?ar?vr(Tr), then thereis some?a-honnded Vr' such that Vr'(1) = vr(1) fort ? [0, [)rll
and aT divides + 1)T). If pr(0) = p\r(O), then a trajectory that tracks E1r
also tracks Fr almost as closely.
87
A
J
I			I
Vr
J
aT I
--HT			0			T
Figure 3.7: Prepending a trajectory segment in provii?g Lemma 3.3.8 or 3.3.15:
if ?ar1vr(O), then there is some ?a-hounded V.'r such that Vr'(t t T) vr(O) for
t E [0, Tr] and aT divides Vr'(O) Here the time-scale for vlr is offset by --HT to show
this relationship.
88
Furthermore, Fq(O) can be calculated from a, v, r, and Fr(O).
Proof: Suppose Fr is a trajectory obeying the hypotheses of the corollary, and
suppose vr(O) is not a multiple of aT. Let vo be the multiple of t?T closest to vr(O).
We can then define a trajectory ?`r such that F'r(T)
Vr(O) = Vo,
vr'(t)			=			Vo + rt(vr(O) --H vo)			if t E [0, Tj,
p'r(0)			=			pr(O)?%?vrI(1)dt,			and,
F'r(t)			=			Fr(t --H			if t ? [T,Tr + Tj.
Clearly, ?r(O) = (s*,5*) as defined in the lemma. By Corollary 3.3.7, there is
an (a, ?)-bang trajectory l'q such that
1. Fq tracks F'r to tolerance (3rLJ + 9(414?$$%2? ma4r)
5t?r2
2. pq(Tr + r) --H p'r(Tr + r)I ? 2 ` and
3. Ivq(Tr + T) --H vr'(Tr + T)I ? ThU47
Since F'r(t) = Fr(1--Hr) for all t ? [r, Tr+Tj, this establishes the last three conditions
of the corollary. Since Fq(0) = FT'(0) and l'lr(T) = Fr(0), the first two conditions
follow from the a bound. [fi
3.3.2.5 Proving the Efficient Tracking Lemma
We now apply the lemmas and corollaries of the previous two sections to prove the
Efficient Tracking Lemma (3.3.1).
Proof (of lemma 3.3.1): ?Ve apply lemma 3.3.4 and the corollaries that follow.
Specifically, suppose that trajectory l'? respects acceleration bound and
velocity bound v and takes time Tr, and that a? divides T'. Using Corollary 3.3.8,
for any T such that aT divides v, we can compute the components (s?, ?,*) of a state
S* such that for some (?a, T)-bang trajectory l'q beginning at S*,
1. Fq(T) approximates F1)(0) to tolerance (?a?2, Th(?LT);
2. Fq(t + T) tracks Fr(t) to tolerance (3$r2 + 9(?14?(+)2:a?2, 7-??7);
3. and Fq(Tr + T) approximates Fp(Tr) to tolerance (5%J',
____			9(1+?)2ar2
Therefore1 if 3?a?r2 + ? n? and 2?ar < ?it', then Fq(t + T) tracks Fr(t)
to tolerance (iiz, Tlv) over the interval [0, Tri. Thus the choice of T in equation (3.2)
is sufficient. [3
3.3.3 The Strong `macking Lemma
The proof structure and specific techniques from Section 3.3.2 can be used to
show the following lemma, which is essential to our proof of Theorem 3.1.2 using
Algorithm 3.
Lemma 3.3.9 (The Strong Tracking Lemma) Suppose trajectory Fr respects
acceleration bound a and velocity bound ? and takes time 7%. Furthermore, suppose
that aT divides v. Then in the absence of obstacles, the following hold.
1. For any positive 71, 71v there exists a timestep Size T and a choice of root
vertex 5* such Uiat
(a) for any graph trajecto?? V and all I E [01T], V(t) is within (77,71v) of
Vr(O), and V(T) i.? ithin (?a?2,2a?) of F?(O):
(b) there is a graph-trajectory l'q suJ' that 1'q(t + r) is within tolerance
(71,71v) 0f Vr(t) for alll ? [O,Tr], and Vq(Tr + T) is within
of Fr(Tr)
2. Aloreover, T is only polynornially small in 71 71v, -T' and a' Specifically, T
can be chosen as the largest T such thai ttr divides v and
r ? min(571;1 712?aV)			(3.7)
As in the proof of the Efficient Tracking Lemma, it is sufficient examine the
one-dimensional case. NVe cotisider an arbitrary trajectory Ur in the absence of
obstacles, obeying dynamics bounds v and a. NVe again initially assume that
Tr = ArT for some integer IV, and that v, vr(O) and vr(Tr) are multiples of aT.
Using the "bounding" velocity ffiiictions vfast and ?stow from Section 3.3.2.2, we
show there exists a grid-bang trajectory f;q that tracks Vr to a tolerance dependent
on a, v, and T. ?Ve then show that there exists a grid-bang trajectory that is similar
to Fq but approximates l'? better at Tr. Finally we relax our assumptions about
Fr, except for the condition that tiT divide v.
90
3.3.3.1 ?acking A Restricted, But Not e-Tirne-Rescaled, `b?ajectory
The intuition behind the next lemma is similar that for Lemma 3.3.3: if at some
time difference in position between the "reference" trajectory Fq and the "tracking"
trajectory Fq is big enough and the difference in their velocities has the same sign,
then we can bound the duration before the tracking trajectory catches up in velocity
and thus begins to "catch up" in position. However, instead of using a difference
in maximum accelerations to calculate this bound, we will use the amount of time
a maximum acceleration can be maintained without violating the velocity bounds.
Lemma 3.3.10 Let r be fixed such that ?ari?v. Let Fr have velocity function Vr
obeying dynamics bounds v and a, and let r Tr. Suppose aT divides vr(O) and
vr(Tr).
Then there is an (?a, r)-bang traicetory f;q such that for all times t ? [0, Tri,
Ivr(t) --H iiq(t) ? 7?ar			and			(3.8)
--H			4
Ipr(t)--Hp%(t) < 2?vrQar2			(3.9)
Proof:
We inductively define a (?a, ?)-bang trajectory Fq that tracks Vr to the tolerance
in the lemma. In stage n of the construction, we define vq(t) for all t E [n?, (n%1)T)
and possibly alter a previously defined section of the function.4 Our choice of Fq(O)
serves as the root vertex of the reachability graph and determines the position-
space alignment of the TC-grid coordinates. Let Vr, Vfast and Vsiow be defined as
in Lemma 3.3.2.
o+ Initially, set p?(0) = pr(O) and vq(O) = vr(0)
implicitly chosen so that (pq(0), vq(0)) is a gridpoint.
The origin of the grid is
o+ Stage n (>0): We choose c? E f--H1,0,1?, and set t%(t) = vq(nT)+cfla(t
nr) fort Ei [nr, (n t 1)T). The following rules determine c?.
1. Initially, c? is
the condition
chosen c0,.
4The definition works for semi-infinite (i.c.,infinitely long) trajectories, and is in this way
similar to simple finite injury constructions from recursion theory.
chosen to minimize pr((n+l)r)--Hpq((n+l)T) subject to
vstow(t) ? vq(t) < Vfa3t(t), and subject to the previously
91
2. Let y(n) =Pr((fl+1)T)?P%((fl+l)T). We call y(fl) the lag during stage
n. If Iy???I > 2?vT, then
(a) for the greatest integer rn < n such that crn # sgn(y(fl)), set cm
?m + sgn(y(fl)),
if there is a greatest integer j, rn ? j < n such that setting c3
&+sgn(y(fl)) minimizes pr((n+l)r)--Hpq((n+ 1)T)I, or if otherwise
vq((n+l)T) would not be bounded by Vfast((fl+l)T) and vsmw((n+
1)r), then set ci = ci --H sgn(y(fl))
By this definition, for all t e [0, Tr], vsmw(t) ? vq(t) ? Vfast(t), so vq(t) satisfies
the first claim in the lemma.
Before we show Vq satisfies the second claim, we observe that if we only use the
first rule at each stage n, then
pr(t) --H p?(t)I ? 7?vr + aT2
2
for all t E [0, Tri. To see this, first note that a can maintain maximum accel-
eration for at most time Th?v since otherwise it would violate the velocity bounds.
a'
Therefore, the rule implies that if at time 1b
Pr(tb) --H pq(tb) > aT2,
then v? will equal or exceed Vr at some time no later than t6 + LaV
We now verify that the second rule for stage n of the construction is consistent.
Wiog, we will assume that the rule is applied when the lag y(n) exceeds 2?vr; the
negative lag case is symmetrical.
Let
--H (p(fl)?(fl)
q --H q ,Vq
be the trajectory just before the rule is applied. Since y(n) > 2?vr, there is some
minimal tw such that
vq(fl)(t) < Vfas?(t) for all t E [lw,flT).
Otherwise, the first rule would have been violated at a previous stage. Furthermore,
thereisanintegermsuchthatm? <tw < (n?t)T,cm <0 andcm+l,...,&?l = 1.
Thus, we can set cm = cm + 1
92
Finally, we consider the effect of the rule at stage n of the construction, assum-
ing y(n) > ?r and that pAqfl)(?) obeys (3.9) for all t e [O,n?1. In particular, we
must ensure that this does not cause the new pq(fl)(?) to become too different from
pr(t) in the interval [mr, (n + 1)T]. Let denote the trajectory at the end of
stage n. First, for I E [nr, (n + 1)T],
?a?T2 ? p%(fl)(t) --H pq(fl)(t) ? 2vr.
Now recall that for all integers k, vr(kT) --H vsiow(kr) ? flar Since flT --H mr ?2fflv
--H a
and y(n) > 2?vr,
pr(mT) --H p%(?)(mr) >
2
Since
p%(fl)(t) --H pq(fl)(t) ? (I --H rn?)a?
for I ? [mr, (n + 1)T], the rule therefore does not cause a violation of (3.9) for any
I ? [mr, n?] Thus p%(fl+i)(1) obeys (3.9) for all5 t E [0, (n + 1)r]. El
3.3.3.2 Improving Approximation and Removing Restrictions
A lemma similar to Lemma 3.3.4 and a series of corollaries similar to Corollaries
3.3.5--H3.3.5 can be shown with the most minor of changes to the corresponding
proofs.
Lemma 3.3.11 Let r be fixed such that arl?v. Let Fr have velocity function vr
obeying dynamics bounds v and a, and let TjTr. Suppose aTlvr(0), vr(Tr).
Then, there exists a grid-bang Fq that tracks Fr to tolerance (4?vr + ar2, Th???)
such that Fq(O) Fr(O), vq(Tr) vr(Tr), and pq(Tr) --H pr(Tr)l ? aT2
Proof: The construction is identical to that in the proof of Lemma 3.3.4, except
that pq(t) --H pq(1)1 < 2vr. El
Corollary 3.3.12 Let r be fixed such that a?jv. Let Ur have velocity function vr
obeying dynamics bounds v and a, and let T Tr. Suppose that Fr(Tr) is a TC-
gridpoint relative to Fr(O), r, and the dynamics bounds.
Then, there exists a grid-bang t'raiectory Fq that tracks Fr to tolerance (4?vr +
aT2, t) such that Fq(O) = Fr(O) and Fq(Tr) = Fr(Tr)
5The 2?vr has been chosen conservatively to shorten an already long proof.
93
Proof: Same as for Corollary 3.3.5, except that Lemma 3.3.11 is applied instead
of Lemma 3.3.4. El
Corollary 3.3.13 Let r be fixed such that ?arJ?v. Let Fr have velocity function Vr
obeying dynamics bounds v and a such that arlvr(0) and ?arIvr(Tr).
Then, there exists a grid-bang Fq that tracks Fr to tolerance (4vr + ?ar2, TT)
3ar2
such that Fq(O) = Fr(O), vq(Tr) --H vr(Tr)I <?ar, and jpq(Tr) --H pr(Tr)J ? 2
Proof: Same as for Corollary 3.3.6, except that Lemma 3.3.11 is applied instead
of Lemma 3.3.4. El
Corollary 3.3.14 Let r be fixed such that aTIv. Let Fr have velocity function Vr
obeying dynamics bounds v and a such that a? vr(O).
Then, there exists a grid-bang Fq that tracks Fr to tolerance (4?vT + ar 2, 7?r)
Thar			5ar2
such that Fq(O) = Fr(O), vq(Tr) --H vr(Tr) < ? and Ipq(Tr) --H pr(Tr)I ? 2
Proof: Same as for Corollary 3.3.7, except we apply 3.3.11 instead of Lemma
3.3.4. El
Corollary 3.3.15 Let r be fixed such that ?arI?v. Let Fr have velocity frt.nction v?
obeying dynamics bounds v.
Suppose that
= the multiple of ttT closest to v?(0)
= pr(O) --H L2(vr(O) + s*)
Then, there exists a grid-bang Fq such that
1. Fq(O) =
2. Fq(r) approximates Fr(O) to tolerance (ar2, Th???);
? For all t e [O,Tr], Fq(t + r) tracks Vr(t) to tolerance (4?vr +?aT2, Th???); and
4. For all t c [O,Tr], Fq(Tr + T) approximates Fr(Tr) to tolerance (5?a?r2, T)
Furthermore, Fq(O) is computable from a, v, r, and Fr(O).
Proof: Same as for Corollary 3.3.8, but we use Corollary 3.3.14 instead of
Corollary 3.3.7. El
94
3.3.3.3 Proving the Strong Tracking Lemma
(of lemma 3.3.9): We apply lemma 3.3.11 and the corollaries that
Proof
follow.
Suppose that atrajectory Fr respects acceleration bound a and velocity bound
v and takes time Tr, and that ar divides v. By Corollary 3.3.15, for any T such
that ar divides v, we can compute 5* such that for some (a, r)-bang trajectory Fq
beginning at 5*
1. Fq(r) approximates Fp(0) to tolerance (ar2, 7???);
2. Fq(t + r) tracks Fr(t) to tolerance (4vr + ?ar2, Tha4r);
3. and Fq(Tr + r) approximates Fp(Tr) to tolerance (5(?? ,
If 4vr+?ar2 < ?1x and 2ar ? `lv, then Fq(t+r) tracks Fr(t) to tolerance (nx,"v)
El
3.3.4
6?'-Safe ("Also Safe") Grid-Bang Trajectories and
Deriving Final Bounds
3.3.4.1 Final Timestep Choices
Recall that the Efficient Tracking and Strong Tracking Lemmas (3.3.1 and 3.3.9)
specify timesteps r that are polynomially dependent on i7r and ?v. Applying these
lemmas and Lemma 2.2.3 and choosing f3 to maximize the upper bound on r yields
the following theorems.
Theorern 3.3.16 Given dynamics bounds a and v, obstacles 0, and positive sca-
lars e < 1 Co, and cl, let r be chosen such that ar divides v and
r			--He)+49?1;;o?ee)2+48c0(3+loc+5(2)N -			3.10
a (7ci(1
Then for any start state 5 we can choose a root vertex state 5* such that if Fr
begins at 5, is ?v-safe, and takes time Tr, there exists a ??`-safe trajectory Fq with
the following properties:
1. Fq(O) = 5*,
2. Fq(r) approximates Fr(O) to within (?ar2,2ar), and
95
3. Fq(Tr + 7) approximates Fr(Tr) to within (5?a?r2,2?a7).
Theorem 3.3.17 Given dynamics bounds a and v, obstacles 0, and positive sca-
lars e < 1, co, and ci, let 7 be chosen such that a? divides v and
7 < 2?a?(1 --H e) + 5?v			(3.11)
Then for any start state S we can choose a root vertex state S* such that if Fr
begins at S, is 6?-safe, and takes time Tr, there exists a ??`-safe trajectory Fq with
the following properties:
1. Fq(O)
2. Fq(7) approximates Fr(O) to within (?ar2,2?a?), and
3. Fq(Tr + 7) approximates Fr(Tr) to within (5%r ,2a?).
Details of the proof of Theorem 3.3.17 are simpler than those in the proof of
Theorem 3.3.16. Therefore, we will prove Theorem 3.3.17 first.
Proof (of Theorem 3.3.17): Suppose Fr is a &?(co, c? )-safe trajectory taking time
Tr obeying acceleration bound a. Then by lemma 2.2.3 the choice of a tolerance
(nx, liv) given in (2.10) ensures that if a trajectory Fq approximately tracks Fr to
tolerance (lix, liv), then the ?-tnbe induced by f'q lies entirely inside the 6,,-tube
induced by Fr. Since the ?v-tube induced by Fr intersects no obstacles in 0, Fq is
therefore ?-safe. Given the tolerance (lix, liv), we use the Strong Tracking Lemma
to choose the time-spacing ? and the root vertex S* (via Corollary 3.3.15.)
To get the desired bounds, we must choose ,3 so that using (3.11) yields a
maximal 7 as given by (3.7). Let us therefore define for f3 > 0
c0?P
--H 5?c1(1--H()+P)
Cp(
Tv(P)			2a(ci(1--Hc)+P)			(3.12)
min(rz(P),rv(P)).
By inspection, 7x(O) ? 7t,(O), ?x is monotonically increasing, and m, is mono-
tonically decreasing. Thus, 7(P) is maximized when Tx(P) 7v(P). ?equiring P to
be positive and doing a little computation, we find that 7(?) is maximized when
2?v			(3.13)
5?a
96
Applying either Tx or Tv to this P yields the desired T in (3.tt). Finally, we see
that the choice of T and the closeness of Fq(T) to S guarantee that the trajectory
segment from the root vertex to Fq(T) will be ?-safe. E
To prove Theorem 3.3.16, we show how to choose a root vertex S* and a time-
step r so that some graph trajectory will track an ?-1ime-resca1ed ?(co, ci)-safe
trajectory to an ?-safe-tracking tolerance.
Proof (of Theorem 3.3.16): Suppose Fr is a ?(co, ci)-safe trajectory taking time
Tr obeying acceleration bound a. Then the ?-time-rescaled trajectory ?`r given by
= pr(i+t?);
Pr'(t) = i+l?pr(i+t?)
obeys velocity bound and acceleration bound ?i?%?? Furthermore, F'r is
?(co,ci)-safe and takes time (1 + e)Tr.
By definition, if a trajectory Fq approximately tracks F'r to an ?-safe-tracking
tolerance (7!x, ??v), then Fq is ?-safe. Given the a one-parameter family of ?-safe-
tracking tolerances (71z, 71v) from Lemma 2.2.3 (2.10), we use the Efficient Tracking
Lemma (3.3.1) to choose the timestep r and the root vertex S*.
To get the desired bounds, we must choose 4' to maximize T as given by (3.2)
Define for,3> 0:
Tx (P)			=			4c(2+?)(co(P)
3a(3t1Oc+5c2)(ci(1--H?)+,t)
Tv(P) --H			4cp?			(3.14)
7?a(ci(1--Hc)+P)
= min(Tx(4),Tv(P)).
By inspection, Tx (0) ? rv(O), Tx is monotonically increasing, and Tv is mono-
tonically decreasing. Thus, T(4) is maximized when Tx (4) = T,,(4). Consulting
Mathematica for an analytical solution, we find the maximum value of r(4) is given
by (3.10).
Finally, we see that the choice of T and the closeness of Vq(T) to 5' guarantee
that the trajectory segment from the root vertex to Fq(T) will be ?-safe. Li
3.3.4.2 Approximation Goodness and Overall Complexity Bounds
As the choice of timestep and root vertex in Algorithm 2 agrees with Theorem
3.3.16 and Corollary 3.3.8, we have shown the approximation bounds in Theorem
97
3.1.1. The same is true for Algonthm 3, Theorem 3.3.17, Corollary 3.3.15, and
Theorem 3.1.2.
To determine the total complexity of the algorithms we must bound the number
of (?a, r)-gridpoints for a Cartesian robot with maximum (L?) speed v in a
dimensional free-space of diameter 1. As the results in this Chapter are used in the
implementation discussed in Section 5.2, the discussion will be more detailed than
in Chapter 2.
First we consider the d = 1 case. R?ecall from section 2.2.6 that &? (?a, T, v, 1, d)
denotes the maximum number of TC-gridpoints for a point robot with maximum
(L?) speed v and acceleration a in a d-dimensional free-space of diameter 1. It is
clear that G?(a, T,Th 1,1) is equal to the maximum number of possible velocities
at any given time kT multiplied by the maximum number of possible positions
at that time. Since at each timestep the change in velocity is aT, --HaT, or 0, the
number of possible velocities is at most ft; +1. To see that the number of possible
positions at a given velocity is at most Ur- + 1, let v??? denote the velocity and
x(?) the position at timestep i for all i. Then x(k+1) --H v(k)? + a(k)?a2r2, where
E f--H1,O,1J. Wiog, let v(o) = 0 and x(0) = 0. Since v(k) = c(k)a7 for some
integer c(k), by using induction we can show that
x(k)			--H			(2T(k)+l)a72			if c(k) odd
x(k)			--H			2T(k2fr2			if c(k) even,			(3.15)
for some integer T(k). It follows that
Goo(QThTh!,d)= ((ffi??+1)
+			(3.16)
Hence, in a bounded workspace with velocity limits, we can use a polynomial-
sized reachability graph to find an approximate optimal safe solution. Given a
problem for which ?-safe solution exists, the algorithm finds a trajectory Fq that
satisfies the time approximation Tq? T, in addition to respecting the kinodynamic
constraints and being ?-safe. Noting that the maximum out-degree of the reacha-
bility graph is 3d and that checking the safety of trajectory segments with respect
to c-space obstacles that are (the union of) simple convex polytopes requires time
O(Nd), we substitute the algorithm's choice of r, (3.10) or (3.11), into (3.16) to
obtain the complexity bounds in Theorem 3.1.1 and Theorem 3.1.2.
98
3.4 An Implementation for the L? Cartesian
Robot
3.4.1 Brief Description
To get some idea for how our algorithms for Cartesian robots obeying L? dynamics
bounds might behave in practice, we have completed a prototype Common Lisp
implementation for a point robot in two dimensions. This planner, which we have
run on a Sun SparcStation2, is the first implementation of any algorithm that
generates provably good, provably near-optimal kinodynamic plans for problems
in more than one dimension.6
Our planner is a simple implementation and uses no search-pruning heuristics.
Depending on how several parameters are set, it executes a version of Algorithm 2
or Algorithm 3; it can also be used to execute a version of the L? Cartesian R?obot
algorithm described in Chapter 5. The planner basically does a breadth-first graph
search of the reachability graph, computing the neighbors of a state (vertex) only
when it is on the search frontier. A bit array is used to record whether a state has
been reached, and each vertex found keeps a pointer to its ?parentV. The planner
enforces ?-safety, so each edge is a ?-safe (ci, ?)-bang.
Although the planner can be easily modified to execute the theoretical algo-
rithms exactly, the present version uses simplified methods to choose the root
vertex and and to check closeness to the goal. In addition, an inexpensive "dis-
cretizing" preprocessing step7 allows the implementation to reduce its use of the
safety-checking procedure from Section 2.2.5 (implemented using floating-point op-
erations, however) to the obstacles "relevant" to each trajectory segment.
The implementation retains provable ?-approximation properties that can be
shown by extending techniques presented in this thesis. The principal difference is
that for each example, if Topt is the optimal kinodynamic trajectory time for the
problem instance (assuming it is solvable) and r O(?) is the timestep chosen
by the algorithm, the implementation will first find a trajectory taking time at
most (1 + ?)Topt + T time (or Topt + r time for Algorithm 3), and then proceed to
search for a trajectory that comes within a "grid-spacing" of the t-time-rescaled
goal state at a multiple of T. For all the runs we tried, this took at most three
6Actually, it's the second incarnation of our first implementation
7For all the runs done to this day, the preprocessing took at most a few seconds. Details are
rudimentary in principle and beyond the scope of this thesis.
99
more time steps. Thus, the trajectories shown take time at most (1 t ?)Topi + 4T
(or Topt + 4r).
3.4.2 Simple Examples
An example of a solution found by the planner running Algorithm 2 is shown in
Figure 3.8. The start state, at left, has an upward velocity of 0.1, and the goal state
has a downward velocity of 0.1. The small dots indicate the position component
of the planned trajectory at each time step. The "lines" or "spikes" attached to
each dot indicate the velocity at that position. In this problem, the world is 3.5
by 1.9 units. For this run ? = 0.6, Co = 0.2, Ci = 0.5, a = 0.2, and v = 0.3. The
planner run took 4 minutes and 32 seconds of run time (5 m. 31 sec. total elapsed
time) on a 32Mbyte SparcStation2 to find the trajectory in the figure.8
Two examples of solutions found by our planner running Algorithm 3 are shown
in fig. 3.9. Although the asymptotic complexity of this algorithm in the resolution
and the geometry N the problem is the same as that of Algorithm 2, comparing
(3.11) to (3.10) reveals that it is more sensitive to the kinodynamic bounds (?a,
? Co, and ci). In this problem instance, the world is 3,5 by 1.9 units; v = 0.12;
a = 0.1; Co = 0.31; Cl = 0.1; and ? = 0.8. Note that the larger Co is compared
to the maximum velocity, the larger the ?-safe tracking tolerance, and hence the
easier the problem. The start position is at lower left, and the start velocity is in
0.08 the positive y direction; the goal, at the upper right, has 0.08 velocity in the
negative y direction. T was chosen as 0.4, resulting in a TC-grid of approximately
800,000 states. The implementation searched over 740,000 states and required
approximately 50 minutes of run time9 on a 48Mbyte SparcStation2 to generate
this solution.
3.4.3 Discussion
As we stated above, we implemented the algorithms to see how they might behave
in practice. Because there were no prior provably good approximation algorithms
for 2D optimal kinodynamic planning, ad hoc methods previously had to be used
8"Run time" is the time the process is actually active, i.e., computer time. Total elapsed
time" is how much real time passes on a wall clock; it thus includes time the process is idle, ?.g.,
swapping.
9Total elapsed time averaged about three hours, but this varied greatly, since the machine
was shared.
loo
Figure 3.8: A trajectory found by our implementation running Algorithm 2. Here,
the world size is 3.5 by 1.9 units. ? = 0.6, Co = 0.2, c? = 0.5, a = 0.2, and v = 0.3.
Start is at lower left, goal at upper right. (The "spikes" indicate the velocities).
Our implementation took 4 minutes and 32 seconds of run time on a 32Mbyte
SparcStation2 to plan the trajectory above.
101
(a)
v
v
v
v
v
v
v
v
v
v
v
Figure 3.9: Near-optimal trajectories that were found by two versions of our im-
plementation running Algorithm 3. If we choose to minimize (3.17) as a secondary
performance measure, the implementation returns the solution in (a), which is less
"curvy" than the solution in (b).
102
(a)
(6)
4
Th
Figure 3.10: Trajectories from previous figure, with commanded accelerations.
Thus, this figure represents the "plan" of commanded forces. The "diagonal"
accelerations correspond to L?-norm (a, r)-bangs that are flilly-saturated in along
both the x and y axes.
103
to argue the near-optimality of trajectories generated by other algorithms. Before
our implementation, we had no way to study what provably good kinodynamic
solutions look like without first having to solve problems by hand.
Despite the slowness of the our current implementation, we do not believe that
the algorithms, Algorithm 2 in particular, are inherently impractical. Allowing
solutions that take at most time (1 + ?)Topt instead of Topt, where Topt is the time
for an optimal kinodynamic trajectory, results in a larger timestep size for a given
set of kinodynamic bounds. This dramatically reduces the size of the reachability
graph and the running time. Later, in Chapter 5, we use two approximation
parameters es and CT to describe the quality of approximation in terms of safety
and time-optimality separately; this enables the user to trade off trajectory safety,
trajectory time, and planning time to a certain extent.
In a practical implementation we could exploit parallelism at two levels of our
algorithms. At a high level, since the TC-grid is a low-degree directed graph, it can
be explored using parallel search techniques. Because of the spatial properties of
the reachability graph and its underlying regular structure, we expect reasonable
speedups on both SIMD and MIMD machines when the number of processors is
appropriately smaller than the size of the TC-grid. At a lower level, we observe
that checking the safety of (?a, ?)-bangs can be done in parallel, since detecting a
possible collision with one obstacle face is completely independent of checking for
collisions with other faces.
One way in which we have experimented with the implementation is in the use
of a secondary performance measure. In general, there may be many minimal-
time (--Hu, r)-bang trajectories from a vertex S* to a vertex G*, and the algorithm
as described in previous sections might find any one of these. These trajectories
can differ from each other in many properties, such as homotopy class, maximum
curvature, and average speed. Since the algorithms each return only one trajectory,
we would like to have a secondary optimization criterion that yields trajectories
that are in some sense "good". One quantity of interest is
loT IIa(t)Ilidt.
(3.17)
Minimizing this quantity as the secondary optimization criterion appears to pro-
duce trajectories with minimal chatter. (Chatter is a potential problem with "bang-
bang" trajectories.) More importantly, it also seems to select trajectories that are
"globally the least curvy" and that agree with our intuition of "good trajectories"
104
(Compare the trajectories in fig. 3.9.)
To minimize (3.17) we associate a unit "fuel cost" with axial bangs and a 2-unit
cost with diagonal bangs. Let Fa denote the total fuel cost of a trajectory Fa. The
total cost of a trajectory combines time with fuel consumption by lexicographic
ordering on time x fuel-cost: C(Fa) < C(Fb) if (a) Ta < T6 or (b) Ta Tb and
Ea < E6. At the start of any timestep n, the implemented algorithm knows a min-
fuel min-time (a, r)-bang trajectory to each state it has found. The implementation
finds a min-fuel min-time (?a, r)-bang trajectory to a state Y in the breadth-first
search frontier for the stage by extending a previous min-fuel min-time trajectory
with an (a, r)-bang to reach Y with minimal total fuel cost. Thus, for a given
problem, our implementation will return minimal-time (?a, r)-bang trajectory with
the lowest fuel cost.
We can, in general, incorporate a secondary cost function L using lexicographic
ordering when the following is true of L: ifF = F1*F2, then L(F) L(F1)tL(F2),
where "*" denotes trajectory concatenation. This follows from the mapping of
trajectory segments to graph edges. However, we will only be guaranteed to find
the (?a, r)-bang trajectory Fq with minimal L-cost.
Let T denote the class of minimal-time (?a, r)-bang trajectories from S* to G*,
and let Let FLopt be the time-optimal ?-safe trajectory with minimal L-cost. We
would like to compare L(Fq) to L(FLopt)? but for an arbitrary cost functional L,
the lexicographic ordering technique does not imply any particular relationship.
To prove for some predicate 9? that 9?(Fq, FLopt)? we need to extend the tracking
lemma to state that if FLopt is an L-minimal time-minimal trajectory, then there is
a trajectory Fq in T such that 9?(Fq, FLopt) General techniques for proving such
lemmas would be of considerable interest.
3.5 Summary
In this chapter we again considered optimal kinodynamic planning for Cartesian
robots obeying L? dynamics bounds. While we retained the problem formulation
and made only minor modifications to the algorithm from the previous chapter, we
proved stronger tracking lemmas to improve the complexity and accuracy bounds
of the new algorithms significantly. Exploiting the decoupled dynamics and decou-
pled dynamics bounds of Cartesian robots, we are able to (a) tighten the complexity
bound to 0 (dcdN (1E)3d), where (c = cB or cc) is a constant polynomially depen-
105
dent on the robot dynamics bound and the environment diameter, and (b) show
that Algorithm 3 finds a trajectory taking at most time Topt (the optimal time)
instead of merely time Topt(l t ?).
We prove the vital tracking lemmas by construction. Given a velocity function
Vr obeying the relevant acceleration bounds, we first show the existence of velocity
functions that are realizable by (ci, r)-bangs, bound the given velocity function
Vr above and below, and track it to within rn--H??. Subsequent constructions prove
the existence of a velocity functions whose integrals track the integral of the given
function to the required tolerances while staying between the "bounding functions."
Similar techniques will be used in proofs in later chapters.
We have described a preliminary implementation and reported the results of
some simple experiments. This is the first implementation of polynomial-time,
provably-good approximation algorithms for kinodynamic planning. While the
current implementation runs slowly, eventually an improved implementation may
be reasonable for practical off-line motion planning.
Chapter 4
Kinodynamic Planning With
Coupled Dynamics Bounds
We have so far discussed optimal kinodynamic planning for a class of robots where
the dynamics constraints allow us to reduce most of the necessary analysis to a
simple one-dimensional system. Specifically, for Cartesian robots with L? dy-
namics bounds, the acceleration and velocity bounds act independently in each
coordinate direction. Furthermore, the set of instantaneous accelerations allowed
by the dynamics constraints is constant.
We extend the results from previous chapters to robots that obey more general
dynamics laws and dynamics bounds in particular, d-link 3D robots with full
rigid-body dynamics amidst obstacles. Specifically, we describe polynomial-time
approximation algorithms for Cartesian robots obeying L2 dynamics bounds and
for open-kinematic-chain manipulators with revolute and prismatic joints. (See
figures 1.3 and 1.4.) The correctuess and complexity of these algorithms rely on new
trajectory tracking lemmas for robots with coupled dynamics bounds. We obtain
o (cdN(d t log N)(?)6d--Hl) asymptotic bounds, where N measures the geometric
complexity of the problem and c is a robot-dependent constant.
106
107
4.1 Kinodynamic Motion Planning for Robots
With Coupled Dynamics Bounds
4.1.1 A More General Kinodynamic Planning Problem
We now reformulate the optimal and e-optimal kinodynamic planning problems to
accommodate a wider class of robots.
In a kinodynamic planning problem, a robot with d degrees of freedom must
move from a start state 5 = (s, s) to a goal state G = (g, g) while avoiding a set of
obstacles and configuration (e.g. joint) limits; these are the kinematic constraints.
We will denote robot configuration space by 0, and its phase space, the robot
state space, by TO. A robot motion taking time Tf can be specified by a twice-
differentiable map p : [O,Tf] H C This map is the path of the motion. The Ira-
jectory of a robot motion is the map V [0, Tf] TO given by F(t) (p(I), p(t)).
Thus, a motion is determined by an initial state (po, v0) and an acceleration func-
tion a = We denote the position and velocity components of a subscripted
trajectory Fr by Pr and Pr, respectively.
The robot motion is governed by a dynamics law, which relates applied gen-
eralized forces f to states, accelerations, and forces G(p) induced by gravity. For
open kinematic chains [AS86,11o183]:
f(t) = ?(P(I))a(t) + [PT(t)c(P(t))P(t)] + G(p(t)).
(4.1)
M(P(t)), the robot inertia tensor, is orthogonal, symmetric, and positive-definite.
C(p(t)) is a tensor of rank three,and [pT(t)c(p(t))p(t)] denotes the column vector
in which
[PT(t)C(P(t))P(t)lt --H
C?(P(t))5,k --H
PT (t)C?(p(t))p(!),
where
(4.2)
(See [AS86] for a derivation.)
We call a robot whose inertia tensor is constant and whose dynamics law sim-
plifies to
f(t) = Ma(t)			(4.3)
a Cartesian robot.
108
A robot motion p obeys dynamics bounds (?v,f) if for all times I the joint
velocities p and the applied generalized forces f obey the following at each joint i:
I%(I)I			<			Vj, and			(4.4)
Ifi(t)I			<			fi,
These bounds imply global acceleration bounds a, and we define
Amar = maxa?.
For a Cartesian robot, acceleration can be substituted for force, and all these
bounds are usually given in an L?-norm. We consider the L2-norm here, whereas
in previous chapters we used the L?-norm. The dynamics laws and dynamics
bounds that apply to a robot are its dynamics constraints. (See figure 1.4.) Note
that when the norm makes a difference, we will specify it, except in the L?-case.
Generally, the problem parameters must include an encoding M of the robot's
dynamics equation.1 In addition, there must be an encoding 0 of the workspace
obstacles. An instance of the general kinodynamic planning problem, then, is a
tuple )c = (O,S,G,f,v,M). An eract solution is a a trajectory F such that
F(0) = S, F(Tf) = G at some time Tf, and F obeys the kinematic and dynamics
constraints. Thus, the corresponding map p must avoid all obstacles and respect
(4.1) and (4.4). The time for solution 1' is simply Tf. The time-optimal kinodyna-
mic planning problem is to find a minimal-time solution, whid? is represented as a
suitable encoding of the start state and the acceleration function a.
We assume that the robot and obstacles are polyhedral, and that they have been
mapped to configuration-space (c-space) obstacles with geometric complexity N in
the configuration space, i.e., the c-space constraints consist of N facets (c-space
surfaces) as in [LP83,DonS7,CDS8] Free space is the complement of the c-space
obstacles in C. For a polyhedral robot of geometric complexity m and a set of
polyhedral obstacles with geometrical complexity n, the number of configuration
space constraints2 N = O(rn(m + n)) [LP83J. Finally, we assume that all linear
degrees of freedom are above bounded by a length 1.
`Since the general form of the equation is given by equation (4.1), this involves supplying an
encoding of the matrices M(p) and C(p) and the vector G(p) in terms of configuration p E C.
Thus, for a given link of a manipulator, A4 include constants such as the mass, center of mass,
inertia, and the distance and relative orientation between joint axes. For an example of what
must be encoded in M, see section C.1.1 in Appendix C.
2An arm must avoid self collisions.
109
4.1.2 Safe and Near-Optimal Kinodynamic Plans
A theoretically time-optimal solution may require unrealizable precision in control
or sensing to execute. Because we wish to concisely associate a deviation from a
safe trajectory with a decrease in safety, we again require a safe solution to avoid
obstacles by a margin that is a strictly-positive affine function of speed.3 As before,
we define a trajectory to be ?-safr if and only if for all times tin [0, Tf], there
exists a ball about p(t) in free space of radius
Co + cilip(1) ,			(2.3)
where Co > 0 and ci > 0 are problem parameters. For any scalars Co > 0 and Ci > 0,
we define an optimal (6v-saft) kinodynamic solution to be a ?-safe solution whose
time is minimal. An instance of the optimal (safe) kinodynamic planning problem
is a tuple (O,S,G,f,?v,l,M,co,q). f, v, Co, and Ci the kinodynamic bounds.
Together, the kinodynamic bounds and M are the kinodynamic specifications of
the robot.
As before, the quality of a solution is measured in terms of an approximation
parameter e. A solution Fq : [O,Tq] H TC is e-optimal if (a) Fq(O) and Fq(Tq) are
within 0(e) tolerances of S and G, respectively; (b) Tq < (it e)Topt, where Topt is
the time of an optimal solution; and (c) Fq observes ?(co, Ci )-safety, as in equation
(2.4). (See section 2.1.1.)
4.1.3 Statement of Results
4.1.3.1 Robots With Coupled Dynamics Bounds
A robot system obeys coupled dynamics bounds if (a) its dynamics equations can-
not be separated into the dynamics equations of d independent one-dimensional
systems or (b) or there is no fixed coordinate transformation such that the velocity
and acceleration bounds in each axial direction are independent of the bounds in
all other axial directions. (See figure 1.4.)
We describe provably good approximation algorithms for the optimal (safe)
kinodynamic planning problems for two classes of robots with coupled dynamics
3Another analysis suggests the use a quadratic function of speed, or more generally, one that
contains a quadratic form vTA(x)v, where A(x) is symmetric and positive definite. Assuming
bounded derivitives, we can use vector calculus to derive an ?-sale tracking tolerance that is ?(?)
in both configuration (position) and velocity, similar to that in the Safe Tracking Lemma 2.2.4.
110
bounds: (a) Cartesian Robots with L2 dynamics bounds and (b) open-chain manip-
ulators with revolute and prismatic joints. Somewhat surprisingly, the algorithms
for these two different problems are based on similar underlying principles. Given
a problem instance and an approximation parameter e, these algorithms will find
an e-optimal solution if a ?(co, ci)-safe solution exists.
These algorithms run in time polynomial in the geometric complexity N of
the configuration space obstacles and in the resolution (l?) The constant in
the cd? (cdE) factor of the overall complexity (see Table 1.5.1) depends on the
kinodynamic specifications of the robot, and thus this factor is constant for a
given robot. Our algorithms are e-approximation schemes that are fully polynomial
in the combinatorial complexity of the geometry and pseudo-polynomial in the
kinodynamic specifications.
Formally stated, we show the following:
Theorem 4.1.1 Let a and v be velocity and acceleration bounds, respectively. Let
(0, S, G, a, v, 1, c?, ci) be an optimal kinodynamic planning problem for a Cartesian
robot obeying L2 dynamics bounds, Let 0 < e < 1.
Suppose there is a ?(c0,c1)-safr trajectory from S to G taking time Topt. Then
the algorithms in Sections 4.2.1.1 and 4.2.1.2 each find a (1 --H e)?(c0,c1)-safr
trajectory taking time at most Topt(1 + e) from some = (s*,s*) to some --H
(g*, g*) such that S* and G* are e-close to S and G, respectively.
The asymptotic running time of the algorithms is 0 (cd?p(N, e, d) (lE)6d?1),
where N is the geometric complexity of the c-space obstacles, c? is a constant
dependent on the kinodynamic specifications, and p(N, e, d) is the time-complexity
of checking the (1--He)?v(co,ci)-safety of one of the "prn'mitive" trajectory segments
the algorithms consider.
Theorem 4.1.2 Let (0,S,G,f,v,M,l,c0,c1) be an optimal kinodynamic plan-
ntng problem for an open-chain manipulator with revolute and/or prismatic joints.
Let 0 ? e < 1.
Suppose there is a ?(co, ci)-safe trajectory from S to G taking time Topt. Then
the algorithms in Sections 4.2.1.1 and 4.2.1.2 each find a (1 --H e)6v(c0,ci)-safe
trajectory taking time at most Topt(1 + e) from some = (s*,s*) to some --H
(g*,g*) such that S* and G* are e-cThse to S and G, respectively.
The asymptotic running time of the algorithms is 0 (cd?p(N, e, d) (??l)6d?1)
where N is the geometric complexity of the c-space obstacles, c? is a constant
111
dependent on the kinodynamic specifications, and p(N, e, d) is the time-complexity
of checking the (1 --H e)?v(co, ci)-safety of one of the "primitive" trajectory segments
the algorithms consider.
We have omitted the complexity factors containing kinodynamic specifications
because they are constant for a given robot and because the terms can be very
complicated. Our claim that the complexities are pseudopolynomial in these pa-
rameters is substantiated in in Sections 4.3.1 through 4.3.3, with further detail
given in Appendix C. We express their overall contribution to the complexity
bounds as a factor of cd? (or edE), for constants derived from the kinodynamic
specifications.
In Section 4.3.4 we argue that p(N,e,d) is roughly O(N(d + logN)). Sub-
stitution yields the complexities in Table 1.5.1. As we discuss in that section,
for non-Cartesian open-chain manipulators amidst polyhedral real-space obstacles,
exact collision detection for quadratic paths requires the solution of trignometric
equations that cannot be transformed into algebraic equations using the usual sub-
stitution methods,4 such as [Can86]. Furthermore, for these manipulators, trajec-
tory segments corresponding to constant extremal controls are solutions to systems
of trignometric differential equations, and the trajectories found by the correspond-
ing algorithm are extremal to the accuracy of the representation.5 In both of these
cases, numerical techniques would be used to do safety-d?ecking approximately.
For a choice of c-space coordinates in which the path is algebraic in time and in
which the obstacles are represented as semi-algebraic sets in configuration space,
p(N, e, d) would be O(N log N) for the True Extremal Algorithm (applied to Carte-
sian robots) and to the Near-Extremal algorithm (in general). In the subcase where
the configuration space obstacles are the union of convex polyhedra with geometric
complexity N, p(N, e, d) O(dN).
4Specifically, if the workspace obstacles are semi-algebraic sets and the robot has at least one
rotational degree of freedom, then surfaces of some c-space obstacles might be non-algebraic, e.g.,
trignometric. If path p(t) is quadratic in time, then substitutions (such as Uj = tanO?) to get
semi-algebraic c-space obstacle representations result in path representations that are no longer
algebraic.
5See section 4.2.1.1.
112
4.1.4
Generalizing the Basic Algorithms to Robots with
Coupled Dynamics Bounds
Intuitively, we would hope to extend the approach from previous chapters to robots
with coupled dynamics by choosing appropriate analogues of (a, ?)-bangs and prov-
ing an applicable tracking lemma. Lemma4.2.1, which we call The Coupled Track
tng Lemma, relates a desired tracking tolerance and a dynamics advantage to a
sufficiently small timestep for a class of sets of control primitives. Given a tra-
jectory FT, a tracking tolerance (iix, ?iv), and a set of control primitives that has
a given dynamics advantage over Fr, we can find a timestep T guaranteeing that
some trajectory Fq generated by the control primitives and r will track Fr to toler-
ance (?1z, ?1v) The class of control primitives includes (a) near-extremal, piecewise-
constant accelerations and (b) extremal (possibly time-varying) accelerations that
result from applying piecewise-constant extremal controls. By applying Lemma
4.2.1 we obtain a provably-good kinodynamic planning algorithm that generates
trajectories obeying near-extremal accelerations, and hence, near-extremal con-
trols.
In robotics and control theory there is a family of results on the feasibility
of planning and approximating optimal trajectories using only piecewise-extremal
controls.6 (These results are often called "bang-bang" theorems.) We can use
Lemma 4.2.1 to find a timestep r sufficiently small so that there exists some
piecewise-extremal, e-optimal trajectory having control switchings only at mul-
tiples of r. Unfortunately, the lemma does not yield a polynomial-time approxi-
mation scheme using these trajectories, however, because the number of vertices
in the reachability graph might be exponential in the path length. Fortunately, we
can prove a more robust Lemma 4.2.2, which provides conditions that allow us to
prune the search to a sufficiently small subset of the graph. We can thus show how
to plan provably near-optimal kinodynamic solutions that only employ extremal
controls with no additional cost in asymptotic complexity for a given robot.
6See Section 1.4 for references.
113
4.2 Robots with Coupled Dynamics Bounds
4.2.1 The Algorithms
We describe two general algorithms for finding near-optimal kinodynamic trajec-
tories for Cartesian robots with L2-norm dynamics bounds and for open-chain
manipulators. The first algorithm searches a reachability graph corresponding to
piecewise-constant, extremal forces and torques, and we will refer to it as "the
True-Extremal Algorithm". The second uses piecewise-constant, near-extremal
accelerations, and we call it "the Near-Extremal Algorithm". We present both al-
gorithms in outline form. Derivations of key parameters are described in Sections
4.3.2 and 4.3.3.
Since we will use the notion of a ?slowed-down" trajectory often in this section
we now formalize two definitions.
Definition 4.2.1 Let Vr : [0, Tf] H TC, and suppose thai e ? 0. Then we define
the e-time-rescaled trajectory F'r : [0, (1 t e)Tfl H TC asfollows:
F'rY)			(pr(1+1e)?(1+1e)pr(1s1e)).			(4.5)
We wilt be extensively comparing dynamical properties associated with states in
rescaled trajectories.
Definition 4.2.2 Let X (x,x) c TC, and suppose that e > 0. Then &time-
rescaled state X' = (x',x') is defined by:
x = x
___			(4.6)
4.2.1.1 The ?ue-ExtremaI Algorithm
Although the correctuess of the True-Extremal Algoritlim is harder to prove than
the correctness of the Near-Extremal Algorithm, the algorithm itself is easier to to
describe.
We first describe the control primitives used to generate the reachability graph.
Let 1! be the set of generalized (control) forces that are extremal with respect to
given bounds f. (We can define similar sets for Lp bounds J and ?a.) Given a
scalar discretization parameter ? > 0, we say that UTt C ti is a a Ti-discretization
114
of the control extremals if for every Ua ? 1! there is some u6 Ei U--H? such that
-21j?ua --H Ublico < Th (See figure 4.1.) As along as jt is small enough, our algorithm
may choose any such U--H?, so we henceforth refer to U--H? as the ?-discretization of
control extremals.
The application of a member of U--H? for duration T is called a true (jt, r).bang7
Recall that the dynamics equation maps states and generalized forces to accelera-
tions. From equation (4.1), the acceleration is given by
a(p,p,f) = M?1(p) (f [pTc(p)p] --H G(p)) (4.7)
Thus, a true (?, r)-bang and a state X together determine a trajectory segment of
duration r starting at X, i.e., the solution to (4.7) with (p(0),p(0)) = X, and f
given by the (?, r)-bang. If this trajectory ends at state Y, then we say that there
is a true (?, ?)-bang from X to Y. We will sometimes use "true (?, r)-bang" to
refer to the trajectory segment generated by a bang when the meaning is clear.
For an L2 Cartesian robot or open-chain manipulator, the number of states
reachable from a root vertex S* via a sequence of m (1 --H e)?-sale (ij, ?)-bangs
can, in general, be exponential in the path length m; the total number states
reachable via an infinite number of such bangs can be unbounded. Therefore, we
will apply (bz, bv)-bucket pruning, a spatial hashing technique, in the search. (See
figure 4.2.) In (bx, bv)-bucket pruning, TC is divided into voxels of diameter bx
in spatial dimensions and bv in velocity dimension. When the graph search finds
a vertex in a voxel in which another vertex has previously been found, the edges
from the new vertex are simply not explored, and we say they are pruned from
the search. (Compare this to the search in [Don87], figure 31 on page 315.) Since
the state space is bounded and the reachability graph has a 0((?)?d) branching
factor, the size of the subgraph searched is O((itbxbv)?d).
?ue-Extrema1 Algorithm Outline
Given a kinodynamic planning problem instance (0, S, G,f,v, M, Co, ci) for an
L2 Cartesian robot or an open-chain manipulator, the True-Extremal Algorithm
does the following.
1. Chooses the root vertex S* by e-rescaling S, i.e, s* = s and s* = (i+ic) s
2. It chooses a timestep T and a control discretization T? that are both Q(e) and
chooses a ?-discretion of the control extremals.
7The "true" distinguishes it from uses of "bang" which refer to controls that are not truly
extremal, such the "(?, r)-hangs" in later sections of this thesis and I) [DX90a,DX90b]
115
1!
Il-It
ipIt,
H
-`I
Figure 4.1: An extremal set of controls Il and two Ti-discretizations of 1!: IlIt and
116
3. Let ? be the reachability graph rooted at S* whose edges are (1 --H e)?(co, ci)-
safe (?, r)-bangs. The algorithm searches for the shortest path from S* to
any vertex that is within (iiz, nv) of (g, j+1?g) n?, and ?7v are both ?(e). The
search is done breadth-first with (bx, bt,)-bucket pruning in which br is ?(e3),
and bv is ?(e2). The atgorithm only constructs the part of the graph that it
actually searches.
4. The algorithm returns the trajectory that corresponds the shortest path in
the reachability graph (from step 3) if a ?(co, c1)-safe solution exits.
The acceleration functions associated with true (?u? r)-bangs beginning at a
given state X will be important to our discussion. We call these functions the true
(?, r)-bangs at X, and their set will be denoted by TL(X, 7). A true (--H?, r)-bang tra-
jectory is a concatenation of true (?, ?)-bangs. If Fr is a true (T, r)-bang trajectory,
then during each open time interval (nr, (n + 1)r), ar(t --H nT) ? ?Y(Vr(nr), 7).
Thus, the controls of a trajectory found by the True-Extremal Algorithm are
constant and extremal over each individiual timestep, while the acceleration will
be extremal but may vary with time, according to the integration of equation
(4.7). This contrasts with the trajectories found by the Near-Extremal Algorithm,
whose accelerations are constant and near-extremal over each timestep, but whose
controls may be time-varying, as given by equation (4.1).
Note that for robots with revolute joints there is generally no closed form for
trajectory segments corresponding to true (jt, ?)-bangs. However, using an rth-
order numerical integration procedure will yield trajectory segments correspond-
ing to controls within O(rT) (and therefore within O(er)) of being constant and
extremal. Thus, as long as e is sufficiently small, we can use say r 4 and
Runge-Kutta numerical integration.
4.2.1.2 The Near-Extremal Algorithm
The Near-Extremal Algorithm builds a reachability graph using trajectory seg-
ments corresponding to constant near-extremal accelerations each lasting one time-
step. These accelerations and a root vertex are chosen so that the size of the reach-
ability is automatically bounded. We now describe these accelerations and outline
the algorithm.
A positive vector ? ? ?d determines a grid that discretizes the acceleration
space ?d Precisely, let ui be the grid-spacing in dimension i, and let the grid be
117
L			J
r
Figure 4.2: Search pruning. During the search, an edge (trajectory segment) to
vertex (state) X is found, but a previously found vertex Y is in the same bucket.
Consequently, edges from X are pruned from the search. (Compare this to the
search in [Don87], figure 31 on page 315.)
118
aligned with the coordinate axes so that the origin 0 is a gridpoint, i.e., it lies at
one of the interstices of the grid. We call the set of gridpoints the ?-grid. For now,
suppose timestep r has been chosen, and suppose that the set of accelerations has
been discretized. Given a state X Ei TC, let Ax denote the set of instantaneous
accelerations possible at X under the dynamics constraints.
Let Ax c Ax denote the set of constant accelerations8 possible for trajectories
of duration T beginning at state X:
Ax (a ? Ax a ? fl A?(x,a,t?t
tE[O,r]
where F(X, a,.) is the trajectory that results in applying constant acceleration a at
state X. Then, a "constant acceleration" analogue of the set of true (?, r)-bangs
would be the set of ?-gridpoints that lie within a ?-grid-spacing of the boundary
aAx of Ax. Unfortunately, it appears that this set might not be easy to enumerate
in general, because of non-linear dynamics and the apparent "fixed-point" nature9
of Ax. For the algorithm we will therefore define sets of near-extremal constant
accelerations using "conservative approximations" of the sets Ax.
Let Dx c TC denote the set of states that can be reached from X within time T
without violating the dynamics constraints. Then one conservative approximation
is
Ax fl Ay c Ax.
YEDx
(4.8)
This set still might be difficult to compute when the robot dynamics are non-linear,
since the intersection is taken over a set of states.
The dynamics and dynamics bounds determine a global bounds on the magni-
tudes Ax and Av of the possible position and velocity changes during a duration of
length r. Using these bounds, we can compute an analogous "acceleration space
bound EA, which has the following property: if a? E Ay for some Y within
(Ax,Av) of X, then there is some aX ? Ax such that a? --H ax ? EA. (See
Section 4.3.3.1 and Appendix C.)
We now define:
Ax* =			fl			(a'i Ax aa' C Ax,a= a1+Aa?			(4.9)
AaEBE? (0)
to
8In this section (4.2.1.2) we will treat constant accelerations as the vectors that correspond
constant, vector-valued functions.
9Define Frobx(A) fla?? Atc[orlAr?x,at? Then Ax is the solution of Fro6x(A) = A.
119
where B6(y) denotes the ?ball about y. Since Ax is a parallelpiped or a closed
L?-norm d-ball, this set has a simple geometry. Thus,
Ax* c Ax c Ax c Ax,
with equality for Cartesian robots. (See figure 4.3.)
It
TC
acceleration
x
Figure 4.3: Sets of accelerations. Ax is the set of possible accelerations at state X.
Ax is the set of accelerations that can be maintained for duration T. Ax = A Ay,
where the intersection is taken over all Y Ei Dx, is a conservative estimate of
this set. Ax*, used by the Near-Extremal to obtain the (It, ?)-extremal shell of
accelerations ?(X, r) is "geometrically computable". a is a member of TL(X, T).
This figure only shows set relationships; the actual corners of Ax* would be beveled.
We will call Ax* the set of conservatively ?-feasible constant accelerations at X.
Our algorithms will treat10
120
(4.10)
as the set of "most-extremal" constant accelerations feasible for a trajectory of
duration T starting at state X.
We now define the set of (?, r)-extremal shell of accelerations at X to be given
by
?-grid ?a' ? Ax*Vi, jaj --H a'?I ? jiji. (4.11)
This notation and this term will also refer to the corresponding set of constant
acceleration functions.
The application of a (?, r)-extremal acceleration for duration T is called a
(i, r)-bang." Terminology concerning (?a, r)-bangs and true (--H?, r)-bangs will be
analogously extended to (i, r)?bangs. For example, if a (?, ?)-bang results in the
transition from state, X to state Y, we say there is (j,?)-bang from X to Y.
Suppose that for a state S* and in each dimension i, the acceleration dis-
cretization ?j divides the state velocity s?* Then all states reachable from S* via
a sequence of (?, r)-bangs lie at the interstices of an underlying grid covering TC,
with a grid spacing ?f2 in position dimension z and ?jT in velocity dimension z.
Thus, a ?-gnd, a timestep r, and a choice of origin in C induce an underlying
regular TC-grid. If s* is the root vertex of a reachability graph, then the number
of its vertices is bounded above by the number of TC-gridpoints.
Near-Extremal Algorithm Outline
Given a kinodynamic planning problem (0, S, G,f,v, 1, M, co, ci) for an L2
Cartesian robot or an open-chain manipulator, the Near-Extremal Algorithm does
the following.
1. Chooses a timestep r and an acceleration space discretization ?. r and all
are ?(e). If the jth joint has configuration space ?1 (as revolute joints
without limits do), then `ji must be chosen so that KjJi?T2 47r for some
integer Ki
2 It then chooses the root vertex state S* such that s* s and us* --H i$cSII ?
min? ?jT.
10Recall that "?" denotes `the boundary of".
11For historical reasons. See [DX9Oa,DX9Ob]
merely close to being extremal.
Strictly speaking, they are not bangs and are
121
3. Let g be the (?, r)-reachability graph rooted at S* whose edges are (1 --H
e)?(co, ci )-safe (tt? r)-bangs. The algorithm searches for the shortest path
from S* to any vertex that is within (7iz, nv) of (g, 1+'?g) ??, and n? are both
?(e). The search is done breadth-first.
4. If a ?v(co, ci )-safe solution exists, then the algorithm returns the trajectory
that corresponds to this path in the reachability graph.
4.2.2 Key Concepts
4.2.2.1 Overview
The Safe Tracking Lemma (2.2.3) from Section 2.2.3 relates co, ci, and e to a family
of tracking tolerances. Specifically, given Co, ci and e, the lemma provides a set
of tracking tolerances (?ix, 1?v) such that if a trajectory Fa is ?(co, ci)-safe and Fb
tracks Fa to tolerance (nz, liv), then Fb will be (1 --H e)&(co, ci)-safe. This lemma
is briefly presented again in Section 4.2.2.2, and it ensures that the algorithms can
choose sufficiently small e-safe tracking tolerances lix and liv that are
R?ecall that for a trajectory V, V, denotes V e-time-rescaled, as in (4.5). To
show the correctness of each algorithm we show that if a given problem has an
optimal solution Fopt taking time Topt, then the algorithm's choice of root vertex
state S*, timestep T, and discretization ? (or jt) guarantee it will at worst find
a graph trajectory Fq that tracks V'opt to tolerance (lix, lit') Fq will therefore be
(1 --H e)?(co, ci)-safe and approximate G' to within 0(e) at time (1 + e)T0pt. Since
e < 1, S' and G' are within 0(e) of S and G. It follows that Fq(O) and Vq((1+e)Topt)
will also be within 0(e) of S and G.
Following a general analysis similar to that found in the earlier chapters, we
first observe that if a Cartesian robot trajectory Fr respects L2 dynamics bounds
a and v, then the e-time-rescaled trajectory V'r will respect bounds ?a' and ?v' that
are smaller than a and v by an ?(e) factor. This is also true for open-chain ma-
nipulators as long as the force bound f is great enough to overcome gravity in
all configurations when the robot is stationary. In Sections 4.2.2.3 and 4.2.2.4 we
generalize from feasible instantaneous accelerations to state-dependent sets of ac-
celeration functions and formalize an appropriate notion of acceleration advantage
over a time interval.
The Coupled Tracking Lemma (4.2.1) presented in Section 4.2.2.5 relates such
an acceleration advantage ?,, a maximum acceleration, and a desired tracking tol-
122
erance to a timestep r. Using this lemma we can choose ?(e) parameters jtj, ?, and
T that guarantee the existence of graph trajectories that will track F' to the
opt
tolerances obtained via the Safe Tracking Lemma. For the Near Extremal Algo-
rithm, it is adequate to find conditions that guarantee that some graph trajectory
will track an e-time-rescaled optimal trajectory F?0pj closely enough. In Section
4.2.2.6 we present the Robust Coupled Tracking Lemma (4.2.2), which gives condi-
tions ensuring that (bx, b?)-pruning during a breadth-first search of the reachability
graph will not eliminate trajectories the True-Extremal Algorithm needs to find.
Proofs for Lemmas 4.2.1 and 4.2.2 are presented in Section 4.3.1. Details of
how the algorithms choose the reachability graph parameters are given in Sections
4.3.2 and 4.3.3.
4.2.2.2 Safe `b?acking
Assuming that a optimal trajectory Fopt exists, we must determine a tracking
tolerance that guarantees that any trajectory tracking Fo'pt to that tolerance will
be (1 --H e)?(co,ci)-safe. kecall the definition of a ?-tube from section 4.1.2, and
recall the Safe Tracking Lemma:
Leinma 2.2.3 (The Safe `I??acking Lemma) Suppose that ? is specified by co
and ci and that Fr is a 6v-safr trajectory. Let 0 ? e < 1, and let %` (1 --H
Then there exists a tolerance (`lx, `lv) such that for any trajectory Vq, the following
hold:
1. If Fq tracks Fr to tolerance (`lx,'lv), then Fq is ?,-safe.
2. Furthermore, for any positive ,3, the following choices suffice:
cp(
`lv			--H			c1(i--H?+P			(2.10)
`lx			?			P'lv'
It should be noted again that the lemma is independent of norm, as long as
a particular norm is consistently used, and that distances and velocities are in
TC. If we wish the safety margin to correspond to the distance between the
physical robot and the obstacles in real space,'2 then Tix can he divided by the
maximum modulus of the forward kinematic map. Similarly, if we wish the velocity
to correspond to the velocity of point on the physical robot, then `lv can be divided
12This is distinguished from the margin by which c-space obstacles are avoided, which is more
natural to use in our discussion.
123
by maxp,p IIJ(p)pII, where J(p) is the manipulator Jacobian. Let us denote these
divisors by Pmax and Jmaz, respectively. Then we would (conservatively) require:
cp(
71v --H Jrnax(ci(1--H?)+P)			(4.12)
? Pmax(cPlC(Olffi()+p)
By choosing p = 1 we see that TIz and ?? can both be 0(e). The algorithm
chooses P to maximize the timestep and thus minimize the overall complexity, as
in Section 3.3.4.1.
Given Co, Cl, and e, we will call a tolerance (?x, ?iv) that obeys (4.12) a physical
e-safe-tracking tolerance.
4.2.2.3 Time-Rescaling and Instantaneous Acceleration Advantages
For an L? Cartesian robot, if trajectory Fr obeys dynamics bounds a and v,
then the e-time-rescaled trajectory F'r obeys dynamics bounds and l+V( For
any d-vector a of 1's and --H1's there is a d-vector a of l's and --Hl's such that if
IIa(t)IIoo ?			, then for each dimension t
aj(a7a--Ha?(t))> a(2e+e2)
(l+e)2
Therefore, a trajectory obeying (?a, r)-bang control can always out-accelerate the
e-time-rescaled trajectory Fr' by an			in any direction over the en-
margin
tirety of a timestep, as long as this would not violate the velocity bound. We can
immediately reduce the analysis for L? Cartesian robots to the one-dimensional
case, where we exploit this acceleration advantage to relate the timestep T to how
closely some (?a, r)-bang trajectory is guaranteed to track vtT (in the absence of
obstacles). We will use the acceleration advantage together with the dynamics
bounds and Lemma 2.2.3 to choose a timestep T.
Before we introduce a notion of acceleration advantage appropriate for sets of
acceleration functions, as used in the Coupled Tracking Lemma, we will describe
a generalization of "instantaneous acceleration advantage" to both L2 Cartesian
robots and open-chain manipulators.
Recall that the robot motion obeys the following equation
f(t) = ?(p(t))a(t) + [pT(t)C(p(t))p(t)] + G(p(t)). (4.1)
124
Suppose a trajectory Fr obeys the bounds f. Recall that `r is Fr ?-time-rescaled
(4.5). We find that at time (1 + e)1 [Hol83]:
fr'((1 + ?)t)			fr(1)			2?+?2
(1 + ?)2 + (1 + E)2G(Pr(t))			(4.13)
This immediately means that F'r obeys some tighter bounds r if the robot is Carte-
sian or if there is no gravity. For an open-chain manipulator, ,r obeys tighter
bounds r as long as the bounds f are large enough for the robot to be held sta-
tionary in the presence of gravity in all configurations. In particular, suppose that
the forces necessary to balance gravity obey the bound (1 --H ?f)f, so that the scalar
?? is the "advantage over gravity". Then F'r obeys the bounds
--H, _ 1+(1--H??)(2?+?)--H
f --H			(1 + ()2			f			in addition to			(4.14)
vi			= (1+1?)v.			(4.15)
Then, for every degree of freedom 2,
??(2?+?2)			___
7?--H fi? > (1+ ?)2 fi >			(4.16)
Recall that Ax denotes the sets of feasible instantaneous accelerations at the
state X under the force bound f. Let A'x denote the corresponding set under the
force bounds r, and let fmin = min? k Recall that for any y C
II?II? < 11Y112 < ? I?I (4.17)
Now, because M(x) is orthogonal (4.1), (4.16) implies that for all a E Ax and
a' e Ax',
Ija --H a'II? > 4??)x?' (4.18)
where Amaz is the maximum eigenvalue of M(x) taken over all of C. (4.18) is also
true for a Cartesian manipulator obeying L2-norm dynamics bounds; in this case
it follows from (4.3) that Kf = 1 and fmin = f. The L2 acceleration bound case is
trivial, since it is equivalent to setting M to the identity matrix.
In all three cases, Ax' lies entirely inside Ax by some ?(?) margin. In particular,
we can always find a scalar tct > 0 such that for any a'i A'x and any d-vector a of
1's and --H1's there is an a ? ?Ax such that for all degrees of freedom z,
--H a?) > ?j.
Hence, we say that Ax has a &j-ins[antaneous acceleration advantage over
125
Axt			I
`I,
[1=
Figure 4.4: Allowable accelerations for the L2 Cartesian robot at any state X. Ax
obeys a, A'x obeys wm%o+ For any a E A'x, in each direction given by a vector of
l's and --Hl's, there is some a E Ax that has a tcI advantage over a; the intersections
of the dotted lines and the outer circle (OAx) represent "witnesses". Thus, Ax
has a uniform ?t advantage over Ax' for any duration T (as long as the velocity
bound is not violated). If the actual uniform advantage is large enough and all
are small enough, then the (?, ?)-extremal shell of accelerations Tt(X, T) (see
equation (4.11)) has a uniform ?j advantage over
126
4.2.2.4 Acceleration Functions and Uniform Advantages
We now introduce a form of acceleration advantage useful for comparing sets of
acceleration functions. We will say that a trajectory Fr respects a set Y of accel-
eration functions in interval [t1, t2] if there is an acceleration function a Ei Y such
that ar(t) = a(t --H 1i) for all t ? [1i, 12]. Suppose that Y is also a set of acceleration
functions, and suppose that for every a Ei Y and each d-vector a of l's and --Hl's,
there is some a E Y such that for all t E [0, r]
%?a?(s)ds
a?
(4.19)
Then we say that Y has a uniform cj advantage over Y for duration T. (When
"for duration r" is omitted, it is implied.)
Because the set of feasible accelerations for a Cartesian robot is state-invariant,
we can find and acceleration advantage cj and discretizations ?i, ji that are
and such that both the (?, r)-extremal shell of accelerations Tt(X, T) and the set
of true (jt, r)-bangs TL(X, r) at every state X have a uniform cj-advantage over the
acceleration of ?opt(t) at every time I for any duration T (as long as the velocity
bound is not violated).
We now generalize the notion of a set of instantaneous accelerations Ax to a
mapping
A : TC H f? Y is a set of acceleration functions)
from TC to sets of acceleration functions for a given robot. A(X) is defined to be
the set of acceleration functions obeying the dynamics bounds f and V for a robot
motion (in the absence of obstacles) beginning at state X. Thus, a? ? A(X) if
and only if there is a trajectory Fr that at state X and that obeys the dynamics
bounds. Furthermore, instantneous accleration'3 aX e Ax if and only if there
is some acceleration function a E A(X) such that aX --H a(0). Strictly speaking,
7i(X, r) cA(X), and for each a E 7Y(X, r) there is a constant aX ? Ax such that
a(1) = aX for all times I Ei
We will say that Fr respects A if for all intervals [11,12], Fr(ti) = X implies that
Fr respects A(X) in [11,12]. A' will denote the map from TC to all acceleration
functions feasible under the bounds f and V' given in (4.14) and (4.15)
Since the set of feasible instantaneous accelerations for an open-chain manip-
ulator is state-dependent, to choose the discretization parameter 7 or ?, and a
13Recall that an instantaneous acceleration is a vector not a functiou.
127
timestep r, we consider the relationship between feasible acceleration functions
(such as the members of A(X)) and feasible sets of instantaneous accelerations
(such as Ax). Suppose Fr respects A'. Then, the average acceleration of Fr over
the interval [nr, (n + 1)r]
vr((n+I)r)--HVr(flT)			U			A'pr(t)			(4.20)
r			tE[nr,(n+1)rl
Suppose that Fq is a (tL, r)-bang trajectory. Then for all I ? [nT, (n + 1)T],
aq(t) --H a(?) for some constant a(?) such that for all i, ?j divides a2(n), and
a(n) E			fl			AFq(t)			(4.21)
tE[nr?(n+1)r]
If Fq(flT) and Fr(flT) are close enough and T is small enough, then
A'Fr(t) C			AFq(t) C AF*q(o)			(4.22)
u			n
te[nr,(n+1)rl			tE[nr,(n+1)r]
and furthermore, the boundaries of these two sets are separated by some distance.
(Recall equation (4.9).)
If this separation is great enough compared to ???? and ji, then both the
(it, ?)-extremal shell at Fq(nr) and the set of true (jt, T)-bangs at Fq(flT) will
have a uniform tcj advantages over A'(l'r(flT)) We will later use this relationship
between sets of instantaneous accelerations as a criterion for comparing maps from
TC to sets of acceleration functions.
Since we can calculate the minimal distance between members of ?Ax and
members of ?A'x, we now consider how points on ()Ax and cAx' ?move" in re-
sponse to smalt, continuously realizable changes, or pe?turba1ions, to state X. (See
figure 4.5. It is easily shown that (physically possible) state perturbations (Ax, Av)
cause points on ?Ax and ?A'x to move by distances that are O(Ax) and O(Av).
Therefore, because we assume global dynamics bounds, over duration Al, points
on ?AFr(t) move by O(Al). Recalling (4.14), this means that we can find ?(e)
uniform acceleration advantage at, discretization ?, fundamental timestep'4 TO,
and maximal tracking tolerances ?xO and ?vO such that if X is within (iiro, ?vo) of
Y, then the (it,T)-extremal shell of accelerations at X, ?i(X, T), has a uniform
cj advantage over A(Y) for duration min(r, TO). Following similar steps, we can
choose ? to guarantee that the set of true (?, ?)-bangs at X, ?(X, 7), also has a
uniform `ci advantage.
14This is the maximum timestep that guarantees a some (p T)-bang trajectory will be able to
stay within some constant tracking tolerance of an e-time-rescaled trajectory.
128
4.2.2.5 The Coupled Tracking Lemma
The Coupled Tracking Lemma is the key to choosing, for a given tracking tolerance,
a timestep T and other paramenters that guarantees the following: for any e-
time-rescaled trajectory F'r beginning at s', some (?, r)-bang trajectory and some
true (jt, r)-bang trajectory will track F'r to that tolerance. Note that the set of
acceleration functions associated with a given state can be arbitrary, so long as
it has the necessary uniform acceleration advantage. This allows us to apply the
lemma to sets of true (?, r)-bangs without knowing their forms as path functions.15
More generally, the lemma can be applied to state-dependent sets of bounded
acceleration functions whose exact form is unknown but whose time-derivatives
are also bounded.
Lemma 4.2.1 (The Coupled Tracking Lemma) Let Amax be the global L?
acceleration bound. Consider functions ?, Q : TC : Y is a set of acceleration
functionsj. Let P have the property that if for some state X, a ? ?(X), then for
all times t, Ia(t)Ioo ? Amax tct. Let Q have the property that if acceleration
function a ? Q(X), then for all times t, Ia(t)Ioo < Arnax. Let Fr(t) respect ?.
Suppose that there exist a maximal tracking tolerance (nxo, nvo) and a funda-
mental timestep To such that for all T ? To the following is true: if state X is within
(??xo, Tivo) of state Y, then the set of acceleration functions Q(X) has a uniform
K? advantage over ?(Y) for duration r
Then, for any L? tracking tolerance (TIx,1?v), there exist a timestep r and a
velocity vo such that if trajectory Fr respects ? and IIpr(O) --H volloo ? 2AmaxT,
then there exists a trajectory Fq such that
1. vq(O) = vo;
2. pr(0) pq(0);
3. Fq tracks Fr to tolerance (nx,7iy); and
4. For each n, 1'q respects Q(Fq(nT)) in [nr, (n + 1)Tj.
Moreover, it is sufficient that
T ? To;
T			min(??,?xo)?t			and
Amax(8Ama?+6hi)
T < min(?,?o)
4Am ax
15Le., we don't need a series or closed iorrn representation.
(4.23)
129
Proof: Presented in Section 4.3.1.
Recall from Section 4.2.2.2 that there is a ?-safe tracking tolerance (7?x, 7?v)
where both n and n? are ?(?). The Near Extremal algorithm uses the Coupled
Tracking Lemma (above) to choose a timestep r that is ?(?) and that guarantees
that if a solution Fopt exists, there will be a (?, r)-graph trajectory that tracks
F1opt to tolerance (Tix, TIv). The True Extremal Algorithm can choose r similarly,
but because of the search pruning, this alone will not guarantee it will find the
required trajectory.
(`Ii --H
Figure 4.5: Sets in acceleration space. Ax obeys bound f; A'x obeys r. The outer
dotted parallelogram represents the boundary of the set obtained by considering
all "perturbations" of Ax by up to hqi(nx,nv) t hqo(r) and taking the interse&
tion. The intersection of this set with the ?-grid has a tcj advantage over the set
(represented by the inner dotted parallelogram) obtained by considering all "per-
turbations" of Ax' by up to hr(T) and taking the union. The eight jt-grid patches
crossed by dashed lines show this ?? advantage with respect to an acceleration
(vector) a.
4.2.2.6 Coupled ?acking and (bx, b?)-Bucket Pruning
In the True-Extremal Algorithm the kinodynamic constraints, ?, and the choices of
discretization parameter ?, timestep T, and root vertex S* determine a reachability
130
graph g whose vertices are states and whose edges correspond to (1 --H e)?(co, ci)-
safe (?, T)-bangs.
Recall that in
(br, bt,)-bucket pruning, TC is divided into voxels with diameter
bz in spatial dimensions and bv in velocity dimensions. A breadth-first search with
(br, bt,)-bucket pruning procedes as a normal breadth-first search does, except that
when the search finds a vertex in a voxel in which another vertex has previously
been found16, the edges out of the newly found vertex will be pruned from the
search, i.e., no edges out of that vertex will be explored.
Consider any breadth-first search of ? with (br, bv)-bucket pruning. We call
removing all edges and vertices in ? that are not explored during this search a
breadth-first (bz, bv)-bucket pruning of?. We say that a graph-trajectory F rema?ns
after that breadth-first (bz, bv)-bucket pruning if the path in g corresponding to it
is not affected the pruning.
Like the Coupled Tracking Lemma, the Robust Coupled Tracking Lemma ap-
plies to sets of acceleration functions more general than (T', ?)-bangs and (?, T)-
bangs. We introduce three new terms used in its statement. Let Q be a mapping
Q TC H : Y is a finite set of acceleration functionsi. We say that a tra-
jectory segment from state X to state Y is a (Q, r)-trajectory segment if there is
a trajectory Fr such that Fr(0) X, Fr(r) = Y, and ar e Q(X). A vertex S*
and the set of (Q, r)-trajectory segments determine the (Q, T)-reachabihty graph
? rooted at S*. If 7? is a subset of TC, then the maximal subgraph of U lying in 7?
is the maximal subgraph ?? of ? such that all vertices and all edges (as trajectory
segments) in g' lie in 7?.
Lemma 4.2.2 (The Robust Coupled Tracking Lemma) Consider functions
P, Q : TC H : Y is a set of acceleration functions?. Let ? have the property
that if a e ?(X) for some state X, then for all times t, Ia(t)Ioo ? Amax --H tc?. Let
Q have the properties that Q(X) is finite for all X e TC and that if a ?
then for all times t IIa(t)IIoo < Amax.
Suppose that there exist a maximal tracking tolerance (lixo, ilvo) and a funda-
mental timestep To such that for all T ? To the folThwing is tn?e: if slate X is within
(?Ixo, Tivo) of state Y, then the set of acceleration flinclions Q(A?) has a uniform tcj
advantage'7 over ?(Y) for duration T-
16By previously we mean in a previous generation or earlier in the current generation
17??ecall equation (4.19).
131
Let (??z, `iv) be an L? tracking tolerance, and let timestep T obey the following
inequalities:
r < To;
T			4 rnin(?xo,?x)?t
24Amax?t+2?12+(8Ainaz+?i)2
T ? 2min(?o,nv)
--H 8Amax+?i
and			(4.24)
Let be ? the (Q,T)-reachabih'ty graph rooted at some
Now, suppose that Fr respectsP, Fr(O) is within 2A?axT ofs*, andpr(O)
Suppose that 1? is a subset of TC containing the (nz, "v)-tube induced by Fr, and
that ?? results from some breadth-first (?? , %`r)?pruning of the maximal subgraph
of? that lies in 1Q.
Then there is a Vq in ?` such that Tq < Tr, Uq(O) S*, and Fq(Tq) approxi-
mates Fr(Tr) to tolerance (nz,iiv)
Proof: Presented in Section 4.3.1
Having chosen jt as described in Section 4.2.2.4 (and in detail in 4.3.2 and
4.3.3), the True Extremal algorithm uses Lemma 4.2.2 to choose a timestep T that
is ?(e) and pruning bucket dimensions b1, and bv that are ?(e3) and ?(e2). The
parameters chosen will ensure that if a ?-safe solution 1'opt exists, there exist a
(i, ?)-true-bang graph trajectory that tracks 1" to the e-safe tracking tolerance
opt
4.2.2.7 Asymptotic Bounds
In the preceding sections we have described the key concepts towards asymp-
totic bounds for choosing parameters that guarantee the algorithms will find an
e-optimal solution when a solution exists. Given a problem and an approximation
parameter e, the Safe Tracking Lemma shows how to find a family of e-safe-tracking
tolerances (n,?iv) such that nr' and 7Iv are Q(e). Sections 4.2.2.3 and 4.2.2.4 sketch
why there will be sufficiently small discretization parameters jt and ?j and a uni-
form acceleration advantage ?? that are ?(e). For open chain manipulators we find
71xO, 71vO, and To that are fl(e). The lemmas in Sections 4.2.1 and 4.2.2 indicate that
there is a correct choice of T that is ?(e). Section 4.2.2 implies that since ?? and
T are both ?(e), we can choose (6z, bv)-pruning bucket dimensions that are
and ?(e2) respectively.
132
It follows that for a given problem, the number of (bx, 6?)-pruning buckets is
o((1?)5). Since ? and the ?j are ?(?), the out-degrees of the graphs are 0((j1)d?1)
The complexity each algorithm, then, is O(p(N,E,d)(c1)6d?1)? where p(N,?,d)
is the cost of checking the (1 --H ?)6?(co,ci)-safety of a (?,?)-bang or a
bang, depending on the algorithm. In Section 4.3.4 we review techniques for safety
checking that that will have a cost of O(N(d t log N) per bang in the asymptotic
case. Our algorithms therefore have an overall asymptotic complexity bound of
O(N(d + logN)(l?)6d?l)
4.3 Deriving Bounds
We first present proofs of the Coupled Tracking Lemma (4.2.1) and the kobust
Coupled Tracking Lemma (4.2.2). These two lemmas are fundamental to obtaining
our results. We use them to show how the algorithm can choose the discretization
parameters ? and ? and the timestep r, and how we can calculate the uniform
acceleration advantage ??. We then briefly review the algorithmic complexity of
appropriate techniques that can be used for checking whether a (?, r)-bang or
(?,r)-bang respects (1 --H ?)?v(co,ci) safety.
4.3.1 Proving the Key Lemmas
If a system has decoupled dynamics bounds, the acceleration or velocity chosen
along any one axis never affects what accelerations or velocities are possible along
other any other axis. This is why tracking the trajectory of a Cartesian robot obey-
ing L? bounds reduces trivially to the one-dimensional case. On the other hand,
if a system has coupled dynamics bounds, choosing a maximal acceleration along
one axis can limit the accelerations along another. F?urthermore, if the dynamics
equations are state dependent, then the set of instantaneous extremal accelerations
is state-dependent. The definition of a uniform ?t acceleration advantage over a
timestep is thus crucial to our proofs of Lemmas 4.2.1 and 4.2.2.
To prove these two lemmas, we begin by considering a game against an ad-
versary in a one-dimensional space. In this game certain rules simulate dynamics.
When simultaneous independent games are played, the adversary can force our
movements to be governed by accelerations in the (ji, ?)-extrernal shell or by true
(?it? r)-bangs.
133
4.3.1.1 The Tracking Game
Consider a game in which you are trying to track an adversary in a one dimensional
space. The game is a series of rounds, each of which simulates motions taking
duration r. The game begins with adversary in state (xo, vo) and you in state
(x0,, v0,). A round simulates an interval of time with length T.
Let game parameters Ama? and ?, such that Amax > ?j > 0 be given. When
discussing the game, we will use X?, v?, etc, to denote game variables that corre-
spond to round n.
In each round the following takes place:
Play for Round n:
1. The adversary plays first by choosing an acceleration function an(1) for him-
self such that for all I ? [nT, (n + 1)r], Ian(I)I ? Amax --H tc,. His state
(xn+i, vn+i) at the end of the round (beginning of round n + 1 ) is computed
from his state (xn, vn) and this acceleration function by integration over the
time interval [nT, (n + 1)T].
2. For your play in round n, you then choose either HIGH and LOW. This
choice limits the acceleration function that the adversary can choose for you,
and that determines your state (X'?+1, v?+1) at the end of the round. Note
that you play only HIGH or LOW; the adversary chooses your acceleration
as well as his own. However, his choice of your acceleration is constrained
by your play.
3. Let a0? vn+yvn If you played HIGH, then your adversary can choose any
function a'?(t) such that a'?(t) > ano+&t for all I E [nr, (n+1)r]. If instead you
played LOW, then your adversary's choice must obey the condition a'?(I) ?
a?O --H ?j for all t ? [nT, (n + 1)T]. In both cases, a'?(I) must obey the condition
ja'?(I)j ? Amax for all I e [nT, (n + 1)7].
4. Your state (x'n+1, v?'+i) at the end of the round is computed using your state
at the end of the previous round and integrating a'?(t) over the time interval
[nr, (n + 1)r].
134
LOW			HIGH
--H--H- -,			------ -
Amaz			a --H
I).
O a0 + tc?
acceleration
Amax
Figure 4.6: Details from a game against an adversary. Suppose in the current
round the adversary's average acceleration is a?. Then, if you choose HIGH, the
adversary assigns you an acceleration function whose range lies in [a0 t ?t, Amax];
if you choose LOW, the adversary assigns you an acceleration function whose range
lies in [--HAmax, a0 --H ?i] This function controls your motion for the current round,
which covers a time interval r.
4.3.1.2 A Winning Simple Strategy for the ?acking Game
A simple high-level strategy for the Tracking Came is: try to go faster than the
adversary if you have fallen behind; try to go slower than the adversary if you have
gotten ahead; but never go much faster or much slower than the adversary. The
uniform ?j advantage you have over the adversary's average acceleration during a
round assures that we can follow such a strategy. (See fig. 4.7.)
We now give the detailed strategy in terms of the game parameters.
Simple Strategy
In round n choose HIGH or LOW according to the following rules.
1. If IXn --H x'nI ? 2AmaxT2, then choose HIGH if V?+? > , LOW otherwise.
2. Else if X? --H X1? > 2AmaxT2, then choose HIGH if v' ? t'n+1 + 2AmarT, LOW
otherwise.
3. Else choose LOW if v' ? Vn+1 --H 2AmaxT, HIGH otherwise.
We make the following claim:
Claim 4.3.1 Suppose that you play the Tracking Came against an adversary and
follow the Simple Strategy above, and suppose that Ixo --H x'oI ? 2AmaxT2 and Ivo --H
vo, < 2AmaxT
135
8AmaxItCt
v
Vr
Vr
Figure 4.7: Intuition for proving Lemma 4.2.1: a game in iD. We try to track an
adversary whose acceleration a? obeys the condition Iar(t)I < Amaz --H tc?. We gen-
erate a our velocity function Vq timestep by timestep, trying to limit Ivq(t) --H Vr(t)I
and f(vr(t)?vq(t))dtj. During each round we can only choose whether our acceler-
ation aq(t) is above or below the adversary's average acceleration over the timestep
by tc?. The adversary otherwise controls our acceleration, except for the restriction
that Iaq(t)I < Amax. Mimicking the adversary's velocity Vr to within 2AmaxT is
straightforward, but in making up for position error, our strategy can result in a
velocity error of 4Amaxr; Vr+ Vr t 4AmaxT and v7 = Vr --H 4Amaxr in the figure.
Following a good strategy will keep f(vr(t) --H vq(1))dtI within a constant bound
dependent on T. The game conditions give us a uniform kj acceleration advantage
over each timestep.
136
Let two trajectories Fr and l'q be defined as follows:
Then for all t:
Fr(O)			=			(ro,vo);
ar(t)			=			an(t--HnT)
Fq(O)			=			(x01,v0,);
aq(t)			=			a?'(t --H nr)
for all tE [nr,(n+1)r);
for all t E [nm(n+1)r).
IVr(t) --H vq(t)1 ? 4Amaxr;
lxr(t) --H xq(t)I ?			+ 6AmaxT2.
(4.25)
Proof: Suppose the hypotheses hold. (See figure 4.7.)
The velocity condition in (4.25) holds by inspection, since ar(t) --H aq(t)I ?
2Amaz for all t.
Let us now define
xcrr(t)			=			Xr(l)			--H xq(t);
verr(t)			=			vr(1)			--H vq(t)
To obtain a bound on Irr(!) --H xq(t)1, we first bound the length of an interval for
which Xerr and Verr can have the same non-zero sign. Suppose that Ixerr(nr)I >
2AmaxT2 and that Verr > 0. Then following the strategy ensures that verr(tc) = 0
at some least time tc > nr, and in particular, tc ? nr + 4AnAa?r. Then
Ixerr(tc) --H xerr(nT)I ? 8A2mar T2
The velocity condition ensures that if rerr(nT) ? 2AmarT2 but Ixerr((n +
1)r)( > 2AmazT2 then we know that
Therefore,
Ei
Irerr((n + 1)r)j ? 6Ama??9
m1ax Irerr(t)I ? SA2??axT2 + 6AmarT2.
(4.26)
Simple arithmetic manipulation shows the following corollary.
137
Corollary 4.3.2 The following conditions together guarantee that in the Tracking
Game the adversary can be tracked to tolerance of (`ix, `iv):
Ixo--Hr1ol			?			2AmaxT2;			(4.27)
Ivo--H%I			<			and			(4.28)
T _			`ir?t			7/v			(4.29)
2AmarT;
min 8Amax + 6?t) 4Arnax
4.3.1.3 Applying the Game to Prove Lemma 4.2.1
We now use facts proven about the Tracking Game to obtain a proof of Lemma
4.2.1.
Proof (of Lemma 4.2.1): Let T' and Q be functions `P, Q : TC :
is a set of acceleration fi?nctions). Let ? have the property that if x c TC and
a E ?(X), then for all times t, Ia(t) ? Amax --H ??. Let Q have the property that
if X e TC and a E Q(X), then for all times t, Ia(t)I ? Arnax. Let Fr(t) respect ?.
Suppose that there exist a maximal tracking tolerance (`ixo, `ivo) and fundamen
tal timestep To such that for all T ? To the following is true: (a) if state X is within
(lixo, `ivo) of state Y, then the set of acceleration functions Q(X) has a uniform tcj
advantage over ?(Y) for duration T.
Suppose that (`ix,'i?) is given, and that T obeys the conditions
T ? To;
)lJ?n$a;$8)6?I)Amax
4Arnax
and			(4.30)
Consider d simultaneous independent playings of the Tracking Game in which
your adversary's trajectory in the ith game is Fri and yours is Fq,i, and in which
Fr,i(O) and Fq,j(O) meet the starting conditions (t = 0) of Claim 4.3.1
We consider play during round n, assuming that you have followed the Simple
Strategy and play has proceeded legally through the end of the previous round.
By Claim 4.3.1, in each game i, Vq,i(nT) is within (`ixo,'ivo) of Fr,i(nT). Then
during round n, for each of the 2d combinations of HIGH and LOW, there is some
function a?'? E Yq(Fq(nT)) such that the adversary can legally return18 a??'? in the
`8That is he can choose it as your acceleration in part 3 of the round; see Section 4.3.1.1.
138
jth game, provided you follow the Simple Strategy. Suppose you do so. Then by the
claim, in each game i, Fq,i(t) is within (nzo, ?vo) of Fr?i(t) for all I ? [nr, (n + 1)r].
By induction, we see that (4.25) holds for the duration of the game.
Therefore, if for all n, for all I ? [nr, (n + 1)7], aq(t) aY'?(t), then Fq tracks
Fr to L? tolerance (?z,?iv) E]
4.3.1.4 Altering the Game to Prove the Robust Lemma (4.2.2)
Let us take the Tracking Game and alter the rules:
4. A temporary state (xn+i, vn+i) at the end of the round is computed using
your state at the beginning of the round (end of the previous round) (xn,vn)
and integrating a'?(l) over the time interval [nr, (n + 1)7] from (Xn, vn).
5. The adversary chooses a position perturbation An and a velocity perturbation
vnA to be applied to your position and velocity. Your position at the end of
round n is
A
r?+j = Xn+i + x?;
A
v?+1			Vn+1 + v?.
We call the resulting game the Tracking Game Willi Perturbations.
Claim 4.3.3 Suppose that you play the Tracking Game With Perturbations against
an adversary and follow the Simple Strategy above. Suppose that at the start of the
game Ixo --H x'ol < 2AmazT2 and vo vo,l < 2AmaxT. Furthermore, suppose that the
adversary always chooses perturbations such that 1xA1 ? ?r2 and vnAI ?
Let two trajectories Fr and Fq be defined asfollows:
Fr(nr)			=			(xn,v?);
=			an(t --H
Fq(nr)			=
aq(t)			=
for all I ? (nr, (n + 1)r);
for all I e (nm (n + 1)7).
Then for all I:
IVr(t) --H vq(t)j ? 4Ama?T + %17;
Ixr(t) --H xq(l)1 ? (8Amax+?t)2r2
4?j
2+ ?!9T2			(4.31)
___________- mar7
139
I			I			I I I
I			I			I I I
I			I			I o+I I
I			I			I			- I I
I I
I I
I I
6? I
Vr++
Vr
v
Vq
Vr
Figure 4.8: Intuition for proving Lemma 4.2.2: another tracking game in iD.
This time, the adversary can perturb our state at the end of each round, by
in position and in velocity; the discontinuities in Vr correspond to the
perturbations. However, we can still track him, though to a looser tolerance.
Vr++ = Vr + 4AmaxT + KIT and v?			KIT
2			r			= Vr --H 4Arnazr --H
140
Proof:
The analysis proceeds similarly to that for Claim 4.3.1.
Again, we let xerr(1) = xr(t) --H xq(t), and VeTr(t) = vr(t) --H vq(t) and bound
the length of an interval for which Xerr and Verr can have the same non-zero sign.
Because of the velocity perturbation, our effective acceleration advantage is only ?.
Therefore, if Ixerr(nr)1 > 2AmaxT2, following the strategy ensures that verr(tc) = 0
at some later time tc < flT + (8Amax+?i)r Then,
jxerr(tc) --H xerr(nr) < (8Amax+tCj)2T2
Because of the perturbation at the end of a round, the %17 term must be added
to the velocity-tracking tolerance and must be added to the position term in
the final tracking accuracy.
E
Therefore
(SArnaxt?j)2T2			9 hffitT2
mrx Ixerr(t)1 ?			4h'j			e 6AmazT ? 2
(4.32)
We immediately get the Corollary:
Corollary 4.3.4 The following conditions together guarantee that in the Tiacking
Game with Perturbations the adversary can be tracked to a tolerance of (nz, ??v):
Ixo--HYoI			?			2AmazT2;
Ivo--Hvo1l			<			2Amaxr; and			(4.33)
24Ama??i+2?j +(8Arnax+?i)2, 8Arna?+Kt
We can now prove Lemma 4.2.2
Proof (of Lemma 4.2.2): Let ? and Q be functions ?, Q : TC fy: Y is
a set of accelerations). Let ? have the property that if a C P(p,p), then for all
times t, Ia(1)Ioo < Amax --H ??. Let Q have the properties that Q(X) is finite for all
X E TC and that if a E Q(X), then for all times t a(t)Ioo ? Arnax.
Suppose that there exist a maximal tracking tolerance (lixo, nvo) and a funda-
mental timestep To such that for all T < TO the following is true: if state X is within
(7ixo, Tivo) of state Y, then the set of acceleration functions Q(X) has a uniform ?t
advantage over ?(Y) for duration ?.
t4t
Suppose that (?1x, ?v) is an L? tracking tolerance and that
T			<			To;
r			<			24Arna%?1$fl2(?12%O$8)mh1a?+?1)2; and
7			?			2min(?o,?)
(4.24)
Let be g is the obstacle-free (Q, r)-reachability graph rooted at some S*. Suppose
that Fr respects ?, Fr(O) is within 2AmaxT of s*, and pr(O) s*. Suppose that 1?
is a subset of TC containing the (?ix, ?Iv) tube induced by Fr, and that g' results
from some breadth-first (?I?r2, %ffi )-pruning of the maximal subgraph of g that lies
in ?.
Consider d simultaneous independent playings of the Tracking Game With Per-
turbations in which your adversary's trajectory in the jth game is Fr,i and yours is
Fq,i. Suppose that all the f'r,i(O) and Fq,i(O) meet the starting conditions (t = 0)
of Claim 4.3.3. Let (Xn, vn) denote your state (i.e., the vector of your states in the
individual games) at the beginning of round n.
Now, suppose you follow the Simple Stra\egy. Because the breadth first pruning
buckets and perturbation size are both (?? ,?%?r), Claim 4.3.3 and the definition
of Q, ?, and g? assure that the following can legally occur in each round n of the
game:
1. First, the adversary chooses a member of Q(l'q(flT)) for your (vector of)
accelerations.
2. Then, the adversary perturbs your state (xn+i, vnsi) if and only if it is not
in ??. In particular, if he perturbs your state, he perturbs it to the state
in ?? that caused (xn+i,vn+i) to be pruned from ??. Thus, for some graph
trajectory ?q(fl+l) in g' and some m n + 1, ?q(fl+l) (mT) is this state.
If the adversary plays in this manner, then by Corollary 4.3.4 for all n:
1. (Xn,Vn) will be within (`lr,?iv) of Vr(flT),
2. (xn,vn) will be within (?r.,mv) of Vr(nr), and
3. your trajectory Fq(l) will be within (?,mv) of Fr(t) for all I ? [nr,(n + 1)7].
Furthermore, for each n there will be some q(fl) in ?` such that for some m __
142
1.			?q(fl)(??)			Fq(nr), and
2.			?q(fl)(? --H			--H m)?) = Fq(t) for all t ? [nr, (n + 1)r).
Since this holds for all n, there must be some trajectory Fq* ill ?? such that for
some Tq < Tr, Fq*(Tq) approximates Fr(T?) to within (?,?). E]
4.3.2 Search-Space Bounds for Cartesian Manipulators
Here we first consider an L2 Cartesian robot with acceleration bound a. We choose
an acceleration discretization ? and an Q(?) scalar ?? such that (a) all Iii are
and (b) for all X, Y E TC, the (?, ?)-extremal shell of accelerations at X has a
uniform ??-advantage over A? for any T. We then derive a similar choice of ji for
cases when an L2-norm force bound J is given instead of an acceleration bound.
We then return our focus to the acceleration bound case and examine the Near
Extremal Algorithm in detail to illustrate how to derive final reachability graph
parameters'9 chosen by the algorithm. We briefly discuss the derivation of the
control discretization parameter TI for the True Extremal algorithm.
4.3.2.1 Parameter Choices and Acceleration Advantages for an L2
Cartesian Robot
The set of constant accelerations obeying the L2-norm bound a has a uniform
acceleration advantage of
?a(2?+?2)			____
Yd(1 + ?)2 > 27d
over the set of accelerations obeying the L2-norm bound ?,?%??. Recall that
II?II? ? 11y112 ? $dIIyI for any y ? ?d (4.17). If ? and K? have the prop
erty that
IIiIIoo ?
47d'			(4.34)
then for any state X and any duration ?, the (?, ?)-extremal shell of accelerations
?i(X, T) has a uniform ?t-advantage over the set of acceleration functions obeying
the L2-norm bound
For any state X, consider the sets of feasible instantaneous accelerations Ax
and Ax' that correspond to force bounds 7 and #?1+c)?' respectively. Recall that
the inertia matrix M is symmetric and positive definite and thus orthogonal. In
`9These parameters are ? and r. n?i is used in the proof and by the True Extrema! Algorithm
143
I			I
I			I
L			J
I			I
I			I
I			I
I			I
A
Figure 4.9: The Control Extremal Algorithm still has the desired completeness
when we use single-occupancy cells to bound the search. If we choose parameters
for the Tracking Game with Perturbations that correspond to the parameters cho-
sen by the algorithm, then some trace of d simultaneous playings of the Tracking
Game with Perturbations in which you follow the Simple Strategy corresponds to
a "virtual path" in the pruned reachability graph, where `virtual edge" of zero
cost goes from each pruned vertex to the vertex that caused its pruning.
144
addition, recall that M is constant for a Cartesian robot. If Amin and Amaz are
the minimum and maximum eigenvalues of M, respectively, and a ? ?Ax, then
Ia --H a'II? > ??`?J\+ifc\2 More compactly,
It follows that if
IIa--Ha'II?> 2Amaz
and
--H 4$dAmar'
II,tIIoo <
_			YdAmax
then 7Y(X, r) has a uniform `ct-advantage over ??(Y) for any r, X, and Y.
4.3.2.2 Timestep Choice and Searchspace Bounds for the
Near-Extrernal Algorithm
(4.35)
(4.36)
By applying the Safe Tracking Lemma, we find a family of ?? and ?v that are
and guarantee that any trajectory tracking a ?(co, c1)-safe trajectory to tolerance
(?z,'iv) will be (1 --H ?)6v(co,ci)-safe. Specifically,
coc
ci(1--Hc)+?			(4.10)
< P7Iv
It is simplest to choose p 1, which implies Tir 7/v However, by using a
technique from Chapter 3, we show how to choose ? to minimize the bound on the
possible size of the reachability graph searched by the Near-Extremal Algorithm
applied to a Cartesian robot with L2 acceleration and velocity bounds a and v.
First, we choose an underlying acceleration discretization ? consistent with a
uniform acceleration advantage `ci (4.34):
Iii			= = `ct = 4'Fd			(4.37)
We parameterize our choice of timestep r as a ffinction of ?. We use Lemma 4.2.1
to obtain
rx(P) =
=			cp?			(4.38)
= min(T(?), Tv(4)).
145
Since Tz monotonic and P must be positive, r(P) is maximized when rz(P) and
rv(P) are equal. Solving for P to get a positive r yields
p=--H21 --Hci(1--H?)+ c21(1--H??+			(4.39)
We use the Coupled Tracking Lemma (4.2.1) to guarantee that a tracking trajectory
would obey the velocity bound. Thus we choose
r = min			co?
4a
2?a ci(1 --H ?) + c?(1 --H ?)2 + co(16$d+3
(4.40)
Given the desired start state 5, the Near-Extremal Algorithm chooses the root
vertex 5* to meet the conditions at t = 0 in the Coupled Tracking Lemma, relative
to ?-time-rescaled start state 5'. (R?ecall equation (4.5).) If an optimal trajectory
Fopt exists, then there will be some graph trajectory Fq that tracks F'0pt to the
tolerance (?1r, 71v) determined by the Safe Tracking Lemma and the choice of ?
above. (See equations (2.10) and (4.39).) Since (?x,?iv) meets the conditions of the
Safe Tracking Lemma, such a Fq would be guaranteed to be (1 --H ?)?(co, ci )-safe.
Since ? and 7iv are O(?), Fq would meet the approximation criteria at the desired
start and goal states.
We can now bound the size of the reachability graph for a Cartesian robot
whose maximum speed is v and whose mufiguration space is contained within a
d-dimensional cube with diameter 1. Let
[L= minji?.
Then the total number of possible velocities for reachability graph vertices is
bounded above by (?$Vr + 1)d and number of configurations by (7?rt + 1)d Thus,
the total number of reachability graph vertices is
GvWa,ii,T,Th1,d)? ffl?+1 ??
+1
(4.41)
Using the choices of ? and T above in (4.34) (or (4.36)) and (4.40), we see that the
right hand side of (4.41) is o((?vl)d(1?)5d). Recalling the definition of the (j,r)-
extremal shell, the out-degree of this graph is bounded above by d (ztat)d?i Thus
146
the total number of graph edges is
Gc(?a,?,r,?v,l,d) <			(4.42)
=0
d			ffir +1			[?T12 + 1
ad?l(?vl)d (e1)6d?1
We can follow a similar, straightforward development if L2 force bounds (instead
of acceleration bounds) are used. In particular, we d?oose i using (4.36), define
analogues of (4.38), and choose P to maximize r to get the necessary reachability
graph parameters. We substitute for a in (4.43) to obtain similar bounds on
the size of the reachability graph.
We summarize these results for a Cartesian robot obeying L2 dynamics bounds
with the following lemma.
Lemma 4.3.5 Given a kinoJynamic planning problem for a Ca4esian Robot with
L2-norm dynamics bounds and given an approximation parameter e, the Near-
Extremal Algorithm will search a reachability graph with O(cdD((1)6d?1) vertices
and edges; CD is a constant dependent on the kinodynamic specifications. If an
optimal (safe) solution 1'opt exists, then some graph trajectory is e-optimal.
4.3.2.3 ?ue-Extremal Algorithrn Searchspace for a Cartesian Robot
Now, suppose the (Cartesian) robot obeys L2 force bound J, and again let Amin
and Amax denote the minimumand maximumeigenvalues ofits inertia matrix M.
If Ifall2,IIfbII2 < J and Ilfa --H fbiloo ? jt, then
IIM?1f? --H M?1f?II? <
At the same time, if Ilfall2 = 7 and IIfbII2 = m7, then
Je
IIM?'fa --H M--H1f?II2 > 2Arnax
Therefore, if we choose ?j as in (4.36) and
Aminfe
4dAmax'
(4.43)
147
we guarantee that set of true (jt, r)-bangs ?i(X, r) (Section 4.2.1.1) has a uniform
?1-advantage over ??(Y) for any two states X and Y over any duration T.
Again using the Safe-Tracking Lemma (2.2.3) to find a family of sufficiently
close tracking tolerances (7?x, TIv), we can now apply the Robust Coupled Tracking
Lemma (4.2.2) to find a maximal timestep r using the uniform acceleration advan-
tage ?j above. The algorithm's choice of S* trivially satisfies the I 0 condition of
the lemma. Clearly, since ??, n?, and n? are ?(e), r will be ?(e) also. Finally, the
algorithm chooses pruning-bucket dimensions prescribed by the Lemma (4.2.2):
br			?ir			in configuration, and
2			(4.44)
bv			in velocity.
These quantities will clearly be ?(e3) and ?(e2), respectively. The number of
buckets is therefore O((1c)5d) Recalling the definition of true (?, ?)-bangs, we see
that each vertex has 0((1?)d--H1) out-edges. Since the algorithrn explores the out-
edges of at most one vertex from each bucket, the True Extremal Algorithm will
search 0((1?)6d--H1) vertices and edges of a reachability graph. Summarizing:
Lemma 4.3.6 Given a kinodynamic planning problem for a Cartesian Robot with
L2-norm dynamics bounds and given a approximation parameter e, the True Ex-
tremal Algoritlim will search a reachability graph with O(cdE((1)6d?1) vertices and
edges; cE is a constant dependent on the kinodynamic specifications. If an optimal
(safe) solution Fopt exists, then some graph trajectory is ?-approximately optimal.
Since the complexity of each algorithm is the number of graph edges multiplied
by the time it takes to check the (1 --He)?(co,ci)-safety of each edge (as a trajectory
segment), we determine the asymptotic complexities by combining this lemma with
the bounds from Section 4.3.4. This yields Theorem 4.1.1.
4.3.3
Applying the Coupled Tracking Lemma and
Robots with State-Dependent Dynamics
We now derive lower bounds for the discretization and timestep parameters used
by the Near Extremal and True Extremal Algorithms for robots obeying the open-
chain dynamics equation (4.1). The Near Extremal Algorithm must choose un-
derlying acceleration discretizations u? and a timestep T ensuring that if a ?-safe
solution solution exists, then the algorithm will find an e-approximately optimal
148
solution. We describe how to find sufficiently small parameters that are all fl(e).
We also show that the True Extremal Algorithm can choose a timestep T and a
control discretization parameter ? that are fl(e) and that ensure the algorithm will
find an e-approximately optimal solution under the same conditions.
Let the problem parameters and e be given. Recall from Section 4.2.2.4 that
A and ?` denote mappings from TC to fy: Y is a set of acceleration functions)
under bounds f and r (from (4.14)), respectively. Now, let trajectory Fr respect
A', and let Fq respect A.
We now sketch how to find a set of consistent parameters (!,To, ?1xO, ?1vO, ci): a
maximal discretization parameter jt for either acceleration or control, a maximal
timestep To, a maximal tracking tolerance (Tixo, nvo), and a uniform acceleration
advantage ci. For any timestep T ? To and any trajectory Fr that respects A', the
following will be true: if state X is within (nro, ?vo) of l'r(nT), and T?, IIitII? ? ii,
then both the (?, ?)-extremal shell of accelerations 1Y(X, T) and the set ?i(X, r)
of true (jt, ?)-bangs have uniform tc?-advantages over A'(Fr(t --H n?)) for duration
T. We will show that sufficiently small j?, tCt, To, ?x0, and ?7v0 are
Having obtained these parameters, the algorithms can apply the Coupled Track-
ing Lemmas to choose T. This leads us to bounds on the size of the reachability
graphs the algorithms search.
4.3.3.1 Sufficient Conditions
Observe that the sets of feasible instantaneous accelerations Ax and ??` are
dimensional parallelpipeds for all X E TC. Let Amin and Amax be the minimum20
and maximum eigenvalues of the inertia tensor M(p) over all positions.21 Let
fmin = min? fj, and let J = max? fj. Let the force required to hold the robot
stationary in the presence of gravity obey the bound (1 --H c?)7. From Section
4.2.2.3, the minimum L2 distance between OAx and ?A'x is greater than
tcjfrnin(
= 2Arnur			generally, and
in the absence of gravity.			(4.4.5)
20Amin is the only parameter that is neither given in the problem instance nor bounded (in
smallness) in the derivation; a "quick and dirty" bound is the minimum of the smallest link mass
and smallest link inertia (in generalized units). A hound for Amax follows from the bound on
JIM(p)I found in Appendix C.
21In this section and Section 4.3.3.2 we will use (p' p) to denote both trajectories and arbitrary
states. In equations such as (4.7), both types clearly make sense.
149
Suppose that Fq respects22 the mapping A and tracks r to tolerance (?izo, nvo),
and fix To > 0. For any T ? To, consider what happens over a timestep [nr, (fl+1)T].
For any I E [nT, (n + 1)r], Fr(t) --H Fr(flT) and Fq(1) --H Fq(flT) belong to sets of
possible state perturbations that are functions of To. That is, there exist minimal
sets ?r(To),?q(To) C TC such that for all h ?
Fr(t+h)?Fr(1)			E ?(To), and			(4.46)
Fq(t + h) --H Fq(t)			E
State perturbations about Fr(flT) and Fq(nT) result in perturbations of
and ?AFq(t) about ?A'?r (nr) and ?Avq(nr) We now bound the magnitudes of these
perturbations as functions of r/?o, 7ivo, and To, respectively.
Let a(p, p, f) be given by
a(p, p, f)			M?'(p) (f --H [pTC(p)p] --H G(p)),			(4.7)
and let B6(y) denote the ?-ball about y. We denote global perturbation bounds
hr(T0) =
hqo(To) =
hql(i?xo,71vo) =
maxlla(p,p,f') --H a(p + Ap?,p + ?p?,f')II2
max a(p,p,f) --H a(p + Ap?,p + Ap?,f) 2
maxIIa(p,p,f) --H a(p + Ap?0,p + Ap?0,f)II2
(4.47)
where the maximaare taken as (a) (p,p) ranges over TC; (b) f and f' obey the
constraints fi < J? and L? ? 7'? for all coordinates i; (c) (Ap?, Ap?) ranges over
??(To); (d) (Ap?,Ap?) ranges over ?q(To); and (e) (Ap?0,Ap?0) ranges over
B?0(o) x B?0(o). Note that hqo(To) determines the acceleration perturbation
bound EA from Section 4.2.1.2, modulo norm.
Suppose that ????co = ? and T < To. Recall (4.17). If
Ij?+?'?tVa+hr(To)+hqo(To)+hq1(77ro,7/vo) ?			(4.48)
then for an interval of length T, the (?, ?)-extremal shell of accelerations at Fq(flT),
or TL(Fq(flT), T), will have a uniform ?j-advantage over the set of acceleration
--HI
functions A'(Fr(flT)) feasible under the bound f. Similarly, if
!Amffi + KjYd+ liqo(To) + hql(7Io,Thvo) ? 0,			(4.49)
22Recall from Section 4.2.2.4 that ??r respects A if for all intervals [t1, t2] Fr(ti) X implies
that Fr respects A(X) in [t? t2].
150
then set of true (jt, ?)-bangs at Fq(nT), Tt(Fq(nT), T) will have a uniform ?j-
advantage over A'(Fr(nro)).
The two constraints (4.48) and (4.49) are obeyed if
(4.50)
hr(To),hqo(To),hql(7?zo,?7vo) <
4.3.3.2 Bounds for Perturbations
We show that hr(ro) and hqo(ro) are O(ro), and that hql(7ixo,flvo) is O(?o) and
O(?o). We first note that IIM(p) --H M(p + ??)II is O(IIApII). Now, define
(f, p, p) f --H [pTC(p)p] --H
Substituting and differentiating
hr(r0) ?
hqo(ro) ?			__
hql(?ix,?1v) <
Now, recall a derivation of (4.1), say from [ASSO] and recall that
[pT(1)C(p(t))p(t)]i = pT(t)c?(p(t))p(t), where
C?(P(t))jk = Zj,k faMai;$P) --H 2l?%1p$P)?
Now, M(p) is simply the inertia tensor, and
G(p) =
ap
To max 3M0p1p ?(f,p,p) + M?'(p) [%)p + %4a1
To max m-atlp. ?(f,p,p)+M?'(p)[3a?p+?a1
max ? ?Mapl Ap ?(f, p, p) + M?'(p) [d4Ap + %4Ap1
(4.51)
(4.52)
(4.2)
where UG(p) is gravitational potential energy at state p. Hence, each component
of M(p) and G(p) is a sum of products of components of p and their sines and
cosines (e.g, pi, cos(p5), etc.) This implies that O?(pfj??P) and are globally
bounded. Specifically, it follows that there are Cr, Cqo, Cqi, and Cq'i such that:
hr(T0) ?
hqo(To) ? CqoTo;
hql(71x,?7v) ? Cql?7x0 + Cq'i?1vo
(4.53)
151
Cr, Cqo, Cqi can be bounded, given the robot parameters, by bounding the
norms of the tensors arising from aF(f,p,p) and 0?(f,p,p) This can be done by
op			op
inspection because all terms are bounded. However, simple expressions bounding
the tensor norms have been calculated by [HP89], and derivations of Cr, Cqo, Cqi,
and Cq'i can be found in Appendix C. Recalling (4.45) and (4.48), we therefore
choose
To = min (5car, 5?aq0)
`Iro --H			(4.54)
i0Cqi
?v0			a
l0Cq2
Recalling the previous section, we thus can choose consistent parameters 7?)?
?!x0, TIvO, ?, and tc? that are all ?(e). If IIiII? ? ji and T? ? it, then the maps
TLro,Tfro : TC ?Y Y is a set of acceThration functions? defined by
%Lr0(X)			=			?i(X,ro),			and			(4.55)
7i70(X)			=			?Y(X,7o)			(4.56)
meet the hypotheses of Coupled Tracking Lemmas. Since we can use the the Safe
Tracking Lemma to find sufficiently small 4z and ?v that are ?(e), we obtain a
timestep T that is Q(e) by applying the appropriate Coupled Tracking Lemma
(4.2.1 or 4.2.2). The pruning-bucket size is again given by (4.44).
Thus, we have the following lemma:
Lemma 4.3.7 Given a kinodynamic planning problem for an open-chain manipu-
lator and given approximation parameter e, the iVear-Extremal Algorithm and the
True Extremal Algorithm will search reachability graphs ?vith O(cd(??1)6d?1) vertices
and edges; c is a constant dependent on the kinodynamic parameters. If an optimal
(safe) solution Fopt exisL?, then each algorithm will find some graph trajectory that
?s e-approximately optimaL
4.3.4 Safety-Checking and Final Bounds
4.3.4.1 Quadratic Paths and Polyhedral C-Space Obstacles
Observe that a kinodynamic solution Fq found by the True Extremal Algorithm,
when applied to Cartesian robots, or by the Near Extremal Algorithm will have a
piecewise-constant acceleration p?q Hence the solution trajectory Fq is piecewise
152
algebraic: ?q is quadratic and Pq is linear in time I. Therefore, when the c-space
obstacles are polyhedral, we can check for safety violations exactly as in the L?
dynamics bounds case (Cartesian Kinodynamic Planning) in previous chapters. As
described in Section 2.2.5, the c-space obstacles are "grown" affinely with trajec-
tory speed (Ilvil ? ?) to obtain expanded c-space obstacles in C x ?, which are
determined by 0(N) algebraic surfaces.
Safety-checking for a single (?a, ?)-bang or (?, ?)-bang can be accomplished by
intersecting the quadratic trajectory (parameterized by time) with these surfaces,23
and performing 0(N) sign-tests on each intersection point. This naive approach
would yield an upper bound of O(dN2) per bang, with the d factor coming from
the cost of taking dot products, or 0(N2) for fixed d. In Section 2.2.5 we sketched
an analysis showing that the necessary operations can be done in time 0(N) per
trajectory segment for d ? 3, by observing that the total number of sign-tests
is bounded by twice the number of edges. This important observation can be
generalized to higher dimensions: if the c-space obstacles are convex polytopes with
0(N) geometric complexity, and then the cost of checking the safety of quadratic
paths is O(dN).
4.3.4.2 Polyhedral Workspace Obstacles
For the more general case of open chain manipulators with revolute joints and
polyhedral workspace obstacles, safety is also checked by affinely growing the c-
space obstacles and checking for intersections along the trajectory. We describe
a c-space obstacle representation and a robot-obstacle collision detection method
that can be generalized to a safety-checking method similar to that described in
Section 2.2.5 and reviewed above. It would be convenient if we could apply an
algebraic collision detection predicate such as that described by Canny [Can86,
Can88a]. (We shall soon describe why we cannot.) In fact our 6?-safety predicate
uses the structure of his predicate and the same logical evaluation method for each
pair of polyhedra that could possibly collide.
The non-overlap condition for two convex polyhedra is given by the non-overlap
predicate
AVAV(C?jkt(x) > 0)			(4.57)
ijkt
described in [CD88], with the exact form of constraint functions Cijkl : C
23Hence, we "lift" collision detection to C x ? to so salety-checking.
153
given in [Can86j. The signs of the Cijkt(X) correspond exactly to spatial rela-
tionships among the vertices, edges, and faces of the possibly overlapping poly-
hedra. The zeros of the Cijki are surfaces that contain faces of c-space obstacles,
so we call them c-space obstacle surface functions. To detect collisions on a path
p : time H TC, one substitutes p(t) for x and "merges" the sign intervals of the
resulting functions in time. While the exact form of the Cijkt found in [Can86] for
a robot polyhedron uses quaternions to represent orientation, we can use any other
c-space representation and obstacle surface functions that have the same sign corre-
spondence. For an open-kinematic chain, one set of natural c-space obstacle surface
functions would be mixed trignometric polynomials the sums of products of c-
space coordinates and their cosines and/or sines. In [Can88a], Canny represents
these surfaces algebraically by using quarternion and half-angle substitutions.
Unfortunately, algebraic collision detection requires a c-space coordinate system
in which the surfaces of the c-space obstacles are algebraic and in which the robot
path is algebraic in time. In the joint coordinate system for an open kinematic
chain, the position components pj of (?, ?)-bangs are quadratic in time, and the
velocity components t)j are linear. While it is possible to describe the c-space
obstacles algebraically by using substitutions such as u5 = tan for revolute
joints j, for the trajectories generated by our algorithms there is no way (when the
configuration space has more than one dimension) to simultaneously algebraically
parameterize the paths frinctions that result from this substitution. Since our
algorithm only considers motions where each p? is a quadratic polynomial in time
1, we can choose either (a) a coordinate system in which the c-space obstacle
surfaces are the zeros of algebraic functions, and some path-position components
are inverse trignometric functions of time, or (b) a coordinate system in which
the c-space obstacle surfaces are the zeros of trignometric polynomials, but each
path-position component p? is an algebraic function of time. Thus, there is no
coordinate system in which both the c-space obstacle surface functions and (i, T)-
bang robot paths are algebraic, and we cannot in general perform safety-checking
algebraically.
We can, however, approximately evaluate the collision predicate for (Ij, 7)-
bangs by using approximating polynomials for each of the c-space obstacle surface
functions. The same can be done for a corresponding 6?-safety predicate. The
degree r of the polynomials we need depends on how accurately we wish to ap-
proximate the trignometric polynomials. Intuitively, we expect that since each
154
timestep is finite, and in practice would be very short, this polynomial approxi-
mation is reasonable. The resulting error ?? in checking safety can be bounded
and made arbitrarily small by increasing the degree of the polynomial. Then, one
could implement either a conservative algorithm, which only finds solutions that
are (?v'(V) + ?r)-safe, or an "optimistic" algorithm that finds solutions that are
--H er)-safe.
We now argue that for the purpose of deriving an asymptotic complexity we can
fix the degree of the approximating polynomials to a constant. Simply put, if we
truncate the Taylor expansions of expanded c-space obstacle surface functions to
rth order polynomials, ?r will be 0(?r) because the timestep T is ?(?). Thus, if we
set r 2, ?? will be o(?2), and thus the error of the safety-checking approximation
will be asymptotically smaller than ?.
Since the structure of our 6?-safety violation predicate is similar to that of
[Can86], it contains 0(N) polynomials in t and requires O(N log N) time to eval-
uate, once the sign-intervals of the polynomials are known. If we restrict ourselves
to polynomials of degree r?, the total number of terms in the polynomials will be
O(rcN), and forming the expressions for expanded c-space obstacle lituctions will
take will take time O(rcdN). If finding the sign-intervals of a polynomial of degree
rc has cost 5(rc), then checking the (1 --H ?)6?(co,ci)-safety of a single trajectory
segment could be done in time
O(Nr?(logN + d+ logr?) + ATS(rc)).
By the truncated series approximation error argument above, we can choose a
constant r? as long as ? is guaranteed to be sufficiently small. For this reason we
say that safety-checking takes time O(N(d+log N)). This completes the discussion
of safety-checking for the Near Extremal Algorithm. We now turn to the True-
Extremal case
For the general open chain manipulator, true (--H?, r)-bangs are the solutions to
the set of ordinary differential equations
VM($l(p(t)) ff --H [v(t)Tc(p(1))v(t)] G(p(I))? 4.58)
Since right-hand side of (4.58) is C0o and its time-derivative can be bounded glob-
ally, standard integration techniques (e.g., Rung&R7utta) can be used to approxi-
mate the image of a (T?, r)-bang time t ? T to arbitrary precision, with computation
155
time growing sub-linearly with the accuracy [PFTV8SJ. Similar techniques can be
used to approximate the trajectory segment with a set of polynomials of fixed order
(e.g., 4th order Runge-Kutta) that approximate it in respective subintervals of the
timestep; the closeness of the trajectories to true extremal will be exponential in the
degree of the polynomials. To do safety-checking of the True Extremal Algorithm,
then, we can again use approximating polynomials in our &-safety predicate.
To summarize, under a combinatorial model that charges unit cost for finding
the roots of a univariate trignometric polynomial of fixed degree and size, safety
checking for the Near-Extremal algorithm can be done in time O(N(d+log N)). In
practice, numerical methods would be used to find sign-intervals for approximating
polynomials in 1. The error in these methods can be bounded and incorporated
into the safety margin. Approximating polynomials can also be used to check the
safety of general open-chain manipulator (?jt, ?)-bangs. Thus, for a fixed number
of degrees of freedom the algorithms have a tim&complexity of O(N log N) in the
asymptotic case.
4.3.4.3 Asymptotic Bounds
For each of our algorithms, the final complexity is the complexity of the number
of graph edges explored multiplied by the cost of checking the safety of each edge
as a trajectory segment. By combining the graph-size bounds of Lemmas 4.3.5,
4.3.6, and 4.3.7 with the safety checking costs described above, we obtain a final
approximate cost of O(N(d t l0gN)l?)6d?l), assuming sufficiently small ? > 0.
Letting p(]V, ?, d) express the cost of safety checking yields Theorems 4.1.1 and
4.1.2.
4.4 Summary
In this chapter we obtained provably good approximation algorithms for kinodyna-
mic planning that extend the approach presented in earlier chapters to open kine-
matic chains and to Cartesian robots obeying L2-norm dynamics bounds. These
algorithms find trajectories that are approximately optimal with respect to a pos-
sibly speed-dependent safety margin. We presented algorithms that find near-
extremal trajectories and algorithms that find truly extremal trajectories. The
True Extremal algorithm is the first such provably good algorithm that uses a
state density condition to prune an otherwise exponential search to polynomial
156
size. We believe that both algorithms apply to a more general class of robots, as
discussed below.
To obtain our results we proved two crucial lemmas by using a game-theoretic
proof technique. By using the first lemma (4.2.1), given robot dynamics bounds,
a safety margin, and e we can find parameters that determine a state reachability
graph such that for every optimal trajectory whose initial state is approximated by
the root of the graph, there is a graph trajectory that is within e of being optimal. A
second lemma (4.2.2) allows us to simultaneously derive the state density condition
above (4.44).
Although our results directly apply to two classes of robots, they can be easily
extended to a larger class. We conjecture this class is the class of robots that
have finite degrees of freedom, bounded configuration, convex generalized force
and velocity bounds, and acceleration maps that obey the following constraints:
(a) the dimension of the set of feasible accelerations is equal to the dimension of
the configuration space, (b) the set of feasible accelerations is convex at each state,
and (c) state perturbations (Ap, Ap) result in perturbations to the acceleration
map that are O(IIApII t IlAp I).
Chapter 5
A "Bang-Bang" Planner and
Time-Safety Trade- Offs
We now extend the main results to obtain ?-approximation algorithrns that do not
discretize accelerations or controls (applied forces) and to extend our approxima-
tion model to consider time- and safety-optimality separately.
First, in Section 5.1 we consider finding ?-optimal trajectories that are bang-
bang, i.e., for which all controls are piecewise-saturated.1 We demonstrate po-
lynomial-time ?-approximation algorithms for this kind of optimal kinodynamic
planning. These "bang-bang" algorithms search reachability graphs with a 2d
branching-factor. For open kinematic chains with d degrees of freedom, such an
algorithm is a natural extension of the algorithms in Chapters 2 and 3.
In Section 5.2, we consider a finer complexity analysis of the kinodynamic
planning algorithms from Chapters 3 and 4. In the previous chapters, a single
parameter E characterized closeness to optimality both in execution time and in
observance of the safety margin. We introduce separate parameters ?T and ?s
to describe time- and safety-optimality. Analyses using these parameters show
how the algorithms from previous chapters allow one to trade off planning time,
execution time, and observance of the safety margin.
We illustrate this first step towards practical provably good kinodynamic plan-
ning with (a) graphical representations of the theoretical trade-offs and (b) trajec-
tories found by our planar Cartesian implementation.
1A (single) one-dimensional control is said to be sa?nral?d when it takes one of its two extreme
values We say control is piecewise-saturated if it is saturated almost everywhere and has a finite
number of switchings over every finite interval of time.
157
158
v
5.1 &Optimal Bang-Bang Trajectories
The algorithms for Cartesian Kinodynamic Planning in Chapters 2 and 3 find
trajectories generated by a set of control primitives, (a, r)-bangs. NVhile the corn-
ponents of these acceleration functions assume values from the set f --Ha, 0, aJ, it
is possible to find t-optimal trajectories having acceleration functions that are
piecewise-constant and whose components only take values from the set f --Ha,
Thus, these trajectories belong to a class and of trajectories that obey piecewise
constant, component-wise-fully-saturated controls, also known as bang-bang con-
trols. (See figure 5.1.)
(a)
4
.64
Figure 5.1: Ranges control functions from four classes: (a) simple bounds, (b)
extremals, (c) (f, r)-bang, and (d) bang-bang.
Since a large class of minimal-time trajectory problems for time-invariant linear
systems have bang-bang solutions, we might hope to find a provably-good approx-
159
imation algoritlim for e-Optimal Cartesian Kinodynamic Planning that only con-
siders bang-bang controls. However, the characterization of time-optimal controls
for open-kinematic chains is still generally an open problem, even in the absence
of state constraints. On the other hand, the Pontryagin Maximum Principle can
be used to show that a time-optimal trajectory is extremal when not "on" a state
constraint [PBGM62,Lei66].
The main result of this section is an algorithm for e-Optimal Open-Chain Ki-
nodynamic Planning that directly finds e-optimal trajectories obeying bang-bang
control. The algorithm searches a reachability graph whose edges are generated
by constant fully-saturated control forces lasting for a problem-dependent Q(e2)
timestep rB. Thus, we can analytically relate e to a sufficiently short switching
time2. Although the algorithm is the most `?natural" generalization of our algo-
rithms for Cartesian Kinodynamic Planning to open-chain robots, we are only able
to show its correctuess and complexity using the advanced ?adversary game with
perturbations" proof technique introduced in Section 4.3.1.4.
5.1.1 "Minimalist" Control
5.1.1.1 Bang-Bang Control for Simple Dynamics
Recall that for a given instance of the Cartesian Kinodynamic Planning prob-
lem (O,S,G,?a,?v,l,co,ci) and approximation parameter e > 0, the algorithms
from Chapters 2 and 3 generate (?a, ?)-bang trajectories, wbere T is the problem-
dependent timestep chosen by the algorithm. By definition, the (vector) com-
ponents of the acceleration function of such a trajectory are piecewise-constant,
taking values fiom the set f--H?a, O,a? and switching only at multiples of r.
Suppose that each component of the acceleration function for a trajectory is
piecewise-constant and only takes values from the set ? --Ha, a). Then we call that
trajectory and its control completely bang-bang. (In this case, the control is the
acceleration function.) If, flirthermore, switching times only occur at multiples of
some timestep T, then we call that trajectory and its control r-bang-bang. Note
that because a single control value corresponds to only one trajectory for a given
initial state, bang-bang controls can be regarded as a minimal set of non-trivial
controls.
2Intuitively, we might expect that a discrete control function having the range in figure 5.1(d)
might, by "averaging", approximate the effect ofacontrol function with the range in figure 5.1(a),
provided that its switching times can be sufficiently short.
160
By extending the techniques used to show the approximation accuracy and
complexity of Algorithm 2, we obtain a reachability-graph algorithm that directly
finds a r-bang-bang ?-optimal solution, where T is problem dependent, whenever
a solution exists. The algorithm has complexity O(cdN(1?)3d)? for some robot-
dependent constant c.
5.1.1.2 Bang-Bang Control for Open Chain Dynamics
Now, recall the
open kinematic
kinodynamic planning problem formulation from Chapter 4. For
chain robots, the analogue of an (a,?)-bang is the application of
a constant control force f, such that fi E f?J?,O,J?,? for all i, for a timestep r.
We call this type of control primitive and the corresponding trajectory segment an
(f, r)-bang.
If the timestep r is adequately small, then the `?average" accelerations3 pro-
ducible by some number N? of consecutive (f, r)-bangs will have an acceleration
advantage over an &time-rescaled trajectory segment of duration N(?. By apply-
ing this observation and building on the ted?niques used to show the complexity
and correctness of the True-Extremal Algorithm from Chapter 4, we might ex-
pect to obtain a provably-good kinodynamic planning algorithm that synthesizes
solutions by searching a reachability graph generated by (f, r)-bangs. Such an al-
gorithm would be the natural generalization of our Cartesian robot algorithms to
open-kinematic chain robots.
With this in mind, and since there is an provably good approximation for
?-optimal Cartesian Ninodynamic Planning that considers only bang-bang trajec-
tory segments, one might expect there to be a similar "bang-bang" kinodynamic
planning algorithm for robots with open kinematic chain dynamics. This would
find bang-bangsolutions ones obeying piecewise-constant control force functions
f such that fj(t) E ??f?,ft,? for each joint i. Here, we demonstrate such an
algorithm. This algorithm searches a reachability graph whose edges are constant-
control bang-bang trajectory segments lasting for time T, the timestep chosen by
the algorithm. We call these trajectory segments r-bangs. Generalizing the previ-
ous definition in Section 5.1.1, a trajectory composed of ?-bangs is a r-bang-bang
trajectory.
The results of the previous chapter shows that we can find ?-optimal kinody-
namic trajectories that obey piecewise extremal control with ?(?) time between
3This will be defined shortly in Section 5.1.4.1
161
I			iO
f
v
control forces
dynamics laws,
state X
:1
A
I!
/1
`I
--H --H --H I
--H I
II			a
I			o+
I			??
VI			A
accelerations
Figure 5.2: The robot dynamics laws and the robot state X map applied forces
to accelerations. N r-bangs determine an average force to O( W) accuracy. If r
were infinitesimally small, or the dynamics equation time-invariant, we could use
?-bang-bang control to achieve 0(w') average acceleration control over a chunk of
N timesteps. If Nr is small enough, we still obtain a useful accuracy.
162
switchings. By showing the accuracy of the bang-bang algorithm, we show that
an e-optimal completely bang-bang kinodynamic trajectory with an Q(e2) time-
step will also exist. Note that like the algorithms in the previous chapters, this
algorithm is only sure to find e-optimal bang-bang solutions when some ?-safe
kinodynamic solution exists.
To summarize, by extending techniques used to show the accuracy and the com-
plexity of the True Extremal Algorithm (Chapter 4), we can show a theorem on
the adequacy of bang-bang control for near-optimal kinodynamic planning. Essen-
tially, the safety margin and start (goal) conditions of the e-optimal kinodynamic
planning problem regularize the classical optimal trajectory planning problem to
guarantee that bang-bang control is sufficient. This enables us to obtain an algo-
rithm that finds e-optimal trajectories by only considering trajectories that obey
bang-bang control; it is the first provably good bang-bang approximation algorithm
for open-chain manipulators.
5.1.2 Bang-Bang Results
We first describe a simple modification to the timestep criteria of of Algorithm
2 that yields a probably good e-approximation algorithm for Optimal Cartesian
Kinodynamic Planning that finds only bang-bang trajectories. In this manner, we
sketch a proof of the following:
Theorem 5.1.1 Let (0, S, G,?a, v, I, co, ci) be an optimal kinodyinic planning
problem for a point mass robot in d dimensions obeying L? dynamics bounds. Let
0< e < 1.
Suppose there is a ?(co,ci)-safr trajectory from S to G taking lime Topi. Then
we can find a (1 --H e)?(?,ci)-safe trajectory going from some S* (5*,5*) to
some = (g*,g*) within 0(e) of S and G, respectively, and taking time at most
(1 + e)Topt. The approximation error at S and G is indyendent of?v and 1.
For a given robot in a world offixed diameter, we can do this in 0 (cdN ((l)3d)
time, where c is a robot-dependent constant and N is the geometric complexity of
the obstacles.
For robots with open-kinematic-chain dynamics we describe an algorithm, the
Bang-Bang Algonthm, that attempts to find an e-optimal trajectory by chosing a
163
timestep TB and searching a reachability graph whose edges are r?-bangs.4 Since
the size of such a graph is generally exponential in path length (in terms of edges),
the search uses (bx, bv)-bucket pruning, which we described in Chapter 4?5
Theorem 5.1.2 Let (O,S,G,f,?v,M,c0,c1) be an optimal kinodynamic planning
problem for an open-chain manipulator with revolute and/or prismatic joints. Let
O<e<1
Suppose there is an optimal ?(?, ci)-safe trajectory from S to G taking time
Top?. Then the Bang-Bang AlgornOim finds a (1 --H e)?(co, ci )-safe trajectory taking
time at most Topt(l + e) from some = (s*,s*) to some = (g*,g*) such that
S* and G* are e-close to S and G, respectively. This trajectory obeys bang-bang
control, with switchings occun4ng only at multiples of an Q(e) timestep.
The asymptotic running time of the algorithm is 0 (cd?N2(d + log N) (l?)7d)
where c? is a constant dependent on the kinodynamic specifications and N is the
geometric complexity of the obstacles.
5.1.3 Minimalist Cartesian Kinodynamic Planning
5.1.3.1 Bang-Bang Approximation of an (a, r)-bang Velocity Function
The key to seeing Theorem 5.1.1 is to recognize that any (?a, ?)-bang trajectory
Fr can be approximated by a completely bang-bang trajectory as long as state-
constraints do not interfere.
Lemma 5.1.3 Suppose that Fr is an (a, r)-bang trajectory in the absence of ob-
stacles, and that it obeys velocity bound (?v --H L??T) Then there is a r-bang-bang
trajectory Fq that tracks ?q to tolerance (?jTj, L???) and obeys velocity bound v.
Proof:
Since the dynamics bounds are decoupled, it is sufficient to consider the one-
dimensional case. Let Ur satisfy the hypothesis of the lemma. Using the convention
from section 3.3.2.2, we will let c?? describe the acceleration of Fr in the interval
[nT, (n + 1)r], and c? describe the acceleration of Fq in the interval [fl47? (n+:)rl.
Define Fq as follows:
4This extends previous notation: for L?-norm acceleration bound a, a r-bang is a trajectory
segment having a constant acceleration whose components take values from the set f--H?a, ?aJ.
5Recall that the True-Extremal Algorithm uses (b?, b?)-bucket pruning with a different bucket
size.
164
1. Fq(O) = Fr(O).
2. Ifc??=1,then
4n			4n+1			4n+2			4n+3
Cq = cq			= Cq			= cq			= 1;
3.			otherwise, if c??			--H1, then
c4qfl = c4qfl+1 = c4qfl+2 = c4qfl+3 = --H1;
4. otherwise (c?? = 0),
4n
Cq			= c4?+3 = 1
q
c4?+1			--H			c4?+2			=			--H1.
q			q
By inspection, Ivr(1) --H vq(l)1 ? for all I E [0, Tri. Furthern?ore,
Fq(nT) for all relevant n. Position error only occurs when c??' = 0 and at most a;r6
accumulates. E
5.1.3.2 Parameters for a "Bang-Bang" Reachability Graph
Combining the lemma above with Corollary 3.3.4 we immediately get the following.
Corollary 5.1.4 Let r be fixed such that --Harir?. Let Vr have veThcity function Vr
obeying dynamics bounds (?v --H and
Suppose that
the multiple of TiT closest to vr(0)
5* vr(0)?--H97(vr(0)+s*).
Then, there exists a ?-ban9-bang trajectory Vq such that
1. Fq(O) =
2. Fq(T) approximates Fr(O) to tolerance (T?T2, 7??T);
9(1+c)2?ar2
3. For all I Ei [0, Tri, Vq(l + T) tracks 1'r(t) to toThrance (2T?T2 + 4?(2+?) ,2Tt?);
and
?. Fq(Tr + r) approximates Fr(Tr) to tolerance (3a?2,2a?).
Furthermore, Fq(O) is computable from a, v, T, and Fr(O).
165
Now, suppose we are given a Cartesian Ninodynamic Planning Problem in-
stance (0, S, G, a, v, 1, co, ci) and an approximation parameter ?. We can again
use the Safe Tracking Lemma (2.2.3) to choose ?(?) ?-safe-tracking parameters 71x
and ?Iv, which guarantee that if a trajectory Fr is ?(co, c1)-safe and Fq tracks Fr
to tolerance (?Jx,1?v), then Fq will be (1 --H ?)?(co,ci)-safe. R?ecalling the Efficient
Tracking Lemma 3.3.1, we can apply Corollary 5.1.4 and follow the development
of Section 3.3.2 and 3.3.4.1 to choose a root vertex S* and an ?(E) timestep r
to determine a reachability graph whose edges are (1 --H ?)&v(co, c1)-safe r-bangs.
Specifically, S* and r can be chosen so that if Fopt is an optimal solution, then some
r-bang graph trajectory Fq, F4q7 will track F10pi closely enough to be ?-optimal.6
Hence, the "bang-bang" algorithm for Cartesian Ninodynamic Planning simply
searches such a reachability graph to find an e-optimal trajectory. This is similar
to Algorithm 2.
5.1.4 A More General Bang-Bang Algorithm
For a given TB, recall that a r?-bang-bang trajectory is a bang-bang trajectory
with switchings occurring only at multiples of TB, and that a ??-bang is a constant
completely-saturated control of duration TB or a trajectory segment arising from
such a control and a particular state.
For a given an optimal kinodynamic planning problem for an open-chain ma-
nipulator, together with an approximation parameter (, we sketch how to choose
timestep TB, root vertex S*, and (6x, bv)-bucket pruning dimensions such that if
an optimal solution exists, then
1. some r?-bang-bang trajectory starting at S* tracks the ?-time-rescaled tra-
jectory F'r to a sufficiently close tolerance to be ?-optimal, and
2. the reachability graph rooted at S* and whose edges are (1 --H ?)?-safe ?B-
bangs will, after a breadth-first (bx, 6v)-bucket pruning, contain a path cor-
responding to an ?-optimal trajectory.
The Bang-Bang Algorithm is identical to the True-Extremal Algorithm (see
Section 4.2.1.1), except that it searches a reachability graph generated by ??-bangs
instead of by (a, ??)-bangs. Below, we show that TB and the dimensions of the
6Recall that F?r denotes Fr ?-tirne-rescaled and that F4qT(t) Fq(t + 4r)? See equations (4.5)
and (3.1).
166
pruning buckets, & and bv, are polynomially small in e, and that these parameters
result in the algorithm complexity in Theorem 5.1.2.
5.1.4.1 Bang-Bang Trajectories and Average Acceleration Advantages
Suppose that for a given kinodynamic planning problem the True Extremal Al-
gorithm selects an ?(e) timestep TE and a control discretization parameter iiE
Suppose that trajectory Fr obeys dynamics bounds f and v, and recall that F'r is
this trajectory e-time rescaled.
Furthermore, suppose that TB ? for some integer N. Over the interval
[nTE, (n + 1)TE], the average applied force can be approximated to resolution Af
N
at each joint i by some bang-bang control f? : [nr?, (n + l)TEl H fOrce, with
switchings only occurring at multiples of TB. Since the largest possible change in
the map from forces to accelerations over the interval is 0(TE) is 0(e), it follows
that average acceleration under fq approximates the average of ?`? over the interval
to resolution 2A?rna? + 0(TE) for each dimension i. (Recall that Arnar denotes the
global acceleration bound and ?? denotes the acceleration ffinction of the e-time-
rescaled trajectory Fr.)
To put this observation to use, we define an avei?ge acceleration advantage.
Let ? be a function ? : time fX : X is a set of accelerations). Let Y be a set
of acceleration functions. Suppose that for interval 1 [71, 71 + ATj] the following
is true: if a Ei ?(t) for all t E I and a is a d-vector of l's and l's, then there is
some a? E Y such that
--H a??(t))dt > tc1ATj
(5.1)
for each i. Then we say that y has an average h?j advantage ove? A during I.
5.1.4.2 Tracking With Bang-Bang Trajectories
We now consider a game similar to the Tracking Game of Section 4.3.1, differing in
the following way: in each round when you choose HIGH or LOW, the adversary is
only obligated to return an acceleration (for you) that has an average hi advantage
over his acceleration for that round. We let TG denote the amount of time simulated
in one round of this game. We only need to modify step three of round play in the
Tracking Game With Perturbations:7
7See Sections 4.3.1.1 and 4.3.1.4.
167
3. Let a0? = vn4?15vn. If you played HIGH, then your adversary can choose any
function a'?(l) (for your acceleration) such that
(n+1)r? a'?(t)dt>?a?0,+tcj.
nr,
If instead you played LOW, then your adversary's choice must obey the
condition
4G 1(n+l)rc a'?(t)dt ? a0? --H
flTg
In both cases, a'?(t) must obey the condition ia?'(1)I ? Amaz for all 1 E
[nr, (n t 1)T].
By following the Sim,ple Strategy of Claim 4.3.1, we can track the adversary to
a tolerance that is 0 (Am?t?rG?, Amaxrc).
Suppose we play d simultaneous games, in which the adversary chooses his
motions (taken as a trajectory in d-dimension) to match F'r Then we can track the
adversary to an 0 (Am2?x1Te2, Amax??) tolerance in each game. It follows that for a
fixed Amax, an average ?j-acceleration advantage, and a desired tracking tolerance
tolerance (?1, 7Iv) there is always a game timestep TG that not only guarantees we
can track the adversary to tolerance (71, 71v) Moreover, we can find such a TG that
is ?(?) if ??, 71, and Thv are all ?(c).
Now, suppose the accelerations the adversary chooses for you must obey the
original (not ?-time-rescaled) dynamics bounds, and that ATTB = TG for some
integer N. If TG is short enough and N is large enough, then for a given round,
the set of accelerations composed of N consecutive ??-bangs will always contain an
acceleration function a such that in each game i the adversary can return a?, as long
our trajectory tracks I'1? closely enough. Intuitively, we can think of a "chunk" of
N r?-bangs as "averaging" to approximate the effect of a a time interval of length
TG.
Using the techniques from Section 4.3.1.1, we can show that a there is a fun-
damental tracking tolerance (i'o, `ivo) a fundamental timestep To, and a minimal
"chunk density" N0 such that if
TG			?			To,
71			<			710,
o+11t			?			71v0, and
iv			?			No
(5.2)
168
and our trajectory tracks F'r to tolerance (7'z, liv), then the adversary can legally
choose a r?-bang-bang acceleration function for us. Moreover, we can find lizo,
livO, and ro that are all ?(e), while still assuring that some O(?(') chunk density N
will be sufficient. This leads us to conclude:
Theorem 5.1.5 (Bang-Bang-Theorem) Suppose that Fr obeys (4.7) and dy-
namics bounds f and V, is ?-saje, and takes time Tr. Then foi any e, we can
choose a timestep rB that is ?(e2) and ensures that U'ere exists some (1 --H e)6?-safe
r13-bang-bang trajectory Fq such that Fq(O) = Fr(O), and Fq((l t e)Tr) and l'r(Tr)
are e-close.
The import of theorem is similar to that of the Coupled Tracking Lemma (4.2.1)
with respect to (?, r)-extremal trajectories. It says that we can choose a root vertex
and a timestep that guarantee some graph trajectory will be e-optimal if an exact
solution exists. llowever, the size to the reachability graph might be exponential
in the longest (non-cyclic) path length.
5.1.4.3 Pruning a Bang-Bang Reachability Graph
To show that we can choose reachability graph and search-pruning parameters that
guarantee an e-optimal bang-bang trajectory will be found in the pruned search,
we again turn to a tracking game.
The Tracking Game With Multi-Perturbations
Consider a game in which you are trying to track an adversary in a one dim-
ensional space. The game is a series of rounds, each of which simulates motions
taking time TG. The game begins with adversary in state (ro vo) and you in state
(x'0, v01). A round simulates an interval of time with length TG. When discussing
the game, we will use X?, v?, etc, to denote your position, your velocity, etc.,
corresponding to round n.
Let game parameters Arnax and hj such that A?ax > Ki > 0 be given. Let N
be an arbitrary positive integer.
In each round the following takes place:
Play for Round n:
1. The adversary plays first by choosing an acceleration ffinction a?(t) for him-
self such that for all t E [nrc, (n + l)rGi, Ian(t)I ? Amax --H tct. His state
(Xn+i, Vn+i) at the end of the round (beginning of round n + 1 ) is computed
169
from his state (xn, Vn) and this acceleration function by integration over the
time interval [nTG, (n + 1)7c'].
2. For your play in round n, you then choose either HIGH and LOW. This
choice limits the acceleration function that the adversary can choose for you,
and that determines your state (X'?+1, v?'+i) at the end of the round. Note
that you play only HIGH or LOW; the adversary chooses your acceleration
as well as his own. However, his choice of your acceleration is constrained
by your play.
3. Let a0? = vn+%?vn. If you played HIGH, then your adversary can choose any
function a'?(t) such that
1? ;)+1)TG a'?(t)dt > a70? + Kj.
If instead you played LOW, then your adversary's choice must obey the
condition
j(n+1)ra a'?(t)dt < a0? --H
nrG
In both cases, a'?(t) must obey the condition Ia$?(t)I ? Amax for a11t E
[nTG, (n + 1)?ci
4. Your state (X'?+1, vr'?+i) at the end of the round is computed using your
state at the end of the previous round and integrating a??(t) over the time
interval [nr?, (n + l)TG]. Howevem the adversary can disturb this integration
by perturbing your state by up to (?, ?NTG) at each time that is a multiple
By following the reasoning from the proof of Claim 4.3,3 we get the following.
Claim 5.1.6 Suppose that you play the Tracking Game With Mulli-Pe4urbations
against an adversary and follow the Simple Strategyft?m Section ?.3.1.1. Suppose
that at the start Ixo--Hx'o ? 2A?axTG2 and Ivo--Hvo' < 2Afl(irTG. Let two trajectories
Fr and Vq be defined as follows:
Fr(nrc)			=
ar(t)			=			an(t--HnTa)
Vq(nrc)			=
aq(t)			=			a'n(t?nrG)
for all t c (n??, (n + l)rc);
for all I c (nr? (n + l)TG).
Then for all t:
170
Ivr(t) --H vq(t)1 < 4AmazTG + ?l2rG;
lxr(t) --H xq(t)1 < (8Amax+?t)2rc2
4-?I
.2 + ?(rr			(5.3)
1max'G			2
This claim and the way the game allows the adversary to perturb your move-
ment round correspond to a generalization of the ?obust Tracking Lemma. We
first describe a mapping from sequences of states to acceleration functions.
Let Q be a function Q : TO : ? is a set of acceleration functionsj, let
N be a positive integer, and let T be a duration of time. Let X be a sequence of
states ...... , XN --H i. We define the function
Qx*,N,T : TO H			: Y is a set of acceleration functions)
as follows. Let F be a trajectory of length T starting at a given state X0, and
let a be the associated acceleration. Then a E Q,Y,N,T(Xo) if and only if for each
n = 0,... --H 1 there is some a? C Q(Xu) such that a(t) = a?(t --H fl4?) for all
te [nT,(n+1)T].
Lemma 5.1.7 (Another Robust Coupled Tracking Lemma) Let ? and Q
be functions ?, Q TO H : Y is a set of acceleration ffinctionsj. Let T, have
the property that if a E `P(X) for some state X, then for all times t, Ia(t)I0o ?
Amax --H tcI. Let Q have the properties that Q(X) is finite for all X ? TO and that
if a E Q(X), then for all times I Ia(t) loo ? Amax.
Suppose that there exist a maximal tracking tolerance (nzo, nvo) a fundamental
timestep TO, and a chunk density N0 such that for all T < TO and N > N0 the
following is true: if the states in the sequence ?` = Xo,. . , Xy?1 are all within
(lizo, ?ivo) of state Y, then the set of acceleration functions Q.*4,N,r(X) has an av-
erage tc? acceleration advantage over ?(Y) over duration r.
Let (7Iz, ?1v) be an Loo tracking tolerance, and let tirnestep T obey the folThwing
znequah'ties:
T			?			To;
T			,y' ?4Amax?i+2?j2+(sAmax+?t)2
T ? 2min(??o,nv)
--H 8Amarthi
Let be q the (Q,?)-reachabihty graph rooted at some
and			(5.4)
171
Now, suppose that Fr respects ?, Fr(O) is within 2AmaxT ofs*, and pr(O) =
Suppose that?? is a subset of TC containing the (n?,nv)-tube induced by Fr, and
that ?` results from some breadth-first (`\`N7 ??`N? )-pruning of the maximal subgraph
of? that lies in 1?.
Then there is a Fq in ?` such that Tq < Tr + T, Fq(O) = S*, and Fq(Tq)
approximates Fr(Tr) to tolerance (nx,nv)
Proof: (Outline.) Similar to the proof of the Robust Coupled Tracking Lemma
(4.2.2). We can consider d simultaneous playings of the Coupled Tracking Game
With Multi-Perturbations where the timestep rG in the game is the same as T in
the lemma, the adversary's choice of our acceleration corresponds to Q, and per-
turbations correspond to bread-first prunings. Again, some trace of d simultaneous
playings of the Tracking Game with Multi-Perturbations in which you follow the
Simple Strategy corresponds to a "virtual path" in the pruned reachability graph,
where a "virtual edge" of zero cost goes from each pruned vertex to the vertex that
caused its pruning. (See figure 5.3.) El
Finally, to prove Theorem 5.1.2 we consider a given problem and approximation
parameter e. We choose Q to map states to accelerations determined by constant,
fully-saturated controls. Using the arguments above from the previous chapter, we
can show that there exist consistent parameters ,7(), ilvo, hj, and To that are
and an 0(e) timestep chunk density N0 that satisfy the hypothesis of the lemma.
The Safe Tracking Lemma would be used to find esafe-tracking tolerances n? and
?1v that are ?(e). Using Lemma 5.1.7, we choose an ?(e) "chunk" of time Tc and
the timestep
Tc
TB =
where N > N0, for the algorithm.
Then, if Fopt is an optimal kinodynamic solution, a breadth-first search of the
reachability graph with (h?NTn2 , ?%?yTB )-pruning would find a path that is e-optimal.
Recalling the argument outlined above, we see that some virtual path in the pruned
reachability graph would correspond to a trajectory that tracks `opt closely enough
to be (1 --H e)-safe and come "e-close" tp the goal state. Since ?j and rc are Q(e)
and N is O(--H?), the pruning bucket dimensions br and bv are ?(e4) and fl(e3) for
each degree of freedom.8
8Here N again is the d?unk size and not the geometric complexity of the problem.
172
L			J
I			(7I?.2,?v)-tube
1?,
`A
Figure 5.3: Reachability graph pruning and the Tracking Game with Multi-Pertur-
bations. In a breadth-first reachability-graph search with (bx, bv)-bucket pruning,
Xkiti causes edges from Xpr to be pruned from the search, However some trace of
d simultaneous playings of the Tracking Game with Multi-Perturbations in which
you follow the Simple Strategy corresponds to a virtual path containing a "virtual
edge" of zero cost going from Xpr to Xkitl.
173
Absorbing robot-dependent terms into a constant CF, it follows that the number
of edges and vertices in the reachability graph searched by the algorithm will be
O(cdF(??)7d).
5.2 Time-Safety Trade-Offs
5.2.1 A Finer Approximation Model
The original theoretical formulation of kinodynamic planning treats time- and
safety- optimality as a single quantity and considers the asymptotic computational
complexity of the algorithm as a function of the single approximation parameter e.
When we look towards using provably good approximation techniques in practice,
however, we observe that the total time needed to plan and execute a trajectory is
often more important than the time needed for plan execution alone. At the same
time, it is often more important to guarantee that the planned trajectory will be
"reasonably" fast and observe the safety margin closely than to guarantee it will be
nearly time-optimal. We therefore need to understand analytically how execution
time can be "traded-off" against safety and planning time while still requiring the
approximation to be provably good. Thus, the hope of reaping practical rewards
from theoretical developments motivates us to study how safety and time each fit
into the structure of kinodynamic planning.
Our planning algorithms allow one to choose the optimum compromise between
time and safety. Let CT and es describe closeness to optimality in execution time
and observance of the safety margin, respectively. A fired computational complex-
ity defines a bounding equicomplexity curve of (es, CT) pairs that can be satisfied
without exceeding the complexity. (See figures 5.4 through .5.7.) As the complex-
ity varies, it parameterizes a family of such curves, which allows us to trade off
es, CT, and planning time against each other. Examples of such curves graphi-
cally illustrate the impact of trade-offs on the size of algorithm search spaces. A
straightforward analysis is developed to cover the algorithms from the previous
chapters.
5.2.1.1 Approximation Closeness, More Precisely
We now specify what it means for one
another in terms of error parameters CT
kinodynamic solution to `approximate"
(speed) and C5 (safety). We can regard
174
the analysis in earlier chapters as having let
CT
to simplify the problem formulation.9
(5.5)
First, we say a solution Fq is e?-time?optimal if the time Tq it requires required
is bounded above by (1 + eT)Topt, where Topt is the time required by the optimal
trajectory. Second, for a given safety function ?(co, ci), we say Fq is es-safety-
optimal if the path ?q lies in an obstacl&free safety tube of diameter (1 --H
Third, letting emaz = max(es, CT), we say that a trajectory Fq : [0, Tq] H TC is
"emaz-Uos&' at the start S (goal G) if, at time 0 (time Tq), it achieves the start
(goal) position to within a O(ernax) additive error, and the start (goal) velocity to
within an 0(emax) multiplicative error plus a constant 0(emax) additive error. For
example, if Fq = (pq,pq), then both pq(O) --H Soc and 11(1 + C?ax)Pq(O) --H si
are10 0(emax). Finally, we define an (Cs, C?)-optirnal solution (relative to ?v(Co, ci)-
safety) to be a solution Fq which is ?5-safety-optimal, C?-tim&optimal, and "Cmax
close" at the start and goal.
5.2.1.2 Alternative Approximation Formulations
Note that the notion of approximate solution to an optimal kinodynamic planning
problem can be extended to observance of the dynamics bounds (4.4). For example,
consider a Cartesian Robot. We would say that a trajectory obeys acceleration
bound a to within CA if it obeys acceleration bound (l+e?)a. Similarly, a trajectory
obeys velocity bound v to within CV if it obeys acceleration bound (1 + ev)v. If
a trajectory obeys the acceleration and velocity bounds to within CA and Cv, we
say it is (CA, Cv)-executable, since it can be executed by a robot whose acceleration
and velocity bounds are greater by factors of CA and CV, respectively. At the same
time, another parameter CE could characterize how closely the solution comes to
meeting the desired trajectory endpoint conditions.
The parameters CA and e? would measure how effectively the planner uses the
dynamical capabilities of the robot in the following manner. Suppose that whenever
an exact solution to a safe kinodynamic planning problem (0, S, G,?a,?v, ?) taking
time T exists, a certain planner is guaranteed to find a solution to the related
9To simplify things we let ?T absorb state error at the start and goal
`0This last choice has been made with the anticipation that discretization will detern?ine the
closeness of the approximation. It is a simple way to characterize the worst-case accnracy and a
closer look at the analysis that follows reveals that the additive errors are in fact O(%m?T).
175
problem (0, S, G, (1 + ??)?a, (1 + ??r?, (1 --H ?s)?) that also takes time T. Then
that planner would "use" at least l+)A and of the robot's acceleration and
velocity capabilities.
Since the planned trajectory can make use of ?vv and ??a dynamics advan-
tages over the bounds obeyed by the optimal trajectory, our algorithms and proof
techniques extend to this problem formulation. However, we will only discuss
(es, e?)-optimal kinodynamic planning here.
5.2.2 Algorithms That Allow Compromise
To search for (es, e?)-optimal trajectories, we us algorithms from Chapters 3 and
4 to search reachability graphs almost exactly as before. Graph edges now obey
(1 --H e5)?s-safety, and other parameter choices are re-derived, using e? and CT
appropriately instead of e. We describe how this is done and revise our complexity
analysis.
5.2.2.1 Parameter Choices For `I?ade-Offs
For a fixed kinodynamic planning problem and given approximation parameters es
and CT, the complexity of Algorithm 2 and 3 (Chapter 3) for Cartesian Kinodyna-
mic planning is determined by the size of the reachability graph that the algorithm
searches. This size, in turn, depends on the timestep T chosen by the algorithm.
The timestep chosen therefore shrinks with both es and CT. For the more general
Near-Extremal and True-Extremal Algorithms (Chapter 4), reachability graph de-
gree depends on the size of the force (or acceleration) discretization parameter ?
(or tt), which shnnks only with CT.
An algorithm for finding (es, e?)-optimal trajectories uses the Safe Tracking
Lemma the same way the related algorithm for finding ?-optimal trajectories does.
Given C? > 0 and ?-safety parameters Co and c1, the lemma specifies a one-
parameter family of e5-safe-tracking tolerances (iix, ?v):
cp(s
ci(1--H(s)+P
<--H ?7?v.
(5.6)
Next, the algorithm chooses reachability graph parameters that guarantee that
if an exact solution exists, for some optimal trajectory Fop?, a graph trajectory Fq
will track F%t to a safe-tracking tolerance11 (7ix, 7/v) For a given tracking toler-
11For a given trajectory Fr we now let F'r denote Fr ??-time-rescaIed
176
ance and ?T, the appropriate tracking lemma(s) be used to choose an adequately
small timestep,12 with ?T substituted for ?. However, to minimize the size of the
reachability graph, the algorithm must maximize the timestep r, which requires
choosing the parameter P in equation (5.6) appropriately.
5.2.2.2 Maximizing the Tjmestep
The technique from Section 3.3.4.1 can be used to choose P in equation (5.6) to
maximize the timestep required by the appropriate tracking lemma. Recalling that
each tracking lemma (i.e., Lemma 3.3.1, 3.3.9, or 4.2.1) dictates that the timestep
r to be at most the minimum of an expression dependent on 7Ix and another
dependent on ??, we define functions Tx(P) and Tt,(P) and consider their values for
? that equalize them.
Let us consider an algorithm for Cartesian Ninodynamic planning that is a
"hybrid" of Algorithms 2 and 3. We will refer to this as the Hybrid Algorithm
for Cartesjan Kinodynamic Planning. It simply computes the timestep that
would be chosen by each algorithm according to ?s and ?T? computes an upper
bound on the size of the reachability graph that would be searched by each algo-
rithm, and executes the one for which the bound is lower.
Corresponding to Algorithm 2, we have
rx2(P) ?			4cT(2+?T)nx(P)
3a(3+lO?T+S(?2)'
Tv9 ?			and			(5.7)
rmaz2(P)			minp>o(Tz?(P),Tv2(?))
We consider the general case for maximizing the timestep chosen by any one
of our algorithms. For arbitrary constants A145, and B, and with ?(?) and
nv(P) given by (5.6), let us specify:
Tv ? B?(4);			and			(5.8)
Tm,ax			minp>o(rr(P), Tv(P))
12The appropriate tracking lemma (i.e., Lemma 3?3.1 3.3.9, or 4.2.1) matd?es the robot dy-
namics while allowing the largest timestep.
177
Consulting Mathematica, we find
(5.9)
(5.10)
(5.11)
(5.12)
(5.13)
2Bcoes
ci(1 --H es) + $2i(1 --H es)
It follows that if
2+
4B2cocs(A3tT2+A4(T+A5)
Al(T2+A2cT
AieT2+A2eT			4B2coes
A3eT2 + A4eT + A5 --H c21(1 --H es)2,
then
otherwise
Tmax =0 so3ck(s)))2):
Tmax = 0 c?B1$O%s)
(Recall that "o( exp)" means "larger than c * exp for some constant c > 0.")
Substituting in the appropriate terms, we obtain
Scoes
7max2 = a 7ci(1 --H es) + 49ci2(l --H e5) +
which simplifies to (3.10) when e = = CT.
5.2.3 Bounds For Trade-off Algorithms
5.2.3.1 ?ade-offs Basic Kinodynarnic Planning
Returning to the Hybrid Algorithm for Cartesian Ninodynamic Planning, we now
tighten Algorithm 3's bound on the timestep to reflect the tighter bounds obeyed
by a rescaled trajectory:
= coes(l+CT)
Tmax3			2?a(1 --H
cl ?+? + 5?v
(5.14)
We conclude:
Theorern 5.2.1 Let K = (0, S, G,a, v, 1, co, ci) be an optim& kinodynamic plan-
fling problem for a polyhedral CaNesian i?bot obeying L? dynamics bounds. Let
0 < es < 1, and let CT > 0.
178
Then if )c has a solution, the Hybrid Algorithm for Cartesian Kinodynam'c
Planning derived from Algorithms 2 and 3 finds an (es, e?)-approximately optimal
safe solution in time
0			___
where N is the geometric complexity of the problem, c is a robot-dependent constant,
and r is the largest timestep such that T _ =? and
r < max			8c?e5			(5.15)
a 7ci(1 --H es) + 49ci2(i --H es)2 + 48co(s(3+l0cT
coes(l + CT)
(ci2ai(+14s) +sv)
A corollary describes the asymptotic (e5, CT H 0) behavior of the hybrid algo-
rithm.
Corollary 5.2.2 Suppose that Tmar2 and Tmax3 as given in (5.13) and (5.13) are
both less than Va Then the following describe the asymptotic complexity of the
Hybrid Algorithm for Cartesian kinodynamic Planning:
1. As long as Tmax2 < Tmax3 and
CT(2+CT)			4Scoes
(3 + lOeT + 5e?2) ? 49ci2(1 --H Cs)2
the complexity of the algorithim is
O(cdN( 1
CsCT
as e? and CT go to zero.
2. Otherwise, the complexity of the algorithm is
O(cd			13
N(--H) d)
es
as es and CT go to zero.
In both cases, N is the geometric complexity of the obstacles, and c is a robot-
dependent constant.
179
5.2.3.2 T?ade-offs Lemmas For Robots With Coupled Dynamics
Bounds
For Cartesian Robots with L2-norm dynamics bounds, we simply alter the choice of
discretization parameter jt and timestep T in the Near-Extremal Algorithm (Chap-
ter 4). Acceleration advantage tcj and discretization jt are given by
Kj = = aCT			(5.16)
4$d
Recalling the Coupled Tracking Lemma (4.2.1) and the development in Section
4.3.2.2, we find that the choice of timestep (4.40) is replaced by
r = mm veT			coes			(5.17)
co?s(167d
4a 2a ci(1 --H es) +			c21(1 --H es)2 +
The minimally modified algorithm is called the L2 Cartesian Algorithm
with ?ade-offs. Straightforward compntation yields the following lemma.
Lemma 5.2.3 Let K = (O'?,S,G,a,Thl,co,ci) be an optTh?al kinodynamic plan-
ning problem for a Cartesian robot with L2-norm dynamics bounds. Then the
complexity of the L2 Cartesian Algorithm With Trade-offs applied to this problem
is O(cdN(D?)6d?1) when
coes
veT
4?a 2a ci(1 --H e5) + c21(1 --H es)2 + cocs(l6?
Otherwise, the complexity is o(cdN(4)9??1 (Ds) 32d) if
3eT			3coes
lGXi+3eT --H 2ac21(1--Hes)2'
and O(jN(WT)3d?1(%s)3d) if not. In both cases, N is the geometric complexity of
the obstacles, and c is a robot-dependent constant.
The three complexity expressions correspond to how the algorithm chooses its
timestep. First, when the safety approximation constraints are weak enough, the
need to obey the velocity bound v appears to determine the timestep; this is caused
by the algorithm's exclusive use of near-extremal accelerations. Second, when the
180
constant and speed-dependent parts of the safety function are "balanced relative
to es and CT", es and CT both contribute significantly to the choice of timestep.
Finally, when the constant part dominates, C? dominates the timestep choice.
For open-kinematic chain robot, the choice of timestep for the corresponding
Near-Extremal Algorithm With `b?ade-offs would be very complicated. How-
ever, we can still characterize the behavior of the algorithm's complexity. Recalling
the origin of fundamental tracking-tolerance parameters ?xO and n?o and timestep
To, we see that these parameters are ?(CT). Similarly, acceleration advantage tc? is
also ?(CT). Referring to the Coupled Tracking Lemma and equations (5.6) (5.8),
we obtain:
Lemma 5.2.4 Let )C (O,S,G,f,v,l,co,c1) be an optimal Mnodynamic plan-
n'ng problem for a open kinematic chain robot. Then the complerity Near-Extremal
Algorithm With Trade-offs'3 for this problem is O(cdN(d$log N)(DT)6d?l) when CT
? sufficiently small compared to es. Otherwise, there erist positive, robot-dependent
constants A1, A2, which are independent of AT, such that the complexity is
if
o(cdy(d+ ?0g???k?9?d?1(Q?r3d?
CT
CT			coC
A2tCT ? Aicf(1--Hes)2,
and O(cdN(%?)3d?l(Ds)3d) otherwise. hi both cases, N is the geometric complexity
of the obstacles and c is a robot-dependent constant.
The complexity describes three ranges of behavior. In the first, either (a) the
timestep chosen is the fundamental timestep (see Section 4.2.2.4), which is depends
on CT but not es, or (b) the need to obey the velocity bound v appears to determine
the timestep. As with the L2 Cartesian algorithm, when the speed-dependent part
of the safety margin is significant, neither nor e? and CT dominates the timestep.
Again, when the constant part dominates, the e5 dominates the timestep choice.
5.2.4 Complexity Surfaces and Equicomplexity Curves
Figures 5.6 and 5.7 show the computational complexity of the Hybrid Cartesian
Kinodynamic Planning Algorithrn and the L2 Cartesian Algorithm with Trade-Offs
`3This is the Near-Extremal Algorithn? fron? Chapter 4 with the n?odiflcations sketched above.
is'
as a function of the approximation parameters ?? and ?T The complexity (vertical)
axis is a log10 scale, so the figures reveal how changing the parameters slightly can
greatly affect the running time of the algorithms, especially when parameters are
small. In particular, changing ?? or ?T by a factor of two can change the running
times of the algorithms by orders of magnitude. Of course, as either ?? or ?T gets
close to zero, the complexity grows enormously.
The contour lines in Figures 5.4 and 5.5 illustrate how a fixed discretization
or, equivalently, a fixed computational complexity defines a one-parameter fam-
ily of solutions to a given kinodynamic planning problem. Each solution can be
classified by its safety approximation ?s its time approximation ?T Hence a fixed
discretization defines a curve of solutions (really equivalence classes) in the ?s-
?T plane. These "equicomplexity curves" sweep out a surface in complexity-?s-??
space which, for a given problem instance, completely characterizes the guaranteed
behavior of the algorithm and its solutions. Hence, we can move along a contour
line to "trade off" ?s between ?T at a fixed complexity bound, or we can move
across contour lines to alter the running time of the planner.
Finally, we note that an equicomplexity curve can be the composite of two level
curves corresponding to the different expressions that can determine the timestep.
(Say, the two determined by the timestep choices from which the minimum is taken
in (5.17).) Furthermore, if we consider equation (5.9) and fix T, we find that the ?T
terms are "separable", and with algebraic manipulation we can reduce the problem
of expressing ?T in terms of ?s and T to a quadratic equation:
4B2(A3?T2 + A4?T + A5) (A1?T2 + A2??)(4Bco((B + ci?)?s --H QT)). (5.18)
5.2.5 Using Trade-offs with an Implementation
We illustrate time-safety trade-offs with trajectories found by our planner. Al-
though the planner has been used on more interesting problems, such as the one
in Figure 5.8, simple examples are sufficient to show how trajectories found by the
planner change with ?s and ?T, and how the planner running time also changes
with these parameters.
In Figure 5.9a we see a simple environment with a start state in the lower left
and a goal state in the upper right. For the examples, we used the kinodynamic
bounds: a = 0.2, v = 0.3, co = 0.2, and c1 = 0.5. With ?S = = 0.6, the planner
1.
i.sj
1.251
0.5
0.251
182
0.2			0.4			0.6			0.8
Figure 5.4: Example of a contour map of computational complexity as a function
of ?S and ET for the Hybrid Algorithm for 2D Cartesian Kinodynamic Planning
(sketched in section 5.2.2.1). Lines represent both constant complexity and con-
stant discretization.
183
1.?5
1.5
1.25
0.?5
0.5
o .25
I			I			I
0.2			0.4			0.6			0.8
Figure 5.5: Example of a contour map of computational complexity as a function
of ?s and ?T for the L2 Cartesian Algorithm with Trade-offs
184
Figure 5.6: Example of a three-dimensional plot of computational complexity as
a function of ?s and ?T for the Hybrid Algorithm for 2D Cartesian Kinodynamic
Planning (sketched in Section 5.2.2.1). For a given problem instance, such a sur-
face completely characterize the guaranteed the behavior of the algorithm and its
solutions. Complexity (vertical) axis is log10 scale,
185
25
Figure 5.7: Example of three-dimensional plot of computational complexity as
a function of ?s and ?T for the two-dimensional L2 Cartesian Algorithm with
Trade-offs. Complexity (vertical) axis is log10 scale.
186
Figure 5.8: A trajectory found by our planner. a = 0.2, v = 0.3, Co = 0.15,
ci = 0.2, Es = 0.6, and e? = 0.8. World dimensions are 4 by 3. The planner took 9
minutes and 23 seconds of runtime (14 minutes and 38 seconds total elapsed time)
on a 32Mbyte Sparcstation2. Coal state is at lower right.
187
found the solution in Figure'4 5.9b. This is the same example as in Figure 3.8,
but the figure here also shows accelerations. The solution takes time 11.1 units,
and required 4 minutes and 32 seconds of run time to compute on a 32Mbyte
SparcStation2. For ? = 0.55 and eT = 0.9, and es = 0.7 and CT = 0.4, the
planner chose the same timestep within two percent, and found the trajectories in
Figure 5.10a and 5.10b, which take time 11.1 and 10.5 respectively. Planning took
took 4 minutes and 29 seconds for the former and 4 minutes and 15 seconds for
the latter.
In Figure 5.11a, which shows the three trajectories together, we see how the
"less safe" trajectories come closer to the obstacles to shave off time. Figure 5.11b
shows the trajectory the planner found for es = 0.45 and CT = 0.5, which takes
time 11.8, and required 20 minutes and 57 seconds of run time to find.'5
The trajectories in Figure 5.11 are all quite similar even though they obey dif-
ferent guaranteed approximation conditions. However for a ?nearby" problem,
different choices of es and CT cause the planner to find trajectories that are not in
the same homotopy class. We see this in Figure 5.12, which shows two solutions
our planner found for problem with a slightly different goal state, but the same ki-
nodynamic parameters. It appears that with es = 0.55, a trajectory going between
the squares has to slow its progress in the r direction enough so that is faster to go
around the second square, whereas with es = 0.7, a trajectory can ?cut corners"
closely enough so that this would not be true. Figure 5.13 and 5.14 illustrate how
the choices of es and CT can affect the trajectories found by our planner even more
dramatically.
`4The original goal state is also shown but the start state is not as it would cover the solution's
start state completely.
15Total elapsed time, which includes idle process time (?.g. swapping), was 52 minutes
188
(a)
Figure 5.9: An illustrative problem and a solution found by our planner, with
= = 0.6. The kinodynamic bounds of the problem were: a = 0.2, v = 0.3,
Co = 0.2, and ci = 0.5.
189
(a)
Figure 5.10: Solutions found by our planner for the same problem, with (a)
= 0.55 and ? = 0.9, and (b) ?s = 0.7 and ? = 0.4.
190
(a)
/ /
r
// /
(bJ
Figure 5.11: Four solutions found by our planner for the same problem. (a) shows
trajectories for ? = 0.55 and ?T = 0.9, ?s = = 0.6, and ts = 0.7 and ?T = 0.4.
(b) shows a trajectory found with ?s = 0.45 and ?T = 0.5, which took the planner
about five times as much runtime.
191
(a)
Figure 5.12: Different Es and ?T choices can result in homotopically different tra-
jectories: (a) ?s = 0.55 and ?T = 0.9(b) ?s = 0.7 and ?T = 0.4.
192
Figure 5.13: Another trajectory found by our planner. a = 0.2, v = 0.4, Co = 0.15,
ci = 0.2, ?s = 0.6, and ET = 0.8. World dimensions are 4 by 3. The planner took
24 minutes and 34 seconds of runtime (29 minutes and 14 seconds total elapsed
time) on a 48Mbyte Sparcstation2. Goal state is at right.
193
Figure 5.14: A trajectory found by our planner for the same problem, but with
= = 0.8, is homotopically different. The planner took 10 minutes and 48
seconds of runtime (11 minutes and 56 seconds total elapsed time) on a 48Mbyte
Sparcstation2.
194
5.3 Summary
We have presented two extensions of our kinodynamic results. First, we showed
how to obtain a provably good polynomial-time ?-approximation algorithm for
optimal kinodynamic planning for open-chain robots that directly finds ?-optimal
bang-bang trajectories. Because the branching-factor of its reachability graph does
not change with ?1 (it is 2d), this algorithm is in a sense the ?natural" extension
of our algorithms for (L?) Cartesian robots. However, to show the correctness
of the algorithm, we found it necessary to extend the techniques of Chapter 4.
In particular, we generalized the notion of an acceleration advantage and adapted
the Tracking Game with Perturbations to show that we could prune the search of
the reachability graph to 0((?1)7d) vertices. Our results also show that an
switching frequency is sufficient for an ?-optimal bang-bang trajectory.
Second, we presented a finer complexity analysis of the algorithms from previous
chapters. Using two approximation parameters ?T and ?s to express closeness to
time-optimality and observance of the safety margin, we showed how, to a certain
extent, the algorithms from previous chapters allow one to trade off planning time,
execution time, and observance of the safety margin. We illustrated these trade-
offs with graphical representations and with empirical results obtained using our
prototype planner, which executes a version of Hybrid Algorithm for Cartesian
Kinodynamic Planning we described in this chapter.
Chapter 6
Conclusion
In this thesis, we have presented the kinodynamic planning framework for optimal
motion planning, and we have described a family of provably-good, (pseudo-)po-
lynomial-time ?-approximation algorithms for optimal ki nodynamic planning.
First, we motivated a constrained optimization problem that (a) matches the
requirements of finding a time-optimal planned trajectory and (b) assures that
when an optimal solutions exist, non-identical ?-optimal solutions would exist.
This problem includes a safety margin, which we allow to be speed dependent, for
obstacle avoidance. We then formalized this problem for decoupled dynamics and
decoupled dynamics bounds, calling it the Optimal ?artesian Kinodynamic Plan-
ning problem, and presented a provably-good polynomial-time ?-approximation
algorithm for it. This algorithm which reduced the problem to finding short-
est path in a uniform-weight reaU?ability graph, was the first such algorithm for
time-optimal motion planning in more than one dimension. The "tim&rescaling-
tracking" proof technique we introduced is the basis of all known correctness and
complexity proofs for provably-good approximation algorithms for optimal kino-
dynamic planning, except for our Algorithm 3 (from (?hapter 2). In this proof
structure, a tracking lemma relates a tracking tolerance and dynamics bounds to
sufficient reachability graph parameters.
Using progressively more sophisticated constructive tracking-trajectory proofs
and adversary game proofs, we obtained a series of tracking lemmas that we used
to obtain more efficient algorithms and algorithms for kinodynamic planning for
robots with less restricted dynamics laws and dynamics bounds, including fully-
controllable robots obeying full ("open-chain") Lagrangian dynamics. Extending
these basic results, we showed the existence of an approximation algorithm for
195
196
finding ?-optimJ bang-bang trajectories, and we presented a closer analysis that
uses separate approximation parameters es and ?T to describe the quality of the
approximation in terms of observance of the safety margin and closeness to time-
optimality. This analysis allows one to trade of safety, execution time, and planning
time to an extent when using our algorithms. We presented an implementation
and some simple experiments, which illustrated these trade-offs, in a restricted
domain. All the algorithms we presented are provably-good polynomial-time ?-
approximation algorithms.
We now sketch some research directions for the future and offer a few closing
words.
6.1 Future Directions
Two main goals stand out for friture research provably-good near-optimal kino-
dynamic planning. First, while tlie polynomial ?-approximation properties of the
algorithms presented here are theoretically encouraging, it would be desirable to
have provably-good approximation algorithms that are fast enough to be useful in
practice. Second, we have addressed a limited problem formulation, and the ques-
tion of whether provably-good polynomial time approximation algorithms exist for
more general problem formulations is open.
6.1.1 Towards Practical, Provably-Good Algorithms
We see several research directions towards practical, provably-good approximation
algorithms for optimal kinodynamic planning.
Improving Techniques Described in This Thesis. One strategy would be
to improve the current algorithms.
o+ Our algorithms naively search for a shortest path in a graph breadth-first.
They could be improved by using A* search. One simple admissible heu?stic
cost function would be the length of the shortest path from a vertex position
to the goal position. A table of approximate L?- or L1- shortest-path lengths
to the goal can be computed cheaply with existing path-planning techniques.'
Other cheaply-computable admissible heuristics might be possible.
iDiscretized all-sources-single-sink L?- or L1- shortest-path planning can be done in several
seconds on a SpracStation2 for a 256x256x128 discretization of configuration space [R,es92]. Also,
see [LRDG9o].
197
o+ A close examination of the proofs for Cartesian IQobots shows that the time-
step is chosen with the assumption that all e-optimal trajectories might have
to pass through a narrow channel between obstacles. Away from such chan-
nels, the current e-safe-tracking-tolerance criterion appears to be unnecessar-
ily harsh for choosing a timestep, and it seems intuitively plausible that a
larger timestep should suffice for generating graph edges. Unfortunately, it
does not appear that the timestep can be increased for robots with state-
dependent dynamics as long as the algoritlim's proof relies on that Coupled
Tracking Lemma (4.2.1) or the Robust Tracking Lemma 4.2.2.
o+ Because of the structure and spatial characteristics of the reachability graphs
they search, parallel implementations of our algorithms hold promise.
1. The shortest path between two vertices in a uniform-cost directed-graph
can be found a parallel "brushfire" or "wavefront" algorithm, which is
based on a simple propagation rule: if in the current search generation
a vertex is "told" for the first time that it has been found, then in the
next generation it tells its neighbors that they have been found.
2. A "natural" assignment of vertices to SIMD processors would map one
processor to each family of vertices that have the same configuration
component. When the robot dynamics laws are constant, the reachabil-
ity graph is translationally invariant in configuration (position) dimen-
sions if there are no obstacles, and this mapping would therefore allow
uniform routing.
3. If configuration space is divided up into sufficiently large (hyper-) cubic
regions, then all neighbors of a given graph vertex whose configuration
lies in a particular region will lie in that region also, or in a spatially
adjacent region. If we map vertices to processors by region, as the
side length gets larger, the ratio of computations to communications
will become proportional to the ratio of the volume of each region to its
area. It might be possible to keep processor utilization high by assigning
multiple regions to each processor, using a repeating pattern.
Approaches that are radically different from what we have presented might be
at least as productive.
19S
Optimization Approaches. One might be able to show that certain opti-
mization-based approximation algorithms for optimal kinodynamic planning are
provably-good. For many applications, some of these algorithms might be fast
enough to be practical, even if their asymptotic complexity is exponential.
o+ Some parameterization of trajectories might include e-optimal motions, grow
reasonably with the geometric complexity of the environment and ? and
otherwise be suitable for a numerical optimization approach. Cubic-spline
paths are a possibility, and they have been used in approaches not shown to
be provably-good e-approximation algorithms. (See Section 1.4.)
o+ The local minima of the parameter space would have to be understood well
enough so that a provable optimization technique would be guaranteed to
find one that corresponds to an e-optimal kinodynamic trajectory when an
optimal solution exists.
Other Notions of Provably Good. Algorithms that are provably good under
reasonable, well-defined alternatives to a worst case analysis of accuracy might be
faster than provably-good c-approximation algorithms.
We might define a p??babli%stica11y p?vab1y-good ?-approximation a(gomlhm to
be one that, under suitable distribution assumptions about the start and goal
states and the obstacles, is guaranteed to find an e-optimal trajectory with
some probability Ps if an optimal trajectory exists. Note that the algorithm
itself does not have to use randomization.
o+ It is to be expected that the geometric complexity of the obstacles, their scale
and the scale of the gaps between them, and P5 will all affect the complexity
of such an algorithm. More generally, we might consider using an expected
case analysis of complexity.
o+ Algorithms that are fast for a fixed e and P3 would be useftil, particularly if it
could be shown that randomly perturbing the start and goal state according
to some distribution would, with some known probability, yield a problem
for which the algorithm would succeed.
o+ Anytime algon'thms [DBss] that have these properties would be desirable.
199
o+ Algorithms for finding trajectories that are guaranteed to be (1 --H ?s)?-safe
and expected to be ??-time-optimal (according to some problem distribu-
tion) would be useful. Furthermore, we could allow ?T be a function of the
complexity of the obstacles.
6.1.2 Theoretical Extensions
There are open questions concerning how much we can demand from provably-good
approximation algorithms for optimal kinodynamic planning.
Tighter Approximation Conditions. It would be interesting to consider
approximate optimal kinodynamic planning with tighter solution requirements.
o+ To ensure that the approximation problem is "reasonable", our problem for-
mulation allows ?-optimal trajectories to be ?-close at the start and goal and
only requires that these trajectories obey (1 --H ?)??-safety. It would be useful
to precisely characterize the conditions that guarantee the existence of a set
of finitely-parameterizable near-optimal trajectories whose paths each inter-
sect the path of the optimal trajectory finitely often. One would then hope
to develop a provably-good approximation algorithm to find near-optimal
trajectories that are exact at the start and goal and/or that are ??-sate when
these conditions are met.
Results in this direction might also enable us develop proof techniques be-
sides "rescaling-tracking" for showing the correctness of ?-approximation al-
gorithms for kinodynamic planning. An alternative proof technique could
yield algorithms of lower time-complexity for the present problem formula-
tion.
Generalizing the Problem. Provably-good approximation algorithms should
be developed for other robot classes and for problem formulations that address
questions in motion planning closely related to the kinodynamic planning problem
defined in this thesis. ?Ve do not include the "single-start, multiple-goal" and
"single-goal, multiple-start" versions of the problem, because our algorithms can
be used to solve them with trivial modifications.
2To solve the forn?er we sii?ply check for approximation of each goal during the read?ahility
graph search, The latter prohlem is the former under `?time-reversal?
200
0
For wheeled robots, the set of feasible accelerations has a lower dimension
than the configuration space. While provably-good algorithms for a vari-
ety of path planning problems for this class have been given [Wil88,FW88,
JC89], the corresponding minimal-time trajectory problems remain open. It
is unclear whether the "rescaling-tracking" technique could be extended to
show the correctness of approximation algorithms for such robots, although
adversary-game proofs would seem to hold promise.
Algorithms for robots with dynamics not described by equation (4.7) should
be explored. Furthermore, the dynamics constraints in the problem formu-
lation should be generalized. It would be particularly interesting to include
power constraints, friction, and non-holonomic constraints [LCS9j.
o+ Moving obstacles could be incorporated into the kinodynamic planning prob-
lem. Fraichard and Laugier [FL92] have made progress in this direction.
o+ There are other quantities besides trajectory time that might be used as a
cost function. A classic example from control theory is hiel consumption.
Secondary cost functions, such as (3.17), and combinations of cost functions
might be useful in obtaining natural" motions with provable properties.
o+ Provably-good ?-approximation algorithms that are also anytime algorithms
[DB88] would be desirable.
6.2 Closing Thoughts
Kinodynamic planning represents a new direction in algorithmic motion planning.
Computational kinodynamics seems a particularly fruitful area in which to pursue
provably fast, provably good approximation algorithms, since while the problems
are of considerable intrinsic interest, exact solutions may well be intractable. We
have presented such algorithms for optimal kinodynamic planning for a family of
robot classes. While the problems have an optimization flavor, our algorithms
and proof techniques draw on several branches of computer science and robotics.
There is a great deal of challenging theoretical and experimental work to be done,
especially in the direction of practical approximation algorithms for optimal kino-
dynamic planning.
Appendix A
Kinodynamic Planning Lower
Bounds
This section sketches how to extend Canny's and Reif's proof [CR87] of ??-
hardness of the 3D shortest-path pwblem to show that Optimal Cartesian Kino-
dynamic Planning in 3D is also Af?-hard?1 While the general description here
should convey the flavor of the result, the technical specifics are intended to ac-
company or follow a close reading Section 2 of [CR87].
The idea for the extension is simple: if we could set the acceleration bound
a to 00, the reduction would be trivial because the problems would be identi-
cal. However, to do a polynomial-time reduction, we must show that for a given
shortest-path problem instance we can find a `?sufHciently large" a that only re-
quires a polynomial number of bits to encode.
A.1 Canny's and Reif's Proof
A.1.1 A Reduction
To prove that the 3D shortest-path problem for a point among polyhedral obstacles
is Af?-hard, [CR87] give a polynomial-time reduction from 35AT. They describe
how to take a given instance of 3SAT and construct an instance of the polyhedral
path-planning problem sud? that a shortest path from the start position to the goal
position can be used to determine whether the 3CNF formula in the instance is
`This claim was made in [CDRX88?, but without proof.
201
202
satisfiable.2 The proof considers a 3CNF formula F of m clauses C1, . . ,Cm over n
variables .1...., bn and their negations. The polyhedral environment constructed
by the reduction has a description of length O(rnn). The construction allows one
to compute (in O(rnn) bits) a path length lower bound 1 and an accuracy 6ac such
that the shortest path between the start and goal positions has length greater than
1 t 6ac if and only if the formula F is not satisfiable.3
A.1.2 Correspondence Between Homotopy Classes and
Truth-Value
The polyhedral environment will be the interior of a square box separated into
levels by flat plates, each having one, two, or three slits. The plate thickness, slit
width, and plate separation will all have the same size ecR. In addition, internal
walls will divide the spaces between certain plates into rooms having one slit on
the "ceiling" and one slit on the "floor". The start position will be above the top
plate, and the goal below the lowest.
The environment is constructed so that it will have special properties that we
describe, avoiding details as much as possible. Recall that each clause Cj in a
3CNF formula is a disjuction of literals 1ji V 1?2 V l??. If we separate paths that go
from the start to the goal without ever visiting a level twice into their homotopy
equivalence classes, then each class will encode a truth assignment for ..... . ,
and the evaluation of a conjunction of m literals, one from each clause of F, using
this truth assignment. In other words, if p? is the disjunctive normal form of F
obtained by distribution, then for each term (conjunction of literals) in F' and
each truth assignment, there will be a corresponding homotopy class. If the truth
assignrnent fails to satisfy the term formula, then some obstacle will `?stretch" the
shortest path in the class. Furthermore, this path will have a length greater than
than 1 + 6ac if and only if the truth assignment fails to satisfy the term formula.
A.1.3 Bit-Counting
To show their reduction is polynomial-time, in the end [CRS7i must show that
the number of bits necessary to encode the corresponding shortest-path problem
2 "3CNF" means "three-literal conjuuctive-norn?al form forn?ula?"
3We mostly follow the notation of [CR87] in this section. In particular, "1" is used as in
[CR87], not as in the rest of the thesis it does not n?ean "world dian?eter". 6ac is not found
in [CR87], but it just takes the place of a n?ore con?plicated tern?.
20:3
instance will be polynomial in n and m. The main problem is to show that ?CR does
not have to be too small. Suppose that the construction procedes for 3CNF formula
F. Let 1?nstretched denote the length of the actual shortest path if F E 35AT, and
1stretched if not.
Again, modulo some details, we can view their analysis as having three steps,
showing that:
1. If the slit width and plate thickness were zero, the construction guarantees
that for path length lower bound 1 = 2?? and another parameter4 ?min
2?nm:
1unstretched --H			1 and
1stretched > 1 + ?m2in
_			41
(Equation (9) in [CR87].)
2. The total number of plates is bounded from above by
Th?rn + tOn + 2m + 4.
Furthermore, each plate can add no more than ec? to the shortest path
length, so that if F c 3SAT, then
1 < 1unstretched < 1 + (7nm + iOn + 2n? + 4)?cR
(Equation (10).)
3. It follows that if
which is guaranteed if ?CR
& =
62
> (7nm + iOn + 2m + 4)?cR,
2nn?--H3n --H3
--H (7nrn+lOn+2rn+4)' ?ueu
the reduction works with
Thus, e?? can be specified in O(nrn) bits. It then follows from other arguments
that any dimension in the environment can be specified in O(nm) bits.
4?min is the minimum (virtual) source spacing described in [CR?87]
204
A.2 The Extension for Kinodynamic Lower
Bounds
A.2.1 The Idea
To extend the A(?-hardness result to kinodynamic planning, we can use the es-
sentially the same polynomial-time reduction to reduce an instance of 3SAT to
an optimal kinodynamic planning problem instance. Hence, we would use a point
amongst polyhedral obstacles. We will also choose the units for time to be the
same as the units for distance. For a given 3CNF formula F, the new reduction
constructs the a kinodynamic planning problem instance identical to the path-
planning problem instance in the [CR?87j reduction except that
1. the slit width and plate thickness ?`CR will be smaller than ?c?;
2. the start and goal positions will he lifted to the corresponding states with
velocity zero;
3. the velocity must obey a unit bound (in the same L?-norm used for distance
in the shortest-path problem);
4. the acceleration will obey some bound (L (in the same norm); and
5. for lower time bound 1 and time-accuracy ?T? the minimal-time kinodynamic
solution (obstacle-avoiding trajectory) would take more than time 1 t 6y if
and only if the given 3CNF formula F is not satisfiable.
The specification of ?-safety parameters co and ci will be omitted from the dis-
cussion; it is easy to choose them, because they can be incorporated linearly into
the choices of slit widths and plate separations.
A.2.2 Bit-Counting Again
Similarly to the above, the minimal-time solution will be stretched" if and only if
F ? 3SAT. Let Tstretched be the minimal solution time in this case, Tunstretched be
the least time otherwise. Suppose that we choose times T1 1 and 6T ?min via
unit conversion. It follows from the path-planning analysis that if the slit width
and plate thickness were zero, and if the acceleration bound were infinite, then
Tunstretched = 1, ?nd
205
Tstrctchcd > 1 + ????.
41
With slit width and plate thickness CcR but infinite acceleration, it follows that
1 < T_st?etched < 1 + (7nm + iOn + 2m + 4)?cR
Now, if the acceleration bound is a, Tunstretched can increase by at most a2 for
each obstacle edge along the shortest path, since this is twice the amount of time
it takes to go from zero velocity to full speed or vice-versa. Since there will be at
most two edges along the shortest path for each plate,
1 ? Tunstretd?ed ? 1 + (7nm + IOn + 2m + 4) (?cR +
By similar reasoning as above, if
9--H2nrn--H3n--H4
I			_____________________________________________________________________
?CR			(7nni +			iOn			+ 2m + 4)'			and
a --H 22nrn+3fl+6(Thini + iOn +			+ 4),
then the reduction will work with 6T = 62			tl?at ?min			2 ??.) Thus,
fl4)ifl. (Recall
?CR and a can be specified in 0(nm) bits.
Appendix B
Guide to Notation
tt was necessary to use a rather large number of synibols in this thesis; we have
tried to keep our notation consistent and "intuitive". The following is a partial
glossary of symbols.
C the configuration space of a particular robot
TC the tangent bundle of this space
d the number of dimensions of the configuration space
0 the encoding of the obstacle set
a safety function parameterized by co > 0 and ci ? 0; 6v(co, cl Co t cl lvii
p(t) a path; p : Time H C
(p, p) a trajectory state; p is position and p is velocity
x a configuration (also y)
v a velocity
X a state (also Y)
F a trajectory; F : Time H TC
S start state, usually the desired start state
G goal state, usually the desired goal state
206
207
a tracking tolerance
a acceleration
a acceleration bound
a vector of acceleration bounds
v velocity bound
v vector of velocity bounds
Amax global (non-dimensional) acceleration bound
f generalized force vector
a vector of generalized force bounds; for each i, Ifil ? Ji
M the encoding of a the dynamics equation obeyed by a robot
1 world diameter or length of greatest trauslational degree of freedom
? approximation parameter; 0 < ? < 1.
?s safety approximation parameter; 0 ? ? 1
?T time approximation parameter; c-T > 0
??` safety function for t-safety; 61v (1 --H ?)6v
F'r the trajectory F, but ?-tirne-rescaled
p' the path (or state) p, but ?-time-rescaled
SI ?-time-rescaled start state
G' ?-time-rescaled goal state
5* &dose approximation of the start state
G* ?-dose approximation of the goal state
Ax the set of all instantaneous accelerations possible at state X obeying dynamics
bounds
2OS
Ax the set of possible constant accelerations of duration r for trajectories begin-
ning at state x; these trajectories must obey the dynamics bounds
Ax* an easier-to-compute conservative approximation (subset) of Ax
A first, a set of acceleration functions; in Chapter 4 and afterwards, a function
A : TC H : Y is a set of acceThration functions1
as above, but corresponding to tighter generalized force bounds
?, Q with single arguments, arbitrary functions `P, Q TC H : Y is a set of
acceleration functions)
r a timestep size
?j a minimal acceleration advantage
a grid-spacing
a vector of grid-spacings; j?j is the spacing in dimension i
1 discretization parameter for generalized forces
?(X, r) the (i,r)-extremal shell at (corresponding to) state X; this is a set of
acceteration functions
?i(X, r) the set of true (Ti, r)-bangs at (corresponding to) state X; another set of
acceleration functions
?r0(X) =1*(X,ro)
Ttr0(X) =TL(X,To)
TO for a given robot and e, the fundamental timestep
(7ixo, 71vo) for a given robot and e, a ftindamentai tracking tolerance
N0 fundamental chunk density; we think of N0 bang-bang T?-bangs as approxi-
mating an average acceleration of duration N0T
Minkowski sum
Subscripts r, q, and u denote trajectories.
Subscript i usually denote the ith coordinate axial direction.
Subscript n usually denotes a timestep.
Appendix C
Computing Parameters for the
True-Extremal and
Near-Extremal Algorithms for
Open-Chain Manipulators
c.1 Introductjon
We wish to show for any given problem such that the force bounds always exceed
the forces necessary to hold the robot stationary and for a given ?, that the pa-
rameters Tiro, ?vO, r0 will be ?(?). We will use (p, p) to denote both trajectories
and arbitrary states.
We first show that M?'(p) --H M?'(p --H Ap)II are O(IIAp I) We can then
compute bounds for the perturbation magnitudes hr, hqo, and hqi, defined in
(4.47), in terms of m, Tix, and 7i?. Given M(p) and Uc(p) (or G(p)), it is possible
to bound their derivatives by inspection, because each of their components is a
sum of products of components of p and their sines and cosines.
Following any derivation of (4.1), say from [AS86], we write
oi(p) --H ?rnjgTJ?J?)(p),
where m5 is the mass of?the 5th link, g is the gravitational acceleration vector,
and Jt'??(p) is the ?th column of the linear velocity Jacobian for link j. More
209
210
importantly,
M(p) = ? (mjJ(;)(p)TJ(?i)(p) + JY(p)TIi(p)J(A?)(p)),
where m? is the mass of the jth link, I?(p) is its inertia tensor, and and
are its linear velocity and angular velocity Jacobians. ?ecalling equations (4.7)
and (4.1.1), we see that an encoding of the Jacobians j(;)(p) and ?A???(P) and the
constants m?, I?, and g fully encodes the dynamics of the manipulator.
link masses: fl?I, ??2
link inertias: Ji, `2
Figure C.1: A planar 2DOF arm, with constant physical parameters mi, ??2, li, /2,
li, `2, L1 and configuration parameters Oi and 02. c? indicates the centroid of joint
tin this simple example, the centroid of the first link lies on the line between the
joint axes. In a problem instance, the constants and equations (C.3) and (C.4) are
encoded in M. Together with equations (C.2), (C.1), (4.1.1), (4.7), they provide
a complete description of the dynamical laws that govern the manipulator.
Example C.1.1 Consider the planar manipulator with two revolute joints in fig
ure C.1. For each link i, let rn? be the mass of the link, ij be the distance from the
j?h joint axis to the link centroid, and I? be the inertia of the link about the axis
parallel to the joint axis through the link centroid. Let L1 be the distance from
the first joint axis to the second, and (to simplify the example) let the centroid
211
of the first link lie on the line between the joint axes. We compute linear velocity
Jacobians.
=			0
j7) =			;1L;Jin1l --H 12 sin(0i + 02) --H12 sin(0i + 02)			C.3
+l?cos(Oi +02)			l?cos(0i +02)
We note that here the angular velocity Jacobians are simple because all revolute
axes are parallel.
=			(1			0)
(9)
=			(1			1)
Clearly, Oi and 02 will only appear in sines and cosines in the dynamics equation.
Furthermore, we observe that an encoding M of the m?, i?, I?, L1, equations (C.3)
and (C.4), and the gravitational acceleration vector g = (0 g)T is thus a sufficient
encoding of the manipulator dynamics.
lleinzinger and Paden [HPS9j exploit the relationships between M(p) and the
robot kinematic map and Jacobian to bound the general derivatives of M(p) and
G(p). We will use their results to show one way to derive the desired bounds.
C.2 Bounding Changes in M-'(p)
Observe that IIM(p)--HM(p+Ap)112 is polynornially dependent on the components
of Ap. Let M' = M(p + Ap), and AM = M --H M' We wish to bound IAM?' I =
jIM--H' --H I. If M and M' are inertia matrices, then they are symmetric and
positive definite. Let Amin be the minimum of their minimum eigenvalues.
Now consider the solutions x and y to the systems
Mx = b;			C5
M'y =
By substitution,
M(y - x) = (M - M')y + M'y - Mx = AMy,
212
and thus IIM(x --H ?)II = II?MyII. We now d?ose to use the L2-norm. Now,
IlAMyll2 < IlAMli2llyll2 < IIAMII2?Iibl?:
Since
it follows that
AminjiX --H Y112 < M(x --H y)112 = IIAMyII2,
IIx--Hy112 ? IIAMA9rnI2fl1Ib1I2
Recall that
AM?'b =			--H			= x --H y.
Since ?N1 is arbitrary except for the condition that NI' be non-singular, and
b is arbitrary, (C.6) and (C.7) imply that
AM?'I 2 < AMIl2
--H A9??in
Therefore, since IlAMIl2 is 0 (IlApil), so is AM--H' 2
C.3 Acceleration Bounds and Perturbations
We first review a notation for tensor-valued functions, as used by [HP89j. M is
a smooth tensor field, and M(p) is a tensor of rank 2. For x ? C, M(x)(v1,v2)
denotes the tensor acting on (vl,v2) E TxC X TxC. We have been representing
M(p) as a matrix. For example, the kinetic energy of the system in state (p, p)
can be expressed as 21pTM(p)p or IM(p)(p, p).
The ?th derivative of M(p) with respect to p is defined as follows:
= d d ??M(p)ij?i?j?1k1?kfl
j,Zj=1 k1,,????=1 0Pk1 ...?p?n
Hence, for a given state (p, p) and acceleration a, (4.1) corresponds to
= M(p)(a,.) + DM(p)(p,.)(p) --H --HDM(p,p)(') + 01(p)(.),
2
(C.10)
213
with fi = f(e?), where e? is the usual jth basis vector in ?d The second and third
terms on the right side are equivalent to the [pTc(p)p] term in (4.1)
We will use four results ((C.11) through (C.14)) from [HP89]. Let Mi denote
the mass of link i, Lj its maximum length from the near joint axis, and Li the
greatest distance of its centroid from the first joint axis of the manipulator. Define
Ilyll; = z;=1 lyit. Then
IM(p)(wi,w2)I <
ID?M(p)(wi,w2)(w3,...,wn+2)I ?
d
? MiL? liwi It w?ll;;
i=1
(C.tl)
d
2d ? MiLt2itWi lI;???IlWn+2llt (C.12)
i=1
IG(p)(wi)l			<			(C.13)
lD?G(p)(wi,...,wn+i)l			?
Now, we bound the acceleration. Let us define
d
ZgMiTiI will;;
i=1
? gliliLi liwi ll;?? llwn+i ;.
i=i
?(f, p, p) = f --H [pTc(p)pj --H
Then we can rewrite (4.7) as
Recalling (4.7),
a(p,p,f) =
(C.14)
(C.15)
(C.t6)
lla(p, p, f)ll			?			I ?--H? (?)ll ll?(f p, p)ll
--H?			A f --H [pTc(p)p] --H G(p)			(C.17)
<			AA5jn fllfll + II [pTc(p)p] II +			G(p)l ?
Using (C.12) and recalling that llxll.? ? llxlli ? Yd xl 2 for any &vector x, we
obtain
214
2dd3/2IIApIIZdi=iMjL?
(AmMin )2
II[pTc(p)p] < in??IIxI??<?1 ? jDM(p)(p, x)(p) + I-21DM(p)(p, p)(x)I?
< 2d?13Zdi=i ?MiL97IIvmaxII??
< 2d--H13d312 Vmaz 12 zd__ MiL97.
lAM--H1Il2 ?			.			(C.23)
Using (C.13)
IIG(p)II < maxIx?2<1 ?;=1 gMiTiIIxII?
? gYdZ?iMFLi.
Thus we have the following global bound for the acceleration:
d			d
IIfmaxIi + 3d312IIvrnaI12 ? MjL? + ??Z MiT
i=1			i=1
(C.t8)
(C.19)
Amax <			(C.20)
A7A?1j?
We now bound hr(ro), hqo(To), and hq?(?7r,7Iv). Recalling the definitions in
(4.47), we obtain
To max ???1			o+M?1?aF			?? a
`P'PPa?P			a?
To max a??1			--HI			aF			a?
,p,ppapP?a
max a? Ap ?(f,p,p)+M?1(p)[aa?Ap+a4Api
hr(TO) <
hqo(To) <			(C.21)
hql(?jx,lJv) <
Recall (C.8), whjch we use to bound M?'(p) --H M?'(p + Ap)ll, and observe
that
IIM(p) --H M(p + Ap)II ? maxiIII=Ii?iI=l IDM(p)(x, y)(Ap)I
< 2(td312IIApII Zdi?i
By substitution, we obtain
(C.22)
It follows that
215
aM-'
TO
ap
aM-1AP
ap
2dd3/2IipT()IIzffi--H1AIiL?
(AmMin )2
2dd312Apliz?--H1M?L:2
(AmMin )2
Applying (C.12) and (C.18), we obtain
and			(C.24)
a[P%C7P)P]?p < maxIIxII?<i ID?Mi??P2)(?P?$)?$$Ap?P?x)I,?p?i
? 2d--H1 3d2 Ipli2llApil Zdi=i AIiL?;
3?%(P)PiAp <
IDM(p)(p, x)(Ap)
+ IDM(p)(Ap,x)(?)I
+ IDM(p)(Ap,p)(x)I
< 2d--H1 :3d3/2IIpIIIIApII ?2?=1 i?LL?';			and
(C.25)
(C.26)
(C.27)
aG(P)A
a?			P < g7dZffi?, ALTi.			(C.28)
We now give the final bonnds for hr(TO), hqo(TO), and hqi(?xo, Tivo) First, we
define
H			=			zffiiALL'??;
H			=			Ztd?i gWI?L?;
F = I fI + 2d--H1 3d31211Vmar11211 + v,?ij??H;
= IIf'II + 2d--H1 3d3K)IIvmlaxII2H + `/=d9H
in this notation, we can rewrite (c;.2o) as
F
Amax ? ATh1in
(C.29)
(C.3O)
Finally,
216
hr(r0) ? (2dd3/2IV'maxllHF'?2d--H13d2IIvlmaxII3)ro
AmMin
2d--H13d312IVm'axIIF'To.
(AmMin)2
hqo(To) < (2dd3/2IIVmazTIIHF+2d--H13d2IIVmaxII3)TO
AmMin
2d--H1 3d312IIVmaxT Fr0
hql(7iro,llvo) <
t
(AmMin )2
2d($11$2F + 2d--H13d2IIvmaz?jH7dgH
AmMin
2d--H1 3d312IIVmaxIIH
+
ArA;I?in
(C.31)
(C.32)
We summarize these bounds by saying that llr(TO) and hqo(T?) are linear in To,
and that hq1(?ro, ?vo) is linear in ?r.0 and 71v0' In other words we can write, with
obvious substitutions for Cr, Cqo, Cqi, and Cq'i:
llr(TO) ? CTrTo;
hqo(To) ? CqoTo;
hql(7/z,?/v) ? CqlT/x t Cq'illvo
(C.53)
Bibliography
[AF66]
[A586]
[BBD+90]
[BD89]
[BDG85]
[BHJ+82]
M. Athans and P. L. Faulb. Optimal Control: An Introduction to the
Theory and Its Application. McGraw-Hill, New York, 1966.
II. Asada and J. J. Slotine. Robol Analysis and Control. John Wiley
& Sons, New York, 1986.
J. Baillieul, R. Brockett, B. Donald, et al. Robotics: Symposia in Ap-
plied Afathematics, volume 41. American Mathematical Society Press,
Providence, Rhode Island, 1990.
M. Boddy and T. Dean. Solving time-dependent planning problems.
In Proceedings JJCA I-89, pages 979--H984,1989.
J. Bobrow, 5. Duhowsky, and J. Gibson. Time-optimal control of robot
manipulators along specified paths. biteniational Jounial of Robotics
Research, 4(3), 1985.
M. Brady, J. Hollerbach, T. Johnson, T. Lozano-P?rez, and M. Mason,
editors. Robot Afotion: Planning and Control. MIT Press, Cambridge,
Massachusetts, 1982.
[Bob82] J. E. Bobrow. Optimal Control of Robotic Alanipulators. Ph.D. disser-
tation, U.C.L.A., Los Angeles, California, 1982.
[Bob88]
J. E. Bobrow. Optimal robot path planning using the minimum-time
criterion. IFEE Jounial of Robotics and A?tomaiion, 4(4):443--H450,
August 1988.
[Bra89] M. Brady, editor. Robotics Science. MIT Press, Cambridge, Mas-
sachusetts, 1989.
M. L. Brown. Optimal robot path planning via state space networks.
Masters dissertation, Department of Mechanical and Aerospace Engi-
neering, Princeton University, June 1984.
[Bro84]
217
218
[Can86]
[Can88a]
J. Canny. Collision detection for moving polyhedra. IFEE Trans-
actions on Pattern Analysis and Machine Intelligence, 8(2):200--H209,
1986.
J. Canny. The Complexity of Robot Motion Planning. MIT Press,
Cambridge, Massachusetts, 1988. Book version of Canny's 1986 Ph.D.
thesis.
[Can88b] J. Canny. Exact solution of some robot motion planning problems. In
International Symposium on Robotics Research, pages 181--H189, 1988.
[CD88] J. Canny and B. Donald. Simplified Voronoi diagrams. Discrete and
Computational Geometry, 3(3):219--H236, 1988.
[CDRX88] J. Canny, B. Donald, J. Reif, and P. Xavier. On the complexity of
kinodynamic planning. In P?oceedings of the 29th Annual Symposium
on the Foundations of Computer Science, pages 30(3--H316, White Plains,
New York, 1988.
[CKS81] A. C. Chandra, D. C I?ozen, and L. 3. Stockmeyer. Alternation. Jour-
nal of the Association of Computing Machinery, 28(1):114?133, Jan
uary 1981.
[Cla87] K. L. Clarkson. Approximation algorithms for shortest path motion
planning. In Proceedings of the 1?h Symposium on the Theory of Com-
puting, pages 56--H65, New York, 1987.
[CR87]
3. Canny and 3 Reif. New lower hound techniques for robot motion
planning. In Proceedings of the 28th Annual Symposium on the Foun-
dations of Computer Sciende, Los Angeles, California, 1987.
[CRR9O] 3. Canny, A. Rege, and 3. Reif. An exact algorithm for kinodyna
mic planning in the plane. In Proceedings of the Sixth Annual Sympo-
sium on Compulalional Geometry, pages 271--H280, Berkeley, California,
1990.
[DB88]
T. Dean and M. Boddy. An analysis of time-dependent planning. In
Proceedings AAAI-88, pages 49--H54,1988.
5. Dubowsky, M. A. Norris, and Z. Shiller. Time optimal trajectory
planning for robotic manipulators with obstacle avoidance: A CAD
approach. In Proceedings 1986 IEEE International Confrrence on
Robotics and Automation, pages 1906--H1912, Sari Fracisco, California,
1986.
[DNS86]
219
[Don87] B. Donald. A search algorithm for motion planning with six degrees of
freedom. Artificial Intelligence, 31(3) :295--H353,1987.
[DX89aJ B. Donald and P. Xavier. Near-optimal kinodynamic planning for
robots with coupled dynamics bounds. In A. C. Sanderson, A. A.
Derochers, and N. Valvanis, editors, Proceedings of the Fourth IFEE
International Symposium on Intelligent Control, pages 354--H359, Al-
bany, New York, September 1989. Invited paper.
[DX89b]
[DX9Oa]
[DX9Ob]			B.
for
[DX91]
[FL92]
B. Donald and P. Xavier. A provably good approximation algorithm
for optimal-time trajectory planning. In Proceedings of the 1989 IEEE
International Conference on Robotics and Automation, pages 958--H963,
Scottsdale, Arizona, 1989.
B. Donald and P. Xavier. Provably good approximation algorithms
for optimal kinodynamic planning for cartesian robots and open chain
manipulators. In Proceedings of the Sixth Annual Symposium on Corn-
putational Geometnj, pages 290--H300, Berkeley, California, June 1990.
Donald and P. Xavier. Provably good approximation algorithms
optimal kinodynarnic planning for cartesian robots and open chain
manipulators. Department of Computer Science Technical Report TR-
1095, Cornell University Department of Computer Science, Ithaca, New
York, February 1990. Supercedes TR-971.
B. Donald and P. Xavier. Time-safety trade-offs and a bang-bang
algorithm for kinodynamic planning. In Proceedings of the 1991 JEFF
Lnternational Conference on Robotics and Automation, Sacramento,
California, 1991.
T. Fraichard and C. Laugier. IKinodynamic planning with moving
obstacles: the case of a structured workspace. In Proceedings of the
IEEE/RSJ International H7orkshop on bitelligent Robots and Systems,
Raleigh, NC (USA), July 1992.
[FW88] 5. Fortune and G. ?Vilf6ng. Planning constrained motion. In Twentieth
Annual ACA?I Symposium on Theory of Computing, Chicago, 1988.
[GJ85] E. C. Gilbert and D. ?V. Johnson. Minimum-tin? robot path plan-
ning in the presence of obstacles. In Proceedings 24th Conference on
Decision and Control, pages 21--H30, Fort Lauderdale, Florida, 1985.
[GM87] 5. K. Ghosh and D. M. Mount. An output sensitive algorithm for com-
puting visibility graphs. In Proceedings of the 27th Annual Symposium
on the Foundations of Computer Science, pages 11--H19, 1987.
220
[llBll88]
[HJCP90]
[Hol83J
[HP89]
[JC89]
[JllCP89]
[Kah69]
[Kir7O]
[KR71]
[Lat91]
E. J. Horvitz, J. 5. Breese, and M. Henron. Decision theory in expert
systems and artificial intelligence. Journal of Approximate Reasoning,
1988.
G. lleinzinger, P. Jacobs, J. Canny, and B. Paden. Time-optimal tra-
jectories for a robot manipulator: A provably good approximation al-
gorithm. In Proceedings ot the 1990 IEEE Jnternational Conference on
Robotics and Automation, pages 150--H155, Cincinnati, Ohio, May 1990.
J. M. Hollerbach. Dynamic scaling of manipulator trajectories. MIT
A.I. Memo 700, Massachusetts Institute of Technology, Cambridge,
Massachusetts, 1983.
G. Heinzinger and B. Paden. Bounds on robot dynamics. In Proceed-
ings 1989 JEEE Jnternational Conference on Robotics and Automation,
pages 1227--H1232, Scottsdale, Arizona, 1989.
P. Jacobs and J. Canny. Planning smooth paths for mobile robots.
In Proceedings ot the 1989 JEFE h?ternational Conference on Robotics
and Automation, pages 2--H7, Scottsdale, Arizona, 1989.
P. Jacobs, C. Heinzinger, J. Canny, and B. Paden. Planning guaran-
teed near-time-optimal planning in a cluttered workspace. Technical
Report ESRC 89-20/RAMP 89-15, Engineering Systems Research Cen-
ter, University of California, Berkeley, California, October 1989.
M. E. Kahn. The Near-Ajinimal-Time Control of Open-Loop Articu-
lated A&nematic Chains. Ph. D. dissertation, Stanford University, Stan-
ford, California, December 1969. Also Stanford Artificial Intelligence
Memo No. AIL-106.
D. E. Kirk. Optimal Control Theory. Prentiss-Hall, Englewood Cliffs,
New Jersey, 1970.
M. E. Kahn and B. Roth. The near-minimum-time control of open-loop
articulated kinematic chains. Journal of Dynamic Sys/ems, Aieasure-
ment, and Control, 93:164--H172, 1971.
J-C. Latombe. Robot Alotion Planning. Kluwer Academic Publishers,
1991.
Z. X. Li and J. Canny. Robot motion planning with non-holonomic con-
straints. UCB ERL Technical Report Memo No. UCB/ERL M89/13,
University of California, Berkeley, California, 19S9.
[LC89]
221
[Lei66]
[LL81]
[LLZ90]
[LP83]
[LPW79]
[LR?DG90]
[LW77]
Leitman. An Introduction to Optimal Control. McGraw-Hill, Inc., New
York, 1966.
J. Y. 5. Luh and C. 5. Lin. Optimum path planning for mechanical ma-
nipulators. Journal of Dynamic Systems, Aleasurement, and Control,
102:142--H151,1981.
R. Lindberg, R. Longman, and M. Zedd. Ninematic and dynamic
properties of an elbow manipulator mounted on a satellite. The Journal
of the Astronautical Sciences, 38(4) :397--H421,1990.
T. Lozano-Pe'rez. Spatial planning: A configuration space approach.
IEEE Transactions on Computers, C--H32(2):108--H120, 1983. Also MIT
A.I. Memo 605, December 1982.
T. Lozano-Pe$rez and M. Wesley. An algorithm for planning collision-
free paths among polyhedral obstacles. Communications of the ACM,
22(10):560--H570, October 1979.
J. Lengyel, NI. Reichert, B. Donald, and D. Greenberg. Real-time
robot motion planning using rasterizing computer graphics hardware.
In Proceedings, SICCPA Ff1 `90, Dallas, Texas, Aug 1990.
J. Y. 5. Luh and NI. W. Walker. Minimum-time along the path for
a mechanical arm. In Proceedings, IFEE Confercnce on Decision and
Control, pages 755--H759, New Orleans, Louisiana, 1977.
[LWP80] J. Y. 5. Luh, M. W. Walker, and R. P. C. Paul. Online computational
scheme for mechanical manipulators. Joun?al of Dynamic Systems,
Measurement, and Control, 102:69--H76, 1980.
[NA84]
[087]
M. Niv and D. M Auslander. Optimal control of a robot with obstacles.
In Proceedings of the 1984 American Control Conference, pages 280--H
287, San Diego, California, 1984.
C. 6,Dunlaing. Motion planning with inertial constraints. Algorith-
mtca, 2(4):43i--H475, 1987.
[Pap85] C. R. Papadimitrion. An algorithm for shortest path motion in three
dimensions. bifo???ation Processing Letters, 20:259--H263, 1985.
[PBGM62] L. Pontryagin, V. Boltyanskii, R. Gaml:relidze, and E. Mishchenko.
The Mathematical Theory of Optimal Processes. Interscience Publish-
ers/John Wiley & Sons, Inc., New York, 1962.
[PFTV88] W. Press, B. Flannery, 5. Teukoisky, and W. Vetterling. Numencal
Recipes in C. Cambridge University Press, New York, 1988.
222
[PS82]
[PT86]
C. H. Papadimitriou and k. Steiglitz. Combinatorial Optimization: Al-
gorithms and Complexity. Prentice-Hall, Englewood Cliffs, New York,
1982.
C. II. Papadimitriou and J. Tsitsiklis. Intractable problems in con-
trol theory. SlAM Journal of Control and Optimization, 4(4):639--H654,
1986.
[Raj85] V. T. Rajan. Minimum time trajectory planning. In Proceedings of
the 1985 IEEE International Conference on Robotics and Automation,
pages 759--H764, St. Louis, Missouri, 1985.
[Rei79]
[Ites92]
[RS85]
[RT9O]
[RT91]
[Sch87]
[SD85]
J. Reif. Complexity of the mover's problem and generalizations. In
Proceedings 2?h IFEE Symposium on the Foundations of Computer
Science, pages 421--H427, 1979.
E. Ressler. Planning 2D solid robot motion on a discrete grid. In
B. Donald and D. Huttenlocher, editors, Course Notes for CS 661/462
"Robotics and Machine Vision". Cornell University Department of
Computer Science, Ithaca, New York, 1992.
J. Reif and J. Storer. Shortest paths in enclidean space with polyhedral
obstacles. Technical Report CS-85-121, Brandeis University Computer
Science Department, April 1985.
J. Reif and 5. Tate. Approximate kinodynamic planning using l?-norm
dynamics bounds. Ted?nical Report CS-1990-13, Duke University De-
partment of Computer Science, Durham, North Carolina, 1990.
J. Reif and 5. Tate. Continuous alternation. Technical report, Duke
University Department of Computer Science, Durham, North Carolina,
1991.
II. M. Schaettler. On the optimality of bang-bang trajectories in ??.
Bulletin of the American Mathematical Society, 16(1):113--H116, 1987.
Z. Shiller and 5. Dubowsky. On the optimal control of robotic manip-
ulators with actuator and end-effector constraints. In Proceedings of
the 1985 JEEF International Conference on Robotics and Automation,
pages 614--H620, St. Louis, Missouri, 1985.
Z. Shiller and 5. Dubowsky. Global time-optimal motions of robotic
manipulators in the presence of obstacles. In Proceedings of the 1988
IEEE Inteniational Conference on Robotics and Automation, pages
370--H375, Philadelphia, Pennsylvania, 1988.
[SD88]
223
[SH85]
[SM85a]
[SM85b]
[5R84]
[5582]
[5583]
[5584]
[5585]
[5586]
[5Y89J
G. Sahar and J. Hollerbach. Planning of minimum-time trajectories for
robot arms. In Pmceedings of the 1985 lEFE International Conference
on Robotics and Automation, pages 751--H758, St. Louis, Missouri, 1985.
K. G. Shin and N. D. McRay. Minimum-time control of robotic ma-
nipulators with geometric path constraints. JEBE Transactions of Au-
tomatic Control, 30(6):531--H541, June 1985.
K. G. Shin and N. D. McNay. Selection of near-minimum geomet-
ric paths for robotic manipulators. JEFE Transactions of Automatic
Control, 31(6):531--H541, June 1985.
V. Scheinman and B. Roth. On the optimal selection and placement
of manipulators. In Proceedings 5th CiSAl-IFToAJAl Symposium on
Theory and Practice of Robots and ?anip ukitors, pages 25--H32, Udine,
Italy, 1984.
J. T. Schwartz and M. Sharir. On the piano mover's problem: Ii. gen-
eral techniques for computing topological properties of real algebraic
manifolds. Technical Report 4t, Computer Science Department, New
York University, 1982.
J. T. Schwartz and M. Sharir. On the piano mover's problem: I. the
case of a rigid polygonal body moving amongst polygonal barriers.
Communications in Pure and Api?lied Ajathematics, 36, 1983.
M. Sharir and
In Proceedings
144--H153, 1984.
A. Schorr. On shortest pathes in polyhedral spaces.
16th ACAl Symposium on Theory of Computing, pages
E. Sontag and H. Sussmai?n. Remarks on the time-optimal control
of two-link manipulators. In Proceedings of the 24ih Conference on
Decision and Conti?l, Et. Lauderdale, Florida, 1985.
E. Sontag and 11. Snssmann. Time-optimal control of manipulators.
Technical report, Department of Mathematics Rutgers University,
New Brunswick, New Jersey, 1986.
J. J. Slotine and H. 5. Yang. Improving the efficiency of time-optimal
path-following algorithms. IEEE Transactions on Robotics and Au-
tomation, 5(1)118--H124, February 1989.
[Wil88] G. Wilfong. Motion planning for an autonomous vehicle. In Pro-
ceedings of the 1988 lEFE hiternational Conference on Robotics and
Automation, pages 529--H533, Philadelphia, Pennsylvania, 1988.
224
C. Yap. Algorithmic motion planning. In J. Schwartz and C. Yap, ed-
itors, Advances in Robotics: Volume 1. Lawrence Eribaum Associates,
1986.
[Yap86]
