船舶用自動操舵装置
【課題】保針中、変針中のいずれにも左右されることなく潮流成分を推定することができる船舶用自動操舵装置を提供する。
【解決手段】推定器１８が、方位誤差の推定を行う方位制御系推定器１８Ａと、航路誤差の推定を行う航路制御系推定器１８Ｂと、対地座標系での潮流の推定を行う対地座標系潮流推定器１８Ｃと、を備えており、対地座標系潮流推定器１８Ｃは、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）から、参照方位を用いずに推定潮流ベクトル（ｄx^，ｄy^）の推定を行う。
【解決手段】推定器１８が、方位誤差の推定を行う方位制御系推定器１８Ａと、航路誤差の推定を行う航路制御系推定器１８Ｂと、対地座標系での潮流の推定を行う対地座標系潮流推定器１８Ｃと、を備えており、対地座標系潮流推定器１８Ｃは、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）から、参照方位を用いずに推定潮流ベクトル（ｄx^，ｄy^）の推定を行う。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、航路制御系（TCS：Track Control System）の船舶用自動操舵装置に関し、潮流成分を推定し操舵を行う船舶用自動操舵装置に関する。尚、ここで潮流成分とは船体を移動させる作用をもつ潮海流や風力などを含むものである。
【背景技術】
【０００２】
船舶用自動操舵装置は、舵角を制御して設定方位に船首方位を追従させる方位制御系（HCS： Heading Control System）と、計画航路に船体位置を追跡させる航路制御系（TCS：Track Control System）とに分けられる。マイクロチップの高機能化、衛星測位システム（GNSS：Global Navigation Satellite System）の小型化・低コスト化・高精度化により位置情報が簡単に得られるようになったことに伴い、航路制御系の要求が高まっている。方位制御系の場合は、適時針路修正を必要とするが、航路制御系の場合は操船が不要で使い勝手がよい。
【０００３】
一般的に航路制御系の船舶用自動操舵装置は、図１に示すように軌道計画部１２、軌道航路誤差演算部１４、フィードバック制御部１６及び加算器１７を備える。計画航路に基づき軌道計画部１２が出力する参照方位ψRと参照位置ｘR、ｙRと、センサから検出される方位ψと位置ｘ、ｙとの誤差を軌道航路誤差演算部１４で求め、その誤差から保針時にフィードバック制御部１６が、船体の方位と位置とを追跡させるべくフィードバック舵角δFBを出力する。フィードバック制御部１６は、図２Ａに示すように、推定器１８と、フィードバックゲイン器２０とからなる。フィードバック制御系の制御時定数は方位制御系の制御時定数より長く、且つ、旋回時間は方位制御系の時定数より通常短いので、旋回で生じた航路誤差は、旋回中にフィードバック制御系で収斂させることは難しい。よって、旋回時には軌道計画部１２からフィードフォワード舵角δFFが出力される。加算器１７は、フィードバック舵角δFBとフィードフォワード舵角δFFとを加算して指令舵角δcを船体２４の操舵機に出力する。計画航路は、直線航路（レグ）と曲線航路（ＣＴ：カーブドトラック）とから定められる。
【０００４】
航路制御系では、航路誤差を直接制御できるアクチュエータを利用しないで船首方位を媒介にして間接的に制御するので劣駆動系である。
【０００５】
フィードバック制御部の設計にあたっては、次数が大きいために、非特許文献１，２では、ＬＱＧベースで代数リカッチ式に帰着させている。このような方法による設計は全体を見通して行なわれるので解の信頼性が高い半面、閉ループ特性は定性的な把握になり、制御対象の特性を把握しないで、直接的に解を求めているので、制御対象に最適な解であるかは不明である。
【０００６】
本発明者は、特許文献１において、推定器を、舵角を制御して設定方位に船首方位を追従させる方位制御系を基礎とした方位制御系推定手段と、航路制御系の航路制御系推定手段とに分離して構成する船舶用自動操舵装置を提案している。即ち、推定器の方位制御のみを行うときの制御対象モデルの状態量を推定するための特性多項式と、航路制御系の推定航路誤差（ｘe^，ｙe^）、推定潮流ベクトル（ｄx^，ｄy^）を推定するための特性多項式とを分けることによって、それぞれの推定ゲインの決定が可能になる。これによって、既存の方位制御系についての推定器の構成をそのまま使用し、航路制御系推定手段の構成を追加することで、推定器を構成するようにしている。
【０００７】
また、本発明者は、特許文献２において、方位制御系において、船首方位を参照方位に遅れなく追従させることができる参照方位とフィードフォワード制御との技術を基礎にして、さらに船体に作用する旋回時の潮流成分を考慮することによって、旋回時に計画旋回の軌跡に乗せることができる船舶用自動操舵装置を提案している。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】Fossen, T. I.,「Marine Control Systems」, Marine Cybernetics AS, 392/394(2002)
【非特許文献２】Zwierzewicz, Z,「On the Ship Guidance Automatic System Design via LQG-Integral Control」, Manoeuvring and Control of Marine Craft 2003, IFAC.
【特許文献】
【０００９】
【特許文献１】特開２００９−２４８８９６号公報
【特許文献２】特開２００９−２４８８９７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
上記従来の操舵処理は、直線航路航行中の保針時に潮流成分を推定し、該推定した潮流成分を用いて変針を行い曲線航路の航行を制御するようになっている。即ち、変針中は潮流成分が変化しないと仮定して、指令舵角δcを求めている。この操舵処理は、旋回時間が短い場合や潮流変化が少ない海域では十分に対応できる。
【００１１】
しかしながら、曲線航路航行中に実際には潮流変化が生じており航路誤差が発生しても、曲線航路航行後の直線航路で潮流の推定が開始されるため、航路誤差の過渡応答が大きく長く残存してしまうという問題がある。
【００１２】
本発明はかかる課題に鑑みなされたもので、保針中、変針中のいずれにも左右されることなく潮流成分を推定することができる船舶用自動操舵装置を提供することをその目的とする。
【課題を解決するための手段】
【００１３】
かかる目的を達成するために、本発明の請求項１に記載の発明は、船体の参照方位及び参照位置を出力する軌道計画部と、センサで検出された船体の方位及び位置から船体の方位と位置とを参照方位及び参照位置に追跡させるべく指令舵角を出力するフィードバック制御部と、を備えた船舶用自動操舵装置において、
前記フィードバック制御部は、方位、航路及び潮流ベクトルの状態量の推定値を求める推定器と、推定器から出力される状態量の推定値に対してフィードバックゲインを作用して指令舵角（δFB）を出力するフィードバックゲイン器とを備えており
前記推定器は、方位に関する推定を行う方位制御系推定手段と、航路に関する推定を行う航路制御系推定手段と、対地座標系での潮流ベクトルの推定を行う対地座標系推定手段と、を備え、対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）から、参照方位を用いずに推定潮流ベクトル（ｄx^，ｄy^）の推定を行うことを特徴とする。
【００１４】
請求項２記載の発明は、請求項１記載の発明において、前記対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）と、船速及び横方向速度とから、推定潮流ベクトル（ｄx^，ｄy^）の推定を行うことを特徴とする。
【００１５】
請求項３記載の発明は、請求項２記載の発明において、前記横方向速度は、方位制御系推定手段で得られる推定旋回角速度の関数として求められることを特徴とする。
【００１６】
請求項４記載の発明は、請求項１ないし３のいずれか１項に記載の発明において、前記軌道計画部は、変針中に、前記対地座標系推定手段で演算された推定潮流ベクトルを用いて、参照方位に従う指令舵角（δFF）を出力することを特徴とする。
【００１７】
請求項５記載の発明は、請求項１ないし４のいずれか１項に記載の発明において、前記航路制御系推定手段は、参照座標系における航路偏差から潮流ベクトルを推定しており、前記航路制御系推定手段で演算された推定潮流ベクトルと、前記対地座標系推定手段で演算された推定潮流ベクトルとのいずれかが選択されて状態量の推定値とされることを特徴とする。
【００１８】
請求項６記載の発明は、請求項５記載の発明において、少なくとも変針中に対地座標系推定手段からの推定潮流ベクトルが選択されることを特徴とする。
【００１９】
請求項７記載の発明は、請求項１ないし６のいずれか１項に記載の発明において、前記航路制御系推定手段の特性多項式の固有周波数よりも、前記対地座標系推定手段の特性多項式の固有周波数が高く設定されることを特徴とする。
【発明の効果】
【００２０】
本発明によれば、対地座標系での潮流ベクトルの推定を行う対地座標系推定手段を備え、対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）から、参照方位を用いずに推定潮流ベクトル（ｄx^，ｄy^）の推定を行うようにしているために、変針中、保針中に左右されることなく、推定潮流ベクトルを求めることができる。
【００２１】
また、参照方位を用いずに、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）等から推定潮流ベクトル（ｄx^，ｄy^）を求めることができるので、処理を簡単化することができる。
【００２２】
潮流ベクトル推定を船体制御と無関係にすることができるので、その推定固有周波数を高くして、推定を短期に行うことができるようになる。
【図面の簡単な説明】
【００２３】
【図１】本発明による船舶用自動操舵装置の全体構成を表すブロック図である。
【図２Ａ】フィードバック制御部の構成を表すブロック図である。
【図２Ｂ】フィードバック制御部の詳細構成を表すブロック図である。
【図３】航路制御系で用いる座標系を表す説明図である。
【図４】舵を取ったときに発生する横方向速度を表す説明図である。
【図５】制御対象のモデルを表すブロック図である。
【図６】制御対象のモデルを偏差系で表したブロック図である。
【図７Ａ】フィードバックゲイン器の構成を表すブロック図である。
【図７Ｂ】特性多項式の根軌跡を表す図である。
【図８】方位制御系推定器のブロック図である。
【図９】航路制御系推定器のブロック図である。
【図１０】対地座標系潮流推定器のブロック図である。
【図１１】軌道計画部のブロック図である。
【図１２】潮流の印加のない場合の従来構成のシミュレーション結果を表すグラフである。
【図１３】潮流の印加のない場合の本発明のシミュレーション結果を表すグラフである。
【図１４】潮流の印加が一様である場合の従来構成のシミュレーション結果を表すグラフである。
【図１５】潮流の印加が一様である場合の本発明のシミュレーション結果を表すグラフである。
【図１６】変針中に潮流の印加がある場合の従来構成のシミュレーション結果を表すグラフである。
【図１７】変針中に潮流の印加がある場合の本発明のシミュレーション結果を表すグラフである。
【発明を実施するための形態】
【００２４】
以下、図面を用いて本発明の実施の形態を説明する。
【００２５】
図１は、船舶用自動操舵装置と制御対象の全体のブロック図である。船舶用自動操舵装置１０は、計画航路に船体位置を追跡させるために舵を制御する装置であり、軌道計画部１２、軌道航路誤差演算部１４、フィードバック制御部１６、加算器１７及び各パラメータを同定する図示しない同定器を備えている。誘導システム２２から計画航路及びセンサ類２６のスピードログからの船速Ｕ（正確には船体のsurge速度ｕ（後述のようにｕ≒Ｕ））が軌道計画部１２に入力され、軌道計画部１２からは参照方位ψR、参照位置ｘR、ｙRといった参照信号及び変針中にはフィードフォワード舵角δFFが出力される。
【００２６】
軌道航路誤差演算部１４には、ジャイロコンパスからの船首方位ψ、ＧＰＳ等の衛星測位システム（ＧＮＳＳ）からの位置（ｘ，ｙ）といったセンサ類２６からの検出信号が入力され、軌道航路誤差演算部１４は、前記参照方位ψR、参照位置ｘR、ｙRと検出信号との比較を行い方位誤差ψe、航路誤差ｘe、ｙe(方位誤差、航路誤差を合わせて軌道誤差とも称する)等を出力する。
【００２７】
船舶用自動操舵装置１０の閉ループ系は図１に示すように船体モデルと外乱モデルとからなる制御対象２４と、フィードバック制御部１６とから構成される。
【００２８】
フィードバック制御部１６は、図２Ａに示すように、推定器１８とフィードバックゲイン器２０とからなる。軌道航路誤差演算部１４からの方位誤差ψe及び航路誤差ｘe、ｙeは、推定器１８に入力される。
【００２９】
推定器１８は、方位誤差の推定を行い外乱が除去された推定方位誤差ψe^、推定旋回角速度ｒ^といった方位に関する状態量の推定値を出力する方位制御系推定器１８Ａと、横方向の推定航路誤差ｙe^といった航路に関する状態量の推定値を出力する航路制御系推定器１８Ｂと、対地座標系での演算を行い、推定潮流ベクトルｄx^，ｄy^といった潮流に関する状態量の推定値を出力する対地座標系潮流推定器１８Ｃとを備える。
【００３０】
フィードバックゲイン器２０は、推定方位誤差ψe^、推定旋回角速度ｒ^に対してフィードバックゲインを掛ける方位制御系フィードバックゲイン器２０Ａと、推定航路誤差ｙe^に対してフィードバックゲインを掛ける航路制御系フィードバックゲイン器２０Ｂとを備え、これらの結果が加算されてフィードバック舵角δFBが出力される。
【００３１】
操舵機は指令舵角に比例した舵角を動かすため、船体は舵角によって旋回角速度を生じ方位、位置が変化する。旋回角速度の発生と共に、斜航角（横方向速度）が発生する。
【００３２】
以下、上記構成の詳細について説明する。
【００３３】
１．運動方程式
１．１ 座標系
航路制御系で用いる座標系は、図３に示すように、以下の座標系から構成する。
・対地座標系（ＮＥＤ）：地球固定の緯度経度座標系で、ＧＮＳＳからの位置出力（ｘ、ｙ）に相当する。
・船体座標系（ＸBＧＹB）：船体固定の運動座標系で、船体の重心を原点とし、船首方位をＸB 軸とし、船体運動を定める。
・参照座標系（ＸRＯRＹR）：誘導システム２２により生成され指定された計画航路から定まる移動座標系である。
【００３４】
尚、座標系の回転極性は右ネジ方向を正とし、Ｚ軸方向は重力方向を正とする。座標系はＸ軸、Ｙ軸の２次元を用いる。
【００３５】
１．２ 船体の運動方程式
前記制御対象である船体モデルを定めるために、船体の運動方程式を導出する。船体の運動方程式は、前進方向を除き、横方向と方位軸回りとの運動を扱うので、
【００３６】
【数１】
【００３７】
を用いる。ここでＭx ，Ｍy はそれぞれｘ，ｙ方向の付加質量を含んだ質量を、Ｉz はＺ軸まわりの付加慣性モーメントを含んだ慣性モーメントを，Ｙ,Ｎはそれぞれｙ方向の流体力、Ｚ軸まわりの流体モーメントを示し、添字は対応する変数を意味する。変数Ｕ,ｖ,ｒ,δはそれぞれ前進速度、横方向（横滑り）速度、旋回（回頭）角速度と舵角とを示す。上式を応答モデルに直すと、舵加速度δ・・(t)≒０とし、
【００３８】
【数２】
を得る。ここで、ｓはラプラス演算子であり、
【００３９】
【数３】
とおいている。
【００４０】
（３）式より、横方向（sway）の横滑り速度とヨー軸回りの旋回角速度との運動方程式は同一の形でかつ舵角による入力係数が異なるだけである。船体特性は舵を切ることによって、方位軸回りに旋回角速度を発生させ、同時に船体の横方向に対水の反力による横滑り速度を発生させる。（３）式において、実用的見地からｓ２項による影響は無視できるためにｓ２項を省略する。すると、（３）式は
【００４１】
【数４】
になる。ここで、
【００４２】
【数５】
であり、Ｋsは旋回力ゲイン、Ｋvは横滑りゲイン、Ｔs3、Ｔv3は時定数である。
【００４３】
また、船舶用自動操舵装置は、舵角を通して制御量を入力するので、制御変数は角度単位の方が都合がよい場合があるため、
【００４４】
【数６】
を用いて横滑り速度ｖを斜航角βに変換することができる。
【００４５】
１．３ 外乱モデル
外乱モデルは、舵角オフセット成分、波浪モデルと潮流成分を考える。舵角オフセットは風などに起因する方位軸まわりに作用する船体モーメントを舵角換算にしたものである。波浪モデルは白色ノイズを入力した狭帯域フィルタの出力を方位相当にしたものである。潮流成分は対地座標系の速度成分である。式にまとめると、
【００４６】
【数７】
【００４７】
になる。ここでδo はほぼ一定値とした舵角オフセット成分を、ψw は波浪成分を、Ｎ（ｓ） は白色ノイズＮ（０，１）を、Ｇw は波浪モデルの伝達関数を、Ｋw ，ζw ，ωw はそれぞれゲイン、減衰係数と中心周波数とを、ｄx，ｄy は対地座標系のそれぞれ北向き，東向きの潮流速度を、Ｕd ，ψd は潮流速度の大きさと方位とを、それぞれ示し、風が船体上部構造物を押すことによる速度成分も含むものとする。
【００４８】
船体モデルと外乱モデルとからなる制御対象の構成を図５（ａ）に示す。ただし同図では非線形項は省く。
【００４９】
１．４ 対地速度と対地位置
船体の位置は、対地座標系で定義され、船体運動と潮流速度とからの対地速度を積分したものとなる。船体速度は対水速度で、潮流速度は対地速度でそれぞれ表す。
【００５０】
船体運動は舵角δを取ると角速度ｒ＝ψ・を生じるが、同時に横方向速度ｖ＝ｙ・も生じる。その様子を、図４を用いて説明する。同図のように定常に旋回しているときまたは参照座標系が接線方位に一致しているとき、船体座標系は接線方位に対して斜航角βだけ内側に傾いて釣り合う。このとき船体速度の対水速度成分は
【００５１】
【数８】
になる。ここでｕx，ｖy は対水速度、ｕ，ｖは船体座標系の速度を示し、添え字ｘ，ｙ：それぞれ北向き、東向きを示す。船体速度と斜航角との関係は、
【００５２】
【数９】
を用いる。
【００５３】
対地座標系の船体重心の速度は、対水速度と潮流速度との和になるから、
【００５４】
【数１０】
になる。ここでｘ、ｙはセンサより得られる船体位置でそれぞれ北向き、東向きを示す。
【００５５】
１．５ 軌道航路誤差
軌道航路誤差演算部１４は、実際では緯度経度座標系によって計算されるが、ここでは説明を簡単にするためで、２次元平面座標を用いる。軌道航路誤差演算部１４で求められる航路誤差及び方位誤差は、参照座標系に対する船体座標系の航路誤差及び方位誤差であり、図３より、
【００５６】
【数１１】
【００５７】
になる。ここでψ，ｘ，ｙは船首方位と対地位置とを、ψR，ｘR，ｙR は参照方位と参照位置とを、ψｅ，ｘｅ，ｙｅは方位誤差と航路誤差とをそれぞれ示す。ｘｅ，ｙｅは船体位置Ｇから参照座標系のＹＲ，ＸＲ 軸にそれぞれ垂線を下ろした距離に相当する。
【００５８】
制御対象の誤差モデルを構成するために、速度誤差を導入する。そこで、参照速度ｕR，ｖR を次のようにおく。
【００５９】
【数１２】
【００６０】
参照座標系における船体座標系の速度誤差は、前記参照速度との偏差を参照座標系に座標変換したものとなる。
【００６１】
よって、速度誤差は、ψｅ＝ψ−ψR（（１３）式より）の関係及び（１０）式を用いて、
【００６２】
【数１３】
になる。
【００６３】
一方、潮流速度による速度誤差は対地座標系成分を参照座標系成分に変換すると
【００６４】
【数１４】
になる。
【００６５】
ここでｕd，ｖd は参照座標系の潮流速度成分を、ｄx，ｄyは対地座標系の潮流速度成分をそれぞれ示す。よって参照座標系速度誤差ｘe・，ｙe・は
【００６６】
【数１５】
で与えられる。
【００６７】
図６は、制御対象のモデルを偏差系で表したものである。
【００６８】
２．保針モード
２．１ 制御対象
直線航路航行中における保針モードの設計で用いる制御対象は、参照座標系に対する誤差を扱う偏差系で定めることとする。
【００６９】
船体運動は舵角を入力とし、旋回角速度と斜航角とを出力とするので、そのまま偏差系として利用される。
【００７０】
旋回運動と横滑り運動とは（４）式及び図５を参照すると
【００７１】
【数１６】
になる。ここで、Ｒ（ｓ），ｒ：旋回角速度、Ｖ（ｓ），ｖ：横方向速度、Δ（ｓ），δ：舵角、Δo（s），δo（δ・o≒０）：舵角オフセットである。
【００７２】
船体座標系の速度誤差成分ｕｅ，ｖｅは（１５）式から得られるが、ψeを微小項として近似し、（１１）式を用いると、
【００７３】
【数１７】
になる。ここで、
【００７４】
【数１８】
ψeｖ≒０としている。
【００７５】
これにより、参照座標系の制御対象は、図５、図６、（１７）式、（１８）式、（１９）式、（２０）式から、
【００７６】
【数１９】
になる。ここで、Ｒx（ｓ）、ｒx、Ｖx（ｓ）、ｖx は状態変数であり（図５（ｂ）、（ｃ）参照）、旋回角速度と横滑り速度とは
【００７７】
【数２０】
になる。ここで、
【００７８】
【数２１】
【００７９】
を示す。（２３）式と（２６）式では、時定数Ｔsが共通であり、図５（ｂ）、（ｃ）に示したように、同様の構成をとっている。よって、（２６）式を消去することにすると、Ｒx（ｓ）、Ｖx（ｓ）とは、
【００８０】
【数２２】
となる。ここで、δo・≒０、ｓΔo≒０と仮定しており、一時遅れ要素を省略しても影響は少ないと仮定している。
【００８１】
よって、横滑り速度Ｖ（ｓ）、ｖは、
【００８２】
【数２３】
と表すことができる。Ｔs3＝Ｔv3＝０とすると、上式は、
【００８３】
【数２４】
となる。（２５）式に、（３３）式を入力することで、ｖの次数を減らすことができる。
【００８４】
方位制御系は、航路制御系から干渉を受けないので、方位制御系の制御ループは、単独で構成される。他方、航路制御系は方位制御系から干渉を受ける。
【００８５】
２．２ フィードバック制御
フィードバックゲイン器２０を図７Ａに示すように、方位制御系フィードバックゲイン器２０Ａと航路制御系フィードバックゲイン器２０Ｂとから構成する。
【００８６】
船体モデルに状態フィードバックした操舵ループの方位制御系の特性多項式は、方位制御系にのみ関連する項をまとめると、ζh，ωh をそれぞれ操舵系減衰係数と固有周波数とし、Ｔs3＝０とすると、
【００８７】
【数２５】
となり、
【００８８】
【数２６】
となる。Ｔs3≠０としたときには、
【００８９】
【数２７】
【００９０】
となる。設計パラメータを、比例ゲインＫP、減衰係数ζとしたときに、微分ゲインＫDと固有周波数ωhとは設計パラメータＫP、ζhと同定器によって同定される船体パラメータＫs,Ｔs,Ｔs3とから一義的に決定される（特開２００７−１１８８２８号参照）。
【００９１】
次に、航路制御系の特性多項式は、Ｔs3＝Ｔv3＝０とすると、
【００９２】
【数２８】
となる。Ｄt（ｓ）の開ループ伝達関数を
【００９３】
【数２９】
【００９４】
とすると、根軌跡は図７Ｂに示すようになる。図７Ｂから特性根は、Ｋy ＝０のとき、根は方位制御系特性根である極（記号×）と原点根とになり、Ｋy＞０ が大きくなると、極にあった根は安定側から不安定側に、原点根は左方向にそれぞれ移動し、実軸でBreak-in point になり左右方向に別れて原点根は左方向にそれぞれ移動した後、Breakaway point から安定側から不安定側に移り、さらにＫy ＞０ が大きくなると、根はゼロ点と左右方向とに向かって移動する、特徴をもつ。
【００９５】
航路ゲインＫyを求めるために、特性多項式を、
【００９６】
【数３０】
【００９７】
として定める。ここでａtは正の実数を、ζt，ωt はそれぞれ航路制御系減衰係数と固有周波数とを示す。航路制御系の設計パラメータをζt をζt＝ζhとして、それぞれの特性多項式の係数を比較すると、
【００９８】
【数３１】
上式を整理すると、ωtに関する３次方程式
【００９９】
【数３２】
を得る。このωtの３つの解に対して、最も小さい正の解を選択すると、図７Ｂから示されるように、
【０１００】
【数３３】
が成り立つ。即ち、航路制御系固有周波数ωtは、方位制御系固有周波数ωhよりも小さくなる。
【０１０１】
航路ゲインＫyは、
【０１０２】
【数３４】
として得られる。Ｔs3≠０、Ｔv3≠０としたときには、
【０１０３】
【数３５】
として得られる。
【０１０４】
また、フィードバックゲインゲイン器２０には、外乱による影響を除去するために修正量として、
【０１０５】
【数３６】
が入力される。ｖd は参照座標系の潮流速度成分である（（１６）式参照）。
【０１０６】
３．推定器
推定器１８は、方位誤差の推定を行う方位制御系推定器１８Ａ及び航路誤差の推定を行う航路制御系推定器１８Ｂと、潮流ベクトルの推定を行う対地座標系潮流推定器１８Ｃとを備える。
【０１０７】
３．１ 方位制御系推定器
方位制御系推定器１８Ａの構成例を図８に示す。この推定器は、波浪モデルと舵角オフセットとを組み込んだものであり、（２２）式、（２３）式及び検出方位誤差
【０１０８】
【数３７】
及びΔ=ΔFB、δ=δFBを用いて、
【０１０９】
【数３８】
【０１１０】
と表すことができる。ここで、^は推定値を、ｋｉ，ｉ＝１〜５は推定ゲインを、ξ^は推定状態量を、ψW^は推定波浪を、δO^、ΔO^は推定舵角オフセットを、それぞれ示す。上式を行列で表すと、
【０１１１】
【数３９】
となる。ここで、Ｋh は方位制御系（ＨＣＳ）の推定ゲインを示し、
【０１１２】
【数４０】
であり、
【０１１３】
【数４１】
であり、ε は微小項を示す。
【０１１４】
この特性多項式は、
【０１１５】
【数４２】
【０１１６】
に定める。ここでＤehは船体モデルの状態推定に、Ｄehwは波浪モデルの状態推定に、そしてＤehoは舵角オフセットモデルの状態推定にそれぞれ対応し、ζeh，ζehwとωeh，ωehw，ωeho とは方位制御系のそれぞれ推定減衰係数と推定固有周波数とで、ζehw，ωehwは、図示しない波浪同定器によって検出方位から同定されるもので、
【０１１７】
【数４３】
を示す。ここでωh は方位制御系の操舵系固有周波数（（３７）式）を、ρeh，ρeho は推定係数で推定速度やパラメータ不確かさに対する許容度から決定され
【０１１８】
【数４４】
とする。
【０１１９】
特性多項式の根を極配置することにより求める推定ゲインは、
【０１２０】
【数４５】
になる。ここで、添字(・)-1 は逆行列を意味し、添字(・)T は転置行列を意味し、
【０１２１】
【数４６】
になる。
【０１２２】
３．２ 航路制御系推定器
航路制御系推定器１８Ｂの構成例を図９に示す。（２４）式、（２５）式及びΔ=ΔFB、δ=δFBを用い、ｘ^e ＝ｘ−ｘ^，ｙ^e ＝ｙ−ｙ^とすると、
【０１２３】
【数４７】
と表すことができる。上式を行列で表すと、
【０１２４】
【数４８】
となる。ここで、Ｋt は方位制御系（ＴＣＳ）の推定ゲインを示し、
【０１２５】
【数４９】
であり、
【０１２６】
【数５０】
であり、ε は微小項を示す。
【０１２７】
この特性多項式は、
【０１２８】
【数５１】
【０１２９】
に定める。ここでζet，ζeto とωet，ωeto とは航路制御系のそれぞれ減衰係数と固有周波数とを
【０１３０】
【数５２】
を示す。ＤTCS の固有周波数を
【０１３１】
【数５３】
と定める。ここでωt は航路制御系の操舵系固有周波数（（４１）式参照）を、ρet，ρeto は推定係数で
【０１３２】
【数５４】
と定める（（５２）式参照）。
【０１３３】
特性多項式の根を極配置を実現するように、且つ、対称性を満足するようにすると、推定ゲインＫtは、
【０１３４】
【数５５】
とおいて
【０１３５】
【数５６】
を用いて、推定ゲインを用いた特性多項式を求めると
【０１３６】
【数５７】
になる。ここで
【０１３７】
【数５８】
を示す。
【０１３８】
設計パラメータとして、
【０１３９】
【数５９】
を定めると、これより推定ゲインは
【０１４０】
【数６０】
として得られる。
【０１４１】
３．３ 対地座標系潮流推定器
以上に説明したように、航路制御系推定器１８Ｂにおいては、参照座標系の航路誤差ｘe、ｙeを用いて対地座標系の潮流成分ｄx、ｄyを推定していた。しかしながら、変針中の参照座標系はψRが変化するために、線形に扱うことができない。
【０１４２】
対地座標系潮流推定器１８Ｃは、航路変化の影響を受けない対地座標系から潮流成分を推定するために、変針中であるなしに拘わらず、潮流推定を行うことができる。
【０１４３】
推定式は対地座標系において、（１２）式を基に（８）式を状態量に組み込むことで構成され
【０１４４】
【数６１】
【０１４５】
になる。ここで、潮流成分は一定と扱い、Ｋc：推定ゲイン、ｕ^x，ｖ^y：（２１）式および方位制御系推定器１８Ａから得られたＲx ^、ΔO^を（３３）式に代入することで得られるまたは（３４）式で得られる推定値ｖ^をそれぞれ（１０）式に代入した値である。
【０１４６】
尚、対地座標系潮流推定器１８Ｃにおいても、航路制御系推定器１８Ｂのように偏差系で構成することもでき、その場合、
【０１４７】
【数６２】
【０１４８】
となる。ここで、ｕNED，ｖNED：測地センサのＮＥＤ系速度である。上式から分かるように、偏差系では、外部ＮＥＤ系速度が新たに必要になり、推定誤差の算出のためｘ^，ｙ^も必要になり、ｘ^e，ｙ^eの計算量も増加する。そのため、偏差系としない方が好ましい。
【０１４９】
推定式は（６０）式より、ｘ，ｙに関して干渉せず同形なので、図１０に示すように、推定ゲインを
【０１５０】
【数６３】
に定める。推定器の特性多項式は
【０１５１】
【数６４】
になる。ここでｓ：ラプラス演算子である。一方、仕様の特性多項式を
【０１５２】
【数６５】
に定める。ここでωec，ωeco：推定固有周波数で
【０１５３】
【数６６】
【０１５４】
と定める。ここでωeh 、ωehoは、方位制御系推定器１８Ａに用いられる方位制御系の推定固有周波数であり、推定係数ρcの短期時は潮流成分を迅速に推定する際に利用する。上式及び（４５）式、（５０）式、（５４）式、（５５）式の関係から、上記推定固有周波数は、航路制御系推定器１８Ｂの推定固有周波数より高い値となり、これにより短期に推定可能である。これは対地座標系潮流推定が船体制御に無関係に実施されるために可能になる。これより推定ゲインは次式になる。
【０１５５】
【数６７】
【０１５６】
３．４ 切替手段
対地座標系潮流推定器１８Ｃの推定潮流ベクトルは常時使用するようにしてもよいが、図２Ｂにおいて切替手段ＳＷ及び図９において切替手段ＳＷ１、ＳＷ２に示すように、航路制御系推定器１８Ｂで演算される推定潮流ベクトルと、対地座標系潮流推定器１８Ｃで演算される推定潮流ベクトルとを、切り替えて選択するようにしてもよい。切替の条件は、計画航路の直線航路と曲線航路との切替指令、即ち保針モードと変針モードとの切替指令に連動させることができる。
【０１５７】
任意には、切替手段を除去し、対地座標系潮流推定器１８Ｃで演算される推定潮流ベクトルを常時使用するようにして、航路制御系推定器１８Ｂにおける推定潮流ベクトルの演算を省略することも可能である。
【０１５８】
また任意には、推定潮流ベクトルの切替とは別に、航路制御系推定器１８Ｂと対地座標系潮流推定器１８Ｃから演算されるそれぞれの推定潮流ベクトルを比較することで、モニタ機能を持たせることも可能である。本来であれば、航路制御系推定器１８Ｂと対地座標系潮流推定器１８Ｃとは、座標系及び推定ゲインが異なるだけであるから、同様の推定値となるはずであるが、それぞれの推定潮流成分の差異から、推定値の精度を評価することができる。
【０１５９】
４． 変針モード
軌道計画部１２は、図１１に示すように、誘導システム２２から曲線航路の計画航路が指令されると、その旋回条件を求め旋回条件に合致する参照方位ψRを発生する参照方位発生部３０と、船体の参照速度を発生する参照速度発生部３４と、船体の参照速度を積分して参照位置ｘR、ｙRを発生する参照位置発生部３６と、推定潮流ｄx、ｄyを座標変換して参照座標系の潮流成分を発生する座標変換部４０と、潮流に対抗するための斜航角βDを発生する潮流修正部４２と、対地速度の修正を行う対地速度修正部４４と、船体方位を参照方位ψR＋斜航角βDに遅れなく追従させるためのフィードフォワード舵角δFFを出力するフィードフォワード舵角発生部４６と、フィードフォワード舵角の修正を行うフィードフォワード舵角修正部４８と、を備える。また、旋回条件の中で、舵角設定値δ0についての修正を行って操舵機の飽和状態を回避する舵角設定部２８を備える(特願２０１１−１７６７６８参照)。
【０１６０】
舵角設定部２８は、舵角設定値δ0に対して、以下の式から、参照舵角設定値δ0Rを、
【０１６１】
【数６８】
【０１６２】
等として求め、参照方位発生部３０は、参照舵角設定値δ0Rを超えないように参照方位ψRを求める。ここで、Ｕdは、推定器１８から出力される選択された推定潮流成分ｄx^，ｄy^を潮流速度成分ｄx，ｄyとして、（９）式から求める。
【０１６３】
参照方位発生部３０から出力された参照方位ψRは、参照速度発生部３４において、（１４）式に用いられて、参照速度ｕR，ｖR が求められる。また、これらを参照位置発生部３６で積分することで、参照位置
【０１６４】
【数６９】
を得る。
【０１６５】
座標変換部４０は、推定器１８から出力される選択された推定潮流成分ｄx^、ｄy^を潮流速度成分ｄx，ｄyとして、参照方位発生部３０で発生された参照方位ψRを用いて（１６）式により、参照座標系の潮流速度成分ｕd、ｖdに変換する。
【０１６６】
参照座標系における対地速度は、対水速度と潮流速度との和になり、
【０１６７】
【数７０】
になる。ここで、ｕ*、ｖ*は参照座標系における速度成分、ｕD、ｖDは参照座標系における対水速度成分で、
【０１６８】
【数７１】
となる。ここで、βDは斜航角、ｕ、ｖは図４を参照されたい。
（６８）式、（６９）式から、βDを微小角として、
【０１６９】
【数７２】
と近似する。ｖは船体座標系の横方向（sway）速度で、旋回時に生じる横方向速度に相当する。
【０１７０】
潮流修正部４２は、
【０１７１】
【数７３】
によって斜航角βDを求める。
【０１７２】
潮流成分も含んだ指定旋回角速度ｒ0*は
【０１７３】
【数７４】
になる。ここで、ＳｅｔΔｕ：速度更新設定値（例えば１ｋｎｏｔ）、ｕ0*：前回の値とする。
【０１７４】
対地速度修正部４４は、（６８）式、（７１）式、（６９）式によって、得られた対地速度ｕ*の変化がΔｕを越えると、（７２）式に従い、指定旋回角速度ｒ0*を更新し、対地速度ｕ*と共にそれを参照方位発生部３０へと出力する。
【０１７５】
参照方位発生部３０は、更新された指定旋回角速度ｒ0*を満足する参照方位ψRを再計算する。その際の変針量は既に変針された方位分を差し引いた量、
【０１７６】
【数７５】
になる。ここでψ0*：変化する指定変針量、Δψ0：計画航路の変針量、Δψ’R：既に変針された方位量である。
【０１７７】
従って、例えば、変針中に潮流が変化することに起因して、対地速度ｕ*が変化して（７２）式の括弧中の条件が成り立つ場合には、対地座標系潮流推定器１８Ｃによって演算される対地座標系の推定潮流成分ｄx^，ｄy^を潮流速度成分ｄx，ｄyとして、（９）式を用いて（６７）式が改めて求められ、参照方位発生部３０で参照方位ψRが順次求められる。
【０１７８】
フィードフォワード舵角発生部４６は、計画航路に船体航路を追跡させるための、参照舵角δRと、潮流の斜航角に対応する潮流舵角δDを求める。（４）式から、
【０１７９】
【数７６】
により得られる。
【０１８０】
潮流の斜航角βDは、（７１）式、（１６）式に示されるように、閉ループ系のフィードバック制御部１６内にある推定器１８から得られる推定潮流成分d x^、d y^から求まるので、参照方位ψRに加算して、軌道航路誤差演算部１４に出力すると、マイナーループが構成されて、制御系特性が変化してしまう。よって、βDは前方に帰還できないので、フィードフォワード舵角修正部４８によって、後方で修正する。フィードフォワード舵角修正部４８は、フィードフォワード舵角発生部４６で得られたフィードフォワード舵角に対して、潮流の斜航角βDに対してフィードバックゲインＧFBを掛けたものを修正フィードバック舵角として、加算して修正する。
【０１８１】
変針中、フィードフォワード舵角修正部４８で修正された指令舵角であるフィードフォワード舵角δFFと、フィードバック制御部１６からの指令舵角であるフィードバック舵角δFBとは加算器１７で加算されて指令舵角δCとなり船体２４の操舵機へと出力される。
【０１８２】
変針中、座標変換部４０には、推定器１８の対地座標系潮流推定器１８Ｃで求められた推定潮流成分ｄx^，ｄy^が提供されるために、変針中に潮流変化が生じたとしても、その潮流変化が反映されるために、軌道計画部１２からより正確なフィードフォワード舵角を出力することができる。
【０１８３】
５．検証
以上、本発明の潮流の推定を用いて、曲線航路航行中に、従来の対地座標系潮流推定器１８Ｃを備えない従来構成における推定潮流ベクトルが一定値でなく、対地座標系潮流推定器１８Ｃによって演算した推定値で制御を行う。その潮流推定を用いて、曲線航路を航行した場合の効果を数値計算によって検証する。検証は次の項目について実施する。
計算条件を示す。
・船体パラメータ：Ｋs ＝ ０．０２７［１／ｓ］、Ｔs ＝１７．５４［ｓ］、Ｔs3 ＝ ０．１［ｓ］、Ｕ ＝ ２０［ｋｎｏｔ］
・操舵機：最大ＦＦ舵角３０［ｄｅｇ］、舵速度２［ｄｅｇ／ｓ］、旋回：半径１［ＮＭ］、旋回角１００［ｄｅｇ］
・潮流印加：
【０１８４】
【数７７】
【０１８５】
計算結果を表１にまとめる。
【０１８６】
【表１】
【０１８７】
以上表より、次の項目における本発明の有効性が確認された。
１．潮流が印加しない、あるいは一様な潮流が印加する場合、本発明の航路誤差および最大舵角は従来とほぼ同等かそれ以下である
２．ＣＴの途中から一様な潮流成分が印加する場合、本発明の航路誤差は従来のものより過渡応答の改善（航路誤差のピーク値減少、静定時間の短縮）が図られる
【符号の説明】
【０１８８】
１０ 船舶用自動操舵装置
１２ 軌道計画部
１４ 軌道航路誤差演算部
１６ フィードバック制御部
１８ 推定器
１８Ａ 方位制御系推定器
１８Ｂ 航路制御系推定器
１８Ｃ 対地座標系潮流推定器
２０ フィードバックゲイン器
２４ 船体
【技術分野】
【０００１】
本発明は、航路制御系（TCS：Track Control System）の船舶用自動操舵装置に関し、潮流成分を推定し操舵を行う船舶用自動操舵装置に関する。尚、ここで潮流成分とは船体を移動させる作用をもつ潮海流や風力などを含むものである。
【背景技術】
【０００２】
船舶用自動操舵装置は、舵角を制御して設定方位に船首方位を追従させる方位制御系（HCS： Heading Control System）と、計画航路に船体位置を追跡させる航路制御系（TCS：Track Control System）とに分けられる。マイクロチップの高機能化、衛星測位システム（GNSS：Global Navigation Satellite System）の小型化・低コスト化・高精度化により位置情報が簡単に得られるようになったことに伴い、航路制御系の要求が高まっている。方位制御系の場合は、適時針路修正を必要とするが、航路制御系の場合は操船が不要で使い勝手がよい。
【０００３】
一般的に航路制御系の船舶用自動操舵装置は、図１に示すように軌道計画部１２、軌道航路誤差演算部１４、フィードバック制御部１６及び加算器１７を備える。計画航路に基づき軌道計画部１２が出力する参照方位ψRと参照位置ｘR、ｙRと、センサから検出される方位ψと位置ｘ、ｙとの誤差を軌道航路誤差演算部１４で求め、その誤差から保針時にフィードバック制御部１６が、船体の方位と位置とを追跡させるべくフィードバック舵角δFBを出力する。フィードバック制御部１６は、図２Ａに示すように、推定器１８と、フィードバックゲイン器２０とからなる。フィードバック制御系の制御時定数は方位制御系の制御時定数より長く、且つ、旋回時間は方位制御系の時定数より通常短いので、旋回で生じた航路誤差は、旋回中にフィードバック制御系で収斂させることは難しい。よって、旋回時には軌道計画部１２からフィードフォワード舵角δFFが出力される。加算器１７は、フィードバック舵角δFBとフィードフォワード舵角δFFとを加算して指令舵角δcを船体２４の操舵機に出力する。計画航路は、直線航路（レグ）と曲線航路（ＣＴ：カーブドトラック）とから定められる。
【０００４】
航路制御系では、航路誤差を直接制御できるアクチュエータを利用しないで船首方位を媒介にして間接的に制御するので劣駆動系である。
【０００５】
フィードバック制御部の設計にあたっては、次数が大きいために、非特許文献１，２では、ＬＱＧベースで代数リカッチ式に帰着させている。このような方法による設計は全体を見通して行なわれるので解の信頼性が高い半面、閉ループ特性は定性的な把握になり、制御対象の特性を把握しないで、直接的に解を求めているので、制御対象に最適な解であるかは不明である。
【０００６】
本発明者は、特許文献１において、推定器を、舵角を制御して設定方位に船首方位を追従させる方位制御系を基礎とした方位制御系推定手段と、航路制御系の航路制御系推定手段とに分離して構成する船舶用自動操舵装置を提案している。即ち、推定器の方位制御のみを行うときの制御対象モデルの状態量を推定するための特性多項式と、航路制御系の推定航路誤差（ｘe^，ｙe^）、推定潮流ベクトル（ｄx^，ｄy^）を推定するための特性多項式とを分けることによって、それぞれの推定ゲインの決定が可能になる。これによって、既存の方位制御系についての推定器の構成をそのまま使用し、航路制御系推定手段の構成を追加することで、推定器を構成するようにしている。
【０００７】
また、本発明者は、特許文献２において、方位制御系において、船首方位を参照方位に遅れなく追従させることができる参照方位とフィードフォワード制御との技術を基礎にして、さらに船体に作用する旋回時の潮流成分を考慮することによって、旋回時に計画旋回の軌跡に乗せることができる船舶用自動操舵装置を提案している。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】Fossen, T. I.,「Marine Control Systems」, Marine Cybernetics AS, 392/394(2002)
【非特許文献２】Zwierzewicz, Z,「On the Ship Guidance Automatic System Design via LQG-Integral Control」, Manoeuvring and Control of Marine Craft 2003, IFAC.
【特許文献】
【０００９】
【特許文献１】特開２００９−２４８８９６号公報
【特許文献２】特開２００９−２４８８９７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
上記従来の操舵処理は、直線航路航行中の保針時に潮流成分を推定し、該推定した潮流成分を用いて変針を行い曲線航路の航行を制御するようになっている。即ち、変針中は潮流成分が変化しないと仮定して、指令舵角δcを求めている。この操舵処理は、旋回時間が短い場合や潮流変化が少ない海域では十分に対応できる。
【００１１】
しかしながら、曲線航路航行中に実際には潮流変化が生じており航路誤差が発生しても、曲線航路航行後の直線航路で潮流の推定が開始されるため、航路誤差の過渡応答が大きく長く残存してしまうという問題がある。
【００１２】
本発明はかかる課題に鑑みなされたもので、保針中、変針中のいずれにも左右されることなく潮流成分を推定することができる船舶用自動操舵装置を提供することをその目的とする。
【課題を解決するための手段】
【００１３】
かかる目的を達成するために、本発明の請求項１に記載の発明は、船体の参照方位及び参照位置を出力する軌道計画部と、センサで検出された船体の方位及び位置から船体の方位と位置とを参照方位及び参照位置に追跡させるべく指令舵角を出力するフィードバック制御部と、を備えた船舶用自動操舵装置において、
前記フィードバック制御部は、方位、航路及び潮流ベクトルの状態量の推定値を求める推定器と、推定器から出力される状態量の推定値に対してフィードバックゲインを作用して指令舵角（δFB）を出力するフィードバックゲイン器とを備えており
前記推定器は、方位に関する推定を行う方位制御系推定手段と、航路に関する推定を行う航路制御系推定手段と、対地座標系での潮流ベクトルの推定を行う対地座標系推定手段と、を備え、対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）から、参照方位を用いずに推定潮流ベクトル（ｄx^，ｄy^）の推定を行うことを特徴とする。
【００１４】
請求項２記載の発明は、請求項１記載の発明において、前記対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）と、船速及び横方向速度とから、推定潮流ベクトル（ｄx^，ｄy^）の推定を行うことを特徴とする。
【００１５】
請求項３記載の発明は、請求項２記載の発明において、前記横方向速度は、方位制御系推定手段で得られる推定旋回角速度の関数として求められることを特徴とする。
【００１６】
請求項４記載の発明は、請求項１ないし３のいずれか１項に記載の発明において、前記軌道計画部は、変針中に、前記対地座標系推定手段で演算された推定潮流ベクトルを用いて、参照方位に従う指令舵角（δFF）を出力することを特徴とする。
【００１７】
請求項５記載の発明は、請求項１ないし４のいずれか１項に記載の発明において、前記航路制御系推定手段は、参照座標系における航路偏差から潮流ベクトルを推定しており、前記航路制御系推定手段で演算された推定潮流ベクトルと、前記対地座標系推定手段で演算された推定潮流ベクトルとのいずれかが選択されて状態量の推定値とされることを特徴とする。
【００１８】
請求項６記載の発明は、請求項５記載の発明において、少なくとも変針中に対地座標系推定手段からの推定潮流ベクトルが選択されることを特徴とする。
【００１９】
請求項７記載の発明は、請求項１ないし６のいずれか１項に記載の発明において、前記航路制御系推定手段の特性多項式の固有周波数よりも、前記対地座標系推定手段の特性多項式の固有周波数が高く設定されることを特徴とする。
【発明の効果】
【００２０】
本発明によれば、対地座標系での潮流ベクトルの推定を行う対地座標系推定手段を備え、対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）から、参照方位を用いずに推定潮流ベクトル（ｄx^，ｄy^）の推定を行うようにしているために、変針中、保針中に左右されることなく、推定潮流ベクトルを求めることができる。
【００２１】
また、参照方位を用いずに、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）等から推定潮流ベクトル（ｄx^，ｄy^）を求めることができるので、処理を簡単化することができる。
【００２２】
潮流ベクトル推定を船体制御と無関係にすることができるので、その推定固有周波数を高くして、推定を短期に行うことができるようになる。
【図面の簡単な説明】
【００２３】
【図１】本発明による船舶用自動操舵装置の全体構成を表すブロック図である。
【図２Ａ】フィードバック制御部の構成を表すブロック図である。
【図２Ｂ】フィードバック制御部の詳細構成を表すブロック図である。
【図３】航路制御系で用いる座標系を表す説明図である。
【図４】舵を取ったときに発生する横方向速度を表す説明図である。
【図５】制御対象のモデルを表すブロック図である。
【図６】制御対象のモデルを偏差系で表したブロック図である。
【図７Ａ】フィードバックゲイン器の構成を表すブロック図である。
【図７Ｂ】特性多項式の根軌跡を表す図である。
【図８】方位制御系推定器のブロック図である。
【図９】航路制御系推定器のブロック図である。
【図１０】対地座標系潮流推定器のブロック図である。
【図１１】軌道計画部のブロック図である。
【図１２】潮流の印加のない場合の従来構成のシミュレーション結果を表すグラフである。
【図１３】潮流の印加のない場合の本発明のシミュレーション結果を表すグラフである。
【図１４】潮流の印加が一様である場合の従来構成のシミュレーション結果を表すグラフである。
【図１５】潮流の印加が一様である場合の本発明のシミュレーション結果を表すグラフである。
【図１６】変針中に潮流の印加がある場合の従来構成のシミュレーション結果を表すグラフである。
【図１７】変針中に潮流の印加がある場合の本発明のシミュレーション結果を表すグラフである。
【発明を実施するための形態】
【００２４】
以下、図面を用いて本発明の実施の形態を説明する。
【００２５】
図１は、船舶用自動操舵装置と制御対象の全体のブロック図である。船舶用自動操舵装置１０は、計画航路に船体位置を追跡させるために舵を制御する装置であり、軌道計画部１２、軌道航路誤差演算部１４、フィードバック制御部１６、加算器１７及び各パラメータを同定する図示しない同定器を備えている。誘導システム２２から計画航路及びセンサ類２６のスピードログからの船速Ｕ（正確には船体のsurge速度ｕ（後述のようにｕ≒Ｕ））が軌道計画部１２に入力され、軌道計画部１２からは参照方位ψR、参照位置ｘR、ｙRといった参照信号及び変針中にはフィードフォワード舵角δFFが出力される。
【００２６】
軌道航路誤差演算部１４には、ジャイロコンパスからの船首方位ψ、ＧＰＳ等の衛星測位システム（ＧＮＳＳ）からの位置（ｘ，ｙ）といったセンサ類２６からの検出信号が入力され、軌道航路誤差演算部１４は、前記参照方位ψR、参照位置ｘR、ｙRと検出信号との比較を行い方位誤差ψe、航路誤差ｘe、ｙe(方位誤差、航路誤差を合わせて軌道誤差とも称する)等を出力する。
【００２７】
船舶用自動操舵装置１０の閉ループ系は図１に示すように船体モデルと外乱モデルとからなる制御対象２４と、フィードバック制御部１６とから構成される。
【００２８】
フィードバック制御部１６は、図２Ａに示すように、推定器１８とフィードバックゲイン器２０とからなる。軌道航路誤差演算部１４からの方位誤差ψe及び航路誤差ｘe、ｙeは、推定器１８に入力される。
【００２９】
推定器１８は、方位誤差の推定を行い外乱が除去された推定方位誤差ψe^、推定旋回角速度ｒ^といった方位に関する状態量の推定値を出力する方位制御系推定器１８Ａと、横方向の推定航路誤差ｙe^といった航路に関する状態量の推定値を出力する航路制御系推定器１８Ｂと、対地座標系での演算を行い、推定潮流ベクトルｄx^，ｄy^といった潮流に関する状態量の推定値を出力する対地座標系潮流推定器１８Ｃとを備える。
【００３０】
フィードバックゲイン器２０は、推定方位誤差ψe^、推定旋回角速度ｒ^に対してフィードバックゲインを掛ける方位制御系フィードバックゲイン器２０Ａと、推定航路誤差ｙe^に対してフィードバックゲインを掛ける航路制御系フィードバックゲイン器２０Ｂとを備え、これらの結果が加算されてフィードバック舵角δFBが出力される。
【００３１】
操舵機は指令舵角に比例した舵角を動かすため、船体は舵角によって旋回角速度を生じ方位、位置が変化する。旋回角速度の発生と共に、斜航角（横方向速度）が発生する。
【００３２】
以下、上記構成の詳細について説明する。
【００３３】
１．運動方程式
１．１ 座標系
航路制御系で用いる座標系は、図３に示すように、以下の座標系から構成する。
・対地座標系（ＮＥＤ）：地球固定の緯度経度座標系で、ＧＮＳＳからの位置出力（ｘ、ｙ）に相当する。
・船体座標系（ＸBＧＹB）：船体固定の運動座標系で、船体の重心を原点とし、船首方位をＸB 軸とし、船体運動を定める。
・参照座標系（ＸRＯRＹR）：誘導システム２２により生成され指定された計画航路から定まる移動座標系である。
【００３４】
尚、座標系の回転極性は右ネジ方向を正とし、Ｚ軸方向は重力方向を正とする。座標系はＸ軸、Ｙ軸の２次元を用いる。
【００３５】
１．２ 船体の運動方程式
前記制御対象である船体モデルを定めるために、船体の運動方程式を導出する。船体の運動方程式は、前進方向を除き、横方向と方位軸回りとの運動を扱うので、
【００３６】
【数１】
【００３７】
を用いる。ここでＭx ，Ｍy はそれぞれｘ，ｙ方向の付加質量を含んだ質量を、Ｉz はＺ軸まわりの付加慣性モーメントを含んだ慣性モーメントを，Ｙ,Ｎはそれぞれｙ方向の流体力、Ｚ軸まわりの流体モーメントを示し、添字は対応する変数を意味する。変数Ｕ,ｖ,ｒ,δはそれぞれ前進速度、横方向（横滑り）速度、旋回（回頭）角速度と舵角とを示す。上式を応答モデルに直すと、舵加速度δ・・(t)≒０とし、
【００３８】
【数２】
を得る。ここで、ｓはラプラス演算子であり、
【００３９】
【数３】
とおいている。
【００４０】
（３）式より、横方向（sway）の横滑り速度とヨー軸回りの旋回角速度との運動方程式は同一の形でかつ舵角による入力係数が異なるだけである。船体特性は舵を切ることによって、方位軸回りに旋回角速度を発生させ、同時に船体の横方向に対水の反力による横滑り速度を発生させる。（３）式において、実用的見地からｓ２項による影響は無視できるためにｓ２項を省略する。すると、（３）式は
【００４１】
【数４】
になる。ここで、
【００４２】
【数５】
であり、Ｋsは旋回力ゲイン、Ｋvは横滑りゲイン、Ｔs3、Ｔv3は時定数である。
【００４３】
また、船舶用自動操舵装置は、舵角を通して制御量を入力するので、制御変数は角度単位の方が都合がよい場合があるため、
【００４４】
【数６】
を用いて横滑り速度ｖを斜航角βに変換することができる。
【００４５】
１．３ 外乱モデル
外乱モデルは、舵角オフセット成分、波浪モデルと潮流成分を考える。舵角オフセットは風などに起因する方位軸まわりに作用する船体モーメントを舵角換算にしたものである。波浪モデルは白色ノイズを入力した狭帯域フィルタの出力を方位相当にしたものである。潮流成分は対地座標系の速度成分である。式にまとめると、
【００４６】
【数７】
【００４７】
になる。ここでδo はほぼ一定値とした舵角オフセット成分を、ψw は波浪成分を、Ｎ（ｓ） は白色ノイズＮ（０，１）を、Ｇw は波浪モデルの伝達関数を、Ｋw ，ζw ，ωw はそれぞれゲイン、減衰係数と中心周波数とを、ｄx，ｄy は対地座標系のそれぞれ北向き，東向きの潮流速度を、Ｕd ，ψd は潮流速度の大きさと方位とを、それぞれ示し、風が船体上部構造物を押すことによる速度成分も含むものとする。
【００４８】
船体モデルと外乱モデルとからなる制御対象の構成を図５（ａ）に示す。ただし同図では非線形項は省く。
【００４９】
１．４ 対地速度と対地位置
船体の位置は、対地座標系で定義され、船体運動と潮流速度とからの対地速度を積分したものとなる。船体速度は対水速度で、潮流速度は対地速度でそれぞれ表す。
【００５０】
船体運動は舵角δを取ると角速度ｒ＝ψ・を生じるが、同時に横方向速度ｖ＝ｙ・も生じる。その様子を、図４を用いて説明する。同図のように定常に旋回しているときまたは参照座標系が接線方位に一致しているとき、船体座標系は接線方位に対して斜航角βだけ内側に傾いて釣り合う。このとき船体速度の対水速度成分は
【００５１】
【数８】
になる。ここでｕx，ｖy は対水速度、ｕ，ｖは船体座標系の速度を示し、添え字ｘ，ｙ：それぞれ北向き、東向きを示す。船体速度と斜航角との関係は、
【００５２】
【数９】
を用いる。
【００５３】
対地座標系の船体重心の速度は、対水速度と潮流速度との和になるから、
【００５４】
【数１０】
になる。ここでｘ、ｙはセンサより得られる船体位置でそれぞれ北向き、東向きを示す。
【００５５】
１．５ 軌道航路誤差
軌道航路誤差演算部１４は、実際では緯度経度座標系によって計算されるが、ここでは説明を簡単にするためで、２次元平面座標を用いる。軌道航路誤差演算部１４で求められる航路誤差及び方位誤差は、参照座標系に対する船体座標系の航路誤差及び方位誤差であり、図３より、
【００５６】
【数１１】
【００５７】
になる。ここでψ，ｘ，ｙは船首方位と対地位置とを、ψR，ｘR，ｙR は参照方位と参照位置とを、ψｅ，ｘｅ，ｙｅは方位誤差と航路誤差とをそれぞれ示す。ｘｅ，ｙｅは船体位置Ｇから参照座標系のＹＲ，ＸＲ 軸にそれぞれ垂線を下ろした距離に相当する。
【００５８】
制御対象の誤差モデルを構成するために、速度誤差を導入する。そこで、参照速度ｕR，ｖR を次のようにおく。
【００５９】
【数１２】
【００６０】
参照座標系における船体座標系の速度誤差は、前記参照速度との偏差を参照座標系に座標変換したものとなる。
【００６１】
よって、速度誤差は、ψｅ＝ψ−ψR（（１３）式より）の関係及び（１０）式を用いて、
【００６２】
【数１３】
になる。
【００６３】
一方、潮流速度による速度誤差は対地座標系成分を参照座標系成分に変換すると
【００６４】
【数１４】
になる。
【００６５】
ここでｕd，ｖd は参照座標系の潮流速度成分を、ｄx，ｄyは対地座標系の潮流速度成分をそれぞれ示す。よって参照座標系速度誤差ｘe・，ｙe・は
【００６６】
【数１５】
で与えられる。
【００６７】
図６は、制御対象のモデルを偏差系で表したものである。
【００６８】
２．保針モード
２．１ 制御対象
直線航路航行中における保針モードの設計で用いる制御対象は、参照座標系に対する誤差を扱う偏差系で定めることとする。
【００６９】
船体運動は舵角を入力とし、旋回角速度と斜航角とを出力とするので、そのまま偏差系として利用される。
【００７０】
旋回運動と横滑り運動とは（４）式及び図５を参照すると
【００７１】
【数１６】
になる。ここで、Ｒ（ｓ），ｒ：旋回角速度、Ｖ（ｓ），ｖ：横方向速度、Δ（ｓ），δ：舵角、Δo（s），δo（δ・o≒０）：舵角オフセットである。
【００７２】
船体座標系の速度誤差成分ｕｅ，ｖｅは（１５）式から得られるが、ψeを微小項として近似し、（１１）式を用いると、
【００７３】
【数１７】
になる。ここで、
【００７４】
【数１８】
ψeｖ≒０としている。
【００７５】
これにより、参照座標系の制御対象は、図５、図６、（１７）式、（１８）式、（１９）式、（２０）式から、
【００７６】
【数１９】
になる。ここで、Ｒx（ｓ）、ｒx、Ｖx（ｓ）、ｖx は状態変数であり（図５（ｂ）、（ｃ）参照）、旋回角速度と横滑り速度とは
【００７７】
【数２０】
になる。ここで、
【００７８】
【数２１】
【００７９】
を示す。（２３）式と（２６）式では、時定数Ｔsが共通であり、図５（ｂ）、（ｃ）に示したように、同様の構成をとっている。よって、（２６）式を消去することにすると、Ｒx（ｓ）、Ｖx（ｓ）とは、
【００８０】
【数２２】
となる。ここで、δo・≒０、ｓΔo≒０と仮定しており、一時遅れ要素を省略しても影響は少ないと仮定している。
【００８１】
よって、横滑り速度Ｖ（ｓ）、ｖは、
【００８２】
【数２３】
と表すことができる。Ｔs3＝Ｔv3＝０とすると、上式は、
【００８３】
【数２４】
となる。（２５）式に、（３３）式を入力することで、ｖの次数を減らすことができる。
【００８４】
方位制御系は、航路制御系から干渉を受けないので、方位制御系の制御ループは、単独で構成される。他方、航路制御系は方位制御系から干渉を受ける。
【００８５】
２．２ フィードバック制御
フィードバックゲイン器２０を図７Ａに示すように、方位制御系フィードバックゲイン器２０Ａと航路制御系フィードバックゲイン器２０Ｂとから構成する。
【００８６】
船体モデルに状態フィードバックした操舵ループの方位制御系の特性多項式は、方位制御系にのみ関連する項をまとめると、ζh，ωh をそれぞれ操舵系減衰係数と固有周波数とし、Ｔs3＝０とすると、
【００８７】
【数２５】
となり、
【００８８】
【数２６】
となる。Ｔs3≠０としたときには、
【００８９】
【数２７】
【００９０】
となる。設計パラメータを、比例ゲインＫP、減衰係数ζとしたときに、微分ゲインＫDと固有周波数ωhとは設計パラメータＫP、ζhと同定器によって同定される船体パラメータＫs,Ｔs,Ｔs3とから一義的に決定される（特開２００７−１１８８２８号参照）。
【００９１】
次に、航路制御系の特性多項式は、Ｔs3＝Ｔv3＝０とすると、
【００９２】
【数２８】
となる。Ｄt（ｓ）の開ループ伝達関数を
【００９３】
【数２９】
【００９４】
とすると、根軌跡は図７Ｂに示すようになる。図７Ｂから特性根は、Ｋy ＝０のとき、根は方位制御系特性根である極（記号×）と原点根とになり、Ｋy＞０ が大きくなると、極にあった根は安定側から不安定側に、原点根は左方向にそれぞれ移動し、実軸でBreak-in point になり左右方向に別れて原点根は左方向にそれぞれ移動した後、Breakaway point から安定側から不安定側に移り、さらにＫy ＞０ が大きくなると、根はゼロ点と左右方向とに向かって移動する、特徴をもつ。
【００９５】
航路ゲインＫyを求めるために、特性多項式を、
【００９６】
【数３０】
【００９７】
として定める。ここでａtは正の実数を、ζt，ωt はそれぞれ航路制御系減衰係数と固有周波数とを示す。航路制御系の設計パラメータをζt をζt＝ζhとして、それぞれの特性多項式の係数を比較すると、
【００９８】
【数３１】
上式を整理すると、ωtに関する３次方程式
【００９９】
【数３２】
を得る。このωtの３つの解に対して、最も小さい正の解を選択すると、図７Ｂから示されるように、
【０１００】
【数３３】
が成り立つ。即ち、航路制御系固有周波数ωtは、方位制御系固有周波数ωhよりも小さくなる。
【０１０１】
航路ゲインＫyは、
【０１０２】
【数３４】
として得られる。Ｔs3≠０、Ｔv3≠０としたときには、
【０１０３】
【数３５】
として得られる。
【０１０４】
また、フィードバックゲインゲイン器２０には、外乱による影響を除去するために修正量として、
【０１０５】
【数３６】
が入力される。ｖd は参照座標系の潮流速度成分である（（１６）式参照）。
【０１０６】
３．推定器
推定器１８は、方位誤差の推定を行う方位制御系推定器１８Ａ及び航路誤差の推定を行う航路制御系推定器１８Ｂと、潮流ベクトルの推定を行う対地座標系潮流推定器１８Ｃとを備える。
【０１０７】
３．１ 方位制御系推定器
方位制御系推定器１８Ａの構成例を図８に示す。この推定器は、波浪モデルと舵角オフセットとを組み込んだものであり、（２２）式、（２３）式及び検出方位誤差
【０１０８】
【数３７】
及びΔ=ΔFB、δ=δFBを用いて、
【０１０９】
【数３８】
【０１１０】
と表すことができる。ここで、^は推定値を、ｋｉ，ｉ＝１〜５は推定ゲインを、ξ^は推定状態量を、ψW^は推定波浪を、δO^、ΔO^は推定舵角オフセットを、それぞれ示す。上式を行列で表すと、
【０１１１】
【数３９】
となる。ここで、Ｋh は方位制御系（ＨＣＳ）の推定ゲインを示し、
【０１１２】
【数４０】
であり、
【０１１３】
【数４１】
であり、ε は微小項を示す。
【０１１４】
この特性多項式は、
【０１１５】
【数４２】
【０１１６】
に定める。ここでＤehは船体モデルの状態推定に、Ｄehwは波浪モデルの状態推定に、そしてＤehoは舵角オフセットモデルの状態推定にそれぞれ対応し、ζeh，ζehwとωeh，ωehw，ωeho とは方位制御系のそれぞれ推定減衰係数と推定固有周波数とで、ζehw，ωehwは、図示しない波浪同定器によって検出方位から同定されるもので、
【０１１７】
【数４３】
を示す。ここでωh は方位制御系の操舵系固有周波数（（３７）式）を、ρeh，ρeho は推定係数で推定速度やパラメータ不確かさに対する許容度から決定され
【０１１８】
【数４４】
とする。
【０１１９】
特性多項式の根を極配置することにより求める推定ゲインは、
【０１２０】
【数４５】
になる。ここで、添字(・)-1 は逆行列を意味し、添字(・)T は転置行列を意味し、
【０１２１】
【数４６】
になる。
【０１２２】
３．２ 航路制御系推定器
航路制御系推定器１８Ｂの構成例を図９に示す。（２４）式、（２５）式及びΔ=ΔFB、δ=δFBを用い、ｘ^e ＝ｘ−ｘ^，ｙ^e ＝ｙ−ｙ^とすると、
【０１２３】
【数４７】
と表すことができる。上式を行列で表すと、
【０１２４】
【数４８】
となる。ここで、Ｋt は方位制御系（ＴＣＳ）の推定ゲインを示し、
【０１２５】
【数４９】
であり、
【０１２６】
【数５０】
であり、ε は微小項を示す。
【０１２７】
この特性多項式は、
【０１２８】
【数５１】
【０１２９】
に定める。ここでζet，ζeto とωet，ωeto とは航路制御系のそれぞれ減衰係数と固有周波数とを
【０１３０】
【数５２】
を示す。ＤTCS の固有周波数を
【０１３１】
【数５３】
と定める。ここでωt は航路制御系の操舵系固有周波数（（４１）式参照）を、ρet，ρeto は推定係数で
【０１３２】
【数５４】
と定める（（５２）式参照）。
【０１３３】
特性多項式の根を極配置を実現するように、且つ、対称性を満足するようにすると、推定ゲインＫtは、
【０１３４】
【数５５】
とおいて
【０１３５】
【数５６】
を用いて、推定ゲインを用いた特性多項式を求めると
【０１３６】
【数５７】
になる。ここで
【０１３７】
【数５８】
を示す。
【０１３８】
設計パラメータとして、
【０１３９】
【数５９】
を定めると、これより推定ゲインは
【０１４０】
【数６０】
として得られる。
【０１４１】
３．３ 対地座標系潮流推定器
以上に説明したように、航路制御系推定器１８Ｂにおいては、参照座標系の航路誤差ｘe、ｙeを用いて対地座標系の潮流成分ｄx、ｄyを推定していた。しかしながら、変針中の参照座標系はψRが変化するために、線形に扱うことができない。
【０１４２】
対地座標系潮流推定器１８Ｃは、航路変化の影響を受けない対地座標系から潮流成分を推定するために、変針中であるなしに拘わらず、潮流推定を行うことができる。
【０１４３】
推定式は対地座標系において、（１２）式を基に（８）式を状態量に組み込むことで構成され
【０１４４】
【数６１】
【０１４５】
になる。ここで、潮流成分は一定と扱い、Ｋc：推定ゲイン、ｕ^x，ｖ^y：（２１）式および方位制御系推定器１８Ａから得られたＲx ^、ΔO^を（３３）式に代入することで得られるまたは（３４）式で得られる推定値ｖ^をそれぞれ（１０）式に代入した値である。
【０１４６】
尚、対地座標系潮流推定器１８Ｃにおいても、航路制御系推定器１８Ｂのように偏差系で構成することもでき、その場合、
【０１４７】
【数６２】
【０１４８】
となる。ここで、ｕNED，ｖNED：測地センサのＮＥＤ系速度である。上式から分かるように、偏差系では、外部ＮＥＤ系速度が新たに必要になり、推定誤差の算出のためｘ^，ｙ^も必要になり、ｘ^e，ｙ^eの計算量も増加する。そのため、偏差系としない方が好ましい。
【０１４９】
推定式は（６０）式より、ｘ，ｙに関して干渉せず同形なので、図１０に示すように、推定ゲインを
【０１５０】
【数６３】
に定める。推定器の特性多項式は
【０１５１】
【数６４】
になる。ここでｓ：ラプラス演算子である。一方、仕様の特性多項式を
【０１５２】
【数６５】
に定める。ここでωec，ωeco：推定固有周波数で
【０１５３】
【数６６】
【０１５４】
と定める。ここでωeh 、ωehoは、方位制御系推定器１８Ａに用いられる方位制御系の推定固有周波数であり、推定係数ρcの短期時は潮流成分を迅速に推定する際に利用する。上式及び（４５）式、（５０）式、（５４）式、（５５）式の関係から、上記推定固有周波数は、航路制御系推定器１８Ｂの推定固有周波数より高い値となり、これにより短期に推定可能である。これは対地座標系潮流推定が船体制御に無関係に実施されるために可能になる。これより推定ゲインは次式になる。
【０１５５】
【数６７】
【０１５６】
３．４ 切替手段
対地座標系潮流推定器１８Ｃの推定潮流ベクトルは常時使用するようにしてもよいが、図２Ｂにおいて切替手段ＳＷ及び図９において切替手段ＳＷ１、ＳＷ２に示すように、航路制御系推定器１８Ｂで演算される推定潮流ベクトルと、対地座標系潮流推定器１８Ｃで演算される推定潮流ベクトルとを、切り替えて選択するようにしてもよい。切替の条件は、計画航路の直線航路と曲線航路との切替指令、即ち保針モードと変針モードとの切替指令に連動させることができる。
【０１５７】
任意には、切替手段を除去し、対地座標系潮流推定器１８Ｃで演算される推定潮流ベクトルを常時使用するようにして、航路制御系推定器１８Ｂにおける推定潮流ベクトルの演算を省略することも可能である。
【０１５８】
また任意には、推定潮流ベクトルの切替とは別に、航路制御系推定器１８Ｂと対地座標系潮流推定器１８Ｃから演算されるそれぞれの推定潮流ベクトルを比較することで、モニタ機能を持たせることも可能である。本来であれば、航路制御系推定器１８Ｂと対地座標系潮流推定器１８Ｃとは、座標系及び推定ゲインが異なるだけであるから、同様の推定値となるはずであるが、それぞれの推定潮流成分の差異から、推定値の精度を評価することができる。
【０１５９】
４． 変針モード
軌道計画部１２は、図１１に示すように、誘導システム２２から曲線航路の計画航路が指令されると、その旋回条件を求め旋回条件に合致する参照方位ψRを発生する参照方位発生部３０と、船体の参照速度を発生する参照速度発生部３４と、船体の参照速度を積分して参照位置ｘR、ｙRを発生する参照位置発生部３６と、推定潮流ｄx、ｄyを座標変換して参照座標系の潮流成分を発生する座標変換部４０と、潮流に対抗するための斜航角βDを発生する潮流修正部４２と、対地速度の修正を行う対地速度修正部４４と、船体方位を参照方位ψR＋斜航角βDに遅れなく追従させるためのフィードフォワード舵角δFFを出力するフィードフォワード舵角発生部４６と、フィードフォワード舵角の修正を行うフィードフォワード舵角修正部４８と、を備える。また、旋回条件の中で、舵角設定値δ0についての修正を行って操舵機の飽和状態を回避する舵角設定部２８を備える(特願２０１１−１７６７６８参照)。
【０１６０】
舵角設定部２８は、舵角設定値δ0に対して、以下の式から、参照舵角設定値δ0Rを、
【０１６１】
【数６８】
【０１６２】
等として求め、参照方位発生部３０は、参照舵角設定値δ0Rを超えないように参照方位ψRを求める。ここで、Ｕdは、推定器１８から出力される選択された推定潮流成分ｄx^，ｄy^を潮流速度成分ｄx，ｄyとして、（９）式から求める。
【０１６３】
参照方位発生部３０から出力された参照方位ψRは、参照速度発生部３４において、（１４）式に用いられて、参照速度ｕR，ｖR が求められる。また、これらを参照位置発生部３６で積分することで、参照位置
【０１６４】
【数６９】
を得る。
【０１６５】
座標変換部４０は、推定器１８から出力される選択された推定潮流成分ｄx^、ｄy^を潮流速度成分ｄx，ｄyとして、参照方位発生部３０で発生された参照方位ψRを用いて（１６）式により、参照座標系の潮流速度成分ｕd、ｖdに変換する。
【０１６６】
参照座標系における対地速度は、対水速度と潮流速度との和になり、
【０１６７】
【数７０】
になる。ここで、ｕ*、ｖ*は参照座標系における速度成分、ｕD、ｖDは参照座標系における対水速度成分で、
【０１６８】
【数７１】
となる。ここで、βDは斜航角、ｕ、ｖは図４を参照されたい。
（６８）式、（６９）式から、βDを微小角として、
【０１６９】
【数７２】
と近似する。ｖは船体座標系の横方向（sway）速度で、旋回時に生じる横方向速度に相当する。
【０１７０】
潮流修正部４２は、
【０１７１】
【数７３】
によって斜航角βDを求める。
【０１７２】
潮流成分も含んだ指定旋回角速度ｒ0*は
【０１７３】
【数７４】
になる。ここで、ＳｅｔΔｕ：速度更新設定値（例えば１ｋｎｏｔ）、ｕ0*：前回の値とする。
【０１７４】
対地速度修正部４４は、（６８）式、（７１）式、（６９）式によって、得られた対地速度ｕ*の変化がΔｕを越えると、（７２）式に従い、指定旋回角速度ｒ0*を更新し、対地速度ｕ*と共にそれを参照方位発生部３０へと出力する。
【０１７５】
参照方位発生部３０は、更新された指定旋回角速度ｒ0*を満足する参照方位ψRを再計算する。その際の変針量は既に変針された方位分を差し引いた量、
【０１７６】
【数７５】
になる。ここでψ0*：変化する指定変針量、Δψ0：計画航路の変針量、Δψ’R：既に変針された方位量である。
【０１７７】
従って、例えば、変針中に潮流が変化することに起因して、対地速度ｕ*が変化して（７２）式の括弧中の条件が成り立つ場合には、対地座標系潮流推定器１８Ｃによって演算される対地座標系の推定潮流成分ｄx^，ｄy^を潮流速度成分ｄx，ｄyとして、（９）式を用いて（６７）式が改めて求められ、参照方位発生部３０で参照方位ψRが順次求められる。
【０１７８】
フィードフォワード舵角発生部４６は、計画航路に船体航路を追跡させるための、参照舵角δRと、潮流の斜航角に対応する潮流舵角δDを求める。（４）式から、
【０１７９】
【数７６】
により得られる。
【０１８０】
潮流の斜航角βDは、（７１）式、（１６）式に示されるように、閉ループ系のフィードバック制御部１６内にある推定器１８から得られる推定潮流成分d x^、d y^から求まるので、参照方位ψRに加算して、軌道航路誤差演算部１４に出力すると、マイナーループが構成されて、制御系特性が変化してしまう。よって、βDは前方に帰還できないので、フィードフォワード舵角修正部４８によって、後方で修正する。フィードフォワード舵角修正部４８は、フィードフォワード舵角発生部４６で得られたフィードフォワード舵角に対して、潮流の斜航角βDに対してフィードバックゲインＧFBを掛けたものを修正フィードバック舵角として、加算して修正する。
【０１８１】
変針中、フィードフォワード舵角修正部４８で修正された指令舵角であるフィードフォワード舵角δFFと、フィードバック制御部１６からの指令舵角であるフィードバック舵角δFBとは加算器１７で加算されて指令舵角δCとなり船体２４の操舵機へと出力される。
【０１８２】
変針中、座標変換部４０には、推定器１８の対地座標系潮流推定器１８Ｃで求められた推定潮流成分ｄx^，ｄy^が提供されるために、変針中に潮流変化が生じたとしても、その潮流変化が反映されるために、軌道計画部１２からより正確なフィードフォワード舵角を出力することができる。
【０１８３】
５．検証
以上、本発明の潮流の推定を用いて、曲線航路航行中に、従来の対地座標系潮流推定器１８Ｃを備えない従来構成における推定潮流ベクトルが一定値でなく、対地座標系潮流推定器１８Ｃによって演算した推定値で制御を行う。その潮流推定を用いて、曲線航路を航行した場合の効果を数値計算によって検証する。検証は次の項目について実施する。
計算条件を示す。
・船体パラメータ：Ｋs ＝ ０．０２７［１／ｓ］、Ｔs ＝１７．５４［ｓ］、Ｔs3 ＝ ０．１［ｓ］、Ｕ ＝ ２０［ｋｎｏｔ］
・操舵機：最大ＦＦ舵角３０［ｄｅｇ］、舵速度２［ｄｅｇ／ｓ］、旋回：半径１［ＮＭ］、旋回角１００［ｄｅｇ］
・潮流印加：
【０１８４】
【数７７】
【０１８５】
計算結果を表１にまとめる。
【０１８６】
【表１】
【０１８７】
以上表より、次の項目における本発明の有効性が確認された。
１．潮流が印加しない、あるいは一様な潮流が印加する場合、本発明の航路誤差および最大舵角は従来とほぼ同等かそれ以下である
２．ＣＴの途中から一様な潮流成分が印加する場合、本発明の航路誤差は従来のものより過渡応答の改善（航路誤差のピーク値減少、静定時間の短縮）が図られる
【符号の説明】
【０１８８】
１０ 船舶用自動操舵装置
１２ 軌道計画部
１４ 軌道航路誤差演算部
１６ フィードバック制御部
１８ 推定器
１８Ａ 方位制御系推定器
１８Ｂ 航路制御系推定器
１８Ｃ 対地座標系潮流推定器
２０ フィードバックゲイン器
２４ 船体
【特許請求の範囲】
【請求項１】
船体の参照方位及び参照位置を出力する軌道計画部と、センサで検出された船体の方位及び位置から船体の方位と位置とを参照方位及び参照位置に追跡させるべく指令舵角を出力するフィードバック制御部と、を備えた船舶用自動操舵装置において、
前記フィードバック制御部は、方位、航路及び潮流ベクトルの状態量の推定値を求める推定器と、推定器から出力される状態量の推定値に対してフィードバックゲインを作用して指令舵角（δFB）を出力するフィードバックゲイン器とを備えており
前記推定器は、方位に関する推定を行う方位制御系推定手段と、航路に関する推定を行う航路制御系推定手段と、対地座標系での潮流ベクトルの推定を行う対地座標系推定手段と、を備え、対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）から、参照方位を用いずに推定潮流ベクトル（ｄx^，ｄy^）の推定を行うことを特徴とする船舶用自動操舵装置。
【請求項２】
前記対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）と、船速及び横方向速度とから、推定潮流ベクトル（ｄx^，ｄy^）の推定を行うことを特徴とする請求項１記載の船舶用自動操舵装置。
【請求項３】
前記横方向速度は、方位制御系推定手段で得られる推定旋回角速度の関数として求められることを特徴とする請求項２記載の船舶用自動操舵装置。
【請求項４】
前記軌道計画部は、変針中に、前記対地座標系推定手段で演算された推定潮流ベクトルを用いて、参照方位に従う指令舵角（δFF）を出力することを特徴とする請求項１ないし３のいずれか１項に記載の船舶用自動操舵装置。
【請求項５】
前記航路制御系推定手段は、参照座標系における航路偏差から潮流ベクトルを推定しており、前記航路制御系推定手段で演算された推定潮流ベクトルと、前記対地座標系推定手段で演算された推定潮流ベクトルとのいずれかが選択されて状態量の推定値とされることを特徴とする請求項１ないし４のいずれか１項に記載の船舶用自動操舵装置。
【請求項６】
少なくとも変針中に対地座標系推定手段からの推定潮流ベクトルが選択されることを特徴とする請求項５記載の船舶用自動操舵装置。
【請求項７】
前記航路制御系推定手段の特性多項式の固有周波数よりも、前記対地座標系推定手段の特性多項式の固有周波数が高く設定されることを特徴とする請求項１ないし６のいずれか１項に記載の船舶用自動操舵装置。
【請求項１】
船体の参照方位及び参照位置を出力する軌道計画部と、センサで検出された船体の方位及び位置から船体の方位と位置とを参照方位及び参照位置に追跡させるべく指令舵角を出力するフィードバック制御部と、を備えた船舶用自動操舵装置において、
前記フィードバック制御部は、方位、航路及び潮流ベクトルの状態量の推定値を求める推定器と、推定器から出力される状態量の推定値に対してフィードバックゲインを作用して指令舵角（δFB）を出力するフィードバックゲイン器とを備えており
前記推定器は、方位に関する推定を行う方位制御系推定手段と、航路に関する推定を行う航路制御系推定手段と、対地座標系での潮流ベクトルの推定を行う対地座標系推定手段と、を備え、対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）から、参照方位を用いずに推定潮流ベクトル（ｄx^，ｄy^）の推定を行うことを特徴とする船舶用自動操舵装置。
【請求項２】
前記対地座標系推定手段は、センサから得られた船体の方位（ψ）及び対地座標系における船体の位置（ｘ，ｙ）と、船速及び横方向速度とから、推定潮流ベクトル（ｄx^，ｄy^）の推定を行うことを特徴とする請求項１記載の船舶用自動操舵装置。
【請求項３】
前記横方向速度は、方位制御系推定手段で得られる推定旋回角速度の関数として求められることを特徴とする請求項２記載の船舶用自動操舵装置。
【請求項４】
前記軌道計画部は、変針中に、前記対地座標系推定手段で演算された推定潮流ベクトルを用いて、参照方位に従う指令舵角（δFF）を出力することを特徴とする請求項１ないし３のいずれか１項に記載の船舶用自動操舵装置。
【請求項５】
前記航路制御系推定手段は、参照座標系における航路偏差から潮流ベクトルを推定しており、前記航路制御系推定手段で演算された推定潮流ベクトルと、前記対地座標系推定手段で演算された推定潮流ベクトルとのいずれかが選択されて状態量の推定値とされることを特徴とする請求項１ないし４のいずれか１項に記載の船舶用自動操舵装置。
【請求項６】
少なくとも変針中に対地座標系推定手段からの推定潮流ベクトルが選択されることを特徴とする請求項５記載の船舶用自動操舵装置。
【請求項７】
前記航路制御系推定手段の特性多項式の固有周波数よりも、前記対地座標系推定手段の特性多項式の固有周波数が高く設定されることを特徴とする請求項１ないし６のいずれか１項に記載の船舶用自動操舵装置。
【図１】
【図２Ａ】
【図２Ｂ】
【図３】
【図４】
【図５】
【図６】
【図７Ａ】
【図７Ｂ】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図２Ａ】
【図２Ｂ】
【図３】
【図４】
【図５】
【図６】
【図７Ａ】
【図７Ｂ】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【公開番号】特開２０１３−８６７４５（Ｐ２０１３−８６７４５Ａ）
【公開日】平成２５年５月１３日（２０１３．５．１３）
【国際特許分類】
【出願番号】特願２０１１−２３１５９４（Ｐ２０１１−２３１５９４）
【出願日】平成２３年１０月２１日（２０１１．１０．２１）
【出願人】（０００００３３８８）東京計器株式会社 (103)
【公開日】平成２５年５月１３日（２０１３．５．１３）
【国際特許分類】
【出願日】平成２３年１０月２１日（２０１１．１０．２１）
【出願人】（０００００３３８８）東京計器株式会社 (103)
[ Back to top ]