CN106767797B - inertial/GPS combined navigation method based on dual quaternion - Google Patents

inertial/GPS combined navigation method based on dual quaternion Download PDF

Info

Publication number
CN106767797B
CN106767797B CN201710178159.6A CN201710178159A CN106767797B CN 106767797 B CN106767797 B CN 106767797B CN 201710178159 A CN201710178159 A CN 201710178159A CN 106767797 B CN106767797 B CN 106767797B
Authority
CN
China
Prior art keywords
quaternion
matrix
dual
multiplication
error
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
CN201710178159.6A
Other languages
Chinese (zh)
Other versions
CN106767797A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201710178159.6A priority Critical patent/CN106767797B/en
Publication of CN106767797A publication Critical patent/CN106767797A/en
Application granted granted Critical
Publication of CN106767797B publication Critical patent/CN106767797B/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/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention discloses an inertia/GPS integrated navigation method based on dual quaternion, belonging to the technical field of integrated navigation of aircrafts. Firstly, establishing an error model of a gyroscope and an accelerometer, expanding errors of the gyroscope and the accelerometer into system state variables, and constructing an inertia/GPS (global positioning system) combined Kalman filtering state equation based on dual quaternions; then, combining the measurement information of the GPS and the calculation value of the dual quaternion strapdown inertial navigation algorithm to construct an inertia/GPS combined Kalman filtering measurement equation based on the dual quaternion; and finally, discretizing a system state equation and a measurement equation, performing closed-loop estimation by adopting Kalman filtering, and correcting a thrust velocity dual quaternion, a gravitational velocity dual quaternion and a position dual quaternion in the inertial navigation algorithm to obtain navigation information of the carrier. The method can effectively utilize the output information of the GPS to correct the inertial navigation resolving error, improves the performance of the inertial navigation system, and is suitable for engineering application.

Description

inertial/GPS combined navigation method based on dual quaternion
Technical Field
The invention relates to an inertia/GPS integrated navigation method based on dual quaternion, belonging to the technical field of integrated navigation of aircrafts.
Background
In recent years, with the development and development of high-dynamic aircrafts such as hypersonic aircrafts and the like, higher requirements are put forward on the performance of navigation systems. The inertial navigation system has the outstanding advantages of high short-time precision, continuous output, complete autonomy and the like, but the error is accumulated along with the time and needs other navigation means for assistance.
The GPS is a high-precision global three-dimensional real-time satellite system, and the globality and high precision of navigation positioning make the GPS an advanced navigation device. However, the GPS has some disadvantages, mainly that the GPS is prone to signal loss under the condition of being shielded, and is prone to human control and interference, and therefore, the GPS is mainly used as an auxiliary navigation device. The inertia/GPS combination overcomes the defects of the inertia/GPS combination, makes up for the shortages, and ensures that the navigation precision after the combination is higher than the precision of the independent work of the two systems.
The dual quaternion strapdown inertial navigation algorithm considers the rotation and the translation of the carrier uniformly, represents general rigid motion in the simplest form, has higher precision than the traditional strapdown inertial navigation algorithm in a high dynamic environment, and can meet the requirement of a high dynamic aircraft on high-precision navigation performance. The traditional inertial navigation algorithm error model is usually a linear error equation based on a mathematical platform misalignment angle, but the model is only suitable for the case that the platform misalignment angle is small, when the attitude angle error of a carrier is large, the filtering precision is greatly reduced by using the model in the combined navigation algorithm, and the convergence time is prolonged or even divergence is caused. The strapdown inertial navigation algorithm based on the dual quaternion can directly establish a linear error model by using the dual quaternion, and has the advantages of higher convergence speed and better filtering precision even under the condition of a large misalignment angle, thereby improving the performance of the integrated navigation system. Therefore, the research of the inertia/GPS combined navigation algorithm based on the dual quaternion has important research significance.
Disclosure of Invention
The invention provides an inertial/GPS integrated navigation method based on dual quaternion, which effectively utilizes speed position information provided by a GPS in the dynamic flight process of an aircraft to correct inertial navigation resolving parameter errors and obviously improves the precision of an inertial navigation system.
The invention adopts the following technical scheme for solving the technical problems:
an inertia/GPS combined navigation method based on dual quaternion comprises the following steps:
step 1, establishing a gyro and accelerometer error model, wherein the gyro error comprises a constant drift error, a first-order Markov process random noise and a white noise random error, and the accelerometer error is the first-order Markov process random noise;
step 2, on the basis of modeling the errors of the gyroscope and the accelerometer in the step 1, expanding the gyro constant drift error, the gyro first-order Markov process random noise and the accelerometer first-order Markov process random noise in the step 1 into system state variables, and constructing an inertia/GPS combined Kalman filtering state equation based on dual quaternions;
step 3, modeling the measurement error of the geographic system speed and the earth system position output by the GPS into white noise, converting the measured geographic system speed into an inertial system speed, and constructing an inertial/GPS combined Kalman filtering measurement equation based on dual quaternions by combining the inertial system speed and the earth system position which are calculated by a dual quaternion strapdown inertial navigation algorithm;
and 4, discretizing a system state equation and a measurement equation, performing closed-loop estimation on the state quantity by adopting Kalman filtering, and correcting a thrust dual quaternion, a gravity dual quaternion and a position dual quaternion in the inertial navigation algorithm by using the dual quaternion error obtained by estimation so as to obtain navigation information such as the speed, the position, the attitude and the like of the carrier.
Step 1, the gyro and accelerometer error model is as follows:
Figure BDA0001252914680000021
Figure BDA0001252914680000022
wherein the content of the first and second substances,
Figure BDA0001252914680000031
is gyro error, epsilonbIs the gyro constant drift error, epsilonrFor top first order Markov process random noise, omegagIs white noise; δ fBIn order to be an accelerometer error,
Figure BDA0001252914680000032
random noise for an accelerometer first order Markov process;
for epsilon in the above formulab、εr、
Figure BDA0001252914680000033
The derivation yields the following mathematical expression:
Figure BDA0001252914680000034
Figure BDA0001252914680000035
Figure BDA0001252914680000036
wherein the content of the first and second substances,
Figure BDA0001252914680000037
is epsilonbThe first derivative of (a);
Figure BDA0001252914680000038
is epsilonrThe first derivative of (a);
Figure BDA0001252914680000039
is composed of
Figure BDA00012529146800000310
The first derivative of (a); t isgFor the time associated with the top first order markov process,ωrdriving white noise for a top first order Markov process; t isaTime, omega, of relevance for the first order Markov process of an accelerometeraWhite noise is driven for the accelerometer first order markov process.
Step 2, the dual quaternion-based inertia/GPS combined Kalman filtering state equation is as follows:
Figure BDA00012529146800000311
wherein X ∈ R28×1For the system state quantity, F ∈ R28×28For the system matrix, G ∈ R28×12For the noise coefficient matrix, w ∈ R12×1In order to be the system noise vector,
Figure BDA00012529146800000312
for the first derivative of X, each matrix is represented as:
Figure BDA00012529146800000313
Figure BDA0001252914680000041
Figure BDA0001252914680000042
in the system matrix F and the noise coefficient matrix G, when a quaternion q is written as q ═ q0q1q2q3]TIn the form of (1), we define a matrix
Figure BDA0001252914680000043
For q pre-multiplying matrices in quaternion multiplication,
Figure BDA0001252914680000044
is a post-multiplication matrix of q in quaternion multiplication, which can be specifically expressed as:
Figure BDA0001252914680000045
0 in the system matrix F and the noise coefficient matrix G is a fourth-order zero matrix I4Is a fourth order identity matrix, each variable is a quaternion, wherein the three-dimensional vector is represented as a quaternion with a scalar part of 0. δ qITIs the real part of the thrust velocity dual quaternion error, δ q'ITIs the dual part of the thrust speed dual quaternion error, δ q'IGIs the dual part of gravity velocity dual quaternion error, δ q'IUIs the dual part of the position dual quaternion error;
Figure BDA0001252914680000046
the information is output for the gyro or the gyroscope,
Figure BDA0001252914680000047
is composed of
Figure BDA0001252914680000048
A post-multiplication matrix in quaternion multiplication; q. q.sITIs the real part of the thrust velocity dual quaternion,
Figure BDA0001252914680000049
is qITA pre-multiplication matrix in a quaternion multiplication; q's'ITIs the dual part of the thrust dual quaternion,
Figure BDA0001252914680000051
is q'ITA pre-multiplication matrix in a quaternion multiplication;
Figure BDA0001252914680000052
is the angular velocity of the earth's rotation,
Figure BDA0001252914680000053
is composed of
Figure BDA0001252914680000054
A post-multiplication matrix in quaternion multiplication; q. q.sIUReal numbers being position-duality quaternionsIn part (a) of the above-described embodiments,
Figure BDA0001252914680000055
for its pre-multiplication matrix in the quaternion multiplication,
Figure BDA0001252914680000056
a post-multiplication matrix for the post-multiplication matrix in quaternion multiplication; q. q.s* ITIs qITThe number of the conjugate quaternion of (c),
Figure BDA0001252914680000057
for its pre-multiplication matrix in the quaternion multiplication,
Figure BDA0001252914680000058
a post-multiplication matrix for the post-multiplication matrix in quaternion multiplication;
m in system matrix F and noise coefficient matrix G1、M2、M3Can be respectively expressed as:
Figure BDA0001252914680000059
the GPS output geosystem speed and geosystem position measurement error model in the step 3 is as follows:
VG＝Vn+δV，
RG＝Re+δR，
wherein, VGGeographic system velocity, V, output for GPSnThe actual geographic system speed of the carrier is adopted, the delta V is the speed measurement error of the GPS, and the speed measurement error is modeled as white noise; rGPosition of the earth system, R, measured for GPSeThe position of the real earth system of the carrier is delta R, the position measurement error of the GPS is measured, and the position measurement error is modeled as white noise;
v is expressed by the following formulaGConversion to the inertial system:
Figure BDA00012529146800000510
wherein, VGIIs a VGSwitch over toThe value of the inertia system is set to be,
Figure BDA00012529146800000513
is a transformation matrix, omega, from the earth system to the inertial systemieIs a quaternion
Figure BDA00012529146800000511
The vector portion of (a) is,
Figure BDA00012529146800000512
a transformation matrix from a geographical system to an earth system;
thus, the dual quaternion-based inertial/GPS combined kalman filter measurement equation of step 3 can be established:
Z＝HX+v，
wherein the content of the first and second substances,
Figure BDA0001252914680000061
for measuring the vector, VICalculating the resulting inertial system velocity, R, for a dual quaternion inertial navigation algorithmICalculating the obtained earth system position for a dual quaternion inertial navigation algorithm; h is a matrix of measurement coefficients, X is a system state quantity,
Figure BDA0001252914680000062
measuring a noise matrix for the system;
the specific expression of the measurement coefficient matrix H is:
Figure BDA0001252914680000063
wherein q isIGThe real part of the gravity velocity dual quaternion,
Figure BDA0001252914680000064
is qIGThe number of the conjugate quaternion of (c),
Figure BDA0001252914680000065
is q* IGA post-multiplication matrix in quaternion multiplication; q. q.sIUThe real part of the position-pair quaternion,* IUis qIUThe number of the conjugate quaternion of (c),
Figure BDA0001252914680000066
is q* IUThe pre-multiplication matrix in the quaternion multiplication,
Figure BDA0001252914680000067
is q'ITPre-multiplication matrix in quaternion multiplication.
The specific process of the step 4 is as follows:
(401) discretizing a system state equation and a measurement equation:
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1，
Zk＝HkXk+Vk，
wherein, XkIs tkTime of day system state quantity, Xk-1Is tk-1Time of day system state quantity, phik,k-1Is tk-1Time to tkState transition matrix of time system, gammak,k-1Is tk-1Time to tkNoise-driven matrix of time-of-day system, Wk-1Is tk-1Noise matrix of time of day system, ZkIs tkMeasurement matrix of time system, HkIs tkMeasurement coefficient matrix of time, VkIs tkAn observed quantity noise matrix of a time;
(402) performing closed-loop estimation on the state quantity by adopting Kalman filtering:
Figure BDA0001252914680000068
Figure BDA0001252914680000069
Figure BDA00012529146800000610
Figure BDA00012529146800000611
Figure BDA00012529146800000612
wherein the content of the first and second substances,
Figure BDA00012529146800000613
is a state quantity Xk-1One-step predictive estimate of, Pk-1Is tk-1Time-of-day filtering state estimation covariance matrix, Qk-1Is tk-1The time of day system noise covariance matrix is,
Figure BDA0001252914680000071
is phik,k-1The transpose of (a) is performed,
Figure BDA0001252914680000072
is gammak,k-1Transpose of (P)k,k-1Is tk-1Time tkState one-step prediction covariance matrix, R, for a time instantkIs tkTime of day measurement noise covariance matrix, KkIs tkThe time-of-day filter gain matrix is,
Figure BDA0001252914680000073
is HkThe transpose of (a) is performed,
Figure BDA0001252914680000074
is a state quantity XkI is an identity matrix,
Figure BDA0001252914680000075
is KkTranspose of (P)kIs tkEstimating a covariance matrix by a time filtering state;
(403) after obtaining each state quantity estimated value (402), correcting a thrust dual quaternion, an attraction dual quaternion and a position dual quaternion in the inertial navigation algorithm by using the dual quaternion error obtained by estimation, wherein a correction model is as follows:
Figure BDA0001252914680000076
Figure BDA0001252914680000077
Figure BDA0001252914680000078
Figure BDA0001252914680000079
in the above formula, the first and second carbon atoms are,
Figure BDA00012529146800000710
is δ qITIs determined by the estimated value of (c),
Figure BDA00012529146800000711
the real part of the modified thrust velocity dual quaternion is used as the real part;
Figure BDA00012529146800000712
is δ q'ITIs determined by the estimated value of (c),
Figure BDA00012529146800000713
the dual part is the modified thrust velocity dual quaternion;
Figure BDA00012529146800000714
is δ q'IGIs determined by the estimated value of (c),
Figure BDA00012529146800000715
a dual part that is a modified gravity velocity dual quaternion;
Figure BDA00012529146800000716
is δ q'IUIs determined by the estimated value of (c),
Figure BDA00012529146800000717
the dual part of the modified position dual quaternion.
The invention has the following beneficial effects:
1. the dual quaternion-based inertial/GPS algorithm can directly utilize the dual quaternion to establish a linear error model, so that the combined navigation system can also have higher filtering convergence speed and better filtering precision even under the condition of a large misalignment angle, thereby improving the performance of the combined navigation system and being suitable for engineering application.
2. The method provides a conversion method of GPS output information before combination and a system measurement noise model after coordinate conversion, and the model can effectively improve dual quaternion inertia/GPS combined navigation precision and is suitable for engineering application.
Drawings
FIG. 1 is an architecture diagram of the dual quaternion based inertial/GPS combined navigation method of the present invention.
Detailed Description
The invention is described in further detail below with reference to the accompanying drawings.
As shown in fig. 1, the principle of the dual quaternion-based inertial/GPS combined navigation method of the present invention is: the GPS outputs the geographic system speed and the earth system position information, and the geographic system speed output by the GPS is converted into the inertial system speed through coordinates. Meanwhile, the dual quaternion strapdown inertial navigation algorithm can correspondingly obtain the speed information of the carrier under an inertial system and the position information under the earth system. The earth system position and the inertia system speed of the GPS and the dual quaternion strapdown inertial navigation algorithm are fused by a Kalman filter, dual quaternion error parameters are obtained through online estimation, and the dual quaternion is compensated and corrected by the information, so that the performance of the inertial navigation system is improved.
The specific embodiment of the invention is as follows:
1. establishing error model of gyroscope and accelerometer
The gyro error comprises a constant drift error, a first-order Markov process random noise and a white noise error, the accelerometer error is modeled into the first-order Markov process random noise, and a specific error model is as follows:
Figure BDA0001252914680000081
Figure BDA0001252914680000082
wherein the content of the first and second substances,
Figure BDA0001252914680000083
is gyro error, epsilonbIs the gyro constant drift error, epsilonrFor top first order Markov process random noise, omegagIs white noise; δ fBIn order to be an accelerometer error,
Figure BDA0001252914680000084
random noise for an accelerometer first order Markov process;
for epsilon in the above formulab、εr、
Figure BDA0001252914680000085
The derivation yields the following mathematical expression:
Figure BDA0001252914680000086
Figure BDA0001252914680000087
Figure BDA0001252914680000088
wherein the content of the first and second substances,
Figure BDA0001252914680000091
is epsilonbThe first derivative of (a);
Figure BDA0001252914680000092
is epsilonrThe first derivative of (a);
Figure BDA0001252914680000093
is composed of
Figure BDA0001252914680000094
The first derivative of (a); t isgFor the gyro first order Markov process correlation time, omegarDriving white noise for a top first order Markov process; t isaTime, omega, of relevance for the first order Markov process of an accelerometeraWhite noise is driven for the accelerometer first order markov process.
2. Kalman filtering state equation based on gyro and accelerometer error model is established
The equation of state is as follows:
Figure BDA0001252914680000095
wherein X ∈ R28×1For the system state quantity, F ∈ R28×28For the system matrix, G ∈ R28×12For the noise coefficient matrix, w ∈ R12×1In order to be the system noise vector,
Figure BDA0001252914680000096
for the first derivative of X, each matrix is represented as:
Figure BDA0001252914680000097
Figure BDA0001252914680000098
Figure BDA0001252914680000099
Figure BDA0001252914680000101
system matrix F and noise figureIn the matrix G, when a quaternion q is written as q ═ q0q1q2q3]TIn the form of (1), we define a matrix
Figure BDA0001252914680000102
For q pre-multiplying matrices in quaternion multiplication,
Figure BDA0001252914680000103
is a post-multiplication matrix of q in quaternion multiplication, which can be specifically expressed as:
Figure BDA0001252914680000104
Figure BDA0001252914680000105
in the formulae (7) to (10), 0 is a fourth-order zero matrix, I4Is a fourth order identity matrix, each variable is a quaternion, wherein the three-dimensional vector is represented as a quaternion with a scalar part of 0. δ qITIs the real part of the thrust velocity dual quaternion error, δ q'ITIs the dual part of the thrust speed dual quaternion error, δ q'IGIs the dual part of gravity velocity dual quaternion error, δ q'IUIs the dual part of the position dual quaternion error;
Figure BDA0001252914680000106
the information is output for the gyro or the gyroscope,
Figure BDA0001252914680000107
is composed of
Figure BDA0001252914680000108
A post-multiplication matrix in quaternion multiplication; q. q.sITIs the real part of the thrust velocity dual quaternion,
Figure BDA0001252914680000109
is qITA pre-multiplication matrix in a quaternion multiplication; q's'ITIs the dual part of the thrust dual quaternion,
Figure BDA00012529146800001010
is q'ITA pre-multiplication matrix in a quaternion multiplication;
Figure BDA00012529146800001011
is the angular velocity of the earth's rotation,
Figure BDA00012529146800001012
is composed of
Figure BDA00012529146800001013
A post-multiplication matrix in quaternion multiplication; q. q.sIUThe real part of the position-pair quaternion,
Figure BDA00012529146800001014
for its pre-multiplication matrix in the quaternion multiplication,
Figure BDA00012529146800001015
a post-multiplication matrix for the post-multiplication matrix in quaternion multiplication; q. q.s* ITIs qITThe number of the conjugate quaternion of (c),
Figure BDA0001252914680000111
for its pre-multiplication matrix in the quaternion multiplication,
Figure BDA0001252914680000112
a post-multiplication matrix for the post-multiplication matrix in quaternion multiplication;
m in the formulae (9) and (10)1、M2、M3Can be respectively expressed as:
Figure BDA0001252914680000113
Figure BDA0001252914680000114
Figure BDA0001252914680000115
3. kalman filtering measurement equation based on GPS measurement error model
(3.1) GPS measurement error model
The GPS measurement error model is as follows:
VG＝Vn+δV (16)
RG＝Re+δR (17)
wherein, VGGeographic system velocity, V, output for GPSnThe actual geographic system speed of the carrier is adopted, the delta V is the speed measurement error of the GPS, and the speed measurement error is modeled as white noise; rGPosition of the earth system, R, measured for GPSeThe position of the real earth system of the carrier is delta R, the position measurement error of the GPS is measured, and the position measurement error is modeled as white noise;
v is expressed by the following formulaGConversion to the inertial system:
Figure BDA0001252914680000116
wherein, VGIIs a VGThe value is converted to a value in the inertial system,
Figure BDA0001252914680000121
is a transformation matrix, omega, from the earth system to the inertial systemieIs a quaternion
Figure BDA0001252914680000122
The vector portion of (a) is,
Figure BDA0001252914680000123
a transformation matrix from a geographical system to an earth system;
(3.2) establishing a Kalman filtering measurement equation
The measurement equation is as follows:
Z＝HX+v (19)
wherein the content of the first and second substances,
Figure BDA0001252914680000124
for measuring the vector, VICalculating the resulting inertial system velocity, R, for a dual quaternion inertial navigation algorithmICalculating the obtained earth system position for a dual quaternion inertial navigation algorithm; h is a matrix of measurement coefficients, X is a system state quantity,
Figure BDA0001252914680000125
measuring a noise matrix for the system;
the specific expression of the measurement coefficient matrix H is:
Figure BDA0001252914680000126
wherein q isIGThe real part of the gravity velocity dual quaternion, q* IGIs qIGThe number of the conjugate quaternion of (c),
Figure BDA0001252914680000127
is q* IGA post-multiplication matrix in quaternion multiplication; q. q.sIUReal part of a position-pair quaternion, q* IUIs qIUThe number of the conjugate quaternion of (c),
Figure BDA0001252914680000128
is q* IUThe pre-multiplication matrix in the quaternion multiplication,
Figure BDA0001252914680000129
is q'ITPre-multiplication matrix in quaternion multiplication.
4. Dual quaternion error online estimation and compensation correction
(4.1) discretization of System State equation and measurement equation
Discretizing a system state equation and a measurement equation:
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1(21)
Zk＝HkXk+Vk(22)
wherein, XkIs tkTime of day system state quantity, Xk-1Is tk-1Time of day system state quantity, phik,k-1Is tk-1Time to tkState transition matrix of time system, gammak,k-1Is tk-1Time to tkNoise-driven matrix of time-of-day system, Wk-1Is tk-1Noise matrix of time of day system, ZkIs tkMeasurement matrix of time system, HkIs tkMeasurement coefficient matrix of time, VkIs tkAn observed quantity noise matrix of a time;
(4.2) Kalman filtering closed-loop estimation
Performing closed-loop estimation on the state quantity by adopting Kalman filtering:
Figure BDA0001252914680000131
Figure BDA0001252914680000132
Figure BDA0001252914680000133
Figure BDA0001252914680000134
Figure BDA0001252914680000135
wherein the content of the first and second substances,
Figure BDA0001252914680000136
is a state quantity Xk-1One-step predictive estimate of, Pk-1Is tk-1Time-of-day filtering state estimation covariance matrix, Qk-1Is tk-1The time of day system noise covariance matrix is,
Figure BDA0001252914680000137
is phik,k-1The transpose of (a) is performed,
Figure BDA0001252914680000138
is gammak,k-1Transpose of (P)k,k-1Is tk-1Time tkState one-step prediction covariance matrix, R, for a time instantkIs tkTime of day measurement noise covariance matrix, KkIs tkThe time-of-day filter gain matrix is,
Figure BDA0001252914680000139
is HkThe transpose of (a) is performed,
Figure BDA00012529146800001310
is a state quantity XkI is an identity matrix,
Figure BDA00012529146800001311
is KkTranspose of (P)kIs tkEstimating a covariance matrix by a time filtering state;
(4.3) correction of on-line compensation of dual quaternion errors
After the estimated values of the state quantities are obtained from the step (4.2), the dual quaternion errors obtained by estimation are utilized to correct the thrust dual quaternion, the gravity dual quaternion and the position dual quaternion in the inertial navigation algorithm, and the correction model is as follows:
Figure BDA00012529146800001312
Figure BDA00012529146800001313
Figure BDA00012529146800001314
Figure BDA00012529146800001315
in the above formula, the first and second carbon atoms are,
Figure BDA00012529146800001316
is δ qITIs determined by the estimated value of (c),
Figure BDA00012529146800001317
the real part of the modified thrust velocity dual quaternion is used as the real part;
Figure BDA00012529146800001318
is δ q'ITIs determined by the estimated value of (c),
Figure BDA00012529146800001319
the dual part is the modified thrust velocity dual quaternion;
Figure BDA00012529146800001320
is δ q'IGIs determined by the estimated value of (c),
Figure BDA00012529146800001321
a dual part that is a modified gravity velocity dual quaternion;
Figure BDA00012529146800001322
is δ q'IUIs determined by the estimated value of (c),
Figure BDA00012529146800001323
the dual part of the modified position dual quaternion.

Claims (5)

1. An inertia/GPS combined navigation method based on dual quaternion is characterized by comprising the following steps:
step 1, establishing a gyro and accelerometer error model, wherein the gyro error comprises a constant drift error, a first-order Markov process random noise and a white noise random error, and the accelerometer error is the first-order Markov process random noise;
step 2, on the basis of modeling the errors of the gyroscope and the accelerometer in the step 1, expanding the gyro constant drift error, the gyro first-order Markov process random noise and the accelerometer first-order Markov process random noise in the step 1 into system state variables, and constructing an inertia/GPS combined Kalman filtering state equation based on dual quaternions;
step 3, modeling the measurement error of the geographic system speed and the earth system position output by the GPS into white noise, converting the measured geographic system speed into an inertial system speed, and constructing an inertial/GPS combined Kalman filtering measurement equation based on dual quaternions by combining the inertial system speed and the earth system position which are calculated by a dual quaternion strapdown inertial navigation algorithm;
and 4, discretizing a system state equation and a measurement equation, performing closed-loop estimation on the state quantity by adopting Kalman filtering, and correcting a thrust dual quaternion, a gravity dual quaternion and a position dual quaternion in the inertial navigation algorithm by using the dual quaternion error obtained by estimation so as to obtain navigation information such as the speed, the position, the attitude and the like of the carrier.
2. The dual quaternion-based inertial/GPS combined navigation method according to claim 1, wherein the gyro and accelerometer error model in step 1 is:
Figure FDA0002253368750000011
δfB＝▽a，
wherein the content of the first and second substances,
Figure FDA0002253368750000012
is gyro error, epsilonbIs the gyro constant drift error, epsilonrFor top first order Markov process random noise, omegagIs white noise; δ fBFor accelerometer error, ▽aRandom noise for an accelerometer first order Markov process;
for epsilon in the above formulab、εr、▽aThe derivation yields the following mathematical expression:
Figure FDA0002253368750000013
Figure FDA0002253368750000021
Figure FDA0002253368750000022
wherein the content of the first and second substances,
Figure FDA0002253368750000023
is epsilonbThe first derivative of (a);
Figure FDA0002253368750000024
is epsilonrThe first derivative of (a);
Figure FDA0002253368750000025
is ▽aThe first derivative of (a); t isgFor the gyro first order Markov process correlation time, omegarDriving white noise for a top first order Markov process; t isaTime, omega, of relevance for the first order Markov process of an accelerometeraWhite noise is driven for the accelerometer first order markov process.
3. The dual quaternion-based inertial/GPS integrated navigation method according to claim 2, wherein the dual quaternion-based inertial/GPS integrated kalman filter state equation of step 2 is:
Figure FDA0002253368750000026
wherein X ∈ R28×1For the system state quantity, F ∈ R28×28For the system matrix, G ∈ R28×12For the noise coefficient matrix, w ∈ R12×1In order to be the system noise vector,
Figure FDA0002253368750000027
for the first derivative of X, each matrix is represented as:
Figure FDA0002253368750000028
Figure FDA0002253368750000029
Figure FDA0002253368750000031
in the system matrix F and the noise coefficient matrix G, when a quaternion q is written as q ═ q0q1q2q3]TIn the form of (1), we define a matrix
Figure FDA0002253368750000032
For q pre-multiplying matrices in quaternion multiplication,
Figure FDA0002253368750000033
is a post-multiplication matrix of q in quaternion multiplication, which can be specifically expressed as:
Figure FDA0002253368750000034
0 in the system matrix F and the noise coefficient matrix G is a fourth-order zero matrix I4The vector is a four-order identity matrix, each variable is a quaternion, and a three-dimensional vector is expressed as a quaternion with a scalar part of 0; δ qITIs the real part of the thrust velocity dual quaternion error, δ q'ITIs the dual part of the thrust speed dual quaternion error, δ q'IGIs the dual part of gravity velocity dual quaternion error, δ q'IUIs the dual part of the position dual quaternion error;
Figure FDA0002253368750000035
the information is output for the gyro or the gyroscope,
Figure FDA0002253368750000036
is composed of
Figure FDA0002253368750000037
A post-multiplication matrix in quaternion multiplication; q. q.sITIs the real part of the thrust velocity dual quaternion,
Figure FDA0002253368750000038
is qITA pre-multiplication matrix in a quaternion multiplication; q's'ITIs the dual part of the thrust dual quaternion,
Figure FDA0002253368750000039
is q'ITA pre-multiplication matrix in a quaternion multiplication;
Figure FDA00022533687500000310
is the angular velocity of the earth's rotation,
Figure FDA00022533687500000311
is composed of
Figure FDA00022533687500000312
A post-multiplication matrix in quaternion multiplication; q. q.sIUThe real part of the position-pair quaternion,
Figure FDA00022533687500000313
for its pre-multiplication matrix in the quaternion multiplication,
Figure FDA00022533687500000314
a post-multiplication matrix for the post-multiplication matrix in quaternion multiplication; q. q.s* ITIs qITThe number of the conjugate quaternion of (c),
Figure FDA00022533687500000315
for its pre-multiplication matrix in the quaternion multiplication,
Figure FDA00022533687500000316
a post-multiplication matrix for the post-multiplication matrix in quaternion multiplication;
m in system matrix F and noise coefficient matrix G1、M2、M3Can be respectively expressed as:
Figure FDA0002253368750000041
4. the dual quaternion-based inertial/GPS integrated navigation method according to claim 3, wherein the GPS output geodetic velocity and geodetic position measurement error model of step 3 is:
VG＝Vn+δV，
RG＝Re+δR，
wherein, VGGeographic system velocity, V, output for GPSnThe actual geographic system speed of the carrier is adopted, the delta V is the speed measurement error of the GPS, and the speed measurement error is modeled as white noise; rGPosition of the earth system, R, measured for GPSeThe position of the real earth system of the carrier is delta R, the position measurement error of the GPS is measured, and the position measurement error is modeled as white noise;
v is expressed by the following formulaGConversion to the inertial system:
Figure FDA0002253368750000042
wherein, VGIIs a VGThe value is converted to a value in the inertial system,
Figure FDA0002253368750000043
is a transformation matrix, omega, from the earth system to the inertial systemieIs a quaternion
Figure FDA0002253368750000044
The vector portion of (a) is,
Figure FDA0002253368750000045
a transformation matrix from a geographical system to an earth system;
thus, the dual quaternion-based inertial/GPS combined kalman filter measurement equation of step 3 can be established:
Z＝HX+v，
wherein the content of the first and second substances,
Figure FDA0002253368750000046
for measuring the vector, VICalculating the resulting inertial system velocity, R, for a dual quaternion inertial navigation algorithmICalculating the obtained earth system position for a dual quaternion inertial navigation algorithm; h is a matrix of measurement coefficients, X is a system state quantity,
Figure FDA0002253368750000047
measuring a noise matrix for the system;
the specific expression of the measurement coefficient matrix H is:
Figure FDA0002253368750000048
wherein q isIGThe real part of the gravity velocity dual quaternion, q* IGIs qIGThe number of the conjugate quaternion of (c),
Figure FDA0002253368750000051
is q* IGA post-multiplication matrix in quaternion multiplication; q. q.sIUReal part of a position-pair quaternion, q* IUIs qIUThe number of the conjugate quaternion of (c),
Figure FDA0002253368750000052
is q* IUThe pre-multiplication matrix in the quaternion multiplication,
Figure FDA0002253368750000053
is q'ITPre-multiplication matrix in quaternion multiplication.
5. The dual quaternion-based inertial/GPS combined navigation method according to claim 3, wherein the specific process of step 4 is as follows:
(401) discretizing a system state equation and a measurement equation:
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1，
Zk＝HkXk+Vk，
wherein, XkIs tkTime of day system state quantity, Xk-1Is tk-1Time of day system state quantity, phik,k-1Is tk-1Time to tkState transition matrix of time system, gammak,k-1Is tk-1Time to tkNoise-driven matrix of time-of-day system, Wk-1Is tk-1Noise matrix of time of day system, ZkIs tkMeasurement matrix of time system, HkIs tkMeasurement coefficient matrix of time, VkIs tkAn observed quantity noise matrix of a time;
(402) performing closed-loop estimation on the state quantity by adopting Kalman filtering:
Figure FDA0002253368750000054
Figure FDA0002253368750000055
Figure FDA0002253368750000056
Figure FDA0002253368750000057
Figure FDA0002253368750000058
wherein the content of the first and second substances,
Figure FDA0002253368750000059
is a state quantity Xk-1One-step predictive estimate of, Pk-1Is tk-1Time-of-day filtering state estimation covariance matrix, Qk-1Is tk-1The time of day system noise covariance matrix is,
Figure FDA00022533687500000510
is phik,k-1The transpose of (a) is performed,
Figure FDA00022533687500000511
is gammak,k-1Transpose of (P)k,k-1Is tk-1Time tkState one-step prediction covariance matrix, R, for a time instantkIs tkTime of day measurement noise covariance matrix, KkIs tkThe time-of-day filter gain matrix is,
Figure FDA00022533687500000512
is HkThe transpose of (a) is performed,
Figure FDA00022533687500000513
is a state quantity XkI is an identity matrix,
Figure FDA00022533687500000514
is KkTranspose of (P)kIs tkEstimating a covariance matrix by a time filtering state;
(403) after obtaining each state quantity estimated value (402), correcting a thrust dual quaternion, an attraction dual quaternion and a position dual quaternion in the inertial navigation algorithm by using the dual quaternion error obtained by estimation, wherein a correction model is as follows:
Figure FDA0002253368750000061
Figure FDA0002253368750000062
Figure FDA0002253368750000063
Figure FDA0002253368750000064
in the above formula, the first and second carbon atoms are,
Figure FDA0002253368750000065
is δ qITIs determined by the estimated value of (c),
Figure FDA0002253368750000066
the real part of the modified thrust velocity dual quaternion is used as the real part;
Figure FDA0002253368750000067
is δ q'ITIs determined by the estimated value of (c),
Figure FDA0002253368750000068
the dual part is the modified thrust velocity dual quaternion;
Figure FDA0002253368750000069
is δ q'IGIs determined by the estimated value of (c),
Figure FDA00022533687500000610
a dual part that is a modified gravity velocity dual quaternion;
Figure FDA00022533687500000611
is δ q'IUIs determined by the estimated value of (c),
Figure FDA00022533687500000612
the dual part of the modified position dual quaternion.
CN201710178159.6A 2017-03-23 2017-03-23 inertial/GPS combined navigation method based on dual quaternion Active CN106767797B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710178159.6A CN106767797B (en) 2017-03-23 2017-03-23 inertial/GPS combined navigation method based on dual quaternion

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710178159.6A CN106767797B (en) 2017-03-23 2017-03-23 inertial/GPS combined navigation method based on dual quaternion

Publications (2)

Publication Number Publication Date
CN106767797A CN106767797A (en) 2017-05-31
CN106767797B true CN106767797B (en) 2020-03-17

Family

ID=58966597

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710178159.6A Active CN106767797B (en) 2017-03-23 2017-03-23 inertial/GPS combined navigation method based on dual quaternion

Country Status (1)

Country Link
CN (1) CN106767797B (en)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108387918A (en) * 2018-01-18 2018-08-10 和芯星通（上海）科技有限公司 A kind of pedestrian navigation method and cloud system server, storage medium, electronic equipment
CN108827301A (en) * 2018-04-16 2018-11-16 南京航空航天大学 A kind of improvement error quaternion Kalman filtering robot pose calculation method
CN109470251A (en) * 2018-12-21 2019-03-15 陕西航天时代导航设备有限公司 A kind of partial feedback filtering method in integrated navigation system
CN110285804B (en) * 2019-06-26 2022-06-17 南京航空航天大学 Vehicle collaborative navigation method based on relative motion model constraint
CN111351482B (en) * 2020-03-19 2023-08-18 南京理工大学 Multi-rotor aircraft combined navigation method based on error state Kalman filtering
CN111982126B (en) * 2020-08-31 2023-03-17 郑州轻工业大学 Design method of full-source BeiDou/SINS elastic state observer model
CN113051757B (en) * 2021-03-23 2022-08-09 中国人民解放军海军工程大学 Strapdown inertial navigation generalized PSI angle error model construction method
CN113091754B (en) * 2021-03-30 2023-02-28 北京航空航天大学 Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method
CN117606474B (en) * 2024-01-24 2024-03-29 北京神导科技股份有限公司 Inertial astronomical combined navigation time synchronization method based on quaternion second-order interpolation

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104181574B (en) * 2013-05-25 2016-08-10 成都国星通信有限公司 A kind of SINS/GLONASS integrated navigation filtering system and method
CN103700082B (en) * 2013-12-23 2016-09-07 南京航空航天大学 Image split-joint method based on dual quaterion relative orientation
CN105512391B (en) * 2015-12-04 2018-09-25 上海新跃仪表厂 More star appearance rail dynamic modeling methods based on dual quaterion and its verification system
CN106052716B (en) * 2016-05-25 2019-04-05 南京航空航天大学 Gyro error online calibration method based on starlight information auxiliary under inertial system

Also Published As

Publication number Publication date
CN106767797A (en) 2017-05-31

Similar Documents

Publication Publication Date Title
CN106767797B (en) inertial/GPS combined navigation method based on dual quaternion
CN107655476B (en) Pedestrian high-precision foot navigation method based on multi-information fusion compensation
CN101949703B (en) Strapdown inertial/satellite combined navigation filtering method
CN102608596B (en) Information fusion method for airborne inertia/Doppler radar integrated navigation system
CN110702143B (en) Rapid initial alignment method for SINS strapdown inertial navigation system moving base based on lie group description
CN103235328B (en) GNSS (global navigation satellite system) and MEMS (micro-electromechanical systems) integrated navigation method
CN109931955B (en) Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering
CN105806363B (en) The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN110595503B (en) Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation
CN111024070A (en) Inertial foot binding type pedestrian positioning method based on course self-observation
CN102809377A (en) Aircraft inertia/pneumatic model integrated navigation method
CN104697520B (en) Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method
CN108168548B (en) Pedestrian inertial navigation system and method assisted by machine learning algorithm and model
CN111024074B (en) Inertial navigation speed error determination method based on recursive least square parameter identification
CN110849360B (en) Distributed relative navigation method for multi-machine collaborative formation flight
CN107036598A (en) Dual quaterion inertia/celestial combined navigation method based on gyro error amendment
CN112146655A (en) Elastic model design method for BeiDou/SINS tight integrated navigation system
CN112325886A (en) Spacecraft autonomous attitude determination system based on combination of gravity gradiometer and gyroscope
CN103674059A (en) External measured speed information-based horizontal attitude error correction method for SINS (serial inertial navigation system)
CN110926465A (en) MEMS/GPS loose combination navigation method
CN103245357A (en) Secondary quick alignment method of marine strapdown inertial navigation system
Bai et al. Improved preintegration method for GNSS/IMU/in-vehicle sensors navigation using graph optimization
CN105606093A (en) Inertial navigation method and device based on real-time gravity compensation
CN110926499A (en) Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation
CN108981691B (en) Sky polarized light combined navigation on-line filtering and smoothing method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant