CN103791903A - Ship long voyage and large maneuverability-targeted star sensor dynamic compensation method - Google Patents

Abstract

The invention provides a ship long voyage and large maneuverability-targeted star sensor dynamic compensation method. The method comprises the following steps: (1) collecting the attitude quaternion q1 of a combined navigation system in T1 moment; (2) drifting a compensated gyroscope to provide the angular speed omega of a ship at the current moment; (3) resolving the rotation angles Delta phi and Delta q within the time period of T2-T1 according to the angular speed; (4) predicting the attitude quaternion q<2c>of a star sensor at the T2 moment according to the information in the steps (1) and (3); (5) according to the predicted attitude, resolving the optical axis pointing (A,D) of the star sensor at the T2 moment, the rolling angle thi of the star sensor, and the forecast stellar image coordinates (x', y'), and resolving to obtain the practical output q<2c> of the star sensor; (6), performing data fusion on the resolved attitude q<gyo> and q<2c> of the gyroscope at the T2 moment by utilizing kalman filtering, to obtain the attitude output q<2> of the combined navigation system, and correcting the errors of the gyroscope. According to the method, the working efficiency of the star sensor can be improved, the requirement of the real-time sway of the ship on high output frequency can be met, and the method is suitable for long-time voyage of the ship.

Description

A kind of star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships

Technical field

What the present invention relates to is a kind of star sensor dynamic compensation method for ship's navigation feature, is a kind of work efficiency of navigator and method of navigation accuracy of improving.

Background technology

Star sensor is using indestructible natural celestial body as its navigation beacon, without any prior imformation, only need to the star chart photographing successively carry out the courses of work such as importance in star map recognition, the extraction of celestial body barycenter, star pattern matching and attitude algorithm just exportable star sensor coordinate system with respect to the attitude quaternion of inertial system.The courses of work such as the inner importance in star map recognition of star sensor and asterism extraction will expend the plenty of time, have a strong impact on the output frequency of star sensor.Meanwhile, gyro exists error to increase this shortcoming along with the accumulation of time, and these are inapplicable beyond doubt in long this specific application environment of oscillating motion, hours underway for a long time for boats and ships sail.

Summary of the invention

The object of the present invention is to provide a kind of work efficiency high, can avoid moving because Ship Swaying makes asterism produce picture the problem that causes star sensor output accuracy to reduce, be suitable for the star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships in the long-time course of boats and ships.

The object of the present invention is achieved like this:

(1) gather T ₁moment integrated navigation system attitude quaternion q ₁;

(2) gyro after drift compensation provides boats and ships current time angular velocity omega;

(3) solve at T according to angular velocity ₂-T ₁corner in time period

and then try to achieve and rotate hypercomplex number δ q;

(4) according to step (1), (3) information prediction T ₂the attitude quaternion of moment star sensor

(5) according to prediction attitude, resolve T ₂moment star sensor optical axis points to (A, D), the star image coordinate (x' of the roll angle θ of star sensor and forecast, y'), in the subrange centered by forecasting star image coordinate, carry out asterism scanning and barycenter and extract, resolve the actual output q that obtains star sensor _2c;

(6) utilize Kalman filtering by T ₂moment gyro resolves attitude q _gyowith q _2ccarry out data fusion, obtain the attitude output q of integrated navigation ₂and gyro error is proofreaied and correct.

In star sensor location, horizontal reference is generally provided by Platform Inertial Navigation System, but platform-type inertial navigation cost is higher, and volume and quality are larger simultaneously, have limited the application of star sensor.Methods of Strapdown Inertial Navigation System cost is low, the ability of anti-vibration and impact is strong.The attitude matrix of strapdown inertial navigation system has similar physical significance to physical platform, can make star sensor break away from the restriction of physics horizontal reference, thereby reduce costs and weight.The horizontal reference error of known 10'' can cause the positioning error of 300m, so the present invention is the star sensor positioning precision that raising strapdown is installed on naval vessel, the horizontal reference when horizontal attitude providing with strapdown inertial navigation system is located as star sensor, so the horizontal attitude precision of strapdown inertial navitation system (SINS) output will seriously limit the positioning precision of star sensor.Because Inertial Measurement Unit error accumulates in time, make the horizontal accuracy that inertial navigation system provides cannot meet the long-time navigation of demand and naval vessel of star sensor for the requirement of navigation accuracy.

Solve that the star sensor output cycle is long, maneuverability is large, hours underway is long etc., and problem becomes star sensor is applied to the key of ship navigation.The present invention has proposed a kind of star sensor dynamic compensation method for the large maneuverability of the long boat in naval vessel according to this background.

Method of the present invention has the following advantages:

(1) this method utilizes gyro to gather the real-time angular velocity in naval vessel, meets the naval vessel requirement in swinging condition for a long time;

(2) by the prediction that next moment star sensor optical axis is pointed to, instruct completing fast of the courses of work such as star chart scanning, barycenter extraction, star pattern matching, improve star sensor output frequency;

(3), simultaneously for meeting the long boat demand in naval vessel, the attitude quaternion filtering that star sensor and gyro are resolved, estimates and compensate gyro error, has also avoided making due to ship sway the problem of star sensor output accuracy decline.

Accompanying drawing explanation

Fig. 1 has schematically shown schematic diagram of the present invention.In figure, for the star sensor output attitude quaternion of prediction; q _2cfor the actual output attitude quaternion of star sensor, q _gyofor the attitude quaternion that Information Monitoring is resolved according to gyro.

Embodiment

For example the present invention is described in more detail below in conjunction with accompanying drawing:

(1) gather T ₁the attitude quaternion q of moment integrated navigation system ₁, T ₁the attitude quaternion in moment is the attitude of exporting after a time cycle integrated navigation filter correction.

(2) gyro provides naval vessel current time angular velocity omega.Affected by the uncertain factors such as stormy waves, naval vessel is for a long time in swinging condition, so the angular velocity on naval vessel is provided in real time by gyro.Because gyro error accumulates in time, the present invention carries out error correction to gyro in (6) step, meets the requirement of the long boat in naval vessel to navigation accuracy.

(3) solve at T according to angular velocity ₂-T ₁corner in time period

and then try to achieve and rotate hypercomplex number δ q:

Known to T ₁to T ₂time period inside lock is:

Rotating hypercomplex number is expressed as:

Wherein, e _x, e _y, e _zbe expressed as x, y, the vector of unit length of z axle.

(4) can predict T according to (1), (3) information ₂the attitude quaternion in moment

$\widehat{q_{2c}}=q_1\mathrm{\&delta;q}=\widehat{q_{2c1}}\stackrel{\&RightArrow;}{i}+\hat{q_{2\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000465075860000011.png"\; state="\{\{state\}\}">c</figure-callout>}2}}\stackrel{\&RightArrow;}{j}+\widehat{q_{2c3}}\stackrel{\&RightArrow;}{k}+\widehat{q_{2c4}}$

(5) attitude quaternion according to weather report

can be in the hope of T ₂moment star sensor optical axis points to (A, D), the star image coordinate (x' of the roll angle θ of star sensor and forecast, y'), in the subrange centered by forecasting star image coordinate, carry out asterism scanning and barycenter and extract, obtain the actual output q of star sensor _2c.

$A=\arctan \left(\frac{\hat{q_{2\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000465075860000011.png"\; state="\{\{state\}\}">c</figure-callout>}2}}\widehat{q_{2c3}}-\widehat{q_{2c1}}\widehat{q_{2c4}}}{\widehat{q_{2c1}}\widehat{q_{2c3}}+\hat{q_{2\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000465075860000011.png"\; state="\{\{state\}\}">c</figure-callout>}2}}\widehat{q_{2c4}}}\right)$

$D=\arcsin (-{\widehat{q_{2c1}}}^2-{\hat{q_{2\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000465075860000011.png"\; state="\{\{state\}\}">c</figure-callout>}2}}}^2+{\widehat{q_{2c3}}}^2+{\widehat{q_{2c4}}}^2)$

$\mathrm{\&theta;}=\arctan \left(\frac{-2(\widehat{q_{2c1}}\hat{q_{2\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000465075860000011.png"\; state="\{\{state\}\}">c</figure-callout>}2}}-\widehat{q_{2c3}}\widehat{q_{2c4}})}{{\widehat{q_{2c1}}}^2-{\hat{q_{2\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000465075860000011.png"\; state="\{\{state\}\}">c</figure-callout>}2}}}^2-{\widehat{q_{2c3}}}^2+{\widehat{q_{2c4}}}^2}\right)$

The ideal coordinates that fixed star of known right ascension declination (α, δ) is predicted in as plane are:

$\left\{\begin{array}{c}x=\frac{\cos \mathrm{\&delta;}\sin (\mathrm{\&alpha;}-A)}{\sin \mathrm{\&delta;}\sin D+\cos \mathrm{\&delta;}\cos D\cos (\mathrm{\&alpha;}-A)}\\ y=\frac{\sin \mathrm{\&delta;}\cos D-\cos \mathrm{\&delta;}\sin D\cos (\mathrm{\&alpha;}-A)}{\sin \mathrm{\&delta;}\sin D+\cos \mathrm{\&delta;}\cos D\cos (\mathrm{\&alpha;}-A)}\end{array}\right.$

But because star sensor strapdown is arranged on naval vessel, the angular motion on naval vessel makes star sensor produce roll angle θ, and then the fixed star of known right ascension declination at the actual coordinate as planar prediction is:

$\left\{\begin{array}{c}{x}^{\&prime;}=x\cos \mathrm{\&theta;}-y\sin \mathrm{\&theta;}\\ y^{\&prime;}=x\sin \mathrm{\&theta;}+y\cos \mathrm{\&theta;}\end{array}\right.$

The prediction star image coordinate that obtains known fixed star, carries out asterism scanning at its subrange, compared with the scanning of view picture star chart, greatly save sweep time, because the right ascension declination of fixed star is known, omit star pattern matching process simultaneously, improved star sensor work efficiency.In asterism scanning, barycenter extracts, and after star pattern matching, obtains the actual output q of star sensor according to attitude algorithm _2c.

(6) utilize Kalman filtering by T ₂moment gyro resolves attitude q _gyowith q _2ccarry out data fusion, obtain the attitude output q of integrated navigation ₂and gyro error is proofreaied and correct, detailed process is as follows:

Utilize gyro data to resolve attitude process as follows:

Gyro data

the projection at naval vessel coordinate system with respect to the angular velocity of inertial coordinates system for naval vessel coordinate system, its antisymmetric matrix is expressed as:

${\mathrm{\&Omega;}}_{\mathrm{ib}}^b=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]$

In formula ${\mathrm{\&omega;}}_{\mathrm{ib}}^b=0+{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\stackrel{\&RightArrow;}{i_b}+{\mathrm{\&omega;}}_{\mathrm{iby}}^b\stackrel{\&RightArrow;}{j_b}+{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\stackrel{\&RightArrow;}{k_b},\stackrel{\&RightArrow;}{i_b},\stackrel{\&RightArrow;}{j_b},\stackrel{\&RightArrow;}{k_b}$ For the base of body axis system,

for

component on three axles.

The attitude quaternion q that gyro resolves _gyoits differential equation is expressed as:

$\stackrel{\&CenterDot;}{q_{\mathrm{gyo}}}=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{ib}}^b{q}_{\mathrm{gyo}}$

Expression is:

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{q_{\mathrm{gyo}1}}\\ \stackrel{\&CenterDot;}{{\mathrm{<figure-callout\; id="2"\; label="q\; gyo"\; filenames="HDA0000465075860000011.png"\; state="\{\{state\}\}">q</figure-callout>}}_{\mathrm{gyo}}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}3}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}4}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{\mathrm{gyo}1}\\ q_{\mathrm{gyo}2}\\ q_{\mathrm{gyo}3}\\ q_{\mathrm{gyo}4}\end{array}\right]$

It is solved and can obtain T ₂moment q _gyo.

Next utilize Kalman Filter Technology by T ₂moment gyro resolves attitude q _gyowith star sensor output attitude q _2ccarry out data fusion.

The difference of the attitude quaternion of the attitude quaternion of star sensor output and gyro output is expressed as: Δ q=q _2c-q _gyo

Choose poor, the gyroscopic drift b of attitude quaternion of star sensor and gyro as the state vector of filtering system, be designated as x _k.

x _k=[Δq ^T,b ^T] ^T

Choose the difference of attitude quaternion of star sensor and gyro output as observation vector, be designated as z _k.

z _k=Δq=q _2c-q _gyo

If φ _{k, k-1}for t _k-1time be carved into t _kthe Matrix of shifting of a step in moment, τ _k-1for system noise drives battle array, H _kfor measuring battle array, ν _kfor measurement noise sequence, ω _k-1for system incentive noise sequence.Set up following filtering kinetics equation:

$\left\{\begin{array}{c}{x}_k={\mathrm{\&phi;}}_{k,k-1}{x}_{k-1}+{\mathrm{\&tau;}}_{k-1}{\mathrm{\&omega;}}_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}\right.$

Wherein, the statistical property of system noise and measurement noise meets following formula:

$\left\{\begin{array}{cc}E\left[v_k\right]=0& E\left[v_k{v}_j^T\right]=Q_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[{\mathrm{\&tau;}}_k\right]=0& E\left[{\mathrm{\&tau;}}_k{\mathrm{\&tau;}}_j^T\right]=R_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[v_k{\mathrm{\&tau;}}_j^T\right]& \mathrm{}\end{array}\right.$

Wherein, δ _kjfor Crow Buddhist nun gram function, Q _kand R _kit is respectively the covariance matrix of process noise and measurement noise.

Obtain the hypercomplex number error in corresponding moment and the optimal value of gyroscopic drift by filtering, gyroscopic drift is fed back to gyro unit and compensate, the attitude q that utilizes error quaternion to resolve gyro _gyorevise, obtain the attitude data q2 of degree of precision.

Claims (9)

1. for a star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships, it is characterized in that:

(1) gather T ₁moment integrated navigation system attitude quaternion q ₁;

(2) gyro after drift compensation provides boats and ships current time angular velocity omega;

(3) solve at T according to angular velocity ₂-T ₁corner in time period

and then try to achieve and rotate hypercomplex number δ q;

(4) according to step (1), (3) information prediction T ₂the attitude quaternion of moment star sensor

(5) according to prediction attitude, resolve T ₂moment star sensor optical axis points to (A, D), the star image coordinate (x' of the roll angle θ of star sensor and forecast, y'), in the subrange centered by forecasting star image coordinate, carry out asterism scanning and barycenter and extract, resolve the actual output q that obtains star sensor _2c;

(6) utilize Kalman filtering by T ₂moment gyro resolves attitude q _gyowith q _2ccarry out data fusion, obtain the attitude output q of integrated navigation ₂and gyro error is proofreaied and correct.

2. the star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships according to claim 1, is characterized in that describedly solving at T according to angular velocity ₂-T ₁corner in time period

and then try to achieve and rotate hypercomplex number δ q and specifically comprise:

Known to T ₁to T ₂time period inside lock is:

Rotating hypercomplex number is expressed as:

Wherein, e _x, e _y, e _zbe expressed as x, y, the vector of unit length of z axle.

3. the star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships according to claim 1 and 2, is characterized in that described prediction T ₂the attitude quaternion in moment

for:

$\widehat{q_{2c}}=q_1\mathrm{\&delta;q}=\widehat{q_{2c1}}\stackrel{\&RightArrow;}{i}+\widehat{q_{2c2}}\stackrel{\&RightArrow;}{j}+\widehat{q_{2c3}}\stackrel{\&RightArrow;}{k}+\widehat{q_{2c4}}.$

4. the star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships according to claim 1 and 2, is characterized in that:

$A=\arctan \left(\frac{\widehat{q_{2c2}}\widehat{q_{2c3}}-\widehat{q_{2c1}}\widehat{q_{2c4}}}{\widehat{q_{2c1}}\widehat{q_{2c3}}+\widehat{q_{2c2}}\widehat{q_{2c4}}}\right)$

$D=\arcsin (-{\widehat{q_{2c1}}}^2-{\widehat{q_{2c2}}}^2+{\widehat{q_{2c3}}}^2+{\widehat{q_{2c4}}}^2)$

$\mathrm{\&theta;}=\arctan \left(\frac{-2(\widehat{q_{2c1}}\widehat{q_{2c2}}-\widehat{q_{2c3}}\widehat{q_{2c4}})}{{\widehat{q_{2c1}}}^2-{\widehat{q_{2c2}}}^2-{\widehat{q_{2c3}}}^2+{\widehat{q_{2c4}}}^2}\right)$

$\left\{\begin{array}{c}{x}^{\&prime;}=x\cos \mathrm{\&theta;}-y\sin \mathrm{\&theta;}\\ y^{\&prime;}=x\sin \mathrm{\&theta;}+y\cos \mathrm{\&theta;}\end{array}\right..$

5. the star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships according to claim 3, is characterized in that:

$A=\arctan \left(\frac{\widehat{q_{2c2}}\widehat{q_{2c3}}-\widehat{q_{2c1}}\widehat{q_{2c4}}}{\widehat{q_{2c1}}\widehat{q_{2c3}}+\widehat{q_{2c2}}\widehat{q_{2c4}}}\right)$

$D=\arcsin (-{\widehat{q_{2c1}}}^2-{\widehat{q_{2c2}}}^2+{\widehat{q_{2c3}}}^2+{\widehat{q_{2c4}}}^2)$

$\mathrm{\&theta;}=\arctan \left(\frac{-2(\widehat{q_{2c1}}\widehat{q_{2c2}}-\widehat{q_{2c3}}\widehat{q_{2c4}})}{{\widehat{q_{2c1}}}^2-{\widehat{q_{2c2}}}^2-{\widehat{q_{2c3}}}^2+{\widehat{q_{2c4}}}^2}\right)$

$\left\{\begin{array}{c}{x}^{\&prime;}=x\cos \mathrm{\&theta;}-y\sin \mathrm{\&theta;}\\ y^{\&prime;}=x\sin \mathrm{\&theta;}+y\cos \mathrm{\&theta;}\end{array}\right..$

6. the star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships according to claim 1 and 2, is characterized in that the described Kalman filtering of utilizing is by T ₂moment gyro resolves attitude q _gyowith q _2ccarry out data fusion, obtain the attitude output q of integrated navigation ₂and gyro error is proofreaied and correct, detailed process is as follows:

Utilize gyro data to resolve attitude process to be:

Gyro data

the projection at naval vessel coordinate system with respect to the angular velocity of inertial coordinates system for naval vessel coordinate system, its antisymmetric matrix is expressed as:

${\mathrm{\&Omega;}}_{\mathrm{ib}}^b=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]$

In formula ${\mathrm{\&omega;}}_{\mathrm{ib}}^b=0+{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\stackrel{\&RightArrow;}{i_b}+{\mathrm{\&omega;}}_{\mathrm{iby}}^b\stackrel{\&RightArrow;}{j_b}+{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\stackrel{\&RightArrow;}{k_b},\stackrel{\&RightArrow;}{i_b},\stackrel{\&RightArrow;}{j_b},\stackrel{\&RightArrow;}{k_b}$ For the base of body axis system,

for

component on three axles;

The attitude quaternion q that gyro resolves _gyoits differential equation is expressed as:

$\stackrel{\&CenterDot;}{q_{\mathrm{gyo}}}=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{ib}}^b{q}_{\mathrm{gyo}}$

Expression is:

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{q_{\mathrm{gyo}1}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}2}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}3}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}4}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{\mathrm{gyo}1}\\ q_{\mathrm{gyo}2}\\ q_{\mathrm{gyo}3}\\ q_{\mathrm{gyo}4}\end{array}\right]$

It is solved and obtains T ₂moment q _gyo;

Utilize Kalman Filter Technology by T ₂moment gyro resolves attitude q _gyowith star sensor output attitude q _2ccarry out data fusion;

The difference of the attitude quaternion of the attitude quaternion of star sensor output and gyro output is expressed as: Δ q=q _2c-q _gyo

Choose poor, the gyroscopic drift b of attitude quaternion of star sensor and gyro as the state vector of filtering system, be designated as x _k.

x _k=[Δq ^T,b ^T] ^T

Choose the difference of attitude quaternion of star sensor and gyro output as observation vector, be designated as z _k,

z _k=Δq=q _2c-q _gyo

If φ _{k, k-1}for t _k-1time be carved into t _kthe Matrix of shifting of a step in moment, τ _k-1for system noise drives battle array, H _kfor measuring battle array, ν _kfor measurement noise sequence, ω _k-1for system incentive noise sequence; Set up following filtering kinetics equation:

$\left\{\begin{array}{c}{x}_k={\mathrm{\&phi;}}_{k,k-1}{x}_{k-1}+{\mathrm{\&tau;}}_{k-1}{\mathrm{\&omega;}}_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}\right.$

Wherein, the statistical property of system noise and measurement noise meets following formula:

$\left\{\begin{array}{cc}E\left[v_k\right]=0& E\left[v_k{v}_j^T\right]=Q_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[{\mathrm{\&tau;}}_k\right]=0& E\left[{\mathrm{\&tau;}}_k{\mathrm{\&tau;}}_j^T\right]=R_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[v_k{\mathrm{\&tau;}}_j^T\right]& \mathrm{}\end{array}\right.$

Wherein, δ _kjfor Crow Buddhist nun gram function, Q _kand R _kit is respectively the covariance matrix of process noise and measurement noise;

Obtain the hypercomplex number error in corresponding moment and the optimal value of gyroscopic drift by filtering, gyroscopic drift is fed back to gyro unit and compensate, the attitude q that utilizes error quaternion to resolve gyro _gyorevise, obtain attitude data q ₂.

7. the star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships according to claim 3, is characterized in that the described Kalman filtering of utilizing is by T ₂moment gyro resolves attitude q _gyowith q _2ccarry out data fusion, obtain the attitude output q of integrated navigation ₂and gyro error is proofreaied and correct, detailed process is as follows:

Utilize gyro data to resolve attitude process to be:

Gyro data the projection at naval vessel coordinate system with respect to the angular velocity of inertial coordinates system for naval vessel coordinate system, its antisymmetric matrix is expressed as:

${\mathrm{\&Omega;}}_{\mathrm{ib}}^b=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]$

In formula ${\mathrm{\&omega;}}_{\mathrm{ib}}^b=0+{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\stackrel{\&RightArrow;}{i_b}+{\mathrm{\&omega;}}_{\mathrm{iby}}^b\stackrel{\&RightArrow;}{j_b}+{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\stackrel{\&RightArrow;}{k_b},\stackrel{\&RightArrow;}{i_b},\stackrel{\&RightArrow;}{j_b},\stackrel{\&RightArrow;}{k_b}$ For the base of body axis system,

for

component on three axles;

The attitude quaternion q that gyro resolves _gyoits differential equation is expressed as:

$\stackrel{\&CenterDot;}{q_{\mathrm{gyo}}}=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{ib}}^b{q}_{\mathrm{gyo}}$

Expression is:

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{q_{\mathrm{gyo}1}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}2}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}3}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}4}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{\mathrm{gyo}1}\\ q_{\mathrm{gyo}2}\\ q_{\mathrm{gyo}3}\\ q_{\mathrm{gyo}4}\end{array}\right]$

It is solved and obtains T ₂moment q _gyo;

Utilize Kalman Filter Technology by T ₂moment gyro resolves attitude q _gyowith star sensor output attitude q _2ccarry out data fusion;

The difference of the attitude quaternion of the attitude quaternion of star sensor output and gyro output is expressed as: Δ q=q _2c-q _gyo

Choose poor, the gyroscopic drift b of attitude quaternion of star sensor and gyro as the state vector of filtering system, be designated as x _k.

x _k=[Δq ^T,b ^T] ^T

Choose the difference of attitude quaternion of star sensor and gyro output as observation vector, be designated as z _k,

z _k=Δq=q _2c-q _gyo

If φ _{k, k-1}for t _k-1time be carved into t _kthe Matrix of shifting of a step in moment, τ _k-1for system noise drives battle array, H _kfor measuring battle array, ν _kfor measurement noise sequence, ω _k-1for system incentive noise sequence; Set up following filtering kinetics equation:

$\left\{\begin{array}{c}{x}_k={\mathrm{\&phi;}}_{k,k-1}{x}_{k-1}+{\mathrm{\&tau;}}_{k-1}{\mathrm{\&omega;}}_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}\right.$

Wherein, the statistical property of system noise and measurement noise meets following formula:

$\left\{\begin{array}{cc}E\left[v_k\right]=0& E\left[v_k{v}_j^T\right]=Q_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[{\mathrm{\&tau;}}_k\right]=0& E\left[{\mathrm{\&tau;}}_k{\mathrm{\&tau;}}_j^T\right]=R_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[v_k{\mathrm{\&tau;}}_j^T\right]& \mathrm{}\end{array}\right.$

Wherein, δ _kjfor Crow Buddhist nun gram function, Q _kand R _kit is respectively the covariance matrix of process noise and measurement noise;

Obtain the hypercomplex number error in corresponding moment and the optimal value of gyroscopic drift by filtering, gyroscopic drift is fed back to gyro unit and compensate, the attitude q that utilizes error quaternion to resolve gyro _gyorevise, obtain attitude data q ₂.

8. the star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships according to claim 4, is characterized in that the described Kalman filtering of utilizing is by T ₂moment gyro resolves attitude q _gyowith q _2ccarry out data fusion, obtain the attitude output q of integrated navigation ₂and gyro error is proofreaied and correct, detailed process is as follows:

Utilize gyro data to resolve attitude process to be:

Gyro data

the projection at naval vessel coordinate system with respect to the angular velocity of inertial coordinates system for naval vessel coordinate system, its antisymmetric matrix is expressed as:

${\mathrm{\&Omega;}}_{\mathrm{ib}}^b=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]$

In formula ${\mathrm{\&omega;}}_{\mathrm{ib}}^b=0+{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\stackrel{\&RightArrow;}{i_b}+{\mathrm{\&omega;}}_{\mathrm{iby}}^b\stackrel{\&RightArrow;}{j_b}+{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\stackrel{\&RightArrow;}{k_b},\stackrel{\&RightArrow;}{i_b},\stackrel{\&RightArrow;}{j_b},\stackrel{\&RightArrow;}{k_b}$ For the base of body axis system,

for

component on three axles;

The attitude quaternion q that gyro resolves _gyoits differential equation is expressed as:

$\stackrel{\&CenterDot;}{q_{\mathrm{gyo}}}=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{ib}}^b{q}_{\mathrm{gyo}}$

Expression is:

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{q_{\mathrm{gyo}1}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}2}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}3}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}4}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{\mathrm{gyo}1}\\ q_{\mathrm{gyo}2}\\ q_{\mathrm{gyo}3}\\ q_{\mathrm{gyo}4}\end{array}\right]$

It is solved and obtains T ₂moment q _gyo;

Utilize Kalman Filter Technology by T ₂moment gyro resolves attitude q _gyowith star sensor output attitude q _2ccarry out data fusion;

The difference of the attitude quaternion of the attitude quaternion of star sensor output and gyro output is expressed as: Δ q=q _2c-q _gyo

Choose poor, the gyroscopic drift b of attitude quaternion of star sensor and gyro as the state vector of filtering system, be designated as x _k.

x _k=[Δq ^T,b ^T] ^T

Choose the difference of attitude quaternion of star sensor and gyro output as observation vector, be designated as z _k,

z _k=Δq=q _2c-q _gyo

If φ _{k, k-1}for t _k-1time be carved into t _kthe Matrix of shifting of a step in moment, τ _k-1for system noise drives battle array, H _kfor measuring battle array, ν _kfor measurement noise sequence, ω _k-1for system incentive noise sequence; Set up following filtering kinetics equation:

$\left\{\begin{array}{c}{x}_k={\mathrm{\&phi;}}_{k,k-1}{x}_{k-1}+{\mathrm{\&tau;}}_{k-1}{\mathrm{\&omega;}}_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}\right.$

Wherein, the statistical property of system noise and measurement noise meets following formula:

$\left\{\begin{array}{cc}E\left[v_k\right]=0& E\left[v_k{v}_j^T\right]=Q_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[{\mathrm{\&tau;}}_k\right]=0& E\left[{\mathrm{\&tau;}}_k{\mathrm{\&tau;}}_j^T\right]=R_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[v_k{\mathrm{\&tau;}}_j^T\right]& \mathrm{}\end{array}\right.$

Wherein, δ _kjfor Crow Buddhist nun gram function, Q _kand R _kit is respectively the covariance matrix of process noise and measurement noise;

Obtain the hypercomplex number error in corresponding moment and the optimal value of gyroscopic drift by filtering, gyroscopic drift is fed back to gyro unit and compensate, the attitude q that utilizes error quaternion to resolve gyro _gyorevise, obtain attitude data q ₂.

9. the star sensor dynamic compensation method for the large maneuverability of the long boat of boats and ships according to claim 5, is characterized in that the described Kalman filtering of utilizing is by T ₂moment gyro resolves attitude q _gyowith q _2ccarry out data fusion, obtain the attitude output q of integrated navigation ₂and gyro error is proofreaied and correct, detailed process is as follows:

Utilize gyro data to resolve attitude process to be:

Gyro data

the projection at naval vessel coordinate system with respect to the angular velocity of inertial coordinates system for naval vessel coordinate system, its antisymmetric matrix is expressed as:

${\mathrm{\&Omega;}}_{\mathrm{ib}}^b=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]$

In formula ${\mathrm{\&omega;}}_{\mathrm{ib}}^b=0+{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\stackrel{\&RightArrow;}{i_b}+{\mathrm{\&omega;}}_{\mathrm{iby}}^b\stackrel{\&RightArrow;}{j_b}+{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\stackrel{\&RightArrow;}{k_b},\stackrel{\&RightArrow;}{i_b},\stackrel{\&RightArrow;}{j_b},\stackrel{\&RightArrow;}{k_b}$ For the base of body axis system,

for

component on three axles;

The attitude quaternion q that gyro resolves _gyoits differential equation is expressed as:

$\stackrel{\&CenterDot;}{q_{\mathrm{gyo}}}=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{ib}}^b{q}_{\mathrm{gyo}}$

Expression is:

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{q_{\mathrm{gyo}1}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}2}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}3}}\\ \stackrel{\&CenterDot;}{q_{\mathrm{gyo}4}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& -{\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b& {\mathrm{\&omega;}}_{\mathrm{iby}}^b& -{\mathrm{\&omega;}}_{\mathrm{ibx}}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{\mathrm{gyo}1}\\ q_{\mathrm{gyo}2}\\ q_{\mathrm{gyo}3}\\ q_{\mathrm{gyo}4}\end{array}\right]$

It is solved and obtains T ₂moment q _gyo;

Utilize Kalman Filter Technology by T ₂moment gyro resolves attitude q _gyowith star sensor output attitude q _2ccarry out data fusion;

The difference of the attitude quaternion of the attitude quaternion of star sensor output and gyro output is expressed as: Δ q=q _2c-q _gyo

Choose poor, the gyroscopic drift b of attitude quaternion of star sensor and gyro as the state vector of filtering system, be designated as x _k.

x _k=[Δq ^T,b ^T] ^T

Choose the difference of attitude quaternion of star sensor and gyro output as observation vector, be designated as z _k,

z _k=Δq=q _2c-q _gyo

If φ _{k, k-1}for t _k-1time be carved into t _kthe Matrix of shifting of a step in moment, τ _k-1for system noise drives battle array, H _kfor measuring battle array, ν _kfor measurement noise sequence, ω _k-1for system incentive noise sequence; Set up following filtering kinetics equation:

$\left\{\begin{array}{c}{x}_k={\mathrm{\&phi;}}_{k,k-1}{x}_{k-1}+{\mathrm{\&tau;}}_{k-1}{\mathrm{\&omega;}}_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}\right.$

Wherein, the statistical property of system noise and measurement noise meets following formula:

$\left\{\begin{array}{cc}E\left[v_k\right]=0& E\left[v_k{v}_j^T\right]=Q_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[{\mathrm{\&tau;}}_k\right]=0& E\left[{\mathrm{\&tau;}}_k{\mathrm{\&tau;}}_j^T\right]=R_k{\mathrm{\&delta;}}_{\mathrm{kj}}\\ E\left[v_k{\mathrm{\&tau;}}_j^T\right]& \mathrm{}\end{array}\right.$

Wherein, δ _kjfor Crow Buddhist nun gram function, Q _kand R _kit is respectively the covariance matrix of process noise and measurement noise;

Obtain the hypercomplex number error in corresponding moment and the optimal value of gyroscopic drift by filtering, gyroscopic drift is fed back to gyro unit and compensate, the attitude q that utilizes error quaternion to resolve gyro _gyorevise, obtain attitude data q ₂.

Images