CN113551669B - Combined navigation positioning method and device based on short base line - Google Patents
Combined navigation positioning method and device based on short base line Download PDFInfo
- Publication number
- CN113551669B CN113551669B CN202110841082.2A CN202110841082A CN113551669B CN 113551669 B CN113551669 B CN 113551669B CN 202110841082 A CN202110841082 A CN 202110841082A CN 113551669 B CN113551669 B CN 113551669B
- Authority
- CN
- China
- Prior art keywords
- information
- positioning
- equation
- state
- ins
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 238000001914 filtration Methods 0.000 claims abstract description 38
- 238000004364 calculation method Methods 0.000 claims abstract description 27
- 239000011159 matrix material Substances 0.000 claims description 40
- 238000005070 sampling Methods 0.000 claims description 22
- 238000006243 chemical reaction Methods 0.000 claims description 19
- 230000008569 process Effects 0.000 claims description 18
- 238000005259 measurement Methods 0.000 claims description 15
- 230000001133 acceleration Effects 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 10
- 230000009466 transformation Effects 0.000 claims description 7
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 239000004734 Polyphenylene sulfide Substances 0.000 claims 16
- 229920000069 polyphenylene sulfide Polymers 0.000 claims 16
- 238000010606 normalization Methods 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 230000021615 conjugation Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/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 provides a combined navigation positioning method and device based on a short base line, wherein the method comprises the steps of obtaining a PPS signal of a multimode receiver and positioning the PPS signal in an RTK mode; taking the positioning information in the PPS signal as an initial value of the INS, and performing INS calculation to obtain INS information; and combining the successfully positioned corresponding PPS signal and the INS information through Kalman filtering to obtain the optimal attitude and position information. According to the invention, a short base line is adopted, the length of the base line is only 320mm, the weight of the base line is only 5kg, the equipment is small and exquisite, the movement is convenient, the GNSS and INS integrated navigation can be realized, the INS can be used for rapidly positioning when the carrier is shielded, the high-precision INS inertial element is adopted, meanwhile, the error of the inertial element is optimized by Kalman filtering, the deviation of the INS inertial element can be corrected by the positioning information of the GNSS, and the positioning precision and reliability of the system are ensured.
Description
Technical Field
The invention relates to the technical field of inertial navigation, in particular to a combined navigation positioning method and device based on a short base line.
Background
The existing inertial navigation system is mainly a GPS/INS (GPS, global Positioning System global positioning system; INS, inertial Navigation System, inertial navigation system) integrated navigation system. The system is a frameless system and consists of three gyroscopes, three accelerometers and a microcomputer, wherein the gyroscopes and the accelerometers are directly and fixedly connected to a carrier, and attitude data are obtained by calculating output values of the gyroscopes and the accelerometers through the microcomputer.
As shown in fig. 1, the accelerometer measures acceleration along three axes of the carrier coordinate system, performs coordinate transformation calculation after error compensation calculation, sums the acceleration and the gravitational acceleration and integrates the sum to obtain speed, and double integration of the acceleration obtains a position value of the carrier. The gyroscope measures angular velocities along three axes of a carrier coordinate system, coordinate system conversion and attitude matrix calculation are carried out after error compensation calculation, the attitude matrix is changed continuously along with time, and the attitude angle of the carrier can be determined from the attitude matrix. Finally, three direction angles, speeds and position information of the carrier are obtained.
Because the inertial element directly bears the environmental conditions such as vibration, impact, temperature fluctuation and the like of the carrier, the output information of the inertial element can generate serious dynamic errors, the errors are difficult to compensate, and the errors are accumulated after long-term use, so that the errors are larger, and the accuracy of the navigation system is reduced. In order to ensure high positioning accuracy, the conventional integrated navigation system generally adopts a plurality of antennas, and a base line between antenna receivers is generally larger than 1m and the weight is larger than 10kg, so that the problems of large equipment volume, heavy weight, inconvenient movement and the like are caused.
Disclosure of Invention
The invention provides a combined navigation positioning method and device based on a short base line, which are used for solving the problems of large volume and inconvenient movement of the existing equipment.
In order to achieve the above purpose, the present invention adopts the following technical scheme:
the first aspect of the invention provides a combined navigation and positioning method based on a short baseline, which comprises the following steps:
obtaining a PPS signal of a multimode receiver and positioning the PPS signal in an RTK mode;
taking the positioning information in the PPS signal as an initial value of the INS, and performing INS calculation to obtain INS information;
and combining the successfully positioned corresponding PPS signal and the INS information through Kalman filtering to obtain the optimal attitude and position information.
Further, the multimode receiver comprises a front antenna receiver and a rear antenna receiver, the front antenna receiver sends observation data to the rear antenna receiver, and the rear antenna receiver establishes a carrier double difference equation to perform relative positioning by utilizing own observation data and difference information.
Further, the specific process of positioning by the RTK mode is as follows:
let k+1 co-vision satellites of front antenna receiver and back antenna receiver, the carrier phase double difference equation after linearization be ζ=aη+bn+v, wherein: ζ -E-R 2k Is a vector formed by the carrier phase double-difference observed value and the pseudo-range double-difference observed value, eta epsilon R 3 Represents a baseline vector, N ε Z k Representing carrier phase double-difference integer ambiguity vector v e R 2k Represents observation noise, A epsilon R 2k×3 Represents a base constant coefficient matrix, B epsilon R 2k×k Representing a fuzzy degree constant coefficient matrix;
solving the carrier phase double difference equation, if the coordinate of the front antenna receiver is known to be eta a Then the post antenna receiver coordinates η b Is the difference estimated value of (2) Is a baseline coordinate vector.
Further, the INS information includes longitude and latitude, altitude information and attitude angle of the carrier.
Further, the specific process of obtaining the INS information by performing INS calculation is as follows:
angular velocity sum of carrier relative to inertial coordinate system based on gyroscope outputObtaining the angular velocity of the carrier coordinate system relative to the navigation coordinate system>Multiplying the angular velocity by the sampling time to obtain a current rotating angle vector value, and carrying out normalization processing on the angle vector values in three directions of northeast days to obtain an attitude angle;
the method comprises the steps of multiplying the output value of an accelerometer by sampling time to obtain a current speed vector value, carrying out quaternion conversion on the speed vector value to obtain the speed of a carrier relative to a navigation coordinate system, carrying out coordinate conversion on the angular speed and the speed of the carrier relative to a geographic coordinate system, carrying out normalization processing on the angular speed and the speed of the carrier, correcting the speed increment caused by the Coriolis force, calculating the speed in three directions of northeast days, carrying out integral calculation on the speed in the front and back two times to obtain a position increment, and adding the position increment with the previous position to obtain current longitude, latitude and altitude information.
Further, the specific process of obtaining the optimal attitude and position information through Kalman filtering combination is as follows:
taking the positioning information as a Kalman filtering estimation system positioning information state, and establishing a Kalman filtering equation of INS information to obtain a state vector x and a state equation x (t) of the integrated navigation system;
taking carrier speed information and position information in the PPS signal as measurement to obtain a measurement equation of the integrated navigation system, and obtaining an observation equation through discretization;
and carrying out Kalman filtering calculation on the random state variable X at the moment k to obtain the optimal estimation of the position information of the carrier at the moment k+1.
Further, the specific process of performing the kalman filter calculation is as follows:
obtaining a group of state acquisition points and corresponding weights by utilizing UT conversion;
obtaining one-step prediction and covariance matrix of the system state by weighting and summing the predicted values of the state acquisition points;
carrying out UT conversion on the one-step predicted value to generate a new state acquisition point;
substituting the new state acquisition points into an observation equation to obtain a predicted observation value, and carrying out weighted summation on the observation value to obtain a predicted mean value and a predicted covariance;
and calculating a Kalman gain matrix, and updating the state and covariance, wherein the updated state is the optimal estimation of the position information of the carrier at the time k+1.
Further, the judging of the successful positioning specifically includes:
the received PPS signal contains complete positioning information, and the number of satellite visible satellites of the antenna receiver is greater than 4.
The invention provides a combined navigation positioning device based on a short base line, which comprises a combined navigation system formed by a multimode receiver and an INS and a microcomputer, wherein the multimode receiver is used for acquiring PPS signals, the microcomputer is used for positioning by using the PPS signals in an RTK mode and calculating INS information, and the successfully positioned corresponding PPS signals and the INS information are combined through Kalman filtering to obtain optimal attitude and position information.
Further, the multimode receiver has a baseline length of 320 millimeters.
The effects provided in the summary of the invention are merely effects of embodiments, not all effects of the invention, and one of the above technical solutions has the following advantages or beneficial effects:
according to the invention, a short base line is adopted, the length of the base line is only 320mm, the weight of the base line is only 5kg, the equipment is small and exquisite, the movement is convenient, the GNSS and INS integrated navigation can be realized, the INS can be used for rapidly positioning when the carrier is shielded, the high-precision INS inertial element is adopted, meanwhile, the error of the inertial element is optimized by Kalman filtering, the deviation of the INS inertial element can be corrected by the positioning information of the GNSS, and the positioning precision and reliability of the system are ensured.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required to be used in the description of the embodiments or the prior art will be briefly described below, and it will be obvious to those skilled in the art that other drawings can be obtained from these drawings without inventive effort.
FIG. 1 is a schematic diagram of a prior art combined inertial navigation system;
FIG. 2 is a schematic flow chart of the method of the present invention;
FIG. 3 is a schematic diagram of an integrated navigation system according to the present invention;
FIG. 4 is a schematic flow chart of an embodiment of the method of the present invention;
FIG. 5 is a schematic diagram of a comparison of simulated position values of the present invention;
FIG. 6 is a comparative schematic of the offset values of the present invention;
FIG. 7 is a graph of heading angle output values of a prior art inertial navigation system;
FIG. 8 is a graph of system heading angle output values in accordance with the present invention;
FIG. 9 is a graph of pitch angle output values of a conventional inertial navigation system;
FIG. 10 is a graph of pitch angle output of the system of the present invention.
Detailed Description
In order to clearly illustrate the technical features of the present solution, the present invention will be described in detail below with reference to the following detailed description and the accompanying drawings. The following disclosure provides many different embodiments, or examples, for implementing different structures of the invention. In order to simplify the present disclosure, components and arrangements of specific examples are described below. Furthermore, the present invention may repeat reference numerals and/or letters in the various examples. This repetition is for the purpose of simplicity and clarity and does not in itself dictate a relationship between the various embodiments and/or configurations discussed. It should be noted that the components illustrated in the figures are not necessarily drawn to scale. Descriptions of well-known components and processing techniques and processes are omitted so as to not unnecessarily obscure the present invention.
As shown in fig. 2, the integrated navigation positioning method based on the short base line of the invention comprises the following steps:
s1, obtaining a PPS signal of a multimode receiver, and positioning in an RTK mode;
s2, taking the positioning information in the PPS signal as an initial value of the INS, and performing INS calculation to obtain INS information;
and S3, combining the corresponding PPS signal and the INS information which are successfully positioned through Kalman filtering to obtain the optimal attitude and position information.
As shown in fig. 3 and 4, the microcomputer receives PPS (pulse per second) signals through the GPS/BDS multimode receiver, obtains position information by adopting an RTK positioning mode after receiving PPS signals, gives the obtained speed, longitude and latitude and altitude information to initial values of INS information (including UTC time, longitude and latitude, altitude, heading angle and angular speed in three directions of northeast and north) after receiving complete positioning information, determines the number of satellites received by the receiver according to the intensity of satellite signals, judges that the positioning information is valid when the signal intensity is greater than 15dBm and judges that the positioning information is valid when the number of satellites visible to GPS/BDS is greater than 4, otherwise re-receives PPS signals for repositioning, and combines the GPS/BDS information and the calculated INS information through kalman filtering after successful positioning to obtain optimal attitude and position information.
The multimode receiver in step S1 includes a front antenna receiver and a rear antenna receiver, where the front antenna receiver sends observation data to the rear antenna receiver, and the rear antenna receiver uses own observation data and differential information to establish a carrier double difference equation for relative positioning.
The specific process for positioning by the RTK mode is as follows:
let k+1 co-vision satellites of front antenna receiver and back antenna receiver, the carrier phase double difference equation after linearization be ζ=aη+bn+v, wherein: ζ -E-R 2k Is a vector formed by the carrier phase double-difference observed value and the pseudo-range double-difference observed value, eta epsilon R 3 Represents a baseline vector, N ε Z k Representing carrier phase double-difference integer ambiguity vector v e R 2k Represents observation noise, A epsilon R 2k×3 Represents a base constant coefficient matrix, B epsilon R 2k×k Representing a fuzzy degree constant coefficient matrix;
solving the carrier phase double difference equation, if the coordinate of the front antenna receiver is known to be eta a Rear antennaReceiver coordinates eta b Is the difference estimated value of (2) Is a baseline coordinate vector.
The specific process for solving the carrier phase double difference equation is as follows:
first, solving the floating solution of eta and N by a least square method (the precision of the floating solution is between 0.1 and 0.5 m), namely
Covariance matrix of floating point solution isThe four quantities in the matrix represent the covariance between η and N two by two.
Second, fixed ambiguity floating point solution (the accuracy of the fixed solution can reach 1 cm, the accuracy is improved), namely
Third step, according toSum of covariance->Determining the base line vector coordinates, i.e.
According to the RTK differential algorithm, if the front antenna coordinate is known to be eta a Rear antenna coordinate eta b Is the difference estimated value of (2)
The INS information in step S2 includes longitude and latitude, altitude information and attitude angle of the carrier.
The specific process of obtaining INS information by INS calculation is as follows:
s21, based on angular velocity and sum of carrier output by gyroscope relative to inertial coordinate systemObtaining the angular velocity of the carrier coordinate system relative to the navigation coordinate system>Multiplying the angular velocity by the sampling time to obtain a current rotating angle vector value, and carrying out normalization processing on the angle vector values in three directions of northeast days to obtain an attitude angle;
s22, multiplying the output value of the accelerometer by the sampling time to obtain a current speed vector value, performing quaternion conversion on the speed vector value to obtain the speed of the carrier relative to a navigation coordinate system, performing coordinate conversion, performing normalization processing on the angular speed and the speed of the carrier relative to a geographic coordinate system, correcting the speed increment caused by the Coriolis force, calculating the speed in three directions of northeast, integrating the speeds in front and back twice to obtain a position increment, and adding the position increment with the previous position to obtain the current longitude, latitude and altitude information.
The specific implementation process of step S21 is as follows:
obtaining output value of gyroscopeNamely, the angular velocity of the carrier relative to the inertial coordinate system, and the rotational angular velocity of the earth in the carrier navigation coordinate system is obtained from the latitude obtained by GPS positioning> Wherein omega ie Self-earth as equatorial positionThe rotation angle speed, lambda is the latitude of the carrier;
position rateThe earth speed of the carrier along the navigation coordinate system is determined by firstly calculating the radius R of curvature of the earth in the meridian plane and the plane perpendicular to the meridian plane according to the latitude of the position of the carrier M And R is N The +.A +.can be calculated according to the speed of the carrier in three directions of northeast and north, the curvature radius of the position, the latitude and the height> v e Is the eastern speed of the carrier, v n Is the carrier north speed, h is the altitude at which the carrier is located;
obtaining an angular velocity value of the carrier in a geographic coordinate system;
obtaining the angular speed of the navigation coordinate system relative to the inertial coordinate system under the navigation coordinate system;
the roll angle and pitch angle of the carrier are calculated according to the output value of the accelerometer and the gravitational acceleration, the course angle of the carrier is obtained according to GPS positioning, the three angles are converted into quaternions q0, q1, q2 and q3, and the angular velocity of the navigation coordinate system relative to the inertial coordinate system is converted through the gesture coordinate q represents the conjugation of the quaternion.
Obtaining angular velocity of the carrier coordinate system relative to the navigation coordinate system
According toThe angular vector value of the rotation at the moment is obtained by multiplying the sampling time, the higher the sampling frequency is, the higher the accuracy of the angle is, and the high-accuracy attitude angle is obtained after the angles in three directions are subjected to normalization processing and calculation.
The specific implementation process of step S22 is as follows:
obtaining a speed vector value at the moment by multiplying the output value of the accelerometer by sampling time, wherein the higher the sampling frequency is, the higher the speed accuracy is, and the speed vector value is subjected to quaternion conversion to obtain the speed v of the carrier relative to a navigation coordinate system n ＝q×v b ×q*；
The gravity acceleration of the carrier position is calculated according to the earth curvature radius and the carrier latitude and altitude,
g 0 ＝9.780318×(1+(5.3024×10-3)×sin2(λ)-(5.9×10-6)×sin2(2λ))；
g＝g 0 /((1+h/R N )2)；
g 0 the gravity acceleration value of the equatorial position is lambda, the latitude of the carrier is the position, and h is the altitude of the carrier.
The influence error of gravity acceleration needs to be removed from the tangential velocity vector, so the tangential velocity u d ＝u d +g×t (t is sampling time).
And carrying out normalization processing on the angular speed and the speed of the carrier relative to a geographic coordinate system, calculating and correcting the speed increment caused by the Coriolis force, calculating the speeds in three directions, calculating the speed integral of the front and rear two times to obtain a position increment, and adding the position increment with the previous position to obtain longitude and latitude altitude information at the moment.
In step S3, the specific process of obtaining the optimal attitude and position information through kalman filtering combination is as follows:
s31, taking the positioning information as a Kalman filtering estimation system positioning information state, and establishing a Kalman filtering equation of INS information to obtain a state vector x and a state equation x (t) of the integrated navigation system.
The state vector x of the integrated navigation system is:
v e 、v n 、v u represents the speeds in three directions, lambdah represents longitude, latitude and altitude, phi e 、φ n And phi u Represents angles of three directions, d e 、d n 、d u Representing the movement distance in three directions, a e 、a n 、a u Indicating acceleration in three directions.
The state equation of the integrated navigation system is:wherein f [ ■ ]]W (t) is the system noise as a nonlinear continuous function.
S32, taking carrier speed information and position information in the PPS signal as measurement to obtain a measurement equation of the integrated navigation system, and obtaining an observation equation through discretization.
Selecting carrier speed information v output by GPS/BDS eg 、v ng 、v ug Position information lambda g 、h g As a measure for the Z-value of the measurement,
the measurement equation for the integrated navigation system is obtained as: z is Z k ＝H(x k ,v k )
Discretizing the system model, wherein the system equation and the observation equation obtained after the processing are respectively as follows:
X(k+1)＝ΦX(k)+ΓW(k)
Z(k)＝HX(k)+V(k)
wherein k is discrete time, and the state of the system at time k is X (k) ∈R n ；Z(k)∈R m An observation signal in a corresponding state; w (k) ∈R r White noise as input; v (k) ∈R m For observing noise, Φ is a state transition matrix, Γ is a noise driving matrix, and H is an observation matrix.
The nonlinear system is constructed as follows:
X(k+1)＝f(x(k),W(k))
Z(k)＝h(x(k),V(k))
f is a nonlinear state equation function; h is a nonlinear observation equation function.
S33, carrying out Kalman filtering calculation on the random state variable X at the moment k to obtain the optimal estimation of the position information of the carrier at the moment k+1.
Using UT transformation, a nonlinear transformation y=f (x) is set, the state vector x is an n-dimensional random variable, and the mean value is knownAnd variance P, calculate the statistical features of y by transforming with the following UT to 2n+1 sigma points X and corresponding weights ω:
2n+1 sigma points, i.e. sampling points, are calculated, n being the system state dimension.
In the method, in the process of the invention,the ith column of the matrix square root is represented.
Is the mean value of 2n+1 sigma point sets X after UT transformation of the state vector X.
Calculating the corresponding weights of the sampling points
In the formula, the subscript m is the mean value, c is the covariance, and the subscript is the ith sampling point. Parameter λ=a 2 (n+kappa) -n is a scaling parameter for reducing the total prediction error, the selection of a controls the distribution state of sampling points, kappa is a parameter to be selected, the specific value is not limited, and the matrix (n+lambda) P is usually ensured to be a semi-positive matrix. The candidate parameter beta is a non-negative weight coefficient, and the dynamic differences of the higher-order terms in the equations can be combined, so that the influence of the higher-order terms can be calculated.
And obtaining a group of state sampling points (called sigma point set) and corresponding weights by utilizing the UT conversion.
Calculate the prediction of the next moment of 2n+1 sigma point sets, i=1, 2
X (i) (k+1|k)＝f[k,X (i) (k|k)]
Calculating a one-step prediction and covariance matrix of the system state, wherein the one-step prediction and covariance matrix is obtained by weighting and summing the predicted values of a sigma point set, and the weight omega is obtained (i) The method is obtained through the calculation mode of the UT conversion.
And carrying out UT conversion again according to the one-step predicted value to generate a new sigma point set.
Substituting the new sigma point set into the observation equation yields a predicted observed quantity, i=1, 2.
Z (i) (k+1|k)＝h[X (i) (k+1|k)]
And obtaining the observed predicted value of the sigma point set, and obtaining the mean value and covariance of the system prediction through weighted summation.
A kalman gain matrix is calculated.
State update and covariance update for a computing system
P (k+1|k+1) is Kalman filteredThe resulting covariance after the wave-back is obtained,the optimal estimation of the position information of the carrier at the time k+1 obtained after Kalman filtering is obtained, and the optimal carrier posture position information is obtained through calculation through the steps.
The simulation calculation is performed in the following manner.
As shown in fig. 5 and fig. 6, fig. 5 is a real value of a simulation position, a measured value obtained by directly calculating inertial navigation, and an optimal estimated value obtained by performing kalman filtering on the inertial navigation, and fig. 6 is a deviation between the directly calculated inertial navigation and the actual value and a deviation after performing kalman filtering, respectively. From the simulation graph, it can be seen that the Kalman filtering greatly reduces the deviation compared with the value directly calculated by inertial navigation, and is relatively close to the true value. Although the error is not completely disappeared after the Kalman filtering, the error approximates to a true value as much as possible, and the influence of the error is reduced, so that the accuracy of the navigation positioning system is improved.
Taking the directional obtained course angle as an example for comparison, fig. 7 is a graph of long-time output values of the course angle of a common inertial navigation system, and fig. 8 is a graph of the output values of the course angle of the combined navigation system adopted in the scheme. As can be seen from the figure, the orientation precision of the common inertial navigation system is not high, the maximum course angle deviation can reach 15 degrees, the course angle deviation obtained by the algorithm adopted by the scheme is only more than 1 degree, the precision is high, meanwhile, large deviation can not occur in long-time measurement, and the reliability is good.
Fig. 9 and 10 are graphs of pitch angle output values of inertial navigation at sinusoidal motion of a swing table. Fig. 9 is an output value of a general inertial navigation system, and fig. 10 is an output value of a pitch angle of the integrated navigation system according to the present embodiment. The curve comparison in the graph shows that the dynamic accuracy of the algorithm adopted by the scheme is also high, and the system equipment is ensured to be oriented accurately in the motion process.
The invention also provides a combined navigation positioning device based on the short base line, which comprises a combined navigation system formed by a multimode receiver and an INS and a microcomputer, wherein the multimode receiver is used for acquiring PPS signals, the microcomputer is used for positioning by using the PPS signals in an RTK mode, calculating INS information, and combining the PPS signals corresponding to successful positioning and the INS information through Kalman filtering to obtain optimal attitude and position information. The multimode receiver has a baseline length of 320 millimeters.
The invention overcomes the defects of large volume, unstable positioning, easy interference and poor reliability of the traditional inertial navigation equipment, and has small volume, easy carrying, short distance between antennas, small errors between the longitude and latitude and the altitude of positioning and the carrier position and relatively simple calculation due to the adoption of the short base line design. According to the invention, GNSS/INS integrated navigation positioning is adopted, positioning is rapid, under the condition that a carrier is blocked and satellite signals cannot be received to perform GPS/BDS positioning, independent positioning can be performed according to INS information, reliability is high relative to a common inertial navigation system, the optimal position information is obtained by adopting Kalman filtering to integrate positioning information of the GPS/BDS and the INS, and the deviation of a gyroscope and an accelerometer is corrected by adopting an algorithm, so that the positioning precision of the system is higher than that of the common inertial navigation system.
While the foregoing description of the embodiments of the present invention has been presented in conjunction with the drawings, it should be understood that it is not intended to limit the scope of the invention, but rather, it is intended to cover all modifications or variations within the scope of the invention as defined by the claims of the present invention.
Claims (4)
1. The integrated navigation positioning method based on the short base line is characterized by comprising the following steps of:
obtaining a PPS signal of a multimode receiver and positioning the PPS signal in an RTK mode;
taking the positioning information in the PPS signal as an initial value of the INS, and performing INS calculation to obtain INS information;
combining the successfully positioned corresponding PPS signal and the INS information through Kalman filtering to obtain optimal attitude and position information;
the multimode receiver comprises a front antenna receiver and a rear antenna receiver, wherein the front antenna receiver sends observation data to the rear antenna receiver, and the rear antenna receiver establishes a carrier double difference equation by utilizing own observation data and differential information to perform relative positioning;
the INS information comprises longitude and latitude, altitude information and attitude angle of the carrier;
the microcomputer receives the PPS signal through the GPS/BDS multimode receiver, obtains position information by adopting an RTK positioning mode after receiving the PPS signal, and gives the obtained speed, longitude and latitude and altitude information to the initial value of INS information after receiving the complete positioning information, wherein the initial value comprises UTC time, longitude and latitude, altitude, heading angle and angular speed in three directions of northeast and north; determining the satellite number of a received satellite according to the intensity of the satellite signal received by a receiver, when the signal intensity is greater than 15dBm, considering the satellite signal to be effective, when the satellite visible satellite number of the GPS/BDS is greater than 4, judging that positioning information is effective, otherwise, re-receiving the PPS signal for re-positioning, and combining the GPS/BDS information and the INS information obtained by calculation through Kalman filtering after the positioning is successful to obtain optimal attitude and position information;
the specific process of obtaining the optimal attitude and position information through Kalman filtering combination is as follows:
taking the positioning information as a Kalman filtering estimation system positioning information state, and establishing a Kalman filtering equation of INS information to obtain a state vector x and a state equation x (t) of the integrated navigation system;
the state vector x of the integrated navigation system is:
v e 、v n 、v u represents the speeds in three directions, lambdah represents longitude, latitude and altitude, phi e 、φ n And phi u Represents angles of three directions, d e 、d n 、d u Representing the movement distance in three directions, a e 、a n 、a u Acceleration representing three directions;
the state equation of the integrated navigation system is:wherein f [ ■ ]]W (t) is system noise as a nonlinear continuous function;
taking carrier speed information and position information in the PPS signal as measurement to obtain a measurement equation of the integrated navigation system, and obtaining an observation equation through discretization;
selecting carrier speed information v output by GPS/BDS eg 、v ng 、v ug Position information lambda g 、h g As a measure for the Z-value of the measurement,
the measurement equation for the integrated navigation system is obtained as: z is Z k ＝H(x k ,v k )；
Discretizing the system model, wherein the system equation and the observation equation obtained after the processing are respectively as follows:
X(k+1)＝ΦX(k)+ΓW(k)；
Z(k)＝HX(k)+V(k)；
wherein k is discrete time, and the state of the system at time k is X (k) ∈R n ；Z(k)∈R m An observation signal in a corresponding state; w (k) ∈R r White noise as input; v (k) ∈R m For observing noise, phi is a state transition matrix, Γ is a noise driving matrix, and H is an observation matrix;
the nonlinear system is constructed as follows:
X(k+1)＝f(x(k),W(k))
Z(k)＝h(x(k),V(k))；
f is a nonlinear state equation function; h is a nonlinear observation equation function;
carrying out Kalman filtering calculation on the random state variable X at the moment k to obtain the optimal estimation of the position information of the carrier at the moment k+1;
by using UT conversionA nonlinear transformation y=f (x), the state vector x is an n-dimensional random variable, and the mean value thereof is knownAnd variance P, calculate the statistical features of y by transforming with the following UT to 2n+1 sigma points X and corresponding weights ω:
calculating 2n+1 sigma points, namely sampling points, wherein n is the system state dimension;
in the method, in the process of the invention,an ith column representing a matrix square root;
is the mean value of 2n+1 sigma point sets X after the state vector X is subjected to UT conversion;
calculating corresponding weights of the sampling points:
in the formula, subscript m is the mean value, c is the covariance, and superscript is the ith sampling point; parameter λ=a 2 (n+kappa) -n is a scaling parameter for reducing total prediction error, the selection of a controls the distribution state of sampling points, kappa is a parameter to be selected, the specific value is not limited, the matrix (n+lambda) P is usually ensured to be a semi-positive matrix, the parameter to be selected beta is a non-negative weight coefficient, and the dynamic differences of higher-order terms in the equation can be combined;
obtaining a sigma point set and corresponding weights by utilizing the UT conversion:
calculating a prediction of the next moment of 2n+1 sigma point sets, i=1, 2,..2n+1;
X (i) (k+1|k)＝f[k,X (i) (k|k)]；
calculating a one-step prediction and covariance matrix of the system state, wherein the one-step prediction and covariance matrix is obtained by weighting and summing the predicted values of a sigma point set, and the weight omega is obtained (i) The method is obtained through the calculation mode of the UT conversion;
carrying out UT conversion again according to the one-step predicted value to generate a new sigma point set;
substituting the new sigma point set into an observation equation to obtain a predicted observed quantity, i=1, 2,..2n+1;
Z (i) (k+1|k)＝h[X (i) (k+1|k)]；
obtaining an observation predicted value of a sigma point set, and obtaining a mean value and covariance of system prediction through weighted summation;
calculating a Kalman gain matrix:
state update and covariance update of the computing system:
p (k +1|k + 1) is the covariance obtained after kalman filtering,the optimal estimation of the position information of the carrier at the time k+1 obtained after Kalman filtering is obtained, and the optimal carrier posture position information is obtained through calculation through the steps.
2. The integrated navigation positioning method based on the short baseline according to claim 1, wherein the specific process of positioning by the RTK method is as follows:
let k+1 co-vision satellites of front antenna receiver and back antenna receiver, the carrier phase double difference equation after linearization be ζ=aη+bn+v, wherein: ζ -E-R 2k Is a carrier phaseVector formed by bit double difference observation value and pseudo-range double difference observation value, eta epsilon R 3 Represents a baseline vector, N ε Z k Representing carrier phase double-difference integer ambiguity vector v e R 2k Represents observation noise, A epsilon R 2k×3 Represents a base constant coefficient matrix, B epsilon R 2k×k Representing a fuzzy degree constant coefficient matrix;
solving the carrier phase double difference equation, if the coordinate of the front antenna receiver is known to be eta a Then the post antenna receiver coordinates η b Is the difference estimated value of (2) Is a baseline coordinate vector.
3. The integrated navigation positioning device based on the short base line is characterized by comprising an integrated navigation system formed by a multimode receiver and an INS (inertial navigation system), and a microcomputer, wherein the multimode receiver is used for acquiring a PPS (polyphenylene sulfide) signal, the microcomputer is used for positioning by using the PPS signal in an RTK (real time kinematic) mode, calculating INS information, and combining the PPS signal corresponding to successful positioning and the INS information through Kalman filtering to obtain optimal attitude position information;
the multimode receiver comprises a front antenna receiver and a rear antenna receiver, wherein the front antenna receiver sends observation data to the rear antenna receiver, and the rear antenna receiver establishes a carrier double difference equation by utilizing own observation data and differential information to perform relative positioning;
the INS information comprises longitude and latitude, altitude information and attitude angle of the carrier;
the microcomputer receives the PPS signal through the GPS/BDS multimode receiver, obtains position information by adopting an RTK positioning mode after receiving the PPS signal, and gives the obtained speed, longitude and latitude and altitude information to the initial value of INS information after receiving the complete positioning information, wherein the initial value comprises UTC time, longitude and latitude, altitude, heading angle and angular speed in three directions of northeast and north; determining the satellite number of a received satellite according to the intensity of the satellite signal received by a receiver, when the signal intensity is greater than 15dBm, considering the satellite signal to be effective, when the satellite visible satellite number of the GPS/BDS is greater than 4, judging that positioning information is effective, otherwise, re-receiving the PPS signal for re-positioning, and combining the GPS/BDS information and the INS information obtained by calculation through Kalman filtering after the positioning is successful to obtain optimal attitude and position information;
the specific process of obtaining the optimal attitude and position information through Kalman filtering combination is as follows:
taking the positioning information as a Kalman filtering estimation system positioning information state, and establishing a Kalman filtering equation of INS information to obtain a state vector x and a state equation x (t) of the integrated navigation system;
the state vector x of the integrated navigation system is:
v e 、v n 、v u represents the speeds in three directions, lambdah represents longitude, latitude and altitude, phi e 、φ n And phi u Represents angles of three directions, d e 、d n 、d u Representing the movement distance in three directions, a e 、a n 、a u Acceleration representing three directions;
the state equation of the integrated navigation system is:wherein f [ ■ ]]W (t) is system noise as a nonlinear continuous function;
taking carrier speed information and position information in the PPS signal as measurement to obtain a measurement equation of the integrated navigation system, and obtaining an observation equation through discretization;
selecting carrier speed information v output by GPS/BDS eg 、v ng 、v ug Position information lambda g 、h g As a measure for the Z-value of the measurement,
the measurement equation for the integrated navigation system is obtained as: z is Z k ＝H(x k ,v k )；
Discretizing the system model, wherein the system equation and the observation equation obtained after the processing are respectively as follows:
X(k+1)＝ΦX(k)+ΓW(k)；
Z(k)＝HX(k)+V(k)；
wherein k is discrete time, and the state of the system at time k is X (k) ∈R n ；Z(k)∈R m An observation signal in a corresponding state; w (k) ∈R r White noise as input; v (k) ∈R m For observing noise, phi is a state transition matrix, Γ is a noise driving matrix, and H is an observation matrix;
the nonlinear system is constructed as follows:
X(k+1)＝f(x(k),W(k))
Z(k)＝h(x(k),V(k))；
f is a nonlinear state equation function; h is a nonlinear observation equation function;
carrying out Kalman filtering calculation on the random state variable X at the moment k to obtain the optimal estimation of the position information of the carrier at the moment k+1;
using UT transformation, a nonlinear transformation y=f (x) is set, the state vector x is an n-dimensional random variable, and the mean value is knownAnd variance P, calculate the statistical features of y by transforming with the following UT to 2n+1 sigma points X and corresponding weights ω:
calculating 2n+1 sigma points, namely sampling points, wherein n is the system state dimension;
in the method, in the process of the invention,an ith column representing a matrix square root;
is the mean value of 2n+1 sigma point sets X after the state vector X is subjected to UT conversion;
calculating corresponding weights of the sampling points:
in the formula, subscript m is the mean value, c is the covariance, and superscript is the ith sampling point; parameter λ=a 2 (n+kappa) -n is a scaling parameter for reducing total prediction error, the selection of a controls the distribution state of sampling points, kappa is a parameter to be selected, the specific value is not limited, the matrix (n+lambda) P is usually ensured to be a semi-positive matrix, the parameter to be selected beta is a non-negative weight coefficient, and the dynamic differences of higher-order terms in the equation can be combined;
obtaining a sigma point set and corresponding weights by utilizing the UT conversion:
calculating a prediction of the next moment of 2n+1 sigma point sets, i=1, 2,..2n+1;
X (i) (k+1|k)＝f[k,X (i) (k|k)]；
calculating a one-step prediction and covariance matrix of the system state, wherein the one-step prediction and covariance matrix is obtained by weighting and summing the predicted values of a sigma point set, and the weight omega is obtained (i) The method is obtained through the calculation mode of the UT conversion;
carrying out UT conversion again according to the one-step predicted value to generate a new sigma point set;
substituting the new sigma point set into an observation equation to obtain a predicted observed quantity, i=1, 2,..2n+1;
Z (i) (k+1|k)＝h[X (i) (k+1|k)]；
obtaining an observation predicted value of a sigma point set, and obtaining a mean value and covariance of system prediction through weighted summation;
calculating a Kalman gain matrix:
state update and covariance update of the computing system:
p (k +1|k + 1) is the covariance obtained after kalman filtering,the optimal estimation of the position information of the carrier at the time k+1 obtained after Kalman filtering is obtained, and the optimal carrier posture position information is obtained through calculation through the steps.
4. A combined short baseline based navigation positioning device according to claim 3 wherein the multimode receiver has a baseline length of 320 millimeters.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110841082.2A CN113551669B (en) | 2021-07-23 | 2021-07-23 | Combined navigation positioning method and device based on short base line |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110841082.2A CN113551669B (en) | 2021-07-23 | 2021-07-23 | Combined navigation positioning method and device based on short base line |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113551669A CN113551669A (en) | 2021-10-26 |
| CN113551669B true CN113551669B (en) | 2024-04-02 |
Family
ID=78132863
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110841082.2A Active CN113551669B (en) | 2021-07-23 | 2021-07-23 | Combined navigation positioning method and device based on short base line |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113551669B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114994732B (en) * | 2022-08-04 | 2022-11-01 | 武汉大学 | Vehicle-mounted course rapid initialization device and method based on GNSS carrier phase |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7292185B2 (en) * | 2005-10-04 | 2007-11-06 | Csi Wireless Inc. | Attitude determination exploiting geometry constraints |
| CN106990424A (en) * | 2017-06-07 | 2017-07-28 | 重庆重邮汇测通信技术有限公司 | A kind of double antenna GPS surveys attitude positioning method |
| CN208488105U (en) * | 2018-05-03 | 2019-02-12 | 广州市中海达测绘仪器有限公司 | Navigation elements and navigation device |
| CN109613585A (en) * | 2018-12-14 | 2019-04-12 | 中国科学院国家授时中心 | A kind of method of pair of real-time direction finding of antenna for base station ultra-short baseline GNSS double antenna |
| CN110780326A (en) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | Vehicle-mounted integrated navigation system and positioning method |
| CN112378400A (en) * | 2020-10-30 | 2021-02-19 | 湖南航天机电设备与特种材料研究所 | Dual-antenna GNSS assisted strapdown inertial navigation integrated navigation method |
-
2021
- 2021-07-23 CN CN202110841082.2A patent/CN113551669B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7292185B2 (en) * | 2005-10-04 | 2007-11-06 | Csi Wireless Inc. | Attitude determination exploiting geometry constraints |
| CN106990424A (en) * | 2017-06-07 | 2017-07-28 | 重庆重邮汇测通信技术有限公司 | A kind of double antenna GPS surveys attitude positioning method |
| CN208488105U (en) * | 2018-05-03 | 2019-02-12 | 广州市中海达测绘仪器有限公司 | Navigation elements and navigation device |
| CN109613585A (en) * | 2018-12-14 | 2019-04-12 | 中国科学院国家授时中心 | A kind of method of pair of real-time direction finding of antenna for base station ultra-short baseline GNSS double antenna |
| CN110780326A (en) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | Vehicle-mounted integrated navigation system and positioning method |
| CN112378400A (en) * | 2020-10-30 | 2021-02-19 | 湖南航天机电设备与特种材料研究所 | Dual-antenna GNSS assisted strapdown inertial navigation integrated navigation method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113551669A (en) | 2021-10-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109324330B (en) | USBL/SINS tight combination navigation positioning method based on mixed derivative-free extended Kalman filtering | |
| US6459990B1 (en) | Self-contained positioning method and system thereof for water and land vehicles | |
| CN105607093B (en) | A kind of integrated navigation system and the method for obtaining navigation coordinate | |
| CN111156994B (en) | INS/DR & GNSS loose combination navigation method based on MEMS inertial component | |
| Niu et al. | An accurate land‐vehicle MEMS IMU/GPS navigation system using 3D auxiliary velocity updates | |
| US20030176970A1 (en) | Interruption free navigator | |
| CN107270893A (en) | Lever arm, time in-synchronization error estimation and the compensation method measured towards real estate | |
| CN105509739A (en) | Tightly coupled INS/UWB integrated navigation system and method adopting fixed-interval CRTS smoothing | |
| CN111121766B (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN103076026B (en) | A kind of method determining Doppler log range rate error in SINS | |
| CN110057356B (en) | Method and device for positioning vehicles in tunnel | |
| CN110133692B (en) | Inertial navigation technology-assisted high-precision GNSS dynamic inclination measurement system and method | |
| CN104698485A (en) | BD, GPS and MEMS based integrated navigation system and method | |
| CN115096303B (en) | GNSS multi-antenna and INS tightly-combined positioning and attitude determination method and equipment | |
| CN114739425A (en) | Coal mining machine positioning calibration system based on RTK-GNSS and total station and application method | |
| CN113503892A (en) | Inertial navigation system moving base initial alignment method based on odometer and backtracking navigation | |
| CN105988129A (en) | Scalar-estimation-algorithm-based INS/GNSS combined navigation method | |
| CN113551669B (en) | Combined navigation positioning method and device based on short base line | |
| CN109084755B (en) | Accelerometer zero offset estimation method based on gravity apparent velocity and parameter identification | |
| Vaknin et al. | Coarse leveling of gyro-free INS | |
| CN112197765B (en) | Method for realizing fine navigation of underwater robot | |
| CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN117053782A (en) | Combined navigation method for amphibious robot | |
| CN114994732B (en) | Vehicle-mounted course rapid initialization device and method based on GNSS carrier phase | |
| CN112882118B (en) | Method and system for estimating gravity vector of movable base under earth-fixed coordinate system and storage medium |
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 |