Robust Vehicle Localization in Urban Environments Using ...

1 Robust Vehicle Localization in UrbanEnvironments Using probabilistic MapsJesse Levinson, Sebastian ThrunStanford Artificial Intelligence Autonomous Vehicle navigation in dynamic urbanenvironments requires Localization accuracy exceeding thatavailable from GPS-based inertial guidance systems. We haveshown previously that GPS, IMU, and LIDAR data can beused to generate a high-resolution infrared remittance groundmap that can be subsequently used for Localization [4]. Wenow propose an extension to this approach that yields substan-tial improvements over previous work in Vehicle Localization ,including higher precision, the ability to learn and improvemaps over time, and increased robustness to environmentchanges and dynamic obstacles. Specifically, we model theenvironment, instead of as a spatial grid of fixed infraredremittance values, as a probabilistic grid whereby every cellis represented as its own gaussian distribution over remittancevalues.

2 Subsequently, Bayesian inference is able to preferentiallyweight parts of the map most likely to be stationary andof consistent angular reflectivity, thereby reducing uncertaintyand catastrophic errors. Furthermore, by Using offline SLAMto align multiple passes of the same environment , possiblyseparated in time by days or even months, it is possible tobuild an increasingly Robust understanding of the world thatcan be then exploited for validate the effectiveness of our approach by Using thesealgorithms to localize our Vehicle against probabilistic mapsin various dynamic Environments , achieving RMS accuracyin the 10cm-range and thus outperforming previous , this approach has enabled us to autonomouslydrive our Vehicle for hundreds of miles in dense traffic onnarrow Urban roads which were formerly unnavigable withprevious Localization INTRODUCTIONI nterest in autonomous vehicles and advanced driver as-sistance systems continues to increase rapidly.

3 In particular,the DARPA Grand Challenges of 2004 and 2005, in whichvehicles competed to autonomously navigate through desertterrain, and the DARPA Urban Challenge of 2007, in whichvehicles competed to autonomously navigate through a mockurban environment amidst other traffic, have generated con-siderable enthusiasm and research interest in the field ofautonomous driving [15], [16].The 2007 Urban Challenge was the first significant demon-stration of vehicles driving themselves through a city-likeenvironment. As important as this result was, many simpli-fications were made for the purposes of the competition,and the course itself, while more difficult in many waysthan previous challenges, featured roads wide enough toaccomodate even military-sized into autonomous driving in real environmentshas been ongoing for many years, but much of it has focusedFig.

4 Autonomous Vehicle . Velodyne HD-LIDAR 64-beam scanneris circled in specific Environments ( highways with obvious lanemarkers) [3]. Some positive results have been achieved formap-based driving, though significant limitations order for a Vehicle to handle a variety of Environments ,including ones with dense traffic, it must be able to localizeitself in such situations without relying on particular patternsor features. In fact, in order to enable autonomous driving,alocalization system should be able to handle situations wherethe environment has changed since the map was this paper we present a new method of map-baseddriving that extends previous work by considering maps asprobability distributions over environment properties ratherthan as fixed representations of the environment at a snapshotin time. As in [4], we build infrared reflectivity maps of theenvironment and align overlapping portions of the same ordisparate trajectories with GraphSLAM, Using similar offlinerelaxation techniques to recent SLAM methods[2], [1],[5], [6], [8].

5 However, by extending the format of the mapto encapsulate the probabilistic nature of the environment ,we are able to represent the world more accurately andlocalize with fewer errors. In particular, previous methodsoften use a binary classification for deciding whether or notto incorporate a piece of evidence into a map. That is, asensor reading such as a laser scan is either assumed to be alegitimate part of the map, or it is assumed to be spurious andtherefore ignored. In [4], exactly this approach was used;any scans that were thought to be vertical according to abinary classification were thrown , instead of having to explicitly decide whethereach measurement either is or is not part of the static envi-ronment, we propose as an alternative considering the sumof all observed data and modeling the variances observedin each part of the map.

6 This new approach has severaladvantages compared to the non- probabilistic , while much research incorporating laser remission datahas assumed surfaces to be equally reflective at any angle ofincidence, this approximation is often quite poor [10], [11],[12]. Whereas Lambertian and retroreflective surfaces havethe fortuitous property that remissions are relatively invariantto angle of incidence, angular-reflective surfaces such asshiny objects yield vastly different returns from differentpositions. Instead of ignoring these differences, which canlead to Localization errors, we now implicitly account further advantage of our proposed method is an in-creased robustness to dynamic obstacles; by modeling dis-tributions of reflectivity observations in the map, dynamicobstacles are automatically discounted in Localization viatheir trails in the map.

7 Finally, in addition to capturing moreinformation about the environment , our approach enables aremarkably straightforward probabilistic interpretation of themeasurement model used in Vehicle is equipped with an Applanix LV-420 tightlycoupled GPS/IMU system that provides both intertial updatesand global position estimates at 200 Hz. The environment issensed by a Velodyne HD-LIDAR laser rangefinder with 64separate beams; the entire unit spins at 10 Hz and providesapproximately one million 3-D points and associated infraredintensity values per second. This vast quantity of data ensuresthat most map cells are hit multiple times, thereby enablingthe computation of intensity variances on a per-cell first present the details of our mapping algorithm,including a novel unsupervised laser calibration routine,andthen explain how the localizer uses incoming data and a prob-abilistic map to localize the Vehicle .

8 Finally, we show resultsfrom several experiments that demonstrate the accuracy androbustness of our PROBABILISTICMAPSOur ultimate goal in building a map is to obtain a grid-cellrepresentation of the observed environment in which eachcell stores both the average infrared reflectivity observedatthat location as well as the variance of those values. Wegenerate such a map in three steps: first, we post-processall trajectories so that areas of overlap are brought intoalignment; second, we calibrate the intensity returns of eachlaser beam so that the beams have similar response curves;and finally, we project the calibrated laser returns from thealigned trajectories into a high-resolution probabilistic of these steps is described in detail Map alignment Using GraphSLAMG iven one or more logfiles containing GPS, inertial, andlaser data, we wish to refine the trajectories in order tobring areas of overlap into alignment.

9 Specifically, whenthere exist sections of the logfiles that are spatially near buttemporally separated, the Vehicle s pose in these sectionsFig. infrared refectivity map of a large Urban block showing theaverage reflectivity of each 15x15cm cell. Many features arevisible in be corrected so that the two sections align task has been successfully accomplished in previouswork [4]. Briefly, GraphSLAM is used to optimize an ob-jective function in which adjacent Vehicle poses are linkedby inertial and odometry data, Vehicle poses are linked totheir estimated global position, and matched sections fromthe logfile ( for loop closure) are linked by their computedalignment this implementation, alignment offsets between matchedposes are computed Using iterative closest point on a clusterof 5 adjacent 360-degree laser scans from each of the twosections; the x, y, z, yaw, pitch, and roll are all a list of matches has been computed, the GraphSLAM objective function is minimized and the Vehicle trajectoriesare updated Laser calibrationBefore we generate our map, it is important to calibratethe separate laser beams so that they respond similarly tothe objects with the same brightness.

10 With a well calibratedlaser, this step can be skipped without effect, but creatingaprobabilistic map from a poorly calibrated laser suffers fromtwo disadvantages: first, the intensity averages for each cellwill depend heavily on which beams happened to hit it; andsecond, the computed intensity variances will significantlyoverstate the reality. In practice, we find that it is not neces-sary to recalibrate our laser every time we use it, but usingthe factory calibration is unquestionably detrimental. Thus inour implementation the following calibration procedure canbe performed only once and the resulting calibration tablecan be used for all further mapping and single 360-degree scan from the uncalibrated laser canbe seen in Fig. 3(a). As is readily apparent, some beamsare generally too bright and others are generally too than compute a single parameter for each beam(which would still an improvement over the uncalibrateddata), we instead compute an entire response curve for everybeam, so that we have a complete mapping function foreach beam from observed intensity to output intensity.