Introduction to RRT*

Like RRT, RRT* is a sampling-based algorithm that finds a path from a start to a goal configuration. However, RRT* adds two key features that allow it to find increasingly better paths as it runs for longer:

1. "Choose Parent" (Finding the Best Connection): When a new node is created, instead of just connecting it to the single nearest node in the tree, RRT* looks at a neighborhood of nodes around it. It chooses the neighbor that can act as its parent while resulting in the shortest possible path from the start node.
2. "Rewiring" the Tree: After adding the new node, the algorithm checks its neighbors again. If reaching any of those neighbors through the new node creates a shorter path for them, the algorithm "rewires" the tree by updating their parent to be the new node.
   These two steps ensure that the tree is always optimizing its connections, leading to an asymptotically optimal solution.

Core Data Structures

The data structures are slightly different from a basic RRT.

The Node

The Node for RRT* needs to store one additional piece of information: its cost.

• position: The coordinates of the point (e.g., [x, y]).
• parent: A reference to its parent Node. The root (start) node has a parent of null.
• cost: The total path cost (usually Euclidean distance) from the start_node to this node. The start_node has a cost of 0.

// Pseudocode for an RRT* Node class
class Node {
  constructor(position) {
    this.position = position; // e.g., [x, y]
    this.parent = null;       // Reference to the parent Node object
    this.cost = 0.0;          // Cost from the start node
  }
}

The Tree

Unlike RRT-Connect, RRT* uses a single tree that grows from the start_node. You can represent it as a list or array of Node objects.

• tree = [start_node]

The Main Algorithm

Here is the high-level logic of the RRT* algorithm.

Initialization

1. Define your start_position and goal_position.
2. Create the start_node, set its cost to 0, and initialize the tree with it.
3. Set a maximum number of iterations.

The Main Loop

// Pseudocode for the main loop
for i in 0 to max_iterations:
    // 1. Get a random point
    q_rand = generate_random_point()

    // 2. Find the nearest node in the tree to this random point
    q_near = find_nearest_node(tree, q_rand)

    // 3. Steer from q_near towards q_rand to get a new point, q_new
    q_new_pos = steer(q_near.position, q_rand, step_size)

    // 4. If the path to q_new is collision-free, proceed
    if is_collision_free(q_near.position, q_new_pos):
        // This is the core of RRT*
        // Create a temporary node for q_new to work with
        q_new_node = new Node(q_new_pos)

        // a. Find the best parent for q_new in its neighborhood
        choose_parent(q_new_node, q_near, tree, search_radius)

        // b. Add the new node to the tree
        tree.add(q_new_node)

        // c. Rewire the tree to account for the new node
        rewire_tree(q_new_node, tree, search_radius)

// After the loop, find the node in the tree closest to the goal and reconstruct the path
goal_node = find_node_near_goal(tree, goal_position)
path = reconstruct_path(goal_node)
return path

Core Functions

These functions contain the unique logic of RRT*.

choose_parent(q_new_node, q_near, tree, radius)

This function finds the best node in a local neighborhood to be the parent of q_new_node.

1. Initialize Best Parent: Initially, assume the best parent is q_near. Set q_new_node.parent to q_near and calculate its initial cost: q_new_node.cost = q_near.cost + distance(q_near.position, q_new_node.position).
2. Find Neighbors: Find all nodes in the tree that are within a certain radius of q_new_node.position. Let's call this list neighbors.
3. Iterate Through Neighbors: For each neighbor in neighbors:
   • Calculate the potential cost to reach q_new_node from this neighbor: potential_cost = neighbor.cost + distance(neighbor.position, q_new_node.position).
   • Check for Improvement: If this potential_cost is less than q_new_node.cost AND the path from neighbor.position to q_new_node.position is collision-free:
     • This neighbor is a better parent! Update q_new_node.parent to be this neighbor.
     • Update q_new_node.cost to the new, lower potential_cost.

rewire_tree(q_new_node, tree, radius)

After finding the best parent for q_new_node and adding it to the tree, this function checks if q_new_node can serve as a better parent for any of its neighbors.

1. Find Neighbors: Find all nodes in the tree within the radius of q_new_node.position.
2. Iterate Through Neighbors: For each neighbor in this list:
   • Important: Do not consider q_new_node's own parent in this check.
   • Calculate the potential cost to reach the neighbor by going through q_new_node: potential_cost = q_new_node.cost + distance(q_new_node.position, neighbor.position).
   • Check for Improvement: If this potential_cost is less than the neighbor's current cost AND the path from q_new_node.position to neighbor.position is collision-free:
     • We found a shortcut! "Rewire" the tree by setting neighbor.parent to q_new_node.
     • Update the neighbor's cost to this new, lower potential_cost.
     • Propagate Cost Update: This cost change must be propagated to all descendants of the rewired neighbor. You would need another function to recursively update the costs of its children.

Helper Functions & Path Reconstruction

Many helpers are the same as in RRT, but some are new or modified.

• generate_random_point(): Same as before. Returns a random point, occasionally biased towards the goal.
• find_nearest_node(tree, point): Same as before. Finds the node in the tree closest to point.
• steer(...): Same as before. Takes a small step from one point towards another.
• is_collision_free(pos1, pos2): Same as before. Checks for obstacles on the line segment between two points.
• find_neighbors(tree, node, radius) (New): Iterates through the tree and returns a list of all nodes whose position is within radius of the given node.position. The search radius is a critical parameter for RRT*. A common formula is radius = min(gamma * (log(n)/n)^(1/d), eta), where n is the number of nodes in the tree, d is the dimension of the space, and gamma and eta are tuning constants.
• reconstruct_path(final_node): This is simpler than in RRT-Connect.
  1. Create an empty path.
  2. Start with current_node = final_node.
  3. While current_node is not null:
     • Add current_node.position to the front of the path.
     • Set current_node = current_node.parent.
  4. Return the path.
     This breakdown provides the full logic required to implement RRT*. The most complex parts are the choose_parent and rewire_tree functions, which are the core of what makes RRT* "optimal."