CN101713666B - Single-shaft rotation-stop scheme-based mooring and drift estimating method - Google Patents

Abstract

The invention provides a single-shaft rotation-stop scheme-based mooring and drift estimating method, which comprises the following steps: determining an initial position parameter through GPS; acquiring output of an optical fiber gyroscope and data output by and an accelerometer and processing the data; adopting 8 rotation-stop sequences as a transposition scheme of one rotation period by an IMU; performing real-time update on a strapdown matrix according to the output of the gyroscope under the rotation state of the IMU, and simultaneously performing coordinate conversion on acceleration information acquired under the rotation state of the IMU and converting the acceleration information to a mathematic platform coordinate system; establishing a Kalman drift estimation module of a separation position loop in a mooring state of a load according to a load moving base error model; and processing constant errors of inertial apparatuses obtained after Kalman filtering by an average filtering method to obtain a relatively stable estimated mean value. When the load is in a mooring state, the drift estimating method provided by the invention can accurately estimate the constant errors of the inertial apparatuses by the Kalman filtering technology.

Description

A kind of mooring and drift estimating method based on single-shaft rotation-stop scheme

Technical field

What the present invention relates to is a kind of measuring method, in particular a kind of mooring and drift estimating method based on single-shaft rotation-stop scheme.

Background technology

Strapdown inertial navigation system SINS is a kind of autonomous navigational system fully, utilize the angular motion and the line kinematic parameter in gyroscope and accelerometer measures carrier relative inertness space, under given starting condition, carry out integral operation by computing machine, position, speed and attitude information are provided continuously, in real time.Because SINS relies on its own inertial element fully, do not rely on any external information to measure navigational parameter, therefore has good concealment, be not subjected to weather condition restriction, advantage such as interference-free, be a kind of complete autonomous type, round-the-clock navigational system, be widely used in fields such as Aeronautics and Astronautics, navigation.According to the ultimate principle of SINS, the existence that SINS inertia device in navigation procedure often is worth deviation is the principal element that causes the inertial navigation system bearing accuracy to be difficult to improve.How limiting the inertial navigation error effectively, to disperse, improve the inertial navigation system precision be the very important problem in inertial navigation field.

According to the ultimate principle of SINS, SINS needs to obtain initial information before entering navigational state, comprises initial position, speed and attitude.Wherein the precision of initial attitude is exactly the initial alignment precision of SINS when entering navigational state, therefore must at first finish determining of initial attitude before strapdown inertial navitation system (SINS) enters navigational state.Because " platform " is the benchmark of measuring specific force, therefore the initial alignment of " platform " is just extremely important; The strapdown matrix plays the effect of mathematical platform in strapdown inertial navigation system.Therefore the critical problem of initial alignment is exactly the initial value that how to obtain the initial strapdown matrix of degree of precision in the short period of time.And the inertia device deviation is to cause initial strapdown matrix to have the principal element of deviation, and the importance of therefore estimating the technology of floating is just embodied.

The rotation modulation technique is a kind of automatic correcting method of inertial navigation system.It does not need to introduce external calibration information, can be automatically the normal value deviation of inertia device in the system be averaged, and reaches and offsets the influence of drift to system accuracy.Thereby can improve the precision that inertial navigation system works long hours, give full play to the advantage of inertial navigation " autonomous type ".Use the rotation modulation technique, can also use the inertia device of lower accuracy, constitute the inertial navigation system of degree of precision, help reducing the cost of inertial navigation system, simultaneously owing to introduce the observability degree that extraneous motion can improve the inertial navigation system partial parameters effectively.

When the naval vessel is in moored condition, because being subjected to the influence of wave factor produces and to wave and swing motion, disturbing acceleration and disturbance angle velocity degree of being accelerated meter and gyro perception respectively that these disturbed motions produce, wherein exist bigger error in the bearer rate that the integral element in the disturbing acceleration process inertial navigation system calculates, compare the observed quantity of back as the system filter model with the carrier actual linear velocity.The inaccurate speed of convergence that will cause system can reduce parameter when estimating of observed quantity reduces the estimated accuracy of parameter simultaneously.

Open report related to the present invention in the CNKI database has: 1, " the error Modulation analysis of spin fiber strapdown inertial navigation system ", a kind of novel optical fiber strapdown inertial navigation system with the continous-stable rotation platform is proposed, when being intended to improve system's navigation accuracy, do not increase the cost of inertial navigation system.Study the composition structure and the principle of work of this inertial navigation system, analyze spin fiber strapdown inertial navigation system error modulated process, at last a whole set of inertial navigation system is carried out emulation.2, " utilize the alignment of orientation of rotation modulation technology quick high accuracy ", proposed the technical indicator of development rotation modulating system.But all do not propose modulator approach related to the present invention and estimate bleaching method.

Summary of the invention

When the object of the present invention is to provide a kind of carrier to be in moored condition, can estimate a kind of mooring and drift estimating method of the normal value deviation of inertia device exactly based on single-shaft rotation-stop scheme.

The object of the present invention is achieved like this: may further comprise the steps:

(1) determines the initial position parameters of carrier by GPS, and bind to navigational computer;

(2) strapdown inertial navitation system (SINS) is carried out preheating and is prepared, and gathers the data of fibre optic gyroscope and quartz accelerometer output and data are handled;

(3) Inertial Measurement Unit (IMU) adopts 8 commentaries on classics to stop the transposition scheme that order is a swing circle;

(4) determine the strapdown matrix T according to gyrostatic output under the rotation status _s ^p, and the output of degree of will speed up meter is transformed into the mathematical platform coordinate system;

Model is floated in estimating when (5) setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system, the bearer rate v that provides with GPS ₀Be benchmark, and with the difference of inertial navigation system clearing speed as observed quantity, utilize Kalman Filter Technology to realize the estimation of inertia device deviation;

(6) inertia device that Kalman Filter Estimation is gone out often is worth deviation and adopts the method for average filter it to be carried out smoothly the influence of filtering fluctuation within a narrow range.

The present invention can also comprise:

1, to adopt 8 commentaries on classics to stop order be that the transposition scheme of a swing circle is for described IMU: order 1, IMU clockwise rotates 180 ° of in-position C, T when stopping from the A point _tOrder 2, IMU clockwise rotates 90 ° of in-position D, stand-by time T from the C point _tOrder 3, IMU rotates counterclockwise 180 ° of in-position B, stand-by time T from the D point _tOrder 4, IMU rotates counterclockwise 90 ° of in-position A, stand-by time T from the B point _tOrder 5, IMU rotates counterclockwise 180 ° of in-position C, stand-by time T from the A point _tOrder 6, IMU rotates counterclockwise 90 ° of in-position B, stand-by time T from the C point _tOrder 7, IMU clockwise rotates 180 ° of in-position D, stand-by time T from the B point _tOrder 8, IMU clockwise rotates 90 ° of in-position A, stand-by time T from the D point _tIMU rotates sequential loop according to this to carry out; IMU pause point p3, p8 and p4, p7 on the wherein horizontal east orientation axle are symmetrical in the rotating shaft center; Pause point p1 on the north orientation axle, p5 and p2, p6 are symmetrical in the rotating shaft center.

2, describedly determine the strapdown matrix T according to gyrostatic output under the rotation status _s ^p, and the method that the output of degree of will speed up meter is transformed into the mathematical platform coordinate system is:

According to the quadravalence runge kutta method, the application hypercomplex number is carried out real-time update to the strapdown matrix of inertial navigation system, and its formulate is:

${\stackrel{\·}{T}}_s^p=T_s^p[\×{\mathrm{\ω}}_{\mathrm{is}}^s]$

ω wherein _Is ^sThe output of expression gyroscope; Utilize the transformational relation of IMU coordinate system and mathematical platform coordinate system, calculate the accekeration under the mathematical platform coordinate system, $f^p=T_s^p{f}^s.$

3, described estimating when setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system floats model, the bearer rate v that provides with GPS ₀Be benchmark, and with the difference of inertial navigation system clearing speed as observed quantity, utilize Kalman Filter Technology to realize that the estimation approach of inertia device deviation is:

Adopt carrier of separating error of position information model to float model as estimating of system, foundation is that the Kalman filtering state equation and the velocity error of state variable is the measurement equation of measurement amount with the velocity error;

1) set up the state equation of Kalman filtering:

The state error of rotation strapdown inertial navitation system (SINS) is described with linear first-order differential equation:

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)$

The state vector of etching system when wherein X (t) is t; A (t) and B (t) are respectively the state matrix and the noise matrix of system; W (t) is the system noise vector;

The state vector of system is:

The white noise vector of system is:

W＝[a _x?a _y?ω _x?ω _y?ω _z?0?0?0?0?0] ^T

δ V wherein _e, δ V _nThe velocity error of representing east orientation, north orientation respectively;

Be respectively IMU coordinate system ox _s, oy _sAxis accelerometer zero partially; ε _x, ε _y, ε _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant value drift of axle gyro; a _x, a _yBe respectively IMU coordinate system ox _s, oy _sThe white noise error of axis accelerometer; ω _x, ω _y, ω _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe white noise error of axle gyro;

The state-transition matrix of system is:

$A\left(t\right)=\left[\begin{array}{cc}{\mathrm{<figure-callout\; id="5"\; label="F"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">F</figure-callout>}}_{5\&times;5}& {\mathrm{<figure-callout\; id="5"\; label="T"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">T</figure-callout>}}_{5\&times;5}\\ 0_{5\&times;5}& 0_{5\&times;5}\end{array}\right]$

Wherein:

${\mathrm{<figure-callout\; id="5"\; label="F"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">F</figure-callout>}}_{5\&times;5}=\left[\begin{array}{cc}{\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;3}\\ F_{3\&times;2}& F_{3\&times;3}\end{array}\right]$

${\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;2}=\left[\begin{array}{cc}\frac{V_n\tan \mathrm{<figure-callout\; id="2"\; label="L\; R\; n"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">L</figure-callout>}}{R_n}& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}\\ -2({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0\end{array}\right]$

${\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;3}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\end{array}\right]$

$F_{3\&times;2}=\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{{\mathrm{<figure-callout\; id="0"\; label="R\; n"\; filenames="H2009100732329E00022.png"\; state="\{\{state\}\}">R</figure-callout>}}_n}& 0\\ \frac{\tan \mathrm{<figure-callout\; id="0"\; label="L\; R\; n"\; filenames="H2009100732329E00022.png"\; state="\{\{state\}\}">L</figure-callout>}}{R_n}& 0\end{array}\right]$

$F_{3\&times;3}=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0& -\frac{V_n}{R_m}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n}& \frac{V_{\mathrm{<figure-callout\; id="0"\; label="n\; R\; m"\; filenames="H2009100732329E00022.png"\; state="\{\{state\}\}">n</figure-callout>}}}{R_m}& 0\end{array}\right]$

V _e, V _nThe speed of representing east orientation, north orientation respectively; ω _IeThe expression rotational-angular velocity of the earth; R _m, R _nRepresent earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches respectively; L represents local latitude; f _e, f _n, f _uBe expressed as respectively navigation coordinate system down east orientation, north orientation, day to specific force;

The strapdown matrix T _s ^pFor:

$T_s^p=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

Then

${\mathrm{<figure-callout\; id="5"\; label="T"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">T</figure-callout>}}_{5\&times;5}=\left[\begin{array}{ccccc}{T}_{11}& {\mathrm{<figure-callout\; id="12"\; label="T"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">T</figure-callout>}}_{12}& 0& 0& 0\\ T_{21}& T_{22}& 0& 0& 0\\ 0& 0& -T_{11}& -T_{12}& -T_{13}\\ 0& 0& -T_{21}& -T_{22}& -T_{23}\\ 0& 0& -T_{31}& -T_{32}& -T_{33}\end{array}\right];$

2) set up the measurement equation of Kalman filtering:

The measurement equation of describing the rotation strapdown inertial navitation system (SINS) with linear first-order differential equation is as follows:

Z(t)＝H(t)X(t)+v(t)

Wherein: the measurement vector of etching system during Z (t) expression t; The measurement matrix of H (t) expression system; The measurement noise of v (t) expression system;

The system measurements matrix is:

$H\left(t\right)=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

Amount is measured as the horizontal velocity error:

v(t)＝[v _e?v _n] ^T。

4, the described inertia device that Kalman Filter Estimation is gone out often is worth deviation and adopts the method for average filter that it is carried out smoothly, and the method for the influence of filtering fluctuation within a narrow range is: known equidistant sampled point x ₀＜x ₁＜...＜x _N-1＜... on observation data be y ₀, y ₁..., y _N-1, y _iExpression y _iSmooth value; Getting weighting coefficient is 1, get 400 seconds to 600 seconds data after the low-pass filtering do once level and smooth, then:

${\stackrel{\&OverBar;}{y}}_i=\frac{y_{i-n}+y_{i-n+1}+\&CenterDot;\&CenterDot;\&CenterDot;+y_0+{\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="H2009100732329E00031.png"\; state="\{\{state\}\}">y</figure-callout>}}_1+\&CenterDot;\&CenterDot;\&CenterDot;+y_{i+n-2}+y_{i+n-1}}{n}.$

The present invention utilizes the intermittent observability degree that improves partial parameters in the inertial navigation system that rotates of Inertial Measurement Unit (IMU); According to the little characteristics of carrier positions variation range under the moored condition, the Kalman Filter Technology that the application vector position loop separates with system estimates on three directions accelerometer bias on the gyroscope constant value drift and horizontal direction.Consider that the inertia device that estimates under the carrier moored condition often is worth in the deviation fluctuation that exists among a small circle, the method for the The data average filter that estimates is carried out it smoothly.

The present invention's advantage compared with prior art is: the present invention has broken under the naval vessel moored condition, because the interference of waving and swinging motion causes navigation system to estimate the technology of floating down and is difficult to suitable this problem, utilize the rotation of IMU can improve the observability degree of components of system as directed parameter effectively, consider that simultaneously naval vessel moored condition upper/lower positions information change amplitude causes ignoring the coupled relation of position loop and system for a short time, makes carrier be issued to estimated accuracy preferably at moored condition.

The effect useful to the present invention is described as follows:

Under Visual C++ simulated conditions, this method is carried out emulation experiment:

Setting the gyroscope constant value drift is 0.01 °/h, and the accelerometer zero drift is 10 ^-4G; System's initial alignment error is 0.1 °, 0.1 °, 0.5 °; Carrier is done the three-axis swinging motion with sinusoidal rule around pitch axis, axis of roll and course axle, and its mathematical model is:

$\left\{\begin{array}{c}\mathrm{\&theta;}={\mathrm{\&theta;}}_m\sin ({\mathrm{\&omega;}}_{\mathrm{\&theta;}}t+{\mathrm{\&phi;}}_{\mathrm{\&theta;}})\\ \mathrm{\&gamma;}={\mathrm{\&gamma;}}_m\sin ({\mathrm{\&omega;}}_{\mathrm{\&gamma;}}t+{\mathrm{\&phi;}}_{\mathrm{\&gamma;}})\\ \mathrm{\&psi;}={\mathrm{\&psi;}}_m\sin ({\mathrm{\&omega;}}_{\mathrm{\&psi;}}t+{\mathrm{\&phi;}}_{\mathrm{\&psi;}})+k\end{array}\right.$

Wherein: θ, γ, ψ represent the angle variables of waving of pitch angle, roll angle and course angle respectively; θ _m, γ _m, ψ _mThe angle amplitude is waved in expression accordingly respectively; ω _θ, ω _γ, ω _ψRepresent corresponding angle of oscillation frequency respectively; φ _θ, φ _γ, φ _ψRepresent corresponding initial phase respectively; ω _i=2 π/T _i, i=θ, γ, ψ, T _iRepresent corresponding rolling period, k is the angle, initial heading.Get during emulation: θ _m=6 °, γ _m=12 °, ψ _m=10 °, T _θ=8s, T _γ=10s, T _ψ=6s, k=0 °.

The swaying of carrier, surging and hang down and swing the linear velocity that causes and be:

In the formula, i=x, y, z be geographic coordinate system east orientation, north orientation, day to. $A_{D_x}=0.02m,$ $A_{D_y}=0.03m,$ $A_{D_z}=0.3m;$ $T_{D_x}=7s,$ $T_{D_y}=6s,$ $T_{D_z}=8s;$

For going up, [0,2 π] obey equally distributed random phase.

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

The initial attitude error angle: three initial attitude error angles are zero;

Equatorial radius: R _e=6378393.0m;

Ellipsoid degree: e=3.367e-3;

The earth surface acceleration of gravity that can get by universal gravitation: g ₀=9.78049;

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

The gyroscope constant value drift: 0.01 degree/hour;

The gyroscope random walk: 0.001 degree/

Accelerometer bias: 10 ^-4g ₀

Accelerometer noise: 10 ^-6g ₀

Constant: π=3.1415926.

Description of drawings

Fig. 1 is a kind of mooring and drift estimating method based on single-shaft rotation-stop scheme of the present invention;

Fig. 2 is an IMU uniaxial four-position rotation and stop of the present invention;

Fig. 3 is the gyroscope constant value drift of estimation of the present invention;

Fig. 4 is the accelerometer bias on the horizontal direction of estimation of the present invention.

Embodiment

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

(1) determines the initial position parameters of carrier by GPS, and bind to navigational computer;

(2) strapdown inertial navitation system (SINS) is carried out preheating and is prepared, and gathers the data of fibre optic gyroscope and quartz accelerometer output and data are handled;

(3) IMU adopts 8 commentaries on classics to stop the transposition scheme that order is a swing circle (as accompanying drawing 2);

Order 1, IMU clockwise rotates 180 ° of in-position C, stand-by time T from the A point _tOrder 2, IMU clockwise rotates 90 ° of in-position D, stand-by time T from the C point _tOrder 3, IMU rotates counterclockwise 180 ° of in-position B, stand-by time T from the D point _tOrder 4, IMU rotates counterclockwise 90 ° of in-position A, stand-by time T from the B point _tOrder 5, IMU rotates counterclockwise 180 ° of in-position C, stand-by time T from the A point _tOrder 6, IMU rotates counterclockwise 90 ° of in-position B, stand-by time T from the C point _tOrder 7, IMU clockwise rotates 180 ° of in-position D, stand-by time T from the B point _tOrder 8, IMU clockwise rotates 90 ° of in-position A, stand-by time T from the D point _tIMU rotates sequential loop according to this to carry out.Positive and negative average in order effectively the inertia device deviation on the horizontal direction to be carried out on symmetric position, IMU pause point p3, p8 and p4, p7 on the horizontal east orientation axle of definition are symmetrical in the rotating shaft center; Pause point p1 on the north orientation axle, p5 and p2, p6 are symmetrical in the rotating shaft center.It is that carry out at 180 ° or 90 ° of intervals that four-position rotation and stop scheme remains rotational angle.

(4) determine the strapdown matrix T according to gyrostatic output under the rotation status _s ^p, and the output of degree of will speed up meter is transformed into the mathematical platform coordinate system;

According to the quadravalence runge kutta method, the application hypercomplex number is carried out real-time update to the strapdown matrix of inertial navigation system, and its formulate is:

${\stackrel{\&CenterDot;}{T}}_s^p=T_s^p[\&times;{\mathrm{\&omega;}}_{\mathrm{is}}^s]---\left(1\right)$

ω wherein _Is ^sThe output of expression gyroscope.Utilize the transformational relation of IMU coordinate system and mathematical platform coordinate system, calculate the accekeration under the mathematical platform coordinate system.

$f^p=T_s^p{f}^s---\left(2\right)$

Model is floated in estimating when (5) setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system, the bearer rate v that provides with GPS ₀Be benchmark, and settle accounts the difference of speed as observed quantity with inertial navigation system.Utilize Kalman Filter Technology to realize the estimation of inertia device deviation;

Because adopt the rotation of IMU can improve the observability degree of partial parameters in the inertial navigation system effectively, the change in location amplitude of carrier is little when considering moored condition simultaneously, therefore can adopt carrier of separating error of position information model to float model as estimating of system.Foundation is that the Kalman filtering state equation and the velocity error of state variable is the measurement equation of measurement amount with the velocity error;

1) set up the state equation of Kalman filtering:

The state error of rotation strapdown inertial navitation system (SINS) is described with linear first-order differential equation:

$\stackrel{\&CenterDot;}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(3\right)$

The state vector of etching system when wherein X (t) is t; A (t) and B (t) are respectively the state matrix and the noise matrix of system; W (t) is the system noise vector;

The state vector of system is:

The white noise vector of system is:

W＝[a _x?a _y?ω _x?ω _y?ω _z?0?0?0?0?0] ^T (5)

δ V wherein _e, δ V _nThe velocity error of representing east orientation, north orientation respectively;

Be respectively IMU coordinate system ox _s, oy _sAxis accelerometer zero partially; ε _x, ε _y, ε _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant value drift of axle gyro; a _x, a _yBe respectively IMU coordinate system ox _s, oy _sThe white noise error of axis accelerometer; ω _x, ω _y, ω _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe white noise error of axle gyro;

The state-transition matrix of system is:

$A\left(t\right)=\left[\begin{array}{cc}{\mathrm{<figure-callout\; id="5"\; label="F"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">F</figure-callout>}}_{5\&times;5}& {\mathrm{<figure-callout\; id="5"\; label="T"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">T</figure-callout>}}_{5\&times;5}\\ 0_{5\&times;5}& 0_{5\&times;5}\end{array}\right]---\left(6\right)$

Wherein:

${\mathrm{<figure-callout\; id="5"\; label="F"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">F</figure-callout>}}_{5\&times;5}=\left[\begin{array}{cc}{\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;2}& {\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;3}\\ F_{3\&times;2}& F_{3\&times;3}\end{array}\right]---\left(7\right)$

${\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;2}=\left[\begin{array}{cc}\frac{V_n\tan \mathrm{<figure-callout\; id="2"\; label="L\; R\; n"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">L</figure-callout>}}{R_n}& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}\\ -2({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0\end{array}\right]---\left(8\right)$

${\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">F</figure-callout>}}_{2\&times;3}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\end{array}\right]---\left(9\right)$

$F_{3\&times;2}=\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{{\mathrm{<figure-callout\; id="0"\; label="R\; n"\; filenames="H2009100732329E00022.png"\; state="\{\{state\}\}">R</figure-callout>}}_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right]---\left(10\right)$

$F_{3\&times;3}=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0& -\frac{V_n}{R_m}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n}& \frac{V_n}{R_m}& 0\end{array}\right]---\left(11\right)$

V _e, V _nThe speed of representing east orientation, north orientation respectively; ω _IeThe expression rotational-angular velocity of the earth; R _m, R _nRepresent earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches respectively; L represents local latitude; f _e, f _n, f _uBe expressed as respectively navigation coordinate system down east orientation, north orientation, day to specific force.

Make the strapdown matrix T _s ^pFor:

$T_s^p=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(12\right)$

Then

${\mathrm{<figure-callout\; id="5"\; label="T"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">T</figure-callout>}}_{5\&times;5}=\left[\begin{array}{ccccc}{T}_{11}& {\mathrm{<figure-callout\; id="12"\; label="T"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">T</figure-callout>}}_{12}& 0& 0& 0\\ T_{21}& T_{22}& 0& 0& 0\\ 0& 0& -T_{11}& -T_{12}& -T_{13}\\ 0& 0& -T_{21}& -T_{22}& -T_{23}\\ 0& 0& -T_{31}& -T_{32}& -T_{33}\end{array}\right]---\left(13\right)$

2) set up the measurement equation of Kalman filtering:

The measurement equation of describing the rotation strapdown inertial navitation system (SINS) with linear first-order differential equation is as follows:

Z(t)＝H(t)X(t)+v(t) (14)

Wherein: the measurement vector of etching system during Z (t) expression t; The measurement matrix of H (t) expression system; The measurement noise of v (t) expression system;

The system measurements matrix is:

$H\left(t\right)=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(15\right)$

Amount is measured as the horizontal velocity error:

v(t)＝[v _e?v _n] ^T (16)

(6) inertia device that Kalman Filter Estimation is gone out often is worth deviation and adopts the method for average filter it to be carried out smoothly the influence of filtering fluctuation within a narrow range;

Consider that the inertia device that estimates under the carrier moored condition often is worth the fluctuation that exists in the deviation among a small circle, therefore such signal is unfavorable for that inertia device often is worth the correction of deviation, adopts inertia device after weighting asks the average filter algorithm to Kalman filtering often to be worth deviation information and carries out denoising Processing once more.This method is the simplest a kind of signal processing mode, and algorithmic formula is as follows: establish known equidistant sampled point x ₀＜x ₁＜...＜x _N-1＜... on observation data be y ₀, y ₁..., y _N-1, y _iExpression y _iSmooth value.We get weighting coefficient is 1, gets 400 seconds to 600 seconds data after the low-pass filtering and does once smoothly, then has:

${\stackrel{\&OverBar;}{y}}_i=\frac{y_{i-n}+y_{i-\mathrm{<figure-callout\; id="1"\; label="n\; +"\; filenames="H2009100732329E00031.png"\; state="\{\{state\}\}">n</figure-callout>}+1}+\&CenterDot;\&CenterDot;\&CenterDot;+{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="H2009100732329E00022.png"\; state="\{\{state\}\}">y</figure-callout>}}_0+{\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="H2009100732329E00031.png"\; state="\{\{state\}\}">y</figure-callout>}}_1+\&CenterDot;\&CenterDot;\&CenterDot;+y_{i+n-2}+y_{i+n-1}}{n}---\left(17\right).$

Claims (3)

1. mooring and drift estimating method based on single-shaft rotation-stop scheme is characterized in that may further comprise the steps:

(1) determines the initial position parameters of carrier by GPS, and bind to navigational computer;

(2) strapdown inertial navitation system (SINS) is carried out preheating and is prepared, and gathers the data of fibre optic gyroscope and quartz accelerometer output and data are handled;

(3) IMU adopts 8 commentaries on classics to stop the transposition scheme that order is a swing circle;

(4) determine the strapdown matrix according to gyrostatic output under the rotation status

And the output of degree of will speed up meter is transformed into the mathematical platform coordinate system;

Model is floated in estimating when (5) setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system, the bearer rate v that provides with GPS ₀Be benchmark, and with the difference of inertial navigation system clearing speed as observed quantity, utilize Kalman Filter Technology to realize that inertia device often is worth the estimation of deviation;

(6) inertia device that Kalman Filter Estimation is gone out often is worth deviation and adopts the method for average filter it to be carried out smoothly the influence of filtering fluctuation within a narrow range;

It is that the transposition scheme of a swing circle is that described IMU adopts 8 commentaries on classics to stop order: order 1, IMU clockwise rotates 180 ° of in-position C, stand-by time T from the A point _tOrder 2, IMU clockwise rotates 90 ° of in-position D, stand-by time T from the C point _tOrder 3, IMU rotates counterclockwise 180 ° of in-position B, stand-by time T from the D point _tOrder 4, IMU rotates counterclockwise 90 ° of in-position A, stand-by time T from the B point _tOrder 5, IMU rotates counterclockwise 180 ° of in-position C, stand-by time T from the A point _tOrder 6, IMU rotates counterclockwise 90 ° of in-position B, stand-by time T from the C point _tOrder 7, IMU clockwise rotates 180 ° of in-position D, stand-by time T from the B point _tOrder 8, IMU clockwise rotates 90 ° of in-position A, stand-by time T from the D point _tIMU rotates sequential loop according to this to carry out; IMU pause point p3, p8 and p4, p7 on the wherein horizontal east orientation axle are symmetrical in the rotating shaft center; Pause point p1 on the north orientation axle, p5 and p2, p6 are symmetrical in the rotating shaft center.

2. a kind of mooring and drift estimating method based on single-shaft rotation-stop scheme according to claim 1 is characterized in that describedly determining the strapdown matrix according to gyrostatic output under the rotation status

And the method that the output of degree of will speed up meter is transformed into the mathematical platform coordinate system is:

According to the quadravalence runge kutta method, the application hypercomplex number is carried out real-time update to the strapdown matrix of inertial navigation system, and its formulate is:

${\stackrel{\&CenterDot;}{T}}_s^p=T_s^p[\&times;{\mathrm{\&omega;}}_{\mathrm{is}}^s]$

Wherein

The output of expression gyroscope; Utilize the transformational relation of IMU coordinate system and mathematical platform coordinate system, calculate the accekeration under the mathematical platform coordinate system,

3. a kind of mooring and drift estimating method based on single-shaft rotation-stop scheme according to claim 2 is characterized in that described estimating when setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system floats model, the bearer rate v that provides with GPS ₀Be benchmark, and with the difference of inertial navigation system clearing speed as observed quantity, utilize Kalman Filter Technology to realize that the estimation approach of inertia device deviation is:

Adopt carrier of separating error of position information model to float model as estimating of system, foundation is that the Kalman filtering state equation and the velocity error of state variable is the measurement equation of measurement amount with the velocity error;

1) set up the state equation of Kalman filtering:

The state error of rotation strapdown inertial navitation system (SINS) is described with linear first-order differential equation:

$\stackrel{\&CenterDot;}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)$

The state vector of etching system when wherein X (t) is t; A (t) and B (t) are respectively the state matrix and the noise matrix of system;

The state vector of system is:

The white noise vector of system is:

$W={\left[\begin{array}{cccccccccc}{a}_x& a_y& {\mathrm{\&omega;}}_x& {\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_z& 0& 0& 0& 0& 0\end{array}\right]}^T$

δ V wherein _e, δ V _nThe velocity error of representing east orientation, north orientation respectively;

Be respectively IMU coordinate system ox _s, oy _sAxis accelerometer zero partially; ε _x, ε _y, ε _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant value drift of axle gyro; a _x, a _yBe respectively IMU coordinate system ox _s, oy _sThe white noise error of axis accelerometer; ω _x, ω _y, ω _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe white noise error of axle gyro;

The state matrix of system is:

$A\left(t\right)=\left[\begin{array}{cc}{F}_{5\&times;5}& T_{5\&times;5}\\ 0_{5\&times;5}& 0_{5\&times;5}\end{array}\right]$

Wherein: $F_{5\&times;5}=\left[\begin{array}{cc}{F}_{2\&times;2}& F_{2\&times;3}\\ F_{3\&times;2}& F_{3\&times;3}\end{array}\right]$

$F_{2\&times;2}=\left[\begin{array}{cc}\frac{V_n\tan L}{R_n}& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}\\ -2({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0\end{array}\right]$

$F_{2\&times;3}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\end{array}\right]$

$F_{3\&times;2}=\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right]$

$F_{3\&times;3}=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0& -\frac{\mathrm{Vn}}{R_m}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n}& \frac{V_n}{R_m}& 0\end{array}\right]$

V _e, V _nThe speed of representing east orientation, north orientation respectively; ω _IeThe expression rotational-angular velocity of the earth; R _m, R _nRepresent earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches respectively; L represents local latitude; f _e, f _n, f _uBe expressed as respectively navigation coordinate system down east orientation, north orientation, day to specific force;

The strapdown matrix

For:

$T_s^p=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

Then

$T_{5\&times;5}=\left[\begin{array}{ccccc}{T}_{11}& T_{12}& 0& 0& 0\\ T_{21}& T_{22}& 0& 0& 0\\ 0& 0& -T_{11}& -T_{12}& -T_{13}\\ 0& 0& -T_{21}& -T_{22}& -T_{23}\\ 0& 0& -T_{31}& -T_{32}& -T_{33}\end{array}\right];$

2) set up the measurement equation of Kalman filtering:

The measurement equation of describing the rotation strapdown inertial navitation system (SINS) with linear first-order differential equation is as follows:

Z(t)＝H(t)X(t)+v(t)

Wherein: the measurement vector of etching system during Z (t) expression t; The measurement matrix of H (t) expression system;

The system measurements matrix is:

$H\left(t\right)=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

Amount is measured as the horizontal velocity error:

v(t)＝[v _e?v _n] ^T。

Images