JP5354931B2 - 衛星航法／推測航法統合測位装置 - Google Patents

Description

この発明は、航法衛星からの測位信号を受信して得られる観測量と、加速度センサ等の外部センサより得られる観測量とを統合して測位を行う衛星航法／推測航法統合測位装置に関するものである。

非自立システムである例えばＧＰＳ測位装置より得られる観測量を基にした衛星航法演算と、加速度センサや角速度センサ等の外部センサの検出値を基にした慣性航法演算とを統合して移動体の位置・速度・方位等の航法データを求める衛星航法／推測航法統合測位装置が従来用いられている（例えば特許文献１参照）。
特許第３３８０４０４号公報

このような衛星航法／推測航法統合測位装置においては、外部センサの検出値の精度が問題となる。

安価な加速度センサと角速度センサを用いた衛星航法／推測航法統合測位装置では、これらの外部センサの補正が適正に行われていないと、航法衛星からの測位信号の中断等が生じた際、外部センサからの検出値の低精度の影響によって測位精度は著しく劣化する。

ところが、航法衛星からの測位信号の中断時においては外部センサの補正が適正に行われているか否かを判定する手段がなく、外部センサの検出値を用いた慣性航法演算を継続し、その結果を出力し続けるしかなかった。

そこで、この発明の目的は、航法衛星からの測位信号を受信できずに慣性航法を行っているときでも、外部センサの検出値の補正が適正であるか否かを判定できるようにして、誤った測位演算の継続または誤った航法データの出力を停止して上述の問題を解消した衛星航法／推測航法統合測位装置を提供することにある。

前記課題を解決するために、この発明の衛星航法／推測航法統合測位装置は次のように構成する。

（１）航法衛星の測位信号と外部センサの検出値とを基に移動体の航法データ（位置・速度等）を求めるとともに、前記外部センサの検出値に対する補正値を求めて、前記外部センサの検出値にフィードバックする統合測位演算手段を備えた衛星航法／推測航法統合測位装置において、
前記外部センサの検出値が適正であるか否かを判定する適正補正判定手段を備え、
前記外部センサは、移動体の前後方向であるＸ軸方向と、前記移動体の左右方向であるＹ軸方向の少なくとも２軸の加速度を検出する加速度センサと、前記角速度センサは、前記Ｘ軸方向及び前記Ｙ軸方向に対して直交するＺ軸回りの方位方向（旋回方向）の角速度を検出する少なくとも１軸の角速度センサとを含み、
前記適正補正判定手段は、前記Ｘ軸方向の加速度を検出する加速度センサの検出値を積分したＸ軸方向（円弧の接線方向）の速度と、前記角速度センサの検出値との積（＝遠心力）を求めるとともに、Ｙ軸方向の加速度を検出する加速度センサの検出値と前記積（遠心力）との差を求め、当該差が所定のしきい値を超えるか否かを判定することによって前記加速度センサおよび角速度センサの検出値が適正であるか否かを判定するもので、
前記統合測位演算手段は、前記航法衛星の測位信号が得られないときであって、前記検出値が適正であると判定されたときは、前記加速度センサ及び前記角速度センサの検出値を用いた測位演算を継続し、前記検出値が不適正と判定されたときは、前記外部センサの検出値を用いた測位演算を停止するものである。

すなわちここで、
Ｖｘ：前進方向の速度
ωｚ：方位方向の角速度
ａｙ：遠心力
とすると、
ａｙ＝Ｖｘ・ωｚ…（１）
の関係を、ある許容誤差または許容差の範囲内で満たすか否かを判定することによって加速度センサおよび角速度センサの補正が適正であるか否かを判定する。

これにより、航法衛星からの測位信号が中断された状態でも外部センサの補正が適正に行われているか否かを判定できる。そのため、適正な補正がなされていない外部センサの検出値を用いた慣性航法演算が継続される問題や、適正な補正がなされていない外部センサの検出値を用いた著しく劣化した航法データが出力される、という問題が回避できる。

しかも、この構成により、著しく精度の劣化した航法データが出力されるのを防止でき、異常な航法データがユーザーに提供されるのを防止することができる。

（２）前記適正補正判定手段は、前記Ｘ軸方向の速度と前記方位方向の角速度の絶対値がそれぞれ所定のしきい値を超えるときに前記判定を行うものとする。

これにより、無用な判定が防止できるとともに判定精度を高めることができる。

（３）前記適正補正判定手段は、前記Ｘ軸方向の速度と前記方位方向の角速度との積の絶対値が前記Ｙ軸方向の加速度の絶対値を含む比較値を超えるか否かによって前記判定を行うこととする。

これにより、簡単な演算によって各センサのうちいずれかの検出値の補正が不適正であるか否かを的確に判定できる。

（４）前記適正補正判定手段は、前記Ｘ軸方向の速度と前記方位方向の角速度との積が前記Ｙ軸方向の加速度と同符号であるか否かによって前記判定を行う。

これにより、Ｘ軸方向の加速度センサの検出値、方位方向の角速度センサの検出値、またはＹ軸方向の加速度センサの検出値のいずれかの補正が不適正であるか否かを的確に判定できる。

この発明によれば、航法衛星からの測位信号が中断された状態でも外部センサの補正が適正に行われているか否かを判定できる。そのため、適正な補正がなされていない外部センサの検出値を用いた慣性航法演算が継続される問題や、適正な補正がなされていない外部センサの検出値を用いた著しく劣化した航法データが出力される、という問題が回避できる。

《第１の実施形態》
本発明の衛星航法／推測航法統合測位装置（以下、単に「航法装置」という。）の構成を図１に示す。この図１に示すように、航法装置１００は、ＧＰＳ受信機１１と、加速度センサ１２と、振動ジャイロからなる角速度センサ１３と、ＣＰＵ等からなる統合演算部３０と、を備えていて、移動体に搭載される。

ＧＰＳ受信機１１は航法衛星であるＧＰＳ衛星から送信される測位信号を受信し、ＧＰＳ測位信号を出力する。

加速度センサ１２は、移動体の前後方向であるＸ軸方向と、前記移動体の左右方向であるＹ軸方向の少なくとも２軸の加速度を検出する。角速度センサ１３は、Ｘ軸方向及びＹ軸方向に対してそれぞれ直交するＺ軸回りの方位方向（旋回方向）の角速度を検出する。この加速度センサ１２及び角速度センサ１３が本発明に係る「外部センサ」に相当する。

統合演算部３０は、演算処理内容をブロック化すれば、追尾処理部２０、ＧＰＳ／ＩＮＳ演算部２１、及びセンサ検出値の適正補正判定部２２で表すことができる。

追尾処理部２０は、ＧＰＳ受信機１１からのＧＰＳ測位信号を入力し、各ＧＰＳ衛星からの信号を受信して得られるベースバンドのＧＰＳ測位信号の位相を追尾するとともに擬似距離、ドップラー周波数観測量、各衛星の位置・速度などを求める。

ＧＰＳ／ＩＮＳ演算部２１は、擬似距離及びドップラー周波数観測量、各衛星の位置・速度等に基づいて受信点の位置・速度を求める。また、擬似距離、ドップラー周波数周波数観測量、及び角速度センサ１３と加速度センサ１２の検出信号を基にＧＰＳ／ＩＮＳ統合演算を行う。このＧＰＳ／ＩＮＳ演算部２１が本発明に係る「統合測位演算手段」に相当する。

ＧＰＳ／ＩＮＳ演算部２１は、ＧＰＳ測位信号が得られているとき、ＧＰＳ／ＩＮＳ統合測位演算を行って、ユーザー装置に対して位置・速度・方位等の航法データを測位結果として出力する。また、このＧＰＳ／ＩＮＳの統合演算を行うとともに、加速度センサ１２及び角速度センサ１３の検出値の補正を行う。すなわち、慣性航法演算の結果がＧＰＳ演算の結果に一致するように、加速度センサ１２及び角速度センサ１３の検出値に対する補正値を求め、これらを加速度センサ１２及び角速度センサ１３の検出値にフィードバックする。

ＧＰＳ／ＩＮＳ演算部２１は、ＧＰＳ測位信号が得られていないとき、加速度センサ１２及び角速度センサ１３の検出値を基にして慣性航法演算を行って、ユーザー装置に対して位置・速度・方位等の航法データを測位結果として出力する。

センサ検出値の適正補正判定部２２は、後述する方法により加速度センサ１２及び角速度センサ１３の検出値の補正が適正であるか否かを判定する。

図２は、図１に示した航法装置の統合演算部３０における、センサ検出値の補正が適正であるか否かの判定およびその判定結果に伴う処理内容を示すフローチャートである。

まず、ＧＰＳ／ＩＮＳ演算を行う（Ｓ１）。ＧＰＳ測位信号が得られているときには、前述したとおり、衛星航法と慣性航法の統合演算を行う。また、ＧＰＳ測位信号が得られていないときには慣性航法演算のみを行う。

続いて、センサ検出値の補正が適正か否かの判定を行う（Ｓ２）。この判定ではＧＰＳ測位信号が得られているか否かに依らずに判定を行っているが、ＧＰＳ測位信号が得られている場合にはセンサ検出値の補正が適正であると見なし、ＧＰＳ測位信号が得られていない場合に限りセンサ検出値の補正が適正かの判定を行ってもよい。

補正が適正と判定すれば、ステップＳ１で求めたＧＰＳ／ＩＮＳ演算の結果を更新するとともにユーザーへ通知する（Ｓ３）。また補正が適正でないと判定した場合には測位結果を更新せず、測位演算を停止する直前の測位結果をユーザーへ通知する（Ｓ４）。

上記補正が適正と判定したとき、「ＧＰＳ／ＩＮＳ演算の結果を更新する」という処理に代えて、「測位状態を表すステータスを有効にする」という処理を行い、補正が適正でないと判定したとき、上記「ＧＰＳ／ＩＮＳ演算結果を更新しない」という処理に代えて、「測位状態を表すステータスを無効とする」という処理を行うようにしてもよい。

図３は、図２に示したセンサ検出値の補正が適正かの判定処理の具体例を示すフローチャートである。図３において、Ｖｘは移動体の前進方向（Ｘ軸方向）の加速度を検出する加速度センサ１２の検出値を補正し積分した速度、ωｚは方位方向の角速度を検出する角速度センサ１３の検出値を補正した角速度、ａｙは移動体の左右方向（Ｙ軸方向）の加速度を検出する加速度センサ１２の検出値を補正した遠心力である。

まず移動体（車両）の前進方向の速度Ｖｘの絶対値が所定のしきい値ｔｈ１を超えていて、且つ方位方向（旋回方向）の角速度ωｚの絶対値が所定のしきい値ｔｈ２を超えているか否かを判定する（Ｓ１１）。

また、速度Ｖｘと角速度ωｚの積の絶対値が所定の比較値１を超えているか否かを判定する。または、速度Ｖｘと角速度ωｚの積が遠心力ａｙと同符号であるか否かを判定する。この方法としては、速度Ｖｘと角速度ωｚと遠心力ａｙの３つの積が所定の比較値２より小さいか否かを判定することによって判定する（Ｓ１２）。

上記ステップＳ１１とＳ１２の判定条件を連続して満たす場合に、その連続回数をカウントする（Ｓ１３→Ｓ１４）。

上記連続回数がしきい値ｔｈ３に達した時には、それ以上のカウントアップは行わない（Ｓ１３→Ｓ１６）。また上記連続回数がしきい値ｔｈ３に達するまでにステップＳ１１，Ｓ１２での判定条件を満たさなかったなら、その連続回数を０にクリアする（Ｓ１５）。

その後、上記連続回数がしきい値ｔｈ３に達していれば、センサ検出値の補正が適正でないものと判定する（Ｓ１６→Ｓ１７）。また連続回数がｔｈ３未満である間はセンサ検出値の補正が適正であるものと判定する（Ｓ１８）。

図３に示した判定処理は例えば１秒に１回の頻度で繰り返し実行する。

上記各しきい値と比較値の具体的な設定値は次のとおりである。

しきい値ｔｈ１：０．５ ［ｍ／ｓ］
しきい値ｔｈ２：５．０ ［°／ｓ］
しきい値ｔｈ３：５（回）
比較値１：３×｜ａｙ｜ ＋ ３×（ａｙの観測雑音）
比較値２：−１×ｔｈ１×ｔｈ２×（３×（ａｙの観測雑音））
上記しきい値及び比較値の設定方法は次のとおりである。

しきい値ｔｈ１には、前進時には超えると見なせる速度を設定する。歩行者を想定すれば５０ｃｍ／ｓ程度、車両を想定すれば５ｋｍ／ｈ程度であるので、０．５ｍ／ｓ（＝５０ｃｍ／ｓ）〜１８ｍ／ｓ（＝５ｋｍ／ｈ）の範囲で設定すればよい。

しきい値ｔｈ２は、旋回時には超えると見なせる角速度を設定する。歩行者および車両を想定すれば１°／ｓ〜１０°／ｓ程度に設定すればよい。

このしきい値ｔｈ１，ｔｈ２を用いた判定処理（図３におけるステップＳ１１）を実行する手段が、請求項１に記載の「センサ検出値の適正補正判定手段」に相当する。

しきい値ｔｈ３を用いて判定を行うのは、センサ検出値に突発的に大きな雑音が生じたときでも、それをセンサ補正の不適正と誤判定しないようにするためである。数回ならばセンサ雑音が突発的に大きいこともあり得ると想定して、しきい値ｔｈ３は数回（２〜１０回）程度に設定すればよい。

比較値１は、（１）式の右辺（Ｖｘ・ωｚ）の値の大きさが、（１）式の左辺ａｙの値の大きさと比較して補正が適正に行われているか否かの判定に用いるので、ａｙ（遠心力）の大きさを“（１＋加速度センサの感度誤差％のカタログ仕様値×０．０１）倍”〜“３倍程度（安価な加速度センサ程、遠心力の検出精度が低いために大きな倍率）”した値と停止時に超えないと見なせる値（停止時に通常とり得る観測雑音の３倍〜６倍程度）の加算値に設定すればよい。

遠心力ａｙの停止時に通常とり得る観測雑音は、加速度センサのカタログ仕様値でも良いし、停止時における遠心力ａｙの標準偏差として予め計測した値でもよい。

比較値２は、（１）式の右辺（Ｖｘ・ωｚ）の値と左辺ａｙの値とを掛けた値の符号がマイナスで、絶対値が停止時には超えないと見なせる値より大きいか否かの判定に用いるので、停止時に超えないと見なせる速度（＝しきい値ｔｈ１）と停止時に超えないと見なせる角速度（＝しきい値ｔｈ２）と停止時に超えないと見なせる遠心力（＝ａｙの停止時に通常取り得る観測雑音の３倍〜６倍程度）を掛けた大きさとなる値の負の値に設定すればよい。

なお、図３のステップＳ１１の条件を満たさなかったときには、以降の判定処理をスキップするようにしてもよい。すなわち、移動体が停止しているときや直進しているときには、Ｘ軸方向の加速度センサの検出値および角速度センサの検出値のいずれもが小さいので的確な判定が行えない可能性がある。その場合にはセンサ検出値の補正が適正か否かの判定を行わないようにしてもよい。

《第２の実施形態》
図４は、第２の実施形態に係る航法装置の統合演算部における、センサ検出値の補正が適正かの判定およびその判定結果に伴う処理内容を示すフローチャートである。第２の実施形態に係る航法装置のハードウェアの構成は図１、センサ検出値の補正が適正かの判定内容については図３に示したものと同様であり、ここでは説明を省略する。

図４に示す処理内容は次のとおりである。

まず、ＧＰＳ／ＩＮＳ演算を行う。前述したとおり、必要なＧＰＳ測位信号が得られているときには、衛星航法と慣性航法の航法演算を行い、ＧＰＳ測位信号が得られないときには、慣性航法演算のみを行う（Ｓ２１）。

そして、必要なＧＰＳ測位信号が得られて、ＧＰＳ演算結果による有効または無効のステータスのうち、有効なステータスが得られたかを判定する（Ｓ２２）。

続いてＧＰＳ／ＩＮＳ演算結果の信頼性判定を行う（Ｓ２２）。具体的には、前回までに求めたＧＰＳ／ＩＮＳ演算結果とＧＰＳ演算結果との差が所定値より小さい場合に、ＧＰＳ／ＩＮＳ演算結果の信頼性を「有り」と判定し、所定値より大きい場合はＧＰＳ／ＩＮＳ演算結果の信頼性を「無し」と判定する（Ｓ２３）。そしてＧＰＳ／ＩＮＳ演算結果の信頼性を「有り」と判定した場合、初期状態のＧＰＳ／ＩＮＳ演算結果の信頼性「無し」から「有り」に変更し、ＧＰＳ／ＩＮＳ演算結果の信頼性を「無し」と判定した場合には、初期状態のＧＰＳ／ＩＮＳ演算結果の信頼性「無し」を維持する（Ｓ２４）。ここでこの判定に用いる所定値には、ＧＰＳ演算結果の誤差と同等以上に設定する。

その後、センサ検出値の補正が適正であるか否かの判定を行う（Ｓ２５）。このセンサ検出値の補正が適正であるか否かの判定は図２に示したステップＳ２（図３の処理）と同様であり、ここでの説明は省略する。

センサ検出値が適正に補正されていないと判定した場合、ＧＰＳ／ＩＮＳ演算結果の信頼性を「無し」に変更する（Ｓ２６）。

そして、ＧＰＳ／ＩＮＳ演算結果の信頼性の判定を行う（Ｓ２７）。具体的にはステップＳ２３と同様の処理を行い、その結果、ＧＰＳ／ＩＮＳ演算結果の信頼性を「有り」と判定した場合、ＧＰＳ／ＩＮＳ演算結果を用いて測位結果を更新する（Ｓ２８）。

ＧＰＳ／ＩＮＳ演算結果の信頼性を「無し」と判定した場合、ＧＰＳ演算結果で有効なステータスが得られているか否かを判定する（Ｓ２９）。そしてＧＰＳ測位結果で有効なステータスが得られている場合は、ＧＰＳ演算結果を用いて測位結果を更新する（Ｓ３０）。

しかし、ＧＰＳ／ＩＮＳ演算結果の信頼性を「無し」と判定し、ＧＰＳ演算結果で無効なステータスが得られている場合には測位結果を更新しない（Ｓ３１）。

上記ＧＰＳ／ＩＮＳ演算結果の信頼性を「有り」と判定したとき、「ＧＰＳ／ＩＮＳ演算の結果を更新する」という処理に代えて、「測位状態を表すステータスを有効にする」処理を行い、ＧＰＳ／ＩＮＳ演算結果の信頼性を「無し」と判定したとき、「ＧＰＳ／ＩＮＳ演算結果を更新しない」という処理に代えて、「測位状態を表すステータスを無効にする」という処理を行ってもよい。

図２・図３に示した第１の実施形態の処理では、移動体の直進時や停車時には図３のステップＳ１１で常にＮＯとなり、連続回数が０（零）にクリアされて、センサ検出値が適正に補正されていると判定されるので、測位結果が更新されてしまう。これに対して、図４に示した第２の実施形態では、ＧＰＳ中断時にセンサ検出値の補正が適正に行われていないと判断された場合には、ＧＰＳ測位が再開するまでは測位結果が更新されない。そのため、精度の劣化した測位結果の出力を防ぐことができる。

また、図２・図３に示した第１の実施形態の処理では、センサ検出値の補正が適正に行われていないと判定された場合にはＧＰＳ演算結果が有効であっても測位結果を更新しないが、図４に示した第２の実施形態では、ＧＰＳ演算結果が有効になればＧＰＳ測位演算により測位結果が速やかに更新される。

補正されたセンサ検出値（加速度，角速度）はＧＰＳ／ＩＮＳ演算結果の微分値に相当するものであるため、妥当性のない値の積分値は信頼できない。そのため、センサ検出値の補正が適正に行われていないと判定された場合にはＧＰＳ／ＩＮＳ演算結果は信頼できない。しかし、センサ検出値の補正が適正である、ということはＧＰＳ／ＩＮＳ演算結果の微分値が信頼できることにしかならないので、センサ検出値の補正が適正に行われていると判定された場合であっても、ＧＰＳ／ＩＮＳ演算結果が信頼できるとは必ずしも限らない。そこで、図４のステップ（Ｓ２５→Ｓ２６）のように、センサ検出値の補正が適正に行われていないと判定された場合には、ＧＰＳ／ＩＮＳ演算結果の信頼性を「なし」に変更する。

なお、以上に示した例では衛星航法測位のためにＧＰＳを用いる例を示したが、別の衛星航法測位システムを利用する場合にも同様に適用できる。

本発明の実施形態に係る衛星航法／推測航法統合測位装置の構成を示すブロック図である。 図１に示した航法装置の統合演算部３０における、センサ検出値の適正補正の判定およびその判定結果に伴う処理内容を示すフローチャートである。 図２に示したステップＳ２でのセンサ検出値の適正補正の判定処理の具体的な内容の例を示すフローチャートである。 第２の実施形態に係る航法装置の統合演算部における、センサ検出値の適正補正の判定およびその判定結果に伴う処理内容を示すフローチャートである。

１１…ＧＰＳ受信機
１２…加速度センサ
１３…角速度センサ
２０…追尾処理部
２１…ＧＰＳ／ＩＮＳ演算部
２２…センサ検出値の適正補正判定部
３０…統合演算部
１００…航法装置

Claims (4)

1. 航法衛星の測位信号と外部センサの検出値とを基に移動体の航法データを求めるとともに、前記外部センサの検出値に対する補正値を求めて、前記外部センサの検出値にフィードバックする統合測位演算手段を備えた衛星航法／推測航法統合測位装置において、
   前記外部センサの検出値が適正であるか否かを判定する適正補正判定手段を備え、
   前記外部センサは、移動体の前後方向であるＸ軸方向と、前記移動体の左右方向であるＹ軸方向の少なくとも２軸の加速度を検出する加速度センサと、前記角速度センサは、前記Ｘ軸方向及び前記Ｙ軸方向に対して直交するＺ軸回りの方位方向（旋回方向）の角速度を検出する少なくとも１軸の角速度センサとを含み、
   前記適正補正判定手段は、前記Ｘ軸方向の加速度を検出する加速度センサの検出値を積分したＸ軸方向（円弧の接線方向）の速度と、前記角速度センサの検出値との積（＝遠心力）を求めるとともに、Ｙ軸方向の加速度を検出する加速度センサの検出値と前記積（遠心力）との差を求め、当該差が所定のしきい値を超えるか否かを判定することによって前記加速度センサおよび角速度センサの検出値が適正であるか否かを判定するもので、
   前記統合測位演算手段は、前記航法衛星の測位信号が得られないときであって、前記検出値が適正であると判定されたときは、前記加速度センサ及び前記角速度センサの検出値を用いた測位演算を継続し、前記検出値が不適正と判定されたときは、前記外部センサの検出値を用いた測位演算を停止するものである衛星航法／推測航法統合測位装置。
2. 前記適正補正判定手段は、前記Ｘ軸方向の速度と前記方位方向の角速度の絶対値がそれぞれ所定のしきい値を超えるときに前記判定を行う、請求項１に記載の衛星航法／推測航法統合測位装置。
3. 前記適正補正判定手段は、前記Ｘ軸方向の速度と前記方位方向の角速度との積の絶対値が前記Ｙ軸方向の加速度の絶対値を含む比較値を超えるか否かによって前記判定を行う、請求項１または２に記載の衛星航法／推測航法統合測位装置。
4. 前記適正補正判定手段は、前記Ｘ軸方向の速度と前記方位方向の角速度との積が前記Ｙ軸方向の加速度と同符号であるか否かによって前記判定を行う、請求項１〜３のいずれかに記載の衛星航法／推測航法統合測位装置。

Images