A visual inertial odometry method and system based on transformation error state

Abstract

The present invention discloses a visual inertial odometry method and system based on transformation error state, and relates to the technical field of visual inertial odometry optimization. The technical points of the present invention include: estimating posture information using a transformation extended Kalman filter, including: establishing a system continuous-time motion model and measurement equation based on real-time acquired visual and IMU data; establishing a linearized error state system based on the system continuous-time motion model and measurement equation; designing a linear time-varying transformation, and using the linear time-varying transformation to transform the linearized error state system into an error state system in which the system state is independent of the system's unobservable subspace; performing state estimation based on the transformed linearized error state system to obtain estimated posture information. The present invention proposes a transformation-based method to solve the inconsistency problem in VINS, alleviates the observability mismatch problem, and ensures that the visual inertial odometry using the transformation extended Kalman filter has consistent estimation results.

Description

Technical Field

[0001] The present invention relates to the technical field of visual inertial odometry optimization, and in particular to a visual inertial odometry method and system based on transformation error state. Background Art

[0002] In recent years, visual-inertial navigation systems (VINS) have been widely used in robotics, virtual reality, and augmented reality due to their compact size and low cost. By leveraging the complementary properties of inertial measurement units (IMUs) and cameras in navigation and localization tasks, VINS can provide highly accurate position information.

[0003] VINS can be categorized into two main approaches: optimization-based methods and filtering-based methods. Optimization-based methods achieve high localization accuracy through nonlinear optimization, while filtering-based methods prioritize computational efficiency. Notably, filtering-based methods have recently made significant progress, achieving accuracy levels approaching those of optimization-based methods.

[0004] Classical filter-based VINS estimators, such as the multiplicative extended Kalman filter (M-EKF), also known as the error-state Kalman filter (ESKF), use a special orthogonal group, SO(3), to represent the robot pose. The remaining variables, including the robot's position, velocity, and landmark positions, are modeled in a vector space. Although the ESKF concisely models the state, it is inherently inconsistent. In the absence of absolute information, the VINS system originally has four unobservable dimensions: the global translation and the rotation along the gravity direction. However, the ESKF mistakenly treats the unobservable rotation as observable. As a result, the ESKF obtains false information about this erroneously observable state, leading to inconsistency. Summary of the Invention

[0005] In view of the above problems, the present invention proposes a visual inertial odometry based on transformation error state, which uses the transformation extended Kalman filter to estimate motion information.

[0006] According to one aspect of the present invention, a visual inertial odometry method based on a transformation error state is proposed. The method uses a transformation extended Kalman filter to estimate pose information, including:

[0007] Establish the system continuous-time motion model and measurement equations based on the real-time visual and IMU data;

[0008] Establishing a linearized error state system based on the system continuous-time motion model and the measurement equation;

[0009] Designing a linear time-varying transformation, and utilizing the linear time-varying transformation to transform the linearized error state system into an error state system whose system state is independent of the system's unobservable subspace;

[0010] The state estimation is performed based on the transformed linearized error state system to obtain the estimated pose information.

[0011] Furthermore, the continuous-time motion model of the system is established as follows:

[0012]

[0013] in, represents the derivative of the system state x; R is the posture of the IMU in the global coordinate system; ω m and a m are the gyroscope and accelerometer measurements in the IMU coordinate system; n g and n a are Gaussian white noise with zero mean; g is the gravity vector in the global coordinate system; f is the system kinematic function, [.] × represents the Lie algebra defined on SO(3);

[0014] The measurement equation is established as follows:

[0015] y=h( I p L )+∈

[0016] Where h represents the transformation function that transforms the point from the IMU coordinate system to the camera coordinate system, I p L represents the position of the landmark relative to the IMU; ∈ is zero-mean Gaussian noise.

[0017] Furthermore, the linearization error state system is established as follows:

[0018]

[0019] Where, Indicates error status The derivative of the error state x represents the true state, represents the state estimate, p is the position of the IMU in the global coordinate system, v is the velocity of the IMU in the global coordinate system, and 1 is the landmark position; is the corresponding estimated value; is the corresponding error; the state transfer Jacobian matrix I3 is the 3×3 identity matrix; the noise propagation Jacobian matrix Represents the measurement residual; the measurement Jacobian matrix H = ΠH e ,in Essential measure Jacobian matrix

[0020] Furthermore, the linear time-varying transformation is designed as follows:

[0021]

[0022] Where T represents the linear time-varying transformation matrix, which is a non-singular matrix function;

[0023] The linear error state system is transformed using the linear time-varying transformation matrix. The transformed linear error state system is:

[0024]

[0025] Where, the transformed state transfer Jacobian matrix is Represents the error state after transformation The derivative of is the derivative of the linear time-varying transformation matrix; the noise propagation Jacobian matrix G after transformation * =TG; the transformed measurement Jacobian matrix H * =HT -1 , The transformed essential measure Jacobian matrix

[0026] Furthermore, the performing state estimation based on the transformed linearized error state system to obtain estimated pose information includes:

[0027] State recursion: At each timestamp t k+1 , once a new IMU measurement is received, the last moment t k The state estimate of is propagated to the current time by numerically integrating the continuous-time motion model; accordingly, the transformed error state covariance is propagated as follows:

[0028]

[0029] Among them, the error state transfer matrix Φ * (t k+1 ,t k ) is calculated by the following integro-differential equation: τ represents t k to t k+1 At any moment between Calculated by the following formula:

[0030]

[0031] in, E(.) represents mathematical expectation;

[0032] State update: Get the Kalman state correction in the transformed space and inverse transform the Kalman state correction back to the original space to correct the estimated value;

[0033] make Represents the Kalman state correction in the transformation space; calculate the Kalman gain K * as follows:

[0034]

[0035] in, In the current best state estimate conduct assessments;

[0036] The transformed error state covariance is updated as:

[0037]

[0038] The Kalman state correction is inversely transformed from the transformed space to the original space by the following formula:

[0039]

[0040] Among them, δx represents the Kalman state correction in the original space;

[0041] The state estimate is corrected using the following formula:

[0042]

[0043] According to another aspect of the present invention, a visual inertial odometry system based on a transformation error state is proposed, the system comprising:

[0044] a data acquisition module configured to acquire vision and IMU data;

[0045] A posture estimation module is configured to estimate posture information using a transformed extended Kalman filter, including an error state construction submodule and a state estimation submodule, wherein the error state construction submodule is configured to establish a system continuous-time motion model and measurement equations based on real-time acquired visual and IMU data; establish a linearized error state system based on the system continuous-time motion model and the measurement equations; design a linear time-varying transformation, and use the linear time-varying transformation to transform the linearized error state system into an error state system in which the system state is independent of the system's unobservable subspace; and the state estimation submodule is configured to perform state estimation based on the transformed linearized error state system to obtain estimated posture information.

[0046] Furthermore, the system continuous-time motion model in the error state construction submodule is established as follows:

[0047]

[0048] in, represents the derivative of the system state x; R is the posture of the IMU in the global coordinate system; ω m and a m are the gyroscope and accelerometer measurements in the IMU coordinate system; n g and n a are Gaussian white noise with zero mean; g is the gravity vector in the global coordinate system; f is the system kinematic function, [.] × represents the Lie algebra defined on SO(3);

[0049] The measurement equation is established as follows:

[0050] y=h( I p L )+∈

[0051] Where h represents the transformation function that transforms the point from the IMU coordinate system to the camera coordinate system, I p L represents the position of the landmark relative to the IMU; ∈ is zero-mean Gaussian noise.

[0052] Furthermore, the linearized error state system in the error state construction submodule is established as follows:

[0053]

[0054] Where, Indicates error status The derivative of the error state x represents the true state, represents the state estimate, p is the position of the IMU in the global coordinate system, v is the velocity of the IMU in the global coordinate system, and 1 is the landmark position; is the corresponding estimated value; is the corresponding error; the state transfer Jacobian matrix I3 is the 3×3 identity matrix; the noise propagation Jacobian matrix Represents the measurement residual; the measurement Jacobian matrix H = ΠH e ,in Essential measure Jacobian matrix

[0055] Furthermore, the linear time-varying transformation in the error state construction submodule is designed as follows:

[0056]

[0057] Where T represents the linear time-varying transformation matrix, which is a non-singular matrix function;

[0058] The linear error state system is transformed using the linear time-varying transformation matrix. The transformed linear error state system is:

[0059]

[0060] Where, the transformed state transfer Jacobian matrix is Represents the error state after transformation The derivative of is the derivative of the linear time-varying transformation matrix; the noise propagation Jacobian matrix G after transformation * =TG; the transformed measurement Jacobian matrix H * =HT -1 , The transformed essential measure Jacobian matrix

[0061] Furthermore, the state estimation submodule performs state estimation based on the transformed linearized error state system to obtain estimated pose information, including:

[0062] State recursion: At each timestamp t k+1 , once a new IMU measurement is received, the last moment t k The state estimate of is propagated to the current time by numerically integrating the continuous-time motion model; accordingly, the transformed error state covariance is propagated as follows:

[0063]

[0064] Among them, the error state transfer matrix Φ * (t k+1 ,t k ) is calculated by the following integro-differential equation: τ represents t k to t k+1 At any moment between Calculated by the following formula:

[0065]

[0066] in, E(.) represents mathematical expectation;

[0067] State update: Get the Kalman state correction in the transformed space and inverse transform the Kalman state correction back to the original space to correct the estimated value;

[0068] make Represents the Kalman state correction in the transformation space; calculate the Kalman gain K * as follows:

[0069]

[0070] in, In the current best state estimate conduct assessments;

[0071] The transformed error state covariance is updated as:

[0072]

[0073] The Kalman state correction is inversely transformed from the transformed space to the original space by the following formula:

[0074]

[0075] Among them, δx represents the Kalman state correction in the original space;

[0076] The state estimate is corrected using the following formula:

[0077]

[0078] The beneficial technical effects of the present invention are:

[0079] In visual inertial odometry, Kalman filters are used to fuse visual and inertial data. By combining the high-frequency data of the IMU and the low-frequency data of the visual sensor, the Kalman filter can obtain high-precision pose estimation. Based on this, the present invention proposes a visual inertial odometry method and system based on transformation error state. Among them, a transformation-based method is proposed to solve the inconsistency problem in VINS; a linear time-varying transformation is designed to make the unobservable subspace of the transformation system independent of the state; and a transformation extended Kalman filter is proposed, which is a consistent VINS estimator that performs state estimation based on the transformed linearized error state system. Analysis proves that the transformation extended Kalman filter has correct observability under the most elegant comparability. Therefore, the transformation extended Kalman filter proposed in the present invention alleviates the observability mismatch problem and ensures that the visual inertial odometry using the transformation extended Kalman filter has consistent estimation results. BRIEF DESCRIPTION OF THE DRAWINGS

[0080] The present invention can be better understood by referring to the description given below in conjunction with the accompanying drawings, which together with the following detailed description are included in this specification and form a part of this specification, and are used to further illustrate the preferred embodiments of the present invention and explain the principles and advantages of the present invention.

[0081] Figure 1 This is a flowchart of a visual inertial odometry method based on transformation error state according to an embodiment of the present invention.

[0082] Figure 2 Schematic diagram of the transformation of an original system whose unobservable subspace depends on the state into a system whose unobservable subspace is independent of the state in an embodiment of the present invention.

[0083] Figure 3 This is the process of propagating and updating the covariance estimation of T-ESKF in the transformation space in an embodiment of the present invention.

[0084] Figure 4 : The simulated trajectory in the embodiment of the present invention; wherein, the black dotted line is the true value of the trajectory, the blue solid line is the ESKF estimated trajectory, and the red solid line is the T-ESKF estimated trajectory.

[0085] Figure 5 This is an example diagram of the direction and position RMSE of 100 Monte Carlo simulations under different measurement noises in an embodiment of the present invention.

[0086] Figure 6 1000 Monte Carlo simulation NEES results in an embodiment of the present invention; the black dotted line is the theoretical optimal value. DETAILED DESCRIPTION

[0087] In order to enable those skilled in the art to better understand the present invention, exemplary embodiments or examples of the present invention will be described below with reference to the accompanying drawings. Obviously, the described embodiments or examples are only some of the embodiments or examples of the present invention, and not all of them. Based on the embodiments or examples of the present invention, all other embodiments or examples obtained by those skilled in the art without creative work should fall within the scope of protection of the present invention.

[0088] The present invention proposes a visual inertial odometry method and system based on transformed error states, in which a novel method is proposed to solve the inconsistency caused by observability mismatch in VINS. The key idea is to apply a transformation to the error state system to ensure that the transformed error state system has correct observability. Specifically, a transformation is designed to ensure that the unobservable subspace of the transformed system is independent of the state and is not affected by changes in the linearization point. Further proposed is a transformed extended Kalman filter (T-ESKF), which is a consistent VINS estimator that uses the transformed error state system for state estimation. A large number of simulations and experiments show that the performance of the method proposed in the present invention is significantly improved compared with ESKF in terms of accuracy and consistency.

[0089] The embodiment of the present invention proposes a visual inertial odometry method based on the transformation error state, such as Figure 1 As shown in Figure 1, this method uses a transform extended Kalman filter to estimate pose information, specifically including:

[0090] S1. Establish the system continuous-time motion model and measurement equations based on the real-time visual and IMU data;

[0091] S2. Establishing a linearized error state system based on the system continuous-time motion model and the measurement equation;

[0092] S3. Design a linear time-varying transformation, and utilize the linear time-varying transformation to transform the linearized error state system into an error state system whose system state is independent of the system unobservable subspace;

[0093] S4. Perform state estimation based on the transformed linearized error state system to obtain estimated pose information.

[0094] The embodiments of the present invention are described in detail below.

[0095] 1. Problem Description

[0096] The state of the nonlinear model system is expressed as follows:

[0097] x=(R,p,v,l1,…,1 m )

[0098] Among them, R∈SO(3) and They are the attitude and position of IMU in the global coordinate system, is the velocity of the IMU in the global coordinate system, and is the unknown landmark position in the global coordinate system. Because the case of multiple landmarks is a simple generalization of the case of a single landmark, for simplicity, only the case of a single landmark l is considered in the subsequent description.

[0099] IMU motion model: The continuous time motion model is expressed as follows:

[0100]

[0101] in, and are the gyroscope and accelerometer measurements in the IMU coordinate system, and is a Gaussian white noise with zero mean, satisfying g is the gravity vector in the global coordinate system; f is the system kinematic function, [.] × represents the Lie algebra defined on SO(3).

[0102] make I p L Represents the position of a landmark relative to the IMU as follows:

[0103]

[0104] As the camera explores the environment and obtains visual measurements of landmarks, the measurement equation is expressed as:

[0105] y=h( I p L )+∈ (2)

[0106] in, Convert points from the IMU coordinate system to the camera coordinate system, is the camera perspective projection function, is Gaussian noise with zero mean, satisfying the mathematical expectation

[0107] 2. Linearized error state system

[0108] set up Denotes the state estimate, which is propagated according to Equation (1) by setting the process noise to zero:

[0109]

[0110] make Represents the error state, describing the true state x and its estimated value In the error state Kalman filter, the error state of the direction is defined using a logarithmic map on SO(3), while the errors of the other variables are defined in the vector space:

[0111]

[0112] Where l is the landmark location; is the corresponding estimated value; is the corresponding error;

[0113] The measurement residual is:

[0114]

[0115] Linearizing Equations (3) and (4) at the current state estimate yields the following linearized error state system:

[0116]

[0117] The state transfer Jacobian matrix is

[0118]

[0119] The noise propagation Jacobian matrix is

[0120]

[0121] The measure Jacobian matrix is:

[0122] H=ΠH e (6.a)

[0123]

[0124] H e is the essential measure Jacobian matrix. Note that in F and H e middle is state-dependent, while other blocks are fixed values.

[0125] 3. Inconsistency of ESKF

[0126] Observability refers to the ability of a system to restore its initial state using all available measurements. The set of states that cannot be restored by measurements constitutes the unobservable subspace of the system. For the system described in equation (3), the local observability matrix can be used for observability analysis. Let M represent the local observability matrix of system (3), then

[0127]

[0128] Where M0=ΠH e ,

[0129] First, assume that F and H e are all evaluated at the same linearization point. The unobservable subspace MN = 0 of the system is:

[0130]

[0131] N indicates that VINS has four unobservable directions: the global position (represented by the first three columns) and the global orientation around the z-axis (represented by the last column).

[0132] However, the above assumptions are not practical for ESKF. ESKF evaluates F and H in the posterior and prior estimations respectively. e Evaluating the Jacobian matrix on two different state trajectories causes the state-dependent unobservable directions to vanish from the unobservable subspace. Therefore, the unobservable subspace of the ESKF is:

[0133]

[0134] In the ESKF, the global orientation around the z-axis is mistakenly considered observable, leading to inconsistencies. Note that the unobservable orientation, which is independent of the state, remains unchanged when the linearization point changes.

[0135] To this end, the present invention designs a linear time-varying transformation, which is applied to the error state, such as Figure 2 As shown, the dimension of the unobservable subspace of the ESKF is reduced by one (in the ESKF, the global rotation becomes incorrectly observable). In contrast, the unobservable subspace of the T-ESKF remains unchanged to changes in the linearization point. This makes the unobservable subspace independent of the state, thereby maintaining correct observability under changing linearization points.

[0136] 4. Transformed linearized error state system

[0137] Inconsistencies in VINS are addressed by transforming the linearized error state system into a state-independent unobservable subspace. First, the transformed linearized error state system is derived. Subsequently, to prevent observability from being affected by changes in the linearization point, a transformation is designed such that the unobservable subspace of the transformed system is state-independent.

[0138] Express the linear time-varying transformation as Where T(·) is a 12×12 non-singular matrix function, which is yet to be selected. The error state after transformation is obtained by Multiplying with the original error state yields:

[0139]

[0140] For simplicity, if there is no ambiguity, the variable It will be omitted in the following text. Taking the derivative of both sides of the above equation with respect to time, we get

[0141]

[0142] Substituting (9.a) and (9.b) into system (3) yields the transformed linearized error state system, as shown below:

[0143]

[0144] in:

[0145]

[0146] According to Equation (6.a) and Equation (11), the essential measurement Jacobian matrix H e is transformed into:

[0147]

[0148] In order to make the unobservable subspace independent of the state, an effective method is to make F * and It has nothing to do with the state. To this end, the following transformation matrix is designed:

[0149]

[0150] Substituting equation (13) into equations (11)-(12), we get the transformed Jacobian matrix:

[0151]

[0152] Next, we verify that the unobservable subspace of the transformation system (10) is independent of the state. The local observability matrix M of the transformation system is * This is done by replacing (F,H e ) with The unobservable subspace M of the transformation system * N * =0 is:

[0153]

[0154] The above formula shows that the designed transformation successfully ensures a state-independent unobservable subspace.

[0155] 4. Transformation-based Error State Kalman Filter

[0156] A consistent VINS estimator based on the transformed linearized error state system (10) is proposed. The process of T-ESKF is as follows Figure 3 shown.

[0157] 1) State recursion: At each timestamp t k+1 , once a new IMU measurement is received, the last moment t k The state estimate of is propagated to the current time by numerically integrating (1). Accordingly, the transformed error state covariance is propagated as follows:

[0158]

[0159] The error state transfer matrix Φ * (t k+1 ,t k ) is calculated by integrating the differential equation:

[0160]

[0161] The initial condition is Φ * (t k ,t k )=I 12 The noise propagation matrix is calculated as follows:

[0162]

[0163] Where τ represents t k to t k+1 Any time in between.

[0164] 2) State update: In T-ESKF, the Kalman state correction is first obtained in the transformed space and then inversely transformed back to the original space to correct the estimated value.

[0165] make Represents the Kalman state correction in the transformation space. The Kalman gain is calculated according to the classical extended Kalman filter (EKF):

[0166]

[0167] in In the current best state estimate The transformed error state covariance is updated as:

[0168]

[0169] The Kalman state correction is inversely transformed from the transformed space to the original space by the following formula:

[0170]

[0171] Among them, δx represents the Kalman state correction in the original space.

[0172] Finally, the state estimate is corrected as follows:

[0173]

[0174] It is further demonstrated that T-ESKF has the optimal Jacobian matrix and correct observability. * and H * It is evaluated under the current best estimate, so the optimality of the Jacobian matrix is automatically preserved. The observability matrix of T-ESKF is:

[0175]

[0176] Correspondingly, the unobservable subspace of T-ESKF is:

[0177]

[0178] Equations (15) and (16) show that the error reduction of unobservable dimensions is avoided and T-ESKF has correct observability. Therefore, T-ESKF does not suffer from the problem of inconsistent estimates caused by observability mismatch.

[0179] The effectiveness of the present invention is further verified through Monte Carlo simulation.

[0180] T-ESKF is compared with ESKF. The specific simulation parameters are shown in Table 1.

[0181] Table 1 Monte Carlo simulation parameters

[0182] parameter Numerical parameter Numerical Accelerometer measurement noise 2.00e-03 Gyroscope measurement noise 1.70e-04 Accelerometer wander noise 3.00e-03 Gyroscope wander noise 2.00e-05 Camera measurement error 2 IMU frequency 400 Number of landmarks 100 Camera frequency 10

[0183] The following metrics are used to evaluate performance: Root Mean Square Error (RMSE) for trajectory accuracy evaluation and Normalized Estimation Error Square (NEES) for consistency evaluation. Smaller RMSE indicates better accuracy, while NEES values closer to 1 indicate better consistency.

[0184] To evaluate the accuracy, Figure 4 T-ESKF is tested on the three trajectories shown, performing 100 Monte Carlo simulations for each trajectory. Table 2 reports the average RMSE resulting from these 100 simulations. As can be seen in Table 2, T-ESKF demonstrates significant accuracy advantages over ESKF.

[0185] Table 2. Root mean square error (RMSE) of average angle (degrees) and position (meters) for 1100 Monte Carlo simulations

[0186] Trajectory ESKF T-ESKF Udel-Gore 1.13 / 0.26 0.58 / 0.20 Udel-Neig. 11.6 / 72.7 6.83 / 41.8 TUM-Corr. 0.68 / 0.28 0.34 / 0.15

[0187] To compare the performance of ESKF and T-ESKF under different measurement noise levels, the measurement noise level is set to [0.4, 0.7, 1, 2, 3, 4, 5] pixels. Each method performs 100 Monte Carlo simulations on the Udel-Gore trajectory. The RMSE results of these 100 simulations are shown in Figure 2. Figure 5 shown. Figure 5 It shows that T-ESKF has significant advantages over ESKF under different noise levels.

[0188] To evaluate the consistency, 1000 Monte Carlo experiments were performed using the Udel-Gore trajectory. The simulation parameters were consistent with those listed in Table 1. The NEES results of the 1000 Monte Carlo experiments were Figure 6 Given in. Figure 6 The top three subplots show the average NEES over time. Compared to ESKF, the average NEES value of T-ESKF is closer to the theoretical value of 1, indicating a higher level of consistency. The bottom three subplots show the NEES frequency distribution of 1000 trials throughout the simulation. The NEES histogram of T-ESKF closely matches the theoretical chi-square distribution, further demonstrating good consistency.

[0189] This paper proposes a novel approach to address the VINS inconsistency problem caused by observability mismatch. A linear time-varying transform is designed and applied to the linearized error state system of the ESKF, making the unobservable subspace states of the transformed system independent. This transformation ensures that the observability of the system remains unchanged despite changes in the linearization point, thereby ensuring consistency. Based on the transformed error state system, a consistent VINS estimator, called T-ESKF, is proposed. Test results show that the performance of T-ESKF is comparable to that of estimators based on Lie groups.

[0190] Another embodiment of the present invention provides a visual inertial odometry system based on a transformation error state, the system comprising:

[0191] a data acquisition module configured to acquire vision and IMU data;

[0192] A posture estimation module is configured to estimate posture information using a transformed extended Kalman filter, including an error state construction submodule and a state estimation submodule, wherein the error state construction submodule is configured to establish a system continuous-time motion model and measurement equations based on real-time acquired visual and IMU data; establish a linearized error state system based on the system continuous-time motion model and the measurement equations; design a linear time-varying transformation, and use the linear time-varying transformation to transform the linearized error state system into an error state system in which the system state is independent of the system's unobservable subspace; and the state estimation submodule is configured to perform state estimation based on the transformed linearized error state system to obtain estimated posture information.

[0193] In this embodiment, preferably, the system continuous-time motion model in the error state construction submodule is established as follows:

[0194]

[0195] in, represents the derivative of the system state x; R is the posture of the IMU in the global coordinate system; ω m and a m are the gyroscope and accelerometer measurements in the IMU coordinate system; n g and n a are Gaussian white noise with zero mean; g is the gravity vector in the global coordinate system; f is the system kinematic function, [.] × represents the Lie algebra defined on SO(3);

[0196] The measurement equation is established as follows:

[0197] y=h( I p L )+∈

[0198] Where h represents the transformation function that transforms the point from the IMU coordinate system to the camera coordinate system, I p L represents the position of the landmark relative to the IMU; ∈ is zero-mean Gaussian noise.

[0199] In this embodiment, preferably, the linearized error state system in the error state construction submodule is established as follows:

[0200]

[0201] Where, Indicates error status The derivative of the error state x represents the true state, represents the state estimate, p is the position of the IMU in the global coordinate system, v is the velocity of the IMU in the global coordinate system, and 1 is the landmark position; is the corresponding estimated value; is the corresponding error; the state transfer Jacobian matrix I3 is the 3×3 identity matrix; the noise propagation Jacobian matrix Represents the measurement residual; the measurement Jacobian matrix H = ΠH e ,in Essential measure Jacobian matrix

[0202] In this embodiment, preferably, the linear time-varying transformation in the error state construction submodule is designed as follows:

[0203]

[0204] Where T represents the linear time-varying transformation matrix, which is a non-singular matrix function;

[0205] The linear error state system is transformed using the linear time-varying transformation matrix. The transformed linear error state system is:

[0206]

[0207] Where, the transformed state transfer Jacobian matrix is Represents the error state after transformation The derivative of is the derivative of the linear time-varying transformation matrix; the noise propagation Jacobian matrix G after transformation * =TG; the transformed measurement Jacobian matrix H * =HT -1 , The transformed essential measure Jacobian matrix

[0208] In this embodiment, preferably, the state estimation submodule performs state estimation based on the transformed linearized error state system to obtain estimated pose information, including:

[0209] State recursion: At each timestamp t k+1 , once a new IMU measurement is received, the last moment t k The state estimate of is propagated to the current time by numerically integrating the continuous-time motion model; accordingly, the transformed error state covariance is propagated as follows:

[0210]

[0211] Among them, the error state transfer matrix Φ * (t k+1 ,t k ) is calculated by the following integro-differential equation: τ represents t k to t k+1 At any moment between Calculated by the following formula:

[0212]

[0213] in, E(.) represents mathematical expectation;

[0214] State update: Get the Kalman state correction in the transformed space and inverse transform the Kalman state correction back to the original space to correct the estimated value;

[0215] make Represents the Kalman state correction in the transformation space; calculate the Kalman gain K * as follows:

[0216]

[0217] in, In the current best state estimate conduct assessments;

[0218] The transformed error state covariance is updated as:

[0219]

[0220] The Kalman state correction is inversely transformed from the transformed space to the original space by the following formula:

[0221]

[0222] Among them, δx represents the Kalman state correction in the original space;

[0223] The state estimate is corrected using the following formula:

[0224]

[0225] The functions of a visual-inertial odometry system based on a transformation error state in an embodiment of the present invention can be described by the aforementioned visual-inertial odometry method based on a transformation error state. Therefore, for the parts not described in detail in the system embodiment, please refer to the above method embodiment and will not be repeated here.

[0226] Although the present invention has been described with respect to a limited number of embodiments, those skilled in the art, having benefit of the foregoing description, will appreciate that other embodiments are contemplated within the scope of the invention thus described. This disclosure is intended to be illustrative rather than restrictive of the scope of the invention, which is defined by the appended claims.

Claims

1. A visual inertial odometry method based on transformation error state, characterized in that: Use the transformed extended Kalman filter to estimate pose information, including: Establish the system continuous-time motion model and measurement equations based on the real-time visual and IMU data; Establishing a linearized error state system based on the system continuous-time motion model and the measurement equation; A linear time-varying transformation is designed, and the linearized error state system is transformed into an error state system whose system state is independent of the system unobservable subspace by using the linear time-varying transformation; the linear time-varying transformation is designed as follows: Where T represents the linear time-varying transformation matrix, which is a non-singular matrix function; p is the position of the IMU in the global coordinate system, v is the velocity of the IMU in the global coordinate system, and l is the landmark position; is the corresponding estimated value; The linear error state system is transformed using the linear time-varying transformation matrix. The transformed linear error state system is: Where, the transformed state transfer Jacobian matrix is g is the gravity vector in the global coordinate system; Represents the error state after transformation The derivative of is the derivative of the linear time-varying transformation matrix; the noise propagation Jacobian matrix G after transformation * =TG; the transformed measurement Jacobian matrix H * =HT -1 , The transformed essential measure Jacobian matrix The state estimation is performed based on the transformed linearized error state system to obtain the estimated pose information.

2. The visual inertial odometry method based on transformation error state according to claim 1, characterized in that: The continuous-time motion model of the system is established as follows: in, represents the derivative of the system state x; R is the posture of the IMU in the global coordinate system; ω m and a m are the gyroscope and accelerometer measurements in the IMU coordinate system; n g and n a are Gaussian white noise with zero mean; g is the gravity vector in the global coordinate system; f is the system kinematic function, [.] × represents the Lie algebra defined on SO(3); The measurement equation is established as follows: y=h( I p L )+∈ Where h represents the transformation function that transforms the point from the IMU coordinate system to the camera coordinate system, I p L represents the position of the landmark relative to the IMU; ∈ is zero-mean Gaussian noise.

3. The visual inertial odometry method based on transformation error state according to claim 2, characterized in that: The linearized error state system is established as follows: Where, Indicates error status The derivative of the error state x represents the true state, represents the state estimate, p is the position of the IMU in the global coordinate system, v is the velocity of the IMU in the global coordinate system, and is the landmark position; is the corresponding estimated value; is the corresponding error; the state transfer Jacobian matrix I3 is the 3×3 identity matrix; the noise propagation Jacobian matrix Represents the measurement residual; the measurement Jacobian matrix H = ΠH e ,in Essential measure Jacobian matrix 4. The visual inertial odometry method based on transformation error state according to claim 3, characterized in that: The performing state estimation based on the transformed linearized error state system to obtain estimated pose information includes: State recursion: At each timestamp t k+1 , once a new IMU measurement is received, the last moment t k The state estimate of is propagated to the current time by numerically integrating the continuous-time motion model; accordingly, the transformed error state covariance is propagated as follows: Among them, the error state transfer matrix Φ * (t k+1 ,t k ) is calculated by the following integro-differential equation: τ represents t k to t k+1 At any moment between Calculated by the following formula: in, E(.) represents mathematical expectation; State update: Get the Kalman state correction in the transformed space and inverse transform the Kalman state correction back to the original space to correct the estimated value; make Represents the Kalman state correction in the transformation space; calculate the Kalman gain K * as follows: in, In the current best state estimate conduct assessments; The transformed error state covariance is updated as: The Kalman state correction is inversely transformed from the transformed space to the original space by the following formula: Among them, δx represents the Kalman state correction in the original space; The state estimate is corrected using the following formula:

5. A visual inertial odometry system based on transformation error state, characterized in that: include: a data acquisition module configured to acquire vision and IMU data; A pose estimation module configured to estimate pose information using a transform extended Kalman filter, comprising an error state construction submodule and a state estimation submodule, wherein the error state construction submodule is configured to establish a system continuous-time motion model and measurement equations based on visual and IMU data acquired in real time; Establishing a linearized error state system based on the continuous-time motion model of the system and the measurement equation; designing a linear time-varying transformation, and utilizing the linear time-varying transformation to transform the linearized error state system into an error state system in which the system state is independent of the system's unobservable subspace; The state estimation submodule is configured to perform state estimation based on the transformed linearized error state system to obtain estimated pose information; wherein the linear time-varying transformation in the error state construction submodule is designed as follows: Where T represents the linear time-varying transformation matrix, which is a non-singular matrix function; The linear error state system is transformed using the linear time-varying transformation matrix. The transformed linear error state system is: Where, the transformed state transfer Jacobian matrix is g is the gravity vector in the global coordinate system; Represents the error state after transformation The derivative of is the derivative of the linear time-varying transformation matrix; the noise propagation Jacobian matrix G after transformation * =TG; the transformed measurement Jacobian matrix H * =HT -1 , The transformed essential measure Jacobian matrix 6. The visual inertial odometry system based on transformation error state according to claim 5, characterized in that: The system continuous time motion model in the error state construction submodule is established as follows: in, represents the derivative of the system state x; R is the posture of the IMU in the global coordinate system; ω m and a m are the gyroscope and accelerometer measurements in the IMU coordinate system; n g and n a are Gaussian white noise with zero mean; g is the gravity vector in the global coordinate system; f is the system kinematic function, [.] × represents the Lie algebra defined on SO(3); The measurement equation is established as follows: y=h( I P L )+∈ Where h represents the transformation function that transforms the point from the IMU coordinate system to the camera coordinate system, I p L represents the position of the landmark relative to the IMU; ∈ is zero-mean Gaussian noise.

7. The visual inertial odometry system based on transformation error state according to claim 6, characterized in that: The linearized error state system in the error state construction submodule is established as follows: Where, Indicates error status The derivative of the error state x represents the true state, represents the state estimate, p is the position of the IMU in the global coordinate system, v is the velocity of the IMU in the global coordinate system, and is the landmark position; is the corresponding estimated value; is the corresponding error; the state transfer Jacobian matrix I3 is the 3×3 identity matrix; the noise propagation Jacobian matrix Represents the measurement residual; the measurement Jacobian matrix H = ΠH e ,in Essential measure Jacobian matrix 8. The visual inertial odometry system based on transformation error state according to claim 7, characterized in that: The state estimation submodule performs state estimation based on the transformed linearized error state system to obtain estimated pose information, including: State recursion: At each timestamp t k+1 , once a new IMU measurement is received, the last moment t k The state estimate of is propagated to the current time by numerically integrating the continuous-time motion model; accordingly, the transformed error state covariance is propagated as follows: Among them, the error state transfer matrix Φ * (t k+1 ,t k ) is calculated by the following integro-differential equation: τ represents t k to t k+1 At any moment between Calculated by the following formula: in, E(.) represents mathematical expectation; State update: Get the Kalman state correction in the transformed space and inverse transform the Kalman state correction back to the original space to correct the estimated value; make Represents the Kalman state correction in the transformation space; calculate the Kalman gain K * as follows: in, In the current best state estimate conduct assessments; The transformed error state covariance is updated as: The Kalman state correction is inversely transformed from the transformed space to the original space by the following formula: Among them, δx represents the Kalman state correction in the original space; The state estimate is corrected using the following formula:

Images