(57)【特許請求の範囲】
【請求項１】
加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定装置であって、
前記躯体の加速度を検出する加速度センサーと、
前記躯体が向いている方位を検出する方位センサーと、
前記躯体の角速度を検出するジャイロセンサーと、
前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求める暫定角度算出部と、
前記躯体の前記三次元姿勢角度を推定する姿勢角度推定部と、
前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定する判定部と、
前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が直近に推定した三次元姿勢角度と前記暫定角度算出部が直近に求めた暫定角度とに基づいて補正値を求め、前記判定部が加減速運動状態にあると判定した場合には、前記判定部が加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持する補正値算出部と、
を備え、
前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度と前記補正値算出部が求めた補正値とに基づいて、前記三次元姿勢角度を推定し、
前記判定部は、該角速度と該補正値に基づいて前記姿勢角度推定部が推定した前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、
前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、
前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定装置。
【請求項２】
請求項１に記載の三次元姿勢推定装置であって、
前記補正値算出部は、前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が推定する三次元姿勢角度が、前記暫定角度算出部が直近に求めた暫定角度と等しくなるような補正値を求めることを特徴とする三次元姿勢推定装置。
【請求項３】
請求項１又は２に記載の三次元姿勢推定装置であって、
前記補正値算出部は、前記姿勢角度推定部が推定する三次元姿勢角度と、前記暫定角度算出部が求めた暫定角度との差分の累積値に基づいて、補正値を求めることを特徴とする三次元姿勢推定装置。
【請求項４】
請求項１〜３の何れかに記載の三次元姿勢推定装置であって、
前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度から前記補正値算出部が求めた補正値を差し引いて三次元姿勢角度を推定することを特徴とする三次元姿勢推定装置。
【請求項５】
加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定方法であって、
前記躯体の加速度を加速度センサーで検出することと、
前記躯体が向いている方位を方位センサーで検出することと、
前記躯体の角速度をジャイロセンサーで検出することと、
前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、
前記躯体の前記三次元姿勢角度を推定することと、
前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、
加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、を有し、
前記三次元姿勢角度を推定する際に、加減速運動状態にないと判定した場合には、前記ジャイロセンサーが検出した角速度と求めた前記補正値とに基づいて、前記三次元姿勢角度を推定し、加減速運動状態にあると判定した場合には、前記ジャイロセンサーが検出した角速度と保持した前記補正値とに基づいて、前記三次元姿勢角度を推定し、
前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、
前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、
前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定方法。
【請求項６】
加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定プログラムであって、
情報処理装置に、
加速度センサーで検出した前記躯体の加速度を取得することと、
方位センサーで検出した前記躯体が向いている方位を取得することと、
ジャイロセンサーで検出した前記躯体の角速度を取得することと、
前記加速度と前記方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、
前記躯体の前記三次元姿勢角度を推定することと、
前記加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、
加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、
を実行させ、
前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、
前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、
前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、三次元姿勢推定装置、三次元姿勢推定方法及び三次元姿勢推定プログラムに関する。
【背景技術】
【０００２】
従来より、ジャイロセンサーと、加速度センサー及び方位センサーの独立な２つの系統によって躯体の三次元姿勢角度を算出し、両者の算出値の差異からこれら算出された三次元姿勢角度に含まれる現実の三次元姿勢角度との誤差をカルマンフィルターによって推定し、この誤差を除去した躯体の現実の三次元姿勢角度を推定する技術がある（例えば、非特許文献１）。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】鈴木智、外３名、「動加速度環境下における姿勢推定アルゴリズムの研究」日本ロボット学会誌  Ｖｏｌ．２６  Ｎｏ.６，  ｐ．６２６〜６３４  ２００８年
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、カルマンフィルターによる誤差の推定では、演算負荷が高いという課題がある。  
本発明はかかる従来の課題に鑑みて成されたもので、軽量な演算処理によって、三次元姿勢角度を推定することを目的とする。
【課題を解決するための手段】
【０００５】
かかる目的を達成するための主たる本発明は、加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定装置であって、前記躯体の加速度を検出する加速度センサーと、前記躯体が向いている方位を検出する方位センサーと、前記躯体の角速度を検出するジャイロセンサーと、前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求める暫定角度算出部と、前記躯体の前記三次元姿勢角度を推定する姿勢角度推定部と、前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定する判定部と、前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が直近に推定した三次元姿勢角度と前記暫定角度算出部が直近に求めた暫定角度とに基づいて補正値を求め、前記判定部が加減速運動状態にあると判定した場合には、前記判定部が加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持する補正値算出部と、を備え、前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度と前記補正値算出部が求めた補正値とに基づいて、前記三次元姿勢角度を推定し、前記判定部は、該角速度と該補正値に基づいて前記姿勢角度推定部が推定した前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定装置である。
この発明によれば、軽量な演算処理によって、三次元姿勢角度を推定することができ、加減速運動状態であるか否かの判定を高い精度で行うことができる。
【０００６】
かかる三次元姿勢角度推定装置であって、前記補正値算出部は、前記判定部が加減速運動状態にないと判定した場合には、前記姿勢角度推定部が推定する三次元姿勢角度が、前記暫定角度算出部が直近に求めた暫定角度と等しくなるような補正値を求めることを特徴とする三次元姿勢推定装置である。
【０００７】
かかる三次元姿勢角度推定装置であって、前記補正値算出部は、前記姿勢角度推定部が推定する三次元姿勢角度と、前記暫定角度算出部が求めた暫定角度との差分の累積値に基づいて、補正値を求めることを特徴とする三次元姿勢推定装置である。
【０００８】
かかる三次元姿勢角度推定装置であって、前記姿勢角度推定部は、前記ジャイロセンサーが検出した角速度から前記補正値算出部が求めた補正値を差し引いて三次元姿勢角度を推定することを特徴とする三次元姿勢推定装置である。
【００１１】
また、加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定方法であって、前記躯体の加速度を加速度センサーで検出することと、前記躯体が向いている方位を方位センサーで検出することと、前記躯体の角速度をジャイロセンサーで検出することと、前記加速度センサーが検出した加速度と前記方位センサーが検出した方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、前記躯体の前記三次元姿勢角度を推定することと、前記加速度センサーが検出した加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、を有し、前記三次元姿勢角度を推定する際に、加減速運動状態にないと判定した場合には、前記ジャイロセンサーが検出した角速度と求めた前記補正値とに基づいて、前記三次元姿勢角度を推定し、加減速運動状態にあると判定した場合には、前記ジャイロセンサーが検出した角速度と保持した前記補正値とに基づいて、前記三次元姿勢角度を推定し、前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定方法である。
この発明によれば、軽量な演算処理によって、三次元姿勢角度を推定することができ、加減速運動状態であるか否かの判定を高い精度で行うことができる。
【００１２】
また、加減速運動可能な躯体の三次元姿勢角度を推定する三次元姿勢推定プログラムであって、情報処理装置に、加速度センサーで検出した前記躯体の加速度を取得することと、方位センサーで検出した前記躯体が向いている方位を取得することと、ジャイロセンサーで検出した前記躯体の角速度を取得することと、前記加速度と前記方位とに基づいて、前記躯体の三次元姿勢角度の暫定値である暫定角度を求めることと、前記躯体の前記三次元姿勢角度を推定することと、前記加速度に基づいて、前記躯体が加減速運動状態にあるかないかを判定することと、加減速運動状態にないと判定した場合には、直近に推定した前記三次元姿勢角度と直近に求めた前記暫定角度とに基づいて補正値を求め、加減速運動状態にあると判定した場合には、加減速運動状態にないと判定した時点であって直近の時点において求めた前記補正値を保持することと、を実行させ、前記躯体が加減速運動状態にあるかないかを判定する際には、該角速度と該補正値に基づいて推定された前記三次元姿勢角度と、前記加速度センサーが検出する加速度と、に基づいて、重力加速度成分を除去した加速度を求め、前記重力加速度成分を除去した加速度が所定の大きさ未満であれば加減速運動状態でないと判定し、前記重力加速度成分を除去した加速度が所定の大きさ以上であれば加減速運動状態であると判定することを特徴とする三次元姿勢推定プログラムである。
この発明によれば、軽量な演算処理によって、三次元姿勢角度を推定することができ、加減速運動状態であるか否かの判定を高い精度で行うことができる。
【発明の効果】
【００１３】
本発明によれば、軽量な演算処理によって、算出された三次元姿勢角度に含まれる現実の三次元姿勢角度との誤差を推定することができる。また、加減速運動状態であるか否かの判定を高い精度で行うことができる。
【図面の簡単な説明】
【００１４】
【図１】第１実施形態の三次元姿勢角度推定装置１のブロック構成図である。
【図２】情報処理装置２のハードウェア構成を示す図である。
【図３】各機能部３１〜３４が実行する三次元姿勢推定アルゴリズムを示すブロック図である。
【図４】図３に示す三次元姿勢推定アルゴリズムを分かりやすく説明するためのフロー図である。
【図５】判定基準となる閾値の算出根拠となる考え方を示す概念図である。
【図６】三次元姿勢変動に加えて２０秒〜３０秒の期間に加速度外乱を印加したときにおいて、外乱加速度ａｄを推定する実験の結果を示すグラフである。
【図７Ａ】三次元姿勢角度推定装置１によって推定したＲｏｌｌ軸についての三次元姿勢角度の推定結果（実線で示す）とエンコーダーによる実測値（点線で示す）を対比して示す折れ線グラフである。
【図７Ｂ】三次元姿勢角度推定装置１によって推定したＰｉｔｃｈ軸についての三次元姿勢角度の推定結果（実線で示す）とエンコーダーによる実測値（点線で示す）を対比して示す折れ線グラフである。
【図７Ｃ】三次元姿勢角度推定装置１によって推定したＹａｗ軸についての三次元姿勢角度の推定結果（実線で示す）とエンコーダーによる実測値（点線で示す）を対比して示す折れ線グラフである。
【図８】判定基準となる閾値の算出根拠となる別の考え方を示す概念図である。
【発明を実施するための形態】
【００１５】
＝＝＝第１実施形態＝＝＝
本発明の第１実施形態である三次元姿勢角度推定装置１は、例えば、上下・左右・前後に加減速可能なロボット（「躯体」に相当）に装着され、三次元姿勢角度推定装置１が推定する三次元姿勢角度に基づいて、そのロボットの姿勢を制御することができる。
【００１６】
なお、第１実施形態の説明において用いる記号θ１、θ２、ａｍ、ｎｍ、ωｍ、Δθ、Δωｃ、ａｇｌｏｂａｌｄ、ｇｇｌｏｂａｌは、次の通りである。  
θ１＝［θ１ｘ  θ１ｙ  θ１ｚ］Ｔ：３次元姿勢角度ベクトル推定値
θ２＝［θ２ｘ  θ２ｙ  θ２ｚ］Ｔ：３次元姿勢角度ベクトル暫定値
ａｍ＝［ａｍｘ  ａｍｙ  ａｍｚ］Ｔ：３軸加速度センサーにより取得される加速度ベクトル
ｎｍ＝［ｎｍｘ  ｎｍｙ  ｎｍｚ］Ｔ：３軸方位センサーにより取得される方位ベクトル
ωｍ＝［ωｍｘ  ωｍｙ  ωｍｚ］Ｔ：３軸ジャイロセンサーにより取得される角速度ベクトル
Δθ＝［Δθｘ  Δθｙ  Δθｚ］Ｔ：３次元姿勢角度ベクトルにおける推定値と暫定値の差分
Δωｃ＝［Δωｃｘ  Δωｃｙ  Δωｃｚ］Ｔ：３軸角速度ベクトルに対する補正値ベクトル
ａｇｌｏｂａｌｄ＝［ａｇｌｏｂａｌｄｘ  ａｇｌｏｂａｌｄｙ  Δａｇｌｏｂａｌｄｚ］Ｔ：３軸加速度ベクトルにおける重量加速度以外の成分
ｇｇｌｏｂａｌ＝［０  ０  −１］Ｔ：慣性座標系での重力加速度ベクトル
【００１７】
図１は、本発明の第１実施形態である三次元姿勢角度推定装置１のブロック構成図である。同図に示すように、三次元姿勢角度推定装置１は、方位センサー２１、加速度センサー２２、ジャイロセンサー２３の各センサー群２１〜２３と、情報処理装置２とを備える。また、情報処理装置２は、暫定角度算出部３１、判定部３２、補正値算出部３３、姿勢角度推定部３４の各機能部３１〜３４を備える。
【００１８】
図２は、情報処理装置２のハードウェア構成を示す図である。情報処理装置２は、マイクロコンピューター等のコンピューターと周辺機器とが接続されることにより構成され、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）１０、メモリー１１、記憶手段１２、通信インターフェイス１３等を備えている。
【００１９】
ＣＰＵ１０は、記憶手段１２に記憶されているプログラムをメモリー１１に読み出して実行する。メモリー１１は、ＣＰＵ１０のプログラムを格納する領域や作業領域等を確保するためのものであって、例えばＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）等である。記憶手段１２は、例えばフラッシュメモリー等である。通信インターフェイス１３は、例えばシリアルポート等であって、各センサー群２１〜２３が検出した情報などを受信するとともに、ＣＰＵ１０が実行したプログラムの処理結果を外部に送信する。
【００２０】
なお、各機能部３１〜３４は、ＣＰＵ１０が記憶手段１２に記憶されたプログラムをメモリー１１に読み出して実行することにより実現される。
【００２１】
方位センサー２１は、地磁気に基づいていずれが北の方角であるかを検出する。加速度センサー２２は、ロボットの独立な３軸方向の加速度を検出する。ジャイロセンサー２３は、独立な３軸方向の角速度を検出する。
【００２２】
暫定角度算出部３１は、方位センサー２１が検出した方位と加速度センサー２２が検出した加速度とに基づいて、座標変換によってロボットの三次元姿勢角度θ２の暫定値である暫定角度を求める。
【００２３】
なお、暫定角度算出部３１が算出する三次元姿勢角度は、ロボットが加減速運動を行っている状態においては誤差を含むので、必ずしも信頼できる値ではないことから暫定値とする。この暫定値は、後述するように三次元姿勢角度を推定するための補正値の算出に用いる。
【００２４】
判定部３２は、加速度センサー２２が検出する加速度ａｍと、姿勢角度推定部３４が推定した三次元姿勢角度θ１とに基づいて、重力加速度成分を除去した加速度を求める。この重力加速度成分を除去した加速度は、暫定角度算出部３１の暫定角度の算出において外乱要因となるが、この重力加速度成分を除去した加速度（以下、「外乱加速度ａｄ」）が重力加速度ｇｇｌｏｂａｌの１／１０（「所定の大きさ」に相当）未満であれば加減速運動状態でないと判定する。また、外乱加速度ａｄが重力加速度ｇｇｌｏｂａｌの１／１０以上であれば加減速運動状態であると判定する。
【００２５】
より具体的には、外乱加速度ａｄは、姿勢角度推定部３４による三次元姿勢角度θ１の推定が良好に行われていることを前提とし、座標変換を施した重力加速度ベクトルを、加速度センサー２１が検出した加速度ベクトルａｍから差引くことにより得られる。そして、外乱加速度ａｄの大きさにおける重力加速度ｇｇｌｏｂａｌの１／１０未満であれば加減速運動状態でないと判定し、重力加速度ｇｇｌｏｂａｌの１／１０以上であれば加減速運動状態であると判定する。
【００２６】
補正値算出部３３は、判定部３２が加減速運動状態にないと判定した場合には、後述する姿勢角度推定部３４が直近に推定した三次元姿勢角度θ１と、暫定角度算出部３１が直近に求めた暫定角度θ２との差分Δθの累積値に基づいて、補正値Δωｃを求める。また、補正値算出部３３は、判定部３２が加減速運動状態にあると判定した場合には、判定部３２が加減速運動状態にないと判定した時点であって直近の時点において求めた補正値Δωｃを保持する。
【００２７】
より具体的には、補正値算出部３３は、判定部３２が加減速運動状態にないと判定した場合（ｒ＝１）には、三次元姿勢角度θ１と暫定角度θ２との慣性座標系での差分Δθをヤコビ行列によってボディ座標系での角速度に変換し、さらにこの角速度を積分項及び比例項によって変換することによってΔωｃを求める。ここで、積分項は、時間変動を伴う補正値に対して推定する三次元姿勢角度の追従性を向上する役割を持ち、三次元姿勢角度θ１が暫定角度θ２に収束した場合においては、積分項によって保持された補正値Δωｃにより、角速度の修正が継続される。また、比例項は差分Δθの収束性を確保する。このように、補正値算出部３３は、後述する姿勢角度推定部３４が推定する三次元姿勢角度θ１を、暫定角度θ２に収束させ、差分Δθ→０とする。つまり、補正値算出部３３は、三次元姿勢角度θ１は、暫定角度算出部３１が直近に求めた暫定角度θ２と等しくなるような補正値Δωｃを求める。
【００２８】
また、補正値算出部３３は、判定部３２が加減速運動状態にあると判定した場合（ｒ＝０）には、三次元姿勢角度θ１と暫定角度θ２との差分Δθにｒ＝０を乗じて、補正値Δωｃの算出への差分Δθの入力を遮断する。すなわち、補正値算出の積分項は、加減速運動状態にないと判定した時点の最後の時点に用いられた補正Δωｃを保持する。
【００２９】
姿勢角度推定部３４は、ジャイロセンサー２３が検出した角速度ωｍから補正値算出部３３が求めた補正値Δωｃを差し引き、座標変換と数値積分によってロボットの三次元姿勢角度θ１を推定する。
【００３０】
図３は、各機能部３１〜３４が実行する三次元姿勢推定アルゴリズムを示すブロック図であり、図４は、図３に示す三次元姿勢推定アルゴリズムを分かりやすく説明するためのフロー図である。以下、図４に沿って説明する。
【００３１】
まず、暫定角度算出部３１は、方位センサー２１が検出した方位と加速度センサー２２が検出した加速度とに基づいて、θ２＝ｈ（ａｍ，ｎｍ）によってロボットの三次元姿勢角度の暫定値である暫定角度θ２を求める（Ｓ１０２）。ここで、θ２ｘ、θ２ｙ、θ２ｚは、式（１）、式（２）、式（３）の通りである。  
0005419268.tif 000002
0005419268.tif 000003
【００３２】
次に、後述するＳ１２４で算出する三次元姿勢角度θ１と、暫定角度算出部３１が直近に求めた暫定角度θ２との差分Δθを算出する（Ｓ１０４）。
【００３３】
一方で、判定部３２は、加速度センサー２２が検出する加速度ａｍと、姿勢角度推定部３４が推定した三次元姿勢角度θ１とに基づいて、重力加速度成分ｇｇｌｏｂａｌを除去した外乱加速度ａｄｇｌｏｂａｌを求める（Ｓ１０６）。具体的には、ｇｇｌｏｂａｌと三次元姿勢角度θ１とを式（４）、式（５）で座標変換することによって算出する。  
0005419268.tif 000004
ここで、
0005419268.tif 000005
である。
【００３４】
外乱加速度ａｄが重力加速度ｇｇｌｏｂａｌの１／１０（「所定の大きさ」又は「ｘ」に相当）未満であれば加減速運動状態でないと判定し（Ｓ１０８：ＮＯ）、外乱加速度ａｄが重力加速度ｇｇｌｏｂａｌの１／１０以上であれば加減速運動状態であると判定する（Ｓ１０８：ＹＥＳ）。
【００３５】
ところで、図５は、判定基準となる閾値「重力加速度ｇｇｌｏｂａｌの１／１０」の算出根拠となる考え方を示す概念図である。ここで、外乱加速度ａｄを求める手順を説明する。まず、ロボット全体にかかる加速度ａｍはボディ座標系で得られる。また、重力加速度ｇは、慣性座標系では鉛直下向きで一定である。重力加速度ｇと、後述のＳ１２４において推定した三次元姿勢角度とに基づいて、式（６）によって座標変換すれば、ボディ座標系の重力加速度
0005419268.tif 000006
を推定できる。  
0005419268.tif 000007
したがって、式（７）により求められる外乱加速度の推定値の絶対値ｅ２が閾値以上であるか閾値未満であるかによって加減速運動状態であるか否かを判定できる。  
0005419268.tif 000008
【００３６】
図６は、三次元姿勢変動に加えて２０秒〜３０秒の期間に加速度外乱を印加したときにおいて、式（４）〜（７）によって外乱加速度の推定値の重力加速度に対する比率を測定する実験の結果を示すグラフである。同図に示す線分が外乱加速度の推定値の絶対値ｅ２の重力加速度に対する比率である。同図に示すように、推定された外乱加速度の絶対値ｅ２が重力加速度の１／１０以上であるときには加速度状態にあると判定し、重力加速度の１／１０未満であるときには加速度状態にないと判定すると、２０秒〜３０秒の期間のみ加速度状態にあった実際のロボットの動きと概ね符合することが分かる。すなわち、重力加速度の１／１０が適切な閾値であることが確認できた。
【００３７】
さて、加減速運動状態でないと判定した場合（Ｓ１０８：ＮＯ）には、後述する補正に用いる係数ｒ＝１とし（Ｓ１１０）、加減速運動状態であると判定した場合（Ｓ１０８：ＹＥＳ）には、後述する補正に用いる係数ｒ＝０とする（Ｓ１１２）。
【００３８】
Ｓ１１０又はＳ１１２において求めた係数ｒに差分Δθを乗じ（Ｓ１１４）、その乗じた値Δθ・ｒを角速度へ変換し（Ｓ１１６）、補正値Δωｃを算出する。なお、ここで判定部３２が加減速運動状態にないと判定した場合（Ｓ１０８：ＮＯ）にはｒ＝１であるので差分Δθに応じて積分計算などにより補正値Δωｃは順次更新されることとなる。一方で、加減速運動状態であると判定した場合（Ｓ１０８：ＹＥＳ）にはｒ＝０であるので補正値Δωｃは更新されず、加減速運動状態にないと判定した場合（Ｓ１０８：ＮＯ）であって直近の時点において算出された補正値Δωｃが保持されることとなる。
【００３９】
ジャイロセンサー２３が検出した角速度ωｍと補正値算出部３３が求めた補正値Δωｃとに基づいて、次式（８）によりωを求める（Ｓ１２０）。  
0005419268.tif 000009
そして、それを次式（９）により、
0005419268.tif 000010
θ１を慣性座標系の角速度に座標変換する（Ｓ１２２）。なお、ｆ（θ１）は次式（１０）に示す通りである。  
0005419268.tif 000011
【００４０】
更に数値積分することによってロボットの三次元姿勢角度θ１を推定する（Ｓ１２４）。なお、推定した三次元姿勢角度θ１は、Ｓ１０４の差分算出に用いる。
【００４１】
以上、本実施形態の三次元姿勢角度推定装置１によれば、軽量な演算処理によって、三次元姿勢角度を推定することができる。
【００４２】
従来技術においては、理想環境下での特定の最適性が理論証明されるものの、モデルパラメーターの変動に対する鋭敏性、実装時の動作調整に大きな労力を要し、更には演算負荷が高く高周期推定に向かないなどの課題があった。しかし、本実施形態の三次元姿勢角度推定装置１によれば、モデルパラメーターの変動に対する追従性は高く、軽量な演算処理であるので実装時の動作調整にかかる労力は少なく、演算負荷が小さく高周期推定も可能である。
【００４３】
具体的には、ジャイロセンサー２３と、加速度センサー２２及び方位センサーとによる独立な２系統の姿勢情報が得られ、これらの姿勢情報により加速度外乱の検知状況に応じて、それらを統合処理して三次元姿勢角度を推定できる。本発明の特徴は、累積誤差の要因となるジャイロセンサー２３が検出した角速度に係るジャイロバイアス項の継続的な推定・除去機能を、構造の簡単なフィードバックにより達成することができる。そして、これにより、ジャイロセンサーが検出した角速度に含まれるバイアス値の実時間かつ継続的な推定・除去能力を確保し、その上で、加速度外乱の有無を判定する判定部を設けて、２系統の姿勢角度情報を統合する。こうしたフィードバック構造を基本とした統合処理により、累積誤差の除去と動加速度外乱の影響軽減化の機能が得られ、従来技術において適用することが前提となっていたカルマンフィルター（Ｋａｌｍａｎ Ｆｉｌｔｅｒ）などによる処理と比較して、極めて軽量な演算処理によって三次元姿勢角度を推定することができる。
【００４４】
ところで、本実施形態の三次元姿勢角度推定装置１によれば、適切に三次元姿勢角度を推定できることを確認するため、次のような実験を行った。本実験では、振幅及び周波数が異なる正弦波動作をロール軸、ピッチ軸、ヨー軸の３軸に与え、さらに動作中の特定期間（２０〜３０秒）においては、並進軸での正弦波動作によって加速度外乱を加える。このような実験において、図７Ａは、本実施形態の三次元姿勢角度推定装置１によって推定したロール軸についての三次元姿勢角度の推定結果（実線で示す）とエンコーダーによる実測値（点線で示す）を対比して示す折れ線グラフである。また、図７Ｂは同様にピッチ軸について、図７Ｃは同様にヨー軸について、それぞれ推定結果と実測値を対比して示す折れ線グラフである。図７Ａ〜図７Ｃに示すように、三次元姿勢角度推定装置１によれば、いずれも全試験期間（１０〜４０秒）において推定結果と実測値がほぼ同じ値となっていることから、良好な推定結果を得られることが確認できた。
【００４５】
また、判定部３２は、加速度センサー２２が検出する加速度と、姿勢角度推定部３４が推定した三次元姿勢角度とに基づいて、加減速運動状態であるか否かを判定することで、判定を高い精度で行うことができる。
【００４６】
＝＝＝その他の実施の形態＝＝＝
以上、本発明の実施形態について説明したが、本発明は、かかる実施形態に限定されるものではなく、その要旨を逸脱しない範囲で以下に示すような変形が可能である。
【００４７】
例えば、本実施形態の三次元姿勢角度推定装置１はロボットの姿勢制御に用いることとしたが、これに限られず、例えば、ウェアラブルモーションキャプチャーや、小型無人航空機、ヘッドマウントディスプレイ、ゲーム機の操作部等にも用いることができる。
【００４８】
また、補正値算出部３３は、判定部３２が加減速運動状態にないと判定した場合（ｒ＝１）には、三次元姿勢角度θ１と暫定角度θ２との慣性座標系での差分Δθをヤコビ行列によってボディ座標系での角速度に変換することとしたが、鉛直軸近傍にて姿勢変動を行う物体の３次元姿勢角度推定には、ヤコビ行列を使用しなくともよい。
【００４９】
これにより、演算処理はさらに軽減される一方で、鉛直軸近傍にて姿勢変動を行う場合には、演算結果に大きな差異は生じない。
【００５０】
また、例えば、判定部３２は、加速度センサー２２が検出する加速度と、姿勢角度推定部３４が推定した三次元姿勢角度とに基づいて、加減速運動状態であるか否かを判定することとしたが、加速度センサー２２が検出する加速度のみに基づいて、加減速運動状態であるか否かを判定してもよい。
【００５１】
図８は、判定基準となる閾値「重力加速度ｇｇｌｏｂａｌの１／１０」の算出根拠となる別の考え方を示す概念図である。ここで、外乱加速度ａｄの影響度ｅ１を求める手順を説明する。まず、ロボット全体にかかる加速度ａｍはボディ座標系で得られる。また、重力加速度ｇの大きさは、どの座標系でも一定値である。したがって、次式（１１）により、外乱加速度の影響度ｅ１を求めることができる。  
0005419268.tif 000012
そして、この値が重力加速度の１／１０以上であれば加速度状態にあると判定でき、重力加速度の１／１０未満であれば加速度状態にないと判定できる。
【００５２】
このような判定によれば、加減速運動状態であるか否かの判定を簡便に行うことができる。
【符号の説明】
【００５３】
１  三次元姿勢角度推定装置
２  情報処理装置
１０  ＣＰＵ
１１  メモリー
１２  記憶手段
１３  通信インターフェイス
２１  方位センサー
２２  速度センサー
２３  ジャイロセンサー
３１  暫定角度算出部
３２  判定部
３３  補正値算出部
３４  姿勢角度推定部
【図１】
0005419268.tif 000013
【図２】
0005419268.tif 000014
【図３】
0005419268.tif 000015
【図４】
0005419268.tif 000016
【図５】
0005419268.tif 000017
【図６】
0005419268.tif 000018
【図７Ａ】
0005419268.tif 000019
【図７Ｂ】
0005419268.tif 000020
【図７Ｃ】
0005419268.tif 000021
【図８】
0005419268.tif 000022