CN115876194A - An Adaptive Ship Heave Measurement Method Based on Strapdown Inertial Navigation - Google Patents

Abstract

The invention provides an adaptive warship heave measurement method based on the strapdown inertial navigation, the steps are as follows: 1. Obtain the vertical acceleration a _z including the low-frequency harmonics by solving the strapdown inertial navigation. 2. Perform N-point fast Fourier transform on a _z to obtain the dominant heave frequency ω ₀ of the ship. 3. Integrate a _z to obtain the vertical velocity information v _{no_filter} containing low-frequency noise, and use the adaptive vertical velocity high-pass filter to filter v _{no_filter} to obtain the vertical velocity v _z with amplitude and phase errors. 4. Use RLS-BMFLC to fit v _z , and correct the amplitude and phase of each fundamental frequency point to obtain the accurate vertical velocity v″ _z . 5. Integrate v″ _z to obtain the low-frequency noise For heave information x _{no_filter} , the adaptive vertical displacement high-pass filter is used to filter x _{no_filter} to obtain the heave displacement x _z including amplitude and phase errors. 6. Use RLS-BMFLC to fit x _z , and correct the amplitude and phase of each fundamental frequency point to obtain the precise heave displacement x″ _z.

Description

An Adaptive Ship Heave Measurement Method Based on Strapdown Inertial Navigation

technical field

The invention provides a ship heave measurement method based on an adaptive band-limited Fourier linear combiner based on recursive least square optimization weights.

Background technique

Since the ship will inevitably be disturbed by complex marine environmental factors such as waves and sea winds during the movement of the sea surface, it will passively generate swaying motions with six degrees of freedom. The so-called six degrees of freedom mean rotation around three axes in the Cartesian coordinate system: pitch, roll and yaw, and movement along three axes: sway, surge and heave (ie heave). Among them, the periodic heave motion along the vertical axis has the greatest influence and harm on the ship. Therefore, accurate measurement of ship heave parameters has great application value in practical engineering. Since the altitude channel of the strapdown inertial navigation system includes not only the zero bias and noise of the accelerometer, but also low-frequency harmonic interference such as Schuler oscillation and earth oscillation, the velocity obtained by directly integrating the vertical acceleration is divergent. Therefore, it is necessary to properly process the vertical specific force calculated by the strapdown inertial navigation system in order to obtain accurate heave motion information.

Some scholars at home and abroad have done some related research on ship heave information. John-Morten Godhavn proposed a standard fourth-order heave filter in "Adaptive tuning of heave filter inmotion sensor" published by "IEEE Oceanic Engineering Society", which can filter low-frequency harmonics and perform secondary integration on the desired frequency band signal, but The high-pass filter mainly has problems such as phase lead and amplitude attenuation, and the measured heave information has a large error. Yan Gongmin first designed the IIR digital low-pass filter, and then used the complementary method to convert it into timeless Delayed digital high-pass filter, but its parameters are demanding and difficult to design. "Real-time Zero PhaseFiltering for Heave Measurement" published by Hu Yongpan in "IEEE International Conference on Electronic Measurement and Instruments" combines a digital all-pass filter on the basis of the heave high-pass filter to compensate the phase, but the output of the heave Shen displacement still has amplitude attenuation and small phase error. Win TunLatt published "Drift-Free Position Estimation of Periodic or Quasi-Periodic Motion Using Inertial Sensors" in "Sensors" No. 6, 2011, for the first time proposed to use the band-limited Fourier linear combination (BMFLC) algorithm to fit periodic signals, and to Phase compensation and amplitude compensation are performed on the fitted signal to estimate periodic motion and obtain better results. In the patent application No. 201710202159.5, titled "A Method for Ship Heave Measurement Based on Band-Limited Fourier Linear Combination", the BMFLC algorithm is applied to the measurement of ship heave information for the first time. The main method is The heave information is fitted and the amplitude and phase are compensated, so that the amplitude, phase and frequency characteristics of the heave filter in a specific frequency band are close to the quadratic integration link. However, since there is no prior heave frequency information, the fitting bandwidth is large and phase compensation is difficult. And the weight iteration uses the LMS algorithm, so the algorithm accuracy is slightly lower.

Contents of the invention

The purpose of the present invention is to provide a kind of adaptive recursive least square band-limited Fourier linear combiner (RLS-BMFLC) ship heave measurement method based on strapdown inertial navigation, the purpose is to solve the problem of traditional heave measurement The problem of amplitude attenuation and phase lead can realize accurate measurement of ship heave information.

The object of the present invention is achieved like this: step is as follows:

Step 1. Start the strapdown inertial navigation system installed in the ship and perform related initialization work. After the initialization, the vertical acceleration a _z of the ship including low-frequency noise obtained by the inertial navigation system is collected.

Step 2. Obtain its frequency domain feature to _az obtained in step 1 by N-point Fast Fourier Transform (FFT). Since the cycle of ocean waves is generally 1-25s, the corresponding frequency is 0.04-1Hz, and the obtained amplitudes are compared in this frequency range, and the corresponding maximum value is the dominant frequency of ocean waves, denoted as ω ₀ .

Step 3. Integrate the vertical acceleration a _z obtained in step 1 to obtain the vertical velocity v _{z_nofilter} including low-frequency noise.

Step 4. Determine the transfer function of the vertical velocity high-pass filter

Among them, ξ is the damping coefficient; ω _c is the cut-off frequency of the system, generally take ω _c =0.37ω ₀ . Let the v _{z_nofilter} obtained in step 3 pass through the vertical high-pass filter. Since the signal passes through the high-pass filter, amplitude attenuation and phase lead will occur, so the vertical velocity _vz obtained after filtering contains amplitude and phase errors.

Step 5. Use RLS-BMFLC to fit v _z in step 4 to obtain the fitted vertical velocity v′ _z , with an iterative form as follows

W _k+1 ＝W _k +G _k e _k

In the formula, k is the number of iterations; X _rk =[X _1k X _2k ... X _2Mk ] ^Τ is an element in X _k , which is the basic fitting amount of each frequency point in the adaptive frequency band of RLS-BMFLC; T is the system Sampling period; f _r is the fitting fundamental frequency, the unit is Hz; M is the number of frequency points in the adaptive frequency band; v _k is the kth element of the vertical velocity v _z ; v′ _k is the vertical velocity v _k The result obtained by using RLS-BMFLC fitting; G _k is the recursive least squares gain; λ is the recursive least squares factor, usually the value range is [0.9,1]; P _k is the recursive least squares E _k is the recursive least squares error; W _k = [W _1k W _2k ... W _2Mk ] ^Τ is an element in W _k , which is the weight of each frequency point corrected in real time in the RLS algorithm at the last moment coefficient.

Step 6. Calculate the amplitude attenuation m _r and the phase lead p _r of each fundamental frequency point in the RLS-BMFLC according to the amplitude-phase-frequency characteristics of the vertical velocity high-pass filter in step 4, and calculate the fitted curve After compensation, the precise vertical velocity v″ _z can be obtained. The specific calculation method is as follows

m _r ＝|H ₁ (jω _i )|

p _r ＝∠H ₁ (jω _i )

Step 7. Integrate the precise vertical velocity v″ _z obtained in step 6 to obtain the vertical displacement x _{z_nofilter} including low-frequency noise.

Step 8. Determine the transfer function of the vertical displacement high-pass filter

Pass the x _{z_nofilter} obtained in step 7 through a digital high-pass filter. Since the signal passes through the high-pass filter, amplitude attenuation and phase lead will occur, so the vertical displacement x _z obtained after filtering contains amplitude and phase errors.

Step 9. Using the same calculation method as Step 5 and Step 6, first use RLS-BMFLC to fit the vertical displacement x _z to get the curve x _z ′, and then compensate the amplitude and phase of x _z ′ to get Accurate heave displacement x _z ″.

Compared with the prior art, the beneficial effect of the present invention is: the present invention can make full use of the wave frequency as prior information, thereby adaptively changing the parameters of the vertical velocity and the heave filter and the baseband of the Fourier linear combination, Furthermore, RLS-BMLFC is used to fit and compensate the vertical velocity and heave parameters that produce amplitude and phase errors after passing through the high-pass filter, and realize high-precision measurement of heave information.

Description of drawings

Fig. 1 is the overall flowchart of the present invention;

Fig. 2 and Fig. 3 are Bode diagrams of H ₁ (s), H ₂ (s) of the present invention;

Fig. 4, Fig. 5 are RLS-BMFLC schematic diagram and amplitude phase compensation schematic diagram of the present invention;

Fig. 6 is the error of traditional digital high-pass filter measuring heave (method A), traditional digital high-pass filter and digital all-pass filter combination measuring heave (method B) and the present invention RLS-BMFLC measuring heave (method C) Comparison chart.

Detailed ways

The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

The invention provides a ship heave measurement method based on an adaptive band-limited Fourier linear combiner (RLS-BMFLC) based on recursive least square optimized weights. The main steps are as follows: 1. Obtain the vertical acceleration a _z including low-frequency harmonics by solving the strapdown inertial navigation system. 2. Perform N-point fast Fourier transform on a _z to obtain the dominant heave frequency ω ₀ of the ship. 3. Integrate a _z to obtain the vertical velocity information v _{no_filter} containing low-frequency noise, and use the adaptive vertical velocity high-pass filter to filter v _{no_filter} to obtain the vertical velocity v _z with amplitude and phase errors. 4. Use RLS-BMFLC to fit v _z , and correct the amplitude and phase of each fundamental frequency point to obtain the precise vertical velocity v _z ″. 5. Integrate v _z ″ to obtain the low-frequency noise For heave information x _{no_filter} , the adaptive vertical displacement high-pass filter is used to filter x _{no_filter} to obtain the heave displacement x _z including amplitude and phase errors. 6. Use RLS-BMFLC to fit x _z , and correct the amplitude and phase of each fundamental frequency point to get the precise heave displacement x _z ″.

(1) The cycle of ocean waves is mainly concentrated in 1-25s, and the frequency of ship heave motion is mainly concentrated in a relatively narrow frequency band centered on a certain frequency. Therefore, it is very important for the design of subsequent high-pass filter and RLS-BMFLC to fully understand the distribution of the dominant frequency of heave motion in the whole heave frequency band by performing fast Fourier transform on the calculated _az meaning.

由于IMU输出的采样信号均为离散信号，因此需要采用双线性变换法将模拟滤波器转变为数字滤波器，自适应高通数字滤波器具体形式如下(2) The Bode diagrams of high-pass filters H ₁ (s) and H ₂ (s) are shown in Fig. 2. The cut-off frequency of the filters can be adaptively adjusted according to the dominant heave frequency of the ship measured by FFT. Therefore, the adaptive high-pass filter can effectively remove low-frequency harmonics and noise in the signal. Since the high-pass filter is a second-order Butterworth analog filter, the damping coefficient ξ is generally taken as

Since the sampling signals output by the IMU are all discrete signals, it is necessary to use the bilinear transformation method to convert the analog filter into a digital filter. The specific form of the adaptive high-pass digital filter is as follows

Among them, T is the IMU sampling period; ω _c is the system cut-off frequency; ξ is the damping coefficient.

(3) The dominant heave frequency of the ship measured by FFT can divide the window frequency band of RLS-BMFLC more accurately, and further narrow the baseband width and frequency interval in the frequency band of RLS-BMFLC fitting, so that the estimation result can be as far as possible The fundamental frequency point in the frequency band can be covered, so as to achieve high-precision fitting of the signal in the frequency band, and further improve the compensation accuracy of the amplitude and phase.

(4) Figure 4 is a block diagram of RLS-BMFLC. After determining the dominant frequency and the RLS-BMFLC fitting frequency band, carry out M equal divisions in the frequency band, and divide into each frequency point f _r as the fundamental frequency for linear fitting with sine and cosine functions. In the iterative process, the recursive least squares algorithm is used to optimize the weight of the fundamental frequency, which can make the fitting achieve higher precision. Use the sine and cosine trigonometric functions to combine the analog signal y _k and the frequency interval Δf of the fundamental frequency points in the frequency band as follows

为直流分量；a_r和b_r分别为正余弦系数；f_M为拟合频带上限；f₁为拟合频带下限；M为基频点数，通常取25～200。由于上述高通滤波器对直流衰减较大，故可忽略直流分量/>

in,

a _r and b _r are sine and cosine coefficients respectively; f _M is the upper limit of the fitting frequency band; f ₁ is the lower limit of the fitting frequency band; M is the number of fundamental frequency points, usually 25 to 200. Since the above-mentioned high-pass filter has a large attenuation of DC, the DC component can be ignored />

(5) The specific phase lead and amplitude attenuation in step 6 are as follows

Wherein, the fundamental frequency angular frequency ω _i =2πfr _r , and f _r is the fundamental frequency point.

(6) Figure 5 is a functional block diagram of RLS-BMFLC compensation amplitude and phase. In step 9, the specific iterative formulas for fitting and amplitude-phase compensation to the heave displacement are as follows

W _k+1 ＝W _k +G _k e _k

m _r ＝|H ₂ (jω _i )|

p _r ＝∠H ₂ (jω _i )

In the formula, x _k is the element of heave displacement x _z at time k; x′ _k is the result of RLS-BMFLC fitting for heave displacement x _k ; x″ _k is the amplitude and phase of x′ _k Accurate heave displacement obtained after compensation.

It can be seen from the above iterative form that the fundamental frequency point f _r remains unchanged during the fitting iteration process, and the weight coefficient W _k of the frequency point is corrected in real time by the RLS algorithm to achieve signal fitting. In the compensation of the signal amplitude and phase, only the amplitude and phase of the basic fitting value X _rk are adjusted, and the iteration weight is not corrected, and the optimal weight W _k estimated by the RLS algorithm at the previous moment is still used . Accurate heave displacement x″ _k can be obtained through the above iterative method.

Claims (3)

1. A self-adaptive ship heave measurement method based on strapdown inertial navigation is characterized by comprising the following steps:

step 1: starting a strapdown inertial navigation system installed in a ship, initializing, and acquiring an inertial navigation system after initialization is finished to calculate to obtain the vertical acceleration a of the ship containing low-frequency noise _z ；

Step 2: for a obtained in step 1 _z Obtaining the frequency omega including the main frequency by N-point Fast Fourier Transform (FFT) ₀ The relevant frequency domain characteristics of the inner sea wave;

and step 3: for the vertical acceleration a obtained in the step 1 _z Performing primary integration to obtain a vertical velocity v containing low-frequency noise _{z_nofilter} ；

And 4, step 4: determining transfer function of vertical velocity high pass filter

Wherein xi is a damping coefficient; omega _c Is the cut-off frequency of the system; and order v _{z_nofilter} Obtaining a vertical velocity v including amplitude attenuation and phase advance by a digital high-pass filter _z ；

And 5: v in step 4 Using RLS-BMFLC _z Fitting to obtain the fitted vertical velocity v _z ′；

And 6: calculating the amplitude attenuation quantity and the phase lead quantity of each fundamental frequency point in the RLS-BMFLC according to the amplitude-phase-frequency characteristic of the vertical velocity high-pass filter in the step 4, and compensating the fitted vertical velocity to obtain the accurate vertical velocity v ″ _z ；

And 7: for the precise vertical velocity v ″' obtained in step 6 _z Performing primary integration to obtain the vertical displacement x before filtering _{z_nofilter} ；

And 8: determining the transfer function of the vertical displacement high-pass filter as:

wherein xi is a damping coefficient; omega _c Is the cut-off frequency of the system; let x obtained in step 7 _{z_nofilter} Obtaining a vertical displacement x containing an amplitude-phase error through a digital high-pass filter _z ；

And step 9: using the same calculation method as steps 5 and 6, firstly using RLS-BMFLC to make vertical displacement x _z Fitting to obtain a curve x _z ', subsequently to x _z ' Compensation of amplitude and phase is carried out to obtain accurate heave displacement x _z 。

2. The adaptive ship heave measurement method based on strapdown inertial navigation according to claim 1, wherein the specific iterative form of the RLS-BMFLC fitting and compensation of step 5 and step 9 is as follows:

y′ _k ＝W _k ^Τ X _k

in the formula, k is iteration times; x _rk Is the basic amount of fitting X _k One element of (1); f. of _r Fitting the fundamental frequency; t is a system sampling period; m is the number of frequency points in the self-adaptive frequency band; y is _k Is the kth element of the original signal y; y' _k To the original signal y _k Fitting results obtained by adopting RLS-BMFLC; w is a group of _k The corrected weight value of the RLS algorithm at the last moment; g _k Is a recursive least squares gain; λ is a recursive least square factor; p _k Recursion of the least square error covariance for the previous time instant; e.g. of the type _k Is a recursive least squares error.

3. The adaptive ship heave measurement method based on strapdown inertial navigation according to claim 1 or 2, wherein the calculation formula for amplitude-phase compensation in step 6 is as follows:

y′ _k ′＝W _k ^Τ X _k ′

in the formula, m _r And p _r Respectively corresponding to the amplitude attenuation and the phase lead in the high-pass filter for each base frequency point; y' _k 'is to post-fitting signal y' _k And (5) performing amplitude and phase compensation.

Images