CSCE 452/752: 16. Asymptotically Optimal Motion Planning

Introduction

Asymptotically optimal motion planning algorithms improve the quality of the solution over time.

Overview:

Standard sampling-based motion planning algorithms tend to produce paths that have lots of extra motion.
Asymptotically optimal (AO) motion planning algorithms overcome this problem by rewiring the graph as more vertices are added.
The most widely used AO algorithm is called RRT$^*$.

$ \def\R{\mathbb{R}} \def\C{\mathcal{C}} \def\Cfree{\C_{\rm free}} \def\Cobst{\C_{\rm obst}} $

Aside: Path smoothing

Path smoothing can help, but its limited by the homotopy class of the initial solution.

RRT$^*$: Main idea

RRT$^*$ differs from RRT in two ways:

Change 1: Each new node connects to the best nearby parent, determined by the new node's distance to the root. Not necessarily the node that was nearest neighbor.

Change 2: For each new node, check nearby nodes to see if any would be have smaller distances with the new node as their parent. If so, rewire the tree.

RRT$^*$: Example 1

Start from a tree with just the start configuration.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 1 candidate this time.

Choose the best parent for the new node.

Find rewiring candidates near the new node; 0 candidates this time.

Rewire if needed; 0 changes this time.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 1 candidate this time.

Choose the best parent for the new node.

Find rewiring candidates near the new node; 0 candidates this time.

Rewire if needed; 0 changes this time.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 2 candidates this time.

Choose the best parent for the new node.

Find rewiring candidates near the new node; 1 candidate this time.

Rewire if needed; 0 changes this time.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 1 candidate this time.

Choose the best parent for the new node.

Find rewiring candidates near the new node; 0 candidates this time.

Rewire if needed; 0 changes this time.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 3 candidates this time.

Choose the best parent for the new node.

Find rewiring candidates near the new node; 2 candidates this time.

Rewire if needed; 2 changes this time.

RRT$^*$: Example 2

Start from a tree with just the start configuration.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 1 candidate this time.

Choose the best parent for the new node.

Find rewiring candidates near the new node; 0 candidates this time.

Rewire if needed; 0 changes this time.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 1 candidate this time.

No valid parent available for the new node.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 1 candidate this time.

Choose the best parent for the new node.

Find rewiring candidates near the new node; 0 candidates this time.

Rewire if needed; 0 changes this time.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 2 candidates this time.

Choose the best parent for the new node.

Find rewiring candidates near the new node; 1 candidate this time.

Rewire if needed; 0 changes this time.

Choose a random sample.

Find the nearest neighbor.

Extend from the nearest neighbor toward the sample.

Find candidate parents for the new node; 3 candidates this time.

Choose the best parent for the new node.

Find rewiring candidates near the new node; 1 candidate this time.

Rewire if needed; 0 changes this time.

After 100 iterations

After 200 iterations

After 300 iterations

After 400 iterations

After 500 iterations

RRT$^*$: Example 3

Start from a tree with just the start configuration.

After 3000 iterations

Asymptotic optimality

Definition A probabilistically complete motion planner is asymptotically optimal if the expected length of the solution it produces converges to the optimal length as the number of samples increases. \[ \lim_{n \to \infty} E[c(P)] = c^* \]

Theorem RRT$^*$ is asymptotically optimal.