CN115164886B - Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system - Google Patents

Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system Download PDF

Info

Publication number
CN115164886B
CN115164886B CN202210868179.7A CN202210868179A CN115164886B CN 115164886 B CN115164886 B CN 115164886B CN 202210868179 A CN202210868179 A CN 202210868179A CN 115164886 B CN115164886 B CN 115164886B
Authority
CN
China
Prior art keywords
error
scale factor
gnss
state quantity
navigation
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
CN202210868179.7A
Other languages
Chinese (zh)
Other versions
CN115164886A (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.)
Jilin University
Original Assignee
Jilin University
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 Jilin University filed Critical Jilin University
Priority to CN202210868179.7A priority Critical patent/CN115164886B/en
Publication of CN115164886A publication Critical patent/CN115164886A/en
Application granted granted Critical
Publication of CN115164886B publication Critical patent/CN115164886B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/35—Constructional details or hardware or software details of the signal processing chain
    • G01S19/37—Hardware or software details of the signal processing chain
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • 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

Abstract

The invention discloses a scale factor error compensation method of a vehicle-mounted GNSS/INS integrated navigation system, and belongs to the technical field of navigation. The method comprises the following specific steps: firstly, establishing an INS error equation; establishing a GNSS/INS integrated navigation system model; thirdly, characterizing the scale factor error in a GNSS/INS integrated navigation system model equation; and fourthly, decoupling the GNSS/INS integrated navigation system model containing the scale factor error to obtain a parallel navigation state quantity estimator and a scale factor error observer which are used for simultaneously estimating the navigation state quantity and the scale factor error. The Kalman filtering-based navigation state quantity and scale factor error decoupling estimation algorithm provided by the invention can realize the estimation of the scale factor error, thereby compensating the estimation error of the navigation state quantity in real time. Simulation results show that the method can improve the precision of the GNSS/INS integrated navigation system; and particularly, the divergence phenomenon of pure inertial navigation solution under the condition of GNSS signal failure can be relieved.

Description

Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system
Technical Field
The invention belongs to the technical field of navigation, and particularly relates to a scale factor error compensation method of a vehicle-mounted GNSS/INS integrated navigation system.
Background
The global navigation satellite system (Global Navigation Satellite System, GNSS) has the advantages of high positioning accuracy, small volume, no accumulation of errors with time, easy fusion with other sensors and the like, and has the defects that signals are easily affected by external environments, such as signals in a basement or a high building are shielded, and the GNSS fails. The inertial navigation system (Inertial Navigation System, INS) can provide information of position, speed and attitude at a higher frequency than GNSS, and can avoid interference and shielding of external signals, but navigation accuracy is easily affected by errors of inertial sensors, and navigation errors are sharply increased when used alone for a long time. The GNSS/INS integrated navigation system utilizes the output of an inertial measurement unit (Inertial Measurement Unit, IMU) accelerometer and a gyroscope to calculate a navigation result, corrects an INS navigation error in a Kalman Filter (KF) based on an INS error model through the difference value of navigation results of two subsystems, and improves the overall navigation performance. However, when the GNSS fails, the Kalman filter cannot obtain the observed quantity and cannot work normally, and the integrated navigation system can only obtain the navigation result through INS solution.
In order to reduce adverse effects of GNSS failure on integrated navigation accuracy, the methods currently employed mainly include: (1) Auxiliary sensors such as a visual sensor, an odometer and a magnetic compass are added to provide the navigation state observance quantity required by the Kalman filter. Clearly, the secondary sensor adds to the hardware cost and does not provide absolute position information as a GNSS. (2) And a map matching algorithm is adopted to enhance GNSS and correct INS navigation errors. The algorithm relies on a prefabricated high-precision map and cannot be updated in real time. (3) By means of a neural network algorithm, a good experience model is established through training to predict the speed and position errors when GNSS signals are lost, the speed and position errors are provided for a filter to estimate and correct INS navigation errors, and divergence of the navigation errors is reduced. The neural network prediction method relies on a model trained by a large amount of data, and is inconvenient for engineering application.
When the GNSS fails, the main factor affecting the accuracy of the INS solution is the measurement error of the IMU. If the IMU measurement errors are reasonably modeled and integrated into the combined navigation error model state equation as system state quantities for estimation, the feedback correction sensor errors will improve the navigation solution accuracy. This approach is widely adopted since it is also helpful for improving the accuracy of integrated navigation systems when GNSS is active. IMU measurement errors can be divided into two parts: deterministic errors and random errors. Deterministic errors mainly include constant errors, scale factor errors, and the like. The computational effort of the kalman filter is approximately proportional to the 3 rd power of the state quantity dimension. In order to achieve both model accuracy and calculation amount, the IMU error is usually modeled in a form of 'random constant value plus white noise', and errors such as a scale factor and the like are not counted. In practice, scaling factor errors occur in both the processing process and operating temperature variations of low cost IMUs. A sensor that is not well calibrated will have a large residual scale factor error. Some consumer IMUs may even have scale factor errors up to 10%. Unlike random constant errors, the scale factor error is constant, but the magnitude of the error that it causes is not fixed, but rather is related to the measurement of the IMU. Due to the influence of the sensor scale factor error, IMU error modeling is inaccurate, and model errors exist in the system.
The above methods have certain problems. Therefore, research on a GNSS/INS integrated navigation Kalman filtering algorithm which can estimate and compensate the error of the scale factor so as to improve the model precision and not cause the rapid increase of the calculated amount has positive significance.
Disclosure of Invention
In order to solve the problems of identification, estimation and compensation of the scale factor error in the IMU model, the invention provides a scale factor error compensation method of a vehicle-mounted GNSS/INS integrated navigation system.
The technical scheme adopted by the invention is as follows:
a scale factor error compensation method of a vehicle-mounted GNSS/INS integrated navigation system comprises the following steps:
step one, an INS error equation is established, and the expression is as follows:
wherein ,ψn 、δV n and δPn Respectively an attitude error, a speed error and a position error under a navigation coordinate system (n system); f (f) n The specific force is the specific force under the navigation coordinate system;a direction cosine matrix which is converted from a vehicle body coordinate system (b system) to a navigation coordinate system; />For the projection of the rotational angular velocity of the earth in the navigation coordinate system,/->For navigating the coordinate systemProjection of the angular velocity of the earth coordinate system on the navigation coordinate system, < >>The projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system; δf b and δωb Measuring errors of an accelerometer and a gyroscope under a vehicle body system respectively;
step two, establishing a GNSS/INS integrated navigation system model
The accelerometer and gyroscope error models are respectively:
δf＝b f +ε f
δω＝b ω +ε ω
in the formula ,
wherein δf and δω are the errors of the accelerometer and gyroscope, respectively; b f and bω Random constant errors of the accelerometer and the gyroscope respectively; epsilon f and εω Representing random noise in the accelerometer and gyroscope, respectively; combining with an INS error equation to obtain a GNSS/INS integrated navigation system model based on Kalman filtering;
step three, the scale factor error is expressed in a GNSS/INS integrated navigation system model equation
When there is a scale factor error in accelerometer and gyroscope measurements, their error models can be expressed as:
wherein , and />Model errors in the accelerometer and gyroscope, respectively, generated by scale factor errors; s is(s) f and sω The scale factor errors in the accelerometer and the gyroscope are respectively represented and are constants; f and ω represent measurements of the accelerometer and gyroscope, respectively;
and step four, decoupling the GNSS/INS integrated navigation system model containing the scale factor error to obtain a parallel navigation state quantity estimator and a scale factor error observer which are used for simultaneously estimating the navigation state quantity and the scale factor error.
Further, in the second step, the first step,
the GNSS/INS integrated navigation system model equation based on the Kalman filtering is as follows:
where w and v are zero-mean and uncorrelated gaussian white noise;
y＝[V INS -V GNSS P INS -P GNSS ] T ＝[δV 3×1 δP 3×1 ] T ；
C＝[0 6×3 I 6×6 0 6×6 ]；
wherein, psi, δV, δP are respectively an attitude error, a velocity error and a position error, b f and bω Random constant errors of accelerometer and gyroscope, V INS and PINS Velocity and position, V, respectively, of INS output GNSS and PGNSS The speed and the position of GNSS output are respectively, 0 is zero matrix, and I is identity matrix.
Further, step three, characterizing the scale factor error in a GNSS/INS integrated navigation system model equation, and discretizing the equation to obtain the following steps:
wherein I is an identity matrix; a is that k The state transition matrix is obtained by discretizing a matrix F; b k Scaling factor error vectors for gyroscopes and accelerometers; b (B) k A propagation matrix representing the entry of model errors into the state equation; the other symbols have the same meaning and are only converted into a discretization form; and has the following steps:
A k ＝(I+FΔt)；
wherein Δt is the system sampling period, ω b and f b Measured values of a gyroscope and an accelerometer under a car body system respectively.
Further, in the fourth step, the navigation state quantity estimator iterates the process as follows:
when k=0, initializing:
in the formula ,is the initial value of the state quantity; />The initial value of the state quantity error covariance; x is x 0 Is the true value of the state quantity,
when k=1, 2,3 … …,
updating state quantity prior estimation value: in the formula ,/>Is a state quantity a priori estimate,/->Is a state quantity posterior estimate;
state quantity prior error covariance update: in the formula ,/>Is the state quantity prior error covariance of the navigation state quantity estimator,/>Is the state quantity posterior error covariance of the navigation state quantity estimator, Q k-1 Is the noise w in the model equation of the integrated navigation system k-1 Is a covariance of (2);
calculating Kalman gain: in the formula ,Kx,k Kalman gain, R, is the navigation state quantity estimator k Is the noise v in the model equation of the integrated navigation system k Is a covariance of (2);
update of the innovation: in the formula ,rx,k Is a new information of the navigation state quantity estimator;
updating state quantity posterior estimation values:
state quantity posterior error covariance update:
further, in the fourth step, the scale factor error observer iterative process is:
when k=0, initializing:
in the formula ,an initial value of the error amount of the scaling factor; u (U) 0 The initial value of the iteration matrix is; p (P) b,0 An error covariance initial value which is the error amount of the scale factor; b 0 Is the true value of the error amount of the scale factor;
when k=1, 2,3 … …,
priori estimationAnd (5) value counting and updating: in the formula ,/>Is a priori estimate of the error amount of the scale factor, +.>Is a posterior estimate of the scale factor error amount;
updating an algorithm iteration matrix: in the formula ,Vk 、U k 、S k The method is an intermediate link matrix which is convenient to set for algorithm iterative computation, and has no special meaning;
error covariance matrix update: in the formula ,Pb,k An error covariance that is the amount of scale factor error;
calculating Kalman gain: in the formula ,Kb,k The kalman gain of the scale factor error observer;
update of the innovation: in the formula ,rb,k Is a innovation of the scale factor error observer,
updating the posterior estimated value:
further, in the fourth step, the first step,
at the end of each iteration, the estimated value of the navigation state quantity is compensated by the estimated value of the proportional factor error, so that the optimal estimation of the navigation state quantity is obtained:
in the formula ,is the optimal estimated value of the navigation state quantity.
Compared with the prior art, the invention provides a scale factor error compensation method of a vehicle-mounted GNSS/INS integrated navigation system, which has the following beneficial effects:
the invention maintains the advantages of GNSS and INS combined navigation, and the navigation state quantity estimator fuses the information of the two systems, thereby improving the navigation precision; meanwhile, the parallel scale factor error observer can estimate the scale factor error in the IMU measurement error, correct the IMU measurement error equation in time, and avoid inaccurate navigation state quantity estimation caused by model error, so that the navigation precision is reduced. The invention realizes the identification and estimation of the proportional factor model error, and can carry out error compensation on the navigation state quantity to obtain more accurate navigation state quantity estimation; meanwhile, the calculation amount of the navigation filter is reduced through model decoupling.
Drawings
FIG. 1 is a flowchart of a parallel estimation algorithm for navigation state quantity and scale factor error in an embodiment of a scale factor error compensation method of a vehicle-mounted GNSS/INS integrated navigation system according to the present invention.
FIG. 2 is a diagram illustrating a result of a navigation simulation before GNSS signal failure in an embodiment of a scale factor error compensation method for a vehicle-mounted GNSS/INS integrated navigation system according to the present invention.
FIG. 3 is a scale factor error estimation result in an embodiment of a scale factor error compensation method for a GNSS/INS integrated navigation system.
FIG. 4 is a navigation simulation result during GNSS signal failure in an embodiment of a scale factor error compensation method for a vehicle-mounted GNSS/INS integrated navigation system according to the present invention.
FIG. 5 is a navigation simulation result after GNSS signal recovery in an embodiment of the scale factor error compensation method of the vehicle-mounted GNSS/INS integrated navigation system of the present invention.
Detailed Description
The invention is further described below in connection with specific embodiments. The examples are intended to illustrate the invention but are not to be construed as limiting the invention.
The invention provides a scale factor error compensation method of a vehicle-mounted GNSS/INS integrated navigation system, which comprises the following steps:
step one, an INS error equation is established.
The INS error equation expression is:
wherein ,ψn 、δV n and δPn Respectively an attitude error, a speed error and a position error under a navigation coordinate system (n system); f (f) n The specific force is the specific force under the navigation coordinate system;a direction cosine matrix which is converted from a vehicle body coordinate system (b system) to a navigation coordinate system; />For the projection of the rotational angular velocity of the earth in the navigation coordinate system,/->For the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, +.>The projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system; δf b and δωb And measuring errors of an accelerometer and a gyroscope under a car body system respectively.
And step two, establishing a GNSS/INS integrated navigation system model.
Modeling the IMU error by adopting a form of random constant value plus white noise, namely, an accelerometer and a gyroscope error model are respectively:
δf＝b f +ε f
δω＝b ω +ε ω
in the formula ,
δf and δω are the errors of the accelerometer and gyroscope, respectively; b f and bω Random constant errors of the accelerometer and the gyroscope respectively; epsilon f and εω Representing random noise in the accelerometer and gyroscope, respectively.
And then combining with an INS error equation to obtain a GNSS/INS integrated navigation system model equation based on Kalman filtering, wherein the model equation is as follows:
where w and v are zero-mean and uncorrelated gaussian white noise;
y＝[V INS -V GNSS P INS -P GNSS ] T ＝[δV 3×1 δP 3×1 ] T ；
C＝[0 6×3 I 6×6 0 6×6 ]。
wherein, psi, δV, δP are respectively an attitude error, a velocity error and a position error, b f and bω Random constant errors of accelerometer and gyroscope, V INS and PINS Velocity and position, V, respectively, of INS output GNSS and PGNSS The speed and the position of GNSS output are respectively, 0 is zero matrix, and I is identity matrix.
The Kalman filtering-based integrated navigation algorithm for modeling the IMU error in the form of 'random constant + white noise' is a common GNSS/INS integrated navigation Kalman filtering algorithm, wherein the scale factor error is not considered.
And thirdly, characterizing the scale factor error in a GNSS/INS integrated navigation system model equation.
When there is a scale factor error in accelerometer and gyroscope measurements, their error models can be expressed as:
wherein , and />Model errors in the accelerometer and gyroscope, respectively, generated by scale factor errors; s is(s) f and sω The scale factor errors in the accelerometer and the gyroscope are respectively represented and are constants; f and ω represent measurements of the accelerometer and gyroscope, respectively.
Characterizing the scale factor error in a GNSS/INS integrated navigation system model equation, and discretizing the equation to obtain the following steps:
wherein I is an identity matrix; a is that k The state transition matrix is obtained by discretizing a matrix F; b k Scaling factor error vectors for gyroscopes and accelerometers; b (B) k A propagation matrix representing the entry of model errors into the state equation; the other symbols have the same meaning and are only converted into a discretization form; and has the following steps:
A k ＝(I+FΔt)；
wherein Δt is the system sampling period, ω b and f b Measured values of a gyroscope and an accelerometer under a car body system respectively.
And step four, decoupling the GNSS/INS integrated navigation system model containing the scale factor error to obtain a parallel navigation state quantity estimator and a scale factor error observer which are used for simultaneously estimating the navigation state quantity and the scale factor error.
In the case of a higher state quantity dimension, this method of decoupling a single kalman filter into two parallel filters can significantly reduce the computational effort.
(1) Navigation state quantity estimator iterative process:
when k=0, initializing:
in the formula ,is the initial value of the state quantity; />The initial value of the state quantity error covariance; x is x 0 Is the true value of the state quantity.
When k=1, 2,3 … …,
updating state quantity prior estimation value: in the formula ,/>Is a state quantity a priori estimate,/->Is a state quantity posterior estimate.
State quantity prior error covariance update: in the formula ,/>Is the state quantity prior error covariance of the navigation state quantity estimator,/>Is the state quantity posterior error covariance of the navigation state quantity estimator, Q k-1 Is the noise w in the model equation of the integrated navigation system k-1 Is a covariance of (c).
Calculating Kalman gain: in the formula ,Kx,k Kalman gain, R, is the navigation state quantity estimator k Is the noise v in the model equation of the integrated navigation system k Is a covariance of (c).
Update of the innovation: in the formula ,rx,k Is a innovation of the navigation state quantity estimator.
Updating state quantity posterior estimation values:
state quantity posterior error covariance update:
(2) Scaling factor error observer iterative process:
when k=0, initializing:
in the formula ,an initial value of the error amount of the scaling factor; u (U) 0 The initial value of the iteration matrix is; p (P) b,0 An error covariance initial value which is the error amount of the scale factor; b 0 Is the true value of the scale factor error amount.
When k=1, 2,3 … …,
updating the prior estimated value: in the formula ,/>Is a priori estimate of the error amount of the scale factor, +.>Is a scale factor error amount posterior estimate.
Updating an algorithm iteration matrix: in the formula ,Vk 、U k 、S k The method is an intermediate link matrix which is convenient to set for algorithm iterative computation, and has no special meaning.
Error covariance matrix update: in the formula ,Pb,k Is the error covariance of the scale factor error amount.
Calculating Kalman gain: in the formula ,Kb,k Is the kalman gain of the scale factor error observer.
Update of the innovation: in the formula ,rb,k Is a innovation of the scale factor error observer.
Updating the posterior estimated value:
at the end of each iteration, the estimated value of the navigation state quantity is compensated by the estimated value of the proportional factor error, so that the optimal estimation of the navigation state quantity is obtained:
in the formula ,is the optimal estimated value of the navigation state quantity.
The navigation state quantity and the scale factor error parallel estimation and scale factor error compensation method are provided, and the flow chart of the method is shown in fig. 1. For convenience, the algorithm proposed by the invention is simply referred to as the BCKF (Bias-corrected Kalman Filter) algorithm.
The BCKF algorithm is compared with a common combined navigation state quantity estimation algorithm (hereinafter referred to as KF algorithm) based on Kalman filtering without considering scale factor errors through simulation. Assume that there is a 1% scale factor error in the IMU measurement. The simulation process is divided into three sections, wherein the first section is the front 2000s, and GNSS signals are normal; in the second period of 2000s to 2060s, GNSS signals fail, and the system performs pure inertial navigation solution; and the third section is 2060s to the last, the GNSS signals are recovered, and the system carries out integrated navigation solution again.
FIG. 2 is a navigation solution before GNSS signal failure. It can be seen that under the premise that the GNSS signals are effective, the BCKF algorithm and the KF algorithm can well estimate the navigation error state quantity. Since the BCKF algorithm can realize effective estimation and compensation of the error of the comparison factor, the error of the horizontal direction speed and the position estimation is slightly smaller than that of the KF algorithm, and the advantages are obvious in estimating the navigation angle. Fig. 3 shows that the BCKF algorithm can track the scale factor error to a true value of 1%, i.e. the scale factor error of the accelerometer and gyroscope can be estimated effectively.
FIG. 4 is a result of the system performing inertial navigation solution during GNSS signal failure. The maximum speed and the position error of the horizontal direction of the BCKF algorithm in the stage are respectively 0.16m/s and 3.96m, the course angle error is 1.38', and the maximum speed and the position error are far smaller than 0.28m/s, 12.96m and 24.14' of the KF algorithm. The BCKF algorithm is shown to be capable of maintaining good navigation precision when the system performs pure inertial navigation solution during the GNSS signal failure period after effective estimation and compensation of the scale factor errors of the accelerometer and the gyroscope in the early stage of the first stage.
Fig. 5 is a system navigation solution result after GNSS signal recovery. It can be seen that the navigation accuracy of the BCKF algorithm is still superior to that of the KF algorithm, and particularly, the fluctuation of the heading angle error curve and the horizontal direction speed error curve of the BCKF is obviously reduced compared with that of the KF algorithm, and the error value is also obviously reduced.
The invention provides a method for realizing real-time estimation and compensation of a scale factor error in a vehicle-mounted GNSS/INS integrated navigation system from the angle of Kalman filtering model error. The simulation result shows that the algorithm has good adaptability to complex working conditions of the vehicle navigation system, can realize real-time online estimation of the scale factor error, and improves the accuracy of state quantity estimation of the navigation system. It should be emphasized that, because GNSS signals are often blocked during the movement of the vehicle, the integrated navigation system can only perform pure inertial navigation calculation at this time, and the method provided by the present invention can provide a certain help for improving the navigation accuracy of the system under such a working condition.
The foregoing is only a preferred embodiment of the present invention. It should be noted that modifications can be made by those skilled in the art without departing from the principles of the present invention, which modifications are also to be considered as being within the scope of the present invention.

Claims (6)

1. The scale factor error compensation method of the vehicle-mounted GNSS/INS integrated navigation system is characterized by comprising the following steps of:
step one, an INS error equation is established, and the expression is as follows:
wherein ,ψn 、δV n and δPn Respectively an attitude error, a speed error and a position error under a navigation coordinate system, wherein the navigation coordinate system is an n-system; f (f) n The specific force is the specific force under the navigation coordinate system;conversion for a vehicle body coordinate systemA direction cosine matrix to a navigation coordinate system, wherein a vehicle body coordinate system is a b system; />For the projection of the rotational angular velocity of the earth in the navigation coordinate system,/->For the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, +.>The projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system; δf b and δωb Measuring errors of an accelerometer and a gyroscope under a vehicle body system respectively;
step two, establishing a GNSS/INS integrated navigation system model
The accelerometer and gyroscope error models are respectively:
δf＝b f +ε f
δω＝b ω +ε ω
in the formula ,
wherein δf and δω are the errors of the accelerometer and gyroscope, respectively; b f and bω Random constant errors of the accelerometer and the gyroscope respectively; epsilon f and εω Representing random noise in the accelerometer and gyroscope, respectively; combining with an INS error equation to obtain a GNSS/INS integrated navigation system model based on Kalman filtering;
step three, the scale factor error is expressed in a GNSS/INS integrated navigation system model equation
When there is a scale factor error in accelerometer and gyroscope measurements, their error models can be expressed as:
wherein , and />Model errors in the accelerometer and gyroscope, respectively, generated by scale factor errors; s is(s) f and sω The scale factor errors in the accelerometer and the gyroscope are respectively represented and are constants; f and ω represent measurements of the accelerometer and gyroscope, respectively;
and step four, decoupling the GNSS/INS integrated navigation system model containing the scale factor error to obtain a parallel navigation state quantity estimator and a scale factor error observer which are used for simultaneously estimating the navigation state quantity and the scale factor error.
2. The method for scale factor error compensation of an on-board GNSS/INS integrated navigation system as set forth in claim 1, wherein, in step two,
the GNSS/INS integrated navigation system model equation based on the Kalman filtering is as follows:
where w and v are zero-mean and uncorrelated gaussian white noise;
y＝[V INS -V GNSS P INS -P GNSS ] T ＝[δV 3×1 δP 3×1 ] T ；
C＝[0 6×3 I 6×6 0 6×6 ]；
wherein, psi, δV, δP are respectively an attitude error, a velocity error and a position error, b f and bω Random constant errors of accelerometer and gyroscope, V INS and PINS Velocity and position, V, respectively, of INS output GNSS and PGNSS The speed and the position of GNSS output are respectively, 0 is zero matrix, and I is identity matrix.
3. The method for compensating the scale factor error of the vehicle-mounted GNSS/INS integrated navigation system according to claim 2, wherein the third step is to characterize the scale factor error in a model equation of the GNSS/INS integrated navigation system and discretize the equation to obtain:
wherein I is an identity matrix; a is that k The state transition matrix is obtained by discretizing a matrix F; b k Scaling factor error vectors for gyroscopes and accelerometers; b (B) k A propagation matrix representing the entry of model errors into the state equation; the other symbols have the same meaning and are only converted into a discretization form; and has the following steps:
A k ＝(I+F△t)；
wherein Δt is the system sampling period, ω b and fb Measured values of a gyroscope and an accelerometer under a car body system respectively.
4. The method for compensating scale factor error in a vehicle-mounted GNSS/INS integrated navigation system according to claim 3, wherein in the fourth step, the navigation state quantity estimator iterates the process as follows:
when k=0, initializing:
in the formula ,is the initial value of the state quantity; />Is a state quantity error assistant recipeA difference initial value; x is x 0 Is the true value of the state quantity,
when k=1, 2,3 … …,
updating state quantity prior estimation value:
in the formula ,is a state quantity a priori estimate,/->Is a state quantity posterior estimate;
state quantity prior error covariance update:
in the formula ,is the state quantity prior error covariance of the navigation state quantity estimator,/>Is the state quantity posterior error covariance of the navigation state quantity estimator, Q k-1 Is the noise w in the model equation of the integrated navigation system k-1 Is a covariance of (2);
calculating Kalman gain:
in the formula ,Kx,k Kalman gain, R, is the navigation state quantity estimator k Is the noise v in the model equation of the integrated navigation system k Is a covariance of (2);
update of the innovation:
in the formula ,rx,k Is a new information of the navigation state quantity estimator;
updating state quantity posterior estimation values:
state quantity posterior error covariance update:
5. the method for compensating scale factor error in a vehicle-mounted GNSS/INS integrated navigation system according to claim 4, wherein in the fourth step, the scale factor error observer iterative process is as follows:
when k=0, initializing:
U 0 ＝0,/>
in the formula ,an initial value of the error amount of the scaling factor; u (U) 0 The initial value of the iteration matrix is; p (P) b,0 An error covariance initial value which is the error amount of the scale factor; b 0 Is the true value of the error amount of the scale factor;
when k=1, 2,3 … …,
updating the prior estimated value:
in the formula ,is a priori estimate of the error amount of the scale factor, +.>Is a posterior estimate of the scale factor error amount;
updating an algorithm iteration matrix:
in the formula ,Vk 、U k 、S k The method is an intermediate link matrix which is convenient to set for algorithm iterative computation, and has no special meaning;
error covariance matrix update:
in the formula ,Pb,k An error covariance that is the amount of scale factor error;
calculating Kalman gain:
in the formula ,Kb,k The kalman gain of the scale factor error observer;
update of the innovation:
in the formula ,rb,k Is a innovation of the scale factor error observer,
updating the posterior estimated value:
6. the method for scale factor error compensation of a vehicle GNSS/INS integrated navigation system as set forth in claim 5, wherein, in step four,
at the end of each iteration, the estimated value of the navigation state quantity is compensated by the estimated value of the proportional factor error, so that the optimal estimation of the navigation state quantity is obtained:
in the formula ,is the optimal estimated value of the navigation state quantity.
CN202210868179.7A 2022-07-22 2022-07-22 Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system Active CN115164886B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210868179.7A CN115164886B (en) 2022-07-22 2022-07-22 Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210868179.7A CN115164886B (en) 2022-07-22 2022-07-22 Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system

Publications (2)

Publication Number Publication Date
CN115164886A CN115164886A (en) 2022-10-11
CN115164886B true CN115164886B (en) 2023-09-05

Family

ID=83496113

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210868179.7A Active CN115164886B (en) 2022-07-22 2022-07-22 Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system

Country Status (1)

Country Link
CN (1) CN115164886B (en)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106840211A (en) * 2017-03-24 2017-06-13 东南大学 A kind of SINS Initial Alignment of Large Azimuth Misalignment On methods based on KF and STUPF combined filters
CN107643534A (en) * 2017-09-11 2018-01-30 东南大学 A kind of dual rate kalman filter method based on GNSS/INS deep integrated navigations
CN110501024A (en) * 2019-04-11 2019-11-26 同济大学 A kind of error in measurement compensation method of vehicle-mounted INS/ laser radar integrated navigation system
CN110988951A (en) * 2019-12-06 2020-04-10 中国地质大学（北京） Multi-source data fusion real-time navigation positioning method and system
CN113625174A (en) * 2021-06-18 2021-11-09 吉林大学 Lithium ion battery SOC and capacity joint estimation method

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9791575B2 (en) * 2016-01-27 2017-10-17 Novatel Inc. GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106840211A (en) * 2017-03-24 2017-06-13 东南大学 A kind of SINS Initial Alignment of Large Azimuth Misalignment On methods based on KF and STUPF combined filters
CN107643534A (en) * 2017-09-11 2018-01-30 东南大学 A kind of dual rate kalman filter method based on GNSS/INS deep integrated navigations
CN110501024A (en) * 2019-04-11 2019-11-26 同济大学 A kind of error in measurement compensation method of vehicle-mounted INS/ laser radar integrated navigation system
CN110988951A (en) * 2019-12-06 2020-04-10 中国地质大学（北京） Multi-source data fusion real-time navigation positioning method and system
CN113625174A (en) * 2021-06-18 2021-11-09 吉林大学 Lithium ion battery SOC and capacity joint estimation method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于KF的锂离子电池SOC估计的模型误差研究;穆嘉毅;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》(第11期);第C035-366页 *

Also Published As

Publication number Publication date
CN115164886A (en) 2022-10-11

Similar Documents

Publication Publication Date Title
US7643939B2 (en) Methods and systems for implementing an iterated extended Kalman filter within a navigation system
CN111156994B (en) INS/DR &amp; GNSS loose combination navigation method based on MEMS inertial component
JP4789216B2 (en) Improved GPS cumulative delta distance processing method for navigation applications
CN110017837B (en) Attitude anti-magnetic interference combined navigation method
CN112505737B (en) GNSS/INS integrated navigation method
EP2026037A2 (en) Navigation system and corresponding method for gyrocompass alignment using dynamically calibrated sensor data and an iterative extended kalman filter
CN108827310A (en) A kind of star sensor secondary gyroscope online calibration method peculiar to vessel
CN112146655B (en) Elastic model design method for BeiDou/SINS tight integrated navigation system
CN109870173A (en) A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
CN109000640A (en) Vehicle GNSS/INS Combinated navigation method based on discrete Grey Neural Network Model
CN112432642B (en) Gravity beacon and inertial navigation fusion positioning method and system
Zhao et al. Adaptive two-stage Kalman filter for SINS/odometer integrated navigation systems
CN114545472B (en) Navigation method and device of GNSS/INS combined system
CN115060257B (en) Vehicle lane change detection method based on civil-grade inertia measurement unit
CN116007620A (en) Combined navigation filtering method, system, electronic equipment and storage medium
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN111982126B (en) Design method of full-source BeiDou/SINS elastic state observer model
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN115164886B (en) Scale factor error compensation method of vehicle-mounted GNSS/INS integrated navigation system
CN114935345A (en) Vehicle-mounted inertial navigation mounting angle error compensation method based on pattern recognition
Fang et al. Integrating SINS sensors with odometer measurements for land vehicle navigation system
Eldesoky et al. Real time localization solution for land vehicle application using low-cost integrated sensors with GPS
CN111238530B (en) Initial alignment method for air moving base of strapdown inertial navigation system
CN117367430A (en) Vehicle-mounted positioning method for improved factor graph
CN116642495A (en) MVC-based adaptive robust Kalman filtering integrated navigation method

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