Joint calibration method of imu / camera / lidar based on graph optimization

Abstract

The application discloses a kind of IMU / camera / laser radar joint calibration method based on graph optimization, specific steps include: first, according to IMU information fitting rotating B spline curve, and using NDT algorithm to estimate radar rotation, initialization IMU / radar rotation external parameter;Subsequently, using feature point algorithm to estimate camera rotation, initialization IMU / camera rotation external parameter, gravity, scale factor, gyroscope zero offset;Then, using checkerboard calibration board, according to visual feature point and laser radar face distance minimum construction target function, initialization radar / camera external parameter;Finally, according to IMU constraint, camera constraint, laser radar constraint, based on calibration board camera and laser radar external parameter constraint, IMU / camera / laser radar external parameter mutual constraint construction joint optimization equation, solve and obtain IMU / camera / laser radar external parameter.Compared with prior art calibration method, the present application can realize IMU / camera / laser radar two two simultaneous calibration, effectively improve calibration convergence speed, precision and robustness.

Description

Technical Field

[0001] This invention is a graph-optimized joint calibration method for IMU / camera / LiDAR, belonging to multi-sensor calibration technology, and is particularly suitable for navigation in unknown environments. Background Technology

[0002] In the field of unmanned navigation, IMUs (Inertial Measurement Units) are widely used due to their high autonomy and anti-interference capabilities, but their errors accumulate over time. Cameras are inexpensive and possess rich semantic information, offering high navigation accuracy in textured environments, but they cannot acquire precise depth information. LiDAR can acquire precise 3D environmental information, especially when GNSS (Global Navigation Satellite System) data is unavailable, providing accurate pose information for autonomous vehicles. Integrated navigation based on IMU / camera / LiDAR is one of the future development directions for unmanned navigation. In IMU / camera / LiDAR systems, rotation and translation extrinsic parameters affect positioning accuracy and require online calibration. Existing calibration techniques calibrate IMU / camera, IMU / LiDAR, and camera / LiDAR separately, failing to calibrate the extrinsic parameters of all three simultaneously, neglecting the mutual constraints among them, and exhibiting low adaptability and robustness in complex environments. Therefore, a more effective calibration method is urgently needed to address these issues. Summary of the Invention

[0003] Purpose of the invention: In order to improve calibration efficiency and enhance calibration adaptability, this invention proposes a graph optimization-based joint calibration method for IMU / camera / LiDAR.

[0004] Technical solution: To achieve the objectives of this invention, the technical solution adopted is as follows:

[0005] A graph optimization-based joint calibration method for IMU / camera / LiDAR includes the following steps:

[0006] S1: Obtain IMU attitude information by fitting a rotated B-spline curve based on the IMU output angular velocity information;

[0007] S2: The lidar rotation is estimated using the NDT (Normal Distribution Transform) algorithm. The IMU / lidar rotation extrinsic parameters are initialized based on the IMU attitude information change estimated in S1 and the lidar rotation.

[0008] S3: Perform LiDAR distortion correction based on the IMU / LiDAR rotation extrinsic parameters initialized in S2;

[0009] S4: The feature point algorithm is used to estimate the camera rotation. Based on the IMU attitude information change estimated in S1 and the camera rotation, the IMU / camera rotation extrinsic parameters are initialized, and gravity, scale factor, gyroscope zero bias and velocity are initialized.

[0010] S5: Using a checkerboard calibration board, extract the coordinates of checkerboard points in the camera system. Based on the clustering algorithm, extract the normal vectors and center points of the laser points in the checkerboard system. Construct the objective function based on the minimum point-to-surface distance and initialize the extrinsic parameters of the lidar-camera system.

[0011] S6: Construct a joint optimization objective equation based on IMU constraints, LiDAR constraints, camera constraints, camera and LiDAR extrinsic parameter constraints based on the calibration board, and mutual constraints of IMU / camera / LiDAR extrinsic parameters. Solve the objective equation to obtain the IMU / camera / LiDAR extrinsic parameters.

[0012] The graph-optimized IMU / camera / LiDAR joint calibration method described above, specifically includes the following steps in step S1:

[0013] Based on the angular velocity information output by the IMU, a rotating B-spline curve is fitted, and the quaternion control points of the fitted curve are calculated by solving a least-squares problem.

[0014]

[0015] Where q0,....q N Let k be the quaternion control points of the B-spline curve, k be the current time, and M be the total sampling time. The IMU outputs angular velocity information at time k. This is the rotation matrix of the IMU from time 0 to time k.

[0016] The graph-optimized IMU / camera / LiDAR joint calibration method described above, specifically includes the following steps in step S2:

[0017] S2.1 uses the NDT algorithm to estimate the LiDAR rotation, with the objective function being:

[0018]

[0019] in, Let be the transformation matrix of the lidar from time k-1 to time k, where i is the current lidar point and n is the total number of lidar points. Let i be the position of the i-th laser point at time k. Let i be the position of the i-th laser point at time k-1;

[0020] S2.2 Initialize the IMU / LiDAR rotation extrinsic parameters based on the IMU attitude change estimated in S1 and the LiDAR rotation estimated in S2.1:

[0021]

[0022] in, Let be the rotational quaternion of the lidar from time k-1 to time k. For the rotation quaternion of the laser radar to the IMU, Let be the rotation quaternion of the IMU from time k-1 to time k.

[0023] The graph-optimized IMU / camera / LiDAR joint calibration method described above, specifically includes the following steps in step S3:

[0024] LiDAR distortion correction is performed based on the IMU / LiDAR rotation extrinsic parameters initialized by S2:

[0025]

[0026] in, Let i be the i-th lidar point in the 0th scan system at time k. Let i be the i-th lidar point in the j-th scanning system at time k. Let be the rotation matrix of the lidar from the j-th scan system to the 0-th scan system at time k.

[0027] The graph-optimized IMU / camera / LiDAR joint calibration method described above, specifically includes the following steps in step S4:

[0028] S4.1 uses a feature point algorithm to estimate camera rotation and solve for the pose and 3D position of landmarks in all frames;

[0029] S4.2 Initialize the IMU / camera rotation extrinsic parameters based on the IMU attitude change estimated by S1 and the camera rotation:

[0030]

[0031] in, Let k be the rotation quaternion of the camera from time k-1 to time k. For the rotation quaternion from the camera to the IMU, The rotation quaternion of the IMU from time k-1 to time k;

[0032] S4.3 Initialize gravity, scale factor, gyroscope zero bias, and velocity.

[0033] The graph-optimized IMU / camera / LiDAR joint calibration method described above, specifically includes the following steps in step S5:

[0034] Using a checkerboard calibration board, the coordinates of checkerboard points and plane normal vectors under the camera system are extracted. Based on the clustering algorithm, the plane normal vectors and center points corresponding to the checkerboard laser points under the lidar system are extracted. The objective function is constructed based on minimizing the point-to-plane distance, and the lidar / camera extrinsic parameters are initialized.

[0035] S5.1 Based on the plane normal vector N under the camera frame CAnd the plane normal vector N in the lidar system L Solve for the rotation matrix from the camera to the lidar:

[0036]

[0037] in, Let N be the rotation matrix from the camera to the lidar. C Let N be the plane normal vector in the camera frame. L Let be the plane normal vector in the lidar system.

[0038] S5.2 Construct the objective function based on minimizing the distance from a point in the camera system to the lidar surface and solve for it:

[0039]

[0040] Where t(x,y,z) is the translation extrinsic parameter from the camera to the lidar. The rotation matrix from the camera to the lidar. Let j be the point in the i-th frame of the camera system. Let i be the center point of the lidar system in the i-th frame. Let be the normal vector in the i-th frame of the lidar system.

[0041] The graph-optimized IMU / camera / LiDAR joint calibration method described above, specifically includes the following steps in step S6:

[0042] A joint optimization objective equation is constructed based on IMU constraints, LiDAR constraints, camera constraints, camera and LiDAR extrinsic parameter constraints based on the calibration board, and mutual constraints between IMU / camera / LiDAR extrinsic parameters:

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]

[0049]

[0050]

[0051] Where x is the optimization variable, To add residuals to the table, For the gyroscope residual, rl j For lidar residuals, For camera residuals, The distance residual between the camera and the laser point surface. Here, A represents the number of samples added to the external parameter residuals, W represents the number of gyroscope samples, L represents the number of radar samples, C represents the number of camera samples, O represents the number of camera-laser matching samples, and P represents the number of external parameter matching samples. For the rotation quaternion from the camera to the lidar, For the translation extrinsic parameters from the camera to the lidar, For the rotation quaternion from the camera to the IMU, For the translation extrinsic parameters from the camera to the IMU, For the rotation quaternion of the laser radar to the IMU, For the translational extrinsic parameters of the laser radar to the IMU, x q x is the control point for the continuous time trajectory rotation. p b is the control point for continuous time trajectory rotation and translation. a To add zero bias to the table, b g For zero bias of the gyroscope, The rotational quaternion from the initial IMU frame to gravity. For time k, the IMU outputs acceleration information, a(t) k Let be the fitted acceleration at time k. The IMU outputs angular velocity information at time k, w(t) k ) represents the fitted angular velocity at time k. For the rotation matrix from the lidar system to the IMU system, Let be the rotation matrix of the IMU from time j to time 0. Let i be the coordinates of the i-th lidar point at time j. For the translational extrinsic parameters from the lidar system to the IMU system, For the translation of the IMU from time j to time 0, For the translation extrinsic parameters from IMU to lidar, Let j be the coordinates of the lidar point at time 0. and Let be any two orthogonal bases on the tangent plane. For the estimated l-th landmark point at C i coordinate system For the l-th landmark at C i The system is the observation coordinate system, π c For camera projection matrix, Let x be the pixel x-coordinate of the l-th landmark. Let be the pixel ordinate of the l-th landmark. The rotation matrix from the camera to the IMU. For Ii To I i′ The rotation matrix, For the i-th point in C i coordinate system For the translation extrinsic parameters from the camera to the IMU, For I i To I i′ Translation transformation For the translation extrinsic parameters from the IMU to the camera, Let be the j-th point in the i-th frame of the camera system, and t(x,y,z) be the translation extrinsic parameter from the camera to the lidar. Let i be the center point of the lidar system in the i-th frame. This is the normal vector in the lidar system.

[0052] Compared with the prior art, the advantages of the present invention are as follows:

[0053] 1. This invention employs mutual constraints among the extrinsic parameters of IMU / LiDAR, IMU / Camera, and LiDAR / Camera. By constructing triangular constraints, the robustness of extrinsic parameter estimation is enhanced, and the efficiency of extrinsic parameter estimation is improved.

[0054] 2. This invention uses B-spline interpolation to calculate the continuous time trajectory estimation of the IMU, aligning the IMU / LiDAR / camera and reducing linear interpolation errors. Attached Figure Description

[0055] Figure 1 This is a flowchart of the graph optimization-based IMU / camera / LiDAR joint calibration method of the present invention;

[0056] Figure 2 This is a schematic diagram of factor graph optimization according to the present invention. Detailed Implementation

[0057] The technical solution of the present invention will be further described below with reference to the accompanying drawings. It should be understood that the embodiments provided below are merely for the purpose of fully and completely disclosing the present invention and fully conveying the technical concept of the invention to those skilled in the art. The present invention can be implemented in many different forms and is not limited to the embodiments described herein. The terminology used in the exemplary embodiments shown in the accompanying drawings is not intended to limit the invention.

[0058] Example 1: The present invention provides a graph optimization-based joint calibration method for IMU / camera / LiDAR, the implementation principle of which is as follows: Figure 1 As shown, the process mainly includes the following steps:

[0059] Step S1: Fit a rotated B-spline curve to obtain the IMU attitude information based on the IMU output angular velocity information.

[0060] Specifically, the process includes the following:

[0061] Based on the angular velocity information output by the IMU, a rotating B-spline curve is fitted, and the quaternion control points of the fitted curve are calculated by solving a least-squares problem.

[0062]

[0063] Where q0,....q N Let k be the quaternion control points of the B-spline curve, k be the current time, and M be the total sampling time. The IMU outputs angular velocity information at time k. This is the rotation matrix of the IMU from time 0 to time k.

[0064] Step S2: The NDT algorithm is used to estimate the lidar rotation. Based on the IMU attitude information change estimated in S1 and the lidar rotation, the IMU / lidar rotation extrinsic parameters are initialized.

[0065] Specifically, the process includes the following:

[0066] S2.1 uses the NDT algorithm to estimate the LiDAR rotation, with the objective function being:

[0067]

[0068] in, Let be the transformation matrix of the lidar from time k-1 to time k, where i is the current lidar point and n is the total number of lidar points. Let i be the position of the i-th laser point at time k. Let i be the position of the i-th laser point at time k-1;

[0069] S2.2 Initialize the IMU / LiDAR rotation extrinsic parameters based on the IMU attitude change estimated in S1 and the LiDAR rotation estimated in S2.1:

[0070]

[0071] in, Let be the rotational quaternion of the lidar from time k-1 to time k. For the rotation quaternion of the laser radar to the IMU, Let be the rotation quaternion of the IMU from time k-1 to time k.

[0072] Step S3: Perform LiDAR distortion correction based on the IMU / LiDAR rotation extrinsic parameters initialized in S2.

[0073] Specifically, the process includes the following:

[0074] LiDAR distortion correction is performed based on the IMU / LiDAR rotation extrinsic parameters initialized by S2:

[0075]

[0076] in, Let i be the i-th lidar point in the 0th scan system at time k. Let i be the i-th lidar point in the j-th scanning system at time k. Let be the rotation matrix of the lidar from the j-th scan system to the 0-th scan system at time k.

[0077] Step S4: The feature point algorithm is used to estimate the camera rotation. Based on the IMU attitude information change estimated in S1 and the camera rotation, the IMU / camera rotation extrinsic parameters are initialized, and gravity, scale factor, gyroscope bias, and velocity are initialized.

[0078] Specifically, the process includes the following:

[0079] S4.1 uses a feature point algorithm to estimate camera rotation and solve for the pose and 3D position of landmarks in all frames;

[0080] S4.2 Initialize the IMU / camera rotation extrinsic parameters based on the IMU attitude change estimated by S1 and the camera rotation:

[0081]

[0082] in, Let k be the rotation quaternion of the camera from time k-1 to time k. For the rotation quaternion from the camera to the IMU, The rotation quaternion of the IMU from time k-1 to time k;

[0083] S4.3 Initialize gravity, scale factor, gyroscope zero bias, and velocity.

[0084] Step S5: Using a checkerboard calibration board, extract the coordinates of the checkerboard points in the camera system. Based on a clustering algorithm, extract the normal vectors and center points of the laser points in the checkerboard system. Construct the objective function based on minimizing the point-to-surface distance, and initialize the extrinsic parameters of the LiDAR-camera system.

[0085] Specifically, the process includes the following:

[0086] S5.1 Based on the plane normal vector N under the camera frame C And the plane normal vector N in the lidar system L Solve for the rotation matrix from the camera to the lidar:

[0087]

[0088] in, Let N be the rotation matrix from the camera to the lidar. LLet N be the plane normal vector in the camera frame. L Let be the plane normal vector in the lidar system.

[0089] S5.2 Construct the objective function based on minimizing the distance from a point in the camera system to the lidar surface and solve for it:

[0090]

[0091] Where t(x,y,z) is the translation extrinsic parameter from the camera to the lidar. The rotation matrix from the camera to the lidar. Let j be the point in the i-th frame of the camera system. Let i be the center point of the lidar system in the i-th frame. Let be the normal vector in the i-th frame of the lidar system.

[0092] Step S6: Construct a joint optimization objective equation based on IMU constraints, LiDAR constraints, camera constraints, camera and LiDAR extrinsic parameter constraints based on the calibration board, and mutual constraints between IMU / camera / LiDAR extrinsic parameters. Solve the objective equation to obtain the IMU / camera / LiDAR extrinsic parameters.

[0093] Specifically, the process includes the following:

[0094] A joint optimization objective equation is constructed based on IMU constraints, LiDAR constraints, camera constraints, camera and LiDAR extrinsic parameter constraints based on the calibration board, and mutual constraints between IMU / camera / LiDAR extrinsic parameters:

[0095]

[0096]

[0097]

[0098]

[0099]

[0100]

[0101]

[0102]

[0103] Where x is the optimization variable, To add residuals to the table, For the gyroscope residual, r l j For lidar residuals, For camera residuals, The distance residual between the camera and the laser point surface. Here, A represents the number of samples added to the external parameter residuals, W represents the number of gyroscope samples, L represents the number of radar samples, C represents the number of camera samples, O represents the number of camera-laser matching samples, and P represents the number of external parameter matching samples. For the rotation quaternion from the camera to the lidar, For the translation extrinsic parameters from the camera to the lidar, For the rotation quaternion from the camera to the IMU, For the translation extrinsic parameters from the camera to the IMU, For the rotation quaternion of the laser radar to the IMU, For the translational extrinsic parameters of the laser radar to the IMU, x q x is the control point for the continuous time trajectory rotation. p b is the control point for continuous time trajectory rotation and translation. a To add zero bias to the table, b g For zero bias of the gyroscope, The rotational quaternion from the initial IMU frame to gravity. For time k, the IMU outputs acceleration information, a(t) k Let be the fitted acceleration at time k. The IMU outputs angular velocity information at time k, w(t) k ) represents the fitted angular velocity at time k. For the rotation matrix from the lidar system to the IMU system, Let be the rotation matrix of the IMU from time j to time 0. Let i be the coordinates of the i-th lidar point at time j. For the translational extrinsic parameters from the lidar system to the IMU system, For the translation of the IMU from time j to time 0, For the translation extrinsic parameters from IMU to lidar, Let j be the coordinates of the lidar point at time 0. and Let be any two orthogonal bases on the tangent plane. For the estimated l-th landmark point at C i coordinate system For the l-th landmark at C i The system is the observation coordinate system, π c For camera projection matrix, Let x be the pixel x-coordinate of the l-th landmark. Let be the pixel ordinate of the l-th landmark. The rotation matrix from the camera to the IMU. For I i To I i′ The rotation matrix, For the i-th point in Ci coordinate system For the translation extrinsic parameters from the camera to the IMU, For I i To I i′ Translation transformation For the translation extrinsic parameters from the IMU to the camera, Let be the j-th point in the i-th frame of the camera system, and t(x,y,z) be the translation extrinsic parameter from the camera to the lidar. Let i be the center point of the lidar system in the i-th frame. This is the normal vector in the lidar system.

[0104] The technical means disclosed in this invention are not limited to those disclosed by the aforementioned implementers, but also include technical solutions composed of any combination of the above-mentioned technical features. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of this invention, and these improvements and modifications are also considered within the scope of protection of this invention.

Claims

1. A graph optimization-based IMU / camera / LiDAR joint calibration method, characterized in that, Includes the following steps: S1: Obtain IMU attitude information by fitting a rotated B-spline curve based on the IMU output angular velocity information; S2: The NDT algorithm is used to estimate the LiDAR rotation. The IMU / LiDAR rotation extrinsic parameters are initialized based on the IMU attitude information change estimated in S1 and the LiDAR rotation. S3: Perform LiDAR distortion correction based on the IMU / LiDAR rotation extrinsic parameters initialized in S2; S4: The feature point algorithm is used to estimate the camera rotation. Based on the IMU attitude information change estimated in S1 and the camera rotation, the IMU / camera rotation extrinsic parameters are initialized, and gravity, scale factor, gyroscope zero bias and velocity are initialized. S5: Using a checkerboard calibration board, extract the coordinates of checkerboard points in the camera system. Based on the clustering algorithm, extract the normal vectors and center points of the laser points in the checkerboard system. Construct the objective function based on the minimum point-to-surface distance and initialize the extrinsic parameters of the lidar-camera system. S6: Construct a joint optimization objective equation based on IMU constraints, lidar constraints, camera constraints, camera and lidar extrinsic parameter constraints based on the calibration board, and mutual constraints of IMU / camera / lidar extrinsic parameters. Solve the objective equation to obtain the IMU / camera / lidar extrinsic parameters. Specifically, step S5 includes the following process: Using a checkerboard calibration board, the coordinates of checkerboard points and plane normal vectors under the camera system are extracted. Based on the clustering algorithm, the plane normal vectors and center points of the laser points on the checkerboard under the lidar system are extracted. The objective function is constructed based on the minimum point-to-plane distance, and the lidar / camera extrinsic parameters are initialized. S5.1 Based on the plane normal vector under the camera frame and the plane normal vector in the lidar system Solve for the rotation matrix from the camera to the lidar: in, The rotation matrix from the camera to the lidar. Let be the plane normal vector in the camera frame. This is the plane normal vector in the lidar system; S5.2 Construct the objective function based on minimizing the distance from a point in the camera system to the lidar surface and solve for it: in, For the translation extrinsic parameters from the camera to the lidar, The rotation matrix from the camera to the lidar. Let j be the point in the i-th frame of the camera system. Let i be the center point of the lidar system in the i-th frame. Let be the normal vector in the i-th frame of the lidar system; Step S6 specifically includes the following process: A joint optimization objective equation is constructed based on IMU constraints, LiDAR constraints, camera constraints, camera and LiDAR extrinsic parameter constraints based on the calibration board, and mutual constraints between IMU / camera / LiDAR extrinsic parameters: in, To optimize variables, To add residuals to the table, For the gyroscope residual, For lidar residuals, For camera residuals, The distance residual between the camera and the laser point surface. Here, A represents the number of samples added to the external parameter residuals, W represents the number of gyroscope samples, L represents the number of radar samples, C represents the number of camera samples, O represents the number of camera-laser matching samples, and P represents the number of external parameter matching samples. For the rotation quaternion from the camera to the lidar, For the translation extrinsic parameters from the camera to the lidar, For the rotation quaternion from the camera to the IMU, For the translation extrinsic parameters from the camera to the IMU, For the rotation quaternion of the laser radar to the IMU, For the translational extrinsic parameters of the laser radar to the IMU, For continuous time trajectory rotation control points, For continuous time trajectory rotation and translation control points, To add zero bias to the table, For zero bias of the gyroscope, The rotational quaternion from the initial IMU frame to gravity. The IMU outputs acceleration information at time k. Let k be the fitted acceleration. The IMU outputs angular velocity information at time k. Let k be the fitted angular velocity. For the rotation matrix from the lidar system to the IMU system, Let be the rotation matrix of the IMU from time j to time 0. Let i be the coordinates of the i-th lidar point at time j. For the translational extrinsic parameters from the lidar system to the IMU system, For the translation of the IMU from time j to time 0, For the translation extrinsic parameters from IMU to lidar, Let j be the coordinates of the lidar point at time 0. and Let be any two orthogonal bases on the tangent plane. To estimate the l-th landmark point at coordinate system For the l-th landmark point System observation coordinates, For camera projection matrix, Let x be the pixel x-coordinate of the l-th landmark. Let be the pixel ordinate of the l-th landmark. The rotation matrix from the camera to the IMU. for arrive The rotation matrix, For the i-th point at coordinate system For the translation extrinsic parameters from the camera to the IMU, for arrive Translation transformation For the translation extrinsic parameters from the IMU to the camera, Let j be the point in the i-th frame of the camera system. For the translation extrinsic parameters from the camera to the lidar, Let i be the center point of the lidar system in the i-th frame. This is the normal vector in the lidar system.

2. The graph-optimized IMU / camera / LiDAR joint calibration method according to claim 1, characterized in that, Step S1 specifically includes the following process: Based on the angular velocity information output by the IMU, a rotating B-spline curve is fitted, and the quaternion control points of the fitted curve are calculated by solving a least-squares problem. in, Let k be the quaternion control points of the B-spline curve, k be the current time, and M be the total sampling time. The IMU outputs angular velocity information at time k. This is the rotation matrix of the IMU from time 0 to time k.

3. The graph-optimized IMU / camera / LiDAR joint calibration method according to claim 2, characterized in that, Step S2 specifically includes the following process: S2.1 uses the NDT algorithm to estimate the LiDAR rotation, with the objective function being: in, for Time's up The transformation matrix of the lidar at any given time, where i is the current lidar point and n is the total number of lidar points. for The position of the i-th laser point at time i. for Position of the i-th laser point at time -1; S2.2 Initialize the IMU / LiDAR rotation extrinsic parameters based on the IMU attitude change estimated in S1 and the LiDAR rotation estimated in S2.1: in, for Time's up Rotational quaternions of a time-sensitive lidar For the rotation quaternion of the laser radar to the IMU, for Time's up The rotation quaternion of the IMU at time step.

4. The graph-optimized IMU / camera / LiDAR joint calibration method according to claim 2, characterized in that, Step S3 specifically includes the following process: LiDAR distortion correction is performed based on the S2-initialized IMU / LiDAR rotation extrinsic parameters: in, Let i be the i-th lidar point in the 0th scan system at time k. Let i be the i-th lidar point in the j-th scanning system at time k. Let be the rotation matrix of the lidar from the j-th scan system to the 0-th scan system at time k.

5. The graph-optimized IMU / camera / LiDAR joint calibration method according to claim 2, characterized in that, Step S4 specifically includes the following process: S4.1 uses a feature point algorithm to estimate camera rotation and solve for the pose and 3D position of landmarks in all frames; S4.2 Initialize the IMU / camera rotation extrinsic parameters based on the IMU attitude change estimated by S1 and the camera rotation: in, for Time's up The rotation quaternion of the time camera, For the rotation quaternion from the camera to the IMU, for Time's up Rotational quaternion of the IMU at time step; S4.3 Initialize gravity, scale factor, gyroscope zero bias, and velocity.

Images