RRT* Algorithm (Rapidly-exploring Random Tree Star)

RRT* is a sampling-based path planning algorithm that starts like RRT, but keeps improving the tree after new samples are added.

Imagine a robot has already used RRT to find a collision-free path through a cluttered map. The path is valid, but it bends more than necessary because each new point was connected to the nearest existing node at the time. RRT* asks a better question: can this new point make the tree cheaper for itself or for nearby nodes?

Start With the RRT Tree

RRT grows a tree with four basic actions:

Randomly sample a point in free space.
Find the nearest node already in the tree.
Steer a short distance toward the sample.
Add the new node if the edge is collision-free.

This is enough to find a feasible route. However, the route depends heavily on the order of random samples. A node may connect to a nearby parent even when a slightly farther parent would produce a much shorter path from the start.

The Problem: Feasible Does Not Mean Efficient

For a robot, a valid path is only the first goal. A path that is much longer than necessary can waste battery, time, and space for maneuvering.

Basic RRT does not go back and improve old choices. Once a node is attached to a parent, that connection usually stays fixed. RRT* changes this by treating every new node as a chance to locally repair the tree.

The RRT* Idea

When RRT* adds a new node \(x_{new}\), it does two extra checks:

Choose parent: connect \(x_{new}\) to the nearby parent that gives the cheapest path from the start.
Rewire neighbors: check whether nearby existing nodes become cheaper if they connect through \(x_{new}\).

The tree still explores like RRT, but it also improves its path cost over time.

Step 1: Add a New Node

RRT* begins the same way as RRT. It samples a point, finds the nearest tree node, steers toward the sample, and checks for collision. If the new edge is blocked by an obstacle, the sample is ignored.

If the edge is collision-free, RRT* does not immediately attach the new node to the nearest node. First, it looks around the new node for other possible parents.

Step 2: Choose the Best Parent

Instead of asking, "Which node is closest to \(x_{new}\)?", RRT* asks, "Which nearby node gives the cheapest full path from the start to \(x_{new}\)?"

The nearest node might have a high cost from the start. Another nearby node may be slightly farther away from \(x_{new}\), but much cheaper overall.

RRT star selecting best parent example — RRT*: Select the best parent.

The selected parent is the one that gives the lowest total cost while still keeping the edge collision-free.

Step 3: Rewire Nearby Nodes

After \(x_{new}\) is added, RRT* checks nearby existing nodes. Some of those nodes may currently have expensive parents. If connecting through \(x_{new}\) gives a cheaper route from the start, RRT* changes their parent.

This is called rewiring. It is the step that lets RRT* improve old parts of the tree instead of only adding new branches.

RRT star rewiring example — RRT*: Check cost for rewiring and then rewire if appropriate.

Step 4: Use Cost and Radius to Make the Decision

Now we can make the previous steps precise. Rewiring decisions depend on two values:

A cost-to-come value that measures how expensive it is to reach each node from the start.
A near-node radius that decides which nodes are close enough to consider.

Defining the Cost Function in RRT*

In RRT, each node stores a cost-to-come* from the start:

\[ J(x) = J(\text{parent}(x)) + c(\text{parent}(x), x) \]

Here, \(J(x)\) is the cost-to-come (the total path cost from the start node to \(x\) along the tree), and \(c(a,b)\) is the edge cost from node \(a\) to node \(b\). The total path cost is the sum of all edge costs along the branch.

To choose the parent of \(x_{new}\), RRT* compares each candidate \(x_i\) (any node already in the tree that is close enough to \(x_{new}\)):

\[ J(x_i) + \text{dist}(x_i, x_{new}) \]

Numeric example. Suppose candidate node \(A\) already has cost-to-come \(J(A) = 5\), and the edge from \(A\) to the new position is \(\text{dist}(A, x_{new}) = 3\). Then the cost of reaching \(x_{new}\) through \(A\) is \(5 + 3 = 8\). If another candidate \(B\) gives \(J(B) + \text{dist}(B, x_{new}) = 2 + 4 = 6\), RRT* prefers \(B\) as the parent, even if \(A\) is geometrically closer to \(x_{new}\).

To decide whether to rewire a nearby node \(x_{near}\), RRT* checks:

\[ J(x_{new}) + \text{dist}(x_{new}, x_{near}) < J(x_{near}) \]

If the inequality is true and the edge is collision-free, the nearby node gets rewired through \(x_{new}\).

How to Choose the Near-Node Update Radius

RRT* only compares nodes inside a local neighborhood. A common theoretical radius is:

\[ r_n = \min\left\{\eta,\ \gamma\left(\frac{\log n}{n}\right)^{1/d}\right\} \]

where:

\(n\): number of nodes currently in the tree.
\(d\): planning space dimension.
\(\gamma\): scaling constant (controls neighbor count).
\(\eta\): upper bound radius (often tied to steering step size).

This radius is larger early in planning, when the tree is sparse. Later, as the tree becomes dense, the radius shrinks and rewiring becomes more local.

Interactive Sandbox: RRT* Parent Selection and Rewiring

Before reading the pseudocode, play with the sandbox below. It lets you move a new sample node and watch the RRT* parent-selection and rewiring logic update live — the same two checks (choose cheapest parent, then rewire neighbors) that the loop below will spell out in code.

Setting: a robot already has a rough RRT tree rooted at start S. Click in the canvas to place a new sample node X, then drag X around. RRT* will choose the cheapest parent for X and show which nearby nodes would rewire through X.

Tree edge Chosen parent Successful rewire Checked, no improvement

Move the new sample X

Click empty canvas to place X. Drag X to see parent selection and rewiring update live.

Near radius r 120 px

Chose parent: none

Rewired: none

Tree cost (local repair)

Without rewire:-

With rewire:-

Saved by rewiring:-

Here is the entire rewiring process:

RRT* Loop

In code, the main loop looks like this:

for k in range(max_iterations):
    x_rand = sample_free()
    x_nearest = nearest(tree, x_rand)
    x_new = steer(x_nearest, x_rand, step_size)

    if collision_free(x_nearest, x_new):
        n = len(tree.nodes)
        r_n = min(eta, gamma * (log(n) / n) ** (1.0 / d))
        X_near = near(tree, x_new, r_n)

        # 1) Choose parent for x_new
        x_parent = argmin_over_collision_free(
            J(x) + dist(x, x_new) for x in X_near
        )
        add_node(tree, x_new, parent=x_parent)

        # 2) Rewire neighbors through x_new
        for x_near in X_near:
            if collision_free(x_new, x_near):
                if J(x_new) + dist(x_new, x_near) < J(x_near):
                    change_parent(x_near, x_new)

Rewire Intuition Example

Assume node 9 is newly added and the yellow circle is its neighborhood. RRT* first computes the best parent for node 9, then checks if 4, 5, or 8 become cheaper when connected through 9.

RRT star rewiring neighborhood example — RRT* local neighborhood: choose best parent for node 9, then rewire neighbors if cost improves.

Quick Check Quiz 1 (Best Parent Selection)

Use the numbered figure directly above this quiz. Assume all dashed candidate edges are collision-free and edge labels are edge costs.

Which node should be selected as the parent of node 9?

A

Node 5

B

Node 6

C

Node 8

D

Node 4

Compare total cost-to-come to node 9 through each candidate:

Through 6: \(0 \rightarrow 2 \rightarrow 6 \rightarrow 9 = 3 + 5 + 3 = 11\)
Through 8: \(0 \rightarrow 2 \rightarrow 6 \rightarrow 8 \rightarrow 9 = 3 + 5 + 1 + 3 = 12\)
Through 5: \(0 \rightarrow 5 \rightarrow 9 = 19 + 4 = 23\)
Through 4: \(0 \rightarrow 5 \rightarrow 4 \rightarrow 9 = 19 + 5 + 1 = 25\)

Minimum cost is through node 6.

Quick Check Quiz 2 (Rewire Decision)

After selecting node 6 as parent of node 9, RRT* checks whether each nearby node gets a cheaper cost-from-start by switching its parent to node 9. Which neighbors should switch their parent to 9?

Which option is correct?

A

Only node 8 switches its parent to 9.

B

Nodes 4 and 5 switch their parent to 9.

C

Nodes 4, 5, and 8 all switch their parent to 9.

D

No node switches its parent.

From Quiz 1, \(J(9)=11\). Now compare each nearby node's current cost with the cost through 9:

Node 4 current: \(24\). Through 9: \(11 + 1 = 12\) -> improve, so rewire.
Node 5 current: \(19\). Through 9: \(11 + 4 = 15\) -> improve, so rewire.
Node 8 current: \(9\). Through 9: \(11 + 3 = 14\) -> no improvement, keep current parent.

So only nodes 4 and 5 are rewired.