JP2015179002A - 姿勢推定方法、姿勢推定装置及びプログラム - Google Patents

Abstract

【課題】慣性センサーの出力信号を用いて十分な精度で物体の姿勢を推定することが可能な姿勢推定方法、姿勢推定装置及びプログラムを提供すること。
【解決手段】姿勢推定方法は、角速度センサー１２の出力を用いて、サンプリング周期Δｔを変数とする多項式で近似して対象物の姿勢変化量を計算する工程（Ｓ８）と、加速度センサー１４の出力と角速度センサー１２の出力とを用いて、対象物の速度変化量を計算する工程（Ｓ９）と、姿勢変化量と速度変化量とを用いて、対象物の姿勢を推定する工程（Ｓ１０）と、を含む。
【選択図】図４

Description

本発明は、対象物の姿勢を推定する、姿勢推定方法、姿勢推定装置及びプログラムに関する。

物体に慣性計測装置（ＩＭＵ：Inertial Measurement Unit）を装着し、慣性計測装置（ＩＭＵ）の出力信号を用いて物体の位置や姿勢を算出する装置やシステムが知られている。慣性計測装置（ＩＭＵ）の出力信号にはバイアス誤差があり、姿勢計算にも誤差が生じるため、カルマンフィルターを用いてこれらの誤差を補正し、物体の正確な姿勢を推定する手法が提案されている。例えば、特許文献１では、ジャイロ、加速度センサー、磁気センサー、速度センサーの観測値をカルマンフィルターに入力し、移動体の姿勢を推定する姿勢推定装置が提案されている。

特開２０１１−２２７０１７号公報

しかしながら、特許文献１に記載の姿勢推定装置では、高精度な姿勢推定を実現するために、磁気センサーの観測値を用いることが必須であるため、センサーの部品点数が多くなり、低コスト化や小型化が難しいという問題がある。

本発明は、以上のような問題点に鑑みてなされたものであり、本発明のいくつかの態様によれば、慣性センサーの出力信号を用いて十分な精度で物体の姿勢を推定することが可能な姿勢推定方法、姿勢推定装置及びプログラムを提供することができる。

本発明は前述の課題の少なくとも一部を解決するためになされたものであり、以下の態様または適用例として実現することが可能である。

［適用例１］
本適用例に係る姿勢推定方法は、角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する工程と、加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する工程と、前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する工程と、を含む。

対象物は、例えば、人、動物、移動体、運動器具であってもよい。

慣性センサーは、加速度や角速度等の慣性量を計測可能なセンサーであればよく、例えば、加速度や角速度を計測可能な慣性計測ユニット（ＩＭＵ：Inertial Measurement Unit）でもよい。また、慣性センサーは、対象物に対して脱着可能であってもよいし、例えば、対象物に内蔵されるなど、対象物に固定されていて取り外すことができないものでもよい。

本適用例に係る姿勢推定方法によれば、サンプリング周期の多項式で近似して姿勢変化
量を計算することで、従来よりも姿勢変化量の計算誤差を小さくすることができる。また、本適用例に係る姿勢推定方法によれば、加速度センサーの出力だけでなく角速度センサーの出力も用いて速度変化量を計算することで、対象物の回転がすぐに速度変化量に反映され、対象物が急激に回転した場合でも、速度変化量の計算精度の劣化を低減させることができる。従って、本適用例に係る姿勢推定方法によれば、角速度センサーの出力と加速度センサーの出力とを用いて、十分な精度で物体の姿勢を推定することができる。

［適用例２］
上記適用例に係る姿勢推定方法は、前記対象物の速度変化量を計算する工程において、前記角速度センサーの出力と前記加速度センサーの出力とを用いて前記サンプリング周期を変数とする多項式で近似して前記対象物の加速度を計算し、当該加速度を用いて前記速度変化量を計算してもよい。

本適用例に係る姿勢推定方法によれば、サンプリング周期の多項式で近似して速度変化量を計算することで、従来よりも速度変化量の計算誤差を小さくすることができる。従って、本適用例に係る姿勢推定方法によれば、角速度センサーの出力と加速度センサーの出力とを用いて、十分な精度で物体の姿勢を推定することができる。

［適用例３］
上記適用例に係る姿勢推定方法において、前記対象物の姿勢を推定する工程は、前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢と速度とを予測する工程と、前記予測した前記対象物の速度とゼロとの差を用いて、前記予測した前記対象物の姿勢を補正する工程と、を含んでもよい。

本適用例に係る姿勢推定方法によれば、対象物の運動速度は長期的にはゼロであるとして対象物の速度の観測残差を用いて、対象物の姿勢を補正するので、方位角の観測情報が無くても、対象物の姿勢を精度よく推定することができる。

［適用例４］
上記適用例に係る姿勢推定方法において、前記対象物の姿勢を推定する工程は、前記角速度センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、を含み、前記対象物の姿勢を補正する工程において、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正してもよい。

角速度センサーの出力と加速度センサーの出力とを用いて対象物の姿勢を補正する場合、これらの出力には対象物の方位角についての情報が含まれないので、対象物の方位角誤差については補正されない。従って、更新された姿勢誤差に含まれる方位角誤差を残しておくと、方位角誤差の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本適用例に係る姿勢推定方法によれば、角速度センサーの出力を用いて更新した対象物の姿勢誤差に含まれる方位角誤差を除去するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。

［適用例５］
上記適用例に係る姿勢推定方法において、前記対象物の姿勢を推定する工程は、前記角速度センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程を含み、前記対象物の姿勢を補正する工程において、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正してもよい。

角速度センサーの出力と加速度センサーの出力とを用いて対象物の姿勢を補正する場合、これらの出力には対象物の水平方向の回転誤差についての情報が含まれないので、対象物の水平方向の回転誤差については補正されない。従って、対象物の回転誤差が方位角誤差成分を含むものとして対象物の姿勢誤差を更新すると、回転誤差の方位角誤差成分の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本適用例に係る姿勢推定方法によれば、角速度センサーの出力を用いて、対象物の回転誤差が方位角誤差成分を含まないものとして対象物の姿勢誤差を更新するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。

［適用例６］
本適用例に係る姿勢推定方法は、慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、を含む。

慣性センサーの出力を用いて対象物の姿勢を補正する場合、慣性センサーの出力には対象物の方位角についての情報が含まれないので、対象物の方位角誤差については補正されない。従って、更新された姿勢誤差に含まれる方位角誤差を残しておくと、方位角誤差の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本適用例に係る姿勢推定方法によれば、慣性センサーの出力を用いて更新した対象物の姿勢誤差に含まれる方位角誤差を除去するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。

［適用例７］
本適用例に係る姿勢推定方法は、慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、を含む。

慣性センサーの出力を用いて対象物の姿勢を補正する場合、慣性センサーの出力には対象物の水平方向の回転誤差についての情報が含まれないので、対象物の水平方向の回転誤差については補正されない。従って、対象物の回転誤差が方位角誤差成分を含むものとして対象物の姿勢誤差を更新すると、回転誤差の方位角誤差成分の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本適用例に係る姿勢推定方法によれば、慣性センサーの出力を用いて、対象物の回転誤差が方位角誤差成分を含まないものとして対象物の姿勢誤差を更新するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。

［適用例８］
上記適用例に係る姿勢推定方法は、前記慣性センサーの出力を用いて、前記対象物の速度変化量を計算する工程を含み、前記対象物の姿勢を予測する工程において、前記速度変化量を用いて、前記対象物の速度を予測し、前記対象物の姿勢を補正する工程において、前記予測した前記対象物の速度とゼロとの差を用いて、前記予測した前記対象物の姿勢を補正してもよい。

本適用例に係る姿勢推定方法によれば、対象物の運動速度は長期的にはゼロであるとして対象物の速度の観測残差を用いて、対象物の姿勢を補正するので、方位角の観測情報が無くても、対象物の姿勢を精度よく推定することができる。

［適用例９］
上記適用例に係る姿勢推定方法は、前記慣性センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して前記対象物の姿勢変化量を計算する工程を含み、前記対象物の姿勢を予測する工程において、前記姿勢変化量を用いて、前記対象物の姿勢を予測してもよい。

本適用例に係る姿勢推定方法によれば、サンプリング周期の多項式で近似して姿勢変化量を計算することで、従来よりも姿勢変化量の計算誤差を小さくすることができる。従って、本適用例に係る姿勢推定方法によれば、慣性センサーの出力を用いて、十分な精度で物体の姿勢を推定することができる。

［適用例１０］
本適用例に係る姿勢推定装置は、角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する姿勢変化量計算部と、加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する速度変化量計算部と、前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する姿勢推定部と、を含む。

［適用例１１］
上記適用例に係る姿勢推定装置は、前記角速度センサー及び前記加速度センサーを含んでもよい。

［適用例１２］
本適用例に係る姿勢推定装置は、慣性センサーの出力を用いて、対象物の姿勢を予測する姿勢予測部と、前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する姿勢誤差更新部と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する姿勢補正部と、前記更新された前記姿勢誤差に含まれる方位角誤差を除去する方位角誤差除去部と、を含む。

［適用例１３］
本適用例に係る姿勢推定装置は、慣性センサーの出力を用いて、対象物の姿勢を予測する姿勢予測部と、前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する姿勢誤差更新部と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する姿勢補正部と、を含む。

［適用例１４］
上記適用例に係る姿勢推定装置は、前記慣性センサーを含んでもよい。

［適用例１５］
本適用例に係るプログラムは、角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する工程と、加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する工程と、前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する工程と、をコンピューターに実行させる。

［適用例１６］
本適用例に係るプログラムは、慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、前
記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、をコンピューターに実行させる。

［適用例１７］
本適用例に係るプログラムは、前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程と、前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、をコンピューターに実行させる。

本実施形態の姿勢推定装置の構成例を示す図。 センサー座標系とローカル座標系について説明するための図。 本実施形態における処理部の構成例を示す図。 姿勢推定処理の手順の一例を示すフローチャート図。 状態ベクトルｘ_ｋ及び誤差共分散行列Σ_ｘ，ｋ ^２を推定する処理の手順の一例を示すフローチャート図。 本実施形態の姿勢推定システムの構成例を示す図。

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

１．姿勢推定理論
１−１．ＩＭＵ出力モデル
慣性計測装置（ＩＭＵ）の（補正済み）出力は、各サンプリング時刻（ｔ_ｋ）における３軸ジャイロセンサー出力ｄ_ω，ｋと３軸加速度センサー出力ｄ_ａ，ｋで構成される。ここで、３軸ジャイロセンサー出力ｄ_ω，ｋは、式（１）のように、サンプリング期間（Δｔ＝ｔ_ｋ−ｔ_ｋ−１）中の角速度ωの平均値と残存バイアス誤差ｂ_ωとの和で表される。

３軸加速度センサー出力ｄ_ａ，ｋも同様に、式（２）のように、加速度αの平均値と残存バイアス誤差ｂ_αとの和で表される。

１−２．角速度積分による３次元姿勢算出
３次元姿勢をクォータニオン（四元数）で表す場合、その姿勢ｑと角速度ω［ｒａｄ／ｓ］との関係は式（３）の微分方程式で表される。

ここで、○と■を重ねた記号はクォータニオン乗算を表し、例えばｑとｐのクォータニオン乗算の各要素は、式（４）のように計算される。

また、３次元ベクトルωは、式（５）のように、実（スカラー）成分がゼロで純虚（ベクトル）成分がωの各成分と一致するクォータニオンと等価とみなす。

微分方程式（３）を解くことで３次元姿勢を算出することができるが、残念ながらその一般解は見つかっていない。さらに角速度ωの値も離散的な平均値としてしか得られないため、式（６）を使って微小（サンプリング）時間ごとに逐一近似計算を行わなければならない。

式（６）は、姿勢クォータニオンｑと、角速度ωの軸ごとの積分関数を考え、それぞれのｔ＝ｔ_ｋ−１におけるテイラー展開からΔｔの３次項までに基づいて算出した式であり、式中のω_ｋ−１を含む項がコーニング補正項に相当する。記号■は３次元ベクトルのクロス積（ベクトル積）を表し、例えばｖ■ｗの各要素は式（７）のように計算される。

１−３．重力加速度による傾き誤差観測
加速度センサーはその運動によって生じる加速度を検出するが、地球上では常に約１［Ｇ］（＝９．８０６６５［ｍ／ｓ^２］）の重力加速度が加算されて検出される。この重力加速度は必ず鉛直（上）方向のベクトルであるため、３軸加速度センサー出力との比較によって姿勢の傾き（ロールとピッチ）成分の誤差を知ることができる。そのために、まず３軸加速度センサーによって観測されたセンサー座標系（ｘｙｚ座標教）の加速度ベクトルαを水平な直交軸と鉛直下向き軸によるローカル空間の座標系（ＸＹＺ座標系）でのベクトルα'に変換しなければならない。この座標（回転）変換は姿勢クォータニオンｑとその共役クォータニオンｑ^＊を使って式（８）のように計算することができる。

この式（８）は３次元座標変換行列Ｃを使って式（９）のように表すこともできる。

この加速度α’を同じローカル空間座標系（ＸＹＺ座標系）における重力加速度ベクトルｇと比較することで傾き誤差を得られる。重力加速度ベクトルｇは、式（１０）で表される。式（１０）において、Δｇは重力加速度ｇの標準値との差［Ｇ］を表す。

１−４．ゼロ運動速度観測
特に、ユーザインターフェース用途においてＩＭＵの運動速度は長期的にほぼゼロに等しいと考えられる。ローカル空間座標系における運動速度ｖとセンサー座標系における加速度αおよび角速度ωとの関係は座標変換行列Ｃを使って式（１１）の微分方程式で表される。

ここでも加速度αおよび角速度ωの値は離散的な平均値としてしか得られないため、式（１２）を使って微小（サンプリング）時間ごとに逐一近似計算することで運動速度ベクトルを算出する。

式（１２）は、速度ベクトルｖと、加速度αおよび角速度ωの軸ごとの積分関数を考え、それぞれのｔ＝ｔ_ｋ−１におけるテイラー展開からΔＴの３次項までに基づいて算出した式である。なお、３次項には残存誤差ε_λが存在するが十分小さいため式（１２）では無視する。記号・は３次元ベクトルのドット積（スカラー積）を表し、例えばｖ・ｗは式（１３）のように計算される。

１−５．姿勢クォータニオンとその誤差
算出される姿勢クォータニオンｑは、式（１４）のようにその真値（＾）に対して誤差ε_ｑを持つと考えられる。

ここでΣ_ｑ ^２は誤差ε_ｑの大きさを表す誤差共分散行列である。またＥ［・］は期待値を、右肩のＴはベクトル・行列の転置をそれぞれ表す。

クォータニオンおよびその誤差はその要素として四つの値を持つが、三次元姿勢（回転変換）の自由度は３つしかない。姿勢クォータニオンが持つ第４の自由度は拡大／縮小変換に相当するが、姿勢検出処理においてはこの拡大／縮小率は常に１に固定されなければならない。実際には各種の演算誤差によってこの拡大／縮小率成分が変化してしまうので、これを抑制するための処理が必要になる。

姿勢クォータニオンｑの場合はその絶対値の二乗が拡大／縮小率に相当するので、式（１５）のように絶対値を１にする正規化処理によってその変化を抑制する。

姿勢誤差ε_ｑの場合は、その分散共分散行列Σ_ｑ ^２のランク（階数）を３に保つ必要がある。そこで（姿勢誤差角度は十分小さいとして）ローカル空間座標での（３次元）誤差回転ベクトルε_θを考えて、式（１６）のようにランクを制限する。

１−６．方位角誤差の除去（無視）
姿勢検出において磁気センサーなどの方位角観測手段を持たない場合、姿勢誤差の方位角成分は単調に増加するだけで何の役にも立たない。更には、その増大した誤差見積もりがフィードバックゲインを不必要に上げる事となり予期しない方位角の変化や振動の原因となる。そこで、式（１６）から方位角誤差成分ε_θｚを除去する。

１−７．拡張カルマンフィルター
上記の各モデル式に基づいて３次元姿勢を算出する拡張カルマンフィルターを設計することができる。

［状態ベクトルと誤差共分散行列］
式（１８）のように、求めるべき未知の状態値として、姿勢クォータニオンｑ、運動速度ベクトルｖ、ジャイロセンサーバイアスｂ_ω（角速度ωに対するオフセット）、加速度センサーバイアスｂ_α（加速度αに対するオフセット）、および重力加速度補正値Δｇで拡張カルマンフィルターの状態ベクトルｘ（１４次元のベクトル）を構成し、その誤差の共分散行列Σ_ｘ ^２を定義する。

［プロセスモデル］
プロセスモデルでは、サンプリング間隔Δｔ、ジャイロセンサー出力ｄ_ωおよび加速度センサー出力ｄ_αの値に基づいて最新の状態ベクトルの値を式（１９）のように予測する。

状態誤差の共分散行列は、ジャイロセンサー出力ｄ_ωと加速度センサー出力ｄ_αのノイズ成分η_ω、η_αと、ジャイロセンサーバイアスｂ_ω、加速度センサーバイアスｂ_α、および重力加速度値ｇの不安定性を表すプロセスノイズρ_ω、ρ_α、およびρ_ｇの影響を受けて式（２０）のように更新される。

ここで０_ｎ×ｍはｎ行ｍ列の零行列を、Ｉ_ｎ×ｍはｎ行ｍ列の恒等行列を表し、Ｊ_…は、このプロセスモデルの偏微分で求められる式（２１）のような各誤差の伝播係数の行列である。また、Σ_…は式（２２）のような各ノイズの共分散行列である。

［観測モデル］
観測モデルでは、加速度センサー出力ｄ_αに基づく重力加速度の観測残差と、ゼロ運動速度の観測残差を式（２３）のように算出する。

ここに加速度センサー出力ｄ_αのノイズ成分η_αおよび観測誤差となる運動加速度成分ζ_ａと運動速度成分ζ_ｖを加味して式（２４）のようにカルマン係数Ｋを算出する。

ここでＪ_…は、この観測モデルの偏微分で求められる式（２５）のような各誤差の伝播係数の行列である。また、Σ_…は式（２６）のような各ノイズの共分散行列である。

ここで、μは予測運動加速度から各軸の誤差ＲＭＳを算出する係数である。このカルマン係数Ｋを使って式（２７）のように状態ベクトルｘを補正するとともに、その誤差共分散行列Σ_ｘ ^２を更新する。

［姿勢正規化モデル］
姿勢正規化モデルは、姿勢クォータニオンとその誤差共分散を適切な値に維持するために式（２８）の更新を行う。

ここで、Ｄ’^ＴＤ’は姿勢誤差のランク制限と方位角成分除去を行う式（２９）の行列である。

１−８．初期値
［状態ベクトルと誤差共分散行列］
状態ベクトルｘと誤差共分散行列Σ_ｘ ^２の初期値は式（３０）のように与える。

［姿勢クォータニオン］
ＩＭＵの姿勢をクォータニオン表現で与える必要がある。航空機の姿勢などで用いられるロール（バンク）角φ［ｒａｄ］、ピッチ（仰）角θ［ｒａｄ］、およびヨー（方位）角ψ［ｒａｄ］からは式（３１）のように姿勢クォータニオンｑを算出できる。

また、その誤差共分散行列Σ_ｑ ^２はロール角誤差のＲＭＳσ_φ［ｒａｄ ＲＭＳ］とピッチ角誤差のＲＭＳσ_θ［ｒａｄ ＲＭＳ］から式（３２）のように算出される（ヨー角誤差は無視される）。

［速度ベクトル］
初期状態が静止ならば、速度ベクトルｖをゼロにするべきである。また、静止かどうかにかかわらず速度ベクトルｖの各軸の誤差ＲＭＳσ_ｖｘ、σ_ｖｙ、およびσ_ｖｚ［Ｇｓ ＲＭＳ］に基づいて式（３３）のように誤差共分散行列Σ_ｖ ^２を与える。

［ジャイロ／加速度センサー残存バイアス］
ジャイロセンサーの残存バイアスｂ_ωおよび加速度センサーの残存バイアスｂ_αが分かっているなら適切に設定されるべきである。不明な場合は期待値としてゼロを与える。また、ジャイロセンサーの各軸の残存バイアスの誤差ＲＭＳσ_ｂωｘ、σ_ｂωｙ、およびσ_ｂωｚ［ｒａｄ／ｓ ＲＭＳ］と、加速度センサーの各軸の残存バイアスの誤差ＲＭＳσ_ｂαｘ、σ_ｂαｙ、およびσ_ｂαｚ［Ｇ ＲＭＳ］に基づいて式（３４）のように誤差共分散行列Σ_ｂω ^２及びΣ_ｂα ^２を与える。

［重力加速度補正値］
重力加速度値が分かっているなら標準値１［Ｇ］（＝９．８０６６５［ｍ／ｓ^２］）との差が適切に設定されるべきである。不明な場合は期待値としてゼロを与える。また、その値の誤差ＲＭＳσ_Δｇ［Ｇ ＲＭＳ］を誤差共分散行列に反映する。

１−９．設定値
［サンプリング間隔］
ＩＭＳ出力からの姿勢検出処理は原理的に時間積分演算のため、サンプリング間隔Δｔ［ｓ］は重要な値であり、適切に設定されなければならない。

［ジャイロ／加速度センサー出力ノイズ］
ジャイロセンサーおよび加速度センサーの出力に含まれるノイズ成分ηを、軸ごとに独立した平均ゼロ、分散σ_η ^２のホワイトガウスノイズとみなし、その大きさを式（３５）のようにそれぞれのＲＭＳ値（σ_ηωｘ，σ_ηωｙ，σ_ηωｚ）［ｒａｄ／ｓ ＲＭＳ］および（σ_ηαｘ，σ_ηαｙ，σ_ηαｚ）［Ｇ ＲＭＳ］で指定する。

［ジャイロ／加速度センサーバイアスおよび重力加速度値の不安定性］
ジャイロセンサーおよび加速度センサーのバイアスは一定ではなく時間とともに変動すると考えられる。また、重力加速度値も周囲の環境によってわずかながら変動すると考えられる。これらの変動を個別のランダムウォークとみなして不安定性を式（３６）のように（σ_ρωｘ，σ_ρωｙ，σ_ρωｚ）［ｒａｄ／ｓ／√ｓ］、（σ_ραｘ，σ_ραｙ，σ_ραｚ）［Ｇ／√ｓ］およびσ_ρｇ［Ｇ／√ｓ］で指定する。

［運動加速度］
姿勢補正のための重力加速度観測において、運動加速度成分は観測誤差ζ_ａとなる。これを単純なホワイトガウスノイズとみなした場合、急激な運動による大きな運動加速度に対して姿勢が過敏に反応する結果となる。そこで、推定運動加速度（観測加速度と推定重力加速度の差）の大きさに応じてレベルが変化する式（３７）のようなノイズモデルを使い、その線形係数μ［Ｎ／Ａ］と定数項（σ_ζαｘ，σ_ζαｙ，σ_ζαｚ）［Ｇ ＲＭＳ］を設定項目とする。

［運動速度］
ゼロ運動速度観測では、ＩＭＵの運動速度が長期的にほぼゼロであることを観測するため、短期的に現れる運動速度は観測誤差ζ_ｖとなる。これを軸ごとに独立したホワイトガウスノイズとみなし、その大きさを式（３８）のようにＲＭＳ値（σ_ζｖｘ，σ_ζｖｙ，σ_ζｖｚ）［Ｇｓ ＲＭＳ］で指定する。

２．姿勢推定装置
［姿勢推定装置の構成］
図１は、本実施形態の姿勢推定装置の構成例を示す図である。図１に示すように、本実施形態の姿勢推定装置１は、慣性計測ユニット（ＩＭＵ）１０、処理部２０、ＲＯＭ３０、ＲＡＭ４０、記録媒体５０及び通信部６０を含んで構成されている。ただし、本実施形態の姿勢推定装置１は、これらの一部の要素を変更又は削除し、あるいは、他の要素を追加した構成であってもよい。

姿勢推定装置１は、ＩＭＵ１０と処理部２０が１つの筐体に収容されて構成されていてもよいし、ＩＭＵ１０が処理部２０を収容する本体から分離されて（あるいは分離可能に）構成されていてもよい。前者の場合は姿勢推定装置１が対象物に装着され、後者の場合はＩＭＵ１０が対象物に装着され、姿勢推定装置１は、当該対象物の姿勢を推定する処理を行う。

本実施形態では、ＩＭＵ１０は、角速度センサー１２、加速度センサー１４及び信号処理部１６を含んで構成されている。ただし、本実施形態のＩＭＵ１０は、これらの一部の要素を変更又は削除し、あるいは、他の要素を追加した構成であってもよい。

角速度センサー１２は、互いに交差する（理想的には直交する）３軸方向の各々の角速度を計測し、計測した３軸角速度の大きさ及び向きに応じたアナログ信号を出力する。

加速度センサー１４は、互いに交差する（理想的には直交する）３軸方向の各々の加速度を計測し、計測した３軸加速度の大きさ及び向きに応じたアナログ信号を出力する。

信号処理部１６は、角速度センサー１２の出力信号を所定の周期Δｔでサンプリングしてデジタル値の３軸角速度データに変換する処理を行う。また、信号処理部１６は、加速度センサー１４の出力信号を所定の周期Δｔでサンプリングしてデジタル値の３軸加速度データに変換する処理を行う。

角速度センサー１２及び加速度センサー１４は、それぞれ３軸が、ＩＭＵ１０に対して定義される直交座標系（センサー座標系）の３軸（ｘ軸、ｙ軸、ｚ軸）と一致するようにＩＭＵ１０に取り付けられるのが理想的だが、実際には取り付け角の誤差が生じる。そこで、信号処理部１６は、取り付け角誤差に応じてあらかじめ算出された補正パラメーターを用いて、３軸角速度データ及び３軸加速度データをｘｙｚ座標系のデータに変換する処理も行う。さらに、信号処理部１６は、角速度センサー１２及び加速度センサー１４の温度特性に応じて、３軸角速度データ及び３軸加速度データを温度補正する処理も行う。

なお、角速度センサー１２及び加速度センサー１４にＡ／Ｄ変換や温度補正の機能が組み込まれていてもよい。

ＩＭＵ１０は、信号処理部１６による処理後の３軸角速度データｄ_ω，ω及び３軸加速度データｄ_ω，αを処理部２０に出力する。

ＲＯＭ３０は、処理部２０が各種の処理を行うためのプログラムや、アプリケーション
機能を実現するための各種プログラムやデータ等を記憶している。

ＲＡＭ４０は、処理部２０の作業領域として用いられ、ＲＯＭ３０から読み出されたプログラムやデータ、処理部２０が各種プログラムに従って実行した演算結果等を一時的に記憶する記憶部である。

記録媒体５０は、処理部２０の処理により生成されたデータのうち、長期的な保存が必要なデータを記憶する不揮発性の記憶部である。また、記録媒体５０は、処理部２０が各種の処理を行うためのプログラムや、アプリケーション機能を実現するための各種プログラムやデータ等を記憶していてもよい。

処理部２０は、ＲＯＭ３０あるいは記録媒体５０に記憶されているプログラム、あるいはネットワークを介してサーバーから受信してＲＡＭ４０や記録媒体５０に記憶したプログラムに従って各種の処理を行う。特に、本実施形態では、処理部２０は、当該プログラムを実行することにより、バイアス除去部２２、姿勢変化量計算部２４、速度変化量計算部２６及び姿勢推定部２８として機能し、ＩＭＵ１０がΔｔ間隔で出力する３軸角速度データｄ_ω，ｋ及び３軸加速度データｄ_ω，αに対して所定の演算を行い、対象物の姿勢を推定する処理を行う。

本実施形態では、図２に示すように、ＩＭＵ１０の座標系であるセンサー座標系（互いに直交するｘ軸、ｙ軸、ｚ軸で構成されるｘｙｚ座標系）と対象物が存在する空間の座標系であるローカル空間座標系（互いに直交するＸ軸、Ｙ軸、Ｚ軸で構成されるＸＹＺ座標系）を考える。処理部２０は、対象物に装着されたＩＭＵ１０から出力されるセンサー座標系の３軸角速度と３軸加速度からローカル空間座標系における対象物の姿勢（ＩＭＵ１０の姿勢ともいえる）を推定する。

バイアス除去部２２は、角速度センサー１２の出力からバイアス誤差を除去した３軸角速度を計算する処理、及び加速度センサー１４の出力からバイアス誤差を除去した３軸加速度を計算する処理を行う。

姿勢変化量計算部２４は、角速度センサー１２の出力を用いて（具体的には、バイアス除去部２２によりバイアス誤差が除去された３軸角速度を用いて）、サンプリング周期Δｔを変数とする多項式で近似して対象物の姿勢変化量を計算する処理を行う。

速度変化量計算部２６は、加速度センサー１４の出力と角速度センサー１２の出力とを用いて（具体的には、バイアス除去部２２によりバイアス誤差が除去された３軸角速度と３軸加速度とを用いて）、対象物の速度変化量を計算する処理を行う。

姿勢推定部２８は、積分計算部１０１、姿勢予測部１０２、姿勢誤差更新部１０３、補正係数計算部１０４、姿勢補正部１０５、正規化部１０６、姿勢誤差補正部１０７及び方位角誤差除去部１０８として機能し、姿勢変化量計算部２４が計算した姿勢変化量と速度変化量計算部２６が計算した速度変化量とを用いて、対象物の姿勢を推定する処理を行う。実際には、姿勢推定部２８は、拡張カルマンフィルターにより、前出の式（１８）で定義した状態ベクトルｘ及びその誤差共分散行列Σ_ｘ ^２を推定する処理を行う。

積分計算部１０１は、対象物の姿勢（具体的には、姿勢補正部１０５により補正され、正規化部１０６により正規化された直前の姿勢の推定値）に姿勢変化量計算部２４が計算した姿勢変化量を積算する積分処理を行う。また、積分計算部１０１は、対象物の速度（具体的には、姿勢補正部１０５により補正され、正規化部１０６により正規化された直前の速度の推定値）に速度変化量計算部２６が計算した速度変化量を積算する積分処理を行
う。

姿勢予測部１０２は、角速度センサー１２の出力を用いて（具体的には、姿勢変化量計算部２４が計算した姿勢変化量を用いて）、対象物の姿勢を予測する処理を行う。また、姿勢予測部１０２は、速度変化量計算部２６が計算した速度変化量を用いて、対象物の速度を予測する処理も行う。実際には、姿勢予測部１０２は、状態ベクトルｘを予測する処理を行うが、この処理の詳細については後述する。

姿勢誤差更新部１０３は、角速度センサー１２の出力を用いて（具体的には、バイアス除去部２２によりバイアス誤差が除去された３軸角速度を用いて）、対象物の姿勢誤差を更新する処理を行う。言い換えれば、姿勢誤差更新部１０３は、角速度センサー１２の出力を用いて（具体的には、バイアス除去部２２によりバイアス誤差が除去された３軸角速度を用いて）、対象物の回転誤差が方位角誤差成分を含まないものとして対象物の姿勢誤差を更新する処理を行う。実際には、姿勢誤差更新部１０３は、拡張カルマンフィルターにより、誤差共分散行列Σ_ｘ ^２を更新する処理を行うが、この処理の詳細については後述する。

補正係数計算部１０４は、姿勢誤差更新部１０３により更新された姿勢誤差を用いて、姿勢補正部１０５による対象物の姿勢の補正量や姿勢誤差補正部１０７による姿勢誤差の補正量を決める補正係数を計算する処理を行う。実際には、補正係数計算部１０４は、カルマン係数Ｋを計算する処理を行うが、この処理の詳細については後述する。

姿勢補正部１０５は、姿勢誤差更新部１０３により更新された姿勢誤差を用いて、姿勢予測部１０２が予測した対象物の姿勢を、補正係数計算部１０４が計算した補正係数に応じて補正する処理を行う。特に、姿勢補正部１０５は、姿勢予測部１０２が予測した対象物の速度とゼロとの差を用いて、姿勢予測部１０２が予測した対象物の姿勢を補正する。実際には、姿勢補正部１０５は、拡張カルマンフィルターにより、姿勢予測部１０２が予測した状態ベクトルｘを補正する処理を行うが、この処理の詳細については後述する。

正規化部１０６は、姿勢補正部１０５が補正した対象物の姿勢を、その大きさが変わらないように正規化する処理を行う。実際には、正規化部１０６は、姿勢補正部１０５が補正した状態ベクトルｘを正規化する処理を行うが、この処理の詳細については後述する。

姿勢誤差補正部１０７は、姿勢誤差更新部１０３により更新された姿勢誤差を、補正係数計算部１０４が計算した補正係数に応じて補正する処理を行う。実際には、姿勢誤差補正部１０７は、拡張カルマンフィルターにより、姿勢誤差更新部１０３が更新した誤差共分散行列Σ_ｘ ^２を補正する処理を行うが、この処理の詳細については後述する。

方位角誤差除去部１０８は、姿勢誤差更新部１０３により更新された姿勢誤差（具体的には、さらに姿勢誤差補正部１０７により補正された姿勢誤差）に含まれる方位角誤差を除去する処理を行う。実際には、方位角誤差除去部１０８は、姿勢誤差補正部１０７が補正した誤差共分散行列Σ_ｘ ^２に対して、姿勢の誤差共分散行列Σ_ｑ ^２のランク制限と方位角誤差成分の除去を行った誤差共分散行列Σ_ｘ ^２を生成する処理を行うが、この処理の詳細については後述する。

処理部２０が推定した対象物の姿勢の情報は、通信部６０を介して他の装置に送信可能である。

［処理部の構成］
図３は、処理部２０の具体的な構成例を示す図である。図３において、図１と同じ構成
要素には同じ符号を付している。図３に示すように、ＩＭＵ１０が出力する時刻ｔ_ｋにおける角速度データｄ_ω，ｋ及び加速度データｄ_α，ｋは、バイアス除去部２２に入力される。角速度データｄ_ω，ｋは、時刻ｔ_ｋ−１から時刻ｔ_ｋまでの期間Δｔの角速度ωの平均値と角速度センサー１２の残存バイアス誤差ｂ_ωの和で表される（前出の式（１）参照）。同様に、加速度データｄ_α，ｋは、時刻ｔ_ｋ−１から時刻ｔ_ｋまでの期間Δｔの加速度αの平均値と加速度センサー１４の残存バイアス誤差ｂ_αの和で表される（前出の式（２）参照）。

バイアス除去部２２は、時刻ｔ_ｋにおける角速度データｄ_ω，ｋから、推定された時刻ｔ_ｋ−１における残存バイアス誤差ｂ_{ω，ｋ−１}を差し引いて、時刻ｔ_ｋ−１から時刻ｔ_ｋまでの期間Δｔの角速度ωの平均値を計算する（前出の式（１９）参照）。また、バイアス除去部２２は、時刻ｔ_ｋにおける加速度データｄ_α，ｋから、推定された時刻ｔ_ｋ−１における残存バイアス誤差ｂ_{α，ｋ−１}を差し引いて、時刻ｔ_ｋ−１から時刻ｔ_ｋまでの期間Δｔの加速度αの平均値を計算する（前出の式（１９）参照）。

姿勢変化量計算部２４は、バイアス除去部２２が計算した時刻ｔ_ｋ−１から時刻ｔ_ｋまでの期間Δｔの角速度ωの平均値及び時刻ｔ_ｋ−２から時刻ｔ_ｋ−１までの期間Δｔの角速度ωの平均値を前出の式（６）の多項式に代入し、時刻ｔ_ｋにおける姿勢変化量Δｑ_ｋを近似計算する。

速度変化量計算部２６は、前出の式（９）により、推定された時刻ｔ_ｋ−１における姿勢クォータニオンｑ_ｋ−１から時刻ｔ_ｋにおける座標変換行列Ｃ_ｋを計算する。また、速度変化量計算部２６は、バイアス除去部２２が計算した時刻ｔ_ｋ−１から時刻ｔ_ｋまでの期間Δｔの加速度αの平均値、角速度ωの平均値及び時刻ｔ_ｋ−２から時刻ｔ_ｋ−１までの期間Δｔの加速度αの平均値、角速度ωの平均値を前出の式（１２）の多項式に代入し、時刻ｔ_ｋにおける加速度λ_ｋを近似計算する。また、速度変化量計算部２６は、推定された時刻ｔ_ｋ−１における重力加速度補正値Δｇ_ｋ−１を前出の式（１０）に代入し、時刻ｔ_ｋ−１における重力加速度ベクトルｇ_ｋ−１を計算する。さらに、速度変化量計算部２６は、計算した時刻ｔ_ｋにおける座標変換行列Ｃ_ｋ，加速度λ_ｋ及び時刻ｔ_ｋ−１における重力加速度ベクトルｇ_ｋ−１から時刻ｔ_ｋにおける速度変化量（Ｃ_ｋλ_ｋ−ｇ_ｋ−１）Δｔを計算する。

図３に示すように、姿勢推定部２８は、積分計算部１０１、姿勢予測部１０２、姿勢誤差更新部１０３、補正係数計算部１０４、姿勢補正部１０５、正規化部１０６、姿勢誤差補正部１０７及び方位角誤差除去部１０８を含んで構成されており、拡張カルマンフィルターにより、時刻ｔ_ｋにおける状態ベクトルｘ及びその共分散行列Σ_ｘ，ｋ ^２を推定する。

積分計算部１０１は、推定された時刻ｔ_ｋ−１における姿勢クォータニオンｑ_ｋ−１と姿勢変化量計算部２４が計算した時刻ｔ_ｋにおける姿勢変化量Δｑ_ｋとのクォータニオン乗算（前出の式（６）の計算）を行う。また、積分計算部１０１は、推定された時刻ｔ_ｋ−１における速度ベクトルｖ_ｋ−１と速度変化量計算部２６が計算した時刻ｔ_ｋにおける速度変化量（Ｃ_ｋλ_ｋ−ｇ_ｋ−１）Δｔとの加算（前出の式（１２）の計算）を行う。

姿勢予測部１０２は、前出の式（１９）により、時刻ｔ_ｋにおける状態ベクトルｘ_ｋの各要素である、姿勢クォータニオンｑ_ｋ、運動速度ベクトルｖ_ｋ、角速度センサー１２のバイアスｂ_ω，ｋ、加速度センサー１４のバイアスｂ_α，ｋ、重力加速度補正値Δｇ_ｋを、それぞれ、積分計算部１０１による姿勢クォータニオンｑ_ｋ−１と姿勢変化量Δｑ_ｋとのクォータニオン乗算結果、積分計算部１０１による速度ベクトルｖ_ｋ−１と速度変化量（Ｃ_ｋλ_ｋ−ｇ_ｋ−１）Δｔとの加算結果、推定された時刻ｔ_ｋ−１における角速度セン
サー１２の残存バイアス誤差ｂ_{ω，ｋ−１}、推定された時刻ｔ_ｋ−１における加速度センサー１４の残存バイアス誤差ｂ_{α，ｋ−１}、推定された時刻ｔ_ｋ−１における重力加速度補正値Δｇ_ｋ−１であると予測する。

姿勢誤差更新部１０３は、姿勢変化量計算部２４が計算した時刻ｔ_ｋにおける姿勢変化量Δｑ_ｋ、速度変化量計算部２６が計算した時刻ｔ_ｋにおける加速度λ_ｋ及び座標変換行列Ｃ_ｋ、推定された時刻ｔ_ｋ−１における姿勢クォータニオンｑ_ｋ−１及び時刻ｔ_ｋ−１における誤差共分散行列Σ_{ｘ，ｋ−１} ^２を用いて、前出の式（２０）及び式（２１）より、時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を更新する。

補正係数計算部１０４は、姿勢誤差更新部１０３が更新した時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２、速度変化量計算部２６が計算した時刻ｔ_ｋにおける座標変換行列Ｃ_ｋ、バイアス除去部２２が計算した時刻ｔ_ｋ−１から時刻ｔ_ｋまでの期間Δｔの加速度αの平均値、姿勢予測部が予測した時刻ｔ_ｋにおける姿勢クォータニオンｑ_ｋ及び重力加速度補正値Δｇ_ｋを用いて、前出の式（２３）、式（２４）、式（２５）及び式（２６）より、時刻ｔ_ｋにおける変換行列Ｈ_ｋ及びカルマン係数Ｋ_ｋを計算する。

姿勢補正部１０５は、補正係数計算部１０４が計算した時刻ｔ_ｋにおける変換行列Ｈ_ｋ及びカルマン係数Ｋ_ｋ用いて、前出の式（２７）より、姿勢予測部１０２が予測した時刻ｔ_ｋにおける状態ベクトルｘ_ｋの各要素（姿勢クォータニオンｑ_ｋ、運動速度ベクトルｖ_ｋ、角速度センサー１２のバイアスｂ_ω，ｋ、加速度センサー１４のバイアスｂ_α，ｋ、重力加速度補正値Δｇ_ｋ）を補正する。

正規化部１０６は、姿勢補正部１０５が補正した時刻ｔ_ｋにおける状態ベクトルｘ_ｋの各要素（姿勢クォータニオンｑ_ｋ、運動速度ベクトルｖ_ｋ、角速度センサー１２のバイアスｂ_ω，ｋ、加速度センサー１４のバイアスｂ_α，ｋ、重力加速度補正値Δｇ_ｋ）を、前出の式（２８）により正規化する。

姿勢誤差補正部１０７は、補正係数計算部１０４が計算した時刻ｔ_ｋにおける変換行列Ｈ_ｋ及びカルマン係数Ｋ_ｋ用いて、前出の式（２７）より、姿勢誤差更新部１０３が更新した時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を補正する。

方位角誤差除去部１０８は、正規化部１０６が正規化した時刻ｔ_ｋにおける姿勢クォータニオンｑ_ｋを用いて、前出の式（２９）により行列Ｄ’^ＴＤ’を計算し、行列Ｄ’^ＴＤ’を用いて、式（２８）により、姿勢誤差補正部１０７が補正した時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を計算する。これにより、姿勢クォータニオンｑ_ｋの誤差共分散行列Σ_ｑ，ｋ ^２のランクを３に保つ（制限する）とともに誤差共分散行列Σ_ｑ，ｋ ^２から方位角誤差成分ε_θｚが除去された誤差共分散行列Σ_ｘ，ｋ ^２が生成される。

姿勢推定部２８は、正規化部１０６が計算した時刻ｔ_ｋにおける状態ベクトルｘ_ｋ及び方位角誤差除去部１０８が計算した時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を、それぞれ時刻ｔ_ｋにおける状態ベクトル及びその誤差共分散行列であると推定する。そして、このように推定された時刻ｔ_ｋにおける状態ベクトルｘ_ｋ及び誤差共分散行列Σ_ｘ，ｋ ^２は、次のサンプリング周期Δｔが経過すると、時刻ｔ_ｋ−１における状態ベクトルｘ_ｋ−１及び誤差共分散行列Σ_{ｘ，ｋ−１} ^２として、バイアス除去部２２、速度変化量計算部２６、積分計算部１０１、姿勢予測部１０２及び姿勢誤差更新部１０３にフィードバックされる。

［姿勢推定処理の手順］
図４は、図３に示した処理部２０による姿勢推定処理の手順の一例を示すフローチャー
ト図である。

図４に示すように、処理部２００は、まず、ｋ＝０、ｔ０＝０に設定し（Ｓ２）、時刻ｔ_０における状態ベクトルｘ_０及び誤差共分散行列Σ_ｘ，０ ^２を設定（初期化）する（Ｓ２）。具体的には、処理部２００は、前出の式（３０）のように表される、状態ベクトルｘ_０の各要素ｑ_０，ｖ_０，ｂ_ω，０，ｂ_α，０，Δｇ_０及び誤差共分散行列Σ_ｘ，０ ^２に含まれるΣ_ｑ，０ ^２，Σ_ｖ，０ ^２，Σ_ｂω，０ ^２，Σ_ｂα，０ ^２，σ_Δｇ，０ ^２を設定する。

処理部２０は、例えば、ＩＭＵ１０の初期姿勢があらかじめ決められたロール角、ピッチ角、ヨー角となるようにしておいて当該ロール角、ピッチ角、ヨー角を前出の式（３１）に代入してｑ_０を設定してもよい。あるいは、処理部２０は、ＩＭＵ１０が静止した状態で加速度センサー１４から加速度データを取得し、当該加速度データから重力加速度の方向を特定してロール角、ピッチ角を計算し、ヨー角を所定値（例えば０）として、当該ロール角、ピッチ角、ヨー角を前出の式（３１）に代入してｑ_０を設定してもよい。また、処理部２０は、ロール角誤差のＲＭＳσ_φとピッチ角誤差のＲＭＳσ_θを式（３２）に代入してΣ_ｑ，０ ^２を設定する。

また、処理部２０は、例えば、ＩＭＵ１０が静止した状態を初期状態としてｖ_０を０に設定し、速度ベクトルｖの各軸の誤差ＲＭＳσ_ｖｘ，σ_ｖｙ、σ_ｖｚを式（３３）に代入してΣ_ｖ，０ ^２を設定する。

また、処理部２０は、角速度センサー１２の残存バイアスｂ_ω及び加速度センサー１４の残存バイアスｂ_αがわかっていればこれらの値をｂ_ω，０，ｂ_α，０に設定し、ｂ_ω，ｂ_αが不明であれば、ｂ_ω，０，ｂ_α，０にゼロを設定する。また、処理部２０は、角速度センサー１２の各軸の残存バイアス誤差ＲＭＳσ_ｂωｘ，σ_ｂωｙ，σ_ｂωｚ及び加速度ンサー１１０の各軸の残存バイアス誤差ＲＭＳσ_ｂαｘ，σ_ｂαｙ，σ_ｂαｚを式（３４）に代入してΣ_ｂω，０ ^２，Σ_ｂα，０ ^２を設定する。

また、処理部２０は、重力加速度値がわかっていれば１Ｇとの差をΔｇ_０に設定し、重力加速度値が不明であれば、Δｇ_０にゼロを設定する。また、処理部２０は、σ_Δｇ，０ ^２に重力加速度値の誤差ＲＭＳσ_Δｇを設定する。

次に、処理部２０は、サンプリング周期Δｔが経過するまで待機し（Ｓ３のＮ）、サンプリング周期Δｔが経過すると（Ｓ３のＹ）、ｋ＝ｋ＋１に設定した後（Ｓ４）、ｔ_ｋ＝ｔ_ｋ−１＋Δｔに設定する（Ｓ５）。

次に、処理部２０は、ＩＭＵ１０から角速度データｄ_ω，ｋ及び加速度データｄ_α，ｋを取得する（Ｓ６）。

次に、処理部２０（バイアス除去部２２）は、時刻ｔ_ｋにおける角速度データｄ_ω，ｋ及び加速度データｄ_α，ｋから、それぞれ、時刻ｔ_ｋ−１で推定されたバイアスb_{ω，ｋ−１}及びb_{α，ｋ−１}を除去する（Ｓ７）。

次に、処理部２０（姿勢変化量計算部２４）は、時刻ｔ_ｋにおける姿勢変化量Δｑ_ｋを計算する（Ｓ８）。

次に、処理部２０（速度変化量計算部２６）は、時刻ｔ_ｋにおける速度変化量（Ｃ_ｋλ_ｋ−ｇ_ｋ−１）を計算する（Ｓ９）。

次に、処理部２０（姿勢推定部２８）は、時刻ｔ_ｋにおける状態ベクトルｘ_ｋ及び誤差共分散行列Σ_ｘ，ｋ ^２を推定する（Ｓ１０）。

そして、処理部２０は、姿勢推定処理の終了イベントが発生するまで（Ｓ１１のＮ）、ステップＳ３〜Ｓ１０の処理を繰り返し、終了イベントが発生すれば（Ｓ１１のＹ）、姿勢推定処理を終了する。

図５は、図３に示した姿勢推定部２８による時刻ｔ_ｋにおける状態ベクトルｘ_ｋ及び誤差共分散行列Σ_ｘ，ｋ ^２を推定する処理（図４のステップＳ１０の処理）の手順の一例を示すフローチャート図である。

図５に示すように、まず、姿勢推定部２８（積分計算部１０１）は、時刻ｔ_ｋ−１における姿勢クォータニオンｑ_ｋ−１と時刻ｔ_ｋにおける姿勢変化量Δｑ_ｋとをクォータニオン乗算する（Ｓ１００）。

次に、姿勢推定部２８（積分計算部１０１）は、時刻ｔ_ｋ−１における速度ベクトルｖ_ｋ−１と時刻ｔ_ｋにおける速度変化量（Ｃ_ｋλ_ｋ−ｇ_ｋ−１）とを加算する（Ｓ１００）。

次に、姿勢推定部２８（姿勢予測部１０２）は、時刻ｔ_ｋにおける状態ベクトルｘ_ｋを予測する（Ｓ１０２）。

次に、姿勢推定部２８（姿勢誤差更新部１０３）は、時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を更新する（Ｓ１０３）。

次に、姿勢推定部２８（姿勢誤差更新部１０３）は、時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を更新する（Ｓ１０３）。

次に、姿勢推定部２８（補正係数計算部１０４）は、時刻ｔ_ｋにおけるカルマン係数Ｋ_ｋを計算する（Ｓ１０４）。

次に、姿勢推定部２８（姿勢補正部１０５）は、時刻ｔ_ｋにおける状態ベクトルｘ_ｋを補正する（Ｓ１０５）。

次に、姿勢推定部２８（正規化部１０６）は、時刻ｔ_ｋにおける状態ベクトルｘ_ｋを正規化する（Ｓ１０６）。

次に、姿勢推定部２８（姿勢誤差補正部１０７）は、時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を補正する（Ｓ１０７）。

最後に、姿勢推定部２８（方位角誤差除去部１０８）は、正規化された姿勢クォータニオンｑ_ｋの誤差共分散行列Σ_ｑ，ｋ ^２のランク制限と方位角誤差成分ε_θｚが除去された、時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を生成する（Ｓ１０８）。

以上に説明したように、本実施形態の姿勢推定装置１によれば、ＩＭＵの出力モデルの式（１）及び式（２）から導かれる式（６）及び式（１２）を用いて対象物の姿勢変化量及び速度変化量を計算し、この姿勢変化量と速度変化量とを用いて対象物の姿勢を推定する。式（６）及び式（１２）では、Δｔの１次項のみならず２次項及び３次項も用いて姿勢変化量及び速度変化量を計算することで、従来よりも姿勢変化量や速度変化量の計算誤差が小さくなる。

また、対象物が回転すると座標変換行列Ｃ_ｋが変化するが、座標変換行列Ｃ_ｋはカルマンフィルターによって推定された姿勢クォータニオンｑ_ｋの要素から計算されるため、対象物が急激に回転した場合、座標変換行列Ｃ_ｋにすぐに追従しない場合がある。本実施形態の姿勢推定装置１によれば、式（１２）において、加速度だけでなく角速度も用いて加速度λ_ｋを計算することで、対象物の回転がすぐに加速度λ_ｋに反映され、対象物が急激に回転した場合でも、速度変化量の計算精度の劣化を低減させることができる。

さらに、本実施形態の姿勢推定装置１によれば、式（２３）より、加速度センサーの出力による重力加速度の観測残差と対象物の運動速度は長期的にはゼロであるとしてゼロ運動速度の観測残差を用いて、式（２４）のカルマン係数Ｋ_ｋを計算するので、方位角の観測情報が無くても、対象物の姿勢を精度よく推定することができる。

また、角速度センサーの出力と加速度センサーの出力には象物の方位角についての情報が含まれないため対象物の方位角誤差については補正されないが、更新された姿勢誤差に含まれる方位角誤差を残しておくと、方位角誤差の確からしさが単調に減少し、姿勢誤差が単調に増加するため、姿勢の推定精度が劣化するおそれがある。本実施形態の姿勢推定装置１によれば、式（２９）により、角速度センサー１２の出力と加速度センサー１４の出力とを用いて更新した対象物の姿勢誤差に含まれる方位角誤差を除去するので、姿勢誤差が単調に増加することを防止し、姿勢の推定精度を劣化させないようにすることができる。

以上より、本実施形態の姿勢推定装置１によれば、角速度センサー１２の出力と加速度センサー１４の出力のみを用いて、対象物の姿勢を十分な精度で推定することができる。

３．姿勢推定システム
図６は、本実施形態の姿勢推定システムの構成例を示す図である。図６に示すように、本実施形態の姿勢推定システム２は、慣性計測装置（ＩＭＵ）３と姿勢推定装置４を含んで構成されている。

本実施形態では、ＩＭＵ３は、角速度センサー２１２、加速度センサー２１４、信号処理部２１６及び通信部２１８を含んで構成されており、対象物に装着される。ただし、本実施形態のＩＭＵ３は、これらの一部の要素を変更又は削除し、あるいは、他の要素を追加した構成であってもよい。

角速度センサー２１２は、互いに交差する（理想的には直交する）３軸方向の各々の角速度を計測し、計測した３軸角速度の大きさ及び向きに応じたアナログ信号を出力する。

加速度センサー２１４は、互いに交差する（理想的には直交する）３軸方向の各々の加速度を計測し、計測した３軸加速度の大きさ及び向きに応じたアナログ信号を出力する。

信号処理部２１６は、角速度センサー１２と加速度センサー１４から、それぞれ３角速度データと３加速度データを受け取ってＡ／Ｄ変換する処理を行う。

角速度センサー２１２及び加速度センサー２１４は、それぞれ３軸が、ＩＭＵ３に対して定義される直交座標系（センサー座標系）の３軸（ｘ軸、ｙ軸、ｚ軸）と一致するようにＩＭＵ３に取り付けられるのが理想的だが、実際には取り付け角の誤差が生じる。そこで、信号処理部２１６は、取り付け角誤差に応じてあらかじめ算出された補正パラメーターを用いて、３軸角速度データ及び３軸加速度データをｘｙｚ座標系のデータに変換する処理も行う。また、信号処理部２１６は、角速度センサー２１２及び加速度センサー２１
４の温度特性に応じて、３軸角速度データ及び３軸加速度データを温度補正する処理も行う。

なお、角速度センサー２１２及び加速度センサー２１４にＡ／Ｄ変換や温度補正の機能が組み込まれていてもよい。

さらに、信号処理部２１６は、これらの処理後の３軸角速度データｄ_ω，ω及び３軸加速度データｄ_ω，αに時刻情報を付して通信用のフォーマットに合わせたパケットデータを生成し、通信部２１８に出力する。

通信部２１８は、信号処理部２１６から受け取ったパケットデータを姿勢推定装置４に送信する処理や、姿勢推定装置４から制御コマンドを受信して信号処理部２１６に送る処理等を行う。信号処理部２１６は、制御コマンドに応じた各種処理を行う。

姿勢推定装置４は、通信部３１０、処理部３２０、ＲＯＭ３３０、ＲＡＭ３４０、記録媒体３５０、操作部３６０、表示部３７０及び音出力部３８０を含んで構成されている。ただし、本実施形態の姿勢推定装置４は、これらの一部の要素を変更又は削除し、あるいは、他の要素を追加した構成であってもよい。姿勢推定装置４は、例えば、パーソナルコンピューター（ＰＣ）やスマートフォンなどの携帯機器であってもよい。

通信部３１０は、ＩＭＵ３から送信されたパケットデータを受信し、処理部３２０に送る処理や、処理部３２０からの制御コマンドをＩＭＵ３に送信する処理等を行う。

操作部３６０は、ユーザーからの操作データを取得し、処理部３２０に送る処理を行う。操作部３６０は、例えば、タッチパネル型ディスプレイ、ボタン、キー、マイクなどであってもよい。

表示部３７０は、処理部３２０の処理結果を文字、グラフ、表、アニメーション、その他の画像として表示するものである。表示部３７０は、例えば、ＣＲＴ、ＬＣＤ、タッチパネル型ディスプレイ、ＨＭＤ（ヘッドマウントディスプレイ）などであってもよい。なお、１つのタッチパネル型ディスプレイで操作部３６０と表示部３７０の機能を実現するようにしてもよい。

音出力部３８０は、処理部３２０の処理結果を音声やブザー音等の音として表示するものである。音出力部３８０は、例えば、スピーカーやブザーなどであってもよい。

ＲＯＭ３３０は、処理部３２０が各種の処理を行うためのプログラムや、アプリケーション機能を実現するための各種プログラムやデータ等を記憶している。

ＲＡＭ３４０は、処理部３２０の作業領域として用いられ、ＲＯＭ３３０から読み出されたプログラムやデータ、処理部３２０が各種プログラムに従って実行した演算結果等を一時的に記憶する記憶部である。

記録媒体３５０は、処理部３２０の処理により生成されたデータのうち、長期的な保存が必要なデータを記憶する不揮発性の記憶部である。また、記録媒体３５０は、処理部３２０が各種の処理を行うためのプログラムや、アプリケーション機能を実現するための各種プログラムやデータ等を記憶していてもよい。

処理部３２０は、ＲＯＭ３３０あるいは記録媒体３５０に記憶されているプログラム、あるいはネットワークを介してサーバーから受信してＲＡＭ３４０や記録媒体３５０に記
憶したプログラムに従って各種の処理を行う。特に、本実施形態では、処理部３２０は、当該プログラムを実行することにより、データ取得部３２１、バイアス除去部３２２、姿勢変化量計算部３２４、速度変化量計算部３２６及び姿勢推定部３２８として機能する。そして、処理部３２０は、ＩＭＵ３がΔｔ間隔で計測したセンサー座標系（ｘｙｚ座標系）の３軸角速度データｄ_ω，ｋ及び３軸加速度データｄ_ω，αに対して所定の演算を行い、ＩＭＵ１０が装着された対象物のローカル空間座標系（ＸＹＺ座標系）における姿勢（ＩＭＵ３の姿勢ともいえる）を推定する。

データ取得部３２１は、通信部３１０がＩＭＵ３から受信したパケットデータを受け取り、受け取ったパケットデータから時刻情報及び計測データ（３軸角速度データｄ_ω，ｋ及び３軸加速度データｄ_ω，α）を取得し、バイアス除去部３２２に送る処理を行う。

バイアス除去部３２２、姿勢変化量計算部３２４及び速度変化量計算部３２６の各処理は、それぞれ、図１のバイアス除去部２２、姿勢変化量計算部２４及び速度変化量計算部２６の各処理と同じであるため、その説明を省略する。

姿勢推定部３２８は、積分計算部４０１、姿勢予測部４０２、姿勢誤差更新部４０３、補正係数計算部４０４、姿勢補正部４０５、正規化部４０６、姿勢誤差補正部４０７及び方位角誤差除去部４０８として機能し、姿勢変化量計算部３２４が計算した姿勢変化量と速度変化量計算部３２６が計算した速度変化量とを用いて、対象物の姿勢を推定する処理を行う。積分計算部４０１、姿勢予測部４０２、姿勢誤差更新部４０３、補正係数計算部４０４、姿勢補正部４０５、正規化部４０６、姿勢誤差補正部４０７及び方位角誤差除去部４０８の各処理は、それぞれ、図１の積分計算部１０１、姿勢予測部１０２、姿勢誤差更新部１０３、補正係数計算部１０４、姿勢補正部１０５、正規化部１０６、姿勢誤差補正部１０７及び方位角誤差除去部１０８の各処理と同じであるため、その説明を省略する。

処理部２０は、推定した対象物の姿勢に応じた画像データや音データを生成し、表示部３７０や音出力部３８０に出力する。

姿勢推定装置４の処理部３２０の具体的な構成は図３と同様であってもよいため、その図示及び説明を省略する。また、処理部３２０による姿勢推定処理の手順は図４のフローチャート図と同様であってもよいため、その図示及び説明を省略する。また、姿勢推定部３２８による時刻ｔ_ｋにおける状態ベクトルｘ_ｋ及び誤差共分散行列Σ_ｘ，ｋ ^２を推定する処理（図４のステップＳ１０の処理）の手順も図５のフローチャート図と同様であってもよいため、その図示及び説明を省略する。

本実施形態の姿勢推定システム２によれば、姿勢推定装置４により、角速度センサー２１２の出力と加速度センサー２１４の出力のみを用いて、対象物の姿勢を十分な精度で推定することができる。

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

例えば、上述した実施形態では、角速度センサー１２と加速度センサー１４は１つのＩＭＵ１０に収容され、一体化されているが、角速度センサー１２と加速度センサー１４は別体であってもよい。同様に、角速度センサー２１２と加速度センサー２１４は１つのＩＭＵ３に収容され、一体化されているが、角速度センサー２１２と加速度センサー２１４は別体であってもよい。

また、例えば、上述した実施形態では、姿勢推定装置１あるいは姿勢推定装置４は、対象物の姿勢情報のみを出力しているが、さらに他の情報を出力してもよい。例えば、姿勢推定装置１あるいは姿勢推定装置４は、時刻ｔ_ｋにおける速度ベクトルｖ_ｋに基づく対象物の速度情報や速度ベクトルｖ_ｋを積分して得られる対象物の位置情報を出力してもよい。

上述した各実施形態および各変形例は一例であって、これらに限定されるわけではない。例えば、各実施形態および各変形例を適宜組み合わせることも可能である。

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

１ 姿勢推定装置、２ 姿勢推定システム、３ 慣性計測装置（ＩＭＵ）、４ 姿勢推定装置、１０ 慣性計測ユニット（ＩＭＵ）、１２ 角速度センサー、１４ 加速度センサー、１６ 信号処理部、２０ 処理部、２２ バイアス除去部、２４ 姿勢変化量計算部、２６ 速度変化量計算部、２８ 姿勢推定部、３０ ＲＯＭ、４０ ＲＡＭ、５０ 記録媒体、６０ 通信部、１０１ 積分計算部、１０２ 姿勢予測部、１０３ 姿勢誤差更新部、１０４ 補正係数計算部、１０５ 姿勢補正部、１０６ 正規化部、１０７ 姿勢誤差補正部、１０８ 方位角誤差除去部、２１２ 角速度センサー、２１４ 加速度センサー、２１６ 信号処理部、２１８ 通信部、３１０ 通信部、３２０ 処理部、３２１
データ取得部、３２２ バイアス除去部、３２４ 姿勢変化量計算部、３２６ 速度変化量計算部、３２８ 姿勢推定部、３３０ ＲＯＭ、３４０ ＲＡＭ、３５０ 記録媒体、３６０ 操作部、３７０ 表示部、３８０ 音出力部、４０１ 積分計算部、４０２ 姿勢予測部、４０３ 姿勢誤差更新部、４０４ 補正係数計算部、４０５ 姿勢補正部、４０６ 正規化部、４０７ 姿勢誤差補正部、４０８ 方位角誤差除去部

Claims (17)

1. 角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する工程と、
   加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する工程と、
   前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する工程と、を含む、姿勢推定方法。
2. 前記対象物の速度変化量を計算する工程において、
   前記角速度センサーの出力と前記加速度センサーの出力とを用いて前記サンプリング周期を変数とする多項式で近似して前記対象物の加速度を計算し、当該加速度を用いて前記速度変化量を計算する、請求項１に記載の姿勢推定方法。
3. 前記対象物の姿勢を推定する工程は、
   前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢と速度とを予測する工程と、
   前記予測した前記対象物の速度とゼロとの差を用いて、前記予測した前記対象物の姿勢を補正する工程と、を含む、請求項１又は２に記載の姿勢推定方法。
4. 前記対象物の姿勢を推定する工程は、
   前記角速度センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、
   前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、を含み、
   前記対象物の姿勢を補正する工程において、
   前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する、請求項３に記載の姿勢推定方法。
5. 前記対象物の姿勢を推定する工程は、
   前記角速度センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程を含み、
   前記対象物の姿勢を補正する工程において、
   前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する、請求項３に記載の姿勢推定方法。
6. 慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、
   前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、
   前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、
   前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、を含む、姿勢推定方法。
7. 慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、
   前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する工程と、
   前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、を含む、姿勢推定方法。
8. 前記慣性センサーの出力を用いて、前記対象物の速度変化量を計算する工程を含み、
   前記対象物の姿勢を予測する工程において、
   前記速度変化量を用いて、前記対象物の速度を予測し、
   前記対象物の姿勢を補正する工程において、
   前記予測した前記対象物の速度とゼロとの差を用いて、前記予測した前記対象物の姿勢を補正する、請求項６又は７に記載の姿勢推定方法。
9. 前記慣性センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して前記対象物の姿勢変化量を計算する工程を含み、
   前記対象物の姿勢を予測する工程において、
   前記姿勢変化量を用いて、前記対象物の姿勢を予測する、請求項６乃至８のいずれか一項に記載の姿勢推定方法。
10. 角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する姿勢変化量計算部と、
    加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する速度変化量計算部と、
    前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する姿勢推定部と、を含む、姿勢推定装置。
11. 前記角速度センサー及び前記加速度センサーを含む、請求項１０に記載の姿勢推定装置。
12. 慣性センサーの出力を用いて、対象物の姿勢を予測する姿勢予測部と、
    前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する姿勢誤差更新部と、
    前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する姿勢補正部と、
    前記更新された前記姿勢誤差に含まれる方位角誤差を除去する方位角誤差除去部と、を含む、姿勢推定装置。
13. 慣性センサーの出力を用いて、対象物の姿勢を予測する姿勢予測部と、
    前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まないものとして前記対象物の姿勢誤差を更新する姿勢誤差更新部と、
    前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する姿勢補正部と、を含む、姿勢推定装置。
14. 前記慣性センサーを含む、請求項１２又は１３に記載の姿勢推定装置。
15. 角速度センサーの出力を用いて、サンプリング周期を変数とする多項式で近似して対象物の姿勢変化量を計算する工程と、
    加速度センサーの出力と前記角速度センサーの出力とを用いて、前記対象物の速度変化量を計算する工程と、
    前記姿勢変化量と前記速度変化量とを用いて、前記対象物の姿勢を推定する工程と、をコンピューターに実行させる、プログラム。
16. 慣性センサーの出力を用いて、対象物の姿勢を予測する工程と、
    前記慣性センサーの出力を用いて、前記対象物の姿勢誤差を更新する工程と、
    前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、
    前記更新された前記姿勢誤差に含まれる方位角誤差を除去する工程と、をコンピューターに実行させる、プログラム。
17. 前記慣性センサーの出力を用いて、前記対象物の回転誤差が方位角誤差成分を含まない
    ものとして前記対象物の姿勢誤差を更新する工程と、
    前記更新された前記姿勢誤差を用いて、前記予測した前記対象物の姿勢を補正する工程と、をコンピューターに実行させる、プログラム。

Images