(57)【特許請求の範囲】
【請求項１】
内部に原点を有する直交３軸ＸＹＺの各軸まわりの角速度ωＸ、ωＹ、ωＺを検出する３台の角速度センサ（１２）と、重力方向を検出する２軸加速度センサ（１４）と、地磁気を検出する磁気方位センサ（１６）と、前記各センサからの検出データから方位と姿勢角を出力する演算装置（１８）を備え、
演算装置（１８）により、初期航走時はレート積分方式で方位・姿勢角を演算し、定常航走時は２軸加速度センサと磁気方位センサの検出データから方位・姿勢角を演算する、ことを特徴とする水中航走体。
【請求項２】
前記２軸加速度センサ（１４）は、傾斜計である、ことを特徴とする請求項１に記載の水中航走体。
【請求項３】
水中航走体に備えられた演算装置が、
（ａ）初期航走時に、水中航走体の内部に原点を有する直交３軸ＸＹＺの各軸まわりの角速度ωＸ、ωＹ、ωＺを検出して、その時間積分を積分初期値に加算して各軸まわりの角度θＸ、θＹ、θＺを出力し、
（ｂ）定常航走時に、重力方向と地磁気を検出して、方位と姿勢角を出力する、ことを特徴とする水中航走体の方位・姿勢角検出方法。
【請求項４】
前記演算装置が、前記初期航走時から定常航走時への切り換えを、重力方向と地磁気の変動幅が所定のしきい値以下になった時に行う、ことを特徴とする請求項３に記載の方位・姿勢角検出方法。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本発明は、方位・姿勢角を高精度に検出する水中航走体とその方位・姿勢角検出方法に関する。
【０００２】
【従来の技術】
水中を航走する水中航走体として、例えば特開平１−７５８９９号に「揚力舵を持つ水中航走体」が開示されている。この水中航走体１は、図３に模式的に示すように、水中航走体の尾部に方向の異なる４枚のフラップ舵１ａを有し、このフラップ舵１ａの揺動により、上下、左右の姿勢制御と、本体２の回転を防ぐロール制御を行うようになっている。なお、この図において、３は固定舵、４はスクリューである。
【０００３】
上述した水中航走体は、例えば図４に示すように、曳航ケーブル５を介して曳航体６（送波器、受波器等、例えばハイドロフォン）を曳航し、自己ノイズの影響なしに水中音を発信又は受信するようになっている。
【０００４】
図５は、水中航走体の姿勢と座標軸との関係を示している。Ｘ軸を水中航走体の進行方向、Ｙ軸を水平方向、Ｚ軸を上下方向とする直交３軸ＸＹＺを定義し、それぞれの軸回りの角度をθX、θY、θZ、角速度をωX、ωY、ωZとする。Ｘ軸回りの角度θXは、姿勢角のロール、Ｙ軸回りの角度θYは、姿勢角のピッチ、Ｚ軸回りの角度θZは、方位角のヨーにそれぞれ相当する。
【０００５】
【発明が解決しようとする課題】
水中航走体は、プラットホーム（船舶や潜水艇）から発射され、その後、自己の推進装置により水中を航走する。この際、水中航走体が計画された経路を正確に航走するためには、自己の方位と姿勢角を正確に検出する必要がある。
【０００６】
従来、この目的のために、３軸レートセンサを用いたレート積分ジャイロ方式が主として用いられていた。この方式は、水中航走体に角速度ωX、ωY、ωZを検出する３台の角速度センサ（レートセンサと呼ぶ）を備え、プラットホームから発射後にそれぞれの角速度ωX、ωY、ωZを時間積分して自己の方位角（ヨー）と姿勢角（ピッチとロール）を求めるものである。この場合に、方位角（ヨー）と姿勢角（ピッチとロール）の積分初期値が必要となる。以下、初期姿勢角を求めることを初期レベリングと呼ぶ。
【０００７】
プラットホームが激しく動揺している場合には、正確に初期姿勢角を求めることは非常に困難である。そのため、プラットホームを完全に静止させるか、初期姿勢角に大きな誤差がある状態で発射せざるを得ない。しかし、プラットホームを静止させることは困難な場合が多く、逆に誤差があるまま発射（レート積分を開始）すると、その後の姿勢角はずっと誤差をもったままになってしまう。すなわち、図６に示すように、発射直後に初期レベリング誤差があり、更に、レート（角速度）を積分することで方位・姿勢角を求めるため、高精度の角速度センサを用いてもレート信号の誤差が積分誤差となり、時間が経過するほど方位・姿勢角誤差が増大してしまう。そのため、従来のレート積分ジャイロ方式では、発射後の時間が経過するほど誤差が蓄積されてしまい計画通りの経路を航走することができなくなる問題点があった。
【０００８】
一方、水中航走体に傾斜計（又は２軸加速度センサ）と磁気方位センサを備えた方式が用いられる場合もある。この方式では、２軸加速度センサあるいは傾斜計で重力方向を検出することにより姿勢角（ピッチとロール）を求め、磁気方位センサで地磁気を検出することにより方位角を求めるものである。
【０００９】
しかし、この方式では、水中航走体の発射直後は、外乱加速度や鉄の塊であるプラットホームによる地磁気の乱れにより、加速度計や磁気方位センサでは方位・姿勢角を検出することができず、その結果、発射直後は自己の方位・姿勢角が全くわからず盲運転の状態となる問題点があった。
【００１０】
本発明は上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、プラットホームを静止させることなく激しい動揺下でも発射でき、発射直後も自己の方位及び姿勢角を検出することができ、かつ発射後の時間の経過と共に誤差を低減して方位・姿勢角を高精度に検出できる水中航走体とその方位・姿勢角検出方法を提供することにある。
【００１１】
【課題を解決するための手段】
本発明によれば、内部に原点を有する直交３軸ＸＹＺの各軸まわりの角速度ωX、ωY、ωZを検出する３台の角速度センサ（１２）と、重力方向を検出する２軸加速度センサ（１４）と、地磁気を検出する磁気方位センサ（１６）と、前記各センサからの検出データから方位と姿勢角を出力する演算装置（１８）を備え、演算装置（１８）により、初期航走時はレート積分方式で方位・姿勢角を演算し、定常航走時は２軸加速度センサと磁気方位センサの検出データから方位・姿勢角を演算する、ことを特徴とする水中航走体が提供される。
【００１２】
また、本発明によれば、水中航走体に備えられた演算装置が、
（ａ）初期航走時に、水中航走体の内部に原点を有する直交３軸ＸＹＺの各軸まわりの角速度ωX、ωY、ωZを検出して、その時間積分を積分初期値に加算して各軸まわりの角度θX、θY、θZを出力し、
（ｂ）定常航走時に、重力方向と地磁気を検出して、方位と姿勢角を出力する、ことを特徴とする水中航走体の方位・姿勢角検出方法が提供される。
【００１３】
本発明の好ましい実施形態によれば、前記２軸加速度センサ（１４）は、傾斜計である。また、前記演算装置が、前記初期航走時から定常航走時への切り換えを、重力方向と地磁気の変動幅が所定のしきい値以下になった時に行う、ことが好ましい。
【００１４】
上記本発明の装置及び方法によれば、初期航走時（発射直後）はレート積分方式で方位・姿勢角を検出するため、外乱加速度やプラットホーム（鉄の塊）による地磁気の乱れにまったく影響されない。また、プラットホームの動揺等により初期姿勢角に誤差があったとしても、レート積分方式で方位・姿勢角を検出するのは、初期航走時のほんの短い時間であり、その後の定常航走時に、重力方向と地磁気の絶対基準を検出して方位・姿勢角を補正するため、動揺下でレベリングを実施しても、定常航走時にはまったく影響しない。また、角速度センサ（１２）に誤差がある場合でも、定常航走時に絶対基準（重力方向、地磁気）を検出して補正するため、積分誤差が蓄積されるおそれもない。
【００１５】
【発明の実施の形態】
以下、本発明の好ましい実施形態を図面を参照して説明する。なお、各図において、共通する部分には同一の符号を付して使用し重複した説明を省略する。
図１は、本発明による水中航走体の構成図である。本発明の水中航走体１０は、その内部に、３台の角速度センサ１２、少なくともそれぞれ１台の２軸加速度センサ１４、磁気方位センサ１６、及び演算装置１８を備えている。
【００１６】
図５と同様に水中航走体１０の内部に原点Ｏを有する直交３軸ＸＹＺが設定されており、角速度センサ１２は、その各軸まわりの角速度ωX、ωY、ωZを検出し、２軸加速度センサ１４（例えば傾斜計）は、重力方向を検出し、磁気方位センサ１６は地磁気を検出する。これらの各検出データは常時演算装置１８に入力するようになっている。なお２軸加速度センサ１４は傾斜計に限定されず、重力を検出できればなんでも良い。
【００１７】
演算装置１８は、小型のマイクロコンピュータ又は演算素子であり、各センサからの検出データから初期航走時はレート積分方式で方位・姿勢角を演算し、定常航走時は２軸加速度センサと磁気方位センサの検出データから方位・姿勢角を演算する。
【００１８】
すなわち、演算装置１８により、（ａ）初期航走時に、水中航走体の内部に原点を有する直交３軸ＸＹＺの各軸まわりの角速度ωX、ωY、ωZを検出して、その時間積分を積分初期値に加算して各軸まわりの角度θX、θY、θZを出力し、（ｂ）定常航走時に、重力方向と地磁気を検出して、方位と姿勢角を出力する。これらの出力信号は、そのまま水中航走体１０に組み込まれた航走制御装置に入力して水中航走体の航走制御に用いる。また、無線装置によりプラットホームに出力信号を発信し、プラットホーム側で水中航走体の航走制御等に用いてもよい。
【００１９】
初期航走時から定常航走時への切り換えは、例えば重力方向と地磁気の変動幅が所定のしきい値以下になった時に行うのがよい。或いは、一定の時間（例えば１０秒前後）を予め設定し、この時間経過後に自動的に切り換えてもよい。
【００２０】
図２は、本発明の方位・姿勢角検出方法による誤差説明図である。この図に示すように、上述した本発明の装置及び方法によれば、初期航走時（発射直後）はレート積分方式で方位・姿勢角を検出するため、外乱加速度やプラットホーム（鉄の塊）による地磁気の乱れにまったく影響されない。また、プラットホームの動揺等により初期姿勢角に誤差（初期レベリング誤差）があったとしても、レート積分方式で方位・姿勢角を検出するのは、初期航走時のほんの短い時間であり、その後の定常航走時に、重力方向と地磁気の絶対基準を検出して方位・姿勢角を補正するため、動揺下でレベリングを実施しても、定常航走時にはまったく影響しない。また、角速度センサ１２に誤差がある場合でも、定常航走時に絶対基準（重力方向、地磁気）を検出して補正するため、積分誤差が蓄積されるおそれもなく、調定射進角に対して小さい誤差で方位・姿勢角を制御することができる。
【００２１】
以下、本発明の方法を更に詳細に説明する。
１．本発明の背景
現状のレート積分ジャイロの問題点は、動揺下で精度よく初期セルフレベリングできないことである。その解決法として、プラットホーム姿勢データを受け取り初期レベリングする方法が考えられる。しかし、この場合、データ精度とデータの時間遅れがあり、これらは初期レベリング精度に大きく影響し、ジャイロの要求精度を満足できない。
【００２２】
２．本発明の水中航走体
（１）機器構成
機器構成は磁気方位センサ１、レートセンサ３、加速度センサ２、コントローラ１となる。この構成における３台のレートセンサは、高精度の必要性が低いので安価なものを用い、全体コストを下げることができる。また、コントローラも航走制御装置と統合することにより、更に、コストダウン、小型軽量化を図ることができる。
（２）方位・姿勢角検出方式
本発明では、初期航走時と定常航走時で異なる方位・姿勢角センシングを行う。初期航走時はレートセンサを用いてレート積分することにより方位・姿勢角を検出し、定常航走時は磁気方位センサと加速度センサで方位・姿勢角を検出する。このようにセンシング法を分ける理由は以下の通りである。
【００２３】
初期航走時も磁気方位センサと加速度センサで方位・姿勢角を検出できれば、動揺下での初期レベリング問題を解決することができる。すなわち、初期レベリングが必要でない。しかし、初期航走時はプラットホームの影響による地磁気の乱れと外乱加速度のため、磁気方位センサと加速度センサでは正確な方位・姿勢角を検出することはできない。そこで、初期航走時は地磁気の乱れと外乱加速度に影響されないレートセンサを用いたレート積分方式で方位・姿勢角を検出し、地磁気の乱れと外乱加速度が小さい定常航走時は磁気方位センサと加速度センサで方位・姿勢角を検出する。
【００２４】
（３）初期レベリング方式
本発明では、初期航走時はレート積分方式で方位・姿勢角を検出するので、初期レベリングは必要である。ただし、定常航走時は磁気方位センサと加速度センサで絶対基準（地磁気と重力加速度）を検出して方位・姿勢角を求めるので、初期レベリング精度は定常航走時の方位・姿勢角精度にまったく影響ない。従って、初期レベリング精度を大幅に緩和できる。
【００２５】
初期レベリング方式としては、以下の２方式が考えられる。
ａ．外部からプラットホーム姿勢データを受け取り、初期レベリングする方法
従来と大きく異なる点は、プラットホーム姿勢データのデータ誤差と時間遅れを大幅に緩和できることである。
ｂ．プラットホームへの取付角を初期レベリングデータとする方法
この場合、外部からプラットホーム姿勢データをもらう必要がない。ただし、初期航走時のプラットホーム動揺がそのまま初期レベリング誤差となる。
【００２６】
３．本発明の利点
（１）レート積分ジャイロよりコスト、重量、精度の面で有利である。
（２）外部からのプラットホーム姿勢データが必ずしも必須ではない。
（３）ジャイロ固有のノウハウなしにジャイロ制御ソストウェアを開発することができ、ソフトウェアで実行する演算が簡単である。従って、ジャイロ制御装置と航走制御装置を統合することが可能であり、さらにコストダウン、小型・軽量化ができる。
【００２７】
なお、本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更できることは勿論である。
【００２８】
【発明の効果】
上述したように、本発明の水中航走体とその方位・姿勢角検出方法は、プラットホームを静止させることなく発射でき、発射直後も自己の方位及び姿勢角を検出することができ、かつ発射後の時間の経過と共に誤差を低減して方位・姿勢角を高精度に検出できる等の優れた効果を有する。
【図面の簡単な説明】
【図１】本発明による水中航走体の構成図である。
【図２】本発明の方位・姿勢角検出方法による誤差の説明図である。
【図３】従来の水中航走体の模式図である。
【図４】従来の水中航走体の使用状態を示す図である。
【図５】水中航走体の姿勢と座標軸との関係図である。
【図６】従来の水中航走体の方位・姿勢角検出方法による誤差の説明図である。
【符号の説明】
１  水中航走体、１ａ  フラップ舵、２  本体、３  固定舵、
４  スクリュー、５  胴部、６  曳航体、
１０  水中航走体、１２  角速度センサ、
１４  ２軸加速度センサ（傾斜計）、
１６  磁気方位センサ、１８  演算装置
【図１】
0004517258.tif 000002
【図２】
0004517258.tif 000003
【図３】
0004517258.tif 000004
【図４】
0004517258.tif 000005
【図５】
0004517258.tif 000006
【図６】
0004517258.tif 000007