CN110221332B - Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation - Google Patents

Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation Download PDF

Info

Publication number
CN110221332B
CN110221332B CN201910288631.0A CN201910288631A CN110221332B CN 110221332 B CN110221332 B CN 110221332B CN 201910288631 A CN201910288631 A CN 201910288631A CN 110221332 B CN110221332 B CN 110221332B
Authority
CN
China
Prior art keywords
lever arm
error
vehicle
navigation
compensation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910288631.0A
Other languages
Chinese (zh)
Other versions
CN110221332A (en
Inventor
熊璐
陈辛波
韩燕群
夏新
陆逸适
高乐天
胡英杰
魏琰超
宋舜辉
刘伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tongji University
Original Assignee
Tongji University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tongji University filed Critical Tongji University
Priority to CN201910288631.0A priority Critical patent/CN110221332B/en
Publication of CN110221332A publication Critical patent/CN110221332A/en
Application granted granted Critical
Publication of CN110221332B publication Critical patent/CN110221332B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • 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

Abstract

The invention relates to a dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS combined navigation, which considers the difference between the quality of a data signal of a GNSS and an INS as navigation subsystems to be respectively processed, considers the error change caused by the lever arm change due to the deformation generated by the vehicle state change in the vehicle-mounted navigation, divides the lever arm into a static lever arm part and a dynamic lever arm part, models the dynamic lever arm and establishes a compensation mechanism, feeds back and compensates the speed and position error generated by the lever arm effect, and realizes the method for improving the positioning accuracy of the combined navigation based on the non-overlapping position.

Description

Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation
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 calibration and state expansion, and the compensation correction mainly applies the lever arm error to the output result or the combined navigation filter, namely output correction and 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 following formula:
Figure BDA0002024140860000031
in the formula, Q GNSS Indicating signal availability.
Further, the dynamic lever arm compensation judgment in the step 4 specifically includes: according to the currently input triaxial angular velocity and triaxial acceleration value, the characterization parameters of the vehicle working condition are solved by the set weight value, different thresholds are set to characterize the rapid acceleration, rapid deceleration and large steering or severe working condition, if the characterization 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 characterization parameters of the vehicle working condition are greater than the set threshold, the dynamic lever arm compensation is carried out, and the calculation formula of the characterization parameters of the vehicle working condition is as follows:
Figure BDA0002024140860000032
wherein, R is a characteristic parameter of the working condition of the vehicle, k 1 、k 2 And k 3 The weight values corresponding to the three axes are respectively,
Figure BDA0002024140860000033
Figure BDA0002024140860000034
and
Figure BDA0002024140860000035
are respectively the current triaxial ratio values,
Figure BDA0002024140860000036
and
Figure BDA0002024140860000037
respectively the current input three-axis angular velocity.
Further, the calculation formula of the compensation amount in step 4 is as follows:
Figure BDA0002024140860000041
Figure BDA0002024140860000042
Figure BDA0002024140860000043
and R is less than or equal to R th2
In the formula (I), the compound is shown in the specification,
Figure BDA0002024140860000044
for static measurements, δ l b In order to be the value of the lever arm,
Figure BDA0002024140860000045
and
Figure BDA0002024140860000046
lever arms of three coordinate axes of a GNSS phase center and an inertial measurement unit center in a vehicle coordinate system under static state respectively x 、φ y And phi z Are respectively the vehicle attitude angles of three coordinate axes, | R | calculation th And | R |) th2 To set a threshold value for defining a decision interval,
Figure BDA0002024140860000047
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:
Figure BDA0002024140860000048
Figure BDA0002024140860000049
Figure BDA00020241408600000410
Figure BDA00020241408600000411
in the formula,. DELTA.theta. m1 And Δ θ m2 Corresponding angle increment is sampled for the gyro at two equal intervals, T is sampling time,
Figure BDA00020241408600000412
for reference with inertial coordinate system, the carrier system is started from t m-1 Time t m The change in the rotation at a moment in time,
Figure BDA00020241408600000413
for reference by the inertial frame, the geographic system is from t m Time t m-1 Time of dayThe subscript i represents the inertial navigation system solution value, the upper subscript b represents the carrier system, the upper subscript n represents the geographic system, and (m) represents t m Time, (m-1) represents t m-1 At the moment, phi represents the corresponding posture with the subscript, I represents the identity matrix,
Figure BDA00020241408600000414
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 δv E δv N δv U δL δλ δh ε x ε y ε z ▽ x ▽ y ▽ z ] T
wherein X is a state vector, phi E 、φ N And phi U Respectively, attitude error, δ v, in east-north-sky geographic coordinate system E 、δv N And δ v U Respectively, velocity errors in an east-north-sky geographic coordinate system, δ L, δ λ and δ h are position errors of longitude, latitude and altitude, ε x 、ε y And ε z Is a deviation of zero of three coordinate axes of the gyroscope respectively x 、▽ y And & z Zero offset for three coordinate axes of the accelerometer respectively;
Figure BDA0002024140860000051
Figure BDA0002024140860000052
Figure BDA0002024140860000053
Figure BDA0002024140860000054
Figure BDA0002024140860000055
in the formula (I), the compound is shown in the specification,
Figure BDA0002024140860000056
is the angular velocity of the geographic system relative to the inertial system,
Figure BDA0002024140860000057
is the angular velocity error of the earth system relative to the inertial system,
Figure BDA0002024140860000058
the angular velocity error of the geographic system relative to the earth system,
Figure BDA0002024140860000059
is a coordinate transformation matrix of the carrier system to the geographical system,
Figure BDA00020241408600000510
is the angular velocity error of the carrier system relative to the inertial system,
Figure BDA00020241408600000511
is the output specific force, v, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n Is the speed of the carrier under the geographic system,
Figure BDA00020241408600000512
is the angular velocity of the earth system relative to the inertial system,
Figure BDA00020241408600000513
is the angular velocity, δ v, of the geographic system relative to the Earth's system n Is the speed error of the carrier under the geographical system,
Figure BDA00020241408600000514
is the output specific force error, delta g, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n As error of gravitational acceleration, R M Radius of the fourth quarter, h local altitude, L local latitude and R N And for the meridian radius, phi alone represents the mathematical platform error angle in the strapdown inertial navigation system.
Further, the measurement equation in step 52 is described as:
Figure BDA00020241408600000515
Figure BDA00020241408600000516
Figure BDA00020241408600000517
Figure BDA00020241408600000518
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,
Figure BDA0002024140860000061
representing the angular velocity, R, of the carrier system relative to the earth system Mh ＝R M +h，R Nh ＝R N +h。
Further, the step 5 further comprises: kalman filtered gyro sumThe acceleration zero offset is fed back to the device compensation position for correction, the attitude is fed back to the attitude updating compensation position, and the speed and position errors are fed back to the output value calculated by the INS for correction, namely: by modified
Figure BDA0002024140860000062
The 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 modified
Figure BDA0002024140860000063
The 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:
Figure BDA0002024140860000064
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 making any creative effort based on the embodiments in the present invention, shall fall within the protection scope 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 carries out 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 system
Figure BDA0002024140860000071
As lever arms in static state and small dynamic state, as shown in fig. 1;
2) Initializing an INS in a static state of the vehicle;
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 V E Velocity in north direction V N Velocity in the direction of the sky V U ；
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 Q GNSS Input to a combined filter; wherein, the first and the second end of the pipe are connected with each other,
the velocity position measurements are:
Figure BDA0002024140860000072
the availability of the signal is:
Figure BDA0002024140860000073
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 G lb ]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:
Figure BDA0002024140860000074
specific force of three axes
Figure BDA0002024140860000075
Value, current vehicle state
Figure BDA0002024140860000076
According to the characteristics of rapid acceleration, rapid deceleration and large steering or severe working conditions, setting a threshold value | R- th And | R |) th2 。
Wherein:
Figure BDA0002024140860000077
when R | ≦ R | th No dynamic lever arm compensation is performed; | R | > | R | th Compensation is performed.
Dynamic lever arm compensation amount calculation
Since the lever arm is the sum of static and dynamic:
Figure BDA0002024140860000081
Figure BDA0002024140860000082
in order to be a static measurement value,
Figure BDA0002024140860000083
is a dynamic lever arm, G lb Is the feedback coefficient of the dynamic lever arm.
Since the vehicle deformation is related to the magnitude of the stress/torque thereof, the torsional rigidity of the vehicle and the actual torsional angle, the magnitude of the stress/torque is reflected as the motion state change of the vehicle, the torsional rigidity is regarded as constant in the motion process, the actual torsional angle is difficult to measure, but is related to the attitude angle of the vehicle, and therefore, the functional relationship between the deformation amount and the attitude angle of the vehicle and the static lever arm is established:
a dynamic lever arm:
Figure BDA0002024140860000084
Figure BDA0002024140860000085
and R is less than or equal to R th2
6) And (4) after the data in the step (4) and the step (5) are input into the filter, estimating the 15-dimensional error state vector 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 v. After each filtering, the position error estimated by the filtering
Figure BDA0002024140860000086
Error in velocity
Figure BDA0002024140860000087
Error of misalignment angle
Figure BDA0002024140860000088
Gyro random constant drift
Figure BDA0002024140860000089
Accelerometer random constant zero offset
Figure BDA00020241408600000810
And 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 (g system) as a navigation reference coordinate system of the strapdown inertial navigation system, and recording as an N system again, wherein an attitude differential equation taking the N system as the reference system is as follows:
Figure BDA00020241408600000811
Figure BDA00020241408600000812
wherein, the matrix
Figure BDA00020241408600000813
Indicating that i system (inertial coordinate system) is used as a reference and b system is from t m-1 Time t m The change in the rotation at a moment in time,
Figure BDA00020241408600000814
can be controlled by the angular velocity of a gyroscope
Figure BDA00020241408600000815
Determining;
Figure BDA00020241408600000816
denotes that i is used as reference base and n is from t m Time t m-1 The change in the rotation at a moment in time,
Figure BDA00020241408600000817
can be calculated from the angular velocity
Figure BDA00020241408600000818
It is determined that,
Figure BDA00020241408600000819
and
Figure BDA00020241408600000820
respectively represent t m-1 And t m A strapdown attitude matrix of the time of day. If the gyro is in the time period t m-1 ,t m ]Inner (T = T) m -t m-1 ) Two equal-interval samples are taken, and the angular increment is delta theta m1 And Δ θ m2 A two-subsample cone error compensation algorithm is adopted, and comprises the following steps:
Figure BDA00020241408600000821
taking fourth order truncation and approximation:
Figure BDA0002024140860000091
Figure BDA0002024140860000092
navigation update period [ t ] m-1 ,t m ]In which the velocity and position can be considered to be
Figure BDA0002024140860000093
Very small in variation, i.e. visible
Figure BDA0002024140860000094
Is constant and is recorded as
Figure BDA0002024140860000095
Then there are:
Figure BDA0002024140860000096
Figure BDA0002024140860000097
second, the filtering in step (6) is solved
1. Filtering and resolving:
establishing a system equation
Figure BDA0002024140860000098
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 associated subscript position denotes the carrier system, n denotes the geographic system, e denotes the earth system, and i denotes the inertial system.
X＝[φ E φ N φ U δv E δv N δv U δL δλ δh ε x ε y ε z ▽ x ▽ y ▽ z ] T
Wherein X is a state vector, phi E 、φ N And phi U Respectively, attitude error, δ v, in east-north-sky geographic coordinate system E 、δv N And δ v U Respectively, velocity error in east-north-sky geographic coordinate system, position error of longitude, latitude and altitude, delta L, delta lambda and delta h, epsilon x 、ε y And ε z Is a deviation of zero of three coordinate axes of the gyroscope respectively x 、▽ y And + z Zero offset for three coordinate axes of the accelerometer respectively;
Figure BDA0002024140860000101
Figure BDA0002024140860000102
Figure BDA0002024140860000103
Figure BDA0002024140860000104
Figure BDA0002024140860000105
in the formula (I), the compound is shown in the specification,
Figure BDA0002024140860000106
is the angular velocity of the geographic system relative to the inertial system,
Figure BDA0002024140860000107
is the angular velocity error of the earth system relative to the inertial system,
Figure BDA0002024140860000108
is the angular velocity error of the geographic system relative to the earth system,
Figure BDA0002024140860000109
is a coordinate transformation matrix of the carrier system to the geographical system,
Figure BDA00020241408600001010
is the angular velocity error of the carrier system relative to the inertial system,
Figure BDA00020241408600001011
is the output specific force, v, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n Is the speed of the carrier under the geographic system,
Figure BDA00020241408600001012
is groundThe angular velocity of the ball system relative to the inertial system,
Figure BDA00020241408600001013
is the angular velocity, δ v, of the geographic system relative to the Earth's system n Is the speed error of the carrier under the geographical region,
Figure BDA00020241408600001014
is the output specific force error, delta g, of the inertial navigation system accelerometer under the carrier system relative to the geographic system n As error of gravitational acceleration, R M Radius of the fourth quarter, h local altitude, L local latitude and R N And for the meridian radius, phi alone represents the mathematical platform error angle in the strapdown inertial navigation system.
Gyro zero bias under carrier system:
Figure BDA00020241408600001015
accelerometer zero bias under carrier system:
Figure BDA00020241408600001016
the following develops the equations (attitude-velocity-position) in turn:
Figure BDA0002024140860000111
wherein
Figure BDA0002024140860000112
Figure BDA0002024140860000113
Figure BDA0002024140860000114
Figure BDA0002024140860000115
Figure BDA0002024140860000121
Wherein:
Figure BDA0002024140860000122
for the error of the gyro measurement, m-band different subscripts of a, x, y and z are expressed as cross coupling coefficients between two axes in the gyro measurement, and s-band subscripts of a, x and z are expressed as scale factor errors in the gyro measurement.
Figure BDA0002024140860000123
Figure BDA0002024140860000124
Wherein:
Figure BDA0002024140860000125
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.
Figure BDA0002024140860000126
Figure BDA0002024140860000131
The earth parameters given by the WGS-84 (World Geodetic System 1984) Earth series are:
semi-major axis: r e =6378137m, oblateness: f =1/298.257223563,
gravitational constant (including atmosphere): μ =3.986004418 × 10 14 m 3 /s 2 ，
Earth rotation angular rate: omega ie ＝7.2921151467×10 -5 rad/s
g e And g p Equator gravity and pole gravity respectively, and the earth gravity oblateness is as follows:
Figure BDA0002024140860000132
β 1 expressed as a ratio to equatorial gravity:
Figure BDA0002024140860000133
β 2 represents the gradient of gravity falling with height:
Figure BDA0002024140860000134
the finishing formula is shown as formula 1-5:
Figure BDA0002024140860000135
Figure BDA0002024140860000136
Figure BDA0002024140860000137
F 15 ＝0 3×3
Figure BDA0002024140860000138
Figure BDA0002024140860000141
F 24 ＝0 3×3 ，
Figure BDA0002024140860000142
F 31 ＝0 3×3
Figure BDA0002024140860000143
F 34 ＝0 3×3 ，F 35 ＝0 3×3 ，F 41 ＝F 42 ＝F 43 ＝F 44 ＝F 45 ＝F 51 ＝F 52 ＝F 53 ＝F 54 ＝F 55 ＝0 3×3
2. establishing a measurement equation:
Figure BDA0002024140860000144
Figure BDA0002024140860000145
Figure BDA0002024140860000146
Figure BDA0002024140860000147
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,
Figure BDA0002024140860000148
representing the angular velocity, R, of the carrier system relative to the earth system Mh ＝R M +h，R Nh ＝R N +h。
The finishing method comprises the following steps:
Figure BDA0002024140860000149
discretization of Kalman filtering system equation and measurement equation
Making approximate discretization equivalence:
X k ＝Φ k/k-1 X k-1 +Γ k-1 W k-1
in which a discretized time interval T is set s ＝t k -t k-1 Then the state transition matrix takes a first order truncation, having:
Figure BDA0002024140860000151
Figure BDA0002024140860000152
W k-1 as a system noise vector, V k To measure the noise vector, both are zero-mean gaussian white noise vector sequences (obeying normal distribution), and they are uncorrelated with each other, i.e. they satisfy:
{E[W k ]＝0,
Figure BDA0002024140860000153
E[V k ]＝0,
Figure BDA0002024140860000154
the fundamental assumption of noise requirements in a Kalman Filter State space model, generally requires Q k Is semi-positive and R k Is positive, i.e. Q k Not less than 0 and R k Is greater than 0. The complete Kalman filtering algorithm can be divided into five basic formulas as follows:
(1) State one-step prediction
Figure BDA0002024140860000155
(2) State one-step prediction mean square error
Figure BDA0002024140860000156
(3) Filter gain
Figure BDA0002024140860000157
(4) State estimation
Figure BDA0002024140860000158
(5) State estimation mean square error
P k ＝(I-K k H k )P k/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, it will be understood by those skilled in the art that various changes in form and details may be made therein without departing from the spirit and scope of the invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims (8)

1. A dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated 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;
and 2, step: acquiring INS original navigation data and GNSS data in the vehicle running process;
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, compensation quantity, INS navigation related information, speed increment attitude, speed and position into a combined filter;
and 5: 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 an INS calculation result by using a result estimated by filtering;
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:
Figure FDA0003953848410000011
wherein, R is a characteristic parameter of the working condition of the vehicle, and k 1 、k 2 And k 3 The weight values corresponding to the three axes are respectively,
Figure FDA0003953848410000012
Figure FDA0003953848410000013
and
Figure FDA0003953848410000019
are respectively the current triaxial ratio values,
Figure FDA0003953848410000014
and
Figure FDA0003953848410000015
respectively the current input three-axis angular velocity;
the calculation formula of the compensation amount in the step 4 is as follows:
Figure FDA0003953848410000016
Figure FDA0003953848410000017
Figure FDA0003953848410000018
and R is less than or equal to R th2
In the formula (I), the compound is shown in the specification,
Figure FDA0003953848410000021
for static measurements, δ l b For the value of the lever arm,
Figure FDA0003953848410000022
and
Figure FDA0003953848410000023
lever 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 respectively x 、φ y And phi z Respectively the vehicle attitude angles, | R th And | R |) th2 For the set threshold value for defining the decision section,
Figure FDA0003953848410000024
to dynamically compensate for lever arm values.
2. The method as claimed in claim 1, wherein the usability judgment in step 3 is described as follows:
Figure FDA0003953848410000025
in the formula, Q GNSS Indicating signal availability.
3. 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:
Figure FDA0003953848410000026
Figure FDA0003953848410000027
Figure FDA0003953848410000028
Figure FDA0003953848410000029
in the formula,. DELTA.theta. m1 And Δ θ m2 Corresponding angle increment is sampled for the gyro at two equal intervals, T is sampling time,
Figure FDA00039538484100000210
for reference with inertial coordinate system, the carrier system is started from t m-1 Time t m The change in the rotation at a moment in time,
Figure FDA00039538484100000211
for reference by the inertial frame, the geographic system is from t m Time t m-1 The 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 t m Time, (m-1) represents t m-1 At the moment, phi represents the corresponding posture with the subscript, I represents the identity matrix,
Figure FDA00039538484100000212
is a constant value.
4. 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.
5. The method as claimed in claim 4, wherein the system equation in step 51 is described as:
Figure FDA0003953848410000031
wherein X is a state vector, phi E 、φ N And phi U Respectively, attitude error, δ v, in east-north-sky geographic coordinate system E 、δv N And δ v U Respectively, velocity error in east-north-sky geographic coordinate system, position error of longitude, latitude and altitude, delta L, delta lambda and delta h, epsilon x 、ε y And epsilon z Respectively the zero offset of three coordinate axes of the gyroscope,
Figure FDA0003953848410000032
and
Figure FDA0003953848410000033
zero offset for three coordinate axes of the accelerometer respectively;
Figure FDA0003953848410000034
Figure FDA0003953848410000035
Figure FDA0003953848410000036
Figure FDA0003953848410000037
Figure FDA0003953848410000038
in the formula (I), the compound is shown in the specification,
Figure FDA0003953848410000039
is the angular velocity of the geographic system relative to the inertial system,
Figure FDA00039538484100000310
the angular velocity error of the earth system relative to the inertial system,
Figure FDA00039538484100000311
is the angular velocity error of the geographic system relative to the earth system,
Figure FDA00039538484100000312
is a coordinate transformation matrix of the carrier system to the geographical system,
Figure FDA00039538484100000313
is the angular velocity error of the carrier system relative to the inertial system,
Figure FDA00039538484100000314
is the output specific force, v, of the carrier system relative to the inertial navigation system accelerometer under the geographic system n Is the speed of the carrier under the geographic system,
Figure FDA00039538484100000315
is the angular velocity of the earth system relative to the inertial system,
Figure FDA00039538484100000316
is the angular velocity, δ v, of the geographic system relative to the Earth's system n Is the speed error of the carrier under the geographical system,
Figure FDA00039538484100000317
is the output specific force error, delta g, of the inertial navigation system accelerometer under the carrier system relative to the geographic system n As error of gravitational acceleration, R M Radius of the mortise, h local altitude, L local latitude and R N And for the meridian radius, the single phi represents a mathematical platform error angle in the strapdown inertial navigation system.
6. The method of claim 5, wherein the measurement equation in step 52 describes the formula as:
Figure FDA00039538484100000318
Figure FDA00039538484100000319
Figure FDA00039538484100000320
Figure FDA0003953848410000041
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,
Figure FDA0003953848410000042
representing the angular velocity, R, of the carrier system relative to the earth system Mh ＝R M +h，R Nh ＝R N +h。
7. The method as claimed in claim 1, wherein the step 5 further comprises: feeding back the kalman filtered gyroscope 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 the output value calculated by the INS for correction, namely: by modified
Figure FDA0003953848410000043
The course angle psi and the pitching angle can be obtainedThe angle theta and the rolling angle gamma, and after one-time filtering feedback, the error state returns to 0.
8. The method as claimed in claim 7, 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 system
Figure FDA0003953848410000044
The heading angle psi, the pitch angle theta and the roll angle gamma can be obtained through solution, and the corresponding description formulas are as follows:
Figure FDA0003953848410000045
in the formula, (numeral 1, numeral 2) represents a specific corresponding matrix element in the matrix.
CN201910288631.0A 2019-04-11 2019-04-11 Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation Active CN110221332B (en)

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 CN110221332A (en) 2019-09-10
CN110221332B true 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)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
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
CN113971846B (en) * 2020-07-22 2023-05-09 宇通客车股份有限公司 Positioning failure detection method and device for automatic driving vehicle
CN111678538B (en) * 2020-07-29 2023-06-09 中国电子科技集团公司第二十六研究所 Dynamic level error compensation method based on speed matching
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
CN113029139B (en) * 2021-04-07 2023-07-28 中国电子科技集团公司第二十八研究所 Airport flight area vehicle differential Beidou/SINS combined navigation method based on motion detection
CN113865583B (en) * 2021-07-20 2024-02-09 北京航天控制仪器研究所 Accelerometer combination dynamic installation deviation matrix determining and compensating method
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
CN114894222B (en) * 2022-07-12 2022-10-28 深圳元戎启行科技有限公司 External parameter calibration method of IMU-GNSS antenna and related method and equipment

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2006104552A1 (en) * 2005-03-29 2006-10-05 Honeywell International Inc. Method and apparatus for high accuracy relative motion determinatation using inertial sensors

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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)

* Cited by examiner, † Cited by third party
Title
光纤陀螺捷联惯导系统速度算法的改进研究;肖虎等;《弹箭与制导学报》;20100215;第30卷(第01期);80-83 *
减小动态误差的捷联系统姿态算法研究;缪玲娟等;《兵工学报》;20000830;第21卷(第03期);229-232 *
弱ＧＮＳＳ信号区域的ＧＮＳＳ／ＩＮＳ／ＬｉＤＡＲ高精度定位方法及其应用研究;钱闯;《中国优秀博硕士学位论文全文数据库(博士)信息科技辑》;20190115(第12期);40-54 *
考虑机抖激光陀螺信号滤波特性的圆锥算法修正;潘献飞等;《中国惯性技术学报》;20070615;第15卷(第03期);259-264 *
高动态GPS/BD2组合导航信号模拟器关键技术研究;王海峰等;《宇航计测技术》;20130615;第33卷(第03期);17-22 *

Also Published As

Publication number Publication date
CN110221332A (en) 2019-09-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
CN112629538B (en) Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
CN110031882B (en) External measurement information compensation method based on SINS/DVL integrated navigation system
US9541392B2 (en) Surveying system and method
CN111024064B (en) SINS/DVL combined navigation method for improving Sage-Husa adaptive filtering
CN111156994B (en) INS/DR & GNSS loose combination navigation method based on MEMS inertial component
JP5068531B2 (en) Method and system for improving the accuracy of inertial navigation measurements using measured and stored gravity gradients
CN110779521A (en) Multi-source fusion high-precision positioning method and device
CN110146075B (en) SINS/DVL combined positioning method of gain compensation adaptive filtering
EP1582840A1 (en) Inertial navigation system error correction
CN110146076B (en) SINS/DVL combined positioning method without inverse matrix adaptive filtering
CN113203418B (en) GNSSINS visual fusion positioning method and system based on sequential Kalman filtering
CN111121766B (en) Astronomical and inertial integrated navigation method based on starlight vector
CN112504275B (en) Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm
CN111024074B (en) Inertial navigation speed error determination method based on recursive least square parameter identification
CN112507281B (en) Method for estimating SINS/DVL tight combination system based on double-state multi-factor robust difference
CN109612460B (en) Plumb line deviation measuring method based on static correction
CN112432642B (en) Gravity beacon and inertial navigation fusion positioning method and system
CN111141273A (en) Combined navigation method and system based on multi-sensor fusion
CN108225312B (en) Lever arm estimation and compensation method in GNSS/INS loose combination
CN114964222A (en) Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device
CN109084755B (en) Accelerometer zero offset estimation method based on gravity apparent velocity and parameter identification
CN114526731A (en) Inertia combination navigation direction positioning method based on moped

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