JP5397915B2 - 移動体の姿勢角処理装置 - Google Patents

Description

本発明は、移動体の姿勢角を継続的に計測する場合において累積的な誤差を減少できる移動体の姿勢角処理装置に関するものである。

着用型システムにより人（移動体）の状態を取得する技術は、人のコンテキスト情報を把握し、計算機との知的なインタフェースを実現する上で欠かせない。歩行者ナビや遠隔作業支援における作業員の状況把握、博物館や大規模展示施設の展示ガイドなど、多くの応用への展開が期待されている。

例えば、人が携帯することで、その人の姿勢角の情報を正確に得ることのできる姿勢角処理装置は、被計測対象（携帯電話やＰＤＡなどの小型デバイス、人体など）に着用されてその姿勢角を小型・軽量かつ、安価なセンサ群から信号により取得される。

従来から、磁気センサと傾斜角センサ、あるいは重力加速度センサを用いて絶対姿勢角を取得する技術は知られている。また、角速度センサを用いて、ある絶対基準姿勢角からの相対的な姿勢変化を計測する技術は知られている。これらのセンサを組み合わせて、より正確に、ロバストに計測対象の絶対姿勢角を計測する試みがなされている。

特許第３０３８４５２号公報 特開平１１−２１１４７９号公報

磁気センサを用いて推定される地磁気ベクトルに基づいて現在の絶対姿勢角を計測する方法は、特に、屋内環境においては、さまざまな電子機器や建築構造物などによる磁場の乱れが存在するため、信頼できる地磁気を計測することが難しく、広範囲な環境下では、安定して正しく動作しない問題点がある。

また、ジャイロセンサと加速度センサを用いた姿勢角を計測する方法においては、基準となる絶対姿勢角から、ジャイロセンサが計測する角速度ベクトルと加速度センサが計測する加速度ベクトルを積算することによって、逐一絶対姿勢角を更新して姿勢角を推定するが、ジャイロセンサの出力に含まれるドリフト成分等の問題により、長期間継続して絶対姿勢角を取得し続けることは難しいという問題点があった。

航空機等に搭載される慣性航行装置（ＩＭＵ）に用いられるジャイロセンサ・加速度センサは非常に高い精度で、基準となる絶対姿勢角からの相対的な姿勢角変化を計測することが可能であるが、サイズ・重量と経済性の問題から、小型デバイスや人体への装着が困難であるという問題点がある。

歩行者向けナビゲーションシステムに用いる自蔵航法システムを構成する上では、高精度・高価格のジャイロセンサを用いることは、一般的には価格や重量の観点から困難であり、低価格なＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ）デバイスのジャイロセンサが用いられることがほとんどである。

しかしながら、ＭＥＭＳデバイスのジャイロセンサの角速度のゼロ点出力（センサの静止時の角速度出力）にはオフセット成分が含まれ、その成分が長期的にわたってゆるやかに変動することが知られている。ジャイロセンサの温度によってそのゼロ点出力を一部推定可能ではあるが、多くの場合、長期的な変動を捉えてそのオフセット成分を補正することが必要である。

このような変動を補正することができない場合、自蔵航法システムは長期的に継続して移動方位を正しく推定することができなくなり、その測位誤差は非常に大きくなる問題がある。

本発明は、このような問題を解決するためになされたものであり、本発明の目的は、移動体の姿勢角を継続的に計測する場合において累積的な誤差を減少できる移動体の姿勢角処理装置を提供することにある。具体的には、ＭＥＭＳデバイスのジャイロセンサなどに含まれる角速度のゼロ点出力のオフセット成分を推定する手段を備えた移動体の姿勢角処理装置を提供することにある。

上記のような目的を達成するため、本発明においては、次のような考え方に基づいて、移動体の姿勢角処理装置を構成する。すなわち、所定の閾値以上の距離を移動している区間において得られる測定結果は同一地点におけるものではなく、測定される条件と環境が刻一刻と変化することから、観測データの誤差は平均値ベクトルが０のホワイトノイズ的な性質に近づくことが期待される。このため、移動中に得られる一連の測定結果は、カルマンフィルタやパーティクルフィルタなどへの入力としてより適切な性質を持ったデータであると考えられる。

そこで、本発明においては、移動を検知することによって、移動中であるときと停止しているときの二つの状態を判別し、それぞれの状態における測定結果を異なる方法で取り扱うことにより、より適切に移動体の姿勢角の推定処理を行えるようにする。

具体的には、重力方位と運動を検知するために用いる加速度センサ（３軸）と、角速度を計測するジャイロセンサ（３軸）と、地磁気および環境磁場を計測する磁気センサ（３軸）と、これらのセンサにより移動状態を検知する手段を組み合わせた装置により、ジャイロセンサに含まれる角速度のゼロ点出力のオフセット成分を推定する装置を構成し、推定したオフセット成分を利用して、姿勢角の推定処理を行う。

このため、ジャイロセンサ（３軸）の角速度のゼロ点出力のオフセット値（３次元）と、重力加速度ベクトル（３次元）、環境磁気ベクトル（３次元）、角速度（３次元）を状態ベクトルとして持つ拡張カルマンフィルタを構成し、これをジャイロセンサゼロ点出力推定装置として利用する。

具体的に、本発明は、第１の態様として、本発明による移動体の姿勢角処理装置が、移動体に装着された加速度センサ、磁気センサ、およびジャイロセンサからの出力により当該移動体の姿勢角を推定して出力する姿勢角推定装置であって、角速度ベクトルを出力するジャイロセンサと、加速度ベクトルを出力する加速度センサと、磁場ベクトルを出力する磁気センサと、前記加速度センサの出力に基づいて移動体の移動状態を判定する移動状態判定装置と、前記移動状態判定装置による前記移動状態の判定が移動体の静止を検知したときに、前記加速度センサの出力と、前記磁気センサの出力、前記ジャイロセンサの出力に基づき、前記ジャイロセンサのゼロ点出力を推定する処理を行うジャイロセンサゼロ点出力推定装置とを備え、前記ジャイロセンサゼロ点出力推定装置の出力を利用して、前記ジャイロセンサのゼロ点を補正することを特徴とする。

また、この場合に、加速度センサ、磁気センサ、およびジャイロセンサの出力により、姿勢角を推定する姿勢角推定装置が、移動体の姿勢角を推定処理する場合に、前記ジャイロセンサゼロ点出力推定装置の出力に基づいて、移動体の姿勢角を推定した出力を補正することを特徴とする。

本発明は、第２の態様として、本発明による移動体の姿勢角処理装置は、移動体に装着された加速度センサ、磁気センサ、およびジャイロセンサからの出力により当該移動体の姿勢角を推定して出力する姿勢角推定装置であって、角速度ベクトルを出力するジャイロセンサと、加速度ベクトルを出力する加速度センサと、磁場ベクトルを出力する磁気センサと、前記加速度センサおよび前記ジャイロセンサの出力に基づいて移動体の移動状態を判定する移動状態判定装置と、前記移動状態判定装置による前記移動状態の判定が移動体の静止を検知したときに、前記加速度センサの出力と、前記磁気センサの出力、前記ジャイロセンサの出力に基づき、前記ジャイロセンサのゼロ点出力を推定する処理を行うジャイロセンサゼロ点出力推定装置とを備え、前記ジャイロセンサゼロ点出力推定装置の出力を利用して、前記ジャイロセンサのゼロ点を補正することを特徴とする。

また、この場合において、加速度センサ、磁気センサ、およびジャイロセンサの出力により、姿勢角を推定する姿勢角推定装置が、移動体の姿勢角を推定処理する場合に、前記ジャイロセンサゼロ点出力推定装置の出力に基づいて、移動体の姿勢角を推定した出力を補正することを特徴とする。

本発明の移動体の姿勢角処理装置の構成によれば、移動体の姿勢角を推定する処理を行う場合に、移動体の移動の有無を検知することで、移動の有無に応じて、具体的には立ち止まりの検知することにより、ジャイロセンサのゼロ点を補正し、または、移動体の姿勢角を推定した出力の可否を判定し、姿勢角の推定処理の出力を補正して出力するようにデータ処理する。

本装置の装着者がその場で立ち止っていると判定される場合は、磁気センサによって計測される環境磁場は同一地点のもので一定であると見なせる。すなわち、移動状態判定装置の出力が真信号であるとき、磁気センサで計測される磁気ベクトルを環境磁場の観測結果として利用することで、拡張カルマンフィルタの状態ベクトルであるゼロ点出力のオフセット値を推定することが可能となる。

ここで、移動状態判定装置は、加速度センサ単独もしくは加速度センサとジャイロセンサの出力の変動の大きさをその分散などの統計量を用いて静止状態を検知する装置として構成できる。したがって、他の計測手段を特に追加することなく、ジャイロセンサゼロ点出力推定装置を構成して、姿勢角の推定処理装置の構成することができる。

また、ジャイロセンサ（３軸）のゼロ点出力のオフセット値が前記拡張カルマンフィルタによって正しく推定されることで、そのジャイロセンサの角速度出力から積算結果として算出される移動方位の推定誤差を軽減することができ、したがって、長期的に測位誤差の少ない歩行者向けの自蔵航法システムを実現することが可能となる。

本発明の移動体の姿勢角処理装置の主要な構成を示すブロック図である。 本発明の移動体の姿勢角処理装置の構成を示すブロック図である。 本発明の移動体の姿勢角処理装置の別の構成を示すブロック図である。 本発明の移動体の姿勢角処理装置の別の構成を示すブロック図である。

以下、本発明による移動体の姿勢角処理装置を実施する場合に形態について、図面を用いて具体的に説明する。図１は、本発明の移動体の姿勢角処理装置の主要な構成を示すブロック図である。図１において、２０１は重力方位と運動を検知するために用いる加速度センサ、２０２は地磁気および環境磁場を計測する磁気センサ、２０３は角速度を計測するジャイロセンサ、２０４は移動状態判定装置、２０５はジャイロセンサゼロ点出力推定装置である。

移動体の姿勢角処理装置では、移動体に装着された３軸の加速度センサ２０１と、３軸の磁気センサ２０２と、３軸のジャイロセンサ２０３とをセンシング手段とし、加速度センサ２０１の出力である加速度ベクトル２１０に基づいて、移動状態判定装置２０４が、移動体の移動の有無を判定する。これにより、移動体判定装置２０４は、移動体の立ち止まりを判定する。なお、ここでの移動状態判定装置２０４が、これらセンサが装着されている移動体の移動状態を判定する手法としては、たとえば、一定期間内の加速度ベクトルの分散値を算出し、それが所定の閾値を越えるとき移動体が移動していると判定する。このような手法は、移動体が歩行者である場合など、加速度ベクトルに現れる振動を伴って移動するときに有効である。

移動状態判定装置２０４は、移動状態が検知されるときには真（ＴＲＵＥ）信号を出力信号２１３として出力し、静止していると検知されるときには、偽（ＦＡＬＳＥ）信号を出力信号２１３として出力する。ジャイロセンサゼロ点出力推定装置２０５は、拡張カルマンフィルタによって実装される。この拡張カルマンフィルタは、その状態ベクトルとして、重力加速度ベクトル（３軸）と、環境磁場ベクトル（３軸）と、角速度ベクトル（３軸）と、ジャイロセンサ（３軸）の角速度のゼロ点出力のオフセット値（３次元）、その成分として持つ。これにより、ジャイロセンサのゼロ点出力（３軸）が推定される。

重力方向の観測としては、加速度センサ２０１の出力である加速度ベクトル２１０を用いることにより、環境磁場ベクトルの観測としては、磁気センサ２０２の出力である磁気ベクトル２１１を用いることにより、角速度とジャイロセンサのゼロ点出力の観測としては、ジャイロセンサ２０３の出力である角速度ベクトル２１２を用いることにより、カルマンフィルタの状態が更新される。

立ち止っていると判定される場合に、磁気センサによって計測される環境磁場は同一地点のものでは一定であると見なせる。このため、十分に多くの観測が得られると、状態ベクトルは時間経過とともに収束し、その収束結果としてジャイロセンサのゼロ点出力を取得することができる。これをジャイロセンサゼロ点推定装置２０５からの推定結果２１４として出力する。この推定結果は直接的には、ジャイロセンサのゼロ点を補正するデータとして利用される。または、例えば、移動体の姿勢角を推定した結果を利用するか否かを判定する結果として利用される。

図２は、本発明による移動体の姿勢角処理装置の構成を示すブロック図である。図２において、２０１は加速度センサ、２０２は磁気センサ、２０３はジャイロセンサ、２０４は移動状態判定装置、２０５はジャイロセンサゼロ点出力推定装置、２０６は姿勢角推定装置である。この構成によると、加速度センサ２０１からの出力データ、磁気センサ２０２からの出力データ、ジャイロセンサ２０３からの出力データは、ジャイロセンサゼロ点の推定処理に使われるだけでなくて、姿勢角推定処理装置２０６において姿勢角の推定処理を行うためにも利用される。

姿勢角推定装置２０６は、加速度ベクトル２１０と、磁気ベクトル２１１と、角速度ベクトル２１２と、ジャイロセンサのゼロ点出力２１４からの出力データを入力する。姿勢角推定装置２０６は、具体的には、カルマンフィルタによって実装される。重力方向ベクトル（３軸）と、地磁気による真北方向ベクトル（３軸）、角速度ベクトル（３軸）の９個の成分を持つ状態ベクトルが更新される。

重力方向ベクトルの観測として、加速度ベクトル２１０を用いて、真北方向ベクトルの観測として、磁気ベクトル２１１から磁北との偏角と伏角を補正した真北ベクトルを用いて、角速度ベクトルの観測として、角速度ベクトル１１２からゼロ点出力２１４を補正したものを用いる。ここで観測と更新によって得られる重力方向ベクトルと真北方向ベクトルの二つを用いて、移動体の姿勢角を算出して出力する（２１５）。

図３は、本発明の移動体の姿勢角処理装置の別の構成を示すブロック図である。移動体に装着された３軸加速度センサ２０１と３軸磁気センサ２０２、３軸ジャイロセンサ２０３をセンシング手段とし、加速度センサ２０１の出力である加速度ベクトル２１０とジャイロセンサ２０３の出力である角速度ベクトル２１２に基づいて、移動状態判定装置３０４は前記移動体の移動の有無を判定する。

移動状態を判定する手段として、たとえば、一定期間内の加速度ベクトルの分散値を算出し、それが所定の閾値を越えるとき、または、角速度ベクトルが一定の所定の閾値を越えるとき、移動体が移動していると判定することができる。これは、移動体が歩行者である場合など、加速度ベクトルと角速度ベクトルに現れる振動と回転運動を伴って移動する場合には有効である。移動状態判定装置３０４は移動状態が検知されるとき、真（ＴＲＵＥ）信号を出力信号３１３として出力し、静止していると検知されるとき、偽（ＦＡＬＳＥ）信号を出力信号３１３として出力する。

ジャイロセンサゼロ点出力推定装置２０５は、カルマンフィルタによって実装することができる。このカルマンフィルタは状態ベクトルとして、重力方向ベクトル（３軸）と環境磁場ベクトル（３軸）、角速度ベクトル（３軸）、ジャイロセンサのゼロ点出力（３軸）をその成分として持つ。重力方向の観測として加速度センサ２０１の出力である加速度ベクトル２１０を用いて、環境磁場ベクトルの観測として磁気センサ２０２の出力である磁気ベクトル２１１を用いて、角速度とジャイロセンサのゼロ点出力の観測として、ジャイロセンサ２０３の出力である角速度ベクトル２１２を用いて、カルマンフィルタの状態を更新する。

十分に多くの観測が得られると、状態ベクトルは時間経過とともに収束し、その収束結果としてジャイロセンサのゼロ点出力を取得することができ、これをジャイロセンサゼロ点推定装置２０５の推定結果として出力する（２１４）。

図４は、本発明の移動体の姿勢角処理装置の別の構成を示すブロック図である。図３の構成に加えて、姿勢角推定装置２０６を備える。姿勢角推定装置２０６は、加速度ベクトル２１０と磁気ベクトル２１１、角速度ベクトル２１２、移動状態フラグ３１３、ジャイロセンサのゼロ点出力２１４を入力とする。姿勢角推定装置２０６は、たとえば、カルマンフィルタによって実装される。

その状態ベクトルは、状態ベクトルとして重力方向ベクトル（３軸）と真北方向ベクトル（３軸）、角速度ベクトル（３軸）の９個の成分を持つ。重力方向ベクトルの観測として、加速度ベクトル２１０を用いて、真北方向ベクトルの観測として、磁気ベクトル２１１から磁北との偏角と伏角を補正した真北ベクトルを用いて、角速度ベクトルの観測として、角速度ベクトル２１２からゼロ点出力２１４を補正したものを用いる。ここで観測と更新によって得られる重力方向ベクトルと真北方向ベクトルの二つを用いて、姿勢角推定装置２０６から、移動体の姿勢角を算出して出力する（２１５）。

２０１ 加速度センサ、
２０２ 磁気センサ、
２０３ ジャイロセンサ、
２０４ 移動状態判定装置、
２０５ ジャイロセンサゼロ点出力推定装置、
２０６ 姿勢角推定装置、
３０４ 移動状態判定装置、

Claims (4)

1. 移動体に装着された加速度センサ、磁気センサ、およびジャイロセンサからの出力により当該移動体の姿勢角を推定して出力する姿勢角推定装置であって、
   角速度ベクトルを出力するジャイロセンサと
   加速度ベクトルを出力する加速度センサと、
   磁場ベクトルを出力する磁気センサと、
   前記加速度センサの出力に基づいて移動体の移動状態を判定する移動状態判定装置と、
   ジャイロセンサゼロ点出力推定装置と、を備え、
   前記ジャイロセンサゼロ点出力推定装置は、重力加速度ベクトルと、磁場ベクトルと、角速度ベクトルと、前記ジャイロセンサの角速度のゼロ点出力のオフセット値とを成分として持つ拡張カルマンフィルタであり、前記移動状態判定装置による前記移動状態の判定結果が、前記移動体が静止していることを示しているときに、前記加速度センサの出力、前記磁気センサの出力、および前記ジャイロセンサの出力に基づき、前記ジャイロセンサのゼロ点出力を推定する処理を行い、
   前記ジャイロセンサゼロ点出力推定装置の出力を利用して、前記ジャイロセンサのゼロ点を補正することを特徴とする移動体の姿勢角処理装置。
2. 請求項１に記載の移動体の姿勢角処理装置において、
   加速度センサ、磁気センサ、およびジャイロセンサの出力により、姿勢角を推定する姿勢角推定装置は、前記ジャイロセンサゼロ点出力推定装置の出力に基づいて、
   移動体の姿勢角を推定した出力を補正する
   ことを特徴する移動体の姿勢角処理装置。
3. 移動体に装着された加速度センサ、磁気センサ、およびジャイロセンサからの出力により当該移動体の姿勢角を推定して出力する姿勢角推定装置であって、
   角速度ベクトルを出力するジャイロセンサと
   加速度ベクトルを出力する加速度センサと、
   磁場ベクトルを出力する磁気センサと、
   前記加速度センサおよび前記ジャイロセンサの出力に基づいて移動体の移動状態を判定する移動状態判定装置と、
   ジャイロセンサゼロ点出力推定装置と、を備え、
   前記ジャイロセンサゼロ点出力推定装置は、重力加速度ベクトルと、磁場ベクトルと、角速度ベクトルと、前記ジャイロセンサの角速度のゼロ点出力のオフセット値とを成分として持つ拡張カルマンフィルタであり、前記移動状態判定装置による前記移動状態の判定結果が、前記移動体が静止していることを示しているときに、前記加速度センサの出力、前記磁気センサの出力、および前記ジャイロセンサの出力に基づき、前記ジャイロセンサのゼロ点出力を推定する処理を行い、
   前記ジャイロセンサゼロ点出力推定装置の出力を利用して、前記ジャイロセンサのゼロ点を補正することを特徴とする移動体の姿勢角処理装置。
4. 請求項３に記載の移動体の姿勢角処理装置において、
   加速度センサ、磁気センサ、およびジャイロセンサの出力により、姿勢角を推定する姿勢角推定装置は、前記ジャイロセンサゼロ点出力推定装置の出力に基づいて、
   移動体の姿勢角を推定した出力を補正する
   ことを特徴する移動体の姿勢角処理装置。

Images