Hardcore pathfinding for self-driving cars

What if you have a car and want to find the shortest route from point A to point B. A normal car can't turn around its own axis 360 degrees like a tank can, so you have to come up with another solution that takes the wheels into account. One solution is called Hybrid A star, which is a modification of the classic A star algorithm. It looks like this:

The yellow lines in the image above are the possible paths if the car is driving forward and the blue lines are the possible paths if the car is reversing. If you want more details about the Hybrid A star pathfinding algorithm, I've written a more detailed description here: Explaining the Hybrid A Star pathfinding algorithm for selfdriving cars. But the problem with Hybrid A star is that it doesn't care about the final orientation of the car. So if you want to make sure that your car is ending up at a position with a certain heading, then you need to modify Hybrid A star. 

There are two common ways to modify the algorithm so you end up at a position with a certain heading: Dubins paths and Reeds-Shepp paths. Both paths types are very similar and the basic idea is that you get the shortest path by trying different combinations of circles and straight lines between circles. 

The main difference between Dubins paths and Reeds-Shepp paths is that a car following a Dubins path can just drive forward. The final 6 paths looks like this:

Depending on the position of the cars, you may end up with fewer than 6 Dubins paths. It is possible to modify these 6 Dubins paths so the car can also reverse, which is just a matter of mirroring the calculations. You will then end up with something like this, where the white line is forward and the gray line is reverse:

You can actually add these 12 paths to Hybrid A star and get a solution where Hybrid A star will (sometimes) end up at the desired position and heading. So when you expand the search tree you can test if a fixed path solution is available by testing if the car can drive the entire path and not collide with an obstacle. It will be way too slow to test it when you expand each branch so make sure to test it every x branch and test it more often when you are closer to the goal. It will not always work because an obstacle may be in the way, but it's a better solution than the solution we had before. If you want to learn how to make Dubins paths, I've written a tutorial on how to do it: How to make Dubins paths in Unity with C# code.

If you want to be really hardcore, you can modify the Dubins paths and end up with the Reeds-Shepp paths. Instead of just 12 paths (or 6 if you just use the standard Dubins paths) you will now have no less than 36 paths to calculate. The good thing is that if you have already calculated the 6 Dubins paths, you will know how to calculate the first 20 paths. And I argue that knowing these 20 paths may be enough, and adding more paths will just take longer time and speed of the algorithm is here more important than necessarily finding the shortest path. One of these paths is looking like this:

If you want to test the final self-driving car, you can test it here: A self-driving car finding its way through a maze.