CN102768043B - Integrated attitude determination method without external observed quantity for modulated strapdown system - Google Patents

Abstract

The invention provides an integrated attitude determination method without external observed quantity for a modulated strapdown system. The method comprises the steps of: determining initial position parameters of a carrier by a global positioning system (GPS) and binding the parameters to a navigation computer; acquiring data output by a fiber optic gyros and a quartz accelerometer, and processing the data; setting an inertial measurement unit (IMU) single shaft four position rotation stopping scheme; converting the accelerometer output to a carrier semi-fixed coordinate system; designing an infinite impulse response (IIR) digital high-pass filter, and carrying out filtering on the carrier velocity resolved under a navigation system; subtracting the filtered velocity from a velocity calculated by the modulated strapdown system, and using the difference as a system observed quantity; and estimating attitude information of the modulated strapdown inertial navigation system by a Kalman filtering technique. The method provided by the invention does not need external auxiliary equipment to provide observation information, and can effectively solve the problem of mismatching between the frequency of information supply by the auxiliary equipment and the frequency of cultivation of the modulated strapdown system, and realize integrated attitude determination of the modulated strapdown inertial navigation system.

Description

A kind of modulation type strapdown system assembled gesture defining method without semblance measure

(1) technical field

What the present invention relates to is a kind of measuring method, in particular a kind of modulation type strapdown system assembled gesture defining method without semblance measure.

(2) background technology

In strapdown inertial navigation system, all inertial measurement component are directly installed on carrier, what inertance element exported is exactly carrier relative to the angular velocity of inertial space and acceleration, by computing machine, the acceleration information recorded under carrier coordinate system is transformed into navigational coordinate system and carries out navigation calculation again, be equivalent to utilize gyroscope to export data in computing machine, build the reference of a mathematical platform as navigation calculating.Because strapdown system does not have platform framework and connected servo control mechanism, thus simplify hardware, have compared with Platform INS that volume is little, lightweight, cost is low, Reliability comparotive advantages of higher.Just because of above advantage, it is widely applied at Aeronautics and Astronautics, navigation and a lot of civil area.

Rotation modulation technology is a kind of automatic correcting method of inertial navigation system.Modulation type strapdown inertial navigation system adds rotating mechanism and angle-measuring equipment in the outside of strapdown inertial navitation system (SINS), and navigation calculation still adopts inertial navigation algorithm.It does not need to introduce external calibration information, can automatically be averaged to the constant value deviation of inertia device in system, reaches and offsets the impact of drift on system accuracy.Thus can improve the precision that inertial navigation system works long hours, give full play to the advantage of inertial navigation " autonomous type ".Application rotation modulation technology, the inertia device of lower accuracy can also be applied, form the inertial navigation system of degree of precision, be conducive to the cost reducing inertial navigation system, effectively can improve the observability degree of inertial navigation system partial parameters simultaneously owing to introducing extraneous motion.

Initial attitude error is one of main error source of inertial navigation system, and the error of initial attitude not only shows in attitude index on the impact of systematic error, and shows in the acquisition of speed and positional information.The precision that initial attitude is determined directly affects the precision of navigation.Divide by the motion state of pedestal, the initial attitude determination method of inertial navigation system can be divided into two classes, and namely quiet pedestal initial attitude is determined to determine with moving base initial attitude.So-called quiet pedestal initial attitude is determined to refer to that IMU determines the initial state information of carrier when carrier is completely static.At present, this technology comparative maturity, can reach quite high precision by Multiple station method and Kalman filtering.But quiet pedestal initial attitude determination technology can not adapt to their rapid-action requirements, so the emphasis of current research mainly concentrates on moving base initial attitude determination method for numerous airborne and shipborne weapons system.So-called moving base initial attitude is determined to refer to that IMU completes when carrier movement or external disturbance.Its difficult point is to construct fast, the wave filter of stable, strong robustness, and evaluates the observability of filter status and observability degree, thus devise optimum wave filter.According to the situation using external information, moving base initial attitude determination method can be divided three classes: out-damping formula, delivery type and external auxiliary attitude information formula.

The function of wave filter is exactly allow the signal of a certain component frequency to pass through smoothly, and the signal of other a part of frequency is then subject to larger suppression, and it is in fact a frequency selection circuit.Infinite impulse response (IIR) wave filter belongs to classical filter device, namely supposes that the useful component in input signal occupies different frequency bands separately with the composition of hope removing.This supposition has met objective law to a certain extent, and for Hi-pass filter, the HFS of sample sequence comprises useful signal, and low frequency part is then mainly controlled by Schuler cycle oscillation.Therefore by Hi-pass filter reasonable in design, the object removing Schuler cycle oscillation can just be reached.

(3) summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiencies in the prior art, provides a kind of modulation type strapdown system assembled gesture defining method without semblance measure.

Technical solution of the present invention is: a kind of modulation type strapdown system assembled gesture defining method without semblance measure, it is characterized in that adopting Inertial Measurement Unit uniaxial four-position rotation and stop, the extracting method of carrier instantaneous velocity is proposed, according to the error characteristics of instantaneous velocity, adopt the Schuler period in infinite impulse response (IIR) digital high-pass filter filtering bearer rate, as the observed quantity of system after the velocity information work difference that filtered velocity information and inertial reference calculation are gone out, Kalman Filter Technology is adopted to realize the assembled gesture of strapdown inertial navitation system (SINS), its concrete steps are as follows:

(1) initial position parameters of carrier is determined by GPS, by their bookbindings in navigational computer;

(2) modulation type strapdown inertial navitation system (SINS) carries out preheating preparation, gathers the data of fibre optic gyroscope and quartz accelerometer output and processes data;

(3) IMU adopts 4 turns to stop the transposition scheme (as accompanying drawing 2) that order is a swing circle;

Order 1, IMU, by the A of position, turns clockwise 180 ° to position B, and at position B residence time T _s; Order 2, IMU is by the B of position, and turn clockwise 90 ° of in-position C, and at position C residence time T _s; Order 3, IMU, by the C of position, is rotated counterclockwise 180 ° to position D, at position D residence time T _s; Order 4, IMU is rotated counterclockwise 90 ° and gets back to position A by the D of position, and at position A residence time T _s; Then move according to the sequential loop of order 1 ~ 4.

IMU rotates 180 ° at every turn or 90 ° of intervals are carried out.Rotate 180 ° to symmetric position from a position, on these two symmetrical positions, in horizontal direction, the constant value drift of inertia sensitive element can be cancelled out each other when carrying out navigation and calculating.Another one reposition is arrived by half-twist.

(4) output of degree of will speed up meter is transformed into carrier semi-fixed axes system, utilizes the integral element in modulation type strapdown inertial navitation system (SINS) to extract carrier instantaneous linear velocity information;

1) carrier semi-fixed axes system is introduced

With naval vessel center of gravity for carrier semi-fixed axes system initial point, longitudinal axis OY _dpoint to the main row on naval vessel to direction, transverse axis OX _dperpendicular to longitudinal axis in surface level, point to starboard direction on naval vessel without during pitching.Z-axis OZ _dvertical with front diaxon, just (as accompanying drawing 3) be upwards along ship vertical pivot.Wherein ψ _gfor base course angle, γ angle is course angle of oscillation (namely yaw angle defines itself and base course angle in the same way for just).The introducing of carrier semi-fixed axes system makes measurement result and angular motion substantially depart from, and can describe the instantaneous line motion on naval vessel exactly, therefore adopts carrier semi-fixed axes system as the instantaneous line motion in research naval vessel or the base coordinate system being called translation.

2) transition matrix of Inertial Measurement Unit coordinate system and carrier semi-fixed axes system is set up

First set up between carrier coordinate system with carrier semi-fixed axes system and differ three rotation angle, can be considered that semi-fixed axes system overlaps with carrier coordinate system after rotating through three times, three angles are respectively: pitch angle α, roll angle β and yaw angle γ (as accompanying drawing 4).Carrier coordinate system (b system) is transformed into the direction cosine matrix of carrier semi-fixed axes system (d system)

$C_b^d=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ -\sin \mathrm{\γ}\cos \mathrm{\β}& \cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\γ}\sin \mathrm{\α}-\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ \sin \mathrm{\β}& -\cos \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\β}\cos \mathrm{\α}\end{array}\right]$

Existing inertial navigation system can provide comparatively accurate attitude angle information, wherein level two attitude angle information are pitch angle information and roll angle information, and course information provides course angle information ψ, this is different from yaw angle γ, but the operator on naval vessel can provide base course information ψ accurately _g, can obtain

γ＝ψ-ψ _G

γ is substituted into accounting equation in, the direction cosine matrix that carrier coordinate system is transformed into carrier semi-fixed axes system can be obtained.

Because Inertial Measurement Unit opposite carrier exists the indexing motion around azimuth axis, the transition matrix therefore between inertia measurement coordinate system (s coordinate system) and carrier coordinate system can utilize following formula to calculate:

$C_s^b=\left[\begin{array}{ccc}\cos \mathrm{\ωt}& -\sin \mathrm{\ωt}& 0\\ \sin \mathrm{\ωt}& \cos \mathrm{\ωt}& 0\\ 0& 0& 1\end{array}\right]$

In formula, ω t represents the relative angular relationship of Inertial Measurement Unit opposite carrier coordinate system.Therefore the direction cosine matrix of Inertial Measurement Unit ordinate transform to carrier semi-fixed axes system can be obtained:

$C_s^d=C_b^d{C}_s^b$

(5) infinite impulse response digital high-pass filter (IIR) reasonable in design, carries out high-pass filtering process by the bearer rate calculated under navigation system;

1) technical indicator of designed digital high-pass filter is determined

High-pass digital filter f _p1, f _s1, δ _p, δ _stechnical indicator be according to signal characteristic and sample frequency f _sgiven.Wherein, f _p1for cut-off frequecy of passband, f _s1for stopband cutoff frequency, δ _pfor passband ripple, namely depart from the maximal value of unity gain in filter transmission band, passband edge gain is 1-δ _p, δ _sfor stopband ripple, namely depart from the maximal value of unity gain in filter stop band, the gain of stopband edge place wave filter is δ _s.The attenuation alpha of passband and stopband _p, α _sbe defined as-20log (1-δ respectively _p) ,-20log (1-δ _s).

Schuler cycle oscillation signal belongs to low frequency signal comparatively speaking, and oscillation period is 84.4 minutes.And the motion of the instantaneous line in naval vessel is caused by Marine Environment Factors, topmost producing cause is the impact of wave, so the instantaneous line in the naval vessel motion to-and-fro movement that to be frequency and wave frequencies unanimous on the whole.And the instantaneous line motion in naval vessel is relative to the navigation campaign on naval vessel, belong to high frequency motion, the period of motion is shorter, and generally at 1.5 seconds ~ about 10 seconds, frequency is 0.67 hertz.According to the difference in the kinetic characteristic of heave swaying surge motion and naval vessel routine work campaign, the technical requirement of the wave filter that design is wanted, specific design index adjusts according to filter effect in process of the test, is as the criterion to reach optimal filtering effect.

2) technical indicator is transformed into analog filter from digital filter

The conversion of technical indicator from analog filter to digital filter adopts bilinearity transform method, and the technical indicator of design digital high-pass filter is f _p1, f _s1, δ _p, δ _s, t _s=0.0102.First digital edge frequency omega should be obtained, because 2 π are corresponding sample frequency f _s, and f _s=1/t _s, so have:

$\frac{f_{p1}}{f_s}=\frac{{\mathrm{\Ω}}_{p1}}{2\mathrm{\π}}$

$\frac{f_{s1}}{f_s}=\frac{{\mathrm{\Ω}}_{s1}}{2\mathrm{\π}}$

Institute in the hope of:

Ω _s1＝2πf _s1/f _s

Ω _p1＝2πf _p1/f _s

According to the frequency inverted relation ω=2f of bilinearity transform method _stan (Ω/2) continues conversion to be had:

${\mathrm{\Ω}}_p=\tan \left(\frac{{\mathrm{\ω}}_p}{2}\right)$

${\mathrm{\Ω}}_s=\tan \left(\frac{{\mathrm{\ω}}_s}{2}\right)$

With this, technical indicator of digital high-pass filter is just converted into the technical indicator of mimic high pass filter.

(6) assembled gesture error model when setting up carrier moored condition according to modulation type strapdown inertial navitation system (SINS) moving base error equation, the speed that the speed obtained after high-pass filtering and modulation type strapdown inertial navitation system (SINS) Directly solution calculate is measured as systematic perspective after making difference.Kalman Filter Technology is utilized to realize the determination of modulation type strapdown inertial navitation system (SINS) assembled gesture;

As the Kalman filter model of observed quantity after foundation makes difference using the speed that the horizontal velocity after high-pass filtering and modulation type strapdown inertial navitation system (SINS) Directly solution calculate;

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

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW}$

Wherein, X is the state vector of system; A and B is respectively state matrix and the noise matrix of system; W is system noise vector;

The state vector of system is:

The white noise vector of system is:

W＝[a _xa _yω _xω _yω _z0 0 0 0 0] ^T

Wherein δ V _e, δ V _nrepresent the velocity error of east orientation, north orientation respectively; be respectively IMU coordinate system ox _s, oy _saxis accelerometer zero is inclined; ε _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[\begin{array}{cccc}{F}_{2\×2}^1& f_{2\×3}& {\tilde{T}}_{2\×2}& O_{2\×3}\\ F_{3\×2}^2& F_{3\×3}^3& O_{3\×2}& T_{3\×3}\\ O_{5\×2}& O_{5\×3}& O_{5\×2}& O_{5\×3}\end{array}\right]$

$F_{2\×2}^1=\left[\begin{array}{cc}\frac{V_N}{R_n}\tan L^{\′}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_n}\tan L\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{2V_E}{R_n}\tan L)& 0\end{array}\right]$

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

$F_{3\×3}^3=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n})& 0& -\frac{V_N}{R_m}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n}& \frac{V_N}{R_m}& 0\end{array}\right]$

$f_{2\×3}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& f_E\end{array}\right]$

${\tilde{T}}_{2\×2}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]$

$T_{3\×6}=\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]$

V _e, V _nrepresent the speed of east orientation, north orientation respectively; ω _x, ω _y, ω _zrepresent three input angular velocities of gyro respectively; ω _ierepresent 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; L ' expression moored condition initial time carrier latitude information; f _e, f _n, f _ube expressed as east orientation under navigational coordinate system, north orientation, sky to specific force.

2) measurement equation of Kalman filtering is set up:

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

Z＝HX+V

Wherein: Z represents the measurement vector of system; H represents the measurement matrix of system; V represents the measurement noise of system;

System measurements matrix is:

$H=\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 east orientation speed V that modulation type strapdown inertial navitation system (SINS) is resolved _e, north orientation speed V _nthe east orientation speed obtained respectively and through high-pass filtering process north orientation speed difference:

$Z=\left[\begin{array}{c}{V}_E-{\tilde{V}}_E\\ V_N-{\tilde{V}}_N\end{array}\right]$

The present invention's advantage is compared with prior art: the present invention has broken modulation type strapdown inertial navitation system (SINS) assembled gesture deterministic process peripheral equipment provides information frequency and modulation type strapdown inertial navitation system (SINS) to provide information frequency not mate this problem, in modulation type strapdown inertial navitation system (SINS) under Inertial Measurement Unit four-position rotation and stop scheme, utilize this characteristic of high frequency components information of waving and swinging motion of Schuler period information and the carrier moored condition existence that there is low frequency in carrier instantaneous velocity information, the Schuler period item utilized in IIR digital high-pass filter filtering carrier instantaneous velocity information is proposed, filtered speed and inertial reference calculation speed are done the observed quantity as system after difference, Kalman Filter Technology is adopted to realize the assembled gesture of strapdown inertial navitation system (SINS).Therefore external unit is not needed to provide reference information for it.

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

Under VC++ simulated conditions, emulation experiment is carried out to the method:

Carrier does three-axis swinging motion.Carrier waves around pitch axis, axis of roll and course axle with sinusoidal rule, and its mathematical model is:

$\left\{\begin{array}{c}\mathrm{\θ}={\mathrm{\θ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\θ}}t+{\mathrm{\φ}}_{\mathrm{\θ}})\\ \mathrm{\γ}={\mathrm{\γ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\γ}}t+{\mathrm{\φ}}_{\mathrm{\γ}})\\ \mathrm{\ψ}={\mathrm{\ψ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\ψ}}t+{\mathrm{\φ}}_{\mathrm{\ψ}})+k\end{array}\right.$

Wherein: θ, γ, ψ represent the swing angle variable of pitch angle, roll angle and course angle respectively; θ _m, γ _m, ψ _mrepresent corresponding swing angle amplitude 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 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 caused and be:

In formula, i=x, y, z be the east orientation of geographic coordinate system, north orientation, sky to. equally distributed random phase is obeyed for [0,2 π] is upper.

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

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

Equatorial radius: R _e=6378393.0m;

Ellipsoid degree: e=3.367e-3;

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;

Gyro drift: 0.01 degree/hour;

Gyroscope random walk:

Accelerometer bias: 10 ^-4g ₀;

Accelerometer noise: 10 ^-6g ₀;

Constant: π=3.1415926;

The mathematical model parameter of IMU four-position rotation and stop scheme:

The dead time of four positions: T _s=5min;

The time consumed when rotating 180 ° and 90 °: T _z=12s;

Rotate in the process of 180 ° and 90 °, the Acceleration and deceleration time in each transposition is respectively 4s;

Method of the present invention is utilized to obtain modulation type strapdown system misalignment curve, as shown in Figure 5.Under result shows to wave disturbed condition, adopt the inventive method can obtain higher alignment precision.

(4) accompanying drawing explanation

Fig. 1 is a kind of modulation type strapdown system assembled gesture defining method process flow diagram without semblance measure of the present invention;

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

Fig. 3 is definition carrier semi-fixed axes system of the present invention;

Fig. 4 is the transformational relation of carrier coordinate system of the present invention and carrier semi-fixed axes system;

Fig. 5 is the misalignment curve of the modulation type strapdown system Kalman Filter Estimation without semblance measure of the present invention.

(5) embodiment

Below in conjunction with accompanying drawing, the specific embodiment of the present invention is described in detail:

(1) initial position parameters of carrier is determined by GPS, by their bookbindings in navigational computer;

(2) modulation type strapdown inertial navitation system (SINS) carries out preheating preparation, gathers the data of fibre optic gyroscope and quartz accelerometer output and processes data;

(3) IMU adopts 4 turns to stop the transposition scheme (as accompanying drawing 2) that order is a swing circle;

Order 1, IMU, by the A of position, turns clockwise 180 ° to position B, and at position B residence time T _s; Order 2, IMU is by the B of position, and turn clockwise 90 ° of in-position C, and at position C residence time T _s; Order 3, IMU, by the C of position, is rotated counterclockwise 180 ° to position D, at position D residence time T _s; Order 4, IMU is rotated counterclockwise 90 ° and gets back to position A by the D of position, and at position A residence time T _s; Then move according to the sequential loop of order 1 ~ 4.

IMU rotates 180 ° at every turn or 90 ° of intervals are carried out.Rotate 180 ° to symmetric position from a position, on these two symmetrical positions, in horizontal direction, the constant value drift of inertia sensitive element can be cancelled out each other when carrying out navigation and calculating.Another one reposition is arrived by half-twist.

(4) output of degree of will speed up meter is transformed into carrier semi-fixed axes system, utilizes the integral element in modulation type strapdown inertial navitation system (SINS) to extract carrier instantaneous linear velocity information;

1) carrier semi-fixed axes system is introduced

With naval vessel center of gravity for carrier semi-fixed axes system initial point, longitudinal axis OY _dpoint to the main row on naval vessel to direction, transverse axis OX _dperpendicular to longitudinal axis in surface level, point to starboard direction on naval vessel without during pitching.Z-axis OZ _dvertical with front diaxon, just (as accompanying drawing 3) be upwards along ship vertical pivot.Wherein ψ _gfor base course angle, γ angle is course angle of oscillation (namely yaw angle defines itself and base course angle in the same way for just).The introducing of carrier semi-fixed axes system makes measurement result and angular motion substantially depart from, and can describe the instantaneous line motion on naval vessel exactly, therefore adopts carrier semi-fixed axes system as the instantaneous line motion in research naval vessel or the base coordinate system being called translation.

2) transition matrix of Inertial Measurement Unit coordinate system and carrier semi-fixed axes system is set up

First set up between carrier coordinate system with carrier semi-fixed axes system and differ three rotation angle, can be considered that semi-fixed axes system overlaps with carrier coordinate system after rotating through three times, three angles are respectively: pitch angle α, roll angle β and yaw angle γ (as accompanying drawing 4).Carrier coordinate system (b system) is transformed into the direction cosine matrix of carrier semi-fixed axes system (d system)

$C_b^d=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ -\sin \mathrm{\γ}\cos \mathrm{\β}& \cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\γ}\sin \mathrm{\α}-\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ \sin \mathrm{\β}& -\cos \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\β}\cos \mathrm{\α}\end{array}\right]---\left(1\right)$

Existing inertial navigation system can provide comparatively accurate attitude angle information, wherein level two attitude angle information are pitch angle information and roll angle information, and course information provides course angle information ψ, this is different from yaw angle γ, but the operator on naval vessel can provide base course information ψ accurately _g, can obtain

γ＝ψ-ψ _G(2)

γ is substituted into accounting equation in, the direction cosine matrix that carrier coordinate system is transformed into carrier semi-fixed axes system can be obtained.

Because Inertial Measurement Unit opposite carrier exists the indexing motion around azimuth axis, the transition matrix therefore between inertia measurement coordinate system (s coordinate system) and carrier coordinate system can utilize following formula to calculate:

$C_s^b=\left[\begin{array}{ccc}\cos \mathrm{\ωt}& -\sin \mathrm{\ωt}& 0\\ \sin \mathrm{\ωt}& \cos \mathrm{\ωt}& 0\\ 0& 0& 1\end{array}\right]---\left(3\right)$

In formula, ω t represents the relative angular relationship of Inertial Measurement Unit opposite carrier coordinate system.Therefore the direction cosine matrix of Inertial Measurement Unit ordinate transform to carrier semi-fixed axes system can be obtained:

$C_s^d=C_b^d{C}_s^b---\left(4\right)$

(5) infinite impulse response digital high-pass filter (IIR) reasonable in design, carries out high-pass filtering process by the bearer rate calculated under navigation system;

1) technical indicator of designed digital high-pass filter is determined

High-pass digital filter f _p1, f _s1, δ _p, δ _stechnical indicator be according to signal characteristic and sample frequency f _sgiven.Wherein, f _p1for cut-off frequecy of passband, f _s1for stopband cutoff frequency, δ _pfor passband ripple, namely depart from the maximal value of unity gain in filter transmission band, passband edge gain is 1-δ _p, δ _sfor stopband ripple, namely depart from the maximal value of unity gain in filter stop band, the gain of stopband edge place wave filter is δ _s.The attenuation alpha of passband and stopband _p, α _sbe defined as-20log (1-δ respectively _p) ,-20log (1-δ _s).

Schuler cycle oscillation signal belongs to low frequency signal comparatively speaking, and oscillation period is 84.4 minutes.And the motion of the instantaneous line in naval vessel is caused by Marine Environment Factors, topmost producing cause is the impact of wave, so the instantaneous line in the naval vessel motion to-and-fro movement that to be frequency and wave frequencies unanimous on the whole.And the instantaneous line motion in naval vessel is relative to the navigation campaign on naval vessel, belong to high frequency motion, the period of motion is shorter, and generally at 1.5 seconds ~ about 10 seconds, frequency is 0.67 hertz.According to the difference in the kinetic characteristic of heave swaying surge motion and naval vessel routine work campaign, the technical requirement of the wave filter that design is wanted, specific design index adjusts according to filter effect in process of the test, is as the criterion to reach optimal filtering effect.

2) technical indicator is transformed into analog filter from digital filter

The conversion of technical indicator from analog filter to digital filter adopts bilinearity transform method, and the technical indicator of design digital high-pass filter is f _p1, f _s1, δ _p, δ _s, t _s=0.0102.First digital edge frequency omega should be obtained, because 2 π are corresponding sample frequency f _s, and f _s=1/t _s, so have:

$\frac{f_{p1}}{f_s}=\frac{{\mathrm{\Ω}}_{p1}}{2\mathrm{\π}}$ (5)

$\frac{f_{s1}}{f_s}=\frac{{\mathrm{\Ω}}_{s1}}{2\mathrm{\π}}$

Institute in the hope of:

Ω _s1＝2πf _s1/f _s(6)

Ω _p1＝2πf _p1/f _s

According to the frequency inverted relation ω=2f of bilinearity transform method _stan (Ω/2) continues to be converted to:

${\mathrm{\Ω}}_p=\tan \left(\frac{{\mathrm{\ω}}_p}{2}\right)$ (7)

${\mathrm{\Ω}}_s=\tan \left(\frac{{\mathrm{\ω}}_s}{2}\right)$

With this, technical indicator of digital high-pass filter is just converted into the technical indicator of mimic high pass filter.

(6) assembled gesture error model when setting up carrier moored condition according to modulation type strapdown inertial navitation system (SINS) moving base error equation, the speed that the speed obtained after high-pass filtering and modulation type strapdown inertial navitation system (SINS) Directly solution calculate is measured as systematic perspective after making difference.Kalman Filter Technology is utilized to realize the determination of modulation type strapdown inertial navitation system (SINS) assembled gesture;

As the Kalman filter model of observed quantity after foundation makes difference using the speed that the horizontal velocity after high-pass filtering and modulation type strapdown inertial navitation system (SINS) Directly solution calculate;

1) state equation of Kalman filtering is set up:

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

$\stackrel{\·}{X}=\mathrm{AX}+\mathrm{BW}---\left(8\right)$

Wherein, X is the state vector of system; A and B is respectively state matrix and the noise matrix of system; W is system noise vector;

The state vector of system is:

The white noise vector of system is:

W＝[a _xa _yω _xω _yω _z0 0 0 0 0] ^T(10)

Wherein δ V _e, δ V _nrepresent the velocity error of east orientation, north orientation respectively; be respectively IMU coordinate system ox _s, oy _saxis accelerometer zero is inclined; ε _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[\begin{array}{cccc}{F}_{2\×2}^1& f_{2\×3}& {\tilde{T}}_{2\×2}& O_{2\×3}\\ F_{3\×2}^2& F_{3\×3}^3& O_{3\×2}& T_{3\×3}\\ O_{5\×2}& O_{5\×3}& O_{5\×2}& O_{5\×3}\end{array}\right]---\left(11\right)$

$F_{2\×2}^1=\left[\begin{array}{cc}\frac{V_N}{R_n}\tan L^{\′}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_n}\tan L\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{2V_E}{R_n}\tan L)& 0\end{array}\right]---\left(12\right)$

$F_{3\×2}^2=\begin{array}{c}\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right]---\left(13\right)\end{array}$

$F_{3\×3}^3=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_n})& 0& -\frac{V_N}{R_m}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_n}& \frac{V_N}{R_m}& 0\end{array}\right]---\left(14\right)$

$f_{2\×3}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& f_E\end{array}\right]---\left(15\right)$

${\tilde{T}}_{2\×2}=\left[\begin{array}{cc}{T}_{11}& T_{12}\\ T_{21}& T_{22}\end{array}\right]---\left(16\right)$

$T_{3\×6}=\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(17\right)$

V _e, V _nrepresent the speed of east orientation, north orientation respectively; ω _x, ω _y, ω _zrepresent three input angular velocities of gyro respectively; ω _ierepresent 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; L ' expression moored condition initial time carrier latitude information; f _e, f _n, f _ube expressed as east orientation under navigational coordinate system, north orientation, sky to specific force.

2) measurement equation of Kalman filtering is set up:

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

Z＝HX+V (18)

Wherein: Z represents that the measurement vector H of system represents the measurement matrix of system; V represents the measurement noise of system;

System measurements matrix is:

$H=\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(19\right)$

Amount is measured as the east orientation speed V that modulation type strapdown inertial navitation system (SINS) is resolved _e, north orientation speed V _nthe east orientation speed obtained respectively and through high-pass filtering process north orientation speed difference:

$Z=\left[\begin{array}{c}{V}_E-{\tilde{V}}_E\\ V_N-{\tilde{V}}_N\end{array}\right]---\left(20\right)$

Claims (1)

1., without a modulation type strapdown system assembled gesture defining method for semblance measure, it is characterized in that comprising the following steps:

(1) initial position parameters of carrier is determined by GPS, by their bookbindings in navigational computer;

(2) modulation type strapdown inertial navitation system (SINS) carries out preheating preparation, gathers the data of fibre optic gyroscope and quartz accelerometer output and processes data;

(3) IMU adopts 4 turns to stop the transposition scheme that order is a swing circle;

Order 1, IMU, by the A of position, turns clockwise 180 ° to position B, and at position B residence time T _s; Order 2, IMU is by the B of position, and turn clockwise 90 ° of in-position C, and at position C residence time T _s; Order 3, IMU, by the C of position, is rotated counterclockwise 180 ° to position D, at position D residence time T _s; Order 4, IMU is rotated counterclockwise 90 ° and gets back to position A by the D of position, and at position A residence time T _s; Then move according to the sequential loop of order 1 ~ 4;

IMU rotates 180 ° at every turn or 90 ° of intervals are carried out, and rotates 180 ° to symmetric position from a position, and on these two symmetrical positions, in horizontal direction, the constant value drift of inertia sensitive element can be cancelled out each other when carrying out navigation and calculating; Another one reposition is arrived by half-twist;

(4) output of degree of will speed up meter is transformed into carrier semi-fixed axes system, utilizes the integral element in modulation type strapdown inertial navitation system (SINS) to extract carrier instantaneous linear velocity information;

1) carrier semi-fixed axes system is introduced

With naval vessel center of gravity for carrier semi-fixed axes system initial point, longitudinal axis OY _dpoint to the main row on naval vessel to direction, transverse axis OX _dperpendicular to longitudinal axis in surface level, point to starboard direction on naval vessel without during pitching; Z-axis OZ _dvertical with front diaxon, be just upwards along ship vertical pivot; Wherein ψ _gfor base course angle, γ angle is that course angle of oscillation and yaw angle define it and base course angle is just in the same way; The introducing of carrier semi-fixed axes system makes measurement result and angular motion substantially depart from, and can describe the instantaneous line motion on naval vessel exactly, therefore adopts carrier semi-fixed axes system as the instantaneous line motion in research naval vessel or the base coordinate system being called translation;

2) transition matrix of Inertial Measurement Unit coordinate system and carrier semi-fixed axes system is set up

First set up between carrier coordinate system with carrier semi-fixed axes system and differ three rotation angle, can be considered that semi-fixed axes system overlaps with carrier coordinate system after rotating through three times, three angles are respectively: pitch angle α, roll angle β and yaw angle γ; Carrier coordinate system b system is transformed into the direction cosine matrix of d system of carrier semi-fixed axes system

Existing inertial navigation system can provide comparatively accurate attitude angle information, wherein level two attitude angle information are pitch angle information and roll angle information, and course information provides course angle information ψ, this is different from yaw angle γ, but the operator on naval vessel can provide base course information ψ accurately _g, can obtain

γ＝ψ-ψ _G

γ is substituted into accounting equation in, the direction cosine matrix that carrier coordinate system is transformed into carrier semi-fixed axes system can be obtained;

Because Inertial Measurement Unit opposite carrier exists the indexing motion around azimuth axis, the transition matrix therefore between inertia measurement coordinate system s coordinate system and carrier coordinate system can utilize following formula to calculate:

In formula, ω t represents the relative angular relationship of Inertial Measurement Unit opposite carrier coordinate system; Therefore the direction cosine matrix of Inertial Measurement Unit ordinate transform to carrier semi-fixed axes system can be obtained:

(5) infinite impulse response digital high-pass filter IIR reasonable in design, carries out high-pass filtering process by the bearer rate calculated under navigation system;

1) technical indicator of designed digital high-pass filter is determined

High-pass digital filter f _p1, f _s1, δ _p, δ _stechnical indicator be according to signal characteristic and sample frequency f _sgiven, wherein, f _p1for cut-off frequecy of passband, f _s1for stopband cutoff frequency, δ _pfor passband ripple, namely depart from the maximal value of unity gain in filter transmission band, passband edge gain is 1-δ _p, δ _sfor stopband ripple, namely depart from the maximal value of unity gain in filter stop band, the gain of stopband edge place wave filter is δ _s, the attenuation alpha of passband and stopband _p, α _sbe defined as-20log (1-δ respectively _p) ,-20log (1-δ _s);

Schuler cycle oscillation signal belongs to low frequency signal comparatively speaking, oscillation period is 84.4 minutes, and the instantaneous line motion in naval vessel is caused by Marine Environment Factors, topmost producing cause is the impact of wave, so the motion of the instantaneous line in naval vessel is the to-and-fro movement unanimous on the whole of frequency and wave frequencies, and the instantaneous line in naval vessel moves relative to the navigation campaign on naval vessel, belong to high frequency motion, the period of motion is shorter, and generally at 1.5 seconds ~ about 10 seconds, frequency is 0.67 hertz; According to the difference in the kinetic characteristic of heave swaying surge motion and naval vessel routine work campaign, the technical requirement of the wave filter that design is wanted, specific design index adjusts according to filter effect in process of the test, is as the criterion to reach optimal filtering effect;

2) technical indicator is transformed into analog filter from digital filter

The conversion of technical indicator from analog filter to digital filter adopts bilinearity transform method, and the technical indicator of design digital high-pass filter is f _p1, f _s1, δ _p, δ _s, t _s=0.0102; First digital edge frequency omega should be obtained, because 2 π are corresponding sample frequency f _s, and f _s=1/t _s, so have:

Institute in the hope of:

Ω _s1＝2πf _s1/f _s

Ω _p1＝2πf _p1/f _s

According to the frequency inverted relation ω=2f of bilinearity transform method _stan (Ω/2) continues conversion to be had:

With this, technical indicator of digital high-pass filter is just converted into the technical indicator of mimic high pass filter;

(6) assembled gesture error model when setting up carrier moored condition according to modulation type strapdown inertial navitation system (SINS) moving base error equation, the speed that the speed obtained after high-pass filtering and modulation type strapdown inertial navitation system (SINS) Directly solution calculate is measured as systematic perspective after making difference; Kalman Filter Technology is utilized to realize the determination of modulation type strapdown inertial navitation system (SINS) assembled gesture;

As the Kalman filter model of observed quantity after foundation makes difference using the speed that the horizontal velocity after high-pass filtering and modulation type strapdown inertial navitation system (SINS) Directly solution calculate;

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

Wherein, X is the state vector of system; A and B is respectively state matrix and the noise matrix of system; W is system noise vector;

The state vector of system is:

The white noise vector of system is:

W＝[a _xa _yω _xω _yω _z0 0 0 0 0] ^T

Wherein δ V _e, δ V _nrepresent the velocity error of east orientation, north orientation respectively; be respectively IMU coordinate system ox _s, oy _saxis accelerometer zero is inclined; ε _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:

V _e, V _nrepresent the speed of east orientation, north orientation respectively; ω _x, ω _y, ω _zrepresent three input angular velocities of gyro respectively; ω _ierepresent 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; L ' expression moored condition initial time carrier latitude information; f _e, f _n, f _ube expressed as east orientation under navigational coordinate system, north orientation, sky to specific force;

2) measurement equation of Kalman filtering is set up:

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

Z＝HX+V

Wherein: Z represents the measurement vector of system; H represents the measurement matrix of system; V represents the measurement noise of system;

System measurements matrix is:

Amount is measured as the east orientation speed V that modulation type strapdown inertial navitation system (SINS) is resolved _e, north orientation speed V _nthe east orientation speed obtained respectively and through high-pass filtering process north orientation speed difference

。