CN103076026B - A kind of method determining Doppler log range rate error in SINS - Google Patents

Abstract

The invention provides a kind of method determining Doppler log range rate error in SINS.The method determines the initial position parameters of carrier by GPS, gathers fibre optic gyroscope output and accelerometer output data, and data process and are initially directed at, it is determined that initial strap-down matrix；Then gathering angular movement and the line movable information of the carrier that inertia assembly is measured, be respectively adopted compass method and inertial navigation method is navigated resolving, wherein compass method introduces the DVL carrier movement velocity information measured in resolving；Two methods are resolved the two groups of attitude informations obtained and does difference, carry out being converted to two groups of azimuthal misalignment angular difference values resolving attitude；Finally the conversion of azimuthal misalignment angular difference value is obtained DVL range rate error.The inventive method can estimate DVL range rate error in carrier navigation process, result is compensated to, after DVL, improving DVL rate accuracy, and method is simple, easily operates.

Description

A kind of method determining Doppler log range rate error in SINS

Technical field

The present invention relates to the navigation calculation technical field of SINS, specifically the defining method of a kind of Doppler log (DVL) range rate error based on the double; two solver of SINS.

Background technology

SINS SINS is a kind of full autonomous navigation system that can export bearer rate, position and attitude information continuously, inertia assembly (gyroscope and accelerometer) directly peace is connected on carrier by it, by measuring linear velocity and the angular velocity information of carrier movement, after navigation calculation, obtain navigation information.Due to its there is good concealment, do not limited by synoptic climate condition, the advantage such as volume is little, lightweight, cost is low, easy care, be widely used in the field such as Aeronautics and Astronautics, navigation.But, according to SINS ultimate principle it can be seen that be one of key factor restricting the SINS suitability owing to the interference factors such as inertia assembly output error, initial alignment misalignment cause that each navigation error of system is vibrated, dispersed.

In order to suppress navigation error, various airmanships are interspersed in navigation calculation: such as, in order to eliminate Schuler, earth cycle oscillation error, system solution process introduces damping, this technology is as a kind of modification method, tested the speed by system self computing speed and Doppler log (DVL) to do to differ from and obtain velocity error, then through oscillation-damped error after damping network, improve the long-time homing capability of inertial navigation system with this；Such as, moving alignment technology, this technology is passed through in carrier navigation process, the observed quantity doing difference as Kalman filter of testing the speed using system computing speed and DVL, by raising strap-down matrix precision after estimation misalignment compensation, and then reduces misalignment to impact of navigating.The common ground of every airmanship mentioned above is all to have employed the DVL velocity information provided as Velocity Reference.But in practical engineering application, DVL offer speed will not be absolutely accurate, certainly exists error, causes that DVL velocity error is introduced in navigation calculation, final influential system navigation accuracy.How effectively estimation DVL range rate error is that to strengthen all kinds of airmanship suitabilitys, improve navigation accuracy be a very important problem.

Traditional navigation calculation method has compass method to resolve and inertial navigation method resolves: the compass method navigation calculation based on compass principle is to utilize inertia device measured value to resolve attitude of carrier information, does not carry out speed and position calculation.Therefore, in carrier navigation process, it is necessary to DVL provides velocity information to update compass parameter.In this calculation method, DVL range rate error is equivalent to gyroscopic drift to resolving attitude impact, affects its steady-state error；It is different from compass method to resolve, inertial navigation method navigation calculation speed, positional information, the steady-state error of therefore inertial navigation method resolving attitude does not comprise the impact of DVL range rate error.

" China's inertial technology journal " interim " periodic damping signal is to externalabsolutedamped INS site error influence research (ocean current correction) " write by Huang Yihe et al. of the 7th volume the 2nd in 1999, this article have studied the Damping Theory problem of externalabsolutedamped INS, and namely outer degree of testing the speed comprises rule, the feature that inertial navigation system error is affected by periodic error；" Dalian sea-freight journal " interim " analysis of Errors from Doppler Log " write by Ren Maodong of the 11st volume the 4th in 1985, this article utilizes mathematical analysis method to analyze some error of dualbeam Doppler log, and namely sound velocity error, signal fluctuation error, transducer alignment error and beam transmission angle change the error etc. caused." the adapting to eliminate Doppler navigation range rate error " that " modern radar " the 16th volume the 4th phase in 1999 is write by Sun Yuanhong, this article is mainly studied the test the speed characteristic parameter of echo spectrum bias distortion, nonparametric of marine Doppler navigation and is processed Spectrum Distortion in real time and recover the principle of Gaussian spectrum, and the impact on revising of signal fluctuation and signal to noise ratio and the criterion that processes in real time.Document above all analyzes the impact of DVL error, illustrates to determine that DVL error has necessity, but prior art is all without providing the method how estimating DVL range rate error.

Summary of the invention

It is an object of the invention to provide a kind of carrier of working as and be under operational configuration, based on the DVL range rate error evaluation method of the double; two solver of SINS.In order to obtain DVL range rate error, the present invention utilizes one group of inertia assembly measured value to carry out two kinds of calculation methods simultaneously and resolves, two groups are resolved attitude and calculates estimation DVL range rate error further, the method determining Doppler log range rate error in concrete a kind of SINS provided by the invention, comprises the steps:

Step 1: utilize global location GPS system to provide carrier positions information, and bind to navigational computer.

Step 2: after fiber-optic gyroscope strapdown inertial navigation system preheats, gathers fibre optic gyroscope and the data of quartz accelerometer output；Relation according to accelerometer output with acceleration of gravity, and the relation of gyroscope output and rotational-angular velocity of the earth, it is determined that attitude of carrier angle, completion system is initially directed at, and sets up the initial strap-down matrix of inertial navigation system

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

Wherein, φ_x0、φ_y0、φ_z0Represent initial time carrier pitch angle, roll angle, course angle respectively.

Step 3: utilize inertia assembly to measure carrier angular movement and line movable information, adopts compass method to be navigated resolving, updates strap-down matrixAnd resolve the pitch angle φ obtaining carrier_x(C), roll angle φ_y(C)With course angle φ_z(C), subscript (C) represents that compass method resolves.In compass solution process, introduce the DVL carrier movement velocity information measured and update compass parameter.

Step 4: while carrying out step 3, utilizes angular movement and the line movable information of the carrier of same group of inertia assembly measurement of step 3, carries out inertial navigation method navigation calculation, update strap-down matrixAnd resolve the pitch angle φ obtaining carrier_x(I), roll angle φ_y(I)With course angle φ_z(I), subscript (I) represents that inertial navigation method resolves.

Step 5: compass method and inertial navigation method are resolved attitude angle and does difference, set up azimuthal misalignment angular difference value:

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

Wherein, γ_(I)、γ_(C)Represent the azimuthal misalignment angle that inertial navigation method and compass method resolve respectively.

Step 6: utilize azimuthal misalignment angular difference value in step 5, it is determined that DVL range rate error δ V_D:

Wherein, R represents earth radius；Ω represents rotational-angular velocity of the earth；Represent and utilize DVL to test the speed the latitude of the calculated carrier position of pushing.

Present invention advantage compared with prior art is in that: the method for the determination DVL range rate error that the present invention proposes, the measured value using one group of strapdown inertial measurement unit supports inertial navigation method and the calculating of two solver of compass method simultaneously, wherein compass method introduces the DVL velocity information measured renewal compass parameter, the attitude information of two solver resolvings couples calculating further and obtains DVL range rate error, the method has following features, (1) present invention utilizes strapdown inertial navigation system to estimate DVL range rate error as aid system, this estimation result is compensated to after DVL, improve DVL rate accuracy.(2) utilizing the measured value deriving from same group of measurement assembly, output result has dependency；(3) metrical information Complete Synchronization, and it is absent from any installation deviation；(4) present invention does not need any external information, utilize in SINS navigation calculation, navigation information errors of form this feature different that different solver resolve, two groups of SINSs resolving attitudes are coupled calculating further and obtains DVL range rate error, method is simple, easily operates.

Accompanying drawing explanation

Fig. 1 is the flow chart of the determination DVL range rate error method of the present invention；

Fig. 2 is that the present invention utilizes visual c++ emulation to obtain ten estimation DVL range rate error curves；

Fig. 3 is that the present invention utilizes visual c++ emulation estimation range rate error to compensate front and back system resolving attitude error comparison curves；

In order to utilize the inventive method that sea trial is estimated, DVL range rate error compensates front and back system and resolves attitude comparison curves Fig. 4；

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

Detailed description of the invention

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

As it is shown in figure 1, the method determining Doppler log (DVL) range rate error in a kind of SINS provided by the invention, specifically include following steps.

Step 1: utilize the positional information that global location GPS system provides carrier initial, 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 preheats, gathers fibre optic gyroscope and the data of quartz accelerometer output；Relation according to accelerometer output with acceleration of gravity, and the relation of gyroscope output and rotational-angular velocity of the earth, it is determined that attitude of carrier angle, completion system is initially directed at, and sets up the initial strap-down matrix of inertial navigation system

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

Wherein, φ_x0、φ_y0、φ_z0Represent initial time carrier pitch angle, roll angle, course angle respectively.

Step 3: utilize inertia assembly to measure carrier angular movement and line movable information, carry out compass method navigation calculation.In compass solution process, in order to update compass parameter, introduce DVL and measure carrier movement velocity information.In formula below, subscript (C) represents compass method calculation result, and little footnote (I) represents inertial navigation method calculation result.

Gather gyroscope measurement data and update strap-down matrixSpecifically:

First angular velocity is updated:

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

Wherein, i represents Earth central inertial system, and e represents that terrestrial coordinate system, b represent that carrier coordinate system, n represent navigational coordinate system, and in the present invention, navigational coordinate system adopts local geographic coordinate system；Represent that b is tied to the strap-down matrix of n system, navigation calculation initial time, adopt utilization in step 2 to be initially directed at the initial strap-down matrix obtainedBelow in solution process, adopt the strap-down matrix after updating；Represent that the angular velocity of rotation of relative m system of p system projects in q system；For pilot angle speed in the n projection fastened；Project in n system for rotational-angular velocity of the earth；·^TRepresenting matrix transposition.Rotational-angular velocity of the earth is in the projection of navigation systemUpdate:

Ω=0004167 °/s, represents rotational-angular velocity of the earth；Representing and utilize DVL to test the speed the latitude of the calculated carrier position of pushing, its computational methods are:

Wherein,Represent the latitude utilizing the GPS initial time carrier position obtained in step 1, v_y(D)Represent that DVL tests the speed at navigational coordinate system oy_nComponent on axle；T represents current carrier hours underway；R represents earth radius.

Owing to carrier movement causes that the angular velocity of rotation that navigational coordinate system relatively spherical coordinate system changes projects in navigational coordinate systemUpdate according to following formula:

Wherein, v_x(D)Represent that DVL tests the speed at navigational coordinate system ox_nComponent on axle.

Pilot angle speedAt navigational coordinate system ox_nAxle, oy_nAxle, oz_nComponent on axleWithIt is updated to:

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

${\mathrm{\ω}}_{\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)Respectively compass method resolves carrier along navigational coordinate system ox_nAxle and oy_nThe movement velocity of axle；k₁、k₂、k_E、k_N、k_UFor compass parameter, the embodiment of the present invention arranges k₁=0.002828, k₂=0.002828, k₃=6.1218 × 10^-7、k_E=k_N=k_U=8.0429 × 10^-9.S represents complex field parameter.

Then adopt and update Quaternion Method renewal strap-down matrix

If the rotation quaternary 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)Four real numbers of quaternary number in resolving for compass method；i_b、j_bAnd k_bRepresent carrier coordinate system ox respectively_bAxle, oy_bAxle and oz_bUnit direction vector on axle.

The timely correction of quaternary number Q:

$\left[\begin{array}{c}{\stackrel{\·}{q}}_{0\left(C\right)}\\ {\stackrel{\·}{q}}_{1\left(C\right)}\\ {\stackrel{\·}{q}}_{2\left(C\right)}\\ {\stackrel{\·}{q}}_{3\left(C\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nbx}\left(C\right)}^b& -{\mathrm{\ω}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbz}\left(C\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nbx}\left(C\right)}^b& 0& {\mathrm{\ω}}_{\mathrm{nbz}\left(C\right)}^b& -{\mathrm{\ω}}_{\mathrm{nby}\left(C\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbz}\left(C\right)}^b& 0& {\mathrm{\ω}}_{\mathrm{nbx}\left(C\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}\left(C\right)}^b& {\mathrm{\ω}}_{\mathrm{nby}\left(C\right)}^b& -{\mathrm{\ω}}_{\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 in compass algorithm, the angular velocity of rotation of carrier coordinate system Relative Navigation coordinate system is at carrier coordinate system ox respectively_bAxle, oy_bAxle, oz_bComponent on axle.Represent q respectively_0(C)、q_1(C)、q_2(C)、q_3(C)Micro component.

Utilize the q obtained_0(C)、q_1(C)、q_2(C)、q_3(C)Update the strap-down matrix in 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)$

Update attitude of carrier information, specifically resolve the pitch angle φ of the carrier obtained through compass method_x(C), roll angle φ_y(C)With course angle φ_z(C)It is respectively as follows:

$\left\{\begin{array}{c}{\mathrm{\φ}}_{x\left(C\right)}=\arcsin \left(c_{33\left(C\right)}\right)\\ {\mathrm{\φ}}_{y\left(C\right)}=\arctan (c_{32\left(C\right)}/c_{31\left(C\right)})\\ {\mathrm{\φ}}_{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) representIn the i-th row jth column matrix element.

Accelerometer measures specific force is utilized to pass through strap-down matrixConversion:

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

Wherein, fⁿ、f^sRepresent that accelerometer measures specific force projects in n system and s system respectively.

Utilize following differential equation carrier movement speed:

$\left[\begin{array}{c}{\stackrel{\·}{v}}_{x\left(C\right)}\\ {\stackrel{\·}{v}}_{y\left(C\right)}\\ {\stackrel{\·}{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{\ω}}_{\mathrm{iez}\left(C\right)}^n& -(2{\mathrm{\ω}}_{\mathrm{iey}\left(C\right)}^n+{\mathrm{\ω}}_{\mathrm{eny}\left(C\right)}^n)\\ -{\mathrm{\ω}}_{\mathrm{iez}\left(C\right)}^n& 0& 2{\mathrm{\ω}}_{\mathrm{iex}\left(C\right)}^n+{\mathrm{\ω}}_{\mathrm{enx}\left(C\right)}^n\\ 2{\mathrm{\ω}}_{\mathrm{iey}\left(C\right)}^n+{\mathrm{\ω}}_{\mathrm{eny}\left(C\right)}^n& -(2{\mathrm{\ω}}_{\mathrm{iex}\left(C\right)}^n+{\mathrm{\ω}}_{\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 accelerometer measures specific force is at navigational coordinate system ox respectively_nAxle, oy_nAxle, oz_nComponent on axle；G is acceleration of gravity.WithRepresent rotational-angular velocity of the earth respectivelyAt navigational coordinate system ox_nAxle, oy_nAxle, oz_nComponent on axle.Represent that the angular velocity of rotation causing navigational coordinate system relatively spherical coordinate system to change due to carrier movement is at navigational coordinate system ox respectively_nAxle, oy_nProjection on axle.Represent v respectively_x(C)、v_y(C)、v_z(C)Micro component.

Step 4: while carrying out step 3, utilizes angular movement and the line movable information of the same group of carrier that inertia assembly is measured in step 3, adopts inertial navigation method to be navigated resolving.Detailed process is:

Gather gyroscope measurement data and update strap-down matrixFirst angular velocity is updated:

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

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

More new formula is as follows:

Wherein,Represent that inertial navigation method resolves the latitude of the carrier position obtained.

More new formula is as follows:

Wherein,Represent owing to carrier movement causes that angular velocity of rotation that navigational coordinate system relatively spherical coordinate system changes is in the projection of navigational coordinate system；v_x(I)、v_y(I)The carrier that inertial navigation method respectively resolves is ox along navigation_nAxle, oy_nMovement velocity on axle.R represents earth radius.

If the rotation quaternary 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 quaternary number.

The timely correction of quaternary number Q:

$\left[\begin{array}{c}{\stackrel{\·}{q}}_{0\left(I\right)}\\ {\stackrel{\·}{q}}_{1\left(I\right)}\\ {\stackrel{\·}{q}}_{2\left(I\right)}\\ {\stackrel{\·}{q}}_{3\left(I\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nbx}\left(I\right)}^b& -{\mathrm{\ω}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbz}\left(I\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nbx}\left(I\right)}^b& 0& {\mathrm{\ω}}_{\mathrm{nbz}\left(I\right)}^b& -{\mathrm{\ω}}_{\mathrm{nby}\left(I\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\ω}}_{\mathrm{nbz}\left(I\right)}^b& 0& {\mathrm{\ω}}_{\mathrm{nbx}\left(I\right)}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}\left(I\right)}^b& {\mathrm{\ω}}_{\mathrm{nby}\left(I\right)}^b& -{\mathrm{\ω}}_{\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 in inertial navigation method, the angular velocity of satellite motion of carrier coordinate system Relative Navigation coordinate system is at carrier coordinate system ox respectively_bAxle, oy_bAxle, oz_bComponent on axle.Represent q respectively_0(I)、q_1(I)、q_2(I)、q_3(I)Micro component.

Utilize the q obtained_0(I)、q_1(I)、q_2(I)、q_3(I)Update strap-down 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)$

Update attitude of carrier information, the concrete carrier pitch angle φ adopting the resolving of inertial navigation method to obtain_x(I), roll angle φ_y(I)With course angle φ_z(I)It is respectively as follows:

$\left\{\begin{array}{c}{\mathrm{\φ}}_{x\left(I\right)}=\arcsin \left(c_{33\left(I\right)}\right)\\ {\mathrm{\φ}}_{y\left(I\right)}=\arctan (c_{32\left(I\right)}/c_{31\left(I\right)})\\ {\mathrm{\φ}}_{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) representIn the i-th row jth column matrix element.

Accelerometer measures specific force is utilized to pass through strap-down matrixConversion:

$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{\·}{v}}_{x\left(I\right)}\\ {\stackrel{\·}{v}}_{y\left(I\right)}\\ {\stackrel{\·}{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{\ω}}_{\mathrm{iez}\left(I\right)}^n& -(2{\mathrm{\ω}}_{\mathrm{iey}\left(I\right)}^n+{\mathrm{\ω}}_{\mathrm{eny}\left(I\right)}^n)\\ -{\mathrm{\ω}}_{\mathrm{iez}\left(I\right)}^n& 0& 2{\mathrm{\ω}}_{\mathrm{iex}\left(I\right)}^n+{\mathrm{\ω}}_{\mathrm{enx}\left(I\right)}^n\\ 2{\mathrm{\ω}}_{\mathrm{iey}\left(I\right)}^n+{\mathrm{\ω}}_{\mathrm{eny}\left(I\right)}^n& -(2{\mathrm{\ω}}_{\mathrm{iex}\left(I\right)}^n+{\mathrm{\ω}}_{\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: the attitude angle that compass method and inertial navigation method resolve is done difference, sets up azimuthal misalignment angular difference value:

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

Wherein, γ_(I)、γ_(C)Represent the azimuthal misalignment angle that inertial navigation method and compass method resolve respectively.

Step 6: the azimuthal misalignment angular difference value obtained according to step 5, obtains DVL range rate error δ V further_D:

Beneficial effects of the present invention is verified as follows:

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

Carrier does three-axis swinging motion.Carrier is deviated from voyage route to angle with sinusoidal rule, pitch angle and roll angle are waved, and its mathematical model is:

Wherein, φ_x、φ_y、φ_zRepresent the pitch angle of carrier, roll angle and course angle respectively；φ_xm、π_ym、π_zmRepresent corresponding swing angle amplitude, φ respectively_xm=φ_ym=φ_zm=5 °；Represent the initial phase that pitch angle, roll angle and course angle are waved respectively,T_x、T_y、T_zRepresent the rolling period T of corresponding swinging shaft respectively_x=T_y=T_z=4s；K is true flight path, k=30 °.

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

Carrier at the uniform velocity sails 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 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 inertial reference calculation, system is operated in interior absolute damping state, damped coefficient 0.5；

Simulation time: t=48h；

Sample frequency: Hn=0.01s；

Utilize and invent described method, obtain ten estimation DVL range rate error results as in figure 2 it is shown, ten estimation result statistics are such as table 1, be illustrated in figure 3 ten estimation averages compensate before and after the comparison curves of system resolving attitudes.Result shows that the present invention can better estimate DVL range rate error.

Table 1DVL range rate error estimation result statistics

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

This test is the Sea Trials adopting fiber-optic gyroscope strapdown inertial navigation system to carry out, and this test utilizes the DVL being arranged on stem to provide it to measure carrier movement speed, and gyroscope inertial navigation system the key technical indexes used is as follows:

Dynamic range: ± 100 °/s；

Bias instaility :≤0.005 °/h；

Random walk:

Scale factory non-linearity degree :≤5ppm.

Utilizing the inventive method, obtain estimation DVL range rate error and compensate front and back system resolving attitude comparison curves as shown in Figure 4, carrier ship trajectory is as shown in Figure 5.Result shows that the present invention can better estimate DVL range rate error, improves system and resolves attitude accuracy.

Claims (3)

1. the method determining Doppler log (DVL) range rate error in a SINS, it is characterised in that comprise the following steps:

Step 1: utilize global location GPS system to provide the positional information of carrier initial time, and bind to navigational computer；Described positional information includes: the latitude of carrier initial time position and longitude；

Step 2: after fiber-optic gyroscope strapdown inertial navigation system preheats, gathers fibre optic gyroscope and the data of quartz accelerometer output, it is then determined that attitude of carrier angle, completion system is initially directed at, and sets up the initial strap-down matrix of SINS

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

Wherein, φ_x0、φ_y0And φ_z0Representing the pitch angle of initial time carrier, roll angle and course angle respectively, b represents carrier coordinate system, and n represents navigational coordinate system；

Step 3: utilize inertia assembly to measure angular movement and the line movable information of carrier, carry out compass method navigation calculation, update strap-down matrixAnd resolve the pitch angle φ obtaining carrier_x(C), roll angle φ_y(C)With course angle φ_z(C), subscript (C) represents that compass method resolves；

Step 4: utilize angular movement and the line movable information of the carrier that inertia assembly is measured in step 3, adopts inertial navigation method to be navigated resolving, updates strap-down matrixAnd resolve the pitch angle φ obtaining carrier_x(I), roll angle φ_y(I)With course angle φ_z(I), subscript (I) represents that inertial navigation method resolves；

Step 5: the attitude angle that compass method and inertial navigation method resolve is done difference, sets up azimuthal misalignment angular difference value:

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

Wherein, γ_(I)、γ_(C)Represent the azimuthal misalignment angle that inertial navigation method and compass method resolve respectively；

Step 6: utilize azimuthal misalignment angular difference value in step 5, it is determined that DVL range rate error δ V_D:

Wherein, R represents earth radius；Ω is rotational-angular velocity of the earth, and value is 0.004167 °/s；Represent and utilize DVL to test the speed the latitude of the calculated carrier position of pushing.

2. the method determining Doppler log range rate error in a kind of SINS according to claim 1, it is characterised in that carry out compass method navigation calculation described in step 3, detailed process is:

Angular velocity renewal is carried out initially with formula below:

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

I represents Earth central inertial system, and e represents terrestrial coordinate system；Represent the angular velocity of rotation projection in q system of relative m system of p system, m=n, i, e, p=b, e, n, q=b, n；It it is the rotational-angular velocity of the earth projection in n system；For pilot angle speed in n system

Projection；·^TRepresenting matrix transposition；InitiallyAdopt the initial strap-down matrix obtained in step 2

More new formula be: Represent and utilize DVL to test the speed the latitude of the calculated carrier position of pushing, Represent the latitude of carrier initial time position, v_y(D)Represent that DVL tests the speed at navigational coordinate system oy_nComponent on axle；T represents current carrier hours underway；R represents earth radius；

More new formula is:Wherein, v_x(D)Represent that DVL tests the speed at navigational coordinate system ox_nComponent on axle；

Pilot angle speedAt navigational coordinate system ox_nAxle, oy_nAxle and oz_nComponent on axleWithIt is updated to:

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

${\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)Respectively compass method resolves carrier along navigational coordinate system ox_nAxle and oy_nThe movement velocity of axle；S represents complex field parameter；Compass parameter k₁、k₂、k_E、k_NAnd k_UIt is set to: k₁=0.002828, k₂=0.002828, k_E=k_N=k_U=8.0429 × 10^-9；

Then adopt and update Quaternion Method renewal strap-down matrix

If the rotation quaternary 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)Four real numbers of quaternary number in resolving for compass method；i_b、j_bAnd k_bRepresent carrier coordinate system ox respectively_bAxle, oy_bAxle and oz_bUnit direction vector on axle；

The timely correction of quaternary number Q:

$\left[\begin{array}{c}{\stackrel{\·}{q}}_{0\left(C\right)}\\ {\stackrel{\·}{q}}_{1\left(C\right)}\\ {\stackrel{\·}{q}}_{2\left(C\right)}\\ {\stackrel{\·}{q}}_{3\left(C\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx\left(C\right)}^b& -{\mathrm{\ω}}_{nby\left(C\right)}^b& -{\mathrm{\ω}}_{nbz\left(C\right)}^b\\ {\mathrm{\ω}}_{nbx\left(C\right)}^b& 0& {\mathrm{\ω}}_{nbz\left(C\right)}^b& -{\mathrm{\ω}}_{nby\left(C\right)}^b\\ {\mathrm{\ω}}_{nby\left(C\right)}^b& -{\mathrm{\ω}}_{nbz\left(C\right)}^b& 0& {\mathrm{\ω}}_{nbx\left(C\right)}^b\\ {\mathrm{\ω}}_{nbz\left(C\right)}^b& {\mathrm{\ω}}_{nby\left(C\right)}^b& -{\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 the angular velocity of rotation of carrier coordinate system Relative Navigation coordinate system is at carrier coordinate system ox respectively_bAxle, oy_bAxle, oz_bComponent on axle；Represent q respectively_0(C)、q_1(C)、q_2(C)、q_3(C)Micro component；

Utilize the q obtained_0(C)、q_1(C)、q_2(C)、q_3(C)Update the strap-down matrix in 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_{2\left(C\right)}^2-q_{2\left(C\right)}^2+q_{3\left(C\right)}^2\end{array}\right]$

The pitch angle φ of the carrier obtained is resolved through compass method_x(C), roll angle φ_y(C)With course angle φ_z(C)For:

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

Wherein, c_ij(C)RepresentIn the i-th row jth column matrix element, i=1,2,3, j=1,2,3.

3. the method determining Doppler log range rate error in a kind of SINS according to claim 1, it is characterised in that the employing inertial navigation method described in step 4 is navigated resolving, and detailed process is:

First angular velocity is updated:

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

Wherein, i represents Earth central inertial system, and e represents terrestrial coordinate system；InitiallyAdopt the initial strap-down matrix obtained in step 2Represent the angular velocity of rotation projection in q system of relative m system of p system, m=n, i, e, p=b, e, n, q=b, n；It it is the rotational-angular velocity of the earth projection in n system；

More new formula is: Represent that inertial navigation method resolves the latitude of the carrier position obtained；

More new formula is:Wherein, v_x(I)、v_y(I)The carrier that inertial navigation method respectively resolves is ox along navigation_nAxle, oy_nMovement velocity on axle, R represents earth radius；

Then strap-down matrix is updated

If the rotation quaternary number Q of carrier coordinate system opposed platforms 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)Four real numbers of quaternary number, i in resolving for inertial navigation method_b、j_bAnd k_bRepresent carrier coordinate system ox respectively_bAxle, oy_bAxle and oz_bUnit direction vector on axle；

The timely correction of quaternary number Q:

$\left[\begin{array}{c}{\stackrel{\·}{q}}_{0\left(I\right)}\\ {\stackrel{\·}{q}}_{1\left(I\right)}\\ {\stackrel{\·}{q}}_{2\left(I\right)}\\ {\stackrel{\·}{q}}_{3\left(I\right)}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{nbx\left(I\right)}^b& -{\mathrm{\ω}}_{nby\left(I\right)}^b& -{\mathrm{\ω}}_{nbz\left(I\right)}^b\\ {\mathrm{\ω}}_{nbx\left(I\right)}^b& 0& {\mathrm{\ω}}_{nbz\left(I\right)}^b& -{\mathrm{\ω}}_{nby\left(I\right)}^b\\ {\mathrm{\ω}}_{nby\left(I\right)}^b& -{\mathrm{\ω}}_{nbz\left(I\right)}^b& 0& {\mathrm{\ω}}_{nbx\left(I\right)}^b\\ {\mathrm{\ω}}_{nbz\left(I\right)}^b& {\mathrm{\ω}}_{nby\left(I\right)}^b& -{\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 the angular velocity of satellite motion of carrier coordinate system Relative Navigation coordinate system is at carrier coordinate system ox respectively_bAxle, oy_bAxle, oz_bComponent on axle；Represent q respectively_0(I)、q_1(I)、q_2(I)、q_3(I)Micro component；

Utilize the q obtained_0(I)、q_1(I)、q_2(I)、q_3(I)Update strap-down 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_{2\left(I\right)}^2-q_{2\left(I\right)}^2+q_{3\left(I\right)}^2\end{array}\right]$

Inertial navigation method is adopted to resolve the pitch angle φ of the carrier obtained_x(I), roll angle φ_y(I)With course angle φ_z(I)For:

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

Wherein, c_ij(I)RepresentIn the i-th row jth column matrix element, i=1,2,3, j=1,2,3.