Mileage calculation method and device based on iterative extended Kalman filter

The visual inertial odometer is dynamically initialized and iteratively updated by the iterative expansion of the Kalman filter, which solves the problems of low accuracy and inapplicable static initialization in the prior art, and achieves higher accuracy and ease of use.

CN115342829BActive Publication Date: 2024-12-06NANJING ARTIFICIAL INTELLIGENCE CHIPS RES INST OF AUTOMATION CHINESE ACAD OF SCI
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210863221.6
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-07-20
Publication Date
2024-12-06
Estimated Expiration
2042-07-20

AI Technical Summary

Technical Problem

The existing filter-based visual inertial odometer method has low accuracy, large linear errors, and static initialization is not suitable for use during movement.

Method used

The method based on the iterative extension Kalman filter is used to dynamically initialize the visual inertial odometer, establish an initial space three-dimensional map, and reduce nonlinear errors through multiple iterations.

Benefits of technology

提高了视觉惯性里程计的精度和易用性,适用于运动过程中的初始化,并显著降低了非线性误差。

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115342829B_ABST
    Figure CN115342829B_ABST
Patent Text Reader

Abstract

The present application provides a mileage calculation method and device based on an iterative extended Kalman filter, which is applied to a visual inertial odometer, wherein the visual inertial odometer includes a camera and an IMU. The method includes: dynamically initializing the visual inertial odometer to obtain initialization state information of the visual inertial odometer, and establishing an initial spatial three-dimensional map based on the initialization state information; performing state propagation according to the initialization state information, image data collected in real time by the camera, and IMU data collected in real time by the IMU to obtain a linearized model of the visual inertial odometer; performing multiple iterative update processing on the linearized model based on the iterative extended Kalman filter method to obtain the current system state vector of the visual inertial odometer, and the current system state vector is used to indicate the position and posture of the visual inertial odometer in the initial spatial three-dimensional map. The accuracy of the visual inertial odometer is improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present application relates to the field of computer vision technology, and in particular to a mileage calculation method and device based on an iterative extended Kalman filter. Background Art

[0002] In recent years, due to the rapid development of low-cost micro-electro-mechanical system (MEMS) inertial sensor technology and the extremely important application of low-cost and lightweight spatial positioning technology in augmented reality (AR) / virtual reality (VR), robotics, drones and other fields, more and more people are studying the fusion of camera data and inertial measurement unit (IMU) data to estimate the 6-DOF spatial pose of the device.

[0003] Filtering is a common approach in visual-inertial odometry. It offers the advantages of high speed and robustness, making it suitable for embedded platforms with limited computing resources. However, current filter-based methods suffer from low accuracy and large linear errors. Summary of the Invention

[0004] This application provides a mileage calculation method and device based on an iterative extended Kalman filter to improve the accuracy of current visual inertial odometers.

[0005] In a first aspect, the present application provides a mileage calculation method based on an iterative extended Kalman filter, which is applied to a visual inertial odometer, wherein the visual inertial odometer includes a camera and an IMU, and the method includes:

[0006] Dynamically initializing the visual inertial odometry to obtain initialization state information of the visual inertial odometry, and establishing an initial spatial three-dimensional map based on the initialization state information, wherein the initialization state information includes camera poses corresponding to a plurality of initial images captured by the camera and spatial positions of three-dimensional feature points on each of the initial images;

[0007] Performing state propagation according to the initialization state information, the image data collected in real time by the camera, and the IMU data collected in real time by the IMU to obtain a linearized model of the visual inertial odometry;

[0008] The linearized model is iteratively updated multiple times based on an iterative extended Kalman filter method to obtain a current system state vector of the visual inertial odometry, where the current system state vector is used to indicate the position and posture of the visual inertial odometry in the initial spatial three-dimensional map.

[0009] In a possible implementation, the dynamically initializing the visual inertial odometry to obtain initialization state information of the visual inertial odometry, and establishing an initial spatial three-dimensional map based on the initialization state information, includes:

[0010] constructing sliding windows in the plurality of initial images according to a disparity threshold, and determining matching feature points between the sliding windows as the three-dimensional feature points;

[0011] Obtaining, according to the initial IMU data corresponding to each of the initial images in the sliding window, a gyroscope bias, an IMU speed, a gravity direction, and a scale factor corresponding to each of the initial images;

[0012] Acquiring the initialization state information according to the gyroscope bias, the speed of the IMU, and the direction of gravity;

[0013] An initial spatial three-dimensional map is established according to the initialization state information and the scale factor.

[0014] In a possible implementation, the acquiring the initialization state information according to the gyroscope bias, the speed of the IMU, and the gravity direction includes:

[0015] Correcting the gravity direction to obtain a corrected gravity direction;

[0016] Obtaining a rotation angle between a global coordinate system and a camera coordinate system according to a rotation angle between the corrected gravity direction and the direction vector;

[0017] Converting the gyroscope bias and the IMU velocity corresponding to each of the initial images to the global coordinate system according to a rotation angle between the global coordinate system and the camera coordinate system;

[0018] The initialization state information is obtained according to the gyroscope bias and the speed of the IMU in the global coordinate system.

[0019] In one possible implementation, performing state propagation based on the initialization state information, the image data collected in real time by the camera, and the IMU data collected in real time by the IMU to obtain a linearized model of the visual inertial odometry includes:

[0020] Acquire an IMU measurement model according to the IMU data;

[0021] State vector augmentation processing is performed according to the IMU measurement model and the image data to obtain the linearized model.

[0022] In a possible implementation, performing multiple iterative update processes on the linearized model based on the iterative extended Kalman filter method to obtain a current system state vector of the visual inertial odometry includes:

[0023] Set the initial system state vector estimate;

[0024] Executing an i-th iteration operation, the i-th iteration operation comprising: calculating the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration under the system state at the i-1-th iteration; calculating the system state of the visual inertial odometry at the i+1-th iteration according to the linearized model, the Jacobian matrix of the i-th iteration, and the Kalman gain of the i-th iteration; i is initially 1, and i is sequentially 1, 2, ..., N-1, where N is the number of iterations, N is a positive integer, and the system state at iteration 0 is the estimated value of the initial system state vector;

[0025] The system state of the visual inertial odometry at the Nth iteration is determined as a current system state vector of the visual inertial odometry.

[0026] In one possible implementation, the method further includes:

[0027] Obtain a covariance matrix of the current system state vector according to the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration.

[0028] In a second aspect, the present application provides an odometer calculation device based on an iterative extended Kalman filter, which is applied to a visual inertial odometer, wherein the visual inertial odometer includes a camera and an IMU, and the device includes:

[0029] An initialization module is used to dynamically initialize the visual inertial odometry to obtain initialization state information of the visual inertial odometry, and to establish an initial spatial three-dimensional map based on the initialization state information, wherein the initialization state information includes the camera poses corresponding to the multiple initial images taken by the camera and the spatial positions of the three-dimensional feature points on each of the initial images;

[0030] A first processing module is configured to perform state propagation based on the initialization state information, the image data collected in real time by the camera, and the IMU data collected in real time by the IMU, so as to obtain a linearized model of the visual inertial odometry;

[0031] The second processing module is used to perform multiple iterative update processes on the linearized model based on the iterative extended Kalman filter method to obtain a current system state vector of the visual inertial odometry, and the current system state vector is used to indicate the position and posture of the visual inertial odometry in the initial spatial three-dimensional map.

[0032] In a possible implementation, the initialization module is specifically configured to:

[0033] constructing sliding windows in the plurality of initial images according to a disparity threshold, and determining matching feature points between the sliding windows as the three-dimensional feature points;

[0034] Obtaining, according to the initial IMU data corresponding to each of the initial images in the sliding window, a gyroscope bias, an IMU speed, a gravity direction, and a scale factor corresponding to each of the initial images;

[0035] Acquiring the initialization state information according to the gyroscope bias, the speed of the IMU, and the direction of gravity;

[0036] An initial spatial three-dimensional map is established according to the initialization state information and the scale factor.

[0037] In a possible implementation, the initialization module is specifically configured to:

[0038] Correcting the gravity direction to obtain a corrected gravity direction;

[0039] Obtaining a rotation angle between a global coordinate system and a camera coordinate system according to a rotation angle between the corrected gravity direction and the direction vector;

[0040] Converting the gyroscope bias and the IMU velocity corresponding to each of the initial images to the global coordinate system according to a rotation angle between the global coordinate system and the camera coordinate system;

[0041] The initialization state information is obtained according to the gyroscope bias in the global coordinate system and the speed of the IMU.

[0042] In a possible implementation manner, the first processing module is specifically configured to:

[0043] Acquire an IMU measurement model according to the IMU data;

[0044] State vector augmentation processing is performed according to the IMU measurement model and the image data to obtain the linearized model.

[0045] In a possible implementation manner, the second processing module is specifically configured to:

[0046] Set the initial system state vector estimate;

[0047] Executing an i-th iteration operation, the i-th iteration operation comprising: calculating the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration under the system state at the i-1-th iteration; calculating the system state of the visual inertial odometry at the i+1-th iteration according to the linearized model, the Jacobian matrix of the i-th iteration, and the Kalman gain of the i-th iteration; i is initially 1, and i is sequentially 1, 2, ..., N-1, where N is the number of iterations, N is a positive integer, and the system state at iteration 0 is the estimated value of the initial system state vector;

[0048] The system state of the visual inertial odometry at the Nth iteration is determined as a current system state vector of the visual inertial odometry.

[0049] In a possible implementation manner, the second processing module is further configured to:

[0050] Obtain a covariance matrix of the current system state vector according to the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration.

[0051] In a third aspect, the present application provides an electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, the mileage calculation method based on the iterative extended Kalman filter as described in any one of the first aspects is implemented.

[0052] In a fourth aspect, the present application provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the mileage calculation method based on the iterative extended Kalman filter as described in any one of the first aspects.

[0053] In a fifth aspect, the present application provides a computer program product, comprising a computer program, which, when executed by a processor, implements the mileage calculation method based on the iterative extended Kalman filter as described in any one of the first aspects.

[0054] The mileage calculation method and device based on the iterative extended Kalman filter provided in the present application are applied to a visual inertial odometry, which includes a camera and an IMU. First, the visual inertial odometry is dynamically initialized to obtain the initialization state information of the visual inertial odometry, and an initial spatial three-dimensional map is established based on the initialization state information. The initialization state information includes the camera pose corresponding to the initial images taken by multiple cameras and the spatial position of the three-dimensional feature points on each initial image; then, state propagation is performed based on the initialization state information, the image data collected in real time by the camera, and the IMU data collected in real time by the IMU to obtain a linearized model of the visual inertial odometry; finally, the linearized model is iteratively updated multiple times based on the iterative extended Kalman filter method to obtain the current system state vector of the visual inertial odometry, and the current system state vector is used to indicate the position and posture of the visual inertial odometry in the initial spatial three-dimensional map. In the filter-based VIO system, most systems use static initialization. The solution of this application uses dynamic initialization, so that the solution can be used to initialize the filter-based VIO system during movement, which is more user-friendly. At the same time, in order to address the problem that the current extended Kalman filter system only performs one iteration in the update step, resulting in a large linearization error, this application adopts an iterative extended Kalman filter method, that is, multiple iterations are performed in the update step, thereby reducing the nonlinear error of the VIO system. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] In order to more clearly illustrate the technical solutions in the present invention or the prior art, a brief introduction is given below to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0056] Figure 1 A flow chart of a mileage calculation method based on an iterative extended Kalman filter provided in an embodiment of the present application;

[0057] Figure 2 A schematic diagram of visual inertial odometry processing based on an iterative extended Kalman filter provided in an embodiment of the present application;

[0058] Figure 3 A schematic diagram of the dynamic initialization process provided in an embodiment of the present application;

[0059] Figure 4 A schematic diagram of dynamic initialization provided in an embodiment of the present application;

[0060] Figure 5 A schematic diagram of the process flow of iterative extended Kalman filter processing provided in an embodiment of the present application;

[0061] Figure 6 A schematic diagram of image data collected by a camera according to an embodiment of the present application;

[0062] Figure 7 A schematic diagram of a simple scene sequence trajectory on the third floor provided in an embodiment of the present application;

[0063] Figure 8 A schematic diagram of a difficult stairwell scenario sequence trajectory provided in an embodiment of the present application;

[0064] Figure 9 A schematic diagram of the structure of a mileage calculation device based on an iterative extended Kalman filter provided in an embodiment of the present application;

[0065] Figure 10 A schematic diagram of the physical structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0066] To make the objectives, technical solutions, and advantages of this application more clear, the technical solutions of this application will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the embodiments described are only part of the embodiments of this application, not all of them. Based on the embodiments of this application, all other embodiments obtained by ordinary technicians in this field without making any creative efforts are within the scope of protection of this application.

[0067] In recent years, due to the rapid development of low-cost MEMS inertial sensor technology and the extremely important application of low-cost and lightweight spatial positioning technology in AR / VR, robots, drones and other fields, more and more people are studying the fusion of camera data and IMU data to estimate the 6-DOF spatial pose of the device. Many excellent visual inertial odometry methods have been proposed, mainly in two categories: optimization-based methods and filtering-based methods. In actual systems, filtering-based methods can achieve accuracy comparable to optimization-based methods. Filtering-based methods also have the advantages of high speed and high robustness, and are more suitable for running on embedded platforms with limited computing resources. However, filtering-based visual-inertial odometry (VIO) systems often use static initialization, which is not user-friendly. In addition, the accuracy of filtering-based VIO systems can be further improved. Therefore, this application focuses on using the iterative extended Kalman filter method to improve the accuracy of filtering-based VIO systems, and uses motion initialization.

[0068] Figure 1 A flow chart of a mileage calculation method based on an iterative extended Kalman filter provided in an embodiment of the present application is applied to a visual inertial odometer, which includes a camera and an IMU, such as Figure 1As shown, the method may include:

[0069] S11, dynamically initialize the visual inertial odometry to obtain initialization state information of the visual inertial odometry, and establish an initial spatial three-dimensional map based on the initialization state information. The initialization state information includes the camera poses corresponding to the initial images taken by multiple cameras and the spatial positions of the three-dimensional feature points on each initial image.

[0070] In the filter-based VIO system, most systems use static initialization. In order to facilitate the user to initialize the filter-based VIO system during movement, the embodiment of the present application uses motion initialization, that is, dynamic initialization. Dynamic initialization requires a certain amount of motion excitation and does not need to assume that the system is in a static state to complete the initialization process. In the dynamic initialization stage, the following four operations are completed respectively: align the IMU local coordinate system with the direction of gravity, restore the scale information of the monocular vision, calculate the initial position and movement speed, and calculate the acceleration bias and gyroscope bias.

[0071] After dynamically initializing the visual inertial odometry, its initialization state information can be obtained. This initialization state information represents the initial state of the visual inertial odometry before mileage calculations are performed. It includes the camera poses corresponding to the initial images captured by multiple cameras, as well as the spatial positions of three-dimensional feature points on each initial image. Three-dimensional feature points are points on each initial image with certain characteristics that facilitate tracking, such as image corners, building vertices, and so on. After obtaining this initialization state information, an initial spatial three-dimensional map can be established based on this initialization state information. Each captured object then has a corresponding position and coordinates in the initial spatial three-dimensional map.

[0072] S12, performing state propagation based on the initialization state information, the image data collected in real time by the camera, and the IMU data collected in real time by the IMU, to obtain a linearized model of the visual inertial odometry.

[0073] For state propagation, it mainly includes IMU prediction system state and state vector augmentation. IMU prediction system state refers to the use of IMU measurement value prediction and its covariance matrix, and state vector augmentation refers to adding the image pose to the state vector and updating its covariance matrix. In an embodiment of the present application, after obtaining the initialization state information, state propagation is performed based on the initialization state information, the image data collected in real time by the camera, and the IMU data collected in real time by the IMU, and a linearized model of the visual inertial odometer can be obtained. The linearized model includes the IMU measurement value and its covariance matrix, as well as the pose of the image data and its covariance matrix.

[0074] S13, performing multiple iterative updates on the linearized model based on the iterative extended Kalman filter method to obtain a current system state vector of the visual inertial odometry, where the current system state vector is used to indicate the position and posture of the visual inertial odometry in the initial spatial three-dimensional map.

[0075] The traditional extended Kalman filter system is mainly divided into two steps: prediction and update, and only one iteration is performed in the update step. However, there is still a large linearization error after only one iteration. To this end, the embodiment of the present application uses an iterative extended Kalman filter to perform multiple iterative updates on the linearized model to obtain the current system state vector of the visual inertial odometer. Since the current system state vector indicates the position and posture of the visual inertial odometer in the initial spatial three-dimensional map, the current posture of the visual inertial odometer can be determined based on the current system state vector. At any time, the above scheme can be used to obtain the current posture of the visual inertial odometer, and then connected in chronological order to obtain the mileage, posture and other information of the visual inertial odometer.

[0076] The mileage calculation method based on an iterative extended Kalman filter provided in this application is applied to a visual inertial odometry. The visual inertial odometry includes a camera and an IMU. First, the visual inertial odometry is dynamically initialized to obtain initialization state information of the visual inertial odometry, and an initial spatial three-dimensional map is established based on the initialization state information. The initialization state information includes the camera pose corresponding to the initial images taken by multiple cameras and the spatial position of the three-dimensional feature points on each initial image; then, state propagation is performed based on the initialization state information, image data collected in real time by the camera, and IMU data collected in real time by the IMU to obtain a linearized model of the visual inertial odometry; finally, the linearized model is iteratively updated multiple times based on the iterative extended Kalman filter method to obtain the current system state vector of the visual inertial odometry, and the current system state vector is used to indicate the position and posture of the visual inertial odometry in the initial spatial three-dimensional map. In the filter-based VIO system, most systems use static initialization. The solution of this application uses dynamic initialization, so that the solution can be used to initialize the filter-based VIO system during movement, which is more user-friendly. At the same time, in order to address the problem that the current extended Kalman filter system only performs one iteration in the update step, resulting in a large linearization error, this application adopts an iterative extended Kalman filter method, that is, multiple iterations are performed in the update step, thereby reducing the nonlinear error of the VIO system.

[0077] The solution of this application is described in detail below with reference to the accompanying drawings.

[0078] The present invention proposes a visual inertial odometry technology based on dynamic initialization and iterative extended Kalman filter. This technology improves the accuracy of the filter-based visual inertial odometry and increases the usability of the filter-based visual inertial odometry by using dynamic initialization.

[0079] The embodiment of the present application mainly includes three parts: dynamic initialization, state propagation and image-based state update. Figure 2 A schematic diagram of visual inertial odometry processing based on iterative extended Kalman filter provided in an embodiment of the present application is shown in FIG. Figure 2 As shown in FIG, the VIO based on iterative extended Kalman filter in the present invention mainly includes five parts:

[0080] A. Image processing. Image processing primarily involves feature extraction and tracking. During feature extraction, FAST feature points (defined by the FAST algorithm) are uniformly extracted across the image. A FAST feature point is defined as a pixel in an image that is separated from a sufficient number of surrounding pixels by a distinct region. During feature tracking, features are tracked using optical flow, and outliers are removed using epipolar geometry and Random Sample Consensus (RANSAC).

[0081] B. State propagation,For state propagation, it mainly includes IMU prediction system state and state vector augmentation.

[0082] C. Hybrid feature points. Hybrid feature points mainly include Multi-State Constrained Kalman Filter (MSCKF) points and Simultaneous Localization and Mapping (SLAM) points. MSCKF points are feature points with a tracking length of less than m frames and are not included in the state vector. SLAM points are feature points with a tracking length of greater than or equal to m frames and are included in the state vector.

[0083] D. Update SLAM points.

[0084] E. Update the MSCKF point.

[0085] Updating the SLAM points and the MSCKF points corresponds to updating the system state with the SLAM points and the MSCKF points, respectively. After updating the SLAM points and the MSCKF points, the state vector and the covariance matrix are obtained, and the iterative extended Kalman filter update can be performed.

[0086] First combine Figure 3 This section describes the initialization process.

[0087] Figure 3 A schematic diagram of the dynamic initialization process provided in the embodiment of the present application is shown in FIG. Figure 3 Shown, including:

[0088] S31 , constructing sliding windows in the multiple initial images according to the disparity threshold, and determining matching feature points between the sliding windows as three-dimensional feature points.

[0089] In the VIO system, the embodiments of the present application use dynamic initialization. Dynamic initialization requires a certain amount of motion excitation and does not require the system to be in a static state to complete the initialization process. During the dynamic initialization phase, the following four operations are completed: aligning the IMU local coordinate system with the direction of gravity, restoring the scale information of the monocular vision, calculating the initial position and motion speed, and calculating the acceleration bias and gyroscope bias.

[0090] Assuming the camera is in motion for a period of time, the camera data is subjected to visual structure from motion (SFM) while the IMU data is pre-integrated. The visual SFM information and the IMU pre-integrated information are aligned to complete dynamic initialization.

[0091] From multiple initial input images, keyframes are selected based on a certain disparity threshold and a sliding window is constructed. Matching feature points between the current keyframe and all previous keyframes are found as 3D feature points. Triangulation is then performed based on these matching feature points, and the spatial positions of the feature points are calculated. To ensure consistency between the keyframe poses and the triangulated feature points, a global bundle adjustment is performed on all keyframe poses and feature point spatial positions within the sliding window.

[0092] S32 , according to the initial IMU data corresponding to each initial image in the sliding window, obtain the gyroscope bias, IMU speed, gravity direction and scale factor corresponding to each initial image.

[0093] The gyroscope bias b can be calculated based on the following formula (1): g estimate:

[0094]

[0095] Where c0 represents the reference coordinate system of the visual SFM, I k and I k+1 Represent the IMU coordinate systems at time k and time k+1 respectively, is the rotation matrix of the IMU coordinates at time k in the reference coordinate system of the visual SFM, is the rotation matrix of the IMU coordinates at time k+1 in the reference coordinate system of the visual SFM, represents four-element multiplication, represents the pre-integrated rotation measurement, is the Jacobian matrix, and B is the set of values ​​of k.

[0096] Then, the velocity, gravity direction and scale factor of the IMU frame corresponding to each initial image frame in the sliding window are estimated as shown in the following formula (2):

[0097]

[0098] in, Indicates the speed of the IMU at time n, represents the gravitational acceleration in the visual SFM reference coordinate system, and s represents the scale factor;

[0099] represents the pre-integrated position measurement value, represents the pre-integrated velocity measurement, and:

[0100]

[0101]

[0102]

[0103] in, Represents the reference coordinate system c0 of the visual SFM to the IMU coordinate system I at time k k The rotation matrix of Represents the IMU coordinate system I at time k+1 k+1 The translation vector to the reference coordinate system c0 of the visual SFM, Represents the IMU coordinate system I at time k k The translation vector to the reference coordinate system c0 of the visual SFM, Represents the IMU coordinate system I at time k k The rotation matrix to the reference coordinate system c0 of the visual SFM, Δt k Indicates the time difference, represents the speed of the IMU at time k, Represents the IMU coordinate system I at time k+1 k+1 The rotation matrix to the reference coordinate system c0 of the visual SFM, Indicates the speed of the IMU at time k+1, is the gravitational acceleration in the reference coordinate system c0 of the visual SFM, I is the unit matrix,.

[0104] S33, obtaining initialization status information according to the gyroscope bias, the speed of the IMU, and the direction of gravity.

[0105] Specifically, we first correct the direction of gravity. Generally speaking, the magnitude of gravity is known. Therefore, based on this prior, we further correct g to obtain the corrected direction of gravity.

[0106] Then, the rotation angle between the global coordinate system and the camera coordinate system is obtained based on the rotation angle between the corrected gravity direction and the direction vector. The gyroscope bias and IMU velocity corresponding to each initial image are converted to the global coordinate system based on the rotation angle between the global coordinate system and the camera coordinate system. After obtaining the gravitational acceleration g, the rotation between it and the direction vector [0,0,1] is used to obtain the rotation between the global coordinate system and the first frame's camera coordinate system. This allows the rotation of all image frames to be converted to the global coordinate system.

[0107] Initialization state information is obtained based on the gyroscope bias and IMU velocity in the global coordinate system. Map point coordinates and translations are scaled using the obtained scale factor s. To further improve the accuracy of the image frame's corresponding state within the sliding window, an objective function is established using the IMU residual term and the image reprojection error term. Bundled adjustment optimization is then performed to obtain the initialization state information.

[0108] S34: establishing an initial spatial three-dimensional map according to the initialization state information and the scale factor.

[0109] Finally, the results of dynamic initialization are seamlessly integrated with the VIO system. Dynamic initialization calculates the camera pose and 3D feature points for all image frames in the sliding window. After successful dynamic initialization, the tracked 3D feature points are retriangulated using the camera pose estimated by dynamic initialization. Feature points identified by the Mahalanobis distance test and the number of tracked frames are then added to the state vector.

[0110] Figure 4 The dynamic initialization diagram provided in the embodiment of the present application is as follows: Figure 4 As shown in the figure, by aligning the visual SFM information with the IMU pre-integration information, the initialization state of the VIO system is obtained. The obtained initialization state of the VIO system is given to the visual inertial odometry based on the iterative extended Kalman filter, and the visual inertial odometry based on the iterative extended Kalman filter can be successfully started.

[0111] The benefits of adding initialized SLAM feature points to the state vector are: First, SLAM points are features observed over a long period of time. These three-dimensional feature points can be considered stable features. Adding them to the state vector continuously estimates the system state, and the system state becomes increasingly accurate, and the higher the system accuracy. Second, the existence of SLAM points can partially solve the situation where three-dimensional feature points such as pure rotation or stillness cannot be triangulated. In this case, SLAM points can still form a constraint on the system. Therefore, adding SLAM features can significantly improve the robustness of the system and reduce the jitter of the system trajectory after successful initialization.

[0112] After dynamic initialization, state propagation can be performed. Specifically, the IMU measurement model is first obtained based on the IMU data, and then the state vector is augmented based on the IMU measurement model and image data to obtain a linearized model.

[0113] State propagation mainly includes IMU prediction system state and state vector augmentation. IMU prediction system state refers to using IMU measurement values ​​to predict X I And its covariance matrix, state vector augmentation refers to adding the image pose X to the state vector c And update the covariance matrix.

[0114] The measurement model of IMU is as follows:

[0115] ω m (t)= I ω(t)+b g (t)+n r (t) (3)

[0116]

[0117] Among them, ω m (t) and a m (t) are the measurements of the IMU gyroscope and accelerometer, I ω(t) is the IMU angular velocity, b g (t) is the bias of the IMU angular velocity, n r (t) is the noise of IMU angular velocity. Represents the rotation matrix from the global coordinate system to the IMU coordinate system, G a(t) is the acceleration of IMU in the global coordinate system, G g is the acceleration of gravity in the global coordinate system, b is a (t) is the IMU acceleration bias, n a (t) is the noise of IMU acceleration.

[0118] Therefore, according to the above IMU measurement model, assuming that the time is from t k to tk+1 , the prediction equation of IMU is as follows:

[0119]

[0120] in, Represents the IMU coordinate system I from the global coordinate system to the k+1 time k+1 The rotation quaternion of Represents the IMU coordinate system I from the global coordinate system to the k-th moment k The rotation quaternion of Ω represents the antisymmetric matrix operation symbol, ω m (t k ) represents the measurement value of the gyroscope at time k, Indicates the bias of the IMU angular velocity at time k.

[0121]

[0122]

[0123] in, Indicates the velocity of the IMU in the global coordinate system G at time k+1, represents the velocity of the IMU in the global coordinate system G at time k, is the rotation matrix from the IMU coordinate system to the global coordinate system, G g is the acceleration of gravity in the global coordinate system, is the position of the IMU in the global coordinate system at time k+1, is the position of the IMU in the global coordinate system at time k, Δt = t k+1 -t k ,and and The calculation formula is as follows:

[0124]

[0125]

[0126] in, The rotation matrix from the coordinate system to the IMU coordinate system at time k, a m (t k ) is the measurement value of the accelerometer at time k, is the IMU acceleration bias at time k. Further, the error state transfer equation is calculated based on these equations, and the linearized model is obtained as shown in the following equation (8):

[0127]

[0128] is the error state vector at time k+1, Φ(t k+1 ,t k ) represents the state transition matrix from time k to time k+1, is the error state vector at time k, G k is the Jacobian matrix of IMU noise. According to this linearized model, the update of the covariance matrix P is as follows:

[0129] P(t k+1 )=Φ(t k+1 ,t k )P(t k )Φ(t k+1 ,t k ) T +Q d (9)

[0130] Among them, P(t k+1 ) is the covariance matrix at time k+1, P(t k ) is the covariance matrix at time k, Q d represents the covariance matrix of the noise.

[0131] When a new frame of image is received, the pose of the image at that moment should be added to the state vector, which is called state vector augmentation. In the process of state vector augmentation, both the state vector and the covariance matrix need to be updated.

[0132] In the VIO system, the image-based state update in the embodiment of the present application includes MSCKF point updates and SLAM point updates. MSCKF points refer to short-term feature information and are not added to the state vector. SLAM points refer to long-term feature information and are added to the state vector. The update process for both is similar: an image-based reprojection error observation equation is established, the Jacobian matrix is ​​calculated using the chain rule, and then it is linearized. Finally, an iterative extended Kalman filter is used for update.

[0133] Before constructing the linearized observation equation, the Jacobian matrix is ​​first calculated using the chain rule. For a certain feature, assume that the spatial point of the feature in the world coordinate system is G p f , the spatial point in the camera coordinate system is C p f , the coordinate of the normalized plane is z n , the pixel coordinate system coordinate is z m According to the camera imaging model, we can get:

[0134]

[0135] Among them, x m and y mare the image pixel coordinate measurements (i.e., horizontal and vertical coordinates, respectively), f x and f y is the focal length of the camera, c x and c y The horizontal and vertical coordinates of the camera's principal point. The 3D position in the camera coordinate system C p f To the normalized plane z n :

[0136]

[0137] and The horizontal and vertical coordinates of the space point in the camera coordinate system are respectively. The transformation from the world coordinate system space point to the camera coordinate system space point is:

[0138]

[0139] is the rotation matrix between the camera and IMU, is the rotation matrix from the global coordinate system to the IMU coordinate system, G p f is the position of the IMU in the camera coordinate system, G p I is the position of the IMU in the global coordinate system.

[0140] Calculate the Jacobian matrix [H x ] 2×6 for:

[0141]

[0142] in,

[0143] Calculate the Jacobian matrix [H f ] 2×3 for:

[0144]

[0145] Therefore, for the update of SLAM points, the linearized observation equation can be written as:

[0146]

[0147] is the error state vector, H f is the Jacobian matrix of the residual to the spatial point, is the error of the spatial point. The SLAM point is added to the state vector, and its linearized observation equation can be written as:

[0148]

[0149] is the entire error state vector. Assuming that the total number of observation frames of all features is N, the dimension of the residual r is 2N×1, and the total dimension of H is 2N×M, where M represents the total dimension of the system state vector. The total dimension is 2N×6C, where C represents the number of camera poses in the state vector (the size of the sliding window), and H f The total dimension is 2N×3P, where P represents the number of SLAM points in the state vector. Indicates the current IMU error state, represents the error state of all camera poses in the sliding window, Represents the error state of all SLAM points, and n represents the noise. is the Jacobian matrix of all feature observations for all camera poses (the matrix put together). For the update of the MSCKF point, since it is not added to the state vector, its linearized observation equation can be written as:

[0150]

[0151] After obtaining the linearized observation equation, the system state vector is updated using an iterative extended Kalman filter. The traditional extended Kalman filter system is mainly divided into two steps: prediction and update. Only one iteration is performed in the update step. However, there is still a large linearization error after only one iteration. To this end, the present invention considers introducing an iterative extended Kalman filter, that is, performing multiple iterations in the update step to reduce the nonlinear error of the system. Figure 5 The specific steps of iterative extended Kalman filter are introduced.

[0152] Figure 5 A schematic diagram of the process of iterative extended Kalman filter processing provided in the embodiment of the present application is shown as follows: Figure 5 Shown, including:

[0153] S51, setting the initial system state vector estimate.

[0154] First, the number of iterations i is initially set to 1, and the initial system state vector estimate is set to

[0155] S52, calculates the Jacobian matrix, Kalman gain and system state for the next iteration at the current state.

[0156] For the i-th iteration, perform the i-th iteration operation, which includes: calculating the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration; calculating the system state of the visual inertial odometry at the i+1-th iteration based on the linearized model, the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration, where i is initially 1 and is successively 1, 2, ..., N-1, and N is the number of iterations, which is a positive integer.

[0157]

[0158] h k (s) is the residual function, is the state vector at time k during the i-th iteration.

[0159]

[0160] P k|k-1 is the covariance matrix, is the noise covariance matrix.

[0161]

[0162] S53, determine whether i is equal to N-1, if so, execute S55, if not, execute S54.

[0163] S54, update i to i+1, and execute S52.

[0164] When the iteration stop condition is not reached, S52 is repeatedly executed.

[0165] S55 : Determine the system state of the visual inertial odometry at the Nth iteration as the current system state vector of the visual inertial odometry.

[0166] Save the last iteration result and update the system state and the corresponding covariance matrix.

[0167]

[0168]

[0169] in is the current system state vector, P k|k for The corresponding covariance matrix, I is the identity matrix, is the Kalman gain.

[0170] Figure 6 This is a schematic diagram of image data collected by a camera according to an embodiment of the present application. Figure 7 This is a schematic diagram of a simple scene sequence trajectory on the third floor provided in the embodiment of this application. Figure 8This is a schematic diagram of a difficult stairwell scene sequence trajectory provided by an embodiment of the present application, wherein Figure 7 and Figure 8 The sequence trajectory is based on Figure 6 The image data provided is obtained, Figure 7 and Figure 8 The starting point and the end point are marked in the figure. The closer the starting point and the end point are, the higher the accuracy of the trajectory.

[0171] Table 1 shows the comparison of the pose accuracy of this scheme and VINS-Mono on the EuRoC dataset. The smaller the number, the higher the pose accuracy.

[0172] Table 1

[0173]

[0174]

[0175] Table 2 compares the execution time of our scheme and VINS-Mono on the EuRoC dataset, with smaller numbers indicating faster execution. As shown in Table 2, our scheme has significantly lower execution time on all sequences compared to the VINS-Mono algorithm, indicating faster execution and lower time overhead.

[0176] Table 2

[0177] Sequence / Algorithm VINS-Mono (milliseconds) This solution (milliseconds) MH_01_Simple 47.6 19.3 MH_02_Simple 49.9 19.9 MH_03_Medium 49.7 18.0 MH_04_Difficult 50.7 17.3 MH_05_Difficult 51.4 20.3 V1_01_Simple 49.0 21.7 V1_02_Medium 49.9 16.9 V1_03_Difficult 46.2 15.1 V2_01_Simple 52.5 22.0 V2_02_Medium 49.9 17.0 V2_03_Difficult 46.1 14.9 average 49.4 18.4

[0178] Table 3 shows the performance of this solution compared with VINS-Mono. Figure 6 Comparison of pose accuracy on the provided image data. Smaller numbers indicate higher pose accuracy. As shown in Table 3, compared to the VINS-Mono algorithm, the pose accuracy of this scheme is significantly higher, and its positioning effect is better.

[0179] Table 3

[0180]

[0181] Table 4 shows the performance of this solution compared with VINS-Mono. Figure 8 Comparison of execution time on the provided image data, with smaller numbers indicating faster speeds. As shown in Table 4, compared to the VINS-Mono algorithm, the execution time of this scheme on each sequence is significantly lower, indicating that this scheme is faster and has less time overhead.

[0182] Table 4

[0183]

[0184] like Figures 6 to 8As shown in Tables 1 to 4, this solution has been tested in detail on the industry-recognized EuRoC dataset, with a transmission speed of 50 to 60 frames per second (FPS), and positioning accuracy of less than 0.1m and 2.0 degrees in multiple different scenarios. The speed and accuracy are significantly better than the existing optimal method VINS-Mono. Furthermore, the embodiment of the present application is Figure 6 New data was collected from a more challenging real-world scenario, demonstrating the performance of this solution in extremely challenging scenarios (such as weak textures, repetitive structures, and reflections). On this dataset, our solution achieved an average positioning accuracy of 0.43 meters, a 75% reduction compared to VINS-Mono's 1.64 meters, while maintaining a speed exceeding 50 FPS. These experimental results demonstrate the effectiveness of our solution and its significant advantages in both speed and accuracy.

[0185] In summary, the mileage calculation method based on the iterative extended Kalman filter provided by this application adopts dynamic initialization, so that the scheme can be applied to initialize the filter-based VIO system during the movement process, which is more user-friendly. At the same time, in order to address the problem that the current extended Kalman filter system only performs one iteration in the update step, resulting in a large linearization error, this application adopts an iterative extended Kalman filter method, that is, multiple iterations are performed in the update step, thereby reducing the nonlinear error of the VIO system.

[0186] Figure 9 This is a schematic diagram of the structure of a mileage calculation device based on an iterative extended Kalman filter provided in an embodiment of the present application. The device is applied to a visual inertial odometer, which includes a camera and an IMU, such as Figure 9 As shown, the device includes:

[0187] An initialization module 91 is configured to dynamically initialize the visual inertial odometry to obtain initialization state information of the visual inertial odometry and to establish an initial spatial three-dimensional map based on the initialization state information, wherein the initialization state information includes camera poses corresponding to a plurality of initial images captured by the camera and spatial positions of three-dimensional feature points on each of the initial images;

[0188] A first processing module 92 is configured to perform state propagation based on the initialization state information, the image data collected in real time by the camera, and the IMU data collected in real time by the IMU to obtain a linearized model of the visual inertial odometry;

[0189] The second processing module 93 is used to perform multiple iterative update processes on the linearized model based on the iterative extended Kalman filter method to obtain the current system state vector of the visual inertial odometry, and the current system state vector is used to indicate the position and posture of the visual inertial odometry in the initial spatial three-dimensional map.

[0190] In a possible implementation, the initialization module 91 is specifically configured to:

[0191] constructing sliding windows in the plurality of initial images according to a disparity threshold, and determining matching feature points between the sliding windows as the three-dimensional feature points;

[0192] Obtaining, according to the initial IMU data corresponding to each of the initial images in the sliding window, a gyroscope bias, an IMU speed, a gravity direction, and a scale factor corresponding to each of the initial images;

[0193] Acquiring the initialization state information according to the gyroscope bias, the speed of the IMU, and the direction of gravity;

[0194] An initial spatial three-dimensional map is established according to the initialization state information and the scale factor.

[0195] In a possible implementation, the initialization module 91 is specifically configured to:

[0196] Correcting the gravity direction to obtain a corrected gravity direction;

[0197] Obtaining a rotation angle between a global coordinate system and a camera coordinate system according to a rotation angle between the corrected gravity direction and the direction vector;

[0198] Converting the gyroscope bias and the IMU velocity corresponding to each of the initial images to the global coordinate system according to a rotation angle between the global coordinate system and the camera coordinate system;

[0199] The initialization state information is obtained according to the gyroscope bias in the global coordinate system and the speed of the IMU.

[0200] In a possible implementation, the first processing module 92 is specifically configured to:

[0201] Acquire an IMU measurement model according to the IMU data;

[0202] State vector augmentation processing is performed according to the IMU measurement model and the image data to obtain the linearized model.

[0203] In a possible implementation, the second processing module 93 is specifically configured to:

[0204] Set the initial system state vector estimate;

[0205] Executing an i-th iteration operation, the i-th iteration operation comprising: calculating the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration under the system state at the i-1-th iteration; calculating the system state of the visual inertial odometry at the i+1-th iteration according to the linearized model, the Jacobian matrix of the i-th iteration, and the Kalman gain of the i-th iteration; i is initially 1, and i is sequentially 1, 2, ..., N-1, where N is the number of iterations, N is a positive integer, and the system state at iteration 0 is the estimated value of the initial system state vector;

[0206] The system state of the visual inertial odometry at the Nth iteration is determined as a current system state vector of the visual inertial odometry.

[0207] In a possible implementation, the second processing module 93 is further configured to:

[0208] Obtain a covariance matrix of the current system state vector according to the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration.

[0209] The mileage calculation device based on the iterative extended Kalman filter provided in the embodiment of the present application is used to execute the above-mentioned method embodiment. Its implementation principle and technical effects are similar and will not be repeated here in this embodiment.

[0210] Figure 10 An example of a physical structure diagram of an electronic device is shown below. Figure 10As shown, the electronic device may include: a processor (processor) 1010, a communication interface (Communications Interface) 1020, a memory (memory) 1030 and a communication bus 1040, wherein the processor 1010, the communication interface 1020, and the memory 1030 communicate with each other through the communication bus 1040. The processor 1010 can call the logic instructions in the memory 1030 to execute the mileage calculation method based on the iterative extended Kalman filter, which includes: dynamically initializing the visual inertial odometry to obtain the initialization state information of the visual inertial odometry, and establishing an initial spatial three-dimensional map based on the initialization state information, wherein the initialization state information includes the camera pose corresponding to the multiple initial images taken by the camera and the spatial position of the three-dimensional feature points on each of the initial images; performing state propagation according to the initialization state information, the image data collected by the camera in real time, and the IMU data collected by the IMU in real time to obtain a linearized model of the visual inertial odometry; performing multiple iterative update processes on the linearized model based on the iterative extended Kalman filter method to obtain the current system state vector of the visual inertial odometry, and the current system state vector is used to indicate the position and posture of the visual inertial odometry in the initial spatial three-dimensional map.

[0211] In addition, the logic instructions in the above-mentioned memory 1030 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when sold or used as an independent product. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0212] On the other hand, the present invention also provides a computer program product, which includes a computer program, which can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the mileage calculation method based on the iterative extended Kalman filter provided by the above methods. The method includes: dynamically initializing the visual inertial odometry to obtain initialization state information of the visual inertial odometry, and establishing an initial spatial three-dimensional map based on the initialization state information, wherein the initialization state information includes the camera pose corresponding to multiple initial images taken by the camera and the spatial position of the three-dimensional feature points on each of the initial images; performing state propagation based on the initialization state information, the image data collected by the camera in real time, and the IMU data collected by the IMU in real time to obtain a linearized model of the visual inertial odometry; performing multiple iterative update processes on the linearized model based on the iterative extended Kalman filter method to obtain the current system state vector of the visual inertial odometry, and the current system state vector is used to indicate the position and posture of the visual inertial odometry in the initial spatial three-dimensional map.

[0213] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to execute the mileage calculation method based on the iterative extended Kalman filter provided by the above-mentioned methods, the method comprising: dynamically initializing the visual inertial odometer to obtain initialization state information of the visual inertial odometer, and establishing an initial spatial three-dimensional map based on the initialization state information, the initialization state information including the camera pose corresponding to multiple initial images taken by the camera and the spatial position of the three-dimensional feature points on each of the initial images; performing state propagation according to the initialization state information, the image data collected by the camera in real time, and the IMU data collected by the IMU in real time to obtain a linearized model of the visual inertial odometer; performing multiple iterative update processes on the linearized model based on the iterative extended Kalman filter method to obtain a current system state vector of the visual inertial odometer, the current system state vector being used to indicate the position and posture of the visual inertial odometer in the initial spatial three-dimensional map.

[0214] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0215] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, or of course, by hardware. Based on this understanding, the essence of the above technical solution or the part that contributes to the existing technology can be embodied in the form of a software product. The computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or certain parts of the embodiments.

[0216] Finally, it should be noted that 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 aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A mileage calculation method based on iterative extended Kalman filter, characterized in that: Applied to a visual inertial odometer, the visual inertial odometer includes a camera and an inertial measurement unit IMU, and the method includes: Dynamically initialize the visual inertial odometer to obtain initialization state information of the visual inertial odometer, and establish an initial spatial three-dimensional map based on the initialization state information, wherein the initialization state information includes camera poses corresponding to a plurality of initial images taken by the camera and spatial positions of three-dimensional feature points on each of the initial images; Performing state propagation according to the initialization state information, the image data collected in real time by the camera, and the IMU data collected in real time by the IMU to obtain a linearized model of the visual inertial odometer; Performing multiple iterative update processes on the linearized model based on an iterative extended Kalman filter method to obtain a current system state vector of the visual inertial odometer, wherein the current system state vector is used to indicate a position and a posture of the visual inertial odometer in the initial spatial three-dimensional map; The state propagation is performed according to the initialization state information, the image data collected by the camera in real time, and the IMU data collected by the IMU in real time to obtain the linearized model of the visual inertial odometer, including: Acquire an IMU measurement model according to the IMU data; Performing state vector augmentation processing according to the IMU measurement model and the image data to obtain the linearized model; Wherein, the state vector augmentation process includes: updating the state vector and updating the covariance matrix; the updating state vector includes: updating the SLAM point and updating the MSCKF point; the SLAM point refers to the long-term feature information, and the SLAM point is added to the state vector; the MSCKF point refers to the short-term feature information, and the MSCKF point is not added to the state vector; The linearized observation equation in which the SLAM point is added to the state vector is: The linearized observation equation without the MSCKF point added to the state vector is: in, is the Jacobian matrix of all feature observations for all camera poses, H f is the Jacobian matrix of the residual to the spatial point; is the entire error state vector, Indicates the current IMU error state, represents the error state of all camera poses in the sliding window, represents the error state of all SLAM points, and n represents the noise.

2. The method according to claim 1, characterized in that The dynamically initializing the visual inertial odometer to obtain initialization state information of the visual inertial odometer, and establishing an initial spatial three-dimensional map based on the initialization state information, includes: constructing sliding windows in the plurality of initial images according to a disparity threshold, and determining matching feature points between the sliding windows as the three-dimensional feature points; According to the initial IMU data corresponding to each of the initial images in the sliding window, the gyroscope bias, the speed, the gravity direction and the scale factor of the IMU corresponding to each of the initial images are obtained; Acquire the initialization state information according to the gyroscope bias, the speed of the IMU and the gravity direction; An initial spatial three-dimensional map is established according to the initialization state information and the scale factor.

3. The method according to claim 2, characterized in that The obtaining the initialization state information according to the gyroscope bias, the speed of the IMU and the gravity direction includes: Correcting the gravity direction to obtain a corrected gravity direction; Obtaining a rotation angle between a global coordinate system and a camera coordinate system according to a rotation angle between the corrected gravity direction and the direction vector; According to the rotation angle between the global coordinate system and the camera coordinate system, the gyroscope bias and the speed of the IMU corresponding to each of the initial images are converted into the global coordinate system; The initialization state information is obtained according to the gyroscope bias and the speed of the IMU in the global coordinate system.

4. The method according to claim 1, characterized in that The method of performing multiple iterative updating processes on the linearized model based on the iterative extended Kalman filter method to obtain the current system state vector of the visual inertial odometer includes: Set the initial system state vector estimate; Execute an i-th iteration operation, wherein the i-th iteration operation includes: calculating the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration under the system state at the i-1-th iteration; calculating the system state of the visual inertial odometer at the i+1-th iteration according to the linearized model, the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration; i is initially 1, i is 1, 2, ..., N-1 in sequence, N is the number of iterations, N is a positive integer, and the system state at the 0th iteration is the initial system state vector estimate; The system state of the visual inertial odometry at the Nth iteration is determined as a current system state vector of the visual inertial odometry.

5. The method according to claim 4, characterized in that The method further comprises: The covariance matrix of the current system state vector is obtained according to the Jacobian matrix of the i-th iteration and the Kalman gain of the i-th iteration.

6. A mileage calculation device based on iterative extended Kalman filter, characterized in that: Applied to a visual inertial odometer, the visual inertial odometer includes a camera and an IMU, and the device includes: An initialization module is used to dynamically initialize the visual inertial odometer to obtain initialization state information of the visual inertial odometer, and establish an initial spatial three-dimensional map based on the initialization state information, wherein the initialization state information includes camera poses corresponding to a plurality of initial images taken by the camera and spatial positions of three-dimensional feature points on each of the initial images; A first processing module, configured to perform state propagation according to the initialization state information, the image data collected in real time by the camera, and the IMU data collected in real time by the IMU, so as to obtain a linearized model of the visual inertial odometer; A second processing module is used to perform multiple iterative update processing on the linearized model based on an iterative extended Kalman filter method to obtain a current system state vector of the visual inertial odometer, wherein the current system state vector is used to indicate a position and a posture of the visual inertial odometer in the initial spatial three-dimensional map; The first processing module is further used for: Acquire an IMU measurement model according to the IMU data; Performing state vector augmentation processing according to the IMU measurement model and the image data to obtain the linearized model; Wherein, the state vector augmentation process includes: updating the state vector and updating the covariance matrix; the updating state vector includes: updating the SLAM point and updating the MSCKF point; the SLAM point refers to the long-term feature information, and the SLAM point is added to the state vector; the MSCKF point refers to the short-term feature information, and the MSCKF point is not added to the state vector; The linearized observation equation in which the SLAM point is added to the state vector is: The linearized observation equation without the MSCKF point added to the state vector is: in, is the Jacobian matrix of all feature observations for all camera poses, H f is the Jacobian matrix of the residual to the spatial point; is the entire error state vector, Indicates the current IMU error state, represents the error state of all camera poses in the sliding window, represents the error state of all SLAM points, and n represents the noise.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the mileage calculation method based on the iterative extended Kalman filter as described in any one of claims 1 to 5 is implemented.

8. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the mileage calculation method based on iterative extended Kalman filter as described in any one of claims 1 to 5 is implemented.

9. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the mileage calculation method based on iterative extended Kalman filter as described in any one of claims 1 to 5 is implemented.