CN103424127B - A kind of speed adds specific force coupling Transfer Alignment - Google Patents

Abstract

The invention provides a kind of speed and add specific force coupling Transfer alignment algorithm, it achieve dynamic environment lower within a short period of time to the accurate estimation of the misaligned angle of the platform.Its method is: according to principle of work and the feature of Platform Inertial Navigation System, sets up the velocity error model between main and sub inertial navigation and attitude error model; Set up the state equation of system filter according to gained SYSTEM ERROR MODEL, set up the observation equation of filtering using speed and specific force as observed quantity; Utilize Kalman filtering to estimate system state, finally obtain sub-platform inertial navigation relative to the misaligned angle of the platform.The present invention is applicable to the Transfer Alignment of dynamic condition lower platform inertial navigation system.

Description

A kind of speed adds specific force coupling Transfer Alignment

Technical field

The present invention relates to ship platform inertial navigation system Transfer Alignment, specifically a kind of speed adds specific force coupling Transfer Alignment.

Background technology

Transfer Alignment is a technology for resolved vector Initial Alignment under moving base condition.In Transfer Alignment process, need the navigation information introducing the main inertial navigation system of carrier with high accuracy, and with this information for benchmark, by certain navigation, eliminate the alignment error factor between two cover systems, determine sub-inertial navigation course, attitude and speed accurately.Be directed to Platform Inertial Navigation System, the attitude due to platform inertial navigation is by platform microsyn output, and it, because of factors such as synchronizer error, inertial navigation alignment error and carrier flexible deformations, makes alignment precision be greatly affected, the use of Chang Zuowei coarse alignment.Therefore for Platform Inertial Navigation System, traditional Transfer Alignment only has speeds match.And the high precision of Transfer Alignment and rapidity are the key factors of evaluation algorithms, because the speed and motor-driven of boats and ships is restricted, adopt traditional speeds match scheme that the Transfer Alignment time is greatly affected.

Application number is in the patent of invention file of 200910031769.9, discloses one " the specific force difference-product surely taking aim at gondola divides coupling Transfer Alignment and Combinated navigation method thereof ".The method use only specific force difference-product point coupling Transfer alignment algorithm.

Summary of the invention

A kind of speed that can shorten the ship platform inertial navigation Transfer Alignment time is the object of the present invention is to provide to add specific force coupling Transfer Alignment.

The object of the present invention is achieved like this:

(1) sub-inertial navigation is before entering navigation duty, and the bearer rate main inertial navigation provided, position and attitude information are as the initial velocity of sub-inertial navigation and position and attitude;

(2) in order to obtain the state error information of sub-inertial navigation system, setting up the state equation of Kalman filter, obtaining sub-inertial navigation state equation;

(3) speed utilizing main inertial navigation to provide and than force information, obtains the observational error information of sub-inertial navigation speed and specific force, obtains observation equation;

(4) state equation set up according to (2), (3) and observation equation, utilize Kalman filtering to carry out Transfer Alignment, complete the estimation of error of antithetical phrase inertial navigation.

Compared with the present invention is the technical scheme in the patent document of 200910031769.9 with application number, its application, the Transfer Alignment used are all different.

The beneficial effect of the inventive method is mainly manifested in: 1. the speed resolved using inertial device adds specific force as observed quantity, accelerometer precision is greatly improved, and versus speed has lacked an integration, more more direct on the impact of Kalman filtering than force information, for shortening Transfer Alignment filtering time successful; 2. under maneuvering condition, meet Transfer Alignment high precision and rapidity; 3. be measurement information than force information, directly obtain by accelerometer, carrying out it, the researchs such as time delay and deflection deformation are relative simple.The present invention can estimate the platform error angle of sub-inertial navigation relative to navigational system fast and accurately, compensates to improve navigation accuracy, have the remarkable advantages such as speed is fast, precision is high, calculated amount is little to it.

Accompanying drawing explanation

Fig. 1 is overall flow figure of the present invention;

Fig. 2 is Kalman filtering recursion flow process;

Fig. 3 is the platform error angular estimation curve utilizing two kinds of Transfer alignment algorithms to draw under hull linear uniform motion state;

Fig. 4 is the platform error angular estimation curve utilizing two kinds of Transfer alignment algorithms to draw under hull maneuvering condition.

Embodiment

Composition graphs 1, speed of the present invention adds specific force coupling Transfer alignment algorithm and comprises the following steps:

(1) sub-inertial navigation is before entering navigation duty, and the bearer rate main inertial navigation provided, position and attitude information are as the initial velocity of sub-inertial navigation and position and attitude.If the navigational coordinate system n of the Platform Requirements simulation of Platform INS Inertial is geographic coordinate system, the actual platform coordinate set up is n '.Because the error of calculation, error source affect and execute square error, n ' is that relative n system has deviation angle φ ⁿ.

(2) in order to obtain the state error information of Platform Inertial Navigation System, the state equation of Kalman filter is set up:

Choosing quantity of state is: $X={\left[\begin{array}{cccccccccc}{\mathrm{\δv}}_e& {\mathrm{\δv}}_n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T,$ Can obtain according to Platform Inertial Navigation System mechanization equation:

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{.}{v}}^n=f^n\×{\mathrm{\φ}}^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×\mathrm{\δv}+{\▿}^n\\ \mathrm{\δ}{\stackrel{.}{\mathrm{\φ}}}^n=-{\mathrm{\ω}}_{\mathrm{in}}^n\×{\mathrm{\φ}}^n+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{en}}^n+{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{.}{\▿}}^n=0\\ \mathrm{\δ}{\stackrel{.}{\mathrm{\ϵ}}}^n=0\end{array}\right.$

Wherein for boss's inertial navigation velocity error δ v ⁿabout the derivative of time t in the projection of n system; φ ⁿit is the platform error angle between main inertial navigation and sub-inertial navigation; f ⁿit is the projection that main inertial navigation specific force exports in n system; for earth rotation angular speed is the projection of n in navigation, for the projection of turning rate in n system that navigation is relative earth system; for sub-inertial navigation accelerometer zero is partially in the projection of n system. for the differential at attitude error angle between boss's inertial navigation, for partially opening ideal value deviation, and ε ^sfor sub-inertial navigation gyroscopic drift.

Then state equation is: $\stackrel{.}{X}=\mathrm{AX}+\mathrm{BW}$

Wherein

$A=\left[\begin{array}{cccc}{A}_{11}& A_{12}& I_{2\×3}& 0_{2\×3}\\ A_{21}& A_{22}& 0_{3\×3}& I_{3\×3}\\ 0_{2\×3}& 0_{2\×3}& 0_{2\×3}& 0_{2\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

$B=\left[\begin{array}{ccc}{I}_{2\×2}& 0_{2\×3}& 0_{2\×5}\\ 0_{3\×2}& I_{3\×3}& 0_{3\×5}\\ 0_{5\×2}& 0_{5\×3}& 0_{5\×5}\end{array}\right]$

In formula

$A_{12}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\end{array}\right]$

$A_{22}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{inu}}^n& -{\mathrm{\ω}}_{\mathrm{inn}}^n\\ -{\mathrm{\ω}}_{\mathrm{inu}}^n& 0& {\mathrm{\ω}}_{\mathrm{ine}}^n\\ {\mathrm{\ω}}_{\mathrm{inn}}^n& -{\mathrm{\ω}}_{\mathrm{ine}}^n& 0\end{array}\right].$

Noise battle array is: W=[w _axw _ayw _{ε x}w _{ε y}w _az0000 0] ^t

Wherein w _ax, w _ayfor accelerometer bias random white noise, w _{ε x}, w _{ε y}, w _{ε z}for gyroscopic drift random white noise.

(3) speed utilizing main inertial navigation to provide and than force information, obtain the observational error information of speed and specific force, observation equation is as follows:

Z=HX+V

With boss's inertial navigation for reference information, by the speed of sub-inertial navigation and the speed of specific force and main inertial navigation and specific force difference as observed quantity

Z=[δv _xδv _yδf _xδf _y] ^T

The pass of their every errors is:

$\left[\begin{array}{c}{Z}_1\\ Z_2\\ Z_3\\ Z_4\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ}{v}_e\\ \mathrm{\δ}{v}_n\\ \mathrm{\δ}{f}_e\\ \mathrm{\δ}{f}_n\end{array}\right]+\left[\begin{array}{c}\mathrm{\δ}{v}_{\mathrm{eMINS}}\\ \mathrm{\δ}{v}_{\mathrm{nMINS}}\\ \mathrm{\δ}{f}_{\mathrm{eMINS}}\\ \mathrm{\δ}{f}_{\mathrm{nMINS}}\end{array}\right]$

Wherein δ v _eMINS, δ v _nMINS, δ f _eMINS, δ f _nMINSit is main inertial navigation measuring error.

Observing matrix $H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -f_u& f_n& 1& 0& 0& 0& 0\\ 0& 0& f_u& 0& -f_e& 0& 1& 0& 0& 0\end{array}\right]$

Measurement noise V=[w _vxw _vyw _fxw _fy].

(4) state equation utilizing step 3 and step 4 to set up and observation equation, utilize Kalman filtering to carry out Transfer Alignment, complete the estimation of error of antithetical phrase inertial navigation, the Kalman filter formulation used is as follows:

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k[Z_k-H_k{\hat{X}}_{k,k-1}]$

$K_k=P_{k,k-1}{H}_k^T[H_k{P}_{k,k-1}{H}_k^T+R_k{]}^{-1}$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

$P_k=[I-K_k{H}_k]P_{k,k-1}[I-K_k{H}_k{]}^T+K_k{R}_k{K}_k^T$

P _k=[I-K _kH _k]P _k，k-1

$P_k^{-1}=P_{k,k-1}^{-1}+H_k^T{P}_k{H}_k$

Filtering recursion flow process is shown in Fig. 2.For Kalman filter, as gain matrix K _kwhen being reduced to a very low value, wave filter is by the measured value of dependence renewal one step less and less, and this just means, the state variable of all non-modelings can make filter divergence, in order to prevent the appearance of this phenomenon, to Q _kdiagonal entry with little on the occasion of, this is called imaginary process noise, like this after repetitive measurement, K _kreach the steady-state level of a non-zero, and the observation information of measurement is after this utilized.In addition, in order to make filter stability, can make that the priori covariance that designs a model is larger than normal value to be accomplished.

Below in conjunction with accompanying drawing the present invention done and describe further.The embodiment of the present invention is used for explaining and the present invention is described, instead of limits the invention, and in the protection domain of spirit of the present invention and claim, any amendment make the present invention and change, all fall into protection scope of the present invention.

Embodiment

As shown in Figure 3, Figure 4, under following starting condition, the estimation of Transfer Alignment and evaluated error are emulated, and com-parison and analysis has been carried out to simulation result.

Starting condition:

1) suppose that main inertial navigation is error free, lever arm effect error is fully compensated;

2) carrier initial position: longitude 117 °, 39 °, latitude.Carrier initial attitude angle (pitching, rolling, course) is respectively: 0 °, 0 °, 45 °; Sub-Inertial navigation platform initial error angle is ψ _x=5 ', ψ _y=5 ', ψ _z=5 ';

3) sub-inertial navigation gyro drift is 0.01 (°)/h, and random drift noise is 0.001 (°)/h; Accelerometer constant value zero is 1 × 10 partially ^-4g, random zero inclined noise is 1 × 10 ^-5g;

4) state estimation initial value is 0; Initial variance battle array P ₀the above-mentioned inertial device error of parameter foundation is arranged, and ensures that Kalman filters the optimality of estimation,

P ₀=diag{(0.1m／s) ²(0.1m／s) ²(5′) ²(5′) ²(5′) ²(1×10 ^-4g) ²(1×10 ^-4g) ²(1°×10 ^-3) ²(1°×10 ^-3) ²(1°×10 ^-3) ²)；

5) to be that carrier does at the uniform velocity motor-driven for Fig. 3, at first with the speed linear uniform motion of 10m/s.It is motor-driven that Fig. 4 is that carrier does serpentine, and at first with the speed linear uniform motion of 10m/s, the motor-driven turning rate of serpentine is 1 (°)/s.

Com-parison and analysis:

Fig. 3 provides boats and ships under at the uniform velocity state, the platform error angular estimation curve utilizing two kinds of Transfer alignment algorithms to draw, wherein solid line is the evaluated error value of speeds match to platform error angle, and dotted line is that speed adds the evaluated error value of specific force coupling to platform error angle.Being found out by Fig. 3 does at the uniform velocity under maneuvering condition at boats and ships, and speed adds the estimated time of specific force matching way for lateral error angle within 10s, and precision is high, and the estimated time at azimuthal error angle is within 80s, but precision is not high.As seen from Figure 4, do serpentine motor-driven at carrier, have had the estimated accuracy at azimuthal error angle and estimated time and significantly improved, therefore speed adds certain motor-driven of specific force coupling needs and reaches satisfied effect.Simultaneously comparison diagram 3 and Fig. 4 medium velocity add the estimation curve of specific force coupling and speeds match, can find out do motor-driven after, speed adds ratio match for estimated time at azimuthal error angle obtaining obvious lifting.

Claims (5)

1. speed adds a specific force coupling Transfer alignment algorithm, it is characterized in that comprising the steps:

(1) sub-inertial navigation is before entering navigation duty, and the bearer rate main inertial navigation provided, position and attitude information are as the initial velocity of sub-inertial navigation and position and attitude;

(2) set up the state equation of Kalman filter, obtain the state equation of sub-inertial navigation;

(3) speed utilizing main inertial navigation to provide and than force information, obtains the observational error information of sub-inertial navigation speed and specific force, obtains observation equation;

(4) state equation set up according to (2), (3) and observation equation, utilize Kalman filtering to carry out Transfer Alignment, complete the estimation of error of antithetical phrase inertial navigation, obtain the estimated value at sub-ins error angle.

2. speed according to claim 1 adds specific force coupling Transfer alignment algorithm, it is characterized in that the described state equation setting up Kalman filter comprises further:

Choosing quantity of state is: $X={\left[\begin{array}{cccccccccc}{\mathrm{\δv}}_e& {\mathrm{\δv}}_n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T$ Obtain according to Platform Inertial Navigation System mechanization equation:

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}^n=f^n\×{\mathrm{\φ}}^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×\mathrm{\δ}v+{\▿}^n\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\φ}}}^n=-{\mathrm{\ω}}_{in}^n\×{\mathrm{\φ}}^n+{\mathrm{\δ\ω}}_{ie}^n+{\mathrm{\δ\ω}}_{en}^n+{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{\·}{\▿}}^n=0\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\ϵ}}}^n=0\end{array}\right.$

Wherein for boss's inertial navigation velocity error δ v ⁿabout the derivative of time t in the projection of n system; φ ⁿit is the platform error angle between main inertial navigation and sub-inertial navigation; f ⁿit is the projection that main inertial navigation specific force exports in n system; for earth rotation angular speed is the projection of n in navigation, for the projection of turning rate in n system that navigation is relative earth system; for sub-inertial navigation accelerometer zero is partially in the projection of n system; for the differential at attitude error angle between boss's inertial navigation, for partially opening ideal value deviation, and ε ⁿfor sub-inertial navigation gyroscopic drift;

Then state equation is: $\stackrel{\·}{X}=AX+BW$

Wherein

$A=\left[\begin{array}{cccc}{A}_{11}& A_{12}& I_{2\×3}& 0_{2\×3}\\ A_{21}& A_{22}& 0_{3\×3}& I_{3\×3}\\ 0_{2\×3}& 0_{2\×3}& 0_{2\×3}& 0_{2\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

$B=\left[\begin{array}{ccc}{I}_{2\×2}& 0_{2\×3}& 0_{2\×5}\\ 0_{3\×2}& I_{3\×3}& 0_{3\×5}\\ 0_{5\×2}& 0_{5\×3}& 0_{5\×5}\end{array}\right]$

In formula

$A_{12}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\end{array}\right]$

$A_{22}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{inu}^n& -{\mathrm{\ω}}_{inn}^n\\ -{\mathrm{\ω}}_{inu}^n& 0& {\mathrm{\ω}}_{ine}^n\\ {\mathrm{\ω}}_{inn}^n& -{\mathrm{\ω}}_{ine}^n& 0\end{array}\right]\·$

System noise acoustic matrix is: W=[w _axw _ayw _{ε x}w _{ε y}w _{ε z}0000 0] ^t

Wherein w _ax, w _ayfor accelerometer bias random white noise, w _{ε x}, w _{ε y}, w _{ε z}for gyroscopic drift random white noise.

3. speed according to claim 1 and 2 adds specific force coupling Transfer alignment algorithm, it is characterized in that described observation equation is:

Z＝HX+V

With boss's inertial navigation for reference information, by the speed of sub-platform inertial navigation and the speed of specific force and main Platform INS and specific force difference as observed quantity

Z＝[δv _xδv _yδf _xδf _y] ^T

The pass of their every errors is:

$\left[\begin{array}{c}{Z}_1\\ Z_2\\ Z_3\\ Z_4\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δv}}_e\\ {\mathrm{\δv}}_n\\ {\mathrm{\δf}}_e\\ {\mathrm{\δf}}_n\end{array}\right]+\left[\begin{array}{c}{\mathrm{\δv}}_{eMINS}\\ {\mathrm{\δv}}_{nMINS}\\ {\mathrm{\δf}}_{eMINS}\\ {\mathrm{\δf}}_{nMINS}\end{array}\right]$

Wherein δ v _eMINS, δ v _nMINS, δ f _eMINS, δ f _nMINSit is main inertial navigation measuring error;

Observing matrix $H=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& -f_u& f_n& 1& 0& 0& 0& 0\\ 0& 0& f_u& 0& -f_e& 0& 1& 0& 0& 0\end{array}\right]$

Measurement noise V=[w _vxw _vyw _fxw _fy].

4. speed according to claim 1 and 2 adds specific force coupling Transfer alignment algorithm, it is characterized in that the formula of Kalman filtering is:

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\[Z_k-H_k{\hat{X}}_{k,k-1}\]$

$K_k=P_{k,k-1}{H}_k^T{\[H_k{P}_{k,k-1}{H}_k^T+R_k\]}^{-1}$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

$P_k=\[I-K_k{H}_k\]P_{k,k-1}{\[I-K_k{H}_k\]}^T+K_k{R}_k{K}_k^T$

P _k＝[I-K _kH _k]P _k,k-1

$P_k^{-1}=P_{k,k-1}^{-1}+H_k^T{R}_k{H}_k.$

5. speed according to claim 3 adds specific force coupling Transfer alignment algorithm, it is characterized in that the formula of Kalman filtering is:

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\[Z_k-H_k{\hat{X}}_{k,k-1}\]$

$K_k=P_{k,k-1}{H}_k^T{\[H_k{P}_{k,k-1}{H}_k^T+R_k\]}^{-1}$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

$P_k=\[I-K_k{H}_k\]P_{k,k-1}{\[I-K_k{H}_k\]}^T+K_k{R}_k{K}_k^T$

P _k＝[I-K _kH _k]P _k,k-1

$P_k^{-1}=P_{k,k-1}^{-1}+H_k^T{R}_k{H}_k.$