(57)【要約】【課題】  主機と追従機を設定せずに編隊での移動を可能とする。【解決手段】  一連の編隊を組んで移動する複数の水中航走体Ｘ１〜Ｘ７について、各水中航走体Ｘ１〜Ｘ７をグラフ理論における頂点とし、測位手段による測位可能な状態を弧として、必須測位対象の水中航走体Ｘ１〜Ｘ７を、全域木のグラフ構造が形成される状態に設定する。各水中航走体Ｘ１〜Ｘ７では、記憶部に、自機と、必須測位対象を含む他の移動体のウェイポイントファイルを記憶しておく。各水中航走体Ｘ１〜Ｘ７が編隊を組んで移動するときには、それぞれの制御装置にて、測位手段により検出される他の水中航走体Ｘ１〜Ｘ７と自機との相対位置の現在値について、ウェイポイントファイルから求められる相対位置の目標値からの偏差を求め、その偏差がゼロになるように、アクチュエータへの制御入力を定める。【選択図】図２
2018077728.tif 000002
【特許請求の範囲】
【請求項１】
編隊を組んで移動する複数の移動体について、
各移動体をグラフ理論における頂点とし、該各移動体に備えた測位手段による他の移動体の測位可能な状態をグラフ理論における弧として、前記移動体に、必ず測位すべき他の移動体である必須測位対象の移動体を、全域木または強連結なグラフ構造が形成される状態に設定する処理と、
前記各移動体に備えた記憶部に、自機の移動計画と、前記測位手段で測位される前記必須測位対象を含む他の移動体の移動計画とを記憶させる処理とを行い、
前記各移動体の編隊を組んでの移動時には、
該各移動体の前記測位手段により前記必須測位対象を含む他の移動体の測位を行う処理と、
前記各移動体にて、前記測位手段の測位結果から、測位時点について、自機と前記必須測位対象を含む他の移動体との相対位置の実測値を求める処理と、
前記記憶部に記憶されている前記各移動計画を基に、前記測位時点よりも後に設定される編隊修正時点で、自機と前記必須測位対象を含む他の移動体とが取るべき相対位置の目標値を求める処理と、
前記相対位置の実測値について、前記相対位置の目標値からの偏差を求める処理と、
前記偏差が前記編隊修正時点でゼロとなるように、自機のアクチュエータへ制御入力を与える処理とを行う
ことを特徴とする移動体制御方法。
【請求項２】
前記複数の移動体が組む編隊を、一連の編隊とする
請求項１記載の移動体制御方法。
【請求項３】
編隊を組んで移動する移動体に、
自機の位置を検出する位置検出手段と、
他の移動体の測位を行う測位手段と、
アクチュエータと、
制御装置と、
記憶部と、を備え、
前記制御装置には、前記編隊を組む各移動体をグラフ理論における頂点とし、該各移動体の前記測位手段による他の移動体の測位可能な状態をグラフ理論における弧として、全域木または強連結なグラフ構造が形成される状態になるよう必ず測位すべき他の移動体である必須測位対象の移動体が設定され、
前記記憶部は、自機の移動計画と、前記測位手段で測位される前記必須測位対象を含む他の移動体の移動計画とを記憶する機能を備え、
更に、前記制御装置は、
前記測位手段の測位結果から、自機と前記必須測位対象を含む他の移動体との相対位置の実測値を求める処理と、
前記記憶部に記憶されている前記各移動計画を基に、前記測位時点よりも後に設定される編隊修正時点で、自機と前記必須測位対象を含む他の移動体とが取るべき相対位置の目標値を求める処理と、
前記相対位置の実測値について、前記相対位置の目標値からの偏差を求める処理と、
前記偏差が前記編隊修正時点でゼロとなるように、前記アクチュエータへ制御入力を与える処理とを行う機能を備えること
を特徴とする移動体制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の移動体を、編隊を組んだ状態で移動させるために用いる移動体制御方法および移動体制御システムに関するものである。
【背景技術】
【０００２】
ＡＵＶ（Autonomous Underwater Vehicle）あるいはＵＵＶ（Unmanned Underwater Vehicle）と呼ばれる自律航走型の無人の水中航走体は、海底資源探査、機雷の探査とその除去、海洋遭難者の探索（探査）などに利用することが考えられている。
【０００３】
前記のような探査を行う場合、探査の効率を高めるためには、広範囲の探査を短時間で実施することが望まれる。
【０００４】
そこで、効率の良い探査を実施する手法としては、複数の水中航走体を、編隊を組んで航走させることが考えられている。
【０００５】
複数の移動体を編隊を組んで移動させる手法の一つとしては、複数の移動体のうちで主機と追従機とを定め、追従機を主機の移動に追従させて移動させる手法がある（たとえば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００４−２５９７１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
ところが、特許文献１に示された手法では、編隊を組む移動体のうち、追従機に設定されたものは、主機と通信を行う必要がある。そのため、追従機は、主機と通信可能な領域に位置している必要がある。
【０００８】
よって、特許文献１に示された手法では、主機と追従機で構成する編隊のサイズに、主機と通信可能な距離に依存する制限が生じる。
【０００９】
なお、水中では、通信は主に超音波を用いて行われるが、一般に、水中で通信可能な距離は、無線通信に比して短い。したがって、特許文献１に示された手法は、水中航走体のような水中を移動する移動体の編隊に適用すると、構成可能な編隊のサイズに対する制限が大きくなるというのが実状である。
【００１０】
更に、特許文献１に示された手法では、追従機が主機との通信を必要とするため、編隊を構成している追従機の数が多くなると、主機の通信負荷が大となってしまう。
【００１１】
よって、特許文献１に示された手法では、追従機の数に、主機の通信負荷に依存した制限が生じるというのが実状である。
【００１２】
特に、水中での通信は、無線通信に比して送受信可能な情報量が制限される。したがって、特許文献１に示された手法は、水中航走体のような水中を移動する移動体の編隊に適用すると、編隊を構成する移動体の数の制限が大きくなってしまう。
【００１３】
そこで、本発明は、複数の移動体について主機や追従機を定めることなく、複数の移動体を、編隊を組んだ状態で移動させることができる移動体制御方法および移動体制御システムを提供しようとするものである。
【課題を解決するための手段】
【００１４】
本発明は、前記課題を解決するために、編隊を組んで移動する複数の移動体について、各移動体をグラフ理論における頂点とし、該各移動体に備えた測位手段による他の移動体の測位可能な状態をグラフ理論における弧として、前記移動体に、必ず測位すべき他の移動体である必須測位対象の移動体を、全域木または強連結なグラフ構造が形成される状態に設定する処理と、前記各移動体に備えた記憶部に、自機の移動計画と、前記測位手段で測位される前記必須測位対象を含む他の移動体の移動計画とを記憶させる処理とを行い、前記各移動体の編隊を組んでの移動時には、該各移動体の前記測位手段により前記必須測位対象を含む他の移動体の測位を行う処理と、前記各移動体にて、前記測位手段の測位結果から、測位時点について、自機と前記必須測位対象を含む他の移動体との相対位置の実測値を求める処理と、前記記憶部に記憶されている前記各移動計画を基に、前記測位時点よりも後に設定される編隊修正時点で、自機と前記必須測位対象を含む他の移動体とが取るべき相対位置の目標値を求める処理と、前記相対位置の実測値について、前記相対位置の目標値からの偏差を求める処理と、前記偏差が前記編隊修正時点でゼロとなるように、自機のアクチュエータへ制御入力を与える処理とを行う移動体制御方法とする。
【００１５】
前記複数の移動体が組む編隊を、一連の編隊とする方法としてある。
【００１６】
また、編隊を組んで移動する移動体に、自機の位置を検出する位置検出手段と、他の移動体の測位を行う測位手段と、アクチュエータと、制御装置と、記憶部と、を備え、前記制御装置には、前記編隊を組む各移動体をグラフ理論における頂点とし、該各移動体の前記測位手段による他の移動体の測位可能な状態をグラフ理論における弧として、全域木または強連結なグラフ構造が形成される状態になるよう必ず測位すべき他の移動体である必須測位対象の移動体が設定され、前記記憶部は、自機の移動計画と、前記測位手段で測位される前記必須測位対象を含む他の移動体の移動計画とを記憶する機能を備え、更に、前記制御装置は、前記測位手段の測位結果から、自機と前記必須測位対象を含む他の移動体との相対位置の実測値を求める処理と、前記記憶部に記憶されている前記各移動計画を基に、前記測位時点よりも後に設定される編隊修正時点で、自機と前記必須測位対象を含む他の移動体とが取るべき相対位置の目標値を求める処理と、前記相対位置の実測値について、前記相対位置の目標値からの偏差を求める処理と、前記偏差が前記編隊修正時点でゼロとなるように、前記アクチュエータへ制御入力を与える処理とを行う機能を備えた構成を有する移動体制御システムとする。
【発明の効果】
【００１７】
本発明の移動体制御方法および移動体制御システムによれば、複数の移動体について主機や追従機を定めることなく、複数の移動体を、編隊を組んだ状態で移動させることができる。
【図面の簡単な説明】
【００１８】
【図１】移動体制御システムの第１実施形態を示す概要図である。
【図２】水中航走体が組む編隊の形状の例を示す図である。
【図３】グラフ理論の説明を行うための図である。
【図４】相対位置の目標値と、相対位置の現在地に偏差が生じている状態を示す図である。
【発明を実施するための形態】
【００１９】
以下、本発明を実施するための形態を図面を参照して説明する。
【００２０】
［第１実施形態］
図１乃至図４は、移動体制御方法および移動体制御システムの第１実施形態として、制御対象となる複数の移動体を、水中航走体とした例を示すものである。
【００２１】
図１は、本実施形態で制御対象とする水中航走体を示す概要図である。図２は、複数の移動体が組む編隊の形状の例を示すもので、図２（ａ）は横一列のＩ字型の編隊、図２（ｂ）はＶ字型の編隊、図２（ｃ）はＯ字型の編隊をそれぞれ示す概略平面図である。図３はグラフ理論の概要を説明するための図である。図４は、水中航走体同士の相対位置の目標値と、相対位置の現在地に偏差が生じている状態を示す図である。
【００２２】
本実施形態の移動体制御システムは、複数の図１に示す如き水中航走体Ｘにより構成される。
【００２３】
水中航走体Ｘは、位置検出手段１と、測位手段２と、アクチュエータ３と、制御装置４とを備えた構成とされている。
【００２４】
位置検出手段１は、水中航走体Ｘの自機の位置を検出する機能を備えるものである。位置検出手段１としては、たとえば、水面に浮上した状態のときに自機の位置を、たとえば、地球座標における位置として検出を行う全地球航法衛星システム（ＧＮＳＳ）の受信装置や、無線測位装置、および、水中に潜った状態のときに自己の位置を検出する慣性航法装置や、ドップラー流速計などから構成されていればよい。
【００２５】
測位手段２は、他の水中航走体Ｘの自機に対する相対位置を検出する機能を備えるものである。測位手段２としては、たとえば、音響測位装置や、短いレンジであれば電波を用いる測位装置、カメラなどの撮影画像に基づく光学的な測位装置などから構成されていればよい。
【００２６】
なお、測位手段２は、自機と他の水中航走体Ｘが図示しない音響通信機を備え、且つ音響通信可能な環境に配置されている場合に、他の水中航走体Ｘが位置検出手段１で自機の位置として検出した位置の情報を音響通信により取得し、その情報と、位置検出手段１で検出される自機の位置との差を基に、他の水中航走体Ｘの自機に対する相対位置を検出する機能を更に備えていてもよい。
【００２７】
アクチュエータ３は、水中航走体Ｘが自機の進行する方向と速度とを制御する機能を備えるものである。アクチュエータ３は、たとえば、各種のスラスタや舵などから構成されている。
【００２８】
制御装置４は、位置検出手段１と測位手段２から制御装置４へ入力される信号に基づき、後述する制御則に従う処理を行って、アクチュエータ３に指令（制御入力）を与える機能を備えている。
【００２９】
更に、制御装置４は、移動計画としてのウェイポイントファイルを記憶する記憶部５を備えている。ここで、ウェイポイントファイルとは、水中航走体Ｘが移動体として移動するコースの設定用のファイルである。ウェイポイントファイルは、辿るべき順序に従って番号付けされた複数のウェイポイントの座標情報であり、水中航走体Ｘは、自機用のウェイポイントファイルで設定された各ウェイポイントを番号の順に通過することで、設定されたコースに従って移動することができる。また、ウェイポイントファイルには、各ウェイポイントについて、水中航走体Ｘが各ウェイポイントを通過するときの通過目標時刻も設定されている。これにより、水中航走体Ｘは、各ウェイポイントを設定された通過目標時刻に通過することで、設定されたコースを設定された時間で移動することができる。
【００３０】
次に、制御装置４の機能の説明を行い、併せて、移動体制御方法について説明する。
【００３１】
本実施形態では、図２（ａ）に示すように、複数の水中航走体Ｘ、たとえば、７機の水中航走体Ｘが、横一列のＩ字型の編隊を組んで移動する場合の例について説明する。
【００３２】
先ず、準備として、各水中航走体Ｘには、個別の識別コードが設定される。ここでは、説明の便宜上、各水中航走体Ｘに、識別コードとして１〜７の自然数による連続した番号を付してある。図２（ａ）では、編隊の一方の端（図では左端）に位置する水中航走体Ｘから順に、符号Ｘにそれぞれの識別コードの数字１〜７が付してある。
【００３３】
各水中航走体Ｘ１〜Ｘ７の制御装置４（図１参照）には、自機の測位手段２（図１参照）で確実に測位する必要のある他の水中航走体Ｘ１〜Ｘ７が設定される。以下、測位手段２で確実に測位する必要のある他の水中航走体Ｘ１〜Ｘ７は、必須測位対象の水中航走体Ｘ１〜Ｘ７という。この際、ｉ番目の水中航走体Ｘｉは、（ｉ−１）番目の水中航走体Ｘ（ｉ−１）を必須測位対象とする（ｉ＝２，３、…、７）。これは、後述するグラフ理論にて、全域木のグラフ構造を形成する条件を少なくとも確保するためである。この各水中航走体Ｘ１〜Ｘ７による必須測位対象との関係は、図２（ａ）に矢印で示す。矢印の先端側が必須測位対象である。
【００３４】
なお、図２（ａ）に示すＩ字型のような一連（一筆書きが可能な形状）の編隊の場合は、編隊の一方の端に位置する水中航走体Ｘ１の制御装置４には、必須測位対象を２番目の水中航走体Ｘ２に設定してもよいし、設定しなくてもよい。
【００３５】
また、各水中航走体Ｘ１〜Ｘ７の制御装置４は、記憶部５に、自機用のウェイポイントファイルと、前記必須測位対象の水中航走体Ｘ１〜Ｘ７のコース設定用のウェイポイントファイルとを記憶しておく。
【００３６】
更に、各水中航走体Ｘ１〜Ｘ７の測位手段２による測位が可能となる範囲内に、必須測位対象とは別の水中航走体Ｘ１〜Ｘ７が位置すると想定される場合は、記憶部５には、必須測位対象以外の測位可能な水中航走体Ｘ１〜Ｘ７のコース設定用のウェイポイントファイルも記憶しておく。
【００３７】
なお、本実施形態では、各水中航走体Ｘ１〜Ｘ７は、図２（ａ）に示した如き横一列の編隊を組んだ状態で移動させる。したがって、各水中航走体Ｘ１〜Ｘ７のウェイポイントファイルでは、各水中航走体Ｘ１〜Ｘ７の移動コースが、設定された間隔を隔てたほぼ平行な配置で設定されている。また、各水中航走体Ｘ１〜Ｘ７の移動コースにおけるウェイポイントは、各水中航走体Ｘ１〜Ｘ７が並んで進行するように、通過目標時刻が設定されている。
【００３８】
これらのウェイポイントファイルの記憶部５への記憶は、陸上や母船上など、各水中航走体Ｘ１〜Ｘ７を航走させる前に調整しておく。
【００３９】
次いで、グラフ理論について説明する。
【００４０】
グラフは、たとえば、図３のように、複数の頂点ｖ１，ｖ２，ｖ３と、弧ａ１，ａ２，ａ３とから構成されている。
【００４１】
本実施形態では、各頂点ｖ１，ｖ２，ｖ３は、個々の水中航走体Ｘ１〜Ｘ７に対応する。なお、説明の便宜上、図３では、頂点の数は３としてある。
【００４２】
また、本実施形態では、双方向の矢印で示される弧ａ１は、頂点ｖ１の水中航走体Ｘ１〜Ｘ７と、頂点ｖ２の水中航走体Ｘ１〜Ｘ７との間で、互いに測位が可能であることを示している。
【００４３】
これに対し、一方向の矢印で示される弧ａ２は、矢印の基端側の頂点ｖ２の水中航走体Ｘ１〜Ｘ７から、矢印の先端側の頂点ｖ３の水中航走体Ｘ１〜Ｘ７が測位可能であることを示している。一方向の矢印で示される弧ａ３も同様に、矢印の基端側の頂点ｖ３の水中航走体Ｘ１〜Ｘ７から、矢印の先端側の頂点ｖ１の水中航走体Ｘ１〜Ｘ７が測位可能であることを示している。
【００４４】
したがって、本実施形態では、グラフにおける隣接は、頂点ｖｉから頂点ｖｊの測位が可能であることを示している。
【００４５】
また、グラフにおける連結性は、頂点ｖｉから頂点ｖｊへの道が存在することを示している。
【００４６】
グラフ構造は行列で表現可能であり、隣接行列Ａは、隣接なら１、それ以外は０で示す。よって、図３のグラフ構造の場合の隣接行列Ａは、以下のようになる。
【００４７】
【数１】
2018077728.tif 000003
【００４８】
次数行列Ｄは、隣接数を表し、図３のグラフ構造の場合は以下のようになる。
【００４９】
【数２】
2018077728.tif 000004
【００５０】
したがって、グラフラプラシアンＬは、以下のようになる。
【００５１】
【数３】
2018077728.tif 000005
【００５２】
グラフ理論では、グラフラプラシアンＬを見れば、そのグラフ構造を有するシステムが合意を得られるかどうかが分かる。ここで、合意とは、すべての頂点の値がある定数αに一致することである。所謂リーダー・フォロア型ではない分散システムの場合、合意条件は、グラフ構造が全域木、あるいは、強連結であることが知られている。
【００５３】
以上の準備が整っている状態で、各水中航走体Ｘ１〜Ｘ７は、図２（ａ）に示した編隊を組んで航走を開始する。
【００５４】
この際、それぞれの水中航走体Ｘ１〜Ｘ７では、記憶部５に、自機用のウェイポイントファイルが記憶されている。したがって、各水中航走体Ｘ１〜Ｘ７の制御装置４は、記憶部５に記憶されている自機用のウェイポイントファイルから、現時点で移動目標とするウェイポイントの座標を読み出す。
【００５５】
また、制御装置４は、位置検出手段１からの信号を基に、自機の現在位置の情報を得る。
【００５６】
したがって、制御装置４は、単にウェイポイント制御を行う場合は、自機の現在位置から、現在の移動目標としているウェイポイントへ向かうための制御入力を、アクチュエータ３に与えるようにすればよい。この場合の制御入力は、以下の（１）式となる。
（制御入力）＝（自機のウェイポイントファイルから求めた入力）    …（１）
【００５７】
なお、前記（１）式の右辺は、水中航走体のような移動体をウェイポイント制御する場合に、従来一般的に利用されている計算手法を採用してよい。これは、たとえば、自機の現在位置について、現在の移動目標からの偏差を求め、求められた偏差をゼロにするように、アクチュエータ３の制御入力を算出する計算手法とすればよい。この種の計算手法は、たとえば、特開２００７−１６０９７２号公報、特開２０１３−１３９１８５号公報などに示されている。
【００５８】
ところで、各水中航走体Ｘ１〜Ｘ７を実際に運用する場合には、たとえば、各水中航走体Ｘ１〜Ｘ７のうちのいずれかが、障害物回避などのためにウェイポイントファイルで設定されている移動コースから外れたところを航走すると、編隊の形状を維持できなくなることがある。
【００５９】
また、位置検出手段１として慣性航法装置を使用している場合は、長時間航走を続けると、慣性航法装置で検出している自機の位置に誤差が生じる。よって、この場合も、編隊の形状を維持できなくなる可能性がある。
【００６０】
そこで、本実施形態では、各水中航走体Ｘ１〜Ｘ７の各制御装置４は、測位手段２で測位した必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７の相対位置の検出結果を基に、測位時点について、自機と必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７との相対位置の実測値を求める。
【００６１】
また、各制御装置４は、記憶部５に記憶されているウェイポイントファイルを基に、前記測位時点よりも後に設定される編隊修正時点で、自機と必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７とが取るべき相対位置の目標値を求める。
【００６２】
更に、各制御装置４は、求めた相対位置の実測値について、相対位置の目標値からの偏差を求め、その偏差が前記編隊修正時点でゼロになるように、自機のアクチュエータ３へ制御入力を与える処理を行うようにしてある。
【００６３】
なお、前記補正修正時点は、各水中航走体Ｘ１〜Ｘ７の現在の移動目標としているウェイポイントの通過目標時刻が同時刻の場合は、そのウェイポイントの通過目標時刻を、編隊修正時点に設定してよい。この場合、各制御装置４では、自機と必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７とが取るべき相対位置の目標値は、記憶部５に記憶されている自機と必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７のウェイポイントの座標の差分として求めるようにすればよい。
【００６４】
また、各水中航走体Ｘ１〜Ｘ７の現在の移動目標としているウェイポイントの通過目標時刻が一致していない場合は、いずれかの水中航走体Ｘ１〜Ｘ７のウェイポイントの通過目標時刻を、編隊修正時点に設定してよい。更に、たとえば、各水中航走体Ｘ１〜Ｘ７の現在位置が、現在の移動目標としているウェイポイントに接近している場合などには、編隊修正時点は、ウェイポイントの通過目標時刻とは関連なく、一定のＴ秒後というように設定してもよい。
【００６５】
これらの場合、各制御装置４では、記憶部５に記憶されている自機と必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７のウェイポイントファイルを基に、前記編隊修正時点における自機と必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７の移動予定位置の座標を求める。次いで、各制御装置４では、求めた各移動予定位置の差分から、自機と必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７とが取るべき相対位置の目標値を求めるようにすればよい。
【００６６】
具体的には、各水中航走体Ｘ１〜Ｘ７の制御装置４は、たとえば、測位手段２で測位した他の水中航走体Ｘ１〜Ｘ７の自機に対する相対位置の検出結果の情報を基に、前記（１）式で求まる制御入力に、補正項を加える処理を行う。この補正項を加えた制御入力は、以下の（２）式となる。
（制御入力）＝（自機のウェイポイントファイルから求めた入力）
＋（音響測位とグラフ理論から求めた補正入力）      …（２）
【００６７】
前記（２）式の右辺第１項は、前記（１）式の右辺と同じである。右辺第２項は、補正項であり、本実施形態では、補正項は以下の（３）式のようなＰＩ制御とされている。
ｕｉ＝Ｊｉ＊１／Ｄｉｉ＊Σｊ＝１〜ｎ［Ａｉｊ＊｛Ｋｉ＊（ｄｉｊ−ｐｉｊ）
＋Ｉｉ＊∫（ｄｉｊ−ｐｉｊ）ｄｔ｝］                          …（３）
補正項＝変換行列＊（比例制御＋積分制御）
【００６８】
ｕｉは、補正項として、測位手段２による測位結果とグラフ理論から求めた入力であり、アクチュエータ３への制御入力の指令となる値である。
【００６９】
Ｊｉは、水中航走体Ｘ１〜Ｘ７の位置検出手段１で検出される地球座標系（慣性航法装置を使用している場合は慣性座標系）と、アクチュエータ座標系との変換行列である。
【００７０】
ここで、水中航走体Ｘ１〜Ｘ７では、各種スラスタや舵などのようにアクチュエータ３が複数の場合は、それぞれのアクチュエータ３の配置と向きが、並進運動と回転運動に関与し、この並進運動と回転運動の結果である自機の進行する方向と速度に関与している。したがって、水中航走体Ｘ１〜Ｘ７では、複数のアクチュエータ３の個々の制御量を行列により決めて合成すると、複数のアクチュエータ３の制御の結果として、地球座標系における水中航走体Ｘ１〜Ｘ７の移動方向と速度とが定まる。
【００７１】
よって、アクチュエータ３の配置と向きから定義されるアクチュエータ３の制御量を合成した行列を、アクチュエータ座標系という。これは、アクチュエータ数×６次の行列となる。
【００７２】
Ｄｉｉは、グラフの次数行列Ｄのｉ行ｉ列成分であり、グラフの隣接数を意味する。ｉ番目の水中航走体Ｘ１〜Ｘ７が測位できている他の水中航走体Ｘ１〜Ｘ７の数が代入される。
【００７３】
Ａｉｊは、グラフの隣接行列Ａのｉ行ｊ列成分であり、ｉ番目の水中航走体Ｘ１〜Ｘ７がｊ番目の水中航走体Ｘ１〜Ｘ７を測位できていれば１、そうでなければ０が代入される。
【００７４】
ｄｉｊは、ｉ番目の水中航走体Ｘ１〜Ｘ７とｊ番目の水中航走体Ｘ１〜Ｘ７との相対位置の目標値である。
【００７５】
ｐｉｊは、ｉ番目の水中航走体Ｘ１〜Ｘ７とｊ番目の水中航走体Ｘ１〜Ｘ７の相対位置の現在値である。
【００７６】
Ｋｉは、比例ゲインである。この比例ゲインは、限界感度法など一般的に用いられている手法で決定すればよい。
【００７７】
Ｉｉは、積分ゲインであり、一般的な方法で決定してよい。なお、測位が一定時間以上途切れた場合は、積分した値をゼロリセットするものとする。
【００７８】
以上の構成としてある水中航走体Ｘ１〜Ｘ７を備えた本実施形態の移動体制御システムでは、各水中航走体Ｘ１〜Ｘ７を図２（ａ）に示した横一列のＩ字型の編隊を組んで移動させる。
【００７９】
この際、各水中航走体Ｘ１〜Ｘ７のうちのいずれかが、障害物回避などのためにウェイポイントファイルで設定されている移動コースから外れたところを航走すると、その移動コースから外れた水中航走体Ｘ１〜Ｘ７では、自機と必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７との相対位置の実測値が、相対位置の目標値から偏差を生じる。
【００８０】
また、移動コースから外れた水中航走体Ｘ１〜Ｘ７を必須測位対象としている他の水中航走体Ｘ１〜Ｘ７においても、自機と必須測位対象を含む他の水中航走体Ｘ１〜Ｘ７との相対位置の実測値が、相対位置の目標値から偏差を生じる。
【００８１】
このように、前記相対位置の実測値が、相対位置の目標値から偏差を生じた水中航走体Ｘ１〜Ｘ７では、制御装置４より、その変化が解消するための制御入力がアクチュエータ３へ指令として与えられるようになる。
【００８２】
これにより、図４に概要を示すように、水中航走体Ｘｉと水中航走体Ｘｊの編隊修正時点での相対位置の目標値が図４に一点鎖線Ｐ０で示す如き状態であるのに対し、相対位置の現在値が図４に実線Ｐ１で示す状態である場合は、この相対位置の現在値Ｐ１が、編隊修正時点で相対位置の目標値Ｐ０に一致するように修正される。
【００８３】
このため、各水中航走体Ｘ１〜Ｘ７は、相対位置関係を保持した状態で移動することができて、全体では前記所定形状の編隊を保持することができる。あるいは、前記所定形状の編隊が一時的に崩れたとしても、各水中航走体Ｘ１〜Ｘ７は、所定形状の編隊を再び組んで移動することができる。
【００８４】
したがって、本実施形態の移動体制御方法および移動体制御システムによれば、複数の水中航走体Ｘ１〜Ｘ７を、所定形状の編隊を組んだ状態で移動させることができる。この際、各水中航走体Ｘ１〜Ｘ７は、主機や追従機を定める必要はない。
【００８５】
更に、本実施形態の移動体制御方法および移動体制御システムは、水中航走体Ｘ１〜Ｘ７では、測位手段２による他の水中航走体Ｘ１〜Ｘ７の測位を行うのみでよく、通信は必要としない。なお、一般に、測位に用いる信号は、通信に用いる信号に比して情報量が少なくてよいため、測位が可能な距離は、通信が可能な距離に比して大となる。
【００８６】
したがって、本実施形態の移動体制御方法および移動体制御システムでは、水中航走体Ｘ１〜Ｘ７により構成する編隊のサイズの拡大化を図ることができる。
【００８７】
また、個々の水中航走体Ｘ１〜Ｘ７では、自機に装備されている測位手段２の測位範囲に存在している他の水中航走体Ｘ１〜Ｘ７の測位を行うことができればよいため、編隊を構成する水中航走体Ｘ１〜Ｘ７の数の制限を解消することが可能である。
【００８８】
制御装置４にて求めるアクチュエータ３の制御入力の補正項は、（３）式のようなＰＩ制御とすることが好ましい。これは、比例制御項および積分制御項が共に目標値と現在値のフィードバック制御のため、相対位置の目標値と現在値との差が大きくなると、補正項の影響が大きくなるためである。
【００８９】
更に、補正項は、前記ＰＩ制御に代えて、以下に示すようなＰ制御や、ＰＩＤ制御や、フィードバック制御など、目標値に追従させるための制御則として既存の、あるいは、従来提案されている任意の制御則を適用してもよい。
【００９０】
Ｐ制御の場合は、補正項は以下の（４）式のようにすればよい。
ｕｉ＝Ｊｉ＊１／Ｄｉｉ＊Σｊ＝１〜ｎ［Ａｉｊ＊｛Ｋｉ＊（ｄｉｊ−ｐｉｊ）｝］    …（４）
補正項＝変換行列＊比例制御
【００９１】
ＰＩＤ制御の場合は、補正項は以下の（５）式のようにすればよい。
ｕｉ＝Ｊｉ＊１／Ｄｉｉ＊Σｊ＝１〜ｎ［Ａｉｊ＊｛Ｋｉ＊（ｄｉｊ−ｐｉｊ）
＋Ｉｉ＊∫（ｄｉｊ−ｐｉｊ）ｄｔ＋Ｄｉ＊（０−ｖｉｊ）｝］    …（５）
補正項＝変換行列＊（比例制御＋積分制御＋微分制御）
【００９２】
（５）式において、Ｄｉは、微分ゲインである。ｖｉｊは、ｉ番目の水中航走体Ｘ１〜Ｘ７とｊ番目の水中航走体Ｘ１〜Ｘ７との相対速度である。微分制御項の目標は、前記相対速度を０とすることである。
【００９３】
フィードバック制御の場合は、補正項は以下の（６）式のようにすればよい。
ｕｉ＝Ｊｉ＊１／Ｄｉｉ＊Σｊ＝１〜ｎ{Ａｉｊ＊Ｆｉｊ}              …（６）
補正項＝変換行列＊フィードバック制御
【００９４】
（６）式において、Ｆｉｊは、ｉ番目の水中航走体Ｘ１〜Ｘ７とｊ番目の水中航走体Ｘ１〜Ｘ７がとるべき相対位置関係についてのフィードバック制御である。
【００９５】
なお、Ｐ制御の場合は、位置検出手段１は、前述したＰＩ制御の場合と同様に、水中航走体Ｘの自機の位置を検出する機能を備えるものとすればよい。これに対し、ＰＩＤ制御の場合は、位置検出手段１は、水中航走体Ｘの自機の位置と速度とを検出する機能を備えるものとすればよい。また、フィードバック制御の場合は、位置検出手段１は、水中航走体Ｘの自機のフィードバック制御に必要な情報を検出する機能を備えるものとすればよい。
【００９６】
ところで、本実施形態では、各水中航走体Ｘ１〜Ｘ７に、必須測位対象の水中航走体Ｘ１〜Ｘ７を設定することにより、グラフ理論で合意条件が得られる全域木のグラフ構造を形成している。
【００９７】
そのため、各水中航走体Ｘ１〜Ｘ７にて、必須測位対象の水中航走体Ｘ１〜Ｘ７の測位を行うことが完全に不能になると、前記グラフ理論の合意条件（グラフの連結条件）が崩れてしまい、編隊を維持できなくなる。
【００９８】
このような自体を避ける手法の一つとしては、各水中航走体Ｘ１〜Ｘ７と必須測位対象の水中航走体Ｘ１〜Ｘ７との相対位置の目標値を、測位手段２による測位可能な範囲の限界に比して、十分に小さく設定するようにすればよい。
【００９９】
あるいは、各水中航走体Ｘ１〜Ｘ７の制御装置４にて、以下の応用例に示すような処理を更に行うようにしてもよい。
【０１００】
［第１実施形態の応用例］
本応用例では、各水中航走体Ｘ１〜Ｘ７の制御装置４は、第１実施形態で説明した機能に加えて、必須測位対象の水中航走体Ｘ１〜Ｘ７についての測位が不安定になると、完全に測位できなくなる前に、測位を再確立させるように自機を制御する機能を備える。
【０１０１】
ここで、測位が不安定とは、測位手段２による測位の処理が、ある間隔で繰り返し行われている状態にて、過去の設定された期間、あるいは、過去の設定された回数の測位処理において、必須測位対象の水中航走体Ｘ１〜Ｘ７についての測位が成功した回数の割合が、設定されたしきい値以下になることを意味している。たとえば、制御装置４は、測位手段２による過去１０回の測位処理において、必須測位対象の水中航走体Ｘ１〜Ｘ７の測位が成功した回数の割合が５０％以下になると、測位が不安定と判断するようにすればよい。
【０１０２】
あるいは、測位手段２が、測位の状況や測位の精度についての情報を出力する機能を有するものである場合は、制御装置４は、その情報に基づいて、測位の状況や測位の精度が設定されたしきい値以下になると、測位が不安定と判断するようにしてもよい。
【０１０３】
各水中航走体Ｘ１〜Ｘ７の制御装置４は、必須測位対象の水中航走体Ｘ１〜Ｘ７についての測位が不安定になると、必須測位対象の水中航走体Ｘ１〜Ｘ７に近付く方向に移動する制御入力を、アクチュエータ３へ指令する。
【０１０４】
このとき、制御装置４は、必須測位対象の水中航走体Ｘ１〜Ｘ７の現在位置については、測位情報が十分に得られていない。そこで、制御装置４は、以下の２つの情報を基に、必須測位対象の水中航走体Ｘ１〜Ｘ７の位置を予測するようにすればよい。
【０１０５】
１つ目は、測位が不安定になるまでの必須測位対象の水中航走体Ｘ１〜Ｘ７の測位結果である。２つ目は、記憶部５に記憶されている必須測位対象の水中航走体Ｘ１〜Ｘ７のウェイポイントファイルの情報である。なお、予測方法は、線形近似や２次近似などの一般的な手法を用いるようにすればよい。
【０１０６】
制御装置４は、このようにして予測した必須測位対象の水中航走体Ｘ１〜Ｘ７の位置に自機が近付く方向に移動する制御入力を、アクチュエータ３へ指令する。
【０１０７】
これにより、各水中航走体Ｘ１〜Ｘ７は、必須測位対象の水中航走体Ｘ１〜Ｘ７に近付く方向へ移動する。
【０１０８】
この移動に伴って、必須測位対象の水中航走体Ｘ１〜Ｘ７に対する測位の不安定な状況が解消されると、制御装置４は、前記した第１実施形態の制御を再開する。
【０１０９】
これにより、本応用例の移動体制御方法および移動体制御システムによれば、各水中航走体Ｘ１〜Ｘ７の測位手段２では、必須測位対象の水中航走体Ｘ１〜Ｘ７の測位が可能な状態を保持することができるため、水中航走体Ｘ１〜Ｘ７による編隊を、より確実に、あるいは、より安定に保持することができる。
【０１１０】
［第１実施形態の変形例］
なお、第１実施形態では、図２（ａ）に示したように、７機の水中航走体Ｘ１〜Ｘ７が、横一列のＩ字型の編隊を組んで移動する場合の例について説明したが、斜め方向に延びる一列の編隊や、縦一列の編隊を組んで移動する場合に適用してもよい。
【０１１１】
また、一連（一筆書きが可能な形状）の編隊の例としては、図２（ｂ）に示すようなＶ字型の編隊や、図２（ｂ）の編隊の前後を入れ替えた形状（逆Ｖ字型）の編隊を組んで移動する場合に適用してもよい。この場合、Ｖ字の一方の直線部分と、他方の直線部分に配置される水中航走体Ｘ１〜Ｘ７の数は異なっていてもよい。
【０１１２】
更に、図２（ｃ）に示すような複数の水中航走体Ｘ、たとえば、８機の水中航走体Ｘ１〜Ｘ８がＯ字型の編隊を組んで航走する場合に適用してもよい。
【０１１３】
このように、編隊が環状の構成となる場合は、１番目の水中航走体Ｘ１は、順番が最後の水中航走体Ｘ８を必須測位対象に設定する構成としてもよい。この場合、グラフ理論における合意条件は、グラフ構造が強連結であることになる。この場合も、第１実施形態と同様の効果を得ることができる。
【０１１４】
なお、本発明は前記実施形態にのみ限定されるものではなく、前記実施形態と応用例、変形例においては、水中航走体Ｘ１〜Ｘ７，Ｘ８のいずれかが、無人水上艇の測位を行う機能を備えるようにしてもよい。この場合、無人水上艇は、全地球航法衛星システム（ＧＮＳＳ）の受信装置を備えていれば、地球座標系における位置情報を得られるため、この無人水上艇が、地球座標系に設定された正しい移動コースを移動すれば、水中航走体Ｘ１〜Ｘ７，Ｘ８の編隊の形状の保持に加えて、編隊の位置を、各水中航走体Ｘ１〜Ｘ７，Ｘ８のウェイポイントで地球座標系に設定されたコースに合わせて、補正することができる。
【０１１５】
また、海底に地球座標系における位置が既知のランドマークを設置し、水中航走体Ｘ１〜Ｘ７，Ｘ８のいずれかが、このランドマークの測位を行う機能を備える構成としてもよい。この場合は、水中航走体Ｘ１〜Ｘ７，Ｘ８の位置検出手段１として慣性航法装置を用いているときに、各水中航走体Ｘ１〜Ｘ７，Ｘ８のうち、前記ランドマークの近くを通過して測位可能なものでは、位置検出手段１としての慣性航法装置の位置誤差を修正することができる。このように位置誤差を修正した水中航走体Ｘ１〜Ｘ７，Ｘ８は、地球座標系に設定されたウェイポイントに正しくしたがって移動することができる。したがって、この位置誤差が修正された水中航走体Ｘ１〜Ｘ７，Ｘ８を他の水中航走体Ｘ１〜Ｘ７，Ｘ８が測位することで、水中航走体Ｘ１〜Ｘ７，Ｘ８の編隊の形状の保持に加えて、編隊の位置を、各水中航走体Ｘ１〜Ｘ７，Ｘ８のウェイポイントで地球座標系に設定されたコースに合わせて、補正することができる。
【０１１６】
本発明の移動体制御方法および移動体制御システムは、編隊を組んで移動させる移動体であれば、水中航走体以外の移動体を適用対象としてもよい。
【０１１７】
その他本発明の要旨を逸脱しない範囲内で種々変更を加え得ることは勿論である。
【符号の説明】
【０１１８】
１  位置検出手段、２  測位手段、３  アクチュエータ、４  制御装置、５  記憶部、Ｘ，Ｘ１，Ｘ２，Ｘ３，Ｘ４，Ｘ５，Ｘ６，Ｘ７，Ｘ８  水中航走体（移動体）、ｖ１，ｖ２，ｖ３  頂点、ａ１，ａ２，ａ３  弧、Ｐ０  相対位置の目標値、Ｐ１  相対位置の現在値
【図１】
2018077728.tif 000006
【図２】
2018077728.tif 000007
【図３】
2018077728.tif 000008
【図４】
2018077728.tif 000009