JP2005219205A - 直交座標系上で柔らかさが調節可能なサーボ系 - Google Patents

Abstract

【課題】 直交座標系上でソフトフロート機能をロボットに実現すること。
【解決手段】 直交座標系Σ0 上における位置偏差と設定されたパラメータ（Ｋx ，Ｋy ，Ｋz ）を用い、直交座標系Σ0 上における力（Ｆx ，Ｆy ，Ｆz ）を求める。ステップＳ２で、求められた力（Ｆx，Ｆy，Ｆz）をロボットの姿勢データを用いてツール座標系Σn+1 上のデータ（Ｆx⁽ⁿ⁺¹⁾，Ｆy⁽ⁿ⁺¹⁾，Ｆz⁽ⁿ⁺¹⁾）に変換する。更に、（Ｆx⁽ⁿ⁺¹⁾，Ｆy⁽ⁿ⁺¹⁾，Ｆz⁽ⁿ⁺¹⁾）＝（ｆx⁽ⁿ⁺¹⁾，ｆy⁽ⁿ⁺¹⁾，ｆz⁽ⁿ⁺¹⁾）として、Ｎｅｗｔｏｎ−Ｅｕｌｅｒ法により、各軸のトルクＴiを計算し、トルク指令に採用する（ステップＳ３）。Ｔi ／（Ｋp Ｋv ）から位置ループ入力Ｅi を計算し（ステップＳ４）、位置ループ入力Ｅiを入力とする位置ループ処理、位置ループ出力を入力とする積分処理と速度ループ処理、積分処理と速度ループ処理の出力を加算する処理等を実行しても良い（ステップＳ５）。
【選択図】 図８

Description

本願発明は産業用ロボット（以下、単に「ロボット」と言う。）の制御技術に関し、更に詳しく言えば、ロボット軸駆動用のサーボモータを制御する柔らかさを必要に応じて変更し、可変に指定乃至設定すること（以下、「調節」と言う。）が出来るようにする為の技術に関する。

ロボットのアームを駆動する各軸のサーボモータの制御は、通常、位置制御ループ及び速度制御ループを有するサーボ系によって制御されている。図１は、これを示したブロック図で、符号１は位置ループゲインＫp の項、符号２は速度ループゲインＫv の項である。また符号３，４はモータの伝達関数の項であり、３はトルク定数Ｋｔ、４はイナーシャＪである。更に、符号５はモータ速度ｖを積分してモータ位置ｑを求める伝達関数である。なお、ｓはラプラス演算子を表わしている。

ロボットコントローラの内部で作成される移動指令ｒとモータ位置ｑより位置偏差ｅが算出され、該位置偏差ｅに位置ループゲインＫpを乗じて速度指令ｖcが出力される。更に、速度偏差ｅv が速度指令ｖcとモータ速度ｖより算出され、この速度偏差ｅvに速度ループゲインＫvを乗じてトルク指令ｔcが出力される。そして、該トルク指令ｔc に応じた駆動電流がモータに供給される。なお、速度ループの制御においては、Ｐ制御に代えてＰＩ制御またはＩＰ制御が適用されることもある。

このようなサーボ系（ＰＩ制御もしくはＩＰ制御）によって各軸のモータが制御されるロボットのツール先端点が目標位置に向かって移動中になんらかの障害物に遭遇・接触した場合、ロボットのツール先端点は障害物に抗して目標位置に向かって移動を続行しようとする現象が生じる。

この現象は次のように説明される。即ち、障害物の存在に関わらずサーボモータは目標位置に向かって移動しようとするが、実際には障害物によりその目標位置に移動することが阻まれ位置偏差ｅが増大する。その結果、この位置偏差に位置ループゲインＫpを乗じて得られる速度指令ｖcも増大する。そして、この増大する速度指令ｖcとモータの速度ｖ（障害物に当接しているときには、速度ｖは「０」に近いと考えられる。）との差は、速度ループの持つ積分器によって積分されて増大し、トルク指令ｔc は大きな値となる。

結局、サーボモータは目標位置への移動を実現しようとして最大のトルクを出力するようになり、ロボットの停止やワーク、エンドエフェクタ等の破壊事故（干渉事故）を招く原因となる。そこで、このような不都合を回避する手段として、必要に応じて位置ループゲインＫp及び速度ループゲインＫvを低下させることにより速度指令ｖc及びトルク指令ｔcの値の増大を抑止する方式が用いられている。

この方式においては、通常、柔軟制御用のゲイン値Ｋp'，Ｋv'が予め設定され、柔軟制御指令の入力時にゲインの値がＫp，ＫvからＫp'，Ｋv'に各々切り換えられる。

このような従来方式は、サーボ系の柔らかさを各ロボット軸（以下、単に「各軸」とも言う。）毎に調節することを通して「各軸空間上でのソフトフロート機能」を実現するものである。

例えば、ロボットの手首側の３軸について非常に小さなＫp'，Ｋv'を設定し、手首側の３軸について特に柔らかいサーボ制御が実現される。また、全軸について非常に小さなＫp'，Ｋv'を設定すれば、全軸について非常に柔らかいサーボ系が構成される。なお、各軸の位置ループのゲインや速度ループのゲインを変更可能に構成して、サーボ系の各軸上での柔らかさを変えられるようにすることは、例えば下記特許文献１に開示されている。

特開平６−２０８４０４号公報

しかし、上記した従来方式では、サーボ系の柔らかさは各軸上でしか調節することが出来ないので、作業空間内の方向別に柔らかさを調節し、嵌め合い、バリ取り加工等、実際の作業に適合した柔らかさをロボットに持たせることが困難であった。

例えば、嵌め合いにおいては、一般に、ロボットに把持されたワークの挿入方向に関しては硬く、挿入方向と直交する方向に関しては柔らかいサーボ系による制御が行なわれることが望ましいと考えられるが、作業空間内に定義された座標系上でサーボ系の柔らかさを調節出来ない従来方式ではこのようなニーズには応えられない。
また、バリ取り加工においては、加工線の延在方向とそれに直交する２方向の動作について各々別の柔らかさを持たせることが望ましいが、それをロボットの各軸についてサーボ系の柔らかさを調節する従来方式で実現することも極めて困難である。

そこで本願発明の目的は、作業空間内に定義された直交座標系（以下、単に「直交座標系」とも言う。）上で柔らかさを調節することが出来るサーボ系を提供し、ロボットに直交座標系上でのソフトフロート機能を持たせることにある。また、本願発明は、そのことを通して、ロボットを用いた作業の精度と安全性を向上させることを企図するものである。

本願発明は、ロボットの各軸を駆動するサーボモータを制御する為に各軸毎に位置ループ処理と速度ループ処理を含む処理を行なうサーボ系に、直交座標系上でソフトフロート機能を持たせるようにしたもので、各軸のトルクＴi（ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を、作業空間内に定義された直交座標系上での前記サーボ系の柔らかさを表わすパラメータの設定値の組と前記直交座標系上での位置偏差とに基づいて計算される前記直交座標系上での力から逆動力学で解いて求め、このＴi を前記各軸のトルク指令とすることを特徴とする。
ここで、前記作業空間内に定義された直交座標系上での前記サーボ系の柔らかさを表わすパラメータの組は、画面入力により設定するようにしても良い。

なお、各軸の前記位置ループ処理の入力Ｅiについては、これを上記トルクＴi と、位置ループ処理のゲインＫpと、速度ループ処理のゲインＫvとに基づいて算出して採用しても良い。算出式としては、Ｔ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 側へ向かって各リンクに作用する力／モーメントが計算される。

各記号の意味は次の通りである。
ｎ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をツール座標系として定義することにより、ツール座標系の原点に働く力をツール座標系上で表わしていると考えることが出来る。

そこで、ツール座標系の姿勢を表わす現在データを行列Ｕとし、Ｆ＝（Ｆx，Ｆy，Ｆz）^T＝（Ｆx⁽⁰⁾，Ｆy⁽⁰⁾，Ｆz⁽⁰⁾）^Tとすれば、行列演算ＵＦによって、Ｆx，Ｆy，Ｆzのツール座標系上の表現が計算される。これをＦ⁽ⁿ⁺¹⁾＝（Ｆx⁽ⁿ⁺¹⁾，Ｆy⁽ⁿ⁺¹⁾，Ｆz⁽ⁿ⁺¹⁾）^Tとすれば、ｆn+1⁽ⁿ⁺¹⁾＝Ｆ⁽ⁿ⁺¹⁾となる。

ここで、重力を考慮しない条件；ｇ＝（０，０，０）で上記アルゴリズムを用いて求められた軸ｉのトルクτi（ｇ＝０）をＴiとすれば、その軸のサーボ系は次のように表わされる。

ＫpＫvＥi＝Ｔi ・・・（５）
但し、Ｋpは位置ループゲイン、Ｋvは速度ループゲイン、Ｅiは軸ｉの位置ループの入力を表わしている。図２は、このサーボ系を図１に準じた表記で示したものである（Ｋp，Ｋvは便宜上各軸共通の表記としたが、各軸毎に設定される場合にはＫp，Ｋvに代えてＫpi，Ｋviとすれば良い）。ここでは、位置ループ処理への入力としてＥiが採用されている。

ここで、直交座標系上での力は、各軸のトルクＴiは、直交座標系上でのサーボ系の柔らかさを表わすパラメータの組Ｋx，Ｋy，Ｋzと直交座標系上での位置偏差（ｘd−ｘ），（ｙd−ｙ），（ｚd−ｚ）から計算される。そこで、本発明ではこの直交座標系上での力から逆動力学で解いて求めた各軸のトルクＴiを、各軸のトルク指令として採用することで、直交座標系上で柔らかさが調節可能なサーボ系を提供する。

なお、Ｅiは、この各軸トルクＴiと、位置ループゲインＫp と、速度ループゲインＫv に基づいて算出されたものであり、これを各軸の位置偏差ｅを修正したものと解釈して各軸の位置ループ入力とすることができる。Ｅの算出式は次式（６）のようになる。
Ｅ＝Ｔ／（Ｋ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⁽ⁿ⁺¹⁾，Ｆy⁽ⁿ⁺¹⁾，Ｆz⁽ⁿ⁺¹⁾）に変換する。

更に、（Ｆx⁽ⁿ⁺¹⁾，Ｆy⁽ⁿ⁺¹⁾，Ｆz⁽ⁿ⁺¹⁾）＝（ｆx⁽ⁿ⁺¹⁾，ｆy⁽ⁿ⁺¹⁾，ｆz⁽ⁿ⁺¹⁾）として、作用の欄で説明したＮｅｗｔｏｎ−Ｅｕｌｅｒ法により、各軸のトルクＴiを計算し、各軸のトルク指令とする（ステップＳ３）。但し、Ｔi／（ＫpＫv）から位置ループ入力Ｅi を計算し（ステップＳ４）、計算された位置ループ入力Ｅiを入力とする位置ループ処理、位置ループ出力を入力とする積分処理と速度ループ処理、積分処理と速度ループ処理の出力を加算する処理等を実行しても良い（ステップＳ５）。積分処理と速度ループ処理の出力の加算を行う場合は、設定されたＩSOFT-LIMの下で、前出の（７），（８）式の制限を守った範囲で行うものとする。

これにより、ワークＷ1のガイドピンＧＰが孔ＨＬの上縁部に当接すると、ツール先端点ＴＣＰは孔ＨＬの内壁をなぞるように孔ＨＬの軸線ＷＪ上の点Ｑ（教示位置Ｐ2に対応する位置）まで円滑に移動する。次いで、ロボットハンドを開放して、ワークＷ1 の把持を解けば嵌め合い作業は完了する。

なお、上記実施例では、直交座標系上で柔らかさを表わすパラメータの設定は画面入力で行なったが、動作プログラムの中で指定する方式としても良い。例えば、バリ取りへのアプリケーションにおいて、加工線の延在方向が屈曲している場合に、動作プログラム中でそれに応じたパラメータ変更を行なえば、加工線の延在方向に即した柔らかさの設定が可能となる。

従来のサーボ系の構成を示したブロック図である。 重力を考慮しない場合の本願発明のサーボ系の構成を示したブロック図である。 重力を考慮した場合の本願発明のサーボ系の構成を示したブロック図である。 本願発明の実施例で使用されるロボットコントローラの概要を説明する要部ブロック図である。 本願発明の適用例として考えられる嵌め合い作業について説明する図である。 直交座標系上で柔らかさを入力する為の画面を説明する図である。 図５に示した嵌め合いを実行する為に直交座標系ソフトフロート機能を用いた動作プログラムの一例を表わしたものである。 実施例におけるソフトフロート機能有効化時の処理の概要を説明するフローチャートである。

符号の説明

１ 位置ループゲインの項
２ 速度ループゲインの項
３，４ モータの伝達関数の項
５ モータ速度からモータ位置を求める伝達関数
６重力分を補償する積分項
１０ ホストＣＰＵ
１１ ＲＯＭ
１２ ＲＡＭ
１３ 不揮発性メモリ
１４ 入出力装置
１５ インターフェイス
１６ 共有ＲＡＭ
１７ ディジタルサーボ回路
１８ 帰還レジスタ
１９ バスライン
２０ 教示操作盤
３０ ロボット
４０ 外部装置
ＫＢ キーボード
Ｋp 位置ループゲイン
Ｋv 速度ループゲイン
Ｋt トルク定数
Ｋg 積分ゲイン
ＧＰ ガイドピン
ＨＬ 孔
ＬＣＤ 液晶ディスプレイ
Ｐ1 ，Ｐ2 教示点
ＲＣ ロボットコントローラ
Ｗ1 ，Ｗ2 嵌め合い対象ワーク
ＷＪ 孔の軸線

Claims (2)

1. ロボットの各軸を駆動するサーボモータを制御する為に前記各軸毎に位置ループ処理と速度ループ処理を含む処理を行なうサーボ系であって、
   各軸のトルクＴi（ｉ＝１，２・・・ｎ；但し、ｎはロボットの軸数）を、作業空間内に定義された直交座標系上での前記サーボ系の柔らかさを表わすパラメータの設定値の組と前記直交座標系上での位置偏差とに基づいて計算される前記直交座標系上での力から逆動力学で解いて求め、該Ｔiを前記各軸のトルク指令とした、直交座標系上で柔らかさが調節可能な前記サーボ系。
2. 前記作業空間内に定義された直交座標系上での前記サーボ系の柔らかさを表わすパラメータの組が、画面入力により設定される請求項１に記載された、直交座標系上で柔らかさが調節可能な前記サーボ系。
   

Images