CN113916261B - Attitude error assessment method based on inertial navigation optimization alignment - Google Patents
Attitude error assessment method based on inertial navigation optimization alignment Download PDFInfo
- Publication number
- CN113916261B CN113916261B CN202111176947.4A CN202111176947A CN113916261B CN 113916261 B CN113916261 B CN 113916261B CN 202111176947 A CN202111176947 A CN 202111176947A CN 113916261 B CN113916261 B CN 113916261B
- Authority
- CN
- China
- Prior art keywords
- alignment
- vector
- matrix
- measurement
- inertial navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000005457 optimization Methods 0.000 title claims abstract description 15
- 239000013598 vector Substances 0.000 claims abstract description 96
- 239000011159 matrix material Substances 0.000 claims abstract description 95
- 238000005259 measurement Methods 0.000 claims abstract description 92
- 230000003068 static effect Effects 0.000 claims abstract description 29
- 238000004364 calculation method Methods 0.000 claims description 39
- 229910002114 biscuit porcelain Inorganic materials 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000021615 conjugation Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 abstract description 5
- 230000007547 defect Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
The invention provides an attitude error assessment method based on inertial navigation optimization alignment, which comprises the following steps: step 1: according to the inertial navigation kinematic equation and the attitude differential equation of the navigation system, acquiring an initial attitude constraint equation of the navigation carrier under the conditions of static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment, and calculating a measurement vector in a sliding window; step 2: calculating a measurement matrix and a feature vector corresponding to the minimum feature value of the matrix according to static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment respectively; step 3: and calculating a covariance matrix of the initial attitude error angle according to vector measurement error characteristics under the conditions of static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment, and carrying out attitude error assessment. According to the method and the device, the covariance matrix of the attitude error can be calculated according to the measurement information of the navigation system, and a real-time precision evaluation index is provided for optimizing alignment.
Description
Technical Field
The invention relates to the technical field of inertial navigation initial alignment, in particular to an attitude error assessment method based on inertial navigation optimal alignment.
Background
Inertial device-based navigation vehicles, such as vehicles, airplanes, etc., need to achieve determination of their initial pose before navigation positioning begins. It is generally necessary to determine the attitude error to within a few degrees so that the navigation filter can function properly. This process of initial pose determination is also referred to as initial alignment, and may be specifically classified into stationary alignment and inter-travel alignment according to the state of motion of the carrier at the time of alignment. Stationary alignment typically requires the use of navigation-grade inertial devices to achieve solution of the initial attitude by sensing earth rotation. The inertial device comprises a gyroscope and an accelerometer, which sense the angular velocity and acceleration of the carrier, respectively. Inter-travel alignment typically also requires the use of other auxiliary measurement information, for example, alignment using satellite-provided velocity information as an auxiliary measurement is referred to as inertial/GPS inter-travel alignment, and alignment using odometer velocity as an auxiliary measurement is referred to as inertial/odometer inter-travel alignment. The initial pose matrix of the carrier can be directly solved by a multi-vector or multi-position method. However, these methods use limited measurement information, are susceptible to system disturbances and measurement errors, and are less robust. In contrast, the optimization-based alignment method is one of the most representative initial alignment methods in recent years. The optimization alignment method can fully utilize the output of the inertial device and other auxiliary measurement information in the alignment process, and solve the rough initial attitude of the carrier through the least square method.
Patent document CN102519485B (application number: CN 201110406806.7) discloses a two-position strapdown inertial navigation system initial alignment method introducing gyro information, which comprises establishing a strapdown inertial navigation system initial alignment state equation; establishing an initial alignment measurement equation for introducing gyro information by a strapdown inertial navigation system; constructing a Kalman filter for initial alignment of the strapdown inertial navigation system; acquiring information of a strapdown inertial navigation system, and finishing initial alignment filtering of a first position; and acquiring information of the strapdown inertial navigation system, and finishing initial alignment filtering of the second position.
Current optimized alignment schemes require a period of measurement information to be accumulated to achieve satisfactory attitude determination accuracy. As more measurement data is used in the optimization algorithm, the attitude determination error gradually decreases and tends to stabilize. However, the existing methods cannot provide an evaluation index of the current attitude determination accuracy. The user can only preset a relatively conservative alignment time, and cannot guarantee to achieve satisfactory precision. Therefore, the current optimization alignment method needs to be subjected to corresponding error analysis so as to evaluate the accuracy of the gesture solving in real time.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide an attitude error assessment method based on inertial navigation optimization alignment.
The attitude error assessment method based on inertial navigation optimization alignment provided by the invention comprises the following steps:
step 1: according to the inertial navigation kinematic equation and the attitude differential equation of the navigation system, acquiring an initial attitude constraint equation of the navigation carrier under the conditions of static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment, and calculating a measurement vector in a sliding window;
step 2: calculating a measurement matrix and a feature vector corresponding to the minimum feature value of the matrix according to static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment respectively;
step 3: and calculating a covariance matrix of the initial attitude error angle according to the vector measurement error characteristics of the static alignment, the inertial navigation/odometer alignment and the inertial navigation/GPS alignment, so as to evaluate the attitude error.
Preferably, the differential equation of the motion speed and the gesture of the navigation carrier with the inertial device in the step 1 in the navigation system is as follows:
wherein: the body coordinate system of the gyroscope/accelerometer is represented as a b-system, the inertial coordinate system is represented as an i-system, and the navigation coordinate system is represented as an n-system;
gesture matrix representing navigation coordinate system relative to body coordinate system, < >>
Indicating the angular velocity vector in the gyroscopic b-series,>
an angular velocity vector of n series relative to i series at n series is represented by v n Representing carrier velocity under navigation, f b Representing the specific force acceleration measurement in line b, < >>
Represents the earth rotation angular velocity vector in n series, ">
An angular velocity vector of n series relative to e series; g n Represents the gravity in the n series;
the operation ax represents a three-dimensional vector a= [ a ] 1 a 2 a 3 ] T The a x expansion of the constructed cross matrix is as follows:
preferably, the differential equation in step 1 is further organized into the following equation constraint relationship:
wherein,,
representing a posture matrix of the carrier at an initial moment; />
The transformation matrix from the carrier coordinate system at the time t to the carrier coordinate system at the initial time is represented; />
The transformation matrix from the navigation coordinate system at the time t to the navigation coordinate system at the initial time is represented; after time integration of both sides of the equation:
the two-sided integral of the equation is expressed as:
the rotation matrix is represented by using a gesture quaternion, and the equation relation is rewritten as follows:
wherein q nb Representing the attitude quaternion of the carrier system b relative to the navigation system n;
represents the conjugation of the quaternion, quaternion q= [ sη ] T ] T Is conjugated to q * ＝[s -η T ] T Where s represents the real part of the quaternion and η represents the imaginary part of the quaternion; an operator. Representing quaternion multiplication;
quaternion
Is defined as:
wherein I is 3 Is an identity matrix with the dimension of 3; quaternion matrix
Can also be abbreviated as->
Preferably, according to the different optimized alignment problems in step 1, the measurement vector in the static alignment case is reduced to:
the vector measurement in integrated form is further converted into discrete summation form by inertial navigation bisque calculation procedure, and the vector measurement within the ith sliding window is calculated as:
wherein,,
representing the length of a summation window, and calculating the number of intervals for the double subsamples; k represents a kth duplex-like region; beta i A value representing a vector β (t) within the ith sliding window; alpha i A value of vector alpha (t) in the ith sliding window; t is the length of the double sub-sample interval; deltav 1 ,Δv 2 ，Δθ 1 ,Δθ 2 Accelerometer velocity increment output and gyro angle increment output in k+1th double subsampled calculation interval respectively, deltaV k+1 Vectors calculated for using the above-described increments; />
Represents t k A rotation matrix of the time navigation coordinate system relative to the initial time navigation coordinate system; />
Represents t k A rotation matrix of the time carrier coordinate system relative to the initial time carrier coordinate system.
Preferably, in inertial navigation/odometer alignment applications, the measurement vector in step 1 is expressed as:
wherein v is b And
representing the odometer speed measurement and its derivative, respectively;
through the inertial navigation bisque calculation flow, the measurement vector in the ith sliding window is calculated in a discrete mode as follows:
wherein v is b (t i )、
Respectively t i 、/>
Time of dayIs a speedometer of (a) an odometer.
Preferably, in inertial navigation/GPS inter-travel alignment applications, the measurement vector in step 1 is calculated as:
through the double subsampled calculation flow, the measurement vector in the ith sliding window is further calculated as:
wherein v is n (t i ),
Respectively represent t i ,/>
GPS speed measurement information of time.
Preferably, in step 2, the calculation of the optimal initial pose requires accumulating the measurement data of the sliding window of preset time, and then calculating the following objective function by the least squares method:
wherein the method comprises the steps ofMin represents minimization in q nb An objective function for solving the variables; s.t. is an abbreviation for subject to, indicating that the optimization variable needs to satisfy constraints
N represents the accumulated sliding window measurement number; />
Representing the measurement of beta with the ith vector i Constructing a quaternion multiplication matrix; />
Representing a quaternion multiplication matrix constructed with an ith vector measurement;
measurement matrix K OBA The calculation is as follows:
wherein alpha is i ,β i Measuring a vector for the ith sliding window calculated in the step 1; tr (B) represents the trace of the matrix, summing the diagonal elements;
for static alignment, inertial navigation/odometer alignment and inertial navigation/GPS alignment, by solving the least squares problem, the optimal solution q of the attitude quaternion at the initial moment of the carrier 4 For measuring matrix K OBA Is a minimum eigenvalue lambda of (1) 4 Corresponding feature vectors.
Preferably, in step 3, the vector α is measured in a stationary alignment situation i Error delta alpha of (a) i The calculation is as follows:
wherein, (n) a ) k+1 Gyro noise for the k+1th summation interval, (n) g ) k+1 Accelerometer noise for the k+1th summation interval;
standard deviation sigma of gyro noise and accelerometer noise δt Sigma is walked randomly s The conversion relation between the two is as follows:
δt is the length of the bisque interval;
the coefficients can be output by delta v of the gyroscope and the accelerometer 1 ,Δv 2 ，Δθ 1 ,Δθ 2 Calculating to obtain;
due to the measurement of vector beta i Is of the error delta beta i Within a preset range, the covariance is directly calculated as:
wherein sigma β Is the standard deviation;
according to the measured vector error delta alpha i ,δβ i The statistical property, covariance of the initial attitude angle error θ in the static alignment case is calculated as:
the coefficient matrix Φ in the above is calculated as:
wherein, the operation symbol
Is Croneck product sign, lambda 4 For matrix K OBA Is a minimum feature value of (2);
coefficient matrix F i The calculation is as follows:
F i ＝[f 1 f 2 …f 16 ] T ∈ 16×3 ,
f 14 ＝f 8 ,f 15 ＝f 12 ,
wherein F is i Is the vector f 1 ,f 2 ,…,f 16 Matrix of components r 1 ,r 2 ,r 3 For measuring vector alpha i Alpha, alpha i ＝[r 1 ,r 2 ,r 3 ] T ；
Coefficient matrix G i The calculation is as follows:
G i ＝[g 1 g 2 …g 16 ] T ∈ 16×3 ,
g 15 ＝g 12 ,
wherein G is i Is the vector g 1 ,g 2 ,…,g 16 Matrix of components b 1 ,b 2 ,b 3 For measuring vector beta i Beta, beta i ＝[b 1 ,b 2 ,b 3 ] T 。
Preferably, in step 3, vector measurement α is performed with inertial navigation/odometer alignment during travel i Error delta alpha of (a) i The calculation is as follows:
wherein the term related to the odometer speed error is expressed as
Respectively correspond to t i ,/>
The odometer speed measurement error at the moment; terms related to accelerometer and gyro noise are expressed as
Coefficient matrix
The calculation is as follows:
the initial attitude error covariance correspondence is calculated as:
wherein matrix F i ，G i The calculation method of (1) is the same as the static alignment case; psi b For the speedometer speed error covariance, the speed error correlation is calculated by considering the speed error correlation in the sliding window measurement:
wherein R is v Representing an odometer speed measurement error covariance matrix.
Preferably, the vector α is measured in step 3 under inertial navigation/GPS inter-travel alignment i ,β i The error is calculated as:
wherein,,
respectively correspond to t i ,/>
GPS speed measurement error at moment;
the initial attitude angle error covariance is correspondingly calculated as:
wherein, the coefficient matrix phi is the same as the static alignment condition; psi n For the covariance of the GPS speed error, the correlation of the sliding window measurement speed error is considered and then is calculated as follows:
wherein matrix F i ，G i The calculation method of (1) is the same as the static alignment case; r is R v Error covariance matrix is measured for GPS velocity.
Compared with the prior art, the invention has the following beneficial effects:
(1) The covariance calculation method for optimizing alignment of the inertial navigation system can calculate the covariance of the attitude error of optimizing alignment in real time, and provides real-time precision evaluation for the optimizing alignment algorithm;
(2) The alignment error covariance calculation method provided by the invention is suitable for inertial navigation static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment, and overcomes the defect that the current optimization alignment algorithm is lack of real-time precision evaluation;
(3) The invention can be popularized and applied to the initial gesture alignment of various inertial base integrated navigation systems, such as a Doppler speed measurement assisted ground/underwater inertial integrated system and the like.
Drawings
Other features, objects and advantages of the present invention will become more apparent upon reading of the detailed description of non-limiting embodiments, given with reference to the accompanying drawings in which:
FIG. 1 is a flow chart of the method of the present invention.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the present invention, but are not intended to limit the invention in any way. It should be noted that variations and modifications could be made by those skilled in the art without departing from the inventive concept. These are all within the scope of the present invention.
Examples:
referring to fig. 1, the attitude error evaluation method based on inertial navigation optimization alignment provided by the invention comprises the following steps:
step 1: according to the inertial navigation kinematic equation and the attitude differential equation of the navigation system, acquiring an initial attitude constraint equation of the navigation carrier under the conditions of static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment, and calculating a measurement vector in a sliding window;
step 2: calculating a measurement matrix and a feature vector corresponding to the minimum feature value of the matrix according to static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment respectively;
step 3: and calculating a covariance matrix of the initial attitude error angle according to the vector measurement error characteristics of the static alignment, the inertial navigation/odometer alignment and the inertial navigation/GPS alignment, so as to evaluate the attitude error.
The differential equation of the motion speed and the gesture of the navigation carrier with the inertial device in the step 1 in the navigation system is as follows:
wherein: the body coordinate system of the gyroscope/accelerometer is represented as a b-system, the inertial coordinate system is represented as an i-system, and the navigation coordinate system is represented as an n-system;
gesture matrix representing navigation coordinate system relative to body coordinate system, < >>
Indicating the angular velocity vector in the gyroscopic b-series,>
an angular velocity vector of n series relative to b series at n series is represented by v n Representing carrier velocity under navigation, f b Representing the specific force acceleration measurement in line b, < >>
Represents the earth rotation angular velocity vector in n series, ">
An angular velocity vector of n series relative to e series; g n Represents the gravity in the n series;
the operation ax represents a three-dimensional vector a= [ a ] 1 a 2 a 3 ] T The a x expansion of the constructed cross matrix is as follows:
the differential equation in step 1 is further organized into the following equation constraint relationship:
wherein,,
representing a posture matrix of the carrier at an initial moment; />
The transformation matrix from the carrier coordinate system at the time t to the carrier coordinate system at the initial time is represented; />
The transformation matrix from the navigation coordinate system at the time t to the navigation coordinate system at the initial time is represented; after time integration of both sides of the equation:
the two-sided integral of the equation is expressed as:
the rotation matrix is represented by using a gesture quaternion, and the equation relation is rewritten as follows:
wherein q nb Representing the attitude quaternion of the carrier system b relative to the navigation system n;
represents the conjugation of the quaternion, quaternion q= [ sη ] T ] T Is conjugated to q * ＝[s -η T ] T Where s represents the real part of the quaternion and η represents the imaginary part of the quaternion; an operator. Representing quaternion multiplication;
quaternion
Is defined as:
wherein I is 3 Is an identity matrix with the dimension of 3; quaternion matrix
Can also be abbreviated as->
According to the different optimized alignment problems in step 1, the measurement vector is reduced to:
the vector measurement in integrated form is further converted into discrete summation form by inertial navigation bisque calculation procedure, and the vector measurement within the ith sliding window is calculated as:
wherein,,
representing the length of a summation window, and calculating the number of intervals for the double subsamples; k represents a kth duplex-like region; beta i A value representing a vector β (t) within the ith sliding window; alpha i A value of vector alpha (t) in the ith sliding window; t is the length of the double sub-sample interval; deltav 1 ,Δv 2 ，Δθ 1 ,Δθ 2 Accelerometer velocity increment output and gyro angle increment output in k+1th double subsampled calculation interval respectively, deltaV k+1 Vectors calculated for using the above-described increments; />
Represents t k A rotation matrix of the time navigation coordinate system relative to the initial time navigation coordinate system; />
Represents t k A rotation matrix of the time carrier coordinate system relative to the initial time carrier coordinate system.
In inertial navigation/odometer inter-travel alignment applications, the measurement vector in step 1 is expressed as:
wherein,,v b and
representing the odometer speed measurement and its derivative, respectively;
through the inertial navigation bisque calculation flow, the measurement vector in the ith sliding window is calculated in a discrete mode as follows:
wherein v is b (t i )、
Respectively t i 、/>
Odometer speed measurement at time. />
In inertial navigation/GPS inter-travel alignment applications, the measurement vector in step 1 is calculated as:
through the double subsampled calculation flow, the measurement vector in the ith sliding window is further calculated as:
wherein v is n (t i ),
Respectively represent t i ,/>
GPS speed measurement information of time.
In step 2, the preset time sliding window measurement data is accumulated to solve the optimal initial gesture, and then the following objective function is solved by a least square method:
wherein min represents minimization in q nb An objective function for solving the variables; s.t. is an abbreviation for subject to, indicating that the optimization variable needs to satisfy constraints
N represents the accumulated sliding window measurement number; />
Representing the measurement of beta with the ith vector i Constructing a quaternion multiplication matrix; />
Representing a quaternion multiplication matrix constructed with an ith vector measurement;
measurement matrix K OBA The calculation is as follows:
wherein alpha is i ,β i Measuring a vector for the ith sliding window calculated in the step 1; tr (B) represents the trace of the matrix, summing the diagonal elements;
for static alignment, inertial navigation/odometer alignment and inertial navigation/GPS alignment, by solving the least squares problem, the optimal solution q of the attitude quaternion at the initial moment of the carrier 4 For measuring matrix K OBA Is a minimum eigenvalue lambda of (1) 4 Corresponding feature vectors.
In step 3, the measurement vector α is measured in the stationary alignment situation i Error delta alpha of (a) i The calculation is as follows:
wherein, (n) a ) k+1 Gyro noise for the k+1th summation interval, (n) g ) k+1 Accelerometer noise for the k+1th summation interval;
standard deviation sigma of gyro noise and accelerometer noise δt Sigma is walked randomly s The conversion relation between the two is as follows:
δt is the length of the bisque interval;
the coefficients can be output by delta v of the gyroscope and the accelerometer 1 ,Δv 2 ，Δθ 1 ,Δθ 2 Calculating to obtain;
due to the measurement of vector beta i Is of the error delta beta i Within a preset range, the covariance is directly calculated as:
wherein sigma β Is the standard deviation;
according to the measured vector error delta alpha i ,δβ i The statistical property, covariance of the initial attitude angle error θ in the static alignment case is calculated as:
the coefficient matrix Φ in the above is calculated as:
wherein, the operation symbol
Is Croneck product sign, lambda 4 For matrix K OBA Is a minimum feature value of (2);
coefficient matrix F i The calculation is as follows:
F i ＝[f 1 f 2 …f 16 ] T ∈ 16×3 ,
f 14 ＝f 8 ,f 15 ＝f 12 ,
wherein F is i Is the vector f 1 ,f 2 ,…,f 16 Matrix of components r 1 ,r 2 ,r 3 For measuring vector alpha i Alpha, alpha i ＝[r 1 ,r 2 ,r 3 ] T ；
Coefficient matrix G i The calculation is as follows:
G i ＝[g 1 g 2 …g 16 ] T ∈ 16×3 ,
g 15 ＝g 12 ,
wherein G is i Is the vectorQuantity g 1 ,g 2 ,…,g 16 Matrix of components b 1 ,b 2 ,b 3 For measuring vector beta i Beta, beta i ＝[b 1 ,b 2 ,b 3 ] T 。
In step 3, vector measurement α under inertial navigation/odometer inter-travel alignment i Error delta alpha of (a) i The calculation is as follows:
wherein the term related to the odometer speed error is expressed as
Respectively correspond to t i ,/>
The odometer speed measurement error at the moment; terms related to accelerometer and gyro noise are expressed as
Coefficient matrix
The calculation is as follows:
the initial attitude error covariance correspondence is calculated as:
wherein matrix F i ，G i The calculation method of (1) is the same as the static alignment case; psi b For the speedometer speed error covariance, the speed error correlation is calculated by considering the speed error correlation in the sliding window measurement:
wherein R is v Representing an odometer speed measurement error covariance matrix.
Measuring vector alpha in step 3 under inertial navigation/GPS inter-travel alignment i ,β i The error is calculated as:
wherein,,
respectively correspond to t i ,/>
GPS speed measurement error at moment;
the initial attitude angle error covariance is correspondingly calculated as:
wherein, the coefficient matrix phi is the same as the static alignment condition; psi n For the covariance of the GPS speed error, the correlation of the sliding window measurement speed error is considered and then is calculated as follows:
wherein matrix F i ，G i The calculation method of (1) is the same as the static alignment case; r is R v Error covariance matrix is measured for GPS velocity.
Those skilled in the art will appreciate that the systems, apparatus, and their respective modules provided herein may be implemented entirely by logic programming of method steps such that the systems, apparatus, and their respective modules are implemented as logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers, etc., in addition to the systems, apparatus, and their respective modules being implemented as pure computer readable program code. Therefore, the system, the apparatus, and the respective modules thereof provided by the present invention may be regarded as one hardware component, and the modules included therein for implementing various programs may also be regarded as structures within the hardware component; modules for implementing various functions may also be regarded as being either software programs for implementing the methods or structures within hardware components.
The foregoing describes specific embodiments of the present invention. It is to be understood that the invention is not limited to the particular embodiments described above, and that various changes or modifications may be made by those skilled in the art within the scope of the appended claims without affecting the spirit of the invention. The embodiments of the present application and features in the embodiments may be combined with each other arbitrarily without conflict.
Claims (1)
1. An attitude error assessment method based on inertial navigation optimization alignment is characterized by comprising the following steps:
step 1: according to the inertial navigation kinematic equation and the attitude differential equation of the navigation system, acquiring an initial attitude constraint equation of the navigation carrier under the conditions of static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment, and calculating a measurement vector in a sliding window;
step 2: calculating a measurement matrix and a feature vector corresponding to the minimum feature value of the matrix according to static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment respectively;
step 3: calculating a covariance matrix of an initial attitude error angle according to vector measurement error characteristics under the conditions of static alignment, inertial navigation/odometer inter-travel alignment and inertial navigation/GPS inter-travel alignment respectively, so as to evaluate the attitude error;
the differential equation of the motion speed and the gesture of the navigation carrier with the inertial device in the step 1 in the navigation system is as follows:
wherein: the body coordinate system of the gyroscope/accelerometer is represented as a b-system, the inertial coordinate system is represented as an i-system, and the navigation coordinate system is represented as an n-system;
gesture matrix representing navigation coordinate system relative to body coordinate system, < >>
Indicating the angular velocity vector in the gyroscopic b-series,>
an angular velocity vector of n series relative to i series at n series is represented by v n Representing carrier velocity under navigation, f b Representing the specific force acceleration measurement in line b, < >>
Represents the earth rotation angular velocity vector in n series, ">
An angular velocity vector of n series relative to e series; g n Represents the gravity in the n series;
the operation ax represents a three-dimensional vector a= [ a ] 1 a 2 a 3 ] T The a x expansion of the constructed cross matrix is as follows:
the differential equation in step 1 is further organized into the following equation constraint relationship:
wherein,,
representing a posture matrix of the carrier at an initial moment; />
The transformation matrix from the carrier coordinate system at the time t to the carrier coordinate system at the initial time is represented; />
The transformation matrix from the navigation coordinate system at the time t to the navigation coordinate system at the initial time is represented; after time integration of both sides of the equation:
the two-sided integral of the equation is expressed as:
the rotation matrix is represented by using a gesture quaternion, and the equation relation is rewritten as follows:
wherein q nb Representing the attitude quaternion of the carrier system b relative to the navigation system n;
represents the conjugation of the quaternion, quaternion q= [ sη ] T ] T Is conjugated to q * ＝[s-η T ] T Where s represents the real part of the quaternion and η represents the imaginary part of the quaternion; operator->
Representing quaternion multiplication;
quaternion
Is defined as:
wherein I is 3 Is an identity matrix with the dimension of 3; quaternion matrix
Can also be abbreviated as->
According to the different optimized alignment problems in step 1, the measurement vector is reduced to:
the vector measurement in integrated form is further converted into discrete summation form by inertial navigation bisque calculation procedure, and the vector measurement within the ith sliding window is calculated as:
wherein,,
representing the length of a summation window, and calculating the number of intervals for the double subsamples; k represents a kth duplex-like region; beta i A value representing a vector β (t) within the ith sliding window; alpha i A value of vector alpha (t) in the ith sliding window; t is the length of the double sub-sample interval; deltav 1 ,Δv 2 ，Δθ 1 ,Δθ 2 Accelerometer velocity increment output and gyro angle increment output in k+1th double subsampled calculation interval respectively, deltaV k+1 Vectors calculated for using the above-described increments; />
Represents t k A rotation matrix of the time navigation coordinate system relative to the initial time navigation coordinate system; />
Represents t k A rotation matrix of the time carrier coordinate system relative to the initial time carrier coordinate system;
in inertial navigation/odometer inter-travel alignment applications, the measurement vector in step 1 is expressed as:
wherein v is b And
representing the odometer speed measurement and its derivative, respectively;
through the inertial navigation bisque calculation flow, the measurement vector in the ith sliding window is calculated in a discrete mode as follows:
wherein v is b (t i )、
Respectively t i 、/>
Measuring the speed of the speedometer at the moment;
in inertial navigation/GPS inter-travel alignment applications, the measurement vector in step 1 is calculated as:
through the double subsampled calculation flow, the measurement vector in the ith sliding window is further calculated as:
wherein v is n (t i ),
Respectively represent->
GPS speed measurement information of moment;
in step 2, the preset time sliding window measurement data is accumulated to solve the optimal initial gesture, and then the following objective function is solved by a least square method:
wherein min represents minimization in q nb An objective function for solving the variables; s.t. is an abbreviation for subject to, indicating that the optimization variable needs to satisfy constraints
N represents the accumulated sliding window measurement number; />
Representing the measurement of beta with the ith vector i Constructing a quaternion multiplication matrix; />
Representing a quaternion multiplication matrix constructed with an ith vector measurement;
measurement matrix K OBA The calculation is as follows:
wherein alpha is i ,β i Measuring a vector for the ith sliding window calculated in the step 1; tr (B) represents the trace of the matrix, summing the diagonal elements;
for static alignment, inertial navigation/odometer alignment and inertial navigation/GPS alignment, by solving the least squares problem, the optimal attitude quaternion of the carrier at the initial momentSolution q 4 For measuring matrix K OBA Is a minimum eigenvalue lambda of (1) 4 A corresponding feature vector;
in step 3, the measurement vector α is measured in the stationary alignment situation i Error delta alpha of (a) i The calculation is as follows:
wherein, (n) a ) k+1 Gyro noise for the k+1th summation interval, (n) g ) k+1 Accelerometer noise for the k+1th summation interval;
standard deviation sigma of gyro noise and accelerometer noise δt Sigma is walked randomly s The conversion relation between the two is as follows:
δt is the length of the bisque interval;
the coefficients can be output by delta v of the gyroscope and the accelerometer 1 ,Δv 2 ，Δθ 1 ,Δθ 2 Calculating to obtain;
due to the measurement of vector beta i Is of the error delta beta i Within a preset range, the covariance is directly calculated as:
wherein sigma β Is the standard deviation;
according to the measured vector error delta alpha i ,δβ i The statistical property, covariance of the initial attitude angle error θ in the static alignment case is calculated as:
the coefficient matrix Φ in the above is calculated as:
wherein, the operation symbol
Is Croneck product sign, lambda 4 For matrix K OBA Is a minimum feature value of (2);
coefficient matrix F i The calculation is as follows:
wherein F is i Is the vector f 1 ,f 2 ,…,f 16 Matrix of components r 1 ,r 2 ,r 3 For measuring vector alpha i Alpha, alpha i ＝[r 1 ,r 2 ,r 3 ] T ；
Coefficient matrix G i The calculation is as follows:
wherein G is i Is the vector g 1 ,g 2 ,…,g 16 Matrix of components b 1 ,b 2 ,b 3 For measuring vector beta i Beta, beta i ＝[b 1 ,b 2 ,b 3 ] T ；
In step 3, vector measurement α under inertial navigation/odometer inter-travel alignment i Error delta alpha of (a) i The calculation is as follows:
wherein the term related to the odometer speed error is expressed as
Respectively correspond to->
The odometer speed measurement error at the moment; terms related to accelerometer and gyro noise are expressed as
Coefficient matrix
The calculation is as follows:
the initial attitude error covariance correspondence is calculated as:
wherein matrix F i ，G i The calculation method of (1) is the same as the static alignment case; psi b For the speedometer speed error covariance, the speed error correlation is calculated by considering the speed error correlation in the sliding window measurement:
wherein R is v Representing an odometer speed measurement error covariance matrix;
measuring vector alpha in step 3 under inertial navigation/GPS inter-travel alignment i ,β i The error is calculated as:
wherein,,
respectively correspond to->
GPS speed measurement error at moment;
the initial attitude angle error covariance is correspondingly calculated as:
wherein, the coefficient matrix phi is the same as the static alignment condition; psi n For the covariance of the GPS speed error, the correlation of the sliding window measurement speed error is considered and then is calculated as follows:
wherein matrix F i ，G i The calculation method of (1) is the same as the static alignment case; r is R v Error covariance matrix is measured for GPS velocity.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111176947.4A CN113916261B (en) | 2021-10-09 | 2021-10-09 | Attitude error assessment method based on inertial navigation optimization alignment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111176947.4A CN113916261B (en) | 2021-10-09 | 2021-10-09 | Attitude error assessment method based on inertial navigation optimization alignment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113916261A CN113916261A (en) | 2022-01-11 |
| CN113916261B true CN113916261B (en) | 2023-06-27 |
Family
ID=79238721
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111176947.4A Active CN113916261B (en) | 2021-10-09 | 2021-10-09 | Attitude error assessment method based on inertial navigation optimization alignment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113916261B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115031763B (en) * | 2022-04-25 | 2023-06-13 | 北京自动化控制设备研究所 | Rapid alignment method for rotary shell based on angular rate information |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102519485A (en) * | 2011-12-08 | 2012-06-27 | 南昌大学 | Gyro information-introduced double-position strapdown inertial navigation system initial alignment method |
| CN108151737A (en) * | 2017-12-19 | 2018-06-12 | 南京航空航天大学 | A kind of unmanned plane bee colony collaborative navigation method under the conditions of the mutual observed relationships of dynamic |
| CN108225375A (en) * | 2018-01-08 | 2018-06-29 | 哈尔滨工程大学 | A kind of optimization coarse alignment method of the anti-outer speed outlier based on medium filtering |
| CN109631883A (en) * | 2018-12-17 | 2019-04-16 | 西安理工大学 | A kind of carrier aircraft local pose precise Estimation Method shared based on nodal information |
| CN110100151A (en) * | 2017-01-04 | 2019-08-06 | 高通股份有限公司 | The system and method for global positioning system speed is used in vision inertia ranging |
| CN110398257A (en) * | 2019-07-17 | 2019-11-01 | 哈尔滨工程大学 | The quick initial alignment on moving base method of SINS system of GPS auxiliary |
| CN112697141A (en) * | 2020-12-16 | 2021-04-23 | 北京航空航天大学 | Inertial navigation/odometer moving base posture and position alignment method based on reverse navigation |
-
2021
- 2021-10-09 CN CN202111176947.4A patent/CN113916261B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102519485A (en) * | 2011-12-08 | 2012-06-27 | 南昌大学 | Gyro information-introduced double-position strapdown inertial navigation system initial alignment method |
| CN110100151A (en) * | 2017-01-04 | 2019-08-06 | 高通股份有限公司 | The system and method for global positioning system speed is used in vision inertia ranging |
| CN108151737A (en) * | 2017-12-19 | 2018-06-12 | 南京航空航天大学 | A kind of unmanned plane bee colony collaborative navigation method under the conditions of the mutual observed relationships of dynamic |
| CN108225375A (en) * | 2018-01-08 | 2018-06-29 | 哈尔滨工程大学 | A kind of optimization coarse alignment method of the anti-outer speed outlier based on medium filtering |
| CN109631883A (en) * | 2018-12-17 | 2019-04-16 | 西安理工大学 | A kind of carrier aircraft local pose precise Estimation Method shared based on nodal information |
| CN110398257A (en) * | 2019-07-17 | 2019-11-01 | 哈尔滨工程大学 | The quick initial alignment on moving base method of SINS system of GPS auxiliary |
| CN112697141A (en) * | 2020-12-16 | 2021-04-23 | 北京航空航天大学 | Inertial navigation/odometer moving base posture and position alignment method based on reverse navigation |
Non-Patent Citations (2)
| Title |
|---|
| Optimization-based alignment for inertial navigation systems: Theory and algorithm;Meiping Wu,rt al;《Aerosp. Sci. Technol.》;第第15卷卷(第第1期期);第1-17页 * |
| Velocity/position integration formula part I: Application to inflight coarse alignment;Y. Wu and X. Pan;《IEEE Trans. Aerosp. Electron. Syst.》;第第49卷卷(第第2期期);第1006–1023页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113916261A (en) | 2022-01-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108731670B (en) | Inertial/visual odometer integrated navigation positioning method based on measurement model optimization | |
| Gebre-Egziabher et al. | A gyro-free quaternion-based attitude determination system suitable for implementation using low cost sensors | |
| CN100516775C (en) | Method for determining initial status of strapdown inertial navigation system | |
| Phuong et al. | A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass | |
| CN102486377B (en) | Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system | |
| CN105300379B (en) | A kind of Kalman filtering Attitude estimation method and system based on acceleration | |
| Youn et al. | Combined quaternion-based error state Kalman filtering and smooth variable structure filtering for robust attitude estimation | |
| CN109931955B (en) | Initial alignment method of strap-down inertial navigation system based on state-dependent lie group filtering | |
| Anjum et al. | Sensor data fusion using unscented kalman filter for accurate localization of mobile robots | |
| CN103822633A (en) | Low-cost attitude estimation method based on second-order measurement update | |
| CN111380516B (en) | Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information | |
| CN103364817B (en) | POS system double-strapdown calculating post-processing method based on R-T-S smoothness | |
| CN110954102A (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN114002725A (en) | Lane line auxiliary positioning method and device, electronic equipment and storage medium | |
| Amirsadri et al. | Practical considerations in precise calibration of a low-cost MEMS IMU for road-mapping applications | |
| Sabet et al. | Experimental analysis of a low-cost dead reckoning navigation system for a land vehicle using a robust AHRS | |
| CN110395297A (en) | Train locating method | |
| CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
| CN113916261B (en) | Attitude error assessment method based on inertial navigation optimization alignment | |
| CN113916226B (en) | Minimum variance-based interference rejection filtering method for integrated navigation system | |
| CN106595669B (en) | Method for resolving attitude of rotating body | |
| CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| Zhe et al. | Adaptive complementary filtering algorithm for imu based on mems | |
| CN111141283A (en) | Method for judging advancing direction through geomagnetic data | |
| CN114858166B (en) | IMU attitude resolving method based on maximum correlation entropy Kalman filter |
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 |