(57)【要約】      （修正有）【課題】部品の変形を考慮した接触力と拘束力を用いたロボットの動作シミュレーション装置を提供する。【解決手段】ロボットシステム１００におけるシミュレーションシステム５５０のコンピュータにより、拘束を有するロボット２００の動作のシミュレーションを行うのに用いる接触力及び拘束力を計算する。この計算方法は、まず、接触・拘束空間と関節空間との関係を示すヤコビアンを求める。このヤコビアンを用いてロボット２００の運動方程式を接触・拘束空間に変換する。更に変換した運動方程式を物体の単位力あたりの変形変化を示す行列Ｗを用いて線形化する。これにより得られる連立方程式を解くことで、接触力及び拘束力を同時に求める。【選択図】図１
2020075329.tif 000002
【特許請求の範囲】
【請求項１】
関節を有するロボットが物体を保持する動作をシミュレーションするのに用いる、前記ロボットと変形した前記物体の接触力及び前記ロボットの関節における拘束力を求める演算部を備えたシミュレーション装置であって、
前記演算部は、
前記ロボットの関節の拘束点を基準とする第１の座標空間における拘束力ｆＡと、前記ロボットの関節空間における拘束力τＡとの関係を示す第１ヤコビアンＪＡを算出し、
前記ロボットの関節の接触点を基準とする第２の座標空間における接触力ｆＢと、前記ロボットの関節空間における接触力τＢとの関係を示す第２ヤコビアンＪＢを算出し、
前記第１ヤコビアンＪＡと第２ヤコビアンＪＢを合わせた第３ヤコビアンＪを算出し、
前記関節空間における、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力および遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとした、前記関節空間における前記ロボットの運動方程式
【数１】
2020075329.tif 000030
を前記第３ヤコビアンＪを用いて、前記第１の座標空間および前記第２の座標空間における加速度ｘ（・・）、質量行列Λ、コリオリ力／遠心力項をμ、重力項ｐ、発生力をｆａｃｔとした、第２の運動方程式
【数２】
2020075329.tif 000031
を求める変換処理をし、
所定の時間をΔｔ、前記物体の単位力あたりの変形変化を示す行列Ｗ、前記接触点の相対距離を示すδｆｒｅｅを用いて、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
【数３】
2020075329.tif 000032
を求める線形化処理をし、
前記線形化処理した運動方程式に基づく連立方程式を作成し、
前記連立方程式を解くことにより、前記拘束力ｆＡと、変形した物体の接触力を考慮した接触力ｆＢとを算出することを特徴とするシミュレーション装置。
【請求項２】
前記δｆｒｅｅは、剛体項と変形項からなり、
前記演算部は、前記第３の運動方程式を求める際に、行列サイズを変形する処理を行う請求項１に記載のシミュレーション装置。
【請求項３】
前記演算部は、
前記拘束力ｆＡ及び前記接触力ｆＢを前記第３のヤコビアンＪを用いて、前記拘束力τＡ及び前記接触力τＢを足し合わせた関節の入力トルクτに変換する処理を更に行うこと請求項１または２に記載のシミュレーション装置。
【請求項４】
前記ロボットの関節が、駆動源により駆動される第１のギヤと、前記第１のギヤに噛合する第２のギヤとを有し、前記拘束条件には、前記第１のギヤと前記第２のギヤとの速度伝達比が含まれていることを特徴とする請求項１乃至３のいずれか１項に記載のシミュレーション装置。
【請求項５】
請求項１乃至４のいずれか１項に記載のシミュレーション装置と、
前記シミュレーション装置によるシミュレーションにより作成した前記ロボットの動作プログラムに基づき、前記ロボットの動作を制御するロボット制御装置と、を備えた制御システム。
【請求項６】
前記ロボットと、
請求項５に記載の制御システムと、を備えたロボットシステム。
【請求項７】
演算部が、関節を有するロボットの動作をシミュレーションするのに用いる、前記ロボットと変形した前記物体の接触力及び前記ロボットの関節における拘束力を求めるシミュレーション方法であって、
前記演算部が、
前記ロボットの関節の拘束点を基準とする第１の座標空間における拘束力ｆＡと、前記ロボットの関節空間における拘束力τＡとの関係を示す第１ヤコビアンＪＡを算出する工程と、
前記ロボットの関節の接触点を基準とする第２の座標空間における接触力ｆＢと、前記ロボットの関節空間における接触力τＢとの関係を示す第２ヤコビアンＪＢを算出する工程と、
前記第１ヤコビアンＪＡと第２ヤコビアンＪＢを合わせた第３ヤコビアンＪを算出する工程と、
前記関節空間における、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力および遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとした、前記関節空間における前記ロボットの運動方程式
【数４】
2020075329.tif 000033
を前記第３ヤコビアンＪを用いて、前記第１の座標空間および前記第２の座標空間における加速度ｘ（・・）、質量行列Λ、コリオリ力／遠心力項をμ、重力項ｐ、発生力をｆａｃｔとした、第２の運動方程式
【数５】
2020075329.tif 000034
を求める変換処理をし、
所定の時間をΔｔ、前記物体の単位力あたりの変形変化を示す行列Ｗ、前記接触点の相対距離を示すδｆｒｅｅを用いて、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
【数６】
2020075329.tif 000035
を求める線形化処理をし、
前記線形化処理した運動方程式に基づく連立方程式を作成し、
前記連立方程式を解くことにより、前記拘束力ｆＡと、変形した物体の接触力を考慮した接触力ｆＢとを算出する工程と、
を有することを特徴とするシミュレーション方法。
【請求項８】
請求項７に記載のシミュレーション方法の各工程をコンピュータに実行させるためのプログラム。
【請求項９】
請求項８に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、変形する物体に対して拘束を有するロボットを用いて所定の動作を行う際のロボットのシミュレーションの技術に関する。
【背景技術】
【０００２】
近年、ロボット開発において、ロボットの動作を詳細にシミュレーションするために、様々な技術開発が行われている。拘束力の導入もその一つであり、パラレルリンクを用いたロボットのシミュレーションにおいて高速な演算が可能となっている。ここで、拘束力とは、ある物体に対して他の物体の動作を拘束する力をいう。拘束力の一例として、ギヤとギヤとが速度伝達比ＺＡ：ＺＢで伝達する力が挙げられる。
【０００３】
一方、ロボットは、ワークの搬送や組み立てなどの生産ラインにも適用されている。例えば、ギヤを駆動源としたエンドエフェクタで組立部品を保持し、被組立部品と組付けて物品を製造する。
【０００４】
このようなロボットの動作をシミュレーションする技術として、特許文献１には、拘束を有するロボットの動作をシミュレートする際の接触力と拘束力の計算方法が開示されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１８−０３０２１０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上述したようなロボットを用いた物品の製造工程において、組立部品の剛性が低いとエンドエフェクタの保持力により組立部品が変形することがある。特許文献１に記載の計算方法では、ワーク（組立部品および被組立部品）の変形が考慮されておらず、部品の変形を伴う動作をシミュレーションすることはできなかった。
【課題を解決するための手段】
【０００７】
上記課題を解決するためのシミュレーション装置は、関節を有するロボットが物体を保持する動作をシミュレーションするのに用いる、前記ロボットと変形した前記物体の接触力及び前記ロボットの関節における拘束力を求める演算部を備えたシミュレーション装置であって、
前記演算部は、
前記ロボットの関節の拘束点を基準とする第１の座標空間における拘束力ｆＡと、前記ロボットの関節空間における拘束力τＡとの関係を示す第１ヤコビアンＪＡを算出し、
前記ロボットの関節の接触点を基準とする第２の座標空間における接触力ｆＢと、前記ロボットの関節空間における接触力τＢとの関係を示す第２ヤコビアンＪＢを算出し、
前記第１ヤコビアンＪＡと第２ヤコビアンＪＢを合わせた第３ヤコビアンＪを算出し、
前記関節空間における、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力および遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとした、前記関節空間における前記ロボットの運動方程式
【０００８】
【数１】
2020075329.tif 000003
を前記第３ヤコビアンＪを用いて、前記第１の座標空間および前記第２の座標空間における加速度ｘ（・・）、質量行列Λ、コリオリ力／遠心力項をμ、重力項ｐ、発生力をｆａｃｔとした、第２の運動方程式
【０００９】
【数２】
2020075329.tif 000004
を求める変換処理をし、
所定の時間をΔｔ、前記物体の単位力あたりの変形変化を示す行列Ｗ、前記接触点の相対距離を示すδｆｒｅｅを用いて、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
【００１０】
【数３】
2020075329.tif 000005
を求める線形化処理をし、
前記線形化処理した運動方程式に基づく連立方程式を作成し、
前記連立方程式を解くことにより、前記拘束力ｆＡと、変形した物体の接触力を考慮した接触力ｆＢとを算出することを特徴とする。
【発明の効果】
【００１１】
本発明によれば、部品の変形を考慮した接触力を計算し、その接触力と拘束力とを用いてロボットの動作シミュレーションを行うことができる。そのため、ワークが変形するような動作においても正確にロボットの動作をシミュレーションすることができる。
【図面の簡単な説明】
【００１２】
【図１】実施形態に係るロボットシステムを示す概略図である。
【図２】ロボットアームの複数の関節のうちの１つの関節を示す部分断面図である。
【図３】実施形態に係る制御システムを示すブロック図である。
【図４】実施形態に係るシミュレーション方法を示すフローチャートである。
【図５】接触力及び拘束力の計算方法を示すフローチャートである。
【図６】実施例１におけるロボットの概略図である。
【発明を実施するための形態】
【００１３】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。図１は、実施形態に係るロボットシステムを示す説明図である。図１に示すように、ロボットシステム１００は、ロボット２００と、制御システム６００と、を備えている。
【００１４】
ロボット２００は、垂直多関節のロボットアーム２０１と、ロボットアーム２０１の先端に取り付けられたエンドエフェクタとしてのロボットハンド２０２と、を備えている。
【００１５】
ロボットアーム２０１の基端は、台座１５０に固定された固定端である。ロボットアーム２０１の先端は、自由端である。ロボットアーム２０１は、可動範囲の中であれば任意の３次元位置で任意の３方向の姿勢に、手先（ロボットアーム２０１の先端）を向けることができる。
【００１６】
ロボットアーム２０１は、複数の関節、例えば６つ関節Ｊ１〜Ｊ６を有している。ロボットアーム２０１は、各関節Ｊ１〜Ｊ６をそれぞれ回転駆動する複数（６つ）の駆動機構を有している。
【００１７】
ロボットアーム２０１は、複数のリンク２１００〜２１０６を有し、複数のリンク２１００〜２１０６が関節Ｊ１〜Ｊ６で回転可能に連結されている。ここで、基端側から先端側に向かって、リンク２１００〜２１０６が順に直列に連結されている。
【００１８】
図２は、ロボットアームの複数の関節のうちの１つの関節を示す部分断面図である。以下、関節Ｊ２を例に代表して説明し、他の関節Ｊ１，Ｊ３〜Ｊ６については、同様の構成であるため、説明を省略する。
【００１９】
関節Ｊ２を駆動する駆動機構２３０は、駆動源である電動モータ（以下、モータ）２３１と、モータ２３１の出力を減速する減速機２３３と、を有している。
【００２０】
モータ２３１は、サーボモータであり、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。モータ２３１の回転軸２３２の回転が減速機２３３で減速されて関節（リンク）を駆動する。例えば減速機２３３の減速比が５０：１であれば、モータ２３１が５０回転で減速機２３３の出力側に配置されているリンク２１０２が１回転する。また、リンク２１０１とリンク２１０２とは、クロスローラベアリング２３７を介して回転自在に結合されている。
【００２１】
減速機２３３は、本実施形態では波動歯車減速機である。減速機２３３は、モータ２３１の回転軸２３２に結合された、入力軸であるウェブジェネレータ２４１と、リンク２１０２に固定された、出力軸であるサーキュラスプライン（ギヤ）２４２と、を備えている。なお、サーキュラスプライン２４２は、リンク２１０２に直結されているが、リンク２１０２に一体に形成されていてもよい。
【００２２】
また、減速機２３３は、ウェブジェネレータ２４１とサーキュラスプライン２４２との間に配置され、リンク２１０１に固定されたフレクスプライン（ギヤ）２４３を備えている。フレクスプライン２４３は、ウェブジェネレータ２４１の回転に対して減速比Ｎで減速され、サーキュラスプライン２４２に対して相対的に回転する。従って、モータ２３１の回転は、減速機２３３で１／Ｎの減速比で減速されて、フレクスプライン２４３が固定されたリンク２１０１に対してサーキュラスプライン２４２が固定されたリンク２１０２を相対的に回転運動させ、関節Ｊ２を回転させる。
【００２３】
図３は、実施形態に係る制御システムを示すブロック図である。制御システム６００は、ロボット２００（図１）の動作を制御するロボット制御装置３００と、シミュレーションシステム５５０と、を有している。シミュレーションシステム５５０は、コンピュータで構成されたシミュレーション装置５００と、シミュレーション装置５００に接続された、キーボード５２１、マウス５２２等の操作入力部５２０、モニタ５２３及び外部記憶装置５２４と、を備えている。
【００２４】
シミュレーション装置５００は、オフライン教示によってユーザの操作に従ってロボット２００の動作プログラムを作成し、作成した動作プログラムに基づき、ロボット２００に対応する仮想ロボットを三次元仮想空間内で動作させるシミュレーションを行う。
【００２５】
シミュレーション装置５００は、演算部としてのＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）５０１を備えている。また、シミュレーション装置５００は、記憶部として、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）５０２、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）５０３、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）５０４を備えている。また、シミュレーション装置５００は、記録ディスクドライブ５０５及び複数のインタフェース５１１〜５１５を備えている。
【００２６】
ＣＰＵ５０１には、ＲＯＭ５０２、ＲＡＭ５０３、ＨＤＤ５０４、記録ディスクドライブ５０５及びインタフェース５１１〜５１５が、バス５１０を介して相互に通信可能に接続されている。
【００２７】
ＲＯＭ５０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ５０３は、ＣＰＵ５０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。ＨＤＤ５０４は、ＣＰＵ５０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ５０１に、後述する各種演算処理を実行させるためのプログラム５３１を記録するものである。ＣＰＵ５０１は、ＨＤＤ５０４に記録（格納）されたプログラム５３１に基づいてシミュレーション方法の各工程を実行する。
【００２８】
また、ＨＤＤ５０４には、ロボット言語（命令文）で記述された動作プログラムが格納される。更に、ＨＤＤ５０４には、ロボット２００を構成する部品の三次元データ（ＣＡＤデータ）、三次元有限要素法の計算式、パラメータ値等の各種情報が予め格納されている。
【００２９】
記録ディスクドライブ５０５は、記録媒体の一例である記録ディスク５３２に記録された各種データを読み出すことができる。
【００３０】
インタフェース５１１にはキーボード５２１が接続され、インタフェース５１２にはマウス５２２が接続されている。これらキーボード５２１及びマウス５２２からなる操作入力部５２０により、各種の操作入力を受付可能となっている。
【００３１】
インタフェース５１３には、表示部であるモニタ５２３が接続されており、モニタ５２３には、データ入力（編集）画像、仮想ロボットが配置された仮想三次元空間の表示画像などの各種画像が表示可能である。
【００３２】
インタフェース５１４には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置５２４が接続可能に構成されている。なお、本実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ５０４であり、ＨＤＤ５０４にプログラム５３１が格納される場合について説明するが、これに限定するものではない。プログラム５３１は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム５３１を供給するための記録媒体としては、図３に示すＲＯＭ５０２や、記録ディスク５３２、外部記憶装置等５２４を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ＨＤＤ、ＳＳＤ、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、磁気テープ、ＵＳＢメモリ等の不揮発性メモリ、ＲＯＭ等を用いることができる。また、記憶部としてＨＤＤ５０４の場合について説明するが、ＳＳＤや書き換え可能な不揮発性メモリであってもよい。
【００３３】
インタフェース５１５には、ロボット制御装置３００が接続されている。ロボット制御装置３００は、動作プログラムに基づき作成された軌道データをシミュレーション装置５００から取得し、軌道データに従ってロボット２００の動作を制御する。
【００３４】
ここで、ロボットアーム２０１の基端には、互いに直交する３軸ｘ０，ｙ０，ｚ０からなるワールド座標系Σ０が設定されている。また、ロボット２００の手先、つまりロボットハンド２０２には、ツールセンターポイント（ＴＣＰ）が設定される。ＴＣＰは、ロボットアーム２０１の基端に設定されたワールド座標系Σ０を基準に、位置を表す３つのパラメータと、姿勢を表す３つのパラメータで表される。このＴＣＰの目標値である教示点が、オフライン教示により、記憶部であるＨＤＤ５０４に格納される動作プログラムに記述される。
【００３５】
ＣＰＵ５０１は、動作プログラムに記述された補間方法に従い教示点間を補間し、逆運動学計算により各関節の値に変換したロボット２００（ロボットアーム２０１）の軌道データを生成する。教示点間を補間する補間方法としては、直線補間、円弧補間、関節補間、Ｓｐｌｉｎｅ補間、Ｂ−Ｓｐｌｉｎｅ補間、ベジェ曲線など、種々の方法がある。
【００３６】
ロボットアーム２０１の動作の制御として、位置制御とインピーダンス制御がある。位置制御では、ロボット２００の手先に設定したＴＣＰがワールド座標系Σ０の３軸ｘ０，ｙ０，ｚ０で目標位置（軌道）を辿るようにロボットアーム２０１の動作を制御する。一方、インピーダンス制御では、ロボット２００の手先に係る力（接触力）が所定値（より具体的には０）に近づくようにロボットアーム２０１の動作を制御する。よって、インピーダンス制御の設計段階におけるシミュレーションにおいて、接触力を計算する必要がある。接触力とは、物体と物体が接触した際に生じる反発力と摩擦力をいう。ここで、物体と物体が接触する際に、物体間の剛性に大きな差があると、一方の物体には変形が生じる。そのため、接触力には物体の変形に関する情報を反映することが求められる。
【００３７】
ところで、本実施形態のロボット２００は、拘束を有するロボットである。拘束を有するロボットの動作をシミュレーションする際には、接触力と拘束力の計算が必要である。ここで、拘束を有するロボットとは、ある動作軸とある動作軸との間に速度拘束（回転速度と回転速度との関係性）を有するものを指す。例えば、パラレルリンク機構を使ったものは拘束を有するロボットであるが、関節機構にモータ軸とリンク軸がギヤで速度拘束されているものについても、拘束を有するロボットである。拘束力とは、ある物体に対して他の物体の動作を拘束する力をいう。
【００３８】
本実施形態では、拘束を有するロボット２００がワーク（物体）と接触するときを想定し、ＣＰＵ５０１の演算処理により、ロボット２００にワークが接触する接触力及びロボット２００の関節における拘束力を求める。ここで、接触力はワークの変形を考慮したものである。
【００３９】
以下、オフライン教示におけるロボット２００のダイナミクスシミュレーションについて説明する。図４は、実施形態に係るシミュレーション方法を示すフローチャートである。
【００４０】
ＣＰＵ５０１は、ユーザの操作によりロボット２００の動作プログラムを作成する（Ｓ１）。ＣＰＵ５０１は、リンク２１００〜２１０６とワークの相対位置情報や、各リンク２１００〜２１０６およびワークの重さ、慣性モーメント、重心位置、速度、各リンクの初期の関節角度、ワークの初期変位などの情報を読み込むことで、初期設定を行う（Ｓ２）。
【００４１】
ここで、ＣＰＵ５０１は、ロボット２００の三次元データ（ＣＡＤデータ）に基づく仮想モデル（仮想ロボット）を仮想三次元空間に配置する。ＣＰＵ５０１は、この仮想ロボットが配置された仮想三次元空間をモニタ５２３に表示させる。
【００４２】
次に、ＣＰＵ５０１は、ロボット２００のバネ要素やダンパー要素などの受動要素のトルクを計算する（Ｓ３）。また、ＣＰＵ５０１は、モータの入力トルクτを計算する（Ｓ４）。即ち、これらステップＳ３，Ｓ４の計算により、力発生手段（モータ、バネ要素、ダンパー要素）の発生力としてのトルクが計算される。そして、ＣＰＵ３０１は、これらステップＳ３，Ｓ４の計算結果を用いて、ロボット２００のワークの変形を考慮した接触力及び拘束力を計算する（Ｓ５）。
【００４３】
次に、ＣＰＵ５０１は、ステップＳ３，Ｓ４，Ｓ５の計算結果を用いて順動力学計算を行い、関節の加速度を算出する（Ｓ６）。
【００４４】
ＣＰＵ５０１は、関節の加速度に基づき、関節の速度及び位置を計算してこれら関節の加速度、速度及び位置等からなる関節情報を更新する（Ｓ７）。続いて、３Ｄ画面情報（仮想ロボットの位置姿勢）を更新する。
【００４５】
次に、ＣＰＵ５０１は、ＴＣＰが終点に到達したか否かを判断し（Ｓ８）、終点に到達していなければ（Ｓ８：Ｎｏ）、予め決められた時間、仮想ロボットの動作を進め（Ｓ９）、ステップＳ３の処理に戻る。ＣＰＵ５０１は、ＴＣＰが終点に到達していれば（Ｓ８：Ｙｅｓ）、仮想ロボットを用いたロボット２００の動作のシミュレーションを終了する。
【００４６】
ユーザは、このシミュレーション結果に基づき動作プログラムを必要に応じて適宜修正する。ユーザは、再度シミュレーション装置５００にシミュレーションを行わせるなどして、動作プログラムを確定させる。動作プログラムが確定したら、ユーザの操作により、動作プログラムに基づく軌道データをロボット制御装置３００に送る。ロボット制御装置３００は、実際のロボット２００を、軌道データに従って動作させ、組み付け動作などをロボット２００に行わせる。このように、オフライン教示で軌道データを作成することで、オンライン教示に比べ、ロボット２００の教示作業が容易となる。
【００４７】
次に、ロボット２００の動作をシミュレーションするのに用いる、ステップＳ５のロボット２００に変形を伴うワークが接触する接触力及びロボット２００の関節における拘束力を求める処理について詳細に説明する。
【００４８】
ＣＰＵ５０１は、拘束空間（第１の座標空間）における拘束力ｆＡ（ｆｃｏｎｓｔｒａｉｎ）と、関節空間における拘束力τＡ（τｃｏｎｓｔｒａｉｎ）との関係を示す拘束ヤコビアン（第１ヤコビアン）ＪＡ（Ｊｃｏｎｓｔｒａｉｎ）を計算する（Ｓ５１）。拘束空間とは、ロボット２００の関節の拘束条件に基づく自由度を持ち、ロボット２００の拘束点の拘束方向を基準とする座標空間である。このとき、ＣＰＵ５０１は、以下の式（１）を満たすように、拘束ヤコビアンＪＡ（Ｊｃｏｎｓｔｒａｉｎ）を算出する。ここで、ｑ（・）は、関節空間におけるロボット２００の関節の速度である。
【００４９】
【数４】
2020075329.tif 000006
【００５０】
次に、ＣＰＵ５０１は、接触点の検出計算を行い（Ｓ５２）、接触点が存在するか否かを判断する（Ｓ５３）。ＣＰＵ５０１は、接触点が存在する場合、接触ヤコビアン（第２ヤコビアン）ＪＢ（Ｊｃｏｎｔａｃｔ）を計算する（Ｓ５４）。具体的に説明すると、ＣＰＵ５０１は、ロボット２００と物体との接触点のワールド座標系Σ０における座標値に基づき、接触ヤコビアンＪＢ（Ｊｃｏｎｔａｃｔ）を算出する。接触ヤコビアンＪＢ（Ｊｃｏｎｔａｃｔ）は、接触空間（第２の座標空間）における接触力ｆＢ（ｆｃｏｎｔａｃｔ）と、関節空間における接触力τＢ（τｃｏｎｔａｃｔ）との関係を示すヤコビアンである。接触空間は、ロボット２００の接触点の法線方向及び接線方向を基準とする三次元直交座標（デカルト座標）で表された座標空間である。
【００５１】
以下、具体的に説明する。なお、ｘｃｏｎｔａｃｔは接触空間における変位（接触点の相対位置）、ｘｃｏｎｓｔｒａｉｎは拘束空間における変位（拘束点の相対位置）とする。よって、ｘｃｏｎｔａｃｔ（・）は接触空間における速度、ｘｃｏｎｔａｃｔ（・・）は接触空間における加速度であり、ｘｃｏｎｓｔｒａｉｎ（・）は拘束空間における速度、ｘｃｏｎｓｔｒａｉｎ（・・）は拘束空間における加速度である。
【００５２】
部品ａの接触点ｘｃｏｎｔａｃｔａ，ｉと、部品ｂの接触点ｘｃｏｎｔａｃｔｂ，ｉとに接触が発生したとする。ｉは、接触点に付した番号である。このとき、それぞれの接触点の速度は以下のように表される。
【００５３】
【数５】
2020075329.tif 000007
【００５４】
ここで、右辺のＪｃｏｎｔａｃｔａ，ｉ及びＪｃｏｎｔａｃｔｂ，ｉは、ｉ番目の接触点に関するヤコビアンである。これを用いると、ｉ番目の接触における接触点の相対速度ｘ（・）ｃｏｎｔａｃｔｉは、以下の式（２）のようになる。
【００５５】
【数６】
2020075329.tif 000008
【００５６】
次に、ｉ番目の接触における法線ベクトルをｎｃｏｎｔａｃｔｉとし、接線ベクトルをｔｃｏｎｔａｃｔｘ，ｉとｔｃｏｎｔａｃｔｙ，ｉとする。これらの表記を用いて、ｉ番目の接触における法線方向速度と接線方向速度を表すヤコビアンＪｃｏｎｔａｃｔｎ，ｉ及びＪｃｏｎｔａｃｔｔ，ｉは以下の式（３），（４）のようになる。
【００５７】
【数７】
2020075329.tif 000009
【００５８】
各接触における法線方向ヤコビアンＪｃｏｎｔａｃｔｎ，ｉと接線方向ヤコビアンＪｃｏｎｔａｃｔｔ，ｉを積みあげることによって、すべての接触点に対する接触ヤコビアンＪｃｏｎｔａｃｔが以下の式（５）のように表現できる。
【００５９】
【数８】
2020075329.tif 000010
【００６０】
次に、ＣＰＵ５０１は、拘束ヤコビアンＪｃｏｎｓｔｒａｉｎ及び接触ヤコビアンＪｃｏｎｔａｃｔを合わせた接触・拘束ヤコビアン（第３ヤコビアン）Ｊを求める（Ｓ５５）。接触・拘束ヤコビアンＪは、以下の式（６）で表される。
【００６１】
【数９】
2020075329.tif 000011
【００６２】
以上で、ロボット２００に対する接触力と拘束力のモデル化が完了する。
【００６３】
そして、ＣＰＵ５０１は、部品変形を考慮した接触力と拘束力を連立した方程式を基に計算を行う。
【００６４】
まず、関節空間におけるロボット２００の運動方程式を計算する（Ｓ５６）。ここで、関節空間とは、ロボット２００の関節の数を自由度とする空間、即ち各関節位置（角度）を座標軸とする空間である。自由度とは、ある物理系の状態を記述するのに必要な変数の最小個数である。
【００６５】
関節空間において、ロボット２００の関節の変位をｑ、ロボット２００の関節の加速度をｑ（・・）、ロボット２００の質量に関する質量行列をＡ、ロボット２００に働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂとする。また、関節空間において、ロボット２００に働く重力に関する重力項をｇ、ロボット２００の関節に発生する発生力（即ち、モータトルクなどの能動要素のトルク、スプリング要素やダンパー要素などの受動要素のトルク）をτａｃｔとする。ここで、関数を時間で一次微分したものを（・）、関数を時間で二次微分したものを（・・）で表記する。
【００６６】
このとき、前記関節空間におけるロボット２００の第１の運動方程式は、以下の式（７）で表される。
【００６７】
【数１０】
2020075329.tif 000012
【００６８】
ここで、運動方程式である式（７）を計算するとは、式（７）における質量行列Ａ、コリオリ力／遠心力項ｂ、重力項ｇを計算することである。
【００６９】
次に、接触・拘束ヤコビアンＪを用いて以下の接触・拘束空間における第２の運動方程式に変換する。すると（７）式は（８）式のように変換できる。
【００７０】
【数１１】
2020075329.tif 000013
【００７１】
ただし、上式のパラメータは以下のとおりである。
【００７２】
【数１２】
2020075329.tif 000014
【００７３】
ここで、加速度ｘ（・・）は接触・拘束空間における加速度であり、ｆａｃｔは接触・拘束空間におけるロボットのモータ、スプリング、ダンパーなどの力発生手段による発生力である。また、質量行列Λは接触・拘束空間における質量行列、μは接触・拘束空間におけるコリオリ力／遠心力項、重力項ｐは接触・拘束空間における重力項である。
【００７４】
即ち、ＣＰＵ５０１は、ヤコビアンＪを用いて第１の運動方程式を第２の運動方程式に変換することで、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆａｃｔの各係数を求める。
【００７５】
次に、この運動方程式を、時間領域Δｔで積分することで線形化処理して、以下の第３の運動方程式を求める（Ｓ５７）。
【００７６】
【数１３】
2020075329.tif 000015
【００７７】
この式は、接触・拘束空間における、力と位置の関係を表している。式中、Λ−１Δｔ２は単位力による位置変化である。行列Ｗは単位力による変形変化を表わす行列であり、変形物体の接触点における剛性から計算する事ができる。
【００７８】
変形量を計算するための方法である有限要素法において、変形物体は複数の要素で構成されており、要素は節点で構成されている。例えば一般的に用いられる四面体要素では、一つの要素を４つの節点で構成している。接触点の剛性を計算する場合、Ｗは節点同士の剛性と、接触点とその近傍の節点からの距離の比を入力して以下の式で計算を行う。
【００７９】
【数１４】
2020075329.tif 000016
【００８０】
ここでＨは接触点とその近傍の節点の距離の比であり、Ｃは各節点の剛性の逆数を表している。また、ｉは接触点近傍の節点の数である。また、接触は２物体間で発生するためペア（以下、接触点ペア）を構成しており、接触点ペア毎に剛性を考慮する必要がある。式中のＡとＢは接触点ペアにおける、Ａ側接触点とＢ側接触点を表している。なお、変形が発生しない剛体部品のＷは０となる。
【００８１】
ここで物体が接触力によって変形する場合を考えた場合、Λ行列のサイズ（「接触点ペア数＋拘束数」×「接触点ペア数＋拘束数」）と、Ｗ行列のサイズ（「接触点ペア数」×「接触点ペア数」）が異なるため、（１２）式の右辺第１項を計算することができない。そこで行列サイズを合わせるためにΛ行列と同じサイズの行列を作成し、接触点に関わる部分にＷ行列を配置する必要がある。
【００８２】
δｆｒｅｅは、接触・拘束空間における、１ステップ後の接触点ペアまたは拘束点ペア同士の相対距離を表しており、剛体項と変形項に分けることができる。
【００８３】
【数１５】
2020075329.tif 000017
【００８４】
（１４）式の右辺第１項は剛体であり以下のように計算できる。
【００８５】
【数１６】
2020075329.tif 000018
【００８６】
また、（１４）式の右辺第２項は変形項であり、外力を０と仮定して変形シミュレーションを１ステップ進めることで計算することができる。
【００８７】
ここで物体が接触力によって変形する場合を考えた場合、剛体項の行列のサイズ（「接触点ペア数＋拘束数」×１）と、変形項の行列のサイズ（「接触点ペア数」×１）が異なるため、（１４）式を計算することができない。そこで行列サイズを合わせるために剛体項と同じサイズの行列を作成し、接触点に関わる部分に変形項の行列を配置する必要がある。
【００８８】
以上の計算結果より、接触・拘束ヤコビアンを用いて線形化した、接触・拘束空間における運動方程式を得る事ができる。つまり、線形化した第３の運動方程式において、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆａｃｔの各係数は既知の値であり、ｆｃｏｎｓｔｒａｉｎ，ｆｃｏｎｔａｃｔが未知数である。第３の運動方程式にこれら係数を適用すれば、第２の運動方程式を第３の運動方程式に変換したことになる。ここで、Δｔは接触力と拘束力が働く所定の時間である。この運動方程式は、拘束力ｆＡ（ｆｃｏｎｓｔｒａｉｎ）と接触力ｆＢ（ｆｃｏｎｔａｃｔ）を未知数とした連立方程式である。この方程式を用いて、接触・拘束空間における接触力と拘束力から関節空間における接触力と拘束力を計算する（Ｓ５８）。
【００８９】
ここで、接触・拘束空間における接触力と拘束力を連立させて計算する際には、下記の制約関係を満たすように計算を行う。
【００９０】
接触時に発生する接触空間における接触力の法線方向成分は、物体同士が離れる方向に発生する。そのため以下の条件が発生する。
【００９１】
【数１７】
2020075329.tif 000019
【００９２】
接触時に発生する接触空間における相対位置の変化は、物体同士が離れる方向に発生する。そのため以下の条件が発生する。
【００９３】
【数１８】
2020075329.tif 000020
【００９４】
接触時に発生する接触空間における接触力の法線方向成分と、接触ペア間の相対位置の変化の関係は、接触力が発生した場合には相対位置の近接方向への位置変化はなくなる。そのため、接触ペア間が閾値距離以内であっても相対位置が広がっているならば接触力は発生しない。そのため以下の条件が発生する。
【００９５】
【数１９】
2020075329.tif 000021
【００９６】
接触時に発生する接触空間における接触力の接線方向成分は、法線方向接触力と摩擦係数を乗じた最大摩擦力以内となる。そのため以下の条件が発生する。
【００９７】
【数２０】
2020075329.tif 000022
【００９８】
また、拘束力によって発生する拘束空間における相対位置の変化は、常に０となる。そのため以下の条件が発生する。
【００９９】
【数２１】
2020075329.tif 000023
【０１００】
以上のように、接触力に関する制約は、接触力の物理的な関係性であり、これらを用いて線形相補性問題に帰着する事ができる。また、拘束力に関する制約は拘束力の物理的な関係性を表している。
【０１０１】
次に、部品の変形を有限要素法で計算する（Ｓ５９）。前記計算した接触力を入力し、有限要素法を用いて部品の変形を計算する。有限要素法については公知の一般的な手法を用いることができる。
【０１０２】
また、関節空間における拘束力τｃｏｎｓｔｒａｉｎ及び接触力τｃｏｎｔａｃｔを足し合わせた関節への入力トルクをτとする。ＣＰＵ５０１は、拘束力ｆｃｏｎｓｔｒａｉｎ及び接触力ｆｃｏｎｔａｃｔを接触・拘束ヤコビアンＪを用いて、入力トルクτに変換する。
【０１０３】
そしてＳ６に戻り、ダイナミクスシミュレーションを実行する。前記計算した接触力と拘束力を入力とし、順動力学計算を行う事でロボットや部品の加速度を計算する。次に、積分計算を行う事でロボットや部品の速度や位置について計算する。
【０１０４】
以上、拘束を有するロボット２００の、変形部品との接触を伴うシミュレーションを実行できる。
【実施例】
【０１０５】
（実施例１）
図６は実施例１におけるロボットハンドを示す模式図である。ダイナミクスシミュレーション方法について、図６に示すような軸間に拘束を有するロボットについて変形を考慮した接触力及び拘束力を計算する具体例を下記に示す。
【０１０６】
図６に示すように、駆動源であるモータに駆動されるギヤ３１が、ロボットハンド５０の関節であるリンク３２，３３に接続されている。そのためギヤ３１の回転に伴い、ギヤ部を有するリンク３２，３３はｙ０方向に移動する。
【０１０７】
実施例１ではギヤ３１とリンク３２，３３のギヤ部との変位が、ある一定の比率で動作するという拘束を有している。即ち、拘束条件には、ギヤ３１とリンク３２，３３のギヤ部との速度伝達比（ギヤ比）が含まれている。拘束を有するロボットのリンク３２，３３に、物体３４と接触したときの接触力を求める。なお物体３４は、ロボットハンドで把持する、変形する部品である。
【０１０８】
まず、第１ヤコビアンである拘束ヤコビアンを導出する。
【０１０９】
本実施例では、ギヤで接続されたモータとリンクを模しており、ｑ１は入力軸を、ｑ２，ｑ３は出力軸を模しており、以下のような比率と角度伝達誤差で表されるギヤで接続されている。
【０１１０】
【数２２】
2020075329.tif 000024
【０１１１】
【数２３】
2020075329.tif 000025
【０１１２】
ここで、Ａはギヤ比である。
【０１１３】
次に、上記の入力軸と出力軸の関係について微分し、速度の関係式に変換する。
【０１１４】
【数２４】
2020075329.tif 000026
【０１１５】
【数２５】
2020075329.tif 000027
【０１１６】
さらに（２１）〜（２４）式をまとめて以下のような式で拘束ヤコビアンを算出する。
【０１１７】
【数２６】
2020075329.tif 000028
【０１１８】
以上が、ロボットに係る機構を拘束力でモデル化する工程である。
【０１１９】
次に、接触点検出計算を行い、第２ヤコビアンである接触ヤコビアンを計算する。
【０１２０】
接触点検出は、部品の変形を考慮して行う必要がある。つまり、部品を構成する有限要素の表面が接触点になるため、部品をメッシュ分割する。部品をメッシュ分割し、接触点を算出したら、発明を実施するための形態に記載の方法で（５）式の接触ヤコビアンの算出を行う。
【０１２１】
以上が、部品変形を考慮した接触点を計算する工程である。
【０１２２】
次に、前記接触・拘束ヤコビアンを用いて線形化した、接触・拘束空間における運動方程式における、ΛとＷとδｆｒｅｅについて計算する。
【０１２３】
【数２７】
2020075329.tif 000029
【０１２４】
このようにして算出した接触・拘束空間における運動方程式を、接触力と拘束力を表すｆについて解く。こうすることで、部品変形を考慮した接触力と拘束力を連立させて計算することができる。
【０１２５】
発明を実施するための形態に記載の方法によって計算される拘束力は、ロボットに係る機構モデルが成り立つのに必要な力が計算される。また、接触力は、事前に設定した部品の摩擦力を反映させた値が計算される（図６（ｃ））。
【０１２６】
以上が、部品変形を考慮した接触力と拘束力を連立させて計算する工程である。
【０１２７】
次に、この接触力と拘束力などの外力を入力として、部品の変形を有限要素法で計算する。以上が部品の変形を有限要素法で計算する工程である（図６（ｄ））。
【０１２８】
最後に、部品の加速度を順動力学計算し、部品の位置を積分計算する。
【０１２９】
以上説明したとおり、図６に示すような軸間に拘束を有するロボットハンドが変形物体を把持するシミュレーションにおいて、接触力と拘束力を連立させて計算する事ができる。そのため、変形物体の接触力と拘束力を連立させたシミュレーションを行う事ができるという効果がある。
【０１３０】
また、本実施例のようにギヤの動作を拘束力によって計算した場合の時間と、接触力を用いた場合の計算時間を比較した場合、接触力計算では接触点が複数出る可能性に加え、線形相補性を考慮しなければならない分、計算時間が多い。また、ギヤの数が増えるほどその影響は大きくなる。そのため、拘束を有するロボットアームの接触問題をシミュレートする場合は、本実施例のように接触力と拘束力を連立させて計算する方法が、計算時間が少ないため有効である。
【０１３１】
本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、実施形態に記載されたものに限定されない。
【０１３２】
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【０１３３】
また、上述の実施形態では、ロボットアームが垂直多関節のロボットアームの場合について説明したが、これに限定するものではない。ロボットアームが、例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい。
【０１３４】
また、上述の実施形態では、シミュレーション装置とロボット制御装置とが別のコンピュータで構成される場合について説明したが、これに限定するものではなく、シミュレーション装置とロボット制御装置とが１つのコンピュータで構成されていてもよい。
【符号の説明】
【０１３５】
１００  ロボットシステム
２００  ロボット
３００  ロボット制御装置
５００  シミュレーション装置
５０１  ＣＰＵ（演算部）
６００  制御システム
【図１】
2020075329.tif 000036
【図２】
2020075329.tif 000037
【図３】
2020075329.tif 000038
【図４】
2020075329.tif 000039
【図５】
2020075329.tif 000040
【図６】
2020075329.tif 000041