An Iterative Dual Closest Point Method for Robot’s Pose Estimation Based on Clustering

To overcome the problem of local extrema existing in iterative closest point(ICP) algorithm when severe occlusions occur,the closest point(CP) rule is modified and dual closest point(DCP) rule is proposed.DCP rule contains twice CP correspondences so that computation complexity is doubled.To decrease the computation complexity,iterative dual closest point based on clustering(IDCP BoC) is proposed.Scan range points are divided into clusters and then a procedure of data reduction is conducted.The reduced data set is used for iterative computation before the error of two consecutive iterations’ residual errors is less than a preset threshold to speed up the algorithm,and the data set without reduction is used after that to guarantee the accuracy.Experimental results show that IDCP BoC can avoid the problem of local extrema effectively and its real-time performance is also acceptable.