CN103344251B - A kind of Transfer Alignment time delay estimation method adding specific force coupling based on speed - Google Patents

Abstract

The present invention discloses a kind of Transfer Alignment time delay estimadon algorithm adding specific force coupling based on speed.Its implementation is: utilize the main inertial navigation antithetical phrase inertial navigation of having aimed to carry out coarse alignment, recycle the velocity contrast of the relatively main inertial navigation of sub-inertial navigation, specific force difference as filtering observed quantity, and the time delay of main inertial navigation information is extended for filtering system state variable, in conjunction with the error model of inertial navigation system, Kalman filtering algorithm is utilized to estimate time delay and the sub-inertial navigation platform error angle relative to main inertial navigation.The present invention is applicable to main and sub inertial navigation when being Platform INS Inertial, and time delay, shorter speed added specific force coupling Transfer Alignment.

Description

A kind of Transfer Alignment time delay estimation method adding specific force coupling based on speed

Technical field

The present invention relates to the method for estimation to the time delay that main inertial navigation match information exists in the method for estimation, particularly a kind of inertial navigation system Transfer Alignment of a kind of information time delay.

Background technology

Transfer Alignment is in a dynamic condition by mating with the output information of the sub-inertial navigation (misalignment) be arranged on carrier the output information of the main inertial navigation of carrier (aiming at), estimate the misalignment of sub-inertial navigation, thus complete a kind of method of sub-inertial alignment.For Platform Inertial Navigation System, because the realization of Transfer Alignment filtering algorithm is carried out in sub-inertial navigation system, this just needs some related information transmission of main inertial navigation system to sub-inertial navigation system, due to the existence of time delay, the navigation information when navigation information causing sub-inertial navigation to receive transmits with main inertial navigation has certain error, sub-inertial navigation carries out Transfer Alignment using the information received as benchmark, will affect the estimated accuracy for platform error angle.

" Chinese inertial technology journal " the 13rd volume the 1st published in February, 2005 is interim, the document compensation method of transfer delay " in the Transfer Alignment " adopts method time delay being expanded to state variable to estimate in real time, but only add attitude Transfer Alignment for speed to analyze, because the impact of time delay on attitude equal angular movement information is larger, therefore adopt speed to add and carry out Kalman filtering than force information, better time delay estimadon effect can be obtained.

Summary of the invention

The object of the present invention is to provide a kind of for less information transmit delay time error, the Transfer Alignment time delay estimation method adding specific force coupling based on speed that estimating speed sooner, precision is higher.

The object of the invention is by one following steps realize:

(1) sub-inertial navigation preheating, and utilize the platform-type main inertial navigation system antithetical phrase inertial navigation system aimed to carry out a step Transfer Alignment, complete the coarse alignment of sub-inertial navigation system;

(2) navigation calculation is carried out in main and sub inertial navigation, and gathers main inertial navigation speed, specific force information transmission to sub-inertial reference calculation computing machine;

(3) in sub-inertial navigation computer, main and sub inertial navigation velocity contrast, the poor Iterative carrying out normal scatter Kalman filtering of specific force is constructed;

Wherein, main and sub inertial navigation is Platform INS Inertial, and the sub-ins error model adopted is:

$\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\\ \mathrm{\Δ}\stackrel{\·}{t}=0\end{array}\right.$

Wherein, n is local horizontal coordinates, δ v ⁿfor sub-inertial navigation velocity error is in the projection of n system; φ ⁿit is the platform error angle between main inertial navigation and sub-inertial navigation system; f ⁿfor sub-inertial navigation specific force exports the projection 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 relative ideal value error, and ε ^sfor sub-inertial navigation gyroscopic drift; Δ t represents time delay.

Selected system state variables is:

$X={\left[\begin{array}{ccccccccccc}\mathrm{\δ}{v}_x& \mathrm{\δ}{v}_y& {\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& \mathrm{\Δt}\end{array}\right]}^T$

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

Wherein

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

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

In formula

$A_{12}=\left[\begin{array}{ccc}0& {-f}_z& f_y\\ f_z& 0& {-f}_x\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].$

System noise acoustic matrix is:

W＝[w _axw _ayw _εxw _εyw _εz0 0 0 0 0 0] ^T

Wherein, for latitude; R _efor earth radius; f _x, f _y, f _zbe respectively f ⁿeast orientation, north orientation, sky to component; w _ax, w _ayfor accelerometer bias random white noise, w _{ε x}, w _{ε y}, w _{ε z}for gyroscopic drift random white noise.

Systematic perspective is measured as:

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

Observation equation is:

Z=HX+V

$H\left[\begin{array}{ccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(1\right)\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(2\right)\\ 0& 0& 0& {-f}_z& f_y& 1& 0& 0& 0& 0& -\mathrm{Da}\left(1\right)\\ 0& 0& f_z& 0& {-f}_x& 0& 1& 0& 0& 0& -\mathrm{Da}\left(2\right)\end{array}\right]$

Wherein, $\mathrm{Dv}\left(t\right)={\stackrel{\·}{V}}_m^n\left(t\right),$ $\mathrm{Da}\left(t\right)={\stackrel{\·}{f}}_m^n\left(t\right),$ And have ${\stackrel{\·}{V}}_m^n(t-\mathrm{\Δt})={\stackrel{\·}{V}}_m^n\left(t\right),$ ${\stackrel{\·}{f}}_m^n(t-\mathrm{\Δt})={\stackrel{\·}{f}}_m^n\left(t\right).$

(4), after filtering terminates, the platform error angle after time-delay estimation value and time delay equalization is obtained.

This Transfer Alignment time delay estimation method has good time delay estimadon effect, can real-time follow-up time delay preferably.Step is simple, enforcement is convenient, is applicable to compensate the transmission delay error of inertial navigation information main in Transfer Alignment.After estimation obtains the propagation delay time of information between main and sub inertial navigation, effectively can promote main and sub Inertial navigation platform error angle φ ⁿestimated accuracy, realize sub-inertial navigation Transfer Alignment fast and effectively.

Accompanying drawing explanation

Fig. 1 is the process flow diagram of this kind of Transfer Alignment time delay estimation method.Fig. 2 be hull do serpentine motor-driven under, add specific force matching scheme by speed and carry out Transfer Alignment, consider before time delay equalization and after compensating, emulate the platform error angular estimation curve obtained.Fig. 3 is the real-time estimation curve of this algorithm for time delay.

Embodiment:

Add a Transfer Alignment time delay estimation method for specific force coupling based on speed, the method is realized by following steps:

(1) sub-inertial navigation preheating, and utilize the platform-type main inertial navigation system antithetical phrase inertial navigation system aimed to carry out a step Transfer Alignment, complete the coarse alignment of sub-inertial navigation system; Further comprises the initialization of all the other navigation informations such as antithetical phrase inertial navigation speed, position in this step, make sub-inertial navigation system can start independently resolve and export navigation information.

(2) navigation calculation is carried out in main and sub inertial navigation, and gathers main inertial navigation speed, specific force information transmission to sub-inertial reference calculation computing machine;

(3) in sub-inertial navigation computer, main and sub inertial navigation velocity contrast, the poor Iterative carrying out normal scatter Kalman filtering of specific force is constructed;

This step mainly comprises 4 parts: the 1) foundation of ins error equation; 2) foundation of system state equation; 3) foundation of observation equation; 4) Kalman Filtering for Discrete Iterative.

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 '.Due to the error of calculation, the impact executing square error and information source error, n ' is that relative n system has deviation angle φ ⁿ.

1) according to the mechanization of Platform Inertial Navigation System, the error model of inertial navigation is set up.Its error equation is:

$\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\\ \mathrm{\Δ}\stackrel{\·}{t}=0\end{array}\right.$

Wherein, δ v ⁿfor sub-inertial navigation velocity error is in the projection of n system; φ ⁿit is the platform error angle between main inertial navigation and sub-inertial navigation system; f ⁿfor sub-inertial navigation specific force exports the projection 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 relative ideal value error, and ε ^sfor sub-inertial navigation gyroscopic drift; Δ t represents time delay, usually main inertial navigation information is set to an arbitrary constant changed between 0 ~ 0.1s time delay.

2) state equation of system is set up.In conjunction with above-mentioned ins error equation, ignore sky to channel speed, information, and will be extended for state variable time delay, obtaining system state amount is:

$X={\left[\begin{array}{ccccccccccc}\mathrm{\δ}{v}_x& \mathrm{\δ}{v}_y& {\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& \mathrm{\Δt}\end{array}\right]}^T$

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

Wherein

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

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

In formula

$A_{12}=\left[\begin{array}{ccc}0& {-f}_z& f_y\\ f_z& 0& {-f}_x\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].$

System noise acoustic matrix is:

W＝[w _axw _ayw _εxw _εyw _εz0 0 0 0 0 0] ^T

Wherein, for latitude; R _efor earth radius; f _x, f _y, f _zbe respectively f ⁿeast orientation, north orientation, sky to component; w _ax, w _ayfor accelerometer bias random white noise, w _{ε x}, w _{ε y}, w _{ε z}for gyroscopic drift random white noise.

3) speed of setting up adds the systematic observation equation of specific force matching process.According to set up system state equation, selection speed adds specific force and measures as systematic perspective, then have:

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

Speed and the specific force of supposing the main inertial navigation that sub-inertial navigation system (SINS) obtains in t are the data of main inertial navigation in t-Δ t.Velocity contrast between boss's inertial navigation and specific force difference are expressed as follows:

$\mathrm{\ΔV}=V_s^n\left(t\right)-V_m^n\left(t\right)$

$=V_s^n\left(t\right)-(V_m^n(t-\mathrm{\Δt})+{\stackrel{\·}{V}}_m^n(t-\mathrm{\Δt})\·\mathrm{\Δt}$

In like manner,

$\mathrm{\Δf}=f_s^n\left(t\right)-f_m^n\left(t\right)$

$=f_s^n\left(t\right)-(f_m^n(t-\mathrm{\Δt})+{\stackrel{\·}{f}}_m^n(t-\mathrm{\Δt})\·\mathrm{\Δt})$

The time chosen due to Δ t is very short, is defaulted as in time delay Δ t, the rate of change of speed with the rate of change of specific force approximate constant within the cycle calculated, namely ${\stackrel{\·}{V}}_m^n(t-\mathrm{\Δt})={\stackrel{\·}{V}}_m^n\left(t\right),$ ${\stackrel{\·}{f}}_m^n(t-\mathrm{\Δt})={\stackrel{\·}{f}}_m^n\left(t\right).$

Order $\mathrm{Dv}\left(t\right)={\stackrel{\·}{V}}_m^n\left(t\right),$ $\mathrm{Da}\left(t\right)={\stackrel{\·}{f}}_m^n\left(t\right),$ Then have:

$\mathrm{\ΔV}+\mathrm{Dv}\left(t\right)=V_s^n\left(t\right)-V_m^n(t-\mathrm{\Δt})$

$\mathrm{\Δf}+\mathrm{Da}\left(t\right)=f_s^n\left(t\right)-f_m^n(t-\mathrm{\Δt})$

The observation equation that the speed that then can obtain adds specific force coupling is:

Z=HX+V

$H=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(1\right)\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(2\right)\\ 0& 0& 0& {-f}_z& f_y& 1& 0& 0& 0& 0& -\mathrm{Da}\left(1\right)\\ 0& 0& f_z& 0& {-f}_x& 0& 1& 0& 0& 0& -\mathrm{Da}\left(2\right)\end{array}\right]$

4) according to state equation and the observation equation of system, the speed in conjunction with main and sub inertial navigation carries out Kalman Filtering for Discrete Iterative with than force information.

(4), after Kalman filtering iterative computation terminates, the platform error angle after the estimated value of time delay and time delay equalization is obtained.

Provided by the invention based on speed add specific force coupling time delay estimation method have the following advantages:

One is the contact that this method only need set up time delay and observed quantity, will be extended for quantity of state time delay, and only affect observation equation, and implement comparatively simple, calculated amount is few; Two is compensating time delay that this method can be real-time in filtering adds for speed the impact that specific force matching algorithm causes; Three is add attitude matching time delay equalization method compared to existing speed, the speed that this method adopts adds specific force match information and is line movable information, effect by time delay is more weak, therefore well dynamic environment can be adapted to, good effect is estimated to the measurement of time delay and attitude misalignment, meet naval vessel under real navigation condition in Transfer Alignment to the real-time estimation of main inertial navigation time delay and the requirement compensated.

In order to further illustrate the beneficial effect of described method, emulate under following starting condition to the estimation of Transfer Alignment and evaluated error, simulation result as shown in Figure 2 and Figure 3, and has carried out com-parison and analysis to it.

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 ₀parameter is arranged according to above-mentioned inertial device error, 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) ²0.05}

5) to do serpentine motor-driven for carrier, and at first with the speed linear uniform motion of 10m/s, the motor-driven turning rate of serpentine is 1 (°)/s, motor-drivenly lasts 100s.In the process of emulation, in order to consider the propagation delay phenomenon that exists in real process, utilize the random function that produces that the data of the main inertial navigation speed of the mainly main inertial navigation here (and than force information) are added that a delay is equally distributed time delay between 0 ~ 0.1s, the average of time delay is 0.05s, then sub-inertial navigation is passed to, mate with the data of sub-inertial navigation, thus complete the process of Transfer Alignment.

Com-parison and analysis:

Fig. 2 provides boats and ships and adds specific force coupling Transfer alignment algorithm to the estimation of error curve at platform error angle in the motor-driven lower speed that utilizes of serpentine, blue solid lines is not to the estimation of error curve that time delay is estimated, green dotted line is the estimation of error curve after estimating time delay by this method.Contrast known, when boats and ships do " S " motor-driven time, the impact that time delay adds specific force coupling Transfer alignment algorithm to speed is very large, can not estimate platform error angle when doing motor-driven.And this method compensating time delay can add the impact of specific force Transfer alignment algorithm effectively for speed, meet high precision and the rapidity of Transfer alignment algorithm.As can be seen from Figure 3, this method can be real-time estimate time delay, for compensating time delay establishes solid foundation to the impact that speed adds specific force coupling Transfer alignment algorithm, owing to being time delay 0 ~ 0.1s, in order to clearly see estimation curve, get the estimation curve of front 20s to time delay.

Claims (1)

1. add a Transfer Alignment time delay estimadon algorithm for specific force coupling based on speed, it is characterized in that:

(1) sub-inertial navigation preheating, and utilize the platform-type main inertial navigation system antithetical phrase inertial navigation system aimed to carry out a step Transfer Alignment, complete the coarse alignment of sub-inertial navigation system;

(2) navigation calculation is carried out in main and sub inertial navigation, and gathers main inertial navigation speed, specific force information transmission to sub-inertial reference calculation computing machine;

(3) in sub-inertial navigation computer, structure Kalman filtering fundamental equation, and utilize main and sub inertial navigation velocity contrast, specific force difference carries out normal scatter Kalman filtering Iterative;

Wherein, main and sub inertial navigation is Platform INS Inertial, and selected filtering system state variable is:

X＝[δv _xδv _yφ _xφ _yφ _z▽ _x▽ _yε _xε _yε _zΔt] ^T

In formula, δ v _x, δ v _ybe respectively sub-inertial navigation east orientation, north orientation velocity error; φ _x, φ _y, φ _zbe respectively the platform error angle of sub-inertial navigation relative to main inertial navigation; ▽ _x, ▽ _ybe respectively sub-inertial navigation accelerometer zero inclined; ε _x, ε _y, ε _zbe respectively sub-inertial navigation gyroscopic drift; Δ t is the time delay of main inertial navigation information;

The zygote inertial navigation system error differential equation, sets up filter state equation;

The systematic perspective that speed adds specific force matching method is measured as:

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

In formula, δ f _x, δ f _ybe respectively the difference of main and sub inertial navigation specific force;

Filtering observation equation is:

Z＝HX+V

$H=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(1\right)\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(2\right)\\ 0& 0& 0& -f_z& f_y& 1& 0& 0& 0& 0& -\mathrm{Da}\left(1\right)\\ 0& 0& f_z& 0& -f_x& 0& 1& 0& 0& 0& -\mathrm{Da}\left(2\right)\end{array}\right]$

In formula, $\mathrm{Dv}\left(t\right)={\stackrel{\·}{V}}_m^n\left(t\right),\mathrm{Da}\left(t\right)={\stackrel{\·}{f}}_m^n\left(t\right);$

Wherein, be respectively the differential of main inertial navigation speed and specific force;

(4), after Kalman filtering terminates, the platform error angle after time-delay estimation value and time delay equalization is obtained.