JP7215965B2 - 姿勢推定装置、姿勢推定方法、および姿勢推定プログラム - Google Patents

Description

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

従来、角速度および加速度を計測することが可能なＩＭＵセンサ(Inertial Measurement Unit：慣性計測センサ)を身体に複数個取り付けることで、身体姿勢とその変化（運動）を推定する技術（モーションキャプチャ）が開示されている（例えば、非特許文献１参照）。一般に、ＩＭＵセンサで計測した角速度はノイズやバイアス誤差が含まれており、単に積分するだけでは正確な姿勢を計算することができない。そこで、多くの姿勢推定技術では、加速度を用いてロール軸方向およびピッチ軸方向の補正を行うことで推定精度を向上させている。

S. O. H. Madgwick, A. J. L. Harrison, and R. Vaidyanathan, "Estimation of IMU and MARG orientation using a gradient descent algorithm" in 2011 IEEE International Conference on Rehabilitation Robotics, 2011

しかしながら、従来技術の手法では、加速度のみではヨー方向の補正を行うことができない。このため、既存のＩＭＵセンサによるモーションキャプチャシステムでは、角速度および加速度に加え、地磁気計で方位を計測し、ヨー方向の補正を行うことが一般的である。しかし、地磁気は金属でできた建物や家具などの影響を受けて乱れやすいため、ヨー方向補正が正常に動作する環境は限られている。特に長時間計測においては、地磁気の乱れはヨー方向推定精度に大きな影響を及ぼす。ヨー方向の誤差が大きくなると、各身体部位（セグメント）がそれぞれ異なる方向に捻じれた状態になり、人の全身姿勢として不自然なものになる。従来技術の手法では、計測した後の処理で不自然な関節変位や姿勢を修正する機能はあるが、処理に時間がかかるうえ、リアルタイムに修正することはできない場合があった。

本発明は、このような事情を考慮してなされたものであり、より正確な姿勢推定ができる姿勢推定装置、姿勢推定方法、および姿勢推定プログラムを提供することを目的の一つとする。

この発明に係る姿勢推定装置、姿勢推定方法、および姿勢推定プログラムは、以下の構成を採用した。
（１）：この発明の一態様に係る姿勢推定装置は、人、動物、または関節の可動域が制限されたロボットである推定対象の複数の箇所に取り付けられ、角速度および加速度を検出する複数のセンサから、角速度および加速度の情報を取得する取得部と、前記取得部により取得された情報を、センサ座標系から基準座標系の情報に変換する変換部と、前記変換された前記角速度を積分することで、前記推定対象において前記センサが取り付けられた参照部位の向きを前記推定対象の姿勢の一部として算出する積分部と、前記推定対象に含まれる基準部位を通る代表平面を想定し、前記代表平面の法線と、前記積分部により算出される前記参照部位の向きとが直交する方向に近づくように、前記参照部位の前記変換された角速度を補正する補正部と、を備える姿勢推定装置である。

（２）：上記（１）の態様において、前記積分部および前記補正部の処理は、処理サイクルごとに繰り返し実行され、前記補正部は、前回処理サイクルにおいて前記積分部により算出された前記参照部位の向きが前記代表平面の法線に対して直交する向きから乖離することが継続するのに応じて、前記参照部位の前記変換された角速度を補正する度合いを大きくするものである。

（３）：上記（１）または（２）の態様において、前記補正部は、前記代表平面の法線と、前記積分部により算出される前記参照部位の向きとの内積を小さくするように、前記参照部位の前記変換された角速度を補正するものである。

（４）：上記（１）から（３）のいずれかの態様において、前記補正部は、更に、前記積分部により算出された前記参照部位の向きから導出される前記参照部位ごとの想定重力加速度方向と、前記取得部により取得された加速度の情報に基づいて認識される前記参照部位ごとの計測重力加速度方向との乖離が小さくなるように、前記参照部位の前記変換された角速度を補正するものである。

（５）：この発明の一態様に係る姿勢推定方法は、コンピュータが、人、動物、または関節の可動域が制限されたロボットである推定対象の複数の箇所に取り付けられ、角速度および加速度を検出する複数のセンサから、角速度および加速度の情報を取得し、取得された情報を、センサ座標系から基準座標系の情報に変換し、変換された前記角速度を積分することで、前記推定対象において前記センサが取り付けられた参照部位の向きを前記推定対象の姿勢の一部として算出し、前記推定対象に含まれる基準部位を通る代表平面を想定し、前記代表平面の法線と、算出された前記参照部位の向きが直交する方向に近づくように、前記参照部位の前記変換された角速度を補正する、姿勢推定方法である。

（６）：この発明の一態様に係る姿勢推定プログラムは、コンピュータに、人、動物、または関節の可動域が制限されたロボットである推定対象の複数の箇所に取り付けられ、角速度および加速度を検出する複数のセンサから、角速度および加速度の情報を取得させ、取得された情報を、センサ座標系から基準座標系の情報に変換させ、変換された前記角速度を積分することで、前記推定対象において前記センサが取り付けられた参照部位の向きを前記推定対象の姿勢の一部として算出させ、前記推定対象に含まれる基準部位を通る代表平面を想定させ、前記代表平面の法線と、算出された前記参照部位の向きとが直交する方向に近づくように、前記参照部位の前記変換された角速度を補正させる、姿勢推定プログラムである。

請求項１～６に記載の発明によれば、ＩＭＵセンサが磁気の影響を受けやすい環境でも、より正確に姿勢推定することができる。

また、請求項２～４に記載の発明によれば、前記被測定対象が一部のセグメントを前記ヨー角方向に対して回転させた状態が継続している場合、前記回転が解消されると仮定した補正をすることができる。

実施形態の姿勢推定装置１００の構成図である。 ＩＭＵセンサについて説明するための図である。 補正部１４０による平面想定処理を説明するための図である。 補正部１４０による方向ベクトルｖｉの定義処理について説明するための図である。 方向ベクトルｖｉが旋回した様子を示す図である。 姿勢推定装置１００による補正処理の概要を説明するための図である。 全身補正量計算部１４４の構成の一例を示す図である。 全身補正量計算部１４４の構成の他の一例を示す図である。 全身補正量計算部１４４の演算処理の流れを模式的に示す図である。 全身補正量計算部１４４の演算処理の流れを模式的に示す図である。 全身補正量計算部１４４の演算処理の流れを模式的に示す図である。 全身補正量計算部１４４の演算処理の流れを模式的に示す図である。 姿勢推定装置１００を用いたモーションキャプチャ処理の流れの一例を示すフローチャートである。 姿勢推定装置１００による姿勢推定処理の流れの一例を示すフローチャートである。

以下、図面を参照し、本発明の姿勢推定装置、姿勢推定方法、および姿勢推定プログラムの実施形態について説明する。

〔全体構成〕
図１は、実施形態の姿勢推定装置１００の構成図である。姿勢推定装置１００は、例えば、取得部１１０と、変換部１２０と、積分部１３０と、補正部１４０と、記憶部１５０とを備える。姿勢推定装置１００の構成要素（記憶部１５０を除く）は、例えば、ＣＰＵ（Central Processing Unit）等のプロセッサがプログラムを実行することにより実現される。また、これらのうち一部または全部は、ＬＳＩ（Large Scale Integration）やＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）等のハードウェアによって実現されてもよいし、ソフトウェアとハードウェアの協働によって実現されてもよい。

取得部１１０、変換部１２０、積分部１３０、および補正部１４０による処理は、処理サイクルごとに繰り返し実行される。例えば、補正部１４０は、ひとつ前の処理サイクルの積分部１３０による処理結果に基づいて補正量を導出し、今回の処理サイクルの積分部１３０に反映する。

取得部１１０は、例えば、人、動物、または関節の可動域が制限されたロボットである推定対象の複数の箇所に取り付けられ、角速度および加速度を検出する複数のＩＭＵセンサから、角速度および加速度の情報を取得する。取得部１１０は、例えば、有線または無線で通信可能に接続されており、ＩＭＵセンサと通信する。取得部１１０は、取得結果を変換部１２０に出力する。

図２は、ＩＭＵセンサについて説明するための図である。推定対象ＴＧＴが人である場合、人の頭部、胸部、骨盤周辺、左右の手足などの複数の箇所にＩＭＵセンサＪＳ―１～ＪＳ―Ｎ（ＮはＩＭＵセンサの総数）が取り付けられる。以下、いずれのＩＭＵセンサであるかを特に区別しない場合はハイフン以下の符号を省略し、単にＩＭＵセンサＪＳと称する。また、１～Ｎのいずれかという意味で引数iを採用し、ＩＭＵセンサＪＳ－ｉなどと称する。ＩＭＵセンサＪＳは、例えば、着脱可能な衣服のような部材（ＩＭＵスーツ）に取り付けられており、推定対象ＴＧＴがその部材を着用することで、所望の位置にＩＭＵセンサＪＳが位置するようになっている。ＩＭＵセンサＪＳのそれぞれは、角速度および加速度をそれぞれ３軸で計測することができる。

例えば、ＩＭＵセンサＪＳ－１が右肩、ＩＭＵセンサＪＳ－２が右上腕、ＩＭＵセンサＪＳ－８が左大腿、ＩＭＵセンサＪＳ－９が左膝下、というように、ＩＭＵセンサＪＳが配置される。また、ＩＭＵセンサＪＳ―ｐは、基準部位となる部位の周辺に取り付けられる。基準部位とは、例えば、推定対象が人である場合、その人の骨盤など体幹部の一部が該当する。以下の説明において、一以上のＩＭＵセンサＪＳが取り付けられ、その動きが計測される対象の部位のことを「セグメント」と称する。セグメントには、基準部位と基準部位以外のセンサ取り付け部位（以下、参照部位と称する）が含まれる。なお、ＩＭＵスーツには、ＩＭＵセンサＪＳに加えて、さらに心拍センサＨＲＳ、温度センサＴＳなどの姿勢推定の参考情報を取得する種々のセンサが取り付けられてもよい。

以下の説明において、ＩＭＵセンサＪＳ―１～ＪＳ－Ｎのそれぞれに対応する構成要素には、符号にハイフン以下の符号を付けて説明する。

図１に戻り、変換部１２０は、取得部１１０により取得された情報を、ＩＭＵセンサＪＳのそれぞれにおける３軸方向の座標系（以下、センサ座標系と称する）から基準座標系の情報に変換する。基準座標系は、例えば、重力方向を一つの軸とする地上座標系である。変換部１２０は、変換結果を補正部１４０に出力する。また、変換部１２０は、補正部１４０を介して積分部１３０に変換結果を出力する。

変換部１２０は、例えば、各セグメントに対応するセグメント角速度計算部１２２－ｉと、加速度集約部１２４と、を備える。セグメント角速度計算部１２２－ｉは、取得部１１０により出力されたＩＭＵセンサＪＳ―ｉの角速度を、基準座標系の情報に変換する。セグメント角速度計算部１２２－ｉによる処理結果（ＩＭＵセンサＪＳの検出結果に基づくものであり、推定対象ＴＧＴの姿勢を表す情報）は、例えば、クオータニオンの形式で保持される。なお、ＩＭＵセンサＪＳ―ｉの計測結果をクオータニオンの形式で表現するのはあくまで一例であり、３次元の回転群ＳＯ３の回転行列など他の表現方法が用いられてもよい。

加速度集約部１２４は、セグメントに対応するＩＭＵセンサＪＳ―ｉの検出した加速度のそれぞれを集約する。加速度集約部１２４は、集約結果を、推定対象ＴＧＴの全身の加速度（以下、全ＩＭＵ加速度と称する場合がある）に変換する。

積分部１３０は、セグメント角速度計算部１２２－ｉにより基準座標系の情報に変換されたセグメントに対応する角速度を積分することで、推定対象ＴＧＴにおいてＩＭＵセンサＪＳ―ｉが取り付けられたセグメント（参照部位）の向きを、推定対象の姿勢の一部として算出する。積分部１３０は、積分結果を補正部１４０および記憶部１５０に出力する。

なお、積分部１３０には、処理サイクルが初回である場合には、変換部１２０により出力された角速度（補正部１４０による補正が行われていない角速度）が入力されるが、それ以降は後述する補正部１４０により前回処理サイクルにおける処理結果に基づいて導出された補正が反映された角速度が入力される。

積分部１３０は、例えば、各セグメントに対応する角速度積分部１３２－ｉを備える。角速度積分部１３２－ｉは、セグメント角速度計算部１２２－ｉにより出力されたセグメントの角速度を積分することで、推定対象においてＩＭＵセンサＪＳ－ｉが取り付けられた参照部位の向きを、推定対象の姿勢の一部として算出する。

補正部１４０は、推定対象に含まれる基準部位を通る代表平面を想定し、代表平面の法線と、積分部１３０により算出される参照部位の向きとが直交する方向に近づくように、参照部位の変換された角速度を補正する。代表平面については後述する。

補正部１４０は、例えば、推定姿勢集約部１４２と、全身補正量計算部１４４と、補正量分解部１４６と、各セグメントに対応する角速度補正部１４８－ｉとを備える。

推定姿勢集約部１４２は、角速度積分部１３２―ｉによる計算結果である各セグメントの姿勢を表すクオータニオンを一つのベクトルに集約するものである。集約したベクトルを推定全身姿勢ベクトルと呼ぶ。

全身補正量計算部１４４は、加速度集約部１２４により出力された全ＩＭＵ加速度と、推定姿勢集約部１４２により出力された推定全身姿勢ベクトルとに基づいて、すべてのセグメントの角速度の補正量を計算する。なお、全身補正量計算部１４４により計算される補正量は、推定対象の全身姿勢として不自然とならないように、セグメントのそれぞれの関係性が考慮されて調整されたものである。全身補正量計算部１４４は、計算結果を補正量分解部１４６に出力する。

補正量分解部１４６は、全身補正量計算部１４４により計算された補正量を、それぞれのセグメントの角速度に反映できるよう、セグメント毎の角速度の補正量に分解する。補正量分解部１４６は、分解したセグメント毎の角速度の補正量を、対応するセグメントの角速度補正部１４８－ｉに出力する。

角速度補正部１４８－ｉは、セグメント角速度計算部１２２－ｉにより出力された、対応するセグメントの角速度の計算結果に、補正量分解部１４６により出力された、対応付いたセグメントの角速度の補正量の分解結果を反映する。これにより、次サイクルの処理において積分部１３０の処理対象が積分する対象が、補正部１４０による補正が反映された状態の角速度になる。

角速度補正部１４８－ｉは、変換部１２０により出力された対応付いたセグメントの角速度に、補正量分解部１４６による分解結果を反映することで補正する。角速度補正部１４８－ｉは、補正結果を、角速度積分部１３２－ｉに出力する。

記憶部１５０は、例えば、ＨＤＤ（Hard Disc Drive）、フラッシュメモリ、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）、ＲＯＭ（Read Only Memory）、またはＲＡＭ（Random Access Memory）等を備え、ファームウェアやアプリケーションプログラム等、姿勢推定装置１００が備えるＣＰＵ等のプロセッサが実行するための各種プログラムやプロセッサが実行した処理の結果等を記憶する。記憶部１５０は、例えば、積分部１３０による積分結果であるセグメント毎の姿勢の推定結果や、ＩＭＵセンサ情報１５２などを保持する。ＩＭＵセンサ情報１５２は、例えば、複数のＩＭＵセンサＪＳが取り付けられるセグメントに関する情報や、キャリブレーションプログラムなどである。

なお、積分部１３０による積分結果であるセグメント毎の姿勢の推定結果は、他装置（出力ディスプレイや、姿勢推定結果を用いた演算を行う装置など）に出力されてもよい。

図３は、補正部１４０による平面想定処理を説明するための図である。補正部１４０は、図３の左図のように、基準部位が推定対象の骨盤である場合、骨盤中央を通る正中矢状面（図中のSagittal plane）を代表平面として想定する。正中矢状面とは、左右相称な推定対象の体の正中に対し平行に、体を左右に分ける面のことである。さらに、補正部１４０は、図３の右図のように、想定した正中矢状面の法線ｎ（図中の矢印Normal vector）を設定する。

図４は、補正部１４０による方向ベクトルｖｉの定義処理について説明するための図である。補正部１４０は、あるＩＭＵセンサＪＳ－ｉの出力を初期状態とし、その向きを、水平且つ代表平面に平行な向きであると定義する（キャリブレーション処理）。その後、ＩＭＵセンサＪＳ－ｉの出力を積分して得られる三方向の回転に沿って方向ベクトルは三方向に旋回する。

図４に示すように、推定対象ＴＧＴの参照部位に、胸部、左右の大腿部、および左右の膝下部が含まれる場合、補正部１４０は、キャリブレーション処理の結果に基づいてＩＭＵセンサＪＳの取り付け姿勢の推定を行い、法線ｎと積分部１３０により算出される参照部位の向きとが直交する方向に近づくように、参照部位の変換された角速度をそれぞれ補正し、図示のように参照部位の向く方向ベクトルｖ１～ｖ５（図中のForward vector）を導出する。図示のように、方向ベクトルｖ１は胸部の方向ベクトルを示し、方向ベクトルｖ２およびｖ３は大腿部の方向ベクトルを示し、方向ベクトルｖ４およびｖ５は膝下部の方向ベクトルを示すものである。なお、図中のｘ軸、ｙ軸、ｚ軸は、基準座標系の方向の一例である。

図５は、推定対象の姿勢変化によって方向ベクトルｖｉが旋回した様子を示す図である。代表平面は、ある基準部位のＩＭＵセンサＪＳ－ｐの出力を初期状態とした場合、ＩＭＵセンサＪＳ－ｐの出力を積分して得られるヨー方向の変位に沿ってヨー方向に旋回する。補正部１４０は、前回サイクルにおいて積分部１３０により算出された参照部位の向きが正中矢状面の法線ｎに対して直交する向きから乖離することが継続するのに応じて、参照部位の変換された角速度を補正する度合いを大きくする。

〔姿勢の推定〕
補正部１４０は、例えば、図４に示すように参照部位の方向ベクトルｖｉと法線ｎとの内積が０である場合に、参照部位の向きが正中矢状面の法線ｎに対して直交する向きから乖離していないホームポジションの姿勢であると判定する。補正部１４０は、図５に示すように方向ベクトルｖｉと法線ｎとの内積が０より大きい場合に、参照部位の向きが正中矢状面の法線ｎに対して直交する向きから乖離していると判定する。ホームポジションとは、推定対象ＴＧＴにＩＭＵセンサＪＳを取り付けた後のキャリブレーション処理の結果として取得された、推定対象ＴＧＴの基本的な体勢（但し、代表平面に対する相対的なもの）のことであり、例えば、静止直立状態である。従って、キャリブレーション処理とは、推定対象ＴＧＴに所定の動作（キャリブレーション動作）を行わせた結果得られたＩＭＵセンサＪＳの計測結果に基づいて、ホームポジションの定義を行う処理のことである。

これにより、補正部１４０は、推定対象が正中矢状面の法線ｎに対して直交する向きから乖離した姿勢（すなわち、図５の左図に示すように体を捻った状態）が長時間継続したり、正中矢状面の法線ｎに対して直交する向きから乖離した姿勢を保ったままで運動したりすることが稀であるという仮定に基づいて、時間が経過すれば乖離が小さくなること（例えば、図５の右図に示すようなホームポジションに近づくこと）を反映した補正を行う。

図６は、姿勢推定装置１００による補正処理の概要を説明するための図である。姿勢推定装置１００は、推定対象ＴＧＴの骨盤と、それ以外のセグメントで異なる最適化問題を定義する。まず、姿勢推定装置１００は、推定対象ＴＧＴの骨盤姿勢を計算し、骨盤姿勢を利用してその他のセグメント姿勢を計算する。

仮に、骨盤姿勢の計算と、骨盤以外の他のセグメントの姿勢の計算を別々に解くと、骨盤姿勢は重力補正のみを用いた推定となってしまう。姿勢推定装置１００では、その他のセグメント姿勢も考慮して骨盤姿勢の推定を行うことができるよう、骨盤姿勢の推定とその他のセグメント姿勢の推定を同時に行い、全てのＩＭＵセンサＪＳ同士の影響を考慮した最適化を目指すものである。

〔演算例〕
以下、姿勢の推定時の具体的な演算例について、図示した数式に沿って説明する。

姿勢を表現するためのクオータニオンの表現方法について説明する。ある座標系ｆｒａｍｅ Ａからｆｒａｍｅ Ｂまでの回転をクオータニオンで表すと以下の数式（１）のようになる。ただし、ｆｒａｍｅ Ｂはｆｒａｍｅ Ａに対して正規化された軸の回りにθだけ回転している。

なお、以下の説明において、クオータニオンｑにハット記号を付したもの（回転を表現する単位クオータニオン）を「ｑ（ｈ）」と示して説明する。単位クオータニオンとは、クオータニオンをノルムで除算したものである。ｑ（ｈ）は式（１）に示すような４つの実数値要素を持つ列ベクトルである。この表現方法を用いて、推定対象ＴＧＴの推定全身姿勢ベクトルＱを示すと、以下の数式（２）のように示すことができる。

なお、^Ｓ _Ｅｑ（ｈ）_ｉ（ｉはセグメントを示す１～Ｎの整数、または基準位置を示すｐ）は、参照部位のＩＭＵセンサＪＳの座標系Ｓ（セグメント座標系）における基準位置から、基準座標位置Ｅ（例えば、地球の重力方向から定義可能な座標系）までの回転をクオータニオンで表すものである。この推定対象ＴＧＴの推定全身姿勢ベクトルＱは、全てのセグメントの姿勢を表す単位クオータニオンを１つに集約した、４（Ｎ＋１）つの実数値要素を持つ列ベクトルである。

推定対象ＴＧＴの姿勢を推定するため、まずは、ＩＭＵセンサＪＳが取り付けられたある１つのセグメントの姿勢推定について考える。

数式（３）は、最適化問題の更新式の一例であり、数式（４）に示す関数の導出結果のノルムの１／２の最小値を導出することにより、ロール・ピッチ方向の補正量を導出するための式である。数式（４）の右辺は、センサ座標系で表現した推定姿勢から求めた基準のあるべき方向（例えば、重力や地磁気などの方向）を示す情報から、センサ座標系で表現したＩＭＵセンサＪＳが計測した基準の方向を減算する式である。

数式（５）に示すように、単位クオータニオン^Ｓ _Ｅｑ（ｈ）を行列形式で表現した一例である。また、数式（６）に示すように、^Ｅｄ（ｈ）はヨー方向を補正するために用いる基準方向（例えば、重力や地磁気などの方向）を示すベクトルである。また、数式（７）に示すように、^Ｓｓ（ｈ）は、センサ座標系で表現したＩＭＵセンサＪＳが計測した基準の方向を示すベクトルである。

なお、重力を基準に使う場合、数式（６）および数式（７）は、以下の数式（８）および数式（９）のように示すことができる。ａ_ｘ、ａ_ｙ、ａ_ｚのそれぞれは、ｘ軸方向の加速度、ｙ軸方向の加速度、およびｚ軸方向の加速度を示す。

^Ｅｄ（ｈ）＝〔０ ０ ０ １〕 …（８）
^Ｓｓ（ｈ）＝〔０ ａ_ｘ ａ_ｙ ａ_ｚ〕…（９）

数式（３）に示した関係式は、例えば、勾配降下法により解くことができる。その場合、推定姿勢の更新式は、数式（１０）で示すことができる。また、目的関数の勾配は、以下の数式（１１）を用いて示す。また、勾配を示す数式（１１）は、数式（１２）に示すように、ヤコビアンを用いて計算することができる。なお、数式（１２）に示すヤコビアンは、重力誤差項とヨー方向誤差項を全身の方向ベクトルｖｉの各要素で偏微分した行列である。重力誤差項とヨー方向誤差項については後述する。

数式（１０）の右辺に示すように、単位クオータニオン^Ｓ _Ｅｑ（ｈ）_ｋ＋１は、現在の推定姿勢を示す単位クオータニオン^Ｓ _Ｅｑ（ｈ）_ｋから係数μ（１以下の定数）と勾配の積を減算することで導出することができる。また、数式（１１）および数式（１２）に示すように、勾配は比較的少ない計算量で導出することができる。

なお、重力を基準に使う場合の数式（４）および数式（１２）の実際の計算例を、以下の数式（１３）および数式（１４）に示す。

上図の数式（３）～（７）、および数式（１０）～（１２）を用いて示した方法では、サンプリングごとに、更新式を1回計算することにより、姿勢の推定が可能となる。また、数式（８）、（９）、（１３）、（１４）に挙げたように重力を基準に使う場合には、ロール軸方向、ピッチ軸方向の補正ができる。

〔全身補正量計算〕
以下、推定姿勢に対する全身補正量（特にヨー方向の補正量）を導出する方法について説明する。図７は、全身補正量計算部１４４の構成の一例を示す図である。全身補正量計算部１４４は、例えば、ヨー方向誤差項計算部１４６ａと、重力誤差項計算部１４６ｂと、目的関数計算部１４６ｃと、ヤコビアン計算部１４６ｄと、勾配計算部１４６ｅと、補正量計算部１４６ｆとを備える。

ヨー方向誤差項計算部１４６ａは、推定した全身の姿勢から、ヨー角方向の補正を実現するためのヨー方向誤差項を計算する。

重力誤差項計算部１４６ｂは、推定した全身の姿勢と、ＩＭＵセンサＪＳで検出した加速度からロール軸方向およびピッチ軸方向の補正を実現するための重力誤差項を計算する。

目的関数計算部１４６ｃは、推定した全身の姿勢と、ＩＭＵセンサＪＳで検出した加速度と、ヨー方向誤差項計算部１４６ａの計算結果と、重力誤差項計算部１４６ｂの計算結果とに基づいて、推定対象ＴＧＴの正中矢状面と方向ベクトルｖｉが平行となるように補正するための目的関数を計算する。なお、重力誤差項とヨー方向誤差項の二乗和を目的関数とする。目的関数の詳細については後述する。

ヤコビアン計算部１４６ｄは、推定した全身の姿勢と、ＩＭＵセンサＪＳで検出した加速度から推定全身姿勢ベクトルＱの偏微分で求めたヤコビアンを計算する。

勾配計算部１４６ｅは、目的関数計算部１４６ｃによる計算結果と、ヤコビアン計算部１４６ｄによる計算結果とを用いて、最適化問題の解を導出して、勾配を計算する。

補正量計算部１４６ｆは、勾配計算部１４６ｅの計算結果を用いて推定対象ＴＧＴの推定全身姿勢ベクトルＱに対して適用する全身補正量を導出する。

図８は、全身補正量計算部１４４の構成の他の一例を示す図である。図８に示す全身補正量計算部１４４は、正中矢状面と各セグメントの方向ベクトルｖｉとを用いて全身補正量を導出するものであり、図７に示す構成要素に加えて、代表平面法線計算部１４６ｇと、セグメントベクトル計算部１４６ｈとをさらに備える。

代表平面法線計算部１４６ｇは、全身推定姿勢に基づいて、代表平面である正中矢状面の法線ｎを計算する。セグメントベクトル計算部１４６ｈは、全身推定姿勢に基づいて、セグメントの方向ベクトルｖｉを計算する。

［全身補正量の導出例〕
以下、全身補正量の導出例について説明する。

ヨー方向誤差項計算部１４６ａは、以下に示す数式（１５）を用いて、正中矢状面とセグメントの方向ベクトルが平行となるように補正するためのヨー方向誤差項ｆ_ｂの内積計算を行う。

ヨー方向誤差項ｆ_ｂは、セグメントｉの推定姿勢を示す単位クオータニオン^Ｓ _Ｅｑ（ｈ）_ｉと、基準部位である骨盤の推定姿勢を示す単位クオータニオン^Ｓ _Ｅｑ（ｈ）_ｐとに基づいて補正量を導出する式である。数式（１５）の右辺は、代表平面法線計算部１４６ｇにより計算された、センサ座標系で表した正中矢状面の法線ｎと、セグメントベクトル計算部１４６ｈにより計算された、センサ座標系で表したセグメントの方向ベクトルｖｉの内積を導出するものである。これにより、推定対象ＴＧＴが体を捻った状態である場合に、その捻りが解消されること（図５の右図に示すようなホームポジションに近づく）を補正内容に加味した補正を行うことができる。

次に、重力誤差項計算部１４６ｂは、数式（１６）に示すように、セグメント毎の基準補正（例えば重力補正）をするための計算を行う。

数式（１６）は、任意のセグメントｉの推定姿勢を示す単位クオータニオン^Ｓ _Ｅｑ（ｈ）_ｉと、ＩＭＵセンサＪＳ－ｉが計測した加速度（重力）との関係式であり、数式（１６）の右辺に示すように、推定姿勢から求めた、センサ座標系で表現した重力のあるべき方向（想定重力加速度方向）から、センサ座標系で表現した計測した重力の方向（計測重力加速度方向）^Ｓａ_ｉ（ｈ）を減算することで導出することができる。

ここで、計測した重力の方向^Ｓａ_ｉ（ｈ）の具体例を数式（１７）に示す。また、重力方向を示す定数^Ｅｄ_ｇ（ｈ）は、それぞれ数式（１８）に示すような定数で表現することができる。

次に、目的関数計算部１４６ｃは、重力誤差項とヨー方向誤差項を統合した、セグメントｉの補正関数として数式（１９）を計算する。

ここで、ｃ_iは代表平面補正の重み係数である。セグメントｉの補正関数を示す数式（１９）は、最適化問題として形式化すると数式（２０）のように表現することができる。

なお、数式（２０）は、重力補正と代表平面補正の目的関数の和で示すことができる補正関数の数式（２１）と等価である。

目的関数計算部１４６ｃは、すべてのセグメントに対して同様に姿勢推定を行い、全身の目的関数を統合した最適化問題を定義する。数式（２２）は、全身の目的関数を統合した補正関数Ｆ（Ｑ，α）である。αは、ＩＭＵセンサにより計測された全ＩＭＵ加速度であり、数式（２３）のように示すことができる。

なお、数式（２２）の右辺の第１行は、骨盤に対応した補正関数を示し、右辺の第２行以降は、骨盤以外の各セグメントに対応した補正関数を示すものである。数式（２２）に示す補正関数を用いて、推定対象ＴＧＴの全身の姿勢を補正するための最適化問題は下記の数式（２４）のように定義することができる。数式（２４）は、既出の各セグメントの補正関数である数式（２１）と同様の形式で、数式（２５）に示すように変形することができる。

次に、勾配計算部１４６ｅは、この目的関数の勾配を、推定全身姿勢ベクトルＱの偏微分で求めたヤコビアンＪ_Ｆを用いて、以下の数式（２６）のように計算する。なお、ヤコビアンＪ_Ｆは、数式（２７）に示す。

数式（２７）に示すそれぞれの要素のサイズは以下の数式（２８）および（２９）のようになっている。

すなわち、数式（２７）に示すヤコビアンＪ_Ｆは（３＋４Ｎ）×４（Ｎ＋１）（Ｎは基準部位計測用のＩＭＵセンサ以外の全ＩＭＵセンサ数）の大きな行列となるが、実際には以下の数式（３０）および（３１）に示す要素は０となるため計算は割愛することができ、低速な演算装置でもリアルタイムな姿勢推定が可能となる。

数式（３０）および（３１）を既出の数式（２７）に代入すると、次の数式（３２）のように示すことができる。

勾配計算部１４６ｅは、数式（３２）の計算結果を用いて数式（２６）に示す勾配を計算することができる。

［全身補正量計算部の処理イメージ］
図９～１２は、全身補正量計算部１４４の演算処理の流れを模式的に示す図である。図９は、全身補正量計算部１４４の全体を模式的に示す図であり、図１０～図１２は全身補正量計算部１４４の処理の流れを段階的に説明するための図である。

図９に示すように、加速度集約部１２４は、時刻ｔに計測された各ＩＭＵセンサＪＳ－ｉの加速度^Ｓａ_ｉ，ｔ（ｉは基準部位である骨盤を示すｐであってもよい、以下同様）の取得部１１０による取得結果を変換し、集約結果である推定対象ＴＧＴの全ＩＭＵ加速度α_ｔに変換する。また、取得部１１０により取得された、時刻ｔに計測された各ＩＭＵセンサＪＳ－ｉの角速度^Ｓω_ｉ，ｔは、それぞれ対応する角速度積分部１３２－ｉに出力される。

また、図９の右上部分に示されるＺ^－１からβまでの処理ブロックは、補正部１４０が次回処理サイクルにおける補正量を導出することを表すものである。

なお、図９～図１２において、下記の数式（３３）で示す目的関数の勾配をΔＱ_ｔとすると、時刻ｔの角速度Ｑ_ｔ（・）（Ｑ_ｔの上文字としてドット記号を付したものであり、時刻ｔの推定全身姿勢ベクトルＱ_ｔの時間微分結果）へのフィードバックは下記の数式（３４）のように表現することができる。なお、数式（３４）のβは、補正量のゲインを調整するための０≦β≦１となる実数である。

全身補正量計算部１４４は、数式（３４）に示すように、角速度Ｑ_ｔ（・）に勾配ΔＱを正規化した結果に任意の実数βを補正量として反映する。

積分部１３０は、図１０に示すように、各セグメントの角速度を積分する。次に、補正部１４０は、図１１に示すように、各セグメントの角速度と推定姿勢を使って、勾配ΔＱを計算する。次に、補正部１４０は、図１２に示すように、導出した勾配ΔＱを各ＩＭＵセンサの角速度にフィードバックする。ＩＭＵセンサＪＳによる次の計測結果を取得部１１０が取得すると、積分部１３０は、図１０に示すように、再度各セグメントの角速度を積分する。姿勢推定装置１００は、図１０～図１２に示す処理を繰り返すことで推定対象ＴＧＴの姿勢推定の処理を行うことにより、人の身体の特性や経験則が各セグメントの姿勢の推定結果に反映されるため、姿勢推定装置１００の推定結果の精度が向上する。

図１０～図１２に示すような処理が繰り返し行われ、推定姿勢集約部１４２が積分部１３０の角速度の積分結果を集約することで、それぞれのＩＭＵセンサＪＳの計測角速度がもつ誤差が平均化され、式（２）の推定全身姿勢ベクトルＱを導出することができる。この推定全身姿勢ベクトルＱには、人の身体の特性や経験則を用いた、全身姿勢からヨー方向補正量を計算した結果が反映されている。上述の方法で推定対象ＴＧＴの姿勢推定を行うことで、地磁気を利用せずに、ヨー角方向ドリフトを抑えた尤もらしい人の全身姿勢を推定することができるため、長時間計測においても、ヨー方向ドリフトを抑えた全身姿勢推定ができる。

〔処理フロー〕
図１３は、姿勢推定装置１００を用いたモーションキャプチャ処理の流れの一例を示すフローチャートである。

まず、姿勢推定装置１００の利用者は、推定対象ＴＧＴにＩＭＵセンサを取り付ける（ステップＳＴ１００）。次に、姿勢推定装置１００の利用者は、推定対象ＴＧＴにモーションキャプチャのためのキャリブレーション動作を行わせ（ステップＳ１０２）、キャリブレーションを行う（ステップＳＴ１０４）。次に、姿勢推定装置１００は、推定対象ＴＧＴのセグメント姿勢の初期化を行い（ステップＳＴ１０６）、ＩＭＵセンサの計測を開始して姿勢推定の処理を行う（ステップＳＴ１０８）。なお、ステップＳＴ１０８の処理の詳細は図１４を用いて説明する。以上、本フローチャートの処理の説明を終了する。

図１４は、姿勢推定装置１００による姿勢推定処理の流れの一例を示すフローチャートである。まず、取得部１１０は、ＩＭＵセンサの計測結果である角速度および加速度を取得する（ステップＳＴ２００）。次に、変換部１２０は、取得部１１０により取得された角速度および加速度をセグメント座標系に変換する（ステップＳＴ２０２）。

次に、補正部１４０は、前回処理サイクルにおいて導出された補正量を、ステップＳＴ２０２の処理結果の角速度に反映することで補正する（ステップＳＴ２０４）。次に、積分部１３０は、ステップＳＴ２０４の処理結果である角速度を積分することで、推定対象ＴＧＴの姿勢推定を行う（ステップＳＴ２０６）。

また、ステップＳＴ２０２の処理の後、補正部１４０は、次の処理サイクルに用いる補正量である目的関数の勾配ΔＱの導出を行う（ステップＳＴ２０８）。ステップＳＴ２０８において導出された補正量は、次の処理サイクルのステップＳＴ２０４において用いられる。

ステップＳＴ２０６およびステップＳＴ２０８の処理の後、取得部１１０は、次のＩＭＵセンサＪＳの計測結果を取得したか否か（すなわち、処理サイクルを終了するか否か）を判定する。取得部１１０は、次のＩＭＵセンサの計測結果を取得した場合、処理サイクルを終了しないと判定して、ステップＳＴ２００に処理を戻す。取得部１１０は、次のＩＭＵセンサの計測結果を取得しない状態が所定時間以上継続した場合（または、姿勢推定装置１００の利用者により計測終了を示す指令の入力を受け付けた場合）、処理サイクルを終了すると判定して、本フローチャートの処理を終了する。

以上説明した実施形態によれば、ＩＭＵセンサＪＳにより計測された加速度と角速度から、基準部位である推定対象ＴＧＴの骨盤の姿勢を基準として、参照部位である各セグメントの姿勢推定を行うことにより、より正確な姿勢推定ができる。

以上、本発明を実施するための形態について実施形態を用いて説明したが、本発明はこうした実施形態に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。

例えば、補正部１４０による代表平面の導出処理、およびセグメントのそれぞれの方向ベクトルｖｉの導出処理には、機械学習による推定が行われてもよい。

また、例えば、補正部１４０が最適化問題を解く際に用いる基準のあるべき方向として、重力や地磁気などの方向以外の、１以上のベクトルが用いられてもよい。重力や地磁気以外のベクトルを基準とする場合、例えば、推定対象ＴＧＴの基準部位である骨盤の前方方向と、基準部位である骨盤から上下方向のベクトルとを定義して補正に用いる。

１００…姿勢推定装置、１１０…取得部、１２０…変換部、１２２…セグメント角速度計算部、１２４…加速度集約部、１３０…積分部、１３２…角速度積分部、１４０…補正部、１４２…推定姿勢集約部、１４４…全身補正量計算部、１４６…補正量分解部、１４６ａ…ヨー方向誤差項計算部、１４６ｂ…重力誤差項計算部、１４６ｃ…目的関数計算部、１４６ｄ…ヤコビアン計算部、１４６ｅ…勾配計算部、１４６ｆ…補正量計算部、１４６ｇ…代表平面法線計算部、１４６ｈ…セグメントベクトル計算部、１４８…角速度補正部、Ｑ…推定全身姿勢ベクトル、ＪＳ…ＩＭＵセンサ

Claims (5)

1. 人、動物、または関節の可動域が制限されたロボットである推定対象の複数の箇所に取り付けられ、角速度および加速度を検出する複数のセンサから、角速度および加速度の情報を取得する取得部と、
   前記取得部により取得された情報を、センサ座標系から基準座標系の情報に変換する変換部と、
   前記変換された前記角速度を積分することで、前記推定対象において前記センサが取り付けられた参照部位の向きを前記推定対象の姿勢の一部として算出する積分部と、
   前記推定対象に含まれる基準部位を通る代表平面を想定し、前記代表平面の法線と、前記積分部により算出される前記参照部位の向きとが直交する方向に近づくように、前記参照部位の前記変換された角速度を補正する補正部と、を備え、
   前記積分部および前記補正部の処理は、処理サイクルごとに繰り返し実行され、
   前記補正部は、前回処理サイクルにおいて前記積分部により算出された前記参照部位の向きが前記代表平面の法線に対して直交する向きから乖離することが継続する乖離時間に応じて、前記参照部位の前記変換された角速度を補正する度合いを大きくする、
   姿勢推定装置。
2. 前記補正部は、前記代表平面の法線と、前記積分部により算出される前記参照部位の向きとの内積を小さくするように、前記参照部位の前記変換された角速度を補正する、
   請求項１記載の姿勢推定装置。
3. 前記補正部は、更に、前記積分部により算出された前記参照部位の向きから導出される前記参照部位ごとの想定重力加速度方向と、前記取得部により取得された加速度の情報に基づいて認識される前記参照部位ごとの計測重力加速度方向との乖離が小さくなるように、前記参照部位の前記変換された角速度を補正する、
   請求項１又は２に記載の姿勢推定装置。
4. コンピュータが、
   人、動物、または関節の可動域が制限されたロボットである推定対象の複数の箇所に取り付けられ、角速度および加速度を検出する複数のセンサから、角速度および加速度の情報を取得し、
   取得された情報を、センサ座標系から基準座標系の情報に変換し、
   変換された前記角速度を積分することで、前記推定対象において前記センサが取り付けられた参照部位の向きを前記推定対象の姿勢の一部として算出し、
   前記推定対象に含まれる基準部位を通る代表平面を想定し、前記代表平面の法線と、算出された前記参照部位の向きとが直交する方向に近づくように、前記参照部位の前記変換された角速度を補正し、
   前記積分および前記補正の処理は、処理サイクルごとに繰り返し実行され、
   前記補正は、前回処理サイクルにおいて前記積分により算出された前記参照部位の向きが前記代表平面の法線に対して直交する向きから乖離することが継続する乖離時間に応じて、前記参照部位の前記変換された角速度を補正する度合いを大きくする、
   姿勢推定方法。
5. コンピュータに、
   人、動物、または関節の可動域が制限されたロボットである推定対象の複数の箇所に取り付けられ、角速度および加速度を検出する複数のセンサから、角速度および加速度の情報を取得させ、
   取得された情報を、センサ座標系から基準座標系の情報に変換させ、
   変換された前記角速度を積分することで、前記推定対象において前記センサが取り付けられた参照部位の向きを前記推定対象の姿勢の一部として算出させ、
   前記推定対象に含まれる基準部位を通る代表平面を想定させ、前記代表平面の法線と、
   算出された前記参照部位の向きとが直交する方向に近づくように、前記参照部位の前記変換された角速度を補正させ、
   前記積分および前記補正の処理は、処理サイクルごとに繰り返し実行され、
   前記補正は、前回処理サイクルにおいて前記積分により算出された前記参照部位の向きが前記代表平面の法線に対して直交する向きから乖離することが継続する乖離時間に応じて、前記参照部位の前記変換された角速度を補正する度合いを大きくさせる、
   姿勢推定プログラム。

Images