PaperlayerView exact revision on arXiv

Source-linked AI summary

Robust and Precise Vehicle Localization based on Multi-sensor Fusion in Diverse City Scenes

Guowei Wan, Xiaolong Yang, Renlan Cai, Hao Li, Hao Wang, Shiyu Song

arXiv:1711.05805v2cs.CVcs.RO

TL;DR

Vehicle localization must remain accurate despite complementary sensors having different failure conditions. The paper presents adaptive GNSS, LiDAR, and IMU fusion with intensity-and-altitude LiDAR cues, achieving 5-10cm RMS accuracy across challenging scenes and deployment in crowded city streets.

  • Problem

    GNSS, LiDAR, and IMU each have scene-dependent limitations, motivating robust and precise localization across urban downtown, highways, and tunnels.

  • Method

    The system adaptively fuses GNSS, LiDAR, and IMU through an error-state Kalman filter, using LiDAR intensity and altitude cues for localization.

  • Results

    5-10cm RMS accuracy was achieved longitudinally and laterally across challenging scenes, with robust centimeter-level results in regular and weak GNSS scenarios.

  • Takeaways & Limitations

    Deployed in a large autonomous driving fleet, the system supported fully autonomous driving in crowded city streets and varied challenging scenes.

Abstract

from arXiv · show

We present a robust and precise localization system that achieves centimeter-level localization accuracy in disparate city scenes. Our system adaptively uses information from complementary sensors such as GNSS, LiDAR, and IMU to achieve high localization accuracy and resilience in challenging scenes, such as urban downtown, highways, and tunnels. Rather than relying only on LiDAR intensity or 3D geometry, we make innovative use of LiDAR intensity and altitude cues to significantly improve localization system accuracy and robustness. Our GNSS RTK module utilizes the help of the multi-sensor fusion framework and achieves a better ambiguity resolution success rate. An error-state Kalman filter is applied to fuse the localization measurements from different sources with novel uncertainty estimation. We validate, in detail, the effectiveness of our approaches, achieving 5-10cm RMS accuracy and outperforming previous state-of-the-art systems. Importantly, our system, while deployed in a large autonomous driving fleet, made our vehicles fully autonomous in crowded city streets despite road construction that occurred from time to time. A dataset including more than 60 km real traffic driving in various urban roads is used to comprehensively test our system.

I. INTRODUCTION

The paper proposes an adaptive LiDAR–RTK–IMU fusion system for precise, resilient vehicle localization across complex driving scenes. It combines complementary sensor strengths and introduces LiDAR intensity-and-altitude localization improvements for challenging conditions.

  • GNSS RTK provides centimeter positioning but is vulnerable to signal blockage and multipath, while IMU dead reckoning suffers from integration drift.
  • The proposed framework adaptively fuses LiDAR, RTK, and IMU measurements to provide stable localization across downtown, tunnels, tree-lined roads, parking garages, and highways.The system was evaluated using over 60 km of data in dynamic urban and highway scenes.
  • The LiDAR module combines intensity and altitude cues to improve robustness during challenging situations such as road construction.
  • The system was tested in crowded urban streets and challenging scenes, supporting fully autonomous vehicle operation in those conditions.
  • Unlike prior LiDAR-only approaches, the framework uses RTK as a complementary localization source, particularly in open spaces and streets with road construction.

III. LIDAR MAP GENERATION

The LiDAR localization module represents mapped environments using laser intensity and altitude statistics, then estimates heading and horizontal position separately. This design reduces the search burden while supporting registration against sparse online scans.

  • Each pre-built map grid cell stores statistics of laser reflection intensity and altitude, modeled with a single Gaussian distribution.
  • The LiDAR module estimates the vehicle state as 2D position, altitude, and heading, using road-surface altitude from the pre-built map.
  • Heading is estimated separately with the Lucas-Kanade algorithm, while a histogram filter estimates horizontal position.
  • The localization algorithm transforms a sparse online point cloud, evaluates candidate positions using intensity and altitude likelihoods, and returns the best registration with covariance.
  • Lucas-Kanade matching uses projected online intensity images and was verified to converge to sub-degree heading solutions, while its horizontal translation estimate was not sufficiently robust.

B. Horizontal Localization

Horizontal localization uses a histogram filter that combines motion prediction with map-matching likelihoods from LiDAR intensity and altitude. An adaptive weight balances these cues according to their distributional uncertainty.

  • B. Horizontal Localization: The histogram filter estimates the horizontal state (x, y) by discretizing the state space and exhaustively evaluating posterior probabilities.Its prediction and measurement-update phases correspond to Equations 3 and 4.
  • 1) The Prediction Step:: The prediction step updates each cell using motion from the SINS and a Gaussian-noise random walk.This allows the filter to propagate belief between frames while modeling drift with σ.
  • 2) The Update Step:: The measurement likelihood matches the online point cloud to the pre-built map and combines intensity and altitude probabilities with dynamic weight γ.The fused likelihood is P(z|x, y, m) = η · P(zr|x, y, m)^γ · P(za|x, y, m)^(1−γ), where zr and za are intensity and altitude measurements.
  • 2) The Update Step:: Intensity similarity uses variance-weighted SSD, while altitude similarity uses SSD without a variance term because altitude varies vertically by nature.The variance term reduces the influence of environmental changes in the intensity comparison.
  • 2) The Update Step:: The adaptive weight γ is determined from the variances of the intensity and altitude probability distributions to balance their influence with the prediction.The KL divergence is used to balance the prediction and measurement distributions.

3) Optimal Offset:

The method selects a compact search region around the strongest posterior hypothesis before estimating the optimal horizontal offset. A second strong, more central hypothesis can determine the region instead.

  • 3) Optimal Offset:: The optimal offset is estimated from the posterior distribution of the histogram filter rather than from every state.The method restricts computation to a small squared area around the strongest hypotheses.
  • 3) Optimal Offset:: The search region is centered on the second-largest posterior state when it reaches a specified ratio of the largest belief and is closer to the histogram center.Otherwise, the largest-posterior state defines the center of the small area.
  • 3) Optimal Offset:: Within the selected area Z, the method calculates the optimal offset (ˆx, ˆy).

4) Uncertainty Estimation:

The system estimates uncertainty for the localization result through a covariance matrix associated with the optimized offset. Experimental filter distributions show consistency between this covariance estimate and observed ground-truth error.

  • 4) Uncertainty Estimation:: The optimized offset and the offset of each (x, y) cell define the quantities used to compute the covariance matrix Cxy.
  • 4) Uncertainty Estimation:: The covariance matrix represents uncertainty associated with the state estimates used to update the Kalman filter.
  • 4) Uncertainty Estimation:: In Figure 6, the estimated covariance matrix is consistent with the observed error compared with ground truth.The figure presents filter distributions, covariance estimates, and state estimates.

V. GNSS BASED LOCALIZATION

The GNSS module aids RTK ambiguity resolution with multi-sensor predictions and uncertainty-aware Kalman-filter updates, while handling degraded observations and cycle slips.

  • RTK observation model: RTK forms single-differenced pseudo-range and carrier-phase observations between rover and base receivers.The model includes rover-position corrections, relative receiver-clock offsets, satellite geometry, ambiguities, and phase or range noise.
  • Ambiguity resolution: Using GPS, BeiDou, and GLONASS observations with estimated GLONASS inter-frequency bias improves ambiguity resolution inputs before MLAMBDA fixing.Least-squares float ambiguities are transformed into double-differenced ambiguities with integer nature, then resolved with MLAMBDA.
  • Degraded GNSS conditions: Sub-meter positioning remains available through floated RTK or code-based differential GNSS when multipath and signal blockage prevent ambiguity resolution.Such degradation is reported under urban buildings or in forests.
  • INS-aided ambiguity resolution: LiDAR-constrained INS provides position predictions that narrow ambiguity search after GNSS outages, including tunnel passages until signals return.The framework uses LiDAR to constrain INS errors during the outage and supports subsequent ambiguity resolution.
  • Fusion coupling: The framework currently loosely couples sensor observations by treating INS integration as a virtual position observation with variance Rins.Tightly coupled fusion is identified as future work.
  • Cycle-slip handling: Cycle slips caused by lost GNSS signal tracking create carrier-phase discontinuities and require detection and repair in obstructed urban environments.Urban multipath and signal obstruction make cycle slips more frequent than in static open conditions.

VI. SENSOR FUSION

The sensor-fusion framework combines IMU integration with RTK and LiDAR measurements through an error-state Kalman filter to maintain accurate state estimates between external updates.

  • Fusion framework: An error-state Kalman filter fuses IMU measurements with LiDAR and RTK localization measurements.The filter combines IMU orientation-rate and accelerometer information with external localization updates.
  • Fusion framework: IMU integration supplies robust state estimates between LiDAR and RTK measurements.The IMU contributes continuous motion information while external sensors provide updates.
  • Inertial navigation: SINS estimates position, velocity, and attitude by integrating IMU data in an east-north-up navigation frame and right-forward-up body frame.The formulation also uses earth and inertial reference frames.
  • Inertial navigation: The SINS model represents vehicle position, velocity, attitude, gravity, inertial biases, and IMU gyroscope and accelerometer outputs.Quaternion and direction-cosine representations describe attitude transformations between body and navigation frames.
  • Inertial navigation: The integration model includes frame-rate definitions, quaternion multiplication, and curvature transformations from velocity to longitude, latitude, and altitude.The curvature matrix uses transverse and meridian radii.
  • Error modeling: For tactical-grade MEMS IMUs, biases are modeled as constant values and corrected through an error-state Kalman filter when combined with RTK or LiDAR.The filter uses a SINS error model for velocity, position, and attitude errors.

B. Filter State Equation

The filter state equation models SINS error growth and system noise so estimated error states can correct the inertial navigation solution.

  • Filter state equation: Because SINS error grows over time, the filter estimates error states and uses them to correct the SINS solution.This correction supports precise position, velocity, and attitude estimates.
  • Filter state equation: The attitude-error model uses a small-angle approximation around zero.This assumption makes the attitude error suitable for the error-state formulation.
  • Filter state equation: The Kalman-filter state variables include position, velocity, and attitude-related SINS quantities.The supplied state expression is presented as X.
  • Filter state equation: The state equation is derived from the SINS error equation and the IMU model.The resulting formulation supplies the filter's process model.
  • Filter state equation: System noise comprises IMU output noise and IMU bias noise.
  • Filter state equation: The matrices F(X) and G(X) describe the state and noise mappings, with vector cross products represented by skew-symmetric matrices.

C. Filter Measurement Update Equation

The measurement-update stage uses LiDAR and GNSS corrections to update state uncertainty, with LiDAR providing position and heading measurements against the SINS prediction.

  • Measurement update: The measurement update has LiDAR and GNSS components that update state uncertainty using a global correction Z.Other time and measurement updates follow the standard Kalman filter.
  • LiDAR update: LiDAR localization supplies vehicle position and heading angle as filter measurements.
  • LiDAR update: The LiDAR update compares SINS predictions with LiDAR measurements and models LiDAR estimation noise with covariance RL.The longitude and latitude portions of RL are computed from the LiDAR model, while altitude and heading portions are set constant.
  • LiDAR update: The LiDAR measurement matrix HL is constructed from the navigation-frame direction-cosine matrix elements cij.

2) GNSS Measurement Update Equation:

The GNSS measurement update integrates delayed and disordered measurements into the localization filter while using real-world testing to assess performance against a prior method.

  • The GNSS measurement update incorporates GNSS estimation noise and measurement models into the filter state update.
  • Delayed measurements are processed with a second filter and chronological buffers containing filter states, measurements, and IMU data.
  • The delayed-measurement procedure updates the state at the measurement time, replays buffered IMU data, and handles measurements received out of order.
  • Ground-truth trajectories are generated using GNSS/INS post-processing in open spaces and map reconstruction techniques in weak-GNSS scenarios.
  • The system is evaluated in regular and weak-GNSS roads using LiDAR + IMU and LiDAR + GNSS + IMU configurations.
  • The proposed solution achieves better lateral and longitudinal performance than consistently over time.

B. Qualitative Analysis

Qualitative and component analyses examine sensor behavior, LiDAR improvements, and deployment scope, while noting that GNSS-denied roads lack quantitative ground truth.

  • B. Qualitative Analysis: GNSS-denied roads receive qualitative rather than quantitative comparison because ground truth is unavailable.
  • B. Qualitative Analysis: The fused system remains effective when LiDAR fails from an outdated map or RTK becomes unstable because of signal blockage.
  • B. Qualitative Analysis: Crowded scenes with people or cars do not prevent the system from performing well.
  • C. Detailed Analysis of LiDAR: Altitude cues improve LiDAR localization on a newly paved road with a recently built wall where intensity-only matching fails.
  • C. Detailed Analysis of LiDAR: Heading refinement, altitude cues, and adaptive weights each improve localization errors, with adaptive weighting increasing robustness to environmental changes.
  • C. Detailed Analysis of LiDAR: The system has two hardware implementations with different computing requirements while providing similar localization results.
  • VIII. CONCLUSION AND FUTURE WORK: The complete system achieves 5-10cm RMS accuracy longitudinally and laterally across challenging scenes and is deployed in a large autonomous driving fleet.
Loading 1711.05805v2…