Real time 3D terrain elevation mapping using ants Optimization Algorithm and stereo vision

Reliable perception of terrain slope and terrain traversability is a key-feature for any off-road unmanned ground vehicle, as well as for any Driver Assistance Systems designed to work in extreme environments, like mining. In this paper we want to present an innovative technique to build a 3D elevation map of the traversable terrain from a world´s 3D dense data set, in real time. The 3D points are grouped into lateral and longitudinal equally spaced slices, then they are projected onto the corresponding slices´ reference planes. The projections are then analyzed by a biologically inspired Optimization Algorithm able to segment points into terrains inlier and outlier; the resulting 2D terrain slopes represent an optimal terrain approximation along each slice. Finally, the 2D approximations are merged together, to create the overall 3D terrain surface. The algorithm has been successfully tested with 3D data provided by a stereo camera system mounted on a Cat^® wheel loader operating in a mining environment.