A slam method of tightly coupling laser odometry and inertial odometry
By tightly coupling laser odometry and inertial odometry, the problems of easy failure of laser odometry and large cumulative error of inertial odometry in traditional SLAM are solved. This method realizes an effective and accurate technical means to compensate for sensor errors. In the dynamic interference environment of laser odometry, high-precision pose estimation and system robustness are achieved by compensating for laser odometry through IMU pre-integration processing.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- JIANGSU UNIV OF TECH
- Filing Date
- 2025-04-03
- Publication Date
- 2026-05-19
AI Technical Summary
Traditional SLAM methods suffer from the following problems: laser odometry is prone to failure in environments with dynamic object interference or feature degradation, inertial odometry has large cumulative errors, and bidirectional depth error compensation is not achieved.
A tightly coupled laser odometry and inertial odometry method is adopted. Laser frame motion distortion is compensated by IMU pre-integration processing, feature points are selected based on curvature calculation, uniform filtering is implemented to optimize point cloud distribution, and multiple factors are used to optimize global pose and construct a global map.
Achieve bidirectional depth compensation for sensor errors, improve pose estimation accuracy, suppress accumulated errors, enhance system robustness, and meet the real-time processing requirements of complex scenarios.
Smart Images
Figure CN120252685B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of real-time localization and mapping (SLAM) technology, specifically relating to a SLAM method of tightly coupled laser odometry and inertial odometry. Background Technology
[0002] In traditional SLAM, laser odometry is prone to point cloud matching failure in environments with dynamic object interference or feature degradation, leading to pose drift. While inertial odometry (IMU) has high-frequency characteristics, it suffers from cumulative errors, resulting in a significant decrease in accuracy over long-term operation. Existing methods mostly employ loosely coupled architectures, relying solely on filtering and fusing sensor data, without achieving bidirectional depth error compensation. Summary of the Invention
[0003] To overcome the current problems and shortcomings in focusing on targets with weak features, this invention proposes a tightly coupled SLAM method using laser odometry and inertial odometry, characterized by the following steps:
[0004] S1. Receive IMU data and lidar point cloud data, perform pre-integration processing on the IMU data and compensate for laser frame motion distortion;
[0005] S2. Feature point selection is performed on the laser point cloud based on curvature calculation to distinguish between corner points and planar points;
[0006] S3. Apply uniform filtering to feature points to optimize point cloud distribution density;
[0007] S4. Calculate the pose change matrix of the laser odometry based on the filtered feature points, and correct the IMU data of the inertial odometry using the pose change matrix;
[0008] S5. Use multiple factors to incorporate into the factor graph to optimize the global pose and construct a global map.
[0009] Furthermore, the specific method for IMU pre-integration processing and laser frame motion distortion compensation in step S1 includes:
[0010] Trim and align the IMU data queue to ensure that the IMU timestamps and LiDAR timestamps are synchronized;
[0011] Pre-integrate the IMU data within the initial and second frame time intervals of the lidar to obtain the changes in IMU pose, thereby correcting the motion distortion of the lidar in the initial and second frames.
[0012] The attitude of the IMU data closest to the laser initiation time is used as the initial value for the laser initiation time.
[0013] Furthermore, the IMU pre-integration processing in step S1 includes a recursive initialization process, specifically as follows:
[0014] The initialization status detection module is dynamically triggered by determining the time alignment between the IMU data buffer queue and the current lidar frame.
[0015] When an uninitialized state is detected, a state propagation model under the framework of Lie group and Lie algebra is constructed. Quaternion interpolation algorithm is used to achieve spatial alignment between the IMU and the lidar coordinate system. By constructing a pre-integral observation model that includes compensation for angular velocity deviation and acceleration deviation, the rotation matrix, translation vector and bias parameters are optimized simultaneously.
[0016] Furthermore, the method for selecting feature points from the laser point cloud based on curvature calculation in step S2 is as follows:
[0017] S21. Select any point Simultaneously select points Several adjacent points on the laser line, and The points where the closest positions of the two laser lines above and below a given laser line are the same number of points used to determine the selected points. Is it a sample set of feature points?
[0018] S22. Calculation Point curvature:
[0019]
[0020] in Representative point curvature, The current point depth, It is the depth of other points in the sample;
[0021] S23. Set dynamic noise threshold:
[0022]
[0023] in, The weight is set to 0.1 in this experiment; Represents the number of valid points within the neighborhood;
[0024] S24. If the curvature of the current point If the value is less than a certain threshold, it is identified as an outlier. At the same time, several adjacent points on the scan line before and after the point are marked as invalid and will not be added to feature extraction to avoid feature clustering. Points less than a certain threshold are retained and used for subsequent feature extraction.
[0025] Simultaneously, if the curvature of the remaining laser points is greater than 5 and there are enough nearby valid points, they are identified as corner points; if the curvature is less than 0.1 and there are enough nearby valid points, they are identified as planar points. Finally, all remaining unprocessed points are set as planar points. The number of valid points here refers to the number of valid points in the neighborhood of the selected point. Whether a point in the neighborhood is valid is determined by the intensity of the laser data obtained through OpenCV functions. The strength of the intensity value is used to determine whether it is an outlier; otherwise, it is considered a valid point.
[0026] Furthermore, in step S3, the uniform filtering of feature points is performed based on adaptive sampling using spatial grid density analysis, and the farthest point sampling method is used to ensure the uniformity of the spatial distribution of feature points.
[0027] Furthermore, in step S4, the calculation of the pose change matrix under laser odometry based on the filtered feature points is specifically as follows:
[0028] Corner residual calculation:
[0029] Select 5 nearest neighbor points and calculate their centroids. Construct the covariance matrix For the matrix Eigenvalue decomposition The direction of the line corresponding to the largest eigenvalue is obtained. Finally, the residual formula is constructed to calculate the residual:
[0030]
[0031] in, For the current point, The endpoints of the line;
[0032] Plane point residual calculation:
[0033] Constructing an overdetermined system of equations ,in It is a matrix of point coordinates. .
[0034] Using least squares solution To obtain the plane parameters ;
[0035] Construct the residual formula:
[0036]
[0037] Joint optimization of the residuals is performed using the least squares method:
[0038] .
[0039] That is and ,in, The weights are as follows: the farther the point, the greater its impact on the real-time performance and accuracy of the odometer; therefore, the farther the point, the greater its weight. T is the pose change matrix under laser odometer conditions.
[0040] Furthermore, the specific method for correcting the IMU data of the inertial odometry using the optimized feature points in step S4 is as follows:
[0041] S41. Construct an inertial odometry factor to constrain the pose, velocity, and bias at adjacent time points:
[0042]
[0043] Where R represents the rotation matrix, p represents the quaternion corresponding to the pose, v represents the velocity, and ba and bg are the accelerometer and gyroscope biases, respectively. It refers to the speed change from k to k+1. For time change Let be the quaternion representing the pose change from point k to k+1. Rotation matrix from k to k+1.
[0044] S42. Constructing the residual model
[0045]
[0046] in To optimize the laser odometry observations, Indicates IMU observations, To calibrate external parameters.
[0047] S43. Align the high-frequency IMU data between two laser frames with the timestamps, insert the laser odometry data into both ends of the inertial odometry, correct the high-frequency, low-precision inertial odometry, and finally repropagate the corrected inertial odometry to ensure consistency.
[0048] Furthermore, step S4 employs a dynamic noise adjustment strategy to address the degradation problem of the laser odometry. The specific method is as follows:
[0049] The degree of environmental degradation can be determined by analyzing the eigenvalues of the point cloud covariance matrix.
[0050] When the minimum eigenvalue λ3 < the threshold τ, the covariance coefficient of the laser odometer noise is increased.
[0051] Furthermore, step S5, which uses multiple factors to optimize the global pose in the factor graph, specifically involves:
[0052] The multiple factors include laser odometry factor, inertial odometry factor, GPS factor, and loopback factor;
[0053] S51. Add laser odometry factor and inertial odometry factor:
[0054] Define the relative pose constraints between adjacent keyframes:
[0055]
[0056] in: For keyframes The position, It is the first Frames and The pose transformation matrix of the frame is calculated by the front-end odometry.
[0057] The noise model is a diagonal covariance matrix:
[0058]
[0059] Rotational component variance The variance of the translation component is ;
[0060] S52. Add GPS factor:
[0061] GPS factors provide absolute position observations:
[0062]
[0063] For keyframes The translation vector, These are GPS measurements;
[0064] Noise covariance:
[0065]
[0066] Horizontal accuracy Elevation accuracy
[0067] Add a cyclic factor:
[0068] The current frame is matched with several historical frames. If a match is successful, the relative positional relationship between this frame and the matched historical frames is calculated, and the relative pose is calculated through ICP matching.
[0069] Closed-loop factor constrains keyframe poses across time:
[0070]
[0071] in, Cross-frame matching results are obtained from ICP matching. , It is the pose matrix of two frames.
[0072] Noise covariance is adaptively adjusted based on ICP score:
[0073]
[0074] , As the baseline value;
[0075] The global pose optimization method is to use the ISM2 function of GTSAM for optimization.
[0076] Furthermore, in step S5, the specific pose optimization method also includes LM method optimization.
[0077] The beneficial effects of this invention are:
[0078] Tightly coupled bidirectional correction mechanism: Combining the advantages of laser odometry and inertial odometry, it achieves deep bidirectional compensation of sensor error, effectively improving the accuracy of pose estimation, and significantly suppressing accumulated error, especially in dynamic interference scenarios.
[0079] Incremental optimization: Adopting an advanced multi-factor graph optimization strategy, integrating multi-source constraints such as laser, inertial, and closed-loop, it significantly improves computational efficiency while ensuring global consistency, meeting the real-time processing needs of complex scenarios.
[0080] Dynamic noise adaptation: Automatically adjusts sensor weights through environmental degradation detection, effectively suppressing pose drift in feature-sparse or degraded environments and enhancing system robustness. Attached Figure Description
[0081] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0082] Figure 1 Flowchart of laser odometer correction for inertial odometer
[0083] Figure 2 Schematic diagram of sample point extraction
[0084] Figure 3 Feature extraction diagram
[0085] Figure 4 Uniform filtering effect diagram
[0086] Figure 5Odometer performance on the M2DGR dataset Detailed Implementation
[0087] The present application will be described below with reference to specific embodiments:
[0088] Example 1:
[0089] This embodiment presents a SLAM method for tightly coupled laser odometry and inertial odometry, including the following steps:
[0090] S1. Receive IMU data and lidar point cloud data, perform pre-integration processing on the IMU data and compensate for laser frame motion distortion.
[0091] The system receives raw measurements from the IMU's accelerometer and gyroscope. It also acquires pose estimates from the laser odometry as constraints. The purpose of IMU pre-integration is to correct a low-precision, high-frequency inertial odometry using a high-precision, low-frequency laser odometry. Prior to this, the IMU data needs to be preprocessed: the IMU data queue is pruned and aligned to ensure synchronization between the IMU timestamp and the lidar timestamp.
[0092] Then pre-integration is performed, specifically as follows:
[0093] Calculate the relative motion by integrating IMU data between adjacent laser frames:
[0094] Relative rotation:
[0095]
[0096] Time, This represents the gyroscope measurement value. Indicates zero bias;
[0097] It is an operational exponent mapping on the Lie group SO(3).
[0098] Relative velocity change:
[0099]
[0100] Indicates from time arrive speed change, It is a relative rotation matrix. It is an accelerometer measurement. It is zero bias
[0101] Relative displacement:
[0102]
[0103] Indicates from time arrive The relative displacement, It is the cumulative change in velocity at the current moment.
[0104] Error state propagation:
[0105]
[0106] in, Here is the state transition matrix. The noise Jacobian matrix, This represents the IMU noise covariance.
[0107] The method for compensating for motion distortion in laser frames is as follows:
[0108] The IMU data within the initial and second frame time intervals of the lidar are pre-integrated to obtain the IMU pose change, thereby correcting the motion distortion of the lidar in the initial and second frames. The IMU data closest to the laser start time is found, and its pose is used as the initial value for the laser start time. By performing linear interpolation on each frame of the point cloud data, the IMU pose corresponding to the current time is obtained. Then, the pose change of the current frame is obtained based on the IMU pre-integration to achieve point cloud skew correction. The point cloud features of each corrected frame are extracted. Specifically:
[0109] Set lidar frame timestamp Search the IMU odometer queue for items that meet the requirements. The smallest IMU data is used to obtain the inertial odometry of the most recent frame, which is aligned with the current frame's laser odometry timestamp. Its pose is then used as the initial pose of the lidar in the current frame.
[0110] The above IMU pre-integration process adopts a recursive initialization mechanism based on timestamp synchronization verification. The specific process is as follows:
[0111] The initialization status detection module is dynamically triggered by determining the time alignment between the IMU data buffer queue and the current lidar frame.
[0112] If the system is in an uninitialized state, the following operations are performed: The inertial measurement unit (IMU) pre-integration module is reset; based on the state estimation parameters of the previous keyframe and the inertial sensor bias parameters, a state propagation model under the Lie group and Lie algebra framework is constructed; the attitude prior estimate at the current moment is derived through forward integration, and a quaternion interpolation algorithm is used to achieve spatial alignment between the IMU coordinate system and the lidar coordinate system; a nonlinear optimization problem containing inertial constraint factors is constructed, and a nonlinear optimization algorithm is used to jointly optimize the state variables, synchronously updating the initial estimates of the rotation matrix, translation vector, and IMU bias parameters; based on the optimized bias parameters, the IMU pre-integration quantity within the current lidar scanning cycle is recalculated, and a pre-integration observation model including angular velocity deviation and acceleration deviation compensation is established; the updated pose transformation matrix is stored in the pre-integration buffer, the state transition equation of the laser-inertial tightly coupled system is constructed, and finally, a six-degree-of-freedom pose estimation result with time continuity is output.
[0113] S2. Based on curvature calculation, feature extraction is performed on the laser point cloud to distinguish between corner points and planar points.
[0114] First, input the distortion-corrected point cloud data. ,in , Let be the depth of the point. For a point... The sample points cover the three laser points adjacent to each of the two laser lines to the left and right, and the five points corresponding to the two laser lines above and below. To ensure the quality of the extracted sample points, we use a distance difference threshold to filter effective neighboring points, ensuring the geometric consistency of the extracted feature points. When the extracted laser point value is abnormal and is removed, the search radius can be expanded to ensure sufficient sample points are obtained. In this experiment, the search radius is set to five points to the left and right, i.e.:
[0115] Same scanning line: horizontal direction ;
[0116] Other adjacent scan lines: vertical direction , 1.
[0117] A schematic diagram of sample point extraction is shown below. Figure 2 As shown:
[0118] This solution provides a method for calculating curvature:
[0119]
[0120] in, Representative point curvature, Represents the current point depth, This represents the depth of other points in the sample. During feature extraction, the point cloud data of the first and last scan lines (the first and last lines) need to be discarded.
[0121] The calculated curvature is not always a valid value and needs to be determined based on the specific scenario. We establish a dynamic threshold formula:
[0122]
[0123] in, The weight is set to 0.1 in this experiment. If the curvature of the current point... If a point is found to be an outlier, it is identified as such. Simultaneously, the five neighboring points on the scan line preceding and following that point are marked as invalid and will not be included in feature extraction, thus preventing feature clustering.
[0124] Our proposed feature point classification method is as follows: If the curvature of the laser point is greater than 0.1, and the number of effective neighborhood points is satisfied... If the curvature is less than the threshold and the number of valid neighborhood points is satisfied, then it is considered a corner point and added to the corner point set; If a point is considered a planar point, it is added to the set of planar points.
[0125] S3. Apply uniform filtering to feature points to optimize point cloud distribution density.
[0126] Divide the point cloud space into sections with sides of length [missing information]. Cube grid:
[0127]
[0128] in, Minimum spacing The maximum spacing needs to be set according to the specific scenario.
[0129] For each grid Count the number of points in each grid. Calculate the point density:
[0130]
[0131] Adjust the number of sampling points according to the density:
[0132]
[0133] in, The target density.
[0134] In each grid cell with excessively high density, the point cloud is sampled at its furthest point to ensure spacing.
[0135]
[0136] in, This is the set of selected points.
[0137] Figure 3 This is a diagram of the feature points after uniform filtering, where blue dots represent planar points and red dots represent corner points.
[0138] S4. Correct the IMU data of the inertial odometry using the optimized feature points;
[0139] The raw measurements of IMU acceleration and angular velocity can be defined as follows:
[0140]
[0141] Among them, and These represent the original measurements in the IMU coordinate system at time t. The rotation matrix in the world coordinate system. For accelerometer and gyroscope bias. The time interval of one frame of laser odometry. Inside, the IMU pre-integral quantity is discretized as:
[0142]
[0143] It is the IMU sampling interval. The pre-integral quantity of the relative rotation from time t_k to t_(k+1). The pre-integral quantity of the velocity change Δv_(k+1) is obtained by integrating the acceleration. ΔR_i is the rotation matrix at the current time, which transforms the acceleration to the initial coordinate system. The pre-integral quantity of displacement change consists of the second integral of velocity change and acceleration; and These are the original measurement values mentioned above. It's about bias.
[0144] Preintegral covariance via the error state transfer matrix With noise matrix To spread:
[0145]
[0146] It is the covariance matrix of the IMU measurement noise.
[0147] Construct an optimization problem that includes the IMU pre-integration factor and the laser odometry factor:
[0148]
[0149] Among them, state variables Includes pose, velocity, and bias. The IMU residual term is defined as:
[0150]
[0151] in:
[0152] Rotation matrix (attitude) in world coordinate system Position in world coordinate system
[0153] :speed
[0154] and : Time-varying bias of accelerometer and gyroscope.
[0155] The laser odometry residuals are constrained by SE(3):
[0156]
[0157] in These are laser odometry observations. To calibrate external parameters.
[0158] During calibration, the laser odometer is assumed to be of high precision. However, in real-world conditions, laser odometers also experience degradation. To prevent significant errors in the inertial odometer due to laser odometer degradation, an environmental degradation detection index is introduced. ,in To match the Hessian matrix to the point cloud, when Large noise covariance Reduce the observation weights in degraded scenarios, and use normal covariance in other cases. In our solution, we set... , .
[0159]
[0160] Incremental optimization using the iSAM2 algorithm from the GTSAM optimization library can be represented as follows:
[0161]
[0162] The state variables at the current moment, including pose. , IMU bias wait;
[0163] Observational data, including IMU pre-integration and lidar pose constraints.
[0164] Optimized execution of bias update and pre-integral repropagation:
[0165]
[0166] From time arrive The rotation pre-integral value represents the cumulative amount of relative rotation between two frames. : Raw angular velocity measurement of the gyroscope, including noise and bias.
[0167] : The optimized gyroscope bias correction amount, the bias residual obtained through iSAM2 optimization.
[0168] The IMU sampling time interval is usually a fixed value, which is derived from the IMU's sampling frequency.
[0169] Ensure that subsequent IMU integrations are based on the latest bias estimates to maintain time series consistency.
[0170] S5. Use multiple factors to incorporate into the factor graph to optimize the global pose and construct a global map.
[0171] Building a local map:
[0172] First, a KD-Tree search is used to search for and extract historical keyframes within the set radius. These keyframes are used for subsequent point cloud map creation. Second, the average depth of the laser points in each keyframe is calculated. The sparsity of the sensor's environment is determined based on the average depth value. The point cloud is then downsampled based on the sparsity of the environment, retaining more points in sparse environments and fewer points in dense environments.
[0173] Scan point optimization:
[0174] Perform pose initialization on the first frame, inputting the laser point cloud from the first frame. Obtain the initial attitude of the IMU ,in This represents the initial pose vector. Let the position of the first frame be... The initial pose is represented using homogeneous coordinates, and a transformation matrix is constructed. ,in This is the Euler angle rotation matrix function.
[0175] Pose optimization is performed on subsequent frames, the first... Laser point cloud of a frame The optimized pose of the previous frame IMU's stance .
[0176] Attitude Increment Calculation .
[0177] Preserve translation vector Keep it unchanged, only update the rotation, and update the predicted pose. .
[0178] For each laser point Calculate map projection error:
[0179]
[0180] in, The nearest neighbor on the map. This is the corresponding normal vector.
[0181] Add IMU constraint terms .
[0182] Optimize objective function The LM algorithm is used for iterative optimization, where, Dynamically adjusted based on point cloud curvature This represents the IMU confidence coefficient. The output is the optimized laser odometry pose. .
[0183] Calculate corner residuals:
[0184] Calculating corner and area residuals optimizes subsequent frames. The first frame uses the IMU's attitude information and the radar's position information from the first frame. .
[0185] First, select 5 nearest neighbor points and calculate their centroids. Secondly, construct the covariance matrix. For the matrix Eigenvalue decomposition The direction of the line corresponding to the largest eigenvalue is obtained. Finally, the residual formula is constructed to calculate the residual:
[0186]
[0187] in, For the current point, The endpoints of the line.
[0188] Calculate the plane point residuals:
[0189] Constructing an overdetermined system of equations ,in It is a matrix of point coordinates. .
[0190] Using least squares solution To obtain the plane parameters .
[0191] Construct the residual formula:
[0192]
[0193] Joint optimization of residuals is performed using the least squares method:
[0194]
[0195] in, The weight is determined by the distance; since distant points have a significant impact on the real-time performance and accuracy of the odometer, the weight of more distant points is greater.
[0196] Pose optimization:
[0197] Using LM optimization, firstly, the residuals are obtained by differentiation. Pose parameters Jacobian matrix: ,
[0198] Then, update the parameters using the following formula.
[0199]
[0200] The initial value is set to It gradually decreases after convergence.
[0201] Optimize global pose by incorporating multiple factors into the factor graph and construct a global map:
[0202] S51. Add laser odometry factor and inertial odometry factor:
[0203] Define the relative pose constraints between adjacent keyframes:
[0204]
[0205] in:
[0206] For keyframes The position, Calculated by the front-end odometer;
[0207] The noise model is a diagonal covariance matrix:
[0208]
[0209] S52. Add GPS factor:
[0210] GPS factors provide absolute position observations:
[0211]
[0212] For keyframes The translation vector, These are GPS measurements;
[0213] The noise covariance is:
[0214]
[0215] S53. Add a cyclic factor:
[0216] The current frame is matched with several historical frames. If a match is successful, the relative positional relationship between this frame and the matched historical frames is calculated, and the relative pose is calculated through ICP matching.
[0217] Closed-loop factor constrains keyframe poses across time:
[0218]
[0219] in, Obtained by ICP matching;
[0220] Noise covariance is adaptively adjusted based on ICP score:
[0221]
[0222] , As the baseline value;
[0223] The optimization function incorporating each factor was optimized using the ISAM2 optimization method from the GTSAM optimization library. The optimized function was then stitched together to form a global map, the effect of which is shown below. Figure 4 As shown.
[0224] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.
[0225] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the specific details of the above embodiments. Within the scope of the technical concept of the present invention, various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical solutions of the present invention, and these equivalent transformations are all protected by the present invention.
Claims
1. A SLAM method for tightly coupled laser odometry and inertial odometry, characterized in that, Includes the following steps: S1. Receive IMU data and lidar point cloud data, perform pre-integration processing on the IMU data and compensate for laser frame motion distortion; S2. Feature point selection is performed on the laser point cloud based on curvature calculation, distinguishing between corner points and planar points. The method is as follows: S21. Select any point Simultaneously select points Several adjacent points on the laser line, and The points where the closest positions of the two laser lines above and below a given laser line are the same number of points used to determine the selected points. Is it a sample set of feature points? S22. Calculation Point curvature: ； in Representative point curvature, The current point depth, It is the depth of other points in the sample; This refers to the search range of adjacent scan lines in the vertical direction. The search range for adjacent points on the same horizontal scan line; S23. Set dynamic noise threshold: ； in, The weight is set to 0.1; Represents the number of valid points within the neighborhood; S24. If the curvature of the current point If the value is less than a certain threshold, it is identified as an outlier. At the same time, several adjacent points on the scan line before and after the point are marked as invalid and will not be added to feature extraction to avoid feature clustering. Points less than a certain threshold are retained and used for subsequent feature extraction. Meanwhile, if the curvature of the remaining laser point is greater than 5 and there are enough nearby valid points, it is identified as a corner point; if the curvature is less than 0.1 and there are enough nearby valid points, it is identified as a planar point. Finally, all the remaining unprocessed points are set as planar points. S3. Apply uniform filtering to feature points to optimize point cloud distribution density; S4. Calculate the pose change matrix of the laser odometry based on the filtered feature point matching. Correct the IMU data of the inertial odometry using the pose change matrix T. Employ a dynamic noise adjustment strategy to address the degradation problem of the laser odometry. The specific method is as follows: The degree of environmental degradation is determined by analyzing the eigenvalues of the point cloud covariance matrix; when the minimum eigenvalue λ3 < the threshold τ, the covariance coefficient of the laser odometer noise is increased. The specific method for correcting the IMU data of the inertial odometry using optimized feature points is as follows: S41. Construct an inertial odometry factor to constrain the pose, velocity, and bias at adjacent time points: ； Where R represents the rotation matrix, p represents the quaternion corresponding to the pose, v represents the velocity, and ba and bg are the accelerometer and gyroscope biases, respectively. It is the velocity change from k to k+1. For time change Let be the quaternion representing the pose change from point k to k+1. Rotation matrix from k to k+1; S42. Construct the laser odometry factor and transform the pose of two consecutive laser odometry frames to the IMU coordinate system: ； Constructing a residual model ； in To optimize the laser odometry observations, Indicates IMU observations, To calibrate external parameters; S43. Align the high-frequency IMU data between two laser frames with the preceding and following timestamps, insert the laser odometry data into both ends of the inertial odometry, and correct the inertial odometry for re-propagation; S5. Use multiple factors to optimize the global pose in the factor graph and construct a global map; the multiple factors include laser odometry factor, inertial odometry factor, GPS factor, and loop closure factor; specifically: S51. Add laser odometry factor and inertial odometry factor: Define the relative pose constraints between adjacent keyframes: ； in: For keyframes The position, Calculated by the front-end odometer; The noise model is a diagonal covariance matrix: ； S52. Add GPS factor: GPS factors provide absolute position observations: ； For keyframes The translation vector, These are GPS measurements; The noise covariance is: ； S53. Add a cyclic factor: The current frame is matched with several historical frames. If a match is successful, the relative positional relationship between this frame and the matched historical frames is calculated, and the relative pose is calculated through ICP matching. Closed-loop factor constrains keyframe pose across time: ； in, Obtained by ICP matching; Noise covariance is adaptively adjusted based on ICP score: ； , As the baseline value; The global pose optimization method is to use the ISM2 function of GTSAM.
2. The SLAM method of tightly coupled laser odometry and inertial odometry according to claim 1, characterized in that, The specific method for IMU pre-integration processing and laser frame motion distortion compensation in step S1 includes: Trim and align the IMU data queue to ensure that the IMU timestamps and LiDAR timestamps are synchronized; Pre-integrate the IMU data within the initial and final frame time intervals of the lidar to obtain the changes in IMU pose, thereby correcting the motion distortion of the lidar in the initial and second frames. The attitude of the IMU data closest to the laser initiation time is used as the initial value for the laser initiation time.
3. The SLAM method for tightly coupled laser odometry and inertial odometry according to claim 1 or 2, characterized in that, The IMU pre-integration process in step S1 includes a recursive initialization procedure, specifically as follows: The initialization status detection module is dynamically triggered by determining the time alignment between the IMU data buffer queue and the current lidar frame. When an uninitialized state is detected, a state propagation model under the framework of Lie group and Lie algebra is constructed. An interpolation algorithm is used to achieve spatial alignment between the IMU and the lidar coordinate system. By constructing a pre-integral observation model that includes compensation for angular velocity deviation and acceleration deviation, the rotation matrix, translation vector and bias parameters are optimized simultaneously.
4. The SLAM method for tightly coupled laser odometry and inertial odometry according to claim 1, characterized in that: In step S3, the uniform filtering of feature points is performed based on adaptive sampling using spatial grid density analysis, and the farthest point sampling method is used to ensure the uniformity of the spatial distribution of feature points.
5. The SLAM method for tightly coupled laser odometry and inertial odometry according to claim 1, characterized in that, In step S4, the specific steps for calculating the pose change matrix under laser odometry based on the filtered feature points are as follows: Corner residual calculation: Select 5 nearest neighbor points and calculate their centroids. Construct the covariance matrix For the matrix Eigenvalue decomposition Finally, the residual formula is constructed to calculate the residual: ； in, For the current point, The endpoints of the line; Plane point residual calculation: Constructing an overdetermined system of equations ,in It is a matrix of point coordinates. ; Using least squares solution To obtain the plane parameters ; Construct the residual formula: ； Joint optimization of the residuals is performed using the least squares method: ; in, The weights are denoted by , with more weights for points that are farther away; T is the pose change matrix under laser odometry.
6. The SLAM method for tightly coupled laser odometry and inertial odometry according to claim 1, characterized in that: In step S5, the specific pose optimization method also includes LM method optimization.