CN113432604B - IMU/GPS integrated navigation method capable of sensitively detecting faults - Google Patents

IMU/GPS integrated navigation method capable of sensitively detecting faults Download PDF

Info

Publication number
CN113432604B
CN113432604B CN202110729883.XA CN202110729883A CN113432604B CN 113432604 B CN113432604 B CN 113432604B CN 202110729883 A CN202110729883 A CN 202110729883A CN 113432604 B CN113432604 B CN 113432604B
Authority
CN
China
Prior art keywords
gps
imu
measurement
time
optimization
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.)
Active
Application number
CN202110729883.XA
Other languages
Chinese (zh)
Other versions
CN113432604A (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 integrated navigation method capable of sensitively detecting faults, which comprises the following steps: adopting a UTM projection algorithm to project GPS measurement information under a WGS84 coordinate system to a UTM plane; introducing a chi-square checking algorithm, establishing a GPS fault detection model based on a pre-integration theory, wherein the pre-integration value based on the IMU is taken as an expected value of the pre-integration value based on the GPS; converting the deviation of the pre-integral value based on the IMU and the pre-integral value based on the GPS into a chi-square value for reflecting the degree of abnormality of the GPS measurement; when the system detects the latest GPS measurement abnormality through a chi-square checking algorithm, firstly removing the GPS measurement points, and using IMU measurement propagation as system state estimation at the moment; adopting an M-estimator function to reduce the influence of GPS abnormal measurement on the output result of the system; and constructing a combined optimization framework by adopting factor graph optimization, and fusing measurement information of the IMU and the GPS to realize the IMU/GPS combined navigation method.

Description

IMU/GPS integrated navigation method capable of sensitively detecting faults
Technical Field
The invention relates to the technical field of positioning navigation, in particular to an IMU/GPS integrated navigation method capable of sensitively detecting faults.
Background
At present, the IMU/GPS integrated navigation scheme is single, and mainly adopts an extended Kalman filtering algorithm or a nonlinear optimization algorithm to fuse the IMU and GPS measurements, and the specific scheme is as follows:
such as Chinese patent publication No.: CN108709552a, publication date: 2018-10-26, provides an IMU and GPS tight combination method based on MEMS, which adopts a 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 perform initial alignment, and performs calculation on IMU measurement data to obtain IMU navigation information; calculating Doppler frequency by using IMU navigation information, providing the Doppler frequency for GPS to carry out capturing and tracking assistance, and improving the performance of the GPS; and establishing a dynamic error model, carrying out data fusion on 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 feeding back errors, and outputting fused and corrected optimal position, speed and posture information. The scheme uses an IMU measurement propagation equation as a system prediction step, and propagates the system state based on Euler integral or median integral. Then the GPS measurement is used as an external observation quantity, and the UTM projection algorithm is used as an observation equation. Based on a Kalman filter algorithm, the GPS observed quantity is used for correcting the state predicted by the system, so that the fusion positioning of the IMU and the GPS is realized.
Such as Chinese patent publication No.: CN111121767a, publication date: 2020-05-08, a combined positioning method of robot vision inertial navigation integrating GPS is disclosed, comprising extracting and matching left and right images of a binocular camera with front and rear image feature points, and calculating the relative pose of the feature point three-dimensional coordinates and image frames; selecting a key frame in an image stream, creating a sliding window, and adding the key frame into the sliding window; calculating a visual re-projection error, and combining an IMU pre-integral residual error and a zero offset residual error into a combined pose estimation residual error; performing nonlinear optimization on the joint pose estimation residual error by using an L-M method to obtain an optimized visual inertial navigation (VIO) robot pose; if GPS data are present at the current moment, performing self-adaptive robust Kalman filtering on the GPS position data and the VIO pose estimation data to obtain the final robot pose; if the GPS data is not available, the VIO pose data is used for replacing the final pose data. The scheme is as follows: in the nonlinear optimization algorithm, state quantity of a system is used as a variable to be optimized, and IMU measurement and GPS measurement are used as optimization constraint items. Through the measurement equation, the IMU measurement and the GPS measurement are converted into parameters of a cost function, respectively, and the 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 state quantity of the system is the fusion positioning result of the IMU and the GPS.
The above-listed fusion method of IMU/GPS integrated navigation lacks a reliable sensor anomaly detection method. The GPS system may provide centimeter level positioning services, but in GPS failure scenarios (e.g., tunnels, building dense areas, canyons, and other environments where GPS signals are obscured), GPS receivers have difficulty capturing valid GPS signals resulting in GPS measurements having outliers. The GPS anomaly measurement point tends to be far from the actual location and once it is introduced into the fusion system will severely distort the positioning system output. GPS outlier detection is therefore a very important step, but current IMU/GPS integrated 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 smaller than a threshold value, the GPS is considered to be abnormal. However, due to multipath effects (commonly found in tunnel entrances or building-dense areas), it is often the case that the number of satellites searched is significantly greater than a threshold, but at this point the GPS measurements have suffered from large scale anomalies.
Disclosure of Invention
The invention provides the IMU/GPS integrated navigation method capable of sensitively detecting faults in order to solve the defects in the prior art.
In order to achieve the above purpose of the present invention, the following technical scheme is adopted:
an IMU/GPS integrated navigation method capable of sensitively detecting faults, comprising the following steps:
s1: adopting a UTM projection algorithm to project GPS measurement information under a WGS84 coordinate system to a UTM plane;
s2: introducing a chi-square checking algorithm, establishing a GPS fault detection model based on a pre-integration theory, wherein the pre-integration value based on the IMU is taken as an expected value of the pre-integration value based on the GPS; converting the deviation of the pre-integral value based on the IMU and the pre-integral value based on the GPS into a chi-square value for reflecting the degree of abnormality of the GPS measurement;
s3: when the system detects the latest GPS measurement abnormality through a chi-square checking algorithm, firstly removing the GPS measurement points, 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 constructing a combined optimization framework by adopting factor graph optimization, and fusing the measurement information of the IMU and the GPS.
Preferably, in step S1, specifically, a coordinate of a point in the WGS84 coordinate system is defined as (L, B), wherein L is longitude and B is latitude; projecting the point to an (x, y) position on the UTM plane; given an equatorial radius of the earth of a, a first eccentricity of e and a second eccentricity of e'; the origin of the WGS84 coordinate system is set to (L 0 0); based on the UTM projection algorithm, (L, B) is converted to the corresponding (x, y) as follows:
Figure BDA0003138913650000031
wherein T, C, A, M and N are all intermediate variables.
Further, alignment of GPS measurements at different times with the IMU measurements using linear interpolation; let the time stamp corresponding to the k+1th IMU measurement be t I Previous GPS measurement data G measured by the (k+1) th IMU 1 The corresponding time stamp is
Figure BDA0003138913650000032
The (k+1) th IMU measured subsequent GPS measurement data G 2 The corresponding timestamp is->
Figure BDA0003138913650000033
The saidThe process of linear interpolation of (2) can be expressed as:
Figure BDA0003138913650000034
wherein ,Gk+1 Represents the kth+1th GPS measurement data;
to this end, the GPS measurements are aligned with the time stamps of the IMU measurements.
Still further, step S2, specifically comprises the following steps:
s201: defining a measurement model of angular velocity and acceleration of the IMU;
s202: using the measurement model of the IMU to infer the speed, displacement and rotation of the system in the world coordinate system at time t+Δt:
s203: calculating a measurement value of a pre-integration value of the system between the time i and the time j based on the IMU measurement according to the formula (4);
s204: according to formula (4), calculating an estimated value of a pre-integrated value of the system from time i to time j based on GPS measurement;
s205: based on the residual chi-square test, the residual value r is defined as IMU-based
Figure BDA0003138913650000041
And GPS based->
Figure BDA0003138913650000042
A difference between them; r reflects the degree of consistency of the GPS measurements and IMU measurements, such as:
Figure BDA0003138913650000043
r follows a zero gaussian distribution if the GPS measurement is normal, otherwise r contains a bias; the decision variable s is defined as the sum of squares of residuals weighted by the correlation covariance matrix a:
Figure BDA0003138913650000044
decision variable s obeys χ of 3 degrees of freedom 2 Distribution;
s206: based on chi-square test, GPS outliers are judged by the following criteria:
Figure BDA0003138913650000045
still further, in step S201, the measurement model expression is as follows:
Figure BDA0003138913650000046
wherein ,
Figure BDA0003138913650000047
and />
Figure BDA0003138913650000048
The original IMU measurement is performed under a matrix coordinate system at the moment t; omega t and at Is the true value of angular velocity and acceleration; b t Representing the bias; n is n t Representing gaussian white noise; />
Figure BDA0003138913650000049
Is the rotation from the world coordinate system W to the matrix coordinate system B at the moment t; g is the gravity vector in 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, an expression of a measured value of the pre-integration value between the time i and the 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
A Gaussian white noise of the accelerometer at time k is represented;
still further, in step S204, an expression for calculating an estimated value of a pre-integrated value of the system between the time i and the time j based on the GPS measurement is as follows:
Figure BDA0003138913650000055
still further, specifically, when the GPS measurement is taken as a constraint term to participate in pose optimization, a Huber loss function ρ (·) is adopted to replace a mahalanobis distance function of the target optimization object; the target optimization function abstract expression is 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, and a factor graph optimization is adopted to construct a joint optimization framework; under the combined optimization framework, each time GPS measurement information is returned, the system generates a new key frame; the key frame stores information of the pose (x, y, z) and the posture (roll, pitch, yaw) of the system under the current time stamp t, and the key frame is converted into nodes and added into a factor graph to serve as an optimization object;
meanwhile, GPS measurement data are converted into unitary factors and added into factor graphs, and IMU data between two adjacent GPS measurements are converted into IMU pre-integration factors and added into factor graphs;
after the factor graph optimization framework is built, nonlinear optimization is carried out on the built factor graph by using a GTSAM factor graph optimization library, and the optimal estimation of the latest key frame is obtained by solving, so that the IMU/GPS integrated navigation method is realized.
The beneficial effects of the invention are as follows:
the invention utilizes the inherent stability, reliability and concealment of the IMU to make the IMU measurement value measure and judge the abnormal condition of GPS measurement. More specifically, the invention creatively utilizes the IMU pre-integration theory and the chi-square test theory to construct a theoretical formula capable of describing the relation between the IMU measurement and the GPS measurement, thereby effectively avoiding the system from fusing the GPS abnormal measurement. Meanwhile, a nonlinear optimization function under an M-escimer improvement factor graph optimization framework is introduced, so that the influence of GPS anomaly measurement on a system is further reduced. In addition, as the system is considered to be a low-cost IMU at the beginning of system modeling, the invention is very suitable for a low-cost IMU/GPS integrated navigation system, and the system can realize more reliable positioning performance than the traditional IMU/GPS integrated navigation based on a Kalman filter through the improvement of a fusion algorithm under the condition of not changing hardware.
Drawings
FIG. 1 is a flow chart of the steps of the IMU/GPS integrated navigation method of 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 integrated navigation method capable of sensitively detecting a fault, the method includes the steps of:
s1: because the positioning information acquired by the GPS is longitude and latitude data under the WGS84 geographic coordinate system, and the units of the inertial measurement data acquired by the IMU are meters and radians. The data obtained by the two methods are different in dimension, and data fusion is difficult to carry out. Therefore, the UTM projection algorithm is adopted to project GPS measurement information under the WGS84 coordinate system to a UTM plane;
in a specific embodiment, a point in the WGS84 coordinate system is defined as (L, B), where L is longitude and B is latitude; projecting the point to an (x, y) position on the UTM plane; given an equatorial radius of the earth of a, a first eccentricity of e and a second eccentricity of e'; the origin of the WGS84 coordinate system is set to (L 0 0); based on the UTM projection algorithm, (L, B) is converted to the corresponding (x, y) as follows:
Figure BDA0003138913650000071
wherein T, C, A, M and N are all intermediate variables.
By (1), the measurement data of the GPS is projected onto the UTM plane, and the dimension of the data is consistent with the IMU. Because the IMU and GPS measurements are asynchronous, measurement time errors can affect the accuracy of the information fusion system. For example, since gps_1 data is returned at 3s, imu_1 data is returned at 4s, and gps_2 data is returned at 5 seconds, in order to unify the time stamps of both data, gps_1 and gps_2 are linearly interpolated to estimate what the GPS data should be at 4 seconds.
Specifically, alignment of GPS measurements at different times with IMU measurements using linear interpolation; let the time stamp corresponding to the k+1th IMU measurement be t I Previous GPS measurement data G measured by the (k+1) th IMU 1 The corresponding time stamp is
Figure BDA0003138913650000072
The (k+1) th IMU measured subsequent GPS measurement data G 2 The corresponding timestamp is->
Figure BDA0003138913650000073
The linear interpolation process can be expressed as: />
Figure BDA0003138913650000074
wherein ,Gk+1 Represents the kth+1th 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 outliers (GPS outliers) are introduced into the fusion system, the system output results are often severely distorted and even the system collapses, and in order to quickly and effectively detect the GPS outliers, the IMU measurements are used as reference objects for the GPS outliers. Although the accumulated error of the IMU integral is unbounded, it is considered in the present system that no abnormal measurements occur due to its inherent stability, concealment. In order to avoid large-scale errors generated by IMU long-time integration, the reliability degree of GPS measurement is evaluated by adopting an IMU pre-integration theory.
Introducing a chi-square checking algorithm, establishing a GPS fault detection model based on a pre-integration theory, wherein the pre-integration value based on the IMU is taken as an expected value of the pre-integration value based on the GPS; converting the deviation of the pre-integral value based on the IMU and the pre-integral value based on the GPS into a chi-square value for reflecting the degree of abnormality of the GPS measurement;
step S2, the specific steps are as follows:
s201: a measurement model of the angular velocity and acceleration of the IMU is defined, expressed as follows:
Figure BDA0003138913650000081
wherein ,
Figure BDA0003138913650000082
and />
Figure BDA0003138913650000083
The original IMU measurement is performed under a matrix coordinate system at the moment t; omega t and at Is the true value of angular velocity and acceleration; b t Representing the bias; n is n t Representing gaussian white noise; />
Figure BDA0003138913650000084
Is the rotation from the world coordinate system W to the matrix coordinate system B at the moment t; g is the gravity vector in world coordinate system.
S202: using the measurement model of the IMU to infer the speed, displacement and rotation of the system in the world coordinate system at time t+Δt, the expression is as follows:
Figure BDA0003138913650000085
in the formula,
Figure BDA0003138913650000086
s203: based on the measurement value of the pre-integration value of the IMU measurement calculation system between the time i and the time j according to the expression (4), 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 system's pre-integration value between time i and time j is calculated based on the GPS measurement, as follows:
Figure BDA0003138913650000091
s205: note that Δv ij and Δpij Neither of the actual velocity nor displacement variations, the most important role of equations (5) and (6) is to separate the IMU measurements 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, so that IMU measurement and GPS measurement are comparable. In addition, the pre-integration value is only related to the IMU measurement from time i to time j, which avoids the need to re-integrate all IMU measurements each time by the formula (4), and greatly reduces the system calculation amount.
By equation (6), the GPS measurement can estimate the pre-integration value from time i to time j. Based on the residual chi-square test, the residual value r is defined as IMU-based
Figure BDA0003138913650000092
And GPS based->
Figure BDA0003138913650000093
A difference between them; r reflects the degree of consistency of the GPS measurements and IMU measurements, such as:
Figure BDA0003138913650000094
r follows a zero gaussian distribution if the GPS measurement is normal, otherwise r contains a bias; the decision variable s is defined as the sum of squares of residuals weighted by the correlation covariance matrix a:
Figure BDA0003138913650000095
decision variable s obeys χ of 3 degrees of freedom 2 Distribution;
s206: based on chi-square test, GPS outliers are judged by the following criteria:
Figure BDA0003138913650000096
s3: when the system detects the latest GPS measurement abnormality through a chi-square checking algorithm, firstly removing the GPS measurement points, and using IMU measurement propagation as system state estimation at the moment;
s4: in practical applications, 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 the GPS measurement is taken as a constraint term to participate in pose optimization, the Huber loss function ρ (·) is used instead of the mahalanobis distance function of the target optimization object. The target optimization function abstract expression is as follows:
Figure BDA0003138913650000097
s5: and constructing a combined optimization framework by adopting factor graph optimization, 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, and a factor graph optimization is adopted to construct a joint optimization framework; under the combined optimization framework, each time GPS measurement information is returned, the system generates a new key frame; the key frame stores information of the pose (x, y, z) and the posture (roll, pitch, yaw) of the system under the current time stamp t, and the key frame is converted into nodes and added into a factor graph to serve as an optimization object;
meanwhile, GPS measurement data are converted into unitary factors and added into factor graphs, and IMU data between two adjacent GPS measurements are converted into IMU pre-integration factors and added into factor graphs;
after the factor graph optimization framework is built, nonlinear optimization is carried out on the built factor graph by using a GTSAM factor graph optimization library, and the optimal estimation of the latest key frame is obtained by solving, so that the IMU/GPS integrated navigation method is realized.
In the embodiment, firstly, the GPS measurement information under the WGS84 coordinate system is projected to a UTM plane through a UTM projection algorithm, so that the dimensions of the GPS measurement (longitude and latitude) and the IMU measurement information (meter) are consistent. The idea of chi-square test is introduced, and a GPS fault detection model based on a pre-integration theory is established. The IMU measurement and the GPS measurement can both independently calculate a pre-integration value, which will be the expected value of the GPS-based pre-integration value based on the IMU, and the deviation between them will be converted into a chi-square value for reflecting the degree of abnormality of the GPS measurement. In order to further improve the robustness of the IMU/GPS fusion system, an M-escriptor algorithm is adopted to reduce the influence of GPS abnormal measurement on the output result of the system. Finally, the factor graph optimization is applied to fuse the measurement information of the IMU and the GPS, so that a positioning system with high frequency, high precision and high robustness is realized.
In the embodiment, the system can realize more reliable positioning performance than the traditional IMU/GPS integrated navigation based on a Kalman filter through the improvement of a fusion algorithm under the condition of not changing hardware. Under the condition of lower cost, the accuracy and the robustness of the system are greatly improved, so that the application value of the IMU/GPS integrated navigation system is improved.
It is to be understood that the above examples of the present invention are provided by way of illustration only and not by way of limitation of the embodiments of the present invention. Any modification, equivalent replacement, improvement, etc. which come within the spirit and principles of the invention are desired to be protected by the following claims.

Claims (7)

1. An IMU/GPS integrated navigation method capable of sensitively detecting faults is characterized by comprising the following steps of: the method comprises the following steps:
s1: adopting a UTM projection algorithm to project GPS measurement information under a WGS84 coordinate system to a UTM plane;
s2: introducing a chi-square checking algorithm, establishing a GPS fault detection model based on a pre-integration theory, and taking the 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 pre-integral value based on the IMU and the pre-integral value based on the GPS into a chi-square value for reflecting the degree of abnormality of the GPS measurement;
s3: when the system detects the latest GPS measurement abnormality through a chi-square checking algorithm, firstly removing the GPS measurement points, 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: adopting factor graph optimization to construct a combined optimization framework, and fusing measurement information of the IMU and the GPS to realize an IMU/GPS combined navigation method;
step S2, the specific steps are as follows:
s201: defining a measurement model of angular velocity and acceleration of the IMU;
s202: using the measurement model of the IMU to infer the speed, displacement and rotation of the system in the world coordinate system at time t+Δt:
s203: calculating a measurement value of a pre-integration value of the system between the time i and the time j based on the IMU measurement according to the formula (4);
s204: according to formula (4), calculating an estimated value of a pre-integrated value of the system from time i to time j based on GPS measurement;
s205: based on the residual chi-square test, the residual value r is defined as IMU-based
Figure FDA0004153818560000011
And GPS based->
Figure FDA0004153818560000012
A difference between them; r reflects the degree of consistency of the GPS measurements and IMU measurements, such as:
Figure FDA0004153818560000013
r follows a zero gaussian distribution if the GPS measurement is normal, otherwise r contains a bias; the decision variable s is defined as the sum of squares of residuals weighted by the correlation covariance matrix a:
Figure FDA0004153818560000014
decision variable s obeys χ of 3 degrees of freedom 2 Distribution;
s206: based on chi-square test, GPS outliers are judged by the following criteria:
Figure FDA0004153818560000021
in step S201, the measurement model expression is as follows:
Figure FDA0004153818560000022
wherein ,
Figure FDA0004153818560000023
and />
Figure FDA0004153818560000024
The original IMU measurement is performed under a matrix coordinate system at the moment t; omega t and at Is the true value of angular velocity and acceleration; b t Representing the bias; n is n t Representing gaussian white noise; />
Figure FDA0004153818560000025
Is the rotation from the world coordinate system W to the matrix coordinate system B at the moment t; g is the gravity vector in world coordinate system;
step S202, velocity v t+Δt Displacement p t+Δt And rotating R t+Δt The expression of (2) is as follows:
Figure FDA0004153818560000026
in the formula,
Figure FDA0004153818560000027
2. the IMU/GPS integrated navigation method capable of sensitively detecting a fault according to claim 1, characterized in that: step S1, specifically, defining a point coordinate under a WGS84 coordinate system as (L, B), wherein L is longitude and B is latitude; projecting the point to an (x, y) position on the UTM plane; given an equatorial radius of the earth of a, a first eccentricity of e and a second eccentricity of e'; the origin of the WGS84 coordinate system is set to (L 0 0); based on the UTM projection algorithm, (L, B) is converted to the corresponding (x, y) as follows:
Figure FDA0004153818560000031
wherein T, C, A, M and N are all intermediate variables.
3. The IMU/GPS integrated navigation method capable of sensitively detecting a fault according to claim 2, characterized in that: aligning GPS measurements at different times with IMU measurements using linear interpolation; let the time stamp corresponding to the k+1th IMU measurement be t I Previous GPS measurement data G measured by the (k+1) th IMU 1 The corresponding time stamp is
Figure FDA0004153818560000036
The (k+1) th IMU measured subsequent GPS measurement data G 2 The corresponding timestamp is->
Figure FDA0004153818560000037
The linear interpolation process can be expressed as:
Figure FDA0004153818560000032
wherein ,Gk+1 Represents the kth+1th GPS measurement data;
to this end, the GPS measurements are aligned with the time stamps of the IMU measurements.
4. The IMU/GPS integrated navigation method capable of sensitively detecting a fault according to claim 1, characterized in that: in step S203, the expression of the measurement value based on the pre-integration value of the IMU measurement calculation system between the time i and the time j is as follows:
Figure FDA0004153818560000033
in the formula,
Figure FDA0004153818560000034
representing a linear acceleration vector in the IMU measurement data; />
Figure FDA0004153818560000035
Representing the static bias of the accelerometer at time k,
Figure FDA0004153818560000041
Representing the gaussian white noise of the accelerometer at time k.
5. The IMU/GPS integrated navigation method capable of sensitively detecting a fault according to claim 1, characterized in that: step S204, an expression for obtaining an estimated value of the pre-integration value of the system between the time i and the time j based on the GPS measurement is as follows:
Figure FDA0004153818560000042
6. the IMU/GPS integrated navigation method capable of sensitively detecting a fault according to claim 5, characterized in that: 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 abstract expression is as follows:
Figure FDA0004153818560000043
7. the IMU/GPS integrated navigation method capable of sensitively detecting a fault according to claim 6, characterized in that: 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, and a factor graph optimization is adopted to construct a joint optimization framework; under the combined optimization framework, each time GPS measurement information is returned, the system generates a new key frame; the key frame stores information of the pose (x, y, z) and the posture (roll, pitch, yaw) of the system under the current time stamp t, and the key frame is converted into nodes and added into a factor graph to serve as an optimization object;
meanwhile, GPS measurement data are converted into unitary factors and added into factor graphs, and IMU data between two adjacent GPS measurements are converted into IMU pre-integration factors and added into factor graphs;
after the factor graph optimization framework is built, nonlinear optimization is carried out on the built factor graph by using a GTSAM factor graph optimization library, and the optimal estimation of the latest key frame is obtained by solving, so that the IMU/GPS integrated 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 CN113432604A (en) 2021-09-24
CN113432604B true 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)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113791435B (en) * 2021-11-18 2022-04-05 智道网联科技(北京)有限公司 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
CN114895196B (en) * 2022-07-13 2022-10-25 深圳市威特利电源有限公司 New energy battery fault diagnosis method based on artificial intelligence

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7702459B2 (en) * 2006-10-17 2010-04-20 Alpine Electronics, Inc. GPS accuracy adjustment to mitigate multipath problems for MEMS based integrated INS/GPS navigation systems
US8174568B2 (en) * 2006-12-01 2012-05-08 Sri International Unified framework for precise vision-aided navigation

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
GNSS/INS/LiDAR多源组合导航***关键技术研究;徐昊玮;《中国博士学位论文全文数据库信息科技辑(月刊)》(第4期);第I136-51页 *
Multi-Sensor Fusion with Interaction Multiple Model and Chi-Square Test Tolerant Filter;Chun Yang等;《sensors》;第16卷(第11期);第1-25页 *
Vehicle INS/Odometer Integrated Navigation Algorithm Based on Factor Graph;Xiao-kai Wei等;《2019 IEEE International Conference on Unmanned Systems (ICUS)》;第216-220页 *

Also Published As

Publication number Publication date
CN113432604A (en) 2021-09-24

Similar Documents

Publication Publication Date Title
CN113432604B (en) IMU/GPS integrated navigation method capable of sensitively detecting faults
US10295365B2 (en) State estimation for aerial vehicles using multi-sensor fusion
Chiang et al. Assessment for INS/GNSS/odometer/barometer integration in loosely-coupled and tightly-coupled scheme in a GNSS-degraded environment
US9031782B1 (en) System to use digital cameras and other sensors in navigation
CN111795686B (en) Mobile robot positioning and mapping method
US6424915B1 (en) System for determining the heading and/or attitude of a body
US10757333B2 (en) Method for determining bias in an inertial measurement unit of an image acquisition device
CN112923919B (en) Pedestrian positioning method and system based on graph optimization
Jiang et al. An effective integrity monitoring scheme for GNSS/INS/vision integration based on error state EKF model
US11959749B2 (en) Mobile mapping system
CN114396943B (en) Fusion positioning method and terminal
Li et al. Multi-GNSS PPP/INS/Vision/LiDAR tightly integrated system for precise navigation in urban environments
CN113516692A (en) Multi-sensor fusion SLAM method and device
CN111044051A (en) Fault-tolerant integrated navigation method for composite wing unmanned aerial vehicle
CN115435779A (en) Intelligent body pose estimation method based on GNSS/IMU/optical flow information fusion
Caruso et al. An inverse square root filter for robust indoor/outdoor magneto-visual-inertial odometry
Wang et al. GIVE: A tightly coupled RTK-inertial–visual state estimator for robust and precise positioning
Luna et al. An indoor pedestrian positioning system based on inertial measurement unit and wireless local area network
Hu et al. Performance evaluation of stereo vision aided loosely coupled GNSS/SINS integration for land vehicle navigation in different urban environments
Ding et al. Time synchronization design for integrated positioning and georeferencing systems
Maklouf et al. Performance evaluation of GPS\INS main integration approach
Afia et al. A low-cost gnss/imu/visual monoslam/wss integration based on federated kalman filtering for navigation in urban environments
Gong et al. Airborne earth observation positioning and orientation by SINS/GPS integration using CD RTS smoothing
Shen et al. A Novel Factor Graph Framework for Tightly Coupled GNSS/INS Integration With Carrier-Phase Ambiguity Resolution
Wang et al. Development of a low-cost solution for GPS/gyro attitude determination

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