A three-stage improved point cloud registration ICP algorithm is proposed to deal with the problem of the low reconstruction accuracy, position trajectory loss caused by matching error in the process of inter-frame registration in visual simultaneous location and mapping (V-SLAM) of mobile robots. Firstly, the RANSAC algorithm is used to eliminate the mis-matching of RGB images in preprocessing phrase. Then, rough registration is accomplished by a double distance threshold method based on rigid body transformation consistency. Under a good initial pose, a dynamic iterative angle factor is used to complete the accurate registration of ICP. On the back end, a key frame screening mechanism combining the sliding window method and the random sampling method is introduced. And the robot pose is optimized by combining the general algorithm optimization (g2o) algorithm to construct a globally consistent map V-SLAM system. The standard point cloud model is used to verify that the proposed algorithm has a significant improvement in the accuracy of the registration compared experiment. The effectiveness of the proposed algorithm is validated by the map reconstruction experiment in real environment on the mobile robot. Finally, based on the experiment on TUM dataset, the experimental result shows that this algorithm can estimate the robot′s trajectory effectively.