RRT* Motion Planning with Vehicle Kinematic Constraints

The problem

Sampling-based planners such as RRT* handle complex, continuous spaces where grid methods struggle, and with vehicle steering functions they produce drivable paths. However, RRT* converges slowly in narrow passages and its results vary from run to run, so its practical use needs informed sampling, good steering functions and statistical evaluation.

Objectives

• Implement RRT and RRT* in SE(2) with collision checking for a rectangular vehicle.
• Use Dubins and Reeds-Shepp curves as steering functions.
• Add goal biasing and informed sampling to speed convergence.
• Evaluate path cost against time statistically over many runs.

What you need

Key concepts

RRT*

A rapidly exploring random tree that, when adding a node, chooses the lowest-cost parent nearby and rewires neighbours through the new node, so path cost improves as samples grow.

Steering function

Connects two poses with a drivable curve; Dubins curves allow forward motion only, Reeds-Shepp curves allow reversing.

Informed sampling

After a first solution is found, sampling only within the ellipse-like region that can contain a better path, which speeds convergence.

Narrow passage problem

Random samples rarely fall in thin free regions, so planners take a long time to find paths through them.

Step-by-step guide

1. 1

   Define the study

   Specify the vehicle (rectangle with length, width and minimum turning radius), the scenarios (open car park, cluttered lot, narrow gate and parallel parking slot) and the metrics: success within a time limit, path cost (length plus penalty for reversing), time to first solution, cost after fixed time and number of collision checks. Plan to run every configuration at least 30 times with different seeds.

   Checkpoint: A study plan with scenarios, metrics and the statistical design.

2. 2

   Build maps and collision checking

   Create occupancy maps of the scenarios at 0.1 m resolution. Implement collision checking for the rectangular vehicle along a curve by sampling poses at small intervals and checking the footprint, or use several overlapping circles for speed. Validate the checker with known colliding and free poses and measure the time per check. Choose the sampling interval along curves relative to the vehicle's smallest dimension so that thin obstacles cannot be skipped between checks.

   Checkpoint: A collision checker that passes all hand-made test poses with timing recorded.

3. 3

   Implement RRT

   Implement basic RRT in SE(2): sample a random pose, find the nearest tree node with a suitable distance metric, steer towards the sample with a maximum extension length using a Dubins or Reeds-Shepp curve, and add the node if collision-free. Add goal biasing by sampling the goal with a small probability. Plot the tree and first solution on each map.

   Checkpoint: RRT finds a path in the open and cluttered scenarios with tree plots saved.

4. 4

   Extend to RRT*

   Add the choose-parent and rewire steps within a radius that shrinks with the number of nodes, using the steering function's path length as cost. Keep the cost of each node consistent after rewiring by propagating to descendants. Verify that path cost decreases with more iterations on the open map. Cross-check against plannerRRTStar from Navigation Toolbox for one scenario. Also check that no node's cost ever increases after a rewire.

   Checkpoint: A cost-versus-iterations plot that decreases monotonically for RRT*.

5. 5

   Compare steering functions

   Run RRT* with Dubins and Reeds-Shepp steering on all scenarios. For parallel parking and the narrow gate, Dubins will often fail or produce long detours because it cannot reverse. Record success rate, cost and time for each. Visualise representative paths to show the qualitative difference. Note the extra cost of computing Reeds-Shepp connections compared with Dubins, and whether that cost changes the time to first solution in the open scenarios.

   Checkpoint: A table and figures comparing Dubins and Reeds-Shepp results.

6. 6

   Add informed sampling

   Once a solution exists, restrict sampling to poses whose straight-line distance via start and goal is less than the current best cost, an approximation of the informed set for car-like vehicles. Also try sampling biased near obstacles' boundaries for narrow passages. Compare convergence of plain, informed and boundary-biased variants. Run each variant with the same seeds and time limits, and report how often boundary-biased sampling helps in narrow passages but slows convergence in open areas.

   Checkpoint: Convergence curves showing faster cost reduction with informed sampling.

7. 7

   Run the statistical evaluation

   Run every planner variant, steering function and scenario 30 or more times with fixed time limits. Report median and interquartile range of time to first solution and final cost, and the success rate. Use plots of cost over time with shaded spread. Apply a non-parametric test such as Mann-Whitney U when comparing variants. Store every run's raw results so the statistics can be recomputed.

   Checkpoint: Statistical tables and plots with spread for all variants.

8. 8

   Post-process and track

   Shortcut the best path by trying direct steering connections between non-adjacent nodes, then check curvature continuity. Track the result with a pure pursuit or Stanley controller in a kinematic simulation to confirm it is drivable. Record tracking error, especially at direction changes. Compare path cost and curvature before and after shortcutting, and confirm that every shortcut segment still passes the full collision checker with the vehicle footprint at fine sampling.

   Checkpoint: Tracked paths with small errors confirming drivability.

9. 9

   Report

   Present the implementation, scenarios, statistical results and discussion of where RRT* works well and where it struggles. Compare with grid or Hybrid A* if those results exist. Discuss the role of randomness in safety arguments, since a planner that succeeds 95 per cent of the time needs a fallback. Include convergence plots for each scenario and an appendix with all parameter values and seeds so the experiments can be repeated.

   Checkpoint: A thesis chapter with statistically supported conclusions.

Testing & validation

• Success rate within time limits per scenario and variant.
• Median and spread of time to first solution and final cost.
• Convergence curves for plain and informed RRT*.
• Tracking error on post-processed paths.

Expected results

A working RRT* planner with Dubins and Reeds-Shepp steering, informed sampling variants and a statistically sound evaluation of cost, time and success across vehicle-planning scenarios.

Take it further

• Implement bidirectional RRT* or batch-informed trees.
• Add a learned sampling distribution trained on previous solutions.
• Compare against Hybrid A* on the same scenarios.

Viva and review questions

Learn it at DIYguru

These DIYguru courses and programmes teach what this project needs.