Simulator in action.This was my first major contribution to MIT's first DARPA Grand Challenge team. We had a few expensive LIDAR sensors sitting around, but no testing platform. I decided to quickly write a simulator so that we could test road-following algorithms "on the bench" and get results more quickly. My first iteration was 2d and written in QBASIC. It was basically a ray-tracer with a 2d display. It was a nice proof of concept, but QBASIC's memory limitations made this highly impractical. I'm still looking for a screenshot of this program. I still think it's pretty awesome.
The next version was written in C. This, in fact, was the first program I had ever written in C. The code is poorly formatted and not documented very well, but it worked surprisingly well. I used SDL and OpenGL to do the display, and used an image in .dat format to import terrain data.
Wireframe mode.A LIDAR is a time-of-flight distance sensor. It sends out a laser pulse, waits for the reflection, and processes the data for transmission to a computer for further analysis. My model consists of an XYZ coordinate system with a "robot" able to move in the XY plane, but fixed in the Z plane. The robot has a height, position, pose, and sensor angle, which will be called rZ, (rX, rY), theta, and phi. The LIDAR has parameters such as sweep angle, sweep step, and maximum beam distance. The ray-tracer works by iterating over each LIDAR "beam" until the height of the LIDAR is less than or equal to the height of the ground. The results are stored in an array in polar coordinates.
Road following.The range data is pretty powerful. I wrote an extremely simple road-following algorithm that seems to work very well. It just looks for angle corresponding to the lowest spot in front of the robot, and applies a weighted correction to the robot's pose (theta) so it steers along valleys in the terrain map. The lowest spot is determined by looking applying a low-pass filter to the range data and finding the maximum value. It's fun to watch! The simulator looks like some alien planet video game when it's running in realtime (about 30FPS). I add a velocity term in the direction of the robot's pose, so it really does look like it's "driving" along the road. The screenshots don't do it justice, so you'll have to take my word for it. It's too bad our team didn't get far enough to test my algorithms in the real world. Maybe next time...
The code is available for download below
To top of page