RRT* (Rapidly-exploring Random Trees) is a path-planning algorithm that builds a tree in order to quickly span a large space and find a valid path from a start node to a goal node. The algorithm has four important parts: the TreeNodes, the tree-building, the tree rewiring, and then the path extraction.
We created a custom class for our RRT* implementation called the TreeNode. It was useful to use this class in order to manage the data and properties of nodes in the tree for simplicity within the code. We decided to leave the function for finding neighbors outside of the class; however, we could see a reason for having it be built into the object like the function for finding distances. We wanted to keep the TreeNode functions more specific to just a single TreeNode rather than requiring interaction with the entirety of the tree.
The RRT* tree-building starts with the root TreeNode. This begins a list of TreeNodes representing the tree itself. The root TreeNode has no parent and assumes position and direction from external input. The algorithm also initializes the distance between the start and goal to be infinity - tracking the distance and what TreeNode corresponds to closest distances permits the algorithm to be run in a constrained or unconstrained mode. In constrained mode, the algorithm exits immediately upon reaching the goal - this allows for faster runtime but less optimized trees during rewiring because fewer optimizations can be explored.
For a given iteration, capped at a pre-specified value, the algorithm performs a series of actions:
Rewire the tree is the process of taking a known path and optimizing it to reduce total distance (weight). Starting at the goal and working backwards, each TreeNode along the path is scrutinized. Optimization is done by finding the neighbors within a region around the TreeNode and calculating whether the changing of the parent to that other TreeNode would reduce the weight. This calculation can be purely distance or impacted by weighing in a factor relative to the angle disparity in order to try to coax longer but smoother paths. As TreeNodes change to more optimal parents, the path becomes lower cost. This is a computationally expensive operation and is known to undo some of the path smoothing effects achieved in the Tree-Building; however, the Tree-Building optimizations are still useful because they can coax the overall path around a specific side of an obstacle to better smoothen the path, something that generally cannot be replanned around in the rewiring step. Thereby these optimizations work towards optimizing the path at different scales and do not entirely conflict on all aspects.
Path extraction is the final step in generating a path of waypoints. This simply entails traversing the tree recursively from the goal to the start and converting/saving each TreeNode to a Point32 waypoint.
There are five main parameters that can be used to tune the RRT* algorithm for different use cases. These parameters are separate from the direct modifications that can be made to centers of sampling distributions, choosing to skip rewiring, and other high-level behaviors.
Here are some possible parameter sets for quick-and-dirty vs. slow-and-precise runs determined through experimentation in simulation:
| Parameter | Quick | Slow |
| Tolerance [m] | 0.2 | 0.2 |
| Threshold [m] | 0.45 | 0.45 |
| Neighborhood [m] | 0.5 | 0.5 |
| Step [m] | 0.5 | 0.3 |
| Depth [count] | 2000 | 5000 |
These quick and slow runs are triggered in the code sequentially to get a mixture of System 1 and System 2 thinking. Impulsiveness on demand and thought on the decision horizon afterwards.
For now, the path output represents a low-resolution pathing solution and would benefit from the use of splines in order to smoothen it or a control algorithm that better traverses it in a smooth manner.