Robust Tight Coupling System of Solid-State LiDAR and IMU Assisted by Point Cloud Intensity

Abstract

The present invention proposes a robust tight-coupling system of a solid-state lidar and an IMU assisted by point cloud intensity. First, the system preprocesses the solid-state lidar point cloud, removes the defective points of the point cloud, corrects the point cloud intensity values and downsamples them. Then, geometric feature extraction and intensity feature extraction are performed on the preprocessed point cloud, and intensity calibration of the lidar point cloud is carried out. Subsequently, the high-frequency prior information predicted by the IMU is used to compensate for the movement of the point cloud. For the geometric and intensity feature point clouds, classical registration and fast plane fitting methods are respectively used to calculate the residuals, and a point cloud residual measurement model assisted by intensity features is constructed. And the robust strategy based on the IGGⅢ robust weight function is designed by using the residual estimation value in the filtering algorithm. The present invention improves the positioning and mapping capabilities and robustness of the small-view lidar tight-coupling system in complex environments while maintaining a low computational cost.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation and positioning, and specifically to a robust tightly-coupled system of a solid-state lidar and an IMU assisted by point cloud intensity. Background Art

[0002] The lidar odometry calculation method is an indispensable part of the lidar SLAM system, and the performance of the odometer will directly affect the system positioning and mapping accuracy. Although excellent 3D lidar odometry calculation method frameworks such as Lego Loam have demonstrated good performance on various data sets, the low sensor frequency of a single lidar odometer and the simple distortion processing mechanism still have a certain impact on the positioning and mapping accuracy of the system. The IMU is considered a scheme that can complement the lidar well. The high device update frequency of the IMU effectively supplements the system's ability to estimate the motion pose between two frames of point cloud data. The complementarity between the IMU and the lidar is mainly reflected in: the IMU can use reliable integral data within a short time to make up for the distortion caused by the carrier motion between two frames of point clouds, and provide a better initial value for the matching of two frames of point clouds; the divergence problem of the long-term IMU calculation can be well constrained by the pose results of point cloud registration.

[0003] The coupling methods of the lidar and the IMU are mainly divided into loose coupling and tight coupling. Loose coupling is that the two sensors independently calculate the pose, without referring to each other, and finally fuse and optimize the pose results of the two modules. Tight coupling is a coupling method that fully considers the important characteristics of the sensors and uses a non-modular way for data fusion. Specifically, in the tightly-coupled system of the IMU and the lidar, if the system can fully consider the residual change of the registration between laser point clouds, then the system can be called a tightly-coupled system of the lidar and the IMU. There are many coupling algorithms between multiple sensors, but mainly two mainstream methods have been formed: the filtering method and the factor graph optimization method. Although the factor graph method can provide a globally optimal estimate and thus has a greater advantage in terms of accuracy, its intensive computational requirements and the problem accumulation caused by long-term operation will continuously increase the overhead of calculation, storage, and optimization. In contrast, the filtering-based method focuses on correcting the estimation of the current state and uses recursive calculation, which is more suitable for complex environments that require quick response. Due to its low memory requirements and fast computational response speed, this method is particularly suitable for the SLAM system on small unmanned vehicles with limited computing power. This system will select the error-state Kalman filter (ESKF) to fuse the state estimates of the IMU and the lidar point cloud. Compared with the traditional EKF fusion, the small quantity characteristic of the error state has a better estimation and solution ability for complex motions, and has better numerical stability for the solution of rotational motions.

[0004] The technical comparison is as follows:

[0005] Technical Comparison with Patent CN117930272A, "A Robust Fusion Localization Method and Device for LiDAR in Dynamic Scenarios"

[0006] I. For Patent CN117930272A, it realizes the tight coupling of IMU and LiDAR based on error-state Kalman filter, and constructs the measurement equation using the distance residual from LiDAR points to the corresponding plane. While we extract the geometric features in the environment from the preprocessed point cloud data, mainly including the straight line and plane structures in the environment, and use the corrected intensity value to screen out the point cloud features in the environment, forming two types of feature point clouds, and perform registration according to the historical point cloud map in the world coordinate system maintained in the system, and then construct a point cloud residual measurement model. Moreover, in the prediction and update process of the filtering algorithm, using the residual estimation value, an anti-robust strategy based on the IGG

[0007] III robust weight function is designed to reconstruct the prior information and measurement information in the filtering estimation.

[0008] Technical Comparison with Patent CN117367412B, "A Tightly Coupled Laser Inertial Odometry and Mapping Method Incorporating Bundle Adjustment"

[0009] I. For Patent CN117367412B, by extracting the plane and edge features of the laser point cloud, optimizing the pre-integration factor of the LiDAR odometer and IMU, and updating the state variables including pose, velocity and bias in real time, it realizes the tightly coupled laser inertial odometer. While we preprocess the solid-state LiDAR point cloud, remove the defective points of the point cloud, correct and downsample the intensity value of the point cloud to improve the subsequent point cloud registration performance. Extract the geometric features in the environment from the preprocessed point cloud data, mainly including the straight line and plane structures in the environment, and use the corrected intensity value to screen out the point cloud features in the environment. The cumulative vector of the residuals of various feature point clouds and intermediate quantities such as their corresponding Jacobian matrices are used as measurement information and input into the filter. The filter iteratively processes these point cloud residual data multiple times to minimize the residuals and reach a convergent state. Finally, the filter generates a pose estimation matrix by updating the posterior information, thereby providing the output of the odometer. And in the prediction and update process of the filtering algorithm, using the residual estimation value, an anti-robust strategy based on the IGG III robust weight function is designed to reconstruct the prior information and measurement information in the filtering estimation. Summary of the Invention

[0010] To solve the above technical problems, the present invention proposes a robust tight coupling system of a solid-state lidar and an IMU assisted by point cloud intensity. The method can effectively reduce the dependence of classical algorithms on scene geometric features, improve the system's ability to localize and map in unstructured scenes, reduce the negative impact of abnormal point cloud measurement noise, and enhance the positioning accuracy and robustness of a small-field-of-view solid-state lidar SLAM system in complex unstructured environments.

[0011] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0012] A robust tight coupling system of a solid-state lidar and an IMU assisted by point cloud intensity, characterized in that the method comprises the following steps:

[0013] (1) The system preprocesses the solid-state lidar point cloud, removes point cloud defect points, corrects the point cloud intensity values and downsamples them;

[0014] (2) Extract geometric features and intensity features in the environment from the preprocessed point cloud data obtained in step (1);

[0015] (3) Use the high-frequency prior information predicted by the IMU to compensate for the movement of the point cloud, register the various types of feature point clouds obtained in step (2) according to the historical point cloud map in the world coordinate system maintained in the system, and construct a point cloud residual measurement model;

[0016] (4) Input measurement information such as the residual cumulative vector obtained during the feature point cloud registration in step (3) into the filter, add a robust factor constructed by the anti-robust strategy of the IGGⅢ weight function to the update link of the iterative error Kalman filter, and after multiple iterations, minimize the residual and reach a convergence state;

[0017] (5) The filter in step (4) generates a pose estimation matrix by updating the posterior information, thereby providing the output of the odometer.

[0018] Further, in step (2), various feature extractions are carried out respectively, and the specific process is as follows:

[0019] (2.1) When extracting point cloud geometric features, classify the high-curvature point cloud on a beam as corner points and the low-curvature point cloud as plane points. Since the acquisition time of each point cloud is available, make full use of the time before and after acquisition to calculate the curvature of points on the same beam. For a point P i on a beam, the set of all points on its beam is {L}, and the set of points before and after according to the acquisition time is {S}, then the curvature C i of this point is:

[0020]

[0021] Among them, and represent the coordinates of the point clouds i and j on the same wire harness. When {S} is controlled within a relatively small scale and the curvature of each point is obtained, the plane points and edge points are divided according to the following threshold:

[0022] P corner = {P i ∈ S | C i > C corner}

[0023] P plane = {P i ∈ S | C i < C plane}

[0024] Among them, C corner and C plane are related to the number of point clouds. The selection of P corner and P plane lays the foundation for more efficient point cloud registration in the follow-up;

[0025] (2.2) When extracting the point cloud intensity feature, first perform the intensity calibration of the lidar point cloud. This calibration process is described as using the mapping function to calibrate the original intensity reading η r :

[0026]

[0027] Among them, d is the distance reading of the point cloud from the sensor. After that, perform the normalization process on the calibrated lidar intensity feature point cloud, map the intensity value η cal to the interval [0, 1]. Let {S} be a small-scale point cloud set of points before and after the lidar acquisition time. Design a certain intensity threshold M, and screen the point clouds with the calibrated intensity value η cal greater than M and mark them as scene intensity feature points:

[0028] P intensity = {P i ∈ S | η cal > M}.

[0029] Further, step (3) specifically includes:

[0030] (3.1) Point cloud motion compensation:

[0031] In the formula, is the feature point cloud compensated for motion in the radar coordinate system at time point t k+1 , T ILis the extrinsic parameter matrix for converting the Lidar coordinate system to the IMU coordinate system, T(t k+1 ) IW T(t k ) WI are the IMU poses at time points t k+1 and t k respectively;

[0032] (3.2) Use the feature points of the new frame point cloud and the previous frame point cloud to construct the cumulative vector of the point cloud residuals corresponding to the geometric feature points using the Euclidean distance

[0033]

[0034] (3.3) For each intensity feature point, find neighboring points in the corresponding intensity feature historical point cloud map for local plane fitting, thereby constructing a plane patch. In this way, use the distance from the point to the fitted plane to construct the intensity point cloud residuals;

[0035] (3.4) For each one obtained in (2.2) Use the point cloud motion compensation formula in (3.1) to transform it into the in the global coordinate system, and then directly find the set of nearest neighbor points in the intensity historical point cloud map and denote it as N j ={n j1 ,n j2 ,n j3 ,n j4 ,...,n jk}°. Use these nearest neighbor points Nj to fit a local plane patch and denote the centroid of the fitted plane as q j , the unit normal vector of the plane as u j , and the centroid q j of the plane is obtained by taking the average position of all points in each coordinate direction. The optimal plane normal vector u j is obtained by minimizing the sum of the distances from the plane to the points. The search for the local plane patch can be transformed into solving the following optimization problem:

[0036]

[0037] (3.5) For the points (x j ,y i ,z i ) in the neighboring points N i , construct the matrix A and the vector b as follows:

[0038]

[0039] (3.6) In each plane fitting process, construct the following normal equation: A T Ax = A T b;

[0040] (3.7) Solve it using fast LDLT decomposition:

[0041] Ly = A T b, Dz = y, L T x = z

[0042]

[0043] (3.8) Further, use the additional point cloud residual cumulative vector composed of the intensity feature point cloud of the new k+1 frame and the midpoints of the local plane patches is expressed as follows:

[0044]

[0045] (3.9) The filtering update process of the lidar-IMU tightly coupled system is generally expressed as follows:

[0046]

[0047] where, K k,j , H k,j , δx j+1 represent the results of the j-th iterative update process of the filter, and H k,j represents the Jacobian matrix calculated for the δx corresponding to the j-th iteration with respect to the lidar point cloud residual vector originating from the measurement, and H k,j latitude is determined by the number of feature points extracted from the point cloud, represents the generalized manifold addition including rotation;

[0048] (3.10) The iterative process of the measurement model of the iterative error Kalman is essentially regarded as a least squares optimization problem incorporating the prior error state. The measurement model in this filtering process should actually be expressed as follows:

[0049]

[0050] where, the first term is the prior residual obtained by iterative projection, and in the second term is the measurement residual, which is composed of the prior state and the continuously iteratively updated error state δx, and V is the original measurement noise of the lidar, P kLet it be the obtained prior covariance, ‖‖ be the Mahalanobis norm, and f(.) be the cumulative residual vector obtained from each feature point of the lidar point cloud.

[0051] Further, step (4) specifically includes:

[0052] (4.1) Discrete-time nonlinear dynamic state space model:

[0053]

[0054] Wherein, and are the independent process noise vector and measurement noise vector respectively;

[0055] (4.2) For the measurement noise V in (4.1), use the residual vector v of measurement and prediction k =Z k -H k δx k,k-1 to update the state estimate, and define a cost function to minimize v k :

[0056] (4.3) The loss function constructed by the ρ(·) function is the IGGⅢ weight function:

[0057]

[0058] Wherein, c 0 and c 1 are two specific constants respectively, c 0 =1.0 - 1.5, c 1 =3.0 - 4.5, τ is the prediction residual statistic, and ψ(τ) is the robust factor used to adjust V;

[0059] (4.4) In the measurement model, use the RLS method to define the SSE chi-square from the 1st epoch to the kth epoch:

[0060]

[0061] Wherein, The increment of the chi-square of the updated state quantity of the Kalman filter at k is defined as:

[0062]

[0063] (4.5) Further expand the definition of chi-square in (4.4) to the Kalman filter field. During the filtering process, is defined as follows:

[0064]

[0065] (4.6) Define the expression at the k-th moment through the prediction process in the form of propagating values at the (k - 1)-th moment by the filter:

[0066]

[0067] (4.7) Combine the chi-square factor derived in (4.6) with the IGG III weight function form in (4.3) to construct a more accurate and sensitive robust factor β for outlier measurements to adjust the measurement noise covariance matrix: k where c

[0068]

[0069] and c 0 and c 1 are two specific constants respectively. Adding the robust factor β during the filtering process k effectively improves the noise tolerance of the measurement model and further enhances the robustness of the tightly coupled system. Therefore, the process expression of the robust tightly coupled filtering algorithm in this design is finally as follows:

[0070]

[0071] Beneficial effects: Compared with the prior art, the significant advantages of the present invention are as follows: The present invention proposes a robust tightly coupled system of a solid-state lidar and an IMU enhanced by an outlier-resistant strategy assisted by point cloud intensity. The present invention utilizes an intensity feature point cloud registration algorithm based on fast plane fitting, and constructs a measurement model of a tightly coupled system assisted by intensity features based on this algorithm, effectively reducing the dependence of the classical algorithm on the scene geometric features and improving the positioning and mapping ability of the system in unstructured scenes. In addition, by incorporating the IGG III robust outlier-resistant strategy, the present invention improves the filtering update step in the data fusion process, effectively reducing the negative impact of outlier point cloud measurement noise and enhancing the positioning accuracy and robustness of the small-field-of-view solid-state lidar SLAM system in complex unstructured environments. Compared with other odometry algorithms, the improved tightly coupled system of the present invention shows higher accuracy and better robustness on the solid-state radar dataset. Brief Description of the Drawings

[0072] Figure 1 is a flowchart of an embodiment of the present invention. Detailed Embodiments

[0073] The following details the embodiments of the present invention, and the examples of the embodiments are shown in the drawings. The embodiments described below with reference to the drawings are exemplary and are only used to explain the present invention and should not be construed as limiting the present invention.

[0074] AsFigure 1 As shown in the figure, the solid-state lidar and IMU robust tight coupling system enhanced by the robust strategy assisted by point cloud intensity of the present invention includes:

[0075] (1) The system will preprocess the solid-state lidar point cloud, remove the defective points of the point cloud, correct the point cloud intensity value and downsample it;

[0076] (2) Extract geometric features and intensity features in the environment from the preprocessed point cloud data obtained in step (1);

[0077] Step (2) specifically includes:

[0078] (2.1) When extracting the geometric features of the point cloud, classify the high-curvature point cloud on a beam as a corner point and the low-curvature point cloud as a plane point. Since the acquisition time of each point cloud is available, the acquisition time before and after can be fully utilized to calculate the curvature of the points on the same beam. For a point P i on a beam, the set of all points on its beam is {L}, and the set of points before and after according to the acquisition time is {S}, then the curvature C i of this point is:

[0079]

[0080] Among them, and represent the coordinates of the point clouds i and j on the same beam. The scale of {S} is generally controlled within a relatively small range. After obtaining the curvatures of each point, divide the plane points and edge points according to the following thresholds:

[0081] P corner = {P i ∈ S | C i > C corner}

[0082] P plane = {P i ∈ S | C i < C plane}

[0083] Among them, the values of C corner and C plane are related to the number of point clouds. The selection of P corner and P plane lays a foundation for more efficient point cloud registration in the future.

[0084] (2.2) When extracting the intensity features of the point cloud, first perform the intensity calibration of the lidar point cloud. This calibration process can be described as using the mapping function to calibrate the original intensity reading η r :

[0085]

[0086] Among them, d is the distance reading of the point cloud distance sensor. After that, normalization processing is performed on the calibrated lidar intensity feature point cloud, and the intensity value η cal is mapped to the interval [0, 1]. Let {S} be a small-scale point cloud set of points before and after the lidar acquisition time. A certain intensity threshold M is designed to screen the calibrated intensity value η cal of the point cloud greater than M and mark it as the scene intensity feature point:

[0087] P intensity ={P i ∈S|η cal >M}

[0088] (3) Use the high-frequency prior information predicted by the IMU to compensate for the movement of the point cloud. Register the various types of feature point clouds obtained in step (2) according to the historical point cloud map in the world coordinate system maintained in the system, and construct a point cloud residual measurement model;

[0089] Step (3) specifically includes:

[0090] (3.1) Point cloud motion compensation:

[0091] In the formula, is the feature point cloud compensated for movement in the radar coordinate system at time point t, T k+1 is the external parameter matrix for converting the Lidar coordinate system to the IMU coordinate system, T(t IL ) k+1 ) IW T(t k ) WI are the IMU poses at time points t k+1 and t k respectively.

[0092] (3.2) Use the feature points of the new frame point cloud and the previous frame point cloud Euclidean distance to construct the point cloud residual cumulative vector corresponding to the geometric feature points

[0093]

[0094] (3.3) For each intensity feature point, find neighboring points in the corresponding intensity feature historical point cloud map for local plane fitting, thereby constructing a plane patch. In this way, use the distance from the point to the fitted plane to construct the intensity point cloud residual.

[0095] (3.4) For each one obtained in (2.2) Convert it into the global coordinate system by using the point cloud motion compensation formula in (3.1). Then, directly find the latest frame in the intensity history point cloud map. The set of nearest neighbor points is denoted as N j ={n j1 ,n j2 ,n j3 ,n j4 ,...,n jk}}. Fit a local plane patch using these nearest neighbor points Nj, and denote the centroid of the fitted plane as q j , the unit normal vector of the plane as u j . The centroid q j of the plane can be obtained by using the average position of all points in each coordinate direction. Generally, the optimal plane normal vector u is obtained by minimizing the sum of the distances from the plane to the points. j The search for the local plane patch can be transformed into solving the following optimization problem:

[0096]

[0097] (3.5) For the points (x j ,y i ,z i ) in the set of neighboring points N i , construct the matrix A and the vector b as follows:

[0098]

[0099] (3.6) In each plane fitting process, construct the following normal equation: A T Ax = A T b

[0100] (3.7) Solve it using fast LDLT factorization:

[0101] Ly = A T b, Dz = y, L T x = z

[0102]

[0103] (3.8) Further, use the additional point cloud residual cumulative vector formed by the new k + 1 frame intensity feature point cloud and the points in the local plane patch to represent as follows:

[0104]

[0105] (3.9) The filtering update process of the lidar and IMU tightly coupled system is generally represented as follows:

[0106]

[0107] Among them, K k,j 、H k,j 、δx j+1 represent the results of the j-th iterative update process of the filter. H k,j represents the Jacobian matrix calculated from the residual vector of the lidar point cloud originating from the measurement corresponding to δx in the j-th iteration. H k,j The latitude is determined by the number of feature points extracted from the point cloud. represents the addition of a generalized manifold including rotation.

[0108] (3.10) The iterative process of the measurement model of the iterative error Kalman can essentially be regarded as a least-squares optimization problem incorporating the prior error state. The measurement model in this filtering process should actually be expressed as follows:

[0109]

[0110] Among them, the former term is the prior residual obtained by iterative projection, and in the latter term is the measurement residual, which is composed of the prior state and the continuously iteratively updated error state δx. V is the original measurement noise of the lidar, and P k is the obtained prior covariance, ‖‖ is the Mahalanobis norm, and f(.) is the cumulative residual vector obtained from each feature point of the lidar point cloud.

[0111] (4) Input the cumulative residual vector of the feature point cloud registration obtained in step (3) and other measurement information into the filter, add the robust factor constructed by the anti-robust strategy of the IGGⅢ weight function to the update link of the iterative error Kalman filter, and after multiple iterations, minimize the residual and reach the convergence state;

[0112] Step (4) specifically includes:

[0113] (4.1) General discrete-time nonlinear dynamic state space model:

[0114]

[0115] Among them, and are independent process noise vector and measurement noise vector respectively.

[0116] (4.2) For the measurement noise V in (4.1), use the residual vector v of the measurement and prediction k =Z k -H k δxk,k-1 To update the state estimate, a cost function is defined to minimize v k :

[0117] (4.3) The loss function constructed by the ρ(·) function is the IGGⅢ weight function:

[0118]

[0119] where c 0 and c 1 are two specific constants respectively, c 0 = 1.0 - 1.5, c 1 = 3.0 - 4.5, τ is the predicted residual statistic, and ψ(τ) is the robust factor used to adjust V.

[0120] (4.4) In the measurement model, the RLS method is used to define the SSE (chi-square) from the 1st epoch to the kth epoch:

[0121]

[0122] where The increment of the chi-square of the updated state quantity of the Kalman filter at k is defined as:

[0123]

[0124] (4.5) The definition of chi-square in (4.4) is further extended to the Kalman filter domain. During the filtering process, it can be defined as follows:

[0125]

[0126] (4.6) The expression at the kth moment is defined through the value propagation form of the filter at the (k - 1)th moment, i.e., the prediction process: Expression:

[0127]

[0128] (4.7) Combining the chi-square factor derived in (4.6) with the form of the IGGⅢ weight function in (4.3) can construct a more accurate and sensitive robust factor β for abnormal measurement values to adjust the measurement noise covariance matrix: k to adjust the measurement noise covariance matrix:

[0129]

[0130] where c 0 and c 1 are two specific constants respectively. The robust factor β is added during the filtering processk It can effectively improve the tolerance of the measurement model to noise and further enhance the robustness of the tightly coupled system. Therefore, the process expression of the robust tightly coupled filtering algorithm in this design is ultimately as follows:

[0131] δx k,k-1 = F k δx k-1

[0132] P pred = P k,k-1 = F k P k-1 F k T + Γ k QΓ k T

[0133]

[0134] (5) Generate a pose estimation matrix by updating the posterior information of the filter in step (4), thereby providing the output of the odometer.

[0135] The above is only a preferred embodiment of the present invention and is not a limitation to the present invention in any other form. Any modification or equivalent change made according to the technical essence of the present invention still falls within the scope claimed by the present invention.

Claims

1. A robust tight coupling method between solid-state laser radar and IMU assisted by point cloud intensity, characterized in that: The method comprises the following steps: (1) The system will pre-process the solid-state laser radar point cloud, remove defective points in the point cloud, calibrate the point cloud intensity value and downsample it; (2) extracting geometric features and intensity features in the environment based on the preprocessed point cloud data obtained in step (1); (3) Using the high-frequency prior information predicted by the IMU to compensate for the motion of the point cloud, the various types of feature point clouds obtained in step (2) are registered according to the historical point cloud map in the world coordinate system maintained in the system, and a point cloud residual measurement model is constructed; (4) The residual cumulative vector measurement information obtained during the feature point cloud registration in step (3) is input into the filter, and the robust factor constructed by the anti-error strategy of the IGG III weight function is added to the update link of the iterative error Kalman filter, and after multiple iterations, the residual is minimized and a convergence state is achieved; (5) The filter in step (4) is used to generate a pose estimation matrix by updating the posterior information, thereby providing the output of the odometer.

2. The method for robust tight coupling of solid-state laser radar and IMU assisted by point cloud intensity according to claim 1, characterized in that: In step (2), various feature extractions are performed respectively, and the specific process is as follows: (2.1) When extracting geometric features of point clouds, high curvature point clouds on a line bundle are classified as corner points, and low curvature point clouds are classified as plane points. Since we have the acquisition time of each point cloud, we can make full use of the time before and after acquisition to calculate the curvature of the points on the same line bundle. For a point P on the line bundle, i , the set of all points in the line beam is {L}, and the set of points before and after the acquisition time is {S}, then the curvature C of the point i : in, and Represents the coordinates of point clouds i and j on the same line bundle. {S} is controlled within a small scale. After obtaining the curvature of each point, plane points and edge points are divided according to the following thresholds: P corner ={P i ∈S∣C i >C corner } P plane ={P i ∈S∣C i <C plane } Among them C corner and C plane The value is related to the number of point clouds, P corner With P plane The selection lays the foundation for more efficient point cloud registration; (2.2) When extracting the intensity features of the point cloud, the intensity calibration of the LiDAR point cloud is first performed. This calibration process is described by using a mapping function The original intensity reading η r To perform a calibration: Where d is the distance reading of the point cloud distance sensor. Then the calibrated lidar intensity feature point cloud is normalized to convert the intensity value η cal Map to the interval [0,1], let {S} be a small-scale point cloud set before and after the laser radar acquisition time, design a certain intensity threshold M, and screen the calibrated intensity value η cal Point clouds larger than M are marked as scene intensity feature points: P intensity ={P i ∈S∣η cal >M}。 3. The method for robust tight coupling of solid-state laser radar and IMU assisted by point cloud intensity according to claim 2, characterized in that: Step (3) specifically includes: (3.1) Point cloud motion compensation: In the formula, Yes k+1 The motion-compensated feature point cloud in the radar coordinate system at the time point, T IL is the external parameter matrix for transforming the Lidar coordinate system to the IMU coordinate system, T(t k+1 ) IW T(t k ) WI t k+1 With t k IMU pose at a given time point; (3.2) Using new frame point cloud feature points With the previous frame point cloud Euclidean distance is used to construct the point cloud residual cumulative vector corresponding to the geometric feature points (3.3) For each intensity feature point, find neighboring points in the corresponding intensity feature historical point cloud map for local plane fitting, so as to construct a plane patch. In this way, the distance from the point to the fitting plane is used to construct the intensity point cloud residual; (3.4) For each obtained in (2.2) Use the point cloud motion compensation formula in (3.1) to transform it into the global coordinate system Then, in the intensity history point cloud map, directly search for the latest frame The set of neighboring points is denoted as N j ={n j1 ,n j2 ,n j3 ,n j4 ,...,n jk }Use these neighboring points Nj to fit a local plane patch, and the centroid of the fitting plane is q j , the unit normal vector of the plane is u j , plane center of mass q j The optimal plane normal vector u is obtained by minimizing the sum of the distances from the plane to the point using the average position of all points in each coordinate direction. j , the local plane patch search can be transformed into solving the following optimization problem: (3.5) For the neighboring point N j The point (x i ,y i ,z i ), construct the matrix A and vector b as follows: (3.6) In each plane fitting process, the following normal equation is constructed: A T Ax=A T b; (3.7) is solved using fast LDLT decomposition: Ly=A T b,Dz=y,L T x=z (3.8) Further, using the new k+1 frame intensity feature point cloud and the midpoint of the local plane patch The additional point cloud residual accumulation vector composed of It is expressed as follows: (3.9) The filtering update process of the lidar and IMU tightly coupled system is generally expressed as follows: Among them, K k,j , H k,j ,δx j+1 represents the result of the jth iterative update process of the filter, H k,j Represents the residual vector of δx corresponding to the jth iteration for the measured LiDAR point cloud The calculated Jacobian matrix, H k,j The latitude is determined by the number of feature points extracted from the point cloud. represents generalized manifold addition involving rotations; (3.10) The iterative process of the iterative error Kalman measurement model is essentially regarded as a least squares optimization problem that incorporates the prior error state. The measurement model in this filtering process should actually be expressed as follows: The first term is the prior residual obtained by iterative projection, and the second term is To measure the residual, the prior state It is composed of the error state δx that is continuously updated iteratively, V is the original measurement noise of the lidar, and P k is the obtained prior covariance, ‖‖ is the Mahalanobis norm, and f(.) is the cumulative residual vector obtained from each feature point of the lidar point cloud.

4. The method for robust tight coupling of solid-state laser radar and IMU assisted by point cloud intensity according to claim 1, characterized in that: Step (4) specifically includes: (4.1) Discrete-time nonlinear dynamic state space model: in, and are the independent process noise vector and measurement noise vector respectively; (4.2) For the measurement noise V in (4.1), use the residual vector v between measurement and prediction k =Z k -H k δx k,k-1 To update the state estimate, define the cost function to minimize v k : (4.3) The loss function constructed by the ρ(·) function is the IGGⅢ weight function: Among them, c0 and c1 are two specific constants, c0 = 1.0 ~ 1.5, c1 = 3.0 ~ 4.5, τ is the prediction residual statistic, ψ(τ) is the robust factor used to adjust V; (4.4) In the measurement model, the RLS method is used to define the SSE chi-square from the 1st epoch to the kth epoch: in, The chi-square increment of the updated state of the Kalman filter at k is defined as: (4.5) The definition of chi-square in (4.4) is further extended to the field of Kalman filtering. In the filtering process, The definition is as follows: (4.6) The prediction process is defined by propagating the value of the filter at time k-1. expression: (4.7) Substituting the result derived in (4.6) The chi-square factor combined with the IGGⅢ weight function form of (4.3) can construct a more accurate and sensitive robust factor β for abnormal measurement values. k To adjust the measurement noise covariance matrix: Among them, c0 and c1 are two specific constants, and the robust factor β is added in the filtering process. k Effectively improve the tolerance of the measurement model to noise and further improve the robustness of the tightly coupled system. Therefore, the process expression of the robust tightly coupled filtering algorithm designed in this paper is finally as follows:

Images