CN113514064A - Robust factor graph multi-source fault-tolerant navigation method - Google Patents
Robust factor graph multi-source fault-tolerant navigation method Download PDFInfo
- Publication number
- CN113514064A CN113514064A CN202110439408.9A CN202110439408A CN113514064A CN 113514064 A CN113514064 A CN 113514064A CN 202110439408 A CN202110439408 A CN 202110439408A CN 113514064 A CN113514064 A CN 113514064A
- Authority
- CN
- China
- Prior art keywords
- navigation
- carrier
- information
- navigation sensor
- fault
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000005259 measurement Methods 0.000 claims abstract description 34
- 238000001514 detection method Methods 0.000 claims abstract description 29
- 238000005457 optimization Methods 0.000 claims abstract description 16
- 238000005070 sampling Methods 0.000 claims description 24
- 239000011159 matrix material Substances 0.000 claims description 22
- 239000013598 vector Substances 0.000 claims description 18
- 239000000126 substance Substances 0.000 claims description 14
- 230000010354 integration Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 4
- 238000003745 diagnosis Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 239000000969 carrier Substances 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 7
- 239000000243 solution Substances 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 4
- 230000004927 fusion Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000007257 malfunction Effects 0.000 description 2
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 238000005094 computer simulation Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002347 injection Methods 0.000 description 1
- 239000007924 injection Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 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/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
The invention discloses a robust factor graph multi-source fault-tolerant navigation method, which comprises the following steps: initializing a system: initializing the navigation state quantity and the navigation sensor residual of each navigation sensor; obtaining IMU navigation information: acquiring accelerometer data and gyroscope data through an IMU (inertial measurement Unit), and calculating first carrier navigation information based on the accelerometer data and the gyroscope data; navigation sensor residual acquisition: acquiring second carrier navigation information in real time based on each navigation sensor, and calculating residual errors of each navigation sensor based on the first carrier navigation information and the second carrier navigation information; navigation sensor fault detection: fault detection is carried out on each navigation sensor based on residual error data of each navigation sensor; navigation optimization: and isolating the fault navigation sensor, and optimizing and solving the carrier navigation information by utilizing a factor graph based on the fault information. The invention can solve the problem that the positioning accuracy of the combined navigation system based on the factor graph algorithm is reduced due to the failure of the navigation sensor, and has high reliability and strong applicability.
Description
Technical Field
The invention relates to the technical field of integrated navigation, in particular to a robust factor graph multi-source fault-tolerant navigation method.
Background
The inertial-based multi-source navigation information fusion technology is a multi-source information fusion technology which is based on the inertial navigation technology and fuses other navigation information. The construction of a robust information fusion method based on an inertial navigation technology is always a research hotspot in the field of integrated navigation. However, if other navigation sensors generate faults, the fault information is brought into the integrated navigation system, and the estimation accuracy of the navigation process is greatly reduced. At present, a robust fusion method for a combined navigation system mainly adopts a fault detection algorithm based on a model and a fault detection method based on signal processing to detect fault information. Various filtering algorithms are combined on the basis to provide a robust fusion navigation method.
Compared with the traditional Kalman filtering algorithm, the factor graph algorithm can simultaneously carry out batch estimation on the state quantities at the historical moments by using a plurality of measurement information at the historical moments to obtain the global optimal solution of the state quantities of the system, so that the state estimation precision is improved. In order to improve the robustness of the navigation method, a hierarchical factor graph mode can be adopted. The method carries out fault diagnosis layer by layer from outside to inside and judges the position of the fault. However, the method needs to establish a complete and clear factor graph hierarchical model and determine the probability dependence relationship of variables in each hierarchy, and the system modeling has large calculation amount and high complexity. Therefore, a robust factor graph multi-source fault-tolerant navigation method which can utilize the characteristics of the existing factor graph model is needed.
Disclosure of Invention
The invention aims to provide a robust factor graph multi-source fault-tolerant navigation method, which aims to solve the problems in the prior art, can solve the problem that the positioning accuracy of a combined navigation system based on a factor graph algorithm is reduced due to the failure of a navigation sensor, and has high reliability and strong applicability.
In order to achieve the purpose, the invention provides the following scheme: the invention provides a robust factor graph multi-source fault-tolerant navigation method, which comprises the following steps:
initializing a system: initializing the navigation state quantity and the navigation sensor residual of each navigation sensor;
obtaining navigation information of an inertial measurement unit IMU: acquiring accelerometer data and gyroscope data through an IMU (inertial measurement Unit), and calculating first carrier navigation information based on the accelerometer data and the gyroscope data;
navigation sensor residual acquisition: acquiring second carrier navigation information in real time based on each navigation sensor, and calculating residual errors of each navigation sensor based on the first carrier navigation information and the second carrier navigation information;
navigation sensor fault detection: fault detection is carried out on each navigation sensor based on residual error data of each navigation sensor;
navigation optimization: and isolating the fault navigation sensor, and optimizing and solving the carrier navigation information by utilizing a factor graph based on the fault information.
Preferably, in the system initialization step, the navigation state quantity of each navigation sensor includes: carrier position information, carrier speed information, carrier attitude information, wherein the carrier position information includes: longitude, latitude and height of the position of the carrier; the carrier speed information is initialized to 0; and the carrier attitude information is an included angle between a machine system and a geographical system which are expressed in a quaternion form.
Preferably, in the system initialization step, the initialization result of the navigation sensor residual is 0, and the measurement covariance matrix is initialized by using the navigation sensor noise.
Preferably, the obtaining step of IMU navigation information specifically includes:
respectively constructing output models of an accelerometer and a gyroscope in the IMU;
and respectively calculating first carrier navigation information of the carrier based on output results of output models of the accelerometer and the gyroscope at two adjacent sampling moments of the IMU, wherein the first carrier navigation information comprises first carrier attitude information, first carrier speed information and first carrier position information.
Preferably, the calculation method of the first carrier attitude information includes:
for two adjacent sampling instants t of the IMUkAnd tk+1Calculating the output result of the output model of the gyroscopeFirst carrier attitude information, as shown in the following equation:
wherein the content of the first and second substances,
for carriers represented in quaternion form at tk+1The attitude information of the time of day,
for attitude information of the carrier represented in the form of quaternions over an integration time t,
calculating the required carrier motion angular rate for actual navigation in the integration time t;
the method for calculating the first carrier velocity information comprises the following steps:
for two adjacent sampling instants t of the IMUkAnd tk+1Calculating the first carrier velocity information according to the output result of the output model of the accelerometer, as shown in the following formula:
wherein the content of the first and second substances,
denotes the vector at tk+1The speed information of the time of day,
denotes the vector at tkThe speed information of the time of day,
represents tkThe angular rate of rotation of the earth at that moment,
represents tkComponent of angular velocity of rotation of the time navigation system relative to the global coordinate system in the navigation system, Δ TIMURepresenting the sampling time interval of the IMU,
denotes the vector at tk+1A coordinate transformation matrix from the time machine system to the navigation system,
represents tkOutput value of time accelerometer, naAs white noise of the accelerometer, baZero offset is given to the accelerometer, g is the gravity acceleration of the earth, and n is the sampling number of the navigation sensor;
the calculation method of the first carrier position information comprises the following steps:
for two adjacent sampling instants t of the IMUkAnd tk+1Calculating the first carrier position information according to the output result of the output model of the accelerometer, as shown in the following formula:
wherein λ isk、λk+1Respectively represent the vectors tkAnd tk+1Longitude information of time, Lk、Lk+1Respectively represent the vectors tkAnd tk+1Latitude information of time, hk、hk+1Respectively represent the vectors tkAnd tk+1Information on the height of the moment of time,
respectively representing the components of the carrier velocity in the x, y and z axial directions, RN、RMRespectively representing the curvature radius of the earth prime unit circle and the curvature radius of the meridian circle.
Preferably, in the navigation sensor residual obtaining step, the second carrier navigation information includes: second carrier attitude information, second carrier velocity information, second carrier position information; at sampling time tk+1Time of day, residual r of the navigation sensork+1As shown in the following formula:
rk+1＝[rvk+1 rpk+1 rqk+1]T
wherein r isvk+1、rpk+1、rqk+1Respectively represent tk+1And a speed residual error item, a position residual error item and a posture residual error item of the navigation sensor at the moment.
Preferably, the velocity residual term rvk+1The calculating method comprises the following steps:
when the speed information of the second carrier collected by the navigation sensor is Zvk+1Calculating t from said first carrier velocity informationk+1Time-of-day velocity residual term rvk+1Comprises the following steps:
wherein the content of the first and second substances,
represents tk+1First carrier velocity information of time of day, QVRepresenting the navigation sensor velocity measurement covariance matrix;
the position residual error term rpk+1The calculating method comprises the following steps:
when the position information of the second carrier acquired by the navigation sensor is Zpk+1Calculating t from said first carrier position informationk+1Time position residual error term rpk+1Comprises the following steps:
rpk+1＝(Zpk+1-pk+1)TQP -1(Zpk+1-pk+1)
wherein p isk+1Represents tk+1First carrier position information of time, QPRepresenting the navigation sensor position measurement covariance matrix;
the attitude residual error term rqk+1The calculating method comprises the following steps:
when the second carrier attitude information Z represented by quaternion is acquired by the navigation sensorqk+1Calculating t from the first carrier attitude informationk+1Time attitude residual error term rqk+1Comprises the following steps:
wherein the content of the first and second substances,
representing the first carrier attitude information,
representing the imaginary part, Q, of the quaternion of the attitude errorqRepresenting the navigation sensor attitude measurement covariance matrix.
Preferably, in the navigation sensor malfunction detection step, the method of detecting malfunction of the navigation sensor includes: calculating a residual mean value of each of the navigation sensors
Based on the residual error of each navigation sensor and the residual error mean value
The fault diagnosis is performed as shown in the following formula:
wherein r isk+1The residual error of the navigation sensor is obtained, eta is a fault detection threshold coefficient, S is a fault detection result, and if the detection result is 0, the navigation information of the current navigation sensor is indicated to have a fault; and if the detection result is 1, indicating that no fault exists in the navigation information of the current navigation sensor.
Preferably, the navigation optimization step specifically includes:
if the navigation sensor has faults, the navigation optimization objective function is shown as the following formula:
if the navigation sensor has no fault, the navigation optimization objective function is shown as the following formula:
wherein, | | rp-HpX||2For the purpose of the a-priori constraint of marginalization,
for inertial residuals, B is the set of all IMU measurements,
is an inertial measurement covariance matrix,
measuring residuals for the navigation sensors, C is a set of navigation sensors,
representing a navigation sensor measurement covariance matrix.
Preferably, a Gaussian and Newton nonlinear optimization method is used for solving the objective function, and the IMU navigation information obtaining step is repeatedly executed until a preset condition is reached, so that the navigation information of the carrier is obtained.
The invention discloses the following technical effects:
the invention provides a robust factor graph multi-source fault-tolerant navigation method, which comprises the steps of acquiring accelerometer data and gyroscope data through an IMU (inertial measurement Unit), calculating to obtain first carrier navigation information, acquiring second carrier navigation information through navigation sensors, calculating residual errors of the navigation sensors through the first carrier navigation information and the second carrier navigation information, carrying out fault detection on the navigation sensors through residual error data, and isolating and carrying out system optimization on the fault navigation sensors, so that the problem that the positioning accuracy of an integrated navigation system based on a factor graph algorithm is reduced due to the fault of the navigation sensors can be effectively solved, and the robust factor graph multi-source fault-tolerant navigation method is high in reliability and strong in applicability.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
FIG. 1 is a flow chart of a robust factor graph multi-source fault-tolerant navigation method of the present invention;
FIG. 2 is a schematic diagram of fault injection simulation in an embodiment of the present invention;
fig. 3 is a schematic diagram of fault detection in the embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
Referring to fig. 1, the embodiment provides a robust factor graph multi-source fault-tolerant navigation method, including the following steps:
step S1, system initialization: initializing the navigation state quantity and the navigation sensor residual of each navigation sensor; initializing a system navigation state quantity and a navigation sensor residual error; the method specifically comprises the following steps:
1) initializing carrier position information, carrier speed information and carrier attitude information, wherein the carrier position information comprises: longitude λ of the location of the carrier0Latitude L0Height h0(ii) a The carrier speed information
Initialization is 0; the carrier attitude information
Is an included angle between a machine system and a geographic system expressed in a quaternion form, wherein e is a terrestrial coordinate system, k represents the current sampling moment of the navigation sensor, and nkA navigation system representing the current sampling instant of said navigation sensor, bkAnd the body system represents the current sampling moment of the navigation sensor.
2) Initializing navigation sensor residual rkTo 0, initializing a measurement covariance Q with the navigation sensor noisek。
Step S2, obtaining navigation information of an Inertial Measurement Unit (IMU): accelerometer data acquisition by IMU
Gyroscope data
And based on the accelerometer data
Gyroscope data
Calculating first carrier navigation information; the method specifically comprises the following steps:
1) respectively constructing output models of an accelerometer and a gyroscope in the IMU, wherein the output models are shown as the following formula:
wherein the content of the first and second substances,
the carrier motion angular rate required by the actual navigation solution is the component of the body system relative to the navigation system under the body system,
is the output value of the gyroscope, bωZero bias for the gyroscope, nωWhite noise for the gyroscope;
is the component of the acceleration of the actual motion of the carrier under the navigation system,
is a coordinate transformation matrix from a machine system to a navigation system,
converting a coordinate matrix from a navigation system to a body system; f. ofbIs the output value of the accelerometer, g is the acceleration of the earth gravity, naAs white noise of the accelerometer, baZero-offset for the accelerometer, n represents the number of navigation sensor samples,
representing the earth rotation angular rate;
representing the involvement angular rate of the navigation system relative to the earth coordinate system under the influence of the carrier motion;
representing the carrier movement rate; i represents an inertial coordinate system; b denotes a body coordinate system, also called machine system.
2) For two adjacent sampling instants t of the IMUkAnd tk+1And calculating first carrier attitude information according to an output result of the output model of the gyroscope, wherein the first carrier attitude information is represented by the following formula:
wherein the content of the first and second substances,
for carriers represented in quaternion form at tk+1The attitude information of the time of day,
for attitude information of the carrier represented in the form of quaternions over an integration time t,
the required angular rate of motion of the carrier is solved for the actual navigation within the integration time t.
3) For two adjacent sampling instants t of the IMUkAnd tk+1Calculating the first carrier velocity information according to the output result of the output model of the accelerometer, as shown in the following formula:
wherein the content of the first and second substances,
denotes the vector at tk+1The speed information of the time of day,
denotes the vector at tkThe speed information of the time of day,
represents tkThe angular rate of rotation of the earth at that moment,
represents tkComponent of angular velocity of rotation of the time navigation system relative to the global coordinate system in the navigation system, Δ TIMURepresenting the sampling time interval of the IMU.
Denotes the vector at tk+1Coordinate transformation matrix from time frame system to navigation system, from tk+1The time representing the carrier attitude information in the form of quaternion is calculated in the following form:
wherein the content of the first and second substances,
represents tk+1The real part of the carrier attitude quaternion,
representing the three imaginary parts of the carrier attitude quaternion, respectively.
4) For two adjacent sampling instants t of the IMUkAnd tk+1Calculating the first carrier position information according to the output result of the output model of the accelerometer, as shown in the following formula:
wherein λ isk、λk+1Respectively represent the vectors tkAnd tk+1Longitude information of time, Lk、Lk+1Respectively represent the vectors tkAnd tk+1Latitude information of time, hk、hk+1Respectively represent the vectors tkAnd tk+1Information on the height of the moment of time,
respectively representing the components of the carrier velocity in the x, y and z axial directions, RN、RMRespectively representing the curvature radius of the earth prime unit circle and the curvature radius of the meridian circle.
Step S3, navigation sensor residual acquisition: acquiring second carrier navigation information in real time based on each navigation sensor, and calculating residual errors of each navigation sensor based on the first carrier navigation information and the second carrier navigation information; the method specifically comprises the following steps:
the second carrier navigation information comprises: second carrier attitude information, second carrier velocity information, second carrier position information. Recording the sampling time t of the navigation sensork+1At the moment when the navigation sensor navigation information Z is collectedkThe residual r of the navigation sensor is calculated as followsk+1：
rk+1＝[rvk+1 rpk+1 rqk+1]T
Wherein r isvk+1、rpk+1、rqk+1Respectively representtk+1The speed residual error item, the position residual error item and the posture residual error item of the navigation sensor at the moment specifically comprise:
velocity residual term rvk+1The calculating method comprises the following steps: when the speed information of the second carrier collected by the navigation sensor is Zvk+1Then, t is calculated from the first carrier velocity information calculated by the IMU in step S2k+1Time-of-day velocity residual term rvk+1Comprises the following steps:
wherein the content of the first and second substances,
indicating that the IMU has recursively resolved to t according to step S2k+1First carrier velocity information of time of day, QVThe navigation sensor speed measurement covariance matrix is represented by:
wherein n isvRepresenting the navigation sensor speed measurement noise.
Position residual error term rpk+1The calculating method comprises the following steps: when the position information of the second carrier acquired by the navigation sensor is Zpk+1Then, t is calculated by the first carrier position information calculated by the IMU in step S2k+1Time position residual error term rpk+1Comprises the following steps:
rpk+1＝(Zpk+1-pk+1)TQP -1(Zpk+1-pk+1)
wherein p isk+1Indicating that the IMU has recursively resolved to t according to step S2k+1First carrier position information of time, QPThe covariance matrix of the position measurement of the navigation sensor is represented by:
wherein n isvRepresenting the navigation sensor speed measurement noise.
Attitude residual term rqk+1The calculating method comprises the following steps: when the second carrier attitude information Z represented by quaternion is acquired by the navigation sensorqk+1Then, t is calculated by the first carrier attitude information calculated by the IMU in step S2k+1Time attitude residual error term rqk+1Comprises the following steps:
wherein the content of the first and second substances,
indicating that the IMU has recursively resolved to t according to step S2k+1The first carrier attitude information of the time of day,
representing the imaginary part, Q, of the quaternion of the attitude errorqThe covariance matrix of the attitude measurement of the navigation sensor is represented by:
wherein n isqRepresenting the navigation sensor attitude measurement noise.
Step S4, navigation sensor fault detection: fault detection is carried out on each navigation sensor based on residual error data of each navigation sensor;
the method for fault detection of the navigation sensor comprises the following steps:
1) calculating a residual mean value of each of the navigation sensors
As shown in the following formula:
wherein Z isjRepresenting navigation information provided by the navigation sensor, h (X)jRepresenting first carrier navigation information, Δ T, resolved by IMU recursionjRepresenting the navigation sensor sampling time interval, and n represents the sampling number of the navigation sensor.
2) The fault diagnosis function is calculated as follows:
wherein eta is a fault detection threshold coefficient, S is a fault detection result, and if the detection result is 0, the current navigation sensor navigation information is indicated to have a fault; if the detection result is 1, the navigation information of the current navigation sensor is indicated to have no fault,
is the residual mean value of the navigation sensor.
Step S5, navigation optimization: isolating the fault navigation sensor, and optimizing and solving carrier navigation information by utilizing a factor graph based on fault information;
in this embodiment, an objective function to be optimized of the whole system including the fault detection result is constructed, as shown in the following formula:
wherein, | | rp-HpX||2For the prior constraint of marginalization, only a small amount of measurement and states are reserved in the optimization, and other measurement and states are marginalized and converted into prior;
for inertial residuals, B is the set of all IMU measurements,
an inertia measurement covariance matrix;
measuring residuals for the navigation sensors, C is a set of navigation sensors,
representing a navigation sensor measurement covariance matrix.
If the fault of the navigation sensor is detected, S is 0, namely the part of the objective function to be optimized is 0, the objective function to be optimized does not need to be optimized, and the overall objective function to be optimized of the system is calculated according to the following form:
if the sensor has no fault, the overall objective function to be optimized of the system is calculated according to the following form:
and solving the objective function by using a Gaussian and Newton nonlinear optimization method, repeating the steps S2-S5, stopping optimization when an error convergence state is reached or the iteration times reach a threshold value, outputting navigation information of the carrier, and finishing navigation of the carrier.
The above-described embodiments are merely illustrative of the preferred embodiments of the present invention, and do not limit the scope of the present invention, and various modifications and improvements of the technical solutions of the present invention can be made by those skilled in the art without departing from the spirit of the present invention, and the technical solutions of the present invention are within the scope of the present invention defined by the claims.
Claims (10)
1. A robust factor graph multi-source fault-tolerant navigation method is characterized by comprising the following steps:
initializing a system: initializing the navigation state quantity and the navigation sensor residual of each navigation sensor;
obtaining navigation information of an inertial measurement unit IMU: acquiring accelerometer data and gyroscope data through an IMU (inertial measurement Unit), and calculating first carrier navigation information based on the accelerometer data and the gyroscope data;
navigation sensor residual acquisition: acquiring second carrier navigation information in real time based on each navigation sensor, and calculating residual errors of each navigation sensor based on the first carrier navigation information and the second carrier navigation information;
navigation sensor fault detection: fault detection is carried out on each navigation sensor based on residual error data of each navigation sensor;
navigation optimization: and isolating the fault navigation sensor, and optimizing and solving the carrier navigation information by utilizing a factor graph based on the fault information.
2. The robust factor graph multi-source fault-tolerant navigation method according to claim 1, wherein in the system initialization step, the navigation state quantity of each navigation sensor comprises: carrier position information, carrier speed information, carrier attitude information, wherein the carrier position information includes: longitude, latitude and height of the position of the carrier; the carrier speed information is initialized to 0; and the carrier attitude information is an included angle between a machine system and a geographical system which are expressed in a quaternion form.
3. The robust factor graph multi-source fault-tolerant navigation method according to claim 1, wherein in the system initialization step, the initialization result of the navigation sensor residual error is 0, and a measurement covariance matrix is initialized by using the navigation sensor noise.
4. The robust factor graph multi-source fault-tolerant navigation method according to claim 1, wherein the IMU navigation information obtaining step specifically comprises:
respectively constructing output models of an accelerometer and a gyroscope in the IMU;
and respectively calculating first carrier navigation information of the carrier based on output results of output models of the accelerometer and the gyroscope at two adjacent sampling moments of the IMU, wherein the first carrier navigation information comprises first carrier attitude information, first carrier speed information and first carrier position information.
5. The robust factor graph multi-source fault-tolerant navigation method of claim 4, wherein the calculation method of the first carrier attitude information comprises:
for two adjacent sampling instants t of the IMUkAnd tk+1And calculating the attitude information of the first carrier according to the output result of the output model of the gyroscope, wherein the attitude information of the first carrier is represented by the following formula:
wherein the content of the first and second substances,
for carriers represented in quaternion form at tk+1The attitude information of the time of day,
for attitude information of the carrier represented in the form of quaternions over an integration time t,
calculating the required carrier motion angular rate for actual navigation in the integration time t;
the method for calculating the first carrier velocity information comprises the following steps:
for two adjacent sampling instants t of the IMUkAnd tk+1Calculating the first carrier velocity information according to the output result of the output model of the accelerometer, as shown in the following formula:
wherein the content of the first and second substances,
denotes the vector at tk+1The speed information of the time of day,
denotes the vector at tkThe speed information of the time of day,
represents tkThe angular rate of rotation of the earth at that moment,
represents tkComponent of angular velocity of rotation of the time navigation system relative to the global coordinate system in the navigation system, Δ TIMURepresenting the sampling time interval of the IMU,
denotes the vector at tk+1A coordinate transformation matrix from the time machine system to the navigation system,
represents tkOutput value of time accelerometer, naAs white noise of the accelerometer, baZero offset is given to the accelerometer, g is the gravity acceleration of the earth, and n is the sampling number of the navigation sensor;
the calculation method of the first carrier position information comprises the following steps:
for two adjacent sampling instants t of the IMUkAnd tk+1Calculating the first carrier position information according to the output result of the output model of the accelerometer, as shown in the following formula:
wherein λ isk、λk+1Respectively represent the vectors tkAnd tk+1Longitude information of time, Lk、Lk+1Respectively represent the vectors tkAnd tk+1Latitude information of time, hk、hk+1Respectively represent the vectors tkAnd tk+1Information on the height of the moment of time,
respectively representing the components of the carrier velocity in the x, y and z axial directions, RN、RMRespectively representing the curvature radius of the earth prime unit circle and the curvature radius of the meridian circle.
6. The robust factor graph multi-source fault-tolerant navigation method according to claim 1, wherein in the navigation sensor residual obtaining step, the second carrier navigation information comprises: second carrier attitude information, second carrier velocity information, second carrier position information; at sampling time tk+1Time of day, residual r of the navigation sensork+1As shown in the following formula:
rk+1＝[rvk+1 rpk+1 rqk+1]T
wherein r isvk+1、rpk+1、rqk+1Respectively represent tk+1And a speed residual error item, a position residual error item and a posture residual error item of the navigation sensor at the moment.
7. The robust factor graph multi-source fault-tolerant navigation method as claimed in claim 6, wherein the speed residual term rvk+1The calculating method comprises the following steps:
when the speed information of the second carrier collected by the navigation sensor is Zvk+1Calculating t from said first carrier velocity informationk+1Time-of-day velocity residual term rvk+1Comprises the following steps:
wherein the content of the first and second substances,
represents tk+1First carrier velocity information of time of day, QVRepresenting the navigation sensor velocity measurement covariance matrix;
the position residual error term rpk+1The calculating method comprises the following steps:
when the position information of the second carrier acquired by the navigation sensor is Zpk+1Calculating t from said first carrier position informationk+1Time position residual error term rpk+1Comprises the following steps:
rpk+1＝(Zpk+1-pk+1)TQP -1(Zpk+1-pk+1)
wherein p isk+1Represents tk+1First carrier position information of time, QPRepresenting the navigation sensor position measurement covariance matrix;
the attitude residual error term rqk+1The calculating method comprises the following steps:
when the second carrier attitude information Z represented by quaternion is acquired by the navigation sensorqk+1Calculating t from the first carrier attitude informationk+1Time attitude residual error term rqk+1Comprises the following steps:
wherein the content of the first and second substances,
representing the first carrier attitude information,
representing the imaginary part, Q, of the quaternion of the attitude errorqRepresenting the navigation sensor attitude measurement covariance matrix.
8. The robust factor graph multi-source fault-tolerant navigation method according to claim 1, wherein in the navigation sensor fault detection step, the method for fault detection of the navigation sensor comprises the following steps: calculating a residual mean value of each of the navigation sensors
Based on the residual error of each navigation sensor and the residual error mean value
The fault diagnosis is performed as shown in the following formula:
wherein r isk+1The residual error of the navigation sensor is obtained, eta is a fault detection threshold coefficient, S is a fault detection result, and if the detection result is 0, the navigation information of the current navigation sensor is indicated to have a fault; and if the detection result is 1, indicating that no fault exists in the navigation information of the current navigation sensor.
9. The robust factor graph multi-source fault-tolerant navigation method according to claim 1, wherein the navigation optimization step specifically comprises:
if the navigation sensor has faults, the navigation optimization objective function is shown as the following formula:
if the navigation sensor has no fault, the navigation optimization objective function is shown as the following formula:
wherein, | | rp-HpX||2For the purpose of the a-priori constraint of marginalization,
for inertial residuals, B is the set of all IMU measurements,
is an inertial measurement covariance matrix,
measuring residuals for the navigation sensors, C is a set of navigation sensors,
representing a navigation sensor measurement covariance matrix.
10. The robust factor graph multi-source fault-tolerant navigation method according to claim 9, wherein the objective function is solved by using a gauss-newton nonlinear optimization method, and the IMU navigation information obtaining step is repeatedly performed until a preset condition is reached, so as to obtain the navigation information of the carrier.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110439408.9A CN113514064B (en) | 2021-04-23 | 2021-04-23 | Multi-source fault-tolerant navigation method for robust factor graph |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110439408.9A CN113514064B (en) | 2021-04-23 | 2021-04-23 | Multi-source fault-tolerant navigation method for robust factor graph |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113514064A true CN113514064A (en) | 2021-10-19 |
| CN113514064B CN113514064B (en) | 2024-01-30 |
Family
ID=78061125
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110439408.9A Active CN113514064B (en) | 2021-04-23 | 2021-04-23 | Multi-source fault-tolerant navigation method for robust factor graph |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113514064B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113984061A (en) * | 2021-10-25 | 2022-01-28 | 哈尔滨工程大学 | UUV multi-sea-area integrated navigation system design method based on factor graph optimization |
| CN115615437A (en) * | 2022-09-20 | 2023-01-17 | 哈尔滨工程大学 | Factor graph combined navigation method |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103134491A (en) * | 2011-11-30 | 2013-06-05 | 上海宇航***工程研究所 | Integrated navigation system of strapdown inertial navigation system (SINS)/central nervous system (CNS)/global navigation satellite system (GNSS) of geostationary earth orbit (GEO) transfer vehicle |
| CN104075734A (en) * | 2014-07-01 | 2014-10-01 | 东南大学 | Intelligent underwater combined navigation fault diagnosis method |
| CN108981708A (en) * | 2018-08-02 | 2018-12-11 | 南京航空航天大学 | Quadrotor torque model/directional gyro/Magnetic Sensor fault-tolerance combined navigation method |
| CN110196068A (en) * | 2019-05-27 | 2019-09-03 | 哈尔滨工程大学 | A kind of polar region concentrate filtering integrated navigation system residual vector fault detection and partition method |
| CN110207697A (en) * | 2019-04-29 | 2019-09-06 | 南京航空航天大学 | Method is resolved based on angular accelerometer/gyro/accelerometer inertial navigation |
| CN110296701A (en) * | 2019-07-09 | 2019-10-01 | 哈尔滨工程大学 | Inertia and satellite combined guidance system gradation type failure recall fault-tolerance approach |
-
2021
- 2021-04-23 CN CN202110439408.9A patent/CN113514064B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103134491A (en) * | 2011-11-30 | 2013-06-05 | 上海宇航***工程研究所 | Integrated navigation system of strapdown inertial navigation system (SINS)/central nervous system (CNS)/global navigation satellite system (GNSS) of geostationary earth orbit (GEO) transfer vehicle |
| CN104075734A (en) * | 2014-07-01 | 2014-10-01 | 东南大学 | Intelligent underwater combined navigation fault diagnosis method |
| CN108981708A (en) * | 2018-08-02 | 2018-12-11 | 南京航空航天大学 | Quadrotor torque model/directional gyro/Magnetic Sensor fault-tolerance combined navigation method |
| CN110207697A (en) * | 2019-04-29 | 2019-09-06 | 南京航空航天大学 | Method is resolved based on angular accelerometer/gyro/accelerometer inertial navigation |
| WO2020220729A1 (en) * | 2019-04-29 | 2020-11-05 | 南京航空航天大学 | Inertial navigation solution method based on angular accelerometer/gyroscope/accelerometer |
| CN110196068A (en) * | 2019-05-27 | 2019-09-03 | 哈尔滨工程大学 | A kind of polar region concentrate filtering integrated navigation system residual vector fault detection and partition method |
| CN110296701A (en) * | 2019-07-09 | 2019-10-01 | 哈尔滨工程大学 | Inertia and satellite combined guidance system gradation type failure recall fault-tolerance approach |
Non-Patent Citations (1)
| Title |
|---|
| 白师宇等: "基于IMU/ODO 预积分的多传感器即插即用因子图融合方法", 《中国惯性技术学报》, vol. 28, no. 5, pages 624 - 628 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113984061A (en) * | 2021-10-25 | 2022-01-28 | 哈尔滨工程大学 | UUV multi-sea-area integrated navigation system design method based on factor graph optimization |
| CN113984061B (en) * | 2021-10-25 | 2023-02-14 | 哈尔滨工程大学 | UUV multi-sea-area integrated navigation system design method based on factor graph optimization |
| CN115615437A (en) * | 2022-09-20 | 2023-01-17 | 哈尔滨工程大学 | Factor graph combined navigation method |
| CN115615437B (en) * | 2022-09-20 | 2024-04-30 | 哈尔滨工程大学 | Factor graph integrated navigation method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113514064B (en) | 2024-01-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Li et al. | A robust graph optimization realization of tightly coupled GNSS/INS integrated navigation system for urban vehicles | |
| CN110702104B (en) | Inertial navigation error correction method based on vehicle zero speed detection | |
| CN111156994B (en) | INS/DR & GNSS loose combination navigation method based on MEMS inertial component | |
| CN109991636A (en) | Map constructing method and system based on GPS, IMU and binocular vision | |
| US20110208473A1 (en) | Method for an improved estimation of an object orientation and attitude control system implementing said method | |
| CN113175933B (en) | Factor graph integrated navigation method based on high-precision inertial pre-integration | |
| CN113252033B (en) | Positioning method, positioning system and robot based on multi-sensor fusion | |
| CN111238535B (en) | IMU error online calibration method based on factor graph | |
| CN113514064A (en) | Robust factor graph multi-source fault-tolerant navigation method | |
| CN110006427B (en) | BDS/INS tightly-combined navigation method in low-dynamic high-vibration environment | |
| CN110715659A (en) | Zero-speed detection method, pedestrian inertial navigation method, device and storage medium | |
| CN110207692B (en) | Map-assisted inertial pre-integration pedestrian navigation method | |
| CN112577493B (en) | Unmanned aerial vehicle autonomous positioning method and system based on remote sensing map assistance | |
| CN114002725A (en) | Lane line auxiliary positioning method and device, electronic equipment and storage medium | |
| CN110672095A (en) | Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation | |
| CN116007620A (en) | Combined navigation filtering method, system, electronic equipment and storage medium | |
| CN112562077A (en) | Pedestrian indoor positioning method integrating PDR and prior map | |
| CN116295511A (en) | Robust initial alignment method and system for pipeline submerged robot | |
| CN114061591A (en) | Contour line matching method based on sliding window data backtracking | |
| CN111121820B (en) | MEMS inertial sensor array fusion method based on Kalman filtering | |
| CN116047567A (en) | Deep learning assistance-based guard and inertial navigation combined positioning method and navigation method | |
| CN104101345B (en) | Multisensor attitude fusion method based on complementary reconstruction technology | |
| CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN112729283A (en) | Navigation method based on depth camera/MEMS inertial navigation/odometer combination | |
| CN117073720A (en) | Method and equipment for quick visual inertia calibration and initialization under weak environment and weak action control |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |