November 9, 2015

Explaining the Hybrid A Star pathfinding algorithm for selfdriving cars

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

9 comments:

  1. Great work with Unity. I was looking for this explanation. Thanks.

    ReplyDelete
  2. Why don't you please post the source code for the hydrid A* algorithm, like you do for your other tutorials.

    ReplyDelete
    Replies
    1. Because there's only 24 hours a day and the code is a bit messy, but you can send me an email and I will reply with the project

      Delete
  3. Can you please comment on the grid size/ work space dimensions you are considering ?

    ReplyDelete
  4. Can you explain how to use PID controller to follow that path?
    Thanks alot

    ReplyDelete
  5. First of all thanks for your tutorials/ I find very usefull boat tutorial/ but find that the form of collider play big role in simulation of boat. And waves generated with collider don't
    interacted with underwater mesh.
    If its not difficult for please sent source of pathfind in Unity.
    geotyper@gmail.com
    Thanks
    Alexander

    ReplyDelete
    Replies
    1. sorry waves generated with shader not collider

      Delete
  6. Hi,
    I am also implementing this algorithm. In case you want to check out: https://github.com/trgtylcnky/global_planner_turgut

    The problem I am facing of is getting to the final. In your simulation, I see a path consists of two arc and one line is calculated. I cannot calculate this unfortunately (I am surprised of my lack of geometry skills). Can you give me a formula or any other help of calculating that?

    Thank you.

    ReplyDelete
    Replies
    1. The path is called a Reed-Shepp path. It's a generalization of the Dubins path. Implementing it is not hard but it would probably take a lot of time. It's easier/faster if you go with Dubins path.

      Delete