(57)【要約】【課題】ロボットの接触力と拘束力とを共に計算し、計算負荷を低減する。【解決手段】ロボットシステム１００におけるシミュレーションシステム５５０のコンピュータにより、拘束を有するロボット２００の動作のシミュレーションを行うのに用いる接触力及び拘束力を計算する。この計算方法は、まず、接触・拘束空間と関節空間との関係を示すヤコビアンを求める。このヤコビアンを用いてロボット２００の運動方程式を接触・拘束空間に変換する。更に変換した運動方程式を線形化して、これにより得られる連立方程式を解くことで、接触力及び拘束力を同時に求める。【選択図】図１
2018030210.tif 000002
【特許請求の範囲】
【請求項１】
関節を有するロボットの動作をシミュレーションするのに用いる、前記ロボットに物体が接触する接触力及び前記ロボットの関節における拘束力を求める演算部を備えたシミュレーション装置であって、
前記演算部は、
前記ロボットの関節の数を自由度とする関節空間において前記ロボットの関節の速度をｑ（・）としたとき、前記ロボットの関節の拘束条件に基づく自由度を持ち、前記ロボットの拘束点の拘束方向を基準とする第１の座標空間における拘束力ｆＡと、前記関節空間における拘束力τＡとの関係を示す第１のヤコビアンＪＡを、
【数１】
2018030210.tif 000089
を満たすように算出する第１のヤコビアン算出処理と、
前記ロボットと前記物体との接触点のワールド座標系における座標値に基づき、前記ロボットの接触点の法線方向及び接線方向を基準とする直交座標で表された第２の座標空間における接触力ｆＢと、前記関節空間における接触力τＢとの関係を示す第２のヤコビアンＪＢを算出する第２のヤコビアン算出処理と、
前記第１のヤコビアンＪＡ及び前記第２のヤコビアンＪＢを合わせた第３のヤコビアンＪを求める第３のヤコビアン算出処理と、
前記関節空間において、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとし、前記関節空間における前記ロボットの第１の運動方程式
【数２】
2018030210.tif 000090
を前記第３のヤコビアンＪを用いて変換することで、前記第１の座標空間及び前記第２の座標空間における、加速度ｘ（・・）、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、発生力ｆａｃｔとなる前記ロボットの第２の運動方程式
【数３】
2018030210.tif 000091
を求める変換処理と、
所定の時間をΔｔとし、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
【数４】
2018030210.tif 000092
を求める線形化処理と、
前記線形化した前記第３の運動方程式を前記拘束力ｆＡ及び前記接触力ｆＢを未知数とする連立方程式とし、前記連立方程式を解くことにより前記拘束力ｆＡ及び前記接触力ｆＢを算出する力算出処理と、を実行することを特徴とするシミュレーション装置。
【請求項２】
前記演算部は、
前記拘束力ｆＡ及び前記接触力ｆＢを前記第３のヤコビアンＪを用いて、前記拘束力τＡ及び前記接触力τＢを足し合わせた関節の入力トルクτに変換する力変換処理を更に実行することを特徴とする請求項１に記載のシミュレーション装置。
【請求項３】
前記ロボットの関節が、駆動源により駆動される第１のギヤと、前記第１のギヤに噛合する第２のギヤとを有し、
前記拘束条件には、前記第１のギヤと前記第２のギヤとの速度伝達比が含まれていることを特徴とする請求項１又は２に記載のシミュレーション装置。
【請求項４】
前記拘束条件には、前記第１のギヤと前記第２のギヤとの間の伝達誤差が含まれていることを特徴とする請求項３に記載のシミュレーション装置。
【請求項５】
請求項１乃至４のいずれか１項に記載のシミュレーション装置と、
前記シミュレーション装置によるシミュレーションにより作成した前記ロボットの動作プログラムに基づき、前記ロボットの動作を制御するロボット制御装置と、を備えた制御システム。
【請求項６】
前記ロボットと、
請求項５に記載の制御システムと、を備えたロボットシステム。
【請求項７】
演算部が、関節を有するロボットの動作をシミュレーションするのに用いる、前記ロボットに物体が接触する接触力及び前記ロボットの関節における拘束力を求めるシミュレーション方法であって、
前記演算部が、前記ロボットの関節の数を自由度とする関節空間において前記ロボットの関節の速度をｑ（・）としたとき、前記ロボットの関節の拘束条件に基づく自由度とする第１の座標空間における拘束力ｆＡと、前記関節空間における拘束力τＡとの関係を示す第１のヤコビアンＪＡを、
【数５】
2018030210.tif 000093
を満たすように算出する第１のヤコビアン算出工程と、
前記演算部が、前記ロボットと前記物体との接触点のワールド座標系における座標値に基づき、前記ロボットの接触点の法線方向及び接線方向を基準とする直交座標で表された第２の座標空間における接触力ｆＢと、前記関節空間における接触力τＢとの関係を示す第２のヤコビアンＪＢを算出する第２のヤコビアン算出工程と、
前記演算部が、前記第１のヤコビアンＪＡ及び前記第２のヤコビアンＪＢを合わせた第３のヤコビアンＪを求める第３のヤコビアン算出工程と、
前記関節空間において、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとし、前記関節空間における前記ロボットの第１の運動方程式
【数６】
2018030210.tif 000094
を前記第３のヤコビアンＪを用いて変換することで、前記第１の座標空間及び前記第２の座標空間における、加速度ｘ（・・）、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、発生力ｆａｃｔとなる前記ロボットの第２の運動方程式
【数７】
2018030210.tif 000095
を求める変換工程と、
前記演算部が、所定の時間をΔｔとし、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
【数８】
2018030210.tif 000096
を求める線形化工程と、
前記演算部が、前記線形化した前記第３の運動方程式を前記拘束力ｆＡ及び前記接触力ｆＢを未知数とする連立方程式とし、前記連立方程式を解くことにより前記拘束力ｆＡ及び前記接触力ｆＢを算出する力算出工程と、を備えたことを特徴とするシミュレーション方法。
【請求項８】
コンピュータに請求項７に記載のシミュレーション方法の各工程を実行させるためのプログラム。
【請求項９】
請求項８に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、拘束を有するロボットのシミュレーションの技術に関する。
【背景技術】
【０００２】
近年、ロボット開発において、ロボットの動作を詳細にシミュレーションするために、様々な技術開発が行われている。拘束力の導入もその一つであり、パラレルリンクを用いたロボットのシミュレーションにおいて高速な演算が可能となっている。
【０００３】
ここで、拘束力とは、ある物体に対して他の物体の動作を拘束する力をいう。拘束力の一例として、ギヤとギヤとが速度伝達比ＺＡ：ＺＢで伝達する力が挙げられる。また、拘束力の他の例として、物体上の任意点の自由度を、別の物体の座標系に拘束する力が挙げられる。この他の例では、例えば物体上の任意点の並進自由度を、別の物体に拘束する拘束力を計算することで、物体と別の物体を球面軸で繋いだような動作をシミュレーションすることができ、閉リンクロボットのシミュレーションが可能となる。
【０００４】
一方、ロボットは、ワークの搬送や組み立てなどの生産ラインにも適用されている。また、ギヤやパラレルリンク機構などのロボットを、接触力のみを用いてシミュレーションする方法もある。このようなロボットの動作をシミュレーションするには、接触力の計算が必要となる。接触力の計算方式に関しては、従来はモデル同士の侵入量から接触力を疑似的に計算する方式が主流であったが、現在は解析的でより現実に即した計算方式が提案されている。
【０００５】
ロボットの接触力の計算方法の従来例として非特許文献１が知られている。非特許文献１では、ロボットに作用する接触力を、コリオリ力、遠心力項などの影響を考慮して算出する方法について記載されている。まず、接触点におけるロボットの運動方程式を導出し、次に、接触点が満たすべき条件を考慮して接触力を算出している。ここで、満たすべき条件とは、接触点における接触力の向き、接触点における速度、法線方向の接触力と接線方向の接触力との関係などである。また、接触力を算出する方法として、ガウスサイデル法に似た繰り返し計算を用いて、満たすべき条件に合致した接触力を計算している。
【先行技術文献】
【非特許文献】
【０００６】
【非特許文献１】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
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上記非特許文献１に記載の方法では、ロボットの接触力を求める際に、関節における拘束力については考慮されていなかった。そのため、拘束（具体的には速度拘束）を有するロボットがワークと接触するような場合のシミュレーションを行うことができなかった。
【０００８】
また、パラレルリンク機構などの拘束を有するロボットを、接触力を用いてシミュレーションする方法では、ロボットの拘束機構についても接触点の接触力について連立方程式を立てる必要がある。拘束機構については多数の接触点が存在するため、接触点の数が増えるほど連立方程式の数が増えることになり、拘束を有するロボットについて接触点のみでシミュレーションを行う場合、計算負荷が高く、多くの計算時間が必要であった。
【０００９】
そこで、本発明は、ロボットの接触力と拘束力とを共に計算し、計算負荷を低減することを目的とする。
【課題を解決するための手段】
【００１０】
関節を有するロボットの動作をシミュレーションするのに用いる、前記ロボットに物体が接触する接触力及び前記ロボットの関節における拘束力を求める演算部を備えたシミュレーション装置であって、
前記演算部は、
前記ロボットの関節の数を自由度とする関節空間において前記ロボットの関節の速度をｑ（・）としたとき、前記ロボットの関節の拘束条件に基づく自由度を持ち、前記ロボットの拘束点の拘束方向を基準とする第１の座標空間における拘束力ｆＡと、前記関節空間における拘束力τＡとの関係を示す第１のヤコビアンＪＡを、
【数１】
2018030210.tif 000003
を満たすように算出する第１のヤコビアン算出処理と、
前記ロボットと前記物体との接触点のワールド座標系における座標値に基づき、前記ロボットの接触点の法線方向及び接線方向を基準とする直交座標で表された第２の座標空間における接触力ｆＢと、前記関節空間における接触力τＢとの関係を示す第２のヤコビアンＪＢを算出する第２のヤコビアン算出処理と、
前記第１のヤコビアンＪＡ及び前記第２のヤコビアンＪＢを合わせた第３のヤコビアンＪを求める第３のヤコビアン算出処理と、
前記関節空間において、前記ロボットの関節の加速度をｑ（・・）、前記ロボットの質量に関する質量行列をＡ、前記ロボットに働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂ、前記ロボットに働く重力に関する重力項をｇ、前記ロボットの関節に発生する発生力をτａｃｔとし、前記関節空間における前記ロボットの第１の運動方程式
【数２】
2018030210.tif 000004
を前記第３のヤコビアンＪを用いて変換することで、前記第１の座標空間及び前記第２の座標空間における、加速度ｘ（・・）、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、発生力ｆａｃｔとなる前記ロボットの第２の運動方程式
【数３】
2018030210.tif 000005
を求める変換処理と、
所定の時間をΔｔとし、前記第２の運動方程式を時間領域で積分して線形化することにより第３の運動方程式
【数４】
2018030210.tif 000006
を求める線形化処理と、
前記線形化した前記第３の運動方程式を前記拘束力ｆＡ及び前記接触力ｆＢを未知数とする連立方程式とし、前記連立方程式を解くことにより前記拘束力ｆＡ及び前記接触力ｆＢを算出する力算出処理と、を実行することを特徴とする。
【発明の効果】
【００１１】
本発明によれば、ロボットが物体と接触する場面をシミュレーションする場合、拘束機構である関節については拘束力でモデル化することで、拘束機構である関節を接触力でモデル化する場合と比べて運動方程式で求めるべき力の数が少なくなる。したがって、演算負荷が低減し、高速なシミュレーションが可能となる。
【図面の簡単な説明】
【００１２】
【図１】実施形態に係るロボットシステムを示す説明図である。
【図２】ロボットアームの複数の関節のうちの１つの関節を示す部分断面図である。
【図３】実施形態に係る制御システムを示すブロック図である。
【図４】実施形態に係るシミュレーション方法を示すフローチャートである。
【図５】接触力及び拘束力の計算方法を示すフローチャートである。
【図６】実施例１におけるロボットを示す模式図である。
【図７】（ａ）は、実施例３におけるロボットを示す模式図である。（ｂ）は（ａ）に示すロボットの１つの回転ジョイントを、互いに拘束関係にある２つの拘束点とした模式図である。（ｃ）は、実施例３においてロボットに物体が接触した状態を示す模式図である。
【発明を実施するための形態】
【００１３】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。図１は、実施形態に係るロボットシステムを示す説明図である。図１に示すように、ロボットシステム１００は、ロボット２００と、制御システム６００と、を備えている。
【００１４】
ロボット２００は、垂直多関節のロボットアーム２０１と、ロボットアーム２０１の先端に取り付けられたエンドエフェクタとしてのロボットハンド２０２と、を備えている。
【００１５】
ロボットアーム２０１の基端は、台座１５０に固定された固定端である。ロボットアーム２０１の先端は、自由端である。ロボットアーム２０１は、可動範囲の中であれば任意の３次元位置で任意の３方向の姿勢に、手先（ロボットアーム２０１の先端）を向けることができる。
【００１６】
ロボットアーム２０１は、複数の関節、例えば６つ関節Ｊ１〜Ｊ６を有している。ロボットアーム２０１は、各関節Ｊ１〜Ｊ６をそれぞれ回転駆動する複数（６つ）の駆動機構を有している。
【００１７】
ロボットアーム２０１は、複数のリンク２１００〜２１０６を有し、複数のリンク２１００〜２１０６が関節Ｊ１〜Ｊ６で回転可能に連結されている。ここで、基端側から先端側に向かって、リンク２１００〜２１０６が順に直列に連結されている。
【００１８】
図２は、ロボットアームの複数の関節のうちの１つの関節を示す部分断面図である。以下、関節Ｊ２を例に代表して説明し、他の関節Ｊ１，Ｊ３〜Ｊ６については、同様の構成であるため、説明を省略する。
【００１９】
関節Ｊ２を駆動する駆動機構２３０は、駆動源である電動モータ（以下、「モータ」という）２３１と、モータ２３１の出力を減速する減速機２３３と、を有している。
【００２０】
モータ２３１は、サーボモータであり、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。モータ２３１の回転軸２３２の回転が減速機２３３で減速されて関節（リンク）を駆動する。例えば減速機２３３の減速比が５０：１であれば、モータ２３１が５０回転で減速機２３３の出力側に配置されているリンク２１０２が１回転する。また、リンク２１０１とリンク２１０２とは、クロスローラベアリング２３７を介して回転自在に結合されている。
【００２１】
減速機２３３は、本実施形態では波動歯車減速機である。減速機２３３は、モータ２３１の回転軸２３２に結合された、入力軸であるウェブジェネレータ２４１と、リンク２１０２に固定された、出力軸であるサーキュラスプライン（ギヤ）２４２と、を備えている。なお、サーキュラスプライン２４２は、リンク２１０２に直結されているが、リンク２１０２に一体に形成されていてもよい。
【００２２】
また、減速機２３３は、ウェブジェネレータ２４１とサーキュラスプライン２４２との間に配置され、リンク２１０１に固定されたフレクスプライン（ギヤ）２４３を備えている。フレクスプライン２４３は、ウェブジェネレータ２４１の回転に対して減速比Ｎで減速され、サーキュラスプライン２４２に対して相対的に回転する。従って、モータ２３１の回転は、減速機２３３で１／Ｎの減速比で減速されて、フレクスプライン２４３が固定されたリンク２１０１に対してサーキュラスプライン２４２が固定されたリンク２１０２を相対的に回転運動させ、関節Ｊ２を回転させる。
【００２３】
図３は、実施形態に係る制御システムを示すブロック図である。制御システム６００は、ロボット２００（図１）の動作を制御するロボット制御装置３００と、シミュレーションシステム５５０と、を有している。シミュレーションシステム５５０は、コンピュータで構成されたシミュレーション装置５００と、シミュレーション装置５００に接続された、キーボード５２１、マウス５２２等の操作入力部５２０、モニタ５２３及び外部記憶装置５２４と、を備えている。
【００２４】
シミュレーション装置５００は、オフライン教示によってユーザの操作に従ってロボット２００の動作プログラムを作成し、作成した動作プログラムに基づき、ロボット２００に対応する仮想ロボットを三次元仮想空間内で動作させるシミュレーションを行う。
【００２５】
シミュレーション装置５００は、演算部としてのＣＰＵ（Central  Processing  Unit）５０１を備えている。また、シミュレーション装置５００は、記憶部として、ＲＯＭ（Read  Only  Memory）５０２、ＲＡＭ（Random  Access  Memory）５０３、ＨＤＤ（Hard  Disk  Drive）５０４を備えている。また、シミュレーション装置５００は、記録ディスクドライブ５０５及び複数のインタフェース５１１〜５１５を備えている。
【００２６】
ＣＰＵ５０１には、ＲＯＭ５０２、ＲＡＭ５０３、ＨＤＤ５０４、記録ディスクドライブ５０５及びインタフェース５１１〜５１５が、バス５１０を介して相互に通信可能に接続されている。
【００２７】
ＲＯＭ５０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ５０３は、ＣＰＵ５０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。ＨＤＤ５０４は、ＣＰＵ５０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ５０１に、後述する各種演算処理を実行させるためのプログラム５３１を記録するものである。ＣＰＵ５０１は、ＨＤＤ５０４に記録（格納）されたプログラム５３１に基づいてシミュレーション方法の各工程を実行する。
【００２８】
また、ＨＤＤ５０４には、ロボット言語（命令文）で記述された動作プログラムが格納される。更に、ＨＤＤ５０４には、ロボット２００を構成する部品の三次元データ（ＣＡＤデータ）、三次元有限要素法の計算式、パラメータ値等の各種情報が予め格納されている。
【００２９】
記録ディスクドライブ５０５は、記録媒体の一例である記録ディスク５３２に記録された各種データを読み出すことができる。
【００３０】
インタフェース５１１にはキーボード５２１が接続され、インタフェース５１２にはマウス５２２が接続されている。これらキーボード５２１及びマウス５２２からなる操作入力部５２０により、各種の操作入力を受付可能となっている。
【００３１】
インタフェース５１３には、表示部であるモニタ５２３が接続されており、モニタ５２３には、データ入力（編集）画像、仮想ロボットが配置された仮想三次元空間の表示画像などの各種画像が表示可能である。
【００３２】
インタフェース５１４には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の外部記憶装置５２４が接続可能に構成されている。なお、本実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ５０４であり、ＨＤＤ５０４にプログラム５３１が格納される場合について説明するが、これに限定するものではない。プログラム５３１は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム５３１を供給するための記録媒体としては、図３に示すＲＯＭ５０２や、記録ディスク５３２、外部記憶装置等５２４を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ＨＤＤ、ＳＳＤ、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、磁気テープ、ＵＳＢメモリ等の不揮発性メモリ、ＲＯＭ等を用いることができる。また、記憶部としてＨＤＤ５０４の場合について説明するが、ＳＳＤや書き換え可能な不揮発性メモリであってもよい。
【００３３】
インタフェース５１５には、ロボット制御装置３００が接続されている。ロボット制御装置３００は、動作プログラムに基づき作成された軌道データをシミュレーション装置５００から取得し、軌道データに従ってロボット２００の動作を制御する。
【００３４】
ここで、ロボットアーム２０１の基端には、互いに直交する３軸ｘ０，ｙ０，ｚ０からなるワールド座標系Σ０が設定されている。また、ロボット２００の手先、つまりロボットハンド２０２には、ツールセンターポイント（ＴＣＰ）が設定される。ＴＣＰは、ロボットアーム２０１の基端に設定されたワールド座標系Σ０を基準に、位置を表す３つのパラメータと、姿勢を表す３つのパラメータで表される。このＴＣＰの目標値である教示点が、オフライン教示により、記憶部であるＨＤＤ５０４に格納される動作プログラムに記述される。
【００３５】
ＣＰＵ５０１は、動作プログラムに記述された補間方法に従い教示点間を補間し、逆運動学計算により各関節の値に変換したロボット２００（ロボットアーム２０１）の軌道データを生成する。教示点間を補間する補間方法としては、直線補間、円弧補間、関節補間、Ｓｐｌｉｎｅ補間、Ｂ−Ｓｐｌｉｎｅ補間、ベジェ曲線など、種々の方法がある。
【００３６】
ロボットアーム２０１の動作の制御として、位置制御とインピーダンス制御がある。位置制御では、ロボット２００の手先に設定したＴＣＰがワールド座標系Σ０の３軸ｘ０，ｙ０，ｚ０で目標位置（軌道）を辿るようにロボットアーム２０１の動作を制御する。一方、インピーダンス制御では、ロボット２００の手先に係る力（接触力）が所定値（より具体的には０）に近づくようにロボットアーム２０１の動作を制御する。よって、インピーダンス制御の設計段階におけるシミュレーションにおいて、接触力を計算する必要がある。接触力とは、物体と物体が接触した際に生じる反発力と摩擦力をいう。
【００３７】
ところで、本実施形態のロボット２００は、拘束を有するロボットである。拘束を有するロボットの動作をシミュレーションする際には、接触力と拘束力の計算が必要である。ここで、拘束を有するロボットとは、ある動作軸とある動作軸との間に速度拘束（回転速度と回転速度との関係性）を有するものを指す。例えば、パラレルリンク機構を使ったものは拘束を有するロボットであるが、関節機構にモータ軸とリンク軸がギヤで速度拘束されているものについても、拘束を有するロボットである。拘束力とは、ある物体に対して他の物体の動作を拘束する力をいう。
【００３８】
本実施形態では、拘束を有するロボット２００がワーク（物体）と接触するときを想定し、ＣＰＵ５０１の演算処理により、ロボット２００にワークが接触する接触力及びロボット２００の関節における拘束力を求める。
【００３９】
以下、オフライン教示におけるロボット２００のダイナミクスシミュレーションについて説明する。図４は、実施形態に係るシミュレーション方法を示すフローチャートである。
【００４０】
ＣＰＵ５０１は、ユーザの操作によりロボット２００の動作プログラムを作成する（Ｓ１）。ＣＰＵ５０１は、ロボット２００のリンク２１００〜２１０６の相対位置情報や、各リンク２１００〜２１０６の重さ、慣性モーメント、重心位置、初期の関節角度、速度などの情報を読み込むことで、初期設定を行う（Ｓ２）。
【００４１】
ここで、ＣＰＵ５０１は、ロボット２００の三次元データ（ＣＡＤデータ）に基づく仮想モデル（仮想ロボット）を仮想三次元空間に配置する。ＣＰＵ５０１は、この仮想ロボットが配置された仮想三次元空間をモニタ５２３に表示させる。
【００４２】
次に、ＣＰＵ５０１は、ロボット２００のバネ要素やダンパー要素などの受動要素のトルクを計算する（Ｓ３）。また、ＣＰＵ５０１は、モータトルクを計算する（Ｓ４）。即ち、これらステップＳ３，Ｓ４の計算により、力発生手段（モータ、バネ要素、ダンパー要素）の発生力としてのトルクが計算される。そして、ＣＰＵ３０１は、これらステップＳ３，Ｓ４の計算結果を用いて、ロボット２００の接触力及び拘束力を計算する（Ｓ５）。
【００４３】
次に、ＣＰＵ５０１は、ステップＳ３，Ｓ４，Ｓ５の計算結果を用いて順動力学計算を行い、関節の加速度を算出する（Ｓ６）。
【００４４】
ＣＰＵ５０１は、関節の加速度に基づき、関節の速度及び位置を計算してこれら関節の加速度、速度及び位置等からなる関節情報を更新し（Ｓ７）、３Ｄ画面情報（仮想ロボットの位置姿勢）を更新する。
【００４５】
次に、ＣＰＵ５０１は、ＴＣＰが終点に到達したか否かを判断し（Ｓ８）、終点に到達していなければ（Ｓ８：Ｎｏ）、予め決められた時間、仮想ロボットの動作を進め（Ｓ９）、ステップＳ３の処理に戻る。ＣＰＵ５０１は、ＴＣＰが終点に到達していれば（Ｓ８：Ｙｅｓ）、仮想ロボットを用いたロボット２００の動作のシミュレーションを終了する。
【００４６】
ユーザは、このシミュレーション結果に基づき動作プログラムを必要に応じて適宜修正する。ユーザは、再度シミュレーション装置５００にシミュレーションを行わせるなどして、動作プログラムを確定させる。動作プログラムが確定したら、ユーザの操作により、動作プログラムに基づく軌道データをロボット制御装置３００に送る。ロボット制御装置３００は、実際のロボット２００を、軌道データに従って動作させ、組み付け動作などをロボット２００に行わせる。このように、オフライン教示で軌道データを作成することで、オンライン教示に比べ、ロボット２００の教示作業が容易となる。
【００４７】
次に、ロボット２００の動作をシミュレーションするのに用いる、ステップＳ５のロボット２００に物体が接触する接触力及びロボット２００の関節における拘束力を求める処理について詳細に説明する。
【００４８】
図５は、接触力及び拘束力の計算方法を示すフローチャートである。なお、以下の説明で、ヤコビアンとは、ヤコビ行列のことである。また、実施形態中の拘束力τｃｏｎｓｔｒａｉｎは、拘束力τＡに相当し、実施形態中の接触力τｃｏｎｔａｃｔは、接触力τＢに相当する。また、実施形態中の拘束力ｆｃｏｎｓｔｒａｉｎは、拘束力ｆＡに相当し、実施形態中の接触力ｆｃｏｎｔａｃｔは、接触力ｆＢに相当する。更に、実施形態中の拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは、第１のヤコビアンＪＡに相当し、実施形態中の接触ヤコビアンＪｃｏｎｔａｃｔは、第２のヤコビアンＪＢに相当し、実施形態中の接触・拘束ヤコビアンＪは、第３のヤコビアンＪに相当する。
【００４９】
まずＣＰＵ５０１は、関節空間におけるロボット２００の運動方程式（第１の運動方程式）を計算する（Ｓ１１）。ここで、関節空間とは、ロボット２００の関節の数を自由度とする空間、即ち各関節位置（角度）を座標軸とする空間である。自由度とは、ある物理系の状態を記述するのに必要な変数の最小個数である。
【００５０】
関節空間において、ロボット２００の関節の変位をｑ、ロボット２００の関節の加速度をｑ（・・）、ロボット２００の質量に関する質量行列をＡ、ロボット２００に働くコリオリ力及び遠心力に関するコリオリ力／遠心力項をｂとする。また、関節空間において、ロボット２００に働く重力に関する重力項をｇ、ロボット２００の関節に発生する発生力（即ち、モータトルクなどの能動要素のトルク、スプリング要素やダンパー要素などの受動要素のトルク）をτａｃｔとする。また、関節空間において、ロボット２００の拘束力をτｃｏｎｓｔｒａｉｎ、ロボット２００の接触力をτｃｏｎｔａｃｔとする。ここで、関数を時間で一次微分したものを（・）、関数を時間で二次微分したものを（・・）で表記する。
【００５１】
関節空間におけるロボット２００の第１の運動方程式は、加速度ｑ（・・）、質量行列Ａ、コリオリ力／遠心力項ｂ、重力項ｇ、発生力τａｃｔ、拘束力τｃｏｎｓｔｒａｉｎ、接触力τｃｏｎｔａｃｔを用いて以下の式（１）で表される。
【００５２】
【数５】
2018030210.tif 000007
【００５３】
ここで、第１の運動方程式である式（１）を計算するとは、式（１）における質量行列Ａ、コリオリ力／遠心力項ｂ、重力項ｇを計算することである。また、式（１）は、拘束を取り去ったロボット２００の運動方程式である。
【００５４】
次に、ＣＰＵ５０１は、拘束空間（第１の座標空間）における拘束力ｆｃｏｎｓｔｒａｉｎと、関節空間における拘束力τｃｏｎｓｔｒａｉｎとの関係を示す拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを計算する（Ｓ１２）。拘束空間とは、ロボット２００の関節の拘束条件に基づく自由度を持ち、ロボット２００の拘束点の拘束方向を基準とする座標空間である。
【００５５】
このとき、ＣＰＵ５０１は、以下の式（２）を満たすように、拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを算出する（第１のヤコビアン算出工程、第１のヤコビアン算出処理）。ここで、ｑ（・）は、関節空間におけるロボット２００の関節の速度である。
【００５６】
【数６】
2018030210.tif 000008
【００５７】
具体例として、ギヤの拘束のように、回転部品の軸ｑ１と回転部品の軸ｑ２がある減速比Ｒで拘束される場合の拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは、以下のように計算できる。
【００５８】
【数７】
2018030210.tif 000009
【００５９】
この場合の拘束点は軸ｑ１と軸ｑ２の位置であり、このヤコビアンのサイズは、１つのギヤの拘束につき「１×自由度」である。
【００６０】
異なる具体例として、部品同士を固定する拘束のように、部品１の任意点Ｐ１と部品２の任意点Ｐ２の位置を拘束する場合の拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは、以下のように計算できる。
【００６１】
【数８】
2018030210.tif 000010
【００６２】
ここで、ＪＰ１は任意点Ｐ１のヤコビアンを表し、ＪＰ２は任意点Ｐ２のヤコビアンを表し、関節の速度と任意点の速度を関係づける一般的なヤコビアンである。この場合の拘束点は任意点Ｐ１と任意点Ｐ２の位置であり、このヤコビアンのサイズは、１つの固定点につき「（並進成分＋回転成分）×自由度」である。
【００６３】
さらにこの例において、部品１の任意点Ｐ１と部品２の任意点Ｐ２を並進方向にのみ固定する拘束のような場合の拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは、以下のように計算できる。
【００６４】
【数９】
2018030210.tif 000011
【００６５】
ここで、Ｊ’Ｐ１は任意点Ｐ１のヤコビアンの回転成分を表し、Ｊ’Ｐ２は任意点Ｐ２のヤコビアンの並進成分を表している。この場合の拘束点は任意点Ｐ１と任意点Ｐ２の位置であり、このヤコビアンのサイズは、１つの固定点につき「（並進成分）×自由度」である。
【００６６】
次に、ＣＰＵ５０１は、接触点の検出計算を行い（Ｓ１３）、接触点が存在するか否かを判断する（Ｓ１４）。ＣＰＵ５０１は、接触点が存在する場合、接触ヤコビアンＪｃｏｎｔａｃｔを計算する（Ｓ１５：第２のヤコビアン算出工程、第２のヤコビアン算出処理）。具体的に説明すると、ＣＰＵ５０１は、ロボット２００と物体との接触点のワールド座標系Σ０における座標値に基づき、接触ヤコビアンＪｃｏｎｔａｃｔを算出する。接触ヤコビアンＪｃｏｎｔａｃｔは、接触空間（第２の座標空間）における接触力ｆｃｏｎｔａｃｔと、関節空間における接触力τｃｏｎｔａｃｔとの関係を示すヤコビアンである。接触空間は、ロボット２００の接触点の法線方向及び接線方向を基準とする三次元直交座標（デカルト座標）で表された座標空間である。
【００６７】
以下、更に具体的に説明する。なお、ｘｃｏｎｔａｃｔは接触空間における変位（接触点の相対位置）、ｘｃｏｎｓｔｒａｉｎは拘束空間における変位（拘束点の相対位置）とする。よって、ｘｃｏｎｔａｃｔ（・）は接触空間における速度、ｘｃｏｎｔａｃｔ（・・）は接触空間における加速度であり、ｘｃｏｎｓｔｒａｉｎ（・）は拘束空間における速度、ｘｃｏｎｓｔｒａｉｎ（・・）は拘束空間における加速度である。
【００６８】
部品ａの接触点ｘｃｏｎｔａｃｔａ，ｉと、部品ｂの接触点ｘｃｏｎｔａｃｔｂ，ｉとに接触が発生したとする。ｉは、接触点に付した番号である。このとき、それぞれの接触点の速度は以下のように表される。
【００６９】
【数１０】
2018030210.tif 000012
【００７０】
ここで、右辺のＪｃｏｎｔａｃｔａ，ｉ及びＪｃｏｎｔａｃｔｂ，ｉは、ｉ番目の接触点のヤコビアンである。これを用いると、ｉ番目の接触における接触点の相対速度ｘ（・）ｃｏｎｔａｃｔｉは、以下の式（３）のようになる。
【００７１】
【数１１】
2018030210.tif 000013
【００７２】
次に、ｉ番目の接触における法線ベクトルをｎｃｏｎｔａｃｔｉとし、接線ベクトルをｔｃｏｎｔａｃｔｘ，ｉとｔｃｏｎｔａｃｔｙ，ｉとする。これらの表記を用いて、ｉ番目の接触における法線方向速度と接線方向速度を表すヤコビアンＪｃｏｎｔａｃｔｎ，ｉ及びＪｃｏｎｔａｃｔｔ，ｉは以下の式（４），（５）のようになる。
【００７３】
【数１２】
2018030210.tif 000014
【００７４】
各接触における法線方向ヤコビアンＪｃｏｎｔａｃｔｎ，ｉと接線方向ヤコビアンＪｃｏｎｔａｃｔｔ，ｉを積みあげることによって、すべての接触点に対する接触ヤコビアンＪｃｏｎｔａｃｔが以下の式（６）のように表現できる。
【００７５】
【数１３】
2018030210.tif 000015
【００７６】
次に、ＣＰＵ５０１は、拘束ヤコビアンＪｃｏｎｓｔｒａｉｎ及び接触ヤコビアンＪｃｏｎｔａｃｔを合わせた接触・拘束ヤコビアンＪを求める（Ｓ１６：第３のヤコビアン算出工程，第３のヤコビアン算出処理）。接触・拘束ヤコビアンＪは、以下の式（７）で表される。
【００７７】
【数１４】
2018030210.tif 000016
【００７８】
次に、ＣＰＵ５０１は、接触・拘束ヤコビアンＪを用いて、関節空間における第１の運動方程式（式（１））を、以下の接触・拘束空間（第１の座標空間及び第２の座標空間）における第２の運動方程式（式（８））に変換する（Ｓ１７：変換工程，変換処理）。
【００７９】
【数１５】
2018030210.tif 000017
【００８０】
なお、式（８）は、以下の式で表される。
【００８１】
【数１６】
2018030210.tif 000018
【００８２】
ただし、上式のパラメータは以下の通りである。
【００８３】
【数１７】
2018030210.tif 000019
【００８４】
即ち、ＣＰＵ５０１は、ヤコビアンＪを用いて第１の運動方程式（式（１））を第２の運動方程式（式（８））に変換することで、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆａｃｔの各係数を求める。なお、加速度ｘ（・・）は、ｘｃｏｎｓｔｒａｉｎ（・・），ｘｃｏｎｔａｃｔ（・・）からなる。
【００８５】
次に、ＣＰＵ５０１は、式（８）に示す第２の運動方程式を、時間領域で積分することで線形化して、以下の第３の運動方程式を求める（Ｓ１８：線形化工程，線形化処理）。
【００８６】
【数１８】
2018030210.tif 000020
【００８７】
つまり、線形化した第３の運動方程式において、質量行列Λ、コリオリ力／遠心力項μ、重力項ｐ、及び発生力ｆａｃｔの各係数は既知の値であり、ｆｃｏｎｓｔｒａｉｎ，ｆｃｏｎｔａｃｔが未知数である。第３の運動方程式にこれら係数を適用すれば、第２の運動方程式を第３の運動方程式に変換したことになる。ここで、Δｔは接触力と拘束力が働く所定の時間である。
【００８８】
式（８）に示す第２の運動方程式を時間領域で積分し線形化した第３の運動方程式は、以下のようにも表される。
【００８９】
【数１９】
2018030210.tif 000021
【００９０】
以上の第３の運動方程式は、接触・拘束空間における拘束点の拘束力ｆｃｏｎｓｔｒａｉｎ及び接触点の接触力ｆｃｏｎｔａｃｔを未知数とする連立方程式となる。
【００９１】
次に、ＣＰＵ５０１は、線形化した連立方程式を解くことにより、接触・拘束空間における拘束力ｆｃｏｎｓｔｒａｉｎと接触力ｆｃｏｎｔａｃｔを算出する（Ｓ１９：力算出工程、力算出処理）。このように、本実施形態では、拘束力ｆｃｏｎｓｔｒａｉｎ及び接触力ｆｃｏｎｔａｃｔを同時に求めることができる。
【００９２】
ここで、接触・拘束空間における接触力ｆｃｏｎｔａｃｔ及び拘束力ｆｃｏｎｓｔｒａｉｎを連立させて計算する際には、下記の制約条件を満たすように計算を行う。
【００９３】
制約条件１．接触時に発生する接触空間における接触力の法線方向成分は、物体同士が離れる方向に発生する。
【００９４】
【数２０】
2018030210.tif 000022
【００９５】
制約条件２．接触時に発生する接触空間における速度は、物体同士が離れる方向に発生する。
【００９６】
【数２１】
2018030210.tif 000023
【００９７】
制約条件３．接触時に発生する接触空間における接触力の法線方向成分と速度の関係について、反発係数＝０の場合においては、接触力が発生した場合には速度は０になり、逆に接触力が０になる場合に速度は０にならない。
【００９８】
【数２２】
2018030210.tif 000024
【００９９】
制約条件４．接触時に発生する接触空間における接触力の接線方向成分は、法線方向接触力と摩擦係数ｋを掛けた最大摩擦力以内となる。
【０１００】
【数２３】
2018030210.tif 000025
【０１０１】
制約条件５．拘束力によって発生する拘束空間における速度は、常に０となる。
【０１０２】
【数２４】
2018030210.tif 000026
【０１０３】
接触力ｆｃｏｎｔａｃｔに関する制約は、接触力の物理的な関係性であり、これらを用いて線形相補性問題に帰着することができる。拘束力ｆｃｏｎｓｔｒａｉｎに関する制約は、拘束力の物理的な関係性を表している。以上の制約をまとめると、以下のようになる。
【０１０４】
【数２５】
2018030210.tif 000027
【０１０５】
式（１７）は、接触点において法線方向の接触力は、物体同士が離れる方向にしか作用しないことを意味している。式（１８）は、接触点において物体同士がめり込む方向に速度が発生しないことを意味している。式（１９）は、反発係数＝０の場合においては、接触力が発生した場合には接触空間における速度は０になり、逆に接触力が０になる場合には接触空間における速度は０にならないことを意味している。式（２０）は、接線方向接触力が法線方向接触力の最大摩擦係数以内であることを意味している。式（２１）は、拘束空間での速度が常に０となることを意味している。
【０１０６】
したがって、ＣＰＵ５０１は、ステップＳ１９では、式（１７）〜式（２１）の関係式を満たすように、拘束力ｆｃｏｎｓｔｒａｉｎと接触力ｆｃｏｎｔａｃｔを求める。具体的に説明すると、ＣＰＵ５０１は、以下の演算処理１〜５を行う。
【０１０７】
ｌ＝０とする。ｆに初期解ｆｌを代入する（演算処理１）。
【０１０８】
ｆｌのｉ成分ｆｉｌを以下の式に従い更新する（演算処理２）。
【０１０９】
【数２６】
2018030210.tif 000028
【０１１０】
ここで、ｍｉ，ｊは式（１３）の行列Ｍの（ｉ，ｊ）成分である。Ｚｉは式（１４）のベクトルｚの成分ｉである。
【０１１１】
次に、ｆｉｌ＋１がｆｃｏｎｔａｃｔの法線方向の力に相当する成分であり、かつ、ｆｉｌ＋１＜０ならｆｉｌ＋１＝０とする（演算処理３）。
【０１１２】
次に、演算処理４として、ｆｉｌ＋１とｆｉ＋１ｌ＋１がｆｃｏｎｔａｃｔの接線方向の力に相当する成分である場合、以下の処理４−１，４−２を実行する。即ち、処理４−１として、以下の計算をする。
【０１１３】
【数２７】
2018030210.tif 000029
【０１１４】
また、処理４−２として、以下の計算をする。
【０１１５】
【数２８】
2018030210.tif 000030
【０１１６】
次に、演算処理５として、以下の式の演算結果を満たす場合は、ｆｌ＋１をｆとして終了する。
【０１１７】
【数２９】
2018030210.tif 000031
【０１１８】
以上の式を満たさない場合は、ｌ＝ｌ＋１として、演算処理２へ戻る。以上の演算処理により、式（１７）〜式（２１）を満たす式（１２）に対する拘束力ｆｃｏｎｓｔｒａｉｎと接触力ｆｃｏｎｔａｃｔを求めることができる。即ち、拘束を有するロボットの接触力を求めることが可能になる。
【０１１９】
関節空間における拘束力τｃｏｎｓｔｒａｉｎ及び接触力τｃｏｎｔａｃｔを足し合わせた関節への入力トルクをτとする。ＣＰＵ５０１は、拘束力ｆｃｏｎｓｔｒａｉｎ及び接触力ｆｃｏｎｔａｃｔを接触・拘束ヤコビアンＪを用いて、入力トルクτに変換する（Ｓ２０：力変換工程，力変換処理）。
【０１２０】
以上、ロボット２００が物体と接触する場面をシミュレーションする場合、拘束機構である関節については拘束力でモデル化することで、拘束機構である関節を接触力でモデル化する場合と比べて運動方程式で求めるべき力の数が少なくなる。したがって、演算負荷が低減し、高速なシミュレーションが可能となる。
【０１２１】
［実施例］
（実施例１）ギヤ拘束
図６は、実施例１におけるロボットを示す模式図である。ダイナミクスシミュレーション方法について、図６に示すような軸間に拘束を有するロボットについて接触力及び拘束力を計算する具体例を下記に示す。
【０１２２】
図６に示すように、ロボット５０の関節である回転ジョイント４１が、駆動源であるモータに駆動される第１のギヤである入力ギヤ３１で構成され、回転ジョイント４２が、入力ギヤ３１に噛合する第２のギヤである出力ギヤ３２で構成されている。したがって、回転ジョイント４１が入力軸であり、回転ジョイント４２が出力軸である。出力ギヤ３２には、リンク３３が接続されている。したがって、リンク３３は出力ギヤ３２と共に回転する。
【０１２３】
実施例１では、入力ギヤ３１と出力ギヤ３２との変位が、ある一定の比率で動作するという拘束を有している。即ち、拘束条件には、入力ギヤ３１と出力ギヤ３２との速度伝達比（ギヤ比）が含まれている。例えば、入力ギヤ３１と出力ギヤ３２との速度伝達比が２：１の拘束条件で拘束されているものとする。拘束を有するロボットのリンク３３に、物体３４と接触したときの接触力を求める。
【０１２４】
ロボットのリンク間の相対位置情報を表１にまとめる。
【０１２５】
【表１】
2018030210.tif 000032
【０１２６】
また、リンクの緒元を表２にまとめる。
【０１２７】
【表２】
2018030210.tif 000033
【０１２８】
回転ジョイント４１と回転ジョイント４２に関する拘束条件を表３にまとめる。
【０１２９】
【表３】
2018030210.tif 000034
【０１３０】
ここでリンク３３は出力ギヤ３２に固定されているものとし、以降では一体部品として扱う。まず図６の状態における関節変位は、以下のようになる。
【０１３１】
【数３０】
2018030210.tif 000035
【０１３２】
各関節速度は、以下のようになる。
【０１３３】
【数３１】
2018030210.tif 000036
【０１３４】
このときの質量行列Ａは、以下のようになる。
【０１３５】
【数３２】
2018030210.tif 000037
【０１３６】
重力項ｇは、以下のようになる。
【０１３７】
【数３３】
2018030210.tif 000038
【０１３８】
また、遠心力／コリオリ力ｂは、以下のようになる。
【０１３９】
【数３４】
2018030210.tif 000039
【０１４０】
以上により、拘束を取り去った状態におけるロボットの以下の運動方程式のＡ，ｂ，ｇに関する項が求まる（Ｓ１１）。
【０１４１】
【数３５】
2018030210.tif 000040
【０１４２】
ここで発生力τｍｏｔｏｒは、駆動源であるモータの駆動力である。なお、実施例１では、これらＡ，ｂ，ｇの計算手順については省略しているが、これらＡ，ｂ，ｇの数値は、各リンクの慣性モーメントや重さ、重心位置、各リンク間の相対位置情報から計算されるものである。拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは第１軸と第２軸の速度伝達比（ギヤ比）が２：１のギヤで接続されているという拘束なので、以下のように算出される（Ｓ１２）。
【０１４３】
【数３６】
2018030210.tif 000041
【０１４４】
次に、接触点検出計算を行う（Ｓ１３）。接触点情報を表４及び表５に示す。
【０１４５】
【表４】
2018030210.tif 000042
【０１４６】
【表５】
2018030210.tif 000043
【０１４７】
接触点情報と法線方向の情報から接触ヤコビアンＪｃｏｎｔａｃｔは以下のように算出される（Ｓ１５）。
【０１４８】
【数３７】
2018030210.tif 000044
【０１４９】
これらの接触ヤコビアンＪｃｏｎｔａｃｔと拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを用いて、接触・拘束ヤコビアンＪを算出する（Ｓ１６）。また、接触・拘束ヤコビアンＪを用いて運動方程式を変換し、式（１２）を算出する。このとき、Ｍとｚは以下のように算出される。
【０１５０】
【数３８】
2018030210.tif 000045
【０１５１】
ここで接触力と拘束力が働く時間を表すΔｔについては、３．７５Ｅ−０８［ｓ］とした。前述した方法により、摩擦係数を０．２としてｆを求める（Ｓ１９）。
【０１５２】
【数３９】
2018030210.tif 000046
【０１５３】
ｆの第１成分が拘束力ｆｃｏｎｓｔｒａｉｎを表し、第２成分と第３成分が接触力ｆｃｏｎｔａｃｔの法線方向成分、第４〜第７成分までが接触力ｆｃｏｎｔａｃｔの接線方向成分に対応している。算出したｆによりｘ（・）を算出すると、以下のようになる。
【０１５４】
【数４０】
2018030210.tif 000047
【０１５５】
拘束点ｘｃｏｎｓｔｒａｉｎにおける速度である第１成分は０となっており、式（２１）を満たしていることが確認できる。また、第２成分と第３成分は接触点ｘｃｏｎｔａｃｔにおける法線方向の速度も０となっている。ｆの対応する成分を見てみるといずれも正の値を取っており、式（１７）と式（１８）と式（１９）を満たしていることが確認できる。
【０１５６】
また、ｆの接触力の法線方向と接線方向の力を比較すると、接触１においては、以下の不等式が成り立つ。
【０１５７】
【数４１】
2018030210.tif 000048
【０１５８】
接触２においても、以下の不等式が成り立つ。
【０１５９】
【数４２】
2018030210.tif 000049
【０１６０】
いずれも式（２０）を満たしていることが確認できる。このとき、接触力と拘束力を足し合わせた各軸の力を示すトルクτは、以下のように求まる（Ｓ２０）。
【０１６１】
【数４３】
2018030210.tif 000050
【０１６２】
以上説明したとおり、図６に示すような、軸間に拘束を有するロボット５０に対して、接触力を計算することができるため、ギヤ３１，３２のダイナミクスを考慮したロボットの接触シミュレーションを行うことができる。
【０１６３】
実施例１では、ロボット５０と物体３４が接触した瞬間を具体例として挙げている。接触力と拘束力とを、連立方程式を用いて解いており、ギヤの拘束１つにつき１個の変数、接触１つにつき３個の変数を解く必要がある。実施例１では接触点が２つあるため、７個の変数を解いている。この変数の個数は、拘束ヤコビアン、接触ヤコビアンの行数に対応している。また、接触力計算では式（１７）〜式（２０）を考慮しなければならない分、計算量が大きい。つまり、ある１つの接触点の接触力の計算量は、ある１つのギヤの拘束力の計算量に比べて、最低３倍以上の計算量が必要である。さらに、実際にギヤを接触でシミュレーションする場合、複数の接触点が発生する可能性がある。仮に実施例１において拘束力を廃しギヤを接触力でシミュレーションした場合では、ギヤ間において最低２つの接触点が発生するため、最低６倍以上の計算量が必要である。さらに、ギヤの数が増えるほどその影響は大きくなる。そのため、拘束を有するロボットの接触問題をシミュレーションする場合は、実施例１のように接触力と拘束力を連立させて計算することにより、計算時間を短縮することができる。これにより、高速なシミュレーションが可能となる。
【０１６４】
（実施例２）角度の伝達誤差つきギヤ拘束
実施例２では、ロボットで一般的に使用される波動歯車減速機などの角度の伝達誤差を持つギヤを模した軸間拘束を有するロボットに対して、接触力の計算方法を適用したときの具体例を示す。
【０１６５】
ロボットで使用される波動歯車減速機など柔軟物を含むギヤの伝達力計算を行うには、接触力計算に加えて変形計算を行う必要があり、計算コストがかかる。そのため、実施例２のように拘束力でシミュレーションを実現するのが有効である。
【０１６６】
なお、ロボットの基本構成、リンクの配置、リンクの緒元は実施例１と同一とし、説明を省略する。
【０１６７】
実施例２は、実施例１と同様にギヤで接続されたリンクを模しており、回転ジョイント４１が入力軸、回転ジョイント４２が出力軸となる。そして、実施例２では、以下のような比率と伝達誤差で表されるギヤで接続されている。即ち、拘束条件には、入力ギヤ３１と出力ギヤ３２との速度伝達比（ギヤ比）と、入力ギヤ３１と出力ギヤ３２との間の角度の伝達誤差が含まれているものとする。
【０１６８】
【数４４】
2018030210.tif 000051
【０１６９】
ここで、右辺第一項がギヤ比を、右辺第二項が角度伝達誤差を表している。拘束ヤコビアンＪｃｏｎｓｔｒａｉｎは以下のように算出される。まず、上記の入力軸と出力軸の関係について微分し、速度の関係式に変換する。
【０１７０】
【数４５】
2018030210.tif 000052
【０１７１】
その後、式をまとめて以下のような式で拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを算出する。
【０１７２】
【数４６】
2018030210.tif 000053
【０１７３】
実施例２では、表６のようなギヤ比と角度伝達誤差パラメータを選択した。
【０１７４】
【表６】
2018030210.tif 000054
【０１７５】
角度伝達誤差以外のパラメータについては実施例１と同様であるため、ロボットのリンク間の相対位置情報とリンクの緒元は省略する。まず図６の状態における各関節変位は、以下のようになる。
【０１７６】
【数４７】
2018030210.tif 000055
【０１７７】
各関節速度は、以下のようになる。
【０１７８】
【数４８】
2018030210.tif 000056
【０１７９】
このときの質量行列Ａは、以下のようになる。
【０１８０】
【数４９】
2018030210.tif 000057
【０１８１】
重力項ｇは、以下のようになる。
【０１８２】
【数５０】
2018030210.tif 000058
【０１８３】
また、遠心力／コリオリ力ｂは、以下のようになる。
【０１８４】
【数５１】
2018030210.tif 000059
【０１８５】
以上により、拘束を取り去った状態におけるロボットの以下の運動方程式のＡ，ｂ，ｇに関する項が求まる（Ｓ１１）。
【０１８６】
【数５２】
2018030210.tif 000060
【０１８７】
なお、実施例２ではこれらＡ，ｂ，ｇの計算手順については省略しているが、これらＡ，ｂ，ｇの数値は、各リンクの慣性モーメントや重さ、重心位置、各リンク間の相対位置情報から計算されるものである。拘束ヤコビアンは第１軸と第２軸の変位比率が１０：１のギヤで接続されており、かつ角度伝達誤差を含むという拘束であり、前段で計算した式に値を入れると以下のようになる（Ｓ１２）。
【０１８８】
【数５３】
2018030210.tif 000061
【０１８９】
次に、接触点検出計算を行う（Ｓ１３）。接触点情報を表７及び表８に示す。
【０１９０】
【表７】
2018030210.tif 000062
【０１９１】
【表８】
2018030210.tif 000063
【０１９２】
接触点情報と法線方向の情報から接触ヤコビアンＪｃｏｎｔａｃｔは以下のように算出される（Ｓ１４）。
【０１９３】
【数５４】
2018030210.tif 000064
【０１９４】
これらの接触ヤコビアンＪｃｏｎｔａｃｔと拘束ヤコビアンＪｃｏｎｓｔｒａｉｎを用いて、接触・拘束ヤコビアンを算出する（Ｓ１５）。また、接触・拘束ヤコビアンＪを用いて運動方程式を変換し、式（１２）を算出する。このとき、Ｍとｚは以下のように算出される（Ｓ１７）。
【０１９５】
【数５５】
2018030210.tif 000065
【０１９６】
ここで接触力と拘束力が働く所定の時間Δｔについては、１．００Ｅ−０７［ｓ］とした。前述した方法により、摩擦係数を０．２としてｆを求める。
【０１９７】
【数５６】
2018030210.tif 000066
【０１９８】
ｆの第１成分が拘束力を表し、第２成分と第３成分が接触力の法線方向成分、第４〜第７成分までが接触力の接線方向成分に対応している。算出したｆによりｘ（・）を算出すると、以下のようになる。
【０１９９】
【数５７】
2018030210.tif 000067
【０２００】
拘束点における速度である第１成分は０となっており式（２１）を満たしていることが確認できる。また、第２成分と第３成分は接触点における法線方向の速度も０となっている。このとき、ｆの対応する成分を見てみるといずれも正の値を取っており、式（１７）と式（１８）と式（１９）を満たしていることが確認できる。
【０２０１】
また、ｆの接触力の法線方向と接線方向の力を比較すると、以下の不等式が成り立つ。
【０２０２】
【数５８】
2018030210.tif 000068
【０２０３】
よって、式（２０）を満たしていることが確認できる。また、ｆの接触力の法線方向と接線方向の力を比較すると、以下の不等式が成り立つ。
【０２０４】
【数５９】
2018030210.tif 000069
【０２０５】
よって、式（２０）を満たしていることが確認できる。このとき、接触力と拘束力を足し合わせた各軸の力（トルク）τは、以下のように求まる。
【０２０６】
【数６０】
2018030210.tif 000070
【０２０７】
以上説明したとおり、ギヤの角度伝達誤差を考慮したロボットの接触シミュレーションを行うことができる。
【０２０８】
［実施例３］
次に、実施例３のダイナミクスシミュレーション方法について説明する。図７（ａ）は、実施例３におけるロボットを示す模式図である。図７（ａ）に示すロボット８０は、パラレルリンク型のロボットである。図７（ａ）に示すような、拘束点の相対速度が常に０になるような拘束を有するロボット８０に対して、接触計算方法を適用したときの具体例について説明する。図７（ａ）に示すように、ロボット８０は、複数のリンク６１〜６４が、関節である回転ジョイント７１〜７５で回転可能に連結されている。
【０２０９】
図７（ｂ）は、図７（ａ）に示すロボットの１つの回転ジョイントを、互いに拘束関係にある２つの拘束点とした模式図である。ロボット８０は、回転ジョイント７４を取り去り、代わりに拘束点８１と拘束点８２とがｘ０方向とｚ０方向の座標が常時一致するという拘束条件が設けられた、２本別々のロボットと考えてもよい。２本のロボットのうち、１本目はリンク６１〜６３を有し、２本目はリンク４４のみによって構成される。図７（ａ）に示すロボット８０は、拘束を有するロボットである。
【０２１０】
図７（ｃ）は、実施例３においてロボットに物体が接触した状態を示す模式図である。この拘束を有するロボット８０が、図７（ｃ）に示すような物体９１と接触したときの接触力を求める。
【０２１１】
ロボット８０のリンク間の相対位置情報を表９にまとめる。また、リンクの緒元は実施例１と同様であるため省略する。表１０には、拘束点８１と拘束点８２に関する拘束条件をまとめる。
【０２１２】
【表９】
2018030210.tif 000071
【０２１３】
【表１０】
2018030210.tif 000072
【０２１４】
図７（ｃ）の状態における各関節変位は、以下のようになる。
【０２１５】
【数６１】
2018030210.tif 000073
【０２１６】
各関節速度は、以下のようになる。
【０２１７】
【数６２】
2018030210.tif 000074
【０２１８】
このときの質量行列Ａは、以下のようになる。
【０２１９】
【数６３】
2018030210.tif 000075
【０２２０】
重力項ｇは、以下のようになる。
【０２２１】
【数６４】
2018030210.tif 000076
【０２２２】
また、遠心力／コリオリ力ｂは、以下のようになる。
【０２２３】
【数６５】
2018030210.tif 000077
【０２２４】
以上により、拘束を取り去った状態におけるロボット８０の運動方程式のＡ，ｂ，ｇに関する項が求まる（Ｓ１１）。
【０２２５】
【数６６】
2018030210.tif 000078
【０２２６】
なお、実施例３ではこれらＡ，ｂ，ｇの計算手順については省略しているが、これらＡ，ｂ，ｇの数値は、表１と表２に示した各リンクの慣性モーメントや重さ、重心位置、また、各リンク間の相対位置情報から計算されるものである。
【０２２７】
拘束ヤコビアンは、表１０の拘束点情報と各軸の姿勢から以下のように算出される（Ｓ１２）。具体的には、拘束点８１と拘束点８２の位置を計算し、各点におけるヤコビアンを計算し、それぞれのヤコビアンの差を取り、そこから拘束する軸成分を取得する。本シミュレーションでは、ｙ並進成分とｚ並進成分を拘束することとした。
【０２２８】
【数６７】
2018030210.tif 000079
【０２２９】
次に接触点情報を表１１及び表１２に示す。
【０２３０】
【表１１】
2018030210.tif 000080
【０２３１】
【表１２】
2018030210.tif 000081
【０２３２】
接触点情報と法線方向の情報から接触ヤコビアンは以下のように算出される（Ｓ１５）。
【０２３３】
【数６８】
2018030210.tif 000082
【０２３４】
これらの接触ヤコビアンと拘束ヤコビアンを用いて、Ｍとｚは以下のように算出される。
【０２３５】
【数６９】
2018030210.tif 000083
【０２３６】
ここで接触力と拘束力が働く所定の時間Δｔについては、１．００Ｅ−０７［ｓ］とした。前述した方法により、摩擦係数を０．２としてｆを求める。
【０２３７】
【数７０】
2018030210.tif 000084
【０２３８】
ｆの第１成分と第２成分が拘束力を表し、第３成分と第４成分が接触力の法線方向成分、第５〜第８成分までが接触力の接線方向成分に対応している。算出したｆによりｘ（・）を算出すると、以下のようになる。
【０２３９】
【数７１】
2018030210.tif 000085
【０２４０】
拘束点における速度である第１成分と第２成分は０となっており、式（２１）を満たしていることが確認できる。また、第３成分と第４成分は接触点における法線方向の速度も０となっている。ｆの対応する成分を見てみるといずれも正の値を取っており、式（１７）と式（１８）と式（１９）を満たしていることが確認できる。また、ｆの接触力の法線方向と接線方向の力を比較すると１つ目の接触について、以下の不等式が成り立つ。
【０２４１】
【数７２】
2018030210.tif 000086
【０２４２】
２つ目の接触について、以下の不等式が成り立つ。
【０２４３】
【数７３】
2018030210.tif 000087
【０２４４】
いずれも式（２０）を満たしていることが確認できる。このとき、接触力と拘束力を足し合わせた各軸の力（トルク）τは、以下のように求まる。
【０２４５】
【数７４】
2018030210.tif 000088
【０２４６】
以上説明したとおり、図７（ｂ）に示すような拘束点８１，８２の相対速度が常に０になるような拘束を有するロボットに対して接触力を計算することができる。このため、パラレルリンクを考慮したロボットの接触シミュレーションを行うことができる。
【０２４７】
なお、シミュレーション中の接触や拘束の数やその種類は、これらの実施例に限定されるものではない。
【０２４８】
本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、実施形態に記載されたものに限定されない。
【０２４９】
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【０２５０】
また、上述の実施形態では、ロボットアームが垂直多関節のロボットアームの場合について説明したが、これに限定するものではない。ロボットアームが、例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい。
【０２５１】
また、上述の実施形態では、シミュレーション装置とロボット制御装置とが別のコンピュータで構成される場合について説明したが、これに限定するものではなく、シミュレーション装置とロボット制御装置とが１つのコンピュータで構成されていてもよい。
【符号の説明】
【０２５２】
１００…ロボットシステム、２００…ロボット、３００…ロボット制御装置、５００…シミュレーション装置、５０１…ＣＰＵ（演算部）、６００…制御システム
【図１】
2018030210.tif 000097
【図２】
2018030210.tif 000098
【図３】
2018030210.tif 000099
【図４】
2018030210.tif 000100
【図５】
2018030210.tif 000101
【図６】
2018030210.tif 000102
【図７】
2018030210.tif 000103