Geometric feature based 2 1/2D map building and planning with laser, sonar and tactile sensors

In this paper, a 2 1/2D geometric feature based map is introduced. It is a closed map that integrates laser, sonar range data and bumper tactile data into a multi-layer map under an unified geometric description. We use line segments, circular arcs and point clusters as the basic geometric features. An Extended Kalman Filter (EKF) based on geometric features provides localization and the associated uncertainty. Virtual Line Segment Pursuit strategy is used for systematic exploration. Sensor placement planning is introduced for this Simultaneous Localization and Mapping (SLAM) problem and the motion is planned to improve the localization accuracy and target observability. Experimental results are shown to verify the effectiveness of the proposed scheme.