A real-time removal method for moving targets using LiDAR assisted by millimeter-wave radar

Abstract

This invention discloses a real-time method for removing moving targets from a laser radar (LiDAR) using millimeter-wave radar. The specific implementation steps are as follows: 1) millimeter-wave point cloud clustering; 2) ground point removal and ground point cloud clustering from the laser point cloud; 3) platform motion estimation based on a laser point cloud registration method; 4) moving target detection from the millimeter-wave point cloud; 5) moving target mapping from the millimeter-wave point cloud; and 6) moving target detection and removal from the laser point cloud. This method fully utilizes the characteristics of both sensors, detecting moving targets in the scene using the Doppler frequency extracted by the millimeter-wave radar. Coordinate mapping is performed using the geometric relationship between the two sensors to detect and remove moving targets from the laser point cloud, laying the foundation for subsequent applications such as three-dimensional reconstruction of laser point clouds.

Description

Technical field:

[0001] The present invention relates to the field of three-dimensional modeling of scenes, and in particular to a method for real-time detection and removal of moving targets using a three-dimensional laser radar. Background technology:

[0002] LiDAR (LiDAR) is a sensor that uses laser light as a light source for ranging. It emits laser light into the surrounding area and determines the distance to an object based on the time interval between emission and reception. The object's position is then calculated based on the distance and the angle of the laser beam. LiDAR operates over a wide wavelength range, typically between 600nm and 1600nm. This allows for high ranging accuracy and the ability to precisely digitize the actual scene. This has led to LiDAR gradually replacing vision as the primary sensor in simultaneous localization and mapping (SLAM) systems. Laser SLAM estimates the pose of prominent objects in a scene, such as buildings, tree trunks, and light poles, based on their geometric features. This assumes that the objects are stationary. However, in real-world applications, moving objects such as pedestrians and vehicles often exist, which directly impacts the accuracy of 3D reconstruction and can even lead to reconstruction failure.

[0003] By removing moving targets from the LiDAR scene in real time, the interference of moving targets on the pose estimation process in the three-dimensional reconstruction of laser point clouds can be reduced, thereby improving the robustness of pose estimation. Existing laser point cloud moving target detection algorithms are basically divided into three categories: frame difference method, change detection, and convolutional neural network target detection. However, the frame difference method is prone to "smear" and "holes", change detection requires multiple frames of accumulation and is difficult to achieve the purpose of real-time processing, and the convolutional neural network method is difficult to achieve the purpose of real-time processing due to its high complexity and low operating efficiency. To this end, the present invention introduces millimeter-wave radar to assist LiDAR in moving target detection. Summary of the invention:

[0004] The purpose of this invention is to provide a real-time detection method for moving targets in laser radar point clouds. The key is to effectively fuse the information obtained by millimeter wave radar and laser radar based on the spatiotemporal alignment of sensors to achieve real-time removal of moving targets in laser point clouds.

[0005] The present invention follows the following technical solution: a millimeter wave radar-assisted laser radar moving target real-time removal method, comprising the following steps:

[0006] Step 1: Cluster the millimeter-wave point cloud to obtain the number of points, center position, and average speed of each point cloud cluster in the millimeter-wave radar coordinate system;

[0007] Step 2: Remove ground points from the laser point cloud and cluster the ground point cloud to obtain the number of points and center position of each point cloud cluster in the laser radar coordinate system;

[0008] Step 3: Estimate the platform motion based on the laser point cloud registration method and obtain the platform velocity v x ;

[0009] Step 4: Millimeter wave point cloud moving target detection;

[0010] Step 5: Millimeter wave point cloud moving target mapping;

[0011] Step 6: Detection and removal of moving targets in laser point cloud.

[0012] The advantages of the present invention compared with the prior art are:

[0013] (1) This invention utilizes a combination of millimeter-wave radar and lidar to detect moving targets. The millimeter-wave radar data is used to calculate the Doppler frequency in real time, and the sensor platform's velocity is estimated based on the lidar data. The combination of these two methods enables detection of moving targets in a scene. This method is both highly sensitive and real-time.

[0014] (2) Based on the pre-calculated spatial geometric relationship between the lidar and the millimeter-wave radar, the point cloud coordinate conversion between the two sensors is realized, and the implementation process does not involve complex calculations and is simple and easy. Description of the drawings:

[0015] Figure 1 :Method implementation flow chart.

[0016] Figure 2 : The geometric relationship between the moving target and the platform.

[0017] Figure 3 : Single-frame laser point cloud containing moving targets.

[0018] Figure 4 : Real-time detection results of moving targets in millimeter wave point cloud.

[0019] Figure 5 : Single-frame laser point cloud after removing moving targets. Specific implementation method:

[0020] The implementation process of the present invention is as follows Figure 1 The specific implementation steps are as follows:

[0021] 1. Millimeter-wave point cloud clustering

[0022] Cluster the millimeter wave point cloud into N points by the Euclidean distance between point clouds. M point cloud clusters with different centers, each point cloud cluster can be represented by the number of points m in the cluster.i , cluster center position and cluster average velocity v i To describe, there is 1≤i≤N M , the superscript M represents the millimeter wave radar coordinate system. The center position and average speed are calculated as follows:

[0023]

[0024] In the formula is the three-dimensional coordinate of the jth point in the i-th point cloud cluster, is the velocity of the jth point in the i-th point cloud cluster, 1≤j≤m i Velocity can be calculated using the coherent integration method, using FFT to obtain the Doppler frequency, which is then converted to velocity. To eliminate the influence of noise, point cloud clusters with fewer than a certain number of points are filtered out. The remaining point cloud clusters are potential moving targets in the scene.

[0025] 2. Laser point cloud ground point removal and ground point cloud clustering

[0026] The coordinates of any point in the laser point cloud are The superscript L represents the laser radar coordinate system. The laser point cloud is subjected to ground point removal and ground point cloud clustering. Ground point removal can utilize the height information of the point cloud. Points with a height less than a set threshold are directly removed as ground points, as shown in the following formula (3), where th height This is the set ground point height threshold, which can be set according to the degree of scene undulation.

[0027]

[0028] After removing the ground points, the region growing algorithm is used to cluster the ground point cloud into N L non-overlapping point cloud clusters, where the kth laser point cloud cluster is R k ={n k ,(x k ,y k ,z k )},n k is the number of points contained in the k-th point cloud cluster, (x k ,y k ,z k ) is the center position of the point cloud cluster, and 1≤k≤N L .

[0029] 3. Platform motion estimation based on laser point cloud registration

[0030] In actual observation, the platform equipped with millimeter-wave radar and laser radar is also in motion. By registering two adjacent frames of laser point cloud data, the platform's own motion, including speed and direction, can be estimated. Assume that the registration result of the two frames of laser point cloud data is R,t, where R is the platform's rotation amount and t = (t x ,t y ,t z ) is the translation of the platform. The specific registration algorithm can adopt the ICP algorithm or a nonlinear optimization method based on feature points, such as the LM optimization algorithm.

[0031] According to the X-direction translation t x And the time difference between frames ΔT, the platform's motion speed v can be estimated x .

[0032]

[0033] 4. Millimeter-wave point cloud moving target detection

[0034] The basis of millimeter wave radar moving target detection is Doppler frequency. However, due to the simultaneous movement of the platform and the target, and the possible deviation of the target position from the X-axis direction, the targets at different positions have different Doppler frequencies. Based on the center position of each point cloud cluster obtained in step 1, Average speed v i and the platform velocity v obtained in step 3 x , we can use threshold detection to determine the moving targets in the scene. Figure 2 In the scene shown, for the i-th point cloud cluster in step 1, when the following equation (5) is satisfied, the i-th point cloud cluster is a moving target.

[0035] |v x cosθ-v i |>v th (5)

[0036] where v th is the moving target detection threshold, θ=atan(y i / x i ).

[0037] 5. Millimeter-wave point cloud moving target mapping

[0038] Based on the millimeter wave point cloud motion target detection, the geometric relationship between the millimeter wave radar and the lidar is used to map the motion target detected by the millimeter wave radar to the lidar coordinate system. The geometric relationship between the two sensors can be expressed as the external parameter matrix and Represents the rotation matrix from the millimeter wave radar coordinate system to the lidar coordinate system, Represents the translation matrix from the millimeter-wave radar coordinate system to the lidar coordinate system. For any moving target in the millimeter-wave radar coordinate system, assume that the center position is The position mapped to the lidar coordinate system is:

[0039]

[0040] 6. Laser point cloud moving target detection and removal

[0041] Based on the results of millimeter-wave point cloud motion target detection, the detection and removal of moving targets in the laser point cloud are realized. Although the mechanisms of millimeter-wave radar and laser radar to obtain echo point clouds are different, in theory, after the millimeter-wave radar point cloud is mapped to the laser radar coordinate system, it has a close center position to the target in the laser radar. In addition, since the millimeter-wave radar has high range resolution and low azimuth resolution, the number of laser radar point clouds from the same target is much larger than the number of millimeter-wave radar point clouds, but the range direction target size is consistent. Based on this prior information, the detection rules for moving targets in the laser radar point cloud can be designed. For the moving target in the millimeter radar, according to the mapping relationship of formula (6), its mapping position in the laser radar is obtained. The mapping only changes the center coordinates of the point cloud cluster, without changing the number of points m contained. i , for the laser point cloud cluster R obtained in the second step k , the number of points included is n k , the center position of the point cloud cluster is When the following equation (7) is satisfied, the laser point cloud cluster R k For sports goals.

[0042]

[0043] In the above formula, |·| represents the distance between two points, and d is the distance threshold, which is no more than half of the target geometric size. L and Δx M are the sizes of the point cloud cluster along the X direction in the lidar coordinate system and the millimeter wave radar coordinate system, respectively. λ and μ are scale coefficients, with λ ranging from 0.1 to 0.5 and μ ranging from 0.3 to 0.5.

[0044] The moving targets detected in the laser point cloud are directly removed from the echo point cloud to complete the detection and suppression of the laser radar moving targets and obtain a static point cloud that does not contain moving targets.

[0045] In order to verify the effect of removing moving targets by fusion of LiDAR and millimeter-wave radar, data collection and moving target removal experiments on actual equipment were carried out. Figure 3This is the laser point cloud obtained in the experiment. The scene contains roads, pedestrians, walls, buildings, trees, etc., among which pedestrians are moving targets. Figure 4 The moving targets detected by the millimeter-wave radar include two targets. After projecting the moving targets detected by the millimeter-wave radar into the laser radar coordinate system, the results after detecting and removing the moving targets in the laser point cloud are as follows: Figure 5 The experimental results are consistent with expectations, and the point cloud formed by the two pedestrians in the scene is successfully detected and removed, indicating the effectiveness of the moving target removal method proposed in this invention.

[0046] In summary, the present invention uses the Doppler frequency shift of the millimeter-wave radar to detect moving targets in the scene in real time, and then assists the lidar in eliminating moving targets in the point cloud. The experimental results also verify the good effect of the method proposed in the present invention.

[0047] The contents not described in detail in the specification of the present invention belong to the prior art known to those skilled in the art.

Claims

1. A millimeter wave radar assisted laser radar moving target real-time removal method, characterized in that The following steps are involved: Step 1: Cluster the millimeter-wave point cloud to obtain the number of points, center position, and average speed of each point cloud cluster in the millimeter-wave radar coordinate system; Step 2: Remove ground points from the laser point cloud and cluster the ground point cloud to obtain the number of points and center position of the point cloud cluster in the laser radar coordinate system; Step 3: Estimate the platform motion based on the laser point cloud registration method to obtain the platform velocity v x ; Step 4: Millimeter wave point cloud moving target detection; Step 5: Millimeter wave point cloud moving target mapping; Step 6: Detection and removal of moving targets in laser point cloud; The moving targets in the laser point cloud are detected by comparing the moving targets in the millimeter wave point cloud with the point cloud clusters in the laser radar coordinate system. The moving targets detected by the millimeter wave radar are mapped to the position in the laser radar coordinate system as The number of point clouds included is m i , and the point cloud cluster R in the laser point cloud k ={n k ,(x k ,y k ,z k )} for comparison, where n k is the number of points contained in the point cloud cluster, is the center of the laser point cloud cluster. The condition for the point cloud cluster to be a moving target is: In the formula, |·| represents the distance between two points, d is the distance threshold, which is no more than half of the target geometric size, and Δx L and Δx M are the sizes of the point cloud cluster along the X direction in the lidar coordinate system and the millimeter wave radar coordinate system, respectively. λ and μ are scale coefficients, with λ ranging from 0.1 to 0.5 and μ ranging from 0.3 to 0.

5.

2. The millimeter-wave radar-assisted laser radar moving target real-time removal method according to claim 1, characterized in that: In step 4, the millimeter wave radar and lidar information are comprehensively utilized to realize the moving target detection in the millimeter wave radar coordinate system. The moving target judgment formula is as follows (2), where: v x is the platform motion speed estimated by the lidar, v i is the average velocity of the i-th point cloud cluster estimated by the millimeter-wave radar, θ is the angle of the i-th point cloud cluster relative to the X-axis, and v th is the detection threshold;

Images