(57)【要約】【課題】剛体力学シミュレーションにおいて、接触若しくは衝突する剛体間に作用する接触力若しくは衝突力を精度よく且つ低計算コストで算出する。【解決手段】操作空間慣性行列のノミナル値Λnと物理量ｘの加速度目標値から、操作空間に作用するノミナルな慣性力を求める一方、ノミナルな慣性力以外の作用力を外乱ｆdとして外乱オブザーバにより推定して、ノミナルな慣性力と、推定された外乱ｆdに基づいて、物理量に作用する力ｆを求める。ノミナルな操作空間慣性行列Λnの設定方法として、その非対角成分を零とし、対角成分のみ理論値を算出する。【選択図】  図３
2009099082.tif 000002
【特許請求の範囲】
【請求項１】
複数の物体が共存する環境下で各物体間に働く物理的相互作用の力学シミュレーションを行なう力学シミュレーション装置であって、
物体間の接触若しくは衝突事象を物体に作用する力と発生する加速度の関係を記述する操作空間上で扱い、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する線形等式からなる運動方程式と、各衝突点で働く作用力ｆの各成分に課される線形不等式拘束条件からなる線形相補性問題を解く際に、
操作空間慣性行列のノミナル値Λnを与えるノミナル値付与手段と、
操作空間慣性行列のノミナル値Λnと物理量ｘの加速度目標値から、操作空間に作用するノミナルな慣性力を求めるノミナル慣性力算出手段と、
操作空間に作用する、ノミナルな慣性力以外の力を外乱ｆdとして推定する外乱オブザーバと、
ノミナルな慣性力と、推定された外乱ｆdに基づいて、物理量に作用する力ｆを求める手段と、
を具備することを特徴とする力学シミュレーション装置。
【請求項２】
物理量ｘの加速度目標値は、接触若しくは衝突する物体間における接触点毎の垂直反力方向並びに摩擦力方向の成分を持ち、
前記ノミナル慣性力算出手段は、順動力学演算の積分結果により得られる物理量ｘの垂直反力方向並びに摩擦力方向の操作空間速度の値に基づいて、加速度目標値を求める、
ことを特徴とする請求項１に記載の力学シミュレーション装置。
【請求項３】
前記外乱オブザーバは、物理量ｘに作用する力ｆと操作空間に発生した物理量ｘの加速度から、外乱ｆdを推定する、
ことを特徴とする請求項１に記載の力学シミュレーション装置。
【請求項４】
前記外乱オブザーバは、フィードバック演算により作用力ｆを算出し、順動力学演算により作用力ｆから物理量ｘの加速度を算出し、該算出された物理量ｘの加速度に操作空間慣性行列のノミナル値Λnを掛けた値を作用力ｆから引き算することで、外乱ｆdを推定する、
ことを特徴とする請求項１に記載の力学シミュレーション装置。
【請求項５】
前記外乱オブザーバは、物理量ｘの加速度目標値と順動力学演算により得られる加速度の誤差が十分小さくなるまで、フィードバック演算を繰り返す、
ことを特徴とする請求項１に記載の力学シミュレーション装置。
【請求項６】
前記フィードバック演算により作用力ｆを算出した後に、作用力ｆの各成分に対して前記線形不等式拘束を課すリミット処理を付加する手段をさらに備える、
ことを特徴とする請求項５に記載の力学シミュレーション装置。
【請求項７】
前記ノミナル値付与手段は、操作空間慣性行列Λのクロス・イナーシャ（非対角成分）を零とし、対角成分のみ理論値を算出して与える、
ことを特徴とする請求項１に記載の力学シミュレーション装置。
【請求項８】
前記ノミナル値付与手段は、操作空間のすべての行ｉについて、ｉ番目の操作空間にのみ単位力（すなわちｆ＝ｅi）を作用させる条件下で順動力学演算を実行し、操作空間慣性逆行列Λ-1の第ｉ行ｉ列を順次求めて得られる操作空間慣性逆行列Λ-1の対角成分から、操作空間慣性行列Λの対角成分の理論値を算出する、
ことを特徴とする請求項７に記載の力学シミュレーション装置。
【請求項９】
複数の物体が共存する環境下で各物体間に働く物理的相互作用の力学シミュレーションを行なう力学シミュレーション方法であって、
物体間の接触若しくは衝突事象を物体に作用する力と発生する加速度の関係を記述する操作空間上で扱い、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する線形等式からなる運動方程式と、各衝突点で働く作用力ｆの各成分に課される線形不等式拘束条件からなる線形相補性問題を解く際に、
操作空間慣性行列のノミナル値Λnを与えるノミナル値付与ステップと、
操作空間慣性行列のノミナル値Λnと物理量ｘの加速度目標値から、操作空間に作用するノミナルな慣性力を求めるノミナル慣性力算出ステップと、
操作空間に作用する、ノミナルな慣性力以外の力を外乱ｆdとして推定する外乱観測ステップと、
ノミナルな慣性力と、推定された外乱ｆdに基づいて、物理量に作用する力ｆを求めるステップと、
を具備することを特徴とする力学シミュレーション装置。
【請求項１０】
複数の物体が共存する環境下で各物体間に働く物理的相互作用の力学シミュレーションを行なうための処理をコンピュータ上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、
物体間の接触若しくは衝突事象を物体に作用する力と発生する加速度の関係を記述する操作空間上で扱い、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する線形等式からなる運動方程式と、各衝突点で働く作用力ｆの各成分に課される線形不等式拘束条件からなる線形相補性問題を解く際に、前記コンピュータを、
操作空間慣性行列のノミナル値Λnを与えるノミナル値付与手段と、
操作空間慣性行列のノミナル値Λnと物理量ｘの加速度目標値から、操作空間に作用するノミナルな慣性力を求めるノミナル慣性力算出手段と、
操作空間に作用する、ノミナルな慣性力以外の力を外乱ｆdとして推定する外乱オブザーバと、
ノミナルな慣性力と、推定された外乱ｆdに基づいて、物理量に作用する力ｆを求める手段と、
として機能させるためのコンピュータ・プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば複数の物体が共存する仮想環境や遠隔環境下で各物体間の衝突や接触といった物理的相互作用を計算機上で再現する力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムに係り、特に、剛体力学シミュレーションにおいて、接触若しくは衝突する剛体間に作用する接触力若しくは衝突力を算出する力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムに関する。
【０００２】
さらに詳しくは、本発明は、物体間の接触若しくは衝突事象を物体に作用する力と発生する加速度の関係を記述する操作空間上で扱い、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する線形等式からなる運動方程式と、各衝突点で働く作用力ｆに課される線形不等式拘束条件からなる線形相補性問題を解くことによって、各衝突点で働く作用力ｆを求める力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムに係り、特に、操作空間慣性逆行列Λ-1を算出してから線形相補性問題を解決することによって、各衝突点ｉで働く垂直反力ｆNi並びに摩擦力ｆFiを求める力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムに関する。
【背景技術】
【０００３】
ゲームやバーチャル・リアリティといった技術分野では、視覚情報や聴覚情報に加えて、力学シミュレーションによる物理法則に基づいた動き情報のリアリティ向上、インタラクティビティの向上が望まれている。この種の力学シミュレーションでは、立体間の衝突が発生したことを検出し、さらに、衝突物体の衝突箇所に適切な反力を作用させる必要がある。
【０００４】
立体間の衝突を検出する方法としては、例えば、立体が衝突又は接触している状況において、両者の間に形成される接触多角形を完全に且つ高速に算出する接触形状算出方法について提案がなされている（例えば、特許文献１を参照のこと）。この接触形状算出方法は、衝突検出アルゴリズムで検出された衝突代表点対を結ぶベクトルを法線ベクトルとする接触平面と定義して、立体毎に接触平面との接触形状を与える半接触多角形を求め、続いて、各立体についての半接触多角形の共通領域を立体間の接触形状として算出するようになっている。図９に示すように２つの立方体が衝突する例では、図中のＣで表されるような接触点が算出される。
【０００５】
接触点Ｃには、法線方向に垂直反力ｆNが作用するとともに、法線と直交する方向には互いに直交する摩擦力ｆFX及びｆFY が作用する。力学シミュレーションでは、２つの立方体が指定された反発係数で相互に排斥し合うように、垂直反力ｆN を決定しなければならない。また、「各摩擦力ｆFX及び ｆFYはともに絶対値が摩擦係数μ×ｆN以下」という不等式拘束条件下で、最大限、２つの立方体間の相対速度を０にする（すなわち、力の釣り合いにより立方体が静止する）ように決定しなければならない。
【０００６】
力学シミュレーションにより求めた垂直反力ｆNの値が小さ過ぎると、立方体は相互に浸透し、逆に、垂直反力ｆNの値が大き過ぎると過大な速度で遠のき合い、場合によっては数値発散を起こし不安定となる。また、力学シミュレーションにより求めた摩擦力ｆFX及び ｆFYについても、過小であれば一方の立方体が他方の立方体に対し容易に滑ってしまって静止できないし、逆に過大であれば数値発散を招く。言い換えれば、力学シミュレーションでは、垂直反力ｆNや摩擦力ｆFX及び ｆFYを精度よく求める必要がある。
【０００７】
これら垂直反力ｆNや摩擦力ｆFX及び ｆFYを精度よく求める方法として、「解析型」と呼ばれる方法が知られている。この解析型の方法は、衝突後速度に到達するための外力をダイナミクスを考慮して決定するものであり、計算量は膨大となるが解析的により正確に求めることができる。
【０００８】
ここで、力学並びに運動に関する重要な概念として、「操作空間（Ｏｐｅｒａｔｉｏｎａｌ  Ｓｐａｃｅ）」と呼ばれる概念がある。操作空間は、物体に作用する力と発生する加速度の関係を記述するための空間である。力覚提示を行なう場合や、位置制御ではなく力制御する際に、物体間の接し方を拘束条件として用いるときに操作空間が必須となる。ここで言う拘束条件として、例えば、ロボットの自己干渉に対する拘束条件、関節可動角の拘束条件、「手が物にフィットする」や「左右の眼が水平になる」といった事象が挙げられる。
【０００９】
剛体リンクが関節を介して連なったリンク構造物において、関節の値をすべて連ねてできるベクトルを一般化変数ｑと呼ぶものとする。この一般化変数ｑの時間微分値との関係を、ヤコビアンＪを用いて下式（１）に示す形に関連付けるとき、物理量ｘに対して操作空間を定義することができる。マニピュレータ先端の手先位置姿勢など、タスク遂行のためのデカルト座標系が操作空間の一例である。（例えば、非特許文献１を参照のこと）。
【００１０】
【数１】
2009099082.tif 000003
【００１１】
一般に、リンク構造物全体の運動方程式は、下式（２）に示すような形で表現できることが知られている。
【００１２】
【数２】
2009099082.tif 000004
【００１３】
上式（２）において、τは一般化変数ｑに対応した一般化力、ｂは重力・コリオリ力、ｆは操作空間に作用する外力、Ｊは外力ｆの作用する操作空間と一般化変数ｑを関連付けるヤコビアンである。上式（２）を変形すると、作用力ｆと作用力方向（操作空間）に発生する加速度（すなわち物理量ｘの２階時間微分）の関係は、下式（３）に示すような操作空間に関する運動方程式で関係付けることができる。
【００１４】
【数３】
2009099082.tif 000005
【００１５】
上式（３）において、Λ-1は操作空間慣性逆行列と呼ばれ、下式（４）のように表される。
【００１６】
【数４】
2009099082.tif 000006
【００１７】
但し、式（４）中のＨは構造全体の関節空間に対する慣性行列である。操作空間すなわち加速度と力の関係は、操作空間慣性逆行列Λ-1によって与えられる。
【００１８】
また、上式（３）の右辺第３項のｃは操作空間バイアス加速度（すなわち、外力が作用しない場合に操作空間に発生する加速度）に相当し、下式（５）のように表される。
【００１９】
【数５】
2009099082.tif 000007
【００２０】
操作空間をすべてまとめた変数として物理量ｘを考えると、反力を求める問題は、線形等式（６）と、不等式（７）及び（８）からなる線形拘束条件として記述することができる。
【００２１】
【数６】
2009099082.tif 000008
【００２２】
【数７】
2009099082.tif 000009
【００２３】
【数８】
2009099082.tif 000010
【００２４】
上式（６）は、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する運動方程式（上式（３）と同一）である。また、上式（７）、（８）は作用力ｆに課される線形不等式拘束条件である。上式（７）、（８）において、ｆNi及びｆFiは、外界との接触に関する拘束による作用点（衝突点）ｉにおける法線方向の外力及び摩擦力を表す。また、μiは、当該作用点における摩擦係数である。上記の不等式拘束条件式（７）は、接触する立体間の衝突点ｉの垂直反力方向に対し、立体間に排斥力が作用し、互いに浸透しないような条件を表現したものである。また、上記の不等式拘束条件式（８）は、摩擦条件を表現したものである。
【００２５】
上式（６）〜（８）からなる連立１次方程式は、線形相補性問題（ＬＣＰ）と呼ばれる。そして、操作空間慣性逆行列Λ-1及びバイアス加速度ｃが既知であれば、この線形相補性問題を解くことによって、これらを満足するｘの加速度及び力ｆを決定することができる。線形相補性問題の数学的な解法自体は、既に提案されている（例えば、非特許文献２を参照のこと）。
【００２６】
操作空間慣性逆行列Λ-1は、反力の総数（言い換えれば、立体間の衝突点の総数）を次数とする正方行列であるが、その算出にかかる計算コストは大きい。また、操作空間慣性逆行列Λ-1が得られた上で線形相補性問題を解いてｘの加速度及び力ｆを求めるためには、操作空間慣性逆行列Λ-1の算出と同程度の計算コストを要する。立体間の衝突点数の増大に応じてこれらの計算コストは増大する。
【００２７】
解析型の手法（ＬＣＰ解決器）を用いて上記の線形相補性問題を解くことによって、接触又は衝突する立体間に作用する反力をより厳密に求めることができ、数値計算的な安定性は高いものの、立体間の衝突点数が増大するにつれてシミュレーションが低速化するという問題がある。
【００２８】
一方、解析型の方法の対極として、計算精度を犠牲にしつつ計算量を抑制する「ペナルティ法」が知られている。ペナルティ法は、接触若しくは衝突する立体間の浸透深度に応じたバネ及びダンパの力を仮想的に発生させて反力を求めるものである。すなわち、立体間の衝突点毎に、図１０に示すように、浸透深度に応じたバネの復元力並びにダンパ力を算出する。ペナルティ法によれば、操作空間慣性逆行列Λ-1のような巨大なマトリクスを演算する必要や、操作空間慣性逆行列Λ-1及びバイアス加速度ｃを得た上で線形相補性問題（式（６）〜（８））を解く必要がないので、高速な衝突力解決が可能である。
【００２９】
しかしながら、ペナルティ法により剛体同士の衝突のシミュレーションを行なおうとすると、図１０において極めて硬いバネ並びにダンパを挿入する必要があり、数値発散を起こし易くなるという問題がある。数値発散を回避するために積分周期を短くするといった対応方法も考えられるが、この場合、上記の解析型の方法よりも低速化する可能性もある。
【００３０】
要言すれば、剛体力学シミュレーションにおいて、接触若しくは衝突する剛体間に作用する接触力若しくは衝突力を、精度よく且つ低計算コストで算出するアルゴリズムが望まれている。解析型の方法では、物体間に作用する反力を精度よく求めることができるが、操作空間慣性逆行列Λ-1の算出や、線形相補性問題を解決する演算がボトルネックとなっている。
【００３１】
【特許文献１】特開２００７−１０２３９４号公報
【非特許文献１】“Ａ  ｕｎｉｆｉｅｄ  ａｐｐｒｏａｃｈ  ｔｏ  ｍｏｔｉｏｎ  ａｎｄ  ｆｏｒｃｅ  ｃｏｎｔｒｏｌ  ｏｆ  ｒｏｂｏｔ  ｍａｎｉｐｕｌａｔｏｒｓ”（Ｔｈｅ  ｏｐｅｒａｔｉｏｎａｌ  ｓｐａｃｅ  ｆｏｒｍｕｌａｔｉｏｎ，ＩＥＥＥ  Ｊｏｕｒｎａｌ  ｏｆ  Ｒｏｂｏｔｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ，ＲＡ−３（１），ｐｐ．４３−５３，１９８７）
【非特許文献２】“Ｆａｓｔ Ｃｏｎｔａｃｔ Ｆｏｒｃｅ Ｃｏｍｐｕｔａｔｉｏｎ ｆｏｒ Ｎｏｎｐｅｎｅｔｒａｔｉｎｇ ＲｉｇｉｄＢｏｄｉｅｓ”（ＳＩＧＧＲＡＰＨ９４，ｐｐ．２３−３４，１９９４）
【発明の開示】
【発明が解決しようとする課題】
【００３２】
本発明の目的は、剛体力学シミュレーションにおいて、接触若しくは衝突する剛体間に作用する接触力若しくは衝突力を精度よく且つ低計算コストで算出することができる、優れた力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムを提供することにある。
【００３３】
本発明のさらなる目的は、物体間の接触若しくは衝突事象を物体に作用する力と発生する加速度の関係を記述する操作空間上で扱い、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する線形等式からなる運動方程式と、各衝突点で働く作用力ｆに課される線形不等式拘束条件からなる線形相補性問題を解くことによって、各衝突点で働く作用力ｆを精度よく且つ高速に求めることができる、優れた力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムを提供することにある。
【００３４】
本発明のさらなる目的は、操作空間慣性逆行列Λ-1を算出してから線形相補性問題を解決することによって、各衝突点で働く作用力ｆを精度よく且つ高速に求めることができる、優れた力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムを提供することにある。
【課題を解決するための手段】
【００３５】
本発明は、上記課題を参酌してなされたものであり、その第１の側面は、複数の物体が共存する環境下で各物体間に働く物理的相互作用の力学シミュレーションを行なう力学シミュレーション装置であって、
物体間の接触若しくは衝突事象を物体に作用する力と発生する加速度の関係を記述する操作空間上で扱い、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する線形等式からなる運動方程式と、各衝突点で働く作用力ｆの各成分に課される線形不等式拘束条件からなる線形相補性問題を解く際に、
操作空間慣性行列のノミナル値Λnを与える手段と、
操作空間慣性行列のノミナル値Λnを用いて操作空間に作用するノミナルな慣性力を求める手段と、
操作空間に作用する、ノミナルな慣性力以外の力を推定する外乱オブザーバと、
ノミナルな慣性力と、推定されたノミナルな慣性力以外の力に基づいて、物理量に作用する力ｆを求める手段と、
を具備することを特徴とする力学シミュレーション装置である。
【００３６】
ゲームやバーチャル・リアリティといった技術分野において、物理法則に基づいた動き情報のリアリティ向上、インタラクティビティの向上を図るには、複数の物体が共存する仮想環境下で、各物体間の衝突や接触といった物理的相互作用を計算機上で再現する力学シミュレーションが行なわれる。この種の力学シミュレーションでは、立体間の衝突が発生したことを検出し、さらに、衝突物体の衝突箇所に適切な反力を作用させる必要がある。
【００３７】
立体間の衝突を検出する方法として、例えば特許文献１（前述）で提案されている接触形状算出方法などを用いることができる。
【００３８】
続いて、検出された衝突点に作用する垂直反力や摩擦力を演算により求めるが、求めた垂直反力の値が小さ過ぎると立体が相互に浸透し、大きすぎると過大な速度で遠のき合い、場合によっては数値発散を起こしてしまい不安定である。また、求めた摩擦力が小さ過ぎると立体が衝突点で滑ってしまい、逆に課題であれば数値発散を招く。言い換えれば、力学シミュレーションでは、衝突点に作用する垂直反力や摩擦力を精度よく求める必要がある。
【００３９】
これら垂直反力や摩擦力を精度よく求める方法として、解析型の方法が挙げられる。具体的には、衝突する立体間に作用する力を求める問題を、物体に作用する力と発生する加速度の関係を記述する操作空間上で扱う。この場合、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する線形等式からなる運動方程式と、各衝突点で働く作用力ｆに課される線形不等式拘束条件からなる線形相補性問題を解くことによって、各衝突点で働く作用力ｆを求めることができる。
【００４０】
ところが、操作空間慣性逆行列Λ-1は、反力の総数を次数とする正方行列であるため算出に要する計算コストは大きい。また、解析型の手法で線形相補性問題を解決すると、厳密に反力が求まり、数値計算的な安定性は高いものの、立体間の衝突点数の増大に伴いシミュレーションが低速化する。
【００４１】
これに対し、本発明に係る力学シミュレーション装置によれば、仮想環境若しくは遠隔環境下で立体同士が衝突する力制御系を操作空間で扱い、基本的には算出して得られた操作空間慣性逆行列Λ-1を用いて線形相補性問題を解決することによって各衝突点で働く作用力ｆを求めるものであるが、解析型手法の演算ボトルネックとなっている操作空間慣性逆行列Λ-1の算出や、線形相補性問題解決の一部又は全部を、ロバスト制御側に基づく外乱オブザーバと簡単なフィードバック制御系で代替することで、安定且つ高速に立体間の接触力若しくは衝突力を算出するようにしている。
【００４２】
本発明に係る力学シミュレーション装置は、操作空間慣性行列のノミナル値Λnと物理量ｘの加速度目標値から、操作空間に作用するノミナルな慣性力を求める一方、ノミナルな慣性力以外の作用力を外乱ｆdとして外乱オブザーバにより推定して、ノミナルな慣性力と、推定された外乱ｆdに基づいて、物理量に作用する力ｆを求めるようにしている。
【００４３】
ここで、ノミナルな慣性力を算出する際、順動力学演算の積分結果により得られる物理量ｘの垂直反力方向並びに摩擦力方向の操作空間速度の値に基づいて、加速度目標値を求めることができる。
【００４４】
また、外乱オブザーバは、フィードバック演算により作用力ｆを算出し、順動力学演算により作用力ｆから物理量ｘの加速度を算出し、該算出された物理量ｘの加速度に操作空間慣性行列のノミナル値Λnを掛けた値を作用力ｆから引き算することで、外乱ｆdを推定することができる。その際、物理量ｘの加速度目標値と順動力学演算により得られる加速度の誤差が十分小さくなるまで、フィードバック演算を繰り返すようにすればよい。
【００４５】
また、フィードバック演算により作用力ｆを算出した後に、作用力ｆの各成分に対して前記線形不等式拘束を課すリミット処理を付加するようにしてもよい。
【００４６】
外乱オブザーバは、加速度偏差ｅをフィードバックするのみであり、操作空間慣性行列のノミナル値Λnが理論値から大きく外れていると、外乱オブザーバは解に収束しない。したがって、計算コストが増大せず、且つ、理論値から大きく外れないような操作空間慣性行列のノミナル値Λnをプラントに与えることが重要である。
【００４７】
ノミナルな操作空間慣性行列Λnの設定方法として、そのクロス・イナーシャ（非対角成分）を零とし、対角成分のみ理論値を算出することが考えられる。この場合、操作空間慣性行列Λの非対角成分が外乱として扱われることになる。例えば、操作空間のすべての行ｉについて、ｉ番目の操作空間にのみ単位力（すなわちｆ＝ｅi）を作用させる条件下で順動力学演算を実行し、操作空間慣性逆行列Λ-1の第ｉ行ｉ列を順次求めて逆数を取ることで、操作空間慣性行列Λの対角成分の理論値を算出することができる。
【００４８】
また、本発明の第２の側面は、複数の物体が共存する環境下で各物体間に働く物理的相互作用の力学シミュレーションを行なうための処理をコンピュータ上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、
物体間の接触若しくは衝突事象を物体に作用する力と発生する加速度の関係を記述する操作空間上で扱い、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する線形等式からなる運動方程式と、各衝突点で働く作用力ｆの各成分に課される線形不等式拘束条件からなる線形相補性問題を解く際に、前記コンピュータを、
操作空間慣性行列のノミナル値Λnを与えるノミナル値付与手段と、
操作空間慣性行列のノミナル値Λnと物理量ｘの加速度目標値から、操作空間に作用するノミナルな慣性力を求めるノミナル慣性力算出手段と、
操作空間に作用する、ノミナルな慣性力以外の力を外乱ｆdとして推定する外乱オブザーバと、
ノミナルな慣性力と、推定された外乱ｆdに基づいて、物理量に作用する力ｆを求める手段と、
として機能させるためのコンピュータ・プログラムである。
【００４９】
本発明の第２の側面に係るコンピュータ・プログラムは、コンピュータ上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第２の側面に係るコンピュータ・プログラムをコンピュータにインストールすることによって、コンピュータ上では協働的作用が発揮され、本発明の第１の側面に係る力学シミュレーション装置と同様の作用効果を得ることができる。
【発明の効果】
【００５０】
本発明によれば、剛体力学シミュレーションにおいて、接触若しくは衝突する剛体間に作用する接触力若しくは衝突力を精度よく且つ低計算コストで算出することができる、優れた力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムを提供することができる。
【００５１】
また、本発明によれば、物体間の接触若しくは衝突事象を物体に作用する力と発生する加速度の関係を記述する操作空間上で扱い、物理量ｘに作用する力ｆと発生する加速度とを操作空間慣性逆行列Λ-1を用いて関係付けた操作空間に関する線形等式からなる運動方程式と、各衝突点で働く作用力ｆに課される線形不等式拘束条件からなる線形相補性問題を解くことによって、各衝突点で働く作用力ｆを精度よく且つ高速に求めることができる、優れた力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムを提供することができる。
【００５２】
また、本発明によれば、操作空間慣性逆行列Λ-1を算出してから線形相補性問題を解決することによって、各衝突点で働く作用力ｆを精度よく且つ高速に求めることができる、優れた力学シミュレーション装置及び力学シミュレーション方法、並びにコンピュータ・プログラムを提供することができる。
【００５３】
本発明に係る力学シミュレーション装置によれば、解析型手法の演算ボトルネックとなっている操作空間慣性逆行列Λ-1の算出や、線形相補性問題解決の一部又は全部を、ロバスト制御側に基づく外乱オブザーバと簡単なフィードバック制御系で代替するように構成されているので、数値計算上の安定性確保と計算コストの低減を同時に達成することができる。これによって、剛体力学シミュレーション全体の高速化が可能となり、より複雑な世界の力学シミュレーションを実時間で実行することができるようになる。
【００５４】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【発明を実施するための最良の形態】
【００５５】
以下、図面を参照しながら本発明の実施形態について詳解する。
【００５６】
本発明は、複数の物体が共存する空間における各物体間の衝突や接触といった物理的相互作用を計算機上で再現する力学シミュレーションに関するが、例えば、仮想現実空間において、操作者に視覚情報や聴覚情報に加えて力覚や触覚を提示するＶＲ（Ｖｉｒｔｕａｌ  Ｒｅａｌｉｔｙ）システムに適用することができる。
【００５７】
図１には、ＶＲシステム１０の構成例を模式的に示している。図示のシステム１０は、スピーカ１４、ディスプレイ１５、マウス（図示しない）、力覚提示器（ハプティクス・デバイス）１６を介して、計算機上に構築された３次元空間の様子をユーザに提示する。
【００５８】
スピーカ１４は、３次元空間（仮想現実空間又は遠隔現実空間など）で発生した事象に関連した音を、ディスプレイ１４は３次元空間の視覚情報を、ハプティクス・デバイス１６は３次元空間内の物体に触れた場合に感じられる力覚情報を、それぞれ提示する。これらの入出力系を協調的に駆使し、計算機上に構築された３次元空間があたかも実在する空間であるかの如く、高いリアリティと臨場感を以ってユーザに提示するようになっている。スピーカ１４はサウンド・カードを介してバス１７に接続され、また、ディスプレイ１５はビデオ・カードを介してバス１７に接続されている。
【００５９】
本発明の要旨は、特定のハプティクス・デバイス１６の構成方法に限定されるものではない。例えば、本出願人に既に譲渡されている特願２００７−４６５８０号明細書には、機械構造上に定められた、力触覚を提示するための作用点と、作用点に発生する力を制御する作用力制御手段と、作用点の位置を制御する作用点制御手段で構成され、非接触時にも将来発生する衝突点を予測し、予測された衝突点を作用点とするように指先位置制御系をあらかじめ駆動しておくことによって、不連続に発生した衝突現象においても時間遅延なく良好な作用点提示を実現する力触覚ディスプレイについて開示されており、ハプティクス・デバイス１６として用いることができる。
【００６０】
本システム１０のデータ・プログラムはハード・ディスク・ドライブ（ＨＤＤ：Ｈａｒｄ  Ｄｉｓｃ  Ｄｒｉｖｅ）１３内に格納され、プログラム実行時に仮想３次元空間に関するデータとその実行プログラムとしてメモリ（ＲＡＭ：Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）１２上に展開される。また、仮想３次元空間の時間的変化や、音情報、映像情報、力情報がＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）１１によってリアルタイムで算出される。
【００６１】
ＣＰＵ１１で算出された音情報や映像情報、力情報は、それぞれスピーカ１４、ディスプレイ１５、ハプティクス・デバイス１６を介してユーザに提示される。以上のデバイス１４〜１５は、リアルタイム性を確保するのに十分な速度を有した高速なバス１７によって接続され、相互に情報の授受が可能であるとする。
【００６２】
図２には、ＣＰＵ１１上で実行される制御プログラムの構成を模式的に示している。
【００６３】
仮想３次元空間の状態は、３次元オブジェクト・データ格納部１０１に保持される。同格納部１０１には、仮想３次元空間に共存する各３次元オブジェクトについての、位置、姿勢、寸法、接続関係といった幾何属性や、質量、慣性テンソル、摩擦係数、反発係数といった力学属性、減衰特性や固有振動波形などの振動属性に関するデータが少なくとも保持されているとする。
【００６４】
３次元オブジェクト・データ格納部１０１内の各オブジェクトは、順動力学演算部１０２における力学演算の結果に応じて位置や姿勢が変更される。順動力学演算部１０２は、オブジェクト内部に発生する力やオブジェクト外部より作用する力から、オブジェクトに発生する加速度を得る演算を実行し、この加速度を積分することによって、オブジェクトの運動をＮｅｗｔｏｎ−Ｅｕｌｅｒ力学に基づいてシミュレートする。そして、順動力学演算の結果を各オブジェクトの位置や姿勢に反映することにより、例えばディスプレイ１５に表示される各オブジェクトは力学法則に基づいたリアルな動きを呈することができる。
【００６５】
本発明の要旨は、特定の順動力学演算方法に限定されない。例えば、本出願人に既に譲渡されている特開２００７−１０８９５５号公報には、順動力学演算を４過程に分解して、イナーシャ情報算出⇒速度情報算出⇒力情報算出⇒加速度情報算出の順で実行するようにして、例えばロボットなどのリンク構造物に任意の力を作用させた条件下で任意の場所並びに方向に発生する加速度を得ることができる順運動学演算の構成方法について開示されており、同時に複数の力を作用させたり、複数点における加速度を算出したりといったことを自在に行なうことができる。このような順動力学演算方法を、本実施形態に係る順動力学演算部１０２に適用することができる。
【００６６】
衝突検出部１０３は、３次元オブジェクト・データ格納部１０１内の各オブジェクトの衝突又は干渉を検出し、衝突点集合を算出する。
【００６７】
本発明の要旨は、特定の衝突検出処理方法に限定されない。例えば、本出願人に既に譲渡されている特開２００７−１０２３９４号公報（特許文献１）には、ＧＪＫ（Ｇｉｌｂｅｒｔ−Ｊｏｈｎｓｏｎ−Ｋｅｅｒｔｈｉ ｄｉｓｔａｎｃｅ ａｌｇｏｒｉｔｈｍ）などの既存の衝突検出アルゴリズムから得られた衝突代表点対（最浸透点対若しくは最近傍点対）からその周辺探索を行なって、精密な接触多角形を高速に算出するようにした接触多角形算出方法について開示されている。衝突検出部１０３は、この接触多角形算出方法を適用することができる。
【００６８】
衝突力算出部１０４は、衝突検出部１０３で算出された衝突点においてオブジェクトに作用する外力を力学演算によって算出する。衝突力算出部１０４では、仮想３次元空間上で物体間の接触若しくは衝突事象を操作空間で扱い、基本的には算出して得られた操作空間慣性逆行列Λ-1を用いて線形相補性問題を解決することによって各衝突点で働く作用力ｆを求めるようにしている。操作空間慣性逆行列Λ-1の算出や、線形相補性問題解決は解析型手法の演算ボトルネックとなる。そこで、本実施形態では、操作空間慣性逆行列Λ-1の算出や、線形相補性問題解決の一部又は全部を、ロバスト制御側に基づく外乱オブザーバと簡単なフィードバック制御系で代替することで、安定且つ高速に立体間の接触力若しくは衝突力を算出するようにしているが、この点の詳細については後述に譲る。
【００６９】
衝突音生成部１０５は、衝突力算出部１０４における力学シミュレーションの結果として得られる衝突力に応じた衝突音を合成する。本発明の要旨は、特定の音情報算出方法に限定されるものではない。例えば、本出願人に既に譲渡されている特開２００７−１６４２９１号公報には、物理的なインタラクションが働いた各オブジェクトが持つ減衰特性に基づいてエンベロープ波形に減衰特性を付与し、エンベロープ波形に各オブジェクトが持つ固有振動波形を合成することによって、臨場感のある精緻な効果音を算出する効果音生成方法について開示されている。この効果音生成方法を衝突音生成部１０５に適用することができる。
【００７０】
ハプティクス・ポインタ制御部１０６は、ハプティクス・デバイス１６の操作点の位置変化に応じて、ハプティクス・ポインタ（図示しない）の位置を制御する。また、ハプティクス・ポインタに作用した力をハプティクス・デバイス１６に送信する。
【００７１】
３Ｄ描画部１０８は、３次元オブジェクト・データの情報を立体的に描画し可視化する。この３Ｄ描画情報は、ディスプレイ１５上で表示出力される。なお、３Ｄ描画処理自体は本発明の要旨に直接関連しないので、本明細書ではこれ以上説明しない。
【００７２】
上述したように、衝突力算出部１０４は、物体間の接触若しくは衝突事象を操作空間で扱い、基本的には算出して得られた操作空間慣性逆行列Λ-1を用いて線形相補性問題を解決することによって各衝突点で働く作用力ｆを求める。本実施形態は、衝突力算出部１０４において、オブジェクト相互に作用する反力を精度よく且つ高速に算出する点に主な特徴がある。以下では、衝突力算出部１０４の構成方法について詳解する。
【００７３】
ハプティクス・デバイス１６が３次元空間内の物体に触れた場合に感じられる力覚情報を提示するには、衝突力算出部１０４は、上式（６）〜（８）からなる線形相補性問題の解、すなわち作用力ｆを厳密に求める必要がある。既に述べたように、操作空間慣性逆行列Λ-1は反力の総数を次数とする正方行列であるため、その計算コストは大きい。
【００７４】
演算量を低減するために、操作空間慣性逆行列Λ-1及び操作空間バイアス加速度ｃの算出を省略する方法が考えられるが、解ｆの厳密性が犠牲となる。これに対し、本実施形態では、衝突力算出部１０４は、操作空間慣性逆行列Λ-1の計算を省略するのではなく、操作空間慣性行列のノミナル値（公称値）が与えられているものとし、現実値との差異をモデル化できない外乱として外乱オブザーバにより推定してフィードバック制御を行なうことで、解の厳密性や数値計算上の安定性と計算コストの低減を同時に達成するようにしている。
【００７５】
ここで、あらかじめ与えられている操作空間慣性行列のノミナル値をΛnとすると、上式（３）に示した操作空間に関する運動方程式は、以下のようにも表現することができる。
【００７６】
【数９】
2009099082.tif 000011
【００７７】
ここで、ΔΛは操作空間慣性行列の見積もり誤差であり、ｆcは未知の重力、コリオリ力、遠心力を含む非線形項とする。上式（９）を書き換えることによって、ノミナルな慣性力を下式（１０）のように表すことができる。
【００７８】
【数１０】
2009099082.tif 000012
【００７９】
但し、上式（１０）において、ｆdは以下の通りとする。
【００８０】
【数１１】
2009099082.tif 000013
【００８１】
上式（１１）で表されるｆdは、操作空間に作用する、ノミナルな慣性力以外の力であり、ノミナル値から見て外乱である。この値を外乱オブザーバによって推測することができれば、基本的に上式（９）から以下のように作用力ｆを簡単に求めることができる。
【００８２】
【数１２】
2009099082.tif 000014
【００８３】
物理量の目標値ｘrefの２階時間微分は、操作空間に要求される加速度目標値である。物体間の接触若しくは衝突事象を操作空間で扱う場合、この加速度目標値は、接触点ｉ毎の垂直反力方向並びに摩擦力方向の成分を持つ。
【００８４】
【数１３】
2009099082.tif 000015
【００８５】
ｉ番目の接触点の垂直反力方向及び摩擦力方向の加速度目標値はそれぞれ以下のようになる。但し、νiはｉ番目の接触点における反発係数である。このようにして垂直反力方向には、接触又は衝突する物体同士が浸透しない加速度を目標値として与える。また、摩擦力方向には、物体間で滑りが生じないような加速度を目標値として与える。目標を与えるという意味で、これら物理量の加速度目標値を既知として扱うことができる。
【００８６】
【数１４】
2009099082.tif 000016
【００８７】
【数１５】
2009099082.tif 000017
【００８８】
上式（１４）、（１５）の右辺には、垂直反力方向及び摩擦力方向の操作空間速度をそれぞれ含んでいるが、これらの値は順動力学演算の時間積分の結果より直ちに得ることができる。
【００８９】
問題は、上式（１１）に示した外乱を如何にして見積もるかである。上式（１０）によれば、操作空間に作用する外乱ｆdは下式（１６）のように表すことができる。
【００９０】
【数１６】
2009099082.tif 000018
【００９１】
したがって、物理量ｘに作用する力ｆと操作空間に発生した物理量ｘの加速度から、外乱ｆdを見積もることができる。実際には、後述するように、ローパス・フィルタを介して外乱ｆdの低周波成分のみを推定するようにする。
【００９２】
上式（９）〜（１６）のような観点から、上式（６）〜（８）の線形相補性問題を直接解くことなく、解ｆを上式（１２）から得ることができる。すなわち、線形相補性問題解決を省略できる分、さらに演算量を低減できる効果を期待することができる。
【００９３】
図３には、以上の処理を行なう外乱オブザーバをブロック図で示している。
【００９４】
物理量の加速度目標値に操作空間慣性行列のノミナル値Λnを掛けることで、制御対象すなわちプラントへ印加する力の目標値ｆrefが得られる。プラントには、実際には、力の目標値ｆrefに外乱ｆdを足した力ｆが作用する。
【００９５】
プラント・モデルは、操作空間のダイナミクス、すなわち、上式（３）の右辺から左辺を得る演算であり、力ｆが作用すると物理量ｘの加速度が出力される。
【００９６】
プラントに作用させた力ｆから、プラントから出力される加速度に操作空間慣性行列のノミナル値Λnを掛けた値を引き算することで、上式（１６）に示したように外乱ｆdが求まる。なお、途中に挿入された、ｇ／（ｓ＋ｇ）で表されるローパス・フィルタ（ＬＰＦ）は、系の発散を防ぐためのものである。
【００９７】
そして、外乱ｆdはフィードバックされ、目標値ｆrefに外乱ｆdを足した力ｆがプラントに作用する。
【００９８】
外乱オブザーバについては、例えば、「外乱オブザーバによるロバスト・モーションコントロール」（日本ロボット学会Ｖｏｌ．１１、Ｎｏ．４、ｐｐ４８６−４９３、１９９３）を参照されたい。外乱オブザーバは、プラント内の外乱成分を推定し、制御入力にフィードバックすることで、プラントに未知のパラメータ変動や外乱があっても、目標状態に到達させる効力がある。但し、正しく外乱を推定するには、複数のサイクルに亘ってフィードバック演算を繰り返す必要がある。
【００９９】
図３中のプラントのモデルは、上式（３）の右辺から左辺を得る演算、すなわち、反力作用時に操作空間（衝突点）に発生する加速度を算出する演算であるが、外力や一般化力、重力、速度積に関連する力（コリオリ力など）が作用したときの操作空間における加速度を求める問題である。この問題は、操作空間における加速度を求める点が通常の順動力学計算と異なるが、一種の順動力学演算と捉えることができる。順動力学演算ＦＷＤは、リンク構造物の力学モデルとしての関節モデル並びに各関節の速度、一般化力τ、重力ｇ、並びに作用力ｆをパラメータに持ち、下式（１７）のように表現することができる。
【０１００】
【数１７】
2009099082.tif 000019
【０１０１】
最も基本的な順動力学演算については、例えば、“Ｅｆｆｉｃｉｅｎｔ Ｄｙｎａｍｉｃ Ｃｏｍｐｕｔｅｒ Ｓｉｍｕｌａｔｉｏｎｏｆ Ｒｏｂｏｔｉｃ Ｍｅｃｈａｎｉｓｍｓ”（Ｊｏｕｒｎａｌ ｏｆ Ｄｙｎａｍｉｃ Ｓｙｓｔｅｍｓ，Ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ Ｃｏｎｔｒｏｌ，ｐｐ．２０５−２１１，１９８２）を参照されたい。
【０１０２】
外乱オブザーバにおけるフィードバック演算では、反力ｆを算出し、上式（１７）により順動力学演算を実行して物理量ｘの加速度を算出した後、上式（１６）を実行して外乱ｆdを求め、式（１２）によって反力値を更新する処理を繰り返す。繰り返し演算の第ｋ回目のサイクルにおける外乱推定値をｆdkは、上式（１２）及び上式（１６）より、下式（１８）のように表される。
【０１０３】
【数１８】
2009099082.tif 000020
【０１０４】
ここで、物理量ｘの目標値についての目標加速度と順動力学演算の結果として得られる物理量ｘの加速度との誤差（加速度偏差）ｅを下式（１９）のように定義する。
【０１０５】
【数１９】
2009099082.tif 000021
【０１０６】
すると、この誤差ｅを用いて、外乱オブザーバによるフィードバック演算における外乱の更新式は、下式（２０）のように再帰的に表現することができる。ここで、形の安定性を確保するために、外乱ｆdの低周波成分のみ推定するように、ローパス・フィルタを挿入する。
【０１０７】
【数２０】
2009099082.tif 000022
【０１０８】
また、フィードバック演算の繰り返しの終了条件は、下式（２１）に示すように加速度偏差ｅが十分小さくなるまでとすればよい（但し、εは正の実数である）。
【０１０９】
【数２１】
2009099082.tif 000023
【０１１０】
特開２００７−１０８９５５号公報（前述）には、“Ｔｈｅ  ｃａｌｃｕｌａｔｉｏｎ  ｏｆ  ｒｏｂｏｔ  ｄｙｎａｍｉｃｓ  ｕｓｉｎｇ  ａｒｔｉｃｕｌａｔｅｄ−ｂｏｄｙ  ｉｎｅｒｔｉａｓ”（Ｉｎｔ．Ｊ．Ｒｏｂｏｔｉｃｓ  Ｒｅｓｅａｒｃｈ，ｖｏｌ．２，ｎｏ．１，ｐｐ．１３−３０，１９８３）に記載されている手法を基本とし、一般化変数ｑの次元Ｎ及びｆの次元Ｍに対して１乗オーダ（Ｏ（Ｎ），Ｏ（Ｍ））で順動力学演算を実行する方法について提案している。このような高速な順動力学演算を用いれば、効率よく順動力学演算を実行できる。
【０１１１】
ここまでの説明では、上式（６）〜（８）からなる線形相補性問題のうち、式（６）の運動方程式についてのみ言及し、不等式拘束条件である式（７）及び式（８）については考慮して来なかった。不等式拘束を課す方法として、上記繰り返し演算において式（１２）の反力ｆを算出した後に、下式（２２）及び（２３）に示すリミット処理を付加する方法が考えられる。
【０１１２】
【数２２】
2009099082.tif 000024
【０１１３】
【数２３】
2009099082.tif 000025
【０１１４】
上式において、ｍｉｎ（ｘ，ｙ）、ｍａｘ（ｘ，ｙ）はそれぞれｘ又はｙのうち小さい方、大きい方の値を表す。上式（２２）は、接触点ｉに作用する垂直反力ｆNiは負にならないという拘束条件である。また、上式（２３）は、接触点ｉに作用する摩擦力ｆFiは摩擦係数μi×垂直反力ｆNiよりも小さいとする拘束条件である。
【０１１５】
下式（２４）〜（２６）のような条件を満足している反力ｆNi、ｆFiについては、加速度偏差は存在していても不等式拘束である式（７）並びに式（８）を満足するので、誤差を零とみなす。すなわち、ｉ番目の接触点において、下式（２４）〜（２６）のいずれかの条件を満足したら、加速度誤差ｅi＝０とする。下式（２４）〜（２６）は、誤差ｅiの許容範囲に相当する。
【０１１６】
【数２４】
2009099082.tif 000026
【０１１７】
【数２５】
2009099082.tif 000027
【０１１８】
【数２６】
2009099082.tif 000028
【０１１９】
このような不等式拘束の反映方法で解に収束する数学的証明は得られていないが、本発明者は、実験的に解が得られることを確認した。
【０１２０】
外乱オブザーバは、加速度偏差ｅをフィードバックするのみであり、操作空間慣性行列のノミナル値Λnが理論値から大きく外れていると、外乱オブザーバは解に収束しない。したがって、計算コストが増大せず、且つ、理論値から大きく外れないような操作空間慣性行列のノミナル値Λnをプラントに与えることが重要である。ここでは、操作空間慣性行列のクロス・イナーシャ（非対角成分）を零とし、対角成分のみ理論値を算出することを考える。言い換えれば、操作空間慣性行列Λの非対角成分を外乱として扱う。（操作空間慣性行列の対角成分は作用力の印加方向の物理量の変位に相当し、非対角成分は作用力によるそれ以外の方向への物理量の変位に相当する。）
【０１２１】
本出願人に既に譲渡されている特開２００７−１０８９５５号公報（前述）には、ｉ番目の操作空間にのみ単位力（すなわちｆ＝ｅi）を作用させる条件下で順動力学演算を実行すると操作空間慣性逆行列Λ-1の第ｉ列を求めることができ、したがって下式（２７）の演算をすべての行ｉについて実行すれば操作空間慣性逆行列Λ-1全体が得られることが記載されている。
【０１２２】
【数２７】
2009099082.tif 000029
【０１２３】
上式（２７）は、操作空間慣性逆行列Λ-1の第ｉ列がｉ番目の操作空間に単位力を作用させたときに各操作空間に発生する加速度と一致することを利用したものである。同式より、操作空間慣性行列Λの対角成分は、下式（２８）の通りとなる。
【０１２４】
【数２８】
2009099082.tif 000030
【０１２５】
なお、非対角成分は０なので、次式の通りとする。
【０１２６】
【数２９】
2009099082.tif 000031
【０１２７】
本出願人に既に譲渡されている特開２００７−１０８９５５号公報（前述）には、操作空間慣性逆行列算出に最適化された順動力学演算の構成方法についても記載されている。この演算方法によれば、一般化変数ｑの次元Ｎ及び作用力ｆの次元Ｍに対して１乗オーダ（Ｏ（Ｎ），Ｏ（Ｍ））ですべての対角成分を算出することができる。すなわち、操作空間慣性行列Λの対角成分を算出しても大きな演算量増加には繋がらない。
【０１２８】
図４には、衝突力算出部１０４において実行される処理の手順をフローチャートの形式で示している。
【０１２９】
まず外乱推定値ｆdを０にリセットし（ステップＳ１）、次いで、物理量ｘの各反力方向の加速度目標値を設定する（ステップＳ２）。上述した例では、式（１４）及び式（１５）に基づいて、垂直反力方向及び摩擦力方向の加速度目標値をそれぞれ算出する。
【０１３０】
次いで、操作空間慣性行列のノミナル値Λnを算出する（ステップＳ３）。上述した例では、式（２７）及び式（２８）に基づいて、順動力学演算を用いてその対角成分のみを厳密に求める。
【０１３１】
次いで、式（１２）に基づいて、各操作空間に作用する反力ｆを算出する（ステップＳ４）。但し、不等式拘束を考慮するため、式（２２）及び式（２３）のリミット処理を施す。
【０１３２】
次いで、プラントに反力ｆを作用させたときに発生する物理量ｘの加速度を算出する（ステップＳ５）。具体的には、ステップＳ４で得られた反力ｆを用いて、式（１７）に基づく順動力学演算を実行し、各操作空間に発生する物理量ｘの加速度を算出する。
【０１３３】
次いで、式（１９）に基づいて、操作空間に発生した加速度誤差ｅを算出する（ステップＳ６）。但し、不等式拘束を考慮するため、式（２４）から式（２６）の条件に該当する成分はｅi＝０とする。
【０１３４】
ステップＳ７では、式（２１）に基づいて、外乱オブザーバにおける繰り返しフィードバック演算の終了判定を実行する。ここで、終了条件が満足されたときは当該反力算出処理フロー全体を終了する。一方、終了条件が満足されないときは、後続のステップＳ８へ進む。
【０１３５】
ステップＳ８では、式（２０）に基づいて外乱推定値ｆdを更新する。その後、ステップＳ４に戻る。
【０１３６】
以上の処理を繰り返すことで、各衝突点に作用するべき反力を精度よく且つ高速に決定することができる。
【０１３７】
本実施形態に係る反力算出アルゴリズムによれば、人間型などパートナー型ロボットの力制御系における未知力決定問題を好適に解決することができる（この場合、ロボットが把持する物体や、物体を把持するエンド・エフェクタなどロボットの機体を構成する各リンクが３次元オブジェクト・データ格納部１０１に格納される３次元オブジェクトに相当する）。
【０１３８】
図５並びに図６には、本実施形態に係る反力算出アルゴリズムを用いて、ロボットが複数共存する路世界をシミュレートした例を示している。
【０１３９】
図５には、上方から人間型ロボットが落下し、床面と衝突した後、後方の犬型ロボットに衝突する例を示している。複雑な構造体間の衝突でも発散することなくシミュレートされる様子が分かる。
【０１４０】
また、図６には、続けて、ハプティクス・デバイスを用いて犬型ロボットをドラッグ＆ドロップした例を示している。ハプティクス・ポインタと犬型ロボット間に相対速度＝０を達成するための力を作用することでドラッグを実現することができる。
【０１４１】
上記で説明した例では、衝突反力決定問題に対して外乱オブザーバを適用する例を示したが、不等式拘束のない標準形の反力更新式（１２）及び誤差算出式（１９）を行なえば、このように物体に任意の動きを実現するための外力決定問題にも本発明に係るアルゴリズムを適用できることは自明であろう。
【０１４２】
図７には、本発明に係るアルゴリズムを用いて立方体を積み上げた例を示している。図８には、立方体の数を増大させながら、このような積み上げシミュレーションを、通常の解析型で実行した場合と本発明に係るアルゴリズムで実行した場合で、シミュレーション１ステップ当たりの実行時間計測した結果を示している。本発明に係るアルゴリズムでは標準的な解析型に比較して演算量が低減されている様子が分かる。
【産業上の利用可能性】
【０１４３】
以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【０１４４】
本発明は、操作者に力覚を提示するハプティクス・デバイスや仮想現実感を与えるＶＲシステム、パートナー型ロボットの力制御系における未知力決定問題、ＣＧキャラクタの運動制御、あるいは、物体に所望の運動拘束を与えるためのみ知力解決を行なうその他のさまざまなシステムに適用することができる。例えば、ロボットの運動制御やＣＧキャラクタの運動制御に本発明を適用した場合、安定で効率的なインバース・キネマティクス操作を行なうことができる。
【０１４５】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【図面の簡単な説明】
【０１４６】
【図１】図１は、ＶＲシステムの構成例を模式的に示した図である。
【図２】図２は、ＣＰＵ１１上で実行される制御プログラムの構成を模式的に示した図である。
【図３】図３は、外乱オブザーバのブロック図である。
【図４】図４は、衝突力算出部１０４において実行される処理の手順を示したフローチャートである。
【図５】図５は、本発明に係る反力算出アルゴリズムを用いて、ロボットが複数共存す路世界をシミュレートした例を示した図である。
【図６】図６は、本発明に係る反力算出アルゴリズムを用いて、ロボットが複数共存す路世界をシミュレートした例を示した図である。
【図７】図７は、本発明に係るアルゴリズムを用いて立方体を積み上げた例を示した図である。
【図８】図８は、立方体の数を増大させながら、通常の解析型で実行した場合と本発明に係るアルゴリズムで実行した場合で、シミュレーション１ステップ当たりの実行時間計測した結果を示した図である。
【図９】図９は、立体間の衝突を検出する方法を説明するための図である。
【図１０】図１０は、接触若しくは衝突する立体間の浸透深度に応じたバネ及びダンパの力を仮想的に発生させる様子を示した図である。
【符号の説明】
【０１４７】
１０…ＶＲシステム
１１…ＣＰＵ
１２…ＲＡＭ
１３…ＨＤＤ
１４…スピーカ
１５…ディスプレイ
１６…ハプティクス・デバイス
１７…バス
１０１…３次元オブジェクト・データ格納部
１０２…順動力学演算部
１０３…衝突検出部
１０４…衝突力算出部
１０５…衝突音生成部
１０６…ハプティクス・ポインタ制御部
１０７…ハプティクス・デバイス制御部
１０８…３Ｄ描画部
【図２】
2009099082.tif 000032
【図３】
2009099082.tif 000033
【図４】
2009099082.tif 000034
【図９】
2009099082.tif 000035
【図１０】
2009099082.tif 000036
【図１】
2009099082.tif 000037
【図５】
2009099082.tif 000038
【図６】
2009099082.tif 000039
【図７】
2009099082.tif 000040
【図８】
2009099082.tif 000041