CN116086445A - Multi-source information time delay fusion navigation method based on factor graph optimization - Google Patents

Abstract

The invention provides a multi-source information time delay fusion navigation method based on factor graph optimization, and belongs to the fields of integrated navigation and multi-source information fusion. According to the method, based on a factor graph optimization framework, variable nodes are generated at a specific frequency, IMU pre-integration is carried out, when effective but time-delay measurement is received, historical state variables are matched according to sampling time of the variable nodes, corresponding factor nodes are added, asynchronous updating is carried out on an inertial navigation mechanical arrangement result according to an increment smoothing algorithm of a Bayesian tree structure, inertial navigation errors are effectively corrected, and a robust and accurate positioning result is output.

Description

Multi-source information time delay fusion navigation method based on factor graph optimization

Technical Field

The invention relates to a multi-source information fusion navigation method based on factor graph optimization and with time delay of a plurality of measurement information, belonging to the fields of combined navigation and multi-source information fusion.

Background

In order to realize high-precision adaptive intelligent navigation under various complex environments of unmanned systems, the multi-source fusion navigation system can effectively synthesize complementary and redundant measurement information from a plurality of sensors and provide more robust and accurate system state estimation compared with a single sensor. In a multi-source navigation system, an Inertial Navigation System (INS) and a global satellite navigation system (GNSS) are widely used due to strong complementary advantages, but in complex scenes such as urban canyons, indoor and underground, satellite signals cannot be positioned effectively due to interference, shielding and the like, and at the moment, accumulated errors of INS can be corrected by other navigation sources such as an odometer (ODO), a laser radar (LiDAR) and the like, so that long-term high-precision navigation is realized. Although the multiple sensors can give descriptions of system states from different levels, their measurement is generally independent, and different measurement characteristics, data processing methods, applicable environments and the like will cause the output timings of the sensors to be independent and have measurement delays, and time asynchronous errors caused by the measurement delays will negatively affect navigation results.

Aiming at the problem of multi-source measurement time delay, in order to ensure the real-time performance of the system, a commonly used Kalman filtering frame mostly adopts a time registration method, delay data is calculated to the current fusion time through means such as fitting, compensation and the like, and equivalent measurement information is constructed to participate in fusion, but the method actually introduces estimation errors, is one-sided consideration of actual conditions, and does not completely and effectively realize information utilization. The method based on factor graph optimization adopts a factor graph model to describe the problem of multi-source information fusion, and utilizes a Bei Sheshu structure to judge the state variable influenced by a new measurement factor, and trace and fuse information of different measurement moments in real time at the current moment, so that the full use of hysteresis information is realized.

In the existing method for optimizing and solving the measurement time delay by using the factor graph, except for the INS, only the condition that a GNSS navigation source exists is considered, when the GNSS time delay is overlong or even refused, the real-time precision of a navigation result is seriously reduced, the dispersion of the INS can be effectively restrained by introducing common and easy-to-use ODO measurement, but the ODO is used as an independent sensor, the time delay problem is also introduced while the navigation precision is improved, and therefore, the fusion method for researching the condition that the multi-source measurement has the time delay has practical value.

Disclosure of Invention

Aiming at the defects of the prior art, the invention provides a multi-source information time delay fusion navigation method based on factor graph optimization, which reduces the longest time delay existing in a system by using redundant information of a plurality of sensors under the condition of effectively processing the time delay existing at the same time of a plurality of measurement sources, realizes the combined compensation of measurement delay and further improves the real-time precision of the navigation system.

The invention provides a multi-source information time delay fusion navigation method based on factor graph optimization, which comprises the following steps:

step 1, solving a problem according to a factor graph model, maximum posterior probability inference and state estimation in nonlinear optimization theory modeling navigation;

step 2, constructing a navigation system state variable node and a sensor measurement factor node;

step 3, generating state variables to be optimized in an increment mode according to time sequence and adding pre-integration factors;

step 4, matching historical state variables according to the ODO and GNSS measurement time delay and adding corresponding factor nodes;

and 5, utilizing an incremental smoothing solver to fuse effective measurement in real time to realize asynchronous updating of the navigation result.

As a further improvement of the present invention, the factor graph is a bipartite graph model g= (F, X, F) representing factorization of global multivariate functions; the system consists of three elements, namely a factor node set F, a variable node set X and an undirected edge set E for connecting two types of nodes; the step 1 specifically comprises the following steps:

a. the factor graph is defined as factoring the function f (X):

factor node f _i E F-factored local function, variable node

Refers to a variable set in a local function, and when a variable node is related to a factor node, an undirected edge e is generated between the variable node and the factor node _ij E connection;

b. solving a multisource information fusion problem in navigation by using maximum posterior estimation; given the start t ₀ From moment to current t _k Time observation data Z _k Accurately estimating system state X _k ：

P(X _k |Z _k ) Is a posterior probability, P (X) _k ) Is a priori probability, P (Z _k |X _k ) Is a likelihood probability; because the measurement of each sensor at different moments is irrelevant, according to the Bayesian rule, the maximum posterior estimation can be factorized, and then a factor graph model is introduced;

c. converting the maximum posterior estimation into a least square problem, and solving by adopting nonlinear optimization; by factor node

Representing the measurement of each sensor, assuming that the sensor measurement noise conforms to the gaussian distribution, taking the negative logarithmic equivalent of the maximum a posteriori estimate is a least squares problem:

the err function, which in principle represents the error, is derived from the metrology model,

representing the square of the mahalanobis distance, Σ representing the covariance matrix, and solving the nonlinear optimization problem using a bayesian tree incremental update algorithm of the GTSAM library.

As a further improvement of the present invention, the step 2 specifically includes:

a. the state quantity in the multisource fusion navigation system consists of gesture, speed, position and inertial device error, and variable nodes in a factor graph are defined as:

φ ^T ＝[φ _x φ _y φ _z ]representing pitch, roll and heading three-axis attitude angles; v ^T ＝[v _x v _y v _z ]Representing eastThe speed of the north direction and the sky direction; p is p ^T ＝[λLh]Representing the latitude, longitude and altitude of the user,

and->

The three-axis zero offset error vectors of the gyroscope and the accelerometer are respectively;

the IMU sensor uses a pre-integration algorithm to equivalent IMU measured values in a period of time to a factor node to construct increment constraint between two variable nodes, and the increment constraint is a binary factor; from t _k-1 To t _k The IMU factor node expression at the moment is:

from t _k-1 To t _k IMU measurement data at moment, h ^IMU An IMU measurement equation; />

Is a cost function;

GNSS provides absolute position information, which is a unitary factor, t _k The GNSS factor node expression for time is:

is the GNSS at t _k Carrier position measurement data h provided at the moment ^GNSS Is a measurement equation thereof;

d, ODO outputs carrier forward speed information by counting pulse in specific time, which is a unitary factor, t _k The ODO factor node expression at the moment is:

is ODO at t _k Carrier speed measurement data provided at time, h ^ODO Is a measurement equation thereof;

the construction of various factor nodes shows that the factor graph framework can be flexibly encoded according to a measurement model of the sensor, and the flexibility of processing the multi-source measurement delay problem is embodied according to the accurate matching of the state variables of the time sequence.

As a further improvement of the present invention, the step 3 specifically includes:

in a general factor graph optimization framework, a state variable to be optimized is generated at a fixed frequency along with the time increment, when no effective quantity except an IMU is received, the state variable is frequently generated and nonlinear optimization is performed, so that the calculation force and the resources are wasted, and the following steps are adopted to improve the optimization efficiency:

a. determining the highest measurement frequency of the sensor except IMU and generating state variables in the order of the frequencies

b. Adding IMU pre-integral factor nodes between adjacent variable nodes generated in the step a, measuring time delay when the measurement occurs,

variable nodes can be more accurately matched to past moments.

As a further improvement of the present invention, the step 4 specifically includes:

x _k at t _k The variable node of the moment generates a period T ^S Sampling period T equal to ODO ^O =100 ms, according to the measured result of the related literature, the GNSS receiver delay dt ^G Several hundred milliseconds, ODO delay dt ^O Less than one hundred milliseconds; o (O)The steps of matching and adding the DO and GNSS measurement delay factors are as follows:

a.t ₁ time of day, pair x ₁ Adding a priori factors

b.t ₁ To t ₂ At moment, reading IMU measurement data of the time period to perform pre-integration;

c.t ₂ time of day, pair x ₁ And x ₂ Adding a pre-integration factor

For x ₁ The adding time delay is less than T ^S Is>

d.t ₂ To t ₃ At moment, reading IMU measurement data of the time period to perform pre-integration;

e.t ₃ time of day, pair x ₂ And x ₃ Adding a pre-integration factor

For x ₂ The adding time delay is less than T ^S ODO factor->

For x ₁ The adding time delay is greater than T ^S But less than 2T ^S GNSS factor of->

t ₃ For the current time, since the time delay span of GNSS is greater than 1T ^S ，t ₁ And t ₂ GNSS factors are not added at any time until t ₃ Added at the moment

The factors are constrained, and the intermediate time can only depend on IMU factorsForming an increment constraint, and analyzing ODO measurement time delay; as can be seen from the addition of the above-mentioned existing delay measurement factor nodes, when there is no measurement other than IMU in the system, navigation solution is INS prediction result, if the maximum measurement delay in the system is too long, it will cause serious degradation of real-time accuracy, in the above-mentioned analysis, it is only assumed that the GNSS receiver is normally used that there is delay, when the GNSS signal is interfered or the system is solved too long, the GNSS delay can reach several seconds; after ODO measurement is introduced, real-time constraint on the system is increased, even if ODO has time delay, the ODO can be conveniently processed, and the maximum time delay existing in the system is reduced, so that the purposes of improving the real-time precision and the robustness of the navigation result are achieved.

As a further improvement of the present invention, the step 5 specifically includes:

step 1-4, the factor graph model construction and theoretical solution of the delay navigation state estimation problem when the multisource information exists are completed, and step 5, the specific solution is carried out by utilizing a GTSAM (gateway transfer request) optimization library; the GTSAM is a C++ library which uses a factor graph and a Bayesian network as a calculation mode to realize smoothing and graph construction and is used for solving the state estimation problem in the fields of robots and vision; the method comprises the following specific steps:

a. initial system state variable x ₁ And add a priori factor node

b. Setting IMU zero bias, random walk, weight acceleration and other pre-integration related parameters according to a noise model, setting GNSS position measurement noise and setting ODO speed measurement noise;

c. setting parameters such as an information matrix decomposition method of the ISAM2, a re-linearization step number and the like, and instantiating an ISAM2 solver object;

d. creating a new factor graph and node variables;

e. generating a state variable in a specific period, matching according to the measurement result, and adding factor nodes;

f. pushing a new factor graph and node variables into the ISAM2 solver to realize asynchronous updating, resetting the factor graph and the node variables, and repeating the step e.

The beneficial effects of the invention are as follows: the problem of time delay fusion is solved by adopting a factor graph optimization method, the problems of estimation errors and structure adjustment in a filtering method are eliminated, the problem of navigation result accuracy reduction due to overlong single GNSS time delay or refusal is solved by introducing ODO redundancy measurement in the factor graph optimization, the asynchronous fusion of multi-source time delay is realized by utilizing the characteristic that the factor graph can process measurement of a plurality of information sources at different moments at the same information moment, INS errors are effectively corrected, and a robust and accurate positioning result is output.

Drawings

Fig. 1 is a system block diagram of a multi-source information time delay fusion navigation method based on factor graph optimization.

FIG. 2 is a graph of the matching and addition of GNSS and ODO delay factor node history states.

Fig. 3 is a simulation diagram of the motion trail of the carrier.

FIG. 4 is a comparison graph of position errors for a filtering and factor graph optimization process for multi-source measurement delay cases, wherein the upper graph is the east position error, the middle graph is the north position error, and the lower graph is the sky position error.

Detailed Description

The following detailed description, taken in conjunction with the accompanying drawings, is given in an illustrative view:

as shown in fig. 1, the sensor for the multi-source information navigation system according to the present invention includes: inertial Measurement Unit (IMU), GNSS receiver, ODO speedometer. According to the original data provided by the sensor, the navigation solution is generated at high frequency by adopting an INS mechanical arrangement algorithm, the method for correcting the navigation solution is asynchronously and efficiently fed back by a factor graph increment smoothing algorithm, and finally a robust and accurate navigation result is output.

And step 1, solving a problem according to a factor graph model, maximum posterior probability inference and state estimation in nonlinear optimization theory modeling navigation. The factor graph is a bipartite graph model g= (F, X, E) representing the factorization of the global multivariate function. The method consists of three elements, namely a factor node set F, a variable node set X and an undirected edge set E for connecting two types of nodes.

a. The factor graph is defined as factoring the function f (X):

factor node f _i E F-factored local function, variable node

Refers to a variable set in a local function, and when a variable node is related to a factor node, an undirected edge e is generated between the variable node and the factor node _ij E connection. />

b. And solving the multisource information fusion problem in navigation by using maximum posterior estimation. Given the start t ₀ From moment to current t _k Time observation data Z _k Accurately estimating system state X _k ：

P(X _k |Z _k ) Is a posterior probability, P (X) _k ) Is a priori probability, P (Z _k |X _k ) Is a likelihood probability. Because the measurement of each sensor at different moments is irrelevant, according to the Bayesian rule, the maximum posterior estimation can be factorized, and then a factor graph model is introduced.

c. Converting the maximum posterior estimation into a least square problem, and solving by adopting nonlinear optimization. By factor node

Representing the measurement of each sensor, assuming that the sensor measurement noise conforms to the gaussian distribution, taking the negative logarithmic equivalent of the maximum a posteriori estimate is a least squares problem:

the err function, which in principle represents the error, is derived from the metrology model,

representing the square of the mahalanobis distance, Σ representing the covariance matrix, and solving the nonlinear optimization problem using a bayesian tree incremental update algorithm of the GTSAM library.

Step 2, constructing a navigation system state variable node and a sensor measurement factor node, wherein the specific steps are as follows:

a. the state quantity in the multisource fusion navigation system is generally composed of gesture, speed, position and inertial device error, and the variable nodes in the factor graph are defined as:

φ ^T ＝[φ _x φ _y φ _z ]representing pitch, roll and heading three-axis attitude angles; v ^T ＝[v _x v _y v _z ]Indicating the east, north and sky speeds; p is p ^T ＝[λLh]Representing the latitude, longitude and altitude of the user,

and->

And the three-axis zero offset error vectors of the gyroscope and the accelerometer are respectively.

And b, the IMU sensor uses a pre-integration algorithm to equivalent IMU measured values in a period of time to a factor node to construct increment constraint between two variable nodes, and the increment constraint is a binary factor. From t _k-1 To t _k The IMU factor node expression at the moment is:

from t _k-1 To t _k IMU of moment of timeMeasurement data, h ^IMU Is an IMU measurement equation. />

Is a cost function.

GNSS provides absolute position information, which is a unitary factor, t _k The GNSS factor node expression for time is:

is the GNSS at t _k Carrier position measurement data h provided at the moment ^GNSS Is a measurement equation thereof.

d, ODO outputs carrier forward speed information by counting pulse in specific time, which is a unitary factor, t _k The ODO factor node expression at the moment is:

is ODO at t _k Carrier speed measurement data provided at time, h ^ODO Is a measurement equation thereof.

The construction of various factor nodes shows that the factor graph framework can be flexibly encoded according to a measurement model of the sensor, and the flexibility of processing the multi-source measurement delay problem is embodied according to the accurate matching of the state variables of the time sequence.

Step 3 generating state variables to be optimized in time sequence increment and adding pre-integral factors

In a general factor graph optimization framework, a state variable to be optimized is generated at a fixed frequency along with the time increment, when no effective quantity except an IMU is received, the state variable is frequently generated and nonlinear optimization is performed, so that the calculation force and the resources are wasted, and the following steps are adopted to improve the optimization efficiency:

a. determining the highest measurement frequency of the sensor except IMU and generating state variables in the order of the frequencies

b. Adding IMU pre-integral factor nodes between adjacent variable nodes generated in the step a, measuring time delay when the measurement occurs,

variable nodes can be more accurately matched to past moments.

Step 4, matching the historical state variables according to the ODO and GNSS measurement time delays and adding corresponding factor nodes

As shown in fig. 2, x _k At t _k The variable node of the moment generates a period T ^S Sampling period T equal to ODO ^O =100 ms, according to the measured result of the related literature, the GNSS receiver delay dt ^G Typically hundreds of milliseconds, ODO latency dt ^O Typically less than one hundred milliseconds. Taking the previous 3 moments as an example, the matching and adding steps of the ODO and GNSS measurement delay factors are described:

a.t ₁ time of day, pair x ₁ Adding a priori factors

b.t ₁ To t ₂ And at the moment, reading IMU measurement data of the time period to perform pre-integration.

c.t ₂ Time of day, pair x ₁ And x ₂ Adding a pre-integration factor

For x ₁ The adding time delay is less than T ^S Is the odometer factor of (a)

d.t ₂ To t ₃ And at the moment, reading IMU measurement data of the time period to perform pre-integration.

e.t ₃ Time of day, pair x ₂ And x ₃ Adding a pre-integration factor

For x ₂ The adding time delay is less than T ^S ODO factor->

For x ₁ The adding time delay is greater than T ^S But less than 2T ^S GNSS factor of->

In FIG. 2, t ₃ For the current time, the dotted line represents a newly added factor node at the current time, and the time delay span of the GNSS is larger than 1T ^S ，t ₁ And t ₂ GNSS factors are not added at any time until t ₃ Added at the moment

The factors are constrained, the intermediate moment can only depend on the IMU factors to form incremental constraint, and the analysis of ODO measurement time delay is the same. As can be seen from the addition of the above-mentioned existing delay measurement factor nodes, when there is no measurement other than IMU in the system, the navigation solution is the INS prediction result, if the maximum measurement delay in the system is too long, the real-time accuracy will be seriously degraded, in the above-mentioned analysis, only the delay existing when the GNSS receiver is normally used is assumed, and when the GNSS signal is interfered or the system is solved too long, the delay of the GNSS can reach several seconds. After ODO measurement is introduced, real-time constraint on the system is increased, even if ODO has time delay, the ODO can be conveniently processed, and the maximum time delay existing in the system is reduced, so that the purposes of improving the real-time precision and the robustness of the navigation result are achieved.

Step 5, realizing asynchronous update of navigation results by fusing effective measurement in real time by using incremental smoothing solver

And step 1-4, constructing a factor graph model of a delay navigation state estimation problem when the multisource information exists, solving the factor graph model and the theory, and step 5, carrying out specific solving by utilizing a GTSAM (gateway transfer request message) optimization library. The GTSAM is a C++ library which uses a factor graph and a Bayesian network as a calculation mode to realize smoothing and graph construction and is commonly used for solving the state estimation problem in the fields of robots and vision.

The method comprises the following specific steps:

a. initial system state variable x ₁ And add a priori factor node

b. Setting IMU zero bias, random walk, weight acceleration and other pre-integration related parameters according to a noise model, setting GNSS position measurement noise and setting ODO speed measurement noise

c. Setting parameters such as an information matrix decomposition method of the ISAM2, a re-linearization step number and the like, and instantiating an ISAM2 solver object.

d. Creating new factor graphs and node variables

e. Generating state variables in a specific period, matching and adding factor nodes according to measurement results

f. Pushing new factor graph and node variable into ISAM2 solver to realize asynchronous update, resetting factor graph and node variable, repeating step e

Finally, the effectiveness of the method provided by the invention is verified through a simulation experiment:

as shown in FIG. 3, the carrier motion trail with the GNSS-INS-SIM open source library simulation duration of 110s is adopted, and the carrier reaches the end point after being maneuvered such as acceleration, climbing, continuous large turning, deceleration and the like from the place with the initial position marked by an asterisk of 32 degrees, 120 degrees of longitude and 0m of height. All sensors adopt low cost consumption level, IMU sampling time interval is set to be 0.01s, gyro zero offset instability is 50 degrees/h, and angle random walk is realized

Accelerometer zero bias instability 60ug, velocity random walk +.>

Setting GNSS sampling time interval 1s and time delay1s, weft height measurement standard deviations of 1m,1m and 3m; an ODO sampling time interval of 0.1s and a time delay of 50ms are set, and the standard deviation of triaxial speed measurement is 0.5m/s. The time delay problem is processed by adopting a Kalman filtering INS estimation and factor graph method respectively.

As shown in fig. 4, normal is a situation that the system has no GNSS and ODO delay, and can be used as a reference standard. KF is the condition of real-time measurement and update after the INS is used for compensating the time delay GNSS and ODO data, FG is the condition of asynchronous update by using factor graph optimization and matching with the time delay GNSS and OOD factors. The east, north and sky root mean square error RMSE adopting the factor graph optimization method is almost identical to the condition without time delay, so that the problem of multi-source measurement of asynchronous time delay is well solved, and the real-time and long-term accuracy of the navigation system is improved.

Claims (6)

1. The multi-source information time delay fusion navigation method based on factor graph optimization is characterized by comprising the following steps of:

step 1, solving a problem according to a factor graph model, maximum posterior probability inference and state estimation in nonlinear optimization theory modeling navigation;

step 2, constructing a navigation system state variable node and a sensor measurement factor node;

step 3, generating state variables to be optimized in an increment mode according to time sequence and adding pre-integration factors;

step 4, matching historical state variables according to the ODO and GNSS measurement time delay and adding corresponding factor nodes;

and 5, utilizing an incremental smoothing solver to fuse effective measurement in real time to realize asynchronous updating of the navigation result.

2. The multi-source information time delay fusion navigation method based on factor graph optimization of claim 1, wherein the factor graph is a bipartite graph model g= (F, X, E) representing factorization of global multivariate functions; the system consists of three elements, namely a factor node set F, a variable node set X and an undirected edge set E for connecting two types of nodes; the step 1 specifically comprises the following steps:

a. the factor graph is defined as factoring the function f (X):

factor node f _i E F-factored local function, variable node

Refers to a variable set in a local function, and when a variable node is related to a factor node, an undirected edge e is generated between the variable node and the factor node _ij E connection;

b. solving a multisource information fusion problem in navigation by using maximum posterior estimation; given the start t ₀ From moment to current t _k Time observation data Z _k Accurately estimating system state X _k ：

P(X _k |Z _k ) Is a posterior probability, P (X) _k ) Is a priori probability, P (Z _k |X _k ) Is a likelihood probability; because the measurement of each sensor at different moments is irrelevant, according to the Bayesian rule, the maximum posterior estimation can be factorized, and then a factor graph model is introduced;

c. converting the maximum posterior estimation into a least square problem, and solving by adopting nonlinear optimization; by factor node

Representing the measurement of each sensor, assuming that the sensor measurement noise conforms to the gaussian distribution, taking the negative logarithmic equivalent of the maximum a posteriori estimate is a least squares problem:

the err function, which in principle represents the error, is derived from the metrology model,

representing the square of the mahalanobis distance, Σ representing the covariance matrix, and solving the nonlinear optimization problem using a bayesian tree incremental update algorithm of the GTSAM library.

3. The method for multi-source information time delay fusion navigation based on factor graph optimization according to claim 1, wherein the step 2 is specifically:

a. the state quantity in the multisource fusion navigation system consists of gesture, speed, position and inertial device error, and variable nodes in a factor graph are defined as:

φ ^T ＝[φ _x φ _y φ _z ]representing pitch, roll and heading three-axis attitude angles; v ^T ＝[v _x v _y v _z ]Indicating the east, north and sky speeds; p is p ^T ＝[λLh]Representing the latitude, longitude and altitude of the user,

and->

The three-axis zero offset error vectors of the gyroscope and the accelerometer are respectively;

the IMU sensor uses a pre-integration algorithm to equivalent IMU measured values in a period of time to a factor node to construct increment constraint between two variable nodes, and the increment constraint is a binary factor; from t _k-1 To t _k The IMU factor node expression at the moment is:

from t _k-1 To t _k IMU measurement data at moment, h ^IMU An IMU measurement equation; />

Is a cost function;

GNSS provides absolute position information, which is a unitary factor, t _k The GNSS factor node expression for time is:

is the GNSS at t _k Carrier position measurement data h provided at the moment ^GNSS Is a measurement equation thereof;

d, ODO outputs carrier forward speed information by counting pulse in specific time, which is a unitary factor, t _k The ODO factor node expression at the moment is:

is ODO at t _k Carrier speed measurement data provided at time, h ^ODO Is a measurement equation thereof;

the construction of various factor nodes shows that the factor graph framework can be flexibly encoded according to a measurement model of the sensor, and the flexibility of processing the multi-source measurement delay problem is embodied according to the accurate matching of the state variables of the time sequence.

4. The method for multi-source information time delay fusion navigation based on factor graph optimization according to claim 1, wherein the step 3 is specifically:

in a general factor graph optimization framework, a state variable to be optimized is generated at a fixed frequency along with the time increment, when no effective quantity except an IMU is received, the state variable is frequently generated and nonlinear optimization is performed, so that the calculation force and the resources are wasted, and the following steps are adopted to improve the optimization efficiency:

a. determining the highest measurement frequency of the sensor except IMU and generating state variables in the order of the frequencies

b. And c, adding IMU pre-integral factor nodes between adjacent variable nodes generated in the step a, and measuring time delay when the measurement occurs, so that the variable nodes at the past moment can be accurately matched.

5. The method for multi-source information time delay fusion navigation based on factor graph optimization according to claim 1, wherein the step 4 is specifically:

x _k at t _k The variable node of the moment generates a period T ^S Sampling period T equal to ODO ^O =100 ms, according to the measured result of the related literature, the GNSS receiver delay dt ^G Several hundred milliseconds, ODO delay dt ^O Less than one hundred milliseconds; the matching and adding steps of the ODO and GNSS measurement delay factors are as follows:

a.t ₁ time of day, pair x ₁ Adding a priori factors

b.t ₁ To t ₂ At moment, reading IMU measurement data of the time period to perform pre-integration;

c.t ₂ time of day, pair x ₁ And x ₂ Adding a pre-integration factor

For x ₁ The adding time delay is less than T ^S Is>

d.t ₂ To t ₃ At moment, reading IMU measurement data of the time period to perform pre-integration;

e.t ₃ time of day, pair x ₂ And x ₃ Adding a pre-integration factor

For x ₂ The adding time delay is less than T ^S ODO factor->

For x ₁ The adding time delay is greater than T ^S But less than 2T ^S GNSS factor of->

t ₃ For the current time, since the time delay span of GNSS is greater than 1T ^S ，t ₁ And t ₂ GNSS factors are not added at any time until t ₃ Added at the moment

The factors are constrained, the intermediate moment can only depend on the IMU factors to form incremental constraint, and the analysis of ODO measurement time delay is the same; as can be seen from the addition of the above-mentioned existing delay measurement factor nodes, when there is no measurement other than IMU in the system, navigation solution is INS prediction result, if the maximum measurement delay in the system is too long, it will cause serious degradation of real-time accuracy, in the above-mentioned analysis, it is only assumed that the GNSS receiver is normally used that there is delay, when the GNSS signal is interfered or the system is solved too long, the GNSS delay can reach several seconds; introduction ofAfter ODO measurement, real-time constraint on the system is increased, even if ODO has time delay, the ODO can be conveniently processed, and the maximum time delay in the system is reduced, so that the purposes of improving the real-time precision and the robustness of the navigation result are achieved. />

6. The method for multi-source information time delay fusion navigation based on factor graph optimization according to claim 1, wherein the step 5 is specifically:

step 1-4, the factor graph model construction and theoretical solution of the delay navigation state estimation problem when the multisource information exists are completed, and step 5, the specific solution is carried out by utilizing a GTSAM (gateway transfer request) optimization library; the GTSAM is a C++ library which uses a factor graph and a Bayesian network as a calculation mode to realize smoothing and graph construction and is used for solving the state estimation problem in the fields of robots and vision; the method comprises the following specific steps:

a. initial system state variable x ₁ And add a priori factor node

b. Setting IMU zero bias, random walk, weight acceleration and other pre-integration related parameters according to a noise model, setting GNSS position measurement noise and setting ODO speed measurement noise;

c. setting parameters such as an information matrix decomposition method of the ISAM2, a re-linearization step number and the like, and instantiating an ISAM2 solver object;

d. creating a new factor graph and node variables;

e. generating a state variable in a specific period, matching according to the measurement result, and adding factor nodes;

f. pushing a new factor graph and node variables into the ISAM2 solver to realize asynchronous updating, resetting the factor graph and the node variables, and repeating the step e.

Images