JP6658220B2 - 航走制御方法および装置 - Google Patents

Description

本発明は、航走体を目標位置で定点保持させるための航走制御方法および装置に関するものである。

水上無人航走体（Unmanned Surface Vehicle：以下、ＵＳＶという）や水中無人航走体（Unmanned Undersea Vehicle：以下、ＵＵＶという）などの無人航走体は、定点観測を行う場合、および、母船や他の航走体との会合を行う場合などに、設定された時間、同じ位置に留まる定点保持を行うことが要求される。

しかし、ＵＳＶには、風、波、潮流などの外乱が作用することがある。また、ＵＵＶには、潮流などの外乱が作用することがある。そのため、これらの無人航走体による定点保持を継続して行わせるためには、外乱により無人航走体に引き起こされる位置ずれを補償する航走制御が必要とされる。

ところで、無人航走体には、大別して、調査目標地点もしくはその地点を基準とした狭い範囲に留まって周囲の状況を調査するようにしてあるホバリング型の無人航走体と、設定された航走速度で移動（航走）しながら周囲の状況を調査するようにしてあるクルージング型（巡航型）の無人航走体がある。

このうち、クルージング型の無人航走体は、航走にある程度の高速性が要求されるため、機体の積載重量に制限を受けることが多い。そのため、クルージング型の無人航走体の運動は、前進、後進方向の推力を生じるメインスラスタの制御と、舵（操舵装置）の制御によって実施することが基本とされる。

したがって、クルージング型の無人航走体では、外乱を受ける条件の下での定点保持についても、メインスラスタの制御と舵の制御によって実現することが望まれている。

なお、水上航走体である船舶については、設定された定点付近に自動的に位置保持させるための自動定点復帰制御方法が提案されている（たとえば、特許文献１参照）。

特開平７−２２３５９１号公報

ところが、特許文献１に示された手法は、船舶が外乱（外力）を受けて定点から離れると、バウスラスタを作動させて船首が定点に向くように変位させ、その状態で推進装置を作動させて船舶を定点に向けて前進させて、船舶を定点に復帰させるものである。したがって、この手法の実施には、バウスラスタが必要とされる。

しかも、そのバウスラスタは、船舶に作用している外乱に抗して船首を定点に向けることが可能となる容量の大きさが求められる。

よって、従来は、クルージング型の航走体について、外乱を受ける条件の下で、バウスラスタのようなサブスラスタを使用せずに定点保持する手法は提案されていない、というのが実状である。

そこで、本発明は、クルージング型の航走体について、外乱を受ける環境下で、サブスラスタを使用することなく定点保持を実施できるようにするための航走制御方法および装置を提供しようとするものである。

本発明は、前記課題を解決するために、航走体の定点保持の目標位置を設定する処理と、前記目標位置から設定された距離で前記航走体に外乱が作用する方向の上流側へ移動した位置に、目標方位基準点を設定する処理と、方位制御と速度制御とを備える定点保持処理と、を行い、前記方位制御は、前記航走体の現在位置から前記目標方位基準点に向く方向を目標方位に設定して、該目標方位に前記航走体が向くように前記航走体の舵を制御する処理とし、前記速度制御は、前記航走体の現在位置について、前記目標方位と平行な方向に関する前記目標位置からの偏差を求めて、該偏差に応じた前記航走体の目標速度を設定し、設定された目標速度に前記航走体の速度が一致するように前記航走体の推進装置を制御する処理とする航走制御方法とする。

前記目標位置の周囲に設定された半径で定点保持処理開始範囲を設定し、前記航走体が前記定点保持処理開始範囲以内に配置されているときに、前記定点保持処理を行う方法とする。

前記定点保持処理開始範囲の周囲に、アプローチ開始範囲を設定し、前記アプローチ開始範囲に外部から到達する前記航走体は、前記アプローチ開始範囲と前記定点保持処理開始範囲との間で減速させてから、前記定点保持処理開始範囲に到達させるようにする方法とする。

航走体に、推進装置と、舵と、前記航走体自身の位置を検出する自己位置検出手段と、前記航走体に作用する外乱の方向を検出する外乱検出手段と、制御器とを備え、前記制御器は、前記の航走制御方法を実施する機能を備えるものとした航走制御装置とする。

本発明の航走制御方法および装置によれば、クルージング型の航走体について、外乱が存在する環境下で、サブスラスタを使用することなく定点保持を実施することができる。

航走制御装置の一例を示す概要図である。 航走制御方法における定点保持処理を示す概要図である。 定点保持処理による航走体の位置補正を説明する図である。 航走制御方法における制御フェーズの切り替えを説明する図である。

以下、本発明を実施するための形態を図面を参照して説明する。

［航走制御方法の実施形態］
図１は、航走制御方法の実施に用いる航走制御装置を示す概要図である。図２は、航走制御方法における定点保持処理の概要を示す図である。図３は、定点保持処理による航走体のスウェイ方向の位置補正の原理を示す図である。図４は、航走制御方法で制御フェーズを切り替える範囲を示す概要図である。

本実施形態は、航走制御方法を、無人航走体を適用して行う場合について説明する。

先ず、図１を用いて、本実施形態の航走制御方法の実施に用いる航走制御装置１の構成について説明する。

航走制御装置１は、クルージング型の無人航走体Ａの機体に装備されるもので、無人航走体Ａ自身の位置と姿勢（向き）を検出する手段（以下、自己位置検出手段２という）と、無人航走体Ａの周辺に存在している外乱７（図２参照）を検出する手段（以下、外乱検出手段３という）と、推進装置としてのメインスラスタ４と、舵（操舵装置）５と、メインスラスタ４および舵５の指令値を計算する制御器６とを備えた構成とされている。

自己位置検出手段２は、無人航走体Ａ自身の位置の情報を、図２に示すような定点保持の目標位置Ｔ０および目標方位基準点Ｔ１を含む領域に設定される絶対座標に関して検出する機能を備えている。なお、この絶対座標としては、経度と緯度による座標を用いることが好適であるが、それ以外の任意の地点を原点とする絶対座標を用いるようにしてもよい。以下、この絶対座標については、所定の絶対座標という。

また、自己位置検出手段２は、無人航走体Ａの姿勢として、無人航走体Ａの機体が向いている方向（方位）についての情報を検出する機能を備えている。

更に、自己位置検出手段２は、無人航走体Ａが移動するときの速度に関する情報を検出する機能を備えることが好ましい。

無人航走体ＡがＵＵＶの場合は、自己位置検出手段２としては、たとえば、慣性航法装置（ＩＮＳ）を用いるようにすればよい。

なお、ＵＵＶの図示しない母船（支援船）では、ＧＰＳなどの全地球航法衛星システムを用いて経度と緯度による自船の位置を計測することができると共に、音響測位により自船を基準とするＵＵＶの相対的な位置と移動方向と速度とを求めることができる。よって、これらの情報を基に、母船では、ＵＵＶの所定の絶対座標に関する位置と姿勢と速度の情報を算出することができる。この点に鑑みて、ＵＵＶに備える自己位置検出手段２は、ＵＵＶの所定の絶対座標に関する位置と姿勢と速度の情報を、音響通信により母船から得るものであってもよい。また、自己位置検出手段２は、前記の母船の位置情報と、母船を基準とするＵＵＶの相対的な位置と移動方向と速度の情報とを母船から音響通信により得て、ＵＵＶの所定の絶対座標に関する位置と姿勢と速度の情報を、ＵＵＶ側で算出するものであってもよい。

無人航走体ＡがＵＳＶの場合は、自己位置検出手段２としては、たとえば、慣性航法装置や、全地球航法衛星システムを用いるようにすればよい。

また、自己位置検出手段２としては、ＵＳＶに備えたカメラによる撮影画像や、レーダーなどを用いて、所定の絶対座標における位置の情報が予め分かっている地点や対象物を検出し、検出された地点や対象物を基準とするＵＳＶの相対的な位置と移動方向と速度の情報を求めて、ＵＳＶの所定の絶対座標に関する位置と姿勢と速度の情報を算出するようにしてもよい。

なお、自己位置検出手段２は、無人航走体Ａ自身の位置と姿勢、更には、速度を検出することができるものであれば、前記した以外の形式や方式の自己位置検出手段２を用いてもよいことは勿論である。

自己位置検出手段２で検出された無人航走体Ａ自身の位置と姿勢と速度の情報は、制御器６に入力される。

外乱検出手段３は、たとえば、無人航走体Ａに備えた図示しないドップラー流速計により測定される無人航走体Ａの海底（水底）に対する対地速度と、無人航走体Ａの対水速度との差を求めて、無人航走体Ａの周囲に存在している外乱７の作用する方向α（図２参照）と大きさ（強さ）とを検出する機能を備えている。

また、外乱検出手段３は、たとえば、自己位置検出手段２により検出される無人航走体Ａ自身の位置、姿勢（向き）および移動速度の情報と、メインスラスタ４で発生させている推力、および、舵５の操作状態に基づいて航走体の運動モデルに推定される挙動との差から、無人航走体Ａの周囲に存在している外乱７の作用する方向αと大きさとを検出する機能を備えるものとしてもよい。

無人航走体ＡがＵＵＶの場合は、外乱検出手段３により検出される外乱７は、ＵＵＶの周囲に存在している潮流が主となる。無人航走体ＡがＵＳＶの場合は、外乱検出手段３により検出される外乱７は、ＵＳＶに対する風、波、潮流の影響が合成されたものになる。

なお、外乱検出手段３は、無人航走体Ａの周囲に存在している外乱７の作用する方向αと大きさを検出することができるものであれば、前記した以外の形式や方式の外乱検出手段３を用いてもよいことは勿論である。

外乱検出手段３で検出された無人航走体Ａの周囲に存在している外乱７の作用する方向αと大きさに関する情報は、制御器６に入力される。

メインスラスタ４は、無人航走体Ａの前進方向および後進方向の推力を発生するものである。メインスラスタ４は、たとえば、無人航走体Ａの機体の後部側に設けられている。

また、メインスラスタ４は、制御器６から回転数指令値ｃ１が与えられると、回転数指令値ｃ１に従った回転数で運転を行う機能を備えている。

舵５は、無人航走体Ａの機体の方位（向き）を変更するためのものである。舵５は、たとえば、無人航走体Ａの機体の後部側に、メインスラスタ４に接近した配置で設けられている。

また、舵５は、制御器６から角度指令値ｃ２が与えられると、アクチュエータ（図示せず）を作動させて角度指令値ｃ２に従って角度変更する機能を備えている。

次に、制御器６の機能の説明を行うと共に、本実施形態の航走制御方法について説明する。

制御器６には、図２に示すように、所定の絶対座標に関して、無人航走体Ａの定点保持の目標位置Ｔ０の座標（Ｙ０，Ｘ０）のデータが、事前に登録される。この目標位置Ｔ０の座標データの登録は、入力手段（図示せず）を介してユーザが行うようにすればよい。なお、図２では、所定の絶対座標は、ｘ軸が上の方向を正の向きとする縦軸、ｙ軸が右の方向を正の向きとする横軸となる直交座標で示してある。

無人航走体Ａが、たとえば、ウェイポイント制御される形式のものである場合は、制御器６に登録する目標位置Ｔ０の座標データは、ウェイポイントファイルで指定するようにすればよい。なお、制御器６に登録される目標位置Ｔ０の座標データは、無人航走体Ａの運用時の制御方式に応じて、ウェイポイントファイル以外の形式で指定してもよいことは勿論である。

制御器６には、外乱検出手段３から、図２に矢印で示すような無人航走体Ａの周囲に存在している外乱７の作用する方向αと大きさに関する情報が入力される。本実施形態では、外乱７の作用する方向αは、ｘ軸の正の方向（図２では上向き）を基準（０度）とし時計回り方向を正とする角度で示すものとする。本実施形態は、α＝１８０度の場合の例が示してある。

制御器６は、外乱７の作用する方向αの情報が入力されると、図２に示すように、目標位置Ｔ０の座標（Ｙ０，Ｘ０）を、外乱７の作用する方向αの上流側へ設定された距離Ｌ０だけ並行移動させた座標（Ｙ１，Ｘ１）を次の（１）式で計算し、その座標（Ｙ１，Ｘ１）に目標方位基準点Ｔ１を設定する機能を備えている。
Ｙ１＝Ｙ０＋Ｌ０×ｓｉｎα
Ｘ１＝Ｘ０＋Ｌ０×ｃｏｓα ・・・（１）

なお、距離Ｌ０は、無人航走体Ａの旋回半径などの運動性能を考慮して、事前に設定されていてもよいし、たとえば、外乱検出手段３から制御器６に入力される外乱７の大きさを変数とする関数で算出するなど、外乱７の大きさに応じた値に設定するようにしてもよい。

また、制御器６は、自己位置検出手段２から入力される情報を基に、無人航走体Ａの現在位置の座標（ＹＡ，ＸＡ）を取得する機能を備えている。

更に、制御器６は、目標位置Ｔ０の座標（Ｙ０，Ｘ０）と、目標方位基準点Ｔ１の座標（Ｙ１，Ｘ１）と、無人航走体Ａの現在位置の座標（ＹＡ，ＸＡ）とを基に、以下の定点保持処理を実施する機能を備えている。

定点保持処理では、制御器６は、先ず、無人航走体Ａの現在位置の座標（ＹＡ，ＸＡ）から目標方位基準点Ｔ１の座標（Ｙ１，Ｘ１）に向かう方位を、無人航走体Ａの方位制御の目標方位θとして、次の（２）式で計算する。
θ＝ａｔａｎ（（Ｙ１−ＹＡ）/（Ｘ１−ＸＡ）） ・・・（２）

なお、本実施形態では、目標方位θは、ｘ軸の正の方向（図２では上向き）を基準とし、時計回り方向を正とする角度で示すものとする。

次に、制御器６は、無人航走体Ａの現在位置の座標（ＹＡ，ＸＡ）について、目標方位θと平行な方向に関する目標位置Ｔ０の座標（Ｙ０，Ｘ０）からの偏差を、サージ成分偏差Ｅ１として、次の（３）式で計算する。（３）式の右辺は、ベクトル（Ｙ０−ＹＡ，Ｘ０−ＸＡ）とベクトル（ｓｉｎθ，ｃｏｓθ）の内積である。
Ｅ１＝（Ｙ０−ＹＡ）ｓｉｎθ＋（Ｘ０−ＸＡ）ｃｏｓθ ・・・（３）

次いで、制御器６は、無人航走体Ａの速度制御の目標速度Ｖ１を、次の（４）式で計算する。なお、目標速度Ｖ１は、対地速度である。
Ｖ１＝Ｅ１×Ｐ１ ・・・（４）
ここで、Ｐ１は位置フィードバックゲインである。

その後、制御器６は、方位制御として、無人航走体Ａの機体の向きを（２）式で求めた目標方位θに合わせるために必要とされる舵５の角度指令値ｃ２を演算する。この角度指令値ｃ２の演算は、定点保持処理を実施する無人航走体Ａの旋回性能などに応じて行うようにすればよい。

制御器６は、この演算の結果として求まる角度指令値ｃ２を、舵５に対して出力する。

また、制御器６は、速度制御として、（４）式で求めた目標速度Ｖ１をメインスラスタ４の運転で実現するためのメインスラスタ４の回転数指令値ｃ１を演算する。この回転数指令値ｃ１の演算は、定点保持処理を実施する無人航走体Ａのメインスラスタ４による推進性能に応じて行うようにすればよい。

制御器６は、この演算の結果として求まる回転数指令値ｃ１を、メインスラスタ４に対して出力する。

なお、目標速度Ｖ１の制御手法としては、サージ成分偏差Ｅ１に位置フィードバックゲインＰ１をかける比例制御を行う場合について例示したが、偏差積分値を併用する比例積分制御など、他のフィードバック制御手法を用いてもよいことは勿論である。また、目標速度Ｖ１の計算を比例制御として、目標速度Ｖ１に追従するためのスラスタ回転数制御を比例積分制御としてもよい。

ところで、図２に示した状態のように、目標方位θと平行な方向に関して、目標位置Ｔ０が、無人航走体Ａの現在位置よりも無人航走体Ａの後方側に位置している場合は、（３）式で求まるサージ成分偏差Ｅ１は負値となる。よって、（４）式で求まる目標速度Ｖ１も負値となり、これは、無人航走体Ａを目標方位基準点Ｔ１から離反させる方向の対地速度となる。

無人航走体Ａの目標方位θが、目標位置Ｔ０から目標方位基準点Ｔ１に向く方位β（無人航走体Ａに存在している外乱７の作用する方位と逆となる方位、以下、基準方位βという）に一致しているときは、無人航走体Ａが目標位置Ｔ０と目標方位基準点Ｔ１との間に配置されている状態である。よって、この場合は、外乱７の作用を受けて無人航走体Ａが後退するときの対地速度が負値となる目標速度Ｖ１に比して遅ければ、目標速度Ｖ１が達成されるように、無人航走体Ａをメインスラスタ４の運転により後進させるようにすればよい。

一方、無人航走体Ａの目標方位θが基準方位βに対して傾いている場合は、無人航走体Ａの後進を行うと、基準方位βと直交する方向に関して無人航走体Ａの目標位置Ｔ０からの距離が増す可能性が生じる。よって、無人航走体Ａの目標方位θが基準方位βに対して傾いている場合であって、（４）式で求まる目標速度Ｖ１が負値のときには、無人航走体Ａの目標方位基準点Ｔ１から離反させる方向の移動は、できるだけ外乱７の作用で行わせることが好ましい。

ここで、以上の制御器６の定点保持処理による無人航走体Ａの位置補正の原理を、図３を用いて説明する。

無人航走体Ａが、たとえば、図３における位置ａ１に配置されているときに、制御器６による定点保持処理が実施されると、無人航走体Ａでは、制御器６の方位制御で求められた角度指令値ｃ２に従って舵５の角度が制御される。このため、無人航走体Ａは、機体の向きが、位置ａ１から目標方位基準点Ｔ１に向かう方向、すなわち、その時点での目標方位θ１に一致するように制御される。

この状態で、無人航走体Ａでは、制御器６の速度制御で求められた回転数指令値ｃ１に従ってメインスラスタ４が運転される。

したがって、位置ａ１に配置されている状態の無人航走体Ａは、メインスラスタ４の運転により目標方位θ１に向いて前進することになる。この際、目標方位θ１は基準方位βに対して傾斜しているので、無人航走体Ａの前進の運動には、基準方位βと直交する方向に関して目標位置Ｔ０に接近する方向の成分が含まれている。そのため、たとえば、無人航走体Ａが位置ａ１から位置ａ２まで前進したとすると、無人航走体Ａは、基準方位βと直交する方向に関して目標位置Ｔ０に接近する。

一方、無人航走体Ａが外乱７を受けて移動するときの方向は、無人航走体Ａの周囲に存在している外乱７の作用する方向αに一致している。したがって、たとえば、無人航走体Ａが外乱７を受けて位置ａ２から位置ａ３まで移動したとしても、そのときの無人航走体Ａの運動には、基準方位βと直交する方向の成分が含まれていない。そのため、無人航走体Ａは、基準方位βと直交する方向に関する目標位置Ｔ０からの距離は変化しない。

よって、無人航走体Ａは、位置ａ１から目標方位θ１に向かう方向の前進と、外乱７を受けることによる移動とが合成されることにより、位置ａ１から位置ａ３へ移動するようになる。その結果、無人航走体Ａは、スウェイ方向に関する目標位置Ｔ０からの偏差が減少する。

無人航走体Ａが位置ａ３に配置されているときには、制御器６による定点保持処理により、無人航走体Ａの機体の向きが、位置ａ３から目標方位基準点Ｔ１に向かう方向である目標方位θ２に一致するように制御される。

この状態で、無人航走体Ａでは、制御器６の速度制御で求められた回転数指令値ｃ１に従ってメインスラスタ４が運転されるため、無人航走体Ａは目標方位θ２に前進する。

この場合も、目標方位θ２は基準方位βに対して傾斜しているので、無人航走体Ａが、位置ａ３から位置ａ４まで前進したとすると、そのときの運動には、基準方位βと直交する方向に関して目標位置Ｔ０に接近する方向の成分が含まれている。よって、無人航走体Ａは、基準方位βと直交する方向に関して目標位置Ｔ０に接近する。

一方、無人航走体Ａが外乱７を受けて位置ａ４から位置ａ５まで移動したとしても、そのときの無人航走体Ａの運動には、基準方位βと直交する方向の成分が含まれていないため、基準方位βと直交する方向に関して目標位置Ｔ０との距離は変化しない。

よって、無人航走体Ａは、位置ａ３から目標方位θ２に向かう方向の前進と、外乱７を受けることによる移動とが合成されることにより、位置ａ３から位置ａ５へ移動するようになる。その結果、無人航走体Ａは、スウェイ方向に関する目標位置Ｔ０からの偏差が更に減少する。

その後は、無人航走体Ａは、前記と同様に、無人航走体Ａが配置されている位置に応じて定まる目標方位θの方向への前進と、外乱７を受けての移動との合成が繰り返し行われる。この際、目標方位θの基準方位βに対する傾斜角度が小さくなるにしたがって、無人航走体Ａが目標方位θに向けて前進する運動に含まれている基準方位βと直交する方向に関して目標位置Ｔ０に接近する方向の成分も、次第に小さくなる。

よって、無人航走体Ａは、スウェイ方向に関して、目標位置Ｔ０に漸近するようになる。

また、制御器６による定点保持処理では、前記したように、無人航走体Ａの目標位置Ｔ０に対するサージ成分偏差に応じた速度制御も行っているので、無人航走体Ａの目標位置Ｔ０に対するサージ成分偏差も次第に小さくなる。

したがって、制御器６による定点保持処理によれば、無人航走体Ａは、定点保持の目標位置Ｔ０に漸近させることができる。

この際、制御器６による定点保持処理では、方位制御は、舵５の角度指令値ｃ２による制御で行い、速度制御は、メインスラスタ４の回転数指令値ｃ１による制御で行うようにしてある。このため、制御器６では、定点保持処理における方位制御と速度制御を、独立の２自由度の制御として実施することができる。よって、方位制御と速度制御とが相互に影響することを抑制できて、各制御の制御性を高めることができる。したがって、制御器６の定点保持処理によれば、無人航走体Ａを円滑に目標位置Ｔ０に漸近させることができる。

ところで、無人航走体Ａは、クルージング型としてあるため、通常の航走にはある程度の高速性が要求される。

一方、前述したように、制御器６による定点保持処理では、無人航走体Ａの目標位置Ｔ０との相対的な位置関係に基づく速度制御を行うようにしてある。

そこで、本実施形態では、無人航走体Ａが、巡航している状態から、制御器６による定点保持処理を実施する状態に円滑に移行できるようにするために、制御器６は、無人航走体Ａの目標位置Ｔ０からの距離Ｒ（図４参照）に応じて、以下のような制御の切替えを更に行う機能を備えている。

制御器６には、図４に示すように、目標位置Ｔ０を中心として、半径ｒ１の定点保持処理開始範囲８と、半径ｒ２の定点保持処理逸脱範囲９と、半径ｒ３のアプローチ開始範囲１０とが事前に設定される。なお、ｒ１＜ｒ２＜ｒ３とされている。

定点保持処理開始範囲８は、外部から定点保持の目標位置Ｔ０に接近する無人航走体Ａが定点保持処理開始範囲８以内に位置するようになると、制御器６が定点保持処理を開始する範囲である。したがって、半径ｒ１は、目標方位基準点Ｔ１の設定に用いた距離Ｌ０よりも小さい値に設定される（ｒ１＜Ｌ０）。

なお、定点保持処理開始範囲８を規定する半径ｒ１は、設定値が大きすぎると、無人航走体Ａについて定点保持処理を開始する位置が、定点保持の目標位置Ｔ０から遠くなり、無人航走体Ａが目標位置Ｔ０に到達あるいは近接するまでに時間を要するようになる。

一方、半径ｒ１の設定値が小さすぎる場合は、無人航走体Ａの旋回性能などにもよるが、外部から定点保持処理開始範囲８に接近する無人航走体Ａが、定点保持処理開始範囲８に到達できずに通り過ぎる可能性が生じる。

そこで、定点保持処理開始範囲８を規定する半径ｒ１は、後述するアプローチ速度で定点保持処理開始範囲８に接近する無人航走体Ａが、通り過ぎることなく定点保持処理開始範囲８に確実に到達できるという条件の下で、定点保持処理開始範囲８のサイズができるだけ小さくなるように設定されていることが好ましい。

アプローチ開始範囲１０は、巡航速度で目標位置Ｔ０に向かって航走する無人航走体Ａが外部からアプローチ開始範囲１０に達すると、その無人航走体Ａが定点保持処理開始範囲に達するまでの間に、巡航速度から定点保持処理の開始に適したアプローチ速度まで減速させるための無人航走体Ａの減速処理を行う領域である。このアプローチ速度は、無人航走体Ａの運動性能に基づき、舵５が効く範囲で比較的低速の固定値（対地速度）として制御器６に事前に設定されている。

したがって、アプローチ開始範囲１０を規定する半径ｒ３は、半径ｒ３と半径ｒ１の差の値（＝ｒ３−ｒ１）が、無人航走体Ａが巡航速度からアプローチ速度まで減速するために必要とする距離に対応する値となるように設定されている。なお、図４では、半径ｒ３は、目標方位基準点Ｔ１の設定に用いた距離Ｌ０と同一となる状態が示してあるが、半径ｒ３と距離Ｌ０とが異なっていてもよいことは勿論である。

定点保持処理逸脱範囲９は、定点保持処理開始範囲８に到達して制御器６による定点保持処理が開始された状態の無人航走体Ａを対象として、定点保持処理開始範囲８の外周位置に定点保持処理が解除されるまでの不感帯（ヒステリシス）を形成するための範囲である。

特に、制御器６による定点保持処理は、無人航走体Ａの機体の向きを、目標位置Ｔ０とは別に設定する目標方位基準点Ｔ１の方向（目標方位θ）に向ける方位制御を行うようにしてある。そのため、定点保持処理が開始された状態と、解除された状態とでは、無人航走体Ａの機体を向ける向きが、目標方位基準点Ｔ１と、目標位置Ｔ０とで相違する。

そのため、定点保持処理逸脱範囲９を規定する半径ｒ２は、できるだけ大きな値に設定して、制御器６による定点保持処理が一旦開始された無人航走体Ａについては、外乱などの影響により定点保持処理開始範囲８を逸脱するような変位が生じても、なるべく定点保持処理が維持されるようにすることが好ましい。

また、制御器６は、無人航走体Ａの現在位置の座標（ＹＡ，ＸＡ）と、目標位置Ｔ０の座標（Ｙ０，Ｘ０）とを基に、無人航走体Ａの現在位置の目標位置Ｔ０からの距離Ｒを計算し、その結果に応じて無人航走体Ａの航走状態を以下のように制御する機能を備えている。

制御器６は、距離Ｒが半径ｒ３よりも大となるときには（Ｒ＞ｒ３）、無人航走体Ａを、巡航速度で目標位置Ｔ０に向かって航走させる制御を行う。なお、この制御は、ウェイポイント制御や、その他、既存の自律制御手法で行うようにすればよい。これにより、無人航走体Ａは、アプローチ開始範囲１０の外側に位置しているときには、目標位置Ｔ０に向かって巡航速度で航走する。

また、制御器６は、距離Ｒが半径ｒ３以下で且つ半径ｒ２よりも大となる場合（ｒ２＜Ｒ≦ｒ３）、および、距離Ｒが半径ｒ２以下で且つ半径ｒ１よりも大であると共に（ｒ１＜Ｒ≦ｒ２）、そのときの距離Ｒが半径ｒ１以下の値から増加したものでないという条件を満たす場合は、無人航走体Ａを、アプローチ速度まで減速させると共に、目標位置Ｔ０に向かって航走させる制御を行う。なお、この場合の制御も、ウェイポイント制御や、その他、既存の自律制御手法で行うようにすればよい。これにより、無人航走体Ａは、外部からアプローチ開始範囲１０に到達すると、アプローチ速度まで減速され、その状態で、定点保持処理開始範囲８に達するまで目標位置Ｔ０に向かって航走する。また、無人航走体Ａは、定点保持処理開始範囲８で制御器６による定点保持処理が一旦開始された後に、外乱などにより定点保持処理逸脱範囲９の外方まで変位すると、定点保持処理開始範囲８に達するまで目標位置Ｔ０に向かう航走を再び開始する。

更に、制御器６は、距離Ｒが半径ｒ１以下となる場合（Ｒ≦ｒ１）、および、距離Ｒが半径ｒ２以下で且つ半径ｒ１よりも大であると共に（ｒ１＜Ｒ≦ｒ２）、そのときの距離Ｒが半径ｒ１以下の値から増加したものであるという条件を満たす場合は、前述の定点保持処理を行う。これにより、無人航走体Ａは、外部から定点保持処理開始範囲８に到達すると、定点保持処理が開始される。また、無人航走体Ａは、定点保持処理が開始された後は、外乱７を受けて変位しても、定点保持処理逸脱範囲９の外周まで移動しない限りは、定点保持処理が継続される。

したがって、本実施形態の航走制御方法によれば、クルージング型の無人航走体Ａについて、外乱７が存在する環境の下で、定点保持の目標位置Ｔ０に漸近する移動を行わせることができると共に、無人航走体Ａの目標位置Ｔ０あるいは目標位置Ｔ０付近での定点保持を継続して行うことができる。

更に、無人航走体Ａの定点保持処理は、舵５による方位制御と、メインスラスタ４による速度制御で実現できるため、水平スラスタのようなサブスラスタを使用する必要はない。

また、本実施形態の航走制御方法では、無人航走体Ａが定点保持処理開始範囲８に到達すると、無人航走体Ａの機体の向きは目標方位基準点Ｔ１に向いた目標方位θに合わせられる、その状態で目標位置Ｔ０に接近するようになる。

航走体の向きを常に目標位置に向ける手法では、航走体が何らかの原因で目標位置を通り過ぎた場合は、その時点で航走体の向きを大幅に変更する回頭の動作が必要とされるが、本実施形態の航走制御方法では、たとえ無人航走体Ａが目標位置Ｔ０を通り過ぎたとしても、大きな回頭動作は不要である。

よって、本実施形態の航走制御方法では、無人航走体Ａが、目標位置Ｔ０に対して外乱７の上流側に寄った位置から外乱７に押されつつ目標位置Ｔ０に近付くような場合であっても、無人航走体Ａを目標位置Ｔ０へ円滑に接近させて、定点保持を開始させることができる。

ところで、一般に、舵が効くためには、０．５ノット程度の対水速度が必要とされる。そのため、無人航走体Ａの周囲に存在している外乱７が、定点保持している状態の無人航走体Ａに対して０．５ノット未満の対水速度しか生じさせないような強さの場合は、メインスラスタ４を断続的に運転して、一時的に無人航走体Ａを舵５が効く対水速度まで増速させることで、舵５による方位制御を行うようにすればよい。

なお、無人航走体Ａの周囲に存在している外乱７が、その上流となる方向を特定できないほど小さい環境の場合は、前述した目標方位基準点Ｔ１を設定することができないので、制御器６による舵５の方位制御とメインスラスタ４の速度制御による定点保持処理は実施できない。

このような条件の下でも、前述した制御器６による定点保持処理と同等の性能を得るためには、図１に二点鎖線で示すように、無人航走体Ａには、水平方向のサブスラスタ１１を装備して、方位制御のアシストと、ドリフト制御とを行うことが好ましい。

この場合、方位制御のアシストは、たとえば、制御器６より舵５に与える角度指令値ｃ２に、ゲインをかけた値をサブスラスタの指令値とすればよい。

また、ドリフト制御は、たとえば、無人航走体Ａの現在位置の座標（ＹＡ，ＸＡ）について、目標方位θと直交する方向に関する目標位置Ｔ０の座標（Ｙ０，Ｘ０）からの偏差を、スウェイ成分偏差Ｅ２として、次の（５）式で計算する。なお、スウェイ成分偏差Ｅ２は、無人航走体Ａの機体の左右方向に関する位置偏差である。よって、スウェイ成分偏差Ｅ２は、無人航走体Ａの現在位置から目標位置Ｔ０に向かう偏差ベクトル（Ｙ０−ＹＡ，Ｘ０−ＸＡ）と、無人航走体Ａの機体の向きである目標方位θに対するスウェイ方向の単位ベクトル（ｓｉｎ（θ＋９０ｄｅｇ），ｃｏｓ（θ＋９０ｄｅｇ））＝（ｃｏｓθ，−ｓｉｎθ）との内積で表すことができる。すなわち、以下の式である。
Ｅ２＝（Ｙ０−ＹＡ）ｃｏｓθ−（Ｘ０−ＸＡ）ｓｉｎθ ・・・（５）

次いで、スウェイ成分偏差Ｅ２にゲインをかけた値をドリフト制御、すなわち、スウェイ方向の目標速度として、この目標速度をサブスラスタの運転で実現するためのサブスラスタの指令値（回転数指令値）を演算すればよい。

この際、サブスラスタが無人航走体Ａの機体の前後にある場合は、方位制御のアシストは前後で反転させ、ドリフト制御は前後同方向とすればよい。

なお、スウェイ方向の目標速度の制御手法としては、スウェイ成分偏差Ｅ２にゲインをかける比例制御を行う場合について例示したが、偏差積分値を併用する比例積分制御など、他のフィードバック制御手法を用いてもよいことは勿論である。また、スウェイ方向の目標速度の計算を比例制御として、その目標速度に追従するためのサブスラスタの回転数制御を比例積分制御としてもよい。

なお、本発明は、前記実施形態にのみ限定されるものではなく、自己位置検出手段２、外乱検出手段３、制御器６が、無人航走体Ａの航走や運用に必要とされる別の機能を兼ね備えるものであってもよい。

図１では、航走制御装置１にて、自己位置検出手段２、外乱検出手段３、制御器６をそれぞれ別々に記載したが、いずれか２つ機能がある一つの装置に備えられている構成や、３つの機能がある一つ装置に備えられている構成であってもよい。

無人航走体Ａの周囲に存在している外乱７の作用する方向αは、任意の方向であってもよい。また、この外乱７の作用する方向αや大きさは、時間の経過と共に変化するものでもよい。

本発明の航走制御方法および装置は、船舶や潜水船のような有人の航走体について、設定された目標位置Ｔ０での定点保持を自動制御で行わせる場合に適用してもよい。

その他本発明の要旨を逸脱しない範囲内で種々変更を加え得ることは勿論である。

２ 自己位置検出手段、３ 外乱検出手段、４ メインスラスタ（推進装置）、５ 舵、６ 制御器、７ 外乱、８ 定点保持処理開始範囲、１０ アプローチ開始範囲、Ａ 無人航走体（航走体）、Ｅ１ サージ成分偏差（偏差）、Ｔ０ 目標位置、Ｔ１ 目標方位基準点、θ，θ１，θ２ 目標方位

Claims (4)

1. 航走体の定点保持の目標位置を設定する処理と、
   前記目標位置から設定された距離で前記航走体に外乱が作用する方向の上流側へ移動した位置に、目標方位基準点を設定する処理と、
   方位制御と速度制御とを備える定点保持処理と、を行い、
   前記方位制御は、前記航走体の現在位置から前記目標方位基準点に向く方向を目標方位に設定して、該目標方位に前記航走体が向くように前記航走体の舵を制御する処理とし、
   前記速度制御は、前記航走体の現在位置について、前記目標方位と平行な方向に関する前記目標位置からの偏差を求めて、該偏差に応じた前記航走体の目標速度を設定し、設定された目標速度に前記航走体の速度が一致するように前記航走体の推進装置を制御する処理とすること
   を特徴とする航走制御方法。
2. 前記目標位置の周囲に設定された半径で定点保持処理開始範囲を設定し、
   前記航走体が前記定点保持処理開始範囲以内に配置されているときに、前記定点保持処理を行う
   請求項１記載の航走制御方法。
3. 前記定点保持処理開始範囲の周囲に、アプローチ開始範囲を設定し、
   前記アプローチ開始範囲に外部から到達する前記航走体は、前記アプローチ開始範囲と前記定点保持処理開始範囲との間で減速させてから、前記定点保持処理開始範囲に到達させる
   請求項２記載の航走制御方法。
4. 航走体に、
   推進装置と、
   舵と、
   前記航走体自身の位置を検出する自己位置検出手段と、
   前記航走体に作用する外乱の方向を検出する外乱検出手段と、
   制御器とを備え、
   前記制御器は、請求項１から請求項３のいずれか一項に記載の航走制御方法を実施する機能を備えること
   を特徴とする航走制御装置。

Images