JPH045598B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of the Invention] The present invention relates to an automatic steering system, and more particularly to an automatic steering system that uses a stationary Kalman filter to adjust the weather.

[Prior art and its problems]

The so-called autopilot mechanism of a ship has conventionally been equipped with a weather adjustment device. This weather adjustment device eliminates wasted rudder that responds to high-frequency components of turning motion induced by waves when sea conditions become rough, and prevents loss of horsepower and wear and tear on the rudder, etc. be.

However, conventional weather adjustment devices that use nonlinear elements such as linear filters, butterflies, dead zones, and dual steering angle ratios are not necessarily sufficient for the purpose of removing only high-frequency components that cannot be suppressed by steering. First, for example, components of turning motion related to steering (including components due to wind that should be controlled) are also removed, resulting in a lack of quick response of the steering control system and a limitation in performance. Additionally, since course-keeping performance is greatly affected by how the adjustment values are set, it is virtually impossible to obtain optimal weather adjustment, for example, when the navigator manually adjusts the weather. It was hot.

As a result of extensive research in view of the drawbacks of the prior art, the present inventor found that the steering system, which is an unsteady system, can be regarded as a steady system within a certain period of time. The present inventors have discovered that, by making some improvements, it is possible to utilize the stationary Kalman filter, which is effective for processing disturbances, in the weather adjustment described above, and have accomplished this invention.

[Purpose of the invention]

By maintaining the continuity of the optimal estimated value before and after the initialization of the Kalman filter, the present invention can always reliably perform optimal automatic adjustment without being affected by fluctuations in system characteristics, thereby improving the performance of the automatic steering system. The purpose is to provide an automatic steering device that can be operated effectively without waste.

[Summary of the Invention] The present invention provides an automatic steering system that adjusts the weather using a steady Kalman filter that estimates and outputs optimal values for turning angular velocity and azimuth deviation necessary for automatic steering. In addition to quantizing the signal in steps of A multivalue conversion section is provided that calculates new filter coefficients based on output data and initializes the Kalman filter. After this initialization by the multi-value conversion section, the optimal estimated value calculation circuit restarts using the previous optimal estimated value as the initial value.
The structure is such that the Kalman filter section is provided with the filter. This aims to achieve the above-mentioned purpose.

[Embodiments of the invention]

First, the principle of the present invention will be explained. The Kalman filter is a filter that provides a computer-friendly algorithm for obtaining highly reliable optimal estimates by applying the least squares method to the estimated values obtained from each of the system equation and observation equation, and is effective for disturbance processing. It is. By filtering the azimuth deviation (time series signal) using this Kalman filter, it is possible to remove only noise components, that is, high frequency components that cannot be suppressed by steering.

The Kalman filter method will be described in detail below.

First, we will discuss the system equation and observation equation that are the basis of the Kalman filter. The system equation describes the steering response of the ship, for example, according to Nomoto's first-order approximation model, Tψ¨+ψ=K(δ+v)...(1) where K, T: Maneuverability index ψ〓: Turning angular velocity δ: Rudder Angle v: Represented by plant noise. The constant difference equation that discretizes this is χ(k+1)=Aχ(k)+Bu(k)+Γv(k)...(2) Here, A=1, {EXP(a ₁ τ)−1}/a ₁ 0, EXP(a ₁ τ)=1, α ₁ 0, α ₂ B=a ₂ , {EXP(a ₁ τ)−a ₁ τ−1}/(a
₁ ) ² a ₂ , {EXP (a ₁ τ) − 1} / a ₁ = β ₁ β ₂ Γ = B χ(k) = χ ₁ (k) χ ₂ (k) = ψ _e (k) ψ〓 (k) u(k)=δ(k) a ₁ = -1/T; a ₂ = K/T v(k): Plant noise, normal white noise (mean value O, variance Q) τ: Sampling time becomes.

Next, discretizing the observation equation and expressing it as follows: Y(k+1)=Cχ(k+1)+W(k+1)...(3) where C=I=1 0 0 1 w(k): Observation noise, normal white noise ( The mean value is 0 and the covariance R).

In contrast to equations (2) and (3), the discrete-time Kalman filter is expressed by the following equations (4) to (8), as is well known.

χ〓(k+1｜k)=Aχ〓(k｜k)+Bu(k) …(4) P(k+1｜k)=AP(k｜k)A ^T +ΓQΓ ^T …(5) G(k+1)=P (k+1｜k)C ^T [CP(k+1｜k)×C ^T +R] ^-1 …(6) χ^(k+1｜k+1)=χ〓(k+1｜k)+G(k
+
1) ×[Y(k+1)−Cχ〓(k+1|k)…(7) P(k+1|k+1) =[I−G(k+1)C]P(k+1|k)…(8) Here, χ〓 (k+1|k) represents the estimated value of χ at time (k+1) when observation data at time k is obtained. Also, ^ represents the optimal water estimation value,
P and G are an error covariance matrix and a filter gain vector, respectively. Also, ^T represents transposition.

If formulas (4) to (8) are calculated every time data is sampled, the optimal estimate will be χ^(k+1 | k+1)
is obtained. The system considered here (the hull control system and its observation system), as long as the ship speed and displacement are constant, the dynamics of the signal process and measurement process do not change over time, so the system can be expressed as an equation with constant coefficients. Moreover, since the disturbances such as waves change slowly, their statistical properties can be treated as stationary if we consider a short period of about 30 minutes. Therefore, the Kalman filter becomes a linear dynamical system with constant coefficients. Specifically, the error covariance matrix and the Kalman filter gain matrix are constant.

The steady-state error covariance and filter gain are
From equations (5), (6), and (8), G〓=[AP〓A ^T +ΓQΓ ^T ]C ^T [C[AP〓A ^T +ΓQΓ ^T ]C ^T +R] ^-1 ...(9) P〓= [I-[AP〓A ^T +ΓQΓ ^T ]C ^T [C[AP〓A ^T +ΓQΓ ^T ]×C ^T +R] ^-1 ×C] [AP〓A ^T +ΓQΓ ^T ] ...(10) Here, G 〓 and P〓 respectively represent the estimated values of the filter gain and error covariance in the steady state.
Equation (10) is the Newton-Raphson method (Newton−
Raphson's method), and by substituting the result into equation (9), we can obtain G〓. Therefore, in actual signal processing, it is only necessary to calculate equations (4) and (7) in real time, and implementation is easy.

Here, the average value of plant noise is set to 0, but in reality there is no plant noise with an average value of 0.
It always has a certain bias component. Therefore, instead of formula (2), χ(k+1)+Aχ(k)+Bu(k)+Γ(v(k)+v ₀ )...(11) Here, v ₀ : Use the average value of the plant noise. It is also possible to consider this, but it would increase the amount of calculation. Therefore, it is estimated separately from the Kalman filter.

The bias component v ₀ of this plant noise is considered to be the average value of the azimuth deviation signal because the system is a closed loop and the bias component of the disturbance applied to the hull and the average value of the manipulated variable (rudder angle) are considered to be balanced in a steady state. It can be estimated that V ₀ =h·ψ _e (12) using (actually, a constant difference equation is used).
This is basically the same as the integral control performed in conventional autopilots. However, h is an arbitrary constant and is determined in consideration of the stability of the system.

On the other hand, regarding the variance Q of plant noise and the covariance R of observation noise, as an example, first of all, it is assumed that the observation system is not affected much by changes in sea conditions, so the covariance R of observation noise is assumed to be constant. We extended Mehera's online estimation of the covariance of white Gaussian noise by treating Q as
It is possible to find it using Godbole's method. Although the details are omitted, Godbole's method is a variance estimation algorithm that has a correlation with observation noise and plant noise, and is applicable even when the average value of the noise is not zero. This is a very useful method in the sense that it allows you to quantitatively understand the variance of plant noise.

However, since the amount of calculation is large and the stability of the estimated value is somewhat insufficient, in addition to the above, for example, Q = const. ... (13) R = m√ψ average of ² , 0 0, the average of m′√ψ ² …(14) You may estimate it as follows. Here, m and m' are positive arbitrary constants, which are determined experimentally taking into consideration the stability of the system.

In reality, the ship's hull is driven by waves and a high frequency component is generated, but this is included in the observation system and processed. In other words, when the sea conditions become rough, the observation system is affected and observation noise increases, and R is estimated from the parameters of the sea conditions. Due to the nature of the Kalman filter, the magnitude of R matches the magnitude of the filtering effect (Q is assumed constant for simplicity), so when R becomes large due to rough sea conditions, high frequency components induced by waves will filter out. It will be done. At this time, since the optimal estimated value is obtained mainly from the system equation, high frequency components close to the natural frequency of the steering system are not filtered out.

On the other hand, when sea conditions are taken as a parameter, the influence of individual differences cannot be avoided, and the influence of sea conditions on the hull control system is not proportional to the view force class, but is largely influenced by the direction of waves and swells. be done. As is well known, even if the swells are of the same rank, if the direction of the swell is diagonally backward, it will have a large effect on the control system, and if it is in the front, it will have almost no effect.

Therefore, as the parameters of the sea state seen from the autopilot, parameters obtained by detecting the turning motion of the ship, such as turning angular velocity, and performing predetermined calculation processing are used.
For example, the turning angular velocity "rws", which is not so affected by the controller, is used as a parameter of the sea state, and R is estimated in proportion to this parameter.
In the following, we will discuss the case where Q and R are estimated based on equations (13) and (14) as an example.

Measure the turning angle for a certain period of time and calculate R using equation (14).
can be estimated, the filter gain G can be calculated using equations (10) and (9) together with A and B obtained from the hull maneuverability indexes K and T.
is obtained. For the stationary Kalman filter, when the filter gains G, A, and B are determined, appropriate assumed values are used as initial values, and the optimal estimated value χ (0 | 0) of equation (4) is used.
After entering the steering angle u(0), the measurement data ψ _e (1),
It can be started by inserting ψ〓(1) and δ(1) into equation (7). Hereinafter, each time the measurement data ψ _e , ψ〓, and δ are sampled, the calculations of equations (4) and (7) are repeated to obtain the optimal estimated value χ(k+1|k+1). Optimal estimate χ
High-frequency components that cannot be suppressed by steering have been removed from the time-series signal, and based on this optimal estimate χ, the rudder controller determines the command rudder angle, which is the manipulated variable, and controls the rudder.

By the way, when a steady Kalman filter is used in a steering system, which is an unsteady system, if the system characteristics change due to changes in ship speed, waves, etc., not only will optimal adjustment not be obtained, but the system will become unstable and cause disturbances. There is a fear. Therefore, when the system characteristics change, it is necessary to recalculate the filter coefficients and initialize the Kalman filter. Changes in ship speed affect the maneuverability indexes K and T in equation (1). As is well known, the maneuverability index at service speed S ^o is K ^o ,
Assuming T ^o , the value made dimensionless by hull length L and ship speed S ^o is constant regardless of changes in ship speed, and for ship speed and maneuverability index K, T, K=K ^o (S/S ^o ) ...(15) The relationship T=T ^o (S ^o /S) ...(16) holds true. K ^o and T ^o are determined by Z test, etc.

When the ship speed changes, K for that ship speed S,
Since T is obtained, we can find A and B.
On the other hand, a disturbance change becomes a change in the turning angular velocity "rms" and is directly expressed in the estimated value of R using equation (14).

Therefore, when the ship speed V changes or when an estimated value of R is obtained, the stationary Kalman filter is initialized and new filter coefficients are set.
Calculate using formulas (9) and (10), enter appropriate initial values, and restart. In this case, if the initial value = calculated value, as shown by the dotted line in Figure 3, the optimal estimated value output by the Kalman filter, especially the discontinuity of ψ, is large, and the commanded rudder angle based on this is also discontinuous, and the hull changes every time it is initialized. It will mean a long meander. Therefore, in the present invention, the initial value is set as the previous optimal estimated value, and the continuity of the optimal estimated values output by the Kalman filter before and after initialization is maintained to avoid meandering.

Next, a detailed explanation will be given with reference to FIG.

FIG. 1 shows a system diagram of an automatic steering system including a stationary Kalman filter for weather adjustment. In FIG. 1, reference numeral 1 indicates a ship body as a controlled object. A rudder 2 provided at the stern of the hull 1 is steered by a rudder adjustment section 3. This allows course-keeping control of the target course. In hull 1,
Heading ψ, turning angular speed ψ〓, ship speed S, rudder angle δ
A compass 4, an angular velocity meter 5, a speed log 6, and a steering angle indicator 7 are provided.
Further, after the comparator 8 detects the azimuth deviation ψ _e (= ψ _I − ψ) between the set azimuth ψ _I set by a course setting device (not shown) and the heading ψ output from the compass 4, the turning angular velocity ψ , and are sent to the Kalman filter unit 9 together with the steering angle δ.

This Kalman filter unit 9 includes an R calculation circuit 10 for estimating observation noise, and a Q of plant noise.
Plant noise data generation circuit 1 that outputs values
1. AB calculation circuit 12 for estimating the hull steering indexes K and T and calculating filter coefficients A and B, PG calculation circuit 13 for calculating error covariance and filter gains P and G, and azimuth deviation ψ _e and turning angular velocity ψ〓 and an optimal estimated value calculation circuit 14 that calculates the optimal estimated values ψ^ _e and ψ〓.

The angular velocity meter 5 has the function of detecting the turning motion of the hull 1 and has the function of sending the detected turning angular velocity ψ〓 to the parameter calculating section 15. The parameter calculation unit 15 calculates "rms" based on data within a certain period of time, uses it as a sea condition parameter, and sends it to the R calculation circuit 10. This R calculation circuit 10 is (14)
Observation noise R is estimated according to the formula. Also,
When R can be estimated, the Kalman filter section 9 is also initialized.

On the other hand, as shown in FIG. 2, the ship speed S detected by the speed log 6 is determined by the multi-value converter 16 in a predetermined step width (for example, 2K't, which is a range that allows handling as a steady system). as the ship speed S′
It is designed to be sent by the AB calculation circuit 12.

Similar to the R arithmetic circuit 10, the multi-value conversion section 16 initializes the Kalman filter section 9 when S' changes. Therefore, the filter coefficient is calculated when the ship speed S changes by a certain amount or more.

The AB arithmetic circuit 12 has a function of obtaining the hull steering indices K and T corresponding to the ship speed S'. this is,
This is to deal with fluctuations in system characteristics, and actually calculates the filter coefficients A and B in equation (2). The PG calculation circuit 13 estimates P and G using equations (10) and (9) based on each R, Q, A, and B data.

When the observation noise R can be estimated, or when the ship speed S' changes and is initialized, the Kalman filter unit 9 calculates A, B, P, and G to obtain new filter coefficients. Then, when the optimal estimated value calculation circuit 14 is initialized, it sets the previous optimal estimated value to the initial value χ (0 | 0), also inputs an appropriate value to the steering angle u (0), and the AB calculation circuit 14
A, B data and PG calculation circuit 13 sent from
Based on the G data sent from the comparator 8 and the angular velocity meter 5, ψ _e , ψ〓 and the steering angle δ as Y data sent from the above-mentioned comparator 8 and angular velocity meter 5 are sequentially calculated according to equations (4) and (7) each time they are sampled. The optimal estimated value χ^(k+1 | k+
Perform calculation 1). However, at the beginning of the flight, there is no previous optimal estimate, so an appropriate value is set as the initial value χ.

The optimal estimated values ψ^ _e and ψ〓 filtered by the Kalman filter section 9 in this way are sent to the multivalue conversion section 16 in the rudder gear adjustment section 3, and δ ^* ₁ = K _P (ψ^ _e +T _D ψ〓) is calculated. On the other hand, I in the rudder adjustment section 3
The adjustment circuit 3A performs the calculation of δ ^* ₂ =(τ/T _I )Σψ _e (k), which is a constant difference equation (12). These δ ^* ₁ and δ ^* ₂ are added by an adder 18 and outputted to the steering gear 2 as a command steering angle δ ^* .

In the multi-value conversion section 16, when the ship speed S slightly fluctuates around the boundary of the increment width, S' changes repeatedly, so the Kalman filter section 9 is also initialized repeatedly. and,
Since the previous optimal estimated value is used as the initial value at each initialization, continuity is maintained, but the output of the Kalman filter section 9 remains at a constant value for a certain period of time as shown in FIG. result. In order to improve this problem, hysteresis is provided at the boundaries of the increments of the multi-value converter 16 as shown in FIG. 3 to suppress the occurrence of long-period bow rocking and also to prevent the hunting phenomenon. Good too.

Furthermore, as for the bias component of plant noise, instead of processing it in the I adjustment circuit 3A, as shown in FIG. The bias processing unit 20 calculates δ′=δ−(τ/T _I )Σψ _e (k), and the steering angle δ′ from which the bias component has been removed is used as the steering angle input to the Kalman filter. You can also do that.

〔Effect of the invention〕

As explained above, according to the present invention, it is possible to maintain the continuity of the optimal estimated value output by the Kalman filter even when the system characteristics fluctuate. It is possible to provide an unprecedented automatic steering system that can always reliably perform optimal weather adjustment with good course-keeping performance using a filter.

[Brief explanation of drawings]

FIG. 1 is a system diagram showing an automatic steering system according to an embodiment of the present invention, FIG. 2 is a diagram showing the operation of the multi-value conversion section in FIG. 1, and FIG. 3 is a diagram showing the operation of the multi-value conversion section and other parts. 4 and 5 are waveform diagrams illustrating the output characteristics of the Kalman filter section, respectively.
The figure is a diagram showing another embodiment of the plant noise bias processing method. 1... Hull, 2... Rudder gear, 3... Rudder gear adjustment section,
9...Kalman filter section, 14...Optimum estimate calculation circuit, 16...Multi-value conversion section.

Claims (1)

[Scope of Claims] 1. In an automatic steering system that adjusts the weather using a steady Kalman filter that estimates and outputs optimal values regarding the turning angular velocity and azimuth deviation necessary for automatic steering, the system comprises: Along with quantizing by width,
In this quantization, a ship speed signal in the form of a predetermined hysterin loop is output when the ship speed decreases, and when the output changes as the ship speed changes, a new filter coefficient is generated based on the output data. A multi-value conversion section is provided which calculates the above-mentioned Kalman filter and also initializes the Kalman filter. An automatic steering device characterized by the fact that the section is equipped with an automatic steering device.