(57)【要約】      （修正有）【課題】不適切な平滑化結果が導出される特異区間がなく、また、バッファの段数が少なくても平滑化強度を高めることのできる姿勢データフィルタ装置を提供する。【解決手段】時刻ｉＴ（Ｔは定数、ｉ＝０，１，２・・・）における第一の姿勢を示す入力データ：Ａ'ｉを順次取得する入力手段と、時刻ｉＴにおける第二の姿勢を示す出力データ：Ａｉを順次出力する出力手段と、時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の角度をθｉ、前記出力データの軌跡が示す時刻ｉＴにおける角速度をωｉ、時刻（ｉ−１）Ｔから時刻ｉＴまでの第二の姿勢の回転の角加速度を、Ｎｉ＝Ｎ１ｉ−Ｎ２ｉとするとき、平滑化機能を実現するために定めた条件に基づいて当該角加速度を算出し、その後、Ａｉを、前記入力データと初期値Ａ０と初期値ω０とに基づいて導出する演算手段と、を備える。【選択図】図３
2008298700.tif 000002
【特許請求の範囲】
【請求項１】
時刻ｉＴ（Ｔは定数、ｉ＝０，１，２・・・）における第一の姿勢を示す入力データ：Ａ'ｉを順次取得する入力手段と、
時刻ｉＴにおける第二の姿勢を示す出力データ：Ａｉを順次出力する出力手段と、
時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の角度をθｉ、前記出力データの軌跡が示す時刻ｉＴにおける角速度をωｉ、時刻（ｉ−１）Ｔから時刻ｉＴまでの第二の姿勢の回転の角加速度を、
Ｎｉ＝Ｎ１ｉ−Ｎ２ｉ
とするとき、
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）−π／２≦θｉ≦π／２であるとき、Ｎ１ｉが示す角加速度の大きさはθｉの絶対値に対して単調増加し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに対応し、
（４）Ｎ２ｉが示す角加速度の大きさはωｉが示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉが示す角速度の向きに対応する、
Ａｉを、前記入力データと初期値Ａ０と初期値ω０とに基づいて導出する演算手段と、
を備える姿勢データフィルタ装置。
【請求項２】
前記演算手段は、
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）Ｎ１ｉが示す角加速度の大きさはｓｉｎθｉの絶対値に比例し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに一致し、
（４）Ｎ２ｉが示す角加速度の大きさはωｉ−１が示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉ−１が示す角速度の向きに一致する、
Ａｉを、前記入力データと初期値Ａ０と初期値ω０とに基づいて導出する、
請求項１に記載の姿勢データフィルタ装置。
【請求項３】
前記入力データおよび前記出力データは正格直交行列の成分であり、
前記演算手段は、次式（１）、（２）、（３）の漸化式を用いてＡｉを導出する、
請求項２に記載の姿勢データフィルタ装置。
【数１】
2008298700.tif 000027
ただし、前記出力データが示す姿勢の回転は角速度ベクトルωｉを回転軸とする右ねじ方向への回転とし、||ωｉ||は角速度ωｉの大きさに比例し、
ｅ：ネイピア数
Γ、Ｋ：正の定数
Ｒｍのｉ行ｊ列成分をｒｍ，ｉｊとするとき、
【数２】
2008298700.tif 000028
である。
【請求項４】
時刻ｉＴ（Ｔは定数、ｉ＝０，１，２・・・）における第一の姿勢を示す入力データ：Ａ'ｉを順次取得し、
時刻ｉＴにおける第二の姿勢を示す出力データ：Ａｉを順次出力し、
時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の角度をθｉ、前記出力データの軌跡が示す時刻ｉＴにおける角速度をωｉ、時刻（ｉ−１）Ｔから時刻ｉＴまでの第二の姿勢の回転の角加速度を、
Ｎｉ＝Ｎ１ｉ−Ｎ２ｉ
とするとき、
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）−π／２≦θｉ≦π／２であるとき、Ｎ１ｉが示す角加速度の大きさはθｉの絶対値に対して単調増加し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに対応し、
（４）Ｎ２ｉが示す角加速度の大きさはωｉが示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉが示す角速度の向きに対応する、
Ａｉを、前記入力データと初期値Ａ０と初期値ω０とに基づいて導出する、
ことを含む姿勢データフィルタリング方法。
【請求項５】
時刻ｉＴ（Ｔは定数、ｉ＝０，１，２・・・）における第一の姿勢を示す入力データ：Ａ'ｉを順次取得する入力手段と、
時刻ｉＴにおける第二の姿勢を示す出力データ：Ａｉを順次出力する出力手段と、
時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の角度をθｉ、前記出力データの軌跡が示す時刻ｉＴにおける角速度をωｉ、時刻（ｉ−１）Ｔから時刻ｉＴまでの第二の姿勢の回転の角加速度を、
Ｎｉ＝Ｎ１ｉ−Ｎ２ｉ
とするとき、
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）−π／２≦θｉ≦π／２であるとき、Ｎ１ｉが示す角加速度の大きさはθｉの絶対値に対して単調増加し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに対応し、
（４）Ｎ２ｉが示す角加速度の大きさはωｉが示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉが示す角速度の向きに対応する、
Ａｉを、前記入力データと初期値Ａ０と初期値ω０とに基づいて導出する導出手段と、
してコンピュータを機能させる姿勢データフィルタプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は姿勢データフィルタ装置、姿勢データフィルタリング方法および姿勢データフィルタプログラムに関し、特に時系列の姿勢データを平滑化する技術に関する。
【背景技術】
【０００２】
従来、例えば３次元地磁気センサと３次元加速度センサとを用い、これらが搭載された物体の姿勢を示す姿勢データを導出する方法が知られている（例えば特許文献１参照）。この方法によると、姿勢データにはノイズが混入しており、また、物体に衝撃が加わると、加速度センサを用いて姿勢を導出することに起因して実際の姿勢とは著しく異なる姿勢を示す姿勢データが導出されることになる。このようにノイズが混入した姿勢データや誤った姿勢データをそのまま用いて装置の姿勢を表示することは好ましくない。また姿勢データに基づいて何らかの処理（例えば歩数計算処理）を行う場合にも、ノイズが混入した姿勢データや誤った姿勢データをそのまま用いることは好ましくない。このため、従来は、音響データや画像データなどに適用されるフィルタ処理によって姿勢データを平滑化する処理が行われている。
【特許文献１】国際公開第ＷＯ／２００６−００９２４７号パンフレット
【発明の開示】
【発明が解決しようとする課題】
【０００３】
ところで、物体の姿勢は連続的かつ循環的に変化する状態である。また、従来のフィルタ処理で操作されるオイラー角などの物理量は線形空間において定義される有限個数のデータ集合として表される。仮にオイラー角のように、連続する２つの姿勢が不連続なデータに対応する有限個数の集合、すなわち微小な差しかない２つの姿勢を線形空間において差の大きなデータで示す特異区間のある集合として、回転角度に制限のない物体（例えば携帯型情報機器）の姿勢を示す姿勢データが定義されるとする。このような姿勢データを連続的かつ非循環的な物理量を示すデータとみなし、すなわち実際の物理現象との対応関係を無視し得るデータとみなした上で、移動平均等の数学的操作によって平滑化する従来のフィルタ処理を姿勢データに適用した場合、特異区間において適正な処理結果を得られないという問題がある。
また、移動平均を求めることによって平滑化するフィルタ装置をＦＩＲ（Finite Impulse Response）フィルタ装置として実現する場合、平滑化強度を高めるためにはバッファの段数を多く設計しなければならないという問題がある。
【０００４】
本発明は、不適切な平滑化結果が導出される特異区間がなく、また、バッファの段数が少なくても平滑化強度を高めることのできる姿勢データフィルタ装置、姿勢データフィルタリング方法および姿勢データフィルタプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００５】
本願発明者は、姿勢データを単に数学的なデータとして操作するのではなく、姿勢データが示す物理現象に着目し、時間軸方向に離散的な姿勢データ群に内在する角速度および角加速度を表出させるとともに、出力データが示す第二の姿勢の仮想的な物体の回転を入力データが示す第一の姿勢に向かって加速させる作用としてフィルタ処理をモデル化することによって、不適切な平滑化結果が導出される特異区間のないフィルタ処理を実現することに成功した。
【０００６】
〔１〕すなわち、上記目的を達成するための姿勢データフィルタ装置は、時刻ｉＴ（Ｔは定数、ｉ＝０，１，２・・・）における第一の姿勢を示す入力データ：Ａ'ｉを順次取得する入力手段と、
時刻ｉＴにおける第二の姿勢を示す出力データ：Ａｉを順次出力する出力手段と、
時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の角度をθｉ、前記出力データの軌跡が示す時刻ｉＴにおける角速度をωｉ、時刻（ｉ−１）Ｔから時刻ｉＴまでの第二の姿勢の回転の角加速度を、
Ｎｉ＝Ｎ１ｉ−Ｎ２ｉ
とするとき、
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）−π／２≦θｉ≦π／２であるとき、Ｎ１ｉが示す角加速度の大きさはθｉの絶対値に対して単調増加し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに対応し、
（４）Ｎ２ｉが示す角加速度の大きさはωｉが示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉが示す角速度の向きに対応する、
Ａｉを、前記入力データと初期値Ａ０と初期値ω０とに基づいて導出する演算手段と、
を備える。
【０００７】
図１、図２、図３は、本発明において用いるモデルを示す説明図である。入力データＡ'ｉが第一の姿勢を示す物体を対象物体といい、出力データＡｉが第二の姿勢を示す仮想的な物体を仮想物体というものとする。今、時刻（ｍ−１）Ｔ、ｍＴ、（ｍ＋１）Ｔにおいて入力データＡ'ｉが図１、図２、図３に示す順に変化すると仮定する。仮想物体の姿勢が対象物体の姿勢に追従するようにフィルタ処理を行うため、各時刻において、対象物体および仮想物体はそれぞれに固有の角速度で回転していると考えることとする。仮想物体が時刻（ｍ−２）Ｔから時刻（ｍ−１）Ｔまでの期間に一定の角速度ωｍ−１で回転すると仮定すると、仮想物体の角速度ωｍ−１は図２に示すように仮想物体の姿勢データＡｍ−２、Ａｍ−１の軌跡から導出できる。
【０００８】
平滑化機能を実現するためには、仮想物体の角加速度Ｎを次の条件を満たすように定めればよい。
・出力データＡが示す姿勢から入力データＡ'が示す姿勢に近づくように仮想物体の回転が加速する。
・仮想物体は対象物体に比べて緩やかに姿勢変化する。
・出力データＡが示す姿勢と入力データＡ'が示す姿勢の差が小さくなると仮想物体の角速度ωは入力データＡ'の軌跡が示す角速度に近くなる。
【０００９】
これらの条件を満たすために、仮想物体の角加速度ＮｉをＮ１ｉとＮ２ｉの差として表し、図３に示すＮ１ｉ、Ｎ２ｉをそれぞれ次のように定める。
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）−π／２≦θｉ≦π／２であるとき、Ｎ１ｉが示す角加速度の大きさはθｉの絶対値に対して単調増加し、
（３）Ｎ１ｉが示す角加速度の向きは前記第二の姿勢から前記第一の姿勢への回転の向きに対応する。
【００１０】
Ｎ１は、−π／２≦θｉ≦π／２を満たす任意の時刻において、出力データＡが示す姿勢から入力データＡ'が示す姿勢に近づくように仮想物体の回転を加速させる成分であるとともに、出力データＡが示す姿勢と入力データＡ'が示す姿勢の差を示すθの絶対値が大きくなるほど大きくなる成分である。
【００１１】
ここで仮に対象物体と仮想物体の姿勢が一致した状態でいずれも静止している時刻から対象物体が回転した後に静止したとする。しかし、角加速度成分Ｎ１ｉで仮想物体を加速させるとすれば、対象物体が再び静止しても、加速度成分Ｎ１ｉの積分値に相当する成分が仮想物体の角速度からなくなることはないため、仮想物体の姿勢はいつまでたっても対象物体の姿勢に収束しない。
【００１２】
そこで、仮想物体の回転の抵抗になる加速度成分Ｎ２ｉを導入する。−Ｎ２ｉは仮想物体の回転が速いほど大きくなり、仮想物体の回転の向きと反対の向きの成分を持つ角加速度を示す。すなわち、
（４）Ｎ２ｉが示す角加速度の大きさはωｉ−１が示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉ−１が示す角速度の向きに対応する。
このような角加速度成分Ｎ２ｉの導入によって、仮想物体は対象物体に比べて緩やかに姿勢変化するようになるだけでなく、出力データＡが示す姿勢と入力データＡ'が示す姿勢の差が小さくなると、仮想物体の角速度ωは、出力データＡの軌跡が示す角速度に近くなる。
【００１３】
そして、仮想物体の姿勢の初期値Ａ０と対象物体の角速度の初期値ω０を設定し、Ｎ１ｉとＮ２ｉで表される仮想物体の角加速度Ｎｉ＝Ｎ１ｉ−Ｎ２ｉで仮想物体が加速するように、仮想物体の姿勢を示すＡｉを順次操作すると、姿勢データの平滑化が実現される。
これらの操作は、隣接二項間漸化式の演算によって可能であるため、バッファの段数が少なくても、係数の設定によって平滑化強度を高めることができる。尚、θは、仮想物体の姿勢から対象物体の姿勢まで最短軌跡で回転する回転の角度である。
【００１４】
〔２〕さらに、上記目的を達成するための姿勢データフィルタ装置において、前記演算手段は、
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）Ｎ１ｉが示す角加速度の大きさはｓｉｎθｉの絶対値に比例し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに一致し、
（４）Ｎ２ｉが示す角加速度の大きさはωｉが示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉが示す角速度の向きに一致する、
Ａｉを、前記入力データと初期値Ａ０と初期値ω０とに基づいて導出することが好ましい。
【００１５】
仮想物体の姿勢から対象物体の姿勢まで最短軌跡で回転する回転の角度θｉがπであるとき、仮想物体の姿勢から対象物体の姿勢に最短軌跡で回転させる操作は２通りあり、いずれの操作も最も回転軌跡が長くなる操作である。したがって、仮想物体と対象物体の両方が静止または等角速度運動している状態を除けば、仮想物体の回転を加速しなくても、仮想物体の姿勢は対象物体の姿勢に近づく。ノイズ成分を考慮すれば対象物体が静止または等角速度運動する状態は通常ありえない。そこで、仮想物体の姿勢から対象物体の姿勢までの回転の角度θｉがπであるときにＮ１ｉの成分が０になるモデルを導入する。すなわち、Ｎ１ｉが示す角加速度の大きさをｓｉｎθｉの絶対値に比例させる。
【００１６】
また、仮想物体の姿勢から対象物体の姿勢までの回転を示す行列は次式（７）で示すＲと表すことができる。このとき、仮想物体の姿勢から対象物体の姿勢までの回転の軸の方向を持つベクトルω'は次式（８）と表すことができ、その回転の角度θは、ω'を軸として右ねじ方向を正とすると次式（９）、（１０）と表すことができる。ただし、ベクトルは全て列ベクトルとし、行ベクトルは列ベクトルの転置行列として表現している。
【数３】
2008298700.tif 000003
【００１７】
ここで、ω'の大きさを１で正規化したω"を次式（１１）によって導入する。
【数４】
2008298700.tif 000004
前述のように、
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）Ｎ１ｉが示す角加速度の大きさはｓｉｎθｉの絶対値に比例し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに一致するとき、
Ｎ１ｉ＝Ｋω"ｓｉｎθｉ・・・（１２）
とかける。式（１２）は式（１１）の定義から、
Ｎ１ｉ＝Ｋω'ｉ・・・（１３）
となる。したがって、（２）Ｎ１ｉが示す角加速度の大きさがｓｉｎθｉの絶対値に比例するモデルは、計算量の低減の観点からも好ましい。
【００１８】
また、Ｎ１ｉが示す角加速度の向きを時刻（ｉ−１）Ｔにおける第二の姿勢から時刻ｉＴにおける第一の姿勢への回転の向きに一致させ、Ｎ２ｉが示す角加速度の向きをωｉ−１が示す角速度の向きに一致させることにより、仮想物体の姿勢が対象物体の姿勢に近づける加速操作の効率を最大化することができる。
【００１９】
〔３〕さらに、上記目的を達成するための姿勢データフィルタ装置において、前記入力データおよび前記出力データは正格直交行列の成分であり、前記演算手段は、次式（１）、（２）の漸化式を用いてＡｉを導出する、ことが好ましい。
【数１】
2008298700.tif 000005
ただし、前記出力データが示す姿勢の回転は角速度ベクトルωｉを回転軸とする右ねじ方向への回転とし、||ωｉ||は角速度ωｉの大きさに比例し、
ｅ：ネイピア数
Γ、Ｋ：正の定数
Ｒｍのｉ行ｊ列成分をｒｍ，ｉｊとするとき、
【数２】
2008298700.tif 000006
である。
【００２０】
まず連続時間において上記〔２〕のモデルを数式化することを考える。時刻ｔにおける物体の姿勢を表す正格直交行列をＡ（ｔ）とし、その時刻において物体は次式（１４）で示す角速度ω（ｔ）を回転軸として右ねじ方向に回転しているものとする。ただし、||ω（ｔ）||は角速度ω（ｔ）の大きさに比例するものとする。
【数５】
2008298700.tif 000007
【００２１】
このとき、
【数６】
2008298700.tif 000008
とすると、Ａ（ｔ）は次式（１６）の微分方程式を満たす。
【数７】
2008298700.tif 000009
【００２２】
式（１６）においてΩ（ｔ）＝Ω０で一定とすると、時刻ｔから時間がＴ経過したときの姿勢行列Ａ（ｔ＋Ｔ）（姿勢を示す正格直交行列を姿勢行列というものとする。）は、次式（１７）のように書くことができる。
【数８】
2008298700.tif 000010
【００２３】
したがって、時間Ｔが十分微小であると考えると、式（１６）の微分方程式は次式（１８）よって近似することができる。
【数９】
2008298700.tif 000011
【００２４】
次に連続時間において角速度と角加速度の関係を数式化することを考える。ω（ｔ）の角加速度をＮ（ｔ）とすると、次式（１９）が成立する。
【数１０】
2008298700.tif 000012
【００２５】
連続時間において上記〔２〕のモデルを微分方程式として示したものが次式（２０）である。
【数１１】
2008298700.tif 000013
ここで、
【数１２】
2008298700.tif 000014
である。またＲｍは時刻（ｉ−１）Ｔにおける仮想物体の姿勢（第二の姿勢）から時刻ｉＴにおける対象物体の姿勢（第一の姿勢）への回転を示す正格直交行列である。
【００２６】
時間Ｔが十分微小であると考えると、式（２０）の微分方程式は次式（２３）によって近似することができる。
【数１３】
2008298700.tif 000015
【００２７】
連続時間における微分方程式（１６）、（２０）を式（１８）、（２３）によって近似することによって、入力データＡ'ｉから出力データＡｉを導出するための漸化式は、式（１）、（２）、（３）で表されるように計算量の比較的少ないものになる。尚、式（２）において、
【数１４】
2008298700.tif 000016
である。
また、フィルタ処理の対象となる入力データ及び出力データを正格直交行列の成分とすることにより、特異区間におけるアルゴリズムを変更するための分岐処理などで処理が複雑化することがない。正格直交行列である姿勢行列によって姿勢を表現すると、微小な姿勢変化は、常に、行列成分の微小な変化として表れるからである。すなわち、姿勢行列の成分は、特異区間のないパラメータである。
【００２８】
さて、ここまで説明した発明は、姿勢データフィルタ装置のみならず、姿勢データフィルタリング方法や姿勢データフィルタプログラムにも適用可能である。
〔４〕すなわち、上記目的を達成するための姿勢データフィルタリング方法は、時刻ｉＴ（Ｔは定数、ｉ＝０，１，２・・・）における第一の姿勢を示す入力データ：Ａ'ｉを順次取得し、
時刻ｉＴにおける第二の姿勢を示す出力データ：Ａｉを順次出力し、
時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の角度をθｉ、前記出力データの軌跡が示す時刻ｉＴにおける角速度をωｉ、時刻（ｉ−１）Ｔから時刻ｉＴまでの第二の姿勢の回転の角加速度を、
Ｎｉ＝Ｎ１ｉ−Ｎ２ｉ
とするとき、
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）−π／２≦θｉ≦π／２であるとき、Ｎ１ｉが示す角加速度の大きさはθｉの絶対値に対して単調増加し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに対応し、
（４）Ｎ２ｉが示す角加速度の大きさはωｉが示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉが示す角速度の向きに対応する、
Ａｉを、前記入力データと初期値Ａ０と初期値ω０とに基づいて導出する、
ことを含む。
【００２９】
〔５〕さらに、上記目的を達成するための姿勢データフィルタプログラムは、時刻ｉＴ（Ｔは定数、ｉ＝０，１，２・・・）における第一の姿勢を示す入力データ：Ａ'ｉを順次取得する入力手段と、
時刻ｉＴにおける第二の姿勢を示す出力データ：Ａｉを順次出力する出力手段と、
時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の角度をθｉ、前記出力データの軌跡が示す時刻ｉＴにおける角速度をωｉ、時刻（ｉ−１）Ｔから時刻ｉＴまでの第二の姿勢の回転の角加速度を、
Ｎｉ＝Ｎ１ｉ−Ｎ２ｉ
とするとき、
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）−π／２≦θｉ≦π／２であるとき、Ｎ１ｉが示す角加速度の大きさはθｉの絶対値に対して単調増加し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに対応し、
（４）Ｎ２ｉが示す角加速度の大きさはωｉが示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉが示す角速度の向きに対応する、
Ａｉを、前記入力データと初期値Ａ０と初期値ω０とに基づいて導出する導出手段と、
してコンピュータを機能させる。
【００３０】
尚、請求項に記載された動作の順序は、技術的な阻害要因がない限りにおいて記載順に限定されず、同時に実行されても良いし、記載順の逆順に実行されても良いし、連続した順序で実行されなくても良い。また請求項に記載された各手段の機能は、構成自体で機能が特定されるハードウェア資源、プログラムにより機能が特定されるハードウェア資源、又はそれらの組み合わせにより実現される。また、これら各手段の機能は、各々が物理的に互いに独立したハードウェア資源で実現されるものに限定されない。さらに、本発明は姿勢データフィルタプログラムの記録媒体としても成立する。むろん、そのコンピュータプログラムの記録媒体は、磁気記録媒体であってもよいし光磁気記録媒体であってもよいし、今後開発されるいかなる記録媒体であってもよい。
【発明を実施するための最良の形態】
【００３１】
以下、本発明の実施の形態を添付図面を参照しながら以下の順に説明する。尚、各図において対応する構成要素には同一の符号が付され、重複する説明は省略される。
＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊
１．姿勢データフィルタ装置のハードウェア構成
２．姿勢行列
３．姿勢データフィルタ装置のソフトウェア構成
４．姿勢データフィルタリング処理
５．他の実施形態
＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊
【００３２】
〔１．姿勢データフィルタ装置のハードウェア構成〕
図４は、本発明による姿勢データフィルタ装置の一実施形態を示すブロック図である。図４において姿勢データフィルタ装置は、携帯型情報機器１０の制御部１３として表されている。携帯型情報機器１０は、加速度センサ１１と地磁気センサ１２と制御部１３とディスプレイ１４とを備えている。携帯型情報機器１０は、ＰＤＡ（Personal Digital Assistance）、携帯型電話、携帯型ゲーム機、歩数計、電子コンパスなど、姿勢データを処理する携帯型の情報処理装置である。
【００３３】
加速度センサ１１は、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよく、互いに直交するｘ，ｙ，ｚの３軸の成分によって重力加速度の反対向きの加速度と加速度センサの運動に固有の加速度を合成した加速度を示すベクトルデータである加速度データを出力する。静止した状態において加速度センサ１１から出力される加速度データの方向は重力方向を示しているため、加速度データは携帯型情報機器１０の傾きを示すデータとして利用することが出来る。
【００３４】
地磁気センサ１２は、ＭＩ素子、ＭＲ素子などからなる３つの磁気センサユニットを備え、互いに直交するｘ，ｙ，ｚの３軸の成分によって地磁気の方向を示すベクトルデータである地磁気データを出力する。加速度データと地磁気データとを組み合わせることにより、携帯型情報機器１０からみた方位を正確に表示することが可能になる。
制御部１３は、図示しないプロセッサと記憶媒体（例えばＲＡＭとＲＯＭ）とインタフェースとを備えるコンピュータであって、後述する姿勢データフィルタプログラムを実行することにより入力手段と出力手段と演算手段として機能する。
【００３５】
〔２．姿勢行列〕
ここで、携帯型情報機器１０の姿勢を示す姿勢データとしての姿勢行列について説明する。例えば図５に示す、水平東向きのベクトルａ、水平北向きのベクトルｂ、鉛直上向きのベクトルｃを基底とする座標系で携帯型情報機器１０の姿勢を表現するものとする。これらのベクトルが基準基底となるように、ベクトルａ，ｂ，ｃをそれぞれ次式（２７）で定義する。
【数１５】
2008298700.tif 000017
【００３６】
携帯型情報機器１０に固定された互いに直行する３つの基準軸ｘ，ｙ，ｚの方向を向きとし、大きさが１であるベクトルｘ，ｙ，ｚを基準基底｛ａ，ｂ，ｃ｝の一次結合として表すことは携帯型情報機器１０の姿勢を表すことに等しい。そこで、ベクトルｘ，ｙ，ｚを次式（２８）、（２９）、（３０）のように基準基底｛ａ，ｂ，ｃ｝の線形結合として係数ｘａ，ｘｂ，ｘｃ，ｙａ，ｙｂ，ｙｃ，ｚａ，ｚｂ，ｚｃを用いて表現するとする。
【数１６】
2008298700.tif 000018
【００３７】
式（２８）、（２９）、（３０）の関係は次式（３１）のように書き換えることができる。
【数１７】
2008298700.tif 000019
基準基底を式（２７）で定義しているため、式（３１）は次式（３２）と等価である。
【数１８】
2008298700.tif 000020
【００３８】
ベクトルｘ，ｙ，ｚを１つの行列にまとめて表現した次式（３３）の行列を姿勢行列というものとする。行列Ａは行列式が１である正格直交行列であり、携帯型情報機器１０の自由度３の姿勢と一対一に対応する。また姿勢をこのような姿勢行列で表現すると、微小な姿勢変化は、常に、行列成分の微小な変化として表れる。すなわち、姿勢行列の成分は、特異区間のないパラメータである。
【数１９】
2008298700.tif 000021
【００３９】
〔３．姿勢データフィルタ装置のソフトウェア構成〕
制御部１３は図４に示すモジュール群を備えるコンピュータプログラムを実行することにより、携帯型情報機器１０の姿勢をディスプレイ１４に表示する機能を有する。
加速度データ処理モジュール１３１は、加速度センサ１１から出力される加速度データを時間間隔Ｔ'で入力し、加速度データに所定の補正処理を施す。
地磁気データ処理モジュール１３２は、地磁気センサ１２から出力される地磁気データを時間間隔Ｔ'で入力し、地磁気データに所定の補正処理を施す。
姿勢データ生成モジュール１３３は、加速度データ及び地磁気データに基づいて姿勢行列である入力姿勢行列Ａ'の成分である姿勢データを生成する。加速度データ及び地磁気データに基づいて姿勢データを生成する処理はよく知られた処理であるため説明を省略する。
【００４０】
姿勢データフィルタモジュール１３４は、時刻ｉＴの入力姿勢行列Ａ'ｉにフィルタ処理を施し、携帯型情報機器１０の姿勢を示す姿勢行列である出力姿勢行列Ａｉの成分を出力データとして出力するプログラムである。出力姿勢行列Ａは、制御部１３の不揮発性記憶媒体に記憶されている初期値Ａ０とω０と入力姿勢行列Ａ'ｉとに基づいて導出される。
姿勢データ処理モジュール１３５は、出力姿勢行列Ａｉに基づいて携帯型情報機器１０の姿勢をディスプレイ１４に表示するための処理を実行する。携帯型情報機器１０の姿勢は、オイラー角などの数字で表示してもよいし、画像で表示してもよい。
【００４１】
〔３．姿勢データフィルタリング処理〕
図６は、姿勢データフィルタモジュール１３４による姿勢データフィルタリング処理を示すフローチャートである。
はじめに姿勢データフィルタモジュール１３４は入力姿勢行列Ａ'ｍ（ｍ＝０，１，２・・・）を入力する（ステップＳ１０）。
【００４２】
次に姿勢データフィルタモジュール１３４は入力姿勢行列Ａ'ｍと出力姿勢行列Ａｍ―１とから、Ｒｍを導出する（ステップＳ１１）。Ｒｍは、次式（３４）に示すように時刻（ｍ−１）Ｔにおける仮想物体の姿勢（第二の姿勢）から時刻ｍＴにおける対象物体の姿勢（第一の姿勢）への回転を示す正格直交行列である。
【数２０】
2008298700.tif 000022
【００４３】
次に姿勢データフィルタモジュール１３４は、時刻（ｍ−１）Ｔにおける第二の姿勢から時刻ｍＴにおける第一の姿勢への回転の角速度ω'ｍをＲｍから導出する（ステップＳ１２）。携帯型情報機器１０の回転は角速度ベクトルを回転軸とする右ねじ方向への回転として表現し、||ω'ｉ||は角速度ω'ｉの大きさに比例し、Ｒｍのｉ行ｊ列成分をｒｍ，ｉｊとするとき、ω'ｉは次式（３５）によって定まる。ただしＫは正の定数とする。
【数２１】
2008298700.tif 000023
【００４４】
次に姿勢データフィルタモジュール１３４はω'ｍ−１、ωｍ−１からωｍ、Ωｍを導出する（ステップＳ１３）。ωｉは出力姿勢行列の軌跡が示す携帯型情報機器１０の時刻ｉＴにおける角速度であって、次式（３６）、（３７）によって定まる。
【数２２】
2008298700.tif 000024
またΩｉは次式（３８）によって定まる。
【数２３】
2008298700.tif 000025
【００４５】
次に姿勢データフィルタモジュール１３４は出力姿勢行列Ａｍ−１、Ωｍ、から出力姿勢行列Ａｍの成分を導出する（ステップＳ１４）。出力姿勢行列Ａｍは次式（３９）によって定まる。
【数２４】
2008298700.tif 000026
姿勢データフィルタモジュール１３４は、以上の処理を入力姿勢行列Ａ'ｉ毎に順次繰り返すことにより、出力姿勢行列Ａｉの成分を姿勢データとして順次姿勢データフィルタ処理モジュール１３５に出力する。
以上説明した実施形態によると、フィルタ処理の対象となる入力データ及び出力データを正格直交行列の成分とすることにより、特異区間におけるアルゴリズムを変更するための分岐処理などで処理が複雑化することがない。連続時間における微分方程式（１６）、（２０）を式（１８）、（２３）によって近似することによって、入力データＡ'ｉから出力データＡｉを導出するための計算量を低減することができる。また、隣接二項間漸化式の演算によって入力データＡ'ｉから出力データＡｉを導出できるため、演算部１３に必要なバッファの段数を低減することができる。また、定数ＫおよびΓの設定によって、平滑化強度を自由に設定することができる。
【００４６】
〔４．他の実施形態〕
本発明の技術的範囲は、上述した実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々変更を加え得ることは勿論である。例えば、時間間隔Ｔは仕様に見合う範囲で十分短い時間であればよく、制御部１３に加速度データおよび地磁気データを入力する時間間隔Ｔ'と姿勢データフィルタの漸化式で用いる時間間隔Ｔとは一致していてもよいし、Ｔ＝ｎＴ'（ｎ＝１，２・・・）、Ｔ＝Ｔ'／ｎであってもよい。
【００４７】
また、姿勢を示す入力データおよび出力データの形式は、対象となる物体の姿勢を表現可能な形態であればよく、微小な姿勢変化が常にパラメータの微小な変化として表れる四元数を用いてもよいし、微小な姿勢変化がパラメータの不連続変化として表れる特異区間のあるオイラー角を用いてもよい。
【００４８】
また、出力姿勢行列Ａｉは、時刻（ｉ−１）Ｔにおける第二の姿勢から時刻ｉＴにおける第一の姿勢への回転の角度をθｉ、出力データの軌跡が示す時刻ｉＴにおける角速度をωｉ、時刻（ｉ−１）Ｔから時刻ｉＴまでの第二の姿勢の回転の角加速度を、Ｎｉ＝Ｎ１ｉ−Ｎ２ｉとするとき、次の条件を満たすものであればよい。
（１）θｉ＝０であるとき、Ｎ１ｉ＝０であり、
（２）−π／２≦θｉ≦π／２であるとき、Ｎ１ｉが示す角加速度の大きさはθｉの絶対値に対して単調増加し、
（３）Ｎ１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに対応し、
（４）Ｎ２ｉが示す角加速度の大きさはωｉが示す角速度の大きさに対して単調増加し、
（５）Ｎ２ｉが示す角加速度の向きはωｉが示す角速度の向きに対応する。
すなわち例えば、（２）を満たすθとＮ１ｉの大きさとの関数を図７（Ａ）、図７（Ｂ）に示すような関数としてもよいし、Ｎ１ｉが示す角加速度の向きが時刻（ｉ−１）Ｔにおける第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きから若干ずれてもよいし、Ｎ２ｉが示す角加速度の向きもωｉが示す角速度の向きから若干ずれてもよい。
【図面の簡単な説明】
【００４９】
【図１】本発明の実施形態にかかる説明図。
【図２】本発明の実施形態にかかる説明図。
【図３】本発明の実施形態にかかる説明図。
【図４】本発明の実施形態にかかるブロック図。
【図５】本発明の実施形態にかかる模式図。
【図６】本発明の実施形態にかかるフローチャート。
【図７】本発明の実施形態にかかるグラフ。
【符号の説明】
【００５０】
１０：携帯型情報機器、１１：加速度センサ、１２：地磁気センサ、１３：制御部、１４：ディスプレイ、１３１：地磁気データ処理モジュール、１３２：加速度データ処理モジュール、１３３：姿勢データ生成モジュール、１３４：姿勢データフィルタモジュール、１３５：姿勢データ処理モジュール
【図１】
2008298700.tif 000029
【図２】
2008298700.tif 000030
【図３】
2008298700.tif 000031
【図４】
2008298700.tif 000032
【図５】
2008298700.tif 000033
【図６】
2008298700.tif 000034
【図７】
2008298700.tif 000035