Visual inertia wheel speed odometer diagram optimization online calibration method and system

Abstract

The invention discloses a visual inertia wheel speed odometer diagram optimization online calibration method and system, and the method comprises the steps: calibrating external parameters between a camera and an IMU (Inertial Measurement Unit), and calibrating external parameters between the IMU and a wheel speed odometer. Calibration of external parameters between the IMU and the wheel speed odometer comprises IMU-wheel speed odometer pre-integration derivation and graph optimization based on IMU-wheel speed odometer pre-integration and a visual factor, and in IMU-wheel speed odometer pre-integration derivation, a scale factor of the wheel speed odometer and the influence of attitude and position deviation between the IMU and the wheel speed odometer are considered. The method has the advantages that the scale factor of the wheel speed odometer and the influence of the attitude and position deviation between the IMU and the wheel speed odometer are fully considered, and the attitude and position matrix between the camera and the IMU and the attitude and position deviation between the IMU and the wheel speed odometer are estimated and optimized on line based on the graph model optimization theory; and online estimation and optimization of external parameters among the vision sensor, the IMU sensor and the wheel speed odometer sensor are realized.

Description

Technical Field

[0001] The present invention relates to the field of vehicle positioning, in particular to an online calibration method for visual inertial wheel speed odometer map optimization. Background Art

[0002] In recent years, in an environment where GPS is denied, fusing data from different sensors for vehicle positioning is a popular and challenging task. Many scholars use visual inertial SLAM methods that combine cameras and IMUs (Inertial Measurement Units) to handle this task, including methods based on the Extended Kalman Filter (EKF) and methods based on nonlinear optimization. Compared with pure visual SLAM methods such as ORB-SLAM2 that rely only on cameras, visual inertial SLAM methods can not only determine the absolute scale, pitch, and roll, but also have much stronger robustness in weakly textured areas than pure visual SLAM.

[0003] Theoretically, since the motion of a car often follows a special pattern (for example, usually at an approximately constant speed), visual inertial SLAM methods generally only perform optimization of 4 degrees of freedom. According to three additional observations, especially the scale, is not always accurate. On the other hand, wheel speed odometer provides effective information to determine the scale and is easy to install. Therefore, combining cameras, IMUs, and wheel speed odometers is a wise choice for vehicle positioning in a GPS-denied environment.

[0004] To perfectly fuse and use camera, IMU, and wheel speed odometer data, it is necessary to calibrate the spatial parameters among the three.

[0005] There are many mature methods for the external parameter calibration between a camera and an IMU. Li et al. used the extended Kalman filter method to calibrate and optimize the external parameters between the camera and IMU sensors online (Li M, Mourikis AI. Online temporal calibration for camera–IMU systems: Theory and algorithms. The International Journal of Robotics Research. 2014;33(7):947-964. doi:10.1177 / 0278364913515286). Qin et al. used graph optimization theory to estimate the external parameters between the camera and IMU sensors online and could further optimize them based on the initial calibration parameters (T. Qin and S. Shen, "Online Temporal Calibration for Monocular Visual-Inertial Systems," 2018 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 2018, pp. 3662-3669, doi:10.1109 / IROS.2018.8593603). Currently, most use the Kalibr offline calibration box to obtain the initial calibration parameters, and it has achieved good calibration accuracy.

[0006] However, there are still many problems in the external parameter calibration between the IMU and the wheel odometer.

[0007] Currently, online calibration methods mainly focus on the Kalman filter framework. Wu et al. designed a self-calibration method based on the extended Kalman filter (Y. Wu, "Versatile land navigation using inertial sensors and odometry: Self-calibration, in-motion alignment and positioning," 2014 DGON Inertial Sensors and Systems (ISS), Karlsruhe, Germany, 2014, 1-19), which can estimate misalignment parameters and achieve good positioning performance. By fusing a single-axis rotating IMU with wheel odometry in the Kalman filter framework, on the basis of estimating the extrinsic parameters of the sensors, the scale factor of the wheel odometry was also estimated, improving the positioning performance of sensor fusion. However, their method is affected by nonlinear effects and it is difficult to improve the estimation accuracy. Therefore, Liu et al. developed a fusion method based on the unscented Kalman filter (UKF) and discussed the observability under different trajectories (Liu, Z., El-Sheimy, N., & Qin, Y. Low-cost INS / odometer integration and sensor-to-sensor calibration for land vehicle applications. In Proceedings of the IAG / CPGPS International Conference on GNSS+(ICG+2016)). Zhao et al. proposed an adaptive Kalman filter method to estimate the extrinsic parameters that vary in real time due to noise (Zhao, H., Miao, L., & Shao, H. Adaptive two-stage Kalman filter for SINS / odometer integrated navigation systems. The Journal of Navigation, 2017, 70(2), 242-261). Zhang et al. developed a tightly coupled method for estimating the extrinsic parameters of IMU and wheel odometry and eliminated the adverse effects caused by measurement outliers of the wheel odometry (Zhang, P., Hancock, C. M., Lau, L., Roberts, G. W., & de Ligt, H. Low-cost IMU and odometer tightly coupled integration with Robust Kalman filter for underground 3-D pipeline map**. Measurement, 2019, 137, 454-463).Based on the EKF method, Chen et al. obtained the external parameters between sensors and the scale factor of the wheel speed odometer using a dead reckoning system (Q. Chen, Q. Zhang and X. Niu, "Estimate the Pitch and Heading Mounting Angles of the IMU for Land Vehicular GNSS / INS Integrated System," in IEEE Transactions on Intelligent Transportation Systems, 2021, 22, 6503 - 6515).

[0008] In recent years, graph optimization theory has been widely studied in the field of robotics. Graph optimization theory provides the best estimates of historical and current states and fuses data from different sensors. Compared with the Kalman filter-based method, multiple iterations and linearization can effectively help the graph optimization method approach the optimal value of parameter estimation and improve the robustness of the system. Dang et al. proposed a graph optimization method using wheel odometry (Z. Dang, T. Wang and F. Pang, "Tightly-coupled Data Fusion of VINS and Odometer Based on Wheel Slip Estimation," 2018 IEEE International Conference on Robotics and Biomimetics (ROBIO), Kuala Lumpur, Malaysia, 2018, 1613-1619), but it did not solve the problem of frequent adjustment of the state at each optimization moment and the need for repeated integration of the wheel odometry speed, which increases the computational complexity. To solve this problem, Yuan et al. introduced the pre-integration theory into the wheel odometry system (Yuan. C, Lai, J. Lyu, P. Shi, P. Zhao, W. & Huang, K. A novel fault-tolerant navigation and positioning method with stereo-camera / microelectromechanical system inertial measurement unit (MEMS-IMU) in hostile environment. Micromachines, 2018, 9(12), 626). He et al. derived the covariance matrix of the pre-integration model of wheel odometry and proposed an online extrinsic parameter calibration method for cameras / wheel odometers (Y. He, Y. Guo, A. Ye and K. Yuan, "Camera-odometer calibration and fusion using graph based optimization," IEEE International Conference on Robotics and Biomimetics (ROBIO), Macau, Macao, 2017, 1624-1629). However, this method requires the angular velocity of a two-wheel differential wheel odometer.Liu et al. proposed a pre-integration model for vehicle IMU and wheel speed odometer applied outdoors, which can simultaneously estimate the external parameters between the IMU and the wheel speed odometer (J. Liu, W. Gao and Z. Hu, "Visual-Inertial Odometry Tightly Coupled with Wheel Encoder Adopting Robust Initialization and Online Extrinsic Calibration," IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, 2019, 5391-5397). However, the scale factor of the wheel speed odometer is not included in the IMU-wheel speed odometer model, which will lead to inaccurate estimation of the external parameters.

[0009] In summary, most scholars adopt two calibration methods. One is the online calibration method based on EKF, and the other is the non-linear optimization method based on graph optimization. The former is affected by linearization errors and it is difficult to obtain accurate external parameters between multiple sensors; the latter is difficult to fully calibrate the wheel speed odometer, which affects the positioning accuracy. Summary of the Invention

[0010] The technical problem to be solved by the present invention is how to obtain accurate external parameters between multiple sensors so as to improve the positioning accuracy.

[0011] The present invention solves the above technical problems through the following technical means: A visual-inertial wheel speed odometer graph optimization online calibration method, including the external parameter calibration between the camera and the IMU and the external parameter calibration between the IMU and the wheel speed odometer. The external parameter calibration between the IMU and the wheel speed odometer includes the IMU-wheel speed odometer pre-integration derivation and the graph optimization based on the IMU-wheel speed odometer pre-integration and visual factors. The scale factor of the wheel speed odometer and the influence of the attitude and position deviation between the IMU and the wheel speed odometer are considered in the IMU-wheel speed odometer pre-integration derivation.

[0012] As an optimized technical solution, the external parameter calibration between the camera and the IMU uses a Kalibr offline calibration box to obtain initial calibration parameters, and the graph model optimization method is used in the optimization.

[0013] As an optimized technical solution, the IMU-wheel speed odometer pre-integration is expressed as:

[0014]

[0015] In the formula, and is the computed pre-integration, represents the rotation matrix from the world coordinate system to the b k coordinate system, and represents the rotation matrix from the b k+1 coordinate system and the b k coordinate system to the world coordinate system, and respectively represent the position of the IMU in the world coordinate system, and respectively represent the velocity of the IMU expressed in the world coordinate system, Δt k is the time interval [t k , t k+1 , g w represents the gravity expressed in the world coordinate system, and respectively represent the quaternions from the b k+1 coordinate system and the b k coordinate system to the world coordinate system, represents the bias of the accelerometer at time k+1, represents the bias of the gyroscope at time k+1, represents the translation from the one o k coordinate system to another b k coordinate system, represents the translation from the one o k+1 coordinate system to another b k+1 coordinate system, represents the translation from the one o k+1 coordinate system to another b k coordinate system, represents the translation from the one o k coordinate system to another b k coordinate system, represents the quaternion from the one o k+1 coordinate system to another b k+1 coordinate system.

[0016] As an optimized technical solution, based on the IMU-wheel odometry pre-integration and visual residuals, the error function optimized by the graph model is formulated as:

[0017]

[0018] X represents the set states, including position, velocity, rotation, accelerometer bias, gyroscope bias, wheel odometry scale factor, rotation and translation between camera / IMU, rotation and translation between IMU-wheel odometry, gravity value of the first IMU coordinate system, delay factor between camera / IMU, rp represents the prior information from prior knowledge, H p represents the marginalized prior information matrix. ||r p -H p X|| 2 represents the prior factor represents the IMU - wheel odometer factor represents the visual factor

[0019] The present invention also provides a visual - inertial - wheel - odometer graph optimization online calibration system, including an external parameter calibration module between the camera and the IMU and an external parameter calibration module between the IMU and the wheel odometer. The external parameter calibration module between the IMU and the wheel odometer includes an IMU - wheel - odometer pre - integration derivation unit and a graph optimization unit based on the IMU - wheel - odometer pre - integration and the visual factor. The influence of the scale factor of the wheel odometer and the attitude and position deviation between the IMU and the wheel odometer is considered in the IMU - wheel - odometer pre - integration derivation

[0020] The execution process of each unit in the visual - inertial - wheel - odometer graph optimization online calibration system is the same as the corresponding visual - inertial - wheel - odometer graph optimization online calibration method described above

[0021] The advantages of the present invention are as follows: The present invention proposes a visual - inertial - wheel - odometer graph optimization online calibration method based on the pre - integration theory, and the specific contributions are as follows

[0022] (1) A complete IMU - wheel - odometer pre - integration model is derived, which fully considers the influence of the scale factor of the wheel odometer and the attitude and position deviation between the IMU and the wheel odometer

[0023] (2) Based on the graph model optimization theory, the attitude and position matrices between the camera and the IMU and the attitude and position deviation between the IMU and the wheel odometer are estimated and optimized online, realizing the online estimation and optimization of the external parameters between the three sensors of vision - IMU - wheel odometer

[0024] (3) The proposed algorithm is tested and evaluated on the KITTI dataset. Compared with several advanced online calibration methods, the test results show that the method of the present invention has a higher estimation result of the external parameters between sensors than other methods. At the same time, bringing this estimation result into the system further improves the positioning performance of the system and the navigation performance of the visual - inertial - wheel - odometer fusion Description of the Drawings

[0025] Figure 1 is a relationship diagram between system coordinate systems in an embodiment of the present invention

[0026] Figure 2It is the factor graph structure diagram proposed in the embodiments of the present invention. Detailed implementation manners

[0027] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0028] The present invention proposes an online calibration method for visual inertial wheel speed odometer graph optimization based on the pre-integration theory, mainly including:

[0029] S1. Deduction of IMU-wheel speed odometer pre-integration;

[0030] S2. Graph optimization based on IMU-wheel speed odometer pre-integration and visual factors;

[0031] S3. Calibration of the external parameters of the camera-IMU.

[0032] Based on existing research, the present invention does not study the estimation of the external parameters of the camera-IMU. Similarly, the calibration results of the Kalibr offline toolbox are selected as the initial values and further optimized using the graph model optimization method in the optimization.

[0033] Figure 1 Depicts the relationship between the wheel speed odometer coordinate system (o), the IMU inertial coordinate system (b), and the camera coordinate system (c). In the wheel speed odometer coordinate system, according to the right-hand rule, the x-axis is along the vehicle forward direction, the z-axis is downward, and the y-axis is outward. The IMU inertial coordinate system is roughly aligned with the wheel speed odometer coordinate system on the axis, and the attitude deviation is small, that is, there is an installation error of the inertial sensor in the IMU inertial coordinate system. The installation angle error of the inertial sensor of the IMU inertial coordinate system relative to the wheel speed odometer coordinate system can be described by a set of Euler angles. This description means that the wheel speed odometer coordinate system can be aligned with the IMU inertial coordinate system through three rotations. This study assumes that the installation angle error of the inertial sensor is small, generally less than 5°. After alignment, the IMU inertial coordinate system is considered to be the wheel speed odometer coordinate system within the error range.

[0034] The external parameter matrix between the IMU and the camera is calibrated in advance by the Kalibr offline toolbox and optimized online. At the same time, some typical symbols throughout this article are defined in Table 1.

[0035] Table 1 Definition of typical symbols.

[0036]

[0037]

[0038] The specific calibration method is described as follows.

[0039] S1. A complete IMU - wheel odometer pre - integration model is derived, which fully considers the scale factor of the wheel odometer and the influence of the attitude and position deviations between the IMU and the wheel odometer. The derivation process of the IMU - wheel odometer pre - integration is described as follows.

[0040] In this paper, in order to fuse the visual information provided by the camera, IMU and wheel odometer data, the IMU pre - integration is extended by combining the wheel odometer measurements. Different from the existing visual - inertial - wheel - odometer systems, the present invention takes into account the scale factor of the wheel odometer and the attitude and position deviations between the IMU and the wheel odometer, and derives a complete IMU - wheel odometer pre - integration model. The models of the original gyroscope and accelerometer are given by the following equations:

[0041]

[0042] where and are the angular velocity and motion acceleration readings of the IMU at instant t, and represent the angular velocity and motion acceleration at instant t, and represent the biases of the gyroscope and accelerometer. The rotation matrix from the world coordinate system to the b t coordinate system (the subscript t represents any moment, and b t represents the IMU coordinate system at moment t) is denoted as g w represents the gravity in the world coordinate system, n w represents the random noise of the gyroscope, n a represents the random noise of the accelerometer. It is assumed that the noise in the gyroscope and accelerometer is Gaussian noise, and the gyroscope bias and accelerometer bias are modeled as random walks, and their derivatives are Gaussian.

[0043]

[0044] The wheel odometer model is given by the following equation:

[0045]

[0046] where is the wheel odometer reading at the t - th moment, denotes the true value of the wheel odometer at time t. The original wheel odometer measurement is affected by a scale factor and noise n o . Assuming the noise is Gaussian noise and the scale factor is modeled as a random walk with its derivative being Gaussian,

[0047]

[0048] The position of the wheel odometer in the world coordinate system is given by Equation (5):

[0049]

[0050] denotes the position of the IMU in the world coordinate system, denotes the rotation matrix from the IMU coordinate system to the world coordinate system, and is the position of the wheel odometer in the IMU coordinate system, which is a misalignment in position.

[0051] Given two instants t k and t k+1 , the position, velocity, and orientation states of the world coordinate system are propagated through IMU and wheel odometer measurements and formulated as:

[0052]

[0053] where:

[0054]

[0055] where and respectively denote the position of the IMU in the world coordinate system, and respectively denote the velocity of the IMU expressed in the world coordinate system; and respectively denote the quaternions from the b k+1 coordinate system and the b k coordinate system to the world coordinate system; and denote the rotation matrices from the b k+1 coordinate system and the b k coordinate system to the world coordinate system; Δt k is the duration between the time intervals [t k , t k+1 ; denotes the rotation matrix from one o t coordinate system to another b t coordinate system, which represents the misalignment of the attitude; Represents the quaternion from one b t coordinate system to another b k coordinate system, representing the bias of the accelerometer. Represents the rotation matrix from the b k coordinate system to the world coordinate system; Represents the translation from one o k coordinate system to another b k coordinate system; Represents the translation from one o k+1 coordinate system to another b k+1 coordinate system; Represents the translation from one o t coordinate system to another b t coordinate system; Is the scale factor by which the original wheel speed odometer measurement is affected; Is the wheel speed odometer reading at time t;

[0056] The last equation in Equation (6) is obtained from Equation (5) and the propagation of the wheel speed odometer position represented in the world coordinate system, and is formulated as:

[0057]

[0058] Where, and Are the positions of the wheel speed odometer at instants t k+1 and instant t k represented in the world coordinate system, and n s represents the random walk noise of the wheel speed odometer scale factor.

[0059] It can be seen from Equation (6) that the position propagation of the wheel speed odometer measurement requires the rotation and position at instant t k . In the graph optimization-based algorithm, the state at the optimization moment is often adjusted, which leads to the need to re-propagate the wheel speed odometer measurement. To solve this problem, the pre-integration theory is introduced into the measurement of the wheel speed odometer.

[0060] At instant t k , the reference frame changes from the world coordinate system to the wheel speed odometer coordinate system, and Equation (6) is rewritten as:

[0061]

[0062] Where:

[0063]

[0064] In the formula, and represent the IMU in bk Instantaneous t represented in the coordinate system k+1 Pre-integration of the measured position, velocity, and attitude; Represents at b k Pre-integration of the position measured by the instantaneous wheel odometer represented in the coordinate system, Represents from one b t Coordinate system to another b k Rotation matrix of the coordinate system; Represents from one o t Coordinate system to another b t Rotation matrix of the coordinate system. In formula (10), the reference frame is the b k Coordinate system. Even if the instantaneous state is adjusted during the optimization process, it does not affect the integration in formula (10).

[0065] The discrete form of formula (10) is expressed as:

[0066]

[0067] And Is the pre-integration of the IMU measurement at instantaneous t k+1 , and the coordinate system is b k ; Represents at b k Pre-integration of the instantaneous t k+1 Measured value of the wheel odometer represented in the coordinate system; And Is the pre-integration of the IMU measurement value at instantaneous t k Represented in the coordinate system k+1 Pre-integration; Represents at b k Pre-integration of the instantaneous t t Measured by the wheel odometer; t t+1 ∈[t k , t k+1 , and δt is the sampling interval of the IMU measurement value; Represents the estimated rotation matrix from one coordinate system o t To another coordinate system b t ; Represents the estimated rotation matrix from one o t Coordinate system to another b t Coordinate system; Respectively represent the estimated accelerometer bias, gyroscope bias, and scale factor of the wheel odometer.

[0068] Then, the continuous-time linearized dynamics of the error term of formula (10) is written as:

[0069]

[0070] Among them F t and G t represents the state transfer matrix and noise matrix; represents the pre-integrated error vector including the pre-integrated error and the sensor parameter error; Respectively represent from b t Coordinate system to b k Coordinate system position error, velocity error, and rotation error; They represent the speedometer error, gyroscope error, and wheel speed odometer scale factor error respectively; Respectively represent the coordinate system o t To another coordinate system b t The translation error and rotation error of The rotation error is obtained by the following formula:

[0071]

[0072] In formula (12), the position and attitude error terms are modeled as random walks, whose derivatives are Gaussian, According to formula (12), the discrete form of the error term is formulated as:

[0073]

[0074] represents the pre-integrated error vector of the pre-integrated error and the sensor parameter error; n represents the random walk noise. and Denote the discrete state transfer matrix and noise matrix respectively. Assume that t k and t k+1 are two instants, t i ,t i+1 ,...∈[t k ,t k+1 ]. Then, the transfer equation starting from instant t k To instant t k+1 can be written as:

[0075]

[0076] represents the instantaneous t k+1 The Jacobian matrix of represents the discrete state transfer matrix from instant i to instant k+1, where ∈ is the pre-integrated error vector of the pre-integrated error and the sensor parameter error. For simplicity, ξ is represented by other terms. Therefore, the recursive equation of the state transfer matrix is and the covariance matrix It can be expressed as:

[0077]

[0078] Wherein, Q represents the covariance matrix of the noise, which is given by the following formula:

[0079]

[0080] According to formula (15), it is possible to obtain:

[0081]

[0082] In the graph model optimization method, the reference coordinate system is the IMU coordinate system at instant t k , that is, the coordinate system b k . The error term of the pre-integration at instant t k is This means the error relative to itself. Therefore, the initial value of formula (16) can be set as:

[0083]

[0084] Therefore, the Jacobian matrix k+1 and the covariance matrix at instant t can be calculated by combining formula (16) and (19). The correction equation for the pre-integration measurement can be expressed in the following form:

[0085]

[0086] In the formula, and are the calculated pre-integrations, and are the corrected pre-integrations. and are respectively and the Jacobian matrices corresponding to ; and represent and the Jacobian matrices corresponding to ; represents the Jacobian matrix corresponding to ; and are and the Jacobian matrices corresponding to . The above matrices are sub-block matrices of and can be obtained from Obtained

[0087] The pre-integration form of the IMU-wheel odometer can be expressed as:

[0088]

[0089] Denote the rotation matrix from the world coordinate system to the b k coordinate system. Assume that the accelerometer bias, gyroscope bias, scale factor, and misalignment of position and attitude between the IMU and the wheel odometer are constant in a short period of time. Therefore, the difference between consecutive instants of these terms is zero.

[0090] S2. Graph optimization based on IMU-wheel odometer pre-integration and visual factors

[0091] The sensor fusion problem can be expressed as a graph model to perform non-linear optimization. There are two types of nodes, factor nodes and variable nodes. When a factor involves the corresponding variables, an edge connects the factor node and the variable node. A factor describes the error between the predicted measurement value and the actual measurement value. The proposed graph structure is as Figure 2 shown. The state vector of the proposed method is designed as:

[0092]

[0093] X represents the set state, including position, velocity, rotation, accelerometer bias, gyroscope bias, wheel odometer scale factor, rotation and translation between the camera / IMU, rotation and translation between the IMU-wheel odometer, the value of gravity in the first IMU frame, the delay factor between the camera / IMU, x k represents the state at instant t k , λ represents the number of states in the sliding window, and f0, f1 represent two image frames.

[0094] According to Equation (21), the IMU-wheel odometer pre-integration residual is given by:

[0095]

[0096] The visual reprojection residual formula is:

[0097]

[0098] respectively represent the pixel coordinates of feature l at times i and j. Π represents the camera projection model. λi represents the depth in image i. Denote the rotation matrix from the b coordinate system to the c coordinate system; Denote the rotation matrix from the w coordinate system to the b coordinate system at time j; Denote the rotation matrix from the b coordinate system to the w coordinate system at time i; Denote the translation from the c coordinate system to the b coordinate system; Denote the translation from the b coordinate system to the w coordinate system at time i; Denote the translation from the b coordinate system to the w coordinate system at time j.

[0099] Based on the IMU - wheel odometry pre - integration and visual residuals, the error function optimized by the graph model can be formulated as:

[0100]

[0101] X Represent the set states, including position, velocity, rotation, accelerometer bias, gyroscope bias, wheel odometry scale factor, rotation and translation between the camera / IMU, rotation and translation between the IMU - wheel odometry, gravity value of the first IMU coordinate system, delay factor between the camera / IMU, r p Represent the prior information from prior knowledge, H p Represent the marginalized prior information matrix. ||r p -H p X|| 2 Represent the prior factor, Represent the IMU - wheel odometry factor, Represent the visual factor.

[0102] By solving formula (25), the state variables of x in formula (22) can be solved, and then the rotation matrix and translation vector between the camera - IMU and the rotation matrix and translation vector between the IMU - wheel odometry can be obtained, completing the entire calibration process.

[0103] 4. Experimental Results

[0104] The execution dataset has evaluated the performance of the proposed method compared with other advanced methods. The advanced methods include the method based on Kalman filter calibration proposed by Chang et al. (Chang L, Niu X, Liu T. GNSS / IMU / ODO / LiDAR-SLAM Integrated Navigation System Using IMU / ODO Pre-Integration. Sensors. 2020;20(17):4702), the method based on aided dead reckoning proposed by Chen et al. (Q. Chen, Q. Zhang and X. Niu, "Estimate the Pitch and Heading Mounting Angles of the IMU for Land Vehicular GNSS / INS Integrated System," in IEEE Transactions on Intelligent Transportation Systems, 2021, 22, 6503-6515), and the robust initialization online calibration method proposed by Liu et al. (J. Liu, W. Gao and Z. Hu, "Visual-Inertial Odometry Tightly Coupled with Wheel Encoder Adopting Robust Initialization and Online Extrinsic Calibration," IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, 2019, 5391-5397). All the methods were tested on a PC computer equipped with an NVIDIA GeForce RTX 2060 GPU and an AMD Ryzen 7 4800H CPU. According to the observability analysis by Liu et al., in the case of vehicle maneuvers including turning, accelerating, and decelerating, the misalignment of the y-axis and z-axis in the IMU-wheel odometer attitude is observable, and the misalignment of the x-axis and y-axis in the IMU-wheel odometer position is observable.

[0105] The original data of the KITTI dataset was used to evaluate the effectiveness of the proposed self-calibration method (A. Geiger, P. Lenz and R. Urtasun, "Are we ready for autonomous driving? The KITTI vision benchmark suite," IEEE Conference on Computer Vision and Pattern Recognition, Providence, RI, USA, 2012, 3354-3361). The KITTI dataset provides images, IMU information, and the position and velocity measured by GPS. The misalignment of the IMU-wheel odometer attitude and position, as well as the scale factor of the wheel odometer position, were manually added to the original data to simulate the misaligned camera / IMU-wheel odometer measurements.

[0106] The specific settings of the simulation parameters in the simulation are shown in Table 2. Multiple repeated experiments were carried out in the simulation, and the average root mean square error of multiple simulations is listed in Table 3.

[0107] Table 2 Settings of the simulation parameters in the KITTI dataset

[0108] Simulation parameters Value X-axis position deviation (m) 1.00 Y-axis position deviation (m) 0.95 Y-axis attitude deviation (deg) -0.80 Z-axis attitude deviation (deg) -1.00 Wheel speed odometer scale factor 1.19

[0109] Table 3 Statistical results in the simulation

[0110]

[0111] It can be noted from Table 3 that the estimation results of our self-calibration method are closer to the true values. The EKF-based method proposed by Chang et al. is affected by linearization errors and is inaccurate in estimating the spatial parameters between multiple sensors. The graph optimization-based method proposed by Liu et al. has a low accuracy in estimating spatial parameters because it does not estimate the scale factor of the wheel odometer. This is because the scale factor is the main factor affecting the positioning accuracy of the wheel odometer. The dead reckoning-based calibration method proposed by Chen et al. estimates the spatial offset between multiple sensors and the scale factor of the wheel odometer. However, its calibration accuracy is slightly lower than our method.

[0112] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An online calibration method for optimizing a visual inertial wheel speed odometer diagram, including the external parameter calibration between a camera and an IMU and the external parameter calibration between the IMU and the wheel speed odometer, characterized in that, The external parameter calibration between the IMU and the wheel speed odometer includes the derivation of IMU-wheel speed odometer pre-integration and the graph optimization based on the IMU-wheel speed odometer pre-integration and visual factors. The influence of the scale factor of the wheel speed odometer and the attitude and position deviations between the IMU and the wheel speed odometer is considered in the derivation of IMU-wheel speed odometer pre-integration.

2. The online calibration method for optimizing the vision inertial wheel speed odometer diagram according to claim 1, wherein The external parameter calibration between the camera and the IMU uses the Kalibr offline calibration box to obtain the initial calibration parameters, and the graph model optimization method is adopted in the optimization.

3. The online calibration method for visual inertial wheel speed odometer map optimization according to claim 1, characterized in that, The IMU-wheel speed odometer pre-integration is expressed as: wherein, and are the calculated pre-integrals, represents the rotation matrix from the world coordinate system to the b k coordinate system, and represent the rotation matrices from the b k+1 coordinate system and the b k coordinate system to the world coordinate system, and respectively represent the positions of the IMU in the world coordinate system, and respectively represent the velocities of the IMU expressed in the world coordinate system, Δt k is the time interval between [t k , t k+1 , g w represents the gravity expressed in the world coordinate system, and respectively represent the quaternions from the b k+1 coordinate system and the b k coordinate system to the world coordinate system, represents the bias of the accelerometer at the (k + 1)th moment, represents the bias of the gyroscope at the (k + 1)th moment, represents the translation from one o k coordinate system to another b k coordinate system, represents the translation from one o k+1 coordinate system to another b k+1 coordinate system, represents the translation from one o k+1 coordinate system to another b k coordinate system, represents the quaternion from one o k coordinate system to another b k coordinate system, represents the quaternion from one o k+1 coordinate system to another b k+1 coordinate system.

4. The online calibration method for optimizing the visual inertial wheel speed odometer diagram according to claim 3, characterized in that, The derivation process of IMU-wheel speed odometer pre-integration includes: The models of the original gyroscope and accelerometer are given by the following formula: Wherein, and are the angular velocity and motion acceleration readings of the IMU at instant t, and represent the angular velocity and motion acceleration at instant t, and represent the biases of the gyroscope and accelerometer. The rotation matrix from the world coordinate system to the b t coordinate system is expressed as g w represents the gravity expressed in the world coordinate system, n w represents the random noise of the gyroscope, n a represents the random noise of the accelerometer. It is assumed that the noise in the gyroscope and accelerometer is Gaussian noise, and the gyroscope bias and accelerometer bias are modeled as random walks, and their derivatives are Gaussian, The wheel speed odometer model is given by the following formula: where is the wheel speed odometer reading at the $t$-th moment, represents the true value of the wheel speed odometer at the $t$-th moment. The original wheel speed odometer measurement is affected by the scale factor and the noise $n$ o . Assume that the noise is Gaussian noise and the scale factor is modeled as a random walk with its derivative being Gaussian, Position of the wheel speed odometer in the world coordinate system Given by Equation (5): Represents the position of the IMU in the world coordinate system, Represents the rotation matrix from the IMU coordinate system to the world coordinate system, is the wheel odometer position in the IMU coordinate system, which is a misalignment in position.

5. The online calibration method for visual inertial wheel speed odometer map optimization according to claim 4, wherein Given two instants \(t\) k and \(t\) k+1 , the position, velocity, and orientation states in the world coordinate system are propagated through measurements from the IMU and wheel odometer, and Equation (5) is transformed into: Where: Indicates from one o t Coordinate system to another b t Rotation matrix of the coordinate system, which represents the misalignment of the attitude; Indicates from one b t Coordinate system to another b k Quaternion of the coordinate system, Indicates the bias of the accelerometer; Indicates from b k Rotation matrix of the coordinate system to the world coordinate system; Indicates from one o t Coordinate system to another b t Rotation matrix of the coordinate system; Is the proportionality factor for the original wheel speed odometer measurement; Is the wheel speed odometer reading at the t-th moment; The last equation in formula (6) is obtained from formula (5) and the propagation of the wheel speed odometer position represented in the world coordinate system, and it is formulated as: Wherein, and are the positions represented by the wheel speed odometer at instants t k+1 and instant t k in the world coordinate system, respectively, and n s represents the random walk noise of the wheel speed odometer scale factor.

6. The online calibration method for visual inertial wheel speed odometer diagram optimization according to claim 5, characterized in that, At instant t k The reference frame changes from the world coordinate system to the wheel speed odometer coordinate system, and formula (6) is rewritten as: Where: wherein, and represent the pre-integration of the position, velocity, and attitude measured by the IMU at the instantaneous time t in the b k coordinate system; k+1 represents the pre-integration of the position measured by the instantaneous wheel speed odometer in the b k coordinate system; represents the rotation matrix from one b t coordinate system to another b k coordinate system; represents the rotation matrix from one o t coordinate system to another b t coordinate system;​ The discrete form of formula (10) is expressed as: and is the pre-integration of IMU measurements at instant t k+1 in coordinate system b k ; represents the pre-integration of wheel odometer measurements at instant t k expressed in coordinate system b k+1 ; and is the pre-integration of IMU measurements at instant t k expressed in coordinate system b k+1 ; represents the pre-integration of wheel odometer measurements at instant t k expressed in coordinate system b t ; t t+1 ∈[t k , t k+1 , where δt is the sampling interval of IMU measurements; represents the estimated rotation matrix from one coordinate system o t to another coordinate system b t ; represents the estimated rotation matrix from one o t coordinate system to another b t coordinate system; respectively represent the estimated accelerometer bias, gyroscope bias, and scale factor of the wheel odometer.

7. The online calibration method for optimizing the vision inertial wheel speed odometer diagram according to claim 6, characterized in that, The continuous-time linearized dynamics of the error term in formula (10) are written as: Where F t and G t represent the state transition matrix and the noise matrix; represents the pre-integration error vector of the pre-integration error and the sensor parameter error therein; respectively represent the position error, velocity error, and rotation error from the b t coordinate system to the b k coordinate system; respectively represent the accelerometer error, gyroscope error, and wheel speed odometer scale factor error; respectively represent the translation error and rotation error from one coordinate system o t to another coordinate system b t ; and the rotation error, is obtained through the following formula: In Equation (12), the error terms of position and attitude are modeled as random walks, and their derivatives are Gaussian. According to Equation (12), the discrete form of the error terms is formulated as: denotes the pre-integration error vector of the pre-integration error and the sensor parameter error; n denotes the random walk noise, and denote the discrete state transition matrix and the noise matrix respectively. Assume that t k and t k+1 are two instants, and t i , t i+1 ,... ∈ [t k , t k+1 . Then, the transfer equation starting from the instant is written from the instant t k to the instant t k+1 as: Denote the instantaneous t k+1 Jacobian matrix of, Denote the discrete state transition matrix from instantaneous i to instantaneous k + 1, Denote the pre-integration error vector of the pre-integration error and sensor parameter error therein, ξ is denoted as other terms, the recursive equation of the state transition matrix and covariance matrix Are expressed as: Among them, Q represents the covariance matrix of the noise and is given by the following formula: According to formula (15), it is possible to obtain: In the method for optimizing the map model, the reference coordinate system is the IMU coordinate system at the instant t k which is the coordinate system b k , and the error term of the pre-integration at the instant t k is This means the error relative to itself. Therefore, the initial value of formula (16) is set to: Therefore, at instant t k+1 the Jacobian matrix and covariance matrix can be calculated by combining equations (16) and (19), and the correction equation for the pre-integrated measurements is expressed as: In the formula, and are the calculated pre-integrals, and are the corrected pre-integrals. and are respectively and the Jacobian matrices corresponding to ; and represent the Jacobian matrix corresponding to the corresponding to ; represents the Jacobian matrix corresponding to ; and are and the Jacobian matrices corresponding to . The above matrices are sub-block matrices of and are obtained from .

8. The online calibration method for optimizing the visual inertial wheel speed odometer diagram according to any one of claims 1-7, characterized in that, Based on the IMU-wheel speed odometer pre-integration and visual residuals, the error function of the graph model optimization is formulated as: X Represents the set state, including position, velocity, rotation, accelerometer bias, gyroscope bias, wheel speed odometer scale factor, rotation and translation between camera / IMU, rotation and translation between IMU - wheel speed odometer, gravity value in the first IMU coordinate system, delay factor between camera / IMU, r p Represents the prior information from prior knowledge, H p Represents the marginalized prior information matrix. ||r p -H p X|| 2 Represents the prior factor, Represents the IMU - wheel speed odometer factor, Represents the visual factor.

9. The online calibration method for optimizing the visual inertial wheel speed odometer diagram according to any one of claims 1-7, characterized in that Based on the IMU-wheel speed odometer pre-integration and visual residuals, the state vector of the graph model optimization method is designed as: x k represents the state at instant t k , λ represents the number of states in the sliding window, and f0, f1 represent two image frames; According to formula (21), the IMU-wheel speed odometer pre-integration residual is given by the following formula: The formula for the visual reprojection residual is: respectively represent the pixel coordinates of feature l at times i and j, Π represents the camera projection model, and λi represents the depth in image i. represents the rotation matrix from the b coordinate system to the c coordinate system; represents the rotation matrix from the w coordinate system to the b coordinate system at time j; represents the rotation matrix from the b coordinate system to the w coordinate system at time i; represents the translation from the c coordinate system to the b coordinate system; represents the translation from the b coordinate system to the w coordinate system at time i; represents the translation from the b coordinate system to the w coordinate system at time j; By solving formula (25), the state variables of x in formula (22) are solved, and then the rotation matrix and translation vector between the camera and the IMU, and the rotation matrix and translation vector between the IMU and the wheel speed odometer are obtained, completing the entire calibration process.

10. An online calibration system for optimizing a visual inertial wheel speed odometer diagram, comprising an external parameter calibration module between a camera and an IMU, and an external parameter calibration module between the IMU and the wheel speed odometer, characterized in that, The external parameter calibration module between the IMU and the wheel speed odometer includes an IMU-wheel speed odometer pre-integration derivation unit and a graph optimization unit based on the IMU-wheel speed odometer pre-integration and visual factors. The influence of the scale factor of the wheel speed odometer and the attitude and position deviations between the IMU and the wheel speed odometer is considered in the derivation of IMU-wheel speed odometer pre-integration.

Images