CN110988951A - Multi-source data fusion real-time navigation positioning method and system - Google Patents

Multi-source data fusion real-time navigation positioning method and system Download PDF

Info

Publication number
CN110988951A
CN110988951A CN201911241794.XA CN201911241794A CN110988951A CN 110988951 A CN110988951 A CN 110988951A CN 201911241794 A CN201911241794 A CN 201911241794A CN 110988951 A CN110988951 A CN 110988951A
Authority
CN
China
Prior art keywords
data
gnss
time
updating
kalman filtering
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201911241794.XA
Other languages
Chinese (zh)
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.)
China University of Geosciences Beijing
Original Assignee
China University of Geosciences Beijing
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 China University of Geosciences Beijing filed Critical China University of Geosciences Beijing
Priority to CN201911241794.XA priority Critical patent/CN110988951A/en
Publication of CN110988951A publication Critical patent/CN110988951A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • 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
    • 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/40—Correcting position, velocity or attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a multi-source data fusion real-time navigation positioning method and system. The method comprises the following steps: acquiring GNSS data, GNSS operation data broadcasted by an IGS, IMU data, magnetic intensity data and mileage data; calculating the current position and speed from the GNSS data and the GNSS operational data; initializing the position and the speed according to the current position and speed, and initializing the posture according to IMU data to obtain a pose initialization result; updating the inertial navigation state based on the pose initialization result to obtain a pose updating result; checking whether GNSS data, magnetic strength data or mileage data are acquired at each measuring moment of the inertial measuring unit, and selecting time updating or observation updating for Kalman filtering according to the checking result to obtain a parameter correction vector; and correcting the position and orientation updating result by using an error feedback theory based on the parameter correction vector to obtain navigation positioning information. The invention can improve the positioning precision in the signal unlocking environment.

Description

Multi-source data fusion real-time navigation positioning method and system
Technical Field
The invention relates to the field of navigation positioning, in particular to a multi-source data fusion real-time navigation positioning method and system.
Background
Currently, a Global Navigation Satellite System (GNSS) is an indispensable important technical means in the field of dynamic Navigation and location services, and a plurality of Navigation satellites distributed on different orbital planes are used to uninterruptedly broadcast and modulate radio carrier signals including ranging codes, Satellite orbits, Satellite clock parameters, time information and the like to any Global ground user, and a precise positioning algorithm is adopted to provide high-precision location, speed and time information for the user. Compared with the traditional positioning method, the satellite positioning system has the advantages of global coverage, all weather, real time, high precision, bounded error and the like.
However, as a radio navigation positioning means, GNSS has become widely used, and its own shortcomings become obvious. For example, when the GNSS is located in an environment where signal lock is easily lost, such as an urban canyon, a tunnel, an overpass, a tree shade, and the like, the positioning accuracy of the GNSS is greatly reduced, and even navigation positioning cannot be realized.
Disclosure of Invention
The invention aims to provide a multi-source data fusion real-time navigation positioning method and system, which can improve the positioning accuracy in a signal lock losing environment.
In order to achieve the purpose, the invention provides the following scheme:
a multi-source data fusion real-time navigation positioning method is applied to a multi-source data fusion real-time navigation positioning device, and the positioning device comprises: the system comprises a GNSS receiver, an inertia measurement unit, a magnetometer, a milemeter, a network module and an FPGA mainboard; the GNSS receiver, the inertia measurement unit, the magnetometer, the odometer and the network module are all connected with the FPGA mainboard;
the navigation positioning method comprises the following steps:
acquiring GNSS data acquired by the GNSS receiver, GNSS operation data broadcasted by an international GNSS service organization and received by the network module, IMU data measured by the inertial measurement unit, magnetic strength data measured by the magnetometer and mileage data measured by the milemeter;
calculating the current position and the current speed of the carrier from the GNSS data and the GNSS operation data according to a non-differential non-combination precise single-point positioning theory based on a least square theory and a sequential least square theory;
position initialization and speed initialization are achieved according to the current position and the current speed, posture initialization is achieved according to the IMU data, and a posture initialization result is obtained;
based on the pose initialization result, updating the inertial navigation state by using an inertial navigation system mechanical arrangement theory to obtain a pose updating result;
checking whether the GNSS data, the magnetic strength data or the mileage data are acquired at the measurement time of each inertial measurement unit, and selecting time updating of Kalman filtering or observation updating of Kalman filtering according to the checking result to obtain a parameter correction vector;
and correcting the position and orientation updating result by using an error feedback theory based on the parameter correction vector to obtain navigation positioning information.
Optionally, the checking whether the GNSS data, the magnetic strength data, or the mileage data is acquired at the measurement time of each inertial measurement unit, and selecting time update of kalman filtering or observation update of the kalman filtering according to the check result to obtain a parameter correction vector specifically includes:
judging whether the GNSS data, the magnetic intensity data or the mileage data are acquired at the measurement time of each inertial measurement unit;
aiming at the moment when the GNSS data, the magnetic intensity data and the mileage data are not acquired, taking the pose updating result as the navigation result at the moment, and constructing a Kalman filtering time updating equation and a covariance matrix; the components of the state parameter vector of the time updating equation comprise position information, speed information, attitude information, zero offset, scale factor error, receiver clock drift, troposphere wet delay, receiver intersymbol bias, carrier ambiguity and ionosphere delay;
describing the position information, the speed information and the attitude information by adopting a dynamic function model; modeling the zero-bias and the scale factor errors as a first order Gaussian Markov process; delaying the tropospheric wetting, the receiver intersymbol offset andionospheric delay modeling is a random walk process; modeling the carrier ambiguity as a random constant; modeling the receiver clock difference and the receiver clock drift as
Figure BDA0002306462150000021
Thereby obtaining a well-established Kalman filtering time updating model; wherein t isr，kAnd tr，k-1The receiver clock difference at time k and at time k-1 respectively,
Figure BDA0002306462150000031
and
Figure BDA0002306462150000032
receiver clock drifts at time k and time k-1 respectively; dt is the sampling interval, εt，k-1And
Figure BDA0002306462150000033
receiver clock error noise and receiver clock drift noise respectively;
aiming at the moment when the GNSS data, the magnetic intensity data or the mileage data are obtained, establishing an observation information vector according to the GNSS data, the GNSS operation data, the IMU data, the magnetic intensity data, the mileage data and a carrier state constraint observation quantity, and further establishing a Kalman filtering observation updating equation to obtain an established Kalman filtering observation updating model;
based on an extended Kalman filtering parameter estimation theory, performing optimal estimation on the state parameter vector and the covariance matrix according to the Kalman filtering time updating model and the Kalman filtering observation updating model to obtain an optimal state parameter vector and an optimal covariance matrix;
calculating the parameter correction vector based on the optimal state parameter vector and the optimal covariance matrix.
Optionally, the implementing position initialization and speed initialization according to the current position and the current speed, implementing posture initialization according to the IMU data, and obtaining a posture initialization result specifically includes:
taking the current position as a position initialization result and taking the current speed as a speed initialization result;
calculating an attitude direction cosine matrix according to the attitude conversion relation among the IMU data, the earth rotation angular velocity and the earth gravity;
and calculating an initial attitude angle according to the attitude direction cosine matrix to obtain an attitude initialization result.
Optionally, before the acquiring the GNSS data collected by the GNSS receiver, the GNSS operation data broadcasted by the international GNSS service organization and received by the network module, the IMU data measured by the inertial measurement unit, the magnetic strength data measured by the magnetometer, and the mileage data measured by the odometer, the method further includes:
and synchronizing the time of the inertial measurement unit, the magnetometer and the odometer by taking the time of the GNSS receiver as a reference.
The utility model provides a multisource data fusion real-time navigation positioning system, is applied to a multisource data fusion real-time navigation positioning device, and this positioner includes: the system comprises a GNSS receiver, an inertia measurement unit, a magnetometer, a milemeter, a network module and an FPGA mainboard; the GNSS receiver, the inertia measurement unit, the magnetometer, the odometer and the network module are all connected with the FPGA mainboard;
the navigation positioning system comprises:
the data acquisition module is used for acquiring GNSS data acquired by the GNSS receiver, GNSS operation data broadcasted by an international GNSS service organization and received by the network module, IMU data measured by the inertial measurement unit, magnetic strength data measured by the magnetometer and mileage data measured by the milemeter;
the position preliminary estimation module is used for calculating the current position and the current speed of the carrier according to the non-differential non-combination precise single-point positioning theory and the GNSS operation data based on the least square theory and the sequential least square theory;
the pose initialization module is used for realizing position initialization and speed initialization according to the current position and the current speed and realizing posture initialization according to the IMU data to obtain a pose initialization result;
the inertial navigation state updating module is used for updating the inertial navigation state by utilizing an inertial navigation system mechanical arrangement theory based on the pose initialization result to obtain a pose updating result;
the Kalman filtering module is used for checking whether the GNSS data, the magnetic strength data or the mileage data are acquired at the measuring moment of each inertial measurement unit, and selecting to perform Kalman filtering time updating or Kalman filtering observation updating according to a checking result so as to obtain a parameter correction vector;
and the correction module is used for correcting the position updating result by using an error feedback theory based on the parameter correction number vector to obtain navigation positioning information.
Optionally, the kalman filtering module includes:
the judging unit is used for judging whether the GNSS data, the magnetic intensity data or the mileage data are acquired at the measuring time of each inertial measuring unit;
the time updating equation establishing unit is used for taking the pose updating result as the navigation result at the moment when the GNSS data, the magnetic strength data and the mileage data are not acquired, and establishing a Kalman filtering time updating equation and a covariance matrix; the components of the state parameter vector of the time updating equation comprise position information, speed information, attitude information, zero offset, scale factor error, receiver clock drift, troposphere wet delay, receiver intersymbol bias, carrier ambiguity and ionosphere delay;
the state parameter vector modeling unit is used for describing the position information, the speed information and the attitude information by adopting a dynamic function model; modeling the zero-bias and the scale factor errors as a first order Gaussian Markov process; modeling the tropospheric wet delay, the receiver intersymbol bias and the ionospheric delay as a random walk process; modeling the carrier ambiguity as a random constant; receiving the dataThe clock difference and the receiver clock drift are modeled as
Figure BDA0002306462150000051
Thereby obtaining a well-established Kalman filtering time updating model; wherein t isr，kAnd tr，k-1The receiver clock difference at time k and at time k-1 respectively,
Figure BDA0002306462150000052
and
Figure BDA0002306462150000053
receiver clock drifts at time k and time k-1 respectively; dt is the sampling interval, εt，k-1And
Figure BDA0002306462150000054
receiver clock error noise and receiver clock drift noise respectively;
an observation update equation establishing unit, configured to establish an observation innovation vector according to the GNSS data, the GNSS operation data, the IMU data, the magnetic strength data, the mileage data, and a carrier state constraint observed quantity at a time when the GNSS data, the magnetic strength data, or the mileage data is acquired, and further establish a kalman filtering observation update equation to obtain an established kalman filtering observation update model;
the filtering unit is used for carrying out optimal estimation on the state parameter vector and the covariance matrix according to the Kalman filtering time updating model and the Kalman filtering observation updating model based on an extended Kalman filtering parameter estimation theory to obtain an optimal state parameter vector and an optimal covariance matrix;
and the correction quantity calculating unit is used for calculating the parameter correction quantity vector based on the optimal state parameter vector and the optimal covariance matrix.
Optionally, the pose initialization module includes:
the position and speed initialization unit is used for taking the current position as a position initialization result and taking the current speed as a speed initialization result;
the attitude direction cosine matrix calculation unit is used for calculating an attitude direction cosine matrix according to the attitude conversion relation among the IMU data, the earth rotation angular velocity and the earth gravity;
and the attitude initialization unit is used for calculating an initial attitude angle according to the attitude direction cosine matrix to obtain an attitude initialization result.
Optionally, the navigation positioning system further includes:
and the time synchronization module is used for synchronizing the time of the inertial measurement unit, the magnetometer and the odometer by taking the time of the GNSS receiver as a reference.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects: according to the multi-source data fusion real-time navigation positioning method and system, a GNSS technology is combined with an inertial measurement unit, a magnetometer and a milemeter, the carrier pose is initialized by utilizing GNSS data and IMU data, INS updating is carried out by utilizing initialized data, and finally the updated data are corrected by utilizing Kalman filtering, so that an accurate positioning result can be obtained through correction, accurate positioning can be realized even in the environment of GNSS signal lock losing, and the positioning precision in the environment of lock losing of a number is improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
FIG. 1 is a block diagram of a multi-source data fusion real-time navigation positioning apparatus employed in the present invention;
FIG. 2 is a flowchart of a method of multi-source data fusion real-time navigation positioning in embodiment 1;
fig. 3 is a system structure diagram of a multi-source data fusion real-time navigation positioning system according to embodiment 2 of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.
The multi-source data fusion real-time navigation positioning method and system are applied to a multi-source data fusion real-time navigation positioning device.
FIG. 1 is a block diagram of a multi-source data fusion real-time navigation positioning device used in the present invention.
Referring to fig. 1, the positioning apparatus includes: the GNSS comprises a GNSS receiver 1, an Inertial Measurement Unit (IMU) 2, a magnetometer 3, an Odometer (Odometer) 4, a network module 5, and an FPGA (field programmable Gate Array) motherboard 6; the GNSS receiver 1, the inertia measurement unit 2, the magnetometer 3, the odometer 4 and the network module 5 are all connected with the FPGA mainboard 6 through a feeder 7. The GNSS receiver antenna is a multi-System GNSS receiver antenna, and the received GNSS data includes frequency data of B1 and B2 of the chinese third generation beidou Satellite Navigation System (BDS), frequency data of L1 and L2 of the american Global Positioning System (GPS), frequency data of G1 and G2 of the russian global Navigation Satellite System (GLONASS), frequency data of E1 and E5 of the russian global Navigation Satellite System, frequency data of L1 and L2 of the japanese Quasi-Zenith Navigation Satellite System (QZSS).
The inertial measurement unit 2 employs a Micro-Mechanical sensor (MEMS) inertial measurement unit. The magnetometer 3 employs a geomagnetic navigation system (GMNS).
The positioning device is mounted on a carrier, typically a vehicle carrier.
Based on the three-dimensional space coordinate system, before the equipment is installed, a space three-dimensional carrier coordinate system (body frame, referred to as b system for short) which takes the measurement center of the IMU1 as the coordinate origin and meets the right-hand coordinate system theorem is firstly established, and the b system in the scheme adopts a forward-right-down coordinate system (F-R-D coordinate system for short), wherein the x axis points to the front (F) of the IMU1, the y axis points to the right side (R), and the z axis points vertically to the ground (D)). Then when the device is mounted on the vehicle body it is ensured that the x-axis of the IMU1 and the magnetometer 3 is aligned with the direction of advance of the carrier, the y-axis pointing to the right of the carrier and the z-axis pointing towards the ground downwards. Accurately measuring the space relative position (namely a lever arm) of the GNSS antenna and the odometer 4 under the b system relative to the coordinate origin of the b system, wherein the lever arm of the GNSS antenna relative to the coordinate origin of the b system is
Figure BDA0002306462150000072
The lever arm of the odometer 4 relative to the origin of coordinates b is
Figure BDA0002306462150000071
Where T represents a matrix transpose operation.
The GNSS receiver 1 is configured to collect GNSS data, the network module 5 is configured to receive GNSS operation data broadcasted by an International GNSS Service organization (IGS), the inertial measurement unit 2 is configured to measure IMU data, the magnetometer 3 is configured to measure magnetic strength data, and the odometer 4 is configured to measure mileage data. The GNSS operation data at least comprises GNSS real-time precise orbits, precise clock errors and real-time ionization layer data.
The FPGA mainboard 6 is integrated with a navigation positioning program for realizing navigation positioning, namely, the time difference of the data acquired by the GNSS receiver 1 and the data acquired by the IMU/magnetometer/ODO entering the FPGA mainboard 6 is calculated, so that the time of the data acquired by the IMU/magnetometer/ODO is determined, and the time synchronization work of the GNSS/IMU/GMNS/ODO data is completed. And based on the GNSS operation data broadcasted by the IGS, performing fusion processing on the synchronized GNSS/IMU/magnetometer/ODO data to realize wide-area precise navigation positioning.
The invention provides two embodiments, namely a multi-source data fusion real-time navigation positioning method and a multi-source data fusion real-time navigation positioning system.
Example 1:
fig. 2 is a flowchart of a method of the multi-source data fusion real-time navigation positioning method of embodiment 1.
Referring to fig. 2, the navigation positioning method includes:
step 101: and synchronizing the time of the inertial measurement unit, the magnetometer and the odometer by taking the time of the GNSS receiver as a reference.
Based on the electronic signal time synchronization theory, in the data transmission, the GNSS time (toc) is usedGNSS) As a reference, the local time (receiver _ t) when the IMU/magnetometer/ODO signal is transmitted into the FPGA mainboard is comparedsensors，m) And local time (receiver _ t) when GNSS data is transmitted into the FPGA mainboardGNSS) Time difference between:
Figure BDA0002306462150000081
calculating time of each sensor
Figure BDA0002306462150000082
IMU, magnetometer and ODO data initial time toc which are calculated according to the formula (2) and take GNSS time as referencesensors，IMU，0、tocsensors，GMNS，0And tocsensors，ODO，0. Thereafter, the time in k epochs of the IMU, magnetometer, and ODO data can be calculated from the time in equation (2) and the number of Pulses Per Second (PPS) of each sensor:
Figure BDA0002306462150000083
where PPS represents the number of pulses per second of the sensor. And decoding the synchronized data according to the data formats of different sensors for subsequent data processing.
Step 102: the method comprises the steps of obtaining GNSS data collected by a GNSS receiver, GNSS operation data broadcast by an international GNSS service organization and received by a network module, IMU data measured by an inertial measurement unit, magnetic strength data measured by a magnetometer and mileage data measured by a milemeter.
The data acquired in step 102 are all time-synchronized data.
Step 103: based on least square and sequential least square theory, calculating the current position and current speed of the carrier according to non-difference non-combination Precision Point Positioning (PPP) theory and GNSS operation data.
The GNSS data at least comprises dual-frequency pseudo range, carrier wave and Doppler measured by each navigation system.
This step requires first checking the state parameter X of the non-differential non-combined PPP theorykAn estimate is made and then the current position and current velocity of the carrier are calculated.
State parameter XkThe estimation equation of (a) is:
Figure BDA0002306462150000091
in the formula, Xk＝[δxrδyrδzrctrdwetDCBrN1N2I1]TRepresenting the state parameters of the non-differential non-combined PPP theory, (deltax)rδyrδzr) Representing GNSS receiver position corrections, tr、dwetAnd DCBrRespectively representing receiver clock error, tropospheric wet delay and receiver intersymbol offset, N1、N2And I1Respectively representing carrier ambiguity at a first frequency, carrier ambiguity at a second frequency, and ionospheric delay at the first frequency; pkVector Z representing observation information at k epoch timekThe corresponding prior variance is given by an empirical model; a. thekRepresenting a design coefficient matrix;
Figure BDA0002306462150000092
representing a state vector X at k-1 epoch timek-1The corresponding variance covariance matrix. Wherein, the observation information vector Z of the non-differential non-combination PPP theorykDouble frequency pseudorange (P) from GNSSj) Carrier wave (L)j) Doppler (D)j) Observation equation and ionospheric constraint equation
Figure BDA0002306462150000093
The method comprises the following steps:
Figure BDA0002306462150000094
Figure BDA0002306462150000095
Figure BDA0002306462150000101
Figure BDA0002306462150000102
wherein, Pj、LjAnd DjRespectively represents pseudo range, carrier wave and Doppler observed value on j frequency (each frequency is BDSB1/B2, GPSL1/L2, GLONASS G1/G2, GalileoE1/E5 and QZSSL 1/L2); the indices s and r represent the satellite and the receiver, respectively; c represents the speed of light in vacuum; t is tsAnd trRespectively representing the clock error of a satellite terminal at the moment of signal transmission and the clock error of a receiver terminal at the moment of signal reception;
Figure BDA0002306462150000105
and
Figure BDA0002306462150000106
respectively representing the ionospheric delay and tropospheric delay of the signal at the jth frequency as it propagates from the satellite s to the receiver r; dr，jAnd
Figure BDA0002306462150000107
respectively representing the time delay of pseudorange observations at the jth frequency due to satellite and receiver hardware;
Figure BDA0002306462150000103
and
Figure BDA0002306462150000104
respectively representing relativistic effect errors caused by satellite orbital eccentricity, errors caused by earth rotation, phase center deviations of satellites and receivers, phase center variation errors of antennas, solid tide of the earth and ocean load tide errors and gravitational errors; lambda [ alpha ]jAnd NjA wavelength and integer ambiguity representing a jth frequency carrier phase; br，jAnd
Figure BDA0002306462150000108
respectively representing the phase time delays caused by satellite and receiver hardware delays; epsilonP，j、εL，jAnd εD，jRespectively representing unmodeled errors and receiver thermal noise in pseudo range, carrier wave and Doppler observed values;
Figure BDA0002306462150000109
the method comprises the steps that a carrier phase winding error caused by antenna rotation is shown, the carrier phase winding error is generally caused by satellite antenna rotation under a static condition, and the carrier phase winding error is caused by a receiver antenna and a satellite antenna under a dynamic condition; ρ and
Figure BDA00023064621500001010
respectively representing the geometrical distance and the distance change rate from the signal transmitting moment to the signal receiving moment;
Figure BDA00023064621500001011
represents the rate of change; STEC and f1Representing the total electron content and the first signal frequency of the GNSS in the GNSS data propagation path;
Figure BDA00023064621500001012
representing ionospheric prior model errors.
Step 104: and realizing position initialization and speed initialization according to the current position and the current speed, and realizing posture initialization according to IMU data to obtain a posture initialization result.
The step 104 specifically includes:
1. and taking the current position as a position initialization result and taking the current speed as a speed initialization result.
2. And calculating an attitude direction cosine matrix according to the attitude conversion relation between the IMU data and the earth rotation angular velocity and the earth gravity.
According to the attitude rotation relation between the specific force and angular velocity data measured by the IMU in the b system and the earth rotation angular velocity and the earth gravity in a navigation coordinate system (N-Eat-Down, N-E-D for short) under the static condition, calculating the attitude direction cosine matrix
Figure BDA0002306462150000111
Figure BDA0002306462150000112
Figure BDA0002306462150000113
Figure BDA0002306462150000114
In the formula (I), the compound is shown in the specification,
Figure BDA0002306462150000116
and
Figure BDA0002306462150000117
respectively, the specific force f under static conditionsbAnd angular velocity
Figure BDA0002306462150000118
The mean value of (a); gnRepresenting earth gravity under n series;
Figure BDA0002306462150000119
represents the rotation angular velocity (i.e. the Earth rotation angular velocity) of the Earth center Earth fixed coordinate system under the n system (the Earth center Earth fixed frame, the e system) relative to the inertia system (the i system);
Figure BDA00023064621500001110
and
Figure BDA00023064621500001111
and a third vector constructed by constructing specific force and angular velocity vectors under an n system and a b system. Attitude cosine matrix calculated by formula (9)
Figure BDA00023064621500001112
Calculating and solving an initial attitude angle:
Figure BDA0002306462150000115
wherein θ, φ and ψ represent a roll angle, a pitch angle and a heading angle, respectively; atan is a function for calculating the arctan value in C/C + + language, and the return value is between-pi/2- + pi/2; atan2 is a function in C/C + + language for calculating the arctan values divided by quadrants, with the return value between- π and + π;
Figure BDA00023064621500001113
to represent
Figure BDA00023064621500001114
Line k1 and column k 2.
3. And calculating an initial attitude angle according to the attitude direction cosine matrix to obtain an attitude initialization result.
Step 105: and updating the inertial navigation state by utilizing an inertial navigation system mechanics arrangement theory based on the pose initialization result to obtain a pose updating result.
Based on an INS (inertial navigation system) mechanics arrangement theory and a pose initialization result, updating the INS state according to an inertia mechanics arrangement theory model:
Figure BDA0002306462150000121
Figure BDA0002306462150000122
Figure BDA0002306462150000123
wherein v isnAnd pnRespectively representing n series of carrier speeds and positions;
Figure BDA0002306462150000124
and
Figure BDA0002306462150000125
a differential form representing a velocity, position and attitude rotation matrix; v. ofDAnd h represents the n-series vertical speed and the ground height;
Figure BDA0002306462150000126
represents the rotational angular velocity of n relative to e;
Figure BDA0002306462150000127
represents the rotational angular velocity of n relative to i under n.
Step 106: and checking whether the GNSS data, the magnetic strength data or the mileage data are acquired at the measuring moment of each inertia measuring unit, and selecting to perform Kalman filtering time updating or Kalman filtering observation updating according to the checking result so as to obtain a parameter correction number vector. Because the sampling rate of the IMU is higher than that of the GNSS receiver, the magnetometer, and the ODO, GNSS data, magnetometer data, or mileage data may not necessarily be acquired at each measurement time of the inertial measurement unit.
The step 106 specifically includes:
1. and judging whether the GNSS data, the magnetic intensity data or the mileage data are acquired at the measuring time of each inertia measuring unit.
2. Aiming at the moment when the GNSS data, the magnetic intensity data and the mileage data are not acquired, taking a pose updating result as a navigation result at the moment, and constructing a Kalman filtering time updating equation and a covariance matrix; the components of the state parameter vector of the time update equation comprise position information, speed information, attitude information, zero offset, scale factor error, receiver clock drift, troposphere wet delay, receiver intersymbol offset, carrier ambiguity and ionosphere delay;
the results of equations (13) to (15) are the navigation results at the current epoch time. Updating an equation according to Kalman filtering time:
Xk＝Φk，k-1Xk-1+μk-1，μk-1～(0，Qk-1) (16)
calculating a corresponding covariance matrix:
Figure BDA0002306462150000131
in the formula phik，k-1Represents a state vector XkThe design coefficient matrix and the state transition matrix of (1); mu.sk-1Representing state noise with a prior variance of Qk-1；
Figure BDA0002306462150000137
And the variance covariance matrix corresponding to the state parameter of the k-1 epoch is represented.
In the tightly packed mode, the state parameter vector XkCan be expressed as:
Figure BDA0002306462150000138
where δ represents the number of corrections of the corresponding parameter (e.g.: δ p)nRepresenting position correction numbers), B and S represent zero offset and scale factor error of the IMU, and g and a represent a gyroscope (gyroscope) and an accelerometer (accelerometer); Ψ represents an attitude matrix; state transition matrix phik，k-1And determining according to a dynamic function model of the state parameter.
3. And modeling each component in the state parameter vector, so that the well established Kalman filtering time updating model is obtained by combining a Kalman filtering time updating equation and a covariance matrix. Modeling each component specifically includes:
1) the position, velocity and attitude are described using a kinetic function model as shown in equations (19) to (21):
Figure BDA0002306462150000132
Figure BDA0002306462150000133
Figure BDA0002306462150000134
2) the IMU zero bias and scale factor error are modeled as a first order Gaussian Markov process:
Figure BDA0002306462150000135
in the formula, tau and T respectively represent the relevant time of the Gaussian-Markov process corresponding to the IMU sampling interval and the IMU error; omegaS，k-1And ωB，k-1The drive noise (either given by the sensing manufacturer or calibrated by the turntable) represents the scale factor error and the zero offset error, respectively.
3) The receiver clock error and clock drift are modeled as:
Figure BDA0002306462150000136
wherein t isr，kAnd tr，k-1The receiver clock difference at time k and at time k-1 respectively,
Figure BDA0002306462150000139
and
Figure BDA00023064621500001310
receiver clock drifts at time k and time k-1 respectively; dt is the sampling interval, εt，k-1And
Figure BDA00023064621500001311
receiver clock difference noise and receiver clock drift noise, respectively.
4)dwet、DCBrAnd I1Modeling as a random walk process:
dwet，k＝dwet，k-1+εdwet，k-1(24)
DCBr，k＝DCBr，k-1+εDCB，k-1(25)
I1，k＝I1，k-1+εI1，k-1(26)
in the formula ofdwet，k-1、εDCB，k-1And εI1，k-1Representing receiver tropospheric wet delay, receiver DCB and ionospheric noise, respectively.
4) Ambiguity modeling as random constants:
Nk＝Nk-1(27)
from equations (16) to (24), a coefficient matrix Φ can be obtainedk，k-1。
4. And aiming at the moment of acquiring the GNSS data, the magnetic intensity data or the mileage data, establishing an observation innovation vector according to the GNSS data, the GNSS operation data, the IMU data, the magnetic intensity data, the mileage data and the carrier state constraint observation quantity, and further establishing a Kalman filtering observation updating equation to obtain an established Kalman filtering observation updating model.
Firstly, based on a geomagnetic navigation theoretical model and a odometer measurement principle, based on magnetometer data and an AHRS algorithm, calculating attitude angle information thetaGMNS，φGMNS，ψGMNS(ii) a Meanwhile, based on the data of the carrier odometer, the moving speed v of the carrier along the F direction under the b system is calculatedODO。
And then constructing a Kalman filtering observation updating equation:
Zk＝HkXk+ηk，ηk～N(0，Rk) (28)
in the formula Zk、ηk、RkRespectively representing observation value innovation vectors, observation innovation noises and observation noise intensities in the tight combination; hkRepresents a state vector XkThe design coefficient matrix of (1). In the tightly packed mode, an innovation vector Z is observedkThe method is characterized by comprising a direct observation quantity (GNSS observation information, magnetometer attitude information and odometer speed information) and a carrier state constraint observation quantity (zero-speed correction and non-integrity constraint). If the direct observation quantity exists, respectively constructing corresponding observation innovation vectors according to data observed at the current IMU moment:
if the GNSS observation data exists, the information vector corresponding to the GNSS observation data is as follows:
Figure BDA0002306462150000151
in the formula
Figure BDA0002306462150000155
And
Figure BDA0002306462150000156
means for representing a pseudorange at a first frequency and a pseudorange at a second frequency predicted by the INS;
Figure BDA0002306462150000157
and
Figure BDA0002306462150000158
means for representing a pseudorange at a first frequency and a pseudorange at a second frequency for s satellites observed by a GNSS receiver;
Figure BDA0002306462150000159
and
Figure BDA00023064621500001510
representing the first on the s satellite predicted by the INSPseudoranges at one frequency and at a second frequency;
Figure BDA00023064621500001511
and
Figure BDA00023064621500001512
means for representing a pseudorange at a first frequency and a pseudorange at a second frequency for s satellites observed by a GNSS receiver;
Figure BDA00023064621500001513
and
Figure BDA00023064621500001514
indicating the doppler at the first frequency on the s-satellite predicted by the INS and the doppler observed by the GNSS observation side;
Figure BDA00023064621500001515
and
Figure BDA00023064621500001516
indicating the ionospheric delay at the first frequency on the s-satellite predicted by the INS and the ionospheric delay calculated by the Global Ionospheric Model (GIM).
If the magnetometer attitude information exists, the innovation vector corresponding to the magnetometer observation data is as follows:
Figure BDA0002306462150000152
in the formula, thetaINS、φINSAnd psiINSRespectively representing roll angle, pitch angle and course angle obtained by INS mechanical arrangement calculation; thetaGMNS、φGMNSAnd psiGMNSRespectively representing roll, pitch and heading angles calculated from magnetometer data.
If the ODO speed information exists, the innovation vector corresponding to the ODO observation data is as follows:
Figure BDA0002306462150000153
in the formula, vI NS，F、vI NS，RAnd vI NS，DRespectively representing the forward speed, the right speed and the vertical speed obtained by INS mechanical arrangement calculation; v. ofODO，FRepresenting the forward speed calculated by the ODO.
If the direct observation information does not exist, the observation innovation vector is formed by the condition of non-integrity constraint (the vehicle body only has the speed along the advancing direction F in the motion state, and the motion does not exist in the transverse direction and the vertical direction of the vehicle and is 0) in the motion state, and the innovation observation equation is as follows:
Figure BDA0002306462150000154
in a static state, an observation innovation vector is formed by a zero-speed correction condition (the speed of a vehicle body is 0 in the static state), and an innovation observation equation is as follows:
Figure BDA0002306462150000161
obtaining observation information vector Z of the multi-source data fusion tight combination mode according to the formulas (29) to (33)kCan be expressed as:
Figure BDA0002306462150000162
design coefficient matrix HkThen error perturbation is performed by applying equation (34) according to the error perturbation theory
Figure BDA0002306462150000163
Obtained in the formula
Figure BDA0002306462150000164
Indicating the derivation.
5. And based on the extended Kalman filtering parameter estimation theory, performing optimal estimation on the state parameter vector and the covariance matrix according to the Kalman filtering time updating model and the Kalman filtering observation updating model to obtain an optimal state parameter vector and an optimal covariance matrix.
The method specifically comprises the following steps: calculating the optimal estimated value of the state parameter vector based on the extended Kalman filter parameter estimation theory, the state transition matrix, the observation innovation vector and the design coefficient matrix
Figure BDA0002306462150000168
And its corresponding variance covariance matrix
Figure BDA0002306462150000169
Figure BDA0002306462150000165
Figure BDA0002306462150000166
Figure BDA0002306462150000167
In the formula, I represents an identity matrix.
6. And calculating a parameter correction vector based on the optimal state parameter vector and the optimal covariance matrix.
Step 107: and correcting the position and orientation updating result by using an error feedback theory based on the parameter correction vector to obtain navigation positioning information.
According to the technical scheme, the carrier can be used for calculating the three-dimensional position, speed and attitude information with high sampling, high precision and continuity, and the technical scheme can be used as a technical support for unmanned/auxiliary driving.
Example 2:
the embodiment 2 provides a multi-source data fusion real-time navigation positioning system.
Fig. 3 is a system structure diagram of a multi-source data fusion real-time navigation positioning system according to embodiment 2 of the present invention.
Referring to fig. 3, the navigation and positioning system includes:
and a time synchronization module 201, configured to synchronize the time of the inertial measurement unit, the magnetometer, and the odometer with reference to the time of the GNSS receiver.
The data obtaining module 202 is configured to obtain GNSS data acquired by the GNSS receiver, GNSS operation data broadcast by an international GNSS service organization and received by the network module, IMU data measured by an inertial measurement unit, magnetic strength data measured by a magnetometer, and mileage data measured by a milemeter.
And the position preliminary estimation module 203 is used for calculating the current position and the current speed of the carrier according to the non-differential non-combination precise single-point positioning theory and the GNSS operation data based on the least square theory and the sequential least square theory.
And the pose initialization module 204 is configured to implement position initialization and speed initialization according to the current position and the current speed, and implement pose initialization according to the IMU data to obtain a pose initialization result.
And the inertial navigation state updating module 205 is configured to update the inertial navigation state by using an inertial navigation system mechanics arrangement theory based on the pose initialization result to obtain a pose updating result.
And the kalman filtering module 206 is configured to check whether GNSS data, magnetic strength data, or mileage data is acquired at the measurement time of each inertial measurement unit, and select time update for kalman filtering or observation update for kalman filtering according to a check result to obtain a parameter correction vector.
And the correcting module 207 is used for correcting the position updating result by using an error feedback theory based on the parameter correction number vector to obtain navigation positioning information.
Optionally, the kalman filtering module 206 includes:
the judging unit is used for judging whether the GNSS data, the magnetic strength data or the mileage data are acquired at the measuring time of each inertial measuring unit;
the time updating equation establishing unit is used for establishing a Kalman filtering time updating equation and a covariance matrix by taking a pose updating result as a navigation result at the moment when the GNSS data, the magnetic intensity data and the mileage data are not acquired; the components of the state parameter vector of the time update equation comprise position information, speed information, attitude information, zero offset, scale factor error, receiver clock drift, troposphere wet delay, receiver intersymbol offset, carrier ambiguity and ionosphere delay;
the state parameter vector modeling unit is used for describing the position information, the speed information and the attitude information by adopting a dynamic function model; modeling zero-bias and scale factor errors as a first order gaussian markov process; modeling tropospheric wet delay, receiver inter-symbol bias and ionospheric delay as a random walk process; modeling carrier ambiguity as a random constant; modeling receiver clock error and receiver clock drift as
Figure BDA0002306462150000181
Thereby obtaining a well-established Kalman filtering time updating model; wherein t isr，kAnd tr，k-1The receiver clock difference at time k and at time k-1 respectively,
Figure BDA0002306462150000182
and
Figure BDA0002306462150000183
receiver clock drifts at time k and time k-1 respectively; dt is the sampling interval, εt，k-1And
Figure BDA0002306462150000184
receiver clock error noise and receiver clock drift noise respectively;
the observation update equation establishing unit is used for establishing an observation information vector according to the GNSS data, the GNSS operation data, the IMU data, the magnetic strength data, the mileage data and the carrier state constraint observation quantity at the moment of acquiring the GNSS data, the magnetic strength data or the mileage data, and further establishing a Kalman filtering observation update equation to obtain an established Kalman filtering observation update model;
the filtering unit is used for carrying out optimal estimation on the state parameter vector and the covariance matrix according to a Kalman filtering time updating model and a Kalman filtering observation updating model based on an extended Kalman filtering parameter estimation theory to obtain an optimal state parameter vector and an optimal covariance matrix;
and the correction quantity calculating unit is used for calculating the parameter correction quantity vector based on the optimal state parameter vector and the optimal covariance matrix.
Optionally, the pose initialization module 204 includes:
the position and speed initialization unit is used for taking the current position as a position initialization result and taking the current speed as a speed initialization result;
the attitude direction cosine matrix calculation unit is used for calculating an attitude direction cosine matrix according to the attitude conversion relation among the IMU data, the earth rotation angular velocity and the earth gravity;
and the attitude initialization unit is used for calculating an initial attitude angle according to the attitude direction cosine matrix to obtain an attitude initialization result.
According to the specific embodiment provided by the invention, the invention discloses the following technical effects:
(1) the method adopts a multi-system GNSS (BDS/GPS/GLONASS/Galileo/QZSS), enhances the satellite space distribution, and improves the number of satellites which can be observed by a user and the satellite space geometric configuration; compared with a GPS system, the application of the multi-GNSS system further increases the number of observable satellites and optimizes the satellite constellation structure, so that the number of observable satellites is increased from at most 14 satellites in the GPS to more than 40 satellites in the multi-system, and the distribution of the positioning geometric accuracy factors (PDOP) of the current GPS is greatly improved.
(2) The multi-system GNSS and the active navigation system (INS is adopted in the scheme) are fused, so that the defect that the GNSS navigation positioning performance is influenced by the observation environment is overcome; after the initial state information of the carrier is given, the INS processes the angular acceleration and linear acceleration information of the carrier acquired by the IMU through an inertial navigation algorithm, and then the high-sampling-rate continuous position, posture and speed information of the carrier can be provided for a user. These advantages of INS (autonomous navigation positioning, high data sampling rate (hundreds of hertz), high short-term accuracy) are lacking in GNSS navigation positioning.
(3) And enhancing the reliability and stability of the GNSS/INS combined navigation positioning result by adopting multi-source sensor data and utilizing the characteristics of carrier motion. In the scheme, in order to overcome the weakness of the GNSS/INS combination, the magnetometer and the milemeter are adopted to increase the observation of the attitude and the speed of the vehicle body, and the characteristics of the motion of the carrier are utilized to enhance the precision and the reliability of the GNSS/INS navigation result.
(4) And a precision data processing method is adopted to ensure the precision of the navigation positioning result. Because of the combined GNSS/INS/magnetometer/ODO technology, the absolute positioning accuracy is mainly affected by the GNSS data processing mode. The multisystem GNSS real-time PPP technology adopts double-frequency GNSS data of a single receiver, and can ensure centimeter-level navigation positioning results by using a satellite real-time precision product provided by IGS.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. For the system disclosed by the embodiment, the description is relatively simple because the system corresponds to the method disclosed by the embodiment, and the relevant points can be referred to the method part for description.
The principles and embodiments of the present invention have been described herein using specific examples, which are provided only to help understand the method and the core concept of the present invention; meanwhile, for a person skilled in the art, according to the idea of the present invention, the specific embodiments and the application range may be changed. In view of the above, the present disclosure should not be construed as limiting the invention.

Claims (8)

1. A multi-source data fusion real-time navigation positioning method is characterized by being applied to a multi-source data fusion real-time navigation positioning device, and the positioning device comprises: the system comprises a GNSS receiver, an inertia measurement unit, a magnetometer, a milemeter, a network module and an FPGA mainboard; the GNSS receiver, the inertia measurement unit, the magnetometer, the odometer and the network module are all connected with the FPGA mainboard;
the navigation positioning method comprises the following steps:
acquiring GNSS data acquired by the GNSS receiver, GNSS operation data broadcasted by an international GNSS service organization and received by the network module, IMU data measured by the inertial measurement unit, magnetic strength data measured by the magnetometer and mileage data measured by the milemeter;
calculating the current position and the current speed of the carrier from the GNSS data and the GNSS operation data according to a non-differential non-combination precise single-point positioning theory based on a least square theory and a sequential least square theory;
position initialization and speed initialization are achieved according to the current position and the current speed, posture initialization is achieved according to the IMU data, and a posture initialization result is obtained;
based on the pose initialization result, updating the inertial navigation state by using an inertial navigation system mechanical arrangement theory to obtain a pose updating result;
checking whether the GNSS data, the magnetic strength data or the mileage data are acquired at the measurement time of each inertial measurement unit, and selecting time updating of Kalman filtering or observation updating of Kalman filtering according to the checking result to obtain a parameter correction vector;
and correcting the position and orientation updating result by using an error feedback theory based on the parameter correction vector to obtain navigation positioning information.
2. The multi-source data fusion real-time navigation positioning method according to claim 1, wherein the checking whether the GNSS data, the magnetic strength data, or the mileage data is acquired at the measurement time of each of the inertial measurement units, and selecting time update of kalman filtering or observation update of kalman filtering according to the checking result to obtain a parameter correction vector specifically includes:
judging whether the GNSS data, the magnetic intensity data or the mileage data are acquired at the measurement time of each inertial measurement unit;
aiming at the moment when the GNSS data, the magnetic intensity data and the mileage data are not acquired, taking the pose updating result as the navigation result at the moment, and constructing a Kalman filtering time updating equation and a covariance matrix; the components of the state parameter vector of the time updating equation comprise position information, speed information, attitude information, zero offset, scale factor error, receiver clock drift, troposphere wet delay, receiver intersymbol bias, carrier ambiguity and ionosphere delay;
describing the position information, the speed information and the attitude information by adopting a dynamic function model; modeling the zero-bias and the scale factor errors as a first order Gaussian Markov process; modeling the tropospheric wet delay, the receiver intersymbol bias and the ionospheric delay as a random walk process; modeling the carrier ambiguity as a random constant; modeling the receiver clock difference and the receiver clock drift as
Figure FDA0002306462140000021
Thereby obtaining a well-established Kalman filtering time updating model; wherein t isr，kAnd tr，k-1The receiver clock difference at time k and at time k-1 respectively,
Figure FDA0002306462140000022
and
Figure FDA0002306462140000023
receiver clock drifts at time k and time k-1 respectively; dt is the sampling interval, εt，k-1And
Figure FDA0002306462140000024
receiver clock error noise and receiver clock drift noise respectively;
aiming at the moment when the GNSS data, the magnetic intensity data or the mileage data are obtained, establishing an observation information vector according to the GNSS data, the GNSS operation data, the IMU data, the magnetic intensity data, the mileage data and a carrier state constraint observation quantity, and further establishing a Kalman filtering observation updating equation to obtain an established Kalman filtering observation updating model;
based on an extended Kalman filtering parameter estimation theory, performing optimal estimation on the state parameter vector and the covariance matrix according to the Kalman filtering time updating model and the Kalman filtering observation updating model to obtain an optimal state parameter vector and an optimal covariance matrix;
calculating the parameter correction vector based on the optimal state parameter vector and the optimal covariance matrix.
3. The multi-source data fusion real-time navigation positioning method according to claim 1, wherein the position initialization and the speed initialization are realized according to the current position and the current speed, and the posture initialization is realized according to the IMU data to obtain a posture initialization result, specifically comprising:
taking the current position as a position initialization result and taking the current speed as a speed initialization result;
calculating an attitude direction cosine matrix according to the attitude conversion relation among the IMU data, the earth rotation angular velocity and the earth gravity;
and calculating an initial attitude angle according to the attitude direction cosine matrix to obtain an attitude initialization result.
4. The multi-source data fusion real-time navigation positioning method according to claim 1, further comprising, before the acquiring the GNSS data collected by the GNSS receiver, the GNSS operation data broadcasted by the international GNSS service organization received by the network module, the IMU data measured by the inertial measurement unit, the magnetic strength data measured by the magnetometer, and the mileage data measured by the odometer:
and synchronizing the time of the inertial measurement unit, the magnetometer and the odometer by taking the time of the GNSS receiver as a reference.
5. The utility model provides a multisource data fusion real-time navigation positioning system which characterized in that is applied to a multisource data fusion real-time navigation positioning device, and this positioner includes: the system comprises a GNSS receiver, an inertia measurement unit, a magnetometer, a milemeter, a network module and an FPGA mainboard; the GNSS receiver, the inertia measurement unit, the magnetometer, the odometer and the network module are all connected with the FPGA mainboard;
the navigation positioning system comprises:
the data acquisition module is used for acquiring GNSS data acquired by the GNSS receiver, GNSS operation data broadcasted by an international GNSS service organization and received by the network module, IMU data measured by the inertial measurement unit, magnetic strength data measured by the magnetometer and mileage data measured by the milemeter;
the position preliminary estimation module is used for calculating the current position and the current speed of the carrier according to the non-differential non-combination precise single-point positioning theory and the GNSS operation data based on the least square theory and the sequential least square theory;
the pose initialization module is used for realizing position initialization and speed initialization according to the current position and the current speed and realizing posture initialization according to the IMU data to obtain a pose initialization result;
the inertial navigation state updating module is used for updating the inertial navigation state by utilizing an inertial navigation system mechanical arrangement theory based on the pose initialization result to obtain a pose updating result;
the Kalman filtering module is used for checking whether the GNSS data, the magnetic strength data or the mileage data are acquired at the measuring moment of each inertial measurement unit, and selecting to perform Kalman filtering time updating or Kalman filtering observation updating according to a checking result so as to obtain a parameter correction vector;
and the correction module is used for correcting the position updating result by using an error feedback theory based on the parameter correction number vector to obtain navigation positioning information.
6. The multi-source data fusion real-time navigation and positioning system of claim 5, wherein the Kalman filtering module comprises:
the judging unit is used for judging whether the GNSS data, the magnetic intensity data or the mileage data are acquired at the measuring time of each inertial measuring unit;
the time updating equation establishing unit is used for taking the pose updating result as the navigation result at the moment when the GNSS data, the magnetic strength data and the mileage data are not acquired, and establishing a Kalman filtering time updating equation and a covariance matrix; the components of the state parameter vector of the time updating equation comprise position information, speed information, attitude information, zero offset, scale factor error, receiver clock drift, troposphere wet delay, receiver intersymbol bias, carrier ambiguity and ionosphere delay;
the state parameter vector modeling unit is used for describing the position information, the speed information and the attitude information by adopting a dynamic function model; modeling the zero-bias and the scale factor errors as a first order Gaussian Markov process; modeling the tropospheric wet delay, the receiver intersymbol bias and the ionospheric delay as a random walk process; modeling the carrier ambiguity as a random constant; modeling the receiver clock difference and the receiver clock drift as
Figure FDA0002306462140000041
Thereby obtaining a well-established Kalman filtering time updating model; wherein t isr，kAnd tr，k-1The receiver clock difference at time k and at time k-1 respectively,
Figure FDA0002306462140000042
and
Figure FDA0002306462140000043
receiver clock drifts at time k and time k-1 respectively; dt is the sampling interval, εt，k-1And
Figure FDA0002306462140000044
receiver clock error noise and receiver clock drift noise respectively;
an observation update equation establishing unit, configured to establish an observation innovation vector according to the GNSS data, the GNSS operation data, the IMU data, the magnetic strength data, the mileage data, and a carrier state constraint observed quantity at a time when the GNSS data, the magnetic strength data, or the mileage data is acquired, and further establish a kalman filtering observation update equation to obtain an established kalman filtering observation update model;
the filtering unit is used for carrying out optimal estimation on the state parameter vector and the covariance matrix according to the Kalman filtering time updating model and the Kalman filtering observation updating model based on an extended Kalman filtering parameter estimation theory to obtain an optimal state parameter vector and an optimal covariance matrix;
and the correction quantity calculating unit is used for calculating the parameter correction quantity vector based on the optimal state parameter vector and the optimal covariance matrix.
7. The multi-source data fusion real-time navigation and positioning system of claim 5, wherein the pose initialization module comprises:
the position and speed initialization unit is used for taking the current position as a position initialization result and taking the current speed as a speed initialization result;
the attitude direction cosine matrix calculation unit is used for calculating an attitude direction cosine matrix according to the attitude conversion relation among the IMU data, the earth rotation angular velocity and the earth gravity;
and the attitude initialization unit is used for calculating an initial attitude angle according to the attitude direction cosine matrix to obtain an attitude initialization result.
8. The multi-source data fusion real-time navigation and positioning system of claim 5, further comprising:
and the time synchronization module is used for synchronizing the time of the inertial measurement unit, the magnetometer and the odometer by taking the time of the GNSS receiver as a reference.
CN201911241794.XA 2019-12-06 2019-12-06 Multi-source data fusion real-time navigation positioning method and system Pending CN110988951A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911241794.XA CN110988951A (en) 2019-12-06 2019-12-06 Multi-source data fusion real-time navigation positioning method and system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911241794.XA CN110988951A (en) 2019-12-06 2019-12-06 Multi-source data fusion real-time navigation positioning method and system

Publications (1)

Publication Number Publication Date
CN110988951A true CN110988951A (en) 2020-04-10

Family

ID=70090761

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911241794.XA Pending CN110988951A (en) 2019-12-06 2019-12-06 Multi-source data fusion real-time navigation positioning method and system

Country Status (1)

Country Link
CN (1) CN110988951A (en)

Cited By (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111412912A (en) * 2020-04-14 2020-07-14 上海华测导航技术股份有限公司 Navigation board, multi-source data fusion method for navigation board and carrier
CN111721298A (en) * 2020-06-24 2020-09-29 重庆赛迪奇智人工智能科技有限公司 SLAM outdoor large scene accurate positioning method
CN111812697A (en) * 2020-05-25 2020-10-23 浙江航天润博测控技术有限公司 Combined positioning and attitude measurement data processing method based on multi-mode precision positioning
CN111948686A (en) * 2020-08-05 2020-11-17 航天恒星科技有限公司 Time synchronization method and device
CN111982106A (en) * 2020-08-28 2020-11-24 北京信息科技大学 Navigation method, navigation device, storage medium and electronic device
CN112097763A (en) * 2020-08-28 2020-12-18 西北工业大学 Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination
CN112346084A (en) * 2020-10-26 2021-02-09 上海感探号信息科技有限公司 Automobile positioning method, system, electronic equipment and storage medium
CN112985387A (en) * 2021-02-01 2021-06-18 中北大学 GNSS and IMU time synchronization method and skiing synchronization detection system
CN113222092A (en) * 2021-06-07 2021-08-06 北京迈克恒通科贸发展有限公司 Tire full life cycle management method and system
CN113434427A (en) * 2021-07-06 2021-09-24 和芯星通科技(北京)有限公司 Method and device for realizing test verification, computer storage medium and terminal
FR3113943A1 (en) * 2020-09-09 2022-03-11 Commissariat à l'Energie Atomique et aux Energies Alternatives A method of determining the position and orientation of a vehicle.
CN114295122A (en) * 2021-12-02 2022-04-08 河北汉光重工有限责任公司 SINS _ GNSS time synchronization method and system for embedded system
CN114518587A (en) * 2022-02-18 2022-05-20 中国地质大学（北京） Indoor and outdoor seamless positioning system and method
CN114754765A (en) * 2021-01-08 2022-07-15 上海汽车集团股份有限公司 Vehicle three-dimensional positioning method and system
CN114877892A (en) * 2022-07-11 2022-08-09 泉州通维科技有限责任公司 Fusion positioning method for photovoltaic robot
CN115164886A (en) * 2022-07-22 2022-10-11 吉林大学 Vehicle-mounted GNSS/INS combined navigation system scale factor error compensation method
WO2023071442A1 (en) * 2021-10-29 2023-05-04 华为技术有限公司 Data processing method and apparatus
CN116718153A (en) * 2023-08-07 2023-09-08 成都云智北斗科技有限公司 Deformation monitoring method and system based on GNSS and INS
CN112946693B (en) * 2021-02-03 2024-01-23 中国人民解放军61540部队 Method and system for determining system time deviation of satellite navigation system

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106969762A (en) * 2017-01-12 2017-07-21 广州市泰斗鑫信息科技有限公司 A kind of Combinated navigation method for GNSS+INS+odo
CN108226985A (en) * 2017-12-25 2018-06-29 北京交通大学 Train Combinated navigation method based on Static Precise Point Positioning
CN108344415A (en) * 2018-01-30 2018-07-31 北京大学 A kind of integrated navigation information fusion method
CN108549397A (en) * 2018-04-19 2018-09-18 武汉大学 The unmanned plane Autonomous landing method and system assisted based on Quick Response Code and inertial navigation
US20180328736A1 (en) * 2016-05-16 2018-11-15 Northrop Grumman Systems Corporation Vision-aided aerial navigation
US20190079534A1 (en) * 2017-09-13 2019-03-14 TuSimple Neural network architecture system for deep odometry assisted by static scene optical flow
CN109873827A (en) * 2019-03-05 2019-06-11 长安大学 Bus or train route cooperative system and its data safe transmission method

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180328736A1 (en) * 2016-05-16 2018-11-15 Northrop Grumman Systems Corporation Vision-aided aerial navigation
CN106969762A (en) * 2017-01-12 2017-07-21 广州市泰斗鑫信息科技有限公司 A kind of Combinated navigation method for GNSS+INS+odo
US20190079534A1 (en) * 2017-09-13 2019-03-14 TuSimple Neural network architecture system for deep odometry assisted by static scene optical flow
CN108226985A (en) * 2017-12-25 2018-06-29 北京交通大学 Train Combinated navigation method based on Static Precise Point Positioning
CN108344415A (en) * 2018-01-30 2018-07-31 北京大学 A kind of integrated navigation information fusion method
CN108549397A (en) * 2018-04-19 2018-09-18 武汉大学 The unmanned plane Autonomous landing method and system assisted based on Quick Response Code and inertial navigation
CN109873827A (en) * 2019-03-05 2019-06-11 长安大学 Bus or train route cooperative system and its data safe transmission method

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
WOO JUNG PARK: "MEMS 3D DR/GPS Integrated System for Land Vehicle Application Robust to GPS Outages", 《IEEE ACCESS》 *
ZHOUZHENG GAO: "Odometer and MEMS IMU enhancing PPP under weak satellite observability", 《ADVANCES IN SPACE RESEARCH》 *
刘天雄: "《卫星导航系统概论》", 30 November 2018 *
李增科: "自适应联邦滤波器在GPS-INS-Odometer组合导航的应用", 《测绘学报》 *
王世达: "GNSS/INS组合导航模型研究与性能分析", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
高周正: "多模GNSS PPP/INS组合系统算法与度用研究", 《中国博士学位论文全文数据库 基础科学辑》 *

Cited By (28)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111412912A (en) * 2020-04-14 2020-07-14 上海华测导航技术股份有限公司 Navigation board, multi-source data fusion method for navigation board and carrier
CN111812697A (en) * 2020-05-25 2020-10-23 浙江航天润博测控技术有限公司 Combined positioning and attitude measurement data processing method based on multi-mode precision positioning
CN111812697B (en) * 2020-05-25 2024-05-31 浙江航天润博测控技术有限公司 Combined positioning gesture measurement data processing method based on multi-mode precise positioning
CN111721298A (en) * 2020-06-24 2020-09-29 重庆赛迪奇智人工智能科技有限公司 SLAM outdoor large scene accurate positioning method
CN111948686B (en) * 2020-08-05 2024-04-26 航天恒星科技有限公司 Time synchronization method and device
CN111948686A (en) * 2020-08-05 2020-11-17 航天恒星科技有限公司 Time synchronization method and device
CN112097763A (en) * 2020-08-28 2020-12-18 西北工业大学 Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination
CN111982106A (en) * 2020-08-28 2020-11-24 北京信息科技大学 Navigation method, navigation device, storage medium and electronic device
CN112097763B (en) * 2020-08-28 2022-07-05 西北工业大学 Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination
FR3113943A1 (en) * 2020-09-09 2022-03-11 Commissariat à l'Energie Atomique et aux Energies Alternatives A method of determining the position and orientation of a vehicle.
EP3967975A1 (en) 2020-09-09 2022-03-16 Commissariat à l'énergie atomique et aux énergies alternatives Method for determining the position and orientation of a vehicle
US11892296B2 (en) 2020-09-09 2024-02-06 Commissariat à l'Energie Atomique et aux Energies Alternatives Method for determining the position and the orientation of a vehicle
CN112346084A (en) * 2020-10-26 2021-02-09 上海感探号信息科技有限公司 Automobile positioning method, system, electronic equipment and storage medium
CN114754765A (en) * 2021-01-08 2022-07-15 上海汽车集团股份有限公司 Vehicle three-dimensional positioning method and system
CN112985387A (en) * 2021-02-01 2021-06-18 中北大学 GNSS and IMU time synchronization method and skiing synchronization detection system
CN112985387B (en) * 2021-02-01 2023-08-11 中北大学 Time synchronization method of GNSS and IMU and skiing synchronization detection system
CN112946693B (en) * 2021-02-03 2024-01-23 中国人民解放军61540部队 Method and system for determining system time deviation of satellite navigation system
CN113222092A (en) * 2021-06-07 2021-08-06 北京迈克恒通科贸发展有限公司 Tire full life cycle management method and system
CN113434427A (en) * 2021-07-06 2021-09-24 和芯星通科技(北京)有限公司 Method and device for realizing test verification, computer storage medium and terminal
WO2023071442A1 (en) * 2021-10-29 2023-05-04 华为技术有限公司 Data processing method and apparatus
CN114295122A (en) * 2021-12-02 2022-04-08 河北汉光重工有限责任公司 SINS _ GNSS time synchronization method and system for embedded system
CN114295122B (en) * 2021-12-02 2024-06-11 河北汉光重工有限责任公司 SINS_GNSS time synchronization method and system for embedded system
CN114518587A (en) * 2022-02-18 2022-05-20 中国地质大学（北京） Indoor and outdoor seamless positioning system and method
CN114877892A (en) * 2022-07-11 2022-08-09 泉州通维科技有限责任公司 Fusion positioning method for photovoltaic robot
CN115164886B (en) * 2022-07-22 2023-09-05 吉林大学 Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system
CN115164886A (en) * 2022-07-22 2022-10-11 吉林大学 Vehicle-mounted GNSS/INS combined navigation system scale factor error compensation method
CN116718153A (en) * 2023-08-07 2023-09-08 成都云智北斗科技有限公司 Deformation monitoring method and system based on GNSS and INS
CN116718153B (en) * 2023-08-07 2023-10-27 成都云智北斗科技有限公司 Deformation monitoring method and system based on GNSS and INS

Similar Documents

Publication Publication Date Title
CN110988951A (en) Multi-source data fusion real-time navigation positioning method and system
US9488480B2 (en) Method and apparatus for improved navigation of a moving platform
US7409290B2 (en) Positioning and navigation method and system thereof
US7292185B2 (en) Attitude determination exploiting geometry constraints
US8577606B2 (en) Vehicle guidance and sensor bias determination
US6240367B1 (en) Full fusion positioning method for vehicle
US6292750B1 (en) Vehicle positioning method and system thereof
CN109613585A (en) A kind of method of pair of real-time direction finding of antenna for base station ultra-short baseline GNSS double antenna
CN106767787A (en) A kind of close coupling GNSS/INS combined navigation devices
US20030149528A1 (en) Positioning and navigation method and system thereof
CN110986879A (en) Power line tower inclination real-time monitoring method and system
Gao et al. Odometer, low-cost inertial sensors, and four-GNSS data to enhance PPP and attitude determination
CN113203418A (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
CN106405592B (en) Vehicle-mounted Beidou carrier phase cycle slips detection and restorative procedure and system
CN110793518B (en) Positioning and attitude determining method and system for offshore platform
EP2193336B1 (en) A positioning system and method
CN115327588A (en) Network RTK-based high-precision positioning method for unmanned automatic operation special vehicle
CN114894181A (en) Real-time autonomous combined navigation positioning method and device
CN115683094A (en) Vehicle-mounted double-antenna tight coupling positioning method and system in complex environment
Du et al. Integration of PPP GPS and low cost IMU
Sollie et al. Pose estimation of UAVs based on INS aided by two independent low-cost GNSS receivers
CN114353835A (en) Dynamic calibration system and method for inertial track measuring instrument and application of dynamic calibration system
Zhang et al. Performance analysis of a tightly coupled kalman filter for the integration of un-differenced GPS and inertial data
WO2005050247A1 (en) Position detection device and method
Cina et al. A full-state low-cost GNSS/INS system for Mobile Mapping Applications

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20200410

RJ01 Rejection of invention patent application after publication