CN112284412B - Ground static alignment method for avoiding precision reduction caused by singular Euler transformation - Google Patents
Ground static alignment method for avoiding precision reduction caused by singular Euler transformation Download PDFInfo
- Publication number
- CN112284412B CN112284412B CN202010942368.5A CN202010942368A CN112284412B CN 112284412 B CN112284412 B CN 112284412B CN 202010942368 A CN202010942368 A CN 202010942368A CN 112284412 B CN112284412 B CN 112284412B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- northeast
- quasi
- angle
- quaternion
- 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
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
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 discloses a ground static alignment method for avoiding precision reduction caused by singular Euler transformation, which comprises the following steps: converting the inertial measurement unit gyro angular rate and the adding table specific force into a quasi-northeast coordinate system; correcting a quasi-northeast coordinate system by utilizing quaternion multiplication based on the angular increment calculated by a gyroscope; estimating a misalignment angle using least squares based on the acceleration calculated by the adder table; based on the misalignment angle and the coarse alignment attitude angle, a fine alignment angle is calculated using quaternion multiplication. According to the method, the precise alignment is carried out on the quasi-northeast earth coordinate system established based on the coarse alignment attitude angle, the precise alignment quaternion is calculated based on quaternion multiplication, the high-precision alignment angle is generated, and the problem of precision reduction caused by singular Euler transformation is solved. The ground static alignment algorithm is simple and easy for engineering application.
Description
Technical Field
The invention relates to the technical field of inertial navigation initial alignment, in particular to a ground static alignment method for avoiding accuracy reduction caused by singular Euler transformation, which is used for inertial navigation ground alignment.
Background
Initial alignment is required before launch of a carrier, missile, etc., and horizontal and azimuth alignment is generally performed based on longitude, latitude and altitude of a launch point.
The existing ground alignment generally establishes a navigation coordinate system directly based on the body, and carries out fine alignment on the coordinate system based on angular rate and specific force measured by a gyro and a summator. The conventional correction of the angular increment based on gyro calculation is as follows:
calculating the angle increment:
in the formula (I), the compound is shown in the specification,
projecting the angular velocity increment of the system relative to the geographic system on the system; delta theta x Projecting the angular velocity increment of the system relative to the geography system on the x-axis of the system; delta theta y Is a body system relative to the groundProjecting the rational angular velocity increment on the y-axis of the body; delta theta z Projecting the angular velocity increment of the system relative to the geography system on the z-axis of the system;
inertial angular velocity measured for a gyroscope;
is the angular velocity of the geographic system relative to the inertial system; a. The bn,k-1 A transformation matrix from the geographical system to the main system at the moment k-1; t is a calculation period.
Wherein Δ θ is a vector [ Δ θ ] x Δθ y Δθ z ]Die length of (2).
Constructing a matrix:
updating quaternion and conversion matrix:
in the formula, q bn,k-1 A rotation quaternion from the geographic system to the main system at the moment k-1; q. q.s bn,k Is the rotational quaternion of the geographic system to the system at time k.
Then the transformation matrix from geographic system to body system at time k is:
the precise alignment is directly carried out based on the body coordinate system, the angle increment calculated based on the gyro angular velocity does not correspond to the alignment error angle one by one, and is in a nonlinear relation, particularly when large-angle conversion exists between the rough alignment coordinate system and the north east earth coordinate system.
When the alignment attitude is defined according to the 3-2-1 rotation sequence, when the pitch angle is near 90 degrees, the precision of the conventional ground alignment method is greatly reduced.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: aiming at the defects in the prior art, the ground static alignment method for avoiding the precision reduction caused by the singularity of Euler transformation is provided, the method carries out precise alignment on the quasi-northeast ground coordinate system established based on the coarse alignment attitude angle, calculates the precise alignment quaternion the basis of quaternion multiplication, generates a high-precision alignment angle, and avoids the problem of precision reduction caused by the singularity of Euler transformation, and the ground static alignment algorithm is simple and is easy for engineering application.
The invention is realized by the following technical scheme.
A ground static alignment method for avoiding precision reduction caused by singular Euler transformation comprises the following steps:
step 1, obtaining a coarse alignment attitude angle by adopting a coarse alignment method according to angular velocity information measured by an inertial set gyroscope and specific force information measured by an adder, and converting the angular velocity of the inertial set gyroscope and the specific force of the adder into a quasi-north-east-earth coordinate system;
step 2, calculating an angle increment according to the inertial group gyroscope, and correcting a quasi-north-east-earth coordinate system by utilizing quaternion multiplication;
step 3, calculating the acceleration according to the adding table, and estimating the misalignment angle by using least square;
and 4, calculating a fine alignment quaternion by using quaternion multiplication according to the misalignment angle obtained in the step 3 and the coarse alignment attitude angle obtained in the step 1, and then calculating a fine alignment angle according to a 3-2-1 rotation sequence based on the fine alignment quaternion.
The step 1 specifically comprises the following steps:
step 1.1, establishing a northeast coordinate system based on the rough alignment attitude angle:
calculating the acceleration g of gravity
In the formula, R e Is the equatorial radius of the earth; l is the local latitude; h is the local height.
Initial attitude angle of the body relative to the northeast (3-2-1 rotation order)
If it is not
Then the
γ＝0
In the formula (I), the compound is shown in the specification,
is the specific force measured for the addition table;
angular velocity measured for an inertial gyro; gamma is roll angle, theta is pitch angle, psi is yaw angle.
Otherwise, it is
When the temperature of the water is higher than the set temperature,
according to the rough alignment attitude angle, a quasi-north-east coordinate system (quasi NED system) is established, and an n' navigation coordinate system is established in the quasi NED system.
Calculating a 3-2-1 rotation sequence attitude quaternion:
q n′b ＝q bn′ *
in the formula, q bn′ A conversion quaternion from a quasi-northeast coordinate system to a main system; q. q.s n′b The system is a conversion quaternion to a northeast coordinate system.
Calculating an attitude transformation array:
A n′b ＝A bn′ T
in the formula, A bn′ A transformation matrix from a quasi-northeast coordinate system to a main system; a. The n′b Is a transformation matrix of the system to the northeast coordinate system.
Step 1.2, converting the inertial measurement unit gyro angular velocity and the adding table specific force into a northeast coordinate system, wherein the method comprises the following steps:
converting inertial group gyro angular rate and addition specific force to quasi NED system:
f n′ ＝A n′b ·f b
ω n′ ＝A n′b ·ω b
in the formula (f) n′ Is the specific force of the northeast China series; omega n′ Is the angular velocity of the northeast China series.
Pretreatment:
in the formula (I), the compound is shown in the specification,
a filtered value of the northeast reference force at the k-th time;
is a filtered value of the northeast reference force at the time k-1;
a filtered value of the northeast-oriented angular velocity at the k-th moment;
the k-1 th time is the filtered value of the northeast angular velocity.
The step 2 specifically comprises the following steps:
step 2.1, calculating an angle increment based on a gyro in a quasi-northeast coordinate system:
calculating the component of the earth rotation angular rate in the navigation system
In the formula, ω ei Is the rotational angular velocity of the earth;
is the component of the earth rotation angular rate in the northeast coordinate system;
is the angular velocity of the northeast coordinate system.
Calculating an angle increment:
q n′n,0 ＝[1 0 0 0] T
in the formula, q n′n,0 A rotation quaternion from a north east coordinate system to a quasi-north east coordinate system at the initial moment; a. The n′n,k-1 A transformation matrix from the northeast coordinate system to the quasi-northeast coordinate system at the time k-1; t is the calculation period.
Step 2.2, correcting a coordinate system of the quasi-northeast by utilizing quaternion multiplication:
in the formula, dq ω An error quaternion from the northeast coordinate system to the quasi-northeast coordinate system; q. q.s n′n,k-1 A rotation quaternion from the northeast coordinate system to the quasi-northeast coordinate system at the time of the k-1; q. q of n′n,k Is a rotational quaternion from the northeast coordinate system to the quasi-northeast coordinate system at time k.
The step 3 specifically comprises the following steps:
step 3.1, calculating the acceleration based on the summers in the corrected quasi-north east coordinate system:
from q n′n,k ＝[q 0 q 1 q 2 q 3 ] T Calculating A n′n,k
In the formula, A n′n,k Is a transformation matrix from the northeast coordinate system to the quasi-northeast coordinate system at time k.
And (3) acceleration calculation:
in the formula (I), the compound is shown in the specification,
is the rate of change of speed of the northeast coordinate system at time k.
Step 3.2, estimating the misalignment angle by using least squares:
calculating acceleration
In the formula, a k The total acceleration of the northeast coordinate system at time k. a is a N Is the north acceleration at the k moment; a is E East acceleration at the k-th moment; a is a D The k-th time is the acceleration.
The first step of initialization:
wherein (a) N ) k-1 The north acceleration at the k-1 moment; (a) A E ) k-1 The east acceleration at the k-1 moment; (b) N ) k-1 A north acceleration filter coefficient at the k-1 moment; (b) E ) k-1 Is east acceleration filter coefficient at the k-1 time.
Second step start iterative computation
Calculating the misalignment angle
The step 4 specifically comprises the following steps:
step 4.1, based on the misalignment angle and the coarse alignment attitude angle, calculating a fine alignment quaternion by quaternion multiplication:
by
Obtaining:
in the formula, dq φ An error quaternion from the quasi-northeast coordinate system to the northeast coordinate system; q. q.s bn,k Quaternions are converted to the body system for the northeast coordinate system at time k.
And 4.2, based on the fine alignment quaternion, calculating a fine alignment angle according to a 3-2-1 rotation sequence:
from q bn,k Solving the transformation matrix A from the northeast coordinate system to the main system bn ；
Then is composed of A bn And (5) solving the three-axis attitude angle according to the 3-2-1 rotation sequence.
The attitude transformation matrix of the attitude expressed by 3-2-1 rotation order is as follows:
a is prepared from bn Expressed in matrix form:
the attitude quaternion obtains the three-axis attitude angle according to the 3-2-1 rotation sequence, if | a 13 If the | is less than or equal to 0.99999, then:
sinθ＝-a 13 ，θ＝asin(-a 13 )
otherwise, i.e. | a 13 If | is greater than 0.99999, then
γ＝0
θ＝asin(-a 13 )
The ground static alignment method is based on a quasi-northeast coordinate system established by coarse alignment to calculate an angle increment and estimate a misalignment angle, and avoids the precision reduction caused by Euler transformation singularity:
the kinematic equation for the 3-2-1 rotation attitude is as follows:
when the pitch alignment angle approaches 90 °, the attitude kinematics equation is affected by the divide-by-zero with reduced accuracy. And establishing a northeast coordinate system based on the coarse initial alignment angle obtained by the coarse alignment, wherein the angle increment and the misalignment angle relative to the northeast coordinate system are small angles and cannot be influenced by zero division.
And correcting the coordinate system of the quasi-northeast region based on quaternion multiplication, calculating a precise alignment quaternion, and then calculating a precise alignment angle according to a 3-2-1 rotation sequence based on the precise alignment quaternion.
Compared with the prior art, the ground static alignment method for avoiding the precision reduction caused by the singularity of Euler transformation provided by the invention has the following advantages and beneficial effects:
(1) The invention provides a ground static alignment method for avoiding precision reduction caused by Euler conversion singularity, which is based on a quasi-northeast coordinate system established by a coarse alignment attitude angle to carry out precise alignment, and calculates a precise alignment quaternion based on quaternion multiplication to generate a high-precision alignment angle, thereby avoiding the problem of precision reduction caused by Euler conversion singularity. The ground static alignment algorithm is simple and easy for engineering application.
(2) A quasi-north east-earth coordinate system established based on the coarse alignment attitude angle is subjected to fine alignment, so that the precision reduction caused by the singularity of Euler conversion is avoided; the angle increment calculated on the basis of a gyro in a quasi-northeast coordinate system and the alignment error angle are basically in one-to-one correspondence;
(3) And calculating a fine alignment quaternion based on quaternion multiplication to generate a high-precision alignment angle, wherein the ground static alignment algorithm is simple and is easy for engineering application.
(4) The invention discloses a ground static alignment method for avoiding precision reduction caused by singular Euler transformation. The ground static alignment algorithm is simple and easy for engineering application.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:
FIG. 1 is a schematic drawing of the northeast coordinate system (NED) of the present invention;
FIG. 2 is a ground static alignment calculation process of the present invention.
Detailed Description
The following examples illustrate the invention in detail: the embodiment is implemented on the premise of the technical scheme of the invention, and gives a detailed implementation mode and a specific operation process. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention.
As shown in fig. 1 and fig. 2, a ground static alignment method for avoiding accuracy reduction caused by singularity of euler transformation includes the following steps:
a ground static alignment method for avoiding precision reduction caused by singularity of Euler transformation comprises the following steps:
step 1, converting the inertial unit gyro angular velocity and the adding table specific force into a quasi-north east-earth coordinate system:
(1) Establishing a northeast coordinate system based on the coarse alignment attitude angle;
(2) Converting the inertial measurement unit gyro angular velocity and the adding table specific force into a northeast coordinate system;
and 2, correcting a coordinate system of the northeast by utilizing quaternion multiplication based on the angle increment calculated by the gyroscope:
(1) Calculating an angular increment based on a gyro in a quasi-northeast coordinate system;
(2) Correcting a coordinate system of the quasi-northeast by utilizing quaternion multiplication;
and 3, estimating a misalignment angle by using least square based on the acceleration calculated by the adding table:
(1) Calculating an acceleration based on the accelerometer in the corrected quasi-northeast coordinate system;
(2) Estimating a misalignment angle using least squares;
and 4, calculating a fine alignment angle by utilizing quaternion multiplication based on the misalignment angle and the coarse alignment attitude angle:
(1) Calculating a fine alignment quaternion by using quaternion multiplication based on the misalignment angle and the coarse alignment attitude angle;
(2) And based on the fine alignment quaternion, the fine alignment angle is calculated according to the 3-2-1 rotation sequence.
The step 1 specifically comprises the following steps:
step 1.1, establishing a coordinate system of the quasi-north east earth based on the coarse alignment attitude angle:
calculating the acceleration of gravity
In the formula, R e Is the equatorial radius of the earth; l is the local latitude; h is the local height.
Initial attitude angle of the computer body relative to the northeast (3-2-1 rotation order)
If it is used
γ＝0
In the formula (I), the compound is shown in the specification,
is the specific force measured for the addition table;
angular velocity measured for an inertial stack gyro; gamma is roll angle, theta is pitch angle, psi is yaw angle.
Otherwise
According to the rough alignment attitude angle, a quasi-north-east coordinate system (quasi NED system) is established, and an n' navigation coordinate system is established in the quasi NED system.
Calculating a 3-2-1 rotation sequence attitude quaternion:
q n′b ＝q bn′ *
in the formula, q bn′ A conversion quaternion from a quasi-northeast coordinate system to a main system; q. q.s n′b The system is a conversion quaternion to a northeast coordinate system.
Calculating an attitude transformation array:
A n′b ＝A bn′ T
in the formula, A bn′ A transformation matrix from a quasi-northeast coordinate system to a main system; a. The n′b Is a transformation matrix of the system to the northeast coordinate system.
Step 1.2, converting the inertial measurement unit gyro angular rate and the adding table specific force into a quasi-northeast coordinate system:
the specific force and angular rate of the system are converted into quasi NED system:
f n′ ＝A n′b ·f b
ω n′ ＝A n′b ·ω b
in the formula (f) n′ Is the specific force of the northeast China series; omega n′ Is the angular velocity of the northeast China series.
Pretreatment:
in the formula (I), the compound is shown in the specification,
a filtered value of the northeast reference force at the kth time;
is a filtered value of the northeast reference force at the time k-1;
a filtered value of the quasi-northeast system angular velocity at the kth time;
the k-1 th time is the filtered value of the northeast angular velocity.
The step 2 specifically comprises the following steps:
step 2.1, calculating an angle increment based on a gyro in a quasi-northeast coordinate system:
calculating the component of the earth rotation angular rate in the navigation system
In the formula, ω ei Is the rotational angular velocity of the earth;
is the component of the earth rotation angular rate in the northeast coordinate system;
is the angular velocity of the north east coordinate system.
Calculating the angle increment:
q n′n,0 ＝[1 0 0 0] T
in the formula, q n′n,0 A rotation quaternion from a north east coordinate system to a quasi-north east coordinate system at the initial moment; a. The n′n,k-1 A transformation matrix from the northeast coordinate system to the quasi-northeast coordinate system at the time k-1; t is the calculation period.
Step 2.2, correcting a quasi-northeast coordinate system by quaternion multiplication:
in the formula, dq ω An error quaternion from the north east coordinate system to the quasi-north east coordinate system; q. q.s n′n,k-1 A rotation quaternion from the northeast coordinate system to the quasi-northeast coordinate system at the time of the k-1; q. q.s n′n,k Is a rotational quaternion from the northeast coordinate system to the quasi-northeast coordinate system at time k.
The step 3 specifically comprises the following steps:
step 3.1, calculating the acceleration based on a summeter in the corrected quasi-northeast coordinate system:
from q n′n,k ＝[q 0 q 1 q 2 q 3 ] T Calculating A n′n,k
In the formula, A n′n,k Is a transformation matrix from the northeast coordinate system to the quasi-northeast coordinate system at time k.
Step 3.2, estimating the misalignment angle by using least squares:
calculating acceleration
In the formula, a k The total acceleration of the northeast coordinate system at time k. a is N Is the north acceleration at the k moment; a is E East acceleration at the k-th moment; a is D The k-th time is the acceleration.
The first step of initialization:
wherein (a) N ) k-1 The north acceleration at the k-1 moment; (a) A E ) k-1 The east acceleration at the k-1 moment; (b) N ) k-1 A north acceleration filter coefficient at the k-1 moment; (b) E ) k-1 And the east acceleration filter coefficient is the k-1 time.
Second step starting iterative computation
Calculating misalignment angle
The step 4 specifically comprises the following steps:
step 4.1, based on the misalignment angle and the coarse alignment attitude angle, calculating a fine alignment quaternion by using quaternion multiplication:
by
Obtaining:
in the formula, dq φ An error quaternion from the quasi-northeast coordinate system to the northeast coordinate system; q. q.s bn,k Quaternions are converted to the body system for the northeast coordinate system at time k.
And 4.2, based on the fine alignment quaternion, calculating a fine alignment angle according to a 3-2-1 rotation sequence:
from q bn,k Solving the transformation matrix A from the northeast coordinate system to the main system bn ；
Then from A bn And (5) solving the three-axis attitude angle according to the 3-2-1 rotation sequence.
The attitude transformation matrix of the attitude expressed by 3-2-1 rotation sequence is as follows:
a is to be bn Expressed in matrix form:
the attitude quaternion obtains the three-axis attitude angle according to the 3-2-1 rotation sequence, if | a 13 If the | is less than or equal to 0.99999, then:
sinθ＝-a 13 ，θ＝asin(-a 13 )
otherwise, i.e. | a 13 If | is greater than 0.99999, then
γ＝0
θ＝asin(-a 13 )
The method comprises the following specific steps:
a. converting inertial group gyro angular rate and adding specific force to northeast coordinate system
Establishing a northeast coordinate system based on the coarse alignment attitude angle:
calculating the acceleration g of gravity
In the formula, R e Is the equatorial radius of the earth; l is the local latitude; h is the local height.
Calculate initial attitude angle of body relative to north east (3-2-1 rotation order)
If it is not
Then
γ＝0
In the formula (I), the compound is shown in the specification,
is the specific force measured for the addition table;
angular velocity measured for an inertial gyro; gamma is roll angle, theta is pitch angle, psi is yaw angle.
Otherwise, it is
When the temperature of the water is higher than the set temperature,
according to the rough alignment attitude angle, a quasi-north-east coordinate system (quasi NED system) is established, and an n' navigation coordinate system is established in the quasi NED system.
Calculating a 3-2-1 rotation sequence attitude quaternion:
q n′b ＝q bn′ *
in the formula, q bn′ Is a conversion quaternion from a quasi-northeast coordinate system to a main system; q. q.s n′b The system is a conversion quaternion to a northeast coordinate system.
Calculating an attitude transformation array:
A n′b ＝A bn′ T
in the formula, A bn′ A transformation matrix from a quasi-northeast coordinate system to a main system; a. The n′b Is a transformation matrix of the system to the northeast coordinate system.
The specific force and angular rate of the system are converted to quasi NED system:
f n′ ＝A n′b ·f b
ω n′ ＝A n′b ·ω b
in the formula (f) n′ Is the specific force of the northeast China series; omega n′ Is the angular velocity of the northeast China series.
Pretreatment:
in the formula (I), the compound is shown in the specification,
a filtered value of the northeast reference force at the kth time;
a filtered value of the northeast reference force at time k-1;
corner of northeast east China at the k-th momentA filtered value of the velocity;
the k-1 th time is the filtered value of the northeast angular velocity.
b. Correcting the quasi-northeast coordinate system by quaternion multiplication based on angular increments calculated by a gyroscope
Angular increments are calculated based on a gyro in the northeast coordinate system:
calculating the component of the earth rotation angular rate in the navigation system
In the formula, ω ei The rotational angular velocity of the earth;
is the component of the earth rotation angular rate in the northeast coordinate system;
is the angular velocity of the north east coordinate system.
Calculating an angle increment:
q n′n,0 ＝[1 0 0 0] T
in the formula, q n′n,0 A rotation quaternion from a north east coordinate system to a quasi-north east coordinate system at an initial moment; a. The n′n,k-1 A transformation matrix from the north east coordinate system to the quasi-north east coordinate system at the time k-1; t is the calculation period.
In the formula, dq ω An error quaternion from the north east coordinate system to the quasi-north east coordinate system; q. q.s n′n,k-1 A rotation quaternion from the northeast coordinate system to the quasi-northeast coordinate system at the time of the k-1; q. q.s n′n,k Is a rotational quaternion from the northeast coordinate system to the quasi-northeast coordinate system at time k.
c. Estimating misalignment angle using least squares based on accelerometer calculations
The acceleration is calculated based on the accelerometer in the corrected northeast coordinate system:
from q n′n,k ＝[q 0 q 1 q 2 q 3 ] T Calculating A n′n,k
In the formula, A n′n,k Is a transformation matrix from the northeast coordinate system to the quasi-northeast coordinate system at time k.
Calculating acceleration
In the formula, a k The total acceleration of the north east coordinate system at the time k. a is N Is the north acceleration at the k moment; a is E East acceleration at the k-th moment; a is D The k-th time is the acceleration.
The first step of initialization:
wherein (a) N ) k-1 Is the kth-1, north acceleration at the moment; (a) A E ) k-1 The east acceleration at the k-1 moment; (b) N ) k-1 A north acceleration filter coefficient at the k-1 moment; (b) E ) k-1 Is east acceleration filter coefficient at the k-1 time.
Second step start iterative computation
Calculating the misalignment angle
d. Calculating a fine alignment angle using quaternion multiplication based on the misalignment angle and the coarse alignment attitude angle
Based on the misalignment angle and the coarse alignment attitude angle, calculating a fine alignment quaternion by quaternion multiplication:
step 4.1, based on the misalignment angle and the coarse alignment attitude angle, calculating a fine alignment quaternion by using quaternion multiplication:
by
Solving the following steps:
in the formula, dq φ An error quaternion from the quasi-northeast coordinate system to the northeast coordinate system; q. q.s bn,k Quaternions are converted for the northeast coordinate system to the body system at time k.
From q bn,k Solving the transformation matrix A from the northeast coordinate system to the main system bn ；
Then from A bn And (5) solving the three-axis attitude angle according to the 3-2-1 rotation sequence.
The attitude transformation matrix of the attitude expressed by 3-2-1 rotation sequence is as follows:
a is to be bn Expressed in matrix form:
the attitude quaternion obtains the three-axis attitude angle according to the 3-2-1 rotation sequence, if | a 13 If the | is less than or equal to 0.99999, then:
sinθ＝-a 13 ，θ＝asin(-a 13 )
otherwise, i.e. | a 13 If | is greater than 0.99999, then
γ＝0
θ＝asin(-a 13 )
The ground static alignment method for avoiding precision reduction caused by euler transformation singularity provided by the embodiment provides a ground static alignment method for avoiding precision reduction caused by euler transformation singularity, the method converts inertial group gyro angular rate and addition table specific force into a quasi-northeast earth coordinate system established based on coarse alignment, corrects the quasi-northeast earth coordinate system by quaternion multiplication based on angular increment calculated by a gyro, estimates a misalignment angle based on acceleration calculated by an addition table by least square, and calculates a fine alignment angle based on the misalignment angle and a coarse alignment attitude angle by quaternion multiplication; and a quasi-northeast coordinate system established based on the coarse alignment attitude angle is subjected to fine alignment, so that the precision reduction caused by the singularity of Euler conversion is avoided.
Assuming that the attitude angle of the aircraft body system relative to the northeast coordinate system is [0;89;20]The measurement precision of the inertial group gyro is 0.36 degree/h, and the measurement precision of the inertial group and the table is 0.0001m/s 2 . Obtaining an attitude angle of [0 ] by adopting coarse alignment; 90, respectively; 19.86](ii) there is a measurement error of 1 °; establishing a coordinate system n' of the northeast according to the rough alignment attitude angle, and further obtaining a transformation matrix A from the coordinate system to the coordinate system of the northeast n′b ：
Converting the inertial group gyroscope angular rate and the addition table specific force to a quasi-north-east coordinate system, correcting the quasi-north-east coordinate system by utilizing quaternion multiplication, and calculating a misalignment angle by utilizing a least square method to calculate a precise alignment attitude angle to be [0.03;89;20.14 deg., and an alignment accuracy of 0.14 deg..
In the embodiment, the precise alignment is carried out on the quasi-northeast coordinate system established based on the coarse alignment attitude angle, the precise alignment quaternion is calculated based on quaternion multiplication, the high-precision alignment angle is generated, and the problem of precision reduction caused by singular Euler transformation is solved. The ground static alignment algorithm is simple and easy for engineering application.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes and modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention.
Claims (2)
1. A ground static alignment method for avoiding precision reduction caused by singular Euler transformation is characterized by comprising the following steps:
step 1, obtaining a coarse alignment attitude angle by a coarse alignment method according to angular velocity information measured by an inertial set gyroscope and specific force information measured by an adding table, and converting the inertial set gyroscope angular velocity and the adding table specific force into a northeast coordinate system;
step 2, calculating angle increment according to the inertial measurement unit gyroscope, and correcting a coordinate system of the quasi-northeast by utilizing quaternion multiplication;
step 3, calculating the acceleration according to the adding table, and estimating a misalignment angle by using least square;
step 4, calculating a fine alignment quaternion by using quaternion multiplication according to the misalignment angle obtained in the step 3 and the coarse alignment attitude angle obtained in the step 1, and then calculating a fine alignment angle according to a 3-2-1 rotation sequence based on the fine alignment quaternion;
in step 1, the method for establishing the coordinate system of the northeast is as follows:
calculating the acceleration g of gravity
In the formula, R e Is the equatorial radius of the earth; l is the local latitude; h is the local height;
calculating the initial attitude angle of the body relative to the northeast, and performing 3-2-1 rotation:
if it is not
Then
γ＝0
In the formula (I), the compound is shown in the specification,
is the specific force measured for the add table;
angular velocity measured for an inertial gyro; gamma is a roll angle, theta is a pitch angle, psi is a yaw angle;
otherwise, it is
When the utility model is used, the water is discharged,
establishing a quasi-northeast-earth coordinate system and a quasi-NED system according to the rough alignment attitude angle, and establishing an n' navigation coordinate system in the quasi-NED system;
calculating a 3-2-1 rotation sequence attitude quaternion:
q n′b ＝q bn′ *
in the formula, q bn′ Is a conversion quaternion from a quasi-northeast coordinate system to a main system; q. q of n′b A conversion quaternion of the body system to a quasi-northeast coordinate system;
calculating an attitude transformation array:
A n′b ＝A bn′ T
in the formula, A bn′ A transformation matrix from a quasi-northeast coordinate system to a main system; a. The n′b A transformation matrix from the body system to a quasi-northeast coordinate system;
in the step 1, the inertial set gyro angular velocity and the adding table specific force are converted into a quasi north east earth coordinate system, and the method comprises the following steps:
converting inertial group gyro angular rate and addition specific force to quasi NED system:
f n′ ＝A n′b ·f b
ω n′ ＝A n′b ·ω b
in the formula, f n′ Is the specific force of the northeast China series; omega n′ Is the angular velocity of the northeast China series;
pretreatment:
in the formula (I), the compound is shown in the specification,
a filtered value of the northeast reference force at the k-th time;
a filtered value of the northeast reference force at time k-1;
a filtered value of the northeast-oriented angular velocity at the k-th moment;
the filtered value of the angular velocity of the northeast east China system at the k-1 st moment;
in the step 2, the method for calculating the angle increment according to the inertial measurement unit gyroscope comprises the following steps:
calculating the component of the earth rotation angular rate in the navigation system
In the formula, ω ei The rotational angular velocity of the earth;
is the component of the earth rotation angular rate in the northeast coordinate system;
angular velocity in the northeast coordinate system;
calculating an angle increment:
q n′n,0 ＝[1 0 0 0] T
in the formula, q n′n,0 A rotation quaternion from a north east coordinate system to a quasi-north east coordinate system at an initial moment; a. The n′n,k-1 A transformation matrix from the northeast coordinate system to the quasi-northeast coordinate system at the time k-1; t is a calculation period;
in the step 2, the method for correcting the quasi-northeast coordinate system by using quaternion multiplication comprises the following steps:
in the formula, dq ω An error quaternion from the northeast coordinate system to the quasi-northeast coordinate system; q. q.s n′n,k-1 A rotation quaternion from the northeast coordinate system to the quasi-northeast coordinate system at the time of the k-1; q. q of n′n,k A rotation quaternion from the northeast coordinate system to the quasi-northeast coordinate system at the kth moment;
in the step 3, the method for calculating the acceleration according to the adding table comprises the following steps:
from q n′n,k ＝[q 0 q 1 q 2 q 3 ] T Calculating A n′n,k
In the formula, A n′n,k A transformation matrix from a north east coordinate system to a quasi north east coordinate system at the kth moment;
and (3) acceleration calculation:
in the formula (I), the compound is shown in the specification,
the change rate of the speed of the north east earth coordinate system at the kth moment;
in the step 3, the method for estimating the misalignment angle by using least square comprises the following steps:
calculating acceleration
In the formula, a k The total acceleration of the north east coordinate system at the kth moment; a is N Is the north acceleration at the k moment; a is E East acceleration at the kth moment; a is a D Acceleration at the kth time;
the first step of initialization:
wherein (a) N ) k-1 The north acceleration at the k-1 moment; (a) E ) k-1 The east acceleration at the k-1 moment; (b) N ) k-1 The north acceleration filter coefficient is the k-1 moment; (b) E ) k-1 Is east acceleration filter coefficient at the k-1 moment;
second step starting iterative computation
Calculating misalignment angle
In step 4, based on the misalignment angle and the coarse alignment attitude angle, the method for calculating the fine alignment quaternion by using quaternion multiplication comprises the following steps:
by
Obtaining:
in the formula, dq φ An error quaternion from the quasi-northeast coordinate system to the northeast coordinate system; q. q.s bn,k Quaternions are converted for the northeast coordinate system to the body system at time k.
2. The ground static alignment method for avoiding the precision reduction caused by the singularity of the euler transform as claimed in claim 1, wherein:
in the step 4, based on the fine alignment quaternion, the method for calculating the fine alignment angle according to the 3-2-1 rotation sequence is as follows:
from q bn,k Solving the transformation matrix A from the northeast coordinate system to the main system bn ；
Then from A bn Obtaining a three-axis attitude angle according to a 3-2-1 rotation sequence;
the attitude transformation matrix of the attitude expressed by 3-2-1 rotation order is as follows:
a is prepared from bn Expressed in matrix form:
the attitude quaternion obtains the three-axis attitude angle according to the 3-2-1 rotation sequence, if | a 13 If the | is less than or equal to 0.99999, then:
sinθ＝-a 13 ，θ＝asin(-a 13 )
otherwise, i.e. | a 13 If | is greater than 0.99999, then
γ＝0
θ＝a sin(-a 13 )
The ground static alignment method is based on a quasi-north-east coordinate system established by coarse alignment to calculate the angle increment and estimate the misalignment angle, so that the precision reduction caused by the singularity of Euler transformation is avoided:
the kinematic equation for the 3-2-1 rotation attitude is as follows:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010942368.5A CN112284412B (en) | 2020-09-09 | 2020-09-09 | Ground static alignment method for avoiding precision reduction caused by singular Euler transformation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010942368.5A CN112284412B (en) | 2020-09-09 | 2020-09-09 | Ground static alignment method for avoiding precision reduction caused by singular Euler transformation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112284412A CN112284412A (en) | 2021-01-29 |
| CN112284412B true CN112284412B (en) | 2022-11-11 |
Family
ID=74419833
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010942368.5A Active CN112284412B (en) | 2020-09-09 | 2020-09-09 | Ground static alignment method for avoiding precision reduction caused by singular Euler transformation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112284412B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113720206B (en) * | 2021-09-02 | 2023-04-11 | 重庆零壹空间科技集团有限公司 | Rocket ground aiming method, system, computer equipment and storage medium |
| CN113686333B (en) * | 2021-09-15 | 2023-06-20 | 中国船舶重工集团公司第七0七研究所 | Strapdown inertial navigation full-attitude representation method for radar array surface rotation |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110779550A (en) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | Large azimuth misalignment angle two-stage linear alignment method based on additive quaternion |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105043415B (en) * | 2015-07-13 | 2018-01-05 | 北京工业大学 | Inertial system Alignment Method based on quaternion model |
| CN108827288A (en) * | 2018-04-12 | 2018-11-16 | 东北电力大学 | A kind of dimensionality reduction strapdown inertial navigation system Initial Alignment Method and system based on dual quaterion |
| CN109682397B (en) * | 2018-12-18 | 2021-01-29 | 上海航天控制技术研究所 | Ground static alignment method capable of fast convergence without being influenced by historical data |
| CN109974697B (en) * | 2019-03-21 | 2022-07-26 | 中国船舶重工集团公司第七0七研究所 | High-precision mapping method based on inertial system |
| CN110779551A (en) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | Two-stage linear alignment on-line switching method based on additive quaternion |
| CN111256731A (en) * | 2020-02-28 | 2020-06-09 | 上海航天控制技术研究所 | Ground static alignment method capable of fast convergence without being influenced by historical data |
-
2020
- 2020-09-09 CN CN202010942368.5A patent/CN112284412B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110779550A (en) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | Large azimuth misalignment angle two-stage linear alignment method based on additive quaternion |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112284412A (en) | 2021-01-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109596018B (en) | High-precision estimation method for flight attitude of spinning projectile based on magnetic roll angle rate information | |
| CN104165640B (en) | Near-space missile-borne strap-down inertial navigation system transfer alignment method based on star sensor | |
| CN110926468B (en) | Communication-in-motion antenna multi-platform navigation attitude determination method based on transfer alignment | |
| CN113551668B (en) | Spacecraft inertia/star starlight vector/starlight refraction combined navigation method | |
| CN112284412B (en) | Ground static alignment method for avoiding precision reduction caused by singular Euler transformation | |
| CN110133692B (en) | Inertial navigation technology-assisted high-precision GNSS dynamic inclination measurement system and method | |
| CN113503894B (en) | Inertial navigation system error calibration method based on gyro reference coordinate system | |
| CN110672128B (en) | Starlight/inertia combined navigation and error online calibration method | |
| CN112880669B (en) | Spacecraft starlight refraction and single-axis rotation modulation inertial integrated navigation method | |
| CN111722295B (en) | Underwater strapdown gravity measurement data processing method | |
| CN103604428A (en) | Star sensor positioning method based on high-precision horizon reference | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| CN102168978A (en) | Marine inertial navigation system swing pedestal open loop aligning method | |
| CN104501809B (en) | Attitude coupling-based strapdown inertial navigation/star sensor integrated navigation method | |
| CN105606093A (en) | Inertial navigation method and device based on real-time gravity compensation | |
| CN116105730A (en) | Angle measurement-only optical combination navigation method based on cooperative target satellite very short arc observation | |
| CN108981691B (en) | Sky polarized light combined navigation on-line filtering and smoothing method | |
| CN112325841B (en) | Method for estimating installation error angle of communication-in-motion antenna | |
| CN111207734B (en) | EKF-based unmanned aerial vehicle integrated navigation method | |
| CN108416387A (en) | Height filtering method based on GPS Yu barometer fused data | |
| CN109682397B (en) | Ground static alignment method capable of fast convergence without being influenced by historical data | |
| CN113108788B (en) | Long-endurance inertial navigation/astronomical global integrated navigation method | |
| CN110733671B (en) | Dynamic correction method for small celestial body spin angular velocity | |
| CN111256731A (en) | Ground static alignment method capable of fast convergence without being influenced by historical data | |
| CN110779550A (en) | Large azimuth misalignment angle two-stage linear alignment method based on additive quaternion |
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 |