A LiDAR-IMU calibration algorithm for underground confined spaces

Abstract

The present invention discloses a LiDAR-IMU calibration algorithm for underground confined space, including: original LiDAR point cloud frame segmentation and point cloud distortion correction, merging subframe point clouds and performing spherical projection to generate high-quality intensity images; constructing a point cloud degradation detection module, intelligently detecting underground confined space point cloud degradation, introducing additional constraints to improve the ability of posture estimation; combining LiDAR odometer and IMU data, constructing a residual function, and preliminarily realizing LiDAR and IMU time synchronization; constructing a unified spatiotemporal constraint module, optimizing time offset, initializing the relative posture, accelerometer and gyroscope zero bias between LiDAR and IMU; IMU refined modeling enhanced by reflectivity intensity information, using SFM to calculate intensity image posture and IMU predicted posture to construct an objective function, accurately calibrating gyroscope and accelerometer zero bias, and iteratively optimizing the translation and rotation external parameters of LiDAR / IMU. The present invention can accurately solve IMU bias noise, realize IMU refined modeling, and thus improve the accurate calibration of LiDAR / IMU external parameters in underground confined space.

Description

Technical Field

[0001] The present invention belongs to the cross-technical field of underground space and multi-sensor fusion, and specifically relates to a LiDAR-IMU calibration algorithm for underground confined spaces. Background Art

[0002] High-precision three-dimensional map models of underground space are an important technical means to realize the construction of smart space. In underground confined spaces such as underground parking lots, corridors, tunnels, and mining areas, the space structure is narrow and long, the light is dim, and there are many obstacles. It is difficult to quickly obtain three-dimensional information of underground space using traditional methods such as levels and total stations, and it is difficult to adapt to the current underground space modeling and equipment operation and maintenance requirements. Based on the simultaneous positioning and mapping technology (SLAM) of laser point clouds, high-precision three-dimensional maps in complex spaces can be constructed in real time and efficiently. However, the point clouds in underground confined spaces are unevenly distributed and have similar structures, which can easily lead to the degradation of laser point clouds. Inertial measurement units (IMUs) can provide high-precision pose estimation to compensate for the degradation of point cloud features of laser radar in underground confined spaces. Accurately calibrate the external parameters between LiDAR and IMU, unify heterogeneous data into the same space-time coordinate system, promote laser inertial SLAM data fusion, and significantly improve the quality of environmental modeling.

[0003] At present, the external parameter calibration of laser inertial SLAM system is mostly completed in the preparation stage before the equipment is put into use, and it is assumed that the external parameters between sensors will not change during the long-term operation. However, in a complex working environment, due to the influence of uncertain factors such as external impact, mechanical structure adjustment or cumulative deformation under long-term operation, the initial external parameters are prone to failure. To solve these problems, domestic and foreign scholars have conducted a lot of research and proposed LiDAR / IMU online calibration algorithms to correct the spatial external parameter deviation between sensors in a timely manner to ensure the continuous and stable operation of the SLAM system. However, the current online calibration methods all ignore the precise calibration of IMU zero bias and only use IMU zero bias as the calibration parameter to be determined. At the same time, the geometric features in the underground confined space are single and the point cloud features are severely degraded. It is difficult to extract the environmental plane to constrain the IMU zero bias. The IMU zero bias error gradually accumulates over time, which will cause the IMU trajectory to drift, thereby seriously reducing the accuracy of LiDAR / IMU external parameter calibration. Summary of the invention

[0004] The purpose of the present invention is to provide a LiDAR-IMU calibration algorithm for underground confined spaces, which can accurately solve the IMU bias noise and realize IMU refined modeling, thereby improving the accurate calibration of LiDAR / IMU extrinsic parameters in underground confined spaces.

[0005] To achieve the above objectives, the present invention provides a LiDAR-IMU calibration algorithm for underground confined spaces, comprising the following steps:

[0006] S1: The original LiDAR point cloud frame segmentation and point cloud distortion correction improve data quality, and spherical projection is performed after merging the sub-frame point clouds to generate high-quality intensity images;

[0007] S2: Build a point cloud degradation detection module to detect point cloud degradation in underground confined spaces and introduce additional constraints to improve the accuracy of pose estimation and map construction;

[0008] S3: Based on the cross-correlation method, the LiDAR odometer and IMU data are combined to construct a residual function, and the time synchronization of LiDAR and IMU is preliminarily achieved;

[0009] S4: Build a unified spatiotemporal constraint module, optimize time offset, initialize the relative pose between LiDAR and IMU, and the accelerometer and gyroscope zero bias;

[0010] S5: IMU refined modeling with enhanced reflectivity intensity information, using SFM to calculate the intensity image pose and IMU predicted pose to construct the objective function, accurately calibrate the gyroscope and accelerometer zero bias, and iteratively optimize the translation and rotation extrinsic parameters of LiDAR / IMU.

[0011] As a further solution of the present invention: step S1 comprises:

[0012] S101: First, the original point cloud frame is segmented into multiple sub-frames, and the timestamp ρ is calculated based on the uniform speed model. j ∈(t k ,t k+1 ) to the LiDAR scan end frame L k+1 Interval Δt j The relative position change within t k is the start time of the kth frame laser radar, t k+1 The k+1th frame laser radar end time

[0013] The formula is:

[0014] Δt j =t k+1 -ρ j

[0015]

[0016] in, and are the rotation and translation changes calculated based on the uniform velocity model. The exponential mapping Exp is used to transform the Lie algebra SE(3) to the Lie group SO(3). They are LiDAR scanning frames L k+1 The predicted angular velocity and linear velocity, represents the rotation of the k+1 frame lidar in the global coordinate system;

[0017] S102: LiDAR measurements Projected to the laser radar scanning end frame L k+1 The coordinates after correcting the point cloud distortion are for:

[0018]

[0019] S103: The corrected point cloud subframes are merged and spherical projected to generate an intensity image. The formula of the intensity image coordinates (u, ν) corresponding to each lidar point P (x, y, z) is:

[0020] u=(π-atan2(y,x)) / (2π / w)

[0021]

[0022] Where: w and h are the width and height of the intensity image, respectively, foν max and foν min They are the maximum and minimum vertical angles of LiDAR, respectively.

[0023] As a further solution of the present invention: step S2 comprises:

[0024] S201: Laser odometer status X = [ G R L , G P L , G v L ,w L ] T , the predicted state at time k+1 and covariance for:

[0025]

[0026] in: G R L , G P L , G v L 、w L They are the attitude, position, and velocity of the laser radar in the global coordinate system and the angular velocity in the radar coordinate system. Update the value for the laser odometry, is the “addition” on the state manifold, the discrete state transfer function f(X,W)=[w L , G v L ,nv ,n w ] T , P and Q are the covariance matrix of state estimation and process noise respectively, W = [n v ,n w ] T , n v and n w are the Gaussian noise corresponding to the linear velocity and angular velocity respectively, W is n v With n w The noise vector F W is the coefficient matrix after linearization of the noise model, and F W as follows:

[0027]

[0028] in: is the predicted angular velocity in the LiDAR coordinate system, Δt is the time interval between two frames;

[0029] S202: LiDAR odometer uses the surface element measurement model to fuse the scan frame to the map matching point to plane residual r l Construct the observation equation:

[0030]

[0031] in: is the IMU pose in the current global coordinate system, the transformation matrix of LiDAR relative to IMU I T L , when calibration is not completed I T L is the unit matrix, the point in the LiDAR coordinate system L P j Switch to global map frame G P j Search for the nearest point in the local plane to get the normal u j and the point q in the plane j , for u j The transposed matrix of

[0032] S203: Re-state the point-surface residual in S202 as a quadratic cost optimization problem, and construct the Hessian matrix A′. Use the SVD algorithm to calculate A r ' r and A t ' t The matrix is ​​analyzed and the characteristic matrix is ​​decomposed into A t ' t =V t ∑t V t T , application information pair (p i ,n i )Analyze the rotation vector V r and translation vector V t Constraints on the laser odometry, detecting the localizable vector Thus, the degradation of the LiDAR point cloud is reflected;

[0033]

[0034] Where: {Lv t1 , Lv t2 , Lv t3}∈V t ,{Lv r1 , Lv r2 , Lv r3}∈V r ,Ω j ∈{all, part, none} degenerate cases, n i is the midpoint p of the scan frame i Search for the nearest local normal of the point in the transformed point cloud map, where N is the number of information pairs in each frame and × is the vector cross product;

[0035] S204: When the detection point cloud has feature degradation, the point cloud with the largest pose constraint is resampled and the Lagrange multiplier method is used to construct an additional constraint equation to constrain the ΔX component of the laser odometer state δX to update:

[0036]

[0037] G=ΣC T (CΣC T ) -1

[0038]

[0039] Among them: r0 and t0 are the rotation and translation posture values ​​when the point cloud does not degenerate, v j is the matrix V t or V r The corresponding column vector, (m r +m t )≤6 and m r and m t They represent the degenerate direction counts in the rotation and translation space, ΔX is the rotation increment δr and the translation increment δt, and the symbol * is the vector dot product. is the coefficient matrix after linearization.

[0040] As a further solution of the present invention: step S3 comprises:

[0041] S301: using a zero phase filter to remove measurement noise in the original measurement data, and performing downsampling linear interpolation on the IMU data;

[0042] S302: Quantifying LiDAR Angular Velocity Using Cross-Correlation And the downsampled IMU angular velocity The similarity between trajectories is used to construct a nonlinear optimization problem to estimate the time offset t within the laser odometry index range. d :

[0043]

[0044] Where: ||w|| is the modulus of vector w.

[0045] As a further solution of the present invention: step S4 comprises:

[0046] S401: LiDAR angular velocity obtained from S2 and line speed Data and angular velocity of IMU Perform central difference interpolation processing to obtain LiDAR angular acceleration and linear acceleration Angular acceleration with IMU

[0047] S402: LiDAR and IMU unified spatiotemporal constraints consist of the following two constraints:

[0048] Initialization time offset t d , the angular velocity of the IMU Angular acceleration With LiDAR Common composition constraint 1

[0049]

[0050] The corresponding residual r w for:

[0051]

[0052] in: represent The transpose of b g They are the LiDAR to IMU rotation external parameters and IMU gyroscope zero bias to be determined respectively;

[0053] IMU measurement data With LiDAR Composition constraint 2, its formula is:

[0054]

[0055] The corresponding residual r a for:

[0056]

[0057] in: b a are the LiDAR to IMU translation extrinsic parameters and IMU accelerometer zero bias to be determined, respectively. × Represented as the antisymmetric matrix corresponding to vector a;

[0058] S403: Constraint 1 and constraint 2 together constitute the unified spatiotemporal constraint between LiDAR and IMU, which is used to solve the calibration parameter x calib :

[0059]

[0060] Among them: the calibration parameter x to be determined calib ={ I R L , I P L ,t d ,b a ,b g}, and is the covariance matrix corresponding to the residual rw and ra, ρ w and ρ a is assigned to the residual r w and r a The Cauchy kernel function.

[0061] As a further solution of the present invention: step S5 comprises:

[0062] S501: extracting stable feature points in the intensity image using the ORB algorithm, and then obtaining the position and posture of the intensity image frame based on the SFM algorithm;

[0063] S502: Taking into account the gyroscope zero bias b in the IMU original measurement data g and accelerometer bias b a It will seriously affect the IMU trajectory estimation accuracy. The IMU is refined and modeled to obtain high-precision IMU posture trajectory. The IMU refined modeling mainly includes gyroscope zero bias b g Refinement, scale factor s and gravity vector g w Initialization, scale factor s, acceleration bias b a and gravity vector refinement;

[0064] S503: Based on the inconsistent results between the SFM calculated intensity image frame attitude increment and the IMU predicted attitude increment, a nonlinear residual function is constructed to accurately solve the gyroscope zero bias b g :

[0065]

[0066] in: Represents the IMU attitude prediction result between two adjacent intensity image frames, is the posture of the k-th frame intensity image in the world coordinate system, which can be obtained through SFM; since the system is a rigid body motion and the intensity image is generated by the LiDAR point cloud projection, at the k-th frame image Value and are equal, N represents the number of intensity image frames, Log(·): is the logarithmic mapping used to transform the rotation matrix from the Lie group SO(3) space to the Lie algebra SE(3) space;

[0067] S504: The gyro bias b obtained in S503 is g Substituting into the IMU pre-integration, the posture of the IMU data integration result between two consecutive intensity image frames at time k~k+1 in the world coordinate system is speed and location It can be obtained by the following formula:

[0068]

[0069] Where: Δt k,k+1 is the time interval between two adjacent frames from k to k+1, Represents the integrated value of the gyroscope between adjacent frames, are the first and second integral values ​​of the accelerometer between adjacent frames, Represents the gyro bias b g The Jacobian for the rotation and position errors is, is the accelerometer bias b a The relevant rotational and positional error Jacobians;

[0070] According to the conversion equation between the image coordinate system and the IMU coordinate system: The IMU data is used to calculate the position of the k+1th frame intensity image in world coordinates

[0071]

[0072] Where: s is the scale factor of the intensity image. Since the intensity image is generated from the point cloud, is the pose of the intensity image in the world coordinate system, is the posture of the k-th frame intensity image in the world coordinate system, is the posture of the k+1th frame intensity image in the world coordinate system, is the pose of the k-th frame intensity image in the world coordinate system, and the IMU coordinate system is transformed to the intensity image coordinate system is the translation parameter between LiDAR and IMU

[0073] The ratio between adjacent intensity image frames Results difference elimination speed And use SVD decomposition to initialize the gravity vector g w and scale factor s;

[0074] S505: The magnitude and direction of gravity acceleration on Earth are known, and a priori constraints on the gravity vector, i.e., the direction vector of gravity, can be provided. In the inertial coordinate system, The magnitude of gravity acceleration is G, and the gravity vector g in the world coordinate system w The gravity vector is converted to the inertial coordinate system:

[0075]

[0076] in The rotation matrix from the inertial coordinate system to the world coordinate system is:

[0077]

[0078] Since the IMU yaw angle is not observable, it can only align the Z axis of the world coordinate system with the gravity direction. Therefore, only the perturbation angles δθ of the x and y axes are used for parameterization and constrained optimization as follows:

[0079]

[0080] Where: g w The first-order approximation is Convert it to the intensity image coordinate system:

[0081]

[0082] The intensity images between three consecutive key frames The result is subtracted to eliminate the influence of velocity variables, and SVD is used to accurately solve δθ and acceleration zero bias b a , refine the scale factor s and gravity vector g w , is the displacement increment between adjacent frames in the IMU coordinate system;

[0083] S506: The angular velocity zero bias b obtained by optimization is gand acceleration bias b a Substitute into the S4 unified space-time constraint and optimize x through multiple iterations calib ={ I R L , I P L ,t d ,b a ,b g}.

[0084] Compared with the prior art, the present invention has the following beneficial effects:

[0085] A degradation detection module is constructed to constrain the update of the laser odometry to achieve robust and stable laser odometry in underground confined spaces. At the same time, the IMU zero bias is fully considered, and the IMU refined modeling with intensity information enhancement is innovatively introduced in the LiDAR / IMU calibration process. Additional constraints are introduced, that is, the SFM is used to estimate the intensity image pose and the IMU predicted pose to construct additional residual equations to accurately solve the IMU bias noise, accurately estimate the IMU zero bias, and provide a higher precision IMU trajectory, thereby achieving robust and accurate LiDAR / IMU extrinsic calibration in underground confined spaces. BRIEF DESCRIPTION OF THE DRAWINGS

[0086] Figure 1 This is a flow chart of a LiDAR-IMU calibration algorithm for underground confined spaces according to the present invention;

[0087] Figure 2 The figure is a flow chart of a LiDAR-IMU calibration algorithm for underground confined spaces according to the present invention. DETAILED DESCRIPTION

[0088] The present invention will be further described below by way of examples.

[0089] like Figure 1 and Figure 2 As shown in the figure, a LiDAR-IMU calibration algorithm for underground confined spaces includes the following steps:

[0090] S1: The original LiDAR point cloud frame segmentation and point cloud distortion correction improve the data quality. After merging the sub-frame point clouds, spherical projection is performed to generate a high-quality intensity image. The specific steps include:

[0091] S101: First, the original point cloud frame is segmented into multiple sub-frames, and the timestamp ρ is calculated based on the uniform speed model. j ∈(t k ,t k+1 ) to the LiDAR scan end frame L k+1 Interval Δt j The relative position change within tk is the start time of the kth frame laser radar, t k+1 The k+1th frame laser radar end time

[0092] The formula is:

[0093] Δt j =t k+1 -ρ j

[0094]

[0095] in, and are the rotation and translation changes calculated based on the uniform velocity model. The exponential mapping Exp is used to transform the Lie algebra SE(3) to the Lie group SO(3). They are LiDAR scanning frames L k+1 The predicted angular velocity and linear velocity, represents the rotation of the k+1 frame lidar in the global coordinate system;

[0096] S102: LiDAR measurements Projected to the laser radar scanning end frame L k+1 The coordinates after correcting the point cloud distortion are for:

[0097]

[0098] S103: The corrected point cloud subframes are merged and spherical projection is performed to generate an intensity image. The formula of the intensity image coordinates (u, ν) corresponding to each lidar point P (x, y, z) is:

[0099] u=(π-atan2(y,x)) / (2π / w)

[0100]

[0101] Where: w and h are the width and height of the intensity image, respectively, foν max and foν min They are the maximum and minimum vertical angles of LiDAR respectively;

[0102] S2: Build a point cloud degradation detection module to intelligently detect point cloud degradation in underground confined spaces, introduce additional constraints to improve the pose estimation capability, and significantly improve the pose estimation and map construction accuracy. The specific steps include:

[0103] S201: Laser odometer status X = [ G R L , G P L ,G v L ,w L ] T , the predicted state at time k+1 and covariance for:

[0104]

[0105] in: G R L , G P L , G v L 、w L They are the attitude, position, and velocity of the laser radar in the global coordinate system and the angular velocity in the radar coordinate system. Update the value for the laser odometry, is the “addition” on the state manifold, the discrete state transfer function f(X,W)=[w L , G v L ,n v ,n w ] T , P and Q are the covariance matrix of state estimation and process noise respectively, W = [n v ,n w ] T , n v and n w are the Gaussian noise corresponding to the linear velocity and angular velocity respectively, W is n v With n w The noise vector F W is the coefficient matrix after linearization of the noise model, and F W as follows:

[0106]

[0107] in: is the predicted angular velocity in the LiDAR coordinate system, Δt is the time interval between two frames;

[0108] S202: LiDAR odometer uses the surface element measurement model to fuse the scan frame to the map matching point to plane residual r l Construct the observation equation:

[0109]

[0110] in: is the IMU pose in the current global coordinate system, the transformation matrix of LiDAR relative to IMU I T L, when calibration is not completed I T L is the unit matrix, the point in the LiDAR coordinate system L P j Switch to global map frame G P j Search for the nearest point in the local plane to get the normal u j and the point q in the plane j , for u j The transposed matrix of

[0111] S203: Re-state the point-surface residual in S202 as a quadratic cost optimization problem, and construct the Hessian matrix A′. Use the SVD algorithm to calculate A r ' r and A t ' t The matrix is ​​analyzed and the characteristic matrix is ​​decomposed into A t ' t =V t ∑ t V t T , application information pair (p i ,n i )Analyze the rotation vector V r and translation vector V t Constraints on the laser odometry, detecting the localizable vector Thus, the degradation of the LiDAR point cloud is reflected;

[0112]

[0113] Where: {Lv t1 , Lv t2 , Lv t3}∈V t ,{Lv r1 , Lv r2 , Lv r3}∈V r ,Ω j ∈{all, part, none} degenerate cases, n i is the midpoint p of the scan frame i Search for the nearest local normal of the point in the transformed point cloud map, where N is the number of information pairs in each frame and × is the vector cross product;

[0114] S204: When the detection point cloud has feature degradation, the point cloud with the largest pose constraint is resampled and the Lagrange multiplier method is used to construct an additional constraint equation to constrain the ΔX component of the laser odometer state δX to update:

[0115]

[0116] G=ΣC T (CΣC T ) -1

[0117]

[0118] Among them: r0 and t0 are the rotation and translation posture values ​​when the point cloud does not degenerate, v j is the matrix V t or V r The corresponding column vector, (m r +m t )≤6 and m r and m t They represent the degenerate direction counts in the rotation and translation space, ΔX is the rotation increment δr and the translation increment δt, and the symbol * is the vector dot product. is the coefficient matrix after linearization;

[0119] S3: Based on the cross-correlation method, the LiDAR odometer and IMU data are combined to construct a residual function and preliminarily achieve the time synchronization between LiDAR and IMU. The specific steps include:

[0120] S301: using a zero phase filter to remove measurement noise in the original measurement data, and performing downsampling linear interpolation on the IMU data;

[0121] S302: Quantifying LiDAR Angular Velocity Using Cross-Correlation And the downsampled IMU angular velocity The similarity between trajectories is used to construct a nonlinear optimization problem to estimate the time offset t within the laser odometry index range. d :

[0122]

[0123] Where: ||w|| is the modulus of vector w;

[0124] S4: Build a unified spatiotemporal constraint module, optimize time offset, initialize the relative pose between LiDAR and IMU, and the accelerometer and gyroscope zero bias. The specific steps include:

[0125] S401: LiDAR angular velocity obtained from S2 and line speed Data and angular velocity of IMU Perform central difference interpolation processing to obtain LiDAR angular acceleration and linear acceleration Angular acceleration with IMU

[0126] S402: LiDAR and IMU unified spatiotemporal constraints consist of the following two constraints:

[0127] Initialization time offset t d , the angular velocity of the IMU Angular acceleration With LiDAR Common composition constraint 1

[0128]

[0129] The corresponding residual r w for:

[0130]

[0131] in: represent The transpose of b g They are the LiDAR to IMU rotation external parameters and IMU gyroscope zero bias to be determined respectively;

[0132] IMU measurement data With LiDAR Composition constraint 2, its formula is:

[0133]

[0134] The corresponding residual r a for:

[0135]

[0136] in: b a are the LiDAR to IMU translation extrinsic parameters and IMU accelerometer zero bias to be determined, respectively. × Represented as the antisymmetric matrix corresponding to vector a;

[0137] S403: Constraint 1 and constraint 2 together constitute the unified spatiotemporal constraint between LiDAR and IMU, which is used to solve the calibration parameter x calib :

[0138]

[0139] Among them: the calibration parameter x to be determined calib ={ I R L , I P L ,t d ,b a ,b g}, and is the covariance matrix corresponding to the residual rw and ra, ρ w and ρ a is assigned to the residual r w and r a The Cauchy kernel function of ;

[0140] S5: IMU refined modeling with enhanced reflectivity intensity information, innovatively using SFM to calculate the intensity image pose and IMU predicted pose to construct the objective function, accurately calibrate the gyroscope and accelerometer zero bias, and iteratively optimize the translation and rotation external parameters of LiDAR / IMU. The specific steps include:

[0141] S501: extracting stable feature points in the intensity image using the ORB algorithm, and then obtaining the position and posture of the intensity image frame based on the SFM algorithm;

[0142] S502: Taking into account the gyroscope zero bias b in the IMU original measurement data g and accelerometer bias b a It will seriously affect the IMU trajectory estimation accuracy. The IMU is refined and modeled to obtain high-precision IMU posture trajectory. The IMU refined modeling mainly includes gyroscope zero bias b g Refinement, scale factor s and gravity vector g w Initialization, scale factor s, acceleration bias b a and gravity vector refinement;

[0143] S503: Based on the inconsistent results between the SFM calculated intensity image frame attitude increment and the IMU predicted attitude increment, a nonlinear residual function is constructed to accurately solve the gyroscope zero bias b g :

[0144]

[0145] in: Represents the IMU attitude prediction result between two adjacent intensity image frames, is the posture of the k-th frame intensity image in the world coordinate system, which can be obtained through SFM; since the system is a rigid body motion and the intensity image is generated by the LiDAR point cloud projection, at the k-th frame image Value and are equal, N represents the number of intensity image frames, Log(·): is the logarithmic mapping used to transform the rotation matrix from the Lie group SO(3) space to the Lie algebra SE(3) space;

[0146] S504: The gyro bias b obtained in S503 is g Substituting into the IMU pre-integration, the posture of the IMU data integration result between two consecutive intensity image frames at time k~k+1 in the world coordinate system is speed and location It can be obtained by the following formula:

[0147]

[0148] Where: Δt k,k+1 is the time interval between two adjacent frames from k to k+1, Represents the integrated value of the gyroscope between adjacent frames, are the first and second integral values ​​of the accelerometer between adjacent frames, Represents the gyro bias b g The Jacobian for the rotation and position errors is, is the accelerometer bias b a The relevant rotational and positional error Jacobians;

[0149] According to the conversion equation between the image coordinate system and the IMU coordinate system: The IMU data is used to calculate the position of the k+1th frame intensity image in world coordinates

[0150]

[0151] Where: s is the scale factor of the intensity image. Since the intensity image is generated from the point cloud, is the pose of the intensity image in the world coordinate system, is the posture of the k-th frame intensity image in the world coordinate system, is the posture of the k+1th frame intensity image in the world coordinate system, is the pose of the k-th frame intensity image in the world coordinate system, and the IMU coordinate system is transformed to the intensity image coordinate system is the translation parameter between LiDAR and IMU

[0152] The ratio between adjacent intensity image frames Results difference elimination speed And use SVD decomposition to initialize the gravity vector g w and scale factor s;

[0153] S505: The magnitude and direction of gravity acceleration on Earth are known, and a priori constraints on the gravity vector, i.e., the direction vector of gravity, can be provided. In the inertial coordinate system, The magnitude of gravity acceleration is G, and the gravity vector g in the world coordinate system w The gravity vector is converted to the inertial coordinate system:

[0154]

[0155] in The rotation matrix from the inertial coordinate system to the world coordinate system is:

[0156]

[0157] Since the IMU yaw angle is not observable, it can only align the Z axis of the world coordinate system with the gravity direction. Therefore, only the perturbation angles δθ of the x and y axes are used for parameterization and constrained optimization as follows:

[0158]

[0159] Where: g w The first-order approximation is Convert it to the intensity image coordinate system:

[0160]

[0161] The intensity images between three consecutive key frames The result is subtracted to eliminate the influence of velocity variables, and SVD is used to accurately solve δθ and acceleration zero bias b a , refine the scale factor s and gravity vector g w , is the displacement increment between adjacent frames in the IMU coordinate system;

[0162] S506: The angular velocity zero bias b obtained by optimization is g and acceleration bias b a Substitute into the S4 unified space-time constraint and optimize x through multiple iterations calib ={ I R L , I P L ,t d ,b a ,b g}.

Claims

1. A LiDAR-IMU calibration algorithm for underground confined spaces, characterized in that: It includes the following steps: S1: The original LiDAR point cloud frame segmentation and point cloud distortion correction improve data quality, and spherical projection is performed after merging the sub-frame point clouds to generate high-quality intensity images; S2: Build a point cloud degradation detection module to detect point cloud degradation in underground confined spaces and introduce additional constraints to improve the accuracy of pose estimation and map construction; S3: Based on the cross-correlation method, the LiDAR odometer and IMU data are combined to construct a residual function, and the time synchronization of LiDAR and IMU is preliminarily achieved; S4: Build a unified spatiotemporal constraint module, optimize time offset, initialize the relative pose between LiDAR and IMU, and the accelerometer and gyroscope zero bias; S5: IMU refined modeling with enhanced reflectivity intensity information, using SFM to calculate the intensity image pose and IMU predicted pose to construct the objective function, accurately calibrate the gyroscope and accelerometer zero bias, and iteratively optimize the translation and rotation extrinsic parameters of LiDAR / IMU.

2. The LiDAR-IMU calibration algorithm for underground confined spaces according to claim 1, characterized in that: Step S1 includes: S101: First, the original point cloud frame is segmented into multiple sub-frames, and the timestamp ρ is calculated based on the uniform speed model. j ∈(t k ,t k+1 ) to the LiDAR scan end frame L k+1 Interval Δt j The relative position change within t k is the start time of the kth frame laser radar, t k+1 The k+1th frame laser radar end time The formula is: Δt j =t k+1 -r j in, and are the rotation and translation changes calculated based on the uniform velocity model. The exponential mapping Exp is used to transform the Lie algebra SE(3) to the Lie group SO(3). They are LiDAR scanning frames L k+1 The predicted angular velocity and linear velocity, represents the rotation of the k+1 frame lidar in the global coordinate system; S102: LiDAR measurements Projected to the laser radar scanning end frame L k+1 The coordinates after correcting the point cloud distortion are for: S103: The corrected point cloud subframes are merged and spherical projection is performed to generate an intensity image. The formula of the intensity image coordinates (u, ν) corresponding to each lidar point P (x, y, z) is: u=(π-atan2(y,x)) / (2π / w) Where: w and h are the width and height of the intensity image, respectively, foν max and foν min They are the maximum and minimum vertical angles of LiDAR respectively.

3. The LiDAR-IMU calibration algorithm for underground confined spaces according to claim 2, characterized in that: Step S2 includes: S201: Laser odometer status X = [ G R L , G P L , G v L ,w L ] T , the predicted state at time k+1 and covariance for: in:, G P L , G v L 、w L They are the attitude, position, and velocity of the laser radar in the global coordinate system and the angular velocity in the radar coordinate system. Update the value for the laser odometry, is the "addition" on the state manifold, the discrete state transfer function f(X,W)=[w L , G v L ,n v ,n w ] T , P and Q are the covariance matrix of state estimation and process noise respectively, W = [n v ,n w ] T , n v and n w are the Gaussian noise corresponding to the linear velocity and angular velocity respectively, W is n v With n w The noise vector F W is the coefficient matrix after linearization of the noise model, and F W as follows: in: is the predicted angular velocity in the LiDAR coordinate system, Δt is the time interval between two frames; S202: LiDAR odometer uses the surface element measurement model to fuse the scan frame to the map matching point to plane residual r l Construct the observation equation: in: is the IMU pose in the current global coordinate system, the transformation matrix of LiDAR relative to IMU I T L , when calibration is not completed I T L is the unit matrix, the point in the LiDAR coordinate system L P j Switch to global map frame G P j Search for the nearest point in the local plane to get the normal u j and the point q in the plane j , for u j The transposed matrix of S203: Re-state the point-surface residual in S202 as a quadratic cost optimization problem, and construct the Hessian matrix A′. Use the SVD algorithm to calculate A′ rr and A′ tt The matrix is ​​analyzed and the characteristic matrix is ​​decomposed into A′ tt =V t ∑ t V t T , application information pair (p i ,n i )Analyze the rotation vector V r and translation vector V t Constraints on the laser odometry, detecting the localizable vector Thus, the degradation of the LiDAR point cloud is reflected; Where: {Lv t1 , Lv t2 , Lv t3 }∈V t ,{Lv r1 , Lv r2 , Lv r3 }∈V r ,Ω j ∈{all, part, none} degenerate cases, n i is the midpoint p of the scan frame i Search for the nearest local normal of the point in the transformed point cloud map, where N is the number of information pairs in each frame and × is the vector cross product; S204: When the detection point cloud has feature degradation, the point cloud with the largest pose constraint is resampled and the Lagrange multiplier method is used to construct an additional constraint equation to constrain the ΔX component of the laser odometer state δX to update: G=ΣC T (CΣC T ) -1 Among them: r0 and t0 are the rotation and translation posture values ​​when the point cloud does not degenerate, v j is the matrix V t or V r The corresponding column vector, (m r +m t )≤6 and m r and m t They represent the degenerate direction counts in the rotation and translation space, ΔX is the rotation increment δr and the translation increment δt, and the symbol * is the vector dot product. is the coefficient matrix after linearization.

4. The LiDAR-IMU calibration algorithm for underground confined spaces according to claim 3, characterized in that: Step S3 includes: S301: using a zero phase filter to remove measurement noise in the original measurement data, and performing downsampling linear interpolation on the IMU data; S302: Quantifying LiDAR Angular Velocity Using Cross-Correlation And the downsampled IMU angular velocity The similarity between trajectories is used to construct a nonlinear optimization problem to estimate the time offset t within the laser odometry index range. d : Where: ||w|| is the modulus of vector w.

5. The LiDAR-IMU calibration algorithm for underground confined spaces according to claim 3 or 4, characterized in that: Step S4 includes: S401: LiDAR angular velocity obtained from S2 and line speed Data and angular velocity of IMU Perform central difference interpolation processing to obtain LiDAR angular acceleration and linear acceleration Angular acceleration with IMU S402: LiDAR and IMU unified spatiotemporal constraints consist of the following two constraints: Initialization time offset t d , the angular velocity of the IMU Angular acceleration With LiDAR Common composition constraint 1 The corresponding residual r w for: in: represent The transpose of b g They are the LiDAR to IMU rotation external parameters and IMU gyroscope zero bias to be determined respectively; IMU measurement data With LiDAR Composition constraint 2, its formula is: The corresponding residual r a for: in: b a are the LiDAR to IMU translation extrinsic parameters and IMU accelerometer zero bias to be determined, respectively. × Represented as the antisymmetric matrix corresponding to vector a; S403: Constraint 1 and constraint 2 together constitute the unified spatiotemporal constraint between LiDAR and IMU, which is used to solve the calibration parameter x calib : Among them: the calibration parameter x to be determined calib ={ I R L , I P L ,t d ,b a ,b g }, and is the covariance matrix corresponding to the residual rw and ra, ρ w and ρ a is assigned to the residual r w and r a The Cauchy kernel function.

6. The LiDAR-IMU calibration algorithm for underground confined spaces according to claim 5, characterized in that: Step S5 includes: S501: extracting stable feature points in the intensity image using the ORB algorithm, and then obtaining the position and posture of the intensity image frame based on the SFM algorithm; S502: Taking into account the gyroscope zero bias b in the IMU original measurement data g and accelerometer bias b a It will seriously affect the IMU trajectory estimation accuracy. The IMU is refined and modeled to obtain high-precision IMU posture trajectory. The IMU refined modeling mainly includes gyroscope zero bias b g Refinement, scale factor s and gravity vector g w Initialization, scale factor s, acceleration bias b a and gravity vector refinement; S503: Based on the inconsistent results between the SFM calculated intensity image frame attitude increment and the IMU predicted attitude increment, a nonlinear residual function is constructed to accurately solve the gyroscope zero bias b g : in: Represents the IMU attitude prediction result between two adjacent intensity image frames, is the posture of the k-th frame intensity image in the world coordinate system, which can be obtained through SFM; since the system is a rigid body motion and the intensity image is generated by the LiDAR point cloud projection, at the k-th frame image Value and are equal, N represents the number of intensity image frames, Log(·): is the logarithmic mapping used to transform the rotation matrix from the Lie group SO(3) space to the Lie algebra SE(3) space; S504: The gyro bias b obtained in S503 is g Substituting into the IMU pre-integration, the posture of the IMU data integration result between two consecutive intensity image frames at time k~k+1 in the world coordinate system is speed and location It can be obtained by the following formula: Where: Δt k,k+1 is the time interval between two adjacent frames from k to k+1, Represents the integrated value of the gyroscope between adjacent frames, are the first and second integral values ​​of the accelerometer between adjacent frames, Represents the gyro bias b g The Jacobian for the rotation and position errors is, is the accelerometer bias b a The relevant rotational and positional error Jacobians; According to the conversion equation between the image coordinate system and the IMU coordinate system: The IMU data is used to calculate the position of the k+1th frame intensity image in world coordinates Where: s is the scale factor of the intensity image. Since the intensity image is generated from the point cloud, is the pose of the intensity image in the world coordinate system, is the posture of the k-th frame intensity image in the world coordinate system, is the posture of the k+1th frame intensity image in the world coordinate system, is the pose of the k-th frame intensity image in the world coordinate system, and the IMU coordinate system is transformed to the intensity image coordinate system is the translation parameter between LiDAR and IMU The ratio between adjacent intensity image frames The result is to eliminate the speed of difference And use SVD decomposition to initialize the gravity vector g w with scale factor s; S505: The magnitude and direction of gravity acceleration on Earth are known, and a priori constraints on the gravity vector, i.e., the direction vector of gravity, can be provided. In the inertial coordinate system, The magnitude of gravity acceleration is G, and the gravity vector g in the world coordinate system w The gravity vector is converted to the inertial coordinate system: in The rotation matrix from the inertial coordinate system to the world coordinate system is: Since the IMU yaw angle is not observable, it can only align the Z axis of the world coordinate system with the gravity direction. Therefore, only the perturbation angles δθ of the x and y axes are used for parameterization and constrained optimization as follows: Where: g w The first-order approximation is Convert it to the intensity image coordinate system: The intensity images between three consecutive key frames The result is subtracted to eliminate the influence of velocity variables, and SVD is used to accurately solve δθ and acceleration zero bias b a , refine the scale factor s and gravity vector g w , is the displacement increment between adjacent frames in the IMU coordinate system; S506: The angular velocity zero bias b obtained by optimization is g and acceleration bias b a Substitute into the S4 unified space-time constraint and optimize x through multiple iterations calib ={ I R L , I P L ,t d ,b a ,b g }.

Images