WO2017090359A1 - 姿勢角算出装置、姿勢角算出方法 - Google Patents

Abstract

【課題】慣性センサの出力に基づく統合姿勢角の検定、リセットを容易に実現する。 【解決手段】姿勢角算出装置１０は、統合姿勢角算出部４１、逆算値算出部４２１、基準値算出部４２２、および、判定部４３を備える。統合姿勢角算出部４１は、慣性センサの出力と測位信号とを用いて統合姿勢角を算出する。逆算値算出部４２１は、統合姿勢角を用いて、測位信号を用いた姿勢角の算出に利用する物理量を逆算する。基準値算出部４２２は、測位信号の観測値から、逆算された物理量に対応する基準物理量を算出する。判定部４３は、逆算された物理量と基準物理量とを用いて、統合姿勢角の算出処理のリセットを判定する。

Description

姿勢角算出装置、姿勢角算出方法

　本発明は、慣性センサの出力と測位信号とを用いて姿勢角を算出する姿勢角算出装置、および姿勢角算出方法に関する。

　従来、船舶等の移動体の姿勢角を算出する装置が各種考案されている。例えば、特許文献１には、ＩＭＵ（慣性センサ）の出力データとＧＰＳ測位データとを用いて姿勢角を算出する航法計算システムが記載されている。

　特許文献１に記載の航法計算システムは、ＩＭＵの出力に基づく姿勢角に含まれる誤差を、ＧＰＳ測位データを用いて補正し、統合姿勢角として出力している。特許文献１に記載の航法計算システムは、統合姿勢角と、ＧＰＳの搬送波観測値に基づいて算出されたＧＰＳ姿勢角とを比較し、略一致する場合に、ＧＰＳの搬送波位相観測値が正常であると判断している。

特開２０１２－２０８０３３号公報

　しかしながら、ＩＭＵ、特に、安価なＩＭＵはバイアス誤差が大きく、さらに、姿勢角はＩＭＵの出力値を積算して算出されるため、統合姿勢角は、補正が適正に行われていない場合、時間が経過する程、誤差が大きくなる特性を有する。

　特に、統合姿勢角を用いてＧＰＳ測位データの整数値バイアスを決定する場合、統合姿勢角の誤差によって、整数値バイアスが真値から外れてしまうことがある。このため、この真値から外れた整数値バイアスを用いたＧＰＳ測位データを用いてＩＭＵの出力を補正して得られる統合姿勢角は、さらに誤差を生じてしまう。そして、この誤差の悪循環が繰り返されてしまい、統合姿勢角に大きな誤差が生じてしまうことがある。

　このため、統合姿勢角の検定を行い、適正なリセット処理を行う必要がある。統合姿勢角の検定としては、上述の特許文献１と同様に、統合姿勢角とＧＰＳ姿勢角とを比較し、略一致する場合に統合姿勢角が正しいと判定する検定方法が考えられる。

　しかしながら、この方法では、ＧＰＳ姿勢角が算出できない状態では、統合姿勢角を検定することはできない。一般に、三方向のＧＰＳ姿勢角を算出するには、少なくとも５個のＧＰＳ衛星からのＧＰＳ信号を、時間的に連続して受信しなければならない。このため、このような観測条件を維持できる状況でなければ統合姿勢角の検定を行うことができない。

　したがって、本発明の目的は、慣性センサの出力に基づく統合姿勢角の検定を容易に実現できる姿勢角算出装置、姿勢角算出方法を提供することにある。

　この発明の姿勢角算出装置は、統合姿勢角算出部、逆算値算出部、基準値算出部、および、判定部を備える。統合姿勢角算出部は、慣性センサの出力と測位信号とを用いて統合姿勢角を算出する。逆算値算出部は、統合姿勢角を用いて、当該統合姿勢角の算出に利用する物理量を逆算する。基準値算出部は、測位信号の観測値から、逆算された物理量に対応する基準物理量を算出する。判定部は、逆算された物理量と基準物理量とを用いて、統合姿勢角の算出処理のリセットを判定する。

　この構成では、測位信号を用いた姿勢角が算出されていなくても、統合姿勢角から逆算値と、測位信号の観測値からの算出値とで同じ物理量（例えば、幾何距離差）を算出することで、これらを比較して統合姿勢角の検定、リセットが可能になる。

　この発明によれば、慣性センサの出力に基づく統合姿勢角の検定を、容易に実現することができる。

本発明の実施形態に係る姿勢角算出装置の統合演算部の機能ブロック図 本発明の実施形態に係る姿勢角算出装置の機能ブロック図 本発明の実施形態に係る姿勢角算出方法のフローチャート 本発明の実施形態に係る姿勢角算出方法の第１具体例を示すフローチャート 本発明の実施形態に係る姿勢角算出方法の第２具体例を示すフローチャート 本発明の実施形態に係る姿勢角算出方法の第３具体例を示すフローチャート

　本発明の実施形態に係る姿勢角算出装置、姿勢角算出方法について、図を参照して説明する。図１は、本発明の実施形態に係る姿勢角算出装置の統合演算部の機能ブロック図である。図２は、本発明の実施形態に係る姿勢角算出装置の機能ブロック図である。なお、本実施形態では、移動体として船舶を例に示すが、他の水上移動体、陸上移動体、空中移動体等であってもよい。

　図２に示すように、姿勢角算出装置１０は、アンテナＡＮＴ１，ＡＮＴ２，ＡＮＴ３、受信部２１，２２，２３、ＩＭＵ（慣性センサ）３０、および、統合演算部４０を備える。アンテナＡＮＴ１は、受信部２１に接続されている。アンテナＡＮＴ２は、受信部２２に接続されている。アンテナＡＮＴ３は、受信部２３に接続されている。受信部２１，２２，２３およびＩＭＵ３０は、統合演算部４０に接続されている。

　アンテナＡＮＴ１，ＡＮＴ２，ＡＮＴ３は、これら３つのアンテナが一直線上に並んでいない配置パターンで配置されている。言い換えれば、アンテナＡＮＴ１とアンテナＡＮＴ２を結ぶ直線上にアンテナＡＮＴ３は配置されていない。なお、アンテナ数は、本実施形態では３つであるが、４つ以上でもよく、姿勢角の少なくとも一成分だけを算出する仕様であれば２つでもよい。アンテナＡＮＴ１，ＡＮＴ２，ＡＮＴ３の配置間隔は、測位信号の波長未満であることが好ましい。このような配置間隔を用いることによって、アンテナ間一重位相差の整数値バイアスを容易に決定することが可能になる。

　アンテナＡＮＴ１，ＡＮＴ２，ＡＮＴ３は、測位衛星ＳＡＴ１，ＳＡＴ２の測位信号を受信して、受信部２１，２２，２３に出力する。測位衛星とは、ＧＰＳ（Ｇｒｏｂａｌ　Ｐｏｓｉｔｉｏｎｉｎｇ　Ｓｙｓｔｅｍ）衛星等のＧＮＳＳ（Ｇｒｏｂａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓａｔｅｌｌｉｔｅ　Ｓｙｓｔｅｍｓ）の衛星である。なお、本実施形態では、測位信号を受信している測位衛星数が２つであるが、これ以外の個数であってもよく、少なくとも１つの測位衛星からの測位信号を受信できていれば、後述のリセット判定は可能である。

　なお、測位信号によって三方向の姿勢角（ロール角、ピッチ角、ヘディング角）を算出するためには、少なくとも５つの測位衛星からの測位信号を受信する必要がある。この際にも、測位信号毎にリセット判定を行うことが可能である。

　受信部２１は、アンテナＡＮＴ１で受信した測位信号を追尾し、擬似距離および搬送波位相積算値を算出する。受信部２１は、擬似距離および搬送波位相積算値を統合演算部４０に出力する。受信部２２は、アンテナＡＮＴ２で受信した測位信号を追尾し、擬似距離および搬送波位相を算出する。受信部２２は、擬似距離および搬送波位相積算値を統合演算部４０に出力する。受信部２３は、アンテナＡＮＴ３で受信した測位信号を追尾し、擬似距離および搬送波位相を算出する。受信部２３は、擬似距離および搬送波位相積算値を統合演算部４０に出力する。

　ＩＭＵ３０は、角速度センサを少なくとも備える慣性センサであり、さらに加速度センサ等の他のセンサを備えていてもよい。ＩＭＵ３０は、センサの出力値を統合演算部４０に出力する。

　統合演算部４０は、ＩＭＵ３０の出力値と、受信部２１，２２，２３からの擬似距離および搬送波位相積算値を用いて、既知のＴｉｇｈｔｌｙ　Ｃｏｕｐｌｅｄ方式の姿勢角算出方法によって、統合姿勢角を算出する。概略的には、統合演算部４０は、ＩＭＵ３０の出力値を利用して統合姿勢角を算出する。この際、統合演算部４０は、擬似距離および搬送波位相積算値を用いて、ＩＭＵ３０の誤差（バイアス誤差等）を推定する。統合演算部４０は、推定した誤差を次の統合姿勢角の算出時の補正項として利用する。

　このような構成において、統合演算部４０は、さらに統合姿勢角の検定を行い、統合姿勢角の誤差を大きいと判断すると、統合姿勢角の算出をリセットする。

　図１に示すように、統合演算部４０は、統合姿勢角算出部４１、逆算値算出部４２１、基準値算出部４２２、判定部４３を備える。判定部４３は、判定値算出部４３１、リセット制御部４３２を備える。

　統合姿勢角算出部４１は、受信部２１，２２，２３、および、ＩＭＵ３０に接続されている。統合姿勢角算出部４１は、逆算値算出部４２１に接続されている。逆算値算出部４２１は、判定値算出部４３１に接続されている。基準値算出部４２２は、受信部２１，２２，２３、および、判定値算出部４３１に接続されている。判定値算出部４３１は、リセット制御部４３２に接続されている。リセット制御部４３２は、統合姿勢角算出部４１に接続されている。

　統合姿勢角算出部４１は、ＩＭＵ３０の出力値と、受信部２１，２２，２３からの擬似距離および搬送波位相積算値とを用いて、上述の統合姿勢角の算出処理を行う。統合姿勢角算出部４１は、統合姿勢角を外部に出力するとともに、逆算値算出部４２１に出力する。

　逆算値算出部４２１は、測位信号から姿勢角を算出する場合に利用する物理量を、統合姿勢角から逆算する。逆算値算出部４２１は、逆算した物理量を判定値算出部４３１に出力する。

　例えば、逆算値算出部４２１は、統合姿勢角からアンテナＡＮＴ１，ＡＮＴ２，ＡＮＴ３のうちの２つアンテナ（例えば、アンテナＡＮＴ１とアンテナＡＮＴ２）間の基線ベクトルを逆算する。逆算値算出部４２１は、測位信号に含まれる航法メッセージから測位衛星ＳＡＴ１，ＳＡＴ２の位置を取得する。逆算値算出部４２１は、測位信号の追尾による計測値からアンテナＡＮＴ１，ＡＮＴ２の位置を算出する。逆算値算出部４２１は、アンテナＡＮＴ１，ＡＮＴ２の位置と基線ベクトルと測位衛星ＳＡＴ１の位置とを用いて、アンテナＡＮＴ１と測位衛星ＳＡＴ１との間の幾何距離と、アンテナＡＮＴ２と測位衛星ＳＡＴ１との間の幾何距離との差、すなわち測位衛星ＳＡＴ１に対するアンテナＡＮＴ１とアンテナＡＮＴ２との幾何距離差を算出する。この算出処理は、測位衛星ＳＡＴ２で行ってもよい。逆算値算出部４２１は、この幾何距離差を逆算値（逆算された物理量）として、判定値算出部４３１に出力する。

　基準値算出部４２２は、逆算値算出部４２１で算出された逆算値に対応する物理量（基準物理量）を、測位信号を用いて算出する。基準値算出部４２２は、基準物理量を判定値算出部４３１に出力する。

　例えば、基準値算出部４２２は、アンテナＡＮＴ１，ＡＮＴ２，ＡＮＴ３のうちの２つアンテナ（例えば、アンテナＡＮＴ１とアンテナＡＮＴ２）の搬送波位相積算値から既知の方法を用いて幾何距離差を算出する。具体的な一例としては、基準値算出部４２２は、アンテナＡＮＴ１とアンテナＡＮＴ２で受信した測位衛星ＳＡＴ１からの測位信号の搬送波位相積算値から、測位衛星ＳＡＴ１に対するアンテナＡＮＴ１とアンテナＡＮＴ２との幾何距離差を算出する。なお、上述の逆算の際に測位衛星ＳＡＴ２を用いた場合は、この処理は測位衛星ＳＡＴ２に対して行う。この際、判定値算出部４３は、整数値バイアスを決定する必要があるが、上述のようにアンテナ間の距離が測位信号の搬送波の１波長（より好ましくは１／２波長）未満であれば、整数値バイアスを容易に決定することが可能である。したがって、このようなアンテナの配置の場合、幾何距離差を容易に算出することができる。

　判定値算出部４３１は、逆算値と基準物理量から、リセット判定を行う判定値を算出する。判定値算出部４３１は、判定値をリセット制御部４３２に出力する。

　例えば、判定値算出部４３１は、統合姿勢角からの幾何距離差（逆算値）と、搬送波位相積算値からの幾何距離差（基準物理量）との差を判定値として算出し、リセット制御部４３２に出力する。

　リセット制御部４３２には、リセット判定用の閾値が設定されている。リセット判定用の閾値は、姿勢角算出装置１０の最大誤差に基づいて決定されている。リセット制御部４３２は、判定値と閾値を比較し、統合姿勢角に含まれる誤差が最大誤差を超える比較結果となった場合に、リセット条件を満たすと判定する。リセット制御部４３２は、リセット条件を満たしていれば、統合姿勢角算出部４１に対してリセット制御を実行する。

　例えば、リセット制御部４３２は、幾何距離差の差（判定値）がリセット判定用の閾値よりも大きければ、リセット条件を満たしたと判定して、統合姿勢角算出部４１に対してリセット制御を実行する。一方、リセット制御部４３２は、幾何距離差の差（判定値）がリセット判定用の閾値以下であれば、リセット条件を満たしていないと判定して、統合姿勢角算出部４１に対するリセット制御を実行しない。

　統合姿勢角算出部４１は、このリセット制御を受け、統合姿勢角の算出の再初期化を行う。統合姿勢角算出部４１は、リセット制御を受けていない期間は、上述のＩＭＵ３０に起因する誤差の補正を行いながら統合姿勢角の算出を継続する。

　このように、本実施形態の構成からなる姿勢角算出装置１０は、少なくとも１つの測位衛星からの測位信号を受信できていれば、ＩＭＵ３０の出力に基づく統合姿勢角の検定を行うことができる。これにより、測位信号を用いた姿勢角の算出を行えない状況であっても、統合姿勢角を検定することができる。したがって、統合姿勢角の検定をより容易に実現することができる。

　また、誤差が大きな場合に統合姿勢角の算出をリセットすることができる。これにより、誤差の大きな姿勢角の出力を抑制し、精度の高い姿勢角をより安定して出力することができる。

　なお、上述の実施形態では、アンテナ間一重位相差に対応する幾何距離差を逆算値および基準物理量に用いる態様を示した。しかしながら、複数の測位衛星からの測位信号を受信していれば、二重位相差に対応する幾何距離差の差を逆算値および基準物理量に用いることもできる。

　また、上述の実施形態では、判定値と閾値との比較結果が、統合姿勢角に含まれる誤差が最大誤差を超える結果となった場合にリセット条件を満たすと判定する態様を示した。しかしながら、この判定を複数回行い、リセット条件を満たすと判定した回数が許容回数を超えた場合に、リセット制御を行うようにしてもよい。なお、この判定は、１つの測位衛星に対して行った回数を用いてもよく、複数の測位衛星に対して行った回数を用いてもよい。

　また、上述の実施形態に示すように、幾何距離差をリセットの判定に用いることによって、測位システム間での単位変換を行う必要が無い。すなわち、複数の測位システムのうちのどの測位システムの測位信号であっても、上述の処理を用いて、リセットの判定をより容易にすることができる。特に、測位信号の周波数が異なる複数の測位システムを用いる場合に、より有効である。

　上述の説明では、統合姿勢角の算出、検定、およびリセット処理を、それぞれ個別の機能部で実現する態様を示した。しかしながら、図２に示す統合演算部４０をコンピュータ等の情報処理装置で構成し、統合姿勢角の算出、検定、およびリセット処理をプログラム化して記憶しておき、情報処理装置で読み出して実行してもよい。この場合、図３に示すフローを実行すればよい。図３は、本発明の実施形態に係る姿勢角算出方法のフローチャートである。なお、図３は、本実施形態に係る姿勢角算出方法の概略的な処理を示すフローチャートであり、図３のフローをより具体化したフローの例は、図４、図５、図６にそれぞれ示されている。

　図３に示すように、統合演算部４０は、ＩＭＵ３０の出力と、受信部２１，２２，２３からの擬似距離および搬送波位相積算値を用いて、統合姿勢角を算出する（Ｓ１０１）。統合演算部４０は、算出された統合姿勢角から、測位信号を用いた姿勢角の算出に利用する物理量を逆算する（Ｓ１０２）。統合演算部４０は、統合姿勢角から逆算された物理量に対応する基準物理量を、測位信号の観測値から算出する（Ｓ１０３）。

　統合演算部４０は、逆算された物理量（逆算値）と基準物理量とから、統合姿勢角のリセット判定に用いる判定値を算出する（Ｓ１０４）。

　統合演算部４０は、判定値がリセット条件を満たすか否かを判定する。統合演算部４０は、判定値がリセット条件を満たしていれば（Ｓ１０５：ＹＥＳ）、統合姿勢角の算出を再初期化（リセット）する（Ｓ１０６）。統合演算部４０は、判定値がリセット条件を満たしていなければ（Ｓ１０５：ＮＯ）、統合姿勢角の算出を継続する。

　このように、本実施形態の姿勢角算出方法を用いることによって、測位信号を用いた姿勢角の算出を行えない状況であっても、統合姿勢角を検定することができる。

　次に、統合姿勢角の検定方法の具体的な態様について説明する。

　（１）幾何距離差を用いる場合
　図４は、本発明の実施形態に係る姿勢角算出方法の第１具体例を示すフローチャートである。

　統合演算部４０は、ＩＭＵ３０の出力と、受信部２１，２２，２３からの擬似距離および搬送波位相積算値を用いて、統合姿勢角を算出する（Ｓ２０１）。統合演算部４０は、統合姿勢角から、１つの測位衛星に対する２つのアンテナの幾何距離差を逆算する（Ｓ２０２）。この幾何距離差が逆算値（統合姿勢角から逆算された物理量）に対応する。統合演算部４０は、２つのアンテナにおける測位信号からの搬送波位相積算値の差から、幾何距離差を算出する（Ｓ２０３）。この幾何距離差が基準幾何距離差であり、基準物理量に対応する。

　統合演算部４０は、統合姿勢角から逆算された幾何距離差と、測位信号から算出された幾何距離差（基準幾何距離差）とから判定値を算出する（Ｓ２０４）。具体的には、統合演算部４０は、統合姿勢角から逆算された幾何距離差と測位信号から算出された幾何距離差との差分値を判定値として算出する。

　統合演算部４０は、判定値をリセット判定用の閾値と比較して、判定値が閾値よりも大きければ（Ｓ２０５：ＹＥＳ）、統合姿勢角の算出を再初期化（リセット）する（Ｓ２０６）。統合演算部４０は、判定値が閾値以下であれば（Ｓ２０５：ＮＯ）、統合姿勢角の算出を継続する。

　（２）整数値バイアスを用いる場合
　図５は、本発明の実施形態に係る姿勢角算出方法の第２具体例を示すフローチャートである。

　統合演算部４０は、ＩＭＵ３０の出力と、受信部２１，２２，２３からの擬似距離および搬送波位相積算値を用いて、統合姿勢角を算出する（Ｓ３０１）。統合演算部４０は、１つの測位衛星に対する２つのアンテナ（受信部）間の信号伝搬遅延差を含んだ小数部の推定値（小数部推定値）と統合姿勢角とから、１つの測位衛星に対する２つのアンテナの一重位相差（アンテナ間一重位相差）の整数値バイアスを逆算する（Ｓ３０２）。この整数値バイアスが逆算値（統合姿勢角から逆算された物理量）に対応する。

　なお、小数部推定値は、いわゆるアナログ遅延差であり、アンテナと受信部の組毎の測位信号の伝搬遅延の差である。小数部推定値は、次に、示すような各方法で取得することができる。アンテナと受信部との間のアナログ遅延（伝搬遅延）および受信部内のアナログ遅延（伝搬遅延）をアンテナと受信部の組毎に測定し、これら遅延の差から算出することができる。また、遅延差を状態量としてカルマンフィルタ等によって推定し、この推定値を小数部推定値に用いることができる。また、統合姿勢角から逆算された基線ベクトル、一重位相差、および整数値バイアスから小数部推定値を逆算してもよい。

　統合演算部４０は、２つのアンテナにおける測位信号からの搬送波位相積算値の差から、整数値バイアスを算出する（Ｓ３０３）。この際、上述のように、アンテナ間の距離が測位信号の波長未満であれば、整数値バイアスを容易に決定し易い。この整数値バイアスが基準整数値バイアスであり、基準物理量に対応する。

　統合演算部４０は、統合姿勢角と小数部推定から逆算された整数値バイアスと、測位信号と小数部推定値から算出された整数値バイアス（基準整数値バイアス）とから判定値を算出する（Ｓ３０４）。具体的には、統合演算部４０は、統合姿勢角から逆算された整数値バイアスと、測位信号から算出された整数値バイアスとの差分値を判定値として算出する。

　統合演算部４０は、判定値をリセット判定用の基準値と比較する。この場合、「基準値＝０」である。整数値バイアスは、整数であるので、統合姿勢角が精度良く算出されていれば、統合姿勢角から逆算された整数値バイアスと、測位信号から算出された整数値バイアス（基準整数値バイアス）は一致する。一方、統合姿勢角の精度が劣化すると、統合姿勢角と小数部推定値から逆算された整数値バイアスと、測位信号と小数部推定値から算出された整数値バイアス（基準整数値バイアス）は一致しない。したがって、整数値バイアスを用いる場合は、統合姿勢角と小数部推定値から逆算した整数値バイアスと、測位信号と小数部推定値から算出した整数値バイアスとが一致していないことが、リセット条件を満たしていることに対応する。すなわち、「基準値≠０」がリセット条件を満たすことに対応する。

　統合演算部４０は、判定値と基準値が一致していなければ（Ｓ３０５：ＹＥＳ）、統合姿勢角の算出を再初期化（リセット）する（Ｓ３０６）。統合演算部４０は、判定値と基準値が一致していれば（Ｓ３０５：ＮＯ）、統合姿勢角の算出を継続する。

　このように、整数値バイアスを用いても、統合姿勢角の検定およびリセット処理を実現することができる。また、整数値バイアスを用いた場合、一致しているか否かで判定されるので、統合姿勢角の検定をより簡素な処理にすることができる。

　なお、図５の処理では、一重位相差を用いる態様を示したが、二重位相差を用いてもよい。二重位相差にはアナログ遅延差が含まれないので、二重位相差を用いる場合には、小数部推定値を用いなくてもよい。これにより、小数部推定値を求める処理を省略することができる。

　（３）測位衛星の方位を用いる場合
　図６は、本発明の実施形態に係る姿勢角算出方法の第３具体例を示すフローチャートである。

　統合演算部４０は、ＩＭＵ３０の出力と、受信部２１，２２，２３からの擬似距離および搬送波位相を用いて、統合姿勢角を算出する（Ｓ４０１）。統合演算部４０は、統合姿勢角から、２つのアンテナの基線ベクトルの方位（絶対方位）を逆算する（Ｓ３０２）。この基線ベクトルの方位は、上述の基線ベクトルの逆算処理と、擬似距離から算出される２つのアンテナの位置とを用いることで算出できる。この基線ベクトルの方位が逆算値（統合姿勢角から逆算された物理量）に対応する。

　統合演算部４０は、航法メッセージから測位衛星の位置を取得する（Ｓ４０３）。統合演算部４０は、測位衛星の位置（測位信号を用いた姿勢角の算出に利用する基準物理量に対応する。）、基線ベクトルの方位、および２つのアンテナの位置から、基線ベクトルに対する測位衛星の方位（相対方位）を算出する（Ｓ４０４）。なお、この方位には仰角も含まれている。この基線ベクトルに対する測位衛星の方位が判定値に対応する。

　統合演算部４０は、測位衛星の方位（相対方位）が除外範囲内にあれば（Ｓ４０５：ＹＥＳ）、統合姿勢角の算出を再初期化（リセット）する（Ｓ４０６）。除外範囲とは、整数値バイアスの算出精度が劣化し、正確な整数値バイアスを算出できなくなる方位の範囲である。例えば、２つのアンテナの並ぶ直線上およびその付近の上空に測位衛星が存在する場合等の範囲である。また、アンテナと測位信号を結ぶ方向余弦ベクトルと、２つのアンテナを結ぶ基線ベクトルとの成す角（アンテナが配置された平面上に投影された角）が、小さい範囲である。

　統合演算部４０は、測位衛星の方位（相対方位）が除外範囲内になければ（Ｓ４０５：ＮＯ）、統合姿勢角の算出を継続する。

　このように、測位衛星の方位を用いても、統合姿勢角の検定およびリセット処理を実現することができる。

　なお、上述の３種の具体例に限らず、測位信号を用いて算出または取得でき、且つ、統合姿勢角から逆算できる物理量であれば、本実施形態の姿勢角算出装置および姿勢角算出方法に用いることができる。

　また、上述の説明では、統合姿勢角の算出処理のリセットを行う構成まで含んでいるが、統合姿勢角の検定を行うまでの構成を１つの構成としてもよい。

１０：姿勢角算出装置
２１，２２，２３：受信部
３０：ＩＭＵ
４０：統合演算部
４１：統合姿勢角算出部
４２１：逆算値算出部
４２２：基準値算出部
４３：判定部
４３１：判定値算出部
４３２：リセット制御部
ＡＮＴ１，ＡＮＴ２，ＡＮＴ３：アンテナ
ＳＡＴ１，ＳＡＴ２：測位衛星

Claims (12)

1. 　慣性センサの出力と測位信号とを用いて統合姿勢角を算出する統合姿勢角算出部と、
   　前記統合姿勢角を用いて、当該統合姿勢角の算出に利用する前記測位信号から得られる物理量を逆算する逆算値算出部と、
   　前記測位信号の観測値から、前記逆算された物理量に対応する基準物理量を算出する基準値算出部と、
   　前記逆算された物理量と前記基準物理量とを用いて、前記統合姿勢角の算出処理のリセットを判定する判定部と、
   　を備える、姿勢角算出装置。
2. 　請求項１に記載の姿勢角算出装置であって、
   　前記逆算値算出部は、
   　１つの測位衛星に対する２つのアンテナ間のアンテナ間一重位相差に対応する幾何距離差を、前記統合姿勢角から逆算し、
   　前記基準値算出部は、
   　前記アンテナ間一重位相差に対応する幾何距離差を、前記測位信号の搬送波位相から前記基準物理量として算出し、
   　前記判定部は、
   　逆算された幾何距離差と、前記基準物理量として算出された幾何距離差とを用いて、リセットの判定を行う、
   　姿勢角算出装置。
3. 　請求項１に記載の姿勢角算出装置であって、
   　前記逆算値算出部は、
   　２つの測位衛星に対する２つのアンテナ間の二重位相差に対応する幾何距離差の差を、前記統合姿勢角から逆算し、
   　前記基準値算出部は、
   　前記二重位相差に対応する幾何距離差の差を、前記測位信号の搬送波位相から、前記基準物理量として算出し、
   　前記判定部は、
   　逆算された幾何距離差の差と、前記基準物理量として算出された幾何距離差の差とを用いて、リセットの判定を行う、
   　姿勢角算出装置。
4. 　請求項１に記載の姿勢角算出装置であって、
   　前記逆算値算出部は、
   　前記統合姿勢角から整数値バイアスを逆算し、
   　前記基準値算出部は、
   　前記測位信号の搬送波位相から算出した整数値バイアスを、前記基準物理量として算出し、
   　前記判定値算出部は、
   　前記基準物理量として算出された整数値バイアスと前記統合姿勢角から逆算された整数値バイアスとの差を用いて、前記統合姿勢角の算出処理のリセットを判定する、
   　姿勢角算出装置。
5. 　請求項４に記載の姿勢角算出装置であって、
   　前記整数値バイアスは、１つの測位衛星に対する２つのアンテナ間の一重位相差の整数値バイアスである、
   　姿勢角算出装置。
6. 　請求項４に記載の姿勢角算出装置であって、
   　前記整数値バイアスは、２つの測位衛星に対する２つのアンテナ間の二重位相差の整数値バイアスである、
   　姿勢角算出装置。
7. 　請求項１に記載の姿勢角算出装置であって、
   　前記逆算値算出部は、
   　前記統合姿勢角から２つのアンテナを結ぶ基線ベクトルの方位を逆算し、
   　前記基準値算出部は、
   　前記測位信号から測位衛星の方位を算出し、
   　前記判定値算出部は、
   　逆算した基線ベクトルの方位と前記測位衛星の方位から得られる前記基線ベクトルに対する前記測位衛星の相対方位を用いて、前記統合姿勢角の算出処理のリセットを判定する、
   　姿勢角算出装置。
8. 　請求項１乃至請求項７のいずれか１項に記載の姿勢角算出装置であって、
   　前記判定部は、リセット条件と満たしていると、前記統合姿勢角算出部に対してリセット制御を行うリセット制御部を備える、姿勢角算出装置。
9. 　請求項１乃至請求項８のいずれか１項に記載の姿勢角算出装置であって、
   　前記統合姿勢角算出部は、
   　Ｔｉｇｈｔｌｙ　Ｃｏｕｐｌｅｄ方式によって前記統合姿勢角を算出する、
   　姿勢角算出装置。
10. 　請求項１乃至請求項９のいずれか１項に記載の姿勢角算出装置であって、
    　前記測位信号を受信する複数のアンテナを備え、
    　複数のアンテナの間隔は、前記測位信号の波長未満である、
    　姿勢角算出装置。
11. 　慣性センサの出力と測位信号とを用いて統合姿勢角を算出する統合姿勢角算出工程と、
    　前記統合姿勢角を用いて、当該統合姿勢角の算出に利用する前記測位信号から得られる物理量を逆算する逆算値算出工程と、
    　前記測位信号の観測値から、前記逆算された物理量に対応する基準物理量を算出する基準値算出工程と、
    　前記逆算された物理量と前記基準物理量とを用いて、前記統合姿勢角の算出処理のリセットを判定する判定工程と、
    　を有する、姿勢角算出方法。
12. 　請求項１１に記載の姿勢角算出方法であって、
    　前記判定工程において、リセット条件と満たしていると判定されると、前記統合姿勢角の算出処理に対してリセット制御を行うリセット制御工程を有する、姿勢角算出方法。

Images