SLAM (Simultaneous Localization and Mapping) method for tightly coupling laser odometer and inertial odometer

Abstract

The invention relates to the technical field of real-time positioning and mapping (SLAM), in particular to an SLAM method for tightly coupling a laser odometer and an inertial odometer, which comprises the following steps of: S1, receiving IMU (Inertial Measurement Unit) data and laser radar point cloud data, carrying out pre-integration processing on the IMU data and compensating laser frame motion distortion; s2, feature point selection is carried out on the laser point cloud based on curvature calculation, and angular points and plane points are distinguished; s3, uniform filtering is carried out on the feature points to optimize the point cloud distribution density; s4, calculating a lower pose change matrix of the laser speedometer according to the filtered feature point matching, and correcting IMU data of the inertial speedometer through the pose change matrix; and S5, adding multiple factors into the factor graph to optimize the global pose, and constructing a global map. Through a tight coupling bidirectional correction mechanism, the advantages of a laser odometer and an inertia odometer are combined, deep bidirectional compensation of sensor errors is realized, the pose estimation precision is effectively improved, and particularly, accumulative errors are remarkably inhibited in a dynamic interference scene.

Description

Technical Field

[0001] The present invention belongs to the technical field of simultaneous localization and mapping (SLAM), and particularly relates to a SLAM method that tightly couples a lidar odometer and an inertial odometer. Background Art

[0002] In the traditional SLAM field, in an environment with dynamic object interference or feature degradation, point cloud matching of the lidar odometer is prone to failure, resulting in pose drift; although the inertial odometer (IMU) has high-frequency characteristics, it has cumulative errors, and the accuracy significantly decreases after long-term operation. Existing methods mostly adopt a loose coupling architecture, and only fuse sensor data through filtering, without achieving deep two-way error compensation. Summary of the Invention

[0003] To overcome the problems and defects existing in the current focusing on weak feature targets, the present invention proposes a SLAM method that tightly couples a lidar odometer and an inertial odometer, which is characterized by including the following steps:

[0004] S1. Receive IMU data and lidar point cloud data, perform pre-integration processing on the IMU data, and compensate for the motion distortion of the lidar frame;

[0005] S2. Select feature points from the lidar point cloud based on curvature calculation, and distinguish corner points and plane points;

[0006] S3. Perform uniform filtering on the feature points to optimize the point cloud distribution density;

[0007] S4. Calculate the pose change matrix of the lidar odometer based on the filtered feature points, and correct the IMU data of the inertial odometer through the pose change matrix;

[0008] S5. Use multiple factors to join the factor graph to optimize the global pose and construct a global map.

[0009] Further, the specific method for the IMU pre-integration processing and compensation for the motion distortion of the lidar frame in step S1 includes:

[0010] Trim and align the IMU data queue to ensure the synchronization of the IMU timestamp and the lidar timestamp;

[0011] Pre-integrate the IMU data within the time period between the initial frame and the second frame of the lidar to obtain the change in the IMU pose, thereby correcting the motion distortion between the initial frame and the second frame of the lidar;

[0012] Use the pose of the IMU data closest to the laser start time as the initial value at the laser start time.

[0013] Further, the IMU pre-integration processing in step S1 includes a recursive initialization process, specifically:

[0014] Dynamically trigger the initialization status detection module by judging the time alignment between the IMU data cache queue and the current lidar frame;

[0015] When the uninitialized state is detected, construct a state propagation model in the Lie group and Lie algebra framework, use the quaternion interpolation algorithm to achieve the spatial alignment of the IMU and the lidar coordinate systems, and synchronously optimize the rotation matrix, translation vector, and bias parameters by constructing a pre-integration observation model including angular velocity deviation and acceleration deviation compensation.

[0016] Further, the method for selecting feature points from the lidar point cloud based on curvature calculation in step S2 is as follows:

[0017] S21. Select any point p m,n , and at the same time select several adjacent points on the lidar line where point p m,n is located, and the same number of points at the closest positions on the two lidar lines above and below the lidar line where p m,n is located, as the sample set for judging whether the selected point p m,n is a feature point;

[0018] S22. Calculate the curvature of point p m,n :

[0019]

[0020] where represents the curvature of point p m,n , d (m,n) is the depth of the current point p m,n , and d (m+k,n+l) is the depth of other points in the sample;

[0021] S23. Set the dynamic noise threshold:

[0022] V p = α × N × d (m,n)

[0023] where α is the weight, set to 0.1 in this experiment; N represents the number of valid points in the neighborhood;

[0024] S24. If the curvature of the current point is greater than the threshold, it is determined as an outlier point. At the same time, several adjacent points before and after the scan line where this point is located are marked as invalid and not added to the feature extraction to avoid feature aggregation; those less than the threshold are retained for subsequent feature extraction;

[0025] Meanwhile, if the curvature of the remaining laser points is greater than 5 and the number of nearby valid points is sufficient, they are identified as corner points; if the curvature is less than 0.1 and the number of nearby valid points is sufficient, they are identified as plane points. Finally, all the remaining unprocessed points are set as plane points. The number of valid points here is the number of valid points in the selected point neighborhood. Whether the points in the neighborhood are valid is determined by the intensity of the laser data obtained through the OpenCV function. Whether a point belongs to an abnormal point is judged according to whether its strength value is lower than the threshold, and vice versa for valid points.

[0026] Further, the uniform filtering of the feature points in step S3 is based on adaptive sampling of spatial grid density analysis and uses the farthest point sampling method to ensure the uniformity of the spatial distribution of the feature points.

[0027] Further, in step S4, calculating the pose change matrix of the lower-level laser odometer based on the filtered feature points is specifically as follows:

[0028] Corner point residual calculation:

[0029] Select 5 neighboring points and calculate their centroid Construct the covariance matrix Perform eigenvalue decomposition on matrix A, A = VDV T , to obtain the line direction v corresponding to the maximum eigenvalue; finally, construct the residual formula to calculate the residual:

[0030]

[0031] where p0 is the current point, and p1, p2 are the endpoints of the line;

[0032] Plane point residual calculation:

[0033] Construct the overdetermined system of equations AX = B, where is the point coordinate matrix, and B = [-1, …, -1] T .

[0034] Use the least squares solution X = (A T A) -1 A T B to obtain the plane parameters [a, b, c, d];

[0035] Construct the residual formula:

[0036]

[0037] Use the least squares method to jointly optimize the residuals:

[0038]

[0039] d i is d cor and dsurf , where w i is the weight. The farther points have a greater impact on the real-time performance and accuracy of the odometer. Therefore, the weight value of the farther points is larger; T is the pose change matrix of the lower-level laser odometer.

[0040] Furthermore, the specific method for correcting the IMU data of the inertial odometer using the optimized feature points in step S4 is as follows:

[0041] S41. Construct inertial odometer factors to constrain the pose, velocity, and bias at adjacent times:

[0042]

[0043] Among them, R represents the rotation matrix, p represents the quaternion of the corresponding pose, v represents the velocity, ba and bg are the accelerometer and gyroscope biases respectively, Δv is the velocity change from k to k + 1, and Δt is the time change. Δp k+1 is the quaternion representing the pose change from point k to k + 1, to the rotation matrix of k + 1.

[0044] S42. Construct a residual model

[0045]

[0046] where is the observed value of the optimized laser odometer, represents the IMU observation, is the calibrated external parameter.

[0047] S43. Align the high-frequency IMU data between two laser frames in terms of time stamps before and after, insert the laser odometer data at both ends of the inertial odometer, correct the high-frequency and low-accuracy inertial odometer. Finally, re-propagate the corrected inertial odometer to ensure consistency before and after.

[0048] Furthermore, step S4 adopts a dynamic noise adjustment strategy to handle the problem of laser odometer degradation. The specific method is as follows:

[0049] Judge the degree of environmental degradation through eigenvalue analysis of the point cloud covariance matrix;

[0050] When the minimum eigenvalue λ3 < threshold τ, increase the covariance coefficient of the laser odometer noise.

[0051] Furthermore, the specific method for using multiple factors to join the factor graph to optimize the global pose in step S5 is as follows:

[0052] The multiple factors include laser odometer factors, inertial odometer factors, GPS factors, and loop closure factors; S51. Add laser odometer factors and inertial odometer factors:

[0053] Define the relative pose constraint between adjacent key frames:

[0054]

[0055] where: T k ∈ SE(3) is the pose of key frame k, and ΔT k-1,k is the pose transformation matrix between the k-th frame and the (k-1)-th frame, which is calculated by the front-end odometer;

[0056] The noise model is a diagonal covariance matrix:

[0057] Σ odo = diag(10 -6 , 10 -6 , 10 -6 , 10 -4 , 10 -4 , 10 -4 )

[0058] The variance of the rotation component is 10 -6 rad 2 , and the variance of the translation component is 10 -4 m 2 ;

[0059] S52. Add the GPS factor:

[0060] The GPS factor provides absolute position observations:

[0061]

[0062] is the translation vector of key frame k, is the GPS measurement value;

[0063] Noise covariance:

[0064] Σ gps = diag(1.0, 1.0, 2.0) m 2

[0065] The horizontal accuracy is 1.0 m, and the elevation accuracy is 2.0 m

[0066] Add the loop closure factor:

[0067] The current frame is matched with several historical frames. If the match is successful, calculate the relative position relationship between this frame and the matched history, and calculate the relative pose through ICP matching:

[0068] The loop closure factor constrains the poses of key frames across time:

[0069]

[0070] Among them, ΔT mn The cross-frame matching result is obtained by ICP matching, and T m , T n is the pose matrix of two frames

[0071] The noise covariance is adaptively adjusted according to the ICP score:

[0072]

[0073] σ φ = 0.1 rad, σ t = 0.5 m is the reference value;

[0074] The global pose optimization method is to optimize using the ISM2 function of GTSAM.

[0075] Furthermore, in the step S5, the specific pose optimization method further includes L-M method optimization.

[0076] Advantages of the present invention:

[0077] Tight-coupling bidirectional correction mechanism: Combining the advantages of lidar odometry and inertial odometry, realizing deep bidirectional compensation of sensor errors, effectively improving the pose estimation accuracy, especially significantly suppressing the cumulative error in dynamic interference scenarios.

[0078] Incremental optimization: Adopting an advanced multi-factor graph optimization strategy, fusing multi-source constraints such as lidar, inertial, and loop closure, while ensuring global consistency, greatly improving the calculation efficiency, and meeting the real-time processing requirements of complex scenarios.

[0079] Dynamic noise adaptation: Automatically adjusting the sensor weights through environmental degradation detection, effectively suppressing pose drift in feature-sparse or degraded environments, and enhancing the system robustness.

[0080] Multi-level loop closure detection: Combining kinematic criteria and geometric verification mechanisms, significantly reducing the risk of false matching, improving the reliability of loop closure constraints, and ensuring the general consistency in large-scale scenarios. Figure 1 consistency. Brief description of the drawings

[0081] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the following drawings are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0082] Figure 1 Flow schematic diagram of lidar odometry correcting inertial odometry

[0083] Figure 2 Schematic diagram of sample point extraction

[0084] Figure 3 Schematic diagram of feature extraction

[0085] Figure 4 Effect diagram of uniform filtering

[0086] Figure 5 Effect diagram of the odometer running on the M2DGR dataset Specific implementation manner

[0087] The present application will be described below in conjunction with specific embodiments:

[0088] Embodiment 1:

[0089] This embodiment provides a SLAM method for a tightly coupled lidar odometer and an inertial odometer, 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 the motion distortion of the lidar frame.

[0091] Receive the original measurement values of the accelerometer and gyroscope of the IMU. Obtain the pose estimation provided by the lidar odometer as a constraint. The purpose of IMU pre-integration is to correct the low-precision high-frequency inertial odometer using the high-precision low-frequency lidar odometer. Before this, it is necessary to preprocess the IMU data: trim and align the IMU data queue to ensure the synchronization of the IMU timestamp and the lidar timestamp.

[0092] After that, pre-integration is performed. The specific implementation method is as follows:

[0093] Integrate the IMU data between adjacent lidar frames and calculate the relative motion amount:

[0094] Relative rotation:

[0095]

[0096] k represents time, ω m (t k ) represents the gyroscope measurement value, b g represents the zero bias;

[0097] exp(·) is the operation exponential mapping on the Lie group SO(3).

[0098] Relative velocity change:

[0099]

[0100] Δv ij represents the velocity change from time i to j, ΔRik is the relative rotation matrix, a m (t k ) is the accelerometer measurement, b a is the bias

[0101] Relative displacement:

[0102]

[0103] Δp ij represents the relative displacement from time i to j, Δv ik is the cumulative amount of velocity change at the current time

[0104] Error state propagation:

[0105]

[0106] where F k is the state transition matrix, G k is the noise Jacobian matrix, and Q is the IMU noise covariance.

[0107] The method for compensating the motion distortion of the laser frame is as follows:

[0108] Pre-integrate the IMU data within the time period of the initial frame and the second frame of the lidar to obtain the change in the IMU pose, thereby correcting the motion distortion of the initial frame and the second frame of the lidar; find the IMU data closest to the laser starting time, and use its pose as the initial value at the laser starting time. By linearly interpolating each frame of the point cloud data, obtain the corresponding IMU pose at the current time, and then obtain the pose change of the current frame according to the IMU pre-integration to achieve point cloud skew correction, and extract the features of each frame of the corrected point cloud. Specifically:

[0109] Let the lidar frame timestamp be t l , search for the IMU data in the IMU odometry queue that satisfies |t l -t i | being the smallest, and obtain the inertial odometry of the nearest frame aligned with the current frame lidar odometry timestamp, and use its pose as the initial pose of the current frame of lidar.

[0110] The above IMU pre-integration process adopts a recursive initialization mechanism based on timestamp synchronization verification, and the specific process is as follows:

[0111] Dynamically trigger the initialization state detection module by judging the time alignment between the IMU data cache queue and the current lidar frame.

[0112] If the system is in an uninitialized state, perform the following operations: reset the inertial measurement unit pre-integration module, construct a state propagation model in the Lie group and Lie algebra framework based on the state estimation parameters of the previous key frame and the inertial sensor bias parameters; derive the prior attitude estimation at the current moment through forward integration operation, and use the quaternion interpolation algorithm to achieve the spatial alignment between the IMU coordinate system and the lidar coordinate system; construct a non-linear optimization problem containing inertial constraint factors, and use the non-linear optimization algorithm to jointly optimize the state variables, and synchronously update the initial estimated values of the rotation matrix, translation vector and IMU bias parameters; based on the optimized bias parameters, recalculate the IMU pre-integration quantity within the current lidar scan period, and establish a pre-integration observation model including angular velocity deviation and acceleration deviation compensation; store the updated pose transformation matrix in the pre-integration buffer, construct the state transition equation of the laser-inertial tightly coupled system, and finally output the six-degree-of-freedom pose estimation result with time continuity.

[0113] S2. Feature extraction of the lidar point cloud based on curvature calculation to distinguish corner points and plane points.

[0114] First, input the undistorted point cloud data where p i =(x i , y i , z i , d i ), and d i is the point depth. For the point its sample points cover three laser points adjacent to the left and right sides of the same laser line as this point and five points corresponding to the upper and lower two laser lines. To ensure the quality of the extracted sample points, we use the method of adding a distance difference threshold to screen valid neighborhood points to ensure the geometric consistency of the extracted feature points. When the value of the extracted lidar point is abnormal and is excluded, to ensure obtaining enough sample points, the search radius can be expanded. In this experiment, the search radius is set to five points on the left and right, that is:

[0115] Same scan line: horizontally n±Δh, 3≤Δh≤5;

[0116] Other adjacent scan lines: vertically m±Δv, Δv = 1.

[0117] The schematic diagram of sample point extraction is as shown in Figure 2 as follows:

[0118] This scheme presents a curvature calculation method:

[0119]

[0120] where, represents the curvature of point p (m,n) and d(m,n) Represents the current point p (m,n) The depth of, d (m+k,n+l) Represents the depth of other points in the sample points. During the feature extraction process, the point cloud data of the first and last scan lines (the first line and the last line) need to be removed.

[0121] Not all of the calculated curvatures are valid values, and it needs to be judged according to the specific scenario. We establish a dynamic threshold formula:

[0122] V p = α × N × d (m,n)

[0123] Among them, α is the weight, which is set to 0.1 in this experiment. If the curvature of the current point Then it is judged as an outlier. At the same time, the 5 neighboring points before and after the scan line where the point is located are marked as invalid and no longer added to the feature extraction to avoid feature aggregation.

[0124] The feature point classification method we proposed is as follows: If the curvature of the laser point is greater than 0.1 and the number of valid neighborhood points N ≥ 10, it is considered a corner point and added to the corner point set; if the curvature is less than the threshold and the number of valid neighborhood points N ≥ 6, it is considered a plane point and added to the plane point set.

[0125] S3. Perform uniform filtering on the feature points to optimize the point cloud distribution density.

[0126] Divide the point cloud space into cubic grids with side length L:

[0127]

[0128] Among them, d min Is the minimum spacing, d max Is the maximum spacing, which needs to be set according to the scenario.

[0129] For each grid Count the number of points N in each grid k , Calculate the point density:

[0130]

[0131] Adjust the number of sampling points according to the density:

[0132]

[0133] Among them, Is the target density.

[0134] In each grid with too high density, perform farthest point sampling on the point cloud to ensure the spacing:

[0135]

[0136] Among them, is the set of selected points.

[0137] Figure 3 is the effect diagram of feature points after uniform filtering, where the blue points represent plane points and the red points represent corner points.

[0138] S4. Use the optimized feature points to correct the IMU data of the inertial odometer;

[0139] The original measurements of IMU acceleration and angular velocity can be defined as:

[0140]

[0141] Among them, among them and respectively represent the original measurement values in the IMU coordinate system at time t. is the rotation matrix in the world coordinate system, b a , b g are the accelerometer and gyroscope biases. Within the time interval [t k , t k+1 of one frame of laser odometer, the IMU pre-integration quantities are discretized as:

[0142]

[0143] Δt is the IMU sampling interval time, ΔR k+1 is the relative rotation pre-integration quantity from time t_k to t_(k + 1). Δv_(k + 1) is the pre-integration quantity of velocity change, obtained by integrating acceleration, and ΔR_i is the rotation matrix at the current moment, which converts acceleration to the initial coordinate system.

[0144] Δp k+1 is the pre-integration quantity of displacement change, composed of velocity change and second-order integration of acceleration; and are the above original measurement values, is the bias.

[0145] The pre-integration covariance is propagated through the error state transfer matrix F and the noise matrix G:

[0146]

[0147] Q is the covariance matrix of IMU measurement noise.

[0148] Construct an optimization problem that includes IMU pre-integration factors and laser odometer factors:

[0149]

[0150] Among them, the state variables include pose, velocity, and bias. The IMU residual term is defined as:

[0151]

[0152] Where:

[0153] R i : Rotation matrix (attitude) in the world coordinate system, p i : Position in the world coordinate system

[0154] v i : Velocity

[0155] and the time-varying biases of the accelerometer and gyroscope.

[0156] The laser odometry residual adopts the SE(3) constraint:

[0157]

[0158] Where is the laser odometry observation value, is the calibrated extrinsic parameter.

[0159] During calibration, the laser odometry is defaulted to high precision. However, in actual situations, the laser odometry also has the problem of degradation. To prevent large errors in the inertial odometry in the case of laser odometry degradation, an environmental degradation detection index is introduced. Where H is the point cloud matching Hessian matrix. When γ < τ, a large noise covariance is adopted to reduce the observation weight in the degradation scenario, and a normal covariance is adopted in other cases In our scheme, set

[0160]

[0161] The iSAM2 algorithm of the GTSAM optimization library is used for incremental optimization. Its marginalization process can be expressed as:

[0162] p(X k |Z) ∝ ∫p(z|X)p(X k-1 )dX k-1

[0163] X k : State variables at the current moment, including pose R k , p k , v k , IMU bias etc.;

[0164] Z: Observation data, including pre-integrated IMU quantities and lidar pose constraints.

[0165] After optimization, perform bias update and pre-integration re-propagation:

[0166]

[0167] The pre-integrated rotation quantity from time k to k+1, representing the cumulative relative rotation between two frames.

[0168] The raw angular velocity measurements of the gyroscope, including noise and bias.

[0169] δb g : The optimized gyroscope bias correction quantity, the bias residual obtained through iSAM2 optimization.

[0170] Δt: The IMU sampling time interval, usually a fixed value, which is obtained according to the sampling frequency of the IMU.

[0171] Ensure that subsequent IMU integration is based on the latest bias estimate to maintain temporal consistency.

[0172] S5. Use multiple factors to add to the factor graph to optimize the global pose and construct the global map.

[0173] Construct the local map:

[0174] First, use KD-Tree search to find historical key frames within the set radius and extract the key frames, which are used for subsequent point cloud map construction; secondly, calculate the average depth D of each frame of lidar points i , determine the sparsity of the environment where the sensor is located according to the average depth value, and downsample the point cloud according to the sparsity of the environment, retaining more points in a sparse environment and fewer points in a dense environment.

[0175] Scan point optimization:

[0176] Perform pose initialization on the first frame, input the first frame of lidar point cloud P0, and obtain the initial IMU pose q init =[φ i ,θ i ,ψ i T , where represents the initialized pose vector. Let the first frame position f init =[0,0,0] T . Use homogeneous coordinates to represent the initial pose and construct the transformation matrix where R(*) is the Euler angle rotation matrix function.

[0177] ​Perform pose optimization on subsequent frames. The laser point cloud P of the k-th frame k , the pose after optimization of the previous frame The attitude of the IMU

[0178] Calculate the attitude increment ΔR imu = R(q k ) × R(q k-1 ) T .

[0179] Keep the translation vector unchanged and only update the rotation to update the predicted pose

[0180] For each laser point p i ∈ P k , calculate the map projection error:

[0181]

[0182] where m j is the nearest neighbor point in the map, and n j is the corresponding normal vector.

[0183] Add the IMU constraint term

[0184] Optimize the objective function Use the LM algorithm for iterative optimization. Among them, w i is dynamically adjusted according to the point cloud curvature, and λ is the IMU confidence coefficient. Output the optimized laser odometry pose

[0185] Calculate the corner point residual:

[0186] Calculating the corner point residual and the plane point residual is the optimization of subsequent frames. The first frame uses the attitude information of the IMU and the first frame position information f of the radar init = [0, 0, 0] T .

[0187] First, select 5 neighboring points and calculate their centroid Secondly, construct the covariance matrix Perform eigenvalue decomposition on matrix A, A = VDV T , to obtain the line direction v corresponding to the largest eigenvalue; finally, construct the residual formula to calculate the residual:

[0188]

[0189] where p0 is the current point, and p1, p2 are the endpoints of the line.

[0190] Calculate the plane point residual:

[0191] Construct an overdetermined system of equations \(AX = B\), where is the point coordinate matrix, \(B=[-1,\cdots,-1]\) T .

[0192] Use the least-squares solution \(X=(A T A) -1 A T B\) to obtain the plane parameters \([a, b, c, d]\).

[0193] Construct the residual formula:

[0194]

[0195] Use the least-squares method to jointly optimize the residuals:

[0196]

[0197] where \(w i is the weight. Since the far points have a greater impact on the real-time performance and accuracy of the odometer, the weight value of the farther points is larger.

[0198] Pose optimization:

[0199] Adopt L-M optimization. First, obtain the Jacobian matrix of the residual \(e\) with respect to the pose parameters \(T = [\theta x ,\theta y ,\theta z ,t x ,t y ,t z \):

[0200] Then, update the parameters using the following formula

[0201] \(\Delta T=-(J T J+\lambda I) -1 J T e

[0202] Set the initial value of \(\lambda\) to \(1\times10 -3 and gradually decrease it after convergence.

[0203] Use multiple factors to add factor graph optimization for the global pose and construct a global map:

[0204] S51. Add the laser odometer factor and the inertial odometer factor:

[0205] Define the relative pose constraint between adjacent key frames:

[0206]

[0207] where:

[0208] T k ∈ SE(3) is the pose of the key frame k, and ΔT k-1,k is calculated by the front-end odometer;

[0209] The noise model is a diagonal covariance matrix:

[0210] Σ odo = diag(10 -6 , 10 -6 , 10 -6 , 10 -4 , 10 -4 , 10 -4 )

[0211] S52. Add the GPS factor:

[0212] The GPS factor provides absolute position observations:

[0213]

[0214] is the translation vector of the key frame k, and is the GPS measurement value;

[0215] The noise covariance is:

[0216] Σ gps = diag(1.0, 1.0, 2.0) m 2

[0217] S53. Add the loop closure factor:

[0218] The current frame is matched with several historical frames. If the match is successful, calculate the relative position relationship between this frame and the matched history, and calculate the relative pose through ICP matching:

[0219] The loop closure factor constrains the poses of key frames across time:

[0220]

[0221] where, ΔT mn is obtained by ICP matching;

[0222] The noise covariance is adaptively adjusted according to the ICP score:

[0223]

[0224] σ φ = 0.1 rad, σ t = 0.5 m is the reference value;

[0225] The optimization function with each factor added is optimized using the ISAM2 optimization method of the GTSAM optimization library. After optimization, a global map is constructed by splicing, and the effect is as Figure 4 shown.

[0226] The various embodiments in this specification are described in a progressive manner. Each embodiment focuses on the differences from other embodiments. For the same and similar parts among the various embodiments, reference can be made to each other.

[0227] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the specific details in 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 solution of the present invention, and these equivalent transformations all fall within the protection scope of the present invention.

Claims

1. A SLAM method for tightly coupled laser odometry and inertial odometry, characterized in that, It includes the following steps: S1. Receive IMU data and lidar point cloud data, perform pre-integration processing on the IMU data, and compensate for the motion distortion of the lidar frame; S2. Select feature points from the lidar point cloud based on curvature calculation, and distinguish corner points and plane points; S3. Perform uniform filtering on the feature points to optimize the point cloud distribution density; S4. Calculate the pose change matrix of the lidar odometer based on the filtered feature points, and correct the IMU data of the inertial odometer through the pose change matrix; S5. Use multiple factors to add to the factor graph to optimize the global pose and construct a global map.

2. The SLAM method of a tightly coupled laser odometer and an inertial odometer according to claim 1, characterized in that, The specific method of the IMU pre-integration processing and compensation for the motion distortion of the lidar frame described in step S1 includes: Trim and align the IMU data queue to ensure the synchronization of the IMU timestamp and the lidar timestamp; Pre-integrate the IMU data within the time period of the initial lidar frame and the end frame to obtain the change in the IMU pose, thereby correcting the motion distortion of the initial lidar frame and the second frame; Take the attitude of the IMU data closest to the laser start time as the initial value of the laser start time.

3. The SLAM method of a tightly coupled laser odometer and an inertial odometer according to claim 1 or 2, characterized in that, The IMU pre-integration processing described in step S1 includes a recursive initialization process, specifically: Dynamically trigger the initialization state detection module by judging the time alignment between the IMU data cache queue and the current lidar frame; When the uninitialized state is detected, construct a state propagation model under the Lie group Lie algebra framework, use the interpolation algorithm to achieve the spatial alignment of the IMU and the lidar coordinate systems, and synchronously optimize the rotation matrix, translation vector, and bias parameters by constructing a pre-integration observation model including angular velocity deviation and acceleration deviation compensation.

4. The SLAM method of the tightly coupled laser odometer and inertial odometer according to claim 1, characterized in that, The method of selecting feature points from the lidar point cloud based on curvature calculation described in step S2 is: S21. Select an arbitrary point p m,n , and at the same time select several adjacent points on the laser line where the point p m,n is located, and the same number of points at the closest positions of the two laser lines above and below the laser line where p m,n is located, as the sample set for judging whether the selected point p m,n is a feature point; S22. Calculate the curvature of point p m,n : where represents the curvature of point p m,n , d (m,n) is the depth of the current point p m,n , d (m+k,n+l) is the depth of other points in the sample; S23. Set the dynamic noise threshold: V p = α × N × d (m,n) Among them, α is the weight, which is set to 0.1 in this experiment; N represents the number of valid points within the neighborhood; S24. If the curvature of the current point is determined as an outlier. At the same time, several adjacent points before and after the scan line where this point is located are marked as invalid and not added to feature extraction anymore to avoid feature aggregation; those less than the threshold are retained for subsequent feature extraction; At the same time, if the curvature of the remaining lidar points is greater than 5 and the number of nearby valid points is sufficient, it is identified as a corner point; if the curvature is less than 0.1 and the number of nearby valid points is sufficient, it is identified as a plane point. Finally, all the remaining unprocessed points are set as plane points.

5. The SLAM method of the tightly coupled laser odometer and inertial odometer according to claim 1, wherein: The uniform filtering of the feature points described in step S3 is based on adaptive sampling of the spatial grid density analysis and uses the farthest point sampling method to ensure the uniform spatial distribution of the feature points.

6. The SLAM method of a tightly-coupled laser odometer and an inertial odometer according to claim 1, wherein The specific method of calculating the pose change matrix of the lidar odometer based on the filtered feature points in step S4 is: Corner point residual calculation: Select 5 nearest neighbor points and calculate their centroid Construct the covariance matrix Perform eigenvalue decomposition on matrix A, \(A = VDV\) T , to obtain the line direction \(v\) corresponding to the largest eigenvalue; finally, construct the residual formula to calculate the residual: Among them, p0 is the current point, p a , p b is the end point of the line; Plane point residual calculation: Construct an overdetermined system of equations \(AX = B\), where is the point coordinate matrix, and \(B=[-1,\cdots,-1]\) T . Using the least squares solution X = (A T A) -1 A T B, the plane parameters [a, b, c, d] are obtained; Construct the residual formula: Use the least squares method to jointly optimize the residuals: Among them, w i is the weight. The farther points have a greater impact on the real-time performance and accuracy of the odometer. Therefore, the weight value of the farther points is larger; T is the pose change matrix of the lower-level of the laser odometer.

7. The SLAM method of a tightly coupled laser odometer and an inertial odometer according to claim 1, characterized in that: The specific method of correcting the IMU data of the inertial odometer using the optimized feature points in step S4 is: S41. Construct an inertial odometer factor to constrain the pose, velocity, and bias at adjacent times; Where, R represents the rotation matrix, p represents the quaternion of the corresponding pose, v represents the velocity, ba and bg are the accelerometer and gyroscope biases respectively, Δv is the velocity change from k to k+1, and Δt is the time change Δp k+1 is the quaternion representing the pose change from point k to k+1, to the k+1 rotation matrix. S42. Construct a lidar odometer factor to transform the poses of two consecutive lidar odometers into the IMU coordinate system: Construct a residual model wherein is the optimized laser odometer observation value, represents the IMU observation, is the calibrated extrinsic parameter. S43. Align the front and back timestamps of the high-frequency IMU data between two lidar frames, insert the lidar odometer data at both ends of the inertial odometer, correct the high-frequency and low-precision inertial odometer, and finally, re-propagate the corrected inertial odometer to ensure consistency before and after.

8. The SLAM method of a tightly-coupled laser odometer and an inertial odometer according to claim 6, characterized in that: Step S4 uses a dynamic noise adjustment strategy to handle the problem of laser odometer degradation. The specific method is as follows: Judge the degree of environmental degradation through the eigenvalue analysis of the point cloud covariance matrix; When the minimum eigenvalue λ3 < threshold τ, increase the covariance coefficient of the laser odometer noise.

9. The SLAM method of a tightly coupled laser odometer and an inertial odometer according to claim 1, characterized in that, The specific method of step S5 using multiple factors to join the factor graph to optimize the global pose is as follows: The multiple factors include a laser odometer factor, an inertial odometer factor, a GPS factor, and a loop closure factor; S51. Add the laser odometer factor and the inertial odometer factor: Define the relative pose constraint between adjacent key frames: Among them: T k ∈ SE(3) is the pose of the key frame k, and ΔT k-1,k is calculated by the front-end odometer; The noise model is a diagonal covariance matrix: Σ odo = diag(10 -6 , 10 -6 , 10 -6 , 10 -4 , 10 -4 , 10 -4 ) S52. Add the GPS factor: The GPS factor provides absolute position observations: is the translation vector of key frame k, is the GPS measurement value; The noise covariance is: Σ gps = diag(1.0, 1.0, 2.0)m 2 S53. Add the loop closure factor: The current frame is matched with several historical frames. If the match is successful, calculate the relative position relationship between this frame and the matched history, and calculate the relative pose through ICP matching: The loop closure factor constrains the poses of key frames across time: where ΔT mn is obtained by ICP matching; The noise covariance is adaptively adjusted according to the ICP score: σ φ = 0.1 rad, σ t = 0.5 m is taken as the reference value; The global pose optimization method is to optimize using the ISM2 function of GTSAM.

10. The SLAM method of a tightly coupled laser odometer and an inertial odometer according to claim 1, characterized in that: In step S5, the specific pose optimization method also includes the L-M method for optimization.

Images