Iterative optimization-based binocular camera-IMU (Inertial Measurement Unit) external parameter online calibration method

Abstract

The invention relates to an iterative optimization-based binocular camera-IMU external parameter on-line calibration method, which does not need to manually set a checkerboard calibration plate, and adopts an on-line iterative calibration method to directly iteratively calibrate rotation external parameters and translation external parameters of a left camera and an IMU in a binocular camera until convergence. Monocular vision inertia beam adjustment optimization is carried out based on the rotation external parameter and the translation external parameter of the left camera, 3D point coordinates of the landmark point are obtained, and the rotation external parameter and the translation external parameter between the left camera and the right camera are calibrated based on the 3D point coordinates of the landmark point of the left camera and the 2D point coordinates in the right camera. Compared with the prior art, the method has the advantages of saving time and material cost and the like.

Description

Technical Field

[0001] The present invention relates to the technical field of sensor extrinsic parameter calibration, and in particular to an extrinsic parameter online calibration method of a binocular camera-IMU based on iterative optimization. Background Art

[0002] The development of mobile robots is one of the important achievements of modern science and technology. With the continuous advancement of artificial intelligence technology, mobile robots are widely used in agriculture, industry, service industry and other fields. In order for mobile robots to operate stably in unknown environments, the first problem to be solved is the perception of the surrounding environment and the positioning of their own positions. The use of cameras and sensors can achieve excellent environmental perception and positioning performance with low power consumption. During the operation of mobile robots, the accuracy and stability of the entire system must be maintained. The first thing is to maintain the accuracy and stability of the sensor. One of the important tasks is to accurately calibrate the parameters of the sensor. The parameters of the sensor are divided into internal parameters and external parameters. The internal parameters generally refer to the inherent characteristics and geometric parameters of the sensor itself. These parameters are usually accurately calibrated and fixed when the sensor is designed and manufactured. Under normal use conditions, the internal parameters usually do not change significantly with time or environmental changes, and no calibration is required at any time. External parameters generally refer to the relative spatial position parameters between sensors, including translation, rotation and delay parameters. These parameters will change during the operation of the mobile robot due to possible vibration or friction, resulting in large errors in the robot's estimation of the environment and its own posture. To solve this problem, the external parameters of the sensor need to be calibrated online. The Chinese patent application "CN113763479A" discloses a calibration method for a catadioptric panoramic camera and an IMU sensor. The method calibrates the external parameters of the camera through the internal parameters of the camera, estimates the external parameters based on the IMU measurement parameters, and finally finds the relationship between the coordinate systems of the two based on the measurement models of the camera and IMU, and optimizes the external parameters between the catadioptric camera and the IMU. Although the external parameter calibration number is achieved, the calibration method used is offline calibration, and a checkerboard calibration board is artificially set, which consumes a lot of time and material costs when calibrating the external parameters.

[0003] Therefore, providing a camera-IMU extrinsic parameter calibration method that can save time and material costs is a technical problem that needs to be solved. Summary of the invention

[0004] The purpose of the present invention is to provide an online calibration method for the extrinsic parameters of a binocular camera-IMU based on iterative optimization in order to overcome the defects of the above-mentioned prior art. The rotation extrinsic parameters and translation extrinsic parameters between the left camera and the IMU are first calibrated iteratively, and then the rotation extrinsic parameters and translation extrinsic parameters between the left camera and the right camera are calibrated based on the iteratively calibrated rotation extrinsic parameters and translation extrinsic parameters.

[0005] The purpose of the present invention can be achieved by the following technical solutions:

[0006] The present invention provides an online calibration method for extrinsic parameters of a binocular camera-IMU based on iterative optimization. The method adopts an online iterative calibration method to calibrate the rotation extrinsic parameters and translation extrinsic parameters of a left camera and an IMU in a binocular camera until convergence, and calibrates the rotation extrinsic parameters and translation extrinsic parameters of a right camera based on the rotation extrinsic parameters and translation extrinsic parameters of the left camera.

[0007] As a preferred technical solution, the iterative calibration of the rotational and translational extrinsic parameters of the left camera and the IMU in the binocular camera includes:

[0008] Acquire multiple key frames, perform IMU pre-integration based on the key frames, and calibrate the rotation extrinsic parameters between the left camera and the IMU;

[0009] Estimating a gyroscope bias based on the left camera and the IMU rotation extrinsic parameter, and correcting the IMU pre-integration based on the estimated gyroscope bias;

[0010] The translation relationship between the left camera and the IMU is obtained based on the modified IMU pre-integration, and the translation extrinsic parameters, scale factors and gravity in the world coordinate system between the left camera and the IMU are calibrated based on the translation relationship;

[0011] Considering the accelerometer bias, and based on the calibrated translation extrinsics, scale factor, and gravity in the world coordinate system between the left camera and the IMU, optimize the translation extrinsics, scale factor, and gravity in the world coordinate system between the left camera and the IMU;

[0012] The standard deviation of the rotation extrinsic parameter and the optimized translation extrinsic parameter is calculated. If the standard deviation is less than the specified threshold, the rotation extrinsic parameter and the translation extrinsic parameter converge and the iteration is stopped.

[0013] As a preferred technical solution, the method for calibrating the rotational extrinsic parameters between the left camera and the IMU includes:

[0014] Acquire the feature points of each key frame and perform feature point matching, and use the pairwise constraint to obtain the essential matrix based on the matched feature points, and obtain the inter-frame motion of the camera according to the essential matrix, wherein the inter-frame motion of the camera includes rotation and translation;

[0015] Get IMU data and calculate the speed, rotation and position changes between two moments. The expressions are:

[0016]

[0017] in, Represents the rotation change of the IMU from time i to time j; and Represent the output of the gyroscope and the output of the accelerometer respectively; and They represent the bias of the gyroscope and accelerometer at time i respectively; Δt represents the time interval;

[0018] Based on the speed, rotation and position changes, the IMU is pre-integrated to calculate the rotation, speed and position in the world coordinate system, and the expression is:

[0019]

[0020] in, Indicates the rotation of the IMU in the world coordinate system at time i; Represents the rotation change of the IMU from time i to time j; It means that the gyro deviation δb g Through the Jacobian matrix After transformation, apply the exponential mapping Exp(·) to the rotation; the Jacobian matrix and Indicates the effect of gyroscope bias and accelerometer bias on the measured value; Indicates the speed of the IMU at time i; Δt ij Indicates a time interval; Indicates acceleration bias; Represents the velocity change of the IMU from time i to time j; Indicates the position of the IMU at time i; w g represents the gravity in the world coordinate system; Represents the position change of the IMU from time i to time j;

[0021] The first rotation relationship between the left camera and the IMU is obtained based on the inter-frame motion of the camera and the rotation, speed and position of the IMU. The first rotation relationship is obtained based on two adjacent key frames and is:

[0022]

[0023] Among them, q (·) Indicates rotation R (·) The corresponding quaternion; Represents the multiplication of quaternions; The quaternion representing the rotation of the left camera between time i and time i+1; The quaternion representing the rotation of the IMU between time i and time i+1; [·] L and[·] R Represents the left and right multiplication matrix transformation of the quaternion; Q i,i+1 express

[0024] Based on the first rotation relationship, a rotation linear overdetermined equation group is constructed, and its expression is:

[0025]

[0026] Where N is the number of key frames used; w N-1,N represents the weight, and w i,i+1 =exp(-‖e i,i+1 ‖·K o ), K o Represents the parameter of the amplified residual gain, e i,i+1 Represents the residual and is calculated by the rotation matrix calculated in the previous iteration, and the expression is: Represents the rotation extrinsic parameter of the current iteration;

[0027] The rotational extrinsic parameters are solved based on the rotational linear overdetermined equations.

[0028] As a preferred technical solution, the method for estimating the gyroscope bias is:

[0029] Based on the rotation extrinsic parameter, the second rotation relationship between the left camera and the IMU in two consecutive key frames is constructed, and the expression is:

[0030]

[0031] in, Represents the rotation matrix corresponding to the rotation extrinsic parameter calibrated in the previous iteration; express The inverse matrix of It means that the gyro deviation δb g Through the Jacobian matrix After the transformation, apply the exponential mapping Exp(·) to the rotation;

[0032] Construct the gyroscope bias cost function, which is expressed as:

[0033]

[0034] in, represents the estimated gyroscope bias; represents the IMU rotation from time i to time i+1; N represents the number of key frames;

[0035] The gyroscope bias cost matrix is ​​solved using the Gauss-Newton method.

[0036] As a preferred technical solution, the method for calibrating the translation extrinsic parameters, scale factor and gravity in the world coordinate system between the left camera and the IMU includes:

[0037] The position in the IMU pre-integration is corrected based on the estimated gyroscope bias, and its expression is:

[0038]

[0039] Among them, Δt ij Indicates a time interval; Indicates acceleration bias; Represents the velocity change of the IMU from time i to time j; Indicates the position of the IMU at time i; w g represents the gravity in the world coordinate system; Represents the position change of the IMU from time i to time j; represents the estimated gyroscope bias;

[0040] Based on the corrected position, the translation relationship between the left camera and the IMU in at least five consecutive key frames is constructed, and its expression is:

[0041]

[0042] in, W p B Represents the translation parameters of IMU in the world coordinate system; represents the translation extrinsic parameter from the left camera to the IMU, s represents the scale factor, Represents the translation parameters of the left camera in the world coordinate system; Indicates the rotation of the left camera at time i;

[0043] Based on the translation relationship described in at least five consecutive key frames, a linear equation group of translation, gravity and scale factor is constructed, and the linear equation group of translation, gravity and scale factor is solved to obtain the preliminary calibrated translation extrinsic parameters and the estimated scale factor and gravity in the world coordinate system.

[0044] As a preferred technical solution, the method for optimizing the translation extrinsic parameters, scale factor and gravity in the world coordinate system between the left camera and the IMU is:

[0045] Calculate the rotation of the earth's gravity coordinate system and the world coordinate system based on the estimated gravity in the world coordinate system, and add disturbance to obtain the rotation of gravity and earth's inertia;

[0046] Based on the rotation of gravity and earth inertia and taking into account accelerometer bias, the translation relationship is corrected, and a linear equation system of translation, gravity, scale factor and disturbance is constructed based on the corrected translation relationship;

[0047] The scale factor, perturbation and translation parameters are obtained based on the linear equations of translation, gravity, scale factor and perturbation, and the gravity is optimized through the perturbation.

[0048] As a preferred technical solution, the method for obtaining the scale factor, disturbance and translation parameters based on the linear equations of translation, gravity, scale factor and disturbance is the SVD decomposition method.

[0049] As a preferred technical solution, the method for calibrating the rotational extrinsic parameters and the translational extrinsic parameters of the right camera based on the rotational extrinsic parameters and the translational extrinsic parameters of the left camera is:

[0050] The left camera-IMU visual-inertial bundle adjustment is optimized based on the calibrated rotational and translational extrinsics of the left camera and IMU and the IMU measurement values;

[0051] The 3D points observed by the left camera-IMU visual inertial bundle adjustment and the 2D points observed by the right camera are obtained to construct a reprojection error, and the rotation extrinsic parameters and translation extrinsic parameters from the left camera to the right camera are obtained based on the reprojection error.

[0052] As a preferred technical solution, the left camera-IMU visual inertial bundle adjustment optimization method is:

[0053] The state variables are defined based on the calibrated rotation and translation extrinsics of the left camera and IMU and the measured values ​​of the IMU:

[0054]

[0055] Among them, ξ k represents the state variables of the kth key frame, including position, velocity, rotation in the world coordinate system, left camera bias and IMU bias, and uses the estimated scale factor as the initial value of the position; ρ i is the inverse depth of the i-th landmark point from its first observation;

[0056] Construct an optimized cost function to estimate the state variables, whose expression is:

[0057]

[0058] in, Represents the residual of IMU measurement; is the residual of the monocular visual inertial measurement.

[0059] As a preferred technical solution, a method for obtaining rotational extrinsic parameters and translational extrinsic parameters from the left camera to the right camera based on the reprojection error is:

[0060] Construct the reprojection error, which is expressed as:

[0061]

[0062] in, Represents the transpose of the rotation extrinsic parameter matrix from the left camera to the right camera, represents the translation extrinsic parameter from the left camera to the right camera, and π(·) represents the reprojection error from the 3D point to the normalized image plane;

[0063] The Gauss-Newton method is used to solve the reprojection error to obtain rotational extrinsic parameters and translational extrinsic parameters.

[0064] Compared with the prior art, the present invention introduces an iterative step to online mark the rotational extrinsic parameters and translational extrinsic parameters between the left camera and the IMU, which solves the problem of offline calibration requiring advance preparation of calibration plates, which is time-consuming and costly, and can realize rapid calibration of the extrinsic parameters between the camera and the IMU. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 is a flow chart of the method of the present invention;

[0066] Figure 2 It is a schematic diagram of the rotation relationship between the left camera and the IMU of the present invention;

[0067] Figure 3 Schematic diagram of the reprojection error of the present invention. DETAILED DESCRIPTION

[0068] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0069] Unless otherwise defined, the technical terms or scientific terms involved in this application should be understood by people with ordinary skills in the technical field to which this application belongs. The words "one", "a", "a", "the" and the like involved in this application do not indicate a quantitative limitation, and may represent the singular or plural. The terms "include", "comprise", "have" and any of their variations involved in this application are intended to cover non-exclusive inclusions; for example, a process, method, system, product or device that includes a series of steps or modules (units) is not limited to the listed steps or units, but may also include steps or units that are not listed, or may also include other steps or units inherent to these processes, methods, products or devices. The words "connect", "connected", "coupled" and the like involved in this application are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. The "multiple" involved in this application refers to two or more. "And / or" describes the association relationship of associated objects, indicating that there may be three relationships, for example, "A and / or B" can represent: A exists alone, A and B exist at the same time, and B exists alone. The character " / " generally indicates that the objects before and after are in an "or" relationship. The terms "first", "second", "third", etc. involved in this application are only used to distinguish similar objects and do not represent a specific ordering of the objects.

[0070] The present invention provides an untargeted online calibration method of external parameters of a binocular camera-IMU based on iterative optimization, comprising: obtaining the image data and IMU data of the camera, calculating the key frame and IMU pre-integration, calculating the rotation external parameters according to the rotation data of the camera and the rotation data of the IMU; calibrating the translation external parameters of the left camera and the IMU without considering the deviation of the accelerometer; introducing the direction of the earth's gravity, and optimizing the translation external parameters of the left camera and the IMU after considering the deviation of the accelerometer; iteratively calculating the parameters until convergence; performing the monocular visual inertial bundle adjustment of the left camera and the IMU; minimizing the reprojection error of the 3D point in the left camera to the camera imaging plane only, and calibrating the transfer matrix from the left camera to the right camera. The present invention can perform the initialization and online calibration of the binocular camera-IMU without calibration, has strong adaptability, and is easy to be transplanted in a variety of SLAM systems.

[0071] The process of the method provided by the present invention is as follows Figure 1 As shown, it includes two parts: iteratively marking the left camera-IMU external parameters and calibrating the right camera external parameters. The detailed process is as follows:

[0072] S1. Obtain at least 10 key frames, perform IMU pre-integration based on the key frames and calibrate the rotation extrinsic parameters between the left camera and the IMU, and assume that the rotation extrinsic parameters are the unit matrix and the gyroscope bias is 0.

[0073] S11. Obtain and process the image of the front-end camera and the data of the IMU, obtain at least 10 key frames, identify and match the feature points of each key frame, and use the pair-wise constraint based on the matched feature points to obtain the essential matrix, and obtain the inter-frame motion of the camera according to the essential matrix, wherein the inter-frame motion of the camera includes rotation and translation.

[0074] S12. Obtain IMU data to calculate the speed, rotation and position changes between two moments in the world coordinate system. The expression is:

[0075]

[0076] in, Represents the rotation change of the IMU from time i to time j; and Represent the output of the gyroscope and the output of the accelerometer respectively; and They represent the bias of the gyroscope and accelerometer at time i respectively; Δt represents the time interval.

[0077] S13. Based on the changes in speed, rotation and position, the IMU is pre-integrated to calculate the rotation, speed and position in the world coordinate system. The expression is:

[0078]

[0079] in, Indicates the rotation of the IMU in the world coordinate system at time i; Represents the rotation change of the IMU from time i to time j; It means that the gyro deviation δb g Through the Jacobian matrix After transformation, apply the exponential mapping Exp(·) to the rotation; the Jacobian matrix and Indicates the effect of gyroscope bias and accelerometer bias on the measured value; Indicates the speed of the IMU at time i; Δt ij Indicates a time interval; Indicates acceleration bias; Represents the velocity change of the IMU from time i to time j; Indicates the position of the IMU at time i; w g represents the gravity in the world coordinate system; Represents the position change of the IMU from time i to time j.

[0080] S14: Acquire the first rotation relationship.

[0081] The first rotation relationship between the left camera and the IMU is obtained based on the inter-frame motion of the camera and the rotation, speed and position of the IMU. In two consecutive key frames, the rotation from the camera at time i to the IMU at time i and then to the IMU at time i+1 is equivalent to the rotation from the camera at time i to the camera at time i+1 and then to the IMU at time i+1. For example, Figure 2 The relationship between the rotation of the left camera and the IMU is:

[0082]

[0083] Among them, q (·) Indicates rotation R (·) The corresponding quaternion; Represents the multiplication of quaternions; The quaternion representing the rotation of the left camera between time i and time i+1; The quaternion representing the rotation of the IMU between time i and time i+1; [·] L and[·] R Represents the left and right multiplication matrix transformation of the quaternion; Q i,i+1 express

[0084] S15. Construct a rotational linear overdetermined equation group based on the first rotation relationship.

[0085] When N consecutive key frames are marked as i, i+1, ..., there will be N-1 relations similar to the first rotation relation. Based on these relations, a rotation linear overdetermined equation group is constructed, and for each rotation linear overdetermined equation group, the weight w of the current iteration is calculated according to the residual of the result of the last calibration. i,i+1 , and when the residual is larger, the weight factor is smaller, and its expression is:

[0086]

[0087] Where N is the number of key frames used; w N-1,N represents the weight, and w i,i+1 =exp(-‖e i,i+1 ‖·K o ), K o Represents the parameter of the amplified residual gain, e i,i+1 Represents the residual and is calculated by the rotation matrix calculated in the previous iteration, and the expression is: Represents the rotation extrinsic parameter of the current iteration;

[0088] S16. Solve the rotation extrinsic parameters using the SVD method based on the rotation linear overdetermined equations.

[0089] S2. Without considering the acceleration bias, estimate the gyroscope bias based on the left camera and the IMU rotation extrinsic parameters, correct the IMU pre-integration based on the estimated gyroscope bias, and assume the initial values: the scale factor is -1, and the gravity and translation extrinsics are 0.

[0090] S21. Based on the rotation extrinsic parameter, a second rotation relationship between the left camera and the IMU in two consecutive key frames is constructed. In the two consecutive key frames, the rotation relationship between the camera and the IMU is expressed as: Where W represents the world coordinate system with the first key frame of the left camera as the origin; R WB Represents the rotation coordinate matrix of IMU in the world coordinate system, Represents the rotation coordinate matrix of the left camera in the world coordinate system, represents the rotation matrix corresponding to the rotation extrinsic parameter calibrated in step S1. Substituting it into the IMU pre-integration, the rotation relationship is:

[0091]

[0092] in, Represents the rotation matrix corresponding to the rotation extrinsic parameter calibrated in the previous iteration; express The inverse matrix of It means that the gyro deviation δb g Through the Jacobian matrix After the transformation, an exponential mapping Exp(·) is applied to the rotation.

[0093] S22. Use logarithmic mapping to construct the gyroscope bias cost function, which is expressed as:

[0094]

[0095] in, represents the estimated gyroscope bias; represents the IMU rotation from time i to time i+1; N represents the number of key frames; ||·|| represents the binary norm.

[0096] The above expression transforms small rotation changes into a rotation matrix by mapping to minimize the sum of the rotation residuals of all keyframes.

[0097] S23. Use the Gauss-Newton method to solve the gyroscope bias cost matrix.

[0098] S3. Obtain the translation relationship between the left camera and the IMU based on the modified IMU pre-integration, and calibrate the translation extrinsic parameters, scale factor and gravity in the world coordinate system between the left camera and the IMU based on the translation relationship.

[0099] S31. Without considering the accelerometer bias, a rough estimate is made of the scale factor, gravity, and left camera and IMU translation extrinsics. When the IMU measurement noise is ignored and the bias is assumed to remain constant, the position in the IMU pre-integration is corrected based on the estimated gyroscope bias. The expression is:

[0100]

[0101] Among them, Δt ij Indicates a time interval; Indicates acceleration bias; Represents the velocity change of the IMU from time i to time j; Indicates the position of the IMU at time i; w g represents the gravity in the world coordinate system; Represents the position change of the IMU from time i to time j; represents the estimated gyroscope bias; and, bias and Remains constant during pre-integration.

[0102] S32. Construct a translation relationship between the left camera and the IMU in at least five consecutive key frames based on the corrected position.

[0103] In the world coordinate system, the translation relationship between the left camera and the IMU is:

[0104]

[0105] in, W p B Represents the translation parameter of IMU in the world coordinate system, represents the translation extrinsic parameter from the left camera to the IMU, s represents the scale factor, Indicates the translation parameter of the left camera in the world coordinate system.

[0106] Substituting the corrected position into the above formula, the translation relationship between the left camera and the IMU is obtained as follows:

[0107]

[0108] in, W p B Represents the translation parameters of IMU in the world coordinate system; represents the translation extrinsic parameter from the left camera to the IMU, s represents the scale factor, Represents the translation parameters of the left camera in the world coordinate system; Indicates the rotation of the left camera at time i.

[0109] S33. In three consecutive key frames, the velocity in the pre-integration is substituted into the formula after the corrected position is substituted in step S32 to obtain two relational expressions. The two relational expressions are subtracted to obtain the translation relationship, and the linear equation of translation, gravity and scale factor is constructed, and its expression is:

[0110]

[0111] If the consecutive key frames are recorded as key frame 1, key frame 2 and key frame 3, where,

[0112]

[0113] Then N consecutive key frames can be constructed as a linear system of equations:

[0114]

[0115] Among them, the weight w i Expressed as: w i =exp(-‖e i ‖·K t ), K t Represents the parameter of the residual gain, the residual e i It is expressed as: s * , W g * , Represents the scale factor, gravity and translation extrinsics obtained in the previous iteration.

[0116] For the linear equation system, it can be seen from the linear equation system that for N consecutive key frames, an A can be constructed 3(N-2)×7 ·x 7×1 =B 3(N-2)×1 The linear equations are then decomposed using LU to obtain the decomposition result Note that there are 3(N-2) linear constraints and 7 unknowns, so at least N=5 key frames are required to solve the above equation.

[0117] S4. Considering the accelerometer bias, and based on the calibrated translation extrinsics, scale factor, and gravity in the world coordinate system between the left camera and the IMU, optimize the translation extrinsics, scale factor, and gravity in the world coordinate system between the left camera and the IMU.

[0118] S41. Since acceleration bias and gravity are difficult to distinguish, the gravity in the world coordinate system estimated in step S3 is used to calculate the rotation of the earth gravity coordinate system and the world coordinate system. It is assumed that the gravity acceleration in the earth inertial system is: G = 9.81 m / s -2 , then the rotation R of the earth's gravity coordinate system and the world coordinate system WE Its expression is:

[0119]

[0120] In the formula, is the unit vector representing gravity in the Earth coordinate system, Represents the gravity unit vector in the world coordinate system.

[0121] Adding disturbances to obtain the rotation of gravity and earth inertia, the expression is:

[0122]

[0123] Where δθ is the added disturbance, and a first-order approximation is used: Exp(δθ)≈I+[δθ] × ; I represents the identity matrix; [·] × Represents the antisymmetric matrix of a vector.

[0124] S42. Based on the rotation of gravity and the earth's inertia and taking into account the accelerometer bias, the translation relationship is corrected, and a set of linear equations of translation, gravity, scale factor and disturbance is constructed based on the corrected translation relationship.

[0125] Taking into account the effects of acceleration bias and gravity, the formula after substituting gravity and the rotation of the earth's inertia into the corrected position corrects the translation relationship to:

[0126]

[0127] Every three key frames are composed of linear equations based on the corrected translation relationship, and then N consecutive key frames can form a set of linear equations involving translation, gravity, scale factor and disturbance:

[0128]

[0129] Assume that the three consecutive key frames are 1, 2 and 3, then in the formula,

[0130]

[0131] in,[·] (:,1:2) Represents the first two columns of the matrix, the weight factor w i Defined as:

[0132]

[0133] s * , represents the scale factor, gravity perturbation and translation parameter obtained in the previous iteration; K r Represents the magnification factor of the residual.

[0134] S43. The linear equations based on translation, gravity, scale factor and perturbation are decomposed using SVD to obtain scale factor, perturbation and translation parameters.

[0135] The gravity is optimized by perturbation, and its expression is:

[0136] w g * =R WE Exp(δθ * ) E G.

[0137] S5. Calculate the standard deviation of the rotation extrinsic parameter and the optimized translation extrinsic parameter. If the standard deviation is less than a specified threshold, the rotation extrinsic parameter and the translation extrinsic parameter converge and the iteration is stopped. If the standard deviation is greater than or equal to the specified threshold, the iteration is continued.

[0138] S6. Perform left camera-IMU visual-inertial bundle adjustment optimization based on the calibrated rotational extrinsics and translational extrinsics of the left camera and IMU and the measurement values ​​of the IMU.

[0139] S61, based on the calibrated rotational extrinsic parameters and translational extrinsic parameters of the left camera and IMU and the measured values ​​of the IMU, define the state variables as:

[0140]

[0141] Among them, ξ k represents the state variables of the kth key frame, including position, velocity, rotation in the world coordinate system, left camera bias and IMU bias, and uses the estimated scale factor as the initial value of the position; ρ i is the inverse depth of the i-th landmark point from its first observation; and the scale factor estimated in step S4 is used as to optimize the sum of the IMU measurement residuals and the camera measurement residuals to obtain the maximum a posteriori estimate.

[0142] S62, construct an optimized cost function to estimate the state variables, minimize the sum of all IMU measurement residuals and camera vision measurement residuals to calculate the maximum a posteriori estimate, and its expression is:

[0143]

[0144] in, Represents the residual of IMU measurement; is the residual of monocular visual inertial measurement, and:

[0145]

[0146] in, is the estimated position of the landmark in the jth keyframe; x kis the observation value of the same landmark in the jth keyframe; (·) {xy} Represents the first two columns of a vector.

[0147] Thus, the 3D information of the landmark points can be obtained.

[0148] S7. Obtain the 3D points observed by the left camera-IMU visual inertial bundle after adjustment and the 2D points observed by the right camera to construct a reprojection error, and obtain the rotation extrinsic parameters and translation extrinsic parameters from the left camera to the right camera based on the reprojection error.

[0149] S71, reprojection error Figure 3 As shown, its expression is:

[0150]

[0151] in, Represents the transpose of the rotation extrinsic parameter matrix from the left camera to the right camera, represents the translation extrinsic parameter from the left camera to the right camera, and π(·) represents the reprojection error from the 3D point to the normalized image plane.

[0152] S72. Use the Gauss-Newton method to solve the reprojection error to obtain a transfer matrix from the left camera to the right camera, where the transfer matrix includes a rotation matrix and a translation matrix to calibrate the rotation extrinsic parameters and the translation extrinsic parameters from the left camera to the right camera.

[0153] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of various equivalent modifications or replacements within the technical scope disclosed by the present invention, and these modifications or replacements should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization, characterized in that: The method uses an online iterative calibration method to calibrate the rotation extrinsic parameters and translation extrinsic parameters of the left camera and IMU in the binocular camera until convergence, and calibrates the rotation extrinsic parameters and translation extrinsic parameters of the right camera based on the rotation extrinsic parameters and translation extrinsic parameters of the left camera.

2. The method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization according to claim 1, characterized in that: The iterative calibration of the rotational extrinsic parameters and translational extrinsic parameters of the left camera and the IMU in the binocular camera includes: Acquire multiple key frames, perform IMU pre-integration based on the key frames, and calibrate the rotation extrinsic parameters between the left camera and the IMU; Estimating a gyroscope bias based on the left camera and the IMU rotation extrinsic parameter, and correcting the IMU pre-integration based on the estimated gyroscope bias; The translation relationship between the left camera and the IMU is obtained based on the modified IMU pre-integration, and the translation extrinsic parameters, scale factors and gravity in the world coordinate system between the left camera and the IMU are calibrated based on the translation relationship; Considering the accelerometer bias, and based on the calibrated translation extrinsics, scale factor, and gravity in the world coordinate system between the left camera and the IMU, optimize the translation extrinsics, scale factor, and gravity in the world coordinate system between the left camera and the IMU; The standard deviation of the rotation extrinsic parameter and the optimized translation extrinsic parameter is calculated. If the standard deviation is less than the specified threshold, the rotation extrinsic parameter and the translation extrinsic parameter converge and the iteration is stopped.

3. The method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization according to claim 2, characterized in that: The method for calibrating the rotational external parameters between the left camera and the IMU includes: Acquire the feature points of each key frame and perform feature point matching, and use the pairwise constraint to obtain the essential matrix based on the matched feature points, and obtain the inter-frame motion of the camera according to the essential matrix, wherein the inter-frame motion of the camera includes rotation and translation; Get IMU data and calculate the speed, rotation and position changes between two moments. The expressions are: in, Represents the rotation change of the IMU from time i to time j; and Represent the output of the gyroscope and the output of the accelerometer respectively; and They represent the bias of the gyroscope and accelerometer at time i respectively; Δt represents the time interval; Based on the speed, rotation and position changes, the IMU is pre-integrated to calculate the rotation, speed and position in the world coordinate system, and the expression is: in, Indicates the rotation of the IMU in the world coordinate system at time i; Represents the rotation change of the IMU from time i to time j; It means that the gyro deviation δb g Through the Jacobian matrix After transformation, apply the exponential mapping Exp(·) to the rotation; the Jacobian matrix and Indicates the effect of gyroscope bias and accelerometer bias on the measured value; Indicates the speed of the IMU at time i; Δt ij Indicates a time interval; Indicates acceleration bias; Represents the velocity change of the IMU from time i to time j; Indicates the position of the IMU at time i; w g represents the gravity in the world coordinate system; Represents the position change of the IMU from time i to time j; The first rotation relationship between the left camera and the IMU is obtained based on the inter-frame motion of the camera and the rotation, speed and position of the IMU. The first rotation relationship is obtained based on two adjacent key frames and is: Among them, q (·) Indicates rotation R (·) The corresponding quaternion; Represents the multiplication of quaternions; The quaternion representing the rotation of the left camera between time i and time i+1; The quaternion representing the rotation of the IMU between time i and time i+1; [·] L and[·] R Represents the left and right multiplication matrix transformation of the quaternion; Q i,i+1 express Based on the first rotation relationship, a rotation linear overdetermined equation group is constructed, and its expression is: Where N is the number of key frames used; w N-1,N represents the weight, and w i,i+1 =exp(-‖e i,i+1 ‖·K o ), K o Represents the parameter of the amplified residual gain, e i,i+1 Represents the residual and is calculated by the rotation matrix calculated in the previous iteration, and the expression is: Represents the rotation extrinsic parameter of the current iteration; The rotational extrinsic parameters are solved based on the rotational linear overdetermined equations.

4. The method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization according to claim 2, characterized in that: The method for estimating the gyroscope bias is: Based on the rotation extrinsic parameter, the second rotation relationship between the left camera and the IMU in two consecutive key frames is constructed, and the expression is: in, Represents the rotation matrix corresponding to the rotation extrinsic parameter calibrated in the previous iteration; express The inverse matrix of It means that the gyro deviation δb g Through the Jacobian matrix After the transformation, apply the exponential mapping Exp(·) to the rotation; Construct the gyroscope bias cost function, which is expressed as: in, represents the estimated gyroscope bias; represents the IMU rotation from time i to time i+1; N represents the number of key frames; The gyroscope bias cost matrix is ​​solved using the Gauss-Newton method.

5. The method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization according to claim 2, characterized in that: The method for calibrating the translation extrinsic parameters, scale factor and gravity in the world coordinate system between the left camera and the IMU includes: The position in the IMU pre-integration is corrected based on the estimated gyroscope bias, and its expression is: Among them, Δt ij Indicates a time interval; Indicates acceleration bias; Represents the velocity change of the IMU from time i to time j; Indicates the position of the IMU at time i; w g represents the gravity in the world coordinate system; Represents the position change of the IMU from time i to time j; represents the estimated gyroscope bias; Based on the corrected position, the translation relationship between the left camera and the IMU in at least five consecutive key frames is constructed, and its expression is: in, W p B Represents the translation parameters of IMU in the world coordinate system; represents the translation extrinsic parameter from the left camera to the IMU, s represents the scale factor, Represents the translation parameters of the left camera in the world coordinate system; Indicates the rotation of the left camera at time i; Based on the translation relationship described in at least five consecutive key frames, a linear equation group of translation, gravity and scale factor is constructed, and the linear equation group of translation, gravity and scale factor is solved to obtain the preliminary calibrated translation extrinsic parameters and the estimated scale factor and gravity in the world coordinate system.

6. The method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization according to claim 2, characterized in that: The method for optimizing the translation extrinsic parameters, scale factor and gravity in the world coordinate system between the left camera and the IMU is: Calculate the rotation of the earth gravity coordinate system and the world coordinate system based on the estimated gravity in the world coordinate system, and add disturbance to obtain the rotation of gravity and earth inertia; Based on the rotation of gravity and earth inertia and taking into account accelerometer bias, the translation relationship is corrected, and a linear equation system of translation, gravity, scale factor and disturbance is constructed based on the corrected translation relationship; The scale factor, perturbation and translation parameters are obtained based on the linear equations of translation, gravity, scale factor and perturbation, and the gravity is optimized by the perturbation.

7. The method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization according to claim 6, characterized in that: The method for obtaining the scale factor, perturbation and translation parameters based on the linear equations of translation, gravity, scale factor and perturbation is the SVD decomposition method.

8. The method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization according to claim 1, characterized in that: The method for calibrating the rotational extrinsic parameters and translational extrinsic parameters of the right camera based on the rotational extrinsic parameters and translational extrinsic parameters of the left camera is: The left camera-IMU visual-inertial bundle adjustment is optimized based on the calibrated rotational and translational extrinsics of the left camera and IMU and the IMU measurement values; The 3D points observed by the left camera-IMU visual inertial bundle adjustment and the 2D points observed by the right camera are obtained to construct a reprojection error, and the rotation extrinsic parameters and translation extrinsic parameters from the left camera to the right camera are obtained based on the reprojection error.

9. The method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization according to claim 8, characterized in that: The left camera-IMU visual inertial bundle adjustment optimization method is: The state variables are defined based on the calibrated rotation and translation extrinsics of the left camera and IMU and the measured values ​​of the IMU: Among them, ξ k represents the state variables of the kth key frame, including position, velocity, rotation in the world coordinate system, left camera bias and IMU bias, and uses the estimated scale factor as the initial value of the position; ρ i is the inverse depth of the i-th landmark point from its first observation; Construct an optimized cost function to estimate the state variables, whose expression is: in, Represents the residual of IMU measurement; is the residual of the monocular visual inertial measurement.

10. The method for online calibration of external parameters of a binocular camera-IMU based on iterative optimization according to claim 8, characterized in that: The method for obtaining the rotation extrinsic parameters and translation extrinsic parameters from the left camera to the right camera based on the reprojection error is: Construct the reprojection error, which is expressed as: in, Represents the transpose of the rotation extrinsic parameter matrix from the left camera to the right camera, represents the translation extrinsic parameter from the left camera to the right camera, and π(·) represents the reprojection error from the 3D point to the normalized image plane; The Gauss-Newton method is used to solve the reprojection error to obtain the rotation extrinsic parameters and translation extrinsic parameters from the left camera to the right camera.

Images