CN112729332B - Alignment method based on rotation modulation - Google Patents

Alignment method based on rotation modulation Download PDF

Info

Publication number
CN112729332B
CN112729332B CN202011283857.0A CN202011283857A CN112729332B CN 112729332 B CN112729332 B CN 112729332B CN 202011283857 A CN202011283857 A CN 202011283857A CN 112729332 B CN112729332 B CN 112729332B
Authority
CN
China
Prior art keywords
navigation
alignment
imu
time
rotation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011283857.0A
Other languages
Chinese (zh)
Other versions
CN112729332A (en
Inventor
侯志宁
蒋茂荣
项铮
刘康
沈扬清
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
707th Research Institute of CSIC
Original Assignee
707th Research Institute of CSIC
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 707th Research Institute of CSIC filed Critical 707th Research Institute of CSIC
Priority to CN202011283857.0A priority Critical patent/CN112729332B/en
Publication of CN112729332A publication Critical patent/CN112729332A/en
Application granted granted Critical
Publication of CN112729332B publication Critical patent/CN112729332B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • 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 relates to a rotary modulation alignment method, which is technically characterized in that: the rotation modulation is carried out in the coarse alignment stage, the constant error of the equivalent horizontal accelerometer and the constant error of the equivalent east gyroscope are modulated, the coarse alignment precision is improved, meanwhile, when the reverse navigation is carried out to the coarse alignment stage in the fine alignment stage, the integrity of the rotation modulation period in the fine alignment stage is guaranteed, the precision of precision accuracy is improved, and the state vector estimation of the misalignment angle obtained by forward navigation alignment and reverse navigation alignment is carried out smooth filtering by utilizing information fusion filtering to obtain the optimal estimation of the misalignment angle, so that the precision of the inertial navigation system is further improved.

Description

Alignment method based on rotation modulation
Technical Field
The invention belongs to the technical field of inertial navigation, and particularly relates to an alignment method based on rotation modulation.
Background
With the increasing requirements of the vehicle-mounted weapon systems on rapidity and maneuverability, the vehicle-mounted positioning and orientation system must have the capability of rapidly entering into a working state. The rapidity and the accuracy of the initial alignment are key indexes for the vehicle-mounted positioning and orientation system to rapidly enter a working state. However, the rapidity and the accuracy of the initial alignment are often contradictory, so how to realize the rapid high-accuracy initial alignment of the vehicle-mounted positioning and orientation system has important significance for improving the rapidity and the maneuverability of the vehicle-mounted weapon system.
The initial alignment of the rotary vehicle-mounted positioning and orientation system is mainly divided into two stages of coarse alignment and fine alignment. In the coarse alignment stage, coarse alignment based on an inertial coordinate system is usually adopted, in order to reduce the influence of linear motion interference, an Inertial Measurement Unit (IMU) is in a static state, and the approximate values of the initial horizontal attitude and the course of the vehicle-mounted positioning and orientation system are determined through the measurement output of an inertial element by taking the gravity acceleration and the earth rotation angular rate as reference quantities; and in the fine alignment stage, assuming that a misalignment angle between the coarse alignment attitude and the real attitude is a small error, estimating the misalignment angle through a misalignment angle error model by using a navigation speed error as an observed quantity, so as to obtain a high-precision initial attitude matrix.
Based on the coarse alignment of the inertial coordinate system, the alignment accuracy of the horizontal misalignment angle mainly depends on the equivalent horizontal measurement error of the accelerometer, and the alignment accuracy of the azimuth misalignment angle mainly depends on the equivalent east measurement error of the gyroscope. In the static state of the IMU, equivalent errors of an accelerometer and a gyroscope cannot be eliminated, and the precision of coarse alignment is limited by the constant error of an inertial element.
When reverse navigation is carried out in the fine alignment stage, because the IMU is in a static state during coarse alignment, the rotation modulation period of fine alignment is damaged, and the equivalent errors of the accelerometer and the gyroscope cannot be effectively subjected to rotation modulation, so that the alignment accuracy of fine alignment is further influenced.
The existing alignment method does not carry out smooth filtering on the state vector estimation of the misalignment angle obtained by forward navigation alignment and reverse navigation alignment to obtain the optimal estimation of the misalignment angle.
In summary, the following problems exist in the prior art: 1. equivalent errors of an accelerometer and a gyroscope in a coarse alignment stage cannot be effectively subjected to rotation modulation; 2. the rotation modulation period in the fine alignment stage is damaged, and the complete rotation modulation period in the fine alignment process cannot be ensured; 3. the forward navigation alignment and the reverse navigation alignment can respectively estimate the same state vector, and the prior alignment method does not combine the complementation and the redundant information of the forward navigation alignment and the reverse navigation alignment in time and space according to a certain optimization criterion, and the full-local optimal estimation of the state is obtained after the fusion.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides an alignment method based on rotation modulation, wherein rotation modulation is carried out in a coarse alignment stage, a constant error of an equivalent horizontal accelerometer and a constant error of an equivalent east gyroscope are modulated, the coarse alignment precision is improved, meanwhile, when a reverse navigation is carried out in a fine alignment stage to a coarse alignment stage, the integrity of a rotation modulation period in the fine alignment stage is ensured, the precision of the precision is improved, and the optimal estimation of a misalignment angle is obtained by carrying out smooth filtering on state vector estimation of the misalignment angle obtained by forward navigation alignment and reverse navigation alignment by utilizing information fusion filtering, so that the precision of an inertial navigation system is further improved.
The invention solves the practical problem by adopting the following technical scheme:
an alignment method based on rotation modulation, comprising the steps of:
step 1, after a system receives a starting command, carrying out rotation modulation around an azimuth axis based on a rotation modulation strategy;
step 2, in the rotation modulation process of the step 1, the system adopts an indirect coarse alignment scheme based on an inertial system to perform coarse rotation modulation alignment;
step 3, after the coarse alignment of the rotation modulation is finished, the system carries out the accurate alignment of the rotation modulation forward navigation;
step 4, after finishing the rotary modulation forward navigation fine alignment, setting the initial value posture and position of a reverse navigation algorithm as the posture and position of the forward navigation finish time, and changing the model parameters into a small error model parameter system to carry out rotary modulation reverse navigation fine alignment;
step 5, in the starting time t from the backward navigation Kalman filtering to the coarse alignment 0 Adjusting the error model parameters again, entering the rotation modulation precision alignment by an alignment program, sampling a gyroscope in a forward algorithm, adopting symbols calculated by a forward direction for gyroscope constant drift and earth rotation angular rate symbols, setting an initial value gesture and position of a reverse navigation algorithm as the gesture and position of the forward navigation ending moment, and setting a speed symbol at the reverse navigation ending momentTaking the inverse as the speed of the initial time of forward navigation, starting forward calculation and Kalman filtering on the sample data from the initial time of coarse alignment, carrying out rotary modulation precise alignment, and estimating the misalignment angle until the initial time t of backward navigation re (ii) a Performing fixed point smoothing filtering in the reverse navigation process, and performing online smoothing on the filtered state vector by fully utilizing all observation values in the measurement period to realize the optimal estimation of the state variable;
step 6, the operation times of the step 4 and the step 5 are repeatedly executed, the navigation data and the external reference information in the alignment time are repeatedly utilized, and the repeated calculation times can be determined according to the calculation capability and the alignment time length of the navigation computer;
step 7, after the rotational modulation forward navigation fine alignment of the step 6 is completed, performing information fusion by using misalignment angle state quantity and state estimation mean square error obtained by forward and reverse navigation calculation, and finally obtaining global optimal estimation of the misalignment angle state, thereby improving the alignment precision of the system;
step 8, after the information fusion after the step 7 is completed, from t re The forward navigation fine alignment is carried out from the moment until the alignment end time t e The misalignment angle and velocity information are partially feedback corrected during alignment.
Further, the specific steps of step 1 include:
(1) Alignment start time t 0 (setting t) 0 = 0) the IMU is in a static state relative to the carrier, the IMU is at an electric zero position of the indexing mechanism, at the moment, an IMU coordinate system (p system) is coincident with an inertial navigation carrier coordinate system (b system), and t s At the moment IMU (p series) starts
Figure BDA0002781689880000031
Is rotated by 180 DEG about the azimuth axis relative to the carrier (system b), wherein t r The rotation time of the turntable is long, and the IMU is positioned at an electrical 180-degree position of the indexing mechanism at the moment;
(2) The IMU is then stationary relative to the carrier for a period of time t s IMU (p is) around the azimuth axis
Figure BDA0002781689880000041
Is rotated 180 deg. with respect to the carrier (system b), when the IMU is in the electrical zero position of the indexing mechanism, and then the IMU is stationary with respect to the inertial frame for a period t s ；
(3) Then, IMU (p series) is wound around the azimuth axis
Figure BDA0002781689880000042
Is rotated 180 deg. relative to the carrier (system b), the IMU is now located at the electrical-180 deg. position of the indexing mechanism, the IMU is stationary relative to the inertial frame, the stationary duration is t s ；
(4) Then IMU to
Figure BDA0002781689880000043
Is rotated by 180 DEG about the azimuth axis relative to the inertial navigation coordinate system, the IMU is then located in the electrical zero position of the indexing mechanism, and subsequently the IMU is stationary relative to the inertial navigation coordinate system for a period of time t s To this end, the IMU completes a rotational modulation period T r Rotation of (2);
wherein:
b, defining a carrier coordinate system, defining the right front upper part as positive, and assuming that inertial navigation is superposed with the carrier system;
a p system-IMU coordinate system, wherein the p system of the indexing mechanism in the electric zero position is a b system of a right front upper coordinate system of the inertial navigation carrier;
p 0 system-initial moment IMU inertial system, coinciding with the IMU coordinate system at the initial alignment start moment (p-system), then without rotation with respect to the inertial space;
n is a navigation coordinate system, namely a northeast geographic coordinate system;
n 0 system-initial moment navigation inertial system, coinciding with the navigation coordinate system (n system) at the instant of initial alignment start, then without rotation with respect to inertial space;
Figure BDA0002781689880000044
output of angle measuring devicesRelative to the carrier coordinate system (system b) of the IMU coordinate system (system p)
Figure BDA0002781689880000051
Moreover, the step 2 inertial system is indirectly roughly aligned
Figure BDA0002781689880000052
The key point of indirect coarse alignment of the inertial system is to find p 0 Is related to n 0 Conversion of systems, i.e. constant value matrices
Figure BDA0002781689880000053
The method comprises the following specific steps:
(1)
Figure BDA0002781689880000054
is solved for
Figure BDA0002781689880000055
Due to the fact that
Figure BDA0002781689880000056
Is constant, i.e. n is relative to n 0 The rotation is fixed-axis rotation, so that,
Figure BDA0002781689880000057
wherein L is latitude, omega ie Is the angular velocity of the earth's rotation,
Figure BDA0002781689880000058
is the component of the earth in the geographic system of rotational angular velocity,
Figure BDA0002781689880000059
(2)
Figure BDA00027816898800000510
is solved for
Figure BDA00027816898800000511
Figure BDA00027816898800000512
The initial value of attitude quaternion is the measured value of the gyroscope
Figure BDA00027816898800000513
Real-time attitude matrix obtained by attitude updating algorithm
Figure BDA00027816898800000514
Here need not be to
Figure BDA00027816898800000515
The size of the coarse alignment algorithm is limited, so that the inertial system indirect coarse alignment algorithm has strong capability of resisting angular motion interference.
Angular rate by gyroscope
Figure BDA00027816898800000516
The integration yielding an equivalent rotation vector, i.e.
Figure BDA00027816898800000517
Wherein, for the gyroscope output by the angular increment, phi is the angular increment information output by the gyroscope in real time;
rotation angle phi = | phi |;
the direction of rotation u = φ/φ;
converting the equivalent rotation vector into a quaternion:
Figure BDA0002781689880000061
where m represents the current time and m-1 represents the previous time.
Quaternion at the current moment can be obtained
Figure BDA0002781689880000062
Converting quaternion of current time into quaternion of current time
Figure BDA0002781689880000063
Figure BDA0002781689880000064
(3)
Figure BDA0002781689880000065
Is solved for
Gravity acceleration vector is in n 0 Is projected as
Figure BDA0002781689880000066
Wherein, g n ＝[00-g] T For the component of the gravity acceleration vector under the navigation system, we can obtain:
Figure BDA0002781689880000067
specific force of accelerometer
Figure BDA0002781689880000068
At p is 0 The projection is as follows:
Figure BDA0002781689880000069
by passing
Figure BDA00027816898800000610
A relationship between gravitational acceleration and accelerometer specific force may be established. General formula
Figure BDA00027816898800000611
Are simultaneously left-multiplied on two sides
Figure BDA00027816898800000612
To obtain
Figure BDA00027816898800000613
Namely, it is
Figure BDA00027816898800000614
Wherein the content of the first and second substances,
Figure BDA00027816898800000615
in order to be equivalent to the accelerometer measurement error,
Figure BDA00027816898800000616
is the specific force output of the accelerometer,
Figure BDA00027816898800000617
is shown in p 0 Measuring errors and linear acceleration interference by an accelerometer of the system;
therefore, only two matrix equations are established by obtaining the gravity and specific force measurement values at two moments, and the two matrix equations can be solved through a double-vector attitude determination algorithm
Figure BDA0002781689880000071
Because the inertia system indirect coarse alignment algorithm has relatively weak capacity of resisting line shaking interference in a short time, in order to reduce the influence of the line shaking interference, the above formula is integrated and recorded in the initial alignment process
Figure BDA0002781689880000072
Then there is
Figure BDA0002781689880000073
Usually take t 2 ＝2t 1 ，t 1 Modulating the period T for rotation r ，t 2 ＝t c Is the coarse alignment end time;
by adopting a mode of carrying out quadratic integration on the acceleration information in the inertia space, the following results can be obtained:
Figure BDA0002781689880000074
according to a double-vector attitude determination algorithm, the method can obtain
Figure BDA0002781689880000075
(4)
Figure BDA0002781689880000076
Is solved for
P of the indexing mechanism at the electric zero position is a system b of a right front upper coordinate system of the strapdown inertial navigation carrier, and the IMU winds around z in the alignment process p Has a rotation angle of theta (t) of
Figure BDA0002781689880000077
And theta (t) is obtained in real time from an angle measuring device in a control loop of the indexing mechanism.
Moreover, the specific method of the step 3 is as follows: constructing an attitude matrix by using the rough initial attitude obtained by the rough alignment, carrying out forward navigation fine alignment, and estimating by using a Kalman filter through an misalignment angle error model misalignment angle until a reverse navigation starting moment t re The reverse navigation starting time t can be determined according to the computing power and the alignment time length of the navigation computer re ；
Further, the specific steps of step 4 include:
(1) Calculating the starting time t of backward navigation in forward navigation re Sampling a gyroscope in a forward algorithm, carrying out gyroscope constant drift and earth rotation angular rate symbol negation, setting an initial value posture and position of a reverse navigation algorithm as a posture and position of a forward navigation ending moment, and negating a speed symbol of the forward navigation ending moment as a speed of the reverse navigation initial moment;
(2) The sampling data is processed reversely, so that reverse navigation calculation and Kalman filtering from the reverse navigation starting moment to the coarse alignment starting moment can be realized, and the misalignment angle is estimated until the coarse alignment starting moment t 0 。
Moreover, the method for performing fixed point smoothing filtering processing in the reverse navigation process in step 5 includes:
at the initial moment, the state estimation is obtained by a Kalman filter
Figure BDA0002781689880000081
Sum state error mean square error matrix P k The fixed point smoothing algorithm firstly gives an initial value to a smooth state estimation value and a prediction covariance matrix according to a Kalman filtering result:
Figure BDA0002781689880000082
when the Kalman filtering time k is more than or equal to the smooth time j, calculating a smooth gain matrix by using the following formula
Figure BDA0002781689880000083
Fixed point state smoothing estimate
Figure BDA0002781689880000084
And its covariance
Figure BDA0002781689880000085
Figure BDA0002781689880000086
Where superscript s denotes fixed-point smoothing filtering.
Moreover, the calculation formula of step 7 is:
Figure BDA0002781689880000091
Figure BDA0002781689880000092
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0002781689880000093
representing the local estimation, P, of the same state vector X obtained from a plurality of forward and backtracking null correction processes i Representing the mean square error of state estimation, N representing the sum of times of forward navigation or backtracking zero-speed correction process, and fusing state information estimated in all the forward zero-speed correction and backtracking zero-speed correction processes to obtain the global optimal estimation of state
Figure BDA0002781689880000094
Global estimation error P g Less than any local error P i 。
Moreover, the partial feedback correction method in step 8 is as follows:
suppose P real For estimation of true values of navigation parameters, P calcu For calculating values, X, of navigation parameters p Is an estimated value of a Kalman filter, then
Figure BDA0002781689880000095
Wherein, P c ′ alcu ＝(P calcu -α·X p )，X′ p ＝(1-α)X p ，α∈[0,1]For partial feedback coefficients, i.e. for partial feedback of the state estimate of the Kalman filterAnd (3) obtaining an inertial navigation solution value, enabling the solution value to be closer to a true value, keeping the rest part in the Kalman filter, enabling alpha =0 to represent no feedback, enabling alpha =1 to represent full feedback, enabling the alpha to be more than 0 and less than 1, enabling an error state to be small through partial feedback correction, keeping linearity of an error system, enabling convergence speed of the Kalman filter to be higher, enabling state estimation to be more accurate, and enabling the solution value in the alignment process to be smoother.
Part of the feedback coefficients take the form:
Figure BDA0002781689880000096
wherein, T s The time interval of partial feedback correction of the Kalman filter is represented, tau epsilon (0, and infinity) is a time constant of the partial feedback correction, and is set according to the requirement of the system, the feedback quantity of each time is smaller when tau is larger, and the navigation parameters are smoother.
The invention has the advantages and beneficial effects that:
1. in the coarse alignment stage, equivalent errors of the accelerometer and the gyroscope are modulated through regular rotation of positive and negative rotation stop of a complete period, the convergence time of the coarse alignment is shortened, and the coarse alignment precision is improved.
2. According to the invention, through the rotation modulation in the coarse alignment stage, the integrity of the reverse navigation rotation modulation period in the fine alignment stage is ensured, and the fine alignment convergence speed and the alignment precision are further improved;
3. the invention adopts the scheme of determining the attitude of the position vector in the inertial space, avoids the influence of line interference on the velocity vector caused by rotation modulation and size effect, and improves the precision of coarse precision.
4. The invention fully utilizes all observation values during the measurement period to carry out on-line smoothing processing on the filtered state vector through a smoothing technology, thereby realizing the optimal estimation of the state variable and improving the alignment accuracy of the system.
5. The partial feedback correction of the invention changes the error state into a small amount, ensures the linearity of an error system under the condition of larger misalignment angle error, leads the convergence speed of a linear Kalman filter to be faster, leads the state estimation to be more accurate, and leads the calculation value in the navigation alignment process to be smoother.
Drawings
FIG. 1 is a diagram of a rotary modulation strategy according to the present invention;
FIG. 2 is a schematic diagram of the rotational modulation coarse alignment of the present invention;
FIG. 3 is a reverse navigation fine alignment flowchart of the present invention.
Detailed Description
The embodiments of the invention will be described in further detail below with reference to the accompanying drawings:
an alignment method based on rotation modulation comprises rotation modulation coarse alignment, rotation modulation forward navigation fine alignment, rotation modulation reverse navigation fine alignment, partial feedback correction and misalignment angle information fusion.
The method comprises the following specific steps:
step 1, after a system receives a starting command, performing rotation modulation around an azimuth axis based on a rotation modulation strategy shown in figure 1;
the specific steps of the step 1 comprise:
(1) Alignment start time t 0 (setting t) 0 = 0) the IMU is in a static state relative to the carrier, the IMU is at an electric zero position of the indexing mechanism, at the moment, an IMU coordinate system (p system) is coincident with an inertial navigation carrier coordinate system (b system), and t s At the moment IMU (p series) starts
Figure BDA0002781689880000111
Is rotated by 180 DEG about the azimuth axis relative to the carrier (system b), wherein t r The rotation time of the turntable is long, and the IMU is positioned at an electrical 180-degree position of the indexing mechanism at the moment;
(2) The IMU is then stationary relative to the carrier for a period of time t s IMU (p is) around the azimuth axis
Figure BDA0002781689880000112
Is rotated 180 deg. relative to the carrier (series b), when the IMU is in the electrical null of the indexing mechanism, and then the IMU is oppositeAt rest in the inertial navigation coordinate system, the rest time is t s ；
(3) Then, IMU (p series) is wound around the azimuth axis
Figure BDA0002781689880000113
Is rotated 180 deg. relative to the carrier (system b), the IMU is now located at the electrical-180 deg. position of the indexing mechanism, the IMU is stationary relative to the inertial frame, the stationary duration is t s ；
(4) Then IMU to
Figure BDA0002781689880000114
Is rotated by 180 DEG around the azimuth axis relative to the inertial navigation coordinate system, the IMU is located at the electrical zero position of the indexing mechanism, and then the IMU is stationary relative to the inertial navigation coordinate system for a period of time t s To this end, the IMU completes a rotational modulation period T r The rotation of (2).
In the present embodiment, a coordinate system is defined as:
b, defining a carrier coordinate system, defining the right front upper part as positive, and assuming that inertial navigation is superposed with the carrier system;
a p system-IMU coordinate system, wherein the p system of the indexing mechanism in the electric zero position is a b system of a right front upper coordinate system of the inertial navigation carrier;
p 0 system-initial moment IMU inertial system, coinciding with the IMU coordinate system at the initial alignment start moment (p-system), then without rotation with respect to the inertial space;
n is a navigation coordinate system, namely a northeast geographic coordinate system;
n 0 system-initial moment navigation inertial system, coinciding with the navigation coordinate system (n system) at the initial alignment start instant, then without rotation with respect to the inertial space;
Figure BDA0002781689880000121
the attitude-angle relationship of the IMU coordinate system (system p) of the output of the goniometer with respect to the carrier coordinate system (system b)
Figure BDA0002781689880000122
In the present embodiment, the design rotation modulation strategy is:
an alignment method based on rotation modulation, the rotation modulation strategy of the whole alignment process is shown in figure 1, and the alignment starting time t 0 (setting t) 0 = 0) the IMU is in a static state relative to the carrier, the IMU is in an electric zero position of the rotating mechanism, at the moment, an IMU coordinate system (p system) and an inertial navigation carrier coordinate system (b system) are superposed, and t s At the beginning of IMU (p series)
Figure BDA0002781689880000123
Is rotated by 180 DEG about the azimuth axis relative to the carrier (system b), wherein t r For a period of rotation of the turntable, the IMU is now in the indexing mechanism electrical 180 DEG position and then stationary with respect to the carrier for a period of time t s IMU (p is) around the azimuth axis
Figure BDA0002781689880000124
Is rotated 180 deg. relative to the carrier (system b), when the IMU is in the null position of the indexing mechanism, and then the IMU is stationary relative to the inertial frame for a period t s Then IMU (p series) around azimuth axis
Figure BDA0002781689880000125
Is rotated 180 deg. relative to the carrier (system b), the IMU is now located at the electrical-180 deg. position of the indexing mechanism, the IMU is stationary relative to the inertial frame, the stationary duration is t s Then IMU to
Figure BDA0002781689880000126
The rotation angular velocity is rotated by 180 degrees around the azimuth axis relative to the inertial navigation coordinate system, the IMU is positioned at the electric zero position of the displacement mechanism, then the IMU is static relative to the inertial navigation coordinate system, and the static duration is t s To this end, the IMU completes one rotational modulation cycle T r The rotation of (2).
In general, the coarse alignment time is short (typically in the order of 1 min), and can be completed within one rotation modulation periodCoarse alignment is carried out, in order to fully modulate equivalent errors of a gyroscope and an accelerometer in a coarse alignment stage and improve the coarse alignment precision, the duration of the coarse alignment is integral multiple of the rotation modulation period in the coarse alignment stage, the invention suggests that the coarse alignment time is equal to 2T r That is, the time length of 2 rotation modulation periods, the specific coarse alignment time length is determined according to the system application requirement, and then the rotation modulation period duration is determined by the integral multiple relationship between the coarse alignment time and the coarse alignment rotation modulation period.
The selective modulation transposition sequence of the fine alignment stage is consistent with the transposition sequence of the coarse alignment stage, the rotary modulation period can be designed according to the system alignment time requirement, and the stop time t in the rotary modulation process s And a rotation time t r The fine alignment rotational modulation period is determined according to the fine alignment rotational modulation period, i.e. the fine alignment rotational modulation period can be consistent with the coarse alignment rotational modulation period, or can be designed to be inconsistent with the coarse alignment rotational modulation period.
Step 2, in the rotation modulation process of the step 1, the system adopts an indirect coarse alignment scheme based on an inertial system to perform coarse rotation modulation alignment;
the specific steps of the step 2 comprise:
the inertial system indirect coarse alignment is shown in figure 2,
Figure BDA0002781689880000131
the key point of the inertial system indirect coarse alignment is to find p 0 Is a group of n 0 Conversion of systems, i.e. constant-value matrices
Figure BDA0002781689880000132
(1)
Figure BDA0002781689880000133
Is solved for
Figure BDA0002781689880000134
Due to the fact that
Figure BDA0002781689880000135
Is constant, i.e. n is relative to n 0 The rotation is made to be fixed-axis rotation, therefore,
Figure BDA0002781689880000136
wherein L is latitude, omega ie Is the angular velocity of the earth's rotation,
Figure BDA0002781689880000137
is a component of the earth's rotational angular velocity geography,
Figure BDA0002781689880000138
(2)
Figure BDA0002781689880000139
is solved for
Figure BDA00027816898800001310
Figure BDA00027816898800001311
The initial value of attitude quaternion is the measured value of the gyroscope
Figure BDA00027816898800001312
Real-time attitude matrix obtained by utilizing attitude updating algorithm
Figure BDA0002781689880000141
Here need not be to
Figure BDA0002781689880000142
The magnitude of the inertial system indirect coarse alignment algorithm is limited, so that the inertial system indirect coarse alignment algorithm has strong anti-angular motion interference capability.
Angular rate by gyroscope
Figure BDA0002781689880000143
The integral yields an equivalent rotation vector, i.e.
Figure BDA0002781689880000144
Wherein phi is the angle increment information output by the gyroscope in real time for the gyroscope output by the angle increment;
rotation angle phi = | phi |;
the direction of rotation u = φ/φ;
converting the equivalent rotation vector into a quaternion:
Figure BDA0002781689880000145
where m represents the current time and m-1 represents the previous time.
The quaternion of the current moment can be obtained
Figure BDA0002781689880000146
Converting quaternion of current time into quaternion of current time
Figure BDA0002781689880000147
Figure BDA0002781689880000148
(3)
Figure BDA0002781689880000149
Is solved for
Gravity acceleration vector is in n 0 Is projected as
Figure BDA00027816898800001410
Wherein, g n ＝[00-g] T For the component of the gravity acceleration vector under the navigation system, we can obtain:
Figure BDA00027816898800001411
specific force of accelerometer
Figure BDA00027816898800001412
At p 0 The projection is as follows:
Figure BDA00027816898800001413
by passing
Figure BDA0002781689880000151
A relationship between gravitational acceleration and accelerometer specific force may be established. General formula
Figure BDA0002781689880000152
Are simultaneously left-multiplied on two sides
Figure BDA0002781689880000153
To obtain
Figure BDA0002781689880000154
Namely that
Figure BDA0002781689880000155
Wherein the content of the first and second substances,
Figure BDA0002781689880000156
in order to be equivalent to the accelerometer measurement error,
Figure BDA0002781689880000157
is the specific force output of the accelerometer,
Figure BDA0002781689880000158
is shown at p 0 The accelerometer of the system measures errors and linear acceleration disturbances.
Therefore, only two matrix equations are established by obtaining the gravity and specific force measurement values at two moments, and the two matrix equations can be solved through a double-vector attitude determination algorithm
Figure BDA0002781689880000159
Because the inertia system indirect coarse alignment algorithm has relatively weak capacity of resisting line shaking interference in a short time, in order to reduce the influence of the line shaking interference, the above formula is integrated and recorded in the initial alignment process
Figure BDA00027816898800001510
Then there is
Figure BDA00027816898800001511
Usually take t 2 ＝2t 1 ，t 1 Modulating the period T for rotation r ，t 2 ＝t c Is the coarse alignment end time.
In order to further reduce the influence of line interference on alignment accuracy caused by rotation and size effects, the invention adopts a mode of carrying out quadratic integration on acceleration information in an inertia space, and the method can obtain the acceleration information
Figure BDA00027816898800001512
According to a double-vector attitude determination algorithm, the method can obtain
Figure BDA0002781689880000161
(4)
Figure BDA0002781689880000162
Is solved for
P of the indexing mechanism at the electric zero position is a system b of a right front upper coordinate system of the strapdown inertial navigation carrier, and the IMU winds around z in the alignment process p Has a rotation angle of theta (t) of
Figure BDA0002781689880000163
And theta (t) is obtained in real time from an angle measuring device in a control loop of the indexing mechanism.
In this embodiment, the rotation modulation coarse alignment adopts an indirect coarse alignment scheme based on an inertial system, and the indirect coarse alignment does not limit the angular rate of the inertial system and the IMU, so that the indirect coarse alignment algorithm has a very strong anti-interference capability to angular motion, and is more suitable for the application of rotation modulation, and the coarse alignment time is equal to 2N times of the rotation modulation period by design according to the application requirements of the system, thereby ensuring that the equivalent errors of the accelerometer and the gyroscope in the coarse alignment stage can obtain complete and effective rotation modulation, i.e., t c ＝2Nt r Wherein, t c For coarse alignment time, t r A rotation modulation period;
step 3, after the coarse alignment of the rotation modulation is finished, the system carries out the accurate alignment of the rotation modulation forward navigation;
the specific method of the step 3 comprises the following steps: constructing an attitude matrix by using the rough initial attitude obtained by the rough alignment, carrying out forward navigation fine alignment, and estimating by using a Kalman filter through a misalignment angle error model until a reverse navigation initial time t re The reverse navigation starting time t can be determined according to the computing power and the alignment time length of the navigation computer re ；
(1) Updating the forward navigation algorithm after recursive discretization:
the posture updating formula is as follows:
Figure BDA0002781689880000164
velocity update formula:
Figure BDA0002781689880000171
location update formula:
Figure BDA0002781689880000172
Figure BDA0002781689880000173
Figure BDA0002781689880000174
wherein k represents the kth inertial navigation resolving moment, and k-1 represents the previous moment of the kth moment;
Figure BDA0002781689880000175
Figure BDA0002781689880000176
Figure BDA0002781689880000177
Figure BDA0002781689880000178
Figure BDA0002781689880000179
representing an inertial navigation attitude direction cosine matrix;
v n ＝[v E v N v U ] T representing speed under navigation；
L represents latitude;
λ represents longitude;
h represents height;
Figure BDA00027816898800001710
representing a gyro angular velocity measurement;
Figure BDA00027816898800001711
accelerometer specific force measurement;
ω ie representing the earth rotation angular rate;
g and local gravitational acceleration;
R M represents the meridian and radius of the local earth;
R N representing the radius of a prime circle;
Figure RE-GDA0002980151220000181
x is represented by a rotational angular velocity
Figure RE-GDA0002980151220000182
An inversion symmetry matrix is formed, and an operator (·) represents the inversion symmetry matrix formed by the vectors;
T s representing an inertial navigation sampling period;
(2) Designing a Kalman filter for forward navigation fine alignment:
the state variables are as follows:
Figure BDA0002781689880000184
wherein, the first and the second end of the pipe are connected with each other,
φ E ,φ N ,φ U respectively a pitch misalignment angle, a roll misalignment angle and a heading misalignment angle;
δv E ,δv N ,δv U east-direction speed error, north-direction speed error and vertical-direction speed error are respectively included;
δ L, δ λ, δ h are latitude error, longitude error and altitude error, respectively;
Figure BDA0002781689880000185
x gyro drift, Y gyro drift and Z gyro drift are respectively;
Figure BDA0002781689880000186
respectively X plus zero position, Y plus zero position and Z plus zero position.
(3) According to an error equation of inertial navigation, establishing a state equation of a continuous system as follows:
Figure BDA0002781689880000187
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0002781689880000188
Figure BDA0002781689880000189
Figure BDA0002781689880000191
Figure BDA0002781689880000192
Figure BDA0002781689880000193
wherein f is E ,f N ,f U Respectively representing the projection of the specific force measured by the triaxial accelerometer under the navigation system "east-north-sky", i.e.
Figure BDA0002781689880000194
Figure BDA0002781689880000195
Figure BDA0002781689880000196
Figure BDA0002781689880000197
Figure BDA0002781689880000201
Figure BDA0002781689880000202
Figure BDA0002781689880000203
(4) The measurement equation with velocity as observation is:
Figure BDA0002781689880000204
H＝[I 2×2 0 2×13 ]
step 4, after finishing the rotary modulation forward navigation fine alignment, setting the initial value posture and position of a reverse navigation algorithm as the posture and position of the forward navigation finish time, and changing the model parameters into a small error model parameter system to carry out rotary modulation reverse navigation fine alignment;
as shown in fig. 3, the specific steps of step 4 include:
(1) In the forward direction navigation is calculated toReverse navigation start time t re Sampling a gyroscope in a forward algorithm, negating a gyroscope constant drift and an earth rotation angular rate symbol, setting an initial value posture and a position of a reverse navigation algorithm as a posture and a position of a forward navigation ending moment, negating a speed symbol of the forward navigation ending moment as a speed of the reverse navigation initial moment, and changing a model parameter into a small-error model parameter;
(2) The sampling data is processed reversely, so that reverse navigation calculation and Kalman filtering from the reverse navigation starting moment to the coarse alignment starting moment can be realized, and the misalignment angle is estimated until the coarse alignment starting moment t 0 。
Reverse navigation solution in the forward and reverse navigation processes, the attitude matrices of the two processes at the same time are equal, the position coordinates are also equal, and the speeds are equal in magnitude but opposite in sign.
In this embodiment, during the reverse navigation alignment, the gyro sampling and the earth rotation angle rate symbol in the forward navigation are inverted, and the initial value of the reverse navigation algorithm is set as
Figure BDA0002781689880000205
Figure BDA0002781689880000206
And performing reverse navigation calculation on the sampled data:
the reverse navigation attitude updating formula is as follows:
Figure BDA0002781689880000211
the reverse navigation speed updating formula is as follows:
Figure BDA0002781689880000212
the reverse navigation position updating formula is as follows:
Figure BDA0002781689880000213
Figure BDA0002781689880000214
Figure BDA0002781689880000215
wherein the content of the first and second substances,
m represents the last forward navigation end time;
p represents the pth reverse resolving moment, and p-1 represents the next reverse navigation data moment of the pth moment;
Figure BDA0002781689880000216
Figure BDA0002781689880000217
Figure BDA0002781689880000218
in the reverse navigation fine alignment process, the updating of the system state variables is also carried out according to the reverse sequence. And calculating a continuous time state transition matrix F according to a reverse navigation result, negating the speed of the state variable and the gyro drift symbol, and calculating according to a Kalman filter precisely aligned with the forward navigation.
Step 5, filtering to the initial time t of coarse alignment in the reverse navigation Kalman 0 Adjusting the error model parameters again, entering the rotation modulation precision alignment by an alignment program, sampling a gyroscope in a forward algorithm, adopting symbols calculated by a gyroscope constant drift and an earth rotation angular rate symbol, setting an initial value gesture and position of a reverse navigation algorithm as the gesture and position of the forward navigation ending moment, and negating a speed symbol at the reverse navigation ending momentAs the speed of the initial time of forward navigation, the sampling data starts forward calculation and Kalman filtering from the initial time of coarse alignment, the rotational modulation precision alignment is carried out, the misalignment angle is estimated until the initial time t of reverse navigation re (ii) a Performing fixed point smoothing filtering in the reverse navigation process, and performing online smoothing on the filtered state vector by fully utilizing all observation values in the measurement period to realize the optimized estimation of the state variable;
the method for performing fixed point smoothing filtering processing in the reverse navigation process in the step 5 comprises the following steps:
the fixed point on-line smoothing filtering is to fully utilize all observation values during measurement to carry out on-line smoothing processing on a filtered state vector on the basis of conventional Kalman filtering estimation, thereby realizing the optimized estimation of state variables and improving the alignment precision of a system.
At the initial moment, the state estimate is obtained by a Kalman filter
Figure BDA0002781689880000221
Sum state error mean square error matrix P k The fixed point smoothing algorithm firstly gives an initial value to the smooth state estimation value and the prediction covariance matrix according to a Kalman filtering result:
Figure BDA0002781689880000222
when the Kalman filtering time k is more than or equal to the smooth time j, calculating a smooth gain matrix by using the following formula
Figure BDA0002781689880000223
Fixed point state smoothing estimate
Figure BDA0002781689880000224
And its covariance
Figure BDA0002781689880000225
Figure BDA0002781689880000226
Where the superscript s denotes fixed-point smoothing filtering.
Step 6, repeatedly executing the operation times of the step 4 and the step 5, repeatedly utilizing the navigation data and the external reference information in the alignment time, and determining the repeated calculation times according to the calculation capability and the alignment time length of the navigation computer;
step 7, after the rotational modulation forward navigation fine alignment of the step 6 is completed, performing information fusion by using misalignment angle state quantity and state estimation mean square error obtained by forward and reverse navigation calculation, and finally obtaining global optimal estimation of the misalignment angle state, thereby improving the alignment precision of the system;
the calculation formula of the step 7 is as follows:
Figure BDA0002781689880000231
Figure BDA0002781689880000232
wherein the content of the first and second substances,
Figure BDA0002781689880000233
representing the local estimation, P, of the same state vector X obtained from a plurality of forward and backtracking null correction processes i Representing the mean square error of state estimation, N representing the sum of times of forward navigation or backtracking zero-speed correction process, and fusing state information estimated in all forward zero-speed correction and backtracking zero-speed correction processes to obtain the global optimal estimation of state
Figure BDA0002781689880000234
Global estimation error P g Less than any local error P i 。
Step 8, after the information fusion after the step 7 is completed, from t re Starting to carry out forward navigation at the momentFine alignment until the alignment end time t e The misalignment angle and velocity information are partially feedback corrected during alignment.
The partial feedback correction method in the step 8 comprises the following steps:
suppose P real For estimation of true values of navigation parameters, P calcu For calculating values, X, of navigation parameters p The estimated value of the Kalman filter is
Figure BDA0002781689880000235
Wherein, P c ′ alcu ＝(P calcu -α·X p )，X′ p ＝(1-α)X p ，α∈[0,1]The method is a partial feedback coefficient, namely, a state estimation value of the Kalman filter is partially fed back to an inertial navigation calculation value, so that a calculation value is closer to a true value, the rest part is continuously left in the Kalman filter, alpha =0 represents no feedback, alpha =1 represents full feedback, 0 < alpha < 1 part of feedback, an error state is changed into a small amount through partial feedback correction, the linearity of an error system is kept, the convergence rate of the Kalman filter is higher, the state estimation is more accurate, and meanwhile, the calculation value in the alignment process is smoother.
Part of the feedback coefficients take the form:
Figure BDA0002781689880000236
wherein, T s The time interval of partial feedback correction of the Kalman filter is represented, tau epsilon (0, and +/-infinity) is a time constant of the partial feedback correction, the larger tau is, the smaller the feedback quantity of each time is, and the smoother the navigation parameter is set according to the needs of a system.
The system performs partial feedback corrections during both forward navigational alignment and reverse navigational alignment.
It should be emphasized that the embodiments described herein are illustrative and not restrictive, and thus the present invention includes, but is not limited to, the embodiments described in this detailed description, as well as other embodiments that can be derived by one skilled in the art from the teachings herein, and are within the scope of the present invention.

Claims (6)

1. An alignment method based on rotation modulation, characterized in that: the method comprises the following steps:
step 1, after a system is electrified and receives a starting command, carrying out rotation modulation around an azimuth axis based on a rotation modulation strategy;
step 2, in the rotation modulation process of the step 1, the system adopts an indirect coarse alignment scheme based on an inertial system to perform coarse rotation modulation alignment;
step 3, after the coarse alignment of the rotation modulation is finished, the system carries out the fine alignment of the rotation modulation forward navigation;
step 4, after the rotary modulation forward navigation fine alignment is finished, setting the initial value posture and position of a reverse navigation algorithm as the posture and position of the forward navigation finish time, and changing the model parameters into a small-error model parameter system to carry out rotary modulation reverse navigation fine alignment;
step 5, in the starting time t from the backward navigation Kalman filtering to the coarse alignment 0 Adjusting the error model parameters again, enabling an alignment program to enter rotary modulation precision alignment, sampling a gyroscope in a forward algorithm, adopting symbols calculated by a gyroscope constant drift and an earth rotation angular rate symbol, setting an initial value posture and a position of a reverse navigation algorithm as the posture and the position of a forward navigation ending moment, negating a speed symbol of the reverse navigation ending moment as the speed of the forward navigation initial moment, starting forward calculation and Kalman filtering on the sampled data from a rough alignment starting moment, carrying out rotary modulation precision alignment, estimating a misalignment angle until the reverse navigation starting moment t re (ii) a And performing fixed point smoothing filtering in the reverse navigation process, and making full use of all observation values during measurement to perform filtering wave state vectorPerforming line smoothing to realize the optimized estimation of the state variable;
step 6, repeatedly executing the operation times of the step 4 and the step 5, repeatedly utilizing the navigation data and the external reference information in the alignment time, and determining the repeated calculation times according to the calculation capability and the alignment time length of the navigation computer;
step 7, after the rotational modulation forward navigation fine alignment of the step 6 is completed, performing information fusion by using misalignment angle state quantity and state estimation mean square error obtained by forward and reverse navigation calculation, and finally obtaining global optimal estimation of the misalignment angle state, thereby improving the alignment precision of the system;
step 8, after the information fusion after the step 7 is completed, from t re The forward navigation fine alignment is carried out from the moment until the alignment end time t e Partially feeding back and correcting the misalignment angle and the speed information in the alignment process;
the specific steps of the step 1 comprise:
(1) Alignment start time t 0 The IMU is in a static state relative to the carrier, the IMU is in an electric zero position of the indexing mechanism, at the moment, an IMU coordinate system is coincident with an inertial navigation carrier coordinate system, and t s At the moment IMU starts
Figure FDA0003799484410000021
Is rotated 180 deg. about the azimuth axis relative to the carrier, wherein t r The rotation time of the turntable is long, and the IMU is positioned at an electrical 180-degree position of the indexing mechanism at the moment;
(2) The IMU is then stationary relative to the carrier for a period of time t s The IMU is around the azimuth axis
Figure FDA0003799484410000022
Is rotated 180 deg. relative to the carrier, while the IMU is located in the null position of the indexing mechanism, and then the IMU is stationary relative to the inertial frame for a period t s ；
(3) The IMU is then rotated about the azimuth axis
Figure FDA0003799484410000023
Is rotated 180 deg. relative to the carrier, the IMU is now located at the electrical-180 deg. position of the indexing mechanism, the IMU is stationary relative to the inertial frame, the stationary duration is t s ；
(4) Then IMU to
Figure FDA0003799484410000024
Is rotated by 180 DEG around the azimuth axis relative to the inertial navigation coordinate system, the IMU is located at the electrical zero position of the indexing mechanism, and then the IMU is stationary relative to the inertial navigation coordinate system for a period of time t s To this end, the IMU completes a rotational modulation period T r Rotation of (2);
wherein:
b, defining a carrier coordinate system, defining the right front upper part as positive, and assuming that inertial navigation is superposed with the carrier system;
a p system-IMU coordinate system, wherein the p system of the indexing mechanism in the electric zero position is a b system of a right front upper coordinate system of the inertial navigation carrier;
p 0 system-initial moment IMU inertial system, coinciding with IMU coordinate system at initial alignment start moment, then non-rotating with respect to inertial space;
n is a navigation coordinate system, namely a northeast geographic coordinate system;
n 0 system-initial moment navigation inertial system, coinciding with initial alignment starting instant navigation coordinate system, then no rotation relative to inertial space;
Figure FDA0003799484410000031
the attitude-angle relationship of the IMU coordinate system output by the angle-measuring device with respect to the carrier coordinate system
Figure FDA0003799484410000032
The step 2 of indirect coarse alignment of the inertial system
Figure FDA0003799484410000033
The key point of indirect coarse alignment of the inertial system is to find p 0 Is a group of n 0 Conversion of systems, i.e. constant-value matrices
Figure FDA0003799484410000034
The method comprises the following specific steps:
(1)
Figure FDA0003799484410000035
is solved for
Figure FDA0003799484410000036
Due to the fact that
Figure FDA0003799484410000037
Is constant, i.e. n is relative to n 0 The rotation of the shaft is fixed to the shaft,
therefore, the temperature of the molten metal is controlled,
Figure FDA0003799484410000038
wherein L is latitude, omega ie Is the angular velocity of the rotation of the earth,
Figure FDA0003799484410000039
is a component of the earth in the geographic system of rotational angular velocity,
Figure FDA00037994844100000310
(2)
Figure FDA00037994844100000311
is solved for
Figure FDA0003799484410000041
Figure FDA0003799484410000042
Initial values of attitude quaternion for gyroscope measurements
Figure FDA0003799484410000043
Real-time attitude matrix obtained by utilizing attitude updating algorithm
Figure FDA0003799484410000044
Angular rate by gyroscope
Figure FDA0003799484410000045
The integral yields an equivalent rotation vector, i.e.
Figure FDA0003799484410000046
Wherein, for the gyroscope output by the angular increment, phi is the angular increment information output by the gyroscope in real time;
rotation angle phi = | phi |;
the direction of rotation u = φ/φ;
converting the equivalent rotation vector into a quaternion:
Figure FDA0003799484410000047
wherein m represents the current time, and m-1 represents the last time;
quaternion at the current moment can be obtained
Figure FDA0003799484410000048
Converting quaternion of current time into quaternion of current time
Figure FDA0003799484410000049
Figure FDA00037994844100000410
(3)
Figure FDA00037994844100000411
Is solved for
Gravity acceleration vector is in n 0 Is projected as
Figure FDA00037994844100000412
Wherein, g n ＝[0 0 -g] T For the component of the gravity acceleration vector under the navigation system, we can obtain:
Figure FDA00037994844100000413
specific force of accelerometer
Figure FDA00037994844100000414
At p is 0 The projection is as follows:
Figure FDA0003799484410000051
by passing
Figure FDA0003799484410000052
A relationship between the gravity acceleration and the specific force of the accelerometer can be established; general formula (II)
Figure FDA0003799484410000053
Are simultaneously left-multiplied on two sides
Figure FDA0003799484410000054
To obtain
Figure FDA0003799484410000055
Namely, it is
Figure FDA0003799484410000056
Wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003799484410000057
in order to be equivalent to the accelerometer measurement error,
Figure FDA0003799484410000058
is the specific force output of the accelerometer,
Figure FDA0003799484410000059
is shown at p 0 Measuring errors and linear acceleration interference by an accelerometer;
therefore, as long as the measured values of the gravity and the specific force at two moments are obtained and two matrix equations are established, the two matrix equations can be solved through a double-vector attitude determination algorithm
Figure FDA00037994844100000510
Because the inertia system indirect coarse alignment algorithm has relatively weak capacity of resisting line shaking interference in a short time, in order to reduce the influence of the line shaking interference, the above formula is integrated and recorded in the initial alignment process
Figure FDA00037994844100000511
Then there is
Figure FDA00037994844100000512
Get t 2 ＝2t 1 ，t 1 Modulating the period T for rotation r ，t 2 ＝t c Is the coarse alignment end time;
by adopting a mode of carrying out quadratic integration on the acceleration information in the inertia space, the following results can be obtained:
Figure FDA00037994844100000513
according to a double-vector attitude determination algorithm, the method can obtain
Figure FDA0003799484410000061
(4)
Figure FDA0003799484410000062
Is solved for
P of the indexing mechanism at the electric zero position is a system b of a right front upper coordinate system of the strapdown inertial navigation carrier, and the IMU winds around z in the alignment process p Has a rotation angle of theta (t) of
Figure FDA0003799484410000063
And theta (t) is obtained from an angle measuring device in a control loop of the indexing mechanism in real time.
2. The alignment method based on rotation modulation according to claim 1, wherein: the specific method of the step 3 comprises the following steps: constructing an attitude matrix by using the rough initial attitude obtained by the rough alignment, carrying out forward navigation fine alignment, and estimating by using a Kalman filter through a misalignment angle of a misalignment angle error model until the reverse navigation initial time t re The reverse navigation starting time t can be determined according to the computing power and the alignment time length of the navigation computer re 。
3. The alignment method based on rotation modulation according to claim 1, wherein: the specific steps of the step 4 comprise:
(1) Calculating the starting time t of backward navigation in forward navigation re Sampling a gyroscope in a forward algorithm, negating a gyroscope constant drift and an earth rotation angular rate symbol, setting an initial attitude and a position of a reverse navigation algorithm as an attitude and a position of a forward navigation ending moment, and negating a speed symbol of the forward navigation ending moment as a speed of the reverse navigation initial moment;
(2) The sampling data is processed in reverse direction, so that reverse navigation calculation and Kalman filtering from the start time of reverse navigation to the start time of coarse alignment can be realized, and the misalignment angle is estimated until the start time t of coarse alignment 0 。
4. A method for alignment based on rotational modulation according to claim 1, wherein: the method for performing fixed point smoothing filtering processing in the reverse navigation process in the step 5 comprises the following steps:
at the initial moment, the state estimate is obtained by a Kalman filter
Figure FDA0003799484410000071
Sum state error mean square error matrix P k The fixed point smoothing algorithm firstly gives an initial value to a smooth state estimation value and a prediction covariance matrix according to a Kalman filtering result:
Figure FDA0003799484410000072
when the Kalman filtering time k is more than or equal to the smoothing time j, calculating a smoothing gain matrix by using the following formula
Figure FDA0003799484410000073
Fixed point state smoothing estimate
Figure FDA0003799484410000074
And its covariance
Figure FDA0003799484410000075
Figure FDA0003799484410000076
Where superscript s denotes fixed-point smoothing filtering.
5. The alignment method based on rotation modulation according to claim 1, wherein: the calculation formula of the step 7 is as follows:
Figure FDA0003799484410000077
Figure FDA0003799484410000078
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003799484410000079
representing the local estimation, P, of the same state vector X obtained from a plurality of forward and backtracking null correction processes i Representing the mean square error of state estimation, N representing the sum of times of forward navigation or backtracking zero-speed correction process, and fusing state information estimated in all the forward zero-speed correction and backtracking zero-speed correction processes to obtain the global optimal estimation of state
Figure FDA00037994844100000710
Global estimation error P g Less than any local error P i 。
6. The alignment method based on rotation modulation according to claim 1, wherein: the partial feedback correction method in the step 8 comprises the following steps:
suppose P real To be guideEstimation of true values of flight parameters, P calcu For calculating values, X, of navigation parameters p The estimated value of the Kalman filter is
Figure FDA0003799484410000081
Wherein, P' calcu ＝(P calcu -α·X p )，X′ p ＝(1-α)X p ，α∈[0,1]The method is a partial feedback coefficient, namely, a state estimation value of the Kalman filter is partially fed back to an inertial navigation calculation value, so that a calculation value is closer to a true value, the rest part is continuously left in the Kalman filter, alpha =0 represents no feedback, alpha =1 represents full feedback, alpha is more than 0 and less than 1, partial feedback is performed, an error state is changed into a small amount through partial feedback correction, the linearity of an error system is kept, the convergence rate of the Kalman filter is higher, the state estimation is more accurate, and meanwhile, the calculation value in the alignment process is smoother;
part of the feedback coefficients take the form:
Figure FDA0003799484410000082
wherein, T s The time interval of the partial feedback correction of the Kalman filter is represented, tau epsilon (0, and ∞) is a time constant of the partial feedback correction, and is set according to the needs of the system, the larger tau is, the smaller feedback quantity each time is, and the smoother navigation parameters are.
CN202011283857.0A 2020-11-17 2020-11-17 Alignment method based on rotation modulation Active CN112729332B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011283857.0A CN112729332B (en) 2020-11-17 2020-11-17 Alignment method based on rotation modulation

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011283857.0A CN112729332B (en) 2020-11-17 2020-11-17 Alignment method based on rotation modulation

Publications (2)

Publication Number Publication Date
CN112729332A CN112729332A (en) 2021-04-30
CN112729332B true CN112729332B (en) 2022-10-28

Family

ID=75597486

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011283857.0A Active CN112729332B (en) 2020-11-17 2020-11-17 Alignment method based on rotation modulation

Country Status (1)

Country Link
CN (1) CN112729332B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113959462B (en) * 2021-10-21 2023-09-12 北京机电工程研究所 Quaternion-based inertial navigation system self-alignment method
CN114894222B (en) * 2022-07-12 2022-10-28 深圳元戎启行科技有限公司 External parameter calibration method of IMU-GNSS antenna and related method and equipment

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102721417A (en) * 2011-12-23 2012-10-10 北京理工大学 Method for error suppression of inertial concretionary coarse alignment of strapdown inertial navigation system
BR102017024827A2 (en) * 2017-11-21 2019-06-11 Universidade Estadual De Campinas - Unicamp DIGITAL MODULATION TECHNIQUE FOR INTERFEROMETRIC GYRO OPTICAL FIBER (IFOG) OF CLOSED MESH OF TWO LEVELS AND TWO PERIODS
CN110763252A (en) * 2019-09-24 2020-02-07 中国船舶重工集团公司第七0七研究所 Strapdown inertial navigation reverse order filtering design method based on embedded processor
CN110806220A (en) * 2019-11-23 2020-02-18 中国船舶重工集团公司第七一七研究所 Inertial navigation system initial alignment method and device
CN111102993A (en) * 2019-12-31 2020-05-05 中国人民解放军战略支援部队航天工程大学 Initial alignment method for shaking base of rotary modulation type strapdown inertial navigation system
AU2020101268A4 (en) * 2020-07-06 2020-08-13 Harbin Engineering University The initial alignment method for sway base

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102721417A (en) * 2011-12-23 2012-10-10 北京理工大学 Method for error suppression of inertial concretionary coarse alignment of strapdown inertial navigation system
BR102017024827A2 (en) * 2017-11-21 2019-06-11 Universidade Estadual De Campinas - Unicamp DIGITAL MODULATION TECHNIQUE FOR INTERFEROMETRIC GYRO OPTICAL FIBER (IFOG) OF CLOSED MESH OF TWO LEVELS AND TWO PERIODS
CN110763252A (en) * 2019-09-24 2020-02-07 中国船舶重工集团公司第七0七研究所 Strapdown inertial navigation reverse order filtering design method based on embedded processor
CN110806220A (en) * 2019-11-23 2020-02-18 中国船舶重工集团公司第七一七研究所 Inertial navigation system initial alignment method and device
CN111102993A (en) * 2019-12-31 2020-05-05 中国人民解放军战略支援部队航天工程大学 Initial alignment method for shaking base of rotary modulation type strapdown inertial navigation system
AU2020101268A4 (en) * 2020-07-06 2020-08-13 Harbin Engineering University The initial alignment method for sway base

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Optimization of the collimator mask for the rotational modulation collimator-based gamma-ray/neutron dual-particle imager;Hyun Suk KimSung-Joon YeGeehyun Kim;《Current Applied Physics》;20190329;全文 *
双轴旋转调制惯导系统的极区导航算法;张崇猛等;《中国惯性技术学报》;20180630;第26卷(第3期);全文 *
车载旋转调制捷联惯导系统最优对准技术;汪徐胜等;《兵工自动化》;20170430;第36卷(第4期);全文 *

Also Published As

Publication number Publication date
CN112729332A (en) 2021-04-30

Similar Documents

Publication Publication Date Title
CN108180925B (en) Odometer-assisted vehicle-mounted dynamic alignment method
CN110398257B (en) GPS-assisted SINS system quick-acting base initial alignment method
CN103743414B (en) Initial Alignment Method between the traveling of vehicle-mounted SINS assisted by a kind of speedometer
CN102809377B (en) Aircraft inertia/pneumatic model Combinated navigation method
CN112697141A (en) Inertial navigation/odometer moving base posture and position alignment method based on reverse navigation
CN110954102B (en) Magnetometer-assisted inertial navigation system and method for robot positioning
CN109506660B (en) Attitude optimization resolving method for bionic navigation
CN112729332B (en) Alignment method based on rotation modulation
CN113405563B (en) Inertial measurement unit alignment method
CN106767925B (en) Inertial navigation system three-position parameter identification alignment method with double-shaft indexing mechanism
CN110806220B (en) Inertial navigation system initial alignment method and device
CN107270938A (en) Single-shaft-rotation inertial navigation system posture demodulation method based on Taylor series fitting
CN111207773B (en) Attitude unconstrained optimization solving method for bionic polarized light navigation
CN111190207B (en) PSTCSDREF algorithm-based unmanned aerial vehicle INS BDS integrated navigation method
CN109489661A (en) Gyro constant value drift estimation method when a kind of satellite is initially entered the orbit
CN116222551A (en) Underwater navigation method and device integrating multiple data
CN110388942B (en) Vehicle-mounted posture fine alignment system based on angle and speed increment
CN113959462B (en) Quaternion-based inertial navigation system self-alignment method
CN115033844A (en) Unmanned aerial vehicle state estimation method, system and device and readable storage medium
CN114964222A (en) Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device
CN110986934A (en) Navigation method and system of integrated double-shaft rotation inertial navigation astronomical integrated navigation system
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN113008229A (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN115574817B (en) Navigation method and navigation system based on three-axis rotation type inertial navigation system
CN114111840B (en) DVL error parameter online calibration method based on integrated navigation

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