WO2017090360A1 - センサ誤差算出装置、姿勢角算出装置、センサ誤差算出方法、姿勢角算出方法 - Google Patents

Abstract

【課題】磁気センサの誤差を高精度に且つ素早く算出する。 【解決手段】センサ誤差算出装置１は、ＧＮＳＳ姿勢角算出部２０、ＧＮＳＳ地磁気算出部３０１、および、誤差推定部３０２を備える。ＧＮＳＳ姿勢角算出部２０は、ＧＮＳＳの測位信号からＧＮＳＳ姿勢角を算出する。ＧＮＳＳ地磁気算出部３０１は、ＧＮＳＳ姿勢角からＧＮＳＳ地磁気を算出する。誤差推定部３０２は、磁気センサによる磁気検出値とＧＮＳＳ地磁気とを用いて、磁気センサの感度誤差、ミスアライメント誤差、および、バイアス誤差を推定する。

Description

センサ誤差算出装置、姿勢角算出装置、センサ誤差算出方法、姿勢角算出方法

　本発明は、船舶等の移動体に設置された磁気センサの誤差を算出するセンサ誤差算出装置およびセンサ誤差算出方法と、磁気センサの磁気検出値を用いて船首方位等の方位を算出する姿勢角算出装置および姿勢角算出方法に関する。

　磁気センサで地磁気を検出して、移動体の方位を算出する方位算出装置が各種実用化されている。磁気センサは、直交する二軸または三軸の磁界強度を電圧等によって検出する。方位算出装置は、この二軸もしくは三軸の検出値の関係から方位を算出する。

　一般に汎用の磁気センサは、磁気検出値に誤差を含んでいることが知られている。したがって、方位算出装置は、この誤差を補正することによって、算出する方位の精度を向上させようとしている。

　特許文献１では、磁気センサが装着された船舶を１周旋回させて、複数の磁気検出値を取得し、この複数の磁気検出値の軌跡から、磁気検出値に含まれる外乱磁場による誤差を算出している。

　特許文献２では、外部からの標準地磁気データと、衛星信号を用いて算出した船首方位、位置、および、この位置に対応する地磁気データとを用いて、各方位の残留磁気値（磁気バイアス）を算出している。

　特許文献３では、２回分のＧＰＳ方位と、磁気センサの出力値から算出される磁気方位とを用いて、磁気センサの感度誤差とバイアス誤差を算出している。

特開平９－６８４３１号公報 特許第４８１４６０４号明細書 特開平５－１８７７０号公報

　しかしながら、特許文献１に記載の方法では、磁気センサが装着された船舶（移動体）を１周旋回させなければならず、磁気センサの誤差の算出に時間を要する。

　また、特許文献２，３に記載の方法では、誤差の一部しか算出されていないので、誤差を高精度に算出することができない。

　したがって、本発明の目的は、磁気センサの誤差を高精度に且つ素早く算出することができるセンサ誤差算出装置、センサ誤差算出方法、このセンサ誤差を用いて高精度に方位を算出する姿勢角算出装置および姿勢角算出方法を提供することにある。

　この発明のセンサ誤差算出装置は、ＧＮＳＳ姿勢角算出部、ＧＮＳＳ地磁気算出部、および、誤差推定部を備える。ＧＮＳＳ姿勢角算出部は、ＧＮＳＳの測位信号からＧＮＳＳ姿勢角を算出する。ＧＮＳＳ地磁気算出部は、ＧＮＳＳ姿勢角からＧＮＳＳ地磁気を算出する。誤差推定部は、磁気センサによる磁気検出値とＧＮＳＳ地磁気とを用いて、磁気センサの感度誤差、ミスアライメント誤差、および、バイアス誤差を推定する。

　この構成では、感度誤差、ミスアライメント誤差、および、バイアス誤差の無いＧＮＳＳ姿勢角を用いてＧＮＳＳ地磁気が算出される。したがって、このＧＮＳＳ地磁気を用いて誤差を推定することによって、磁気センサの各誤差（較正値）が高精度に推定される。

　また、この発明の姿勢角算出装置は、上述のいずれかに記載のセンサ誤差算出装置の各構成部、誤差補正部、および、方位算出部を備える。誤差補正部は、磁気センサの感度誤差、ミスアライメント誤差、および、バイアス誤差を用いて、磁気検出値を補正する。方位算出部は、誤差補正後の磁気検出値を用いて方位を算出する。

　この構成では、高精度に推定された誤差によって、磁気センサの磁気検出値の誤差が補正され、方位が高精度に算出される。

　この発明によれば、磁気センサの誤差を高精度に且つ素早く算出することができる。したがって、この算出誤差を用いて磁気センサの誤差を補正することで、高精度な方位を素早く算出することができる。

本発明の第１の実施形態に係る姿勢角算出装置の機能ブロック図 本発明の第１の実施形態に係る姿勢角算出方法のフローチャート 本発明の第２の実施形態に係る姿勢角算出装置の機能ブロック図 本発明の第２の実施形態に係るセンサ誤差算出方法のフローチャート 本発明の第３の実施形態に係る姿勢角算出装置の機能ブロック図 本発明の第３の実施形態に係る姿勢角算出方法のフローチャート 本発明の第４の実施形態に係る姿勢角算出装置の機能ブロック図 本発明の第４の実施形態に係る姿勢角算出方法における信頼性判定に関連する処理のフローチャート 本発明の第４の実施形態に係る姿勢角算出処理の信頼性判定の具体的な処理のフローチャート 本発明の第５の実施形態に係る姿勢角算出装置の機能ブロック図 本発明の第６の実施形態に係る姿勢角算出装置の機能ブロック図 本発明の第６の実施形態に係る姿勢角算出処理の信頼性判定のフローチャート 本発明の第７の実施形態に係る姿勢角算出装置の機能ブロック図 本発明の第８の実施形態に係る姿勢角算出装置の機能ブロック図 本発明の第８の実施形態に係る姿勢角算出方法における信頼性判定のフローチャート 本発明の第９の実施形態に係る姿勢角算出装置の機能ブロック図

　本発明の第１の実施形態に係るセンサ誤差算出装置およびセンサ誤差算出方法と、姿勢角算出装置および姿勢角算出方法について、図を参照して説明する。図１は、本発明の第１の実施形態に係る姿勢角算出装置の機能ブロック図である。

　姿勢角算出装置１０は、船舶等の移動体に装着されている。姿勢角算出装置１０は、磁気センサ４０のｘ軸方向（後述するＧＮＳＳ地磁気のｘ軸方向と同じ）が移動体の前後方向（移動体の中心から先頭方向が＋方向）となり、磁気センサ４０のｙ軸方向（後述するＧＮＳＳ地磁気のｙ軸方向と同じ）が移動体の左右方向（移動体の中心から右側を向く方向が＋方向）となるように、移動体に装着されている。ｘ軸方向が本発明の「第１方向」に対応し、ｙ軸方向が本発明の「第２方向」に対応する。なお、移動体は、船舶等の水上を航行するものだけでなく、水中、陸上、空中を移動するものであってもよい。ただし、素早い旋回が容易でない船舶等の移動体に対して、本願発明はより有効である。

　姿勢角算出装置１０は、ＧＮＳＳ姿勢角算出部２０、較正値推定部３０、磁気センサ４０、記憶部５０、誤差補正部６０、および、方位算出部７０を備える。較正値推定部３０は、ＧＮＳＳ地磁気算出部３０１、誤差推定部３０２、および、較正値算出部３０３を備える。ＧＮＳＳ姿勢角算出部２０、ＧＮＳＳ地磁気算出部３０１、および誤差推定部３０２が、センサ誤差算出装置１に対応する。ＧＮＳＳ姿勢角算出部２０と較正値推定部３０のＧＮＳＳ地磁気算出部３０１および誤差推定部３０２とは、センサ誤差算出装置１が備えるハードウェアプロセッサ１１によって実行される。なお、較正値算出部３０３もハードウェアプロセッサ１１によって実行されていてもよい。ＧＮＳＳは、Ｇｒｏｂａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓａｔｅｌｌｉｔｅ　Ｓｙｓｔｅｍｓの略語であり、ＧＰＳ（Ｇｒｏｂａｌ　Ｐｏｓｉｔｉｏｎｉｎｇ　Ｓｙｓｔｅｍ）、ＧＬＯＮＡＳＳ、Ｇａｌｉｌｅｏ等の衛星測位システムを示す。

　ＧＮＳＳ姿勢角算出部２０には、ＧＮＳＳ受信部（図示せず）が接続されており、ＧＮＳＳ受信部にはＧＮＳＳアンテナ（図示せず）が接続されている。ＧＮＳＳ受信部およびＧＮＳＳアンテナは、複数である。複数のＧＮＳＳアンテナは、移動体に対して所定の位置関係で固定されている。複数のＧＮＳＳアンテナは、測位信号を受信してＧＮＳＳ受信部に出力する。ＧＮＳＳ受信部は、測位信号を追尾して、搬送波位相を含む追尾による計測値を、ＧＮＳＳ姿勢角算出部２０に出力する。また、ＧＮＳＳ受信部は、測位信号の追尾において、測位信号から航法メッセージを復調して、ＧＮＳＳ姿勢角算出部２０に出力する。

　ＧＮＳＳ姿勢角算出部２０は、測位信号の追尾による計測値および航法メッセージを用いて、既知の方法によって姿勢角（ＧＮＳＳ姿勢角）を算出する。ＧＮＳＳ姿勢角は、ロール角φ_ＧＮＳＳ、ピッチ角θ_ＧＮＳＳ、および、方位角（ヨー角）ψ_ＧＮＳＳの三成分からなる。ＧＮＳＳ姿勢角算出部２０は、ＧＮＳＳ姿勢角を較正値推定部３０に出力する。

　磁気センサ４０は、移動体に固定されている。磁気センサ４０は、地磁気を含む外部磁気を検出し、直交二軸の成分である磁気検出値［ｍ_ＭＡＧｘ，ｍ_ＭＡＧｙ］を較正値推定部３０および誤差補正部６０に出力する。

　較正値推定部３０は、ＧＮＳＳ姿勢角［φ_ＧＮＳＳ，θ_ＧＮＳＳ，ψ_ＧＮＳＳ］、および、磁気検出値［ｍ_ＭＡＧｘ，ｍ_ＭＡＧｙ］を用いて、磁気センサ４０の磁気検出値［ｍ_ＭＡＧｘ，ｍ_ＭＡＧｙ］に含まれる感度誤差、ミスアライメント誤差、および、バイアス誤差のそれぞれに対する較正値を推定する。較正値推定部３０は、少なくとも三回のＧＮＳＳ姿勢角および磁気検出値を用いて較正値を推定する。この三回は、それぞれに移動体の姿勢が異なる。

　具体的には、図１（Ｂ）に示すように、較正値推定部３０は、ＧＮＳＳ地磁気算出部３０１、誤差推定部３０２、および、較正値算出部３０３を備える。較正値推定部３０は、次に示す処理を実行することによって較正値を推定する。較正値推定部３０のＧＮＳＳ地磁気算出部３０１は、次の（式１）、（式２）を用いて、ＧＮＳＳ姿勢角［φ_ＧＮＳＳ，θ_ＧＮＳＳ，ψ_ＧＮＳＳ］から、ＧＮＳＳ姿勢角によるＧＮＳＳ地磁気［ｍ_{ＧＮＳＳｘ}，ｍ_{ＧＮＳＳｙ}］を算出する。

　較正値推定部３０の誤差算出部３０２は、次の（式３）を用いて、磁気センサの各誤差（感度誤差、ミスアライメント誤差、および、バイアス誤差）を推定する。

　ここで、［α_ｘｘ，α_ｙｙ］は、磁気センサ４０のｘ軸方向およびｙ軸方向の感度誤差とする。［α_ｘｙ，α_ｙｘ］は、磁気センサ４０のｘ軸方向およびｙ軸方向のミスアライメント誤差とする。［Δｍ_ｘ，Δｍ_ｙ］は、磁気センサ４０のｘ軸方向およびｙ軸方向のバイアス誤差とする。

　（式３）に用いるＧＮＳＳ姿勢角によるＧＮＳＳ地磁気［ｍ_{ＧＮＳＳｘ}，ｍ_{ＧＮＳＳｙ}］は、観測値であるＧＮＳＳ姿勢角［φ_ＧＮＳＳ，θ_ＧＮＳＳ，ψ_ＧＮＳＳ］を（式１）、（式２）に代入することによって算出される。

　較正値推定部３０の較正値算出部３０３は、（式３）によって推定された感度誤差およびミスアライメント誤差から、磁気センサ４０に対する感度・ミスアライメント較正行列ＭＭを、次の（式４）を用いて算出する。

　この際、較正値推定部３０の較正値算出部３０３は、大きさを「１」にする換算係数、および、磁気偏差を補正するための換算係数を含むように、感度・ミスアライメント較正行列ＭＭを設定している。

　較正値推定部３０は、感度・ミスアライメント較正行列ＭＭとバイアス誤差［Δｍ_ｘ，Δｍ_ｙ］とを較正値として、記憶部５０に出力する。

　記憶部５０は、較正値、すなわち、感度・ミスアライメント較正行列ＭＭとバイアス誤差［Δｍ_ｘ，Δｍ_ｙ］とを記憶している。記憶部５０は、新たな較正値が入力される毎に、記憶されている較正値を新たな較正値によって上書きして記憶する。

　誤差補正部６０は、記憶部５０から較正値を取得する。誤差補正部６０は、次の（式５）を用い、磁気センサ４０の磁気検出値［ｍ_ＭＡＧｘ，ｍ_ＭＡＧｙ］を較正値によって補正する。

　［ｍ^ｂ _ＭＡＧｘ，ｍ^ｂ _ＭＡＧｙ］は、誤差補正後の磁気検出値である。

　誤差補正部６０は、誤差補正後の磁気検出値［ｍ^ｂ _ＭＡＧｘ，ｍ^ｂ _ＭＡＧｙ］を方位算出部７０に出力する。

　方位算出部７０は、誤差補正後の磁気検出値［ｍ^ｂ _ＭＡＧｘ，ｍ^ｂ _ＭＡＧｙ］を用いて、既知の方法で方位ψ_ＭＡＧを算出する。概略的には、例えば、方位算出部７０は、誤差補正後の検出値のｘ軸成分ｍ^ｂ _ＭＡＧｘとｙ軸成分ｍ^ｂ _ＭＡＧｙとの比、および、磁気センサ４０のｘ軸、ｙ軸と移動体の先端方位（例えば、船舶の船首方位）との関係を用いて、方位ψ_ＭＡＧを算出する。

　具体的な一例として、磁気センサ４０のｙ軸の＋方向と船首方位とを一致させて磁気センサ４０を船舶に設置した場合、方位ψ_ＭＡＧは、次の（式６）を用いて算出する。

　このように、本実施形態の構成では、感度誤差、ミスアライメント誤差、およびバイアス誤差の影響を受けないＧＮＳＳの測位信号を用いたＧＮＳＳ姿勢角から地磁気を算出し、このＧＮＳＳ姿勢角による地磁気と磁気センサ４０の磁気検出値とから、磁気センサ４０の感度誤差、ミスアライメント誤差、および、バイアス誤差を推定する。したがって、磁気センサ４０の感度誤差、ミスアライメント誤差、および、バイアス誤差、すなわち、磁気センサ４０の誤差を構成する全ての誤差成分を、高精度に推定することができる。この際、三回の観測によるＧＮＳＳ姿勢角と磁気センサの磁気検出値によって誤差を算出できるので、磁気センサ４０の磁気検出値に含まれる誤差を素早く算出することができる。

　また、このように高精度に算出された誤差を用いて補正された磁気検出値を用いることによって、方位を高精度に算出することができる。

　なお、ＧＮＳＳ地磁気［ｍ_{ＧＮＳＳｘ}，ｍ_{ＧＮＳＳｙ}］を算出する際に、ＧＮＳＳ姿勢角のロール角φ_ＧＮＳＳおよびピッチ角θ_ＧＮＳＳが小さい場合に、一次近似が可能であり、（式１）、（式２）は、次の（式１'）、（式２'）に置き換えることが可能である。

　上述の姿勢角算出装置１０は、図１に示すように、それぞれ個別の機能ブロックで各処理を実行している。しかしながら、上述の誤差算出および方位算出の各処理をプログラム化して記憶媒体に記憶しておき、コンピュータ等の情報処理装置が記憶媒体からこのプログラムを読み出して実行してもよい。この場合、図２に示す処理を行えばよい。図２は、本発明の第１の実施形態に係る姿勢角算出方法のフローチャートである。

　情報処理装置は、ＧＮＳＳの測位信号の追尾による計測値を用いて、ＧＮＳＳ姿勢角を算出する（Ｓ１０１）。情報処理装置は、ＧＮＳＳ姿勢角から、上述の（式１）、（式２）を用いて、ＧＮＳＳ地磁気を算出する（Ｓ１０２）。情報処理装置は、磁気センサから磁気検出値を取得する（Ｓ１０３）。

　情報処理装置は、三回分のＧＮＳＳ地磁気と磁気検出値と（式３）とを用いて、磁気センサの較正値（感度・ミスアライメント較正行列とバイアス誤差）を推定する（Ｓ１０４）。ここまでの処理が、本発明の「センサ誤差算出方法」の処理に対応する。

　情報処理装置は、較正値と（式５）を用いて、磁気検出値の誤差を補正する（Ｓ１０５）。情報処理装置は、誤差補正後の磁気検出値を用いて方位を算出する（Ｓ１０６）。

　次に、本発明の第２の実施形態に係るセンサ誤差算出装置およびセンサ誤差算出方法と、姿勢角算出装置および姿勢角算出方法について、図を参照して説明する。図３は、本発明の第２の実施形態に係る姿勢角算出装置の機能ブロック図である。

　本実施形態に係る姿勢角算出装置１０Ａは、第１の実施形態に係る姿勢角算出装置１０に対して、ＧＮＳＳ位置算出部２１が追加されたものであり、較正値推定部３０Ａの処理が較正値推定部３０と異なるものである。

　ＧＮＳＳ位置算出部２１は、ＧＮＳＳ姿勢角算出部２０が接続するＧＮＳＳ受信部（図示せず）に接続されている。ＧＮＳＳ位置算出部２１には、測位信号の追尾による計測値であるコード擬似距離、および、航法メッセージが入力されている。

　ＧＮＳＳ位置算出部２１は、コード擬似距離と、航法メッセージに含まれる測位衛星の位置情報を用いて、既知の方法でＧＮＳＳ位置を算出する。ＧＮＳＳ位置算出部２１は、ＧＮＳＳ位置を較正値推定部３０Ａに出力する。

　較正値推定部３０Ａは、ＧＮＳＳ位置から緯度Ｌａｔを算出する。較正値推定部３０Ａは、次の（式７）、（式８）を用いて、ＧＮＳＳ姿勢角［φ_ＧＮＳＳ，θ_ＧＮＳＳ，ψ_ＧＮＳＳ］から、ＧＮＳＳ姿勢角によるＧＮＳＳ地磁気［ｍ_{ＧＮＳＳｘ}，ｍ_{ＧＮＳＳｙ}］を算出する。

　ここで、ｆｘ（Ｌａｔ，θ_ＧＮＳＳ）は、ＧＮＳＳ地磁気のｘ軸方向に対する動揺補正項であり、緯度Ｌａｔおよびピッチ角θ_ＧＮＳＳによって決定される関数値である。また、ｆｙ（Ｌａｔ，θ_ＧＮＳＳ，φ_ＧＮＳＳ）は、ＧＮＳＳ地磁気のｙ軸方向に対する動揺補正項であり、緯度Ｌａｔ、ピッチ角θ_ＧＮＳＳおよびロール角φ_ＧＮＳＳによって決定される関数値である。

　較正値推定部３０Ａは、（式７）、（式８）から算出したＧＮＳＳ地磁気［ｍ_{ＧＮＳＳｘ}，ｍ_{ＧＮＳＳｙ}］を、（式３）に適用することによって、感度誤差［α_ｘｘ，α_ｙｙ］、ミスアライメント誤差［α_ｘｙ，α_ｙｘ］、および、バイアス誤差［Δｍ_ｘ，Δｍ_ｙ］を推定し、較正値を算出する。

　このような構成および処理によって、感度誤差［α_ｘｘ，α_ｙｙ］、ミスアライメント誤差［α_ｘｙ，α_ｙｘ］、および、バイアス誤差［Δｍ_ｘ，Δｍ_ｙ］に含まれる移動体（磁気センサ）の動揺の影響を抑圧することができる。これにより、感度誤差［α_ｘｘ，α_ｙｙ］、ミスアライメント誤差［α_ｘｙ，α_ｙｘ］、および、バイアス誤差［Δｍ_ｘ，Δｍ_ｙ］をさらに高精度に推定でき、高精度な較正値を算出することができる。そして、この高精度な較正値を用いることによって、方位を高精度に算出することができる。

　上述の姿勢角算出装置１０Ａは、図３に示すように、それぞれ個別の機能ブロックで各処理を実行している。しかしながら、上述の誤差算出および方位算出の各処理をプログラム化して記憶媒体に記憶しておき、コンピュータ等の情報処理装置が記憶媒体からこのプログラムを読み出して実行してもよい。この場合、図４に示す処理を行えばよい。図４は、本発明の第２の実施形態に係るセンサ誤差算出方法のフローチャートである。

　情報処理装置は、ＧＮＳＳの測位信号の追尾による計測値を用いて、ＧＮＳＳ姿勢角を算出する（Ｓ１０１）。情報処理装置は、ＧＮＳＳの測位信号の追尾による計測値を用いて、ＧＮＳＳ位置を算出する（Ｓ１１１）。情報処理装置は、ＧＮＳＳ姿勢角から、上述の（式７）、（式８）を用いて、動揺補正付きのＧＮＳＳ地磁気を算出する（Ｓ１１２）。情報処理装置は、磁気センサから検出値（センサ値）を取得する（Ｓ１０３）。

　情報処理装置は、（式７）、（式８）によって算出された三回分のＧＮＳＳ地磁気とセンサ値と（式３）とを用いて、磁気センサの較正値（感度・ミスアライメント較正行列とバイアス誤差）を推定する（Ｓ１０４）。なお、方位算出処理は、図２に示す第１の実施形態のステップＳ１０５，Ｓ１０６と同じである。

　次に、本発明の第３の実施形態に係る姿勢角算出装置および姿勢角算出方法について、図を参照して説明する。図５は、本発明の第３の実施形態に係る姿勢角算出装置の機能ブロック図である。

　本実施形態に係る姿勢角算出装置１０Ｂは、第２の実施形態に係る姿勢角算出装置１０Ａに対して、動揺補正部６１が追加されたものである。較正値推定部３０Ｂは、第２の実施形態に係る較正値推定部３０Ａと同じである。

　図５に示すように、動揺補正部６１には、ＧＮＳＳ姿勢角算出部２０からＧＮＳＳ姿勢角が入力される。動揺補正部６１には、ＧＮＳＳ位置算出部２１からＧＮＳＳ位置が入力される。動揺補正部６１には、誤差補正部６０から誤差補正後の磁気検出値［ｍ^ｂ _ＭＡＧｘ，ｍ^ｂ _ＭＡＧｙ］が入力される。

　動揺補正部６１は、ＧＮＳＳ位置から緯度Ｌａｔを算出する。動揺補正部６１は、次の（式９）と、ＧＮＳＳ姿勢角［φ_ＧＮＳＳ，θ_ＧＮＳＳ，ψ_ＧＮＳＳ］と、緯度Ｌａｔとを用いて、誤差補正後の磁気検出値［ｍ^ｂ _ＭＡＧｘ，ｍ^ｂ _ＭＡＧｙ］を動揺補正する。

　［ｍ^ｈ _ＭＡＧｘ，ｍ^ｈ _ＭＡＧｙ］は、動揺補正後の検出値である。

　なお、ＧＮＳＳ姿勢角のロール角φ_ＧＮＳＳ、ピッチ角θ_ＧＮＳＳが１次近似可能な角度範囲であった場合には、次の（式１０）を用いて、動揺補正後の磁気検出値［ｍ^ｈ _ＭＡＧｘ，ｍ^ｈ _ＭＡＧｙ］を算出してもよい。

　動揺補正部６１は、動揺補正後の磁気検出値［ｍ^ｈ _ＭＡＧｘ，ｍ^ｈ _ＭＡＧｙ］を方位算出部７０に出力する。

　方位算出部７０は、動揺補正後の磁気検出値［ｍ^ｈ _ＭＡＧｘ，ｍ^ｈ _ＭＡＧｙ］を用いて、第１の実施形態と同様の方法で方位を算出する。

　このような構成を用いることによって、磁気センサ４０の検出値に含まれる移動体（磁気センサ）の動揺の影響を抑圧することができる。したがって、この構成を用いることによって、さらに高精度な方位を算出することができる。

　上述の姿勢角算出装置１０Ｂは、図５に示すように、それぞれ個別の機能ブロックで各処理を実行している。しかしながら、上述の誤差算出および方位算出の各処理をプログラム化して記憶媒体に記憶しておき、コンピュータ等の情報処理装置が記憶媒体からこのプログラムを読み出して実行してもよい。この場合、図６に示す処理を行えばよい。図６は、本発明の第３の実施形態に係る姿勢角算出方法のフローチャートである。

　情報処理装置は、ＧＮＳＳの測位信号の追尾による計測値を用いて、ＧＮＳＳ姿勢角を算出する（Ｓ１０１）。情報処理装置は、ＧＮＳＳの測位信号の追尾による計測値を用いて、ＧＮＳＳ位置を算出する（Ｓ１１１）。情報処理装置は、ＧＮＳＳ位置に基づく緯度とＧＮＳＳ姿勢角とから、上述の（式７）、（式８）を用いて、動揺補正付きのＧＮＳＳ地磁気を算出する（Ｓ１１２）。情報処理装置は、磁気センサから検出値（センサ値）を取得する（Ｓ１０３）。

　情報処理装置は、（式７）、（式８）によって算出された三回分のＧＮＳＳ地磁気とセンサ値と（式３）とを用いて、磁気センサの較正値（感度・ミスアライメント較正行列とバイアス誤差）を推定する（Ｓ１０４）。

　情報処理装置は、較正値と（式５）を用いて、センサ値の誤差を補正する（Ｓ１０５）。情報処理装置は、ＧＮＳＳ姿勢角、緯度、および（式１０）を用いて、誤差補正後のセンサ値に対する動揺補正を行う（Ｓ１２１）。情報処理装置は、動揺補正後のセンサ値を用いて方位を算出する（Ｓ１２２）。

　次に、本発明の第４の実施形態に係る姿勢角算出装置および姿勢角算出方法について、図を参照して説明する。図７は、本発明の第４の実施形態に係る姿勢角算出装置の機能ブロック図である。

　本実施形態に係る姿勢角算出装置１０Ｃは、第３の実施形態に係る姿勢角算出装置１０Ｂに対して、整数値バイアス決定部２２、および、信頼性判定部８０が追加されたものである。また、姿勢角算出装置１０Ｃは、上述の各実施形態の姿勢角算出装置１０，１０Ａ，１０Ｂで図示しなかったＧＮＳＳ受信部２３１，２３２を備える。較正値推定部３０Ｃは、第３の実施形態に係る較正値推定部３０Ｂと同じである。

　ＧＮＳＳ受信部２３１，２３２は、それぞれに接続されたＧＮＳＳアンテナで受信した測位信号を追尾する。ＧＮＳＳ受信部２３１，２３２は、搬送波位相、コード擬似距離をＧＮＳＳ位置算出部２１に出力する。ＧＮＳＳ受信部２３１，２３２は、搬送波位相を整数値バイアス決定部２２に出力する。

　整数値バイアス決定部２２は、後述の信頼性判定部８０から出力される方位ψ_ＭＡＧを用いて、ＧＮＳＳ受信部２３１，２３２で受信する測位信号の搬送波位相の差（アンテナ間一重位相差）の整数値バイアスを決定する。このように、方位を用いることによって、整数値バイアスを容易に決定することができる。整数値バイアス決定部２２は、整数値バイアスを、搬送波位相とともに、ＧＮＳＳ姿勢角算出部２０に出力する。ＧＮＳＳ姿勢角算出部２０は、この整数値バイアスと搬送波位相を用いて、ＧＮＳＳ姿勢角を算出する。

　なお、整数値バイアス決定部２２は、後述のように信頼性判定部８０から方位ψ_ＭＡＧが入力されなければ（方位ψ_ＭＡＧの信頼性が無ければ）、既知のＧＮＳＳによる整数値バイアスの決定処理を実行する。

　記憶部５０Ｃには、較正値とともに、ＧＮＳＳ位置、ＧＮＳＳ姿勢角が記憶されている。なお、記憶部５０Ｃには、較正値とともに、少なくともＧＮＳＳ姿勢角の方位ψ_ＧＮＳＳが記憶されていればよい。この記憶部５０Ｃに記憶された方位が本実施形態におけるバックアップ方位である。

　信頼性判定部８０は、記憶部５０Ｃからバックアップ方位を取得する。信頼性判定部８０は、方位算出部７０が算出した方位ψ_ＭＡＧとバックアップ方位（ＧＮＳＳ姿勢角の方位ψ_ＧＮＳＳ）とが一致していると判定すると、方位ψ_ＭＡＧおよびバックアップ方位は信頼性が有ると判定する。一方、信頼性判定部８０は、方位算出部７０が算出した方位ψ_ＭＡＧとバックアップ方位（ＧＮＳＳ姿勢角の方位ψ_ＧＮＳＳ）とが一致していないと判定すると、方位ψ_ＭＡＧおよびバックアップ方位は信頼性が無いと判定する。

　具体的な判定方法として、信頼性判定部８０は、方位ψ_ＭＡＧとバックアップ方位（ＧＮＳＳ姿勢角の方位ψ_ＧＮＳＳ）との差Δψを算出し、この差Δψが閾値ψ_ＴＨ以下であれば、方位ψ_ＭＡＧとバックアップ方位（ＧＮＳＳ姿勢角の方位ψ_ＧＮＳＳ）とが一致していると判定する。一方、信頼性判定部８０は、この差Δψが閾値ψ_ＴＨよりも大きければ、方位ψ_ＭＡＧとバックアップ方位（ＧＮＳＳ姿勢角の方位ψ_ＧＮＳＳ）とが一致していないと判定する。なお、閾値ψ_ＴＨは、１５°から３０°程度であればよい。

　信頼性判定部８０は、信頼性有りと判定すると、方位ψ_ＭＡＧを外部および整数値バイアス決定部２２に出力する。一方、信頼性判定部８０は、信頼性無しと判定すると、方位ψ_ＭＡＧを出力しない。この際、信頼性判定部８０は、通知部（図示しない表示部、放音部等）を介して信頼性の有無を外部に通知してもよい。

　このような構成を用いることによって、信頼性の高い方位のみを利用者に提供することができる。また、信頼性の高い方位のみを用いて整数値バイアスが決定されるので、ＧＮＳＳ姿勢角の初期演算を、正確且つ高速に行うことができる。

　上述の姿勢角算出装置１０Ｃの信頼性判定部８０は、具体的な処理の一例として図８、図９に示す処理を行っている。図８は、本発明の第４の実施形態に係る姿勢角算出方法における信頼性判定に関連する処理のフローチャートである。

　図８に示すように、方位算出部７０は、動揺補正後のセンサ値を用いて方位ψ_ＭＡＧを算出する（Ｓ２０１）。信頼性判定部８０は、方位ψ_ＭＡＧの信頼性を判定する（Ｓ２０２）。信頼性判定部８０は、信頼性有りと判定すると（Ｓ２０３：ＹＥＳ）、方位ψ_ＭＡＧを整数値バイアスの決定に利用するとともに（Ｓ２０４）、当該方位ψ_ＭＡＧを外部に出力する（Ｓ２０５）。信頼性判定部８０は、信頼性無しと判定すると（Ｓ２０３：ＮＯ）、当該方位ψ_ＭＡＧを出力しない（Ｓ２０６）。

　図９は、本発明の第４の実施形態に係る姿勢角算出処理の信頼性判定の具体的な処理のフローチャートである。

　図９に示すように、信頼性判定部８０は、記憶部５０Ｃからバックアップ方位を取得する（Ｓ３０２）。信頼性判定部８０は、方位算出部７０で算出した方位ψ_ＭＡＧとバックアップ方位とを比較する（Ｓ３０３）。

　信頼性判定部８０は、方位ψ_ＭＡＧとバックアップ方位とが一致していれば（Ｓ３０４：ＹＥＳ）、方位ψ_ＭＡＧとバックアップ方位とは信頼性が有ると判定する（Ｓ３０５）。ここで、方位ψ_ＭＡＧとバックアップ方位とが一致している状態とは、上述の方位ψ_ＭＡＧとバックアップ方位との差Δψが閾値ψ_ＴＨ以下であることを示す。

　信頼性判定部８０は、方位ψ_ＭＡＧとバックアップ方位とが一致していなければ（Ｓ３０４：ＮＯ）、方位ψ_ＭＡＧとバックアップ方位とは信頼性が無いと判定する（Ｓ３０６）。

　次に、本発明の第５の実施形態に係る姿勢角算出装置および姿勢角算出方法について、図を参照して説明する。図１０は、本発明の第５の実施形態に係る姿勢角算出装置の機能ブロック図である。

　本実施形態に係る姿勢角算出装置１０Ｄは、記憶部５０Ｄおよび信頼性判定部８０Ｄにおいて、第４の実施形態に係る姿勢角算出装置１０Ｃと異なる。較正値推定部３０Ｄは、第４の実施形態に係る較正値推定部３０Ｃと同じである。

　記憶部５０Ｄは、較正値とともに、信頼性判定部８０Ｄから出力された方位ψ_ＭＡＧをバックアップ方位として記憶している。信頼性判定部８０Ｄは、方位算出部７０で算出された方位ψ_ＭＡＧとバックアップ方位とを比較して、信頼性の判定を行う。ここで用いるバックアップ方位は、記憶部５０Ｄに記憶されている過去に算出された方位ψ_ＭＡＧである。

　このような構成を用いることによって、ＧＮＳＳ姿勢角が算出できないような環境になっても、方位の信頼性を判定することができる。

　次に、本発明の第６の実施形態に係る姿勢角算出装置および姿勢角算出方法について、図を参照して説明する。図１１は、本発明の第６の実施形態に係る姿勢角算出装置の機能ブロック図である。

　本実施形態に係る姿勢角算出装置１０Ｅは、信頼性判定部８０Ｅにおいて、第５の実施形態に係る姿勢角算出装置１０Ｄと異なる。較正値推定部３０Ｅは、第５の実施形態に係る較正値推定部３０Ｄと同じであり、記憶部５０Ｅは、第５の実施形態に係る記憶部５０Ｄに対して、ＧＮＳＳ位置をバックアップ位置としてさらに記憶する機能を有する。

　信頼性判定部８０Ｅは、次の処理を実行することによって信頼性を判定する。図１２は、本発明の第６の実施形態に係る姿勢角算出処理の信頼性判定のフローチャートである。

　信頼性判定部８０Ｅは、方位算出部７０で算出された方位ψ_ＭＡＧと、ＧＮＳＳ位置算出部２１で算出されたＧＮＳＳ位置を取得する（Ｓ３１１）。信頼性判定部８０Ｅは、記憶部５０Ｅからバックアップ方位とバックアップ位置を取得する（Ｓ３１２）。

　信頼性判定部８０Ｅは、ＧＮＳＳ位置とバックアップ位置を比較する（Ｓ３１３）。信頼性判定部８０Ｅは、ＧＮＳＳ位置とバックアップ位置とが一致すれば（Ｓ３１４：ＹＥＳ）、ステップＳ３１５の処理に進む。例えば、信頼性判定部８０Ｅは、ＧＮＳＳ位置とバックアップ位置との差（各軸方向における座標の差）が閾値以内であれば、ＧＮＳＳ位置とバックアップ位置が一致すると判定する。信頼性判定部８０Ｅは、ＧＮＳＳ位置とバックアップ位置とが一致しなければ（Ｓ３１４：ＮＯ）、算出された方位ψ_ＭＡＧは信頼性無しと判定する（Ｓ３１８）。例えば、信頼性判定部８０Ｅは、ＧＮＳＳ位置とバックアップ位置との差（各軸方向における座標の差）が閾値より大きければ、ＧＮＳＳ位置とバックアップ位置が一致しないと判定する。

　信頼性判定部８０Ｅは、方位ψ_ＭＡＧとバックアップ方位とを比較する（Ｓ３１５）。信頼性判定部８０Ｅは、方位ψ_ＭＡＧとバックアップ方位とが一致すれば（Ｓ３１６：ＹＥＳ）、算出された方位ψ_ＭＡＧは信頼性有りと判定する（Ｓ３１７）。信頼性判定部８０Ｅは、方位ψ_ＭＡＧとバックアップ方位とが一致しなければ（Ｓ３１６：ＮＯ）、算出された方位ψ_ＭＡＧは信頼性無しと判定する（Ｓ３１８）。方位ψ_ＭＡＧとバックアップ方位とが一致するか否かの判定は、上述の実施形態と同様の方法を用いればよい。

　このような構成および処理を用いることによって、位置も含めて、方位ψ_ＭＡＧの信頼性を判定することができる。例えば、ＧＮＳＳ位置（現在の位置に対応）とバックアップ位置（過去の位置に対応）が大きく異なる場合、略同一経度上でなければ当然に方位も異なる。したがって、このような場合には、方位の比較を行わなくても、方位の信頼性は高いとは言えず、方位の信頼性を容易に判定することができる。

　次に、本発明の第７の実施形態に係る姿勢角算出装置および姿勢角算出方法について、図を参照して説明する。図１３は、本発明の第７の実施形態に係る姿勢角算出装置の機能ブロック図である。

　本実施形態に係る姿勢角算出装置１０Ｆは、方位を整数値バイアスの算出に利用する処理が第６の実施形態に係る姿勢角算出装置１０Ｅと異なる。較正値推定部３０Ｆおよび記憶部５０Ｆは、第６の実施形態に係る姿勢角算出装置１０Ｅの較正値推定部３０Ｅおよび記憶部５０Ｅと同じである。

　信頼性判定部８０Ｆは、方位ψ_ＭＡＧを出力するとともに、方位ψ_ＭＡＧの信頼性の有無を出力する。整数値バイアス決定部２２Ｆは、方位ψ_ＭＡＧに信頼性が有れば、方位ψ_ＭＡＧを整数値バイアスの決定に利用する。整数値バイアス決定部２２Ｆは、方位ψ_ＭＡＧに信頼性が無ければ、既知のＧＮＳＳによる整数値バイアスの決定処理を実行する。

　これにより、方位を常時出力する態様であっても、信頼性の有る方位のみを用いて整数値バイアスを決定するので、整数値バイアスを正確に且つ素早く決定することができる。

　次に、本発明の第８の実施形態に係る姿勢角算出装置および姿勢角算出方法について、図を参照して説明する。図１４は、本発明の第８の実施形態に係る姿勢角算出装置の機能ブロック図である。

　本実施形態に係る姿勢角算出装置１０Ｇは、本発明の第４の実施形態に係る姿勢角算出装置１０Ｃに対して、ＧＮＳＳ姿勢角に替えて統合姿勢角を用いる点で異なる。姿勢角算出装置１０Ｇの基本的な処理は、本発明の第４の実施形態に係る姿勢角算出装置１０Ｃと同じであり、以下では異なる箇所を詳細に説明する。

　統合姿勢角とは、測位信号によって得られる搬送波位相等の計測値と、ＩＭＵセンサ４１が計測するＩＭＵセンサ計測値とを用いて算出される姿勢角である。

　姿勢角算出装置１０Ｇは、統合算出部１００、ＧＮＳＳ受信部２３１，２３２、較正値推定部３０Ｇ、磁気センサ４０、ＩＭＵセンサ４１、記憶部５０Ｇ、誤差補正部６０、動揺補正部６１Ｇ、方位算出部７０、および、信頼性判定部８０Ｇを備える。

　ＧＮＳＳ受信部２３１，２３２は、測位信号を追尾して、搬送波位相等の追尾による計測値を取得し、統合算出部１００に出力する。

　ＩＭＵセンサ４１は、加速度センサおよび角速度センサ等の慣性センサの少なくとも１つを備えている。ＩＭＵセンサ４１は、計測したセンサ値（ＩＭＵセンサ値）を統合算出部１００に出力する。

　統合算出部１００は、追尾による計測値とＩＭＵセンサ値とを用いて、既知の方法によって、統合姿勢角を算出する。統合算出部１００は、較正値推定部３０Ｇおよび信頼性判定部８０Ｇに統合姿勢角を出力する。統合算出部１００は、追尾による計測値を用いて、既知の方法によって、統合位置を算出する。統合算出部１００は、統合位置を、記憶部５０Ｇおよび信頼性判定部８０Ｇに出力する。記憶部５０Ｇは、統合位置をバックアップ位置として記憶する。

　磁気センサ４０は、磁気検出値を、較正値推定部３０Ｇおよび誤差補正部６０に出力する。

　較正値推定部３０Ｇは、統合姿勢角から地磁気を算出し、この統合姿勢角による地磁気と磁気センサの検出値を用いて、磁気センサ４０の感度誤差、ミスアライメント誤差、バイアス誤差を推定し、較正値を算出する。較正値推定部３０Ｇは、較正値を記憶部５０Ｇに出力する。

　誤差補正部６０は、記憶部５０Ｇから較正値を取得して、当該較正値を用いて磁気センサの検出値を補正する。動揺補正部６１Ｇは、誤差補正後の磁気センサの検出値を、統合姿勢角を用いて動揺補正する。方位算出部７０は、動揺補正後の磁気センサの検出値を用いて方位を算出する。

　信頼性判定部８０Ｇは、記憶部５０Ｇから取得したバックアップ方位を用いて、磁気センサの検出値を用いて算出した方位の信頼性判定を行う。この際、上述の実施形態に示すＧＮＳＳ位置を用いた場合のように、統合位置を用いてもよい。さらには、統合位置に基づく統合速度を用いてもよい。信頼性判定部８０Ｇは、方位の信頼性が有れば、当該方位を外部に出力するとともに、記憶部５０Ｇおよび統合算出部１００に出力する。記憶部５０Ｇは、この方位をバックアップ方位として記憶する。統合算出部１００は、この方位を用いて整数値バイアスを決定する。

　図１５は、本発明の第８の実施形態に係る姿勢角算出方法における信頼性判定のフローチャートである。

　信頼性判定部８０Ｇは、統合算出部１００で算出された統合位置と、記憶部５０Ｇに記憶されたバックアップ位置およびバックアップ方位を取得する（Ｓ３２１）。

　信頼性判定部８０Ｇは、統合位置とバックアップ位置を比較する（Ｓ３２２）。信頼性判定部８０Ｇは、統合位置とバックアップ位置とが一致すれば（Ｓ３２３：ＹＥＳ）、ステップＳ３２４の処理に進む。信頼性判定部８０Ｇは、統合位置とバックアップ位置とが一致しなければ（Ｓ３２３：ＮＯ）、算出された方位ψ_ＭＡＧは信頼性無しと判定する（Ｓ３２９）。

　信頼性判定部８０Ｇは、方位ψ_ＭＡＧとバックアップ方位とを比較する（Ｓ３２４）。信頼性判定部８０Ｇは、方位ψ_ＭＡＧとバックアップ方位とが一致すれば（Ｓ３２５：ＹＥＳ）、ステップＳ３２６の処理に進む。信頼性判定部８０Ｇは、方位ψ_ＭＡＧとバックアップ方位とが一致しなければ（Ｓ３２５：ＮＯ）、算出された方位ψ_ＭＡＧは信頼性無しと判定する（Ｓ３２９）。

　信頼性判定部８０Ｇは、複数時刻のＧＮＳＳ位置からＧＮＳＳ速度を算出し、ＧＮＳＳ速度による停泊判定を行う（Ｓ３２６）。信頼性判定部８０Ｇは、ＧＮＳＳ速度が略「０」であれば停泊中であると判定し、ＧＮＳＳ速度が略「０」でなければ停泊中でないと判定する。なお、速度は他の方法で取得してもよい。例えば、加速度の積算値等を用いることも可能である。

　信頼性判定部８０Ｇは、停泊中であれば（Ｓ３２７：ＹＥＳ）、算出された方位ψ_ＭＡＧは信頼性有りと判定する（Ｓ３２８）。信頼性判定部８０Ｇは、停泊中でなければ（Ｓ３２７：ＮＯ）、算出された方位ψ_ＭＡＧは信頼性無しと判定する（Ｓ３１８）。

　なお、本実施形態で示す統合姿勢角および統合位置は、他の実施形態におけるＧＮＳＳ姿勢角およびＧＮＳＳ位置に置き換えて利用することも可能である。

　次に、本発明の第９の実施形態に係る姿勢角算出装置および姿勢角算出方法について、図を参照して説明する。図１６は、本発明の第９の実施形態に係る姿勢角算出装置の機能ブロック図である。

　本実施形態に係る姿勢角算出装置１０Ｈは、上述の各実施形態に示す較正値の推定を行わずに、信頼性判定のみを行うものである。また、姿勢角算出装置１０Ｈは、磁気センサによる方位を統合姿勢角に用いている。

　姿勢角算出装置１０Ｈは、統合算出部１００Ｈ、ＧＮＳＳ受信部２３１，２３２、磁気センサ４０、ＩＭＵセンサ４１、記憶部５０Ｈ、方位算出部７０、信頼性判定部８０Ｈを備える。

　ＧＮＳＳ受信部２３１，２３２は、測位信号を追尾して、搬送波位相等の追尾による計測値を取得し、統合算出部１００Ｈに出力する。ＧＮＳＳ受信部２３２は、追尾による計測値を信頼性判定部８０Ｇに出力する。

　ＩＭＵセンサ４１は、加速度センサ４１１および角速度センサ４１２を備えている。加速度センサ４１１および角速度センサ４１２は、計測したセンサ値（加速度センサ値および角速度センサ値）を統合算出部１００に出力する。加速度センサ４１１は、加速度センサ値を信頼性判定部８０Ｈに出力する。

　統合算出部１００Ｈは、追尾による計測値と、加速度センサ値および角速度センサ値と、磁気センサの検出値による方位とを用いて、既知の方法によって、統合姿勢角を算出する。統合算出部１００Ｈは、追尾による計測値を用いて、既知の方法によって、統合位置を算出する。統合算出部１００Ｈは、統合姿勢角および統合位置を、記憶部５０Ｈに出力する。記憶部５０Ｈは、統合位置および統合姿勢角をバックアップ位置およびバックアップ姿勢角として記憶する。

　磁気センサ４０は、磁気センサの検出値を方位算出部７０に出力する。方位算出部７０は、磁気センサの検出値を用いて方位を算出する。

　信頼性判定部８０Ｈは、記憶部５０Ｈから取得したバックアップ方位を用いて、磁気センサの検出値を用いて算出した方位の信頼性判定を行う。この際、信頼性判定部８０Ｈは、ＧＮＳＳ受信部２３２から得られる追尾による計測値を用いてＧＮＳＳ位置を算出し、このＧＮＳＳ位置を信頼性の判定に用いてもよい。信頼性の判定方法は、上述の実施形態に示した方法を用いればよい。信頼性判定部８０Ｈは、算出した方位の信頼性があれば、当該方位を統合算出部１００Ｈに出力する。統合算出部１００Ｈは、この磁気検出値による方位を、整数値バイアスを決定するための初期方位等に利用する。これにより、統合姿勢角をより速く算出することができる。

１：センサ誤差算出装置
１０，１０Ａ，１０Ｂ，１０Ｃ，１０Ｄ，１０Ｅ，１０Ｆ，１０Ｇ，１０Ｈ：姿勢角算出装置
１１：ハードウェアプロセッサ
２０：ＧＮＳＳ姿勢角算出部
２１：ＧＮＳＳ位置算出部
２２，２２Ｆ：整数値バイアス決定部
３０，３０Ａ，３０Ｂ，３０Ｃ，３０Ｄ，３０Ｅ，３０Ｆ，３０Ｇ：較正値推定部
４０：磁気センサ
４１：ＩＭＵセンサ
５０，５０Ｃ，５０Ｄ，５０Ｅ，５０Ｆ，５０Ｇ，５０Ｈ：記憶部
６０：誤差補正部
６１，６１Ｇ：動揺補正部
７０：方位算出部
８０，８０Ｄ，８０Ｅ，８０Ｆ，８０Ｇ，８０Ｈ：信頼性判定部
１００，１００Ｈ：統合算出部
２３１，２３２：ＧＮＳＳ受信部
３０１：ＧＮＳＳ地磁気算出部
３０２：誤差推定部
３０３：較正値算出部
４１１：加速度センサ
４１２：角速度センサ

Claims (12)

1. 　ＧＮＳＳの測位信号からＧＮＳＳ姿勢角を算出するＧＮＳＳ姿勢角算出部と、
   　前記ＧＮＳＳ姿勢角からＧＮＳＳ地磁気を算出するＧＮＳＳ地磁気算出部と、
   　磁気センサによる磁気検出値と前記ＧＮＳＳ地磁気とを用いて、前記磁気センサの感度誤差、ミスアライメント誤差、および、バイアス誤差を推定する誤差推定部と、
   　を備える、センサ誤差算出装置。
2. 　請求項１に記載のセンサ誤差算出装置であって、
   　前記誤差推定部は、
   　三つの異なる姿勢において得られた前記磁気検出値と前記ＧＮＳＳ姿勢角を用いて、前記磁気センサの感度誤差、ミスアライメント誤差、および、バイアス誤差を推定する、
   　センサ誤差算出装置。
3. 　請求項１または請求項２に記載のセンサ誤差算出装置であって、
   　前記ＧＮＳＳ地磁気は、直交する第１方向成分と第２方向成分によって表され、
   　前記ＧＮＳＳ地磁気算出部は、
   　前記ＧＮＳＳ姿勢角の少なくとも方位角の余弦から、前記ＧＮＳＳ地磁気の第１方向成分を算出し、
   　前記ＧＮＳＳ姿勢角の少なくとも方位角の正弦から、前記ＧＮＳＳ地磁気の第２方向成分を算出する、
   　センサ誤差算出装置。
4. 　請求項１乃至請求項３のいずれか１項に記載のセンサ誤差算出装置と、
   　前記磁気センサの感度誤差、ミスアライメント誤差、および、バイアス誤差を用いて、前記磁気検出値を補正する誤差補正部と、
   　誤差補正後の磁気検出値を用いて方位を算出する方位算出部と、
   　を備える、姿勢角算出装置。
5. 　請求項４に記載の姿勢角算出装置であって、
   　前記ＧＮＳＳ姿勢角を用いてバックアップ方位を算出するバックアップ方位算出部と、
   　前記方位算出部で算出された方位と前記バックアップ方位とを用いて、前記方位算出部で算出された方位の信頼性を判定する信頼性判定部と、
   　を備えた姿勢角算出装置。
6. 　請求項４に記載の姿勢角算出装置であって、
   　前記方位算出部で算出された方位と、前記方位算出部によって過去に算出された方位であるバックアップ方位とを用いて、前記方位算出部で算出された方位の信頼性を判定する信頼性判定部と、
   　を備えた姿勢角算出装置。
7. 　請求項５または請求項６に記載の姿勢角算出装置であって、
   　前記信頼性判定部は、
   　前記方位算出部で算出された方位と前記バックアップ方位とが略同じであるときに方位の信頼性が有ると判定し、
   　該方位の信頼性が有ると判定した時に該方位を出力する、
   　姿勢角算出装置。
8. 　請求項７に記載の姿勢角算出装置であって、
   　前記ＧＮＳＳ姿勢角算出部で用いる整数値バイアスを決定する整数値バイアス決定部を備え、
   　前記整数値バイアス決定部は、前記信頼性判定部から出力された方位を用いて、前記整数値バイアスを決定する、
   　姿勢角算出装置。
9. 　ＧＮＳＳの測位信号からＧＮＳＳ姿勢角を算出するＧＮＳＳ姿勢角算出工程と、
   　前記ＧＮＳＳ姿勢角からＧＮＳＳ地磁気を算出するＧＮＳＳ地磁気算出工程と、
   　磁気センサによる磁気検出値と前記ＧＮＳＳ地磁気とを用いて、前記磁気センサの感度誤差、ミスアライメント誤差、および、バイアス誤差を推定する誤差推定工程と、
   　を有する、センサ誤差算出方法。
10. 　請求項９に記載のセンサ誤差算出方法であって、
    　前記誤差推定工程は、
    　三つの姿勢において得られた前記磁気検出値と前記ＧＮＳＳ姿勢角を用いて、前記磁気センサの感度誤差、ミスアライメント誤差、および、バイアス誤差を推定する、
    　センサ誤差算出方法。
11. 　請求項９または請求項１０に記載のセンサ誤差算出方法であって、
    　前記ＧＮＳＳ地磁気は、直交する第１方向成分と第２方向成分によって表され、
    　前記ＧＮＳＳ地磁気算出工程は、
    　前記ＧＮＳＳ姿勢角の少なくとも方位角の余弦から、前記ＧＮＳＳ地磁気の第１方向成分を算出し、
    　前記ＧＮＳＳ姿勢角の少なくとも方位角の正弦から、前記ＧＮＳＳ地磁気の第２方向成分を算出する、
    　センサ誤差算出方法。
12. 　請求項９乃至請求項１１のいずれか１項に記載のセンサ誤差算出方法の各工程と、
    　前記磁気センサの感度誤差、ミスアライメント誤差、および、バイアス誤差を用いて、前記磁気検出値を補正する誤差補正工程と、
    　誤差補正後の磁気検出値を用いて方位を算出する方位算出工程と、
    　を有する、姿勢角算出方法。

Images