According to the relevant characteristics of point cloud acquisition of time-of-flight method (TOF), an improved registration algorithm suitable for TOF point cloud was proposed. Firstly, FPFH features were used to perform rough registration of point cloud. And in the fine registration stage, the angle feature sampling method of normal vector was used to reduce the number of points of the point cloud, while retaining the key information points of the point cloud. At the same time, KD tree and RANSAC method were introduced to improve the registration efficiency of ICP. Experimental results show that this method has good registration efficiency and accuracy, and has a wide range of application.