CN103398713A - Method for synchronizing measured data of star sensor/optical fiber inertial equipment - Google Patents

Abstract

The invention discloses a method for synchronizing measured data of a star sensor/optical fiber inertial equipment. The method comprises the following steps of: (1) collecting an attitude quaternion q, output by the star sensor, of a star sensor coordinate system relative to an inertia coordinate system; (2) obtaining a synchronizing time difference between a gyroscope and the star sensor by adopting a single chip microcomputer; (3) obtaining a posture quaternion q-hat output by the gyroscope by utilizing the synchronizing time difference [delta]t obtained by the step (2) through an extrapolation algorithm; (4) establishing an error quaternion[delta]q by utilizing the star sensor output data q and the gyroscope data q' provided by the steps (1) and (3), so as to establish a system model; (5) carrying out filtering estimation on the system model established by the step (4) by applying an asynchronous filtering algorithm; and (6) carrying out data fusion to obtain high-precision speeds, positions and attitudes. According to the method disclosed by the invention, the extrapolation algorithm and the asynchronous filtering algorithm are adopted so that a delaying problem of the output of the star sensor is effectively solved, a synchronizing value of the gyroscope data and the star sensor data is obtained, and the data fusion precision and the filtering precision are improved.

Description

A kind of star sensor/fiber-optic inertial equipment measures method of data synchronization

Technical field

What the present invention relates to is a kind of Combinated navigation method, particularly relates to the star sensor that multi-frequency measures output/fiber-optic inertial equipment and measures method of data synchronization.

Background technology

At present, improving inertia device precision (hardware point of view) and improving navigation algorithm (software angle) is the main path that improves the navigational system navigation performance: for improving the inertia device precision, not only the rising space of device own is less, and be that the cost that the raising precision is paid is compared with improvement effect, its cost performance is lower, so from the angle of hardware, improve the system navigation performance, just seems that advantage is less; Managing to improve the navigation calculation method is the focus of inertial navigation area research in recent years to improve navigation accuracy, accumulates in time this shortcoming but only rely on inertial navigation system still can't avoid navigation error, the application demand while being difficult to reach long boat.Star sensor is as a kind of high-precision attitude measurement instrument, have that little, the suitable dress property of volume is strong, the measuring error advantage such as accumulation in time not, not only obtained application in the spacecrafts such as spaceship and satellite, and also more and more extensive at naval vessels and missile-borne application.Star sensor/fiber-optic inertial device combination navigational system has the plurality of advantages such as cost is low, precision is high, reliability is high, good concealment, life cycle length, and this also will become the another development trend of navigational system.

In star sensor/fiber-optic inertial device combination navigational system, the inertial navigation system output frequency is exported much higher than star sensor, inertial navigation is generally all greater than 50HZ, and star sensor only has 1-10HZ, carrying out attitude while determining, in conventional kalman filter method, the filtering cycle is elected the update cycle of star sensor as, and the attitude estimated value does not remain unchanged when there is no star sensor output.Hour attitude estimated accuracy is higher when carrier angular velocity, but when carrier angular velocity is larger, the system update cycle length will cause the decline of attitude estimated accuracy; In addition, star sensor output attitude information need to pass through optical device imaging, importance in star map recognition and distinguish that star, attitude determine the processes such as algorithm computing, therefore has certain output delay, and carrier angular velocity is longer larger time delay, so in the moment of carrying out attitude integration with gyro, should be the moment rather than the star sensor output time of taking star chart.This shows, research active data simultaneous techniques has very important significance in the design of star sensor/fiber-optic inertial device combination navigational system.

Summary of the invention

The object of the present invention is to provide a kind of new star sensor/fiber-optic inertial device combination air navigation aid, and can effectively solve multi-frequency measurement output problem, improve navigation accuracy.

The object of the present invention is achieved like this:

A kind of star sensor/fiber-optic inertial equipment measures method of data synchronization, comprises the following steps:

(1) gather the attitude quaternion q of the star sensor coordinate system of star sensor output with respect to inertial coordinates system;

(2) adopt single-chip microcomputer to obtain gyroscope and star sensor synchronization time difference: the square-wave signal of star sensor optical imaging and gyro output is sent into single-chip microcomputer, and utilize the timer of single-chip microcomputer inside, timer arrives and starts the zero clearing timing constantly at gyro data, when the star sensor optical imaging signal arrived, the time of timer was gyro and star sensor synchronization time difference Δ t;

(3) the synchronization time difference Δ t that utilizes step (2) to obtain, obtain by extrapolation algorithm the attitude quaternion that gyro is exported

(4) utilize star sensor output data q and gyro data given in (1), (3) step

Instrument error hypercomplex number δ q, use error quaternion vector part δ e as the measurement amount, and with error quaternion vector part δ e, gyroscope constant value drift b _cWith random drift b _r, for quantity of state, set up system model;

The asynchronous filtering algorithm that a kind of time renewal of system model application of (5) step (4) being set up separates with the measurement renewal carries out filtering and estimates;

(6) gyroscope constant value drift that step (5) is estimated

And random drift

Compensation is the output of the gyro in the time period to t+MT～t+2MT, and each MT all carries out data fusion according to above-mentioned steps in the time period later, completes the error correction of inertial navigation system according to conventional method, obtains high precision velocity, position and attitude.

Described method, described step (3) concrete grammar is: set up following gyro output model:

ω _g＝ω+b _c+b _r+n _g (1)

In formula, ω _gFor the gyro output valve, ω is the gyro true value, b _cFor the constant value drift of gyro, b _rRandom drift for gyro, can be considered first-order Markov process, n _gFor gyro output random noise;

$\left\{\begin{array}{c}{\stackrel{\·}{b}}_c=0\\ {\stackrel{\·}{b}}_r=-{\mathrm{\βb}}_r+\mathrm{\η}\end{array}\right.---\left(2\right)$

In formula (2), β is the inverse correlation time constant of constant value drift, and η is the white noise of random drift;

According to quaternion differential equation:

$\stackrel{\·}{q}=\frac{1}{2}\mathrm{\Ω}\left(\mathrm{\ω}\right)q=\frac{1}{2}q\⊗\mathrm{\ω}---\left(3\right)$

In formula, ω=[ω ₁, ω ₂, ω _r] ^TThe rotational angular velocity of carrier system with respect to inertial system, $\mathrm{\Ω}\left(\mathrm{\ω}\right)=\left[\begin{array}{cc}0& {-\mathrm{\ω}}^T\\ \mathrm{\ω}& -\mathrm{\ω}[\×]\end{array}\right],$

$\mathrm{\ω}[\×]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_3& {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3& 0& {-\mathrm{\ω}}_1\\ -{\mathrm{\ω}}_2& {\mathrm{\ω}}_1& 0\end{array}\right];$

The carrier of gyro output is with respect to the attitude quaternion of inertial space

$\stackrel{\·}{\hat{q}}=\frac{1}{2}\hat{q}\⊗\widehat{\mathrm{\ω}}---\left(4\right)$

In formula (4),

Be respectively the gyro calculated value,

For the constant value drift calculated value of gyro,

Random drift calculated value for gyro;

If t _kGyro output data constantly are

The synchronization time difference of gyro and star sensor is Δ t, t _k+ Δ t is that the gyro data in the moment of star sensor shooting star chart is constantly:

$\hat{q}(t_k+\mathrm{\Δt})=a_0+a_1\mathrm{\Δt}+a_2{\mathrm{\Δt}}^2---\left(5\right)$

In formula (5)

$a_0=\hat{q}\left(t_k\right)$

$a_1=\frac{3\hat{q}\left(t_k\right)-4\hat{q}\left(t_{k-1}\right)+\hat{q}\left(t_{k-2}\right)}{2T}$

$a_2=\frac{\hat{q}\left(t_k\right)-2\hat{q}\left(t_{k-1}\right)+\hat{q}\left(t_{k-2}\right)}{{2T}^2}$

Described method, the concrete grammar of described step (4) is: error quaternion is multiplicative quaternion, is expressed as:

$\mathrm{\&delta;q}={\hat{q}}^{-1}\&CircleTimes;q={[{\mathrm{\&delta;<figure-callout\; id="0"\; label="q"\; filenames="HSA0000092823480000011.png,HSA0000092823480000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_0,\mathrm{\&delta;e}]}^T---\left(6\right)$

In formula, δ q ₀For the scalar part of δ q, δ e is the vector part of δ q, δ e=[δ e ₁δ e ₂δ e ₃] ^T, take δ e as observed quantity, that is:

Z＝[δ _e1 δe ₂ δe ₃] ^T (7)

Observation equation is:

Z(t)＝H(t)X(t)+V(t) (8)

In formula (8), H (t)=[I _{3 * 3}0 _{3 * 12}], V (t) is the three-dimensional measuring noise vector;

Due to:

$\mathrm{\&omega;}-\widehat{\mathrm{\&omega;}}=-(b_c-{\hat{b}}_c)-(b_r-{\hat{b}}_r)-n_g---\left(9\right)$

Drift error:

δω＝-δb _c-δb _r-n _g (10)

Wherein, ${\mathrm{\&delta;b}}_c=b_c-{\hat{b}}_c,{\mathrm{\&delta;b}}_r=b_r-{\hat{b}}_r.$

To formula (6) differentiate, and with formula (3), formula (4), formula (10) substitution:

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{e}=-\widehat{\mathrm{\&omega;}}\&CircleTimes;\mathrm{\&delta;e}-\frac{1}{2}({\mathrm{\&delta;b}}_c+{\mathrm{\&delta;b}}_r)---\left(11\right)$

δq ₀＝0

With error quaternion vector part δ e=[δ e ₁δ e ₂δ e ₃] ^T, gyroscope constant value drift error delta b _c=[δ b _c1δ b _c2δ b _c3] ^TWith random drift δ b _r=[δ b _r1δ b _r2δ b _r3] ^TFor quantity of state, that is:

X＝[δe ₁ δe ₂ δe ₃ δb _c1 δb _c2 δb _c3 δb _r1 δb _r2 δb _r3] ^T (12)

State equation is:

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(13\right)$

In formula (13), $F\left(t\right)=\left[\begin{array}{ccc}-[\widehat{\mathrm{\&omega;}}\&times;]& -\frac{1}{2}{I}_{3\&times;3}& -\frac{1}{2}{I}_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& -{\mathrm{\&beta;I}}_{3\&times;3}\end{array}\right],G\left(t\right)=\left[\begin{array}{ccc}-[\widehat{\mathrm{\&omega;}}\&times;]& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& I_{3\&times;3}\end{array}\right],$ W (t) is the noise vector of system; I is unit matrix.

Described method, described step (5) concrete grammar is: the output gap of establishing star sensor is M times of gyroscope output gap; Star sensor and gyro are exported data time sequence figure as shown in Figure 5, take one-period as example, and t+iT (i=1,2,3 ... M-1; T is the gyroscope output gap) there is no star sensor output in the time period, at t～t _kBetween+Δ t, adopt gyro data to carry out forecast calculation, namely the time upgrades:

${\tilde{b}}_c\left(i\right)={\tilde{b}}_c(i-1),{\tilde{b}}_r\left(i\right)={\tilde{b}}_r(i-1)---\left(15\right)$

In formula (14),

For hypercomplex number state transitions battle array:

The method of error of calculation covariance matrix P (i) is:

P(i)＝Φ·P(i-1)·Φ ^T+Γ·Q·Γ ^T (16)

In formula, Φ is the filter state transfer matrix, and Q is system state noise battle array, and Γ is that system noise drives battle array;

Φ＝I+F(t)·T， $Q=\mathrm{diag}({\mathrm{\&sigma;}}_g^2,{\mathrm{\&sigma;}}_g^2,{\mathrm{\&sigma;}}_g^2,{\mathrm{\&sigma;}}_c^2,{\mathrm{\&sigma;}}_c^2,{\mathrm{\&sigma;}}_c^2,{\mathrm{\&sigma;}}_r^2,{\mathrm{\&sigma;}}_r^2,{\mathrm{\&sigma;}}_r^2)/T,\mathrm{\&Gamma;}=\underset{0}{\overset{T}{\&Integral;}}\mathrm{\&Phi;}\&CenterDot;G\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;},$

For gyro to measure white noise mean square deviation,

Be respectively constant value drift white noise mean square deviation and random drift white noise mean square deviation, fixed by experiment;

At t+MT constantly, the measuring value of star sensor output is arranged, by t _kThe state predicted value of+Δ t obtains:

${\tilde{b}}_c\left(N\right|N-1)={\tilde{b}}_c(N-1),{\tilde{b}}_r\left(N\right|N-1)={\tilde{b}}_r(N-1)---\left(18\right)$

P(N|N-1)＝Φ·P(N-1)·Φ ^T+Γ·Q·Γ ^T (19)

At t _k+ Δ t constantly, carries out the attitude quaternion combination to gyro and star sensor output, measures renewal:

K(N)＝P(N|N-1)H ^T(N)(H(N)P(N|N-1)H ^T(N)+R(N) ^-1 (20)

P(N)＝[I-K(N)H(N)]P(N|N-1) (21)

$\hat{X}\left(N\right)=\hat{X}\left(N\right|N-1)+K\left(N\right)[Z\left(N\right)-H\left(N\right)\hat{X}\left(N\right|N-1)]---\left(22\right)$

Wherein, N=t _k+ Δ t, For the state variable estimated value, Z is observed quantity, and K is the filter gain battle array, and H is for measuring battle array, and R is system measurements noise battle array; R=[σ _w, σ _w, σ _w] ^T/ (MT), σ _wMeasure the white noise mean square deviation for star sensor, be determined by experiment.

Method of the present invention has the following advantages:

(1) adopt hypercomplex number to describe the singular point problem that the inertia attitude has effectively avoided the Eulerian angle method to cause, in addition, than attitude matrix, the hypercomplex number method only has the parameter of a redundancy, and do not comprise trigonometric function operation, be that the higher attitude of a kind of relative efficiency represents mode.

(2) adopt extrapolation algorithm, efficiently solve star sensor output delay problem, obtain gyro data and star sensor data synchronization value, improved the data fusion precision.

(3) adopt asynchronous filtering algorithm, efficiently solve the gyro output frequency and be much higher than the unequal interval information fusion problem of star sensor, improved filtering accuracy.

Description of drawings

Fig. 1 is for adopting traditional algorithm and the improved data synchronized algorithm of the present invention of standard card Kalman Filtering, the east orientation after error compensation, north orientation speed-error curve comparison diagram;

Fig. 2 is for adopting traditional algorithm and the improved data synchronized algorithm of the present invention of standard card Kalman Filtering, the positioning error curve comparison figure after error compensation;

Fig. 3 is for adopting traditional algorithm and the improved data synchronized algorithm of the present invention of standard card Kalman Filtering, the roll error angle after error compensation, pitching error angle, course error angular curve comparison diagram;

Fig. 4 is steps flow chart block diagram of the present invention.

Fig. 5 is star sensor and gyro output data time sequence figure.

Embodiment

Below in conjunction with specific embodiment, the present invention is described in detail.

With reference to figure 4, star sensor/fiber-optic inertial equipment measures method of data synchronization, comprises the following steps:

(1) gather the attitude quaternion q of the star sensor coordinate system of star sensor output with respect to inertial coordinates system;

(2) adopt single-chip microcomputer to obtain gyroscope and star sensor synchronization time difference: the square-wave signal of star sensor optical imaging and gyro output is sent into single-chip microcomputer, and utilize the timer of single-chip microcomputer inside, timer arrives and starts the zero clearing timing constantly at gyro data, when the star sensor optical imaging signal arrived, the time of timer was gyro and star sensor synchronization time difference Δ t.

(3) the synchronization time difference Δ t that utilizes step 2 to obtain, obtain by extrapolation algorithm the attitude quaternion that gyro is exported

Set up following gyro output model:

ω _g＝ω+b _c+b _r+n _g (1)

In formula, ω _gFor the gyro output valve, ω is the gyro true value, b _cFor the constant value drift of gyro, b _rRandom drift for gyro, can be considered first-order Markov process, n _gFor gyro output random noise.

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{b}}_c=0\\ {\stackrel{\&CenterDot;}{b}}_r=-{\mathrm{\&beta;b}}_r+\mathrm{\&eta;}\end{array}\right.---\left(2\right)$

In formula (2), β is the inverse correlation time constant of constant value drift, and η is the white noise of random drift.

According to quaternion differential equation:

$\stackrel{\&CenterDot;}{q}=\frac{1}{2}\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)q=\frac{1}{2}q\&CircleTimes;\mathrm{\&omega;}---\left(3\right)$

In formula, ω=[ω ₁, ω ₂, ω ₃] ^TThe rotational angular velocity of carrier system with respect to inertial system, $\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)=\left[\begin{array}{cc}0& {-\mathrm{\&omega;}}^T\\ \mathrm{\&omega;}& -\mathrm{\&omega;}[\&times;]\end{array}\right],$

$\mathrm{\&omega;}[\&times;]=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_3& {\mathrm{\&omega;}}_2\\ {\mathrm{\&omega;}}_3& 0& {-\mathrm{\&omega;}}_1\\ -{\mathrm{\&omega;}}_2& {\mathrm{\&omega;}}_1& 0\end{array}\right].$

The carrier of gyro output is with respect to the attitude quaternion of inertial space

$\stackrel{\&CenterDot;}{\hat{q}}=\frac{1}{2}\hat{q}\&CircleTimes;\widehat{\mathrm{\&omega;}}---\left(4\right)$

In formula (4),

Be respectively the gyro calculated value,

For the constant value drift calculated value of gyro,

Random drift calculated value for gyro.

If t _kGyro output data constantly are

The synchronization time difference of gyro and star sensor is Δ t, t _k+ Δ t is that the gyro data in the moment of star sensor shooting star chart is constantly

$\hat{q}(t_k+\mathrm{\&Delta;t})=a_0+a_1\mathrm{\&Delta;t}+a_2{\mathrm{\&Delta;t}}^2---\left(5\right)$

In formula (5)

$a_0=\hat{q}\left(t_k\right)$

$a_1=\frac{3\hat{q}\left(t_k\right)-4\hat{q}\left(t_{k-1}\right)+\hat{q}\left(t_{k-2}\right)}{2T}$

$a_2=\frac{\hat{q}\left(t_k\right)-2\hat{q}\left(t_{k-1}\right)+\hat{q}\left(t_{k-2}\right)}{{2T}^2}$

(4) utilize star sensor output data q and gyro data given in (1), (3) step

Instrument error hypercomplex number δ q, use error quaternion vector part δ e as the measurement amount, and with error quaternion vector part δ e, gyroscope constant value drift b _cWith random drift b _r, for quantity of state, set up system model;

Error quaternion is multiplicative quaternion, is expressed as

$\mathrm{\&delta;q}={\hat{q}}^{-1}\&CircleTimes;q={[{\mathrm{\&delta;<figure-callout\; id="0"\; label="q"\; filenames="HSA0000092823480000011.png,HSA0000092823480000012.png"\; state="\{\{state\}\}">q</figure-callout>}}_0,\mathrm{\&delta;e}]}^T---\left(6\right)$

In formula, δ q ₀For the scalar part of δ q, δ e is the vector part of δ q, δ e=[δ e ₁δ e ₂δ e ₃] ^T, take δ e as observed quantity, namely

Z＝[δe ₁ δe ₂ δe ₃] ^T (7)

Observation equation is

Z(t)＝H(t)X(t)+V(t) (8)

In formula (8), H (t)=[I _{3 * 3}0 _{3 * 12}], V (t) is the three-dimensional measuring noise vector.

Due to

$\mathrm{\&omega;}-\widehat{\mathrm{\&omega;}}=-(b_c-{\hat{b}}_c)-(b_r-{\hat{b}}_r)-n_g---\left(9\right)$

Drift error

δω＝-δb _c-δb _r-n _g (10)

Wherein, ${\mathrm{\&delta;b}}_c=b_c-{\hat{b}}_c,{\mathrm{\&delta;b}}_r=b_r-{\hat{b}}_r.$

To formula (6) differentiate, and with formula

(3)

Formula (4), formula (10) substitution,

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{e}=-\widehat{\mathrm{\&omega;}}\&CircleTimes;\mathrm{\&delta;e}-\frac{1}{2}({\mathrm{\&delta;b}}_c+{\mathrm{\&delta;b}}_r)---\left(11\right)$

δq ₀＝0

With error quaternion vector part δ e=[δ e ₁δ e ₂δ e ₃] ^T, gyroscope constant value drift error delta b _c=[δ b _c1δ b _c2δ b _c3] ^TWith random drift δ b _r=[δ b _r1δ b _r2δ b _r3] ^TFor quantity of state, namely

X＝[δe ₁ δe ₂ δe ₃ δb _c1 δb _c2 δb _c3 δb _r1 δb _r2 δb _r3] ^T (12)

State equation is

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(13\right)$

In formula (13), $F\left(t\right)=\left[\begin{array}{ccc}-[\widehat{\mathrm{\&omega;}}\&times;]& -\frac{1}{2}{I}_{3\&times;3}& -\frac{1}{2}{I}_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& {-\mathrm{\&beta;I}}_{3\&times;3}\end{array}\right],G\left(t\right)=\left[\begin{array}{ccc}-[\widehat{\mathrm{\&omega;}}\&times;]& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& I_{3\&times;3}\end{array}\right],$ W (t) is the noise vector of system; I is unit matrix.

The asynchronous filtering algorithm that a kind of time renewal of system model application of (5) step (4) being set up separates with the measurement renewal carries out filtering and estimates;

If the output gap of star sensor is M times of the gyroscope output gap.Star sensor and gyro are exported data time sequence figure as shown in Figure 5, take one-period as example, and t+iT (i=1,2,3 ... M-1; T is the gyroscope output gap) there is no star sensor output in the time period, at t～t _kBetween+Δ t, adopt gyro data to carry out forecast calculation, namely the time upgrades:

${\tilde{b}}_c\left(i\right)={\tilde{b}}_c(i-1),{\tilde{b}}_r\left(i\right)={\tilde{b}}_r(i-1)---\left(15\right)$

In formula (14),

For hypercomplex number state transitions battle array

The method of error of calculation covariance matrix P (i) is

P(i)＝Φ·P(i-1)·Φ ^T+Γ·Q·Γ ^T (16)

In formula, Φ is the filter state transfer matrix, and Q is system state noise battle array, and Γ is that system noise drives battle array.

Φ＝I+F(t)·T， $Q=\mathrm{diag}({\mathrm{\&sigma;}}_g^2,{\mathrm{\&sigma;}}_g^2,{\mathrm{\&sigma;}}_g^2,{\mathrm{\&sigma;}}_c^2,{\mathrm{\&sigma;}}_c^2,{\mathrm{\&sigma;}}_c^2,{\mathrm{\&sigma;}}_r^2,{\mathrm{\&sigma;}}_r^2,{\mathrm{\&sigma;}}_r^2)/T,\mathrm{\&Gamma;}=\underset{0}{\overset{T}{\&Integral;}}\mathrm{\&Phi;}\&CenterDot;G\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;},$

For gyro to measure white noise mean square deviation,

Be respectively constant value drift white noise mean square deviation and random drift white noise mean square deviation, fixed by experiment.

At t+MT constantly, the measuring value of star sensor output is arranged, by t _kThe state predicted value of+Δ t obtains:

${\tilde{b}}_c\left(N\right|N-1)={\tilde{b}}_c(N-1),{\tilde{b}}_r\left(N\right|N-1)={\tilde{b}}_r(N-1)---\left(18\right)$

P(N|N-1)＝Φ·P(N-1)·Φ ^T+Γ·Q·Γ ^T (19)

At t _k+ Δ _tConstantly, gyro and star sensor output are carried out the attitude quaternion combination, measure renewal:

K(N)＝P(N|N-1)H ^T(N)(H(N)P(N|N-1)H ^T(N)+R(N) ^-1 (20)

P(N)＝[I-K(N)H(N)]P(N|N-1) (21)

$\hat{X}\left(N\right)=\hat{X}\left(N\right|N-1)+K\left(N\right)[Z\left(N\right)-H\left(N\right)\hat{X}\left(N\right|N-1)]$

Wherein, N=t _k+ Δ t,

For the state variable estimated value, Z is observed quantity, and K is the filter gain battle array, and H is for measuring battle array, and R is system measurements noise battle array.

R=[σ _w, σ _w, σ _w] ^T/ (MT), σ _wMeasure the white noise mean square deviation for star sensor, be determined by experiment.

(6) gyroscope constant value drift that step (5) is estimated

And random drift

Compensation is the output of the gyro in the time period to t+MT～t+2MT, and each MT all carries out data fusion according to above-mentioned steps in the time period later, completes the error correction of inertial navigation system according to conventional method, obtains high precision velocity, position and attitude.

Beneficial effect of the present invention is verified in the following manner:

Matlab emulation

Under following simulated conditions, the method is carried out emulation experiment:

Wherein: ψ, θ, γ represent respectively the swing angle variable around course angle, pitch angle and roll angle; ψ _m, θ _m, γ _mRepresent respectively corresponding swing angle amplitude; ω _ψ, ω _θ, ω _γRepresent respectively corresponding angle of oscillation frequency;

Represent respectively corresponding initial phase; And ω _i=2 π/T _i, i=ψ, θ, γ, T _iRepresent corresponding rolling period; K is true flight path.T _ψ＝20s，T _θ＝25s，T _γ＝26s。

Carrier initial position: 45.7796 ° of north latitude, 126.6705 ° of east longitudes;

East orientation speed: 3m/s, north orientation speed is read: 2m/s;

The true attitude angle of carrier: ψ=0 °, θ=0 °, γ=30 °;

The Changing Pattern of crab angle: ω=0.1rad/s

Equatorial radius: R _e=6378393.0m;

By the available earth surface acceleration of gravity of universal gravitation: g ₀=9.78049;

Rotational-angular velocity of the earth (radian per second): 7.2921158e-5;

The maximum error of CCD star sensor: η=0.01 °;

Gyro drift: 0.01 °/h;

Gyroscope Random Drift: 0.003 °/h;

Accelerometer bias: 10 ^-4g ₀

Gyro to measure white noise mean square deviation

Constant value drift white noise mean square deviation

Random drift white noise mean square deviation

Star sensor is measured the white noise meansquaredeviationσ _w=1.5 ";

Constant: π=3.1415926;

Inertial navigation output period T=0.02s;

The filtering period T _F=0.1s;

Star sensor output period T _S=ls;

Simulation time: t=1h.

Utilize the described method of invention to obtain the integrated navigation velocity error as shown in Figure 1; As shown in Figure 2, the integrated navigation attitude error as shown in Figure 3 for the integrated navigation site error.Can find out and adopt not only fast convergence rate of algorithm of the present invention by Fig. 1,2,3, and error obviously reduces, navigation accuracy is significantly improved.

Should be understood that, for those of ordinary skills, can be improved according to the above description or conversion, and all these improve and conversion all should belong to the protection domain of claims of the present invention.

Claims (4)

1. star sensor/fiber-optic inertial equipment measures method of data synchronization, it is characterized in that, comprises the following steps:

(1) gather the attitude quaternion q of the star sensor coordinate system of star sensor output with respect to inertial coordinates system;

(2) adopt single-chip microcomputer to obtain gyroscope and star sensor synchronization time difference: the square-wave signal of star sensor optical imaging and gyro output is sent into single-chip microcomputer, and utilize the timer of single-chip microcomputer inside, timer arrives and starts the zero clearing timing constantly at gyro data, when the star sensor optical imaging signal arrived, the time of timer was gyro and star sensor synchronization time difference Δ t;

(3) the synchronization time difference Δ t that utilizes step (2) to obtain, obtain by extrapolation algorithm the attitude quaternion that gyro is exported

(4) utilize star sensor output data q and gyro data given in (1), (3) step

Instrument error hypercomplex number δ q, use error quaternion vector part δ e as the measurement amount, and with error quaternion vector part δ e, gyroscope constant value drift b _cWith random drift b _r, for quantity of state, set up system model;

The asynchronous filtering algorithm that a kind of time renewal of system model application of (5) step (4) being set up separates with the measurement renewal carries out filtering and estimates;

(6) gyroscope constant value drift that step (5) is estimated

And random drift

Compensation is the output of the gyro in the time period to t+MT～t+2MT, and each MT all carries out data fusion according to above-mentioned steps in the time period later, completes the error correction of inertial navigation system according to conventional method, obtains high precision velocity, position and attitude.

2. method according to claim 1, is characterized in that, described step (3) concrete grammar is: set up following gyro output model:

ω _g＝ω+b _c+b _r+n _g (1)

In formula, ω _gFor the gyro output valve, ω is the gyro true value, b _cFor the constant value drift of gyro, b _rRandom drift for gyro, can be considered first-order Markov process, n _gFor gyro output random noise;

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{b}}_c=0\\ {\stackrel{\&CenterDot;}{b}}_r=-{\mathrm{\&beta;b}}_r+\mathrm{\&eta;}\end{array}\right.---\left(2\right)$

In formula (2), β is the inverse correlation time constant of constant value drift, and η is the white noise of random drift;

According to quaternion differential equation:

$\stackrel{\&CenterDot;}{q}=\frac{1}{2}\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)q=\frac{1}{2}q\&CircleTimes;\mathrm{\&omega;}---\left(3\right)$

In formula, ω=[ω ₁, ω ₂, ω ₃] ^TThe rotational angular velocity of carrier system with respect to inertial system, $\mathrm{\&Omega;}\left(\mathrm{\&omega;}\right)=\left[\begin{array}{cc}0& {-\mathrm{\&omega;}}^T\\ \mathrm{\&omega;}& -\mathrm{\&omega;}[\&times;]\end{array}\right],$ $\mathrm{\&omega;}[\&times;]=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_3& {\mathrm{\&omega;}}_2\\ {\mathrm{\&omega;}}_3& 0& {-\mathrm{\&omega;}}_1\\ -{\mathrm{\&omega;}}_2& {\mathrm{\&omega;}}_1& 0\end{array}\right];$

The carrier of gyro output is with respect to the attitude quaternion of inertial space

$\stackrel{\&CenterDot;}{\hat{q}}=\frac{1}{2}\hat{q}\&CircleTimes;\widehat{\mathrm{\&omega;}}---\left(4\right)$

In formula (4),

Be respectively the gyro calculated value,

For the constant value drift calculated value of gyro,

Random drift calculated value for gyro;

If t _kGyro output data constantly are

The synchronization time difference of gyro and star sensor is Δ t, t _k+ Δ t is that the gyro data in the moment of star sensor shooting star chart is constantly:

$\hat{q}(t_k+\mathrm{\&Delta;t})=a_0+a_1\mathrm{\&Delta;t}+a_2{\mathrm{\&Delta;t}}^2---\left(5\right)$

In formula (5)

$a_0=\hat{q}\left(t_k\right)$

$a_1=\frac{3\hat{q}\left(t_k\right)-4\hat{q}\left(t_{k-1}\right)+\hat{q}\left(t_{k-2}\right)}{2T}$

$a_2=\frac{\hat{q}\left(t_k\right)-2\hat{q}\left(t_{k-1}\right)+\hat{q}\left(t_{k-2}\right)}{{2T}^2}$

3. method according to claim 1, is characterized in that, the concrete grammar of described step (4) is: error quaternion is multiplicative quaternion, is expressed as:

$\mathrm{\&delta;q}={\hat{q}}^{-1}\&CircleTimes;q={[{\mathrm{\&delta;q}}_0,\mathrm{\&delta;e}]}^T---\left(6\right)$

In formula, δ q ₀For the scalar part of δ q, δ e is the vector part of δ q, δ e=[δ e ₁δ e ₂δ e ₃] ^T, take δ e as observed quantity, that is:

Z＝[δe ₁ δe ₂ δe ₃] ^T (7)

Observation equation is:

Z(t)＝H(t)X(t)+V(t) (8)

In formula (8), H (t)=[I _{3 * 3}0 _{3 * 12}], V (t) is the three-dimensional measuring noise vector;

Due to:

$\mathrm{\&omega;}-\widehat{\mathrm{\&omega;}}=-(b_c-{\hat{b}}_c)-(b_r-{\hat{b}}_r)-n_g---\left(9\right)$

Drift error:

δω＝-δb _c-δb _r-n _g (10)

Wherein, ${\mathrm{\&delta;b}}_c=b_c-{\hat{b}}_c,{\mathrm{\&delta;b}}_r=b_r-{\hat{b}}_r.$

To formula (6) differentiate, and with formula (3), formula (4), formula (10) substitution:

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{e}=-\widehat{\mathrm{\&omega;}}\&CircleTimes;\mathrm{\&delta;e}-\frac{1}{2}({\mathrm{\&delta;b}}_c+{\mathrm{\&delta;b}}_r)---\left(11\right)$

δq ₀＝0

With error quaternion vector part δ e=[δ e ₁δ e ₂δ e ₃] ^T, gyroscope constant value drift error delta b _c=[δ b _c1δ b _c2δ b _c3] ^TWith random drift δ b _r=[δ b _r1δ b _r2δ b _r3] ^TFor quantity of state, that is:

X＝[δe ₁ δe ₂ δe ₃ δb _c1 δb _c2 δb _c3 δb _r1 δb _r2 δb _r3] ^T(12)

State equation is:

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(13\right)$

In formula (13), $F\left(t\right)=\left[\begin{array}{ccc}-[\widehat{\mathrm{\&omega;}}\&times;]& -\frac{1}{2}{I}_{3\&times;3}& -\frac{1}{2}{I}_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& -{\mathrm{\&beta;I}}_{3\&times;3}\end{array}\right],G\left(t\right)=\left[\begin{array}{ccc}-[\widehat{\mathrm{\&omega;}}\&times;]& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& I_{3\&times;3}\end{array}\right],$ W (t) is the noise vector of system; I is unit matrix.

4. method according to claim 1, is characterized in that, described step (5) concrete grammar is: the output gap of establishing star sensor is M times of gyroscope output gap; Star sensor and gyro are exported data time sequence figure as shown in Figure 5, take one-period as example, and t+iT (i=1,2,3 ... M-1; T is the gyroscope output gap) there is no star sensor output in the time period, at t～t _kBetween+Δ t, adopt gyro data to carry out forecast calculation, namely the time upgrades:

${\tilde{b}}_c\left(i\right)={\tilde{b}}_c(i-1),{\tilde{b}}_r\left(i\right)={\tilde{b}}_r(i-1)---\left(15\right)$

In formula (14),

For hypercomplex number state transitions battle array:

The method of error of calculation covariance matrix P (i) is:

P(i)＝Φ·P(i-1)·Φ ^T+Γ·Q·Γ ^T (16)

In formula, Φ is the filter state transfer matrix, and Q is system state noise battle array, and Γ is that system noise drives battle array;

Φ＝I+F(t)·T， $Q=\mathrm{diag}({\mathrm{\&sigma;}}_g^2,{\mathrm{\&sigma;}}_g^2,{\mathrm{\&sigma;}}_g^2,{\mathrm{\&sigma;}}_c^2,{\mathrm{\&sigma;}}_c^2,{\mathrm{\&sigma;}}_c^2,{\mathrm{\&sigma;}}_r^2,{\mathrm{\&sigma;}}_r^2,{\mathrm{\&sigma;}}_r^2)/T,\mathrm{\&Gamma;}=\underset{0}{\overset{T}{\&Integral;}}\mathrm{\&Phi;}\&CenterDot;G\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;},$ For gyro to measure white noise mean square deviation, Be respectively constant value drift white noise mean square deviation and random drift white noise mean square deviation, fixed by experiment;

At t+MT constantly, the measuring value of star sensor output is arranged, by t _kThe state predicted value of+Δ t obtains:

${\tilde{b}}_c\left(N\right|N-1)={\tilde{b}}_c(N-1),{\tilde{b}}_r\left(N\right|N-1)={\tilde{b}}_r(N-1)---\left(18\right)$

P(N|N-1)=Φ·P(N-1)·Φ ^T+Γ·Q·Γ ^T (19)

At t _k+ Δ t constantly, carries out the attitude quaternion combination to gyro and star sensor output, measures renewal:

K(N)＝P(N|N-1)H ^T(N)(H(N)P(N|N-1)H ^T(N)+R(N) ^-1 (20)

P(N)＝[I-K(N)H(N)]P(N|N-1) (21)

$\hat{X}\left(N\right)=\hat{X}\left(N\right|N-1)+K\left(N\right)[Z\left(N\right)-H\left(N\right)\hat{X}\left(N\right|N-1)]---\left(22\right)$

Wherein, N=t _k+ Δ t,

For the state variable estimated value, Z is observed quantity, and K is the filter gain battle array, and H is for measuring battle array, and R is system measurements noise battle array; R=[σ _w, σ _w, σ _w] ^T/ (MT), σ _wMeasure the white noise mean square deviation for star sensor, be determined by experiment.

Images