Vehicle Localization and Control on Roads with Prior Grade Map

Roya Firoozi, Jacopo Guanetti, Roberto Horowitz, Francesco Borrelli

TL;DR

This paper introduces a novel vehicle localization method that leverages a pre-existing road grade map to accurately estimate a vehicle's longitudinal position, especially in GPS-denied environments. This enhanced localization feeds into a hierarchical control system that optimizes energy efficiency by planning an energy-optimal velocity trajectory based on road grade and traffic conditions.

What Problem Does This Paper Address?

Accurate vehicle localization is fundamental for autonomous driving systems, enabling crucial functions like navigation, motion planning, and real-time control. While GPS is a common localization tool, its reliability is compromised in environments like tunnels, urban canyons, or under dense foliage due to signal degradation. Traditional dead-reckoning methods, which estimate position based on previous states, suffer from cumulative errors over time. Existing map-aided localization approaches often rely on rich environmental features (like lane markings or buildings) which may not be consistently available, especially in rural or suburban areas. This paper addresses the need for robust and accurate localization in GPS-denied scenarios by proposing a method that utilizes a road grade map, a feature more universally present on roads.

Methods

The core of the paper is a map-aided localization approach using an Extended Kalman Filter (EKF). The EKF fuses on-board sensor data (wheel speed encoders for velocity, accelerometers for acceleration) with information from a pre-generated road grade map.

  • Road Grade Map Generation: The authors describe generating this map using high-precision RTK GPS or by querying APIs like Google Elevation API, which provide elevation data for specific coordinates. This data is then processed to derive the road's grade (slope).
  • EKF Localization:
    • Process Model: A point-mass vehicle model is used, with states being longitudinal position ($s$) and velocity ($v$). The model incorporates the vehicle's acceleration, accounting for gravity's effect on inclined roads using the road grade map.
  • Measurement Model: Vehicle speed is measured by wheel speed encoders, and inclination is indirectly derived from accelerometer and wheel speed encoder data.
  • Sensor Noise Filtering: Inertial sensor (IMU) noise is reduced with a low-pass filter, and bias drift is modeled and compensated for.
  • Hierarchical Control System:
    • High-Level Planner (Dynamic Programming): This component plans an energy-optimal velocity trajectory offline. It converts the vehicle's longitudinal dynamics from the time domain to the spatial domain to incorporate position-dependent information like road grade and traffic flow (used as velocity bounds). Dynamic Programming is employed to solve this optimization problem, minimizing power consumption while respecting trip time constraints.
  • Low-Level Controller (Model Predictive Control - MPC): This real-time controller tracks the velocity trajectory generated by the high-level planner. It uses the same vehicle dynamics model and incorporates road grade knowledge to optimize control inputs and minimize energy consumption.

Datasets

  • Road Grade Map: Generated using data from RTK GPS, Google Elevation API, and the National Elevation Dataset (NED). For experimental validation, a map was created using GPS and Google Elevation API for a specific route near UC Berkeley.
  • Traffic Flow Data: Obtained from HERE API for a selected urban route.
  • Simulation Data: Generated using simulated road altitude maps and noisy sensor measurements.
  • Experimental Data: Collected from a test vehicle (Hyundai Genesis) equipped with an OTS RT2002 sensing system (high-precision GPS and IMU) on a hilly road near UC Berkeley.

Key Insights and Ideas

  • Road Grade as a Localization Cue: The paper demonstrates that road grade, often overlooked, can be a powerful and widely available feature for vehicle localization, especially when GPS is unavailable. This is a significant departure from methods relying on more sparse or context-dependent environmental features.
  • Synergy between Localization and Control: The research highlights how accurate, map-aware localization directly benefits energy-efficient control. By knowing its precise position relative to the road grade map, the vehicle can anticipate upcoming inclines and optimize its velocity trajectory for maximum energy savings.
  • Hierarchical Planning for Energy Efficiency: The separation into a high-level planner (DP) for offline trajectory optimization and a low-level controller (MPC) for real-time tracking allows for sophisticated energy management that considers both route geometry and traffic conditions.
  • Impact of Localization Errors on Energy: The paper quantifies how even small localization errors can lead to significant deviations in energy consumption, underscoring the critical importance of accurate localization for energy-efficient autonomous driving.

Key Results

  • Localization Accuracy: In simulations, the EKF-based localization achieved an average Root Mean Square Error (RMSE) of 0.14 m, significantly outperforming simple velocity integration which had an average RMSE of 0.83 m.
  • Experimental Localization: In real-world experiments, the EKF method achieved an average position RMSE of 5.8 m, compared to 21.4 m for velocity integration. The final position error for velocity integration was observed to be up to 30 times larger than the EKF estimate.
  • Energy Efficiency: The optimized velocity profile generated by the DP planner resulted in a 47.8% reduction in energy consumption compared to using a traffic flow velocity profile, albeit with a 26.0% increase in trip time (for a specific parameter setting).
  • Impact of Localization Error: A 0.5% localization error (equivalent to 60 meters off position after 12 km) was shown to affect energy efficiency by approximately 15%.

Limitations and Future Work

  • Limitations:
    • The proposed localization method is primarily restricted to hilly roads where road grade is a meaningful feature.
  • The accuracy of the road grade map directly impacts localization performance.
  • The paper assumes a constant bias drift for inertial sensors, which might not hold true in all scenarios.
  • Future Work:
    • The authors suggest combining this method with other localization techniques to further improve performance.
  • Investigating more sophisticated bias drift modeling for inertial sensors could enhance robustness.
  • Extending the approach to handle more complex road geometries and features beyond just grade.

Summary updated

Abstract

We propose a map-aided vehicle localization method for GPS-denied environments. This approach exploits prior knowledge of the road grade map and vehicle on-board sensor measurements to accurately estimate the longitudinal position of the vehicle. Real-time localization is crucial to systems that utilize position-dependent information for planning and control. We validate the effectiveness of the localization method on a hierarchical control system. The higher level planner optimizes the vehicle velocity to minimize the energy consumption for a given route by employing traffic condition and road grade data. The lower level is a cruise control system that tracks the position-dependent optimal reference velocity. Performance of the proposed localization algorithm is evaluated using both simulations and experiments.
BibTeX
@inproceedings{firoozi2018vehicle,
  author = {Roya Firoozi and Jacopo Guanetti and Roberto Horowitz and Francesco Borrelli},
  title = {{Vehicle Localization and Control on Roads with Prior Grade Map}},
  booktitle = {CDC},
  year = {2018},
  url = {https://openreview.net/forum?id=m3VgMFOWVS0},
}
Figure