LiDAR plays a pivotal role in the field of unmanned driving, but in actual use, it is often accompanied by errors caused by point cloud distortion, which affects the accuracy of various downstream tasks. In this paper, we first describe the feature of point cloud and propose a new feature point selection method Soft-NMS-Select; this method can obtain uniform feature point distribution and effectively improve the result of subsequent point cloud registration. Then, the point cloud registration is completed through the screened feature points, and the odometry information is obtained. For the motion distortion generated in a sweep, the prior information of the LiDAR’s own motion is obtained by using two linear interpolations, thereby improving the effect of motion compensation. Finally, for the distortion caused by the motion of objects in the scene, Euclidean clustering is used to obtain the position and normal vector of the center point of the point cloud cluster, and the motion pose of the object is calculated according to the offset between adjacent sweeps and eliminated distortion. Essentially, our method is a general point cloud compensation method that is applicable to all uses of LiDAR. This paper inserts this method into three SLAM algorithms to illustrate the effectiveness of the method proposed in this paper. The experimental results show that this method can significantly reduce the APE of the original SLAM algorithm and improve the mapping result.