Method and device for constructing a combined multi-constraint laser inertial point cloud map

Abstract

The present invention provides a method and device for constructing a laser inertial point cloud map with combined multiple constraints. Through a data input module, mobile laser point cloud data and inertial measurement data that are hardware time-synchronized and acquired by a lidar and an inertial measurement unit sensor are received. The motion distortion of a single-frame laser point cloud is corrected using the inertial measurement data; based on an iterative error Kalman filter framework, the laser point-to-plane observation and the inertial measurement data are tightly coupled to estimate the pose of the carrier; finally, a factor graph model including relative pose constraints of multiple submaps and gravity constraints is constructed, and the iterative closest point algorithm with a robust function is used to accurately calculate the relative pose between submaps, and the submap poses are optimized to obtain a high-precision point cloud map. The invention is applicable to fields such as unmanned autonomous surveying and mapping, robot navigation, and planetary exploration. Through the front-end single-frame pose estimation based on Kalman filter and the back-end submap pose optimization with multiple types of constraints, the accuracy and efficiency of point cloud mapping are significantly improved.

Description

Technical Field

[0001] Embodiments of the present invention relate to the technical fields of autonomous surveying and mapping and planetary exploration, and particularly to a method and device for constructing a laser inertial point cloud map by combining multiple constraints. Background Art

[0002] Traditional mobile laser scanning technology usually relies on a GNSS / INS (Global Navigation Satellite System / Inertial Navigation System) integrated navigation system and lidar to obtain the pose of the carrier platform and laser observation information. Subsequently, through geocalibration processing, a high-precision three-dimensional point cloud map can be generated. However, this method depends on the stability of GNSS signals and will have problems with inaccurate positioning in environments where GNSS signals are blocked or missing (such as indoors, tunnels, forests, etc.). In recent years, with the rapid development of the Simultaneous Localization and Mapping (SLAM) technology, mobile measurement platforms no longer completely rely on GNSS signals. The SLAM technology can combine multiple sensors such as lidar, cameras, and Inertial Measurement Units (IMUs). Even in the absence of GNSS signals, it can generate accurate three-dimensional maps by obtaining environmental information in real time. This technology has been widely applied in fields such as unmanned autonomous surveying and mapping, robot navigation, and planetary exploration, not only improving the adaptability to complex environments but also greatly enhancing the mapping accuracy and speed of the environment, promoting the technological progress of related industries.

[0003] SLAM methods can be classified into three categories according to sensor types: laser SLAM, visual SLAM, and multi-sensor fusion. Lidar sensors can directly capture high-precision environmental point clouds and are less affected by light compared to visual cameras. Inertial measurement units can be used for positioning and pose estimation and removing the motion distortion of laser point clouds under high dynamics by measuring the motion state of the carrier platform. Combining the advantages of lidar sensors and inertial measurement units can achieve pose estimation and map construction of mobile measurement platforms. However, due to errors in matching between point cloud frames and submaps, and between submaps, pose estimation drift will occur, and further cause geometric inconsistency of the point cloud map. The factor graph optimization method indirectly optimizes the geometric structure of the map by effectively minimizing the relative pose error between graph nodes. However, existing optimization methods do not fully utilize constraints such as co-visibility to construct an accurate factor graph model to optimize errors, and there is still a bottleneck in the low accuracy of integrated positioning and mapping. In complex scenarios, existing methods are difficult to meet the technical requirements of the robustness and accuracy of point cloud mapping. Summary of the Invention

[0004] To overcome the deficiencies of the prior art, the present invention provides a method for constructing a laser inertial point cloud map with combined multiple constraints to generate a high-precision point cloud map, correct the motion distortion of the laser point cloud according to inertial measurement data, accurately calculate the relative pose between submaps through the ICP (Iterative Closest Points) point cloud matching method with a robust function added, and adopt a special strategy to accurately construct three types of submap constraints for pose graph optimization, improving the robustness and accuracy of point cloud mapping during movement.

[0005] A method for constructing a laser inertial point cloud map with combined multiple constraints provided by the present invention includes the following steps:

[0006] Step 1, obtain time-synchronized mobile laser point cloud data and inertial measurement data;

[0007] Step 2, use the IMU to correct the motion distortion of a single frame of laser point cloud;

[0008] Step 3, tightly couple the laser point-to-plane observation and inertial measurement data based on the iterative error Kalman filter framework to estimate the carrier pose;

[0009] Step 4, construct a factor graph model with multiple matching constraints to optimize the pose estimation. Divide the point cloud submaps according to the time span, construct a factor graph model including various relative pose constraints between submaps and gravity constraints, use the ICP method with a robust function added to accurately calculate the relative pose between submaps, and optimize the submap poses to obtain a high-precision point cloud map.

[0010] Furthermore, the discrete inertial pose within a frame of laser is obtained by integrating the inertial measurement data. Each laser point linearly interpolates its pose relative to the starting moment of the laser frame according to the discrete inertial pose close in time, and projects the laser point to the carrier coordinate system at the end moment of the laser frame according to this relative pose to complete the motion distortion correction.

[0011] Furthermore, the specific process of the said Step 3 includes:

[0012] Step 3.1: statically initialize the state variables of the Kalman filter;

[0013] Step 3.2: use the corresponding IMU measurement values within the current frame for Kalman filter state prediction;

[0014] Step 3.3: project the motion-distortion-corrected point cloud to the map coordinate system, use spatial nearest neighbor search to find adjacent points in the map and fit a plane with them, eliminate the matching pairs with large distances, and complete the feature association of point to plane;

[0015] Step 3.4: Based on the result of feature association, calculate the Jacobian matrix of the laser observation, and then update the iterative error Kalman filter to obtain the carrier pose.

[0016] Furthermore, the time synchronization is hardware time synchronization. The timestamps of the laser point cloud data and the inertial measurement data are under the same time reference, and the error is less than 1 ms.

[0017] Furthermore, in step 4, the relative pose constraints between submaps are constructed to optimize the point cloud map. The specific process is as follows:

[0018] According to the set time span, project the sequence of laser frames within the time window to the coordinate system at the starting moment of the window according to the corresponding poses to obtain point cloud submaps.

[0019] Search for and construct three types of submap matching constraints: adjacent, co-visible, and revisited, and use the ICP method with a robust function to obtain the relative pose between submaps.

[0020] Based on the above three types of constraints, construct a factor graph model including various relative pose constraints between submaps and gravity constraints, and then use the LM (Levenberg-Marquardt) algorithm to optimize the submap poses to obtain a high-precision point cloud map.

[0021] Furthermore, in step 4, an adaptive pyramid feature aggregation deep learning loop detection algorithm is used to search for revisited submap matching pairs. After the search for submap matching pairs is completed, each point cloud submap is preprocessed.

[0022] First, based on principal component analysis, remove the non-planar points in the submap, effectively removing noise points and redundant points, and perform spatial downsampling while retaining the geometric structure features of the point cloud. Then, use the ICP method with a robust function to remove the gross errors in the submap matching to obtain the relative matching pose between submaps.

[0023] Even further, in step 4, when the overlap degree between the current submap and the next submap is greater than 0.5, add it to the set of adjacent submap matching pairs.

[0024] Based on the same inventive concept, this solution also designs a device for implementing a laser-inertial point cloud map construction method with joint multiple constraints, including:

[0025] A data acquisition module that acquires time-synchronized mobile laser point cloud data and inertial measurement data;

[0026] A data preprocessing module that uses the IMU to correct the motion distortion of a single-frame laser point cloud;

[0027] A front-end pose estimation module that tightly couples the laser point-to-plane observation and inertial measurement data based on the iterative error Kalman filter framework to estimate the carrier pose;

[0028] The back-end map construction module constructs a factor graph model with multiple matching constraints to optimize pose estimation. The point cloud sub-graphs are divided according to the time span, and a factor graph model including multiple sub-graph relative pose constraints and gravity constraints is constructed. The ICP method with a robust function is used to accurately calculate the relative pose between sub-graphs, and the sub-graph pose is optimized to obtain a high-precision point cloud map.

[0029] Based on the same inventive concept, the present invention also provides an electronic device, including:

[0030] one or more processors;

[0031] A storage device for storing one or more programs;

[0032] When one or more programs are executed by the one or more processors, the one or more processors implement the above-mentioned point cloud map construction method.

[0033] Based on the same inventive concept, the present invention also designs a computer-readable medium on which a computer program is stored. When the program is executed by a processor, the above-mentioned point cloud map construction method is implemented.

[0034] The advantages of the present invention are as follows: the present invention robustly estimates the carrier pose by tightly coupling laser point-to-surface observations and inertial measurement data based on an iterative error Kalman filter framework. The present invention divides the point cloud into sub-graphs, fully utilizes the observation information of the common view area in space, constructs a factor graph model including multiple sub-graph relative pose constraints and gravity constraints to optimize the pose, and effectively reduces the accumulated error in the Kalman filter stage. The present invention analyzes the spatial distribution of neighboring points, filters out non-planar points in the sub-map, and proposes an ICP algorithm with a robust function to accurately calculate the relative pose between sub-maps, obtains the accurate relative pose between sub-maps, and thus improves the accuracy of pose graph optimization. Through the above technical scheme, the present invention can realize a low-cost joint multi-constrained laser and inertial tightly coupled point cloud map construction method, which can accurately construct maps even in scenes with intense motion and large range. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is a flow chart of a method for constructing a laser inertial point cloud map with combined multiple constraints provided in an embodiment of the present invention.

[0036] Figure 2 It is a flow chart of a method for constructing a factor graph model with multiple matching constraints to optimize pose estimation provided in an embodiment of the present invention.

[0037] Figure 3 It is a schematic diagram of the elevation coloring of the point cloud map constructed according to an embodiment of the present invention. DETAILED DESCRIPTION

[0038] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments.

[0039] Embodiment 1

[0040] A method for constructing a laser inertial point cloud map with combined multiple constraints provided in this embodiment has a flowchart as shown in the appendix. Figure 1 The method specifically includes the following steps:

[0041] Step 1: Input laser point cloud and inertial measurement data. This step involves inputting the mobile laser point cloud data and inertial measurement data that are hardware time-synchronized and obtained by a lidar and an IMU sensor, including:

[0042] Step 1.1: Use the lidar to obtain a series of laser frame data. Each data point contains three-dimensional coordinates of x, y, and z and the corresponding measurement timestamp. The point cloud data can reflect the three-dimensional structure of the surrounding environment and, in chronological order, capture the environmental changes during the movement of the platform.

[0043] Step 1.2: Input the inertial data with timestamps collected by the IMU, including the linear acceleration data recorded by the three-axis acceleration sensor and the angular velocity data recorded by the three-axis angular velocity sensor.

[0044] Step 2: Use the IMU to correct the motion distortion of a single-frame laser point cloud, including:

[0045] Step 2.1: Since the lidar collects points one by one at a high frequency instead of all at once at the same moment. Therefore, it is necessary to correct the motion distortion of the laser points collected by the lidar. By pre-integrating the inertial measurement data, calculate the discrete inertial poses within a laser frame. For each laser point P r , select the adjacent discrete inertial poses for linear interpolation according to the timestamp to obtain its relative pose.

[0046] Step 2.2: According to the relative pose obtained in Step 2.1 Project the laser points onto the vehicle coordinate system at the end moment of the laser frame. In the present invention, it is defined that the vehicle coordinate system is consistent with the IMU coordinate system. The projection process is expressed as

[0047]

[0048] where represents the extrinsic parameter from the lidar to the inertial measurement unit, represents the laser point cloud after the motion distortion correction is completed.

[0049] Step 3: Estimate the vehicle pose by tightly coupling the laser point-to-plane observations and inertial measurement data based on the iterative error Kalman filter framework. It includes:

[0050] Step 3.1: Static initialize the state variables of the Kalman filter;

[0051] Step 3.2: Use the corresponding IMU measurement values in the current frame to predict the state of the Kalman filter;

[0052] Step 3.3: Project the motion-distortion-corrected point cloud into the map coordinate system, use spatial nearest neighbor search to find the 5 nearest points in the map and fit a plane with them, eliminate the matching pairs with large distances, and complete the point-to-plane feature association;

[0053] Step 3.4: Based on the results of the feature association, calculate the Jacobian matrix of the laser observation, and then perform the iterative error Kalman filter update to obtain the vehicle pose.

[0054] Step 4: Construct a factor graph model with multiple matching constraints to optimize the pose estimation. Divide the point cloud subgraph according to the time span, and by constructing a factor graph model including various relative pose constraints and gravity constraints between subgraphs, use the ICP method with a robust function to accurately calculate the relative pose between subgraphs, and optimize the subgraph poses to obtain a high-precision point cloud map. The method flow chart is as shown in the appendix Figure 2 as follows, including:

[0055] Step 4.1: According to the set time span, usually several seconds, project the sequence of laser frames within the time window into the coordinate system at the start time of the window based on the pose estimated in Step 3, and generate point cloud subgraphs and their initial poses. These point cloud subgraphs serve as the basic units for subsequent construction of constraints and matching;

[0056] Step 4.2: Search for and construct the matching constraints of adjacent subgraphs, co-visible subgraphs, and revisited subgraphs. When the overlap degree between the current subgraph and the next subgraph is greater than 0.5, add it to the adjacent subgraph matching pair set and establish the adjacent subgraph matching constraint; to improve the search efficiency of co-visible subgraphs, only search for candidates based on spatial overlap degree among the subgraphs adjacent to the current subgraph in space, and when the overlap degree is greater than 0.5, add it to the co-visible subgraph matching pair set; use the loop closure detection algorithm with adaptive pyramid feature aggregation to search for the matching pairs of revisited subgraphs;

[0057] Step 4.3: After completing the search for subgraph matching pairs, in order to obtain the accurate relative pose j between submap S z and submap S accurately calculate the relative pose between subgraphs using the ICP algorithm with a robust function. Specifically, it includes:

[0058] If it is to calculate the relative pose of the revisited subgraph, convert the N adjacent subgraphs into the coordinate system of the current submap to construct a group of submaps participating in the matching;

[0059] Based on the principal component analysis method, remove the non-planar points of the point cloud subgraph and perform spatial downsampling;

[0060] According to the initial subgraph pose value obtained in claim 5, construct a subgraph registration cost function:

[0061]

[0062] where p i represents the planar points in the submap S k , M i and n i represent the parameters of the matching plane in the submap S j . ρ() represents the Huber function. In addition, if the distance between p i and the nearest point in the submap S j exceeds the threshold, the matching will be rejected, and the distance threshold for rejecting points to the plane matching is automatically adjusted during the matching process to avoid incorrect matching caused by point cloud feature degradation or noise. Optimize this cost function through non-linear least squares to obtain the relative pose between subgraphs

[0063] Step 4.4: Based on the three types of subgraph matching constraints (adjacent, co-visible, and revisited constraints) and the gravity state constraint in step 3, construct a factor graph model:

[0064]

[0065] where α1, α2, α3, α4 are the weight coefficients of different constraints, corresponding to the adjacent subgraph constraint C, the co-visible subgraph constraint O, the revisited subgraph constraint L, and the gravity state g constraint respectively. and represent the poses of subgraphs i, j in the world coordinate system, while represents the relative pose observation value of adjacent subgraphs. and represent the poses of subgraphs m, n in the world coordinate system, while represents the relative pose observation value of co-visible subgraphs. and represent the poses of subgraphs p, q in the world coordinate system, while represents the relative pose observation value of revisited subgraphs. ρ() represents the Huber robust kernel function. g i is the gravity estimation, and g0 is the known standard gravity direction. Then, the LM algorithm optimizes the subgraph poses to obtain a high-precision point cloud map. The final point cloud map is colored according to elevation, and the effect diagram is shown in the appendixFigure 3 。

[0066] Embodiment 2

[0067] Based on the same inventive concept, this embodiment discloses a device for implementing the method for constructing a laser inertial point cloud map with combined multiple constraints described in Embodiment 1, including:

[0068] A data acquisition module that acquires time-synchronized mobile laser point cloud data and inertial measurement data;

[0069] A data preprocessing module that corrects the motion distortion of a single-frame laser point cloud using an IMU;

[0070] A front-end pose estimation module that tightly couples the laser point-to-plane observation and inertial measurement data based on an iterative error Kalman filter framework to estimate the pose of the carrier;

[0071] A back-end map construction module that constructs a factor graph model with multiple matching constraints to optimize the pose estimation. The point cloud subgraphs are divided according to the time span. By constructing a factor graph model including various relative pose constraints and gravity constraints between subgraphs, the ICP method with a robust function is used to accurately calculate the relative pose between subgraphs, and the subgraph poses are optimized to obtain a high-precision point cloud map.

[0072] Since the device introduced in Embodiment 2 of the present invention is the device used to implement the method for constructing a laser inertial point cloud map with combined multiple constraints in Embodiment 1 of the present invention, based on the method introduced in Embodiment 1 of the present invention, those skilled in the art can understand the specific structure and variations of this electronic device, so it will not be elaborated here.

[0073] Embodiment 3

[0074] Based on the same inventive concept, the present invention also provides an electronic device, including one or more processors; a storage device for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the method described in Embodiment 1.

[0075] Since the device introduced in Embodiment 3 of the present invention is the electronic device used to implement the method for constructing a laser inertial point cloud map with combined multiple constraints in Embodiment 1 of the present invention, based on the method introduced in Embodiment 1 of the present invention, those skilled in the art can understand the specific structure and variations of this electronic device, so it will not be elaborated here. Any electronic device used in the method of Embodiment 1 of the present invention falls within the scope of protection of the present invention.

[0076] Embodiment 4

[0077] Based on the same inventive concept, the present invention further provides a computer-readable medium, on which a computer program is stored, and when the program is executed by a processor, the method described in Embodiment 1 is implemented.

[0078] Since the device introduced in Embodiment 4 of the present invention is the computer-readable medium adopted for implementing the method for constructing a laser inertial point cloud map with combined multiple constraints in Embodiment 1 of the present invention, based on the method introduced in Embodiment 1 of the present invention, those skilled in the art can understand the specific structure and variations of the electronic device, and thus will not be elaborated herein. Any electronic device adopted by the method in Embodiment 1 of the present invention falls within the scope of protection of the present invention.

[0079] The above specific embodiments do not constitute a limitation to the protection scope of the present invention. For those skilled in the art to which the present invention pertains, any modification within the scope defined by the spirit and claims of the present invention shall fall within the protection scope of the present invention.

Claims

1. A laser inertial point cloud map construction method with multiple constraints, characterized in that: The following steps are involved: Step 1, acquiring time-synchronized mobile laser point cloud data and inertial measurement data; Step 2: Use IMU to correct motion distortion of single-frame laser point cloud; Step 3, based on the iterative error Kalman filter framework, the laser point-to-surface observation and inertial measurement data are tightly coupled to estimate the carrier pose; Step 4: Build a factor graph model with multiple matching constraints to optimize pose estimation. Divide the point cloud subgraphs according to the time span. Build a factor graph model including multiple subgraph relative pose constraints and gravity constraints. Use the ICP method with robust functions to accurately calculate the relative poses between subgraphs. Optimize the subgraph poses to obtain a high-precision point cloud map. The details are as follows: According to the set time span, the laser frames in the time window are projected to the coordinate system at the start time of the window according to the corresponding posture to obtain the point cloud sub-image; Search and construct three types of sub-image matching constraints: adjacent, common view and revisit, and use the ICP method with a robust function to obtain the relative pose between sub-images; Based on the above three types of sub-graph matching constraints, a factor graph model including multiple sub-graph relative pose constraints and gravity constraints is constructed, and then the Levenberg-Marquardt algorithm is used to optimize the sub-graph pose to obtain a high-precision point cloud map; A deep learning loop detection algorithm with adaptive pyramid feature aggregation is used to revisit sub-graph matching pair search. After the sub-graph matching pair search is completed, each point cloud sub-graph is pre-processed. Firstly, based on principal component analysis, non-planar points in sub-images are eliminated to effectively remove noise points and redundant points, and spatial downsampling is performed while retaining the geometric structure characteristics of the point cloud. Then, the ICP method with a robust function is used to eliminate gross errors in sub-image matching and obtain the relative matching poses between sub-images.

2. The method for constructing a laser inertial point cloud map with joint multiple constraints according to claim 1, characterized in that: The discrete inertial pose within a frame of laser is obtained by integrating the inertial measurement data. The pose of each laser point relative to the start time of the laser frame is obtained by linear interpolation of the discrete inertial poses adjacent to it in time. According to the relative pose, the laser point is projected to the carrier coordinate system at the end time of the laser frame to complete the motion distortion correction.

3. The method for constructing a laser inertial point cloud map with joint multiple constraints according to claim 1, characterized in that: The specific process of step 3 includes: Step 3.1: Statically initialize the state of the Kalman filter; Step 3.2: Use the corresponding IMU measurement value in the current frame to predict the Kalman filter state; Step 3.3: Project the point cloud after motion distortion correction to the map coordinate system, use the spatial nearest neighbor search to find the neighboring points in the map and use them to fit the plane, remove the matching pairs with large distances, and complete the feature association from point to surface; Step 3.4: Based on the results of feature association, the Jacobian matrix of the laser observation is calculated, and then the iterative error Kalman filter is updated to obtain the carrier pose.

4. The laser inertial point cloud map construction method with joint multiple constraints according to claim 1, characterized in that: The time synchronization is hardware time synchronization, and the timestamps of the laser point cloud data and the inertial measurement data are based on the same time reference, and the error is less than 1 ms.

5. The method for constructing a laser inertial point cloud map with joint multiple constraints according to claim 1, characterized in that: In step 4, when the overlap between the current subgraph and the next subgraph is greater than 0.5, the adjacent subgraph matching pair set is added.

6. A device for implementing the laser inertial point cloud map construction method with combined multiple constraints as described in any one of claims 1 to 5, characterized in that: Data acquisition module, which acquires time-synchronized mobile laser point cloud data and inertial measurement data; The data preprocessing module uses IMU to perform motion distortion correction on a single-frame laser point cloud; The front-end pose estimation module estimates the carrier pose by tightly coupling laser point-to-surface observation and inertial measurement data based on an iterative error Kalman filter framework; The back-end map construction module constructs a factor graph model with multiple matching constraints to optimize pose estimation, divides the point cloud subgraphs according to the time span, and constructs a factor graph model including multiple subgraph relative pose constraints and gravity constraints. The ICP method with the addition of a robust function is used to accurately calculate the relative pose between subgraphs, and the subgraph pose is optimized to obtain a high-precision point cloud map.

7. An electronic device, characterized in that: include: one or more processors; A storage device for storing one or more programs; When one or more programs are executed by the one or more processors, the one or more processors implement the point cloud map construction method as described in any one of claims 1-5.

8. A computer readable medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the point cloud map construction method as described in any one of claims 1 to 5 is implemented.

Images