JPH0379239B2 - - 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 in which weather adjustment is performed using a Kalman filter.

[Background Art and its Problems] The so-called autopilot mechanism of ships 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.

The present inventor devised a weather adjustment method using a Kalman filter (optimal filter) in order to improve the conventional weather adjustment which has such drawbacks.

Kalman filter is a filter that provides a computer-friendly algorithm to obtain 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 explained below.

First, we will discuss the system equation and observation equation that are the basis of the Kalman filter. The system equation is the steering response of the ship, and 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 x(k+1)=Ax(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 x(k) = x ₁ (k) x ₂ (k) = ψ _e (k) 〓(k) u(k) = δ(k) a ₁ =-1/T; a ₁ =K/T v(k): Plant noise, normal white noise (mean value 0, variance Q) τ: Sampling time.

Next, the observation equation is discretized and expressed as Y(k+1)=C _x (k+1)+w(k+1)...(3) where C=I=10 01 w(k): Observation noise , normal white noise (mean value 0, 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.

X~(k+1|k)=AX~(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) X^(k+1|k+1)= X~(k+1 |k)＋G(k+1)× [Y(k+1)−CX〜(k+1|k) ……(7) P(k+1|k+1)= [I−G(k+ 1)C]P(k+1|k)...(8) Here, X~(k+1|k) is the estimate of x at time (k+1) when observation data at time k was obtained. represents a value. Also, ^ represents the optimal estimate,
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 X~(k+1|k+
1) is obtained. In the system considered here (the hull control system and its observation system), 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, and disturbances such as waves can be expressed as an equation. Since the change is gradual, considering the short time of about 30 minutes, its statistical properties can be treated as stationary. 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 ~, P~ represent estimated values of filter gain and error covariance in a steady state, respectively.
Equation (10) is the Newton-Raphson method (Newton−
Raphson method) can be used to solve the problem, and the result is (9)
By substituting into the formula, G can be obtained. Therefore, in actual signal processing, it is only necessary to calculate equations (4) and (7) in real time, making implementation 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 ) ,} Although it is possible to use the average value, this would result in an increase in 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) (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, the variance Q of plant noise and the covariance R of observation noise are so important that the quality of filtering is determined by how appropriately they are estimated. In particular, plant noise is difficult to grasp quantitatively and is the most problematic when applying a Kalman filter to a system.

First, since it is thought that a normal observation system is not affected much by changes in oceanographic conditions, the covariance R of observation noise is treated as constant. On the other hand, Q is Mehra
It is conceivable to obtain online estimation of the covariance of white Gaussian noise using an extended Godbole method. Although the details are omitted, this is a variance estimation algorithm that can be applied even when there is a correlation between plant noise and observation noise and 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 variance.

However, when using an algorithm to obtain this Q using Godbole's method to obtain the optimal estimated value of the heading deviation (setting heading - heading) and turning angular velocity and performing PD (PID) control of the rudder, the following problems occur. I found out that it happens. That is, in calm sea (state where the sea surface is calm), the rudder angle changes smoothly, but course keeping becomes poor. On the other hand, in rough seas, the rudder angle suddenly increased, causing overuse of the rudder. This is Godbole's method
In calm sea, the variance of plant noise was estimated too small, which produced a filtering effect, but this caused a delay in the optimal estimate and increased yaw, and in rough sea, the azimuth deviation or turning angular velocity Since the high frequency components included (close to the rolling period) are considered to be caused by plant noise, the value of Q becomes extremely large. This is because you get used to it.

Furthermore, the stability of the estimated values themselves was poor, resulting in large variations under the same sea state.

Considering that a time-series signal is a combination of an original meaningful signal component and a meaningless noise component, filtering is a method that separates the noise component as much as possible to make meaning. The purpose is to extract only certain signal components. Applying this to the present case, the originally meaningful signal components are the low frequency components close to the natural frequency of the steering system included in the azimuth deviation and turning angular velocity, and are the meaningless noise components. is not just an error caused by gyros, etc., but also includes high-frequency components that cannot be suppressed by steering induced by waves.

Based on this, we once again write the system equation
Looking at (3), the system equation itself does not express any such clear distinction. Godbole
When the above algorithm is applied, when the sea conditions become rough and there are many high-frequency components that cannot be suppressed by steering, all of these components are considered to be induced by plant noise, and the estimated value of the plant noise variance Q becomes very large. It becomes a large value. Thus, when the estimated value of Q becomes large, all high-frequency components induced by waves are considered to be meaningful signals, resulting in a filtering that is completely different from what we desire. The reason for this is that the system equation does not sufficiently express the filtering that we want, but there are still many kinematic problems in expressing only the high frequency components induced by waves, and When incorporated into the system equation, there is also the disadvantage that the order of the Kalman filter increases and the amount of calculation increases.

In addition, Godbole's method assumes white noise and uses the autocorrelation of the invasion process. Therefore, if the data sampling time is short and the autocorrelation of the invasion process is strong, the variance cannot be calculated, and if the number of data is small, the variance cannot be calculated. This method also has the disadvantage that it takes a long time to prepare for calculations because the variation in estimated values becomes large, and therefore, it is necessary to take a long sampling time and a sufficiently large amount of data.

As a result of intensive research in view of the drawbacks of the above-mentioned method, the present inventor found that by devising a method for estimating filter coefficients, it is possible to prevent the optimal estimated value of azimuth deviation, etc. from being influenced by high-frequency components caused by waves. This discovery led to this invention.

[Purpose of the invention]

The present invention has a simple configuration that fully utilizes the functions of the Kalman filter to improve the stability and adaptability of automatic steering system control. Its purpose is to provide a steering device.

[Summary of the invention]

The present invention provides a rotational motion detection means for detecting the rotational motion of a hull, a turning angular velocity signal outputted from the rotational motion detection means, an azimuth deviation signal calculated from a heading and a set azimuth, and a rudder angle instruction. A Kalman filter for weather adjustment that inputs the rudder angle signal detected by the instrument and the ship speed signal detected by the speedometer, estimates and outputs the optimal values for the turning angular velocity and azimuth deviation necessary for automatic steering. It has a section. Among these, the Kalman filter section includes a plant noise data generation circuit that outputs predetermined plant noise data necessary for the calculation, and an R that estimates observation noise based on predetermined oceanographic parameters.
and an arithmetic circuit, and calculates and outputs predetermined oceanographic parameters based on the output signal from the rotational motion detection means at the input stage of either the R arithmetic circuit or the plant noise data generation circuit. A parameter calculation unit is provided. Furthermore, a method is adopted in which the output value of the other circuit of the R calculation circuit and the plant noise data generation circuit is set constant. This aims to achieve the above-mentioned purpose.

[Embodiments of the invention]

First, the principle of an embodiment of the present invention will be explained.

The Kalman filter algorithm, which uses Godbole's method mentioned above, assumes that the observation system will not be affected by rough sea conditions. As long as we consider this, it is necessary to express the high frequency components induced by waves in the system equation. Therefore, in this example, the system equation (2) is used as is, and although the hull is actually driven by waves and a high frequency component is generated, it was decided to include this in the observation system and process it.

That is, it is assumed that 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, 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 vehicle weight class is the same, if the direction of the swell is diagonally backward, it will have a large effect on the control system, but 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, if the ``rms'' of the turning angular velocity, which is not affected by the controller, is a parameter of the sea state, and R is estimated in proportion to this parameter, then the plant noise and observation noise are Q = const. ... (13 ) Average of R= ^m√2 ...(14) Here, m is a positive arbitrary constant, which is determined experimentally in consideration of the stability of the system.

Next, this will be explained in detail based on FIG.

FIG. 1 is a system diagram of an automatic steering system including a Kalman filter for weather adjustment. In this Figure 1,
Reference numeral 1 indicates a hull 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 to be performed. The hull 1 is equipped with a compass 4, an angular velocity meter 5, a speed log 6, and a rudder angle indicator 7, which respectively measure the heading ψ, turning angular velocity 〓, ship speed s, and rudder angle δ. Also,
Setting direction ψ ₁ set with a course setting device (not shown)
After the comparator 8 detects the azimuth deviation ψ _e (=ψ ₁ - ψ) between the heading ψ and the heading ψ output from the compass 4, the comparator 8 detects the deviation ψ e (=ψ 1 - ψ) between the azimuth ψ and the heading ψ output from the compass 4.
sent to.

This Kalman filter section 9 includes an R calculation circuit 10 that estimates observation noise, a plant noise data generation circuit 11 that outputs a Q value of plant noise, and a filter coefficient that estimates hull steering indices K and T from ship speed s. AB calculation circuit 12 for calculating A and B
, a PG calculation circuit 13 that calculates the error covariance and filter gains P and G, and an optimal estimated value calculation circuit 14 that calculates the optimal estimated values ^ _e and 〓 of the azimuth deviation ψ _e and the turning angular velocity 〓. There is.

The angular velocity meter 5 has a function as a turning motion detecting means of the hull 1, and the detected turning angular velocity 〓 is sent to the parameter calculating section 15. This parameter calculation unit 15 calculates "rms" based on data within a certain period of time.
is determined and sent to the R calculation circuit 10 as a sea condition parameter. This R calculation circuit 10 estimates the observation noise R according to equation (14). AB calculation circuit 12
is the ship speed s when the ship speed s changes by a certain amount or more.
It has a function to obtain hull maneuverability indexes K and T corresponding to the following. This is to deal with fluctuations in system characteristics, and in reality, the filter coefficients A and B in equation (2) are calculated. The PG arithmetic circuit 13 has each R,
Based on the Q, A, B data, P, G is calculated by equations (10) and (9).
Estimate. The Kalman filter unit 9 is initialized when the observation noise R is estimated or when the ship speed s changes by a certain amount or more. When the optimum value calculation circuit 14 is initialized, it inputs appropriate values as the initial optimum estimated value , based on the G data sent from the G calculation circuit 13, each time ψ _e , 〓 and steering angle δ as Y data sent from the comparator 8 and the angular velocity meter 5 are sampled, (4), ( 7), the optimal estimated value x(k+
1|k+1) is performed.

The optimal estimated value ^ _e , 〓 filtered by the Kalman filter section 9 in this way is sent to the PD adjustment circuit 3A in the rudder gear adjustment section 3, and δ ^* ₁ = K _P (^ _e + T _D 〓) The following calculations are performed. On the other hand, the I adjustment circuit 17 in the rudder gear adjustment section 3 calculates δ ^* ₂ = (τ/T ₁ )Σψ _e (K), which is a constant difference of equation (12). These δ ^* ₁ and δ ^* ₂ are added to adder 1
8 and outputted to the aforementioned rudder 2 as the command rudder angle δ ^* .

However, instead of processing the bias component of the plant noise mentioned above in the I adjustment circuit,
As shown in FIG. 2, a bias processing unit 20 is installed between the steering angle indicator 7 and the Kalman filter optimum value calculation circuit 14, and the bias processing unit 20 calculates the following equation: δ′=δ−(τ/T ₁ ) Σψ _e (K) and the steering angle σ' from which the bias component has been removed can also be used as the steering angle input to the Kalman filter section.

Next, instead of formulas (13) and (14) described above as the second embodiment, for example, R=const. (16) However, n may be a positive (experimentally determined) arbitrary constant, and the plant noise Q may be estimated to be inversely proportional to the parameters of the sea state. This is because due to the nature of the Kalman filter, the magnitude of the filtering effect matches the magnitude of Q (when R is constant),
This is because when the sea conditions become rough and Q becomes small, high frequency components induced by waves are filtered out.

Note that the turning motion of the ship may be detected from the heading of the ship, and other methods such as absolute value averaging may be used in addition to "rms" for parametrization of the sea state, and noise estimation may also be used. Methods other than proportional and inverse proportion may be used.

〔Effect of the invention〕

Since the present invention is configured and functions as described above,
According to this, it is possible to stably estimate the noise coefficient of the Kalman filter using a simple algorithm, which reduces the burden on the computer and makes effective only the high-frequency components that cannot be suppressed by the steering system, which are induced by waves. Therefore, an automatic steering system with a smooth steering angle, a small amount of steering, and excellent course keeping performance can be obtained.

[Brief explanation of drawings]

FIG. 1 is a system diagram of an automatic steering system according to one embodiment of the present invention, and FIG. 2 is a block diagram showing another embodiment of a plant noise bias processing method. 1... Hull, 2... Rudder gear, 3... Rudder gear adjustment section,
4... Compass, 5... Angular velocity meter as rotational motion detection means, 6... Speed log, 7... Rudder angle indicator, 8... Comparator, 9... Kalman filter section, 10... R calculation Circuit, 11... Plant noise data generation circuit, 12... AB calculation circuit, 13
...PG calculation circuit, 14...optimum estimated value calculation circuit,
15...Parameter calculation section.

Claims (1)

[Scope of Claims] 1. A rotational motion detection means for detecting the rotational motion of the hull, a turning angular velocity signal output from the rotational motion detection means, an azimuth deviation signal calculated from the ship's heading and a set azimuth, and Weather adjustment device that inputs the rudder angle signal detected by the rudder angle indicator and the ship speed signal detected by the ship speedometer, estimates and outputs the optimal values for the turning angular velocity and azimuth deviation required for automatic steering. A plant noise data generation circuit that outputs predetermined plant noise data necessary for the calculation, and an R calculation circuit that estimates observation noise based on predetermined sea state parameters. and a parameter for calculating and outputting a predetermined sea state parameter based on the output signal from the rotational motion detection means, at the input stage of either the R calculation circuit or the plant noise data generation circuit. An automatic steering system characterized in that a calculation section is provided, and the output value of the R calculation circuit and the output value of the other circuit of the plant noise data generation circuit are set constant.