以下、添付図面を参照しつつ、本願発明の好適な実施形態について説明する。なお特に断らない限り、「力」は、ツールとワークとの間に作用する力によってもたらされ、力検出手段によって得られたデータに基づく物理量とする。また「所定」又は「予め定めた」なる語は、関連する処理が実行されるまでに定められればよいという意味で用いられ、いつ定めるかについての特段の制約はないものとする。また、以降の説明や図においては、第1の粗倣い構成動作を粗倣い構成動作1、第2の粗倣い構成動作を粗倣い構成動作2とも記述する。
図1は、本発明に係るロボット制御装置を含むロボットシステムの構成例を示す図である。ロボットシステム10は、ロボット12と、ロボット12の動作、具体的には該ロボットの各軸の位置を制御するロボット制御装置14とを有する。なお図示例のロボットは6自由度の多関節型ロボットであるが、本願発明を実施するに際し6自由度の多関節型ロボットである必要はない。ロボット12のアーム先端には力検出手段としての力センサ16と、ワーク加工等の用途に供される作業ツール18とが取り付けられ、力センサ16はツール18に作用する力を検出する。またツール18が加工すべきワーク20は、例えば作業台22等の所定の位置に配置される。配置された該ワーク20は、移動動作が可能な装置などによって移動させてもよい。
図2は、ロボット制御装置14の処理機能と各機器等の関係を示すブロック図である。ロボット12の各軸の位置を検出するエンコーダ等の各軸位置検出部24からの検出データが、位置、速度、加速度検出・算出部26に送られる。位置、速度、加速度検出・算出部26は、各軸位置検出部24からの所定時間間隔毎の各軸位置データ等に基づき、ロボット12の位置、速度及び加速度を検出・算出する。或いは加速度は、図示しない加速度センサを用いて求めてもよい。
位置、速度、加速度検出・算出部26の処理結果は、作用力算出部28に送られる。作用力算出部28は、力検出手段16で検出した力又はモーメント、位置、速度、加速度検出・算出部26からの位置、速度及び加速度に関するデータ、ロボットの各構成要素の質量・重心位置等のデータ、並びに作業ツール18の進行方向、押付方向に関するデータに基づいて、ツール18とワーク20との間に作用する作用力、さらには、ツール18とワーク20との間に作用する作用力のうち、所定方向の成分を算出することができる。
作用力算出部28での算出に使用されるパラメータは、第1のパラメータ格納部30及び第2のパラメータ格納部32に格納することができる。図2の例では、第1のパラメータ格納部30には、後述する基本移動動作、粗倣い動作を行うための動作パラメータが格納される。一方第2のパラメータ格納部32には、力センサがロボット手首に設けられ、その先にツール又はワークが取り付けられている場合に、ツールとワークとの間に作用する正味の力以外で、該力センサに作用する力を算出するために必要なパラメータが格納されており、例えばロボットの各構成要素の質量や重心位置のデータが格納されている。また特に示していないが、後述する算出方法において必要となるパラメータも、ロボット制御装置内部に格納することができる。
動作切り替え判定・動作パラメータ調整部34は、ロボットの位置・速度・加速度と、ツールとワークとの間に作用する作用力、又は該作用力のうちの所定方向の成分と、基本移動動作、粗倣い構成動作1、粗倣い構成動作2のうち、現在行われている動作と、基本移動動作のパラメータ及び粗倣い動作のパラメータに基づいて、動作切り替えの判定を行うとともに、場合によっては第1のパラメータ格納部30内のパラメータを変更し、基準進行方向及び基準押付方向等をもとに、指令演算部36に対して、行うべき動作を指定する動作指令を作成する機能を備える。各動作の詳細については後述する。
指令演算部36は、ロボットの位置・速度・加速度と、ツールとワークとの間に作用する作用力又は該作用力のうちの所定方向の成分と、動作切り替え判定・動作パラメータ調整部34による動作指令に基づいて、ロボット12に送る指令を演算する。なお本実施形態では、粗倣い動作実行手段、動作調整手段及び教示データ生成手段の各機能は、指令演算部36が担うものとする。
なお基準進行方向、基準押付方向算出・更新部38は、第1のパラメータ格納部30に格納されているデータと、位置、速度、加速度検出・算出部26の検出・算出結果とに基づいて、必要に応じて、ワークに対するツールの基準進行方向及び基準押付方向(後述)を算出し又は更新する。算出され又は更新された基準進行方向及び基準押付方向は、作用力算出部28での処理、及び、粗倣い構成動作1又は粗倣い構成動作2の移動動作のパラメータの算出や調整等に使用される。
本実施形態における力検出手段16は、X、Y、Z軸方向の並進力及び各軸回りのモーメントを検出できる力センサとする。該力センサは、ロボット12の手首と作業ツール18との間に配置される。好適な力センサは6軸の力センサであるが、目的とする方向に作用する力を検出できる力センサであれば、検出できる力及びモーメントの自由度や検出原理等は問われず、どのようなタイプの力センサであってもよい。また、力センサはロボット12の手首部及びロボット12が載置されている床面のいずれに設置されてもよく、要するにツール18とワーク20との間に作用する力を算出できるのであれば、その設置場所に制約はない。さらに、力センサを用いずに、ロボット12を駆動するアクチュエータの電流値に基づいて力を推定する手段を力検出手段16として用いることも可能である。
次に、ロボット制御装置14からロボット12に対して指令される各動作について説明する。先ず基本移動動作は、ロボット12を用いて、ツール18とワーク20とを相対的に移動させる動作であり、具体的には以下の動作が挙げられる。
(1)予め教示された位置等に基づいて指定された軌道に沿って動作
(2)ツールとワークとの間に作用する力が目標値と一致するように力制御を行いながら、ワークの形状に沿ってツールを倣わせる動作
ここで(2)における力制御としては、以下のような具体的方法が考えられる。
(2a)教示軌道によって与えられる各位置における目標進行方向が、該教示軌道における進行ベクトルによって与えられており、該位置を参考として力制御を行う
(2b)教示軌道を中心にばね成分を持たせた押付制御を行いながら、該教示軌道に基づいて力制御を行う
(2c)ツールとワークとの間に作用する力、及び移動中のツール又はワークの位置に基づいて、ワークの形状を推定しながら力制御を行う
上記以外の方法であっても、ロボットを用いてツールとワークとを相対的に移動させる動作であれば、本発明の趣旨から外れないことは当業者には明らかである。
次に、粗倣い動作について説明する。粗倣い動作は、上述の粗倣い構成動作1及び粗倣い構成動作2から構成される。粗倣い構成動作1は、ツール及びワークの一方を他方から、各時点の位置よりも(通常は局所的に)遠ざける動作であって、予め定めた移動時間又は予め定めた移動距離を限度にツール又はワークを移動させる動作である。また粗倣い構成動作2は、ロボットが移動させている一方に対して他方の存在を確認しようとする動作であり、多くの場合、ツール及びワークの一方を他方から、各時点の位置よりも(通常は局所的に)近付ける動作であって、予め定めた移動時間又は予め定めた移動距離を限度にツール又はワークを移動させる動作である。
詳細は後述するが、粗倣い動作実行中は、力検出手段で検出する力の情報、或いは又はこれに加えて、ロボットが動かしている物体の粗倣い動作実行中の位置情報、基準進行方向及び基準押付方向に基づいて、粗倣い構成動作1又は粗倣い構成動作2の移動方向、移動速度、移動時間及び移動距離の少なくとも1つを、必要に応じて変化させながら、ワーク上の異物や障害物等に対してツールを適切に移動させることができる。
上述の基準進行方向及び基準押付方向とは、粗倣い動作実行中のある位置において、粗倣い構成動作1又は粗倣い構成動作2を行うときに、その移動方向を定めるために基準とする方向である。すなわち基準進行方向とは、予め又は動作実行中に定められる方向であって、粗倣い動作実行中に基準とする進行方向であり、より具体的には、粗倣い動作実行中にツールを進ませるために参考にする(通常は移動方向に概ね一致)方向である。一方基準押付方向とは、予め又は動作実行中に定められる方向であって、粗倣い動作実行中に基準とする押付方向であり、より具体的には、粗倣い動作実行中にワークに対して押付動作をするために参考にする(通常は押付方向に概ね一致)方向であり、上記基準進行方向に基づいてある領域を通過する際の押付方向である。但し、上記基本移動動作が位置制御に基づくものである場合は、該基準押付方向は教示軌道に近付く方向とする。
以下、図3及び図4を参照して、上記基準進行方向及び基準押付方向の定め方の具体例を説明する。
図3に示すように、ワーク20の表面に沿ってツール18を右方から左方へ移動させる場合に、ワーク20に予期せぬ凹凸形状、すなわち凸状部分40及び凹状部分42が存在するとする。この場合、凸状部分40はツールの通過動作の障害となるが、基準進行方向は、矢印44で示すように凸状部分40が存在しない(ワーク形状は破線のように平坦である)とした場合の基準進行方向(予め教示軌道等によって与えられる移動方向)を適用し、一方基準押付方向は、矢印46で示すように、基準進行方向44に垂直かつワークに向かう方向とする。
一方、凹状部分42についても、基準進行方向は、矢印48で示すように凹状部分42が存在しない(ワーク形状は破線のように平坦である)とした場合の基準進行方向(予め教示軌道等によって与えられる移動方向)を適用し、一方基準押付方向は、矢印50で示すように、基準進行方向48に垂直かつワークに向かう方向とする。
他の例として、図4のようにワーク20が凹面状部分52を有し、凹面状部分52に予期せぬ凸状部分54が存在する場合を考える。この場合も、凸状部分54はツールの通過動作の障害となるが、基準進行方向は、矢印56で示すように凸状部分54が存在しない(ワーク形状は破線のような滑らかな凹面形状である)とした場合の基準進行方向(予め教示軌道等によって与えられる移動方向)を適用し、一方基準押付方向は、矢印58で示すように、基準進行方向56に垂直かつワークに向かう方向とする。
図4のように教示軌道56が曲がっていて,教示軌道上の位置に対応する進行方向及び押付方向が算出できる場合は、粗倣い動作実行中の位置から教示軌道に対して直交するように、或いは最短距離となる方向に線を引いたときの交点に対応する、進行方向や押付方向を用いてもよい。或いは押付方向として、対応する教示された進行方向をもとに算出した方向を用いてもよい。
このように、基準進行方向は、ワークに対してツールがある領域を通過する際に大局的に進ませたい方向とする。具体的には、障害となる部分を通過する方向、予期せぬ大きな凹凸形状部分を通過する方向、障害物がないとした場合に教示軌道によって与えられる移動方向である。また基準押付方向は、上記基準進行方向に基づいて、障害物がないとした場合に教示軌道に基づいて倣う場合のツールの押付方向、障害となる形状や予期せぬ大きな凹凸形状部分を通過する際の押付方向となる。但し、上記基本移動動作が位置制御である場合、基準押付方向は教示軌道に近付く方向とする。
図3及び図4は基準進行方向及び基準押付方向の定め方の一例であり、故に基準進行方向及び基準押付方向は、一連の作業を通して設定された方向等の、予め設定された方向を使用してもよい。
また基準進行方向及び基準押付方向は、基本移動動作から粗倣い動作に切り替えたときに予め設定されていた教示データによって与えられる方向としてもよい。
或いは基準進行方向及び基準押付方向は、基本移動動作から粗倣い動作に切り替えたときに実際に移動していた位置に基づいて算出した進行方向と、該進行方向に基づいて算出される押付方向としてもよい。
さらに或いは基準進行方向及び基準押付方向は、粗倣い動作中に更新された値を使用して求めてもよい。上述のように粗倣い動作の開始時(基本移動動作からの切替時)の進行方向及び押付方向に基づいて基準進行方向及び基準押付方向を設定した場合、障害となる形状が大きいと、参考とするツールの進行方向及び押付方向が、ワーク形状に合ったものでなくなることがある。このような場合には、基準進行方向及び基準押付方向を、粗倣い動作実行中に、教示軌道に基づいてワーク形状に沿った方向に更新することが望ましい。
本発明の主たる特徴は、ロボットを用いてツールとワークとを相対的に移動させているときに、障害物、壁、穴、大きな凹凸等の未知の形状を検出した場合、基本移動動作と粗倣い動作とを、力検出手段で検出する力の情報、或いは又はこれに加えて、ロボットが動かしている物体の位置情報に基づいて適宜切り替え、ロボット、作業ツール及びワークに対して過負荷を与えず、なるべく形状に沿って、必要に応じて回避しながら動かすことを迅速に行うことにある。なお粗倣い動作は、力や位置等に基づき、ワーク形状に応じて積極的にツール又はワークの一方を他方に対して現在の位置から局所的に遠ざける動作と、一方が他方の存在を確認する動作とを繰り返して行う、倣い動作を意味する。
但し、ワーク(他方)の形状が未知である場合、ある時点での移動動作が、他方の存在を確認するために近付く動作であるか離れる動作であるかは判別できない。そこで、一方が局所的に他方から離れることを目的とする動作を粗倣い構成動作1と称し、この動作の実行中には、基準進行方向及び基準押付方向に基づいて移動し、力検出手段を用いて検出された力によって移動方向が不適切であると判断された場合は移動方向等を変更しながら、一方が予め定めた(実行するまでに定められさえすればよい)時間又は距離を限度に、もとの位置から離れるような動作が行われる。また一方が他方の存在を確認することを目的とする粗倣い構成動作2では、障害となる物体を通過する際に基準となる基準進行方向及び基準押付方向に基づき、一方を他方が存在する方向に進ませるような動作が行われる。
以下、基本移動動作と粗倣い動作との切り替えの具体例について説明するが、基本移動動作には、力制御に基づくものと位置制御に基づくものとがあるので、先ずその両者に分けて説明を行う。基本移動動作が力制御に基づくものである場合、上記基本移動動作は、力制御によって押付力を制御しながら、ツールとワークとの間に作用する力が目標値(動作中に調整可能)となるようにツールをワーク形状に倣わせる動作であるとする。教示点に基づく教示軌道によって、力制御で倣わせるときに参考とする位置が与えられており、ある位置における進行方向は教示軌道によって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行うこともできる。また粗倣い動作は、ワーク形状に応じて積極的に、ツール又はワークの一方を現在の位置から遠ざける動作(局所的又は相対的に他方から離す動作)と、一方が他方の存在を確認する(通常は一方を他方に近付ける)動作とを繰り返す倣い動作である。また力制御に基づく基本移動動作を行っているときに粗倣い動作を行うべき場合としては、押付力を適宜調整しながらロボットを動かしているときに、障害物、壁、大きな凸凹等の、それまで倣ってきた形状とは異なり、適切に倣わせることが難しい形状物が存在し、それまでの力制御による倣い方法とは異なる方法に切り替えることが必要な場合が考えられる。
一方基本移動動作が位置制御に基づくものである場合は、上記基本移動動作はロボットを用いて作業ツールとワークとを相対移動させるにあたり、予め定めた教示点に基づき設定された教示経路に沿って作業ツールを移動させることを目的とする移動動作である。粗倣い動作については力制御の場合と同様である。また位置制御に基づく基本移動動作を行っているときに粗倣い動作を行うべき場合としては、以下のような場合が考えられる。
(1)ワークに対してツールを位置制御で動かし、バリ取り、研削、研磨作業等を行う場合。軌道を少しずつずらした位置制御を複数回行い、目的の作業を達成する場合もある。
(2)ワークの表面・形状状態や、ワークに存在する異物などを確認する検査等において、予め定めた範囲内でセンサ等(前記力検出手段に取り付けられた接触確認用の作業ツール等)の移動を行い、異物等の未知形状物があった場合に該未知形状になるべく沿ってセンサ等を動かしたい場合。
(3)ロボットの手先部を動かしているときに意図しない障害物があった場合、なるべく該障害物の形状に沿って該手先部を動かしたい場合。
図5は、力制御による倣い動作(基本移動動作)を説明する図である。教示点60、62に基づく教示軌道によってワーク20に対するツール(図示せず)の進行方向が定められ、該進行方向に合わせて教示押付方向を決める倣い動作が行われる。目標進行方向64に沿ってツール進めながら目標押付方向66に対して力制御を行っているときに、軌道上に教示軌道とは異なる形状(例えば図5に示すような山形状の突起)が存在すると、該突起の表面が比較的滑らかであれば、押付方向66について反力68がかかるため好適な倣いが可能となることもある。しかし、押付方向成分に目標力がかかっているときに、その部分の傾斜角度や目標力によっては、進行方向(ここでは実際の位置に基づく進行方向ではなく、教示軌道によって与えられる進行方向を意味する。この場合、基準進行方向と同じ方向を意味しており、以降の説明においても、進行方向は、特に断わらない限り、同様の意味で使用する。)について過大な反力70がかかり、特に進行方向の速度が速いときは好適な倣いができないことがある。このような場合、力に応じて進行速度を下げたり、目標押付力を下げたり、力制御の追従性能を向上させたりする等の工夫が必要であり、異物の形状によってはそれでも対応できない場合がある。
そこで本発明では、図6に示すように、ツール18をワーク20に対して目標力で押し付ける基本移動動作を実行しているときに、進行方向について閾値を超える力がかかった場合、上述の基準進行方向及び基準押付方向に基づき、粗倣い構成動作1と、粗倣い構成動作2と、それらの動作切り替え判定とを繰り返す(切り替えをしない場合もあり、また順序は問わない)。つまりツール18をワーク20から離す動作(矢印72)と、ワーク形状を確認する動作(矢印74)とを適宜切り替えて反復し、基準進行方向44に障害物がないことが確認できたら、もとの基本移動動作に切り替えることによって、大きさや形状が未知な障害物に対しても、該障害物を回避しつつもなるべく該障害物に沿った移動が可能となる。また上述の動作切り替えをより細かく繰り返せば、ワーク形状により正確に沿った移動が可能となる。なお以降の説明において、基準進行方向は実線矢印44、基準押付方向は破線矢印46で表すものとする。
本発明によれば、図7(a)に示すようにツールの移動方向が基準進行方向に対して90度以上変更させられる場合にも対応でき、また図7(b)に示すようにコーナー部の倣い動作において、教示点が2点のみであっても好適に倣うことができる。
この場合の粗倣い動作の動作方法としては、種々考えられるが、粗倣い構成動作1及び粗倣い構成動作2の移動方向、移動速度及び動作時間を状況に応じて変えたり、粗倣い構成動作1、粗倣い構成動作2及び基本移動動作相互間での切り替えを適切に行ったりすることによって、状況に対応したより複雑な動作を実現できる。
次に、粗倣い構成動作1及び粗倣い構成動作2それぞれにおける移動方向の求め方の具体例を説明する。先ず基本移動動作から、粗倣い動作に切り替えたときの進行方向及び押付方向を、それぞれ基準進行方向及び基準押付方向として設定する。ここで、動作中の各位置に対応する教示軌道上の位置(例えば、その位置から最短距離にある教示軌道上の位置)で与えられる進行方向及び押付方向を、それぞれ基準進行方向及び基準押付方向としてもよい。
粗倣い構成動作1及び粗倣い構成動作2の移動方向は、基準進行方向及び基準押付方向の単位ベクトルに、実数(±の値、ゼロも含む)である係数をかけて、合成和をとることにより求める。具体的には、図8に示すように、粗倣い動作に切り替えたときの基準進行方向の単位ベクトルをDrm、基準押付方向の単位ベクトルをDrpとするとき、実数である係数αm1、βp1、αm2及びβp2を用いて、粗倣い構成動作1を実行するときの移動方向Dr1は、(αm1 Drm + βp1 Drp)を正規化したベクトルとする。一方、粗倣い構成動作2を実行するときの移動方向Dr2は、(αm2 Drm + βp2 Drp)を正規化したベクトルとする。なお、Dr1とDr2とは互いに異なるものとし、また互いに反対方向の関係にもならないようにする。なお粗倣い構成動作2では、ツールがワークに接触するまで行わなくともよく、例えば接触前に粗倣い構成動作1に切り替えてもよい。
上述の係数は、ツールがワークに衝突したときは、そのときの反力に基づいて適切な移動方向を求めるべく調整されたり、粗倣い動作における粗倣い構成動作1又は粗倣い構成動作2の繰り返し動作での、粗倣い構成動作1又は粗倣い構成動作2の開始位置の変化量が小さいとき、移動方向を適切な方向にずらすように調整されたりすることが好ましい。また粗倣い構成動作1及び粗倣い構成動作2による粗倣い動作は、複数の直線的な動きを滑らかにつなげた移動や曲線的な移動を含むものとし、全体的により滑らかな動作とすることもできる。さらに、後述するような調整をおこなった場合、該調整をもとに、移動方向を算出するためのパラメータや基準進行方向、基準押付方向を変更し、移動方向を算出してもよい。
本発明の実施形態においては、粗倣い構成動作1及び粗倣い構成動作2の移動方向ベクトルは、上述のように基準進行方向ベクトルと基準押付方向ベクトルの線形結合和により求められる。それぞれのベクトルにかける係数は、ツールとワークとの間に作用する力や、ツール又はワークの粗倣い動作中の位置に基づき、状況に応じて変更可能であり、粗倣い構成動作1及び粗倣い構成動作2のそれぞれにおいて、適切な方向となるように係数を変えるものとする。また粗倣い動作中の基準進行方向ベクトル及び基準押付方向ベクトルは、粗倣い動作に切り替えたときの進行方向及び押付方向を使用するものとする。但し、問題となる部分を通過することができる方向であればその方向を使用してもよい。
粗倣い構成動作1、粗倣い構成動作2及び基本移動動作相互の切り替え方法としては、次のようなものが考えられる。
先ず図9に示すように、矢印72で示す粗倣い構成動作1の実行中に、予め定めた閾値を超える力を検出せず、かつ、予め定めた移動時間が経過し又は予め定めた移動距離だけ移動したら、動作を矢印74で示す粗倣い構成動作2に切り替える。
図10(a)に示すように、矢印72aで示す粗倣い構成動作1の実行中に、予め定めた閾値を超える力が検出され、すなわちツールがワーク又は他の障害物に衝突したが、ツールが比較的狭い領域内にあってワークから離すために所定時間又は所定距離だけ動かすことができない場合は、ワークに対するツールの移動方向を変更して、再度、矢印72bで示すような粗倣い構成動作1を実行するか、ツールをワークに対して停止させることができる。図10(b)は同様の考え方により、矢印72aで示す粗倣い構成動作1から矢印72bで示す粗倣い構成動作1、さらに矢印72cで示す粗倣い構成動作1に切り替えられていく様子を示している。
ここで粗倣い構成動作1の実行時に、予め定めた閾値を超える力を検出することなく所定時間又は所定距離だけツールを移動させることができれば、粗倣い構成動作2に切り替えてもよい。また次回以降の粗倣い構成動作1では、矢印72b、72cで示すような変更された粗倣い構成動作1を用いてもよい。このようにすれば、実際のワーク形状に対して粗倣い構成動作1の移動方向が不適切であっても、所望の方向に所定時間又は所定距離だけツールを移動させることができる。
図11に示すように、矢印74で示す粗倣い構成動作2の実行中に、予め定めた閾値を超える力が検出され、すなわちツールがワーク又は他の障害物に衝突したときは、動作を矢印72で示す粗倣い構成動作1に切り替える。
図12に示すように、矢印74aで示す粗倣い構成動作2の実行後に、予め定めた閾値を超える力が検出されず、すなわちワーク又は障害等となる形状が確認できないとき、矢印74bで示すように移動方向を変更し、粗倣い構成動作2を実行してもよい。これにより、粗倣い構成動作2によるワーク又は障害物等の確認が不十分であることを防止できる。
図13に示すように、矢印74で示す粗倣い構成動作2の実行中に、予め定めた閾値を超える力が検出されず、すなわちワーク又は障害物等となる形状が確認できないときに、矢印76で示す基本移動動作を再開してもよい。なお図13の基本移動動作のように、力制御による倣いでは、ツールを押付方向に動かしワークとの接触を確認するまでは進行方向に進ませないか、或いは押付力に応じて移動速度を変化させると、ワークとツールとが接触しない領域を小さくすることができる。
図14に示すように、矢印74で示す粗倣い構成動作2の実行中に、基準進行方向44について予め定めた閾値を超える力が検出されず、一方基準押付方向46については予め定めた閾値を超える力が検出された場合は、ワークは存在するが、その時点から基本移動動作で倣うことが可能であると判断されるので、矢印76で示すような基本移動動作を再開してもよい。
図15に示すように、矢印72で示す粗倣い構成動作1の実行中に、基準押付方向46について予め定めた閾値を超える力(基準押付方向の逆方向からかかる力)が検出され、基本移動動作を行うためにツールが倣えるような形状の存在が確認されたときは、基本移動動作76を再開してもよい。
図16に示すように、矢印72aで示す粗倣い構成動作1の実行中に、基準押付方向について予め定めた閾値を超える力(基準押付方向の逆方向からかかる力)は検出されないが、それ以外の方向について別の予め定めた閾値を超える力が検出され、すなわちツールとワーク又は障害物との衝突が確認されたときは、ツールの移動方向を変えて、再度、矢印72bで示す粗倣い構成動作1を実行するか、或いはツールを停止させてもよい。再度の粗倣い構成動作1(72b)の後は、粗倣い構成動作2及び粗倣い構成動作1を繰り返している。なお図16(a)はツールが連続しない他のワーク又は障害物に接触した場合であり、図16(b)は連続するワークの他の部分に接触した場合である。
なお図16の例は、矢印72aで示した粗倣い構成動作1の実行時において衝突したワーク又は障害物にツールを倣わせたくない場合を示しているが、該衝突以降、粗倣い動作で該衝突した部分の形状に倣わせたいときであって、さらに、粗倣い構成動作1の実行時に、基準押付方向について予め定めた閾値を超える力(基準押付方向の逆方向からかかる力)は検出されないが、それ以外の方向について別の予め定めた閾値を超える力が検出され、ワークに衝突したことが確認されたときは、図16(b)において破線矢印で示すように、後述する基準進行方向に山形状の突起等の障害物が存在しているときの粗倣い動作に切り替え、ツールが障害物を乗り越えるような粗い動作を行ってもよい。なおこの場合において、力を検出したときの位置が粗倣い構成動作1の開始位置から予め定めた閾値を超えて離れているときは、粗倣い動作を停止し、障害物となる形状を越えるような粗倣い動作をさせてもよい。
図17に示すように、矢印74で示す粗倣い構成動作2の実行中に、基準押付方向について予め定めた閾値を超える力(基準押付方向の逆方向からかかる力)が検出され、基本移動動作を行うためにツールが倣えるような形状の存在が確認されたときは、基本移動動作76に切り替え(再開)してもよい。
図18に示すように、矢印74で示す粗倣い構成動作2の実行中に予め定めた閾値を超える力が検出されず、所定の移動時間又は所定の移動距離を限度にツールが移動したら、ツールがワークに接触していなくとも、矢印72で示す粗倣い構成動作1を実行することができる。図18の例は、粗倣い動作においてツールを必要以上に基準進行方向と逆方向に進ませずに、ワーク形状に対して粗く倣わせたい場合に有効である。なお1回の粗倣い構成動作2において、基準進行方向と逆方向にどの程度を限度としてツールをワークに近付けるかは、各パラメータによって調整可能である。
図19に示すように、矢印74で示す粗倣い構成動作2の実行中に、予め定めた閾値を超える力(基準押付方向の逆方向からかかる力)は検出されないが、それ以外の方向について、別の予め定めた閾値を超える力が検出され、すなわちツールがワークに衝突してワーク形状の存在が確認されたときは、矢印72で示す粗倣い構成動作1を実行することができる。
図20に示すように、矢印74aで示す粗倣い構成動作2の実行後に、ツールの移動方向、移動距離又は移動時間を変えて、再度、矢印74bで示すような粗倣い構成動作2を実行してもよい。これによって、粗倣い構成動作2において、ツールがより確実にワーク形状に沿う(接触する)ようにすることができる。換言すれば矢印74bで示す粗倣い構成動作2は、ツールがワークに接触するように、矢印74aで示す粗倣い構成動作2の実行後に、移動方向、移動距離、移動時間又は移動速度を変えるものである。
図21に示すように、図20のようにツールをワークに接触させずとも、矢印74aで示す粗倣い構成動作2を実行後に移動方向、移動距離、移動時間又は移動速度を変えて、矢印74bで示す粗倣い構成動作2を実行し、ワーク接触前に矢印72で示す粗倣い構成動作1を実行し、その後再び、矢印74a、74bで示す粗倣い構成動作2を行って、ツール移動方向を基準進行方向44の逆方向にずらし、ワーク側に近付けていくようにしてもよい。
図22に示すような窪み形状のワークにツールを倣わせる場合において、図22(a)の左上方に示すように、矢印72aで示す粗倣い構成動作1の移動方向にツールを動かすとワークに接触し、粗倣い構成動作1を所定時間又は所定距離だけ行うことができず、故に反力を受けた方向に基づいて粗倣い構成動作1の移動方向をずらして、再度、矢印72bで示す粗倣い構成動作1を行っている場合を考える。
このとき、図22(b)に示すように、矢印72bで示す粗倣い構成動作1に続く粗倣い構成動作2(矢印74)によってツールを基準進行方向に移動させると、ずっとその位置付近での移動を繰り返す状態に陥ることがある。
そこで、粗倣い構成動作1を繰り返し実行した後の位置が、基準押付方向46と逆方向の位置について、その時点までで該逆方向に最も進んだ位置より手前の位置、又は、該最も進んだ位置から所定距離を超えて該逆方向の位置に存在しないなら、ツールの移動方向を、基準押付方向の逆方向かつ基準進行方向の逆方向にずらし、矢印72cで示すような粗倣い構成動作1を行う。これにより、図22のような窪み部分等において、ツールがある領域内での移動を繰り返して該領域から脱出できないという状況を避けることができる。
なお図22(a)を用いて説明した方法を用いてもなお、粗倣い動作実行中においてある領域内での移動を繰り返す状態に陥っていると判断された場合は、以下のような処置を採ることができる。
(1)基準押付方向46と逆方向(図22では上向)に最も進んだ位置と比較するときの判定距離を大きくする。
(2)粗倣い構成動作1及び粗倣い構成動作2の移動時間又は移動距離を大きくする。
(3)移動中の位置をもとに、ツールの停滞状況が解消されるように粗倣い構成動作1又は粗倣い構成動作2の移動方向を変える。
図23に示すように、矢印72で示す粗倣い構成動作1の実行時に、押付方向成分の力が小さいため、基準押付方向について予め定めた閾値を超える力は検出されないが、基準進行方向について閾値を超える力が検出された場合、以下の方法が考えられる。
(1)矢印74で示す粗倣い構成動作2に切り替え、ツールを基準進行方向と逆方向に移動させる。
(2)又は、ワークからの反力の大きさ、方向に基づき、基準進行方向とは逆方向の成分を持つ方向にツールを移動させる粗倣い構成動作1を行う。移動方向を変えた粗倣い構成動作1によって、ツールがワークと接触し、基準進行方向と逆方向について閾値を超える力が検出されたら、粗倣い構成動作1の移動方向を基準進行方向の成分を含む方向に変更し、移動時間又は移動距離を小さくする。
或いは、元の基本移動動作に切り替え、そのときの力をもとに、直ちに基準進行方向に障害となる形状がある(そのときの位置から基準進行方向に山形状の突起がある)場合に対応した、粗倣い動作に切り替えてもよい。
基準押付方向について予め定めた閾値を超える力(基準押付方向の逆方向からかかる力)が検出されず、かつ、基準進行方向について予め定めた閾値を超える力(基準進行方向の逆方向からかかる力)も検出されず、かつ、別の予め定めた閾値を超える反力が検出された場合、粗倣い構成動作1の移動する向きを該反力をもとに変え、粗倣い構成動作1を実行することが考えられる。例えば、図24に示すように、矢印72aで示す粗倣い構成動作1の実行時に、ツールが本来倣うべきワーク20aから離れ、他のワーク又は障害物20bに接触して、基準押付方向の逆方向について予め定めた閾値を超える反力(基準押付方向からかかる力)が検出された場合、該反力の方向、大きさをもとに、ツールの移動方向を変え、矢印72bで示すような粗倣い構成動作1を行う。また粗倣い構成動作1の実行時に、ツールがワークと接触することなく所定時間又は所定距離動くことができたときは、粗倣い構成動作2に切り替えてもよい。このように動作を切り替えることによって、粗倣い構成動作1の実行中に、実際のワーク形状に応じた動きをさせることができる。
図25に示すように、粗倣い動作実行中において粗倣い構成動作2を行っているときに、ツールとワークとの間に作用する力及びツールの位置をもとに、ワークになるべく近付け又は接触させてワーク形状を確認できるように動作を切り替えることができる。具体的には、粗倣い構成動作2の実行時に、所定時間又は所定距離だけツールを移動させてもツールがワークに接触しない場合、基準進行方向の逆方向への移動距離を大きくするために、先ず全体的な変更許容値と1回動作当たりの変更許容値を決めておき、粗倣い構成動作2の実行時の移動距離、移動時間又は移動速度を大きくする。但し、移動速度を過大にすると衝突する際の力が大きくなるので、移動速度はあまり大きくしないことが望ましい。
図25(a)は、矢印74a、74b及び74cで示すように、粗倣い構成動作2の移動距離を、ある制限範囲内で徐々に大きくしている様子を示している。一方図25(b)は、矢印74a、74b及び74cで示すように、粗倣い構成動作2の移動距離を、ある制限範囲内で徐々に大きくするとともに、移動方向を基準進行方向とは逆方向に少しずつ変化させている様子を示している。このようにすれば、粗倣い構成動作2の実行時に、ツールがワークに接触しない場合に、ツールをワークに接触するように移動させ、よりワーク形状に沿って動かすことができる。
基準押付方向にワークがあることが確認できるまで図25のような動作を続けることにより、基準押付方向に押し付けるような形状がない(そのときの位置から基準進行方向に谷がある)場合に、大きさや形状が未知のワーク又は障害物に対しても、迅速かつ、なるべくツールをワーク又は障害物の形状に倣わせて動かすことができる。
粗倣い動作において、粗倣い構成動作1や粗倣い構成動作2を繰り返し実行しても、ツールの位置があまり変わらず停滞している場合や、ツールの進み方が遅い場合に、粗倣い動作の動きを調整してこれらの問題を解消することができる。以下、具体的に説明する。
先ず、粗倣い動作時に粗倣い構成動作1又は粗倣い構成動作2を、ある回数以上又は所定時間以上繰り返し実行したときに、ツールが移動した位置やその変化を監視する。例えば、粗倣い構成動作1の実行中の開始位置又は粗倣い構成動作2の実行中の開始位置をもとに、粗倣い動作の開始時又はある時点の位置に対する位置の変化や、粗倣い動作実行中に通過したことのある位置との変化量(通過したことのある位置と予め定めた閾値とから領域を求め、その中に入っていないか)や、ある位置を基準としてある回数実行又はある時間経過したときの距離や、ある位置を基準としてある回数実行したとき又はある時間経過したときの移動距離の変化の様子等を監視する。その監視結果から、ツールが同じような位置付近に戻ってきていたり、図22で示したように同じような位置で停滞していたり、ある時間間隔で比較したときの位置の変化量が小さかったりしたときは、動作を変える必要があると判断する。なおこれらの原因としては、粗倣い動作のパラメータが不適切であることや、ワーク形状が複雑であることが考えられる。
一例として、図26(a)に示すように、ワークに対するツールの進み方が遅いと判断される場合、粗倣い構成動作1及び粗倣い構成動作2の一方又は双方の移動方向を変えることが考えられる。なお図26(b)に示す例では、矢印72で示す粗倣い構成動作1の移動方向を変えている。或いは、粗倣い構成動作1及び粗倣い構成動作2の一方又は双方において、予め定めた制限値内で、ツールの移動時間、移動速度又は移動距離を大きくすることも考えられる。このようにすれば、ツールが同じ位置付近からあまり動けていないときや進み方が遅いときに、その停滞状況から脱出したり、ツールがワーク形状に倣う時間を短くしたりすることができる。但し、ワーク形状への倣い方が粗過ぎないようにしたい場合は、そのように上記制限値を調整しておけばよい。
本発明では、粗倣い動作によってツールがワークに対して意図しない方向に移動し続けた場合に、安全のため、ロボットの動作を停止させることができる。ここで「意図しない方向に移動し続けている」状況としては、例えば以下のような状況が考えられる。
(1)ツールが全く予期せぬワーク形状にずっと倣い続けてしまっている。
(2)ツールがある部分に嵌り込んでしまっている。すなわち、移動方向や移動量等を変えても、その位置から動くことができない。
(3)動作パラメータが不適切であり、ある部分をツールが適切に回避して動くことができない。
上述の「意図しない方向に移動し続けている」状況を判定する具体的方法としては、粗倣い動作時に以下のことを検出又は確認することが考えられる。
(1)粗倣い構成動作1又は粗倣い構成動作2を、ある回数以上繰り返している。
(2)粗倣い動作の実行時間が、所定時間を超えている。
(3)粗倣い動作の移動位置が、想定される位置から離れすぎている。
(4)基本移動動作から粗倣い動作へ切り替えたときの位置から現在のツールの位置までの距離又は総移動距離が、所定距離を超えている。
(5)基本移動動作が教示軌道に基づく動作である場合に、ツールが、教示軌道からの最短距離が閾値以上の位置にいる。
上記のような状況が検出又は確認されたら、ツールが「意図しない方向に移動し続けている」と判断し、安全のため、ロボットの動作を止めることが好ましい。これにより、粗倣い動作によって想定外の動作をした場合の危険な状況を回避できる。
本発明では、粗倣い動作時において粗倣い構成動作1又は粗倣い構成動作2の実行中に、ワークに対するツールの移動速度が大きく、ツールとワークとが過大な力で接触するような場合、移動速度を下げる(調整する)ことによって過大な力で衝突しないようにし、ツールやワークの破損等を回避することができる。さらに、最初は移動速度をある程度小さな値に設定し、回避時間を短くするために移動速度を少しずつ大きな値に変え、反力がある程度以上になったら速度を下げ、その後、その速度で動かすようにして速度を適切な値に調整するという、ツール及びワークの状況に応じた適切な速度の自動調整を行うこともできる。
本発明では、ツールをワーク形状に沿って動かしたときの動作をもとに、教示データを生成又は修正することができる。具体的には、粗倣い構成動作1及び粗倣い構成動作2によって、ワークに対してツールを接離させる動作(接触が望ましいが接触していなくともよい)を繰り返して動かすとした場合、ツールとワークとの離隔と接触とが繰り返されるため、その動作ピッチを短くしたとしても粗倣い動作はジグザグな動きをすることになる。そこで、教示データの生成を行うときは、ロボット制御装置の、制御周期毎に計測した、粗倣い構成動作1及び粗倣い構成動作2、或いは又はこれに加え、基本移動動作の位置に基づいて、例えば以下のような方法を用いて、所定の速度指令で動かす教示データを生成し又は教示軌道を修正することができる。
(1)粗倣い動作時には、粗倣い構成動作1及び粗倣い構成動作2の実行中に力検出手段によってツールとワークとの接触を検出したときのツールの位置や、粗倣い構成動作2の最終位置を位置データとして使用する。
(2)連続する位置データにおける位置間の距離が予め定めた閾値以下であるような、互いに近い位置が存在する場合には、それらを軌道生成のためのデータとして用いず、適当な代表点のみを使用する。
(3)例えば図27に示すように、位置データとして使用する点の位置の変化を見たとき、ある点までとそれ以降の部分とで急にツールの移動方向が変わる部分は角部であると推定し、位置データとして適当な補間点78、80を入れ、その位置データをもとに軌道を生成する。
(4)最小自乗近似等により、軌道生成に用いる位置データに沿った滑らかな軌道を生成する。
(5)教示データをもとに位置の補間等を行いながら滑らかに動くように移動するとき、実際に通過する位置と、教示データによって与えられる位置とが、角部やコーナ部などにおいてずれることがある場合、それを考慮し、位置データの位置と実際に通過する位置とのずれが所定閾値以下になるように、位置データを補正し、教示データを生成する。
(6)教示データ上の移動速度はユーザが設定した値を用いる。又は、作業の目的や条件に応じて、予め用意したデータテーブルを参照して、適切な移動速度を設定する。さらに、移動速度によって、実際に通過する位置と、教示データによって与えられる位置とがずれることがある場合、そのずれが所定閾値以下になるように、前記のように与えられる速度をもとに、位置に応じて調整した移動速度を設定してもよい。
(7)教示データ作成時に押付方向も設定する場合には、生成される軌道をもとにその進行方向と直交する方向を押付方向とする。
以上のようにすれば、粗倣い動作実行中の位置を、粗倣い構成動作1及び粗倣い構成動作2の動作の特徴を考慮しながら、或いは又はこれに加え、基本移動動作実行中の位置を、ロボット制御装置の制御周期毎に取得し、その位置をもとに、動作中の速度には関係なく、また教示データには不要な動作中の位置は除き、適当な間隔で位置データを生成することができ、故にワーク形状に沿った軌道を移動させる教示データを生成又は修正することができる。
図9〜図27を参照して説明したように、粗倣い構成動作1及び粗倣い構成動作2における移動方向、移動速度及び移動時間は、ワークに対するツールの位置や両者間に作用する力に基づき、状況に応じて変更されることが好ましい。また粗倣い構成動作1及び粗倣い構成動作2において、これらの動作を行ってもツールがワークに対して同じ位置付近に留まることがないように、粗倣い動作の移動中の位置を監視しながら移動方向、移動速度及び移動時間の少なくとも1つが調整される。さらに、粗倣い構成動作1及び粗倣い構成動作2において、安全の観点から、移動時間又は移動距離に制限を設けておくことが好ましい。
粗倣い構成動作1及び粗倣い構成動作2において、ツールがワークに衝突するときに発生する力が、検出を目的とする閾値以外の別の閾値を超えた場合に、移動速度を小さくすることによってツールやワークに過負荷がかかることを防止できる。粗倣い動作実行中の位置に基づき、粗倣い動作の進み具合が遅いと判断された場合には、速度を適切に大きくしたり、移動方向を基準進行方向又は基準押付方向とは逆方向に変えたり、移動時間又は移動距離を大きくすることが好ましい。但し、そのような調整によってワーク形状への倣いが過度に粗くなって問題となる場合には、適切な値に制限しておくことが望ましい。
粗倣い構成動作2の移動時間又は移動距離は、適切な距離だけ移動するように調整することが好ましく、具体的には、移動距離が短過ぎると進行方向(教示軌道によって与えられる進行方向)にワークが存在していることを検出することができず、逆に移動距離が長過ぎると進行方向にワークが存在しないにも関わらず不必要に移動を続けることを考慮して、調整される。但し基準進行方向にツールを進ませながら、大まかにワーク形状に沿わせることで十分である場合は、ツールをワークに接触するまで移動させ続ける必要はなく、制限距離内の移動で十分である。
上記粗倣い構成動作2の移動時間又は移動距離の調整方法としては、以下のようなものが考えられる。
(1)粗倣い動作中の移動位置に基づいて求められる、ワーク形状及びツールの現在位置に基づき、粗倣い構成動作2の移動距離、移動時間及び移動速度を調整する。例えば、粗倣い構成動作2の実行前の、粗倣い構成動作1の開始位置から、基準進行方向について所定距離だけ先に進ませるように、パラメータを定める。
(2)実験に基づいて、使用するワーク形状に適した移動時間又は移動距離を定め、その値を限度としてツールを動かす。
(3)粗倣い動作中に調整された、移動時間又は移動距離を限度としてツールを動かす。
以下、力制御により基本移動動作を行う種々の形態について説明する。先ず図28は、矢印76a、76b、76cで示す基本移動動作では、教示点82、84により定められる教示軌道に沿って力制御で倣わせるときに参考とする進行方向及び位置が予め与えられており、ある位置におけるツール18の進行方向は、実際の位置に基づく変化ではなく、教示軌道における進行ベクトルによって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行っているとしてもよい。
図28の例では、ワーク20に障害物となる凸状物86が存在しているので、矢印76aで示す基本移動動作から粗倣い動作72a、74aに切り替えられ、凸状物86をツール18が乗り越えたら基本移動動作76bに移行し、次に押付方向について、倣う形状が存在しなくなるので、粗倣い動作72b、74bが実行され、再度基本移動動作(76c)に戻ることになる。状況に応じて粗倣い動作を変えるため、力検出手段で検出される力をもとに、粗倣い構成動作1及び粗倣い構成動作2の動作が設定される。
図28の例では、状況判断のために、粗倣い動作72a、74aによって障害となる形状を回避できたかどうか、粗倣い動作72b、74bによって倣うべき形状が現れたかどうかを確認する。また粗倣い動作72b、74bを用いなくとも、押付力に応じて移動速度をかえるような基本移動動作を行って、接触を確認するまで移動速度を小さくするようにしてもよい。また基本移動動作は、教示軌道をもとに、力制御を行い、教示軌道とは異なった軌道を動いているとする。基本移動動作で力制御を行う場合は、教示軌道が与えられておらず、ワーク形状に沿って倣う形態としてもよい。
図29は、図28の凸状物86とは異なる形状の凸状物88がワーク20に存在している例を示す。矢印76aと76bで示される基本移動動作の間の領域について、検出される力に基づいて粗倣い構成動作1の動作方向を変えながら、粗倣い構成動作2で基準進行方向に動かし障害物がないかどうかを確認しながらツール18が進められる。力をもとに、障害物がないと判断されたら、基本移動動作(76b)でツールを倣わせる。前方に倣わせる物体が急になくなったとき、再び粗倣い動作に切り替え、所定の移動時間又は移動距離を限度に、ワーク20に対するツール18の接離(但し接触しない場合もある)を繰り返しながら、ツールをワークに倣わせる。このとき所定方向について力を検出したら、基本移動動作(76c)に再び切り替えられる。
図30に示すように、ワーク20に未知の形状物90が存在していても該形状物が緩やかな山のような形状であり、基本移動動作の実行中において力制御のみでもツール18を倣わせることができる場合を考える。しかしこの場合において、山形状の突起物90によって、進行方向についての予め定めた閾値を超えるような力はツールにかからないが、教示点82、84間に設定された教示軌道92とツール18の位置との乖離(距離)が予め定めた閾値S1を超えるときは、ツールの軌道が教示軌道から大きく離れ、基本移動動作を続行したのでは移動時間が長くなる虞があるので、ツール18の位置に基づいて粗倣い動作に切り替えてもよい。
図30の例では、現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、該最短距離が予め定めた閾値S1を超え、進行方向(教示軌道によって与えられる方向を意味する。ここでは、基準進行方向と同じ)についてツールとワークとの間に作用する力に応じて粗倣い動作に切り替えるための予め定めた閾値を超えるような力はツールにかからないが、進行方向(教示軌道によって与えられる方向)について別の予め定めた閾値を越える力はかかっているときに、基本移動動作76から粗倣い動作72、74に切り替えられる。最初の粗倣い構成動作1及び粗倣い構成動作2の移動方向は、それまでに基本移動動作を実行していたときの力や位置に基づいて求められる。粗倣い構成動作2の移動方向は基準進行方向と同じ方向の成分を含む方向とし、粗倣い構成動作2の実行中に閾値を超える力が検出されたときは、粗倣い構成動作1に切り替える。このような粗倣い動作実行中に、粗倣い構成動作2において閾値を超える力を検出しなくなったときは、基本移動動作を再開する。粗倣い構成動作1と粗倣い構成動作2の移動方向は、これらからなる粗倣い動作によってツールが基準進行方向に進むように定められることが好ましい。また、ツールとワークとの間に作用する力が、該別の予め定めた閾値を超えているかどうかの確認をしなくとも、該最短距離が予め定めた閾値S1を超えている間、粗倣い動作を行うようにしてもよい。
一方、例えば図31に示すように、ワーク20に未知の形状物94が存在していても該形状物が谷のような形状であり、基本移動動作の実行中において力制御のみでもツール18を倣わせることができる場合を考える。しかしこの場合において、谷形状の形状物94によって、進行方向(教示軌道によって与えられる方向)についての予め定めた閾値を超えるような力はツールにかからず、かつ、押付方向についての別の予め定めた閾値を超えるような力がツールにかかるが、教示点82、84間に設定された教示軌道92とツール18の位置との乖離(距離)が予め定めた閾値S2を超えるときは、ツールの軌道が教示軌道から大きく離れ、基本移動動作を続行したのでは移動時間が長くなる虞があるので、ツール18の位置に基づいて粗倣い動作に切り替えてもよい。
図31の例では、現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、該最短距離が予め定めた閾値S2を超えたときに基本移動動作76から粗倣い動作に切り替えられる。この場合の粗倣い動作では、矢印72で示す粗倣い構成動作1を先ず行い、粗倣い構成動作1によって所定距離又は所定時間だけツールを移動させた後、矢印74で示す粗倣い構成動作2を行う。粗倣い構成動作2の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、粗倣い構成動作2によって所定距離又は所定時間だけツールを移動させた後、基本移動動作に切り替える。但し、粗倣い構成動作2の実行中に閾値を超える力が検出されたときは、粗倣い構成動作1に切り替える。再開された基本移動動作では、ツールを押付方向にも動かしながら移動させ、現在位置と教示軌道との最短距離が閾値S2を超えたとき再度粗倣い動作を行う。以上の動作を繰り返し、最終的には、基本移動動作によってツールをワークに倣わせて移動させる。
図31の場合において、粗倣い構成動作1においてツール位置が教示軌道から閾値を超えて離れた場合、ツールの移動方向を教示軌道へ向かう方向の成分を含むように変更することで、ツールを教示軌道に向かう方向に修正することができる。また図31の例でも、粗倣い構成動作2では、ツールを基準進行方向に進めたときに、ワークが存在するかどうかを確認する。図30では山のような形状がある場合について説明したが、図31の場合でも、粗倣い構成動作2は他方(ワーク)の存在を確認する動作である。また、最初の粗倣い構成動作1において教示軌道と平行な方向に移動させ続け、接触後に、基本移動動作に切り替えるようにしてもよい。
なお図9〜図27を用いて説明した粗倣い動作の各例は、後述する基本移動動作が位置制御である場合にも同様に適用可能である。
次に、基本移動動作が、指定された位置に基づいてワークに対してツールを移動させる位置制御に基づくものである場合について、図32を参照しつつ説明する。図32のように、教示点82、84に基づき定められる教示軌道92に沿ってツール18が移動させられ、該教示軌道上に障害物96が存在しているとする。このように基本移動動作の軌道上に障害となる形状が存在する(山形状の突起がある)場合、基本的には、粗倣い動作によって、ツールをワークに対して停止させたり、ワークやツールに過負荷を与えたりすることなく、なるべくワーク形状に沿ってツールを倣わせる。以下、具体的に説明する。
基準進行方向は、これまでの説明と同様に求めることができる。一方、基準押付方向は、予め定められた方向、各教示点に対して予め定められた方向、又は教示軌道上の進行方向との関係を設定しておくことによって決まる方向に基づき、粗倣い動作に切り替えたときの位置及び進行方向をもとに定める。このようにして定められた以降の基準押付方向は、あるときの位置から教示軌道に対して近づく方向や最短距離となる方向として求めてもよい。
基本移動動作では、押付方向について力制御は行われないので、ツールが指定された位置を外れて押付方向に動かされることはないが、粗倣い動作では教示軌道から離れる。この場合、基準進行方向と基準押付方向とをもとに、これまでの説明と同様に、障害となる形状を回避する動作の移動方向や、障害となる部分に対してツールを近づけて形状を確認する動作の移動方向を定め、さらに、教示軌道に戻る方向を定める。
粗倣い動作は、ワーク20において基準進行方向に障害となる形状(山形状の突起)96が存在する場合(図32のA部)、或いは基準進行方向に障害物はないが、ある時点でのツール位置がもとの基本移動動作の軌道から外れており、もとの基本移動動作の軌道に復帰させようとする場合(図32のB部)に実行される。但し、それぞれの場合でツールの動きは異なるので、力検出手段で検出される力、及び現在位置と教示軌道との位置関係によって、粗倣い動作を変える。
先ず図32のA部での動作方法について説明する。基本移動動作の実行中に、進行方向について閾値を超える力が検出されたときに、粗倣い動作に切り替えられ、先ず、第1の粗倣いパターン動作、すなわち粗倣い動作パターンA(第1の粗倣いパターン動作を、粗倣い動作パターンAとも称することとする。)を実行する。粗倣い動作パターンAでは、粗倣い構成動作2の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、粗倣い構成動作2の実行中に閾値を超える力が検出されたとき、粗倣い構成動作1を実行する。
上記粗倣い動作パターンAによる粗倣い動作の実行中に、粗倣い構成動作2において閾値を超える力を検出しなくなったとき、粗倣い構成動作1及び粗倣い構成動作2の動作を変えた粗倣い軌道復帰動作、すなわち粗倣い動作パターンB(粗倣い軌道復帰動作を、粗倣い動作パターンBとも称することとする。)を実行する。粗倣い軌道復帰動作では、粗倣い構成動作1又は粗倣い構成動作2のどちらを先に実行しても構わないが、粗倣い構成動作2を先に実行することが好ましい。粗倣い軌道復帰動作における、粗倣い構成動作2の移動方向は、基準押付方向と同じ方向の成分を含む方向、又は教示軌道に向かう方向とする。粗倣い動作中に、ツールの現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、現在位置と教示軌道との最短距離が予め定めた閾値以内となったときに、基本移動動作を再開する。また粗倣い軌道復帰動作の実行中に、基準進行方向について予め定めた閾値を超える力が検出されたときは、粗倣い動作パターンAを実行する。
粗倣い軌道復帰動作における、粗倣い構成動作1及び粗倣い構成動作2の移動方向は、これらからなる粗倣い動作によってツールが基準進行方向に進むように定められる。また粗倣い構成動作1及び粗倣い構成動作2は、それらからなる動きによってツールの進み方が遅くならないように、或いはツールが同じ位置付近に留まらないように、粗倣い動作の移動中のツールの位置を監視しながら移動方向、移動速度及び移動時間の少なくとも1つを調整することが好ましい。また図32のような場合では、粗倣い軌道復帰動作における、粗倣い構成動作1の移動方向は、基準進行方向と同じ方向の成分と、基準押付方向と逆方向の成分とを含む方向とすることが好ましい。
なお、基本移動動作が力制御であっても位置制御であっても、その違いによらず、粗倣い構成動作1及び粗倣い構成動作2による粗倣い動作の動作は、これまで示したような直線的な動きを滑らかにつなげてもよいし、図33に示すようにツールを曲線的に動かしてより滑らかに動くようにしてもよい。
図34は、上述の粗倣い動作パターンA及びBを含む粗倣い動作を、図30を用いて説明した、基本移動動作が力制御によるものである場合に適用した場合を示す図である。矢印76で示す基本移動動作は、力制御によって押付力を制御しながら、ツール18とワーク20との間に作用する力が調整可能な目標値となるように、ツールをワーク形状に倣わせる動作であるとする。教示点82、84に基づく教示軌道92によって、力制御で倣わせるときに参考とする位置が与えられており、ある位置におけるツールの進行方向は教示軌道によって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行ってもよい。
力制御による基本移動動作の実行中に、未知の形状であっても力制御で適切に倣うことができ、進行方向(教示軌道によって与えられる方向)について予め定めた閾値を超えるような力はかからない状況であるが、教示軌道とは大きく異なる山のような形状物90が存在する場合、ツール18の位置に基づいて(例えば、教示軌道92からの最短距離が閾値S1を超えるとき)、粗倣い動作に切り替えることが考えられる。このことは、ツールが倣う軌道が教示軌道から大きく離れるような場合、或いはツールをワーク形状にそのまま倣わせたくない場合に有用である。
図34の場合において、基本移動動作の実行中に、ツールの現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、該最短距離が予め定めた閾値S1を超えたとき、基本移動動作から粗倣い動作に切り替えられる。最初の粗倣い構成動作1及び粗倣い構成動作2の移動方向は、それまでに基本移動動作を実行していたときの力や位置に基づいて求められる。
粗倣い動作に切り替えられたときは、先ず、図32に関して説明したものと同等の粗倣い動作パターンAを行う。この粗倣い動作パターンAによる粗倣い動作の実行中に、粗倣い構成動作2において閾値を超える力を検出しなくなったとき、図32に関して説明したものと同等の粗倣い軌道復帰動作、すなわち粗倣い動作パターンBを実行する。
粗倣い軌道復帰動作における、粗倣い構成動作1及び粗倣い構成動作2の移動方向は、これらからなる粗倣い動作によってツールが基準進行方向に進むように定められる。また粗倣い軌道復帰動作における粗倣い構成動作1の移動方向は、基準進行方向と同じ方向の成分と、基準押付方向と逆方向の成分とを含む方向にすることが好ましい。
図35は、図31の場合における粗倣い動作に、上述の粗倣い軌道復帰動作(粗倣い動作パターンB)を適用した例を説明する図である。矢印76で示す基本移動動作は、力制御によって押付力を制御しながら、ツール18とワーク20との間に作用する力が調整可能な目標値となるように、ツールをワーク形状に倣わせる動作であるとする。教示点82、84に基づく教示軌道92によって、力制御で倣わせるときに参考とする位置が与えられており、ある位置におけるツールの進行方向は教示軌道によって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行ってもよい。
力制御による基本移動動作の実行中に、未知の形状であっても力制御で適切に倣うことができ、進行方向(教示軌道によって与えられる方向)について予め定めた閾値を超えるような力はかからず、かつ、押付方向についての別の予め定めた閾値を超えるような力がツールにかかる状況であるが、教示軌道とは大きく異なる谷のような形状物94が存在する場合、ツール18の位置に基づいて(例えば、教示軌道92からの最短距離が閾値S2を超えるとき)、粗倣い動作に切り替えることが考えられる。このことは、ツールが倣うべき軌道が教示軌道から大きく離れるような場合、或いはツールをワーク形状にそのまま倣わせたくない場合に有用である。
図35の例では、先ず粗倣い構成動作1を行う。粗倣い構成動作1によって所定距離又は所定時間だけツール18を移動させた後、粗倣い構成動作2を行う。粗倣い構成動作2での移動方向は、基準進行方向と同じ方向の成分を含む方向とする。粗倣い構成動作2によって所定距離又は所定時間だけ移動させた後、上述の粗倣い軌道復帰動作(粗倣い動作パターンB)を行う。ツールが教示軌道92から閾値S2以内の距離の位置に進んだら、基本移動動作を開始する。但し、図35のように、基本移動動作の実行中に再びツール現在位置と教示軌道との最短距離が閾値S2を超えるときは、粗倣い動作を行う。続いて、粗倣い構成動作1の実行後に粗倣い構成動作2を実行し、粗倣い構成動作2の実行中に基準進行方向について閾値を超える力が検出されたら、障害物を含む領域を通過するための粗倣い動作を行う。障害物を越えて、ツールの現在位置と教示軌道との最短距離が閾値S2以下であれば、基本移動動作を再開する。
図36に示す例では、ワーク20に未知の形状物90が存在していても該形状物が緩やかな山のような形状であり、基本移動動作の実行中において力制御のみでもツール18を倣わせることができる場合を考える。基本移動動作で倣う場合、山形状の上り部分では、進行方向(教示軌道によって与えられる方向を意味し、この場合、基準進行方向と同じ方向を意味する)と逆方向の力がかかり、その部分の先の下り部分では、進行方向(教示軌道によって与えられる方向)の力がかかるようになる。このとき、山形状の突起90の頂点付近に到達し、進行方向と逆方向の力(教示軌道によって与えられる進行方向についての力)が所定閾値より小さくなる場合、ツールの位置に基づいて、例えば、教示軌道からの最短距離が予め定めた閾値S3を超えているとき、基本移動動作76aから粗倣い動作Bに切り替え、該最短距離が閾値S3を下回ったら基本移動動作76bに戻す例が示されている。この例は、ツールが教示軌道から大きく離れ得る形状であって、上述のように、教示軌道から所定距離を超えて離れた位置から教示軌道方向に近づき下っていくような動作のとき、基本移動動作では時間がかかるので移動時間を短縮したい場合等、ツールをワーク形状にそのまま倣わせたくない場合に有用である。
ここでは、現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、前記最短距離が予め定めた閾値を超えたときに粗倣い動作に切り替える。粗倣い動作Bにおける最初の粗倣い構成動作1及び粗倣い構成動作2の移動方向は、それまでに基本移動動作76aを実行していたときの力や位置に基づいて求める。図36の場合、粗倣い構成動作2の移動方向は、基準進行方向と同じ方向の成分を含む方向、又は教示軌道に向かう方向とする。粗倣い構成動作2実行中に、予め定めた閾値を超える力が検出されたとき、粗倣い構成動作1を実行する。この粗倣い動作実行中に、教示軌道からの距離が閾値以内にあり、かつ、基準押付方向について予め定めた閾値を超える力が検出されたとき、基本移動動作76bを再開する。また粗倣い動作の実行中に、基準進行方向について予め定めた閾値を超える力が検出されたときは、上述の障害物を越える方法に切り替えることが好ましい。また粗倣い構成動作1及び粗倣い構成動作2の移動方向は、それらの動作によってツールが基準進行方向に進むように決めることが好ましい。
図37は、力制御によりツール18をワーク20に倣わせている基本移動動作76aの実行中に、ツール18の進行方向(教示軌道によって与えられる方向を意味し、この場合、基準進行方向と同じ方向を意味する。)について、過負荷を検出して粗倣い動作に切り替えるための予め定めた閾値を超えるような力は検出されない状況だが,別の閾値を超えるような力はかかっており、教示軌道とツール位置との最短距離が予め定めた閾値S4を超えているとき、上述の粗倣い動作パターンAによる粗倣い動作に切り替え、矢印76bで示すように基本移動動作を再開し、その後、進行方向(教示軌道によって与えられる方向を意味し、この場合、基準進行方向と同じ方向を意味する。)と逆方向について予め定めた閾値を超える力がかかっており、さらに教示軌道とツール位置との最短距離が該閾値S4を超えているときは、粗倣い動作パターンBによる粗倣い動作に切り替えることも考えられる。図37の例では、ワーク形状が教示軌道から予め定めた閾値を越えて異なることを検出し、進行方向(教示軌道によって与えられる方向)又はその逆方向について力がかかっておらず、基準進行方向がその部分でのワーク形状に沿っていると判断できる、すなわちツールを倣わせやすい状況では基本移動動作を行い、それ以外の部分では粗倣い動作を行うことによって、ワーク形状に応じた倣い動作を実行することができる。
なお図37のように、基本移動動作中のツールの押付方向が基準押付方向46と同じ方向であり、基準進行方向44と基準押付方向46が直交しており、ワーク形状が基準進行方向と平行になっていない場合は、ツールの押付方向がワーク形状に対して直交しないため、力制御では押付方向が適切とならない場合がある。しかしこのような状況においては、ツールをワーク形状に沿ってそのまま倣わせず、粗く倣わせることによって、その形状部分にある程度沿わせつつ、好適にツールを通過させることができる。
図38は、図35と同様の谷のような形状94が存在しているが、ツール18をワーク20にある程度以上は倣わせたくない場合に、ツールの位置に基づいて、例えば教示軌道92からの最短距離が予め定めた閾値S2を超えたときに、基本移動動作76から粗倣い動作に切り替えることも考えられる。これは、倣うべき軌道が教示軌道から大きく離れるような場合に有用であり、想定外の軌道に倣ってツールが全く意図しない方向へ移動したり、ワークから離れすぎて停止させたりすることなく、教示軌道から大きくずれるような部位ではツールをワーク形状に沿って倣わせず、粗倣いによって進ませたい場合に有用である。図38では、基本移動動作76の実行中に、ツール18の現在位置と教示軌道との最短距離が閾値S2を超えたとき、粗倣い動作を行うが、先ず粗倣い構成動作1を行い、粗倣い構成動作1での移動方向は基準進行方向と同じ方向の成分を含む方向とする。
図39は図38の変形例を示す。図35と同様の谷のような形状94が存在しており、基本移動動作76aの実行中に、現在位置と教示軌道との最短距離が予め定めた閾値S2を超えたときに粗倣い動作を行うが、このとき、矢印72aで示す粗倣い構成動作1を先ず行い、粗倣い構成動作1での移動方向は、基準進行方向と同方向の成分を含む方向とする。次に所定距離又は所定時間だけツール18を移動させた後、矢印74で示す粗倣い構成動作2を実行する。粗倣い構成動作2において所定距離又は所定時間だけツール18を移動させた後、矢印72bで示す粗倣い構成動作1を実行する。この粗倣い構成動作1の実行中に、基準進行方向について予め定めた閾値を超える力は検出されないが、基準押付方向について予め定めた閾値を超える力が検出されたとき、基本移動動作76bを再開する。
図40は、図38の変形例を示す。図35と同様の谷のような形状94が存在しており、基本移動動作76aの実行中に、現在位置と教示軌道との最短距離が予め定めた閾値S2を超えたときに粗倣い動作を行うが、このとき、矢印72で示す粗倣い構成動作1を先ず行い、粗倣い構成動作1での移動方向は、基準進行方向と同じ方向の成分を含む方向とする。この粗倣い構成動作1の実行中に、基準進行方向について予め定めた閾値を超える力が検出されたとき、上述の障害物を越える粗倣い動作に切り替える。障害物を越える粗倣い動作においては、矢印74で示す粗倣い構成動作2によって所定距離又は所定時間だけツールを移動させ、粗倣い構成動作1と粗倣い構成動作2とを繰り返してツールが障害物を乗り越えたら、矢印76bで示す基本移動動作を再開する。
なお基本移動動作が力制御によるものである場合に、教示点に基づく教示軌道によってツールの進行方向が定められ、該進行方向に合わせてツールの押付方向を定める倣い動作を行っているとすると、例えば図41に示すように、教示軌道上に、ツール(図示せず)をワーク20に対して基準押付方向46に押し付けるようなワーク形状がない(そのときの位置から基準進行方向に谷がある)場合、進行方向が基準進行方向と同じ方向、押付方向が基準押付方向と同じ方向であり、それらをもとに基本移動動作を継続すると、図41に示すワークの窪み形状の部分に近づくことはなく、進行方向の速度を小さくしツールを押付方向に移動させたとしても、ツールがワークの該窪み形状から離れてしまうことになる。
そこで、所定時間又は所定比較回数だけ、力の進行方向成分が予め定めた閾値より小さく、かつ、押付方向成分が予め定めた閾値より小さい場合は、ツールは押付方向に押し付けることができない部分に差し掛かり、ワークに倣うことができない状態になっていると判定する。この判定において、実際にそのような部分に差し掛かっているのか、或いは単に倣いの性能が悪いため倣えていないのかを、なるべく正確に判断できるように、押付方向についての反力が小さい場合に進行速度を下げて倣いの性能を相対的に向上させることもできる。
そして、粗倣い構成動作1と、粗倣い構成動作2と、それらの動作切り替え判定とを繰り返し(切り替えをしない場合もあり、また順序は問わない)、つまりツールをワークから離す動作と、ワークに近付ける動作と、それらの動作中に倣うべきワーク形状が存在しているかを確認する動作とを繰り返して、基準押付方向及び基準進行方向をもとに、ツールをワーク形状になるべく沿うように動かす。
ここで、粗倣い動作実行中に、状況に応じて、基準押付方向について力がかかっていなくともその後の基本移動動作の押付方向が基準押付方向とは異なる場合には任意の方向について、所定の閾値を超える力がかかれば、或いは、粗倣い動作を継続する場合には基準押付方向について、所定の閾値を超える力がかかれば、基本移動動作に切り替えることによって、図41のように押付方向に形状物が急に存在しなくなった場合(そのときの位置から基準進行方向に谷がある)でも、迅速かつ、なるべくワーク形状に沿った倣い動作を行うことができる。さらに、それらの動作をより細かく(短時間又は短距離で)繰り返すことによって、ツールをワーク形状により正確に沿わせることができる。
これまで述べてきた本発明の応用によって、複雑なワーク形状に対しても簡単な教示によってツールを倣わせることもできる。例えば、図42に示すように、円周上に凹凸部98が設けられているワーク形状に対して円を動くような簡単な教示軌道100によってツールを倣わせたい場合を考える。教示軌道によって進行方向及び押付方向が与えられる通常の倣い動作では倣わせることは困難であるが、本発明では、凹凸部98の各々において、そのときの形状・位置に応じたツールの進行方向及び押付方向をそれぞれ基準進行方向及び基準押付方向として、それをもとに回避動作をおこなうため、教示軌道100とは異なる未知の形状・障害物に対しても、上述の基本移動動作と粗倣い動作とを切り替えながら、なるべくワーク形状に沿いつつ障害物等を回避しながら、迅速にツールを動かすことができる。
なお、図42に示されるような、教示軌道とは異なる実際のワークの複雑な凹凸形状や教示軌道とのサイズの違いなどによる、教示軌道と実際のワーク形状とのずれは、教示軌道上の点を実軌道上の点に射影させて、目標とする移動方向や位置を合わせたり補正したりすることによって所望の方向や位置を取得し、対応することができる。よって、力制御を行う基本移動動作や粗倣い動作実行中に、教示軌道を参考としながら、教示軌道とは異なる複雑な形状に沿ってツールを回避させながら、教示軌道が意図する位置へ移動させることが可能である。さらに、上述のように、ワーク形状になるべく沿った移動をさせたり、他の部分との干渉を避けたりするためには、粗倣い構成動作実行時に実際に移動する距離を短くしておくことが望ましい。
また、ツールとワークとの間に作用する力や力制御による倣い動作中の移動位置をもとにツールの進行方向と押付方向を求め、ワークに対して連続的に倣わせる方法を行う場合でも、ワークに不連続な部分があったり非常に複雑な形状部分があったりする等、ツールをワークに連続的に倣わせ続けるよりも、本発明によって、ワーク形状を確認する動作とワークから局所的にツールを離す動作とを繰り返して、ワーク形状に粗く倣わせる方が望ましい場合もある。このように,ワーク形状が複雑な場合や動作時間をなるべく短縮したい場合等において、ワーク形状にツールを粗く倣わせればよいというときにも本発明は有効である。
なお上述の実施形態では、ロボットアームの先端に取り付けたツールをワークに対して移動させて倣わせる場合について説明したが、本発明の実施に際し、ロボットアーム先端部分に取り付けたハンドにより把持されたワークを、固定された加工ツール等に対して移動させるようにしてもよい。また、ロボットアームの先端に取り付けたツール又は把持させたワークを作用させる、他方のワーク又はツールを固定させた部分は、移動動作が可能な装置によって移動させてもよい。
以上述べたように、本願発明では、ロボットを用いてツールとワークとを相対的に移動させているときに、障害物、壁、穴、大きな凹凸等の未知な形状があった場合でも、「それまでの移動動作」と、「積極的に一方を他方から離す動作と、他方の存在を確認する(通常は他方に近付ける)動作とを繰り返す、粗倣い動作」とを、力検出手段で検出する力の情報、及びロボットが動かしている物体の位置情報の少なくとも一方に基づいて適宜切り替え、ロボット、作業ツール及びワークに対して過負荷を与えず、好適な回避動作を行いつつなるべく形状に沿った迅速な倣い制御が可能となる。本発明は、加工を伴わない教示作業のための倣い制御、バリ取り作業、研磨・研削作業、ロールヘム加工等の押付けによる倣い加工作業、さらにはツールをワークに対して移動させて異物があるか、ワークが不良品であるか等を確認する作業にも有用である。