CN116086445A - Multi-source information time delay fusion navigation method based on factor graph optimization - Google Patents
Multi-source information time delay fusion navigation method based on factor graph optimization Download PDFInfo
- Publication number
- CN116086445A CN116086445A CN202310179198.3A CN202310179198A CN116086445A CN 116086445 A CN116086445 A CN 116086445A CN 202310179198 A CN202310179198 A CN 202310179198A CN 116086445 A CN116086445 A CN 116086445A
- Authority
- CN
- China
- Prior art keywords
- factor
- measurement
- time
- node
- odo
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 41
- 238000000034 method Methods 0.000 title claims abstract description 33
- 230000004927 fusion Effects 0.000 title claims abstract description 27
- 238000005259 measurement Methods 0.000 claims abstract description 104
- 230000010354 integration Effects 0.000 claims abstract description 21
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 9
- 238000009499 grossing Methods 0.000 claims abstract description 8
- 238000005070 sampling Methods 0.000 claims abstract description 7
- 238000010276 construction Methods 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000004458 analytical method Methods 0.000 claims description 5
- 238000005295 random walk Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 239000013598 vector Substances 0.000 claims description 3
- 230000015556 catabolic process Effects 0.000 claims description 2
- 238000006731 degradation reaction Methods 0.000 claims description 2
- 230000006872 improvement Effects 0.000 description 5
- 238000001914 filtration Methods 0.000 description 4
- 230000001934 delay Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000000295 complement effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000009194 climbing Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
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
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 3, generating state variables to be optimized in an increment mode according to time sequence and adding pre-integration factors;
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 nodeRefers 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 0 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 nodeRepresenting 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:
b.t 1 To t 2 At moment, reading IMU measurement data of the time period to perform pre-integration;
c.t 2 time of day, pair x 1 And x 2 Adding a pre-integration factorFor x 1 The adding time delay is less than T S Is>
d.t 2 To t 3 At moment, reading IMU measurement data of the time period to perform pre-integration;
e.t 3 time of day, pair x 2 And x 3 Adding a pre-integration factorFor x 2 The adding time delay is less than T S ODO factor->For x 1 The adding time delay is greater than T S But less than 2T S GNSS factor of->
t 3 For the current time, since the time delay span of GNSS is greater than 1T S ,t 1 And t 2 GNSS factors are not added at any time until t 3 Added at the momentThe 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:
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 nodeRefers 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 0 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 nodeRepresenting 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.
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.
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:
b.t 1 To t 2 And at the moment, reading IMU measurement data of the time period to perform pre-integration.
c.t 2 Time of day, pair x 1 And x 2 Adding a pre-integration factorFor x 1 The adding time delay is less than T S Is the odometer factor of (a)
d.t 2 To t 3 And at the moment, reading IMU measurement data of the time period to perform pre-integration.
e.t 3 Time of day, pair x 2 And x 3 Adding a pre-integration factorFor x 2 The adding time delay is less than T S ODO factor->For x 1 The adding time delay is greater than T S But less than 2T S GNSS factor of->
In FIG. 2, t 3 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 1 And t 2 GNSS factors are not added at any time until t 3 Added at the momentThe 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:
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 realizedAccelerometer 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 nodeRefers 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 0 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 nodeRepresenting 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:
b.t 1 To t 2 At moment, reading IMU measurement data of the time period to perform pre-integration;
c.t 2 time of day, pair x 1 And x 2 Adding a pre-integration factorFor x 1 The adding time delay is less than T S Is>
d.t 2 To t 3 At moment, reading IMU measurement data of the time period to perform pre-integration;
e.t 3 time of day, pair x 2 And x 3 Adding a pre-integration factorFor x 2 The adding time delay is less than T S ODO factor->For x 1 The adding time delay is greater than T S But less than 2T S GNSS factor of->
t 3 For the current time, since the time delay span of GNSS is greater than 1T S ,t 1 And t 2 GNSS factors are not added at any time until t 3 Added at the momentThe 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:
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.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310179198.3A CN116086445A (en) | 2023-02-28 | 2023-02-28 | Multi-source information time delay fusion navigation method based on factor graph optimization |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310179198.3A CN116086445A (en) | 2023-02-28 | 2023-02-28 | Multi-source information time delay fusion navigation method based on factor graph optimization |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116086445A true CN116086445A (en) | 2023-05-09 |
Family
ID=86204479
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310179198.3A Pending CN116086445A (en) | 2023-02-28 | 2023-02-28 | Multi-source information time delay fusion navigation method based on factor graph optimization |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116086445A (en) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116608864A (en) * | 2023-07-19 | 2023-08-18 | 青岛哈尔滨工程大学创新发展中心 | AUV cooperative positioning method based on factor graph under influence of communication time delay |
CN116817928A (en) * | 2023-08-28 | 2023-09-29 | 北京交通大学 | Method for multi-source fusion positioning of guard/inertial navigation train based on factor graph optimization |
-
2023
- 2023-02-28 CN CN202310179198.3A patent/CN116086445A/en active Pending
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116608864A (en) * | 2023-07-19 | 2023-08-18 | 青岛哈尔滨工程大学创新发展中心 | AUV cooperative positioning method based on factor graph under influence of communication time delay |
CN116608864B (en) * | 2023-07-19 | 2023-10-27 | 青岛哈尔滨工程大学创新发展中心 | AUV cooperative positioning method based on factor graph under influence of communication time delay |
CN116817928A (en) * | 2023-08-28 | 2023-09-29 | 北京交通大学 | Method for multi-source fusion positioning of guard/inertial navigation train based on factor graph optimization |
CN116817928B (en) * | 2023-08-28 | 2023-12-01 | 北京交通大学 | Method for multi-source fusion positioning of guard/inertial navigation train based on factor graph optimization |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Zaidner et al. | A novel data fusion algorithm for low-cost localisation and navigation of autonomous vineyard sprayer robots | |
CN116086445A (en) | Multi-source information time delay fusion navigation method based on factor graph optimization | |
CN111337020A (en) | Factor graph fusion positioning method introducing robust estimation | |
CN110275193B (en) | Cluster satellite collaborative navigation method based on factor graph | |
WO2024179023A1 (en) | Flexible chi-square test-based adaptive factor graph optimization integrated navigation method | |
CN117685953A (en) | UWB and vision fusion positioning method and system for multi-unmanned aerial vehicle co-positioning | |
CN116007620A (en) | Combined navigation filtering method, system, electronic equipment and storage medium | |
CN116399351A (en) | Vehicle position estimation method | |
CN114915913A (en) | UWB-IMU combined indoor positioning method based on sliding window factor graph | |
Ascorti | An application of the extended Kalman filter to the attitude control of a quadrotor | |
CN111578931B (en) | High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation | |
CN117419723A (en) | Interactive model self-adaptive integrated navigation method based on factor graph | |
CN116878510A (en) | Multi-source fusion indoor positioning method, device and system based on factor graph optimization | |
CN114993341B (en) | Carrier rocket trajectory estimation method and device based on space-based measurement | |
CN116380038A (en) | Multisource navigation information fusion method based on online incremental scale factor graph | |
CN115014321B (en) | Bionic polarization multisource fusion orientation method based on adaptive robust filtering | |
CN116224407A (en) | GNSS and INS integrated navigation positioning method and system | |
CN106931966B (en) | A kind of Combinated navigation method based on the fitting of Taylor's high-order remainder | |
CN115639585A (en) | Multi-sensor fusion positioning method for GPS/IMU and laser radar | |
Liu et al. | Effective Sage-Husa Kalman filter for SINS/Doppler/Platform compass integrated navigation system | |
Li et al. | Exploring the Potential of Deep Learning Aided Kalman Filter for GNSS/INS Integration: A Study on 2D Simulation Datasets | |
CN114001759A (en) | Array type MEMS sensor control method and system | |
Girrbach et al. | Adaptive compensation of measurement delays in multi-sensor fusion for inertial motion tracking using moving horizon estimation | |
Mercier et al. | Relative Navigation Methods for a Multi-Agent, On-Orbit Inspection Mission | |
Wang et al. | Robust Visual Inertial Odometry Estimation Based on Adaptive Interactive Multiple Model Algorithm |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |