A Hierarchical Iterative Closest Point Algorithm for Simultaneous Localization and Mapping of Mobile Robot

Simultaneous localization and mapping (SLAM) problem of a mobile robot is studied in this paper. An improved particle filters approach is adopted to reduce the number of particles. A laser range finder is utilized to measure the distance of obstructs, and the accurate proposal distribution are obtained by scan match method, which is realized by a hierarchical iterative closest point (ICP) algorithm. A roughly global optimal estimation of robot pose is first obtained by directly searching in the discrete space of pose, and then the estimation of robot pose is refined by gradient descend method. So an accurate estimation of robot pose can be obtained by the hierarchical scan match approach. Experimental tests are carried out with our real mobile robot in an indoor environment. Experimental results show that the consistent map can be obtained by the proposed scan match approach. The efficiency of the proposed scan match approach is also validated by the RoboCup@Home competition.