Strapdown Inertial Navigation Heave Measurement Method Using Multiple Low-Pass Filter Units

Abstract

The disclosure discloses a strapdown inertial navigation heave measurement method using multiple low-pass filter units, including: firstly, collecting data of a gyroscope and an accelerometer by a system, obtaining attitude information of a carrier by using initial alignment, and then, obtaining a relationship matrix between a body coordinate system and a geographic coordinate system by using the attitude information; obtaining a relationship matrix between the geographic coordinate system and a semi-fixed coordinate system according to a geographic position, and obtaining a rough vertical acceleration by using a direction cosine matrix, output information of the accelerometer and gravity information; then, filtering out low-frequency signals by using a double-filter unit and one integral link to obtain a relatively accurate velocity signal; and furthermore, enabling the vertical acceleration to be subjected to a triple-filter unit and two integral links to obtain an accurate heave displacement. The method avoids the problem of phase lead caused by traditional addition of high-pass filters, and can provide a reference for ship swaying reduction operations, ship carrier lifts, ship-borne weapon launch and heave compensation of various offshore platforms.

Claims

1. A strapdown inertial navigation heave measurement method using multiple low-pass filter units, comprising the following steps: step 1: fully preheating a motion attitude reference system, and collecting output signals of a gyroscope and an accelerometer on three axes of a body coordinate system b in real time; step 2: measuring real-time attitude information of a ship, comprising a pitch angle θ, a roll angle γ and a yaw angle φ of the body coordinate system relative to a navigation coordinate system (n system), by using the output signal of the gyroscope, so as to obtain an attitude matrix C.sub.b.sup.n; step 3: calculating an acceleration v&.sup.n of the ship according to a sensitive acceleration signal of the accelerometer by using a navigation solution method; step 4: converting the v&.sup.n to a semi-fixed coordinate system by using a direction cosine matrix C.sub.n.sup.d between the navigation system (n system) and a semi-fixed coordinate system (d system), so as to obtain v&.sup.d; step 5: extracting a z-axis component v&.sub.z.sup.d of the v&.sup.d obtained in step 4 in a vertical direction of the d system; step 6: enabling the v&.sub.z.sup.d to be subjected to a double-filter unit and an integral link to obtain a heave velocity v&.sub.z.sup.d; and step 7: enabling the v&.sub.z.sup.d to be subjected to a triple-filter unit and two integral links to obtain a heave displacement p.sub.z.sup.d.

2. The strapdown inertial navigation heave measurement method using multiple low-pass filter units according to claim 1, wherein a transfer function G.sub.v(s) of the double-filter unit in step 6 is: $G_v\left(s\right)=\frac{s^4+4\zeta {\Omega }_p{s}^2+4{\zeta }^2{\Omega }_p^2}{{\left(s^2+2\zeta {\Omega }_{p}s+{\Omega }_p^2\right)}^2},$ wherein ζ represents a damping coefficient, and Ω.sub.p represents a passband cutoff frequency.

3. The strapdown inertial navigation heave measurement method using multiple low-pass filter units according to claim 1, wherein a transfer function G.sub.p(s) of the triple-filter unit in step 7 is: $G_p\left(s\right)=\frac{{\left(s^2+2\zeta {\Omega }_{p}s\right)}^3}{{\left(s^2+2\zeta {\Omega }_{p}s+{\Omega }_p^2\right)}^3},$ wherein ζ represents a damping coefficient, and Ω.sub.p represents a passband cutoff frequency.

Description

BRIEF DESCRIPTION OF FIGURES

[0023] FIG. 1 shows a flow chart for implementing the disclosure.

[0024] FIG. 2 shows a comparison chart of a heave velocity and a true velocity.

[0025] FIG. 3 shows a heave velocity error.

[0026] FIG. 4 shows a comparison chart of a heave displacement and a true displacement.

[0027] FIG. 5 shows a heave displacement error.

DETAILED DESCRIPTION

[0028] The disclosure will be further described below with reference to the accompanying drawings of the specification and specific implementations.

[0029] In the disclosure, firstly, data of a gyroscope and an accelerometer are collected by a system. Attitude information of a carrier is obtained by initial alignment. Then, an attitude matrix between a body coordinate system and a navigation coordinate system is obtained by using the attitude information. A direction cosine matrix between the navigation coordinate system and a semi-fixed coordinate system is obtained according to a geographic position. A rough vertical acceleration is obtained by using the direction cosine matrix, output information of the accelerometer and gravity information. Then, low-frequency signals are filtered out by a double-filter unit and one integral link to obtain a relatively accurate velocity signal. Furthermore, the vertical acceleration is subjected to a triple-filter unit and two integral links to obtain an accurate heave displacement. The method avoids the problem of phase lead caused by traditional addition of high-pass filters, and can provide a reference for ship swaying reduction operations, ship carrier lifts, ship-borne weapon launch and heave compensation of various offshore platforms. The method mainly includes the following steps:

[0030] Step 1: A motion attitude reference system is fully preheated, and output signals of a gyroscope and an accelerometer on three axes of a body coordinate system are collected in real time.

[0031] Step 2: Real-time attitude information of a ship is measured by using the output signal of the gyroscope, so as to obtain an attitude matrix C.sub.b.sup.n.

[0032] Step 3: An acceleration-v&.sup.n of the ship is obtained by the attitude matrix C.sub.b.sup.n, where

[00003]$C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$

[0033] C.sub.11=cos γ cos φ+sin γ sin φ sin θ,

[0034] C.sub.12=sin φ cos θ,

[0035] C.sub.13=sin γ cos φ−cos γ sin φ sin θ,

[0036] C.sub.21=cos γ sin φ+sin γ cos φ sin θ,

[0037] C.sub.22=cos φ cos θ,

[0038] C.sub.23=−sin γ sin φ+cos γ cos φ sin θ,

[0039] C.sub.31=−sin γ cos θ,

[0040] C.sub.32=sin θ,

[0041] C.sub.33=cos γ cos θ,

[0042] where θ, γ, φ respectively represent a pitch angle, a roll angle and a yaw angle of the ship.

[0043] Step 4: The v&.sup.n is converted to a d system by using a direction cosine matrix C.sub.n.sup.d between a navigation system (n system) and a semi-fixed coordinate system (d system), so as to obtain v&.sup.d, where

[00004]$C_n^d=\left[\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -s\mathrm{in}\psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right],$

[0044] where ψ represents a main heading angle of the ship.

[0045] Step 5: A z-axis component v&.sub.z.sup.d of the v&.sup.d obtained in step 4 in a vertical direction of the d system is extracted.

[0046] Step 6: The v&.sub.z.sup.d is subjected to a double-filter unit and an integral link to obtain a relatively accurate heave velocity v&.sub.z.sup.d, where a transfer function of the integral link is G(s)=1/s, and a transfer function G.sub.v(s) of the double-filter unit is:

[00005]$G_v\left(s\right)=\frac{s^4+4\zeta {\Omega }_p{s}^2+4{\zeta }^2{\Omega }_p^2}{{\left(s^2+2\zeta {\Omega }_{p}s+{\Omega }_p^2\right)}^2},$

[0047] where ζ represents a damping coefficient and is generally 0.707, and Ω.sub.p represents a passband cutoff frequency.

[0048] Step 7: The v&.sub.z.sup.d is subjected to a triple-filter unit and two integral links to obtain an accurate heave displacement p.sub.z.sup.d, where a transfer function G.sub.p(s) of the triple-filter unit is:

[00006]$G_p\left(s\right)=\frac{{\left(s^2+2\zeta {\Omega }_{p}s\right)}^3}{{\left(s^2+2\zeta {\Omega }_{p}s+{\Omega }_p^2\right)}^3}.$

[0049] With reference to FIG. 1, the disclosure includes the following steps:

[0050] Step 1: A motion attitude reference system is fully preheated, and output signals of a gyroscope and an accelerometer on three axes of a body coordinate system b are collected in real time.

[0051] Step 2: Real-time attitude information of a ship, including a pitch angle θ, a roll angle γ and a yaw angle φ, is measured by using the output signal of the gyroscope.

[0052] Step 3: A direction cosine matrix C.sub.b.sup.n between the body coordinate system (b system) and a navigation system (n system) is calculated according to the attitude information, where an expression of the direction cosine matrix C.sub.b.sup.n is:

[00007]$C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$

[0053] where

[0054] C.sub.11=cos γ cos φ+sin γ sin φ sin θ,

[0055] C.sub.12=sin φ cos θ,

[0056] C.sub.13=sin γ cos φ−cos γ sin φ sin θ,

[0057] C.sub.21=−cos γ sin φ+sin γ cos φ sin θ,

[0058] C.sub.22=cos φ cos θ,

[0059] C.sub.23=−sin γ sin φ+cos γ cos φ sin θ,

[0060] C.sub.31=— sin γ cos

[0061] C.sub.32=sin θ,

[0062] C.sub.33 cos γ cos θ,

[0063] Step 4: An acceleration v&.sup.n of the ship is calculated by collecting the data of a sensor installed at the center of gravity of a ship hull in real time by a navigation solution method, and the acceleration of the ship is calculated by a sensitive acceleration signal of the accelerometer through the following formula:

v&.sup.nC.sub.b.sup.nf.sub.sf.sup.b−(2ω.sub.ie.sup.n+ω.sub.en.sup.n)×v.sup.n+g.sub.n,

[0064] where v&.sup.n represents an acceleration of the ship, C.sub.b.sup.nf.sub.sf.sup.b represents a specific force in an up direction obtained after coordinate transformation of a proportional vector of the ship by a ship attitude array obtained by a strapdown inertial navigation system, ω.sub.ie.sup.n represents a Coriolis compensation term, ω.sub.en.sup.n represents a rotational angular velocity of the earth, v.sup.n represents a navigation velocity of the ship, g.sup.n represents a gravitational acceleration of a local ship. In a non-navigation state of the ship, the second term in the formula is 0. That is, v&.sup.n=C.sub.b.sup.nf.sub.sf.sup.b+g.sup.n.

[0065] Step 5: A direction cosine matrix C.sub.n.sup.d between then system and the semi-fixed coordinate system (d system) is obtained according to an included angle between a geographic north direction and a track of the ship at an instantaneous state during navigation and control, that is, a main heading angle ψ of the ship. A specific expression is:

[00008]$C_n^d=\left[\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right].$

[0066] Step 6: The v&.sup.n is converted to the d system by the direction cosine matrix C.sub.n.sup.d obtained in step 5 to obtain v&.sup.d. A specific calculation formula is as follows:

v&.sup.d=C.sub.n.sup.d(C.sub.b.sup.nf.sub.sf.sup.bg.sub.n)=C.sub.b.sup.df.sub.sf.sup.bg.sub.d,

[0067] where g.sup.d represents a projection of the gravitational acceleration at the location of a carrier on the d system.

[0068] Step 7: A projection v&.sub.z.sup.d of the acceleration v&.sup.d on a vertical axis z of the d system is extracted.

[0069] Step 8: The v&.sub.z.sup.d is subjected to a double-filter unit and one integral link to obtain a relatively accurate heave velocity v.sub.z.sup.d, where a transfer function of the integral link is G(s)=1/s, and a transfer function G.sub.v(s) of the double-filter unit is:

[00009]$G_v\left(s\right)=\frac{s^4+4\zeta {\Omega }_p{s}^2+4{\zeta }^2{\Omega }_p^2}{{\left(s^2+2\zeta {\Omega }_{p}s+{\Omega }_p^2\right)}^2},$

[0070] where ζ represents a damping coefficient and is generally 0.707, and Ω.sub.p represents a passband cutoff frequency.

[0071] Step 9: According to an ocean frequency range of 0.04-0.2 Hz at a high-frequency part and a Schuler period frequency range of 0˜1.97470×10.sup.−4 Hz at a low-frequency band, and based on the principle of increasing the passband accuracy as much as possible, a passband cutoff frequency Ω.sub.p may be calculated.

[0072] Step 10: A bilinear transformation method in a z transform is used for converting the above-mentioned analog filter into a digital filter under discrete signals.

[0073] Step 11: The v&.sub.z.sup.d is subjected to a triple-filter unit and two integral links to obtain a relatively accurate heave displacement p.sub.z.sup.d, where a transfer function G.sub.p(s) of the triple-filter unit is:

[00010]$G_p\left(s\right)=\frac{{\left(s^2+2\zeta {\Omega }_{p}s\right)}^3}{{\left(s^2+2\zeta {\Omega }_{p}s+{\Omega }_p^2\right)}^3}.$

[0074] Thus, the heave information of the ship, including a heave velocity and a heave displacement, is obtained.

[0075] The disclosure realizes the filtering of heave information without a phase delay, and simplifies a filter model of traditional heave information. The disclosure can be used in many aspects such as the take-off and landing of carrier-based aircrafts, the launching of carrier-borne weapons, and the design of heave compensation devices for drilling platforms.

[0076] The strapdown inertial navigation heave measurement method based on multiple filter units in the disclosure can realize the filtering of heave information without a phase delay, and can be used in many conventional maritime operations, such as the launching of carrier-borne weapons, submarine diving and many other aspects. FIG. 1 shows a flow chart of the disclosure. Moreover, in order to verify the feasibility of the method, the disclosure provides solution simulation results of heave information at a sampling frequency of 100 Hz, as shown in FIG. 2 to FIG. 5. FIG. 2 and FIG. 3 show comparison charts between a heave velocity and a true value after the processing by a double-filter unit and one integral link. FIG. 3 shows an error signal between the heave velocity and the true value. FIG. 4 shows a comparison chart between a heave displacement and a true value after the processing by a triple-filter unit and two integral links. FIG. 5 shows an error signal between the heave displacement and the true value. As can be seen from FIG. 2 to FIG. 5, there is almost no phase delay between a multi-filter unit algorithm and a heave velocity and displacement curve of a traditional bidirectional filter. Furthermore, the absolute value of a displacement amplitude error is basically within 3 cm, which meets an index requirement of 5% heave error.