Thursday, January 16, 2014

Autonomous Vehicles: Basic Algorithms

Last June, I stopped by the Boulder Reservoir to watch the 2013 Sparkfun Autonomous Vehicle Competition (AVC). There were two races going on, an air race and a ground race. The basic idea is that teams design some kind of vehicle to compete in either race that is capable of navigating the course autonomously. At the start of the race, the teams are allowed to do something simple like pressing a "go" button on their vehicle, but the rest of the race must be completed without human intervention. Scores are based on time to complete the course, as well as additional points for special actions such as avoiding obstacles or hitting small targets.

Chasing your robot, laptop in hand. A common sight that day.

The air race was actually not as exciting as I had anticipated. There were a couple custom flying rigs that gave a good show, but due to the recent popularity of GPS-enabled quadcopters, there were a few cookie-cutter entries. I'm not saying it's easy to get a quadcopter to fly in a specific pattern (I spent months tuning my own hexacopter before going insane getting tired of it), but the availability of advanced pre-made flight controllers for quadcopters reduced some of the excitement of the event. It's a relatively new event, so I'm hoping next year will see some changes.

Ready to fly over the water.

The ground race, however, was hilarious. I've never seen so many home-made robots try so hard to go around a parking lot at top speed while avoiding fences, barrels, and each other. The sheer variety of vehicles running the course offered some interesting scenes. The amount of experience and money needed to make each vehicle was fairly apparent, from a tiny LEGO Mindstorms car with pre-programmed directions to a sponsored RC car equipped with RTK GPS (super-accurate GPS). In the middle were groups of students trying to do their best with limited budgets, hacking apart RC cars and adding Arduino controllers for brains and ultrasonic sensors to avoid obstacles. Most groups seemed capable of building a sturdy ground vehicle to run the course, but that was only the first step.

Needs more LEGO people.

How do you get a robot to win a race autonomously? Easy! You have to write up some code that the robot brain runs during the race that allows it to continually make progress despite obstacles. But how does the robot know what "progress" is? And how does it know what an "obstacle" is? What if it runs into a wall? What if it gets turned around? All of these questions must be considered when writing an algorithm for a race like this.

I'm interested in building a robot to compete in the next AVC, but before I commit any real resources, I figured I should see how hard it is to write an algorithm for an autonomous robot. Without a physical vehicle to test the algorithm on, I need a simulation that will run my code and give a decent guess as to how well it performs. This way I can start with a simple algorithm and work my way towards more advanced solutions while seeing the effect each addition has on the race outcome.

To make the implementation of the algorithm as realistic as possible, I went for a simulation environment that allows the use of pseudo-Arduino code. Each algorithm is written as if it were running on a generic Arduino controller with a setup() and loop() structure:

The simulation initializes a robot at the starting position and runs the setup() method. At every timestep, the simulation moves the robot around the course based on the values for the motor speeds. It checks for collisions with the course boundaries and obstacles to make sure the robot stays on track. Throughout the course of my simulations, I'll add various physical effects that may change the behaviour of the robot. The loop() method is run as often as possible, and when the robot calls the delay() method, control is given back to the simulation for some number of time steps.

Algorithm 1: Pre-Programmed Route
The first method of autonomous racing I wanted to try was where you tell the robot how far to go in each direction and just let it run. Assuming the robot starts facing the correct way, all you need to do is move forward and turn left a few times. How hard could that be?

I've drawn black lines showing the inner boundary of the course. It's not exactly the Sparkfun AVC course, but it's close enough for now. The outer boundary is at 0 and 75 in each direction. I say this, but it's not like any of the boundaries matter for this algorithm. It's perfect. You tell it where to drive and it goes there. The only way this method can fail is if you give it the wrong directions, right?

In a perfect world, this would be true. But a few days ago I stepped in a puddle of ice water thinking it was frozen solid, so I know for a fact that this is not a perfect world. In the real world, you tell a robot to drive straight and it will drive mostly straight. There will be small deviations from the intended path due to motor imperfections, wheel imperfections, ground imperfections, etc. Imagine the robot driving over a rough patch of concrete: one wheel might lose traction a bit, turning the robot by a few degrees. To model these issues in my simulation, I made it so that every now and then, the heading of the robot is altered slightly in a random direction. Then, instead of testing the robot just once, I ran thousands of simulations to see how often the robot would still succeed.

Success=100%, Avg Time=149s

Here, I'm showing a logarithmic heatmap of the paths taken by 10,000 simulated robots. The warmer the color, the more often the robots went there. With a small amount of continuous deflection added, there is a bit of spread in where the robots end up, but 100% of them still make it to the finish line. What if our robot is not so robust and the deflection is larger?

Success=45%, Avg Time=147s

Much better. Less than half of our pre-programmed robots made it to the end. Of course, the real AVC course has a few obstacles, so what happens when we place some barrels on the first stretch?

Success=29%, Avg Time=147s

Even though the barrels are placed away from the pre-determined path, a significant fraction of the robots were deflected into them. The barrels create neat shadows in the data where no robots ended up travelling. Another real-world effect that had a profound impact on some of the robots I saw in the 2013 AVC was collisions with other robots. I've modelled this as an event that happens randomly, but more likely near the start of the race. When it happens, the robot is spun around in a random direction and moved by a few centimeters. This is somewhat optimistic, since a few robots in the AVC broke apart on impact with a competitor.

Success=13%, Avg Time=147s

I could keep adding more real-world effects that might lower the success rate of this first algorithm, but I think you get the point. Anything could go wrong and completely throw off the pre-determined path. It's time for a better algorithm.

Algorithm  2: Random Walk
The first algorithm was too strict. It didn't allow for any freedom, and subsequently did poorly. It's time to give our robot a mind of its own and let it soar. In this algorithm, the robot drives forward for a few seconds, then proceeds to change it's speed and direction randomly every now and then. It's perfectly adaptable to the course in that it doesn't care about anything. Even winning.

Success=0%, Avg Time=N/A

Well that didn't go well. None of the 10,000 robots I sent out ever made it to the finish line. They all either got stuck on walls or ran over their 5 minute time limit. On the plus side, they didn't seem to be concerned with the barrels. Based on these results, I estimate that around one in a billion might make it. So there's a small chance that a robot set to move completely randomly could complete a course like this. As fond as I am of this algorithm, it's probably time to try something a little more intelligent.

Algorithm 3: Bumper Cars
Let's try a classic algorithm. Let the robot drive in a straight line until it hits something. The hit can be detected a few ways in real life, either with a bumper sensor or even an ultrasonic proximity sensor. When the robot detects an obstacle, it turns itself around to point in a random new direction and continues from there. It repeats this process until it either wins or dies.

Success=11%, Avg Time=445s

This time, we get some robots making it to the finish line. Still, this algorithm acts pretty oblivious when it comes to taking corners the right way. How can we inform our robot as to which way is the correct way to go?

Algorithm 4: GPS Positioning
So far the algorithms presented have been pretty dumb. They don't know anything about the course and they just guess which way is the right way to go. Of course, we can't expect much else since we haven't enabled the robots with any kind of sensors except one that sends an alert when an obstacle is hit. It's time to allow the robot to sense the environment a little.

GPS is a common method of giving robots the ability to know where they are outside. If we know the position of the course we want to race around, then we can make the robot know how far it is from various parts of the course. For this algorithm, the robot has a set of way-points (one near each corner of the map) that it will head towards. When it gets close enough to the first one, it targets the next one. If the robot hits an obstacle, it backs up and tries again.

Success=100%, Avg Time=154s

And now we see why all those entries from 2013 had GPS on them. With the ability to know exactly where it is at any point in time, the robot is perfectly capable of finding its way around the course. But is this realistic? The GPS system is neither exact nor instantaneous, so what we have so far is a gross overestimation of the capabilities of this algorithm. I've seen claims of <1 meter accuracy with a standard GPS unit, but I'll put a random noise of 2 meters on the GPS position to be safe. The GPS heading isn't truly measured, but instead just estimated from comparing the current position to the previous one. Finally, I'll set the update rate to 1 Hz, a typical value for cheap GPS units.

Success=99%, Avg Time=275s

With more realistic values for the GPS unit, the results are more believable. The success rate is still very high, but the amount of time the robots typically take has almost doubled. With less accurate measurements of the current position and heading, the robots tend to bounce around the course a little more and spend more time going around.

Other Methods
So far I've only discussed a few simple algorithms for autonomously navigating a known course. While I've tried to add in a few realistic effects like obstacles, continuous deflections, and collisions with competing robots, there is an endless list of physical effects I could try out to see if they are important. The model I've created for the robots is also very simple with two motors, a method of detecting collisions (but no information on direction), and a GPS module.

At some point I'd like to continue this simulation project to include more physical effects of driving a robot around a course and more ways for the robots to sense their surroundings. Some of the sensors I'd like to model are an optical flow sensor, an IMU, and possibly a stereoscopic vision sensor. Eventually, I'd also like to actually build a simple wheeled vehicle and try my hand at implementing one of my algorithms on it.