JP5419268B2

JP5419268B2 - 三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラム

Info

Publication number: JP5419268B2
Application number: JP2009210985A
Authority: JP
Inventors: 秀彰大和; 貴之古田
Original assignee: Chiba Institute of Technology
Current assignee: Chiba Institute of Technology
Priority date: 2009-09-11
Filing date: 2009-09-11
Publication date: 2014-02-19
Anticipated expiration: 2029-09-11
Also published as: JP2011059020A

Description

本発明は、三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラムに関する。

従来より、ジャイロセンサーと、加速度センサー及び方位センサーの独立な２つの系統によって躯体の三次元姿勢角度を算出し、両者の算出値の差異からこれら算出された三次元姿勢角度に含まれる現実の三次元姿勢角度との誤差をカルマンフィルターによって推定し、この誤差を除去した躯体の現実の三次元姿勢角度を推定する技術がある（例えば、非特許文献１）。

鈴木智、外３名、「動加速度環境下における姿勢推定アルゴリズムの研究」日本ロボット学会誌Ｖｏｌ．２６Ｎｏ.６，ｐ．６２６〜６３４２００８年

ところで、カルマンフィルターによる誤差の推定では、演算負荷が高いという課題がある。
本発明はかかる従来の課題に鑑みて成されたもので、軽量な演算処理によって、三次元姿勢角度を推定することを目的とする。

かかる目的を達成するための主たる本発明は、加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定装置であって、前記躯体の加速度を検出する加速度センサーと、前記躯体が向いている方位を検出する方位センサーと、前記躯体の角速度を検出するジャイロセンサーと、前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求める暫定角度算出部と、前記躯体の前記三次元姿勢角度を推定する姿勢角度推定部と、前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定する判定部と、前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が直近に推定した三次元姿勢角度と前記暫定角度算出部が直近に求めた暫定角度とに基づいて補正値を求め、前記判定部が加減速運動状態にあると判定した場合には、前記判定部が加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持する補正値算出部と、を備え、前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度と前記補正値算出部が求めた補正値とに基づいて、前記三次元姿勢角度を推定し、前記判定部は、該角速度と該補正値に基づいて前記姿勢角度推定部が推定した前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定装置である。
この発明によれば、軽量な演算処理によって、三次元姿勢角度を推定することができ、加減速運動状態であるか否かの判定を高い精度で行うことができる。

かかる三次元姿勢角度推定装置であって、前記補正値算出部は、前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が推定する三次元姿勢角度が、前記暫定角度算出部が直近に求めた暫定角度と等しくなるような補正値を求めることを特徴とする三次元姿勢推定装置である。

かかる三次元姿勢角度推定装置であって、前記補正値算出部は、前記姿勢角度推定部が推定する三次元姿勢角度と、前記暫定角度算出部が求めた暫定角度との差分の累積値に基づいて、補正値を求めることを特徴とする三次元姿勢推定装置である。

かかる三次元姿勢角度推定装置であって、前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度から前記補正値算出部が求めた補正値を差し引いて三次元姿勢角度を推定することを特徴とする三次元姿勢推定装置である。

また、加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定方法であって、前記躯体の加速度を加速度センサーで検出することと、前記躯体が向いている方位を方位センサーで検出することと、前記躯体の角速度をジャイロセンサーで検出することと、前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、前記躯体の前記三次元姿勢角度を推定することと、前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、を有し、前記三次元姿勢角度を推定する際に、加減速運動状態にないと判定した場合には、前記ジャイロセンサーが検出した角速度と求めた前記補正値とに基づいて、前記三次元姿勢角度を推定し、加減速運動状態にあると判定した場合には、前記ジャイロセンサーが検出した角速度と保持した前記補正値とに基づいて、前記三次元姿勢角度を推定し、前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定方法である。
この発明によれば、軽量な演算処理によって、三次元姿勢角度を推定することができ、加減速運動状態であるか否かの判定を高い精度で行うことができる。

また、加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定プログラムであって、情報処理装置に、加速度センサーで検出した前記躯体の加速度を取得することと、方位センサーで検出した前記躯体が向いている方位を取得することと、ジャイロセンサーで検出した前記躯体の角速度を取得することと、前記加速度と前記方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、前記躯体の前記三次元姿勢角度を推定することと、前記加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、を実行させ、前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定プログラムである。
この発明によれば、軽量な演算処理によって、三次元姿勢角度を推定することができ、加減速運動状態であるか否かの判定を高い精度で行うことができる。

本発明によれば、軽量な演算処理によって、算出された三次元姿勢角度に含まれる現実の三次元姿勢角度との誤差を推定することができる。また、加減速運動状態であるか否かの判定を高い精度で行うことができる。

第１実施形態の三次元姿勢角度推定装置１のブロック構成図である。情報処理装置２のハードウェア構成を示す図である。各機能部３１〜３４が実行する三次元姿勢推定アルゴリズムを示すブロック図である。図３に示す三次元姿勢推定アルゴリズムを分かりやすく説明するためのフロー図である。判定基準となる閾値の算出根拠となる考え方を示す概念図である。三次元姿勢変動に加えて２０秒〜３０秒の期間に加速度外乱を印加したときにおいて、外乱加速度ａ_ｄを推定する実験の結果を示すグラフである。三次元姿勢角度推定装置１によって推定したＲｏｌｌ軸についての三次元姿勢角度の推定結果（実線で示す）とエンコーダーによる実測値（点線で示す）を対比して示す折れ線グラフである。三次元姿勢角度推定装置１によって推定したＰｉｔｃｈ軸についての三次元姿勢角度の推定結果（実線で示す）とエンコーダーによる実測値（点線で示す）を対比して示す折れ線グラフである。三次元姿勢角度推定装置１によって推定したＹａｗ軸についての三次元姿勢角度の推定結果（実線で示す）とエンコーダーによる実測値（点線で示す）を対比して示す折れ線グラフである。判定基準となる閾値の算出根拠となる別の考え方を示す概念図である。

＝＝＝第１実施形態＝＝＝
本発明の第１実施形態である三次元姿勢角度推定装置１は、例えば、上下・左右・前後に加減速可能なロボット（「躯体」に相当）に装着され、三次元姿勢角度推定装置１が推定する三次元姿勢角度に基づいて、そのロボットの姿勢を制御することができる。

なお、第１実施形態の説明において用いる記号θ_１、θ_２、ａ_ｍ、ｎ_ｍ、ω_ｍ、Δθ、Δω_ｃ、ａ^{ｇｌｏｂａｌ} _ｄ、ｇ^{ｇｌｏｂａｌ}は、次の通りである。
θ_１＝［θ_１ｘ θ_１ｙ θ_１ｚ］^Ｔ：３次元姿勢角度ベクトル推定値
θ_２＝［θ_２ｘ θ_２ｙ θ_２ｚ］^Ｔ：３次元姿勢角度ベクトル暫定値
ａ_ｍ＝［ａ_ｍｘａ_ｍｙａ_ｍｚ］^Ｔ：３軸加速度センサーにより取得される加速度ベクトル
ｎ_ｍ＝［ｎ_ｍｘｎ_ｍｙｎ_ｍｚ］^Ｔ：３軸方位センサーにより取得される方位ベクトル
ω_ｍ＝［ω_ｍｘ ω_ｍｙ ω_ｍｚ］^Ｔ：３軸ジャイロセンサーにより取得される角速度ベクトル
Δθ＝［Δθ_ｘ Δθ_ｙ Δθ_ｚ］^Ｔ：３次元姿勢角度ベクトルにおける推定値と暫定値の差分
Δω_ｃ＝［Δω_ｃｘ Δω_ｃｙ Δω_ｃｚ］^Ｔ：３軸角速度ベクトルに対する補正値ベクトル
ａ^{ｇｌｏｂａｌ} _ｄ＝［ａ^{ｇｌｏｂａｌ} _ｄｘａ^{ｇｌｏｂａｌ} _ｄｙ Δａ^{ｇｌｏｂａｌ} _ｄｚ］^Ｔ：３軸加速度ベクトルにおける重量加速度以外の成分
ｇ^{ｇｌｏｂａｌ}＝［００ −１］^Ｔ：慣性座標系での重力加速度ベクトル

図１は、本発明の第１実施形態である三次元姿勢角度推定装置１のブロック構成図である。同図に示すように、三次元姿勢角度推定装置１は、方位センサー２１、加速度センサー２２、ジャイロセンサー２３の各センサー群２１〜２３と、情報処理装置２とを備える。また、情報処理装置２は、暫定角度算出部３１、判定部３２、補正値算出部３３、姿勢角度推定部３４の各機能部３１〜３４を備える。

図２は、情報処理装置２のハードウェア構成を示す図である。情報処理装置２は、マイクロコンピューター等のコンピューターと周辺機器とが接続されることにより構成され、ＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）１０、メモリー１１、記憶手段１２、通信インターフェイス１３等を備えている。

ＣＰＵ１０は、記憶手段１２に記憶されているプログラムをメモリー１１に読み出して実行する。メモリー１１は、ＣＰＵ１０のプログラムを格納する領域や作業領域等を確保するためのものであって、例えばＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）等である。記憶手段１２は、例えばフラッシュメモリー等である。通信インターフェイス１３は、例えばシリアルポート等であって、各センサー群２１〜２３が検出した情報などを受信するとともに、ＣＰＵ１０が実行したプログラムの処理結果を外部に送信する。

なお、各機能部３１〜３４は、ＣＰＵ１０が記憶手段１２に記憶されたプログラムをメモリー１１に読み出して実行することにより実現される。

方位センサー２１は、地磁気に基づいていずれが北の方角であるかを検出する。加速度センサー２２は、ロボットの独立な３軸方向の加速度を検出する。ジャイロセンサー２３は、独立な３軸方向の角速度を検出する。

暫定角度算出部３１は、方位センサー２１が検出した方位と加速度センサー２２が検出した加速度とに基づいて、座標変換によってロボットの三次元姿勢角度θ_２の暫定値である暫定角度を求める。

なお、暫定角度算出部３１が算出する三次元姿勢角度は、ロボットが加減速運動を行っている状態においては誤差を含むので、必ずしも信頼できる値ではないことから暫定値とする。この暫定値は、後述するように三次元姿勢角度を推定するための補正値の算出に用いる。

判定部３２は、加速度センサー２２が検出する加速度ａ_ｍと、姿勢角度推定部３４が推定した三次元姿勢角度θ_１とに基づいて、重力加速度成分を除去した加速度を求める。この重力加速度成分を除去した加速度は、暫定角度算出部３１の暫定角度の算出において外乱要因となるが、この重力加速度成分を除去した加速度（以下、「外乱加速度ａ_ｄ」）が重力加速度ｇ^{ｇｌｏｂａｌ}の１／１０（「所定の大きさ」に相当）未満であれば加減速運動状態でないと判定する。また、外乱加速度ａ_ｄが重力加速度ｇ^{ｇｌｏｂａｌ}の１／１０以上であれば加減速運動状態であると判定する。

より具体的には、外乱加速度ａ_ｄは、姿勢角度推定部３４による三次元姿勢角度θ_１の推定が良好に行われていることを前提とし、座標変換を施した重力加速度ベクトルを、加速度センサー２１が検出した加速度ベクトルａ_ｍから差引くことにより得られる。そして、外乱加速度ａ_ｄの大きさにおける重力加速度ｇ^{ｇｌｏｂａｌ}の１／１０未満であれば加減速運動状態でないと判定し、重力加速度ｇ^{ｇｌｏｂａｌ}の１／１０以上であれば加減速運動状態であると判定する。

補正値算出部３３は、判定部３２が加減速運動状態にないと判定した場合には、後述する姿勢角度推定部３４が直近に推定した三次元姿勢角度θ_１と、暫定角度算出部３１が直近に求めた暫定角度θ_２との差分Δθの累積値に基づいて、補正値Δω_ｃを求める。また、補正値算出部３３は、判定部３２が加減速運動状態にあると判定した場合には、判定部３２が加減速運動状態にないと判定した時点であって直近の時点において求めた補正値Δω_ｃを保持する。

より具体的には、補正値算出部３３は、判定部３２が加減速運動状態にないと判定した場合（ｒ＝１）には、三次元姿勢角度θ_１と暫定角度θ_２との慣性座標系での差分Δθをヤコビ行列によってボディ座標系での角速度に変換し、さらにこの角速度を積分項及び比例項によって変換することによってΔω_ｃを求める。ここで、積分項は、時間変動を伴う補正値に対して推定する三次元姿勢角度の追従性を向上する役割を持ち、三次元姿勢角度θ_１が暫定角度θ_２に収束した場合においては、積分項によって保持された補正値Δω_ｃにより、角速度の修正が継続される。また、比例項は差分Δθの収束性を確保する。このように、補正値算出部３３は、後述する姿勢角度推定部３４が推定する三次元姿勢角度θ_１を、暫定角度θ_２に収束させ、差分Δθ→０とする。つまり、補正値算出部３３は、三次元姿勢角度θ_１は、暫定角度算出部３１が直近に求めた暫定角度θ_２と等しくなるような補正値Δω_ｃを求める。

また、補正値算出部３３は、判定部３２が加減速運動状態にあると判定した場合（ｒ＝０）には、三次元姿勢角度θ_１と暫定角度θ_２との差分Δθにｒ＝０を乗じて、補正値Δω_ｃの算出への差分Δθの入力を遮断する。すなわち、補正値算出の積分項は、加減速運動状態にないと判定した時点の最後の時点に用いられた補正Δω_ｃを保持する。

姿勢角度推定部３４は、ジャイロセンサー２３が検出した角速度ω_ｍから補正値算出部３３が求めた補正値Δω_ｃを差し引き、座標変換と数値積分によってロボットの三次元姿勢角度θ_１を推定する。

図３は、各機能部３１〜３４が実行する三次元姿勢推定アルゴリズムを示すブロック図であり、図４は、図３に示す三次元姿勢推定アルゴリズムを分かりやすく説明するためのフロー図である。以下、図４に沿って説明する。

まず、暫定角度算出部３１は、方位センサー２１が検出した方位と加速度センサー２２が検出した加速度とに基づいて、θ_２＝ｈ（ａ_ｍ，ｎ_ｍ）によってロボットの三次元姿勢角度の暫定値である暫定角度θ_２を求める（Ｓ１０２）。ここで、θ_２ｘ、θ_２ｙ、θ_２ｚは、式（１）、式（２）、式（３）の通りである。

次に、後述するＳ１２４で算出する三次元姿勢角度θ_１と、暫定角度算出部３１が直近に求めた暫定角度θ_２との差分Δθを算出する（Ｓ１０４）。

一方で、判定部３２は、加速度センサー２２が検出する加速度ａ_ｍと、姿勢角度推定部３４が推定した三次元姿勢角度θ_１とに基づいて、重力加速度成分ｇ^{ｇｌｏｂａｌ}を除去した外乱加速度ａ_ｄ ^{ｇｌｏｂａｌ}を求める（Ｓ１０６）。具体的には、ｇ^{ｇｌｏｂａｌ}と三次元姿勢角度θ_１とを式（４）、式（５）で座標変換することによって算出する。

ここで、

である。

外乱加速度ａ_ｄが重力加速度ｇ^{ｇｌｏｂａｌ}の１／１０（「所定の大きさ」又は「ｘ」に相当）未満であれば加減速運動状態でないと判定し（Ｓ１０８：ＮＯ）、外乱加速度ａ_ｄが重力加速度ｇ^{ｇｌｏｂａｌ}の１／１０以上であれば加減速運動状態であると判定する（Ｓ１０８：ＹＥＳ）。

ところで、図５は、判定基準となる閾値「重力加速度ｇ^{ｇｌｏｂａｌ}の１／１０」の算出根拠となる考え方を示す概念図である。ここで、外乱加速度ａ_ｄを求める手順を説明する。まず、ロボット全体にかかる加速度ａ_ｍはボディ座標系で得られる。また、重力加速度ｇは、慣性座標系では鉛直下向きで一定である。重力加速度ｇと、後述のＳ１２４において推定した三次元姿勢角度とに基づいて、式（６）によって座標変換すれば、ボディ座標系の重力加速度

を推定できる。

したがって、式（７）により求められる外乱加速度の推定値の絶対値ｅ_２が閾値以上であるか閾値未満であるかによって加減速運動状態であるか否かを判定できる。

図６は、三次元姿勢変動に加えて２０秒〜３０秒の期間に加速度外乱を印加したときにおいて、式（４）〜（７）によって外乱加速度の推定値の重力加速度に対する比率を測定する実験の結果を示すグラフである。同図に示す線分が外乱加速度の推定値の絶対値ｅ_２の重力加速度に対する比率である。同図に示すように、推定された外乱加速度の絶対値ｅ_２が重力加速度の１／１０以上であるときには加速度状態にあると判定し、重力加速度の１／１０未満であるときには加速度状態にないと判定すると、２０秒〜３０秒の期間のみ加速度状態にあった実際のロボットの動きと概ね符合することが分かる。すなわち、重力加速度の１／１０が適切な閾値であることが確認できた。

さて、加減速運動状態でないと判定した場合（Ｓ１０８：ＮＯ）には、後述する補正に用いる係数ｒ＝１とし（Ｓ１１０）、加減速運動状態であると判定した場合（Ｓ１０８：ＹＥＳ）には、後述する補正に用いる係数ｒ＝０とする（Ｓ１１２）。

Ｓ１１０又はＳ１１２において求めた係数ｒに差分Δθを乗じ（Ｓ１１４）、その乗じた値Δθ・ｒを角速度へ変換し（Ｓ１１６）、補正値Δω_ｃを算出する。なお、ここで判定部３２が加減速運動状態にないと判定した場合（Ｓ１０８：ＮＯ）にはｒ＝１であるので差分Δθに応じて積分計算などにより補正値Δω_ｃは順次更新されることとなる。一方で、加減速運動状態であると判定した場合（Ｓ１０８：ＹＥＳ）にはｒ＝０であるので補正値Δω_ｃは更新されず、加減速運動状態にないと判定した場合（Ｓ１０８：ＮＯ）であって直近の時点において算出された補正値Δω_ｃが保持されることとなる。

ジャイロセンサー２３が検出した角速度ω_ｍと補正値算出部３３が求めた補正値Δω_ｃとに基づいて、次式（８）によりωを求める（Ｓ１２０）。

そして、それを次式（９）により、

θ_１を慣性座標系の角速度に座標変換する（Ｓ１２２）。なお、ｆ（θ_１）は次式（１０）に示す通りである。

更に数値積分することによってロボットの三次元姿勢角度θ_１を推定する（Ｓ１２４）。なお、推定した三次元姿勢角度θ_１は、Ｓ１０４の差分算出に用いる。

以上、本実施形態の三次元姿勢角度推定装置１によれば、軽量な演算処理によって、三次元姿勢角度を推定することができる。

従来技術においては、理想環境下での特定の最適性が理論証明されるものの、モデルパラメーターの変動に対する鋭敏性、実装時の動作調整に大きな労力を要し、更には演算負荷が高く高周期推定に向かないなどの課題があった。しかし、本実施形態の三次元姿勢角度推定装置１によれば、モデルパラメーターの変動に対する追従性は高く、軽量な演算処理であるので実装時の動作調整にかかる労力は少なく、演算負荷が小さく高周期推定も可能である。

具体的には、ジャイロセンサー２３と、加速度センサー２２及び方位センサーとによる独立な２系統の姿勢情報が得られ、これらの姿勢情報により加速度外乱の検知状況に応じて、それらを統合処理して三次元姿勢角度を推定できる。本発明の特徴は、累積誤差の要因となるジャイロセンサー２３が検出した角速度に係るジャイロバイアス項の継続的な推定・除去機能を、構造の簡単なフィードバックにより達成することができる。そして、これにより、ジャイロセンサーが検出した角速度に含まれるバイアス値の実時間かつ継続的な推定・除去能力を確保し、その上で、加速度外乱の有無を判定する判定部を設けて、２系統の姿勢角度情報を統合する。こうしたフィードバック構造を基本とした統合処理により、累積誤差の除去と動加速度外乱の影響軽減化の機能が得られ、従来技術において適用することが前提となっていたカルマンフィルター（ＫａｌｍａｎＦｉｌｔｅｒ）などによる処理と比較して、極めて軽量な演算処理によって三次元姿勢角度を推定することができる。

ところで、本実施形態の三次元姿勢角度推定装置１によれば、適切に三次元姿勢角度を推定できることを確認するため、次のような実験を行った。本実験では、振幅及び周波数が異なる正弦波動作をロール軸、ピッチ軸、ヨー軸の３軸に与え、さらに動作中の特定期間（２０〜３０秒）においては、並進軸での正弦波動作によって加速度外乱を加える。このような実験において、図７Ａは、本実施形態の三次元姿勢角度推定装置１によって推定したロール軸についての三次元姿勢角度の推定結果（実線で示す）とエンコーダーによる実測値（点線で示す）を対比して示す折れ線グラフである。また、図７Ｂは同様にピッチ軸について、図７Ｃは同様にヨー軸について、それぞれ推定結果と実測値を対比して示す折れ線グラフである。図７Ａ〜図７Ｃに示すように、三次元姿勢角度推定装置１によれば、いずれも全試験期間（１０〜４０秒）において推定結果と実測値がほぼ同じ値となっていることから、良好な推定結果を得られることが確認できた。

また、判定部３２は、加速度センサー２２が検出する加速度と、姿勢角度推定部３４が推定した三次元姿勢角度とに基づいて、加減速運動状態であるか否かを判定することで、判定を高い精度で行うことができる。

＝＝＝その他の実施の形態＝＝＝
以上、本発明の実施形態について説明したが、本発明は、かかる実施形態に限定されるものではなく、その要旨を逸脱しない範囲で以下に示すような変形が可能である。

例えば、本実施形態の三次元姿勢角度推定装置１はロボットの姿勢制御に用いることとしたが、これに限られず、例えば、ウェアラブルモーションキャプチャーや、小型無人航空機、ヘッドマウントディスプレイ、ゲーム機の操作部等にも用いることができる。

また、補正値算出部３３は、判定部３２が加減速運動状態にないと判定した場合（ｒ＝１）には、三次元姿勢角度θ_１と暫定角度θ_２との慣性座標系での差分Δθをヤコビ行列によってボディ座標系での角速度に変換することとしたが、鉛直軸近傍にて姿勢変動を行う物体の３次元姿勢角度推定には、ヤコビ行列を使用しなくともよい。

これにより、演算処理はさらに軽減される一方で、鉛直軸近傍にて姿勢変動を行う場合には、演算結果に大きな差異は生じない。

また、例えば、判定部３２は、加速度センサー２２が検出する加速度と、姿勢角度推定部３４が推定した三次元姿勢角度とに基づいて、加減速運動状態であるか否かを判定することとしたが、加速度センサー２２が検出する加速度のみに基づいて、加減速運動状態であるか否かを判定してもよい。

図８は、判定基準となる閾値「重力加速度ｇ^{ｇｌｏｂａｌ}の１／１０」の算出根拠となる別の考え方を示す概念図である。ここで、外乱加速度ａ_ｄの影響度ｅ_１を求める手順を説明する。まず、ロボット全体にかかる加速度ａ_ｍはボディ座標系で得られる。また、重力加速度ｇの大きさは、どの座標系でも一定値である。したがって、次式（１１）により、外乱加速度の影響度ｅ_１を求めることができる。

そして、この値が重力加速度の１／１０以上であれば加速度状態にあると判定でき、重力加速度の１／１０未満であれば加速度状態にないと判定できる。

このような判定によれば、加減速運動状態であるか否かの判定を簡便に行うことができる。

１三次元姿勢角度推定装置
２情報処理装置
１０ＣＰＵ
１１メモリー
１２記憶手段
１３通信インターフェイス
２１方位センサー
２２速度センサー
２３ジャイロセンサー
３１暫定角度算出部
３２判定部
３３補正値算出部
３４姿勢角度推定部

Claims

加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定装置であって、
前記躯体の加速度を検出する加速度センサーと、
前記躯体が向いている方位を検出する方位センサーと、
前記躯体の角速度を検出するジャイロセンサーと、
前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求める暫定角度算出部と、
前記躯体の前記三次元姿勢角度を推定する姿勢角度推定部と、
前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定する判定部と、
前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が直近に推定した三次元姿勢角度と前記暫定角度算出部が直近に求めた暫定角度とに基づいて補正値を求め、前記判定部が加減速運動状態にあると判定した場合には、前記判定部が加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持する補正値算出部と、
を備え、
前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度と前記補正値算出部が求めた補正値とに基づいて、前記三次元姿勢角度を推定し、
前記判定部は、該角速度と該補正値に基づいて前記姿勢角度推定部が推定した前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、
前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、
前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定装置。
請求項１に記載の三次元姿勢推定装置であって、
前記補正値算出部は、前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が推定する三次元姿勢角度が、前記暫定角度算出部が直近に求めた暫定角度と等しくなるような補正値を求めることを特徴とする三次元姿勢推定装置。
請求項１又は２に記載の三次元姿勢推定装置であって、
前記補正値算出部は、前記姿勢角度推定部が推定する三次元姿勢角度と、前記暫定角度算出部が求めた暫定角度との差分の累積値に基づいて、補正値を求めることを特徴とする三次元姿勢推定装置。
請求項１〜３の何れかに記載の三次元姿勢推定装置であって、
前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度から前記補正値算出部が求めた補正値を差し引いて三次元姿勢角度を推定することを特徴とする三次元姿勢推定装置。
加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定方法であって、
前記躯体の加速度を加速度センサーで検出することと、
前記躯体が向いている方位を方位センサーで検出することと、
前記躯体の角速度をジャイロセンサーで検出することと、
前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、
前記躯体の前記三次元姿勢角度を推定することと、
前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、
加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、を有し、
前記三次元姿勢角度を推定する際に、加減速運動状態にないと判定した場合には、前記ジャイロセンサーが検出した角速度と求めた前記補正値とに基づいて、前記三次元姿勢角度を推定し、加減速運動状態にあると判定した場合には、前記ジャイロセンサーが検出した角速度と保持した前記補正値とに基づいて、前記三次元姿勢角度を推定し、
前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、
前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、
前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定方法。
加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定プログラムであって、
情報処理装置に、
加速度センサーで検出した前記躯体の加速度を取得することと、
方位センサーで検出した前記躯体が向いている方位を取得することと、
ジャイロセンサーで検出した前記躯体の角速度を取得することと、
前記加速度と前記方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、
前記躯体の前記三次元姿勢角度を推定することと、
前記加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、
加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、
を実行させ、
前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、
前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、
前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定プログラム。