Disclosure of Invention
The application provides an autonomous error suppression method for a micro inertial navigation system, which can solve the technical problem that the performance of the micro inertial navigation system is reduced because the gyroscope scale factor is greatly changed due to the projectile emission impact with high measurement level and long duration time in the prior art.
According to an aspect of the application, there is provided an autonomous error suppression method for a micro inertial navigation system, the method comprising:
acquiring the rolling angle of the projectile body in real time by utilizing a micro inertial navigation system;
constructing a least square state variable according to the rolling angle of the alignment initial moment;
constructing a least square observation variable according to the rolling angle at the current moment;
acquiring a least square measurement equation according to the least square state variable and the least square observation variable, and carrying out least square estimation according to the least square measurement equation to obtain a rolling angle estimation value aiming at the initial moment;
a Kalman filtering state equation is constructed by taking the rolling angle error, the gyro scale factor error and the gyro zero position as Kalman filtering state variables;
constructing a Kalman filtering observation equation by taking a difference value of rolling angle estimated values of alignment initial moments obtained by least square estimation of adjacent filtering moments as a Kalman filtering observation variable;
and carrying out Kalman filtering according to a Kalman filtering state equation and a Kalman filtering observation equation to obtain a gyro scale factor error estimated value and a corrected gyro scale factor, thereby realizing autonomous suppression of the micro inertial navigation system error.
Preferably, the Kalman filter state equation is constructed by:
wherein ,
in the formula ,is Kalman filtering state variable, F is continuous state equation state transition matrix, +.>Is the random noise vector of the system, delta gamma is the rolling angle error, delta k x For the error of the scale factor of the X-axis gyro epsilon bx Is X-axis gyro zero position->The rotational axis angular rate is sensitive to the X-axis gyroscope.
Preferably, the Kalman filter observation equation is constructed by:
in the formula ,for Kalman filtering observation variables, +.>For Kalman filtering observation matrix, +.>For observing noise +.>Rolling angle estimation value of alignment initial time obtained for least square estimation of current filtering time,And (3) estimating a rolling angle estimation value of the alignment initial moment for least square estimation of the last filtering moment.
Preferably, the real-time obtaining the roll angle of the projectile body by using the micro inertial navigation system comprises:
acquiring angular rate information under a navigation coordinate system output by a micro inertial navigation system;
acquiring angular rate information under a carrier coordinate system according to the angular rate information under the navigation coordinate system;
and acquiring the rolling angle of the projectile body according to the angular rate information under the carrier coordinate system.
Preferably, the angular rate information in the carrier coordinate system is obtained by:
wherein ,
the roll angle of the projectile was obtained by:
wherein ,
in the formula ,for the carrier coordinate system X-axis gyro angular rate, < >>For the carrier coordinate system Y-axis gyro angular rate, < >>For the carrier coordinate system Z-axis gyro angular rate, < >>For scrolling angular rate +.>For course angular rate, ++>For pitch rate, c 1 C for a conversion matrix rotating around the X-axis 3 For a conversion matrix rotating around the Z axis, gamma is the roll angle of the projectile, theta is the pitch angle of the projectile, +.>Is the heading angle of the projectile.
Preferably, the least squares state variable is constructed by:
X=[cos(γ 0 ) sin(γ 0 )] T ;
the least squares observed variable is constructed by:
Z=[sin(γ) cos(γ)] T ;
wherein X is a least squares state variable, gamma 0 To align the roll angle at the initial time, Z is the least squares observation variable and γ is the roll angle at the current time.
Preferably, obtaining a least square measurement equation according to the least square state variable and the least square observation variable, and performing least square estimation according to the least square measurement equation, the obtaining a roll angle estimation value aligned with the initial time includes:
acquiring a least square observation matrix according to the least square state variable and the least square observation variable;
acquiring a least square measurement equation according to the least square state variable, the least square observation variable and the least square observation matrix;
acquiring a least square measurement equation set according to the least square measurement equation, wherein the least square measurement equation set comprises r least square measurement equations, and r is the measurement times during alignment;
and obtaining the estimated value of the least square state variable according to the least square measurement equation set, thereby obtaining the estimated value of the rolling angle of the alignment initial moment obtained by least square estimation.
Preferably, the least squares observation matrix is obtained by:
the least squares measurement equation is obtained by:
Z=HX+V;
obtaining a least square measurement equation set through the following steps:
wherein H is a least squares observation matrix, t is time, V is measurement noise, Z 1 、Z 2 、......、Z r The least squares observed variable, H, measured first, second, &..the r-th time, respectively 1 、H 2 、......、H r A least squares observation matrix, V, of the first, second, & gt 1 、V 2 、......、V r The measurement noise of the first, second, and r-th measurements, respectively.
Preferably, the estimated value of the least squares state variable is obtained by:
the roll angle estimation value of the alignment initial moment obtained by least square estimation is obtained by the following formula:
in the formula ,estimated value for least squares state variable, < >>For aligning the roll angle estimate at the initial moment, +.>Is the first element of the estimate of the least squares state variable,/for>Is the second element of the estimate of the least squares state variable.
According to a further aspect of the present application there is provided a computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing any of the methods described above when executing the computer program.
By applying the technical scheme of the application, the rolling angle of the projectile body is obtained in real time by utilizing the micro inertial navigation system, the rolling angle estimated value at the initial alignment time is estimated by least square, then a Kalman filtering method is adopted, and the difference value of the rolling angle estimated values at the initial alignment time obtained by least square estimation at the adjacent filtering time is used as a Kalman filtering observation variable to obtain the gyro scale factor error estimated value and the corrected gyro scale factor, thereby realizing the autonomous suppression of the micro inertial navigation system error and improving the navigation precision of the micro inertial navigation system. The method is convenient for engineering realization and does not depend on external auxiliary information, can be widely applied to the field of guided missiles rotating at high speed, and has very important significance for a micro inertial navigation system for guided missiles.
Detailed Description
It should be noted that, without conflict, the embodiments of the present application and features of the embodiments may be combined with each other. The following description of the embodiments of the present application will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present application, but not all embodiments. The following description of at least one exemplary embodiment is merely exemplary in nature and is in no way intended to limit the application, its application, or uses. All other embodiments, which can be made by those skilled in the art based on the embodiments of the application without making any inventive effort, are intended to be within the scope of the application.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of exemplary embodiments according to the present application. As used herein, the singular is also intended to include the plural unless the context clearly indicates otherwise, and furthermore, it is to be understood that the terms "comprises" and/or "comprising" when used in this specification are taken to specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof.
The relative arrangement of the components and steps, numerical expressions and numerical values set forth in these embodiments do not limit the scope of the present application unless it is specifically stated otherwise. Meanwhile, it should be understood that the sizes of the respective parts shown in the drawings are not drawn in actual scale for convenience of description. Techniques, methods, and apparatus known to one of ordinary skill in the relevant art may not be discussed in detail, but should be considered part of the specification where appropriate. In all examples shown and discussed herein, any specific values should be construed as merely illustrative, and not a limitation. Thus, other examples of the exemplary embodiments may have different values. It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further discussion thereof is necessary in subsequent figures.
As shown in fig. 1, the application provides an autonomous error suppression method for a micro inertial navigation system, which comprises the following steps:
s10, acquiring a rolling angle of the projectile body in real time by utilizing a micro inertial navigation system;
s20, constructing a least square state variable according to the rolling angle of the alignment initial moment;
s30, constructing a least square observation variable according to the rolling angle at the current moment;
s40, acquiring a least square measurement equation according to the least square state variable and the least square observation variable, and carrying out least square estimation according to the least square measurement equation to obtain a rolling angle estimated value aligned with the initial moment;
s50, constructing a Kalman filtering state equation by taking the rolling angle error, the gyro scale factor error and the gyro zero position as Kalman filtering state variables;
s60, constructing a Kalman filtering observation equation by taking a difference value of rolling angle estimated values of alignment initial moments obtained by least square estimation of adjacent filtering moments as a Kalman filtering observation variable;
s70, kalman filtering is carried out according to a Kalman filtering state equation and a Kalman filtering observation equation to obtain a gyro scale factor error estimated value and a corrected gyro scale factor, so that autonomous suppression of micro inertial navigation system errors is realized.
According to the application, the rolling angle of the projectile body is obtained in real time by utilizing the micro inertial navigation system, the rolling angle estimated value of the alignment initial moment is estimated by least square, then a Kalman filtering method is adopted, and the difference value of the rolling angle estimated values of the alignment initial moment obtained by least square estimation of adjacent filtering moments is used as a Kalman filtering observation variable to obtain a gyro scale factor error estimated value and a corrected gyro scale factor, so that the autonomous suppression of the micro inertial navigation system error is realized, and the navigation precision of the micro inertial navigation system is improved. The method is convenient for engineering realization and does not depend on external auxiliary information, can be widely applied to the field of guided missiles rotating at high speed, and has very important significance for a micro inertial navigation system for guided missiles.
According to one embodiment of the present application, in S10 of the present application, acquiring the roll angle of the projectile in real time using the micro inertial navigation system includes:
s11, acquiring angular rate information under a navigation coordinate system output by a micro inertial navigation system;
s12, acquiring angular rate information under a carrier coordinate system according to the angular rate information under the navigation coordinate system;
s13, acquiring the rolling angle of the projectile body according to the angular rate information under the carrier coordinate system.
Specifically, in S12 of the present application, the angular rate information in the carrier coordinate system is obtained by the following formula:
wherein ,
will c 1 、c 3 Substituting the above formula and expanding to obtain:
from the above, it is possible to obtain
Order theThen it can be derived from the above equation:
the roll angle of the projectile can be calculated from the above equation as follows:
in the formula ,for the carrier coordinate system X-axis gyro angular rate, < >>For the carrier coordinate system Y-axis gyro angular rate, < >>For the carrier coordinate system Z-axis gyro angular rate, < >>For scrolling angular rate +.>For course angular rate, ++>For pitch rate, c 1 C for a conversion matrix rotating around the X-axis 3 For a transition matrix rotating about the Z-axis, gamma is the roll angle of the projectile and θ is the projectilePitch angle of->Is the heading angle of the projectile.
According to the application, as can be seen from the analysis, the measurement errors of the angular rates of the Y-axis gyroscope and the Z-axis gyroscope can greatly influence the calculation result of the rolling angle, and the calculation result is singular due to the influence of factors such as gyroscope measurement noise and the like by directly calculating by using the method, so that the rolling angle is estimated by using a least square algorithm, and the estimation precision and reliability of the rolling angle are improved.
According to one embodiment of the present application, in S20 of the present application, a least squares state variable is constructed by:
X=[cos(γ 0 ) sin(γ 0 )] T ;
in S30 of the present application, a least squares observation variable is constructed by:
Z=[sin(γ) cos(γ)] T ;
wherein X is a least squares state variable, gamma 0 To align the roll angle at the initial time, Z is the least squares observation variable and γ is the roll angle at the current time.
According to one embodiment of the present application, in S40 of the present application, obtaining a least square measurement equation according to a least square state variable and a least square observation variable, and performing least square estimation according to the least square measurement equation, obtaining a roll angle estimation value aligned to an initial time includes:
s41, acquiring a least square observation matrix according to a least square state variable and a least square observation variable;
s42, acquiring a least square measurement equation according to the least square state variable, the least square observation variable and the least square observation matrix;
s43, acquiring a least square measurement equation set according to the least square measurement equation, wherein the least square measurement equation set comprises r least square measurement equations, and r is the measurement times during the alignment period;
s44, obtaining an estimated value of the least square state variable according to the least square measurement equation set, and accordingly obtaining a rolling angle estimated value of the alignment initial moment obtained through least square estimation.
Specifically, in S41 of the present application, the least squares observation matrix is obtained by:
in S42 of the present application, the least squares measurement equation is obtained by:
Z=HX+V;
in S43 of the present application, a system of least squares measurement equations is obtained by:
wherein H is a least squares observation matrix, t is time, V is measurement noise, Z 1 、Z 2 、......、Z r The least squares observed variable, H, measured first, second, &..the r-th time, respectively 1 、H 2 、......、H r A least squares observation matrix, V, of the first, second, & gt 1 、V 2 、......、V r The measurement noise of the first, second, and r-th measurements, respectively.
Further, in S44 of the present application, the estimated value of the least squares state variable is obtained by:
the roll angle estimation value of the alignment initial moment obtained by least square estimation is obtained by the following formula:
in the formula ,estimated value for least squares state variable, < >>For aligning the roll angle estimate at the initial moment, +.>Is the first element of the estimate of the least squares state variable,/for>Is the second element of the estimate of the least squares state variable.
According to one embodiment of the present application, in S50 of the present application, a kalman filter state equation is constructed by:
wherein ,
in the formula ,is Kalman filtering state variable, F is continuous state equation state transition matrix, +.>Is the random noise vector of the system, delta gamma is the rolling angle error, delta k x For the error of the scale factor of the X-axis gyro epsilon bx Is X-axis gyro zero position->The rotational axis angular rate is sensitive to the X-axis gyroscope.
According to one embodiment of the present application, in S60 of the present application, a kalman filter observation equation is constructed by:
in the formula ,for Kalman filtering observation variables, +.>For Kalman filtering observation matrix, +.>For observing noise +.>Rolling angle estimation value of alignment initial time obtained for least square estimation of current filtering time,And (3) estimating a rolling angle estimation value of the alignment initial moment for least square estimation of the last filtering moment.
The application also provides a computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing any of the methods described above when executing the computer program.
Taking certain model flight test data as an example, the method is used for estimating the gyro scale factor error to obtain a Kalman filtering estimation result as shown in figure 2.
As can be seen from FIG. 2, the gyro scale factor error can be estimated rapidly, and the gyro scale factor is corrected by using the result of estimation, so that the measurement accuracy of the micro inertial navigation system can be improved greatly.
In summary, the application provides an autonomous error suppression method for a micro inertial navigation system, which comprises the steps of acquiring a roll angle of a projectile body in real time by using the micro inertial navigation system, estimating a roll angle estimated value at an alignment initial moment by least square, and using a Kalman filtering method to acquire a gyro scale factor error estimated value and a corrected gyro scale factor by using a difference value of the roll angle estimated values at the alignment initial moment, which is obtained by least square estimation at adjacent filtering moments, as a Kalman filtering observation variable, thereby realizing autonomous error suppression of the micro inertial navigation system and improving navigation precision of the micro inertial navigation system. The method is convenient for engineering realization and does not depend on external auxiliary information, can be widely applied to the field of guided missiles rotating at high speed, and has very important significance for a micro inertial navigation system for guided missiles.
Spatially relative terms, such as "above … …," "above … …," "upper surface at … …," "above," and the like, may be used herein for ease of description to describe one device or feature's spatial location relative to another device or feature as illustrated in the figures. It will be understood that the spatially relative terms are intended to encompass different orientations in use or operation in addition to the orientation depicted in the figures. For example, if the device in the figures is turned over, elements described as "above" or "over" other devices or structures would then be oriented "below" or "beneath" the other devices or structures. Thus, the exemplary term "above … …" may include both orientations of "above … …" and "below … …". The device may also be positioned in other different ways (rotated 90 degrees or at other orientations) and the spatially relative descriptors used herein interpreted accordingly.
In addition, the terms "first", "second", etc. are used to define the components, and are only for convenience of distinguishing the corresponding components, and the terms have no special meaning unless otherwise stated, and therefore should not be construed as limiting the scope of the present application.
The above description is only of the preferred embodiments of the present application and is not intended to limit the present application, but various modifications and variations can be made to the present application by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the protection scope of the present application.