Introduction: Autonomous Racing
Recently, my team from UNCChapel Hill won an F1/Tenth competition, held at CPSWeek 2019, in Montreal. For those who don't know, F1/Tenth is an autonomous R/C car race with an emphasis primarily on a common platformso competition is primarily between algorithms and less about the quality of the physical vehicle. I encourage anyone interested to check out the F1Tenth Website for more information.To the surprise of myself and my teammates, not only did our car win, but it won by a wide margin, even beating out the contest organizers! Among the cars able to reliably complete laps at the event, only two apart from ours were able to do so at a high speed. Both of these teams (as far as I know) were doing some combination of localization and path optimization. One of them, from the University of Modena, completed laps of the ~150 foot track in around 14.7 seconds. The other team was the contest organizers, from Penn, and they completed laps in around 12.7 seconds. (I'll also note that after increasing their speed in order to accomplish these times, neither of these cars completed all four minutes of the timetrials with no accidents.) Team UNC's lap time: ~11.5 seconds! And no crashes whatsoever during any of the 11 total minutes of time trials!
It was apparent to anybody present at the competition that both of the runnerup teams were running elegant code with good, smooth controls, leading to their cars taking a safelooking but short path around the track. I even felt a bit guilty that our car was able to win against these teams who had clearly put far more thought and care into developing an algorithm that not only runs a track, but also could, unlike ours, legitimately further the field of autonomous driving.
But, rules are rules, and the rules of the event were just to complete laps as fast as possible!
So, on to answer the real question: what algorithm did we use to win? Essentially, our algorithm was: follow the shortest safe path through the track, and drive as quickly as possible. This sounds obvious, but it's still relevantthe biggest differentiator between "Team UNC" and Penn or Modena was the fact that the latter two visibly diverged from the shortest path at several points around the track. Both were going fast, but with ~150ft of track and a nearidentical top speed for all of our cars, the slight path differences were enough to give us the second or two of an edge on each lapadding up to quite a few more laps over the span of several minutes.
So, clearly I was happy with our performance, but why do I think other teams should care?
Several teams at the event were doing some simpler algorithms, namely some variants of wall following, centeroftrack following, or "gap finding". Our algorithm is not only faster than any of these, it is also simpler and more robust (in my opinionthere are of course situations that our algorithm doesn't handle properly, which I'll cover in later paragraphs). I honestly believe that, barring major changes to the contest track or format, our algorithm can and should represent a new baseline for every team from now on. At the very least, any team looking to ditch wallfollowing, gap finding, etc., and instead use a simple, competitive solution should check this out.
The remainder of this essay will be as follows:

Describe the algorithm.

Discuss the situations where the algorithm won't behave correctly.

Give a semiformal argument for why this
greedy, localonly algorithm leads to globally optimal behavior under a certain set of assumptions (which closely match the format of the F1/Tenth contest).
 Give concluding remarks about the contest and my hopes for its future.
1: The "Disparity Extender" Algorithm
I'll start by summarizing the algorithm in a nutshell. At every instant:
Find the longest distance, not behind the car, that the car can safely
reach by driving in a straight line.

Go in that direction.
1.1: "Find the shortest path"
The first of these two steps is the one that bears more explaining. First, we assume that the car is regularly receiving data from a LIDAR sensor, which will simply be an array of distances (once again, see the F1Tenth website at the top of this article if you don't know what I'm talking about here). Finding the longest distance in an array of distances is trivial, and we can figure out the angle where this distance appears and drive towards it. For example, see this image:Heading in the longest possible straight line is good for "cutting corners" 
(For the rest of this article, I am just taking for granted that if the angle exceeds the car's steering angle that we just turn as sharply as possible towards the correct angle.)
However, there's a problem with this that quickly becomes apparent: the LIDAR "thinks" of itself as a single point, but in reality it's on a car that has some width. Driving towards the most distant point may cause the wheels and sides of the car to crash into corners or other obstacles, despite the fact that the point is visible. If we followed the path in the first image, we would crash at this point:
Purely following a straight line fails to account for the fact that you're a car, not an infinitesimally small point... 
So, how do we account for this?
We did so by using an algorithm to produce a "filtered" list of distance samples, where each distance in the filtered list corresponds to a distance that's safely reachable by driving in a straight line. Hopefully this makes it obvious why we call our driving approach the "disparity extender" algorithm:

Take the raw array of LIDAR samples (mapping angles to distances).

Identify disparities between subsequent points in the LIDAR samples.

For each pair of points around a disparity, pick the point at the closer
distance. Calculate the number of LIDAR samples needed to cover half the
width of the car, plus some tolerance, at this distance.

Starting at the more distant of the two points at the disparity, and
continuing in the same direction, overwrite the number of samples in the
array with the closer distance. (Do not overwrite any points that are
already closer!) Do this until you've either reached the end of the array
or you've covered the number of samples you calculated in step 3.
The key insight behind this algorithm is that the obstacles that intrude into the path of the car will result in a "disparity" in the LIDAR data. Here, we use "disparity" to refer to two subsequent points in the array of distance values that differ by some amount larger than some predefined threshold. This threshold doesn't need to be very largethere will be only 4 or 5 centimeters between LIDAR samples (using the F1/Tenthrecommended LIDAR) at 78 meters, which is a pretty long distance at the scale of these cars. So, using a threshold of 0.1 or 0.2 meters for identifying a "disparity" between subsequent LIDAR readings should be good enough for any practical purposes.
Here are a summary of the above steps using pictures.
Step 2: Finding a disparity:
We start with LIDAR readings (presented as an array of floatingpoint distances), and look for consecutive readings that differ by an amount over some threshold. 
Step 3: Overwriting LIDAR readings equal to the car's width plus some tolerance at the disparity point ("extend the disparity").
We mask over LIDAR readings (shown in orange) in order to make them appear shorter in the "filtered" arraythese should approximate points that the car can actually reach (more or less) 
Step 3 (continued): Repeat this process for every disparity, but never overwrite closer "distances" with farther ones.
Many LIDAR readings contain multiple disparity points. 
Step 4: Find the farthest reachable distance in the new list of distances, and use that as the target direction.
In the "filtered" array of distances, the longest path should be something that's actually reachable (or at least close to reachable). 
1.2: "Drive as fast as possible"
As I'll argue later, this algorithm is sufficient for "finding the shortest path" through the track. Our system for "drive as fast as possible" is even less sophisticated, but, in our experience, quite effective. We pick a speed based solely on the forward distancethe free space directly in front of the car. If this distance is long enough, we drive at the car's maximum speed. If it's too short for the car to turn or avoid a collision, then we stop.If the forward distance is anywhere in between the minimum safe distance and the safe "fullspeed" distance, then we simply scale the speed based on the distance. We used a simple linearscaling approach along two segments, but this can easily be adjusted to use nonlinear functions of distance should someone else choose to do so. As for why I think it's okay to scale speed using forward distance alone:
There are only two reasons for the car to slow down:

Physical reasons (e.g. don't flip the car, or try not to drift)

Reactiontime reasons (we don't want to miss "seeing" a turn in time)
Reason 1 (don't flip over) is handled fairly well by the forwarddistance approach to scaling speedafter all, this only applies when you're making a turn. If you're making a turn you've either seen the turn from a long way away or suddenly encountered it. If you saw the turn from a long way away, that likely means that it's a gentle turn so the car is perfectly fine to continue moving quickly. If the turn approached quickly, then the car should have been already facing a wall when it encountered a turn, so it would have slowed down like it's supposed to.
The one possible "incorrect" point here is if the car rounds a turn and suddenly encounters a very wide area where the longest path is still sharply in one direction. However, as long as the car doesn't flip over, it's perfectly safe for it to "drift" in such a caseafter all, the only reason it's going fast is because it's already determined that there's adequate space. One may argue that a car that's drifting is moving less efficientlybut on the other hand why would you want to stop your RC car from drifting at high speeds when it's so cool?
1.3: Small tweak: Watch the back of the car
Even though the algorithm as described above adequately accounts for the width of the car, we need to tack on one additional constraint to make sure the rear of the car doesn't strike a corner (or passing car) when trying to turn. We handled this case in a very simple manner that simply assumes that whatever obstacle is at the side of the car is not moving towards the car. (We're assuming that the LIDAR is mounted on the front of the car). The tweak is like this:
Scan all available LIDAR samples below 90 or above +90 degrees (these will
cover the sides of the car to the back).

If any point is below a safe distance on the side of the car in the
direction the car is turning, stop turning and just keep going straight.
This is a situation where we would make the car continue to drive straight, even though it "wants" to turn left. 
1.4: Summary of the Algorithm

Acquire the LIDAR data

Find the "disparities" in the LIDAR data

For each disparity:

Find the nearer of the two points, and calculate the number of LIDAR
samples neccessary to "mask out" the region of samples unsafely close
to the obstacle at that distance.

Overwrite the number of samples calculated previously with the shorter
distance, starting at the disparity, and moving in the direction of the
longer distances. (Don't overwrite distances that are already closer!)

Find the nearer of the two points, and calculate the number of LIDAR
samples neccessary to "mask out" the region of samples unsafely close
to the obstacle at that distance.

Search the new "filtered" distances for the farthest point that isn't
behind the car. Calculate the angle that this point is in.

Steer in the angle you calculated (or as much towards it as possible).
Make sure that there are no obstacles on the side of the car that you'll
hit due to turning; go straight instead if there are.

Set the speed of the car based on the distance directly in front of the
car.
And finally, here's a simulation of the algorithm running in unreal engine (the "filtered" distance points are the ones highlighted in red):
(Credits to my teammate Abel for writing the simulation and providing this video.)
Overall, this algorithm is very reactivethe most computeintensive portions are scanning the LIDAR array for disparities and extending the disparities; the latter of which is only proportional to the number of disparities identified. It requires no trignometric operations or fancy math, is resilient to noise (it may cause some "twitchy" steering, but will quickly selfcorrect), and is capable of dodging obstacles outofthebox.
Later on, I will attempt to argue that this greedy algorithm correctly follows the shortest path, based on a set of assumptions that I believe are reasonable for F1/Tenth races. However, first I will give discuss a case we've identified where the algorithm can fail.
2: Where the Algorithm Fails
With careful tolerances this algorithm should never allow the car to crash into an obstacle that it's able to see, but it has another failure condition: under some situations, it will try to perform a UTurn (even though this is arguably safe behavior, it's definitely not conducive to winning races).Specifically, a Uturn happens in situations like the following, where the car is on the inside corner of a wide portion of track, and must turn into a narrower portion of track:
We risk doing a Uturn here 
The problem occurs because, as the car approaches the turn, it will be able to "see" some reachable paths into the turn. However, these paths will be shorter than the paths to the sideinto the wide part of the track the car is already in. The car would see the correct path if it moved forward slightly more:
Sometimes the algorithm can narrowly avoid a uturn in these cases (especially if the car is moving at slower speeds enabling it to react to the correct path more quickly) 
However, by the time it's far enough forward it may already have turned too much, leaving the correct path behind the car rather than to the right or left of it:
The correct path is now behind the car, and the farthest path the car can "see" in the correct direction is shorter than the path in the wrong direction. 
This issue shouldn't occur on any track with a uniform width, because any portion of track that the car must turn towards will always be the same width as the portion of track the car is already onso the opposite wall of the current portion of track will never be the farthest reachable distance.
Overall, however, we did not encounter this problem at the F1/Tenth contest at CPSWeek 2019, but that may change in the future, especially if the contest organizers build a track specifically to exploit this issue, or if there are more races against other cars or dynamic obstacles.
So, my conclusion is that this problem needs addressing, but I hope it's something we can just detect and avoid. I think the best way forward is to integrate better pathplanning or dynamic mapping into our base algorithm, to hopefully maintain the benefits of the algorithm in general while avoiding pitfalls like this one.
And, for those who want a video of the problem in action, here's one where I stand in front of the car at a specific point at the F1/Tenth contest track:
(Somehow it managed to do a second Uturn right after the first, we don't understand why that happened yet...)
3: Optimality of this Approach
As mentioned earlier, I will now spend the rest of this article arguing that this simple algorithm correctly follows the shortest path around a track under some ideal assumptions, and does very good job at approximating the path in most cases.3.1: Assumptions

The track consists of a single path, with a uniform width. The "single
path" portion of this assumption is reasonable for F1/Tenth (minus
obstacles like other cars), but the uniform width assumption is less so
see the videos I linked earlier. The contest organizers could easily set up a track intended to
trick and humiliate this algorithm at the next race!

The car is physically perfect, can turn instantly, and won't slide, etc. Even though this is not a reasonable assumption, I think it's also less important in
actual races. Sliding is a function of speed and can be avoided,
and a wide turning radius will only make a real car waste a little bit of
distance before returning to the optimal path. (Once again, these assumptions are only so I can arguing for the optimality of the algorithm
in the ideal case.)

LIDAR data is perfect and continuous, and we react with no computation
time. Once again, this is obviously not realistic. In reality, we get
LIDAR data at about 40Hz, it has noticable noise on the order of ~1cm of
distance, and it takes us around 10ms to process it (using Python alone).
However, the good news here is that if we improve computation times and LIDAR quality, our algorithm will continually perform better in the real world. In the meantime,
we can increase our distance tolerances and continue to approximate a good
path.
3.2: Optimality Argument
My first claim is that the "optimal" way to drive around around an ideal uniformwidth track is to take the shortest path, as fast as possible. We can actually restate this definition as something more helpful. If a car is taking the optimal path, then during every time instant, the car makes as much progress as possible around the track. In this sense, "as much progress as possible" really means "reducing the distance to the end of the lap/track as much as possible"First, let's revisit the notion of "going in the direction of the longest safe straightline distance." With the above definition of "optimal", it becomes a bit more clear why this simple rule is the optimal strategy. I don't have a formal proof here, but it seems intuitively true that, for a given instant, under our assumptions, the longest straightline path in a forward direction is also the path that covers the most trackin short, it's the direction that makes the most possible progress towards completing a lap!
But what about places where the longest forward distance ends up targeting a point that's clearly outside of the best possible path, like at a Ubend:
The car's approximate target path at a Ubend in a uniformwidth track (drawn by hand, so just assume it's uniform width!) 
The car is trying to drive towards the farthest point in that elbowclearly that point is far from the optimal path, right? However, on an ideal track, this isn't relevant, because at the instant that the car identifies this longest path, it is still the path that covers the most track. The fact that it's in the (slightly) wrong ultimate direction doesn't matterand furthermore when the car reaches the point where it can "see" around the elbow, it will choose a new path anyway. (If you're still not intuitively convinced, try tracing the shortest safe path from the car's position to around the bend in the figure abovethe car's initial target direction should be the same as the direction indicated in the figure.)
So, that's the general argument for why the algorithm attempts to follow the shortest path, but what about the fastest speed? Under the ideal assumptions outlined above, speed is irrelevant because sensing, reaction, and physical characteristics of the car are perfect, meaning the ideal car should be moving at the maximum speed constantly. However, I'll break from the purely ideal scenario here because speed plays an important role in many aspects of the algorithm's performance in reality.
Fortunately, the seemingly oversimple "forwarddistance" method for scaling speed turns out to have great synergy with our particular algorithm. The reason for this is simple: our algorithm is constantly trying to maximize forward distance! In short, if we're not in a position where we can move at the fastest possible speed, we are actively working to reestablish such a position as soon as possible.
4. Conclusion
My team and I had a great time at the competition, and I would certainly enjoy returning and seeing participation increase. I was slightly surprised that only seven teams participated (including Penn, who helped organize the contest, so I'm not sure if they count as true "participants"). So, in the hopes of encouraging more teams considering entering to bite the bullet and actually participate, I'll conclude with a story about how our team was started, and how we "developed" the winning algorithm.First off, my team was formed in a rather impromptu fashion, after Professor Duggirala joined the computer science department at UNC. When he joined, I was one of a small handful of students who had worked before with the F1/Tenth platform as part of a small experimental class, so I was invited to join the newlyformed UNC team for the F1/Tenth competition.
A few weeks later, our team had met a few times, and managed to assemble a car. Sort of. It ended up taking us another couple weeks before we had figured out all of the "nutsandbolts" problems like configuring our motor controller correctly, figuring out how to actually get the steering servo on our our motor controller to engage (if you've been in this situation as somebody who has little experience working with this kind of stuff, you'll know how frustrating it can be), configuring an adhoc wifi network for the car, and so on. The point is, that we didn't have a car that could move at all until nearly three weeks before the competition.
At that point, we had a car that moved, but our ability to reliably complete laps was questionable, much less its ability to do so quickly. Around this time, we held a meeting on a Saturday, ostensibly to "test" the car, but we had no code worth testing yet! As group meetings often go, this meeting was full of several good ideas, but not much code actually being written for them. It was at that point, that I (in a rather foul mood) suggested that we should start with something based solely on how simple it would be to implement, and then work up from there. The simplest thing I could think of was the "find the farthest distance" algorithm I listed above, and later that evening that I thought of the "disparity extender" portion of it for efficiently navigating around obstacles.
After testing this algorithm on small tracks around our department, we were convinced that it was pretty good, despite occasional UTurns. Ultimately, efforts made by other team members to implement a more sophisticated algorithm weren't finished in time for the competition, so we headed to Montreal intending mostly to see what we could do with what we hadwe had no idea how our simple code would stack up. So, we were quite surprised when we tested the car on the competition track and proceeded to win with virtually no changes necessary! It was only after this, while questioning what made our approach so effective, that I came up with the arguments I outlined above.
So, to any teams out there who think that they can't or shouldn't compete because they don't have a specialist in computer vision, path planning, robotics, or control theory, my message is that you don't need that stuff! You're not trying to build a fleet of selfdriving taxisyou just need to get an R/C car to drive itself around a track. And, if you're just beginning, there's a simple but effective algorithm I'd recommend starting with!
Acknowledgements
To finish up, I also want to give credit to the rest of my team, including Charlotte, who helped with fabricating parts, car assembly, and testing, Tanya, who helped with math, wrote most of the speedcontrol code, and designed the pictures in this article, Abel, who spent many hours with and without me trying to get the car to work and who continues to work to improve the algorithm, and Professor Duggirala who directed this project with the liberating command: "I don't care about the car's safety if going slow is just going to make you lose."From left to right: Me, Charlotte, Professor Sridhar Duggirala, Tanya, and Abel 
Also, thank you to the folks on the Penn team who were willing to chat with us at the contest, and who were good sports about our car rearending them at high speed during headtohead skirmishes!
No comments:
Post a Comment