JP7267019B2 - ビークルユニット及びビークルユニットの移動制御方法 - Google Patents

Description

本開示は、複数のビークルを含むビークルユニット及びビークルユニットの移動制御方法に関する。

水中を移動する複数のビークルを移動させる技術の一つとして、音響通信を行って、相対位置を確認しつつ、移動制御を行うシステムがある（例えば、特許文献１）。

特開２０１７－１６５３３３号公報

特許文献１に記載のシステムは、複数のビークルの位置の基準となる水上航行体が水上を航行する。また、水上航行体は、音響測位装置を用いて、複数の水中航行体の位置を検出し、検出した結果に基づいて、水中航行体の位置の制御を行う。このため、常に水上に航行体が露出していることになる。また、水上航行体と通信できる数に限りがあり、群を構成するビークルの数が制限される。

本開示は、上記の事情に鑑みてなされたものであり、水中を航行する多数のビークルの相対位置の制御を効率よく制御できるビークルユニット及びビークルユニットの移動制御方法を提供することを目的とする。

上述した課題を解決し、目的を達成するために、本発明の少なくとも一実施形態は、第１筐体と、前記第１筐体の３次元方向の移動を制御する第１慣性航法装置と、音波発生装置と、高さ方向の位置を検出する第１高度計測部と、位置情報検出装置と、移動経路の情報を記憶する第１制御装置と、を含み水中を移動するリーダー機と、第２筐体と、前記第２筐体の３次元方向の移動を制御する第２慣性航法装置と、前記音波発生装置から出力される音波を受信する受信装置と、高さ方向の位置を検出する第２高度計測部と、前記リーダー機との３次元の相対位置の情報を記憶する第２制御装置と、を含み、水中を移動する少なくとも１つのフォロワー機と、を備え、前記第１制御装置は、前記音波発生装置から音波を出力させ、前記第２制御装置は、前記受信装置で受信した音波に基づいて前記リーダー機との相対位置を算出し、前記第２高度計測部で検出した高さ方向の位置及び前記リーダー機との相対位置に基づいて前記第２慣性航法装置を制御して前記フォロワー機を移動させるビークルユニットを提供する。

また、前記リーダー機は、所定の時間間隔で水面に浮上し、前記位置情報検出装置で位置情報を検出し、前記第１制御装置は、前記移動経路、前記位置情報検出装置で検出した位置情報及び前記第１高度計測部で検出した高さ方向の位置に基づいて前記第１慣性航法装置を制御して前記リーダー機を移動させることが好ましい。

また、前記第２制御装置は、高さ方向の位置が基準を満たさないと判定した場合、高さ方向の位置を補正し、高さ方向の位置が基準を満たし、かつ、水平方向の位置が基準を満たさないと判定した場合、水平方向の位置を補正することが好ましい。

また、前記第２制御装置は、水平方向の位置が基準を満たさないと判定した場合、推進方向の位置を補正し、その後、水平方向に平行で、推進方向に交差する方向の位置を補正することが好ましい。

また、前記フォロワー機は、識別情報を含む信号を出力する出力装置を含み、前記リーダー機は、前記出力装置から送信される信号を受信する受信装置を含み、前記第１制御装置は、前記受信装置で受信した信号に基づいて、前記フォロワー機の状態の情報を取得することが好ましい。

また、前記出力装置は、前記リーダー機の位置に基づいて、ビームフォーミングした信号を出力することが好ましい。

また、前記音波発生装置は、前記フォロワー機の位置に基づいて、ビームフォーミングした信号を出力することが好ましい。

上述した課題を解決し、目的を達成するために、本発明の少なくとも一実施形態は、リーダー機と複数のフォロワー機とを含み、水中を３次元方向に移動可能な複数のビークルを移動させる複数のビークルの移動制御方法であって、前記リーダー機から音波を出力するステップと、前記フォロワー機が音波を受信し、前記リーダー機との相対位置を取得するステップと、前記フォロワー機が高さ方向の位置を検出するステップと、前記フォロワー機は、検出した高さ方向の位置及び前記リーダー機との相対位置に基づいて移動させるステップと、を含むビークルユニットの移動制御方法を提供する。

また、前記リーダー機が所定の時間間隔で水面に浮上し、位置情報を検出するステップと、前記リーダー機が、予め設定された移動経路、検出した位置情報及び高さ情報に基づいて移動するステップをさらに含むことが好ましい。

また、前記フォロワー機は、高さ方向の位置が基準を満たさないと判定した場合、高さ方向の位置を補正し、高さ方向の位置が基準を満たし、かつ、水平方向の位置が基準を満たさないと判定した場合、水平方向の位置を補正することが好ましい。

また、前記フォロワー機は、水平方向の位置が基準を満たさないと判定した場合、推進方向の位置を補正し、その後、水平方向に平行で、推進方向に交差する方向の位置を補正することが好ましい。

また、前記フォロワー機が、識別情報を含む信号を出力するステップと、前記リーダー機が、前記フォロワー機から送信される信号を受信し、受信した信号に基づいて、前記フォロワー機の状態の情報を取得するステップと、を含むことが好ましい。

本発明の少なくとも一実施形態によれば、水中を航行する多数のビークルの相対位置の制御を効率よく制御することができる。

図１は、ビークルユニットの概略構成を示す上面図である。 図２は、ビークルユニットの概略構成を示す側面図である。 図３は、リーダー機の主要構成を示すブロック図である。 図４は、フォロワー機の主要構成を示すブロック図である。 図５は、リーダー機の移動制御方法の一例を示すフローチャートである。 図６は、フォロワー機の移動制御方法の一例を示すフローチャートである。 図７は、フォロワー機の移動制御のアルゴリズムの一例を示すフローチャートである。 図８は、フォロワー機の推進方向における位置の制御を説明するための説明図である。 図９は、フォロワー機の推進方向における位置の制御を説明するための説明図である。 図１０は、フォロワー機の水平方向における姿勢の制御を説明するための説明図である。 図１１は、フォロワー機の水平方向における姿勢の制御を説明するための説明図である。 図１２は、ビーグルユニットの移動の一例を示す模式図である。 図１３は、ビークルユニットで送受信される音波の一例を示す模式図である。 図１４は、ビークルユニットで送受信される音波の一例を示す模式図である。

以下に、本発明に係る実施形態を図面に基づいて詳細に説明する。なお、この実施形態によりこの発明が限定されるものではない。また、実施形態における構成要素には、当業者が置換可能かつ容易なもの、あるいは実質的に同一のものが含まれる。さらに、以下に記載した構成要素は適宜組み合わせることが可能である。

図１は、ビークルユニットの概略構成を示す上面図である。図２は、ビークルユニットの概略構成を示す側面図である。ビークルユニット１０は、１つのリーダー機１２と、４つのフォロワー機１４_１、１４_２、１４_３、１４_４とを含む。以下、４つのフォロワー機１４_１、１４_２、１４_３、１４_４に共通する事項を説明する場合、フォロワー機１４として説明する。また、本実施形態では、フォロワー機１４を４つとしたが、フォロワー機１４の数は、特に限定されず、１つ以上あればよい。リーダー機１２とフォロワー機１４は、水中を航行する水中航行体である。

ビークルユニット１０は、リーダー機１２と複数のフォロワー機１４とは、予め設定された相対位置に配置された状態で、つまり所定のフォーメーションで、１つの群で移動を行う。具体的には、ビークルユニット１０は、リーダー機１２の位置を基準として、４つのフォロワー機１４_１、１４_２、１４_３、１４_４が３次元の所定の位置に配置された状態で移動する。リーダー機１２は、水面を基準として深さＤａとなる位置で移動する。

フォロワー機１４_１は、水平方向おいて、リーダー機１２と自機とを結んだ線と推進方向とのなす角がβ_１となり、推進方向と深さ方向とで形成される面において、リーダー機１２と自機とを結んだ線と推進方向とのなす角がα_１となる。また、フォロワー機１４_１は、水面を基準として深さＤｂとなる位置で、リーダー機１２との深度差、深さ方向の距離が、深さＤ_１となる。同様にフォロワー機１４_２は、水平方向おいて、リーダー機１２と自機とを結んだ線と推進方向とのなす角がβ_２となり、推進方向と深さ方向とで形成される面において、リーダー機１２と自機とを結んだ線と推進方向とのなす角がα_２となる。また、フォロワー機１４_２は、水面を基準として深さＤｃとなる位置で、リーダー機１２との深度差、深さ方向の距離が、深さＤ_２となる。同様にフォロワー機１４_３は、水平方向おいて、リーダー機１２と自機とを結んだ線と推進方向とのなす角がβ_３となる。フォロワー機１４_４は、水平方向おいて、リーダー機１２と自機とを結んだ線と推進方向とのなす角がβ_４となる。フォロワー機１４_３とフォロワー機１４_４は、推進方向と深さ方向とで形成される面における図示は省略しているが、所定の深さ、所定の角度に配置される。推進方向と深さ方向とで形成される面における図示は省略しているが、所定の深さ、所定の角度に配置される。

図３は、リーダー機の主要構成を示すブロック図である。リーダー機１２は、筐体２０と、第１慣性航行装置２１と、音波発生装置２２と、短パルス受信装置（受信装置）２４と、第１高度計測装置２６と、位置情報検出装置２８と、第１制御装置３０と、を備える。リーダー機１２は、上記構成に加え、水中航行体として備える各種構成を備えている。筐体２０は、リーダー機１２の各部を収納する外殻である。

第１慣性航行装置２１は、駆動力を発生させ、筐体２０を推進し、姿勢を変化させる機器である。第１慣性航行装置２１は、上下ラダー３２と、左右エレベータ３４と、メインスラスタ３６と、を有する。上下ラダー３２は、筐体２０の上下方向にそれぞれ突出する舵板を有し、舵板を上下方向に延びる軸を軸として回転させる。上下ラダー３２は、軸に沿って舵板の向きを変えることで、筐体２０に上下方向の周りに回転する方向の力を作用させる。左右エレベータ３４は、筐体２０の左右方向にそれぞれ突出する舵板を有し、舵板を左右方向に延びる軸を軸として回転させる。左右エレベータ３４は、軸に沿って舵板の向きを変えることで、筐体２０に左右方向の周りに回転する方向の力を作用させる。つまり、左右エレベータ３４は、筐体２０を浮上させる方向、沈降させる方向に回転させる。メインスラスタ３６は、プロペラ、ジェット推進機であり、筐体２０に推進方向に推進させる力を作用させる。第１慣性航行装置２１は、メインスラスタ３６で推進させつつ、上下ラダー３２及び左右エレベータ３４の向きを制御することで、筐体２０の姿勢を変化させることができる。

音波発生装置２２は、音響信号を出力する。音波発生装置２２は、リーダー機１２の自己位置情報のみを変調して短パルスで送波する。短パルス音は、複雑な通信プロトコルに基づくものではなく短時間のパルス信号である。音波発生装置２２は、リーダー機１２が出力したことがわかる音波を出力すればよいが、リーダー機１２の現在位置、姿勢情報を変調して短パルス音に乗せて送信することが好ましい。位置情報としては、リーダー機１２の自己位置のＺ軸方向の値（すなわち深度情報）を送ることが好ましい。深さ方向の情報を出力することで、相対位置をより高い精度で特定することができる。短パルス受信装置２４は、フォロワー機から送信される音響信号を受信する。短パルス受信装置２４は、センサアレイで構成され、音響信号を受信することで、音響信号を出力したフォロワー機１４の方向を検出する。

第１高度計測装置２６は、高さ情報、つまり基準位置からの高さ（鉛直方向の距離）を検出する。リーダー機１２は、水中航行体である。このため、第１高度計測装置２６は、水面からの深さ（深度）を計測する深度計である。

位置情報検出装置２８は、地球上での絶対座標を検出する。位置情報検出装置２８としては、全球測位衛星システム（ＧＮＳＳ：Global Navigation Satellite System)で、衛星から出力される信号を受信して位置座標を検出する装置を用いることができる。具体的構成例として、グローバル・ポジショニング・システム（ＧＰＳ：Global Positioning System）等の測位システムを利用して位置を検出するための測位装置が挙げられる。

第１制御装置３０は、リーダー機１２の各部の動作を制御する、第１制御装置３０は、ＣＰＵ（Central Processing Unit）等の演算回路を含み、移動制御に関する各種の処理を行う演算部と、各種データを記憶する記憶部とを備える。第１制御装置３０は、記憶部に移動制御プログラム４０と、移動経路データ４２と、フォロワー機データ４４と、が記憶される。移動制御プログラム４０は、リーダー機１２の移動を制御するプログラムである。移動経路データ４２は、リーダー機１２に対して設定された移動経路である。移動経路データ４２は、位置情報検出装置２８で検出する座標と、第１高度計測装置２６で計測される深度とを組み合わせた３次元の移動経路のデータである。フォロワー機データ４４は、フォロワー機１４から送られた情報である。フォロワー機１４から出力された信号の受信タイミング等を記憶する。

図４は、フォロワー機の主要構成を示すブロック図である。フォロワー機１４は、筐体５０と、第２慣性航行装置５１と、ソナー５２と、短パルス送信装置（出力装置）５４と、第１高度計測装置２６と、位置情報検出装置２８と、第２制御装置３０と、を有する。フォロワー機１４は、上記構成に加え、水中航行体として備える各種構成を備えている。筐体５０は、フォロワー機１４の各部を収納する外殻である。

第２慣性航行装置５１は、駆動力を発生させ、筐体５０を推進し、姿勢を変化させる機器である。第２慣性航行装置５１は、上下ラダー７２と、左右エレベータ７４と、メインスラスタ７６と、を有する。上下ラダー７２は、筐体５０の上下方向にそれぞれ突出する舵板を有し、舵板を上下方向に延びる軸を軸として回転させる。上下ラダー７２は、軸に沿って舵板の向きを変えることで、筐体５０に上下方向の周りに回転する方向の力を作用させる。左右エレベータ７４は、筐体５０の左右方向にそれぞれ突出する舵板を有し、舵板を左右方向に延びる軸を軸として回転させる。左右エレベータ７４は、軸に沿って舵板の向きを変えることで、筐体５０に左右方向の周りに回転する方向の力を作用させる。つまり、左右エレベータ７４は、筐体５０を浮上させる方向、沈降させる方向に回転させる。メインスラスタ７６は、プロペラ、ジェット推進機であり、筐体５０に推進方向に推進させる力を作用させる。第２慣性航行装置５１は、メインスラスタ７６で推進させつつ、上下ラダー７２及び左右エレベータ７４の向きを制御することで、筐体５０の姿勢を変化させることができる。

ソナー５２は、音波発生装置２２から出力された音響信号を受信する。ソナー５２は、アレイ素子９０と、信号処理部９２と、を有する。アレイ素子９０は、音響信号を受信する素子が列状に配置されている。信号処理部９２は、アレイ素子９０で検出した音響信号を処理し、リーダー機１２から出力された音響信号の水平方向及び垂直方向の到来角を検出する。信号処理部９２は、アレイ素子９０で取得した信号を、ビームフォーマー法やMUSIC(MUltiple SIgnal Classification)など、既存の到来角計測技術を用いて、処理して、到来角を検出する。短パルス送信装置５４は、フォロワー機１４を示す識別情報のみを変調して短パルスで送波する。

第２高度計測装置５６は、高さ情報、つまり基準位置からの高さ（鉛直方向の距離）を検出する。フォロワー機１４は、水中航行体である。このため、第２高度計測装置５６は、水面からの深さを計測する深度計である。

第２制御装置６０は、フォロワー機１４の各部の動作を制御する、第２制御装置６０は、ＣＰＵ（Central Processing Unit）等の演算回路を含み、移動制御に関する各種の処理を行う演算部と、各種データを記憶する記憶部とを備える。第２制御装置６０は、記憶部に移動制御プログラム８０と、基準相対位置データ８２と、が記憶される。移動制御プログラム８０は、フォロワー機１４の移動を制御するプログラムである。基準相対位置データ８２は、リーダー機１２に対する水平方向及び垂直方向の角度、水面に対する深度、リーダー機１２との深さ方向の距離つまり深度差の情報を含む。

次に図５及び図６を用いて、ビークルユニット１０の各部の動作、つまり、リーダー機１２の動作とフォロワー機１４の動作を説明する。図５は、リーダー機の移動制御方法の一例を示すフローチャートである。リーダー機１２は、第１制御装置３０で各部の動作を制御することで、図５に示す処理を実行する。また、図５は、処理の一例であり、図５に示す各処理の一部を並列で実行してもよいし、別々の処理として実行してもよい。

リーダー機１２は、位置情報取得タイミングであるかを判定する（ステップＳ１２）。位置情報取得タイミングは、予め設定した時間間隔ごととしても、リーダー機１２の移動の状態を検出し、所定の条件が成立した場合としてもよい。リーダー機１２は、位置情報取得タイミングではない（ステップＳ１２でＮｏ）と判定した場合、ステップＳ２０に進む。リーダー機１２は、位置情報取得タイミングである（ステップＳ１２でＹｅｓ）と判定した場合、水面に浮上し（ステップＳ１４）、位置情報検出装置２８で位置情報を取得し（ステップＳ１６）、水中に沈降する（ステップＳ１８）。リーダー機１２は、水面に浮上することで、位置情報検出装置２８により位置情報の取得が可能な状態となり、位置情報取得後、沈降することで、移動経度データ４２で設定されている深度に移動する。

リーダー機１２は、ステップＳ１２でＮｏと判定した場合、または、位置情報を取得した後、沈降した場合、高さ情報を取得する（ステップＳ２０）。リーダー機１２は、高さ情報を取得したら、高さ情報と位置情報と移動経路の情報に基づいて、移動を行う（ステップＳ２２）。リーダー機１２は、高さ情報と位置情報と、予め設定されている移動経路と、を比較し、移動経路に沿ってリーダー機１２を移動するように、第１慣性航行装置２１の制御値を決定し、第１慣性航行装置２１を駆動する。なお、位置情報は、位置情報検出装置２８で検出した位置情報を基準とし、検出した位置から移動した情報、つまり第１慣性航行装置２１で航行した情報の蓄積値を加えて、現在の位置を算出してもよい。

リーダー機１２は、ステップＳ２２で移動を行ったら、音波出力タイミングであるかを判定する（ステップＳ２４）。音波出力タイミングは、予め設定した時間間隔ごととしても、リーダー機１２の移動の状態を検出し、所定の条件が成立した場合としてもよい。リーダー機１２は、音波出力タイミングではない（ステップＳ２４でＮｏ）と判定した場合、ステップＳ２０に進む。リーダー機１２は、音波出力タイミングである（ステップＳ１２でＹｅｓ）と判定した場合、音波発生装置２２で自身の位置情報を含む音響信号を出力する（ステップＳ２６）。

リーダー機１２は、ステップＳ２４でＮｏと判定した場合、または、音波を出力した場合、信号を受信したかを判定する（ステップＳ２８）。つまり、リーダー機１２は、短パルス受信装置２４で、短パルス信号を受信したかを判定する。リーダー機１２は、短パルス受信装置２４で短パルス信号を受信した（ステップＳ２８でＹｅｓ）と判定した場合、受信した情報をフォロワー機データ４４に記憶し（ステップＳ３０）、本処理を終了する。リーダー機１２は、短パルス受信装置２４で短パルス信号を受信していない（ステップＳ２８でＮｏ）と判定した場合、本処理を終了する。

リーダー機１２は、位置情報検出装置２８で位置情報を定期的に検出し、かつ、第１慣性航法装置２１を用いて、自己位置を推定しながら、目的位置まで移動する。位置情報検出装置２８で位置情報を定期的に検出することで、第１慣性航法装置２１を用いた移動量の推定で生じる位置推定誤差の蓄積を抑制することができ、高い精度での移動が可能となる。これにより、位置情報検出装置２８を用いた検出が困難な水中での移動でも高い精度での移動が可能となる。また、高度の情報は、第１高度計測装置２６で取得することで、高さ方向の位置のずれの蓄積を抑制することができる。また、リーダー機１２から音響信号を出力することで、フォロワー機１４がリーダー機との相対位置が検出できるようにする。また、リーダー機１２は、フォロワー機１４からの短パルスを受信することで、フォロワー機１４が追従して移動している情報を取得することができる。また、リーダー機１２は、移動経路データ４２を上位の管制システムとの通信で更新、変更するようにしてもよい。

図６は、フォロワー機の移動制御方法の一例を示すフローチャートである。次に、フォロワー機１４の動作を説明する。フォロワー機１４は、音波受信タイミングであるかを判定する（ステップＳ５０）。音波受信タイミングは、移動制御プログラム８０で設定されており、所定の時間間隔としてもよく、ソナー５２をＯＮにしておき、音波を受信した場合、音波受信タイミングであると判定してもよい。フォロワー機１４は、音波受信タイミングではない（ステップＳ５０でＮｏ）と判定した場合、本処理を終了する。

フォロワー機１４は、音波受信タイミングである（ステップＳ５０でＹｅｓ）と判定した場合、ソナー５２で音波を受信し、音響信号を出力したリーダー機１２との相対位置を算出する処理を実行する（ステップＳ５２）。フォロワー機１４は、リーダー機１２との相対位置を算出する処理を実行したら、相対位置情報の取得が完了したかを判定する（ステップＳ５４）。つまり、フォロワー機１４は、ステップＳ５２でリーダー機１２との相対位置が検出できたかを判定する。フォロワー機１４は、相対位置情報の取得が完了していない（ステップＳ５４でＮｏ）と判定した場合、本処理を終了する。

フォロワー機１４は、相対位置情報の取得が完了した（ステップＳ５４でＹｅｓ）と判定した場合、短パルス信号を出力する（ステップＳ５６）。フォロワー機１４は、短パルス信号で、相対位置情報が取得できていることをリーダー機に短パルス信号で伝達する。次に、フォロワー機１４は、第２高度計測装置５６で高さ情報を取得する（ステップＳ５８）。次にフォロワー機１４は、高さ方向の位置（深度情報、高度情報）と相対位置情報とに基づいて、第２慣性航行装置５１を制御し、移動を行い（ステップＳ６０）、本処理終了する。つまり、フォロワー機１４は、高さ情報、相対位置情報に基づいて移動を制御することで、基準相対位置データ８２で設定した所定の高度であり、かつ、リーダー機１２に対して所定の位置となる位置に移動させることができる。

フォロワー機１４は、図６の処理を実行することで、予め設定されたフォーメーションとなるようにそれぞれ位置取りしながら、リーダー機１２に追従して移動することができる。フォロワー機１４は、リーダー機１２との相対位置と、高さ情報を取得し、相対位置と高さ情報と、設定された基準相対位置となるように、移動することで、リーダー機１２と所定のフォーメーションを維持して移動することができる。フォロワー機１４は、リーダー機１２から送信される音響信号をソナー５２で受信し，アレイ信号処理により，リーダー機１２とフォロワー機１４の相対角を計測し、予め設定されたフォーメーションの相対角指令値（所望方位）に合致するように、フィードバック制御で移動を制御する。これにより、フォロワー機１４は、ＧＮＳＳ等を用いた位置情報を取得することなく、フォーメーション制御を行うことができる。

以上より、ビークルユニット１０は、フォロワー機１４がリーダー機１２から出力される音響信号を受信して算出した相対位置情報と、高さ情報とに基づいて、位置を特定し、移動を制御する。これにより、フォロワー機１４でＧＮＳＳ等を使用した絶対座標を取得せずに、移動を制御することができ、ビークルユニット１０のフォーメーション制御を行うことができる。

また、リーダー機１２が自機の位置を音響信号で通知することで、フォロワー機１４はリーダー機１２に対する位置を制御できるため、ビークル毎に個別の情報を送信する必要がなく、リーダー機１２がフォロワー機１４の位置を推定する必要もないため、リーダー機１２からの音響信号が届く範囲に任意の数のフォロワー機１４を配置することができる。つまり、通信機器で追従させるフォロワー機の数が制限されることを抑制できる。

また、ビークルユニット１０は、フォロワー機１４に音響信号を検出する装置と高さ方向の位置を検出する装置を備えていればよく、自機の移動を高度に制御し、位置を推定する慣性航法装置が不要となる。これにより、装置構成を簡単にすることができ、制御を簡単にすることができる。

また、ビークルユニット１０は、フォロワー機１４がＧＮＳＳ等で位置情報を取得する必要がないため、リーダー機１２のみが所定の間隔で浮上すればよい。これにより、浮上、沈降で生じるエネルギーロスを少なくすることができる。また、水上を航行する航行体への影響の発生を抑制することができる。

また、ビークルユニット１０は、リーダー機１２が位置情報を取得するための浮上、沈降（潜航）動作を行っている間は、リーダー機１２とフォロワー機１４との間で相対位置の検出を行わないようにしてもよい。また、フォロワー機１４は、深度位置を維持する設定とすることで、リーダー機１２に追従して浮上することを抑制することもできる。このように、フォロワー機１４は、自機で検出した高さ情報で高さ方向の位置を制御し、リーダー機１２に追従して浮上、沈降しない制御とすることで、エネルギー効率を向上させることができ効率よく移動することができる。

ここで、ビークルユニット１０は、リーダー機１２とフォロワー機１４とが高さ方向に差がある位置を基準相対位置とすることが好ましい。つまり、フォロワー機１４に設定する深さ位置をリーダー機１２と異なる深さ位置とすることが好ましい。これにより、音響信号で検出した相対位置と高さ情報で、リーダー機１２に対するフォロワー機１４の位置を１点に特定することができる。

また、フォロワー機１４からリーダー機１２に短パルス信号を用いて定期的に信号を送信することで、リーダー機１２にフォロワー機１４が追従できていることを通知得ることができる。また、仮に、フォロワー機１４がリーダー機１２に追従できなくなった場合も、リーダー機１２でどの位置までフォロワー機１４追従してしたかを確認することができる。

次に、図７を用いて、フォロワー機の移動制御のアルゴリズム、つまり、ステップＳ６０で実行する移動処理について説明する。図７は、フォロワー機の移動制御のアルゴリズムの一例を示すフローチャートである。なお、リーダー機１２のステップＳ２２の処理も同様の処理で実行することができる。フォロワー機１４は、リーダー機１２との相対位置と、リーダー機１２の推進方向と、深さ方向（鉛直方向）を基準として、３軸と、３軸周りの回転方向が特定される。

フォロワー機１４は、ピッチ・ロール角偏差を計算し（ステップＳ１３２）、ピッチ・ロール角の偏差が大きいかを判定する（ステップＳ１３４）。フォロワー機１４は、ピッチ・ロール角の偏差が大きい（ステップＳ１３４でＹｅｓ）と判定した場合、左右エレベータ７４、上下ラダー７２を駆動して、姿勢を制御する（ステップＳ１３６）。

フォロワー機１４は、ピッチ・ロール角の偏差が小さい（ステップＳ１３４でＮｏ）と判定した場合、高度偏差を計算し（ステップＳ１３８）、高度偏差が大きいかを判定する（ステップＳ１４０）。高度偏差は、設定されている深度と検出した深度との差であり、閾値を基準として判定すればよい。フォロワー機１４は、高度偏差が大きい（ステップＳ１４０でＹｅｓ）と判定した場合、左右エレベータ７４を駆動する（ステップＳ１４２）。フォロワー機１４は、左右エレベータ７４を駆動することで、上昇または下降し、高度を調整できる。

フォロワー機１４は、高度偏差が小さい（ステップＳ１４０でＮｏ）と判定した場合、垂直方位偏差を計算する（ステップＳ１４４）。垂直方向偏差は、フォロワー機１４の推進方向と鉛直方向とを通る平面におけるリーダー機１２とフォロワー機１４の、基準相対位置と検出した相対位置の偏差である。

フォロワー機１４は、垂直偏差が大きい（ステップＳ１４６でＹｅｓ）と判定した場合、メインスラスタ７６を駆動する（ステップＳ１４８）。フォロワー機１４は、メインスラスタ７６を駆動することで、リーダー機１２との距離を調整することができる。なお、フォロワー機１４は、すでにメインスラスタ７６で所定の推進力が生じている場合、検出結果に基づいて、メインスラスタ７６の出力を調整すればよい。

フォロワー機１４は、垂直偏差が小さい（ステップＳ１４８でＮｏ）と判定した場合、水平方向変位を計算し（ステップＳ１５０）、上下ラダー７２を駆動し、水平方向の位置を補正する（ステップＳ１５２）。

フォロワー機１４は、姿勢制御、高さ制御、垂直偏差、水平偏差の順で制御を行うことで、効率よく制御を行うことができる。また、高さ制御を、垂直偏差、水平偏差よりも優先して制御することで、フォロワー機１４とリーダー機１２との相対距離を所定範囲に維持しやすくすることができる。また、姿勢制御を他の制御よりも優先することで、姿勢のずれにより、他の制御が計算からずれることを抑制できる。

ここで、フォロワー機１４は、移動制御は、モデル予測制御を用いることが好ましい。移動制御は、図７に示すように、垂直面（推進方向と鉛直方向で形成される面）の位置制御と、水平面の位置制御を別の制御として、順番に実行する。

以下、垂直面図８は、フォロワー機の推進方向における位置の制御を説明するための説明図である。図９は、フォロワー機の推進方向における位置の制御を説明するための説明図である。図８は、モデル予測制御で、現在の位置Ｐ１と、将来の予測位置Ｐ２との関係を示している。図９は、フォロワー機１４の計測値と、相対角の関係を示している。

モデル予測制御は、逐次制約付き最適化問題を解くことにより、次の時刻の最適な制御入力ｕ_ｍ（ｍ＝１，２、・・・、Ｍ）を求める。ここで、Ｍは、フォロワー機１４の数である。本実施例では，一般化した制御入力として前後・左右・上下方向およびロール・ピッチ・ヨーの速度もしくは角速度指令値を想定し，それぞれｕ_ｍ、ｖ_ｍ、ｗ_ｍ、ｐ_ｍ、ｑ_ｍとする。これらの制御入力をまとめると、下記（式１）となる。

ここで、これらの速度・角速度指令値は、現実の制御入力（メインスラスタの回転速度やエレベータ・ラダーの舵角指令など）に換算する必要がある。フォロワー機１４ｍの６次元の位置姿勢は、下記式（２）とする。

垂直面の制御を行う場合、ｖ_ｍ、ｐ_ｍ、ｒ_ｍが０となる。また、ロール角φ＝０とすることができる。

次に，評価関数として、下記（式３）を導入する。

ここで，Ａ_ｍ，Ｖ（ｋ＋ｍ）とＡ^ｒｅｆ _ｍ，Ｖは、それぞれリーダー・フォロワーの垂直面相対角（ワールド座標系）およびその目標値を表し、ｋは現在時刻を表す．また，ｚ^ｒｅｆ _ｍ，Ｖは深度の目標値を表し，Ｎは、予測ホライズ（先読みするステップの数）である。係数Ｗ_ｖと係数Ｗ_ｚは、それぞれ垂直面相対角と深度に対する重みを表す。（式３）の評価関数は、角度と位置という次元が異なる値を加算しているため、これらの係数は次元や絶対値の違いを考慮して決定する必要がある．
（式３）の評価関数は，制御入力ｕの関数となる。そこで未来の予測値Ａ_ｍ，Ｖ（ｋ＋ｎ）およびＺｍ（ｋ＋ｎ）を、制御入力ｕを用いて表現する。垂直面に運動を限定すると、水中ビークルの運動モデルは、下記（式４）の行列式であらわすことができる。（式４）の左辺は、変化率を表す。

ヨー角ψ_ｍは変化しない前提であるが、初期値が０以外の場合もあるため、（式４）で０とならない。制御入力は、６次元だが垂直面での運動のみを考慮する場合には、上式の通り三つの制御入力のみを最適化する。ｖ_ｍ、ｐ_ｍ、ｒ_ｍはいずれも０とする。

前後方向（推進方向）の移動による垂直面相対角の変化は、図８に示す位置関係から下記（式５）となる。ここで、短時間であり、リーダー機１２の相対的な移動量を微小と仮定としている。

ここで、図９に示す通り、Ａ_ｍ、Ｖ（ｋ）＝α_ｍ（ｋ）－θ_ｍ（ｋ）であり、α_ｍ（ｋ）、はフォロワー機１４ｍのソナーによるリーダー機１２の方位観測値である。また、Ｄ_ｍは、リーダー機１４とフォロワー機１４ｍの深度差を表し、Ｔ_ｃは処理周期を表す。さらに、前後方向の変化量は、下記（式６）となる。

ここで、ヨー角は変化しないため常にψ_ｍ（ｋ）となるため、評価関数の第一項をセンサ観測値と制御入力のみで表現することができる。第二項の深度評価も同様であり、運動モデルの（式４）の二つ目の式を用いることで、下記（式７）で表すことができる。

現在時刻の高さ情報Ｚ_ｍ（ｋ）は、深度計で計測できる値でありｕ_ｍとｗ_ｍは制御入力である。

ここで、処理の制度を高くする、制御を簡単にするために、下記式（８）のような制約条件を設定することが好ましい。ピッチ角上下限の制約を設けることで、ピッチ角を概ね０付近に制限することができる。

フォロワー機１４は、制約付き最適化問題を逐次二次計画法などの公知技術を用いて処理周期毎に解き、その最適解となる下記（式９）を次の制御入力とすることで、制御を実行することができる。

次に、水平面の制御をモデル予測制御で行う一例について説明する。図１０は、フォロワー機の水平方向における姿勢の制御を説明するための説明図である。図１１は、フォロワー機の水平方向における姿勢の制御を説明するための説明図である。水平面の位置制御を実行する場合、垂直面での制御は行わないことで、効率よく制御を行うことができる。水平面の制御を行う場合、ｗ_ｍ、ｐ_ｍ、ｑ_ｍのそれぞれが０となる。また、φ_ｍ、θ_ｍも０とすることができる。

図１０に示すように、水平面の群制御では、深さ変位を用いることができず音響信号で検出した相対位置のみでの制御となるため、リーダー機１２とフォロワー機４の相対距離を制御することはできない。ビークルユニット１０は、図１１に示すように、水平面で群制御を行う際に、フォロワー機１４は、リーダーに対する相対角指令値のみを制御する。つまり、図１１に示すように，フォロワー機１４の水平面相対角を指令値と一致させる制御を行う。フォロワー機１４が、リーダー機１２とフォロワー機１４を結んだ直線状のどの位置に存在しているかは制御しない。なお、上述したように、リーダー機１２とフォロワー機１４との距離は、垂直面の制御で所定の距離に移動させることができる。

フォロワー機１４は、ヨーレート指令値を下記式で表すことで、針路をｘ軸に合わせることができる。つまり、フォロワー機１４を位置Ｐ３から位置Ｐ４の位置に移動させることができる。ここで，Ｋは正の制御ゲインである。

針路が ｘ軸と一致しているとき、水平方位の観測値β_ｍ（ｋ）は、リーダー機１２とフォロワー機１４の水平面相対角Ａ_ｍ，Ｈ（ｋ）に一致する。

以上より、フォロワー機１４は、

となるまで、

を入力してフォロワー機１４にｘ軸を直進させる。Ｖ^ｒｅｆ _ｍは速度指令値である。これにより、フォロワー機１４は、位置Ｐ_５に移動する。

（式１１）、（式１２）の関係を満足することで、所望フォーメーションを達成できる。次に、フォーメーションを維持することができるように、フォロワー１４の向きを制御するために、（式１１）、（式１２）の関係を満足したのち、下記（式１３）を満たす制御を実行する。

フォロワー機１４は、（式１３）の制御を実行し、相対角の変位が大きくなった場合、（式１０）から（式１２）の処理を実行する。

図１２は、ビーグルユニットの移動の一例を示す模式図である。図１２は、移動軌跡１０２が、リーダー機１２の軌跡であり、移動軌跡１０４、１０６が、フォロワー機１４の軌跡である。以上のように、モデル予測制御で水平面の位置制御を行うとで、各時間（ｔ０、ｔ１、ｔ２）で、リーダー機１２と、フォロワー機１４の相対位置が維持できており、移動軌跡１０２、１０４、１０６が同様の軌跡となり、フォロワー機１４がリーダー機１２を追従できていることがわかる。このように、モデル予測制御を用いることで、効率よく制御を行うことができる。

次に、図１３及び図１４を用いて、ビークルユニットの好ましい通信方法について説明する。図１３は、ビークルユニットで送受信される音波の一例を示す模式図である。図１４は、ビークルユニットで送受信される音波の一例を示す模式図である。なお、図１３、１４では、垂直面について記載しているが，水平面についても同様の処理を行う。

図１３に示すリーダー機１２は、音波発生装置２２から出力する音波１１０、１１２をそれぞれのフォロワー機１４に向けてビームフォーミングした状態で出力する。つまり、範囲を絞って出力する。リーダー機１２は、音波発生装置２２のセンサアレイを用いることで、ビームフォーミングを行うことができる。リーダー機１２は、設定されている相対角指令値Ａ^ｒｅｆ _ｍｖに対応した方位へ短パルス音を送信する。なお、音波１１０、１１２は、相対角指令値Ａ^ｒｅｆ _ｍｖを含む所定角度範囲に出力する。これにより、相対的な位置が基準位置からずれている場合でもフォロワー機１４で音波を受信することができる。また、リーダー機１２は、音波１１０、１１２の出力方向を走査して所定角度範囲に出力してもよい。リーダー機１２は、フォロワー機は到来した短パルス音の方位を短パルス受信装置２４のセンサアレイを用いて推定することで，フォロワー機１４の方位を特定し、その方向に音響信号を出力してもよい。センサアレイを用いた方位推定処理には，ビームフォーマー法やＭＵＳＩＣ（MUltiple SIgnal Classification）など公知技術を用いる。

次に、図１４に示すように、フォロワー機１４_１、１４_２は、短パルス送波装置５４から出力する音波１１４、１１６をリーダ機１２に向けてビームフォーミングした状態で出力する。つまり、範囲を絞って出力する。フォロワー機１４もビームフォーミングした音波１１４、１１６を所定角度範囲に出力してもよいし、操作させてもよい。

このように、ビームフォーミングすることで、必要な方向に向けて音波を出力することができ、周囲の装置に影響を与える恐れを低減することができ、ロバスト性を高くすることができる。

実施形態及び変形例は、例として提示したものであり、発明の範囲を限定することは意図していない。実施形態及び変形例は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。実施形態及び変形例は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

１０ ビークルユニット
１２ リーダー機
１４ フォロワー機
２０、５０ 筐体
２１ 第１慣性航行装置
２２ 音波発生装置
２４ 短パルス受信装置
２６ 第１高度計測装置
２８ 位置情報検出装置
３０ 第１制御装置
３２ 上下ラダー
３４ 左右エレベータ
３６ メインスラスタ
４０ 移動制御プログラム
４２ 移動経路データ
４４ フォロワー機データ
５１ 第２慣性航行装置
５２ ソナー（受信装置）
５４ 短パルス送波装置
５６ 第２高度計測装置
６０ 第２制御装置
７２ 上下ラダー
７４ 左右エレベータ
７６ メインスラスタ
８０ 移動制御プログラム
８２ 基準相対位置データ
９０ アレイ素子
９２ 信号処理部

Claims (12)

1. 第１筐体と、前記第１筐体の３次元方向の移動を制御する第１慣性航法装置と、音波発生装置と、高さ方向の位置を検出する第１高度計測部と、位置情報検出装置と、移動経路の情報を記憶する第１制御装置と、を含み水中を移動するリーダー機と、
   第２筐体と、前記第２筐体の３次元方向の移動を制御する第２慣性航法装置と、前記音波発生装置から出力される音波を受信する受信装置と、高さ方向の位置を検出する第２高度計測部と、前記リーダー機との３次元の相対位置の情報を記憶する第２制御装置と、を含み、水中を移動する少なくとも１つのフォロワー機と、
   を備え、
   前記第１制御装置は、前記音波発生装置から音波を出力させ、
   前記第２制御装置は、前記受信装置で受信した音波に基づいて前記リーダー機との相対的な方位を算出し、前記第２高度計測部で検出した高さ方向の位置及び前記リーダー機との相対的な方位に基づいて、前記第２慣性航法装置を制御して前記フォロワー機を移動させるビークルユニット。
2. 前記リーダー機は、所定の時間間隔で水面に浮上し、前記位置情報検出装置で位置情報を検出し、
   前記第１制御装置は、前記移動経路、前記位置情報検出装置で検出した位置情報及び前記第１高度計測部で検出した高さ方向の位置に基づいて前記第１慣性航法装置を制御して前記リーダー機を移動させる請求項１に記載のビークルユニット。
3. 前記第２制御装置は、高さ方向の位置が基準を満たさないと判定した場合、高さ方向の位置を補正し、
   高さ方向の位置が基準を満たし、かつ、水平面上における推進方向と直交する方向まわりの前記リーダー機に対する角度が基準を満たさないと判定した場合、水平方向の前記リーダー機に対する位置を補正する請求項１または請求項２に記載のビークルユニット。
4. 前記第２制御装置は、水平方面上における推進方向と直交する方向まわりの前記リーダー機に対する角度が基準を満たさないと判定した場合、推進方向の位置を補正し、その後、水平方向に平行で、推進方向に直交する方向の位置を補正する請求項３に記載のビークルユニット。
5. 前記フォロワー機は、識別情報を含む信号を出力する出力装置を含み、
   前記リーダー機は、前記出力装置から送信される信号を受信する受信装置を含み、
   前記第１制御装置は、前記受信装置で受信した信号に基づいて、前記フォロワー機の状態の情報を取得する請求項１から請求項４のいずれか一項に記載のビークルユニット。
6. 前記出力装置は、前記リーダー機の位置に基づいて、ビームフォーミングした信号を出力する請求項５に記載のビークルユニット。
7. 前記音波発生装置は、前記フォロワー機の位置に基づいて、ビームフォーミングした信号を出力する請求項１から請求項６のいずれか一項に記載のビークルユニット。
8. リーダー機と複数のフォロワー機とを含み、水中を３次元方向に移動可能な複数のビークルを移動させる複数のビークルの移動制御方法であって、
   前記リーダー機から音波を出力するステップと、
   前記フォロワー機が音波を受信し、前記リーダー機との相対的な方位を取得するステップと、
   前記フォロワー機が高さ方向の位置を検出するステップと、
   前記フォロワー機は、検出した高さ方向の位置及び前記リーダー機との相対的な方位に基づいて移動させるステップと、を含むビークルユニットの移動制御方法。
9. 前記リーダー機が所定の時間間隔で水面に浮上し、位置情報を検出するステップと、
   前記リーダー機が、予め設定された移動経路、検出した位置情報及び高さ情報に基づいて移動するステップをさらに含む請求項８に記載のビークルユニットの移動制御方法。
10. 前記フォロワー機は、高さ方向の位置が基準を満たさないと判定した場合、高さ方向の位置を補正し、高さ方向の位置が基準を満たし、かつ、水平面上における推進方向と直交する方向まわりの前記リーダー機に対する角度の位置が基準を満たさないと判定した場合、水平方向の前記リーダー機に対する位置を補正する請求項８または請求項９に記載のビークルユニットの移動制御方法。
11. 前記フォロワー機は、水平方面上における推進方向と直交する方向まわりの前記リーダー機に対する角度が基準を満たさないと判定した場合、推進方向の位置を補正し、その後、水平方向に平行で、推進方向に直交する方向の位置を補正する請求項１０に記載のビークルユニットの移動制御方法。
12. 前記フォロワー機が、識別情報を含む信号を出力するステップと、
    前記リーダー機が、前記フォロワー機から送信される信号を受信し、受信した信号に基づいて、前記フォロワー機の状態の情報を取得するステップと、を含む請求項８から請求項１１のいずれか一項に記載のビークルユニットの移動制御方法。
    

Images