Multi-roadside laser radar sensor point cloud data temporal-spatial registration method

Abstract

The present invention relates to the technical field of laser radar target detection, and in particular to a multi-roadside laser radar sensor point cloud data temporal-spatial registration method. The method comprises: carrying out background filtering on laser point clouds collected by sensors, and taking ground coordinate planes formed by ground points as reference planes and rotating each reference plane, so that the heights of the ground points in different reference planes are kept consistent to obtain the same reference plane of different sensors; selecting central points of three pairs of traffic targets in the reference plane as reference points, calculating a two-dimensional plane coordinate transformation matrix of each sensor laser point cloud, and then using an iterative nearest point algorithm to calculate a three-dimensional point cloud transformation matrix of the laser point cloud after two-dimensional plane coordinate transformation, so as to carry out spatial registration; and extracting a motion track obtained by scanning the same traffic target object by different roadside laser radar sensors, and adjusting the traffic target identification time of the roadside laser radar sensors by means of a track time difference, so as to carry out time synchronization.

Description

A spatiotemporal registration method for point cloud data from multiple roadside lidar sensors Technical Field

[0001] The present invention relates to the technical field of laser radar target detection, and relates to a method for spatiotemporal registration of point cloud data of multiple roadside laser radar sensors. Background Art

[0002] With the rapid development of society and the economy, the number of vehicles on the road continues to rise, placing higher demands on traffic efficiency, road safety, and energy conservation and emission reduction in the road traffic system. This poses greater challenges to the research and development and application of intelligent traffic control technologies. To achieve precise control and refined services for road traffic, intelligent connected and autonomous driving technologies have emerged and are gradually becoming crucial support for the development of smart cars and intelligent transportation. High-resolution, lane-level, and trajectory-level micro-traffic data are prerequisites for safe driving of smart cars and refined intelligent traffic control. 3D LiDAR, as an emerging sensor, offers high accuracy, excellent real-time performance, and strong anti-interference capabilities. It can obtain information such as the position, direction, speed, and trajectory of surrounding traffic targets. Using LiDAR as roadside infrastructure can provide connected vehicles with comprehensive, reliable, and complete beyond-line-of-sight perception data. It can also provide regional, high-resolution data for refined and personalized traffic safety control and optimization. It can also provide timely conflict warnings and safety alerts for vulnerable groups (pedestrians, cyclists, etc.).

[0003] However, due to the limitations of LiDAR's operating principles and mechanical performance, a single roadside LiDAR is prone to problems such as discontinuous target detection and short distances due to occlusion and point cloud coefficients when scanning and detecting traffic targets. Collaborative perception using multiple roadside LiDARs can address the issues of target occlusion and sparse point clouds, effectively improving the detection accuracy of traffic targets and ensuring the continuity and integrity of traffic target trajectory tracking. To achieve accurate collaborative perception using multiple roadside LiDARs, the primary task is to unify the data space coordinate systems of different LiDAR sensors and synchronize the time of traffic target detection, i.e., to align the spatiotemporal coordinates of multiple LiDAR sensors.

[0004] Summary of the Invention

[0005] The present invention proposes a spatiotemporal registration method for point cloud data of multiple roadside lidar sensors. The method mainly consists of a data preprocessing module, a spatial registration module, and a time synchronization module, as shown in Figure 1. The method performs background filtering on the laser point clouds collected by each sensor to remove background points other than traffic targets and ground points. The ground coordinate plane formed by the ground points is used as the reference plane. The reference planes are rotated to keep the heights of the ground points in different reference planes consistent, thus obtaining the same reference plane for different sensors. The 3D point cloud of the traffic target is mapped to the reference plane. The center points of three pairs of traffic targets in the reference plane are selected as reference points. The 2D plane coordinate transformation matrix of the laser point cloud of each sensor is calculated. Then, the 3D point cloud transformation matrix of the laser point cloud after the 2D plane coordinate transformation is calculated using the iterative closest point algorithm, thereby achieving step-by-step spatial registration of the 3D laser point cloud from the 2D plane to the 3D plane. After mapping the laser point clouds of each sensor to the same spatial coordinate system, the motion trajectories obtained by different roadside lidar sensors scanning the same traffic target object are extracted, and the time difference of the motion trajectory of the same traffic target on different roadside lidar sensors is identified. The traffic target recognition time of the roadside lidar sensors is adjusted based on the time difference, thereby achieving time synchronization of the different roadside lidar sensors. This method can effectively overcome the problems of inaccurate and incomplete traffic target recognition caused by occlusion and sparse point clouds in traffic flow environments. It does not require the use of external equipment, can effectively reduce the maintenance cost of roadside lidar sensors, and achieve accurate, complete and consistent detection of traffic targets.

[0006] The technical solution of the present invention is to deploy roadside lidars at different locations on road sections or road intersections. The roadside lidars transmit lidar point cloud data to roadside edge devices via wireless or wired means, and the edge devices process the point cloud data to achieve spatiotemporal alignment of point cloud data from different roadside lidar sensors.

[0007] The present invention proposes a method for spatiotemporal registration of point cloud data from multiple roadside lidar sensors, which mainly includes the following features:

[0008] 1) Data preprocessing module

[0009] Because the surrounding environmental information acquired during the LiDAR sensor scanning process includes not only information about traffic targets but also information about the surrounding environment, such as buildings, trees, and the ground, to speed up subsequent laser point cloud data processing and reduce computer resource consumption, it is necessary to first filter out background point cloud data such as buildings and trees from the laser point cloud. First, the 3D laser point cloud space is gridded and the distribution characteristics of the point cloud within the grid are analyzed. Second, the grid space attributes are divided according to the change characteristics of the point cloud density: traffic area, edge area, and background area. Finally, the laser point cloud in the background area is eliminated to achieve background filtering of the laser point cloud from the roadside LiDAR sensor.

[0010] 2) Spatial registration module

[0011] The spatial registration module first applies a random sampling consensus algorithm to extract the ground coordinate planes containing the ground points in different point cloud sets, using the horizontal plane of the standard spatial coordinate system as the reference plane. The unit normal vectors of each ground plane are calculated relative to the unit normal vector of the reference plane. The rotation matrix between the ground plane unit normal vector and the reference plane normal vector is calculated. This rotation matrix is ​​then applied to the point cloud set coordinates to align the ground plane with the reference plane, thereby maintaining consistent ground point heights and achieving Z-axis height alignment of the point cloud datasets. Next, the 3D point cloud of the traffic target is projected onto the XY plane. The center points of three pairs of traffic targets in the 2D plane are selected as reference points, and the 2D coordinate transformation matrix of each sensor's laser point cloud is calculated. Finally, the 2D coordinate transformation matrix is ​​converted to a 3D point cloud transformation matrix. This matrix is ​​used as the initial value and then fed into the iterative closest point algorithm along with the two point cloud sets to obtain the optimal 3D point cloud transformation matrix, thus achieving spatial registration of the 3D laser point clouds, as shown in Figure 2.

[0012] 3) Time synchronization module

[0013] The time synchronization module first maps the laser point clouds from each sensor to the same spatial coordinate system. It then extracts the motion trajectories obtained by different roadside LiDAR sensors scanning the same traffic target. Next, it uses a tracking algorithm to complete missing target trajectories, removing trajectories with excessive missing trajectories and retaining the complete target trajectories. The kinematic features of these filtered trajectories are then calculated. A weighted function is then calculated between the distances and kinematic features between trajectories in different point cloud sets until the weighted function converges between frames, thus achieving time synchronization between the different roadside LiDAR sensors, as shown in Figure 3. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1: Flowchart of a method for spatiotemporal registration of point cloud data from multiple roadside lidar sensors.

[0015] Figure 2: Flowchart of spatial registration of point cloud data from multiple roadside lidar sensors.

[0016] Figure 3: A flowchart for time synchronization of point cloud data from multiple roadside lidar sensors. DETAILED DESCRIPTION

[0017] The present invention proposes a spatiotemporal registration method for point cloud data of multiple roadside lidar sensors, which mainly consists of a data preprocessing module, a spatial registration module, and a time synchronization module. The method performs background filtering on the laser point clouds collected by each sensor to remove background points other than traffic targets and ground points. The ground coordinate plane formed by the ground points is used as the reference plane. The reference planes are rotated to keep the heights of the ground points in different reference planes consistent, thus obtaining the same reference plane for different sensors. The 3D point cloud of the traffic target is mapped to the reference plane. The center points of three pairs of traffic targets in the reference plane are selected as reference points. The 2D plane coordinate transformation matrix of the laser point cloud of each sensor is calculated. Then, the 3D point cloud transformation matrix of the laser point cloud after the 2D plane coordinate transformation is calculated using the iterative closest point algorithm, thereby achieving step-by-step spatial registration of the 3D laser point cloud from the 2D plane to the 3D plane. After mapping the laser point clouds of each sensor to the same spatial coordinate system, the motion trajectories obtained by different roadside lidar sensors scanning the same traffic target object are extracted, and the time difference of the motion trajectory of the same traffic target on different roadside lidar sensors is identified. The traffic target recognition time of the roadside lidar sensors is adjusted based on the time difference, thereby achieving time synchronization of the different roadside lidar sensors.

[0018] The present invention provides a method for spatiotemporal registration of point cloud data from multiple roadside lidar sensors, specifically implementing the following steps:

[0019] 1) Data processing module

[0020] The data processing module first divides the 3D laser point cloud space into grids and analyzes the distribution characteristics of the point clouds within the grid. It then divides the grid space into attributes based on the density variation of the point clouds: traffic area, edge area, and background area. Finally, it removes the laser point clouds in the background area to achieve background filtering of the laser point clouds from the roadside LiDAR sensor. The specific working steps are as follows:

[0021] Step 1: The edge device obtains point cloud data from different roadside lidar sensors;

[0022] Step 2: Let the first frame of one laser radar be A and any frame of the other laser radar be B. The difference between the timestamps of the first frames of the two laser radars should be minimized to find the first frame of the other laser radar corresponding to the first laser radar. The process should satisfy the following formula:

[0023] in, is the timestamp of frame A, is the timestamp of the B frame, The timestamp of the first frame of another lidar.

[0024] Step 3: Apply background filtering algorithm to filter out background point cloud and retain ground points and traffic object point cloud.

[0025] Step 3.1: Voxelize the point cloud space and divide it into continuous subspaces with a side length of 0.5m. Count the point cloud density characteristics of each subspace over a continuous time period.

[0026] Step 3.2: Based on the subspace density characteristics, the background space and the traffic participant passage area are distinguished and the traffic participant passage area is extracted. The spatial location of the point cloud is determined to be within the passage area, and the background points that are not in this space are eliminated to achieve background filtering.

[0027] 2) Spatial registration module

[0028] The spatial registration module first applies a random sampling consensus algorithm to extract the ground coordinate plane where the ground points in different point cloud sets are located, and uses the horizontal plane of the spatial standard coordinate system as the reference plane. Calculate the unit normal vectors of different ground planes and the unit normal vectors of the reference plane respectively. Calculate the rotation matrix between the unit normal vector of the ground plane and the normal vector of the reference plane, and operate the rotation matrix with the coordinates of the point cloud set to realize the ground plane so that it coincides with the reference plane, thereby keeping the height of the ground points consistent and achieving the Z-axis height alignment of the point cloud data set. Secondly, project the 3D point cloud of the traffic target into the XY plane, select the center points of 3 pairs of traffic targets in the 2D plane as reference points, and calculate the 2D plane coordinate transformation matrix of each sensor laser point cloud. Finally, convert the 2D plane coordinate transformation matrix into a 3D point cloud transformation matrix, use this as the initial value and input it into the iterative closest point algorithm with the two point cloud sets to obtain the optimal 3D point cloud transformation matrix, thereby achieving spatial registration of the 3D laser point cloud; the specific working steps are as follows:

[0029] Step 1: After data preprocessing, the point cloud contains ground points and traffic target points. When performing spatial registration, it is difficult to directly filter out points that appear simultaneously in different lidars. This invention divides three-dimensional spatial registration into two steps. The first step is to unify the height along the Z axis.

[0030] Step 1.1: In practical applications, a random sampling consensus algorithm is used to extract the ground plane, which is the plane with the most ground points fitted.

[0031] Step 1.2: Rotate and translate the ground planes in different LiDAR coordinate systems so that they coincide with each other at the same height. Assume that the ground plane equation is: Ax + By + Cz + D = 0, then the surface normal vector of the ground plane is: Normalize the surface normal vector of the ground plane to:

[0032] Among them: A, B, C, D are plane parameters; is the surface normal vector of the extracted ground plane; for Normalized vector;

[0033] Step 1.3: Assume that the unit normal vector of the standard horizontal plane is: According to the formula Calculate the transformation matrix between the ground plane unit normal vector and the standard horizontal plane, where R is the transformation matrix from the ground plane to the standard horizontal plane;

[0034] Step 2: After the Z-axis height is unified, multiple point clouds need to be rotated and translated in the two-dimensional horizontal plane. The center point of the vehicle target on the road is used as the alignment reference point. The specific steps include the following:

[0035] Step 2.1: Ignoring the Z-axis coordinate of the point cloud, project the 3D point cloud data onto a 2D plane.

[0036] Step 2.2: Apply a density-based noise spatial clustering algorithm to cluster the two-dimensional point cloud, and then apply an axis-aligned bounding box algorithm to enclose the clustered targets to obtain the bounding box of the road vehicle target;

[0037] Step 2.3: Define the intersection area covered by the laser radar scan, select the target vehicle in the intersection area, and obtain the center coordinates of its bounding box, recorded as (x i ,y i ).

[0038] Step 2.4: If the length of the vehicle bounding box is length box ≤4.8m, then the corrected vehicle bounding box length is 4.8m; if the width of the vehicle bounding box width box ≤1.8m, then the width of the corrected vehicle bounding box is 1.8m; if the length of the vehicle bounding box is length box ≥4.8m, the length of the bounding box still applies the original value; if the width of the vehicle bounding box is box ≥1.8m, the bounding box width still uses the original value;

[0039] Step 2.5: Update the target vehicle center coordinates based on the corrected bounding box to satisfy the following formula: (X i ,Y i )=(x i ±(length-length box ),y i ±(width-widthbox ))

[0040] Where: (x i ,y i ) is the center coordinate of the initial target vehicle bounding box; length and width are the updated bounding box length and width after correction; length box and width box is the length and width of the initial vehicle bounding box; (X i ,Y i ) is the updated target vehicle bounding box center coordinate; the addition and subtraction signs in the formula are determined by the vehicle's moving direction: if the vehicle coordinates of the next frame are to the right of the vehicle coordinates of the previous frame, the vehicle moves from left to right, and when updating the bounding box center coordinates, the original coordinates need to be subtracted from the bounding box difference; if the vehicle coordinates of the next frame are to the left of the vehicle coordinates of the previous frame, the vehicle moves from right to left, and when updating the bounding box center coordinates, the original coordinates need to be added to the bounding box difference;

[0041] Step 2.6: Calculate at least three sets of corresponding vehicle bounding box center points and solve the transformation matrix of the two-dimensional coordinates. The calculation process satisfies the following formula:

[0042] Where: T 2D is the transformation matrix between two-dimensional reference points; (a i ,b i ) is a reference point of one of the lidars, (A i ,B i ) is another lidar reference point.

[0043] Step 2.7: Convert the transformation matrix of the two-dimensional point cloud to the transformation matrix of the three-dimensional point cloud. The derivation process is as follows:

[0044] Where: T 2D is the transformation matrix between the two-dimensional reference points calculated in the previous step; T 3D is the derived three-dimensional transformation matrix; θ is the rotation angle; t x is the translation distance in the X-axis direction; t y is the translation distance in the Y-axis direction;

[0045] The iterative closest point algorithm is used as the initial value to iteratively calculate the transformation matrix of the global 3D point cloud, and finally achieve global point cloud spatial registration;

[0046] 3) Time synchronization module

[0047] The time synchronization module first maps the laser point clouds of each sensor to the same spatial coordinate system and then extracts the motion trajectories obtained by different roadside LiDAR sensors scanning the same traffic target object. Next, a tracking algorithm is used to complete the target's missing trajectories, removing any target trajectories with excessive missing trajectories and retaining the complete target trajectories. The kinematic features of the filtered trajectories are then calculated, and a weighting function is calculated between the distances and kinematic features between trajectories in different point cloud sets until the weighting function converges between frames, thereby achieving time synchronization between the different roadside LiDAR sensors. The specific steps are as follows:

[0048] Step 1: Use the center point of the target vehicle's bounding box as the tracking point for target tracking. The steps for obtaining the tracking point are as follows:

[0049] Step 1.1: Get the center coordinates of each target vehicle's bounding box in each frame and mark them as (x i ,y i ). Count the length and width of the vehicle bounding box and calculate the cumulative distribution curve of the length and width of the vehicle bounding box. Select the length and width of the bounding box corresponding to the 95% percentile and record it as length 95％ and width 95％ ;

[0050] Step 1.2: If the length of the vehicle bounding box is length box ≤length 95％ , then the corrected vehicle bounding box length is length 95％ ; If the width of the vehicle bounding box is width box ≤width 95％ , then the corrected vehicle bounding box length is width 95％ ; If the length of the vehicle bounding box is length box ≥length 95％ , the length of the bounding box still uses the original value; if the width of the vehicle bounding box is width box ≥width 95％ , the bounding box width still uses the original value;

[0051] Step 1.3: Update the target vehicle center coordinates based on the corrected bounding box to satisfy the following formula: (X i ,Y i )=(x i ±(length-length box ),y i ±(width-width box ))

[0052] Where: (x i ,y i) is the center coordinate of the initial target vehicle bounding box, length and width are the updated bounding box length and width after correction; length box and width box is the length and width of the initial vehicle bounding box; (X i ,Y i ) is the updated target vehicle bounding box center coordinate; the addition and subtraction signs in the formula are determined by the vehicle's moving direction: if the vehicle coordinates of the next frame are to the right of the vehicle coordinates of the previous frame, the vehicle moves from left to right, and when updating the bounding box center coordinates, the original coordinates need to be subtracted from the bounding box difference; if the vehicle coordinates of the next frame are to the left of the vehicle coordinates of the previous frame, the vehicle moves from right to left, and when updating the bounding box center coordinates, the original coordinates need to be added to the bounding box difference.

[0053] Step 2: After the tracking point screening is completed, the distance matrix between the targets in two adjacent frames is calculated; the calculation formula is:

[0054] Where: P ix is the x-coordinate value of the i-th target in the previous frame; Q ix is the x-coordinate value of the i-th target in the current frame; P iy is the y coordinate value of the i-th target in the previous frame; Q ix is the y coordinate value of the i-th target in the current frame; is the distance between the i-th target in the previous frame and the i-th target in the current frame;

[0055] Step 2.1: After selecting the tracking points, calculate the distance matrix between the tracking points in the previous frame and the tracking points in the current frame, and associate the same target points in the previous and next frames; the distance matrix operation is as follows:

[0056] Among them: F k-1 is the tracking point set of the previous frame; F k Is the current frame tracking point set; set F k-1 There are m points in the set F k There are n points in D ij Indicates the previous frame F k-1 The i-th point in the current frame F k The distance between the jth points in ;

[0057] Step 2.2: Use the relevant data association algorithm to associate the same target in the previous and next frames and preliminarily extract the target trajectory; the target trajectory is recorded as: Tr = {P0,…,P i ,…P n};

[0058] Where: P iis the position of the target in the i-th frame, and each position coordinate P i Contains the spatial position and time information of the target, specifically expressed as P i =(x i ,y i ,t i );(x i ,y i ) is the two-dimensional coordinate of the target center point, t i It is the current corresponding timestamp information;

[0059] Step 3: After the trajectory extraction is completed, the incomplete trajectory needs to be corrected. The Kalman filter method is used to combine the historical target trajectory to predict the incomplete trajectory, so that the detected target trajectory is more complete. The relevant parameters include the state vector x k , control vector z k , state transfer matrix A and control matrix H; the specific calculation formula is: k =[xyv x v y ] T z k =[xy] T

[0060] Where: x is the horizontal coordinate value of the tracking point in the two-dimensional plane; y is the vertical coordinate value of the tracking point in the two-dimensional plane; v x is the velocity of the tracking point in the horizontal direction; v y The velocity of the tracking point in the vertical direction; t is the time interval between the previous and next frames;

[0061] For a tracking target, if the interruption trajectory is less than or equal to 10 frames, the above method is used to correct the trajectory to predict the trajectory; if the interruption trajectory exceeds 10 frames, the target trajectory is deleted and no subsequent trajectory matching is performed; after the screening is completed, multiple trajectories can be obtained to form a trajectory set, which is recorded as: TR Set ={TR1,TR2,TR3,…,TR n};

[0062] Among them: TR i Represents a trajectory of a target, including position coordinates and time information;

[0063] Step 4: The same target detected by different LiDARs should have the same motion state at the same time. Use velocity, acceleration, and curvature features to represent the vehicle motion characteristics: Assuming that a trajectory has n position coordinates, there are n-1 velocity, n-2 acceleration, and n-1 curvature features. For a trajectory Tr = {P0, ..., P i,…P n}, we can get the velocity set V={v0,…,v i ,…v n-1}, acceleration set A={a0,…,a i ,…a n-2}, curvature set C={c0,…,c i ,…c n-1};

[0064] The calculation formulas for velocity, acceleration, and curvature are as follows:

[0065] Where: v i is the speed of the target at time i; a i is the acceleration of the target at time i; c i Indicates P i to P i+1 With P i to P i-1 The angle between and The angle between i is the position of the target at time i; dis(P i+1 -P i ) indicates P i+1 With P i Δt is the time difference between two consecutive frames. Taking the sensor frequency as 10 Hz as an example, the time difference between two consecutive frames is 0.1 seconds.

[0066] Step 5: Use the transformation matrix in spatial registration to transform the trajectory from one lidar to the coordinate system of the other lidar; match the trajectory sets from the two lidars as follows:

[0067] Step 5.1: Compare the distance difference and motion state characteristics between the trajectories in turn, and set the weight function as: D = ρ a |Δdis|+ρ b |Δv|+ρ c |Δa|+ρ d |Δc|

[0068] Where: i is an adjustable weight parameter; Δdis is the distance difference between trajectories; Δv is the velocity difference between trajectories; Δa is the acceleration difference between trajectories; Δc is the curvature difference between trajectories;

[0069] Step 5.2: If D ≤ ε, the two trajectories are considered to belong to the same target and are considered to be successfully matched.

[0070] Step 5.3: If D > ε, calculate the trajectory weight of the next frame until D ≤ ε. If D ≤ ε cannot be achieved after traversing all frames, the two trajectories are considered to belong to different targets and the two trajectory pairs are deleted.

[0071] Step 6: Filter out the successfully matched track pairs, take a smaller threshold δ, compare the matched track pairs frame by frame, and record the time corresponding to the first frame of the overlapping track. When all track pairs have the same starting frame time, the time synchronization of the two lidars is achieved.

Claims

1. A method for spatiotemporal registration of point cloud data from multiple roadside lidar sensors, the main features of which are include: 1) Spatial registration Firstly, the ground coordinate planes where the ground points in different point cloud sets are located are extracted by using the random sampling consensus algorithm, and the horizontal plane of the spatial standard coordinate system is used as the reference plane; the unit normal vectors of different ground planes and the unit normal vectors of the reference plane are calculated respectively; Calculate the rotation matrix between the unit normal vector of the ground plane and the normal vector of the reference plane, and operate the rotation matrix with the coordinates of the point cloud set to realize the ground plane so that it coincides with the reference plane, so that the height of the ground points is consistent, and the Z-axis height alignment of the point cloud data set is achieved; secondly, the 3D point cloud of the traffic target is projected into the XY plane, and the center points of 3 pairs of traffic targets in the 2D plane are selected as reference points to calculate the 2D plane coordinate transformation matrix of each sensor laser point cloud; finally, the 2D plane coordinate transformation matrix is ​​converted into a 3D point cloud transformation matrix, which is used as the initial value to input it into the iterative closest point algorithm with the two point cloud sets to obtain the optimal 3D point cloud transformation matrix, thereby realizing the spatial registration of the 3D laser point cloud; the specific working steps are as follows: Step 1: After data preprocessing, the point cloud contains ground points and traffic target points. It is difficult to directly filter out points that appear simultaneously in different laser radars when performing spatial position registration. The present invention divides the three-dimensional spatial position registration into two steps. First, the height of the Z-axis direction is unified. Step 1.1: In practical applications, a random sampling consensus algorithm is used to extract the ground plane, which is the plane with the most ground points fitted; Step 1.2: Rotate and translate the ground planes in different laser radar coordinate systems so that they coincide with each other at the same height; suppose the ground plane equation is: Ax+By+Cz+D=0, then the surface normal vector of the ground plane is: Normalize the surface normal vector of the ground plane to: Among them: A, B, C, D are plane parameters; is the surface normal vector of the extracted ground plane; for The normalized vector; Step 1.3: Assume the unit normal vector of the standard horizontal plane is: According to the formula Calculate the transformation matrix between the unit normal vector of the ground plane and the standard horizontal plane, where R is the transformation matrix from the ground plane to the standard horizontal plane; Step 2: After the Z-axis height is unified, multiple point cloud sets need to be rotated and translated in the two-dimensional horizontal plane; the center point of the vehicle target on the road is used as the alignment reference point, which specifically includes the following steps: Step 2.1: Ignoring the Z-axis coordinate of the point cloud, project the three-dimensional point cloud data onto a two-dimensional plane; Step 2.2: Apply density-based noise spatial clustering algorithm to target the two-dimensional point cloud Clustering, then applying the axis-aligned bounding box algorithm to enclose the clustered targets to obtain the bounding box of the road vehicle target; Step 2.3: Define the intersection area covered by the laser radar scan, select the target vehicle in the intersection area, and obtain the center coordinates of its bounding box, recorded as (x i ,y i ); Step 2.4: If the length of the vehicle bounding box is length box ≤4.8m, the corrected vehicle bounding box length is 4.8m; if the width of the vehicle bounding box is box ≤1.8m, the corrected vehicle bounding box width is 1.8m; if the length of the vehicle bounding box is length box ≥4.8m, the length of the bounding box still uses the original value; if the width of the vehicle bounding box is box ≥1.8m, the bounding box width still uses the original value; Step 2.5: Update the target vehicle center coordinates according to the corrected bounding box to satisfy the following formula: (X i ,Y i )=(x i ±(length-length box ),y i ±(width-width box )) Where: (x i ,y i ) is the center coordinate of the initial target vehicle bounding box; length and width are the length and width of the bounding box updated after correction; length box and width box is the length and width of the initial vehicle bounding box; (X i ,Y i ) is the updated target vehicle bounding box center coordinate; the addition and subtraction signs in the formula are determined according to the vehicle moving direction: if the vehicle coordinates of the next frame are to the right of the vehicle coordinates of the previous frame, the vehicle moves from left to right, and when updating the bounding box center coordinates, the original coordinates need to be subtracted from the bounding box difference; if the vehicle coordinates of the next frame are to the left of the vehicle coordinates of the previous frame, the vehicle moves from right to left, and when updating the bounding box center coordinates, the original coordinates need to be added to the bounding box difference; Step 2.6: Calculate at least three groups of corresponding vehicle bounding box center points and solve the transformation matrix of the two-dimensional coordinates. The calculation process satisfies the following formula: Where: T 2D is the transformation matrix between two-dimensional reference points; (a i ,b i ) is a reference point of one of the lidars, (A i ,B i ) is another lidar reference point; Step 2.7: Convert the transformation matrix of the two-dimensional point cloud to the transformation matrix of the three-dimensional point cloud. The derivation process is as follows: Where: T 2D is the transformation matrix between the two-dimensional reference points calculated in the previous step; T 3D is the derived three-dimensional transformation matrix; θ is the rotation angle; t x is the translation distance in the X-axis direction; t y is the translation distance in the Y-axis direction; The iterative closest point algorithm is used as the initial value to iteratively calculate the transformation matrix of the global three-dimensional point cloud, and finally achieve global point cloud space registration; 2) Time synchronization First, after mapping the laser point clouds of each sensor to the same spatial coordinate system, the motion trajectories obtained by scanning the same traffic target object by different roadside lidar sensors are extracted; secondly, the tracking algorithm is used to complete the missing target trajectory to make the trajectory more complete, and the target trajectory with too many missing trajectories is deleted to retain the complete target trajectory; then the kinematic characteristics of the screened trajectory are calculated, and the weight function of the distance and kinematic characteristics between the trajectories in different point cloud sets is calculated until the weight function value between frames converges, thereby achieving time synchronization of different roadside lidar sensors; the specific working steps are as follows: Step 1: Use the center point of the target vehicle's bounding box as the tracking point for target tracking. The steps for obtaining the tracking point are as follows: Step 1.1: Get the center coordinates of each target vehicle encirclement frame in each frame (x i ,y i ) ; Count the length and width of the vehicle bounding box and calculate the cumulative distribution curve of the length and width of the vehicle bounding box, select the length and width of the bounding box corresponding to the 95% percentile, and record it as length 95％ and width 95％ ; Step 1.2: If the length of the vehicle bounding box is length box ≤length 95％ , then the corrected vehicle bounding box length is length 95％ ; If the width of the vehicle bounding box is width box ≤width 95％ , then the corrected vehicle bounding box length is width 95％ ; If the length of the vehicle bounding box is length box ≥length 95％ , the length of the bounding box still uses the original value; if the width of the vehicle bounding box is width box ≥width 95％ , the bounding box width still uses the original value; Step 1.3: Update the target vehicle center coordinates according to the corrected bounding box to satisfy the following formula: (X i ,Y i )=(x i ±(length-length box ),y i ±(width-width box )) Where: (x i ,y i ) is the center coordinate of the initial target vehicle bounding box, length and width are the length and width of the bounding box after correction; length box and width box is the length and width of the initial vehicle bounding box; (X i ,Y i ) is the updated target vehicle bounding box center coordinate; the addition and subtraction signs in the formula are determined according to the vehicle moving direction: if the vehicle coordinates of the next frame are to the right of the vehicle coordinates of the previous frame, the vehicle moves from left to right, and when updating the bounding box center coordinates, the original coordinates need to be subtracted from the bounding box difference; if the vehicle coordinates of the next frame are to the left of the vehicle coordinates of the previous frame, the vehicle moves from right to left, and when updating the bounding box center coordinates, the original coordinates need to be added to the bounding box difference; Step 2: After the tracking point screening is completed, the distance matrix between the targets in two adjacent frames is calculated; the calculation formula is: Where: P ix is the x-coordinate value of the i-th target in the previous frame; Q ix is the x-coordinate value of the i-th target in the current frame; P iy is the y coordinate value of the i-th target in the previous frame; Q ix is the y coordinate value of the i-th target in the current frame; is the distance between the i-th target in the previous frame and the i-th target in the current frame; Step 2.1: After selecting the tracking points, calculate the distance matrix between the tracking points of the previous frame and the tracking points of the current frame, and associate the same target points in the previous and next frames; the operation of the distance matrix is ​​as follows: Among them: F k-1 is the tracking point set of the previous frame; F k is the tracking point set of the current frame; set F k-1 There are m points in the set F k There are n points in D ij Indicates the previous frame F k-1 The i-th point in the current frame F k The distance between the jth points in ; Step 2.2: Use the relevant data association algorithm to associate the same target in the previous and next frames and preliminarily extract the target trajectory; the target trajectory is recorded as: Tr = {P0,…,P i ,…P n }; Where: P i is the position of the target in the i-th frame, and each position coordinate P i Contains the spatial position and time information of the target, specifically represented by P i =(x i ,y i ,t i );(x i ,y i ) is the two-dimensional coordinate of the target center point, t i It is the current corresponding timestamp information; Step 3: After the trajectory extraction is completed, the incomplete trajectory needs to be corrected. The Kalman filter method is used to combine the historical target trajectory to predict the incomplete trajectory, so that the detected target trajectory is more complete; the relevant parameters include the state vector x k , control vector z k , state transfer matrix A and control matrix H; specific calculation formula The formula is: x k =[x y v x v y ] T z k =[x y] T Where: x is the horizontal coordinate value of the tracking point in the two-dimensional plane; y is the vertical coordinate value of the tracking point in the two-dimensional plane; v x is the velocity of the tracking point in the horizontal direction; v y The speed of the tracking point in the vertical direction; t is the time interval between the previous and next frames; For a tracking target, if the interruption trajectory is less than or equal to 10 frames, the above method is used to correct the trajectory to predict the trajectory; if the interruption trajectory exceeds 10 frames, the target trajectory is deleted and no subsequent trajectory matching is performed; after the screening is completed, multiple trajectories can be obtained to form a trajectory set, which is recorded as: TR Set = {TR1,TR2,TR3,…,TR n }; Among them: TR i Represents a trajectory of a target, including position coordinates and time information; Step 4: The same target detected by different laser radars should have the same motion state at the same time; use velocity, acceleration and curvature features to represent the vehicle motion characteristics: Assuming that a trajectory has n position coordinates, there are n-1 velocity, n-2 acceleration and n-1 curvature features; for a trajectory Tr = {P0,…,P i ,…P n }, we can get the velocity set V = {v0,…,v i ,…v n-1 }, acceleration set A={a0,…,a i ,…a n-2 }, the curvature set C = {c0,…,c i ,…c n-1 }; The calculation formulas for velocity, acceleration, and curvature are as follows: Where: v i is the speed of the target at time i; a i is the acceleration of the target at time i; c i Indicates P i To P i+1 With P i To P i-1 The angle between and The angle of i is the position of the target at time i; dis(P i+1 -P i ) indicates P i+1 With P i Δt is the time difference between two consecutive frames. Taking the sensor frequency as 10 Hz as an example, the time difference between two consecutive frames is 0.1 seconds. Step 5: Use the transformation matrix in spatial registration to transform the coordinate system of the trajectory from one laser radar with that of another laser radar; match the trajectory sets from the two laser radars as follows: Step 5.1: Compare the distance difference and motion state characteristics between the trajectories in turn, and set the weight function as: D=ρ a |Δdis|+ρ b |Δv|+ρ c |D|+r d |Δc| Where: i is an adjustable weight parameter; Δdis is the distance difference between trajectories; Δv is the velocity difference between trajectories; Δa is the acceleration difference between trajectories; Δc is the curvature difference between trajectories; Step 5.2: If D ≤ ε, the two trajectories are considered to belong to the same target and are considered to be successfully matched trajectories. Step 5.3: If D>ε, calculate the trajectory weight of the next frame until D≤ε; if D≤ε cannot be made after traversing all frames, it is considered that the two trajectories belong to different targets and the two trajectory pairs are deleted; Step 6: Filter out the successfully matched trajectory pairs, take a smaller threshold δ, compare the matched trajectory pairs frame by frame, and record the time corresponding to the first frame starting from the overlapping trajectory. When all trajectory pairs satisfy the same starting frame time, the time synchronization of the two lidars is achieved.

Images