- A random point is chosen in free space.
- Find the closest existing point.
- If the two can be connected, then connect.
- Otherwise, move the new point to furthest safe place and connect.
- Test for solution.
- Choose a random existing point in the tree.
- Choose a random control. (e.g. max speed, gentle left, for 1 second)
- If the path is safe, add this to the tree.
- Otherwise add the longest safe path.
- Test for solution
This method can eaily be adapted to any control space, and subtle optimisations can be made such as only allowing a certain granularity of control, and keeping the tree points a certain distance apart.
The great thing about this method is that you know that the path is possible in terms of the controls of your robot. Still, the path is far from optimal, and may well need iterating, and smoothing of the path and acceleration.
By tweaking the control space to allow reverse, it can solve three point turns and reversing out of tight corridoors.
Again, using reverse frequently is not optimal, so it would be best to use to imply some cost to reversing, and choose a less expensive path if available.
One optimisation that seems to work well, is to grow two trees at the same time, one from your current location and another from the destination.
This often succeeds at finding a solution in many fewer iterations and can help avoid the search getting stuck in beetle traps when the search resolution is too low and points near the exit of the trap are overlooked.
The same technique has been used to solve much more complex problems, where brute force is far too expensive to try.