CN104457446A - Aerial self-alignment method of spinning guided cartridge - Google Patents

Abstract

The invention discloses an aerial self-alignment method of a spinning guided cartridge. The aerial self-alignment of the spinning guided cartridge is achieved through a GPS and an inertial navigation system. Compared with other aerial self-alignment methods of the spinning guided cartridge, the fast aerial self-alignment purpose of the guided cartridge is achieved, navigation noise is filtered out through a Kalman filter, the initial alignment precision and navigation precision of the spinning guided cartridge are improved, the initial alignment time of the spinning guided cartridge is shortened, and an important foundation is laid for improving the drop point precision of the spinning guided cartridge and shortening the blow time.

Description

A kind of aerial Alignment Method of the guided cartridge that spins

Technical field

The present invention relates to a kind of aerial Alignment Method, particularly relate to a kind of aerial Alignment Method of the guided cartridge that spins, belong to Initial Alignment Technique and Design of Integrated Navigation System technology.

Background technology

Spin guided cartridge is that one is aloft launched, and need a kind of advanced precision strike munitions carrying out self-aligned, it contains the system such as inertial navigation and GPS, is revised the error of inertial navigation system by GPS, reaches the ability of precision strike target.Estimate the misalignment of inertial navigation system the deviation of the navigational parameter (as speed etc.) that Air launching provides from inertial navigation other navigation system relative and correct it.

Inertial navigation system is a voyage Estimation System based on acceleration quadratic integral, and it relies on plant equipment completely and corresponding algorithm is automatic, complete independently navigation task, and any optical, electrical contact does not occur in the external world.Because it has good concealment, working environment not by advantages such as meteorological condition restrictions, become a kind of widely used prime navaid system in space flight, aviation, navigational field.Before inertial navigation system work is resolved, need to provide original state, need exactly to carry out initial alignment.Inertial navigation system is when ground static state, and position can be provided by gps system, and three attitude angle can be provided by inertia system autoregistration, because be inactive state, three speed are zero; Inertial navigation system aloft state of flight time, position and speed still can be provided by gps system, but attitude angle cannot be provided by inertial navigation system autoregistration.Carrying out the self aligned effective way of aerial inertial navigation system is adopt GPS navigation information to resolve and estimation technique, namely the navigation information exported by GPS calculates course angle and the angle of pitch in corresponding moment, because position and speed directly can be exported by GPS, thus in original state also surplus roll angle parameter need to carry out estimation and resolve.Conventional Initial Alignment Method is the classical alignment scheme adopting circuit feedback method to realize, owing to not considering the random factors in inertial navigation working environment, rocking at random of the spin guided cartridge caused as fitful wind etc., the rich song distortion etc. of the guided bomb structure that in flight course, aerodynamic random change causes, and the precision of initial alignment is not high and need the time long.

The graceful filtering algorithm of Caro makes one of algorithm estimated to random signal, compared with many algorithm for estimating such as least square, Wiener filtering, the graceful filtering of Caro has significant advantage: adopt state space method designing filter in time domain, the dynamics of any complex multi-dimensional signal is described with state equation, avoid and do to decompose the trouble brought to power spectrum signal in frequency domain, design of filter is simple; Adopt recursive algorithm, real-time measurement information is concentrated in estimate through refining, and need not measurement amount in storage time process.So the graceful filtering of Caro can be applicable to the estimation of any steady of white-noise excitation or non-stationary random vector process, gained estimates that precision is best in Linear Estimation.Along with the development of computer technology, the application of the graceful filtering of current Caro almost designs all spectras such as communication, navigation, remote sensing, seismic survey, oil exploration, economy and The Study of Sociology.

Therefore, in order to improve the spin accuracy of guided cartridge initial alignment parameter and the rapidity of correction algorithm, reducing system difficulty, revising navigation attitude fast, the accuracy of raising navigation results, need the aerial Alignment Method of research a kind of spin guided cartridge.

Summary of the invention

The technical problem that the present invention solves is: overcome the deficiencies in the prior art, a kind of aerial Alignment Method of the guided cartridge that spins is provided, the accurate measurement achieved the initial position of Navigation Control, speed and attitude angle are really carried out in inertial navigation is resolved, and improves the impact accuracy of spin guided cartridge.

Technical solution of the present invention is: a kind of aerial Alignment Method of the guided cartridge that spins, and step is as follows:

(1) guided cartridge is spinned at t ₀moment gathers the velocity information [V that GPS records _ge0v _gn0v _gu0] and positional information and it can be used as t ₀the speed of moment reality and positional information, calculate t according to the velocity information recorded simultaneously ₀the attitude information of moment spin guided cartridge reality, wherein V _ge0for t ₀the east orientation speed that moment GPS records, V _gn0for t ₀the north orientation speed that moment GPS records, V _gu0for t ₀the sky that moment GPS records to speed, λ _g0for t ₀the longitude that moment GPS records, for t ₀the longitude that moment GPS records, h _g0for t ₀the height that moment GPS records, t ₀moment is the initial time that spin guided cartridge aloft flies;

(2) guided cartridge is spinned at t _nmoment gathers the acceleration that records of inertial navigation system and angular velocity information, and according to the acceleration recorded and angular velocity information and t _n-1the attitude of moment spin guided cartridge reality, position and velocity information carry out navigation calculation, obtain t _nattitude information [the φ of the spin guided cartridge that the moment is calculated by inertial navigation system _zφ _xφ _y], positional information and velocity information [V _ev _nv _u], wherein φ _zfor t _nthe course angle of moment spin guided cartridge, φ _xfor t _nthe angle of pitch of moment spin guided cartridge, φ _yfor t _nthe roll angle of moment spin guided cartridge, λ is t _nthe longitude of moment spin guided cartridge, for t _nthe longitude of moment spin guided cartridge, h is t _nthe height of moment spin guided cartridge, V _efor t _nthe east orientation speed of moment spin guided cartridge, V _nfor t _nthe north orientation speed of moment spin guided cartridge, V _ufor t _nthe sky of moment spin guided cartridge is to speed, and enter step (3), wherein the initial value of n is 1;

(3) at t _nin the moment, spin guided cartridge judges whether to receive the speed and positional information that GPS exports, if do not received, then using the navigation calculation result of step (2) as t _nthe speed of moment spin guided cartridge reality, position and attitude information, enter step (5); Otherwise, enter step (4);

(4) guided cartridge is spinned according to t _nvelocity information [the V of the GPS that reception arrives _gev _gnv _gu] and t _nattitude information [the φ of the spin guided cartridge that the moment is calculated by inertial navigation system _zφ _xφ _y], positional information and velocity information [V _ev _nv _u] carry out integrated navigation calculating, obtain t _nthe speed of moment spin guided cartridge reality, position and attitude information; Wherein, V _gefor t _nthe east orientation speed that moment GPS exports, V _gnfor t _nthe north orientation speed that moment GPS exports, V _gufor t _nthe sky that moment GPS exports is to speed; Enter step (5);

(5) t is judged _nwhether be the spin guided cartridge self aligned final moment, if not, then return step (2) after the value of n adds 1; Otherwise, t _nmoment resolves the speed of the spin guided cartridge reality obtained, position and attitude angle information and is the self aligned result of spin guided cartridge, thus completes the aerial autoregistration of spin guided cartridge;

In above steps, t _nwith t _n-1the time interval be the measuring period of inertial navigation system.

T is calculated according to the velocity information recorded in described step (1) ₀the method of the attitude information of the spin guided cartridge in moment is:

${\mathrm{\φ}}_{\mathrm{gz}0}=\arctan \left(\frac{V_{\mathrm{gn}0}}{V_{\mathrm{ge}0}}\right);$

${\mathrm{\φ}}_{\mathrm{gx}0}=\arctan \left(\frac{V_{\mathrm{gu}0}}{\sqrt{V_{\mathrm{ge}0}^2+V_{\mathrm{gn}0}^2}}\right);$

φ _gy0for arbitrary value;

Wherein, φ _gz0for t ₀the course angle of moment spin guided cartridge, φ _gx0for t ₀the angle of pitch of the spin guided cartridge in moment, φ _gy0for t ₀the roll angle of the spin guided cartridge in moment.

The implementation of described step (4) is:

(3.1) according to t _nvelocity information [the V of the GPS that reception arrives _gev _gnv _gu], utilize following formulae discovery t _nthe course angle φ of moment spin guided cartridge _gzwith angle of pitch φ _gx:

${\mathrm{\φ}}_{\mathrm{gz}}=\arctan \left(\frac{V_{\mathrm{gn}}}{V_{\mathrm{ge}}}\right)$

${\mathrm{\φ}}_{\mathrm{gx}}=\mathrm{ac}\tan \left(\frac{V_{\mathrm{gu}}}{\sqrt{V_{\mathrm{ge}}^2+V_{\mathrm{gn}}^2}}\right);$

(3.2) control modification method is utilized to obtain t _nthe moment spin course angle correction value of guided cartridge, angle of pitch correction value, east orientation position correction value, north orientation position correction value and height correction value:

${\mathrm{\μ}}_{{\mathrm{\φ}}_z}=K_1({\mathrm{\φ}}_z-{\mathrm{\φ}}_{\mathrm{gz}})$

${\mathrm{\μ}}_{{\mathrm{\φ}}_x}=K_2({\mathrm{\φ}}_x-{\mathrm{\φ}}_{\mathrm{gx}})$

μ _λ＝K ₃(λ-λ _g)

μ _h＝K ₅(h-h _g)

Wherein, for t _nthe course angle correction value of moment spin guided cartridge, for t _nthe correction value of the angle of pitch of moment spin guided cartridge, μ _λfor t _nthe east orientation position correction value of moment spin guided cartridge, for t _nthe north orientation position correction value of moment spin guided cartridge, μ _hfor t _nthe height correction value of moment spin guided cartridge, λ _g, and h _gbe respectively t _nthe longitude that moment GPS exports, latitude and height, λ, t is respectively with h _nthe longitude of the spin guided cartridge that the moment is calculated by inertial navigation system, latitude and height, φ _xand φ _zbe respectively t _nthe angle of pitch of the spin guided cartridge that the moment is calculated by inertial navigation system and course angle, K ₁, K ₂, K ₃, K ₄, K ₅for constant value;

(3.3) Kalman filtering is utilized to obtain speed correction, roll angle correction value and three acceleration bias:

By t _nthe east orientation speed V that moment GPS exports _ge, north orientation speed V _gnwith sky to speed V _guas measured value, calculate t _nmoment east orientation speed, north orientation speed and sky are to the measured deviation of speed:

dV _e＝V _e-V _ge

dV _n＝V _n-V _gn

dV _u＝V _u-V _gu

Wherein dV _efor t _nmoment east orientation velocity deviation, dV _nfor t _nmoment north orientation velocity deviation, dV _ufor t _nmoment, sky was to velocity deviation;

Utilize t _nthe east orientation speed in moment, north orientation speed and sky, to the measured deviation of speed, calculate t according to Kalman filtering algorithm _nthe state estimator X in moment;

X=[δ V _e, δ V _n, δ V _u, δ φ _y, ε _x, ε _y, ε _z], utilization state estimator X calculates east orientation speed correction δ V _e, north orientation speed correction δ V _n, sky is to speed correction δ V _u, roll angle correction value δ φ _y, three acceleration bias value ε _x, ε _y, ε _z;

The measurement battle array used in Kalman filtering $H=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\end{array}\right];$

(3.4) t is calculated _nthe implementation of the attitude information of moment spin guided cartridge reality is:

A () is by t _nmoment angle of pitch correction value roll angle correction value course angle correction value be updated to the attitude differential equation represented with Euler's krylov horn cupping in;

Wherein, represent that attitude angle is to the derivative of time T, $U=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sec {\mathrm{\φ}}_x& 0\\ 0& -\tan {\mathrm{\φ}}_x& 1\end{array}\right],$ $R_b^w=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& 0& \sin {\mathrm{\φ}}_y\\ \sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \cos {\mathrm{\φ}}_x& -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right],$

$R_L^w=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right],$

drawn by gyroscope survey, ν _nt _n-1the north orientation speed of the spin guided cartridge reality in moment, ν _et _n-1the east orientation speed of the spin guided cartridge reality in moment, ω _iefor earth rotation speed, R _mfor earth radius of curvature of meridian, R _nfor earth radius of curvature in prime vertical, h is t _n-1the height of the spin guided cartridge in moment, t _n-1the latitude of the spin guided cartridge in moment, φ _xt _n-1the angle of pitch of the spin guided cartridge reality in moment, φ _yt _n-1the roll angle of the spin guided cartridge reality in moment, φ _zt _n-1the course angle of the spin guided cartridge reality in moment;

(b) t _nthe angle of pitch φ of moment spin guided cartridge reality _x', roll angle φ _y', course angle φ _z' computing formula as follows:

$\left[\begin{array}{c}{\mathrm{\φ}}_x^{\′}\\ {\mathrm{\φ}}_y^{\′}\\ {\mathrm{\φ}}_z^{\′}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\\ {\mathrm{\φ}}_z\end{array}\right]+\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}*T;$

T is the sampling period of GPS;

(3.5) t is calculated _nthe implementation of the velocity information of moment spin guided cartridge reality is:

Feeding back in navigation calculation by resolving three speed correction obtained, calculating t _nmoment navigation speed information:

V _e′＝V _e-δV _e

V _n′＝V _n-δV _n

V _u′＝V _u-δV _u

Wherein, V _e' represent t _nthe east orientation speed of moment spin guided cartridge reality, V _n' represent t _nthe north orientation speed of moment spin guided cartridge reality, V _u' represent t _nthe sky of moment spin guided cartridge reality is to speed;

(3.6) t is calculated _nthe implementation of the positional information of moment spin guided cartridge reality is:

λ′＝λ-μ _λ

h′＝h-μ _h

λ ' expression t _nthe longitude of moment spin guided cartridge reality, represent t _nthe latitude of moment spin guided cartridge reality, h ' expression t _nthe height of moment spin guided cartridge reality.

The present invention's advantage is compared with prior art as follows:

(1) to carry out self aligned algorithm stability poor for existing direct use Kalman filtering, and depend critically upon correctness and the levels of precision of navigation error model, method of the present invention is by using controller to carry out signal noise process horizontal attitude information, do not rely on the accuracy of SYSTEM ERROR MODEL, completion system exports the tracking to system input, namely completes the correction to inertial navigation system site error and horizontal attitude error.

(2) existing ten two-dimension Kalman filtering algorithm time overheads are larger, the filtering cycle is longer, present invention uses the graceful filtering algorithm of 7 degree of freedom Caro, use effective information as much as possible, what exceed again adds observation dimension, while guaranteeing accurately to estimate roll angle error, this method fast operation, time overhead is short.

(3) with existing based on compared with the aerial autoregistration algorithm of the graceful filtering of Caro, the present invention is more efficiently make use of GPS information, and by GPS information, Directly solution calculates the angle of pitch and course angle, the result obtained than the method resolved by the graceful filtering of Caro is more accurate, and reduces amount of calculation.

Accompanying drawing explanation

Fig. 1 is the aerial autoregistration loop diagram of spin guided cartridge of the present invention;

Fig. 2 is the Kalman Filter Estimation design sketch using the inventive method design;

Fig. 3 is the navigation curve after the autoregistration using the inventive method to design.

Detailed description of the invention

Due to guided cartridge aloft time be a kind of maneuvering condition, autoregistration is not carried out by inertial navigation system itself, need to introduce GPS supplementary carry out the measurement of inertial navigation system original state and resolve, there are instrument error, Initial Alignment Error, gravity anomaly etc. in inertial navigation system itself, when working long hours, navigation error is dispersed in time.For this reason, need the navigation information introducing GPS output to carry out guided cartridge Air launching, improve the attack precision of guided cartridge.Detailed description of the invention is:

(1) guided cartridge is spinned at t ₀moment gathers the velocity information [V that GPS records _ge0v _gn0v _gu0] and positional information and calculate t according to the velocity information recorded ₀the attitude information of moment spin guided cartridge, wherein V _ge0for t ₀the east orientation speed that moment GPS records, V _gn0for t ₀the north orientation speed that moment GPS records, V _gu0for t ₀the sky that moment GPS records to speed, λ _g0for t ₀the longitude that moment GPS records, for t ₀the longitude that moment GPS records, h _g0for t ₀the height that moment GPS records, t ₀moment is the initial time that spin guided cartridge aloft flies;

${\mathrm{\φ}}_{\mathrm{gz}0}=\arctan \left(\frac{V_{\mathrm{gn}0}}{V_{\mathrm{ge}0}}\right);$

${\mathrm{\φ}}_{\mathrm{gx}0}=\arctan \left(\frac{V_{\mathrm{gu}0}}{\sqrt{V_{\mathrm{ge}0}^2+V_{\mathrm{gn}0}^2}}\right);$

φ _gy0for arbitrary value;

Wherein, φ _gz0for t ₀the course angle of moment spin guided cartridge, φ _gx0for t ₀the angle of pitch of the spin guided cartridge in moment, φ _gy0for t ₀the roll angle of the spin guided cartridge in moment.

(2) make n=1, enter step (3);

(3) guided cartridge is spinned at t _nmoment gathers the acceleration that records of inertial navigation system and angular velocity information, and according to the acceleration recorded and angular velocity information and t _n-1the speed of moment spin guided cartridge reality, position and attitude information carry out navigation calculation, obtain t _nattitude information [the φ of the spin guided cartridge that the moment is calculated by inertial navigation system _zφ _xφ _y], positional information and velocity information [V _ev _nv _u], wherein φ _zfor t _nthe course angle of moment spin guided cartridge, φ _xfor t _nthe angle of pitch of moment spin guided cartridge, φ _yfor t _nthe roll angle of moment spin guided cartridge, V _efor t _nthe east orientation speed of moment spin guided cartridge, V _nfor t _nthe north orientation speed of moment spin guided cartridge, V _ufor t _nthe sky of moment spin guided cartridge is to speed, and λ is t _nthe longitude of moment spin guided cartridge, for t _nthe longitude of moment spin guided cartridge, h is t _nthe height of moment spin guided cartridge, enters step (4);

(4) at t _nin the moment, spin guided cartridge judges whether to receive the speed and positional information that GPS exports, if do not received, then using the navigation calculation result of step (3) as t _nthe speed of moment spin guided cartridge reality, position and attitude information, enter step (6); Otherwise, enter step (5);

(5) guided cartridge is spinned according to t _nvelocity information [the V of the GPS that reception arrives _gev _gnv _gu], calculate t _nthe course angle φ of moment spin guided cartridge _gzwith angle of pitch φ _gx, and obtain t further by control modification method _nthe moment spin course angle correction value of guided cartridge, angle of pitch correction value, east orientation position correction value, north orientation position correction value and height correction value, then through 7 degree of freedom Kalman filter, resolve and obtain speed correction, roll angle correction value and three acceleration bias, carry out integrated navigation according to above-mentioned information and resolve and obtain t _nthe speed of moment spin guided cartridge reality, position and attitude information; Wherein, V _gefor t _nthe east orientation speed in moment, V _gnfor t _nthe north orientation speed in moment, V _gufor t _nthe sky in moment, to speed, enters step (6);

(6) t is judged _nwhether be the spin guided cartridge self aligned final moment, if not, then the value of n adds 1, enters step (3); Otherwise, t _nmoment resolves the speed of the spin guided cartridge reality obtained, position and attitude angle information and is the self aligned result of spin guided cartridge, thus completes the aerial autoregistration of spin guided cartridge;

In above steps, t _nwith t _n-1the time interval be the measuring period of inertial navigation system.

According to t _nvelocity information [the V of the GPS that reception arrives _gev _gnv _gu], resolve t _nthe course angle in moment and the method for the angle of pitch are:

${\mathrm{\φ}}_{\mathrm{gz}}=\arctan \left(\frac{V_{\mathrm{gn}}}{V_{\mathrm{ge}}}\right)$

${\mathrm{\φ}}_{\mathrm{gx}}=\mathrm{ac}\tan \left(\frac{V_{\mathrm{gu}}}{\sqrt{V_{\mathrm{ge}}^2+V_{\mathrm{gn}}^2}}\right);$

Wherein, φ _gzfor t _nthe course angle in moment, φ _gxfor t _nthe angle of pitch in moment, V _gefor t _nthe east orientation speed that moment GPS exports, V _gnfor t _nthe north orientation speed that moment GPS exports, V _gufor t _nthe sky that moment GPS exports is to speed.

The method calculating each navigational parameter correction value is:

${\mathrm{\μ}}_{{\mathrm{\φ}}_z}=K_1({\mathrm{\φ}}_z-{\mathrm{\φ}}_{\mathrm{gz}})$

${\mathrm{\μ}}_{{\mathrm{\φ}}_x}=K_2({\mathrm{\φ}}_x-{\mathrm{\φ}}_{\mathrm{gx}})$

μ _λ＝K ₃(λ-λ _g)

μ _h＝K ₅(h-h _g)

Wherein, for t _nthe course angle correction value of moment spin guided cartridge, for t _nthe correction value of the angle of pitch of moment spin guided cartridge, μ _λfor t _nthe east orientation position correction value of moment spin guided cartridge, for t _nthe north orientation position correction value of moment spin guided cartridge, μ _hfor t _nthe height correction value of moment spin guided cartridge, λ _g, and h _gbe respectively t _nthe longitude that moment GPS exports, latitude and height, λ, t is respectively with h _nthe longitude of the spin guided cartridge that the moment is calculated by inertial navigation system, latitude and height, φ _xand φ _zbe respectively t _nthe angle of pitch of the spin guided cartridge that the moment is calculated by inertial navigation system and course angle, K ₁, K ₂, K ₃, K ₄, K ₅for constant value.

Kalman filtering computational methods are:

By t _nthe east orientation speed V that moment GPS exports _ge, north orientation speed V _gnwith sky to speed V _guas measured value, t can be calculated _nmoment east orientation speed, north orientation speed and sky are to three measured deviations of speed:

dV _e＝V _e-V _ge

dV _n＝V _n-V _gn

dV _u＝V _u-V _gu

Wherein dV _efor t _nmoment east orientation velocity deviation, dV _nfor t _nmoment north orientation velocity deviation, dV _ufor t _nmoment, sky was to velocity deviation.

X＝[δV _e,δV _n,δV _u,δφ _y,ε _x,ε _y,ε _z]

Wherein X is state estimator, δ V _efor east orientation speed correction, δ V _nfor north orientation speed correction, δ V _ufor sky is to speed correction, δ φ _yfor roll angle correction value, ε _x, ε _y, ε _zbe three acceleration bias values.The measurement battle array used in known Kalman filtering herein $H=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\end{array}\right].$

According to system itself and Kalman's general principle, certainty annuity excitation noise variance matrix Q, measuring noise square difference battle array R, estimation mean squared error matrix initial value P ₀with system mode initial value X ₀after, utilize t _nthree velocity deviations in moment calculate t according to Kalman filtering algorithm _nthe state estimator X in moment.

Three solving of attitude methods are:

Three attitude rectification values are updated to the attitude differential equation represented with Euler's krylov horn cupping $\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}=U{R}_b^w{\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ib}}^b-U{R}_L^w({\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ie}}^L+{\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{eL}}^L)+u_{\mathrm{\φ}}$ In;

Then, obtain revised attitude angle through integration, formula is as follows:

$\left[\begin{array}{c}{\mathrm{\φ}}_x^{\′}\\ {\mathrm{\φ}}_y^{\′}\\ {\mathrm{\φ}}_z^{\′}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\\ {\mathrm{\φ}}_z\end{array}\right]+\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}*T;$

Wherein, represent that attitude angle is to the derivative of time T, $U=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sec {\mathrm{\φ}}_x& 0\\ 0& -\tan {\mathrm{\φ}}_x& 1\end{array}\right],$ $R_b^w=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& 0& \sin {\mathrm{\φ}}_y\\ \sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \cos {\mathrm{\φ}}_x& -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right],$

b is drawn by gyroscope survey, ν _nt _n-1the north orientation speed of the spin guided cartridge reality in moment, ν _et _n-1the east orientation speed of the spin guided cartridge reality in moment, ω _iefor earth rotation speed, R _mfor earth radius of curvature of meridian, R _nfor earth radius of curvature in prime vertical, h is t _n-1the height of the spin guided cartridge in moment, t _n-1the latitude of the spin guided cartridge in moment, φ _xt _n-1the angle of pitch of the spin guided cartridge reality in moment, φ _yt _n-1the roll angle of the spin guided cartridge reality in moment, φ _zt _n-1the course angle of the spin guided cartridge reality in moment.

Velocity location calculation method is:

To three speed correction obtaining be resolved and three position correction values feed back in navigation calculation, calculate navigation speed, positional information, and as the initial value of next navigation calculation, enter and resolve, final navigation path and the flight controling parameters obtaining guided cartridge.

Solution formula is:

V _e′＝V _e-δV _e

V _n′＝V _n-δV _n

V _u′＝V _u-δV _u

Wherein, V _e' represent t _nthe east orientation speed of moment spin guided cartridge reality, V _n' represent t _nthe north orientation speed of moment spin guided cartridge reality, V _u' represent t _nthe sky of moment spin guided cartridge reality is to speed;

λ′＝λ-μ _λ

h′＝h-μ _h

λ ' expression t _nthe longitude of moment spin guided cartridge reality, represent t _nthe latitude of moment spin guided cartridge reality, h ' expression t _nthe height of moment spin guided cartridge reality.

The aerial Alignment Method of spin guided cartridge can be completed by said method, Fig. 1 is the aerial autoregistration loop diagram of spin guided cartridge, three position correction values, three speed correction and three attitude rectification values are calculated by control combination method and Kalman filtering algorithm, and feed back in navigation calculation loop, aerial autoregistration is carried out to spin shell.In Fig. 2, the first row is respectively east orientation speed fair curve, north orientation speed fair curve and sky from left to right to speed fair curve, second behavior roll angle fair curve, the third line is respectively three accelerometer error values from left to right, can be as seen from the figure, roll angle correction value rapidly converges to zero, completes the aerial autoregistration of spin guided cartridge.Fig. 3 is the navigation curve after autoregistration, and its cathetus represents the result that GPS calculates, and dotted line represents inertial navigation calculation result, and the first row is respectively east orientation speed, north orientation speed and sky from left to right to speed; Second row is respectively longitude, latitude and height from left to right, the third line is respectively course angle, the angle of pitch and roll angle, can as seen from the figure, after completing autoregistration, navigation results and geometric locus coincide, can accurately to control and the drop point analysis of guided cartridge.

The inventive method also can be used for the needs such as unmanned plane self aligned occasion aloft.

The non-detailed description of the present invention is known to the skilled person technology.

Claims (3)

1. spin the aerial Alignment Method of guided cartridge, it is characterized in that step is as follows:

(1) guided cartridge is spinned at t ₀moment gathers the velocity information [V that GPS records _ge0v _gn0v _gu0] and positional information and it can be used as t ₀the speed of moment reality and positional information, calculate t according to the velocity information recorded simultaneously ₀the attitude information of moment spin guided cartridge reality, wherein V _ge0for t ₀the east orientation speed that moment GPS records, V _gn0for t ₀the north orientation speed that moment GPS records, V _gu0for t ₀the sky that moment GPS records to speed, λ _g0for t ₀the longitude that moment GPS records, for t ₀the longitude that moment GPS records, h _g0for t ₀the height that moment GPS records, t ₀moment is the initial time that spin guided cartridge aloft flies;

(2) guided cartridge is spinned at t _nmoment gathers the acceleration that records of inertial navigation system and angular velocity information, and according to the acceleration recorded and angular velocity information and t _n-1the attitude of moment spin guided cartridge reality, position and velocity information carry out navigation calculation, obtain t _nattitude information [the φ of the spin guided cartridge that the moment is calculated by inertial navigation system _zφ _xφ _y], positional information and velocity information [V _ev _nv _u], wherein φ _zfor t _nthe course angle of moment spin guided cartridge, φ _xfor t _nthe angle of pitch of moment spin guided cartridge, φ _yfor t _nthe roll angle of moment spin guided cartridge, λ is t _nthe longitude of moment spin guided cartridge, for t _nthe longitude of moment spin guided cartridge, h is t _nthe height of moment spin guided cartridge, V _efor t _nthe east orientation speed of moment spin guided cartridge, V _nfor t _nthe north orientation speed of moment spin guided cartridge, V _ufor t _nthe sky of moment spin guided cartridge is to speed, and enter step (3), wherein the initial value of n is 1;

(3) at t _nin the moment, spin guided cartridge judges whether to receive the speed and positional information that GPS exports, if do not received, then using the navigation calculation result of step (2) as t _nthe speed of moment spin guided cartridge reality, position and attitude information, enter step (5); Otherwise, enter step (4);

(4) velocity information [V of the GPS that the guided cartridge that spins arrives according to tn reception _gev _gnv _gu] and t _nattitude information [the φ of the spin guided cartridge that the moment is calculated by inertial navigation system _zφ _xφ _y], positional information and velocity information [V _ev _nv _u] carry out integrated navigation calculating, obtaining the tn moment spins the speed of guided cartridge reality, position and attitude information; Wherein, V _gefor t _nthe east orientation speed that moment GPS exports, V _gnfor t _nthe north orientation speed that moment GPS exports, V _gufor t _nthe sky that moment GPS exports is to speed; Enter step (5);

(5) t is judged _nwhether be the spin guided cartridge self aligned final moment, if not, then return step (2) after the value of n adds 1; Otherwise, t _nmoment resolves the speed of the spin guided cartridge reality obtained, position and attitude angle information and is the self aligned result of spin guided cartridge, thus completes the aerial autoregistration of spin guided cartridge;

In above steps, t _nwith t _n-1the time interval be the measuring period of inertial navigation system.

2. the aerial Alignment Method of a kind of guided cartridge that spins according to claim 1, is characterized in that: calculate t according to the velocity information recorded in described step (1) ₀the method of the attitude information of the spin guided cartridge in moment is:

${\mathrm{\φ}}_{\mathrm{gz}0}=\arctan \left(\frac{V_{\mathrm{gn}0}}{V_{\mathrm{ge}0}}\right)$

${\mathrm{\φ}}_{\mathrm{gx}0}=\arctan \left(\frac{V_{\mathrm{gu}0}}{\sqrt{V_{\mathrm{ge}0}^2+V_{\mathrm{gn}0}^2}}\right);$

φ _gy0for arbitrary value;

Wherein, φ _gz0for t ₀the course angle of moment spin guided cartridge, φ _gx0for t ₀the angle of pitch of the spin guided cartridge in moment, φ _gy0for t ₀the roll angle of the spin guided cartridge in moment.

3. the aerial Alignment Method of a kind of guided cartridge that spins according to claim 1, is characterized in that: the implementation of described step (4) is:

(3.1) velocity information [V of the GPS arrived according to tn reception _gev _gnv _gu], utilize following formulae discovery t _nthe course angle φ of moment spin guided cartridge _gzwith angle of pitch φ _gx:

${\mathrm{\φ}}_{\mathrm{gz}}=\arctan \left(\frac{V_{\mathrm{gn}}}{V_{\mathrm{ge}}}\right)$

${\mathrm{\φ}}_{\mathrm{gx}}=\arctan \left(\frac{V_{\mathrm{gu}}}{\sqrt{V_{\mathrm{ge}}^2+V_{\mathrm{gn}}^2}}\right);$

(3.2) control modification method is utilized to obtain t _nthe moment spin course angle correction value of guided cartridge, angle of pitch correction value, east orientation position correction value, north orientation position correction value and height correction value:

${\mathrm{\μ}}_{{\mathrm{\φ}}_z}=K_1({\mathrm{\φ}}_z-{\mathrm{\φ}}_{\mathrm{gz}})$

${\mathrm{\μ}}_{{\mathrm{\φ}}_x}=K_2({\mathrm{\φ}}_x-{\mathrm{\φ}}_{\mathrm{gx}})$

μ _λ＝K ₃(λ-λ _g)

μ _h＝K ₅(h-h _g)

Wherein, for t _nthe course angle correction value of moment spin guided cartridge, for t _nthe correction value of the angle of pitch of moment spin guided cartridge, μ _λfor t _nthe east orientation position correction value of moment spin guided cartridge, for t _nthe north orientation position correction value of moment spin guided cartridge, μ _hfor t _nthe height correction value of moment spin guided cartridge, λ _g, and h _gbe respectively t _nthe longitude that moment GPS exports, latitude and height, λ, t is respectively with h _nthe longitude of the spin guided cartridge that the moment is calculated by inertial navigation system, latitude and height, φ _xand φ _zbe respectively t _nthe angle of pitch of the spin guided cartridge that the moment is calculated by inertial navigation system and course angle, K ₁, K ₂, K ₃, K ₄, K ₅for constant value;

(3.3) Kalman filtering is utilized to obtain speed correction, roll angle correction value and three acceleration bias:

By t _nthe east orientation speed V that moment GPS exports _ge, north orientation speed V _gnwith sky to speed V _guas measured value, calculate t _nmoment east orientation speed, north orientation speed and sky are to the measured deviation of speed:

dV _e＝V _e-V _ge

dV _n＝V _n-V _gn

dV _u＝V _u-V _gu

Wherein dV _efor t _nmoment east orientation velocity deviation, dV _nfor t _nmoment north orientation velocity deviation, dV _ufor t _nmoment, sky was to velocity deviation;

Utilize t _nthe east orientation speed in moment, north orientation speed and sky, to the measured deviation of speed, calculate t according to Kalman filtering algorithm _nthe state estimator X in moment;

X=[δ V _e, δ V _n, δ V _u, δ φ _y, ε _x, ε _y, ε _z], utilization state estimator X calculates east orientation speed correction δ V _e, north orientation speed correction δ V _n, sky is to speed correction δ V _u, roll angle correction value δ φ _y, three acceleration bias value ε _x, ε _y, ε _z;

The measurement battle array used in Kalman filtering $H\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\end{array}\right];$

(3.4) t is calculated _nthe implementation of the attitude information of moment spin guided cartridge reality is:

A () is by t _nmoment angle of pitch correction value roll angle correction value course angle correction value be updated to the attitude differential equation represented with Euler's krylov horn cupping in;

Wherein, represent that attitude angle is to the derivative of time T, $U=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sec {\mathrm{\φ}}_x& 0\\ 0& -\tan {\mathrm{\φ}}_x& 1\end{array}\right],$ $R_b^w=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& 0& \sin {\mathrm{\φ}}_y\\ \sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \cos {\mathrm{\φ}}_x& -\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\\ -\cos {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y& \sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right],$ $R_L^w=\left[\begin{array}{ccc}\cos \mathrm{\γ}& \sin \mathrm{\γ}& 0\\ -\sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right],$

drawn by gyroscope survey, ν _nt _n-1the north orientation speed of the spin guided cartridge reality in moment, ν _et _n-1the east orientation speed of the spin guided cartridge reality in moment, ω _iefor earth rotation speed, R _mfor earth radius of curvature of meridian, R _nfor earth radius of curvature in prime vertical, h is t _n-1the height of the spin guided cartridge in moment, t _n-1the latitude of the spin guided cartridge in moment, φ _xt _n-1the angle of pitch of the spin guided cartridge reality in moment, φ _yt _n-1the roll angle of the spin guided cartridge reality in moment, φ _zt _n-1the course angle of the spin guided cartridge reality in moment;

(b) t _nthe angle of pitch φ of moment spin guided cartridge reality _x', roll angle φ _y', course angle φ ' _zcomputing formula as follows:

$\left[\begin{array}{c}{\mathrm{\φ}}_x^{\′}\\ {\mathrm{\φ}}_y^{\′}\\ {{\mathrm{\φ}}_z}^{\′}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\\ {\mathrm{\φ}}_z\end{array}\right]+\stackrel{.}{\stackrel{\→}{\mathrm{\φ}}}+T;$

T is the sampling period of GPS;

(3.5) t is calculated _nthe implementation of the velocity information of moment spin guided cartridge reality is:

Feeding back in navigation calculation by resolving three speed correction obtained, calculating t _nmoment navigation speed information:

V _e′＝V _e-δV _e

V _n′＝V _n-δV _n

V _u′＝V _u-δV _u

Wherein, V _e' represent t _nthe east orientation speed of moment spin guided cartridge reality, V _n' represent t _nthe north orientation speed of moment spin guided cartridge reality, V _u' represent t _nthe sky of moment spin guided cartridge reality is to speed;

(3.6) t is calculated _nthe implementation of the positional information of moment spin guided cartridge reality is:

λ′＝λ-μ _λ

h′＝h-μ _h

λ ' expression t _nthe longitude of moment spin guided cartridge reality, represent t _nthe latitude of moment spin guided cartridge reality, h ' expression t _nthe height of moment spin guided cartridge reality.