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