MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

motion.cs.illinois.edu

MotionPlanningHigherDimensions
81–103 minutes

Section III. MOTION PLANNING¶

Chapter 10. Motion Planning in Higher Dimensions¶

The algorithms we have studied for the 2D case do not apply to

many other systems of interest: quadcopters that avoid obstacles
in 3D, articulated robot arms, multi-robot teams, and robots that
manipulate objects. The main issue that we are faced with is that
previous geometric planning methods require the ability to explicitly
perform computations about the shape of obstacles. In higher-
dimensional configuration spaces, this is rather challenging. This
chapter will consider how to address problems with configuration
spaces of arbitrary dimension. (In fact, these algorithms can also
be applied to problems of lower dimension too --- in low-
dimensional spaces, planning is fairly easy!)

Geometric planning methods like visibility graphs and cell

decomposition can often be extended to 3D environments with
polyhedral obstacles with some work. There are also algorithms for
path planning with constraints expressed as semi-algebraic sets
that work in spaces of arbitrarily high dimension, but their running
time is exponential in dimensionality, and are more of an

1 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

intellectual curiosity since they have never been practically

implemented. Grid search planners can also be extended to higher
dimensional grids, but they must in the worst case explore an
exponential number of grid vertices.

In fact, it has been proven that even feasible path planning is NP-
hard in the case of articulated 𝑛-joint robots. Surprisingly, optimal
path planning in the presence of 3D polygonal obstacles is also
NP-hard in the number of obstacle vertices! This dramatic increase
in the complexity of exact algorithms in dimensions 3 and higher
has led to the development of several approximate methods, which
are discussed in detail in this chapter.

1 Implicit C-obstacle representation¶

C-obstacles for general articulated robots are in general, even

more complex than they were in 2D and 3D. As a result, most
motion planning algorithms outside of planar environments do not
attempt to build an explicit representation of C-obstacles, but
instead opt to perform Boolean feasibility queries in which the
collision status of a robot is queried for a given configuration: in
other words, we can test whether 𝑞 ∈ 𝒞 𝑂 for a specific
configuration 𝑞, but we do not have a representation of any points
on ∂𝒞 𝑂. Specifically, the user of a planner defines a subroutine as
follows:

⎧𝑇 if 𝑞 is in the free space

𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑞) = ⎨
⎩𝐹 if 𝑞 is in the forbidden region

A planner can then call this subroutine to probe whether a

configuration is feasible. Since this will be called thousands or
millions of times, fast planning in high-dimensional spaces requires

2 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

efficient collision tests as described in Chapter 7.

Often, we will also need to check whether a motion is feasible as

well, usually a short segment of a path ¯𝑝𝑞
¯¯¯¯¯¯¯¯ between configurations

𝑝, 𝑞 ∈ 𝒞 . The process is called a visibility query in the case of a

straight line path, and can be a user-defined subroutine or
performed by the planner. The query is specified as follows:

⎧𝑇 if ¯𝑝𝑞
¯¯¯¯¯¯¯¯ is completely inside the free space
𝑉𝑖𝑠𝑖𝑏𝑙𝑒(𝑝, 𝑞) = ⎨
⎩𝐹 if ¯𝑝𝑞
¯¯¯¯¯¯¯¯ intersects the forbidden region

In general the process of checking motions for collision is known

as dynamic collision checking. The simplest method for doing so is
simply to take small steps along the path and perform feasibility
queries at each configuration. More details about this and other
techniques are described in the section below.

In addition to the Boolean feasibility query computational model,

we also consider some planners that exploit knowledge encoded in
an implicit function model 𝒞 𝑂 = {𝑞 | 𝑓 (𝑞) ≤ 0}. For example,
one such implicit function 𝑓 may be the signed distance in
workspace between the robot and obstacles. (Specifically, this
would return the distance when there is no collision, and the
negative of the penetration depth when collision exists.) For most
complex geometric models it is far more computationally expensive
to perform distance and penetration depth computations than
collision queries. As a result there is a trade off between using a
computational query that provides richer information vs the added
complexity of invoking the query.

2 Grid Search and the Curse of Dimensionality¶

3 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Let us begin by considering the case of extending grid search to 𝑛

-D space. It is fairly straightforward to build such a grid, and
collision checking for arbitrary 𝑛-D robots at configurations or
paths can be performed relatively quickly (we shall describe
methods for doing so below). However, the number of vertices that
may need to be explored grows exponentially with the dimension of
the space. This growth rapidly overwhelms the available
computational resources, both in time and memory.

It is helpful to get a sense of the absolute scale of exponential

increase to appreciate how difficult this makes the problem.
6
Consider creating a grid in a 6-D unit hypercube [0, 1] with
resolution ℎ on each axis. The number of vertices in the grid is
listed in the right of the below table. Clearly, at high resolutions it
would be impractical to search the entire grid.

Resolution ℎ # vertices

0.5 64

0.25 46,656

0.1 1,000,000

0.05 64,000,000

0.025 46,656,000,000

0.01 1,000,000,000,000

Let us also fix a relatively manageable resolution, say 0.1, and

observe what happens as dimension varies. The following table
shows how many vertices are in a grid of variable dimension [0, 1
𝑑
] .

4 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Dimension 𝑑 # vertices

2 100

3 1,000

6 1,000,000

8 100,000,000

10 10,000,000,000

15 1,000,000,000,000,000

20 100,000,000,000,000,000,000

Yikes! Even if feasibility checking and visibility checking were

super-fast, this becomes impractical for use in dimensions of
around 8. This problem is generally known as the curse of
dimensionality.

Besides the combinatorial explosion in the number of grid cells

needed to span a space, there are several other odd effects in high
dimensional spaces that are counterintuitive to our experience in
2D and 3D spaces. Examples include the fact that the volume of a
hypersphere drops dramatically as dimension increases. In fact the
volume of a unit hypersphere approaches 0 as 𝑑 → ∞! This implies
that almost all points are far in a high dimensional space, for most
reasonable definitions of "far". Another effect is that the complexity
of a polytope grows dramatically. Consider a polytope in 𝑑-D space
with 𝑛 faces, such as that defined by a linear equality 𝐴𝑥 ≤ 𝑏,
with 𝐴 an 𝑛 × 𝑑 matrix and 𝑏 an 𝑛-vector. The number of vertices
of the polytope is 𝑂(( 𝑛𝑑 )), which grows exponentially in 𝑛 and 𝑑. As
a result, the complexity of the free space can be exponential even
for simple constraint representations.

5 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Since this "curse" appears so often in computational problems, it is

of great interest (and often surprising) to find algorithms that
circumvent these limitations. However, they tend to trade-off
computational complexity for other limitations. One example is
Monte-Carlo integration, in which a sum of function values of
randomly sampled points are used to estimate the integral of a
function:
𝑏
𝑏 −𝑎 𝑁
�𝑎 𝑓 (𝑥)𝑑𝑥 ≈ 𝑁 � 𝑓 (𝑥𝑖 )
𝑖=1

where the points 𝑥1 , …, 𝑥𝑁 are sampled uniformly at random from

the range [𝑎, 𝑏]. The approximation error of this estimate,
assuming 𝑓 is well-behaved, is on the order of 𝑂((𝑏 − 𝑎) / √𝑁).

Monte-Carlo integration can be generalized to higher dimensional

functions. If 𝐵 is an axis-aligned, 𝑛-dimensional box [𝑎1 , 𝑏1 ] × ⋯ × [
𝑎𝑛 , 𝑏𝑛 ], then the integral over 𝐵 can be approximated

|𝐵| 𝑁
�𝐵 𝑓 (𝑥)𝑑𝑥 ≈ � 𝑓 (𝑥𝑖 )
𝑁 𝑖=1
𝑛
with |𝐵 | = ∏𝑖 = 1 (𝑏𝑖 − 𝑎𝑖 ) the volume of 𝐵 and 𝑥1 , …, 𝑥𝑛 is a
uniform sampling over 𝐵. Specifically, a uniform sampling random
variables 𝜖𝑖 ∈ [0, 1] uniformly at random, and sets
𝑥𝑖 = 𝑎𝑖 + 𝜖𝑖 (𝑏𝑖 − 𝑎𝑖 ) .

The approximation error of (4) is very similar to the 1D case: 𝑂( |

𝐵 | / √𝑁 ). Observe that the dimensionality 𝑛 did not appear at all
in this equation!

The sampling-based planning methods introduced in this chapter

are somewhat inspired by Monte-Carlo integration. In
particular,their performance is not immediately affected by C-space

6 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

dimensionality. This is extremely appealing!

However, like Monte-Carlo methods, these wins come at a price. In

Monte-Carlo sampling, there is a hidden constant in the
approximation error that depends on the variance of 𝑓 across the
domain. Likewise, sampling-based planning induces a probabilistic
chance of failure, and the risk of failure is highly dependent on the
visibility properties of the free space. We will investigate these
concepts more formally below.

3 Sampling-based motion planning¶

The most popular set of techniques for motion planning on robots

with 5 or more DOFs is the class of sampling-based motion
planners, most notably the probabilistic roadmap (PRMs) and
rapidly-exploring random tree (RRTs) planners. All such techniques
are roadmap methods that build a network of paths in C-space, but
they use different strategies for doing so.

There are three general reasons for their popularity:

1. The same algorithm can be generalized to new problems of

arbitrary dimensionality simply with changes of the 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑞)
and 𝑉𝑖𝑠𝑖𝑏𝑙𝑒(𝑝, 𝑞) subroutines.

2. They often produce paths quickly for high-dimensional problems

that are not too-maze like, and given enough time can eventually
solve problems of arbitrary complexity (probabilistic
completeness).

3. They can be implemented fairly quickly by a student competent in

algorithms and graph data structures.

By "high-dimensional" we mean that sampling-based planners can

7 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

routinely solve problems in spaces of approximately 10D, and with

tuning (or luck) can also find feasible paths in dozens or even
hundreds of dimensions. However, these planners are not a "magic
bullet", and a deeper analysis of their performance characteristics
— both algorithmically and as they relate to the visibility properties
of the underlying planning problem — is required to understand
when they can be used effectively.

3.1 Probabilistic roadmaps¶

Probabilistic roadmaps (PRM) are an approximate roadmap of the

robot's free space built by randomly sampling free connections and
attempting connections between them. The roadmap can then be
searched as usual for a path from the start to the goal. The basic
algorithm for constructing a PRM is as follows:

1. Sample 𝑁 configurations at random from the C-space (Figure 1.a--

b).

2. Add all feasible configurations and the start and goal to the
roadmap. These are known as milestones. (Figure 1.c)

3. Test pairs of nearby milestones for visibility, and add visible edges
to the roadmap. (Figure 1.d--e)

4. Search for a path from the start to the goal. (Figure 1.f)

8 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Figure 1. Illustrating the main steps of the PRM algorithm.

It can be shown that the method is probabilistically complete, in

that if it finds a path, the path will be feasible. If it does not find a
path, then this answer might be incorrect. However, the chance of
this incorrect answer decreases to 0 as 𝑁 increases, given some
relatively mild assumptions on the shape of the free space.
Another useful property is that the likelihood of success is not
directly dependent on dimensionality, but rather then visibility
properties of the free space. As a result, it can reliably solve high-
dimensional problems that have good visibility properties, and
perform poorly in low-dimensional problems that have poor
visibility properties (such as narrow passages).

3.1.1 Algorithm and key parameters¶

The basic PRM algorithm is given in Algorithm Basic-PRM. The

first for loop adds any sampled feasible configurations to the
roadmap (𝑉, 𝐸). The second for loop checks for pairs of "nearby"
milestones, and adds edges as long as the path between them is
collision-free.

Algorithm Basic-PRM(s,g,N)

1. 𝑉 ← {𝑠, 𝑔}.

2. 𝐸 ← {}.

3. for 𝑖 = 1, …, 𝑁 do

4. 𝑞 ← 𝑆𝑎𝑚𝑝𝑙𝑒()

9 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

5. if not Feasible(𝑞) then return to Line 3.

6. Add 𝑞 to 𝑉 (add 𝑞 as a new milestone)

7. for all 𝑝 ∈ 𝑛𝑒𝑎𝑟(𝑞, 𝑉)

8. if Visible(𝑝, 𝑞) then

9. Add (𝑝, 𝑞) to 𝐸.

10. Search 𝐺 = (𝑉, 𝐸), with Cartesian distance as the edge cost, to
connect 𝑠 and 𝑔.

11. return the path if one is found.

There are two key subroutines to tune, which greatly affect running
time and performance:

• The sampling distribution over 𝒞 (the 𝑆𝑎𝑚𝑝𝑙𝑒() subroutine in Line

4).

• The method for determining nearby milestones in Line 7.

Since at most 𝑁 milestones will be added to the roadmap, it is

important to place as many as possible inside ℱ at critical
locations that aid the planner in connecting the start and goal. The
𝑆𝑎𝑚𝑝𝑙𝑒() subroutine can be tuned to do so. First, in order to have
a nonnegligible chance of sampling a milestone in a useful
location, PRMs require that the C-space is bounded. In the most
basic uniform distribution it is assumed that 𝒞 is a box [𝑎1 , 𝑏1 ] × ⋯
× [𝑎𝑛 , 𝑏𝑛 ], and 𝑆𝑎𝑚𝑝𝑙𝑒() samples configurations uniformly across
the box. However, there are other methods for improving
performance, which we shall discuss later.

If we were to check all edges in the roadmap, this would lead to a

2
total of 𝑂(𝑁 ) visibility checks. This can be rather computationally
expensive for large 𝑁, and long paths are much more likely to

10 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

collide than short ones. Hence the idea of restricting visibility tests
only to "nearby" points is a fast way of determining a small set of
potential edges that have a better chance of being feasible. To do
this, we need first to determine a distance metric

𝑑(𝑝, 𝑞)

that measures some notion of path length. The simplest form might
measure Euclidean distance, but for configuration spaces that
blend translation and rotation it is often more suitable to use a
weighted geodesic metric.

Once a metric is defined, most frequently one of two methods are

employed to calculate the set of milestones "near" 𝑞:

• The 𝑘-nearest neighbors of 𝑞 in 𝑉.

• The 𝑅-neighborhood: all milestones in 𝑉 within some radius 𝑅 of 𝑞

.

Using fast nearest neighbors data structures, which will be

described later, the 𝑘-nearest neighbors can be computed in 𝑂(𝑘
+ log 𝑁) time, and the 𝑅-neighborhood can be computed in 𝑂(ℎ +
log 𝑁) time, where ℎ is the number of points returned. In any case,
this usually saves a huge amount of time because 𝑘, ℎ, and log 𝑁
are much smaller than 𝑁, and distance queries are fast. If we are
careful not to double-check the reverse of an edge that has been
checked before, at most 𝑘𝑁 / 2 (or ℎ𝑁 / 2) edges will be checked
in the roadmap.

3.1.2 Incremental variant¶

As written, the basic PRM algorithm places all of the (up to 𝑁)

samples and edges into the roadmap first before performing

11 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

search. But in easy problems, perhaps fewer samples were

needed to construct a roadmap that contained a solution -- say, the
first 𝑀 << 𝑁. If all we wanted was a feasible path from 𝑠 to 𝑔, it
would be foolish to continue adding unnecessary points!
Fortunately, it is straightforward to implement an incremental
variant of PRM with very little additional cost. (This is in fact the
typical variant used for PRM planning queries.)

Figure 2. An incremental version of PRM adds more samples to

the roadmap while maintaining the roadmap's connected
components. It terminates once the start and goal are in the same
component.

Algorithm Incremental-PRM gives an implementation of

Incremental-PRM using a special data structure to determine each
of the connected components of the graph. A connected
component consists of all milestones that are mutually connected
by any path.

Algorithm Incremental-PRM(s,g,T)

1. 𝑉 ← {𝑠, 𝑔}.

2. 𝐸 ← {}.

3. 𝐶𝐶[𝑠] ← {𝑠}.

4. 𝐶𝐶[𝑔] ← {𝑔}.

5. while time-elapsed < 𝑇 do

12 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

6. 𝑞 ← 𝑆𝑎𝑚𝑝𝑙𝑒()

7. if not Feasible(𝑞) return to Line 5.

8. Add 𝑞 to 𝑉

9. 𝐶𝐶[𝑞] ← {𝑞} (for now, 𝑞 gets its own connected component)

10. for all 𝑝 ∈ 𝑛𝑒𝑎𝑟(𝑞, 𝑉)

11. if Visible(𝑝, 𝑞) then

12. Add (𝑝, 𝑞) to 𝐸.

13. Merge 𝐶𝐶[𝑝] and 𝐶𝐶[𝑞] (merge connected

components of connected edge)

14. if 𝑔 ∈ 𝐶𝐶[𝑠] then (start and goal in same connected

component)

15. return the shortest path in 𝐺 = (𝑉, 𝐸) between 𝑠 and 𝑔.

16. return "no path"

Every time an isolated milestone gets added to the graph (Lines 3,

4, and 9), it gets assigned a connected component in the data
structure 𝐶𝐶. The connected components are maintained as more
edges are added to the roadmap (Figure 2). Once 𝑠 and 𝑔 are in
the same connected component (Line 14), the algorithm stops. In
Line 5, the main loop is stopped by a time limit 𝑇 rather than an
iteration limit. This means the overall running time can be
controlled more precisely, which is useful if the robot needs to
generate a path within a certain deadline.

To give more details about the 𝐶𝐶 data structure, it can be thought

of as a map from each milestone to its connected component: that
is, 𝐶𝐶[𝑣] set of milestones 𝑤 ∈ 𝑉 that can be reached from 𝑣 via

13 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

a path in the 𝐺 = (𝑉, 𝐸). After each change of 𝐺, 𝐶𝐶 is updated to

reflect any changes in connected component. The difficulty is that
each time an edge gets added, the connected components of
those two points need to be merged (Line 13). If this were done in
a naive fashion (say, by storing a list of connected milestones per
milestone), it could take an extremely long time (𝑂( | 𝑉 | ), where
|𝑉| is the number of vertices currently in 𝑉) for each update.
Fortunately, there is a special disjoint set (aka union-find) data
structure that is very fast at maintaining these sets. With this data
structure, it has been proven that a merge takes O(𝛼( | 𝑉 | )) time
on average, where 𝛼(𝑛) is a very, very slow growing function of 𝑛
called the inverse Ackermann function. It grows so slowly, in fact,
that for all practical values of 𝑛 it is less than 5, and hence this can
be considered a constant. Overall, to perform |𝐸| edge insertions
the overhead introduced by this data structure is 𝑂( | 𝐸 | 𝛼( | 𝑉
| )), which is essentially 𝑂( | 𝐸 | ), and hence no additional
asymptotic running time penalty is incurred by the incremental
algorithm.

3.1.3 Empirical performance¶

The performance of PRMs in practice can be quite variable from

problem to problem, and even run to run (depending on the initial
seed of a random number generator). In typical problems, the
probability of successfully finding a path increases as 𝑁 increases.
After a "burn in" phase with low 𝑁 where there is no change of
finding a solution, the probability of success rises fairly sharply
before tapering off. The slope of this tapering off depends on the
visibility characteristics of the problem, which we shall discuss
below.

14 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

(a) PRM, w=0.01 (b) PRM, w=0.005 (c) PRM, w=0.0025

Figure 3. Histograms of PRM failure rate as more time is spent

planning, taken over 100 runs on the same narrow passage
problem. As the narrow passage width w shrinks, the likelihood of
failure for a given time duration increases.

The average time needed for incremental PRM to solve a given

problem depends on a number of factors. Firstly, efficient collision
queries are essential, since thousands or millions of queries will be
made during a "reasonably" sized run (𝑁 = 1,000 to 100,000,
usually). Second, the nearness criterion (either number of
neighbors 𝑘 or radius 𝑅) should be set so that a small number of
edges are checked, but not too small.

The path quality of PRM solutions can be quite variable. If there

exists a suboptimal wide passage in ℱ while the optimal passage
is narrow, incremental PRM will find the suboptimal one first (with
very high likelihood). Basic PRM will find the suboptimal one when
𝑁 is low, but does have a chance to find the optimal one once 𝑁 is
large enough. There is also an issue of jerkiness of the produced
path, which tends to be more pronounced with incremental PRM is
used, or fewer neighbors are connected. We shall see strategies to
address the jerkiness of PRM paths when discussing optimizing
roadmaps and shortcutting.

15 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

3.1.4 Analysis of visibility and performance¶

The running time of PRM is composed of 𝑁 configuration collision

checks, 𝑘𝑛 edge collision checks (for the 𝑘-nearest-neighbor
variant), and 𝑛 nearest neighbor querie-. Let's assume the
likelihood of finding a sampling a configuration is constant and
nonzero, and that configuration and edge collision checks are 𝑂(1)
. Assuming brute-force nearest-neighbor queries, the overall
running time is 𝑂(𝑛 ). However, using more sophisticated nearest-
2

neighbor queries, this can be reduced to 𝑂(𝑛 log 𝑛).

The basic PRM and incremental PRM algorithms have been shown
to be probabilistically complete under relatively mild conditions.
This implies that the likelihood that the roadmap connects the start
and goal (assuming they are connected in ℱ) approaches 1 as
more milestones are added to the roadmap. But how quickly do
they converge? A key factor in the theoretical analysis (and
empirical performance) of a PRM are the visibility properties of the
free space. Using the language of unfavorable / favorable visibility
properties, we can mathematically formalize the intuitive notions of
a "narrow passage" and "wide passage". To do this, we will need to
introduce several definitions.

First, we define a measure 𝜇(𝑋) that assigns any free space

subset 𝑋 ⊆ ℱ a nonnegative scalar. Measures have a whole host
of requirements, but intuitively, 𝜇(𝑋) measures the volume of 𝑋.
(Note that if 𝑋 has lower dimension than 𝒞 , then 𝜇(𝑋) = 0; such
sets include points, lines, etc.)

Next, we define the visibility sets.

The Visibility set of a free configuration 𝑞 is the subset 𝒱 (𝑞) ⊆ ℱ

16 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

′
of points that are visible from 𝑞. Specifically, 𝒱 (𝑞) = {𝑞 ∈ ℱ |
′
IsVisible(𝑞, 𝑞 )}.

It is typically useful to think of visibility as respecting a given

connection radius, i.e., the constant 𝑅 in an 𝑅-neighborhood
connection strategy. We can also similarly define the visibility set
of a set of points as the union of the visibility sets of each point: 𝒱 (
′ ′
𝑋) = {𝑞 ∈ ℱ | 𝑞 ∈ 𝒱 (𝑞) for some 𝑞 ∈ 𝑋}.

ϵ=1
ϵ=0

ϵ=0

Figure 4. Some visibility sets of various points and spaces. A PRM

will require more samples to connect to points with small visibility
sets. Moreover, 𝜖-goodness is determined by the point in the free
space with the smallest visibility set. A convex space is (𝜖 = 1)
-good, while spaces with cusps and features of lower dimension
are not 𝜖-good for any 𝜖 > 0.

Intuitively, the milestones in a PRM (𝑉, 𝐸) with 𝑛 milestones are

likely to connect to a new milestone if the visibility set of 𝑉 is large.
Formally, if a new milestone 𝑞 is sampled uniformly at random from
ℱ, then the probability that it can be connected to 𝑉 is exactly 𝜇(𝑉)
/ 𝜇(ℱ). Since visibility is symmetric, the probability that 𝑞 cannot
be connected to any of the milestones in 𝑉 is equal to the
probability that 𝑞 cannot be connected to any of 𝑛 random
configurations. Since the milestones are drawn independently at
𝑛
random, this probability is (1 − 𝜇(𝒱 (𝑞)) / 𝜇(ℱ)) . Hence, obtain the
result:

17 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

The probability that a configuration 𝑞 can be connected to a PRM

𝑛
with 𝑛 milestones is 𝑃𝑟(𝑞 connected) = 1 − (1 − 𝜇(𝒱 (𝑞)) / 𝜇(ℱ)) ,
assuming that 𝑞 and each of the milestones is drawn at random
from ℱ.

Note that this value rapidly approaches 1 as 𝑛 increases, as long

as 𝜇(𝒱 (𝑞)) > 0.

What this also shows is that if visibility properties are not uniform
across the free space — that is, visibility sets are small in some
areas (narrow passages) and large in others (wide passages) —
PRMs will have a harder time connecting milestones in narrow
passages. This is because the speed at which 𝑃𝑟(𝑞 connected)
approaches 1 is dependent on 𝜇(𝒱 (𝑞)) / 𝜇(ℱ), with larger values
converging to 1 much faster than smaller values. (On average, 𝜇(ℱ
) / 𝜇(𝒱 (𝑞)) milestones will be needed in |𝑉| before 𝑞 lies in the
visibility set of 𝑉.)

We can analyze this situation further using bounds that depend on

the shape of the free space. Suppose that the minimum volume of
any configuration's visibility set is 𝜖 = inf𝑞 ∈ ℱ 𝜇(𝒱 (𝑞)) / 𝜇(ℱ). Then,
for any point 𝑞 sampled at random, the probability that it can be
′
connected to a given point 𝑞 is at least 𝜖. If 𝜖 > 0, we say that the
free space is 𝜖-good.

Since the visibility bound 𝜖 holds across the space, we can see that
the probability that any 𝑞 is in the visibility set of 𝑉 is at least 1 − (1
𝑛
− 𝜖) . Keep in mind that we have not mentioned dimensionality 𝑑
in this discussion and only volumetric ratios, so the performance
here has no direct relation to 𝑑. However, note that with a fixed
connection radius 𝑅, the volume of any visible set cannot be
𝑑
greater than 𝑂(𝑅 ) (the volume of an 𝑅-ball), and hence there is

18 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

an implicit exponential dependence of performance on dimension.

This also shows that to improve a PRM's visibility set in spaces of
higher dimension, it is necessary to set the connection radius 𝑅
relatively large.

Is 𝜖-goodness all that we need to analyze PRM performance? No!

Notice that we have only addressed the problem of whether a point
can be connected to a single milestone in the PRM, but not
whether it can reach all other reachable milestones with a feasible
path. Specifically, we need to examine whether milestones in the
same connected component of ℱ are also in the same connected
component of (𝑉, 𝐸). For this, we need a concept called
expansiveness. Intuitively, a space ℱ has high expansiveness if
for any partition of ℱ into two sets 𝐴 and 𝐵, a significant portion of
𝐴 can see a significant portion of 𝐵. This means that the likelihood
that one or more edges of the PRM cross any boundary in ℱ
increases as more milestones are added.

X F\X X F\X

β-lookout

ϵ≈0.5 β small β large

(a) (b) (c) (d)

Figure 5. (a) Even if a space is 𝜖-good, a PRM may have a difficult

time connecting two regions. (b) A 𝛽-lookout of a set 𝑋 is the
subset of 𝑋 that can see a 𝛽 fraction of its complement. (c) A
narrow passage causes certain 𝛽-lookouts to have small volume,
reducing the expansiveness of the space. (d) A convex set is
maximally expansive (𝛽 = 1).

More formally, we describe a simplified version of the argument in

19 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Hsu et al 97. Let us define the 𝛽-lookout of a subset 𝑋 ⊂ ℱ as the

subset of configurations in 𝑋 that can see an 𝛽-fraction of the
complement of 𝑋. Mathematically, this is defined as follows:

The 𝛽-lookout of 𝑋 is the set 𝛽-lookout(𝑋) = {𝑞 ∈ 𝑋 | 𝜇(𝒱 (𝑞)

∩ 𝑋¯ ) ≥ 𝛼𝜇(𝑋¯ )}, where 𝑋¯ = ℱ ∖ 𝑋 is the complement of 𝑋.

We define the expansiveness 𝛽 of ℱ as the largest value such that,

for any partition ℱ = 𝑋 ∪ 𝑋¯ , the volume of 𝛽-lookout(𝑋) is at least
𝛽𝜇(𝑋). If 𝛽 > 0, then we say that the free space is 𝛽-expansive.

(Note that 𝛽 ≤ 𝜖, since each point must see a 𝛽 fraction of its

complement.)

It has been proven that for any 𝛽-expansive space, the probability
that a roadmap fails to connect the start and the goal with a
feasible path drops exponentially toward 0 (provided that they are
in the same connected component of ℱ). Specifically, a bound can
be formulated in the following form:
−𝑑(𝛽)𝑛
𝑃𝑟(failure | 𝑛 milestones) ≤ 𝑐(𝛽)𝑒 .

Moreover, the convergence constants are directly related to 𝛽, with

larger values of 𝛽 leading to faster convergence (smaller 𝑐 and
larger 𝑑). Exponential convergence bounds are favorable because
they show that the expected running time and its variance are
bounded, which is not true for all convergence rates (consider, for
example, the bound 𝑃𝑟(failure | 𝑛) ∝ 1 / 𝑛).

Intuitively, the method of proof considers the idea of a linking

sequence of regions connecting 𝑠 and 𝑔, such that a milestone is
sampled in each region, then 𝑠 and 𝑔 will be connected. If the
space is expansive, then it can be shown that such a linking
sequence exists, has finite length, and the regions have non-zero

20 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

measure. The details of these proofs are out of the scope of this
book.

3.2 PRM variants¶

3.2.1 Rapidly-Exploring Random Trees (RRTs)¶

One of the most popular PRM variants is the Rapidly-Exploring

Random Tree (RRT) algorithm, which grows a tree rather than a
graph. Originally developed for kinodynamic planning, it is easily
adapted to kinematic planning as well. The specific variant we will
discuss is called RRT-Connect, which is a bidirectional search.

RRT-Connect grows two trees of feasible paths, one rooted at the

start and the other at the goal. At each iteration, both the start and
the goal trees are extended toward a randomly sampled
configuration. Then, if the trees are close enough, a connection will
be attempted between them. If connected, the joined trees contain
a unique path from the start to the goal.

Algorithm RRT-Connect

1. 𝑇𝑠 ← {𝑠}.

• 𝑇𝑔 ← {𝑔}.

• for 𝑖 = 1, . . . , 𝑁 do

• 𝑞𝑟𝑎𝑛𝑑 ← 𝑆𝑎𝑚𝑝𝑙𝑒()

• 𝑞𝑒 ← Extend-Tree(𝑇𝑠 , 𝑞𝑟𝑎𝑛𝑑 , 𝛿) (extend start tree at most 𝛿

distance toward 𝑞𝑟𝑎𝑛𝑑 )
′
• 𝑞𝑒 ← Extend-Tree(𝑇𝑔 , 𝑞𝑟𝑎𝑛𝑑 , 𝛿) (extend goal tree at most 𝛿
distance toward 𝑞𝑟𝑎𝑛𝑑 )

21 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

′ ′
• if 𝑑(𝑞𝑒 , 𝑞𝑒 ) ≤ 𝛿 and Visible(𝑞𝑒 , 𝑞𝑒 ) then (trees are close
enough)
′
• Add edge 𝑞𝑒 → 𝑞𝑒 to connect 𝑇𝑠 and 𝑇𝑔

• return the path from 𝑠 to 𝑔

• return "no path"

Algorithm Extend-Tree(𝑇, 𝑞𝑟𝑎𝑛𝑑 , 𝛿)

1. 𝑞𝑛𝑒𝑎𝑟 ← 𝑁𝑒𝑎𝑟𝑒𝑠𝑡(𝑇, 𝑞𝑟𝑎𝑛𝑑 )

2. 𝑞 ← 𝑞𝑛𝑒𝑎𝑟 + min 1, 𝑑(𝑞𝑟𝑎𝑛𝑑𝛿,𝑞𝑛𝑒𝑎𝑟 ) (𝑞𝑟𝑎𝑛𝑑 − 𝑞𝑛𝑒𝑎𝑟 )

3. if Visible(𝑞𝑛𝑒𝑎𝑟 , 𝑞) then

4. Add edge 𝑞𝑛𝑒𝑎𝑟 → 𝑞 to 𝑇.

5. return 𝑞.

6. return 𝑞𝑛𝑒𝑎𝑟 .

Specifically, the pseudocode is listed in Alg. RRT-Connect. 𝑇𝑠 and

𝑇𝑔 denote the trees rooted at the start and goal, respectively. In
Line 3, a random configuration is drawn, and in Lines 4 – 5, the
trees are extended toward it along a straight line path using the
Extend-Tree subroutine. RRT has a key parameter 𝛿, which a limit
to how far a tree can be extended on each step. In other words,
every edge in each tree has length no more than 𝛿. Also, if the two
extended milestones are within distance 𝛿, they are connected. For
small values of 𝛿, it is more likely for each extension to succeed,
but the tree makes slower progress in exploring the free space.

Pseudocode for Extend-Tree is given in Alg. Extend-Tree. It first

performs a nearest-neighbor query on the milestones in the given

22 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

tree to determine a milestone 𝑞𝑛𝑒𝑎𝑟 . It then extends a short path no

more than distance 𝛿 toward the destination 𝑞𝑟𝑎𝑛𝑑 . If this edge is
visible, then it is added to the tree.

Unlike PRMs, RRTs do not use the configurations coming from the
𝑆𝑎𝑚𝑝𝑙𝑒() function directly, nor do they attempt more than one
edge connection per iteration. Hence, they sample points in a
different distribution than PRMs. But what is this distribution? We
first introduce the concept of a Voronoi diagram, which defined for
some set of points 𝑋 = {𝐱1 , …, 𝐱𝑛 }. The Voronoi diagram is a
partition of the space 𝒞 into Voronoi cells , one per point. The cell
𝐶𝑖 corresponding to a point 𝐱𝑖 is the subset of 𝒞 for which 𝐱𝑖 is the
closest point. In other words,

𝐶𝑖 ≡ {𝐱 ∈ 𝒞 | 𝑖 = arg =min 𝑑(𝐱, 𝐱𝑖 )}

𝑖 1,…,𝑛

RRT is said to employ a Voronoi bias strategy because each

milestone in a tree is selected for expansion (i.e., be the nearest
node to 𝑞𝑟𝑎𝑛𝑑 ) with probability proportional to the volume of its
Voronoi cell. This means that milestones that are closer to
unexplored areas of 𝒞 have a higher likelihood of being expanded.
Moreover, the extended milestone will have a higher likelihood of
extending the tree in unexplored directions (and hence the term
rapidly exploring applies here).

RRTs are appealing because tree data structures are a bit simpler
to implement than graphs. Also, the RRT explores locally first, so if
the start and goal are nearby, the RRT may do significantly less
work than a PRM. However, RRT performance is generally more
sensitive to the choice of a distance metric, and is generally better
at exploring than refining.

23 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Figure 6 To escape the mouth of a bugtrap, an RRT needs to

sample a very carefully chosen sequence of milestones within the
general area that it has already explored. But due to the Voronoi
bias, it frequently attempts infeasible extensions from the
highlighted frontier nodes.

As an example, the "bugtrap" problem illustrated in Figure 6 tends

to pose challenges for the RRT. In this and many other problems, a
planner needs to strike a balance between exploration toward new
areas and refinement of the roadmap in existing areas. Let's
assume RRT only grows a tree from the start; it is easy to imagine
double-bugtraps that cause the same behavior for the goal. Here,
the bug has a very difficult time wiggling out of the opening of the
trap because it appears purely from the Voronoi bias that the
frontier has not yet been adequately explored. However, each
attempted extension ends up bumping into the walls of the trap. A
sequence of precisely-chosen values of 𝑞𝑟𝑎𝑛𝑑 are needed to
escape the trap, which is highly unlikely to occur by chance.

Moreover, the theoretical analysis of RRT is more challenging

because its tree expansion strategy is history-dependent. In fact,
the probabilistic completeness proof contained in the original RRT
paper was been shown to be flawed, and has only been corrected
recently! The best exponential convergence bound found so far
also that the expected running time is dependent on a factor of the
−𝑑
form 𝑐 where 𝑐 is the minimum of 𝛿 and the clearance of some

24 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

feasible path connecting the start and goal, and 𝑑 is the dimension
(Kleinbort et al, 2018). This bound is, however, extremely loose,
and RRT empirical performance is not directly correlated with
dimensionality, and like PRM typically enjoys better performance in
spaces with favorable visibility properties. One caveat is that the
expansion radius 𝛿 must be set larger in spaces of higher
dimension to avoid extremely slow convergence. In general it can
be challenging to say whether an RRT or PRM will work better for
a given problem without empirical testing.

3.2.2 Nonuniform sampling strategies¶

Since PRM and RRT performance depends highly on how well

samples are placed in critical regions, several strategies have been
developed to boost performance with nonuniform sampling. PRMs
benefit from placing more samples in low-visibility regions, which
requires identifying areas that are relatively constrained or close to
obstacles. One way to do this is to record how many feasible and
infeasible edges were attempted for each milestone (these are
stored as counts 𝑛𝑓 [𝑞] and 𝑛𝑖 [𝑞], respectively, for each 𝑞 ∈ 𝑉).
After 𝑁 samples, more samples are added near the milestones
with a large fraction of infeasible edges, with the hope that these
milestones are located in low-visibility regions where a denser
sampling is needed to make connections. Specifically, we might
pick a milestone 𝑞 ∈ 𝑉 with probability proportional to 𝑛𝑖 [𝑞] / (𝑛𝑖 [𝑞
] + 𝑛𝑓 [𝑞]) and then sample a new configuration from a disk
centered at 𝑞 with radius 𝑅. If feasible, the new milestone is
connected to the roadmap as usual.

Another method that can boost PRM performance in low-visibility

spaces is the Gaussian sampling strategy. The idea is to increase

25 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

the density of milestones near the boundaries of obstacles, since

low-visibility regions will certainly be located near obstacles. The
method actually draws two samples: one 𝑞1 at random, and the
second 𝑞2 from a multivariate Gaussian distribution (see Appendix
A.3.) centered at 𝑞1 and with standard deviation 𝜎. Then, only if
exactly one of the samples is feasible, that sample is kept.
Otherwise, both are thrown out. This ensures that the segment
between 𝑞1 and 𝑞2 straddles the boundary between the free space
and forbidden region.

It might seem odd to throw away perfectly good feasible samples,

since adding them to the roadmap won't hurt (and can only help)
connectivity. However, every additional milestone incurs additional
work to test and connect edges. In fact, edge collision checking is
often the dominant cost of planning. It turns out that in the
presence of narrow passages, the added cost to generate samples
is worth it, and Gaussian sampling can perform quite well.
However, for best performance the perturbation standard deviation
𝜎 must be tuned to trade off these competing costs.

RRTs benefit from a slight goal bias which drives the tree toward
the goal. In RRT-Connect, this could take the form of sampling
𝑞𝑟𝑎𝑛𝑑 from 𝑇𝑔 some fraction of the time, which would drive
extensions of 𝑇𝑠 toward the goal. Similarly, the reverse search
could sample 𝑞𝑟𝑎𝑛𝑑 from 𝑇𝑠 some fraction of the time, and drive
extensions from 𝑇𝑔 toward the start. This takes the form of
replacing Lines 4 – 6 in Algorithm RRT-Connect with the following
code:

1. if {𝑟𝑎𝑛𝑑() ≤ 𝑝𝑔𝑏 }

• if {𝑟𝑎𝑛𝑑() ≤ 0.5}

26 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

′
• 𝑞𝑒 ← 𝑅𝑎𝑛𝑑𝑜𝑚𝑉𝑒𝑟𝑡𝑒𝑥(𝑇𝑔 )
′
• 𝑞𝑒 ← Extend-Tree(𝑇𝑠 , 𝑞𝑒 , 𝛿)

• else

• 𝑞𝑒 ← 𝑅𝑎𝑛𝑑𝑜𝑚𝑉𝑒𝑟𝑡𝑒𝑥(𝑇𝑠 )

• 𝑞𝑒 ← Extend-Tree(𝑇𝑔 , 𝑞𝑒 , 𝛿)

• else Perform Lines 4 – 6 as usual

Here the term 𝑝𝑔𝑏 in Line 1 is the probability of using goal biasing,
and Line 2 decides according to an unbiased coin flip whether to
extend toward the start or toward the goal. The function 𝑟𝑎𝑛𝑑()
samples from the uniform distribution on [0, 1].

3.2.3 Multi-query PRMs¶

Another variant of PRMs that is useful in some scenarios is the

multi-query PRM. As presented, the PRM method finds a path for a
given (𝑠, 𝑔) query and then throws out the roadmap for the next
query. In some cases, we would like the robot to plan another path
in the same environment. Or, a team of robots may be traversing
the same environment. In this case, it makes sense to precompute
a good PRM and then reuse it for multiple queries. This is because
the primary cost of PRM planning is in the construction phase,
while the graph search phase is quite fast.

PRM construction proceeds like before, but without any endpoints.

Then, to query the existing roadmap for a start and goal (𝑠, 𝑔), we
try connecting 𝑠 and 𝑔 to nearby milestones using visibility queries.
Then, the augmented PRM is searched for a path. To keep the
roadmap from growing if many queries are to be made, 𝑠, 𝑔, and

27 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

all the edges connecting them are removed from the roadmap
before terminating the query.

3.2.4 Lazy collision checking in PRMs¶

For complex robots and/or environments, such as those composed

of CAD models, the most significant computational expense in
PRMs and RRTs is checking visibility of edges (i.e., dynamic
collision checking because each check may require tens,
hundreds, or thousands of static collision checks. Furthermore, for
complex robots, self-collision testing may need to be performed
beween all pairs of links, so even a single static collision check can
take milliseconds of compute time. This can add up quickly, as the
roadmap begins to contain thousands of milestones.

An effective heuristic for accelerating PRM performance is to

perform lazy collision checking, which delays collision checking for
edges until a candidate path to the goal is found. The hypothesis is
that if the endpoints of an edge are collision free, then the path
between them is also likely to be free. Since most edges in the
PRM aren't used in the final path, it is a waste to devote effort
checking their collision status. If the path does contain a collision,
the offending edge can be removed and planning can continue.

The Lazy PRM algorithm can be implemented in both basic and

incremental forms. A lazy Basic PRM variant is as follows:

1. Create a PRM (𝑉, 𝐸), assuming IsVisible always returns true.

2. Find a path from 𝑠 to 𝑔, 𝑣1 = 𝑠, 𝑣2 , …, 𝑣𝑚 − 1 , 𝑣𝑚 = 𝑔 using search. If

no path is found, return failure.

3. Check each edge IsVisible(𝑣𝑖 , 𝑣𝑖 + 1 ), 𝑖 = 1, . . . , 𝑚 − 1 for collision.

28 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

4. If any edge (𝑣𝑖 , 𝑣𝑖 + 1 ) is not feasible, delete it from 𝐸 and return to

2.

5. If all edges are feasible, return 𝑣1 → 𝑣2 → ⋯ → 𝑣𝑚 as the path.

In this design, it is helpful to cache which edges have been found

to be visible to avoid re-checking edges in step 3. Another speed
improvement is to use the costs of optimal paths to 𝑔 in the original
PRM as a heuristic for A search (used in the Batch Informed Trees
algorithm).

A lazy incremental PRM variant is as follows:

1. During roadmap construction, IsVisible is assumed to always

return true.

2. In line 15, once a path is found to the goal, the shortest path
connecting 𝑠 and 𝑔 is checked for collision, as in steps 2 – 5 in the
lazy Basic PRM variant.

3. Connected components need to be recomputed when edges are

found to be infeasible.

To implement this efficiently, step 3 must be implemented so that

connected components can be updated quickly when the graph
changes. One way of doing this in conjunction with the path search
is a dynamic shortest paths data structure, which stores the cost of
the shortest path to every node in the graph. This data structure
should be updated every time an edge is added or removed.
Although in the worst case, 𝑂(𝑛) costs must be updated, the vast
majority of cases are typically cheap.

To adapt RRT to perform lazy collision checking, we have a

problem figuring out what to do with infeasible edges. Suppose we
find that an edge near the start is infeasible: discarding it would

29 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

break the tree structure, or we could delete the subtree of

descendants of the edge, but this would waste a significant
amount of prior effort. Instead, a bidirectional tree-based lazy
collision checking strategy, introduced in the SBL algorithm
(Sanchez-Ante and Latombe, 2003), avoids discarding subtrees.
Instead, it maintains bidirectional trees as in RRT-Connected, and
checks edges for collision once a path is found from the start to the
goal. If an edge is found to be in collision, then it switches the
subtree of descendants of that edge to the other subtree. This
takes a degree of bookkeeping to update the tree data structures,
but can be done quickly.

3.3 Dynamic collision checking¶

So far we have assumed that edges in configuration space can be

checked for collision using the 𝑉𝑖𝑠𝑖𝑏𝑙𝑒 subroutine, but checking
collisions is not as straightforward as in simple geometric spaces,
where we could simply check the collision status of a line segment.

The simplest method for approximating the feasibility of a

configuration-space line segment ¯𝑎𝑏
¯¯¯¯¯¯¯ is to subdivide ¯𝑎𝑏
¯¯¯¯¯¯¯¯ into small

segments, with configurations 𝑞1 = 𝑎, 𝑞2 , …, 𝑞𝑛 − 1 , 𝑞𝑛 = 𝑏 uniformly

spaced no more than 𝜖 distance apart. Then, each of 𝑞1 , . . . , 𝑞𝑛 is
checked for collision using the 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒 subroutine. The segment
is considered visible if all configurations are feasible.

Note that this is only an approximate method that depends on the

resolution 𝜖. If this is too large, then collisions may be missed
between checked points 𝑞𝑖 and 𝑞𝑖 + 1 even if both 𝑞𝑖 and 𝑞𝑖 + 1 are
feasible. On the other hand, if 𝜖 is too small, then this takes a lot of
time. Precisely, the number of collisions checked is 𝑛 = ⌈𝑑(𝑎, 𝑏) / 𝜖⌉
.

30 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Another issue is the order in which the configurations are checked.

In the worst case, the edge is feasible, and all configurations must
be checked for feasibility. However, if the edge is infeasible, then
we can save time by finding an infeasible configuration quickly. Let
us suppose that both 𝑎 and 𝑏 are feasible. Then, in the absence of
additional information, the point that is most likely to lead to a
collision is the midpoint (𝑎 + 𝑏) / 2. This intuition gives a recursive
implementation that is effective in practice:

Algorithm Visible-Recurse(𝑎, 𝑏, 𝜖)

1. If this is the first recursive call, check 𝑎 and 𝑏 for collision. Return
false if either ¬ 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑎) or ¬ 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑏).

2. If 𝑑(𝑎, 𝑏) ≤ 𝜖 return true.

3. 𝑚 ← (𝑎 + 𝑏) / 2

4. If ¬ 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑚), return false.

5. Return Visible-Recurse(𝑎, 𝑚, 𝜖) ∧ Visible-Recurse(𝑚, 𝑏, 𝜖).

This approach is illustrated in Figure 10.a.

(a) Approximate dynamic CC (b) Exact dynamic CC with

with recursion adaptive recursion

Figure 7. Approximate and exact dynamic collision checking

31 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

methods.

Although this approximate technique is by far the most widely used

in practice, exact dynamic collision checking methods are also
available. These methods are based on similar recursions, but use
additional information about the clearance of a configuration.
Recall that the clearance 𝑐(𝑞) of a configuration is the distance in
C-space to the nearest C-obstacle. If we can show that 𝑑(𝑎, 𝑏) ≤
𝑐(𝑎) + 𝑐(𝑏), then we can be certain that ¯𝑎𝑏
¯¯¯¯¯¯¯¯ is collision free (Figure

10.b). This is because the balls centered at 𝑎 and 𝑏 with radii 𝑐(𝑎)
and 𝑐(𝑏), overlap, are in free space, and contain ¯𝑎𝑏
¯¯¯¯¯¯¯¯ in their union.

(In most cases, however, we do not have access to an exact

clearance function, but this reasoning still works when 𝑐(𝑞) is any
lower bound on clearance.) This gives the following exact
algorithm:

Algorithm Visible-Exact1(𝑎, 𝑏)

1. If this is the first recursive call, check 𝑎 and 𝑏 for collision. Return
false if either ¬ 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑎) or ¬ 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑏).

2. If 𝑑(𝑎, 𝑏) ≤ 𝑐(𝑎) + 𝑐(𝑏) return true.

3. 𝑚 ← (𝑎 + 𝑏) / 2

4. If ¬ 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑚), return false.

5. Return Visible-Exact1(𝑎, 𝑚) ∧ Visible-Exact1(𝑚, 𝑏).

This is an adaptive recursion method that terminates quickly when

𝑎 and 𝑏 are far from obstacles, but spends more time when the
line segment passes close to obstacles.

It is more likely to have workspace distance information between

pairs of objects. Let 𝐶𝑂1 , …, 𝐶𝑂𝑁 be the C-obstacles, and let 𝑐𝑖 (𝑞

32 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

) indicate the clearance of the 𝑖'th obstacle in workspace at

′
configuration 𝑞. We also need a function 𝜂𝑖 (𝑞, 𝑞 ) that gives an
upper bound on the distance that any point on the robot moves in
′
workspace during the motion from 𝑞 to 𝑞 .

For example, consider a 2R robot arm with link lengths 𝐿1 and 𝐿2 ,

the link geometries are simple line segments, and there are two
C-obstacles, one for each link. The collision constraint for link 1
only depends on 𝑞1 , and the points on the link are contained within
a circle with radius 𝐿1 . Moreover, in a movement of 𝜃 radians, the
tip of the link moves at most 𝐿1 𝜃 distance in workspace. Hence,
′ ′
𝜂1 (𝑞, 𝑞 ) = 𝐿1 | 𝑞1 − 𝑞1 |

is a suitable upper bound. Through similar reasoning, we can show

that
′ ′ ′
𝜂2 (𝑞, 𝑞 ) = (𝐿1 + 𝐿2 )( | 𝑞1 − 𝑞1 | + | 𝑞2 − 𝑞2 | )

is an upper bound on how far the tip of link 2 moves. There are
general formulas like this for arbitrary articulated robots.
Specifically,
𝑘−1
′ ′ ′
𝜂𝑘 (𝑞, 𝑞 ) = (𝑅𝑘 + � 𝐿𝑖 ) ‖ (𝑞1 , …, 𝑞𝑘 ) − (𝑞1 , …, 𝑞𝑘 )‖1
𝑖

is a suitable function for all nR robots.

The following algorithm uses this bound to perform exact collision

detection.

Algorithm Visible-Exact2(𝑎, 𝑏)

1. If this is the first recursive call, check 𝑎 and 𝑏 for collision. Return
false if either ¬ 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑎) or ¬ 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑏).

2. If 𝜂𝑖 (𝑎, 𝑏) ≤ 𝑐𝑖 (𝑎) + 𝑐𝑖 (𝑏) for all C-obstacles 𝑖 = 1, …, 𝑁, return

33 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

true.

3. 𝑚 ← (𝑎 + 𝑏) / 2

4. If ¬ 𝐹𝑒𝑎𝑠𝑖𝑏𝑙𝑒(𝑚), return false.

5. Return Visible-Exact2(𝑎, 𝑚) ∧ Visible-Exact2(𝑚, 𝑏).

A final performance enhancement is that if the condition in Line 2

is satisfied for any C-obtstacle, you can just ignore it from then on.
This focuses effort only on the constraints that need further
refinement.

A non-recursive variant of this algorithm, known as conservative

advancement, gives the earliest point of contact during a motion
from 𝑎 to 𝑏 by walking along the line segment as far as possible
ensuring that the condition in Line 2 holds. This is useful for
collision detection in simulation.

3.4 Nearest-neighbor queries¶

A significant computational expense in PRM, RRT, and their

variants, is computing nearest-neighbors (and near-neighbors).
There are three types of nearest-neighbor queries:

• (1-)Nearest-neighbor (NN(𝑞, 𝑃)), used in RRT.

• 𝑘-nearest-neighbors (kNN(𝑞, 𝑃, 𝑘)), used in PRM

• 𝑅-neighborhood (near(𝑞, 𝑃, 𝑅)), used in PRM

These are important subroutines for a variety of application areas,

including machine learning and geographic information systems,
and hence the number of nearest neighbors algorithms and
software packages is quite abundant. However, in motion planning,
there are two problems that often times break the assumptions

34 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

used in such packages:

1. The point set 𝑃 is dynamic, and so extensive precomputation of

data structures is not acceptable. Whatever data structures are
used must support fast point insertion.

2. The distance metric is often non-Euclidean, and can even be non-

Cartesian in the case of common geodesic spaces used in
robotics, like SO(2) and SO(3).

Brute-force nearest neighbors simply loops through each point,

and returns the one with smallest distance to the query point. This
runs in 𝑂(𝑛) time, and similar algorithms can be used for kNN()
and near() queries. It is also highly general, and can work with
arbitrary metrics and spaces. However, if brute force nearest
neighbors is used, leads PRM / RRT planning to be quadratic in
the number of milestones. As a result, faster algorithms are usually
needed.

3.4.1 k-d trees¶

A 𝑘-d tree data structure is a spatial hierarchy that recursively

𝑘
divides a Cartesian space ℝ into regions by splitting each region
by a hyperplane. The 𝑘 here refers to the number of dimensions in
the space, not the 𝑘 in the kNN query. In the following, let us revert
back to our original notation where dimensionality is denoted 𝑑.
Each hyperplane is aligned to one of the 𝑑 primary axes. An
illustration of a k-d tree is shown below.

35 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

(a) k-d tree holding (b) First leaf (c) Second leaf
14 2-D points reached in NN reached in NN
query query
A
E
- +
B
B C

- + - +
C
D E

A - + - +
D

Figure 8. (a) 𝑘-d trees recursively divide a space into rectilinear

regions. Each leaf of the tree contains a set of points contained in
that region. (b) For a nearest-neighbor query (blue point), the leaf
containing the point is reached first, and the closest point in the
leaf is found (blue circle). This forms an upper bound on the
nearest neighbor's distance, and any leaves further than this
distance will be pruned. (c) Only one more leaf is visited before the
nearest neighbor is found.

More formally, the binary tree 𝑇𝑘𝑑 is composed of nodes 𝑁. Each

leaf node contains a list of contained points pts, and each non-
leaf node contains a split dimension dim and split value value.
− +
Non-leaf nodes have exactly two children 𝐶 , and 𝐶 , and all
𝑑
points 𝑞 ∈ ℝ such that 𝑞𝑑𝑖𝑚 < 𝑣𝑎𝑙𝑢𝑒 belong to the negative child
−
𝐶 , while points such that 𝑞𝑑𝑖𝑚 ≥ 𝑣𝑎𝑙𝑢𝑒 belong to the positive
+
child 𝐶 .

We will describe how to query for the closest point (NN), with the
kNN and near queries implemented similarly. We traverse the tree
in a branch-and-bound manner similar to the bounding volume

36 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

hierarchy approach. Let's assume the Euclidean distance is used.

We maintain a closest point 𝑝𝑐𝑙𝑜𝑠𝑒 , initialized by picking a point
from 𝑃 at random. We proceed examining nodes 𝑁 recursively
starting from 𝑁 = 𝑟𝑜𝑜𝑡(𝑇𝑘𝑑 ). Psuedocode is below:

Algorithm KDTree-NN-recurse(𝑞, 𝑁, 𝑝𝑐𝑙𝑜𝑠𝑒 )

1. if 𝑁 is a leaf node, then

2. Let pts be the points contained in 𝑁.

3. Let 𝑝 be the closest point in pts to 𝑞.

4. return the closer of 𝑝 and 𝑝𝑐𝑙𝑜𝑠𝑒 .

5. else (non-leaf node)

6. Let its splitting plane be on axis dim with value value. Let its
− +
children be 𝐶 and 𝐶 .

7. if 𝑞𝑑𝑖𝑚 < 𝑣𝑎𝑙𝑢𝑒 then (negative side first)

− +
8. 𝐶1 ← 𝐶 , 𝐶 2 ← 𝐶 .

9. else (positive side first)

+ −
10. 𝐶1 ← 𝐶 , 𝐶2 ← 𝐶 .

11. 𝑝𝑐𝑙𝑜𝑠𝑒 ← KDTree-NN-recurse(𝑞, 𝐶1 , 𝑝𝑐𝑙𝑜𝑠𝑒 )

12. if |𝑞𝑑𝑖𝑚 − 𝑣𝑎𝑙𝑢𝑒 | ≤ 𝑑(𝑞, 𝑝𝑐𝑙𝑜𝑠𝑒 ) then (prune opposite side

if too far)

13. 𝑝𝑐𝑙𝑜𝑠𝑒 ← KDTree-NN-recurse(𝑞, 𝐶2 , 𝑝𝑐𝑙𝑜𝑠𝑒 )

14. return 𝑝𝑐𝑙𝑜𝑠𝑒

If 𝑁 is a leaf node, we check all its points in pts in brute force

manner (Lines 1 – 4). If 𝑁 is a non-leaf node, containing split

37 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

values dim and value, we first examine whether 𝑞𝑑𝑖𝑚 < 𝑣𝑎𝑙𝑢𝑒 or
𝑞𝑑𝑖𝑚 ≥ 𝑣𝑎𝑙𝑢𝑒. We first recurse on the corresponding child (Lines
7 – 11). This recursive call may update 𝑝𝑐𝑙𝑜𝑠𝑒 . Then, Lines 12 – 13
consider whether to check the opposite child. If |𝑞𝑑𝑖𝑚 − 𝑣𝑎𝑙𝑢𝑒 |
≥ 𝑑𝑐𝑙𝑜𝑠𝑒 , the distance |𝑞𝑑𝑖𝑚 − 𝑣𝑎𝑙𝑢𝑒| to the splitting hyperplane
is sufficiently large that there is no chance that the closest point
lies within the region defined by the opposite child. Hence,
recursion on the opposite child can be skipped. Regardless of the
outcome, we return 𝑝𝑐𝑙𝑜𝑠𝑒 .

Insertion. To insert points into a 𝑘-d tree, we can simply locate the
leaf node in which the point is located, and add it to the pts
structure. If the number of points in pts exceeds some threshold,
defined by a given parameter, then the node is converted into a
non-leaf node via splitting. Letting dim be the axis of the parent,
the chosen axis can either be (dim+1) mod d (round-robin
splitting) or the dimension with the largest variation in pts. In
either case, value is set to the median value of pts in that
dimension. A potential problem with incremental insertion is that
unless the points are distributed identically at random, the split
value of a leaf may not bisect the distribution of future points,
leading to an imbalanced tree. More advanced algorithms may
detect imbalanced trees during construction and rebalance them.

Extensions. 𝑘-d trees can be extended easily to weighted

Euclidean distances, since a weighted Euclidean distance is
equivalent to a rescaled version of Euclidean space. They can be
extended to handle other distance metrics or non-Cartesian
spaces with a bit more effort. For other distance metrics, the main
challenge is determining the point-hyperplane distance

38 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

min𝑝 | 𝑝[𝑑𝑖𝑚] = 𝑣𝑎𝑙𝑢𝑒 𝑑(𝑝, 𝑞) rather than the straightforward

calculation in Line 13. Non-Cartesian spaces require an alternative
definition of the splitting plane, sidedness determination (Line 7),
and a point-splitting plane distance (Line 13). Insertion also needs
to be modified to determine a reasonable splitting plane.

Performance. Notice in the NN query, recursion proceeds in depth

first fashion, and the first leaf node found is associated with the
region containing 𝑞. Ideally, the closest point in this region will be
very close to 𝑞, and hence most of the opposite sides will be
pruned. In the best case, all of the opposite sides will be pruned
and this runs in 𝑂(log | 𝑃 | ) time. In the worst case, all 𝑃 points
must be checked, in addition to the overhead of traversing the tree,
making this no better than brute-force search.

It can be seen that performance degrades if the points are

nonuniformly distributed or the tree is imbalanced, that is, the
number of points on either side of a split differ significantly.
Performance also degrades in spaces of higher dimension,
because point-point distances tend to be much larger than point-
hyperplane distances.

3.4.2 Approximate methods¶

Due to the degradation in performance of 𝑘-d trees in spaces of

higher dimension, it is common to apply approximate nearest
neighbors techniques. These sacrifice exactness of the output for
speed improvements. The sacrifice of exactness is usually worth it
in sampling-based planning because for most algorithms there is
no inherent reason to use the exact nearest neighbor(s) for
connection except that a closer milestone is slightly more likely to

39 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

yield a feasible edge than a farther one.

One straightforward approximate method that uses 𝑘-d trees is to

modify the pruning condition |𝑞𝑑𝑖𝑚 − 𝑣𝑎𝑙𝑢𝑒 | ≤ 𝑑(𝑞, 𝑝𝑐𝑙𝑜𝑠𝑒 ) in
Line 13 so that more nodes are pruned. A typical approach is to
inflate the point-hyperplane distance by a relative coefficient 𝜖𝑟𝑒𝑙 ≥
0 and absolute coefficient 𝜖𝑎𝑏𝑠 ≥ 0 so that the condition in Line 13
becomes (1 + 𝜖𝑟𝑒𝑙 ) ⋅ | 𝑞𝑑𝑖𝑚 − 𝑣𝑎𝑙𝑢𝑒 | + 𝜖𝑎𝑏𝑠 ≤ 𝑑(𝑞, 𝑝𝑐𝑙𝑜𝑠𝑒 ). With
such an approach, it is easy to show that the distance of the
⋆
resulting point 𝑝𝑐𝑙𝑜𝑠𝑒 to 𝑞 is no more than (1 + 𝜖𝑟𝑒𝑠 )𝑑 + 𝜖𝑎𝑏𝑠 . With
larger values of 𝜖𝑟𝑒𝑠 and 𝜖𝑎𝑏𝑠 , more branches are pruned at a
sacrifice of optimality.

Another approximation technique is Locality Sensitive Hashing

(LSH), which is based on the idea that if two points are close, then
random projections of the points onto a lower dimensional
subspace are also likely to be close. The details of LSH are
beyond the scope of this book, but many references are available.

Several software packages are available for exact and

approximate nearest neighbors queries. In Python, scipy contains
an implementation of 𝑘-d trees, scikit-learn implements 𝑘-d trees
and ball trees. Both libraries accept an approximation factor. For
approximate nearest neighbors, there are many packages named
ANN, and the FLANN library is a popular choice, used in the Open
Motion Planning Library.

3.5 Optimizing probabilistic roadmaps¶

Both PRM and RRT probabilistically complete, i.e., are increasingly

likely to find a feasible path as more samples are drawn. A natural
question to ask is whether they produce paths that are close to

40 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

optimal? Well, it is clear that if incremental PRM or RRT were to

terminate on the first path found, these paths may be far from
optimal. But if they were to continue planning past the first path,
then perhaps better and better paths could be found. This idea
forms the basis of the PRM and RRT algorithms, which have been
shown to be asymptotically optimal (Karaman and Frazzoli, 2009).

Asymptotically-optimal planner. A planner is asymptotically-

optimal if the cost 𝑐(𝑛) of the path that it produces after 𝑛
⋆
iterations approaches the cost of the optimal path 𝑐 as 𝑛
increases. If the planner is probabilistic, asymptotic optimality
⋆
means that the probability that the cost 𝑐(𝑛) does not approach 𝑐
⋆
is 0. Specifically, 𝑃𝑟( lim𝑛 → ∞ 𝑐(𝑛) − 𝑐 ) ≠ 0) = 0.

3.5.1 PRM* and RRT*¶

PRM has been shown to be asymptotically-optimal using the 𝑅

-neighborhood connection strategy, but not the 𝑘-nearest
neighbors strategy. Using the 𝑅 neighborhood strategy, however, in
the limit of large 𝑛 eventually tries to connect 𝑂(𝑛 ) milestones. It
2

has been proven that using a dynamically choice of 𝑅 and 𝑘 can

lead to an asymptotically optimal PRM planner, specifically the
⋆ log 𝑛 1 / 𝑑 ⋆ ⋆
values of 𝑅 (𝑛) ∝ ( 𝑛 ) and 𝑘 (𝑛) ∝ log 𝑛. Note that 𝑅
⋆
shrinks towards zero and 𝑘 grows, so that in both cases, each
new milestone is expected to be connected to 𝑂(log 𝑛) milestones,
which grows asymptotically. Hence, the number of edges in the
roadmap is expected to be 𝑂(𝑛 log 𝑛). (Note that there is a
constant factor in these expressions that depends on the volume of
free space and distance measure, and must be set sufficiently
large or else asymptotic optimality no longer holds.)

41 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

(a) PRM, 𝑘 = 5 (b) PRM, 𝑅 = 0.1 (c) PRM* (d) RRT*

neighbors connections

Figure 9. Convergence of various PRM and RRT variants. The

fixed-𝑘 strategy is not asymptotically optimal.

RRT has been shown not to be asymptotically-optimal in any case,

since the history-dependence of its tree growth strategy from the
nearest configuration in the tree prevents it from taking advantage
of shorter paths that may arise. The RRT* algorithm introduces a
new technique called "rewiring" that, upon expanding the tree,
changes the tree structure if it is possible to improve path lengths
by passing through a different nearby milestone. Let us assume a
unidirectional RRT. The main differences introduced to RRT are:

1. Optimal costs 𝑐(𝑞) through 𝑇 are stored at each node 𝑞, and

updated during Extend-Tree and Rewire.

2. After each successful extension, points in 𝑇 near the new

milestone 𝑞𝑒 are checked for whether better paths can be found
passing through 𝑞𝑒 .

3. Extend-Tree sets 𝑐(𝑞) = 𝑐(𝑞𝑛𝑒𝑎𝑟 ) + 𝑑(𝑞𝑛𝑒𝑎𝑟 , 𝑞), and returns 𝑛𝑖𝑙 if

the tree could not be successfully extended.

Algorithm RRT*

42 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

1. 𝑇 ← {𝑠}..

2. for 𝑖 = 1, . . . , 𝑁 do

3. 𝑞𝑟𝑎𝑛𝑑 ← 𝑆𝑎𝑚𝑝𝑙𝑒()

4. 𝑞𝑒 ← Extend-Tree(𝑇, 𝑞𝑟𝑎𝑛𝑑 , 𝛿)

5. if 𝑞𝑒 ≠ 𝑛𝑖𝑙 then Rewire(𝑇, 𝑞𝑒 , | 𝑇 | )

6. if 𝑑(𝑞𝑒 , 𝑔) ≤ 𝛿 and Visible(𝑞𝑒 , 𝑔)then

7. Add edge 𝑞𝑒 → 𝑔 to 𝑇

8. 𝑐(𝑔) = cost of optimal path from 𝑠 to 𝑔, if 𝑔 is connected, and ∞

otherwise

9. return "no path"

Algorithm Rewire(𝑇, 𝑞𝑛𝑒𝑤 , 𝑛)

⋆ ⋆
1. Neighbors ← Set of 𝑘 (𝑛)-nearest neighbors in 𝑇, or points in 𝑅 (𝑛
)-neighborhood.

2. for 𝑞 ∈ Neighbors sorted by increasing 𝑐(𝑞) do

3. if 𝑐(𝑞𝑛𝑒𝑤 ) + 𝑑(𝑞𝑛𝑒𝑤 , 𝑞) < 𝑐(𝑞) then (optimal path to 𝑞 passes

through 𝑞𝑛𝑒𝑤 )

4. 𝑐(𝑞) ← 𝑐(𝑞𝑛𝑒𝑤 ) + 𝑑(𝑞𝑛𝑒𝑤 , 𝑞)

5. Update costs of descendants of 𝑞.

6. if 𝑐(𝑞) + 𝑑(𝑞, 𝑞𝑛𝑒𝑤 ) < 𝑐(𝑞𝑛𝑒𝑤 ) then (optimal path to 𝑞𝑛𝑒𝑤

passes through 𝑞)

7. 𝑐(𝑞𝑛𝑒𝑤 ) ← 𝑐(𝑞) + 𝑑(𝑞, 𝑞𝑛𝑒𝑤 )

8. Set parent of 𝑞𝑛𝑒𝑤 to 𝑞.

9. Revise costs and parents of descendants of 𝑞𝑛𝑒𝑤 .

43 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Steps 4 and 8 can involve traversing large parts of the tree to

update costs and parents, using a depth first traversal of the tree.
In particular, in step 8, the parent of 𝑞𝑛𝑒𝑤 's child 𝑞𝑐 should be set
to 𝑞𝑛𝑒𝑤 if 𝑐(𝑞𝑛𝑒𝑤 ) + 𝑑(𝑞𝑛𝑒𝑤 , 𝑞𝑐 ) < 𝑐(𝑞𝑐 ). Then, the update should
be called recursively on 𝑞𝑐 . If that condition does not hold, the
recursion does not continue.

3.5.2 Convergence rate¶

Due to their proven asymptotic optimality and relative ease of

implementation, PRM* and RRT* have gained wide acceptance,
and have spawned many variants. But how quickly do these
planners converge to optimal?

First of all, PRM* and RRT* run slower than their normal
counterparts to find the first feasible path, because they do more
work per iteration. One way of mitigating this in RRT* is to disable
the rewiring step until a first feasible path is found, in which case
the algorithm begins identically to RRT.

Secondly, PRM* and RRT* perform 𝑛 configuration feasibility

checks and 𝑂(𝑛 log 𝑛) edge visibility checks. The number of
configuration checks is the same as in PRM and RRT, but PRM
performs 𝑘𝑛 edge checks and RRT performs 𝑛. So we pay a
logarithmic factor of computation speed to gain asymptotic
optimality.

Third, the number of milestones needed to obtain a desired

decrease in the suboptimality of the best path is exponential in the
dimensionality. Examine two cases: either a) the planner does not
yet have a path in the homotopy class of the optimal path, and
hence must explore the space further globally to make progress,

44 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

and b) the planner has a path fairly close to the optimal path and
can just perform local sampling to improve the current best path. In
case a), it can be shown that for any planner that only performs
binary collision queries, the expected number of samples needed
−𝑑
to obtain a solution in the optimal homotopy class is Ω(𝛿 ) (that is,
−𝑑
at least a constant times 𝛿 ) where 𝛿 is the clearance of a path in
the optimal homotopy class. The counterexample is shown below:
the optimal path passes through the tiny block, and its visibility set
𝑑
volume is 𝑂(𝛿 ), and at least two samples need to be placed there.

2δ
2δ

Figure 10. Sampling-based methods must sample small regions in

order to ensure asymptotic optimality. Using only collision checks
alone, there is not enough information to distinguish the left and
right environments. Hence, at least one sample must be placed in
the light blue region. Generalizing this example to higher
dimensions, we can always construct a counterexample in which
an asymptotically optimal planner must place a sample in a region
𝑑
of volume 𝑂(𝛿 ).

To address case b), we can also show that for a sampling based
planner to locally reduce the cost of the current best path, it must
⋆ 𝑑−1
place samples in a region with volume 𝑂((𝑐(𝑛) − 𝑐 ) ).

TODO: elaborate

45 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

3.5.3 Shortcutting¶

As noted above, PRM and RRT are only concerned with finding a
feasible path, and often produce jerky, unnatural paths.
Shortcutting is a very useful postprocessing heuristic, illustrated in
Figure 9, in which portions of a path are repeatedly replaced with
shorter, feasible segments.

Figure 11. A shortcutting heuristic can quickly smooth out the

jerkiest parts of paths that are generated by a sampling-based
planner.

In order to do so, two random points are sampled along the path.
With the path being a curve 𝑦(𝑠): [0, 1] → 𝒞 parameterized over the
unit interval, we sample two parameters 𝑢, 𝑣 ∈ [0, 1]. If 𝑉𝑖𝑠𝑖𝑏𝑙𝑒(𝑦(
𝑢), 𝑦(𝑣)) is true, then we replace the portion of the path between 𝑢
and 𝑣 with the straight line path. Otherwise, the sampling process
begins again. This repeats for some number of iterations.

Algorithm Shortcutting(𝑦, 𝑁)

1. for 𝑛 iterations do

2. Let 𝑎 ← 𝑦(𝑢) with 𝑢 ∼ 𝑈𝑛𝑖𝑓 𝑜𝑟𝑚([0, 1]).

3. Let 𝑏 ← 𝑦(𝑣) with 𝑣 ∼ 𝑈𝑛𝑖𝑓 𝑜𝑟𝑚([0, 1]).

4. If 𝑢 > 𝑣, swap 𝑢 with 𝑣 and 𝑎 with 𝑏.

46 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

5. if 𝑉𝑖𝑠𝑖𝑏𝑙𝑒(𝑎, 𝑏) then

6. Replace the portion of 𝑦 between 𝑢 and 𝑣 with ¯𝑎𝑏

¯¯¯¯¯¯¯¯.

7. Reparameterize 𝑦 to have unit domain.

8. return 𝑦.

Shortcutting is only a local optimization technique, and not a very

powerful one at that. But it is very fast, and this low overhead
makes it a very practical method for getting rid of the worst parts of
a jerky trajectory. In fact, when the time limit is short, this can give
much better paths than PRM and RRT! However, shortcutting
quickly stops making progress, and the asymptotically-optimal
techniques end up winning out.

We can overcome the "stalling out" problem by combining

shortcutting with random-restarts. This method yields an any-
time, asymptotically-optimal planner that simply applies repeated
restarts of an RRT (or PRM) followed by shortcutting. The shortest
path 𝑦𝑏𝑒𝑠𝑡 found after shortcutting is maintained through each of
these restarts. Eventually, the RRT might get lucky and find a path
close to optimal. It turns out that for many problems, this is an
excellent heuristic that outperforms PRM and RRT!

Algorithm Shortcutting with Random Restarts(𝑇𝑝𝑙𝑎𝑛 , 𝑁𝑠ℎ𝑜𝑟𝑡𝑐𝑢𝑡 ,

𝑁𝑟𝑒𝑓 𝑖𝑛𝑒 , 𝑇𝑡𝑜𝑡𝑎𝑙 )

1. 𝑃 ← RRT-Init()

2. Let 𝑦𝑏𝑒𝑠𝑡 ← RRT-Plan(𝑃, 𝑇).

3. If no path is found, return "failure"

4. 𝑃 ← RRT-Init() (reset planner)

47 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

5. while the time spent planning is less than 𝑇𝑡𝑜𝑡𝑎𝑙 do

6. 𝑦𝑏𝑒𝑠𝑡 ← Shortcut(𝑦𝑏𝑒𝑠𝑡 , 𝑁𝑟𝑒𝑓 𝑖𝑛𝑒 ) (continue to improve best

path)

7. 𝑦 ← RRT-Plan(𝑃, 𝑇𝑝𝑙𝑎𝑛 )

8. if path 𝑦 is found then

9. 𝑃 ← RRT-Init() (reset planner)

10. 𝑦 ← Shortcut(𝑦, 𝑁𝑠ℎ𝑜𝑟𝑡𝑐𝑢𝑡 )

11. if Cost(𝑦) < Cost(𝑦𝑏𝑒𝑠𝑡 ) then 𝑦𝑏𝑒𝑠𝑡 ← 𝑦 (better path found)

12. return 𝑦𝑏𝑒𝑠𝑡 .

In which case would this be this asymptotically optimal (as 𝑇𝑡𝑜𝑡𝑎𝑙 →

∞)? We can observe a few items:

• The main loop will generate an infinite number of RRT plans, as

long as the free space is such that RRT is probabilistically
complete and has finite expected running time. Note that the
planner is never reset until it outputs a feasible plan -- this is
somewhat like a sequential version of running a repeated RRT
loop and a shortcutting loop in parallel.

• The best path will continue to be refined over time, so if it's already
close to optimal, then it will continue to be locally optimized.

• If the best path is only locally optimal, the RRT has some chance
of finding a better one. This chance gets slimmer and slimmer as
the best path approaches the optimal cost 𝐶 ∗ . So, the role of the
shortcutting step in step 10 is primarily to raise this chance.

In [25]:

# Figure 12 (interactive): optimizing probabilistic roadmaps with

48 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

articulated robot↔

Planning with RRT ...

Planning with RRT* ...
Planning with RRT+shortcut ...
Planning with RRT+shortcut+restarts ...

3.6 Common pitfalls in employing PRMs¶

Sampling-based motion planning is appealing since it can be

implemented for a wide variety of problems by non-planning
experts. However, there are several issues that can cause PRMs to
fail. What is often frustrating is that the PRM will not provide a
rationale for failure! It just appears that it just "doesn't work". Some
of the most common pitfalls encountered when implement PRMs
and their variants are:

• Improper handling of non-Euclidean topology of 𝒞 in the distance

49 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

metric 𝑑(𝑝, 𝑞) and dynamic collision checking function.

• Improper scaling of the C-space / badly scaled distance thresholds

𝑅 or 𝛿.

• Providing infeasible start and goal configurations.

• Providing start and goal configurations in "deep pockets":

passages narrowing down as the endpoint is approached.

• Incorrect feasibility tests.

• Applying a planner when the free space volume is negligible, or

narrow passages are infinitely thin.

When debugging, it is often extremely useful to extract and visually

debug the roadmap produced by a planner. This helps diagnose
problems like the planner taking tiny steps, not expanding the
roadmap at all, or detecting phantom obstacles. This can be tricky
in high-dimensional spaces, since visualization must ultimately
take place on a 2D display, and a roadmap may contain thousands
of configurations and edges.

To handle topology, it is extremely important to ensure that the

notion of a "straight line path" in dynamic collision checking
interpolates nearly along a geodesic, and that the distance metric
is relatively close to a geodesic distance. When orientations are
present, if this issue were neglected and the C-space were treated
as Euclidean, then small positive rotations would never be
connected to small negative rotations. This will manifest itself as
artifacts in which the robot will either fail to find a path, or will rotate
in an unnecessarily long fashion.

For choosing thresholds, a rule of thumb is to start by setting 𝑅

and 𝛿 to be approximately 10% of the diameter of the C-space.

50 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

The values can then be fine-tuned to achieve better performance

on a given problem. A good rule of thumb is to aim to achieve
approximately 5 – 15 connections per milestone. This tends to
work well for setting the value of 𝑘 when 𝑘-nearest neighbors is
used as the nearness criterion in PRM.

The infeasible endpoint problem is often encountered when there

is a bit of error in the world model or the robot's sensing of its
configuration, and the robot starts or ends at a configuration that is
in contact (or close to it). There are two approaches to handling
this: before planning, adjust the world model so that the robot is
collision free (which can be hard), or slightly perturb 𝑠 and 𝑔 to
′ ′
new configurations 𝑠 and 𝑔 that are collision free with respect to
the robot's current knowledge. Then, the path is planned between
′ ′ ′ ′
𝑠 and 𝑔 , and the robot executes path 𝑠 → 𝑠 ⇝ 𝑔 → 𝑔. This,
however, assumes that the path to the perturbed configurations is
actually feasible in the real world, which requires a bit of care.

The deep pocket problem is faced particularly often in

manipulation or docking, in which the start or goal has the robot
touching the obstacle, and must make a careful, coordinated
maneuver to leave it. For example, when the robot is grasping an
object, its fingers are touching both sides of the object, and the
hand must slide carefully in or out of position without moving
laterally or rotating about certain axes. Hence, the passage is quite
narrow in at least 2 or 3 dimensions! In these pockets of free
space, the robot must take shorter steps, and most directions of
travel lead to collision. However, once the pocket is escaped (like
when the hand is away from a grasped object), then large steps
can again be taken. In other words, visibility is nonuniform across
ℱ. There are three general ways of handling this issue, all of which

51 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

require studying the manipulation problem more carefully:

1. Manually define a short docking/undocking maneuver that inserts

into / retracts from the pocket. This could be, for example in
manipulation, a Cartesian move that places the gripper in front of
the object with fingers wide open. The inverse of this maneuver is
used to determine the start and goal points for the planner.

2. Start a tree-growing planner like RRT from the constrained

endpoint with small step size. After some time, the farthest node
from the start is assumed to have wiggled out of the pocket and
point-to-point planning can begin from that new endpoint.

3. Develop an obstacle-sliding local planner or extension method that

allows the planner to generate motions that slide against obstacles.

It is easy to make bugs when defining feasibility tests, particularly

in more complex problems where feasibility requires passing many
conditions. This is problematic because the subroutine is the only
representation the planner has about the free space, so it needs to
accurately reproduce the C-obstacles of the problem or else the
planner will produce paths that will collide, or fail to find a solution
where one obviously exists. There are some newer techniques that
search for a small set of problematic C-obstacles blocking the way,
which can help debug incorrect settings (Hauser 2012). But
perhaps the first approach to try is to capture statistics during
planning to detect the frequency at which each condition passes
and fails inside the test. Some motion planning libraries will do this
automatically and ask the user to define individual conditions, but
in others this is up to the user. If a test never fails (or always
passes) this suggests an obvious implementation bug.

Finally, the free space must not contain a non-neglible volume of

52 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

space (that is, 𝜇(ℱ) / 𝜇(𝒞 ) > 0). This condition may be produced
when a constraint is introduced (like an IK constraint, or a
constraint that two objects must touch, or that a joint must take on
a particular value) that leaves all feasible configurations on a
manifold of lower-dimensionality of space. In these cases, the PRM
will not be able to generate samples with non-neglible probability.
One approach to handle this problem is to parameterize the
solution manifold explicitly. Extensions of PRMs are also available
to properly handle manifold constraints without a need for
parameterization; these techniques generate samples by projecting
them onto the feasible manifold, and also constructing paths that
move along the manifold. These will be discussed later in Chapter
12.3

4 Incomplete Methods¶

In addition to the above methods that satisfy some notion of

completeness, there are additional methods based on optimization
techniques that are incomplete: they have no guarantee of finding
a feasible path when one exists. They can, however, generally
produce paths quickly when they do work.

4.1 Potential fields¶

Potential fields are a well-studied technique that works using only

local information to guide the robot's movement, and is therefore
quite fast, making it appropriate for real-time obstacle avoidance as
well as path planning in relatively simple spaces.

The general idea is to consider the robot's configuration as being a

particle in some energy potential landscape. Due to "gravity" the
particle will feel some virtual forces equal to the negative of the

53 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

gradient of this landscape. If the landscape is constructed to have

a global minimum at the goal, then by following the gradient the
particle will, hopefully, arrive at the goal.

To construct such a landscape, the usual method is to combine an

attractive potential field in which the force is some gain constant
𝑘𝑎𝑡𝑡 times the vector pointing direction to the goal:
2
𝑃𝑎𝑡𝑡 (𝑞) = 1 𝑘𝑎𝑡𝑡 ‖ 𝑞 − 𝑞𝑔 ‖
2

along with a repulsive potential generating a repulsive force for

each obstacle. The repulsive force is chosen to grow larger
(typically toward infinity) as the robot gets closer to the obstacle.
Some limiting distance 𝜌0 is typically chosen where the effect of an
obstacle drops off to 0. One such function is the following:
⎧ 1 𝑘𝑟𝑒𝑝 (1 / 𝜌(𝑞) − 1 / 𝜌 ) 2 If 𝜌(𝑞) ≤ 𝜌0
𝑃𝑟𝑒𝑝 (𝑞) = ⎨ 2 0

⎩0 If 𝜌(𝑞) > 𝜌0

Here 𝜌(𝑞) is a function that measures the workspace distance

between the robot and the obstacle, and 𝑘𝑟𝑒𝑝 modulates the
strength of the force. The potential is infinity at 𝜌(𝑞) = 0 and drops
down to 0 at 𝜌(𝑞) = 𝜌0 . (Note that here we must be able to
calculate distance rather than just Boolean collision detection.)

The force acting on the particle is the negated gradient of each

potential:

𝑓𝑎𝑡𝑡 (𝑞) = − 𝑘𝑎𝑡𝑡 (𝑞 − 𝑞𝑔 )

and
⎧𝑘𝑟𝑒𝑝 ( 1 − 𝜌10 ) 1 2 ∂ 𝜌(𝑞) If 𝜌(𝑞) ≤ 𝜌0
𝜌(𝑞) ∂𝑞
𝑓𝑟𝑒𝑝 (𝑞) = ⎪
⎨ 𝜌(𝑞)

⎪
⎩0 If 𝜌(𝑞) > 𝜌0

54 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Then, to evolve the configuration over time as a particle in this

potential field, we use an iterative approach. At the current time
step, the robot is at position 𝑞𝑡 . The next point along the path is
given by:

𝑞𝑡 + 1 = 𝑞𝑡 + Δ𝑚𝑡 (𝑓𝑎𝑡𝑡 (𝑞) + 𝑓𝑟𝑒𝑝 (𝑞))

where 𝑚 is a virtual "mass" of the robot and Δ 𝑡 is the time step.

One potential issue with this method is that the magnitude of the
force vector can be highly varying, from 0 at the goal to infinity at
the boundary of an obstacle. To avoid huge jumps (or little
movement at all) in the path it makes sense to dynamically set the
mass to be proportional to the magnitude of the force. In this way,
consistent rate of progress is ensured as the path evolves.

In [44]:

# Figure 13 (interactive): potential field approach with articulated

robot↔

This method works well when the robot simply needs to move
slightly away from a straight line path to avoid obstacles, provided
that obstacles are relatively convex and spatially distant. Its main
advantages are 1) speed of computation, and 2) only local
information about obstacles is needed. However, like other local
methods it is prone to local minima caused either by concave
obstacles, or narrow passages where the repulsive forces from
either side of the passage cancel out the attractive force.

4.2 Trajectory optimization¶

Trajectory optimization is another potential-based method that

optimizes the overall shape of the robot's path to minimize cost.

55 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Unlike potential fields, for which the optimization variable is the

configuration of a single point in time, trajectory optimization uses
some parameterization of the entire path as the optimization
variable. This helps it avoid future obstacles and, in some cases,
avoid local minima that potential fields would fall prey to. Another
common use of trajectory optimization is as a postprocessor for
global planners such as grid-based A* or RRT to improve the
quality of the resulting path.

(a) Parameterizing an initial (b) Constrained

path optimization

q3
q2
q4
q6
q5
q0 q1

(c) Collocation methods (d) Penalty methods

Figure 14. (a) A 6-segment path connecting a start and goal is

parameterized by the coordinates of the 5 intermediate milestones.
(b) An optimized path after applying constrained optimization to
minimize path length and keep the milestones out of obstacles.

56 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Dotted lines indicate obstacle distance functions, and red arrows

indicate local negative gradients at each milestone. Only the fourth
milestone reaches an obstacle constraint. (c) Additional collocation
points, illustrated as white circles, can be added to prevent
segments from penetrating obstacles. (d) Penalty methods push
milestones further from obstacles at the expense of longer path
length. An an optimum, the negative gradients of the obstacle
potentials (brown and cyan arrows) balance the negative gradients
of the objective function (red arrows). A benefit of penalty methods
is that they allow unconstrained optimization methods to be used,
and these tend to be simpler to implement than constrained
optimization methods.

4.2.1 Parameterizing a path¶

Trajectory optimization begins with the definition of a

parameterized path repesentation in which the shape of a
candidate path 𝑦(𝑠) is dictated by a fixed number of path
𝑁
parameters 𝜃 ∈ ℝ . One example is the class of piecewise linear
paths that connect the start and the goal configurations with 𝑘
straight-line segments. A path interpolates between milestones 𝑞0
= 𝑞𝑠 , 𝑞1 , …, 𝑞𝑘 − 1 , 𝑞𝑘 = 𝑞𝑔 , and we can define the shape of the
path by the values of the 𝑘 − 1 intermediate milestones. This gives
us a path parameter

𝜃 = (𝑞1 , …, 𝑞𝑘 − 1 ) .

Any value of 𝜃 dictates the shape of some path, and any piecewise
linear path with 𝑘 segments corresponds to some value of 𝜃
(Figure 13.a). To make this dependence clear, we shall refer to the
path defined by a value 𝜃 as 𝑦𝜃 .

57 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

If the dimension of C-space is 𝑑, then 𝑁 = 𝑑(𝑘 − 1). Hence the

trajectory optimization problem can be quite high dimensional
(hundreds or thousands of dimensions) even for C-spaces with a
moderate number of dimensions.

4.2.2 Encoding objective functions¶

Next, we must encode the objective function as a function of the

path parameters. For minimizing path length, it may be tempting to
initially define the following cost function that minimizes path
length:
𝑘
𝑓 (𝜃) = � ‖ 𝑞𝑖 − 𝑞𝑖 − 1 ‖ .
𝑖=1

However, this formulation has the drawback that it is not

differentiable when two milestones are equal, and also has a null
direction when three milestones are on the straight line. It is more
numerically convenient to minimize the sum of squared distances
𝑘 2
𝑓 (𝜃) = � ‖ 𝑞𝑖 − 𝑞𝑖 − 1 ‖ .
𝑖=1

It can be shown that minimizing the sum-of-squares objective

function does indeed minimize path length (as 𝑘 grows large) and
ensures that milestones are evenly spaced. (Proving this fact
involves a nuanced argument using arc-length parameterizations
and the Cauchy-Schwarz inequality, so we omit it here.)

This objective function can be thought of as an energy function

that treats the path like an "elastic band". The energy minimum is
reached when the band is taut (i.e., milestones are along a straight
line). We will need to encode how the C-obstacles in a way that
prevents this elastic band from passing through them.

58 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

4.2.3 Encoding constraints¶

Now let us proceed to defining constraints, which we assume are

in the form 𝑔(𝑞) ≤ 0. As a first step, one might choose to simply
enforce constraints on the milestones:

⎡ 𝑔(𝑞1 ) ⎤
ℎ(𝜃) = ⎢ ⋮ ⎥ ≤ 0.
⎢ ⎥
⎣𝑔(𝑞 𝑘−1 ⎦
)

The resulting problem to minimize 𝑓 (𝜃) while enforcing ℎ(𝜃) ≤ 0 is

a constrained optimization problem (Appendix B.3), which can be
solved using a nonlinear program solver, like Sequential Quadratic
Programming (SQP). Efficient implementations will take advantage
of sparseness in the constraint Jacobian.

However, since we have only constrained milestones to lie outside

of obstacles, this formulation runs the risk of having two milestones
on either side of an obstacle, with the intermediate segment
crossing the obstacle (Figure 13.b). Let's consider how to avoid
constraint violations between milestones. One approach is to
"inflate" obstacles by adding a margin to the constraint equation:

𝑔(𝑞) + 𝛾 ≤ 0

with 𝛾 ≥ 0 the constraint inflation value. After optimization, the

milestones will lie a distance of 𝛾 outside of the obstacles, and if
we have chosen 𝛾 well (and with a bit of luck) while the path
segments may lie clear of the obstacles. However, this method
relies on the hope that the obstacles are not too "sharp" to overlap
the intermediate path segment, and also that by inflating obstacles
we do not lose much on optimality or cause the optimization
problem to become infeasible!

59 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

Another way to enforce constraints along segments is the

collocation point method. Collocation points are points along the
path at which constraints will be enforced, and these can be
spaced across the entire path, not just the milestones.

Specifically we can define some number 𝑀 of collocation points at

parameters 𝑠1 , …, 𝑠𝑀 ∈ [0, 1], usually evenly distributed along the
parameter space [0, 1] (Figure 13.c). The 𝑗'th collocation point lies
on a segment indexed by 𝑖𝑗 ∈ {1, …, 𝑘} and lies a fraction 𝑢𝑗 ∈ [0,
1] along the straight line segment, where these are determined so
that the configuration at the collocation point is:

𝑦𝜃 (𝑠𝑗 ) = 𝑞𝑖𝑗 − 1 + 𝑢𝑗 (𝑞𝑖𝑗 − 𝑞𝑖𝑗 − 1 ) .

We then define many inequality constraints on 𝜃 so that constraints

at each collocation point are enforced:
⎡ 𝑔(𝑦𝜃 (𝑠1 )) ⎤
⎢ ⎥
ℎ(𝜃) = ⋮ ≤ 0.
⎢ ⎥
⎣𝑔(𝑦𝜃 (𝑠𝑀 ))⎦

This approach avoids many of the problems of the constraint

inflation approach, but usually leads to higher computational costs
because the optimizer must contend with a larger number of
inequality constraints (𝑀 > 𝑘).

4.2.4 Penalty methods¶

An alternative and simpler approach lets us use unconstrained

optimizations (Appendix B.3) by converting hard constraints to
penalties in the objective function. In this approach we define a
penalty function for violating constraints:

60 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

𝑀
𝑓𝑝𝑒𝑛 (𝜃) = � max (𝑔(𝑦𝜃 (𝑠𝑗 )), 0) .
𝑗=1

Then by minimizing a weighted objective function

𝑓 (𝜃) + 𝑤𝑓𝑝𝑒𝑛 (𝜃)

using standard nonlinear optimization techniques (e.g., Quasi-

Newton methods), portions of the path for which constraints are
violated will be pushed out of the C-obstacle (Figure 13.d).
However, if 𝑤 is not set to be sufficiently high, then the optimizer of
the weighted objective function will still slightly overlap with the
obstacle. To address this, we can progressively increase 𝑤 to
reduce the amount of overlap. To prevent overlap altogether, we
can also allow the constraint violation penalty to extend a distance
𝛾 > 0 outside the region where the constraint is violated.
𝑀
𝑓𝑝𝑒𝑛 (𝜃; 𝛾) = � max (𝑔(𝑦𝜃 (𝑠𝑗 )), − 𝛾) + 𝛾 .
𝑗=1

4.2.5 Performance characteristics and alternative

approaches¶

Regardless of whether a constrained or unconstrained approach is

taken, there are two major issues with trajectory optimization:

• The computational cost of optimization depends strongly on the

number of path parameters and collocation points. If too few path
parameters are chosen then a feasible path may not be found; if
too few collocation points are chosen then the path may violate
constraints.

• For complex environments, the potential landscape in 𝜃 space is

littered with local minima (and typically, more minima appear as
the granularity 𝑘 of the path grows).

61 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

The problem of choosing collocation points can be addressed by

adaptively identifying the point along the path with maximum
constraint violation, in advanced optimization techniques known as
constraint generation or semi-infinite programming. Returning
again to the example of Figure 13.b, you can imagine identifying
the point on the 4th segment that lies deepest within the obstacle.
If we were to insert another milestone along the path at that point,
we now have an optimization problem with 6 milestones. By
solving this optimization problem, we will have pushed that point
out and reduced the maximum constraint violation along the path.
Semi-infinite programming techniques will solve several
optimization problems at increasing levels of discretization, in the
hope that it is cheaper to solve multiple smaller optimizations than
one large optimization with many collocation points.

The local minimum problem can be partially addressed either by

initializing the optimizer with a path from some other motion
planning method, like a sampling-based planner, or by using global
optimization techniques. The approach of seeding an optimizer by
a sampling-based planner is fast and often works well. However,
does not guarantee a globally optimal path, because the planner
may have produced a seed path in a suboptimal homotopy class or
basin of attraction. Global optimization may result in better paths,
but can be extraordinarily slow, particularly in high dimensional
spaces.

5 Summary¶

Key takeaways:

• Sampling-based motion planners can overcome some limitations

62 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

of the curse of dimensionality. However, they pay a cost in the

variance of solution quality and running time.

• The running time of such planners is dependent on the visibility

characteristics of the free space, which does not directly relate to
dimensionality. Running times will be fast in spaces of good
visibility.

• Probabilistic roadmaps (PRMs) and Rapidly-Exploring Random

Trees (RRTs) are the most widely used classes of such planners.
There are many variations on the basic structure.

• Shortcutting can be employed in postprocessing to achieve fast

(but local) improvements in path quality. To achieve global
improvements, optimizing variants of PRMs and RRTs are
available.

• Potential field methods use only local information to determine a

direction of movement and are extremely fast. They can work well
for real-time obstacle avoidance, but are prone to local minima.

• Trajectory optimization methods simultaneously optimize

milestones along an entire trajectory. However, there are tradeoffs
in choosing of the number of milestones used to represent a path
and how constraints are encoded, and they are also prone to local
minima.

6 Exercises¶

1. Let 𝑛 be the number of (feasible) milestones in a probabilistic

roadmap, and 𝑁 be the number of configurations sampled. Prove
that if a PRM algorithm is probabilistically complete as 𝑛 increases,
then it is also probabilistically completeness as 𝑁 increases, as
long as the chance of drawing a feasible sample is nonzero.

63 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

2. A PRM with a fixed connection radius 𝑅 can be thought of as

restricting the visibility set of a point to be intersected a
neighborhood of radius 𝑅. With this interpretation, are the visibility
properties of a space (𝜖-goodness and 𝛽-expansiveness)
dependent on 𝑅? Explain why or why not. How would the visibility
properties vary depending on whether the distance function was
chosen to use an 𝐿1 , 𝐿2 , or 𝐿∞ metric?

3. Suppose the free space is described by a set of 𝑚 C-obstacles 𝐶1

, . . . , 𝐶𝑚 . Let 𝒞 be the space in which configurations are
sampled, and let 𝜇 be the volume measure. For a sampled
configuration 𝑞, what are the probabilities of that 𝑞 lies within each
C-obstacle? If testing each obstacle has the same computational
cost, what is the fastest order in which the C-obstacles should be
tested?

4. Illustrate a free space in which Lazy PRM is expected to check a

large fraction of edges for visibility before finding a solution. Lazy
PRM may take more time than a standard PRM in this case. What
component of Lazy PRM would be the cause of this computational
overhead?

5. Does it make sense to build a lazy PRM in precomputation for

multi-query path planning? If so, give some examples of what
situations in which this approach would be useful. If not, explain
why not.

6. In our discussion of shortcutting, path length was used as the

objective function for optimization. Give an example of an objective
function for which shortcutting does not improve the path cost.
Then, describe a modification to the shortcutting algorithm so that
the objective function does not increase.

64 of 65 1/24/24, 17:41
MotionPlanningHigherDimensions about:reader?url=https%3A%2F%2Ffanyv88.com%3A443%2Fhttp%2Fmotion.cs.illinois....

7. What is the maximum number of static collision checks needed for

a PRM to check a path between milestones 𝑣1 , . . . , 𝑣𝑚 , given a
fixed resolution of 𝜖 for dynamic collision checking? How many
static collision checks and distance queries are needed for a PRM
to solve a problem, using Visible-Exact1 for dynamic collision
checking, where the clearance of the path 𝑦 = 𝑣1 → ⋯ → 𝑣𝑚 is 𝛿?

8. Implement a brute force 𝑘-nearest neighbor algorithm that runs in

𝑂(𝑛𝑘) time. Hint: store the 𝑘 nearest neighbors in an array, and
maintain the index of the neighbor with maximum distance. Can
you improve this to 𝑂(𝑛 log 𝑘) time?

9. Write pseudocode for an 𝑅-neighborhood query for a 𝑘-d tree.

Implement this, double checking that it works properly compared to
a brute-force approach on random datasets.

10. In the potential field approach, we need to evaluate the distance

field and its gradient when calculating the repulsive force (15). The
distance here is expressed in configuration space, but we typically
do not have direct access to such a function when working with an
articulated robot. Instead, we are typically able to compute
geometry-geometry distances in workspace. Show how to evaluate
a distance field as a function of configuration, and how to
approximate its gradient as a function of the configuration
variables. (Hint: forward kinematics and the Jacobian will be used
here. Assume your geometry-geometry distance function also
generates closest points.)

65 of 65 1/24/24, 17:41