Speed estimation method for extracting stationary point cloud by random sampling of FMCW lidar

Abstract

The application discloses a velocity estimation method for random sampling and extraction of static point clouds of FMCW laser radar. The method steps comprise: acquiring a point cloud set of a current frame of an object through FMCW laser radar; calculating the normalized velocity of a scanning point through the position and radial velocity of the scanning point and all surrounding points; extracting static points in the point cloud set through random multiple sampling of a RANSAC algorithm, and forming a static point cloud set from all the static points; estimating the velocity of the FMCW laser radar itself; clustering moving objects and estimating the velocity of the moving objects. The application greatly simplifies the detection complexity of moving objects and improves the detection accuracy of moving objects. According to single-frame data, the application estimates the velocity of the radar itself and the velocity of moving objects through a rough estimation and accurate estimation method, and meets the needs of different scenes.

Description

TECHNICAL FIELD

[0001] The application relates to a radar-based speed estimation method, in particular to a speed estimation method for extracting static point clouds by random sampling of an FMCW laser radar. BACKGROUND

[0002] The FMCW laser radar is a radar with constant transmitting power and periodically changing optical carrier frequency (or phase), which measures the modulation frequency difference introduced by distance delay and the Doppler frequency difference introduced by relative speed between the echo signal and the transmitting signal by means of coherent detection, so as to demodulate the distance and speed of the target. The laser radar based on the frequency modulation continuous wave (FMCW) technology can effectively overcome the poor anti-interference problem of the ToF laser radar, and the continuous wave working mode has smaller average transmitting power and overall power consumption than the pulse working mode, and the coherent detection mode brings higher sensitivity. Therefore, the selection of the laser radar for future unmanned driving technology will gradually transfer from the ToF laser radar to the FMCW laser radar.

[0003] In an unknown environment, the perception of moving objects is particularly important. In the path planning algorithm, the vehicle needs to know which objects around are moving, and track the speed of the moving objects, so as to determine the drivable area around the vehicle; in the SLAM mapping, the vehicle judges which objects are moving through the point cloud map of the current frame, so as to eliminate the related point cloud information, and thus maps out a map without noise points. The information features of the traditional radar scanning do not contain speed information, so the traditional radar needs two frames or even multiple frames of data to estimate the speed information. Moreover, the moving object detection algorithm based on the traditional laser radar usually processes the scanning point coordinate information to detect the moving objects, and the coordinate information is used for moving object detection, which often has the characteristics of large calculation amount and inaccurate estimation of the speed of the target moving object. Compared with the conventional laser radar, the FMCW laser radar has scanning point cloud data containing not only the scanning point coordinate information, but also the radial velocity information of the coordinate point relative to the laser radar. SUMMARY

[0004] In order to solve the problems in the background art, the application provides a speed estimation method for extracting static point clouds by random sampling of an FMCW laser radar, which can be used in the fields of intelligent security, smart city, unmanned driving and assisted driving.

[0005] The technical scheme adopted by the application is:

[0006] The method comprises the following steps:

[0007] Step one: obtaining a point cloud set of a current frame of an object by FMCW laser radar, the point cloud set including all points in the point cloud and the position and radial velocity of each point; the object includes stationary objects and moving objects.

[0008] Step two: traversing each point in the point cloud set, taking the current point as a scanning point, selecting several points as surrounding points from the points around the scanning point, the difference between the radial velocity of the surrounding points and the radial velocity of the scanning point being less than a preset velocity threshold, the points around the scanning point being all points in a circle with the scanning point as the center and a preset radius threshold as the radius except the scanning point; calculating the normalized velocity of the scanning point by the position and radial velocity of the scanning point and all surrounding points;

[0009] Step three: extracting stationary points in the point cloud set by RANSAC algorithm through random sampling multiple times according to the normalized velocity of each point in the point cloud set, the stationary point cloud set being composed of all stationary points;

[0010] Step four: estimating the FMCW laser radar self-velocity according to the stationary point cloud set obtained in step three;

[0011] According to the scene requirements, different estimation methods are used, specifically: using the normalized velocity of each point in the stationary point cloud set obtained in step three to roughly estimate the FMCW laser radar self-velocity, using the position and radial velocity of each point in the stationary point cloud set obtained in step three to accurately estimate the FMCW laser radar self-velocity;

[0012] Step five: clustering moving objects according to the normalized velocity of each point in the point cloud set obtained in step two, and estimating the velocity of the moving objects by the FMCW laser radar self-velocity estimated in step four.

[0013] According to the scene requirements, different estimation methods are used, specifically: using the normalized velocity of each point in the moving objects and the roughly estimated FMCW laser radar self-velocity to roughly estimate the velocity of the moving objects, using the position and radial velocity of each point in the moving objects and the accurately estimated FMCW laser radar self-velocity to accurately estimate the velocity of the moving objects.

[0014] In step two, specifically:

[0015] Traversing each point in the point cloud set, taking the current point as a scanning point, for each scanning point, setting a velocity threshold T th1 The following judgments are made on the points around the scanning point:

[0016] When the radial velocity difference Δv between the points around the scanning point and the scanning point satisfies Δv≥T th1If the radial velocity difference Δv between the points around the scanning point and the scanning point satisfies Δv

[0017] If the radial velocity difference Δv between the points around the scanning point and the scanning point satisfies Δv th1

[0018] If the total number of points in the same class point set is less than 3, the normalized velocity of the scanning point is set to null, i.e. null;

[0019] If the total number of points in the same class point set is not less than 3, the normalized velocity of the scanning point is calculated through the positions and radial velocities of the scanning point and all the surrounding points, specifically as follows:

[0020] The target function of the normalized velocity of the scanning point is established

[0021]

[0022]

[0023] wherein, represents the normalized velocity vector of the scanning point obtained by solving the target function; represents the two-norm; V represents the set of radial velocities of the scanning point and all the surrounding points; A represents the set of unit direction vectors of the scanning point and all the surrounding points, which is calculated through the positions of the scanning point and all the surrounding points;

[0024] The target function of the normalized velocity of the scanning point is calculated by the least square method and the normalized velocity of the scanning point is further calculated.

[0025] In the step three, the RANSAC algorithm is as follows:

[0026] 1) Randomly select n points in the point cloud set and all the n points are regarded as stationary points to form a point cloud subset, wherein n is a preset value;

[0027] 2) According to the normalized velocity of the point cloud subset, the reference velocity of the stationary points of the point cloud subset is set by using the probability statistical method;

[0028] 3) Calculate the point cloud set ​the stationary point velocity difference Δv of each point in the point cloud set except the point cloud subset s , for the point cloud set , for each point in the point cloud set except the point cloud subset, by setting a stationary point threshold T th2 the following judgment is made:

[0029] when the stationary point velocity difference Δv s satisfies Δv s <T th2 , the point is added to the point cloud subset;

[0030] when the stationary point velocity difference Δv s satisfies Δv s ≥T th2 , the point is not added to the point cloud subset;

[0031] 4) count the number of points in the current point cloud subset obtained in step 3):

[0032] if the number of points is greater than a preset number threshold N, the current point cloud subset is retained and the next step is continued;

[0033] if the number of points is less than or equal to the number threshold N, the current point cloud subset is discarded and the operation is returned to step 1);

[0034] 5) steps 1) to 4) are repeatedly executed, in the iteration process, a plurality of different point cloud subsets are randomly formed, for the current point cloud subset, the error metric of the current point cloud subset is calculated, and the following judgment is made:

[0035] if the current point cloud subset is the point cloud subset of the first iteration, the error metric of the current point cloud subset is taken as the optimal error metric, and the current point cloud subset is taken as the stationary point cloud set is retained;

[0036] if the current point cloud subset is not the point cloud subset of the first iteration, the following judgment is made:

[0037] if the error metric of the current point cloud subset is not less than the optimal error metric obtained in the last iteration, the current point cloud subset is discarded, and the stationary point cloud set and the optimal error metric obtained in the last iteration are retained;

[0038] if the error metric of the current point cloud subset is less than the optimal error metric obtained in the last iteration, the current point cloud subset is retained, the stationary point cloud set and the optimal error metric obtained in the last iteration are discarded, and the error metric of the current point cloud subset is taken as the optimal error metric, and the current point cloud subset is taken as the stationary point cloud set is retained;

[0039] Final static point cloud set Each point in the final static point cloud set is a static point.

[0040] In step 2) of step three, the probability and statistics method includes but is not limited to: a) Gaussian distribution probability fitting is performed on the normalized velocity of the point cloud subset, and the expectation of the fitted Gaussian distribution is taken as the reference velocity of the static point; b) the normalized velocity of the point cloud subset is sorted, and the median of the sorted normalized velocity is taken as the reference velocity of the static point; c) the normalized velocity of the point cloud subset is arithmetically averaged, and the arithmetically averaged value is taken as the reference velocity of the static point.

[0041] In step 5) of step three, the square value of the difference between the normalized velocity of each point in the current point cloud subset and the reference velocity of the static point is calculated, and the average of the square values obtained by all points is taken as the error metric.

[0042] Before the iteration starts, the initial value of the preset optimal error metric is infinity.

[0043] In step four:

[0044] a) the normalized velocity of the static point cloud set is used to roughly estimate the FMCW lidar self speed, and the formula is as follows:

[0045]

[0046] wherein, represents the roughly estimated FMCW lidar self speed, represents the normalized velocity vector of the static point in the static point cloud set

[0047] b) the position and radial velocity of the static point cloud set are used to accurately estimate the FMCW lidar self speed, and the specific formula is as follows:

[0048] The objective function of the accurately estimated FMCW lidar speed is established

[0049]

[0050]

[0051] wherein, represents the accurately estimated FMCW lidar self speed; represents the two-norm; V' represents the set of radial velocities of the static point cloud set ; A' represents the set of unit direction vectors of the static point cloud set ; and ​The position calculation is obtained.

[0052] The target function for accurately estimating the FMCW lidar speed is calculated by the least square method The accurately estimated FMCW lidar speed itself speed is further calculated.

[0053] In the step five:

[0054] Extract the point cloud set All points in the point cloud set except the stationary point cloud set form a moving point cloud set th1 According to the normalized speed of each point in the moving point cloud set , for each two points in the moving point cloud set , the following judgment is made:

[0055] When the normalized speed difference Δv m between the two points satisfies Δv m <T th1 , it is considered that the two points are the same moving object;

[0056] When the normalized speed difference Δv m between the two points satisfies Δv m ≥T th1 , it is considered that the two points are not the same moving object;

[0057] Points belonging to the same moving object form a moving point cloud subset, that is, the moving point cloud set is divided and clustered into different moving object moving point cloud subsets, denoted as:

[0058]

[0059] Among them, denotes the first, second, …, and qth moving point cloud subset;

[0060] For each moving point cloud subset, the following processing is performed:

[0061] a) The speed of the moving object is roughly estimated by the normalized speed of the moving point cloud subset and the roughly estimated FMCW lidar speed itself speed The formula is specifically:

[0062]

[0063] Among them, V mq denotes the roughly estimated speed of the qth moving object, and n denotes the total number of points in the qth moving point cloud subset ​represents the radial velocity of the qth moving point cloud subset ;

[0064] b) through the position and radial velocity of the moving point cloud subset and the accurately estimated FMCW laser radar self speed accurately estimate the speed of the qth moving object The formula is specifically:

[0065] establish a target function of accurately estimating the speed of the moving object

[0066]

[0067]

[0068] wherein, represents the accurately estimated speed of the qth moving object; represents the two-norm;

[0069] V mq represents the radial velocity set of the qth moving point cloud subset A mq represents the unit direction vector set of the qth moving point cloud subset , which is calculated through the position of the qth moving point cloud subset ;

[0070] The target function of accurately estimating the speed of the moving object is calculated by the least square method and the accurately estimated speed of the moving object is further calculated.

[0071] The beneficial effects of the present application are:

[0072] 1) The present application proposes the concept of scanning point normalized speed. The normalized speed can quickly segment the stationary point cloud, can quickly detect the moving object in the scene and cluster, greatly simplifies the moving object detection complexity, and improves the moving object detection accuracy.

[0073] 2) The present application proposes a method of extracting stationary point clouds by RANSAC algorithm randomly multiple times, which can accurately extract stationary point clouds in point cloud data set containing a large amount of moving point clouds, and has strong robustness.

[0074] 3) This invention proposes a method for obtaining information about the radar's own velocity and the velocity of a moving object. This velocity information can be used to optimize the accuracy of LiDAR tracking of moving objects. Compared to traditional methods that rely on two or more frames of data to estimate velocity, this invention can estimate the radar's own velocity and the velocity of a moving object based on a single frame of data. Normalized velocity can be used to roughly estimate the radar's own velocity and the velocity of a moving object when accuracy is low, significantly reducing the computational effort. Alternatively, radial velocity information can be used to precisely estimate the radar's own velocity and the velocity of a moving object when accuracy is high, meeting the needs of different scenarios.

[0075] 4) The method of the present invention can be used in fields such as smart security, smart cities, unmanned driving and assisted driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 It is the overall flow chart of the method of the present invention;

[0077] Figure 2 Stationary point cloud Schematic diagram;

[0078] Figure 3 Schematic diagram of the accurately estimated FMCW lidar velocity vector;

[0079] Figure 4 Schematic diagram of the velocity vector of each moving object for accurate estimation Figure 1 ;

[0080] Figure 5 Schematic diagram of the velocity vector of each moving object for accurate estimation Figure 2 . DETAILED DESCRIPTION

[0081] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0082] It should be clear that the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0083] The terms used in the embodiments of the present application are for the purpose of describing specific embodiments only and are not intended to limit the present application. The singular forms "a", "an", "the" and "the" used in the embodiments of the present application and the appended claims are also intended to include plural forms unless the context clearly indicates otherwise.

[0084] like Figure 1 As shown, the method of the present invention comprises the following steps:

[0085] Step one: obtaining a point cloud set of the current frame of the object by FMCW laser radar, the point cloud set including all points in the point cloud and the position and radial velocity of each point; the object includes stationary objects and moving objects.

[0086] Step two: traversing each point in the point cloud set, taking the current point as a scanning point, selecting several points as surrounding points in the points around the scanning point, the difference between the radial velocity of the surrounding points and the radial velocity of the scanning point being less than a preset velocity threshold, the points around the scanning point being all points in a circle with the scanning point as the center and a preset radius threshold as the radius except the scanning point; calculating the normalized velocity of the scanning point by the position and radial velocity of the scanning point and all surrounding points.

[0087] Traversing each point in the point cloud set, taking the current point as a scanning point, for each scanning point, setting a velocity threshold T th1 The points around the scanning point are judged as follows:

[0088] When the radial velocity difference Δv between the points around the scanning point and the scanning point satisfies Δv≥T th1 , it is considered that the points around the scanning point and the scanning point do not belong to the same class, i.e. they belong to different stationary objects or the same moving object, and the normalized velocity of the scanning point is set to null, i.e. null.

[0089] When the radial velocity difference Δv between the points around the scanning point and the scanning point satisfies Δv<T th1 , all the points around the scanning point are considered as surrounding points, and it is considered that the scanning point and all the surrounding points belong to the same class, i.e. they belong to the same stationary object or the same moving object, all the surrounding points belonging to the same class as the scanning point are combined to form a same-class point set, and the following judgment is made:

[0090] When the total number of points in the same-class point set is less than 3, the normalized velocity of the scanning point is set to null, i.e. null.

[0091] When the total number of points in the same-class point set is not less than 3, the normalized velocity of the scanning point is calculated by the position and radial velocity of the scanning point and all the surrounding points corresponding to the scanning point, specifically:

[0092] Establishing a target function of the normalized velocity of the scanning point

[0093]

[0094]

[0095] wherein, represents the normalized velocity vector of the scanning point obtained by solving the objective function; Represents the two-norm; V represents the set of radial velocities of the scanning point and all surrounding points; A represents the set of unit direction vectors of the scanning point and all surrounding points, which is obtained by calculating the positions of the scanning point and all surrounding points.

[0096] The objective function of the normalized velocity of the scanning point is calculated by the least squares method Then the normalized velocity of the scanning point is calculated.

[0097] Step 3: According to the normalized velocity of each point in the point cloud set, the stationary points in the point cloud set are randomly sampled multiple times through the RANSAC algorithm, and a stationary point cloud set is formed by all the stationary points.

[0098] The RANSAC algorithm is as follows:

[0099] 1) Randomly select a point cloud n points in the image, and all n points are regarded as static points to form a point cloud subset, where n is a preset value.

[0100] 2) using a probabilistic statistical method to set a stationary point reference velocity for the point cloud subset based on the normalized velocities of the point cloud subset; the probabilistic statistical method includes but is not limited to: a) performing a Gaussian distribution probabilistic fit on the normalized velocities of the point cloud subset, and using the expectation of the fitted Gaussian distribution as the stationary point reference velocity; b) sorting the normalized velocities of the point cloud subset, and using the median of the sorted normalized velocities as the stationary point reference velocity; c) performing an arithmetic average on the normalized velocities of the point cloud subset, and using the arithmetic average as the stationary point reference velocity;

[0101] Probabilistic statistical methods include but are not limited to: a) performing probabilistic fitting of a Gaussian distribution on the normalized velocities of a point cloud subset, and using the expectation of the fitted Gaussian distribution as the stationary point reference velocity; b) sorting the normalized velocities of the point cloud subset, and using the median of the sorted normalized velocities as the stationary point reference velocity; c) performing arithmetic averaging on the normalized velocities of the point cloud subset, and using the arithmetic average as the stationary point reference velocity.

[0102] 3) Calculate point cloud set The static point velocity difference Δv between the normalized velocity of each point except the point cloud subset and the static point reference velocity obtained in step 2) s , for point cloud collection For every point in the point cloud except the subset, a stationary point threshold T is set th2 Make the following judgments:

[0103] When the speed difference Δv at the stationary point s Satisfy Δv s <T th2When , the point is added to the point cloud subset;

[0104] When the speed difference Δv at the stationary point s Satisfy Δv s ≥T th2 , then the point will not be added to the point cloud subset;

[0105] 4) Count the number of points in the current point cloud subset obtained in step 3):

[0106] If the number of points is greater than the preset threshold N, the current point cloud subset is retained and the next step is continued;

[0107] If the number of points is less than or equal to the threshold value N, the current point cloud subset is discarded and the process returns to step 1) to continue.

[0108] 5) Before the iteration begins, the initial value of the preset optimal error metric is infinite; steps 1) to 4) are repeated. During the iteration process, several different point cloud subsets are randomly formed. For the current point cloud subset, the error metric of the current point cloud subset is calculated: the square of the difference between the normalized velocity of each point in the current point cloud subset and the reference velocity of the stationary point is calculated, and the average of the square values ​​obtained for all points is taken as the error metric; and the following judgment is made:

[0109] If the current point cloud subset is the first iteration point cloud subset, the error metric of the current point cloud subset is used as the optimal error metric, and the current point cloud subset is used as the stationary point cloud set. Make reservations;

[0110] If the current point cloud subset is not the point cloud subset of the first iteration, the following judgment is made:

[0111] If the error metric of the current point cloud subset is not less than the optimal error metric obtained in the previous iteration, the current point cloud subset is discarded and the static point cloud set obtained in the previous iteration is retained. and optimal error metrics;

[0112] If the error metric of the current point cloud subset is less than the optimal error metric obtained in the previous iteration, the current point cloud subset is retained and the static point cloud set obtained in the previous iteration is discarded. and the optimal error metric, and the error metric of the current point cloud subset is used as the optimal error metric, and the current point cloud subset is used as the stationary point cloud set Make reservations;

[0113] Final stationary point cloud Every point in is a stationary point, such as Figure 2 As shown, the white part in the center is the final stationary point cloud The remaining white parts are moving objects.

[0114] Step four: Estimate the FMCW lidar's own speed according to the static point cloud set obtained in step three. Different estimation methods are used according to the scene requirements, specifically: using the normalized speed of each point in the static point cloud set obtained in step three to roughly estimate the FMCW lidar's own speed, using the position and radial speed of each point in the static point cloud set obtained in step three to accurately estimate the FMCW lidar's own speed.

[0115] Step five: Cluster the moving objects according to the normalized speed of each point in the point cloud set obtained in step two, and estimate the speed of the moving objects through the FMCW lidar's own speed estimated in step four. Different estimation methods are used according to the scene requirements, specifically: using the normalized speed of each point in the moving objects and the roughly estimated FMCW lidar's own speed to roughly estimate the speed of the moving objects, using the position and radial speed of each point in the moving objects and the accurately estimated FMCW lidar's own speed to accurately estimate the speed of the moving objects.

[0116] Step four and step five are as follows:

[0117] Extract the point cloud set All points in the point cloud set except the static point cloud set form a moving point cloud set Through the set speed threshold T th1 , according to the normalized speed of each point in the moving point cloud set , for each two points in the moving point cloud set , the following judgments are made:

[0118] When the normalized speed difference Δv m between the two points satisfies Δv m <T th1 , it is considered that the two points are the same moving object;

[0119] When the normalized speed difference Δv m between the two points satisfies Δv m ≥T th1 , it is considered that the two points are not the same moving object;

[0120] Points belonging to the same moving object are grouped into a moving point cloud subset, that is, the moving point cloud set is divided and clustered into different moving object moving point cloud subsets, represented as:

[0121]

[0122] Among them, represents the first, second, …, qth moving point cloud subset.

[0123] For each of the moving point cloud subsets, the following processing is performed:

[0124] a) Using the normalized velocities of the stationary point cloud set to roughly estimate the FMCW lidar ego velocity, which is given by:

[0125]

[0126] where denotes the roughly estimated FMCW lidar ego velocity, denotes the normalized velocity vector of the stationary points in the stationary point cloud set

[0127] By the normalized velocities of the moving point cloud subsets and the roughly estimated FMCW lidar ego velocity to roughly estimate the velocities of the moving objects, which is given by:

[0128]

[0129] where V mq denotes the roughly estimated velocity of the qthmoving object, n denotes the total number of points in the qthmoving point cloud subset denotes the normalized velocity vector of the qthmoving point cloud subset

[0130] b) As shown in FIG. 6, the FMCW lidar ego is taken as the coordinate reference system, and the right upper corner of the display is shown in Figure 3 Figure 3

[0131] Using the positions and radial velocities of the stationary point cloud set to accurately estimate the FMCW lidar ego velocity, which is given by:

[0132] To establish the objective function of the accurately estimated FMCW lidar velocity

[0133]

[0134]

[0135] where denotes the accurately estimated FMCW lidar ego velocity; denotes the two-norm; V' denotes the set of radial velocities of the stationary point cloud set ; A' denotes the set of unit direction vectors of the stationary point cloud set , which is calculated by the positions of the stationary point cloud set ​​​​​​

[0136] The target function of the precise estimation of the FMCW laser radar speed is calculated by the least square method The precise estimation of the FMCW laser radar speed itself speed is calculated.

[0137] As shown in Figure 4 and Figure 5 , the speed vector of each moving object is precisely estimated, and the FMCW laser radar itself is taken as the coordinate reference system, which is displayed above each moving object.

[0138] The position and radial speed of the moving point cloud subset and the precise estimation of the FMCW laser radar itself speed The speed of the qth moving object is precisely estimated The formula is specifically:

[0139] The target function of the precise estimation of the moving object speed is established

[0140]

[0141]

[0142] Wherein, The speed of the qth moving object is precisely estimated; The two-norm is represented;

[0143] V mq The radial speed set of the qth moving point cloud subset A mq The unit direction vector set of the qth moving point cloud subset is calculated by the position of the qth moving point cloud subset

[0144] The target function of the precise estimation of the moving object speed is calculated by the least square method The precise estimation of the moving object speed is calculated.

[0145] In the method of the application, the average operation speed of steps one to three is 0.068s, and the average operation speed of steps four to five is 0.0084s, which significantly improves the average operation speed of the algorithm.​

Claims

1. A velocity estimation method for extracting stationary point clouds by random sampling using an FMCW lidar, characterized by: The method comprises the following steps: Step 1: Obtain the point cloud set of the current frame of the object through FMCW laser radar. The point cloud set includes all points in the point cloud as well as the position and radial velocity of each point; Step 2: Traverse each point in the point cloud, use the current point as the scanning point, select several points around the scanning point as surrounding points, and calculate the normalized speed of the scanning point based on the positions and radial speeds of the scanning point and all surrounding points. Step 3: According to the normalized velocity of each point in the point cloud, the stationary points in the point cloud are randomly sampled multiple times by the RANSAC algorithm, and a stationary point cloud set is formed by all the stationary points; Step 4: Estimate the FMCW lidar's own velocity based on the stationary point cloud obtained in step 3; Step 5: Cluster the moving objects based on the normalized velocity of each point in the point cloud obtained in step 2, and estimate the velocity of the moving objects using the FMCW lidar's own velocity estimated in step 4. The step 2 is specifically as follows: Traverse each point in the point cloud, take the current point as the scanning point, and set the speed threshold T for each scanning point. th1 The following judgments are made on the points around the scan point: When the radial velocity difference Δv between the points around the scanning point and the scanning point satisfies Δv ≥ T th1 When , it is considered that the points around the scanning point and the scanning point do not belong to the same category, and the normalized velocity of the scanning point is set to a null value; When the radial velocity difference Δv between the points around the scanning point and the scanning point satisfies Δv <T th1 , all the points around the scan point are considered as surrounding points, and the scan point and all surrounding points are considered to belong to the same class. All surrounding points belonging to the same class as the scan point are combined into a set of similar points, and the following judgment is performed: When the total number of points in the same type of point set is less than 3, the normalized speed of the scanning point is set to a null value; When the total number of points in the same type of point set is not less than 3, the normalized velocity of the scanning point is calculated by the position and radial velocity of the scanning point and all the surrounding points corresponding to it, specifically: Establish the objective function of the normalized velocity of the scanning point in, represents the normalized velocity vector of the scanning point obtained by solving the objective function; represents the two-norm; V represents the set of radial velocities of the scanning point and all surrounding points; A represents the set of unit direction vectors of the scanning point and all surrounding points; The objective function of the normalized velocity of the scanning point is calculated by the least squares method Then the normalized velocity of the scanning point is calculated.

2. The method for estimating velocity by randomly sampling and extracting stationary point clouds using an FMCW lidar according to claim 1, characterized in that: In step 3, the RANSAC algorithm is as follows: 1) Randomly select a point cloud n points in the point cloud, and all n points are regarded as static points to form a point cloud subset, where n is a preset value; 2) According to the normalized velocity of the point cloud subset, a probability statistical method is used to set the reference velocity of the stationary point of the point cloud subset; 3) Calculate point cloud set The static point velocity difference Δv between the normalized velocity of each point except the point cloud subset and the static point reference velocity obtained in step 2) s , for point cloud collection For every point in the point cloud except the subset, a stationary point threshold T is set th2 Make the following judgments: When the speed difference Δv at the stationary point s Satisfy Δv s <T th2 When , the point is added to the point cloud subset; When the speed difference Δv at the stationary point s Satisfy Δv s ≥T th2 , then the point will not be added to the point cloud subset; 4) Count the number of points in the current point cloud subset obtained in step 3): If the number of points is greater than the preset threshold N, the current point cloud subset is retained and the next step is continued; If the number of points is less than or equal to the threshold value N, the current point cloud subset is discarded and the process returns to step 1) to continue. 5) Repeat steps 1) to 4). During the iteration process, randomly form several different point cloud subsets. For the current point cloud subset, calculate the error metric of the current point cloud subset and make the following judgments: If the current point cloud subset is the first iteration point cloud subset, the error metric of the current point cloud subset is used as the optimal error metric, and the current point cloud subset is used as the stationary point cloud set. Make reservations; If the current point cloud subset is not the point cloud subset of the first iteration, the following judgment is made: If the error metric of the current point cloud subset is not less than the optimal error metric obtained in the previous iteration, the current point cloud subset is discarded and the static point cloud set obtained in the previous iteration is retained. and optimal error metrics; If the error metric of the current point cloud subset is less than the optimal error metric obtained in the previous iteration, the current point cloud subset is retained and the static point cloud set obtained in the previous iteration is discarded. and the optimal error metric, and the error metric of the current point cloud subset is used as the optimal error metric, and the current point cloud subset is used as the stationary point cloud set Make reservations; Final stationary point cloud Every point in is a stationary point.

3. The method for estimating velocity by randomly sampling and extracting stationary point clouds using an FMCW lidar according to claim 2, characterized in that: In step 2) of step 3, the probability statistics method includes: a) performing Gaussian distribution probabilistic fitting on the normalized velocities of the point cloud subset, and using the expectation of the fitted Gaussian distribution as the stationary point reference velocity; b) sorting the normalized velocities of the point cloud subset, and using the median of the sorted normalized velocities as the stationary point reference velocity; c) performing arithmetic averaging on the normalized velocities of the point cloud subset, and using the arithmetic average as the stationary point reference velocity.

4. The method for estimating velocity by randomly sampling and extracting a stationary point cloud using an FMCW lidar according to claim 2, characterized in that: In step 5) of step 3, the square value of the difference between the normalized velocity of each point in the current point cloud subset and the reference velocity of the stationary point is calculated, and the average of the square values ​​obtained for all points is taken as the error metric.

5. The method for velocity estimation using FMCW lidar random sampling to extract stationary point clouds according to claim 1, characterized in that: In the step 4: a) Using a stationary point cloud The normalized speed of the FMCW lidar is roughly estimated to be its own speed. The formula is as follows: in, represents a rough estimate of the FMCW lidar's own speed, Represents a stationary point cloud Normalized velocity vector of the stationary point in ; b) Using a stationary point cloud The radial velocity of the FMCW lidar is used to accurately estimate its own velocity, specifically: Establishing an objective function for accurately estimating FMCW lidar velocity in, represents the accurately estimated FMCW lidar velocity; represents the two-norm; V′ represents the stationary point cloud The radial velocity of the set; A′ represents the stationary point cloud The set of unit direction vectors; The objective function for accurately estimating the FMCW lidar velocity is calculated using the least squares method. Then the accurate estimated FMCW lidar speed itself is calculated.

6. The method for velocity estimation using FMCW lidar random sampling to extract stationary point clouds according to claim 1, characterized in that: In the step five: Extract point cloud Stationary point cloud All points outside the moving point cloud By setting the speed threshold T th1 , according to the motion point cloud Normalized velocity of each point in the moving point cloud For every two points in , make the following judgment: When the normalized velocity difference Δv between two points m Satisfy Δv m <T th1 When , the two points are considered to be the same moving object; When the normalized velocity difference Δv between two points m Satisfy Δv m ≥T th1 When , it is considered that the two points are not the same moving object; The points belonging to the same moving object are grouped into a moving point cloud subset. Distinguish and cluster the motion point cloud subsets of different moving objects, expressed as: in, Represented as the first, second, ...qth motion point cloud subsets; The following processing is performed on each motion point cloud subset: a) Normalized velocity of the moving point cloud subset and roughly estimated FMCW lidar velocity Roughly estimate the speed of a moving object. The formula is: Among them, V mq Represents the roughly estimated speed of the qth moving object, and n represents the qth moving point cloud subset The total number of points in Represents the qth motion point cloud subset The normalized velocity vector of ; b) Radial velocity of the moving point cloud subset and the accurately estimated FMCW lidar velocity Accurately estimate the velocity of the qth moving object The specific formula is: Establish an objective function for accurately estimating the velocity of a moving object in, represents the precise estimated velocity of the qth moving object; represents the two-norm; V mq Represents the qth motion point cloud subset The collection of radial velocities; A mq Represents the qth motion point cloud subset The set of unit direction vectors; Calculate the objective function of accurately estimating the velocity of a moving object by the least squares method Then the speed of the moving object can be accurately estimated.

Images