CN114674313B - Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS - Google Patents

Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS Download PDF

Info

Publication number
CN114674313B
CN114674313B CN202210332360.6A CN202210332360A CN114674313B CN 114674313 B CN114674313 B CN 114674313B CN 202210332360 A CN202210332360 A CN 202210332360A CN 114674313 B CN114674313 B CN 114674313B
Authority
CN
China
Prior art keywords
gps
bds
sins
error
equation
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
CN202210332360.6A
Other languages
Chinese (zh)
Other versions
CN114674313A (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.)
Huaiyin Institute of Technology
Original Assignee
Huaiyin Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Huaiyin Institute of Technology filed Critical Huaiyin Institute of Technology
Priority to CN202210332360.6A priority Critical patent/CN114674313B/en
Publication of CN114674313A publication Critical patent/CN114674313A/en
Application granted granted Critical
Publication of CN114674313B publication Critical patent/CN114674313B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/36—Input/output arrangements for on-board computers
    • 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/33—Multimode operation in different systems which transmit time stamped messages, e.g. GPS/GLONASS
    • 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

Abstract

The invention discloses a GPS/BDS and SINS integrated unmanned distribution vehicle navigation positioning method based on CKF algorithm, which comprises the steps of firstly collecting data in a GPS/BDS receiver and an inertia measurement unit on an unmanned distribution vehicle, and using SINS to solve the information of position, speed and attitude of the data collected by an IMU; then establishing a system error state equation of the tight combination of the SINS and the GPS/DBS; then establishing SINS and GPS/BDS measurement equations; substituting the constructed system state error equation and SINS and GPS/BDS measurement equations into a CKF algorithm to obtain an optimal estimation value of the system state; setting a threshold value to judge whether the GPS/BDS has a data signal or not, correcting the measured positioning result to the accumulated error existing in the SINS calculation when the GPS/BDS has the data signal, and compensating and correcting the GPS/BDS by using the positioning result calculated by the SINS when the GPS/BDS has no signal; and finally, filtering and fusing information output by the SINS and the GPS/BDS obtained by the CKF algorithm to obtain optimal estimation of navigation parameters and correct speed information, position information and a strapdown attitude matrix so as to obtain accurate navigation information.

Description

GPS/BDS and SINS integrated unmanned distribution vehicle navigation positioning method based on CKF algorithm
Technical Field
The invention relates to the related fields of unmanned driving, GPS/BDS positioning navigation, IMU inertial measurement units, embedded equipment and the like, in particular to a navigation and positioning method of a GPS/BDS and SINS integrated unmanned distribution vehicle based on a CKF algorithm.
Background
Chinese patent No. CN1034968843A discloses a GPS/SINS ultra-tight integrated navigation system adaptive hybrid filtering method. The method relates to a self-adaptive hybrid filtering method of a GPS/SINS ultra-tight integrated navigation system, which combines SINS data with corresponding GPS data, adopts a self-adaptive Kalman filtering method to establish a GPS/SINS integrated navigation model and obtain an optimal estimation value and an estimation error square matrix of a system state, utilizes an effective estimation value to output and correct navigation positioning information output by the SINS, uses the obtained estimation error square matrix to analyze observable degree information of each state of the system, and takes the observable degree information as a product of a feedback factor and the optimal estimation value obtained by the system as a feedback quantity to perform feedback correction on parameters of the GPS and the SINS.
The method has great limitation in using the traditional Kalman filtering algorithm, cannot achieve the optimal estimation effect in a nonlinear scene, is easily interfered by random errors, cannot provide stable and accurate positioning signals, is fused and positioned with SINS data, and is difficult to achieve the optimal solution due to the fact that the traditional Kalman filter is added in data dimension, so that accurate navigation positioning cannot be obtained. The main disadvantages are as follows:
(1) The number of visible satellites provided by a single GPS positioning system is small, the positioning precision is low, the satellite signal geometry is poor, and good real-time positioning navigation cannot be provided.
(2) When the dimension of the state quantity is too much, the self-adaptive Kalman filtering method has a divergence problem, so that the optimal estimated value obtained by the method cannot be optimal, and a system model is not accurate enough.
(3) The GPS has weak anti-interference capability, when an external environment is received, particularly, the condition of no line of sight exists, GPS data can have the condition of no signal and can not provide navigation positioning information, a data value depending on the previous moment is calculated and compared at the SINS, and then an IMU (inertial measurement unit) calculates an accumulated error, which is difficult to eliminate.
Disclosure of Invention
Aiming at the technical problems, the technical scheme provides a navigation and positioning method of the unmanned distribution vehicle based on the integration of the GPS/BDS and the SINS based on the CKF algorithm, a GPS/BDS dual-mode positioning module and an IMU inertial measurement unit are adopted, the number of visible satellites during positioning, the reliability and the precision of positioning are greatly improved, and the problems can be effectively solved.
The invention is realized by the following technical scheme:
a GPS/BDS and SINS integrated unmanned distribution vehicle navigation positioning method based on CKF algorithm collects data in a GPS/BDS receiver and an inertial measurement unit on an unmanned distribution vehicle, and the data collected by the inertial measurement unit is resolved by the SINS to calculate information such as position, speed, attitude angle, attitude transfer matrix and the like and position and speed information output by the GPS/BDS and are simultaneously input into a CKF filter; the specific implementation steps are as follows:
step 1: collecting data in a GPS/BDS receiver and an inertia measurement unit on the unmanned distribution vehicle, and solving the information of the position, the speed and the attitude of the data collected by the IMU by using SINS;
the true geographic location L, λ, H, true speed is recorded
Figure GDA0004094146920000031
SINS-resolved position information L S ,λ S ,H S Velocity information v es ,v ns ,v us Attitude angle phi es ,φ ns ,φ us (ii) a Position information L measured by GPS/BDS GB ,λ GB ,H GB Velocity information v eGB 、v nGB 、v uGB ；
And 2, step: establishing a system state error equation of a SINS and GPS/DBS tight combination;
and step 3: establishing SINS and GPS/BDS measurement equations;
and 4, step 4: substituting the state error equation and the measurement equation constructed in the step 2 and the step 3 into a CKF algorithm to obtain an optimal estimation value of the system state;
and 5: setting a threshold value to judge whether a GPS/BDS has a data signal or not;
when a signal exists, correcting the accumulated error existing in the SINS calculation by the measured positioning result, and when the GPS/BDS has no signal, compensating and correcting the GPS/BDS by the positioning result calculated by the SINS;
take e = | z k -x k|k-1 The identification range is:
Figure GDA0004094146920000032
wherein z is k As actual observed values, x k|k-1 If the predicted value is a state predicted value and TH is an error threshold value, comparing e with the set threshold value;
step 6: and filtering and fusing the information output by the SINS and the GPS/BDS obtained by the CKF algorithm to obtain the optimal estimation of navigation parameters and correct speed information, position information and a strapdown attitude matrix so as to obtain accurate navigation information.
Further, the establishing of the system state error equation of the tight combination of the SINS and the GPS/DBS in step 2 specifically operates as follows:
step 2.1: establishing a state error equation of SINS;
recording: the attitude angle error is: phi = [ phi ] u φ e φ n ] T (ii) a The speed error is:
Figure GDA0004094146920000033
the position error is: δ P = [ δ L δ λ δ H] T ；b gx 、b gy 、b gz Is the constant error of the three-axis gyroscope; b ax 、b ay 、b az Random error of triaxial acceleration; w is a gx 、w gy 、w gz Noise of the gyroscope under the carrier coordinate system; w is a ax 、w ay 、w az Noise of an accelerometer under a carrier coordinate system;
setting: the state equation of the SINS is as follows: x s (t)＝F s (t)X s (t)+G s (t)W s (t); the state quantity is measured as:
Figure GDA0004094146920000041
W s (t)＝[w gx ,w gy ,w gz ,w ax ,w ay ,w az ] T ；
then: the nonlinear error model is as follows:
Figure GDA0004094146920000042
wherein epsilon b ＝[b gx b gy b gz ]Is a constant error of the lower three-axis gyroscope,
Figure GDA0004094146920000043
is a white noise process with zero mean value of random error of the triaxial gyro under the system b, and>
Figure GDA0004094146920000044
is b is a constant error of the lower triaxial acceleration>
Figure GDA0004094146920000045
Is a white noise process with zero mean value of random error of triaxial acceleration under n series, f b Is the actual output of the accelerometer, is asserted>
Figure GDA0004094146920000046
The calculated rotation angular velocity, the earth rotation angular velocity and the calculated rotation angular velocity of the mathematical platform relative to the earth are respectively; r E 、R N Respectively the curvature radius of a local meridian circle and a prime circle;
step 2.2: establishing a GPS/BDS state error equation;
the state error equation can be expressed as: x GB (t)＝F GB (t)X GB (t)+G GB (t)W GB (t)；
In the formula (I), the compound is shown in the specification,
Figure GDA0004094146920000047
δt GPS 、δt BDS 、δt ru respectively a GPS clock error, a Beidou clock error and an equivalent distance error; w is a tu 、w tru Is white noise;
step 2.3: combining the SINS and GPS/BDS state error equations obtained in the step 2.1 and the step 2.2 to obtain a final system state error equation:
Figure GDA0004094146920000051
that is to say that the first and second electrodes,
Figure GDA0004094146920000052
further, the SINS and GPS/BDS measurement equations established in step 3 are specifically operated as follows:
firstly, respectively obtaining measurement equations of pseudo range and pseudo range rate of the ith satellite corresponding to SINS and GPS/BDS, and then combining to obtain a final measurement equation;
step 3.1: establishing SINS and GPS/BDS pseudo range measurement equations:
setting: the SINS outputs the position information of (x) S ,y S ,z S ) The position coordinates of the GPS/BDS are: (x) GB ,y GB ,z GB ) (ii) a The position of the ith satellite is (x) i ,y i ,z i ) And analyzing by a pseudo-range measurement equation and a Taylor expansion equation to obtain: solving the pseudo range general formula rho of the ith satellite at the position by SINS si ＝r i +e i1 δx+e i2 δy+e i3 δz；
Similarly, the pseudo range general formula of the GPS/BDS is rho GBi ＝r i -δt BDS -δt GPS -v ρ ；
Wherein the content of the first and second substances,
Figure GDA0004094146920000053
v ρ to measure noise;
the final pseudorange measurement equation can be obtained by the two equations: z ρ (t)＝H ρ (t)X(t)+V ρ (t)；
Step 3.2: establishing an SINS and GPS/BDS pseudorange rate measurement equation:
the pseudorange rate refers to the rate of change of the distance between the location and the true location (x, y, z) solved by SINS and GPS/BDS, respectively, and the ith satellite, and is expressed by the following formula:
Figure GDA0004094146920000054
Figure GDA0004094146920000055
then: the pseudorange rate measurement equation is:
Figure GDA0004094146920000056
step 3.3: establishing a combined measurement equation of SINS, GPS/BDS pseudo range and pseudo range rate:
combining the pseudo-range measurement equation and the pseudo-range rate measurement equation in the steps 3.1 and 3.2 to obtain an integral measurement equation;
Figure GDA0004094146920000061
further, the step 4 of substituting the state error equation and the measurement equation constructed in the steps 2 and 3 into the CKF algorithm to obtain the optimal estimation value of the system state includes the following specific operation modes:
substituting the state error equation and the measurement equation constructed in the step 2.3 and the step 3.3 into a CKF algorithm to obtain an optimal estimation value of the system state;
system state error equations and measurement equations in CKF filters:
Figure GDA0004094146920000062
in the formula x k Is a 17-dimensional state vector, and each component is independent and follows Gaussian distribution; z is a radical of formula k 5-dimensional observation vectors; omega k-1 And v k Process noise and measurement noise, respectively, with an average of 0.
Further, the threshold TH set in step 5 is used to determine whether there is a data signal in the GPS/BDS, and the specific implementation method is as follows:
position information L calculated from the SINS S ,λ S ,H S Velocity information v es ,v ns ,v us Attitude phi es ,φ ns ,φ us To itAfter correction:
Figure GDA0004094146920000063
when the GPS/BDS has no signal, the state value of the GPS/BDS is corrected by the position and speed information corrected by the SINS;
when the GPS/BDS receiver has signals, the measured position information L is GB ,λ GB ,H GB Velocity information v eGB 、v nGB 、v uGB After correcting it, it is:
Figure GDA0004094146920000071
and correcting the corrected information to the SINS to eliminate the accumulated error.
Advantageous effects
Compared with the prior art, the navigation and positioning method of the unmanned distribution vehicle based on the CKF algorithm and integrating the GPS/BDS and the SINS has the following beneficial effects:
(1) The technical scheme adopts a GPS/BDS dual-mode positioning module, so that the number of visible satellites during positioning and the reliability and precision of positioning are greatly improved. The CKF filtering is friendly to processing a nonlinear system, and adopts a third-order spherical radial volume rule. The method can effectively solve the problem of state estimation and divergence of a nonlinear system and ensure the stability of numerical values. And setting a Threshold (TH) to judge whether the GPS/BDS has a data signal or not, correcting the accumulative error existing in the SINS calculation by using the measured positioning result when the GPS/BDS has a signal, and compensating the GPS/BDS by using the positioning result calculated by the SINS calculation when the GPS/BDS does not have a signal to further improve the stability of a system model and calculate the optimal navigation positioning information.
(2) The technical scheme adopts the volume Kalman filtering (CKF), is friendly to processing a nonlinear system, and can effectively solve the problems of state estimation and divergence of the nonlinear system and ensure the stability of numerical values.
(3) The technical scheme is that a Threshold (TH) is set to judge whether a GPS/BDS has a data signal or not, when a signal exists, the measured positioning result is used for correcting an accumulated error existing in SINS calculation, and when the GPS/BDS does not have the signal, the GPS/BDS is compensated by the positioning result calculated by the SINS calculation.
Drawings
FIG. 1 is a schematic block diagram of the overall process flow of the present invention.
FIG. 2 is a comparison of position errors for the method of the present invention.
FIG. 3 is a graph comparing velocity errors for the method 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. The described embodiments are only some embodiments of the invention, not all embodiments. Various modifications and improvements of the technical solutions of the present invention may be made by those skilled in the art without departing from the design concept of the present invention, and all of them should fall into the protection scope of the present invention.
Example 1:
as shown in figure 1, a GPS/BDS and SINS integrated unmanned distribution vehicle navigation positioning method based on CKF algorithm collects data in a GPS/BDS receiver and an inertial measurement unit on an unmanned distribution vehicle, and simultaneously inputs information such as position, speed, attitude angle, attitude transfer matrix and the like, and position and speed information output by the GPS/BDS into a CKF filter by using SINS to solve the data collected by the inertial measurement unit; the specific implementation steps are as follows:
step 1: collecting data in a GPS/BDS receiver and an inertia measurement unit on the unmanned distribution vehicle, and solving the information of the position, the speed and the attitude of the data collected by the IMU by using SINS;
selecting a geographical coordinate system of ' northeast-earth ' as a navigation coordinate system n system, a carrier coordinate system b system, recording a mathematical platform obtained by SINS calculation as an n ' system, recording three Euler angles as a course angle psi, a pitch angle theta, a roll angle gamma, real geographical positions L, lambda and H, and recording real speed as a system
Figure GDA0004094146920000091
The gesture matrix between n and b is recorded as ^ m>
Figure GDA0004094146920000092
The gesture matrix between n and n' is recorded as->
Figure GDA0004094146920000093
The gesture matrix between b and n' is recorded as>
Figure GDA0004094146920000094
Position information L calculated by SINS S ,λ S ,H S Velocity information v es ,v ns ,v us Attitude angle phi es ,φ ns ,φ us (ii) a Position information L measured by GPS/BDS GB ,λ GB ,H GB Velocity information v eGB 、v nGB 、v uGB 。
Step 2: establishing a system state error equation of a tight combination of SINS and GPS/DBS; the specific operation mode is as follows:
step 2.1: establishing a state error equation of SINS;
recording: the attitude angle error is: phi = [ phi ] u φ e φ n ] T (ii) a The speed error is:
Figure GDA0004094146920000095
the position error is: δ P = [ δ L δ λ δ H] T ；b gx 、b gy 、b gz Is the constant error of the three-axis gyroscope; b ax 、b ay 、b az Random error of triaxial acceleration; w is a gx 、w gy 、w gz The noise of the gyroscope under the carrier coordinate system is used; w is a ax 、w ay 、w az Noise of an accelerometer under a carrier coordinate system;
setting: the state equation of the SINS is as follows: x s (t)＝F s (t)X s (t)+G s (t)W s (t); the state quantity is measured as:
Figure GDA0004094146920000096
W s (t)＝[w gx ,w gy ,w gz ,w ax ,w ay ,w az ] T ；
then: the nonlinear error model is as follows:
Figure GDA0004094146920000097
wherein epsilon b ＝[b gx b gy b gz ]Is a constant error of the lower three-axis gyroscope,
Figure GDA0004094146920000101
is a white noise process with zero mean value of random error of the lower triaxial gyro in the b series, and then>
Figure GDA0004094146920000102
Is b is a constant error of the lower triaxial acceleration>
Figure GDA0004094146920000103
Is a white noise process with zero mean value of random error of triaxial acceleration under n system, f b For the actual output of the accelerometer, in combination with a signal from the accelerometer>
Figure GDA0004094146920000104
The calculated rotation angular velocity, the earth rotation angular velocity and the calculated rotation angular velocity of the mathematical platform relative to the earth are respectively; r E 、R N The curvature radiuses of the local meridian circle and the prime circle are respectively;
step 2.2: establishing a GPS/BDS state error equation;
the state error equation can be expressed as: x GB (t)＝F GB (t)X GB (t)+G GB (t)W GB (t)；
In the formula (I), the compound is shown in the specification,
Figure GDA0004094146920000105
δt GPS 、δt BDS 、δt ru respectively a GPS clock error, a Beidou clock error and an equivalent distance error; w is a tu 、w tru Is white noise;
step 2.3: combining the SINS and GPS/BDS state error equations obtained in the step 2.1 and the step 2.2 to obtain a final system state error equation:
Figure GDA0004094146920000106
that is to say that the first and second electrodes,
Figure GDA0004094146920000107
and step 3: establishing SINS and GPS/BDS observation equations, wherein the specific operation mode is as follows:
firstly, respectively obtaining measurement equations of pseudo range and pseudo range rate of the ith satellite corresponding to SINS and GPS/BDS, and then combining to obtain a final measurement equation;
step 3.1: establishing an SINS and GPS/BDS pseudorange measurement equation:
setting: the position information output by the SINS is (x) S ,y S ,z S ) The position coordinates of the GPS/BDS are:
(x GB ,y GB ,z GB ) (ii) a The position of the ith satellite is (x) i ,y i ,z i ) And the pseudo-range measurement equation and Taylor expansion analysis are used for obtaining: SINS solves the pseudo range general formula rho of the ith satellite si ＝r i +e i1 δx+e i2 δy+e i3 δz；
Similarly, the pseudo range general formula of the GPS/BDS is rho GBi ＝r i -δt BDS -δt GPS -v ρ ；
Wherein, the first and the second end of the pipe are connected with each other,
Figure GDA0004094146920000111
v ρ to measure noise;
the final pseudo range quantity can be obtained by the two formulasMeasurement equation: z ρ (t)＝H ρ (t)X(t)+V ρ (t)；
Step 3.2: establishing an SINS and GPS/BDS pseudorange rate measurement equation:
the pseudorange rate refers to the rate of change of the distance between the location and the true location (x, y, z) solved by SINS and GPS/BDS, respectively, and the ith satellite, and is expressed by the following formula:
Figure GDA0004094146920000112
/>
Figure GDA0004094146920000113
then: the pseudorange rate measurement equation is:
Figure GDA0004094146920000114
step 3.3: establishing a combined measurement equation of SINS and GPS/BDS pseudo range and pseudo range rate:
combining the pseudo-range measurement equation and the pseudo-range rate measurement equation in the steps 3.1 and 3.2 to obtain an integral measurement equation;
Figure GDA0004094146920000115
and 4, step 4: substituting the state error equation and the measurement equation constructed in the step 2 and the step 3 into a CKF algorithm to obtain an optimal estimation value of the system state;
substituting the state error equation and the measurement equation constructed in the step 2.3 and the step 3.3 into a CKF algorithm to obtain an optimal estimation value of the system state;
the system state error equation and the measurement equation in the CKF filter are as follows:
Figure GDA0004094146920000116
in the formula x k Is a 17-dimensional state vector, and each component is independent and follows Gaussian distribution; z is a radical of k 5-dimensional observation vectors; omega k-1 And v k Process noise and measurement noise, respectively, with an average of 0.
And 5: setting a threshold value to judge whether a GPS/BDS has a data signal or not;
when a signal exists, correcting the accumulated error existing in the SINS calculation by the measured positioning result, and when the GPS/BDS has no signal, compensating and correcting the GPS/BDS by the positioning result calculated by the SINS;
take e = | z k -x k|k-1 The identification range is:
Figure GDA0004094146920000121
wherein z is k As actual observed value, x k|k-1 If the predicted value is a state predicted value and TH is an error threshold value, comparing e with the set threshold value;
the specific implementation method comprises the following steps:
position information L calculated from the SINS S ,λ S ,H S Velocity information v es ,v ns ,v us Attitude phi es ,φ ns ,φ us After correcting it, it is:
Figure GDA0004094146920000122
when the GPS/BDS has no signal, the state value of the GPS/BDS is corrected by the position and speed information corrected by the SINS;
when the GPS/BDS receiver has signals, the measured position information L is GB ,λ GB ,H GB Velocity information v eGB 、v nGB 、v uGB After correcting it, it is:
Figure GDA0004094146920000123
/>
and correcting the corrected information to the SINS to eliminate the accumulated error.
Step 6: and filtering and fusing information output by the SINS and the GPS/BDS obtained by the CKF algorithm to obtain optimal estimation of navigation parameters and correct speed information, position information and a strapdown attitude matrix so as to obtain accurate navigation information.

Claims (5)

1. A GPS/BDS and SINS integrated unmanned distribution vehicle navigation positioning method based on CKF algorithm collects data in a GPS/BDS receiver and an inertial measurement unit on an unmanned distribution vehicle, and the data collected by the inertial measurement unit is resolved by the SINS to calculate information such as position, speed, attitude angle, attitude transfer matrix and the like and position and speed information output by the GPS/BDS and are simultaneously input into a CKF filter; the specific implementation steps are as follows:
step 1: collecting data in a GPS/BDS receiver and an inertia measurement unit on the unmanned distribution vehicle, and solving the information of the position, the speed and the attitude of the data collected by the IMU by using SINS;
the true geographic location L, λ, H, true speed is recorded
Figure FDA0004094146910000011
Position information L calculated by SINS S ,λ S ,H S Velocity information v es ,v ns ,v us Attitude angle phi es ,φ ns ,φ us (ii) a Position information L measured by GPS/BDS GB ,λ GB ,H GB Velocity information v eGB 、v nGB 、v uGB ；
Step 2: establishing a system state error equation of a tight combination of SINS and GPS/DBS;
and step 3: establishing SINS and GPS/BDS measurement equations;
and 4, step 4: substituting the state error equation and the measurement equation constructed in the step 2 and the step 3 into a CKF algorithm to obtain an optimal estimation value of the system state;
and 5: setting a threshold value to judge whether a GPS/BDS has a data signal or not;
when a signal exists, correcting the accumulated error existing in the SINS calculation by the measured positioning result, and when the GPS/BDS has no signal, compensating and correcting the GPS/BDS by the positioning result calculated by the SINS;
take e = | z k -x k|k-1 The identification range is:
Figure FDA0004094146910000012
wherein z is k As actual observed value, x k|k-1 If the predicted value is a state predicted value and TH is an error threshold value, comparing e with the set threshold value;
and 6: and filtering and fusing the information output by the SINS and the GPS/BDS obtained by the CKF algorithm to obtain the optimal estimation of navigation parameters and correct speed information, position information and a strapdown attitude matrix so as to obtain accurate navigation information.
2. The CKF algorithm-based GPS/BDS and SINS fused unmanned distribution vehicle navigation and positioning method as claimed in claim 1, wherein: the specific operation mode of establishing the system state error equation of the SINS and GPS/DBS tight combination in the step 2 is as follows:
step 2.1: establishing a state error equation of SINS;
recording: the attitude angle error is: phi = [ phi ] u φ e φ n ] T (ii) a The speed error is:
Figure FDA0004094146910000021
the position error is: δ P = [ δ L δ λ δ H] T ；b gx 、b gy 、b gz Is the constant error of the triaxial gyro; b ax 、b ay 、b az Random error of triaxial acceleration; w is a gx 、w gy 、w gz The noise of the gyroscope under the carrier coordinate system is used; w is a ax 、w ay 、w az Noise of an accelerometer under a carrier coordinate system;
setting: the state equation of the SINS is as follows: x s (t)＝F s (t)X s (t)+G s (t)W s (t); the state quantity is measured as:
Figure FDA0004094146910000022
W s (t)＝[w gx ,w gy ,w gz ,w ax ,w ay ,w az ] T ；
then: the nonlinear error model is as follows:
Figure FDA0004094146910000023
wherein epsilon b ＝[b gx b gy b gz ]B is the constant error of the lower triaxial gyro,
Figure FDA0004094146910000031
is a white noise process with zero mean value of random error of the lower triaxial gyro in the b series, and then>
Figure FDA0004094146910000032
B is a lower triaxial acceleration constant error, and>
Figure FDA0004094146910000033
is a white noise process with zero mean value of random error of triaxial acceleration under n system, f b Is the actual output of the accelerometer, is asserted>
Figure FDA0004094146910000034
The calculated rotation angular velocity, the earth rotation angular velocity and the calculated rotation angular velocity of the mathematical platform relative to the earth are respectively; r is E 、R N Respectively the curvature radius of a local meridian circle and a prime circle;
step 2.2: establishing a GPS/BDS state error equation;
the state error equation can be expressed as: x GB (t)＝F GB (t)X GB (t)+G GB (t)W GB (t)；
In the formula (I), the compound is shown in the specification,
Figure FDA0004094146910000035
δt GPS 、δt BDS 、δt ru respectively a GPS clock error, a Beidou clock error and an equivalent distance error; w is a tu 、w tru Is white noise;
step 2.3: combining the SINS and GPS/BDS state error equations obtained in the step 2.1 and the step 2.2 to obtain a final system state error equation:
Figure FDA0004094146910000036
that is to say that the first and second electrodes,
Figure FDA0004094146910000037
3. the CKF algorithm-based GPS/BDS and SINS fused unmanned distribution vehicle navigation and positioning method as claimed in claim 2, wherein: establishing SINS and GPS/BDS measurement equations in the step 3, wherein the specific operation mode is as follows:
firstly, respectively obtaining measurement equations of pseudo range and pseudo range rate of the ith satellite corresponding to SINS and GPS/BDS, and then combining to obtain a final measurement equation;
step 3.1: establishing an SINS and GPS/BDS pseudorange measurement equation:
setting: the position information output by the SINS is (x) S ,y S ,z S ) The position coordinates of the GPS/BDS are: (x) GB ,y GB ,z GB ) (ii) a The position of the ith satellite is (x) i ,y i ,z i ) And the pseudo-range measurement equation and Taylor expansion analysis are used for obtaining: solving the pseudo range general formula rho of the ith satellite at the position by SINS si ＝r i +e i1 δx+e i2 δy+e i3 δz；
Similarly, the pseudo range general formula of the GPS/BDS is rho GBi ＝r i -δt BDS -δt GPS -v ρ ；
Wherein the content of the first and second substances,
Figure FDA0004094146910000041
v ρ to measure noise;
the final pseudorange measurement equation can be obtained by the two equations: z is a linear or branched member ρ (t)＝H ρ (t)X(t)+V ρ (t)；
Step 3.2: establishing an SINS and GPS/BDS pseudorange rate measurement equation:
the pseudo-range rate refers to the rate of change of the distance between the position and the real position (x, y, z) and the ith satellite, which are solved by the SINS and the GPS/BDS respectively, and the formula is respectively:
Figure FDA0004094146910000042
Figure FDA0004094146910000043
then: the pseudorange rate measurement equation is:
Figure FDA0004094146910000044
step 3.3: establishing a combined measurement equation of SINS and GPS/BDS pseudo range and pseudo range rate:
combining the pseudo-range measurement equation and the pseudo-range rate measurement equation in the steps 3.1 and 3.2 to obtain an integral measurement equation;
Figure FDA0004094146910000045
4. the CKF algorithm-based GPS/BDS and SINS fused unmanned distribution vehicle navigation positioning method according to claim 3, wherein the method comprises the following steps: and 4, substituting the state error equation and the measurement equation constructed in the steps 2 and 3 into the CKF algorithm to obtain the optimal estimation value of the system state, wherein the specific operation mode is as follows:
substituting the state error equation and the measurement equation constructed in the step 2.3 and the step 3.3 into a CKF algorithm to obtain an optimal estimation value of the system state;
system state error equations and measurement equations in CKF filters:
Figure FDA0004094146910000051
in the formula x k The vector is a 17-dimensional state vector, and each component is independent and follows Gaussian distribution; z is a radical of k 5-dimensional observation vectors; omega k-1 And v k Process noise and measurement noise, respectively, with an average of 0.
5. The GPS/BDS and SINS fused unmanned dispatching vehicle navigation and positioning method based on CKF algorithm as claimed in claim 1, wherein: the threshold TH set in step 5 is used to determine whether there is a data signal in the GPS/BDS, and the specific implementation method is as follows:
position information L calculated from the SINS S ,λ S ,H S Velocity information v es ,v ns ,v us Attitude phi es ,φ ns ,φ us After correcting it, it is:
Figure FDA0004094146910000052
when the GPS/BDS has no signal, the state value of the GPS/BDS is corrected by the position and speed information corrected by the SINS;
when the GPS/BDS receiver has signals, the measured position information L is GB ,λ GB ,H GB Velocity information v eGB 、v nGB 、v uGB After correcting it, it is:
Figure FDA0004094146910000053
and correcting the corrected information to the SINS to eliminate the accumulated error.
CN202210332360.6A 2022-03-31 2022-03-31 Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS Active CN114674313B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210332360.6A CN114674313B (en) 2022-03-31 2022-03-31 Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210332360.6A CN114674313B (en) 2022-03-31 2022-03-31 Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS

Publications (2)

Publication Number Publication Date
CN114674313A CN114674313A (en) 2022-06-28
CN114674313B true CN114674313B (en) 2023-04-04

Family

ID=82076472

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210332360.6A Active CN114674313B (en) 2022-03-31 2022-03-31 Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS

Country Status (1)

Country Link
CN (1) CN114674313B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116224407B (en) * 2023-05-06 2023-07-18 山东科技大学 GNSS and INS integrated navigation positioning method and system

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102636798B (en) * 2012-04-12 2013-10-02 南京航空航天大学 SINS (Strap-down Inertial Navigation System)/GPS (Global Position System) deeply-coupled navigation method based on loop state self-detection
CN103454662B (en) * 2013-09-04 2016-06-29 哈尔滨工程大学 A kind of SINS/ Big Dipper/DVL based on CKF combines alignment methods
CN105737823B (en) * 2016-02-01 2018-09-21 东南大学 A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF
CN106780699B (en) * 2017-01-09 2020-10-16 东南大学 Visual SLAM method based on SINS/GPS and odometer assistance
US11703353B2 (en) * 2019-12-05 2023-07-18 Spireon, Inc. Error correction for GPS-based mileage tracking
CN112146655B (en) * 2020-08-31 2023-03-31 郑州轻工业大学 Elastic model design method for BeiDou/SINS tight integrated navigation system
CN111982126B (en) * 2020-08-31 2023-03-17 郑州轻工业大学 Design method of full-source BeiDou/SINS elastic state observer model

Also Published As

Publication number Publication date
CN114674313A (en) 2022-06-28

Similar Documents

Publication Publication Date Title
CN108226980B (en) Differential GNSS and INS self-adaptive tightly-coupled navigation method based on inertial measurement unit
CN105606094B (en) A kind of information condition matched filtering method of estimation based on MEMS/GPS combined systems
CN110779521A (en) Multi-source fusion high-precision positioning method and device
CN111156994B (en) INS/DR & GNSS loose combination navigation method based on MEMS inertial component
CN101846734B (en) Agricultural machinery navigation and position method and system and agricultural machinery industrial personal computer
CN110567454B (en) SINS/DVL tightly-combined navigation method in complex environment
CN112505737B (en) GNSS/INS integrated navigation method
CN104181572A (en) Missile-borne inertia/ satellite tight combination navigation method
CN113203418B (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
CN111044075B (en) SINS error online correction method based on satellite pseudo-range/relative measurement information assistance
CN109059909A (en) Satellite based on neural network aiding/inertial navigation train locating method and system
CN112432642B (en) Gravity beacon and inertial navigation fusion positioning method and system
CN109471144A (en) Based on pseudorange/pseudorange rates multisensor tight integration train combined positioning method
CN112762961B (en) On-line calibration method for integrated navigation of vehicle-mounted inertial odometer
CN111722295B (en) Underwater strapdown gravity measurement data processing method
CN115388884A (en) Joint initialization method for intelligent body pose estimator
CN110133695A (en) A kind of double antenna GNSS location delay time dynamic estimation system and method
CN114674313B (en) Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS
CN111595331A (en) Clock model assisted inertial/satellite/relative ranging information combined navigation method
CN108151765A (en) Attitude positioning method is surveyed in a kind of positioning of online real-time estimation compensation magnetometer error
CN114526731A (en) Inertia combination navigation direction positioning method based on moped
CN113253325A (en) Inertial satellite sequential tight combination lie group filtering method
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
RU2277696C2 (en) Integrated satellite inertial-navigational system
CN116576849A (en) Vehicle fusion positioning method and system based on GMM assistance

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