JPH045597B2 - - Google Patents

Description

【発明の詳細な説明】 〔発明の属する分野〕 本発明は、自動操舵装置に係り、とくに天候調
整を、カルマンフイルタを用いて行なうようにし
た自動操舵装置に関する。

〔背景技術とその問題点〕

船舶の所謂オートパイロツト機構には従来から
天候調整装置が装備されている。この天候調整装
置は、海象状態が荒れたとき、波浪によつて誘起
される回頭運動の高周波成分に応答するような無
駄舵をなくし、馬力損失や舵機等の消耗を防止せ
んとするものである。

しかしながら、従来の線形フイルタやバツクラ
ツシ、不感帯、二重舵角比等の非線形要素を用い
た天候調整装置では、操舵で迎えることができな
い高周波成分のみを除去する目的からは必ずしも
十分なものとは言えず、例えば操舵に関連した回
頭運動の成分（本来制御すべき風による成分を含
む）も一緒に除去してしまうので操舵制御系の速
応性に欠け、性能に限界が生じるという不都合が
あつた。また、調整値の設定の仕方によつて保針
性能が大きく左右されるため、例えば航海者が手
動で調整を行なつた場合など最適な天候調整を得
ることは事実上殆んど不可能であつた。

かかる欠点を有する従来の天候調整に対し、こ
れを改善するため、本発明者は、カルマンフイル
タ（最適フイルタ）を利用した天候調整方法を考
案した。カルマンフイルタとはシステム方程式と
観測方程式の各々から得る推定値に最小自乗法を
適用して信頼度の高い最適推定値を求めるための
計算機向きアルゴリズムを提供するフイルタであ
り、外乱処理に有効なものである。このカルマン
フイルタを用いて方位偏差（時系列信号）のフイ
ルタリングを行なうことにより、雑音成分、即ち
操舵で抑えることのできない高周波成分のみを除
去できる可能性がある。以下、カルマンフイルタ
の手法について説明する。

まず、カルマンフイルタの基本となるシステム
方程式と観測方程式について述べる。システム方
程式は船体の操縦応答で「野本の一次近似モデ
ル」によると Tψ¨＋ψ〓 Ｋ（δ＋ｖ） ……(1) ここで Ｋ、Ｔ：操縦性指数 ψ〓：旋回角速度 δ：舵角 ｖ：プラントノイズ で表わされる。これを離散化した定差方程式は、 ｘ（ｋ＋１）＝Ax(k)＋Bu(k)＋Γv(k) ……(2) ここで、 Ａ＝１、｛EXP（a₁τ）−１｝／a₁ ０、EXP（a₁τ）＝１、α₁ ０、α₂ Ｂ＝a₂、｛EXP（a₁τ）−a₁τ−１｝／（a₁）² a₂、｛EXP（a₁τ）−１｝／a₁＝β₁ β₂ Γ＝Ｂ ｘ(k)＝x₁(k) x₂(k)＝ψ_e(k) ψ〓(k) ｕ(k)＝δ(k) a₁＝−１／Ｔ；a₂＝Ｋ／Ｔ ｖ(k)：プラントノイズ、正規性白色ノイズ（平均
値０、分散Ｑ） τ：サンプリングタイム となる。

続いて観測方程式を離散化して表わすと Ｙ（ｋ＋１）＝Cx（Ｋ＋１）＋Ｗ（ｋ＋１） ……(3) ここで Ｃ＝Ｉ＝１ ０ ０ １ ｗ(k)：観測ノイズ、正規性白色ノイズ（平均値
０、共分散Ｒ） となる。

式(2)、(3)に対し離散時間のカルマンフイルタは
周知のように以下の(4)〜(8)式で表わされる。

x〓（ｋ＋１｜ｋ）＝Ax〓（ｋ｜ｋ）＋Bu(k) ……(4) Ｐ（ｋ＋１｜ｋ）＝AP（ｋ｜ｋ）A^T＋ΓQΓ^T ……(5) Ｇ（ｋ＋１）＝Ｐ（ｋ＋１｜ｋ）C^T〔CP（ｋ
＋１｜ｋ）×C^T＋Ｒ〕^-1……(6) x^（ｋ＋１｜ｋ＋１）＝x〓（ｋ＋１｜ｋ）＋Ｇ
（ｋ＋１）×〔Ｙ（ｋ＋１）−Cx〓（ｋ＋１｜ｋ）……
(7) Ｐ（ｋ＋１｜ｋ＋１）＝〔Ｉ−Ｇ（ｋ＋１）
Ｃ〕Ｐ（Ｋ＋１｜ｋ）……(8) ここで、x〓（ｋ＋１｜ｋ）は、ｋ時点での観測
データが得られた時の（ｋ＋１）時点でのｘの推
定値を表わす。また、 ^Tは最適推定値を表わし、
Ｐ、Ｇはそれぞれ誤差の共分散行列とフイルタの
ゲインベクトルである。またＴは転値を表わす。

(4)〜(8)式をデータがサンプリングされる毎に計
算すれば、最適推定値としてx^（ｋ＋１｜ｋ＋１）
が得られる。ここで考えているシステム（船体操
縦系及びその観測系）は、信号過程と測定過程の
ダイナミツクスが時間的に変化しないので、シス
テムを定係数の方程式で表わすことができ、また
波浪等の外乱の変化も緩やかなので30分程度の短
い時間を考えれば、その統計的性質も定常として
取扱うことが可能である。従つて、カルマンフイ
ルタは定係数の線形ダイナミカルシステムとな
る。具体的には誤差の共分散行列及びカルマンフ
イルタのゲイン行列が一定となる。

定常状態の誤差の共分散及びフイルタゲインは
(5)、(6)、(8)式より G〓＝〔AP〓A^T＋ΓQΓ^T〕C^T〔Ｃ〔AP〓A^T＋
ΓQΓ^T〕C^T＋Ｒ〕^-1……(9) P〓＝〔Ｉ−〔AP〓A^T＋ΓQΓ^T〕C^T〔Ｃ〔AP〓A^T＋ΓQΓ
^T〕×C^T＋Ｒ〕^-1×Ｃ〕〔APA^T＋ΓQΓ^T〕……(10) ここで、G〓、P〓はそれぞれ定常状態におけるフ
イルタゲイン、誤差の共分散の推定値を表わす。
(10)式はニユートン・ラプソン法（Newton−
Raphson法）を用いて解くことができる結果を(9)
式に代入すればG〓が得られる。このため実際の信
号処理においては、実時間で(4)、(7)式を計算する
だけでよく、実現が容易となる。

ここではプラントノイズの平均値を０とした
が、実際には平均値０のプラントノイズはなく、
必ずあるバイアス成分を持つている。従つて(2)式
の代わりに、 ｘ（ｋ＋１）＝Ax(k)＋Bu(k)＋Γ（ｖ(k)＋v₀）
……（11） ここで、v₀：プラントノイズの平均値 を用いることも考えられるが計算量の増大を招
く。そこで、カルマンフイルタとは別途に推定す
る。

このプラントノイズのバイアス成分v₀は、系が
クローズドループであり定常状態において船体に
加わる外乱のバイアス成分と操作量（舵角）の平
均値とがバランスすると考えられるので、方位偏
差信号の平均値を用いて V〓₀＝ｈ・ψ_e ……（12） と推定できる（実際には定差方程式を用いる）。
これは従来のオートパイロツトで行なわれていた
積分制御と基本的に同じである。ただし、ｈは任
意常数であり、系の安定性を考慮して決定する。

これに対し、プラントノイズの分散Ｑと観測ノ
イズの共分散Ｒは、その推定を如何に適切に行な
うか否かでフイルタリングの良し悪しが決定され
る程重要である。特にプラントノイズは定量的に
把握し難くカルマンフイルタを実にシステムに適
用する場合に最も問題となるところである。

まず普通観測系は海象状態が変化してもあまり
影響を受けないと考えられるので、観測ノイズの
共分散Ｒは一定として扱う。一方、ＱはMehra
の白色ガウス雑音の共分散のオンライン推定を拡
張したGodboleの方法で求めることが考えられ
る。詳細は略すが、これはプラントノイズと観測
ノイズに相関があり、且つノイズの平均値が零で
ない場合も適用可能な分散推定のアルゴリズムで
あり、カルマンフイルタの係数の中で不明確なプ
ラントノイズの分散を定量的に把握できるという
意味では非常に有用な方法である。

しかしながら、このＱをGodboleの方法で求め
るアルゴリズムを用いて方位偏差（設定方位−船
首方位）及び旋回角速度の最適推定値を得、舵機
のPD（PID）制御を行なうと次のような問題が生
じることがわかつた。即ち、Calm sea（海面がお
だやかな状態）に於て、舵角変化は滑らかである
が保針性が悪くなつた。逆にRough sea（海面が
荒れている状態）では舵角が急激に大きくなり舵
機の酷使を招いた。これはGodboleの方法では
Calm seaのときプラントノイズの分散値を小さ
く見積り過ぎてフイルタリング効果はでたが、そ
の分、最適推定値に遅れを生じヨーイングが大き
くなつたのであり、Rough seaのときは方位偏差
あるいは旋回角速度に含まれる高い周波の成分
（ローリング周期に近い）をプラントノイズに起
因するものとみなすためにＱの値が非常に大きな
ものとなつてしまい、特に旋回角速度に含まれる
高周波成分に忠実に追従するようになつてしまう
からである。

また、推定値自体の安定性も悪く、同じ海象状
態でのバラツキが大きくなるという結果を生じ
た。

考慮するに時系列信号について、信号は、本来
もつている意味のある信号成分と意味のない雑音
成分が加え合わさつたものであると考えれば、フ
イルタリングとはその雑音成分を出来るだけ分離
して、意味のある信号成分のみを取り出すことで
ある。これを今の場合に当てばめれば、本来もつ
ている意味のある信号成分とは方位偏差、旋回角
速度に含まれる操舵系の固有周波数に近い低周波
成分のことであり、意味のない雑音成分とは単に
ジヤイロ等の誤差だけではなくて、波浪によつて
誘起された操舵では迎えることのできない高周波
成分を含めたもののことである。

このことを踏まえてもう一度システム方程式(3)
を見てみると、システム方程式自体はそのような
明確な区別は何も表現してはいない。Godboleの
アルゴリズムを適用すると、海象が荒れて操舵で
は抑えることのできない高周波の成分が多くなる
とこれらはすべてプラントノイズによつて誘起さ
れたものと見なされ、プラントノイズの分散Ｑの
推定値は非常に大きな値になつてしまう。こうし
てＱの推定値が大きくなると波浪によつて誘起さ
れた高周波成分はすべて意味のある信号と見なさ
れ、我々が望むフイルタリングとはまつたく異な
つたものになつてしまうのである。この原因は、
システム方程式が我々の望むフイルタリングを十
分表現していないことによるが、波浪によつて誘
起される高周波成分のみを表現するにはまだ運動
力学的に問題点も多く、またそれをシステム方程
式に組込むとカルマンフイルタの次数が増え計算
量が増大するという不都合もある。

また、Godboleの方法は、ホワイトノイズが前
提でインベーシヨン過程の自己相関を用いること
から、データのサンプリング時間が短くてインベ
ーシヨン過程の自己相関が強くなると分散が求め
られず、また、データ数が少ないと推定値のバラ
ツキが大きくなり、従つて、サンプリング時間を
長くしかつデータ数を十分大きくとる必要がある
ため計算の準備に長時間を要するという欠点もあ
つた。

本発明者はかかる上記手法の有する欠点に鑑み
鋭意研究を重ねた結果、フイルタ係数の推定方法
を工夫すると、方位偏差等の最適推定量が波浪に
よる高周波成分の影響を受けないように出来るこ
とを見出し、この発明をなすに至つた。

〔発明の目的〕

本発明は、簡便な構成によりカルマンフイルタ
の持つ機能を十分に発揮させて自動操舵系の制御
の安定性・速応性を高め、かつ、最適な天候調整
を自動的に確実に行なうことができる自動操舵装
置を提供することを、その目的とする。

〔発明の概要〕

本発明では、船体の回動運動を検出する回動運
動検出手段と、この回動運動検出手段より出力さ
れる旋回角速度信号、船首方位と設定方位より演
算される方位偏差信号、および舵角指示器より検
出された舵角信号、ならびに船速計より検出され
る船速信号をそれぞれ入力して自動操舵に必要な
旋回角速度および方位偏差に係る最適値を推定し
出力する天候調整用のカルマンフイルタ部を備え
ている。そして、カルマンフイルタ部が、その演
算に必要な所定のプラントノイズデータを出力す
るプラントノイズデータ発生回路と、所定の海象
パラメータに基づいて観測ノイズの推定を行うＲ
演算回路とを有すると共に、Ｒ演算回路とプラン
トノイズデータ発生回路の内のいずれか一方の回
路の入力段に、回動運動検出手段からの出力信号
に基づいて所定の海象パラメータを演算し出力す
るパラメータ演算部を設けると共に、Ｒ演算回路
の出力とプラントノイズデータ発生回路の内の他
方の回路の出力値を一定に設定し、パラメータ演
算部からの信号を入力するＲ演算回路又はプラン
トノイズデータ発生回路が、必要に応じて自己の
出力信号を適当な大きさの一定値に設定するリミ
ツタ機能を備えている、という構成を採つてい
る。これによつて前述した目的を達成しようとす
るものである。

〔発明の実施例〕

まず、本発明の一実施例の原理的説明を行な
う。前述したGodboleの方法を用いるカルマンフ
イルタのアルゴリズムでは海象が荒れても観測系
は影響を受けないとした。このように考える限り
システム方程式には波浪によつて誘起される高周
波成分を表現する必要がある。そこで、本実施例
ではシステム方程式は(2)のまま使用し、実際には
波浪によつて船体が駆動され高周波成分が発生す
るのであるが、これを観測系に含めて処理するこ
ととした。

即ち、海象が荒れてくると観測系も影響を受け
て観測ノイズが多くなつてくるとし、海象状態の
パラメータからＲの推定を行なう。カルマンフイ
ルタの性質上、Ｒの大小とフイルタリング効果の
大小が一致するため（Ｑは簡単のため一定とす
る）、海象状態が荒れてＲが大きくなると波浪に
よつて誘起された高周波成分がフイルトアウトさ
れることになるのである。この際、最適推定値は
システム方程式を主にして求められるので操舵系
の固有周波数に近い成分までろ過されることはな
い。

一方、海象状態はビユーフオート階級をパラメ
ータとすると個人差の介入を免れ得ず、また海象
状態が船体操縦系に与える影響もビユーフオート
階級に比例するものではなく、波やうねりの方向
によつて大きく左右される。よく知られているよ
うにビユーフオート階級が同じでもうねりの方向
が斜め後方の場合には操縦系に与える影響は大き
く、前方の場合にはほとんど影響がない。そこで
オートパイロツトから見た海象状態のパラメータ
としては旋回角速度など船体の回頭運動を検出し
所定の演算処理を施したものを用いる。例えばコ
ントローラの影響もそれ程受けない旋回角速度の
「r.m.s」を海象状態のパラメータとしＲはこのパ
ラメータに比例させて推定を行なうとすると、プ
ラントノイズ及び観測ノイズは、 Ｑ＝const. ……（13） Ｒ＝ｍ√ψ²の平均、０ ０、m′√ψ²の平均 ……（14） となる。ここで、m′は正の任意定数であり、系
の安定性等を考慮し実験的に決める。

但し、Calm seaにおいては、波浪によつて誘
起される旋回角速度のレベルが舵角によつて誘起
されるレベルと同程度となり、旋回角速度の「r.
m.s.」にコントローラの影響が大きく出る。又、
このとき、旋回角速度の「r.m.s.」に比例して観
測ノイズの共分散Ｒを小さくすると、最適推定値
に波浪による高周波成分が現れるので、リミツタ
を設けＲを所定レベル（例えばR_eで）一定とす
る。一方、Rough seaでは旋回角速度「r.m.s」
に比例して観測ノイズの共分散Ｒを大きくしたた
め最適推定値に遅れが出るようなときは必要に応
じてＲを一定（例えばR_h）とする（第３図参
照）。

次に第１図に基づいて、これを更に具体的に説
明する。

第１図は天候調整用のカルマンフイルタを含む
自動操舵機の系統図である。この第１図に於て、
符号１は制御対象としての船体を示す。この船体
１の船尾に設けられた舵機２は、舵機調節部３で
操舵される。これにより、目標針路の保針制御が
行なわれるようになつている。船体１には、各々
船首方位ψ、旋回角速度ψ〓、船速Ｓ、舵角δをそ
れぞれ測定するコンパス４、角速度計５、スピー
ドログ６、舵角指示器７が備えられている。ま
た、図示しない針路設定器で設定された設定方位
ψ_Iと前記コンパス４の出力する船首方位ψとの方
位偏差ψ_e（＝ψ_I−ψ）が比較器８で検出されたの
ち旋回角速度ψ〓、舵角δとともにカルマンフイル
タ部９へ送出される。

このカルマンフイルタ部９は、観測ノイズの推
定を行なうＲ演算回路１０、プラントノイズのＱ
値出力を行なうプラントノイズデータ発生部１
１、船体操縦指数Ｋ、Ｔを船速Ｓから推定しフイ
ルタ係数Ａ、Ｂを求めるAB演算回路１２、誤差
の共分散及びフイルタゲインＰ、Ｇを計算する
PG演算回路１３、方位偏差ψ_e及び旋回角速度ψ〓
の最適推定値ψ^_e、ψ〓を求める最適推定値演算回路
１４とから構成されている。角速度計５は船体１
の回頭運動検出手段としての機能を有し、検出し
た旋回角速度ψ〓がパラメータ演算部１５へ送られ
る。このパラデータ演算部１５は、一定時間内の
データを基に「r.m.s.」を求めて海象パラメータ
としてＲ演算回路１０へ送る。このＲ演算回路１
０は、第３図の線図に従つて観測ノイズＲの推定
を行なう。AB演算回路１２は、船速Ｓが一定以
上変化したとき、その船速Ｓに対応する船体操縦
性指数Ｋ、Ｔを得る機能を有している。これはシ
ステム特性の変動に対応するためのものであり、
実際には(2)式中のフイルタ係数Ａ、Ｂを計算す
る。PG演算回路１３は、各Ｒ、Ｑ、Ａ、Ｂデー
タに基づき(10)、(9)式によりＰ、Ｇを推定する。カ
ルマンフイルタ部９は、観測ノイズＲの推定が成
されたとき、又は、船速Ｓが一定以上変化したと
きにイニシヤライズされるようになつている。最
適推定値演算回路１４は、イニシヤライズされる
と初回の最適推定値x^（０｜０）と舵角ｕ（０）と
して適当な値を入れ、AB演算回路１２から送ら
れるＡ、Ｂデータ及びPG演算部１３から送られ
るＧデータに基づき、前述した比較器８及び角速
度計５から送られるＹデータとしてのψ_e、ψ〓並び
に舵角δがサンプリングされる毎に、順次(4)、(7)
式に従つて最適推定値x^（ｋ＋１｜ｋ＋１）の演
算を行なう。

このようにしてカルマンフイルタ部９でフイル
タリングされた最適推定値ψ^_e、ψ〓は、舵機調節部
３内のPD調節回路１６に送出され、 δ^* ₁＝K_P（ψ^_e＋T₀ψ〓） の演算がなされる。一方、舵機調節部３内のＩ調
整回路３Ａでは、（12）式を定差化した δ^* ₂＝（τ／T_I）Σψ_e(K) の演算が行われる。これらのδ^* ₁、δ^* ₂が加算器１８
で加算され指令舵角δ^*として前述した舵機２へ出
力されるようになつている。

ただし、前述したプラントノイズのバイアス成
分については、Ｉ調整回路３Ａで処理するかわり
に、第２図に示す如く、舵角指示器７とカルマン
フイルタの最適値演算回路１４との間にバイアス
処理部２０を装備し、このバイアス処理部２０に
て、 δ′＝δ−（τ／T_I）Σψ_e(K) の演算を行ない、バイアス成分を除去した舵角
δ′をカルマンフイルタの舵角入力とすることもで
きる。

次に第２実施例として前述した（13）、（14）式
の代わりに例えば、 Ｒ＝const. ……（16） 但し、ｎは正の任意定数（実験的に決める）と
し、プラントノイズＱを海象状態のパラメータに
反比例するように推定してもよい。何故ならカル
マンフイルタの性質上フイルタリング効果の大小
とＩの小大が一致するため（Ｒが一定のとき）、
海象状態が荒れてＱが小さくなれば波浪によつて
誘起された高周波成分がフイルトアウトされるか
らである。ただし、Calm sea、Rougn seaでは
第３図と同様にリミツタを設ける（第４図参照）。

尚、船体の回頭運動は船尾方位から検出するよ
うにしてもよく、又海象状態へのパラメータ化は
「r.m.s.」のほか、絶対値平均等他の手法を用い
てよく、更にノイズの推定も比例・反比例以外の
方法で行なつてよい。

〔発明の効果〕 以上説明したように本発明によれば、簡便なア
ルゴリズムでカルマンフイルタのノイズ係数を安
定して推定することができ、計算機の負担が軽減
するとともに、Calm seaで波浪による高周波成
分に追従して無駄舵をとつたり、Rough seaで最
適推定値に遅れを生じ船体が大きく蛇行したりす
ることなく、波浪によつて誘起された操舵系で抑
えることのできない高周波成分のみを有効にフイ
ルトアウトさせることができ、天候調整性能に優
れ、かつ、保針性が著しく高いという従来にない
優れた自動操舵装置を提供することができる。

【図面の簡単な説明】

第１図は本発明の一実施例に係る自動操舵系の
系統図、第２図はプラントノイズのバイアス処理
方法の他の実施例を示すブロツク図、第３図は第
一実施例における観測ノイズＲの推定方法を示す
線図、第４図は第２実施例におけるプラントノイ
ズＱの推定方法を示す線図である。 １……船体、２……舵機、３……舵機調節部、
３Ａ……Ｉ調整回路、４……コンパス、５……回
動運動検出手段としての角速度計、６……スピー
ドログ、７……舵角指示器、８……比較器、９…
…カルマンフイルタ部、１０……Ｒ演算回路、１
１……プラントノイズデータ発生回路、１２……
AB演算回路、１３……PG演算回路、１４……
最適推定値演算回路、１５……パラメータ演算
部。

Claims (1)

1. 【特許請求の範囲】 １ 船体の回動運動を検出する回動運動検出手段
   と、この回動運動検出手段より出力される旋回角
   速度信号、船首方位と設定方位より演算される方
   位偏差信号、および舵角指示器より検出された舵
   角信号、ならびに船速計より検出される船速信号
   をそれぞれ入力して自動操舵に必要な旋回角速度
   および方位偏差に係る最適値を推定し出力する天
   候調整用のカルマンフイルタ部を備え、 前記カルマンフイルタ部が、その演算に必要な
   所定のプラントノイズデータを出力するプラント
   ノイズデータ発生回路と、所定の海象パラメータ
   に基づいて観測ノイズの推定を行うＲ演算回路と
   を有すると共に、 前記Ｒ演算回路とプラントノイズデータ発生回
   路の内のいずれか一方の回路の入力段に、前記回
   動運動検出手段からの出力信号に基づいて所定の
   海象パラメータを演算し出力するパラメータ演算
   部を設けると共に、前記Ｒ演算回路の出力とプラ
   ントノイズデータ発生回路の内の他方の回路の出
   力値を一定に設定し、 前記パラメータ演算部からの信号を入力するＲ
   演算回路又はプラントノイズデータ発生回路が、
   必要に応じて自己の出力信号を適当な大きさの一
   定値に設定するリミツタ機能を備えていることを
   特徴とする自動操舵装置。