A camera-imu high-precision calibration method in a ground plane degradation scene

Abstract

The present application belongs to the technical field of sensor calibration, and particularly relates to a camera-IMU high-precision calibration method in a ground plane degeneration scene, which comprises the following steps: extracting a ground plane from an image captured by a camera, introducing a ground plane feature in a visual SLAM algorithm to obtain a high-precision camera pose, establishing a camera-IMU external parameter relationship based on the camera pose and an IMU pose, camera-IMU external parameter rotation calibration, and camera-IMU external parameter translation calibration based on ground plane constraints. In the method, the ground plane constraint is introduced in the translation relationship of the camera-IMU external parameter, which effectively enhances the observability and calculation accuracy of the vertical component of the translation, and can obtain a higher-precision and more stable camera-IMU external parameter calibration result, solves the vertical direction degeneration problem commonly existing in a ground motion platform, and provides a reliable basis for subsequent multi-sensor fusion positioning and mapping.

Description

Technical Field

[0001] This invention belongs to the field of sensor calibration technology, and specifically relates to a high-precision calibration method for camera-IMU in landscape degradation scenarios. Background Technology

[0002] Accurate calibration of cameras and IMUs is not only fundamental to the effective fusion of image information and inertial data, but also a crucial step in the field of multi-sensor fusion, widely applied in high-precision navigation, attitude estimation, robot perception, and unmanned systems. Existing camera-IMU calibration methods are mainly divided into two categories: one is calibration based on external markers, which uses specific visual markers to provide reference points for the camera and IMU. This method has high calibration accuracy, but it heavily relies on external features and has a cumbersome experimental procedure, making it poorly applicable in large-scale application scenarios. The other is calibration based on odometry. This method can complete calibration in natural environments without external features, so it has good applicability. However, the calibration accuracy is easily affected by the zero-bias error of the IMU and the scale uncertainty of monocular vision.

[0003] "Ground plane degradation scenario" refers to a situation where common unmanned systems (wheeled robots, quadruped robots, autonomous vehicles, etc.) primarily move on the ground. Due to limitations in platform size and weight, their movement is mainly concentrated in a two-dimensional plane, making it impossible to generate sufficient vertical movement during calibration. This presents two problems: firstly, conventional visual SLAM algorithms (such as ORB-SLAM2) are based on the assumption of a three-dimensional world and lack utilization of two-dimensional plane constraints; secondly, insufficient vertical movement leads to poor observability of the vertical components of extrinsic parameter translation, resulting in unstable solutions and decreased accuracy. Therefore, in this "ground plane degradation scenario," the estimation of camera-IMU extrinsic parameters, especially translational quantities, in the vertical direction often degrades or even becomes unobservable, becoming a significant bottleneck restricting high-precision fusion of multiple sensors. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention proposes a high-precision camera-IMU calibration method for ground plane degradation scenarios. First, the ground plane is extracted as a constraint for visual SLAM solution to obtain high-precision camera pose. Based on the camera pose and IMU pose, a camera-IMU extrinsic parameter relationship is constructed. By introducing ground plane constraints into the translation parameter relationship of the camera-IMU extrinsic parameters, the observability and solution accuracy of the vertical translation component are enhanced. This solves the problem that common unmanned systems often experience degradation or even unobservability of translation in the vertical direction during ground motion.

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

[0006] A high-precision camera-IMU calibration method for terrain degradation scenarios includes the following steps:

[0007] Step 1. Mount the camera and IMU on the same rigid carrier located on a flat surface. Secure the camera, IMU, and rigid carrier together so that their mounting parameters do not change over time. Obtain the rotation matrix from the ground plane coordinate system {G} to the camera coordinate system {C} from the images captured by the camera. And extract the ground plane;

[0008] Step 2. Introduce ground plane features into the visual SLAM algorithm to obtain high-precision camera pose: The extracted ground plane information is introduced as a constraint into visual SLAM, constraining the camera to a two-dimensional plane. Set of feature points on the ground plane at any given time The visual SLAM function employing planar constraints is: ;

[0009] Step 3. Establish camera-IMU extrinsic parameter relationship based on camera pose and IMU pose: First, obtain the IMU pose using IMU pre-integration, and then represent the IMU solution in the navigation coordinate system { Below; set For the navigation coordinate system { The transformation to the ground coordinate system {G} determines the camera pose. and posture The transformation relationship is as follows ;

[0010] For two consecutive epochs , The pose relationship between the camera and the IMU is as follows: , ; Position Write as rotation Peaceful relocation The combination is represented by 's', which denotes the monocular visual scale, and rotation is extracted separately. Translation The corresponding relationship yields the camera-IMU extrinsic parameters. , The relationships are as follows: Camera-IMU extrinsic rotation relationship and the camera-IMU extrinsic translation relationship ;

[0011] Step 4. Camera-IMU extrinsic parameter rotation calibration: Based on the camera-IMU extrinsic parameter rotation relationship established in Step 3. ,make , The two represent consecutive epochs, respectively. If the cameras and IMUs rotate relative to each other, then , rotate matrix Write as quaternions The form, that is By constructing a system of equations based on the rotation relationships between the camera and IMU at multiple time points, and using the weighted least squares method to solve for the rotation parameters of the camera-IMU extrinsic parameters, a quaternion is obtained. Solve After that, Convert to rotation matrix That is, to obtain the camera-IMU rotation parameters ;

[0012] Step 5. Camera-IMU extrinsic parameter translation calibration based on ground plane constraints: The rotation parameter relationship between the ground and the camera / IMU can be expressed as... [ R I G R C I R G C e 3 ] 1,2 = 0 , ;in Representing vectors [ 0,0,1 ] T , and The height of the camera above the ground, obtained through direct measurement. and These represent the height of the IMU relative to the ground. and , In step 1, the following has been obtained: These are the rotation parameters of the IMU to the ground obtained through the Madgwick filtering algorithm;

[0013] Ground planar motion constraints for: r g ( t C I )= [ R I G R L I R G L e 3 ] 1,2 e 3 T ( R I L R G I d I - R G L d L - t C I ) ;

[0014] calculate That is, to obtain high-precision camera-IMU translation parameters under the condition of applying planar constraints to the camera-IMU translation. .

[0015] Furthermore, the rotation matrix in step 1 The process of obtaining it is as follows:

[0016] Extract a set of ground points Applying principal component analysis to ground points covariance matrix ,set up Let {C} be the normal vector pointing to the ground in the camera coordinate system, and {a} be a point on the ground plane. satisfy:

[0017] Formula 1

[0018] in The height of the camera above the ground, obtained through direct measurement. It forms the ground plane normal vector, These are the three components of the normal vector, which is determined by the ground plane coordinate system;

[0019] Let the normal vector be relative to the ground coordinate system {G}. From two normal vectors and The rotation matrix from the ground coordinate system {G} to the camera coordinate system {C} can be obtained. :

[0020] R C G =exp([ ϕ GC n GC ] × ) Formula 2

[0021] in , They represent and The rotation angle and rotation vector between them [∙ ] × Represents an oblique symmetric matrix. Represents an exponential mapping; obtains That is, to divide the ground plane.

[0022] Furthermore, the process of obtaining the visual SLAM optimization function with planar constraints in step 2 is as follows:

[0023] Because in The current point obtained at each moment The corresponding adjacent plane in the map superior:

[0024] Formula 3

[0025] in It is the feature point reprojection function. These represent the camera pose state and the feature point coordinate state, respectively. It is relative to The unit normal vector of the nearest corresponding plane is defined. express Camera pose at any moment [ R C k , t C k ] ,point These are feature points located on the inner plane of the map. This indicates the first ground element extracted in the current frame that successfully matches an existing map. One feature point, This is the noise corresponding to the feature point;

[0026] Applying a first-order approximation to Equation 3, we get:

[0027] Formula 4

[0028] in , , express Higher-order subterms;

[0029] Since the robot's motion is restricted to two-dimensional space, a rotation matrix is ​​further used. Roll, pitch, and translation vectors of The constraint that components should maintain consistency; when given Ground feature points normal vector When, vector Should be with The xy planes of the system are orthogonal, that is:

[0030] [ R C G R C k ( n C k + ε n k ) ] 1.2 =0 Formula 5

[0031] in yes Observation noise, [⋅ ] 1.2 This indicates taking the first two elements of a (3×1) vector;

[0032] and of The displacement of the component relative to It is zero, that is:

[0033] Formula 6

[0034] in Representing vectors [ 0,0,1 ] T , This represents the coordinates of the feature point in the camera coordinate system;

[0035] Ground plane measurement model satisfy:

[0036] Formula 7

[0037] Applying a first-order approximation to Equation 7, we get:

[0038] Formula 8

[0039] in, , ;

[0040] The visual SLAM function with planar constraints applied, obtained from the above formula, is: ;in Indicates the process The next iteration estimate , Indicates the number of feature points. This represents the observed values ​​of feature points in a non-planar scene. Indicates the non-planar feature point at the th The Jacobian matrix corresponding to the next iteration. The observation noise matrix represents the non-planar feature points. Indicates the feature point of the ground plane at the th The observation at the next iteration Indicates the feature point of the ground plane at the th Jacobian matrix at the next iteration The observation noise matrix represents the feature points on the ground plane.

[0041] Furthermore, the process of obtaining the camera-IMU extrinsic parameter relationship model in step 3 is as follows:

[0042] First, the IMU pose is obtained using IMU pre-integration, and then the IMU solution is represented in the navigation coordinate system { Below; set For the navigation coordinate system { The transformation to the ground coordinate system {G} determines the camera pose. and posture The transformation relationship is as follows ;

[0043] For two consecutive epochs , The pose relationship between the camera and the IMU is as follows:

[0044] Formula 9

[0045] Formula 10

[0046] Inverting both sides of Equation 9, we get:

[0047] Formula 11

[0048] Multiplying the left and right sides of formulas 10 and 11 respectively, we get:

[0049]

[0050]

[0051] At this point, the unknown quantity is eliminated. , obtained the shape of Hand-eye alignment formula, position Write as rotation Peaceful relocation By combining the results, we get:

[0052] Formula 12

[0053] in Representing the monocular visual scale, expanding the formula and extracting the relationships corresponding to rotation and translation, we obtain:

[0054] Formula 13

[0055] Formula 14

[0056] Formulas 13 and 14 are derived from the hand-eye calibration relationship and include camera-IMU extrinsic parameters. , The relation can be used to solve and .

[0057] Moreover, in step 4 The process of obtaining it is as follows:

[0058] Based on the camera-IMU extrinsic rotation relationship established in step 3 ,make , The two represent consecutive epochs, respectively. The relative rotation of the camera and IMU between them will affect the formula. The right side Move it to the left to get:

[0059] Formula 15

[0060] This formula is also The form is a hand-eye calibration relation for rotation; the rotation matrix Write as quaternions In the form of:

[0061] Formula 16

[0062] in Formula 17

[0063] in represent identity matrix Representing quaternions The real part, Representing quaternions Composed of the imaginary part vector, Representative vector Constructed antisymmetric matrix;

[0064] According to formula 16, The system of equations is constructed at each time step as follows:

[0065] Formula 18

[0066] Quaternion The solution obtained using weighted least squares is as follows:

[0067] Formula 19

[0068] During the solution process, for each new observation in a new epoch, Formula 18 is expanded and the solution is recalculated:

[0069] Formula 20

[0070] After achieving an accurate solution for the relative rotation of the camera and IMU through inter-epoch iterations, the following was obtained. Then Convert to rotation matrix .

[0071] Compared with existing technologies, the beneficial effects of this technical solution are as follows:

[0072] I. To address the problem of unobservable camera-IMU extrinsic parameter translation in the vertical direction during ground motion calibration of common unmanned systems in "ground plane degradation scenarios" where sufficient vertical motion cannot be generated, the method provided in this invention first extracts the ground plane from the image, introduces ground plane constraints to visual SLAM to obtain high-precision camera pose, uses IMU pre-integration to obtain IMU pose, and constructs the camera-IMU extrinsic parameter relationship using the camera pose and IMU pose; solves the camera-IMU extrinsic parameter rotation through weighted least squares; and then applies ground plane constraints to the camera-IMU extrinsic parameter translation based on the camera-IMU extrinsic parameter translation relationship to improve the observability of the camera-IMU extrinsic parameter translation in the vertical direction.

[0073] Second, by extracting the ground from the images acquired by the camera and using them as constraints for visual SLAM calculation, not only is the accuracy of camera pose calculation improved, but the introduction of this constraint into the camera-IMU extrinsic parameter calibration effectively enhances the observability of the camera-IMU extrinsic parameter translation in the vertical direction, thereby obtaining higher accuracy and more stable camera-IMU extrinsic parameter calibration results, and thus providing a reliable foundation for subsequent multi-sensor fusion localization and mapping.

[0074] Third, this method can significantly improve the accuracy and robustness of camera-IMU calibration without additional hardware costs. It is especially suitable for ground motion platforms such as wheeled robots, quadruped robots and unmanned vehicles. Even when the platform size, weight and other limitations prevent sufficient three-axis motion from being obtained, it can still calibrate extrinsic parameters with high accuracy. Attached Figure Description

[0075] Figure 1 This is a schematic diagram of the calculation process of this method;

[0076] Figure 2 A schematic diagram showing the rotational relationship between the ground and the camera / IMU;

[0077] Figure 3 Example of a scene where the ground plane can be easily extracted. Detailed Implementation

[0078] The present invention will now be described in detail with reference to the accompanying drawings and embodiments. However, the scope of the present invention is not limited to the following embodiments.

[0079] "Ground plane degradation scenario" refers to the situation where common unmanned systems (wheeled robots, quadruped robots, unmanned vehicles, etc.) mainly move on the ground. Due to limitations such as platform size and weight, their movements are mainly concentrated in a two-dimensional plane, and they cannot generate sufficient movement in the vertical direction during the calibration process.

[0080] To achieve high-precision calibration of camera-IMU extrinsic parameters in a "degraded ground plane scenario," the overall computational process of this method is as follows: Figure 1As shown, the rotational relationship between the ground and the camera / IMU is illustrated in the diagram. Figure 2 As shown (the sensor platform is the M2DGR dataset). This method requires placing a rigid carrier on a flat ground and mounting the camera and IMU on this rigid carrier. To ensure that the mounting parameters of the camera and IMU do not change over time, the camera, IMU, and rigid carrier are fixedly connected. The camera lens needs to be fixed-focus and have a global shutter. First, images captured by the camera on the mobile platform are acquired and ground plane information is extracted. The extracted ground plane information is used as a constraint to obtain high-precision camera pose. At the same time, the IMU pose at each time step is obtained by pre-integration. The camera-IMU extrinsic parameter relationship is constructed using the camera pose and IMU pose. Then, the rotation parameters of the camera-IMU extrinsic parameters are solved using the weighted least squares method. Finally, based on the translation relationship of the camera-IMU extrinsic parameters, ground plane constraints are applied to the translation parameters of the camera-IMU extrinsic parameters to obtain high-precision translation parameters, enhancing the observability and accuracy of the vertical translation component.

[0081] A high-precision camera-IMU calibration method for terrain degradation scenarios includes the following steps:

[0082] Step 1. Obtain the rotation matrix from the ground coordinate system {G} to the camera coordinate system {C} from the image captured by the camera. And extract the ground plane.

[0083] Ground plane extraction often employs geometric methods and deep learning methods. It only requires that the surrounding environment be clearly distinguishable from the ground and that the ground can be segmented. For example, the surrounding environment can be colored, while the ground displays black and white patterns, and so on. Figure 3 The scene shown is an easy-to-extract ground plane. This example is not a fixed scene; the ground can be any pattern with features.

[0084] exist Figure 3 In the scene, a set of ground points can be quickly extracted based on color. Principal component analysis (PCA) was applied to ground points. covariance matrix ,set up Representing the camera coordinate system The middle point is the normal vector pointing to the ground, and the point on the ground plane. satisfy:

[0085] Formula 1

[0086] in This is a constant term, referring to the camera's height above the ground obtained through direct measurement. This forms the ground plane normal vector. These are the three components of the normal vector, which is determined by the ground plane coordinate system. Let's assume the normal vector is relative to the ground coordinate system. normal vector From two normal vectors and Can obtain Tie Rotation matrix of the system :

[0087] R C G =exp([ ϕ GC n GC ] × ) Formula 2

[0088] in , They represent and The rotation angle and rotation vector between them. [∙ ] × Represents an oblique symmetric matrix. This represents an exponential mapping. (Obtained) That is, to divide the ground plane.

[0089] Step 2. Introduce ground plane features into the visual SLAM algorithm to obtain high-precision camera pose:

[0090] Traditional visual SLAM algorithms are based on point features, such as PTAM and ORB-SLAM2. The objective function for point-based SLAM is:

[0091]

[0092] in These are the observation values ​​of feature points. It is the feature point reprojection function. These represent the camera pose state and the feature point coordinate state, respectively. To obtain more accurate visual SLAM results, this patent extracts... Tie Rotation matrix of the system Subsequently, planar features are introduced into the visual SLAM algorithm to constrain the camera to a two-dimensional plane. Let... Set of feature points on the ground plane at any given time Then in The current point obtained at each moment It should be kept in the corresponding adjacent plane on the map. superior:

[0093] Formula 3

[0094] in It is relative to The unit normal vector of the nearest corresponding plane is defined. express Camera pose at any moment [ R C k , t C k ] ,point These are feature points located on the inner plane of the map. This indicates the first ground element extracted in the current frame that successfully matches an existing map. One feature point, This is the noise corresponding to the feature point. Applying the first-order approximation to Equation 3, we get:

[0095] Formula 4

[0096] in , , express Higher-order sub-items.

[0097] Since the robot's motion is restricted to two-dimensional space, a rotation matrix is ​​further used. Roll, pitch, and translation vectors of The components should maintain consistency. When given... Ground feature points normal vector When, vector Should be with The xy planes of the system are orthogonal:

[0098] [ R C G R C k ( n C k + ε n k ) ] 1.2 =0 Formula 5

[0099] in yes Observation noise. [⋅ ] 1.2 This indicates taking the first two elements of a (3×1) vector. Furthermore, of The displacement of the component relative to It should be zero, that is:

[0100] Formula 6

[0101] in Representing vectors [ 0,0,1 ] T , This represents the coordinates of the feature point in the camera coordinate system.

[0102] Provide a ground plane measurement model :

[0103] Formula 7

[0104] Applying a first-order approximation to Equation 7 yields:

[0105] Formula 8

[0106] in, , Therefore, the planar constraint function is obtained, and the final visual SLAM optimization function with planar constraints is as follows:

[0107]

[0108] in Indicates the process The next iteration estimate , Indicates the number of feature points. This represents the observed values ​​of feature points in a non-planar scene (static features, such as buildings). Indicates the non-planar feature point at the th The Jacobian matrix corresponding to the next iteration. The observation noise matrix represents the non-planar feature points. Indicates the feature point of the ground plane at the th The observation at the next iteration Indicates the feature point of the ground plane at the th Jacobian matrix at the next iteration The observation noise matrix represents the feature points on the ground plane.

[0109] Step 3. Establish camera-IMU extrinsic parameter relationships based on camera pose and IMU pose:

[0110] The camera pose is defined in the world coordinate system. The coordinate system is selected as the world coordinate system, and the IMU solution results are represented in the navigation coordinate system. Below. Let's assume... For the navigation coordinate system { The transformation to the ground coordinate system {G} determines the camera pose. and posture The transformation relationship can be written as:

[0111]

[0112] For two consecutive epochs , The pose relationship between the camera and the IMU can be written in the following form:

[0113] Formula 9

[0114] Formula 10

[0115] Inverting both sides of equation 9, we get:

[0116] Formula 11

[0117] Multiplying the left and right sides of formulas 10 and 11 respectively, we get:

[0118]

[0119] The previous operation eliminated the unknown quantity. Ultimately, it was obtained in the form of Hand-eye alignment formula. Position Write as rotation Peaceful relocation By combining the forms, we can obtain:

[0120] Formula 12

[0121] in, This represents the monocular visual scale. Expanding Equation 12, omitting the derivation process, and extracting the rotation values ​​respectively... Translation The corresponding relation can be obtained as follows:

[0122] Formula 13

[0123] Formula 14

[0124] Formulas 13 and 14 are derived from the hand-eye calibration relationship and include camera-IMU extrinsic parameters. , The relation is also the solution. and The foundation.

[0125] Step 4. Camera-IMU extrinsic parameter rotation calibration:

[0126] According to Formula 13, let , The two represent consecutive epochs, respectively. The camera and IMU rotate relative to each other. The equations are... The right side Moving it to the left yields:

[0127] Formula 15

[0128] Formula 15 is also The form is a hand-eye calibration relation for rotation. Rotation matrix It belongs to the special orthogonal group SO(3) on the manifold, which is not easy to solve directly. The rotation matrix... Write as quaternions From the form, we can obtain:

[0129] Formula 16

[0130] in,

[0131] Formula 17

[0132] In the above formula, represent identity matrix Representing quaternions The real part, Representing quaternions Composed of the imaginary part vector, Representative vector Construct an antisymmetric matrix.

[0133] According to formula 16, The rotation relationship between the camera and the IMU at each moment can be constructed into the following system of equations:

[0134] Formula 18

[0135] Quaternion Solve using weighted least squares:

[0136] Formula 19

[0137] In practice, for each new epoch of observation, i.e., by expanding Formula 18 and resolving once, we obtain:

[0138] Formula 20

[0139] The accurate solution for the relative rotation of the camera and IMU is achieved through inter-epoch iteration. After the solution is obtained, Convert to rotation matrix This is used for the next step of adjusting the translation parameters. The estimate.

[0140] Step 5. Camera-IMU extrinsic parameter translation calibration based on ground plane constraints:

[0141] In the prior art, the translational parts of the two are related according to Formula 14 as follows:

[0142]

[0143] in The above formula includes the scale. Peaceful relocation Two unknown parameters, written in the following form:

[0144]

[0145] This formula can be simplified as follows: , A sequence of keyframes can be composed of For a system of equations, the optimization function can be defined as follows:

[0146]

[0147] Because the camera and IMU do not move sufficiently along the vertical axis on the ground plane, the vertical component of the camera-IMU extrinsic parameter translation calculated by the above formula is often not significant, making it difficult to obtain accurate results.

[0148] Therefore, this method introduces ground plane motion constraints to solve the problem of ground plane motion constraints based on... of The problem arises because components perpendicular to the planar motion direction cannot be observed. This constraint utilizes planar motion information to improve the calibration accuracy of camera-IMU extrinsic parameter translation, including the height of the camera and IMU relative to the ground. It can be directly measured. Furthermore, according to... Figure 3 The geometric relationship of the rotation parameters between the ground and each sensor can be obtained as follows:

[0149] &[ R I G R C I R G C e 3 ] 1,2 = 0 & e 3 T ( R I C R G I d I - R G C d C )= e 3 T t C I Formula 21

[0150] in Representing vectors [ 0,0,1 ] T , and The height of the camera above the ground, obtained through direct measurement in Formula 1. and These represent the height of the IMU relative to the ground. and , As obtained in step 1, It can calculate the rotation parameters of the IMU to the ground, and the process of obtaining them is as follows:

[0151] The raw IMU acceleration was obtained by filtering the IMU observations using the Madgwick filter. and angular velocity Including noise, especially in low-end IMUs, noise has a significant impact on calculation results. Madgwick filtering can be used to... and This results in smoother observations. During processing, this is achieved by constructing... and , The relationship, on Perform simultaneous solution (refer to the Madgwick filtering algorithm for details). The following parameters are obtained:

[0152]

[0153] in, These are the filtered acceleration and angular velocity, which can be used for subsequent IMU pre-integration calculations; These are the obtained rotation parameters from the IMU to the ground. For a flat ground, It is a constant.

[0154] According to Formula 21, the ground plane motion constraint is... for:

[0155] r g ( t C I )= [ R I G R L I R G L e 3 ] 1,2 e 3 T ( R I L R G I d I - R G L d L - t C I ) Formula 22

[0156] The final formula to be calculated is:

[0157] Formula 23

[0158] The calculation yields high-precision camera-IMU translation parameters under planar constraints on camera-IMU translation. .

[0159] In summary, the beneficial effects of this technical solution are as follows:

[0160] I. To address the problem of unobservable camera-IMU extrinsic parameter translation in the vertical direction during ground motion calibration of common unmanned systems in "ground plane degradation scenarios" where sufficient vertical motion cannot be generated, the method provided in this invention first extracts the ground plane from the image, introduces ground plane constraints to visual SLAM to obtain high-precision camera pose, uses IMU pre-integration to obtain IMU pose, and constructs the camera-IMU extrinsic parameter relationship using the camera pose and IMU pose; solves the camera-IMU extrinsic parameter rotation through weighted least squares; and then applies ground plane constraints to the camera-IMU extrinsic parameter translation based on the camera-IMU extrinsic parameter translation relationship to improve the observability of the camera-IMU extrinsic parameter translation in the vertical direction.

[0161] Second, by extracting the ground from the images acquired by the camera and using them as constraints for visual SLAM calculation, not only is the accuracy of camera pose calculation improved, but the introduction of this constraint into the camera-IMU extrinsic parameter calibration effectively enhances the observability of the camera-IMU extrinsic parameter translation in the vertical direction, thereby obtaining higher accuracy and more stable camera-IMU extrinsic parameter calibration results, and thus providing a reliable foundation for subsequent multi-sensor fusion localization and mapping.

[0162] Third, this method can significantly improve the accuracy and robustness of camera-IMU calibration without additional hardware costs. It is especially suitable for ground motion platforms such as wheeled robots, quadruped robots and unmanned vehicles. Even when the platform size, weight and other limitations prevent sufficient three-axis motion from being obtained, it can still calibrate extrinsic parameters with high accuracy.

Claims

1. A high-precision camera-IMU calibration method for terrain degradation scenarios, characterized in that... Includes the following steps: Step 1. Mount the camera and IMU on the same rigid carrier located on a flat surface. Secure the camera, IMU, and rigid carrier together so that their mounting parameters do not change over time. Obtain the rotation matrix from the ground plane coordinate system {G} to the camera coordinate system {C} from the images captured by the camera. And extract the ground plane; Step 2. Introduce ground plane features into the visual SLAM algorithm to obtain high-precision camera pose: The extracted ground plane information is introduced as a constraint into visual SLAM, constraining the camera to a two-dimensional plane. Set of feature points on the ground plane at any given time The visual SLAM function employing planar constraints is: ; Step 3. Establish camera-IMU extrinsic parameter relationship based on camera pose and IMU pose: First, obtain the IMU pose using IMU pre-integration, and then represent the IMU solution in the navigation coordinate system { Below; set For the navigation coordinate system { The transformation to the ground coordinate system {G} determines the camera pose. and posture The transformation relationship is as follows ; For two consecutive epochs , The pose relationship between the camera and the IMU is as follows: , ; position Write as rotation Peaceful relocation The combination is represented by 's', which denotes the monocular visual scale, and rotation is extracted separately. Translation The corresponding relationship yields the camera-IMU extrinsic parameters. , The relationships are as follows: Camera-IMU extrinsic rotation relationship and the camera-IMU extrinsic translation relationship ; Step 4. Camera-IMU extrinsic parameter rotation calibration: Based on the camera-IMU extrinsic parameter rotation relationship established in Step 3. ,make , The two represent consecutive epochs, respectively. If the cameras and IMUs rotate relative to each other, then , rotate matrix Write as quaternions The form, that is By constructing a system of equations based on the rotation relationships between the camera and IMU at multiple time points, and using the weighted least squares method to solve for the rotation parameters of the camera-IMU extrinsic parameters, a quaternion is obtained. Solve After that, Convert to rotation matrix That is, to obtain the camera-IMU rotation parameters ; Step 5. Camera-IMU extrinsic parameter translation calibration based on ground plane constraints: The rotation parameter relationship between the ground and the camera / IMU can be expressed as... , ;in Representing vectors , and The height of the camera above the ground, obtained through direct measurement. and These represent the height of the IMU relative to the ground. and , In step 1, the following has been obtained: These are the rotation parameters of the IMU to the ground obtained through the Madgwick filtering algorithm; Ground planar motion constraints for: ; calculate That is, to obtain high-precision camera-IMU translation parameters under the condition of applying planar constraints to the camera-IMU translation. .

2. The high-precision camera-IMU calibration method for terrain degradation scenarios according to claim 1, characterized in that, Rotation matrix in step 1 The process of obtaining it is as follows: Extract a set of ground points Applying principal component analysis to ground points covariance matrix ,set up Let {C} be the normal vector pointing to the ground in the camera coordinate system, and {a} be a point on the ground plane. satisfy: Official 1 in The height of the camera above the ground, obtained through direct measurement. It forms the ground plane normal vector, These are the three components of the normal vector; Let the normal vector be relative to the ground coordinate system {G}. From two normal vectors and The rotation matrix from the ground coordinate system {G} to the camera coordinate system {C} can be obtained. : Official 2 in , They represent and The rotation angle and rotation vector between them Represents an oblique symmetric matrix. Represents an exponential mapping; obtains That is, to divide the ground plane.

3. The high-precision camera-IMU calibration method for terrain degradation scenarios according to claim 1, characterized in that, The process of obtaining the visual SLAM optimization function with planar constraints in step 2 is as follows: Because in The current point obtained at each moment The corresponding adjacent plane in the map superior: Official 3 in It is the feature point reprojection function. These represent the camera pose state and the feature point coordinate state, respectively. It is relative to The unit normal vector of the nearest corresponding plane is defined. express Camera pose at any moment ,point These are feature points located on the inner plane of the map. This indicates the first ground element extracted in the current frame that successfully matches an existing map. One feature point, This is the noise corresponding to that feature point; Applying a first-order approximation to Equation 3, we get: Official 4 in , , express Higher-order subterms; Since the robot's motion is restricted to two-dimensional space, a rotation matrix is ​​further used. Roll, pitch, and translation vectors of The constraint that components should maintain consistency; when given Ground feature points normal vector When, vector Should be with The xy planes of the system are orthogonal, that is: Official 5 in yes Observation noise, This indicates taking the first two elements of a (3×1) vector; and of The displacement of the component relative to It is zero, that is: Official 6 in Representing vectors , This represents the coordinates of the feature point in the camera coordinate system; Ground plane measurement model satisfy: Official 7 Applying a first-order approximation to Equation 7, we get: Official 8 in, , ; The visual SLAM function with planar constraints applied, obtained from the above formula, is: ;in Indicates the process The next iteration estimate , Indicates the number of feature points. This represents the observed values ​​of feature points in a non-planar scene. Indicates the non-planar feature point at the th The Jacobian matrix corresponding to the next iteration. The observation noise matrix represents the non-planar feature points. Indicates the feature point of the ground plane at the th The observation at the next iteration Indicates the feature point of the ground plane at the th Jacobian matrix at the next iteration The observation noise matrix represents the feature points on the ground plane.

4. The high-precision camera-IMU calibration method for terrain degradation scenarios according to claim 1, characterized in that, The process of obtaining the camera-IMU extrinsic parameter relationship model in step 3 is as follows: First, the IMU pose is obtained using IMU pre-integration, and then the IMU solution is represented in the navigation coordinate system { Below; set For the navigation coordinate system { The transformation to the ground coordinate system {G} determines the camera pose. and posture The transformation relationship is as follows ; For two consecutive epochs , The pose relationship between the camera and the IMU is as follows: Official 9 Official 10 Inverting both sides of Equation 9, we get: Official 11 Multiplying the left and right sides of formulas 10 and 11 respectively, we get: At this point, the unknown quantity is eliminated. , obtained the shape of Hand-eye alignment formula, position Write as rotation Peaceful relocation By combining the results, we get: Official 12 in Representing the monocular visual scale, expanding the formula and extracting the relationships corresponding to rotation and translation, we obtain: Official 13 Official 14 Formulas 13 and 14 are derived from the hand-eye calibration relationship and include camera-IMU extrinsic parameters. , The relation can be used to solve and .

5. The high-precision camera-IMU calibration method for terrain degradation scenarios according to claim 1, characterized in that, In step 4 The process of obtaining it is as follows: Based on the camera-IMU extrinsic rotation relationship established in step 3 ,make , The two represent consecutive epochs, respectively. The relative rotation of the camera and IMU between them will affect the formula. The right side Move it to the left to get: Official 15 This formula is also The form is a hand-eye calibration relation for rotation; the rotation matrix Write as quaternions In the form of: Official 16 in Formula 17 in represent identity matrix Representing quaternions The real part, Representing quaternions The imaginary part is composed of vector, Representative vector Constructed antisymmetric matrix; According to formula 16, The system of equations is constructed at each time step as follows: Official 18 Quaternion The solution obtained using weighted least squares is as follows: Official 19 During the solution process, for each new observation in a new epoch, Formula 18 is expanded and the solution is recalculated: Official 20 After achieving an accurate solution for the relative rotation of the camera and IMU through inter-epoch iterations, the following was obtained. Then Convert to rotation matrix .

Images