Abstract: As one of the core steps in point cloud data processing, the registration result affects on the accuracy of subsequent data processing directly. In view of the limitation of the traditional iterative closest point (ICP) algorithm relying on a better initial position, the accurate registration algorithm based on normal features is studied. Firstly, the local surface fitting method is used to estimate the normal vector of point cloud, and to calculate the fast point feature histograms. Secondly, the sampling consistency algorithm is used to perform the coarse registration of point cloud. Finally, the KD-Tree is built to speed up the search efficiency of the corresponding points, and the normal vector threshold is set to remove corresponding error points. Results show that the coarse registration algorithm based on the normal features can provide a better initial position, and the improved ICP algorithm improves the efficiency and accuracy of registration.