SLAM drift diagnosis and compensation method using IMU angle information

Abstract

The present invention provides a SLAM drift diagnosis and compensation method using IMU angle information, and relates to the technical field of SLAM. The method uses sparse point cloud feature point registration to estimate posture; determines sparse point cloud key frames and their estimated posture; when the IMU is at zero speed or uniform speed, analyzes the pitch angle of the current key frame calculated by IMU data, the pitch angle of the registered current key frame, the pitch angle of the current key frame relative to the hth historical key frame before the current key frame, and the pitch angle of the current key frame calculated by IMU data to determine whether drift occurs; when drifting, the xy coordinates and heading angle of the current key frame are kept unchanged, and the roll angle, pitch angle of the current key frame calculated by IMU data and the z coordinate of the hth historical key frame before the current key frame are assigned to the current key frame; when there is no drift, the cost function of G2O graph optimization is constructed using the posture error and the drift error, and the optimization iteration is performed using G2O graph optimization.

Description

Technical Field

[0001] The present invention relates to the field of laser radar SLAM technology, and in particular to a SLAM drift diagnosis and compensation method using IMU angle information. Background Art

[0002] With the continuous development of autonomous driving technology, simultaneous localization and mapping (SLAM) technology, as one of the key technologies of autonomous driving, has also received increasing attention. At present, the mainstream SLAM systems are mainly divided into two types: lidar SLAM and visual SLAM. Among them, lidar SLAM is widely used in the field of autonomous driving because it is not affected by light, directly obtains spatial position, has high accuracy, and good real-time performance. However, due to error accumulation, environmental degradation, and low vertical resolution of lidar, z-axis drift is prone to occur without loop correction, resulting in excessive errors in lidar SLAM mapping, affecting subsequent positioning and navigation. Lego-LOAM reduces the amount of calculation during matching and improves the registration efficiency by segmenting the ground point cloud and clustering to extract features. LIO-SAM is a tightly coupled algorithm based on IMU and LiDAR. It significantly improves the accuracy and real-time performance of the SLAM algorithm through IMU pre-integration and factor graph optimization. It also adds loop factors to improve the robustness of the system. Fast-LIO2 achieves high computational efficiency and high robustness by directly registering the original points to the map and maintaining the mapping with incremental kd trees. According to the above methods, it is not difficult to see that the common ideas for the drift problem of SLAM are adding ground constraints, fusing GNSS / IMU information, loop detection, etc. These methods can alleviate the drift problem in normal environments to a certain extent, but in environments such as mines, there is a lack of GNSS information and loop detection conditions, and the fusion of ground constraints and IMU will still accumulate errors in long-term operation and eventually lead to drift. Summary of the invention

[0003] In order to solve the above technical problem or at least partially solve the above technical problem, the present invention provides a method for SLAM drift diagnosis and compensation using IMU angle information.

[0004] In a first aspect, the present invention provides a method for SLAM drift diagnosis and compensation using IMU angle information, comprising:

[0005] The timestamps of the original LiDAR point cloud data and the IMU data are aligned. The original point cloud data is motion compensated using the IMU data to obtain the motion compensated point cloud data. The motion compensated point cloud data is point cloud filtered to obtain the sparse point cloud.

[0006] Extract features from sparse point clouds and estimate their poses using feature point registration; select and store sparse point cloud keyframes and their estimated poses;

[0007] When the IMU is at zero speed or constant speed, the roll angle and pitch angle at the corresponding time of the current key frame are calculated using the IMU data;

[0008] Analyze the pitch angle of the current key frame at the time calculated by IMU data and the pitch angle of the current key frame at the time obtained by registration, and the difference between the pitch angle of the hth historical key frame before the current key frame and the pitch angle of the current key frame calculated by IMU data to determine whether SLAM drift occurs;

[0009] When it is determined that SLAM drift occurs, the x, y coordinates and heading angle of the current key frame remain unchanged, the roll angle and pitch angle of the current key frame calculated by the IMU data are assigned to the current key frame, and the z coordinate of the hth historical key frame before the current key frame is assigned to the current key frame;

[0010] When it is determined that SLAM has not drifted or has adjusted its posture after drifting, the posture error of the current key frame and the drift error controlled by the drift flag are used to construct the cost function of G2O graph optimization, and the G2O graph optimization is used for optimization iteration.

[0011] Furthermore, the IMU data within a set time range near the timestamp of the original point cloud frame is found, and the IMU data near the timestamp of the original point cloud frame is weightedly fused with the timestamp of the corresponding original point cloud frame to achieve the alignment of the timestamps of the lidar original point cloud data and the IMU data.

[0012] Furthermore, the feature extraction of the sparse point cloud and the pose estimation of the sparse point cloud using feature point registration include:

[0013] Calculate the curvature of each point in the sparse point cloud, set the curvature threshold, and determine whether the actual curvature of the point is greater than the curvature threshold. If so, it is regarded as a corner point, otherwise it is regarded as a surface point. The curvature calculation formula is:

[0014] ;

[0015] Among them, S is the point to be analyzed in the same sparse point cloud frame A set of consecutively arranged points, is the point in the kth sparse point cloud frame in the set S and Point The coordinates of

[0016] After obtaining the feature points of each sparse point cloud frame: corner points and surface points, a KD tree is created to store the point cloud data;

[0017] Create a local map based on the sparse point cloud frame and add feature points to the local map;

[0018] When a sparse point cloud frame is collected, the KD tree is used to complete the nearest neighbor search of the feature points to achieve a rough match with the local map; the ICP algorithm is used for fine alignment, and the iteration is stopped until convergence or the number of iterations is reached to obtain the estimated pose of the current sparse point cloud frame.

[0019] Furthermore, selecting and storing sparse point cloud keyframes and their estimated poses includes:

[0020] The first sparse point cloud frame for creating a local map is selected as a key frame;

[0021] When the distance or rotation angle between the new sparse point cloud frame and the previous key frame is greater than the corresponding distance threshold or rotation angle threshold, the new sparse point cloud frame is selected as the key frame and the pose is obtained.

[0022] Furthermore, only when the IMU itself maintains zero speed or a constant speed state can it provide relatively accurate posture information for SLAM drift diagnosis and compensation; the process of analyzing whether the IMU is at zero speed or a constant speed includes:

[0023] The average acceleration modulus and average angular velocity modulus are calculated as follows:

[0024] ,

[0025] ,

[0026] in, and are the average acceleration modulus and average angular velocity modulus of N consecutive groups of IMU data before and at time t; , , They are the first position of IMU on the X, Y, and Z axes. Group acceleration, , , It is the acceleration zero bias of IMU on the X, Y, and Z axes; , , They are the first position of IMU on the X, Y, and Z axes. Group angular velocity, , , It is the angular velocity zero bias of IMU on the X, Y, and Z axes;

[0027] Setting the acceleration threshold and angular velocity threshold , when the following conditions are met at the same time, the IMU is considered to be in zero speed or uniform speed state:

[0028] ,

[0029] Where g is the acceleration due to gravity.

[0030] Furthermore, the roll angle of the current key frame is calculated using the following formula: and pitch angle :

[0031] ,

[0032] ,

[0033] in They are the average accelerations of the current key frame timestamp and the N consecutive groups of IMU data before it on the X, Y, and Z axes, respectively.

[0034] Furthermore, the pitch angle of the current key frame calculated by the IMU data and the pitch angle of the current key frame obtained by the registration are analyzed, and the difference between the pitch angle of the hth historical key frame before the current key frame and the pitch angle of the current key frame calculated by the IMU data is used to determine whether SLAM drift occurs, including:

[0035] Set the drift determination conditions as follows:

[0036] ,

[0037] in, is the registered pitch angle of the current keyframe, The pitch angle of the current keyframe calculated from the IMU data, is the spatial coordinate of the current keyframe, is the spatial coordinate of the hth historical key frame before the current key frame, , are a first drift judgment threshold and a second drift judgment threshold;

[0038] When any of the above drift determination conditions is met, SLAM drift is determined.

[0039] Furthermore, the pose error and drift error of the current key frame are used to construct the cost function of G2O graph optimization. The optimization iteration using G2O graph optimization includes:

[0040] Constructing a G2O graph optimized graph, wherein the graph sets the poses of the lidar key frames as nodes and adds the relative pose transformation between two frames as edges;

[0041] Then construct a cost function to optimize the error of the entire graph. The cost function is:

[0042] ,

[0043] in, Represents the pose error term of the kth key frame:

[0044] ,in, is the pose error weight, is the registration pose of the kth key frame, represents the transformation matrix from the jth to the kth key frame, is the registration pose of the jth key frame, where j is the previous one of k;

[0045] Represents the drift error term of the kth key frame:

[0046] ,

[0047] in, It is a drift sign. When drift occurs, Set to 0; when no drift occurs, Set to 1, is the weight, is the registration pitch angle of the current keyframe, is the pitch angle obtained by the spatial coordinates of the current key frame and the previous h-th key frame, The current keyframe pitch angle calculated from the IMU data;

[0048] Finally, the G2O graph optimization framework is used to optimize the entire graph iteratively until Converge to the threshold and finally realize SLAM error compensation.

[0049] In a second aspect, the present invention provides a device for diagnosing and compensating SLAM drift using IMU angle information, comprising: at least one processing unit, the processing unit, a storage unit, an IMU unit and a radar unit are interconnected through a bus unit, the storage unit stores a computer program and data collected by the IMU unit and the radar unit, and when the computer program is executed by the processing unit, the method for diagnosing and compensating SLAM drift using IMU angle information is implemented.

[0050] In a third aspect, the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method for SLAM drift diagnosis and compensation using IMU angle information is implemented.

[0051] The above technical solution provided by the embodiment of the present invention has the following advantages compared with the prior art:

[0052] The present application proposes a method for SLAM drift diagnosis and compensation using IMU angle information, which selects and stores sparse point cloud key frames and their estimated poses; when the IMU is at zero speed or uniform speed, the IMU data is used to calculate the roll angle and pitch angle of the current key frame; the pitch angle of the current key frame calculated by the IMU data and the pitch angle of the current key frame obtained by alignment are analyzed, and the difference between the pitch angle of the current key frame relative to the hth historical key frame before the current key frame and the pitch angle of the current key frame calculated by the IMU data is used to determine whether SLAM drift has occurred; when it is determined that SLAM drift has occurred, the x, y coordinates and heading angle of the current key frame are kept unchanged, the roll angle and pitch angle of the current key frame calculated by the IMU data are assigned to the current key frame, and the z coordinate of the hth historical key frame before the current key frame is assigned to the current key frame; when it is determined that SLAM drift has not occurred, the pose error and drift error of the current key frame are used to construct the cost function of G2O graph optimization, and the G2O graph optimization is used for optimization iteration. The constraints on the three state quantities of the current key frame pitch angle, roll angle and z coordinate are strengthened, which can significantly determine and alleviate the drift problem of SLAM and improve the positioning and mapping accuracy in complex environments. At the same time, the present invention has no special requirements for hardware equipment, does not require the participation of satellite signals, has a wide range of applications, and can cope with a variety of complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0054] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0055] Figure 1 A flowchart of a method for SLAM drift diagnosis and compensation using IMU angle information provided by an embodiment of the present invention;

[0056] Figure 2 A flow chart of acquiring and registering sparse point cloud feature points provided by an embodiment of the present invention;

[0057] Figure 3 A flowchart of using the pose error and drift error of the current key frame to construct a cost function for G2O graph optimization and performing optimization iteration using G2O graph optimization provided by an embodiment of the present invention;

[0058] Figure 4A schematic diagram of a device for SLAM drift diagnosis and compensation using IMU angle information provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0059] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0060] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0061] Example 1

[0062] like Figure 1 As shown, the technology of the present invention realizes a method for diagnosing and compensating SLAM drift using IMU angle information, and diagnoses and compensates the Z-axis drift of the LiDAR SLAM by utilizing the difference between the IMU angle and the key frame pose when the LiDAR SLAM drifts.

[0063] The present invention comprises the following steps:

[0064] Step 1: Align the timestamps of the original LiDAR point cloud data and IMU data; use the IMU data to perform motion compensation on the original point cloud data to obtain the motion compensated point cloud data, and perform point cloud filtering on the motion compensated point cloud data to obtain a sparse point cloud. Since the output frequency of the LiDAR is much lower than that of the IMU, the corresponding IMU data can always be found near the timestamp of the radar point cloud frame. The IMU data near the timestamp of the radar point cloud frame is weighted and fused to correspond to the timestamp of the corresponding radar point cloud frame; after the timestamps are aligned, the IMU data is used to perform motion compensation on the original point cloud data to obtain the motion compensated point cloud data.

[0065] Finally, the point cloud data after motion compensation is filtered. Through downsampling filtering, the number of points is reduced, the calculation scale is reduced to improve the processing speed, and the outliers are removed to improve the registration accuracy.

[0066] Step 2: Extract features from the sparse point cloud, use feature point registration to estimate the pose of the sparse point cloud, and record and store the sparse point cloud key frames and their poses.

[0067] like Figure 2 As shown, the method of step 2 is:

[0068] Calculate the curvature of each point in the sparse point cloud, set the curvature threshold, and determine whether the actual curvature of the point is greater than the curvature threshold. If so, it is regarded as a corner point, otherwise it is regarded as a surface point. The curvature calculation formula is:

[0069] ,

[0070] Among them, S is the point to be analyzed in the same sparse point cloud frame A set of consecutively arranged points, is the point in the kth sparse point cloud frame in the set S and Point The coordinates of .

[0071] After obtaining the feature points of each sparse point cloud frame: corner points and surface points, a KD tree is created to store the point cloud data;

[0072] After obtaining the current sparse point cloud, the feature points of the current sparse point cloud and the KD tree are used to complete the nearest neighbor search of the feature points to achieve a rough match with the local map; fine alignment is performed through the ICP algorithm, and it is iterated until convergence or the number of iterations is reached to obtain the current sparse point cloud frame pose.

[0073] The first sparse point cloud frame for creating a local map is selected as a key frame;

[0074] When the distance or rotation angle between the new sparse point cloud frame and the previous key frame is greater than the corresponding distance threshold or rotation angle threshold, the new sparse point cloud frame is selected as the key frame and the pose is obtained.

[0075] Step 3: When the IMU is at zero speed or constant speed, use the IMU data to calculate the roll angle and pitch angle of the current key frame. Since the IMU is used to detect whether SLAM drifts, the IMU data itself needs to maintain a certain degree of accuracy. When the IMU acceleration and angular velocity changes are in a very small range, that is, it maintains a zero speed / constant speed state, it can provide relatively accurate posture information for SLAM drift state judgment and compensation.

[0076] In the specific implementation process, the process of analyzing whether the IMU is at zero speed or uniform speed includes:

[0077] The average acceleration modulus and average angular velocity modulus are calculated as follows:

[0078] ,

[0079] ,

[0080] in, and are the average acceleration modulus and average angular velocity modulus of N consecutive groups of IMU data before and at time t; , , They are the first position of IMU on the X, Y, and Z axes. Group acceleration, , , It is the acceleration zero bias of IMU on the X, Y, and Z axes; , , They are the first position of IMU on the X, Y, and Z axes. Group angular velocity, , , It is the angular velocity zero bias of IMU on the X, Y, and Z axes;

[0081] Setting the acceleration threshold and angular velocity threshold , when the following conditions are met at the same time, the IMU is considered to be in zero speed or uniform speed state:

[0082] ,

[0083] Where g is the acceleration due to gravity.

[0084] Step 4: SLAM drift state determination. Analyze the pitch angle of the current key frame calculated by IMU data and the pitch angle of the current key frame obtained by registration, and the difference between the pitch angle of the hth historical key frame before the current key frame and the pitch angle of the current key frame calculated by IMU data to determine whether SLAM drift occurs.

[0085] The method of step 4 is: when the IMU determines zero speed or uniform speed, the roll angle of the current key frame is calculated using the following formula: and pitch angle :

[0086] ,

[0087] ,

[0088] in They are the average accelerations of the current key frame timestamp and the N consecutive groups of IMU data before it on the X, Y, and Z axes, respectively.

[0089] After obtaining the pitch angle, the drift determination is made according to the following conditions:

[0090] ,

[0091] in, is the registered pitch angle of the current keyframe, The pitch angle of the current keyframe calculated from the IMU data, is the spatial coordinate of the current keyframe, is the spatial coordinate of the hth historical key frame before the current key frame, , are a first drift judgment threshold and a second drift judgment threshold;

[0092] When any of the above drift determination conditions is met, SLAM drift is determined.

[0093] Step 5: Optimize compensation according to drift status.

[0094] When SLAM is judged to have drifted, the x, y coordinates and heading angle of the current key frame are kept unchanged, the roll angle and pitch angle of the current key frame calculated by IMU data are assigned to the current key frame, and the z coordinate of the hth historical key frame before the current key frame is assigned to the current key frame, that is:

[0095] .

[0096] When it is determined that the SLAM has not drifted or the drift posture has been adjusted, a cost function of G2O graph optimization is constructed using the posture error of the current key frame and the drift error controlled by the drift flag, and optimization iteration is performed using the G2O graph optimization, including: constructing a graph of G2O graph optimization, wherein the posture of the laser radar key frame is set as a node, and the relative posture transformation between two key frames is added as an edge;

[0097] Then construct a cost function to optimize the error of the entire graph, the cost function is:

[0098] ,

[0099] in, Represents the pose error term of the kth key frame:

[0100] ,in, is the pose error weight, is the registration pose of the kth key frame, represents the transformation matrix from the jth to the kth key frame, is the registration pose of the jth key frame, where j is the previous one of k;

[0101] Represents the drift error term of the kth key frame:

[0102] ,

[0103] in, It is a drift sign. When drift occurs, Set to 0; when no drift occurs, Set to 1, is the weight, is the registration pitch angle of the current keyframe, is the pitch angle obtained by the spatial coordinates of the current key frame and the previous h-th key frame, The current keyframe pitch angle calculated for the IMU data.

[0104] Finally, the G2O graph optimization framework is used to optimize the entire graph iteratively until Converge to the threshold and finally realize SLAM error compensation.

[0105] Example 2

[0106] See also Figure 4 As shown, an embodiment of the present invention provides a device for diagnosing and compensating SLAM drift using IMU angle information, including: at least one processing unit, the processing unit, the storage unit, the IMU unit and the radar unit are interconnected through a bus unit, the storage unit is a computer-readable storage medium, and can be used to store software programs, computer executable programs and modules, such as the software programs, computer executable programs and modules corresponding to the method for diagnosing and compensating SLAM drift using IMU angle information in an embodiment of the present invention, which can be used to store data collected by the IMU unit and the radar unit. The processing unit implements the above-mentioned method for diagnosing and compensating SLAM drift using IMU angle information by running the software programs, computer executable programs and modules stored in the storage unit, including:

[0107] The timestamps of the original LiDAR point cloud data and the IMU data are aligned. The original point cloud data is motion compensated using the IMU data to obtain the motion compensated point cloud data. The motion compensated point cloud data is point cloud filtered to obtain the sparse point cloud.

[0108] Extract features from sparse point clouds and estimate their poses using feature point registration; select and store sparse point cloud keyframes and their estimated poses;

[0109] When the IMU is at zero speed or constant speed, the roll angle and pitch angle at the corresponding time of the current key frame are calculated using the IMU data;

[0110] Analyze the pitch angle of the current key frame at the time calculated by IMU data and the pitch angle of the current key frame at the time obtained by registration, and the difference between the pitch angle of the hth historical key frame before the current key frame and the pitch angle of the current key frame calculated by IMU data to determine whether SLAM drift occurs;

[0111] When it is determined that SLAM drift occurs, the x, y coordinates and heading angle of the current key frame remain unchanged, the roll angle and pitch angle of the current key frame calculated by the IMU data are assigned to the current key frame, and the z coordinate of the hth historical key frame before the current key frame is assigned to the current key frame;

[0112] When it is determined that SLAM has not drifted or has adjusted its posture after drifting, the posture error of the current key frame and the drift error controlled by the drift flag are used to construct the cost function of G2O graph optimization, and the G2O graph optimization is used for optimization iteration.

[0113] Of course, the computer program stored in the storage unit of the device for diagnosing and compensating SLAM drift using IMU angle information provided in an embodiment of the present invention is not limited to the method operations described above, and can also execute related operations in the method for diagnosing and compensating SLAM drift using IMU angle information provided in any embodiment of the present invention.

[0114] Example 3

[0115] An embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program. When the computer program is executed, the method for diagnosing and compensating SLAM drift using IMU angle information is implemented, including:

[0116] The timestamps of the original LiDAR point cloud data and the IMU data are aligned. The original point cloud data is motion compensated using the IMU data to obtain the motion compensated point cloud data. The motion compensated point cloud data is point cloud filtered to obtain the sparse point cloud.

[0117] Extract features from sparse point clouds and estimate their poses using feature point registration; select and store sparse point cloud keyframes and their estimated poses;

[0118] When the IMU is at zero speed or constant speed, the roll angle and pitch angle at the corresponding time of the current key frame are calculated using the IMU data;

[0119] Analyze the pitch angle of the current key frame at the time calculated by IMU data and the pitch angle of the current key frame at the time obtained by registration, and the difference between the pitch angle of the hth historical key frame before the current key frame and the pitch angle of the current key frame calculated by IMU data to determine whether SLAM drift occurs;

[0120] When it is determined that SLAM drift occurs, the x, y coordinates and heading angle of the current key frame remain unchanged, the roll angle and pitch angle of the current key frame calculated by the IMU data are assigned to the current key frame, and the z coordinate of the hth historical key frame before the current key frame is assigned to the current key frame;

[0121] When it is determined that SLAM has not drifted or has adjusted its posture after drifting, the posture error of the current key frame and the drift error controlled by the drift flag are used to construct the cost function of G2O graph optimization, and the G2O graph optimization is used for optimization iteration.

[0122] A computer-readable storage medium provided in an embodiment of the present invention stores a computer program which is not limited to the method operations described above, but can also execute related operations in a method for SLAM drift diagnosis and compensation using IMU angle information provided in any embodiment of the present invention.

[0123] In the embodiments provided by the present invention, it should be understood that the disclosed structures and methods can be implemented in other ways. For example, the structural embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, structures or units, which can be electrical, mechanical or other forms.

[0124] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0125] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0126] The foregoing is merely a specific embodiment of the present invention, which enables those skilled in the art to understand or implement the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features claimed herein.

Claims

1. A method for SLAM drift diagnosis and compensation using IMU angle information, characterized in that: include: Align the timestamps of the LiDAR raw point cloud data and IMU data; The IMU data is used to perform motion compensation on the original point cloud data to obtain the motion compensated point cloud data, and the motion compensated point cloud data is subjected to point cloud filtering to obtain a sparse point cloud; Extract features from sparse point clouds and estimate their poses using feature point registration; select and store sparse point cloud keyframes and their estimated poses; When the IMU is at zero speed or constant speed, the roll angle and pitch angle at the corresponding time of the current key frame are calculated using the IMU data; Analyze the pitch angle of the current key frame at the time calculated by IMU data and the pitch angle of the current key frame at the time obtained by registration, and the difference between the pitch angle of the hth historical key frame before the current key frame and the pitch angle of the current key frame calculated by IMU data to determine whether SLAM drift occurs; When it is determined that SLAM drift occurs, the x, y coordinates and heading angle of the current key frame remain unchanged, the roll angle and pitch angle of the current key frame calculated by the IMU data are assigned to the current key frame, and the z coordinate of the hth historical key frame before the current key frame is assigned to the current key frame; When it is determined that SLAM has not drifted or has adjusted its posture after drifting, the posture error of the current key frame and the drift error controlled by the drift flag are used to construct the cost function of G2O graph optimization, and the G2O graph optimization is used for optimization iteration.

2. The method for SLAM drift diagnosis and compensation using IMU angle information according to claim 1, characterized in that: Find the IMU data within the set time range near the timestamp of the original point cloud frame, and weight the IMU data near the timestamp of the original point cloud frame to the timestamp of the corresponding original point cloud frame to align the timestamps of the lidar original point cloud data and the IMU data.

3. The method for SLAM drift diagnosis and compensation using IMU angle information according to claim 1, characterized in that: The feature extraction of the sparse point cloud and the pose estimation of the sparse point cloud using feature point registration include: Calculate the curvature of each point in the sparse point cloud, set the curvature threshold, and determine whether the actual curvature of the point is greater than the curvature threshold. If so, it is regarded as a corner point, otherwise it is regarded as a surface point. The curvature calculation formula is: ； Among them, S is the point to be analyzed in the same sparse point cloud frame A set of consecutively arranged points, is the point in the kth sparse point cloud frame in the set S and Point The coordinates of After obtaining the feature points of each sparse point cloud frame: corner points and surface points, a KD tree is created to store the point cloud data; Create a local map based on the sparse point cloud frame and add feature points to the local map; When a sparse point cloud frame is collected, the KD tree is used to complete the nearest neighbor search of the feature points to achieve a rough match with the local map; the ICP algorithm is used for fine alignment, and the iteration is stopped until convergence or the number of iterations is reached to obtain the current sparse point cloud frame pose.

4. The method for SLAM drift diagnosis and compensation using IMU angle information according to claim 1, characterized in that: Select and store sparse point cloud keyframes and their poses including: The first sparse point cloud frame for creating a local map is selected as a key frame; When the distance or rotation angle between the new sparse point cloud frame and the previous key frame is greater than the corresponding distance threshold or rotation angle threshold, the new sparse point cloud frame is selected as the key frame and the pose is obtained.

5. The method for SLAM drift diagnosis and compensation using IMU angle information according to claim 1, characterized in that: When the IMU itself maintains zero speed or constant speed, it can provide relatively accurate posture information for SLAM drift diagnosis and compensation. The process of analyzing whether the IMU is at zero speed or constant speed includes: The average acceleration modulus and average angular velocity modulus are calculated as follows: ， ， in, and are the average acceleration modulus and average angular velocity modulus of N consecutive groups of IMU data before and at time t; , , They are the first position of IMU on the X, Y and Z axes respectively. Group acceleration, , , It is the acceleration zero bias of IMU on the X, Y, and Z axes; , , They are the first position of IMU on the X, Y and Z axes respectively. Group angular velocity, , , It is the angular velocity zero bias of IMU on the X, Y, and Z axes; Setting the acceleration threshold and angular velocity threshold , when the following conditions are met at the same time, the IMU is considered to be in zero speed or uniform speed state: ， Where g is the acceleration due to gravity.

6. The method for SLAM drift diagnosis and compensation using IMU angle information according to claim 1, characterized in that: Use the following formula to calculate the roll angle of the current key frame: and pitch angle : ， ， in They are the average accelerations of the current key frame timestamp and the N consecutive groups of IMU data before it on the X, Y, and Z axes, respectively.

7. The method for SLAM drift diagnosis and compensation using IMU angle information according to claim 1, characterized in that: Analyze the pitch angle of the current key frame calculated by IMU data and the pitch angle of the current key frame obtained by registration, and the difference between the pitch angle of the hth historical key frame before the current key frame and the pitch angle of the current key frame calculated by IMU data to determine whether SLAM drift occurs, including: Set the drift determination conditions as follows: ， in, is the registered pitch angle of the current keyframe, The pitch angle of the current keyframe calculated from the IMU data, is the spatial coordinate of the current keyframe, is the spatial coordinate of the hth historical key frame before the current key frame, , are a first drift judgment threshold and a second drift judgment threshold; When any of the above drift determination conditions is met, SLAM drift is determined.

8. The method for SLAM drift diagnosis and compensation using IMU angle information according to claim 1, characterized in that: The cost function of G2O graph optimization is constructed using the pose error and drift error of the current key frame. The optimization iteration using G2O graph optimization includes: Constructing a G2O graph optimized graph, wherein the graph sets the poses of the lidar key frames as nodes and adds the relative pose transformation between two frames as edges; Then construct a cost function to optimize the error of the entire graph. The cost function is: ， in, Represents the pose error term of the kth key frame: ,in, is the pose error weight, is the registration pose of the kth key frame, represents the transformation matrix from the jth to the kth key frame, is the registration pose of the jth key frame, where j is the previous one of k; Represents the drift error term of the kth key frame: ， in, It is a drift sign. When drift occurs, Set to 0; when no drift occurs, Set to 1, is the weight, is the registration pitch angle of the current keyframe, is the pitch angle obtained by the spatial coordinates of the current key frame and the previous h-th key frame, The current keyframe pitch angle calculated from the IMU data; Finally, the G2O graph optimization framework is used to optimize the entire graph iteratively until Converge to the threshold and finally realize SLAM error compensation.

9. A device for SLAM drift diagnosis and compensation using IMU angle information, characterized in that: include: At least one processing unit, the processing unit, the storage unit, the IMU unit and the radar unit are interconnected through a bus unit, the storage unit stores a computer program and data collected by the IMU unit and the radar unit, and when the computer program is executed by the processing unit, the method for SLAM drift diagnosis and compensation using IMU angle information as described in any one of claims 1-8 is implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method for SLAM drift diagnosis and compensation using IMU angle information as described in any one of claims 1 to 8 is implemented.

Images