(57)【特許請求の範囲】
【請求項１】
船体の参照方位及び参照位置を出力する軌道計画部と、センサで検出された船体の方位および位置から船体の方位と位置とを参照方位及び参照位置に追従させるべく命令舵角を出力するフィードバック制御部と、を備えた船舶用自動操舵装置において、
前記フィードバック制御部は、方位誤差、航路誤差及び潮流の推定を行う推定器と、方位制御ループを構成する方位制御系フィードバックゲイン器と、前記方位制御ループを含む航路制御ループを構成する航路制御系フィードバックゲイン器とを備え、
前記推定器は、前記方位制御系フィードバックゲイン器へ修正量を入力し、
該修正量は、γｈを舵角オフセットδｏｒの修正量、γｔを航路制御ループの修正量としてγ＝γｈ＋γｔで表され、
γｔは、ｆ１を方位制御系フィードバックゲイン器のフィードバックゲインＦｈ（１）、
ｖｏ＾＝−Ｋｖδｏｒ＾、ここで、Ｋｖは横流れゲイン、δｏｒ＾はｙａｗ舵角オフセット、
ｖｃR＾を参照座標成分に変換され近似されたｓｗａｙ方向の潮流推定誤差、
ｕをｓｕｒｇｅ速度として、
【数１】
0006373205.tif 000100
で表されることを特徴とする船舶用自動操舵装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、航路制御システムの船舶用自動操舵装置に関する。
【背景技術】
【０００２】
船舶用自動操舵装置は、舵角を制御して参照方位に船首方位を追従させる方位制御システム（ＨＣＳ：Ｈｅａｄｉｎｇ  Ｃｏｎｔｒｏｌ  Ｓｙｓｔｅｍ）と、舵角を制御して計画航路に船体位置を追従させる航路制御システム（ＴＣＳ：Ｔｒａｃｋ  Ｃｏｎｔｒｏｌ  Ｓｙｓｔｅｍ）とに分けられる。舵角が変更されると、船体はｙａｗ角速度及びｓｗａｙ速度を生じるため、航路制御システムはこれらの運動を考慮して設計される。ここでｙａｗ角速度は、これを積分した船首方位としてセンサに検出されるが、ｓｗａｙ速度は通常検出されない未知量であり、且つこれを直接的に制御するアクチュエータを利用できない劣駆動量である。
【０００３】
また、船舶用自動操舵装置の対象船は船体が受ける風や船体の左右不釣り合いなどに起因してｙａｗ角速度及びｓｗａｙ速度にオフセット成分を有し、これらのオフセット成分は船体モデルにおいて舵角成分に換算して、ｙａｗ舵角オフセットδｏｒ、ｓｗａｙ舵角オフセットδｏｖとして、それぞれ、ｙａｗ運動モデル、ｓｗａｙ運動モデルに付与されて航路誤差を生じる。
【０００４】
また、航路制御に関連する技術として、状態推定器において方位誤差系を基礎とした方位制御系推定器と、航路誤差系を基礎とした航路制御系推定器とに分離して構成し、航路制御系推定器において、航路誤差系の状態量及び推定潮流ベクトルを推定する船舶用自動操舵装置が知られている（特許文献１参照）。
【０００５】
また、推定器が、方位制御系推定器と、航路制御系推定器と、対地座標系での潮流成分の推定を行う対地座標系潮流推定器とを備え、対地座標系潮流推定器が船首方位及び対地座標系における船体位置から参照方位を用いずに推定潮流ベクトルを推定する船舶用自動操舵装置が知られている（特許文献２参照）。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−２４８８９６号公報
【特許文献２】特開２０１３−８６７４５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
航路制御においては、舵角オフセット、波浪成分、潮流成分を含む外乱成分による航路誤差を修正する必要がある。外乱成分に起因する誤差対策は、１型サーボ系、すなわち航路誤差に積分制御する必要があるが、積分器は閉ループ特性に影響するために、制御システムの見直しが生じることにより、設計が煩雑となる問題がある。
【０００８】
本発明の実施形態は、上述した問題点を解決するためになされたものであり、航路誤差に積分制御することなく、外乱成分に起因する航路誤差を低減することができる船舶用自動操舵装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決するため、本実施形態の船舶用自動操舵装置は、船体の参照方位及び参照位置を出力する軌道計画部と、センサで検出された船体の方位および位置から船体の方位と位置とを参照方位及び参照位置に追従させるべく命令舵角を出力するフィードバック制御部と、を備えた船舶用自動操舵装置において、前記フィードバック制御部は、方位誤差、航路誤差及び潮流の推定を行う推定器と、方位制御ループを構成する方位制御系フィードバックゲイン器と、前記方位制御ループを含む航路制御ループを構成する航路制御系フィードバックゲイン器とを備え、前記推定器は、前記方位制御系フィードバックゲイン器へ修正量を入力し、該修正量は、γｈを舵角オフセットδｏｒの修正量、γｔを航路制御ループの修正量としてγ＝γｈ＋γｔで表され、γｔは、ｆ１を方位制御系フィードバックゲイン器のフィードバックゲインＦｈ（１）、ｖｏ＾＝−Ｋｖδｏｒ＾、ここで、Ｋｖは横流れゲイン、δｏｒ＾はｙａｗ舵角オフセット、ｖｃR＾を参照座標成分に変換され近似されたｓｗａｙ方向の潮流推定誤差、ｕをｓｕｒｇｅ速度として、
【００１０】
【数１】
0006373205.tif 000002
で表されることを特徴とする。
【発明の効果】
【００１１】
本発明の実施形態によれば、航路誤差に積分制御することなく、外乱成分に起因する航路誤差を低減することができる。
【図面の簡単な説明】
【００１２】
【図１】船舶用自動操舵装置と制御対象の全体のブロック図である。
【図２】フィードバック制御部の構成を示すブロック図である。
【図３】フィードバック制御部の詳細構成を示すブロック図である。
【図４】航路制御システムで用いる座標系を示す図である。
【図５】方位誤差と航路誤差を示す図である。
【図６】方位制御ループ及び航路制御ループを示すブロック図である。
【図７】方位制御ループを示すブロック図である。
【図８】方位制御ループの仕様を示す図である。
【図９】ＧＨｈ４Δａ（ｓ）の根軌跡を示す図である。
【図１０】ＧＨｈ４Δａ（ｓ）の漸近線を示す図である。
【図１１】ＧＨｈ４Δｂ（ｓ）の根軌跡を示す図である。
【図１２】航路制御ループを示すブロック図である。
【図１３】ＧＨｔ（ｓ）の根軌跡を示す図である。
【図１４】ｒ１とｒ１’のベクトル表現を示す図である。
【図１５】航路制御ループの仕様を示す図である。
【図１６】積分動作なし、修正量なしの場合のシミュレーション結果を示す図である。
【図１７】積分動作あり、修正量なしの場合のシミュレーション結果を示す図である。
【図１８】積分動作なし、修正量ありの場合のシミュレーション結果を示す図である。
【図１９】積分動作なし、修正量あり、波浪成分ありの場合のシミュレーション結果を示す図である。
【発明を実施するための形態】
【００１３】
以下、図面を参照しながら、本発明の実施形態について説明する。以降の説明において、各記号は、変数、修飾、添字として、以下の表のように定義される。
【００１４】
【表１】
0006373205.tif 000003
【００１５】
【表２】
0006373205.tif 000004
【００１６】
【表３】
0006373205.tif 000005
【００１７】
１．１  船舶用自動操舵装置の構成
まず、本発明の船舶用自動操舵装置を含むシステムについて説明する。図１は、船舶用自動操舵装置と制御対象の全体のブロック図である。図２は、フィードバック制御部の構成を示すブロック図である。図３は、フィードバック制御部の詳細構成を示すブロック図である。なお、図２及び図３においてδｏ＾＝δｏｒ＾を意味するものとする。
【００１８】
図１に示すように、船舶用自動操舵装置１は、計画航路に船体位置を追跡させるために舵を制御する装置であり、軌道計画部１１、軌道航路誤差演算部１２、フィードバック制御部１３、加算器１４及び各パラメータを同定する図示しない同定器を備えている。誘導システム２から計画航路及びセンサ類４のスピードログからの船速Ｕ（正確には船体のｓｕｒｇｅ速度ｕ）が軌道計画部１１に入力され、軌道計画部１１からは参照方位ψＲ、参照方位ｘＲ、ｙＲといった参照信号及び変針中にはフィードフォワード舵角δＦＦが出力される。
【００１９】
船体３のセンサ類４は、船体３のｓｕｒｇｅ速度ｕを検出するスピードログ、船体３の船首方位ψを検出するジャイロコンパス、ＧＰＳ等の衛星測位システム（ＧＮＳＳ）からの船体位置（ｘ，ｙ）を検出するＧＮＳＳセンサを含む。
【００２０】
軌道航路誤差演算部１２には、船首方位ψ、船体位置（ｘ，ｙ）等のセンサ類４からの検出信号が入力され、軌道航路誤差演算部１２は、参照方位ψＲ、参照方位ｘＲ，ｙＲと、検出信号との比較を行い、方位誤差ψｅ、航路誤差ｘｅ，ｙｅ等を出力する。
【００２１】
船舶用自動操舵装置１の閉ループ系は、船体モデルと外乱モデルとからなる制御対象と、フィードバック制御部１３とから構成される。フィードバック制御部１３は、図２に示すように、推定器１３１と制御系フィードバックゲイン器１３２とを備える。推定器１３１には、軌道航路誤差演算部１２からの方位誤差ψｅ、航路誤差ｘｅ、ｙｅが入力される。
【００２２】
推定器１３１は、図３に示すように、方位制御系推定器１３１Ａ、航路制御系推定器１３１Ｂ、対地座標系潮流推定器１３１Ｃを備える。方位制御系推定器１３１Ａは、方位誤差の推定を行い、外乱が除去された推定方位誤差ψｅ＾、推定旋回角誤差ｒ＾といった方位に関する状態量の推定値を出力する。航路制御系推定器１３１Ｂは、横方向の推定航路誤差ｙｅ＾といった航路に関する状態量の推定値を出力する。対地座標潮流推定器１３１Ｃは、対地座標系での演算を行い、推定潮流ベクトルｕｃ＾，ｖｃ＾といった潮流に関する推定値を出力する。
【００２３】
制御系フィードバックゲイン器１３２は、方位制御系フィードバックゲイン器１３２Ａと航路制御系フィードバックゲイン器１３２Ｂとを備える。方位制御系フィードバックゲイン器１３２Ａは、推定方位誤差ψｅ＾、推定旋回角誤差ｒ＾に対してフィードバックゲインを掛ける。航路制御系フィードバックゲイン器１３２Ｂは、推定航路誤差ｙｅ＾に対してフィードバックゲインを掛ける。方位制御系フィードバックゲイン器１３２Ａと航路制御系フィードバックゲイン器１３２Ｂによる結果が加算されてフィードバック舵角δＦＢが出力される。
【００２４】
船体３の操舵機は、上述した構成による船舶用自動操舵装置１による命令舵角δＣに比例した舵角を動かすため、船体３は、舵角によって旋回角速度を生じ、方位、位置が変化する。また旋回角速度の発生と共に、斜航角（横方向速度）が発生する。
【００２５】
２．  定式化
２．１  座標系
ここで、航路座標系について説明する。図４は、航路制御システムで用いる座標系を示す図である。図５は、方位誤差と航路誤差を示す図である。
【００２６】
図４に示すように、航路制御システムで用いる座標系は、地球固定座標系Ｏ−ＸＹ、船体固定座標系Ｇ−ＸＢＹＢと参照座標系ＯＲ−ＸＲＹＲからなり、いずれも右手系３軸直交座標系である。これらの座標系においてＺ軸は重力方向を正とし、回転極性は右ねじ方向を正とする。なお、座標系はＸ軸、Ｙ軸の２次元を用いるため、図４においてＺ軸は省略される。また、地球固定座標系はＸ軸を北向きにとり、船体固定座標系はＸ軸を船首方位にとり、参照座標系はＸ軸を前進方向にとる。
【００２７】
航路制御システムにおいて計画航路は、図５においてＷＰとして示されるウェイポイントを結ぶ経路として設定され、直線航路と曲線航路からなる。参照航路は計画航路上に置かれ、直線航路上においては原点を適当な位置に固定したものとして、また、曲線航路上においては旋回中心と船体位置とを結ぶ線との交点に置いたものとして定められる。なお、図４，Ｂにおいて、ｕはｓｕｒｇｅ速度、ｖはｓｗａｙ速度、ｒは旋回角速度、ψは船首方位、ψＲは参照方位、δは舵角、ｘ，ｙは船体位置、ｘＲ，ｙＲは参照航路、Ｒは旋回半径、ｘＣ，ｙＣは旋回中心、Ｕは対水速度でＵ＝√（ｕ２＋ｖ２）、βは斜航角でｓｉｎβ＝ｖ／Ｕ、添字ｃは潮流成分を示す。これらより、方位誤差ψｅ及び航路誤差ｘｅ，ｙｅは次式になる。
【００２８】
【数２】
0006373205.tif 000006
ここで、ｄｉｓｔｘ＝ｘ’−ｘｃ、ｄｉｓｔｙ＝ｙ’−ｙｃ、Ｍは座標変換行列、
【００２９】
【数３】
0006373205.tif 000007
【００３０】
２．２  制御対象
ここで、制御対象について説明する。制御対象は船体モデルと外乱モデルから構成される。船体モデルは、ｓｕｒｇｅ速度を一定として、野本の応答モデルを改良した本発明者による応答モデルを用いて、
【００３１】
【数４】
0006373205.tif 000008
に定める。ここでｓはラプラス演算子、Δｃ（ｓ）、δｃは命令舵角、Ｋｒは旋回力ゲイン、Ｋｖは横流れゲイン、Ｔｒ，Ｔｒ３，Ｔｖは時定数を示す。本実施の形態においては、安定船を対象とし、δｃは操舵機応答が方位制御応答に比べて十分に速いものとしてδｃ≒δとおく。この船体モデルは野本の船体モデルと比較して、Ｔｒ３を有し、Ｔｖ＝Ｔｒという特徴をもつ。（５）式を次式のように変更する。
【００３２】
【数５】
0006373205.tif 000009
ここでＣＴ３＝１−Ｔｒ３／Ｔｒである。上式の状態空間表現を次式に示す。
【００３３】
【数６】
0006373205.tif 000010
ここでｘｒ＝［ψｒｘ］Ｔ、添字Ｔは転置行列、Δａ，Δｂはパラメータ不確かさを示し、Δａ，Δｂは想定値（添字〜）とノミナル値の差として、
【００３４】
【数７】
0006373205.tif 000011
になる。Ｔｒ３のパラメータ不確かさはＴｒ３／Ｔｒ＜≒０．１として影響を与え難いため省く。またノミナル値を初期値の速度変化によって定めると
【００３５】
【数８】
0006373205.tif 000012
になる。ここで添字０は初期値、Ｋはゲイン、Ｔは時定数を示す。
【００３６】
外乱モデルは、舵角オフセットδｏｒ，δｏｖ、波浪成分ψｗ及び図４に示される潮流成分ｕｃ，ｖｃとする。ここで、δｏｒ，δｏｖは、風や船体特性などに誘起された方位軸回り（ｙａｗ方向）に作用する角速度、ｓｗａｙ方向に作用する速度をそれぞれ舵角換算したものであり、方位誤差と航路誤差を発生させる。ψｗは白色ノイズνが入力した狭帯域フィルタ出力を方位換算したものであり、δｃに無効舵を発生させる。ｕｃ，ｖｃは航路誤差を発生させる。よって、外乱モデルを
【００３７】
【数９】
0006373205.tif 000013
とする。ここでｘｗ＝［ξ  ψｗ］Ｔ、ξは変数、Ｋｗ，ζｗ，ωｗはそれぞれ波浪モデルのゲイン、減衰係数、固有周波数として、
【００３８】
【数１０】
0006373205.tif 000014
【００３９】
２．３  航路誤差モデル
航路誤差モデルを導出する。図６は、方位制御ループ及び航路制御ループを示すブロック図である。船体の対地速度は
【００４０】
【数１１】
0006373205.tif 000015
になる。ここで添字ｇは対地速度成分を示す。これにより参照座標系における航路誤差の速度成分は次式になる。
【００４１】
【数１２】
0006373205.tif 000016
ここでｕＲ，ｖＲは参照速度を示し、航路保持の横方向成分はゼロになり、ψｅを微小として近似ｃｏｓψｅ≒１，ｓｉｎψｅ≒ψｅを用い、
【００４２】
【数１３】
0006373205.tif 000017
【００４３】
本実施の形態において、船舶用自動操舵装置１はｓｕｒｇｅ方向を制御せず、航路保持制御システムを（１５）式から図６に示すように方位制御ループと航路制御ループから構築する。方位制御ループはψｅを制御し、航路制御ループはｖｅを制御し、ｕｅは制御されない。図６に示す航路保持制御システムは、（１）式、（２）式及び（１５）式を用い、また、簡略化のためψＲ＝０を用いる。また、この航路保持制御システムにおいて、外乱モデルとして舵角オフセットと潮流成分とが扱われ、波浪成分は平均値がゼロのため省かれる。
【００４４】
２．４  定常誤差
本実施の形態における方位と航路の定常誤差を外乱モデルのオフセット成分から導出する。制御システムの伝達特性は図６から
【００４５】
【数１４】
0006373205.tif 000018
になる。ここで、Γ（ｓ），γは修正項を示す。Ｆｈ（ｓ）は比例ゲインｆｐと微分ゲインｆｄからなる方位フィードバック、Ｆｔ（ｓ）は航路ゲインｆｙ、積分ゲインｆｉからなる航路フィードバックであり、それぞれ、
【００４６】
【数１５】
0006373205.tif 000019
また、Ｖ（ｓ）は（１７）、（１８）式より
【００４７】
【数１６】
0006373205.tif 000020
になる。ここで
【００４８】
【数１７】
0006373205.tif 000021
よって、方位偏差と航路偏差は（１７）〜（２０）式、（２３）式より
【００４９】
【数１８】
0006373205.tif 000022
になる。定式の分母である閉ループ特性は安定として、Ｆｈ（ｓ）は固定して、Ｆｔ（ｓ）は積分制御の有無による影響を調べる。方位偏差、航路誤差のそれぞれの定常値を求めると
【００５０】
【数１９】
0006373205.tif 000023
になる。方位偏差は横流れ速度成分による斜航角を持ち、航路誤差は積分制御の有無により異なる。積分制御は外乱オフセットに対して１型サーボ特性をもつ。よって、修正量が航路誤差をゼロにすることができるかが重要となる。
【００５１】
３．  方位制御ループ
ここで方位制御ループについて説明する。図７は、方位制御ループを示すブロック図である。
【００５２】
図７に示すように、方位制御ループは、パラメータ不確かさをもつ船体モデルと、舵角オフセットと波浪成分とを含む外乱モデルとからなる制御対象において、方位誤差ψｅ＝ψ‐ψＲをゼロに収斂させる制御システムとして構成される。簡単化のため、一定の参照方位ψＲをゼロとおく。また、方位軸回り扱うため、潮流の影響を受けないものとする。この制御システムは、方位制御系推定器１３１Ａ（確定的オブザーバ）と方位制御系フィードバックゲイン器１３２Ａ（状態フィードバック）とからなり、閉ループ安定性及び外乱除去性を確保する。このとき、方位制御ループの状態空間表現は次式になる。
【００５３】
【数２０】
0006373205.tif 000024
ここでｘｈ＝［ｘｒ  ｘｗ  δｏｒ］Ｔ、添字＾は推定値、添字￣は検出値、ψ￣＝−ψ−ψｗは検出方位、Ｋｈは推定ゲイン、Ｆｈはフィードバックゲイン、Ｏｉ×ｊはｉ行ｊ列のゼロ行列を用いて
【００５４】
【数２１】
0006373205.tif 000025
【００５５】
３．１  フィードバックゲイン
方位制御ループのフィードバックゲインは、方位制御系推定器１３１Ａを用いたノミナル値の特性多項式に仕様の設計パラメータを与えて求められる。特性多項式は（２５）式の分母に（２１）式を代入することで得られる。
【００５６】
【数２２】
0006373205.tif 000026
ここでＤｈ（ｓ）は方位制御ループの特性多項式、
【００５７】
【数２３】
0006373205.tif 000027
ここでζｈは減衰係数、ωｈは固有周波数、ｆｐとζｈを図８に示す設計パラメータとし、上式に与えると
【００５８】
【数２４】
0006373205.tif 000028
が求まる。本実施の形態において、制御システムは方位制御ループのωｈを基準に設定される。フィードバックＦｈ（ｓ）はｒの利用を想定しているため、（７）式のｒｘの利用に変更する。命令舵角δｃ＝−ｆｄｒ−ｆｐψｅに（７）式のｒを代入すると次式を得る。
【００５９】
【数２５】
0006373205.tif 000029
これより、フィードバックゲインは舵角オフセットδｏｒの修正量γｈ＝−δＯＲ＾を加えると次式になる。
【００６０】
【数２６】
0006373205.tif 000030
【００６１】
３．２  推定ゲイン
方位制御ループの推定ゲインは方位制御系推定器１３１Ａの特性多項式を仕様の特性多項式に一致させることによって設定する。すなわち
【００６２】
【数２７】
0006373205.tif 000031
と定める。ここでｄｅｔは行列式、Ｉは適当な単位行列、Ｄｅｈ（ｓ），Ｄｅｗ（ｓ），Ｄｅｏ（ｓ）はそれぞれ方位推定、波浪推定、舵角オフセット推定に対応する特性多項式であり、
【００６３】
【数２８】
0006373205.tif 000032
推定ゲインの設計パラメータは図８に示される。推定ゲインＫｈは推定係数ρｈを与えると、補足として後述する推定ゲインの計算方法から求まる。
【００６４】
３．３  推定係数ρｈ
推定係数ρｈを設定する方法について説明する。上述したようにρｈが求まると推定ゲインＫｈが確定する。船体モデルは近似に起因するモデル化誤差をもつ。また、船体パラメータは速力、喫水やトリムの状態が変化するために変動成分をもつ。このようなパラメータ変動成分やモデル化誤差をパラメータ不確かさとして集約し、制御対象に組み込むと閉ループ安定性は変化する。このときρｈはパラメータ不確かさによる閉ループ安定性の仕様を満足するように設定する。
【００６５】
上述したように、方位制御ループにおけるフィードバックゲインは船体パラメータのノミナル値に基づいて求められ、推定ゲインはパラメータ不確かさを考慮した閉ループ安定性に基づいて求められる。パラメータ不確かさに対する閉ループ安定性のロバスト性は、推定器を除いた制御システムにおいては高いが、推定器を含んだ制御システムにおいては低下する。外乱除去性を確保する推定器は必須であるため、推定係数はパラメータ不確かさを含んだ閉ループ制御システムを用いる必要がある。
【００６６】
パラメータ不確かさを制御対象に与えて、閉ループ制御システムの特性多項式を導出する。特性多項式に基づいて閉ループ安定性（減衰係数）に与える影響が大きいパラメータ不確かさを選ぶ。そのパラメータ不確かさによる減衰係数の設計パラメータを満足する推定係数ρｈを求める。（２８）式の方位制御ループにおいて、拡大系の特性行列ΑｈΔは推定誤差をη＝ｘｈ−ｘｈ＾とおくと、
【００６７】
【数２９】
0006373205.tif 000033
になる。ここで、
【００６８】
【数３０】
0006373205.tif 000034
よって、ＡｈΔの特性多項式をノミナル項とパラメータ不確かさ項とに分離し、Δａ，Δｂを同時に加えないとすれば、
【００６９】
【数３１】
0006373205.tif 000035
になる。ここでｆ１＝Ｆｈ（１），ｆ２＝Ｆｈ（２）を用いて、
【００７０】
【数３２】
0006373205.tif 000036
上式は項数が多く取扱いが難しいため、制御対象を船体モデルのみとする。このとき、推定器は２次系に、閉ループは４次系になり、特定多項式は次式になる。
【００７１】
【数３３】
0006373205.tif 000037
なお、２次系の推定ゲインは後述する（８６）式を用いる。
【００７２】
閉ループ安定性の尺度を特性多項式で最小の減衰係数と定め、減衰係数に対するΔａ，Δｂの影響を以下に説明する。まず、Δａの場合、推定係数をρｈ≫１とすれば、（３７）式は
【００７３】
【数３４】
0006373205.tif 000038
と近似できる。パラメータ不確かさの範囲を次式と定める。
【００７４】
【数３５】
0006373205.tif 000039
これより、減衰係数は（３９）式より
【００７５】
【数３６】
0006373205.tif 000040
のとき最小値となる。後述する検証の条件より−１／３ωｈＴｒ≒−０．２９，ζｈ４Δａ≒０．４２になる。よって、Δａの影響は、推定係数をρｈ≫１とすればρｈに無関係になる。また、（３７）式においてΔａをゲインに、閉ループ伝達関数を
【００７６】
【数３７】
0006373205.tif 000041
におくと、特性多項式はＤｈ４Δａ（ｓ）＝１＋ΔａＧＨｈ４Δａ（ｓ）になる。上式の根軌跡の一例を図９に示す。図９において、×は極、○はゼロ点を示し、漸近線の本数（上式で分母の極数から分子のゼロ点数を差し引いた数）は推定器の次数に関係なく実軸上の１本になる。
【００７７】
次に、Δｂの場合、推定係数ρｈはΔａの結果より、Δｂに関する特定多項式から求める。（３８）式においてΔｂをゲインに、閉ループ伝達関数を
【００７８】
【数３８】
0006373205.tif 000042
におく。図１０に示すように、上式の漸近線は３本になり、Δｂの正負で閉ループは不安定になる。減衰係数はΔｂ＜０の場合には１以外に指定することができないが、Δｂ＞０の場合には０から１まで指定することができる。パラメータ不確かさの範囲を次式と定める。
【００７９】
【数３９】
0006373205.tif 000043
（４２）式の根軌跡を図１１に示す。
【００８０】
ここで、推定係数ρｈの設定に関する仕様をまとめる。
【００８１】
＜１＞パラメータ不確かさ及び設計パラメータを次式とする。
【００８２】
【数４０】
0006373205.tif 000044
【００８３】
＜２＞（３６）式の７次特性多項式を因数分解し、最小の減衰係数を設計パラメータに一致させる推定係数ρｈを求める。
【００８４】
【数４１】
0006373205.tif 000045
ここで添字＊はΔｂを含んだ場合を示す。なお、上式の算法については後述する。
【００８５】
４．  航路制御ループ
次に、航路制御ループの枠組みとそのゲインの設定を説明する。図１２は、航路制御ループを示すブロック図である。
【００８６】
図１２に示すように、航路制御ループの枠組みは方位制御ループと同様である。航路制御ループの状態空間表現は次式になる。
【００８７】
【数４２】
0006373205.tif 000046
ここで、ｘｔ＾＝［ｙｅ＾  ｖｅｏ＾］Ｔ、ｖｅｏ＾はモデル化、検出信号と積分計算などの誤差を吸収するオフセット成分、ｘｔ∫はｙｅ＾の積分量、Ｋｔは推定ゲイン、Ｆｔはフィードバックゲイン、γｔは修正量を示し、
【００８８】
【数４３】
0006373205.tif 000047
システム行列Ａｔと入力行列Ｂｔに制御対象のパラメータを含まない構成を採用し、推定器がパラメータフリーになる。また、図１２においては省略されているが、航路制御ループには潮流成分を推定する対地座標系潮流推定器１３１Ｃが含まれる。
【００８９】
４．１  フィードバックゲイン
航路制御ループにおけるフィードバックゲインは方位制御ループの場合と同様に求め
【００９０】
【数４４】
0006373205.tif 000048
とする。ここで、ｆｙは航路ゲイン、ｆｉは積分ゲインを示す。ｆｙは航路制御ループを安定化し、ｆｉを除いて求める。ｆｉは航路制御ループを外乱のオフセット成分に対して１型サーボ系に構成し、ｆｙを用いて求める。
【００９１】
４．１．１  航路ゲイン
航路ゲインは航路制御ループの特性多項式Ｄｔ（ｓ）に設計パラメータの減衰係数ζｔを与えて求める。Ｄｔ（ｓ）は（２５）式の分母を整理すると次式になる。
【００９２】
【数４５】
0006373205.tif 000049
ここでｚはゼロ点、ａｔは係数、ζｔは減衰係数、ωｔは固有周波数、
【００９３】
【数４６】
0006373205.tif 000050
（４８）式において、ｓの係数を比較すると次式になる。
【００９４】
【数４７】
0006373205.tif 000051
（４８）式からωｔに関して解くと、３次方程式は次式になる。
【００９５】
【数４８】
0006373205.tif 000052
上式で係数の符号は正負あり、解の符号についても同様である。よって、上式にζｔを与えるとωｔの解は正の最小値として求まり、その最大値はωｈになる。これよりａｔ，ｆｙは次式から求まる。
【００９６】
【数４９】
0006373205.tif 000053
【００９７】
４．１．２  設計パラメータζｔ
設計パラメータζｔは、航路制御ループの性質に基づいて設定する。その特性多項式は、ｓｗａｙ船体運動から起因したＣｔ＜０およびｚ＜０の特徴をもつ。そのため航路ゲインを大きくすると、共役根は不安定側に移動して閉ループを不安定にする。（４８）式にゼロ点なしの場合を追加すると次式になる。
【００９８】
【数５０】
0006373205.tif 000054
上式と等価な閉ループ伝達関数はｆｙをゲインとして
【００９９】
【数５１】
0006373205.tif 000055
になる。ここでＤｔ（ｓ）＝１＋ｆｙＧＨｔ（ｓ）である。
【０１００】
上式の根軌跡を図１３に示す。図１３においては、ゼロ点がある場合は実線で示され、ゼロ点がない場合は破線で示され、（ａ）は広範囲を示し、（ｂ）は原点付近の拡大を示す。共役根は極×から移動し、（ａ）より、ゼロ点がない場合は漸近線±６０度に収束し、ゼロ点がある場合はゼロ点○と正の値に向かう。一方、（ｂ）より、ゼロ点がある共役根はゼロ点がない共役根より実軸側に移動する。（５２）式のゼロ点がある場合において、ｆｙに関する特性根の感度を求めるため、微分すると
【０１０１】
【数５２】
0006373205.tif 000056
になる。ここで
【０１０２】
【数５３】
0006373205.tif 000057
これより、感度はｆｙに関するｓの微係数に相当し
【０１０３】
【数５４】
0006373205.tif 000058
になる。ここで
【０１０４】
【数５５】
0006373205.tif 000059
【０１０５】
共役根の極（ｆｙ＝０）での感度を求める。極は方位制御ループの根に相当し、その共役根を次式に選ぶ。
【０１０６】
【数５６】
0006373205.tif 000060
ここで減衰係数ζｒ１＝ζｈと偏角θｒ１との関係はζｒ１＝−ｃｏｓθｒ１になり、ａｒ１＝ωｈ２，θｒ１＝（３／４）π，ｒ１を（５５）式に代入すると
【０１０７】
【数５７】
0006373205.tif 000061
になる。ここで
【０１０８】
【数５８】
0006373205.tif 000062
になる。ｚ＝−∞は（５２）式でゼロ点なしの場合に相当する。よって、極での感度は（５４）式より次式になる。
【０１０９】
【数５９】
0006373205.tif 000063
ここでθ＝θｎ−θｄであり、このとき、感度の偏角は次式になる。
【０１１０】
【数６０】
0006373205.tif 000064
よって偏角はゼロ点の存在により−４５度より下向きになる。したがって、極ｒ１の位置変化は航路ゲインを微小として
【０１１１】
【数６１】
0006373205.tif 000065
に近似できる。上式の関係を図１４に示す。ｒ１’の減衰係数はθ＜−π／４からζｔ’＞ζｈになる。また、共役根はｆｙが大きくなると不安定になる傾向をもつ。この結果、ζｔはζｈから増加し最大値に達してその後減少しζｈを通過する。図１３（ｂ）中、ζｔはζｈとの比較において、小の黒丸が最大、中の黒丸が等価、大の黒丸が減少を示す。設計パラメータである減衰係数は、共役根と実数根のバランスを考慮して次式の条件を選ぶ。
【０１１２】
【数６２】
0006373205.tif 000066
【０１１３】
４．１．３  積分ゲイン
積分ゲインは閉ループ安定性を主に外乱除去性を従に考慮し，既に求めた航路ゲインを用いて求める。ここで外乱除去性は外乱を抑制するまでの応答時間を意味する。また、積分ゲインに比例して閉ループ安定性が低下することに起因して、応答時間を早めるにしたがって過渡現象が大きくなる傾向がある。特性多項式は積分要素の導入によって３次式から４次式になり、新たに原点付近に共役根が派生する。積分ゲインは設計パラメータの減衰係数ζｉを指定して計算する。
【０１１４】
fｉは航路制御ループの特性多項式Ｄｔｉ（ｓ）に設計パラメータζｉを与えて求める。Ｄｔｉ（ｓ）は（２５）式の分母より
【０１１５】
【数６３】
0006373205.tif 000067
になる。ここで添字ｔｉ，ｉは、それぞれ、Ｄｔ（ｓ）の共役根、実根から派生したものを示す。上式において、ｓの係数を比較すると
【０１１６】
【数６４】
0006373205.tif 000068
になる。上式をωｉに関して解くと、４次方程式は次式になる。
【０１１７】
【数６５】
0006373205.tif 000069
上式にζｉを与えれば、ωｔの解は正の最小値として求まる。その最大値はＤｔ（ｓ）の共役根付近になる。よってωｔｉ，ζｔｉとｆｉは次式から求まる。
【０１１８】
【数６６】
0006373205.tif 000070
【０１１９】
フィードバックによる閉ループの根配置は、初めに方位制御ループの２根がｆｐ，ζｈによって決まり、次に航路制御ループの３根がζｔ＝ζｈによって決まり、最後に航路制御ループに積分特性をもつ４根がζｉによって決まる。
【０１２０】
４．１．４  閉ループ安定性
航路制御ループにおけるパラメータ不確かさに対する閉ループ安定性（ロバスト性）について説明する。パラメータ不確かさは（１０）式の速度変化を用いる。航路制御ループはパラメータフリーの推定器を省き、フィードバックのみを用いる。方位制御ループの場合、（３１）式に速度変化を適用すると
【０１２１】
【数６７】
0006373205.tif 000071
になり、減衰係数は速度を修正しなければ，ζｈ≠ζｈ０になる。また、航路制御ループの場合、（４９）式に速度変化を適用すると
【０１２２】
【数６８】
0006373205.tif 000072
になる。上式を（５０）、（５１）式に代入すると
【０１２３】
【数６９】
0006373205.tif 000073
になる。よってｆｙは速度修正に無関係で一定値になり、減衰係数はζｔ＝ζｔ０を保つ。ただしζｈ＝ζｈ０の場合。
【０１２４】
一方、積分ゲインを用いた場合、（６５）、（６６）式に速度変化を適用すると
【０１２５】
【数７０】
0006373205.tif 000074
になる。よってｆｉはζｉを一定に保つため、速度修正が必要になる。
【０１２６】
比例ゲインを除く制御ゲインにおいて、微分ゲインと積分ゲインは共に時間次元のために速度変化の影響を受け易く、航路ゲインは時間次元でないためにその影響を受け難い。したがって、航路制御ループの制御ゲインは閉ループ安定性を確保させるため，積分ゲインを用いず航路ゲインで対応すべきである。しかしながらそのためには、（２７）式に示す航路誤差が、修正量によってほぼゼロに修正されることが必要となる。
【０１２７】
４．２  状態推定器
航路制御ループは航路制御系推定器１３１Ｂおよび対地座標系潮流推定器１３１Ｃをもつ。航路制御系推定器１３１Ｂは検出信号に含まれる外乱成分（ポジションジャンプ、波浪成分等）を除去して適切な信号を抽出する。対地座標系潮流推定器１３１Ｃは潮流成分の他に船体を移動させる要因も含み、航路保持中だけでなく曲線航行中でも動作する。これらの推定器は同一な構成をもち、推定ゲインは固有角周波数の設計パラメータを与えて求める。
【０１２８】
４．２．１  航路制御系推定器
航路誤差推定式は（４６）式より
【０１２９】
【数７１】
0006373205.tif 000075
になる。ここでＫｔ＝［ｋｔ１  ｋｔ２］Ｔは航路推定ゲイン、ｙｅ−は航路誤差検出量。上式の特性多項式を次式と定める。
【０１３０】
【数７２】
0006373205.tif 000076
ここでωｅｔ，ωｅｔｏは仕様の角周波数を示す（詳細は図１５を参照）。これより航路推定ゲインは次式になる。
【０１３１】
【数７３】
0006373205.tif 000077
【０１３２】
４．２．２  対地座標系潮流推定器
潮流成分は航行中に変化するため、航路制御においてそれを常に推定できることが要求される。潮流成分は（１６）式に示すように参照方位の関数が要素になるため、そのままでは非線形になり取り扱いが難しい。そこで潮流成分推定（潮流推定）を対地座標で構成すると、潮流成分は方位を含まない線形式で記述できる。なお潮流推定は船体位置の移動速度から求めるため、風浪や船体の不釣合いによる移動速度を分離できず含む。
【０１３３】
対地速度の推定値は（１４）式より次式になる。
【０１３４】
【数７４】
0006373205.tif 000078
【０１３５】
潮流推定式を上式から構成すると、ｘ系、ｙ系に関して相互干渉せず同形になり、次式のように分離される。
【０１３６】
【数７５】
0006373205.tif 000079
ここでｘ−，ｙ−は船体位置検出量、ｋｃ１，ｋｃ２は潮流推定ゲインを示す。よって潮流推定式は（７０）式と同形になるので、潮流推定ゲインも同様に定まる。上式の特性多項式を次式と定める。
【０１３７】
【数７６】
0006373205.tif 000080
ここでωｅｃ，ωｅｃｏは仕様の角周波数を示す（詳細は図１５を参照）。これより潮流推定ゲインは次式になる。
【０１３８】
【数７７】
0006373205.tif 000081
【０１３９】
次に、船体速度成分に起因する潮流推定誤差を示す。潮流推定の定常値は（７３）式より、次式になる。
【０１４０】
【数７８】
0006373205.tif 000082
【０１４１】
対地船体速度および潮流速度の推定値を次式と定める。
【０１４２】
【数７９】
0006373205.tif 000083
ここでΔは誤差項を示す。上式を（７８）式に代入すると、潮流推定誤差は
【０１４３】
【数８０】
0006373205.tif 000084
として求まる。ここでΔｕ，Δｖはそれぞれｓｕｒｇｅ速度の誤差、ｓｗａｙ速度の誤差を示す。
【０１４４】
４．２．３  修正量
航路制御ループの修正量について説明する。この修正量は、推定器１３１から方位制御系フィードバックゲイン器１３２Ａへ入力される。（７９）式を参照座標成分に変換し近似すると、潮流推定誤差は
【０１４５】
【数８１】
0006373205.tif 000085
になる。このとき修正量にｖｃＲ＾を用いると（２７）式から
【０１４６】
【数８２】
0006373205.tif 000086
になる。ここで
【０１４７】
【数８３】
0006373205.tif 000087
よって、航路誤差をゼロにする修正量は次式から得られる。
【０１４８】
【数８４】
0006373205.tif 000088
【０１４９】
上述したように、潮流推定はｓｗａｙ速度のオフセット成分Δｖに起因して誤差を生じる。一方、航路誤差は未知のｖｏに起因せずｖｏ＾に起因するが、修正量にｖｏを加えることで打ち消すことができる。したがって本実施の形態においては積分制御を用いず、修正量を用いる方法を採用する。なお、航路制御ループの制御ゲインは図１５に示される。
【０１５０】
５．  検証
本実施形態をシミュレーションによって検証する。このシミュレーションの条件を以下に示す。
【０１５１】
＜計画航路＞
計画航路は、初期方位：４０ｄｅｇ，変針量：１０ｄｅｇ，船速：Ｕ＝１０ｋｎ。変針応答は無視する。
【０１５２】
＜制御対象＞
制御対象としての船体モデルは次式の２次系を用いる。
【０１５３】
【数８５】
0006373205.tif 000089
【０１５４】
船体パラメータは、ｍａｒｉｎｅｒ  ｃｌａｓｓでＬ＝１６０．９３ｍ，Ｔ１＝１７７.０ｓ，Ｔ２＝１１.６ｓ，Ｔｒ３＊＝２７．８ｓ，Ｔｖ３＊＝５．９ｓ，Ｋｒ＊＝０．１２３  １／ｓ，Ｋｖ＊＝−９．８ｍ／ｓで、１次モデルで同定した場合はＴｒ〜＝８５．６ｓ，Ｔｒ３〜＝５．９９ｓ，Ｋｒ〜＝０．０７４５  １／ｓ，Ｋｖ〜＝４．８５ｍ／ｓ。
【０１５５】
外乱パラメータは、オフセット成分δｏｒ＝δｏｖ＝３ｄｅｇ，波浪成分ψｗ＝ａｗｓｉｎ２π／周期×ｔとしてａｗ＝１ｄｅｇ，周期１３秒，潮流成分Ｕｃ＝５ｋｎ，ψｃ＝０ｄｅｇ。
【０１５６】
＜制御システム＞
船体パラメータはｕ−＝Ｕとして、Ｋｒ＝０．０２６７  １／ｓ，Ｋｖ＝−２．６７ｍ／ｓ，Ｔｒ＝３６．１ｓ，Ｔｒ３＝３．６ｓ。
【０１５７】
フィードバックゲイン（図８，図１５を参照）は、ωｈ＝０．０３２３ｒａｄ／ｓ（周期１９４秒），ｆ１＝１．４１，ｆ２＝１９．３ｓ，ωｔ＝０．０２１７ｒａｄ／ｓ，ｆｔ＝０．００１９７，ａｔ＝０．０１５０ｒａｄ／ｓ，ωｉ＝０．００５８８ｒａｄ／ｓ，ωｔｉ＝０．０２２９ｒａｄ／ｓ，ｆｉ＝０．００２５７  １／ｓ，ζti＝０．７６６。
【０１５８】
推定ゲインは、ρｈ２＝３．５２（２次系），ρｈ＝４．４３，ωｗ＝０．５２４ｒａｄ／ｓ（周期１２秒），ζｗ＝０．１。
【０１５９】
＜パラメータ不確かさ＞
パラメータ不確かさは、
【０１６０】
【数８６】
0006373205.tif 000090
になる。
【０１６１】
以上の条件によるシミュレーションの結果について説明する。なお、方位制御ループの修正量γｈは有効とする。図１６は、積分動作なし、修正量なしの場合のシミュレーション結果を示す図である。図１７は、積分動作あり、修正量なしの場合のシミュレーション結果を示す図である。図１８は、積分動作なし、修正量ありの場合のシミュレーション結果を示す図である。図１９は、積分動作なし、修正量あり、波浪成分ありの場合のシミュレーション結果を示す図である。
【０１６２】
積分動作なし、修正量なしの場合、図１６に示すように、安定な過渡応答であるが、潮流成分により航路誤差が発生する。方位４０ｄｅｇのとき
【０１６３】
【数８７】
0006373205.tif 000091
【０１６４】
積分動作あり、修正量なしの場合、図１７に示すように、航路制御ループの積分制御により航路誤差がゼロに収斂する。
【０１６５】
積分動作なし、修正量ありの場合、図１８に示すように、航路制御ループの修正量γｔにより航路誤差がほぼゼロになり、過渡応答は、積分動作あり、修正量なしの場合より早い。また、この条件に更に波浪成分を加えた場合であっても、図１９に示すように、航路誤差がほぼゼロになる。よって、修正量γｔによる潮流修正は積分制御の場合と同程度である。したがって、本発明によれば、パラメータ不確かさの感度が高く、閉ループ安定性への影響が大きい積分制御を用いることなく、外乱オフセット成分に起因する航路誤差を低減させることができる。
【０１６６】
６．  推定ゲインの計算
ここで、推定ゲインの計算について補足する。推定器の特性多項式は（３３）式の左辺を展開すると
【０１６７】
【数８８】
0006373205.tif 000092
になる。ここで、Ｈ＝［ｈ４  ｈ３  ｈ２  ｈ１  ｈ０］Ｔ，推定ゲインＫｈ＝［ｋ１  ｋ２  ｋ３  ｋ４  ｋ５］Ｔにおくと
【０１６８】
【数８９】
0006373205.tif 000093
【０１６９】
一方、仕様の特性多項式はその右辺を展開すると
【０１７０】
【数９０】
0006373205.tif 000094
になる。ここで
【０１７１】
【数９１】
0006373205.tif 000095
【０１７２】
よって、推定ゲインは両者のｓの係数を比較すると、次式になる。
【０１７３】
【数９２】
0006373205.tif 000096
【０１７４】
２次系の推定ゲインＫｈ２＝［ｋ１２  ｋ２２］Ｔを求める。
【０１７５】
【数９３】
0006373205.tif 000097
上式からｓの係数を比較すると、２次系の推定ゲインは次式になる。
【０１７６】
【数９４】
0006373205.tif 000098
【０１７７】
７．  推定係数ρｈの算法
ここで、推定係数ρｈの算法について補足する。推定係数ρｈを７次式から計算するとき、適切な初期値が必要である。この推定係数ρｈの算法においては、まず仕様を満足する４次式の解を求め、つぎに７次式の初期値をその解から設定する。
【０１７８】
条件：ζｈΔ付近で、添字２は２次推定器の場合を示し、
＜１＞  最小のζ＊はζｈΔ付近で、ρｈに関してほぼ比例する。
＜２＞  ４次式の初期値ρｈ２０＝３，７次式で初期値ρｈ０，ω０＊を
【０１７９】
【数９５】
0006373205.tif 000099
に定める。なお、刻みは１とおく。
【０１８０】
２段階を経てρｈを求める：収束計算による解法を用いる。
４次式    ζ２＊＝ζｈΔになるρｈ２，ω２＊を求める。
７次式    ζ＊＝ζｈΔになるρｈ，ω＊を求める。
【０１８１】
収束計算によるρｈ解法：代数方程式の解法を用いる。
＜１＞  ζｈΔを挟むρｈの範囲を初期値と刻みで求める。
＜２＞  ζ＊＝ζｈΔとなるρｈをその範囲で収束計算で求める。
【０１８２】
代数方程式の解法：与式を因数分解し、共役根から最小の減衰係数とその固有周波数を求める。
４次式    代数解法で４次式を２つの２次式に因数分解する。
７次式    初期値ω０＊，ζ０＊とＢａｉｒｓｔｏｗ法で７次式を２次式と５時式に、初期値ρ０ω＊とＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法で１次式と４次式にそれぞれ因数分解する。４次式は上式を用いる。
【０１８３】
本発明の実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【０１８４】
１  船舶用自動操舵装置
１１  軌道計画部
１３  フィードバック制御部
１３１  推定器
１３２  制御系フィードバックゲイン器
１３２Ａ  方位制御系フィードバックゲイン器
１３２Ｂ  航路制御系フィードバックゲイン器
【図１】
0006373205.tif 000101
【図２】
0006373205.tif 000102
【図３】
0006373205.tif 000103
【図４】
0006373205.tif 000104
【図５】
0006373205.tif 000105
【図６】
0006373205.tif 000106
【図７】
0006373205.tif 000107
【図８】
0006373205.tif 000108
【図９】
0006373205.tif 000109
【図１０】
0006373205.tif 000110
【図１１】
0006373205.tif 000111
【図１２】
0006373205.tif 000112
【図１３】
0006373205.tif 000113
【図１４】
0006373205.tif 000114
【図１５】
0006373205.tif 000115
【図１６】
0006373205.tif 000116
【図１７】
0006373205.tif 000117
【図１８】
0006373205.tif 000118
【図１９】
0006373205.tif 000119