CN102003967A - Compass principle-based strapdown inertial navigation bearing alignment method for rotary ship - Google Patents

Abstract

The invention aims to provide a compass principle-based strapdown inertial navigation bearing alignment method for a rotary ship. The method comprises the following steps of: setting a rotary frequency, selecting a system undamped frequency and a system damping coefficient to obtain the required alignment time; carrying out coarse alignment initialization to obtain a posture array and a posture quaternion; horizontally placing a strapdown inertial navigation for the rotary ship on a ship; ensuring that an IMU (Inertial Measurement Unit) rotates by using a turntable; simultaneously measuring an acceleration of an IMU system by utilizing an accelerometer; measuring the projection of the IMU system relative to an angular rate of an inertial system under the IMU system; transforming the acceleration of a carrier system to a platform system through a posture matrix; updating and calculating the posture matrix of the current moment by utilizing a quaternion method to obtain a pitching angle, a roll angle and a yaw angle of the system; repeating the steps and entering the cycle of the next time; outputting the posture of a carrier in each system period until the time obtained by calculation is reached; and completing the alignment.

Description

Inertial navigation alignment of orientation method rotary peculiar to vessel based on the compass principle

Technical field

What the present invention relates to is a kind of alignment methods of navigation field.

Background technology

The initial alignment technology is one of gordian technique of inertial navigation system, and it directly influences the navigation performance of system.The high precision initial alignment is a navigation field, particularly the difficult problem in marine navigation field.Improving the simplest method of alignment precision is to improve the device precision, yet improves precision by increasing the hardware input, is difficult to obtain in a short time remarkable effect.Therefore, how the mode by system optimization improves the research direction that alignment precision becomes to attach most importance to.Mode with estimation of error and compensation improves the hot topic that alignment precision becomes research now.Yet device estimation of error technology need be set up device error model accurately, and this method increased the calculated amount of system greatly, is subjected to certain limitation in actual applications.

The alignment of orientation of compass method is a kind of inertial navigation system Alignment Method, and the precision of its aligning is main to be determined by precision of gyroscope.The main factor that influences the alignment of orientation precision is the gyroscopic drift on the geographical east orientation, has a bigger normal value deviation because east orientation often is worth the federation that has alignment of orientation of gyroscopic drift.The rotation modulation technique can automatically be modulated the gyroscope constant value drift and the accelerometer error of zero, becomes the deviation that normal value deviation changed into the cycle.Strapdown inertial navitation system (SINS) is carrying out the normal value gyroscopic drift of east orientation to be changed into the drift that the cycle changes under the state of single shaft rotation to axle around the sky.

Summary of the invention

The object of the present invention is to provide a kind of inertial navigation alignment of orientation method rotary peculiar to vessel that under the prerequisite that does not improve the device precision, can significantly improve the alignment of orientation precision.

The object of the present invention is achieved like this:

The present invention is based on the inertial navigation alignment of orientation method rotary peculiar to vessel of compass principle, it is characterized in that:

(1) gyro frequency is set, makes the IMU rotational speed satisfy following formula

ω _z·ΔT＝δψ

ω wherein _zBe system's single shaft rotational speed, Δ T is the systematic sampling cycle, and δ ψ represents the evaluation coefficient of dynamic deferred error;

(2) according to the rotational speed omega of setting in the step (1) _z, the selecting system undamped frequency

Getting system damping coefficient ξ is 0.707, obtains the required aligning time

$T=\frac{2\mathrm{\π}}{{\mathrm{\ω}}_n\sqrt{1-{\mathrm{\ξ}}^2}};$

(3) it is initial to carry out coarse alignment, obtains the attitude battle array With corresponding attitude quaternion Q;

(4) aboard ship, with rotary inertial navigation horizontal positioned peculiar to vessel utilize turntable make IMU around the Z axle with ω _zAngular speed rotation, use acceleration measuring to measure the acceleration f of IMU system simultaneously _b, measure IMU system with respect to the projection of the angular speed under the inertial system in IMU system

Make that IMU is that b system, inertia are i system, then

${\mathrm{\ω}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ {\mathrm{\ω}}_{\mathrm{iby}}^b\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]$ $f_b=\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right];$

(5) using the carrier that records in the step (4) is that acceleration is through attitude matrix

Being transformed into platform fastens

$f_p=C_b^p{f}_b;$

(6) utilize the f that obtains in the step (5) _pCalculate of the projection of correction angle speed in platform system

(7) by

Obtain the angular speed of carrier system to mathematical platform system

${\mathrm{\ω}}_{\mathrm{pb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_p^b\left({\mathrm{\ω}}_{\mathrm{ie}}^p\right)-C_p^b\left({\mathrm{\ω}}_c^p\right),$

Wherein

Ω is the earth rotation angular speed,

Be local latitude;

(8) utilize and provided in the step (7)

Use the attitude matrix in hypercomplex number method this moment of update calculation

(9) utilize gained attitude matrix in the step (8)

Obtain current attitude

$\mathrm{\θ}=\arcsin C_{b32}^p$

$\mathrm{\γ}=\arctan \frac{{-C}_{b31}^p}{C_{b33}^p}$

$\mathrm{\ψ}=\arctan \frac{{-C}_{b12}^p}{C_{b22}^p}$

Then with three angle modification of gained:

Revised θ, γ and ψ promptly are respectively the angle of pitch, roll angle and the crab angle of system;

(10) repeating step (4) to step (9) enters the circulation of next time, the attitude of output carrier in each system cycle, and the time T of being calculated in reaching step (2) is aimed at and is finished.

Advantage of the present invention is: adopt the single shaft spinning solution that east orientation gyroscopic drift is modulated to periodic quantity, change the frequency of error input, carrying out compass on this basis aims at, system self promptly can effectively suppress the influence of gyroscope constant value drift, need not the compensating device error and can significantly improve the alignment of orientation precision.

Description of drawings

Fig. 1 is a process flow diagram of the present invention;

The schematic diagram of Fig. 2 for realizing that rotary inertial navigation compass method peculiar to vessel is aimed at;

Fig. 3 is the schematic diagram of correction angle rate calculations part among Fig. 2;

Proof diagram in Fig. 4 specific embodiment of the invention.

Embodiment

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

In conjunction with Fig. 1～3, the present invention is based on the inertial navigation alignment of orientation method rotary peculiar to vessel of compass principle, it is characterized in that:

(1) gyro frequency is set, makes the IMU rotational speed satisfy following formula

ω _z·ΔT＝δψ

ω wherein _zBe system's single shaft rotational speed, Δ T is the systematic sampling cycle, and δ ψ represents the evaluation coefficient of dynamic deferred error;

(2) according to the rotational speed omega of setting in the step (1) _z, the selecting system undamped frequency

Getting system damping coefficient ξ is 0.707, obtains the required aligning time

$T=\frac{2\mathrm{\π}}{{\mathrm{\ω}}_n\sqrt{1-{\mathrm{\ξ}}^2}};$

(3) it is initial to carry out coarse alignment, obtains the attitude battle array With corresponding attitude quaternion Q;

(4) aboard ship, with rotary inertial navigation horizontal positioned peculiar to vessel utilize turntable make IMU around the Z axle with ω _zAngular speed rotation, use acceleration measuring to measure the acceleration f of IMU system simultaneously _b, measure IMU system with respect to the projection of the angular speed under the inertial system in IMU system

Make that IMU is that b system, inertia are i system, then

${\mathrm{\ω}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ {\mathrm{\ω}}_{\mathrm{iby}}^b\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]$ $f_b=\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right];$

(5) using the carrier that records in the step (4) is that acceleration is through attitude matrix

Being transformed into platform fastens

$f_p=C_b^p{f}_b;$

(6) utilize the f that obtains in the step (5) _pCalculate of the projection of correction angle speed in platform system

(7) by

Obtain the angular speed of carrier system to mathematical platform system

${\mathrm{\ω}}_{\mathrm{pb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_p^b\left({\mathrm{\ω}}_{\mathrm{ie}}^p\right)-C_p^b\left({\mathrm{\ω}}_c^p\right),$

Wherein

Ω is the earth rotation angular speed,

Be local latitude;

(8) utilize and provided in the step (7)

Use the attitude matrix in hypercomplex number method this moment of update calculation

(9) utilize gained attitude matrix in the step (8)

Obtain current attitude

$\mathrm{\θ}=\arcsin C_{b32}^p$

$\mathrm{\γ}=\arctan \frac{{-C}_{b31}^p}{C_{b33}^p}$

$\mathrm{\ψ}=\arctan \frac{{-C}_{b12}^p}{C_{b22}^p}$

Then with three angle modification of gained:

Revised θ, γ and ψ promptly are respectively the angle of pitch, roll angle and the crab angle of system;

(10) repeating step (4) to step (9) enters the circulation of next time, the attitude of output carrier in each system cycle, and the time T of being calculated in reaching step (2) is aimed at and is finished.

In the described step (2), computing system parameter and the process of the time of aligning are:

Step (2a), for there is very strong inhibiting effect in the full system that makes to gyroscopic drift, be unlikely to make the stabilization time of system long again, press following formula selecting system undamped frequency ω _n:

${\mathrm{\ω}}_n={10}^{\frac{1}{4}}\·{\mathrm{\ω}}_z$

Step (2b), system damping coefficient ξ is taken as 0.707, then with ξ and ω _nThe substitution following formula calculates each systematic parameter:

k ₁＝k ₂＝2ξω _n

$k_3=\frac{{\mathrm{\ξ}}^2{\mathrm{\ω}}_n^4}{g}$

$k_e=k_n=\frac{R({\mathrm{\ξ}}^2{\mathrm{\ω}}_n^2+{\mathrm{\ω}}_n^2)}{g}$

Wherein, R is an earth radius, and g is an acceleration of gravity, and Ω is the earth rotation angular speed,

Be local latitude.k ₁, k ₂, k ₃, k _e, k _n, k _uBe systematic parameter.

Step (2c), calculate and aim at required time T

$T=\frac{2\mathrm{\π}}{{\mathrm{\ω}}_n\sqrt{1-{\mathrm{\ξ}}^2}}$

In the 3 described steps of explanation (6), the process that obtains correction angle speed is in conjunction with the accompanying drawings:

Step (6a), separate the differential equation and calculate velocity equivalent error delta V _xWith δ V _y

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_x=f_{\mathrm{px}}-{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">k</figure-callout>}}_1{\mathrm{\&delta;V}}_x$

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_y=f_{\mathrm{py}}-{\mathrm{<figure-callout\; id="1"\; label="k"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">k</figure-callout>}}_1\mathrm{\&delta;}{V}_y$

δ V wherein _xWith δ V _yThe value in the first moment be zero.Its computing method adopt the single order recurrence method, k+1 point δ V constantly _xWith δ V _yComputation process is as follows:

δV _x(t _k+1)＝[f _px-k ₁δV _x(t _k)]ΔT+δV _x(t _k)

δV _y(t _k+1)＝[f _py-k ₁δV _y(t _k)]ΔT+δV _y(t _k)

Calculate the δ V of gained in step (6b), the use step (6a) _xWith δ V _y, further calculate three correction angle speed.

Wherein, east orientation correction angle speed is directly calculated by following formula

${\mathrm{\&omega;}}_{\mathrm{cx}}^p=-k_n\&CenterDot;\frac{{\mathrm{\&delta;V}}_y}{R}$

Calculate the sky and need separate the following differential equation to correction angle speed

${\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_{\mathrm{cz}}^p=k_u\mathrm{\&delta;}{V}_y-k_2{\mathrm{\&omega;}}_{\mathrm{cz}}^p$

Wherein, The initial time value is zero.Its computing method adopt the single order recurrence method, the k+1 point moment

Computation process is as follows:

${\mathrm{\&omega;}}_{\mathrm{cz}}^p(t_k+1)=[k_u\mathrm{\&delta;}{V}_y\left(t_k\right)-k_2{\mathrm{\&omega;}}_{\mathrm{cz}}^p\left(t_k\right)]\mathrm{\&Delta;T}+{\mathrm{\&omega;}}_{\mathrm{cz}}^p\left(t_k\right)$

The calculating of north orientation correction angle speed is divided into two steps:

At first, utilize δ V _xSeparate the following differential equation and calculate intermediate variable d _y

${\stackrel{\&CenterDot;}{d}}_y=k_3\mathrm{\&delta;}{V}_x-k_2{d}_y$

Wherein, d _yThe initial time value is zero.K+1 point d constantly _yComputation process is as follows:

d _y(t _k+1)＝[k ₃δV _x(t _k)-k ₂d _y(t _k)]ΔT+d _y(t _k)

And then utilize δ V _xWith intermediate variable d _yCalculate north orientation correction angle speed, the k+1 point moment Algorithm is as follows:

${\mathrm{\&omega;}}_{\mathrm{cy}}^p\left(t_{k+1}\right)=d_y\left(t_k\right)+{\mathrm{\&omega;}}_{\mathrm{cy}}^p\left(t_k\right)+k_e\&CenterDot;\frac{\mathrm{\&delta;}{V}_x\left(t_k\right)}{R}$

In the described step (8), the principle of update calculation attitude matrix is:

$\stackrel{\&CenterDot;}{Q}=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{pb}}^b\&CenterDot;Q$

Wherein

$Q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$ ${\mathrm{\&Omega;}}_{\mathrm{pb}}^b=\left[\begin{array}{cccc}0& {-\mathrm{\&omega;}}_{\mathrm{pbx}}^b& -{\mathrm{\&omega;}}_{\mathrm{pby}}^b& -{\mathrm{\&omega;}}_{\mathrm{pbz}}^b\\ {\mathrm{\&omega;}}_{\mathrm{pbx}}^b& & {\mathrm{\&omega;}}_{\mathrm{pbz}}^b& {-\mathrm{\&omega;}}_{\mathrm{pby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{pby}}^b& -{\mathrm{\&omega;}}_{\mathrm{pbz}}^b& & {\mathrm{\&omega;}}_{\mathrm{pbx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{pbz}}^b& {\mathrm{\&omega;}}_{\mathrm{pby}}^b& -{\mathrm{\&omega;}}_{\mathrm{pbx}}^b& \end{array}\right]$

Its solution adopts fourth-order Runge-Kutta method, and computation process is as follows:

Step (8a), at first calculate the slope X of first point ₁With value Y based on first point of this slope ₁

${\mathrm{<figure-callout\; id="1"\; label="X"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">X</figure-callout>}}_1=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{pb}}^b\left(t_k\right)Q\left(t_k\right)$

${\mathrm{<figure-callout\; id="1"\; label="Y"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">Y</figure-callout>}}_1=Q\left(t_k\right)+\frac{\mathrm{\&Delta;T}}{2}\&CenterDot;{\mathrm{<figure-callout\; id="1"\; label="X"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">X</figure-callout>}}_1$

Step (8b), calculate the slope X of second point ₂With value Y based on first point of this slope ₂

$X_2=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{pb}}^b(t_k+\frac{\mathrm{\&Delta;T}}{2}){\mathrm{<figure-callout\; id="1"\; label="Y"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">Y</figure-callout>}}_1$

$Y_2={\mathrm{<figure-callout\; id="1"\; label="Y"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">Y</figure-callout>}}_1+\frac{\mathrm{\&Delta;T}}{2}\&CenterDot;X_2$

Step (8c), calculate the slope X of the 3rd point ₃With value Y based on first point of this slope ₃

$X_3=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{pb}}^b(t_k+\frac{\mathrm{\&Delta;T}}{2})Y_2$

$Y_3=Y_2+\frac{\mathrm{\&Delta;T}}{2}\&CenterDot;X_3$

Step (8d), calculate the slope X of the 4th point ₄And the k+1 Q of ordering.

$X_4=\frac{1}{2}{\mathrm{\&Omega;}}_{\mathrm{pb}}^b(t_k+\mathrm{\&Delta;T})Y_3$

Step (8e), try to achieve optimum slope value X ₂

$X=\frac{1}{6}({\mathrm{<figure-callout\; id="1"\; label="X"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">X</figure-callout>}}_1+{2X}_2+{2X}_3+X_4)$

Q _k+1＝Q _k+XΔT

Step (8f), will calculate the normalization of gained hypercomplex number.

$\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{\sqrt{q_0^2+{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+q_2^2+q_3^2}}\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$

Step (8g), upgrade attitude matrix then

$C_p^b=\left[\begin{array}{ccc}{q}_0^2+{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HSA00000254273400022.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2-q_2^2+q_3^2\end{array}\right]$

Use experimental verification the method for the invention below:

As shown in Figure 4, establishing three gyroscopic drifts is 0.05deg/h, and accelerometer bias is 10 ^-4G (g is an acceleration of gravity), Δ T got 0.01 second, 1 ° of coarse alignment course error.What wherein curve 1 showed is the azimuth angle error that quiet pedestal compass method is aimed at; Curve 2 shows is the azimuth angle error of not using the compass method alignment system employing rotary process aligning that this patent design; The azimuth angle error (δ ψ gets 0.02 °) that rotation compass method after the time process that curve 3 shows designs is aimed at.As seen after passing through the design of system, the method for designing of this patent can effectively suppress the influence of gyroscopic drift, and the alignment of orientation precision significantly improves.

Claims (1)

1. based on the inertial navigation alignment of orientation method rotary peculiar to vessel of compass principle, it is characterized in that:

(1) gyro frequency is set, makes the IMU rotational speed satisfy following formula

ω _z·ΔT＝δψ

ω wherein _zBe system's single shaft rotational speed, Δ T is the systematic sampling cycle, and δ ψ represents the evaluation coefficient of dynamic deferred error;

(2) according to the rotational speed omega of setting in the step (1) _z, the selecting system undamped frequency

Getting system damping coefficient ξ is 0.707, obtains the required aligning time

$T=\frac{2\mathrm{\&pi;}}{{\mathrm{\&omega;}}_n\sqrt{1-{\mathrm{\&xi;}}^2}};$

(3) it is initial to carry out coarse alignment, obtains the attitude battle array

With corresponding attitude quaternion Q;

(4) aboard ship, with rotary inertial navigation horizontal positioned peculiar to vessel utilize turntable make IMU around the Z axle with ω _zAngular speed rotation, use acceleration measuring to measure the acceleration f of IMU system simultaneously _b, measure IMU system with respect to the projection of the angular speed under the inertial system in IMU system

Make that IMU is that b system, inertia are i system, then

${\mathrm{\&omega;}}_{\mathrm{ib}}^b=\left[\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b\end{array}\right]$ $f_b=\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right];$

(5) using the carrier that records in the step (4) is that acceleration is through attitude matrix Being transformed into platform fastens

$f_p=C_b^p{f}_b;$

(6) utilize the f that obtains in the step (5) _pCalculate of the projection of correction angle speed in platform system

(7) by

Obtain the angular speed of carrier system to mathematical platform system

${\mathrm{\&omega;}}_{\mathrm{pb}}^b={\mathrm{\&omega;}}_{\mathrm{ib}}^b-C_p^b\left({\mathrm{\&omega;}}_{\mathrm{ie}}^p\right)-C_p^b\left({\mathrm{\&omega;}}_c^p\right),$

Wherein

Ω is the earth rotation angular speed,

Be local latitude;

(8) utilize and provided in the step (7)

Use the attitude matrix in hypercomplex number method this moment of update calculation

(9) utilize gained attitude matrix in the step (8)

Obtain current attitude

$\mathrm{\&theta;}=\arcsin C_{b32}^p$

$\mathrm{\&gamma;}=\arctan \frac{{-C}_{b31}^p}{C_{b33}^p}$

$\mathrm{\&psi;}=\arctan \frac{{-C}_{b12}^p}{C_{b22}^p}$

Then with three angle modification of gained:

Revised θ, γ and ψ promptly are respectively the angle of pitch, roll angle and the crab angle of system;

(10) repeating step (4) to step (9) enters the circulation of next time, the attitude of output carrier in each system cycle, and the time T of being calculated in reaching step (2) is aimed at and is finished.

Images