CN104215244A - Aerospace vehicle combined navigation robust filtering method based on launching inertia coordinate system - Google Patents

Abstract

The invention discloses an aerospace vehicle combined navigation robust filtering method based on a launching inertia coordinate system, belonging to the technical field of air vehicle combined navigation. The aerospace vehicle combined navigation robust filtering method comprises the following steps of performing inertial navigation algorithm arrangement by taking the launching inertia coordinate system as a reference coordinate system, and constructing a position, speed and posture calculation model of an air vehicle under the launching inertia coordinate system; on the basis of the position, speed and posture calculation model, constructing an equation of navigation system error states including a basic navigation parameter error and an inertia instrument error, and obtaining a posture linear measurement equation through a conversion module according to original star sensor measurement information; and finally estimating and correcting all state values in the constructed state equation by the robust filtering method. According to the aerospace vehicle combined navigation robust filtering method disclosed by the invention, the influence, caused by a geophysical field, on the navigation system of the air vehicle can be effectively reduced; furthermore, under a condition that the navigation system model of the air vehicle is inaccurate, a filter can work stably; therefore, the precision of a combined navigation system is improved.

Description

Based on the re-entry space vehicle integrated navigation robust filtering method of launch inertial coordinate system

Technical field

The invention discloses a kind of re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system, relate to integrated navigation technology field.

Background technology

In recent years, along with the development of aeronautical and space technology, when the course of new aircraft such as near space hypersonic aircraft, re-entry space vehicle present long boat, the flight characteristic of large spatial domain, it improves day by day to the requirement of navigational system performance.

Tradition aviation aircraft is that reference frame carries out strap inertial navigation algorithm layout with geographic coordinate usually, in the layout of strapdown inertial navigation system algorithm, gravity field, radius-of-curvature change etc. are adopted and directly ignore or simplify processes, mainly consider that Department of Geography's layout is comparatively directly perceived, and in short time, short distance in-flight, this disposal route is little to navigational system Accuracy.And the course of new aircraft flight time such as near space hypersonic vehicle, re-entry space vehicle are long, flying distance is far away, terrestrial gravitation, radius-of-curvature etc. change greatly.If these factors are ignored or simplify processes in aircraft flight, navigation accuracy will be affected to a great extent.

In addition, the simple navigation information relying on inertial navigation system to provide is difficult to meet the navigation needs of aircraft, needs to adopt integrated navigation mode to provide navigation information.Merge field at navigation information, kalman filtering, as a kind of linear minimum-variance estimation method, is widely used, and it requires that system model must be accurately known and input noise is strict Guass noise.And the flight environment of vehicle that re-entry space vehicle faces is complicated, uncertain factor is more, usually can bring unpredictable impact to navigational system, is difficult to the accurate model obtaining navigational system.Under these conditions, the filtering performance of Kalman filter will be restricted.Therefore, study the re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system, can effectively reduce on the one hand because geophysical field describes inaccurate and impact that is that bring navigational system, the robust performance of integrated navigation system can be improved on the other hand, there is outstanding application prospect.

Summary of the invention

Technical matters to be solved by this invention is: for the defect of prior art, a kind of re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system is provided, the present invention take launch inertial coordinate system as reference frame, carry out the layout of inertial navigation algorithm, and adopt robust filtering method estimate Navigation system error and revise.

The present invention is for solving the problems of the technologies described above by the following technical solutions:

Based on a re-entry space vehicle integrated navigation robust filtering method for launch inertial coordinate system, described method concrete steps comprise:

Step one, be reference frame with launch inertial coordinate system, carry out the layout of inertial navigation algorithm, set up the speed of aircraft under launch inertial coordinate system, position, attitude algorithm model;

Step 2, according to the speed of aircraft in step one under launch inertial coordinate system, position, attitude algorithm model, obtain corresponding speed, position, attitude error model, in conjunction with the error model of inertia type instrument, build the error state equation and the linearization measurement equation that comprise the inertial navigation system of basic navigation parameter error and inertia type instrument error;

Step 3, the error state equation of the inertial navigation system that step 2 draws and linearization measurement equation carried out to the renewal of sliding-model control and quantity of state, measuring amount, adopt robust filtering method estimate each quantity of state in set up state equation and revise.

As present invention further optimization scheme, the expression-form resolving model described in step one is specially:

(101) the velocity calculated model of aircraft under launch inertial coordinate system is:

${\stackrel{\·}{v}}^l=C_b^l\·f^b+C_i^l\·G^i---\left(1\right)$

Wherein, v ^lfor aircraft is relative to the speed of launching inertial system, for v ^lfirst order derivative, f ^bfor the specific force that accelerometer exports, G ⁱfor the earth universal gravitation under geocentric inertial coordinate system, for carrier coordinate system b is tied to the pose transformation matrix of launch inertial coordinate system l system, for geocentric inertial coordinate system i is tied to the pose transformation matrix of launch inertial coordinate system l system;

(102) the location compute model of aircraft under launch inertial coordinate system is:

${\stackrel{\·}{p}}^l=v^l---\left(2\right)$

Wherein, p ^lfor aircraft is relative to the position of launch inertial coordinate system, for p ^lfirst order derivative;

(103) the attitude algorithm model of aircraft under launch inertial coordinate system is:

$\stackrel{\·}{q}=0.5q\⊗{\mathrm{\ω}}_{\mathrm{lb}}^b---\left(3\right)$

Wherein, q is the attitude quaternion of aircraft relative to launching inertial system, for the first order derivative of q, for the projection that carrier is fastened at carrier relative to the angular speed of launching inertial system, represent hypercomplex number multiplication.

As present invention further optimization scheme, in the error state equation of the inertial navigation system described in step 2, the form that embodies of error state variable X is:

X＝[δq ₁ δq ₂ δq ₃ δp _x δp _y δp _z δv _x δv _y δv _zε _rx ε _ry ε _rz ▽ _ax ▽ _ay ▽ _az] ^T (4)

In formula (4), δ q ₁, δ q ₂, δ q ₃be respectively the vector section of attitude quaternion error delta q in INS errors, δ p _x, δ p _y, δ p _zbe respectively the site error quantity of state of X-axis in INS errors, Y-axis, Z-direction, δ v _x, δ v _y, δ v _zbe respectively the velocity error quantity of state of X-axis in inertial navigation system, Y-axis, Z-direction, ε _rx, ε _ry, ε _rzbe respectively the Gyro Random migration error of X-axis, Y-axis, Z-direction, ▽ _ax, ▽ _ay, ▽ _azbe respectively the accelerometer random walk error of X-axis, Y-axis, Z-direction.

As present invention further optimization scheme, the error state equation of described inertial navigation system is embodied as:

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(5\right)$

In formula (5), X (t) is system state variables, for the first order derivative of X (t), A (t) is system matrix; B (t) is noise figure matrix, and W (t) is noise matrix;

The carrier drawn according to star sensor measurement is relative to the attitude information of inertial coordinates system, the attitude information of carrier relative to launch inertial coordinate system is drawn through conversion, adopt attitude linearization Observation principle under launch inertial coordinate system, set up the linearization measurement equation between attitude observation under launch inertial coordinate system and INS errors quantity of state:

Z(t)＝H _a(t)X(t)+N _s(t) (6)

In formula (6), Z (t) is posture observed quantity matrix, H _at () is attitude measurement matrix of coefficients, N _st () is attitude observation noise battle array.

As present invention further optimization scheme, the employing robust filtering method described in step 3 is estimated each quantity of state in set up state equation and revises, and its concrete steps comprise:

(301) by the error state equation of inertial navigation system and linearization measurement equation sliding-model control:

X _k＝A _kX _k-1+B _kW _k-1 (7)

Z _k＝H _kX _k+N _k (8)

In above-mentioned formula, X _kfor t _kmoment system state amount, X _k-1for t _k-1moment system state amount, A _kfor t _k-1moment is to t _ktime etching system state-transition matrix, B _kfor t _k-1moment is to t _ktime etching system noise drive matrix, W _k-1for t _ktime etching system noise matrix, Z _kfor t _kmoment posture observed quantity matrix, H _kfor t _ktime attitude measure matrix of coefficients, N _kfor t _kthe noise matrix of moment attitude observation;

(302) robust filtering strategy is adopted to estimate INS errors:

${\stackrel{\‾}{S}}_k=L_k^T{S}_k{L}_k---\left(9\right)$

$K_k=A_k{P}_k{[I-\mathrm{\θ}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{H}_k^T{R}_k^{-1}---\left(10\right)$

${\hat{X}}_{k+1}=A_k{\hat{X}}_k+A_k{K}_k(Z_k-H_k{\hat{X}}_k)---\left(11\right)$

$P_{k+1}=A_k{P}_k{[I-\mathrm{\θ}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{A}_k^T+Q_k---\left(12\right)$

In above-mentioned formula, L _kfor the linear combination coefficient matrix of quantity of state, S _kfor weight coefficient matrix, for the weight coefficient matrix after conversion, K _kfor t _kthe filter gain matrix of the moment band robust filtering factor, θ is the robust filtering factor, P _kfor t _kthe covariance matrix of the moment band robust filtering factor, P _k+1for t _k+1the covariance matrix of the moment band robust filtering factor, R _kfor according to t _kthe positive definite symmetric matrices that the priori of moment measurement noise obtains, for t _k+1the filtering estimated value in moment, for t _kthe state estimation in moment, Q _kfor according to t _kthe positive definite symmetric matrices that the priori of moment system noise obtains, I is and P _k+1the unit matrix that dimension is identical;

(303) filtering estimated value is obtained through step (302) after, utilize filtering estimated value to revise inertial navigation system attitude error and Gyro Random migration error.

The present invention adopts above technical scheme compared with prior art, has following technique effect:

(1) be that reference frame carries out the layout of inertial navigation algorithm with launch inertial coordinate system, effectively prevent with geographic coordinate be reference frame bring inaccurate and impact that is that cause navigational system precision is described because of earth model;

(2) take launch inertial coordinate system as the high-precision attitude information that reference frame can effectively use star sensor to provide, because what star sensor was measured is high-precision attitude information relative to inertial system, be reference frame according to geography, then need just can combine relative to the attitude of Department of Geography by being converted to, and need in transfer process to utilize inertia or other navigation means to provide positional information, thus can, owing to being coupled into site error in transfer process, cause starlight sensor measuring accuracy to decline.And when adopting launch inertial coordinate system to be reference frame, the attitude information that star sensor provides then provides positional information without the need to recycling other navigational system, thus attitude measurement accuracy can not be lost.

(3) adopt robust filtering method estimate quantity of state and correct, make in the inaccurate situation of aircraft guidance system model, wave filter can steady operation, the robustness of raising integrated navigation system.

Accompanying drawing explanation

Fig. 1 is method structural drawing of the present invention.

Fig. 2 be with launch inertial coordinate system be reference frame inertial navigation calculation result and be the position simulation comparison figure of inertial navigation calculation result of reference frame with geography.

Fig. 3 is inertial navigation calculation result for taking launch inertial coordinate system as reference frame and is the velocity simulation comparison diagram of inertial navigation calculation result of reference frame with geography.

Fig. 4 is the simulation comparison figure of navigation attitude error of the present invention and traditional filtering (kalman filtering) attitude error that navigates.

Embodiment

Below in conjunction with accompanying drawing, technical scheme of the present invention is described in further detail:

As shown in Figure 1, principle of the present invention is method structural drawing of the present invention: exported containing random error by gyro and accelerometer with resolved by inertial navigation under launch inertial coordinate system, aircraft can be obtained relative to the position of launch inertial coordinate system, speed, attitude information, provide continuous print to navigate metrical information.For effectively revising all kinds of errors of inertial navigation system, resolve on the basis of model setting up inertial navigation system under launch inertial coordinate system, derive and obtain the basic navigation parameter error model of 9 dimension inertial navigation systems, and in conjunction with gyro, accelerometer error model, build filter state amount and the state equation of INS errors; The original attitude information provided by star sensor, obtains the measurement information relative to launch inertial coordinate system by modular converter, according to linearization Observation principle, sets up the linearization measurement equation of attitude under launch inertial coordinate system; Adopt expansion H _∞robust filtering method is estimated filter state amount (9 dimension navigation fundamental errors and 6 dimension inertia type instrument errors), utilizes filtering estimated result to revise INS errors, can effectively improve navigational system performance.

The specific embodiment of the present invention is as follows:

1. under setting up launch inertial coordinate system, inertial navigation system resolves model

(1.1) velocity calculated model under launching inertial system

Coordinate system defines: e system-terrestrial coordinate system; I system-geocentric inertial coordinate system; L system-launch inertial coordinate system; B system-carrier coordinate system (front upper right); G system-geographic coordinate system

Under launch inertial coordinate system, the velocity calculated model of carrier is:

${\stackrel{\·}{v}}^l=C_b^l\·f^b+C_i^l\·G^i---\left(1\right)$

Wherein, v ^lfor aircraft is relative to the speed of launching inertial system, for aircraft speed v ^lfirst order derivative; f ^bfor the specific force that accelerometer exports; G ⁱfor the earth universal gravitation under inertial coordinates system.Because the earth is non-desirable spheroid, geologic structure is different everywhere, and mass distribution is also uneven, and thus earth gravitational field is not desirable central force field.After considering that re-entry space vehicle enters space, orbit altitude design is at about 300-400km, and can be similar to and think near-earth orbit spacecraft, earth universal gravitation is mainly subject to the impact of compression of the earth, and thus terrestrial gravitation bit function can adopt following expression:

$G^i=\left[\begin{array}{c}-\frac{\mathrm{\μx}}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]\\ -\frac{\mathrm{\μy}}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-1.5)]\\ -\frac{\mathrm{\μz}}{r^3}[1-J_2{\left(\frac{R_e}{r}\right)}^2(7.5\frac{z^2}{r^2}-4.5)]\end{array}\right]---\left(2\right)$

In formula, (x, y, z) is the position of aircraft in geocentric inertial coordinate system; μ is the gravitational constant of the earth, μ=3.986 × 10 ¹⁴m ³s ^-2; J ₂for the humorous item gravitational potential coefficient of second-order band, it is maximum to the perturbation of track, J ₂=1.08263 × 10 ^-3; R is the distance of aircraft to the earth's core; R _efor terrestrial equator radius, R _e=6378137m.If desired more high-precision gravitation bit function, can adopt tens of rank, the spherical harmonic coefficient of tens of grades.

(1.2) launch inertial coordinate system upper/lower positions resolves model

Under launch inertial coordinate system, the location compute model of carrier is:

${\stackrel{\·}{p}}^l=v^l---\left(3\right)$

Wherein, p ^lfor aircraft is relative to the position of launch inertial coordinate system.

(1.3) attitude algorithm model under launch inertial coordinate system

Under launch inertial coordinate system, attitude of carrier resolves model and is represented by hypercomplex number:

$\stackrel{\·}{q}=0.5q\⊗{\mathrm{\ω}}_{\mathrm{lb}}^b---\left(4\right)$

Wherein, q is the attitude quaternion of aircraft relative to launching inertial system; for carrier is relative to the projection of angular speed in carrier coordinate system of launching inertial system, wherein gyroscope exports and is because launch inertial coordinate system is constant relative to geocentric inertial coordinate system namely therefore q is corresponding attitude quaternion, the pass of its correspondence is:

Wherein, for the aircraft angle of pitch, ψ is course angle, and γ is roll angle, attitude quaternion q=[q ₀q ₁q ₂q ₃] ^t.

2. set up the robust filter model under launch inertial coordinate system

(2.1) the quantity of state equation of Navigation system error under launch inertial coordinate system is set up

Take launching inertial system as reference frame, the systematic error state variable X of Integrated Inertial Navigation System basic navigation error parameter and inertia type instrument error parameter is:

$\begin{array}{c}X=\left[\begin{array}{ccccccccc}\mathrm{\δ}{q}_1& \mathrm{\δ}{q}_2& \mathrm{\δ}{q}_3& \mathrm{\δ}{p}_x& \mathrm{\δ}{p}_y& \mathrm{\δ}{p}_z& \mathrm{\δ}{v}_x& \mathrm{\δ}{v}_y& \mathrm{\δ}{v}_z\end{array}\right.\\ {\left.\begin{array}{cccccc}{\mathrm{\ϵ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{ry}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\▿}_{\mathrm{ax}}& {\▿}_{\mathrm{ay}}& {\▿}_{\mathrm{az}}\end{array}\right]}^T\end{array}---\left(6\right)$

In formula (6), δ q ₁, δ q ₂, δ q ₃for the vector section of attitude quaternion error delta q in INS errors; δ p _x, δ p _y, δ p _zfor the site error quantity of state of X-axis, Y-axis, Z-direction in INS errors; δ v _x, δ v _y, δ v _zfor the velocity error quantity of state of X-axis, Y-axis, Z-direction in inertial navigation system; ε _rx, ε _ry, ε _rzfor the Gyro Random migration error of X-axis, Y-axis, Z-direction; ▽ _ax, ▽ _ay, ▽ _azfor the accelerometer random walk error of X-axis, Y-axis, Z-direction.

Resolve model according to inertial navigation system under launch inertial coordinate system, the error equation obtaining inertial navigation system basic navigation parameter of can deriving is:

Attitude error equations is:

$\mathrm{\δ}{\stackrel{\·}{q}}_{13}=-[{\widehat{\mathrm{\ω}}}_{\mathrm{ib}}^b\×]\mathrm{\δ}{q}_{13}+\frac{1}{2}\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b---\left(7\right)$

In formula (7), δ q ₁₃for the vector section of attitude quaternion δ q, δ q=[δ q ₀δ q ₁δ q ₂δ q ₃] ^t, for δ q ₁₃first order derivative, δ q ₁₃=[δ q ₁δ q ₂δ q ₃] ^t; for about antisymmetric matrix, for the estimated value of gyro output quantity, $[{\widehat{\mathrm{\ω}}}_{\mathrm{ib}}^b\×]=\left[\begin{array}{ccc}0& -{\widehat{\mathrm{\ω}}}_{\mathrm{ibz}}^b& {\widehat{\mathrm{\ω}}}_{\mathrm{iby}}^b\\ {\widehat{\mathrm{\ω}}}_{\mathrm{ibz}}^b& 0& -{\widehat{\mathrm{\ω}}}_{\mathrm{ibx}}^b\\ -{\widehat{\mathrm{\ω}}}_{\mathrm{iby}}^b& {\widehat{\mathrm{\ω}}}_{\mathrm{ibx}}^b& 0\end{array}\right],$ Wherein be respectively the estimated value of gyro in x-axis, y-axis, z-axis output quantity.

Site error equation is:

$\mathrm{\δ}\stackrel{\·}{p}=\mathrm{\δv}---\left(8\right)$

In formula (8), δ p is site error vector, for the first order derivative of δ p, δ v is velocity error vector.

Velocity error equation is:

$\mathrm{\δ}\stackrel{\·}{v}=2{\hat{C}}_b^l\·[{\hat{f}}^b\×]\·\mathrm{\δ}{q}_{13}+{\hat{C}}_b^l\·\mathrm{\δ}{f}^b+{C_i^l\·\mathrm{\δG}}^i---\left(9\right)$

In formula (9), δ v is site error vector, for the first order derivative of δ v; for the estimated value according to attitude quaternion obtain pose transformation matrix; for about antisymmetric matrix, for the estimated value of accelerometer output quantity, $[{\hat{f}}^b\×]=\left[\begin{array}{ccc}0& -{\hat{f}}_z^b& {\hat{f}}_y^b\\ {\hat{f}}_z^b& 0& -{\hat{f}}_x^b\\ -{\hat{f}}_y^b& {\hat{f}}_x^b& 0\end{array}\right];$ δ G ⁱfor the gravitation error brought by site error, $\mathrm{\δ}{G}^i=M\·C_l^i\·\mathrm{\δp},$ The expression formula of M is as follows:

$M=\left[\begin{array}{ccc}-\frac{\mathrm{\μ}}{r^3}+\frac{3\mathrm{\μ}{x}^2}{r^5}& \frac{3\mathrm{\μxy}}{r^5}& \frac{3\mathrm{\μxz}}{r^5}\\ \frac{3\mathrm{\μxy}}{r^5}& -\frac{\mathrm{\μ}}{r^3}+\frac{3{\mathrm{\μy}}^2}{r^5}& \frac{3\mathrm{\μyz}}{r^5}\\ \frac{3\mathrm{\μxz}}{r^5}& \frac{3\mathrm{\μyz}}{r^5}& -\frac{\mathrm{\μ}}{r^3}+\frac{3\mathrm{\μ}{z}^2}{r^5}\end{array}\right]---\left(10\right)$

In formula (10), the definition of (x, y, z), μ, r is identical with the definition in formula (2).Can find from the expression formula of M, its magnitude is 10 ^-6.When site error magnitude is 10 ², the accelerometer precision of employing is 10 ^-4during g, then the gravitation error brought by site error little accelerometer error order of magnitude, δ G ⁱcan ignore, namely velocity error equation can be reduced to:

$\mathrm{\δ}\stackrel{\·}{v}=2{\hat{C}}_b^l\·[{\hat{f}}^b\×]\·\mathrm{\δ}{q}_{13}+{\hat{C}}_b^l\·\mathrm{\δ}{f}^b---\left(11\right)$

If gyro noise model is random walk and white noise, accelerometer noise model is random walk, then inertia type instrument error equation is:

$\begin{array}{cc}\left\{\begin{array}{c}\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b={\mathrm{\ϵ}}_r+w_g\\ {\stackrel{\·}{\mathrm{\ϵ}}}_r=w_r\end{array}\right.& \left\{\begin{array}{c}\mathrm{\δ}{f}^b={\▿}_a\\ {\stackrel{\·}{\▿}}_a=w_a\end{array}\right.\end{array}---\left(12\right)$

In formula (12), ε _rfor Gyro Random migration, w _rfor driving white noise, w _gfor gyro white noise error; ▽ _afor accelerometer random walk, w _afor driving white noise.

Set up Navigation system error state equation:

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(13\right)$

In formula (7), X (t) is system state variables; for the first order derivative of state variable X (t); A (t) is systematic state transfer matrix; B (t) is system noise factor matrix; W (t) is noise matrix.

Known according to formula (7), (8), (11), (12), systematic state transfer matrix A (t) is:

$A\left(t\right)=\left[\begin{array}{ccccc}-[{\widehat{\mathrm{\ω}}}_{\mathrm{ib}}^b\×]& 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 2{\hat{C}}_b^l\·[{\hat{f}}^b\×]& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& {\hat{C}}_b^l\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]---\left(14\right)$

Wherein, I representation unit matrix, bottom right mark 3 × 3 represents that this matrix dimension is 3 × 3 dimensions.

System noise matrix is:

W＝[w _gx w _gy w _gz w _rx w _ry w _rz w _ax w _ay w _az] ^T (15)

Wherein, w _gx, w _gy, w _gzbe respectively the white noise error of gyro in x-axis, y-axis, z-axis; w _rx, w _ry, w _rzbe respectively the driving white noise w of Gyro Random migration noise _rvalue in x-axis, y-axis, z-axis; w _ax, w _ay, w _azbe respectively the driving white noise w of accelerometer random walk noise _avalue in x-axis, y-axis, z-axis.

System noise factor matrix B (t) is:

$B\left(t\right)=\left[\begin{array}{ccc}\frac{1}{2}{I}_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}\end{array}\right]---\left(16\right)$

(2.2) measurement equation of Navigation system error under launch inertial coordinate system is set up

The aircraft carrier obtained by star sensor measurement is relative to the attitude information of geocentric inertial coordinate system, the measurement information of carrier relative to launch inertial coordinate system is obtained by modular converter, according to linearization Observation principle, set up the linearization measurement equation in attitude measurement information and step (2.1) between Navigation system error quantity of state under launch inertial coordinate system:

The carrier that star sensor exports is q relative to the attitude quaternion of geocentric inertial coordinate system _cns, contain true attitude information q _swith observational error q _ε, for obtaining the attitude parameter of carrier relative to launch inertial coordinate system, need to transform as follows:

$q_c=q_g^{-1}\⊗q_{\mathrm{cns}}=q_g^{-1}\⊗q_s\⊗q_{\mathrm{\ϵ}}^{-1}=q_t\⊗q_{\mathrm{\ϵ}}^{-1}---\left(17\right)$

In formula (17), q _gfor launching inertial system is relative to the attitude quaternion of Earth central inertial system; q _tfor real carrier is relative to the attitude quaternion of launch inertial coordinate system; q _cbe the attitude measurement information of aircraft carrier relative to launch inertial coordinate system containing measuring error through being converted to.

Inertial navigation system resolves the attitude information obtained:

$q_i=q_t\⊗\mathrm{\δ}{q}^{-1}---\left(18\right)$

In formula (18), q _ifor inertial navigation system resolves the attitude information (hypercomplex number represents) obtained; q _tidentical with definition in formula (17), for real carrier is relative to the attitude quaternion of launch inertial coordinate system; δ q is the attitude error information that inertial navigation system is resolved.

According to formula (17), (18), and only consider quaternionic vector part, obtain following attitude measurement equation:

Z(t)＝H _a(t)X(t)+N _s(t) (19)

In formula (19), H _at () is attitude measurement matrix of coefficients; N _st () is attitude observation noise battle array.

3. adopt robust filtering method to carry out filter correction to quantity of state

(3.1) by filter status equation and measurement equation sliding-model control

X _k＝A _kX _k-1+B _kW _k-1 (20)

Z _k＝H _kX _k+N _k (21)

In formula (20), X _kfor t _kmoment system state amount; X _k-1for t _k-1moment system state amount; A _kfor t _k-1moment is to t _ktime etching system state-transition matrix; B _kfor t _k-1moment is to t _ktime etching system noise drive matrix; W _k-1for t _ktime etching system noise matrix; In formula (21), Z _kfor t _kmoment posture observed quantity matrix; H _kfor t _ktime attitude measure matrix of coefficients; N _kfor t _kthe noise matrix of moment attitude observation.

(3.2) robust filtering strategy is adopted to estimate as shown in the formula (22), formula (23), formula (24), the INS errors of formula (25) to step (3.1) part:

${\stackrel{\‾}{S}}_k=L_k^T{S}_k{L}_k---\left(22\right)$

$K_k=A_k{P}_k{[I-\mathrm{\θ}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{H}_k^T{R}_k^{-1}---\left(23\right)$

${\hat{X}}_{k+1}=A_k{\hat{X}}_k+A_k{K}_k(Z_k-H_k{\hat{X}}_k)---\left(24\right)$

$P_{k+1}=A_k{P}_k{[I-\mathrm{\θ}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{A}_k^T+Q_k---\left(25\right)$

Wherein, L _kfor the linear combination coefficient matrix of quantity of state; S _kfor weight coefficient matrix; definition is as shown in formula (22); K _kfor t _kthe filter gain matrix of the moment band robust filtering factor; θ is the robust filtering factor; P _kfor t _kthe covariance matrix of the moment band robust filtering factor; P _k+1for t _k+1the covariance matrix of the moment band robust filtering factor; R _kfor according to t _kthe positive definite symmetric matrices that the priori of moment measurement noise obtains; for t _k+1the state estimation in moment; for t _kthe state estimation in moment; Q _kfor according to t _kthe positive definite symmetric matrices that the priori of moment system noise obtains; I battle array is and P _k+1the unit matrix that dimension is identical.

(3.3) after step (3.2) obtains filtering estimated value, filtering estimated value is utilized to revise inertial navigation system attitude error and Gyro Random migration error.

In order to verify correctness and the validity of the re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system that invention proposes, adopting the inventive method Modling model, carrying out matlab simulating, verifying.Fig. 2, Fig. 3 be respectively with launch inertial coordinate system be reference frame inertial navigation calculation result and with geography be reference frame inertial navigation calculation result position simulation comparison figure and.Velocity simulation comparison diagram.In Fig. 2, Fig. 3, red dot-and-dash line is take launch inertial coordinate system as the inertial navigation calculation result of reference frame, blue solid lines is the inertial navigation calculation result (for contrast is convenient, under the calculation result under geographic coordinate system being transformed into launch inertial coordinate system by transfer algorithm) of reference frame with geographic coordinate.The simulation result of Fig. 2, Fig. 3 shows, is that reference frame can significantly reduce because geophysical field describes the inaccurate navigation error brought with launch inertial coordinate system, improves navigational system performance.

Fig. 4 is the simulation comparison figure of navigation attitude error of the present invention and traditional filtering (kalman filtering) attitude error that navigates.In simulation process, gyroscope error model is inaccurate, and actual random walk parameter is 5 times of model parameter.Red dot-and-dash line is the attitude error curve adopting robust filtering method, and blue solid lines is the attitude error curve adopting traditional kalman filtering method.Fig. 4 simulation result shows, in the inaccurate situation of system model, robust filtering performance is due to traditional kalman wave filter.

In sum, the re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system that the present invention proposes effectively can improve precision and the robustness of integrated navigation system.

By reference to the accompanying drawings embodiments of the present invention are explained in detail above, but the present invention is not limited to above-mentioned embodiment, in the ken that those of ordinary skill in the art possess, can also makes a variety of changes under the prerequisite not departing from present inventive concept.

Claims (5)

1., based on a re-entry space vehicle integrated navigation robust filtering method for launch inertial coordinate system, it is characterized in that, described method concrete steps comprise:

Step one, be reference frame with launch inertial coordinate system, carry out the layout of inertial navigation algorithm, set up the speed of aircraft under launch inertial coordinate system, position, attitude algorithm model;

Step 2, according to the speed of aircraft in step one under launch inertial coordinate system, position, attitude algorithm model, obtain corresponding speed, position, attitude error model, in conjunction with the error model of inertia type instrument, build the error state equation and the linearization measurement equation that comprise the inertial navigation system of basic navigation parameter error and inertia type instrument error;

Step 3, the error state equation of the inertial navigation system that step 2 draws and linearization measurement equation carried out to the renewal of sliding-model control and quantity of state, measuring amount, adopt robust filtering method estimate each quantity of state in set up state equation and revise.

2. a kind of re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system as claimed in claim 1, is characterized in that, in step one, described in resolve model expression-form be specially:

(101) the velocity calculated model of aircraft under launch inertial coordinate system is:

${\stackrel{\·}{v}}^l=C_b^l\·f^b+C_i^l\·G^i---\left(1\right)$

Wherein, v ^lfor aircraft is relative to the speed of launching inertial system, for v ^lfirst order derivative, f ^bfor the specific force that accelerometer exports, G ⁱfor the earth universal gravitation under geocentric inertial coordinate system, for carrier coordinate system b is tied to the pose transformation matrix of launch inertial coordinate system l system, for geocentric inertial coordinate system i is tied to the pose transformation matrix of launch inertial coordinate system l system;

(102) the location compute model of aircraft under launch inertial coordinate system is:

${\stackrel{\·}{p}}^l=v^l---\left(2\right)$

Wherein, p ^lfor aircraft is relative to the position of launch inertial coordinate system, for p ^lfirst order derivative;

(103) the attitude algorithm model of aircraft under launch inertial coordinate system is:

$\stackrel{\·}{q}=0.5q\⊗{\mathrm{\ω}}_{\mathrm{lb}}^b---\left(3\right)$

Wherein, q is the attitude quaternion of aircraft relative to launching inertial system, for the first order derivative of q, for the projection that carrier is fastened at carrier relative to the angular speed of launching inertial system, represent hypercomplex number multiplication.

3. a kind of re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system as claimed in claim 1, it is characterized in that, in the error state equation of the inertial navigation system described in step 2, the form that embodies of error state variable X is:

X＝[δq ₁ δq ₂ δq ₃ δp _x δp _y δp _z δv _x δv _y δv _zε _rx ε _ry ε _rz ▽ _ax ▽ _ay ▽ _az] ^T (4)

In formula (4), δ q ₁, δ q ₂, δ q ₃be respectively the vector section of attitude quaternion error delta q in INS errors, δ p _x, δ p _y, δ p _zbe respectively the site error quantity of state of X-axis in INS errors, Y-axis, Z-direction, δ v _x, δ v _y, δ v _zbe respectively the velocity error quantity of state of X-axis in inertial navigation system, Y-axis, Z-direction, ε _rx, ε _ry, ε _rzbe respectively the Gyro Random migration error of X-axis, Y-axis, Z-direction, ▽ _ax, ▽ _ay, ▽ _azbe respectively the accelerometer random walk error of X-axis, Y-axis, Z-direction.

4. a kind of re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system as claimed in claim 3, is characterized in that: the error state equation of described inertial navigation system is embodied as:

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(5\right)$

In formula (5), X (t) is system state variables, for the first order derivative of X (t), A (t) is system matrix; B (t) is noise figure matrix, and W (t) is noise matrix;

The carrier drawn according to star sensor measurement is relative to the attitude information of inertial coordinates system, the attitude information of carrier relative to launch inertial coordinate system is drawn through conversion, adopt attitude linearization Observation principle under launch inertial coordinate system, set up the linearization measurement equation between attitude observation under launch inertial coordinate system and INS errors quantity of state:

Z(t)＝H _a(t)X(t)+N _s(t) (6)

In formula (6), Z (t) is posture observed quantity matrix, H _at () is attitude measurement matrix of coefficients, N _st () is attitude observation noise battle array.

5. a kind of re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system as claimed in claim 1, it is characterized in that, employing robust filtering method described in step 3 is estimated each quantity of state in set up state equation and revises, and its concrete steps comprise:

(301) by the error state equation of inertial navigation system and linearization measurement equation sliding-model control:

X _k＝A _kX _k-1+B _kW _k-1 (7)

Z _k＝H _kX _k+N _k (8)

In above-mentioned formula, X _kfor t _kmoment system state amount, X _k-1for t _k-1moment system state amount, A _kfor t _k-1moment is to t _ktime etching system state-transition matrix, B _kfor t _k-1moment is to t _ktime etching system noise drive matrix, W _k-1for t _ktime etching system noise matrix, Z _kfor t _kmoment posture observed quantity matrix, H _kfor t _ktime attitude measure matrix of coefficients, N _kfor t _kthe noise matrix of moment attitude observation;

(302) robust filtering strategy is adopted to estimate INS errors:

${\stackrel{\‾}{S}}_k=L_k^T{S}_k{L}_k---\left(9\right)$

$K_k=A_k{P}_k{[I-\mathrm{\θ}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{H}_k^T{R}_k^{-1}---\left(10\right)$

${\hat{X}}_{k+1}=A_k{\hat{X}}_k+A_k{K}_k(Z_k-H_k{\hat{X}}_k)---\left(11\right)$

$P_{k+1}=A_k{P}_k{[I-\mathrm{\θ}{\stackrel{\‾}{S}}_k{P}_k+H_k^T{R}_k^{-1}{H}_k{P}_k]}^{-1}{A}_k^T+Q_k---\left(12\right)$

In above-mentioned formula, L _kfor the linear combination coefficient matrix of quantity of state, S _kfor weight coefficient matrix, for the weight coefficient matrix after conversion, K _kfor t _kthe filter gain matrix of the moment band robust filtering factor, θ is the robust filtering factor, P _kfor t _kthe covariance matrix of the moment band robust filtering factor, P _k+1for t _k+1the covariance matrix of the moment band robust filtering factor, R _kfor according to t _kthe positive definite symmetric matrices that the priori of moment measurement noise obtains, for t _k+1the filtering estimated value in moment, for t _kthe state estimation in moment, Q _kfor according to t _kthe positive definite symmetric matrices that the priori of moment system noise obtains, I is and P _k+1the unit matrix that dimension is identical;

(303) filtering estimated value is obtained through step (302) after, utilize filtering estimated value to revise inertial navigation system attitude error and Gyro Random migration error.