Multi-constraint combined laser inertia point cloud map construction method and device

Abstract

The invention provides a laser inertia point cloud map construction method and device combined with multiple constraints. Through a data input module, mobile laser point cloud data and inertial measurement data which are obtained by a laser radar and an inertial measurement unit sensor and are subjected to hardware time synchronization are received. Performing motion distortion correction on the single-frame laser point cloud by using inertial measurement data; on the basis of an iterative error Kalman filtering framework, laser point-to-surface observation and inertial measurement data are tightly coupled, and a carrier pose is estimated; and finally, constructing a factor graph model comprising relative pose constraints and gravity constraints of a plurality of sub-graphs, accurately calculating relative poses among the sub-graphs by using an iterative nearest neighbor point algorithm added with a robust function, and optimizing the poses of the sub-graphs to obtain a high-precision point cloud map. The method is suitable for the fields of unmanned autonomous surveying and mapping, robot navigation, planetary detection and the like, and the precision and efficiency of point cloud mapping are remarkably improved through front-end single-frame pose estimation based on Kalman filtering and multi-type constraint back-end map pose optimization.

Description

Technical Field

[0001] The embodiments of the present invention relate to the field of autonomous surveying and mapping and planetary exploration technology, and in particular to a laser inertial point cloud map construction method and device combined with multiple constraints. Background Art

[0002] Traditional mobile laser scanning technology usually relies on GNSS / INS (Global Navigation Satellite System / Inertial Navigation System) combined navigation system and laser radar to obtain the position and laser observation information of the carrier platform. After that, through geo-calibration processing, a high-precision three-dimensional point cloud map can be generated. However, this method relies on the stability of GNSS signals, and inaccurate positioning will occur in environments where GNSS signals are blocked or missing (such as indoors, tunnels, forests, etc.). In recent years, with the rapid development of simultaneous positioning and mapping SLAM technology, mobile measurement platforms are no longer completely dependent on GNSS signals. SLAM technology can combine multiple sensors such as laser radar, camera, inertial measurement unit (IMU), etc., and can generate accurate three-dimensional maps by obtaining environmental information in real time even in the absence of GNSS signals. This technology has been widely used in unmanned autonomous mapping, robot navigation, planetary exploration and other fields, which not only improves the adaptability to complex environments, but also greatly improves the accuracy and speed of mapping the environment, and promotes technological progress in related industries.

[0003] SLAM methods can be divided into three categories according to the sensor type: laser SLAM, visual SLAM, and multi-sensor fusion. LiDAR sensors can directly capture high-precision environmental point clouds and are less affected by lighting than visual cameras. Inertial measurement units can be used to locate and remove motion distortion of laser point clouds under high dynamics by measuring the motion state of the carrier platform. Fusion of the advantages of LiDAR sensors and inertial measurement units can realize pose estimation and map construction of mobile measurement platforms. However, due to errors in matching between point cloud frames and sub-images, and between sub-images, pose estimation drifts and further causes geometric inconsistency in point cloud maps. Factor graph optimization methods indirectly optimize the geometric structure of maps by effectively minimizing the relative pose errors between graph nodes. However, existing optimization methods do not make full use of constraints such as common view to construct accurate factor graph models to optimize errors, and there is still a bottleneck of low fusion positioning and mapping accuracy. In complex scenarios, existing methods are difficult to meet the technical requirements of robustness and accuracy of point cloud mapping. Summary of the invention

[0004] In order to overcome the shortcomings of the prior art, the present invention provides a laser inertial point cloud map construction method with combined multiple constraints to generate high-precision point cloud maps, realize motion distortion correction of laser point clouds according to inertial measurement data, accurately calculate the relative poses between sub-maps by adding an ICP (Iterative Closest Points) point cloud matching method with a robust function, and use a special strategy to accurately construct three types of sub-map constraints for pose graph optimization, thereby improving the robustness and accuracy of point cloud mapping during movement.

[0005] The present invention provides a method for constructing a laser inertial point cloud map with multiple constraints, comprising the following steps:

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

[0007] Step 2: Use IMU to correct motion distortion of single-frame laser point cloud;

[0008] 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;

[0009] 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 pose between subgraphs, and optimize the subgraph 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. 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.

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

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

[0013] Step 3.2: Use the corresponding IMU measurement value in the current frame to predict the Kalman filter state;

[0014] 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 adjacent points in the map and use them to fit the plane, eliminate the matching pairs with a large distance, and complete the feature association from point to surface;

[0015] 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.

[0016] Furthermore, 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.

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

[0018] 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;

[0019] 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;

[0020] Based on the above three types of constraints, a factor graph model including multiple sub-graph relative pose constraints and gravity constraints is constructed, and then the LM (Levenberg-Marquardt) algorithm is used to optimize the sub-graph pose to obtain a high-precision point cloud map.

[0021] Furthermore, in step 4, a deep learning loop detection algorithm with adaptive pyramid feature aggregation is used to perform a revisited sub-graph matching pair search. After the sub-graph matching pair search is completed, each point cloud sub-graph is pre-processed;

[0022] 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.

[0023] More specifically, 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.

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

[0025] Data acquisition module, which acquires time-synchronized mobile laser point cloud data and inertial measurement data;

[0026] The data preprocessing module uses IMU to perform motion distortion correction on a single-frame laser point cloud;

[0027] 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;

[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 is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0039] Embodiment 1

[0040] This embodiment provides a method for constructing a laser inertial point cloud map with combined multiple constraints, and the flow chart is as shown in the attached figure. Figure 1 The method specifically comprises the following steps:

[0041] Step 1: Input laser point cloud and inertial measurement data. This step involves inputting mobile laser point cloud data and inertial measurement data acquired by the LiDAR and IMU sensors that have been synchronized with hardware time, including:

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

[0043] Step 1.2: Input IMU to collect inertial data with time stamp, including linear acceleration data recorded by the three-axis accelerometer and angular velocity data recorded by the three-axis angular velocity sensor.

[0044] Step 2: Use IMU to perform motion distortion correction on a single-frame laser point cloud, including:

[0045] Step 2.1: Since the laser radar collects points one by one at a high frequency, rather than all at once, it is necessary to perform motion distortion correction on the laser points collected by the laser radar. By pre-integrating the inertial measurement data, the discrete inertial pose within a frame of laser is calculated. For each laser point P r , select the adjacent discrete inertial pose according to the timestamp and perform linear interpolation to obtain its relative pose

[0046] Step 2.2: Relative pose obtained according to step 2.1 The laser point is projected to the carrier coordinate system at the end of the laser frame. The present invention defines the carrier coordinate system to be consistent with the IMU coordinate system. The projection process is expressed as

[0047]

[0048] in represents the external parameters from the lidar to the inertial measurement unit, Represents the laser point cloud after motion distortion correction.

[0049] Step 3: Estimate the carrier pose by tightly coupling laser point-to-surface observation and inertial measurement data based on the iterative error Kalman filter framework. Including:

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

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

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

[0053] 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.

[0054] Step 4: Construct a factor graph model with multiple matching constraints to optimize pose estimation. Divide the point cloud subgraphs according to the time span, construct 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 pose between subgraphs, and optimize the subgraph pose to obtain a high-precision point cloud map. The method flow chart is attached. Figure 2 As shown, including:

[0055] Step 4.1: According to the set time span, usually a few seconds, the sequential laser frames in the time window are projected to the coordinate system at the start time of the window according to the estimated pose in step 3, and the point cloud sub-image and its initial pose are generated. These point cloud sub-images serve as the basic units for subsequent construction of constraints and matching;

[0056] Step 4.2: Search and construct matching constraints for adjacent subgraphs, common-view subgraphs, and revisited subgraphs. When the overlap between the current subgraph and the next subgraph is greater than 0.5, add the adjacent subgraph matching pair set to establish adjacent subgraph matching constraints; to improve the search efficiency of common-view subgraphs, perform a search based on spatial overlap only in candidate subgraphs that are spatially adjacent to the current subgraph. When the overlap is greater than 0.5, add the common-view subgraph matching pair set; use the loop detection algorithm with adaptive pyramid feature aggregation to search for revisited subgraph matching pairs;

[0057] Step 4.3: After completing the search for sub-graph matching pairs, in order to obtain the sub-map S j and sub-map S z The exact relative position between The ICP algorithm with a robust function is used to accurately calculate the relative poses between sub-images. Specifically, it includes:

[0058] If the relative pose of the revisited submap is calculated, the N adjacent submaps are converted to the coordinate system of the current submap to construct a submap group participating in the matching;

[0059] Based on the principal component analysis method, non-planar points of the point cloud sub-image are removed and spatial downsampling is performed;

[0060] According to the initial value of the sub-image pose obtained in claim 5, the sub-image registration cost function is constructed:

[0061]

[0062] where p i Represents submap S k The plane point in M i and n i Represents submap S j The parameters of the matching plane in . ρ() represents the Huber function. In addition, if p i With submap S j If the distance between the nearest point in the image exceeds the threshold, the match will be rejected. During the matching process, the distance threshold between the rejected point and the plane is automatically adjusted to avoid incorrect matching due to point cloud feature degradation or noise. The relative pose between the sub-images is obtained by optimizing the cost function using nonlinear least squares.

[0063] Step 4.4: Based on the three types of subgraph matching constraints (adjacent, common view and revisit constraints) and the gravity state constraints in step 3, construct a factor graph model:

[0064]

[0065] Among them, α1, α2, α3, and α4 are weight coefficients of different constraints, corresponding to the adjacent subgraph constraint C, the common view subgraph constraint O, the revisited subgraph constraint L, and the gravity state g constraint, respectively. and represents the position of sub-image i, j in the world coordinate system, and Represents the relative pose observations of adjacent sub-graphs. and represents the position of sub-graph m,n in the world coordinate system, and Represents the relative pose observations of the common view subgraph. and represents the position of sub-graph p,q in the world coordinate system, and represents the relative pose observation of the revisited subgraph. ρ() represents the Huber robust kernel function. g i is the gravity estimation, and g0 is the known standard gravity direction. Afterwards, the LM algorithm optimizes the sub-image pose to obtain a high-precision point cloud map. The final point cloud map is colored according to the elevation. See the attached figure. Figure 3 .

[0066] Embodiment 2

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

[0068] Data acquisition module, which acquires time-synchronized mobile laser point cloud data and inertial measurement data;

[0069] The data preprocessing module uses IMU to perform motion distortion correction on a single-frame laser point cloud;

[0070] 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;

[0071] 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.

[0072] Since the device introduced in the second embodiment of the present invention is the device used to implement the laser inertial point cloud map construction method with combined multiple constraints in the first embodiment of the present invention, based on the method introduced in the first embodiment of the present invention, the technical personnel in this field can understand the specific structure and deformation of the electronic device, so it will not be repeated here.

[0073] Embodiment 3

[0074] Based on the same inventive concept, the present invention also provides an electronic device, comprising 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 method described in Example 1.

[0075] Since the device introduced in the third embodiment of the present invention is an electronic device used to implement the method for constructing a laser inertial point cloud map with multiple constraints in the first embodiment of the present invention, a person skilled in the art can understand the specific structure and deformation of the electronic device based on the method introduced in the first embodiment of the present invention, so it is not described here in detail. All electronic devices used in a method in the first embodiment of the present invention belong to 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 the first embodiment is implemented.

[0078] Since the device introduced in the fourth embodiment of the present invention is a computer-readable medium used to implement the method for constructing a laser inertial point cloud map with multiple constraints in the first embodiment of the present invention, a person skilled in the art can understand the specific structure and deformation of the electronic device based on the method introduced in the first embodiment of the present invention, so it is not described here in detail. All electronic devices used in a method in the first embodiment of the present invention belong to the scope of protection of the present invention.

[0079] The above specific implementations do not constitute a limitation on the protection scope of the present invention. For those skilled in the art to which the present invention belongs, any modification within the spirit of the present invention and the scope defined by the claims should be 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: Construct a factor graph model with multiple matching constraints to optimize pose estimation, divide the point cloud subgraphs according to the time span, and construct 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 pose between subgraphs, optimize the subgraph poses, and obtain a high-precision point cloud map.

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 method for constructing a laser inertial point cloud map 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 1ms.

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, relative pose constraints between sub-images are constructed to optimize the point cloud map. The specific process is 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 constraints, a factor graph model including multiple sub-graph relative pose constraints and gravity constraints is constructed, and then the LM (Levenberg-Marquardt) algorithm is used to optimize the sub-graph pose to obtain a high-precision point cloud map.

6. The method for constructing a laser inertial point cloud map with joint multiple constraints according to claim 5, characterized in that: In step 4, a deep learning loop detection algorithm with adaptive pyramid feature aggregation is used to perform a revisited 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.

7. The method for constructing a laser inertial point cloud map with joint multiple constraints according to claim 5, 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.

8. A device for implementing any of the laser inertial point cloud map construction methods of claims 1-7, 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.

9. 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-7.

10. 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 7 is implemented.

Images