CN103076026A - Method for determining speed measurement error of Doppler velocity log (DVL) in strapdown inertial navigation system - Google Patents

Abstract

The invention provides a method for determining speed measurement error of a Doppler velocity log (DVL) in a strapdown inertial navigation system. The method comprises the following steps: determining an initial position parameter of a carrier by a global positioning system (GPS), acquiring data output by an optical fiber gyro and an accelerator, performing initial alignment on data processing and determining an initial strapdown matrix; acquiring angular motion information and linear motion information of the carrier measured by an inertial component, and performing navigation calculation by a compass method and an inertial navigation method respectively, wherein carrier motion speed information measured by the DVL is introduced into the calculation by the compass method; performing subtraction on two groups of gesture information calculated by the two methods and performing transformation to obtain an azimuth misalignment angle value of the two group of calculated gestures; and performing conversion on the azimuth misalignment angle value to obtain the speed measurement error of the DVL. By the method, the speed measurement error of the DVL can be estimated in the navigation process of the carrier, and the speed measurement precision of the DVL is improved after the result can be compensated to the DVL. The method is simple and easy to operate.

Description

Determine the method for Doppler log range rate error in a kind of strapdown inertial navitation system (SINS)

Technical field

The present invention relates to the navigation calculation technical field of strapdown inertial navitation system (SINS), specifically definite method of a kind of Doppler log based on the bilingual calculation program of strapdown inertial navitation system (SINS) (DVL) range rate error.

Background technology

Strapdown inertial navitation system (SINS) SINS be a kind of can the continuous wave output bearer rate, the full autonomous navigation system of position and attitude information, it is connected in the direct peace of inertia assembly (gyroscope and accelerometer) on the carrier, by measuring linear velocity and the angular velocity information of carrier movement, behind navigation calculation, obtain navigation information.Because it has good concealment, be not subjected to the synoptic climate condition restriction, the advantages such as volume is little, lightweight, cost is low, easy care, is widely used in the fields such as Aeronautics and Astronautics, navigation.But, according to the SINS ultimate principle as can be known, be one of key factor of restriction strapdown inertial navitation system (SINS) applicability because the disturbing factors such as inertia assembly output error, initial alignment misalignment cause each navigation error of system to vibrate, disperse.

In order to suppress navigation error, various airmanships are interted in navigation calculation: for example, in order to eliminate Schuler, earth cycle oscillation error, system resolves and introduces damping in the process, this technology is as a kind of modification method, testing the speed to do by system self computing speed and Doppler log (DVL) poor obtains velocity error, passes through oscillation-damped error behind the damping network again, improves the long-time homing capability of inertial navigation system with this; For example, moving alignment technology, this technology be by in the carrier navigation process, tests the speed with system's computing speed and DVL and do poor observed quantity as Kalman filtering, improves strapdown matrix precision after will estimate the misalignment compensation, and then reduce misalignment navigation is affected.The common ground of more than mentioning every airmanship is all to have adopted velocity information that DVL provides as Velocity Reference.But in practical engineering application, DVL provides the speed can absolutely accurate, certainly exists error, causes the DVL velocity error to be introduced in the navigation calculation, finally affects system's navigation accuracy.How to estimate effectively that the DVL range rate error is that all kinds of airmanship applicabilities of enhancing, raising navigation accuracy are very important problems.

Traditional navigation calculation method has the compass method to resolve with the inertial navigation method to resolve: the compass method navigation calculation based on the compass principle is to utilize the inertia device measured value to resolve attitude of carrier information, does not carry out speed and position calculation.Therefore, in carrier navigation process, need DVL to provide velocity information to upgrade the compass parameter.In this calculation method, the DVL range rate error is equivalent to gyroscopic drift to resolving the attitude impact, affects its steady-state error; Be different from the compass method and resolve, inertial navigation method navigation calculation speed, positional information, so the inertial navigation method is resolved and is not comprised the impact of DVL range rate error in the steady-state error of attitude.

The 2nd interim " the periodic damping signal is on externalabsolutedamped INS site error impact research (ocean current correction) " of being write by people such as Huang Yihe of " Chinese inertial technology journal " the 7th volume in 1999, this article has been studied the Damping Theory problem of externalabsolutedamped INS, and namely outer degree of testing the speed comprises periodic error to rule, the characteristics of inertial navigation system error effect; The 4th interim " analysis of Errors from Doppler Log " of being write by Ren Maodong of " Dalian sea-freight journal " the 11st volume in 1985, this article utilizes the mathematical analysis method to analyze some error of dualbeam Doppler log, i.e. the error that sound velocity error, signal fluctuation error, transducer alignment error and the variation of beam transmission angle causes etc." adapt to and eliminate the Doppler navigation range rate error " that " modern radar " the 4th phase of the 16th volume in 1999 is write by Sun Yuanhong, this article is mainly studied test the speed characteristic parameter, the nonparametric of echo spectrum bias distortion of marine Doppler navigation and is processed in real time Spectrum Distortion and recover the principle of Gaussian spectrum, and signal fluctuation and signal to noise ratio (S/N ratio) are on the impact of revising and the criterion of processing in real time.Above document has all been analyzed the impact of DVL error, illustrate to determine that the DVL error has necessity, but prior art does not all provide method how to estimate the DVL range rate error.

Summary of the invention

The object of the present invention is to provide a kind of carrier of working as to be under the operational configuration, based on the DVL range rate error evaluation method of the bilingual calculation program of strapdown inertial navitation system (SINS).In order to obtain the DVL range rate error, the present invention utilizes one group of inertia assembly measured value to carry out simultaneously two kinds of calculation methods and resolves, resolve attitude with two groups and further calculate estimation DVL range rate error, determine the method for Doppler log range rate error in concrete a kind of strapdown inertial navitation system (SINS) provided by the invention, comprise the steps:

Step 1: utilize the global location gps system that carrier positions information is provided, and bind to navigational computer.

Step 2: after fiber-optic gyroscope strapdown inertial navigation system carries out preheating, gather the data of fibre optic gyroscope and quartz accelerometer output; According to the relation of accelerometer output with acceleration of gravity, and the relation of gyroscope output and rotational-angular velocity of the earth, determine the attitude of carrier angle, the completion system initial alignment is set up the initial strapdown matrix of inertial navigation system

$C_{b0}^n=\left[\begin{array}{ccc}\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}\cos {\mathrm{\&phi;}}_{z0}-\sin {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}\sin {\mathrm{\&phi;}}_{z0}& -\cos {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}& \sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}+\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}\sin {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{y0}\\ \sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}\cos {\mathrm{\&phi;}}_{y0}+\sin {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}& \cos {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}& \sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}+\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}\sin {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{y0}\\ -\cos {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}& \sin {\mathrm{\&phi;}}_{x0}& \cos {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{y0}\end{array}\right]---\left(1\right)$

Wherein, φ _X0, φ _Y0, φ _Z0Represent respectively initial time carrier pitch angle, roll angle, course angle.

Step 3: utilize the inertia assembly to measure carrier angular motion and line movable information, adopt the compass method to carry out navigation calculation, upgrade the strapdown matrix And resolve the pitch angle φ that obtains carrier _{X (C)}, roll angle φ _{Y (C)}With course angle φ _{Z (C)}, subscript (C) expression compass method is resolved.Compass resolves in the process, introduces the carrier movement velocity information of DVL measurement and upgrades the compass parameter.

Step 4: when carrying out step 3, utilize angular motion and the line movable information of the carrier of same group of inertia assembly measurement of step 3, carry out inertial navigation method navigation calculation, upgrade the strapdown matrix And resolve the pitch angle φ that obtains carrier _{X (I)}, roll angle φ _{Y (I)}With course angle φ _{Z (I)}, subscript (I) expression inertial navigation method is resolved.

Step 5: compass method and inertial navigation method are resolved attitude angle do poorly, set up orientation misalignment difference:

γ _(I)-γ _(C)＝(φ _y(C)-φ _y(I))sinφ _x(C)+(φ _z(C)-φ _z(I)) （2）

Wherein, γ _(I), γ _(C)Represent respectively the orientation misalignment that inertial navigation method and compass method are resolved.

Step 6: utilize misalignment difference in orientation in the step 5, determine DVL range rate error δ V _D:

Wherein, R represents earth radius; Ω represents rotational-angular velocity of the earth; The latitude of the expression carrier position that pushing calculates that utilizes DVL to test the speed.

The present invention's advantage compared with prior art is: the method for definite DVL range rate error that the present invention proposes, use the measured value of one group of strapdown inertial measurement unit to support simultaneously inertial navigation method and two calculating of resolving program of compass method, wherein the compass method is introduced the velocity information renewal compass parameter that DVL measures, two resolve attitude information that program resolves further coupling calculate the DVL range rate error, the method has following features, (1) the present invention utilizes strapdown inertial navigation system as backup system estimation DVL range rate error, should estimate that the result compensated to behind the DVL, improve the DVL rate accuracy.(2) be used to come from the same group of measured value of measuring assembly, Output rusults has correlativity; (3) metrical information Complete Synchronization, and do not have any installation deviation; (4) the present invention does not need any external information, utilize in the strapdown inertial navitation system (SINS) navigation calculation, difference is resolved different these characteristics of navigation information errors of form that program is resolved, and two groups of strapdown inertial navitation system (SINS) is resolved the further coupling of attitude calculate the DVL range rate error, method is simple, and is easy to operate.

Description of drawings

Fig. 1 is the process flow diagram of definite DVL range rate error method of the present invention;

Fig. 2 is that the present invention utilizes Visual C++ emulation to obtain estimation DVL range rate error curve ten times;

Fig. 3 is that the attitude error comparison curves resolved in system before and after the present invention utilized the compensation of Visual C++ emulation estimation range rate error;

Fig. 4 is for utilizing the inventive method that the attitude comparison curves is resolved in system before and after the compensation of sea trial estimation DVL range rate error;

Fig. 5 is for utilizing sea trial carrier ship trajectory.

Embodiment

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

As shown in Figure 1, determine the method for Doppler log (DVL) range rate error in a kind of strapdown inertial navitation system (SINS) provided by the invention, specifically comprise the steps.

Step 1: utilize the global location gps system to provide carrier initial positional information, and bind to navigational computer.Described positional information refers to latitude and the longitude of carrier initial position.

Step 2: after fiber-optic gyroscope strapdown inertial navigation system carries out preheating, gather the data of fibre optic gyroscope and quartz accelerometer output; According to the relation of accelerometer output with acceleration of gravity, and the relation of gyroscope output and rotational-angular velocity of the earth, determine the attitude of carrier angle, the completion system initial alignment is set up the initial strapdown matrix of inertial navigation system

$C_{b0}^n=\left[\begin{array}{ccc}\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}\cos {\mathrm{\&phi;}}_{z0}-\sin {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}\sin {\mathrm{\&phi;}}_{z0}& -\cos {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}& \sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}+\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}\sin {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{y0}\\ \sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}\cos {\mathrm{\&phi;}}_{y0}+\sin {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}& \cos {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}& \sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}+\cos {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">z</figure-callout>}0}\sin {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{y0}\\ -\cos {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">y</figure-callout>}0}& \sin {\mathrm{\&phi;}}_{x0}& \cos {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{y0}\end{array}\right]---\left(4\right)$

Wherein, φ _X0, φ _Y0, φ _Z0Represent respectively initial time carrier pitch angle, roll angle, course angle.

Step 3: utilize the inertia assembly to measure carrier angular motion and line movable information, carry out compass method navigation calculation.Compass resolves in the process, in order to upgrade the compass parameter, introduces DVL and measures the carrier movement velocity information.Subscript in the following formula (C) expression compass method calculation result, little footnote (I) expression inertial navigation method calculation result.

Gather gyroscope survey Data Update strapdown matrix

Specifically:

At first upgrade angular velocity:

${\mathrm{\&omega;}}_{\mathrm{nb}\left(C\right)}^b={\mathrm{\&omega;}}_{\mathrm{ib}\left(C\right)}^b-{\left(C_{b\left(C\right)}^n\right)}^T({\mathrm{\&omega;}}_{\mathrm{ie}\left(C\right)}^n+{\mathrm{\&omega;}}_{\mathrm{en}\left(C\right)}^n)-{\left(C_{b\left(C\right)}^n\right)}^T{\mathrm{\&omega;}}_{c\left(C\right)}^n---\left(5\right)$

Wherein, i represents Earth central inertial system, and e represents terrestrial coordinate system, and b represents carrier coordinate system, and n represents navigation coordinate system, and navigation coordinate system adopts local geographic coordinate system among the present invention;

Expression b is tied to the strapdown matrix of n system, and the navigation calculation initial time adopts the initial strapdown matrix that utilizes initial alignment to obtain in the step 2

Resolve in the process back, adopts the strapdown matrix after upgrading;

Expression p is that the angular velocity of rotation that relative m is is projection at q;

The projection of fastening at n for pilot angle speed;

For rotational-angular velocity of the earth is projection at n; ^TThe representing matrix transposition.Rotational-angular velocity of the earth is projection in navigation Upgrade:

Ω=0004167 °/s, the expression rotational-angular velocity of the earth;

The latitude of the expression carrier position that pushing calculates that utilizes DVL to test the speed, its computing method are:

Wherein,

Utilize the latitude of the initial time carrier position of GPS acquisition in the expression step 1, v _{Y (D)}It is oy that expression DVL tests the speed at navigation coordinate _nComponent on the axle; T represents current carrier hours underway; R represents earth radius.

Because it is that relatively the angular velocity of rotation that changes of spherical coordinate system is projection at navigation coordinate that carrier movement causes navigation coordinate

Upgrade according to following formula:

Wherein, v _{X (D)}It is ox that expression DVL tests the speed at navigation coordinate _nComponent on the axle.

Pilot angle speed

Be ox at navigation coordinate _nAxle, oy _nAxle, oz _nComponent on the axle With Be updated to:

${\mathrm{\&omega;}}_{\mathrm{cx}\left(C\right)}^n=\frac{k_N}{s+k_1}s(v_{x\left(C\right)}-v_{x\left(D\right)})$

${\mathrm{\&omega;}}_{\mathrm{cz}\left(C\right)}^n=\frac{k_U}{(s+k_1)(s+k_2)}s(v_{x\left(C\right)}-v_{x\left(D\right)})$

Wherein, v _{X (C)}And v _{Y (C)}Being respectively the compass method, to resolve carrier be ox along navigation coordinate _nAxle and oy _nThe movement velocity of axle; k ₁, k ₂, k _E, k _N, k _UBe the compass parameter, k is set in the embodiment of the invention ₁=0.002828, k ₂=0.002828, k ₃=6.1218 * 10 ^-7, k _E=k _N=k _U=8.0429 * 10 ^-9S represents the complex field parameter.

Then adopt and upgrade Quaternion Method renewal strapdown matrix

If the rotation hypercomplex number Q of carrier coordinate system Relative Navigation coordinate system is:

Q _(C)＝q _0(C)+q _1(C)i _b+q _2(C)j _b+q _3(C)k _b （10）

Wherein, q _{0 (C)}, q _{1 (C)}, q _{2 (C)}And q _{3 (C)}Resolve four real numbers of middle hypercomplex number for the compass method; i _b, j _bAnd k _bRepresent respectively carrier coordinate system ox _bAxle, oy _bAxle and oz _bUnit direction vector on the axle.

The timely correction of hypercomplex number Q:

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_{0\left(C\right)}\\ {\stackrel{\&CenterDot;}{q}}_{1\left(C\right)}\\ {\stackrel{\&CenterDot;}{q}}_{2\left(C\right)}\\ {\stackrel{\&CenterDot;}{q}}_{3\left(C\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{nbx}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbz}\left(C\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nbx}\left(C\right)}^{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">b</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{nbz}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nby}\left(C\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbz}\left(C\right)}^{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">b</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{nbx}\left(C\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nbz}\left(C\right)}^b& {\mathrm{\&omega;}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbx}\left(C\right)}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{0\left(C\right)}\\ q_{1\left(C\right)}\\ q_{2\left(C\right)}\\ q_{3\left(C\right)}\end{array}\right]---\left(11\right)$

Wherein,

Represent that respectively the angular velocity of rotation of carrier coordinate system Relative Navigation coordinate system in the compass algorithm is at carrier coordinate system ox _bAxle, oy _bAxle, oz _bComponent on the axle. Represent respectively q _{0 (C)}, q _{1 (C)}, q _{2 (C)}, q _{3 (C)}Micro component.

The q that utilization is obtained _{0 (C)}, q _{1 (C)}, q _{2 (C)}, q _{3 (C)}Upgrade the strapdown matrix in the compass algorithm

$C_{b\left(C\right)}^n=\left[\begin{array}{ccc}{q}_{0\left(C\right)}^2+q_{1\left(C\right)}^2-q_{2\left(C\right)}^2-q_{3\left(C\right)}^2& 2(q_{1\left(C\right)}{q}_{2\left(C\right)}-q_{0\left(C\right)}{q}_{3\left(C\right)})& 2(q_{1\left(C\right)}{q}_{3\left(C\right)}+q_{0\left(C\right)}{q}_{2\left(C\right)})\\ 2(q_{1\left(C\right)}{q}_{2\left(C\right)}+q_{0\left(C\right)}{q}_{3\left(C\right)})& q_{0\left(C\right)}^2-q_{1\left(C\right)}^2+q_{2\left(C\right)}^2-q_{3\left(C\right)}^2& 2(q_{2\left(C\right)}{q}_{3\left(C\right)}-q_{0\left(C\right)}{q}_{1\left(C\right)})\\ 2(q_{1\left(C\right)}{q}_{3\left(C\right)}-q_{0\left(C\right)}{q}_{2\left(C\right)})& 2(q_{2\left(C\right)}{q}_{3\left(C\right)}+q_{0\left(C\right)}{q}_{1\left(C\right)})& q_{0\left(C\right)}^2-q_{1\left(C\right)}^2-q_{2\left(C\right)}^2+q_{3\left(C\right)}^2\end{array}\right]---\left(12\right)$

Upgrade attitude of carrier information, specifically resolve the pitch angle φ of the carrier that obtains through the compass method _{X (C)}, roll angle φ _{Y (C)}With course angle φ _{Z (C)}Be respectively:

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_{x\left(C\right)}=\arcsin \left(c_{33\left(C\right)}\right)\\ {\mathrm{\&phi;}}_{y\left(C\right)}=\arctan (c_{32\left(C\right)}/c_{31\left(C\right)})\\ {\mathrm{\&phi;}}_{z\left(C\right)}=\arctan (c_{13\left(C\right)}/c_{23\left(C\right)})\end{array}\right.---\left(13\right)$

Wherein, c _{Ij (C)}(i=1,2,3, j=1,2,3) expression

In the capable j column matrix of i element.

Utilize the accelerometer measures specific force by the strapdown matrix

Conversion:

$f_{\left(C\right)}^n=C_{b\left(C\right)}^n{f}_{\left(C\right)}^s---\left(14\right)$

Wherein, f ⁿ, f ^sRepresent that respectively the accelerometer measures specific force is projection in n system and s.

Utilize following differential equation carrier movement speed:

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{v}}_{x\left(C\right)}\\ {\stackrel{\&CenterDot;}{v}}_{y\left(C\right)}\\ {\stackrel{\&CenterDot;}{v}}_{z\left(C\right)}\end{array}\right]=\left[\begin{array}{c}{f}_{x\left(C\right)}^n\\ f_{y\left(C\right)}^n\\ f_{z\left(C\right)}^n\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+\left[\begin{array}{ccc}0& 2{\mathrm{\&omega;}}_{\mathrm{iez}\left(C\right)}^n& -(2{\mathrm{\&omega;}}_{\mathrm{iey}\left(C\right)}^n+{\mathrm{\&omega;}}_{\mathrm{eny}\left(C\right)}^n)\\ -{\mathrm{\&omega;}}_{\mathrm{iez}\left(C\right)}^{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">n</figure-callout>}}& 0& 2{\mathrm{\&omega;}}_{\mathrm{iex}\left(C\right)}^n+{\mathrm{\&omega;}}_{\mathrm{enx}\left(C\right)}^{\mathrm{<figure-callout\; id="2"\; label="n"\; filenames="HDA00002714496000012.png"\; state="\{\{state\}\}">n</figure-callout>}}\\ 2{\mathrm{\&omega;}}_{\mathrm{iey}\left(C\right)}^n+{\mathrm{\&omega;}}_{\mathrm{eny}\left(C\right)}^n& -(2{\mathrm{\&omega;}}_{\mathrm{iex}\left(C\right)}^n+{\mathrm{\&omega;}}_{\mathrm{enx}\left(C\right)}^n)& 0\end{array}\right]\left[\begin{array}{c}{v}_{x\left(C\right)}\\ v_{y\left(C\right)}\\ v_{z\left(C\right)}\end{array}\right]---\left(15\right)$

Wherein,

Represent that respectively the accelerometer measures specific force is ox at navigation coordinate _nAxle, oy _nAxle, oz _nComponent on the axle; G is acceleration of gravity.

With

Represent respectively rotational-angular velocity of the earth

Be ox at navigation coordinate _nAxle, oy _nAxle, oz _nComponent on the axle.

Expression is because carrier movement causes navigation coordinate is that relatively the angular velocity of rotation that changes of spherical coordinate system is ox at navigation coordinate respectively _nAxle, oy _nProjection on the axle.

Represent respectively v _{X (C)}, v _{Y (C)}, v _{Z (C)}Micro component.

Step 4: when carrying out step 3, utilize angular motion and the line movable information of the same group of carrier that the inertia assembly is measured in the step 3, adopt the inertial navigation method to carry out navigation calculation.Detailed process is:

Gather gyroscope survey Data Update strapdown matrix

At first upgrade angular velocity:

${\mathrm{\&omega;}}_{\mathrm{nb}\left(I\right)}^b={\mathrm{\&omega;}}_{\mathrm{ib}\left(I\right)}^b-{\left(C_{b\left(I\right)}^n\right)}^T({\mathrm{\&omega;}}_{\mathrm{ie}\left(I\right)}^n+{\mathrm{\&omega;}}_{\mathrm{en}\left(I\right)}^n)---\left(16\right)$

Wherein, subscript (I) expression inertial navigation method calculation result.

More new formula is as follows:

Wherein,

Expression inertial navigation method is resolved the latitude of the carrier position that obtains.

More new formula is as follows:

Wherein,

Expression is that the angular velocity of rotation of relatively spherical coordinate system variation is in the projection of navigation coordinate system owing to carrier movement causes navigation coordinate; v _{X (I)}, v _{Y (I)}The carrier that respectively inertial navigation method is resolved is ox along navigation _nAxle, oy _nMovement velocity on the axle.R represents earth radius.

If the rotation hypercomplex number Q of carrier coordinate system Relative Navigation coordinate system _(I)For

Q _(I)＝q _0(I)+q _1(I)i _b+q _2(I)j _b+q _3(I)k _b （19）

Wherein, q _{0 (I)}, q _{1 (I)}, q _{2 (I)}, q _{3 (I)}Four real numbers for hypercomplex number.

The timely correction of hypercomplex number Q:

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_{0\left(I\right)}\\ {\stackrel{\&CenterDot;}{q}}_{1\left(I\right)}\\ {\stackrel{\&CenterDot;}{q}}_{2\left(I\right)}\\ {\stackrel{\&CenterDot;}{q}}_{3\left(I\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{nbx}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbz}\left(I\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nbx}\left(I\right)}^{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">b</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{nbz}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nby}\left(I\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbz}\left(I\right)}^{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">b</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{nbx}\left(I\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nbz}\left(I\right)}^b& {\mathrm{\&omega;}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbx}\left(I\right)}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{0\left(I\right)}\\ q_{1\left(I\right)}\\ q_{2\left(I\right)}\\ q_{3\left(I\right)}\end{array}\right]---\left(20\right)$

Wherein,

Represent that respectively the motion angular velocity of carrier coordinate system Relative Navigation coordinate system in the inertial navigation method is at carrier coordinate system ox _bAxle, oy _bAxle, oz _bComponent on the axle.

Represent respectively q _{0 (I)}, q _{1 (I)}, q _{2 (I)}, q _{3 (I)}Micro component.

The q that utilization is obtained _{0 (I)}, q _{1 (I)}, q _{2 (I)}, q _{3 (I)}Upgrade the strapdown matrix

$C_{b\left(I\right)}^n=\left[\begin{array}{ccc}{q}_{0\left(I\right)}^2+q_{1\left(I\right)}^2-q_{2\left(I\right)}^2-q_{3\left(I\right)}^2& 2(q_{1\left(I\right)}{q}_{2\left(I\right)}-q_{0\left(I\right)}{q}_{3\left(I\right)})& 2(q_{1\left(I\right)}{q}_{3\left(I\right)}+q_{0\left(I\right)}{q}_{2\left(I\right)})\\ 2(q_{1\left(I\right)}{q}_{2\left(I\right)}+q_{0\left(I\right)}{q}_{3\left(I\right)})& q_{0\left(I\right)}^2-q_{1\left(I\right)}^2+q_{2\left(I\right)}^2-q_{3\left(I\right)}^2& 2(q_{2\left(I\right)}{q}_{3\left(I\right)}-q_{0\left(I\right)}{q}_{1\left(I\right)})\\ 2(q_{1\left(I\right)}{q}_{3\left(I\right)}-q_{0\left(I\right)}{q}_{2\left(I\right)})& 2(q_{2\left(I\right)}{q}_{3\left(I\right)}+q_{0\left(I\right)}{q}_{1\left(I\right)})& q_{0\left(I\right)}^2-q_{1\left(I\right)}^2-q_{2\left(I\right)}^2+q_{3\left(I\right)}^2\end{array}\right]---\left(21\right)$

Upgrade attitude of carrier information, the carrier pitch angle φ that specifically adopts the inertial navigation method to resolve to obtain _{X (I)}, roll angle φ _{Y (I)}With course angle φ _{Z (I)}Be respectively:

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_{x\left(I\right)}=\arcsin \left(c_{33\left(I\right)}\right)\\ {\mathrm{\&phi;}}_{y\left(I\right)}=\arctan (c_{32\left(I\right)}/c_{31\left(I\right)})\\ {\mathrm{\&phi;}}_{z\left(I\right)}=\arctan (c_{13\left(I\right)}/c_{23\left(I\right)})\end{array}\right.---\left(22\right)$

Wherein, c _{Ij (I)}(i=1,2,3, j=1,2,3) expression In the capable j column matrix of i element.

Utilize the accelerometer measures specific force by the strapdown matrix

Conversion:

$f_{\left(I\right)}^n=C_{b\left(I\right)}^n{f}_{\left(I\right)}^s---\left(23\right)$

Utilize following differential equation carrier movement speed:

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{v}}_{x\left(I\right)}\\ {\stackrel{\&CenterDot;}{v}}_{y\left(I\right)}\\ {\stackrel{\&CenterDot;}{v}}_{z\left(I\right)}\end{array}\right]=\left[\begin{array}{c}{f}_{x\left(I\right)}^n\\ f_{y\left(I\right)}^n\\ f_{z\left(I\right)}^n\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]+\left[\begin{array}{ccc}0& 2{\mathrm{\&omega;}}_{\mathrm{iez}\left(I\right)}^n& -(2{\mathrm{\&omega;}}_{\mathrm{iey}\left(I\right)}^n+{\mathrm{\&omega;}}_{\mathrm{eny}\left(I\right)}^n)\\ -{\mathrm{\&omega;}}_{\mathrm{iez}\left(I\right)}^{\mathrm{<figure-callout\; id="0"\; label="n"\; filenames="HDA00002714496000022.png"\; state="\{\{state\}\}">n</figure-callout>}}& 0& 2{\mathrm{\&omega;}}_{\mathrm{iex}\left(I\right)}^n+{\mathrm{\&omega;}}_{\mathrm{enx}\left(I\right)}^{\mathrm{<figure-callout\; id="2"\; label="n"\; filenames="HDA00002714496000012.png"\; state="\{\{state\}\}">n</figure-callout>}}\\ 2{\mathrm{\&omega;}}_{\mathrm{iey}\left(I\right)}^n+{\mathrm{\&omega;}}_{\mathrm{eny}\left(I\right)}^n& -(2{\mathrm{\&omega;}}_{\mathrm{iex}\left(I\right)}^n+{\mathrm{\&omega;}}_{\mathrm{enx}\left(I\right)}^n)& 0\end{array}\right]\left[\begin{array}{c}{v}_{x\left(I\right)}\\ v_{y\left(I\right)}\\ v_{z\left(I\right)}\end{array}\right]---\left(24\right)$

Step 5: it is poor that the attitude angle that compass method and inertial navigation method are resolved is done, and sets up orientation misalignment difference:

γ _(I)-γ _(C)＝(φ _y(C)-φ _y(I))sinφ _x(C)+(φ _z(C)-φ _z(I)) （25）

Wherein, γ _(I), γ _(C)Represent respectively the orientation misalignment that inertial navigation method and compass method are resolved.

Step 6: according to the orientation misalignment difference that step 5 obtains, further obtain DVL range rate error δ V _D:

Verify as follows to beneficial effect of the present invention:

(1) under Visual C++ simulated conditions, the method is carried out emulation experiment:

Carrier is done the three-axis swinging motion.Carrier with the deviation from voyage route of sinusoidal rule to the angle, pitch angle and roll angle wave, its mathematical model is:

Wherein, φ _x, φ _y, φ _zThe pitch angle, roll angle and the course angle that represent respectively carrier; φ _Xm, π _Ym, π _ZmRepresent respectively corresponding swing angle amplitude, φ _Xm=φ _Ym=φ _Zm=5 °;

Represent respectively the initial phase that pitch angle, roll angle and course angle are waved,

T _x, T _y, T _zThe rolling period T that represents respectively corresponding swinging shaft _x=T _y=T _z=4s; K is true flight path, k=30 °.

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

Carrier is at the uniform velocity sailed through to motion, and movement velocity is v=15m/s;

DVL range rate error: δ v _D=1m/s;

Equatorial radius: R=6378393.0m;

By the available earth surface acceleration of gravity of universal gravitation: g=9.78049m/s ²

Rotational-angular velocity of the earth: Ω=7.2921158 * 10 ^-5Rad/s;

Constant: π=3.1415926535;

Fiber optic gyroscope constant drift: 0.01 °/h;

Optical fibre gyro white noise error: 0.005 °/h;

Optical fibre gyro scale factor error: 10ppm;

Optical fibre gyro alignment error: 1 * 10 ^-3Rad;

Accelerometer bias: 10 ^-4g ₀

Accelerometer white noise error: 5 * 10 ^-5g ₀

Accelerometer scale factor error: 10ppm;

Accelerometer alignment error: 1 * 10 ^-3Rad;

Compass parameter: k ₁=0.002828, k ₂=0.002828, k ₃=6.1218 * 10 ^-7, k _E=k _N=k _U=8.0429 * 10 ^-9

In the inertial reference calculation, system works is at interior absolute damping state, ratio of damping 0.5;

Simulation time: t=48h;

Sample frequency: Hn=0.01s;

Utilize and invent described method, obtain ten times and estimate DVL range rate error results as shown in Figure 2, ten estimation result statistics are illustrated in figure 3 as ten estimation averages and compensate the comparison curves that attitude is resolved by the front and back system such as table 1.The result shows that the present invention can better estimate the DVL range rate error.

Table 1 DVL range rate error estimation result statistics

(2) fiber-optic gyroscope strapdown inertial navigation system Sea Trials

This test is the Sea Trials that adopts fiber-optic gyroscope strapdown inertial navigation system to carry out, and the DVL that this test utilization is installed in stem provides it to measure carrier movement speed, and used optical fibre gyro inertial navigation system the key technical indexes is as follows:

Dynamic range: ± 100 °/s;

Zero is partially stable :≤0.005 °/h;

Random walk:

Scale factory non-linearity degree :≤5ppm.

Utilize the inventive method, obtain estimating that the DVL range rate error compensates the front and back system and resolves the attitude comparison curves as shown in Figure 4, the carrier ship trajectory as shown in Figure 5.The result shows that the present invention can better estimate the DVL range rate error, and attitude accuracy resolves in the raising system.

Claims (3)

1. determine the method for Doppler log (DVL) range rate error in the strapdown inertial navitation system (SINS), it is characterized in that, may further comprise the steps:

Step 1: utilize the global location gps system that the positional information of carrier initial time is provided, and bind to navigational computer; Described positional information comprises: latitude and the longitude of carrier initial time position;

Step 2: after fiber-optic gyroscope strapdown inertial navigation system carries out preheating, gather the data of fibre optic gyroscope and quartz accelerometer output, then determine the attitude of carrier angle, the completion system initial alignment is set up the initial strapdown matrix of strapdown inertial navitation system (SINS)

$C_{b0}^n=\left[\begin{array}{ccc}\cos {\mathrm{\&phi;}}_{y0}\cos {\mathrm{\&phi;}}_{z0}-\sin {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{y0}\sin {\mathrm{\&phi;}}_{z0}& -\cos {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{z0}& \sin {\mathrm{\&phi;}}_{y0}\cos {\mathrm{\&phi;}}_{z0}+\sin {\mathrm{\&phi;}}_{z0}\sin {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{y0}\\ \sin {\mathrm{\&phi;}}_{z0}\cos {\mathrm{\&phi;}}_{y0}+\sin {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{y0}\cos {\mathrm{\&phi;}}_{z0}& \cos {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{z0}& \sin {\mathrm{\&phi;}}_{z0}\sin {\mathrm{\&phi;}}_{y0}+\cos {\mathrm{\&phi;}}_{z0}\sin {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{y0}\\ -\cos {\mathrm{\&phi;}}_{x0}\sin {\mathrm{\&phi;}}_{y0}& \sin {\mathrm{\&phi;}}_{x0}& \cos {\mathrm{\&phi;}}_{x0}\cos {\mathrm{\&phi;}}_{y0}\end{array}\right]$

Wherein, φ _X0, φ _Y0And φ _Z0Represent respectively pitch angle, roll angle and the course angle of initial time carrier, b represents carrier coordinate system, and n represents navigation coordinate system;

Step 3: utilize the inertia assembly to measure angular motion and the line movable information of carrier, carry out compass method navigation calculation, upgrade the strapdown matrix

And resolve the pitch angle φ that obtains carrier _{X (C)}, roll angle φ _{Y (C)}With course angle φ _{Z (C)}, subscript (C) expression compass method is resolved;

Step 4: utilize angular motion and the line movable information of the carrier that the inertia assembly is measured in the step 3, adopt the inertial navigation method to carry out navigation calculation, upgrade the strapdown matrix

And resolve the pitch angle φ that obtains carrier _{X (I)}, roll angle φ _{Y (I)}With course angle φ _{Z (I)}, subscript (I) expression inertial navigation method is resolved;

Step 5: it is poor that the attitude angle that compass method and inertial navigation method are resolved is done, and sets up orientation misalignment difference:

γ _(I)-γ _(C)＝(φ _y(C)-φ _y(I))sinφ _x(C)+(φ _z(C)-φ _z(I))

Wherein, γ _(I), γ _(C)Represent respectively the orientation misalignment that inertial navigation method and compass method are resolved;

Step 6: utilize misalignment difference in orientation in the step 5, determine DVL range rate error δ V _D:

Wherein, R represents earth radius; Ω is rotational-angular velocity of the earth, and value is 0.004167 °/s;

The latitude of the expression carrier position that pushing calculates that utilizes DVL to test the speed.

2. determine the method for Doppler log range rate error in a kind of strapdown inertial navitation system (SINS) according to claim 1, it is characterized in that, the compass method of the carrying out navigation calculation described in the step 3, detailed process is:

Formula carries out the angular velocity renewal below at first adopting:

${\mathrm{\&omega;}}_{\mathrm{nb}\left(C\right)}^b={\mathrm{\&omega;}}_{\mathrm{ib}\left(C\right)}^b-{\left(C_{b\left(C\right)}^n\right)}^T({\mathrm{\&omega;}}_{\mathrm{ie}\left(C\right)}^n+{\mathrm{\&omega;}}_{\mathrm{en}\left(C\right)}^n)-{\left(C_{b\left(C\right)}^n\right)}^T{\mathrm{\&omega;}}_{c\left(C\right)}^n$

I represents Earth central inertial system, and e represents terrestrial coordinate system;

Expression p be the angular velocity of rotation of relative m system in the projection of q system,

That rotational-angular velocity of the earth is in the projection of n system; Be the projection of pilot angle speed in n system; ^TThe representing matrix transposition; Initially

Adopt the initial strapdown matrix that obtains in the step 2

More new formula be:

The latitude of the expression carrier position that pushing calculates that utilizes DVL to test the speed,

The latitude of expression carrier initial time position, v _{Y (D)}It is oy that expression DVL tests the speed at navigation coordinate _nComponent on the axle; T represents current carrier hours underway; R represents earth radius;

More new formula is:

Wherein, v _{X (D)}It is ox that expression DVL tests the speed at navigation coordinate _nComponent on the axle;

Pilot angle speed Be ox at navigation coordinate _nAxle, oy _nAxle and oz _nComponent on the axle With

Be updated to:

${\mathrm{\&omega;}}_{\mathrm{cx}\left(C\right)}^n=\frac{k_N}{s+k_1}s(v_{x\left(C\right)}-v_{x\left(D\right)})$

${\mathrm{\&omega;}}_{\mathrm{cz}\left(C\right)}^n=\frac{k_U}{(s+k_1)(s+k_2)}s(v_{x\left(C\right)}-v_{x\left(D\right)})$

Wherein, v _{X (C)}And v _{Y (C)}Being respectively the compass method, to resolve carrier be ox along navigation coordinate _nAxle and oy _nThe movement velocity of axle; S represents the complex field parameter; Compass parameter k ₁, k ₂, k _E, k _NAnd k _UBe set to: k ₁=0.002828, k ₂=0.002828, k ₃=6.1218 * 10 ^-7, k _E=k _N=k _U=8.0429 * 10 ^-9

Then adopt and upgrade Quaternion Method renewal strapdown matrix

If the rotation hypercomplex number Q of carrier coordinate system Relative Navigation coordinate system is:

Q _(C)＝q _0(C)+q _1(C)i _b+q _2(C)j _b+q _3(C)k _b

Wherein, q _{0 (C)}, q _{1 (C)}, q _{2 (C)}And q _{3 (C)}Resolve four real numbers of middle hypercomplex number for the compass method; i _b, j _bAnd k _bRepresent respectively carrier coordinate system ox _bAxle, oy _bAxle and oz _bUnit direction vector on the axle;

The timely correction of hypercomplex number Q:

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_{0\left(C\right)}\\ {\stackrel{\&CenterDot;}{q}}_{1\left(C\right)}\\ {\stackrel{\&CenterDot;}{q}}_{2\left(C\right)}\\ {\stackrel{\&CenterDot;}{q}}_{3\left(C\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{nbx}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbz}\left(C\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nbx}\left(C\right)}^b& 0& {\mathrm{\&omega;}}_{\mathrm{nbz}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nby}\left(C\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbz}\left(C\right)}^b& 0& {\mathrm{\&omega;}}_{\mathrm{nbx}\left(C\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nbz}\left(C\right)}^b& {\mathrm{\&omega;}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbx}\left(C\right)}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{0\left(C\right)}\\ q_{1\left(C\right)}\\ q_{2\left(C\right)}\\ q_{3\left(C\right)}\end{array}\right]$

Wherein,

Represent that respectively the angular velocity of rotation of carrier coordinate system Relative Navigation coordinate system is at carrier coordinate system ox _bAxle, oy _bAxle, oz _bComponent on the axle;

Represent respectively q _{0 (C)}, q _{1 (C)}, q _{2 (C)}, q _{3 (C)}Micro component;

The q that utilization is obtained _{0 (C)}, q _{1 (C)}, q _{2 (C)}, q _{3 (C)}Upgrade the strapdown matrix in the compass algorithm

$C_{b\left(C\right)}^n=\left[\begin{array}{ccc}{q}_{0\left(C\right)}^2+q_{1\left(C\right)}^2-q_{2\left(C\right)}^2-q_{3\left(C\right)}^2& 2(q_{1\left(C\right)}{q}_{2\left(C\right)}-q_{0\left(C\right)}{q}_{3\left(C\right)})& 2(q_{1\left(C\right)}{q}_{3\left(C\right)}+q_{0\left(C\right)}{q}_{2\left(C\right)})\\ 2(q_{1\left(C\right)}{q}_{2\left(C\right)}+q_{0\left(C\right)}{q}_{3\left(C\right)})& q_{0\left(C\right)}^2-q_{1\left(C\right)}^2+q_{2\left(C\right)}^2-q_{3\left(C\right)}^2& 2(q_{2\left(C\right)}{q}_{3\left(C\right)}-q_{0\left(C\right)}{q}_{1\left(C\right)})\\ 2(q_{1\left(C\right)}{q}_{3\left(C\right)}-q_{0\left(C\right)}{q}_{2\left(C\right)})& 2(q_{2\left(C\right)}{q}_{3\left(C\right)}+q_{0\left(C\right)}{q}_{1\left(C\right)})& q_{0\left(C\right)}^2-q_{1\left(C\right)}^2-q_{2\left(C\right)}^2+q_{3\left(C\right)}^2\end{array}\right]$

Resolve the pitch angle φ of the carrier that obtains through the compass method _{X (C)}, roll angle φ _{Y (C)}With course angle φ _{Z (C)}For:

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_{x\left(C\right)}=\arcsin \left(c_{33\left(C\right)}\right)\\ {\mathrm{\&phi;}}_{y\left(C\right)}=\arctan (c_{32\left(C\right)}/c_{31\left(C\right)})\\ {\mathrm{\&phi;}}_{z\left(C\right)}=\arctan (c_{13\left(C\right)}/c_{23\left(C\right)})\end{array}\right.$

Wherein, c _{Ij (C)}(i=1,2,3, j=1,2,3) expression

In the capable j column matrix of i element.

3. determine the method for Doppler log range rate error in a kind of strapdown inertial navitation system (SINS) according to claim 1, it is characterized in that, the employing inertial navigation method described in the step 4 is carried out navigation calculation, and detailed process is:

At first upgrade angular velocity:

${\mathrm{\&omega;}}_{\mathrm{nb}\left(I\right)}^b={\mathrm{\&omega;}}_{\mathrm{ib}\left(I\right)}^b-{\left(C_{b\left(I\right)}^n\right)}^T({\mathrm{\&omega;}}_{\mathrm{ie}\left(I\right)}^n+{\mathrm{\&omega;}}_{\mathrm{en}\left(I\right)}^n)$

Wherein, i represents Earth central inertial system, and e represents terrestrial coordinate system; Initially

Adopt the initial strapdown matrix that obtains in the step 2

Expression p be the angular velocity of rotation of relative m system in the projection of q system,

That rotational-angular velocity of the earth is in the projection of n system;

More new formula is:

Expression inertial navigation method is resolved the latitude of the carrier position that obtains;

More new formula is:

Wherein, v _{X (I)}, v _{Y (I)}The carrier that respectively inertial navigation method is resolved is ox along navigation _nAxle, oy _nMovement velocity on the axle, R represents earth radius;

Then upgrade the strapdown matrix

If carrier coordinate system relative to the hypercomplex number Q that rotates of platform coordinate system is:

Q _(I)＝q _0(I)+q _1(I)i _b+q _2(I)j _b+q _3(I)k _b

Wherein, q _{0 (I)}, q _{1 (I)}, q _{2 (I)}, q _{3 (I)}Resolve four real numbers of middle hypercomplex number for the inertial navigation method, i _b, j _bAnd k _bRepresent respectively carrier coordinate system ox _bAxle, oy _bAxle and oz _bUnit direction vector on the axle;

The timely correction of hypercomplex number Q:

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_{0\left(I\right)}\\ {\stackrel{\&CenterDot;}{q}}_{1\left(I\right)}\\ {\stackrel{\&CenterDot;}{q}}_{2\left(I\right)}\\ {\stackrel{\&CenterDot;}{q}}_{3\left(I\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{nbx}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbz}\left(I\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nbx}\left(I\right)}^b& 0& {\mathrm{\&omega;}}_{\mathrm{nbz}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nby}\left(I\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbz}\left(I\right)}^b& 0& {\mathrm{\&omega;}}_{\mathrm{nbx}\left(I\right)}^b\\ {\mathrm{\&omega;}}_{\mathrm{nbz}\left(I\right)}^b& {\mathrm{\&omega;}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\&omega;}}_{\mathrm{nbx}\left(I\right)}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_{0\left(I\right)}\\ q_{1\left(I\right)}\\ q_{2\left(I\right)}\\ q_{3\left(I\right)}\end{array}\right]$

Wherein,

Represent that respectively the motion angular velocity of carrier coordinate system Relative Navigation coordinate system is at carrier coordinate system ox _bAxle, oy _bAxle, oz _bComponent on the axle;

Represent respectively q _{0 (I)}, q _{1 (I)}, q _{2 (I)}, q _{3 (I)}Micro component;

The q that utilization is obtained _{0 (I)}, q _{1 (I)}, q _{2 (I)}, q _{3 (I)}Upgrade the strapdown matrix

$C_{b\left(I\right)}^n=\left[\begin{array}{ccc}{q}_{0\left(I\right)}^2+q_{1\left(I\right)}^2-q_{2\left(I\right)}^2-q_{3\left(I\right)}^2& 2(q_{1\left(I\right)}{q}_{2\left(I\right)}-q_{0\left(I\right)}{q}_{3\left(I\right)})& 2(q_{1\left(I\right)}{q}_{3\left(I\right)}+q_{0\left(I\right)}{q}_{2\left(I\right)})\\ 2(q_{1\left(I\right)}{q}_{2\left(I\right)}+q_{0\left(I\right)}{q}_{3\left(I\right)})& q_{0\left(I\right)}^2-q_{1\left(I\right)}^2+q_{2\left(I\right)}^2-q_{3\left(I\right)}^2& 2(q_{2\left(I\right)}{q}_{3\left(I\right)}-q_{0\left(I\right)}{q}_{1\left(I\right)})\\ 2(q_{1\left(I\right)}{q}_{3\left(I\right)}-q_{0\left(I\right)}{q}_{2\left(I\right)})& 2(q_{2\left(I\right)}{q}_{3\left(I\right)}+q_{0\left(I\right)}{q}_{1\left(I\right)})& q_{0\left(I\right)}^2-q_{1\left(I\right)}^2-q_{2\left(I\right)}^2+q_{3\left(I\right)}^2\end{array}\right]$

Adopt the inertial navigation method to resolve the pitch angle φ of the carrier that obtains _{X (I)}, roll angle φ _{Y (I)}With course angle φ _{Z (I)}For:

$\left\{\begin{array}{c}{\mathrm{\&phi;}}_{x\left(I\right)}=\arcsin \left(c_{33\left(I\right)}\right)\\ {\mathrm{\&phi;}}_{y\left(I\right)}=\arctan (c_{32\left(I\right)}/c_{31\left(I\right)})\\ {\mathrm{\&phi;}}_{z\left(I\right)}=\arctan (c_{13\left(I\right)}/c_{23\left(I\right)})\end{array}\right.$

Wherein, c _{Ij (I)}(i=1,2,3, j=1,2,3) expression In the capable j column matrix of i element.

Images