(57)【特許請求の範囲】
【請求項１】
ロボットの各軸を駆動するサーボモータを制御する為に前記各軸毎に位置ループ処理と速度ループ処理を含む処理を行なうサーボ系であって、
前記各軸のトルクＴi（ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を、作業空間内に定義された直交座標系上での前記サーボ系の柔らかさを表わすパラメータの設定値の組と前記直交座標系上での位置偏差とに基づいて計算される前記直交座標系上での力を逆動力学で解いて求め、
前記各軸の前記位置ループ処理の入力Ｅi を、前記Ｔiと前記位置ループ処理のゲインＫp と前記速度ループ処理のゲインＫv とに基づいて定める、直交座標系上で柔らかさが調節可能な前記サーボ系。
【請求項２】
ロボットの各軸を駆動するサーボモータを制御する為に前記各軸毎に位置ループ処理と速度ループ処理を含む処理を行なうサーボ系であって、
作業空間内に定義された直交座標系上での前記サーボ系の柔らかさを表わすパラメータの設定値の組と前記直交座標系上での位置偏差から計算される前記直交座標系上での力を逆動力学で解いて求められた前記各軸のトルクＴi （ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を前記位置ループ処理のゲインＫp と速度ループ処理のゲインＫv との積で除した値Ｅi ＝Ｔi ／（Ｋp Ｋv ）を、前記各軸の前記位置ループ処理の入力とした、直交座標系上で柔らかさが調節可能な前記サーボ系。
【請求項３】
ロボットの各軸を駆動するサーボモータを制御する為に前記各軸毎に位置ループ処理と速度ループ処理を含む処理を行なうサーボ系であって、
前記各軸のトルクＴi（ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を、作業空間内に定義された直交座標系上での前記サーボ系の柔らかさを表わすパラメータの設定値の組と前記直交座標系上での位置偏差とに基づいて計算される前記直交座標系上での力を逆動力学で解いて求め、
前記各軸の前記位置ループ処理の入力Ｅi を、前記Ｔiと前記位置ループ処理のゲインＫp と前記速度ループ処理のゲインＫv とに基づいて定めるとともに、
前記位置ループ処理の出力を予め定められた積分ゲインで積分し、前記速度ループ処理の出力に予め定められた所定の制限内で足し込んでトルク指令を出力するようにした、直交座標系上で柔らかさが調節可能な前記サーボ系。
【請求項４】
ロボットの各軸を駆動するサーボモータを制御する為に前記各軸毎に位置ループ処理と速度ループ処理を含む処理を行なうサーボ系であって、
作業空間内に定義された直交座標系上での前記サーボ系の柔らかさを表わすパラメータの設定値の組と前記直交座標系上での位置偏差から計算される前記直交座標系上での力を逆動力学で解いて求められた各軸のトルクＴi （ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を前記位置ループ処理のゲインＫp と速度ループ処理のゲインＫv との積で除した値Ｅi ＝Ｔi ／（Ｋp Ｋv ）を、各軸の前記位置ループ処理の入力とすると共に、
前記位置ループ処理の出力を予め定められた積分ゲインで積分し、前記速度ループ処理の出力に予め定められた所定の制限内で足し込んでトルク指令を出力するようにした、直交座標系上で柔らかさが調節可能な前記サーボ系。
【請求項５】
前記作業空間内に定義された直交座標系上での前記サーボ系の柔らかさを表わすパラメータの組が、画面入力により設定される請求項１〜請求項４いずれか１項に記載された、直交座標系上で柔らかさが調節可能な前記サーボ系。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本願発明は産業用ロボット（以下、単に「ロボット」と言う。）の制御技術に関し、更に詳しく言えば、ロボット軸駆動用のサーボモータを制御するサーボ系の柔らかさを必要に応じて変更し、可変に指定乃至設定すること（以下、「調節」と言う。）が出来るようにする為の技術に関する。
【０００２】
【従来の技術】
ロボットのアームを駆動する各軸のサーボモータの制御は、通常、位置制御ループ及び速度制御ループを有するサーボ系によって制御されている。図１は、これを示したブロック図で、符号１は位置ループゲインＫp の項、符号２は速度ループゲインＫv の項である。また符号３，４はモータの伝達関数の項であり、３はトルク定数Ｋｔ、４はイナーシャＪである。更に、符号５はモータ速度ｖを積分してモータ位置ｑを求める伝達関数である。なお、ｓはラプラス演算子を表わしている。
【０００３】
ロボットコントローラの内部で作成される移動指令ｒとモータ位置ｑより位置偏差ｅが算出され、該位置偏差ｅv に位置ループゲインＫｐを乗じて速度指令ｖc が出力される。更に、速度偏差ｅv が速度指令ｖc とモータ速度ｖより算出され、この速度偏差ｅに速度ループゲインＫｖを乗じてトルク指令ｔc が出力される。そして、該トルク指令ｔc に応じた駆動電流がモータに供給される。なお、速度ループの制御においては、Ｐ制御に代えてＰＩ制御またはＩＰ制御が適用されることもある。
【０００４】
このようなサーボ系（ＰＩ制御もしくはＩＰ制御）によって各軸のモータが制御されるロボットのツール先端点が目標位置に向かって移動中になんらかの障害物に遭遇・接触した場合、ロボットのツール先端点は障害物に抗して目標位置に向かって移動を続行しようとする現象が生じる。
【０００５】
この現象は次のように説明される。即ち、障害物の存在に関わらずサーボモータは目標位置に向かって移動しようとするが、実際には障害物によりその目標位置に移動することが阻まれ位置偏差ｅが増大する。その結果、この位置偏差に位置ゲインＫｐを乗じて得られる速度指令ｖc も増大する。そして、この増大する速度指令ｖc とモータの速度ｖ（障害物に当接しているときには、速度ｖは「０」に近いと考えられる。）との差は、速度ループの持つ積分器によって積分されて増大し、トルク指令Ｔc は大きな値となる。
【０００６】
結局、サーボモータは目標位置への移動を実現しようとして最大のトルクを出力するようになり、ロボットの停止やワーク、エンドエフェクタ等の破壊事故（干渉事故）を招く原因となる。そこで、このような不都合を回避する手段として、必要に応じて位置ループゲインＫｐ及び速度ループゲインＫｖを低下させることにより速度指令ｖc 及びトルク指令ｔc の値の増大を抑止する方式が用いられている。
【０００７】
この方式においては、通常、柔軟制御用のゲイン値Ｋｐ’，Ｋｖ’が予め設定され、柔軟制御指令の入力時にゲインの値がＫｐ，ＫｖからＫｐ’，Ｋｖ’に各々切り換えられる。
【０００８】
このような従来方式は、サーボ系の柔らかさを各ロボット軸（以下、単に「各軸」とも言う。）毎に調節することを通して「各軸空間上でのソフトフロート機能」を実現するものである。
【０００９】
例えば、ロボットの手首側の３軸について非常に小さなＫｐ’，Ｋｖ’を設定し、手首側の３軸について特に柔らかいサーボ制御が実現される。また、全軸について非常に小さなＫｐ’，Ｋｖ’を設定すれば、全軸について非常に柔らかいサーボ系が構成される。
【００１０】
【発明が解決しようとする課題】
しかし、上記した従来方式では、サーボ系の柔らかさは各軸上でしか調節することが出来ないので、作業空間内の方向別に柔らかさを調節し、嵌め合い、バリ取り加工等、実際の作業に適合した柔らかさをロボットに持たせることが困難であった。
【００１１】
例えば、嵌め合いにおいては、一般に、ロボットに把持されたワークの挿入方向に関しては硬く、挿入方向と直交する方向に関しては柔らかいサーボ系による制御が行なわれることが望ましいと考えられが、作業空間内に定義された座標系上でサーボ系の柔らかさを調節出来ない従来方式ではこのようなニーズには応えられない。
【００１２】
また、バリ取り加工においては、加工線の延在方向とそれに直交する２方向の動作について各々別の柔らかさを持たせることが望ましいが、それをロボットの各軸についてサーボ系の柔らかさを調節する従来方式で実現することも極めて困難である。
【００１３】
そこで本願発明の目的は、作業空間内に定義された直交座標系（以下、単に「直交座標系」とも言う。）上で柔らかさを調節することが出来るサーボ系を提供し、ロボットに直交座標系上でのソフトフロート機能を持たせることにある。また、本願発明は、そのことを通して、ロボットを用いた作業の精度と安全性を向上させることを企図するものである。
【００１４】
【課題を解決するための手段】
本願発明は、ロボットの各軸を駆動するサーボモータを制御する為に各軸毎に位置ループ処理と速度ループ処理を含む処理を行なうサーボ系に、直交座標系上でソフトフロート機能を持たせるようにしたものである。
先ず、作業空間内に定義された直交座標系上でのサーボ系の柔らかさを表わすパラメータの組を設定する。設定は、例えばロボットコントローラに付属した教示操作盤から画面入力することで行なわれる。ソフトフロート機能を有効化した場合には、前記設定されたパラメータの組と、直交座標系上での位置偏差から直交座標系上での力を計算する。
【００１５】
そして、この力を逆動力学で解いて各軸のトルクＴi （ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を求め、このトルクＴi と位置ループ処理のゲインＫp と速度ループ処理のゲインＫv とに基づいて各軸の位置ループ処理の入力Ｅi を算出し、このＥi を各軸の前記位置ループ処理の入力とした処理を行なうことにより、直交座標系上で指定された柔らかさが実現される。柔らかさの調節は、パラメータの組の設定を通して実行される。位置ループ処理の入力Ｅi は、前記トルクＴi （ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を、前記位置ループ処理のゲインＫp と速度ループ処理のゲインＫv との積で除すことによって算出することが出来る。
【００１６】
重力の影響を考慮する必要のある場合には、位置ループ処理の出力が予め定められた積分ゲインで積分され、速度ループ処理の出力に足し込まれてトルク指令が出力される。その際の足し込み量には一定の制限を設けることで、柔らかさが失われることを防止する。
【００１７】
【作用】
本願発明は、ロボットの各軸を駆動するサーボモータを制御するサーボ系についてソフトフロート機能を拡充するものである。即ち、本願発明によれば、直交座標系上で柔らかさを指定することが出来るサーボ系が構築され、「直交座標系上でのソフトフロート機能」が実現される。本願発明の原理の概要を説明すれば、次の通りである。
【００１８】
直交座標系（Ｏ−ＸＹＺ）上でのロボット動作制御の柔らかさは、次のように表わすことが出来る。
Ｋx （ｘd −ｘ）＝Ｆx                             ・・・（１）
Ｋy （ｙd −ｙ）＝Ｆy                             ・・・（２）
Ｋz （ｚd −ｚ）＝Ｆz                             ・・・（３）
ここで、
Ｋx ，Ｋy ，Ｋz ；Ｘ，Ｙ，Ｚ各座標系軸方向に関する柔らかさの程度を表わすパラメータ
ｘ，ｙ，ｚ  ；直交座標系上でのフィードバック（Ｘ，Ｙ，Ｚ各成分）
ｘd ，ｙd ，ｚd ；直交座標系上での位置指令（Ｘ，Ｙ，Ｚ各成分）
Ｆx ，Ｆy ，Ｆz ；ツール先端点にかかる力（位置指令で指定された位置に移動しようとする力のＸ，Ｙ，Ｚ各成分）、
である。
【００１９】
従って、直交座標系上でソフトフロート機能を実現する為には、ロボットの動作時に上記（１）〜（３）式中のパラメータＫx ，Ｋy ，Ｋz の値を（可変に定められた）一定値に保つような各軸の柔軟制御を行なうサーボ系を構築すれば良いことになる。
【００２０】
上記（１）〜（３）式は、ツール先端点にかかる力Ｆx ，Ｆy ，Ｆz が、パラメータＫx ，Ｋy ，Ｋz と、直交座標系上における位置偏差で表わされることを示している。
【００２１】
一方、逆動力学のアルゴリズムとして良く知られているＮｅｗｔｏｎ−Ｅｅｌｅｒ法（Ｌｕｈのアルゴリズムとも呼ばれる。）を用いれば、ツール先端点にかかる力Ｆx ，Ｆy ，Ｆz から、ロボットの各軸に必要なトルクτi （ｉ＝１，２・・・ｎ；ｎはロボットの軸数、以下同様。）を求めることが出来る。
【００２２】
Ｎｅｗｔｏｎ−Ｅｅｌｅｒ法は、例えば「機械系のためのロボティクス」（遠山茂樹  著；総合電子出版社、１９９１年５月３０日第２版発行；ｐ５５〜ｐ６５）に詳しいのでその詳細は省略し、使用されるアルゴリズムの結論部分のみを記せば次のようになる。このアルゴリズムは、初期化、フォワードルーチン、バックワードルーチンで構成される。
【００２３】
フォワードルーチンでは、ベース座標系Σ0 側からロボット手先側に向かって各リンクの運動学的な情報が計算される。一方、バックルーチンでは、ロボット手先側からベース座標系Σ0 側へ向かって各リンクに作用する力／モーメントが計算される。
【００２４】
【数１】
0003681431.tif 000002
各記号の意味は次の通りである。
ｎi(i)；ｉ−１番目の軸で駆動されるリンク（以下、リンクｉ−１などと言う。）からリンクｉに及ぼされるトルクのΣi 表示。但し、Σi はｉ番目の軸上に設定された座標系Σi を表わす。
ｆi(i)；リンクｉ−１からリンクｉに及ぼされる力のΣi 表示
Ｆi(i)；リンクｉに及ぼされる外力ベクトルのΣi 表示
Ｎi(i)；リンクｉに及ぼされるモーメントベクトルのΣi 表示
Ｒi ；座標系変換行列Ａi の主座の３×３行列で、回転を表わす。なお、座標系変換行列Ａi は、ｉ番目の軸上に設定された座標系Σi からｉ−１番目の軸上に設定された座標系Σi-1 への座標変換行列である。但し、座標系Σ0 は前記直交座標系よ一致させたベース座標系にとる。
【００２５】
ｐi*；Σi 表示のΣi-1 原点からΣi 原点への位置ベクトル
ｇ；重力ベクトル
ｓi バー；Σi 表示のリンクｉの重心位置を示す位置ベクトル
Ｉi ；Σi 表示のリンクｉの重心周りの慣性モーメント
ｚ0 ；Σ0 のＺ軸方向の単位ベクトルで、Σ0 上では（０，０，１）と表  わされる
ｂi ；ｉ番目の関節軸の粘性抵抗
ｑi ；ｉ番目の関節軸の軸変数で、ｑi ＝θi （リンクｉの角度）として良い
ω，ωドット；Σ0 表示のリンクｉの角速度、角加速度
上記アルゴリズムにおいて使用される諸量は、ロボットの構造パラメータ、各軸の位置、速度、加速度等から計算される。また、前述の式（１）〜（３）に従って位置偏差（ｘd −ｘ），（ｙd −ｙ），（ｚd −ｚ）と柔らかさを表わすパラメータＫx ，Ｋy ，Ｋz から定められるＦx ，Ｆy ，Ｆz は、上記アルゴリズム中のｆn+1(n+1) に取入れられる。ｆn+1(n+1) は、Σn+1 をツール座標系として定義することにより、ツール座標系の原点に働く力をツール座標系上で表わしていると考えることが出来る。
【００２６】
そこで、ツール座標系の姿勢を表わす現在データを行列Ｕとし、Ｆ＝（Ｆx ，Ｆy ，Ｆz ）T ＝（Ｆx(0)，Ｆy(0)，Ｆz(0)）T とすれば、行列演算ＵＦによって、Ｆx ，Ｆy ，Ｆz のツール座標系上の表現が計算される。これをＦ(n+1) ＝（Ｆx(n+1)，Ｆy(n+1)，Ｆz(n+1)）T とすれば、ｆn+1(n+1)＝Ｆ(n+1) となる。
【００２７】
ここで、重力を考慮しない条件；ｇ＝（０，０，０）で上記アルゴリズムを用いて求められた軸ｉのトルクτi （ｇ＝０）をＴi とすれば、その軸のサーボ系は次のように表わされる。
【００２８】
Ｋp Ｋv Ｅi ＝Ｔi                                ・・・（５）
但し、Ｋp は位置ループゲイン、Ｋv は速度ループゲイン、Ｅi は軸ｉの位置ループの入力を表わしている。図２は、このサーボ系を図１に準じた表記で示したものである（Ｋp ，Ｋv は便宜上各軸共通の表記としたが、各軸毎に設定される場合にはＫp ，Ｋv に代えてＫpi，Ｋviとすれば良い）。図１と図２の差異は位置ループ処理への入力がｅからＥi に変わっていることである。即ち、各軸の位置ループ入力Ｅi は従来のサーボ系（図１参照）における位置偏差ｅのように、その軸ｉの位置指令ｒと位置フィードバックｑの偏差で定義されるものではないことである。
【００２９】
このＥi は、直交座標系上でのサーボ系の柔らかさを表わすパラメータの組Ｋx ，Ｋy ，Ｋz と直交座標系上での位置偏差（ｘd −ｘ），（ｙd −ｙ），（ｚd −ｚ）から計算される力を逆動力学で解いて求められた各軸のトルクＴi と、位置ループゲインＫp と、速度ループゲインＫv に基づいて算出されたものである。
【００３０】
換言すれば、このＥi は、直交座標系上でパラメータＫx ，Ｋy ，Ｋz で表わされる柔らかさを実現させる為にそのロボット軸のサーボ系が持つべき柔らかさに応じて各軸の位置偏差ｅを修正したものと解釈することも出来る。Ｅを次式（６）で記す。
Ｅ＝Ｔ／（Ｋp Ｋv ）                              ・・・（６）
以上のことから、各軸について上記（６）式で決定されるＥi を各軸の位置ループ入力とすることにより、前述の関係式（１）〜（３）で表わされる直交座標系上での柔らかさを実現するに必要なトルクＴi が発生する。
【００３１】
次に重力の作用を考慮する。重力の作用を考慮に入れた制御は、図２に示したサーボ系に積分器を付加することで達成出来る。これを、図２に準じた表記で図３に示す。
図３において、Ｋp ，Ｋv は便宜上各軸共通の表記としたが、各軸毎に設定される場合にはＫp ，Ｋv に代えてＫpi，Ｋviとすれば良いことは図２の場合と同様である。図２と図３の差異は、Ｅi を入力とする位置ループ処理で出力される速度指令ｖc に対して重力分を補償する積分項６（Ｋg ／ｓ；但し、Ｋg は積分器のゲイン）が計算され、速度指令ｖc を入力とする速度ループ処理で計算されるトルクＴi に足し込まれることである。
【００３２】
この積分項６は、鉛直上向き方向についてトルク不足となる状態を回避する手段を提供するものである。例えば直交座標系のＺ軸を鉛直上向き方向にとり、Ｋz を小さく（柔らかさは大）に設定した場合、積分項６がない場合にはＴi だけでは重力に打ち勝つことさえ出来ず、落下事故を起こす可能性がある。積分項６があれば、Ｔi だけでは重力に打ち勝つことが出来なくとも、ゲインＫg に応じてトルクＴi'が増大し、十分なトルクが発生する。
【００３３】
但し、この積分項６で表わされる積分器には重力分のトルクが溜まる性質があるので、積分器の出力に一定の制限値を設ける必要がある。もし、制限値を設けないと、積分器に外力以上の力が溜り、柔らかさを失ってその軸の指令位置に向かう強制的な動きが発生する。
【００３４】
そこで、ソフトフロート機能起動時に積分器に溜っている値（出力）をＩGRとして、ソフトフロート機能時には次のような制限を設ける。ＩSOFT-LIMの値はチューニング等によって各軸毎に適当に設定される。
IF Ｉ＞ＩGR＋ＩSOFT-LIM   THEN  Ｉ＝ＩGR＋ＩSOFT-LIM  ・・・（７）
IF Ｉ＜ＩGR−ＩSOFT-LIM   THEN  Ｉ＝ＩGR−ＩSOFT-LIM  ・・・（８）
以上説明した手法に従って各軸のサーボ系を構成することによって、ロボットに直交座標系上で指定された柔らかさを持たせることが出来る。例えば、Ｚ軸方向を挿入方向とする嵌め合いにおいては、Ｋx とＫy は比較的小さく、Ｋz は比較的大きく指定することで、嵌め合いをスムーズに行なうことが出来る。また、バリ取り加工においては、加工線の延在方向（例えばＸ軸方向）とそれに直交する２方向（例えばＹ軸、Ｚ軸方向）について、個別に適正なＫx ，Ｋy ，Ｋz の値を設定し、バリ取り加工の作業精度を向上させることが出来る。
【００３５】
【実施例】
図４は、本願発明の実施例で使用されるロボットコントローラＲＣを関連機器と共に要部ブロック図で示したものである。先ず、ソフトフローティングサーボ制御に直接関係のある部分から説明すると、符号１０はシステム全体を制御するホストコンピュータを表わしている。符号１６は、ホストコンピュータ１０から出力される移動指令や制御信号を後述のディジタルサーボ回路のプロセッサに引渡し、あるいは、逆にディジタルサーボ回路のプロセッサからの各種信号をホストコンピュータ１０に引き渡すための共有ＲＡＭメモリを表わしている。
【００３６】
符号１７は、上述したサーボ制御を実行するディジタルサーボ（ソフトウェアサーボ）回路で、プロセッサ、ＲＯＭ、ＲＡＭ等のメモリ等で構成される。符号１８は、ロボット３０における各軸のサーボモータの位置のフィードバック値、速度のフィードバック値、電流のフィードバック値等が書き込まれる帰還レジスタを表わしている。
【００３７】
他の部分について見ると、ホストＣＰＵ１０にはバスライン１９を介してＲＯＭ１１、ＲＡＭ１２、不揮発性メモリ１３、外部装置４０とのインターフェイスの役割を果たす入出力装置（Ｉ／Ｏ）１４、教示操作盤２０とのインターフェイス（Ｉ／Ｆ）１５が接続されている。ＲＯＭ１１には、各種のシステムプログラムが格納される。ＲＡＭ１２は、ホストＣＰＵ１０によるデータの一時記憶に使用されるメモリである。不揮発性メモリ１３には、ロボット３０及び外部装置４０の動作内容に関する各種プログラム、関連設定値等が格納される。
【００３８】
教示操作盤２０は、液晶ディスプレイ（ＬＣＤ）及びキーボードＫＢを備え、プログラム再生運転指令、ジョグ送り指令、プログラムデータの入力／変更、関連設定値入力／変更等が可能である。本実施例では、後述する柔らかさ設定画面で画面入力されるＸＹＺ各軸方向の柔らかさのデータ、並びに教示操作盤２０から入力されるＩSOFT-LIMの値が不揮発性メモリ１３に格納される。
【００３９】
入出力装置１４に接続される外部装置は、アプリケーションによって異なる。例えば、ワークの把持を伴う作業であればロボットハンド、バリ取り加工であればバリ取りツール（電源等を含む。）が入出力装置１４に接続される。
【００４０】
図５は、本願発明の適用例として考えられる嵌め合い作業について説明する図である。同図において、符号Ｗ1 ，Ｗ2 は嵌め合い対象のワークを表わしている。ワークＷ1 は図示を省略したロボットハンドに把持されて、ワークＷ2 の孔ＨＬ内に嵌入される。嵌入方向はロボットコントローラＲＣに設定済みの直交座標系Σ0 のＺ軸方向と一致しているものとする。符号ＧＰはワークＷ1 の先端部に設けられたガイドピンを表わし、ＴＣＰはツール先端点を表わす。
【００４１】
また、Ｐ1 ，Ｐ2 は各々後述する動作プログラムでイチ［１］及びイチ［２］として教示された位置を表わす。このような嵌め合いにおいては、教示ラインＰ1 Ｐ2 が孔ＨＬの軸線とある程度ずれることが多い。従って、イチ［１］からイチ［２］への移動時に通常の硬いサーボ制御を行なうと、ＴＣＰはあくまで教示ラインＰ1 Ｐ2 に沿って進もうとするので、ガイドピンＧＰが孔ＨＬの入口でつかえて動けなくなり、場合によってはワークＷ1 ，Ｗ2 やロボットハンドの破損事故を起こす。本願発明を適用すれば、このような事態が回避される。以下、その為の手順を説明する。
【００４２】
先ず、ユーザは、教示操作盤２０に付設されたＬＣＤに図６に示されたような柔らかさ入力画面を呼び出す。そして、Σ0 のＸ軸、Ｙ軸、Ｚ軸方向の各々について個別に所望の値を画面入力する。入力される数値が小さい程柔らかい状態が設定され、入力される数値が大きい程硬い状態が設定さる。ここでは、Ｘ軸方向；Ｋx ＝２００ｇ／ｃｍ、Ｙ軸方向；Ｋy ＝２００ｇ／ｃｍ、Ｚ軸方向；Ｋz ＝５００ｇ／ｃｍが各々設定される例が示されている。
【００４３】
図７は、図５に示した嵌め合いを実行する為に直交座標系ソフトフロート機能を用いた動作プログラムの一例を表わしている。
【００４４】
図６の画面入力後に図７の動作プログラムの再生運転を開始すると、ロボットのツール先端点はイチ［１］まで各軸移動形式で移動し、位置決めされる。そして、ハンドを閉じてワークＷ1 を把持した後に、直交座標系ソフトフロート機能を有効にして、イチ［２］に向かって指令速度５０ｍｍ／ｓｅｃで移動する。
直交座標系ソフトフロート機能が有効にされると図８に示した処理が所定周期で実行され、作用の欄で説明した原理によって、ソフトフロート機能が発揮される。先ず、ステップＳ１では、直交座標系Σ0 上における位置偏差と設定されたパラメータ（Ｋx ，Ｋy ，Ｋz ）を用いて、直交座標系Σ0 上における力（Ｆx ，Ｆy ，Ｆz ）を求める。直交座標系Σ0 上における位置偏差は、各軸における位置偏差から順運動学で計算出来る。続くステップＳ２では、直交座標系Σ0 上で求められた力（Ｆx ，Ｆy ，Ｆz ）をロボットの姿勢データを用いてツール座標系Σn+1 上のデータ（Ｆx(n+1)，Ｆy(n+1)，Ｆz(n+1)）に変換する。
【００４５】
更に、（Ｆx(n+1)，Ｆy(n+1)，Ｆz(n+1)）＝（ｆx(n+1)，ｆy(n+1)，ｆz(n+1)）として、作用の欄で説明したＮｅｗｔｏｎ−Ｅｕｌｅｒ法により、各軸のトルクＴi を計算し（ステップＳ３）、Ｔi ／（Ｋp Ｋv ）から位置ループ入力Ｅi を計算する（ステップＳ４）。計算された位置ループ入力Ｅi を入力とする位置ループ処理、位置ループ出力を入力とする積分処理と速度ループ処理、積分処理と速度ループ処理の出力を加算するトルク指令作成処理等を実行すれば（ステップＳ５）、図６の設定画面通りのソフトフロート機能が発揮される。積分処理と速度ループ処理の出力の加算は、設定されたＩSOFT-LIMの下で、前出の（７），（８）式の制限を守った範囲で行なわれる。
【００４６】
これにより、ワークＷ1 のガイドピンＧＰが孔ＨＬの上縁部に当接すると、ツール先端点ＴＣＰは孔ＨＬの内壁をなぞるように孔ＨＬの軸線ＷＪ上の点Ｑ（教示位置Ｐ2 に対応する位置）まで円滑に移動する。次いで、ロボットハンドを開放して、ワークＷ1 の把持を解けば嵌め合い作業は完了する。
【００４７】
なお、上記実施例では、直交座標系上で柔らかさを表わすパラメータの設定は画面入力で行なったが、動作プログラムの中で指定する方式としても良い。例えば、バリ取りへのアプリケーションにおいて、加工線の延在方向が屈曲している場合に、動作プログラム中でそれに応じたパラメータ変更を行なえば、加工線の延在方向に即した柔らかさの設定が可能となる。
【００４８】
【発明の効果】
本願発明によれば、作業空間内に定義された直交座標系上で柔らかさを調節することが出来る。本願発明の手法に従って各軸のサーボ系を構成することによって、例えば、嵌め合い、バリ取り等の作業精度を向上させることが出来る。
【図面の簡単な説明】
【図１】従来のサーボ系の構成を示したブロック図である。
【図２】重力を考慮しない場合の本願発明のサーボ系の構成を示したブロック図である。
【図３】重力を考慮した場合の本願発明のサーボ系の構成を示したブロック図である。
【図４】本願発明の実施例で使用されるロボットコントローラの概要を説明する要部ブロック図である。
【図５】本願発明の適用例として考えられる嵌め合い作業について説明する図である。
【図６】直交座標系上で柔らかさを入力する為の画面を説明する図である。
【図７】図５に示した嵌め合いを実行する為に直交座標系ソフトフロート機能を用いた動作プログラムの一例を表わたものである。
【図８】実施例におけるソフトフロート機能有効化時の処理の概要を説明するフローチャートである。
【符号の説明】
１  位置ループゲインの項
２  速度ループゲインの項
３，４  モータの伝達関数の項
５  モータ速度からモータ位置を求める伝達関数
６  重力分を補償する積分項
１０  ホストＣＰＵ
１１  ＲＯＭ
１２  ＲＡＭ
１３  不揮発性メモリ
１４  入出力装置
１５  インターフェイス
１６  共有ＲＡＭ
１７  ディジタルサーボ回路
１８  帰還レジスタ
１９  バスライン
２０  教示操作盤
３０  ロボット
４０  外部装置
ＫＢ  キーボード
Ｋp   位置ゲイン
Ｋv   速度ループゲイン
Ｋt   トルク定数
Ｋg   積分ゲイン
ＧＰ  ガイドピン
ＨＬ  孔
ＬＣＤ  液晶ディスプレイ
Ｐ1 ，Ｐ2   教示点
ＲＣ  ロボットコントローラ
Ｗ1 ，Ｗ2   嵌め合い対象ワーク
ＷＪ  孔の軸線
【図１】
0003681431.tif 000003
【図２】
0003681431.tif 000004
【図３】
0003681431.tif 000005
【図４】
0003681431.tif 000006
【図５】
0003681431.tif 000007
【図６】
0003681431.tif 000008
【図７】
0003681431.tif 000009
【図８】
0003681431.tif 000010