Point cloud distortion correction method based on dynamic vehicle detection in unmanned driving scene

Abstract

The present application belongs to the field of unmanned driving, and proposes a point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scene, which comprises the following steps: step 1, point cloud distortion analysis and data generation; step 2, point cloud global distortion correction based on laser radar motion analysis; step 3, point cloud local distortion correction based on vehicle motion analysis; and step 4, point cloud distortion correction in an unmanned driving scene. The method of the present application improves the accuracy of three-dimensional reconstruction and three-dimensional positioning in unmanned driving, and effectively overcomes the safety hazards of intelligent motion behavior of unmanned driving.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of unmanned driving, and in particular to a point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scenario. BACKGROUND

[0002] In an unmanned driving scenario, the motion of the vehicle-mounted laser radar and the motion of the objects in the scene will cause distortion in the point cloud collected by the laser radar, which will interfere with point cloud registration, three-dimensional positioning, and three-dimensional reconstruction. Therefore, it is of great significance to study how to effectively eliminate point cloud distortion. Current point cloud distortion correction algorithms are mainly divided into two categories: pre-processing algorithms and post-processing algorithms.

[0003] Pre-processing algorithms use prior knowledge to process point cloud distortion during point cloud collection in order to achieve better real-time performance. A segmented registration algorithm is used to minimize the impact of point cloud distortion on point cloud registration. The point cloud is divided into multiple point cloud subsets to refine the point cloud data. Then, the divided point cloud subsets are matched with the global map. This algorithm effectively increases the sampling frequency of the point cloud, thereby reducing the impact of point cloud distortion on three-dimensional reconstruction. Although this algorithm is relatively simple to implement, it achieves good results in the registration process. Linear interpolation is used to eliminate the impact of point cloud distortion on three-dimensional reconstruction. The linear interpolation algorithm first obtains the overall rotation and translation changes of the previous and next two frames of data. Then, it corrects the distortion of different point cloud data points according to the time ratio. Pre-processing algorithms do not consider the changes in the motion state of the laser radar during point cloud registration, which leads to continuous accumulation of errors.

[0004] Post-processing methods often have better results than pre-processing methods, but they have poor real-time performance. The up-sampling pre-integration method is used to process IMU data to analyze the motion of objects. This method does not require prior assumptions about the motion of the point cloud collection system. Instead, it analyzes the motion characteristics of the system by analyzing the acceleration data collected by the IMU after collecting the point cloud, thereby addressing the motion distortion problem of the point cloud. The method uses heteroscedastic landmark estimation to estimate the data deviation of a binocular stereo camera. The method uses a neural network to reduce the drift problem of the view model. The method adds a Gaussian process to the Slam system to correct the motion error of the odometry.

[0005] In summary, although there have been a large number of studies and methods in the field of point cloud distortion correction, considering the complex driving environment in the unmanned driving scenario, the following problems still need to be considered:

[0006] (1) Complex vehicle motion is not considered: current methods only consider analyzing the motion of the laser radar to correct point cloud distortion, without considering the complex motion of vehicles in the scene. At the same time, the motion of each vehicle in the scene is different, and the motion of different vehicles needs to be processed separately.

[0007] (2) Do not consider the local distortion of point cloud: the existing point cloud distortion correction method only corrects the point cloud distortion from the global, ignores the local distortion caused by the vehicle movement in the unmanned driving scene, causes the corrected point cloud to have incorrect mapping relationship with the real scene (the position relationship of the vehicle itself and other vehicles, the position relationship between other vehicles), and has safety risk. SUMMARY

[0008] In view of the above problems, the present application firstly gives a point cloud global distortion correction method by analyzing the laser radar movement; then, based on the point cloud dynamic vehicle detection, the point cloud dynamic vehicle clustering and vehicle rigid body motion estimation are studied, and a point cloud local distortion correction method is given; on this basis, a point cloud distortion correction method in the unmanned driving scene is given. Experiments show that the method improves the accuracy of vehicle motion estimation by predicting the dynamic vehicle motion area, and effectively corrects the point cloud distortion. Thus, the accuracy of unmanned driving three-dimensional reconstruction and three-dimensional positioning is improved, and the safety risk of intelligent motion behavior of unmanned driving is effectively overcome.

[0009] The technical scheme of the present application specifically comprises the following steps:

[0010] Step 1. Point cloud distortion analysis and data generation

[0011] Step 1.1 Point cloud distortion analysis

[0012] Step 1.2 Point cloud distortion data generation

[0013] Step 2. Point cloud global distortion correction based on laser radar motion analysis

[0014] Step 3. Point cloud local distortion correction based on vehicle motion analysis

[0015] Step 3.1 Point cloud dynamic vehicle clustering method

[0016] Step 3.2 Dynamic vehicle rigid body motion estimation

[0017] Step 3.3 Dynamic vehicle motion area prediction

[0018] Step 3.4 Point cloud local distortion correction

[0019] Step 4. Point cloud distortion correction in unmanned driving scene

[0020] Compared with the prior art, the present application has the following beneficial effects

[0021] The application discloses a point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scene, and provides a method capable of accurately predicting problems such as point cloud distortion caused by the fact that the real position relationship between objects cannot be correctly mapped by the point cloud collected by a laser radar due to vehicle movement in an unmanned driving scene, and effectively correcting point cloud distortion. BRIEF DESCRIPTION OF DRAWINGS

[0022] Figure 1 Global distortion schematic diagram.

[0023] Figure 2 Local distortion schematic diagram.

[0024] Figure 3 Simulation point cloud distortion data example.

[0025] Figure 4 Point cloud global distortion correction schematic diagram.

[0026] Figure 5 Point cloud global distortion correction algorithm flowchart.

[0027] Figure 6 Point cloud dynamic vehicle clustering segmentation schematic diagram.

[0028] Figure 7 Dynamic vehicle point cloud clustering algorithm flowchart.

[0029] Figure 8 Dynamic vehicle rigid body motion estimation schematic diagram.

[0030] Figure 9 Dynamic vehicle motion area prediction schematic diagram.

[0031] Figure 10 Point cloud local distortion correction algorithm flowchart.

[0032] Figure 11 Point cloud distortion correction algorithm flowchart.

[0033] Figure 12 Performance of each point cloud registration algorithm on different input data.

[0034] Figure 13 Rotation error mean of each point cloud registration algorithm for vehicle estimation with different point cloud numbers.

[0035] Figure 14 Rotation error median of each point cloud registration algorithm for vehicle estimation with different point cloud numbers.

[0036] Figure 15 Displacement error mean of each point cloud registration algorithm for vehicle estimation with different point cloud numbers.

[0037] Figure 16 Mean of displacement error estimated by each point cloud registration algorithm for different point cloud number of vehicles.

[0038] Figure 17 Examples of point cloud distortion correction visualization results of the MDDC method.

[0039] Figure 18 Correction of vehicle deformation by the MDDC method.

[0040] Figure 19 Flowchart of the present invention. DETAILED DESCRIPTION

[0041] A point cloud dynamic vehicle detection method in an unmanned driving scene (applicant: Tongji University, patent application number: 2023105469838) invented by Cheng Jijun et al. on May 15, 2023 discloses a point cloud dynamic vehicle detection method in an unmanned driving scene. First, a point cloud data generation method and a dynamic vehicle labeling method in an unmanned driving scene are proposed. On this basis, a point cloud dynamic vehicle detection method based on feature embedding is given, and ablation and comparison experiments are performed. The experimental results show that this method effectively detects the motion state of vehicles in the point cloud in an unmanned driving scene, and the prediction accuracy is higher than that of other methods. Therefore, without the need for a large amount of prior knowledge, the motion of vehicles in an unmanned driving scene can be directly perceived, making it possible to accurately determine whether point cloud distortion occurs due to the relative motion between vehicles and laser radars, thereby improving the accuracy of intelligent perception of unmanned vehicles. The technical solution of this application specifically includes the following steps:

[0042] Step 1. Dynamic vehicle labeling and construction of training data set;

[0043] Step 2. Building a point cloud dynamic vehicle detection model and running a point cloud dynamic vehicle detection algorithm;

[0044] Step 2.1 Data preprocessing module;

[0045] Step 2.2 Feature encoding layer for feature extraction of point cloud;

[0046] Step 2.3 Feature embedding layer;

[0047] Step 2.4 Feature decoding layer;

[0048] Step 2.5 Output layer;

[0049] Step 2.6 Design of loss function.

[0050] The method provided by the application is based on the method of the above-mentioned application "Point cloud dynamic vehicle detection method in unmanned driving scene", and can accurately predict the problem of point cloud distortion caused by the fact that the point cloud collected by the laser radar in the unmanned driving scene cannot correctly map the real position relationship between objects due to vehicle movement, and effectively corrects the point cloud distortion. Therefore, the accuracy of three-dimensional reconstruction and three-dimensional positioning of unmanned driving is improved, and the safety hidden danger of intelligentization of unmanned driving movement behavior is effectively overcome.

[0051] The technical solutions provided by the application will be further described below in combination with specific embodiments and their accompanying drawings. The advantages and features of the application will be clearer in combination with the following description.

[0052] The application specifically includes the following five aspects:

[0053] Step 1. Point cloud distortion analysis and data generation

[0054] Step 1.1 Point cloud distortion analysis

[0055] Step 1.2 Point cloud distortion data generation

[0056] Step 2. Point cloud global distortion correction based on laser radar motion analysis

[0057] Step 3. Point cloud local distortion correction based on vehicle motion analysis

[0058] Step 3.1 Point cloud dynamic vehicle clustering method

[0059] Step 3.2 Dynamic vehicle rigid body motion estimation

[0060] Step 3.3 Dynamic vehicle motion area prediction

[0061] Step 3.4 Point cloud local distortion correction

[0062] Step 4. Point cloud distortion correction in unmanned driving scene

[0063] Step 5. Simulation experiment verification

[0064] The details are as follows:

[0065] Step 1. Point cloud distortion analysis and data generation

[0066] The symbols used in the present application are shown in Table 1. The unmanned driving scene contains a large number of vehicles with different motion states. Due to the development of intelligent transportation systems and unmanned vehicle group technologies, vehicles often maintain high-speed driving, making the point cloud distortion caused by vehicle motion in the scene cannot be ignored. To ensure the quality of the collected data, the existing point cloud dataset in the unmanned driving scene often has a low driving speed of the data collection vehicle, and the scene is mostly a speed-limited driving scene, which is difficult to meet the research needs. Based on the analysis of point cloud distortion in the unmanned driving scene, the present application proposes a point cloud distortion data generation method and simulates the distortion of point cloud in the unmanned driving scene.

[0067] Table 1

[0068]

[0069]

[0070] (1) Point cloud distortion analysis

[0071] Due to the lack of point cloud datasets for point cloud distortion correction and dynamic vehicle detection in the unmanned driving scene, the present application uses simulation software to generate simulation experimental datasets. Before generating the simulation experimental dataset, the point cloud distortion in the unmanned driving scene is analyzed and redefined.

[0072] Firstly, the components of point cloud distortion in the unmanned driving scene are defined and analyzed. The distortion of the point cloud collected by the laser radar in the unmanned driving scene mainly comes from two aspects: ① the distortion caused by the motion of the laser radar; and ② the distortion caused by the motion of the objects (mainly vehicles) in the scene.

[0073] 1) Distortion caused by laser radar motion

[0074] In the unmanned driving scene, the motion of the laser radar mainly includes the translation of the laser radar with the unmanned vehicle and the rotation of the laser radar. As shown in FIG. 1, the translation of the laser radar causes distortion in the point cloud, which causes the collected point cloud to deform and change its relative position with respect to the objects in the scene, and also causes the collected point cloud to lose some information. The distortion point cloud caused by translation Figure 1 can be represented as formula (1):

[0075]

[0076] where P is the point cloud without distortion; is the distortion point cloud caused by translation; p i is the data point in the point cloud without distortion; p j is the data point lost due to translation distortion; d i is the point cloud translation distortion coefficient. ​

[0077] The distortion caused by the rotation of the laser radar is mainly reflected in the change of the curvature of the object surface and the loss of part of the information of the object in the point cloud. The distortion caused by the rotation of the laser radar can be represented as formula (2):

[0078]

[0079] P is the point cloud without distortion; P is the point cloud distorted by the rotation of the laser radar; p i P is the data point in the point cloud without distortion; p j P is the data point lost due to the distortion caused by the rotation of the laser radar; λ i P is the rotation distortion coefficient of the point cloud.

[0080] Since the point cloud distortion caused by the laser radar acts on all data points of the collected point cloud, the point cloud distortion caused by the movement of the laser radar is called global distortion PD g .

[0081] The main factor affecting the degree of global distortion caused by the movement of the laser radar is the speed of the vehicle to which the laser radar belongs.

[0082] 2) Distortion caused by the movement of objects in the scene

[0083] The distortion caused by the movement of objects in the scene is also essentially the relative displacement of the object and the laser radar within the time interval of collecting a frame of data, as shown in Figure 2 The distortion caused by the movement of objects in the scene is also essentially the relative displacement of the object and the laser radar within the time interval of collecting a frame of data, as shown in The distortion caused by the movement of objects in the scene can be represented as formula (3):

[0084]

[0085] P is the point cloud without distortion; P is the point cloud distorted by the movement of objects in the scene; P is the point cloud corresponding to the stationary object in the scene; p i P is the data point in the point cloud; p j P is the data point lost due to the movement of objects in the scene; p k P is the data point corresponding to the stationary object in the scene; d i P and λ i are the translation and rotation distortion coefficients of the point cloud, respectively.

[0086] Due to the complexity of the unmanned scene, there are often stationary objects (buildings, trees) and moving objects (vehicles) in the scene, which makes the distortion caused by the movement of objects in the scene often have locality, so the point cloud distortion caused by the movement of objects in the scene is defined as local distortion PD l .

[0087] Then, the definition of the point cloud distortion PD in the unmanned scenario is:

[0088] PD = {PD g , PD l} (4)

[0089] wherein PD is the point cloud distortion in the unmanned scenario; PD g is the global point cloud distortion; and PD l is the local point cloud distortion.

[0090] The definition of the point cloud distortion correction truth value in the unmanned scenario is a snapshot of the scene at a certain time point, that is, the image collected by the laser radar instantaneously at this time is the truth value at this time. That is:

[0091] truth = Point (t) where v lidar = 0 and v vehicle = 0 (5)

[0092] wherein truth is the truth value at time t, which is equivalent to the image collected when the laser radar and the vehicle are both in a stationary state from this time.

[0093] (2) Point cloud distortion data generation

[0094] On the basis of analyzing the point cloud distortion in the unmanned scenario, different simulation point cloud distortion data are obtained by adjusting the motion of the laser radar and the vehicle model in the scene. The simulation data are mainly obtained in the following three ways: 1) the scene remains stationary, the laser radar starts from different positions, moves at different speeds, and collects point cloud data; 2) the vehicle model in the scene starts from different positions, moves at different speeds, while the laser radar remains stationary, and collects point cloud data; 3) the laser radar and the vehicle move at different speeds at the same time, and collect data.

[0095] In order to simulate the motion of the laser radar and the vehicle in the point cloud collection process, the time is the time interval for moving the positions of the laser radar and the vehicle in the simulation scene. It can be calculated by formula (6):

[0096]

[0097] wherein is the time interval between each two position transformations; f is the scanning frequency of the laser radar; and dpi is the scanning resolution of the laser radar.

[0098] In Within a given timeframe, relative positional changes are created by moving the LiDAR and point cloud positions to generate point cloud distortion. Simultaneously, different speeds are assigned to different vehicles and LiDARs to simulate complex autonomous driving scenarios. The distortion of point cloud data points acquired by lidar within a given time period can be represented as follows:

[0099]

[0100] in, for The set of point cloud distortions caused by the relative motion between the vehicle model set M and the lidar during the time interval; For lidar at intervals The motion vector, For point j in model M at time interval The motion vector is affected by the time interval. The effects of rotation, translation, etc. on the inner model M; For the affine transformation of the model; For model M in time interval The motion vector within.

[0101] During the data acquisition process, random Gaussian noise is added to the data. This noise mainly consists of data deviations caused by the following scenarios: 1) distance error generated by the lidar during the point cloud acquisition process; 2) outliers appearing during the lidar point cloud acquisition process; 3) disturbance noise added to the lidar and vehicle movement speed.

[0102] For each scene, four images are acquired: ① a noise-free point cloud of the static scene acquired by a stationary LiDAR in the first frame; ② a noisy point cloud of the dynamic scene acquired by a dynamic LiDAR in the first frame; ③ a noise-free point cloud of the static scene acquired by a stationary LiDAR in the second frame; and ④ a noisy point cloud of the dynamic scene acquired by a dynamic LiDAR in the second frame. For example... Figure 3 As shown in the example, the lidar remains stationary, vehicle 1 remains stationary, vehicle 2 moves to the left at a speed of 50 km / h, and vehicle 3 moves to the left at a speed of 110 km / h.

[0103] Step 2: Global Distortion Correction of Point Cloud Based on LiDAR Motion Analysis

[0104] In autonomous driving scenarios, point cloud distortion mainly comes from global distortion caused by the movement of lidar and local distortion caused by the movement of objects, primarily vehicles, within the scene.

[0105] This invention corrects the global distortion of the point cloud by analyzing the motion of the lidar to eliminate the interference of lidar motion on the subsequent analysis of vehicle motion in the scene, thus providing a basis for subsequent local distortion correction.

[0106] A global distortion correction schematic diagram is shown in Figure 4 .

[0107] Firstly, the derivation of the global distortion correction formula is given, and then the point cloud global distortion correction algorithm is given. The point cloud global distortion can be expressed as formula (8):

[0108] offset global =f(ΔR Lidar ) (8)

[0109] Wherein, offset local is the point cloud global distortion compensation; ΔR Lidar is the motion of the laser radar in the time interval ; f(ΔR Lidar ) represents the point cloud global distortion correction of the point cloud according to the rigid motion of the laser radar.

[0110] Assuming that the laser radar moves uniformly in the time interval of collecting a frame of data, the speed v k of the vehicle when collecting the kth frame of point cloud can be represented by formula (9):

[0111]

[0112] Wherein, X k , X k+1 are the positions of the vehicle when starting to collect the kth frame and starting to collect the k+1th frame respectively; is the time for the laser radar to collect a frame of data.

[0113] After obtaining the speed of the laser radar when collecting the kth frame of point cloud, for the data point p i in the kth frame of point cloud, the distortion compensation amount Θ i of the data point p i caused by the motion of the laser radar can be obtained by formula (10):

[0114]

[0115] Wherein, t s is the time stamp when starting to collect the kth frame of point cloud; t i is the time stamp when collecting the data point p i ; Δt i is the difference between t i and t s .

[0116] For the time of data point collection in the laser radar, it can be calculated by the position of the data point in the coordinate system with the laser radar as the origin. The data point p iThe unit direction vector to the origin of the lidar is determined solely by the lidar's vertical field of view, lidar resolution, and the number of columns the lidar has scanned in the current frame. The vertical field of view and resolution do not change over time. However, the number of columns scanned by the lidar increases uniformly over time. And point p... i The column number ζ i It can be calculated from equation (11):

[0117]

[0118] Where, α i Let point p be in a coordinate system with the lidar as the origin. i The angle of the corresponding unit vector.

[0119] In this invention, only the rotational change of the lidar in the vertical direction z is considered, then α i It can be expressed by equation (12)

[0120]

[0121] Where, p i,x ,p i,y Data points p i The coordinates in the x and y directions can be determined.

[0122] Based on the above analysis, data point p i Distortion compensation amount Θ i It can be calculated using equation (13):

[0123]

[0124] Because the collected point cloud contains noise, and the points are not arranged according to the scanning pattern of the lidar laser points during acquisition, the calculation of α... i Linear interpolation is used to make the data size conform to the scanning characteristics of the LiDAR, that is, to move the discrete point cloud data points to a regular scanning angle. Then, after linear interpolation, point p... i The corresponding unit vector angle α′ i It can be expressed by equation (14):

[0125] α′ i =n·dpi or(n+1)·dpi,where n·dpi<α i <(n+1)·dpi (14)

[0126] The compensation amount Θ is calculated. i Afterwards, point cloud P k The point cloud P obtained after eliminating the distortion caused by the motion of the lidar using a global distortion correction method ig It can be represented by formula (15):

[0127] P i g = {p' x, p' y, p' z} (15) i = p x, p y, p z i + Θ i | p x, p y, p z i ∈ P k} (15)

[0128] The flow chart of the point cloud global distortion correction algorithm is shown in Figure 5 The specific steps of the point cloud global distortion correction algorithm are as follows:

[0129] Step 2.1 input the point cloud frame P i with distortion.

[0130] Step 2.2 if there is a point cloud data point p i ∈ P i , go to step 2.3; otherwise, go to step 2.8.

[0131] Step 2.3 parse the coordinates x, y, z of the point cloud data point p i .

[0132] Step 2.4 calculate the corresponding laser radar beam angle of the point cloud data point according to the coordinates x and the coordinates y.

[0133] Step 2.5 calculate the number of laser radar beams in a frame of laser radar data, and correct it by linear interpolation.

[0134] Step 2.6 encode the laser radar point cloud for subsequent experimental processing. At the same time, calculate the time corresponding to the laser beam in the frame.

[0135] Step 2.7 calculate the point cloud global distortion of the point cloud p i in the x direction and the y direction, and correct the point cloud global distortion, and put the corrected point cloud data point into P i g .

[0136] Step 2.8 output the point cloud after global distortion correction.

[0137] Step 3 point cloud local distortion correction based on vehicle motion analysis

[0138] The local distortion of the point cloud is mainly caused by the movement of vehicles in the scene. Therefore, first, different dynamic vehicles in the same frame of point cloud are segmented by a point cloud dynamic vehicle clustering method; then, a dynamic vehicle rigid body motion estimation method based on point cloud registration is proposed to analyze the movement of different vehicles in the scene, and the accuracy of vehicle motion estimation is improved by dynamic vehicle motion area prediction; finally, a point cloud local distortion correction method is given based on the vehicle motion analysis result.

[0139] Step 3.1 Point cloud dynamic vehicle clustering method

[0140] As shown in Figure 6 , after obtaining the motion state of the point cloud data points by the point cloud dynamic vehicle detection method, since different vehicles have different motion states, the point cloud in the motion state needs to be segmented to process the point cloud distortion caused by different vehicles differently.

[0141] The point cloud collected by the laser radar has the characteristics of near dense and far sparse, that is, the point cloud at a relatively close distance is usually dense, and the point cloud at a relatively far distance is usually sparse. If an equal distance distance threshold point cloud clustering algorithm is used, when the distance is too large, multiple dynamic vehicles at a close distance may be clustered as one dynamic vehicle, and when the distance is too small, the vehicle point cloud at a far distance may be difficult to aggregate. Based on the Euclidean clustering algorithm, an improved dynamic distance threshold vehicle point cloud clustering algorithm is proposed, which aims to correctly aggregate vehicle point cloud data at different distances. Compared with the Euclidean clustering algorithm, the algorithm can dynamically adjust the distance threshold according to the position of the point cloud data point to correctly segment vehicles at different distances.

[0142] The method first divides the point cloud into two different sets according to the dynamic vehicle detection result, namely a dynamic point cloud set Data dyn and a static point cloud set Data sta , and establishes a KD-tree (K-Dimension Tree) based on the dynamic point cloud set Data dyn . Then, a data point p i is randomly selected from the dynamic point cloud set Data i and put into a single vehicle set vehicle as the starting point of aggregation. Then, the 26 points closest to p i are found by Kd-Tree. The 26 closest points are mainly selected to cover all directions of the data point p to prevent the same vehicle from being incorrectly segmented into multiple vehicles. The distances of the 26 points to p i are calculated, and the points with a distance less than the dynamic distance Dis are added to the single vehicle set vehicle and reselected as the starting point of aggregation to find other data points belonging to the single vehicle set vehicle. The dynamic distance Dis can be represented by formula (16):

[0143]

[0144] where p i,x , p i,y are the data points p i in the x, y and z directions respectively; θ V is the vertical field of view angle of the laser radar, max(θ V ) is the maximum vertical field of view angle; and γ is an expansion coefficient, which is taken as 1.5.

[0145] After the aggregation of the single vehicle set vehicle is completed, the number of vehicle centroids is calculated. When the number of vehicle data points is small, the influence of the overall point cloud distortion correction is not great, so the vehicle with the number of points less than N col is ignored, and the calculation method of N col is defined as follows:

[0146] N col = τ·col (17)

[0147] where col is the number of laser beams in the vertical direction of the laser radar, and τ is an expansion coefficient, which is taken as 3.

[0148] The flow chart of the dynamic vehicle point cloud clustering algorithm is shown in Figure 7 . The specific steps of the dynamic vehicle point cloud clustering algorithm are as follows:

[0149] Step 3.1.1 input the point cloud frame P i processed by the point cloud dynamic vehicle detection method FEMD.

[0150] Step 3.1.2 initialize the visited node set visited and the vehicle set Vehicles.

[0151] Step 3.1.3 according to the motion state of the data points in the point cloud P i , the data points in P i are divided into a dynamic point cloud set Data dyn and a static point cloud set Data sta .

[0152] Step 3.1.4 establish a KDTree based on the dynamic point cloud set Data dyn .

[0153] Step 3.1.5 there is a point cloud data point p i ∈Data dyn that has not been visited, go to step 3.1.6, otherwise, go to step 3.1.14.

[0154] Step 3.1.6 if the data point pi If not in the visited set, proceed to step 3.1.7; otherwise, proceed to step 3.1.5.

[0155] Step 3.1.7 Initialize the vehicle and queue. And set the data point p... i They are placed into a single vehicle set, a queue, and a visited node set.

[0156] Step 3.1.8 If the queue is not empty, proceed to step 3.1.9; otherwise, proceed to step 3.1.13.

[0157] Step 3.1.9 Take the first element p of the queue. k Remove it from the queue and query the distance p using kdt. k The 26 most recent elements form the set distance. i .

[0158] Step 3.1.10 If element p exists j ∈distance i If the step has not been accessed, proceed to step 3.1.11; otherwise, proceed to step 3.1.13.

[0159] Step 3.1.11 If point p j With p k If the distance between them is less than the dynamic distance Dis, proceed to step 3.1.12; otherwise, proceed to step 3.1.10.

[0160] Step 3.1.12 Transfer data point p j They are placed into a single vehicle set, a queue, and a visited node set.

[0161] Step 3.1.13 If the number of points in a vehicle is greater than 128, then add it to the vehicle set Vehicles.

[0162] Step 3.1.14 Output the segmented vehicle set, Vehicles.

[0163] Step 3.2 Estimation of rigid body motion of dynamic vehicle

[0164] like Figure 8 As shown, local distortions arise from the motion of vehicles in the scene. This invention estimates the motion of each vehicle in the scene based on a point cloud registration algorithm.

[0165] Because the time interval of collecting two continuous frames of the laser radar is small, the motion of the vehicle in the scene in the time interval of collecting two continuous frames is also regarded as uniform linear motion. The motion of different vehicles is analyzed by analyzing the position transformation of the different vehicles in the two continuous frames of point clouds, and the distortion caused by different vehicles is corrected respectively, that is:

[0166]

[0167] Wherein, offset local is the point cloud local distortion compensation; vehicle k represents the moving vehicle segmented by the vehicle aggregation method; n represents the number of moving vehicles; represents the motion of the vehicle in the time interval . represents the point cloud local distortion correction according to the motion of the vehicle vehicle i .

[0168] For the vehicle vehicle i in the point cloud P k , the motion of the vehicle in the time interval of two continuous frames can be represented by formula (19):

[0169]

[0170] Wherein, r represents the rotation change of the vehicle vehicle k corresponding to the point cloud data point p i ; t represents the translation change of the vehicle vehicle k corresponding to the point cloud data point p i .

[0171] The formula can be expressed in the form of affine transformation, as shown in formula (20):

[0172]

[0173] Wherein, r is a 3x3 rotation matrix, and t is a 3x1 displacement matrix. The transformation matrix contains six degrees of freedom in total, which are the rotation around x, y and z axes and the displacement. Since the object of the present application is the vehicle driving on the road, the vehicle can be regarded as a rigid body, so for any p i ∈vehicle k , the same rotation matrix and displacement matrix are obtained.

[0174] Before estimating the rigid body motion of vehicles, the global distortion of point cloud is corrected by the method in step 4. After removing the global distortion, the remaining distortion in the point cloud is the local distortion caused by the rigid body motion of the vehicle. Due to the motion of the laser radar, the two frames of point clouds collected are not in the same coordinate system. In order to more simply estimate the rigid body motion of the vehicle, the coordinate system of the second frame of image point cloud is converted to the coordinate system of the first frame of point cloud by coordinate transformation. The coordinate transformation can also be expressed in the form of affine transformation:

[0175]

[0176] Where (x1, y1, z1) and (x2, y2, z2) represent the coordinate systems of the first frame data and the second frame data in the two frames of point clouds collected by the laser radar, respectively; r l and t l represent the rotation transformation and displacement transformation of the laser radar between the two frames of data, respectively.

[0177] Subsequently, the rigid body motion of different vehicles is estimated by a point cloud registration algorithm. The point cloud registration algorithm can align two frames of point clouds by finding corresponding points in the two frames of point clouds and estimating the transformation matrix. Between two consecutive frames, assuming that the vehicle maintains the same speed, the same vehicle has similar distortion in consecutive frames, which ensures the robustness of the point cloud motion estimation by point cloud registration. At the same time, the point cloud registration algorithm can complete the registration under the condition that two frames of point clouds partially overlap, so the motion estimation by point cloud registration can also solve the problem of information loss caused by the motion of the laser radar. The motion estimation by point cloud registration can be represented by the formula:

[0178]

[0179] Where, represents the vehicle motion transformation affine matrix estimated by the point cloud registration algorithm; p i → CP(p j ) represents finding the corresponding points of point cloud p next in the second frame of point cloud p i ; f(p i → CP(p j )) represents estimating the rigid body motion of the vehicle according to the corresponding point relationship.

[0180] Step 3.3 Dynamic Vehicle Motion Area Prediction

[0181] In the unmanned driving scene, there are many vehicles, and a large number of vehicles may have similar shapes, which are difficult to distinguish by point cloud shape. The present application proposes a point cloud segmentation algorithm based on dynamic vehicle motion area prediction. Based on the estimated vehicle speed of the previous point cloud frame, different segmentation areas are adopted for different vehicles segmented in the point cloud dynamic vehicle clustering method in the second frame data to reduce the matching error rate of the vehicle in the two consecutive frames.

[0182] Since the vehicle has inertia, the vehicle speed cannot be instantaneously increased in a short time, but there is an acceleration or deceleration process. This feature is used. Figure 9 As shown in the figure, the vehicle position is centered, and the area that may appear in the next frame time is divided according to the speed and estimated maximum acceleration size, and the possible corresponding position of the vehicle in the next frame data is searched from the area.

[0183] According to the current vehicle acceleration and deceleration rule, the following assumptions are made for the vehicle in the speed direction and perpendicular to the speed direction: the maximum acceleration of the vehicle in the speed direction and perpendicular to the speed direction is 10 m / s 2 , and the minimum acceleration is -10 m / s 2 . Then, assuming that the speed of the vehicle is v0, the possible displacement values of the vehicle in the speed direction and perpendicular to the speed direction after a frame time interval t are:

[0184]

[0185] Where x1 is the predicted displacement value in the speed direction of the vehicle, x2 is the predicted displacement value in the opposite direction of the vehicle speed; y1 and y2 are the predicted displacement values perpendicular to the speed direction of the vehicle, and are in opposite directions; a is the maximum acceleration of the vehicle.

[0186] At the same time, the footprint of the vehicle at the first frame needs to be estimated. For each vehicle segmented by the point cloud dynamic vehicle clustering method, the maximum and minimum values of the point cloud coordinates in the speed direction and perpendicular to the speed direction are calculated as the range of the vehicle. At the same time, since the laser radar can often only collect part of the side point cloud image of the vehicle, the range of the vehicle calculated by this method often cannot completely cover the true range of the vehicle. In order to avoid safety hazards, a compensation value is added to the predicted vehicle range, the length of the vehicle is added in the speed direction, about 3m, and the width of the vehicle is added in the direction perpendicular to the speed direction, about 2m, as shown in equation (24):

[0187]

[0188] Where vehicle x,1 is the estimated range of the vehicle in the speed direction; vehicle x,2vehicle y,1 and vehicle y,2 are the vehicle estimation range perpendicular to the vehicle speed direction, and opposite to each other; offset is a compensation value.

[0189] Therefore, the range where the vehicle can appear in the next frame can be represented as a rectangular frame region composed of (x1+vehicle x,1 ,x2+vehicle x,2 ,y1+vehicle y,1 ,y2+vehicle y,2 ).

[0190] Step 3.4 Local distortion correction of point cloud

[0191] After completing the rigid body motion estimation of all dynamic vehicles in a frame of point cloud, based on the vehicle motion analysis results, the local distortion of the point cloud is corrected. Before performing the local distortion correction of the point cloud, the time stamp of each data point in the point cloud is obtained by analyzing the point cloud data point coding. The global distortion and the local distortion of the point cloud are in a linear relationship, and the movement of the point cloud in the global distortion correction process will not affect the local distortion. After obtaining the motion information of the vehicle between two frames of point cloud, the local distortion correction of the point cloud can be represented as:

[0192]

[0193] wherein, is the local distortion compensation of the point cloud corresponding to the vehicle vehicle k ; time i is the time stamp of the point cloud data point p k in the vehicle vehicle i ; time start is the time stamp when the laser radar starts collecting the first frame of point cloud data.

[0194] Then, the point cloud P after local distortion correction can be represented as formula (26):

[0195]

[0196] wherein, is the point cloud after local distortion correction; P i g is the point cloud after global distortion correction; n is the number of moving vehicles in the point cloud P i ; is the point cloud data point corresponding to the moving vehicle vehicle k .

[0197] The flowchart of the point cloud local distortion correction algorithm is as follows: Figure 10 As shown. The specific steps of the point cloud local distortion correction algorithm are as follows:

[0198] Step 3.4.1 Initialize two consecutive frame point clouds and correct point cloud P using a global distortion correction method. i g and

[0199] It is then used as input to the model.

[0200] Step 3.4.2 Segment the point cloud P using a point cloud dynamic vehicle clustering algorithm. i g And obtain the vehicle set. i At the same time, give vehicle_set i The corresponding point cloud number for the vehicle.

[0201] Step 3.4.3 Detect vehicles using a point cloud dynamic vehicle detection algorithm. The vehicle is in motion, and the point cloud is segmented based on the detection results. Obtain a point cloud containing only moving vehicles.

[0202] Step 3.4.4 Traverse the vehicle set (vehicle_set) i For vehicle_set i Each vehicle k The rigid body transformation matrix of each vehicle is calculated using a dynamic vehicle rigid body motion estimation method, and the calculation results are placed into a set T_set.

[0203] Step 3.4.5: Point cloud data point p exists. i ∈P i g If the access has not been made, proceed to step 3.4.6; otherwise, proceed to step 3.4.7.

[0204] Step 3.4.6 Determine the point cloud p i Does it belong to the corresponding point of a moving vehicle? If not, directly convert the point cloud data point p. i Add to collection If it belongs to the category, obtain the corresponding vehicle number and retrieve the rigid body transformation matrix of the corresponding vehicle from T_set; based on the encoding result in the global distortion correction section of the point cloud based on lidar motion analysis, decode to obtain the timestamp of the corresponding point; calculate the local distortion compensation of the point cloud and correct the point cloud p. i The location is obtained as the point cloud data point. Point cloud data points Add to collection

[0205] Step 3.4.7 outputs the point cloud after local distortion correction

[0206] Step 4 Point cloud distortion correction in unmanned driving scene

[0207] The point cloud distortion correction method (MDDC) in the unmanned driving scene proposed by the application corrects the global distortion of the point cloud through the global distortion correction method based on the motion analysis of the laser radar, and on the basis of the global distortion correction of the point cloud, the local distortion of the point cloud is corrected by analyzing the motion of the dynamic vehicle in the scene, so as to realize the point cloud distortion correction in the unmanned driving scene. The point cloud distortion correction algorithm flow chart is as shown in Figure 11 , and the specific algorithm flow is as follows:

[0208] Step 4.1 input two continuous frames of point cloud P k ,P k+1 and the motion speed v lidar of the laser radar.

[0209] Step 4.2 global distortion correction is performed on the point cloud P k ,P k+1 to obtain the point cloud

[0210] Step 4.3 denoising and down-sampling processing are performed on the point cloud to obtain the point cloud P

[0211] Step 4.4 dynamic vehicle detection method is used to detect the dynamic vehicle in the two frames of point cloud P , to obtain the prediction label lable.

[0212] Step 4.5 according to the dynamic vehicle detection result, motion analysis is performed on the dynamic vehicle in the point cloud, to obtain the vehicle motion information inf motion .

[0213] Step 4.6 based on the vehicle motion information inf motion , local distortion correction is performed on the point cloud, to obtain the point cloud P' without distortion k .

[0214] Step 4.7 outputs the point cloud P' without distortion k .

[0215] Step 5 simulation experiment verification

[0216] (1) evaluation index

[0217] The evaluation indexes used in the experiment include: rotation error, translation error, Hausdorff distance, Chamfer distance, EMD distance.

[0218] 1) Rotation error: Given a predicted rotation matrix R and the ground truth rotation matrix R GT , the rotation error is:

[0219]

[0220] where δ(R, R GT ) is the rotation error; trace(R -1 R GT ) is the trace of the matrix R -1 R GT .

[0221] 2) Translation error: Given a predicted translation matrix t and the ground truth translation matrix t GT , the translation error is:

[0222] δ(t, t GT ) = ||t - t GT || (28)

[0223] where δ(t, t GT ) is the translation error; ||t - t GT || is the distance between the vector t and the vector t GT .

[0224] 3) Hausdorff distance: Given two point clouds P1 and P2, the Hausdorff distance is defined as:

[0225]

[0226] d H (P1, P2) = max(d h (P1, P2), d h (P2, P1)) (30)

[0227] where d h (P1, P2) and d h (P2, P1) represent the one-way Hausdorff distance from point cloud P1 to P2 and from point cloud P2 to P1, respectively; d H (P1, P2) represents the two-way Hausdorff distance. The Hausdorff distance mainly measures the maximum irrelevance between two point clouds.

[0228] 4) Chamfer distance: Given two point clouds P1 and P2, the Chamfer distance is defined as:

[0229]

[0230] Where n(P1) and n(P2) represent the number of points in point clouds P1 and P2, respectively. Chamfer distance mainly measures the difference between two point clouds, and the smaller the Chamfer distance, the smaller the difference between two point clouds.

[0231] 5) EMD distance: Given two point clouds P1 and P2, the EMD distance is defined as follows:

[0232]

[0233] Where φ is the mapping of point clouds P1 to P2; EMD distance represents the cost of converting point cloud P1 to point cloud P2, mainly measuring the similarity between two frames of point clouds.

[0234] (2) Analysis of experimental results

[0235] 1) Dynamic vehicle rigid body motion estimation

[0236] First, compare the performances of different point cloud registration methods ① ICP, ② RANCSC, ③ FGR, and ④ EEMR in rigid body motion estimation of unmanned vehicles, to select the most suitable point cloud registration method for dynamic vehicle rigid body motion estimation. Among them, EEMR represents a point cloud registration method based on deep learning. To reflect the generalization of each algorithm, various deep learning-based methods such as EEMR, PointNetLK, PCRNet, DCP, PRNet, and RPM-Net are trained on the ModelNet40 dataset, and their registration effects on the unmanned vehicle point cloud dataset are compared. Among them, the EEMR algorithm performs better, while the other algorithms cannot effectively register vehicle point clouds due to generalization reasons. The main reason is that EEMR extracts features using deep learning algorithms and uses a confidence module for point cloud registration. Therefore, this experiment selects EEMR to represent deep learning-based algorithms and fine-tunes the model parameters on the unmanned simulation dataset. RANCSC and FGR both use FPFH to extract point cloud features.

[0237] The above algorithms are compared on two different input data: ① the second frame of raw data; and ② data processed by the dynamic vehicle motion area prediction algorithm. At the same time, the above algorithms are compared with another vehicle motion estimation method CPME in terms of displacement error to verify the performance of the vehicle motion estimation algorithm of the present application.

[0238] The accuracy of ICP, RANCSC, FGR, and EEMR algorithms on two different input data is as follows: Figure 12are less than 0.5m in real scene scale. As can be seen from the figure, compared with the second frame data as the original data in the input data, the accuracy of each algorithm is improved after the data is processed by the dynamic vehicle motion area prediction algorithm, among which the ICP algorithm improves greatly. The main reason is that when the scene contains more data, the ICP algorithm is easy to fall into local optimization, leading to prediction error, and after the dynamic vehicle motion area prediction algorithm is processed, the interference of point cloud and other vehicle point cloud in the environment can be reduced, and the prediction accuracy of the ICP algorithm can be effectively improved. Compared with the ICP algorithm, the RANSAC algorithm has better performance on the original point cloud and the point cloud processed by the dynamic vehicle motion area prediction method, mainly because RANSAC iteratively registers the point cloud by sampling, which has better global point cloud registration capability. At the same time, based on the characteristics of vehicle motion, RANSAC also has an excellent initial matrix, which improves the accuracy of its registration. In addition, the performance of FGR and EEMR algorithms is always better than that of ICP and RANSAC algorithms, which is mainly because FGR and EEMR algorithms have better fine registration adjustment modules, which can effectively reduce the rotation error and displacement error of motion estimation, and have better feature fitting ability and robustness.

[0239] Experiments show that the dynamic vehicle motion area prediction method can effectively improve the accuracy of vehicle motion estimation. The following experimental data are all processed by the second frame point cloud of the dynamic vehicle motion area prediction method as input, and the motion estimation performance of different registration algorithms for different data point numbers of vehicles is compared: ① data with vehicle point cloud data point number greater than or equal to 1000; ② data with vehicle point cloud data point number less than 1000 due to occlusion, distance, etc.

[0240] The average rotation error of ICP, RANCSC, FGR and EEMR algorithms in motion estimation of vehicle data with different point cloud numbers is as follows: Figure 13As shown in FIG. 6, it can be seen that the average rotation error of the ICP algorithm on different numbers of vehicle point cloud data is higher than that of other algorithms. The main reason is that there are many similar features in the vehicle point cloud data, such as the side plane of the vehicle body. When the vehicle is far apart in the continuous two frames of point cloud data, the ICP algorithm is easy to fall into local optimization, resulting in a decrease in accuracy. On the vehicle with more than 1000 data points, the EEMR algorithm has better performance than other algorithms, with an average rotation error of 1.9206, which is lower than that of the RANSAC algorithm (2.654) and the FGR algorithm (2.417). The main reason is that the EEMR algorithm based on deep learning algorithm has stronger point cloud feature extraction capability. When the number of point cloud data points is large, the EEMR algorithm can extract more effective features. Compared with the data with vehicle point cloud number greater than or equal to 1000, the average rotation error of each algorithm on the data with vehicle point cloud number less than 1000 is increased, indicating that the estimation accuracy of each algorithm is decreased when the number of vehicle point cloud is small. The rotation error of the RANSAC algorithm increases slightly, mainly because the vehicle mostly rotates around the z-axis, so the RANSAC algorithm can have a good initial rotation matrix. At the same time, the RANSAC algorithm based on sampling can effectively handle the situation of containing invalid points and outliers. The rotation estimation error of FGR and EEMR is increased when the number of vehicle point cloud is less than 1000, mainly because when the number of data points is small, FGR and EEMR can extract fewer features, and occlusion may cause the number and shape of the vehicle point cloud to change, leading to errors in the estimation of FGR and EEMR algorithms. Overall, EEMR has better vehicle rotation estimation performance.

[0241] The median rotation error of ICP, RANCSC, FGR, and EEMR algorithms on vehicle data with different point cloud numbers is shown in FIG. 7. Figure 14 As shown in FIG. 7, it can be seen that the median rotation error is smaller than the average rotation error. The main reason is that when the laser radar and the vehicle in the scene move, ghosting or truncation may occur for part of the vehicle, and the point cloud registration algorithm is difficult to effectively estimate the motion of such data, resulting in some abnormal data and the average rotation error being pulled up. On the data with more than 1000 vehicle point cloud data points, the EEMR algorithm and the FGR algorithm have lower median rotation error, 1.6509 and 1.5481 respectively, indicating that when the number of vehicle point cloud is large, the two algorithms have higher rotation estimation accuracy, and the average rotation error of the EEMR algorithm is lower than that of the FGR algorithm, indicating that the EEMR algorithm has better abnormal value processing capability than the FGR algorithm. In summary, the EEMR algorithm has better rotation estimation accuracy and robustness.

[0242] The dynamic vehicle rigid body motion estimation method used in the application is compared with other vehicle motion estimation methods CPME in terms of displacement error.

[0243] The average displacement error of the motion estimation based on ICP, RANSAC, FGR and EEMR algorithms and the motion estimation by the CPME method on vehicle data with different point cloud numbers is shown in FIG. 3. Figure 15 As can be seen from the figure, when the number of vehicle point clouds is greater than 1000, the average displacement error of the CPME algorithm is close to the average displacement error of the dynamic vehicle rigid body motion estimation based on the RANSAC algorithm, and is higher than the FGR algorithm and the EEMR algorithm, indicating that the performance of the dynamic vehicle rigid body motion estimation algorithm of the application can be significantly improved by improving the point cloud registration algorithm, and when the performance of the point cloud registration algorithm is relatively good, the performance can be better than that of the CPME algorithm. When the number of vehicle point clouds is less than 1000, the displacement error of the CPME algorithm increases significantly, while the dynamic vehicle rigid body motion estimation algorithm of the application can still maintain good performance, among which the FGR algorithm and the EEMR algorithm perform best, with 1.16 m and 1.06 m respectively. This is mainly because the CPME algorithm estimates the vehicle motion based on the center point, and when the vehicle is blocked or other features are incomplete, the center point of the CPME algorithm is easy to deviate, resulting in a large error, while the vehicle motion estimation algorithm based on registration can still complete the matching of the features when the vehicle features are incomplete, ensuring the stability of the algorithm. Therefore, the vehicle motion estimation algorithm based on registration can have better vehicle motion estimation performance and robustness.

[0244] The median displacement error of the motion estimation based on ICP, RANSAC, FGR and EEMR algorithms and the motion estimation by the CPME method on vehicle data with different point cloud numbers is shown in FIG. 4. Figure 16 Compared with the average displacement error, the median displacement error is greatly reduced, mainly because the presence of outliers can greatly increase the median displacement error. When the number of vehicle point clouds is greater than or equal to 1000, the median displacement error of the FGR and EEMR algorithms is significantly lower than the average displacement error, with 0.277 m and 0.189 m respectively, indicating that both algorithms have good displacement estimation accuracy, and are significantly lower than the 0.54 m of the CPME algorithm, indicating that the vehicle motion estimation algorithm based on registration can have higher motion estimation accuracy. When the number of vehicle point clouds is less than 1000, the median displacement error of the CPME algorithm is significantly higher than that of the RANSAC, FGR and EEMR, indicating that when the vehicle point cloud features are incomplete, the vehicle motion estimation algorithm based on registration can extract more information from fewer features to improve the accuracy of the dynamic vehicle rigid body motion estimation, while the CPME is sensitive to the completeness of the vehicle features. In general, the dynamic vehicle rigid body motion estimation algorithm based on registration can achieve higher accuracy.

[0245] In summary, the positive effect of the dynamic vehicle motion area prediction algorithm on vehicle motion estimation is verified through experiments. On the autonomous driving point cloud simulation dataset, the dynamic vehicle rigid body motion estimation algorithm represented by the EEMR algorithm based on deep learning feature extraction has better vehicle motion estimation performance and higher estimation accuracy than the CPME algorithm. In the following point cloud distortion correction, the dynamic vehicle rigid body motion estimation algorithm based on the EEMR algorithm is used to analyze the motion of the vehicle.

[0246] 2) Point cloud distortion correction

[0247] Next, the feasibility of the point cloud distortion correction algorithm MDDC in the autonomous driving scenario is illustrated by comparing the visualization results and quantitative indicators.

[0248] Figure 17 The visualization results of the MDDC method for distortion correction on the autonomous driving simulation dataset. To more accurately compare the performance of the two point cloud distortion correction algorithms, the interference of point cloud noise is ignored. Figure ① is the undistorted point cloud image as the true value of the experiment.

[0249] (a) Original point cloud image

[0250] Figure ② is the original image collected by the laser radar, where the speed of the vehicle in the lower left corner is 0 km / h, the speed of the vehicle in the lower right corner is 85 km / h, and the laser radar moves to the right at a speed of 95 km / h. Compared with the undistorted point cloud image ①, it can be seen that although the vehicle in the lower right corner of the scene is moving at high speed, the laser radar cannot perceive the motion of the vehicle in the scene. The main reason is that at this time the vehicle and the laser radar move at almost the same speed, so that the relative position between the laser radar and the vehicle in the same frame does not change, resulting in the phenomenon of pseudo-stationary vehicle in the scene. Comparing the length of the original point cloud image and the undistorted point cloud image in the horizontal direction, i.e. the direction of laser radar motion, which is about 26m and 28.5m respectively, it can be seen that the point cloud image has been deformed in the direction of laser radar motion. This is mainly because the motion of the laser radar causes global distortion of the point cloud in the direction of laser radar motion, and also causes the loss of part of the information in the point cloud. At the same time, the motion of the vehicle in the lower right corner also obscures the vehicle in the upper right corner, causing the loss of part of the vehicle information.

[0251] (b) Point cloud image after global distortion correction

[0252] Figure 3 is a point cloud image after global distortion correction. The length of the image in the horizontal direction, i.e. the direction of motion of the laser radar, is consistent with the non-distorted point cloud image, indicating that the global distortion correction algorithm can effectively correct the distortion caused by the motion of the laser radar. The position of the moving vehicle in the image after global distortion correction is significantly different from that in the non-distorted point cloud image, and a broken phenomenon appears, indicating that the global distortion correction algorithm can effectively eliminate the distortion caused by the motion of the laser radar, but cannot handle the distortion caused by the motion of the vehicle in the scene.

[0253] (c) Point cloud image after local distortion correction

[0254] Figure 4 is a point cloud image after global distortion correction, i.e. a point cloud image after correction by the MDDC method. Figure 4 has almost equal length in the horizontal direction compared to the non-distorted point cloud image 1, indicating that the MDDC method can effectively correct the global distortion of the point cloud caused by the motion of the laser radar. The positions of both the static vehicle and the moving vehicle in Figure 4 are consistent with those in Figure 1, indicating that the MDDC algorithm can effectively correct the position of the moving vehicle, and the effectiveness of the MDDC algorithm for local distortion correction of the point cloud is demonstrated.

[0255] (d) Comparison image in the same coordinate system

[0256] Figure 5 is a point cloud image after global distortion correction, a point cloud image after correction by the MDDC algorithm, and a non-distorted point cloud image in the same coordinate system. The red color represents non-distorted point cloud data, the blue color represents point cloud data processed by the global distortion correction algorithm, and the green color represents point cloud data processed by the MDDC algorithm. As can be seen from the figure, the image processed by the global distortion correction algorithm is consistent with the non-distorted point cloud image in overall shape, but there is a large gap in the corresponding point cloud of the moving vehicle, indicating that it can effectively correct the global distortion caused by the motion of the laser radar, but cannot correct the local distortion caused by the motion of the vehicle in the scene. The point cloud image processed by the MDDC algorithm is consistent with the non-distorted point cloud image as a whole, indicating that it can effectively correct the global distortion and local distortion of the point cloud.

[0257] In summary, the MDDC algorithm can effectively correct the point cloud distortion in the unmanned driving scene.

[0258] The effect of vehicle deformation correction by the MDDC method is shown in Figure 18The visualizations are shown, where the blue is the original point cloud, the red is the non-distorted point cloud, and the green is the point cloud processed by the MDDC method. The vehicle in the scene moves at a speed of 80 km / h to the left. It can be seen that due to the movement of the vehicle in the scene, the overall length of the vehicle increases, and the vehicle point cloud is locally deformed. After processing by the MDDC algorithm, the vehicle returns to the same length as when there is no distortion, indicating that the MDDC method can effectively correct the deformation of the vehicle caused by the movement of the vehicle in the scene.

[0259] Next, the performance of the original point cloud and the point cloud processed by the MDDC method in different indicators is compared to verify the feasibility of the MMDC method for point cloud distortion correction in the unmanned driving scene.

[0260] The average values of the original point cloud and the point cloud processed by the MDDC method in Hausdorff distance, Chamfer distance, and EMD distance compared with the non-distorted point cloud are shown in Table 2. Among them, the Hausdorff distance is used to measure the maximum irrelevance of two point cloud sets. From the table, it can be seen that after processing by the MDDC method, the average Hausdorff distance of the point cloud compared with the non-distorted point cloud decreases, but the decrease is not obvious. This is mainly because the point cloud dynamic vehicle detection algorithm has certain errors, and the source of these errors is often part of the discrete noise points. The MDDC method is difficult to correct this part of the data, which makes most of the point cloud data still have the same Hausdorff distance as the original point cloud after being corrected by the MDDC method. Chamfer distance has certain similarity with Hausdorff distance in expression form, mainly measuring the similarity of two point clouds. From the table, it can be seen that the average Chamfer distance of the point cloud processed by the MDDC method and the non-distorted point cloud decreases significantly, indicating that after processing, the point cloud has higher similarity with the true value. The MDDC method can effectively correct the point cloud distortion in the unmanned driving scene, and combined with the visual analysis result, it shows that the MDDC method can correct the distorted point cloud to the shape of the non-distorted point cloud. EMD distance mainly reflects the difference between the distributions of two point cloud sets. From the table, it can be seen that the average EMD distance of the point cloud processed by the MDDC method compared with the original point cloud and the non-distorted point cloud decreases by about half, indicating that after processing by the MDDC method, the point cloud distribution is closer to the true value. The point cloud local distortion correction method can restore the distribution of the point cloud without local distortion by analyzing the movement of the vehicle, and can effectively correct the local distortion of the point cloud, so that the local distortion of the point cloud can be restored to the distribution of the non-distorted point cloud to a certain extent.

[0261] Table 2

[0262]

[0263] In summary, by visualizing the results and comparing the point clouds processed by the MDDC method and the original point clouds with the distortion-free point clouds in different indicators, it is verified that the MDDC method can correct the point cloud distortion to a certain extent in the unmanned driving scene.

[0264] The present application aims to solve the problems of not considering complex vehicle motion and the incorrect mapping relationship between point cloud and real scene (the position relationship of the vehicle itself and other vehicles, and the position relationship between other vehicles) in the current unmanned driving scene. Based on (the application for a method for detecting dynamic vehicles in point clouds in an unmanned driving scene (applicant: Tongji University, patent application number: 2023105469838) filed by Cheng Jujun et al. on May 15, 2023), a point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scene is proposed: first, a point cloud global distortion correction method is given by analyzing the motion of the laser radar; then, based on the point cloud dynamic vehicle detection, the point cloud dynamic vehicle clustering and vehicle rigid body motion estimation are studied, and a point cloud local distortion correction method is given; on this basis, a point cloud distortion correction method in an unmanned driving scene is given. Experiments show that this method improves the accuracy of vehicle motion estimation by predicting the dynamic vehicle motion area, effectively corrects the point cloud distortion, improves the accuracy of three-dimensional reconstruction and three-dimensional positioning in unmanned driving, and effectively overcomes the safety hazards of intelligentization of unmanned driving motion behavior. Thus, the accuracy of intelligent perception of unmanned vehicles is improved, and it is expected to break through the barriers of intelligent decision-making of unmanned driving motion behavior.

[0265] The above description is only a description of the preferred embodiments of the present application, and is not any limitation on the scope of the present application. Any modification or modification made by any person skilled in the art according to the above disclosed technical content should be regarded as an equivalent effective embodiment, and belongs to the scope of protection of the technical scheme of the present application.

Claims

1. A point cloud distortion correction method based on dynamic vehicle detection in autonomous driving scenarios, characterized in that, Includes the following steps: Step 1. Point cloud distortion analysis and data generation; specifically including: Step 1.1 Point cloud distortion analysis; Step 1.2 Point cloud distortion data generation; Step 2. Global distortion correction of point clouds based on LiDAR motion analysis; Step 3. Point cloud local distortion correction based on vehicle motion analysis; specifically including: Step 3.1 Point cloud dynamic vehicle clustering method; Step 3.2 Estimation of dynamic vehicle rigid body motion; Step 3.3 Prediction of dynamic vehicle motion area; Step 3.4 Point cloud local distortion correction: The local distortion correction of point clouds is represented as follows: （25） in, For vehicles Local distortion compensation for the corresponding point cloud; For vehicles Midpoint cloud data points The corresponding timestamp; The timestamp when the lidar begins acquiring the first frame of point cloud data; The point cloud after local distortion correction Represented as equation (26): （26） in, This is the point cloud after local distortion correction; The point cloud after global distortion correction; For point clouds The number of vehicles in motion; For sports vehicles The corresponding point cloud data points; The algorithm for point cloud local distortion correction is as follows: Step 3.4.1 Initialize two consecutive frame point clouds and correct the point clouds using a global distortion correction method. and Then it is used as the input to the model; Step 3.4.2 Segment the point cloud using a dynamic vehicle clustering algorithm. And obtain the set of moving vehicles. At the same time, give The corresponding point cloud number of the vehicle in the middle; Step 3.4.3 Detect vehicles using a point cloud dynamic vehicle detection algorithm. The vehicle is in motion, and the point cloud is segmented based on the detection results. Obtain a point cloud containing only moving vehicles. ; Step 3.4.4 Traverse the vehicle set ,right Each vehicle The rigid body transformation matrix of each vehicle is calculated using a dynamic vehicle rigid body motion estimation method, and the calculation results are placed into a set. middle; Step 3.4.5: Point cloud data points exist. If the access has not been made, proceed to step 3.4.6; otherwise, proceed to step 3.4.

7. Step 3.4.6 Determine the point cloud Does it belong to the corresponding point of a moving vehicle? If not, directly convert the point cloud data points. Add to collection If it belongs to the category, obtain the corresponding vehicle number and from... The rigid body transformation matrix of the corresponding vehicle is obtained; based on the encoding results in the global distortion correction section of the point cloud based on LiDAR motion analysis, the timestamp of the corresponding point is decoded; the local distortion compensation of the point cloud is calculated, and the point cloud is corrected. The location is obtained as the point cloud data point. Point cloud data points Add to collection ; Step 3.4.7 Output the point cloud after local distortion correction. ; Step 4. Point cloud distortion correction in autonomous driving scenarios.

2. The point cloud distortion correction method based on dynamic vehicle detection in an autonomous driving scenario as described in claim 1, characterized in that, In step 1, point cloud distortion data is generated: Time required for LiDAR to collect one line of data The positions of the lidar and vehicle in the time-interval moving simulation scenario are given. Calculated from equation (6): （6） in, The time interval between two position changes; This refers to the scanning frequency of the lidar. This refers to the scanning resolution of the lidar. exist The distortion of the point cloud data points acquired by the lidar within a given time period is represented as follows: （7） in, for Due to the vehicle model set during the time interval The set of point cloud distortions caused by relative motion with the lidar; For lidar at intervals The motion vector, For the model middle Point in time interval The motion vector is affected by the time interval. Internal Model The effects of rotation and translation; For the affine transformation of the model; For the model In time interval The motion vector within.

3. The point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scenario as described in claim 1, characterized in that, Step 2: The global distortion of the point cloud is represented by equation (8): （8） in, For global distortion compensation of point clouds; For lidar at time intervals Internal movement; This indicates the global distortion correction of the point cloud based on the rigid body motion of the lidar. The vehicle was collecting the first... Speed ​​when using point clouds Equation (9) represents: （9） in, The vehicles started collecting data at the beginning of the first... Frame and start of acquisition Position at frame time; The time required for the lidar to acquire one frame of data; Data points caused by lidar motion distortion compensation amount From equation (10), we get: （10） in, To begin collecting the first The timestamp of the frame point cloud; For collecting data points The timestamp of the time; for and The difference between the two; Data points The unit direction vector to the origin of the lidar is determined solely by the lidar's vertical field of view, lidar resolution, and the number of columns the lidar has already scanned during the acquisition of the current frame. The column number Calculated from equation (11): （11） in, To define a point in a coordinate system with the lidar as the origin. The angle of the corresponding unit vector; LiDAR in the vertical direction Rotational changes on, then Equation (12) represents （12） in, Data points exist direction and Coordinates of direction; Data points distortion compensation amount The result is obtained by calculation using equation (13): （13） After linear interpolation, the point Corresponding unit vector angle Equation (14) represents: （14） Point cloud The point cloud obtained after eliminating distortions caused by lidar motion using a global distortion correction method Equation (15) represents: （15）。 4. The point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scenario as described in claim 3, characterized in that, Step 2, the algorithm process is as follows: Step 2.1 Input a point cloud frame with distortion. ; Step 2.2 If point cloud data points exist Proceed to step 2.3; Otherwise, proceed to step 2.8; Step 2.3 Parse and obtain point cloud data points coordinates ; Step 2.4 Based on coordinates and coordinates The angle of the lidar beam corresponding to the point cloud data point is calculated. Step 2.5 Calculate the number of the lidar beam in a frame of lidar data and correct it using linear interpolation; Step 2.6 Encode the lidar point cloud for subsequent experimental processing; simultaneously calculate the intra-frame time corresponding to the lidar beam. Step 2.7 Calculate the point cloud Global distortion of the point cloud in the x and y directions is calculated, and the global distortion is corrected. The corrected point cloud data points are then placed into... middle; Step 2.8 Output the point cloud after global distortion correction.

5. The point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scenario as described in claim 1, characterized in that, Step 3: Step 3.1 Point Cloud Dynamic Vehicle Clustering Method Dynamic distance Equation (16) represents: （16） in, Data points exist direction, direction and Coordinates of direction; The vertical field of view of the lidar is [size]. Take the largest vertical field of view; As a magnification factor, we take 1.5; When there are fewer vehicle data points, The calculation method is defined as follows: （17） in, This refers to the number of laser beams in the vertical direction of the lidar. As a magnification factor, we take 3; The algorithm for dynamic vehicle clustering in point clouds is as follows: Step 3.1.1 Input the point cloud frame processed by the Point Cloud Dynamic Vehicle Detection Method (FEMD). ; Step 3.1.2 Initialize the set of visited nodes Gathering with vehicles ; Step 3.1.3 Based on the point cloud The motion state of the data points will The data points are divided into dynamic point cloud sets. and static point cloud collection ; Step 3.1.4 Using dynamic point cloud sets Establish KDTree; Step 3.1.5 Point cloud data points exist. If the access is not successful, proceed to step 3.1.6; otherwise, proceed to step 3.1.

14. Step 3.1.6 If data points Not in the visited set If the condition is met, proceed to step 3.1.7; otherwise, proceed to step 3.1.

5. Step 3.1.7 Initialize the vehicle and queue and data points Add to a single vehicle set ,queue and the set of visited nodes middle; Step 3.1.8 If the queue If not empty, proceed to step 3.1.9; otherwise, proceed to step 3.1.

13. Step 3.1.9 Take the first element of the queue. And remove it from the queue, via Query distance The most recent 26 elements form a set. ; Step 3.1.10 If an element exists If the step has not been accessed, proceed to step 3.1.11; otherwise, proceed to step 3.1.

13. Step 3.1.11 If the point and The distance between them is less than the dynamic distance. If yes, proceed to step 3.1.12; otherwise, proceed to step 3.1.

10. Step 3.1.12 Transfer data points Add to a single vehicle set ,queue and the set of visited nodes middle; Step 3.1.13 If the vehicle If the number of midpoints is greater than 128, then add it to the vehicle set. middle; Step 3.1.14 Output the segmented vehicle set .

6. The point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scenario as described in claim 1, characterized in that, Step 3: Step 3.2 Estimation of rigid body motion of dynamic vehicle The distortion caused by different vehicles is corrected separately, that is: （18） in, For local distortion compensation of point clouds; This represents moving vehicles segmented from a point cloud using a vehicle aggregation method. Indicates the number of moving vehicles; Indicates the vehicle at time intervals Internal movement; Indicates according to vehicle Local distortion correction of point cloud on the motion; For point clouds Vehicles in Its time interval between two consecutive frames Internal movement Equation (19) represents: （19） in, Indicates vehicle Corresponding point cloud data points Rotational changes; Indicates vehicle Corresponding point cloud data points Translational changes; The expression can be represented in the form of an affine transformation, as shown in equation (20): （20） in, for The rotation matrix, and for The displacement matrix, which contains a total of six degrees of freedom, is the displacement matrix around the axis of rotation. Rotation and displacement of the axis; considering the vehicle as a rigid body, then for any... They all have the same rotation matrix and translation matrix; The coordinate system of the point cloud in the second frame is transformed to the coordinate system of the point cloud in the first frame; the coordinate transformation is also represented in the form of an affine transformation. （21） in, and These represent the coordinate systems of the first and second frames of point cloud data collected by the lidar, respectively. and These represent the rotation and displacement transformations of the lidar between the two frames of data acquired; Motion estimation based on point cloud registration is expressed by the following formula: （22） in, This represents the affine transformation matrix of the vehicle motion estimated by the point cloud registration algorithm; Indicates the point cloud in the second frame Finding point clouds The corresponding point; This indicates that the rigid body motion of the vehicle is estimated based on the relationship between corresponding points.

7. The point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scenario as described in claim 1, characterized in that, Step 3: Step 3.3 Prediction of Dynamic Vehicle Motion Area The following assumptions are made regarding the vehicle in its velocity direction and perpendicular to its velocity direction: The maximum acceleration of the vehicle in its velocity direction and perpendicular to its velocity direction is... The magnitude of the minimum acceleration is Then assume the vehicle's speed is... In a frame time interval Subsequently, its displacement in the velocity direction and perpendicular to the velocity direction may be: （23） in, Predict the displacement value in the direction of vehicle velocity. Predict displacement values ​​in the opposite direction to vehicle speed; and The displacement value is predicted perpendicular to the vehicle velocity direction and in opposite directions; This is the vehicle's maximum acceleration; The predicted vehicle range often does not completely cover the actual vehicle range. To avoid safety hazards, a compensation value is added to the predicted vehicle range, specifically the vehicle length in the direction of speed. Add the width of the vehicle to the direction perpendicular to the velocity. As shown in equation (24): （24） in, For vehicle speed and direction, the estimated range of the vehicle; For vehicles traveling in the opposite direction of their speed, the estimated range is provided. and These are the vehicle prediction ranges perpendicular to the vehicle speed direction, and they are in opposite directions. This is the compensation value; The range in which the vehicle may appear in the next frame is represented as follows The rectangular area formed by the boundary.

8. The point cloud distortion correction method based on dynamic vehicle detection in an unmanned driving scenario as described in claim 1, characterized in that, Step 4: The point cloud distortion correction method (MDDC) for autonomous driving scenarios corrects global distortions in point clouds through a global distortion correction method based on LiDAR motion analysis. Building upon this global distortion correction, it further corrects local distortions by analyzing the motion of dynamic vehicles within the scene, thus achieving point cloud distortion correction in autonomous driving scenarios. The algorithm process is as follows: Step 4.1 Input two consecutive point cloud frames and the speed of movement of lidar ; Step 4.2 Point cloud Point cloud obtained by global distortion correction ; Step 4.3 Point cloud Denoising and downsampling are performed to obtain the preprocessed point cloud. ; Step 4.4 Detect two frames of point cloud using the point cloud dynamic vehicle detection method. Dynamic vehicles in the data are used to obtain predicted labels. ; Step 4.5 Perform motion analysis on the dynamic vehicles in the point cloud based on the dynamic vehicle detection results to obtain vehicle motion information. ; Step 4.6 Based on vehicle motion information Local distortion correction is performed on the point cloud to obtain a distortion-free point cloud. ; Step 4.7 Output distortion-free point cloud .

Images