Mobile Robot Hierarchical Simultaneous Localization and Mapping Based on Active Loop Closure Constraint

A hierarchical map representation approach based on active loop closure constraint is proposed to implement mobile robot simultaneous localization and mapping (SLAM) efficiently with the Rao-Blackwellized particle filters (RBPF). The hierarchical map includes the local metric map and the global topological map, and in the global level an active loop closure strategy based on information entropy is proposed to reduce the map uncertainty as well as the robot trajectory uncertainty. The estimation of relative locations between local metric feature maps is maintained with local map alignment algorithm, and a minimization procedure is carried out using the loop closure constraint with backward correction to reduce the uncertainty between local maps. The robot is only equipped with monocular vision and odometer, and the robust observation model is constructed; Scale invariant feature transform (SIFT) is used to extract image features served as the nature landmarks, and SIFT features are invariant to image scaling, rotation, and change in 3D viewpoints, which are highly distinctive due to a special technique for their description. A fast nearest neighbor search algorithm using KD-tree is presented to implement SIFT feature matching in the time cost of O(log\-2N). Experiments on the real robot show that the proposed method provides an efficient and robust method for implementing SLAM.