CN110221332A - A kind of the dynamic lever arm estimation error and compensation method of vehicle-mounted GNSS/INS integrated navigation - Google Patents
A kind of the dynamic lever arm estimation error and compensation method of vehicle-mounted GNSS/INS integrated navigation Download PDFInfo
- Publication number
- CN110221332A CN110221332A CN201910288631.0A CN201910288631A CN110221332A CN 110221332 A CN110221332 A CN 110221332A CN 201910288631 A CN201910288631 A CN 201910288631A CN 110221332 A CN110221332 A CN 110221332A
- Authority
- CN
- China
- Prior art keywords
- lever arm
- error
- vehicle
- compensation
- navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000003068 static effect Effects 0.000 claims abstract description 16
- 230000008859 change Effects 0.000 claims abstract description 14
- 238000012937 correction Methods 0.000 claims description 34
- 238000005259 measurement Methods 0.000 claims description 33
- 238000001914 filtration Methods 0.000 claims description 19
- 238000004364 calculation method Methods 0.000 claims description 17
- 239000011159 matrix material Substances 0.000 claims description 15
- 230000001133 acceleration Effects 0.000 claims description 11
- 230000005484 gravity Effects 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 6
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 4
- 230000007246 mechanism Effects 0.000 abstract description 3
- 230000000875 corresponding effect Effects 0.000 description 6
- 230000036544 posture Effects 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 2
- 238000006880 cross-coupling reaction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 241000287196 Asthenes Species 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000007730 finishing process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000009434 installation Methods 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
- 230000010355 oscillation Effects 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 230000007480 spreading Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000007704 transition 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
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/40—Correcting position, velocity or attitude
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/52—Determining velocity
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The present invention relates to the dynamic lever arm estimation errors and compensation method of a kind of vehicle-mounted GNSS/INS integrated navigation, the present invention considers GNSS and INS and handles respectively as the data-signal superiority and inferiority difference of navigation subsystem, and consider to cause lever arm variation that error is caused to change since vehicle-state variation is deformed in vehicle mounted guidance, lever arm is divided into static and dynamic lever arm two parts, and compensation mechanism is modeled and established to dynamic lever arm, the speed and location error generate to lever arm effect is fed back and is compensated, realize integrated navigation and location method for improving accuracy caused by not being overlapped based on position.
Description
Technical Field
The invention relates to the technical field of vehicle navigation and positioning, in particular to a dynamic lever arm error estimation and compensation method for vehicle GNSS/INS combined navigation.
Background
An Inertial Navigation System (INS) has the characteristics of high autonomy, anti-interference performance, high short-term precision, high data output rate, complete Navigation information, wide application range and the like, but the System error has the characteristic of periodic oscillation, and certain Navigation parameter errors have the characteristic of accumulation along with time and the time required by initial alignment is longer; the GNSS output positioning precision is high, initialization is not required, but signals can be interfered or shielded, and continuous navigation parameters and more accurate vehicle postures cannot be stably provided, so that an external reference information source with GNSS navigation errors not accumulated along with time is utilized to periodically or aperiodically correct navigation parameters of an inertial navigation system and compensate drift of an inertial device, thereby providing continuous, long-term and short-term navigation parameters with higher precision and complete precision for a vehicle, and further realizing vehicle-mounted high-precision positioning. Inertial navigation generally uses the geometric center of an Inertial Measurement Unit (IMU) as a reference datum for navigation positioning or speed measurement, while satellite navigation uses the phase center of a receiver antenna as a reference datum, when vehicle carriers are used simultaneously, they have a certain deviation on the installation position, and the deviation can cause the difference of speed and position in the actual vehicle operation, which is called lever arm error, so that the error needs to be estimated and compensated in the combined navigation.
Currently, estimation and compensation of the error of the combined navigation lever arm are mainly divided into three types: mechanical compensation/mechanical compensation, dynamic on-line calibration/estimation and digital filtering compensation. The estimation of the error is mainly the calibration and the state expansion, and the compensation correction mainly applies the lever arm error to the output result or the combined navigation filter, namely the output correction and the feedback correction. The output correction corrects the output result without changing the accumulated error, the method increases the error of the navigation parameter to be estimated along with the accumulation of time, so that an INS system error model becomes nonlinear, the precision of the combined navigation filter is reduced, and the estimated value of the navigation parameter error returns to zero during the feedback correction, so that the feedback correction must be carried out on the navigation parameter error during the GNSS/INS combined navigation. The current navigation parameter error feedback correction scheme is divided into the following according to the correction method and the corrected state parameters: hybrid correction (initially using output correction and later using feedback correction), incomplete feedback (feedback correction for only position, velocity, attitude errors) and complete feedback correction (feedback correction for position, velocity, attitude errors and inertial device random constant errors). Since the correction of the random constant error of the inertial device has a significant effect on the system output under weak and missing GNSS signals, a feedback correction scheme of the random constant error of the inertial device must be considered, and meanwhile, the feedback correction of the lever arm should be added.
The position and attitude measurement system (POS) dynamic lever arm compensation method for aerial remote sensing as in patent application No. 201110220018.9 discloses the following: aiming at the problem that the lever arm between an Inertial Measurement Unit (IMU) measurement center and a GPS antenna phase center is changed in real time due to rotation of a triaxial inertially stabilized platform frame, the actual lever arm between the IMU measurement center and the GPS antenna phase center is obtained by calculating the dynamic lever arm between the triaxial inertially stabilized platform center and the IMU measurement center in real time, the angular velocity of an initial coordinate system of the triaxial inertially stabilized platform relative to a local geographic coordinate system under the initial coordinate system of the triaxial inertially stabilized platform is calculated in real time, and dynamic lever arm compensation is performed.
A method for feedback correction of an INS/GPS integrated navigation system based on lever arm estimation as disclosed in patent application No. 201310289324.7 is as follows: the method can realize effective estimation of the random constant value error of the inertia device and full feedback correction, and can effectively improve the precision of the INS/GPS combined navigation system, but the measured value of the lever arm in the method is a true value, in the incomplete feedback correction, the estimation result of the lever arm is only used as a judgment condition for switching the correction method, and the error of the lever arm in the full state feedback is used as mechanical compensation, and the change of the lever arm caused by vehicle deflection deformation caused by the state change of the vehicle in the operation process is ignored.
The two lever arm estimation compensation schemes of the GNSS/INS combined navigation system have the following defects:
1. the error of the lever arm lacks estimation and compensation of a dynamic lever arm of the vehicle, so that the accuracy of combined navigation is reduced;
2. the existing lever arm error dynamic compensation needs to establish a stable platform and is not suitable for the use condition of a vehicle;
3. the dynamic error compensation is not fully considered in the coupling relation with the state of the vehicle, and only the corresponding relation between the angular speed and the lever arm error is considered.
Disclosure of Invention
The invention aims to overcome the defects in the prior art and provide a dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS combined navigation, which considers the error caused by the lever arm change between the INS and the GNSS in the running process of a vehicle, divides the lever arm into a static lever arm part and a dynamic lever arm part, models and establishes a compensation mechanism for the dynamic lever arm, feeds back and compensates the speed and position errors generated by the lever arm effect, and realizes the method for improving the positioning accuracy of the combined navigation based on the misalignment of the positions.
The purpose of the invention can be realized by the following technical scheme:
a dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS combined navigation comprises the following steps:
step 1: before the vehicle runs, measuring lever arm values of a GNSS phase center and an inertial measurement unit center under a vehicle coordinate system by using a measuring instrument, and initializing an INS (inertial navigation system) under a vehicle static state;
step 2: acquiring INS original navigation data and GNSS data in the driving process of a vehicle;
and step 3: after availability judgment, abnormal point removal, signal interpolation and filtering are carried out on GNSS data, the obtained output PVT information and course information and the availability information of the signals are all input into a combined filter;
and 4, step 4: performing device compensation and attitude calculation on INS original navigation data, respectively entering dynamic lever arm compensation judgment and navigation calculation, and inputting the obtained judgment result and compensation quantity, INS navigation related information and speed increment attitude, speed and position into a combined filter;
and 5: and after the data input of the combined filter is finished, establishing a state equation of the combined navigation system, estimating the state equation, and after each time of filtering, performing feedback correction on the INS calculation result by using the result of filtering estimation.
Further, the usability judgment in step 3 is described by the formula:
in the formula, QGNSSIndicating signal availability.
Further, the dynamic lever arm compensation judgment in the step 4 specifically includes: according to the current input triaxial angular velocity and triaxial acceleration value, the representation parameters of the vehicle working condition are solved with the set weight value, different thresholds are set to represent the rapid acceleration, rapid deceleration and large steering or severe working condition, if the representation parameters of the vehicle working condition are less than or equal to the set threshold, dynamic lever arm compensation is not carried out, if the representation parameters of the vehicle working condition are greater than the set threshold, dynamic lever arm compensation is carried out, and the calculation formula of the representation parameters of the vehicle working condition is as follows:
wherein, R is a characteristic parameter of the working condition of the vehicle, and k1、k2And k3The weight values corresponding to the three axes are respectively, andare respectively the current triaxial ratio values,andrespectively the current input three-axis angular velocity.
Further, the calculation formula of the compensation amount in step 4 is as follows:
and R is less than or equal to Rth2
In the formula,for static measurements, δ lbFor the value of the lever arm,andlever arms of three coordinate axes of a GNSS phase center and an inertial measurement unit center under a static state in a vehicle coordinate system respectivelyx、φyAnd phizRespectively the vehicle attitude angles, | RthAnd | R |)th2To set a threshold value for defining a decision interval,to dynamically compensate for lever arm values.
Further, the velocity increment posture, the velocity and the position in the step 4 are calculated by adopting a two-subsample cone error compensation algorithm, and a corresponding calculation equation set is as follows:
in the formula,. DELTA.theta.m1And Δ θm2Corresponding angle increment is sampled for the gyro at two equal intervals, T is sampling time,for reference with inertial coordinate system, the carrier system is started from tm-1Time tmThe change in the rotation at a moment in time,for reference to the inertial frame, the geographic system is set from tmTime tm-1The rotation change at the moment, the subscript i represents the inertial navigation system solution value, the upper subscript b represents the load system, the upper subscript n represents the geography system, and the (m) represents tmTime, (m-1) represents tm-1At the moment, phi represents the corresponding posture with the subscript, I represents the identity matrix,is a constant value.
Further, the step 5 comprises the following sub-steps:
step 51: establishing a system equation;
step 52: establishing a measurement equation;
step 53: establishing a kalman filtering system equation and discretizing a measurement equation;
step 54: and performing feedback correction by using a kalman filtering system equation.
Further, the system equation in step 51 describes the formula as:
X＝[φE φN φU δvE δvN δvU δL δλ δh εx εy εz ▽x ▽y ▽z]T
wherein X is a state vector, phiE、φNAnd phiURespectively, attitude error, δ v, in east-north-sky geographic coordinate systemE、δvNAnd δ vURespectively, velocity errors in an east-north-sky geographic coordinate system, δ L, δ λ and δ h are position errors of longitude, latitude and altitude, εx、εyAnd εzZero offset for three axes of the gyroscope, ▽ respectivelyx、▽yAnd ▽zZero offset for three coordinate axes of the accelerometer respectively;
in the formula,is the angular velocity of the geographic system relative to the inertial system,the angular velocity error of the earth system relative to the inertial system,the angular velocity error of the geographic system relative to the earth system,is a coordinate transformation matrix of carrier system to geographic system,is the angular velocity error of the carrier system relative to the inertial system,is the output specific force, v, of the carrier system relative to the inertial navigation system accelerometer under the geographic systemnIs the speed of the carrier under the geographic system,is the angular velocity of the earth system relative to the inertial system,is the angular velocity, δ v, of the geographic system relative to the Earth's systemnIs the speed error of the carrier under the geographical region,is the output specific force error, delta g, of the inertial navigation system accelerometer under the carrier system relative to the geographic systemnAs error of gravitational acceleration, RMRadius of the mortise, h local altitude, L local latitude and RNAnd for the meridian radius, the single phi represents a mathematical platform error angle in the strapdown inertial navigation system.
Further, the measurement equation in step 52 describes the formula:
in the formula, the superscript n represents the geography system, Z represents the measurement equation, the subscript INS represents the inertial system, the subscript GNSS represents the satellite navigation, the superscript indicates the actual value, v indicates the velocity, p indicates the position,representing the angular velocity, R, of the carrier system relative to the earth systemMh＝RM+h，RNh＝RN+h。
Further, the step 5 further comprises: feeding back the Kalman filtered gyro and the acceleration zero offset to a device compensation position for correction, feeding back the attitude to an attitude updating compensation position, and feeding back the speed and position errors to an output value calculated by the INS for correction, namely: by modifiedThe course angle psi, the pitch angle theta and the roll angle gamma can be obtained through solution, and after the primary filtering feedback, the error state returns to 0.
Further, said modifiedThe heading angle psi, the pitch angle theta and the roll angle gamma can be obtained through solution, and the corresponding description formula is as follows:
in the formula, (numeral 1, numeral 2) represents a specific corresponding matrix element in the matrix.
Compared with the prior art, the invention has the following advantages:
(1) in the invention, the difference between the quality of data signals of a GNSS and an INS as navigation subsystems is considered to be respectively processed, the error change caused by the lever arm change due to the deformation generated by the vehicle state change in the vehicle navigation is considered, the lever arm is divided into a static lever arm part and a dynamic lever arm part, a compensation mechanism is established and modeled on the dynamic lever arm, the speed and position errors generated by the lever arm effect are fed back and compensated, and the precision of the GNSS/INS combined navigation system is improved.
Drawings
FIG. 1 is a schematic diagram of a lever arm relative to the center of an INS inertial measurement unit and a GNSS antenna in accordance with the present invention;
FIG. 2 is a block diagram of an integrated navigation system according to the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, not all, embodiments of the present invention. All other embodiments, which can be obtained by a person skilled in the art without any inventive step based on the embodiments of the present invention, shall fall within the scope of protection of the present invention.
Examples
The method measures the lever arm value between the center of the INS inertial measurement unit and the GNSS antenna, distinguishes a dynamic lever arm from a static lever arm, and performs error compensation, and mainly comprises four stages: the first stage is static measurement calibration and initialization, the second stage is raw data processing and signal availability judgment of GNSS and INS, the third stage is dynamic lever arm compensation judgment and navigation calculation, and the fourth stage is sensor fusion filtering and feedback correction.
The specific implementation steps of the invention are shown in fig. 2:
1) before the vehicle runs, a measuring instrument is utilized to measure the GNSS phase center and Inertial Measurement Unit (IMU) center on the vehicle
Lever arm under vehicle coordinate systemAs lever arms in static and small dynamic states, as shown in fig. 1;
2) initializing an INS in a vehicle static state;
3) the method comprises the following steps of collecting GNSS/INS integrated navigation system data in the vehicle running process, wherein the GNSS/INS integrated navigation system data comprises inertia measurement data: three-axis gyroscope data and three-axis accelerometer data, GNSS data, carrier, spreading/ranging codes, retains the original navigation message information (signal state), while Position, Velocity and Time (Position, Velocity, and Time, PVT), pseudoranges and pseudorange rates are determined by the navigation processor. Wherein the location comprises latitude L, longitude λ and altitude h, and the velocity comprises east velocity VEVelocity in north direction VNVelocity in the sky VU；
4) Processing the GNSS signals in the step 3), specifically comprising signal availability judgment, abnormal point removal, signal interpolation and filtering, outputting speed and position information and course information and signal availability QGNSSInput to a combining filter; wherein,
the velocity position measurements are:
the availability of the signal is:
5) performing component compensation and attitude calculation on the original navigation data of the INS in the step 3), and then respectively performing judgment and navigation calculation on dynamic lever arm compensation to obtain whether dynamic lever arm compensation is performed and a compensation value [ delta l ]b Glb]Obtaining information of triaxial acceleration, triaxial angular velocity, angle, velocity increment posture, velocity, position and the like of the INS navigation state, and inputting the information into the combined filter;
and (3) judging the dynamic compensation of the lever arm:
according to the current input three-axis angular velocity:specific force of three axesValue, current vehicle stateAccording to the characteristics of rapid acceleration, rapid deceleration and large steering or severe working conditions, setting a threshold value | R-thAnd | R |)th2。
Wherein:
when R | ≦ R |thNo dynamic lever arm compensation is performed; | R | > | R |)thCompensation is performed.
Dynamic lever arm compensation calculation
Since the lever arm is the sum of static and dynamic:
is in a static stateThe measured value of the measured value is,is a dynamic lever arm, GlbIs the feedback coefficient of the dynamic lever arm.
Since the deformation of the vehicle is related to the magnitude of the stress/torque of the vehicle, the torsional rigidity of the vehicle and the actual torsional angle, the magnitude of the stress/torque is reflected as the change of the motion state of the vehicle, the torsional rigidity is regarded as unchanged in the motion process, the actual torsional angle is difficult to measure and is related to the attitude angle of the vehicle, and therefore, the functional relation between the deformation amount and the attitude angle of the vehicle and the static lever arm is established:
a dynamic lever arm:
and R is less than or equal to Rth2
6) After the data in (4) and (5) are input into the filter, the 15-dimensional error state vector is estimated by adopting the 15-dimensional error state vector which specifically comprises the position, the speed, the attitude, the gyro random constant drift epsilon and the accelerometer random constant zero offset ▽Error in velocityError in misalignment angleGyro random constant driftAccelerometerRandom constant zero offsetAnd performing feedback correction on the INS calculation result.
Firstly, calculating the attitude in the step 5)
Selecting an east-north-sky (E-N-U) geographic coordinate system (a g system) as a navigation reference coordinate system of the strapdown inertial navigation system, and recording the G system as an N system again, wherein an attitude differential equation taking the N system as the reference system is as follows:
wherein, the matrixIndicating that i system (inertial coordinate system) is used as a reference and b system is from tm-1Time tmThe change in the rotation at a moment in time,can be controlled by the angular velocity of a gyroscopeDetermining;denotes i as a reference, n is from tmTime tm-1The change in the rotation at a moment in time,can be calculated from the angular velocityIt is determined that,andrespectively represent tm-1And tmA strapdown attitude matrix of the time of day. If the gyro is in the time period tm-1,tm]Inner (T ═ T)m-tm-1) Two times of equal interval sampling are carried out, and the angular increment is respectively delta thetam1And Δ θm2A two-subsample cone error compensation algorithm is adopted, and comprises the following steps:
taking fourth order truncation and approximation:
navigation update period [ t ]m-1,tm]In the interior, it can be considered that the velocity and position are causedVery small in variation, i.e. visibleIs constant and is recorded asThen there are:
second, the filtering in step (6) is solved
1. Filtering and resolving:
establishing a system equation
Wherein: x: an error state vector;
f: a system matrix;
g: a noise distribution matrix;
w: a zero mean gaussian white noise vector;
z: measuring a vector;
h: measuring a matrix;
v: measuring a noise state vector;
b at the relevant subscript positions denotes the carrier system, n denotes the geographic system, e denotes the earth system, and i denotes the inertial system.
X＝[φE φN φU δvE δvN δvU δL δλ δh εx εy εz ▽x ▽y ▽z]T
Wherein X is a state vector, phiE、φNAnd phiUAre respectively provided withIs the attitude error in east-north-sky geographic coordinate system, delta vE、δvNAnd δ vURespectively, velocity errors in an east-north-sky geographic coordinate system, δ L, δ λ and δ h are position errors of longitude, latitude and altitude, εx、εyAnd εzZero offset for three axes of the gyroscope, ▽ respectivelyx、▽yAnd ▽zZero offset for three coordinate axes of the accelerometer respectively;
in the formula,is the angular velocity of the geographic system relative to the inertial system,the angular velocity error of the earth system relative to the inertial system,the angular velocity error of the geographic system relative to the earth system,is a coordinate transformation matrix of carrier system to geographic system,is the angular velocity error of the carrier system relative to the inertial system,is the output specific force, v, of the carrier system relative to the inertial navigation system accelerometer under the geographic systemnIs the speed of the carrier under the geographic system,is the angular velocity of the earth system relative to the inertial system,is the angular velocity, δ v, of the geographic system relative to the Earth's systemnIs the speed error of the carrier under the geographical region,is the output specific force error, delta g, of the inertial navigation system accelerometer under the carrier system relative to the geographic systemnAs error of gravitational acceleration, RMRadius of the mortise, h local altitude, L local latitude and RNAnd for the meridian radius, the single phi represents a mathematical platform error angle in the strapdown inertial navigation system.
Gyro zero bias under carrier system:
accelerometer zero bias under carrier system:
the following develops the equations (attitude-velocity-position) in turn:
wherein
Wherein:
for gyro measurement errors, m-band different a, x, y and z subscripts are expressed as cross coupling coefficients between two axes in the gyro measurement, and s-band a, x and z subscripts are expressed as scale factor errors in the gyro measurement.
Wherein:
for accelerometer measurement errors, the m-band different g, x, y, z subscripts are expressed as cross-coupling coefficients in the accelerometer measurement, and the s-band g, x, z subscripts are expressed as scale factor errors in the accelerometer measurement.
The earth parameters given by the WGS-84(World Geodetic System 1984) Earth series are:
semi-major axis: re6378137m, flat rate: f is 1/298.257223563, and the ratio of f,
gravitational constant (including atmosphere): mu-3.986004418X 1014m3/s2，
Earth rotation angular rate: omegaie＝7.2921151467×10-5rad/s
geAnd gpEquator gravity and pole gravity respectively, and the earth gravity oblateness is as follows:
β1represents the ratio to the equatorial gravity:
β2represents the gradient of gravity falling with height:
the finishing formula is as follows:
F15＝03×3
F24＝03×3，F31＝03×3
F34＝03×3，F35＝03×3，F41＝F42＝F43＝F44＝F45＝F51＝F52＝F53＝F54＝F55＝03×3
2. establishing a measurement equation:
in the formula, the superscript n represents the geography system, Z represents the measurement equation, the subscript INS represents the inertial system, the subscript GNSS represents the satellite navigation, the superscript indicates the actual value, v indicates the velocity, p indicates the position,representing the angular velocity, R, of the carrier system relative to the earth systemMh＝RM+h，RNh＝RN+h。
The finishing process comprises the following steps:
discretization of Kalman filtering system equation and measurement equation
Making approximate discretization equivalence:
Xk＝Φk/k-1Xk-1+Γk-1Wk-1
in which a discretized time interval T is sets＝tk-tk-1Then the state transition matrix takes a first order truncation, having:
Wk-1is a system noise vector, VkFor measuring the noise vector, both are zero-mean gaussian white noise vector sequences (obeying normal distribution), and they are not correlated with each other, i.e. they satisfy:
{E[Wk]＝0,E[Vk]＝0,
the fundamental assumption of noise requirements in a Kalman Filter State space model, generally requires QkIs semi-positive and RkIs positive, i.e. QkNot less than 0 and Rk＞0。KalmaThe full set of n filtering algorithms can be divided into five basic formulas as follows:
(1) state one-step prediction
(2) State one-step prediction mean square error
(3) Filter gain
(4) State estimation
(5) State estimation mean square error
Pk＝(I-KkHk)Pk/k-1
4. Feedback correction
And feeding back the Kalman filtered gyroscope and acceleration zero offset to a device compensation position for correction, feeding back the attitude to an attitude updating compensation position, feeding back the speed and position errors to the output value calculated by the INS for correction, and returning the error state to 0 after feedback.
While the invention has been described with reference to specific embodiments, the invention is not limited thereto, and various equivalent modifications and substitutions can be easily made by those skilled in the art within the technical scope of the invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (10)
1. A dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS combined navigation is characterized by comprising the following steps:
step 1: before the vehicle runs, measuring lever arm values of a GNSS phase center and an inertial measurement unit center under a vehicle coordinate system by using a measuring instrument, and initializing an INS (inertial navigation system) under a vehicle static state;
step 2: acquiring INS original navigation data and GNSS data in the driving process of a vehicle;
and step 3: after availability judgment, abnormal point removal, signal interpolation and filtering are carried out on GNSS data, the obtained output PVT information and course information and the availability information of the signals are all input into a combined filter;
and 4, step 4: performing device compensation and attitude calculation on INS original navigation data, respectively entering dynamic lever arm compensation judgment and navigation calculation, and inputting the obtained judgment result and compensation quantity, INS navigation related information and speed increment attitude, speed and position into a combined filter;
and 5: and after the data input of the combined filter is finished, establishing a state equation of the combined navigation system, estimating the state equation, and after each time of filtering, performing feedback correction on the INS calculation result by using the result of filtering estimation.
2. The method as claimed in claim 1, wherein the usability judgment in step 3 is described as follows:
in the formula, QGNSSIndicating signal availability.
3. The method as claimed in claim 1, wherein the dynamic lever arm compensation determination in step 4 specifically comprises: according to the current input triaxial angular velocity and triaxial acceleration value, the representation parameters of the vehicle working condition are solved with the set weight value, different thresholds are set to represent the rapid acceleration, rapid deceleration and large steering or severe working condition, if the representation parameters of the vehicle working condition are less than or equal to the set threshold, dynamic lever arm compensation is not carried out, if the representation parameters of the vehicle working condition are greater than the set threshold, dynamic lever arm compensation is carried out, and the calculation formula of the representation parameters of the vehicle working condition is as follows:
wherein, R is a characteristic parameter of the working condition of the vehicle, and k1、k2And k3The weight values corresponding to the three axes are respectively, andare respectively the current triaxial ratio values,andrespectively the current input three-axis angular velocity.
4. The method as claimed in claim 1, wherein the calculation formula of the compensation amount in step 4 is as follows:
and R is less than or equal to Rth2
In the formula,for static measurements, δ lbFor the value of the lever arm,andlever arms of three coordinate axes of a GNSS phase center and an inertial measurement unit center under a static state in a vehicle coordinate system respectivelyx、φyAnd phizRespectively the vehicle attitude angles, | RthAnd | R |)th2To set a threshold value for defining a decision interval,to dynamically compensate for lever arm values.
5. The method as claimed in claim 1, wherein the velocity increment attitude, velocity and position in step 4 are calculated by using a two-subsample cone error compensation algorithm, and the corresponding calculation equation set is:
in the formula,. DELTA.theta.m1And Δ θm2Corresponding angle increment is sampled for the gyro at two equal intervals, T is sampling time,for reference with inertial coordinate system, the carrier system is started from tm-1Time tmThe change in the rotation at a moment in time,for reference to the inertial frame, the geographic system is set from tmTime tm-1The rotation change at the moment, the subscript i represents the inertial navigation system solution value, the upper subscript b represents the load system, the upper subscript n represents the geography system, and the (m) represents tmTime, (m-1) represents tm-1At the moment, phi represents the corresponding posture with the subscript, I represents the identity matrix,is a constant value.
6. The method as claimed in claim 1, wherein the step 5 comprises the following sub-steps:
step 51: establishing a system equation;
step 52: establishing a measurement equation;
step 53: establishing a kalman filtering system equation and discretizing a measurement equation;
step 54: and performing feedback correction by using a kalman filtering system equation.
7. The method as claimed in claim 6, wherein the system equation in step 51 is described as:
wherein X is a state vector, phiE、φNAnd phiURespectively, attitude error, δ v, in east-north-sky geographic coordinate systemE、δvNAnd δ vURespectively, velocity errors in an east-north-sky geographic coordinate system, δ L, δ λ and δ h are position errors of longitude, latitude and altitude, εx、εyAnd εzRespectively the zero offset of three coordinate axes of the gyroscope,andzero offset for three coordinate axes of the accelerometer respectively;
in the formula,is the angular velocity of the geographic system relative to the inertial system,the angular velocity error of the earth system relative to the inertial system,the angular velocity error of the geographic system relative to the earth system,is a coordinate transformation matrix of carrier system to geographic system,is the angular velocity error of the carrier system relative to the inertial system,is the output specific force, v, of the carrier system relative to the inertial navigation system accelerometer under the geographic systemnIs the speed of the carrier under the geographic system,is the angular velocity of the earth system relative to the inertial system,is the angular velocity, δ v, of the geographic system relative to the Earth's systemnIs the speed error of the carrier under the geographical region,is the output specific force error, delta g, of the inertial navigation system accelerometer under the carrier system relative to the geographic systemnIn order to be a gravity acceleration error,RMradius of the mortise, h local altitude, L local latitude and RNAnd for the meridian radius, the single phi represents a mathematical platform error angle in the strapdown inertial navigation system.
8. The method of claim 7, wherein the measurement equation in step 52 describes the formula as:
in the formula, the superscript n represents the geography system, Z represents the measurement equation, the subscript INS represents the inertial system, the subscript GNSS represents the satellite navigation, the superscript indicates the actual value, v indicates the velocity, p indicates the position,representing the angular velocity, R, of the carrier system relative to the earth systemMh＝RM+h，RNh＝RN+h。
9. The method as claimed in claim 1, wherein the step 5 further comprises: zero-offset-inversion of Kalman filtered gyro and accelerationFeeding the data to a device compensation position for correction, feeding the attitude back to an attitude updating compensation position, and feeding the speed and the position error back to an output value calculated by the INS for correction, namely: by modifiedThe course angle psi, the pitch angle theta and the roll angle gamma can be obtained through solution, and after the primary filtering feedback, the error state returns to 0.
10. The method as claimed in claim 9, wherein the modified GNSS/INS combined navigation dynamic lever arm error estimation and compensation method is implemented by using a modified GNSS/INS combined navigation systemThe heading angle psi, the pitch angle theta and the roll angle gamma can be obtained through solution, and the corresponding description formula is as follows:
in the formula, (numeral 1, numeral 2) represents a specific corresponding matrix element in the matrix.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910288631.0A CN110221332B (en) | 2019-04-11 | 2019-04-11 | Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910288631.0A CN110221332B (en) | 2019-04-11 | 2019-04-11 | Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110221332A true CN110221332A (en) | 2019-09-10 |
| CN110221332B CN110221332B (en) | 2023-02-10 |
Family
ID=67822558
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910288631.0A Active CN110221332B (en) | 2019-04-11 | 2019-04-11 | Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110221332B (en) |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110672124A (en) * | 2019-09-27 | 2020-01-10 | 北京耐威时代科技有限公司 | Offline lever arm estimation method |
| CN111469855A (en) * | 2020-04-20 | 2020-07-31 | 北京易控智驾科技有限公司 | Vehicle motion parameter calculation method |
| CN111678538A (en) * | 2020-07-29 | 2020-09-18 | 中国电子科技集团公司第二十六研究所 | Dynamic level meter error compensation method based on speed matching |
| US10996345B2 (en) | 2018-06-11 | 2021-05-04 | Honeywell International Inc. | Signal fault detection for global navigation satellite system using multiple antennas |
| CN113029139A (en) * | 2021-04-07 | 2021-06-25 | 中国电子科技集团公司第二十八研究所 | Airport flight area vehicle differential Beidou/SINS combined navigation method based on motion detection |
| CN113465628A (en) * | 2021-06-17 | 2021-10-01 | 杭州鸿泉物联网技术股份有限公司 | Inertial measurement unit data compensation method and system |
| CN113841025A (en) * | 2020-10-14 | 2021-12-24 | 深圳市大疆创新科技有限公司 | Position and attitude determination method for movable platform, related device and system |
| CN113865583A (en) * | 2021-07-20 | 2021-12-31 | 北京航天控制仪器研究所 | Accelerometer combination dynamic mounting deviation matrix determining and compensating method |
| CN113971846A (en) * | 2020-07-22 | 2022-01-25 | 郑州宇通客车股份有限公司 | Positioning failure detection method and device for automatic driving vehicle |
| CN114076610A (en) * | 2020-08-12 | 2022-02-22 | 千寻位置网络（浙江）有限公司 | Error calibration and navigation method and device of GNSS/MEMS vehicle-mounted integrated navigation system |
| CN114167493A (en) * | 2021-11-23 | 2022-03-11 | 武汉大学 | Earthquake rotation measurement system and method of GNSS double-antenna auxiliary gyroscope |
| CN114563004A (en) * | 2022-04-29 | 2022-05-31 | 季华实验室 | Online measurement method and system for lever arm value of integrated navigation system |
| CN114894222A (en) * | 2022-07-12 | 2022-08-12 | 深圳元戎启行科技有限公司 | External parameter calibration method of IMU-GNSS antenna and related method and equipment |
| US11536854B2 (en) | 2020-04-24 | 2022-12-27 | Honeywell International Inc. | Multiple faulty global navigation satellite system signal detecting system |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20060224321A1 (en) * | 2005-03-29 | 2006-10-05 | Honeywell International Inc. | Method and apparatus for high accuracy relative motion determination using inertial sensors |
| CN103344259A (en) * | 2013-07-11 | 2013-10-09 | 北京航空航天大学 | Method for correcting feedback of inertial navigation system/global position system (INS/GPS) combined navigation system based on lever arm estimation |
| CN107270893A (en) * | 2017-05-27 | 2017-10-20 | 东南大学 | Lever arm, time in-synchronization error estimation and the compensation method measured towards real estate |
-
2019
- 2019-04-11 CN CN201910288631.0A patent/CN110221332B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20060224321A1 (en) * | 2005-03-29 | 2006-10-05 | Honeywell International Inc. | Method and apparatus for high accuracy relative motion determination using inertial sensors |
| CN103344259A (en) * | 2013-07-11 | 2013-10-09 | 北京航空航天大学 | Method for correcting feedback of inertial navigation system/global position system (INS/GPS) combined navigation system based on lever arm estimation |
| CN107270893A (en) * | 2017-05-27 | 2017-10-20 | 东南大学 | Lever arm, time in-synchronization error estimation and the compensation method measured towards real estate |
Non-Patent Citations (5)
| Title |
|---|
| 潘献飞等: "考虑机抖激光陀螺信号滤波特性的圆锥算法修正", 《中国惯性技术学报》 * |
| 王海峰等: "高动态GPS/BD2组合导航信号模拟器关键技术研究", 《宇航计测技术》 * |
| 缪玲娟等: "减小动态误差的捷联系统姿态算法研究", 《兵工学报》 * |
| 肖虎等: "光纤陀螺捷联惯导系统速度算法的改进研究", 《弹箭与制导学报》 * |
| 钱闯: "弱ＧＮＳＳ信号区域的ＧＮＳＳ／ＩＮＳ／ＬｉＤＡＲ高精度定位方法及其应用研究", 《中国优秀博硕士学位论文全文数据库(博士)信息科技辑》 * |
Cited By (21)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10996345B2 (en) | 2018-06-11 | 2021-05-04 | Honeywell International Inc. | Signal fault detection for global navigation satellite system using multiple antennas |
| CN110672124A (en) * | 2019-09-27 | 2020-01-10 | 北京耐威时代科技有限公司 | Offline lever arm estimation method |
| CN111469855A (en) * | 2020-04-20 | 2020-07-31 | 北京易控智驾科技有限公司 | Vehicle motion parameter calculation method |
| US11536854B2 (en) | 2020-04-24 | 2022-12-27 | Honeywell International Inc. | Multiple faulty global navigation satellite system signal detecting system |
| CN113971846A (en) * | 2020-07-22 | 2022-01-25 | 郑州宇通客车股份有限公司 | Positioning failure detection method and device for automatic driving vehicle |
| CN111678538A (en) * | 2020-07-29 | 2020-09-18 | 中国电子科技集团公司第二十六研究所 | Dynamic level meter error compensation method based on speed matching |
| CN111678538B (en) * | 2020-07-29 | 2023-06-09 | 中国电子科技集团公司第二十六研究所 | Dynamic level error compensation method based on speed matching |
| CN114076610B (en) * | 2020-08-12 | 2024-05-28 | 千寻位置网络（浙江）有限公司 | Error calibration and navigation method and device of GNSS/MEMS vehicle-mounted integrated navigation system |
| CN114076610A (en) * | 2020-08-12 | 2022-02-22 | 千寻位置网络（浙江）有限公司 | Error calibration and navigation method and device of GNSS/MEMS vehicle-mounted integrated navigation system |
| CN113841025A (en) * | 2020-10-14 | 2021-12-24 | 深圳市大疆创新科技有限公司 | Position and attitude determination method for movable platform, related device and system |
| CN113029139A (en) * | 2021-04-07 | 2021-06-25 | 中国电子科技集团公司第二十八研究所 | Airport flight area vehicle differential Beidou/SINS combined navigation method based on motion detection |
| CN113029139B (en) * | 2021-04-07 | 2023-07-28 | 中国电子科技集团公司第二十八研究所 | Airport flight area vehicle differential Beidou/SINS combined navigation method based on motion detection |
| CN113465628A (en) * | 2021-06-17 | 2021-10-01 | 杭州鸿泉物联网技术股份有限公司 | Inertial measurement unit data compensation method and system |
| CN113865583B (en) * | 2021-07-20 | 2024-02-09 | 北京航天控制仪器研究所 | Accelerometer combination dynamic installation deviation matrix determining and compensating method |
| CN113865583A (en) * | 2021-07-20 | 2021-12-31 | 北京航天控制仪器研究所 | Accelerometer combination dynamic mounting deviation matrix determining and compensating method |
| CN114167493A (en) * | 2021-11-23 | 2022-03-11 | 武汉大学 | Earthquake rotation measurement system and method of GNSS double-antenna auxiliary gyroscope |
| CN114167493B (en) * | 2021-11-23 | 2023-08-04 | 武汉大学 | Seismic rotation measurement system and method of GNSS double-antenna auxiliary gyroscope |
| CN114563004B (en) * | 2022-04-29 | 2022-07-22 | 季华实验室 | Online measurement method and system for lever arm value of integrated navigation system |
| CN114563004A (en) * | 2022-04-29 | 2022-05-31 | 季华实验室 | Online measurement method and system for lever arm value of integrated navigation system |
| CN114894222A (en) * | 2022-07-12 | 2022-08-12 | 深圳元戎启行科技有限公司 | External parameter calibration method of IMU-GNSS antenna and related method and equipment |
| CN114894222B (en) * | 2022-07-12 | 2022-10-28 | 深圳元戎启行科技有限公司 | External parameter calibration method of IMU-GNSS antenna and related method and equipment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110221332B (en) | 2023-02-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110221332B (en) | Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation | |
| CN110501024B (en) | Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system | |
| CN110221333B (en) | Measurement error compensation method of vehicle-mounted INS/OD integrated navigation system | |
| CN111156994B (en) | INS/DR & GNSS loose combination navigation method based on MEMS inertial component | |
| CN111024064B (en) | SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering | |
| US9541392B2 (en) | Surveying system and method | |
| CN106405670B (en) | A kind of gravity anomaly data processing method suitable for strapdown marine gravitometer | |
| CN108051866B (en) | Based on strap down inertial navigation/GPS combination subsidiary level angular movement isolation Gravimetric Method | |
| JP5068531B2 (en) | Method and system for improving the accuracy of inertial navigation measurements using measured and stored gravity gradients | |
| CN113203418B (en) | GNSSINS visual fusion positioning method and system based on sequential Kalman filtering | |
| CN110779521A (en) | Multi-source fusion high-precision positioning method and device | |
| EP1582840B1 (en) | Inertial navigation system error correction | |
| CN100541135C (en) | Fiber-optic gyroscope strapdown inertial navigation system initial attitude based on Doppler is determined method | |
| CN112629538A (en) | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering | |
| US7844397B2 (en) | Method and apparatus for high accuracy relative motion determination using inertial sensors | |
| CN101949703B (en) | Strapdown inertial/satellite combined navigation filtering method | |
| CN104880189B (en) | A kind of antenna for satellite communication in motion low cost tracking anti-interference method | |
| CN111024074B (en) | Inertial navigation speed error determination method based on recursive least square parameter identification | |
| CN110146076A (en) | A kind of SINS/DVL combined positioning method of no inverse matrix adaptive-filtering | |
| CN112507281A (en) | SINS/DVL tight combination system based on two-state multi-factor robust estimation | |
| CN109612460B (en) | Plumb line deviation measuring method based on static correction | |
| Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
| CN111121766A (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN106403952A (en) | Method for measuring combined attitudes of Satcom on the move with low cost | |
| CN114964222A (en) | Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device |
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 |