(57)【特許請求の範囲】
【請求項１】
慣性センサと、
記憶装置と、
前記記憶装置に記憶された前記慣性センサの零点ずれを取得する零点ずれ取得部と、
前記零点ずれ取得部が取得する前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正部と、
前記記憶装置に記憶された所定の閾値を取得する閾値取得部と、
前記センサ出力補正部が出力する補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算部と、
前記閾値取得部が取得する閾値と前記運動エネルギ計算部が算出する前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新部と、を有する、
零点ずれ補正システム。
【請求項２】
記憶装置に記憶された慣性センサの零点ずれを取得する零点ずれ取得部と、
前記零点ずれ取得部が取得する前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正部と、
前記記憶装置に記憶された所定の閾値を取得する閾値取得部と、
前記センサ出力補正部が出力する補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算部と、
前記閾値取得部が取得する閾値と前記運動エネルギ計算部が算出する前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新部と、を有する、
零点ずれ補正装置。
【請求項３】
記憶装置に記憶された慣性センサの零点ずれを取得する零点ずれ取得ステップと、
前記零点ずれ取得ステップで取得される前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正ステップと、
前記記憶装置に記憶された所定の閾値を取得する閾値取得ステップと、
前記センサ出力補正ステップで出力される補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算ステップと、
前記閾値取得ステップで取得される閾値と前記運動エネルギ計算ステップで算出される前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新ステップと、を有する、
零点ずれ補正方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、慣性センサの零点ずれを補正する零点ずれ補正システム、零点ずれ補正装置、及び零点ずれ補正方法に関する。
【背景技術】
【０００２】
加速度センサがある監視時間に亘って出力するデータの特徴量と閾値との比較に基づきその加速度センサが取り付けられる移動体が停止したかを判定する装置が知られている（特許文献１参照。）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００２−１３１０７７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上述の装置は、その監視時間内におけるセンサ出力の変動幅又は標準偏差を特徴量として導き出すことを開示するのみであり、停止したかを判定する際のセンサ出力の活用方法に改善の余地がある。
【０００５】
上述に鑑み、慣性センサの出力をより有効に活用して慣性センサの零点ずれを補正する零点ずれ補正システム、零点ずれ補正装置、及び零点ずれ補正方法を提供することが望ましい。
【課題を解決するための手段】
【０００６】
本発明の実施例に係る零点ずれ補正システムは、慣性センサと、記憶装置と、前記記憶装置に記憶された前記慣性センサの零点ずれを取得する零点ずれ取得部と、前記零点ずれ取得部が取得する前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正部と、前記記憶装置に記憶された所定の閾値を取得する閾値取得部と、前記センサ出力補正部が出力する補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算部と、前記閾値取得部が取得する閾値と前記運動エネルギ計算部が算出する前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新部と、を有する。
【０００７】
本発明の実施例に係る零点ずれ補正装置は、記憶装置に記憶された慣性センサの零点ずれを取得する零点ずれ取得部と、前記零点ずれ取得部が取得する前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正部と、前記記憶装置に記憶された所定の閾値を取得する閾値取得部と、前記センサ出力補正部が出力する補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算部と、前記閾値取得部が取得する閾値と前記運動エネルギ計算部が算出する前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新部と、を有する。
【０００８】
本発明の実施例に係る零点ずれ補正方法は、記憶装置に記憶された慣性センサの零点ずれを取得する零点ずれ取得ステップと、前記零点ずれ取得ステップで取得される前記慣性センサの零点ずれを用いて前記慣性センサが出力するセンサ値を補正して補正後センサ値を出力するセンサ出力補正ステップと、前記記憶装置に記憶された所定の閾値を取得する閾値取得ステップと、前記センサ出力補正ステップで出力される補正後センサ値から前記慣性センサの運動エネルギを算出する運動エネルギ計算ステップと、前記閾値取得ステップで取得される閾値と前記運動エネルギ計算ステップで算出される前記慣性センサの運動エネルギとを比較し、前記記憶装置に記憶された前記慣性センサの零点ずれを更新する零点ずれ更新ステップと、を有する。
【発明の効果】
【０００９】
上述の手段により、慣性センサの出力をより有効に活用して慣性センサの零点ずれを補正する零点ずれ補正システム、零点ずれ補正装置、及び零点ずれ補正方法を提供できる。
【図面の簡単な説明】
【００１０】
【図１】本発明の実施例に係る零点ずれ補正システムの構成例を示す機能ブロック図である。
【図２】静止中の移動体に取り付けられた慣性センサの出力の温度特性を示す図である。
【図３】図２の領域Ｒ１の拡大図である。
【図４】慣性センサの３つの出力と判定値の時間的推移を示す図である。
【図５】図３に示す慣性センサのセンサ値の推移に対応する補正後センサ値の積算値の推移を示す図である。
【図６】零点ずれ補正処理の流れを示すフローチャートである。
【発明を実施するための形態】
【００１１】
図１は、本発明の実施例に係る零点ずれ補正システムの構成例を示す機能ブロック図である。零点ずれ補正システム１００は、移動体に取り付けられる慣性センサ２０の零点ずれを補正するシステムであり、主に、零点ずれ補正装置１０、慣性センサ２０、記憶装置２１、及び出力先装置２２を含む。なお、零点ずれ補正装置１０及び記憶装置２１は移動体に取り付けられてもよく移動体に取り付けられることなく移動体の外部に設置されてもよい。同様に、出力先装置２２は移動体に取り付けられてもよく移動体に取り付けられることなく移動体の外部に設置されてもよい。また、各構成要素は無線接続されてもよく有線接続されてもよい。
【００１２】
零点ずれ補正装置１０は慣性センサ２０の出力の零点に対するずれを補正して補正後センサ値を出力する装置である。本実施例では、零点ずれ補正装置１０はＣＰＵ、内部メモリ等を備えるコンピュータで構成される。また、零点ずれ補正装置１０は、零点ずれ取得部１１、センサ出力補正部１２、運動エネルギ計算部１３、閾値取得部１４、零点ずれ更新部１５、及び動き推定部１６を機能要素として有する。そして、零点ずれ補正装置１０は、各機能要素に対応するプログラムを内部メモリにロードしてＣＰＵに実行させることで各機能要素を実現する。
【００１３】
慣性センサ２０は、慣性を利用して測定を行うデバイスであり、例えば、加速度センサ、ジャイロスコープ等を含む。本実施例では、慣性センサ２０は、３軸加速度センサと３軸ジャイロスコープの組み合わせで構成される６軸ＭＥＭＳ（Micro Electro Mechanical System（微小電気機械システム））センサである。具体的には、零点ずれ補正装置１０は慣性センサ２０の６つの出力のそれぞれの零点に対するずれを補正する。慣性センサ２０の６つの出力は、Ｘ軸方向の加速度、Ｙ軸方向の加速度、Ｚ軸方向の加速度、Ｘ軸回りの角速度、Ｙ軸回りの角速度、及びＺ軸回りの角速度である。なお、慣性センサ２０は、１軸センサであってもよく、６軸未満の多軸センサであってもよく、７軸以上の多軸センサであってもよい。
【００１４】
記憶装置２１は各種情報を記憶する不揮発性記憶媒体である。本実施例では、記憶装置２１として半導体メモリが採用される。
【００１５】
出力先装置２２は慣性センサの出力先としての装置である。本実施例では、出力先装置２２は補正後センサ値を出力する零点ずれ補正装置１０の出力先装置であり、例えば、ロボットコントローラ、車両コントローラ等である。なお、出力先装置２２は、零点ずれ補正装置１０が出力する情報を表示するディスプレイ、及び、零点ずれ補正装置１０が出力する情報を音声出力するスピーカであってもよい。
【００１６】
次に、零点ずれ補正装置１０が有する機能要素について説明する。
【００１７】
零点ずれ取得部１１は、慣性センサ２０の零点ずれを取得する機能要素である。本実施例では、零点ずれ取得部１１は記憶装置２１に記憶された慣性センサ２０の６つの出力のそれぞれの零点ずれを取得する。零点ずれは、慣性センサ２０の出力が零であるべきときに慣性センサ２０が実際に出力する出力値である。例えば、Ｘ軸方向の加速度が零であるべきときに慣性センサ２０が実際に出力するＸ軸方向の加速度の値である。零点ずれ補正装置１０は、例えば、慣性センサ２０が実際に出力するＸ軸方向の加速度の値から零点ずれを差し引くことでＸ軸方向の加速度の正味の値を得る。なお、零点ずれは、例えば、慣性センサ２０の出荷時に行われるキャリブレーションによって決まる値であり、零点ずれ補正システム１００の出荷時に記憶装置２１に記憶される。以下では、零点ずれ補正システム１００の出荷時に記憶装置２１に記憶される零点ずれを「基準零点ずれ」とする。また、記憶装置２１に記憶された零点ずれは零点ずれ更新部１５によって更新されるが、零点ずれ補正装置１０が所定時間以上停止していた場合等、所定の条件が満たされた場合には基準零点ずれの値にリセットされてもよい。
【００１８】
センサ出力補正部１２は、慣性センサ２０のセンサ値を補正する機能要素である。本実施例では、センサ出力補正部１２は、零点ずれ取得部１１が記憶装置２１から読み出した零点ずれを用いて慣性センサ２０が出力するセンサ値を補正して補正後センサ値を出力する。例えば、センサ出力補正部１２は、慣性センサ２０が出力するセンサ値から、零点ずれ取得部１１が読み出した零点ずれを差し引いた正味のセンサ値を補正後センサ値として出力する。なお、センサ出力補正部１２は、補正後センサ値（電圧）に所定の電圧物理量変換係数を乗じて物理量（加速度、角速度等）を導き出し、それら物理量を出力先装置２２に対して出力してもよい。
【００１９】
運動エネルギ計算部１３は、慣性センサ２０の運動エネルギを算出する機能要素である。本実施例では、運動エネルギ計算部１３はセンサ出力補正部１２が出力する補正後センサ値から慣性センサ２０の運動エネルギを算出する。具体的には、運動エネルギ計算部１３は慣性センサ２０の６つの出力のそれぞれに関して運動エネルギを算出する。すなわち、６つの運動エネルギを算出する。その上で、運動エネルギ計算部１３は６つの運動エネルギを合計して１つの合計運動エネルギを算出する。なお、Ｘ軸方向の加速度に関する運動エネルギとＹ軸方向の加速度に関する運動エネルギとＺ軸方向の加速度に関する運動エネルギとを合計して加速度に関する運動エネルギを算出してもよい。角速度に関する運動エネルギについても同様である。また、Ｘ軸方向の加速度に関する運動エネルギとＸ軸回りの角速度に関する運動エネルギとを合計してＸ軸に関する運動エネルギを算出してもよい。Ｙ軸に関する運動エネルギ、及び、Ｚ軸に関する運動エネルギについても同様である。
【００２０】
具体的には、運動エネルギ計算部１３は、以下の（１）式を用いてＸ軸方向の加速度に関する並進運動の運動エネルギKx1を所定周期毎に算出する。
【００２１】
【数１】
0006282570.tif 000002
なお、mは慣性センサ２０の質量（定数）を表し、vxは慣性センサ２０のＸ軸方向の速度を表す。また、速度vxは慣性センサ２０の出力の１つであるＸ軸方向の加速度を時間積分することで導き出される。
【００２２】
同様にして、運動エネルギ計算部１３は、Ｙ軸方向の加速度に関する運動エネルギKy1、及び、Ｚ軸方向の加速度に関する運動エネルギKz1を算出する。
【００２３】
また、運動エネルギ計算部１３は、以下の（２）式を用いてＸ軸回りの角速度に関する回転運動の運動エネルギKx2を所定周期毎に算出する。
【００２４】
【数２】
0006282570.tif 000003
なお、Iは慣性センサ２０の慣性モーメント（定数）を表し、ωxは慣性センサ２０のＸ軸回りの角速度を表す。
【００２５】
同様にして、運動エネルギ計算部１３は、Ｙ軸回りの角速度に関する運動エネルギKy2、及び、Ｚ軸回りの角速度に関する運動エネルギKz2を算出する。
【００２６】
また、運動エネルギ計算部１３は、以下の（３）式に示すように、６つの運動エネルギKx1、Kx2、Ky1、Ky2、Kz1、Kz2を合計して合計運動エネルギKを算出する。
【００２７】
【数３】
0006282570.tif 000004
このようにして合計運動エネルギKを所定周期毎に算出することで、零点ずれ補正装置１０は慣性センサ２０の６つの出力の全てを有効に利用できる。そして、慣性センサ２０の６つの出力のうちの１つにしか影響が出ないような移動体の動きをより確実に検出することができる。その結果、移動体が停止しているか否かをより正確に判断できる。
【００２８】
また、零点ずれ補正装置１０は、運動エネルギを算出することで、慣性センサ２０の直接の出力である加速度と角速度のようにそのままでは同列に扱うことのできない単位の異なる物理量を同列に扱うことができる。
【００２９】
閾値取得部１４は、運動エネルギ計算部１３が算出する運動エネルギとの比較に用いられる閾値を取得する機能要素である。本実施例では、閾値取得部１４は記憶装置２１に記憶された閾値を取得する。具体的には、閾値取得部１４は、慣性センサ２０の６つの出力のそれぞれに関して算出される運動エネルギのそれぞれに対応する閾値を取得する。すなわち、６つの閾値を取得する。そして、閾値取得部１４は記憶装置２１から読み出した６つの閾値を合計して１つの閾値を算出する。このように、閾値取得部１４は、運動エネルギ計算部１３が算出する運動エネルギの数及び内容に応じ、取得する閾値の数及び内容を変更する。例えば、運動エネルギ計算部１３が６つの運動エネルギを合計して１つの合計運動エネルギを算出する場合、閾値取得部１４は６つの閾値を合計した１つの閾値を取得する。なお、閾値取得部１４は合計運動エネルギとの比較のために予め記憶された１つの閾値を記憶装置２１から読み出してもよい。
【００３０】
閾値は、主に、慣性センサ２０のセンサ値におけるノイズの大きさ、慣性センサ２０の使用環境、及び、慣性センサ２０の温度特性における所要の線形性に基づいて決定される。本実施例では、センサ値としての電圧値に関する電気的ノイズの大きさ（以下、「静的ノイズレベル」とする。）が大きいほど、或いは、使用環境温度の範囲が広いほど、或いは、要求される線形性の範囲が広いほど、閾値は大きめに設定される。
【００３１】
図２は、静止中の移動体に取り付けられた慣性センサ２０の３つの出力の温度特性を示す図である。具体的には、実線はＸ軸回りの角速度に関する出力の推移を表し、破線はＹ軸回りの角速度に関する出力の推移を表し、点線はＺ軸回りの角速度に関する出力の推移を表す。なお、縦軸は角速度［度／秒］に対応し、横軸は温度［℃］に対応する。また、温度Ｔｍｉｎは温度特性の線形性が維持される範囲の下限温度を表し、温度Ｔｍａｘは温度特性の線形性が維持される範囲の上限温度を表す。
【００３２】
また、図２は、慣性センサの出力の温度特性がセンサ毎に異なるように、同じ１つの慣性センサであっても各出力の温度特性が異なることを示す。具体的には、図２は、少なくとも温度が同じであれば３軸回りの角速度に関する各出力が何れも値ゼロを示すべきところ、温度Ｔ１においてＸ軸回りの角速度ωｘ１がＹ軸回りの角速度ωｙ１より大きく、また、Ｙ軸回りの角速度ωｙ１がＺ軸回りの角速度ωｚ１より大きい状態を示す。
【００３３】
図３は、図２の領域Ｒ１の拡大図であり、角速度の幅ＮＬは静的ノイズレベルを表す。静的ノイズレベルは各出力でほぼ同じであり温度変化に応じて増減することもない。
【００３４】
なお、本実施例では零点ずれ及び閾値は同じ記憶装置２１に記憶されるが別の実施例では別々の記憶装置に記憶されていてもよい。また、零点ずれは複数の記憶装置に重複して或いは分散して記憶されてもよい。閾値についても同様である。
【００３５】
零点ずれ更新部１５は、所定の条件が満たされた場合に零点ずれを更新する機能要素である。本実施例では、零点ずれ更新部１５は、運動エネルギ計算部１３が算出する慣性センサ２０の運動エネルギと閾値取得部１４が取得する閾値とを比較する。そして、慣性センサ２０の運動エネルギが閾値以下であると判定した場合に記憶装置２１に記憶された慣性センサ２０の零点ずれを更新する。慣性センサ２０が取り付けられた移動体が停止していると判断できるためである。具体的には、零点ずれ更新部１５は、合計運動エネルギが閾値以下であると判定した場合に記憶装置２１に記憶された零点ずれの値を現在のセンサ値で書き換える。一方で、零点ずれ更新部１５は、合計運動エネルギが閾値より大きいと判定した場合には零点ずれの値を更新しない。慣性センサ２０が取り付けられた移動体が停止していないと判断できるためである。
【００３６】
或いは、零点ずれ更新部１５は、６つの運動エネルギのそれぞれが対応する閾値以下であると判定した場合に記憶装置２１に記憶された６つの零点ずれの値を現在の６つのセンサ値で書き換えてもよい。この場合、零点ずれ更新部１５は、６つの運動エネルギのうちの何れか１つが閾値より大きいと判定した場合には零点ずれを更新しない。慣性センサ２０が取り付けられた移動体が停止していないと判断できるためである。運動エネルギ計算部１３が６つ未満或いは７つ以上の運動エネルギを算出する場合についても同様である。
【００３７】
また、複数の慣性センサ２０が移動体に取り付けられている場合、零点ずれ更新部１５は、複数の慣性センサ２０のそれぞれに関する合計運動エネルギの全てが閾値以下であると判定した場合に記憶装置２１に記憶された零点ずれの値を現在のセンサ値で書き換えてもよい。すなわち、零点ずれ更新部１５は、複数の慣性センサ２０のそれぞれに関する合計運動エネルギが１つでも閾値より大きければ零点ずれの値を更新しないようにしてもよい。
【００３８】
図４は、慣性センサ２０の３つの出力と判定値の時間的推移を示す図である。慣性センサ２０の３つの出力はＸ軸回りの角速度ωｘ、Ｙ軸回りの角速度ωｙ、Ｚ軸回りの角速度ωｚである。また、判定値は零点ずれ更新部１５の判定結果を表す。具体的には、値「１」は慣性センサ２０の合計運動エネルギが閾値以下であると零点ずれ更新部１５が判定したことを表し、値「０」は慣性センサ２０の合計運動エネルギが閾値より大きいと零点ずれ更新部１５が判定したことを表す。なお、図４の４つのグラフの時間軸は共通である。
【００３９】
図４に示すように、零点ずれ更新部１５は、判定値が値「１」の場合に慣性センサ２０の現在の出力（センサ値）で零点ずれの値を書き換え、判定値が値「０」の場合には書き換えを行わない。
【００４０】
動き推定部１６は、慣性センサ２０が取り付けられた移動体の動きを推定する機能要素である。本実施例では、動き推定部１６は、センサ出力補正部１２が出力する補正後センサ値を積算して移動体の並進及び回転（姿勢）の状態を推定する。そして、動き推定部１６は推定結果を出力先装置２２に対して出力する。そのため、出力先装置２２としてのロボットコントローラ又は車両コントローラはその推定結果に基づいてロボット又は車両の動きを制御できる。或いは、出力先装置２２としてのディスプレイはその推定結果を表示でき、出力先装置２２としてのスピーカはその推定結果を音声出力できる。
【００４１】
図５は、図３に示す角速度の時間的推移から推定される回転角度の時間的推移を示す図である。具体的には、図５は静止中の移動体に取り付けられた慣性センサ２０の出力のうちのＸ軸回りの角速度ωｘに関する補正後センサ値を積算することで導き出されるＸ軸回りの回転角度の時間的推移を示す。なお、縦軸は回転角度に対応し、横軸は時間に対応する。また、実線は零点ずれ更新部１５によって零点ずれが更新される場合の推移を表し、破線は図３に示す初期零点ずれＳＰで各センサ値を補正した場合の推移を表し、点線は補正無しの場合の推移を表す。
【００４２】
図５で示すように、移動体が静止している場合であっても、慣性センサ２０のセンサ値をそのまま積算して得られる移動体の回転角度は点線で示すように時間の経過と共に大きく増加していく。センサ値が零点ずれを含み、且つ、慣性センサ２０の温度変化の影響を受けるためである。同様に、慣性センサ２０が制御周期毎に出力するセンサ値のそれぞれから初期零点ずれＳＰを差し引いた補正後センサ値のそれぞれを積算して得られる移動体の回転角度は、補正無しの場合に比べて小さいものの、破線で示すように時間の経過と共に増加していく。零点ずれによる影響が取り除かれるものの、温度変化の影響を依然として受けるためである。これに対し、慣性センサ２０が制御周期毎に出力するセンサ値のそれぞれから零点ずれ更新部１５によって継続的に更新される零点ずれを差し引いた補正後センサ値のそれぞれを積算して得られる移動体の回転角度は、増加することなくほぼ一定のまま推移する。零点ずれ及び温度変化による影響の双方が取り除かれるためである。
【００４３】
次に、図６を参照し、零点ずれ補正装置１０が慣性センサ２０の零点ずれを補正する処理（以下、「零点ずれ補正処理」とする。）について説明する。なお、図６は、零点ずれ補正処理の流れを示すフローチャートである。零点ずれ補正装置１０は、所定の制御周期で繰り返しこの零点ずれ補正処理を実行する。
【００４４】
最初に、零点ずれ補正装置１０は零点ずれを取得する（ステップＳ１）。本実施例では、零点ずれ補正装置１０の零点ずれ取得部１１は記憶装置２１の所定領域に記憶された零点ずれを読み出す。
【００４５】
その後、零点ずれ補正装置１０は慣性センサ２０が出力するセンサ値を取得する（ステップＳ２）。本実施例では、零点ずれ補正装置１０は慣性センサ２０が出力する６つのセンサ値を取得する。
【００４６】
その後、零点ずれ補正装置１０の閾値取得部１４は閾値を取得する（ステップＳ３）。本実施例では、閾値取得部１４は記憶装置２１の所定領域に記憶された６つの零点ずれを読み出す。
【００４７】
その後、零点ずれ補正装置１０のセンサ出力補正部１２はセンサ値を補正する（ステップＳ４）。本実施例では、センサ出力補正部１２は慣性センサ２０が出力するセンサ値から零点ずれの値を差し引いて補正後センサ値を出力する。なお、センサ出力補正部１２は、補正後センサ値（電圧）に所定の電圧物理量変換係数を乗じて物理量（加速度、角速度等）を導き出し、それら物理量を出力先装置２２に対して出力してもよい。
【００４８】
その後、零点ずれ補正装置１０の運動エネルギ計算部１３は慣性センサ２０の運動エネルギを算出する（ステップＳ５）。本実施例では、運動エネルギ計算部１３はセンサ出力補正部１２が出力する補正後センサ値に基づいて６つの運動エネルギを算出する。
【００４９】
なお、ステップＳ１、Ｓ２、及びＳ３は順不同であり同時に実行されてもよい。また、ステップＳ３はステップＳ４又はＳ５の後に実行されてもよくステップＳ４又はＳ５と同時に実行されてもよい。
【００５０】
その後、零点ずれ補正装置１０の零点ずれ更新部１５は運動エネルギと閾値とを比較する（ステップＳ６）。本実施例では、零点ずれ更新部１５は運動エネルギ計算部１３が算出した合計運動エネルギと閾値取得部１４が取得した閾値とを比較する。なお、零点ずれ更新部１５は運動エネルギ計算部１３が算出した６つの運動エネルギと閾値取得部１４が取得した６つの閾値とを対応付けて比較してもよい。
【００５１】
合計運動エネルギが閾値以下であると判定した場合（ステップＳ６のＹＥＳ）、零点ずれ更新部１５は零点ずれを更新する（ステップＳ７）。本実施例では、合計運動エネルギが閾値以下であると判定した場合、零点ずれ更新部１５は慣性センサ２０が取り付けられた移動体が停止していると判断する。そして、記憶装置２１に記憶された零点ずれの値を現在の補正後センサ値で書き換える。なお、零点ずれ更新部１５は、６つの運動エネルギのそれぞれが対応する閾値以下であると判定した場合に慣性センサ２０が取り付けられた移動体が停止していると判断してもよい。
【００５２】
一方で、合計運動エネルギが閾値より大きいと判定した場合（ステップＳ６のＮＯ）、零点ずれ更新部１５は零点ずれを更新することなく今回の零点ずれ補正処理を終了させる。本実施例では、合計運動エネルギが閾値より大きいと判定した場合、零点ずれ更新部１５は慣性センサ２０が取り付けられた移動体が停止していないと判断する。そして、記憶装置２１に記憶された零点ずれの値を現在の補正後センサ値で書き換えることなく今回の零点ずれ補正処理を終了させる。なお、零点ずれ更新部１５は、６つの運動エネルギの何れかが対応する閾値より大きいと判定した場合に慣性センサ２０が取り付けられた移動体が停止していないと判断してもよい。
【００５３】
以上の構成により、零点ずれ補正システム１００は、慣性センサ２０の出力から算出される１又は複数の運動エネルギの値に基づいて慣性センサ２０が取り付けられた移動体が停止しているか否かを判断する。そして、移動体が停止していると判断した場合、記憶装置２１に記憶された零点ずれを更新する。そのため、移動体が停止している場合に例えば慣性センサ２０の温度が上昇し、慣性センサ２０が出力するセンサ値と基準零点ずれとの差が増大する場合であっても、そのセンサ値を適切に補正できる。その結果、移動体が停止しているにもかかわらず零点ずれが積算されてしまい移動体が並進し或いは回転していると推定してしまうのをより確実に防止できる。
【００５４】
また、零点ずれ補正システム１００は、慣性センサ２０の軸数が多いほど、移動体が停止しているか否かの判定結果の信頼性を向上させることができる。特定の軸にしか影響しない移動体の動きをより確実に検出できるためである。また、零点ずれ補正システム１００は、移動体が停止していると判断する毎に零点ずれの値を更新するため、慣性センサ２０の温度を検出するための温度センサを用いなくとも慣性センサ２０の個体毎に異なる温度特性の影響を排除できる。そのため、軸数が多いが温度センサが実装されていない安価なＭＥＭＳセンサの零点ずれを補正する際に特に有効である。
【００５５】
また、零点ずれ補正システム１００は、ハイパスフィルタを用いないため低周波数の感度を犠牲にすることもない。
【００５６】
以上、本発明の好ましい実施例について詳説したが、本発明は、上述した実施例に制限されることはなく、本発明の範囲を逸脱することなしに上述した実施例に種々の変形及び置換を加えることができる。
【符号の説明】
【００５７】
１０・・・零点ずれ補正装置  １１・・・零点ずれ取得部  １２・・・センサ出力補正部  １３・・・運動エネルギ計算部  １４・・・閾値取得部  １５・・・零点ずれ更新部  １６・・・動き推定部  ２０・・・慣性センサ  ２１・・・記憶装置  ２２・・・出力先装置  １００・・・零点ずれ補正システム
【図１】
0006282570.tif 000005
【図２】
0006282570.tif 000006
【図３】
0006282570.tif 000007
【図４】
0006282570.tif 000008
【図５】
0006282570.tif 000009
【図６】
0006282570.tif 000010