Solid-state laser radar and IMU robust tight coupling system under assistance of point cloud intensity

Abstract

The invention provides a solid-state laser radar and IMU (Inertial Measurement Unit) robust tight coupling system under the assistance of point cloud intensity, which comprises the following steps of: firstly, pre-processing a solid-state laser radar point cloud by the system, eliminating point cloud defect points, correcting a point cloud intensity value and performing down-sampling; and then geometric feature extraction and intensity feature extraction are carried out on the preprocessed point cloud, and intensity calibration of the laser radar point cloud is carried out. And then compensating the motion of the point cloud by using the high-frequency prior information predicted by the IMU. For geometric and intensity feature point clouds, a classical registration method and a fast plane fitting method are respectively adopted to calculate residual errors, and an intensity feature assisted point cloud residual error measurement model is constructed. And a robust strategy based on an IGGIII robust weight function is designed by using a residual error estimation value in a filtering algorithm. According to the invention, on the premise of keeping relatively low calculation cost, the positioning and mapping capabilities and robustness of the small-view-angle laser radar tight coupling system in a complex environment are improved.

Description

Technical Field

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

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

[0003] The coupling modes of LiDAR and IMU are mainly divided into loose coupling and tight coupling. Loose coupling is that the two sensors independently calculate the pose, without reference to each other, and finally the pose results of the two modules are fused and optimized. Tight coupling is a coupling method that fully considers the important characteristics of the sensor and uses a non-modular method to perform data fusion. Specifically, in the tightly coupled system of IMU and LiDAR, if the system can fully consider the residual changes of the registration between laser point clouds, the system can be called a tightly coupled system of LiDAR and IMU. There are many coupling algorithms between multiple sensors, but two mainstream methods are mainly formed, namely filtering method and factor graph optimization method. Although the factor graph method can provide global optimal estimation, thus having a great advantage in accuracy, its intensive computing requirements and the accumulation of problems caused by long-term operation will increase the overhead of computing, storage and optimization. In contrast, the filtering-based method focuses on correcting the estimate of the current state and adopts recursive calculation, which is more suitable for complex environments that require fast response. This method is particularly suitable for SLAM systems on small unmanned carriers with limited computing power due to its low memory requirements and fast computing response speed. This system will select the error state Kalman filter (ESKF) to fuse the state estimation of IMU and lidar point cloud. Compared with the traditional EKF fusion, the small amount characteristic of the error state has a better estimation and solution capability for complex motion, and has better numerical stability for the solution of rotational motion.

[0004] The technical comparison is as follows:

[0005] Technical comparison with patent CN117930272A "A robust fusion positioning method and device for laser radar in dynamic scenes"

[0006] 1. Patent CN117930272A, based on the error state Kalman filter, realizes the tight coupling of IMU and LiDAR, and uses the distance residual from the LiDAR point to the corresponding plane to construct the measurement equation. We extract the geometric features in the environment from the preprocessed point cloud data, mainly including the straight lines and plane structures in the environment, and use the corrected intensity values ​​to filter out the point cloud features in the environment to form two types of feature point clouds, and align them according to the historical point cloud map in the world coordinate system maintained in the system, and then build a point cloud residual measurement model. In addition, the residual estimation value is used in the prediction and update process of the filtering algorithm to design an IGG-based

[0007] III The robust weight function's anti-error strategy reconstructs the prior information and measurement information in the filter estimation.

[0008] Technical comparison with patent CN117367412B "A tightly coupled laser inertial navigation odometer and mapping method with integrated bundle adjustment"

[0009] 1. Patent CN117367412B, by extracting the plane and edge features of the laser point cloud, optimizing the laser radar odometer and IMU pre-integration factors, and updating the state quantities including posture, velocity and bias in real time, realizes a tightly coupled laser inertial navigation odometer. We preprocess the solid-state laser radar point cloud, remove point cloud defect points, correct and downsample the point cloud intensity values ​​to improve the subsequent point cloud registration performance. The geometric features in the environment are extracted from the preprocessed point cloud data, mainly including the straight lines and plane structures in the environment, and the point cloud features in the environment are screened out using the corrected intensity values. The residual cumulative vectors of various feature point clouds and their corresponding Jacobian matrices and other intermediate quantities are input into the filter as measurement information. The filter performs multiple iterative processing on these point cloud residual data to minimize the residual and reach a convergence state. Finally, the filter generates a pose estimation matrix by updating the posterior information, thereby providing the output of the odometer. In addition, the residual estimation value is used in the prediction and update process of the filtering algorithm, and an anti-error strategy based on the IGGⅢ robust weight function is designed to reconstruct the prior information and measurement information in the filtering estimation. Summary of the invention

[0010] In order to solve the above technical problems, the present invention proposes a solid-state laser radar and IMU robust tightly coupled system assisted by point cloud intensity. The method can effectively reduce the dependence of the classical algorithm on the geometric features of the scene, improve the system's positioning and mapping capabilities in unstructured scenes, reduce the negative impact of abnormal point cloud measurement noise, and improve the positioning accuracy and robustness of the small-viewing-angle solid-state laser radar SLAM system in complex unstructured environments.

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

[0012] The solid-state laser radar and IMU robust tight coupling system assisted by point cloud intensity is characterized in that the method comprises the following steps:

[0013] (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;

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

[0015] (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;

[0016] (4) The residual cumulative vector and other 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 reached;

[0017] (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.

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

[0019] (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 :

[0020]

[0021] 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:

[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 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;

[0025] (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:

[0026]

[0027] 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:

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

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

[0030] (3.1) Point cloud motion compensation:

[0031] In the formula, Yes k+1 The motion-compensated feature point cloud in the radar coordinate system at the time point, T ILis 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;

[0032] (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

[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, 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;

[0035] (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:

[0036]

[0037] (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:

[0038]

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

[0040] (3.7) is solved using fast LDLT decomposition:

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

[0042]

[0043] (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:

[0044]

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

[0046]

[0047] 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;

[0048] (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:

[0049]

[0050] 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 kis 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.

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

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

[0053]

[0054] in, 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 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 :

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

[0057]

[0058] 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;

[0059] (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:

[0060]

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

[0062]

[0063] (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:

[0064]

[0065] (4.6) The prediction process is defined by propagating the value of the filter at time k-1. expression:

[0066]

[0067] (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:

[0068]

[0069] 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:

[0070]

[0071] Beneficial effects: Compared with the prior art, the present invention has the following significant advantages: The present invention proposes a robust tightly coupled system of solid-state laser radar and IMU with enhanced anti-error strategy assisted by point cloud intensity. The present invention utilizes an intensity feature point cloud registration algorithm based on fast plane fitting, and constructs an intensity feature-assisted tightly coupled system measurement model based on this algorithm, which effectively reduces the dependence of the classical algorithm on the geometric features of the scene and improves the system's positioning and mapping capabilities in unstructured scenes. In addition, by incorporating the IGGⅢ robust anti-error strategy, the present invention improves the filtering and updating steps in the data fusion process, effectively reducing the negative impact of abnormal point cloud measurement noise, and improving the positioning accuracy and robustness of the small-viewing-angle solid-state laser radar SLAM system in complex unstructured environments. Compared with other mileage calculation methods, the improved tightly coupled system of the present invention exhibits higher accuracy and better robustness on solid-state radar data sets. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 is a flow chart of an embodiment of the present invention. DETAILED DESCRIPTION

[0073] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be interpreted as limiting the present invention.

[0074] like Figure 1 As shown, the solid-state laser radar and IMU robust tight coupling system with enhanced anti-error strategy assisted by point cloud intensity of the present invention includes:

[0075] (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;

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

[0077] Step (2) specifically includes:

[0078] (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 :

[0079]

[0080] in, and Represents the coordinates of point clouds i and j on the same line bundle. {S} is generally controlled within a small scale. After obtaining the curvature of each point, the plane points and edge points are divided 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 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.

[0084] (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 can be described as using a mapping function The original intensity reading η r To perform a calibration:

[0085]

[0086] 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 Mapped 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:

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

[0088] (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;

[0089] Step (3) specifically includes:

[0090] (3.1) Point cloud motion compensation:

[0091] 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.

[0092] (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

[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, the distance from the point to the fitting plane is used to construct the intensity point cloud residual.

[0095] (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 It 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 point. j , the local plane patch search can be transformed into solving the following optimization problem:

[0096]

[0097] (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:

[0098]

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

[0100] (3.7) is solved using fast LDLT decomposition:

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

[0102]

[0103] (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:

[0104]

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

[0106]

[0107] 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.

[0108] (3.10) The iterative process of the iterative error Kalman measurement model can essentially be 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:

[0109]

[0110] 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.

[0111] (4) The residual cumulative vector and other 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 reached;

[0112] Step (4) specifically includes:

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

[0114]

[0115] in, and are the 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 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 :

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

[0118]

[0119] Among them, c0 and c1 are two specific constants, c0 = 1.0 ~ 1.5, c1 = 3.0 ~ 4.5, τ is the prediction 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] in, The chi-square increment of the updated state 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 field of Kalman filtering. In the filtering process, It can be defined as follows:

[0125]

[0126] (4.6) The prediction process is defined by propagating the value of the filter at time k-1. expression:

[0127]

[0128] (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:

[0129]

[0130] Among them, c0 and c1 are two specific constants. The robust factor β is added in the filtering process. k It can 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:

[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) 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.

[0135] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. The solid-state laser radar and IMU robust tightly coupled system assisted by point cloud intensity is characterized by: 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 and other 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 reached; (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 solid-state laser radar and IMU robust tightly coupled system 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, the 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 solid-state laser radar and IMU robust tightly coupled system 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(tk +1 ) IW T(tk) 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, (representing the generalized manifold addition including rotation; (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 solid-state laser radar and IMU robust tightly coupled system 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