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.

If you want to see the algorithm in action, you can test it in my self-driving car simulator.

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.  

October 13, 2015

Forest Fire Simulator Update - improved trees and fire

A few week ago I finished the first version of a forest fire simulator in Unity. I used the real physics equations, which worked perfectly fine, but what didn't work fine was the performance of the simulation. The physics equations were not the problem - the problems were the number of trees and the fire and smoke. I even had to add a special button so the user could remove the smoke and flames because everything was so slow. This is how it looked like:

You can see in the image above that the simulation is running at 12 frames per second, which is not that good! To improve that number I first decided to improve the trees. The problem was that the trees were all individual objects, and to improve performance you have to combine them into fewer objects. But I also have to remove trees that have burned down and add trees that are more darker. After a few experiments, I realized that the fastest way was to once in the beginning combine all trees of each tree type, and then cheat by moving trees that are not to be seen to positions the user can't see. It looks like this behind the scenes:

You can see that all black trees are hidden below the ground. And when one of those trees are supposed to be visible, I just move the vertices to above the ground. This is super fast, but tricky to figure out how to do it and no one else had really discussed it when I made a Google search for it. So I decided to make my own tutorial on how to do it. You can find it here: Dynamic Mesh Combining. It took a while to write it, but I'm a big believer in the idea that you should share your knowledge, and if no one else has done so, then people will find you through Google and you might get links and sell more of your products! Moreover, if you do good you will get comments like this on Reddit, which is good for the self esteem:

Anyway, the last problem was the fire and smoke. I used to have one fire and one smoke particle in each square. But when the entire forest is on fire, this will be really slow. A better way is to try to have fewer, but larger fire and smoke particles. So I wrote an algorithm that searches through the map and tries to build larger squares, up to 12 squares by 12 squares. The result is this:

You can clearly see that the entire forest is on fire, but the simulation is running at a fantastic 30 frames per second thanks to the improved trees and fire. I also think the smoke is looking more realistic, so that's a good side-effect: faster and better. 

If you want to test the forest fire simulator you can test it here: Forest Fire Simulator

September 28, 2015

Improving Unity's physics engine PhysX to achieve higher accuracy

Unity is a popular game engine with a built-in physics engine called PhysX. Like all other physics engines, PhysX uses numerical integration techniques to simulate real world physics. Force and movement calculations can get pretty complicated, so in most cases it will be impossible to calculate the exact movements. This is the reason why the physics engine uses numerical integration techniques to approximately integrate the equations of movements. 

The problem with several of the available numerical integration techniques is that the result in not always 100 percent accurate, because the game also has to run fast on your computer. Most game engines prefer less accuracy, but more faster games because the player will not notice the difference anyway. But what if you are going to make a game that requires more accurate movements, like a sniper rifle. 

Low accuracy is a problem I had a few days ago when I simulated bullet trajectories. First I calculated the angle to hit the target, then I fired a bullet with Unity's physics engine, and then I noticed that the bullet didn't hit the target. First I thought I had made an error in the calculation of the angle, but then I realized that the reason the bullet didn't hit the target was because of limitations of the physics engine.

So which integration method is PhysX using? The answer (according to my research on the Internet) is that no one really knows for sure. So let's make an experiment to find out! The first integration method I used was Euler Forward, and you can see the result here:

You can see that the trajectory line when using Euler Forward is overshooting the target and is not following the same path as the bullet, which is using PhysX. So let's try Backward Euler:

You can see that the trajectory line undershoots the target, but follows the same path as the bullet would have (if you compare with the bullet in the image that uses the Euler Forward to calculate the trajectory line). So there's a chance that PhysX is using Backward Euler to calculate all physics. But we are still not hitting the target. So let's try Heun's Method:

You can see that we now hit the target with the trajectory line. But to improve the bullet trajectories, you have to write your own physics engine so that the bullets are also using Heun's Method instead of Backward Euler (or whatever method PhysX is actually using). If you would like to learn how, I've written a tutorial: How to make realistic bullets in Unity.

September 22, 2015

Random Show Episode 29

A new episode of the Random Show with Kevin Rose (founder of Digg) and Tim Ferriss (author of The 4-Hour Workweek) is out! This is episode 29.

Lessons learned
  • Kevin Rose's new watch-blog-company, Hodinkee, has a "small" but engaged audience. They had 1.3 million user sessions in a month from users that have opened the app more than 200 times. By the way, the name is not Swedish for small watch, at least I've never heard of it and I've been speaking Swedish for more than thirty years. According to Google translate, the word "hodinke" is "watch" in Czech/Slovak. 

  • Kevin Rose recommended the book The Okinawa Program, even though he also said some parts of the book were rubbish, and he also recommended eating Germinated brown rice with seaweeds, sesame, and eggs. 
  • It was difficult to hear, but I think Tim Ferriss recommended The Age of Miracles Animal Rescue if you want to adopt a dog.
  • Tim Ferriss recommended the podcast player app Overcast and playing Tetris. Why Tetris you might ask? The reason is that he interviewed Jane McGonigal who argues that playing Tetris a short time after a traumatic event will minimize the risk of post-traumatic stress. He also recommended the book Anything you want

Recommendations If you want to watch the rest of the episodes, you can find them here: The Random Show with Kevin Rose and Tim Ferriss.

September 13, 2015

Simulation of a forest fire in Unity

This summer I decided to learn more about rockets and found an online course called Differential Equations in Action by Udacity. The first and second lessons in that course teaches you how to bring back the unfortunate Apollo 13 space ship from space to Earth. You will also learn about ABS brakes and how many people that how to be vaccinated to stop an outbreak of an epidemic. 
But sixth lesson is all about forest fires, and you will learn how to simulate a forest fire in Python. The differential equations include change in temperature from:
  • Heat diffusion
  • Heat loss
  • Wind speed
  • Combustion of wood      
I thought the simulation of a forest fire was really interesting, but it was boring to simulate it in Python. Wouldn't it be more interesting to see the forest burn in real time? Yes it would! So I decided to make a forest fire in Unity
It was really easy to translate the code from Python to Unity and make a real time simulation. The code in Python used a method called Euler forward to solve the differential equations, and Euler forward is working really well in Unity. This is the result:

The temperature at the start of the fire is about 700°C and the surrounding temperature is set to 37°C.

After about 2 minutes the fire has spread towards north-west, because the wind speed is north-west. The core temperature is now about 2500°C and the fire covers an area of  50*50 meters.

After 20 minutes the fire covers the entire area it can cover in the simulation. Notice that the forest that's not north-west of the fire has not ignited. The temperature at the edge is still between 100°C and 200°C, so you don't want to be there, but the wood has not ignited.

After 1 hour, 20 percent of the wood has gone up in flames. The trees change shape after a certain amount of wood has burned up. The core temperature is now 4000°C. 

Still smoking after 2 hours, but the core temperature has gone down to 2500°C, so the fire is dying.

After 5 hours, 70 percent of the wood has gone up in flames. The core temperature has gone down to 500°C.

The outer part of the forest fire that's not in the wind-direction has now stopped burning.

After 6 hours and 12 minutes, the last part of the forest has finally stopped burning. 

...or if you are more interested in a video (not the same fire as in the images)

Looks interesting? You can test it here: Fore Fire Simulator