CN114397642A - A 3D LiDAR and IMU External Parameters Calibration Method Based on Graph Optimization - Google Patents

Abstract

The invention provides a three-dimensional laser radar and IMU external parameter calibration method based on graph optimization, which relates to the technical field of multi-sensor calibration, and includes: acquiring the measurement data of the laser radar and the IMU in the device; Residual: Project the measurement data of the lidar to the world coordinate system through the IMU coordinate system, obtain a point cloud image, and calculate the distance residual between the lidar point and the corresponding feature line and feature surface in the point cloud image; based on the graphical model, IMU residual, The distance residual obtains the initial objective function and the initial optimization increment equation; sets a constant frame sliding window, obtains the marginalization incremental optimization equation, and calculates the marginalization residual term; based on the graph model, IMU residual, distance residual, marginalization The residual term obtains the objective function, and calculates the external parameter calibration of the lidar and IMU. The invention obtains the latest state of the sensor device in real time through a constant frame sliding window, and realizes the real-time calibration of external parameters.

Description

A 3D LiDAR and IMU External Parameters Calibration Method Based on Graph Optimization

technical field

The invention relates to the technical field of multi-sensor calibration, in particular to a method for calibrating external parameters of three-dimensional laser radar and IMU based on graph optimization.

Background technique

Multi-sensor fusion is an important technology for unmanned vehicles, mobile robots, and surveying and mapping instruments for accurate environmental perception. Due to the increasingly high requirements for the work of robots, only a single sensor cannot accurately realize the robot's environmental perception work. Therefore, multi-sensor fusion technology has attracted the attention of many domestic and foreign researchers; different sensors have different characteristics, for example, inertial measurement unit (IMU) has high update rate, but sensor data has noise and drift; while lidar (LIDAR) has accurate The ability of depth perception, but the problem of motion distortion is prone to occur during the movement; therefore, the defects of a single sensor make the robot positioning results using a single sensor inaccurate. Therefore, through data fusion, the shortcomings of multiple sensors can be compensated for each other, and the overall perception capability of the carrier can be significantly improved.

For the data fusion of the IMU and the lidar, the external parameter calibration of the lidar and the IMU needs to be performed first, that is, the coordinate transformation between the coordinate systems of the two sensors is calculated. As shown in Figure 2, without the external parameter calibration between sensors, when the robot is at position A, the point P observed by the radar will be output by the IMU to P1. When the robot is at position B, the point P observed by the radar will be changed to P2 by the IMU output. Due to the inconsistency of the coordinate system, point P produces two points P1 and P2. Therefore, only by obtaining the precise coordinate transformation relationship between the two sensors (that is, calculating the transformation between the coordinate systems of each sensor), through data fusion, the robot can accurately detect the environment in an unknown environment, complete its own positioning and at the same time. The process of building a surrounding map. Therefore, sensor calibration is a very promising technology with strong research value. It is also necessary to study the external parameter calibration of lidar and IMU sensors.

At present, some researchers have achieved certain results in the research on LIDAR-IMU external parameter calibration. One of the existing methods is to use additional sensors or manually set specific targets to achieve, which is time-consuming and labor-intensive; the other is targetless external parameters. The calibration method has the problems of high requirements on the plane characteristics of the experimental environment and high computational intensity in the optimization process of the objective function. At the same time, the external parameters of LIDAR-IMU are generally calibrated offline, which requires professionals to move the carrier to a fixed calibration environment. When the mechanical configuration of the sensor of the carrier device changes slightly, the calibration work needs to be repeated, which is complicated and laborious.

SUMMARY OF THE INVENTION

In view of the above problems, the present invention provides a method for calibrating external parameters of three-dimensional laser radar and IMU based on graph optimization, so as to realize fast and real-time external parameter calibration of three-dimensional laser radar and IMU.

In order to achieve the above purpose, the present invention provides a method for calibrating external parameters of three-dimensional laser radar and IMU based on graph optimization, including:

Obtain the measurement data of the lidar and IMU in the sensor device respectively;

Perform IMU pre-integration on the measurement data of the IMU, obtain the estimated value of the pose transformation of the IMU at the next moment according to the IMU pre-integration, and calculate the IMU residual error;

Projecting the measurement data of the laser radar to the IMU coordinate system, and then projecting it to the world coordinate system, obtaining a point cloud image, and obtaining the distance residuals between the laser radar points and the characteristic lines and characteristic surfaces corresponding to the point cloud image;

Based on the graphical model, the initial objective function and the initial optimization incremental equation are obtained through the IMU residual and the distance residual;

Set a constant frame sliding window, obtain the marginalization incremental optimization equation according to the initial optimization incremental equation, and calculate the marginalization residual term;

Based on the graph model, the objective function is obtained through the IMU residuals, distance residuals, and marginalized residuals;

Minimize the objective function to obtain the external parameter calibration of lidar and IMU.

As a further improvement of the present invention, the estimated value of the pose transformation of the IMU at the next moment is obtained according to the IMU pre-integration, including:

Build an IMU motion model;

Based on the IMU motion model, a quaternion method is used to obtain the IMU pose and state transformation equation of the lidar point between two adjacent frames;

Calculate the estimated value of the pose transformation of the IMU at the next moment according to the IMU pose state transformation equation.

As a further improvement of the present invention, the IMU residual is calculated according to the estimated value of the pose transformation of the IMU at the next moment and the actual measurement value of the IMU at the next moment.

As a further improvement of the present invention, the acquisition of the distance residuals of the characteristic lines and characteristic surfaces corresponding to the lidar point and the point cloud image includes:

Perform feature extraction on the point cloud image to obtain line feature points and surface feature points;

respectively constructing the line fitting from the lidar point to the line feature point in the point cloud image and the distance equation to the surface fitting the surface feature point in the point cloud image;

Minimize the two distance equations to obtain the point-to-line distance residual and the point-to-surface distance residual, respectively.

As a further improvement of the present invention, feature extraction is performed on the point cloud image to obtain line feature points and surface feature points; including:

以i为中心附近邻点的集合记为S，则曲率c的公式为：Mark the spatial coordinates of the k-th line lidar point i in the lidar coordinate system as

The set of adjacent points near the center of i is denoted as S, and the formula of the curvature c is:

The curvature of each lidar point is calculated according to the curvature formula, and points with a curvature greater than 0.1 are recorded as line feature points, and points with a curvature less than 0.1 are recorded as surface feature points.

As a further improvement of the present invention, based on the graph model, the initial objective function is obtained through the IMU residual and distance residual, including:

Based on the graphical model, the lidar and IMU extrinsic parameters can be expressed as maximum likelihood estimation:

in,

Z represents the measured value;

F represents the initialization objective function;

According to the IMU residual and distance residual, the initialization objective function is expressed as:

in,

de and dp represent the distance residuals between the lidar point and the corresponding feature line and feature surface in the point cloud image, respectively;

表示IMU残差。

represents the IMU residual.

As a further improvement of the present invention, the obtaining the initial optimization incremental equation includes:

By nonlinear optimization of most objective functions, the incremental optimization equation is obtained:

HδX=b

in:

H=ΣJ ^T C ^-1 J

b=Σ ^J TC ^-1 r

J represents the Jacobian equation of the residual state quantity;

C represents the covariance matrix of the state;

r represents the residual of the cost equation;

δX represents the error state to be estimated;

By solving the obtained error state δX to be estimated, and updating the variable X _r to be optimized, then:

in,

○ indicates a small angle update of the quaternion.

As a further improvement of the present invention, the constant frame sliding window is set, the marginalization incremental optimization equation is obtained according to the initial optimization incremental equation, and the marginalization residual term is calculated; including:

Using the marginalization method, when a key frame is entered in the sliding window, while retaining the constraints corresponding to the key frame to be discarded, the marginalization incremental optimization equation is expressed as:

The marginalization optimization equation is obtained by the elimination method:

in,

X _m represents the amount of optimization that needs to be marginalized;

X _r represents the variable that needs to be preserved for optimization;

According to the initialization optimization equation and the marginalization optimization equation, the estimated value of the variable X _r to be optimized before and after marginalization is calculated, and the marginalization error term is obtained.

As a further improvement of the present invention, based on the graph model, the objective function is obtained through the IMU residuals, distance residuals, and marginalization residuals; including:

Based on the graphical model, the lidar and IMU extrinsic parameters can be expressed as maximum likelihood estimation:

in,

Z represents the measured value;

F(X) represents the objective function;

According to the IMU residual, distance residual and marginalization residual terms, the objective function is expressed as:

in,

de and dp represent the distance residuals between the lidar point and the corresponding feature line and feature surface in the point cloud image, respectively;

表示IMU残差；

Represents the IMU residual;

r _p represents the marginalized residual term.

As a further improvement of the present invention, the Gauss-Newton method is used to minimize the IMU residuals, distance residuals and marginalized residuals in the objective function to obtain the external parameter calibration of lidar and IMU.

Compared with the prior art, the beneficial effects of the present invention are:

Compared with the prior art, the present invention realizes the real-time calibration of the external parameters of the laser radar and the IMU by setting a sliding window to obtain the latest state of the sensor device in real time, which is simple and fast; at the same time, the present invention uses the laser radar coordinate system and the IMU coordinate in the calibration calculation. The relationship between the systems is used to generate a point cloud map, and the distance residuals of the LiDAR point-to-surface and LiDAR point-to-line are respectively obtained corresponding to the feature lines and special faces in the point cloud map, and the distance residuals are used as constraints for external parameter calibration. The accuracy of external parameter calibration.

In order to ensure the computational complexity of the optimization process, the invention sets the sliding window as a fixed frame, realizes the balance of computational efficiency and accuracy, and meets the requirements of real-time calibration.

When the invention adopts a fixed frame sliding window, in order to avoid the influence of discarding state data, the marginalization method is used to obtain the marginalized residual item, and then the marginalized residual item is used as the constituent item of the objective function, so as to further improve the external parameters of lidar and IMU. Optimized accuracy.

Description of drawings

FIG. 1 is a flowchart of a method for calibrating external parameters of 3D lidar and IMU based on graph optimization disclosed by an embodiment of the present invention;

FIG. 2 is a diagram showing the difference between the coordinate systems of each sensor in the multi-sensor device disclosed in the background of the present invention;

FIG. 3 is a schematic diagram of a method for calibrating external parameters of 3D lidar and IMU based on graph optimization disclosed by an embodiment of the present invention;

4 is a schematic diagram of the relationship between a lidar factor and an IMU factor in a graph model disclosed by an embodiment of the present invention;

FIG. 5 is a display diagram of a point cloud image disclosed by an embodiment of the present invention;

6 is a schematic diagram of a line feature point disclosed in an embodiment of the present invention;

7 is a schematic diagram of a surface feature point disclosed by an embodiment of the present invention;

FIG. 8 is a schematic diagram of geometric constraints of point-to-line and point-to-surface disclosed in an embodiment of the present invention.

Detailed ways

In order to make the purposes, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments These are some embodiments of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present invention.

Below in conjunction with accompanying drawing, the present invention is described in further detail:

As shown in Figures 1 and 3, the present invention provides a method for calibrating external parameters of 3D lidar and IMU based on graph optimization, including:

S1. Obtain the measurement data of the lidar and the IMU in the sensor device respectively;

S2. Perform IMU pre-integration on the measurement data of the IMU, obtain the estimated value of the pose transformation of the IMU at the next moment according to the IMU pre-integration, and calculate the IMU residual error;

Among them, including:

Build an IMU motion model;

Based on the IMU motion model, the quaternion method is used to obtain the IMU pose state transformation equation of the lidar point between two adjacent frames;

Calculate the estimated value of the pose transformation of the IMU at the next moment according to the IMU pose state transformation equation;

The IMU residual is calculated according to the estimated value of the pose transformation of the IMU at the next moment and the actual measurement value of the IMU at the next moment.

specific:

表示世界坐标系到IMU坐标系的旋转变换矩阵，可以得到：The angular velocity and linear velocity measured by the IMU build the IMU motion model; set the letter b to represent the IMU carrier coordinate system, the letter W to represent the world coordinate system, and use

Representing the rotation transformation matrix from the world coordinate system to the IMU coordinate system, we can get:

in,

wg represents the gravity vector in the world coordinate system.

respectively represent the acceleration at a certain moment and the angular velocity at a certain moment measured in the IMU coordinate system; a _b (t), w _b (t) respectively represent the true values of the acceleration and angular velocity in the IMU coordinate system, including their own deviation and random noise interference.

b ^a (t), b ^g (t) represent the accelerometer bias and the random noise of the accelerometer, respectively;

n ^a (t), n ^g (t) represent the gyro bias and gyro random noise, respectively.

And then get the IMU motion equation:

Among them, p _W , v _W , a _W represent the position, velocity and acceleration in the world coordinate system respectively;

The IMU pose state between two lidar keyframes is expressed as:

in:

得到bk+1时刻在IMU坐标系下的IMU位姿状态：IMU pre-integration can avoid the problem of repeated integration. During IMU pre-integration, it is necessary to convert the reference system from the world coordinate system to the IMU coordinate system, that is, multiply both sides of the above equation by

Get the IMU pose state in the IMU coordinate system at the time of bk+1:

Among them, α, β and γ are the results of IMU pre-integration:

Using the median method to discretize the result of IMU pre-integration, we get:

in:

According to the estimated value of IMU pose transformation obtained by calculation and the actual IMU measurement value at the next moment (for example: the pose data of the IMU when m+1 frames of lidar data arrives and the pose information of the IMU when m frames of lidar data are predicted to get The m+1 frames of IMU pose information are subtracted to obtain the pose residual of the IMU at the time of m frames of lidar data), and the state between the two moments is constrained to obtain the IMU residual:

S3. Project the measurement data of the lidar to the IMU coordinate system, and then project it to the world coordinate system to obtain a point cloud image (as shown in Figure 5), and obtain the distance residual between the lidar point and the characteristic line and characteristic surface corresponding to the point cloud image. Difference;

Among them, including:

Perform feature extraction on the point cloud image to obtain line feature points and surface feature points (as shown in Figures 6 and 7);

Construct the line fitted from the lidar point to the line feature point in the point cloud image and the distance equation to the surface fitted by the feature point in the point cloud image;

Minimize the two distance equations to obtain the point-to-line distance residual and the point-to-surface distance residual, respectively.

specific:

S3.1 extracts features from the lidar point cloud. The specific process of extracting features includes the following sub-steps:

(1) Deal with the point cloud distortion problem of lidar. It is assumed that the carrier such as the robot moves at a uniform speed during the working process, that is, the linear velocity and angular velocity of the lidar remain constant during one scan of the lidar. Assuming that it is the starting moment when the lidar starts scanning, and the end moment when the lidar completes a scan, the relative pose transformation of the lidar relative to the moment is recorded as, and then the lidar point cloud i can be compensated according to the following formula:

则曲率的公式为：(2) Denote the point cloud after lidar point cloud distortion processing as T, a certain point in the point cloud T as i, the set of adjacent points around i as the center as S, and the curvature of each point as c, mark the spatial coordinates of the k-th line lidar point i in the lidar coordinate system as

Then the formula for curvature is:

(3) Divide the lidar point cloud of each frame into six equal parts, and calculate the curvature of the points in each area. After calculating the curvature of each point, sort them according to the size of the curvature. As line feature points, the curvature less than 0.1 is recorded as surface feature points.

S3.2 Project the lidar point to the IMU using coordinate transformation, and use the IMU pre-integration to project it to the world coordinate system. The feature point cloud of the current radar and the feature surface and feature line corresponding to the generated point cloud image are processed based on point-to-point and point-to-point Match the radar feature of the line; use the nearest neighbor search algorithm (KDTree search method) to find the closest point j of point i in the local point cloud image, and find the next closest point l around j, so (j, l) is called a point i corresponds to the edge in the graph spliced into the point cloud; the associated plane feature method: in the same way, first find the closest point j in the graph spliced by i in the point cloud, find the next closest points l and m around j, and set ( j,l,m) is called point i as the surface corresponding to the point cloud image.

As shown in Figure 8, the point-to-line and point-to-surface distance equations can be constructed according to the point-to-line and point-to-surface geometric constraint graphs, which are used as the associated parameters of the IMU measurement data and the lidar measurement data. Then the LiDAR point-to-line and point-to-surface distance residuals are expressed as:

为第k+1帧点云的线特征点的坐标，

为第k+1帧点云的面特征点的坐标。Among them, i, j, v, w are the corresponding feature association points,

is the coordinate of the line feature point of the point cloud of the k+1th frame,

is the coordinate of the surface feature point of the point cloud of the k+1th frame.

S4. Based on the graphical model, the initial objective function and the initial optimization incremental equation are obtained through the IMU residual and the distance residual;

in,

Based on the graphical model, the external parameters of lidar and IMU can be expressed as maximum likelihood estimation:

in,

Z represents the measured value;

F represents the initialization objective function;

According to the IMU residual and distance residual, the initialization objective function is expressed as:

in,

de and dp represent the distance residuals between the lidar point and the corresponding feature line and feature surface in the point cloud image, respectively;

表示IMU残差；

Represents the IMU residual;

Then, the incremental optimization equation is obtained by nonlinear optimization of most objective functions:

HδX=b

in:

J represents the Jacobian equation of the residual state quantity;

C represents the covariance matrix of the state;

r represents the residual of the cost equation;

δX represents the error state to be estimated;

By solving the obtained error state δX to be estimated, and updating the variable X _r to be optimized, then:

in,

○ indicates a small angle update of the quaternion.

S5. Set a constant frame sliding window, obtain the marginalization incremental optimization equation according to the initial optimization incremental equation, and calculate the marginalization residual term;

Among them, including:

In the prior art, in the optimization process of the objective function, the amount of calculation will increase with the number of key frames and the scale of the state to be optimized. It is completed in the sliding window, and a constant number of frames is maintained in the sliding window, for example, the state quantity of 11 frames is maintained, and the balance between calculation efficiency and accuracy can be achieved by setting the number of frames; Move out an old keyframe. Directly discarding the state quantity of the old frame will result in the loss of constraint information between other frames in the sliding window and this frame. By adopting marginalization, the constraint information corresponding to the discarded state quantity can be retained as a priori residual term (marginalized residual term).

Using the marginalization method, when a key frame is entered in the sliding window, while retaining the constraints corresponding to the key frame to be discarded, the marginalization incremental optimization equation is expressed as:

Elimination using Schur:

After elimination, the marginalization optimization equation is obtained:

in,

X _m represents the amount of optimization that needs to be marginalized;

X _r represents the variable that needs to be preserved for optimization;

Since the marginalization optimization equation is obtained by elimination, in fact, no constraint information is lost in the equation, but more constraints are added to the reserved optimization variables. Define _HP and b _po as:

表示为后续状态。在下一个优化中，后续状态的新估计将采用

的形式，它将返回新的

为：During marginalization, the

Represented as a follow-up state. In the next optimization, the new estimate of the subsequent state will take

form, it will return the new

for:

In addition, when the marginalization occurs, b _po is fixed, by providing H _p and b _p , according to the initialization optimization equation and the marginalization optimization equation, calculate the estimated value of the variable X _r that needs to be optimized before and after marginalization, and obtain the marginalization error term; the marginalization error term can be written as:

S6. Based on the graph model, the objective function is obtained through IMU residual, distance residual, and marginalization residual;

in,

As shown in Figure 4, the problem of solving the external parameters of lidar and IMU can be represented by a graph model; the graph structure mainly consists of two parts: nodes and edges. In Figure 4, node C represents the external parameters of the lidar and IMU to be solved, which represents rotation and translation, the black square represents the lidar factor, the black circle represents the IMU factor, and the node In represents the IMU at time tn. posture and speed. The superscript n denotes the nth scan from the lidar, and the subscript W denotes the world coordinate system. The goal of this calibration method is mainly to estimate the external parameters C of the lidar and the IMU, as well as the information such as the direction R, position P and velocity V of the IMU for each lidar scan. The state of the main estimate can be represented using X, namely:

The external parameters of lidar and IMU can be expressed as maximum likelihood estimation, namely:

in,

Z represents the measured value;

F(X) represents the objective function;

The calibration method proposed in this paper adds a sliding window to control the optimization amount, which will involve the problem of marginalizing the prior residual of discarded frames, so that the point-to-surface distance and the point-to-surface distance of the lidar residual can be minimized. The distance of the line, the residual of the IMU measurement, and the residual of the marginalized prior are used to solve the problem of external parameter calibration. That is: solve the external parameters according to the IMU residual, distance residual and marginalized residual terms, so the objective function can be expressed as:

in,

de and dp represent the distance residuals between the lidar point and the corresponding feature line and feature surface in the point cloud image, respectively;

表示IMU残差；

Represents the IMU residual;

r _p represents the marginalized residual term.

S7. Minimize the objective function to obtain the external parameter calibration of the lidar and IMU.

in,

The Gauss-Newton method is used to minimize the IMU residuals, distance residuals and marginalized residuals in the objective function, and the external parameter calibration of lidar and IMU is obtained.

Advantages of the present invention:

Compared with the prior art, the present invention realizes the real-time calibration of the external parameters of the laser radar and the IMU by setting a sliding window to obtain the latest state of the sensor device in real time, which is simple and fast; at the same time, the present invention uses the laser radar coordinate system and the IMU coordinate in the calibration calculation. The relationship between the systems is used to generate a point cloud map, and the distance residuals of the LiDAR point-to-surface and LiDAR point-to-line are obtained corresponding to the feature lines and special faces in the point cloud map, and the distance residuals are used as constraints for external parameter calibration. The accuracy of external parameter calibration.

In order to ensure the computational complexity of the optimization process, the present invention sets the sliding window as a fixed frame, achieves a balance between computational efficiency and accuracy, and meets the requirements of real-time calibration.

When the invention adopts a fixed frame sliding window, in order to avoid the influence of discarding state data, the marginalization method is used to obtain the marginalized residual item, and then the marginalized residual item is used as the constituent item of the objective function, so as to further improve the external parameters of lidar and IMU. Optimized accuracy.

The above are only preferred embodiments of the present invention, and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims (10)

1. a three-dimensional laser radar and IMU external parameter calibration method based on graph optimization, is characterized in that, comprises: Obtain the measurement data of the lidar and IMU in the sensor device respectively; Perform IMU pre-integration on the measurement data of the IMU, obtain the estimated value of the pose transformation of the IMU at the next moment according to the IMU pre-integration, and calculate the IMU residual error; Project the measurement data of the lidar to the IMU coordinate system, and then project it to the world coordinate system to obtain a point cloud image, and obtain the distance residual between the lidar point and the characteristic line and the characteristic surface corresponding to the point cloud image; Based on the graphical model, the initial objective function and the initial optimization incremental equation are obtained through the IMU residual and the distance residual; Set a constant frame sliding window, obtain the marginalization incremental optimization equation according to the initial optimization incremental equation, and calculate the marginalization residual term; Based on the graph model, the objective function is obtained through the IMU residual, distance residual, and marginalized residual items; Minimize the objective function to obtain the external parameter calibration of lidar and IMU. 2. The method according to claim 1, wherein: obtaining the estimated value of the pose transformation of the IMU at the next moment according to the IMU pre-integration, comprising: Build an IMU motion model; Based on the IMU motion model, a quaternion method is used to obtain the IMU pose and state transformation equation of the lidar point between two adjacent frames; Calculate the estimated value of the pose transformation of the IMU at the next moment according to the IMU pose state transformation equation. 3. The method according to claim 1 or 2, wherein the IMU residual is calculated according to the estimated value of the pose transformation of the IMU at the next moment and the actual measurement value of the IMU at the next moment. 4. The method according to claim 1, wherein the obtaining the distance residuals of the characteristic lines and characteristic surfaces corresponding to the lidar point and the point cloud image comprises: Perform feature extraction on the point cloud image to obtain line feature points and surface feature points; respectively constructing the line fitting from the lidar point to the line feature point in the point cloud image and the distance equation to the surface fitting the surface feature point in the point cloud image; Minimize the two distance equations to obtain the point-to-line distance residual and the point-to-surface distance residual, respectively. 5. The method according to claim 4, wherein: feature extraction is performed on the point cloud image to obtain line feature points and surface feature points; comprising: Mark the spatial coordinates of the k-th line lidar point i in the lidar coordinate system as

The set of adjacent points near the center of i is denoted as S, and the formula of the curvature c is:

The curvature of each lidar point is calculated according to the curvature formula, and points with a curvature greater than 0.1 are recorded as line feature points, and points with a curvature less than 0.1 are recorded as surface feature points. 6. The method according to claim 1, wherein, based on the graph model, the initial objective function is obtained through the IMU residual and the distance residual, comprising: Based on the graphical model, the lidar and IMU extrinsic parameters can be expressed as maximum likelihood estimation:

in, Z represents the measured value; F represents the initialization objective function; According to the IMU residual and distance residual, the initialization objective function is expressed as:

in, de and dp represent the distance residuals between the lidar point and the corresponding feature line and feature surface in the point cloud image, respectively;

represents the IMU residual. 7. The method according to claim 1, wherein the obtaining the initial optimization incremental equation comprises: By nonlinear optimization of most objective functions, the incremental optimization equation is obtained: HδX=b in: H=∑J ^T C ^-1 J b=∑J ^T C ^-1 r J represents the Jacobian equation of the residual state quantity; C represents the covariance matrix of the state; r represents the residual of the cost equation; δX represents the error state to be estimated; By solving the obtained error state δX to be estimated, and updating the variable X _r to be optimized, then:

in, ο represents a small angle update of the quaternion. 8. The method according to claim 7, wherein the setting constant frame sliding window, obtaining the marginalization incremental optimization equation according to the initial optimization incremental equation, and calculating the marginalization residual term; comprising: Using the marginalization method, when a key frame is entered in the sliding window, while retaining the constraints corresponding to the key frame to be discarded, the marginalization incremental optimization equation is expressed as:

The marginalization optimization equation is obtained by the elimination method:

in, X _m represents the amount of optimization that needs to be marginalized; X _r represents the variable that needs to be preserved for optimization; According to the initialization optimization equation and the marginalization optimization equation, the estimated value of the variable X _r to be optimized before and after marginalization is calculated, and the marginalization error term is obtained. 9. The method according to claim 1, wherein, based on the graph model, the objective function is obtained through the IMU residual, distance residual, and marginalization residual; comprising: Based on the graphical model, the lidar and IMU extrinsic parameters can be expressed as maximum likelihood estimation:

in, Z represents the measured value; F(X) represents the objective function; According to the IMU residual, distance residual and marginalization residual terms, the objective function is expressed as:

in, de and dp represent the distance residuals between the lidar point and the corresponding feature line and feature surface in the point cloud image, respectively;

Represents the IMU residual; r _p represents the marginalized residual term. 10. The method according to claim 1 or 9, wherein the Gauss-Newton method is used to minimize the IMU residuals, distance residuals and marginalization residuals in the objective function to obtain the external parameters of the lidar and IMU. Calibration.

Images