JP2004226374A - 位置計測システム、位置計測方法、プログラム及び航走体運用システム - Google Patents

Abstract

【課題】母船から離れて航行する航走体の位置を、誤差やばらつきを少なくし、正確に把握する。
【解決手段】母船１の位置としての第１位置情報を出力する測位装置３１と、その第１位置情報と、母船１から送出された航走体２の母船１に対する相対位置としての相対位置情報と、航走体２の対地速度としての速度情報と、航走体２の向かう方位としての方位情報とに基づいて、航走体２の位置としての第２位置情報を推定する位置計測装置４とを具備する位置計測システムを用いる。これにより、母船から離れて航行する航走体の位置を正確に把握することができる。
【選択図】 図１

Description

【０００１】
【発明の属する技術分野】
本発明は、位置計測システム及び位置計測方法等に係り、特に、母船と母船に制御されながら航行する航走体とを有する系における位置計測システム、位置計測方法、当該方法をコンピューターに実行させるプログラム及び航走体運用システムに関する。
【０００２】
【従来の技術】
水中で行なう各種探査においては、基地となる母船により制御された無人の航走体が、実際の探査作業を行なうことが多い。その場合、母船は、航走体の正確な絶対位置（以下、単に「位置」ともいう）を把握している必要がある。
【０００３】
そのような位置把握を行なう方法として、以下のような方法が知られている。すなわち、まずＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ：衛星航法システム）のような測位方法を用いて母船の位置を把握する。同時に、航走体のピンガー（海中の物標や地形の状況などの探査のための音波発信器）のような水中音響装置が発する音響（ピンガー音など）の受信状態により、航走体の母船に対する相対位置を把握する。そして、母船の位置と母船に対する相対位置とに基づいて、航走体の（絶対）位置を把握する方法である。
【０００４】
上記方法では、航走体の位置計測の誤差が大きいという問題があった。例えば、水中音響装置を用いた場合には、海中の状態により突然に遠くの場所に位置計算結果が飛んでしまうことが起こる。それに対応するために、位置計算結果が飛ばないように、現在位置から一定距離内への位置更新のみを認めるロジックを入れる。そうすることで、位置計測結果が遠くの場所に飛ぶことをある程度回避できる。
【０００５】
しかし、初期位置が正しく計測されていないと、ずっと正しくない位置を出力しつづけることになる。また、ピンガーのような水中音響による母船と航走体間の位置計測が途絶する（航走体からの信号を母船が受信できない）と、航走体の位置が求められない。
【０００６】
このため、航走体の位置を、誤差やばらつきを少なく、正確に把握することが可能な技術が求められている。また、母船の測位装置に異常が生じた場合や、水中音響による相対位置の計測が困難な状況でも航走体の位置を正確に把握することが可能な技術が求められている。また、水中のようなＧＰＳ信号が届かない場所にいる航走体の位置を正確に把握する技術が求められている。
【０００７】
【発明が解決しようとする課題】
従って、本発明の目的は、母船から離れて航行する航走体の位置を正確に把握可能な位置計測システム、位置計測方法、当該方法をコンピューターに実行させるプログラム及び航走体運用システムを提供することである。
【０００８】
また、本発明の他の目的は、母船に搭載された測位装置に問題が生じても航走体の位置を正確に把握可能な位置計測システム、位置計測方法、当該方法をコンピューターに実行させるプログラム及び航走体運用システムを提供することである。
【０００９】
本発明の更に他の目的は、水中音響による航走体の相対位置の計測が困難な場合でも航走体の位置を正確に把握可能な位置計測システム、位置計測方法、当該方法をコンピューターに実行させるプログラム及び航走体運用システムを提供することである。
【００１０】
本発明の別の目的は、ＧＰＳ信号の届かない水中においても、航走体の位置を正確に求めることが可能な位置計測システム、位置計測方法、当該方法をコンピューターに実行させるプログラム及び航走体運用システムを提供することである。
【００１１】
【課題を解決するための手段】
以下に、［発明の実施の形態］で使用される番号・符号を用いて、課題を解決するための手段を説明する。これらの番号・符号は、［特許請求の範囲］の記載と［発明の実施の形態］との対応関係を明らかにするために付加されたものである。ただし、それらの番号・符号を、［特許請求の範囲］に記載されている発明の技術的範囲の解釈に用いてはならない。
【００１２】
従って、上記課題を解決するために、本発明の位置計測システムは、母船（１）の位置としての第１位置情報を出力する測位装置（３１）と、その第１位置情報と、母船（１）から送出された航走体（２）の母船（１）に対する相対位置としての相対位置情報と、航走体（２）の対地速度としての速度情報と、航走体（２）の向かう方位としての方位情報とに基づいて、航走体（２）の位置としての第２位置情報を推定する位置計測装置（４）とを具備する。
【００１３】
また、本発明の位置計測システムは、位置計測装置（４）が、航走体（２）から送信されるその速度情報とその方位情報とを受信する送受信部（８）を具備する。
【００１４】
また、本発明の位置計測システムは、位置計測装置（４）が、航走体（２）から発せられる探査信号を受信し、その探査信号の到着時刻としての第２時刻情報と、その探査信号の母船（１）に対する方向とを出力する受波器（６）と、航走体（２）から発せられるその探査信号の発信時刻としての第１時刻情報と、その第２時刻情報と、その方向とに基づいて、その相対位置情報を算出する制御部（５）とを更に具備し、送受信部（８）は、更に、その第１時刻情報を受信する。
【００１５】
また、本発明の位置計測システムは、制御部（５）が、更に、その第１位置情報と、その相対位置情報と、その速度情報と、その方位情報とに基づいて、その第２位置情報を推定する。
【００１６】
更に、本発明の位置計測システムは、制御部（５）が、カルマンフィルタを用いてその第２位置情報を推定する。
【００１７】
更に、本発明の位置計測システムは、制御部（５）が、予め設定された演算時間ΔＴ毎のその第２位置情報を下記式（１）乃至式（３）に基づいて計算し、下記式（４）によりその第２位置情報を推定する。ここで、
【数３】

ただし、添え字ｋ：ｋ番目の演算時間、Ｋ_ｋ：カルマンフィルタゲイン、Ｘ_Ｓ：第１位置情報と相対位置情報とに基づいて求める航走体位置計測値、Ｘ＾：第２位置情報、Ｘ_ｋ＾：演算時間ΔＴ毎の第２位置情報、φ：方位情報、Ａ，Ｂ，Ｃ，Ｄ：航走体の運動モデルの状態空間表現、ｕ：速度情報、Ｑ：システム雑音ｗの分散、Ｒ：測定誤差ｎの分散、Ｎ：システム雑音ｗと測定誤差ｎの共分散である。
【００１８】
更に、本発明の位置計測システムは、制御部（５）が、そのΔＴ毎のその第２位置情報を、前記式（１）及び式（２）を用いず、前記式（３）と予め設定されたカルマンフィルタゲインＫとを用いて計算し、計算されたそのΔＴ毎のその第２位置情報に基づいて、前記式（４）でその第２位置情報を推定する。
【００１９】
更に、本発明の位置計測システムは、母船（１）から送出された航走体（２）の対地速度としての速度情報と、航走体（２）の向かう方位としての方位情報とを航走体（２）から受信する送受信部（８）と、航走体（２）から発せられる探査信号を受信し、その探査信号の到着時刻としての第２時刻情報と、その探査信号の母船（１）に対する方向とを出力する受波器（６）と、航走体（２）から発せられる探査信号の発信時刻としての第１時刻情報と、その第２時刻情報と、その方向とに基づいて、その相対位置情報を算出する制御部（５）とを有する位置計測装置（４）を具備し、送受信部（８）は、更に、その第１時刻情報を受信し、制御部（５）は、更に、その相対位置情報と、その速度情報と、その方位情報とに基づいて、航走体（２）の位置としての第２位置情報を推定する。
【００２０】
更に、本発明の位置計測システムは、制御部（５）が、カルマンフィルタを用いてその第２位置情報を推定する。
【００２１】
更に、本発明の位置計測システムは、制御部（５）が、予め設定された演算時間ΔＴ毎のその第２位置情報を下記式（５）乃至式（７）に基づいて計算し、下記式（８）によりその第２位置情報を推定する。ここで、
【数４】

ただし、添え字ｋ：ｋ番目の演算時間、Ｋ_ｋ：カルマンフィルタゲイン、Ｘ_Ｓ：相対位置情報に基づいて求める航走体位置計測値、Ｘ＾：第２位置情報、Ｘ_ｋ＾：演算時間ΔＴ毎の第２位置情報、φ：方位情報、Ａ，Ｂ，Ｃ，Ｄ：航走体の運動モデルの状態空間表現、ｕ：速度情報、Ｑ：システム雑音ｗの分散、Ｒ：測定誤差ｎの分散、Ｎ：システム雑音ｗと測定誤差ｎの共分散である。
【００２２】
更に、本発明の位置計測システムは、その速度情報が、航走体（２）の対地速度検出器（１１）及び対水速度検出器（１３）の少なくとも一方の計測結果に基づいている。
【００２３】
更に、本発明の位置計測システムは、母船（１）は船舶であり、航走体（２）は水中を探査する潜水艇である。
【００２４】
上記課題を解決するために、本発明の航走体運用システムは、上記各項のいずれか一項に記載の位置計測システムと、その位置計測システムを有する母船（１）と、母船（１）に制御され運動する航走体（２）とを具備する。
【００２５】
上記課題を解決するために、本発明の位置計測方法は、母船（１）の位置としての第１位置情報を取得するステップと、母船（１）から送出された航走体（２）の対地速度としての速度情報と、航走体（２）の向かう方位としての方位情報と、航走体（２）から発せられるその探査信号の発信時刻としての第１時刻情報とを航走体（２）から取得するステップと、その探査信号に基づいて、その探査信号の到着時刻としての第２時刻情報と、その探査信号の母船（１）に対する方向とを取得するステップと、その第１時刻情報と、その第２時刻情報と、その方向とに基づいて、航走体（２）の母船（１）に対する位置である相対位置情報を算出するステップと、その第１位置情報と、その相対位置情報と、その速度情報と、その方位情報とに基づいて、カルマンフィルタを用いて航走体（２）の位置としての第２位置情報を推定するステップとを具備する。
【００２６】
上記課題を解決するために、本発明に関わるプログラムは、母船（１）の位置としての第１位置情報を取得するステップと、母船（１）から送出された航走体（２）の対地速度としての速度情報と、航走体（２）の向かう方位としての方位情報と、航走体（２）から発せられるその探査信号の発信時刻としての第１時刻情報とを航走体（２）から取得するステップと、その探査信号の到着時刻としての第２時刻情報と、その探査信号の母船（１）に対する方向とを取得するステップと、その第１時刻情報と、その第２時刻情報と、その方向とに基づいて、航走体（２）の母船（１）に対する位置である相対位置情報を算出するステップと、その第１位置情報と、その相対位置情報と、その速度情報と、その方位情報とに基づいて、カルマンフィルタを用いて航走体（２）の位置としての第２位置情報を推定するステップとをコンピューターに実行させる。
【００２７】
【発明の実施の形態】
以下、本発明である位置計測システム、位置計測方法、当該方法をコンピューターに実行させるプログラム、及び航走体運用システムの実施の形態に関して、添付図面を参照して説明する。
本実施の形態において、母船に制御され、水中探査に使用される航走体を例に示して説明する。ただし、他の母船に制御され、母船を離れて動作を行なう航走体（水中に限らない）についても適用可能である。
なお、各実施の形態において同一又は相当部分には同一の符号を付して説明する。
【００２８】
本発明である位置計測システム、位置計測方法、プログラム及び航走体運用システムの実施の形態の構成について説明する。
図１は、本発明である位置計測システム、位置計測方法、プログラム及び航走体運用システムの実施の形態に関わる全体構成を示す図である。
位置計測装置４及び測位装置としてのＧＰＳ受信装置３１を有する母船１と、航走体制御システム１０を有する航走体２と、ケーブル３とを有するとともに、ＧＰＳ用衛星群３０を使用する。
【００２９】
母船１は、航走体２を探査海域まで輸送し、探査海域において航走体２を送出し、航走体２の運行を制御する。位置計測システムである位置計測装置４及びＧＰＳ受信装置３１を有する。
測位装置としてのＧＰＳ受信装置３１は、母船１に属し、ＧＰＳ用衛星群３０を利用して、母船１の位置としての第１位置情報を出力する。第１位置情報は、（緯度、経度）で例示される。
位置計測装置４は、母船１に属し、母船１に対する航走体２の相対位置としての相対位置情報（例示：母船１からの距離、方向）を算出すると共に、航走体２の対地速度としての速度情報（例示：航行速度）と、航走体２の向かう方位としての方位情報（例示：経線を基準とする緯度方向及び水深方向の角度）とを取得する。そして、前述の第１位置情報と、相対位置情報と、速度情報と、方位情報とに基づいて、航走体２の位置としての第２位置情報を算出する。第２位置情報は、（緯度、経度、水深）で例示される。
【００３０】
航走体２は、母船１で探査海域まで輸送され、探査海域において母船１から送出され、母船１に制御されながら水中を航行する。航走体２は、無人水中探査船で例示される。航走体制御システム１０を有する。
航走体制御システム１０は、航走体２に属する。母船１からの航走体２の相対位置を母船１において算出するために必要な探査信号（例示：ピンガー音）を母船１に向けて発信する。同時に、探査信号の発信時刻としての第１時刻情報を母船１へ出力する。また、航走体２の対地速度としての速度情報（例示：航行速度）と、航走体２の向かう方位としての方位情報（例示：緯線を基準とする経度方向及び水深方向の角度）を計測する。更に、航走体２の対水速度として対水速度情報（例示：海水に対する相対速度）を計測する。計測された速度情報、方位情報及び対水速度情報は、それぞれ必要に応じて、母船１へ出力される。
【００３１】
ケーブル３は、母船１と航走体２とを電気的に接続する。航走体２（の航走体制御システム１０）から出力される速度情報、方位情報及び対水速度情報を母船１（の位置計測装置４）へ伝達する。また、母船１から出力される航走体２を制御するための制御信号を航走体２へ伝達する。ケーブル３を用いず、無線を用いて行なっても良い。
【００３２】
ＧＰＳ用衛星群３０は、一般に用いられるＧＰＳの運用に使用される複数の衛星である。その衛星群からの情報により、ＧＰＳ受信装置３１は、母船１の第１位置情報を出力（算出）する。
【００３３】
次に、位置計測装置４について、更に説明する。
図２は、本発明である位置計測システム、位置計測方法、プログラム及び航走体運用システムの実施の形態において適用される位置計測装置４の構成を示す図である。位置計測装置４は、制御部５、受波器６、記憶部７、送受信部８及び入出力部９を具備する。
【００３４】
制御部５は、位置計測装置４に属する。位置計測装置４全般の制御及び航走体２全般の制御を行なう。また、探査信号の母船１への到着時刻としての第２時刻情報と、第１時刻情報とに基づいて、母船１からの航走体２の相対位置情報を算出する。そして、第１位置情報と、相対位置情報と、速度情報と、方位情報（必要に応じて対水速度情報）とに基づいて、第２位置情報を算出する。算出方法は、後述する。
送受信部８は、位置計測装置４に属する。航走体２から送信される速度情報と方位情報とを、ケーブル３を介して受信する。そして、航走体２から発せられる探査信号の発信時刻としての第１時刻情報を、ケーブル３を介して受信する。また、航走体２を制御するための信号を制御部５から受け取り、ケーブル３を介して航走体２へ出力する。ただし、ケーブル３を用いない場合には、海中無線装置が送受信部８となる。
受波器６は、位置計測装置４に属し、航走体２からの探査信号を受信する。そして、探査信号に基づいて、探査信号の発信方向と、探査信号の母船１への到着時刻としての第２時刻情報とを制御部５へ出力する。
記憶部７は、位置計測装置４に属し、第２位置情報を算出するために利用されるカルマンフィルタゲイン（後述）のテーブルが記憶されている。必要に応じて、既述の各種情報を保持する。
入出力部９は、位置計測装置４に属し、制御部５での制御に必要な情報の入出力を行なう。ケーブル３を伝送する情報もここを経由する。海中無線装置を用いる場合には、入出力部９はアンテナを含む。
【００３５】
次に、航走体制御システム１０について、更に説明する。
図３は、本発明である位置計測システム、位置計測方法、プログラム及び航走体運用システムの実施の形態において適用される航走体制御システム１０の構成を示す図である。航走体制御システム１０は、対地速度検出器１１、記憶部１２、対水速度検出器１３、方位センサ１４、送受信部１５、船体制御部１６及びピンガー１７を具備する。
【００３６】
対地速度検出器１１は、航走体制御システム１０に属し、航走体２の（対地）速度を計測し、速度情報として出力する。対地速度検出器１１は、ドップラーソナーに例示される。
記憶部１２は、計測した各種情報（速度情報、対水速度情報、方位情報）や航行スケジュール（例示：時刻と航走体２の位置を記したテーブル）、探査信号発信時刻などを記憶する。
対水速度検出器１３は、航走体制御システム１０に属し、航走体２の（対水）速度を計測し、対水速度情報として出力する。対水速度検出器１３は、電磁ログ方式の検出器に例示される。
方位センサ１４は、航走体制御システム１０に属し、航走体２の進行する方位を計測し、方位情報として出力する。方位センサ１４は、方位磁石に例示される。
送受信部１５は、航走体制御システム１０に属する。速度情報と方位情報とを、ケーブル３を介して母船１へ送信する。また、探査信号の発信時刻としての第１時刻情報を、ケーブル３を介して母船１へ送信する。更に、必要に応じて対水速度情報を、ケーブル３を介して母船１へ送信する。また、母船１から送信されてくる航走体２を制御するための信号（電力、制御指令など）を、航走体２へ伝達する。
船体制御部１６は、航走体制御システム１０に属する。母船１から送信されてくる航走体２を制御するための信号に基づいて、航走体２を制御する。また、航走体２から出力される信号・情報の入出力を管理する。
ピンガー１７は、航走体制御システム１０に属する。探査信号を母船１に向けて発信する。探査信号は、ピンガー音に例示される。また、探査信号発信と同時に、発信時刻を示す第１時刻情報を出力する。
【００３７】
次に、本発明である位置計測システム、位置計測方法、プログラム及び航走体運用システムの実施の形態の動作について、添付図面を参照して説明する。
図５は、本発明である位置計測システム、位置計測方法、プログラム及び航走体運用システムの実施の形態の動作に関わる計算モデルに用いる座標系を示す図である。本例では、２次元平面での計算について示し、深さ（水深）方向に付いては省略している。しかし、通常の３次元への数学的拡張方法を用いれば、３次元空間における運動に応用することが可能である。
絶対座標系ｘｙを設定し、母船１の座標を（Ｘ，Ｙ）、航走体２の計測した座標を（ｘ_Ｓ，ｙ_Ｓ）、航走体２の（対地）速度を（ｕ_ｇ，ｖ_ｇ）、航走体２の方位をφとする。
【００３８】
本発明においては、母船１にて第１位置情報（計測値）と相対位置情報（計測値）とから得られた航走体２位置（ｘ_Ｓ，ｙ_Ｓ）（計測値）に基づいて、カルマンフィルタ（Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を用いて、航走体２位置（ｘ＾，ｙ＾）を正確に推定する。この推定により、第１位置情報と相対位置情報とから得られる航走体２の位置に比較して、より精度が高く、ばらつきが少ない位置（第２位置情報）を得ることが可能となる。
ただし、文章中の右肩添字“＾”は、推定値を示す（明細書中で同じ）。数式表現では真上添字で表現される場合も有る。
【００３９】
次に、位置計算方法のアルゴリズムについて説明する。
位置計算のステップｋにおけるアルゴリズムに用いる各式（９）〜（１２）を以下に示す。
【数５】

ただし、
添え字ｋ：ｋ番目の演算時間に関する
Ｋ_ｋ：カルマンフィルタゲイン
Ｘ_Ｓ：航走体２位置（計測値）：第１位置情報と相対位置情報とに基づいて求める航走体位置計測値
（３次元の場合には、（ｘ_Ｓ，ｙ_Ｓ，ｚ_Ｓ）である。）
Ｘ＾：航走体２位置（計算値）：第２位置情報（目的とする最終的な推定値）
（３次元の場合には、（ｘ＾，ｙ＾，ｚ＾）である。）
Ｘ_ｋ＾：演算時間（ΔＴ）ごとの航走体２位置（計算値）（推定値）
（３次元の場合には、（ｘ_ｋ＾，ｙ_ｋ＾，ｚ_ｋ＾）である。）
φ：航走体２方位（計測値）：方位情報
Ａ，Ｂ，Ｃ，Ｄ：航走体２の運動モデルの状態空間表現（後述の式（１７）及び式（１８）参照）
ｕ：航走体２の運動モデルへの入力（絶対座標系での対地速度、計測値）：速度情報
Ｑ：システム雑音ｗの分散
Ｒ：測定誤差ｎの分散
Ｎ：そのシステム雑音ｗとその測定誤差ｎの共分散
である。
【００４０】
本例では、２次元における航走体の運動について推定するので、以下の式（１３）〜（１６）を用いる。すなわち、
【数６】

ただし、
添え字ｋ：ｋ番目の演算時間に関する
Ｋ_ｋ：カルマンフィルタゲイン
（ｘ_Ｓ，ｙ_Ｓ）：航走体２位置（計測値）：第１位置情報と相対位置情報とに基づいて求める
（ｘ＾，ｙ＾）：航走体２位置（計算値）：第２位置情報（目的とする最終的な推定値）
（ｘ_ｋ＾，ｙ_ｋ＾）：演算時間（ΔＴ）ごとの航走体２位置（計算値）（推定値）
φ：航走体２方位（計測値）：方位情報
Ａ，Ｂ，Ｃ，Ｄ：航走体２の運動モデルの状態空間表現（後述の式（１７）及び式（１８）参照）
ｕ：航走体２の運動モデルへの入力（絶対座標系での対地速度、計測値）：速度情報（後述の式（１７）参照）
Ｑ：システム雑音ｗの分散（後述の式（１９）参照）
Ｒ：測定誤差ｎの分散（式（１９）参照）
Ｎ：システム雑音ｗと測定誤差ｎの共分散（式（１８）参照）
【００４１】
ここで、離散時間系での航走体２の運動推定モデルを次式（１７），（１８）で表す。
【数７】

ここで、Ｄは、０行列であり、式（１９）は下記で示される。
【数８】

ただし、
ΔＴ：演算時間刻み
（ｕ_ｇ，ｖ_ｇ）＝ｕ：航走体２の（対地）速度（絶対座標系での対地速度、計測値）：速度情報（前記式（１７）］参照）
（ｕ_ｗ，ｖ_ｗ）：対地速度計測誤差
（ｎ_ｘ，ｎ_ｙ）：測位装置（ＧＰＳ）及び音響測位による位置の計測誤差
ｑ：対地速度計測誤差の分散
ｒ：測位装置（ＧＰＳ）及び音響測位による位置の計測誤差の分散
κ：カルマンフィルタゲイン（演算時間ΔＴ毎に変化）
【００４２】
演算手順（Ｉ）について説明する。
（１）ｋ＝０である初期値Ｐ_０が与えられる（ｋ＝１であるＰ_１でも良い）。
同時に、計測値｛（ｘ_Ｓ，ｙ_Ｓ）：航走体２位置、φ：航走体２方位：方位情報、ｕ：航走体２の運動モデルへの入力（絶対座標系での対地速度）：速度情報｝が与えられる。
（２）ｋ＝ｋ＋１として、前記式（１３）及び（１４）（漸化式）からＰ_ｋが計算される（式（１４）では、Ｐ_ｋ＋１と表示）。
（３）Ｐ_ｋに基づいて、式（１３）からＫ_ｋが計算される。
（４）Ｋ_ｋに基づいて、式（１５）及び式（１６）から（ｘ_ｋ＾，ｙ_ｋ＾）が計算（推定）される。
（５）｜Ｐ_ｋ−Ｐ_ｋ−１｜＜δ（δ：予め設定された基準値）｛ただし、ｋΔＴ＜Ｔ_Ｓ（Ｔ_Ｓ：予め設定された許容時間）｝、の状況になった場合、計算を終了し、（６）に進む。そうでない場合には、（２）へ戻り、計算を繰り返す。
（６）航走体２の位置（ｘ＾，ｙ＾）は、（ｘ_ｋ＾，ｙ_ｋ＾）となり、そのときの時刻は、（直前の推定値算出時刻）＋ｋΔＴ、で示される。
【００４３】
ΔＴ毎の航走体２の位置（ｘ_ｋ＾，ｙ_ｋ＾）が演算（推定）され、前記式（１５）の右辺第３項の因子（中括弧｛ ｝内）に基づいて、ΔＴ毎に、測定誤差（ｎ_ｘ，ｎ_ｙ）を漸化的に除去する数学的フィルタリングが実行される。この数学的フィルタリングは、高精度な数学的推定方法である。
【００４４】
ただし、上記演算方法（Ｉ）においては、計算が発散する｛（５）の条件をｋΔＴ＜Ｔ_Ｓで満足できない｝危険性を有している。従って、演算の発散の危険性を回避するためには、各ステップｋでのカルマンフィルタのフィルタゲインＫ_ｋの更新は行なわず、定常解を用いることとする。定常解は次式（２０）で与えられる。
【数９】

ただし、Ｐは次のＲｉｃｃａｔｉ方程式の正定対称な解である。
【数１０】

ただし、各符号は式（１３）〜（１９）での説明を参照のこと。
【００４５】
演算手順（ＩＩ）について説明する。
（１）計測値｛（ｘ_Ｓ，ｙ_Ｓ）：航走体２位置、φ：航走体２方位：方位情報、ｕ：航走体２の運動モデルへの入力（絶対座標系での対地速度）：速度情報｝が与えられる。
（２）式（２０）及び（２１）からＰが計算される。
（３）Ｐに基づいて、式（２０）からＫが計算される。
（４）Ｋに基づいて、式（１３）及び（１４）から（ｘ_ｋ＾，ｙ_ｋ＾）が計算（推定）される。
（５）航走体２の位置（ｘ＾，ｙ＾）は、（ｘ_ｋ＾，ｙ_ｋ＾）となり、そのときの時刻は、（直前の推定値算出時刻）＋ΔＴ、で示される。
【００４６】
上記演算方法（ＩＩ）においては、上記の式（２０）に基づいて、（２）、（３）でＰ及びＫについて演算を行なっている。しかし、Ｐ及びＫについて事前に演算しておき、実際の制御では、（１）、（４）、（５）のみを実行するようにすることも可能である。
【００４７】
その場合、カルマンフィルタの定常解を予め演算して図４に示すようなテーブルを用意しておく。そして、位置計算のステップにおいては、テーブルから適切なフィルタゲインを選択し、それを用いて演算を行なう。そして、フィルタゲインの更新を行なわないこととする。それにより、カルマンフィルタの発散により航走体２の位置演算に失敗し、航走体２を見失う危険を回避することが出来る。
【００４８】
図４は、フィルタゲインを有するのフィルタゲインテーブルを示す表である。
フィルタゲインテーブル２０は、パターン２１、ｑ２２、ｒ２３及びＫ_ｋ２４とからなる。
パターン２１は、気象や海象の状況に基づいて分類されたパターンである。あるいは、対地速度検出器１１に異常が生じて対水速度検出器１３に切り替えた場合、及び、ＧＰＳ受信装置３１による位置計測に異常が生じて第１位置情報を用いないで制御する場合に対応したパターンもある。航走体２が動作を行なう海域の気象や海象、各機器の状況に対応したパターンを選択する。
ｑ２２は、対地速度検出器１１による対地速度計測の誤差の分散（上記ｑ）である。パターン２１に対応した値である。
ｒ２３は、ＧＰＳ受信装置３１による第１位置情報計測及びピンガー１７−受波器６による相対位置情報計測の誤差の分散（上記ｒ）である。パターン２１に対応した値である。
Ｋ_ｋ２４は、カルマンフィルタにおけるフィルタゲインである。式（２０）及び（２１）においては、“Ｋ”と記されている。式（２０）及び（２１）に基づいて計算される。
【００４９】
次に、本発明である位置計測システム、位置計測方法、プログラム及び航走体運用システムの実施の形態の動作の詳細について説明する。
ｉ）母船１は、搭載していた航走体２を、探査海域において海中へ送出する。母船１と航走体２とは、ケーブル３を介した電気通信が可能である（海中無線通信でも良い）。
ｉｉ）母船１の位置計測装置４の制御部５は、入出力部９からの航走体２への移動指令（例示：緯度、経度、水深など）又は記憶部７の移動スケジュールによる移動指令に基づいて、その移動指令を送受信部８から船走体２の航走体制御システム１０へケーブル３を介して送信する。
ｉｉｉ）航走体２は、送受信部１５を介して移動指令を受信する。そして、その移動指令に基づいて移動を行なう。
ｉｖ）航走体２では、航走体制御システム１０の船体制御部１６からの指令により、対地速度検出器１１が、対地速度を計測している。また、方位センサが、進行方向の方位を計測している。更に、対水速度検出器１３が、海水に対する速度を計測している。それら計測された値は、記憶部１２に記憶されている。
ｖ）航走体２では、適当な時間毎に、航走体制御システム１０の船体制御部１６からの指令により、ピンガー１７が、船外へピンガー音を発する。それと共に、ピンガー音を発した時刻（第１時刻）である第１時刻情報、第１時刻あるいは第１時刻に最も近い時刻での速度情報（対地速度）及び方位情報（進行方向の方位）を、送受信部１５から母船１の位置計測装置４へケーブル３を介して送信する。
ｖｉ）母船１のＧＰＳ受信装置３１は、ＧＰＳ衛星群３０からのＧＰＳ信号に基づいて、母船１の位置を算出する。そして、その位置を第１位置情報として位置計測装置４へ出力する。第１位置情報は、その時刻と共に記憶部１２に保持される。
ｖｉ）母船１の位置計測装置４は、受波器６により、ピンガー音を受信する。そしてそのピンガー音に基づいて、受信した時刻（到着時刻）である第２時刻情報と、ピンガー音の発信方向を記憶部７に記憶する。
ｖｉｉｉ）母船１の位置計測装置４は、送受信部８にて、第１時刻情報、速度情報及び方位情報を受信する。
ｉｘ）母船１の位置計測装置４は、制御部５において、第１時刻情報と、第２時刻情報と、ピンガー音（の発信方向）とに基づいて、航走体２の母船に対する相対位置である相対位置情報を求める。すなわち、ピンガー音の音速×（第２時刻情報−第１時刻情報）＝相対距離である。また、発信方向は、水平方向の角度（例示：緯線からの角度）及び垂直方向の角度（例示：水平面からの角度）である。
ｘ）母船１の位置計測装置４は、制御部５において、第１位置情報と相対位置情報とに基づいて、航走体２の位置（ｘ_Ｓ，ｙ_Ｓ）（計測値）を求める。そして、その航走体位置（計測地）と、速度情報と、方位情報とに基づいて、航走体２の位置としての第２位置情報を推定する。推定方法は、既述の演算手順（Ｉ）あるいは（ＩＩ）を用いる。
なお、演算手順（ＩＩ）を用いる場合には、位置計測装置４の記憶部７に保持されているフィルタゲインテーブル２０を用いる。
ｘｉ）推定された航走体２の第２位置情報を用いて、母船１の位置計測装置４の制御部５は、適宜移動指令を発し、その運動を制御する。
ｘｉｉ）適宜（ｉｉ）｛又は（ｉｖ）｝〜（ｘｉ）を繰り返す。
【００５０】
以上の動作により、カルマンフィルタを用いた新規の位置計測システム、位置計測方法及び当該方法をコンピューターに実行させるプログラムを使用して、航走体２の位置を正確に把握することが可能となる。
特に、短い時間毎に連続して位置推定を繰り返すことにより、常時正確な位置を把握しつづけることが出来る。
【００５１】
また、ＧＰＳ受信装置３１による位置計測に異常が生じ、第１位置情報に問題が生じても、第１位置情報を用いないで、航走体２の位置を推定することが出来る。例えば、演算手順（ＩＩ）における（ｘ_Ｓ，ｙ_Ｓ）について、それまでの第１位置情報及び相対位置情報とに基づいて計算してしていたものを、相対位置情報である航走体２の相対位置（ｘ_Ｓ’，ｙ_Ｓ’）とする。そして、式（１５）において、（ｘ_Ｓ，ｙ_Ｓ）を（ｘ_Ｓ’，ｙ_Ｓ’）に置き換え、予めフィルタゲインテーブル（図４）に記憶された対応するカルマンフィルタゲインＫを用いて、第２位置情報（ｘ＾，ｙ＾）を推定する。
すなわち、対地速度検出器１１のみの演算に切り替えることにより、位置計算を継続することができる。すなわち、航走体２を見失うことが無い。
【００５２】
また、対地速度検出器１１に異常が生じ、速度情報に問題が生じても、対水速度検出器１３に切り替えることにより対応することが可能である。例えば、母船１において海流の速度を計測しておき、海流速度と対水速度とから航走体の対地速度を求めることが出来る。そして、その対地速度を速度情報とすれば良い。
【００５３】
あるいは、演算手順（ＩＩ）における速度情報ｕについて、それまでの対地速度検出器１１で測定された対地速度を用いていたものを、対水速度検出器１３で測定された対水速度を用いることとする（速度情報ｕ’）。そして、式（１５）〜（１７）において、ｕをｕ’に置き換え、予めフィルタゲインテーブル（図４）に記憶された対応するカルマンフィルタゲインＫを用いて、第２位置情報（ｘ＾，ｙ＾）を推定する。
すなわち、対水速度検出器１３による速度情報を用いることにより、位置計算を計測することが出来、位置計算を継続することができる。すなわち、航走体２を見失うことが無い。
【００５４】
上記の位置計測システム及び位置計測方法を実施した実験例を以下に示す。ここでは、航走体２の実際の位置におけるｙの値を固定し、ｘの位置を変化させて、位置を比較している。
図６は、航走体２の実際の位置（ｘ_０，ｙ_０）：航走体位置（実際）、ＧＰＳ受信装置３１による第１位置情報及びピンガー１７−受波器６による相対位置情報とから得られる航走体の位置（ｘ_Ｓ，ｙ_Ｓ）：航走体位置（計測）、カルマンフィルタを用いて計算した航走体２の位置（ｘ＾，ｙ＾）：航走体位置（計算）のそれぞれの航走体２の位置を比較した図である。ただし、縦軸及び横軸は、図５で示した軸である。
図から明らかなように、航走体位置（計算）は、航走体位置（実際）とほぼ同じ位置を示している。そして、航走体位置（計測）に比較して、誤差及びばらつきがほとんど発生していないことがわかる。
【００５５】
図７は、母船１の実際の位置（Ｘ_０，Ｙ_０）：母船位置（実際）、母船１のＧＰＳ受信装置３１による第１位置情報：母船位置（計測）のそれぞれの母船１の位置を比較した図である。ただし、縦軸及び横軸は、図６と同様である。また、母船１の実際の位置におけるＹの値を固定し、Ｘの位置を変化させて、各位置を比較している。
図７から明らかなように、母船位置（計測）は、母船位置（実際）を常に正確に表しているわけではなく、ある程度の誤差とばらつきを有することがわかる。
【００５６】
そして、図６及び図７から明らかなように、図７のように母船位置が必ずしも正確に把握されていない状態にもかかわらず、図６のような正確な航走体２の位置を把握することが可能となる。
【００５７】
【発明の効果】
本発明により、母船から離れて航行する航走体の位置を、誤差やばらつきが少なく、正確に把握することが可能となる。
【図面の簡単な説明】
【図１】本発明である位置計測システム、位置計測方法、プログラム及び航走体運用システムの実施の形態に関わる全体構成を示す説明図である。
【図２】前記実施の形態において適用される位置計測装置の構成を示すブロック図である。
【図３】前記実施の形態において適用される航走体制御システムの構成を示すブロック図である。
【図４】前記実施の形態において使用されるカルマンフィルタゲインを示す表である。
【図５】前記実施の形態の動作に関わる計算モデルに用いる座標系を示す説明図である。
【図６】航走体の実際の位置、航走体の計測による位置及び航走体の推測による位置を比較した説明図である。
【図７】母船の実際の位置、母船の計測による位置を比較した説明図である。
【符号の説明】
１ 母船
２ 航走体
３ ケーブル
４ 位置計測装置
５ 制御部
６ 受波器
７ 記憶部
８ 送受信部
９ 入出力部
１０ 航走体制御システム
１１ 対地速度検出器
１２ 記憶部
１３ 対水速度検出器
１４ 方位センサ
１５ 送受信部
１６ 船体制御部
１７ ピンガー
２０ フィルタゲインテーブル
２１ パターン
２２ ｑ
２３ ｒ
２４ Ｋ_ｋ

Claims (14)

1. 母船の位置としての第１位置情報を出力する測位装置と、
   前記第１位置情報と、前記母船から送出された航走体の前記母船に対する相対位置としての相対位置情報と、前記航走体の対地速度としての速度情報と、前記航走体の向かう方位としての方位情報とに基づいて、前記航走体の位置としての第２位置情報を推定する位置計測装置とを具備することを特徴とする位置計測システム。
2. 前記位置計測装置は、前記航走体から送信される前記速度情報と前記方位情報とを受信する送受信部を具備する請求項１記載の位置計測システム。
3. 前記位置計測装置は、前記航走体から発せられる探査信号を受信し、前記探査信号の到着時刻としての第２時刻情報と、前記探査信号の前記母船に対する方向とを出力する受波器と、
   前記航走体から発せられる前記探査信号の発信時刻としての第１時刻情報と前記第２時刻情報と前記方向とに基づいて前記相対位置情報を算出する制御部とを更に具備し、
   前記送受信部は、更に、前記第１時刻情報を受信する請求項２記載の位置計測システム。
4. 前記制御部は、更に、前記第１位置情報と、前記相対位置情報と、前記速度情報と、前記方位情報とに基づいて、前記第２位置情報を推定する、請求項３記載の位置計測システム。
5. 前記制御部は、カルマンフィルタを用いて前記第２位置情報を推定する請求項４記載の位置計測システム。
6. 前記制御部は、予め設定された演算時間ΔＴ毎の前記第２位置情報を下記式（１）乃至式（３）に基づいて計算するか、
   又は下記式（１）及び式（２）を用いず、下記式（３）と予め設定されたカルマンフィルタゲインＫとを用いて計算し、
   計算された前記ΔＴ毎の前記第２位置情報に基づいて、下記式（４）により前記第２位置情報を推定する、
   

   

   （ただし、
   添え字ｋ：ｋ番目の演算時間
   Ｋ_ｋ：カルマンフィルタゲイン
   Ｘ_Ｓ：前記第１位置情報と前記相対位置情報とに基づいて求める航走体位置計測値
   Ｘ＾：前記第２位置情報
   Ｘ_ｋ＾：演算時間ΔＴ毎の前記第２位置情報
   φ：前記方位情報
   Ａ，Ｂ，Ｃ，Ｄ：前記航走体の運動モデルの状態空間表現
   ｕ：前記速度情報
   Ｑ：システム雑音ｗの分散
   Ｒ：測定誤差ｎの分散
   Ｎ：前記システム雑音ｗと前記測定誤差ｎの共分散
   である）
   請求項５記載の位置計測システム。
7. 母船から送出された航走体の対地速度としての速度情報と、前記航走体の向かう方位としての方位情報とを前記航走体から受信する送受信部と、
   前記航走体から発せられる探査信号を受信し、前記探査信号の到着時刻としての第２時刻情報と、前記探査信号の前記母船に対する方向とを出力する受波器と、
   前記航走体から発せられる前記探査信号の発信時刻としての第１時刻情報と、前記第２時刻情報と、前記方向とに基づいて、前記相対位置情報を算出する制御部とを有する位置計測装置を具備し、
   前記送受信部は、更に、前記第１時刻情報を受信し、
   前記制御部は、更に、前記相対位置情報と、前記速度情報と、前記方位情報とに基づいて、前記航走体の位置としての第２位置情報を推定することを特徴とする位置計測システム。
8. 前記制御部は、カルマンフィルタを用いて前記第２位置情報を推定する請求項７記載の位置計測システム。
9. 前記制御部は、予め設定された演算時間ΔＴ毎の前記第２位置情報を下記式（５）乃至式（７）に基づいて計算し、計算された前記ΔＴ毎の前記第２位置情報に基づいて、下記式（８）により前記第２位置情報を推定する、
   

   

   （ただし、
   添え字ｋ：ｋ番目の演算時間
   Ｋ_ｋ：カルマンフィルタゲイン
   Ｘ_Ｓ：前記相対位置情報に基づいて求める航走体位置計測値
   Ｘ＾：前記第２位置情報
   Ｘ_ｋ＾：演算時間ΔＴ毎の前記第２位置情報
   φ：前記方位情報
   Ａ，Ｂ，Ｃ，Ｄ：前記航走体の運動モデルの状態空間表現
   ｕ：前記速度情報
   Ｑ：システム雑音ｗの分散
   Ｒ：測定誤差ｎの分散
   Ｎ：前記システム雑音ｗと前記測定誤差ｎの共分散
   である）
   請求項８記載の位置計測システム。
10. 前記速度情報は、前記航走体の対地速度検出器及び対水速度検出器の少なくとも一方の計測結果に基づいている請求項７，８又は９記載の位置計測システム。
11. 前記母船は船舶であり、前記航走体は水中を探査する潜水艇である請求項１乃至１０のいずれか一項に記載の位置計測システム。
12. 請求項１乃至１１のいずれか一項に記載の位置計測システムと、前記位置計測システムを有する前記母船と、前記母船に制御され運動する前記航走体とを具備することを特徴とする航走体運用システム。
13. 母船の位置としての第１位置情報を取得するステップと、
    前記母船から送出された航走体の対地速度としての速度情報と、前記航走体の向かう方位としての方位情報と、前記航走体から発せられる前記探査信号の発信時刻としての第１時刻情報とを前記航走体から取得するステップと、
    前記探査信号に基づいて、前記探査信号の到着時刻としての第２時刻情報と、前記探査信号の前記母船に対する方向とを取得するステップと、
    前記第１時刻情報と、前記第２時刻情報と、前記方向とに基づいて、前記航走体の前記母船に対する位置である相対位置情報を算出するステップと、
    前記第１位置情報と、前記相対位置情報と、前記速度情報と、前記方位情報とに基づいて、カルマンフィルタを用いて前記航走体の位置としての第２位置情報を推定するステップとを具備することを特徴とする位置計測方法。
14. 母船の位置としての第１位置情報を取得するステップと、
    前記母船から送出された航走体の対地速度としての速度情報と、前記航走体の向かう方位としての方位情報と、前記航走体から発せられる前記探査信号の発信時刻としての第１時刻情報とを前記航走体から取得するステップと、
    前記探査信号の到着時刻としての第２時刻情報と、前記探査信号の前記母船に対する方向とを取得するステップと、
    前記第１時刻情報と、前記第２時刻情報と、前記方向とに基づいて、前記航走体の前記母船に対する位置である相対位置情報を算出するステップと、
    前記第１位置情報と、前記相対位置情報と、前記速度情報と、前記方位情報とに基づいて、カルマンフィルタを用いて前記航走体の位置としての第２位置情報を推定するステップとをコンピューターに実行させるためのプログラム。

Images