RTK-fused laser inertial navigation odometer mapping method and system

Abstract

The embodiment of the invention discloses an RTK-fused laser inertial navigation odometer mapping method and system, and the method comprises the steps: carrying out the time synchronization and space calibration of a laser radar, an inertial measurement unit and a double-mushroom-head antenna RTK system, and achieving the coordinate system alignment through lever arm compensation; multi-source constraints are fused in pose map optimization, and point cloud ghosting is eliminated by associating poses of a current frame and multiple historical frames; designing a dynamic fusion strategy for RTK positioning data, detecting the height of an environmental obstacle by using a laser radar, dynamically adjusting an RTK constraint weight, adding constraint factors in stages, carrying out intensive accumulation in an initialization stage to realize rapid alignment of a global coordinate system, and adding a suppression accumulative error in a subsequent low frequency; and a GTSAM optimizer is adopted to execute pose map optimization, and the optimized key frame point cloud is converted to a global coordinate system to generate a down-sampling map, so that the map consistency, precision and real-time performance of long-distance mapping are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of tightly coupled mapping technology, and more particularly to a method and system for mapping laser inertial navigation odometers integrated with RTK. Background Art

[0002] In the field of autonomous driving, simultaneous localization and mapping (SLAM) technology is a core means of achieving environmental perception. The current mainstream solution relies on a combination of LiDAR and Inertial Measurement Unit (IMU) to build local maps. However, the cumulative error is significant over long distances, necessitating the introduction of a global navigation satellite system (such as RTK) to provide global positioning corrections. Existing fusion methods fall into two categories:

[0003] The loosely coupled approach uses the lidar odometry pose as the predicted value and the RTK position as the observed value, fusing them through a Kalman filter. This method is computationally efficient, but because it lacks deep integration with the underlying sensor data, positioning accuracy is susceptible to environmental interference.

[0004] Tightly coupled solutions more thoroughly integrate RTK, IMU, and LiDAR data, and simultaneously add IMU zero bias correction, laser odometry constraints, and RTK pose constraints to pose graph optimization. Some algorithms also introduce plane geometry constraints to improve accuracy. However, such solutions still have obvious flaws. For example, when a vehicle repeatedly passes through the same area, the map is prone to point cloud ghosting (i.e., ground Figure One If plane constraints are added to optimize the geometric structure, although consistency can be improved, the amount of calculation increases dramatically, making it difficult to meet the real-time mapping requirements of large-scale scenes. Summary of the Invention

[0005] To this end, the embodiment of the present invention provides a method and system for building a map using a laser inertial navigation odometer integrated with RTK to solve the problem of Figure One Poor consistency can easily lead to point cloud ghosting, which is a technical problem that requires extremely high computing power when building large-scale maps.

[0006] In order to achieve the above objectives, the embodiments of the present invention provide the following technical solutions:

[0007] According to a first aspect of an embodiment of the present invention, a method for mapping laser inertial navigation odometer integrated with RTK is provided, the method comprising:

[0008] Perform spatiotemporal calibration of the LiDAR, IMU, and dual mushroom head antenna RTK to achieve time synchronization and spatial coordinate alignment, and calculate the position of the IMU in the global coordinate system through arm compensation;

[0009] The target function of pose graph optimization is constructed, and the odometer constraint based on IMU pre-integration, the Scan-to-Map inter-frame pose constraint based on the laser radar point cloud after distortion correction, the loop closure detection constraint based on the historical key frame subgraph, and the consistency pose constraint between the current frame and multiple historical frames are sequentially added. Figure One The consistency pose constraint is realized by adding the relative pose relationship between the current frame and all historical frames in a sliding window.

[0010] The height of the environmental obstacle is detected by the laser radar, the openness of the scene is judged, and the covariance of the RTK position constraint is dynamically adjusted.

[0011] An RTK constraint factor is accumulated every first preset distance in the initialization stage, and the target function is added as a whole after driving a second preset distance to align the global coordinate system.

[0012] An RTK constraint factor is added every third preset distance in the mapping stage to suppress accumulated errors.

[0013] The GTSAM optimizer is used for pose graph optimization.

[0014] Based on the optimized key frame pose, the point cloud is converted to the global coordinate system, and the point cloud map is generated after statistical filtering and down-sampling.

[0015] Further, the time synchronization is realized by matching data with a timestamp difference less than a set threshold, and the threshold is not more than 0.01 seconds.

[0016] Further, the time synchronization and spatial coordinate alignment are realized, and further include:

[0017] The external parameter between the laser radar and the IMU is calculated by a hand-eye calibration algorithm.

[0018] The position of the main antenna of the double-mushroom head antenna in the IMU coordinate system is measured to complete the boom compensation.

[0019] Further, the ground Figure One The consistency pose constraint is realized by adding the relative pose relationship between the current frame and all historical frames in a sliding window.

[0020] Further, the loop closure detection constraint is generated by constructing a subgraph for historical key frames and matching the similarity of point clouds between the current frame and the subgraph.

[0021] Further, the covariance of the RTK position constraint is dynamically adjusted, including:

[0022] If the detected obstacle height exceeds the vehicle body height and the distance is less than a threshold, it is determined as a non-open scene, and the covariance of the RTK constraint is increased.

[0023] Otherwise, it is determined as an open scene, and the covariance of the RTK constraint is reduced.

[0024] Furthermore, the dual mushroom head antenna RTK outputs the vehicle body orientation and main antenna position, which are converted into the global pose of the IMU through arm compensation.

[0025] Furthermore, during the initialization phase, an RTK constraint factor is accumulated each time a first preset distance is moved, and after traveling a second preset distance, an objective function is added as a whole to align the global coordinate system, including:

[0026] The first preset distance is 0.5 meters, and the second preset distance is 15 meters.

[0027] Furthermore, during the mapping phase, an RTK constraint factor is added every third preset distance to suppress the cumulative error, including:

[0028] The third preset distance is 10 meters.

[0029] According to a second aspect of an embodiment of the present invention, a laser inertial navigation odometry mapping system integrated with RTK is provided, the system comprising:

[0030] The sensor spatiotemporal calibration module is used to perform spatiotemporal calibration of the lidar, IMU, and dual mushroom head antenna RTK, achieving time synchronization and spatial coordinate alignment, and calculating the position of the IMU in the global coordinate system through arm compensation;

[0031] Add constraint modules to construct the objective function of pose graph optimization, and add odometry constraints based on IMU pre-integration, Scan-to-Map inter-frame pose constraints based on distortion-corrected lidar point cloud, closed-loop detection constraints based on historical keyframe subgraphs, and the pose constraint between the current frame and multiple historical frames. Figure One Consistent posture constraints;

[0032] The dynamic fusion RTK positioning data module is used to perform the following steps:

[0033] Use LiDAR to detect the height of environmental obstacles, determine the scene openness, and dynamically adjust the covariance of RTK position constraints;

[0034] During the initialization phase, an RTK constraint factor is accumulated every time a first preset distance is traveled. After traveling a second preset distance, the objective function is added as a whole to align the global coordinate system.

[0035] During the mapping phase, an RTK constraint factor is added every third preset distance to suppress cumulative errors;

[0036] Pose graph optimization module, used to optimize the pose graph using the GTSAM optimizer;

[0037] The mapping module is used to convert the point cloud into the global coordinate system based on the optimized keyframe pose, and generate a point cloud map through statistical filtering and downsampling.

[0038] The embodiment of the present application has the following advantages:

[0039] The embodiment of the present application synchronizes time and calibrates space for a laser radar, an inertial measurement unit and a double-mushroom-head antenna RTK system, realizes coordinate system alignment through lever arm compensation, fuses multi-source constraints in pose graph optimization, including IMU pre-integration odometer constraints, inter-frame pose constraints after laser radar distortion correction, historical key frame closed-loop detection constraints and innovative consistency pose constraints, the latter eliminates point cloud ghosting by correlating the current frame and multiple historical frames of pose, designs a dynamic fusion strategy for RTK positioning data, dynamically adjusts the RTK constraint weight by using the laser radar to detect the height of the environmental obstacles, and adds the constraint factor in stages, densely accumulates to realize fast alignment of the global coordinate system in the initialization stage, and then adds low frequency to suppress cumulative error, uses a GTSAM optimizer to perform pose graph optimization, converts the optimized key frame point cloud to the global coordinate system to generate a down-sampled map, and significantly improves the consistency, accuracy and real-time performance of long-distance mapping. Figure One Figure One BRIEF DESCRIPTION OF DRAWINGS

[0040] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings needed to be used in the following embodiment or prior art description will be briefly introduced. Obviously, the drawings in the following description are only exemplary, and those skilled in the art can obtain other implementation drawings according to the provided drawings without creating any creative labor.

[0041] The structures, proportions, sizes, etc. shown in the specification are only used to cooperate with the content disclosed in the specification, to be understood and read by those skilled in the art, and do not define the limiting conditions for the implementation of the present application, so they do not have technical significance. Any modification of structure, change of proportion relationship or adjustment of size, without affecting the effect and purpose that the present application can produce, should still fall within the scope of the technical content disclosed by the present application.

[0042] Figure 1 A logic structure schematic diagram of a laser inertial odometer mapping system fused with RTK is provided for the embodiment of the present application;

[0043] Figure 2 A flowchart schematic diagram of a laser inertial odometer mapping method fused with RTK is provided for the embodiment of the present application;

[0044] Figure 3 A point cloud map construction schematic diagram in a laser inertial odometer mapping method fused with RTK is provided for the embodiment of the present application;

[0045] ​​Figure 4 A ground surface in a fusion RTK-based laser inertial navigation odometry mapping method provided by an embodiment of the present application Figure One Consistency constraint schematic diagram. DETAILED DESCRIPTION

[0046] The embodiments of the present application are described below by specific embodiments, and those skilled in the art can easily understand other advantages and effects of the present application from the content disclosed in the specification. Obviously, the described embodiments are part of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0047] The existing fusion technology includes a mapping scheme based on loose-coupled pose fusion and a sensor fusion scheme based on tight coupling: the former takes the pose provided by the laser radar odometry as the prediction model and the RTK position as the observation model, and adds it to the Kalman filter model; the latter closely links the RTK, inertial navigation and laser radar point cloud data, and corrects the acceleration zero offset and angular velocity zero offset of the IMU in the fusion process. Generally, pose graph optimization is adopted, and IMU odometry constraint, laser radar odometry constraint and RTK pose constraint are added to the objective function to optimize the pose of the key frame. In addition, some algorithms extract plane parameters for key frames and add plane constraints to the objective function.

[0048] However, the existing fusion technology has poor Figure One consistency—point cloud ghosting occurs at repeated trajectories; some algorithms add plane constraints to the objective function to enhance the Figure One consistency, which greatly increases the computational load and requires extremely high computing power for large-scale mapping.

[0049] In order to enhance the Figure One consistency and improve the mapping efficiency and accuracy, the above technical problems of poor Figure One consistency and easy occurrence of point cloud ghosting, and extremely high computing power requirement for large-scale mapping are solved.

[0050] SLAM (Simultaneous localization and mapping): while constructing a map in an unknown environment, an unmanned vehicle, unmanned aerial vehicle, robot, etc. localize itself.

[0051] Scan-to-Map: match the current laser radar point cloud with the map to calculate the pose of the current frame in the map.

[0052] BA (Bundle Adjustment) refers to an optimization algorithm that simultaneously adjusts multiple sensor poses and scene geometric features. This algorithm in the laser radar SLAM (Simultaneous Localization and Mapping) technology aims to optimize the pose of the sensor and the position of the geometric features in the map by minimizing the distance between the feature points and their matched edges or planes.

[0053] GTSAM is a C++ class library that uses factor graphs and Bayesian networks as a basis for computation paradigm, implementing smoothing and mapping in robotics and vision.

[0054] PGO (Pose Graph Optimization): Pose graph optimization is the basis of graph-based SLAM (Simultaneous Localization and Mapping) technology, which is used to estimate multiple unknown poses from noisy relative measurements.

[0055] Reference Figure 1 The embodiment of the application discloses a laser inertial navigation odometry mapping system fusing RTK, which comprises: a sensor space-time calibration module for calibrating the space-time of a laser radar, an IMU and a double mushroom head antenna RTK, realizing time synchronization and space coordinate alignment, and calculating the position of the IMU in a global coordinate system through a lever arm compensation 1; an added constraint module 2; a dynamic fusion RTK positioning data module 3; a pose graph optimization module 4; and a mapping module 5.

[0056] Corresponding to the above-mentioned laser inertial navigation odometry mapping system fusing RTK, the embodiment of the application further discloses a laser inertial navigation odometry mapping method fusing RTK. The laser inertial navigation odometry mapping method fusing RTK disclosed in the embodiment of the application is described in detail below in combination with the above-mentioned laser inertial navigation odometry mapping system fusing RTK.

[0057] Reference Figure 2 The application discloses a laser inertial navigation odometry mapping method fusing RTK, wherein in PGO, a geometric constraint is added, and the RTK positioning data is optimized to improve the mapping accuracy. Figure One

[0058] S001, sensor space-time calibration:

[0059] In order to accurately fuse sensor data, it is necessary to ensure the time synchronization and space synchronization between the laser radar and the inertial navigation and RTK.

[0060] In order to ensure time synchronization, after receiving the data, the data is stored in a queue, and when the timestamp difference of the data is less than a certain threshold, the data is determined as a group of data. ​

[0061] To ensure spatial synchronization, the recursive trajectories of the inertial navigation and the lidar are calculated respectively, the external parameters between the lidar and the inertial navigation are calculated through the principle of hand-eye calibration, and the position of the main antenna in the double-mushroom antenna in the IMU coordinate system is measured for the calculation of the lever arm compensation.

[0062] S002, reference Figure 3 and Figure 4 The IMU odometry constraint, the lidar odometry constraint, the loop closure constraint, and the ground consistency constraint are added in the objective function: Figure One

[0063] The IMU odometry constraint is obtained through the pre-integration of the IMU data, the lidar data is rectified through the IMU pose, the lidar pose is calculated through Scan-to-Map, and the pose constraint between the adjacent frames of the lidar is obtained;

[0064] The subgraph is constructed for the historical key frames, the loop closure is detected, and the pose constraint between the loop closure frames is constructed;

[0065] In order to ensure the ground Figure One consistency, that is, the point cloud of the object in the repeated area of the trajectory will not appear ghosting, the inter-frame pose constraint between the current frame and more historical frames is added.

[0066] S003, in the objective function, the RTK position constraint is efficiently added:

[0067] The RTK of the double-mushroom antenna outputs the orientation of the vehicle body and the position of the main antenna, and the position of the IMU in the global coordinate system can be converted through the lever arm compensation; in order to make up for the deficiency of the RTK state and the satellite number in judging the positioning accuracy, the distance between the vehicle body and the obstacle higher than the vehicle body is calculated by using the local point cloud map in the current coordinate system, whether it is an open field is judged, and the covariance of the RTK position constraint is set;

[0068] At the same time, in order to quickly complete the initialization, an RTK constraint factor is accumulated every 0.5 meters, and when the ego vehicle travels 15 meters, all the RTK constraint factors are added to the objective function for optimization, so that the current positioning is quickly converted from the local coordinate system to the global coordinate system;

[0069] After that, an RTK constraint factor is added every 10 meters for the cumulative error of the odometry.

[0070] S004, the pose graph is optimized by using the GTSAM optimizer.

[0071] S005, reference Figure 3 When the vehicle covers the entire working area, the key frame poses optimized are used to convert all the frame point clouds to the global coordinate system, the point cloud is statistically filtered and down-sampled, and the point cloud map is output.​

[0072] In order to enhance the consistency Figure One In short distance mapping, plane constraints can also be added to realize the synchronous optimization of pose and geometry, that is, bundle adjustment algorithm BA.

[0073] The application adds relative pose constraints between the current frame and multiple historical frames in the objective function, so that the consistency of the map is greatly enhanced. Figure One In order to reduce the influence of RTK low-quality positioning on mapping, the application uses laser radar obstacle detection to judge whether the scene is an open space, and dynamically adjusts the covariance of the RTK constraint.In order to efficiently use the RTK constraint factor, the application adjusts the frequency of adding the RTK constraint factor to the objective function at different stages. In the initialization stage, the RTK constraint factor is accumulated frequently, and when the driving distance of the ego vehicle reaches a threshold, the accumulated RTK constraint factor is added to the nonlinear optimization as a whole, so that the pose is quickly converted from the local coordinate system to the global coordinate system. After the initialization is completed, the frequency of adding the RTK factor is reduced, which can ensure the mapping accuracy and reduce the calculation frequency. The embodiment of the application realizes efficient, real-time and long-distance mapping, and the consistency of the map is good in the repeated area of the trajectory. Figure One Figure One

[0074] Although the application has been described in detail above with general description and specific embodiments, some modifications or improvements can be made on the basis of the application, which is obvious to those skilled in the art. Therefore, these modifications or improvements made on the basis of not deviating from the spirit of the application, all belong to the scope of protection required by the application.

Claims

1. A laser inertial navigation odometry mapping method integrated with RTK, characterized in that: The method comprises: Perform spatiotemporal calibration of the LiDAR, IMU, and dual mushroom head antenna RTK to achieve time synchronization and spatial coordinate alignment, and calculate the position of the IMU in the global coordinate system through arm compensation; Construct the objective function for pose graph optimization, sequentially adding odometry constraints based on IMU pre-integration, Scan-to-Map inter-frame pose constraints based on the distortion-corrected lidar point cloud, loop closure constraints based on historical keyframe subgraphs, and map consistency pose constraints between the current frame and multiple historical frames; Use LiDAR to detect the height of environmental obstacles, determine the scene openness, and dynamically adjust the covariance of RTK position constraints; During the initialization phase, an RTK constraint factor is accumulated every time a first preset distance is traveled. After traveling a second preset distance, the objective function is added as a whole to align the global coordinate system. During the mapping phase, an RTK constraint factor is added every third preset distance to suppress cumulative errors; Use GTSAM optimizer for pose graph optimization; Based on the optimized keyframe pose, the point cloud is converted to the global coordinate system, and a point cloud map is generated through statistical filtering and downsampling.

2. The method for mapping laser inertial navigation odometer integrated with RTK according to claim 1, wherein: The time synchronization is achieved by matching data whose timestamp difference is less than a set threshold, and the threshold does not exceed 0.01 seconds.

3. The method for mapping laser inertial navigation odometer integrated with RTK according to claim 2, wherein: Achieving time synchronization and spatial coordinate alignment also includes: Calculate the external parameters between the lidar and IMU through the hand-eye calibration algorithm; Measure the position of the main antenna of the double mushroom head antenna in the IMU coordinate system to complete the arm compensation.

4. The method for mapping laser inertial navigation odometer integrated with RTK according to claim 1, wherein: The map consistency pose constraint is implemented by adding the relative pose relationship between the current frame and all historical frames in the sliding window.

5. The method for mapping laser inertial navigation odometer integrated with RTK according to claim 4, characterized in that: The closed loop detection constraint is generated by constructing a subgraph for historical key frames and matching the similarity between the current frame and the subgraph point cloud.

6. The method for mapping laser inertial navigation odometer integrated with RTK according to claim 1, wherein: Dynamically adjust the covariance of RTK position constraints, including: If the obstacle is detected to be higher than the vehicle height and the distance is less than the threshold, it is considered a non-open scene and the covariance of the RTK constraint is increased; Otherwise, it is determined to be an open scene and the covariance of the RTK constraint is reduced.

7. The method for mapping laser inertial navigation odometer integrated with RTK according to claim 1, wherein: The dual mushroom head antenna RTK outputs the vehicle body orientation and main antenna position, which are converted into the global pose of the IMU through arm compensation.

8. The method for mapping laser inertial navigation odometer integrated with RTK according to claim 1, wherein: During the initialization phase, an RTK constraint factor is accumulated each time the first preset distance is traveled. After traveling the second preset distance, the objective function is added to align the global coordinate system, including: The first preset distance is 0.5 meters, and the second preset distance is 15 meters.

9. The method for mapping laser inertial navigation odometer integrated with RTK according to claim 8, characterized in that: During the mapping phase, an RTK constraint factor is added every third preset distance to suppress cumulative errors, including: The third preset distance is 10 meters.

10. A laser inertial navigation odometer mapping system integrated with RTK, characterized in that: The system comprises: The sensor spatiotemporal calibration module is used to perform spatiotemporal calibration of the lidar, IMU, and dual mushroom head antenna RTK, achieving time synchronization and spatial coordinate alignment, and calculating the position of the IMU in the global coordinate system through arm compensation; Add a constraint module to construct the objective function for pose graph optimization. In this order, add odometry constraints based on IMU pre-integration, Scan-to-Map inter-frame pose constraints based on the distortion-corrected lidar point cloud, loop closure constraints based on historical keyframe subgraphs, and map consistency pose constraints between the current frame and multiple historical frames. The dynamic fusion RTK positioning data module is used to perform the following steps: Use LiDAR to detect the height of environmental obstacles, determine the scene openness, and dynamically adjust the covariance of RTK position constraints; During the initialization phase, an RTK constraint factor is accumulated every time a first preset distance is traveled. After traveling a second preset distance, the objective function is added as a whole to align the global coordinate system. During the mapping phase, an RTK constraint factor is added every third preset distance to suppress cumulative errors; Pose graph optimization module, used to optimize the pose graph using the GTSAM optimizer; The mapping module is used to convert the point cloud into the global coordinate system based on the optimized keyframe pose, and generate a point cloud map through statistical filtering and downsampling.

Images