JP5128778B2 - 方位計測装置 - Google Patents

Description

本発明は、地磁気ベクトル検出手段で検出した移動体における地磁気ベクトルを、この地磁気ベクトルに関する補正パラメータを用いて補正し、磁北に対する前記移動体の方位を算出する方位計測装置に関し、より詳細には、船舶内に配置された磁気センサにより検出した地磁気ベクトルの補正や、補正した地磁気ベクトルに基づく該船舶の方位の算出に好適な方位計測装置に関する。

従来より、船舶等の移動体内に搭載されている方位計測装置として、ＧＰＳを用いた方位計測装置と、磁気センサを用いた方位計測装置とがある。これらの方位計測装置は、ジャイロを用いた方位計測装置と比較して、ＧＰＳによる方位計測装置では低価格であるという利点があるが、ＧＰＳ衛星からの電波を受信できなければ、方位を算出することができないという問題がある。また、磁気センサを用いた方位計測装置は、小型、ＧＰＳによる方位計測装置によりさらに低価格、且つ常に方位を算出し続けるという利点を持つが、真北に対する方位ではなく磁北に対する方位を算出する。さらに、計測した方位が周囲の磁気環境や温度変化に左右されやすいので、例えば、図１２に示す特許文献１に開示された方位計測装置２のように、この方位を補正して正しい方位を算出しなければならない。

前記特許文献１に開示された方位計測装置２では、直交コイル４は、そのｘ軸とｙ軸とを含むｘ−ｙ平面が水平面と略一致するように置かれ、方位を示す地磁気ベクトルの水平方向成分を各軸方向で検出する。

上記の方位計測装置２において、磁気環境や温度変化がないときに前記船舶が３６０［°］旋回し、直交コイル４の各軸の出力データの関係をｘ−ｙ平面上でプロットした場合、そのプロット図は中心が原点である円として描かれる。しかしながら、前記船舶内に磁性体（例えば、車両）がある場合、直交コイル４の各軸に感度差がある場合、前記各軸の直交性が崩れている場合、直交コイル４が温度環境等により特性が変化する場合、前記方位に応じた地磁気ベクトルの水平方向成分に依存しない着磁成分が前記移動体内に含まれる場合等には、これらに起因した検出誤差成分が含まれるため、前記プロット図は、中心が原点からずれた楕円として描かれる。そのため、前記出力データに基づく方位には誤差が含まれることになる。

そこで、方位計測装置２においては、データ記憶手段６は、出力データを記憶し、パラメータ推定手段８は、データ記憶手段６に記憶されている所定数の出力データを読み出し、読み出した前記各データを用いて最小二乗法により前記検出誤差成分を取り除くための補正パラメータを推定し、この補正パラメータをパラメータ記憶手段１０に記憶する。

データ補正手段１２は、パラメータ記憶手段１０から前記補正パラメータを読み出し、この補正パラメータに基づいて、直交コイル４で検出した各データから前記検出誤差成分を排除して、直交コイル４で検出した楕円上のデータを円上のデータに補正する。方位算出手段１４は、補正した前記円上のデータを用いて磁北に対する前記船舶の方位を算出する。

特開平９−６８４３１号公報

前述した方位計測装置２では、補正パラメータを精度よく推定するために該方位計測装置２を搭載する船舶を水平方向に３６０［°］程度旋回する必要があり、その間、出力データを大量にデータ記憶手段６に記憶する必要がある。従って、前記補正パラメータを算出するために装置構成が複雑になると共に、補正した方位の算出までに時間がかかるという問題がある。

また、周囲の磁気環境の変化や、温度変化による直交コイル４の特性の変化が発生した場合には、その都度、補正パラメータを推定する必要がある。

さらに、直交コイル４が傾く環境下では、推定される補正パラメータの精度が低下するという問題もある。

本発明は、上述した問題を解決するためになされたものであり、移動体の方位を効率よく算出できる方位計測装置を提供することを目的とする。

また、本発明は、移動体の地磁気ベクトルを検出した際に、真北に対する前記移動体の方位を検出する方位検出手段からの出力に基づいて前記地磁気ベクトルを補正することにより、磁北に対する前記移動体の方位や、真北に対する前記移動体の方位を算出できる方位計測装置を提供することを目的とする。

さらに、本発明は、真北に対する移動体の方位を検出する方位検出手段が何らかの要因で該方位を出力することができない場合にも、前記移動体の地磁気ベクトルの検出に基づいて前記真北に対する該移動体の方位を精度よく算出し続けることが可能な方位計測装置を提供することを目的とする。

本発明に係る方位計測装置は、移動体における地磁気ベクトルを検出する地磁気ベクトル検出手段と、真北に対する前記移動体の方位を検出する方位検出手段と、前記地磁気ベクトルに関する補正パラメータを前記方位に基づき推定する補正パラメータ推定手段と、前記補正パラメータを用いて前記地磁気ベクトルを補正し、補正した前記地磁気ベクトルに基づいて磁北又は前記真北に対する前記移動体の方位を算出する方位補正・算出手段とを有し、前記補正パラメータは、前記地磁気ベクトルを示す２軸のデータをｘ−ｙ平面上でプロットしたときに、このプロット図の中心が前記ｘ−ｙ平面の原点からずれた楕円として描かれる場合での該楕円の形状に関するパラメータと、前記楕円の中心位置と、水平方向の地磁気の強さと、偏角とであることを特徴とする。

これにより、前記地磁気ベクトル及び前記方位を検出する毎に前記補正パラメータが推定され、この補正パラメータに基づいて前記地磁気ベクトルが補正されるので、従来技術と比較して、前記地磁気ベクトルのデータを記憶する記憶手段が不要となり、さらに、観測を始めてから現在までに時々刻々に変化するデータを用いてリアルタイムに追従して前記補正パラメータの推定を行えるので、前記補正パラメータを効率的且つ高精度に推定することが可能となると共に、前記移動体の方位を前記補正パラメータと前記地磁気ベクトル検出手段とを用いて精度よく算出することができる。

また、前記方位検出手段にて検出した方位の誤差分散が大きくても、前記補正パラメータを長時間にわたり推定すれば、該補正パラメータを高精度に推定することが可能である。

さらに、前記補正パラメータは、前記真北に対する磁北のなす角度（偏角）を含むので、前記方位補正・算出手段では、この補正パラメータを用いて、前記磁北に対する方位だけでなく、前記真北に対する方位も算出することが可能となる。

この場合、前記パラメータ推定手段は、非線形フィルタ（例えば、拡張カルマンフィルタ）にて構成される。

さらに、本発明では、前記方位計測装置が方位検出手段の代わりに、前記真北に対する前記移動体の方位と前記移動体のロール及びピッチとを含む前記移動体の姿勢を検出する姿勢検出手段を有することが好ましい。この場合、前記パラメータ推定手段は、前記地磁気ベクトルに関する補正パラメータを前記姿勢に基づき推定し、前記方位補正・算出手段は、前記補正パラメータ及び前記姿勢を用いて前記地磁気ベクトルを補正し、補正した前記地磁気ベクトルに基づいて前記磁北又は前記真北に対する前記移動体の方位を算出するので、前記移動体が水平面に対して傾斜し、又は旋回している場合でも、前記方位をより高精度に算出することができる。なお、前記姿勢検出手段を有する前記方位計測装置において、前記補正パラメータは、前記楕円の形状に関するパラメータと、前記楕円の中心位置と、前記水平方向の地磁気の強さと、水平面に対する地磁気の強さの垂直成分と、前記偏角とである。

このように、前記姿勢は、前記移動体の傾きに対応するロール、ピッチ及び前記真北に対する方位から構成される。従って、前記補正パラメータ推定手段は、前記姿勢検出手段からの前記ロール、前記ピッチ及び前記方位と、前記地磁気ベクトルとに基づいて前記補正パラメータを推定し、前記方位補正・算出手段は、前記姿勢検出手段からの前記ロール及び前記ピッチや、前記補正パラメータを用いて前記地磁気ベクトルを補正し、補正した前記地磁気ベクトルに基づいて前記磁北又は前記真北に対する前記移動体の方位を算出することができる。

また、前記方位検出手段での方位検出、又は前記姿勢検出手段での姿勢検出が何らかの原因で中断した場合、前記方位補正・算出手段は、これまでに推定された補正パラメータを用いて前記地磁気ベクトルを補正することにより、精度のよい状態のままで前記磁北又は前記真北に対する前記移動体の方位を検出することができる。

さらに、上述した各発明において、前記移動体の周囲環境を検出して前記補正パラメータ推定手段の調節を行う周囲環境検出手段や、前記補正パラメータを少なくとも用いた評価関数が所定範囲内であるか否かを検定し、前記評価関数が前記所定範囲外である場合には前記方位補正・算出手段にて算出した前記移動体の方位を出力しないようにする検定手段をさらに有することが好ましい。この場合、前記移動体の周囲環境は、前記移動体の周辺の温度及び／又は前記移動体の姿勢変動量である。

本発明によれば、方位検出手段にて検出した真北に対する移動体の方位と、地磁気ベクトル検出手段にて検出した地磁気ベクトルとを用いて、検出した前記地磁気ベクトルに対する補正パラメータを補正パラメータ推定手段により効率よく且つ精度よく推定することで、前記地磁気ベクトル検出手段にて検出した前記地磁気ベクトルと、推定した前記補正パラメータとを用いて方位補正・算出手段にて、磁北に対する方位ばかりでなく、真北に対する方位も精度よく算出することが可能となる。

また、前記方位検出手段にて、何らかの原因で方位を出力することができない場合でも、前記方位補正・算出手段は、これまでに推定された補正パラメータを用いて前記地磁気ベクトルを補正して、前記磁北又は前記真北に対する前記移動体の方位を算出し続けることができる。

さらに、前記方位検出手段で検出した方位の誤差分散が大きくても、前記補正パラメータ推定手段を構成するフィルタを適宜設定することにより、前記補正パラメータを精度よく推定することができる。これにより、前記方位補正・算出手段にて算出した前記移動体の方位を、前記方位検出手段にて検出した方位よりも高精度にすることが可能となる。

本発明に係る方位計測装置について、好適な実施の形態を挙げ、添付の図面を参照しながら以下に説明する。

図１は、本実施形態に係る方位計測装置２０のブロック図である。

この方位計測装置２０は、２軸の磁気センサ（地磁気ベクトル検出手段）２２と、ＧＰＳによる方位計測装置等の真北に対する方位を検出する方位検出手段２４と、補正パラメータ推定手段２６と、方位補正・算出手段２８とを有し、移動体としての船舶４０（図４参照）内に配置（搭載）されている。また、方位計測装置２０は、方位検出手段２４に代えて、図２に示すように、ＧＰＳによる真北に対する方位を検出可能な方位計測装置や、加速度センサ等の傾斜成分を検出する装置を複合することにより船舶４０の姿勢（方位、ロール及びピッチ）を出力する姿勢検出手段３０を有することも可能である。

図１において、磁気センサ２２は、磁北に対する船舶４０の船首方向の方位に応じた地磁気ベクトルを検出し、この地磁気ベクトルを示す２軸のデータを補正パラメータ推定手段２６及び方位補正・算出手段２８に出力する。

方位検出手段２４は、真北に対する船舶４０の船首方向の方位を検出し、補正パラメータ推定手段２６に出力する。

補正パラメータ推定手段２６は、図示しない非線形フィルタとしての拡張カルマンフィルタから構成され、前記地磁気ベクトルを示す２軸のデータが入力される毎に、地磁気ベクトルに関する補正パラメータを、方位検出手段２４からの前記方位に基づき推定し、推定した前記補正パラメータを方位補正・算出手段２８に出力する。

なお、前記補正パラメータとは、船舶４０を３６０［°］旋回させて、磁気センサ２２から出力された地磁気ベクトルを示す各軸のデータをｘ−ｙ平面上でプロットしたときに（図３参照）、このプロット図の中心（ｘ₀、ｙ₀）が原点Ｏからずれた楕円として描かれる場合において、この楕円に関するパラメータや、船舶４０の現在の位置における地磁気ベクトルに関するパラメータをいう。

また、前記楕円に関するパラメータは、前記２軸のデータに含まれる検出誤差成分に起因して前記データが原点Ｏからずれた楕円として描かれた場合の該楕円のパラメータをいう。

さらに、前記検出誤差成分は、具体的に、（１）船舶４０（図４参照）内に磁性体（例えば、車両）があって、水平方向（ｘ−ｙ平面３８上）に依存した磁場が前記磁性体から発生する場合、（２）磁気センサ２２（図１及び図２参照）の各軸に感度差がある場合、（３）前記各軸の直交性が崩れている場合、（４）磁気センサ２２にドリフトがある場合、（５）前記地磁気ベクトルに応じた成分（真の方位を示す各軸の成分）に依存しない着磁成分が船舶４０内に含まれる場合がある。このうち（１）〜（３）が円から楕円になる原因であり、（４）及び（５）が中心のオフセット（ｘ₀、ｙ₀）の原因である。従って、実際には、これらの（１）〜（５）に起因した検出誤差成分が前記２軸のデータに含まれることになる。

なお、前記検出誤差成分がない場合、前記２軸のデータは、前記地磁気ベクトルのみで構成されるデータとなるので、前記ｘ−ｙ平面上で原点Ｏを中心とした円として描かれる。

方位補正・算出手段２８（図１参照）は、補正パラメータ推定手段２６からの前記補正パラメータに基づいて、前記２軸のデータから前記検出誤差成分を排除することで、前記２軸のデータを前記楕円上のデータから前記円上のデータに補正し、補正した前記円上のデータを用いて磁北に対する船舶４０の船首方向の方位を算出し、あるいは、補正した前記円上のデータ及び前記補正パラメータを用いて真北に対する船舶４０の船首方向の方位を算出する。

また、方位計測装置２０が方位検出手段２４の代わりに姿勢検出手段３０（図２参照）を有する場合、該姿勢検出手段３０は、船舶４０の姿勢（方位、ピッチ及びロール）を検出して、前記方位、前記ピッチ及び前記ロールを補正パラメータ推定手段２６に出力し、前記ピッチ及び前記ロールを方位補正・算出手段２８に出力する。この場合、補正パラメータ推定手段２６は、前記方位、前記ピッチ及び前記ロールに基づいて、前記補正パラメータを推定する。方位補正・算出手段２８は、前記ロール、前記ピッチ及び前記補正パラメータに基づいて、磁気センサ２２からの前記２軸のデータを補正し、補正した前記データを用いて、磁北又は真北に対する船舶４０の船首方向の方位を算出する。

ここで、図４に示すように、船舶４０の船首の方位をｘ軸とし、船舶４０の甲板と略平行な平面をｘ−ｙ平面３８とし、このｘ−ｙ平面３８上で前記ｘ軸と直交する軸をｙ軸とし、該船舶４０に対して上方向をｚ軸とする右手系の直交座標系を定めた場合に、前記ピッチは、ｙ軸を中心として船舶４０を回転させたときの回転角θであり、換言すれば、船舶４０の船首・船尾（前後）への傾きを表わす。さらに、前記ロールは、前記ピッチが決まった状態でのｘ軸を中心として船舶４０を回転させたときの回転角φであり、換言すれば、船舶４０の右舷・左舷（左右）への傾きを表わす。

図５は、方位検出手段２４を有する方位計測装置２０の処理を示すフローチャートであり、図６は、姿勢検出手段３０を有する方位計測装置２０の処理を示すフローチャートである。

方位検出手段２４（図１参照）を有する方位計測装置２０では、図３のステップＳ１において、磁気センサ２２は、磁北に対する船舶４０の船首方向の方位に応じた地磁気ベクトルを示す２軸のデータを検出して、補正パラメータ推定手段２６及び方位補正・算出手段２８に出力し、方位検出手段２４は、真北に対する船舶４０の船首方向の方位を検出して、補正パラメータ推定手段２６に出力する。

補正パラメータ推定手段２６は、最初に入力された前記２軸のデータ及び前記方位を用いて前記補正パラメータの初期値を計算し、拡張カルマンフィルタの初期設定を行う（ステップＳ２）。

次に、磁気センサ２２は、２回目に検出した前記２軸のデータを補正パラメータ推定手段２６及び方位補正・算出手段２８に出力し、方位検出手段２４は、検出した前記方位を補正パラメータ推定手段２６に出力する（ステップＳ３）。

補正パラメータ推定手段２６は、前記拡張カルマンフィルタを用いて前記初期値と２回目に入力された前記２軸のデータ及び前記方位から補正パラメータを推定し、推定した前記補正パラメータを方位補正・算出手段２８に出力する（ステップＳ４）。

ステップＳ４において推定される補正パラメータは、地磁気に関するパラメータである、水平方向の地磁気の強さを表わすＡと、偏角を表わすα（図７参照）と、楕円の形状に関するパラメータであるＧ及びβと、前記楕円の中心位置を表わすｘ₀及びｙ₀（図３参照）とであり、これらの補正パラメータを前記拡張カルマンフィルタを用いて推定する。従って、前記拡張カルマンフィルタにおける状態変数ベクトルＸ_kは、下記の（１）式で表わされる。
Ｘ_k ^T＝［Ａ_k、Ｇ_k、α_k、β_k、ｘ_0(k)、ｙ_0(k)］ （１）

なお、（１）式中で添字のｋは、状態変数ベクトルＸ_kが磁気センサ２２及び方位検出手段２４のｋ回目のデータ検出に対応することを示し、Ｘ_kの上付き添字のＴは、転置行列であることを示し、磁気センサ２２及び方位検出手段２４からの各データの検出（入力）時刻は略同じである。

前記拡張カルマンフィルタにおけるｋ回目の検出に対応する状態変数ベクトルＸ_kと、（ｋ−１）回目の検出に対応する状態変数ベクトルＸ_k-1との関係を表わす状態方程式は、その変動量をΔＸ_k-1とすれば、下記の（２）式で表される。
Ｘ_k＝Ｘ_k-1＋ΔＸ_k-1 （２）

ここで、変動量ΔＸ_k-1は、未知量であるが、前記拡張カルマンフィルタの計算で必要となる状態方程式における変動量を表わす共分散行列Ｑは既知のものであるとして、適宜設定する。

また、前述した（１）〜（５）の検出誤差成分の要因を考慮して、前記拡張カルマンフィルタにおける磁気センサ２２の各軸の観測値（実際に磁気センサ２２にて検出されるデータ）［ｘ_ms(k)、ｙ_ms(k)］^Tと、推定する補正パラメータを表わす状態変数ベクトルとの関係を表わす観測方程式は、下記の（３）式で表わされる。また、前記観測値と方位との関係は、図７で表わされる。

（３）式において、ψ_E(k)は、方位検出手段２４で検出された方位を示し、船舶４０（図４参照）の船首が反時計回りに回転する方向を正とする。また、ｅ_ms(k)＝［ｅ_msx(k)、ｅ_msy(k)］^Tは、磁気センサ２２の各軸の検出誤差を表わし、ｅ_ψ(k)は、方位検出手段２４で検出した方位の検出誤差を表わす。さらに、ｈ（ｘ_(k)、ψ_E(k)、ｅ_ms(k)、ｅ_ψ(k)）は、前記観測方程式を示す関数である。なお、真の方位ψとψ_E(k)とｅ_ψ(k)との関係は、ψ＝ψ_E(k)＋ｅ_ψ(k)で表わされる。各誤差ｅ_ms(k)、ｅ_ψ(k)の値は知ることができないが、前記拡張カルマンフィルタにおける状態方程式に含まれる誤差成分の共分散行列Ｒは、既知として適宜設定する。

従って、ステップＳ４では、磁気センサ２２からの地磁気ベクトルを示す２軸のデータ（観測値）［ｘ_ms(k)、ｙ_ms(k)］^Tと、方位検出手段２４からの方位とが略同時刻に補正パラメータ推定手段２６に入力したときに、前述した（１）式〜（３）式及び適宜設定された共分散行列Ｑ、Ｒを用いて、前記拡張カルマンフィルタにより補正パラメータ（Ａ_k、Ｇ_k、α_k、β_k、ｘ_0(k)及びｙ_0(k)）を推定し、推定した前記補正パラメータを方位補正・算出手段２８に出力する。

方位補正・算出手段２８は、前記補正パラメータに基づいて、前記２軸のデータから前記検出誤差成分を排除することで、前記２軸のデータを前記楕円上のデータから前記円上のデータに補正し、補正した前記円上のデータを用いて磁北又は真北に対する船舶４０の船首方向の方位を算出する（ステップＳ５）。

この場合、方位補正・算出手段２８は、補正パラメータ推定手段２６から入力された補正パラメータ（Ａ_k、Ｇ_k、α_k、β_k、ｘ_0(k)及びｙ_0(k)）を用いて、補正した前記地磁気ベクトルに対応する２軸のデータｇ_m(k) ^T＝［Ａ_kｃｏｓ（ψ_m(k)）、−Ａ_kｓｉｎ（ψ_m(k)）］を下記の（４）式に基づき算出する。なお、（４）式中、ψ_m(k)は、磁北に対する船舶４０の船首方向の方位（磁気方位）を示し、ｇ_mx(k)は、ｇ_m(k)のｘ方向成分を示し、ｇ_my(k)は、ｇ_m(k)のｙ方向成分を示す。

従って、方位補正・算出手段２８は、（４）式に基づいて補正された２軸のデータｇ_m(k) ^T＝［Ａ_kｃｏｓ（ψ_m(k)）−Ａ_kｓｉｎ（ψ_m(k)）］を用いて磁気方位ψ_m(k)を下記の（５）式を用いて算出する。なお、（５）式中、ｊは、虚数を示す。
ψ_m(k)＝ａｒｇ（ｇ_mx(k)−ｊｇ_my(k)） （５）

また、真北に対する船舶４０の船首方向の方位ψ_Tを、磁気方位ψ_m(k)及び偏角を表わす補正パラメータα_kを用いた下記の（６）式により算出する。
ψ_T＝ψ_m(k)＋α_k （６）

その後、方位計測装置２０は、磁気センサ２２で地磁気ベクトルを検出し、方位検出手段２４で方位を検出する毎（３回目の検出以降）にステップＳ３〜Ｓ５の処理を繰り返し行う。

一方、姿勢検出手段３０（図１参照）を有する方位計測装置２０では、図６のステップＳ６において、磁気センサ２２は、地磁気ベクトルを示す２軸のデータを検出して、補正パラメータ推定手段２６及び方位補正・算出手段２８に出力し、姿勢検出手段３０は、船舶４０の姿勢を検出して、前記姿勢を構成する方位、ロール及びピッチを補正パラメータ推定手段２６に出力し、前記ロール及び前記ピッチを方位補正・算出手段２８に出力する。

補正パラメータ推定手段２６は、最初に入力された前記２軸のデータ及び前記方位、前記ロール及び前記ピッチを用いて前記補正パラメータの初期値を計算し、拡張カルマンフィルタの初期設定を行う（ステップＳ７）。

次に、磁気センサ２２は、２回目に検出した前記２軸のデータを補正パラメータ推定手段２６及び方位補正・算出手段２８に出力し、姿勢検出手段３０は、検出した前記方位、前記ロール及び前記ピッチを補正パラメータ推定手段２６に出力し、検出した前記ロール及び前記ピッチを方位補正・算出手段２８に出力する（ステップＳ８）。

補正パラメータ推定手段２６は、前記拡張カルマンフィルタを用いて前記初期値と、２回目に入力された前記２軸のデータと、前記方位、前記ロール及び前記ピッチとから補正パラメータを推定し、推定した前記補正パラメータを方位補正・算出手段２８に出力する（ステップＳ９）。

ステップＳ９において推定される補正パラメータは、地磁気に関するパラメータである、水平方向の地磁気の強さを表わすＡ_kと、水平面に対する地磁気の強さの垂直成分Ａ_z(k)と、偏角を表わすα_k（図７参照）と、楕円の形状に関するパラメータであるＧ_k、β_k及びγ_kと、前記楕円の中心位置を表わすｘ_0(k)及びｙ_0(k)（図３参照）とであり、これらの補正パラメータを前記拡張カルマンフィルタを用いて推定する。従って、前記拡張カルマンフィルタにおける状態変数ベクトルＸ_kは、下記の（７）式で表わされる。
Ｘ_k ^T＝［Ａ_k、Ａ_z(k)、Ｇ_k、α_k、β_k、γ_k、
ｘ_0(k)、ｙ_0(k)］ （７）

なお、前記拡張カルマンフィルタにおける状態方程式は、（２）式と同じであり、この状態方程式における変動成分の共分散行列Ｑも前述と同様に適宜設定する。

この場合、前述した（１）〜（５）の検出誤差成分の要因を考慮して、磁気センサ２２の各軸の観測値［ｘ_ms(k)、ｙ_ms(k)］^Tと、推定する補正パラメータを表わす状態変数ベクトルＸ_kとの関係を表わす観測方程式は、変換行列Ｔ_2D（ψ、θ、φ）を下記の（８）式で表わすと、（９）式で表わされる。

（９）式において、θ_Ｅ（ｋ）及びφ_Ｅ（ｋ）は、姿勢検出手段３０で検出されたピッチ及びロールであり、θ_Ｅ（ｋ）は、船舶４０（図４参照）の船首の方向が下に傾くときを正とし、φ_Ｅ（ｋ）は、前記船首に向かって右側又は下に傾くときを正とする。さらに、ｈ（ｘ_（ｋ）、Φ_Ｅ（ｋ）、ｅ_{ｍｓ（ｋ）}、ｅ_Φ（ｋ））は、前記観測方程式を示す関数である。この場合、Φ_Ｅ（ｋ）＝（ψ_Ｅ（ｋ）、θ_Ｅ（ｋ）、φ_Ｅ（ｋ））であり、ｅ_Φ（ｋ）＝（ｅ_ψ（ｋ）、ｅ_θ（ｋ）、ｅ_φ（ｋ））であり、ｅ _φ（ｋ）及びｅ _θ（ｋ）は、姿勢検出手段３０で検出したロール及びピッチの検出誤差をそれぞれ表わす。なお、真のピッチθ及びロールφと、θ_Ｅ（ｋ）及びφ_Ｅ（ｋ）と、ｅ_θ（ｋ）及びｅ_φ（ｋ）との関係は、θ＝θ_Ｅ（ｋ）＋ｅ_θ（ｋ）、φ＝φ_Ｅ（ｋ）＋ｅ_φ（ｋ）で表わされる。前記拡張カルマンフィルタにおける状態方程式に含まれる誤差成分の共分散行列Ｒは、既知として前述と同様に適宜設定する。

従って、ステップＳ９では、磁気センサ２２からの地磁気ベクトルを示す２軸のデータ（観測値）［ｘ_ms(k)、ｙ_ms(k)］^Tと、姿勢検出手段３０からの方位、ロール及びピッチとが略同時刻に補正パラメータ推定手段２６に入力したときに、前述した（２）式及び（７）式〜（９）式と、適宜設定された共分散行列Ｑ、Ｒとを用いて、前記拡張カルマンフィルタにより補正パラメータ（Ａ_k、Ａ_z(k)、Ｇ_k、α_k、β_k、γ_k、ｘ_0(k)及びｙ_0(k)）を推定し、推定した前記補正パラメータを方位補正・算出手段２８に出力する。

方位補正・算出手段２８は、前記補正パラメータ、前記ロール及び前記ピッチに基づいて、前記２軸のデータから前記検出誤差成分を排除することで、前記２軸のデータを前記楕円上のデータから前記円上のデータに補正し、補正した前記円上のデータを用いて磁北又は真北に対する船舶４０の船首方向の方位を算出する（ステップＳ１０）。

この場合、方位補正・算出手段２８は、補正パラメータ推定手段２６から入力された補正パラメータ（Ａ_k、Ａ_z(k)、Ｇ_k、α_k、β_k、γ_k、ｘ_0(k)及びｙ_0(k)）を用いて、補正した前記地磁気ベクトルに対応する２軸のデータｇ_m(k) ^T＝［Ａ_kｃｏｓ（ψ_m(k)）、−Ａ_kｓｉｎ（ψ_m(k)）］を下記の（１０）式に基づき算出する。

従って、方位補正・算出手段２８は、（１０）式に基づいて補正された２軸のデータｇ_m(k) ^T＝［Ａ_kｃｏｓ（ψ_m(k)）、−Ａ_kｓｉｎ（ψ_m(k)）］を用いて磁気方位ψ_m(k)を（５）式により算出し、さらに、真北に対する船舶４０の船首方向の方位ψ_Tを、磁気方位ψ_m(k)及び偏角を表わす補正パラメータα_kを用いた（６）式により算出する。

その後、方位計測装置２０は、磁気センサ２２で地磁気ベクトルを検出し、姿勢検出手段３０で前記姿勢を検出する毎にステップＳ８〜Ｓ１０の処理を繰り返し行う。

このように、本実施形態に係る方位計測装置２０は、船舶４０における地磁気ベクトルを検出する磁気センサ２２と、真北に対する船舶４０の船首方向の方位を検出する方位検出手段２４と、前記地磁気ベクトルに関する補正パラメータを前記方位に基づき推定する補正パラメータ推定手段２６と、前記補正パラメータを用いて前記地磁気ベクトルを補正し、補正した前記地磁気ベクトルに基づいて前記磁北又は前記真北に対する船舶４０の船首方向の方位を算出する方位補正・算出手段２８とを有する。

これにより、前記地磁気ベクトル及び前記方位を検出する毎に前記補正パラメータが推定され、この補正パラメータに基づいて前記地磁気ベクトルが補正されるので、従来技術（図１２参照）と比較して、前記地磁気ベクトルを示す２軸のデータを記憶する記憶手段が不要となり、さらに、観測を始めてから現在までに時々刻々に変化するデータを用いてリアルタイムに追従して前記補正パラメータの推定を行えるので、前記補正パラメータを効率的且つ高精度に推定することが可能となると共に、船舶４０の船首方向の方位を前記補正パラメータと磁気センサ２２とを用いて精度よく算出することができる。

また、方位検出手段２４にて検出した方位の誤差分散が大きくても、前記補正パラメータを長時間にわたり推定すれば、該補正パラメータを高精度に推定することが可能である。

さらに、前記補正パラメータは、前記真北に対する磁北のなす角度（偏角）を含むので、方位補正・算出手段２８では、この補正パラメータを用いて、前記磁北に対する船舶４０の船首方向の方位だけでなく、前記真北に対する船舶４０の船首方向の方位も算出することが可能となる。

さらにまた、本実施形態では、方位計測装置２０が方位検出手段２４の代わりに、船舶４０の姿勢を検出する姿勢検出手段３０を有するようにしてもよい。この場合、補正パラメータ推定手段２６は、前記地磁気ベクトルに関するパラメータを前記姿勢を構成する方位、ロール及びピッチに基づき推定し、方位補正・算出手段２８は、前記補正パラメータや前記ロール及び前記ピッチを用いて前記地磁気ベクトルを補正し、補正した前記地磁気ベクトルに基づいて前記磁北又は前記真北に対する船舶４０の船首方向の方位を算出するので、船舶４０が水平面に対して傾斜し、又は旋回している場合でも、前記方位をより高精度に算出することができる。

また、方位検出手段２４での方位検出、又は姿勢検出手段３０での姿勢検出が何らかの原因で中断した場合、方位補正・算出手段２８は、これまでに推定された補正パラメータを用いて前記地磁気ベクトルを補正することにより、精度のよい状態のままで前記磁北又は前記真北に対する船舶４０の船首方向の方位を検出することができる。

なお、本実施形態に係る方位計測装置２０では、磁気センサ２２が２軸のデータを出力し、方位計測装置２０がｘ−ｙ平面上での船舶４０の船首方向の方位を算出する場合について説明したが、磁気センサ２２が３軸のデータを出力する場合にも適用可能であることは勿論である。

また、本実施形態に係る方位計測装置２０では、図８及び図９に示すように、船舶４０周辺の温度を検出する温度計や、船舶４０の姿勢変動量を検出する姿勢変動量測定装置から構成される周囲環境検出手段３２を配置することが望ましい。この場合、周囲環境検出手段３２は、前記温度や前記姿勢変動量等の周囲環境を示すデータを補正パラメータ推定手段２６に出力し、補正パラメータ推定手段２６は、入力された前記データに基づいて前記拡張カルマンフィルタの設定を適宜変更する。具体的には、環境の変化に応じて、拡張カルマンフィルタにおける状態方程式の変動量の共分散行列Ｑの設定を適宜変更する。これにより、補正パラメータを精度よく推定することができる。

さらに、本実施形態に係る方位計測装置２０では、図１０及び図１１に示すように、方位補正・算出手段２８の出力側に、補正パラメータ及び磁北又は真北に対する船舶４０の船首方向の方位を検定する検定手段３４を配置することが望ましい。

図１０の検定手段３４は、方位検出手段２４からの方位に基づいて、前記補正パラメータや前記方位を用いた評価関数が所定範囲（所定の閾値）内であるか否かを検定し、前記補正パラメータや前記方位を用いた評価関数が前記所定範囲外である場合（前記閾値を越える場合）には、前記拡張カルマンフィルタの設定を変更し、あるいは、初期化を行う。

また、前記評価関数が前記閾値を越える場合には、方位を出力しないようにする。評価関数としては、具体的に図１０の場合には、下記の（１１）式及び（１２）式で表わされるような、補正パラメータと、方位検出手段２４からの方位と、磁気センサ２２からの２軸のデータとより推定される磁気センサ２２の雑音ｅ_mag(k)や、その２乗和ｒが用いられる。
ｅ_mag(k)＝ｙ_k−ｈ（ｘ_(k)、ψ_E(k)、０、０） （１１）
ｒ＝ｅ_mag(k) ^Tｅ_mag(k) （１２）

ここで、ｙ_k＝［ｘ_ms(k)、ｙ_ms(k)］^Tを示す。

図１１の検定手段３４は、姿勢検出手段３０からの方位、ロール及びピッチに基づいて、前記補正パラメータや前記方位を用いた評価関数が所定範囲（所定の閾値）内であるか否かを検定し、前記補正パラメータや前記方位が前記所定範囲外である場合（前記閾値を越える場合）には、前記拡張カルマンフィルタの設定を変更し、あるいは、初期化を行う。

また、前記評価関数が前記閾値を越える場合には、方位を出力しないようにする。具体的に、図１１の場合には、下記の（１３）式及び（１２）式で表わされるような、補正パラメータと、姿勢検出手段３０からの姿勢と、磁気センサ２２からの２軸のデータとより推定される磁気センサ２２の雑音ｅ_mag(k)や、その２乗和ｒが用いられる。
ｅ_mag(k)＝ｙ_k−ｈ（ｘ_(k)、Φ_E(k)、０、０） （１３）

これにより、補正パラメータ推定手段２６における補正パラメータの推定を円滑に行えるようにし、さらに、方位補正・算出手段２８にて算出された方位の信頼性を向上して、誤った方位を出力しないようにすることが可能となる。

本発明に係る方位計測装置は、上述の実施形態に限らず、本発明の要旨を逸脱することなく、種々の構成を採り得ることは勿論である。

方位検出手段を有する本実施形態に係る方位計測装置のブロック図である。 姿勢検出手段を有する本実施形態に係る方位計測装置のブロック図である。 ２軸のデータが楕円として描かれているグラフである。 船舶の姿勢を説明するための斜視図である。 図１の方位計測装置のフローチャートである。 図２の方位計測装置のフローチャートである。 ２軸のデータにおける定数Ｇ、角度α、β及びオフセットｘ₀、ｙ₀を説明するためのグラフである。 図１の方位計測装置内に周囲環境検出手段を配置したブロック図である。 図２の方位計測装置内に周囲環境検出手段を配置したブロック図である。 図１の方位計測装置内に検定手段を配置したブロック図である。 図２の方位計測装置内に検定手段を配置したブロック図である。 従来技術に係る方位計測装置のブロック図である。

符号の説明

２０…方位計測装置 ２２…磁気センサ
２４…方位検出手段 ２６…補正パラメータ推定手段
２８…方位補正・算出手段 ３０…姿勢検出手段
３２…周囲環境検出手段 ３４…検定手段
３８…ｘ−ｙ平面 ４０…船舶

Claims (4)

1. 移動体における地磁気ベクトルを検出する地磁気ベクトル検出手段と、
   真北に対する前記移動体の方位を検出する方位検出手段と、
   拡張カルマンフィルタから構成され、該拡張カルマンフィルタにおける状態変数ベクトルＸ _ｋ と、前記地磁気ベクトルと、前記方位と、前記地磁気ベクトルに関する補正パラメータとの関係を表す観測方程式を用い、前記拡張カルマンフィルタにより該観測方程式を解くことで前記補正パラメータを推定する補正パラメータ推定手段と、
   前記補正パラメータを用いて前記地磁気ベクトルを補正し、補正した前記地磁気ベクトルに基づいて磁北又は前記真北に対する前記移動体の方位を算出する方位補正・算出手段と、
   を有し、
   前記補正パラメータは、地磁気に関するパラメータである、水平方向の地磁気の強さを表すＡ _ｋ と、偏角を表わすα _ｋ と、前記地磁気ベクトルを示す２軸のデータをｘ−ｙ平面上でプロットしたときに、このプロット図の中心が前記ｘ−ｙ平面の原点からずれた楕円として描かれる場合での該楕円の形状に関するパラメータＧ _ｋ 及びβ _ｋ と、前記楕円の中心位置を表わすｘ _０（ｋ） 及びｙ _０（ｋ） とである
   ことを特徴とする方位計測装置。
2. 請求項１記載の方位計測装置において、
   前記補正パラメータ推定手段は、次の式
   

   

   但し、
   ｘ _{ｍｓ（ｋ）} 及びｙ _{ｍｓ（ｋ）} ：前記地磁気ベクトル検出手段で検出された地磁気の各軸のデータ、
   ｅ _{ｍｓｘ（ｋ）} 及びｅ _{ｍｓｙ（ｋ）} ：前記地磁気ベクトル検出手段の各軸の検出誤差、
   ｅ _{ｍｓ（ｋ）} ＝［ｅ _{ｍｓｘ（ｋ）} 、ｅ _{ｍｓｙ（ｋ）} ］ ^Ｔ 、
   ψ _Ｅ（ｋ） ：前記方位検出手段で検出された方位、
   ｅ _ψ（ｋ） ：前記方位検出手段で検出された方位の検出誤差、
   ｈ（ｘ _（ｋ） 、ψ _Ｅ（ｋ） 、ｅ _{ｍｓ（ｋ）} 、ｅ _ψ（ｋ） ）：前記観測方程式を示す関数、
   を前記観測方程式として用い、前記補正パラメータを推定する
   ことを特徴とする方位計測装置。
3. 移動体における地磁気ベクトルを検出する地磁気ベクトル検出手段と、
   真北に対する前記移動体の方位と前記移動体のロール及びピッチとを含む前記移動体の姿勢を検出する姿勢検出手段と、
   拡張カルマンフィルタから構成され、該拡張カルマンフィルタにおける状態変数ベクトルＸ _ｋ と、前記地磁気ベクトルと、前記姿勢と、前記地磁気ベクトルに関する補正パラメータとの関係を表す観測方程式を用い、前記拡張カルマンフィルタにより該観測方程式を解くことで前記補正パラメータを推定する補正パラメータ推定手段と、
   前記補正パラメータ及び前記姿勢を用いて前記地磁気ベクトルを補正し、補正した前記地磁気ベクトルに基づいて磁北又は前記真北に対する前記移動体の方位を算出する方位補正・算出手段と、
   を有し、
   前記補正パラメータは、地磁気に関するパラメータである、水平方向の地磁気の強さを表すＡ _ｋ と、水平面に対する地磁気の強さの垂直成分Ａ _ｚ（ｋ） と、偏角を表わすα _ｋ と、前記地磁気ベクトルを示す２軸のデータをｘ−ｙ平面上でプロットしたときに、このプロット図の中心が前記ｘ−ｙ平面の原点からずれた楕円として描かれる場合での該楕円の形状に関するパラメータＧ _ｋ 、β _ｋ 及びγ _ｋ と、前記楕円の中心位置を表わすｘ _０（ｋ） 及びｙ _０（ｋ） とである
   ことを特徴とする方位計測装置。
4. 請求項３記載の方位計測装置において、
   前記補正パラメータ推定手段は、次の式
   

   

   但し、
   ｘ _{ｍｓ（ｋ）} 及びｙ _{ｍｓ（ｋ）} ：前記地磁気ベクトル検出手段で検出された地磁気の各軸のデータ、
   ｅ _{ｍｓｘ（ｋ）} 及びｅ _{ｍｓｙ（ｋ）} ：前記地磁気ベクトル検出手段の各軸の検出誤差、
   ｅ _{ｍｓ（ｋ）} ＝［ｅ _{ｍｓｘ（ｋ）} 、ｅ _{ｍｓｙ（ｋ）} ］ ^Ｔ 、
   ψ _Ｅ（ｋ） ：前記姿勢検出手段で検出された方位、
   ｅ _ψ（ｋ） ：前記姿勢検出手段で検出された方位の検出誤差、
   θ _Ｅ（ｋ） ：前記姿勢検出手段で検出されたピッチ、
   ｅ _θ（ｋ） ：前記姿勢検出手段で検出されたピッチの検出誤差、
   φ _Ｅ（ｋ） ：前記姿勢検出手段で検出されたロール、
   ｅ _φ（ｋ） ：前記姿勢検出手段で検出されたロールの検出誤差、
   Φ _Ｅ（ｋ） ＝（ψ _Ｅ（ｋ） 、θ _Ｅ（ｋ） 、φ _Ｅ（ｋ） ）、
   ｅ _Φ（ｋ） ＝（ｅ _ψ（ｋ） 、ｅ _θ（ｋ） 、ｅ _φ（ｋ） ）
   Ｔ _２Ｄ （ψ _Ｅ（ｋ） ＋ｅ _ψ（ｋ） 、θ _Ｅ（ｋ） ＋ｅ _θ（ｋ） 、φ _Ｅ（ｋ） ＋ｅ _φ（ｋ） ）：変換行列、
   ｈ（ｘ _（ｋ） 、Φ _Ｅ（ｋ） 、ｅ _{ｍｓ（ｋ）} 、ｅ _Φ（ｋ） ）：前記観測方程式を示す関数、
   を前記観測方程式として用い、前記補正パラメータを推定する
   ことを特徴とする方位計測装置。

Images