Visual inertial odometer method and system based on transformation error state

Abstract

The invention discloses a visual inertial odometer method and system based on a transformation error state, and relates to the technical field of visual inertial odometer optimization. The method is characterized by comprising the following steps: estimating pose information by using a transformation extended Kalman filter: establishing a system continuous time motion model and a measurement equation according to vision and IMU data acquired in real time; establishing a linearization error state system based on a system continuous time motion model and a measurement equation; linear time-varying transformation is designed, and the linear error state system is transformed into an error state system with the system state irrelevant to the unobservable subspace of the system through the linear time-varying transformation; and performing state estimation based on the transformed linearization error state system to obtain estimated pose information. The method based on transformation solves the problem of inconsistency in VINS, relieves the observability mismatching problem, and ensures that the visual inertial odometer applying the transformation extended Kalman filter has a consistent estimation result.

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 System (VINS) has been widely used in robotics, virtual reality and augmented reality due to its compact size and low cost. By leveraging the complementary characteristics of Inertial Measurement Unit (IMU) and camera in navigation and localization tasks, VINS can provide high-precision position information.

[0003] VINS can be divided into two main approaches: optimization-based and filter-based. Optimization-based approaches achieve high localization accuracy through nonlinear optimization, while filter-based approaches prioritize computational efficiency. It is worth noting that filter-based approaches have made significant progress recently, and their accuracy levels are close to those of optimization-based approaches.

[0004] Classical filter-based VINS estimators, such as the multiplicative extended Kalman filter (M-EKF), also known as the error-state Kalman filter (ESKF), adopt 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. Therefore, the ESKF obtains false information about this erroneously observable state, resulting in inconsistency. Summary of the invention

[0005] In view of the above problems, the present invention proposes a visual inertial odometer based on transformation error state, which estimates motion information by using transformation extended Kalman filter.

[0006] According to one aspect of the present invention, a visual inertial odometer method based on a transformation error state is proposed. The method estimates pose information by using a transformation extended Kalman filter, comprising:

[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] 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 whose system state is independent of the system 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] Further, the linearization error state system is established as follows:

[0018]

[0019] In the formula, 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 The essential measure Jacobian matrix

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

[0021]

[0022] In the formula, 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, and the transformed linear error state system is:

[0024]

[0025] In the formula, 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 transformed noise propagation Jacobian matrix G * =TG; the transformed measurement Jacobian matrix H * =HT -1 , The transformed essential measure Jacobian matrix

[0026] Further, 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 time 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 k+1 At any time 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; calculates 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 by the following formula:

[0042]

[0043] According to another aspect of the present invention, a visual inertial odometer 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] In the formula, 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 The essential measure Jacobian matrix

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

[0056]

[0057] In the formula, 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, and the transformed linear error state system is:

[0059]

[0060] In the formula, 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 transformed noise propagation Jacobian matrix G * =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 time 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 k+1 At any time 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; calculates 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 by 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. It is proved by analysis 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 The present invention is a flowchart of a visual inertial odometer method based on a transformation error state according to an embodiment of the present invention.

[0082] Figure 2 Schematic diagram of the transformation of an original system in which the unobservable subspace depends on the state into a system in which the 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 It is the simulation 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 RMSE of direction and position 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 are shown as an example; 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 scheme of the present invention, exemplary implementations or embodiments of the present invention will be described below in conjunction with the accompanying drawings. Obviously, the described implementations or embodiments are only implementations or embodiments of a part of the present invention, not all of them. Based on the implementations or embodiments of the present invention, all other implementations or embodiments obtained by ordinary technicians in the field without creative work should fall within the scope of protection of the present invention.

[0088] The present invention proposes a visual inertial odometer method and system based on transformed error states, wherein a novel method is proposed to solve the inconsistency caused by observability mismatch in VINS, and 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 the change of the linearization point. A transformed extended Kalman filter (T-ESKF) is further proposed, 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 in both accuracy and consistency compared to ESKF.

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

[0090] S1. Establish the system continuous time motion model and measurement equations based on the real-time acquired 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, 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 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 posture 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 the 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 a single landmark, for simplicity, only the case of one 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, it obtains visual measurements of landmarks, and 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 a zero-mean Gaussian noise that satisfies the mathematical expectation

[0107] 2. Linearized error state system

[0108] set up represents the state estimation, 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 ESKF, the error state for the direction is defined using a logarithmic mapping on SO(3), while the errors for the other variables are defined in 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] By linearizing equations (3) and (4) at the current state estimate, the linearized error state system is obtained as follows:

[0116]

[0117] where the state transfer Jacobian matrix is

[0118]

[0119] The noise propagation Jacobian matrix is

[0120]

[0121] The measurement 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 the 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 disappear 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 that 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 invariant 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] The inconsistency in VINS is addressed by transforming the linearized error state system into a state-independent unobservable subspace. First, the transformed linearized error state system is derived. Subsequently, in order to prevent the observability from being affected by the change of the linearization point, a transformation is designed such that the unobservable subspace of the transformed system is state-independent.

[0138] The linear time-varying transformation is expressed as where T(·) is a 12×12 non-singular matrix function that needs to be selected. The transformed error state 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), we can obtain the transformed linearized error state system as follows:

[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 * This is done by replacing (F,H e ) with The unobservable subspace M of the transformation system * N * =0 is:

[0153]

[0154] The above equation 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 time 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 by the following formula:

[0162]

[0163] Where τ represents t k to 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 estimate.

[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. Since the Jacobian matrix Φ in T-ESKF * and H * 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 erroneous reduction of unobservable dimensions is avoided and T-ESKF has the correct observability. Therefore, T-ESKF does not suffer from the problem of inconsistent estimates caused by mismatched observability.

[0179] The effectiveness of the present invention is further verified by 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 Numeric parameter Numeric Accelerometer Measurement Noise 2.00e-03 Gyro 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 indicators are used to evaluate the performance: Root Mean Square Error (RMSE) for trajectory accuracy evaluation and Normalized Estimation Error Square (NEES) for consistency evaluation. Smaller RMSE indicates better accuracy, and NEES values ​​closer to 1 indicate better consistency.

[0184] To evaluate the accuracy, Figure 4 T-ESKF is tested on the three trajectories shown in Figure 2, and 100 Monte Carlo simulations are performed for each trajectory. The average RMSE produced by the 100 simulations is reported in Table 2. As can be seen in Table 2, T-ESKF shows a significant accuracy advantage over ESKF.

[0185] Table 2. Average angle (degrees) and position (meters) root mean square error (RMSE) of 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] In order 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 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 are consistent with those listed in Table 1. The NEES results of 1000 Monte Carlo experiments are 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 that its consistency level is higher than ESKF. The bottom three subplots show the NEES frequency distribution of 1000 trials throughout the simulation. The NEES histogram of T-ESKF is very consistent with the theoretical chi-square distribution, further indicating that it has good consistency.

[0189] The present invention proposes a novel method to solve the VINS inconsistency problem caused by observability mismatch. A linear time-varying transformation is designed and applied to the linearized error state system of ESKF, so that the unobservable subspace states of the transformed system are independent. Through this transformation, the observability of the system remains unchanged to the changes of the linearization points, 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 the estimator based on Lie groups.

[0190] Another embodiment of the present invention provides a visual inertial odometer 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 building submodule is established as follows:

[0200]

[0201] In the formula, 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 The 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] In the formula, 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, and the transformed linear error state system is:

[0206]

[0207] In the formula, 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 transformed noise propagation Jacobian matrix G * =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 time 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 k+1 At any time 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; calculates 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 by the following formula:

[0224]

[0225] The function 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 according to a limited number of embodiments, it will be apparent to those skilled in the art, with the benefit of the above description, that other embodiments are contemplated within the scope of the invention thus described. The disclosure of the present invention 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; 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 whose system state is independent of the system unobservable subspace; The state estimation is performed based on the transformed linearized error state system to obtain the estimated pose information.

2. A visual inertial odometer 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 odometer method based on transformation error state according to claim 2, characterized in that: The linearization error state system is established as follows: In the formula, 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 l 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 The essential measure Jacobian matrix 4. The visual inertial odometer method based on transformation error state according to claim 3, characterized in that: The linear time-varying transformation is designed as follows: In the formula, 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, and the transformed linear error state system is: In the formula, 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 transformed noise propagation Jacobian matrix G * =TG; the transformed measurement Jacobian matrix H * =HT -1 , The transformed essential measure Jacobian matrix 5. The visual inertial odometer method based on transformation error state according to claim 4, characterized in that: The performing state estimation based on the transformed linearized error state system to obtain estimated pose information comprises: State recursion: At each timestamp t k+1 Once a new IMU measurement is received, the last time 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 k+1 At any time 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; calculates 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 by the following formula:

6. A visual inertial odometry system based on a transformation error state, characterized in that: include: a data acquisition module configured to acquire vision and IMU data; A posture estimation module, which is configured to estimate posture information using a transform 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 visual and IMU data acquired in real time; Establish a linearized error state system based on the continuous-time motion model of the system and the measurement equation; 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 whose 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 posture information.

7. The visual inertial odometer system based on transformation error state according to claim 6, 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.

8. The visual inertial odometer system based on transformation error state according to claim 7, characterized in that: The linearized error state system in the error state construction submodule is established as follows: In the formula, 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 The essential measure Jacobian matrix 9. A visual inertial odometer system based on transformation error state according to claim 8, characterized in that: The linear time-varying transformation in the error state construction submodule is designed as follows: In the formula, 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, and the transformed linear error state system is: In the formula, 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 transformed noise propagation Jacobian matrix G * =TG; the transformed measurement Jacobian matrix H * =HT -1 , The transformed essential measure Jacobian matrix 10. A visual inertial odometer system based on transformation error state according to claim 9, 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 time 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 k+1 At any time 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; calculates 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 by the following formula:

Images