Laser and inertia fused point cloud map construction method and equipment

Abstract

The invention provides a laser and inertia fusion point cloud map construction method and equipment. Mobile laser point cloud data and inertial measurement data which are subjected to hardware time synchronization are obtained through a laser radar and an inertial measurement unit sensor; performing motion distortion correction on the laser point cloud in one frame according to inertial measurement data; projecting the laser observation point after distortion correction to a map coordinate system for feature association according to the position and attitude obtained by inertial measurement data integration, and calculating the position and attitude of a carrier by considering the laser positioning degradation condition; dividing the point cloud sub-graphs according to the time span, constructing the relative pose constraint between the sub-graphs, and optimizing the poses of the sub-graphs to obtain the high-precision point cloud map. The three-dimensional sparse sampling of the environment is directly obtained through the laser radar, the discrete inertia pose is corrected by using the continuous time model to obtain more accurate point cloud distortion correction, the carrier pose is updated by considering laser degradation, and the robustness and precision of point cloud mapping in the moving process are improved.

Description

Technical Field

[0001] The embodiments of the present invention relate to the fields of mobile measurement and robotics technology, and in particular to a method and equipment for constructing a point cloud map by integrating laser and inertial measurement. Background Art

[0002] Traditional mobile laser scanning technology uses a GNSS / INS (Global Navigation Satellite System / Inertial Navigation System) combined navigation system and laser radar to obtain the carrier platform's position and laser observation, and then obtains a high-precision point cloud map through geographic orientation. In recent years, simultaneous positioning and mapping (SLAM) technology has developed rapidly. This technology uses sensors such as laser radar to achieve fast and accurate three-dimensional mapping of the environment without relying on GNSS.

[0003] SLAM methods can be divided into three categories according to the sensor type: laser SLAM, visual SLAM, and multi-sensor fusion. By measuring the motion state of the carrier platform, the inertial measurement unit can be used for positioning and removing the motion distortion of the laser point cloud under high dynamic conditions. However, existing methods rarely consider the impact of the cumulative error of inertial data integration on the correction of point cloud motion distortion. In addition, in the tightly coupled framework, the impact of laser positioning degradation on pose fusion estimation is rarely considered, which greatly reduces the accuracy of fusion positioning and mapping. In complex scenarios, it is difficult for existing methods 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 point cloud map construction method and equipment that integrates laser and inertial sensing, so as to generate high-precision point cloud maps, use a continuous-time model to correct discrete inertial poses to obtain more accurate point cloud distortion correction, and consider laser degradation to update the carrier pose, so as to improve the robustness and accuracy of point cloud mapping during movement.

[0005] The present invention provides a method for constructing a point cloud map by integrating laser and inertial sensing, comprising the following steps:

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

[0007] Step 2: The discrete inertial pose within a laser frame is obtained by integrating the inertial measurement data. Each laser point is linearly interpolated according to the discrete inertial poses adjacent to it in time to obtain its pose relative to the start time of the laser frame. The laser point is projected to the carrier coordinate system at the end time of the laser frame according to the relative pose to complete the first motion distortion correction.

[0008] Match the distortion-corrected laser frame with the current map to obtain the laser matching pose;

[0009] Within the set time window, the matching spline trajectory model fitted by the carrier pose and the laser matching pose, and the integral spline trajectory model fitted by the carrier pose and the inertial pose of the current frame are respectively fitted to update the discrete inertial pose;

[0010] According to the updated discrete inertial pose, the laser point is subjected to a second motion distortion correction;

[0011] Step 3: According to the updated discrete inertial pose, project the distortion-corrected laser points to the map coordinate system, perform feature association, and obtain the updated carrier pose;

[0012] Step 4: Build a high-precision point cloud map based on the updated carrier pose.

[0013] Preferably, 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.

[0014] Preferably, in step 3, updating the Kalman filter taking into account laser positioning degradation specifically includes:

[0015] According to the inertial measurement data within the current laser frame time range, iterative error Kalman filter prediction is performed, and the laser observation point is projected into the map coordinate system according to the predicted posture;

[0016] The observation points are associated with the map plane features based on the spatial nearest neighbor search, and the matching pairs whose distance from the point to the plane is greater than the set threshold are eliminated;

[0017] The Jacobian matrix of laser observation is constructed according to the matching pairs, and the laser degradation is detected by a heuristic method. According to the results of laser degradation detection, the Jacobian matrix is ​​remapped and the iterative error Kalman filter is updated to obtain the carrier pose.

[0018] Preferably, in step 4, the point cloud sub-graphs are divided according to the time span, the pose constraints between the sub-graphs are constructed, and the poses of the sub-graphs are optimized to obtain a high-precision point cloud map.

[0019] Preferably, the specific process of step 4 is as follows:

[0020] According to the set time span, the laser frames in the time window are projected according to the corresponding postures to the coordinate system at the start time of the window to obtain a point cloud sub-image;

[0021] Search and construct two types of sub-graph matching constraints: adjacent and revisited, and use the ICP method to obtain the relative pose between sub-graphs;

[0022] Based on the above two types of constraints, a pose graph model is constructed, the pose obtained in claim 4 is used as the initial value for optimization, and then the LM algorithm is used to optimize the sub-graph pose to obtain a high-precision point cloud map.

[0023] Furthermore, in step 4, a deep learning loop detection algorithm based on the Transformer architecture is used to revisit the sub-image matching pair search. After the sub-image matching pair search is completed, each point cloud sub-image is pre-processed, and non-planar points in the sub-image are removed based on principal component analysis, and spatial downsampling is performed. A robust kernel function is used to eliminate gross errors in sub-image matching to obtain the relative matching pose between sub-images.

[0024] Furthermore, in step 4, when the overlap between the current sub-graph and the next sub-graph is greater than 0.5, the adjacent sub-graph matching pair set is added.

[0025] Based on the same inventive concept, the present invention also designs a device for realizing the point cloud map construction method of laser and inertial fusion, including:

[0026] A data acquisition module, including a laser radar and an inertial measurement unit, for acquiring time-synchronized mobile laser point cloud data and inertial measurement data;

[0027] The data processing module performs the following operations on the data collected by the data acquisition module through the onboard processor:

[0028] The discrete inertial pose within a laser frame is obtained by integrating the inertial measurement data. Each laser point is linearly interpolated according to the discrete inertial poses adjacent to it in time to obtain its pose relative to the start time of the laser frame. 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 first motion distortion correction.

[0029] Match the laser frame with the current map to obtain the laser matching pose;

[0030] In the set time window, two uniform B-spline trajectory models are fitted, including the matching spline fitted by the carrier pose and the laser matching pose, and the integral spline fitted by the carrier pose and the inertial pose of the current frame, respectively, to update the discrete inertial pose. The updating process is:

[0031]

[0032] Among them, Spline LidAR (t) represents the pose corresponding to the matching spline at time t. IMU (t) represents the position corresponding to the integral spline at time t, represents the discrete inertial pose;

[0033] According to the updated discrete inertial pose, the laser point is subjected to a second motion distortion correction;

[0034] According to the discrete inertial pose, the distortion-corrected laser observation points are projected into the map coordinate system, and feature association is performed to obtain the carrier pose;

[0035] Construct a high-precision point cloud map based on the updated carrier posture.

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

[0037] one or more processors;

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

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

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

[0041] The advantages of the present invention are as follows: the present invention improves the existing technical process of correcting motion distortion of laser point clouds based on inertial measurement data, and uses the laser matching pose and inertial integral pose in the sliding window to fit two B-spline trajectory models respectively, thereby updating the discrete inertial pose, thereby reducing the low-cost IMU integral error, thereby improving the accuracy of point cloud distortion correction. The present invention detects laser positioning degradation on the basis of the existing iterative Kalman filter fusion laser and inertial measurement framework, and realizes the Kalman filter update of laser degradation perception, thereby improving the robustness and accuracy of point cloud mapping. The present invention divides the point cloud subgraphs and constructs subgraph constraints including adjacent and revisit types, thereby effectively reducing the accumulated errors in the Kalman filter stage. Through the above technical solutions, the present invention can realize a low-cost robust high-precision point cloud map construction that integrates laser radar and inertial measurement unit, and can accurately construct maps even in the case of violent motion and temporary degradation of laser radar. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 It is a flow chart of a method for constructing a point cloud map by integrating laser and inertial sensing provided in an embodiment of the present invention;

[0043] Figure 2 is a flow chart of a laser point cloud motion distortion correction method provided in an embodiment of the present invention;

[0044] Figure 3 is a schematic diagram of elevation coloring of a point cloud map constructed according to an embodiment of the present invention;

[0045] Figure 4 It is a structural schematic diagram of a point cloud map construction equipment integrating laser and inertial sensing provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0046] The present invention is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0047] Embodiment 1

[0048] This embodiment provides a method for constructing a point cloud by fusion of laser and inertial. The flowchart is as shown in the attached figure. Figure 1 The method specifically comprises the following steps:

[0049] Step 1: Obtain mobile laser point cloud data and inertial measurement data that have been synchronized with hardware time through the LiDAR and Inertial Measurement Unit (IMU) sensors, including:

[0050] Step 1.1: Obtain sequential laser frame data with x, y, z coordinates and measurement time through the laser radar;

[0051] Step 1.2: Obtain IMU data with time stamp through the inertial measurement unit, including three-axis acceleration data and three-axis angular velocity data.

[0052] Step 2: Figure 2 It is a flow chart of a method for correcting motion distortion of a laser point cloud within a frame according to inertial measurement data, including:

[0053] Step 2.1: When the frame number k is less than 5, only step 2.1 is executed to enter step 3. When the frame number k is greater than or equal to 5, the carrier poses with frame numbers k-1, k-2, k-3, and k-4 are searched from the results for processing in step 2.3. The discrete inertial poses within a laser frame are obtained based on the IMU integration. Each laser point P raw According to the timestamp, select the adjacent discrete inertial pose and linearly interpolate to get its pose relative to the start time of the laser frame, and calculate the pose relative to the end time of the laser frame. And according to the relative position, the laser point is projected to the carrier coordinate system at the end of the laser frame (the carrier coordinate system is defined in the present invention to be consistent with the IMU coordinate system). The projection process is expressed as

[0054]

[0055] in Represents the external parameters from the LiDAR to the inertial measurement unit. This step completes the first motion distortion correction;

[0056] Step 2.2: Use the iterative nearest neighbor algorithm to match the distortion-corrected laser frame with the current map and the inertial pose at the end of the frame. As the initial value of the matching, the laser matching pose is obtained Inertial posture from The interpolated value is obtained;

[0057] Step 2.3: Within the set time window, fit two uniform B-spline trajectory models, called matching spline and integral spline, respectively. The fitting data of the matching spline is the carrier pose and laser matching pose of the first 4 frames. That is, the frame numbers are k-1, k-2, k-3, k-4; the fitting data of the integral spline is the carrier pose of the previous 4 frames and the discrete inertial pose of the current frame The discrete inertial pose is updated accordingly, and the updating process is expressed as

[0058]

[0059] Among them, Spline LiDAR (t) represents the pose corresponding to the matching spline at time t. IMU (t) represents the position and posture corresponding to the integral spline at time t;

[0060] Step 2.4: According to the updated discrete inertial pose, perform a second motion distortion correction on the laser point.

[0061] Step 3: In the present invention, the map coordinate system is defined to be consistent with the carrier coordinate system at the initial moment. According to the discrete inertial posture in step 2, the distortion-corrected laser observation points are projected to the map coordinate system for feature association, and the Kalman filter is updated considering the laser positioning degradation to obtain the carrier position and posture, including:

[0062] Step 3.1: Perform iterative error Kalman filter prediction based on the inertial measurement data within the current laser frame time range, and project the laser observation point to the map coordinate system according to the predicted pose;

[0063] Step 3.2: Associate the observation points with the map plane features based on the spatial nearest neighbor search, and eliminate the matching pairs whose point-to-plane distance is greater than the set threshold to obtain the screened matching pairs. represents the laser observation projected to the map coordinate system, and {n,d} represents the plane equation parameters;

[0064] Step 3.3: Construct the Jacobian matrix J of point-to-plane observation based on the matching pairs, and use a heuristic method to detect whether the laser positioning is degraded. Construct H = J T J performs eigenvalue decomposition, and if its minimum eigenvalue is less than the set threshold μD , then it is determined that degradation occurs;

[0065] Step 3.4: If laser positioning degradation occurs, perform singular value decomposition on the Jacobian matrix, expressed as J = USV T If any singular value in the diagonal matrix S is less than It is set to a very small value. In this embodiment, the value is set to 0.00001. J is updated accordingly to complete the remapping of the Jacobian matrix, and the iterative error Kalman filter is updated to obtain the updated carrier posture.

[0066] Step 4: Divide the point cloud sub-graphs according to the time span, construct relative pose constraints between sub-graphs, and optimize the sub-graph poses to obtain a high-precision point cloud map, including:

[0067] Step 4.1: 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 updated carrier posture to obtain the point cloud sub-image. The time span is usually set to a few seconds;

[0068] Step 4.2: Search and construct two types of sub-graph matching constraints: adjacent and revisited. When the overlap between the current sub-graph and the next sub-graph is greater than 0.5, add the adjacent sub-graph matching pair set. A deep learning loop detection algorithm based on the Transformer architecture is used to search for revisited sub-graph matching pairs. After the sub-graph matching pair search is completed, each point cloud sub-graph is pre-processed, non-planar points in the sub-graph are removed based on principal component analysis, and spatial downsampling is performed. A robust kernel function is used to remove gross errors in sub-graph matching to obtain the relative matching pose between sub-graphs;

[0069] Step 4.3: Based on the above three types of constraints, construct a pose graph model, use the pose obtained in step 3 as the initial value for optimization, and use the LM algorithm to optimize the sub-graph pose to obtain a high-precision point cloud map. The final point cloud map is colored according to the elevation. Figure 3 .

[0070] Figure 4 : is a schematic diagram of the structure of a point cloud map construction equipment for laser and inertial fusion provided in an embodiment of the present invention, which includes:

[0071] The data acquisition module is used to obtain the laser point cloud data and inertial measurement data of the carrier platform that have undergone hardware time synchronization at the current moment through the laser radar and the inertial measurement unit. The control unit receives the time signal sent by the inertial measurement unit and records its reception time; the control unit sends a pulse signal of a fixed frequency to the laser radar and records the time when the pulse is sent, thereby completing the hardware time synchronization of the laser point cloud data and the inertial measurement data.

[0072] The data processing module is used to receive the above data and execute the method of the present invention through the onboard processor to obtain a high-precision point cloud map.

[0073] Embodiment 2

[0074] Based on the same inventive concept, this embodiment discloses a device for implementing the method for constructing a point cloud map by integrating laser and inertial sensing as described in the first embodiment, including:

[0075] A data acquisition module, including a laser radar and an inertial measurement unit, for acquiring time-synchronized mobile laser point cloud data and inertial measurement data;

[0076] The data processing module performs the following operations on the data collected by the data acquisition module through the onboard processor:

[0077] The discrete inertial pose within a laser frame is obtained by integrating the inertial measurement data. Each laser point is linearly interpolated according to the discrete inertial poses adjacent to it in time to obtain its pose relative to the start time of the laser frame. 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 first motion distortion correction.

[0078] Match the laser frame with the current map to obtain the laser matching pose;

[0079] In the set time window, two uniform B-spline trajectory models are fitted, including the matching spline fitted by the carrier pose and the laser matching pose, and the integral spline fitted by the carrier pose and the inertial pose of the current frame, respectively, to update the discrete inertial pose. The updating process is:

[0080]

[0081] Among them, Spline LiDAR (t) represents the pose corresponding to the matching spline at time t. IMU (t) represents the position corresponding to the integral spline at time t, represents the discrete inertial pose;

[0082] According to the updated discrete inertial pose, the laser point is subjected to a second motion distortion correction;

[0083] According to the discrete inertial pose, the distortion-corrected laser observation points are projected to the map coordinate system, and feature association is performed to obtain the carrier pose;

[0084] Construct a high-precision point cloud map based on the updated carrier posture.

[0085] Embodiment 3

[0086] 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 one or more programs are executed by the one or more processors, the one or more processors implement the method described in Example 1.

[0087] Since the device introduced in the third embodiment of the present invention is an electronic device used to implement the method for constructing a point cloud map by laser and inertial fusion 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 will not be described in detail here. 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.

[0088] Embodiment 4

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

[0090] 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 point cloud map by laser and inertial fusion 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 embodiment of the present invention belong to the scope of protection of the present invention.

[0091] 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 method for constructing a point cloud map by integrating laser and inertial sensing, characterized in that: The following steps are involved: Step 1, acquiring time-synchronized mobile laser point cloud data and inertial measurement data; Step 2: The discrete inertial pose within a laser frame is obtained by integrating the inertial measurement data. Each laser point is linearly interpolated according to the discrete inertial poses adjacent to it in time to obtain its pose relative to the start time of the laser frame. The laser point is projected to the carrier coordinate system at the end time of the laser frame according to the relative pose to complete the first motion distortion correction. Match the distortion-corrected laser frame with the current map to obtain the laser matching pose; Within the set time window, the matching spline trajectory model fitted by the carrier pose and the laser matching pose, and the integral spline trajectory model fitted by the carrier pose and the inertial pose of the current frame are respectively fitted to update the discrete inertial pose; According to the updated discrete inertial pose, the laser point is subjected to a second motion distortion correction; Step 3: According to the updated discrete inertial pose, project the distortion-corrected laser points to the map coordinate system, perform feature association, and obtain the updated carrier pose; Step 4: Build a high-precision point cloud map based on the updated carrier pose.

2. The method for constructing a point cloud map by fusion of laser and inertial 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 under the same time reference.

3. The method for constructing a point cloud map by fusion of laser and inertial according to claim 1, characterized in that: In step 3, the Kalman filter is updated considering laser positioning degradation, which specifically includes: According to the inertial measurement data within the current laser frame time range, iterative error Kalman filter prediction is performed, and the laser observation point is projected into the map coordinate system according to the predicted posture; The observation points are associated with the map plane features based on the spatial nearest neighbor search, and the matching pairs whose distance from the point to the plane is greater than the set threshold are eliminated; The Jacobian matrix of laser observation is constructed according to the matching pairs, and the laser degradation is detected by a heuristic method. According to the results of laser degradation detection, the Jacobian matrix is ​​remapped and the iterative error Kalman filter is updated to obtain the carrier pose.

4. The method for constructing a point cloud map by fusion of laser and inertial according to claim 1, characterized in that: In step 4, the point cloud sub-graphs are divided according to the time span, the pose constraints between the sub-graphs are constructed, and the poses of the sub-graphs are optimized to obtain a high-precision point cloud map.

5. The method for constructing a point cloud map by fusion of laser and inertial according to claim 4, characterized in that: The specific process of step 4 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 updated carrier posture to obtain a point cloud sub-image; Search and construct two types of sub-graph matching constraints: adjacent and revisited, and use the ICP method to obtain the relative pose between sub-graphs; Based on the above two types of constraints, a pose graph model is constructed, and the carrier pose updated in step 3 is used as the optimization initial value. The LM algorithm is then used to optimize the sub-graph pose to obtain a high-precision point cloud map.

6. The method for constructing a point cloud map by fusion of laser and inertial according to claim 5, characterized in that: In step 4, a deep learning loop detection algorithm based on the Transformer architecture is used to revisit the sub-image matching pair search. After the sub-image matching pair search is completed, each point cloud sub-image is pre-processed, and non-planar points in the sub-image are removed based on principal component analysis, and spatial downsampling is performed. A robust kernel function is used to eliminate gross errors in sub-image matching to obtain the relative matching pose between sub-images.

7. The method for constructing a point cloud map by fusion of laser and inertial 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. An equipment for implementing the method for constructing a point cloud map by integrating laser and inertial sensing as described in any one of claims 1 to 7, characterized in that: A data acquisition module, including a laser radar and an inertial measurement unit, for acquiring time-synchronized mobile laser point cloud data and inertial measurement data; The data processing module performs the following operations on the data collected by the data acquisition module through the onboard processor: The discrete inertial pose within a laser frame is obtained by integrating the inertial measurement data. Each laser point is linearly interpolated according to the discrete inertial poses adjacent to it in time to obtain its pose relative to the start time of the laser frame. 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 first motion distortion correction. Match the laser frame with the current map to obtain the laser matching pose; Within the set time window, two uniform B-spline trajectory models are fitted respectively, including a matching spline fitted by the carrier pose and the laser matching pose, and an integral spline fitted by the carrier pose and the inertial pose of the current frame, to update the discrete inertial pose; According to the updated discrete inertial pose, the laser point is subjected to a second motion distortion correction; According to the discrete inertial pose, the distortion-corrected laser observation points are projected to the map coordinate system, and feature association is performed to obtain the carrier pose; Construct a high-precision point cloud map based on the updated carrier posture.

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