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

Abstract

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

Description

本発明は、変形する物体に対して拘束を有するロボットを用いて所定の動作を行う際のロボットのシミュレーションの技術に関する。

近年、ロボット開発において、ロボットの動作を詳細にシミュレーションするために、様々な技術開発が行われている。拘束力の導入もその一つであり、パラレルリンクを用いたロボットのシミュレーションにおいて高速な演算が可能となっている。ここで、拘束力とは、ある物体に対して他の物体の動作を拘束する力をいう。拘束力の一例として、ギヤとギヤとが速度伝達比Ｚ_Ａ：Ｚ_Ｂで伝達する力が挙げられる。

一方、ロボットは、ワークの搬送や組み立てなどの生産ラインにも適用されている。例えば、ギヤを駆動源としたエンドエフェクタで組立部品を保持し、被組立部品と組付けて物品を製造する。

このようなロボットの動作をシミュレーションする技術として、特許文献１には、拘束を有するロボットの動作をシミュレートする際の接触力と拘束力の計算方法が開示されている。

特開２０１８−０３０２１０号公報

しかしながら、上述したようなロボットを用いた物品の製造工程において、組立部品の剛性が低いとエンドエフェクタの保持力により組立部品が変形することがある。特許文献１に記載の計算方法では、ワーク（組立部品および被組立部品）の変形が考慮されておらず、部品の変形を伴う動作をシミュレーションすることはできなかった。

上記課題を解決するためのシミュレーション装置は、関節を有するロボットが物体を保持する動作をシミュレーションするのに用いる、前記ロボットと変形した前記物体の接触力及び前記ロボットの関節における拘束力を求める演算部を備えたシミュレーション装置であって、
前記演算部は、
前記ロボットの関節の拘束点を基準とする第１の座標空間における拘束力ｆ_Ａと、前記ロボットの関節空間における拘束力τ_Ａとの関係を示す第１ヤコビアンＪ_Ａを算出し、
前記ロボットの関節の接触点を基準とする第２の座標空間における接触力ｆ_Ｂと、前記ロボットの関節空間における接触力τ_Ｂとの関係を示す第２ヤコビアンＪ_Ｂを算出し、
前記第１ヤコビアンＪ_Ａと第２ヤコビアンＪ_Ｂを合わせた第３ヤコビアンＪを算出し、
前記関節空間における、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力および遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτ_ａｃｔとした、前記関節空間における前記ロボットの運動方程式

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

を求める変換処理をし、
所定の時間をΔｔ、前記物体の単位力あたりの変形変化を示す行列Ｗ、前記接触点の相対距離を示すδ^ｆｒｅｅを用いて、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式

を求める線形化処理をし、
前記線形化処理した運動方程式に基づく連立方程式を作成し、
前記連立方程式を解くことにより、前記拘束力ｆ_Ａと、変形した物体の接触力を考慮した接触力ｆ_Ｂとを算出することを特徴とする。

本発明によれば、部品の変形を考慮した接触力を計算し、その接触力と拘束力とを用いてロボットの動作シミュレーションを行うことができる。そのため、ワークが変形するような動作においても正確にロボットの動作をシミュレーションすることができる。

実施形態に係るロボットシステムを示す概略図である。 ロボットアームの複数の関節のうちの１つの関節を示す部分断面図である。 実施形態に係る制御システムを示すブロック図である。 実施形態に係るシミュレーション方法を示すフローチャートである。 接触力及び拘束力の計算方法を示すフローチャートである。 実施例１におけるロボットの概略図である。

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

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

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

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

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

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

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

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

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

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

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

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

シミュレーション装置５００は、演算部としてのＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）５０１を備えている。また、シミュレーション装置５００は、記憶部として、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）５０２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）５０３、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）５０４を備えている。また、シミュレーション装置５００は、記録ディスクドライブ５０５及び複数のインタフェース５１１〜５１５を備えている。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

以上で、ロボット２００に対する接触力と拘束力のモデル化が完了する。

そして、ＣＰＵ５０１は、部品変形を考慮した接触力と拘束力を連立した方程式を基に計算を行う。

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

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

このとき、前記関節空間におけるロボット２００の第１の運動方程式は、以下の式（７）で表される。

ここで、運動方程式である式（７）を計算するとは、式（７）における質量行列Ａ、コリオリ力／遠心力項ｂ、重力項ｇを計算することである。

次に、接触・拘束ヤコビアンＪを用いて以下の接触・拘束空間における第２の運動方程式に変換する。すると（７）式は（８）式のように変換できる。

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

ここで、加速度ｘ（・・）は接触・拘束空間における加速度であり、ｆ_ａｃｔは接触・拘束空間におけるロボットのモータ、スプリング、ダンパーなどの力発生手段による発生力である。また、質量行列Λは接触・拘束空間における質量行列、μは接触・拘束空間におけるコリオリ力／遠心力項、重力項ｐは接触・拘束空間における重力項である。

即ち、ＣＰＵ５０１は、ヤコビアンＪを用いて第１の運動方程式を第２の運動方程式に変換することで、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆ_ａｃｔの各係数を求める。

次に、この運動方程式を、時間領域Δｔで積分することで線形化処理して、以下の第３の運動方程式を求める（Ｓ５７）。

この式は、接触・拘束空間における、力と位置の関係を表している。式中、Λ^−１Δｔ^２は単位力による位置変化である。行列Ｗは単位力による変形変化を表わす行列であり、変形物体の接触点における剛性から計算する事ができる。

変形量を計算するための方法である有限要素法において、変形物体は複数の要素で構成されており、要素は節点で構成されている。例えば一般的に用いられる四面体要素では、一つの要素を４つの節点で構成している。接触点の剛性を計算する場合、Ｗは節点同士の剛性と、接触点とその近傍の節点からの距離の比を入力して以下の式で計算を行う。

ここでＨは接触点とその近傍の節点の距離の比であり、Ｃは各節点の剛性の逆数を表している。また、ｉは接触点近傍の節点の数である。また、接触は２物体間で発生するためペア（以下、接触点ペア）を構成しており、接触点ペア毎に剛性を考慮する必要がある。式中のＡとＢは接触点ペアにおける、Ａ側接触点とＢ側接触点を表している。なお、変形が発生しない剛体部品のＷは０となる。

ここで物体が接触力によって変形する場合を考えた場合、Λ行列のサイズ（「接触点ペア数＋拘束数」×「接触点ペア数＋拘束数」）と、Ｗ行列のサイズ（「接触点ペア数」×「接触点ペア数」）が異なるため、（１２）式の右辺第１項を計算することができない。そこで行列サイズを合わせるためにΛ行列と同じサイズの行列を作成し、接触点に関わる部分にＷ行列を配置する必要がある。

δｆｒｅｅは、接触・拘束空間における、１ステップ後の接触点ペアまたは拘束点ペア同士の相対距離を表しており、剛体項と変形項に分けることができる。

（１４）式の右辺第１項は剛体であり以下のように計算できる。

また、（１４）式の右辺第２項は変形項であり、外力を０と仮定して変形シミュレーションを１ステップ進めることで計算することができる。

ここで物体が接触力によって変形する場合を考えた場合、剛体項の行列のサイズ（「接触点ペア数＋拘束数」×１）と、変形項の行列のサイズ（「接触点ペア数」×１）が異なるため、（１４）式を計算することができない。そこで行列サイズを合わせるために剛体項と同じサイズの行列を作成し、接触点に関わる部分に変形項の行列を配置する必要がある。

以上の計算結果より、接触・拘束ヤコビアンを用いて線形化した、接触・拘束空間における運動方程式を得る事ができる。つまり、線形化した第３の運動方程式において、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆａｃｔの各係数は既知の値であり、ｆｃｏｎｓｔｒａｉｎ，ｆｃｏｎｔａｃｔが未知数である。第３の運動方程式にこれら係数を適用すれば、第２の運動方程式を第３の運動方程式に変換したことになる。ここで、Δｔは接触力と拘束力が働く所定の時間である。この運動方程式は、拘束力ｆ_Ａ（ｆｃｏｎｓｔｒａｉｎ）と接触力ｆ_Ｂ（ｆｃｏｎｔａｃｔ）を未知数とした連立方程式である。この方程式を用いて、接触・拘束空間における接触力と拘束力から関節空間における接触力と拘束力を計算する（Ｓ５８）。

ここで、接触・拘束空間における接触力と拘束力を連立させて計算する際には、下記の制約関係を満たすように計算を行う。

接触時に発生する接触空間における接触力の法線方向成分は、物体同士が離れる方向に発生する。そのため以下の条件が発生する。

接触時に発生する接触空間における相対位置の変化は、物体同士が離れる方向に発生する。そのため以下の条件が発生する。

接触時に発生する接触空間における接触力の法線方向成分と、接触ペア間の相対位置の変化の関係は、接触力が発生した場合には相対位置の近接方向への位置変化はなくなる。そのため、接触ペア間が閾値距離以内であっても相対位置が広がっているならば接触力は発生しない。そのため以下の条件が発生する。

接触時に発生する接触空間における接触力の接線方向成分は、法線方向接触力と摩擦係数を乗じた最大摩擦力以内となる。そのため以下の条件が発生する。

また、拘束力によって発生する拘束空間における相対位置の変化は、常に０となる。そのため以下の条件が発生する。

以上のように、接触力に関する制約は、接触力の物理的な関係性であり、これらを用いて線形相補性問題に帰着する事ができる。また、拘束力に関する制約は拘束力の物理的な関係性を表している。

次に、部品の変形を有限要素法で計算する（Ｓ５９）。前記計算した接触力を入力し、有限要素法を用いて部品の変形を計算する。有限要素法については公知の一般的な手法を用いることができる。

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

そしてＳ６に戻り、ダイナミクスシミュレーションを実行する。前記計算した接触力と拘束力を入力とし、順動力学計算を行う事でロボットや部品の加速度を計算する。次に、積分計算を行う事でロボットや部品の速度や位置について計算する。

以上、拘束を有するロボット２００の、変形部品との接触を伴うシミュレーションを実行できる。

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

図６に示すように、駆動源であるモータに駆動されるギヤ３１が、ロボットハンド５０の関節であるリンク３２，３３に接続されている。そのためギヤ３１の回転に伴い、ギヤ部を有するリンク３２，３３はｙ０方向に移動する。

実施例１ではギヤ３１とリンク３２，３３のギヤ部との変位が、ある一定の比率で動作するという拘束を有している。即ち、拘束条件には、ギヤ３１とリンク３２，３３のギヤ部との速度伝達比（ギヤ比）が含まれている。拘束を有するロボットのリンク３２，３３に、物体３４と接触したときの接触力を求める。なお物体３４は、ロボットハンドで把持する、変形する部品である。

まず、第１ヤコビアンである拘束ヤコビアンを導出する。

本実施例では、ギヤで接続されたモータとリンクを模しており、ｑ１は入力軸を、ｑ２，ｑ３は出力軸を模しており、以下のような比率と角度伝達誤差で表されるギヤで接続されている。

ここで、Ａはギヤ比である。

次に、上記の入力軸と出力軸の関係について微分し、速度の関係式に変換する。

さらに（２１）〜（２４）式をまとめて以下のような式で拘束ヤコビアンを算出する。

以上が、ロボットに係る機構を拘束力でモデル化する工程である。

次に、接触点検出計算を行い、第２ヤコビアンである接触ヤコビアンを計算する。

接触点検出は、部品の変形を考慮して行う必要がある。つまり、部品を構成する有限要素の表面が接触点になるため、部品をメッシュ分割する。部品をメッシュ分割し、接触点を算出したら、発明を実施するための形態に記載の方法で（５）式の接触ヤコビアンの算出を行う。

以上が、部品変形を考慮した接触点を計算する工程である。

次に、前記接触・拘束ヤコビアンを用いて線形化した、接触・拘束空間における運動方程式における、ΛとＷとδ^ｆｒｅｅについて計算する。

このようにして算出した接触・拘束空間における運動方程式を、接触力と拘束力を表すｆについて解く。こうすることで、部品変形を考慮した接触力と拘束力を連立させて計算することができる。

発明を実施するための形態に記載の方法によって計算される拘束力は、ロボットに係る機構モデルが成り立つのに必要な力が計算される。また、接触力は、事前に設定した部品の摩擦力を反映させた値が計算される（図６（ｃ））。

以上が、部品変形を考慮した接触力と拘束力を連立させて計算する工程である。

次に、この接触力と拘束力などの外力を入力として、部品の変形を有限要素法で計算する。以上が部品の変形を有限要素法で計算する工程である（図６（ｄ））。

最後に、部品の加速度を順動力学計算し、部品の位置を積分計算する。

以上説明したとおり、図６に示すような軸間に拘束を有するロボットハンドが変形物体を把持するシミュレーションにおいて、接触力と拘束力を連立させて計算する事ができる。そのため、変形物体の接触力と拘束力を連立させたシミュレーションを行う事ができるという効果がある。

また、本実施例のようにギヤの動作を拘束力によって計算した場合の時間と、接触力を用いた場合の計算時間を比較した場合、接触力計算では接触点が複数出る可能性に加え、線形相補性を考慮しなければならない分、計算時間が多い。また、ギヤの数が増えるほどその影響は大きくなる。そのため、拘束を有するロボットアームの接触問題をシミュレートする場合は、本実施例のように接触力と拘束力を連立させて計算する方法が、計算時間が少ないため有効である。

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

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

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

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

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

Claims (9)

1. 関節を有するロボットが物体を保持する動作をシミュレーションするのに用いる、前記ロボットと変形した前記物体の接触力及び前記ロボットの関節における拘束力を求める演算部を備えたシミュレーション装置であって、
   前記演算部は、
   前記ロボットの関節の拘束点を基準とする第１の座標空間における拘束力ｆ_Ａと、前記ロボットの関節空間における拘束力τ_Ａとの関係を示す第１ヤコビアンＪ_Ａを算出し、
   前記ロボットの関節の接触点を基準とする第２の座標空間における接触力ｆ_Ｂと、前記ロボットの関節空間における接触力τ_Ｂとの関係を示す第２ヤコビアンＪ_Ｂを算出し、
   前記第１ヤコビアンＪ_Ａと第２ヤコビアンＪ_Ｂを合わせた第３ヤコビアンＪを算出し、
   前記関節空間における、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力および遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτ_ａｃｔとした、前記関節空間における前記ロボットの運動方程式
   

   

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

   

   
   を求める変換処理をし、
   所定の時間をΔｔ、前記物体の単位力あたりの変形変化を示す行列Ｗ、前記接触点の相対距離を示すδ^ｆｒｅｅを用いて、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
   

   

   
   を求める線形化処理をし、
   前記線形化処理した運動方程式に基づく連立方程式を作成し、
   前記連立方程式を解くことにより、前記拘束力ｆ_Ａと、変形した物体の接触力を考慮した接触力ｆ_Ｂとを算出することを特徴とするシミュレーション装置。
2. 前記δ^ｆｒｅｅは、剛体項と変形項からなり、
   前記演算部は、前記第３の運動方程式を求める際に、行列サイズを変形する処理を行う請求項１に記載のシミュレーション装置。
3. 前記演算部は、
   前記拘束力ｆ_Ａ及び前記接触力ｆ_Ｂを前記第３のヤコビアンＪを用いて、前記拘束力τ_Ａ及び前記接触力τ_Ｂを足し合わせた関節の入力トルクτに変換する処理を更に行うこと請求項１または２に記載のシミュレーション装置。
4. 前記ロボットの関節が、駆動源により駆動される第１のギヤと、前記第１のギヤに噛合する第２のギヤとを有し、前記拘束条件には、前記第１のギヤと前記第２のギヤとの速度伝達比が含まれていることを特徴とする請求項１乃至３のいずれか１項に記載のシミュレーション装置。
5. 請求項１乃至４のいずれか１項に記載のシミュレーション装置と、
   前記シミュレーション装置によるシミュレーションにより作成した前記ロボットの動作プログラムに基づき、前記ロボットの動作を制御するロボット制御装置と、を備えた制御システム。
6. 前記ロボットと、
   請求項５に記載の制御システムと、を備えたロボットシステム。
7. 演算部が、関節を有するロボットの動作をシミュレーションするのに用いる、前記ロボットと変形した前記物体の接触力及び前記ロボットの関節における拘束力を求めるシミュレーション方法であって、
   前記演算部が、
   前記ロボットの関節の拘束点を基準とする第１の座標空間における拘束力ｆ_Ａと、前記ロボットの関節空間における拘束力τ_Ａとの関係を示す第１ヤコビアンＪ_Ａを算出する工程と、
   前記ロボットの関節の接触点を基準とする第２の座標空間における接触力ｆ_Ｂと、前記ロボットの関節空間における接触力τ_Ｂとの関係を示す第２ヤコビアンＪ_Ｂを算出する工程と、
   前記第１ヤコビアンＪ_Ａと第２ヤコビアンＪ_Ｂを合わせた第３ヤコビアンＪを算出する工程と、
   前記関節空間における、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力および遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとした、前記関節空間における前記ロボットの運動方程式
   

   

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

   

   
   を求める変換処理をし、
   所定の時間をΔｔ、前記物体の単位力あたりの変形変化を示す行列Ｗ、前記接触点の相対距離を示すδ^ｆｒｅｅを用いて、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
   

   

   
   を求める線形化処理をし、
   前記線形化処理した運動方程式に基づく連立方程式を作成し、
   前記連立方程式を解くことにより、前記拘束力ｆ_Ａと、変形した物体の接触力を考慮した接触力ｆ_Ｂとを算出する工程と、
   を有することを特徴とするシミュレーション方法。
8. 請求項７に記載のシミュレーション方法の各工程をコンピュータに実行させるためのプログラム。
9. 請求項８に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images