A Kalman-filtering-based Approach for Improving Terrain Mapping in off-road Autonomous Vehicles

Abstract

The generation of accurate terrain maps while navigating over off-road, irregular terrains is a complex challenge, due to the difficulties in the estimation of the pose of the laser rangefinders, which is required for the proper registration of the range measurements. This paper addresses this problem. The proposed methodology uses an Extended Kalman filter to estimate in real-time the instantaneous pose of the vehicle and the laser rangefinders by considering measurements acquired from an inertial measurement unit, internal sensorial data of the vehicle and the estimated heights of the four wheels, which are obtained from the terrain map and allow determination of the vehicle's inclination. The estimated 6D pose of the laser rangefinders is used to correctly project the laser measurements onto the terrain map. The terrain map is a 2.5D map that stores in each cell the mean value and variance of the terrain height. In each map's cell position, new laser observations are fused with existing height estimations using a Kalman filter. The proposed methodology is validated in the real world using an autonomous vehicle. Field trials show that the use of the proposed state estimation methodology produces maps with much higher accuracy than the standard approaches.