CN113432604A - IMU/GPS combined navigation method capable of sensitively detecting fault - Google Patents

IMU/GPS combined navigation method capable of sensitively detecting fault Download PDF

Info

Publication number
CN113432604A
CN113432604A CN202110729883.XA CN202110729883A CN113432604A CN 113432604 A CN113432604 A CN 113432604A CN 202110729883 A CN202110729883 A CN 202110729883A CN 113432604 A CN113432604 A CN 113432604A
Authority
CN
China
Prior art keywords
gps
imu
measurement
time
navigation method
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
Application number
CN202110729883.XA
Other languages
Chinese (zh)
Other versions
CN113432604B (en
Inventor
赵家俊
吴元清
鲁仁全
黄政杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202110729883.XA priority Critical patent/CN113432604B/en
Publication of CN113432604A publication Critical patent/CN113432604A/en
Application granted granted Critical
Publication of CN113432604B publication Critical patent/CN113432604B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/52—Determining velocity
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • 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)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Mathematical Physics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention discloses an IMU/GPS combined navigation method capable of sensitively detecting faults, which comprises the following steps: projecting the GPS measurement information under the WGS84 coordinate system to a UTM plane by adopting a UTM projection algorithm; introducing a chi-square test algorithm, establishing a GPS fault detection model based on a pre-integration theory, and taking a pre-integration value based on the IMU as an expected value of the pre-integration value based on the GPS; converting the deviation of the IMU-based pre-integration value and the GPS-based pre-integration value into a chi-square value for reflecting the abnormal degree of the GPS measurement; when the system detects the latest GPS measurement abnormality through a chi-square checking algorithm, firstly eliminating the GPS measurement point, and using IMU measurement propagation as system state estimation at the moment; the influence of GPS abnormal measurement on a system output result is reduced by adopting an M-estimator function; and (3) adopting factor graph optimization to construct a joint optimization framework, fusing the measurement information of the IMU and the GPS, and realizing the IMU/GPS integrated navigation method.

Description

IMU/GPS combined navigation method capable of sensitively detecting fault
Technical Field
The invention relates to the technical field of positioning navigation, in particular to an IMU/GPS combined navigation method capable of sensitively detecting faults.
Background
At present, an IMU/GPS combined navigation scheme is single, an IMU and GPS measurement are fused mainly by adopting an extended Kalman filtering algorithm or a nonlinear optimization algorithm, and the specific scheme is as follows:
as disclosed in chinese patent publication No.: CN108709552A, published: 2018-10-26, which provides an IMU and GPS tight combination method based on MEMS, the method adopts least square method to estimate GPS original data, and outputs GPS navigation estimated value; providing navigation estimated values of the position and the speed to the IMU as initial position and speed information; the IMU adopts a Kalman filtering method to carry out initial alignment, and the IMU measurement data is resolved to obtain IMU navigation information; the IMU navigation information is utilized to calculate Doppler frequency and provide the Doppler frequency for GPS to capture, track and assist, and the performance of the GPS is improved; and establishing a dynamic error model, performing data fusion on the navigation information of the GPS and the IMU by adopting an extended Kalman filter, correcting the navigation information of the IMU at the same time by feedback errors, and outputting the fused and corrected optimal position, speed and attitude information. According to the scheme, an IMU measurement propagation equation is used as a system prediction step, and the system state is propagated based on Euler integration or median integration. Then the GPS measurement is used as an external observation quantity, and the UTM projection algorithm is used as an observation equation. Based on the Kalman filter algorithm, the GPS observed quantity is used for correcting the predicted state of the system, so that the fusion positioning of the IMU and the GPS is realized.
As disclosed in chinese patent publication No.: CN111121767A, published: 2020-05-08, discloses a robot vision inertial navigation combined positioning method fused with a GPS, which comprises the steps of extracting and matching feature points of left and right images and front and back images of a binocular camera, and calculating the three-dimensional coordinates of the feature points and the relative pose of an image frame; selecting a key frame in the image stream, creating a sliding window, and adding the key frame into the sliding window; calculating a visual re-projection error, an IMU pre-integration residual error and a zero-offset residual error and combining the visual re-projection error, the IMU pre-integration residual error and the zero-offset residual error into a combined pose estimation residual error; carrying out nonlinear optimization on the combined pose estimation residual error by using an L-M method to obtain an optimized visual inertial navigation (VIO) robot pose; if the current moment has GPS data, performing adaptive robust Kalman filtering on the GPS position data and the VIO pose estimation data to obtain a final robot pose; and if no GPS data exists, replacing the final pose data with the VIO pose data. The scheme is as follows: in the nonlinear optimization algorithm, the state quantity of the system is used as a variable to be optimized, and IMU measurement and GPS measurement are used as optimization constraint items. The IMU measurement and the GPS measurement are converted into parameters of a cost function, respectively, by a measurement equation, and mahalanobis distance of the measured value from the state quantity of the system is taken as a residual error. And finally calculating the state quantity of the system which minimizes the residual error through a nonlinear optimization theory. The optimal system state quantity is the fusion positioning result of the IMU and the GPS.
The above listed fusion method of IMU/GPS combined navigation lacks a reliable sensor anomaly detection method. The GPS system can provide centimeter-level positioning services, but in GPS failure scenarios (e.g., tunnels, densely built areas, canyons, and other environments that block GPS signals), it is difficult for a GPS receiver to capture valid GPS signals resulting in GPS measurements with outliers. GPS outlier measurement points tend to be far from the actual location, which can severely distort the positioning system output once it is introduced into the fusion system. Therefore, GPS outlier detection is a very important step, but current IMU/GPS combined navigation systems generally rely on the number of satellites searched by the GPS receiver to estimate the current measurement state. And when the number of the searched satellites is less than a threshold value, the GPS is considered to be abnormal. However, due to multipath effects (which are often found at tunnel entrances and exits or in densely-built areas), it often happens that the number of searched satellites is obviously larger than a threshold value, but at the moment, a large-scale anomaly of the GPS measurement already occurs.
Disclosure of Invention
In order to overcome the defects in the prior art, the invention provides the IMU/GPS combined navigation method capable of sensitively detecting the fault.
In order to achieve the purpose of the invention, the technical scheme is as follows:
an IMU/GPS combination navigation method capable of sensitively detecting a malfunction, the method comprising the steps of:
s1: projecting the GPS measurement information under the WGS84 coordinate system to a UTM plane by adopting a UTM projection algorithm;
s2: introducing a chi-square test algorithm, establishing a GPS fault detection model based on a pre-integration theory, and taking a pre-integration value based on the IMU as an expected value of the pre-integration value based on the GPS; converting the deviation of the IMU-based pre-integration value and the GPS-based pre-integration value into a chi-square value for reflecting the abnormal degree of the GPS measurement;
s3: when the system detects the latest GPS measurement abnormality through a chi-square checking algorithm, firstly eliminating the GPS measurement point, and using IMU measurement propagation as system state estimation at the moment;
s4: in order to further improve the robustness of the IMU/GPS fusion system, an M-estimator function is adopted to reduce the influence of GPS abnormal measurement on the output result of the system;
s5: and (3) adopting factor graph optimization to construct a joint optimization framework, and fusing the measurement information of the IMU and the GPS.
Preferably, in step S1, specifically, coordinates of a point in WGS84 coordinate system are defined as (L, B), where L is longitude and B is latitude; projecting the point to an (x, y) location on the UTM plane; the equator radius of the earth is given as a, the first eccentricity is given as e, and the second eccentricity is given as e'; the origin of the WGS84 coordinate system is set to (L)00); based on the UTM projection algorithm, (L, B) translates to the corresponding (x, y) as follows:
Figure BDA0003138913650000031
wherein T, C, A, M and N are intermediate variables.
Further, linear interpolation is used to align GPS measurements at different times with IMU measurements; let the time stamp corresponding to the k +1 IMU measurement be tIPrevious GPS measurement data G of k +1 IMU measurement1Corresponding time stamp is
Figure BDA0003138913650000032
The last GPS measurement data G measured by the k +1 IMU2Corresponding time stamp is
Figure BDA0003138913650000033
The process of linear interpolation can be expressed as:
Figure BDA0003138913650000034
wherein ,Gk+1Represents the k +1 th GPS measurement data;
to this end, the GPS measurements are aligned with the time stamps of the IMU measurements.
Still further, in step S2, the specific steps are as follows:
s201: defining a measurement model of angular velocity and acceleration of the IMU;
s202: the velocity, displacement and rotation of the system in the world coordinate system at time t + Δ t are inferred using a measurement model of the IMU:
s203: calculating a measurement value of a pre-integration value of the system between a time i to a time j based on the IMU measurement according to equation (4);
s204: according to the formula (4), calculating and obtaining an estimated value of a pre-integration value of the system between the time i and the time j based on GPS measurement;
s205: the residual value r is defined as IMU-based on the residual Chi-Square test
Figure BDA0003138913650000041
And based on GPS
Figure BDA0003138913650000042
The difference between them; r reflects the degree of consistency of the GPS measurements and IMU measurements, such as:
Figure BDA0003138913650000043
if the GPS measurement is normal, r obeys zero Gaussian distribution, otherwise r contains offset; the decision variable s is defined as the sum of the squared residuals weighted by the correlation covariance matrix a:
Figure BDA0003138913650000044
decision variable s obeys χ of 3 degrees of freedom2Distributing;
s206: based on chi-square test, the GPS anomaly point is judged by the following criteria:
Figure BDA0003138913650000045
still further, in step S201, the expression of the measurement model is as follows:
Figure BDA0003138913650000046
wherein ,
Figure BDA0003138913650000047
and
Figure BDA0003138913650000048
measuring an original IMU under a base coordinate system at the moment t; omegat and atAre the true values of angular velocity and acceleration; btRepresents a bias; n istRepresenting white gaussian noise;
Figure BDA0003138913650000049
rotation from a world coordinate system W to a base coordinate system B at the moment t; g is the gravity vector in the world coordinate system.
Still further, in step S202, the expressions of speed, displacement, and rotation are as follows:
Figure BDA00031389136500000410
in the formula ,
Figure BDA00031389136500000411
still further, in step S203, the expression of the measured value of the pre-integrated value between time i and time j based on the IMU measurement calculation system is as follows:
Figure BDA0003138913650000051
in the formula ,
Figure BDA0003138913650000052
representing a linear acceleration vector in the IMU measurement data;
Figure BDA0003138913650000053
representing the static bias of the accelerometer at time k,
Figure BDA0003138913650000054
Representing the gaussian white noise of the accelerometer at time k;
still further, in step S204, an expression for calculating an estimated value of the pre-integration value of the system from time i to time j based on the GPS measurement is as follows:
Figure BDA0003138913650000055
still further, specifically, when the GPS measurement is used as a constraint item to participate in pose optimization, a Huber loss function rho (-) is adopted to replace a Mahalanobis distance function of a target optimization object; the target optimization function is abstractly expressed as follows:
Figure BDA0003138913650000056
in order to realize the joint optimization of the formula (10), so that the system fuses the measurement information of the IMU and the GPS, a joint optimization framework is constructed by adopting factor graph optimization; under a combined optimization framework, the system generates a new key frame every time GPS measurement information is returned; the key frame stores the pose (x, y, z) and posture (roll, pitch, yaw) information of the system under the current timestamp t, and the key frame is converted into nodes to be added into the factor graph as an optimization object;
meanwhile, GPS measurement data is converted into a unitary factor and added into the factor graph, and IMU data between two adjacent GPS measurements is converted into an IMU pre-integration factor and added into the factor graph;
after the factor graph optimization framework is constructed, nonlinear optimization is carried out on the constructed factor graph by using a GTSAM factor graph optimization library, and optimal estimation of the latest key frame is obtained by solving, so that the IMU/GPS combined navigation method is realized.
The invention has the following beneficial effects:
the invention uses the inherent stability, reliability and concealment of the IMU to measure and judge the abnormal condition of GPS measurement by the IMU measurement value. More specifically, the invention creatively utilizes IMU pre-integration theory and chi-square test theory to construct a theoretical formula capable of describing the relation between IMU measurement and GPS measurement, thereby effectively avoiding the system from fusing GPS abnormal measurement. And meanwhile, a nonlinear optimization function under an M-estimator improved factor graph optimization framework is introduced, so that the influence of GPS abnormal measurement on the system is further reduced. In addition, the system is considered to use the low-cost IMU at the beginning of system modeling, so the method is very suitable for the low-cost IMU/GPS combined navigation system, and the system can realize more reliable positioning performance than the traditional IMU/GPS combined navigation based on the Kalman filter only by improving the fusion algorithm under the condition of not changing hardware.
Drawings
FIG. 1 is a flowchart illustrating the steps of the IMU/GPS integrated navigation method according to embodiment 1.
FIG. 2 is a schematic diagram of the factor graph optimization framework described in example 1.
Detailed Description
The invention is described in detail below with reference to the drawings and the detailed description.
Example 1
As shown in fig. 1, an IMU/GPS combined navigation method capable of sensitively detecting a fault, the method includes the steps of:
s1: the positioning information acquired by the GPS is longitude and latitude data under a WGS84 geographic coordinate system, and the units of the inertial measurement data acquired by the IMU are meters and radians. The dimension of the data obtained by the two is different, and the data fusion is difficult to carry out. Therefore, the UTM projection algorithm is adopted to project the GPS measurement information under the WGS84 coordinate system to the UTM plane;
in one particular embodiment, the coordinates defining a point in the WGS84 coordinate system are (L, B), where L is longitude and B is latitude; projecting the point to an (x, y) location on the UTM plane; the equator radius of the earth is given as a, the first eccentricity is given as e, and the second eccentricity is given as e'; the origin of the WGS84 coordinate system is set to (L)00); based on the UTM projection algorithm, (L, B) translates to the corresponding (x, y) as follows:
Figure BDA0003138913650000071
wherein T, C, A, M and N are intermediate variables.
By equation (1), the measurement data of the GPS is projected onto the UTM plane, and the dimension of the data coincides with the IMU. Because the measurements of the IMU and the GPS are asynchronous, measurement time errors can affect the accuracy of the information fusion system. For example, since GPS _1 data is returned at the 3 rd time, IMU _1 data is returned at the 4 th time, and GPS _2 data is returned at the 5 th time, linear interpolation is performed on GPS _1 and GPS _2 to estimate how much GPS data should be at the 4 th time, in order to unify the time stamps of the two types of data.
Specifically, linear interpolation is used to align GPS measurements at different times with IMU measurements; let the time stamp corresponding to the k +1 IMU measurement be tIPrevious GPS measurement data G of k +1 IMU measurement1Corresponding time stamp is
Figure BDA0003138913650000072
The last GPS measurement data G measured by the k +1 IMU2Corresponding time stamp is
Figure BDA0003138913650000073
The process of linear interpolation can be expressed as:
Figure BDA0003138913650000074
wherein ,Gk+1Represents the k +1 th GPS measurement data;
to this end, the GPS measurements are aligned with the time stamps of the IMU measurements.
S2: in indoor or building-dense areas, GPS signals tend to be obscured. If GPS anomaly measurements (GPS anomaly points) are introduced into the fusion system, the system output results are often severely distorted or even system crashes, and in order to quickly and efficiently detect GPS anomaly points, IMU measurements will be used as a reference object for GPS outliers. Although the cumulative error of the IMU integration is unbounded, it is considered in the present system that no anomalous measurements occur due to its inherent stability, concealment. In order to avoid the large-scale error caused by the IMU long-time integration, the reliability of the GPS measurement is evaluated by adopting an IMU pre-integration theory.
Introducing a chi-square test algorithm, establishing a GPS fault detection model based on a pre-integration theory, and taking a pre-integration value based on the IMU as an expected value of the pre-integration value based on the GPS; converting the deviation of the IMU-based pre-integration value and the GPS-based pre-integration value into a chi-square value for reflecting the abnormal degree of the GPS measurement;
step S2, the specific steps are as follows:
s201: defining a measurement model of angular velocity and acceleration of the IMU, the expression is as follows:
Figure BDA0003138913650000081
wherein ,
Figure BDA0003138913650000082
and
Figure BDA0003138913650000083
measuring an original IMU under a base coordinate system at the moment t; omegat and atAre the true values of angular velocity and acceleration; btRepresents a bias; n istRepresenting white gaussian noise;
Figure BDA0003138913650000084
rotation from a world coordinate system W to a base coordinate system B at the moment t; g is the gravity vector in the world coordinate system.
S202: using a measurement model of the IMU to infer the speed, displacement and rotation of the system at time t + Δ t in the world coordinate system, the expression is as follows:
Figure BDA0003138913650000085
in the formula ,
Figure BDA0003138913650000086
s203: according to equation (4), the system calculates the measurement value of the pre-integration value from time i to time j based on the IMU measurement, and the expression is as follows:
Figure BDA0003138913650000087
in the formula ,
Figure BDA0003138913650000088
representing a linear acceleration vector in the IMU measurement data;
Figure BDA0003138913650000089
representing the static bias of the accelerometer at time k,
Figure BDA00031389136500000810
Representing the gaussian white noise of the accelerometer at time k.
S204: according to equation (4), an estimated value of the pre-integrated value of the system between the time i and the time j is calculated based on the GPS measurement, and the expression is as follows:
Figure BDA0003138913650000091
s205: note that Δ vij and ΔpijNeither corresponds to the actual speed nor displacement variation, and the most important function of equations (5) and (6) is to separate the IMU measurement from the system state quantities. By the equations (5) and (6), the pre-integration value can be calculated by IMU measurement or GPS measurement, respectively, thereby making IMU measurement and GPS measurement comparable. In addition, the pre-integration value is only related to the IMU measurements from time i to time j, which avoids the need to re-integrate all IMU measurements each time using equation (4), greatly reducing the system computation.
By equation (6), the GPS measurement can estimate the pre-integration value from time i to time j. The residual value r is defined as IMU-based on the residual Chi-Square test
Figure BDA0003138913650000092
And based on GPS
Figure BDA0003138913650000093
The difference between them; r reflects the degree of consistency of the GPS measurements and IMU measurements, such as:
Figure BDA0003138913650000094
if the GPS measurement is normal, r obeys zero Gaussian distribution, otherwise r contains offset; the decision variable s is defined as the sum of the squared residuals weighted by the correlation covariance matrix a:
Figure BDA0003138913650000095
decision variable s obeys χ of 3 degrees of freedom2Distributing;
s206: based on chi-square test, the GPS anomaly point is judged by the following criteria:
Figure BDA0003138913650000096
s3: when the system detects the latest GPS measurement abnormality through a chi-square checking algorithm, firstly eliminating the GPS measurement point, and using IMU measurement propagation as system state estimation at the moment;
s4: in practical application, it is difficult to ensure that equation (9) can detect all GPS outliers. In order to further improve the robustness of the IMU/GPS fusion system, an M-estimator function is adopted to reduce the influence of GPS abnormal measurement on the output result of the system; specifically, when GPS measurement is used as a constraint term to participate in pose optimization, a Huber loss function rho (-) is adopted to replace a Mahalanobis distance function of a target optimization object. The target optimization function is abstractly expressed as follows:
Figure BDA0003138913650000097
s5: and (3) adopting factor graph optimization to construct a joint optimization framework, and fusing the measurement information of the IMU and the GPS. In order to realize the joint optimization of the formula (10), so that the system fuses the measurement information of the IMU and the GPS, a joint optimization framework is constructed by adopting factor graph optimization; under a combined optimization framework, the system generates a new key frame every time GPS measurement information is returned; the key frame stores the pose (x, y, z) and posture (roll, pitch, yaw) information of the system under the current timestamp t, and the key frame is converted into nodes to be added into the factor graph as an optimization object;
meanwhile, GPS measurement data is converted into a unitary factor and added into the factor graph, and IMU data between two adjacent GPS measurements is converted into an IMU pre-integration factor and added into the factor graph;
after the factor graph optimization framework is constructed, nonlinear optimization is carried out on the constructed factor graph by using a GTSAM factor graph optimization library, and optimal estimation of the latest key frame is obtained by solving, so that the IMU/GPS combined navigation method is realized.
In the embodiment, firstly, the GPS measurement information under the WGS84 coordinate system is projected to the UTM plane through the UTM projection algorithm, so that the dimensions of the GPS measurement (longitude and latitude) and the IMU measurement information (meter) are consistent. And a chi-square inspection thought is introduced, and a GPS fault detection model based on a pre-integration theory is established. The IMU measurement and the GPS measurement can independently calculate a pre-integration value, the pre-integration value based on the IMU is used as an expected value of the pre-integration value based on the GPS, and the deviation between the pre-integration value and the GPS is converted into a chi-square value for reflecting the abnormal degree of the GPS measurement. In order to further improve the robustness of the IMU/GPS fusion system, the influence of GPS abnormal measurement on the output result of the system is reduced by adopting an M-estimator algorithm. And finally, factor graph optimization is applied to fuse measurement information of the IMU and the GPS, so that a high-frequency, high-precision and high-robustness positioning system is realized.
In the embodiment, the system can realize more reliable positioning performance than the traditional IMU/GPS combined navigation based on the Kalman filter only by improving the fusion algorithm without changing hardware. Under the condition of lower cost, the precision and the robustness of the system are greatly improved, so that the application value of the IMU/GPS combined navigation system is improved.
It should be understood that the above-described embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. Any modification, equivalent replacement, and improvement made within the spirit and principle of the present invention should be included in the protection scope of the claims of the present invention.

Claims (10)

1. An IMU/GPS combined navigation method capable of sensitively detecting faults is characterized in that: the method comprises the following steps:
s1: projecting the GPS measurement information under the WGS84 coordinate system to a UTM plane by adopting a UTM projection algorithm;
s2: introducing a chi-square test algorithm, establishing a GPS fault detection model based on a pre-integration theory, and taking a pre-integration value based on the IMU as an expected value of the pre-integration value based on the GPS; converting the deviation of the IMU-based pre-integration value and the GPS-based pre-integration value into a chi-square value for reflecting the abnormal degree of the GPS measurement;
s3: when the system detects the latest GPS measurement abnormality through a chi-square checking algorithm, firstly eliminating the GPS measurement point, and using IMU measurement propagation as system state estimation at the moment;
s4: in order to further improve the robustness of the IMU/GPS fusion system, an M-estimator function is adopted to reduce the influence of GPS abnormal measurement on the output result of the system;
s5: and (3) adopting factor graph optimization to construct a joint optimization framework, fusing the measurement information of the IMU and the GPS, and realizing the IMU/GPS integrated navigation method.
2. The IMU/GPS combination navigation method capable of sensitively detecting a malfunction according to claim 1, wherein: step S1, specifically, defining coordinates of a point in WGS84 coordinate system as (L, B), where L is longitude and B is latitude; projecting the point to an (x, y) location on the UTM plane; the equator radius of the earth is given as a, the first eccentricity is given as e, and the second eccentricity is given as e'; the origin of the WGS84 coordinate system is set to (L)00); based on the UTM projection algorithm, (L, B) translates to the corresponding (x, y) as follows:
Figure FDA0003138913640000021
wherein T, C, A, M and N are intermediate variables.
3. The IMU/GPS combination navigation method capable of sensitively detecting a malfunction according to claim 2, wherein: aligning the GPS measurements at different times with the IMU measurements using linear interpolation; let the time stamp corresponding to the k +1 IMU measurement be tIPrevious GPS measurement data G of k +1 IMU measurement1Corresponding time stamp is tG1The last GPS measurement data G of the k +1 IMU measurement2Corresponding time stamp is tG2(ii) a The process of linear interpolation can be expressed as:
Figure FDA0003138913640000022
wherein ,Gk+1Represents the k +1 th GPS measurement data;
to this end, the GPS measurements are aligned with the time stamps of the IMU measurements.
4. The IMU/GPS combined navigation method capable of sensitively detecting faults as claimed in claim 3, wherein: step S2, the specific steps are as follows:
s201: defining a measurement model of angular velocity and acceleration of the IMU;
s202: the velocity, displacement and rotation of the system in the world coordinate system at time t + Δ t are inferred using a measurement model of the IMU:
s203: calculating a measurement value of a pre-integration value of the system between a time i to a time j based on the IMU measurement according to equation (4);
s204: according to the formula (4), calculating and obtaining an estimated value of a pre-integration value of the system between the time i and the time j based on GPS measurement;
s205: the residual value r is defined as IMU-based on the residual Chi-Square test
Figure FDA0003138913640000031
And based on GPS
Figure FDA0003138913640000032
The difference between them; r reflects the degree of consistency of the GPS measurements and IMU measurements, such as:
Figure FDA0003138913640000033
if the GPS measurement is normal, r obeys zero Gaussian distribution, otherwise r contains offset; the decision variable s is defined as the sum of the squared residuals weighted by the correlation covariance matrix a:
Figure FDA0003138913640000034
decision variable s obeys χ of 3 degrees of freedom2Distributing;
s206: based on chi-square test, the GPS anomaly point is judged by the following criteria:
Figure FDA0003138913640000035
5. the IMU/GPS combined navigation method capable of sensitively detecting faults as claimed in claim 4, wherein: in step S201, the expression of the measurement model is as follows:
Figure FDA0003138913640000036
wherein ,
Figure FDA0003138913640000037
and
Figure FDA0003138913640000038
measuring an original IMU under a base coordinate system at the moment t; omegat and atAre the true values of angular velocity and acceleration; btRepresents a bias; n istRepresenting white gaussian noise;
Figure FDA0003138913640000039
rotation from a world coordinate system W to a base coordinate system B at the moment t; g is the gravity vector in the world coordinate system.
6. The IMU/GPS combined navigation method capable of sensitively detecting faults as claimed in claim 5, wherein: in step S202, the expressions for speed, displacement, and rotation are as follows:
Figure FDA00031389136400000310
in the formula ,
Figure FDA00031389136400000311
7. the IMU/GPS combined navigation method capable of sensitively detecting faults as claimed in claim 6, wherein: in step S203, the expression based on the measured value of the pre-integrated value of the IMU measurement calculation system from time i to time j is as follows:
Figure FDA0003138913640000041
in the formula ,
Figure FDA0003138913640000042
representing a linear acceleration vector in the IMU measurement data;
Figure FDA0003138913640000043
representing the static bias of the accelerometer at time k,
Figure FDA0003138913640000044
Representing the gaussian white noise of the accelerometer at time k;
8. the IMU/GPS combination navigation method capable of sensitively detecting a malfunction according to claim 7, wherein: in step S204, an expression for calculating an estimated value of the pre-integration value of the system between time i and time j based on GPS measurement is as follows:
Figure FDA0003138913640000045
9. the IMU/GPS combination navigation method capable of sensitively detecting a malfunction according to claim 8, wherein: specifically, when GPS measurement is used as a constraint item to participate in pose optimization, a Huber loss function rho (-) is adopted to replace a Mahalanobis distance function of a target optimization object; the target optimization function is abstractly expressed as follows:
Figure FDA0003138913640000046
10. the IMU/GPS combination navigation method capable of sensitively detecting a malfunction according to claim 9, wherein: in order to realize the joint optimization of the formula (10), so that the system fuses the measurement information of the IMU and the GPS, a joint optimization framework is constructed by adopting factor graph optimization; under a combined optimization framework, the system generates a new key frame every time GPS measurement information is returned; the key frame stores the pose (x, y, z) and posture (roll, pitch, yaw) information of the system under the current timestamp t, and the key frame is converted into nodes to be added into the factor graph as an optimization object;
meanwhile, GPS measurement data is converted into a unitary factor and added into the factor graph, and IMU data between two adjacent GPS measurements is converted into an IMU pre-integration factor and added into the factor graph;
after the factor graph optimization framework is constructed, nonlinear optimization is carried out on the constructed factor graph by using a GTSAM factor graph optimization library, optimal estimation of the latest key frame is obtained through solving, and the IMU/GPS combined navigation method is realized.
CN202110729883.XA 2021-06-29 2021-06-29 IMU/GPS integrated navigation method capable of sensitively detecting faults Active CN113432604B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110729883.XA CN113432604B (en) 2021-06-29 2021-06-29 IMU/GPS integrated navigation method capable of sensitively detecting faults

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110729883.XA CN113432604B (en) 2021-06-29 2021-06-29 IMU/GPS integrated navigation method capable of sensitively detecting faults

Publications (2)

Publication Number Publication Date
CN113432604A true CN113432604A (en) 2021-09-24
CN113432604B CN113432604B (en) 2023-05-19

Family

ID=77757837

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110729883.XA Active CN113432604B (en) 2021-06-29 2021-06-29 IMU/GPS integrated navigation method capable of sensitively detecting faults

Country Status (1)

Country Link
CN (1) CN113432604B (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113791435A (en) * 2021-11-18 2021-12-14 智道网联科技(北京)有限公司 GNSS signal abnormal value detection method and device, electronic equipment and storage medium
CN114895196A (en) * 2022-07-13 2022-08-12 深圳市威特利电源有限公司 New energy battery fault diagnosis method based on artificial intelligence
CN114915913A (en) * 2022-05-17 2022-08-16 东南大学 UWB-IMU combined indoor positioning method based on sliding window factor graph

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080091351A1 (en) * 2006-10-17 2008-04-17 Takayuki Hoshizaki GPS accuracy adjustment to mitigate multipath problems for MEMS based integrated INS/GPS navigation systems
US20080167814A1 (en) * 2006-12-01 2008-07-10 Supun Samarasekera Unified framework for precise vision-aided navigation
CN109471146A (en) * 2018-12-04 2019-03-15 北京壹氢科技有限公司 A kind of self-adapted tolerance GPS/INS Combinated navigation method based on LS-SVM
CN109813299A (en) * 2019-03-06 2019-05-28 南京理工大学 A kind of integrated navigation information fusion method based on Interactive Multiple-Model
CN109813342A (en) * 2019-02-28 2019-05-28 北京讯腾智慧科技股份有限公司 A kind of fault detection method and system of inertial navigation-satellite combined guidance system
CN110296701A (en) * 2019-07-09 2019-10-01 哈尔滨工程大学 Inertia and satellite combined guidance system gradation type failure recall fault-tolerance approach
CN111024124A (en) * 2019-12-25 2020-04-17 南京航空航天大学 Multi-sensor information fusion combined navigation fault diagnosis method
CN111044051A (en) * 2019-12-30 2020-04-21 星际(江苏)航空科技有限公司 Fault-tolerant integrated navigation method for composite wing unmanned aerial vehicle
CN111337020A (en) * 2020-03-06 2020-06-26 兰州交通大学 Factor graph fusion positioning method introducing robust estimation
CN112946711A (en) * 2021-01-29 2021-06-11 中国人民解放军国防科技大学 Navigation method of GNSS/INS integrated navigation system

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080091351A1 (en) * 2006-10-17 2008-04-17 Takayuki Hoshizaki GPS accuracy adjustment to mitigate multipath problems for MEMS based integrated INS/GPS navigation systems
US20080167814A1 (en) * 2006-12-01 2008-07-10 Supun Samarasekera Unified framework for precise vision-aided navigation
CN109471146A (en) * 2018-12-04 2019-03-15 北京壹氢科技有限公司 A kind of self-adapted tolerance GPS/INS Combinated navigation method based on LS-SVM
CN109813342A (en) * 2019-02-28 2019-05-28 北京讯腾智慧科技股份有限公司 A kind of fault detection method and system of inertial navigation-satellite combined guidance system
CN109813299A (en) * 2019-03-06 2019-05-28 南京理工大学 A kind of integrated navigation information fusion method based on Interactive Multiple-Model
CN110296701A (en) * 2019-07-09 2019-10-01 哈尔滨工程大学 Inertia and satellite combined guidance system gradation type failure recall fault-tolerance approach
CN111024124A (en) * 2019-12-25 2020-04-17 南京航空航天大学 Multi-sensor information fusion combined navigation fault diagnosis method
CN111044051A (en) * 2019-12-30 2020-04-21 星际(江苏)航空科技有限公司 Fault-tolerant integrated navigation method for composite wing unmanned aerial vehicle
CN111337020A (en) * 2020-03-06 2020-06-26 兰州交通大学 Factor graph fusion positioning method introducing robust estimation
CN112946711A (en) * 2021-01-29 2021-06-11 中国人民解放军国防科技大学 Navigation method of GNSS/INS integrated navigation system

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
CHUN YANG等: "Multi-Sensor Fusion with Interaction Multiple Model and Chi-Square Test Tolerant Filter", 《SENSORS》 *
XIAO-KAI WEI等: "Vehicle INS/Odometer Integrated Navigation Algorithm Based on Factor Graph", 《2019 IEEE INTERNATIONAL CONFERENCE ON UNMANNED SYSTEMS (ICUS)》 *
徐昊玮: "GNSS/INS/LiDAR多源组合导航系统关键技术研究", 《中国博士学位论文全文数据库信息科技辑(月刊)》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113791435A (en) * 2021-11-18 2021-12-14 智道网联科技(北京)有限公司 GNSS signal abnormal value detection method and device, electronic equipment and storage medium
CN114915913A (en) * 2022-05-17 2022-08-16 东南大学 UWB-IMU combined indoor positioning method based on sliding window factor graph
CN114895196A (en) * 2022-07-13 2022-08-12 深圳市威特利电源有限公司 New energy battery fault diagnosis method based on artificial intelligence
CN114895196B (en) * 2022-07-13 2022-10-25 深圳市威特利电源有限公司 New energy battery fault diagnosis method based on artificial intelligence

Also Published As

Publication number Publication date
CN113432604B (en) 2023-05-19

Similar Documents

Publication Publication Date Title
CN113432604B (en) IMU/GPS integrated navigation method capable of sensitively detecting faults
Chiang et al. Assessment for INS/GNSS/odometer/barometer integration in loosely-coupled and tightly-coupled scheme in a GNSS-degraded environment
Wang et al. Constrained MEMS-based GNSS/INS tightly coupled system with robust Kalman filter for accurate land vehicular navigation
US9031782B1 (en) System to use digital cameras and other sensors in navigation
CN113074739B (en) UWB/INS fusion positioning method based on dynamic robust volume Kalman
US20180031387A1 (en) State estimation for aerial vehicles using multi-sensor fusion
CN111795686B (en) Mobile robot positioning and mapping method
CN112073909B (en) UWB (ultra wide band)/MEMS (micro-electromechanical systems) combination based UWB base station position error compensation method
CN109471146B (en) Self-adaptive fault-tolerant GPS/INS integrated navigation method based on LS-SVM
CN112923919B (en) Pedestrian positioning method and system based on graph optimization
CN109827545B (en) Online inclination angle measuring method based on double MEMS accelerometers
CN112946681B (en) Laser radar positioning method fusing combined navigation information
CN111044051A (en) Fault-tolerant integrated navigation method for composite wing unmanned aerial vehicle
CN113375666A (en) Sensor fusion positioning system and method
CN110672095A (en) Pedestrian indoor autonomous positioning algorithm based on micro inertial navigation
CN109764870A (en) Carrier initial heading evaluation method based on transformation estimator modeling scheme
CN115435779A (en) Intelligent body pose estimation method based on GNSS/IMU/optical flow information fusion
CN117268372B (en) INS/GNSS integrated navigation method and system integrating magnetic navigation information
Wang et al. GIVE: A tightly coupled RTK-inertial–visual state estimator for robust and precise positioning
Chu et al. Performance comparison of tight and loose INS-Camera integration
Maklouf et al. Performance evaluation of GPS\INS main integration approach
CN112540345A (en) Dual-model positioning method and system for detecting UWB quality based on Gaussian distribution
CN116482735A (en) High-precision positioning method for inside and outside of limited space
CN116203611A (en) Cableway bracket deformation and posture monitoring method based on GNSS-IMU
Nassar et al. Accurate INS/GPS positioning with different inertial systems using various algorithms for bridging GPS outages

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