JP2018030210A - シミュレーション装置、制御システム、ロボットシステム、シミュレーション方法、プログラム及び記録媒体 - Google Patents

Abstract

【課題】ロボットの接触力と拘束力とを共に計算し、計算負荷を低減する。【解決手段】ロボットシステム１００におけるシミュレーションシステム５５０のコンピュータにより、拘束を有するロボット２００の動作のシミュレーションを行うのに用いる接触力及び拘束力を計算する。この計算方法は、まず、接触・拘束空間と関節空間との関係を示すヤコビアンを求める。このヤコビアンを用いてロボット２００の運動方程式を接触・拘束空間に変換する。更に変換した運動方程式を線形化して、これにより得られる連立方程式を解くことで、接触力及び拘束力を同時に求める。【選択図】図１

Description

本発明は、拘束を有するロボットのシミュレーションの技術に関する。

近年、ロボット開発において、ロボットの動作を詳細にシミュレーションするために、様々な技術開発が行われている。拘束力の導入もその一つであり、パラレルリンクを用いたロボットのシミュレーションにおいて高速な演算が可能となっている。

ここで、拘束力とは、ある物体に対して他の物体の動作を拘束する力をいう。拘束力の一例として、ギヤとギヤとが速度伝達比Ｚ_Ａ：Ｚ_Ｂで伝達する力が挙げられる。また、拘束力の他の例として、物体上の任意点の自由度を、別の物体の座標系に拘束する力が挙げられる。この他の例では、例えば物体上の任意点の並進自由度を、別の物体に拘束する拘束力を計算することで、物体と別の物体を球面軸で繋いだような動作をシミュレーションすることができ、閉リンクロボットのシミュレーションが可能となる。

一方、ロボットは、ワークの搬送や組み立てなどの生産ラインにも適用されている。また、ギヤやパラレルリンク機構などのロボットを、接触力のみを用いてシミュレーションする方法もある。このようなロボットの動作をシミュレーションするには、接触力の計算が必要となる。接触力の計算方式に関しては、従来はモデル同士の侵入量から接触力を疑似的に計算する方式が主流であったが、現在は解析的でより現実に即した計算方式が提案されている。

ロボットの接触力の計算方法の従来例として非特許文献１が知られている。非特許文献１では、ロボットに作用する接触力を、コリオリ力、遠心力項などの影響を考慮して算出する方法について記載されている。まず、接触点におけるロボットの運動方程式を導出し、次に、接触点が満たすべき条件を考慮して接触力を算出している。ここで、満たすべき条件とは、接触点における接触力の向き、接触点における速度、法線方向の接触力と接線方向の接触力との関係などである。また、接触力を算出する方法として、ガウスサイデル法に似た繰り返し計算を用いて、満たすべき条件に合致した接触力を計算している。

Shin’ichiro Nakaoka,"Constraint-based Dynamics Simulator for Humanoid Robots with Shock Absorbing Mechanisms", Proceedings of the 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems,pp.3641-3647,2007

しかしながら、上記非特許文献１に記載の方法では、ロボットの接触力を求める際に、関節における拘束力については考慮されていなかった。そのため、拘束（具体的には速度拘束）を有するロボットがワークと接触するような場合のシミュレーションを行うことができなかった。

また、パラレルリンク機構などの拘束を有するロボットを、接触力を用いてシミュレーションする方法では、ロボットの拘束機構についても接触点の接触力について連立方程式を立てる必要がある。拘束機構については多数の接触点が存在するため、接触点の数が増えるほど連立方程式の数が増えることになり、拘束を有するロボットについて接触点のみでシミュレーションを行う場合、計算負荷が高く、多くの計算時間が必要であった。

そこで、本発明は、ロボットの接触力と拘束力とを共に計算し、計算負荷を低減することを目的とする。

関節を有するロボットの動作をシミュレーションするのに用いる、前記ロボットに物体が接触する接触力及び前記ロボットの関節における拘束力を求める演算部を備えたシミュレーション装置であって、
前記演算部は、
前記ロボットの関節の数を自由度とする関節空間において前記ロボットの関節の速度をｑ（・）としたとき、前記ロボットの関節の拘束条件に基づく自由度を持ち、前記ロボットの拘束点の拘束方向を基準とする第１の座標空間における拘束力ｆ_Ａと、前記関節空間における拘束力τ_Ａとの関係を示す第１のヤコビアンＪ_Ａを、

を満たすように算出する第１のヤコビアン算出処理と、
前記ロボットと前記物体との接触点のワールド座標系における座標値に基づき、前記ロボットの接触点の法線方向及び接線方向を基準とする直交座標で表された第２の座標空間における接触力ｆ_Ｂと、前記関節空間における接触力τ_Ｂとの関係を示す第２のヤコビアンＪ_Ｂを算出する第２のヤコビアン算出処理と、
前記第１のヤコビアンＪ_Ａ及び前記第２のヤコビアンＪ_Ｂを合わせた第３のヤコビアンＪを求める第３のヤコビアン算出処理と、
前記関節空間において、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτ_ａｃｔとし、前記関節空間における前記ロボットの第１の運動方程式

を前記第３のヤコビアンＪを用いて変換することで、前記第１の座標空間及び前記第２の座標空間における、加速度ｘ（・・）、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、発生力ｆ_ａｃｔとなる前記ロボットの第２の運動方程式

を求める変換処理と、
所定の時間をΔｔとし、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式

を求める線形化処理と、
前記線形化した前記第３の運動方程式を前記拘束力ｆ_Ａ及び前記接触力ｆ_Ｂを未知数とする連立方程式とし、前記連立方程式を解くことにより前記拘束力ｆ_Ａ及び前記接触力ｆ_Ｂを算出する力算出処理と、を実行することを特徴とする。

本発明によれば、ロボットが物体と接触する場面をシミュレーションする場合、拘束機構である関節については拘束力でモデル化することで、拘束機構である関節を接触力でモデル化する場合と比べて運動方程式で求めるべき力の数が少なくなる。したがって、演算負荷が低減し、高速なシミュレーションが可能となる。

実施形態に係るロボットシステムを示す説明図である。 ロボットアームの複数の関節のうちの１つの関節を示す部分断面図である。 実施形態に係る制御システムを示すブロック図である。 実施形態に係るシミュレーション方法を示すフローチャートである。 接触力及び拘束力の計算方法を示すフローチャートである。 実施例１におけるロボットを示す模式図である。 （ａ）は、実施例３におけるロボットを示す模式図である。（ｂ）は（ａ）に示すロボットの１つの回転ジョイントを、互いに拘束関係にある２つの拘束点とした模式図である。（ｃ）は、実施例３においてロボットに物体が接触した状態を示す模式図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。図１は、実施形態に係るロボットシステムを示す説明図である。図１に示すように、ロボットシステム１００は、ロボット２００と、制御システム６００と、を備えている。

ロボット２００は、垂直多関節のロボットアーム２０１と、ロボットアーム２０１の先端に取り付けられたエンドエフェクタとしてのロボットハンド２０２と、を備えている。

ロボットアーム２０１の基端は、台座１５０に固定された固定端である。ロボットアーム２０１の先端は、自由端である。ロボットアーム２０１は、可動範囲の中であれば任意の３次元位置で任意の３方向の姿勢に、手先（ロボットアーム２０１の先端）を向けることができる。

ロボットアーム２０１は、複数の関節、例えば６つ関節Ｊ_１〜Ｊ_６を有している。ロボットアーム２０１は、各関節Ｊ_１〜Ｊ_６をそれぞれ回転駆動する複数（６つ）の駆動機構を有している。

ロボットアーム２０１は、複数のリンク２１０_０〜２１０_６を有し、複数のリンク２１０_０〜２１０_６が関節Ｊ_１〜Ｊ_６で回転可能に連結されている。ここで、基端側から先端側に向かって、リンク２１０_０〜２１０_６が順に直列に連結されている。

図２は、ロボットアームの複数の関節のうちの１つの関節を示す部分断面図である。以下、関節Ｊ_２を例に代表して説明し、他の関節Ｊ_１，Ｊ_３〜Ｊ_６については、同様の構成であるため、説明を省略する。

関節Ｊ_２を駆動する駆動機構２３０は、駆動源である電動モータ（以下、「モータ」という）２３１と、モータ２３１の出力を減速する減速機２３３と、を有している。

モータ２３１は、サーボモータであり、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。モータ２３１の回転軸２３２の回転が減速機２３３で減速されて関節（リンク）を駆動する。例えば減速機２３３の減速比が５０：１であれば、モータ２３１が５０回転で減速機２３３の出力側に配置されているリンク２１０_２が１回転する。また、リンク２１０_１とリンク２１０_２とは、クロスローラベアリング２３７を介して回転自在に結合されている。

減速機２３３は、本実施形態では波動歯車減速機である。減速機２３３は、モータ２３１の回転軸２３２に結合された、入力軸であるウェブジェネレータ２４１と、リンク２１０_２に固定された、出力軸であるサーキュラスプライン（ギヤ）２４２と、を備えている。なお、サーキュラスプライン２４２は、リンク２１０_２に直結されているが、リンク２１０_２に一体に形成されていてもよい。

また、減速機２３３は、ウェブジェネレータ２４１とサーキュラスプライン２４２との間に配置され、リンク２１０_１に固定されたフレクスプライン（ギヤ）２４３を備えている。フレクスプライン２４３は、ウェブジェネレータ２４１の回転に対して減速比Ｎで減速され、サーキュラスプライン２４２に対して相対的に回転する。従って、モータ２３１の回転は、減速機２３３で１／Ｎの減速比で減速されて、フレクスプライン２４３が固定されたリンク２１０_１に対してサーキュラスプライン２４２が固定されたリンク２１０_２を相対的に回転運動させ、関節Ｊ_２を回転させる。

図３は、実施形態に係る制御システムを示すブロック図である。制御システム６００は、ロボット２００（図１）の動作を制御するロボット制御装置３００と、シミュレーションシステム５５０と、を有している。シミュレーションシステム５５０は、コンピュータで構成されたシミュレーション装置５００と、シミュレーション装置５００に接続された、キーボード５２１、マウス５２２等の操作入力部５２０、モニタ５２３及び外部記憶装置５２４と、を備えている。

シミュレーション装置５００は、オフライン教示によってユーザの操作に従ってロボット２００の動作プログラムを作成し、作成した動作プログラムに基づき、ロボット２００に対応する仮想ロボットを三次元仮想空間内で動作させるシミュレーションを行う。

シミュレーション装置５００は、演算部としてのＣＰＵ（Central Processing Unit）５０１を備えている。また、シミュレーション装置５００は、記憶部として、ＲＯＭ（Read Only Memory）５０２、ＲＡＭ（Random Access Memory）５０３、ＨＤＤ（Hard Disk Drive）５０４を備えている。また、シミュレーション装置５００は、記録ディスクドライブ５０５及び複数のインタフェース５１１〜５１５を備えている。

ＣＰＵ５０１には、ＲＯＭ５０２、ＲＡＭ５０３、ＨＤＤ５０４、記録ディスクドライブ５０５及びインタフェース５１１〜５１５が、バス５１０を介して相互に通信可能に接続されている。

ＲＯＭ５０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ５０３は、ＣＰＵ５０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。ＨＤＤ５０４は、ＣＰＵ５０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ５０１に、後述する各種演算処理を実行させるためのプログラム５３１を記録するものである。ＣＰＵ５０１は、ＨＤＤ５０４に記録（格納）されたプログラム５３１に基づいてシミュレーション方法の各工程を実行する。

また、ＨＤＤ５０４には、ロボット言語（命令文）で記述された動作プログラムが格納される。更に、ＨＤＤ５０４には、ロボット２００を構成する部品の三次元データ（ＣＡＤデータ）、三次元有限要素法の計算式、パラメータ値等の各種情報が予め格納されている。

記録ディスクドライブ５０５は、記録媒体の一例である記録ディスク５３２に記録された各種データを読み出すことができる。

インタフェース５１１にはキーボード５２１が接続され、インタフェース５１２にはマウス５２２が接続されている。これらキーボード５２１及びマウス５２２からなる操作入力部５２０により、各種の操作入力を受付可能となっている。

インタフェース５１３には、表示部であるモニタ５２３が接続されており、モニタ５２３には、データ入力（編集）画像、仮想ロボットが配置された仮想三次元空間の表示画像などの各種画像が表示可能である。

インタフェース５１４には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置５２４が接続可能に構成されている。なお、本実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ５０４であり、ＨＤＤ５０４にプログラム５３１が格納される場合について説明するが、これに限定するものではない。プログラム５３１は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム５３１を供給するための記録媒体としては、図３に示すＲＯＭ５０２や、記録ディスク５３２、外部記憶装置等５２４を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ＨＤＤ、ＳＳＤ、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、磁気テープ、ＵＳＢメモリ等の不揮発性メモリ、ＲＯＭ等を用いることができる。また、記憶部としてＨＤＤ５０４の場合について説明するが、ＳＳＤや書き換え可能な不揮発性メモリであってもよい。

インタフェース５１５には、ロボット制御装置３００が接続されている。ロボット制御装置３００は、動作プログラムに基づき作成された軌道データをシミュレーション装置５００から取得し、軌道データに従ってロボット２００の動作を制御する。

ここで、ロボットアーム２０１の基端には、互いに直交する３軸ｘ_０，ｙ_０，ｚ_０からなるワールド座標系Σ_０が設定されている。また、ロボット２００の手先、つまりロボットハンド２０２には、ツールセンターポイント（ＴＣＰ）が設定される。ＴＣＰは、ロボットアーム２０１の基端に設定されたワールド座標系Σ_０を基準に、位置を表す３つのパラメータと、姿勢を表す３つのパラメータで表される。このＴＣＰの目標値である教示点が、オフライン教示により、記憶部であるＨＤＤ５０４に格納される動作プログラムに記述される。

ＣＰＵ５０１は、動作プログラムに記述された補間方法に従い教示点間を補間し、逆運動学計算により各関節の値に変換したロボット２００（ロボットアーム２０１）の軌道データを生成する。教示点間を補間する補間方法としては、直線補間、円弧補間、関節補間、Ｓｐｌｉｎｅ補間、Ｂ−Ｓｐｌｉｎｅ補間、ベジェ曲線など、種々の方法がある。

ロボットアーム２０１の動作の制御として、位置制御とインピーダンス制御がある。位置制御では、ロボット２００の手先に設定したＴＣＰがワールド座標系Σ_０の３軸ｘ_０，ｙ_０，ｚ_０で目標位置（軌道）を辿るようにロボットアーム２０１の動作を制御する。一方、インピーダンス制御では、ロボット２００の手先に係る力（接触力）が所定値（より具体的には０）に近づくようにロボットアーム２０１の動作を制御する。よって、インピーダンス制御の設計段階におけるシミュレーションにおいて、接触力を計算する必要がある。接触力とは、物体と物体が接触した際に生じる反発力と摩擦力をいう。

ところで、本実施形態のロボット２００は、拘束を有するロボットである。拘束を有するロボットの動作をシミュレーションする際には、接触力と拘束力の計算が必要である。ここで、拘束を有するロボットとは、ある動作軸とある動作軸との間に速度拘束（回転速度と回転速度との関係性）を有するものを指す。例えば、パラレルリンク機構を使ったものは拘束を有するロボットであるが、関節機構にモータ軸とリンク軸がギヤで速度拘束されているものについても、拘束を有するロボットである。拘束力とは、ある物体に対して他の物体の動作を拘束する力をいう。

本実施形態では、拘束を有するロボット２００がワーク（物体）と接触するときを想定し、ＣＰＵ５０１の演算処理により、ロボット２００にワークが接触する接触力及びロボット２００の関節における拘束力を求める。

以下、オフライン教示におけるロボット２００のダイナミクスシミュレーションについて説明する。図４は、実施形態に係るシミュレーション方法を示すフローチャートである。

ＣＰＵ５０１は、ユーザの操作によりロボット２００の動作プログラムを作成する（Ｓ１）。ＣＰＵ５０１は、ロボット２００のリンク２１０_０〜２１０_６の相対位置情報や、各リンク２１０_０〜２１０_６の重さ、慣性モーメント、重心位置、初期の関節角度、速度などの情報を読み込むことで、初期設定を行う（Ｓ２）。

ここで、ＣＰＵ５０１は、ロボット２００の三次元データ（ＣＡＤデータ）に基づく仮想モデル（仮想ロボット）を仮想三次元空間に配置する。ＣＰＵ５０１は、この仮想ロボットが配置された仮想三次元空間をモニタ５２３に表示させる。

次に、ＣＰＵ５０１は、ロボット２００のバネ要素やダンパー要素などの受動要素のトルクを計算する（Ｓ３）。また、ＣＰＵ５０１は、モータトルクを計算する（Ｓ４）。即ち、これらステップＳ３，Ｓ４の計算により、力発生手段（モータ、バネ要素、ダンパー要素）の発生力としてのトルクが計算される。そして、ＣＰＵ３０１は、これらステップＳ３，Ｓ４の計算結果を用いて、ロボット２００の接触力及び拘束力を計算する（Ｓ５）。

次に、ＣＰＵ５０１は、ステップＳ３，Ｓ４，Ｓ５の計算結果を用いて順動力学計算を行い、関節の加速度を算出する（Ｓ６）。

ＣＰＵ５０１は、関節の加速度に基づき、関節の速度及び位置を計算してこれら関節の加速度、速度及び位置等からなる関節情報を更新し（Ｓ７）、３Ｄ画面情報（仮想ロボットの位置姿勢）を更新する。

次に、ＣＰＵ５０１は、ＴＣＰが終点に到達したか否かを判断し（Ｓ８）、終点に到達していなければ（Ｓ８：Ｎｏ）、予め決められた時間、仮想ロボットの動作を進め（Ｓ９）、ステップＳ３の処理に戻る。ＣＰＵ５０１は、ＴＣＰが終点に到達していれば（Ｓ８：Ｙｅｓ）、仮想ロボットを用いたロボット２００の動作のシミュレーションを終了する。

ユーザは、このシミュレーション結果に基づき動作プログラムを必要に応じて適宜修正する。ユーザは、再度シミュレーション装置５００にシミュレーションを行わせるなどして、動作プログラムを確定させる。動作プログラムが確定したら、ユーザの操作により、動作プログラムに基づく軌道データをロボット制御装置３００に送る。ロボット制御装置３００は、実際のロボット２００を、軌道データに従って動作させ、組み付け動作などをロボット２００に行わせる。このように、オフライン教示で軌道データを作成することで、オンライン教示に比べ、ロボット２００の教示作業が容易となる。

次に、ロボット２００の動作をシミュレーションするのに用いる、ステップＳ５のロボット２００に物体が接触する接触力及びロボット２００の関節における拘束力を求める処理について詳細に説明する。

図５は、接触力及び拘束力の計算方法を示すフローチャートである。なお、以下の説明で、ヤコビアンとは、ヤコビ行列のことである。また、実施形態中の拘束力τ_{ｃｏｎｓｔｒａｉｎ}は、拘束力τ_Ａに相当し、実施形態中の接触力τ_{ｃｏｎｔａｃｔ}は、接触力τ_Ｂに相当する。また、実施形態中の拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}は、拘束力ｆ_Ａに相当し、実施形態中の接触力ｆ_{ｃｏｎｔａｃｔ}は、接触力ｆ_Ｂに相当する。更に、実施形態中の拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}は、第１のヤコビアンＪ_Ａに相当し、実施形態中の接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}は、第２のヤコビアンＪ_Ｂに相当し、実施形態中の接触・拘束ヤコビアンＪは、第３のヤコビアンＪに相当する。

まずＣＰＵ５０１は、関節空間におけるロボット２００の運動方程式（第１の運動方程式）を計算する（Ｓ１１）。ここで、関節空間とは、ロボット２００の関節の数を自由度とする空間、即ち各関節位置（角度）を座標軸とする空間である。自由度とは、ある物理系の状態を記述するのに必要な変数の最小個数である。

関節空間において、ロボット２００の関節の変位をｑ、ロボット２００の関節の加速度をｑ（・・）、ロボット２００の質量に関する質量行列をＡ、ロボット２００に働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂとする。また、関節空間において、ロボット２００に働く重力に関する重力項をｇ、ロボット２００の関節に発生する発生力（即ち、モータトルクなどの能動要素のトルク、スプリング要素やダンパー要素などの受動要素のトルク）をτ_ａｃｔとする。また、関節空間において、ロボット２００の拘束力をτ_{ｃｏｎｓｔｒａｉｎ}、ロボット２００の接触力をτ_{ｃｏｎｔａｃｔ}とする。ここで、関数を時間で一次微分したものを（・）、関数を時間で二次微分したものを（・・）で表記する。

関節空間におけるロボット２００の第１の運動方程式は、加速度ｑ（・・）、質量行列Ａ、コリオリ力／遠心力項ｂ、重力項ｇ、発生力τ_ａｃｔ、拘束力τ_{ｃｏｎｓｔｒａｉｎ}、接触力τ_{ｃｏｎｔａｃｔ}を用いて以下の式（１）で表される。

ここで、第１の運動方程式である式（１）を計算するとは、式（１）における質量行列Ａ、コリオリ力／遠心力項ｂ、重力項ｇを計算することである。また、式（１）は、拘束を取り去ったロボット２００の運動方程式である。

次に、ＣＰＵ５０１は、拘束空間（第１の座標空間）における拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}と、関節空間における拘束力τ_{ｃｏｎｓｔｒａｉｎ}との関係を示す拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}を計算する（Ｓ１２）。拘束空間とは、ロボット２００の関節の拘束条件に基づく自由度を持ち、ロボット２００の拘束点の拘束方向を基準とする座標空間である。

このとき、ＣＰＵ５０１は、以下の式（２）を満たすように、拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}を算出する（第１のヤコビアン算出工程、第１のヤコビアン算出処理）。ここで、ｑ（・）は、関節空間におけるロボット２００の関節の速度である。

具体例として、ギヤの拘束のように、回転部品の軸ｑ_１と回転部品の軸ｑ_２がある減速比Ｒで拘束される場合の拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}は、以下のように計算できる。

この場合の拘束点は軸ｑ_１と軸ｑ_２の位置であり、このヤコビアンのサイズは、１つのギヤの拘束につき「１×自由度」である。

異なる具体例として、部品同士を固定する拘束のように、部品１の任意点Ｐ_１と部品２の任意点Ｐ_２の位置を拘束する場合の拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}は、以下のように計算できる。

ここで、Ｊ_Ｐ１は任意点Ｐ_１のヤコビアンを表し、Ｊ_Ｐ２は任意点Ｐ_２のヤコビアンを表し、関節の速度と任意点の速度を関係づける一般的なヤコビアンである。この場合の拘束点は任意点Ｐ_１と任意点Ｐ_２の位置であり、このヤコビアンのサイズは、１つの固定点につき「（並進成分＋回転成分）×自由度」である。

さらにこの例において、部品１の任意点Ｐ_１と部品２の任意点Ｐ_２を並進方向にのみ固定する拘束のような場合の拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}は、以下のように計算できる。

ここで、Ｊ’_Ｐ１は任意点Ｐ_１のヤコビアンの回転成分を表し、Ｊ’_Ｐ２は任意点Ｐ_２のヤコビアンの並進成分を表している。この場合の拘束点は任意点Ｐ_１と任意点Ｐ_２の位置であり、このヤコビアンのサイズは、１つの固定点につき「（並進成分）×自由度」である。

次に、ＣＰＵ５０１は、接触点の検出計算を行い（Ｓ１３）、接触点が存在するか否かを判断する（Ｓ１４）。ＣＰＵ５０１は、接触点が存在する場合、接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}を計算する（Ｓ１５：第２のヤコビアン算出工程、第２のヤコビアン算出処理）。具体的に説明すると、ＣＰＵ５０１は、ロボット２００と物体との接触点のワールド座標系Σ_０における座標値に基づき、接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}を算出する。接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}は、接触空間（第２の座標空間）における接触力ｆ_{ｃｏｎｔａｃｔ}と、関節空間における接触力τ_{ｃｏｎｔａｃｔ}との関係を示すヤコビアンである。接触空間は、ロボット２００の接触点の法線方向及び接線方向を基準とする三次元直交座標（デカルト座標）で表された座標空間である。

以下、更に具体的に説明する。なお、ｘ_{ｃｏｎｔａｃｔ}は接触空間における変位（接触点の相対位置）、ｘ_{ｃｏｎｓｔｒａｉｎ}は拘束空間における変位（拘束点の相対位置）とする。よって、ｘ_{ｃｏｎｔａｃｔ}（・）は接触空間における速度、ｘ_{ｃｏｎｔａｃｔ}（・・）は接触空間における加速度であり、ｘ_{ｃｏｎｓｔｒａｉｎ}（・）は拘束空間における速度、ｘ_{ｃｏｎｓｔｒａｉｎ}（・・）は拘束空間における加速度である。

部品ａの接触点ｘ_{ｃｏｎｔａｃｔ} ^ａ，ｉと、部品ｂの接触点ｘ_{ｃｏｎｔａｃｔ} ^ｂ，ｉとに接触が発生したとする。ｉは、接触点に付した番号である。このとき、それぞれの接触点の速度は以下のように表される。

ここで、右辺のＪ_{ｃｏｎｔａｃｔ} ^ａ，ｉ及びＪ_{ｃｏｎｔａｃｔ} ^ｂ，ｉは、ｉ番目の接触点のヤコビアンである。これを用いると、ｉ番目の接触における接触点の相対速度ｘ（・）_{ｃｏｎｔａｃｔ} ^ｉは、以下の式（３）のようになる。

次に、ｉ番目の接触における法線ベクトルをｎ_{ｃｏｎｔａｃｔ} ^ｉとし、接線ベクトルをｔ_{ｃｏｎｔａｃｔ} ^ｘ，ｉとｔ_{ｃｏｎｔａｃｔ} ^ｙ，ｉとする。これらの表記を用いて、ｉ番目の接触における法線方向速度と接線方向速度を表すヤコビアンＪ_{ｃｏｎｔａｃｔ} ^ｎ，ｉ及びＪ_{ｃｏｎｔａｃｔ} ^ｔ，ｉは以下の式（４），（５）のようになる。

各接触における法線方向ヤコビアンＪ_{ｃｏｎｔａｃｔ} ^ｎ，ｉと接線方向ヤコビアンＪ_{ｃｏｎｔａｃｔ} ^ｔ，ｉを積みあげることによって、すべての接触点に対する接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}が以下の式（６）のように表現できる。

次に、ＣＰＵ５０１は、拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}及び接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}を合わせた接触・拘束ヤコビアンＪを求める（Ｓ１６：第３のヤコビアン算出工程，第３のヤコビアン算出処理）。接触・拘束ヤコビアンＪは、以下の式（７）で表される。

次に、ＣＰＵ５０１は、接触・拘束ヤコビアンＪを用いて、関節空間における第１の運動方程式（式（１））を、以下の接触・拘束空間（第１の座標空間及び第２の座標空間）における第２の運動方程式（式（８））に変換する（Ｓ１７：変換工程，変換処理）。

なお、式（８）は、以下の式で表される。

ただし、上式のパラメータは以下の通りである。

即ち、ＣＰＵ５０１は、ヤコビアンＪを用いて第１の運動方程式（式（１））を第２の運動方程式（式（８））に変換することで、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆ_ａｃｔの各係数を求める。なお、加速度ｘ（・・）は、ｘ_{ｃｏｎｓｔｒａｉｎ}（・・），ｘ_{ｃｏｎｔａｃｔ}（・・）からなる。

次に、ＣＰＵ５０１は、式（８）に示す第２の運動方程式を、時間領域で積分することで線形化して、以下の第３の運動方程式を求める（Ｓ１８：線形化工程，線形化処理）。

つまり、線形化した第３の運動方程式において、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆ_ａｃｔの各係数は既知の値であり、ｆ_{ｃｏｎｓｔｒａｉｎ}，ｆ_{ｃｏｎｔａｃｔ}が未知数である。第３の運動方程式にこれら係数を適用すれば、第２の運動方程式を第３の運動方程式に変換したことになる。ここで、Δｔは接触力と拘束力が働く所定の時間である。

式（８）に示す第２の運動方程式を時間領域で積分し線形化した第３の運動方程式は、以下のようにも表される。

以上の第３の運動方程式は、接触・拘束空間における拘束点の拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}及び接触点の接触力ｆ_{ｃｏｎｔａｃｔ}を未知数とする連立方程式となる。

次に、ＣＰＵ５０１は、線形化した連立方程式を解くことにより、接触・拘束空間における拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}と接触力ｆ_{ｃｏｎｔａｃｔ}を算出する（Ｓ１９：力算出工程、力算出処理）。このように、本実施形態では、拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}及び接触力ｆ_{ｃｏｎｔａｃｔ}を同時に求めることができる。

ここで、接触・拘束空間における接触力ｆ_{ｃｏｎｔａｃｔ}及び拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}を連立させて計算する際には、下記の制約条件を満たすように計算を行う。

制約条件１．接触時に発生する接触空間における接触力の法線方向成分は、物体同士が離れる方向に発生する。

制約条件２．接触時に発生する接触空間における速度は、物体同士が離れる方向に発生する。

制約条件３．接触時に発生する接触空間における接触力の法線方向成分と速度の関係について、反発係数＝０の場合においては、接触力が発生した場合には速度は０になり、逆に接触力が０になる場合に速度は０にならない。

制約条件４．接触時に発生する接触空間における接触力の接線方向成分は、法線方向接触力と摩擦係数ｋを掛けた最大摩擦力以内となる。

制約条件５．拘束力によって発生する拘束空間における速度は、常に０となる。

接触力ｆ_{ｃｏｎｔａｃｔ}に関する制約は、接触力の物理的な関係性であり、これらを用いて線形相補性問題に帰着することができる。拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}に関する制約は、拘束力の物理的な関係性を表している。以上の制約をまとめると、以下のようになる。

式（１７）は、接触点において法線方向の接触力は、物体同士が離れる方向にしか作用しないことを意味している。式（１８）は、接触点において物体同士がめり込む方向に速度が発生しないことを意味している。式（１９）は、反発係数＝０の場合においては、接触力が発生した場合には接触空間における速度は０になり、逆に接触力が０になる場合には接触空間における速度は０にならないことを意味している。式（２０）は、接線方向接触力が法線方向接触力の最大摩擦係数以内であることを意味している。式（２１）は、拘束空間での速度が常に０となることを意味している。

したがって、ＣＰＵ５０１は、ステップＳ１９では、式（１７）〜式（２１）の関係式を満たすように、拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}と接触力ｆ_{ｃｏｎｔａｃｔ}を求める。具体的に説明すると、ＣＰＵ５０１は、以下の演算処理１〜５を行う。

ｌ＝０とする。ｆに初期解ｆ^ｌを代入する（演算処理１）。

ｆ^ｌのｉ成分ｆ_ｉ ^ｌを以下の式に従い更新する（演算処理２）。

ここで、ｍ_ｉ，ｊは式（１３）の行列Ｍの（ｉ，ｊ）成分である。Ｚ_ｉは式（１４）のベクトルｚの成分ｉである。

次に、ｆ_ｉ ^ｌ＋１がｆ_{ｃｏｎｔａｃｔ}の法線方向の力に相当する成分であり、かつ、ｆ_ｉ ^ｌ＋１＜０ならｆ_ｉ ^ｌ＋１＝０とする（演算処理３）。

次に、演算処理４として、ｆ_ｉ ^ｌ＋１とｆ_ｉ＋１ ^ｌ＋１がｆ_{ｃｏｎｔａｃｔ}の接線方向の力に相当する成分である場合、以下の処理４−１，４−２を実行する。即ち、処理４−１として、以下の計算をする。

また、処理４−２として、以下の計算をする。

次に、演算処理５として、以下の式の演算結果を満たす場合は、ｆ^ｌ＋１をｆとして終了する。

以上の式を満たさない場合は、ｌ＝ｌ＋１として、演算処理２へ戻る。以上の演算処理により、式（１７）〜式（２１）を満たす式（１２）に対する拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}と接触力ｆ_{ｃｏｎｔａｃｔ}を求めることができる。即ち、拘束を有するロボットの接触力を求めることが可能になる。

関節空間における拘束力τ_{ｃｏｎｓｔｒａｉｎ}及び接触力τ_{ｃｏｎｔａｃｔ}を足し合わせた関節への入力トルクをτとする。ＣＰＵ５０１は、拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}及び接触力ｆ_{ｃｏｎｔａｃｔ}を接触・拘束ヤコビアンＪを用いて、入力トルクτに変換する（Ｓ２０：力変換工程，力変換処理）。

以上、ロボット２００が物体と接触する場面をシミュレーションする場合、拘束機構である関節については拘束力でモデル化することで、拘束機構である関節を接触力でモデル化する場合と比べて運動方程式で求めるべき力の数が少なくなる。したがって、演算負荷が低減し、高速なシミュレーションが可能となる。

［実施例］
（実施例１）ギヤ拘束
図６は、実施例１におけるロボットを示す模式図である。ダイナミクスシミュレーション方法について、図６に示すような軸間に拘束を有するロボットについて接触力及び拘束力を計算する具体例を下記に示す。

図６に示すように、ロボット５０の関節である回転ジョイント４１が、駆動源であるモータに駆動される第１のギヤである入力ギヤ３１で構成され、回転ジョイント４２が、入力ギヤ３１に噛合する第２のギヤである出力ギヤ３２で構成されている。したがって、回転ジョイント４１が入力軸であり、回転ジョイント４２が出力軸である。出力ギヤ３２には、リンク３３が接続されている。したがって、リンク３３は出力ギヤ３２と共に回転する。

実施例１では、入力ギヤ３１と出力ギヤ３２との変位が、ある一定の比率で動作するという拘束を有している。即ち、拘束条件には、入力ギヤ３１と出力ギヤ３２との速度伝達比（ギヤ比）が含まれている。例えば、入力ギヤ３１と出力ギヤ３２との速度伝達比が２：１の拘束条件で拘束されているものとする。拘束を有するロボットのリンク３３に、物体３４と接触したときの接触力を求める。

ロボットのリンク間の相対位置情報を表１にまとめる。

また、リンクの緒元を表２にまとめる。

回転ジョイント４１と回転ジョイント４２に関する拘束条件を表３にまとめる。

ここでリンク３３は出力ギヤ３２に固定されているものとし、以降では一体部品として扱う。まず図６の状態における関節変位は、以下のようになる。

各関節速度は、以下のようになる。

このときの質量行列Ａは、以下のようになる。

重力項ｇは、以下のようになる。

また、遠心力／コリオリ力ｂは、以下のようになる。

以上により、拘束を取り去った状態におけるロボットの以下の運動方程式のＡ，ｂ，ｇに関する項が求まる（Ｓ１１）。

ここで発生力τ_{ｍｏｔｏｒ}は、駆動源であるモータの駆動力である。なお、実施例１では、これらＡ，ｂ，ｇの計算手順については省略しているが、これらＡ，ｂ，ｇの数値は、各リンクの慣性モーメントや重さ、重心位置、各リンク間の相対位置情報から計算されるものである。拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}は第１軸と第２軸の速度伝達比（ギヤ比）が２：１のギヤで接続されているという拘束なので、以下のように算出される（Ｓ１２）。

次に、接触点検出計算を行う（Ｓ１３）。接触点情報を表４及び表５に示す。

接触点情報と法線方向の情報から接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}は以下のように算出される（Ｓ１５）。

これらの接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}と拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}を用いて、接触・拘束ヤコビアンＪを算出する（Ｓ１６）。また、接触・拘束ヤコビアンＪを用いて運動方程式を変換し、式（１２）を算出する。このとき、Ｍとｚは以下のように算出される。

ここで接触力と拘束力が働く時間を表すΔｔについては、３．７５Ｅ−０８［ｓ］とした。前述した方法により、摩擦係数を０．２としてｆを求める（Ｓ１９）。

ｆの第１成分が拘束力ｆ_{ｃｏｎｓｔｒａｉｎ}を表し、第２成分と第３成分が接触力ｆ_{ｃｏｎｔａｃｔ}の法線方向成分、第４〜第７成分までが接触力ｆ_{ｃｏｎｔａｃｔ}の接線方向成分に対応している。算出したｆによりｘ（・）を算出すると、以下のようになる。

拘束点ｘ_{ｃｏｎｓｔｒａｉｎ}における速度である第１成分は０となっており、式（２１）を満たしていることが確認できる。また、第２成分と第３成分は接触点ｘ_{ｃｏｎｔａｃｔ}における法線方向の速度も０となっている。ｆの対応する成分を見てみるといずれも正の値を取っており、式（１７）と式（１８）と式（１９）を満たしていることが確認できる。

また、ｆの接触力の法線方向と接線方向の力を比較すると、接触１においては、以下の不等式が成り立つ。

接触２においても、以下の不等式が成り立つ。

いずれも式（２０）を満たしていることが確認できる。このとき、接触力と拘束力を足し合わせた各軸の力を示すトルクτは、以下のように求まる（Ｓ２０）。

以上説明したとおり、図６に示すような、軸間に拘束を有するロボット５０に対して、接触力を計算することができるため、ギヤ３１，３２のダイナミクスを考慮したロボットの接触シミュレーションを行うことができる。

実施例１では、ロボット５０と物体３４が接触した瞬間を具体例として挙げている。接触力と拘束力とを、連立方程式を用いて解いており、ギヤの拘束１つにつき１個の変数、接触１つにつき３個の変数を解く必要がある。実施例１では接触点が２つあるため、７個の変数を解いている。この変数の個数は、拘束ヤコビアン、接触ヤコビアンの行数に対応している。また、接触力計算では式（１７）〜式（２０）を考慮しなければならない分、計算量が大きい。つまり、ある１つの接触点の接触力の計算量は、ある１つのギヤの拘束力の計算量に比べて、最低３倍以上の計算量が必要である。さらに、実際にギヤを接触でシミュレーションする場合、複数の接触点が発生する可能性がある。仮に実施例１において拘束力を廃しギヤを接触力でシミュレーションした場合では、ギヤ間において最低２つの接触点が発生するため、最低６倍以上の計算量が必要である。さらに、ギヤの数が増えるほどその影響は大きくなる。そのため、拘束を有するロボットの接触問題をシミュレーションする場合は、実施例１のように接触力と拘束力を連立させて計算することにより、計算時間を短縮することができる。これにより、高速なシミュレーションが可能となる。

（実施例２）角度の伝達誤差つきギヤ拘束
実施例２では、ロボットで一般的に使用される波動歯車減速機などの角度の伝達誤差を持つギヤを模した軸間拘束を有するロボットに対して、接触力の計算方法を適用したときの具体例を示す。

ロボットで使用される波動歯車減速機など柔軟物を含むギヤの伝達力計算を行うには、接触力計算に加えて変形計算を行う必要があり、計算コストがかかる。そのため、実施例２のように拘束力でシミュレーションを実現するのが有効である。

なお、ロボットの基本構成、リンクの配置、リンクの緒元は実施例１と同一とし、説明を省略する。

実施例２は、実施例１と同様にギヤで接続されたリンクを模しており、回転ジョイント４１が入力軸、回転ジョイント４２が出力軸となる。そして、実施例２では、以下のような比率と伝達誤差で表されるギヤで接続されている。即ち、拘束条件には、入力ギヤ３１と出力ギヤ３２との速度伝達比（ギヤ比）と、入力ギヤ３１と出力ギヤ３２との間の角度の伝達誤差が含まれているものとする。

ここで、右辺第一項がギヤ比を、右辺第二項が角度伝達誤差を表している。拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}は以下のように算出される。まず、上記の入力軸と出力軸の関係について微分し、速度の関係式に変換する。

その後、式をまとめて以下のような式で拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}を算出する。

実施例２では、表６のようなギヤ比と角度伝達誤差パラメータを選択した。

角度伝達誤差以外のパラメータについては実施例１と同様であるため、ロボットのリンク間の相対位置情報とリンクの緒元は省略する。まず図６の状態における各関節変位は、以下のようになる。

各関節速度は、以下のようになる。

このときの質量行列Ａは、以下のようになる。

重力項ｇは、以下のようになる。

また、遠心力／コリオリ力ｂは、以下のようになる。

以上により、拘束を取り去った状態におけるロボットの以下の運動方程式のＡ，ｂ，ｇに関する項が求まる（Ｓ１１）。

なお、実施例２ではこれらＡ，ｂ，ｇの計算手順については省略しているが、これらＡ，ｂ，ｇの数値は、各リンクの慣性モーメントや重さ、重心位置、各リンク間の相対位置情報から計算されるものである。拘束ヤコビアンは第１軸と第２軸の変位比率が１０：１のギヤで接続されており、かつ角度伝達誤差を含むという拘束であり、前段で計算した式に値を入れると以下のようになる（Ｓ１２）。

次に、接触点検出計算を行う（Ｓ１３）。接触点情報を表７及び表８に示す。

接触点情報と法線方向の情報から接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}は以下のように算出される（Ｓ１４）。

これらの接触ヤコビアンＪ_{ｃｏｎｔａｃｔ}と拘束ヤコビアンＪ_{ｃｏｎｓｔｒａｉｎ}を用いて、接触・拘束ヤコビアンを算出する（Ｓ１５）。また、接触・拘束ヤコビアンＪを用いて運動方程式を変換し、式（１２）を算出する。このとき、Ｍとｚは以下のように算出される（Ｓ１７）。

ここで接触力と拘束力が働く所定の時間Δｔについては、１．００Ｅ−０７［ｓ］とした。前述した方法により、摩擦係数を０．２としてｆを求める。

ｆの第１成分が拘束力を表し、第２成分と第３成分が接触力の法線方向成分、第４〜第７成分までが接触力の接線方向成分に対応している。算出したｆによりｘ（・）を算出すると、以下のようになる。

拘束点における速度である第１成分は０となっており式（２１）を満たしていることが確認できる。また、第２成分と第３成分は接触点における法線方向の速度も０となっている。このとき、ｆの対応する成分を見てみるといずれも正の値を取っており、式（１７）と式（１８）と式（１９）を満たしていることが確認できる。

また、ｆの接触力の法線方向と接線方向の力を比較すると、以下の不等式が成り立つ。

よって、式（２０）を満たしていることが確認できる。また、ｆの接触力の法線方向と接線方向の力を比較すると、以下の不等式が成り立つ。

よって、式（２０）を満たしていることが確認できる。このとき、接触力と拘束力を足し合わせた各軸の力（トルク）τは、以下のように求まる。

以上説明したとおり、ギヤの角度伝達誤差を考慮したロボットの接触シミュレーションを行うことができる。

［実施例３］
次に、実施例３のダイナミクスシミュレーション方法について説明する。図７（ａ）は、実施例３におけるロボットを示す模式図である。図７（ａ）に示すロボット８０は、パラレルリンク型のロボットである。図７（ａ）に示すような、拘束点の相対速度が常に０になるような拘束を有するロボット８０に対して、接触計算方法を適用したときの具体例について説明する。図７（ａ）に示すように、ロボット８０は、複数のリンク６１〜６４が、関節である回転ジョイント７１〜７５で回転可能に連結されている。

図７（ｂ）は、図７（ａ）に示すロボットの１つの回転ジョイントを、互いに拘束関係にある２つの拘束点とした模式図である。ロボット８０は、回転ジョイント７４を取り去り、代わりに拘束点８１と拘束点８２とがｘ_０方向とｚ_０方向の座標が常時一致するという拘束条件が設けられた、２本別々のロボットと考えてもよい。２本のロボットのうち、１本目はリンク６１〜６３を有し、２本目はリンク４４のみによって構成される。図７（ａ）に示すロボット８０は、拘束を有するロボットである。

図７（ｃ）は、実施例３においてロボットに物体が接触した状態を示す模式図である。この拘束を有するロボット８０が、図７（ｃ）に示すような物体９１と接触したときの接触力を求める。

ロボット８０のリンク間の相対位置情報を表９にまとめる。また、リンクの緒元は実施例１と同様であるため省略する。表１０には、拘束点８１と拘束点８２に関する拘束条件をまとめる。

図７（ｃ）の状態における各関節変位は、以下のようになる。

各関節速度は、以下のようになる。

このときの質量行列Ａは、以下のようになる。

重力項ｇは、以下のようになる。

また、遠心力／コリオリ力ｂは、以下のようになる。

以上により、拘束を取り去った状態におけるロボット８０の運動方程式のＡ，ｂ，ｇに関する項が求まる（Ｓ１１）。

なお、実施例３ではこれらＡ，ｂ，ｇの計算手順については省略しているが、これらＡ，ｂ，ｇの数値は、表１と表２に示した各リンクの慣性モーメントや重さ、重心位置、また、各リンク間の相対位置情報から計算されるものである。

拘束ヤコビアンは、表１０の拘束点情報と各軸の姿勢から以下のように算出される（Ｓ１２）。具体的には、拘束点８１と拘束点８２の位置を計算し、各点におけるヤコビアンを計算し、それぞれのヤコビアンの差を取り、そこから拘束する軸成分を取得する。本シミュレーションでは、ｙ並進成分とｚ並進成分を拘束することとした。

次に接触点情報を表１１及び表１２に示す。

接触点情報と法線方向の情報から接触ヤコビアンは以下のように算出される（Ｓ１５）。

これらの接触ヤコビアンと拘束ヤコビアンを用いて、Ｍとｚは以下のように算出される。

ここで接触力と拘束力が働く所定の時間Δｔについては、１．００Ｅ−０７［ｓ］とした。前述した方法により、摩擦係数を０．２としてｆを求める。

ｆの第１成分と第２成分が拘束力を表し、第３成分と第４成分が接触力の法線方向成分、第５〜第８成分までが接触力の接線方向成分に対応している。算出したｆによりｘ（・）を算出すると、以下のようになる。

拘束点における速度である第１成分と第２成分は０となっており、式（２１）を満たしていることが確認できる。また、第３成分と第４成分は接触点における法線方向の速度も０となっている。ｆの対応する成分を見てみるといずれも正の値を取っており、式（１７）と式（１８）と式（１９）を満たしていることが確認できる。また、ｆの接触力の法線方向と接線方向の力を比較すると１つ目の接触について、以下の不等式が成り立つ。

２つ目の接触について、以下の不等式が成り立つ。

いずれも式（２０）を満たしていることが確認できる。このとき、接触力と拘束力を足し合わせた各軸の力（トルク）τは、以下のように求まる。

以上説明したとおり、図７（ｂ）に示すような拘束点８１，８２の相対速度が常に０になるような拘束を有するロボットに対して接触力を計算することができる。このため、パラレルリンクを考慮したロボットの接触シミュレーションを行うことができる。

なお、シミュレーション中の接触や拘束の数やその種類は、これらの実施例に限定されるものではない。

本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、実施形態に記載されたものに限定されない。

本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

また、上述の実施形態では、ロボットアームが垂直多関節のロボットアームの場合について説明したが、これに限定するものではない。ロボットアームが、例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい。

また、上述の実施形態では、シミュレーション装置とロボット制御装置とが別のコンピュータで構成される場合について説明したが、これに限定するものではなく、シミュレーション装置とロボット制御装置とが１つのコンピュータで構成されていてもよい。

１００…ロボットシステム、２００…ロボット、３００…ロボット制御装置、５００…シミュレーション装置、５０１…ＣＰＵ（演算部）、６００…制御システム

Claims (9)

1. 関節を有するロボットの動作をシミュレーションするのに用いる、前記ロボットに物体が接触する接触力及び前記ロボットの関節における拘束力を求める演算部を備えたシミュレーション装置であって、
   前記演算部は、
   前記ロボットの関節の数を自由度とする関節空間において前記ロボットの関節の速度をｑ（・）としたとき、前記ロボットの関節の拘束条件に基づく自由度を持ち、前記ロボットの拘束点の拘束方向を基準とする第１の座標空間における拘束力ｆ_Ａと、前記関節空間における拘束力τ_Ａとの関係を示す第１のヤコビアンＪ_Ａを、
   

   

   を満たすように算出する第１のヤコビアン算出処理と、
   前記ロボットと前記物体との接触点のワールド座標系における座標値に基づき、前記ロボットの接触点の法線方向及び接線方向を基準とする直交座標で表された第２の座標空間における接触力ｆ_Ｂと、前記関節空間における接触力τ_Ｂとの関係を示す第２のヤコビアンＪ_Ｂを算出する第２のヤコビアン算出処理と、
   前記第１のヤコビアンＪ_Ａ及び前記第２のヤコビアンＪ_Ｂを合わせた第３のヤコビアンＪを求める第３のヤコビアン算出処理と、
   前記関節空間において、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτ_ａｃｔとし、前記関節空間における前記ロボットの第１の運動方程式
   

   

   を前記第３のヤコビアンＪを用いて変換することで、前記第１の座標空間及び前記第２の座標空間における、加速度ｘ（・・）、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、発生力ｆ_ａｃｔとなる前記ロボットの第２の運動方程式
   

   

   を求める変換処理と、
   所定の時間をΔｔとし、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
   

   

   を求める線形化処理と、
   前記線形化した前記第３の運動方程式を前記拘束力ｆ_Ａ及び前記接触力ｆ_Ｂを未知数とする連立方程式とし、前記連立方程式を解くことにより前記拘束力ｆ_Ａ及び前記接触力ｆ_Ｂを算出する力算出処理と、を実行することを特徴とするシミュレーション装置。
2. 前記演算部は、
   前記拘束力ｆ_Ａ及び前記接触力ｆ_Ｂを前記第３のヤコビアンＪを用いて、前記拘束力τ_Ａ及び前記接触力τ_Ｂを足し合わせた関節の入力トルクτに変換する力変換処理を更に実行することを特徴とする請求項１に記載のシミュレーション装置。
3. 前記ロボットの関節が、駆動源により駆動される第１のギヤと、前記第１のギヤに噛合する第２のギヤとを有し、
   前記拘束条件には、前記第１のギヤと前記第２のギヤとの速度伝達比が含まれていることを特徴とする請求項１又は２に記載のシミュレーション装置。
4. 前記拘束条件には、前記第１のギヤと前記第２のギヤとの間の伝達誤差が含まれていることを特徴とする請求項３に記載のシミュレーション装置。
5. 請求項１乃至４のいずれか１項に記載のシミュレーション装置と、
   前記シミュレーション装置によるシミュレーションにより作成した前記ロボットの動作プログラムに基づき、前記ロボットの動作を制御するロボット制御装置と、を備えた制御システム。
6. 前記ロボットと、
   請求項５に記載の制御システムと、を備えたロボットシステム。
7. 演算部が、関節を有するロボットの動作をシミュレーションするのに用いる、前記ロボットに物体が接触する接触力及び前記ロボットの関節における拘束力を求めるシミュレーション方法であって、
   前記演算部が、前記ロボットの関節の数を自由度とする関節空間において前記ロボットの関節の速度をｑ（・）としたとき、前記ロボットの関節の拘束条件に基づく自由度とする第１の座標空間における拘束力ｆ_Ａと、前記関節空間における拘束力τ_Ａとの関係を示す第１のヤコビアンＪ_Ａを、
   

   

   を満たすように算出する第１のヤコビアン算出工程と、
   前記演算部が、前記ロボットと前記物体との接触点のワールド座標系における座標値に基づき、前記ロボットの接触点の法線方向及び接線方向を基準とする直交座標で表された第２の座標空間における接触力ｆ_Ｂと、前記関節空間における接触力τ_Ｂとの関係を示す第２のヤコビアンＪ_Ｂを算出する第２のヤコビアン算出工程と、
   前記演算部が、前記第１のヤコビアンＪ_Ａ及び前記第２のヤコビアンＪ_Ｂを合わせた第３のヤコビアンＪを求める第３のヤコビアン算出工程と、
   前記関節空間において、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτ_ａｃｔとし、前記関節空間における前記ロボットの第１の運動方程式
   

   

   を前記第３のヤコビアンＪを用いて変換することで、前記第１の座標空間及び前記第２の座標空間における、加速度ｘ（・・）、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、発生力ｆ_ａｃｔとなる前記ロボットの第２の運動方程式
   

   

   を求める変換工程と、
   前記演算部が、所定の時間をΔｔとし、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
   

   

   を求める線形化工程と、
   前記演算部が、前記線形化した前記第３の運動方程式を前記拘束力ｆ_Ａ及び前記接触力ｆ_Ｂを未知数とする連立方程式とし、前記連立方程式を解くことにより前記拘束力ｆ_Ａ及び前記接触力ｆ_Ｂを算出する力算出工程と、を備えたことを特徴とするシミュレーション方法。
8. コンピュータに請求項７に記載のシミュレーション方法の各工程を実行させるためのプログラム。
9. 請求項８に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images