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

Abstract

The invention provides a high-precision map generation method, a high-precision map generation device, a high-precision map generation equipment and a high-precision map generation storage medium, wherein a first pose estimation value is estimated according to IMU data between a recent key frame and a current laser point cloud frame and the pose of a last laser point cloud frame after back-end optimization; according to the first pose estimation value and IMU data corresponding to the time for scanning the current laser point cloud, carrying out distortion removal on the current laser point cloud and extracting a feature point; obtaining a current descriptor according to the current laser point cloud frame after distortion removal, judging whether loop is detected or not according to the current descriptor, and if loop is detected, estimating a second attitude estimation value; obtaining a third attitude estimation value according to the first attitude estimation value and the characteristic points; inputting the second posture estimation value and the third posture estimation value into a factor graph to obtain a posture estimation value optimized at the rear end of the current frame; and splicing the current laser point cloud frame subjected to distortion removal into a historical point cloud map according to the pose estimation value optimized by the rear end to generate a static point cloud map, so that the map precision is improved.

Description

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

technical field

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

Background technique

With the development of artificial intelligence technology, the integration of lidar, camera, and GPS multi-sensors for mapping and positioning is an important basis for the automatic navigation of intelligent unmanned vehicles. The existing general navigation maps are generally road-level maps, which contain less map information and have meter-level accuracy. The high-precision map can be accurate to the level of 10-20cm, and contains map information such as traffic signs, lane centerlines, and zebra crossings, which can provide a more comprehensive data basis for autonomous driving.

The existing high-precision map construction method based on lidar, when encountering long-distance similar scenes such as tunnels, will experience map drift, poor robustness, and is susceptible to single-sensor errors. The usual high-precision map construction method uses a device equipped with a single/binocular/depth camera or 2D/3D lidar for data collection, and then performs adjacent data frame matching on the collected data, usually using the ICP algorithm (Iterative Closest Point) or NDT (NormalDistributions Transform) matching algorithm to obtain the trajectory of the device (that is, the device integrated with the above-mentioned sensors, cameras or radars, which is also equivalent to the vehicle body of automatic driving) (the trajectory includes: the three-dimensional position and attitude of the device , referred to as pose), the trajectory calculated by matching adjacent frames is generally called the front-end odometer. The local point cloud map can be obtained by splicing the point cloud data obtained by the camera or lidar through the front-end odometer. Since the front-end odometer is calculated from adjacent frame data, there will inevitably be errors, and the errors will continue to accumulate during the advancement of the device. In order to prevent the map from drifting due to excessive cumulative errors, it is necessary to jointly optimize all point cloud data in the local map, which may also include IMU (Inertial Measurement Unit) data (including acceleration and angular velocity), RTK (Real - time kinematic) data (That is, real-time dynamic measurement technology, based on carrier phase observation, real-time differential GPS technology, sensors that can output 1-2cm precision longitude and latitude height data), loopback detection data, etc. for auxiliary optimization, the joint optimization part is usually called the back-end Optimize threads. Finally, the optimized trajectory is used to stitch the corresponding point cloud data to obtain a complete map. This process is called a map processing thread. In addition, the loop detection mentioned above is a common means of suppressing accumulated errors. Specifically, in the process of collecting data, the device will pass through the places it has arrived. At this time, the similarity between the current frame point cloud data and the historical map data is judged by loopback detection. If the similarity is greater than the threshold, it is judged that the device has reached the historical track 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 ICP or other matching algorithms to eliminate 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.

In the above-mentioned front-end odometer process, the estimated value of the pose is obtained through the matching method of adjacent frame data. If the ICP algorithm is adopted, all points between adjacent frames are matched, which is likely to cause false matching and reduce the position. The accuracy of attitude estimation will affect the accuracy of the map, and the calculation is very large; if the NDT algorithm is used to save the voxel map, the loop detection will not be able to be performed, which will greatly affect the accuracy of long-distance mapping. At the same time, most algorithms directly use the ICP algorithm as a matching similar frame strategy in the loopback detection thread. The accuracy of loopback detection is low and the robustness is poor. For example, loopbacks cannot be formed in scenarios with large differences in lidar scanning angles.

Contents of the invention

The invention discloses a high-precision map generation method, device, equipment and storage medium, which solves the problem that the existing map generation method has low precision and poor robustness, which leads to inaccurate pose estimation and further affects the map generation accuracy.

According to the first aspect of the embodiments of the present invention, a method for generating a high-precision map is provided, including:

Obtain current laser point cloud frame data and IMU data;

If the current laser point cloud frame data is the first frame data collected by the laser radar, it is used as the first key frame, and the point cloud map is generated through the first key frame, and the above steps are re-executed; otherwise, according to the latest key frame The IMU data between the frame and the current laser point cloud frame and the back-end optimized pose of the last laser point cloud frame are estimated to obtain the first pose estimation value of the current laser point cloud frame;

According to the first pose estimation value and the IMU data corresponding to the scanning time of the current laser point cloud, the current laser point cloud is de-distorted to obtain the current laser point cloud frame after de-distortion, and extract feature points therefrom;

Obtain the current descriptor according to the current laser point cloud frame after de-distortion, judge whether to detect the loop closure according to the current descriptor, if the loop closure is detected, estimate the second pose estimation value of the current laser point cloud frame; otherwise, If no loop closure is detected, set the second pose estimate to null;

Obtain a 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 back-end optimized value of the current frame Pose estimate;

Judging whether the current laser point cloud frame can be used as a key frame, if possible, splicing the de-distorted current laser point cloud frame into the historical point cloud map according to the pose estimation value optimized by the back end of the current frame, Remove dynamic objects and generate a static point cloud map; if not, perform the above steps again until the map generation is stopped.

The second aspect of the embodiments of the present invention provides a high-precision map generation device, including:

The acquisition module is used to acquire the current laser point cloud frame data and IMU data;

The IMU preprocessing module is used for if the current laser point cloud frame data is the first frame data collected by the laser radar, then send it as the first key frame to the map generation module and return to the acquisition module; otherwise, according to the latest key frame The IMU data between the frame and the current laser point cloud frame and the back-end optimized pose of the previous laser point cloud frame fed back by the back-end optimization module are estimated to obtain the first pose estimation value of the current laser point cloud frame;

The front-end odometer module is used to de-distort the current laser point cloud according to the first estimated pose value and the IMU data corresponding to the scanning time of the current laser point cloud, to obtain the de-distorted current laser point cloud frame, and Extract feature points from it;

The loopback detection module is used to obtain the current descriptor according to the current laser point cloud frame after dedistortion, and judge whether to detect a loopback according to the current descriptor. If a loopback is detected, it is estimated to obtain the second bit of the current laser point cloud frame pose estimate; otherwise, no loop closure is detected, and the second pose estimate is set to null;

The back-end optimization module is used 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 the factor graph to obtain Backend-optimized pose estimates for the current frame;

The map generation module is used to generate a point cloud map through the first key frame if the current frame is the first frame, otherwise, judge whether the current frame can be used as a key frame, and if so, optimize according to the backend of the current frame Splice the de-distorted current laser point cloud frame into the historical point cloud map, and generate a static point cloud map after removing dynamic objects; if not, return to the acquisition module until the map generation is stopped.

The third aspect of the embodiments 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 the implementation of the present invention. The high-precision map generation method described in any one of the examples.

According to the fourth aspect of the embodiments of the present invention, a readable storage medium is provided, and 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 embodiments of the present invention. The high-precision map generation method described above.

Beneficial effects: the present invention estimates the current laser point according to the IMU data between the key frame closest to the current laser point cloud frame in time and the pose optimized by the backend of the last laser point cloud frame The first pose estimation value of the cloud frame; according to the first pose estimation value and the IMU data corresponding to the radar scanning time of the current laser point cloud, the current laser point cloud is de-distorted to obtain the current laser point cloud frame after de-distortion , and extract feature points from it; since the first pose is estimated by the back-end optimized pose, the estimated first pose value is more accurate, and the current laser point cloud frame after dedistortion is more accurate. precise;

Obtain the current descriptor according to the current laser point cloud frame after dedistortion; judge whether a loop is detected according to the current descriptor, if a loop is detected, estimate the second pose estimation value of the current laser point cloud frame, otherwise, not detect to the loop, and set the second pose estimation value to be empty; the present invention constructs a descriptor to detect the loop, instead of detecting the loop according to all points in the frame, reduces the false detection rate, improves the accuracy of the loop detection, and has good robustness. In turn, the accuracy of subsequent generation of point cloud maps is improved.

Then obtain the third pose estimate according to the first pose estimate and the feature points; input the second pose estimate and the third pose estimate into the factor graph to obtain the current laser point cloud frame optimized by the backend Pose; determine whether the current laser point cloud frame can be used as a key frame, and if it can be used as a key frame, then according to the back-end optimized pose, the de-distorted current laser point cloud is spliced into the historical point cloud map, and the Generate static point cloud maps after dynamic objects.

The present invention not only receives the IMU data, but also receives the pose after back-end optimization processing, and performs the first pose estimation on the current laser point cloud together, and the obtained first pose estimation value is more accurate; The point cloud is rectified, and the feature points are extracted, so that the corrected pose and the extracted feature points are more accurate; through the descriptor, the mismatch is reduced, the accuracy of pose estimation is improved, and the accuracy of the generated map is improved, and the calculation Portions are small.

The high-precision map generation device, equipment and storage medium provided by the present invention also have the above-mentioned beneficial effects.

Description of drawings

FIG. 1 is a schematic diagram of a method for generating a high-precision map provided by an embodiment of the present invention;

Fig. 2 is a schematic diagram of a high-precision map generation device provided by an embodiment of the present invention.

Detailed ways

A method, device, device, and storage medium for generating a high-precision map of the present invention will be further described and explained below in conjunction with the drawings and embodiments.

The key frame is a filtered laser point cloud frame used to generate a point cloud map; the point cloud map is generated by several key frames.

As shown in Figure 1, a method for generating a high-precision map provided in this embodiment includes the following steps:

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;

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

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 a self-driving car body.

The laser point collected by the laser radar for each scanning circle is called a frame of laser point cloud data. Since the laser point cloud data collection frequency is lower than the IMU data collection frequency, multiple sets of IMU data will be included between two frames of laser point cloud data.

Save the data collected each time after the lidar and IMU sensors start collecting. Among them, the IMU data are all stored in the data processing queue Opt_Queue.

Step 2, if the current laser point cloud frame data is the first frame data collected by the laser radar, 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, According to the IMU data between the latest key frame and the current laser point cloud frame and the back-end optimized pose of the last laser point cloud frame, estimate the first pose estimation value of the current laser point cloud frame;

That is: if the laser point cloud data is not the first frame data collected by the laser radar, then according to the IMU data between the K-1 key frame and the current laser point cloud frame, the last laser point cloud frame is optimized by the backend The final pose is estimated to obtain the first pose estimation value of the current laser point cloud frame; otherwise, the laser point cloud data is saved as the first frame key frame, and the corresponding optimized pose is set as unit Matrix, generate a point cloud map through the first key frame (generated by the subsequent map generation module), and return to step 1 until the map generation is stopped.

This step is performed in the IMU preprocessing module.

The key frame is the laser point cloud frame used to generate a high-precision map, which is the feature point selected from all the collected laser point cloud frames according to the set distance (that is, the corner feature point and the surface feature point mentioned below). There are many point cloud frames, and the map is generated through key frames to reduce the storage space and calculation amount of the map. Number the key frames, and the number range is 1~K-1. The present invention needs to determine whether the current laser point cloud frame can be used as the Kth frame key frame. If so, it will be used to splicing into the historical point cloud map to generate a new point cloud map;

The pose refers to the pose of the vehicle body for autonomous driving, and may also be referred to as the device pose for short.

The optimized pose is set to the identity matrix indicating that the device pose has no transformation.

Specifically, the estimation is performed according to the IMU data between the K-1 key frame and the current laser point cloud frame, and the pose after back-end optimization, to obtain the first pose estimation value of the current laser point cloud frame, including:

According to the IMU data between the K-1 key frame and the timestamp of the current laser point cloud frame, and the optimized pose from the back-end optimization module, a joint nonlinear optimization is performed, with the goal of obtaining the maximum probability of the current laser The first pose estimation value of the point cloud frame, the optimization algorithm here adopts the Gauss-Newton method;

Step 3: Dedistort the laser point cloud of the current frame according to the estimated value of the first pose and the corresponding IMU data within the scanning time of the current laser point cloud, obtain the laser point cloud data of the current frame after dedistortion, and obtain Feature points are extracted.

This step is performed in the front-end odometer module.

Dedistort the laser point cloud of the current frame to obtain the laser point cloud data of the current frame after dedistortion, and extract feature points from it, including:

(1) Calculate the time when each point in the current laser point cloud frame is scanned by the laser radar;

Specifically, the lidar also has IMU data corresponding to the time range of scanning a circle; the time stamp of each frame of lidar point cloud data is the moment when the scan starts, and at the end of each scan, the lidar will send an end time , therefore, the time for each point to be scanned can be calculated.

(2) Calculate the relative rotation of each point in the laser radar according to the corresponding IMU data within the scanning time of the current laser point cloud and the scanning time of each point; according to the K-1 key frame to the current frame According to the relative rotation amount and the relative displacement amount, the laser point cloud of the current frame is de-distorted to obtain the laser point cloud data of the current frame after de-distortion;

That is, the relative rotation and relative displacement are added to the original point cloud coordinates of the laser point, and the point that is observed to be distorted due to equipment movement is corrected to the correct pose;

Among them, the relative rotation amount = the angular velocity in the IMU data corresponding to the scanning time of the current laser point cloud * (the scanning time of each point - the scanning start time);

Relative displacement = speed * (time to scan each point - start scanning time), speed = (3D position in the first pose estimation value - back-end optimized position corresponding to the K-1th key frame The three-dimensional position in the pose)/the two-frame time interval between the current frame and the K-1th key frame.

(3) For the current frame of laser point cloud data after de-distortion, calculate the curvature of each point, the point whose curvature exceeds the angle threshold is extracted as a corner feature point, and the point whose curvature is smaller than the surface threshold is extracted as a surface feature point, and finally the angle The feature point set and the surface feature point set are collectively referred to as the feature point set;

Among them, the smaller the curvature, the greater the possibility that the current point belongs to the plane (wall, ground), and the greater the curvature, the greater the possibility that the current point belongs to the edge of the object.

Calculate the curvature of each point, for example: calculate the sum of the depth differences between the current point and several adjacent points (for example, 10 points). The angle threshold is greater than the surface threshold, which are obtained based on empirical values;

Furthermore, when extracting feature points, the point cloud is evenly selected according to the area divided by angle, so as to prevent the feature points from being too concentrated.

For example, the 360-degree point cloud data scanned in one circle is divided into 6 regions, and the corner feature points and surface feature points in each region are distributed as evenly as possible. If there are too many corner feature points in a certain region, some of them will be removed. Corner feature points with smaller curvature; if there are too many surface feature points in a certain area, remove some of the surface feature points with larger curvature. This prevents feature points from being too concentrated in certain areas.

In addition, ground points are separated from the current frame of laser point cloud data after dedistortion, and all ground points are marked on the current frame of laser point cloud data after dedistortion.

Step 4: Obtain the current descriptor according to the current laser point cloud frame after dedistortion, judge whether a loop closure is detected according to the current descriptor, if a loop closure is detected, estimate and obtain the second pose estimation value of the current laser point cloud frame ; Otherwise, no loop closure is detected, and the second pose estimation value is set to null;

specific:

Obtain the current descriptor according to the laser point cloud frame of the current frame after dedistortion, judge whether to detect a loopback according to the current descriptor, if a loopback is detected, select the laser point of the current frame after dedistortion from the historical point cloud map Points whose distance is less than the set distance threshold are used as matching point pairs, and the second pose estimation value is estimated; otherwise, the second pose estimation value is set to empty, and the next step is performed;

Through the above-mentioned loop closure detection, the initial pose estimation is optimized, and the second pose estimation value is obtained, which makes the subsequent map generation more accurate.

This step is performed in the loop detection module. The historical point cloud map is the latest point cloud map that is currently saved, and the map to be generated by this method is a historical map.

Specifically, the descriptor is obtained according to the laser point cloud frame of the current frame after dedistortion, including:

Divide the de-distorted current frame laser point cloud frame data into several fan-shaped areas according to the radar scanning angle and depth, record the maximum value of the emission intensity of all points in each area, and divide the maximum value of the emission intensity in all areas Save to an array, 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 will be obtained, and the value in the array is the reflection intensity. This array is referred to as a descriptor for short.

Each keyframe that generates the history map also corresponds to a descriptor.

That is, the descriptor is: divided into several fan-shaped areas according to the radar scanning angle and depth, recording the maximum value of the emission intensity of all laser points in each area, and an array composed of the maximum value of the emission intensity in all areas.

Specifically, estimating and obtaining a second pose estimation value, the steps include:

(1) Project the de-distorted current frame laser point cloud into the coordinate system of the historical point cloud map through the first pose estimation value, and judge the coincidence degree between the projected current frame laser point cloud and the historical point cloud map Whether it is greater than the coincidence degree threshold, if it is greater, it means that a loop is initially detected, and the next step (2) is performed; otherwise, it is judged that no loop is detected, indicating that the scene in the historical point cloud map has not been reached.

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

(2) Compare the current descriptor with the descriptor corresponding to each key frame in the historical point cloud map to obtain the similarity corresponding to each key frame. If a certain similarity is greater than the loop-closing threshold, it is judged as loop-closing , in the corresponding key frame, select the point whose distance from the laser point in the current frame after dedistortion is less than the set distance threshold to form a matching point pair, and calculate the current laser point cloud according to the inter-frame matching algorithm such as: ICP algorithm (Iterative Closest Point) The second pose estimation value of the frame; otherwise, it is judged that no loop closure is detected.

It should be noted that the step of estimating and obtaining the second pose estimation value may only include the process of the above step (2). In the loop detection part, in the prior art, the matching of the loop detection is directly performed through the ICP algorithm, and the false detection rate is high. The present invention constructs a descriptor through the reflection intensity data of the point cloud, compares the current descriptor with the descriptor of each key frame in the historical point cloud map, and judges whether it is looped, which can improve the accuracy of loopback detection and reduce the false detection rate. The loop is detected, and the pose is further optimized, thereby improving the accuracy of the subsequent point cloud map generation.

Before performing the above step (2), the coincidence degree judgment in the step (1) can be performed first, which can further improve the detection accuracy and efficiency of the loop closure.

Step 5: Obtain a third estimated pose value according to the first estimated pose value and the feature points; input the second estimated pose value and the third estimated pose value into the factor graph to obtain the post End-optimized pose estimation.

This step is performed in the backend optimization module.

The obtaining the third pose estimation value specifically includes:

(1) Stitch a local map based on several key frames closest to the current frame. Specifically: during splicing, the pose values corresponding to several key frames closest to the current frame are the precise pose values optimized by the back-end optimization module.

Specifically: if no loop is found in step 4, extract the closest set distance to the current frame or several key frames within the set time to stitch together a local map; if a loop is found, extract the setting closest to the current frame Several keyframes within the distance or set time and the historical keyframes corresponding to the loopbacks in the extracted closed-loop container are spliced into a local map, which can improve the accuracy of map generation; the closed-loop container stores the corresponding historical keyframes that detect loopbacks.

(2) Perform voxel filtering on the local map and downsample, that is, reduce the number of points in the local map to reduce the calculation time, and then convert the feature points in the current frame to the local map coordinate system through the first pose estimation value In , the nearest point of the feature point is searched in the local map, and the third pose estimation value of the current laser point cloud frame is obtained by calculating the factor graph.

Specifically: for corner feature points, search for the nearest points (such as 2 or 3 points) in the local map to form a line segment, and calculate the minimum distance from point to line as the point-line residual for optimization; for surface features point, then search for the 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 is completed, all residuals (residuals corresponding to all corner feature points and surface feature points) are input into the nonlinear optimizer built by the GTSAM library, that is, the factor graph (existing), and the third digit of the current laser point cloud frame is output attitude estimate.

Further, a speed judgment is performed on the third pose estimation value, if the ratio of the motion speed from the K-1 key frame to the current frame to the motion speed from the K-2 key frame to the K-1 key frame is greater than the preset Speed ratio (such as 2 times relationship), it is judged that the current frame is degraded, that is, the trajectory drift that will appear in long-distance similar scenes such as tunnels. Then discard the calculated third pose estimation value, and recalculate the third pose estimation value according to the uniform velocity, that is: the default uniform velocity model is used, and the pose transformation from the K-2 key frame to the K-1 key frame is used as K-1 The pose transformation from the key frame to the current frame; among them, the motion speed from the K-1 key frame to the current frame is: the pose transformation from the K-1 key frame to the current frame/the time difference from the current frame to the K-1 key frame ;

Recalculate the estimated value of the third pose according to the constant velocity, specifically:

The recalculated third pose estimate = the pose of the K-1 keyframe + (the speed from the K-2 keyframe to the K-1 keyframe) * (the time difference from the current frame to the K-1 keyframe).

The factor graph also stores all previous historical pose data, and finally performs joint factor graph optimization on the above-mentioned second pose estimate and third pose estimate through the GTSAM library, so that the back-end optimized pose can be obtained through the factor graph , which is called: the estimated value of the pose after the back-end optimization of the current laser point cloud frame is saved as the track of the map as the pose data of the final current frame; the input parameter amount of the factor map of the present invention is less, so the amount of calculation is relatively small. few.

Further, in order to make the post-optimized pose more accurate, the current latitude and longitude data collected by the RTK sensor obtained in step 1 can be combined with the latitude and longitude data of the K-1 key frame to calculate the K-1 key frame to the current The pose transformation matrix of the frame (that is, the pose transformation obtained through GPS information), and then input the pose transformation matrix together with the above-mentioned second and third pose estimation values into the factor graph to obtain a more accurate post End-optimized pose.

Step 6. Determine whether the current laser point cloud frame can be used as a key frame. If so, splicing the de-distorted current laser point cloud frame into the historical point cloud according to the pose estimation value optimized by the back end of the current frame In the map, remove the dynamic object and generate a static point cloud map; if not, perform the above steps again until the map generation stops.

Among them, the method of judging whether the current frame is a key frame: if the distance between the current frame and the K-1 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 judged as a key frame. Calculate the distance according to the difference between the three-dimensional position in the optimized pose of the back end of the current frame and the optimized three-dimensional position of the K-1 key frame;

By removing dynamic objects, such as cars and pedestrians, a static high-precision point cloud map is obtained. Removing dynamic objects adopts existing methods.

The present invention estimates the IMU data between the key frame closest to the current laser point cloud frame in time and the current laser point cloud frame and the back-end optimized pose of the previous laser point cloud frame, and obtains the current laser point cloud frame The first pose estimation value; according to the first pose estimation value and the IMU data corresponding to the radar scanning time of the current laser point cloud, the current laser point cloud is de-distorted to obtain the current laser point cloud frame after de-distortion, and from it Feature points are extracted; since the first pose is estimated using the back-end optimized pose, the estimated first pose value is more accurate, and the current laser point cloud frame after dedistortion is more accurate;

Obtain the current descriptor according to the current laser point cloud frame after dedistortion; judge whether a loop is detected according to the current descriptor, if a loop is detected, estimate the second pose estimation value of the current laser point cloud frame, otherwise, not detect to the loop, and set the second pose estimation value to be empty; the present invention constructs a descriptor to detect the loop, instead of detecting the loop according to all points in the frame, reduces the false detection rate, improves the accuracy of the loop detection, and has good robustness. This improves the accuracy of the point cloud map.

Then obtain the third pose estimate according to the first pose estimate and the feature points; input the second pose estimate and the third pose estimate into the factor graph to obtain the current laser point cloud frame optimized by the backend Pose; determine whether the current laser point cloud frame can be used as a key frame, and if it can be used as a key frame, then according to the back-end optimized pose, the de-distorted current laser point cloud is spliced into the historical point cloud map, and the Generate static point cloud maps after dynamic objects.

The present invention not only receives the IMU data, but also receives the pose after back-end optimization processing, and performs the first pose estimation on the current laser point cloud together, and the obtained first pose estimation value is more accurate; The point cloud is rectified, and the feature points are extracted, so that the corrected pose and the extracted feature points are more accurate; through the descriptor, the mismatch is reduced, the accuracy of pose estimation is improved, and the accuracy of the generated map is improved, and the calculation Portions are small.

As shown in Figure 2, the embodiment of the present invention also provides a high-precision map generation device, including:

The acquisition module is used to acquire the current laser point cloud frame data and IMU data;

The IMU preprocessing module is used for if the current laser point cloud frame data is the first frame data collected by the laser radar, then send it as the first key frame to the map generation module and return to the acquisition module; otherwise, according to the latest key frame The IMU data between the frame and the current laser point cloud frame and the back-end optimized pose of the previous laser point cloud frame fed back by the back-end optimization module are estimated to obtain the first pose estimation value of the current laser point cloud frame;

The front-end odometer module is used to de-distort the current laser point cloud according to the first estimated pose value and the IMU data corresponding to the scanning time of the current laser point cloud, to obtain the de-distorted current laser point cloud frame, and Extract feature points from it;

The loopback detection module is used to obtain the current descriptor according to the current laser point cloud frame after dedistortion, and judge whether to detect a loopback according to the current descriptor. If a loopback is detected, it is estimated to obtain the second bit of the current laser point cloud frame pose estimate; otherwise, no loop closure is detected, and the second pose estimate is set to null;

The back-end optimization module is used 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 the factor graph to obtain Backend-optimized pose estimates for the current frame;

The map generation module is used to generate a point cloud map through the first key frame if the current frame is the first frame, otherwise, judge whether the current frame can be used as a key frame, and if so, optimize according to the backend of the current frame Splice the de-distorted current laser point cloud frame into the historical point cloud map, and generate a static point cloud map after removing dynamic objects; if not, return to the acquisition module until the map generation is stopped.

The specific method steps adopted in each module are the same as the above-mentioned map generation method, and will not be repeated here.

In the high-precision map generation device provided by the embodiment of the present invention, the IMU preprocessing module is based on the IMU data between the key frame closest to the current laser point cloud frame and the current laser point cloud frame and the previous laser point cloud frame. The back-end optimized pose is estimated to obtain the first pose estimation value of the current laser point cloud frame; the front-end odometer module calculates the current The laser point cloud is de-distorted, and the current laser point cloud frame after de-distortion is obtained, and the feature points are extracted from it; since the first pose is estimated by the back-end optimized pose, the estimated first position The attitude value is more accurate, which also makes the current laser point cloud frame after dedistortion more accurate; the loopback detection module obtains the current descriptor according to the current laser point cloud frame after dedistortion; judges whether to detect the loopback according to the current descriptor, if detected loopback, then estimate the second pose estimation value of the current laser point cloud frame, otherwise, no loopback is detected, and the second pose estimation value is set to empty; the present invention constructs a descriptor to detect loopback instead of according to All points are used to detect loops, reduce the false detection rate, improve the accuracy of loop detection, and have good robustness, thereby improving the accuracy of point cloud maps. The back-end optimization module obtains the third pose estimation value 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 current laser point cloud frame after end-optimized pose; the map generation module judges whether the current laser point cloud frame can be used as a key frame, and if it can be used as a key frame, then splicing the de-distorted current laser point cloud into the history according to the back-end optimized pose In the point cloud map, a static point cloud map is generated after removing dynamic objects.

The present invention not only receives the IMU data, but also receives the pose after back-end optimization processing, and performs the first pose estimation on the current laser point cloud together, and the obtained first pose estimation value is more accurate; The point cloud is rectified, and the feature points are extracted, so that the corrected pose and the extracted feature points are more accurate; through the descriptor, the mismatch is reduced, the accuracy of pose estimation is improved, and the accuracy of the generated map is improved, and the calculation Portions are small.

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 above-mentioned embodiments The high-precision map generation method described above.

An embodiment of the present invention provides a readable storage medium, wherein a computer program is stored in the readable storage medium, and when the computer program is executed by a processor, it is used to realize the high-precision map described in any one of the above-mentioned embodiments generate method.

The above is only a preferred embodiment of the present invention, it should be pointed out that for those of ordinary skill in the art, without departing from the principle of the present invention, some improvements and modifications can also be made, and these improvements and modifications are also possible. It should be regarded as the protection scope of the present invention.

Claims (14)

1. A high-precision map generation method is characterized by comprising the following steps:

acquiring current laser point cloud frame data and IMU data;

if the current laser point cloud frame data is first frame data acquired by a laser radar, taking the current laser point cloud frame data as a first key frame, generating a point cloud map through the first key frame, and executing the steps again; otherwise, estimating to obtain a first pose estimation value of the current laser point cloud frame according to IMU data between the latest key frame and the current laser point cloud frame and the pose of the last laser point cloud frame after back-end optimization;

according to the first pose estimation value and IMU data corresponding to the time for scanning the current laser point cloud, carrying out distortion removal on the current laser point cloud to obtain a current laser point cloud frame after distortion removal, and extracting feature points from the current laser point cloud frame;

obtaining a current descriptor according to the current laser point cloud frame after distortion removal, judging whether loop is detected or not according to the current descriptor, and if loop is detected, estimating to obtain a second position attitude estimation value of the current laser point cloud frame; otherwise, if no loop is detected, setting the second position estimation value to be null;

obtaining a third attitude estimation value according to the first attitude estimation value and the characteristic point; inputting the second pose estimation value and the third pose estimation value into a factor graph to obtain a pose estimation value optimized at the rear end of the current frame;

judging whether the current laser point cloud frame can be used as a key frame, if so, splicing the undistorted current laser point cloud frame into a historical point cloud map according to the pose estimation value optimized at the rear end of the current frame, removing a dynamic object, and generating a static point cloud map; and if not, re-executing the steps until the map generation is stopped.

2. The method as claimed in claim 1, wherein obtaining the current descriptor from the undistorted current laser point cloud frame comprises:

dividing points in the current laser point cloud frame after distortion removal into a plurality of areas according to the radar scanning angle and depth;

the maximum value of the emission intensity of the laser spot in each area is saved into an array, which is taken as the current descriptor.

3. The method as claimed in claim 1, wherein the estimating of the second pose estimation value of the current laser point cloud frame includes:

respectively comparing the current descriptor with the descriptor corresponding to each key frame in the historical point cloud map to obtain the similarity corresponding to each key frame;

and if the similarity is greater than a loop threshold, judging to detect a loop, selecting points with the distance from the points in the undistorted current laser cloud frame to be less than a set distance threshold from the corresponding key frame to form a matching point pair, and estimating according to a matching algorithm to obtain a second pose estimation value of the current laser point cloud frame.

4. A high accuracy map generation method according to claim 1, further comprising, before detecting the loop, preliminarily detecting the loop, including:

and projecting the undistorted current laser point cloud frame to a coordinate system of a historical point cloud map through the first pose estimation value, judging whether the coincidence degree of the projected current laser point cloud and the historical point cloud map is greater than a coincidence degree threshold value, if so, indicating that loop is preliminarily detected, otherwise, judging that no loop is detected.

5. The method as claimed in claim 1, wherein the step of performing the distortion removal on the current laser point cloud to obtain a current laser point cloud frame after the distortion removal comprises:

calculating the time of each point in the current laser point cloud frame scanned by the laser radar;

calculating the relative rotation amount of each point in the laser radar according to the corresponding IMU data in the current laser point cloud scanning time and the scanning time of each point;

calculating the relative displacement of each point according to the pose change from the nearest key frame to the current laser point cloud frame;

and according to the relative rotation amount and the relative displacement amount, carrying out distortion removal on points in the current laser point cloud frame to obtain the current laser point cloud frame after distortion removal.

6. The method as claimed in claim 1, wherein extracting feature points from the undistorted current laser point cloud frame comprises:

and calculating the curvature of each point of the current laser point cloud frame after distortion removal, taking the point with the curvature exceeding a preset angle threshold value as an angle characteristic point, and taking the point with the curvature smaller than a preset surface threshold value as a surface characteristic.

7. A high-precision map generation method according to claim 1, wherein the obtaining a third pose estimation value according to the first pose estimation value and the feature point comprises:

splicing a plurality of key frames closest to the current laser point cloud frame into a local map;

and converting the characteristic points into the local map coordinate system through the first pose estimation value, calculating point-line residual errors of the angular characteristic points and point-surface residual errors of the surface characteristic points, and inputting the point-line residual errors and the point-surface residual errors into a factor graph to obtain a third pose estimation value of the current laser point cloud frame.

8. A high accuracy map generating method according to claim 7,

for the angle feature point, searching a plurality of points which are closest to the angle feature point in the local map to form a line segment, and calculating the minimum distance from the angle feature point to the line segment as the point line residual error;

and for the surface feature point, searching a plurality of points which are closest to the surface feature point in the local map, constructing a plane, and calculating the minimum distance from the surface feature point to the plane as the point-surface residual error.

9. A high-precision map generation method according to claim 1, further comprising, after obtaining the third pose estimation value:

and judging the speed of the third posture estimation value, if the ratio of the moving speed from the latest key frame to the current frame to the moving speed from the next-nearest key frame to the latest key frame is greater than a preset speed ratio, judging that the current frame has a degradation phenomenon, and recalculating the third posture estimation value according to the uniform speed.

10. The method as claimed in claim 1, wherein the determining whether the current laser point cloud frame can be used as a key frame comprises:

if the distance between the current laser point cloud frame and the nearest key frame is larger than the key frame threshold value, judging that the current laser point cloud frame is a key frame;

or, if the distance between the current laser point cloud frame and the nearest key frame is greater than the key frame threshold value, and the number of the feature points in the current frame is greater than the set feature point threshold value, determining that the current laser point cloud frame is the key frame.

11. A high-precision map generation method according to claim 1, further comprising:

and acquiring current longitude and latitude height data, calculating a pose transformation matrix from the latest key frame to the current frame by combining the current longitude and latitude height data with the longitude and latitude height data of the latest key frame, and inputting the pose transformation matrix, the second pose estimation value and the third pose estimation value into a factor graph to obtain a pose estimation value optimized at the rear end of the current frame.

12. A high-precision map generation apparatus, comprising:

the acquisition module is used for acquiring current laser point cloud frame data and IMU data;

the IMU preprocessing module is used for taking the current laser point cloud frame data as a first key frame if the current laser point cloud frame data is first frame data acquired by a laser radar, sending the first key frame to the map generating module and returning the first key frame to the acquisition module; otherwise, estimating to obtain a first pose estimation value of the current laser point cloud frame according to IMU data between the latest key frame and the current laser point cloud frame and the pose of the last laser point cloud frame fed back by the back-end optimization module through back-end optimization;

the front-end odometer module is used for carrying out distortion removal on the current laser point cloud according to the first position posture estimation value and IMU data corresponding to the time of scanning the current laser point cloud to obtain a current laser point cloud frame after distortion removal and extracting feature points from the current laser point cloud frame;

the loop detection module is used for obtaining a current descriptor according to the current laser point cloud frame after distortion removal, judging whether loop is detected or not according to the current descriptor, and estimating to obtain a second position posture estimation value of the current laser point cloud frame if the loop is detected; otherwise, if no loop is detected, setting the second position and posture estimation value to be null;

the rear-end optimization module is used for obtaining a third attitude estimation value according to the first attitude estimation value and the characteristic points; inputting the second posture estimation value and the third posture estimation value into a factor graph to obtain a rear-end optimized posture estimation value of the current frame;

the map generation module is used for generating a point cloud map through the first key frame if the current frame is the first frame, otherwise, judging whether the current frame can be used as the key frame, if so, splicing the undistorted current laser point cloud frame into a historical point cloud map according to a pose estimation value optimized by the rear end of the current frame, and generating a static point cloud map after removing a dynamic object; and if not, returning to the acquisition module until the generation of the map is stopped.

13. A map generation apparatus characterized by comprising: a memory, a processor, and a computer program, the computer program being stored in the memory, the processor running the computer program to perform the high accuracy map generation method of any one of claims 1 to 11.

14. A readable storage medium, wherein a computer program is stored in the readable storage medium, and when executed by a processor, the computer program is configured to implement the high accuracy map generation method according to any one of claims 1 to 11.

Images