CN111780758A - Gravity stabilization platform attitude determination method based on dual-mode calculation and application - Google Patents

Gravity stabilization platform attitude determination method based on dual-mode calculation and application Download PDF

Info

Publication number
CN111780758A
CN111780758A CN202010653207.4A CN202010653207A CN111780758A CN 111780758 A CN111780758 A CN 111780758A CN 202010653207 A CN202010653207 A CN 202010653207A CN 111780758 A CN111780758 A CN 111780758A
Authority
CN
China
Prior art keywords
loop
imu
navigation
updating
resolving
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.)
Pending
Application number
CN202010653207.4A
Other languages
Chinese (zh)
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.)
Naval University of Engineering PLA
Original Assignee
Naval University of Engineering PLA
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 Naval University of Engineering PLA filed Critical Naval University of Engineering PLA
Priority to CN202010653207.4A priority Critical patent/CN111780758A/en
Publication of CN111780758A publication Critical patent/CN111780758A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention belongs to the technical field of navigation, and discloses a gravity stabilized platform attitude determination method based on dual-mode calculation and application thereof, wherein the gravity stabilized platform attitude determination method based on dual-mode calculation comprises the following steps: establishing two resolving loops in the same IMU, wherein the two loops are used for navigation resolving based on strapdown inertial navigation mechanics arrangement; dividing the working state of the IMU into three stages, wherein two loops are solved by adopting different data under different states, one loop is normally solved, and the other loop is solved based on a virtual expansion updating period in a certain stage; and generating a phase difference, and inhibiting the attitude calculation error of the IMU through data averaging to realize high-precision attitude determination of the gravity stabilized platform. The method can solve the problem that the error suppression algorithm of the traditional damping algorithm is invalid in the carrier maneuvering state, and effectively suppresses the Schuler oscillatory error of IMU navigation calculation.

Description

Gravity stabilization platform attitude determination method based on dual-mode calculation and application
Technical Field
The invention belongs to the technical field of navigation, and particularly relates to a gravity stabilized platform attitude determination method and system based on dual-mode calculation.
Background
At present, the IMU can realize the solution of the attitude, the speed and the position of the IMU based on the inertial navigation mechanics arrangement without depending on any external information source, and the working state is called as a pure inertial state. The duration of the gravity measurement task varies from several hours to several months, and a gravity stabilization platform taking the IMU as a posture reference sometimes needs to continuously work for a long time, so that the IMU is required to ensure the long-term stability of the posture accuracy in a navigation solution phase. However, the long-term working accuracy of some autonomous speed measuring devices is difficult to guarantee, and for example, the speed measuring accuracy of the log is greatly influenced by factors such as the maneuvering condition of the carrier and the current condition, and the problems of speed measuring and lock losing to the ground may occur. The external speed error can affect the navigation resolving precision of the IMU, and meanwhile, the subsequent attitude resolving precision can be affected by the external speed error. Therefore, the navigation calculation precision for improving the pure inertial state of the IMU is the most reliable means for guaranteeing the long-term attitude precision of the IMU. The aim of improving the navigation precision of the IMU in the pure inertia state is also always pursued in the field of inertial navigation.
At present, error suppression in an IMU pure inertia state is mainly realized by a damping network. Namely, a damping correction network is inserted into a resolving loop of the strapdown inertial navigation, so that the oscillatory errors of IMU navigation resolving are restrained. When the IMU is positioned on a static base or a shaking base, the traditional damping algorithm can effectively inhibit Schuler oscillatory errors of IMU navigation calculation; however, in a carrier maneuvering state, the damping network can excite the attitude error of the inertial navigation resolving loop to further influence the navigation resolving accuracy of the IMU.
The error propagation characteristics of the inertial navigation system working for a long time can be mainly divided into: schuller oscillations, foucault oscillations and earth oscillations. In which the amplitude of the schuller oscillations increases with time, and in the purely inertial state, such oscillation errors are difficult to suppress.
Through the above analysis, the problems and defects of the prior art are as follows: (1) the long-term working precision of the existing autonomous speed measuring equipment is difficult to guarantee.
If the log has the problems that the speed measurement precision is greatly influenced by factors such as the maneuvering condition of a carrier, the ocean current condition and the like, and the speed measurement to the ground is possible to be unlocked, and the like. The external speed error can affect the navigation resolving precision of the IMU, and the subsequent attitude resolving precision can be affected by the influence
(2) The existing error correction method is realized through a damping network, the traditional damping algorithm has contradiction in mechanism, however, in a carrier maneuvering state, the damping network can excite the attitude error of an inertial navigation resolving loop to further influence the navigation resolving precision of an IMU, so that the navigation error suppression algorithm based on the damping network fails.
The difficulty in solving the above problems and defects is: the error propagation characteristics of the inertial navigation system working for a long time can be mainly divided into: schuller oscillations, foucault oscillations, and earth oscillations; the amplitude of the schuller oscillation increases with time, and this oscillation error is difficult to suppress in the pure inertial state.
The amplitude of the schuller oscillation increases with time. Taking the north-bound velocity error as an example, the schuller oscillation modulated by the foucault oscillation is periodically varying and grows in amplitude over time.
So far, error suppression in the IMU pure inertial state is mainly achieved by a damping network. Namely, a damping correction network is inserted into a resolving loop of the strapdown inertial navigation, so that the oscillatory errors of IMU navigation resolving are inhibited, and when the IMU is positioned on a static base or a shaking base, the Schuler oscillatory errors of the IMU navigation resolving can be effectively inhibited by a traditional damping algorithm; however, in a carrier maneuvering state, the damping network can excite the attitude error of the inertial navigation resolving loop to further influence the navigation resolving accuracy of the IMU.
The significance of solving the problems and the defects is as follows: the navigation resolving precision of the IMU in the pure inertia state is the most reliable means for guaranteeing the long-term attitude precision of the IMU; the aim of improving the navigation precision of the IMU in the pure inertia state is also always pursued in the field of inertial navigation.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a gravity stabilization platform attitude determination method based on dual-mode calculation and application thereof.
The invention is realized in such a way that a gravity stabilized platform attitude determination method based on dual-mode calculation comprises the following steps:
establishing two resolving loops in the same IMU, wherein the two loops are used for navigation resolving based on strapdown inertial navigation mechanics arrangement; the working state of the IMU is divided into three stages, the two loops adopt different data to carry out resolving under different states to generate phase difference, attitude resolving errors of the IMU are inhibited through data averaging, and high-precision attitude determination of the gravity stabilization platform is achieved.
As shown in fig. 2, the algorithm proposed by the present invention divides the operating state of the IMU into three phases: the "undamped" state, the "damped" state and the "damped" state. Based on the differential equation of the inertial navigation system, two resolving loops, namely a loop 1 and a loop 2, are established on the same IMU. In the two loops, the same original data is utilized to independently and simultaneously carry out inertial navigation solution.
In the undamped state, the resolving modes of the loop 1 and the loop 2 are completely the same, and the actual updating period represents that the inertial navigation resolving is carried out by using the data of the gyroscope and the accelerometer at the current time. In the stage of 'entering damping', inertial navigation calculation is carried out in the loop 2 based on the updating period of virtual expansion. The "virtual expansion update period" indicates that the inertial navigation is updated by using virtual IMU data. The virtual data refers to data obtained by virtually expanding the update period of real IMU original data by multiple times.
Further, the two loops adopt different data to perform resolving under different states, which includes:
one circuit is normally solved, and the other circuit is solved based on a virtual expansion updating period in a certain stage;
the method specifically comprises the following steps:
(1) the undamped state:
the circuit 1 and the circuit 2 both utilize real data to carry out inertial navigation calculation based on the gyroscope and accelerometer data at the current moment;
(2) entering a damping state:
the loop 1 utilizes real data to carry out inertial navigation resolving based on an actual updating period; the navigation error is X (t) ═ VE；VN；L；φx；φy；φz]；
The loop 2 utilizes the virtual data to carry out inertial navigation resolving based on the virtual expanded updating period; the navigation error is X' (t) ═ VE'；VN'；L'；φx'；φy'；φz']；
φ＝[φxφyφz]TIs the attitude error; vn＝[VEVNVU]TIs the speed error; [ L,. lambda.h]Is a position error.
The difference between the update periods of loop 1 and loop 2 results in a gradual change in the phase of X (t) and X' (t);
(3) damping state:
when the phase lead of the loop 2 relative to the loop 1 reaches 180 degrees at a certain moment, namely the error of the two loops is reversed, the damping stage is started, the loop 1 and the loop 2 use real original data to carry out resolving, and the mean value of resolving outputs of the loop 1 and the loop 2 is used as a final resolving result.
Further, the virtual extension update period includes:
the virtual expansion updating period represents that inertial navigation is updated by virtual IMU data;
the virtual data is obtained by virtually expanding the update period of the real IMU original data by multiple times.
The specific idea of the algorithm of the present invention and its mathematical expression are as follows.
The basic equation of the strapdown inertial navigation system is as follows:
Figure BDA0002575746730000041
Figure BDA0002575746730000042
Figure BDA0002575746730000043
in the formula (I), the compound is shown in the specification,
Figure BDA0002575746730000044
gn＝[0 0 -g]T，
Figure BDA0002575746730000045
Figure BDA0002575746730000046
Figure BDA0002575746730000047
the attitude matrix from the carrier system to the navigation system; v. ofnIs the speed of movement of the carrier relative to the earth; [ L,. lambda.h]Latitude, longitude and altitude, respectively;
Figure BDA0002575746730000048
the angular rate of motion of the carrier measured for the gyroscope; f. ofbSpecific force under the carrier system measured for the accelerometer; omegaieThe rotation angular rate of the earth under the inertial system; gnIs a gravity vector; rMIs the radius of the earth meridian; rNIs the radius of the earth-unitary mortise ring.
Further, the two loops adopt different data to resolve under different states further comprises:
1) the undamped state: the pose, velocity and position update process of the IMU can be expressed as:
and (3) posture updating:
Figure BDA0002575746730000051
and (3) updating the speed:
Figure BDA0002575746730000052
and (3) updating the position:
Figure BDA0002575746730000053
Figure BDA0002575746730000054
Figure BDA0002575746730000055
in the formula (I), the compound is shown in the specification,
Figure BDA0002575746730000056
Figure BDA0002575746730000057
k＝1,2,3...,Tsupdating a strapdown inertial navigation period;
the updating output of the navigation postures of the loop 1 and the loop 2 is as follows:
Figure BDA0002575746730000058
let us assume that the duration of the "enter damping" phase is tpThe duration of the "damping" phase is tq
3) Entering a damping state:
the undamped period starts from time 0 to tnEnding the moment; n1 is the navigation coordinate system of loop 1; n2 is the navigation coordinate system of loop 2; b1 is the carrier coordinate system of loop 1; b2 is the carrier coordinate system of loop 2; when the carrier is in a damping state, the carrier is in a quasi-static state, and the total duration of the carrier in the damping state is 1/k of the Schuler cycle;
and (3) posture updating:
updating the attitude of the loop 1:
Figure BDA0002575746730000061
loop 2 attitude update:
Figure BDA0002575746730000062
wherein the loop 2 attitude update calculates the angular rate
Figure BDA0002575746730000063
From angular velocity information
Figure BDA0002575746730000064
By multiplying by a corresponding scaling factor, i.e.
Figure BDA0002575746730000065
In loop 2, the IMU is based on the formula
Figure BDA0002575746730000066
The update cycle in (4) performs navigation solution.
Let k be 2. Then the process of the first step is carried out,
Figure BDA0002575746730000067
the duration of the "enter damping" phase is mainly determined by the parameter k therein. If there are other values for k that would cause the phase of loop 2 to advance loop 1 by half the schuller cycle in a shorter time, the duration of the "enter damping" phase is shortened. For example, setting k to 5, the phase of loop 2 will lead 180 ° relative to loop 1 over a period of 1/8 schuller cycles. That is, the duration of the "enter damping" phase is shortened to 1/8 Schuler cycles. If k is set to 9, the duration of the "enter damping" phase is shortened to 1/16 schuller cycles. That is, by setting the value of k appropriately, the time for the IMU damping state to switch will be shortened.
Therefore, the attitude update process of the loop 2 is:
Figure BDA0002575746730000068
to ensure the formula
Figure BDA0002575746730000069
And formula
Figure BDA00025757467300000610
The posture updating results in (1) are consistent. Calculating angular rate in loop 2
Figure BDA00025757467300000611
Should be that
Figure BDA00025757467300000612
1/2 of (1). Then there are:
Figure BDA00025757467300000613
therefore, the temperature of the molten metal is controlled,
Figure BDA0002575746730000071
and (3) updating the speed:
loop 2 speed update:
Figure BDA0002575746730000072
and (3) updating the position:
loop 2 location update:
Figure BDA0002575746730000073
and (3) posture updating navigation output:
loop 1 outputs:
Figure BDA0002575746730000074
the loop 2 outputs:
Figure BDA0002575746730000075
another object of the present invention is to provide a program storage medium for receiving a user input, the stored computer program causing an electronic device to execute the dual-mode calculation-based gravity-stabilized platform attitude determination method.
It is another object of the present invention to provide a computer program product stored on a computer readable medium, comprising a computer readable program for providing a user input interface to implement the dual-mode solution based gravity-stabilized platform attitude determination method when executed on an electronic device.
Another object of the present invention is to provide an IMU for performing the dual-mode solution based gravity stabilized platform attitude determination method.
By combining all the technical schemes, the invention has the advantages and positive effects that:
the invention provides a gravity stabilized platform attitude determination method based on 'dual-mode' resolving, wherein two resolving loops are established in the same IMU, and the two loops are used for navigation resolving based on strapdown inertial navigation mechanics arrangement; one of the circuits is normally solved, and the other circuit is solved based on a virtual expansion updating period in a certain stage, so that the error characteristics of the two circuits generate a phase difference, the attitude calculation error of the IMU is averagely inhibited through data, and the high-precision attitude determination of the gravity stabilized platform is further realized.
The method can solve the problem that the error suppression algorithm of the traditional damping algorithm is invalid in the carrier maneuvering state, and effectively suppresses the Schuler oscillatory error of IMU navigation calculation.
Drawings
Fig. 1 is a flowchart of a gravity stabilized platform attitude determination method based on dual-mode solution according to an embodiment of the present invention.
Fig. 2 is a schematic diagram of a gravity stabilized platform attitude determination method based on dual-mode solution according to an embodiment of the present invention.
FIG. 3 is a schematic diagram of an oscillation characteristic of a strapdown inertial navigation error according to an embodiment of the present invention.
Fig. 4 is a schematic diagram of an error suppression effect of a damping algorithm based on a virtual expansion update period according to an embodiment of the present invention.
Fig. 5 is an east-direction velocity error suppression effect provided by an embodiment of the present invention.
Fig. 6 shows the effect of suppressing the error in the northbound speed according to the embodiment of the present invention.
Fig. 7 is a pitch angle error suppression effect provided by an embodiment of the present invention.
Fig. 8 shows the roll angle error suppression effect according to the embodiment of the present invention.
FIG. 9 is a graph of carrier velocity during a test provided by an embodiment of the present invention.
Fig. 10 is a diagram of carrier attitude during a test provided by an embodiment of the present invention.
FIG. 11 is a diagram of a second embodiment of the dynamic test method according to the present invention.
Fig. 12 is a diagram of the damping effect (measured data) of the damping algorithm in this context in a dynamic environment according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is further described in detail with reference to the following embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Aiming at the problems in the prior art, the invention provides a gravity stabilized platform attitude determination method based on dual-mode calculation and application thereof, and the invention is described in detail below with reference to the accompanying drawings.
As shown in fig. 1-2, a gravity stabilized platform attitude determination method based on dual-mode solution according to an embodiment of the present invention includes:
s101, two resolving loops are established in the same IMU, and navigation resolving is carried out on the two loops based on strapdown inertial navigation mechanics arrangement.
S102, dividing the working state of the IMU into three stages, namely a non-damping state, a damping entering state and a damping state.
S103, in an undamped state, the loop 1 and the loop 2 use real data to carry out inertial navigation calculation based on the gyroscope and accelerometer data at the current moment; entering a damping state, and performing inertial navigation resolving on the basis of an actual updating period by using real data through the loop 1; the loop 2 utilizes the virtual data to carry out inertial navigation resolving based on the virtual expanded updating period; and in the damping state, the loop 1 and the loop 2 use real original data to calculate, and the mean value of the calculation output of the loop 1 and the loop 2 is used as a final calculation result.
The two loops provided by the embodiment of the invention adopt different data to perform resolving under different states, and the resolving comprises the following steps:
one loop is normally solved, and the other loop is solved based on a virtual expansion updating period in a certain stage.
The method specifically comprises the following steps:
(1) the undamped state:
and the loop 1 and the loop 2 both utilize real data to carry out inertial navigation calculation based on the gyroscope and accelerometer data at the current moment.
(2) Entering a damping state:
the loop 1 utilizes real data to carry out inertial navigation resolving based on an actual updating period; the navigation error is X (t) ═ VE；VN；L；φx；φy；φz]。
The loop 2 utilizes the virtual data to carry out inertial navigation resolving based on the virtual expanded updating period; the navigation error is X' (t) ═ VE'；VN'；L'；φx'；φy'；φz']。
The difference in update periods between loop 1 and loop 2 results in a gradual change in the phase of X (t) and X' (t).
(3) Damping state:
when the phase lead of the loop 2 relative to the loop 1 reaches 180 degrees at a certain moment, namely the error of the two loops is reversed, the damping stage is started, the loop 1 and the loop 2 use real original data to carry out resolving, and the mean value of resolving outputs of the loop 1 and the loop 2 is used as a final resolving result.
The virtual expansion update cycle provided by the embodiment of the invention comprises the following steps:
the virtual expansion updating period represents that the inertial navigation is updated by utilizing virtual IMU data.
The virtual data is obtained by virtually expanding the update period of the real IMU original data by multiple times.
The two loops provided by the embodiment of the invention adopt different data to resolve under different states, and the resolving further comprises the following steps:
1) the undamped state: the pose, velocity and position update process of the IMU can be expressed as:
and (3) posture updating:
Figure BDA0002575746730000101
and (3) updating the speed:
Figure BDA0002575746730000102
and (3) updating the position:
Figure BDA0002575746730000103
Figure BDA0002575746730000104
Figure BDA0002575746730000105
in the formula (I), the compound is shown in the specification,
Figure BDA0002575746730000106
Figure BDA0002575746730000111
Tsis the update period of the strapdown inertial navigation.
The updating output of the navigation postures of the loop 1 and the loop 2 is as follows:
Figure BDA0002575746730000112
4) entering a damping state:
the undamped period starts from time 0 to tnEnding the moment; n1 is the navigation coordinate system of loop 1; n2 is the navigation coordinate system of loop 2; b1 is the carrier coordinate system of loop 1; b2 is the carrier coordinate system of loop 2; when the carrier is in the damping state, the carrier is in a quasi-static state, and the total duration of the damping state is 1/k of the Schuler cycle.
And (3) posture updating:
updating the attitude of the loop 1:
loop 2 attitude update:
Figure BDA0002575746730000113
wherein the loop 2 attitude update calculates the angular rate
Figure BDA0002575746730000114
From angular velocity information
Figure BDA0002575746730000115
By multiplying by a corresponding scaling factor, i.e.
Figure BDA0002575746730000116
And (3) updating the speed:
loop 2 speed update:
Figure BDA0002575746730000117
and (3) updating the position:
loop 2 location update:
Figure BDA0002575746730000118
and (3) posture updating navigation output:
loop 1 outputs:
Figure BDA0002575746730000121
the loop 2 outputs:
Figure BDA0002575746730000122
the technical solution of the present invention is further illustrated by the following specific examples.
Example 1:
the method is mainly used for researching the navigation error suppression method in the IMU pure inertia state. Because the traditional damping algorithm has contradiction in mechanism, the navigation error suppression algorithm based on the damping network fails in a carrier maneuvering state.
Taking the north velocity error as an example, the schuller oscillation modulated by the foucault oscillation is shown as a solid line in fig. 3. Aiming at the problem of navigation error suppression of a carrier in any maneuvering state, the invention provides a damping algorithm based on a virtual expansion period. The main idea of the algorithm is shown in fig. 2:
as shown in fig. 2, the algorithm proposed by the present invention divides the operating state of the IMU into three phases: the "undamped" state, the "damped" state and the "damped" state. Based on the differential equation of the inertial navigation system, two resolving loops, namely a loop 1 and a loop 2, are established on the same IMU. In the two loops, the same original data is utilized to independently and simultaneously carry out inertial navigation solution.
In the undamped state, the resolving modes of the loop 1 and the loop 2 are completely the same, and the actual updating period represents that the inertial navigation resolving is carried out by using the data of the gyroscope and the accelerometer at the current time. In the stage of 'entering damping', inertial navigation calculation is carried out in the loop 2 based on the updating period of virtual expansion. The "virtual expansion update period" indicates that the inertial navigation is updated by using virtual IMU data. The virtual data refers to data obtained by virtually expanding the update period of real IMU original data by multiple times. At the same time, in timeIn the way 1, the inertial navigation solution is performed by using real data. Notation X (t) ═ VE；VN；L；φx；φy；φz]Is the navigation error of loop 1;
X'(t)＝[VE'；VN'；L'；φx'；φy'；φz']is the navigation error of loop 2. The difference in the update periods of the two loops results in a gradual change in the phase of X (t) and X' (t). At some point, the phase advance of loop 2 relative to loop 1 will reach 180 °, i.e., the error of the two loops will be reversed. After this time, the loop 2 is solved with real raw data. The inertial navigation error X' (t) is characterized by a dashed line in fig. 3. It is noted that if the dummy data extends the update period of the real data by a factor of 2, the duration of the "enter damping" phase may be set to 2532s (i.e. half a schuller period), during which the carrier needs to be quasi-static. In the 'damping' stage, the average value of the outputs of the two loops is used as the system output, so that the Schuler oscillation error of the inertial navigation system is restrained.
In fig. 3, the thin blue solid line indicates the speed error characteristic of the loop 1 when the IMU is in the "damping" phase; the green thin dashed line indicates the speed error characteristic of loop 2 when the IMU is in the "damped" state; the red thick solid line represents the velocity error characteristics of the IMU after damping. As shown in fig. 3, the IMU navigation solution achieves significant suppression of the schuler oscillatory error compared to the loop 1 output and the output after damping.
In view of the above analysis, it is understood that the key of the algorithm proposed by the present invention lies in the virtual expansion update cycle algorithm in the design loop 2, so as to obtain the corresponding IMU output, which has the error characteristic shown by the dotted line in fig. 3.
The specific idea of the algorithm of the present invention and its mathematical expression are as follows.
The basic equation of the strapdown inertial navigation system is as follows:
Figure BDA0002575746730000131
Figure BDA0002575746730000132
Figure BDA0002575746730000133
in the "undamped" phase, the pose, velocity and position update process of the IMU can be expressed as:
and (3) posture updating:
Figure BDA0002575746730000134
and (3) updating the speed:
Figure BDA0002575746730000135
and (3) updating the position:
Figure BDA0002575746730000141
in the formula (I), the compound is shown in the specification,
Figure BDA0002575746730000142
Figure BDA0002575746730000143
Tsis the update period of the strapdown inertial navigation.
Assume that the phase "undamped" phase starts from time 0 to tnThe time is over. Note n1 as the navigation coordinate system for loop 1; n2 is the navigation coordinate system of loop 2; b1 is the carrier coordinate system of loop 1; b2 is the carrier coordinate system for loop 2. Taking the attitude update as an example, the navigation outputs of the loop 1 and the loop 2 are:
Figure BDA0002575746730000144
in the "enter damping" phase, the loop 1 performs attitude update according to equation (6.2.7). However, in the loop 2, the IMU performs navigation solution based on the update cycle in equation (7).
Figure BDA0002575746730000145
Let k be 2. Then the process of the first step is carried out,
Figure BDA0002575746730000146
therefore, the attitude update process of the loop 2 is:
Figure BDA0002575746730000147
in order to ensure that the posture update results in the formula (3) and the formula (8) are consistent. Calculating angular rate in loop 2
Figure BDA0002575746730000148
Should be that
Figure BDA0002575746730000149
1/2 of (1). Then there are:
Figure BDA00025757467300001410
therefore, the temperature of the molten metal is controlled,
Figure BDA00025757467300001411
in loop 2, the speed and position update process is:
Figure BDA00025757467300001412
Figure BDA0002575746730000151
as shown in equations (12) - (13), the calculated update period of loop 2 is twice the actual original data update period during the speed and position update. However, the specific force terms for speed, position update do not change. Thus, in the presence of linear motion of the carrier, the velocity calculated in the loop 2 is not accurate. Thus, during the "damping on" phase, the carrier needs to be in a quasi-static state. That is, angular motion is allowed, but long term, unidirectional line motion should be avoided. Thus, the speed and position updates may be made based on equations (12) and (13).
Let us assume that the duration of the "enter damping" phase is tpTaking attitude update as an example, in the "enter damping" stage, the navigation outputs of the loop 1 and the loop 2 are as follows:
loop 1 outputs:
Figure BDA0002575746730000152
the loop 2 outputs:
Figure BDA0002575746730000153
if t p2532s (half a schuller cycle), it is clear that at the end of the "enter damping" phase, the schuller oscillation phase of loop 2 leads loop 1 by half a schuller cycle. At this time, the error oscillation curves of the loop 1 and the loop 2 are approximately in a staggered state. It is also explained here that when k in equation (9) is 2, the "damping" stage is entered for 2532 s.
Let the duration of the "damping" phase be tqTake attitude update as an example. In the "damping" stage, the inertial navigation outputs of the loop 1 and the loop 2 are respectively:
loop 1 outputs:
Figure BDA0002575746730000154
the loop 2 outputs:
Figure BDA0002575746730000161
the navigational outputs of the IMU are:
Figure BDA0002575746730000162
in the formula phi1Is the attitude output of the loop 1; phi2Is the attitude output of the loop 2. These two variables can be represented by
Figure BDA0002575746730000163
And (4) determining.
Figure BDA0002575746730000164
Can be expressed as:
Figure BDA0002575746730000165
Φican be expressed as:
Figure BDA0002575746730000166
according to equations (16) - (18), during the "damping" phase, the schuller oscillation phase of loop 2 will lead loop 1 by half a schuller cycle.
The above conclusions can be theoretically illustrated based on the error equation of the inertial navigation system. In the "damping" phase, the schuller and foucault oscillation characteristics of X' (t) can be expressed as:
Figure BDA0002575746730000167
due to omegaiesinL＜＜ωs，
x'(t)≈2x0cos(ωiesinLt)·sin(ωst+π) (22)
Therefore, when the inertial measurement unit is in the "damped" state, the schuler oscillation phase difference of X' (t) and X (t) is 180 °, and the foucault oscillation phase difference is 0 °. Therefore, if the average value of the two loop outputs is taken as the system output, the schuler oscillation error can be suppressed to some extent.
The key points of the algorithm provided by the invention can be summarized as follows: (1) in the stage of 'entering damping', navigation calculation is carried out in the loop 2 based on a virtual expansion updating period, and meanwhile, angular velocity information used for attitude updating is multiplied by a corresponding proportionality coefficient; (2) the total duration of the 'damping' stage is 1/k of the Schuler cycle; (3) in the "damping on" phase, the carrier needs to be in a quasi-static state.
The technical effects of the present invention are further described below by simulation tests.
Simulation test
The error suppression effect of the damping algorithm in the pure inertia state is qualitatively demonstrated through simulation tests. The IMU is set to be stationary mounted at a latitude of 30.58 deg., and the error suppression effect shown in fig. 3 can be obtained by the proposed algorithm.
As shown in fig. 4, the thin solid line represents the output of loop 1 in the IMU; the dashed line represents the output of loop 2; the thick solid line represents the output of the damping algorithm based on the virtually extended update period. As can be seen from FIG. 4, the Schuler oscillatory error of IMU navigation solution can be effectively suppressed by the algorithm provided by the invention. (a) East-direction velocity error suppression effect; (b) a northbound speed error suppression effect; (c) a pitch angle error suppression effect; (d) and inhibiting the error of the roll angle.
The invention is further developed by the following measurement tests.
The error suppression effect of the damping algorithm based on the virtual expansion updating period is proved through the measured data, and the positive effect of the invention is further explained. Installing a navigation-level fiber optic gyroscope IMU on a vehicle, extinguishing the vehicle, getting off the vehicle by personnel, and collecting test data for 4 hours for quasi-static test of an algorithm; the vehicle is then started to run a dynamic test of the algorithm. The performance index of the inertial device of the fiber-optic gyroscope IMU used in the test is shown in table 1, and the frequency of the IMU outputting the original data is 100 Hz.
TABLE 1 Performance indices of fiber optic gyroscopes and accelerometers
Figure BDA0002575746730000171
(I) static test
The optical fiber IMU is installed on a vehicle, and the position of the optical fiber IMU is 30.58 degrees in north latitude and 114.2429 degrees in east longitude. And under the conditions that the vehicle is static and the person gets off, acquiring test data, wherein the data length is 4 hours. The calculation is carried out based on the damping algorithm provided by the invention. The results of the tests are shown in FIGS. 5-8.
Two resolving loops exist in the damping algorithm based on the virtual expansion updating period. In fig. 5 to 8, the thin solid line represents the output of the circuit 1; the dashed line represents the output of loop 2; the thick solid line represents the system output. In the resolving process of the loop 1 and the loop 2, resolving is carried out in the loop 1 based on the traditional strapdown inertial navigation mechanics arrangement, and a resolving result has obvious Schuler oscillatory errors. As shown in fig. 5-8, the damping algorithm proposed by the present invention effectively suppresses such oscillatory errors in a static situation. Comparing the velocity error peak value and the attitude error peak value in fig. 5-8, under the device noise level and the experimental conditions of the test, the damping algorithm provided by the invention reduces the velocity error by about 50% and the attitude error by more than 50% compared with the traditional strapdown inertial navigation resolving method.
(II) dynamic test
The dynamic test was performed in Wuhan City of Hubei province. In this test, a GPS antenna was mounted on the roof of the vehicle, and the result of the SINS/GPS combined navigation was used as reference information. The speed and attitude during the test are shown in fig. 9 and 10. Wherein, FIG. 9(a) the east velocity of the vector during the test; FIG. 9(b) the carrier northing speed during the test. FIG. 10(a) horizontal attitude angle of the carrier during the test; FIG. 10(b) carrier heading angle during the test.
In order to verify the error suppression effect of the algorithm provided by the invention in a dynamic environment, the following two schemes are compared:
the first scheme is as follows: non-damping calculation based on strapdown inertial navigation mechanics arrangement;
scheme II: the invention provides a damping algorithm based on a virtual expansion updating period.
According to fig. 11 and 12, the movement state of the vehicle can be divided into 5 stages. Therefore, the implementation process of the second scheme is shown in fig. 11. In the initial stage, the undamped inertial navigation is used for resolving, when the vehicle is parked for the first time, the strapdown inertial navigation is switched to a 'damping entering' stage, and then the strapdown inertial navigation is automatically switched to a 'damping' stage. The test results are shown in fig. 12. (a) East-direction velocity errors of different solutions; (b) north direction speed errors of different solutions; (c) pitch angle errors of different solutions; (d) roll angle error for different solutions.
In fig. 12, the broken line indicates the navigation solution result of the first scenario; and the black solid line represents the navigation solution result of the second scheme. Obviously, under the pure inertial state, the IMU navigation solution result based on the traditional strapdown inertial navigation mechanics arrangement has obvious Schuler oscillation error. As shown in fig. 12, the damping algorithm based on the virtual extended update period proposed herein can effectively suppress such oscillation errors in a dynamic situation. Namely, the damping algorithm provided by the invention can effectively improve the navigation resolving precision in the IMU pure inertia state. Comparing the velocity error peak value and the attitude error peak value in fig. 12, it can be known that, under the device noise level and the experimental conditions of the test, the velocity error is reduced by about 50% and the attitude error is reduced by more than 50% in the damping algorithm provided herein compared with the conventional strapdown inertial navigation solution method.
It should be noted that the embodiments of the present invention can be realized by hardware, software, or a combination of software and hardware. The hardware portion may be implemented using dedicated logic; the software portions may be stored in a memory and executed by a suitable instruction execution system, such as a microprocessor or specially designed hardware. Those skilled in the art will appreciate that the apparatus and methods described above may be implemented using computer executable instructions and/or embodied in processor control code, such code being provided on a carrier medium such as a disk, CD-or DVD-ROM, programmable memory such as read only memory (firmware), or a data carrier such as an optical or electronic signal carrier, for example. The apparatus and modules thereof of the present invention may be implemented by hardware circuits such as very large scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, or programmable hardware devices such as field programmable gate arrays, programmable logic devices, or software executed by various types of processors, or a combination of the above hardware circuits and software, e.g., firmware
The above description is only for the purpose of illustrating the present invention and the appended claims are not to be construed as limiting the scope of the invention, which is intended to cover all modifications, equivalents and improvements that are within the spirit and scope of the invention as defined by the appended claims.

Claims (7)

1. A gravity stabilized platform attitude determination method based on dual-mode calculation is characterized by comprising the following steps of:
two resolving loops established in the same IMU adopt different data to resolve under different working states of the IMU, phase difference is generated, IMU attitude resolving errors are inhibited through data averaging, and the attitude of the gravity stabilization platform is determined.
2. The dual-mode-calculation-based attitude determination method for the gravity stabilization platform, according to claim 1, characterized in that in the calculation of the two loops by adopting different data in different states, one loop is normally calculated, and the other loop is calculated in a certain stage based on a virtual expansion update cycle;
the method specifically comprises the following steps:
(1) the undamped state:
the circuit 1 and the circuit 2 both utilize real data to carry out inertial navigation calculation based on the gyroscope and accelerometer data at the current moment;
(2) entering a damping state:
the loop 1 utilizes real data to carry out inertial navigation resolving based on an actual updating period; the navigation error is X (t) ═ VE；VN；L；φx；φy；φz]；
The loop 2 utilizes the virtual data to carry out inertial navigation resolving based on the virtual expanded updating period; the navigation error is X' (t) ═ VE'；VN'；L'；φx'；φy'；φz']；
The difference between the update periods of loop 1 and loop 2 results in a gradual change in the phase of X (t) and X' (t);
(3) damping state:
at a certain moment, the phase lead of the loop 2 relative to the loop 1 reaches 180 degrees, namely, after errors of the two loops are reversed, the two loops enter a damping stage, the loop 1 and the loop 2 use real original data to carry out resolving, and the mean value of resolving outputs of the loop 1 and the loop 2 is used as a final resolving result.
3. The dual-mode-resolution-based gravity-stabilized platform attitude determination method according to claim 2, wherein the virtual expansion update period comprises:
the virtual expansion updating period represents that inertial navigation is updated by virtual IMU data;
the virtual data is obtained by virtually expanding the update period of the real IMU original data by multiple times.
4. The dual-mode-resolution-based attitude determination method for the gravity stabilized platform according to claim 2, wherein the two loops performing resolution using different data in different states further comprises:
1) the undamped state: the pose, velocity and position update process of the IMU can be expressed as:
and (3) posture updating:
Figure FDA0002575746720000021
and (3) updating the speed:
Figure FDA0002575746720000022
and (3) updating the position:
Figure FDA0002575746720000023
Figure FDA0002575746720000024
Figure FDA0002575746720000025
in the formula (I), the compound is shown in the specification,
Figure FDA0002575746720000026
Figure FDA0002575746720000027
Tsupdating a strapdown inertial navigation period;
the updating output of the navigation postures of the loop 1 and the loop 2 is as follows:
Figure FDA0002575746720000028
2) entering a damping state:
the undamped period starts from time 0 to tnEnding the moment; n1 is the navigation coordinate system of loop 1; n2 is the navigation coordinate system of loop 2; b1 is the carrier coordinate system of loop 1; b2 is the carrier coordinate system of loop 2; when the carrier is in a damping state, the carrier is in a quasi-static state, and the total duration of the carrier in the damping state is 1/k of the Schuler cycle;
and (3) posture updating:
updating the attitude of the loop 1:
loop 2 attitude update:
Figure FDA0002575746720000031
wherein the loop 2 attitude update calculates the angular rate
Figure FDA0002575746720000032
From angular velocity information
Figure FDA0002575746720000033
By multiplying by a corresponding scaling factor, i.e.
Figure FDA0002575746720000034
And (3) updating the speed:
loop 2 speed update:
Figure FDA0002575746720000035
and (3) updating the position:
loop 2 location update:
Figure FDA0002575746720000036
and (3) posture updating navigation output:
loop 1 outputs:
Figure FDA0002575746720000037
the loop 2 outputs:
Figure FDA0002575746720000038
5. a program storage medium for receiving user input, the stored computer program causing an electronic device to perform the dual-mode-solution-based gravity-stabilized platform attitude determination method of any one of claims 1 to 4.
6. A computer program product stored on a computer readable medium, comprising a computer readable program for providing a user input interface for implementing a dual-mode solution based gravity-stabilized platform attitude determination method as claimed in any one of claims 1 to 4 when executed on an electronic device.
7. An IMU for executing the dual-mode-solution-based gravity-stabilized platform attitude determination method according to any one of claims 1 to 4.
CN202010653207.4A 2020-07-08 2020-07-08 Gravity stabilization platform attitude determination method based on dual-mode calculation and application Pending CN111780758A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010653207.4A CN111780758A (en) 2020-07-08 2020-07-08 Gravity stabilization platform attitude determination method based on dual-mode calculation and application

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010653207.4A CN111780758A (en) 2020-07-08 2020-07-08 Gravity stabilization platform attitude determination method based on dual-mode calculation and application

Publications (1)

Publication Number Publication Date
CN111780758A true CN111780758A (en) 2020-10-16

Family

ID=72758366

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010653207.4A Pending CN111780758A (en) 2020-07-08 2020-07-08 Gravity stabilization platform attitude determination method based on dual-mode calculation and application

Country Status (1)

Country Link
CN (1) CN111780758A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112129289A (en) * 2020-11-30 2020-12-25 中国人民解放军国防科技大学 Fault-tolerant horizontal damping method based on output correction
CN112697139A (en) * 2020-12-08 2021-04-23 中国人民解放军海军工程大学 Fiber-optic gyroscope strapdown inertial navigation damping method and system, carrier machine, terminal and application

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8275544B1 (en) * 2005-11-21 2012-09-25 Miltec Missiles & Space Magnetically stabilized forward observation platform
CN103925924A (en) * 2014-04-25 2014-07-16 哈尔滨工程大学 Damping switch delay and overshoot control method of space stable inertial navigation system
US20150219460A1 (en) * 2013-07-22 2015-08-06 Airbus Operations S.A.S. Device and method for prediction on the ground of characteristics of the position of an aircraft along a path
EP2939857A2 (en) * 2014-04-11 2015-11-04 Fox Factory, Inc. Method and apparatus for an adjustable damper
RU2617565C1 (en) * 2015-12-02 2017-04-25 Акционерное общество "Раменское приборостроительное конструкторское бюро" Method of inertial data estimation and its correction according to measurement of satellite navigation system
CN107941216A (en) * 2017-09-18 2018-04-20 哈尔滨工程大学 A kind of Strapdown Inertial Navigation System outer level damp method based on adaptive complementary filter
CN108680186A (en) * 2018-05-17 2018-10-19 中国人民解放军海军工程大学 Methods of Strapdown Inertial Navigation System nonlinear initial alignment method based on gravimeter platform
CN110160524A (en) * 2019-05-23 2019-08-23 深圳市道通智能航空技术有限公司 A kind of the sensing data acquisition methods and device of inertial navigation system
CN110426033A (en) * 2019-07-30 2019-11-08 上海理工大学 Time synchronization algorithm based on loose coupling IMU array navigation system
CN110926464A (en) * 2019-12-11 2020-03-27 中国人民解放军海军潜艇学院 Inertial navigation method and system based on dual modes

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8275544B1 (en) * 2005-11-21 2012-09-25 Miltec Missiles & Space Magnetically stabilized forward observation platform
US20150219460A1 (en) * 2013-07-22 2015-08-06 Airbus Operations S.A.S. Device and method for prediction on the ground of characteristics of the position of an aircraft along a path
EP2939857A2 (en) * 2014-04-11 2015-11-04 Fox Factory, Inc. Method and apparatus for an adjustable damper
CN103925924A (en) * 2014-04-25 2014-07-16 哈尔滨工程大学 Damping switch delay and overshoot control method of space stable inertial navigation system
RU2617565C1 (en) * 2015-12-02 2017-04-25 Акционерное общество "Раменское приборостроительное конструкторское бюро" Method of inertial data estimation and its correction according to measurement of satellite navigation system
CN107941216A (en) * 2017-09-18 2018-04-20 哈尔滨工程大学 A kind of Strapdown Inertial Navigation System outer level damp method based on adaptive complementary filter
CN108680186A (en) * 2018-05-17 2018-10-19 中国人民解放军海军工程大学 Methods of Strapdown Inertial Navigation System nonlinear initial alignment method based on gravimeter platform
CN110160524A (en) * 2019-05-23 2019-08-23 深圳市道通智能航空技术有限公司 A kind of the sensing data acquisition methods and device of inertial navigation system
CN110426033A (en) * 2019-07-30 2019-11-08 上海理工大学 Time synchronization algorithm based on loose coupling IMU array navigation system
CN110926464A (en) * 2019-12-11 2020-03-27 中国人民解放军海军潜艇学院 Inertial navigation method and system based on dual modes

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
HE, HY (HE HONGYANG)等: "Research on generalized inertial navigation system damping technology based on dual-model mean", 《PROCEEDINGS OF THE INSTITUTION OF MECHANICAL ENGINEERS PART G-JOURNAL OF AEROSPACE ENGINEERING》 *
STELLA, L.等: "Lense-thirring precession and QPOs in low mass x-ray binaries", 《NUCLEAR PHYSICS B》 *
何泓洋等: "一种捷联惯导阻尼超调误差抑制算法研究", 《舰船电子工程》 *
查峰等: "光学陀螺捷联惯性系统的发展与展望", 《激光与光电子学进展》 *
顾鹏飞: "激光陀螺捷联姿态测量系统内阻尼技术研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112129289A (en) * 2020-11-30 2020-12-25 中国人民解放军国防科技大学 Fault-tolerant horizontal damping method based on output correction
CN112129289B (en) * 2020-11-30 2021-02-05 中国人民解放军国防科技大学 Fault-tolerant horizontal damping method based on output correction
CN112697139A (en) * 2020-12-08 2021-04-23 中国人民解放军海军工程大学 Fiber-optic gyroscope strapdown inertial navigation damping method and system, carrier machine, terminal and application

Similar Documents

Publication Publication Date Title
CA2530903C (en) Method and system for improving accuracy of inertial navigation measurements using measured and stored gravity gradients
CN107655493B (en) SINS six-position system-level calibration method for fiber-optic gyroscope
CN110044376B (en) Correction method and device for inertial navigation equipment
CN105806340B (en) A kind of adaptive Zero velocity Updating algorithm smooth based on window
CN109596144B (en) GNSS position-assisted SINS inter-travel initial alignment method
CN101701825A (en) High-precision laser gyroscope single-shaft rotating inertial navigation system
CN101713666B (en) Single-shaft rotation-stop scheme-based mooring and drift estimating method
CN111780758A (en) Gravity stabilization platform attitude determination method based on dual-mode calculation and application
CN110940340A (en) Multi-sensor information fusion method based on small UUV platform
CN103245357A (en) Secondary quick alignment method of marine strapdown inertial navigation system
CN103674064A (en) Initial calibration method of strapdown inertial navigation system
RU2324897C1 (en) Azimuthal orientation of free gyro platform by precession angle of gyro unit
CN113959462B (en) Quaternion-based inertial navigation system self-alignment method
CN114001731B (en) Polar region inertial navigation phase modulation damping method and system under virtual sphere model
Zheng et al. Compensation for stochastic error of gyros in a dual-axis rotational inertial navigation system
US8566055B1 (en) Gyro indexing compensation method and system
Nie et al. Research on accuracy improvement of INS with continuous rotation
Wang et al. Research on innovative self-calibration strategy for error parameters of dual-axis RINS
RU2723976C1 (en) Method for determining angular orientation of ground vehicle
CN112882118B (en) Method and system for estimating gravity vector of movable base under earth-fixed coordinate system and storage medium
CN102221366B (en) Quick accurate alignment method based on fuzzy mapping earth spin velocity
Fangjun et al. Phase modulation-based SINS damping method for autonomous vehicles
Fu et al. Multiposition alignment for rotational INS based on real-time estimation of inner lever arms
Cheng et al. Research on nonlinear comprehensive calibration algorithm for the single-axis rotation inertial navigation system based on modified unscented kalman filter
Ben et al. System reset for underwater strapdown inertial navigation system

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