An offline map construction method based on dense constraints and graph optimization

Abstract

The application discloses an offline map construction method based on dense constraints and graph optimization, which takes a laser radar as a main sensor, takes point cloud frame information and laser odometer information as main input information, and introduces the concept of dense constraints, and can be divided into six modules of raw data collection, sensor data processing, local map and key frame construction, constraint construction and parallel registration, key frame global optimization and map splicing according to a flow, and finally generates a three-dimensional point cloud map describing a surrounding environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving, and particularly relates to an offline map construction method based on dense constraints and graph optimization. BACKGROUND

[0002] In recent years, with the development of computer vision, artificial intelligence and remote sensing surveying and mapping technologies, simultaneous localization and mapping technology has gradually matured, which can make environment maps and vehicle positions online without relying on other prior information, and thus gradually becomes a mainstream method in the field of map construction. After years of development, simultaneous localization and mapping technology mainly consists of a front end and a back end. The main work of the front end includes: extracting data in the sensor, performing data association, and calculating inter-frame pose transformation using associated data. For a laser radar-based simultaneous localization and mapping scheme, data association is reflected in obtaining corresponding point information in two frames, and then calculating inter-frame transformation according to the associated data points. After the inter-frame transformation is calculated, the back end can establish a mathematical model of the fusion scheme according to this. The mathematical model of the back end can be understood as a maximum a posteriori probability problem. Solving this problem in the back end can obtain the optimized pose, and the final environment map can be completed by point cloud splicing according to the calculated laser radar pose.

[0003] Among the existing methods of simultaneous mapping and positioning, LOAM is a pioneering work in the feature point method, and many scholars have carried out subsequent research on this idea. On this basis, various variants such as A-LOAM, F-LOAM and LeGO-LOAM have been proposed. The LOAM method extracts edge and corner points with large curvature and planar points with small curvature in the front end according to the curvature information of each point in the point cloud, which is used to describe the whole frame of point cloud. In the aspect of inter-frame matching, the current frame feature points are matched with the lines and surfaces in the previous frame point cloud, and the sum of point-line distance and point-surface distance is optimized by using a nonlinear optimization method, so as to obtain the pose transformation of the two frames of point cloud. In the back end, the pose graph method is used to optimize the overall key frame pose, and the cumulative error of the front end is corrected by using loop constraints and other observation information, so as to improve the accuracy of map construction and online positioning.

[0004] However, the traditional simultaneous localization and mapping scheme using laser radar such as the above method makes many compromises for real-time performance, such as in the calculation of the front-end odometer, by extracting feature points, only a small number of points in the original point cloud are selected to calculate the inter-frame pose change to obtain shorter calculation time, which to some extent discards existing information; in addition, in the back-end optimization module, due to the characteristics of the front-end odometer recursive calculation of the pose, the constraints are established by connecting the adjacent front and rear frames of a key frame, and the constraints between the frames are ignored, which is a sparse linking method from the perspective of graph optimization, that is, the observation information used in the overall optimization of all poses is scarce. Therefore, when the traditional simultaneous localization and mapping scheme using laser radar is used for large-scale map construction, the accumulated error cannot be well eliminated, and the visual display is that the building walls, corners and other elements near the loop points will have a visible offset phenomenon and ghosting phenomenon. SUMMARY

[0005] In view of the shortcomings of the existing method mentioned above, the present application provides an offline map construction method based on dense constraints and graph optimization, which modifies and improves the compromise processing in the field of simultaneous localization and mapping, and introduces the concept of dense constraints to construct the environment map. We will explain the specific details of this method in detail below, which can be divided into six modules according to the process: raw data acquisition, sensor data processing, local map and key frame construction, constraint construction and parallel registration, global optimization of key frames, and map stitching.

[0006] An offline map construction method based on dense constraints and graph optimization, comprising the following steps:

[0007] Step one: use the laser radar to scan the surrounding environment, obtain the laser radar original point cloud information, and use the laser radar original point cloud information for point cloud registration to obtain the front-end laser odometer information, and store the laser radar original point cloud information and the front-end laser odometer information;

[0008] Step two: use pose transformation to eliminate the distortion caused by laser radar motion, and align the frame time of the front-end laser odometer information to the frame time of the laser radar original point cloud information;

[0009] Step three: splice the laser radar original point cloud information through coordinate transformation to form a local map describing the surrounding environment, and select key frames according to the interval of the frames in the laser radar original point cloud information, and bind the local map with the key frames;

[0010] Step four: constructing a nearby key frame constraint through a distance threshold for the key frame selected in step three; and searching for a loop-back frame, constructing a loop-back key frame constraint for the searched key frame, and if two key frames are close in spatial position but far in time, the two frames are called loop-back; the nearby key frame constraint and the loop-back key frame constraint cooperatively form a dense constraint, and the specific value of the dense constraint is calculated in a parallel manner;

[0011] Step five: constructing a pose graph by taking the key frame pose as a node variable and the dense constraint as an edge, and globally optimizing the optimization problem of the pose graph through a nonlinear optimization manner to obtain an optimal key frame pose;

[0012] Step six: transforming the local map bound with the key frame into a global coordinate system through the key frame pose obtained in step five to splice and obtain a point cloud map of the environment.

[0013] As a preferred scheme of the application, the point cloud registration in step one is completed using an ICP method.

[0014] As a preferred scheme of the application, the step two specifically comprises the following steps: firstly, reading the stored laser radar original point cloud information and front-end laser odometry information, since the vehicle is synchronously moving while the laser radar is collecting data, the original point cloud information needs to be first processed for distortion, and then the spherical interpolation and linear interpolation are combined to align the odometry information to the laser point cloud data frame time.

[0015] As a preferred scheme of the application, the key frame construction method in step three specifically comprises the following steps:

[0016] 3.1) setting as a frame set of laser radar original point cloud information in time sequence, as a frame set of front-end laser odometry information in time sequence, wherein P k represents a certain frame point cloud, T k represents a laser radar pose at the same time;

[0017] 3.2) setting a key frame every N frames, the key frame corresponds to a local map, that is, if the current key frame is KF i ={submap i ,T k}, the next key frame is KF i+1 ={submap i+1 ,T k+N};

[0018] 3.3) constructing a local map by using M frames before and after the key frame and the key frame itself, that is, submap i ={P k-M,…P k ,…P k+M}where N>M;

[0019] 3.4) When constructing the local map, transform the M frames before and after the key frame into the laser radar coordinate system corresponding to the key frame to splice the point cloud, as shown in the following formula:

[0020]

[0021] 3.5) Obtain the key frame containing the local map submap and the pose information T, and the local map is in the coordinate system corresponding to the pose information T; the pose information is optimized as a priori value by the subsequent graph optimization link, so the local map is the optimized key frame pose T opt Transformed into the global coordinate system to participate in the splicing of the global environment map, assuming that the local map contained in the key frame is submap local Transformed into the global coordinate system is submap global According to the following formula:

[0022] submap global =T opt ·submap local

[0023] As a preferred scheme of the present application, the constraint construction and parallel registration method in step four is realized through the following sub-steps:

[0024] 4.1): Use the Euclidean distance between key frames as the basis for constructing the adjacent frame constraint, and the distance threshold is manually set to d th , that is, all key frames within the d th range around the current key frame are considered to be able to establish an adjacent frame constraint with the current frame;

[0025] 4.2): Establish a Scan Context descriptor vector for each key frame, and select the key frame pair that meets the loop closing condition according to the distance between the descriptor vectors, and add it as a loop frame constraint;

[0026] 4.3): Parallel computing is performed on the adjacent frame constraint established in step 4.1) and the loop frame constraint constructed in step 4.2), the computing task is allocated to each thread of the CPU using the OpenMP library, and the specific calculation method uses the ICP point cloud registration method, and the constraint pair is screened using the matching score.

[0027] Compared with the prior art, the present application has the following beneficial effects

[0028] The method firstly combines the collected point cloud information into a local map, uses the concept of key frame to organize all the local maps, then performs dense constraint construction according to a distance threshold and the similarity of a Scan Context descriptor, then optimizes the key frame pose by using a global pose graph optimization method, splices the local map by using the optimized pose, and finally forms a point cloud map of a given scene. Compared with the existing synchronous mapping and positioning related methods, the scheme uses denser local map point clouds for registration in the front end, and adds denser edge constraints in the back end, and the two modification schemes enable the scheme to construct a more accurate point cloud map. BRIEF DESCRIPTION OF DRAWINGS

[0029] Figure 1 is a general flowchart of an offline map construction method based on dense constraints and graph optimization;

[0030] Figure 2 is a local map and key frame construction schematic diagram. DETAILED DESCRIPTION

[0031] In order to more specifically describe the present application, the technical solutions of the present application will be described in detail below in combination with the drawings and specific embodiments.

[0032] The present application designs an offline map construction method based on dense constraints and graph optimization, as shown in Figure 1 , which includes the following six steps:

[0033] Step 1: In the original data acquisition stage, save the laser radar original point cloud information obtained in the process of acquiring environmental data and the front laser odometry information obtained by point cloud registration: record the original data by using the record command of the rosbag module built in the ROS operating system, and store the recorded data as a.bag file in the hard disk of the vehicle-mounted computing platform. The point cloud registration is completed by using the ICP method, and the target is to: given the source point cloud P and the target point cloud Q, through solving the rotation matrix R and the translation vector t, make each point p in the source point cloud P i , and the distance between the nearest point q i in the target point cloud Q after rotation and translation transformation reaches a minimum value, and the total error objective function can be represented by the following formula:

[0034]

[0035] The optimization problem can be solved by using the singular value decomposition based method, and the least square solution satisfying the above formula should make the two point clouds have the same centroid position. First, define the centroid of the point cloud:

[0036]

[0037]

[0038] The points in the two point clouds are centroid-removed to obtain centroid-removed coordinates, respectively denoted as and

[0039]

[0040]

[0041] After centroid removal, the minimization of the optimization objective is completed in two steps. First, the rotation matrix R is solved:

[0042]

[0043] The two-norm can be expanded to obtain:

[0044]

[0045] From the above formula, it can be found that the objective function can be minimized by maximizing the term . To this end, the H matrix is defined as follows:

[0046]

[0047] The maximization of the term can be achieved by maximizing the trace of the matrix RH. To this end, the matrix H is singular value decomposed to obtain:

[0048] H = UΛV T

[0049] When the rotation matrix R = VU T , the trace of the matrix RH takes the maximum value, and thus the value of the rotation matrix is obtained. After obtaining the rotation matrix R, the translation vector t can be directly calculated by the centroid coordinates:

[0050]

[0051] At this point, the update calculation of the rotation matrix and the translation vector can be completed. After multiple iterations, when the error is less than the error requirement or the number of iterations reaches the upper limit, the iteration is completed, and the final transformation relationship (rotation matrix R and translation vector t) between the two point clouds is obtained.

[0052] Step two: the sensor data processing stage mainly completes two tasks of laser radar motion distortion correction and laser radar point sensor data timestamp alignment. First, read the bag file stored in the hard disk to obtain all the information. Since the laser radar collects data at the same time as the vehicle is moving synchronously, the original point cloud information needs to be first de-distorted, and then the spherical interpolation and linear interpolation combined method is used to align the laser point cloud data frame time to the odometer information.

[0053] For the kth frame of point cloud data P k , the frame header time is t k , the corresponding laser radar coordinate system is L k , the frame tail time is t k+1 , and the corresponding laser radar coordinate system is L k+1 . Assuming that the pose of L k in the global coordinate system is known q k = [q x , q y , q z , q w ] T is a quaternion used to represent the orientation information in the pose, t k = [t x , t y , t z ] T is a three-dimensional vector used to represent the translation information in the pose, and correspondingly, the pose of L k+1 in the global coordinate system is set as The uniform motion model is used to interpolate the pose. During interpolation, the interpolation ratio s can be obtained from the point time information attached to each point, as follows:

[0054]

[0055] where t (k,i) is the point time information of point p (k,i) , Period is the laser radar scanning period, and the pose interpolation result is as follows

[0056]

[0057] Using the laser radar pose corresponding to the time of point p (k,i) solved above , point p (k,i) can be projected into the laser radar coordinate system L k at the frame header time to obtain the projected point as shown below, let q k and t ka transformation matrix composed of for q (k,i) and t (k,i) a transformation matrix composed of a distortion-free point cloud composed of

[0058]

[0059] Step three: In the local map and key frame construction stage, the original laser point cloud frame information is spliced into a local map and a key frame is generated. Let be a frame set of laser radar original point cloud information in time sequence, be a frame set of front-end laser odometry information in time sequence, where P k represents a certain frame point cloud, and T k represents the laser radar pose at the same time, which is expressed in the form of a transformation matrix. Since the two have undergone timestamp alignment processing, the two sets have the same set size, and the elements with the same subscript also have the same timestamp.

[0060] 1) Set a key frame every N frames, and the key frame corresponds to the local map, that is, if the current key frame is KF i ={submap i ,T k}, then the next key frame is KF i+1 ={submap i+1 ,T k+N};

[0061] 2) The M frames before and after the key frame are used together with the key frame itself to constitute the local map, that is, submap i ={P k-M ,…P k ,…P k+M} where N>M;

[0062] 3) When constructing the local map, the M frames before and after the key frame need to be transformed into the laser radar coordinate system corresponding to the key frame for point cloud splicing, as shown in the following formula:

[0063]

[0064] After the above steps, the key frame containing the local map submap and the pose information T can be obtained, and the local map is in the coordinate system corresponding to the pose information T. The pose information will be optimized by the subsequent graph optimization link, so the local map can be transformed into the global coordinate system by the optimized key frame pose T opt to participate in the splicing of the global environment map, and let the local map contained in the key frame be submaplocal , transform to global coordinate system then have submap global , transform is completed according to the following formula:

[0065] submap global = T opt ·submap local

[0066] Step four: in the constraint construction and parallel registration stage, two types of constraints are constructed, which are adjacent frame constraints and loop frame constraints, and the selected two types of constraints are calculated by parallel registration.

[0067] 1) In the construction of adjacent frame constraints, the dense constraint construction method based on distance threshold is selected, and the distance threshold is set by manual setting, which is d th , that is, all key frames within d th range around the current key frame are considered to be able to establish adjacent frame constraints with the current frame.

[0068] 2) In the construction of loop frame constraints, the Scan Context descriptor method is used for loop frame search, and loop constraints are added between the key frames with good matching. The specific calculation method is as follows:

[0069] Assume that the effective scanning distance of the laser radar is L max , divide N r ring regions and N s fan-shaped regions in the radial and rotational directions respectively, and the complete laser point cloud data can be represented by the following formula after division in this way:

[0070]

[0071] In the above formula, P ij is the smallest fan-shaped grid unit, which can be uniquely determined by the corresponding index in the radial and rotational directions. For each fan-shaped grid unit, the highest point value of the point cloud in the grid region is used to encode the grid unit, and the encoding function is as follows:

[0072]

[0073] In the above formula, z(·) returns the height value of the point coordinates, and the value of the deleted unit without points is 0. The global descriptor I of the entire point cloud can be obtained, which is a two-dimensional matrix with a size of N r ×N s :

[0074]

[0075] Assume that two Scan Context global descriptors are Iq (current frame descriptor) and I c (history frame descriptor), representing the jth column in I q , the distance between two descriptors is defined as: representing the jth column in I c , the distance between two descriptors is defined as:

[0076]

[0077] When the distance between two descriptors is less than a certain threshold, it can be considered that the current two frames can constitute a loop, and be added to the loop frame constraint.

[0078] 3) In the aspect of parallel registration, ICP is combined with OpenMP to calculate the observation values of all constraints more quickly, and the score of the registration result is calculated, and the effective constraint pairs are selected according to the score. As for the concept of matching degree score, the present application uses fitness(R, t) to represent it, which is defined as:

[0079]

[0080] Where R and t are the transformation relationship between two local maps solved by the ICP algorithm, q i and p i are the closest point pairs in the two local maps, and M is the number of point pairs participating in the matching degree score calculation.

[0081] Step five: the key frame global optimization stage, a pose graph structure containing two types of elements is constructed: the first type of element is the key frame pose node, and the output of the front-end odometry is used as the prior value of the node participating in optimization; the second type of element is a binary edge, which is the pose constraint edge between adjacent frames and the pose constraint edge between loop frames. Then the graph structure is represented as a least square problem, and a nonlinear optimization method is used to optimize all pose nodes to obtain the optimized pose of all key frames.

[0082] Step six: the map stitching stage is based on the globally optimized key frame pose, and the local map corresponding to each key frame is transformed into the map coordinate system through the pose, so that the final three-dimensional point cloud map can be obtained.

Claims

1. An offline map construction method based on dense constraints and graph optimization, characterized in that, Includes the following steps: Step 1: Use LiDAR to scan the surrounding environment, obtain the raw point cloud information of LiDAR, and use the raw point cloud information of LiDAR for point cloud registration to obtain the front-end laser odometry information. Store the raw point cloud information of LiDAR and the front-end laser odometry information. Step 2: Eliminate the distortion caused by the motion of the lidar by using pose transformation, and align the frame time of the front-end lidar odometry information to the frame time of the original point cloud information of the lidar. Step 3: The original point cloud information of the LiDAR is stitched together by coordinate transformation to form a local map describing the surrounding environment. Key frames are selected according to the frame interval in the original point cloud information of the LiDAR and the local map is bound to the key frames. Step 4: Construct neighboring keyframe constraints for the keyframes selected in Step 3 using a distance threshold; and perform a loop closure frame search, constructing loop closure keyframe constraints for the searched keyframes. If two keyframes are close in spatial location but far apart in time, they are said to form a loop; neighboring keyframe constraints and loop closure keyframe constraints work together to form dense constraints, and the specific values ​​of the dense constraints are calculated in a parallel manner. Step 5: Construct a pose graph using keyframe poses as node variables and dense constraints as edges. Perform global optimization on the pose graph using nonlinear optimization to obtain the optimal keyframe pose. Step 6: Transform the local map bound to the keyframes into the global coordinate system using the keyframe poses obtained in Step 5, and then stitch them together to obtain the point cloud map of the environment.

2. The offline map construction method based on dense constraints and graph optimization according to claim 1, characterized in that, In step one, point cloud registration is completed using the ICP method.

3. The offline map construction method based on dense constraints and graph optimization according to claim 1, characterized in that, The second step is as follows: First, the stored original point cloud information of the lidar and the front-end lidar odometer information are read. Since the vehicle is moving synchronously while the lidar is collecting data, it is necessary to first perform distortion correction on the original point cloud information, and then use a combination of spherical interpolation and linear interpolation to align the odometer information to the time frame of the lidar point cloud data.

4. The offline map construction method based on dense constraints and graph optimization according to claim 1, characterized in that, The specific method for constructing the local map and keyframes in step three is as follows: 3.1) Let This is a set of frames of raw point cloud information from LiDAR, arranged in chronological order. Let P be a set of frames of front-end laser odometry information in chronological order. k T represents a point cloud in a certain frame. k Indicates the lidar pose at the same moment; 3.2) Set a keyframe every N frames. Each keyframe corresponds to a local map. That is, if the current keyframe is KF... i ={submap i ,T k If}, then the next keyframe is KF. i+1 ={submap i+1 ,T k+N }; 3.3) Construct a local map, i.e., a submap, using the M frames before and after the keyframe and the keyframe itself. i ={P k-M ,…P k ,…P k+M Where N > M; 3.4) When constructing a local map, transform the M frames before and after the keyframe to the corresponding LiDAR coordinate system for point cloud stitching, as shown in the following formula: 3.5) Obtain keyframes containing a local map (submap) and pose information T, where the local map is in the coordinate system corresponding to the pose information T. The pose information will be used as a priori value for subsequent graph optimization. Therefore, the local map is derived from the optimized keyframe pose T. opt Transform to the global coordinate system to participate in the stitching of the global environment map; let the local map contained in the keyframe be called the submap. local Transforming to the global coordinate system results in submap. global The transformation is performed according to the following formula: subfolder global =T opt ·subfolder local 。 5. The offline map construction method based on dense constraints and graph optimization according to claim 1, characterized in that, The constraint construction and parallel registration method in step four is implemented through the following sub-steps: 4.1): The Euclidean distance between keyframes is used as the criterion for constructing neighboring frame constraints. The distance threshold is set manually and is set to d. th That is, the area around the current keyframe d th All keyframes within the range are considered to be able to establish neighboring frame constraints with the current frame; 4.2): For each keyframe, a Scan Context descriptor vector is created. Based on the distance between the descriptor vectors, keyframe pairs that meet the loop closure condition are selected and added as loop closure frame constraints. 4.3): Parallel computation is performed on the neighboring frame constraints established in step 4.1) and the loop closure frame constraints constructed in step 4.2). The computation tasks are distributed to various threads of the CPU using the OpenMP library. The specific computation method uses the ICP point cloud registration method, and the constraint pairs are filtered using the matching degree score.

Images