(57)【要約】【課題】  操縦装置の指示を受けて移動する水中移動体の位置の情報を基に、水中移動体の方位角を推定する。【解決手段】  方位角推定システムは、水中移動体２と、その操縦装置３と、計測装置４と、方位角推定装置５を備える。方位角推定装置５は、計測装置４で計測されたデータを基に推定された水中移動体２の位置の情報を受け取り、その情報を基に、方位角推定処理により移動を開始する水中移動体２の移動開始時の位置の情報と、設定されたステップごとに水中移動体２が移動した位置の情報とを取得し、水中移動体２の移動軌跡の基準方向からの傾きと、分散とを、逐次最小二乗法により更新し、移動軌跡の傾きの基準方向からの角度と、水中移動体２が移動した位置の履歴とを基に、水中移動体２の方位角を推定する処理を行う。【選択図】図１
2019207180.tif 000002
【特許請求の範囲】
【請求項１】
水中移動体と、
前記水中移動体の操縦装置と、
前記水中移動体の位置の検出に用いるデータを計測する計測装置と、
方位角推定装置と、を備え、
前記方位角推定装置は、
前記操縦装置より、該操縦装置で指示された前記水中移動体の移動方向に関する情報を受け取る機能と、
前記計測装置で計測されたデータを基に推定された前記水中移動体の位置の情報を受け取る機能と、
前記情報を基に、方位角推定処理に伴い移動を開始する前記水中移動体の移動開始時の位置の情報を取得する機能と、
設定されたステップごとに前記水中移動体が移動した位置の情報を取得する機能と、
前記水中移動体の移動軌跡の基準となる方向からの傾きと、分散とを、逐次最小二乗法により更新する更新処理を行う機能と、
前記移動軌跡の基準となる方向に対する傾きの角度を演算する演算処理を行う機能と、
前記水中移動体が移動した位置の履歴と、前記演算処理の結果を基に、前記水中移動体の方位角を推定する処理を行う機能と、を備えたこと
を特徴とする方位角推定システム。
【請求項２】
前記計測装置は、イメージングソーナーとした
請求項１に記載の方位角推定システム。
【請求項３】
前記操縦装置は、前記方位角推定処理により、前記水中移動体に、前進、後進、左移動、右移動のいずれかの方向への移動方向を指示する機能を備えた
請求項１または２に記載の方位角推定システム。
【請求項４】
操縦装置の指示で移動する水中移動体の方位角を推定する方位角推定装置を備え、該方位角推定装置が、方位角推定処理として、
前記操縦装置より前記水中移動体に指示した移動方向に関する情報を受け取る処理と、
前記水中移動体の位置の検出に用いるデータを計測する計測装置で計測されたデータを基に推定された前記水中移動体の位置の情報を受け取る処理と、
前記操縦装置の指示により移動を開始する前記水中移動体の移動開始時の位置の情報を取得する処理と、
設定されたステップごとに前記水中移動体が移動した位置の情報を取得する処理と、
前記水中移動体の移動軌跡の基準となる方向からの傾きと、分散とを、逐次最小二乗法により更新する更新処理と、
前記移動軌跡の前記基準となる方向に対する傾きの角度を演算する演算処理と、
前記水中移動体が移動した位置の履歴と、前記演算処理の結果を基に、前記水中移動体の方位角を推定する処理と、を行うこと
を特徴とする方位角推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、操縦装置により遠隔操作する水中移動体の方位角を推定する方位角推定システムおよび方位角推定方法に関するものである。
【背景技術】
【０００２】
水中ロボットのような水中移動体を移動させる手法の一つとしては、操縦者が操縦装置を用いて水中移動体を遠隔操作する手法がある。
【０００３】
ところで、水中移動体には、前後方向の推力を発生させる推進装置に加えて、左右方向の成分を含む推力を発生させる推進装置を備えた構成として、前後方向と左右方向とを組み合わせた方向へ移動可能な形式のものがある。この種の水中移動体の操縦装置は、操作対象となる水中移動体の機体の前後左右の方向を基準として、前進、後進、左移動、右移動などのように、水中移動体の移動方向を指示する形式のものが、一般的である。
【０００４】
そのため、操縦装置を用いて、水中移動体を或る目標方向（方位）へ移動させる操作を行う場合は、水中移動体の機首の方向である方位角の情報を取得し、方位角と目標方向との角度の差に応じて、操縦装置で水中移動体の移動方向を指示する必要がある。
【０００５】
なお、移動体の方位角を求める手法としては、磁気方位計を用いて方位角を直接検出する手法や、ジャイロセンサから出力される角速度の積分から算出する手法が、一般的に行われている。更に、ＧＰＳ情報を基に推定する手法も、従来提案されている（たとえば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２０１２−９８１８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
ところが、水中ロボットのような水中移動体は、橋梁、港湾、その他の構造物にて、閉所で且つ鉄板により遮蔽された空間で使用されることがあり、このような使用環境では、磁気方位計により方位角を検出する手法は実施が難しい。
【０００８】
また、水中移動体は、水中に没している状態では、電波の受信が難しいことから、ＧＰＳ情報を用いることが難しく、更に、水中移動体が前記のような鉄板により遮蔽された空間で使用される場合は、ＧＰＳ情報を用いることは困難である。
【０００９】
これに対し、前記のような使用環境であっても、ジャイロセンサから出力される角速度の積分により移動体の方位角を求める手法は使用可能である。しかし、この手法は、時間の経過と共に積分誤差が蓄積することの影響により、移動体の方位角に誤差が生じてしまう。
【００１０】
そこで、本発明者は、地磁気およびＧＰＳ情報に依存することなく水中移動体の方位角を推定でき、しかも、時間の経過に伴う積分誤差の蓄積の影響を受けずに水中移動体の方位角を推定できるようにするための工夫、研究を重ねた結果、操縦装置により移動が操作される水中移動体の位置情報を基に、水中移動体の方位角を推定できることを見出して、本開示の発明をなした。
【００１１】
したがって、本発明の目的とするところは、操縦装置より移動方向の指示を受けて移動する水中移動体の位置の情報を基に、水中移動体の方位角を推定できる方位角推定システムおよび方位角推定方法を提供しようとするものである。
【課題を解決するための手段】
【００１２】
本発明は、前記課題を解決するために、水中移動体と、前記水中移動体の操縦装置と、前記水中移動体の位置の検出に用いるデータを計測する計測装置と、方位角推定装置と、を備え、前記方位角推定装置は、前記操縦装置より、該操縦装置で指示された前記水中移動体の移動方向に関する情報を受け取る機能と、前記計測装置で計測されたデータを基に推定された前記水中移動体の位置の情報を受け取る機能と、前記情報を基に、方位角推定処理に伴い移動を開始する前記水中移動体の移動開始時の位置の情報を取得する機能と、設定されたステップごとに前記水中移動体が移動した位置の情報を取得する機能と、前記水中移動体の移動軌跡の基準となる方向からの傾きと、分散とを、逐次最小二乗法により更新する更新処理を行う機能と、前記移動軌跡の基準となる方向に対する傾きの角度を演算する演算処理を行う機能と、前記水中移動体が移動した位置の履歴と、前記演算処理の結果を基に、前記水中移動体の方位角を推定する処理を行う機能と、を備えた構成を有する方位角推定システムとする。
【００１３】
前記計測装置は、イメージングソーナーとした構成としてもよい。
【００１４】
前記操縦装置は、前記方位角推定処理により、前記水中移動体に、前進、後進、左移動、右移動のいずれかの方向への移動方向を指示する機能を備えた構成としてもよい。
【００１５】
操縦装置の指示で移動する水中移動体の方位角を推定する方位角推定装置を備え、該方位角推定装置が、方位角推定処理として、前記操縦装置より前記水中移動体に指示した移動方向に関する情報を受け取る処理と、前記水中移動体の位置の検出に用いるデータを計測する計測装置で計測されたデータを基に推定された前記水中移動体の位置の情報を受け取る処理と、前記操縦装置の指示により移動を開始する前記水中移動体の移動開始時の位置の情報を取得する処理と、設定されたステップごとに前記水中移動体が移動した位置の情報を取得する処理と、前記水中移動体の移動軌跡の基準となる方向からの傾きと、分散とを、逐次最小二乗法により更新する更新処理と、前記移動軌跡の前記基準となる方向に対する傾きの角度を演算する演算処理と、前記水中移動体が移動した位置の履歴と、前記演算処理の結果を基に、前記水中移動体の方位角を推定する処理と、を行う方位角推定方法とする。
【発明の効果】
【００１６】
本発明の方位角推定システムおよび方位角推定方法によれば、操縦装置より移動方向の指示を受けて移動する水中移動体の位置の情報を基に、水中移動体の方位角を推定することができる。
【図面の簡単な説明】
【００１７】
【図１】方位角推定システムの実施形態を示す概要図である。
【図２】方位角推定システムで実施する方位角推定方法を説明するための図である。
【発明を実施するための形態】
【００１８】
以下、本発明を実施するための形態を図面を参照して説明する。
【００１９】
図１は、方位角推定システムの実施形態を示す概要図である。図２は、方位角推定システムで使用する水中移動体の位置情報の概要を示す図である。
【００２０】
本実施形態の方位角推定システムは、図１に符号１で示すもので、水中移動体２と、水中移動体２の操縦装置３と、水中移動体２の位置の検出に用いるデータを計測する計測装置４と、方位角推定装置５とを備えた構成とされている。
【００２１】
水中移動体２の方位角とは、水中移動体２の機首の方向を、或る基準となる方位からの角度で示すものである。本実施形態では、水中移動体２の方位角を示すための基準となる方位を、図２に示すｘｙ直交座標系におけるｘ軸の正の方向（図２では右方向）として説明する。なお、図２に示すｘｙ直交座標系は、水中移動体２の使用領域に設定されるもので、緯度と経度のような地球に固定された座標系であってもよいし、水中移動体２の使用領域の形状や、使用領域となる構造物に応じて設定された座標系であってもよい。
【００２２】
水中移動体２は、図示しない推進装置として、前後方向の成分を含む推力、および、左右方向の成分を含む推力を発生可能な推進装置を備えた構成として、少なくとも、方位角を保持した状態で、前進と後進と左移動と右移動が可能な構成を備えている。
【００２３】
水中移動体２に備える推進装置は、水中移動体２の方位角を保持した状態での前進、後進、左移動、右移動を実現できれば、推進装置の形式、数、水中移動体における配置は、任意に設定してよい。
【００２４】
また、水中移動体２は、機体の向き、すなわち、方位角を変更するために用いる図示しない舵を備えた構成としてもよい。
【００２５】
操縦装置３は、図示しない操縦者の操作に応じて、水中移動体２に移動の指示を与える装置である。操縦装置３は、水中移動体２に対して、少なくとも、方位角を保持した状態での前進、後進、左移動、右移動を指示することが可能な機能を備えている。この機能を備えていれば、操縦装置３は、パッド形式、ジョイスティック形式などのコントローラを備えた構成、ＶＲゴーグルなどの表示装置を備えた構成、その他、任意の機器構成でよいことは勿論である。
【００２６】
なお、以下の説明では、水中移動体２の方位角を保持した状態での前進、後進、左移動、右移動は、説明の便宜上、単に、水中移動体２の前進、後進、左移動、右移動という。
【００２７】
操縦装置３と水中移動体２は、操縦装置３から水中移動体２に前進、後進、左移動、右移動の指示を行うと、水中移動体２が、移動開始位置から、指示された方向に前進、後進、左移動、右移動するように調整されている。この際、水中移動体２の使用領域の環境が、水中移動体２に影響する外乱が存在する環境である場合は、水中移動体２が、自機に受ける外乱の影響を修正しながら、移動開始位置から前進、後進、左移動、右移動するように、操縦装置３と水中移動体２が調整される。
【００２８】
操縦装置３と水中移動体２は、有線接続されていてもよいし、有線接続以外の手段で移動の指示などの情報の授受を行うものであってもよい。
【００２９】
計測装置４は、本実施形態では、アクティブソーナーの一種であるイメージングソーナー（音響カメラとも称す）を用いている。
【００３０】
計測装置４は、水中移動体２の使用領域に設置される。この際、計測装置４は、水中移動体２の使用領域にて、水中移動体２の運用を開始する位置から、水中移動体２の移動の目標位置までが計測範囲に含まれるように、計測装置４の計測範囲の配置と向きが設定されている。これにより、水中移動体２の使用領域に設定されたｘｙ直交座標系における計測装置４の設置位置の座標は明らかとなる。また、計測装置４の仕様を基に、ｘｙ直交座標系における計測装置４の計測範囲も明らかとなる。
【００３１】
計測装置４としてのイメージングソーナーは、計測範囲を俯瞰した画像を得ることができ、その画像中に、計測範囲内に存在してエコーを生じる物体である水中移動体２の像を得ることができる。
【００３２】
そのため、本実施形態における計測装置４は、計測範囲を俯瞰した画像の情報を、水中移動体２の位置の情報を含んだ画像データとして出力する。そこで、本実施形態の方位角推定システム１は、計測装置４より水中移動体２の位置の情報を含んだ画像データを受け取る位置推定装置としての画像処理部６を備えた構成とし、画像処理部６が、計測装置４より受け取る画像データの画像処理を行って、水中移動体２の位置を推定する機能を備えるようにしてある。
【００３３】
計測装置４で得られる画像データにて、エコーを生じる水中移動体２の像は、周囲と異なる輝度値を持っている。ところで、画像データは、計測装置４の計測範囲に存在しているエコーを生じる水中移動体２以外の固定物の像も含んでいる。
【００３４】
そこで、画像処理部６は、計測装置４より或る時間差で受け取る複数の画像データを基に、背景差分の処理を行い、画像全体から、移動物である水中移動体２の像のピクセル情報のみを抽出する。
【００３５】
次いで、画像処理部６は、抽出された水中移動体２の像の輝度重心を求めることで、水中移動体２の位置情報を、ピクセルの位置情報として得る。
【００３６】
前記したように、ｘｙ直交座標系における計測装置４の設置位置および計測範囲は明らかである。よって、その情報は、画像処理部６に予め格納しておく。
【００３７】
これにより、画像処理部６は、水中移動体２の位置を示すピクセルの位置情報を、ｘｙ直交座標系における位置座標の情報へ変換することができ、この変換後のｘｙ直交座標系における位置座標を、水中移動体２の位置の座標として推定する処理を行う。
【００３８】
画像処理部６は、以上の処理により推定された水中移動体２の位置の情報を、方位角推定装置５へ送る機能を備える。
【００３９】
なお、画像処理部６は、画像処理専用の演算装置を備えた構成としてもよいし、方位角推定装置５や、その他の演算機能を有する装置に、機能の一つとして実装されていてもよい。
【００４０】
次に、方位角推定装置５の機能の説明を行うと共に、本実施形態の方位角推定方法について説明する。
【００４１】
方位角推定装置５は、画像処理部６から受け取る水中移動体２の推定された位置の情報を基に、方位角推定処理を行う機能を備えている。
【００４２】
この方位角推定処理を行う場合は、方位角推定装置５は、推定処理の計算に必要とされる以下のパラメータについて、事前に初期化の処理を行う。
【００４３】
たとえば、傾きφＮ−１＝０、分散ＰＮ−１＝１０３、忘却係数ρ＝０．１とする。
【００４４】
なお、傾きφＮ−１の値は、初期値を与えることが目的であるため、後述する各式の数値計算が成立すれば、０以外の任意の値としてもよい。分散ＰＮ−１は、後述する逐次最小二乗法を実施する場合は、なるべく大きめの値を設定しておくことが一般的である。
【００４５】
忘却係数ρは、０から１の範囲で定めればよい。この際、忘却係数ρは、値が１に近くなる程、後述する傾きφＮおよび分散ＰＮの計算にて、ノイズに対しては強くなるが、過去のステップの影響が大きくなる。一方、忘却係数ρの値が０に近くなる程、過去のステップの影響は小さくなるが、ノイズに対しては弱くなる。したがって、忘却係数ρの値は、過去のステップの影響の大きさと、ノイズに対する耐性のバランスを考慮して、適宜設定すればよい。
【００４６】
この状態で、方位角推定処理を開始する場合は、操縦装置３にて、水中移動体２の前進、後進、左移動、右移動のいずれかを指示する処理を行う。この処理に伴い、方位角推定装置５は、操縦装置３で指示された水中移動体２の移動方向に関する情報を、操縦装置３より受け取る機能を備えている。
【００４７】
この際、たとえば、計測装置４の画像データなどからは、水中移動体２の周辺に存在する固定物の情報を得ることができる。そのため、水中移動体２の移動方向としては、水中移動体２を現在位置から移動させても周辺の固定物との干渉が生じない方向を選定するようにすればよい。
【００４８】
移動方向の指示は、操縦装置３を図示しない操作者が手動で操作することで行うようにすればよい。また、操縦装置３に、方位角推定処理の開始スイッチとプログラムとを備えて、開始スイッチが操作されると、操縦装置３が、水中移動体２の前進、後進、左移動、右移動のうち、水中移動体２を現在位置から移動させても周辺の固定物との干渉が生じない方向を選定して、その方向への移動の指示を行うようにしてもよい。なお、開始スイッチは、物理的なスイッチでもよいし、画面上で操作するスイッチでもよい。
【００４９】
これにより、水中移動体２は、現在位置から操縦装置３によって指示された方向への移動を開始する。
【００５０】
方位角推定装置５は、画像処理部６より受け取る情報を基に、移動開始時の水中移動体２の位置を検出して、その位置を座標（ｘ０、ｙ０）とする。
【００５１】
次に、方位角推定装置５は、方位角推定処理の開始後の設定されたステップごとに、計測装置４が出力する画像データを基に画像処理部６で推定された水中移動体２の位置の情報を受け取り、その位置を座標（ｘＮ、ｙＮ）とする。Ｎは、方位角推定処理の開始後のステップ数を示す。なお、ステップの周期は、一定であってもよいし、一定でなくてもよい。
【００５２】
次いで、方位角推定装置５は、次の２つの式を用いて、逐次最小二乗法により移動軌跡の傾きφＮと、分散ＰＮを更新する処理を行う。
【００５３】
【数１】
2019207180.tif 000003
【００５４】
この処理によれば、図２に示すように、水中移動体２が外乱やその他の様々な影響を受けて、実際の水中移動体２の移動軌跡７が蛇行していたとしても、水中移動体２の移動の軌跡を、図２に二点鎖線で示す如き直線８として推定することができる。
【００５５】
傾きφＮと、分散ＰＮが更新されると、方位角推定装置５は、次の式により、傾きφＮについて、ｘ軸の正の方向を基準（０［ｒａｄ］）とし、反時計回り方向を正とする角度θＮ［ｒａｄ］を演算する処理を行う。
【００５６】
【数２】
2019207180.tif 000004
【００５７】
なお、ｔａｎ−１は、角度θＮを、−π／２［ｒａｄ］からπ／２［ｒａｄ］の範囲でしか推定できない。この範囲は、図２のｘｙ直交座標系では、或る位置を基準とする第１象限と第４象限の範囲となる。そのため、推定された角度θＮについては、実際に水中移動体２が移動した、０［ｒａｄ］から２π［ｒａｄ］の方向に変換する必要がある。
【００５８】
そこで、方位角推定装置５は、方位角推定処理の移動開始時の位置の座標（ｘ０，ｙ０）と、推定中の位置の座標（ｘＮ、ｙＮ）とを用いて、水中移動体２が座標（ｘ０，ｙ０）から、いずれの象限へ移動したかを特定し、角度を補正する処理を行う。
【００５９】
更に、方位角推定装置５は、この角度の補正を行う際、方位角推定処理の開始時に操縦装置３により指示した水中移動体２の移動方向が、前進、後進、左移動、右移動のいずれであるかを基に、水中移動体２の方位角ψＮ［ｒａｄ］を算出する処理を行う。
【００６０】
具体的には、水中移動体２が移動した象限の特定は、水中移動体２が移動した位置の履歴を基に、「ｘＮ−ｘ０＞０」という第１条件、および、「ｙＮ−ｙ０＞０」という第２条件を用いて、それぞれの条件が真と偽のいずれであるかに応じて判断する。両条件が共に真の場合は、水中移動体２は第１象限に移動したと特定できる。また、第１条件が真、第２条件が偽の場合は、水中移動体２は第４象限に移動したと特定できる。第１条件が偽、第２条件が真の場合は、水中移動体２は第２象限に移動したと特定できる。両条件が共に偽の場合は、水中移動体２は第３象限に移動したと特定できる。
【００６１】
そこで、方位角推定装置５は、方位角推定処理の開始時に操縦装置３により指示した水中移動体２の移動方向が前進の場合は、次の第１表に従い、水中移動体２の方位角ψＮ［ｒａｄ］を算出する処理を行う。なお、表中のＴは真、Ｆは偽の判断結果を示す（後述する第２表、第３表、第４表も同様）。
【００６２】
【表１】
2019207180.tif 000005
【００６３】
方位角推定装置５は、方位角推定処理の開始時に操縦装置３により指示した水中移動体２の移動方向が、後進の場合は、次の第２表に従い、水中移動体２の方位角ψＮ［ｒａｄ］を算出する処理を行う。
【００６４】
【表２】
2019207180.tif 000006
【００６５】
方位角推定装置５は、方位角推定処理の開始時に操縦装置３により指示した水中移動体２の移動方向が、左移動の場合は、次の第３表に従い、水中移動体２の方位角ψＮ［ｒａｄ］を算出する処理を行う。
【００６６】
【表３】
2019207180.tif 000007
【００６７】
方位角推定装置５は、方位角推定処理の開始時に操縦装置３により指示した水中移動体２の移動方向が、右移動の場合は、次の第４表に従い、水中移動体２の方位角ψＮ［ｒａｄ］を算出する処理を行う。
【００６８】
【表４】
2019207180.tif 000008
【００６９】
なお、前記各表に示した、操縦装置３により指示した水中移動体２の移動方向と、前記第１条件と第２条件の判断結果に応じて水中移動体２の方位角ψＮを算出する式との関連は、方位角推定装置５に格納しておけばよい。
【００７０】
ところで、水中移動体２の移動方向がｙ軸方向に沿う場合は、ｔａｎ−１φＮの計算ができないため、角度θＮ［ｒａｄ］を演算で求めることはできない。しかし、φＮの値が大きくなることからは、角度θＮ［ｒａｄ］がπ／２付近となることが分かる。
【００７１】
そこで、方位角推定装置５は、φＮの値についての閾値が設定される機能と、φＮの値が閾値を超えた場合には、角度θＮ［ｒａｄ］をπ／２と見做す処理を行う機能を備えている。なお、この場合の閾値は、水中移動体２の位置の推定精度や、方位角の推定に要求される精度に応じて、適宜設定される。
【００７２】
更に、方位角推定装置５は、水中移動体２が移動した位置の履歴を基に、（ｙＮ−ｙ０）の計算を行い、計算結果が正の値である場合は、水中移動体２の移動方向がｙ軸の正の方向であると推定し、計算結果が負の値である場合は、水中移動体２の移動方向がｙ軸の負の方向であると推定する機能を備えている。
【００７３】
方位角推定装置５は、このように求めた水中移動体２の移動方向を、前記第１表から第４表における前記第１条件および第２条件が共に真の場合の式におけるθＮに代入することで、水中移動体の方位角ψＮ［ｒａｄ］を算出する。
【００７４】
これにより、方位角推定装置５は、水中移動体２の方位角ψＮを求めることができる。
【００７５】
その後、方位角推定装置５は、傾きについて、φＮ−１＝φＮとし、分散について、ＰＮ−１＝ＰＮとしてから、ステップを１つ進めて、計測装置４が出力した画像データを基に推定された水中移動体２の位置の情報を、画像処理部６から受け取り、その位置を座標（ｘＮ、ｙＮ）とする処理から、水中移動体２の方位角ψＮを算出する処理までを繰り返し実施する。
【００７６】
したがって、本実施形態の方位角推定システム１は、前記した手順による方位角推定方法を実施することにより、方位角推定装置５が算出する結果として、水中移動体２の方位角ψＮを求めることができる。
【００７７】
この際、本実施形態の方位角推定システムおよび方位角推定方法は、操縦装置３より移動方向の指示を受けて移動する水中移動体２の位置の情報を基に、水中移動体２の方位角ψＮを推定することができる。
【００７８】
したがって、本実施形態の方位角推定システムおよび方位角推定方法は、地磁気やＧＰＳに依存しないため、橋梁、港湾、その他の構造物にて、閉所で且つ鉄板により遮蔽された空間で水中移動体２を使用する場合であっても、水中移動体２の方位角ψＮを推定することができる。
【００７９】
また、本実施形態の方位角推定システムおよび方位角推定方法は、水中移動体２の外部に備えた計測装置４で検出する水中移動体２の位置の情報を基に、水中移動体２の方位角ψＮを推定しているため、従来のジャイロセンサから出力される角速度の積分から方位角を算出する手法で生じていたような、時間の経過に伴う積分誤差の蓄積に起因する誤差の影響を受けることはない。
【００８０】
なお、本実施形態の方位角推定システム１は、磁気方位計を用いる方位角の検出装置、ジャイロセンサを用いる方位角の検出装置、ＧＰＳ情報を基にした方位角の検出装置などの他の形式の方位角の検出装置と併用してもよい。このような併用を行うことにより、本実施形態の方位角推定システムおよび方位角推定方法による水中移動体２の方位角ψＮの推定結果の精度の向上化を図る効果が期待できる。
【００８１】
前記実施形態では、方位角推定処理を行うときに、操縦装置３により水中移動体２に指示する移動方向として、前進、後進、左移動、右移動の４方向を示した。これに対し、実施形態の方位角推定システムおよび方位角推定方法の応用例としては、操縦装置３により、水中移動体２に、任意の移動方向を指示するようにしてもよい。
【００８２】
この場合は、操縦装置３により水中移動体２に移動方向を指示した時点で、機首の方向を基準として、指示した移動方向までの角度を、たとえば、反時計回り方向の角度α（＝０〜２π［ｒａｄ］）として検出して、その角度αの情報を記憶する。
【００８３】
次に、前記実施形態と同様に、方位角推定装置５は、角度θＮを推定するまでの処理を行う。次いで、方位角推定装置５は、角度θＮと、水中移動体２が移動した位置の履歴を基に、前記第１条件および第２条件と、水中移動体２に指示した移動方向に関わらず前記第１表に示された式を用いて、水中移動体２の方位角ψＮに代えて、水中移動体２が移動した方位を求める処理を行う。その後、方位角推定装置５は、求めた水中移動体２が移動した方位から、前記角度αを引く処理を行うことで、水中移動体２の方位角ψＮを求める処理を行うようにすればよい。
【００８４】
更に、本実施形態の方位角推定システムおよび方位角推定方法の別の応用例としては、操縦装置３により、水中移動体２に、回転移動が含まれる移動として、或る角速度での移動を指示し、その指示に従って移動する水中移動体２の位置を検出して、その位置の変化から、指示された角速度で移動する水中移動体２の移動方向を求め、求めた移動方向を基に水中移動体２の方位角ψＮを推定することも考えられる。
【００８５】
なお、本開示の方位角推定システムおよび方位角推定方法は、前記各実施形態にのみ限定されるものではない。
【００８６】
図１では、方位角推定システム１における操縦装置３と方位角推定装置５とを別々に記載したが、操縦装置３と方位角推定装置５は、或る装置に機能としてまとめて備えられていてもよいことは勿論である。
【００８７】
水中移動体２の位置の検出に用いるデータを計測する計測装置４は、イメージングソーナーを例示した。これに対し、水中移動体２の位置を検出し、水中移動体２の使用領域に設定される直交座標系にて、水中移動体２の位置の座標を特定することが可能となるデータを計測する機能を備えていれば、計測装置４は、イメージングソーナー以外のアクティブソーナー、あるいは、アクティブソーナー以外の計測装置を採用してもよいことは勿論である。この場合、計測装置４から出力されるデータは、そのデータの形式に応じた位置推定装置で処理を行って、水中移動体２の位置を推定し、その推定された水中移動体２の位置の情報が、方位角推定装置５へ送られるようにすればよい。
【００８８】
実施形態では、方位角推定処理を行うときに、操縦装置３により水中移動体２に指示可能な移動方向として、前進、後進、左移動、右移動の４方向を備える場合について説明した。これに対し、本開示の方位角推定システムおよび方位角推定方法は、水中移動体２の推進装置の形式、水中移動体２の運動性能などを考慮して、方位角推定処理を行うときに、操縦装置３により水中移動体２に指示可能な移動方向として、前進、後進、左移動、右移動の４方向のうち、いずれか一つの方向のみ、あるいは、２または３方向を指示可能なものとしてもよい。この場合、方位角推定装置５は、前記第１表から第４表のうち、指示可能な方向に対応する表のみを格納していればよい。
【００８９】
水中移動体２は、操縦装置３による移動方向の指示に従い、方位角ψＮを保持した状態で、指示された方向に移動可能な機能を備えていれば、水中ロボット以外のいかなる形式の水中移動体２であってもよく、また、使用目的は特に限定されないことは勿論である。
【００９０】
本開示の方位角推定システムおよび方位角推定方法は、更に、方位角を変更することなく、前後方向と左右方向への移動が可能な移動体であれば、所謂ドローンといわれるような空中を飛行する形式の移動体や、地上を移動する移動体の方位角の推定にも応用することが可能である。
【００９１】
その他、本発明の要旨を逸脱しない範囲内で種々変更を加え得ることは勿論である。
【符号の説明】
【００９２】
２  水中移動体、３  操縦装置、４  計測装置、５  方位角推定装置、７  移動軌跡、φＮ  傾き、ＰＮ  分散、θＮ  角度、ψＮ  方位角
【図１】
2019207180.tif 000009
【図２】
2019207180.tif 000010