CN102288177A - Strapdown system speed calculating method based on angular speed output - Google Patents

Abstract

The invention provides a strapdown system speed calculating method based on angular speed output, which comprises the following steps of: according to the principle of sculling compensation algorithm of speed, deducing the sculling compensation algorithm based on angular rate, specific force and measurement bandwidth through fitting a carrier angular speed and a specific force function; under the condition of typical sculling movement, determining the optimal factor of the sculling compensation algorithm through the minimum difference value of direct current quantity in the sculling compensation quantity and a true value; and thus, obtaining the optimal algorithm of the sculling compensation, and realizing high-precision calculating of the speed.

Description

A kind of strapdown system velocity calculated method based on angular speed output

Technical field

The present invention relates to a kind of strapdown system velocity calculated method, especially at strapdown system velocity calculated method based on angular speed output.

Background technology

Strapdown system is a kind of inertial navigation system that gyroscope and accelerometer is installed on carrier, it comes actual platform in the analog platform formula inertial navigation system by calculating " mathematical platform ", thereby cost reduces greatly, but strapdown system but improves greatly to the requirement of navigation calculation.In the past, researchers are output as angle signal as prerequisite with gyroscope, have proposed traditional velocity calculated method based on the increment output signal.In recent years, development along with optical fiber communication technology and optical fiber sensing technology, fibre optic gyroscope becomes the important technology of inertial navigation, for the strapdown system that constitutes by optical fibre gyro (as interfere type), gyroscope is output as angle rate signal, when traditional strapdown system velocity calculated method based on the increment output signal is applied to optical fiber gyroscope strapping system, need extract angle increment by angular speed, and then utilize classic method to realize velocity calculated, so both increased the Design of System Software difficulty, greater loss had also been arranged on calculation accuracy simultaneously.

Invention Inner holds

In order to overcome the deficiency of conventional speed calculation method in optical fiber gyroscope strapping system is used, the present invention proposes a kind of strapdown system velocity calculated method based on angular speed output, directly utilize the angle rate signal of gyroscope output to calculate, improve calculation accuracy, reduce computation complexity.

A kind of strapdown system velocity calculated method based on angular speed output, be specially: carrier is at t _mSpeed constantly $V_m=V_{m-1}+{\∫}_{t_{m-1}}^{t_m}[g\left(t\right)-({2\mathrm{\ω}}_{\mathrm{ie}}\left(t\right)+{\mathrm{\ω}}_{\mathrm{en}}\left(t\right))\×V_{m-1}]\mathrm{dt}+{\mathrm{\ΔV}}_{s{f}_m},$

Wherein,

V _M-1For carrier at t _M-1Speed constantly,

C _M-1For carrier at t _M-1Attitude matrix constantly,

G (t), ω _Ie(t), ω _En(t) being respectively t acceleration of gravity, rotational-angular velocity of the earth, navigation constantly is the rotational angular velocity that the relative earth is;

$\mathrm{\Δ}{V}_{s{f}_m}={\mathrm{\ΔV}}_m+\frac{1}{2}\mathrm{\Δ}{\mathrm{\θ}}_m\×{\mathrm{\ΔV}}_m+{\mathrm{\Δ}\hat{V}}_{{\mathrm{scul}}_m};$

F (t) is that accelerometer is at t output specific force constantly;

ω (t) is that gyro is in t output angle speed constantly;

Paddle effect compensating item

N predictor process as follows:

(a1) cross n sampled point with velocity calculated period T=t _m-t _M-1Be divided into and be spaced apart

N sub-period, with $\left\{\begin{array}{c}\mathrm{\ω}\left(t\right)=a_1+2a_2(t-t_{m-1})+\·\·\·+{\mathrm{na}}_n{(t-t_{m-1})}^{n-1}\\ f\left(t\right)=A_1+{2A}_2(t-t_{m-1})+\·\·\·+{\mathrm{nA}}_n{(t-t_{m-1})}^{n-1}\end{array}\right.$ For objective function is made the polynomial expression linear fit to the specific force output of gyrostatic angular speed output of this n sample point and accelerometer;

(a2) utilize step (a1) to fit definite normal vector a ₁A _n, A ₁A _nCalculate

$\left\{\begin{array}{c}\mathrm{\Δ\θ}\left(t\right)={\∫}_{t_{m-1}}^t\mathrm{\ω}\left(\mathrm{\τ}\right)\mathrm{d\τ}=a_1(t-t_{m-1})+a_2{(t-t_{m-1})}^2+\·\·\·+a_n{(t-t_{m-1})}^n\\ \mathrm{\ΔV}\left(t\right)={\∫}_{t_{m-1}}^{t}f\left(\mathrm{\τ}\right)\mathrm{d\τ}=A_1(t-t_{m-1})+A_2{(t-t_{m-1})}^2+\·\·\·+A_n{(t-t_{m-1})}^n\end{array}\right.,t_{m-1}\≤t\≤t_m;$

(a3) make up paddle effect compensating item computing formula

${\mathrm{\ΔV}}_{{\mathrm{scul}}_m}=\frac{1}{2}{\∫}_{t_{m-1}}^{t_m}[\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right)+\mathrm{\ΔV}\left(t\right)\×\mathrm{\ω}\left(t\right)]\mathrm{dt}.$

Further,

N predictor process also comprise step:

(a4) utilize the paddle effect compensating item computing formula of step (a3) to calculate the exact value of paddle effect compensating item under the paddle moving condition

(a5) DC quantity in the paddle effect compensating item computing formula of determining step (a3)

(a6) with difference

Minimum is the paddle effect compensating item computing formula in the objective optimization step (a3).

The present invention compares traditional algorithm and has following advantage:

1. when being applied to be output as the strapdown system of angle rate signal, can directly utilize the output signal of inertia device, need not conversion, simple.

2. when being applied to be output as the strapdown system of angle rate signal, calculation accuracy is far superior to traditional algorithm.

Description of drawings

Fig. 1 is that three increment algorithms and the traditional three increment Algorithm Error that the present invention designs contrast synoptic diagram with paddle motion frequency change curve.

Fig. 2 is that three increment algorithms and the traditional three increment Algorithm Error that the present invention designs contrast synoptic diagram with the sampling period change curve.

Embodiment

The present invention is from strapdown system velocity calculated principle, paddle campaign with classics is an environmental baseline, derive and designed the algorithm that resolves under the angular speed output, and by making paddle compensation term DC quantity and the minimum next coefficient of determining to optimize of actual value deviation in the algorithm, and finally provide the remainder error expression formula of optimized Algorithm, proved that by emulation experiment the calculation accuracy of new method will be better than classic method greatly at last.

Purpose of the present invention can reach by following measure:

The velocity calculated of 1 strapdown system

Getting geographic coordinate is navigation system, and strapdown inertial navitation system (SINS) speed fundamental equation can be expressed as:

${\stackrel{\·}{V}}^n=C_b^n{f}^b-({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n+g^n---\left(1\right)$

Wherein: n, b represent navigation system and carrier system, V respectively ⁿThe projection that expression speed is fastened in navigation, Be attitude matrix, The projection that the expression rotational-angular velocity of the earth is fastened in navigation,

Expression navigation is the projection that the rotational angular velocity of relative earth system is fastened in navigation, f ^bThe specific force of expression accelerometer output, g ⁿThe projection that expression acceleration of gravity is fastened in navigation.

To (1) formula integration, can get the digital recursive algorithm (, omitting the subscript of expression projected coordinate system) of velocity calculated for writing conveniently:

$V_m=V_{m-1}+C_{m-1}{\∫}_{t_{m-1}}^{t_m}(f\left(t\right)+\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right))\mathrm{dt}+{\∫}_{t_{m-1}}^{t_m}[g\left(t\right)-({2\mathrm{\ω}}_{\mathrm{ie}}\left(t\right)+{\mathrm{\ω}}_{\mathrm{en}}\left(t\right))\×V_{m-1}]\mathrm{dt}---\left(2\right)$

In the formula: V _mAnd V _M-1Represent t respectively _mThe moment and t _M-1Velocity constantly, C _M-1Expression t _M-1Moment attitude matrix, Velocity Updating period T=t _m-t _M-1, t _M-1To interior angle step of t time

To t _mAngle step in time

ω (t) is a t gyro output angle speed constantly, t _M-1To interior speed increment of t time

t _M-1To t _mSpeed increment in time

By formula (2) as can be known: the critical item of velocity calculated is

Note ${\mathrm{\ΔV}}_{{\mathrm{sf}}_m}={\∫}_{t_{m-1}}^{t_m}(f\left(t\right)+\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right))\mathrm{dt},$ Then ${\mathrm{\ΔV}}_{{\mathrm{sf}}_m}={\mathrm{\ΔV}}_m+{\∫}_{t_{m-1}}^{t_m}\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right)\mathrm{dt},$ Can further be expressed as by conversion:

${\mathrm{\ΔV}}_{{\mathrm{sf}}_m}={\mathrm{\ΔV}}_m+\frac{1}{2}{\mathrm{\Δ\θ}}_m\×{\mathrm{\ΔV}}_m+\frac{1}{2}{\∫}_{t_{m-1}}^{t_m}[\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right)+\mathrm{\ΔV}\left(t\right)\×\mathrm{\ω}\left(t\right)]\mathrm{dt}---\left(3\right)$

Second of formula (3) right-hand member:

The rotation effect compensation term that is called speed;

The 3rd of right-hand member:

${\mathrm{\ΔV}}_{{\mathrm{scul}}_m}=\frac{1}{2}{\∫}_{t_{m-1}}^{t_m}[\mathrm{\Δ\θ}\left(t\right)\×f\left(t\right)+\mathrm{\ΔV}\left(t\right)\×\mathrm{\ω}\left(t\right)]\mathrm{dt}---\left(4\right)$

The paddle effect compensating item that is called speed.

When strapdown system is done the speed renewal by formula (2), because the speed increment that acceleration causes must be considered rotation effect compensation term and paddle effect compensating item simultaneously, otherwise will there be paddle sum of errors rotation error in speed calculation, the present invention is directed to wherein main error component paddle error design paddle backoff algorithm and optimized Algorithm thereof.

The design of 2 paddle backoff algorithms and optimized Algorithm thereof

At first be described at generalized case of the present invention, specifically derive then design process of the present invention is described at three increment situations, obtained three increment paddle backoff algorithm formula at last and optimized coefficient also directly having provided two increment situation paddle backoff algorithm formula and having optimized coefficient, and carried out the simulation example contrast with traditional algorithm.Paddle backoff algorithm formula under other increment situation and optimization coefficient can design according to the general process that the present invention describes, and do not give unnecessary details one by one at this.

The design process that the present invention is directed to n (n＞1) increment paddle backoff algorithm is as follows:

At velocity calculated in the cycle, to the angular velocity of carrier with than the following linear polynomial match of masterpiece:

$\left\{\begin{array}{c}\mathrm{\ω}\left(t\right)=a_1+{2a}_2(t-t_{m-1})+\·\·\·+{\mathrm{na}}_n{(t-t_{m-1})}^{n-1}\\ f\left(t\right)=A_1+2A_2(t-t_{m-1})+\·\·\·+n{A}_n{(t-t_{m-1})}^{n-1}\end{array}\right.---\left(5\right)$

In the formula: a ₁A _n, A ₁A _nBe normal vector.

Under formula (5) linear fit condition, t _M-1Be respectively to interior angle step of t time and speed increment:

$\left\{\begin{array}{c}\mathrm{\Δ\θ}\left(t\right)={\∫}_{t_{m-1}}^t\mathrm{\ω}\left(\mathrm{\τ}\right)\mathrm{d\τ}=a_1(t-t_{m-1})+a_2{(t-t_{m-1})}^2+\·\·\·+a_n{(t-t_{m-1})}^n\\ \mathrm{\ΔV}\left(t\right)={\∫}_{t_{m-1}}^{t}f\left(\mathrm{\τ}\right)\mathrm{d\τ}=A_1(t-t_{m-1})+A_2{(t-t_{m-1})}^2+\·\·\·+A_n{(t-t_{m-1})}^n\end{array}\right.---\left(6\right)$

By n sampled point with velocity calculated period T=t _m-t _M-1Be divided into and be spaced apart

N sub-period, then can get sample point gyroscope angular speed output and accelerometer specific force and export and be respectively by formula (5):

$\left\{\begin{array}{c}\mathrm{\ω}(\mathrm{\ΔT}-t_{m-1})=a_1+{2a}_2\mathrm{\ΔT}+\·\·\·+{\mathrm{na}}_n{\mathrm{\ΔT}}^{n-1}\\ \mathrm{\ω}(2\mathrm{\ΔT}+t_{m-1})=a_1+{4a}_2\mathrm{\ΔT}+\·\·\·+2^{n-1}{\mathrm{na}}_n\mathrm{\Δ}{T}^{n-1}\\ \·\\ \·\\ \·\\ \mathrm{\ω}(\mathrm{n\ΔT}+t_{m-1})=a_1+{2\mathrm{na}}_2\mathrm{\ΔT}+\·\·\·+n^n{a}_n{\mathrm{\ΔT}}^{n-1}\end{array}\right.---\left(7\right)$

$\left\{\begin{array}{c}f(\mathrm{\&Delta;T}+t_{m-1})={\mathrm{<figure-callout\; id="1"\; label="A"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">A</figure-callout>}}_1+{2A}_2\mathrm{\&Delta;T}+\&CenterDot;\&CenterDot;\&CenterDot;+{\mathrm{nA}}_n{\mathrm{\&Delta;T}}^{n-1}\\ f(2\mathrm{\&Delta;T}+t_{m-1})=A_1+{4A}_2\mathrm{\&Delta;T}+\&CenterDot;\&CenterDot;\&CenterDot;+2^{n-1}{\mathrm{nA}}_n\mathrm{\&Delta;}{T}^{n-1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ f(\mathrm{n\&Delta;T}+t_{m-1})=A_1+{2\mathrm{nA}}_2\mathrm{\&Delta;T}+\&CenterDot;\&CenterDot;\&CenterDot;+n^n{A}_n{\mathrm{\&Delta;T}}^{n-1}\end{array}\right.---\left(8\right)$

Solving equation group (7) and system of equations (8) can get coefficient a respectively ₁A _nAnd A ₁A _n, with two groups of coefficient substitution formulas (6), association type (5) is found the solution formula (4) again, gets final product based on the n increment paddle backoff algorithm formula of angular speed output.

N increment paddle backoff algorithm optimizing process is described below:

1. at first utilize formula (4) to calculate the exact value of paddle effect compensating item under the paddle moving condition;

2. determine the DC quantity in the n increment paddle backoff algorithm formula then;

3. it is poor at last the exact value of DC quantity in the n increment paddle backoff algorithm formula and paddle effect compensating item to be done, difference is made Taylor series expansion, guaranteeing that difference is to optimize n increment paddle backoff algorithm formula coefficient under the minimum situation, this coefficient is the optimization coefficient.

Provide the concrete derivation of three increment paddle backoff algorithms and optimized Algorithm thereof below.

At velocity calculated in the cycle, to the angular velocity of carrier with than the following linear polynomial match of masterpiece:

$\left\{\begin{array}{c}\mathrm{\&omega;}\left(t\right)=a_1+{2a}_2(t-t_{m-1})+\&CenterDot;\&CenterDot;\&CenterDot;+{3a}_3{(t-t_{m-1})}^2\\ f\left(t\right)=A_1+2A_2(t-t_{m-1})+\&CenterDot;\&CenterDot;\&CenterDot;+3A_3{(t-t_{m-1})}^2\end{array}\right.---\left(9\right)$

In the formula: a ₁, a ₂, a ₃, A ₁, A ₂, A ₃Be normal vector.

Under formula (9) linear fit condition, t _M-1Be respectively to interior angle step of t time and speed increment:

$\left\{\begin{array}{c}\mathrm{\&Delta;\&theta;}\left(t\right)={\&Integral;}_{t_{m-1}}^t\mathrm{\&omega;}\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;}=a_1(t-t_{m-1})+a_2{(t-t_{m-1})}^2+a_3{(t-t_{m-1})}^3\\ \mathrm{\&Delta;V}\left(t\right)={\&Integral;}_{t_{m-1}}^{t}f\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;}=A_1(t-t_{m-1})+A_2{(t-t_{m-1})}^2+A_3{(t-t_{m-1})}^3\end{array}\right.---\left(10\right)$

By three sampled points with velocity calculated period T=t _m-t _M-1Be divided into and be spaced apart Three sub-periods, then can get sample point gyroscope angular speed output and the accelerometer specific force is output as by formula (9):

$\left\{\begin{array}{c}\mathrm{\&omega;}(\mathrm{\&Delta;T}+t_{m-1})=a_1+{2a}_2\mathrm{\&Delta;T}+{3a}_3+\mathrm{\&Delta;}{T}^2\\ \mathrm{\&omega;}(2\mathrm{\&Delta;T}+t_{m-1})=a_1+{4a}_2\mathrm{\&Delta;T}+12a_3{\mathrm{\&Delta;T}}^2\\ \mathrm{\&omega;}(3\mathrm{\&Delta;T}+t_{m-1})={\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">q</figure-callout>}}_1+{6a}_2\mathrm{\&Delta;T}+27a_3\mathrm{\&Delta;}{T}^2\end{array}\right.---\left(11\right)$

$\left\{\begin{array}{c}f(\mathrm{\&Delta;T}+t_{m-1})=A_1+{2A}_2\mathrm{\&Delta;T}+3{\mathrm{<figure-callout\; id="3"\; label="A"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">A</figure-callout>}}_3{\mathrm{\&Delta;T}}^2\\ f(2\mathrm{\&Delta;T}+t_{m-1})=A_1+{4A}_2\mathrm{\&Delta;T}+12{\mathrm{<figure-callout\; id="3"\; label="A"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">A</figure-callout>}}_3{\mathrm{\&Delta;T}}^2\\ f(3\mathrm{\&Delta;T}+t_{m-1})=A_1+{6A}_2\mathrm{\&Delta;T}+27{\mathrm{<figure-callout\; id="3"\; label="A"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">A</figure-callout>}}_3{\mathrm{\&Delta;T}}^2\end{array}\right.---\left(12\right)$

And order:

$\left\{\begin{array}{c}{\mathrm{\&omega;}}_m\left(1\right)=\mathrm{\&omega;}(\mathrm{\&Delta;T}+t_{m-1})\\ {\mathrm{\&omega;}}_m\left(2\right)=\mathrm{\&omega;}(2\mathrm{\&Delta;T}+t_{m-1})\\ {\mathrm{\&omega;}}_m\left(3\right)=\mathrm{\&omega;}(3\mathrm{\&Delta;T}-t_{m-1})\end{array}\right.,,$ $\left\{\begin{array}{c}{f}_m\left(1\right)=f(\mathrm{\&Delta;T}+t_{m-1})\\ f_m\left(2\right)=f(2\mathrm{\&Delta;T}+t_{m-1})\\ f_m\left(3\right)=f(3\mathrm{\&Delta;t}+t_{m-1})\end{array}\right.$

Find the solution formula (11) respectively and formula (12) can get:

a ₁＝3ω _m(1)-3ω _m(2)+ω _m(3)，A ₁＝3f _m(1)-3f _m(2)+f _m(3)

$a_2=\frac{1}{\mathrm{\&Delta;T}}[-\frac{5}{4}{\mathrm{\&omega;}}_m\left(1\right)+{2\mathrm{\&omega;}}_m\left(2\right)-\frac{3}{4}{\mathrm{\&omega;}}_m\left(3\right)],$ $A_2=\frac{1}{\mathrm{\&Delta;T}}[-\frac{5}{4}{f}_m\left(1\right)+{2f}_m\left(2\right)-\frac{3}{4}{f}_m\left(3\right)]---\left(13\right)$

$a_2=\frac{1}{\mathrm{\&Delta;}{T}^2}[\frac{1}{6}{\mathrm{\&omega;}}_m\left(1\right)-\frac{1}{3}{\mathrm{\&omega;}}_m\left(2\right)+\frac{1}{6}{\mathrm{\&omega;}}_m\left(3\right)],$ $A_2=\frac{1}{\mathrm{\&Delta;}{T}^2}[\frac{1}{6}{f}_m\left(1\right)-\frac{1}{3}{f}_m\left(2\right)+\frac{1}{6}{f}_m\left(3\right)]$

With formula (13) substitution formula (10), association type (9) again, formula (4) institute aspect then, that is based on the paddle backoff algorithm formula of angular speed output be:

$\mathrm{\&Delta;}{\hat{V}}_{\mathrm{scu}{l}_m}={\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1\mathrm{\&Delta;}{T}^2[{\mathrm{\&omega;}}_m\left(1\right)\&times;f_m\left(2\right)+f_m\left(1\right)\&times;{\mathrm{\&omega;}}_m\left(2\right)]+$

$k_2{\mathrm{\&Delta;T}}^2[{\mathrm{\&omega;}}_m\left(1\right)\&times;f_m\left(3\right)-{\mathrm{\&omega;}}_m\left(2\right)\&times;f_m\left(3\right)]+---\left(14\right)$

$k_2{\mathrm{\&Delta;T}}^2[f_m\left(1\right)\&times;{\mathrm{\&omega;}}_m\left(3\right)-f_m\left(2\right)\&times;{\mathrm{\&omega;}}_m\left(3\right)]$

Wherein: $k_1=\frac{81}{40},$ $k_2=\frac{9}{40}.$

The Design of Optimal Algorithm process is as follows:

Suppose the classical paddle campaign of carrier do, its model is as follows:

$\left\{\begin{array}{c}\mathrm{\&omega;}\left(t\right)=\mathrm{iB\&Omega;}\cos \mathrm{\&Omega;t}\\ f\left(t\right)=\mathrm{jC}\sin \mathrm{\&Omega;t}\end{array}\right.---\left(15\right)$

Wherein: B, C are respectively to be the angular oscillation amplitude and the line vibration amplitude of two Z-axises along carrier, and Ω is the carrier vibration frequency, and i, j are unit vector.

If note λ=Ω Δ T, τ _n=t _M-1+ n Δ T, n=1,2,3, consider following two formulas (wherein k is a unit vector):

${\mathrm{\&omega;}}_m\left(n\right)\&times;f_m(n+l)=\mathrm{iB\&Omega;}\cos \mathrm{\&Omega;}(t_{m-1}+\mathrm{n\&Delta;T})\&times;\mathrm{jC}\sin \mathrm{\&Omega;}(t_{m-1}+(n+l)\mathrm{\&Delta;T})$

$=\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}\frac{1}{2}\mathrm{BC\&Omega;}(\sin \mathrm{l\&lambda;}+\sin (2{\mathrm{\&Omega;\&tau;}}_n+\mathrm{l\&lambda;}))---\left(16\right)$

$f_m\left(n\right)\&times;{\mathrm{\&omega;}}_m(n+l)=\mathrm{jC}\sin \mathrm{\&Omega;}(t_{m-1}+\mathrm{n\&Delta;T})\&times;\mathrm{iB\&Omega;}\cos \mathrm{\&Omega;}(t_{m-1}+(n+l)\mathrm{\&Delta;T})$

$=\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}\frac{1}{2}\mathrm{BC\&Omega;}(\sin \mathrm{l\&lambda;}-\sin (2\mathrm{\&Omega;}{\mathrm{\&tau;}}_n+\mathrm{l\&lambda;}))---\left(17\right)$

Comparison expression (16) and formula (17) be as can be known: ω _m(n) * f _m(n+l) and f _m(n) * ω _m(n+l) DC quantity among the result is only relevant with sampling interval l, with sampling instant and multiplication cross sequence independence.Therefore formula (14) DC quantity is:

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{\hat{V}}}_{{\mathrm{scul}}_m}k\frac{\mathrm{BC}}{\mathrm{\&Omega;}}{\mathrm{\&lambda;}}^2({\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1\sin \mathrm{\&lambda;}+k_2\sin 2\mathrm{\&lambda;}-k_2\sin \mathrm{\&lambda;})---\left(18\right)$

Calculate as can be known, the exact value of paddle effect compensating item is under the paddle moving condition:

So the Algorithm Error that above-mentioned three increment paddle backoff algorithms cause is:

${\tilde{V}}_{{\mathrm{scul}}_m}=\mathrm{\&Delta;}{\stackrel{\&OverBar;}{\hat{V}}}_{{\mathrm{scul}}_m}-\mathrm{\&Delta;}{V_{\mathrm{scu}{l}_m}}^{\&prime;}$

$=k\frac{\mathrm{BC}}{\mathrm{\&Omega;}}({\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1{\mathrm{\&lambda;}}^2\sin \mathrm{\&lambda;}+k_2{\mathrm{\&lambda;}}^2\sin 2\mathrm{\&lambda;})+k\frac{\mathrm{BC}}{\mathrm{\&Omega;}}(-k_2{\mathrm{\&lambda;}}^2\sin \mathrm{\&lambda;}-\frac{3}{2}\mathrm{\&lambda;}+\frac{1}{2}\mathrm{<figure-callout\; id="3"\; label="sin"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">sin</figure-callout>}3\mathrm{\&lambda;})---\left(19\right)$

It is as follows that sine term in the formula (19) is obtained each power of λ time after by Taylor series expansion:

${\mathrm{\&lambda;}}^3({\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1+k_2-\frac{9}{4}),{\mathrm{\&lambda;}}^5(-\frac{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}{6}-\frac{{7k}_2}{6}+\frac{81}{80}),{\mathrm{\&lambda;}}^7(\frac{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}{240}+\frac{{31k}_2}{240}-\frac{243}{1120}),...\left(20\right)$

Because λ=Ω Δ T, the carrier vibration is mechanical vibration, usually frequency is not high, and Δ T is the sampling period, and magnitude is a Millisecond, so general λ＜1, as seen time low more influence to error of λ power is big more, and should make low order power coefficient when therefore selecting coefficient is zero as far as possible, for three increment algorithms, suppose that third power and five power coefficients are zero, promptly have:

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1+k_2-\frac{9}{4}=0\\ -\frac{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}{6}-\frac{{7k}_2}{6}+\frac{81}{80}=0\end{array}\right.---\left(21\right)$

Find the solution above-mentioned system of equations, can optimize coefficient and be:

The remainder error of being known optimized Algorithm by formula (21) is:

$\mathrm{\&Delta;}{\tilde{V}}_{{\mathrm{scul}}_m}{\&ap;\mathrm{\&lambda;}}^7(\frac{{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HDA0000075816450000011.png"\; state="\{\{state\}\}">k</figure-callout>}}_1}{240}+\frac{{31k}_2}{240}-\frac{243}{1120})=-\frac{573}{4480}\mathrm{BC}{\mathrm{\&Omega;}}^6\mathrm{\&Delta;}{T}^7---\left(22\right)$

Table 1 has been listed two increments and the three increment paddle backoff algorithm formula of the present invention's design and has been optimized coefficient, other many increments paddle backoff algorithm formula and optimization coefficient can and be optimized the derivation of coefficient design process according to said n increment paddle backoff algorithm and draw, and have not enumerated one by one here.

3 three traditional increment paddle backoff algorithms

Be output as under the increment situation at inertia device (gyroscope and accelerometer), three increment paddle backoff algorithms are:

${\hat{V}}_{{\mathrm{scul}}_m}=k_1[{\mathrm{\&Delta;V}}_m\left(1\right)\&times;{\mathrm{\&Delta;\&theta;}}_m\left(3\right)+{\mathrm{\&Delta;\&theta;}}_m\left(1\right)\&times;{\mathrm{\&Delta;V}}_m\left(3\right)]+k_2[{\mathrm{\&Delta;V}}_m\left(1\right)\&times;{\mathrm{\&Delta;\&theta;}}_m\left(2\right)+{\mathrm{\&Delta;V}}_m\left(2\right)\&times;{\mathrm{\&Delta;\&theta;}}_m\left(3\right)]+$ (23)

$k_2[{\mathrm{\&Delta;\&theta;}}_m\left(1\right)\&times;{\mathrm{\&Delta;V}}_m\left(2\right)+{\mathrm{\&Delta;\&theta;}}_m\left(2\right)\&times;{\mathrm{\&Delta;V}}_m\left(3\right)]$

Wherein:

Δ V _m(1), Δ V _m(2), Δ V _m(3) and Δ θ _m(1), Δ θ _m(2), Δ θ _m(3) be respectively sampling time section [t _M-1, t _M-1+ Δ T], [t _M-1+ Δ T, t _M-1+ 2 Δ T], [t _M-1+ 2 Δ T, t _m] interior speed increment and angle increment, the sampling period

Under the paddle motion conditions, optimize coefficient:

Table 1 has been listed traditional two increments and three increment paddle backoff algorithm formula and has been optimized coefficient.What table 2 was listed is that carrier is done amplitude along X-axis

Angular oscillation, do the line vibration of amplitude C=100 along Y-axis, and sampling period Δ T=0.01s, when carrier is taked the different motion frequency, two increment Algorithm Error values of the present invention's design.What table 3 was listed is that carrier is done amplitude along X-axis

Angular oscillation, do the line vibration of amplitude C=100 along Y-axis, and sampling period Δ T=0.01s, when carrier is taked the different motion frequency, the contrast of the three increment Algorithm Error that traditional three increment algorithms and the present invention design.

Table 1 algorithmic formula and optimization index contrast

The error of two increment algorithms of table 2 the present invention design

The traditional three increment algorithms of table 3 and the present invention design the contrast of three increment Algorithm Error

Motion frequency/Hz	Traditional algorithm (m/s)	The present invention (m/s)
			10	-0.0208	-1.3394e-007
20	-0.0822	-8.5721e-006
			50	-0.4665	-0.0021
80	-0.9970	-0.0351

From above-mentioned table as can be seen, under the same movement frequency condition, three increment paddle backoff algorithm errors of the present invention's design are far smaller than traditional three increment Algorithm Error, and Fig. 1 has reflected more intuitively that with curve form traditional three increment paddle backoff algorithms and the present invention design the rule of the error of three increment paddle backoff algorithms with the motion frequency variation.What Fig. 2 reflected is that traditional three increment paddle backoff algorithms and the present invention design the rule of the error of three increment paddle backoff algorithms with the sampling period variation, as can be seen from the figure, under the identical sampling period condition, the three increment paddles compensation of the present invention's design is calculated precision and is higher than traditional algorithm far away.

Claims (2)

1. strapdown system velocity calculated method based on angular speed output, be specially: carrier is at t _mSpeed constantly $V_m=V_{m-1}+{\&Integral;}_{t_{m-1}}^{t_m}[g\left(t\right)-({2\mathrm{\&omega;}}_{\mathrm{ie}}\left(t\right)+{\mathrm{\&omega;}}_{\mathrm{en}}\left(t\right))\&times;V_{m-1}]\mathrm{dt}+{\mathrm{\&Delta;V}}_{s{f}_m},$

Wherein,

V _M-1For carrier at t _M-1Speed constantly,

C _M-1For carrier at t _M-1Attitude matrix constantly,

G (t), ω _Ie(t), ω _En(t) being respectively t acceleration of gravity, rotational-angular velocity of the earth, navigation constantly is the rotational angular velocity that the relative earth is;

$\mathrm{\&Delta;}{V}_{s{f}_m}={\mathrm{\&Delta;V}}_m+\frac{1}{2}\mathrm{\&Delta;}{\mathrm{\&theta;}}_m\&times;{\mathrm{\&Delta;V}}_m+{\mathrm{\&Delta;}\hat{V}}_{{\mathrm{scul}}_m};$

F (t) is that accelerometer is at t output specific force constantly;

ω (t) is that gyro is in t output angle speed constantly;

Paddle effect compensating item

N predictor process as follows:

(a1) cross n sampled point with velocity calculated period T=t _m-t _M-1Be divided into and be spaced apart N sub-period, with $\left\{\begin{array}{c}\mathrm{\&omega;}\left(t\right)=a_1+{2a}_2(t-t_{m-1})+\&CenterDot;\&CenterDot;\&CenterDot;+{\mathrm{na}}_n{(t-t_{m-1})}^{n-1}\\ f\left(t\right)=A_1+2A_2(t-t_{m-1})+\&CenterDot;\&CenterDot;\&CenterDot;+n{A}_n{(t-t_{m-1})}^{n-1}\end{array}\right.$ For objective function is made the polynomial expression linear fit to the specific force output of gyrostatic angular speed output of this n sample point and accelerometer;

(a2) utilize step (a1) to fit definite normal vector a ₁A _n, A ₁A _nCalculate

$\left\{\begin{array}{c}\mathrm{\&Delta;\&theta;}\left(t\right)={\&Integral;}_{t_{m-1}}^t\mathrm{\&omega;}\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;}=a_1(t-t_{m-1})+a_2{(t-t_{m-1})}^2+\&CenterDot;\&CenterDot;\&CenterDot;+a_n{(t-t_{m-1})}^n\\ \mathrm{\&Delta;V}\left(t\right)={\&Integral;}_{t_{m-1}}^{t}f\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;}=A_1(t-t_{m-1})+A_2{(t-t_{m-1})}^2+\&CenterDot;\&CenterDot;\&CenterDot;+A_n{(t-t_{m-1})}^n\end{array}\right.,t_{m-1}\&le;t\&le;t_m;$

(a3) make up paddle effect compensating item computing formula

${\mathrm{\&Delta;V}}_{{\mathrm{scul}}_m}=\frac{1}{2}{\&Integral;}_{t_{m-1}}^{t_m}[\mathrm{\&Delta;\&theta;}\left(t\right)\&times;f\left(t\right)+\mathrm{\&Delta;V}\left(t\right)\&times;\mathrm{\&omega;}\left(t\right)]\mathrm{dt}.$

2. the strapdown system velocity calculated method based on angular speed output according to claim 1 is characterized in that,

N predictor process also comprise step:

(a4) utilize the paddle effect compensating item computing formula of step (a3) to calculate the exact value of paddle effect compensating item under the paddle moving condition

(a5) DC quantity in the paddle effect compensating item computing formula of determining step (a3)

(a6) with difference

Minimum is the paddle effect compensating item computing formula in the objective optimization step (a3).

Images