JP2012202781A - 移動体位置推定検出装置及び移動体位置推定検出のプログラム - Google Patents

Abstract

【課題】高精度に船舶等の移動体における位置等を推定検出することができる装置等を提供する。
【解決手段】移動体に係る磁界を磁気信号として直交３軸方向の各成分で検知する磁気検知器１と、磁気検知器１がそれぞれ同時に検知した信号を磁気データとして記録するデータ収集器３と、移動体の形状に合わせて設定した複数のモデルに対し、データ収集器３に記録された磁気データを代入して、残差を用いた最小自乗法により算出したパラメータの値に基づいて位置推定値を算出する位置演算手段４Ａ、４Ｂと、位置推定値の中から、移動体の位置を決定する位置検出手段４Ｃとを備えるものである。
【選択図】図１

Description

本発明は、例えば移動体に係る磁気を表す信号を３次元に検知し、移動体の位置等を検出する移動体位置推定検出装置等に関するものである。

移動体の位置を検出する方法として、例えば、３軸磁気センサを用いて船舶が発生する磁気信号の直交３軸成分を検知し、３軸磁気センサと例えば等速直線運動をする移動船舶との相対位置（以下、位置とする）を最小自乗法を用いて算出するものがある（例えば特許文献１参照）。

特開２００２−３３３４８４号公報（図１）

上記の方法では、定めたモデルに基づいて、移動する船舶等の位置を算出しているが、船舶等の形状によっては定めたモデルがあてはまらない場合がある。このような場合には、実際の船舶等の位置と算出した位置に大きな誤差が生じる場合がある。また、場合によっては計算結果が収束等せずに位置を算出することができない場合もある。

そこで、本発明は上記のような問題点を解決し、より高精度に船舶等の移動体における位置等を推定検出することができる装置等を提供することを目的とする。

本発明に係る移動体位置推定検出装置は、移動体に係る磁界を磁気信号として直交３軸方向の各成分で検知する磁気検知手段と、磁気検知手段がそれぞれ同時に検知した信号を磁気データとして記録するデータ記録手段と、移動体の形状に合わせて設定した複数のモデルに対し、データ記録手段に記録された磁気データを代入して、残差を用いた最小自乗法により算出したパラメータの値に基づいて位置推定値を算出する位置演算手段と、位置推定値の中から、移動体の位置を決定する位置検出手段とを備えるものである。

本発明によれば、磁気検知手段の検知に係る信号を、位置演算手段が複数のモデルに基づいて移動体の位置を演算し、位置検出手段が演算した値のうちから、選択して最終的に移動体の位置を推定検出するようにしたので、移動体の形状に近いモデルに基づいて位置演算を行うことができ、精度の高い位置推定検出を行うことができる。

実施の形態１に係る移動体位置推定検出装置の構成ブロック図である。 データ記録部３Ｂに記録するデータを行列で示した図である。 移動体の位置と磁気信号との関係を表す図である。 回転楕円体に係る移動体と磁気信号との関係を説明するための図である。

実施の形態１．
図１は本発明の第１の実施の形態に係る移動体位置推定検出装置の構成ブロック図である。図１において、磁気検知器（磁気センサ）１は磁界強度を信号として検知（受信）し、電気信号（以下、磁気信号という）に変換する（場合によっては、検知した他の電気的物理量に基づく信号に基づいて磁界強度を算出した電気信号を磁気信号とすることもある。また、例えば光信号に変換した後に磁気信号に変換する場合もある）。ここで、磁気検知器１は、直交する３つの検知手段を有する検知器又は３軸センサとなる検知器であり、直交（相対）座標系の３軸方向での検知ができるものとする。Ａ／Ｄ変換器２は、例えばサンプリング等の処理を施して、磁気検知器１が検知した磁気信号をデジタル信号に変換する。

図２はデータ記録部３Ｂに記録するデータを行列で示した図である。データ収集器３は、データ処理部３Ａとデータ記録部３Ｂで構成される。データ演算部５Ａは、デジタル信号に変換された、磁気検知器１による３軸方向の磁気信号及び検知した時刻を関連づける処理を行う、いわゆるデータベース管理システム（ＤＢＭＳ）である。データ記録部３Ｂは、記録装置で構成されており、磁気信号（３軸方向の成分Ｈ_X ，Ｈ_Y ，Ｈ_Z ）及び検知した時刻をそれぞれデータ（以下これらを磁気データ、時刻データという）として、少なくとも一定時間分又は一定個数分記録する。このデータ収集器３はいわゆるデータベースの役割を果たす。図２では、ｍ回のサンプリング（ｍの時刻）によるそれぞれのデータが記録されているものとする。

位置等推定検出器４は、データ収集器３に記録された磁気データに基づいて船舶等の移動体の位置等を推定演算し、検出する。本実施の形態の位置等推定検出器４は、位置演算手段４Ａ、４Ｂ及び位置検出手段４Ｃで構成する。位置演算手段４Ａ、４Ｂは、磁気データに基づいて演算を行い、移動体の位置推定値を算出する。演算は、移動体をほぼ点に近い磁気源（磁気モーメント）としてダイポール（双極子）磁気モーメントに基づくモデル（以下、ダイポールモデルという）と、移動体を回転楕円体とした磁気源（磁気モーメント）とした場合に基づくモデル（以下、回転楕円体モデルという）との２種類のモデルに基づいて行うものとする。特に限定するものではないが回転楕円体モデルに関しても磁気源は双極子であるものとする。ここでは、位置演算手段４Ａがダイポールモデルで演算を行い、位置演算手段４Ｂが回転楕円体モデルで演算を行うものとする。位置検出手段４Ｃは、例えば位置演算手段４Ａ、４Ｂがそれぞれ演算により算出した位置推定値のうち、いずれを採用するかを選択し、最終的に移動体の位置の推定検出を行う。

ここで、通常、位置等推定検出器４は例えばＣＰＵ（Central Processing Unit ）を中心としたコンピュータ等のような制御演算処理手段で構成されている。そして、位置等の推定演算処理手順をあらかじめプログラム化したものを制御演算処理手段が実行し、そのプログラムに基づく処理を行うことで、後述する各式に基づく加減乗除等の演算を行い、さらに演算結果に基づいて収束等を判断することにより位置等の推定検出を実現する。

また、本実施の形態の装置において必須の手段ではないが、接近検知器５、移動検知器６を設け、移動体の接近、移動を判断し、例えばデータ収集器３において、磁気信号の記録開始又は終了等を制御することもできる。

ここで、本実施の形態における移動体は、海面又は海水中を移動する航走体であるものと想定する。また、移動体は水平方向に等速直線運動による移動を行っているものとする。そして、例えば、磁気検知器１の座標系に関し、ＸＹ平面を海面と平行とするために、例えばジンバル機構（図示せず）を用い、Ｚ軸と海面とが垂直に交わるように設置すればよい。また、例えば設置場所が凹凸によりジンバル機構を用いることができない場合、傾斜計（図示せず）を設け、傾斜計により得た磁気検知器１の傾きのデータに基づいて位置等推定検出器４が軸方向について補正を行い、演算を行うようにしてもよい。

図３は移動体の位置と磁気モーメントに係る磁気信号との関係を表す図である。図３に基づいて、まず、位置演算手段４Ａによるダイポールモデルに基づく位置推定値の算出等について説明する。例えば磁気検知器１の直上近傍を移動体が等速直線運動で移動する場合、移動体をダイポールモデルとして考えたときの磁気信号の３軸成分（Ｈ_XA，Ｈ_YA，Ｈ_ZA）は、次式（１）〜（３）で表すことができる。ここで本実施の形態では、磁気信号に基づいて移動体の位置を推定検出するため、磁気モーメントの位置と移動体の位置とが対応しているものとする。

Ｈ_XA＝｛（２Ｘ² −Ｙ² −Ｚ² ）Ｍ_mx＋３ＸＹＭ_my＋３ＺＸＭ_mz｝
／（Ｘ² ＋Ｙ² ＋Ｚ² ）^5/2 …（１）
Ｈ_YA＝｛３ＸＹＭ_mx＋（２Ｙ² −Ｚ² −Ｘ² ）Ｍ_my＋３ＹＺＭ_mz｝
／（Ｘ² ＋Ｙ² ＋Ｚ² ）^5/2 …（２）
Ｈ_ZA＝｛３ＸＺＭ_mx＋３ＹＺＭ_my＋（２Ｚ² −Ｙ² −Ｘ² ）Ｍ_mz｝
／（Ｘ² ＋Ｙ² ＋Ｚ² ）^5/2 …（３）
ここで、Ｍ_mx，Ｍ_my，Ｍ_mzは次式（４）〜（６）となる。
Ｍ_mx＝Ｍ_m ・ｓｉｎθ・ｃｏｓψ …（４）
Ｍ_my＝Ｍ_m ・ｓｉｎθ・ｓｉｎψ …（５）
Ｍ_mz＝Ｍ_m ・ｃｏｓθ …（６）
また、Ｘ，Ｙ，Ｚについては、次式（７）〜（９）となる。
Ｘ＝Ｘ₀ ＋Ｖｃｏｓα・ｔ …（７）
Ｙ＝Ｙ₀ ＋Ｖｓｉｎα・ｔ …（８）
Ｚ＝Ｚ₀ …（９）

ただし、
Ｍ_m ：移動体の磁気モーメント
θ ：磁気モーメントの方向と磁気検出器のｚ軸方向とのなす角
ψ ：磁気モーメントの方向と磁気検出器のｘ軸方向とのなす角
ｔ ：時間
Ｖ ：移動体速度
α ：磁界検知器１と移動体の移動方向とのなす角
Ｘ₀ ，Ｙ₀ ，Ｚ₀ ：移動体の基準位置
である。本実施の形態では、移動体と装置との再接近位置が基準位置（ｔ＝０のときの位置）となるようにする。

磁気信号による位置等の検出において、磁気モーメント数が１とした場合、磁気信号（磁気信号のデータ）から算出するパラメータの組は次式（１０）のようになる。ここで、磁気信号の場合、例えば磁気検知器１の検知に係る磁界強度については、磁気検知器１との距離が遠く磁気モーメントの大きい移動体と、距離が近く磁気モーメントが小さい移動体とで同じ場合がある。このため、（１０）式に示す全てのパラメータが未知であると位置等の特定ができない。そこで、（１０）式に示すパラメータのうち、１つ以上のパラメータが既知となるようにする。例えば、水面を航走する移動体の位置を推定するものであれば、水面と磁気検知器１との距離（水深）をＺ₀ として既知とすることができる。また、音響装置等（図示せず）を用いて測定した移動体の速度をＶとして既知とすることができる。

ａ_m ＝（Ｍ_m ，Ｘ₀ ，Ｙ₀ ，Ｚ₀ ，Ｖ，α，θ，ψ） …（１０）

位置等推定検出器４の位置演算手段４Ａは、データ収集器３に記録されたデータに基づいて、磁気検知器１がそれぞれ実際に検知した磁界に基づく値が理論式を満足させるように、最小自乗法によって各パラメータを調整して値を決定する。最小自乗法を適用する際、パラメータに対して初期値を与え、計算を行う。例えば、基準位置Ｘ₀ ，Ｙ₀ については、Ｘ₀ ＝０、Ｙ₀ ＝０（磁気検知器１の直上）を初期値とする。

最小自乗法には様々な方法があるが、本実施の形態ではガウス・ニュートン法を適用する。ここでは計算時間と収束安定性とのバランス、関数との関係等を考慮した上で、ガウス・ニュートン法を基本としたレーベンベルグ・マルカート（Levenberg-Marquardt ）法を用いて行うことを想定しているが、ダンピング法、パウエル法等、ガウス・ニュートン法に基づく他の解法を用いてもよい。また、ガウス・ニュートン法でなくても、最急降下法等、他の非線形最小自乗法の解法を用いてもよい。上記のような最小自乗法を磁気信号（磁気データ）に適用すると、次式（１１）〜（１３）が成立する。

Ｐ_m ^tＰ_m ・Δａ_m ＝−Ｐ_m ^tｒ_m …（１１）
Ｐ_m ^tＰ_m ＝Ｐ_mx ^t Ｐ_mx＋Ｐ_my ^t Ｐ_my＋Ｐ_mz ^t Ｐ_mz …（１２）
Ｐ_m ^tｒ_m ＝Ｐ_mx ^t ｒ_mx＋Ｐ_my ^t ｒ_my＋Ｐ_mz ^t ｒ_mz …（１３）

ただし、
Ｐ_mx：ｘ軸方向の磁気信号の各パラメータの一階微分からなる行列
Ｐ_my：ｙ軸方向の磁気信号の各パラメータの一階微分からなる行列
Ｐ_mz：ｚ軸方向の磁気信号の各パラメータの一階微分からなる行列
ｒ_m ：残差の自乗和
Δａ_m ：パラメータの修正値
である。

そして、位置演算手段４Ａは、（１１）式における残差の自乗和が、所定の収束条件を満たすまで（例えば残差の自乗和が所定の値以下になる又は自乗和の変化率の減少が所定の値以下になるまで）反復計算を行う。ここで、実際には、移動体の形状等が異なることでモデルがあてはまらず、発散等してしまい、収束条件が満たされないことがある。そこで、位置演算手段４Ａが、反復計算を所定回数行っても収束条件を満たしていないと判断した場合には、移動体の位置の代わりに、位置演算ができなかった旨のデータ信号等を位置検出手段４Ｃに送る。

図４は回転楕円体に係る移動体と磁気信号との関係を説明するための図である。図４は回転楕円体に係る移動体に設定する座標系を表している。次に図４に基づいて、位置演算手段４Ｂによる回転楕円体モデルに基づく位置推定値の算出等について説明する。図４に示すように直角座標系を設定し、その座標を（ｘ，ｙ，ｚ）と表す。磁気検知器１の座標系にするとｘ＝−Ｘ、ｙ＝−Ｙ、ｚ＝−Ｚとなる。また、ｘ軸上のｘ＝±ｃの位置に点Ｃ１、Ｃ２を焦点とする回転楕円体面群（面群における点η：η≧１）、回転双曲面群（面群における点ξ：−１≦ξ≦１）及びｘ軸を含みその一方に拡がる半平面群（面群における点φ：０≦φ≦π）を設定し、これらの面の交点を点Ｐとする。ここで焦点間の距離を２Ｃとし、点Ｐの長球座標（η，ξ，φ）を直交座標系で表すと（ｘ，ｙ，ｚ）で表すと、次式（１４）〜（１６）となる。

η＝［｛（ｘ−Ｃ）² ＋ｙ² ＋ｚ² ｝^1/2 ＋｛（ｘ＋Ｃ）² ＋ｙ² ＋ｚ² ｝^1/2 ］
／２Ｃ …（１４）
ξ＝［｛（ｘ＋Ｃ）² ＋ｙ² ＋ｚ² ｝^1/2 −｛（ｘ−Ｃ）² ＋ｙ² ＋ｚ² ｝^1/2 ］
／２Ｃ …（１５）
φ＝ｔａｎ^-1（ｙ／ｚ） …（１６）

そして、移動体を回転楕円体モデルとして考えたときの磁気信号の３軸成分（Ｈ_XB，Ｈ_YB，Ｈ_ZB）は、次式（１７）〜（１９）で表すことができる。

ここで、Ｅ、Ｆ、Ｇは、次式（２０）〜（２２）である。
Ｅ＝３Ｍ_mx／４πＣ² …（２０）
Ｆ＝−３Ｍ_mz／２πＣ² …（２１）
Ｇ＝−３Ｍ_my／２πＣ² …（２２）

以上の（Ｈ_XB，Ｈ_YB，Ｈ_ZB）に関しても、（４）〜（９）式の関係が成り立つ。そして、（１１）〜（１３）式に基づく最小自乗法を適用し、（１０）式に示す各パラメータの値を決定する。

このとき、位置演算手段４Ｂについても、（１１）式における残差の自乗和が所定の収束条件を満たすまで（例えば残差の自乗和が所定の値以下になる又は自乗和の変化率の減少が所定の値以下になるまで）反復計算を行う。そして、位置演算手段４Ｂが、反復計算を所定回数行っても収束条件を満たしていないと判断した場合には、移動体の位置の代わりに、位置演算ができなかった旨のデータ信号等を位置検出手段４Ｃに送る。

位置検出手段４Ｃは、位置演算手段４Ａ、４Ｂが算出した位置推定値から、誤差が少ないと考えられる方の値を選択し、最終的な移動体の位置として決定する。本実施の形態における選択方法について、例えば各位置推定値は収束条件を満たしているが、収束条件のうち、残差の自乗和が最も小さい位置推定値を選択するようにする。

以上のようにして、収束条件を満たし、推定演算したパラメータ（Ｘ₀ ，Ｙ₀ ，Ｖ，α）に基づいて、ＣＰＡ（Closest Point of Approach ：最接近位置（ｔ＝０における移動体の位置））を推定検出することができる。さらに例えば時間ｔにおける移動体の位置を推定検出することもできる。

また、例えば移動体から発生される電気的物理量や音響信号、パラメータの値等のデータと移動体又はその種別のデータとを関連づけておけば、得られた磁気信号のデータ、決定したパラメータの値から、移動体、移動体の種類等を特定することもできる。

以上のように実施の形態１の移動体位置推定検出装置によれば、磁気検知器１が検知した磁気信号を、位置演算手段４Ａ、４Ｂがそれぞれモデルに基づいて位置推定値を算出し、位置検出手段４Ｃが、所定の基準に基づいて選択した位置推定値を最終的に移動体の位置として決定し、推定検出するようにしたので、移動体の形状等に対応して設定したモデルに基づいて位置演算を行うようにすることができるようになり、精度の高い位置推定検出を行うことができる。このとき、位置検出手段４Ｃは、算出されたすべての位置推定値のうち、残差の自乗和が最も小さい位置推定値を最終的に選択するようにしたので、誤差が少なく、さらに精度の高い位置推定検出を行うことができる。

また、移動体の現在及び／又は将来の位置を推定検出するだけでなく、決定したパラメータの値に基づいて最接近位置、速度等も推定検出するようにすれば、移動体に関する詳細なデータを推定し、得ることができる。そして、例えば移動体による磁界強度、パラメータの値等のデータと移動体又はその種別のデータとを関連づけておけば、得られた磁気信号のデータ、決定したパラメータの値から、移動体又はその種類を特定することもできる。そして、例えば磁界等の信号に基づいて、船首の位置を特定することができれば船体長を推定することができるし、移動体又はその種類を特定することもできる。

さらに、最小自乗法の解法としてガウス・ニュートン法、特にレーベンベルグ・マルカート法又は修正マルカート法を用いるようにしたので、計算時間の短縮を図りつつ、収束安定性のよい最小自乗法を適用し、位置等の検出をすばやく高精度に行うことができる。

実施の形態２．
上述の実施の形態１では、磁気モーメント数を１として演算を行ったが、これに限定するものではない。例えば、ダイポールモデルにおいて移動体に磁気モーメントが複数直列に並んで存在するものとして位置の推定等を行うことができる。この場合、磁気モーメントの間隔ｄをパラメータとして（１０）式に含めた上で、位置等推定検出器４は、上述の演算を行って、各パラメータの値について推定演算を行うようにする。

実施の形態３．
上述の実施の形態では、移動体のＺ軸方向の値が定まっていないため、複数のＺ₀ を設定し、位置演算手段４Ａ、４Ｂは、各Ｚ₀ に基づいて位置の演算を行ったが、これに限定するものではない。例えば、水面上を航走する船舶を移動体とする場合には、Ｚ₀ を１つ設定すればよい。また、移動体に関しても、船舶等だけでなく、航空機等を想定することもできる。

１ 磁気検知器
２ Ａ／Ｄ変換器
３ データ収集器
３Ａ データ処理部
３Ｂ データ記録部
４ 位置等推定検出器
４Ａ、４Ｂ 位置演算手段
４Ｃ 位置検出手段
５ 接近検知器
６ 移動検知器

Claims (12)

1. 移動体に係る磁界を磁気信号として直交３軸方向の各成分で検知する磁気検知手段と、
   該磁気検知手段がそれぞれ同時に検知した信号を磁気データとして記録するデータ記録手段と、
   前記移動体の形状に合わせて設定した複数のモデルに対し、前記データ記録手段に記録された前記磁気データを代入して、残差を用いた最小自乗法により算出したパラメータの値に基づいて位置推定値を算出する位置演算手段と、
   前記位置推定値の中から、前記移動体の位置を決定する位置検出手段と
   を備えることを特徴とする移動体位置推定検出装置。
2. 前記位置演算手段は、
   前記移動体を、ダイポール磁気モーメントを磁気源とした場合のモデルと回転楕円体を磁気源とした場合のモデルとに基づいてそれぞれ推定値を算出することを特徴とする請求項１記載の移動体位置推定検出装置。
3. 前記位置検出手段は、前記位置演算手段が算出した前記位置推定値のうち、最も収束値が小さい値に係る位置を前記移動体の位置として決定することを特徴とする請求項１又は２記載の移動体位置推定検出装置。
4. 前記位置検出手段は、前記パラメータの値に基づいて前記移動体の速度及び／又は前記移動体と前記磁気検知手段との最接近位置をさらに推定検出することを特徴とする請求項１〜３のいずれかに記載の移動体位置推定検出装置。
5. 前記位置検出手段は、前記磁界の強度又は前記パラメータの値に基づいて、前記移動体又はその種別を判断することを特徴とする請求項１〜４のいずれかに記載の移動体位置推定検出装置。
6. 海面又は海水中を航走する航走体を前記移動体とすることを特徴とする請求項１〜５のいずれかに記載の移動体位置推定検出装置。
7. 磁気検知手段が移動体に係る磁界を磁気信号として直交３軸方向の成分毎に同時に受信して得られた時系列の磁気データを、前記移動体の形状に合わせて設定した複数のモデルに代入して、残差を用いた最小自乗法により算出したパラメータの値に基づいて位置推定値を算出する工程と、
   前記位置推定値の中から、前記移動体の位置を決定する工程と
   をコンピュータに行わせることを特徴とする移動体位置推定検出のプログラム。
8. 前記移動体を、ダイポール磁気モーメントを磁気源とした場合のモデルと回転楕円体を磁気源とした場合のモデルとに基づいてそれぞれ推定値を算出することを特徴とする請求項７記載の移動体位置推定検出のプログラム。
9. 前記位置推定値のうち、最も収束値が小さい値に係る位置を前記移動体の位置として決定することを特徴とする請求項７又は８記載の移動体位置推定検出のプログラム。
10. 前記最小自乗法としてガウス・ニュートン法をコンピュータに行わせることを特徴とする請求項７〜９のいずれかに記載の移動体位置推定検出のプログラム。
11. 前記移動体の速度及び／又は前記磁気信号の受信位置と最接近する位置の推定検出をさらにコンピュータに行わせることを特徴とする請求項７〜１０のいずれかに記載の移動体位置推定検出のプログラム。
12. 前記磁界の強度又は前記パラメータの値に基づいて前記移動体又はその種別の判断をさらにコンピュータに行わせることを特徴とする請求項７〜１１のいずれかに記載の移動体位置推定検出のプログラム。

Images