JP2011185669A - 姿勢情報算出装置、姿勢情報算出システム、姿勢情報算出方法及び姿勢情報算出プログラム - Google Patents

Abstract

【課題】簡易な構成でありながら、位置の発散を抑制して仮想３次元空間における姿勢を適切に算出することができる姿勢情報算出装置、姿勢情報算出システム、姿勢情報算出方法及び姿勢情報算出プログラムを提供する。
【解決手段】センサー情報取得部２１０は、３つの角速度センサーがそれぞれ検出する３軸回りの角速度情報（ＧＸ、ＧＹ、ＧＺ）と３つの加速度センサーがそれぞれ検出する３軸方向の加速度情報（ＡＸ、ＡＹ、ＡＺ）を取得する。姿勢情報算出部２２０は、角速度情報（ＧＸ、ＧＹ、ＧＺ）と加速度情報（ＡＸ、ＡＹ、ＡＺ）に基づいて、仮想３次元空間における姿勢角と位置座標を算出する。特に、姿勢情報算出部２２０は、加速度情報（ＡＸ、ＡＹ、ＡＺ）から得られる慣性座標系加速度ベクトル（Ａ）に基づいて固定座標系速度ベクトルを算出し、当該固定座標系速度ベクトルに対応づけて仮想３次元空間における位置座標を算出する。
【選択図】図３

Description

本発明は、姿勢情報算出装置、姿勢情報算出システム、姿勢情報算出方法及び姿勢情報算出プログラムに関する。

近年、慣性センサーを用いて物体の姿勢を算出するシステムが様々な場面において利用されている。例えば、自動車やヘリコプター等の移動体に３軸角速度センサーと３軸加速度センサーを取り付け、センサーから得られる情報に基づいて移動体の姿勢を計算することにより、移動体の姿勢の安定制御を行うシステム等が実現されている。

一般に、物体の姿勢は姿勢角と位置により特定することができる。物体の姿勢角は、３軸角速度センサーが検出した角速度ベクトルを１階積分することで得られる。ところが、３軸角速度センサーの出力値には微小なオフセットが含まれるため、角速度ベクトルを１階積分することによりオフセット分が徐々に蓄積されてしまう。そのため、計算により得られる姿勢角の真の姿勢角に対する誤差がオフセットに比例して徐々に増大し、例えば、物体が静止しているにもかかわらず微小な角速度で回転しているかのような結果が得られることになる。この問題を解決する手法として、カルマンフィルターを用いて、センサー出力値から姿勢角の真値を推定する手法が知られている。

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

一方、物体の位置は、３軸加速度センサーが検出した加速度ベクトルを２階積分することで得られる。３軸加速度センサーの出力値にも微小なオフセットが含まれるので、オフセット分が徐々に蓄積されていき、物体が静止しているにもかかわらず移動しているかのような結果が得られることになる。しかも、加速度ベクトルを２階積分するので、計算により得られる位置の真の位置に対する誤差が増加する速度はオフセットの２乗に比例し、姿勢角の誤差が増加する速度よりもずっと速い。そのため、カルマンフィルターを用いて位置の真値を推定するためには、カルマンゲインを大きくしなければならない。しかし、カルマンゲインを大きくすると、物体の計算上の位置を真の位置に引き戻す速度が速すぎて、実際には物体がゆっくり移動しているのに静止しているかのような計算結果が得られることになる。特に、センサーのオフセットが比較的大きければカルマンゲインをより大きく設定しなければならなくなり、カルマンフィルターを用いて加速度センサーの出力値のみで物体の位置を推定することは現実的に極めて困難である。

この問題を解決するために、現実的には、加速度センサーの出力値と外部から得られる特定の情報に基づいて、物体の位置を推定する手法が採用されている。例えば、赤外線発光装置を所定位置に固定し、物体に固定されたＣＣＤカメラでこの赤外線発光装置を撮像することで物体と赤外線発光装置との相対的な位置関係の情報を得ることができるので、この情報を用いてカルマンフィルターにより物体の位置の真値を推定することができる。しかし、このような手法では、センサー以外の装置が必要になるためシステムが大型化する。

その一方で、例えば、ポインティングデバイスを用いて仮想３Ｄモデルを操作するような用途では、時間が経過しても仮想３Ｄモデルの位置が発散しないことと、ポインティングデバイスの動きに追従して仮想３Ｄモデルが動くことは要求されるが、仮想３Ｄモデルの厳密な位置の算出までは要求されない場合もある。このような厳密な位置の算出が不要なシステムでは低コスト化の要求も高く、システムの大型化は望ましくない。

本発明は、以上のような問題点に鑑みてなされたものであり、本発明のいくつかの態様によれば、簡易な構成でありながら、位置の発散を抑制して仮想３次元空間における姿勢を適切に算出することができる姿勢情報算出装置、姿勢情報算出システム、姿勢情報算出方法及び姿勢情報算出プログラムを提供することができる。

（１）本発明は、３つの角速度センサーがそれぞれ検出する互いに平行でない３軸回りの角速度情報と、３つの加速度センサーがそれぞれ検出する互いに平行でない３軸方向の加速度情報と、に基づいて仮想３次元空間における姿勢角と位置座標を算出する姿勢情報算出装置であって、前記３軸回りの角速度情報と前記３軸方向の加速度情報を取得するセンサー情報取得部と、前記センサー情報取得部が取得した前記３軸回りの角速度情報と前記３軸方向の加速度情報に基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を算出する姿勢情報算出部と、を含み、前記姿勢情報算出部は、前記３軸方向の加速度情報から得られる慣性座標系加速度ベクトルに基づいて固定座標系速度ベクトルを算出し、当該固定座標系速度ベクトルに対応づけて前記仮想３次元空間における前記位置座標を算出する。

本発明に係る姿勢情報算出装置では、まず、慣性座標系（センサー座標系）の加速度ベクトルを固定座標系の速度ベクトルに変換する。ここで、慣性座標系の加速度ベクトルから固定座標系の位置（距離）を算出するには２階積分が必要だが、慣性座標系の加速度ベクトルから固定座標系の速度ベクトルを算出するには１階積分で足りる。さらに、本発明に係る姿勢情報算出装置では、この固定座標系の速度ベクトルに対応づけて仮想３次元空間における位置座標を生成する点に特徴がある。こうすることにより、センサーオフセットに起因する位置座標（加速度ベクトルを１階積分して得られる）のドリフト量と姿勢角（角速度ベクトルを１階積分して得られる）のドリフト量は同程度であり、従来より姿勢角の算出に適用されてきたカルマンフィルターを用いた手法等が本発明における位置座標の算出にも適用することができる。従って、本発明によれば、簡易な構成でありながら位置の発散を抑制して仮想３次元空間における姿勢を適切に算出することができる姿勢情報算出装置を提供することができる。

（２）この姿勢情報算出装置において、前記姿勢情報算出部は、前記３軸回りの角速度情報と３軸方向の加速度情報に基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を特定するために必要な所与のパラメーターを更新する姿勢情報更新部と、前記姿勢情報更新部が更新した後の前記パラメーターを、所与の拘束条件に従い修正する姿勢情報修正部と、前記姿勢情報修正部が修正した後の前記パラメーターに基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を算出する姿勢情報生成部と、を含み、前記姿勢情報更新部は、前記慣性座標系加速度ベクトルと時刻ｋ−１における修正後の前記固定座標系速度ベクトルに基づいて、時刻ｋにおける前記固定座標系速度ベクトルを更新する速度ベクトル更新手段を含み、前記姿勢情報修正部は、前記固定座標系速度ベクトルのノルムを０とする拘束条件に従い、前記速度ベクトル更新手段が更新した時刻ｋにおける前記固定座標系速度ベクトルを修正する速度ベクトル修正手段を含み、前記姿勢情報生成部は、前記速度ベクトル修正手段が修正した時刻ｋにおける前記固定座標系速度ベクトルに対応づけて前記仮想３次元空間における前記位置座標を生成する位置座標生成手段を含むようにしてもよい。

この姿勢情報算出装置では、固定座標系速度ベクトルの更新処理を行った後、ノルムを０とする（すなわち、固定座標系速度ベクトルを（０，０，０）とする）拘束条件に従って修正処理を行い、修正後の固定座標系速度ベクトルに対応づけて仮想３次元空間における位置座標を生成する。こうすることにより、固定座標系速度ベクトル（０，０，０）に対応づけられた所定座標（例えば原点）に引き戻されるように位置座標が生成されるので、オフセットドリフトに起因する位置座標の発散を抑制することができる。

また、この姿勢情報算出装置によれば、固定座標系の速度ベクトルを仮想３次元空間の位置座標に対応づける（固定座標系の位置座標を仮想３次元空間の位置座標に対応づけるわけではない）ので、位置座標がセンサー出力に適切に追従するように、所定座標に引き戻される速度を比較的容易に調整することができる。

（３）この姿勢情報算出装置において、前記速度ベクトル更新手段は、前記慣性座標系加速度ベクトルを、時刻ｋ−１における前記固定座標系姿勢角ベクトルに基づいて回転させて前記固定座標系加速度ベクトルに変換し、当該固定座標系加速度ベクトルから固定座標系重力加速度ベクトルを減算し、減算結果を所定の単位時間で１階積分して単位時間固定座標系速度ベクトルを算出し、時刻ｋ−１における修正後の前記固定座標系速度ベクトルに前記単位時間固定座標系速度ベクトルを加算することにより時刻ｋにおける前記固定座標系速度ベクトルを更新するようにしてもよい。

（４）この姿勢情報算出装置において、前記位置座標生成手段は、時刻ｋにおける修正後の前記固定座標系速度ベクトルを、時刻ｋにおける前記位置座標とするようにしてもよい。

こうすることにより、修正後の固定座標系速度ベクトルから位置座標を生成する計算が不要となるので、計算量を低減することができる。

（５）この姿勢情報算出装置において、前記姿勢情報算出部は、前記３軸回りの角速度情報から得られる慣性座標系角速度ベクトルに基づいて固定座標系姿勢角ベクトルを算出し、当該固定座標系姿勢角ベクトルに基づいて前記仮想３次元空間における前記姿勢角を算出し、前記姿勢情報更新部は、前記慣性座標系角速度ベクトルと時刻ｋ−１における修正後の前記固定座標系姿勢角ベクトルに基づいて、時刻ｋにおける前記固定座標系姿勢角ベクトルを更新する姿勢角ベクトル更新手段を含み、前記姿勢情報修正部は、前記慣性座標系加速度ベクトルに基づく拘束条件に従い、前記姿勢角ベクトル更新手段が更新した時刻ｋにおける前記固定座標系姿勢角ベクトルを修正する姿勢角ベクトル修正手段を含み、前記姿勢情報生成部は、修正後の前記固定座標系姿勢角ベクトルに基づいて、前記仮想３次元空間における前記姿勢角を生成する姿勢角生成手段を含むようにしてもよい。

この姿勢情報算出装置では、固定座標系姿勢角ベクトルの更新処理を行った後、慣性座標系加速度ベクトルに基づく拘束条件に従って修正処理を行い、修正後の固定座標系姿勢角ベクトルに対応づけて仮想３次元空間における姿勢角を生成する。こうすることにより、現在の慣性座標系加速度ベクトルに基づく姿勢に基づく姿勢角が生成されるので、センサーオフセットに起因する姿勢角の発散を抑制することができる。

（６）この姿勢情報算出装置において、前記姿勢角ベクトル更新手段は、前記慣性座標系角速度ベクトルを所定の単位時間で１階積分して単位時間回転角クオータ二オンを算出し、時刻ｋ−１における修正後の前記固定座標系姿勢角ベクトルに前記単位時間回転角クオータ二オンを作用させることにより時刻ｋにおける前記固定座標系姿勢角ベクトルを更新するようにしてもよい。

（７）この姿勢情報算出装置において、前記姿勢角生成手段は、時刻ｋにおける修正後の前記固定座標系姿勢角ベクトルを、時刻ｋにおける前記姿勢角とするようにしてもよい。

こうすることにより、修正後の固定座標系姿勢角ベクトルから姿勢角を生成する計算が不要となるので、計算量を低減することができる。

（８）この姿勢情報算出装置において、前記センサー情報取得部は、温度センサーが検出する温度情報を受け取り、前記姿勢情報更新部は、前記温度情報と時刻ｋ−１における修正後の前記３つの角速度センサーのオフセットに基づいて、時刻ｋにおける前記３つの角速度センサーのオフセットを更新する角速度オフセット更新手段と、前記温度情報と時刻ｋ−１における修正後の前記３つの加速度センサーのオフセットに基づいて、時刻ｋにおける前記３つの加速度センサーのオフセットを更新する加速度オフセット更新手段と、を含み、前記姿勢情報修正部は、所与の拘束条件に従い、前記角速度オフセット更新手段が更新した時刻ｋにおける前記３つの角速度センサーのオフセットを修正する角速度オフセット修正手段と、所与の拘束条件に従い、前記加速度オフセット更新手段が更新した時刻ｋにおける前記３つの加速度センサーのオフセットを修正する加速度オフセット修正手段と、を含むようにしてもよい。

こうすることにより、温度によるセンサーオフセットの変動を考慮してセンサーオフセットを更新することができる。そのため、センサーオフセットによるドリフト量が低減し、位置座標や姿勢角が引き戻される速度をさらに容易に調整することができる。従って、仮想３次元空間における位置座標と姿勢角を、センサー出力に、より迅速に追従させることができる。

（９）本発明は、互いに平行でない３軸回りの角速度情報をそれぞれ検出する３つの角速度センサーと、互いに平行でない３軸方向の加速度情報をそれぞれ検出する３つの加速度センサーと、上記のいずれかの姿勢情報算出装置と、を含む、姿勢情報算出システムである。

（１０）本発明は、３つの角速度センサーがそれぞれ検出する互いに平行でない３軸回りの角速度情報と、３つの加速度センサーがそれぞれ検出する互いに平行でない３軸方向の加速度情報と、に基づいて仮想３次元空間における姿勢角と位置座標を算出する姿勢情報算出方法であって、前記３軸回りの角速度情報と前記３軸方向の加速度情報を取得するセンサー情報取得ステップと、前記センサー情報取得ステップにおいて取得した前記３軸回りの角速度情報と前記３軸方向の加速度情報に基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を算出する姿勢情報算出ステップと、を含み、前記姿勢情報算出ステップにおいて、前記３軸方向の加速度情報から得られる慣性座標系加速度ベクトルに基づいて固定座標系速度ベクトルを算出し、当該固定座標系速度ベクトルに対応づけて前記仮想３次元空間における前記位置座標を算出する。

（１１）本発明は、３つの角速度センサーがそれぞれ検出する互いに平行でない３軸回りの角速度情報と、３つの加速度センサーがそれぞれ検出する互いに平行でない３軸方向の加速度情報と、に基づいて仮想３次元空間における姿勢角と位置座標を算出する姿勢情報算出プログラムであって、前記３軸回りの角速度情報と前記３軸方向の加速度情報に基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を算出する姿勢情報算出部としてコンピューターを機能させ、前記姿勢情報算出部は、前記３軸方向の加速度情報から得られる慣性座標系加速度ベクトルに基づいて固定座標系速度ベクトルを算出し、当該固定座標系速度ベクトルに対応づけて前記仮想３次元空間における前記位置座標を算出する。

第１実施形態の姿勢情報算出システムの概略構成図。 センサー情報出力装置の構成例を示す図。 姿勢情報算出装置の構成例を示す図。 姿勢情報算出装置の処理手順を示すフローチャート図。 姿勢情報算出部による姿勢情報算出処理の手順を示すフローチャート図。 カルマンフィルターの更新処理の手順の一例を示すフローチャート図。 カルマンフィルターの修正処理の手順の一例を示すフローチャート図。 第２実施形態の姿勢情報算出システムの概略構成図。 センサー情報出力装置の構成例を示す図。 姿勢情報算出装置の構成例を示す図。 カルマンフィルターの更新処理の手順の一例を示すフローチャート図。 姿勢情報算出システムを適用したゲームシステムの一例を示す図。

以下、本発明の好適な実施形態について図面を用いて詳細に説明する。なお、以下に説明する実施の形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また以下で説明される構成の全てが本発明の必須構成要件であるとは限らない。

１．第１実施形態
１−１．姿勢情報算出システムの構成
図１は、第１実施形態の姿勢情報算出システムの概略構成図である。図１に示すように、第１実施形態の姿勢情報算出システム１Ａは、センサー情報出力装置１０Ａと姿勢情報算出装置２０Ａを含んで構成されている。

センサー情報出力装置１０Ａは、互いに直交する３軸（Ｘ軸、Ｙ軸、Ｚ軸）方向の角速度情報をそれぞれ検出する３つのジャイロセンサー（角速度センサー）１１０Ｘ、１１０Ｙ、１１０Ｚを含み、当該Ｘ軸、Ｙ軸、Ｚ軸方向の角速度情報を出力する。さらに、センサー装置１０Ａは、互いに直交する３軸（Ｘ軸、Ｙ軸、Ｚ軸）方向の加速度情報をそれぞれ検出する３つの加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚを含み、当該Ｘ軸、Ｙ軸、Ｚ軸方向の加速度情報を出力する。

姿勢情報算出装置２０Ａは、センサー情報取得部２１０と姿勢情報算出部２２０を含んで構成されており、センサー情報出力装置１０Ａによる検出情報（角速度情報と加速度情報）に基づいて仮想３次元空間における姿勢角と位置座標を算出する処理を行う。

センサー情報取得部２１０は、ジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚがそれぞれ検出するＸ軸、Ｙ軸、Ｚ軸方向の角速度情報と加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚがそれぞれ検出するＸ軸、Ｙ軸、Ｚ軸方向の加速度情報を取得する処理（センサー情報取得処理）を行う。

姿勢情報算出部２２０は、センサー情報取得部２１０が取得したＸ軸、Ｙ軸、Ｚ軸方向の角速度情報とＸ軸、Ｙ軸、Ｚ軸方向の加速度情報に基づいて、仮想３次元空間における姿勢角と位置座標を算出する処理（姿勢情報算出処理）を行う。具体的には、姿勢情報算出部２２０は、Ｘ軸、Ｙ軸、Ｚ軸方向の加速度情報から得られる慣性座標系加速度ベクトル（センサー座標系（慣性座標系）での加速度ベクトル）に基づいて固定座標系速度ベクトル（実３次元空間（固定座標系）での速度ベクトル）を算出し、当該固定座標系速度ベクトルに対応づけて仮想３次元空間における位置座標を算出する。また、姿勢情報算出部２２０は、Ｘ軸、Ｙ軸、Ｚ軸方向の角速度情報から得られる慣性座標系角速度ベクトル（センサー座標系（慣性座標系）での角速度ベクトル）に基づいて固定座標系における姿勢角ベクトル（実３次元空間（固定座標系）での姿勢角ベクトル）を算出し、当該固定座標系姿勢角ベクトルに基づいて仮想３次元空間における姿勢角を算出する。

より具体的には、姿勢情報算出部２２０は、姿勢情報更新部２３０、姿勢情報修正部２４０、姿勢情報生成部２５０を含んで構成されており、仮想３次元空間における姿勢角と位置座標を算出する上記の処理を行う。ここで、姿勢情報更新部２３０は、Ｘ軸、Ｙ軸、Ｚ軸回りの角速度情報とＸ軸、Ｙ軸、Ｚ軸方向の加速度情報に基づいて、仮想３次元空間における姿勢角と位置座標を特定するために必要な所与のパラメーターを更新する処理（姿勢情報更新処理）を行う。姿勢情報修正部２４０は、姿勢情報更新部２４０が更新した後のパラメーターを、所与の拘束条件に従い修正する処理（姿勢情報修正処理）を行う。姿勢情報生成部２５０は、姿勢情報修正部２４０が修正した後のパラメーターに基づいて、仮想３次元空間における姿勢角と位置座標を生成する処理（姿勢情報生成処理）を行う。

１−２．センサー情報出力装置の構成
図２は、センサー情報出力装置１０Ａの構成例を示す図である。

センサーモジュール１００には、図１で説明した３つのジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚと３つの加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚが固定されている。ここで、ジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚは、検出軸が互いに直交するように配置されている。同様に、加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚは、検出軸が互いに直交するように配置されている。さらに、ジャイロセンサー１１０Ｘと加速度センサー１１２Ｘ、ジャイロセンサー１１０Ｙと加速度センサー１１２Ｙ、ジャイロセンサー１１０Ｚと加速度センサー１１２Ｚは、互いの検出軸が同じ方向（順に、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向と定義する）を向くように配置されている。

ジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚは、それぞれ、センサーモジュール１００に加わったＸ軸回り、Ｙ軸回り、Ｚ軸回りの角速度に応じた大きさの直流電圧を出力する。また、加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚは、それぞれ、センサーモジュール１００に加わったＸ軸方向、Ｙ軸方向、Ｚ軸方向の加速度に応じた大きさの直流電圧を出力する。

ジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚの各出力電圧は、それぞれ増幅器（ＡＭＰ）１２０Ｘ、１２０Ｙ、１２０Ｚで増幅された後、それぞれローパスフィルター（ＬＰＦ）１３０Ｘ、１３０Ｙ、１３０Ｚを通してノイズ成分が除去される。

同様に、加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚの各出力電圧は、それぞれ増幅器（ＡＭＰ）１２２Ｘ、１２２Ｙ、１２２Ｚで増幅された後、それぞれローパスフィルター（ＬＰＦ）１３２Ｘ、１３２Ｙ、１３２Ｚを通してノイズ成分が除去される。

Ａ／Ｄ変換器（ＡＤＣ）１４０は、ローパスフィルター（ＬＰＦ）１３０Ｘ、１３０Ｙ、１３０Ｚ、１３２Ｘ、１３２Ｙ、１３２Ｚの各出力電圧Ｖ_ＧＸ、Ｖ_ＧＹ、Ｖ_ＧＺ、Ｖ_ＡＸ、Ｖ_ＡＹ、Ｖ_ＡＺを時分割にサンプリングしてそれぞれＮビットのデジタル値ＧＸ、ＧＹ、ＧＺ、ＡＸ、ＡＹ、ＡＺに変換し、時分割に出力する。

ここで、ＧＸ、ＧＹ、ＧＺは、それぞれ、センサーモジュール１００に加わったＸ軸回り、Ｙ軸回り、Ｚ軸回りの角速度の向きと大きさに応じたデジタル値（角速度情報）であり、ＡＸ、ＡＹ、ＡＺは、それぞれ、センサーモジュール１００に加わったＸ軸方向、Ｙ軸方向、Ｚ軸方向の加速度の向きと大きさに応じたデジタル値（加速度情報）である。このデジタル値ＧＸ、ＧＹ、ＧＺ（角速度情報）とＡＸ、ＡＹ、ＡＺ（加速度情報）が、センサー情報出力装置１０Ａから姿勢情報算出装置２０Ａに送信される。

１−３．姿勢情報算出装置の構成
図３は、姿勢情報算出装置２０Ａの構成例を示す図である。

姿勢情報算出装置２０Ａは、ＡＤＣインターフェース回路（ＡＤＣＩ／Ｆ）２１２、マイコンインターフェース回路（マイコンＩ／Ｆ）２１４、マイクロコンピューター３００を含んで構成されている。

ＡＤＣインターフェース回路（ＡＤＣＩ／Ｆ）２１２は、センサー情報出力装置１０Ａが時分割に出力する角速度情報（ＧＸ、ＧＹ、ＧＺ）と加速度情報（ＡＸ、ＡＹ、ＡＺ）を、適当なタイミングで順番にサンプリングして取得する処理を行う。

マイコンインターフェース回路（マイコンＩ／Ｆ）２１４は、ＡＤＣインターフェース回路（ＡＤＣＩ／Ｆ）２１２が取得した角速度情報（ＧＸ、ＧＹ、ＧＺ）から１組の角速度ベクトル（慣性座標系角速度ベクトル）Ｇ＝（ＧＸ，ＧＹ，ＧＺ）を生成する処理を行う。また、マイコンインターフェース回路（マイコンＩ／Ｆ）２１４は、ＡＤＣインターフェース回路（ＡＤＣＩ／Ｆ）２１２が取得した加速度情報（ＡＸ、ＡＹ、ＡＺ）から１組の加速度ベクトル（慣性座標系加速度ベクトル）Ａ＝（ＡＸ，ＡＹ，ＡＺ）を生成する処理を行う。マイコンインターフェース回路（マイコンＩ／Ｆ）２１４は、マイクロコンピューターの仕様に合わせてビット拡張又は丸め処理等の種々の処理も合わせて行う。このようにして生成された慣性座標系角速度ベクトルＧと慣性座標系加速度ベクトルＡはマイクロコンピューター３００に供給される。

なお、ＡＤＣインターフェース回路（ＡＤＣＩ／Ｆ）２１２とマイコンインターフェース回路（マイコンＩ／Ｆ）２１４は、図１に示したセンサー情報取得部２１０として機能する。

マイクロコンピューター３００は、処理部３１０、記憶部３２０、通信部３３０等を含んで構成されている。また、マイクロコンピューター３００は、情報記憶媒体４００を接続可能に構成されている。

情報記憶媒体４００は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＣＤ、ＤＶＤ等）、光磁気ディスク（ＭＯ）、磁気ディスク、ハードディスク、磁気テープ、或いはメモリ（ＲＯＭ）などのハードウェアにより実現することができる。

記憶部３２０は、処理部３１０や通信部３３０などのワーク領域となるもので、その機能はＲＡＭなどのハードウェアにより実現することができる。

処理部３１０は、情報記憶媒体４００に格納されるプログラム（姿勢情報算出プログラム）や情報記憶媒体４００から読み出されたデータなどに基づいて、記憶部３２０をワーク領域として各種処理を行う。即ち情報記憶媒体４００には、本実施形態の各手段としてマイクロコンピューター３００（コンピューターの一例）を機能させるためのプログラム（各手段の処理をコンピュータに実行させるためのプログラム）が記憶される。処理部３１０の機能は各種プロセッサ（ＣＰＵ、ＤＳＰ等）、ＡＳＩＣ（ゲートアレイ等）などのハードウェアや、アプリケーションプログラム、ＯＳ（例えば汎用ＯＳ等）により実現することができる。

通信部３３０は、外部（例えばサーバー装置や他の端末機）との間で通信を行うための各種の制御を行うものであり、その機能は、各種プロセッサ又は通信用ＡＳＩＣなどのハードウェアや、プログラムなどにより実現することができる。

本実施形態では、処理部３１０は、速度ベクトル更新手段２３２、姿勢角ベクトル更新手段２３４、角速度オフセット更新手段２３６、加速度オフセット更新手段２３８を含む。

速度ベクトル更新手段２３２は、慣性座標系加速度ベクトル（Ａ）と時刻ｋ−１における修正後の固定座標系速度ベクトルに基づいて、時刻ｋにおける固定座標系速度ベクトルを更新する処理を行う。例えば、速度ベクトル更新手段２３２は、慣性座標系加速度ベクトル（Ａ）を、時刻ｋ−１における固定座標系姿勢角ベクトルに基づいて回転させて固定座標系加速度ベクトルに変換し、当該固定座標系加速度ベクトルから固定座標系重力加速度ベクトルを減算し、減算結果を所定の単位時間Δｔで１階積分して単位時間固定座標系速度ベクトル（単位時間Δｔにおける速度変化量）を算出し、時刻ｋ−１における修正後の固定座標系速度ベクトルに単位時間固定座標系速度ベクトルを加算することにより時刻ｋにおける固定座標系速度ベクトルを更新するようにしてもよい。

姿勢角ベクトル更新手段２３４は、慣性座標系角速度ベクトル（Ｇ）と時刻ｋ−１における修正後の固定座標系姿勢角ベクトルに基づいて、時刻ｋにおける固定座標系姿勢角ベクトルを更新する処理を行う。例えば、姿勢角ベクトル更新手段２３４は、慣性座標系角速度ベクトル（Ｇ）を所定の単位時間Δｔで１階積分して単位時間回転角クオータ二オン（単位時間Δｔにおける回転角）を算出し、時刻ｋ−１における修正後の固定座標系姿勢角ベクトルに単位時間回転角クオータ二オンを作用させることにより時刻ｋにおける固定座標系姿勢角ベクトルを更新するようにしてもよい。

角速度オフセット更新手段２３６は、時刻ｋにおけるジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚのオフセットを更新する処理を行う。

加速度オフセット更新手段２３８は、時刻ｋにおける加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚのオフセットを更新する処理を行う。

なお、速度ベクトル更新手段２３２、姿勢角ベクトル更新手段２３４、角速度オフセット更新手段２３６、加速度オフセット更新手段２３８は、図１に示した姿勢情報更新部２３０として機能する。

また、本実施形態では、処理部３１０は、速度ベクトル修正手段２４２、姿勢角ベクトル修正手段２４４、角速度オフセット修正手段２４６、加速度オフセット修正手段２４８を含む。

速度ベクトル修正手段２４２は、固定座標系速度ベクトルのノルムを０とする拘束条件に従い、速度ベクトル更新手段２３２が更新した時刻ｋにおける固定座標系速度ベクトルを修正する処理を行う。

姿勢角ベクトル修正手段２４４は、慣性座標系加速度ベクトル（Ａ）に基づく拘束条件に従い、姿勢角ベクトル更新手段２３４が更新した時刻ｋにおける固定座標系姿勢角ベクトルを修正する処理を行う。

角速度オフセット修正手段２４６は、所与の拘束条件に従い、角速度オフセット更新手段２３６が更新した時刻ｋにおけるジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚのオフセットを修正する処理を行う。

加速度オフセット修正手段２４８は、所与の拘束条件に従い、加速度オフセット更新手段２３８が更新した時刻ｋにおける加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚのオフセットを修正する処理を行う。

なお、速度ベクトル修正手段２４２、姿勢角ベクトル修正手段２４４、角速度オフセット修正手段２４６、加速度オフセット修正手段２４８は、図１に示した姿勢情報修正部２４０として機能する。

さらに、本実施形態では、処理部３１０は、位置座標生成手段２５２、姿勢角生成手段２５４を含む。

位置座標生成手段２５２は、速度ベクトル修正手段２４２が修正した時刻ｋにおける固定座標系速度ベクトルに対応づけて仮想３次元空間における位置座標を生成する処理を行う。例えば、位置座標生成手段２５２は、時刻ｋにおける修正後の固定座標系速度ベクトルを、時刻ｋにおける仮想３次元空間での位置座標とするように構成してもよい。

姿勢角生成手段２５４は、修正後の固定座標系姿勢角ベクトルに基づいて、仮想３次元空間における姿勢角を生成する処理を行う。例えば、姿勢角生成手段２５４は、時刻ｋにおける修正後の固定座標系姿勢角ベクトルを、時刻ｋにおける仮想３次元空間での姿勢角とするように構成してもよい。

なお、位置座標生成手段２５２と姿勢角生成手段２５４は、図１に示した姿勢情報生成部２５０として機能する。

また、処理部３１０は、姿勢情報修正部２４０として機能する上記各手段、姿勢情報生成部２５０として機能する上記各手段、姿勢情報生成部２５０として機能する上記各手段
により、図１に示した姿勢情報算出部２２０として機能する。

なお、本実施形態の各手段としてコンピュータを機能させるためのプログラム（姿勢情報算出プログラム）は、ホスト装置（サーバー）が有する情報記憶媒体からネットワーク及び通信部３３０を介して情報記憶媒体４００（記憶部３２０）に配信してもよい。このようなホスト装置（サーバー）の情報記憶媒体の使用も本発明の範囲内に含めることができる。

１−４．姿勢情報算出処理の手順
１−４−１．処理手順の概略
図４（Ａ）は、姿勢情報算出装置２０Ａの処理手順を示すフローチャート図である。図４（Ａ）に示すように、姿勢情報算出装置２０Ａは、終了イベントが発生するまで（ステップＳ３０でＹｅｓの場合）、前述したセンサー情報取得部２１０によるセンサー情報取得処理と姿勢情報算出部２２０による姿勢情報算出処理を繰り返す。

図４（Ｂ）は、姿勢情報算出部２２０による姿勢情報算出処理（図４（Ａ）のステップＳ２０の処理）の手順を示すフローチャート図である。図４（Ｂ）に示すように、姿勢情報算出部２２０は、まず、姿勢情報更新部２３０により姿勢情報更新処理を行う（ステップＳ２２）。具体的には、速度ベクトル更新手段２３２、姿勢角ベクトル更新手段２３４、角速度オフセット更新手段２３６、加速度オフセット更新手段２３８が前述した各処理を行う。

次に、姿勢情報算出部２２０は、姿勢情報修正部２４０により姿勢情報修正処理を行う（ステップＳ２４）。具体的には、速度ベクトル修正手段２４２、姿勢角ベクトル修正手段２４４、角速度オフセット修正手段２４６、加速度オフセット修正手段２４８が前述した各処理を行う。

最後に、姿勢情報算出部２２０は、姿勢情報生成部２５０により姿勢情報生成処理を行う（ステップＳ２６）。具体的には、位置座標生成手段２５２と姿勢角生成手段２５４が前述した各処理を行う。

特に、本実施形態では、姿勢情報算出部２２０は、カルマンフィルターを用いて、所与のパラメーター（変数）の更新処理と修正処理を行い、仮想３次元空間における姿勢角の真値と位置座標の真値を推定する。このカルマンフィルターの更新処理と修正処理は、所定時間を単位時間Δｔとして時刻０、１、・・・の各々で離散的に行われる。この単位時間Δｔは、慣性座標系角速度ベクトルＧ及び慣性座標系加速度ベクトルＡの更新周期や姿勢情報算出システム１Ａを使用するアプリケーションの仕様等に基づいて、適当な時間が決定される。

次に、本実施形態におけるカルマンフィルターの構成について詳細に説明する。

１−４−２．カルマンフィルター
［使用変数］
本実施形態では、カルマンフィルターの更新処理と修正処理において以下に示す変数を使用する。

Δｔ：離散積分用微小ステップ時間（ｓｅｃ）
ｗ_ω：角速度センサーにおける、時間的に相関関係がなく平均が０のノイズ（ｍＶ）
ｗ_α：加速度センサーにおける、時間的に相関関係がなく平均が０のノイズ（ｍＶ）
Ｖ_ｇｉ：３つの角速度センサーが出力する角速度ベクトル（ｍＶ）（慣性座標系）
Ｖ_ａｉ：３つの加速度センサーが出力する加速度ベクトル（ｍＶ）（慣性座標系）
Ｓｅ_ｇ：角速度センサー感度（ｍＶ／ｄｐｓ）
Ｓｅ_ａ：加速度センサー感度（ｍＶ／（ｍ／ｓｅｃ^２））
ｂω_０：角速度センサーの初期オフセット電圧（ｍＶ）
ｂα_０：加速度センサーの初期オフセット電圧（ｍＶ）
ｂω＾_ｋ：時刻ｋにおける更新後の角速度センサーオフセット電圧（ｍＶ）
ｂω_ｋ：時刻ｋにおける修正後の角速度センサーオフセット電圧（ｍＶ）
ｂα＾_ｋ：時刻ｋにおける更新後の加速度センサーオフセット電圧（ｍＶ）
ｂα_ｋ：時刻ｋにおける修正後の加速度センサーオフセット電圧（ｍＶ）
ΔＱｔ_ｇ：３軸微小ステップ時間回転角クオータニオン（度）
Ｑｔ＾_ｋ：時刻ｋにおける更新後の姿勢角ベクトル（固定座標系）
Ｑｔ_ｋ：時刻ｋにおける修正後の姿勢角ベクトル（固定座標系）
ΔＶｅ_ａ：３軸微小ステップ時間速度変化量（ｍ／ｓｅｃ）
Ｖｅ＾_ｋ：時刻ｋにおける更新後の速度ベクトル（固定座標系）
Ｖｅ_ｋ：時刻ｋにおける修正後の速度ベクトル（固定座標系）

本実施形態では、カルマンフィルターによる更新・修正の対象となるパラメーターとして、固定座標系速度ベクトルＶｅ_ｋ（Ｖｅ＾_ｋ）、固定座標系姿勢角ベクトルＱｔ_ｋ（Ｑｔ＾_ｋ）、ジャイロセンサー（１１０Ｘ、１１０Ｙ、１１０Ｚ）のオフセット電圧ｂω_ｋ（ｂω＾_ｋ）、加速度センサー（１１２Ｘ、１１２Ｙ、１１２Ｚ）のオフセット電圧ｂα_ｋ（ｂα＾_ｋ）の４種類のパラメーターを設定する。

固定座標系速度ベクトルＶｅ_ｋ（Ｖｅ＾_ｋ）は、実３次元空間におけるセンサーモジュール１００の速度の向きと大きさを示すものである。固定座標系姿勢角ベクトルＱｔ_ｋ（Ｑｔ＾_ｋ）は、実３次元空間におけるセンサーモジュール１００の姿勢角をベクトルで表すものであり、例えば法線ベクトルを用いることができる。ジャイロセンサーのオフセット電圧ｂω_ｋ（ｂω＾_ｋ）は、角速度が加わっていない時（例えば静止時）に当該ジャイロセンサーが出力する電圧値である。加速度センサーのオフセット電圧電圧ｂα_ｋ（ｂα＾_ｋ）は、加速度が加わっていない時（例えば静止時）に当該加速度センサーが出力する電圧値である。

［処理手順］
図５は、カルマンフィルターを用いた姿勢情報算出処理の手順を示すフローチャート図である。図５に示すように、姿勢情報算出部２２０は、まず、現在の時刻を表す変数ｋを０に初期化した後（ステップＳ１１０）、カルマンフィルターの初期化処理を行う（ステップＳ１２０）。この初期化処理（ステップＳ１２０）では、カルマンフィルターによる更新・修正の対象となるパラメーターに初期値を設定する処理が行われる。ここで、Ｖｅ_０，Ｑｔ_０，ｂω_０，ｂα_０としてあらかじめ決められた固定値を設定してもよい。例えば、Ｖｅ_０としてノルムが０のベクトル（０，０，０）を設定し、Ｑｔ_０として鉛直方向単位ベクトル（０，０，−１）を設定し、ｂω_０やｂα_０としてあらかじめ測定された所定温度（２５℃等）でのオフセット電圧値を設定してもよい。あるいは、起動時に毎回測定を行い、Ｖｅ_０，Ｑｔ_０，ｂω_０，ｂα_０に当該測定値を設定するようにしてもよい。また、ｂω_０，ｂα_０には、前回の姿勢情報算出処理における最終値（前回起動終了時の値）を設定するようにしてもよい。

次に、姿勢情報算出部２２０は、時刻ｋを１だけ増やした後（ステップＳ１３０）、カルマンフィルターの更新処理を行う（ステップＳ１４０）。この更新処理（ステップＳ１４０）では、センサー情報取得部２１０から供給される情報（慣性座標系角速度ベクトル（Ｇ）と慣性座標系加速度ベクトル（Ａ））と時刻ｋ−１におけるパラメーター値に基づいて、時刻ｋにおけるパラメーター値を更新する処理が行われる。

次に、姿勢情報算出部２２０は、カルマンフィルターの修正処理を行う（ステップＳ１５０）。この修正処理（ステップＳ１５０）では、時刻ｋにおける更新後のパラメーター値と所与の拘束条件に基づいて、時刻ｋにおけるパラメーター値を修正する処理が行われる。

次に、姿勢情報算出部２２０は、姿勢情報生成処理を行う（ステップＳ１６０）。この姿勢情報生成処理（ステップＳ１６０）では、時刻ｋにおける修正後のパラメーター値に基づいて、時刻ｋにおける姿勢情報（仮想３次元空間における姿勢角と位置座標）を生成する処理が行われる。

その後、単位時間Δｔが経過するまでに姿勢情報算出処理の終了イベントが発生すると（ステップＳ１７０でＹｅｓの場合）、姿勢情報算出部２２０は姿勢情報算出処理を終了する。一方、単位時間Δｔが経過しても姿勢情報算出処理の終了イベントが発生しなければ（ステップＳ１７０でＮｏの場合）、姿勢情報算出部２２０は、時刻ｋを１だけ増やした後（ステップＳ１３０）、カルマンフィルター更新処理（ステップＳ１４０）、カルマンフィルター修正処理（ステップＳ１５０）、姿勢情報生成処理（ステップＳ１６０）を再度行う。

このようにして、終了イベントが発生するまで、単位時間Δｔ毎にカルマンフィルター更新処理（ステップＳ１４０）とカルマンフィルター修正処理（ステップＳ１５０）が繰り返され、新たな姿勢情報が生成される。

［カルマンフィルター更新処理］
本実施形態におけるカルマンフィルターの更新処理では、後述する修正処理により時刻ｋ−１において修正された後の４種類のパラメーターｂω_ｋ−１、ｂα_ｋ−１、Ｑｔ_ｋ−１、Ｖｅ_ｋ−１（またはこれらの初期値ｂω_０、ｂα_０、Ｑｔ_０、Ｖｅ_０）を、時刻ｋにおいて、それぞれｂω＾_ｋ、ｂα＾_ｋ、Ｑｔ＾_ｋ、Ｖｅ＾_ｋに更新する処理を行う。

拡張カルマンフィルターで用いる更新式は、次式（１）のように表される。

Ｘ＾_ｋ＝（ｂω＾_ｋ，ｂα＾_ｋ，Ｑｔ＾_ｋ，Ｖｅ＾_ｋ）^ｔ、Ｘ_ｋ−１＝（ｂω_ｋ−１，ｂα_ｋ−１，Ｑｔ_ｋ−１，Ｖｅ_ｋ−１）^ｔ、ｕ＝（Ｖ_ｇｉ，Ｖ_ａｉ）、ｗ_ｋ−１＝（ｗ_ω，ｗ_α）である。式（１）は、時刻ｋ−１における修正後のパラメーターＸ_ｋ−１とセンサー出力値ｕとセンサーノイズｗ_ｋ−１に対して関数ｆを適用すると、時刻ｋにおける更新後のパラメーターＸ＾_ｋが得られることを表している。この関数ｆは非常に複雑な関数であるため、式（１）を厳密に計算すると莫大な計算処理量になり、リアルタイムに姿勢情報を算出したい場合には現実的ではない。そこで、本実施形態では、関数ｆのヤコビアンＦ_ｋ−１を定義し、次の更新式（２）を用いる。

更新式（２）によれば、Ｆ_ｋ−１とＸ_ｋ−１の行列積を計算することでＸ＾_ｋが得られるので、リアルタイムな姿勢情報の算出に適している。ここで、前述の通り、Ｘ＾_ｋ＝（ｂω＾_ｋ，ｂα＾_ｋ，Ｑｔ＾_ｋ，Ｖｅ＾_ｋ）^ｔ、Ｘ_ｋ−１＝（ｂω_ｋ−１，ｂα_ｋ−１，Ｑｔ_ｋ−１，Ｖｅ_ｋ−１）^ｔなので、ｂω＾_ｋ，ｂα＾_ｋ，Ｑｔ＾_ｋ，Ｖｅ＾_ｋを求める更新式を定義し、これらの更新式の関係を満たすように種々の行列変形を施すことでＦ_ｋ−１を求めることができる。

本実施形態では、ｂω＾_ｋ，ｂα＾_ｋを求める更新式はそれぞれ次式（３）、（４）のように定義する。

また、本実施形態では、Ｑｔ＾_ｋを求める更新式を次式（５）のように定義する。

式（５）におけるΔＱｔ_ｇは次式（６）により求めることができる。

式（６）におけるωは次式（７）により求めることができる。

関数φは角速度ベクトルをクオータ二オンに変換する関数であり、ｉ，ｊ，ｋは３つのクオータ二オン基底である。また、（ａｒ_ｘ，ａｒ_ｙ，ａｒ_ｚ）は、慣性座標系における角速度ベクトル（慣性座標系角速度ベクトル）であり、ａｒ_ｘ，ａｒ_ｙ，ａｒ_ｚは、それぞれＸ軸回り、Ｙ軸回り、Ｚ軸回りの角速度である。（ａｒ_ｘ，ａｒ_ｙ，ａｒ_ｚ）は次式（８）により求めることができる。

また、本実施形態では、Ｖｅ＾_ｋを求める更新式を次式（９）のように定義する。

ΔＶｅ_ａは次式（１０）により求めることができる。

関数ρは、Ｑｔ_ｋ−１を、初期姿勢角ベクトルＱｔ_０からＱｔ_ｋ−１への回転クオータ二オンに変換する関数である。（ρ（Ｑｔ_ｋ−１））^＊は、（ρ（Ｑｔ_ｋ−１））の共役クオータ二オンである。Ｇは、重力加速度ベクトルである。また、（ｇｒ_ｘ，ｇｒ_ｙ，ｇｒ_ｚ）は、慣性座標系における加速度ベクトル（慣性座標系加速度ベクトル）であり、ｇｒ_ｘ，ｇｒ_ｙ，ｇｒ_ｚは、それぞれＸ軸方向、Ｙ軸方向、Ｚ軸方向の加速度である。（ｇｒ_ｘ，ｇｒ_ｙ，ｇｒ_ｚ）は次式（１１）により求めることができる。

このように定義した４つの更新式（３）、（４）、（５）、（９）を満たすように種々の行列変形を施すことでＦ_ｋ−１を求めることができる。

さらに、本実施形態におけるカルマンフィルターの更新処理では、後述する修正処理により時刻ｋ−１において修正された後のＸ_ｋ−１と真値の残差の共分散行列Ｐ_ｋ−１（：Ｅ［ε_ｋ−１，ε_ｋ−１ ^ｔ］）を、時刻ｋにおいて、Ｆ_ｋ−１を用いてＰ＾_ｋに更新する処理も行う。共分散行列Ｐ＾_ｋを求める更新式は次式（１１）のように定義される。

Ｑ_ｋ−１（：Ｅ［ｗ_ｋ−１，ｗ_ｋ−１ ^ｔ］）は、ｗ_ｋ−１の共分散行列を表す。

次に、カルマンフィルターの更新処理の手順の一例について説明する。図６は、本実施形態におけるカルマンフィルターの更新処理の手順の一例を示すフローチャート図である。

まず、式（８）を用いて、３つのジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚの出力電圧Ｖ_ｇｉから慣性座標系角速度ベクトル（ａｒ_ｘ，ａｒ_ｙ，ａｒ_ｚ）を計算する（ステップＳ１４１）。

次に、式（６）と式（７）を用いて、ステップＳ１４１で計算した慣性座標系角速度ベクトル（ａｒ_ｘ，ａｒ_ｙ，ａｒ_ｚ）から３軸微小ステップ時間回転角クオータニオンΔＱｔ_ｇを計算する（ステップＳ１４２）。

次に、式（１１）を用いて、３つの加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚの出力電圧Ｖ_ａｉから慣性座標系加速度ベクトル（ｇｒ_ｘ，ｇｒ_ｙ，ｇｒ_ｚ）を計算する（ステップＳ１４３）。

次に、式（１０）を用いて、ステップＳ１４３で計算した慣性座標系加速度ベクトルから３軸微小ステップ時間速度変化量ΔＶｅ_ａを計算する（ステップＳ１４４）。

次に、ステップＳ１４２で計算した３軸微小ステップ時間回転角クオータニオンΔＱｔ_ｇとステップＳ１４４で計算した３軸微小ステップ時間速度変化量ΔＶｅ_ａを用いて、式（３）、（４）、（５）、（９）を満たすように種々の行列変形を施してＦ_ｋ−１を作成する（ステップＳ１４５）。

次に、更新式（２）を用いてＦ_ｋ−１とＸ_ｋ−１の行列積を計算し、Ｘ_ｋ−１をＸ＾_ｋに更新する（ステップＳ１４６）。

最後に、更新式（１２）を用いてＰ_ｋ−１をＰ＾_ｋに更新する（ステップＳ１４７）。

［カルマンフィルター修正処理］
本実施形態におけるカルマンフィルターの更新処理では、更新処理による時刻ｋにおける更新後の４種類のパラメーターｂω＾_ｋ、ｂα＾_ｋ、Ｑｔ＾_ｋ、Ｖｅ＾_ｋを、時刻ｋにおいて、それぞれｂω_ｋ、ｂα_ｋ、Ｑｔ_ｋ、Ｖｅ_ｋに修正する処理を行う。

カルマンフィルターの修正処理では、次式（１３）のような観測方程式が定義される。

式（１３）において、Ｚは観測値ベクトル、Ｘは求める値、ｙは観測残差、Ｈは観測方程式傾き成分行列である。

カルマンフィルター修正処理では、観測方程式（１３）における観測残差ｙが最小になるような、すなわち次式（１４）で定義される評価関数Ｊ_ＷＬＳを最小にするようなＸを求める処理を行う。

Ｗは重みを表す対象行列である。

本実施形態では、求める値ＸはＸ_ｋ＝（ｂω_ｋ、ｂα_ｋ、Ｑｔ_ｋ、Ｖｅ_ｋ）^ｔに対応し、観測値ベクトルＺは時刻ｋにおける観測値ベクトルＺ_ｋに対応し、Ｈは時刻ｋ−１における観測方程式傾き成分行列Ｈ_ｋ−１に対応する。ただし、Ｈ_ｋ−１は各時刻で毎回計算しなおしてもよいし、固定値であってもよい。

本実施形態で用いる修正式は次式（１５）のように定義される。

Ｋ_ｋはカルマンゲインであり、次式（１６）のように定義される。

Ｓ_ｋは次式（１７）のように定義される。

Ｒ_ｋ−１は次式（１８）のように定義される。

ここで、修正式（１５）が評価関数Ｊ_ＷＬＳを最小にするようにＸ＾_ｋを修正するということは、Ｚ_ｋ−Ｈ_ｋ−１Ｘ_ｋが最小になる、言い換えるとＨ_ｋ−１Ｘ_ｋがＺ_ｋに近づくように修正処理が行われる。従って、Ｘ_ｋの真値の推定精度を高めるためには、観測値ベクトルＺ_ｋが最も確からしい値であることが望ましい。

そこで、本実施形態では、時刻ｋにおける更新後の姿勢角ベクトルＱｔ＾_ｋを修正するための観測値として、式（１１）を用いて加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚの出力電圧から計算される加速度ベクトル（ｇｒ_ｘ，ｇｒ_ｙ，ｇｒ_ｚ）（＝Ｖ_ａｉ−ｂα_ｋ−１／Ｓｅ_ａ）を採用する。センサーモジュール１００が静止状態の時は加速度センサーには重力加速度のみが加わるので、加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚの出力電圧から計算される重力方向に基づいて、姿勢角ベクトルＱｔ＾_ｋを修正することができる。これにより、ジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚのオフセット誤差から生ずる水平面ドリフトを補正することができる。

また、本実施形態では、時刻ｋにおける更新後の速度ベクトルＶｅ＾_ｋを修正するための観測値として（０，０，０）を採用する。すなわち、時刻ｋにおける修正後の速度ベクトルＶｅ_ｋは（０，０，０）に近づく。そして、本実施形態では、速度ベクトルＶｅ_ｋを仮想３次元空間における位置座標と定義するので、センサーモジュール１００が静止状態の時は仮想３次元空間における位置座標が原点（０，０，０）に引き戻される。これにより、加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚのオフセット誤差から生ずる、仮想３次元空間における位置座標のドリフトを補正することができる。

さらに、本実施形態におけるカルマンフィルターの修正処理では、更新処理により時刻ｋにおいて更新された後の共分散行列Ｐ＾_ｋを、時刻ｋにおいて、Ｋ_ｋとＨ_ｋ−１を用いてＰ_ｋに修正する処理も行う。共分散行列Ｐ＾_ｋの修正式は次式（１９）のように定義される。

次に、カルマンフィルターの修正処理の手順の一例について説明する。図７は、本実施形態におけるカルマンフィルターの修正処理の手順の一例を示すフローチャート図である。

まず、式（１６）、式（１７）、式（１８）を用いて、カルマンゲインＫ_ｋを計算する（ステップＳ１５１）。

次に、観測値ベクトルＺｋを計算する（ステップＳ１５２）。前述のように、特に、本実施形態では、Ｑｔ＾_ｋを修正するための観測値として（ｇｒ_ｘ，ｇｒ_ｙ，ｇｒ_ｚ）を計算し、Ｖｅ＾_ｋを修正するための観測値は（０，０，０）にする。なお、ｂω＾_ｋを修正するための観測値とｂα＾_ｋを修正するための観測値は、一般的な既知の式に従って計算すればよい。

次に、修正式（１５）を用いてＸ＾_ｋをＸ_ｋに修正する（ステップＳ１５３）。

最後に、修正式（１９）を用いてＰ＾_ｋをＰ_ｋに修正する（ステップＳ１５４）。

１−５．効果
以上に説明したように、本実施形態では、慣性座標系（センサー座標系）の加速度ベクトルを固定座標系加速度ベクトルに変換した後に１階積分して得られる固定座標系速度ベクトルを仮想３次元空間における位置座標と１対１に対応づける。こうすることにより、固定座標系速度ベクトルをノルムとカルマンゲインの積に比例する速度で原点に引き戻すことができ、システムを起動してからある程度時間が経過しても、安定して位置座標を生成することができる。より具体的にいうと、本実施形態では、固定座標系速度ベクトルの更新処理を行った後、固定座標系速度ベクトルを（０，０，０）とする拘束条件に従って修正処理を行い、修正後の固定座標系速度ベクトルに対応づけて仮想３次元空間における位置座標を生成する。こうすることにより、原点に引き戻されるように位置座標が生成されるので、センサーオフセットに起因する位置座標の発散を効果的に抑制することができる。さらに、速度ベクトルは加速度ベクトルを１階積分して得られるので、速度ベクトルを位置座標と対応づけることで、位置座標のドリフト量はセンサーオフセットに比例し（従来のように、センサーオフセットの２乗に比例するのではない）、急激な増加が抑えられるので、カルマンゲインを比較的小さくすることができる。

また、本実施形態では、慣性座標系（センサー座標系）の角速度ベクトルを固定座標系角速度ベクトルに変換した後に１階積分して得られる固定座標系姿勢角ベクトルを仮想３次元空間における姿勢角と１対１に対応づける。こうすることにより、センサーオフセットによる姿勢角のドリフト量の急激な増加が抑えられ、カルマンゲインを比較的小さくすることができる。

また、本実施形態では、修正後の固定座標系速度ベクトル及び修正後の固定座標系姿勢角ベクトルを、それぞれ、そのまま仮想３次元空間における位置座標及び姿勢角とするので、あらためて位置座標を生成する計算が不要となり、計算量を低減することができる。

さらに、本実施形態によれば、ユーザーアプリケーションで姿勢を用いる必要があるとき、慣性センサーを用いて位置を算出することができるため、外部にセンサーを用意して、慣性センサーが据え付けられた系、あるいは系の外部に位置するユーザーアプリケーションに加速度データを送信し処理を行う必要がなくなる。

また、位置を算出するシステムが、慣性センサーが位置する系単独でシステムを構成することができるため、複数の系と系間の通信インターフェイスを用いる必要はなくなり、ある程度規模の大きいハードウェア、やステムリソースを必要とすることもなくなる。

また、システムのＧＵＩにおいてウインドウとして３Ｄウインドウを用い、センサーモジュールをポインテッドデバイスとして用いる場合、モジュール慣性力情報（角速度情報、加速度情報）を３Ｄウインドウに物理的に反映するシステムがＵＩとして自然であるが、その際、位置（３Ｄウインドウ）も含めた姿勢を算出するアルゴリズムを提案できる。

このように、本実施形態によれば、簡易な構成でありながら位置座標と姿勢角の発散を抑制して仮想３次元空間における姿勢情報を適切に算出することができる。

２．第２実施形態
２−１．姿勢情報算出システムの構成
図８は、第２実施形態の姿勢情報算出システムの概略構成図である。図８に示すように、第２実施形態の姿勢情報算出システム１Ｂでは、センサー情報出力装置１０Ｂが温度センサー１１４を含んで構成されている点において、第１実施形態の姿勢情報算出システム１Ａと異なる。すなわち、センサー情報出力装置１０Ｂは、互いに直交する３軸（Ｘ軸、Ｙ軸、Ｚ軸）方向の角速度情報と加速度情報に加えて、温度情報も出力する。

姿勢情報算出装置２０Ｂは、センサー情報出力装置１０Ｂによる検出情報（加速度情報、角速度情報、温度情報）に基づいて仮想３次元空間における姿勢角と位置座標を算出する処理を行う。

２−２．センサー情報出力装置の構成
図９は、センサー情報出力装置１０Ｂの構成例を示す図である。センサーモジュール１０２には、第１実施形態におけるセンサーモジュール１００と同様に、３つのジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚと３つの加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚが固定されている。さらに、センサーモジュール１０２は、ジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚ、加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚの近傍に温度センサー１１４が固定されている。温度センサー１１４は、周辺温度に応じた大きさの直流電圧を出力する。

温度センサー１１４の出力電圧は、増幅器（ＡＭＰ）１２４で増幅された後、ローパスフィルター（ＬＰＦ）１３４を通してノイズ成分が除去される。

Ａ／Ｄ変換器（ＡＤＣ）１４０は、ローパスフィルター（ＬＰＦ）１３０Ｘ、１３０Ｙ、１３０Ｚ、１３２Ｘ、１３２Ｙ、１３２Ｚ、１３４の各出力電圧Ｖ_ＧＸ、Ｖ_ＧＹ、Ｖ_ＧＺ、Ｖ_ＡＸ、Ｖ_ＡＹ、Ｖ_ＡＺ、Ｖ_Ｔを時分割にサンプリングしてそれぞれＮビットのデジタル値ＧＸ、ＧＹ、ＧＺ、ＡＸ、ＡＹ、ＡＺ、Ｔ（Ｔは、温度センサー１１４の周辺温度に応じたデジタル値（温度情報））に変換し、時分割に出力する。このデジタル値ＧＸ、ＧＹ、ＧＺ（角速度情報）、ＡＸ、ＡＹ、ＡＺ（加速度情報）、Ｔ（温度情報）が、センサー情報出力装置１０Ｂから姿勢情報算出装置２０Ｂに送信される。

センサー情報出力装置１０Ｂのその他の構成については、センサー情報出力装置１０Ａと同じであるので、その説明を省略する。

２−３．姿勢情報算出装置の構成
図１０は、姿勢情報算出装置２０Ｂの構成例を示す図である。

姿勢情報算出装置２０Ｂでは、ＡＤＣインターフェース回路（ＡＤＣＩ／Ｆ）２１２は、センサー情報出力装置１０Ｂが時分割に出力する角速度情報（ＧＸ、ＧＹ、ＧＺ）、加速度情報（ＡＸ、ＡＹ、ＡＺ）、温度情報（Ｔ）を、適当なタイミングで順番にサンプリングして取得する処理を行う。

マイコンインターフェース回路（マイコンＩ／Ｆ）２１４は、第１実施形態における姿勢情報算出装置２０Ａと同様に、慣性座標系角速度ベクトル（Ｇ）や慣性座標系加速度ベクトル（Ａ）を生成する処理やその他の種々の処理を行い、さらに、慣性座標系角速度ベクトルＧや慣性座標系加速度ベクトルＡとともに温度情報Ｔもマイクロコンピューター３００に供給する。

マイクロコンピューター３００の構成は姿勢情報算出装置２０Ａと同じであるが、角速度オフセット更新手段２３６と加速度オフセット更新手段２３８の機能が第１実施形態と異なる。

角速度オフセット更新手段２３６は、温度情報（Ｔ）と時刻ｋ−１における修正後のジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚのオフセットに基づいて、時刻ｋにおけるジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚのオフセットを更新する処理を行う。例えば、角速度オフセット更新手段２３６は、時刻ｋ−１における修正後のジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚのオフセットに、あらかじめ定義された、温度情報（Ｔ）とジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚのオフセット補正値の対応関係を参照して得られるオフセット補正値を加算することにより、時刻ｋにおけるジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚのオフセットを更新するようにしてもよい。

加速度オフセット更新手段２３８は、温度情報（Ｔ）と時刻ｋ−１における修正後の加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚのオフセットに基づいて、時刻ｋにおける加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚのオフセットを更新する処理を行う。例えば、加速度オフセット更新手段２３８は、時刻ｋ−１における修正後の加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚのオフセットに、あらかじめ定義された、温度情報（Ｔ）と加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚのオフセット補正値の対応関係を参照して得られるオフセット補正値を加算することにより、時刻ｋにおける加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚのオフセットを更新するようにしてもよい。

姿勢情報算出装置２０Ｂのその他の構成については、姿勢情報算出装置２０Ａと同じであるので、その説明を省略する。

２−４．姿勢情報算出処理の手順
２−４−１．処理手順の概略
第２実施形態における姿勢情報算出装置２０Ｂの全体的な処理手順は、図４（Ａ）及び図４（Ｂ）で説明した処理手順と同様であるので、その説明を省略する。

次に、第２実施形態におけるカルマンフィルターの構成について詳細に説明する。

２−４−２．カルマンフィルター
［使用変数］
第２実施形態におけるカルマンフィルターの更新処理と修正処理では、第１実施形態で説明した変数に加えて以下に示す変数を使用する。

Ｓｅ_ｔ：温度感度（ｍＶ／℃）
Ｖ_ｔ：温度センサーの出力値（ｍＶ）

第１実施形態と同様に、第２実施形態でも、これらの変数のうちカルマンフィルターによる更新・修正の対象となる変数（パラメーター）は、ｂω_ｋ（ｂω＾_ｋ）、ｂα_ｋ（ｂα＾_ｋ）、Ｑｔ_ｋ（Ｑｔ＾_ｋ）、Ｖｅ_ｋ（Ｖｅ＾_ｋ）の４種類である。

［処理手順］
第２実施形態におけるカルマンフィルターを用いた姿勢情報算出処理の手順は、図５で説明した処理手順と同様であるので、その説明を省略する。ただし、第２実施形態では、カルマンフィルターの更新処理（ステップＳ１４０）において、慣性座標系角速度ベクトル（Ｇ）、慣性座標系加速度ベクトル（Ａ）、温度情報（Ｔ）と時刻ｋ−１におけるパラメーター値に基づいて、時刻ｋにおけるパラメーター値を更新する処理が行われる点が第１実施形態と異なる。

［カルマンフィルター更新処理］
第２実施形態では、第１実施形態で説明した４つの更新式（３）、（４）、（５）、（９）のうち、更新式（５）と（９）の形は変わらないが、更新式（３）と（４）の形が異なる。

第２実施形態では、ｂω＾_ｋを求める更新式を次式（２０）のように定義する。

関数ｂωは温度Ｔを角速度センサーオフセット補正用電圧値に変換する関数である。温度Ｔは次式（２１）により求めることができる。

また、第２実施形態では、ｂα＾_ｋを求める更新式を次式（２２）のように定義する。

関数ｂαは温度Ｔを加速度センサーオフセット補正用電圧値に変換する関数である。温度Ｔは前述の式（２１）により求めることができる。

第２実施形態におけるカルマンフィルター更新処理のその他の式は第１実施形態と同じであるので、その説明を省略する。

次に、カルマンフィルターの更新処理の手順の一例について説明する。図１１は、第２実施形態におけるカルマンフィルターの更新処理の手順の一例を示すフローチャート図である。

まず、式（２１）を用いて、温度センサー１１４の出力電圧Ｖ_ｔから温度Ｔを計算する（ステップＳ２４０）。

次に、式（２０）を用いて、ステップＳ２４０で計算した温度値Ｔから角速度センサーオフセット補正用電圧値ｂω（Ｔ）を計算する（ステップＳ２４１）。

次に、式（２２）を用いて、ステップＳ２４０で計算した温度値Ｔから加速度センサーオフセット補正用電圧値ｂα（Ｔ）を計算する（ステップＳ２４２）。

続いて、ステップＳ２４３〜Ｓ２４９の処理を行って更新処理を終了するが、ステップＳ２４３〜Ｓ２４９の処理は、図１１に示したステップＳ１４１〜Ｓ１４７の処理と同じであるので、その説明を省略する。

［カルマンフィルター修正処理］
第２実施形態におけるカルマンフィルター修正処理の内容及び手順は、第１実施形態と同じであるので、その説明を省略する。

２−５．効果
以上に説明した第２実施形態によれば、第１実施形態と同様の効果に加えて、以下の効果を奏する。

すなわち、第２実施形態では、ｂω＾_ｋを求める更新式（２０）とｂα＾_ｋを求める更新式（２１）にともに温度補正の項が追加されているので、修正式（１５）において、ｂω＾_ｋに温度補正項に基づく修正も加えられる。このように、温度によるセンサーオフセットの変動を考慮してセンサーオフセットｂω＾_ｋ、ｂα＾_ｋを更新することでセンサーオフセットによるドリフト量を低減することができるので、カルマンゲインをより小さくすることができる。従って、仮想３次元空間における位置座標と姿勢角を、より迅速にセンサー出力に追従させることができる。

３．適用例
本実施形態の姿勢情報算出システム１Ａ（１Ｂ）は、例えば、ポインティングデバイスとしての３Ｄマウス、バーチャルリアリティー等に使用されるヘッドマウントディスプレイ、頭の姿勢を検出するトラッカー、３Ｄゲームパッド等を使用する種々のシステムに適用することができる。一例として、図１２に、本実施形態の姿勢情報算出システムを適用したゲームシステムを示す。

図１２に示すゲームシステム１０００は、コントローラー５００、ゲーム機本体６００、ディスプレイ装置７００で構成されており、コントローラー５００とゲーム機本体６００、ゲーム機本体６００とディスプレイ装置７００は、それぞれ、通信ケーブル６１０と６２０で接続されており、情報通信が可能になっている。

このゲームシステム１０００では、プレイヤーがコントローラー５００を介して仮想３次元空間に配置された戦闘機オブジェクト（３Ｄモデル）を操作することができ、ゲーム機本体６００のゲーム演算処理により、この戦闘機オブジェクトがディスプレイ装置７００の表示部７１０に戦闘機７２０として２次元表示される。

このゲームシステム１０００では、戦闘機７２０の姿勢がコントローラーの姿勢に連動する。例えば、ゲームコントローラー５００を水平面内で左右方向（図のＡ方向）に移動させると、それに応じて戦闘機７２０が左右方向（図のａ方向）に移動する。また、ゲームコントローラー５００を水平面内で前後方向（図のＢ方向）に移動させると、それに応じて戦闘機７２０が上下方向（図のｂ方向）に移動する。さらに、ゲームコントローラー５００を水平面内で回転させると（図のＣ方向の回転）、それに応じて戦闘機７２０が回転する（図のｃ方向の回転）。

この操作を可能とするため、コントローラー５００には、その内部に、検出軸が互いに直交するように配置された３つの角速度センサーと、検出軸が互いに直交するように配置された３つの加速度センサーが設けられており、これらのセンサーの出力値はゲーム機本体６００に送信され、仮想３次元空間における戦闘機オブジェクトの姿勢角と位置座標の算出処理が行われる。

コントローラー５００とディスプレイ装置７００をそれぞれ本実施形態のセンサー情報出力装置１０Ａ（１０Ｂ）と姿勢情報算出装置２０Ａ（２０Ｂ）として構成することにより、このゲームシステム１０００を実現することができる。こうすることにより、コントローラー５００の姿勢角と移動速度を、それぞれ戦闘機オブジェクトの姿勢角と位置座標に対応させることができ、コントローラ５００の移動を停止する毎にセンサーオフセットによるドリフトが補正され、戦闘機７２０を表示部７１０の中心（原点）に引き戻すことができる。また、コントローラー５００の移動速度を戦闘機オブジェクトの位置座標と対応させることでカルマンゲインを比較的小さくすることができるので、戦闘機オブジェクトの姿勢をユーザーの操作にリアルタイムに追従させることも容易である。

なお、コントローラー５００には操作レバー５１０と５２０が設けられており、プレイヤーは操作レバー５１０、５２０を操作することで、戦闘機オブジェクトが備えるミサイルなどの種々の装備を扱うことができる。すなわち、このゲームシステム１０００によれば、コントローラー５００の姿勢を変えることでそれに応じて戦闘機オブジェクトの姿勢を変えながら、操作レバー５１０、５２０の操作により、この姿勢操作と同時に戦闘機オブジェクトの装備を扱うことができるので、プレイヤーの操作性やゲームの趣向性を向上させることができる。

なお、本発明は本実施形態に限定されず、本発明の要旨の範囲内で種々の変形実施が可能である。

例えば、センサー情報出力装置１０Ａに含まれる３つのジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚは、互いに直交する３軸（Ｘ軸、Ｙ軸、Ｚ軸）方向の角速度を検出するものとして説明したが、本発明の範囲はこれに限られず、ジャイロセンサー１１０Ｘ、１１０Ｙ、１１０Ｚが互いに平行でない任意の３軸方向の角速度を検出する場合も本発明の範囲に含まれる。この場合、姿勢情報算出部２２０は、アラインメント補正により、互いに平行でない任意の３軸における角速度ベクトルを直交座標系における角速度ベクトルに変換した後、本実施形態において説明した処理を行うようにすればよい。

同様に、センサー情報出力装置１０Ａに含まれる３つの加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚは、互いに直交する３軸（Ｘ軸、Ｙ軸、Ｚ軸）方向の加速度を検出するものとして説明したが、本発明の範囲はこれに限られず、加速度センサー１１２Ｘ、１１２Ｙ、１１２Ｚが互いに平行でない任意の３軸方向の加速度を検出する場合も本発明の範囲に含まれる。この場合、姿勢情報算出部２２０は、アラインメント補正により、互いに平行でない任意の３軸における加速度ベクトルを直交座標系における加速度ベクトルに変換した後、本実施形態において説明した処理を行うようにすればよい。

本発明は、実施の形態で説明した構成と実質的に同一の構成（例えば、機能、方法及び結果が同一の構成、あるいは目的及び効果が同一の構成）を含む。また、本発明は、実施の形態で説明した構成の本質的でない部分を置き換えた構成を含む。また、本発明は、実施の形態で説明した構成と同一の作用効果を奏する構成又は同一の目的を達成することができる構成を含む。また、本発明は、実施の形態で説明した構成に公知技術を付加した構成を含む。

１Ａ，１Ｂ 姿勢情報算出システム、１０Ａ，１０Ｂ センサー情報出力装置、２０Ａ，２０Ｂ 姿勢情報算出装置、１００，１０２ センサーモジュール、１１０Ｘ，１１０Ｙ，１１０Ｚ ジャイロセンサー、１１２Ｘ，１１２Ｙ，１１２Ｚ 加速度センサー、１１４ 温度センサー、１２０Ｘ，１２０Ｙ，１２０Ｚ，１２２Ｘ，１２２Ｙ，１２２Ｚ，１２４ 増幅器（ＡＭＰ）、１３０Ｘ，１３０Ｙ，１３０Ｚ，１３２Ｘ，１３２Ｙ，１３２Ｚ，１３４ ローパスフィルター（ＬＰＦ）、１４０ Ａ／Ｄ変換器（ＡＤＣ）、２１０ センサー情報取得部、２１２ ＡＤＣインターフェース回路（ＡＤＣＩ／Ｆ）、２１４ マイコンインターフェース回路（マイコンＩ／Ｆ）、２２０ 姿勢情報算出部、２３０ 姿勢情報更新部、２３２ 速度ベクトル更新手段、２３４ 姿勢角ベクトル更新手段、２３６ 角速度オフセット更新手段、２３８ 加速度オフセット更新手段、２４０ 姿勢情報修正部、２４２ 速度ベクトル修正手段、２４４ 姿勢角ベクトル修正手段、２４６ 角速度オフセット修正手段、２４８ 加速度オフセット修正手段、２５０ 姿勢情報生成部、２５２ 位置座標生成手段、２５４ 姿勢角生成手段、３００ マイクロコンピューター、３１０ 処理部、３２０ 記憶部、３３０ 通信部、４００ 情報記憶媒体、５００ コントローラー、５１０，５２０ 操作レバー、６００ ゲーム機本体、６１０，６２０ 通信ケーブル、７００ ディスプレイ装置、７１０ 表示部、７２０ 戦闘機、１０００ ゲームシステム

Claims (11)

1. ３つの角速度センサーがそれぞれ検出する互いに平行でない３軸回りの角速度情報と、３つの加速度センサーがそれぞれ検出する互いに平行でない３軸方向の加速度情報と、に基づいて仮想３次元空間における姿勢角と位置座標を算出する姿勢情報算出装置であって、
   前記３軸回りの角速度情報と前記３軸方向の加速度情報を取得するセンサー情報取得部と、
   前記センサー情報取得部が取得した前記３軸回りの角速度情報と前記３軸方向の加速度情報に基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を算出する姿勢情報算出部と、を含み、
   前記姿勢情報算出部は、
   前記３軸方向の加速度情報から得られる慣性座標系加速度ベクトルに基づいて固定座標系速度ベクトルを算出し、当該固定座標系速度ベクトルに対応づけて前記仮想３次元空間における前記位置座標を算出する、姿勢情報算出装置。
2. 請求項１において、
   前記姿勢情報算出部は、
   前記３軸回りの角速度情報と３軸方向の加速度情報に基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を特定するために必要な所与のパラメーターを更新する姿勢情報更新部と、
   前記姿勢情報更新部が更新した後の前記パラメーターを、所与の拘束条件に従い修正する姿勢情報修正部と、
   前記姿勢情報修正部が修正した後の前記パラメーターに基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を算出する姿勢情報生成部と、を含み、
   前記姿勢情報更新部は、
   前記慣性座標系加速度ベクトルと時刻ｋ−１における修正後の前記固定座標系速度ベクトルに基づいて、時刻ｋにおける前記固定座標系速度ベクトルを更新する速度ベクトル更新手段を含み、
   前記姿勢情報修正部は、
   前記固定座標系速度ベクトルのノルムを０とする拘束条件に従い、前記速度ベクトル更新手段が更新した時刻ｋにおける前記固定座標系速度ベクトルを修正する速度ベクトル修正手段を含み、
   前記姿勢情報生成部は、
   前記速度ベクトル修正手段が修正した時刻ｋにおける前記固定座標系速度ベクトルに対応づけて前記仮想３次元空間における前記位置座標を生成する位置座標生成手段を含む、姿勢情報算出装置。
3. 請求項２において、
   前記速度ベクトル更新手段は、
   前記慣性座標系加速度ベクトルを、時刻ｋ−１における前記固定座標系姿勢角ベクトルに基づいて回転させて前記固定座標系加速度ベクトルに変換し、当該固定座標系加速度ベクトルから固定座標系重力加速度ベクトルを減算し、減算結果を所定の単位時間で１階積分して単位時間固定座標系速度ベクトルを算出し、時刻ｋ−１における修正後の前記固定座標系速度ベクトルに前記単位時間固定座標系速度ベクトルを加算することにより時刻ｋにおける前記固定座標系速度ベクトルを更新する、姿勢情報算出装置。
4. 請求項２又は３において、
   前記位置座標生成手段は、
   時刻ｋにおける修正後の前記固定座標系速度ベクトルを、時刻ｋにおける前記位置座標とする、姿勢情報算出装置。
5. 請求項２乃至４のいずれかにおいて、
   前記姿勢情報算出部は、
   前記３軸回りの角速度情報から得られる慣性座標系角速度ベクトルに基づいて固定座標系姿勢角ベクトルを算出し、当該固定座標系姿勢角ベクトルに基づいて前記仮想３次元空間における前記姿勢角を算出し、
   前記姿勢情報更新部は、
   前記慣性座標系角速度ベクトルと時刻ｋ−１における修正後の前記固定座標系姿勢角ベクトルに基づいて、時刻ｋにおける前記固定座標系姿勢角ベクトルを更新する姿勢角ベクトル更新手段を含み、
   前記姿勢情報修正部は、
   前記慣性座標系加速度ベクトルに基づく拘束条件に従い、前記姿勢角ベクトル更新手段が更新した時刻ｋにおける前記固定座標系姿勢角ベクトルを修正する姿勢角ベクトル修正手段を含み、
   前記姿勢情報生成部は、
   修正後の前記固定座標系姿勢角ベクトルに基づいて、前記仮想３次元空間における前記姿勢角を生成する姿勢角生成手段を含む、姿勢情報算出装置。
6. 請求項５において、
   前記姿勢角ベクトル更新手段は、
   前記慣性座標系角速度ベクトルを所定の単位時間で１階積分して単位時間回転角クオータ二オンを算出し、時刻ｋ−１における修正後の前記固定座標系姿勢角ベクトルに前記単位時間回転角クオータ二オンを作用させることにより時刻ｋにおける前記固定座標系姿勢角ベクトルを更新する、姿勢情報算出装置。
7. 請求項５又は６において、
   前記姿勢角生成手段は、
   時刻ｋにおける修正後の前記固定座標系姿勢角ベクトルを、時刻ｋにおける前記姿勢角とする、姿勢情報算出装置。
8. 請求項２乃至７のいずれかにおいて、
   前記センサー情報取得部は、
   温度センサーが検出する温度情報を受け取り、
   前記姿勢情報更新部は、
   前記温度情報に対応づけられた角速度センサーのオフセットと時刻ｋ−１における修正後の前記３つの角速度センサーのオフセットに基づいて、時刻ｋにおける前記３つの角速度センサーのオフセットを更新する角速度オフセット更新手段と、
   前記温度情報に対応づけられた加速度センサーのオフセットと時刻ｋ−１における修正後の前記３つの加速度センサーのオフセットに基づいて、時刻ｋにおける前記３つの加速度センサーのオフセットを更新する加速度オフセット更新手段と、を含み、
   前記姿勢情報修正部は、
   所与の拘束条件に従い、前記角速度オフセット更新手段が更新した時刻ｋにおける前記３つの角速度センサーのオフセットを修正する角速度オフセット修正手段と、
   所与の拘束条件に従い、前記加速度オフセット更新手段が更新した時刻ｋにおける前記３つの加速度センサーのオフセットを修正する加速度オフセット修正手段と、を含む、姿勢情報算出装置。
9. 互いに平行でない３軸回りの角速度情報をそれぞれ検出する３つの角速度センサーと、
   互いに平行でない３軸方向の加速度情報をそれぞれ検出する３つの加速度センサーと、
   請求項１乃至８のいずれかに記載の姿勢情報算出装置と、を含む、姿勢情報算出システム。
10. ３つの角速度センサーがそれぞれ検出する互いに平行でない３軸回りの角速度情報と、３つの加速度センサーがそれぞれ検出する互いに平行でない３軸方向の加速度情報と、に基づいて仮想３次元空間における姿勢角と位置座標を算出する姿勢情報算出方法であって、
    前記３軸回りの角速度情報と前記３軸方向の加速度情報を取得するセンサー情報取得ステップと、
    前記センサー情報取得ステップにおいて取得した前記３軸回りの角速度情報と前記３軸方向の加速度情報に基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を算出する姿勢情報算出ステップと、を含み、
    前記姿勢情報算出ステップにおいて、
    前記３軸方向の加速度情報から得られる慣性座標系加速度ベクトルに基づいて固定座標系速度ベクトルを算出し、当該固定座標系速度ベクトルに対応づけて前記仮想３次元空間における前記位置座標を算出する、姿勢情報算出方法。
11. ３つの角速度センサーがそれぞれ検出する互いに平行でない３軸回りの角速度情報と、３つの加速度センサーがそれぞれ検出する互いに平行でない３軸方向の加速度情報と、に基づいて仮想３次元空間における姿勢角と位置座標を算出する姿勢情報算出プログラムであって、
    前記３軸回りの角速度情報と前記３軸方向の加速度情報に基づいて、前記仮想３次元空間における前記姿勢角と前記位置座標を算出する姿勢情報算出部としてコンピューターを機能させ、
    前記姿勢情報算出部は、
    前記３軸方向の加速度情報から得られる慣性座標系加速度ベクトルに基づいて固定座標系速度ベクトルを算出し、当該固定座標系速度ベクトルに対応づけて前記仮想３次元空間における前記位置座標を算出する、姿勢情報算出プログラム。

Images