JP4682791B2 - 操作空間物理量算出装置及び操作空間物理量算出方法、並びにコンピュータ・プログラム - Google Patents

Description

本発明は、脚式移動ロボットなど、複雑なリンク構造を有し、外界とのインタラクションを行ないながらタスクを遂行するロボットの動作を制御するために必要となる物理量を算出する操作空間物理量算出装置及び操作空間物理量算出方法に係り、特に、複数の剛体が連なって構成されるリンク構造体の動作に力制御を取り入れて、外力に柔軟に応じながら速度や加速度の制御を精密に行なうために必要となる物理量を算出する操作空間物理量算出装置及び操作空間物理量算出方法に関する。

さらに詳しくは、本発明は、力制御系で駆動されるリンク構造物に作用する力と加速度の関係を記述する操作空間における物理量を算出する操作空間物理量算出装置及び操作空間物理量算出方法に係り、特に、１つの操作空間が構造物の複数点の位置及び姿勢に関わる場合にも適用可能な操作空間物理量算出装置及び操作空間物理量算出方法に関する。

最近、脚式移動ロボットの構造やその安定歩行制御に関する研究開発が進展し、実用化への期待も高まってきている。これら脚式移動ロボットは、クローラ式ロボットに比し不安定で姿勢制御や歩行制御が難しくなるが、階段の昇降や障害物の乗り越えなど、柔軟な歩行・走行動作を実現できるという点で優れている。例えば、移動手段としての脚を備え、接地・無接地間を遷移する安定な運動パターンを実時間で生成することができるヒューマノイド・ロボットについて提案がなされている（例えば、特許文献１を参照のこと）。

従来の脚式ロボットは、制御上の簡便さやシステムの構成し易さから、位置制御によって駆動される関節によって構成されるものが大半である。位置制御は、基本的に位置を保持することが目的であることから、俗に「硬い制御」と呼ばれ、外力に柔軟に応じたり、速度や加速度の制御を精密に行なったりするのには適さない。

とりわけ、パートナー型の脚式ロボットは、多様な外界との物理インタラクションを行ないながらタスクを遂行するロボットであることから、本来は位置制御との親和性は低い。制御則、システム構成は複雑化するが、理想的には、力制御系で駆動されることが望まれる。

ここで、力制御系のロボット制御において、「操作空間（Ｏｐｅｒａｔｉｏｎａｌ Ｓｐａｃｅ）」と呼ばれる重要な概念がある。この操作空間とは、ロボットに作用する力と発生する加速度の関係を記述するための空間のことである。例えば、ロボットの関節角を位置制御ではなくトルク制御する際に、ロボットと環境との接し方を拘束条件に用いるときに、操作空間が必須になる。ここで言う拘束条件とは、例えば、ロボットの自己干渉に対する拘束条件、関節可動角の拘束条件、「手が物にフィットする」や「左右の眼が水平になる」といった事象が挙げられる。

剛体が関節を介して連なった構造物において、関節の値をすべて連ねてできるベクトルを一般化変数と呼び、ｑで表すものとする。この一般化変数ｑの時間微分値との関係がヤコビアンＪを用いて下式（１）に示す形に関連付けられたとき、物理量ｘに対して操作空間を定義することができる。

直感的には、マニピュレータ先端の手先位置姿勢やその他のエンド・エフェクタなど、タスク遂行のためのデカルト座標系が操作空間の一例である（例えば、非特許文献１を参照のこと）。

操作空間すなわち物理量ｘの加速度と力の関係については後に詳解するが、基本的には操作空間慣性逆行列と呼ばれる行列によって与えられる。すなわち、操作空間慣性逆行列は、力と加速度を紐付けする表現方法であり、下式（２）に示す行列Λ^-1として定義される。但し、Ｈは構造全体の関節空間に対する慣性行列である。

操作空間慣性逆行列は、力制御、力学シミュレーション、コンピュータ・アニメーション、姿勢編集、外力推定など、多くの分野への応用を可能にする重要な物理量である。例えば、動力学を考慮した拘束条件を追加して関節加速度を計算し、又はリンクに働く力とそれによる加速度との関係を表す操作空間慣性逆行列を用いて定式化された動力学的な拘束条件と、運動学的な拘束条件とを連立させて解くことにより、人型リンク形の関節の運動を生成する運動生成方法について提案がなされている（例えば、特許文献２を参照のこと）。

しかしながら、上式（２）に示した定義通りの計算では、操作空間慣性逆行列の算出には非常に多くの計算量となるため、実時間処理に向かないという問題がある。

例えば、上式（２）において構造物の自由度（関節数） をＮとすると、慣性行列Ｈ はＮ×Ｎのサイズを有する。一方、操作空間ｘの次元をＭとすると、Λ^-1のサイズはＭ×Ｍであり、Ｎ＞Ｍの場合は明らかに、定義式（２）に基づいて計算すると計算の無駄が発生する。特に、ヒューマノイドのようにＮ＝３０以上の多くの関節を有している場合、この差はより顕著となり、操作空間慣性逆行列をリアルタイムで算出すること自体が困難となる。多くの場合、ロボットの自由度数Ｎよりも操作自由度Ｍの方が小さい。よって、慣性行列Ｈを介することなく、操作空間慣性逆行列Λ^-1を直接的に求められることが望ましい。

このような問題を解決するために、操作空間慣性逆行列の直接解法が提案されている。一例として、加速度伝播子と力伝播子という物理量を用いて、操作空間慣性逆行列Λ^-1のブロック対角成分とブロック非対角成分を動力学演算により直接求める方法が提案されている（例えば、非特許文献２を参照のこと）。しかし、その適用対象は、１つの操作空間がリンク構造物の１点の位置及び姿勢に対応するケースに限定されている。例えば、図１７に示すような多軸ロボットについて考えると、同図左に示すように１つの操作空間に対応する力がロボットの構造上の１点Ａのみである場合には適用できるが、同図右に示すように２点Ｂ及びＣ間の相対位置と姿勢というように、１つの操作空間に対応する力がロボットの構造上の２点以上に作用する場合には直接適用することはできない。

図１７右に示した例では、Ｂ及びＣそれぞれの点における操作空間慣性逆行列をまず求めた後、これらの差をとるなどの方法をとることにより最終的には算出することはできるものの、計算の無駄が発生する。また、力の作用点Ｂ及びＣ間の衝突、すなわち移動ロボットのリンク間の干渉（自己干渉）を扱う場合に必須となる。

その他、脚式移動ロボットの力制御では、構造物全体の重心や運動量を操作空間の考えに基づいてコントロールすることが必要である。とりわけ２足移動型のヒューマノイドなどでは、そのバランス制御のために機体全体の重心と運動量は必須な要素と言える。重心や運動量を操作空間とする場合、詳しくは後述するが、図１８に示すように、１つの操作空間に対応する力がすべてのリンクの重心位置に作用する。これについても上述の自己干渉の例と同様の理由で、操作空間慣性逆行列の直接解法（非特許文献２を参照のこと）を適用することができない。また、すべてのリンクの力を計算しようとしても、操作空間慣性逆行列Λ^-1のみならず、ヤコビアンＪの算出にも相当の計算を有し、多大な計算負荷を発生してしまう。

特開２００４−１６７６７６号公報 特開２００３−２３１０７７号公報 Ａ ｕｎｉｆｉｅｄ ａｐｐｒｏａｃｈ ｔｏ ｍｏｔｉｏｎ ａｎｄ ｆｏｒｃｅ ｃｏｎｔｒｏｌｏｆ ｒｏｂｏｔ ｍａｎｉｐｕｌａｔｏｒｓ：Ｔｈｅ ｏｐｅｒａｔｉｏｎａｌ ｓｐａｃｅ ｆｏｒｍｕｌａｔｉｏｎ， ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓａｎｄ Ａｕｔｏｍａｔｉｏｎ，ＲＡ−３（１），ｐｐ．４３−５３，１９８７ Ｏｐｅｒａｔｉｏｎａｌ Ｓｐａｃｅ Ｄｙｎａｍｉｃｓ： Ｅｆｆｉｃｉｅｎｔ Ａｌｇｏｒｉｔｈｍｓ ｆｏｒ Ｍｏｄｅｌｉｎｇａｎｄ Ｃｏｎｔｒｏｌ ｏｆ Ｂｒａｎｃｈｉｎｇ Ｍｅｃｈａｎｉｓｍｓ，Ｐｒｏｃ．ｏｆ ｔｈｅ ２０００ ＩＥＥＥ Ｉｎｔ．Ｃｏｎｆ．ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄＡｕｔｏｍａｔｉｏｎ，ｐｐ．８５０−８５６，２０００

本発明の目的は、脚式移動ロボットなど、複雑な構造を有し、外界とのインタラクションを行いながらタスクを遂行するロボットの動作に力制御を取り入れて、外力に柔軟に応じながら速度や加速度の制御を精密に行なうために必要となる物理量を好適に算出することができる、優れた操作空間物理量算出装置及び操作空間物理量算出方法を提供することにある。

本発明のさらなる目的は、タスクを遂行するロボットなどの複雑なリンク構造物を力制御下で動作させるのに重要な物理量である操作空間慣性逆行列並びにそれに付随するバイアス加速度を高速に算出することができる、優れた操作空間物理量算出装置及び操作空間物理量算出方法を提供することにある。

本発明のさらなる目的は、１つの操作空間が構造物の複数点の位置及び姿勢に関わる場合にも適用可能な、効率のよい操作空間慣性逆行列の直接的算出を行なうことにより、脚式移動ロボットの力制御を好適に実現することができる、優れた操作空間物理量算出装置及び操作空間物理量算出方法を提供することにある。

本発明のさらなる目的は、重心、角運動量といった操作点を持たない物理量に対する操作空間（運動量操作空間）を定義して、その操作空間慣性逆行列を求めることにより、力制御下で構造物全体の運動量を制御することができる、優れた操作空間物理量算出装置及び操作空間物理量算出方法を提供することにある。

本発明は、上記課題を参酌してなされたものであり、その第１の側面は、複数の剛体が連なって構成されるリンク構造物に作用する力と加速度の関係を記述する操作空間における物理量を算出する操作空間物理量算出装置であって、前記リンク構造物の力学モデルと、前記リンク構造物に作用する力情報に基づいて前記リンク構造物の各点に発生する加速度を得るための順動力学演算を実行する順動力学演算手段と、前記リンク構造物の力学モデルを用いて前記順動力学演算手段による順動力学演算を実行させることにより、操作空間慣性逆行列及び操作空間バイアス加速度を求める操作空間物理量算出手段と、を具備することを特徴とする操作空間物理量算出装置である。

脚式ロボットを始めとするリンク構造物の動作制御は、制御上の簡便さやシステムの構成し易さの観点から、位置制御を採用するのが一般的であったが、外力に柔軟に応じたり、速度や加速度の制御を精密に行なったりするのには適さない。言い換えれば、制御則、システム構成は複雑化するが、理想的には、力制御系で駆動されることが望まれる。

力制御系では、ロボットに作用する力と発生する加速度の関係を記述するための、「操作空間」という重要な概念がある。ロボットの関節角を位置制御ではなく力制御する際にロボットと環境の接し方を拘束条件として用いるときに、操作空間が必須となる。操作空間、すなわち加速度と力の関係は操作空間慣性逆行列によって与えられる。

しかしながら、定義通りの計算では、構造全体の関節空間に対する慣性行列Ｈが介在して計算の無駄が発生するために、操作空間慣性逆行列の算出には非常に多くの計算量となるため、実時間処理に向かないという問題がある。また、この問題を解決するために、加速度伝播子と力伝播子という物理量を用いて、操作空間慣性逆行列Λ^-1のブロック対角成分とブロック非対角成分を動力学演算により直接求める方法が提案されているが、その適用対象は１つの操作空間がリンク構造物の１点の位置及び姿勢に対応するケースに限定されて、１つの操作空間に対応する力がロボットの構造上の２点以上に作用する場合には直接適用することはできない。

そこで、本発明では、リンク構造物の一般化力（関節力）から一般化加速度（関節加速度）を得る順動力学演算を応用することで、操作空間慣性逆行列を高速に算出するとともに計算負荷の軽減を図ることにした。

前記操作空間物理量算出手段は、前記リンク構造物の関節に発生する力、関節の速度、重力をすべてゼロとし、外力のみが作用し得る制約条件下で順動力学演算を実行させて、リンク構造物の各点に発生する加速度を得ることができる。ｉ番目の操作空間にのみ単位外力を作用させる制約条件下で順動力学演算を実行して操作空間慣性逆行列の第ｉ列を順次算出することにより、操作空間慣性逆行列全体を求めることができる訳である。

また、前記操作空間物理量算出手段は、外力をゼロとし、関節に発生する力、関節の速度、重力のみが作用する制約条件下で順動力学演算を実行して、リンク構造物の各点に発生する加速度を操作空間バイアス加速度として求めることができる。

また、順動力学演算手段は、順動力学演算を４過程に分解して、イナーシャ情報算出⇒速度情報算出⇒力情報算出⇒加速度情報算出の順で実行するようにしてもよい。このような場合、脚式移動ロボットなどのリンク構造物に任意の力を作用させた条件下で任意の場所並びに方向に発生する加速度を得ることができる。すなわち、順動力学演算は、その特質上、同時に複数の力を作用させたり、複数点における加速度を算出したりといったことを自在に行なうことができることから、１つの操作空間が構造物の複数点の位置及び姿勢に関わる場合の操作空間慣性逆行列の算出にも容易に応用が可能となる。

また、前記順動力学演算手段は、同時に複数の外力を作用できるように順動力学演算を構成したり、複数の点の加速度を算出できるように順動力学演算を構成したりすることができる。

また、順動力学演算手段は、順動力学演算を４過程に分解して実行する際に、それぞれの過程を最適化することができる。

例えば、イナーシャ情報は、リンク構造物の姿勢（すなわち関節値）が変化しない限り変化しないことから、前記順動力学演算手段は、一般化変数としての関節値ｑが変化したときに一度だけイナーシャ情報を算出するようにしてもよい。

また、操作空間慣性逆行列を求める際には、すべての関節速度がゼロに設定されるため、ｖ_i＝０は自明であるから、前記順動力学演算手段は、操作空間バイアス加速度を算出するときにのみ速度情報の算出を実行し、操作空間慣性逆行列を算出するときには実行しないようにしてもよい。

また、操作空間慣性逆行列を算出する際には、関節の速度と関節に発生する力はゼロであるから、前記順動力学演算手段は、これらを含む項の計算を省略することで、力情報算出を最適化することができる。

また、力の作用点と基底を結ぶ経路上にないリンクではバイアス力ｐ^A _i＝０が自明なので、前記順動力学演算手段は、力の作用点と基底を結ぶ経路上のリンクに関してのみリンクのバイアス力ｐ^A _iを算出することで、力情報の算出を最適化することができる。

また、順動力学演算手段は、操作空間慣性逆行列を求める際には、関節の速度と関節に発生する力はゼロであること、並びに力情報の最適化の結果を考慮して、加速度情報算出を最適化することができる。その際、ある点の加速度算出を行なう場合、既に加速度算出の行なわれた最寄りの点を起点として前記のある点に向かう曲線上の経路に沿って加速度更新を実行することにより、加速度情報算出をより最適化することができる。また、操作空間バイアス加速度を算出する際には、基底から加速度算出を行なうリンクに向かう経路上でのみ加速度算出を行なうことによって、加速度情報算出を最適化することができる。

このように、順動力学演算を構成する４つの過程それぞれにおいて最適化を実施することにより、１つの操作空間が構造物の複数点の位置及び姿勢に関わる場合にも順動力学演算を適用することができるようになり、且つ高速に操作空間慣性逆行列を算出することが可能である。

また、操作空間慣性逆行列は対称行列となるので、前記操作空間物理量算出手段は、操作空間慣性逆行列の上三角行列又は下三角行列のみを算出するようにしてもよい。

また、前記順動力学演算手段は、剛体間の再帰演算で構成することができる。この場合、再帰演算の対象となる剛体を抽出する計算経路決定手段をさらに備え、前記順動力学演算手段は、前記計算経路決定手段により決定される剛体のみを再帰演算の対象とするようにすればよい。

上述した操作空間慣性逆行列並びに操作空間バイアス加速度の計算方法は、グリッド・コンピュータやクラスタＰＣとも呼ばれる並列計算機上で実行することで、さらに高速化することができる。何故ならば、操作空間慣性逆行列はＭ×Ｍの正方行列であるが、その各要素は別々のＣＰＵで独立に求めることができるからである。また、操作空間バイアス加速度は要素数Ｍのベクトルであるが、その各要素も別々のＣＰＵで独立に求めることができる。

例えば、前記順動力学演算手段は、操作空間慣性逆行列用の力情報を算出するためのプロセッサを操作空間慣性逆行列の列毎に独立に設け、各列の演算を並列実行することができる。この場合、さらに、前記の列毎に設けられたプロセッサから出力される力情報を用いて操作空間慣性逆行列用の加速度情報を算出するためのプロセッサを行毎に独立に設け、同一列内の行方向の各要素の演算を並列実行することができる。

また、前記順動力学演算手段は、操作空間バイアス加速度用の力情報を算出するためのプロセッサをバイアス加速度ベクトルの行毎に独立に設け、バイアス加速度用加速度情報の各行の演算を並列実行することができる。

このように、操作空間慣性逆行列及びバイアス加速度を並列計算するように装置を構成することで、行列又はベクトルの要素毎に並列に計算が行なわれることから、行列又はベクトルの１要素を計算する計算時間と同程度の時間で全要素の算出を完了することができるので、全体として大幅な高速化を図ることができる。勿論、操作空間慣性逆行列の各要素をすべて並列実行するのではなく、その一部を並列化して実行するのみでも大幅な高速化を図ることができる。

あるいは、前記順動力学演算手段は、前記の列毎に設けられたプロセッサから出力される力情報を用いて操作空間慣性逆行列用の加速度情報を算出するためのプロセッサを列毎に１つずつ設け、それぞれの列でプロセッサを繰り返し用いて１列分の加速度を算出するようにしてもよい。このような場合、操作空間慣性逆行列の列毎に並列に計算が行なわれることから、操作空間慣性逆行列の行数に比例した時間内で操作空間慣性逆行列の全要素の算出を完了することができ、効果的に高速化を図ることができる。

このように、本発明によれば高い汎用性と高速性を兼ね備えた操作空間慣性逆行列の算出が可能である。脚式移動ロボットを力制御下で全身運動を実行する際には、多数の拘束条件を考慮してタスクを遂行しなければならないが、本発明を適用することにより、現在の計算機速度でも実現可能である。

例えば、前記リンク構造物を力制御下で動作制御するために用いられる場合には、該求められた操作空間慣性逆行列及び操作空間バイアス加速度を用いて前記リンク構造物における関節力を決定する関節力決定手段をさらに備えていればよい。この前記関節力決定手段は、操作空間慣性逆行列及び操作空間バイアス加速度を用いて記述される操作空間における加速度と力との関係を記述した等式と、前記リンク構造体のリンク同士及びリンクと環境との間で形成される拘束条件からなる線形相補性問題を満足する加速度と力を、前記操作空間物理量算出手段により求められた操作空間慣性逆行列及び操作空間バイアス加速度を用いて求めることができる。そして、得られた力に基づいてリンクの関節力を決定するようにすればよい。

また、本発明の第２の側面は、複数の仮想物体が共存する仮想環境内で、仮想物体間の衝突又は接触その他の物理的相互作用を再現する力学シミュレーション装置であって、物理的相互作用が働く仮想物体の力学モデルと、物理的相互作用による外力に基づいて仮想物体に発生する加速度を得るための順動力学演算を実行する順動力学演算手段と、仮想物体の力学モデルを用いて前記順動力学演算手段による順動力学演算を実行させることにより、操作空間慣性逆行列及び操作空間バイアス加速度を求める操作空間物理量算出手段と、操作空間慣性逆行列及び操作空間バイアス加速度を用いて記述される操作空間の加速度と力との関係式と、仮想物体間で物理的相互作用が働いたときに課せられる拘束条件からなる線形相補性問題を満足する加速度と力を、前記操作空間物理量算出手段により求められた操作空間慣性逆行列及び操作空間バイアス加速度を用いて求め、得られた力に基づいて仮想物体に働く外力を決定する外力決定手段と、を具備することを特徴とする力学シミュレーション装置である。

本発明に係る操作空間慣性逆行列の算出方法は、ロボットの力制御だけでなく、力学シミュレーション、コンピュータ・アニメーション、姿勢編集、外力推定など、多くの分野に適用することができる。例えば、仮想環境内に複数の物体が共存し、それらの間の衝突や接触といった物理的相互作用を計算機上で再現する力学シミュレーションを実時間で実行したり、その中のオブジェクトを操作したりする場合にも有効である。力学シミュレーションの力学演算は、順動力学演算に対し、物体間の力を外力として作用させることで実行することにより、複雑な環境であってもリアルタイムで精緻な力学シミュレーションを実行することが可能となる。

例えば、前記操作空間物理量算出手段は、物理的相互作用によって仮想物体に働く垂直反力及び摩擦力の作用方向に操作空間を設定し、前記外力決定手段は、仮想物体間に指定した反発係数の反発と適切な摩擦力を実現させるための拘束条件を用いて線形相補性問題を設定し、該線形相補性問題を解決することによって、物理的相互作用が発生したときに仮想物体に働く外力を決定することができる。

また、前記操作空間物理量算出手段は、仮想物体間の相対位置偏差方向に操作空間を設定し、前記外力決定手段は、仮想物体間が指定した相対位置となるための拘束条件を用いて線形相補性問題を設定し、該線形相補性問題を解決することによって、物理的相互作用が発生したときに仮想物体に働く外力を決定することができる。

また、物理的相互作用が働く仮想物体が人型キャラクタのような複数の剛体が連なって構成されるリンク構造物である場合には、前記外力決定手段は、前記リンク構造物を構成する各リンクの位置又は姿勢に関する拘束条件を用いて線形相補性問題を設定するようにすればよい。

また、この力学シミュレーション装置は、前記外力決定手段により決定された外力をアクチュエータによって発生して提示する力提示手段をさらに備えていてもよい。この力提示手段により、仮想環境内の物体に触れた感じをリアルタイムに且つ精密に再現することが可能となる。あるいは、仮想空間内の物体の所定の位置を引張ったり捩じったりしたときに仮想物体に作用する力をフィードバックすることにより、仮想環境内で引張ったり捩じったりした反力を精緻に感じとることもできる。

したがって、例えばユーザは、実際の物体を触れて姿勢を編集するのと同様の力をリアルタイムに感じながら、キャラクタの運動制御や姿勢編集などを行なうことができるようになる。また、このような力ベースの姿勢編集により、逆キネマティクスによく見られる特異点付近での不安定な挙動も除去することができる。

また、本発明の第３の側面は、複数の剛体が連なって構成されるリンク構造物に作用する力と加速度の関係を記述する操作空間における物理量を算出するための処理をコンピュータ・システム上で実行させるようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、前記リンク構造物の力学モデルと、前記リンク構造物に作用する力情報に基づいて前記リンク構造物の各点に発生する加速度を得るための順動力学演算を実行させることにより、操作空間慣性逆行列を求める操作空間慣性逆行列算出手順と、前記順動力学演算を実行させることにより、操作空間バイアス加速度を求める操作空間バイアス加速度算出手順と、を実行させることを特徴とするコンピュータ・プログラムである。

また、本発明の第４の側面は、複数の仮想物体が共存する仮想環境内で、仮想物体間の衝突又は接触その他の物理的相互作用を再現する力学シミュレーション方法をコンピュータ・システム上で実行させるようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、物理的相互作用が働く仮想物体の力学モデルと、物理的相互作用による外力に基づいて仮想物体に発生する加速度を得るための順動力学演算を実行させることにより、操作空間慣性逆行列及び操作空間バイアス加速度を求める操作空間物理量算出手順と、操作空間慣性逆行列及び操作空間バイアス加速度を用いて記述される操作空間の加速度と力との関係式と、仮想物体間で物理的相互作用が働いたときに課せられる拘束条件からなる線形相補性問題を満足する加速度と力を、前記操作空間物理量算出手段により求められた操作空間慣性逆行列及び操作空間バイアス加速度を用いて求め、得られた力に基づいて仮想物体に働く外力を決定する外力決定手順と、を実行させることを特徴とするコンピュータ・プログラムである。

本発明の第３及び第４の各側面に係るコンピュータ・プログラムは、コンピュータ・システム上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第３及び第４の各側面に係るコンピュータ・プログラムをコンピュータ・システムにインストールすることによって、コンピュータ・システム上では協働的作用が発揮され、本発明の第１及び第２の各側面に係る操作空間物理量算出装置及び力学シミュレーション装置のそれぞれと同様の作用効果を得ることができる。

本発明によれば、脚式移動ロボットなど、複雑な構造を有し、外界とのインタラクションを行ないながらタスクを遂行するロボットの動作に力制御を取り入れて、外力に柔軟に応じながら速度や加速度の制御を精密に行なうために必要となる物理量を好適に算出することができる、優れた操作空間物理量算出装置及び操作空間物理量算出方法を提供することができる。

また、本発明によれば、高速な順動力学演算を応用することで、ロボットを力制御下で動作させるのに重要な物理量である操作空間慣性逆行列並びにそれに付随するバイアス加速度を高速で且つ低い計算負荷で算出することができる、優れた操作空間物理量算出装置及び操作空間物理量算出方法を提供することができる。

また、本発明によれば、重心、角運動量といった操作点を持たない物理量に対する操作空間（運動量操作空間）を定義して、その操作空間慣性逆行列を求めることができる、優れた操作空間物理量算出装置及び操作空間物理量算出方法を提供することができる。

本発明によれば、操作空間慣性逆行列を複数のＣＰＵを用いた並列計算によりさらに高速に求めることができる。

本発明によれば、順動力学演算を基本とするアルゴリズム構成とすることで、１つの操作空間が構造物の複数点の位置及び姿勢に関わるような操作空間慣性逆行列（及び操作空間バイアス・ベクトル）に関しても、優れた計算効率で算出することが可能である。

本発明は、リンク構造体の力制御に広範に適用することが可能である。例えば、複雑な構造体の運動に多様な拘束が課される場合であっても、高速に操作空間慣性逆行列（及び操作空間バイアスベクトル）を得ることができ、リアルタイムで力制御を行なうことができる。また、自己干渉や重心操作や角運動量など、１つの操作空間が構造物の複数点に対応する場合も効率的に扱うことができ、より高度な制御を実現することができる。

また、本発明を剛体力学シミュレーションの衝突力算出に応用することで、高速なシミュレーションを実現することができる。例えば、図１７右に示した例では、２つの剛体間の相対位置に操作空間を取り入れることにより剛体間の衝突で扱うことができる。本発明によれば、このように１つの操作空間が構造物の複数点に対応する場合も高速に操作空間慣性逆行列（及び操作空間バイアスベクトル）を得ることができる。

また、本発明による操作空間慣性逆行列のリアルタイム算出を応用して、ハプティクス・デバイスを用いた精緻な力覚提示、力覚提示を受けながらリアリティの高いキャラクタポーズ編集などを行なうことも可能である。

本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。

以下、図面を参照しながら本発明の実施形態について詳解する。

本発明は、力制御を取り入れることで、複雑な構造を有する脚式移動ロボットが外界とのインタラクションを行ないながらタスクを遂行する動作を、外力に柔軟に応じながら速度や加速度の制御を精密に行なう動作制御装置に関する。本発明では、高速な順動力学演算を応用することで、ロボットを力制御下で動作させるのに重要な物理量である操作空間慣性逆行列を高速に算出して、リアルタイムな力制御を実現する。

ロボット装置の構成：
図１には、本発明に係る力制御の適用対象となるロボット装置の外観構成を示している。図示のロボット装置は、いわゆる人間型ロボットであり、骨盤部には、移動手段としての２肢の脚体と、腰関節を介して上体が接続されている。上体には、２肢の腕部と、首関節を介して頭部が接続されている。

左右の脚体は、それぞれ股関節３自由度と、膝関節１自由度と、足首関節２自由度の、計６自由度を備えている。また、左右の腕部は、それぞれ肩関節３自由度と、肘関節１自由度と、手首関節２自由度の、計６自由度を備えている。首関節及び腰関節は、ともにＸ、Ｙ、Ｚ軸回りに３自由度を有するとする。

各関節軸を駆動するアクチュエータは、例えばＤＣブラシレス・モータと減速機、並びに減速機の出力軸の回転位置を検出する位置センサで構成される。これら関節駆動アクチュエータは、ロボット装置全体の動作を統括的にコントロールするホスト・コンピュータと接続され、その位置制御目標値が与えられるとともに、現在の出力トルクや、関節角度、関節角速度をホスト・コンピュータに送信することができるものとする。

図２には、図１に示したロボット装置における結線トポロジの構成例を示している。また、図３には、図１に示したロボット装置の関節自由度モデルを示している。

ロボット装置は、胴体部に、３軸の腰関節アクチュエータａ１、ａ２、ａ３、及び３軸の首関節アクチュエータａ１６、ａ１７、ａ１８を持ち、これらはホスト・コンピュータに対しシリアル接続されている。各関節アクチュエータは、シリアル・ケーブルを通じて、その位置制御目標値を受け取るとともに、現在の出力トルクや、関節角度、関節角速度をホスト・コンピュータに送信する。

また、ロボット装置は、左腕部に、３軸の肩関節アクチュエータａ４、ａ５、ａ６、１軸の肘関節アクチュエータａ７、及び２軸の手首関節アクチュエータａ８、ａ９を持ち、これらはホスト・コンピュータに対しシリアル接続されている。同様に、右腕部には、３軸の肩関節アクチュエータａ１０、ａ１１、ａ１２、１軸の肘関節アクチュエータａ１３、及び２軸の手首関節アクチュエータａ１４、ａ１５を持ち、これらはホスト・コンピュータに対しシリアル接続されている。

また、ロボット装置は、左脚部に、３軸の股関節アクチュエータａ１９、ａ２０、ａ２１、１軸の膝関節アクチュエータａ２２、及び２軸の足首関節アクチュエータａ２３、ａ２４を持ち、これらはホスト・コンピュータに対しシリアル接続されている。同様に、右脚部には、３軸の肩関節アクチュエータａ２５、ａ２６、ａ２７、１軸の肘関節アクチュエータａ２８、及び２軸の足首関節アクチュエータａ２９、ａ３０を持ち、これらはホスト・コンピュータに対しシリアル接続されている。

各関節で使用されるアクチュエータａ１〜ａ３０は、例えばＤＣブラシレス・モータと減速機、並びに減速機の出力軸の回転位置を検出する位置センサで構成され、外部から与えられた位置制御目標値に従って回転駆動するとともに、現在の出力トルクや、関節角度、関節角速度を出力する。この種の関節アクチュエータについては、例えば本出願人に既に譲渡されている特開２００４−１８１６１３号公報に記載されている。

図３から分るように、２足歩行の移動ロボットを、骨盤Ｂを基底とする開リンク木構造として表現することができる。

移動ロボットの場合、ロボットがワールド空間中を自由に移動し、姿勢を変化させることができる。したがって、ロボット全体の状態を表すための状態変数として、骨盤Ｂのオイラー角α＝（α，β，γ）^Tとその位置ｐ₀（ｐ_0x，ｐ_0y，ｐ_0z）^Tを導入する。すると、ロボットは複数の剛体が連なって構成されるリンク構造物として扱うことができるので、ロボットの運動方程式は、下式（３）のような形で表現することができる。

ここで、ｑはロボット全体の一般化変数であり、ロボットの運動状態としてのベースの姿勢α及び位置ｐ₀を並べたベクトルと、アクチュエータの現在状態としての全関節値を並べてできるベクトルθで構成される。すなわち、ｑは、リンク構造物としてのロボットの関節の値をすべて連ねてできるベクトルであり、下式（４）のような形で表現される。

また、上式（３）中で、τは、一般化変数ｑに対応した一般化力であり、αとｐ₀に対応する成分は（力を発生するアクチュエータが存在しないため）０となり、θに対応する成分は各関節部を駆動するアクチュエータの発生力に等しい。Ｈは、ロボット全体の慣性行列、ｂは重力やコリオリ力などを表す。ｆは外力である。Ｊは、外力作用方向に発生する速度と、一般化変数ｑの時間微分である一般化速度とを対応付けるヤコビアンであり、下式（５）の通りとなる。

操作空間慣性逆行列の解法：
本発明は、リンク構造物の一般化力（関節力）から一般化加速度（関節加速度）を得る順動力学演算を応用することで、高速算出や計算負荷の軽減といった要求を満足する操作空間慣性逆行列解法を提供する。順動力学演算はいわゆる力学シミュレーションに用いられるコア演算であり、勢力的に高速化のための研究が行なわれてきた。また、順動力学演算の並列化手法も幾つか提案されている。本発明では、優れた順動力学演算をベースとすることで、高い演算効率の操作空間慣性逆行列解法を提供する。

既に述べたように、複数の剛体が連なって構成されるリンク構造物全体の運動方程式は下式（６）のような形で表現できることが知られている．

ここで、τは一般化変数ｑ に対応した一般化力、ｂは重力・コリオリ力、ｆは操作空間に作用する外力である。上式（６）を以下のように変形することができる。

但し、ｃは操作空間バイアス加速度（外力が作用しない場合に操作空間に発生する加速度）であり、下式のように表される。

上式（７）において、右辺から左辺を得ることを考えると、外力や一般化力、重力、速度積に関連する力（コリオリ力など）が作用したときの操作空間における加速度を求める問題と見ることができる。操作空間における加速度を求める点が通常の順動力学計算と異なるが、一種の順動力学演算と捉えることができる。順動力学演算ＦＷＤは、リンク構造物の関節の値をすべて連ねてできるベクトルである一般化変数ｑとその１次微分値、一般化変数ｑに対応した一般化力としての関節トルクτ、重力ｇ、並びに外力ｆをパラメータに持ち、下式（９）のように表現することができる。但し、ｇは重力加速度である。

この順動力学演算ＦＷＤによれば、リンク構造物の関節の値をすべて連ねてできるベクトルである一般化変数ｑと、一般化力τ、重力ｇ、外力ｆといったリンク構造物に作用する力情報から、リンク構造物の各点に発生する加速度を得ることができる。ここで、上式（９）において、順動力学演算ＦＷＤの入力パラメータのうち一般化変数ｑ及び外力ｆ以外をすべて０とする制約条件下では、重力、関節力、速度積に関連する力（コリオリ力など）が発生しない状況において操作空間に発生する加速度を求めることができる。すなわち、式（８）においてバイアス加速度ｃ＝０とすることができる。さらに、ｆ＝ｅ_i、すなわち、ｉ番目の操作空間にのみ単位外力を作用させる条件下で計算を実行すると、操作空間慣性逆行列Λ^-1の第ｉ列を求めることができることが分る。よって、下式（１０）の演算をすべての行ｉについて実行すれば操作空間慣性逆行列Λ^-1全体を得ることができる。

また、上式（ ９）の順動力学演算ＦＷＤを外力ｆ＝０とし、関節に発生する力及び関節の速度、重力のみが作用するという制約条件下で実行することで、下式（１１）に示すように操作空間バイアス加速度ｃを抽出することができる。

要するに、上式（１０）及び（１１）を併せて、リンク構造物の関節の値をすべて連ねてできるベクトルである一般化変数ｑを用いて順動力学演算ＦＷＤを実行することにより、操作空間における重要な物理量である操作空間慣性逆行列Λ^-1と操作空間バイアス加速度ｃを求めることができる訳である。

順動力学演算を用いた操作空間慣性逆行列算出のための演算構成法：
以下では、具体的な順動力学演算を例にとって、操作空間慣性逆行列Λ^-1及びこれに付随する操作空間バイアス加速度ｃを算出する演算装置の構成法について詳解する。順動力学演算は、当初は逆動力学演算（加速度から力を求める演算）を利用して構成する方法が一般的であったが、Ｏ（Ｎ³）の計算量を有し、自由度数が増すと演算量が嵩むという問題がある。その後、研究が進み、現在では、単一のＣＰＵを用いてＯ（Ｎ）、複数のＣＰＵを並列化してＯ（ｌｏｇ（Ｎ））の計算量を達成することができるようになっている。

本明細書では、単一のＣＰＵを用いた例（Ｏ（Ｎ））として、Ａｒｔｉｃｕｌａｔｅｄ Ｂｏｄｙ法を用いた構成方法について説明する。ＡｒｔｉｃｕｌａｔｅｄＢｏｄｙ法自体については、例えば、“Ｔｈｅ ｃａｌｃｕｌａｔｉｏｎ ｏｆ ｒｏｂｏｔ ｄｙｎａｍｉｃｓ ｕｓｉｎｇ ａｒｔｉｃｕｌａｔｅｄ−ｂｏｄｙ ｉｎｅｒｔｉａｓ”（Ｉｎｔ．Ｊ．ＲｏｂｏｔｉｃｓＲｅｓｅａｒｃｈ，ｖｏｌ．２，ｎｏ．１，ｐｐ．１３−３０，１９８３）に記載されている。本発明では、Ａｒｔｉｃｕｌａｔｅｄ Ｂｏｄｙ法をイナーシャ情報算出、速度情報算出、力情報算出、加速度情報算出の４つの過程に分解する。

イナーシャ情報算出：
イナーシャ情報は、すべてのリンクのＩ^A _i（Ａｒｔｉｃｕｌａｔｅｄ Ｂｏｄｙ Ｉｎｅｒｔｉａ）を、下式（１２）をロボット装置の末端から基底に向かって実行することで求める。

ここで、Ｉ_iはリンクｉのイナーシャ、Ｃ（ｉ）はリンクｉの子リンクのインデックス集合とする。

速度情報算出：
速度情報は、すべてのリンクの原点の速度を、下式（１３）を基底から末端に向かって実行することで求める。

ここで、ｖ_iはリンクｉの速度、ｐはその親リンクのインデックス、Ｓ_iはリンクｉの関節の運動自由度を表す行列、ｑ_iはリンクｉの関節値である。

力情報算出：
力情報は、すべてのリンクのバイアス力ｐ^A _iを、下式（１４）〜（１５）を末端から基底に向かって実行することで求める。図４には、床面に設置された多リンク構成の装置において、末端から基底に向かって力情報を算出する通常の経路を示している。

ここで、ｆ_kはリンクｉの中でｋ番目の外力、Ｆ（ｉ）はリンクｉに作用する外力のインデックス集合、τ_iはリンクｉの関節力である。

加速度情報算出：
加速度情報は、すべてのリンクの加速度ａ_iを、下式（１６）〜（１７）を基底から末端に向かって実行することで求める。図５には、床面に設置された多リンク構成の装置において、基底から末端に向かって加速度情報を算出する通常の経路を示している。

以上の結果、リンクｉに作用する単位外力ｎ方向に発生する物理量ｘの加速度を下式（１８）で自由に求めることができる。

なお、上記の計算で、速度情報算出の起点となる基底の速度はｖ₀＝０である。また、重力の効果は、加速度情報算出において、基底が重力加速度で上昇しているとして考慮する。

４過程に分解した順動力学演算を、イナーシャ情報算出⇒速度情報算出⇒力情報算出⇒加速度情報算出の順で実行することにより、脚式移動ロボットなどのリンク構造物に任意の力を作用させた条件下で任意の場所並びに方向に発生する加速度を得ることができる。このように、順動力学演算は、その特質上、同時に複数の力を作用させたり、複数点における加速度を算出したりといったことを自在に行なうことができることから、１つの操作空間が構造物の複数点の位置及び姿勢に関わる場合の操作空間慣性逆行列の算出にも容易に応用が可能となる。

順動力学演算の最適化：
以上で上式（９）に示した順動力学演算の結果が得られたので、このままこの演算を式（１０）及び式（１１）のような形で応用することによって、操作空間慣性逆行列Λ^-1並びに操作空間バイアス加速度ｃを算出することができる。しかしながら、未だ計算の無駄が含まれており、これらの算出のために、より最適化された算出手段を過程毎に用意することが望ましいと思料される。

イナーシャ情報算出の最適化：
上述したように、イナーシャ情報は、上式（１２）をリンク構造物のすべてのリンクについて末端から基底に向かって実行することで求めることができる。同式から分るように、イナーシャ情報は、リンク構造物の姿勢（すなわち関節値）が変化しない限り変化しない。式（１０）に示した操作空間慣性逆行列、及び式（１１）に示した操作空間バイアス加速度式ともに、一般化変数としての関節値ｑが共通で変化しないことから、イナーシャ情報算出式（１２）は一度だけ実行すればよい。

速度情報算出の最適化：
式（１０）に示した操作空間慣性逆行列を求める際には、すべての関節速度がゼロに設定されるため、ｖ_i＝０は自明である。したがって、式（１３）に示した速度情報算出式を実行する必要はない。同式は、式（１１）に示した操作空間バイアス加速度式を算出するときにのみ実行すればよい。

力情報算出の最適化：
式（１０）に示した操作空間慣性逆行列を求める際には、関節の速度と関節に発生する力はゼロであるから、上式（１４）〜（１５）に示した力情報算出式は、下式（２０）のように簡略化される。

さらに、力の作用点と基底を結ぶルート上にないリンクではｐ^A _i＝０が自明なので、力の作用点と基底を結ぶルート上のリンクに関してのみ上式（２０）を実行すればよい。図６には、床面に設置された多リンク構成の装置において、Ａ、Ｂ、及びＣの３点に力が作用する場合を例にとって、末端から基底に向かって力情報を算出する最適化された経路を示している。同図に示すように、力の作用点と基底を結ぶ経路上にないリンクにおける計算が省略されている。

一方、上式（１１）に示した操作空間バイアス加速度算出式では、ｆ_k＝０ゆえに式（１４）〜（１５）に示した力情報算出式ではｆ_kの項を省略できるが、算出ルートは図４に示した通り、すべてのリンクに関して実行する。

加速度情報算出の最適化：
式（１０）に示した操作空間慣性逆行列を求める際には、上式（１６）〜（１７）に示した加速度情報算出式は、下式（２１）〜（２２）のように簡略化される。但し、ｉが力情報算出の経路上に存在しないときはｐ^A _i＝０とする。また、重力加速度の含まれる項ではｇ＝０とする。

図７には、床面に設置された多リンク構成の装置において、Ｄ、Ｅ、及びＦの３点における加速度を基底から末端に向かって算出するときの最適化された経路を示している。同図において、ハッチングが掛けられたリンクは、図６に示したようにＡ、Ｂ、及びＣの３点に力が作用するときに力情報算出の経路となったリンクである。図７から分るように、力情報算出が実行されたリンクではｐ^A _iの値が代入されて計算が実施されるが、それ以外のリンクに対してはｐ^A _i＝０ゆえ代入は行なわず、これによって計算が省略される。

続けて、図７中のＧ点の加速度算出を行なう場合、加速度情報は力情報が更新されない限り変化しないので、同図中で、既に加速度算出の行なわれたＦ点を起点として、ここからＧ点に向かう曲線上の経路に沿って加速度更新を追加で行なえばよい。

一方、上式（１１）に示した操作空間バイアス加速度算出式では、上式（１６）〜（１７）に示した力情報算出式において省略可能な項はない。但し、図５に示した通常の加速度情報算出経路のようにすべてのリンクについて加速度情報算出を行なう必要はなく、操作空間慣性逆行列算出の場合と同様に、図７に示したように、基底から加速度算出を行なうリンクに向かう経路上でのみ加速度算出を行なえばよい。追加点の加速度算出を行なう場合も、操作空間慣性逆行列算出と同様、加速度算出が行なわれた最近傍のリンクを起点として加速度算出を追加して実行すればよい。

以上説明してきたように、順動力学演算を構成する４つの過程それぞれにおいて最適化を実施することにより、１つの操作空間が構造物の複数点の位置及び姿勢に関わる場合にも順動力学演算を適用することができるようになり、且つ高速に操作空間慣性逆行列を算出することが可能である。

単純な例への適用：
ここでは、図１７に示したような（ＢとＣの間で衝突（自己干渉）が生じる）、床面に設置された比較的単純なリンク構成の装置を例にとると、上述した方法により操作空間慣性逆行列を算出する手順は以下の通りとなる。

（ステップ１）末端から基底に向かってすべてのリンクに関して式（１２）を実行して、イナーシャ情報を算出する。
（ステップ２）操作空間慣性逆行列を求める際に、速度情報の算出は不要である。
（ステップ３）Ｂ及びＣの各点に（操作空間方向の）単位力を作用した状態で、Ｂ及びＣ点と基底を結ぶそれぞれの経路上のリンクに関して、末端から基底に向かって式（２０）を実行して力情報を算出する。
（ステップ４）基底からＢ及びＣの各点に向かって式（２１）及び式（２２）を実行して加速度情報を算出する。但し、起点の重力加速度ｇは０として算出する。
（ステップ５）Ｂ及びＣの各点に対して式（１８） を適用し両者の差をとれば、ＢとＣの間の相対運動に関する操作空間慣性逆行列が得られる。但し、重力加速度ｇは０として算出する。

続いて、操作空間バイアス加速度を算出する手順は以下の通りとなる。

（ステップ１）イナーシャ情報は操作空間慣性逆行列で求められているので更新が不要である。
（ステップ２）基底から末端に向かってすべてのリンクに関して式（１３）を実行して速度情報を算出する。
（ステップ３）すべてのリンクに一般化力τを作用した状態で、末端から基底に向かって式（１４）を実行して力情報を算出する。
（ステップ４）基底からＢ及びＣの各点に向かって式（１６）及び式（１７）を実行して加速度情報を算出する。
（ステップ５）式（１８）をＢ及びＣの各点に対して適用し両者の差をとれば、ＢとＣの間の相対運動に関するバイアス加速度が得られる。

このように、本発明によれば、１つの操作空間が構造物の複数点の位置及び姿勢に関わる場合であっても、効率よく操作空間慣性逆行列及びバイアス加速度を得ることができる。

重心操作空間への適用例：
より複雑な例として、重心操作空間に関する操作空間慣性逆行列も本発明により効率的に求めることができる。

重心速度は下式（２３）のように定義される。但し、ｘ_gはリンク構造全体の重心、ｘ_giは剛体ｉの重心、Ｍはリンク構造全体の質量、ｍ_iは剛体ｉの質量を表す。

よって、重心の転置ヤコビアンは下式（２４）で定義される。但し、Ｊ_gはリンク構造全体の重心ヤコビアン、Ｊ_giは剛体ｉの重心ヤコビアンを表す。

上式（２４）から、重心に単位力を作用させるには、図１８に示した複数の剛体リンクからなる構造体において、そのｍ_i／Ｍ倍の力を各剛体の重心に作用させればよいことが分る。このとき、下式（２５）を用いて重心に発生する加速度を算出すれば、操作空間慣性逆行列を求めることができる。

したがって、重心操作空間における操作空間慣性逆行列は以下の手順に従って求めることができる。

（ステップ１）末端から基底に向かってすべてのリンクに関して式（１２）を実行してイナーシャ情報を算出する。
（ステップ２）操作空間慣性逆行列を求める際に、速度情報の算出は不要である。
（ステップ３）図１８に示すように、全リンクに（操作空間方向の）単位力のｍ_i／Ｍ の力を作用した状態で、末端から基底に向かって式（２０）を実行して力情報を算出する。
（ステップ４）基底から末端に向かって式（２１）及び式（２２）を実行して、加速度情報を算出する。但し、起点の重力加速度は０として算出する。
（ステップ５）式（１８）を各剛体ｉの重心に対して適用して重心ｘ_giの加速度を算出し、式（２５）でリンク構造全体の重心に発生する加速度を算出することにより、操作空間慣性逆行列が得られる。但し、重力加速度は０として算出する。

この例のように、本発明によれば、同時に複数の力を作用しなければならない場合についても、効率良く操作空間慣性逆行列を得ることができる。

角運動量操作空間への適用例：
本発明の他の重要な応用例として、角運動量に関する操作空間慣性逆行列の算出を挙げることができるので、以下に詳解する。

重心回りの角運動量は、下式（２６）のように定義される。但し、Ｌはリンク構造全体の角運動量、ω_iは剛体ｉの角速度を表す。

よって、角運動量の転置ヤコビアンは下式（２７）で定義される。但し、Ｊ_Lはリンク構造全体の角運動量ヤコビアン、Ｊ_ωiは剛体ｉの角速度ヤコビアンを表す。

上式（２７）より、角運動量操作空間に単位力を作用させるには、それに外積［−ｍ_i（ｘ_gi−ｘ_g）×］を掛けた力と、Ｉ_iを掛けたモーメントを各剛体の重心に作用させればよいことが分る。このときに角運動量操作空間に発生する加速度を下式（２８）から算出すれば、操作空間慣性逆行列を求めることができる。

本発明によれば、このように同時に複数の力やモーメントを作用させなければならない場合であっても、効率よく操作空間慣性逆行列を得ることができる。

全身運動制御への適用例：
ここまで説明してきたように、本発明によれば高い汎用性と高速性を兼ね備えた操作空間慣性逆行列の算出が可能である。脚式移動ロボットを力制御下で全身運動を実行する際には、多数の拘束条件を考慮してタスクを遂行しなければならないが、本発明を適用することにより、現在の計算機速度でも実現可能である。

図８には、脚式移動ロボットが全身運動を行なう際に考慮しなければならない拘束条件を図解している。同図（ａ）（ｂ）（ｃ）は環境との接触に関する拘束、（ｄ）は自己干渉に関する拘束を表しており、垂直反力及び摩擦力の作用方向に操作空間を設定する。（ｅ）は関節可動範囲に関する拘束であり、関節の回転方向に操作空間を設定する。（ｆ）は物体に対する手先運動の拘束であり、物体とエンド・エフェクタの相対位置及び姿勢方向に操作空間を設定する。（ｇ）は頭部カメラを水平に維持するための拘束であり、絶対空間における頭部の姿勢に関する操作空間を設定する。（ｈ）は重心に関する拘束であり重心に関する操作空間を設定し、（ｉ）は角運動量に関する拘束であり角運動量に関する操作空間を設定する。

これらの操作空間をすべてまとめた変数としてｘを考えると、すべての拘束は以下のような不等式を含む線形拘束条件として記述することができる。

ここで、上式（３０）及び（３１）中でそれぞれ存在するｆ_Ni及びｆ_Fiは、図８中で示した（ａ）（ｂ）（ｃ）のように外界との接触に関する拘束による作用点ｉにおける法線方向の外力及び摩擦力を表す。μ_iは当該作用点における摩擦係数である。

上式（２９）〜（３１）は線形相補性問題と呼ばれる。そして、操作空間慣性逆行列Λ^-1及びバイアス加速度ｃが既知であれば、この線形相補性問題を解くことによって、これらを満足するｘの加速度及び力ｆを決定することができる。線形相補性問題の数学的な解法自体については、例えば“ＦａｓｔＣｏｎｔａｃｔ Ｆｏｒｃｅ Ｃｏｍｐｕｔａｔｉｏｎ ｆｏｒ Ｎｏｎｐｅｎｅｔｒａｔｉｎｇ Ｒｉｇｉｄ Ｂｏｄｉｅｓ”（ＳＩＧＧＲＡＰＨ９４，ｐｐ．２３−３４，１９９４）に記載されているので、ここでは説明を省略する。

線形相補性問題としてｘの加速度及び力ｆを求める場合の多くは、最も計算を要する部分は操作空間慣性逆行列Λ^-1を得る部分となる。上述したように、本発明によれば、多様な操作空間に対しても高速な操作空間慣性逆行列の算出を行なうことができるので、ロボットの実機上でも実時間の処理が可能となる。こうして力ｆが得られれば、以下の操作空間制御（非特許文献１を参照のこと）により、上記で解いた拘束を満足する関節力τを下式（３２）のように決定することができる。

上式（３２）において、τ_nullはｆで表される拘束よりも優先順位の低い校則によって与えられた関節力であり、Ｊ^#はＪの一般化逆行列の一種である。上式（３２）を複数の拘束条件群の間で再帰的に用いることにより、拘束条件群の間に優先順位を付けることもできる。

仮想空間構築及び仮想空間操作への応用：
本発明に係る操作空間慣性逆行列の算出方法は、ロボットの力制御だけでなく、力学シミュレーション、コンピュータ・アニメーション、姿勢編集、外力推定など、多くの分野に適用することができる。例えば、仮想環境内に複数の物体が共存し、それらの間の衝突や接触といった物理的相互作用を計算機上で再現する力学シミュレーションを実時間で実行したり、その中のオブジェクトを操作したりする場合にも有効である。

力学シミュレーションの力学演算は、順動力学演算（非特許文献２を参照のこと）に対し、物体間の力を外力として作用させることで実行することができる。

図９には、物体間で衝突や接触といった物理的相互作用が印加されている様子を例示している。図９（Ａ）では、同図中（ａ）及び（ｂ）で示す箇所において仮想物体間の衝突が検出されるが、その際の外力は、物体間に指定した反発係数の反発と、適切な摩擦が実現するように、上式（２９）〜（３１）で示した線形相補性問題を解いて決定することができる。このとき、物体が衝突する部位（ａ）及び（ｂ）に垂直反力及び摩擦力の作用方向に操作空間を設定し、これまで説明してきた方法により操作空間慣性逆行列Λ^-1及びバイアス加速度ｃを高速に算出すれば、短時間で精密にこの外力を得ることができる。したがって、複雑な環境であってもリアルタイムで精緻な力学シミュレーションを実行することが可能となる。

また、図９（Ｂ）には、このような実時間力学シミュレーション環境に対し、ハプティック・デバイスＤ１を接続し、ハプティック・デバイスＤ１の先端の位置に基づいて仮想環境内のオブジェクトＰ１（プロキシ）の位置を変更するとともに、オブジェクトＰ１に作用した力をハプティック・デバイスＤ１にフィードバックするように構成した例を示している。

この場合、オブジェクトＰ１の位置がハプティック・デバイスＤ１の指令位置に一致するように、その相対位置偏差に関して操作空間（ｃ）を設定する。また、図９（Ａ）に示した例と同様に、オブジェクトＰ１と他オブジェクトとの衝突に関して垂直反力及び摩擦力の作用方向に操作空間（ｄ）を設定する。これらの拘束も上式（２９）〜（３１）で示される線形相補性問題として定式化される。この構成で計算量が最も多い操作空間逆行列Λ^-1も、本発明に係る方法であれば実時間で算出可能となり、仮想環境内の物体に触れた感じを、ハプティック・デバイスＤ１上でリアルタイムに且つ精密に再現することが可能となる。

また、図９（Ｃ）には、同図（Ｂ）に示した構成に加え、仮想空間内のオブジェクト（例えば人型キャラクタ）のさまざまな位置に、位置や姿勢に関する拘束（Ｌ１〜Ｌ４）を課せるようにした構成を示している。

位置や姿勢の拘束は式（２９）に示したような等式条件で表現することができる。操作空間は、位置や姿勢の拘束を設定する位置にワールド座標系と並行な３方向を設定すればよい。また、このような位置や姿勢に関する拘束をオブジェクトＰ２と物体間の相対位置及び姿勢に適用し、ここに操作空間（ｅ）を設定することで、ハプティック・デバイスＤ２を用いて仮想空間内の物体の所定の位置を引張ったり捩じったりして、そのときにオブジェクトＰ２に作用する力をハプティック・デバイスＤ２にフィードバックすることにより、仮想環境内で引張ったり捩じったりした反力を精緻に感じとることもできる。

これらの問題も操作空間を用いて定式化することができ、上式（２９）〜（３１）で示される線形相補性問題に還元される。図９（Ｃ）に示した構成でも計算量の最も多い箇所は操作空間逆行列Λ^-1の算出部となるが、本発明に係る算出方法によれば実時間でも算出可能となる。したがって、例えばユーザは、実際の物体を触れて姿勢を編集するのと同様の力をリアルタイムに感じながら、キャラクタの運動制御や姿勢編集などを行なうことができるようになる。また、このような力ベースの姿勢編集により、逆キネマティクスによく見られる特異点付近での不安定な挙動も除去することができる。

操作空間慣性逆行列を算出する機能的構成：
図１０には、本発明に係る方法に基づいて操作空間慣性逆行列を算出する装置の機能的構成を模式的に示している。図示の装置は、リンク構造モデル入力部１０と、順動力学演算部２０と、操作空間物理量演算部３０を備えている。当該装置は、例えば、パーソナル・コンピュータなどの一般的な計算機システム上で、順動力学演算や操作空間物理量演算を行なうソフトウェア・プログラムを起動することによって実現される。

リンク構造モデル入力部１０は、リンク構造を構成する個々の剛体の質量やイナーシャ、重心、幾何寸法、関節属性（摩擦、慣性）などの情報と、剛体間の接続方法、一般化変数（関節値）、一般化速度（関節速度）、一般化力（関節力）、順動力学演算に必要な変数（ＡｒｔｉｃｕｌａｔｅｄＢｏｄｙ Ｉｎｅｒｔｉａ、速度、力、加速度情報など）を入力し保持する。一般化変数、一般化速度、一般化力に変更が生じた場合は、保持しているリンク構造モデルに反映する。

順動力学演算部２０は、イナーシャ情報算出部２１と、速度情報算出部２２と、操作空間慣性逆行列用力情報算出部２３と、操作空間慣性逆行列用加速度情報算出部２４と、バイアス加速度用力情報算出部２５と、バイアス加速度用加速度情報算出部２６で構成され、操作空間慣性逆行列Λ^-1及びバイアス加速度ｃを算出する際に必要となる順動力学演算要素を提供する。

イナーシャ情報算出部２１は、上式（１２）に基づいて、リンク構造の末端から基底に向かって全リンクのイナーシャ情報（ＡｒｔｉｃｕｌａｔｅｄＢｏｄｙ Ｉｎｅｒｔｉａ）を算出する。その算出結果はリンク構造モデル入力部１０に記録しておく。イナーシャ情報の算出は、一般化変数の変更があった際に一度だけ実行される。

速度情報算出部２２は、上式（１３）に基づいて、リンク構造の基底から末端に向かって全リンクの速度を算出する。その算出結果はリンク構造モデル入力部１０に記録しておく。速度情報の算出は、一般化変数若しくは一般化速度の変更があった際に一度だけ実行される。

操作空間慣性逆行列用力情報算出部２３は、上式（２０）に基づいて、外力の作用点と基底を結ぶ経路上の剛体の力情報を算出する。外力は、操作空間物理量演算部３０によって与えられる。これに基づいて力情報算出経路が決定され、外力の作用点から基底に向かって力情報の計算が行なわれる。算出された力情報及び算出経路はリンク構造モデル入力部１０に記録される。

操作空間慣性逆行列用加速度情報算出部２４は、上式（２１）及び（２２）に基づいて加速度算出点と基底を結ぶ経路上の剛体の加速度情報を算出し、加速度算出点における加速度を上式（１８）によって算出する。但し、重力加速度は０として扱われる。加速度算出点は操作空間物理量演算部３０によって与えられる。これに基づいて加速度情報算出経路が決定され、基底から加速度算出点に向かって計算が行なわれる。一旦計算を行なった剛体についてはリンク構造モデル入力部１０に記録しておき、次の加速度算出点の加速度を算出する際の計算の起点とする。また、加速度情報が算出された結果は操作空間物理量演算部３０に送られる。

バイアス加速度用力情報算出部２５は、外力が作用しない条件下で上式（１４）及び式（１５）を算出し、末端から基底に向かって全剛体の力情報を算出する。算出された力情報はリンク構造モデル入力部１０に記録される。

バイアス加速度用加速度情報算出部２６は、式（１６）及び式（１７）に基づいて加速度算出点と基底を結ぶ経路上の剛体の加速度情報を算出し、加速度算出点における加速度を式（１８）によって算出する。加速度算出点は、操作空間物理量演算部３０によって与えられる。加速度算出点に基づいて加速度情報算出経路が決定され、基底から加速度算出点に向かって計算が行なわれる。一旦計算を行った剛体についてはリンク構造モデル入力部１０に記録しておき、次の加速度算出点の加速度を算出する際の計算の起点とする。算出された結果は操作空間物理量演算部３０に送られる。

操作空間物理量演算部３０は、操作空間慣性逆行列算出部３１と、操作空間バイアス加速度算出部３２で構成され、順動力学演算を繰り返し用いて，操作空間慣性逆行列及び操作空間バイアス加速度を算出する。

操作空間慣性逆行列算出部３１は、上式（１０）を繰り返し実行して、操作空間慣性逆行列Λ^-1の各列ｉを決定する。式（１０）の実行に際して、イナーシャ情報算出部２１、操作空間慣性逆行列用力情報算出部２２、操作空間慣性逆行列用加速度情報算出部２４による算出結果を用いる。イナーシャ情報は一般化変数に変更があった場合のみ更新する。外力の作用点と加速度計測点はユーザから与えられる操作空間情報に基づいて決定する。１つの操作空間に対応する外力を作用させた状態（操作空間慣性逆行列用力情報の算出を実行した状態）で、すべての操作空間に対応する加速度計測点の加速度を算出（操作空間慣性逆行列用加速度情報の算出を実行）し、各操作空間に発生する加速度を操作空間情報に基づいて算出するという作業を繰り返し、操作空間慣性逆行列Λ^-1を完成する。

操作空間バイアス加速度算出部３２は、上式（１１）を実行して、操作空間バイアス加速度を決定する。式（１１）の実行に際して、イナーシャ情報算出部２１、速度情報算出部２２、操作空間慣性逆行列用力情報算出部２３、操作空間慣性逆行列用加速度情報算出部２４による算出結果を用いる。イナーシャ情報は一般化変数に変更があった場合のみ更新する。加速度計測点をユーザから与えられる操作空間情報に基づいて決定し、一般化力、重力、速度積が作用する状態（バイアス加速度用力情報算出を実行した状態）で、すべての操作空間に対応する加速度計測点の加速度を算出（バイアス加速度用加速度情報算出を実行）し、各操作空間に発生する加速度を操作空間情報に基づいて算出することで、操作空間バイアス加速度ｃを得る。

図１１には、図１０に示した機能的構成を備えた装置上で操作空間慣性逆行列並びに操作空間バイアス加速度を求めるための処理手順をフローチャートの形式で示している。同図中、Ｐ１〜Ｐ１４の処理ステップでは操作空間慣性逆行列を求める処理を実行し、Ｑ１〜Ｑ８の処理ステップでは操作空間バイアス加速度を求める処理を実行する。

Ｐ１では、一般化変数、一般化速度、一般化力をリンク構造モデルに設定する。

Ｐ２では、ユーザより、算出対象となる操作空間に関する情報を取得する。ある剛体の特定部位に作用する力の方向や、２剛体間の相対変位、重心速度などの操作空間の種類毎に情報を保持する構造体を定義し、そのリストを入力する。

Ｐ３では、Ｐ２で得られた操作空間の情報から、各操作空間に対応する外力（１つの操作空間には複数の外力が対応し得る）のリストを作成する。

Ｐ４では、Ｐ３と同様に、Ｐ２で得られた操作空間の情報から、各操作空間に対応する加速度計測点（１つの操作空間には複数の加速度計測点が対応し得る）のリストを作成する。

Ｐ５及びＰ６では、Ａｒｔｉｃｕｌａｔｅｄ Ｂｏｄｙ Ｉｎｅｒｔｉａの更新が行なわれていない場合のみ、イナーシャ情報算出部２１により、上式（１２）を用いて全剛体のＡｒｔｉｃｕｌａｔｅｄＢｏｄｙ Ｉｎｅｒｔｉａを算出する。

Ｐ７では、Ｐ３で作成した外力リストから外力を１組ずつ取り出し、外力作用点から基底を結ぶ力情報算出経路を作成し、リンク構造モデルに記録する。

Ｐ８では、操作空間慣性逆行列用力情報算出部２３により、上式（２０）に基づいて、Ｐ７で抽出した外力が印加している状態における各剛体の力情報を算出する。

Ｐ９では、Ｐ４で作成した加速度計測点のリストから加速度計測点を１組ずつ取り出し、加速度計測点から加速度情報算出の終了している最近傍の剛体を結ぶ加速度情報算出経路を作成し、リンク構造モデルに記録する。

Ｐ１０では、操作空間慣性逆行列用加速度情報算出部２４により、上式（２１）及び（２２）に基づいて、Ｐ９で抽出した加速度計測点における加速度を算出する。

Ｐ１１では、Ｐ１０で得られた加速度計測点における加速度から操作空間加速度を算出する（例えば図１７下に示した例ならばＢ及びＣの各点での加速度の差を求める）。

Ｐ１２では、操作空間慣性逆行列算出部３１は、Ｐ１１で操作空間慣性逆行列加速度情報算出部２４が求めた操作空間加速度を操作空間慣性逆行列の所定の位置（外力のインデクスをｉ、加速度のインデクスをｊとすると、ｉ列ｊ行）に代入する。

Ｐ１３では、Ｐ４で作成した加速度測定点のすべてに関して加速度算出が終了したかどうかを判定する。終了していなければ、Ｐ９に戻って、同一の外力印加条件において別の加速度測定点における加速度を求める。

Ｐ１４では、Ｐ３で作成したすべての外力印加が終了したかどうかを判定する。終了していなければ、Ｐ７に戻って、別の外力印加を行なう。

以上の処理をすべての外力の組、及び加速度測定点の組に関して順次実行することで、操作空間慣性逆行列を得ることができる。なお、操作空間慣性逆行列は対称行列となるので、上記処理をｉ≧ｊ（あるいは、ｉ＜ｊ）についてのみ行なうように構成するとなお良い。

操作空間バイアス加速度を求める場合は、上記の操作空間慣性逆行列の算出に続けて以下の処理を実行する。

Ｑ１及びＱ２では、速度情報の更新が行なわれていない場合のみ、速度情報算出部２２により、全剛体の速度情報の算出を行なう。

Ｑ３では、バイアス加速度用力情報算出部２５により、外力が作用しない条件下で上式（１４）及び式（１５）を算出し、一般化力、重力、速度積に基づく力が印加している状態における各剛体の力情報を算出する。

Ｑ４では、Ｐ４で作成した加速度計測点のリストから加速度計測点を１組ずつ取り出し、加速度計測点から加速度情報算出の終了している最近傍の剛体を結ぶ加速度情報算出経路を作成して、リンク構造モデルに記録する。

Ｑ５では、操作空間バイアス加速度用加速度情報算出部２６により、式（１６）及び式（１７）に基づいて加速度算出点と基底を結ぶ経路上の剛体の加速度情報を算出し、Ｑ４で抽出した加速度計測点における加速度を式（１８）によって算出する。

Ｑ６では、操作空間バイアス加速度算出部３２は、上式（１１）を実行して、Ｑ５で得られた加速度計測点における加速度から、操作空間バイアス加速度を算出する（例えば、図１７下に示した例ならば、Ｂ並びにＣの各点での加速度の差を求める）。

Ｑ７では、Ｑ６で求めた操作空間加速度を操作空間バイアス加速度ベクトルの所定の位置（加速度のインデクスをｊとすると、ベクトルの第ｊ成分）に代入する。

Ｑ８では、Ｐ４で作成した加速度測定点のすべてに関して加速度算出が終了したかどうかを判定する。終了していなければ、Ｑ４に戻って、別の加速度測定点における加速度を求める。

以上の処理をすべての加速度測定点の組に関して実行することで、操作空間バイアス加速度を得ることができる。

並列化：
上述した操作空間慣性逆行列並びに操作空間バイアス加速度の計算方法は、グリッド・コンピュータやクラスタＰＣとも呼ばれる並列計算機上で実行することで、さらに高速化することができる。何故ならば、操作空間慣性逆行列はＭ×Ｍの正方行列であるが、その各要素は別々のＣＰＵで独立に求めることができるからである。また、操作空間バイアス加速度は要素数Ｍのベクトルであるが、その各要素も別々のＣＰＵで独立に求めることができる。

図１０には本発明に係る方法に基づいて操作空間慣性逆行列及びバイアス加速度を算出する装置の機能的構成を示したが、図１２には処理の並列化を実現した変形例を示している。図１０のとの主な相違は以下の３点である。

（１）順動力学演算部２０内の操作空間慣性逆行列用力情報算出部２５を、操作空間慣性逆行列の列毎に独立に設け、各列の演算を別々のＣＰＵに分配して実行できるように構成した。
（２）上記の列毎に設けた操作空間慣性逆行列用力情報算出部２５の力情報を用い、同一列内の行方向の各要素を求めるため、操作空間慣性逆行列用加速度情報算出部２６を行毎に設け、各要素の演算をそれぞれ別々のＣＰＵに分配して実行できるように構成した。
（３） バイアス加速度用力情報算出部２５の力情報を用いてバイアス加速度ベクトルの各行の要素を求めるため、バイアス加速度用加速度情報算出部２６を行毎に設け、各行の演算をそれぞれ別々のＣＰＵに分配して実行できるように構成した。

図１３には、操作空間慣性逆行列及びバイアス加速度を並列計算する装置のうち、操作空間慣性逆行列を算出する部分についての処理手順をフローチャートの形式で示している。但し、同図では２×２の操作空間慣性逆行列を求める場合について例示している。

並列化した場合も、Ｐ６のイナーシャ情報算出までは図１０に示した処理手順と同様である。

順動力学演算部２０内の操作空間慣性逆行列用力情報算出部２５を実行するＣＰＵを操作空間慣性逆行列の第１列及び第２列で独立に設けている。すなわち、Ｐ７の力情報算出は列毎に設けられた別々のＣＰＵ１１及び１２で並行して実行される。

さらに、第１列及び第２列では、操作空間慣性逆行列用加速度情報算出部２６を実行するＣＰＵを行毎に設け、Ｐ９以降における各要素の演算をそれぞれ別々のＣＰＵに分配して実行できるように構成している。すなわち、第１列では、ＣＰＵ１１を用いて算出された操作空間慣性逆行列用力情報を用いた加速度情報の算出処理をそれぞれ独立したＣＰＵ１１及び２１に分配して、当該列内の行方向の２要素を並行して求める。同様に、第２列では、ＣＰＵ１２を用いて算出された操作空間慣性逆行列用力情報を用いた加速度情報の算出処理をそれぞれ独立したＣＰＵ１２及び２２に分配して、当該列内の行方向の２要素を並行して求める。

また、図１４には、操作空間慣性逆行列及びバイアス加速度を並列計算する装置のうち、操作空間バイアス加速度を算出する部分についての処理手順をフローチャートの形式で示している。但し、同図でも２×２の操作空間慣性逆行列を求める場合について例示している。

並列化した場合も、Ｑ３の一般化力印加（力情報算出）までは図１０に示した処理手順と同様である。そして、バイアス加速度用力情報算出部２５の力情報を用いてバイアス加速度ベクトルの各行の要素を求めるため、バイアス加速度用加速度情報算出部２６を実行するＣＰＵを行毎に設けており、Ｑ４以降の加速度情報算出を行毎に設けられたＣＰＵ０１及び０２を用いて並列実行している。

図１２〜図１４に示したように、操作空間慣性逆行列及びバイアス加速度を並列計算するように装置を構成することで、行列又はベクトルの要素毎に並列に計算が行なわれることから、行列又はベクトルの１要素を計算する計算時間と同程度の時間で全要素の算出を完了することができるので、全体として大幅な高速化を図ることができる。

上記では、操作空間慣性逆行列の各要素をすべて別々のＣＰＵを用いて並列に求める構成を示したが、その一部を並列化して実行するのみでも大幅な高速化を図ることができる。

図１５には、操作空間慣性逆行列の列毎に並列に求めるように構成した例を示している。図１０のとの主な相違は以下の３点である。

（１）順動力学演算部２０内の操作空間慣性逆行列用力情報算出部２５を、操作空間慣性逆行列の列毎に独立に設け、各列の演算を別々のＣＰＵに分配して実行できるようにした。
（２）上記の列毎に設けた操作空間慣性逆行列用力情報算出部２５の力情報を用い、同一列内の行方向の各要素を求めるため、操作空間慣性逆行列用加速度情報算出部２６を列毎に１つずつ設け、これを繰り返し用いて１列分の加速度を算出するようにした。

図１６には、図１５に示した装置構成により操作空間慣性逆行列の算出を行なうための処理手順をフローチャートの形式で示している。但し、同図では３×３の操作空間慣性逆行列を求める場合について例示している。

並列化した場合も、Ｐ６のイナーシャ情報算出までは図１０に示した処理手順と同様である。

順動力学演算部２０内の操作空間慣性逆行列用力情報算出部２５を実行するＣＰＵを操作空間慣性逆行列の第１列及び第２列で独立に設けている。すなわち、Ｐ７の力情報算出は列毎に設けられた別々のＣＰＵ１〜３で並行して実行される。

そして、列毎に設けられたＣＰＵ１〜３を用い、同一列内の行方向の各要素を求めるための操作空間慣性逆行列用加速度情報算出部２６に相当する演算を繰り返し実行して、それぞれで１列分の加速度情報を算出する。

図１５〜図１６に示したように操作空間慣性逆行列及びバイアス加速度を並列計算するように装置を構成することで、操作空間慣性逆行列の列毎に並列に計算が行なわれることから、操作空間慣性逆行列の行数に比例した時間内で操作空間慣性逆行列の全要素の算出を完了することができ、効果的に高速化を図ることができる。

複数のＣＰＵを利用可能な場合、例えば、“Ａ Ｄｉｖｉｄｅ−ａｎｄ−Ｃｏｎｑｕｅｒ Ａｒｔｉｃｕｌａｔｅｄ−ＢｏｄｙＡｌｇｏｒｉｔｈｍ ｆｏｒ Ｐａｒａｌｌｅｌ Ｏ（ｌｏｇ（ｎ）） Ｃａｌｃｕｌａｔｉｏｎ ｏｆ Ｒｉｇｉｄ−Ｂｏｄｙ Ｄｙｎａｍｉｃｓ”（Ｊ． ＲｏｂｏｔｉｃｓＲｅｓｅａｒｃｈ，ｖｏｌ．１８，ｎｏ．９，ｐｐ．８６７−８７５，１９９９）に記載されている、並列計算に対応した順動力学演算を用いることで、さらに計算速度を向上させることができる。

以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。

本明細書では、脚式移動ロボットなど、複雑な構造を有し、外界とのインタラクションを行ないながらタスクを遂行するロボットの動作を力制御する実施形態を中心に説明してきたが、本発明の要旨はこれに限定されるものではない。本発明に係る操作空間慣性逆行列の算出方法は、力制御、力学シミュレーション、コンピュータ・アニメーション、姿勢編集、外力推定など、多くの分野に適用することができる。

また、本明細書では、順動力学演算にＡｒｔｉｃｕｌａｔｅｄ Ｂｏｄｙ法を用いることでＯ（Ｎ）の演算効率で操作空間慣性逆行列（及び操作空間バイアスベクトル）の算出を可能としたが、ベースとするアルゴリズムはこれに限定されない。

要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。

図１は、本発明に係る力制御の適用対象となるロボット装置の外観構成を示した図である。 図２は、図１に示したロボット装置における結線トポロジの構成例を示した図である。 図３は、図１に示したロボット装置の関節自由度モデルを示した図である。 図４は、床面に設置された多リンク構成の装置において、末端から基底に向かって力情報を算出する通常の経路を示した図である。 図５は、床面に設置された多リンク構成の装置において、基底から末端に向かって加速度情報を算出する通常の経路を示した図である。 図６は、床面に設置された多リンク構成の装置において、末端から基底に向かって力情報を算出する最適化された経路を示した図である。 図７は、床面に設置された多リンク構成の装置において、各点における加速度を基底から末端に向かって算出するときの最適化された経路を示した図である。 図８は、脚式移動ロボットが全身運動を行なう際に考慮しなければならない拘束条件を説明するための図である。 図９は、物体間で衝突や接触といった物理的相互作用が印加されている様子を例示した図である。 図１０は、本発明に係る方法に基づいて操作空間慣性逆行列を算出する装置の機能的構成を模式的に示した図である。 図１１は、図１０に示した機能的構成を備えた装置上で操作空間慣性逆行列並びに操作空間バイアス加速度を求めるための処理手順を示したフローチャートである。 図１２は、操作空間慣性逆行列の各要素を算出する処理を並列実行する装置の機能的構成を模式的に示した図である。 図１３は、操作空間慣性逆行列及びバイアス加速度を並列計算する装置のうち、操作空間慣性逆行列を算出する部分についての処理手順を示したフローチャートである。 図１４は、操作空間慣性逆行列及びバイアス加速度を並列計算する装置のうち、操作空間バイアス加速度を算出する部分についての処理手順を示したフローチャートである。 図１５は、操作空間慣性逆行列の列毎に並列に求めるように構成した例を示した図である。 図１６は、図１５に示した装置構成により操作空間慣性逆行列の算出を行なうための処理手順を示したフローチャートである。 図１７は、多軸ロボットの構成系を示した図である。 図１８は、１つの操作空間に対応する力がすべてのリンクの重心位置に作用する様子を示した図である。

符号の説明

１０…リンク構造モデル入力部
２０…順動力学演算部
２１…イナーシャ情報算出部
２２…速度情報算出部
２３…操作空間慣性逆行列用力情報算出部
２４…操作空間慣性逆行列用加速度情報算出部
２５…バイアス加速度用力情報算出部
２６…バイアス加速度用加速度情報算出部
３０…操作空間物理量演算部
３１…操作空間慣性逆行列算出部
３２…操作空間バイアス加速度算出部

Claims (32)

1. 複数の剛体が連なって構成されるリンク構造物の関節の値をすべて連ねてできるベクトルである一般化変数ｑの時間微分値とヤコビアンＪを用いて関連付けられる物理量ｘに対して定義される操作空間における物理量を算出する操作空間物理量算出装置であって、
   前記リンク構造物の力学モデルを、一般化変数ｑ及びその高階微分値と重力又はコリオリ力ｂと外力ｆと一般化変数ｑに対応した一般化力τとの関係を表す運動方程式に基づいて、外力ｆ及び外力が作用しない場合に操作空間に発生する操作空間バイアス加速度ｃと前記リンク構造物の各点に発生する加速度の間の操作空間慣性逆行列Λ^-1で与えられる関係で表わし、
   前記リンク構造物の一般化変数ｑ及びその速度と、一般化力τ、重力ｇ、外力ｆのうち少なくとも１つを含む前記リンク構造物に作用する力情報に基づいて前記リンク構造物の各点に発生する加速度を得るための順動力学演算を実行する順動力学演算手段と、
   一般化変数ｑ及び外力ｆ以外をすべてゼロとし、外力のみが作用し得る制約条件下で、ｉ番目の操作空間にのみ単位外力を作用させて前記順動力学演算手段による順動力学演算を実行して操作空間慣性逆行列の第ｉ列を順次算出することにより、操作空間慣性逆行列全体を求める操作空間慣性逆行列算出手段と、
   外力ｆをゼロとし、一般化変数ｑ及びその速度、一般化力τ、重力ｇのみが作用する制約条件下で前記順動力学演算手段による順動力学演算を実行して、リンク構造物の各点に発生する加速度を操作空間バイアス加速度ｃとして求める操作空間バイアス加速度算出手段と、
   を具備することを特徴とする操作空間物理量算出装置。
2. 前記順動力学演算手段は、順動力学演算をイナーシャ情報算出、速度情報算出、力情報算出、加速度情報算出の４つの過程に分解して実行する、
   ことを特徴とする請求項１に記載の操作空間物理量算出装置。
3. 前記順動力学演算手段は、すべてのリンクのＩ^A _i（Ａｒｔｉｃｕｌａｔｅｄ Ｂｏｄｙ Ｉｎｅｒｔｉａ）について下式を末端から基底に向かって実行することでイナーシャ情報を算出する（但し、Ｉ_iはリンクｉのイナーシャ、Ｃ（ｉ）はリンクｉの子リンクのインデックス集合とする）、
   ことを特徴とする請求項２に記載の操作空間物理量算出装置。
   

   
4. 前記順動力学演算手段は、下式を基底から末端に向かって実行することですべてのリンクの原点の速度を求めることにより、速度情報を算出する（但し、ｖ_iはリンクｉの速度、ｐはその親リンクのインデックス、Ｓ_iはリンクｉの関節の運動自由度を表す行列、ｑ_iはリンクｉの関節値である）、
   ことを特徴とする請求項２に記載の操作空間物理量算出装置。
   

   
5. 前記順動力学演算手段は、同時に複数の外力を作用できるように順動力学演算を構成する、
   ことを特徴とする請求項２に記載の操作空間物理量算出装置。
6. 前記順動力学演算手段は、下式を末端から基底に向かって実行してすべてのリンクのバイアス力ｐ^A _iを求めることにより、力情報を算出する（ｆ_kはリンクｉの中でｋ番目の外力、Ｆ（ｉ）はリンクｉに作用する外力のインデックス集合、τ_iはリンクｉの関節力である）、
   ことを特徴とする請求項５に記載の操作空間物理量算出装置。
   

   
7. 前記順動力学演算手段は、複数の点の加速度を算出できるように順動力学演算を構成する、
   ことを特徴とする請求項２に記載の操作空間物理量算出装置。
8. 前記順動力学演算手段は、下式を基底から末端に向かって実行してすべてのリンクの加速度ａ_iを求め、
   

   

   リンクｉに作用する単位外力ｎ方向に発生する物理量ｘの加速度を下式から求めることにより、
   

   

   加速度情報を算出する（但し、ａ_iはリンクｉの加速度）、
   ことを特徴とする請求項７に記載の操作空間物理量算出装置。
9. 前記順動力学演算手段は、一般化変数としての関節値ｑが変化したときに一度だけイナーシャ情報を算出する、
   ことを特徴とする請求項３に記載の操作空間物理量算出装置。
10. 前記順動力学演算手段は、操作空間バイアス加速度を算出するときにのみ速度情報の算出を実行し、操作空間慣性逆行列を算出するときには実行しない、
    ことを特徴とする請求項４に記載の操作空間物理量算出装置。
11. 前記順動力学演算手段は、操作空間慣性逆行列を算出する際には、下式を末端から基底に向かって実行してすべてのリンクのバイアス力ｐ^A _iを求めることにより、力情報を算出する、
    ことを特徴とする請求項６に記載の操作空間物理量算出装置。
    

    
12. 前記順動力学演算手段は、力の作用点と基底を結ぶ経路上のリンクに関してのみリンクのバイアス力ｐ^A _iを算出する、
    ことを特徴とする請求項１１に記載の操作空間物理量算出装置。
13. 前記順動力学演算手段は、操作空間慣性逆行列を算出する際には、下式を基底から末端に向かって実行してすべてのリンクの加速度ａ_iを求める、
    ことを特徴とする請求項８に記載の操作空間物理量算出装置。
    

    
14. 前記順動力学演算手段は、ある点の加速度算出を行なう場合、既に加速度算出の行なわれた最寄りの点を起点として前記のある点に向かう曲線上の経路に沿って加速度更新を実行する、
    ことを特徴とする請求項１３に記載の操作空間物理量算出装置。
15. 前記順動力学演算手段は、操作空間バイアス加速度を算出する際には、基底から加速度算出を行なうリンクに向かう経路上でのみ加速度算出を行なう、
    ことを特徴とする請求項８に記載の操作空間物理量算出装置。
16. 前記操作空間に対応する力が前記リンク構造物に２点に作用する場合に、前記操作空間慣性逆行列算出手段は、末端から基底に向かってすべてのリンクに関してイナーシャ情報を算出し、前記の各作用点に（操作空間方向の）単位力を作用した状態で、各作用点と基底を結ぶそれぞれの経路上のリンクに関して、末端から基底に向かって力情報を算出し、基底から各作用点に向かって加速度情報を算出し（但し、重力加速度ｇは０として算出する）、各作用点における加速度の差をとって、両点間の相対運動に関する操作空間慣性逆行列を得る、
    ことを特徴とする請求項２に記載の操作空間物理量算出装置。
17. 前記操作空間バイアス加速度算出手段は、基底から末端に向かってすべてのリンクに関して速度情報を算出し、すべてのリンクに関節力τを作用した状態で末端から基底に向かって力情報を算出し、基底から各作用点に向かって加速度情報を算出して、各作用点における加速度の差をとって、両点間の相対運動に関する操作空間バイアス加速度を得る、
    ことを特徴とする請求項１６に記載の操作空間物理量算出装置。
18. 前記操作空間慣性逆行列算出手段は、末端から基底に向かってすべてのリンクに関してイナーシャ情報を算出し、全リンクに（操作空間方向の）単位力のｍ_i／Ｍ の力を作用した状態で末端から基底に向かって力情報を算出し（但し、Ｍはリンク構造全体の質量、ｍ_iは剛体ｉの質量を表す）、基底から末端に向かって加速度情報を算出し（但し、起点の重力加速度は０として算出する）、各剛体ｉの重心ｘ_giの加速度を算出し、リンク構造全体の重心に発生する加速度を算出することにより、前記リンク構造体の重心操作空間における操作空間慣性逆行列を得る、
    ことを特徴とする請求項２に記載の操作空間物理量算出装置。
19. 前記操作空間慣性逆行列算出手段は、角運動量操作空間における操作空間慣性逆行列を求める際に、角運動量操作空間に外積［−ｍ_i（ｘ_gi−ｘ_g）×］を掛けた力と、Ｉ_iを掛けたモーメントを各剛体の重心に作用させて、角運動量操作空間に単位力を作用させる（但し、ｍ_iは剛体ｉの質量、ｘ_gはリンク構造全体の重心，ｘ_giは剛体ｉの重心を表す）、
    ことを特徴とする請求項１に記載の操作空間物理量算出装置。
20. 前記操作空間慣性逆行列算出手段は、角運動量操作空間に発生する加速度を下式より算出して操作空間慣性逆行列を求める（但し、Ｌは前記リンク構造体全体の角運動量）、
    ことを特徴とする請求項１９に記載の操作空間物理量算出装置。
    

    
21. 前記操作空間慣性逆行列算出手段は、操作空間慣性逆行列の上三角行列又は下三角行列のみを算出する、
    ことを特徴とする請求項１に記載の操作空間物理量算出装置。
22. 前記順動力学演算手段は、剛体間の再帰演算で構成される、
    ことを特徴とする請求項１に記載の操作空間物理量算出装置。
23. 再帰演算の対象となる剛体を抽出する計算経路決定手段をさらに備え、
    前記順動力学演算手段は、前記計算経路決定手段により決定される剛体のみを再帰演算の対象とする、
    ことを特徴とする請求項２２に記載の操作空間物理量算出装置。
24. 操作空間慣性逆行列又は操作空間バイアス加速度の各要素の計算を複数のプロセッサに分配して並列実行する、
    ことを特徴とする請求項１に記載の操作空間物理量算出装置。
25. 前記順動力学演算手段は、操作空間慣性逆行列用の力情報を算出するためのプロセッサを操作空間慣性逆行列の列毎に独立に設け、操作空間慣性逆行列の各列の演算を並列実行する、
    ことを特徴とする請求項２４に記載の操作空間物理量算出装置。
26. 前記順動力学演算手段は、前記の列毎に設けられたプロセッサから出力される力情報を用いて操作空間慣性逆行列用の加速度情報を算出するためのプロセッサを行毎に独立に設け、同一列内の行方向の各要素の演算を並列実行する、
    ことを特徴とする請求項２５に記載の操作空間物理量算出装置。
27. 前記順動力学演算手段は、操作空間バイアス加速度用の力情報を算出するためのプロセッサをバイアス加速度ベクトルの行毎に独立に設け、バイアス加速度用加速度情報の各行の演算を並列実行する、
    ことを特徴とする請求項２５に記載の操作空間物理量算出装置。
28. 前記順動力学演算手段は、前記の列毎に設けられたプロセッサから出力される力情報を用いて操作空間慣性逆行列用の加速度情報を算出するためのプロセッサを列毎に１つずつ設け、それぞれの列でプロセッサを繰り返し用いて１列分の加速度を算出する、
    ことを特徴とする請求項２５に記載の操作空間物理量算出装置。
29. 前記リンク構造物を力制御下で動作制御するために用いられ、
    該求められた操作空間慣性逆行列及び操作空間バイアス加速度を用いて前記リンク構造物における関節力を決定する関節力決定手段をさらに備える、
    ことを特徴とする請求項１に記載の操作空間物理量算出装置。
30. 前記関節力決定手段は、操作空間慣性逆行列及び操作空間バイアス加速度を用いて記述される操作空間における加速度と力との関係を記述した等式と、前記リンク構造体のリンク同士及びリンクと環境との間で形成される拘束条件からなる線形相補性問題を満足する加速度と力を、前記操作空間物理量算出手段により求められた操作空間慣性逆行列及び操作空間バイアス加速度を用いて求め、得られた力に基づいてリンクの関節力を決定する、
    ことを特徴とする請求項２９に記載の操作空間物理量算出装置。
31. 複数の剛体が連なって構成されるリンク構造物の関節の値をすべて連ねてできるベクトルである一般化変数ｑの時間微分値とヤコビアンＪを用いて関連付けられる物理量ｘに対して定義される操作空間における物理量を算出する操作空間物理量算出方法であって、
    前記リンク構造物の力学モデルを、一般化変数ｑ及びその高階微分値と重力又はコリオリ力ｂと外力ｆと一般化変数ｑに対応した一般化力τとの関係を表す運動方程式に基づいて、外力ｆ及び外力が作用しない場合に操作空間に発生する操作空間バイアス加速度ｃと前記リンク構造物の各点に発生する加速度の間の操作空間慣性逆行列Λ^-1で与えられる関係で表わし、
    前記リンク構造物の一般化変数ｑ及びその速度と、一般化力τ、重力ｇ、外力ｆのうち少なくとも１つを含む前記リンク構造物に作用する力情報に基づいて前記リンク構造物の各点に発生する加速度を得るための順動力学演算を実行する順動力学演算ステップと、
    一般化変数ｑ及び外力ｆ以外をすべてゼロとし、外力のみが作用し得る制約条件下で、ｉ番目の操作空間にのみ単位外力を作用させて前記順動力学演算を実行して操作空間慣性逆行列の第ｉ列を順次算出することにより、操作空間慣性逆行列全体を求める操作空間慣性逆行列算出ステップと、
    外力ｆをゼロとし、一般化変数ｑ及びその速度、一般化力τ、重力ｇのみが作用する制約条件下で前記順動力学演算を実行して、リンク構造物の各点に発生する加速度を操作空間バイアス加速度ｃとして求める操作空間バイアス加速度算出ステップと、
    を有することを特徴とする操作空間物理量算出方法。
32. 複数の剛体が連なって構成されるリンク構造物の関節の値をすべて連ねてできるベクトルである一般化変数ｑの時間微分値とヤコビアンＪを用いて関連付けられる物理量ｘに対して定義される操作空間における物理量を算出するための処理をコンピュータ・システム上で実行させるようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
    前記リンク構造物の力学モデルを、一般化変数ｑ及びその高階微分値と重力又はコリオリ力ｂと外力ｆと一般化変数ｑに対応した一般化力τとの関係を表す運動方程式に基づいて、外力ｆ及び外力が作用しない場合に操作空間に発生する操作空間バイアス加速度ｃと前記リンク構造物の各点に発生する加速度の間の操作空間慣性逆行列Λ^-1で与えられる関係で表わし、
    前記リンク構造物の一般化変数ｑ及びその速度と、一般化力τ、重力ｇ、外力ｆのうち少なくとも１つを含む前記リンク構造物に作用する力情報に基づいて前記リンク構造物の各点に発生する加速度を得るための順動力学演算を実行する順動力学演算手順と、
    一般化変数ｑ及び外力ｆ以外をすべてゼロとし、外力のみが作用し得る制約条件下で、ｉ番目の操作空間にのみ単位外力を作用させて前記順動力学演算を実行して操作空間慣性逆行列の第ｉ列を順次算出することにより、操作空間慣性逆行列全体を求める操作空間慣性逆行列算出手順と、
    外力ｆをゼロとし、一般化変数ｑ及びその速度、一般化力τ、重力ｇのみが作用する制約条件下で前記順動力学演算を実行して、リンク構造物の各点に発生する加速度を操作空間バイアス加速度ｃとして求める操作空間バイアス加速度算出手順と、
    を実行させることを特徴とするコンピュータ・プログラム。
    

Images