The basic idea of the particle tracking algorithm is to use this sample-based distribution model as the representation for bel(x), which is now a set of pose hypotheses Xt = {x[i]t }for any given time stept.
In the prediction step, we incorporate the motion noise intoXt−1by sampling from our motion model to obtain Xt. If we now have a way q(Bx) to assess a particular hypothesis’ quality – the degree to which sensor data and map agree at that pose – we can remove some of the prediction’s uncertainty and reflect that information in a new sample set Xt that has, in comparison, more particles in high-quality regions of the pose space. We do this simply by drawing particles from Xt using importance sampling withqas the importance weight.
In other words, we carry along multiple hypotheses for the truex, apply the motion model to each of them individually, then rate their respective qualities regarding the sensor data and finally use that to build a set of better hypotheses. Over the iterations
of this process, the unlikely hypotheses that have “drifted” away from the robot’s true pose increasingly die out.
This idea is called a particle filter and is usually applied over the complete pose space, resulting in global localization. It is well known to give good results while at the same time remaining conceptually simple. See [Thrun, 96ff] for an overview.
In the case of our tracking problem, the region of pose space that contains high-quality particles is comparatively small, so that we can potentially get away with much smaller sample sizes than are usually required with this type of algorithm.
But we have to guarantee that the particles do not diverge too much from the previous pose, or else this intuition will not hold anymore. Because large parts of the model track look very self-similar, wrong guesses can easily be rated as high-quality in multiple consecutive iterations, even with a good quality estimator. Consider, for example, a straight road on the track: the only local distinguishing features in forward direction are the middle lane markers. The sensor image at two distinct poses at the beginning of consecutive lane markers look exactly the same.
We can only hope that these “false optima” in the quality function collapse quickly, by more distinguishing features – like a curve or junction for instance – coming into view.
To prevent the belief distribution from diluting too far in this manner, it is truncated to a predefined region around the car. This partially neglects the advantages gained from the particle representation, because it can discard very valid hypotheses. But this price has to be paid to achieve acceptable running times and accuracy on our platform as our resources are limited.
5.2 Implementation
The particle-based algorithm was implemented as an extension to the force field tracker, so much of the code could be reused. The sensor model in this approach is exactly equivalent to the one described in section 4.2.2 for the force field algorithm and is not repeated here. However, we need some extensions to the motion model and the map to implement the particle tracker.
5.2.1 Motion Model
For the prediction step, we need to extend the motion model from section 4.2.1 so that it also includes motion noise; specifically, we need to sample from the resulting
probability distribution. This is achieved by prepending the noiseless velocity motion model with a step that adds random noise to the motion control informationu. This is the full version of the velocity motion model from [Thrun, 121ff] that had been stripped of the motion noise for the force field algorithm.
The noise distribution should have a mean of zero and a varianceb2that depends on the absolute value of uso that faster motion introduces a stronger uncertainty. The distribution chosen for this implementation is asymmetric triangular distribution that can be sampled from using algorithm 5.
Algorithm 5 SampleFromTriangular Require: varianceb2
return q
3
2(SampleFromUniform(−b, b) +SampleFromUniform(−b, b)) Here,SampleFromUniform(l, u) is a library method that generates a pseudorandom number from the interval [l, u].
The variables vt and ωt were obtained from seperate measurements, but they are not necessarily independent. For example, the car is unlikely to turn without driving forward. The variance of the noise added to each of the two variables therefore depends on both of them.
But just mutatinguis not enough: because we assume that we move along a circle arc starting in xt−1, we can never reach all possible poses in the target region of the pose space. This is because we are only using two degrees of freedom to change a value that has three, and therefore the image of that operation is a 2-dimensional manifold in the 3-dimensional pose space. Hence we introduce a third source of noise that is applied directly to the final orientation θt. For more information on why this is necessary, see [Thrun, 129].
The resulting algorithm 6 now provides a mechanism to sample from our motion model. Theαparameters here are robot-specific constants that define the accuracy of the measurements foruand need to be found by experimentation.
5.2.2 Map
As previously mentioned, we need a wayq(Bx) to assess the quality of any given pose hypothesisx[i]t regarding the current sensor datazt. In other words, we wish to know how likely a measurement likeztis assuming we are atx[i]t . For this reason, we need a wayl(p) to tell how likely the road is to be white at any given positionp– alikelihood field.
Algorithm 6 VelocityMotionModel
Require: previous pose bel(xt−1) asXt−1, motion dataut
∆t←length of current time frame Xt← ∅
In an ideal world without sensor noise, we could just useG(p) for this purpose, and for this particle filter, this would probably work reasonably well. But if we also want to model sensor noise, we need to incorporate it intol.
Another problem of Gas a likelihood field is that it is non-smooth. That means that if x[i]t is sufficiently far off, the likelihood distribution in the vicinity is flat, so there is no new information to incorporate into our belief distribution bel(xt) and we have little chance of correcting the uncertainty introduced by the motion model.
Therefore, we need a better model for l. A reasonable assumption would be to say that the likelihood of reading a white line at pis higher the closer it is to a white cell in G. Recall that we already have that kind of information in F~NN, so we can define the likelihood field as
l(p) =e−λ|F~NN(p)| (5.1) where λ is a positive implementation constant modeling the strength of the sensor noise, the shape of which is described by the wrapping functione−λ(·). Other wrapping functions could be used, but usually they should be monotonically decreasing and normalized to the interval [0,1].
5.2.3 Localization
A pose quality measure ql can now be obtained by integrating the likelihood of the individual sensor measurements bi ∈ B – which define places where we are seeing
Figure 5.1: This shows the same situation as figure 4.5 right before the correction step.
The perception B from figure 4.2 is transformed to the predicted pose estimatex (green), where the visible points fromBx are rated regarding l. The points’ color indicates their likelihood of being where they are seen, red meaning a high likelihood decreasing towards blue.
white lane markings – into a single likelihood by averaging over them:
ql(Bx) = X
bi∈Bx
l(bi)
|Bx| . (5.2)
Figure 5.1 shows the likelihood of thebi atx right before they are aggregated.
Note that using this quality definition, we are ignoring negative measurements – the places where we do not see any white lines. But the usefulness of that information is dubious anyway, because the CaroloCup track might have had some of its lane markings removed since map creation. Using negative sensor information would require a much more complex sensor model that includes the information we have about these missing lane markings.
It is also noteworthy thatqlis still flat in the target range, becauseF~NNis built from G, which does not actually contain white lines but white areas instead. In a future implementation, this could be improved by using a skeleton of Ginstead of using it directly.
With q defined, we now have everything we need for the correction step, which is given in algorithm 7. Note that the first loop’s body – which contains the expensive Algorithm 7 ParticleFilterPoseCorrection
Require: predicted pose bel(xt) asXt, sensor datazt, mapM as l
B ←line points extracted fromztusing sensor model{in robot-local coordinates}
for xi∈ Xt do
Bxi←B transformed to global coordinates assuming xi as the robot’s pose wi←ql(Bxi)
end for Xt← ∅ forptimesdo
˜
x←sample with returning fromXt weighed byw Xt← Xt∪ {˜x}
end for
return bel(xt) asXt
Herepis the desired number of particles, which can change in any iteration if necessary.
ql operations – is reentrant, so it can be executed in parallel. Consequently, the im-plementation uses all four available cores to support an increased number of particles.
The full particle tracker is shown in algorithm 8, where “truncate” is an operation Algorithm 8 ParticleFilterPoseUpdate
Require: previous pose bel(xt−1) as Xt−1, motion dataut, sensor data zt, mapM as l
bel(xt)←VelocityMotionModel(bel(xt−1),ut)
bel(xt)←ParticleFilterPoseCorrection(bel(xt),zt, M) truncate bel(xt)
return bel(xt) asXt
that drops particles fromXtwhich are very far off. It can, for example, be implemented by predicting the new pose of the most likelyxt−1 using the noiseless velocity motion model and then define particles that are further than a predefined distance away from the resulting pose as “far off”. This is what the current implementation is doing, and the reasoning behind it was explained in section 5.1.
Possibly, the belief over the new pose bel(xt) needs to be converted to some other
Speed [ms] Successful Laps (out of 10)
1.4 10
1.5 7
1.6 0
Table 5.1: The particle tracker stability test. Trials for the speed values lower than 1.4ms are omitted as the tracker never lost the pose there.
representation before it can be used by other components of the software, because a collection of pose hypotheses is not – for example – very handy for motion control.
The current implementation just outputs the most likely pose hypothesis from bel(xt), which can be picked up along the way when calculating the qualities.
Also, the variance in bel(xt) could potentially be used to detect situations where the pose estimate has been lost completely. This, however, has not been implemented.
The current implementation is a vanilla particle filter, which could of course be extended with the usual refinements, such as not resampling in each frame, and using low variance resampling. [Thrun, 109f] This seems especially useful in light of the
“flatness” of the likelihood distribution in the target range.
5.3 Evaluation
Compared to the force field tracking implementation from chapter 4, this particle-based tracker works even better. In addition to localizating the car with centimeter-accuracy, it can handle the removal of parts of the lane markings and can also recover from small tracking errors to a certain degree.
For example, it can handle the previously mentioned white field border line of the underlying soccer field. Instead of losing the pose completely, it then just “jumps” to the correct pose; this kind of incontinuity might have to be smoothed for the motion control to achieve better results.
The stability test from section 4.4 was also done for the particle tracker, and the results are shown in table 5.1. From this we can see that driving with the particle tracker is safe up to 1.4ms. However, the limiting factor here is the speed in the tight curves, because there the pose hypotheses spread over the largest area in pose space.
A similar problem occurs when the car starts meandering at higher speeds, which further complicates localization. When going straight, the car could drive faster, and
Speed [ms] µdistance travelled [m] µpenalty [m] µoverall score [m]
Table 5.2: The driving quality test. Trials for the speed values lower than 1.0ms are omitted as the car never left the lane there. All meter values were rounded down to the nearest integer.
incorporating new improved odometry noise parameters1 into the motion model could further alleviate this problem.
To evaluate the driving behavior itself, another experiment was conducted: the car’s task was to follow the circuit track from the previous experiment for two minutes, and its performance was evaluated according to Carolo-Cup rules [CCRules15] for driving without obstacles. (No lane markings were removed.) The score was determined by taking the overall distance travelled on the track minus 5 penalty meters for each instance of leaving the correct lane (which means that at least two tires were outside).
This was done for constant speeds up to 1.4ms, with three trials each, and the results are shown in table 5.2. This test shows that localization is precise enough for driving.
Almost all of the driving mistakes were made by overshooting in one of the two tight curves in the second half of the circuit, so they could likely be avoided by passing those curves more slowly. Dynamic speed control was one of the motivations for using localization in the first place and is currently being implemented as part of motion control.
The downside of the particle filter is that the ability to track multiple hypotheses comes at the cost of considerable additional computational load. Even using all cores, the number of particles is limited to around 1000 particles before the framerate starts to deteriorate. This may sound like a lot, but because the odometry readings – especially from the gyroscope – are quite noisy, and we need to track the car in the centimeter range to enable driving directly with localization, we have to pay the price here.
Although this implementation can recover from smaller errors, like the force field approach it is still fundamentally a tracker, so it can neither recover from total failures
1Daniel Krakowczyk recently managed to greatly improve the accuracy of the speed measurements, but the impact on motion prediction, and therefore on localization, could not be fully assessed yet. It seems likely that the required number of particles will decrease significantly. This requires the motion model’s noise parameters to be readjusted.
nor find its initial pose on its own. Nevertheless, for our flat-featured model track it works quite well.
6 Conclusion
In summary, we have achieved online localization for the autonomous model car in the described model setting. The advantage of using global navigation at all for this application is that it is much more flexible, and easier to define behavior for, than using local methods only.
Two methods for tracking the car were described, each using odometry information for a first estimate and then correcting its inherent uncertainty with with sensor data from an omnidirectional camera:
• An approach based on force fields that is very fast, but can only track a single pose hypothesis, making otherwise small estimation errors potentially critical.
• Another approach using a particle filter to make tracking more robust at the cost of higher computational requirements.
Both methods are accurate enough that the resulting pose estimate can be used directly for driving, even though odometry is extremely noisy and the camera has a very limited range. They are also stable enough that the car can drive autonomously for several hours.
However, although the results look promising, the implementations have only been tested on a laboratory track. Adapting them to the longer and more feature-less tournament track might require some tweaks:
• The recent improvements on the odometry need to be incorporated into the particle filter’s motion model by adjusting its noise parameters.
• Image data from the new front-facing stereo camera could be integrated into the sensor model to extend the forward vision range, improving stability especially on long, straight track segments.
Also, some general improvements to the particle tracker are possible. For example, introducing low variance resampling is an obvious extension to any particle filter to improve its robustness, as is not resampling in every time frame. Another possibility
is sacrificing accuracy to achieve global localization, which could then be used for behavior control while an accurate local lane driving strategy keeps the car going.
It would also be interesting to cut out the tedious manual mapping process by using SLAM. This can for example be achieved by recording motion and image data and then building the map using offline SLAM on a different machine. FastSLAM with occupancy grid mapping appears promising here, and it also fits well with the current particle filter approach. Because of the good parallelizability of that algorithm, it could even be distributed throughout multiple compute nodes.
In other words, there is still a lot to be done.
List of Algorithms
1 PoseUpdate . . . 16
2 ForceFieldPoseCorrection. . . 25
3 NoiselessVelocityMotionModel . . . 27
4 ForceFieldPoseUpdate . . . 33
5 SampleFromTriangular . . . 41
6 VelocityMotionModel. . . 42
7 ParticleFilterPoseCorrection . . . 44
8 ParticleFilterPoseUpdate . . . 44
Bibliography
[Prewitt] J. M. S. Prewitt
Object Enhancement and Extraction.
Picture Processing and Psychopictorics January 1970
[Thrun] Sebastian Thrun, Wolfram Burgard and Dieter Fox
Probabilistic Robotics (Intelligent Robotics and Autonomous Agents).
ISBN 0262201623 The MIT Press 2005
[Hardware] Jan Frederik Boldt and Severin Junker
Evaluation von Methoden zur Umfelderkennung mit Hilfe omnidirektionaler Kameras am Beispiel eines Modellfahrzeugs.
Freie Universit¨at Berlin, Institut f¨ur Informatik December 2012
[ForceField] Felix von Hundelshausen, Michael Schreiber, Fabian Wiesel, Achim Liers and Ra´ul Rojas
MATRIX: A force field pattern matching method for mobile robots.
Technical Report B-08-03
Freie Universit¨at Berlin, Institut f¨ur Informatik August 2003
[Kumar] Nitesh Kumar
Localization and Mapping of Autonomous Car for Carolo Cup.
Thesis Report
International Institute of Information Technology Bangalore June 2013
[CCRules15] Carolo-Cup Regelwerk 2015.
https://wiki.ifr.ing.tu-bs.de/carolocup/system/files/
Hauptwettbewerb2015.pdf
Technische Universit¨at Braunschweig 03.06.2014