JP2020020631A - 姿勢推定方法、姿勢推定装置及び移動体 - Google Patents

Abstract

【課題】角速度センサーの出力が一時的に有効範囲内でない状況が発生した場合でも対象物の姿勢の推定精度が低下するおそれを低減することが可能な姿勢推定方法を提供すること。【解決手段】角速度センサーの出力に基づいて、対象物の姿勢変化量を計算する姿勢変化量計算工程と、前記姿勢変化量を用いて、前記対象物の姿勢情報を予測する姿勢情報予測工程と、前記角速度センサーの出力が有効範囲内であるか否かを判定し、前記角速度センサーの出力が有効範囲内ではないと判定した場合に、誤差情報における姿勢誤差成分を増加させ、かつ、前記誤差情報における前記姿勢誤差成分と前記姿勢誤差成分以外の誤差成分との相関成分を低減させる誤差情報調整工程と、前記誤差情報に基づいて、予測した前記対象物の姿勢情報を補正する姿勢情報補正工程と、を含む、姿勢推定方法。【選択図】図１

Description

本発明は、姿勢推定方法、姿勢推定装置及び移動体に関する。

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

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

しかしながら、角速度センサーには有効計測範囲が定められており、この範囲を超える角速度が一時的に入力された場合、角速度センサーの出力に想定されない大きな誤差が含まれることになる。これに対して、特許文献１に記載の姿勢推定方法では、一時的にでも角速度センサーの出力が有効範囲を超える場合が想定されておらず、対象物の姿勢の推定精度が低下するおそれがあった。

本発明に係る姿勢推定方法の一態様は、
角速度センサーの出力に基づいて、対象物の姿勢変化量を計算する姿勢変化量計算工程と、
前記姿勢変化量を用いて、前記対象物の姿勢情報を予測する姿勢情報予測工程と、
前記角速度センサーの出力が有効範囲内であるか否かを判定し、前記角速度センサーの出力が有効範囲内ではないと判定した場合に、誤差情報における姿勢誤差成分を増加させ、かつ、前記誤差情報における前記姿勢誤差成分と前記姿勢誤差成分以外の誤差成分との相関成分を低減させる誤差情報調整工程と、
前記誤差情報に基づいて、予測した前記対象物の姿勢情報を補正する姿勢情報補正工程と、
を含む。

前記姿勢推定方法の一態様において、
前記誤差情報調整工程は、
前記角速度センサーの出力が有効範囲内ではないと判定した後の第１期間中であるか否かを判定する工程と、
前記第１期間において、前記姿勢誤差成分を増加させる工程と、
前記第１期間において、前記姿勢誤差成分と前記姿勢誤差成分以外の誤差成分との相関成分を低減させる工程と、
を含んでもよい。

前記姿勢推定方法の一態様において、
前記姿勢誤差成分以外の誤差成分は、角速度のバイアス誤差成分を含んでもよい。

前記姿勢推定方法の一態様において、
前記姿勢誤差成分と前記姿勢誤差成分以外の誤差成分との相関成分はゼロであってもよい。

前記姿勢推定方法の一態様は、
加速度センサーの出力と前記角速度センサーの出力とに基づいて、前記対象物の速度変化量を計算する速度変化量計算工程を含み、
前記誤差情報調整工程では、
前記加速度センサーの出力が有効範囲内であるか否かを判定し、前記角速度センサーの出力又は前記加速度センサーの出力が有効範囲内ではないと判定した場合に、前記誤差情報における運動速度誤差成分を増加させ、かつ、前記誤差情報における前記運動速度誤差成分と前記運動速度誤差成分以外の誤差成分との相関成分を低減させ、
前記姿勢情報予測工程では、
前記速度変化量を用いて、前記対象物の速度情報を予測してもよい。

前記姿勢推定方法の一態様において、
前記誤差情報調整工程は、
前記加速度センサーの出力が有効範囲内ではないと判定した後の第２期間中であるか否かを判定する工程と、
前記第２期間において、前記運動速度誤差成分を増加させる工程と、
前記第２期間において、前記運動速度誤差成分と前記運動速度誤差成分以外の誤差成分との相関成分を低減させる工程と、
を含んでもよい。

前記姿勢推定方法の一態様において、
前記運動速度誤差成分以外の誤差成分は、加速度のバイアス誤差成分を含んでもよい。

前記姿勢推定方法の一態様において、
前記運動速度誤差成分と前記運動速度誤差成分以外の誤差成分との相関成分はゼロである、姿勢推定方法

本発明に係る姿勢推定装置の一態様は、
角速度センサーの出力に基づいて、対象物の姿勢変化量を計算する姿勢変化量計算部と、
前記姿勢変化量を用いて、前記対象物の姿勢情報を予測する姿勢情報予測部と、
前記角速度センサーの出力が有効範囲内であるか否かを判定し、前記角速度センサーの出力が有効範囲内ではないと判定した場合に、誤差情報における姿勢誤差成分を増加させ、かつ、前記誤差情報における前記姿勢誤差成分と前記姿勢誤差成分以外の誤差成分との相関成分を低減させる誤差情報調整部と、
前記誤差情報に基づいて、予測した前記対象物の姿勢情報を補正する姿勢情報補正部と、
を含む。

本発明に係る移動体の一態様は、
前記姿勢推定装置の一態様と、
前記姿勢推定装置が推定した移動体の姿勢情報に基づいて、前記移動体の姿勢の制御を行う制御装置と、
を含む。

姿勢推定方法の手順の一例を示すフローチャート図。 図１の工程Ｓ３の手順の一例を示すフローチャート図。 図１の工程Ｓ５の手順の一例を示すフローチャート図。 図１の工程Ｓ６の手順の一例を示すフローチャート図。 図１の工程Ｓ７の手順の一例を示すフローチャート図。 本実施形態の姿勢推定装置の構成例を示す図。 センサー座標系とローカル座標系について説明するための図。 本実施形態における処理部の構成例を示す図。 図１の工程Ｓ５の他の手順の一例を示すフローチャート図。 本実施形態の電子機器の構成例を示すブロック図。 携帯型電子機器である腕時計型の活動計の平面図。 携帯型電子機器である腕時計型の活動計の構成例を示すブロック図。 本実施形態の移動体の一例。 移動体の構成例を示すブロック図。 本実施形態の他の移動体の一例。 移動体の構成例を示すブロック図。

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

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

加速度データｄ_α，ｋも同様に、式（２）のように、加速度ベクトルαの平均値と残存バイアスｂ_αとの和で表される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

１−１−１０．角速度センサーのバイアス誤差のヨー軸成分制限
磁気センサーなどの方位角観測手段を持たない場合、角速度センサーのバイアス誤差についてもそのヨー軸成分（鉛直成分）のみが単調増加してしまう。通常、ヨー軸方向は姿勢とともにセンサー座標に対して変化するので、例えばある時点で角速度センサーのｚ軸がヨー軸と一致して誤差見積もりが増加しても、姿勢が変わってｚ軸がヨー軸以外の傾き（例えば、水平軸方向）になった時に重力加速度観測によって補正され、誤差見積もりは減少する。しかし、姿勢変化が少なくほぼ同じ姿勢を長時間継続するような場合、このヨー軸成分の誤差見積もりが増大してフィードバックゲインを上げることとなり、角速度センサーのバイアス推定値の意図しない変化となって姿勢方位角のドリフトを引き起こす原因となる。一方で、たとえヨー軸成分であったとしても実際の角速度センサーのバイアス推定誤差が、例えば初期バイアス誤差を超えて際限なく大きくなるようなことは考えにくい。そこで、状態誤差共分散行列の角速度センサーのバイアス推定誤差のヨー軸成分のみに上限値を設けてそれを超えないよう制限する。

まずは、式（３９）のように角速度センサーの残存バイアスの誤差共分散行列Σ_ｂω ^２から、現在の姿勢クォータニオンに基づいてヨー軸成分の分散σ_ｂωｖ ^２を算出する。

ヨー軸成分の分散σ_ｂωｖ ^２が上限値σ_{ｂωｍａｘ} ^２を超えている場合は、以下の式（４０）のように制限する。

１−１−１１．センサーオフスケール対策
現実の慣性センサー（角速度センサー及び加速度センサー）には有効な計測範囲が定められており、この範囲を超える物理量（角速度や加速度）が入力された場合、慣性センサーの内部のおける信号の飽和によって正確な出力を得ることはできない。以下、有効な計測範囲を超えることを「オフスケール」という。通常はそうならないように計測すべき範囲に合わせて使用するセンサーを選択するが、用途によっては衝撃等のためにごく短時間ではあるが範囲を超える状態（オフスケール状態）が想定される。

［センサーオフスケールの影響］
角速度センサーがオフスケール状態になると角速度に大きな誤差が含まれることになり、角速度を積分した姿勢角度の誤差が増大する。また、誤差が増大した姿勢角度に基づく加速度の積分によって運動速度の誤差が増大する。また、加速度センサーがオフスケール状態になると加速度に大きな誤差が含まれることになり、加速度を積分した運動速度の誤差が増大する。一方で、オフスケールによる誤差は確率過程ではないので平均や分散を求められない。誤差量が文字通り桁違いに甚大かつ予測不可能なためにカルマンフィルターのモデル化が難しいため、現状のカルマンフィルターではこれらを補正すべき誤差とは認識していない。そこで、センサーの出力値がその有効範囲を逸脱した場合にオフスケールとして認識し、その時点以後の適切な時間をオフスケールからの回復期間と定めて、通常とは異なる処理を行う。

［オフスケール回復処理］
オフスケールの影響で増大した状態誤差に対応するよう状態誤差共分散行列に以下の処理を加える。すなわち、誤差の増大に対応して共分散行列の該当誤差分散成分を適切に水増しする。また、増大した誤差を他の状態変数誤差から隔離するために共分散成分をゼロにする。これらを補正処理の前に行うことで誤差の増大した状態変数に対する補正が強めにかかるとともに、バイアス推定値等の他の状態変数への悪影響を最小限に抑えることができる。

＜角速度オフスケール回復処理＞
角速度センサーのオフスケール状態によって増大する姿勢角度と運動速度の誤差に対応するよう以下の式（４１）の処理を加える。すなわち、姿勢角度誤差の増大に対応して、誤差共分散行列Σ_ｘ ^２における、姿勢クォータニオンｑの誤差成分（姿勢誤差成分）を適切に増加させる。また、誤差共分散行列Σ_ｘ ^２において、姿勢誤差成分を他の誤差成分から隔離するために、姿勢誤差成分と姿勢誤差成分以外の誤差成分との相関成分（共分散成分）をゼロにする。姿勢誤差成分以外の誤差成分は、運動速度誤差成分、角速度センサーの残存バイアスｂ_ωの誤差成分（角速度のバイアス誤差成分）、加速度センサーの残存バイアスｂ_αの誤差成分（加速度のバイアス誤差成分）、重力加速度補正値Δｇの誤差成分である。なお、相関成分（共分散成分）をゼロにするとは、ゼロに近い値や、所定の誤差成分を他の誤差成分から隔離することができる値を含むことができる。

また、運動速度誤差の増大に対応して、誤差共分散行列Σ_ｘ ^２における、運動速度ベクトルｖの誤差成分（運動速度誤差成分）を適切に増加させる。また、誤差共分散行列Σ_ｘ ^２において、運動速度誤差成分を他の誤差成分から隔離するために、運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分（共分散成分）をゼロにする。運動速度誤差成分以外の誤差成分は、姿勢誤差成分、角速度のバイアス誤差成分、加速度のバイアス誤差成分、重力加速度補正値の誤差成分である。なお、式（４１）において、σ_оｑ ^２はオフスケール回復時の姿勢誤差の分散であり、σ_оｖ ^２はオフスケール回復時の運動速度誤差の分散である。

＜加速度オフスケール回復処理＞
加速度センサーのオフスケール状態によって増大する運動速度の誤差に対応するよう以下の式（４２）の処理を加える。すなわち、運動速度誤差の増大に対応して誤差共分散行列Σ_ｘ ^２の運動速度誤差成分を適切に増加させる。また、誤差共分散行列Σ_ｘ ^２において、運動速度誤差成分を他の状態変数誤差から隔離するために、運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分（共分散成分）をゼロにする。

１−２．姿勢推定方法のフローチャート
図１は、本実施形態の姿勢推定方法の手順の一例を示すフローチャート図である。なお、図１の手順は、例えば、ＩＭＵが取り付けられた対象物の姿勢を推定する装置である姿
勢推定装置によって実行される。対象物は特に限定されず、例えば、移動体、電子機器、運動器具、人、動物等であってもよい。ＩＭＵは、対象物に対して脱着可能であってもよいし、例えば、対象物に内蔵されるなど、対象物に固定されていて取り外すことができないものでもよい。姿勢推定装置は、例えば、パーソナルコンピューター（ＰＣ）であってもよいし、スマートフォンなどの各種の携帯機器であってもよい。

図１に示すように、本実施形態の姿勢推定方法では、まず、姿勢推定装置は、状態ベクトルｘ_ｋと誤差情報である誤差共分散行列Σ_ｘ，ｋ ^２の初期化を行う（初期化工程Ｓ１）。すなわち、姿勢推定装置は、ｋ＝０に設定し、時刻ｔ_０における状態ベクトルｘ_０及び誤差共分散行列Σ_ｘ，０ ^２を設定（初期化）する。具体的には、姿勢推定装置は、前出の式（３０）のように表される、状態ベクトルｘ_０の各要素ｑ_０，ｖ_０，ｂ_ω，０，ｂ_α，０，Δｇ_０及び誤差共分散行列Σ_ｘ，０ ^２に含まれるΣ_ｑ，０ ^２，Σ_ｖ，０ ^２，Σ_ｂω，０ ^２，Σ_ｂα，０ ^２，σ_Δｇ，０ ^２を設定する。

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

また、姿勢推定装置は、例えば、慣性計測装置（ＩＭＵ）が静止した状態を初期状態としてｖ_０を０に設定し、運動速度ベクトルｖの各軸の誤差のＲＭＳσ_ｖｘ，σ_ｖｙ、σ_ｖｚを式（３３）に代入してΣ_ｖ，０ ^２を設定する。

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

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

次に、姿勢推定装置は、慣性計測装置（ＩＭＵ）の計測値を取得する（計測値取得工程Ｓ２）。具体的には、姿勢推定装置は、サンプリング間隔Δｔが経過するまで待機し、サンプリング間隔Δｔが経過すると、ｋ＝ｋ＋１、ｔ_ｋ＝ｔ_ｋ−１＋Δｔに設定し、慣性計測装置（ＩＭＵ）から角速度データｄ_ω，ｋ及び加速度データｄ_α，ｋを取得する。

次に、姿勢推定装置は、時刻ｔ_ｋにおける姿勢情報である姿勢クォータニオンｑ_ｋを要素として含む状態ベクトルｘ_ｋと時刻ｔ_ｋにおける誤差情報である誤差共分散行列Σ_ｘ，ｋ ^２の予測処理（時間更新処理ともいう）を行う（予測工程Ｓ３）。

図２は、図１の工程Ｓ３の手順の一例を示すフローチャート図である。図２に示すように、姿勢推定装置は、まず、式（１９）により、時刻ｔ_ｋにおける角速度データｄ_ω，ｋ及び加速度データｄ_α，ｋから、それぞれ、時刻ｔ_ｋ−１で推定された残存バイアスb_{ω
，ｋ−１}及びb_{α，ｋ−１}を除去する処理を行う（バイアス除去工程Ｓ３１）。

次に、姿勢推定装置は、角速度センサーの出力に基づいて、時刻ｔ_ｋにおける姿勢変化量Δｑ_ｋを計算する処理（姿勢変化量計算処理）を行う（姿勢変化量計算工程Ｓ３２）。具体的には、姿勢推定装置は、角速度データｄ_ω，ｋに基づいて、式（６）により姿勢変化量Δｑ_ｋを計算する。

次に、姿勢推定装置は、加速度センサーの出力と前記角速度センサーの出力とに基づいて、対象物の速度変化量（Ｃ_ｋλ_ｋ−ｇ_ｋ−１）Δｔを計算する処理（速度変化量計算処理）を行う（速度変化量計算工程Ｓ３３）。具体的には、姿勢推定装置は、加速度データｄ_α，ｋ及び角速度データｄ_ω，ｋに基づいて、式（９）、式（１０）及び式（１２）により速度変化量（Ｃ_ｋλ_ｋ−ｇ_ｋ−１）Δｔを計算する。

次に、姿勢推定装置は、姿勢変化量Δｑ_ｋを用いて、時刻ｔ_ｋにおける対象物の姿勢情報である姿勢クォータニオンｑ_ｋを予測する処理（姿勢情報予測処理）を行う（姿勢情報予測工程Ｓ３４）。また、姿勢情報予測工程Ｓ３４において、姿勢推定装置は、速度変化量（Ｃ_ｋλ_ｋ−ｇ_ｋ−１）Δｔを用いて、時刻ｔ_ｋにおける対象物の速度情報である運動速度ベクトルｖ_ｋを予測する処理（速度情報予測処理）をさらに行う。具体的には、姿勢情報予測工程Ｓ３４において、姿勢推定装置は、式（６）、式（１２）及び式（１９）により、姿勢クォータニオンｑ_ｋ及び運動速度ベクトルｖ_ｋを要素として含む状態ベクトルｘ_ｋを予測する処理を行う。

最後に、姿勢推定装置は、式（２０）及び式（２１）により、時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を更新する処理を行う（誤差情報更新工程Ｓ３５）。

図１に戻り、次に、姿勢推定装置は、誤差情報である誤差共分散行列Σ_ｘ，ｋ ^２における、基準ベクトルの周りの回転誤差成分を除去する処理（回転誤差成分除去処理）を行う（回転誤差成分除去工程Ｓ４）。基準ベクトルは、観測手段を用いて観測されたベクトルである。本実施形態では、基準ベクトルは、観測手段である加速度センサーを用いて観測された重力加速度ベクトルである。また、本実施形態では、基準ベクトルの周りの回転誤差は、方位角誤差である。したがって、工程Ｓ４において、姿勢推定装置は、誤差共分散行列Σ_ｘ，ｋ ^２における方位角誤差を除去する処理を行う。具体的には、姿勢推定装置は、式（２８）及び式（２９）により、姿勢クォータニオンｑ_ｋの誤差共分散行列Σ_ｑ，ｋ ^２のランク制限と方位角誤差成分ε_θｚの除去がされた誤差共分散行列Σ_ｘ，ｋ ^２を生成する。

次に、姿勢推定装置は、オフスケール回復処理を行う（誤差情報調整工程Ｓ５）。具体的には、誤差情報調整工程Ｓ５において、姿勢推定装置は、角速度センサーの出力が有効範囲内であるか否かを判定し、角速度センサーの出力が有効範囲内ではないと判定した場合に、誤差情報である誤差共分散行列Σ_ｘ，ｋ ^２における姿勢誤差成分を増加させ、かつ、誤差共分散行列Σ_ｘ，ｋ ^２における姿勢誤差成分と姿勢誤差成分以外の誤差成分との相関成分を低減させる処理を行う（誤差情報調整工程Ｓ５）。また、誤差情報調整工程Ｓ５において、姿勢推定装置は、加速度センサーの出力が有効範囲内であるか否かを判定し、角速度センサーの出力又は加速度センサーの出力が有効範囲内ではないと判定した場合に、誤差情報である誤差共分散行列Σ_ｘ，ｋ ^２における運動速度誤差成分を増加させ、かつ、誤差共分散行列Σ_ｘ，ｋ ^２における運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分を低減させる処理を行う。

図３は、図１の工程Ｓ５の手順の一例を示すフローチャート図である。図３に示すように、姿勢推定装置は、まず、角速度センサーの出力（角速度データｄ_ω）が有効範囲内か
否かを判定する（工程Ｓ５１）。そして、姿勢推定装置は、角速度センサーの出力が有効範囲内であると判定した場合には（工程Ｓ５１のＹ）、角速度オフスケール回復期間の残りを確認する（工程Ｓ５２ａ）。また、姿勢推定装置は、角速度センサーの出力が有効範囲内でないと判定した場合には（工程Ｓ５１のＮ）、角速度オフスケール回復期間を開始又は延長する（工程Ｓ５２ｂ）。

次に、姿勢推定装置は、加速度センサーの出力（加速度データｄ_α）が有効範囲内か否かを判定する（工程Ｓ５３）。そして、姿勢推定装置は、加速度センサーの出力が有効範囲内であると判定した場合には（工程Ｓ５３のＹ）、加速度オフスケール回復期間の残りを確認する（工程Ｓ５４ａ）。また、姿勢推定装置は、加速度センサーの出力が有効範囲内でないと判定した場合には（工程Ｓ５３のＮ）、加速度オフスケール回復期間を開始又は延長する（工程Ｓ５４ｂ）。

次に、姿勢推定装置は、角速度センサーの出力が有効範囲内ではないと判定した後の第１期間である角速度オフスケール回復期間中であるか否かを判定する（工程Ｓ５５）。そして、姿勢推定装置は、角速度オフスケール回復期間中であると判定した場合には（工程Ｓ５５のＹ）、誤差情報である誤差共分散行列Σ_ｘ，ｋ ^２の姿勢誤差成分を増加させる（工程Ｓ５６ａ）。また、姿勢推定装置は、誤差共分散行列Σ_ｘ，ｋ ^２の姿勢誤差成分と姿勢誤差成分以外の誤差成分との相関成分を低減させ、例えばゼロにする（工程Ｓ５６ｂ）。また、姿勢推定装置は、誤差共分散行列Σ_ｘ，ｋ ^２の運動速度誤差成分を増加させる（工程Ｓ５８ａ）。また、姿勢推定装置は、誤差共分散行列Σ_ｘ，ｋ ^２の運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分を低減させ、例えばゼロにする（工程Ｓ５８ｂ）。実際には、姿勢推定装置は、式（４１）により誤差共分散行列Σ_ｘ，ｋ ^２を更新することで、工程Ｓ５６ａ、工程Ｓ５６ｂ、工程Ｓ５８ａ及び工程Ｓ５８ｂの処理を行う。

一方、姿勢推定装置は、角速度オフスケール回復期間中でないと判定した場合には（工程Ｓ５５のＮ）、加速度センサーの出力が有効範囲内ではないと判定した後の第２期間である加速度オフスケール回復期間中であるか否かを判定する（工程Ｓ５７）。そして、姿勢推定装置は、加速度オフスケール回復期間中であると判定した場合には（工程Ｓ５７のＹ）、姿勢推定装置は、誤差共分散行列Σ_ｘ，ｋ ^２の運動速度誤差成分を増加させる（工程Ｓ５８ａ）。また、姿勢推定装置は、誤差共分散行列Σ_ｘ，ｋ ^２の運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分を低減させ、例えばゼロにする（工程Ｓ５８ｂ）。実際には、姿勢推定装置は、式（４２）により誤差共分散行列Σ_ｘ，ｋ ^２を更新することで、工程Ｓ５８ａ及び工程Ｓ５８ｂの処理を行う。

なお、姿勢推定装置は、角速度オフスケール回復期間中でも加速度オフスケール回復期間中でもないと判定した場合には（工程Ｓ５５のＮかつ工程Ｓ５７のＮ）、工程Ｓ５６ａ、工程Ｓ５６ｂ、工程Ｓ５８ａ及び工程Ｓ５８ｂの処理を行わない。

図１に戻り、次に、姿勢推定装置は、誤差情報である誤差共分散行列Σ_ｘ，ｋ ^２における、基準ベクトルの周りの角速度のバイアス誤差成分に対して制限をかける処理（バイアス誤差制限処理）を行う（バイアス誤差制限工程Ｓ６）。前述の通り、本実施形態では、基準ベクトルは重力加速度ベクトルであるから、工程Ｓ６において、姿勢推定装置は、重力加速度ベクトルの周りの角速度のバイアス誤差成分、すなわち、角速度のバイアス誤差の鉛直成分（ヨー軸成分）に対して制限をかける。

図４は、図１の工程Ｓ６の手順の一例を示すフローチャート図である。図４に示すように、姿勢推定装置は、まず、時刻ｔ_ｋにおける状態ベクトルｘ_ｋ及び誤差共分散行列Σ_ｘ，ｋ ^２を用いて、式（３９）により、角速度のバイアス誤差の鉛直成分の分散σ_ｂωｖ ^２
を算出する処理を行う（バイアス誤差鉛直成分算出工程Ｓ６１）。

次に、姿勢推定装置は、分散σ_ｂωｖ ^２が上限値を超えているか否かを判定する処理を行う（バイアス誤差判定工程Ｓ６２）。

そして、姿勢推定装置は、分散σ_ｂωｖ ^２が上限値を超えている場合に（工程Ｓ６２のＹ）、角速度のバイアス誤差の鉛直成分に対して制限をかける制限演算処理を行う（制限演算工程Ｓ６３）。具体的には、姿勢推定装置は、分散σ_ｂωｖ ^２が上限値σ_{ｂωｍａｘ} ^２を超えている場合は、式（４０）により、誤差共分散行列Σ_ｘ，ｋ ^２を更新する。一方、分散σ_ｂωｖ ^２が上限値以下である場合は（工程Ｓ６２のＮ）、姿勢推定装置は、工程Ｓ６３の制限演算処理を行わない。

図１に戻り、次に、姿勢推定装置は、時刻ｔ_ｋにおける状態ベクトルｘ_ｋと誤差共分散行列Σ_ｘ，ｋ ^２の補正処理（観測更新処理ともいう）を行う（補正工程Ｓ７）。本実施形態では、姿勢推定装置は、誤差情報である誤差共分散行列Σ_ｘ，ｋ ^２に基づいて、工程Ｓ３で予測した、対象物の姿勢情報である姿勢クォータニオンｑ_ｋを補正する処理（姿勢情報補正工程）を行う。具体的には、姿勢推定装置は、姿勢情報補正工程では、加速度センサーの出力に基づいて得られる加速度ベクトルと基準ベクトルである重力加速度ベクトルとの差である観測残差Δｚ_ａ，ｋに基づいて、姿勢情報である姿勢クォータニオンｑ_ｋを要素として含む状態ベクトルｘ_ｋを補正する。図５は、図１の工程Ｓ７の手順の一例を示すフローチャート図である。

図５に示すように、姿勢推定装置は、まず、式（２３）及び式（２４）により、時刻ｔ_ｋにおける観測残差Δｚ_ｋ、カルマン係数Ｋ_ｋ及び変換行列Ｈ_ｋを計算する処理を行う（補正係数計算工程Ｓ７１）。

次に、姿勢推定装置は、予測した時刻ｔ_ｋにおける対象物の姿勢情報である姿勢クォータニオンｑ_ｋを補正する処理（姿勢情報補正処理）を行う（姿勢情報補正工程Ｓ７２）。具体的には、姿勢推定装置は、観測残差Δｚ_ｋ及びカルマン係数Ｋ_ｋを用いて、式（２７）により、時刻ｔ_ｋにおける状態ベクトルｘ_ｋを補正する処理を行う。

次に、姿勢推定装置は、式（２８）により、時刻ｔ_ｋにおける状態ベクトルｘ_ｋを正規化する処理を行う（正規化工程Ｓ７３）。

次に、姿勢推定装置は、カルマン係数Ｋ_ｋ及び変換行列Ｈ_ｋを用いて、式（２７）により、時刻ｔ_ｋにおける誤差共分散行列Σ_ｘ，ｋ ^２を補正する処理を行う（誤差情報補正工程Ｓ７４）。

図１に戻り、姿勢推定装置は、終了指示があるまで（工程Ｓ８のＮ）、工程Ｓ２〜工程Ｓ７の処理を繰り返し、終了指示があれば（工程Ｓ８のＹ）、処理を終了する。

なお、図１において工程の順序は適宜変更可能である。例えば、工程Ｓ４と工程Ｓ５と工程Ｓ６の順番を入れ替えてもよいし、工程Ｓ４、工程Ｓ５及び工程Ｓ６の少なくとも１つが工程Ｓ７の後に実行されてもよい。

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

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

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

また、本実施形態の姿勢推定方法では、角速度センサーの出力が有効範囲内ではない場合に、式（４１）により、誤差共分散行列Σ_ｘ，ｋ ^２において、姿勢誤差成分を増加させ、かつ、姿勢誤差成分と姿勢誤差成分以外の誤差成分との相関成分をゼロにする。したがって、角速度センサーの有効計測範囲を超える角速度が一時的に入力され、角速度センサーの出力が一時的に有効範囲内でない状況が発生した場合でも対象物の姿勢の推定精度が低下するおそれを低減することができる。

また、本実施形態の姿勢推定方法では、角速度センサーの出力が有効範囲内ではない場合に、式（４１）により、誤差共分散行列Σ_ｘ，ｋ ^２において、運動速度誤差成分を増加させ、かつ、運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分をゼロにする。また、加速度センサーの出力が有効範囲内ではない場合に、式（４２）により、誤差共分散行列Σ_ｘ，ｋ ^２において、運動速度誤差成分を増加させ、かつ、運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分をゼロにする。したがって、角速度センサーの有効計測範囲を超える角速度又は加速度センサーの有効計測範囲を超える加速度が一時的に入力され、角速度センサーの出力又は加速度センサーの出力が一時的に有効範囲内でない状況が発生した場合でも対象物の運動速度の推定精度が低下するおそれを低減することができる。

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

また、角速度センサーの出力と加速度センサーの出力には対象物の方位角についての情報が含まれないため、対象物がほぼ同じ姿勢を長時間継続するような場合、更新された角速度センサーのバイアス誤差の鉛直成分が単調に増大し、カルマン係数Ｋ_ｋが大きくなりすぎることにより、姿勢の推定精度が劣化するおそれがある。これに対して、本実施形態の姿勢推定方法によれば、角速度センサーのバイアス誤差の鉛直成分が上限値を超えた場合に、式（４０）により、当該バイアス誤差の鉛直成分を上限値に制限するので、カルマン係数Ｋ_ｋが大きくなりすぎるおそれが低減され、姿勢の推定精度が低下するおそれを低減することができる。

以上より、本実施形態の姿勢推定方法によれば、対象物の姿勢変化が少ない場合でも対象物の姿勢の推定精度が低下するおそれが低減され、対象物の姿勢を十分な精度で推定することができる。

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

図６に示すように、本実施形態では、姿勢推定装置１は、ＩＭＵ１０と分離されているが、姿勢推定装置１がＩＭＵ１０を含んでいてもよい。また、ＩＭＵ１０と姿勢推定装置１が１つの筐体に収容されて構成されていてもよいし、ＩＭＵ１０が姿勢推定装置１を収容する本体から分離されて、あるいは分離可能に構成されていてもよい。前者の場合は姿勢推定装置１が対象物に装着され、後者の場合はＩＭＵ１０が対象物に装着される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

姿勢情報予測部１０２は、姿勢変化量計算部２４が計算した姿勢変化量を用いて、対象物の姿勢情報である姿勢クォータニオンｑを予測する処理を行う。また、姿勢情報予測部１０２は、速度変化量計算部２６が計算した速度変化量に基づいて、対象物の速度情報である運動速度ベクトルｖを予測する処理も行う。実際には、姿勢情報予測部１０２は、姿勢クォータニオンｑ及び運動速度ベクトルｖを要素として含む状態ベクトルｘを予測する処理を行う。

誤差情報更新部１０３は、角速度センサー１２の出力に基づいて、誤差情報である誤差共分散行列Σ_ｘ ^２を更新する処理を行う。具体的には、誤差情報更新部１０３は、バイアス除去部２２によりバイアス誤差が除去された３軸角速度を用いて、対象物の姿勢誤差を更新する処理を行う。実際には、誤差情報更新部１０３は、拡張カルマンフィルターにより、誤差共分散行列Σ_ｘ ^２を更新する処理を行う。

回転誤差成分除去部１０８は、誤差情報である誤差共分散行列Σ_ｘ ^２における、基準ベクトルの周りの回転誤差成分を除去する処理を行う。具体的には、回転誤差成分除去部１０８は、誤差情報更新部１０３により更新された誤差共分散行列Σ_ｘ ^２における姿勢誤差に含まれる方位角誤差成分を除去する処理を行う。実際には、回転誤差成分除去部１０８は、誤差共分散行列Σ_ｘ ^２に対して、姿勢の誤差共分散行列Σ_ｑ ^２のランク制限と方位角誤差成分の除去を行った誤差共分散行列Σ_ｘ ^２を生成する処理を行う。

誤差情報調整部１１０は、角速度センサー１２の出力が有効範囲内であるか否かを判定し、角速度センサー１２の出力が有効範囲内ではないと判定した場合に、誤差情報である誤差共分散行列Σ_ｘ ^２における姿勢誤差成分を増加させ、かつ、誤差共分散行列Σ_ｘ ^２における姿勢誤差成分と姿勢誤差成分以外の誤差成分との相関成分を低減させる（例えばゼロにする）処理を行う。また、誤差情報調整部１１０は、加速度センサー１４の出力が有効範囲内であるか否かを判定し、角速度センサー１２の出力又は加速度センサー１４の出力が有効範囲内ではないと判定した場合に、誤差共分散行列Σ_ｘ，ｋ ^２における運動速度誤差成分を増加させ、かつ、誤差共分散行列Σ_ｘ，ｋ ^２における運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分を低減させる（例えばゼロにする）処理を行う。具体的には、誤差情報調整部１１０は、角速度センサー１２の出力が有効範囲内でないと判定した後の角速度オフスケール回復期間において、回転誤差成分除去部１０８により生成された誤差共分散行列Σ_ｘ ^２における姿勢誤差成分及び運動速度誤差成分を増加させ、かつ、姿勢誤差成分と姿勢誤差成分以外の誤差成分との相関成分及び運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分を低減させる（例えばゼロにする）処理を行う。また、誤差情報調整部１１０は、角速度センサー１２の出力が有効範囲内であり、かつ、加速度センサー１４の出力が有効範囲内でないと判定した後の加速度オフスケール回復期間において、回転誤差成分除去部１０８により生成された誤差共分散行列Σ_ｘ ^２における運動速度誤差成分を増加させ、かつ、運動速度誤差成分と運動速度誤差成分以外の誤差成分との相関成分を低減させる（例えばゼロにする）処理を行う。

バイアス誤差制限部１０９は、誤差情報である誤差共分散行列Σ_ｘ ^２における、基準ベクトルの周りの角速度のバイアス誤差成分に対して制限をかける処理を行う。具体的には、バイアス誤差制限部１０９は、誤差情報調整部１１０により生成された誤差共分散行列Σ_ｘ ^２における、角速度のバイアス誤差の鉛直成分に対して制限をかける処理を行う。実際には、バイアス誤差制限部１０９は、角速度のバイアス誤差の鉛直成分が上限値を超えたか否かを判定し、当該鉛直成分が上限値を超えた場合に、当該鉛直成分が当該上限値となるように制限をかけた誤差共分散行列Σ_ｘ ^２を生成する処理を行う。

補正係数計算部１０４は、バイアス誤差制限部１０９により生成された誤差情報である誤差共分散行列Σ_ｘ ^２に基づいて、姿勢情報補正部１０５による対象物の姿勢情報（姿勢クォータニオンｑ）や速度情報（運動速度ベクトルｖ）の補正量及び誤差情報補正部１０７による誤差情報（誤差共分散行列Σ_ｘ）の補正量を決める補正係数を計算する処理を行う。実際には、補正係数計算部１０４は、観測残差Δｚ、カルマン係数Ｋ及び変換行列Ｈを計算する処理を行う。

姿勢情報補正部１０５は、誤差情報である誤差共分散行列Σ_ｘに基づいて、姿勢情報予測部１０２が予測した対象物の姿勢情報（姿勢クォータニオンｑ）を補正する処理を行う。具体的には、姿勢情報補正部１０５は、バイアス誤差制限部１０９により生成された誤差共分散行列Σ_ｘ、並びに基準ベクトルである重力加速度ベクトルｇと加速度センサー１４の出力から求められる加速度ベクトルαとに基づいて補正係数計算部１０４が計算したカルマン係数Ｋ及び重力加速度の観測残差Δｚ_ａを用いて、姿勢クォータニオンｑを補正する処理を行う。実際には、姿勢情報補正部１０５は、拡張カルマンフィルターにより、姿勢情報予測部１０２が予測した状態ベクトルｘを補正する処理を行う。

正規化部１０６は、姿勢情報補正部１０５が補正した対象物の姿勢情報（姿勢クォータニオンｑ）を、その大きさが変わらないように正規化する処理を行う。実際には、正規化部１０６は、姿勢情報補正部１０５が補正した状態ベクトルｘを正規化する処理を行う。

誤差情報補正部１０７は、誤差情報である誤差共分散行列Σ_ｘを補正する処理を行う。具体的には、誤差情報補正部１０７は、補正係数計算部１０４が計算した変換行列Ｈ及びカルマン係数Ｋを用いて、拡張カルマンフィルターにより、バイアス誤差制限部１０９により生成された誤差共分散行列Σ_ｘを補正する処理を行う。

処理部２０が推定した対象物の姿勢情報（姿勢クォータニオンｑ）は、通信部６０を介して他の装置に送信可能である。

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

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

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

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

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

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

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

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

回転誤差成分除去部１０８は、姿勢情報予測部１０２が予測した姿勢クォータニオンｑ_ｋを用いて、前出の式（２９）により行列Ｄ’^ＴＤ’を計算し、行列Ｄ’^ＴＤ’を用いて、式（２８）により、誤差情報更新部１０３が更新した誤差共分散行列Σ_ｘ，ｋ ^２を更新する。これにより、姿勢クォータニオンｑ_ｋの誤差共分散行列Σ_ｑ，ｋ ^２のランクが３に制限されるとともに、誤差共分散行列Σ_ｑ，ｋ ^２から方位角誤差成分ε_θｚが除去された
誤差共分散行列Σ_ｘ，ｋ ^２が生成される。

誤差情報調整部１１０は、角速度オフスケール回復期間において、姿勢情報予測部１０２が予測した姿勢クォータニオンｑ_ｋを用いて、前出の式（４１）により、回転誤差成分除去部１０８が生成した誤差共分散行列Σ_ｘ，ｋ ^２を更新する。また、加速度オフスケール回復期間において、姿勢情報予測部１０２が予測した姿勢クォータニオンｑ_ｋを用いて、前出の式（４２）により、回転誤差成分除去部１０８が生成した誤差共分散行列Σ_ｘ，ｋ ^２を更新する。

バイアス誤差制限部１０９は、姿勢情報予測部１０２が予測した姿勢クォータニオンｑ_ｋと、誤差情報調整部１１０が生成した誤差共分散行列Σ_ｘ，ｋ ^２とを用いて、前出の式（３９）により、角速度センサー１２のバイアス誤差の鉛直成分の分散σ_ｂωｖ ^２を算出する。そして、バイアス誤差制限部１０９は、分散σ_ｂωｖ ^２が上限値σ_{ｂωｍａｘ} ^２を超えている場合は、式（４０）により、誤差情報調整部１１０が生成した誤差共分散行列Σ_ｘ，ｋ ^２を更新する。これにより、誤差共分散行列Σ_ｘ，ｋ ^２において、角速度センサー１２のバイアス誤差の鉛直成分の分散σ_ｂωｖ ^２が上限値σ_{ｂωｍａｘ} ^２に制限される。

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

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

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

誤差情報補正部１０７は、補正係数計算部１０４が計算した変換行列Ｈ_ｋ及びカルマン係数Ｋ_ｋ用いて、前出の式（２７）より、バイアス誤差制限部１０９が生成した誤差共分散行列Σ_ｘ，ｋ ^２を補正する。

正規化部１０６が計算した状態ベクトルｘ_ｋ及び誤差情報補正部１０７が補正した誤差共分散行列Σ_ｘ，ｋ ^２は、次のサンプリング間隔Δｔが経過すると、時刻ｔ_ｋ−１における状態ベクトルｘ_ｋ−１及び誤差共分散行列Σ_{ｘ，ｋ−１} ^２として、バイアス除去部２２、速度変化量計算部２６、積分計算部１０１、姿勢情報予測部１０２及び誤差情報更新部１０３にフィードバックされる。

以上に説明した処理部２０は、例えば、前述の図１〜図５に示した手順で、対象物の姿勢を推定する姿勢推定処理を行う。

以上に説明した本実施形態の姿勢推定装置１によれば、処理部２０が前述の図１〜図５
に示した手順で対象物の姿勢を推定することにより、角速度センサーの出力が一時的に有効範囲内でない状況が発生した場合でも対象物の姿勢の推定精度が低下するおそれが低減され、対象物の姿勢を十分な精度で推定することができる。また、本実施形態の姿勢推定装置１によれば、角速度センサーの出力又は加速度センサーの出力が一時的に有効範囲内でない状況が発生した場合でも対象物の運動速度の推定精度が低下するおそれが低減され、対象物の運動速度を十分な精度で推定することができる。その他、本実施形態の姿勢推定装置１によれば、上述した本実施形態の姿勢推定方法と同様の効果を奏することができる。

３．変形例
上述した実施形態では、角速度センサーと加速度センサーは１つの慣性計測装置（ＩＭＵ）に収容され、一体化されているが、角速度センサーと加速度センサーは別体であってもよい。

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

また、上述した実施形態では、加速度センサーと角速度センサーは同一のサンプリング間隔Δｔで出力を更新しているが、加速度センサーのサンプリング間隔Δｔ_ａと角速度センサーのサンプリング間隔Δｔ_ωは異なっていてもよい。この場合、図１の工程Ｓ３の状態ベクトルと誤差共分散行列の予測処理（時間更新処理）はΔｔ_ω毎に行われ、図１の工程Ｓ７の状態ベクトルと誤差共分散行列の補正処理（観測更新処理）はΔｔ_ａ毎に行われてもよい。また、図１の工程Ｓ４の回転誤差成分除去処理や図１の工程Ｓ６のバイアス誤差制限処理は、Δｔ_ω毎に行われてもよいし、Δｔ_ａ毎に行われてもよいし、Δｔ_ωやΔｔ_ａとは異なる周期で行われてもよい。

また、上述した実施形態では、姿勢推定装置は、加速度センサーを用いて観測される重力加速度ベクトルを基準ベクトルとして、角速度センサーの出力と基準ベクトルとを用いて対象物の姿勢を推定しているが、基準ベクトルは重力加速度ベクトルでなくてもよい。例えば、姿勢推定装置が角速度センサーと地磁気センサーとを用いて対象物の姿勢を推定する場合には、基準ベクトルは、地磁気センサーを用いて観測される地磁気ベクトル（北方向を向くベクトル）であってもよい。また、例えば、姿勢推定装置が角速度センサーとスタートラッカーとを用いて対象物である人工衛星の姿勢を推定する場合には、基準ベクトルは、スタートラッカーを用いて観測される、対象物から恒星の方向を向くベクトルであってもよい。なお、加速度センサー、地磁気センサー、スタートラッカーは、基準ベクトルを観測する基準観測センサーの一例である。

また、上述した実施形態では、対象物の姿勢情報をクォータニオンで表現したが、姿勢情報は、ロール角、ピッチ角、ヨー角で表現された情報であってもよいし、姿勢変換行列であってもよい。

また、上述した実施形態では、状態ベクトルｘ_ｋは、要素として、姿勢クォータニオンｑ_ｋ、運動速度ベクトルｖ_ｋ、角速度センサーの残存バイアスｂ_ω，ｋ、加速度センサーの残存バイアスｂ_α，ｋ及び重力加速度補正値Δｇ_ｋを含むが、状態ベクトルｘ_ｋはこれに限られない。例えば、状態ベクトルｘ_ｋは、要素として、姿勢クォータニオンｑ_ｋ、角速度センサーの残存バイアスｂ_ω，ｋ、加速度センサーの残存バイアスｂ_α，ｋ及び重力加速度補正値Δｇ_ｋを含み、運動速度ベクトルｖ_ｋを含まなくてもよい。この場合、図３に示したバイアス誤差制限工程のフローチャートは、例えば、図９に示すフローチャート
に変形される。なお、図９において図３と同じ工程には同じ符号を付しており、図９のフローチャートは図３のフローチャートの一部の工程を削除したものであって新たな工程を追加するものではないから、その説明を省略する。

４．電子機器
図１０は本実施形態の電子機器３００の構成例を示すブロック図である。電子機器３００は、上記実施形態の姿勢推定装置１と、慣性計測装置１０と、を含む。また、電子機器３００は、通信部３１０、操作部３３０、表示部３４０、記憶部３５０、アンテナ３１２を含むことができる。

通信部３１０は、例えば無線回路であり、アンテナ３１２を介して外部からデータを受信したり、外部にデータを送信する処理を行う。

姿勢推定装置１は、慣性計測装置１０の出力信号に基づいて処理を行う。具体的には、姿勢推定装置１は、慣性計測装置１０の検出データ等の出力信号（出力データ）に基づいて、電子機器３００の姿勢を推定する処理を行う。また、姿勢推定装置１は、慣性計測装置１０の検出データ等の出力信号（出力データ）に対して補正処理やフィルター処理などの信号処理を行ったり、或いは当該出力信号に基づいて、電子機器３００の制御処理や、通信部３１０を介して送受信されるデータの種々のデジタル処理などの電子機器３００についての各種の制御処理を行ってもよい。この姿勢推定装置１の機能は、例えばＭＰＵ、ＣＰＵなどのプロセッサーにより実現できる。

操作部３３０はユーザーが入力操作を行うためのものであり、操作ボタンやタッチパネルディスプレイをなどにより実現できる。

表示部３４０は各種の情報を表示するものであり、液晶や有機ＥＬなどのディスプレイにより実現できる。記憶部３５０はデータを記憶するものであり、その機能はＲＡＭやＲＯＭなどの半導体メモリーなどにより実現できる。

なお、本実施形態の電子機器３００は、例えばデジタルスチールカメラ又はビデオカメラ等の映像関連機器、車載機器、頭部装着型表示装置や時計関連機器などのウェアラブル機器、インクジェット式吐出装置、ロボット、パーソナルコンピューター、携帯情報端末、印刷装置、或いは投影装置等に適用できる。車載機器はカーナビゲーション装置や自動運転用の機器等である。時計関連機器は時計やスマートウォッチなどである。インクジェット式吐出装置としてはインクジェットプリンターなどがある。携帯情報端末は、スマートフォン、携帯電話機、携帯型ゲーム装置、ノートＰＣ又はタブレット端末などである。また本実施形態の電子機器３００は、電子手帳、電子辞書、電卓、ワードプロセッサー、ワークステーション、テレビ電話、防犯用テレビモニター、電子双眼鏡、ＰＯＳ端末、医療機器、魚群探知機、測定機器、移動体端末基地局用機器、計器類、フライトシミュレーター、ネットワークサーバー等にも適用できる。医療機器は、電子体温計、血圧計、血糖計、心電図計測装置、超音波診断装置、電子内視鏡などである。計器類は車両、航空機、船舶などの計器類である。

図１１は、携帯型電子機器である腕時計型の活動計４００を示す平面図であり、図１２は活動計４００の構成例を示すブロック図である。活動計４００は、バンド４０１によってユーザーの手首等の部位に装着される。アクティブトラッカーである活動計４００は、デジタル表示の表示部４０２を備えると共に、Bluetooth（登録商標）、Ｗｉ−Ｆｉ（登録商標）などによる無線通信が可能である。

図１１、図１２に示すように、活動計４００は、慣性計測装置１０が収容されたケース
４０３と、ケース４０３に収容され、慣性計測装置１０からの出力信号に基づいて処理を行う姿勢推定装置１と、ケース４０３に収容されている表示部４０２と、ケース４０３の開口部を塞いでいる透光性カバー４０４と、を含む。また、透光性カバー４０４の外側にはベゼル４０５が設けられ、ケース４０３の側面には複数の操作ボタン４０６、４０７が設けられている。慣性計測装置１０には、３軸の加速度を検出する加速度センサー１４と、３軸の角速度を検出する角速度センサー１２とが設けられている。

表示部４０２には、種々の検出モードに応じて、ＧＰＳセンサー４１１や地磁気センサー４１２を用いて求められた位置情報や移動量、加速度センサー１４や角速度センサー１２を用いて求められた運動量などの運動情報、脈拍センサー４１６を用いて求められた脈拍数などの生体情報、現在時刻などの時刻情報が表示される。また、温度センサー４１７を用いて求められた環境温度を表示することもできる。通信部４２２は、ユーザー端末などの情報端末との通信を行う。姿勢推定装置１は、例えばＭＰＵ、ＤＳＰ、ＡＳＩＣなどにより実現される。姿勢推定装置１は、記憶部４２０に記憶されるプログラムと、操作ボタン４０６、４０７などの操作部４１８により入力された情報とに基づき、各種の処理を実行する。姿勢推定装置１は、慣性計測装置１０の出力信号に基づいて、活動計４００の姿勢情報を推定する処理を行う。また、姿勢推定装置１は、ＧＰＳセンサー４１１、地磁気センサー４１２、圧力センサー４１３、加速度センサー１４、角速度センサー１２、脈拍センサー４１６、温度センサー４１７、計時部４１９の出力信号に基づく処理を行ってもよい。また、姿勢推定装置１は、表示部４０２に画像を表示させる表示処理、音出力部４２１に音を出力させる音出力処理、通信部４２２を介して情報端末と通信を行う通信処理、バッテリー４２３からの電力を各部へ供給する電力制御処理なども行うことができる。

以上のような構成の本実施形態の活動計４００によれば、前述した姿勢推定装置１の効果を享受でき、高い信頼性を発揮することができる。また、活動計４００は、ＧＰＳセンサー４１１を含み、ユーザーの移動距離や移動軌跡を計測することができるため、利便性の高い活動計４００が得られる。なお、活動計４００は、ランニングウォッチ、ランナーズウォッチ、アウトドアウォッチ、或いはＧＰＳを搭載したＧＰＳウォッチなどに広く適用できる。

５．移動体
本実施形態の移動体は、上記実施形態の姿勢推定装置１と、姿勢推定装置１が推定した移動体の姿勢情報に基づいて、移動体の姿勢の制御を行う制御装置と、を含む。

図１３に移動体５００の一例を示す。図１４は移動体５００の構成例を示すブロック図である。図１３に示すように、移動体５００は、車体５０２や車輪５０４を有している。また、移動体５００には測位装置５１０が装着されており、車両制御などを行う制御装置５７０が内部に設けられている。また、図１４に示すように、移動体５００は、エンジンやモーター等の駆動機構５８０と、ディスクブレーキやドラムブレーキ等の制動機構５８２と、ハンドルやステアリングギアボックス等で実現される操舵機構５８４と、を有する。このように、移動体５００は、駆動機構５８０や制動機構５８２や操舵機構５８４を備えて、地上や空や海上を移動する機器・装置である。例えば、移動体５００は農業機械（農機）などの四輪自動車である。

測位装置５１０は、移動体５００に装着されて、移動体５００の測位を行う装置である。測位装置５１０は、慣性計測装置１０と、ＧＰＳ受信部５２０と、ＧＰＳ受信用のアンテナ５２２と、姿勢推定装置１を含む。姿勢推定装置１は、位置情報取得部５３２と、位置合成部５３４と、演算処理部５３６と、処理部５３８と、を含む。慣性計測装置１０は、３軸の加速度センサーと３軸の角速度センサーを有している。演算処理部５３６は、加
速度センサー、角速度センサーからの加速度データ、角速度データを受け、これらデータに対して慣性航法演算処理を行い、慣性航法測位データを出力する。慣性航法測位データは移動体５００の加速度や姿勢を表すデータである。

ＧＰＳ受信部５２０は、アンテナ５２２を介してＧＰＳ衛星からの信号を受信する。位置情報取得部５３２は、ＧＰＳ受信部５２０が受信した信号に基づいて、測位装置５１０が装着された移動体５００の位置、速度、方位を表すＧＰＳ測位データを出力する。位置合成部５３４は、演算処理部５３６から出力された慣性航法測位データと、位置情報取得部５３２から出力されたＧＰＳ測位データとに基づいて、移動体５００が地面のどの位置を走行しているかを算出する。例えば、ＧＰＳ測位データに含まれている移動体５００の位置が同じであっても、図１３に示すように地面の傾斜（θ）などの影響によって移動体５００の姿勢が異なっていれば、地面の異なる位置を移動体５００が走行していることになる。そのため、ＧＰＳ測位データだけでは移動体５００の正確な位置を算出できない。そこで、位置合成部５３４は、慣性航法測位データのうちの特に移動体５００の姿勢に関するデータを用いて、移動体５００が地面のどの位置を走行しているのかを算出する。位置合成部５３４から出力された位置データは、処理部５３８によって所定の処理が行われ、測位結果として、表示部５５０に表示される。また、位置データは通信部５６０によって外部装置に送信されるようになっていてもよい。

制御装置５７０は、移動体５００の駆動機構５８０、制動機構５８２、操舵機構５８４の制御を行う。制御装置５７０は、車両制御用のコントローラーであり、例えば複数のコントロールユニットにより実現できる。制御装置５７０は、車両制御を行うコントロールユニットである車両制御部５７２と、自動運転制御を行うコントロールユニットである自動運転制御部５７４と、半導体メモリーなどにより実現される記憶部５７６と、を有する。監視装置５７８は、移動体５００の周辺の障害物等の物体を監視する装置であり、周辺監視カメラ、ミリ波レーダー又はソナーなどにより実現される。

そして、本実施形態の移動体５００は、図１４に示すように、姿勢推定装置１と、制御装置５７０と、を含む。制御装置５７０は、姿勢推定装置１が推定した移動体５００の姿勢情報に基づいて、移動体５００の姿勢の制御を行う。例えば、姿勢推定装置１は、慣性計測装置１０からの検出データを含む出力信号に基づいて、上述したような各種の処理を行って、移動体５００の位置や姿勢の情報を求める。例えば、姿勢推定装置１は、移動体５００の位置の情報を、上述したようにＧＰＳ測位データと慣性航法測位データとに基づき求めることができる。また、姿勢推定装置１は、移動体５００の姿勢の情報を、例えば慣性航法測位データに含まれる角速度データなどに基づいて推定することができる。移動体５００の姿勢の情報は、例えば、クォータニオンや、ロール角、ピッチ角、ヨー角などにより表すことができる。そして、制御装置５７０は、姿勢推定装置１の処理により推定された移動体５００の姿勢情報に基づいて、移動体５００の姿勢の制御を行う。この制御は例えば車両制御部５７２により行われる。この姿勢の制御は、例えば制御装置５７０が操舵機構５８４を制御することで実現できる。或いは、スリップ制御などの移動体５００の姿勢を安定化させる制御においては、制御装置５７０が駆動機構５８０を制御したり、制動機構５８２を制御してもよい。本実施形態によれば、姿勢推定装置１により、姿勢の情報を高精度に推定することができるため、移動体５００の適切な姿勢制御を実現できる。

また、本実施形態では、制御装置５７０は、姿勢推定装置１より求められた移動体５００の位置及び姿勢の情報に基づいて、移動体５００の加速、制動及び操舵の少なくとも１つを制御する。例えば、制御装置５７０は、移動体５００の位置及び姿勢の情報に基づいて、駆動機構５８０、制動機構５８２及び操舵機構５８４の少なくとも１つを制御する。これにより、例えば自動運転制御部５７４による移動体５００の自動運転制御を実現でき
る。この自動運転制御では、移動体５００の位置及び姿勢の情報に加えて、監視装置５７８による周囲の物体の監視結果や、記憶部５７６に記憶される地図情報や走行ルート情報などが用いられる。そして、制御装置５７０は、移動体５００の自動運転の実施又は不実施を、慣性計測装置１０の出力信号の監視結果に基づいて切り替える。例えば、姿勢推定装置１が、慣性計測装置１０からの検出データなどの出力信号を監視する。そして、例えば監視結果に基づいて、慣性計測装置１０の検出精度の低下やセンシング異常が検出された場合に、制御装置５７０は、自動運転の実施から、自動運転の不実施に切り替える。例えば、自動運転では、移動体５００の加速、制動及び操舵の少なくとも１つが自動で制御される。一方、自動運転の不実施では、このような加速、制動、操舵の自動制御が実施されない。このようにすることで、自動運転を行う移動体５００の走行について、より信頼性の高い支援が可能になる。なお、慣性計測装置１０の出力信号の監視結果に基づいて、自動運転の自動化レベルを切り替えてもよい。

図１５に他の移動体６００の一例を示す。図１６は移動体６００の構成例を示すブロック図である。上記実施形態の姿勢推定装置１は、建設機械（建機）の姿勢制御などにおいて効果的に用いることができ、図１５及び図１６は、移動体６００として建設機械（建機）の一例である油圧ショベルを例示している。

図１５に示すように、移動体６００は、車体を下部走行体６１２と、下部走行体６１２上に旋回可能に搭載された上部旋回体６１１とで構成され、上部旋回体６１１の前部側に上下方向に回動可能な複数の部材で構成された作業機構６２０が設けられている。上部旋回体６１１には不図示の運転席が設けられ、運転席には、作業機構６２０を構成する各部材を操作する不図示の操作装置が設けられている。そして、上部旋回体６１１には、上部旋回体６１１の傾斜角を検出する傾斜センサーとして機能する慣性計測装置１０ｄが配置されている。

作業機構６２０は、複数の部材として、上部旋回体６１１の前部側に俯仰動可能に取付けられたブーム６１３と、ブーム６１３の先端側に俯仰動可能に取付けられたアーム６１４と、アーム６１４の先端側に回動可能に取付けられたバケットリンク６１６と、アーム６１４およびバケットリンク６１６の先端側に回動可能に取付けられたバケット６１５と、ブーム６１３を駆動するブームシリンダー６１７と、アーム６１４を駆動するアームシリンダー６１８と、バケット６１５をバケットリンク６１６を介して駆動するバケットシリンダー６１９とを備えている。

ブーム６１３の基端側は、上部旋回体６１１に上下方向に回動可能に支持され、ブームシリンダー６１７の伸縮によってブーム６１３が上部旋回体６１１に対して相対的に回転駆動される。そして、ブーム６１３には、ブーム６１３の動きの状態を検出する慣性センサーとして機能する慣性計測装置１０ｃが配置されている。

ブーム６１３の先端側には、アーム６１４の一端側が回転可能に支持され、アームシリンダー６１８の伸縮によってアーム６１４がブーム６１３に対して相対的に回転駆動される。アーム６１４には、アーム６１４の動きの状態を検出する慣性センサーとして機能する慣性計測装置１０ｂが配置されている。

アーム６１４の先端側には、バケットリンク６１６とバケット６１５とが回動可能に支持されていて、バケットシリンダー６１９の伸縮に応じてバケットリンク６１６がアーム６１４に対して相対的に回転駆動され、それに連動してバケット６１５がアーム６１４に対して相対的に回転駆動される。そして、バケットリンク６１６には、バケットリンク６１６の動きの状態を検出する慣性センサーとして機能する慣性計測装置１０ａが配置されている。

ここで、慣性計測装置１０ａ，１０ｂ，１０ｃ，１０ｄとして、前述の実施形態において説明した慣性計測装置１０を用いることができる。慣性計測装置１０ａ，１０ｂ，１０ｃ，１０ｄは、作業機構６２０の各部材や上部旋回体６１１に作用する角速度、および加速度の少なくともいずれかを検出することができる。また、慣性計測装置１０ａ，１０ｂ，１０ｃは、図１６に示すように、直列的に接続され、検出信号を演算装置６３０に送信することができる。このように、慣性計測装置１０ａ，１０ｂ，１０ｃを直列接続することにより、可動領域内における検出信号を送信するための配線数を減らし、コンパクトな配線構造を得ることができる。コンパクトな配線構造により、配線の敷設方法の選択が容易となり、配線の劣化や損傷などの発生を低減させることが可能となる。

更に、移動体６００には、図１５に示すように、上部旋回体６１１の傾斜角や作業機構６２０を構成するブーム６１３、アーム６１４、バケット６１５の位置姿勢を演算する演算装置６３０が設けられている。図１６に示すように、演算装置６３０は、上記実施形態の姿勢推定装置１と制御装置６３２とを含む。姿勢推定装置１は、慣性計測装置１０ａ，１０ｂ，１０ｃ，１０ｄの出力信号に基づいて、移動体６００の姿勢情報を推定する。制御装置６３２は、姿勢推定装置１が推定した移動体６００の姿勢情報に基づいて、移動体６００の姿勢の制御を行う。具体的には、演算装置６３０は、各慣性計測装置１０ａ，１０ｂ，１０ｃ，１０ｄからの各種検出信号を入力し、各種検出信号に基づいてブーム６１３、アーム６１４、バケット６１５の位置姿勢（姿勢角）や上部旋回体６１１の傾斜状態を演算する。演算されたブーム６１３、アーム６１４、バケット６１５の姿勢角を含む位置姿勢信号や上部旋回体６１１の姿勢角を含む傾斜信号、例えばバケット６１５の位置姿勢信号は、運転席のモニター装置（図示せず）の表示、または作業機構６２０や上部旋回体６１１の動作を制御するためのフィードバック情報に用いられる。

なお、上記実施形態の姿勢推定装置１が用いられる建設機械（建機）としては、上記に例示した油圧ショベル（ユンボ、バックホー、パワーショベル）の他にも、例えば、ラフテレーンクレーン（クレーン車）、ブルドーザー、掘削機・積み込み機、ホイールローダー、高所作業車（リフト車）などがある。

本実施形態によれば、姿勢推定装置１により、姿勢の情報を高精度に求めることができるため、移動体６００の適切な姿勢制御を実現できる。また、移動体６００によれば、コンパクトな慣性計測装置１０を装着しているため、例えば、バケットリンク６１６などの極めて限られた狭い領域内であっても、慣性計測装置１０の設置箇所毎に、複数の慣性計測装置を直列接続（マルチ接続）してコンパクトに配置したり、各箇所に設置されている慣性計測装置１０同士をケーブルで直列的に接続するケーブルの引き回しをコンパクトにしたりすることが可能な建設機械を提供することができる。

なお、本実施形態では、姿勢推定装置１が用いられる移動体として、農業機械（農機）等の四輪自動車や建設機械（建機）を例にとり説明したが、これら以外にも、オートバイ、自転車、電車、飛行機、二足歩行ロボット、又は、ラジコン飛行機、ラジコンヘリコプター、ドローンなどの遠隔操縦あるいは自律式の飛行体、ロケット、人工衛星、船舶、ＡＧＶ（無人搬送車）などがある。

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

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

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

１…姿勢推定装置、１０…慣性計測ユニット（ＩＭＵ）、１２…角速度センサー、１４…加速度センサー、１６…信号処理部、２０…処理部、２２…バイアス除去部、２４…姿勢変化量計算部、２６…速度変化量計算部、２８…姿勢推定部、３０…ＲＯＭ、４０…ＲＡＭ、５０…記録媒体、６０…通信部、１０１…積分計算部、１０２…姿勢情報予測部、１０３…誤差情報更新部、１０４…補正係数計算部、１０５…姿勢情報補正部、１０６…正規化部、１０７…誤差情報補正部、１０８…回転誤差成分除去部、１０９…バイアス誤差制限部、１１０…誤差情報調整部、３００…電子機器、３１０…通信部、３１２…アンテナ、３３０…操作部、３４０…表示部、３５０…記憶部、４００…活動計、４０１…バンド、４０２…表示部、４０３…ケース、４０４…透光性カバー、４０５…ベゼル、４０６，４０７…操作ボタン、４１１…ＧＰＳセンサー、４１２…地磁気センサー、４１３…圧力センサー、４１６…脈拍センサー、４１７…温度センサー、４１８…操作部、４１９…計時部、４２０…記憶部、４２１…音出力部、４２２…通信部、４２３…バッテリー、５００…移動体、５０２…車体、５０４…車輪、５１０…測位装置、５２０…ＧＰＳ受信部、５２２…アンテナ、５３２…位置情報取得部、５３４…位置合成部、５３６…演算処理部、５３８…処理部、５５０…表示部、５６０…通信部、５７０…制御装置、５７２…車両制御部、５７４…自動運転制御部、５７６…記憶部、５７８…監視装置、５８０…駆動機構、５８２…制動機構、５８４…操舵機構、６１１…上部旋回体、６１２…下部走行体、６１３…ブーム、６１４…アーム、６１５…バケット、６１６…バケットリンク、６１７…ブームシリンダー、６１８…アームシリンダー、６１９…バケットシリンダー、６２０…作業機構、６３０…演算装置、６３２…制御装置

Claims (10)

1. 角速度センサーの出力に基づいて、対象物の姿勢変化量を計算する姿勢変化量計算工程と、
   前記姿勢変化量を用いて、前記対象物の姿勢情報を予測する姿勢情報予測工程と、
   前記角速度センサーの出力が有効範囲内であるか否かを判定し、前記角速度センサーの出力が有効範囲内ではないと判定した場合に、誤差情報における姿勢誤差成分を増加させ、かつ、前記誤差情報における前記姿勢誤差成分と前記姿勢誤差成分以外の誤差成分との相関成分を低減させる誤差情報調整工程と、
   前記誤差情報に基づいて、予測した前記対象物の姿勢情報を補正する姿勢情報補正工程と、
   を含む、姿勢推定方法。
2. 請求項１において、
   前記誤差情報調整工程は、
   前記角速度センサーの出力が有効範囲内ではないと判定した後の第１期間中であるか否かを判定する工程と、
   前記第１期間において、前記姿勢誤差成分を増加させる工程と、
   前記第１期間において、前記姿勢誤差成分と前記姿勢誤差成分以外の誤差成分との相関成分を低減させる工程と、
   を含む、姿勢推定方法。
3. 請求項１又は２において、
   前記姿勢誤差成分以外の誤差成分は、角速度のバイアス誤差成分を含む、姿勢推定方法。
4. 請求項１乃至３のいずれか一項において、
   前記姿勢誤差成分と前記姿勢誤差成分以外の誤差成分との相関成分はゼロである、姿勢推定方法。
5. 請求項１乃至４のいずれか一項において、
   加速度センサーの出力と前記角速度センサーの出力とに基づいて、前記対象物の速度変化量を計算する速度変化量計算工程を含み、
   前記誤差情報調整工程では、
   前記加速度センサーの出力が有効範囲内であるか否かを判定し、前記角速度センサーの出力又は前記加速度センサーの出力が有効範囲内ではないと判定した場合に、前記誤差情報における運動速度誤差成分を増加させ、かつ、前記誤差情報における前記運動速度誤差成分と前記運動速度誤差成分以外の誤差成分との相関成分を低減させ、
   前記姿勢情報予測工程では、
   前記速度変化量を用いて、前記対象物の速度情報を予測する、姿勢推定方法。
6. 請求項５において、
   前記誤差情報調整工程は、
   前記加速度センサーの出力が有効範囲内ではないと判定した後の第２期間中であるか否かを判定する工程と、
   前記第２期間において、前記運動速度誤差成分を増加させる工程と、
   前記第２期間において、前記運動速度誤差成分と前記運動速度誤差成分以外の誤差成分との相関成分を低減させる工程と、
   を含む、姿勢推定方法。
7. 請求項５又は６において、
   前記運動速度誤差成分以外の誤差成分は、加速度のバイアス誤差成分を含む、姿勢推定方法。
8. 請求項５乃至７のいずれか一項において、
   前記運動速度誤差成分と前記運動速度誤差成分以外の誤差成分との相関成分はゼロである、姿勢推定方法。
9. 角速度センサーの出力に基づいて、対象物の姿勢変化量を計算する姿勢変化量計算部と、
   前記姿勢変化量を用いて、前記対象物の姿勢情報を予測する姿勢情報予測部と、
   前記角速度センサーの出力が有効範囲内であるか否かを判定し、前記角速度センサーの出力が有効範囲内ではないと判定した場合に、誤差情報における姿勢誤差成分を増加させ、かつ、前記誤差情報における前記姿勢誤差成分と前記姿勢誤差成分以外の誤差成分との相関成分を低減させる誤差情報調整部と、
   前記誤差情報に基づいて、予測した前記対象物の姿勢情報を補正する姿勢情報補正部と、
   を含む、姿勢推定装置。
10. 請求項９に記載の姿勢推定装置と、
    前記姿勢推定装置が推定した移動体の姿勢情報に基づいて、前記移動体の姿勢の制御を行う制御装置と、
    を含む、移動体。

Images