Savage Solder competed in the Robomagellan event in the newly restored and most impressive Robogames 2015 this past weekend. We had two successful runs out of 3, one with no bonus cones and one which touched the challenging 0.01 bonus cone. Both runs put us on the top of the leaderboard.
Here are links to videos I found or uploaded from other other competitors from last weekend:
As part of preparing Savage Solder for events this spring, I’ve been overhauling the computer and sensing platform to have higher reliability and lower weight. One of the changes was a new LCD and input interface.
I wrote up the previous LCD here before, which was the width of the car, pretty heavy, and not resistant to the elements. For this version, I wanted to use a smaller screen, and remove the possibility of moisture getting into the case, while still maintaining roughly the same USB interface.
The display I ended up using was the 2.2″ TFT LCD from Adafruit. It is color, has sufficient pixels to display everything we wanted, and has a SPI interface. The other half of the solution is the input device, for which I decided to use an off the shelf MPR121 capacitive touch sensor controller, with a breakout board, also from Adafruit.
As before, these were glued together with a custom board holding an ATMega32U4 and a few other miscellaneous components. It is mezzanine mounted to the LCD and touch board, and has and external JST connector for USB. Also custom printed was the touch sensor itself. It is nothing more than a PCB with solder-masked copper where each of the touch sensitive areas should be, pulled out to a connector on the back.
The entire assembly was designed to be mounted to the lid of an off the shelf enclosure with a clear polycarbonate lid (which is where the rest of the electronics is going). The LCD and touch board each have four mounting holes, which are attached with a bolt, nut and lockwasher assembly. Sealing is accomplished with neoprene o-rings on the outer surface of the lid.
The MPR121 has a lot of options, many of which influence the performance of the device. The enclosure I am using is an off the shelf one from BUD (PN-1324-C amazon link). It has a polycarbonate lid approximately 1/8″ thick, which is kind of at the extreme of what you can get capacitive touch sensors to work through. I was inspired by Adafruit’s MPR121 library, but rewrote it to not require Arduino. I had to tweak a number of the MPR121 constants as well to work under the thick polycarbonate:
| MPR121 Register | Adafruit | Savage Solder |
|---|---|---|
| CONFIG1 | 0x10 | 0x20 (32uA charge current) |
| Thresholds | 12, 6 | 4, 2 |
For rendering text on the display, I also based my code on the Adafruit ILI9340 library. Since this LCD controller has to render text one pixel at a time, it was much slower than the HD44780 text-based controller we were using previously. To get remotely acceptable performance, I had to make the AVR keep a framebuffer around, and only re-draw characters which had actually changed.
We ran Savage Solder in two competitions this spring, Robomagellan in Robogames 2013, and the Sparkfun Autonomous Vehicle Competition 2013. In neither contest did we fare particularly well, especially given the level of preparation we put in. I’ll describe our results here, along with a little analysis from each event.
In brief, the Robomagellan event requires that your robot touch a target orange traffic cone. It’s made harder because the cone can be in an arbitrary outdoor environment, with challenging terrain, changing features, people, GPS obstructions and the like. Your robot can achieve time deductions by touching other “bonus” cones. You get 3 runs, and your score is the best of the three.
Savage Solder only made two runs this year. A summary of the failure modes:
The Sparkfun Autonomous Vehicle Competition, held for the last couple of years, requires the robot to drive around a loop course. Bonus points are awarded for passing through a medium sized metal hoop and jumping over a ramp. This year, the overall score was the sum total of three runs.
After our dissapointing Robomagellan performance, we doubled down for Sparkfun AVC. We replaced our ESC and got spares… of nearly everything. We built a ramp, hoop, and obtained barrels. The two weekends before the event the car was autonomously completing our practice course 100% of the time including hitting both the ramp and the jump about 90%-95% of the time over maybe a hundred runs.
How did this translate into our competition performance? We completed one run, failed after the second corner on the second run, and failed after the first corner on the third run. End result: not so hot.
Through both the spring competitions, we had 1 successful run out of 5. Of the 4 failed runs, 3 were from root causes we had never before seen, or had seen entirely too infrequently to diagnose. The 4th, (the u-blox slewing that caused our trash can hit), had been happening only every 4 or 5 robomagellan runs, so it was a known, but limited, risk.
Obviously, we are fixing all the individual failure modes that we observed. I am not sure what the larger lesson here is, other than that there are many many failure modes and the only way to find them all is through exhaustive testing. Maybe using more expensive components would have helped, as the cheap u-blox and our ESC with no overcurrent protection cost us 3 runs total (4 if you count our missed 3rd robomagellan run).
I suppose the ultimate conclusion is that building robots remains hard.
In part 1 and part 2, I covered the details of the localization solution we use on Savage Solder. To finish off, I’ll look at the areas we are exploring for future improvement.
While in motion, our u-blox GPS can occasionally slew to an erroneous position that may be 20m away from ground truth, then slew back 20s later, even while reporting WAAS corrections, good HDOP and a large number of satellites. These slews of course confuse the localization algorithm, as they do not match the system dynamics at all. The estimation filter’s heading and position can diverge during these events.
One possible indication of these events can be found in the pseudo-range residuals the u-blox reports. The GPS receiver operates by measuring “pseudo-ranges” to each of the satellites it is tracking in the sky. In general, there are more ranges than necessary to determine both the receiver’s position and its clock bias at any point in time. Thus for any given position the receiver reports, there will be some error between what it thinks the range should be to each satellite, and what the actual measured pseudo-range was. This is reported as the “pseudo-range residual” via the NMEA GPGRS message.
During these error slew events, we have identified that the u-blox does generally report one or more of the residuals growing very large (sometimes more than 100m), then shrinking back down as the position corrects itself. This is even as it reports the same approximately constant horizontal position accuracy! For future competitions, such as Sparkfun AVC, we’re experimenting with using these residual measurements to de-weight GPS readings which are suspect. I’ve also put together the below video to describe the problem in more detail:
The Kalman filter configuration we use now is a total state filter. In a total state localization filter, quantities such as the yaw rate are estimated as part of the filter, in addition to their error parameters. This makes the filter formulation straightforward, but results in an effective low pass filter applied on the yaw rate, as it is only updated through the Kalman filter measurement update step. Because of this filtering, in high dynamic maneuvers the heading can suffer from accuracy problems until the GPS is able to correct matters.
One alternate filter configuration, the “error-state filter”, estimates the error between a forward integrated solution and reality rather than estimating the solution directly. In this formulation, the error model parameters of the yaw rate gyro are included as states, but the yaw rate itself is not. This removes the low-pass filtering on the yaw rate, potentially improving filter response during fast maneuvering.
In part 1 I looked at the reasons why we use a state estimation filter for Savage Solder’s localization system and what the options were. This time, I’ll look at the specific measurements we take and the resulting states that we estimate along with some implementation details.
Our localization system uses the following measurements:
Each of these measurements is integrated into the state estimation filter at their natural rate. The filter only emits a solution on each IMU update, just to reduce the variability in the rest of the system.
The Unscented Kalman Filter (UKF) estimates the following system states:
When designing a state estimation filter, there are a lot of constants that need to be selected. For Kalman filters, you need an initial covariance and process noise for all estimated states, and measurement noise for all dimensions of the measured values. By choosing a specific selection of constants, you can trade off between the bandwidth (performance or accuracy) of the filter, and its stability. Generally, if you make the filter more accurate, you increase the risk that the filter will become unstable if the system model or measurement models do not accurately track the real world.
For Savage Solder, we used a largely ad-hoc approach, with no rigorous backing for our constant selection. We picked process noises that roughly corresponded to the amount of system error we expected to see, and did the same with measurement noises. One exception was GPS measurement error. Since we are using a naive total state filter, and GPS measurements from cheap receivers are highly time correlated (because of their own internal estimation filters), there are a number of other factors to consider. If the GPS measurement is set too high in this formulation, heading cannot be estimated accurately. If it is set too low, the global position will jump around a lot with every GPS update. In no case is the filter’s assumption, that of a measurement corrupted purely by white noise, satisfied, so measurements noises that are too low also run the risk of filter divergence. We just picked a happy medium that tends to work well in our simulations and practice.
There are also a fair number of heuristics included. For instance, if we know the car is stopped from the odometer, we artificially fix the heading and curvature estimates and covariances. At least when it is driving under its own power, Savage Solder rarely rotates in place. Without holding these fixed, noise in the GPS and gyroscope can cause the estimated heading to drift with no bounds and the heading uncertainty to grow arbitrarily large.
As I mentioned early in part 1, different subsystems in Savage Solder have differing requirements for the state estimates they consume. Some, like global driving, want the estimate to be close to the globally correct position so that the car can drive close to a pre-surveyed path. Others, like cone tracking, would much rather have a locally consistent frame of reference that is divorced from global coordinates entirely.
We solve this by deriving a separate local coordinate estimate from the global estimate emitted by the UKF. This local estimate always starts at position 0, 0 with a heading of 0. At each time step, the velocity, heading, and curvature of the global solution are integrated forward. This results in an estimate which is by definition smooth, yet still is relatively accurate over short distances, as the velocity and heading inputs have full access to the sensor suite of the car.
In the next, and final installment, I’ll look at some techniques we are, or want to be exploring to improve the localization solution on Savage Solder.
When it isn’t crashing into trash cans, Savage Solder uses a combination of several sensors to form accurate estimates about where it is in the world. This series describes the sensors and techniques that it uses to identify where it is in the world, providing data that is useful for the driving and cone tracking subsystems.
First, why does the car need to know where it is? Several other subsystems in Savage Solder rely on having accurate estimates of where the car is, measured in different ways. First, when driving to try and find a cone, it follows a path surveyed by GPS. In that case, the driving algorithm needs to know where the car is relative to the pre-surveyed path in order to select the appropriate steering and throttle commands. Here, absolute accuracy is most useful. We rely on surveyed paths to avoid obstacles, and the driving algorithm is relatively insensitive to position estimates that move around as GPS measurements change.
When moving in proximity to cones, the car needs a locally stable coordinate system in order to identify where the cone is relative to the car itself. For this, an estimate that is smooth is most important, as the car’s position relative to a cone can only change in a continuous manner.
For both of these cases, the localization subsystem on Savage Solder takes measurements from various sensors, and produces an estimate of the current position of the car. This estimate is updated on a regular basis, and fed to each other subsystem which needs to know where the car is.
At the top level, Savage Solder uses a total-state unscented Kalman filter to incorporate measurements and produce localization estimates. Kalman filters are a class of algorithms which, given a series of noisy measurements of a system, estimate what the internal state variables the system are. In the localization problem, the noisy measurements are things like a GPS reading, a gyroscope measurement, or the value from an odometer, while the estimated state is perhaps the latitude and longitude of the vehicle along with its heading, heading rate and speed.
The Unscented Kalman filter is just one type of estimation algorithm. Traditionally, a designer would select it over other non-linear estimators when higher accuracy is required. As compared to estimators like the Extended Kalman Filter, it achieves this higher accuracy through an increase in computational cost.
For Savage Solder, our PC based computer platform has power to spare, so we chose the unscented Kalman filter (UKF) for a different reason. We used the UKF because it only requires numerical system models. Most non-linear Kalman filters, like the EKF, require analytical derivations of the partial derivatives of the system and measurement functions. These can be involved to create. When you want to change your design, and modify the estimated states or measurement variables, the process is slow and error prone. The UKF only requires a numerical definition of the system and measurement functions themselves. Because of this, we can experiment with different filter structures much more rapidly.
In the next installment, I’ll look in more detail at the measurements we use, and the states that we estimate.
Robogames 2013 has come and gone, with all manner of amazing robot mahem unleashed. Unfortunately, Savage Solder didn’t have the best performance in Robomagellan, hitting a trash can on its first run, and a newly added picnic table on its second. Our motor controller popped after that run, so we didn’t get a third attempt. Hopefully I will post a more detailed post-mortem later, but for now, I’ve uploaded the video of our best competition performance this year.
The final pieces of the cone detection and tracking system for Savage Solder are the tools we used to derive all the constants necessary for each of the algorithms. In part 1 (cone detector) and part 2 (cone tracker) I described how we first pick out possible range and bearings to cones in an image, and then take those range and bearings and turn them into a local Cartesian coordinate through the tracking process. As mentioned there, each of those stages has many tunable knobs.For the first stage, the following are the key parameters:
Additionally, the cone tracker has its own set of parameters:
What we did in 2012, during the preparation for our first RoboMagellan, was take a lot of pictures of cones during our practice runs. Savage Solder normally is configured to save an image twice a second all the time it is running. These image datasets formed the basis of our ability to tune parameters and develop algorithms that would be robust in a wide range of conditions.
The basic idea we worked off was to create a metric for how good the system is, and then evaluate the metric over a set of data using our current algorithms and constants. You can then easily tweak the constants and algorithms as much as you want without running the car one bit and have good confidence that the results will be applicable to actual live runs.
Most of the metrics we wanted involved knowing where the cones actually were in the images. If we could just robustly identify cones in images programmatically, we wouldn’t really be worrying about this to begin with. So instead, we created a set of tools that let us rapidly mark, or annotate, images to indicate where the absolute ground truth of cones could be found. This is just a simple custom OpenCV program with some keyboard shortcuts that let us classify the cones in about 3000 images in a couple of hours. We selected images from varying times of day, angles, and lighting conditions, so that we would have a robust training set.
Then, with a little wrapper script, we ran our cone detection algorithm over each of the frames. As mentioned in the on the cone detector, it outputs one or more range/bearing pairs to each of the prospective cones in the image. The quality metric then scores the cone detector based on accuracy of bearing and range to real cones, missed detections, and false detections. It also keeps histograms of each detection category by range. In the end, for a given set of cone detector parameters, we end up with a table that looks like the one below.
| Range | Detection Rate | False Positive Rate |
|---|---|---|
| 3m | 91% | 2% |
| 5m | 95% | 7% |
| 7.5m | 93% | 21% |
| 10m | 89% | 34% |
| 15m | 72% | 26% |
| 20m | 37% | 23% |
After each run over all the images, we would try changing the parameters to the cone detector, then seeing what the resultant table would like. Ideally, you have a much higher detection rate than false positive rate over the ranges you care about. The table above is actually the final one we were able to achieve for 2012, which allowed us to reliably sense cones at 15 meters of range using just our 640×480 stock webcam.
We got our final day of testing in at Danehy park in preparation for Robogames 2013 this weekend. Everything worked as well as we expected, the only things that would have caused problems on race day were trying to fit through areas too narrow for the basic GPS we were getting at the time. All in all, our speed, cornering, and tracking performance are much improved over 2012.
Last time I covered the techniques we use on Savage Solder to pick out orange traffic cones from webcam images in the “Cone Detector”. This time, I’ll look at the next stage in that pipeline, turning the range and bearing reported by the cone tracker into Cartesian coordinate estimates of where any given cone is.
As mentioned last time, our “Cone Tracker” takes as input the range and bearing, (along with their uncertainties), from the cone detector. It also receives the current navigation state of the car. This includes things like the current estimate of geodetic position (latitude and longitude), current map position, (UTM easting and northing), and an unreferenced local coordinate system (x, y). For each of these, it reports the vehicle’s speed and heading.
I won’t go into the details of each of these coordinate systems here, but since the cone tracker actually only uses the local one, I can discuss it a bit. The local coordinate system on Savage Solder starts out at x,y=(0m,0m) with a heading of 0 degrees at the beginning of every run. As time progresses, the heading and position are updated primarily based on our onboard dead reckoning sensors. The local coordinate system is smooth, and never jumps for GPS updates. As a consequence, it isn’t really useful to compare positions in it from more than a minute or two apart, nor is it useful to do GPS based navigation. Fortunately, for cone tracking, from the time to when we see the cone to when we touch it is usually only a couple of seconds total over which time the local solution is very accurate.
Now, let’s cover some broad details of the implementation. The guts of our cone tracker consists of a bank of identical Kalman Filters. Each Kalman filter estimates the position of one particular cone and the error in that estimate. This lets the car keep around several possible cones that could be in sight at one time while still distinguishing them. By storing the cones in a local coordinate system, it allows for easy driving towards, or alongside, the cone and accurate speed control leading up to it. The position uncertainty could be used to control behavior, but we don’t bother in our current implementation.
Additionally, the tracker has to handle seeing new cones for the first time, and discarding cones that maybe were false detections in the cone detector. It does this by assigning each cone a “likelihood”, which is just a probability that the cone is a true detection. When data arrives that match the cone well, its likelihood is increased. When the available data doesn’t match the cone very well, or no cone is observed at all when one is expected, the likelihood is decreased.
If a range and bearing arrive which corresponds to no known cones, a new one is created with a relatively low likelihood. Then once it has reached a certain threshold, it is reported to the outside world as a valid target. Conversely, when an existing cone’s likelihood reaches a level which is too low, it is removed entirely on the thesis that it was actually a false detection to begin with.
More specifically, the likelihood update is handled using Bayes theorem. We have an empirically derived table showing, for our detector, the odds that a cone will be detected or a false one will be seen at various ranges. These are used to fill in the various probabilities in the equations.
A “measurement” in this problem is simply the range and bearing that the cone detector reports. To incorporate a new measurement, the tracker looks through all the cones currently in its filter bank. For each, it computes a measure of the likelihood that the given cone could produce the current measurement. This is done using what’s called the Mahalanobis distance. The Mahalanobis distance is just a measure of how far away you are from an uncertain target in a multi-dimensional space.
If the best matching cone has a Mahalanobis distance small enough to count as valid, then the standard Kalman filter update equation is applied to that cone. If no cones match, then we create a new one as described above.
One final detail, is that in addition to estimating the position of each cone, the tracker also estimates its “scale” as seen by the cone detector. The image based detector we use has the property that the range values are likely to have a fixed scale error for a number of reasons. One, the cones could actually be bigger or smaller than the regulation sized ones. Second, lighting conditions can sometimes cause a fraction of the cone to be not detected, which will result in the cone being seen as smaller than it actually is.
Since the range values are not correct, the position will be similarly off. This error isn’t as bad as it seems, since we (and most every RoboMagellan entrant), approach the cone in a straight line. Thus the position errors will always be directly towards or away from the robot, and as long as you keep moving, you’ll hit it eventually.
Savage Solder has two reasons to care. First, is that we decelerate from a high speed to an appropriate “bumping” speed with just a half meter to spare. Thus, if the cone is estimated as too close, we run the risk of colliding with it at an unsafe speed and damaging our front end. Secondly, we have a mode where the car can just “clip” the cone by driving alongside it and tapping it with a protruding stick. This allows us to avoid stopping entirely when the geometry allows it. However, if the position estimate is incorrect here, we might miss the cone entirely.
To summarize, the cone tracker handles each new range and bearing with the following steps:
One of the challenges with this approach is that there are a lot of constants to tune. I’ll cover the details in a later post, but for most of them we tried to find some way to empirically measure reasonable values for our application. Another problem was debugging. printf doesn’t cut it when you have this much geometry. For that, we created a number of custom debugging and visualization tools which help show what is going on in the system, some of which I’ll cover later too.
A Modicum of Fun 