A high-precision map generation method, device, equipment and storage medium

Abstract

The present invention provides a high-precision map generation method, device, equipment and storage medium. According to the IMU data between the nearest key frame and the current LiDAR point cloud frame and the pose optimized by the backend of the previous LiDAR point cloud frame, a first pose estimation value is estimated; according to the first pose estimation value and the IMU data corresponding to the time when the current LiDAR point cloud is scanned, the current LiDAR point cloud is de-distorted and feature points are extracted; according to the de-distorted current LiDAR point cloud frame, a current descriptor is obtained, and whether a loop is detected is judged according to the current descriptor. If a loop is detected, a second pose estimation value is estimated; a third pose estimation value is obtained according to the first pose estimation value and the feature points; the second pose estimation value and the third pose estimation value are input into a factor graph to obtain the pose estimation value optimized by the backend of the current frame; according to the pose estimation value optimized by the backend, the de-distorted current LiDAR point cloud frame is spliced into the historical point cloud map to generate a static point cloud map, improving the map accuracy.

Description

Technical Field

[0001] The present invention relates to the technical field of computer vision image processing, and in particular to a high-precision map generation method, device, equipment and storage medium. Background Art

[0002] With the development of artificial intelligence technology, map building and positioning by fusing lidar, cameras, and GPS multi-sensors is an important basis for the automatic navigation of intelligent unmanned vehicles. Existing general navigation maps Figure 1 are generally road-level maps, with less map information and a precision of meter level. High-precision maps can be accurate to the 10-20 cm level and contain map information such as traffic signs, lane centerlines, and zebra crossings, providing a more comprehensive data basis for autonomous driving.

[0003] Existing high-precision map construction methods based on lidar will exhibit map drift phenomena when encountering long-distance similar scenarios such as tunnels, with poor robustness and being vulnerable to single-sensor errors. In general high-precision map construction methods, devices equipped with single / dual binocular / depth cameras or 2D / 3D lidar are used for data acquisition, and then adjacent data frames of the acquired data are matched. Usually, the ICP algorithm (Iterative Closest Point) or the NDT (Normal Distributions Transform) matching algorithm is used for calculation to obtain the trajectory passed by the device (the device is a device integrated with the above sensors, cameras or lidar, and is also equivalent to the vehicle body of autonomous driving), including the three-dimensional position and attitude of the device, referred to as pose). Generally, the trajectory calculated by adjacent frame matching is called the front-end odometer. By stitching the point cloud data obtained by the front-end odometer with the camera or lidar, a local point cloud map can be obtained. Since the front-end odometer is calculated from adjacent frame data, errors will inevitably exist, and the errors will also accumulate continuously during the forward movement of the device. To prevent the map from shifting due to excessive accumulated errors, it is necessary to jointly optimize all the point cloud data in the local map, and IMU (Inertial Measurement Unit) data (including acceleration and angular velocity), RTK (Real-time kinematic) data (that is, real-time kinematic measurement technology, a real-time differential GPS technology based on carrier phase observation, and a sensor that can output centimeter-level accurate longitude, latitude and altitude data), loop detection data, etc. may be added for auxiliary optimization. This joint optimization part is usually called the back-end optimization thread. Finally, the optimized trajectory is used to stitch the corresponding point cloud data to obtain a complete map, and this process is called the map processing thread. In addition, the loop detection mentioned above is a common means to suppress accumulated errors. Specifically, when the device passes through a location it has reached during data acquisition, the similarity between the current frame of point cloud data and the historical map data is judged through loop detection at this time. If the similarity is greater than the threshold, it is judged that the device has reached the historical trajectory point. At this time, the adjacent data frame matching strategy will be enabled, and the position and attitude transformation from the current frame to the historical map will be calculated through the ICP or other matching algorithms to eliminate the accumulated errors. Therefore, a high-precision map construction method usually includes threads such as front-end odometer, back-end optimization, loop detection, and map processing.

[0004] In the above front - end odometry process, the pose estimation value is obtained through the adjacent frame data matching method. If the ICP algorithm is adopted, all point - to - point matches are performed between adjacent frames, which is likely to cause false matches, reduce the accuracy of pose estimation, and then affect the map accuracy, and the computational complexity is very high; if the NDT algorithm is adopted to save the voxel map, loop detection cannot be performed, which greatly affects the long - distance mapping accuracy. At the same time, most algorithms directly adopt the ICP algorithm as the strategy for matching similar frames in the loop detection thread, resulting in low loop detection accuracy and poor robustness. For example, loops cannot be formed in scenarios where the scanning angles of lidars are quite different. Summary of the Invention

[0005] The present invention discloses a high - precision map generation method, device, equipment, and storage medium, which solves the problem that the existing map generation methods have low accuracy and poor robustness, resulting in inaccurate pose estimation and further affecting the map generation accuracy.

[0006] In the first aspect of the embodiments of the present invention, a high - precision map generation method is provided, including:

[0007] Obtain the current laser point cloud frame data and IMU data;

[0008] If the current laser point cloud frame data is the first frame data collected by the lidar, then use it as the first key frame, generate a point cloud map through the first key frame, and re - execute the above steps; otherwise, estimate the first pose estimation value of the current laser point cloud frame according to the IMU data between the nearest key frame and the current laser point cloud frame and the pose of the previous laser point cloud frame optimized by the backend.

[0009] According to the first pose estimation value and the IMU data corresponding to the time when the current laser point cloud is scanned, distort the current laser point cloud to obtain the distorted - removed current laser point cloud frame, and extract feature points from it;

[0010] Obtain the current descriptor according to the distorted - removed current laser point cloud frame, and judge whether a loop is detected according to the current descriptor. If a loop is detected, then estimate the second pose estimation value of the current laser point cloud frame; otherwise, if no loop is detected, set the second pose estimation value to be empty.

[0011] Obtain the third pose estimation value according to the first pose estimation value and the feature points; input the second pose estimation value and the third pose estimation value into the factor graph to obtain the pose estimation value of the current frame optimized by the backend.

[0012] Determine whether the current lidar point cloud frame can be used as a key frame. If it can, splice the undistorted current lidar point cloud frame into the historical point cloud map according to the pose estimation value optimized by the backend of the current frame, remove dynamic objects, and generate a static point cloud map; if not, re-execute the above steps until the map generation stops.

[0013] In the second aspect of the embodiments of the present invention, a high-precision map generation device is provided, including:

[0014] An acquisition module, configured to acquire current lidar point cloud frame data and IMU data;

[0015] An IMU preprocessing module, configured to, if the current lidar point cloud frame data is the first frame data collected by the lidar, use it as the first key frame, send it to the map generation module, and return to the acquisition module; otherwise, estimate the first pose estimation value of the current lidar point cloud frame according to the IMU data between the nearest key frame and the current lidar point cloud frame and the pose optimized by the backend of the previous lidar point cloud frame fed back by the backend optimization module;

[0016] A front-end odometry module, configured to undistort the current lidar point cloud according to the first pose estimation value and the IMU data corresponding to the scanning time of the current lidar point cloud, obtain the undistorted current lidar point cloud frame, and extract feature points therefrom;

[0017] A loop detection module, configured to obtain the current descriptor according to the undistorted current lidar point cloud frame, determine whether a loop is detected according to the current descriptor, and if a loop is detected, estimate the second pose estimation value of the current lidar point cloud frame; otherwise, if no loop is detected, set the second pose estimation value to be empty;

[0018] A backend optimization module, configured to obtain the third pose estimation value according to the first pose estimation value and the feature points; input the second pose estimation value and the third pose estimation value into a factor graph to obtain the pose estimation value optimized by the backend of the current frame;

[0019] A map generation module, configured to, if the current frame is the first frame, generate a point cloud map through the first key frame, otherwise, determine whether the current frame can be used as a key frame. If it can, splice the undistorted current lidar point cloud frame into the historical point cloud map according to the pose estimation value optimized by the backend of the current frame, and generate a static point cloud map after removing dynamic objects; if not, return to the acquisition module until the map generation stops.

[0020] In the third aspect of the embodiments of the present invention, a map generation device is provided, including: a memory, a processor, and a computer program. The computer program is stored in the memory, and the processor runs the computer program to execute any one of the high-precision map generation methods in the embodiments of the present invention.

[0021] In the fourth aspect of the embodiments of the present invention, a readable storage medium is provided. A computer program is stored in the readable storage medium, and when the computer program is executed by a processor, it is used to implement any one of the high-precision map generation methods in the embodiments of the present invention.

[0022] Beneficial effects: According to the IMU data between the key frame closest to the current lidar point cloud frame in terms of time and the current lidar point cloud frame and the pose optimized by the backend of the previous lidar point cloud frame, the first pose estimation value of the current lidar point cloud frame is estimated; according to the first pose estimation value and the IMU data corresponding to the time when the lidar scans the current lidar point cloud, the current lidar point cloud is de-distorted to obtain the de-distorted current lidar point cloud frame, and feature points are extracted therefrom; since the pose optimized by the backend is used to estimate the first pose, the estimated first pose value is more accurate, and the de-distorted current lidar point cloud frame is also more accurate;

[0023] The current descriptor is obtained according to the de-distorted current lidar point cloud frame; it is judged whether a loop is detected according to the current descriptor. If a loop is detected, the second pose estimation value of the current lidar point cloud frame is estimated, otherwise, if no loop is detected, the second pose estimation value is set to null; the present invention constructs a descriptor to detect loops, rather than detecting loops according to all points in the frame, reducing the false detection rate, improving the loop detection accuracy, having good robustness, and thus improving the accuracy of the subsequent generated point cloud map.

[0024] The third pose estimation value is obtained according to the first pose estimation value and the feature points; the second pose estimation value and the third pose estimation value are input into the factor graph to obtain the pose optimized by the backend of the current lidar point cloud frame; it is judged whether the current lidar point cloud frame can be used as a key frame. If it can be used as a key frame, the de-distorted current lidar point cloud is spliced into the historical point cloud map according to its pose optimized by the backend, and a static point cloud map is generated after removing dynamic objects.

[0025] The present invention not only receives IMU data, but also receives the pose optimized by the backend processing, and jointly estimates the first pose of the current lidar point cloud, and the obtained first pose estimation value is more accurate; then the current lidar point cloud with motion distortion is corrected, and feature points are extracted, and thus the corrected pose and the extracted feature points are more accurate; through the descriptor, the false matching is reduced, the accuracy of the pose estimation is improved, and thus the accuracy of the generated map is improved, and the calculation amount is very small.

[0026] The high-precision map generation device, equipment, and storage medium provided by the present invention also have the above-mentioned beneficial effects. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 is a schematic diagram of a high-precision map generation method provided by an embodiment of the present invention;

[0028] Figure 2 is a schematic diagram of a high-precision map generation device provided by an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0029] The following further describes and explains a high-precision map generation method, device, equipment, and storage medium of the present invention with reference to the accompanying drawings and embodiments.

[0030] The key frame is a laser point cloud frame used to generate a point cloud map after screening; the point cloud map is generated by a plurality of key frames.

[0031] As Figure 1 shown, a high-precision map generation method provided in this embodiment includes the following steps:

[0032] Step 1, obtain the laser point cloud data collected by the lidar as the current laser point cloud frame (hereinafter referred to as the current frame) and the IMU data collected by the IMU sensor;

[0033] Furthermore, the current latitude, longitude, and altitude data collected by the RTK sensor can also be obtained together;

[0034] Specifically, the IMU data collected by the IMU sensor includes the angular velocity and acceleration of the device; the device here is a device integrated with sensors and radars, and can also be understood as the vehicle body of autonomous driving.

[0035] The laser points collected by the lidar in each scan are called a frame of laser point cloud data. Since the collection frequency of the laser point cloud data is lower than that of the IMU data, multiple groups of IMU data are included between two frames of laser point cloud data.

[0036] Save the data collected each time after the lidar and the IMU sensor start collecting. Among them, all IMU data is saved to the data processing queue Opt_Queue.

[0037] Step 2, if the current laser point cloud frame data is the first frame of data collected by the lidar, use it as the first key frame, generate a point cloud map through the first key frame, and re-execute the above steps; otherwise, estimate the first pose estimate value of the current laser point cloud frame based on the IMU data between the nearest key frame and the current laser point cloud frame and the pose of the previous laser point cloud frame optimized by the backend.

[0038] That is: if the laser point cloud data is not the first frame of data collected by the lidar, then estimate based on the IMU data between the (K - 1)-th key frame and the current laser point cloud frame, and the pose of the previous laser point cloud frame after backend optimization, to obtain the first pose estimate value of the current laser point cloud frame; otherwise, save the laser point cloud data as the first key frame, set its corresponding optimized pose as the identity matrix, generate a point cloud map through this first key frame (generated by the subsequent map generation module), and return to step one until the map generation stops.

[0039] This step is executed in the IMU preprocessing module.

[0040] The key frame is a laser point cloud frame used to generate a high-precision map, which is a point cloud frame with more feature points (i.e., the corner feature points and surface feature points mentioned below) selected from all the collected laser point cloud frames according to a set distance. Generating a map through key frames is to reduce the storage space and computational amount of the map. Number the key frames, and the number range is 1 to K - 1. Subsequently, in the present invention, it is necessary to determine whether the current laser point cloud frame can be used as the K-th key frame. If it can, it is used to splice into the historical point cloud map to generate a new point cloud map;

[0041] The pose is the pose of the vehicle body for autonomous driving, and can also be simply referred to as the device pose.

[0042] Setting the optimized pose as the identity matrix means that the device pose has not changed.

[0043] Specifically, the estimating based on the IMU data between the (K - 1)-th key frame and the current laser point cloud frame, and the pose after backend optimization, to obtain the first pose estimate value of the current laser point cloud frame, includes:

[0044] Based on the IMU data between the (K - 1)-th key frame and the time stamp of the current laser point cloud frame, and the optimized pose transmitted from the backend optimization module, perform a joint non-linear optimization, with the goal of obtaining the first pose estimate value of the current laser point cloud frame with the highest probability. Here, the optimization algorithm uses the Gauss-Newton method;

[0045] Step three, according to the first pose estimate value and the IMU data corresponding to the time of scanning the current laser point cloud, perform distortion removal on the laser point cloud of the current frame to obtain the distorted-removed laser point cloud data of the current frame, and extract feature points from it.

[0046] This step is executed in the front-end odometry module.

[0047] Performing distortion removal on the laser point cloud of the current frame to obtain the distorted-removed laser point cloud data of the current frame, and extracting feature points from it, includes:

[0048] (1) Calculate the time when each point in the current lidar point cloud frame is scanned by the lidar;

[0049] Specifically, there is also corresponding IMU data within the time range of one scan of the lidar; the timestamp of each frame of lidar point cloud data is the start time of the scan. At the end of each scan, the lidar will send an end time. Therefore, the time when each point is scanned can be calculated.

[0050] (2) According to the IMU data corresponding to the time of scanning the current lidar point cloud and the scanning time of each point, calculate the relative rotation amount of each point in the lidar; according to the pose change from the K-1 key frame to the current frame, calculate the relative displacement amount of each point; according to the relative rotation amount and relative displacement amount, correct the distortion of the current frame of lidar point cloud to obtain the distortion-corrected current frame of lidar point cloud data;

[0051] That is, add the relative rotation amount and relative displacement amount to the original point cloud coordinates of the lidar point respectively, and correct the points with observation distortion caused by the movement of the device to the correct pose;

[0052] Among them, the relative rotation amount = the angular velocity in the IMU data corresponding to the time of scanning the current lidar point cloud * (the time when each point is scanned - the start time of the scan);

[0053] The relative displacement amount = the velocity * (the time when each point is scanned - the start time of the scan), and the velocity = (the three-dimensional position in the first pose estimate - the three-dimensional position in the pose optimized by the backend corresponding to the K-1 key frame) / the time interval between the current frame and the K-1 key frame.

[0054] (3) For the distortion-corrected current frame of lidar point cloud data, calculate the curvature of each point. The points with curvature exceeding the angular threshold are extracted as corner feature points, and the points with curvature less than the surface threshold are extracted as surface feature points. Finally, a set of corner feature points and a set of surface feature points are obtained, collectively referred to as the set of feature points;

[0055] Among them, the smaller the curvature, the greater the possibility that the current point belongs to a plane (wall, ground). The greater the curvature, the greater the possibility that the current point belongs to the edge of an object.

[0056] Calculate the curvature of each point. An example is to calculate the sum of the depth differences between the current point and several adjacent points (for example, 10 points). The angular threshold is greater than the surface threshold, and both are obtained based on empirical values;

[0057] Further, when extracting feature points, evenly sample points in the regions divided by the angle of the point cloud to prevent the feature points from being too concentrated.

[0058] For example, divide the point cloud data scanned in a full circle, i.e., 360 degrees, into 6 regions, and try to evenly distribute the corner feature points and surface feature points in each region. If there are too many corner feature points in a certain region, remove some corner feature points with relatively small curvature; if there are too many surface feature points in a certain region, remove some surface feature points with relatively large curvature. This can prevent feature points from being overly concentrated in certain regions.

[0059] In addition, separate the ground points from the current frame of laser point cloud data after distortion removal, and mark all the ground points in the current frame of laser point cloud data after distortion removal.

[0060] Step 4: Obtain the current descriptor based on the current frame of laser point cloud after distortion removal, and determine whether a loop is detected according to the current descriptor. If a loop is detected, estimate the second pose estimate value of the current frame of laser point cloud; otherwise, if no loop is detected, set the second pose estimate value to null.

[0061] Specifically:

[0062] Obtain the current descriptor based on the current frame of laser point cloud after distortion removal, and determine whether a loop is detected according to the current descriptor. If a loop is detected, select points with a distance less than the set distance threshold from the historical point cloud map to the current frame of laser point after distortion removal as matching point pairs, and estimate the second pose estimate value; otherwise, set the second pose estimate value to null and execute the next step.

[0063] Through the above loop detection, the initial pose estimate is optimized to obtain the second pose estimate value, making the subsequent generated map more accurate.

[0064] This step is executed in the loop detection module. The historical point cloud map is the latest point cloud map that has been saved currently, and it is a historical map relative to the map to be generated by this method.

[0065] Specifically, obtaining the descriptor based on the current frame of laser point cloud after distortion removal includes:

[0066] Divide the data of the current frame of laser point cloud after distortion removal into several fan-shaped regions according to the radar scanning angle and depth, record the maximum value of the emission intensity of all points in each region, save the maximum values of the emission intensities in all regions into an array, and the length of the array is equal to the number of divided regions; for example, if it is divided into 120 fan-shaped regions, an array with a length of 120 is obtained, and the values in the array are the reflection intensities. This array is simply referred to as the descriptor.

[0067] Each key frame of the generated historical map also corresponds to a descriptor.

[0068] That is, the descriptor is: divided into several fan-shaped regions according to the radar scanning angle and depth, recording the maximum emission intensity of all laser points in each region, and forming an array of the maximum emission intensities in all regions.

[0069] Specifically, to estimate the second pose estimate value, the steps include:

[0070] (1) Project the de-distorted current frame of the laser point cloud into the coordinate system of the historical point cloud map through the first pose estimate value, and determine whether the overlap degree between the projected current frame of the laser point cloud and the historical point cloud map is greater than the overlap degree threshold. If it is greater, it indicates that a loop is preliminarily detected, and the next step (2) is executed. Otherwise, it is determined that no loop is detected, indicating that the scene in the historical point cloud map has not been reached.

[0071] For example, if 80% of the points in the current frame of the laser point cloud are within the range of the historical point cloud map, it is considered that a loop is detected, indicating that it is possible to have reached the scene in the historical point cloud map. At this time, the overlap degree threshold is set to 80%. In this step, the overlap degree judgment is performed first, which can improve the detection accuracy and efficiency of the loop.

[0072] (2) Compare the current descriptor with the descriptors corresponding to each key frame in the historical point cloud map respectively to obtain the similarity corresponding to each key frame. If a certain similarity is greater than the loop threshold, it is determined as a loop. Select the points whose distance from the de-distorted current frame of the laser points is less than the set distance threshold in the corresponding key frame to form a matching point pair, and calculate the second pose estimate value of the current laser point cloud frame according to the inter-frame matching algorithm such as the ICP algorithm (Iterative Closest Point); otherwise, it is determined that no loop is detected.

[0073] It should be noted that the steps to estimate the second pose estimate value may only include the process of the above step (2). In the loop detection part, the prior art directly performs the matching of loop detection through the ICP algorithm, and the false detection rate is relatively high. The present invention constructs a descriptor through the reflection intensity data of the point cloud, compares the current descriptor with the descriptors of each key frame in the historical point cloud map to determine whether there is a loop, which can improve the loop detection accuracy, reduce the false detection rate, further optimize the pose by detecting the loop, and thus improve the accuracy of the subsequent generated point cloud map.

[0074] Before performing the above step (2), the overlap degree judgment in step (1) can also be performed first, which can further improve the detection accuracy and efficiency of the loop.

[0075] Step five, obtain the third pose estimate value according to the first pose estimate value and the feature points; input the second pose estimate value and the third pose estimate value into the factor graph to obtain the pose estimate value optimized at the back end of the current frame.

[0076] This step is executed in the backend optimization module.

[0077] The obtaining of the third pose estimate specifically includes:

[0078] (1) Stitch several key frames closest to the current frame to form a local map. Specifically: When stitching, the pose values corresponding to several key frames closest to the current frame are the accurate pose values optimized by the backend optimization module.

[0079] Specifically: If no loop is found in step four, several key frames within a set distance or set time closest to the current frame are extracted to stitch the local map. If a loop is found, several key frames within a set distance or set time closest to the current frame and the historical key frames corresponding to the loop in the loop container are extracted to stitch the local map, which can improve the map generation accuracy; the loop container stores the historical key frames corresponding to the detected loop.

[0080] (2) Perform voxel filtering on the local map and downsample it, that is, reduce the number of points in the local map to reduce the calculation time. Then, transform the feature points in the current frame to the local map coordinate system through the first pose estimate value, search for the nearest points of the feature points in the local map, and calculate the third pose estimate value of the current laser point cloud frame through factor graph calculation.

[0081] Specifically: For corner feature points, search for several nearest points (such as 2 or 3 points) in the local map to form a line segment, calculate the minimum distance from the point to the line as the point-line residual required for optimization; for surface feature points, search for several nearest points (such as 5 points), construct a plane with these points, and calculate the minimum distance from the point to the plane as the point-plane residual required for optimization. After the calculation, all residuals (the residuals corresponding to all corner feature points and surface feature points) are input into the nonlinear optimizer, i.e., the factor graph (existing) constructed by the GTSAM library, and the third pose estimate value of the current laser point cloud frame is output.

[0082] Furthermore, perform speed judgment on the third pose estimate value. If the ratio of the movement speed from the (K - 1)th key frame to the current frame to the movement speed from the (K - 2)th key frame to the (K - 1)th key frame is greater than a preset speed ratio (such as a 2-fold relationship), it is determined that the current frame has a degradation phenomenon, that is, the trajectory drift phenomenon that occurs in long-distance similar scenarios such as tunnels. Then discard the calculated third pose estimate value and recalculate the third pose estimate value according to uniform motion, that is: by default, use the uniform motion model, and use the pose transformation from the (K - 2)th key frame to the (K - 1)th key frame as the pose transformation from the (K - 1)th key frame to the current frame; where the movement speed from the (K - 1)th key frame to the current frame is: the pose transformation from the (K - 1)th key frame to the current frame / the time difference from the current frame to the (K - 1)th key frame;

[0083] Recalculate the third pose estimate value according to a uniform speed, specifically:

[0084] The recalculated third pose estimate value = the pose of the (K - 1)th key frame + (the speed from the (K - 2)th key frame to the (K - 1)th key frame) * (the time difference from the current frame to the (K - 1)th key frame).

[0085] The factor graph also stores all previous historical pose data. Finally, the GTSAM library is used to optimize the above second pose estimate value and third pose estimate value jointly in the factor graph. In this way, the pose optimized at the backend obtained through the factor graph is called: the pose estimate value optimized at the backend of the current laser point cloud frame, which is saved as the trajectory of the map as the pose data of the final current frame; the number of input parameters of the factor graph of the present invention is small, so the computational amount is small.

[0086] Furthermore, in order to make the pose optimized at the backend more accurate, the pose transformation matrix from the (K - 1)th key frame to the current frame (i.e., the pose transformation obtained through GPS information) can be jointly calculated according to the current latitude, longitude and altitude data collected by the RTK sensor obtained in Step 1 and the latitude, longitude and altitude data of the (K - 1)th key frame, and then the pose transformation matrix, together with the above second pose estimate value and third pose estimate value, is input into the factor graph to obtain a more accurate pose optimized at the backend.

[0087] Step 6: Determine whether the current laser point cloud frame can be used as a key frame. If it can, splice the undistorted current laser point cloud frame into the historical point cloud map according to the pose estimate value optimized at the backend of the current frame, remove dynamic objects, and generate a static point cloud map; if not, re - execute the above steps until the map generation stops.

[0088] Among them, the method for determining whether the current frame is a key frame: If the distance between the current frame and the (K - 1)th key frame is greater than the key frame threshold (such as one meter), and the number of feature points in the current frame is greater than the set feature point threshold, then it is determined as a key frame. Calculate the distance according to the difference between the three - dimensional positions in the pose optimized at the backend of the current frame and the three - dimensional positions optimized at the (K - 1)th key frame;

[0089] By removing dynamic objects, such as vehicles and pedestrians, a static high - precision point cloud map is obtained. The existing method is used to remove dynamic objects.

[0090] The present invention estimates based on the IMU data between the key frame closest to the current lidar point cloud frame in terms of time and the current lidar point cloud frame and the pose optimized by the backend of the previous lidar point cloud frame to obtain the first pose estimate value of the current lidar point cloud frame; according to the first pose estimate value and the IMU data corresponding to the time when the lidar scans the current lidar point cloud, the current lidar point cloud is de-distorted to obtain the de-distorted current lidar point cloud frame, and feature points are extracted therefrom; since the optimized pose by the backend is used to estimate the first pose, the estimated first pose value is more accurate, and the de-distorted current lidar point cloud frame is also more accurate;

[0091] The current descriptor is obtained according to the de-distorted current lidar point cloud frame; it is judged whether a loop is detected according to the current descriptor. If a loop is detected, the second pose estimate value of the current lidar point cloud frame is estimated. Otherwise, if no loop is detected, the second pose estimate value is set to be empty; the present invention constructs a descriptor to detect loops, rather than detecting loops based on all points in the frame, reducing the false detection rate, improving the loop detection accuracy, having good robustness, and further improving the accuracy of the point cloud map.

[0092] Then, the third pose estimate value is obtained according to the first pose estimate value and the feature points; the second pose estimate value and the third pose estimate value are input into the factor graph to obtain the pose optimized by the backend of the current lidar point cloud frame; it is judged whether the current lidar point cloud frame can be used as a key frame. If it can be used as a key frame, the de-distorted current lidar point cloud is spliced into the historical point cloud map according to its pose optimized by the backend, and a static point cloud map is generated after removing dynamic objects.

[0093] The present invention not only receives IMU data, but also receives the pose optimized by the backend processing, and together estimates the first pose of the current lidar point cloud, and the obtained first pose estimate value is more accurate; then, the current lidar point cloud with motion distortion is corrected, and feature points are extracted. Furthermore, the corrected pose and the extracted feature points are more accurate; through the descriptor, the false matching is reduced, the accuracy of the pose estimation is improved, and thus the accuracy of the generated map is improved, and the calculation amount is very small.

[0094] As Figure 2 shown, the embodiment of the present invention further provides a high-precision map generation device, including:

[0095] An acquisition module, configured to acquire current lidar point cloud frame data and IMU data;

[0096] The IMU preprocessing module is used to: if the current laser point cloud frame data is the first frame data collected by the lidar, then use it as the first key frame, send it to the map generation module, and return it to the acquisition module; otherwise, based on the IMU data between the nearest key frame and the current laser point cloud frame and the pose of the previous laser point cloud frame optimized by the backend optimization module, estimate the first pose estimate value of the current laser point cloud frame.

[0097] The front-end odometry module is used to undistort the current laser point cloud according to the first pose estimate value and the corresponding IMU data during the scanning time of the current laser point cloud, obtain the current laser point cloud frame after undistortion, and extract feature points from it.

[0098] The loop detection module is used to obtain the current descriptor according to the current laser point cloud frame after undistortion, judge whether a loop is detected according to the current descriptor. If a loop is detected, then estimate the second pose estimate value of the current laser point cloud frame; otherwise, if no loop is detected, set the second pose estimate value to be empty.

[0099] The backend optimization module is used to obtain the third pose estimate value according to the first pose estimate value and the feature points; input the second pose estimate value and the third pose estimate value into the factor graph to obtain the pose estimate value of the current frame optimized by the backend.

[0100] The map generation module is used to: if the current frame is the first frame, generate a point cloud map through the first key frame; otherwise, judge whether the current frame can be used as a key frame. If it can, then splice the current laser point cloud frame after undistortion into the historical point cloud map according to the pose estimate value of the current frame optimized by the backend, and generate a static point cloud map after removing dynamic objects; if not, then return it to the acquisition module until the map generation stops.

[0101] The specific method steps adopted in each module are the same as those of the above map generation method, and will not be elaborated here.

[0102] A high-precision map generation device provided by an embodiment of the present invention. The IMU preprocessing module estimates based on the IMU data between the key frame closest to the current lidar point cloud frame in terms of time and the pose optimized by the backend for the previous lidar point cloud frame to the current lidar point cloud frame, and obtains the first pose estimation value of the current lidar point cloud frame. The front-end odometry module distorts the current lidar point cloud according to the first pose estimation value and the IMU data corresponding to the current lidar scanning time, obtains the current lidar point cloud frame after distortion removal, and extracts feature points therefrom. Since the pose optimized by the backend is used to estimate the first pose, the estimated first pose value is more accurate, and the current lidar point cloud frame after distortion removal is also more accurate. The loop detection module obtains the current descriptor according to the current lidar point cloud frame after distortion removal; determines whether a loop is detected according to the current descriptor. If a loop is detected, the second pose estimation value of the current lidar point cloud frame is estimated. Otherwise, if no loop is detected, the second pose estimation value is set to null. The present invention constructs a descriptor to detect loops, rather than detecting loops based on all points in the frame, reducing the false detection rate, improving the loop detection accuracy, having good robustness, and thus improving the accuracy of the point cloud map. The backend optimization module obtains the third pose estimation value according to the first pose estimation value and the feature points; inputs the second pose estimation value and the third pose estimation value into the factor graph to obtain the pose of the current lidar point cloud frame optimized by the backend. The map generation module determines whether the current lidar point cloud frame can be used as a key frame. If it can be used as a key frame, the current lidar point cloud after distortion removal is spliced into the historical point cloud map according to its pose optimized by the backend, and a static point cloud map is generated after removing dynamic objects.

[0103] The present invention not only receives IMU data, but also receives the pose optimized by the backend processing, and jointly estimates the first pose of the current lidar point cloud, and the obtained first pose estimation value is more accurate; then corrects the current lidar point cloud with motion distortion, and extracts feature points, and thus the corrected pose and the extracted feature points are more accurate; through the descriptor, the false matching is reduced, the accuracy of the pose estimation is improved, and thus the accuracy of the generated map is improved, and the calculation amount is very small.

[0104] An embodiment of the present invention provides a map generation device, including: a memory, a processor, and a computer program. The computer program is stored in the memory, and the processor runs the computer program to execute any one of the high-precision map generation methods in the above embodiments.

[0105] An embodiment of the present invention provides a readable storage medium, in which a computer program is stored, and when the computer program is executed by a processor, it is used to implement any one of the high-precision map generation methods in the above embodiments.

[0106] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. A high-precision map generation method, characterized in that, it includes: Obtain the current lidar point cloud frame data and IMU data; If the current lidar point cloud frame data is the first frame data collected by the lidar, use it as the first key frame, generate a point cloud map through the first key frame, and re-execute the above steps; otherwise, estimate the first pose estimate value of the current lidar point cloud frame according to the IMU data between the nearest key frame and the current lidar point cloud frame and the pose of the previous lidar point cloud frame optimized by the backend; According to the first pose estimate value and the IMU data corresponding to the time when the current lidar point cloud is scanned, distort the current lidar point cloud to obtain the distorted current lidar point cloud frame, and extract feature points from it; Obtain the current descriptor according to the distorted current lidar point cloud frame, and judge whether a loop is detected according to the current descriptor. If a loop is detected, estimate the second pose estimate value of the current lidar point cloud frame; otherwise, if no loop is detected, set the second pose estimate value to null; Obtain the third pose estimate value according to the first pose estimate value and the feature points; input the second pose estimate value and the third pose estimate value into the factor graph to obtain the pose estimate value optimized by the backend of the current frame; Judge whether the current lidar point cloud frame can be used as a key frame. If so, splice the distorted current lidar point cloud frame into the historical point cloud map according to the pose estimate value optimized by the backend of the current frame, remove dynamic objects, and generate a static point cloud map; if not, re-execute the above steps until the map generation stops.

2. A high-precision map generation method according to claim 1, characterized in that, The obtaining the current descriptor according to the distorted current lidar point cloud frame includes: Divide the points in the distorted current lidar point cloud frame into several regions according to the radar scanning angle and depth; Save the maximum value of the emission intensity of the lidar points in each region into an array, and use this array as the current descriptor.

3. A high-precision map generation method according to claim 1, characterized in that, The estimating the second pose estimate value of the current lidar point cloud frame includes: Compare the current descriptor with the descriptors corresponding to each key frame in the historical point cloud map respectively to obtain the similarity corresponding to each key frame; If a certain similarity is greater than the loop threshold, it is judged that a loop is detected. Select the points in the corresponding key frame whose distance from the points in the distorted current lidar cloud frame is less than the set distance threshold to form a matching point pair, and estimate the second pose estimate value of the current lidar point cloud frame according to the matching algorithm.

4. A high-precision map generation method according to claim 1, characterized in that, Before detecting the loop, it also includes a preliminary detection of the loop, including: Project the distorted current lidar point cloud frame into the coordinate system of the historical point cloud map through the first pose estimate value, and judge whether the coincidence degree between the projected current lidar point cloud and the historical point cloud map is greater than the coincidence degree threshold. If it is greater, it means that a loop is preliminarily detected, otherwise, it is judged that no loop is detected.

5. A high-precision map generation method according to claim 1, characterized in that, the distortion removal of the current lidar point cloud to obtain the current lidar point cloud frame after distortion removal includes: calculating the time when each point in the current lidar point cloud frame is scanned by the lidar; calculating the relative rotation amount of each point in the lidar according to the corresponding IMU data during the time of scanning the current lidar point cloud and the scanned time of each point; calculating the relative displacement amount of each point according to the pose change from the nearest key frame to the current lidar point cloud frame; performing distortion removal on the points in the current lidar point cloud frame according to the relative rotation amount and relative displacement amount to obtain the current lidar point cloud frame after distortion removal.

6. A high-precision map generation method according to claim 1, characterized in that, extracting feature points from the current lidar point cloud frame after distortion removal includes: for the current lidar point cloud frame after distortion removal, calculating the curvature of each point, taking the points with curvature exceeding the preset angular threshold as corner feature points, and taking the points with curvature less than the preset plane threshold as plane features.

7. A high-precision map generation method according to claim 1, characterized in that, the obtaining the third pose estimation value according to the first pose estimation value and the feature points includes: stitching several key frames closest to the current lidar point cloud frame into a local map; transforming the feature points into the local map coordinate system through the first pose estimation value, calculating the point-line residual of the corner feature points and the point-plane residual of the plane feature points, and inputting them into the factor graph to obtain the third pose estimation value of the current lidar point cloud frame.

8. A high-precision map generation method according to claim 7, characterized in that, for the corner feature points, searching for several points closest to the corner feature points in the local map, forming a line segment, and calculating the minimum distance from the corner feature points to the line segment as the point-line residual; for the plane feature points, searching for several points closest to the plane feature points in the local map, constructing a plane, and calculating the minimum distance from the plane feature points to the plane as the point-plane residual.

9. A high-precision map generation method according to claim 1, characterized in that, after obtaining the third pose estimation value, it further includes: performing speed judgment on the third pose estimation value. If the ratio of the movement speed from the nearest key frame to the current frame to the movement speed from the second nearest key frame to the nearest key frame is greater than the preset speed ratio, it is determined that the current frame has a degradation phenomenon, and the third pose estimation value is recalculated according to uniform motion.

10. A high-precision map generation method according to claim 1, characterized in that, the judgment of whether the current lidar point cloud frame can be used as a key frame includes: if the distance between the current lidar point cloud frame and the nearest key frame is greater than the key frame threshold, it is determined as a key frame; or, if the distance between the current lidar point cloud frame and the nearest key frame is greater than the key frame threshold, and the number of feature points in the current frame is greater than the set feature point threshold, it is determined as a key frame.

11. A high-precision map generation method according to claim 1, characterized in that, it further includes: Obtain the current latitude, longitude, and altitude data, jointly calculate the pose transformation matrix from the nearest key frame to the current frame with the latitude, longitude, and altitude data of the nearest key frame, and input the pose transformation matrix, the second pose estimate value, and the third pose estimate value into the factor graph to obtain the pose estimate value of the current frame optimized by the backend.

12. A high-precision map generation device Characterized in that It includes: An acquisition module for acquiring current lidar point cloud frame data and IMU data; An IMU preprocessing module, if the current lidar point cloud frame data is the first frame data collected by the lidar, then use it as the first key frame, send it to the map generation module, and return to the acquisition module; otherwise, according to the IMU data between the nearest key frame and the current lidar point cloud frame and the pose of the previous lidar point cloud frame optimized by the backend feedback from the backend optimization module, estimate the first pose estimate value of the current lidar point cloud frame; A front-end odometry module for de-distorting the current lidar point cloud according to the first pose estimate value and the IMU data corresponding to the scanning time of the current lidar point cloud to obtain the de-distorted current lidar point cloud frame, and extracting feature points therefrom; A loop detection module for obtaining the current descriptor according to the de-distorted current lidar point cloud frame, judging whether a loop is detected according to the current descriptor, if a loop is detected, then estimating the second pose estimate value of the current lidar point cloud frame; otherwise, if no loop is detected, set the second pose estimate value to null; A backend optimization module for obtaining the third pose estimate value according to the first pose estimate value and the feature points; inputting the second pose estimate value and the third pose estimate value into the factor graph to obtain the pose estimate value of the current frame optimized by the backend; A map generation module, if the current frame is the first frame, then generate a point cloud map through the first key frame, otherwise, judge whether the current frame can be used as a key frame, if it can, then splice the de-distorted current lidar point cloud frame into the historical point cloud map according to the pose estimate value of the current frame optimized by the backend, and generate a static point cloud map after removing dynamic objects; if not, then return to the acquisition module until the map generation stops.

13. A map generation device Characterized in that It includes: A memory, a processor, and a computer program, the computer program is stored in the memory, and the processor runs the computer program to execute the high-precision map generation method according to any one of claims 1 to 11.

14. A readable storage medium Characterized in that The readable storage medium stores a computer program, and when the computer program is executed by a processor, it is used to implement the high-precision map generation method according to any one of claims 1 to 11.

Images