Visual IMU batch external parameter calibration method based on servo drive variable motion

Abstract

The invention discloses a visual IMU batch external parameter calibration method based on servo drive variable motion, and the method comprises the steps: enabling a camera and an IMU to be fixed together, and then arranging the fixed camera and IMU on a rotating plate, and enabling the rotating plate to be driven by a servo motor to rotate; the method comprises the following steps: pre-calibrating known camera internal reference and IMU internal reference, and manually measuring a translation vector between an IMU and a camera; then a servo motor is started to drive the rotating plate to rotate, and when the camera and the IMU conduct variable-speed circular motion on the rotating plate along with the rotating plate, pose data are collected and subjected to optimization solving to obtain external parameter information of the camera and the IMU. According to the method, multiple groups of sensor external parameters can be measured at the same time, initial values are provided through initialization, back-end optimization is established, the sensor external parameters are optimized, and camera IMU external parameters with reliable precision are obtained.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot positioning and navigation technology, and in particular to a method for calibrating batch external parameters of a visual IMU based on servo-driven variable speed motion. Background Art

[0002] With the advancement of science and technology and the development of the market, in order to reduce production costs and improve production efficiency, the visual inertial sensor solution has gradually become one of the sensor fusion solutions for indoor positioning, mapping, and navigation. It has the advantages of low cost, rich visual information, and high output odometer frequency. It is worth noting that the installation external parameters between the camera and the IMU (the coordinate transformation between the camera coordinate system and the IMU coordinate system) are one of the necessary conditions for the stable and accurate positioning of the visual inertial solution. Since installation errors are inevitable during manual installation, each set of sensors needs external parameters.

[0003] The prior art discloses a method for self-calibration of the spatial relationship between a camera and an inertial sensor based on Lie group filtering, and the patent application number is 202110273084.6. The purpose of this technical solution is the same as that of the present technical solution in that both are to calibrate and obtain the external parameters of the camera and IMU; the difference from the present technical solution is that this technical solution establishes a linear calibration filter model based on the description of the Lie group. And according to the physical definition of Lie algebra, the error Lie algebra is calculated using the direction of the product of the predicted vector and the observed vector and the angle between the predicted vector and the observed vector. The mapping relationship between the SO(4) group and the Lie algebra is used to compensate for the camera-IMU spatial relationship rotation matrix and the gyroscope constant drift.

[0004] This patent solution collects the rotation angular rate information of the carrier system relative to the inertial system output by the gyroscope in the inertial measurement unit (IMU) and projects it on the carrier system. i ; Use the image provided by the camera to extract FAST feature points and track the LK optical flow method, and calculate the rotation matrix R of the camera between the two frames based on the matching feature points between the two frames c ; This technical solution has the following two disadvantages:

[0005] 1) Only use IMU angular velocity to establish a self-calibration model;

[0006] 2) Only the rotational extrinsic parameters are calibrated, but not the translational extrinsic parameters.

[0007] Therefore, this patent also cannot meet the demand for batch adjustment of IMU external parameters. Summary of the invention

[0008] The purpose of the present invention is to overcome the shortcomings of the prior art and provide a method for calibrating the external parameters of a visual IMU in batches based on servo-driven variable speed motion. The method can simultaneously measure multiple sets of sensor external parameters, use initialization to provide initial values, and establish back-end optimization to optimize the sensor external parameters to obtain accurate and reliable camera IMU external parameters. Batch calibration of IMU external parameters can be achieved with low cost and high efficiency.

[0009] In order to achieve the above object, the technical solution adopted by the present invention is: a visual IMU batch extrinsic parameter calibration method based on servo-driven variable speed motion, wherein the camera and the IMU are fixed together and arranged on a rotating plate, and the rotating plate is driven to rotate by a servo motor;

[0010] Pre-calibrate known camera intrinsic parameters, IMU intrinsic parameters, and manually measure the translation vector between IMU and camera;

[0011] Then start the servo motor to drive the rotating plate to rotate. When the camera and IMU follow the rotating plate's variable speed circular motion, the posture data is collected and the camera and IMU external parameter information is obtained through optimization.

[0012] When the rotating plate drives the camera IMU group to perform variable speed circular motion, the camera sensor uses the epipolar constraint to solve the relative posture of the camera motion between two adjacent frames, and the IMU sensor establishes a pre-integration constraint to obtain the relative posture of the IMU between two adjacent frames. Then, the posture extrinsic parameter model is established, and the least squares solution of the overpositive equation is solved; the least squares solution is used as the initial value for the optimization of the rotation extrinsic parameter and brought into the back-end visual inertial navigation SLAM optimization for optimization and solution; based on the sliding window visual inertial SLAM system, visual reprojection constraints, IMU pre-integration constraints, and sliding window marginalization constraints are established to optimize the camera IMU rotation extrinsic parameters and obtain the optimized translation and rotation extrinsic parameter values.

[0013] When the camera IMU device moves in a variable speed circular motion on the rotating plate, the essential matrix E is obtained by solving the epipolar constraints of the image feature points tracked by the camera. Four sets of solutions can be obtained by decomposing the essential matrix. Then, the 3D coordinates of the feature points are obtained by triangulating the feature points between the two frames. According to whether the depth of the 3D coordinate point in the camera coordinate system is greater than 0, the solution with more number is judged as the correct solution, thereby obtaining the c of two adjacent frames of the camera. k to c k+1 The frame rotation quaternion is represented as

[0014] b obtained by IMU pre-integration k to b k+1 The frame rotation quaternion is represented as Camera IMU rotation extrinsics Will Right multiplication Left multiplication Since the camera and IMU are rigidly connected, the external parameters are fixed at any time, so we get:

[0015]

[0016]

[0017] According to the definition of quaternion multiplication, quaternion matrix multiplication is obtained and rotation quaternion multiplication is converted into matrix multiplication:

[0018]

[0019] Simplified to:

[0020]

[0021] The matrix multiplications of multiple sets of two adjacent frames are combined into a system of equations:

[0022]

[0023] calculate and The kernel function is set to remove the rotation constraint with an angle difference greater than 7°, that is, the weight s is set to 0. For the angle difference between 5° and 7°, the weight s is set to 5 / angle difference, and the following model superpositive equation is obtained:

[0024]

[0025] Solution The least squares solution of this super-positive equation is used as the rotation external parameter and the manually measured camera and IMU translation external parameters are used as initial values. They are then brought into the established visual-inertial SLAM system for back-end optimization to optimize the camera, IMU external parameters and other visual-inertial system state quantities.

[0026] IMU pre-integration is only with It is related to the IMU sensor reading. Therefore, the direct constraint of the external parameter is in the visual reprojection residual:

[0027]

[0028] In the formula is the pixel coordinate of the 3D point in the normalized camera coordinate system of the i-th frame after being transformed to the camera coordinate system of the j-th frame, is the pixel coordinate actually tracked;

[0029] The normalized camera coordinate system of the i-th frame is transformed into the camera coordinate system of the j-th frame

[0030]

[0031] The optimization equation for constructing the backend SLAM system is:

[0032]

[0033] In the formula Residuals used for IMU and visual measurement, r p , H p is the prior information from marginalization.

[0034] The Gauss-Newton method is used to solve the least squares solution of the SLAM system optimization equation to obtain the optimized camera and IMU external parameter information.

[0035] A QR code is arranged around the rotating plate, and accurate position information is obtained by identifying the features of the QR code.

[0036] The advantages of the present invention are: using a servo drive board, the drive board can be installed with multiple sets of camera IMU devices, and the external parameter information of multiple sets of equipment can be measured simultaneously. The system is simple and low-cost, with less environmental modification and high efficiency. The method can simultaneously measure multiple sets of sensor external parameters, use initialization to provide initial values, and establish back-end optimization to optimize the sensor external parameters to obtain accurate and reliable camera IMU external parameters. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] The following is a brief description of the contents expressed in the drawings of the present invention and the symbols in the drawings:

[0038] Figure 1 A schematic diagram of the installation of an IMU camera when calibrating external parameters in the present invention;

[0039] Figure 2 This is a flow chart of the calibration method of the present invention. DETAILED DESCRIPTION

[0040] The specific implementation of the present invention will be further explained in detail below by describing the optimal embodiment with reference to the accompanying drawings.

[0041] This scheme proposes a visual IMU batch extrinsic parameter calibration method based on servo-driven variable speed motion. This method can simultaneously measure multiple groups of sensor extrinsic parameters, use initialization to provide initial values, and establish back-end optimization to optimize the sensor extrinsic parameters to obtain accurate and reliable camera IMU extrinsic parameters.

[0042] like Figure 1 , 2As shown, in this scheme, the visual IMU batch extrinsic parameter calibration method based on servo-driven variable speed motion is to fix the camera and IMU together and then set them on a rotating plate, and the rotating plate is driven to rotate by a servo motor; the known camera intrinsic parameters, IMU intrinsic parameters and the translation vector between the IMU and the camera are pre-calibrated; then the servo motor is started to drive the rotating plate to rotate, and when the camera and IMU follow the variable speed circular motion of the rotating plate on the rotating plate, the posture data is collected and the camera and IMU extrinsic parameter information is obtained through optimization solution.

[0043] When the rotating plate drives the camera IMU group to perform variable speed circular motion, the camera sensor uses the epipolar constraint to solve the relative posture of the camera motion between two adjacent frames, and the IMU sensor establishes a pre-integration constraint to obtain the relative posture of the IMU between two adjacent frames, and then establishes a posture extrinsic parameter model and solves the least squares solution of the super-positive equation; the least squares solution is used as the initial value for the rotation extrinsic optimization and brought into the back-end visual inertial navigation SLAM optimization for optimization and solution; based on the sliding window visual inertial SLAM system, a visual reprojection constraint, an IMU pre-integration constraint sliding window marginalization constraint are established to optimize the camera IMU rotation extrinsic parameters and obtain the optimized translation and rotation extrinsic parameter values. The specific scheme is introduced as follows:

[0044] The present invention aims to solve the problem of camera IMU external parameter calibration. Before the implementation of the technical solution, the following needs to be done accurately to obtain the corresponding data, specifically:

[0045] 1) Before using this calibration method, fix the position of the camera and IMU to ensure that no relative movement occurs during subsequent operations; that is, fix the camera and IMU together and when they rotate on the rotating plate, the two do not move relative to each other.

[0046] 2) Place the camera and IMU device on the servo-driven rotating plate (see the attached servo-driven rotating plate). Figure 1 shown);

[0047] 3) Given the calibrated camera intrinsic parameters, the relationship between camera pixels and real space can be obtained;

[0048] 4) Know the calibrated IMU internal parameters to remove random walk and scale factor errors caused by processing;

[0049] 5) Manually measure the translation vector between the IMU and the camera as the initial value for backend optimization;

[0050] 6) The surrounding environment of the rotating board needs to have obvious features to facilitate image feature tracking.

[0051] The specific process of the technical solution is as follows, and its flow chart is as follows Figure 2 As shown, the specific steps include:

[0052] 1) Start the servo drive board. When the camera IMU device moves in a variable speed circular motion on the rotating plate, the essential matrix E is obtained by solving the epipolar constraints of the image feature points tracked by the camera. Four sets of solutions can be obtained by decomposing the essential matrix. Then, the 3D coordinates of the feature points are obtained by triangulating the feature points between the two frames. According to whether the depth of the 3D point in the camera coordinate system is greater than 0, the solution with more number is judged as the correct solution, thereby obtaining the camera c k to c k+1 The frame rotation quaternion is represented as

[0053] 2) b obtained by IMU pre-integration k to b k+1 The frame rotation quaternion is represented as Camera IMU rotation extrinsics Will Right multiplication Left multiplication Since the camera and IMU are rigidly connected, the external parameters are fixed at any time, so we get:

[0054]

[0055]

[0056] In the formula Represents quaternion multiplication; Represents the camera IMU rotation quaternion external parameter; Indicates b in the IMU coordinate system k →b k+1 Rotate external parameters; Indicates b in the camera coordinate system k →b k+1 Rotate the external reference.

[0057] According to the definition of quaternion multiplication:

[0058]

[0059] Get quaternion matrix multiplication:

[0060]

[0061] in

[0062] Where [a] × Antisymmetric matrix:

[0063]

[0064] According to the above conversion, the rotation quaternion multiplication is converted into matrix multiplication:

[0065]

[0066] Simplified to:

[0067]

[0068] 3) Through b k →b k+1 、b k+1 →b k+2 、b k+2 →b k+3 …many frames form a system of equations:

[0069]

[0070] 4) Calculation and The kernel function is set to remove the rotation constraint when the angle difference is greater than 7°, that is, the weight s is set to 0. For the angle difference between 5° and 7°, the weight s is set to 5 / angle difference. According to the above settings, S1-S N The value of .

[0071]

[0072] 5) Solution The least squares solution of this superpositive definite equation is: SVD singular value decomposition of the A matrix:

[0073]

[0074] UDV is the matrix obtained by performing singular value decomposition on matrix A, where U and V are orthogonal matrices, D is a diagonal matrix, the column vectors of U are called left singular vectors, and the column vectors of V are called right singular vectors.

[0075] The matrix of right singular vectors:

[0076] V = [a0, a1, a2, a3]

[0077] where ||a i ||=1,a i ·a j = 0, and

[0078] Among them, a0, a1, a2, and a3 are the composition forms of the matrix V written as column vectors.

[0079] Solved (The rotation extrinsic parameter is equal to the right singular vector corresponding to the smallest singular value of the A matrix).

[0080] 6) Using the rotation extrinsic parameters solved above, manually measure the camera and IMU translation extrinsic parameters as initial values, establish the visual inertial SLAM system for back-end optimization, and optimize the camera, IMU extrinsic parameters and other visual inertial system state quantities. Most importantly, IMU pre-integration is only related to (b k Time to b k+t rotation) and IMU sensor readings. Therefore, the direct constraints on the external parameters are in the visual reprojection residual:

[0081]

[0082] In the formula is the pixel coordinate of the 3D point in the normalized camera coordinate system of the i-th frame after being transformed to the camera coordinate system of the j-th frame, is the actual tracked pixel coordinate.

[0083] The normalized camera coordinate system of the i-th frame is transformed into the camera coordinate system of the j-th frame

[0084]

[0085] In the formula is the rotation matrix from the IMU coordinate system to the camera coordinate system, is the rotation matrix from the world coordinate system to the IMU coordinate system of the jth frame is the rotation matrix from the IMU coordinate system to the world coordinate system in the i-th frame, R bc is the rotation matrix from the camera coordinate system to the IMU coordinate system, P ci is the coordinate of the feature point in the normalized camera coordinate system of the i-th frame, p bc is the translation vector from the camera coordinate system to the IMU coordinate system, is the translation transformation from the IMU coordinate system to the world coordinate system of the i-th frame, It is the translation transformation from the j-th frame IMU coordinate system to the world coordinate system.

[0086] The optimization equation for constructing the backend SLAM system is:

[0087]

[0088] In the formula, x represents the pose sequence of all image key frames in the sliding window, B represents the set of IMU data frames between adjacent image key frames in the sliding window, represents the set of all image key frames in the sliding window, ρ represents the kernel function, Indicates that from IMU data frame b k To IMU data frame b k+1 The change weight of represents the lth image key frame c in the sliding window lTo the jth image key frame c j The change weight, r p , H p Represents the prior information of self-marginalization.

[0089] The Gauss-Newton method can be used to solve the least squares solution of the equation, and the optimized camera and IMU external parameter information can be obtained.

[0090] This solution needs to be measured and calibrated in a place with good lighting and distinct surrounding features to ensure that enough feature points can be tracked. It is very easy to fail in a single and repetitive environment. Moreover, in the process of visual tracking of feature points, the reliability of the pose value solved by the visual extreme constraint is not high. Therefore, a QR code can be placed near the rotating plate, and accurate pose information can be obtained by identifying the QR code features. This pose information can be used as a known quantity in the back-end constraints to avoid optimizing the depth value of the QR code features. Another method is to use the uniform acceleration motion of the servo motor and set the acceleration as a known quantity to establish a pre-integration constraint model and optimize the external parameters.

[0091] Obviously, the specific implementation of the present invention is not limited to the above-mentioned methods. As long as various non-substantial improvements are made using the method concept and technical solution of the present invention, they are all within the protection scope of the present invention.

Claims

1. A batch extrinsic parameter calibration method for visual IMU based on servo-driven variable speed motion, characterized by: The camera and the IMU are fixed together and placed on a rotating plate, and the rotating plate is driven to rotate by a servo motor; Pre-calibrate known camera intrinsic parameters, IMU intrinsic parameters, and manually measure the translation vector between IMU and camera; Then start the servo motor to drive the rotating plate to rotate. When the camera and IMU follow the rotating plate's variable speed circular motion, the posture data is collected and the camera and IMU external parameter information is obtained through optimization.

2. The method for batch extrinsic parameter calibration of a visual IMU based on servo-driven variable speed motion as claimed in claim 1, characterized in that: When the rotating plate drives the camera IMU group to perform variable speed circular motion, the camera sensor uses the epipolar constraint to solve the relative posture of the camera motion between two adjacent frames, and the IMU sensor establishes a pre-integration constraint to obtain the relative posture of the IMU between two adjacent frames. Then, the posture extrinsic parameter model is established, and the least squares solution of the overpositive equation is solved; the least squares solution is used as the initial value for the optimization of the rotation extrinsic parameter and brought into the back-end visual inertial navigation SLAM optimization for optimization and solution; based on the sliding window visual inertial SLAM system, visual reprojection constraints, IMU pre-integration constraints, and sliding window marginalization constraints are established to optimize the camera IMU rotation extrinsic parameters and obtain the optimized translation and rotation extrinsic parameter values.

3. The method for batch extrinsic parameter calibration of a visual IMU based on servo-driven variable speed motion as claimed in claim 2, characterized in that: When the camera IMU device moves in a variable speed circular motion on the rotating plate, the essential matrix E is obtained by solving the epipolar constraints of the image feature points tracked by the camera. Four sets of solutions can be obtained by decomposing the essential matrix. Then, the 3D coordinates of the feature points are obtained by triangulating the feature points between the two frames. According to whether the depth of the 3D coordinate point in the camera coordinate system is greater than 0, the solution with more number is judged as the correct solution, thereby obtaining the c of two adjacent frames of the camera. k to c k+1 The frame rotation quaternion is represented as 4. The method for batch extrinsic parameter calibration of a visual IMU based on servo-driven variable speed motion as claimed in claim 2, characterized in that: b obtained by IMU pre-integration k to b k+1 The frame rotation quaternion is represented as Camera IMU rotation extrinsics Will Right multiplication Left multiplication Since the camera and IMU are rigidly connected, the external parameters are fixed at any time, so we get: According to the definition of quaternion multiplication, quaternion matrix multiplication is obtained and rotation quaternion multiplication is converted into matrix multiplication: Simplified to:

5. The method for batch extrinsic parameter calibration of a visual IMU based on servo-driven variable speed motion as claimed in claim 3 or 4, characterized in that: The matrix multiplications of multiple sets of two adjacent frames are combined into a system of equations: calculate and The kernel function is set to remove the rotation constraint with an angle difference greater than 7°, that is, the weight s is set to 0. For the angle difference between 5° and 7°, the weight s is set to 5 / angle difference, and the following model superpositive equation is obtained:

6. The method for batch extrinsic parameter calibration of a visual IMU based on servo-driven variable speed motion as claimed in claim 5, characterized in that: Solution The least squares solution of this over-positive definite equation is taken as the rotation extrinsic parameter and the manually measured camera and IMU translation extrinsic parameters are taken as initial values. They are then brought into the established visual-inertial SLAM system for back-end optimization to optimize the camera, IMU extrinsic parameters and other visual-inertial system state quantities.

7. The method for batch extrinsic parameter calibration of a visual IMU based on servo-driven variable speed motion as claimed in claim 6, characterized in that: IMU pre-integration is only with It is related to the IMU sensor readings, so the direct constraint of the external parameters is in the visual reprojection residual: In the formula is the pixel coordinate of the 3D point in the normalized camera coordinate system of the i-th frame after being transformed to the camera coordinate system of the j-th frame, is the pixel coordinate actually tracked; The normalized camera coordinate system of the i-th frame is transformed into the camera coordinate system of the j-th frame The optimization equation for constructing the backend SLAM system is: In the formula Residuals used for IMU and visual measurement, r p , H p is the prior information from marginalization.

8. The method for batch extrinsic parameter calibration of a visual IMU based on servo-driven variable speed motion as claimed in claim 7, characterized in that: The Gauss-Newton method is used to solve the least squares solution of the SLAM system optimization equation to obtain the optimized camera and IMU external parameter information.

9. The method for batch extrinsic parameter calibration of a visual IMU based on servo-driven variable speed motion as described in any one of claims 1 to 8, characterized in that: A QR code is arranged around the rotating plate, and accurate position information is obtained by identifying the features of the QR code.

Images