November 9, 2015

Explaining the Hybrid A Star pathfinding algorithm for selfdriving cars

9 comments
Let's say you are standing somewhere in a room and would like to find the shortest path to a goal. You can see a few obstacles, such as a table, that you would like to avoid. The easiest way to solve the problem (if you are a computer) is to divide the room into squares and then use the common A* (A Star) search algorithm to find the shortest path. But what if you are a car and can't turn around like a human. Then you have a problem! Well, at least until you learn the Hybrid A Star search algorithm. With that algorithm you will be able to find a drivable path to the goal.

The reason I wanted to learn the Hybrid A* algorithm was that I took a class in self-driving cars, where the teacher showed a really cool video with a car that drove around in a maze until it found the goal:


The problem was that the teacher didn't explain the Hybrid A* algorithm - he just explained the normal A* algorithm, so I had to figure it out on my own. A few days later, I had found the pieces I needed and could build it. Because no-one else had really explained the algorithm, I decided to write this short summary.

Before you begin, you really have to learn the normal A* algorithm, so if you don't know how you should do that, I suggest that you take the same class as I took: Artificial Intelligence for Robotics - Programming a Robotic Car. It's free to take it, so don't worry. You should also read the sources I used to figure out the algorithm, mainly the following reports:

While those reports above will explain the more complicated algorithm that will be a little faster and maybe produce a better result, I will explain the very basic idea so you can get started. And when you can understand the basic idea behind the Hybrid A* search algorithm you can always improve it.

First of all you will need to be able to build a simulated vehicle that you can drive with the help of math. These types of vehicles are called skeleton vehicles and you can learn how to build one by watching this video from the class I took (I had to replace the sin with cos and vice versa when I converted the math to from Python to Unity, so you might have to experiment a little):


Next step is to watch this video where the teacher Sebastian Thrun is explaining the basic idea behind the Hybrid A Star algorithm. The video is actually from the basic course in Artifical Intelligence, which is funny because he didn't explain it in the more advanced course.


The problem with the video is that Sebastian Thrun is just drawing one line from the first square, even though he should have drawn several lines (one for each turning angle). According to the reports from above, the resolution is 5 degrees, so I believe you should simulate a skeleton car with steering angles that are going from the lowest possible steering angle to the largest possible steering angle (so -30, -25. -20, ...). When I did that I thought it took too long time, so I'm just simulating three angles: [-35, 0, 35] (but converted to radians). 

The driving distance (d) is the same as the width of one square. But I realized that the driving distance should be a little longer, such as 1.05 m if your square is 1 m if you want to get a better result. But you have to experiment a little.

When you have expanded the first node by simulating all these three angles, you should close the squares they have arrived to, so another path from another node can't arrive to the same square (as he explains in the video). But it is important that you are closing the squares after you have simulated all possible angles from one node, not just after one simulated car arrives to the square, because we want to save all possible new nodes for later, so we can find the one with the lowest cost.

As you add these new nodes, you also have to calculate the cost and heuristic. The self-driving car Junior used a more complicated heuristic, but I realized that you can use the traditional Euclidean distance as the heuristic before you begin calculating something more advanced.

You also need a few extra costs. According to the reports, you should add an extra cost to those nodes that change steering angle compared with the previous node, so you have to save the steering angle in each node. You should also add an extra cost if the node is close to an obstacle, or if you are reversing. By the way, reversing is easy if you are using the same skeleton car as explained above. If that is the case, just use a negative distance to reverse. 

That's it! If you add the ideas from above you will be able to make something that looks like this:


Notice from the image that the car prefers to drive forward (because of the extra cost of reversing). It also prefers to use the same steering angle as in the previous node because of the extra cost of changing steering angle.

Update! I finished implementing the Hybrid A Star algorithm. The car can now find a path from the bottom of the map to the top of the map in less than 1 second.


The biggest problem I had from a performance point of view was the search among the open nodes for the node with lowest cost f. After a few attempts with different algorithms I realized that a heap was the fastest algorithm.
I also realized that using two lists (or arrays) for the closed nodes produced the best result. One list should contain nodes where the car is driving forward and the other should contain nodes where the car is reversing. So the car can reverse into a node where it was previously driving forward. This will result in more expanded nodes and thus slower performance, but I think it produces a better result.

This is how the basic algorithm looks like:


Test it and download the source code
If you want to test the latest version of the algorithm and download the source code, you can find it here: Self-driving car