(57)【特許請求の範囲】
【請求項１】
水中航走体のスクリュウのスクリュウ速度(13)及び角速度計(11A)と加速度計(12A)とストラップダウン演算部(14)とからなる慣性航法装置(30)の速度信号(14a)に基づき、水流に含まれる少なくとも潮流の速度成分、前記スクリュウに含まれる少なくともスリップの速度成分及び前記慣性航法装置(30)に含まれる誤差速度成分を推定し、前記水中航走体の位置を検出するようにした水中航走体における位置検出方法において、
前記慣性航法装置(30)からの角速度信号(11)と加速度信号(12)が前記ストラップダウン演算部(14)でストラップダウン演算されて前記速度信号(14a)が得られ、
前記速度信号(14a)は前記ストラップダウン演算部(14)に接続された速度演算部(15)で演算された後に位置演算部(16)に入力され、
前記スクリュウ速度(13)はスクリュウ潮流演算部(17)にて誤差補正された後に前記速度信号(14a)と比較器(18)で比較され、前記比較器(18)で検出された差(18a)がカルマンフィルター(10)に観測値として入力され、
前記カルマンフィルター(10)から演算された速度誤差(10a)は前記速度演算部(15)に、姿勢誤差(10b)は前記ストラップダウン演算部(14)に、ジャイロバイアス(10c)は前記角速度信号(11)に、加速度バイアス(10d)は前記加速度信号(12)に、スクリュウ／潮流誤差(10e)は前記スクリュウ潮流演算部(17)に、位置／方位誤差(10f)は前記位置演算部(16)に各々帰還されて制御され、
前記潮流を前記水中航走体の主軸方向と慣性座標方向に分け、前記水中航走体の主軸方向の潮流は前記スクリュウ速度のスクリュウ速度誤差に含めて推定することを特徴とする水中航走体における位置検出方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、水中航走体における位置検出方法に関し、特に、高精度な慣性航法装置又はドップラーソーナー等を用いることなく、中精度程度の慣性航法装置とスクリュウからなる比較的安価な設備のみを用いて水中航走体の位置を高精度に検出するための新規な改良に関する。
【０００２】
【従来の技術】
従来、用いられていたこの種の水中航走体における位置検出方法としては、一般に２種類の方法が行われてきた。
すなわち、第１従来例としては、水中航走体の姿勢及び方位のみを検出し、特にはその位置精度に言及しない方法であり、例えば、ワイヤーで地上と通信し、カメラ画像を地上で認知することによって検出を行う方法である。
第２従来例としては、この第１従来例よりも位置計測に主眼を置いた方法である。すなわち、高精度な慣性航法装置を用いる潜水艦か、又は、ドップラーソーナーにより対地（海底）速度を検出し、慣性航法装置と共に位置の計測を行う方法である。
【０００３】
【発明が解決しようとする課題】
従来の水中航走体における位置検出方法は、以上のように構成されていたため、次のような課題が存在していた。
すなわち、第１従来例の場合、ワイヤーによって水中と地上とを連結し、カメラ画像の認知で行っているため、位置検出精度は限られたものであると共に、ワイヤーの使用により水中航走体の移動範囲が極めて限られることになっていた。
また、第２従来例の場合、極めて高精度な慣性航法装置を用いると、装置自体が極めて高価でかつ大型となり、小型の水中航走体には適用が不可能であった。
また、ドップラーソーナーによる方法の場合には、ドップラーソーナーの音波の届く範囲の海域での運用を前提としているため、運用制限を受けることが多く、自由に用いることは不可能であった。
【０００４】
本発明は、以上のような課題を解決するためになされたもので、特に、高精度な慣性航法装置又はドップラーソーナー等を用いることなく、中精度程度の慣性航法装置とスクリュウからなる比較的安価な設備のみを用いて水中航走体の位置を高精度に検出するようにした水中航走体における位置検出方法を提供することを目的とする。
【０００５】
【課題を解決するための手段】
本発明による水中航走体における位置検出方法は、水中航走体のスクリュウのスクリュウ速度及び角速度計と加速度計とストラップダウン演算部とからなる慣性航法装置の速度信号に基づき、水流に含まれる少なくとも潮流の速度成分、前記スクリュウに含まれる少なくともスリップの速度成分及び前記慣性航法装置に含まれる誤差速度成分を推定し、前記水中航走体の位置を検出するようにした水中航走体における位置検出方法において、前記慣性航法装置からの角速度信号と加速度信号が前記ストラップダウン演算部でストラップダウン演算されて前記速度信号が得られ、前記速度信号は前記ストラップダウン演算部に接続された速度演算部で演算された後に位置演算部に入力され、前記スクリュウ速度はスクリュウ潮流演算部にて誤差補正された後に前記速度信号と比較器で比較され、前記比較器で検出された差がカルマンフィルターに観測値として入力され、前記カルマンフィルターから演算された速度誤差は前記速度演算部に、姿勢誤差は前記ストラップダウン演算部に、ジャイロバイアスは前記角速度信号に、加速度バイアスは前記加速度信号に、スクリュウ／潮流誤差は前記スクリュウ潮流演算部に、位置／方位誤差は前記位置演算部に各々帰還されて制御され、前記潮流を前記水中航走体の主軸方向と慣性座標方向に分け、前記水中航走体の主軸方向の潮流は前記スクリュウ速度のスクリュウ速度誤差に含めて推定する方法である。
【０００６】
【発明の実施の形態】
以下、図面と共に本発明による水中航走体における位置検出方法の好適な実施の形態について説明する。
まず、図１においては本発明による水中航走体における位置検出方法の概念的な説明を行う。まず、図１における符号Ｎ、Ｅは、緯度・経度を基準にした慣性座標であり、Ｘｃ，Ｙｃ，Ｚｃは、真北からαだけ偏角した慣性航法装置の計算の基準となる慣性座標である。また、Ｘｂ，Ｙｂ，Ｚｂは、ＸｃからＹαｗ角だけ偏角した図示しない水中航走体の基準座標である。
また、Ｖｓｔは、真のスクリュウ速度であり、スクリュウ速度誤差εｓを導入することで検出されたスクリュウ速度Ｖｓとの間に以下の関係が成り立つ。
Ｖｓｔ＝（１＋εｓ）Ｖｓ
今、これらの慣性空間にＶｃと云う潮流が存在したとすると、前記水中航走体の移動方向は、
Ｖｔ＝Ｖｓｔ＋Ｖｃ
と表現される。Ｖｔは、真の水中航走体の速度である。本発明は、未知の変数  εｓ（スクリュウ速度誤差），Ｖｃ（潮流速度成分）さらには、慣性航法装置の本質的速度誤差δＶｂｘ，δＶｂｙをそれぞれ推定するのではなく、潮流Ｖｃを水中航走体の主軸方向と慣性座標Ｙｃ方向に分け、水中航走体の主軸方向の潮流はスクリュウ速度誤差に含めて推定することを特徴とする。これは、潮流という変数の特性が、慣性航法装置の速度誤差の特性と似ているために分離した変数として処理することが難しいことに起因する。この操作によって真の速度に関する運動方程式は次の数１の(1)式のように記述できる。
【０００７】
【数１】
0003863043.tif 000002
【０００８】
従って、本発明において推定する変数は、前記スクリュウ速度誤差と潮流速度成分と前記慣性航法装置の変数で次の数２の(2)、(3)及び(4)式の通りである。
【０００９】
【数２】
0003863043.tif 000003
【００１０】
図２は本発明による位置検出方法を示す実施例としてのブロック図である。
図２において、入力データとしては３軸の角速度計１１Ａからの角速度信号１１と３軸の加速度計１２Ａからの加速度信号１２並びにスクリュウの速度を検出したスクリュウ速度１３である。
前記角速度信号１１と加速度信号１２は、初期値を用いてストラップダウン演算部１４でストラップダウン演算され、姿勢と速度が演算される。尚、前述の角速度計１１Ａ、加速度計１２Ａ及びストラップダウン演算部１４により慣性航法装置３０を構成している。
この速度を表す速度信号１４ａは速度演算部１５で演算されて位置演算部１６に入力されている。前記スクリュウ速度１３は、スクリュウ潮流演算部１７にて誤差補正された後に、前記速度演算部１５からすなわち慣性航法装置３０からの速度と比較器１８で比較され、この比較器１８で検出された差１８ａがカルマンフィルター１０の観測値としてフィルター演算動作に入力される。
【００１１】
前記カルマンフィルター１０におけるフィルター演算では、その状態変数の分散値を用いて、逐次、状態量を推定し、次の観測量に帰還入力されている。
従って、カルマンフィルター１０から演算された速度誤差１０ａは速度演算部１５に、姿勢誤差１０ｂはストラップダウン演算部１４に、ジャイロバイアス１０ｃは角速度信号１１に、加速度バイアス１０ｄは加速度信号１２に、スクリュウ／潮流誤差１０ｅはスクリュウ潮流演算部１７に、位置／方位誤差１０ｆは位置演算部１６に、各々帰還されて制御されている。
前記カルマンフィルター１０のフィルター演算では、その状態変数の分散値の初期値を用いて、逐次状態量を推定し、次の観測量に帰還入力されている。
尚、前述のスクリュウに含まれるスリップの速度成分については、前記スクリュウ速度誤差は前記スリップの速度成分のことである。
【００１２】
【発明の効果】
本発明による水中航走体における位置検出方法は、以上のように構成されているため、次のような効果を得ることができる。
すなわち、スクリュウのスクリュウ速度と慣性航法装置の速度信号に基づき、スクリュウ速度に含まれるスリップ並びに潮流、さらに慣性座標の任意方向に含まれる本質的誤差が推定され、結果として高精度な位置を計測することができる。
【図面の簡単な説明】
【図１】本発明による水中航走体における位置検出方法の概念を示す説明図である。
【図２】本発明による水中航走体における位置検出方法を示すブロック図である。
【符号の説明】
１３    スクリュウ速度
１４ａ    速度信号
３０    慣性航法装置
【図１】
0003863043.tif 000004
【図２】
0003863043.tif 000005