CN103697887A - Optimized navigation method based on strapdown inertial guidance and Doppler log - Google Patents

Abstract

The invention discloses an optimized navigation method based on strapdown inertial guidance and Doppler log. The optimized navigation method is characterized in that the navigation method is applied to a navigation system based on strapdown inertial guidance and Doppler log, wherein the navigation system comprises a Doppler log, a strapdown inertial measurement unit, a strapdown inertial measurement unit processing module and a central processing unit; the Doppler log comprises a transceiver, four transducers and an interface unit; the Doppler log is connected with the central processing unit through the interface unit; the strapdown inertial measurement unit comprises a three-axis gyroscope and a three-axis accelerator; the strapdown inertial measurement unit is connected with the strapdown inertial measurement unit processing module; the strapdown inertial measurement unit processing module is connected with the central processing unit; the navigation method comprises a plurality of steps, the method can be used for integrating information of a plurality of sub-navigation sensors to have complementary advantages so as to obtain navigation information with higher precision and better reliability.

Description

Optimization air navigation aid based on strap-down inertial guidance and Doppler log

Technical field

The present invention relates to a kind of optimization air navigation aid based on strap-down inertial guidance and Doppler log, belong under water and the navigation of navigation unit by water and tracking field, location.

Background technology

AUV (Autonomous Underwater Vehicle Autonomous Underwater Vehicle) is the important tool of exploring and develop marine environment and resource, also aspect the Military Application of ocean, is playing an important role.Because of its remoteness, disguise and independence, make the navigation problem of AUV be still one of major technique challenge facing at present.Because underwater environment is complicated, often adopt integrated navigation mode to carry out navigator fix to AUV, and integrated navigation system design of filter is the key that guarantees navigation accuracy.The navigation and positioning accuracy of current conventional AUV can reach <0.8nmile/h left and right, has met to a certain extent the accuracy requirement of AUV navigator fix.

SINS (Strapdown Inertial Navigation Systems strapdown inertial navigation system) system has independent navigation ability, be not subject to environment, carrier is motor-driven and the impact of radio interference, can be continuous the navigator fix information such as carrier positions, speed and attitude are provided, its Data Update frequency is fast, and has at short notice higher relative accuracy.But, prolongation along with System production time, the navigation error of strapdown inertial guidance system can accumulate growth thereupon, now just need to utilize the observation information of external sensor by filtering algorithm, to carry out correction-compensation strapdown inertial guidance system, the error accumulating in time to suppress it.

DVL (Doppler Velocity Log Doppler log) is widely used under water and the velocity measuring device of navigation unit by water combined system, has the acoustic navigation positioning system of real-time output carrier three-dimensional velocity and voyage ability.Doppler system is tried to achieve sound wave frequency displacement by being installed on four transducer information of AUV bottom, and then calculates carrier at the three-dimensional velocity information of carrier system, has the advantages such as precision is high, reliability, real-time.But due to underwater environment is complicated, dead reckoning alliance error is accumulated in time and during the long boat of AUV, disguised requirement, the conventional filtering algorithm precision of the dead reckoning combined system based on strap-down inertial guidance/Doppler is difficult to reach high precision navigation locating function demand.

H ∞ filtering algorithm based on Krein space is not made any hypothesis to system statistics characteristic, has advantages of that Filtering Model is simple, robustness good, is therefore often applied to AUV navigational system.But H ∞ filter tracks mobile process ability a little less than, real-time is difficult to guarantee.When the impact due to factors such as model uncertainties, while causing the state estimation value of wave filter to depart from system state, will inevitably in average and the amplitude of output residual sequence, show to some extent, if now adjust online time-varying gain matrix, when making system estimation error variance minimum, keep residual sequence still mutually orthogonal, meet orthogonality principle, can force wave filter to keep the high precision tracking to real system state.The present invention implements to fade to the data in past by introduce suboptimum fading factor in H ∞ filtering algorithm, to weaken the impact of old data on current filtering estimated value, can be by real-time adjusting estimation error variance matrix and corresponding gain matrix to meet orthogonality principle, force the high precision tracking of wave filter to real system state, and farthest extract and export all effective informations in residual error, in order further to improve the adaptive ability of wave filter, to take the solving of the matrix that fades that fading factor is diagonal entry, do optimization process, make wave filter no longer responsive to initial parameter, obtain a kind of optimization filtering algorithm of high tracking performance.This filtering algorithm has the stronger robustness about model uncertainty and the tracking power to system mobile process, and kept the simple advantage of H ∞ Filtering Model, use the optimization filtering algorithm proposing can significantly improve tracking performance on the basis that guarantees navigation accuracy, contribute to further to promote strapdown inertial navitation system (SINS) navigator fix performance, meet the requirement of modern AUV navigational system to disguise, accuracy, applicability, also for AUV integrated navigation system provides a brand-new thinking, there is important theory significance and engineering practical value.

AUV integrated navigation system based on strap-down inertial guidance/Doppler is by strap-down inertial guidance, the advantage of Doppler and optimization filtering algorithm proposed by the invention combines, the real-time navigation tracking parameter of carrier with high accuracy can be provided, solve AUV navigational system and be subject to underwater complex environmental impact, the navigation positioning error of strap-down inertial guidance continue in time continuous accumulation do not reach accuracy requirement and conventional H ∞ filtering algorithm in application process, there will be to mobile process tracking performance a little less than, the problem that causes filtering accuracy to decline, system navigation and positioning accuracy and robustness have been improved.

Summary of the invention

Goal of the invention: the object of the invention is to overcome the deficiencies in the prior art, proposed a kind of optimization filtering method for the AUV integrated navigation system based on strap-down inertial guidance/Doppler.Utilize designed wave filter that Doppler range rate measurement information is introduced to AUV combined system, to assist strap-down inertial guidance to navigate and track and localization.The method can be integrated a plurality of sub-navigation informations, overcome AUV navigational system easily navigation positioning error affected by environment, strapdown inertial guidance system continue in time continuous accumulation do not reach accuracy requirement and conventional H ∞ filtering algorithm in application process, there will be to mobile process tracking performance a little less than, cause the defect of filtering algorithm precise decreasing.

Technical scheme: a kind of optimization air navigation aid based on strap-down inertial guidance and Doppler log of the present invention, this air navigation aid is applied to a navigational system based on strap-down inertial guidance and Doppler log, and this navigational system comprises Doppler log, quick-connecting inertia measurement unit, quick-connecting inertia measurement cell processing module and CPU (central processing unit); Described Doppler log comprises transceiver, four transducers and interface unit; Described Doppler log is connected with CPU (central processing unit) by this interface unit; Described quick-connecting inertia measurement unit comprises three axle gyro and three axis accelerometers; Described quick-connecting inertia measurement unit is connected with quick-connecting inertia measurement cell processing module; Described quick-connecting inertia measurement cell processing module is connected with CPU (central processing unit); This air navigation aid comprises the following steps:

(1) by three axle gyros in quick-connecting inertia measurement unit block, record three axis angular rate information and accelerometer records 3-axis acceleration information, quick-connecting inertia measurement cell processing module receives the navigation information of quick-connecting inertia measurement unit output, by navigation integral and calculating, obtains the navigation informations such as carrier positions, speed and attitude;

(2) in the transceiver by Doppler log, launch electric oscillation signal, give transducer;

(3) four transducers transmit ultrasonic waves by Doppler log and receive the reflection echo with frequency shift property;

(4) utilize echoed signal that in transceiver, receiving system is sent transducer here to give interface unit through amplifying to try to achieve Doppler shift and be converted to speed of a ship or plane simulating signal after processing.If carrier headway is V, the beam transmission angle of depression is θ, velocity of sound C ≈ 1500m/s, and single beam frequency displacement computing formula is: Δ f=2Vf ₀cos θ/C, can obtain thus speed computing formula and be: V=Δ fC/ (2f ₀cos θ);

(5) utilize four speed of a ship or plane simulating signals that in transceiver, interface unit is sent transceiver here to be converted to the speed of a ship or plane, and export to CPU (central processing unit) with digital form;

(6) the real-time three-dimensional carrier that the arithmetical unit in CPU (central processing unit) periodically calculates carrier according to the velocity information of the interface unit output of Doppler log is speed amount;

(7) filtration module in CPU (central processing unit) carries out filtering fusion calculation to the three-dimensional velocity information of the strap-down inertial navigation information receiving, Doppler log and obtains k inertia Strapdown inertial measurement system compensation amount constantly

after correction, obtain final high-precision navigator fix information;

The method that described filtering fusion calculation obtains inertia Strapdown inertial measurement system compensation amount is:

Using strap-down inertial as with reference to system, state variable X gets velocity error

attitude error (φ _x, φ _y, φ _z), the random normal value biasing of accelerometer

and Gyro Random Constant Drift (ε _x, ε _y, ε _z), totally 10 tie up: $X={[{\mathrm{\&delta;V}}_e^n,{\mathrm{\&delta;V}}_n^n,{\mathrm{\&phi;}}_x,{\mathrm{\&phi;}}_y,{\mathrm{\&phi;}}_z,{\&dtri;}_x,{\&dtri;}_y,{\mathrm{\&epsiv;}}_x,{\mathrm{\&epsiv;}}_y,{\mathrm{\&epsiv;}}_z]}^T;$ W=[w _ax, w _ay, w _gx, w _gy, w _gz, 0,0,0,0,0] ^tfor system noise vector;

The carrier that the speed that the strap-down inertial of usining calculates and Doppler log record both differences that is speed after being transformed into navigation system are as the measuring value of system, and measurement equation is expressed as:

$Z=\left[\begin{array}{c}{V}_{\mathrm{SINSe}}^n-C_b^n{V}_{\mathrm{DVLe}}\\ V_{\mathrm{SINSn}}^n-C_b^n{V}_{\mathrm{DVLn}}\end{array}\right]=\mathrm{HX}+V$

Wherein, observation vector is

measurement noise vector V=[w _vx, w _vy] ^t, system measurements matrix is: $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\end{array}\right],$ System state equation and measurement equation discretize can be obtained to discrete system filtering equations;

Utilize initial value

and P ₀, according to k measurement Z constantly _kjust can recursion calculate k state estimation constantly ${\hat{X}}_k(k=\mathrm{1,2,3},...):$

${\hat{X}}_k={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1})$

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

Wherein,

$P_k={\mathrm{\&Gamma;}}_{k,k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T+\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}\right\}{\mathrm{\&Lambda;}}^T\left(k\right)$

By orthogonality principle, have:

${\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T=H_k\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\right\}{\mathrm{\&Lambda;}}^T\left(k\right)H_k^T$

For general situation, H _krow dimension is less, has following approximate data:

$\left\{\begin{array}{c}{\left(H_k^T{H}_k\right)}^{-1}{H}_k^T\&ap;H_k^T\\ H_k{\left(H_k^T{H}_k\right)}^{-1}\&ap;H_k\end{array}\right.$

So can convert, obtain:

$H_k^T\{{\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T\}{H}_k=\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}\right\}{\mathrm{\&Lambda;}}^T\left(k\right)$

Definition M (k) and N (k) are:

$\left\{\begin{array}{c}M\left(k\right)={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\\ N\left(k\right)=H_k^T\{{\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T\}{H}_k\end{array}\right.$

V in formula _{0, k}calculation method is the same, and substitution can obtain: N (k)=Λ (k) M (k) Λ ^t(k)

By approximate the resolving of above formula, obtained:

${\mathrm{\&lambda;}}_{0i,k}=\sqrt{\frac{N_{\mathrm{ii}}\left(k\right)}{M_{\mathrm{ii}}\left(k\right)}}$

Each element fading in matrix Λ (k) is:

${\mathrm{\&lambda;}}_i\left(k\right)=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_{0i,k},& {\mathrm{\&lambda;}}_{0i,k}\&GreaterEqual;1\\ 1,& {\mathrm{\&lambda;}}_{0i,k}\&le;1\end{array}\right.$

N in formula _iiand M _iiitem is respectively i element of principal diagonal of N (k) and M (k).

In order to improve the applicability of this algorithm in real system, can adjust coefficient a, artificial adjusting fading factor λ by further introducing _i(k), have:

${\mathrm{\&lambda;}}_i\left(k\right)=\left\{\begin{array}{cc}a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k},& a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k}\&GreaterEqual;1\\ 1,& a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k}\&le;1\end{array}\right.$

Wherein adjust specifically choosing according to actual conditions and determine of coefficient a, desirable [0.1,10], generally gets a=5.

Further, the three-axis gyroscope three axle quadratures of the quick-connecting inertia measurement unit of described navigational system are installed, and described three axis accelerometer three axle quadratures are installed.

Further, the three-axis gyroscope of the quick-connecting inertia measurement unit of described navigational system is fibre optic gyroscope.

Further, the 3-axis acceleration of described navigational system is counted quartz flexible accelerometer.

Further, the transceiver of the Doppler log of described navigational system comprises emission coefficient, receiving system and calculating compensating circuit.

Further, described navigational system also comprises display, and described display is connected with arithmetical unit, shows the navigation information that central processing unit computing obtains.

In this air navigation aid, quick-connecting inertia measurement unit is used for exporting inertia measurement data; Quick-connecting inertia measurement cell processing module, for receiving the digital signal of quick-connecting inertia measurement unit output, obtains carrier positions, speed and attitude data by navigation integral and calculating; Doppler log is for measuring carrier with respect to longitudinal velocity and the transverse velocity in seabed; Be installed on the transducer of AUV bottom for launching ultrasound wave and receiving reflection echo; The transceiver that is installed on AUV bottom comprises emission coefficient, receiving system and calculates the unit such as compensating circuit; Interface unit is converted to the speed of a ship or plane for the speed of a ship or plane signal that transceiver is sent here, and to external unit, exports data with digital form or analog form; CPU (central processing unit) comprises an arithmetical unit and an optimization filtration module, can receive the navigation information of above-mentioned quick-connecting inertia measurement cell processing module, Doppler log, by Real-Time Filtering, calculate, the parameter of revising quick-connecting inertia measurement cell processing module, obtains final integrated navigation information data.The method can be integrated a plurality of sub-navigation sensor information, has complementary advantages to obtain more high precision and more reliable navigation information.

The present invention can be applicable to AUV system, and AUV is the important tool of exploring and develop marine environment and resource, also aspect the Military Application of ocean, is playing an important role.Because of its remoteness, disguise and complicated underwater environment, often adopt integrated navigation mode to carry out navigator fix to AUV.Strapdown inertial guidance system has independent navigation ability, be not subject to environment, carrier is motor-driven and the impact of radio interference, can be continuous the navigator fix information such as carrier positions, speed and attitude are provided, its Data Update frequency is fast, and has at short notice higher relative accuracy.But the prolongation along with System production time, the navigation error of strapdown inertial guidance system can accumulate growth thereupon, now just need to utilize the observation information of external sensor by filtering algorithm, to carry out correction-compensation strapdown inertial guidance system, the error accumulating in time to suppress it.Doppler is widely used under water and the velocity measuring device of navigation unit by water combined system, has the acoustic navigation positioning system of real-time output carrier three-dimensional velocity and voyage ability.There is the advantages such as precision is high, reliability, real-time.But due to underwater environment is complicated, dead reckoning alliance error is accumulated in time and during the long boat of AUV, disguised requirement, the conventional filtering algorithm precision of the dead reckoning combined system based on strap-down inertial guidance/Doppler is difficult to reach high precision navigation locating function demand.H ∞ filtering algorithm based on Krein space is not made any hypothesis to system statistics characteristic, has advantages of that Filtering Model is simple, robustness good.But H ∞ filter tracks mobile process ability a little less than, real-time is difficult to guarantee.The present invention implements to fade to the data in past by introduce suboptimum fading factor in H ∞ filtering algorithm, to weaken the impact of old data on current filtering estimated value, can be by real-time adjusting estimation error variance matrix and corresponding gain matrix to meet orthogonality principle, force the high precision tracking of wave filter to real system state, and farthest extract and export all effective informations in residual error, in order further to improve the adaptive ability of wave filter, to take the solving of the matrix that fades that fading factor is diagonal entry, do optimization process, make wave filter no longer responsive to initial parameter, obtain a kind of optimization filtering algorithm of high tracking performance.This filtering algorithm has the stronger robustness about model uncertainty and the tracking power to system mobile process, and kept the simple advantage of H ∞ Filtering Model, utilize the optimization filtering technique proposing that the three-dimensional velocity information of strap-down inertial guidance navigation information, Doppler log is carried out to data fusion calculating, on the basis that guarantees navigation accuracy and system robustness, can significantly improve tracking performance, further promote strapdown inertial navitation system (SINS) navigator fix performance.

Be described as follows:

(1) earth geometric model and physical parameter

1. the figure of the earth is described

Inertial navigation resolves middle use WGS-84 reference ellipsoid as earth model, its equatorial radius major semi-axis R _e=6378137.0m, flattening of ellipsoid e=1/298.257223563.Radius of meridional section R _nwith radius of curvature in prime vertical R _ecan be obtained by following Solving Equations:

$\left\{\begin{array}{c}{R}_N=\frac{R_e{(1-e)}^2}{{[{(1-e)}^2{\sin }^{2}L+{\cos }^{2}L]}^{\frac{3}{2}}}\\ R_E=\frac{R_e}{{[{(1-e)}^2{\sin }^{2}L+{\cos }^{2}L]}^{\frac{1}{2}}}\end{array}\right.$

2. earth rotation angular speed (ω _ie)

In inertial navigation system, rotational-angular velocity of the earth is:

ω _ie=15.0411°／h=7.29211585×10 ^-5rad／s

3. terrestrial gravitation acceleration (g)

The acceleration of gravity of WGS-84 reference ellipsoid is:

$g=9.7803267714\&times;\frac{1+0.00193185138639{\sin }^{2}L}{\sqrt{1-0.00669437999013{\sin }^{2}L}}{(1+\frac{h}{R_e})}^{-2}$

In formula, L represents local latitude, and h represents height, R _efor equatorial radius major semi-axis.

(2) Coordinate system definition

1. geocentric inertial coordinate system (i system)

Use ox _iy _iz _irepresent, initial point is positioned at ground ball center, ox _iwith oy _iaxle in earth equatorial plane, ox _iaxle points to the first point of Aries, oz _iaxle and earth's axis overlap, oy _iwith ox _i, oz _iform right hand rectangular coordinate system.Three coordinate axis are fixed in the sensing of inertial space.The output of quick-connecting inertia measurement unit is reference data with i.

2. terrestrial coordinate system (e system)

Use ox _ey _ez _erepresent, initial point is positioned at ground ball center, oz _eaxle and earth's axis overlap, ox _eaxle is along the intersection of Greenwich meridian ellipse and earth equatorial plane, oy _eaxle is under the line in plane, ox _e, oy _e, oz _ethree axles form right hand rectangular coordinate system.Terrestrial coordinate system and the earth are connected, and e is that the angular speed rotating relative to i system is rotational-angular velocity of the earth ω _ie.

3. geographic coordinate system (g system)

Use ox _gy _gz _grepresent, initial point is positioned at carrier center of gravity, generally adopts sky, northeast coordinate system as geographic coordinate system, i.e. ox _gaxle points to east (E), oy _gaxle energized north (N), oz _gaxle points to day (U).

4. navigation coordinate is (n system)

Use ox _ny _nz _nrepresent, adopt sky, northeast geographic coordinate system (ENU) as navigation coordinate to be.

5. carrier coordinate system (b system)

Use ox _by _bz _brepresent, initial point is generally got quick-connecting inertia measurement cell geometry center, ox _baxle along carrier transverse axis to the right, oy _baxle before carrier Y, oz _baxle along carrier vertical shaft upwards.

(3) transformational relation between attitude angle and attitude matrix

1. attitude angle definition

Course angle: carrier longitudinal axis oy _bangle between projection on surface level and geographic meridian north orientation, is called course angle, counts ψ.Course angle numerical value be take geographic north to calculating as starting point clockwise direction, and its field of definition is 0～360 °.

The angle of pitch: carrier is around transverse axis ox _bduring rotation, the angle of the carrier longitudinal axis and surface level, is called the angle of pitch, is designated as θ.The angle of pitch is counted from surface level,, for just, is upwards downwards negative, and its field of definition is-90 °～+ 90 °.

Roll angle: the longitudinal symmetrical plane of carrier and the longitudinally angle between vertical plane, be called roll angle, be designated as γ.Roll angle is counted from vertical plane, and Right deviation is for just, and left-leaning for negative, its field of definition is-180 °～+ 180 °.

2. attitude matrix

Realizing the conversion that carrier coordinate system b is tied to geographic coordinate system (navigation be n system) can obtain through three rotation in the following order, is sequentially specifically: around-oz _gaxle turns ψ angle, around ox ₁axle turns θ angle, then around oy ₂axle turns γ angle.Transformational relation between them is shown below.

Rotating corresponding transformation matrix three times is respectively

$C_{\mathrm{\&psi;}}=\left[\begin{array}{ccc}\cos \mathrm{\&psi;}& -\sin \mathrm{\&psi;}& 0\\ \sin \mathrm{\&psi;}& \cos \mathrm{\&psi;}& 0\\ 0& 0& 1\end{array}\right]$

$C_{\mathrm{\&theta;}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ 0& -\sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right]$

$C_{\mathrm{\&gamma;}}=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}& 0& -\sin \mathrm{\&gamma;}\\ 0& 1& 0\\ \sin \mathrm{\&gamma;}& 0& \cos \mathrm{\&gamma;}\end{array}\right]$

So geographic coordinate is tied to the transition matrix of carrier coordinate system:

$C_g^b=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \sin \mathrm{\&psi;}\cos \mathrm{\&theta;}& \cos \mathrm{\&psi;}\cos \mathrm{\&theta;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}\sin \mathrm{\&theta;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}\sin \mathrm{\&theta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]$

The geographic coordinate system of usining is to have strapdown inertial navigation system attitude matrix to be as navigation coordinate

$C_b^n={\left(C_g^b\right)}^T$

3. the transformational relation of attitude angle and attitude matrix

$C_b^n={\left(C_g^b\right)}^T={\left[T_{\mathrm{ij}}\right]}_{3\&times;3}$

Can solve attitude matrix to the transformational relation of principal value interval attitude angle is:

(4) strapdown inertial guidance system ultimate principle

1. the strapdown inertial guidance system differential equation

The attitude matrix differential equation is:

Wherein,

for carrier angular velocity, by gyroscope, recorded,

and have:

${\mathrm{\&omega;}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L\end{array}\right],{\mathrm{\&omega;}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\frac{V_N^n}{R_N+h}\\ \frac{V_E^n}{R_E+h}\\ \frac{V_E^n}{R_E+h}\tan L\end{array}\right]$

In formula, $V^n={\left[\begin{array}{ccc}{V}_E^n& V_N^n& V_U^n\end{array}\right]}^T$ For the size of carrier ground speed in navigation system.

The specific force equation of carrier in navigation system is: ${\stackrel{\&CenterDot;}{V}}^n=f^n-(2{\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)\&times;V^n+g^n$

In formula

f ^bfor the specific force that accelerometer records, g ⁿ=[0 0-g] ^t.

The differential equation is upgraded in position: usining in the system of geographic coordinate system as navigation coordinate system, the differential equation of latitude L, longitude λ and height h is:

$\stackrel{\&CenterDot;}{L}=\frac{V_N^n}{R_N+h},\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{V_E^n}{(R_E+h)\cos L},\stackrel{\&CenterDot;}{h}=V_U^n$

2. strapdown inertial guidance system error model

Gyro error model: ε _i=ε _bi+ ε _wi(i=x, y, z), ε _bifor arbitrary constant, ε _wifor white noise; Accelerometer error model: ${\&dtri;}_i={\&dtri;}_{\mathrm{bi}}+{\&dtri;}_{\mathrm{wi}}(i=x,y,z),{\&dtri;}_{\mathrm{bi}}$ For arbitrary constant,

for white noise:

Attitude error equations is: ${\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}^n=-{\mathrm{\&omega;}}_{\mathrm{in}}^n\&times;{\mathrm{\&phi;}}^n+\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{ie}}^n+\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{en}}^n-C_b^n{\mathrm{\&epsiv;}}^b,{\mathrm{\&phi;}}^n={\left[\begin{array}{ccc}{\mathrm{\&phi;}}_x& {\mathrm{\&phi;}}_y& {\mathrm{\&phi;}}_z\end{array}\right]}^T$ For attitude error, ε ^b=[ε _xε _yε _z] ^tfor gyro error.

Velocity error equation is:

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}^n=-\mathrm{\&phi;}\&times;f^n-(2\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{ie}}^n+\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{en}}^n)\&times;V^n-(2{\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)\&times;\mathrm{\&delta;}{V}^n+C_b^n{\&dtri;}^b,$

${\mathrm{\&delta;V}}^n={\left[\begin{array}{ccc}{\mathrm{\&delta;V}}_e^n& {\mathrm{\&delta;V}}_n^n& {\mathrm{\&delta;V}}_u^n\end{array}\right]}^T$ For velocity error, ${\&dtri;}^b={\left[\begin{array}{ccc}{\&dtri;}_x& {\&dtri;}_y& {\&dtri;}_z\end{array}\right]}^T$ For accelerometer error.

Site error equation is:

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{L}=\frac{\mathrm{\&delta;}{V}_n^n}{R_N+h}-\frac{V_N^n}{{(R_N+h)}^2}\mathrm{\&delta;h}$

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{\sec L}{R_E+h}\mathrm{\&delta;}{V}_e^n+\frac{V_E^n\sec L\tan L}{R_E+h}\mathrm{\&delta;L}-\frac{V_E^n\sec L}{{(R_E+h)}^2}\mathrm{\&delta;h}$

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{h}=\mathrm{\&delta;}{V}_u^n$

(5) filtering algorithm

1. H ∞ filtering algorithm is introduced

Consider as follows based on Krein spatial spreading state space system model:

$\left\{\begin{array}{c}{X}_k={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}+{\mathrm{\&Gamma;}}_{k,k-1}{W}_{k-1}\\ \left[\begin{array}{c}{Z}_k\\ y_k\end{array}\right]=\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]X_k+V_k\end{array}\right.$

X _kfor t _kconstantly treat estimating system state vector;

Φ _{k, k-1}for t _k-1constantly to t _kmatrix of shifting of a step constantly;

Γ _k-1for system noise drives battle array;

Z _kfor state quantity measurement vector;

H _kfor measuring battle array;

W _k-1for system incentive noise sequence;

V _kfor measurement noise sequence;

X _{k, k-1}for system state one-step prediction;

P _kfor estimation error variance battle array;

K _kfor filter gain matrix;

Y _kgiven system state X _klinear combination observed quantity.

Wherein, W _kand V _kfor l ₂the noise of energy bounded,

its statistical property is not done to any hypothesis.The original state of uniting of setting up departments is X ₀,

expression is to system initial state X ₀an estimation, definition initial estimation error covariance matrix is:

Order

be illustrated in given observed reading { Z _kunder condition to y _kestimation, the filtering error being defined as follows $e_k={\hat{y}}_k-L_k{X}_k.$

If T _k(F _f) represent unknown disturbances

map to filtering error { e _ktransport function.Suboptimum H ∞ estimation problem is described: given positive number γ >0, finds suboptimum H ∞ and estimates

make || T _k(F _f) || _∞< γ, meets

$\underset{F_f}{\inf }\underset{{\mathrm{<figure-callout\; id="0"\; label="X"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">X</figure-callout>}}_0,W,V\&Element;h_2}{\sup }\frac{{\left|\right|e_k\left|\right|}_2^2}{{\left|\right|X_0-{\hat{X}}_0\left|\right|}_{P_0^{-1}}^2+{\left|\right|W_k\left|\right|}_2^2+{\left|\right|V_k\left|\right|}_2^2}<{\mathrm{\&gamma;}}^2$

For given γ >0, if [Φ _{k, k-1}, Г _{k, k-1}] be full rank, satisfy condition || T _k(F _f) || _∞the wave filter of≤γ exists, and and if only if to all k, has

$P_k^{-1}+H_k^T{H}_k-{\mathrm{\&gamma;}}^{-2}{L}_k^T{L}_k>0$

Wherein, P _kmeet following recursion Riccati equation

$P_k={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k,k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T$

$R_{e,k}=\left[\begin{array}{cc}I& 0\\ 0& {-\mathrm{\&gamma;}}^{2}I\end{array}\right]+\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_k\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]$

If above-mentioned inequality formula is set up, can obtain H ∞ filtering recursive algorithm as follows:

${\hat{y}}_k=L_k{\hat{X}}_k$

${\hat{X}}_k={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1})$

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

Suboptimum H ∞ filtering algorithm is at P _ksolution procedure in, given positive number γ value is passed through R _{e, k}and then adjustment P _k, the robustness of filtering algorithm is better.As long as given initial value

and P ₀, according to k measurement Z constantly _kjust can recursion calculate k state estimation constantly ${\hat{X}}_k(k=\mathrm{1,2,3},...).$

2. improve algorithm

Filter status estimated value has following general structure:

In formula, $\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1},\\ {\mathrm{\&gamma;}}_k=Z_k-H_k{\hat{X}}_{k,k-1}\end{array},$ γ _krepresent k residual vector constantly;

Suitable time-varying gain matrix K of on-line selection _k, make to meet following condition, i.e. orthogonality principle simultaneously:

$E\left[(X_k-{\hat{X}}_k){(X_k-{\hat{X}}_k)}^T\right]=\min$

$E\left[{\mathrm{\&gamma;}}_{k+j}{\mathrm{\&gamma;}}_k^T\right]=0,k=\mathrm{0,1,2},...,j=\mathrm{1,2},...$

When the impact due to factors such as model uncertainties, while causing the state estimation value of wave filter to depart from system state, will inevitably be in performance to some extent in the average of output residual sequence and amplitude, if the online gain gust K that adjusts now _k, make residual sequence still keep mutually orthogonal, can force wave filter to keep the tracking to real system state.

In H ∞ filtering algorithm, introduce suboptimum fading factor the data in past are implemented to fade, to weaken the impact of old data on current filtering estimated value, by real-time adjusting estimation error variance matrix P _kand corresponding gain matrix K _kforce output residual error to be approximately white Gaussian noise, can farthest extract all effective informations in output residual error, obtain the filtering algorithm that a kind of tracking performance is stronger.

By P _kwrite as following form:

$P_k={\mathrm{\&Gamma;}}_{k,k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T+\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}\right\}$

Wherein: $\mathrm{\&Lambda;}\left(k\right)=\mathrm{diag}[{\mathrm{\&lambda;}}_1\left(k\right),{\mathrm{\&lambda;}}_2\left(k\right),...{\mathrm{\&lambda;}}_n\left(k\right)],{\mathrm{\&lambda;}}_i\left(k\right)\&GreaterEqual;1;i=\mathrm{1,2},...,n.$ First, according to system priori, roughly determine initial value: λ ₁(k): λ ₂(k): ...: λ _n(k)=α ₁: α ₂: ...: α _n, in formula, α _i>=1 (i=1,2 ..., n), can obtain λ _i(k) approximate data is as follows:

${\mathrm{\&lambda;}}_i\left(k\right)=\left\{\begin{array}{cc}{\mathrm{\&alpha;}}_i{\mathrm{\&lambda;}}_{0i,k},& {\mathrm{\&alpha;}}_i{\mathrm{\&lambda;}}_{0i,k}\&GreaterEqual;1\\ 1,& {\mathrm{\&alpha;}}_i{\mathrm{\&lambda;}}_{0i,k}\&le;1\end{array}\right.$

Wherein,

${\mathrm{\&lambda;}}_{0i,k}=\frac{\mathrm{tr}\left[N\left(k\right)\right]}{\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_i\&CenterDot;M_{\mathrm{ii}}\left(k\right)}$

$N\left(k\right)={\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T-{\mathrm{\&beta;I}}_k$

$M\left(k\right)=\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]\end{array}{R}_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\right\}{H}_k^T{H}_k$

Tr (A) represents matrix A to ask mark computing, the V in formula _{0, k}by following formula, calculated:

${\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_1{\mathrm{\&gamma;}}_1^T,& k=0\\ \frac{{\mathrm{\&rho;<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k-1}+{\mathrm{\&gamma;}}_k{\mathrm{\&gamma;}}_k^T}{1+\mathrm{\&rho;}},& k\&GreaterEqual;1\end{array}\right.$

In above formula, 0.95≤ρ≤1 is forgetting factor, generally gets ρ=0.95; β >=1 is a selected reduction factor, and introducing this reduction factor object is to make state estimation value more level and smooth.

When in system, some state variable is subject to larger interference or undergos mutation, the matrix Λ (k) that fades regulates according to fixing proportionate relationship all states simultaneously, rather than carries out self-adaptation adjusting according to certain state.And in the derivation of matrix Λ (k) that fades, the value of Λ (k) and initial value λ ₁(k): λ ₂(k): ...: λ _n(k)=α ₁: α ₂: ...: α _nrelevant, thus the adaptive ability of wave filter reduced, and in actual applications, wish that wave filter is insensitive to initial parameter.

For AUV integrated navigation system, consider H _kbe not square formation and

h _kbe the situation of irreversible square formation, the method for solving of the matrix Λ (k) that fades has been carried out to following optimization.Revise P _kas follows:

$P_k={\mathrm{\&Gamma;}}_{k,k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T+\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}\right\}{\mathrm{\&Lambda;}}^T\left(k\right)$

By orthogonality principle, have:

${\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T=H_k\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\right\}{\mathrm{\&Lambda;}}^T\left(k\right)H_k^T$

For general situation, H _krow dimension is less, has following approximate data:

$\left\{\begin{array}{c}{\left(H_k^T{H}_k\right)}^{-1}{H}_k^T\&ap;H_k^T\\ H_k{\left(H_k^T{H}_k\right)}^{-1}\&ap;H_k\end{array}\right.$

So can convert, obtain:

$H_k^T\{{\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T\}{H}_k=\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}\right\}{\mathrm{\&Lambda;}}^T\left(k\right)$

Definition M (k) and N (k) are

$\left\{\begin{array}{c}M\left(k\right)={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\\ N\left(k\right)=H_k^T\{{\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T\}{H}_k\end{array}\right.$

V in formula _{0, k}calculation method is the same, and substitution can obtain: N (k)=Λ (k) M (k) Λ ^t(k)

By approximate the resolving of above formula, obtained:

${\mathrm{\&lambda;}}_{0i,k}=\sqrt{\frac{N_{\mathrm{ii}}\left(k\right)}{M_{\mathrm{ii}}\left(k\right)}}$

Each element fading in matrix Λ (k) is:

${\mathrm{\&lambda;}}_i\left(k\right)=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_{0i,k},& {\mathrm{\&lambda;}}_{0i,k}\&GreaterEqual;1\\ 1,& {\mathrm{\&lambda;}}_{0i,k}\&le;1\end{array}\right.$

N in formula _iiand M _iiitem is respectively i element of principal diagonal of N (k) and M (k).

In order to improve the applicability of this algorithm in real system, can adjust coefficient a, artificial adjusting fading factor λ by further introducing _i(k), have:

${\mathrm{\&lambda;}}_i\left(k\right)=\left\{\begin{array}{cc}a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k},& a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k}\&GreaterEqual;1\\ 1,& a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k}\&le;1\end{array}\right.$

Wherein adjust specifically choosing according to actual conditions and determine of coefficient a.

This air navigation aid be take H ∞ filtering algorithm as basis, by real-time adjusting estimation error variance matrix and corresponding gain matrix to meet orthogonality principle, force wave filter to keep the high precision tracking to real system state, again solving of the matrix that fades done to optimization process, make wave filter no longer responsive to put forward its adaptive ability to initial parameter, obtain a kind of filtering algorithm of high tracking performance.This optimization filtering algorithm has the stronger robustness about model uncertainty and the tracking power to system mobile process, and has kept the simple advantage of H ∞ Filtering Model, realizes wave filter for the high precision tracking of carrier mobile process system state.

The optimization filtering algorithm of above-mentioned proposition is applied to the AUV integrated navigation system based on strap-down inertial guidance/Doppler, the three-dimensional velocity information of strap-down inertial guidance navigation information, Doppler log is carried out to data message fusion, obtain precision high, real-time, the navigator fix result that continuation is good, effectively raises integrated navigation system navigation and track and localization performance.

Compared with prior art, its beneficial effect is in the present invention: the method can be integrated a plurality of sub-navigation sensor information, has complementary advantages to obtain more high precision and more reliable navigation information.

Accompanying drawing explanation

Fig. 1 the present invention is based on the main block diagram that the navigational system based on strap-down inertial guidance and Doppler log in the optimization air navigation aid of strap-down inertial guidance and Doppler log forms;

Fig. 2 is the optimization air navigation aid built-up pattern block diagram that the present invention is based on strap-down inertial guidance and Doppler log;

Fig. 3 is the optimization air navigation aid structure principle chart that the present invention is based on strap-down inertial guidance and Doppler log;

Fig. 4 the present invention is based on filtering fusion calculation algorithm flow chart in the optimization air navigation aid of strap-down inertial guidance and Doppler log;

Fig. 5 is strap-down inertial and the doppler system overview flow chart the present invention is based in the optimization air navigation aid of strap-down inertial guidance and Doppler log;

Fig. 6 is the simulation analysis design sketch of embodiment 1 that the present invention is based on the optimization air navigation aid of strap-down inertial guidance and Doppler log for this.

Embodiment

Below technical solution of the present invention is elaborated, but protection scope of the present invention is not limited to described embodiment.

Embodiment:

A kind of optimization air navigation aid based on strap-down inertial guidance and Doppler log of the present embodiment, this air navigation aid is applied to a navigational system based on strap-down inertial guidance and Doppler log, and this navigational system comprises Doppler log, quick-connecting inertia measurement unit, quick-connecting inertia measurement cell processing module and CPU (central processing unit); Described Doppler log comprises transceiver, four transducers and interface unit; Described Doppler log is connected with CPU (central processing unit) by this interface unit; Described quick-connecting inertia measurement unit comprises three axle gyro and three axis accelerometers; Described quick-connecting inertia measurement unit is connected with quick-connecting inertia measurement cell processing module; Described quick-connecting inertia measurement cell processing module is connected with CPU (central processing unit); This air navigation aid comprises the following steps:

(1) by three axle gyros in quick-connecting inertia measurement unit block, record three axis angular rate information and accelerometer records 3-axis acceleration information, quick-connecting inertia measurement cell processing module receives the navigation information of quick-connecting inertia measurement unit output, by navigation integral and calculating, obtains the navigation informations such as carrier positions, speed and attitude;

(2) in the transceiver by Doppler log, launch electric oscillation signal, give transducer;

(3) four transducers transmit ultrasonic waves by Doppler log and receive the reflection echo with frequency shift property;

(4) utilize echoed signal that in transceiver, receiving system is sent transducer here to give interface unit through amplifying to try to achieve Doppler shift and be converted to speed of a ship or plane simulating signal after processing.If carrier headway is V, the beam transmission angle of depression is θ, velocity of sound C ≈ 1500m/s, and single beam frequency displacement computing formula is: Δ f=2Vf ₀cos θ/C, can obtain thus speed computing formula and be: V=Δ fC/ (2f ₀cos θ);

(5) utilize four speed of a ship or plane simulating signals that in transceiver, interface unit is sent transceiver here to be converted to the speed of a ship or plane, and export to CPU (central processing unit) with digital form;

(6) the real-time three-dimensional carrier that the arithmetical unit in CPU (central processing unit) periodically calculates carrier according to the velocity information of the interface unit output of Doppler log is speed amount;

(7) filtration module in CPU (central processing unit) carries out filtering fusion calculation to the three-dimensional velocity information of the strap-down inertial navigation information receiving, Doppler log and obtains in k inertia Strapdown inertial measurement system compensation constantly after correction, obtain final high-precision navigator fix information;

The method that described filtering fusion calculation obtains inertia Strapdown inertial measurement system compensation amount is:

Using strap-down inertial as with reference to system, state variable X gets velocity error

attitude error (φ _x, φ _y, φ _z), the random normal value biasing of accelerometer

and Gyro Random Constant Drift (ε _x, ε _y, ε _z), totally 10 tie up: $X={[{\mathrm{\&delta;V}}_e^n,{\mathrm{\&delta;V}}_n^n,{\mathrm{\&phi;}}_x,{\mathrm{\&phi;}}_y,{\mathrm{\&phi;}}_z,{\&dtri;}_x,{\&dtri;}_y,{\mathrm{\&epsiv;}}_x,{\mathrm{\&epsiv;}}_y,{\mathrm{\&epsiv;}}_z]}^T;$ W=[w _ax, w _ay, w _gx, w _gy, w _gz, 0,0,0,0,0] ^tfor system noise vector;

The carrier that the speed that the strap-down inertial of usining calculates and Doppler log record both differences that is speed after being transformed into navigation system are as the measuring value of system, and measurement equation is expressed as:

$Z=\left[\begin{array}{c}{V}_{\mathrm{SINSe}}^n-C_b^n{V}_{\mathrm{DVLe}}\\ V_{\mathrm{SINSn}}^n-C_b^n{V}_{\mathrm{DVLn}}\end{array}\right]=\mathrm{HX}+V$

Wherein, observation vector is

measurement noise vector V=[w _vx, w _vy] ^t, system measurements matrix is: $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\end{array}\right],$ System state equation and measurement equation discretize can be obtained to discrete system filtering equations;

Utilize initial value

and P ₀, according to k measurement Z constantly _kjust can recursion calculate k state estimation constantly ${\hat{X}}_k(k=\mathrm{1,2,3},...):$

${\hat{X}}_k={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1})$

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

Wherein,

$P_k={\mathrm{\&Gamma;}}_{k,k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T+\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}\right\}{\mathrm{\&Lambda;}}^T\left(k\right)$

By orthogonality principle, have:

${\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T=H_k\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\right\}{\mathrm{\&Lambda;}}^T\left(k\right)H_k^T$

For general situation, H _krow dimension is less, has following approximate data:

$\left\{\begin{array}{c}{\left(H_k^T{H}_k\right)}^{-1}{H}_k^T\&ap;H_k^T\\ H_k{\left(H_k^T{H}_k\right)}^{-1}\&ap;H_k\end{array}\right.$

So can convert, obtain:

$H_k^T\{{\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T\}{H}_k=\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}\right\}{\mathrm{\&Lambda;}}^T\left(k\right)$

Definition M (k) and N (k) are:

$\left\{\begin{array}{c}M\left(k\right)={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\\ N\left(k\right)=H_k^T\{{\mathrm{<figure-callout\; id="0"\; label="V"\; filenames="HDA0000431107920000021.png"\; state="\{\{state\}\}">V</figure-callout>}}_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T\}{H}_k\end{array}\right.$

V in formula _{0, k}calculation method is the same, and substitution can obtain: N (k)=Λ (k) M (k) Λ ^t(k)

By approximate the resolving of above formula, obtained:

${\mathrm{\&lambda;}}_{0i,k}=\sqrt{\frac{N_{\mathrm{ii}}\left(k\right)}{M_{\mathrm{ii}}\left(k\right)}}$

Each element fading in matrix Λ (k) is:

${\mathrm{\&lambda;}}_i\left(k\right)=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_{0i,k},& {\mathrm{\&lambda;}}_{0i,k}\&GreaterEqual;1\\ 1,& {\mathrm{\&lambda;}}_{0i,k}\&le;1\end{array}\right.$

N in formula _iiand M _iiitem is respectively i element of principal diagonal of N (k) and M (k).

In order to improve the applicability of this algorithm in real system, can adjust coefficient a, artificial adjusting fading factor λ by further introducing _i(k), have:

${\mathrm{\&lambda;}}_i\left(k\right)=\left\{\begin{array}{cc}a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k},& a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k}\&GreaterEqual;1\\ 1,& a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k}\&le;1\end{array}\right.$

Wherein adjust specifically choosing according to actual conditions and determine of coefficient a, desirable [0.1,10], generally gets a=5.

The three-axis gyroscope three axle quadratures of the quick-connecting inertia measurement unit of the present embodiment are installed, and described three axis accelerometer three axle quadratures are installed.

The three-axis gyroscope of the quick-connecting inertia measurement unit of the present embodiment is fibre optic gyroscope.

The 3-axis acceleration of the present embodiment is counted quartz flexible accelerometer.

The transceiver of the Doppler log of the present embodiment comprises emission coefficient, receiving system and calculating compensating circuit.

The navigational system of the present embodiment also comprises display, and described display is connected with arithmetical unit, shows the navigation information that central processing unit computing obtains.

Filtering performance by the air navigation aid based on inertia strap-down navigation and Doppler log (embodiment) of the conventional air navigation aid (comparative example) based on H ∞ filtering algorithm and the present embodiment in AUV integrated navigation system carries out simulation analysis, provided AUV flight path curve and followed the tracks of contrast effect as shown in Figure 6, this new air navigation aid more approaches theoretic predetermined navigation AUV flight path curve.

As mentioned above, although represented and explained the present invention with reference to specific preferred embodiment, it shall not be construed as the restriction to the present invention self.Not departing under the spirit and scope of the present invention prerequisite of claims definition, can make in the form and details various variations to it.

Claims (6)

1. the optimization air navigation aid based on strap-down inertial guidance and Doppler log, it is characterized in that, this air navigation aid is applied to a navigational system based on strap-down inertial guidance and Doppler log, and this navigational system comprises Doppler log, quick-connecting inertia measurement unit, quick-connecting inertia measurement cell processing module and CPU (central processing unit); Described Doppler log comprises transceiver, four transducers and interface unit; Described Doppler log is connected with CPU (central processing unit) by this interface unit; Described quick-connecting inertia measurement unit comprises three axle gyro and three axis accelerometers; Described quick-connecting inertia measurement unit is connected with quick-connecting inertia measurement cell processing module; Described quick-connecting inertia measurement cell processing module is connected with CPU (central processing unit); This air navigation aid comprises the following steps:

(1) by three axle gyros in quick-connecting inertia measurement unit block, record three axis angular rate information and accelerometer records 3-axis acceleration information, quick-connecting inertia measurement cell processing module receives the navigation information of quick-connecting inertia measurement unit output, by navigation integral and calculating, obtains the navigation informations such as carrier positions, speed and attitude;

(2) in the transceiver by Doppler log, launch electric oscillation signal, give transducer;

(3) four transducers transmit ultrasonic waves by Doppler log and receive the reflection echo with frequency shift property;

(4) utilize echoed signal that in transceiver, receiving system is sent transducer here to give interface unit through amplifying to try to achieve Doppler shift and be converted to speed of a ship or plane simulating signal after processing.If carrier headway is V, the beam transmission angle of depression is θ, velocity of sound C ≈ 1500m/s, and single beam frequency displacement computing formula is: Δ f=2Vf ₀cos θ/C, can obtain thus speed computing formula and be: V=Δ fC/ (2f ₀cos θ);

(5) utilize four speed of a ship or plane simulating signals that in transceiver, interface unit is sent transceiver here to be converted to the speed of a ship or plane, and export to CPU (central processing unit) with digital form;

(6) the real-time three-dimensional carrier that the arithmetical unit in CPU (central processing unit) periodically calculates carrier according to the velocity information of the interface unit output of Doppler log is speed amount;

(7) filtration module in CPU (central processing unit) carries out filtering fusion calculation to the three-dimensional velocity information of the strap-down inertial navigation information receiving, Doppler log and obtains k inertia Strapdown inertial measurement system compensation amount constantly after correction, obtain final high-precision navigator fix information;

The method that described filtering fusion calculation obtains inertia Strapdown inertial measurement system compensation amount is:

Using strap-down inertial as with reference to system, state variable X gets velocity error

attitude error (φ _x, φ _y, φ _z), the random normal value biasing of accelerometer and Gyro Random Constant Drift (ε _x, ε _y, ε _z), totally 10 tie up: $X={[{\mathrm{\&delta;V}}_e^n,{\mathrm{\&delta;V}}_n^n,{\mathrm{\&phi;}}_x,{\mathrm{\&phi;}}_y,{\mathrm{\&phi;}}_z,{\&dtri;}_x,{\&dtri;}_y,{\mathrm{\&epsiv;}}_x,{\mathrm{\&epsiv;}}_y,{\mathrm{\&epsiv;}}_z]}^T;$ W=[w _ax, w _ay, w _gx, w _gy, w _gz, 0,0,0,0,0] ^tfor system noise vector;

The carrier that the speed that the strap-down inertial of usining calculates and Doppler log record both differences that is speed after being transformed into navigation system are as the measuring value of system, and measurement equation is expressed as:

$Z=\left[\begin{array}{c}{V}_{\mathrm{SINSe}}^n-C_b^n{V}_{\mathrm{DVLe}}\\ V_{\mathrm{SINSn}}^n-C_b^n{V}_{\mathrm{DVLn}}\end{array}\right]=\mathrm{HX}+V$

Wherein, observation vector is

measurement noise vector V=[w _vx, w _vy] ^t, system measurements matrix is H, and system state equation and measurement equation discretize can be obtained to discrete system filtering equations; Utilize initial value

and P ₀, according to k measurement Z constantly _kjust can recursion calculate k state estimation constantly ${\hat{X}}_k(k=\mathrm{1,2,3}...):$

${\hat{X}}_k={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}+K_k(Z_k-H_k{\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1})$

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

Wherein,

$P_k={\mathrm{\&Gamma;}}_{k,k-1}{\mathrm{\&Gamma;}}_{k,k-1}^T+\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}\right\}{\mathrm{\&Lambda;}}^T\left(k\right)$

By orthogonality principle, have:

$V_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T=H_k\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\right\}{\mathrm{\&Lambda;}}^T\left(k\right)H_k^T$

For general situation, H _krow dimension is less, has following approximate data:

$\left\{\begin{array}{c}{\left(H_k^T{H}_k\right)}^{-1}{H}_k^T\&ap;H_k^T\\ H_k{\left(H_k^T{H}_k\right)}^{-1}\&ap;H_k\end{array}\right.$

So can convert, obtain:

$H_k^T\{V_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T\}{H}_k=\mathrm{\&Lambda;}\left(k\right)\left\{\begin{array}{c}{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\end{array}\right\}{\mathrm{\&Lambda;}}^T\left(k\right)$ Definition M (k) and N (k) are:

$\left\{\begin{array}{c}M\left(k\right)={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T-{\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}\left[\begin{array}{cc}{H}_k^T& L_k^T\end{array}\right]R_{e,k}^{-1}\left[\begin{array}{c}{H}_k\\ L_k\end{array}\right]P_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T\\ N\left(k\right)=H_k^T\{V_{0,k}-{\mathrm{\&beta;I}}_k-H_k{\mathrm{\&Gamma;}}_k{\mathrm{\&Gamma;}}_k^T{H}_k^T\}{H}_k\end{array}\right.$

V in formula _{0, k}calculation method is the same, and substitution can obtain: N (k)=Λ (k) M (k) Λ ^t(k)

By approximate the resolving of above formula, obtained:

${\mathrm{\&lambda;}}_{0i,k}=\sqrt{\frac{N_{\mathrm{ii}}\left(k\right)}{M_{\mathrm{ii}}\left(k\right)}}$

Each element fading in matrix Λ (k) is:

${\mathrm{\&lambda;}}_i\left(k\right)=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_{0i,k},& {\mathrm{\&lambda;}}_{0i,k}\&GreaterEqual;1\\ 1,& {\mathrm{\&lambda;}}_{0i,k}\&le;1\end{array}\right.$

N in formula _iiand M _iiitem is respectively i element of principal diagonal of N (k) and M (k);

In order to improve the applicability of this algorithm in real system, can adjust coefficient a, artificial adjusting fading factor λ by further introducing _i(k), have:

${\mathrm{\&lambda;}}_i\left(k\right)=\left\{\begin{array}{cc}a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k},& a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k}\&GreaterEqual;1\\ 1,& a\&CenterDot;{\mathrm{\&lambda;}}_{0i,k}\&le;1\end{array}\right.$

Wherein, adjust specifically choosing according to actual conditions of coefficient a and determine, desirable [0.1,10], generally gets a=5.

2. a kind of air navigation aid based on strap-down inertial guidance and Doppler log according to claim 1, is characterized in that, the three-axis gyroscope three axle quadratures of described quick-connecting inertia measurement unit are installed, and described three axis accelerometer three axle quadratures are installed.

3. a kind of air navigation aid based on strap-down inertial guidance and Doppler log according to claim 1, is characterized in that, the three-axis gyroscope of described quick-connecting inertia measurement unit is fibre optic gyroscope.

4. a kind of air navigation aid based on strap-down inertial guidance and Doppler log according to claim 1, is characterized in that, described 3-axis acceleration is counted quartz flexible accelerometer.

5. a kind of air navigation aid based on strap-down inertial guidance and Doppler log according to claim 1, is characterized in that, the transceiver of described Doppler log comprises emission coefficient, receiving system and calculating compensating circuit.

6. a kind of air navigation aid based on strap-down inertial guidance and Doppler log according to claim 1, it is characterized in that, described navigational system also comprises display, and described display is connected with arithmetical unit, shows the navigation information that central processing unit computing obtains.

Images