Vehicle localization in outdoor mountainous forested paths and extension of two-dimensional road centerline maps to three-dimensional maps

This paper presents an approach for vehicle three-dimensional (3-D) localization in outdoor woodland environments where a previously available two-dimensional road centerline map is used in combination with a loosely coupled multi-sensor system to estimate the vehicle position in mountainous forested paths. The localization system is composed of a wheel encoder, an inertial measurement unit, a DGPS, a laser sensor and a barometer. An extended Kalman filter is used for sensor data fusion and pose estimation. When available, DGPS is used for 3-D dead reckoning accumulated error correction. During DGPS blackouts, the laser sensor is used for road extraction and measurement of the displacement of the vehicle to the road centerline, then the position is corrected towards the map. Moreover, the barometer that measures the height difference towards a reference is used to correct the estimated height in absence of DGPS 3-D data. The estimated height is added to the available road map to obtain a 3-D road centerline map that includes the road width measured with the laser sensor. Experimental results in large-scale real mountainous woodland environments show the robustness and simplicity of the proposed approach for vehicle localization and 3-D map extension.