- Text
- Intensity,
- Localization,
- Laser,
- Urban,
- Probabilistic,
- Autonomous,
- Maps,
- Calibration,
- Dynamic,
- Sensor,
- Robust,
- Environments,
- Freiburg.de

Robust Vehicle Localization in Urban Environments Using ...

(a) Average **in**frared reflectivity. (b) Standard deviation of **in**frared reflectivity values. Fig. 4. The two channels of our probabilistic maps. In (a) we see the average **in**frared reflectivity, of brightness, of each cell. The **in**novation of this paper is to also consider (b), the extent to which the brightness of each cell varies. Note that the trails of the pass**in**g vehicles are much more prom**in**ent **in** (b). is later used for localization, **in**tensity values from **in**com**in**g sensor data that do not match those **in** the map will not be overly punished. It is also **in**terest**in**g to note that, while it appears at first glance that the lane mark**in**gs also have high variances, upon closer **in**spection it is clear that it is actually the edges of the mark**in**gs which have high variance. This observation is easily expla**in**ed by the fact that slight sensor miscalibrations and pose errors will cause the cells near a large gradient to have larger range of **in**tensity returns. III. ONLINE LOCALIZATION Once we have built a map of the environment, we can use it to localize the vehicle **in** real time. We represent the likelihood distribution of possible x and y offsets with a 2-dimensional histogram filter. 1 As usual, the filter is comprised of two parts: the motion update, to reduce confidence **in** our estimate based on motion, and the measurement update, to **in**crease confidence **in** our estimate based on sensor data. 1 Although particle filters are a popular alternative method, hav**in**g GPS available allows us to constra**in** our search space to with**in** several meters of the GPS estimate and thus to calculate directly the probability of all possible offsets at a 15-cm cell size. This method confers the significant advantage of not hav**in**g to worry that a particle is miss**in**g near the correct location. Further, the possibility of achiev**in**g accuracy much better than the size of one grid cell would afford us no additional advantage given the other sources of error **in** our system (e.g. sensor miscalibration, controller error, etc.) A. Motion update Our GPS/IMU system reports both **in**ertial updates and a global position estimate at 200Hz. By **in**tegrat**in**g the **in**ertial updates we ma**in**ta**in** a ”smooth coord**in**ate” system which is **in**variant to jumps **in** GPS pose but which, necessarily, diverges arbitrarily over time. Fortunately, because the smooth coord**in**ate system is updated by **in**tegrat**in**g velocities, its offset from the true global coord**in**ate system can be modeled very accurately by a random walk with Gaussian noise. Of course, recover**in**g the offset between the two coord**in**ate frames is equivalent to know**in**g our true global position, so it is this offset that we strive to estimate. As a result, the motion model for our filter is surpris**in**gly simple; rather than need**in**g to model the uncerta**in**ty of the motion of the vehicle itself as is typically done, we need only model the drift between the smooth and global coord**in**ate systems. We note that the car’s motion model is not actually be**in**g ignored; rather, it is used **in**ternally **in** the tightlycoupled GPS/IMU system precisely to m**in**imize the rate at which the smooth and global coord**in**ate systems drift apart. **Vehicle** dynamics are discussed **in** [9]. Because the smooth coord**in**ate system’s drift is modeled as a Gaussian noise variable with zero-mean, the motion model updates the probability of each cell as follows: P(x,y) = η ·∑P(i, j) · exp i, j (− 1 2 (i − x)2 ( j − y) 2 /σ 2 ) where P(x,y) is the posterior probability, after the motion update, that the vehicle is **in** cell (x,y), η is the normaliz**in**g constant, and σ is the parameter describ**in**g the rate of drift of the smooth coord**in**ate system. Although this update is theoretically quadratic **in** the number of cells, and thus quartic **in** the search radius, because the drift rate is relatively low and the update frequency can be arbitrarily high, it is **in** practice perfectly acceptable to only consider consider neighbor**in**g cells with a distance of two or three from the cell to be updated. For **in**stance, the probability that the smooth coord**in**ate frame drifts more than 45cm **in** .1 seconds is vanish**in**gly small, even though such jumps are relatively common for the global GPS estimate. We process the motion update at a rate proportional to the speed of the vehicle, as the expected drift **in** the smooth coord**in**ate system is roughly proportional to the magnitude of the vehicle’s velocity. B. Measurement update The second component of the histogram filter is the measurement update, **in** which **in**com**in**g laser scans are used to ref**in**e the vehicle’s position estimate. The way **in** which we process **in**com**in**g laser scans is identical to the mapp**in**g process described **in** the previous section. That is, rather than treat**in**g every laser return as its own observation, we **in**stead build a roll**in**g grid from accumlated sensor data **in** the exact same form as our map. This method enables us to directly compare cells from our sensor data to cells from the map, and avoids overweight**in**g (2) 4375

cells which have a high number of returns (e.g. trees and large dynamic obstacles). If z is our sensor data and m is our map, and x and y are possible offsets from the GPS pose, then Bayes’ Rule gives: P(x,y|z,m) = η · P(z|x,y,m) · P(x,y) (3) We may approximate the uncerta**in**ty of the GPS/IMU pose estimate by a Gaussian with variance σGPS 2 , so we may estimate P(x,y) simply as a product of the GPS Gaussian and the posterior belief after the motion update: ( x 2 + y 2 ) P(x,y) = η · exp · P(x,y) (4) −2σ 2 GPS To calculate the probability of sensor data z given an offset (x,y) and map m, we take the product over all cells of the probability of observ**in**g the sensor data cell’s average **in**tensity given the map cell’s average **in**tensity and both of their variances. This value is then raised to an exponent α < 1 to account for the fact that the data are likely not entirely **in**dependent, for example due to systemic calibration errors or m**in**or structural changes **in** the environment which are measured **in** multiple frames. [7] Let us call the two-dimensional arrays of the standard deviations of the **in**tensity values **in** the map and sensor data m σ and z σ , respectively. Then, for example, the standard deviation of the **in**tensity values **in** the map cell .45m east and 1.2m north of the GPS estimate would be expressed as m σ(.45,1.2) . We will use the same notation for the average **in**tensity value, with r denot**in**g the average **in**tensity (reflectivity) of the cell. Aga**in**, to use the same example, the average **in**tensity seen **in** the map at the cell .45m east and 1.2 north of the GPS estimate would be expressed as m r(.45,1.2) . Thus, we have: ( −(mr(i−x, j−y) − z r(i, j) ) 2 ) α P(z|x,y,m) = ∏exp i, j 2(m σ(i−x, j−y) + z σ(i, j) ) 2 (5) Putt**in**g it all together, we obta**in**: P(x,y|z,m) = η ·∏ ( −(mr(i−x, j−y) − z r(i, j) ) 2 ) α exp i, j 2(m σ(i−x, j−y) + z σ(i, j) ) 2 ( x 2 + y 2 ) ·exp · P(x,y) (6) −2σ 2 GPS Towards the end of achiev**in**g robustness to partially outdated maps, we further impose a m**in**imum on the comb**in**ed standard deviation for the **in**tensity values of the map and sensor data. This implicitly accounts for the not-unlikely phenomenon that an environment change s**in**ce the map acquisition simultaneously enabled a low variance **in** both the map and the sensor data, yet with the two show**in**g significantly different **in**tensity values. For computational reasons we restrict the computation of P(x,y|z,m) to cells with**in** several meters of the GPS estimate; however, this search radius could easily be **in**creased if a less accurate GPS system were used. (a) GPS localization **in**duces ≥1 meter of error. (b) No noticeable error after localization. Fig. 5. Incom**in**g laser scans (grayscale) superimposed on map (gold). (a) GPS localization is prone to error, even (as shown here) with a high-end **in**tegrated **in**ertial system and differential GPS us**in**g a nearby stationary antenna. (b) With localization there is no noticeable error, even **in** the presence of large dynamic obstacles such as this pass**in**g bus. C. Most likely estimate Given the f**in**al posterior distribution, the last step is to select a s**in**gle x and y offset that best represents our estimation. Tak**in**g the offset to be max x,y P(x,y) is, by def**in**ition, probabilistically optimal at any given **in**stant, but such an approach could add unnecessary danger as part of the pipel**in**e **in** an autonomous vehicle. Because the maximum of a multimodal distribution can easily jump around discont**in**uously, us**in**g that approach may cause the vehicle to oscillate under unfortuitous circumstances, even if the vehicle’s navigation planner performed some variety of smooth**in**g. An alternative approach would be to choose the center of mass of the posterior distribution; this would improve consistency, but would tend to cause the chosen offset to be biased too much towards the center. As a compromise, we use the center of mass with the variation that we raise P(x,y) to some exponent α > 1, as follows: x = ∑ x,y P(x,y) α · x ∑ x,y P(x,y) α y = ∑ x,y P(x,y) α · y ∑ x,y P(x,y) α (7) This (x,y) offset is the f**in**al value which is sent to the vehicle’s navigation planner. While an advanced plann**in**g and decision-mak**in**g algorithm could take advantage of the entire posterior distribution over poses rather than requir**in**g a s**in**gle, unimodal pose estimate, our vehicle’s planner expects 4376

- Page 1 and 2: 2010 IEEE International Conference
- Page 3: eam, so that we have a complete map
- Page 7: that were previously too challengin