Since February 2016, I have been participating in DIYRobocars meetups here in the SF Bay Area, wherein a bunch of amateur roboticists compete in autonomous time trials with RC cars. I had been doing very well in the competition with essentially just a glorified linefollowing robot, but have lately upgraded to a full SLAMlike approach with a planned trajectory.
In this series of posts I will try to braindump everything I’ve learned in the last two years, and explain how my car works.
Here’s a race between my car and @otaviogood’s Carputer, which uses an endtoend behavioral cloning neural network to drive. It had to dodge a lot of other cars in training that day, which we think is why it sort of chokes when my car cuts it off. My car is completely oblivious to other cars; the wheeltowheel race here was just for fun.
In the video above you can see everything the car can see/sense: a birdseye view of the ground, gyroscope, accelerometer, and wheel velocity measurements.
Here’s a more recent video showing the current state of the same car, using a slightly different but still fundamentally linefollowing strategy (instead of following the line on the ground, it’s following a preoptimized racing line):
Linefollowing theory
Imagine you are a line following robot. Your mission is to move forward and keep the line in the middle of your sensors (maybe they’re IR sensors looking at the ground, maybe it’s a camera). The line can go straight or it can turn, and so can you. As far as you’re concerned, the world consists of only you and the line, so we need to think about your position in space relative to the line.
Curvilinear coordinates
A linefollowing robot lives in a curvilinear coordinate system: all measurements of position are relative to a line which has some (probably varying) curvature. Therefore instead of saying the robot has an x, y position and maybe an angle θ, I try to follow the notation I’ve seen in the robotics / automotive control literature.
We assume that the car is traveling along a circle with curvature κ, which is equivalent to 1/radius r, except it can be either positive or negative – in the convention I’m using, if it’s positive, the circle curves to the left and negative curvature goes right (if this seems backwards, think about how angles conventionally run counterclockwise on the plane, as in (x, y) = (cos θ, sin θ)). 0 curvature of course means a perfectly straight line.
At the point on the track centerline closest to the car’s position, the forward direction is called x and the direction toward the left is y. (Again if y seems backwards, it’s because of the righthand rule given that x goes forwards. Why is x forwards? Not really sure, that’s just how they tend to do it.)
The distance from the car to the nearest point on the center line is called y_{e}, which is positive if the car is on the left side of the center, negative on the right. The heading angle of the car with respect to the centerline is denoted Ψ_{e}, which increases as the car turns counterclockwise with respect to the centerline.
Control strategies
The most obvious thing to do is to ignore everything but y_{e} – if the line is on the left, then turn left, and if it’s on the right, turn right. That’ll get you started, but it will wander back and forth and won’t work at all when you crank up the speed. Here’s a little simulation:
Pure Proportional Control
control: \(\kappa' = K_p y_e\)
This is the first thing I tried, and is about as far as many people get when making linefollowing robots. But there’s a simple tweak that makes it work much better, if you can determine not just the distance to the centerline but also the relative angle of the line Ψ_{e}.
Pure ProportionalDifferential Control
We need a way to damp out those oscillations, and the classical way to do that is to add a derivative feedback term. We could either numerically differentiate our y_{e} error from the last frame, or we could use the sine of the angle between our heading and our centerline, which is pretty much equivalent (except it doesn’t depend on velocity).
control: \(\kappa' = K_p (y_e + K_d \sin \psi_e)\)
That’s a lot better, isn’t it? Only it tends to get “surprised” when there’s a turn, and it will always overshoot. If we know the curvature of the path we’re following, we can just add that in to our control signal.
Curvatureaware ProportionalDifferential Control
control: \(\kappa' = K_p (y_e + K_d \sin \psi_e) + \kappa\)
Even better. It still overshoots a bit, but it can be made to track very accurately. The only real issue with this is the curvature of the track actually depends on the car’s y position; the inside of a turn has a higher curvature than the outside.
There’s a paper from 1993 that studies this problem in depth that I’ve found very helpful: Trajectory tracking for unicycletype and twosteeringwheels mobile robots.^{1} The authors derive a nice closedform PD control law (a bit hairier than the above, but not hard to compute) adjusted for curvy target trajectories.
control: \(\kappa' = \frac{\cos \psi_e}{1  \kappa y_e}\left[ K_p \frac{y_e \cos^2 \psi_e}{1  \kappa y_e} + \sin \psi_e \left(\kappa \sin \psi_e  K_d \cos \psi_e \right) \right]\)
If you play around with the constants, you’ll see this one gets around the track faster than any of the others above.
Target velocity in a curve
The above give us control targets for curvature, but don’t say anything about exactly how fast we should be driving in a turn. In fact the above simulations are just driving the motor at a constant speed, and the simulator understeers in turns (in other words, the front wheels skid, it doesn’t actually turn as far as it wants to, and it slows the car down).
Ideally we’d brake for the turns and not understeer in them. But how fast should we go?
This can be very complicated due to tire dynamics and weight transfer, but the simplest thing that works is to shoot for a given lateral acceleration – the tires will be able to exert a certain amount of g force sideways, and you can measure this by driving the car around in circles and seeing what the accelerometer (or the product of the forward velocity and gyro yaw rate) says.
There’s a simple formula for lateral acceleration, and we can solve it for velocity given a maximum lateral acceleration.
\[a_L = \frac{v^2}{r} = v^2 \kappa\] \[v_{Max} = \sqrt{\left\frac{a_{LMax}}{\kappa}\right}\]There are some minor practicalities to consider here (\(\kappa\) can be near zero) – I compute \(\kappa_{min} = a_{Lmax} / v_{max}^2\) and set \(v = v_{max}\) if \(\left\kappa\right < \kappa_{min}\).
Now it speeds up for straights and slows down for turns. The only problem is it doesn’t have instantlyacting brakes, so it’s still surprised by turns; I’ve added one more parameter which is how far it looks ahead before determining the \(\kappa\) to use to compute its velocity – it will look ahead on the track and take the maximum of its control curvature and the curvature on the track coming ahead to determine its speed limit.
Future work
At this point we’re reaching the limits of what we can do with simple PDtype control, and to get really great tracking we need to start doing finitehorizon planning, but that’s out of the scope of what I want to cover here. Iterated Linear Quadratic Regulators / Differential Dynamic Programming are what I would use next, but we haven’t talked about even making the simple algorithms practical yet!
Finding y, Ψ, κ, and all that
In a real robot, we need to use our sensors to make measurements of the line with respect to our own position / orientation. This can be as simple as some phototransistors pointing at the ground, or a frontfacing (preferably wideangle) camera.
Either way, we can at least indirectly measure these parameters. To track them over time and refine our estimates requires a Kalman filter.
For now, this is a very brief overview of how it was done in the first video above:
 Calibrate the camera with OpenCV
 Generate a lookup table mapping pixels in our frontfacing camera to a virtual birdseye view
 Set the camera to take video in YUV color space (most cameras can do this natively) – YUV is a perceptual color space, and makes it really easy to find colors like yellow and orange
 Remap each image and use a convolutional filter on the virtual birdseye view to find the lane lines (note: not a convolutional neural network or anything like that, just a handdesigned function which “activates” on lane lines and is relatively immune to lighting changes)
 Use linear regression to fit a parabola to the lines
 Compute y_{e}, Ψ_{e}, κ from the equation of the parabola at the point where the fit was best.
Of course, the measurement isn’t perfect and if you completely leave the track or even just turn too far to one side, it provides no information on your rapidly increasing y_{e}. This is why I added a Kalman filter to track the line’s position relative to the car, and wheel encoders, brushless motor sensor feedback, and/or MEMS gyroscopes can help tremendously with this approach.

Alain Micaelli, Claude Samson. Trajectory tracking for unicycletype and twosteeringwheels mobile robots. [Research Report] RR2097, INRIA. 1993. ↩