粗倣い制御を行うロボットの制御装置
【課題】形状が未知の部分や障害物等がワークに存在する場合でも、ロボット、作業ツール及びワークに対して過大な負荷をかけることなく、なるべくワークの形状に沿った迅速な倣い制御を可能とするロボット制御装置の提供。
【解決手段】ロボット制御装置１４は、ツールとワークとの間に作用する力を検出する力検出手段１６と、ロボット１２の動作切り替えの判定及びパラメータ調整を行う動作切り替え判定・動作パラメータ調整部３４と、動作切り替え判定・動作パラメータ調整部３４による動作指令に基づいて、ロボット１２に送る指令を演算する指令演算部３６とを有する。
【解決手段】ロボット制御装置１４は、ツールとワークとの間に作用する力を検出する力検出手段１６と、ロボット１２の動作切り替えの判定及びパラメータ調整を行う動作切り替え判定・動作パラメータ調整部３４と、動作切り替え判定・動作パラメータ調整部３４による動作指令に基づいて、ロボット１２に送る指令を演算する指令演算部３６とを有する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、広くは倣い制御を行うロボット制御装置に関し、特には、ロボットによるバリ取り、研削、研磨、ロールヘム加工等の倣い作業の自動化、或いはロボットの移動動作中における障害物への対応の自動化を実現するためのロボット制御装置に関する。
【背景技術】
【０００２】
ロボットによりバリ取り、研磨、研削等の加工作業を行うため、或いはそのための教示データの生成のために、ロボットアーム先端部分に取り付けた作業ツールを適切な力でワークに押付けながら作業ツールを移動させる倣い動作が行われている。同様の目的のために、ロボットアーム先端部分に取り付けたハンドにより把持されたワークを適切な力で、固定された加工機械に押付けながらハンドを移動させる場合もある。このとき、ワークと作業ツールとの間に働く作用力が適切な値になるように、該作用力を、力覚センサを用いて検出し、又は、機械駆動部のアクチュエータの電流値を用いることにより該作用力を推定し、検出又は推定された作用力をフィードバックして作業ツールの位置が制御される。
【０００３】
このような倣い動作の方法として、従来、以下に述べるような様々な方法が実施され又は提案されている。
（１）倣い加工の方法としての、加工工具を教示された軌道に沿って動かし、該軌道上に存在するバリなどを削る方法
（２）倣い加工・教示の方法としての、特許文献１のように、ワークからの反力に基づいて、ロボットとワークとの接点の法線方向を求め、押付方向及び進行方向を算出し、ワーク表面に対して一定力を加えながら倣う方法
（３）特許文献２−４に記載のような、予めワークから離れた位置、押付方向の粗教示をしておき、それに基づいて、位置制御をしながら押付方向について力制御を行い、ワーク表面に倣う方法
【０００４】
上述のような倣い方法において、大きくかつ不連続な凸形状部や凹形状部が存在すると、ツールを適切に倣わして動かすことができないという問題が発生する。
【０００５】
これに対して特許文献５では、過負荷が発生した場合に、過負荷発生位置を基準として、過負荷発生要因部に対して進行方向側の背後に研削工具を移動させ、進行方向と逆側に移動させながら研削を行い、過負荷発生要因部を取り除き、その後、もとの研削作業を再開するという方法が提案されている。また特許文献６では、予め教示された軌道に沿ってグラインダを動かしているときに、過大バリに接触して負荷電流が一定の設定値を越えた場合、段階的に離れた複数の研削軌道を生成して、該研削軌道のうち教示軌道より離れたものから近いものに向かい順にグラインダを動かしながら段階的にバリのある方向に近づけて繰り返し研削するという方法が提案されている。
【０００６】
また特許文献７では、加工済みワークの加工面に対して粗教示点を設け、該粗教示点と１つ前の粗教示点とを結ぶ方向に加工工具を進めるとき、力の検出値が予め設定した値以上なら、次の粗教示点の方向に動かすことによって凹形状の角部に対応するという方法が提案されている。また特許文献８では、ツールの移動方向の力が設定値よりも大きいかどうかを判定し、予め設定した動作に切り替えることによって、ツールの押付方向と移動方向を変え、凹コーナー部の次の面に連続的に倣わせる方法が提案されている。さらに特許文献９では、エンドエフェクタにかかる反力が所定範囲にあるかどうかを判断し、凹コーナー部に対しては反力が所定範囲より大きいか、凸コーナー部に対しては反力が所定範囲より小さいかを見ることによって、反力が所定範囲にないとき、予め次に指定した動作命令を実行することによってコーナー部に倣わせるという方法が提案されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開平３−１８４７８６号公報
【特許文献２】特開平６−１０２９２４号公報
【特許文献３】特開平７−２６６２６９号公報
【特許文献４】特開平５−８８７４２号公報
【特許文献５】特開平４−３５８６０号公報
【特許文献６】実開平５−９２８０８号公報
【特許文献７】特開平６−２６２５６２号公報
【特許文献８】特開平３−１１１１５１号公報
【特許文献９】特開平８−１１８２７６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
上述のように特許文献１−４に記載の発明では、対象物において、教示軌道とは大きく異なる形状部が存在したり、サイズが大きくかつ不連続な形状部が存在したりすると、ツールを適切に倣わして動かすことができないため、ロボットやツールやワークに過負荷をかけたり、システムの稼動を止めたりするという問題が発生する。このとき特許文献２−４のように粗教示点をもとに進行方向を決め、押付方向に対して力制御を行う場合では、与えられる進行方向と大きく異なる凸形状部に衝突した場合や、凹形状部に嵌った場合、適切に回避するための移動指令を得ることができないため、該形状部を回避することができない。
【０００９】
特許文献５及び６の方法は、決められた軌道上を動きながら過負荷発生要因を削除することを目的としており、背後に回って削る動作や段階的に離れた複数の軌道を生成する動作において、予めワークや工具毎に過負荷要因部の大きさを予測し、予め冗長な軌道を用意しなければならなかったり、工具等が必要以上に大きく動いたりするため、未知形状のワークに対して好適な倣い動作を行うことが難しい。
【００１０】
また特許文献７−９の方法では、条件判定後の次の動作に切り替えるときに、次の部分に沿う動作を予め教示しておかねばならず、手間であり、またその動作が実際の形状と異なる場合は工具等を倣わすことができず、故に未知な形状に対しては倣わせることができない。また、ワークからの反力やワークの形状をもとに進行方向と押付方向を求め、ワークを連続的に倣わせる場合であっても、連続的に倣わせ続けるよりも、場所によっては形状の確認動作や対象物に対する離隔を繰り返して、ワーク形状に近い位置を素早く動かすことが望ましい場合もある。
【００１１】
そこで本発明は、形状が未知の部分や障害物等がワークに存在する場合でも、ロボット、作業ツール及びワークに対して過大な負荷をかけることなく、なるべくワークの形状に沿った迅速な倣い制御を可能とするロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１２】
上記目的を達成するために、本発明は、作業ツールとワークの一方に対して他方をロボットの手先部によって相対的に動かし、両者間に作用する力を制御するロボット制御装置において、前記両者間に作用する力を検出する力検出手段と、所定の基準進行方向と基準押付方向とに基づいて実行され、所定の方向に、所定移動時間又は所定移動距離を限度に移動する第１の粗倣い構成動作及び前記所定方向とは異なる方向に、所定移動時間又は所定移動距離を限度に移動する第２の粗倣い構成動作からなる粗倣い動作を実行する粗倣い動作実行手段と、前記ロボットの手先部を移動させる基本移動動作の実行中に検出された、前記両者間に作用する力における、所定方向の成分の評価結果、又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に基づいて基本移動動作から粗倣い動作に切り替える第１の動作切り替え手段と、粗倣い動作実行中の動作切り替え判定において、粗倣い動作中に検出された、前記両者間に作用する力の評価結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に基づいて、粗倣い動作から基本移動動作への切り替え、第１の粗倣い構成動作から第２の粗倣い構成動作への切り替え、第１の粗倣い構成動作から別の第１の粗倣い構成動作への切り替え、第２の粗倣い構成動作から第１の粗倣い構成動作への切り替え、及び、第２の粗倣い構成動作から別の第２の粗倣い構成動作への切り替えのいずれかを実行する、第２の動作切り替え手段と、を備えたことを特徴とするロボット制御装置を提供する。
【００１３】
また本発明のロボット制御装置は、前記基本移動動作は、前記両者間に作用する力が目標値と一致するように、前記一方に前記他方を接触し続けながら移動させる倣い動作であり、前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第１の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第２の粗倣い構成動作の移動方向は、基準進行方向と同じ方向の成分を含む方向であり、第２の粗倣い構成動作の実行中に、前記両者間に作用する力と予め定めた第２の閾値との比較結果に応じて第１の粗倣い構成動作又は基本移動動作に切り替え、第２の粗倣い構成動作の最後まで、第１の粗倣い構成動作への前記切り替えを行わないとき、粗倣い動作から基本移動動作に切り替えるものであることを特徴とする。
【００１４】
また本発明のロボット制御装置は、前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第１の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、粗倣い動作において、第２の粗倣い構成動作の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、前記第２の動作切り替え手段が、第２の粗倣い構成動作の実行中に、前記両者間に作用する力と予め定めた第２の閾値との比較結果に応じて第１の粗倣い構成動作に切り替える動作を、第１の粗倣いパターン動作とし、粗倣い動作において、第２の粗倣い構成動作の移動方向は、基準押付方向と同じ方向の成分を含む方向とし、又は、基本移動動作が教示軌道に基づいて移動する場合、前記教示軌道に近付く方向とし、前記第２の動作切り替え手段が、前記ツールまたはワークの位置と、前記指定された軌道の距離の評価結果に基づいて、粗倣い動作から基本移動動作に切り替える動作を粗倣い軌道復帰動作とし、前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第１の粗倣いパターン動作の実行中に、第２の粗倣い構成動作の最後まで、第１の粗倣い構成動作への動作切り替えを行わないとき、粗倣い軌道復帰動作に切り替え、粗倣い軌道復帰動作の実行中に、基準進行方向成分の力の大きさが予め定めた第３の閾値以上である場合、第１の粗倣いパターン動作に切り替えることを特徴とする。
【００１５】
また本発明のロボット制御装置は、前記基本移動動作は、該両者間に作用する力が目標値と一致するように、前記一方に前記他方を接触し続けながら移動させる倣い動作であり、前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第４の閾値との所定比較時間又は所定比較回数の比較結果、及び、前記両者間に作用する力における、前記ロボットの手先部の押付方向の成分と、予め定めた第５の閾値との所定比較時間又は所定比較回数の比較結果又は前記ロボットが動かしている前記作業ツール又はワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第２の粗倣い構成動作の移動方向は、基準進行方向と逆方向の成分を含む方向であり、粗倣い動作実行中に、前記両者間に作用する力における、所定方向の成分と、予め定めた第６の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、粗倣い動作から基本移動動作に切り替えるものであることを特徴とする。
【００１６】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、前記動作切り替え判定において、第１の粗倣い構成動作の実行時に、前記両者間に作用する力が、予め定めた第７の閾値を超えたとき、第１の粗倣い構成動作の移動する向きを、検出される力の大きさ及び方向に基づいて変更し、別の第１の粗倣い構成動作を行い、別の第１の粗倣い構成動作の実行時に、該別の第１の粗倣い構成動作が完了するまで、所定の条件を満たさないとき、第２の粗倣い構成動作に切り替えることを特徴とする。
【００１７】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、第１の粗倣い構成動作の実行後に、別の第１の粗倣い構成動作を行う場合において、別の第１の粗倣い構成動作で所定移動時間又は所定移動距離、移動したときの位置が、基準押付方向と逆方向の位置について、一連の第１の粗倣い構成動作における開始時からそれまでで、前記基準押付方向と逆方向に最も進んだ位置から所定距離以下なら、移動方向を基準押付方向の逆方向の成分を含む方向にし、又は基準進行方向の逆方向の成分を含む方向にし、別の第１の粗倣い構成動作を行うように動作を切り替えることを特徴とする。
【００１８】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、前記動作切り替え判定において、第１の粗倣い構成動作の実行時に、前記両者間に作用する力における、基準押付方向の成分が、予め定めた第８の閾値以上である場合、粗倣い動作から基本移動動作に切り替え、前記両者間に作用する力における、基準押付方向の成分が、前記第８の閾値以上でなく、かつ、前記両者間に作用する力における、基準進行方向の成分が、予め定めた第９の閾値以上である場合、第２の粗倣い構成動作、又は別の第１の粗倣い構成動作、又は基本移動動作に切り替え、前記両者間に作用する力における、基準押付方向の成分が前記第８の閾値以上でなく、かつ、前記両者間に作用する力における、基準進行方向の成分が、前記第９の閾値以上でなく、かつ、前記作業ツールと前記ワークとの間に作用する力が予め定めた第１０の閾値以上である場合、第１の粗倣い構成動作の移動する向きを、検出される力の大きさ及び方向に基づいて変更し、別の第１の粗倣い構成動作を行うように動作を切り替え、別の第１の粗倣い構成動作の実行時に、該別の第１の粗倣い構成動作が完了するまで、所定の条件を満たさなければ、第２の粗倣い構成動作に切り替えることを特徴とする。
【００１９】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、前記動作切り替え判定において、第２の粗倣い構成動作の実行時に、前記両者間に作用する力における、基準押付方向の成分が、予め定めた第１１の閾値以上である場合、粗倣い動作から基本移動動作に切り替え、前記両者間に作用する力における、基準押付方向の成分が、前記第１１の閾値未満、かつ、前記両者間に作用する力が、予め定めた第１２の閾値以上であるとき、第１の粗倣い構成動作に切り替え、第２の粗倣い構成動作の実行時に、該第２の粗倣い構成動作が完了するまで、所定の条件を満たさないとき、第１の粗倣い構成動作に切り替えることを特徴とする。
【００２０】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、第２の粗倣い構成動作の実行時に、前記所定移動時間又は前記所定移動距離だけ移動しても、前記両者に作用する力が、予め定めた第１３の閾値未満である場合、第２の粗倣い構成動作の所定移動時間、所定移動速度及び所定距離のうち少なくとも１つを大きくするか、又は、前記移動方向を基準進行方向の逆方向に所定量だけずらす、動作調整手段を含むことを特徴とする。
【００２１】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、粗倣い動作実行時に、第１の粗倣い構成動作若しくは第２の粗倣い構成動作を所定回数以上繰り返し実行しても、又は、所定時間が経過するまで繰り返し実行しても、第１の粗倣い構成動作の開始位置、又は第２の粗倣い構成動作の開始位置の少なくとも２つ以上のそれぞれの開始位置に基づく開始位置の変化量が所定値以下のとき、第１の粗倣い構成動作又は第２の粗倣い構成動作を行うときの、移動方向を変えるか、又は、第１の粗倣い構成動作の所定移動時間、所定移動速度及び所定移動距離、並びに第２の粗倣い構成動作の所定移動時間、所定移動速度及び所定移動距離のうち少なくとも１つを大きくする動作調整手段を含むことを特徴とする。
【００２２】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、前記粗倣い動作実行時に、第１の粗倣い構成動作若しくは第２の粗倣い構成動作を所定回数以上繰り返した場合若しくは所定時間経過した場合、粗倣い動作への切り替え時の開始位置からの距離が予め定めた第１４の閾値以上の場合、又は、基本移動動作が教示軌道に基づく場合かつ教示軌道からの最短距離が前記第１４の閾値以上の場合、前記ロボットの手先部の動作を止めることを特徴とする。
【００２３】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、粗倣い動作実行時の、第１の粗倣い構成動作又は第２の粗倣い構成動作の実行時に、前記両者間に作用する力が、予め定めた第１５の閾値以上である場合、第１の粗倣い構成動作又は第２の粗倣い構成動作における所定移動速度を、そのときの所定移動速度より小さくする動作調整手段を含むことを特徴とする。
【００２４】
また本発明のロボット制御装置は、粗倣い実行中又は基本移動動作の実行中に取得した位置データをもとに、所定の速度指令で動かす教示データを生成する教示データ生成手段を備えることを特徴とする。
【発明の効果】
【００２５】
本発明によれば、ロボットを用いてツールとワークとを相対的に移動させているときに、障害物、壁、穴、大きな凹凸等の未知な形状があった場合でも、基本移動動作と粗倣い動作とを、力検出手段で検出する力の情報、及びロボットが動かしている物体の位置情報の少なくとも一方に基づいて適宜切り替えることにより、ロボット、作業ツール及びワークに対して過負荷を与えず、好適な回避動作を行いつつなるべく形状に沿った迅速な動作を行うことが可能となる。
【図面の簡単な説明】
【００２６】
【図１】本発明に係るロボット制御装置を含むロボットシステムの概略構成を示す図である。
【図２】図１のロボット制御装置の機能ブロック図である。
【図３】ワークに予期せぬ凹凸形状がある場合に基準進行方向及び基準押付方向を規定する例を示す概略図である。
【図４】ワークに予期せぬ凸形状がある場合に基準進行方向及び基準押付方向を規定する他の例を示す概略図である。
【図５】力制御による倣い動作を説明する図である。
【図６】進行方向について閾値を超える力がかかった場合の処理について説明する図である。
【図７】本発明によって好適に倣うことができるワーク形状の例を説明する図である。
【図８】粗倣い構成動作１及び粗倣い構成動作２の移動方向の具体的求め方を説明する図である。
【図９】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１０】粗倣い構成動作１を含む粗倣い動作の具体例を示す図である。
【図１１】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１２】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１３】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１４】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１５】粗倣い構成動作１を含む粗倣い動作の具体例を示す図である。
【図１６】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１７】粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１８】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１９】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２０】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２１】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２２】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２３】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２４】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２５】粗倣い構成動作２の移動距離又は移動方向を徐々に変化させた例を示す図である。
【図２６】粗倣い構成動作１及び粗倣い構成動作２の一方又は双方の移動方向を変えた例を示す図である。
【図２７】教示経路に適当な補間点を挿入した例を示す図である。
【図２８】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図２９】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３０】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３１】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３２】基本移動動作が位置制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３３】図３２の場合において、粗倣い動作を滑らかに行った例を説明する図である。
【図３４】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３５】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３６】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３７】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３８】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３９】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図４０】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図４１】教示軌道上に、ツールを基準押付方向に押し付けるようなワーク形状がない場合の、ツールの好適な移動例を説明する図である。
【図４２】本発明の応用例として、ワーク形状が円周上に凹凸部が設けられたものである例を示す図である。
【発明を実施するための形態】
【００２７】
以下、添付図面を参照しつつ、本願発明の好適な実施形態について説明する。なお特に断らない限り、「力」は、ツールとワークとの間に作用する力によってもたらされ、力検出手段によって得られたデータに基づく物理量とする。また「所定」又は「予め定めた」なる語は、関連する処理が実行されるまでに定められればよいという意味で用いられ、いつ定めるかについての特段の制約はないものとする。また、以降の説明や図においては、第１の粗倣い構成動作を粗倣い構成動作１、第２の粗倣い構成動作を粗倣い構成動作２とも記述する。
【００２８】
図１は、本発明に係るロボット制御装置を含むロボットシステムの構成例を示す図である。ロボットシステム１０は、ロボット１２と、ロボット１２の動作、具体的には該ロボットの各軸の位置を制御するロボット制御装置１４とを有する。なお図示例のロボットは６自由度の多関節型ロボットであるが、本願発明を実施するに際し６自由度の多関節型ロボットである必要はない。ロボット１２のアーム先端には力検出手段としての力センサ１６と、ワーク加工等の用途に供される作業ツール１８とが取り付けられ、力センサ１６はツール１８に作用する力を検出する。またツール１８が加工すべきワーク２０は、例えば作業台２２等の所定の位置に配置される。配置された該ワーク２０は、移動動作が可能な装置などによって移動させてもよい。
【００２９】
図２は、ロボット制御装置１４の処理機能と各機器等の関係を示すブロック図である。ロボット１２の各軸の位置を検出するエンコーダ等の各軸位置検出部２４からの検出データが、位置、速度、加速度検出・算出部２６に送られる。位置、速度、加速度検出・算出部２６は、各軸位置検出部２４からの所定時間間隔毎の各軸位置データ等に基づき、ロボット１２の位置、速度及び加速度を検出・算出する。或いは加速度は、図示しない加速度センサを用いて求めてもよい。
【００３０】
位置、速度、加速度検出・算出部２６の処理結果は、作用力算出部２８に送られる。作用力算出部２８は、力検出手段１６で検出した力又はモーメント、位置、速度、加速度検出・算出部２６からの位置、速度及び加速度に関するデータ、ロボットの各構成要素の質量・重心位置等のデータ、並びに作業ツール１８の進行方向、押付方向に関するデータに基づいて、ツール１８とワーク２０との間に作用する作用力、さらには、ツール１８とワーク２０との間に作用する作用力のうち、所定方向の成分を算出することができる。
【００３１】
作用力算出部２８での算出に使用されるパラメータは、第１のパラメータ格納部３０及び第２のパラメータ格納部３２に格納することができる。図２の例では、第１のパラメータ格納部３０には、後述する基本移動動作、粗倣い動作を行うための動作パラメータが格納される。一方第２のパラメータ格納部３２には、力センサがロボット手首に設けられ、その先にツール又はワークが取り付けられている場合に、ツールとワークとの間に作用する正味の力以外で、該力センサに作用する力を算出するために必要なパラメータが格納されており、例えばロボットの各構成要素の質量や重心位置のデータが格納されている。また特に示していないが、後述する算出方法において必要となるパラメータも、ロボット制御装置内部に格納することができる。
【００３２】
動作切り替え判定・動作パラメータ調整部３４は、ロボットの位置・速度・加速度と、ツールとワークとの間に作用する作用力、又は該作用力のうちの所定方向の成分と、基本移動動作、粗倣い構成動作１、粗倣い構成動作２のうち、現在行われている動作と、基本移動動作のパラメータ及び粗倣い動作のパラメータに基づいて、動作切り替えの判定を行うとともに、場合によっては第１のパラメータ格納部３０内のパラメータを変更し、基準進行方向及び基準押付方向等をもとに、指令演算部３６に対して、行うべき動作を指定する動作指令を作成する機能を備える。各動作の詳細については後述する。
【００３３】
指令演算部３６は、ロボットの位置・速度・加速度と、ツールとワークとの間に作用する作用力又は該作用力のうちの所定方向の成分と、動作切り替え判定・動作パラメータ調整部３４による動作指令に基づいて、ロボット１２に送る指令を演算する。なお本実施形態では、粗倣い動作実行手段、動作調整手段及び教示データ生成手段の各機能は、指令演算部３６が担うものとする。
【００３４】
なお基準進行方向、基準押付方向算出・更新部３８は、第１のパラメータ格納部３０に格納されているデータと、位置、速度、加速度検出・算出部２６の検出・算出結果とに基づいて、必要に応じて、ワークに対するツールの基準進行方向及び基準押付方向（後述）を算出し又は更新する。算出され又は更新された基準進行方向及び基準押付方向は、作用力算出部２８での処理、及び、粗倣い構成動作１又は粗倣い構成動作２の移動動作のパラメータの算出や調整等に使用される。
【００３５】
本実施形態における力検出手段１６は、Ｘ、Ｙ、Ｚ軸方向の並進力及び各軸回りのモーメントを検出できる力センサとする。該力センサは、ロボット１２の手首と作業ツール１８との間に配置される。好適な力センサは６軸の力センサであるが、目的とする方向に作用する力を検出できる力センサであれば、検出できる力及びモーメントの自由度や検出原理等は問われず、どのようなタイプの力センサであってもよい。また、力センサはロボット１２の手首部及びロボット１２が載置されている床面のいずれに設置されてもよく、要するにツール１８とワーク２０との間に作用する力を算出できるのであれば、その設置場所に制約はない。さらに、力センサを用いずに、ロボット１２を駆動するアクチュエータの電流値に基づいて力を推定する手段を力検出手段１６として用いることも可能である。
【００３６】
次に、ロボット制御装置１４からロボット１２に対して指令される各動作について説明する。先ず基本移動動作は、ロボット１２を用いて、ツール１８とワーク２０とを相対的に移動させる動作であり、具体的には以下の動作が挙げられる。
（１）予め教示された位置等に基づいて指定された軌道に沿って動作
（２）ツールとワークとの間に作用する力が目標値と一致するように力制御を行いながら、ワークの形状に沿ってツールを倣わせる動作
ここで（２）における力制御としては、以下のような具体的方法が考えられる。
（２ａ）教示軌道によって与えられる各位置における目標進行方向が、該教示軌道における進行ベクトルによって与えられており、該位置を参考として力制御を行う
（２ｂ）教示軌道を中心にばね成分を持たせた押付制御を行いながら、該教示軌道に基づいて力制御を行う
（２ｃ）ツールとワークとの間に作用する力、及び移動中のツール又はワークの位置に基づいて、ワークの形状を推定しながら力制御を行う
上記以外の方法であっても、ロボットを用いてツールとワークとを相対的に移動させる動作であれば、本発明の趣旨から外れないことは当業者には明らかである。
【００３７】
次に、粗倣い動作について説明する。粗倣い動作は、上述の粗倣い構成動作１及び粗倣い構成動作２から構成される。粗倣い構成動作１は、ツール及びワークの一方を他方から、各時点の位置よりも（通常は局所的に）遠ざける動作であって、予め定めた移動時間又は予め定めた移動距離を限度にツール又はワークを移動させる動作である。また粗倣い構成動作２は、ロボットが移動させている一方に対して他方の存在を確認しようとする動作であり、多くの場合、ツール及びワークの一方を他方から、各時点の位置よりも（通常は局所的に）近付ける動作であって、予め定めた移動時間又は予め定めた移動距離を限度にツール又はワークを移動させる動作である。
【００３８】
詳細は後述するが、粗倣い動作実行中は、力検出手段で検出する力の情報、或いは又はこれに加えて、ロボットが動かしている物体の粗倣い動作実行中の位置情報、基準進行方向及び基準押付方向に基づいて、粗倣い構成動作１又は粗倣い構成動作２の移動方向、移動速度、移動時間及び移動距離の少なくとも１つを、必要に応じて変化させながら、ワーク上の異物や障害物等に対してツールを適切に移動させることができる。
【００３９】
上述の基準進行方向及び基準押付方向とは、粗倣い動作実行中のある位置において、粗倣い構成動作１又は粗倣い構成動作２を行うときに、その移動方向を定めるために基準とする方向である。すなわち基準進行方向とは、予め又は動作実行中に定められる方向であって、粗倣い動作実行中に基準とする進行方向であり、より具体的には、粗倣い動作実行中にツールを進ませるために参考にする（通常は移動方向に概ね一致）方向である。一方基準押付方向とは、予め又は動作実行中に定められる方向であって、粗倣い動作実行中に基準とする押付方向であり、より具体的には、粗倣い動作実行中にワークに対して押付動作をするために参考にする（通常は押付方向に概ね一致）方向であり、上記基準進行方向に基づいてある領域を通過する際の押付方向である。但し、上記基本移動動作が位置制御に基づくものである場合は、該基準押付方向は教示軌道に近付く方向とする。
【００４０】
以下、図３及び図４を参照して、上記基準進行方向及び基準押付方向の定め方の具体例を説明する。
図３に示すように、ワーク２０の表面に沿ってツール１８を右方から左方へ移動させる場合に、ワーク２０に予期せぬ凹凸形状、すなわち凸状部分４０及び凹状部分４２が存在するとする。この場合、凸状部分４０はツールの通過動作の障害となるが、基準進行方向は、矢印４４で示すように凸状部分４０が存在しない（ワーク形状は破線のように平坦である）とした場合の基準進行方向（予め教示軌道等によって与えられる移動方向）を適用し、一方基準押付方向は、矢印４６で示すように、基準進行方向４４に垂直かつワークに向かう方向とする。
【００４１】
一方、凹状部分４２についても、基準進行方向は、矢印４８で示すように凹状部分４２が存在しない（ワーク形状は破線のように平坦である）とした場合の基準進行方向（予め教示軌道等によって与えられる移動方向）を適用し、一方基準押付方向は、矢印５０で示すように、基準進行方向４８に垂直かつワークに向かう方向とする。
【００４２】
他の例として、図４のようにワーク２０が凹面状部分５２を有し、凹面状部分５２に予期せぬ凸状部分５４が存在する場合を考える。この場合も、凸状部分５４はツールの通過動作の障害となるが、基準進行方向は、矢印５６で示すように凸状部分５４が存在しない（ワーク形状は破線のような滑らかな凹面形状である）とした場合の基準進行方向（予め教示軌道等によって与えられる移動方向）を適用し、一方基準押付方向は、矢印５８で示すように、基準進行方向５６に垂直かつワークに向かう方向とする。
【００４３】
図４のように教示軌道５６が曲がっていて，教示軌道上の位置に対応する進行方向及び押付方向が算出できる場合は、粗倣い動作実行中の位置から教示軌道に対して直交するように、或いは最短距離となる方向に線を引いたときの交点に対応する、進行方向や押付方向を用いてもよい。或いは押付方向として、対応する教示された進行方向をもとに算出した方向を用いてもよい。
【００４４】
このように、基準進行方向は、ワークに対してツールがある領域を通過する際に大局的に進ませたい方向とする。具体的には、障害となる部分を通過する方向、予期せぬ大きな凹凸形状部分を通過する方向、障害物がないとした場合に教示軌道によって与えられる移動方向である。また基準押付方向は、上記基準進行方向に基づいて、障害物がないとした場合に教示軌道に基づいて倣う場合のツールの押付方向、障害となる形状や予期せぬ大きな凹凸形状部分を通過する際の押付方向となる。但し、上記基本移動動作が位置制御である場合、基準押付方向は教示軌道に近付く方向とする。
【００４５】
図３及び図４は基準進行方向及び基準押付方向の定め方の一例であり、故に基準進行方向及び基準押付方向は、一連の作業を通して設定された方向等の、予め設定された方向を使用してもよい。
【００４６】
また基準進行方向及び基準押付方向は、基本移動動作から粗倣い動作に切り替えたときに予め設定されていた教示データによって与えられる方向としてもよい。
【００４７】
或いは基準進行方向及び基準押付方向は、基本移動動作から粗倣い動作に切り替えたときに実際に移動していた位置に基づいて算出した進行方向と、該進行方向に基づいて算出される押付方向としてもよい。
【００４８】
さらに或いは基準進行方向及び基準押付方向は、粗倣い動作中に更新された値を使用して求めてもよい。上述のように粗倣い動作の開始時（基本移動動作からの切替時）の進行方向及び押付方向に基づいて基準進行方向及び基準押付方向を設定した場合、障害となる形状が大きいと、参考とするツールの進行方向及び押付方向が、ワーク形状に合ったものでなくなることがある。このような場合には、基準進行方向及び基準押付方向を、粗倣い動作実行中に、教示軌道に基づいてワーク形状に沿った方向に更新することが望ましい。
【００４９】
本発明の主たる特徴は、ロボットを用いてツールとワークとを相対的に移動させているときに、障害物、壁、穴、大きな凹凸等の未知の形状を検出した場合、基本移動動作と粗倣い動作とを、力検出手段で検出する力の情報、或いは又はこれに加えて、ロボットが動かしている物体の位置情報に基づいて適宜切り替え、ロボット、作業ツール及びワークに対して過負荷を与えず、なるべく形状に沿って、必要に応じて回避しながら動かすことを迅速に行うことにある。なお粗倣い動作は、力や位置等に基づき、ワーク形状に応じて積極的にツール又はワークの一方を他方に対して現在の位置から局所的に遠ざける動作と、一方が他方の存在を確認する動作とを繰り返して行う、倣い動作を意味する。
【００５０】
但し、ワーク（他方）の形状が未知である場合、ある時点での移動動作が、他方の存在を確認するために近付く動作であるか離れる動作であるかは判別できない。そこで、一方が局所的に他方から離れることを目的とする動作を粗倣い構成動作１と称し、この動作の実行中には、基準進行方向及び基準押付方向に基づいて移動し、力検出手段を用いて検出された力によって移動方向が不適切であると判断された場合は移動方向等を変更しながら、一方が予め定めた（実行するまでに定められさえすればよい）時間又は距離を限度に、もとの位置から離れるような動作が行われる。また一方が他方の存在を確認することを目的とする粗倣い構成動作２では、障害となる物体を通過する際に基準となる基準進行方向及び基準押付方向に基づき、一方を他方が存在する方向に進ませるような動作が行われる。
【００５１】
以下、基本移動動作と粗倣い動作との切り替えの具体例について説明するが、基本移動動作には、力制御に基づくものと位置制御に基づくものとがあるので、先ずその両者に分けて説明を行う。基本移動動作が力制御に基づくものである場合、上記基本移動動作は、力制御によって押付力を制御しながら、ツールとワークとの間に作用する力が目標値（動作中に調整可能）となるようにツールをワーク形状に倣わせる動作であるとする。教示点に基づく教示軌道によって、力制御で倣わせるときに参考とする位置が与えられており、ある位置における進行方向は教示軌道によって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行うこともできる。また粗倣い動作は、ワーク形状に応じて積極的に、ツール又はワークの一方を現在の位置から遠ざける動作（局所的又は相対的に他方から離す動作）と、一方が他方の存在を確認する（通常は一方を他方に近付ける）動作とを繰り返す倣い動作である。また力制御に基づく基本移動動作を行っているときに粗倣い動作を行うべき場合としては、押付力を適宜調整しながらロボットを動かしているときに、障害物、壁、大きな凸凹等の、それまで倣ってきた形状とは異なり、適切に倣わせることが難しい形状物が存在し、それまでの力制御による倣い方法とは異なる方法に切り替えることが必要な場合が考えられる。
【００５２】
一方基本移動動作が位置制御に基づくものである場合は、上記基本移動動作はロボットを用いて作業ツールとワークとを相対移動させるにあたり、予め定めた教示点に基づき設定された教示経路に沿って作業ツールを移動させることを目的とする移動動作である。粗倣い動作については力制御の場合と同様である。また位置制御に基づく基本移動動作を行っているときに粗倣い動作を行うべき場合としては、以下のような場合が考えられる。
（１）ワークに対してツールを位置制御で動かし、バリ取り、研削、研磨作業等を行う場合。軌道を少しずつずらした位置制御を複数回行い、目的の作業を達成する場合もある。
（２）ワークの表面・形状状態や、ワークに存在する異物などを確認する検査等において、予め定めた範囲内でセンサ等（前記力検出手段に取り付けられた接触確認用の作業ツール等）の移動を行い、異物等の未知形状物があった場合に該未知形状になるべく沿ってセンサ等を動かしたい場合。
（３）ロボットの手先部を動かしているときに意図しない障害物があった場合、なるべく該障害物の形状に沿って該手先部を動かしたい場合。
【００５３】
図５は、力制御による倣い動作（基本移動動作）を説明する図である。教示点６０、６２に基づく教示軌道によってワーク２０に対するツール（図示せず）の進行方向が定められ、該進行方向に合わせて教示押付方向を決める倣い動作が行われる。目標進行方向６４に沿ってツール進めながら目標押付方向６６に対して力制御を行っているときに、軌道上に教示軌道とは異なる形状（例えば図５に示すような山形状の突起）が存在すると、該突起の表面が比較的滑らかであれば、押付方向６６について反力６８がかかるため好適な倣いが可能となることもある。しかし、押付方向成分に目標力がかかっているときに、その部分の傾斜角度や目標力によっては、進行方向（ここでは実際の位置に基づく進行方向ではなく、教示軌道によって与えられる進行方向を意味する。この場合、基準進行方向と同じ方向を意味しており、以降の説明においても、進行方向は、特に断わらない限り、同様の意味で使用する。）について過大な反力７０がかかり、特に進行方向の速度が速いときは好適な倣いができないことがある。このような場合、力に応じて進行速度を下げたり、目標押付力を下げたり、力制御の追従性能を向上させたりする等の工夫が必要であり、異物の形状によってはそれでも対応できない場合がある。
【００５４】
そこで本発明では、図６に示すように、ツール１８をワーク２０に対して目標力で押し付ける基本移動動作を実行しているときに、進行方向について閾値を超える力がかかった場合、上述の基準進行方向及び基準押付方向に基づき、粗倣い構成動作１と、粗倣い構成動作２と、それらの動作切り替え判定とを繰り返す（切り替えをしない場合もあり、また順序は問わない）。つまりツール１８をワーク２０から離す動作（矢印７２）と、ワーク形状を確認する動作（矢印７４）とを適宜切り替えて反復し、基準進行方向４４に障害物がないことが確認できたら、もとの基本移動動作に切り替えることによって、大きさや形状が未知な障害物に対しても、該障害物を回避しつつもなるべく該障害物に沿った移動が可能となる。また上述の動作切り替えをより細かく繰り返せば、ワーク形状により正確に沿った移動が可能となる。なお以降の説明において、基準進行方向は実線矢印４４、基準押付方向は破線矢印４６で表すものとする。
【００５５】
本発明によれば、図７（ａ）に示すようにツールの移動方向が基準進行方向に対して９０度以上変更させられる場合にも対応でき、また図７（ｂ）に示すようにコーナー部の倣い動作において、教示点が２点のみであっても好適に倣うことができる。
【００５６】
この場合の粗倣い動作の動作方法としては、種々考えられるが、粗倣い構成動作１及び粗倣い構成動作２の移動方向、移動速度及び動作時間を状況に応じて変えたり、粗倣い構成動作１、粗倣い構成動作２及び基本移動動作相互間での切り替えを適切に行ったりすることによって、状況に対応したより複雑な動作を実現できる。
【００５７】
次に、粗倣い構成動作１及び粗倣い構成動作２それぞれにおける移動方向の求め方の具体例を説明する。先ず基本移動動作から、粗倣い動作に切り替えたときの進行方向及び押付方向を、それぞれ基準進行方向及び基準押付方向として設定する。ここで、動作中の各位置に対応する教示軌道上の位置（例えば、その位置から最短距離にある教示軌道上の位置）で与えられる進行方向及び押付方向を、それぞれ基準進行方向及び基準押付方向としてもよい。
【００５８】
粗倣い構成動作１及び粗倣い構成動作２の移動方向は、基準進行方向及び基準押付方向の単位ベクトルに、実数（±の値、ゼロも含む）である係数をかけて、合成和をとることにより求める。具体的には、図８に示すように、粗倣い動作に切り替えたときの基準進行方向の単位ベクトルをＤrm、基準押付方向の単位ベクトルをＤrpとするとき、実数である係数αm1、βp1、αm2及びβp2を用いて、粗倣い構成動作１を実行するときの移動方向Ｄr1は、（αm1 Ｄrm ＋ βp1 Ｄrp）を正規化したベクトルとする。一方、粗倣い構成動作２を実行するときの移動方向Ｄr2は、（αm2 Ｄrm ＋ βp2 Ｄrp）を正規化したベクトルとする。なお、Ｄr1とＤr2とは互いに異なるものとし、また互いに反対方向の関係にもならないようにする。なお粗倣い構成動作２では、ツールがワークに接触するまで行わなくともよく、例えば接触前に粗倣い構成動作１に切り替えてもよい。
【００５９】
上述の係数は、ツールがワークに衝突したときは、そのときの反力に基づいて適切な移動方向を求めるべく調整されたり、粗倣い動作における粗倣い構成動作１又は粗倣い構成動作２の繰り返し動作での、粗倣い構成動作１又は粗倣い構成動作２の開始位置の変化量が小さいとき、移動方向を適切な方向にずらすように調整されたりすることが好ましい。また粗倣い構成動作１及び粗倣い構成動作２による粗倣い動作は、複数の直線的な動きを滑らかにつなげた移動や曲線的な移動を含むものとし、全体的により滑らかな動作とすることもできる。さらに、後述するような調整をおこなった場合、該調整をもとに、移動方向を算出するためのパラメータや基準進行方向、基準押付方向を変更し、移動方向を算出してもよい。
【００６０】
本発明の実施形態においては、粗倣い構成動作１及び粗倣い構成動作２の移動方向ベクトルは、上述のように基準進行方向ベクトルと基準押付方向ベクトルの線形結合和により求められる。それぞれのベクトルにかける係数は、ツールとワークとの間に作用する力や、ツール又はワークの粗倣い動作中の位置に基づき、状況に応じて変更可能であり、粗倣い構成動作１及び粗倣い構成動作２のそれぞれにおいて、適切な方向となるように係数を変えるものとする。また粗倣い動作中の基準進行方向ベクトル及び基準押付方向ベクトルは、粗倣い動作に切り替えたときの進行方向及び押付方向を使用するものとする。但し、問題となる部分を通過することができる方向であればその方向を使用してもよい。
【００６１】
粗倣い構成動作１、粗倣い構成動作２及び基本移動動作相互の切り替え方法としては、次のようなものが考えられる。
先ず図９に示すように、矢印７２で示す粗倣い構成動作１の実行中に、予め定めた閾値を超える力を検出せず、かつ、予め定めた移動時間が経過し又は予め定めた移動距離だけ移動したら、動作を矢印７４で示す粗倣い構成動作２に切り替える。
【００６２】
図１０（ａ）に示すように、矢印７２ａで示す粗倣い構成動作１の実行中に、予め定めた閾値を超える力が検出され、すなわちツールがワーク又は他の障害物に衝突したが、ツールが比較的狭い領域内にあってワークから離すために所定時間又は所定距離だけ動かすことができない場合は、ワークに対するツールの移動方向を変更して、再度、矢印７２ｂで示すような粗倣い構成動作１を実行するか、ツールをワークに対して停止させることができる。図１０（ｂ）は同様の考え方により、矢印７２ａで示す粗倣い構成動作１から矢印７２ｂで示す粗倣い構成動作１、さらに矢印７２ｃで示す粗倣い構成動作１に切り替えられていく様子を示している。
【００６３】
ここで粗倣い構成動作１の実行時に、予め定めた閾値を超える力を検出することなく所定時間又は所定距離だけツールを移動させることができれば、粗倣い構成動作２に切り替えてもよい。また次回以降の粗倣い構成動作１では、矢印７２ｂ、７２ｃで示すような変更された粗倣い構成動作１を用いてもよい。このようにすれば、実際のワーク形状に対して粗倣い構成動作１の移動方向が不適切であっても、所望の方向に所定時間又は所定距離だけツールを移動させることができる。
【００６４】
図１１に示すように、矢印７４で示す粗倣い構成動作２の実行中に、予め定めた閾値を超える力が検出され、すなわちツールがワーク又は他の障害物に衝突したときは、動作を矢印７２で示す粗倣い構成動作１に切り替える。
【００６５】
図１２に示すように、矢印７４ａで示す粗倣い構成動作２の実行後に、予め定めた閾値を超える力が検出されず、すなわちワーク又は障害等となる形状が確認できないとき、矢印７４ｂで示すように移動方向を変更し、粗倣い構成動作２を実行してもよい。これにより、粗倣い構成動作２によるワーク又は障害物等の確認が不十分であることを防止できる。
【００６６】
図１３に示すように、矢印７４で示す粗倣い構成動作２の実行中に、予め定めた閾値を超える力が検出されず、すなわちワーク又は障害物等となる形状が確認できないときに、矢印７６で示す基本移動動作を再開してもよい。なお図１３の基本移動動作のように、力制御による倣いでは、ツールを押付方向に動かしワークとの接触を確認するまでは進行方向に進ませないか、或いは押付力に応じて移動速度を変化させると、ワークとツールとが接触しない領域を小さくすることができる。
【００６７】
図１４に示すように、矢印７４で示す粗倣い構成動作２の実行中に、基準進行方向４４について予め定めた閾値を超える力が検出されず、一方基準押付方向４６については予め定めた閾値を超える力が検出された場合は、ワークは存在するが、その時点から基本移動動作で倣うことが可能であると判断されるので、矢印７６で示すような基本移動動作を再開してもよい。
【００６８】
図１５に示すように、矢印７２で示す粗倣い構成動作１の実行中に、基準押付方向４６について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）が検出され、基本移動動作を行うためにツールが倣えるような形状の存在が確認されたときは、基本移動動作７６を再開してもよい。
【００６９】
図１６に示すように、矢印７２ａで示す粗倣い構成動作１の実行中に、基準押付方向について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）は検出されないが、それ以外の方向について別の予め定めた閾値を超える力が検出され、すなわちツールとワーク又は障害物との衝突が確認されたときは、ツールの移動方向を変えて、再度、矢印７２ｂで示す粗倣い構成動作１を実行するか、或いはツールを停止させてもよい。再度の粗倣い構成動作１（７２ｂ）の後は、粗倣い構成動作２及び粗倣い構成動作１を繰り返している。なお図１６（ａ）はツールが連続しない他のワーク又は障害物に接触した場合であり、図１６（ｂ）は連続するワークの他の部分に接触した場合である。
【００７０】
なお図１６の例は、矢印７２ａで示した粗倣い構成動作１の実行時において衝突したワーク又は障害物にツールを倣わせたくない場合を示しているが、該衝突以降、粗倣い動作で該衝突した部分の形状に倣わせたいときであって、さらに、粗倣い構成動作１の実行時に、基準押付方向について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）は検出されないが、それ以外の方向について別の予め定めた閾値を超える力が検出され、ワークに衝突したことが確認されたときは、図１６（ｂ）において破線矢印で示すように、後述する基準進行方向に山形状の突起等の障害物が存在しているときの粗倣い動作に切り替え、ツールが障害物を乗り越えるような粗い動作を行ってもよい。なおこの場合において、力を検出したときの位置が粗倣い構成動作１の開始位置から予め定めた閾値を超えて離れているときは、粗倣い動作を停止し、障害物となる形状を越えるような粗倣い動作をさせてもよい。
【００７１】
図１７に示すように、矢印７４で示す粗倣い構成動作２の実行中に、基準押付方向について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）が検出され、基本移動動作を行うためにツールが倣えるような形状の存在が確認されたときは、基本移動動作７６に切り替え（再開）してもよい。
【００７２】
図１８に示すように、矢印７４で示す粗倣い構成動作２の実行中に予め定めた閾値を超える力が検出されず、所定の移動時間又は所定の移動距離を限度にツールが移動したら、ツールがワークに接触していなくとも、矢印７２で示す粗倣い構成動作１を実行することができる。図１８の例は、粗倣い動作においてツールを必要以上に基準進行方向と逆方向に進ませずに、ワーク形状に対して粗く倣わせたい場合に有効である。なお１回の粗倣い構成動作２において、基準進行方向と逆方向にどの程度を限度としてツールをワークに近付けるかは、各パラメータによって調整可能である。
【００７３】
図１９に示すように、矢印７４で示す粗倣い構成動作２の実行中に、予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）は検出されないが、それ以外の方向について、別の予め定めた閾値を超える力が検出され、すなわちツールがワークに衝突してワーク形状の存在が確認されたときは、矢印７２で示す粗倣い構成動作１を実行することができる。
【００７４】
図２０に示すように、矢印７４ａで示す粗倣い構成動作２の実行後に、ツールの移動方向、移動距離又は移動時間を変えて、再度、矢印７４ｂで示すような粗倣い構成動作２を実行してもよい。これによって、粗倣い構成動作２において、ツールがより確実にワーク形状に沿う（接触する）ようにすることができる。換言すれば矢印７４ｂで示す粗倣い構成動作２は、ツールがワークに接触するように、矢印７４ａで示す粗倣い構成動作２の実行後に、移動方向、移動距離、移動時間又は移動速度を変えるものである。
【００７５】
図２１に示すように、図２０のようにツールをワークに接触させずとも、矢印７４ａで示す粗倣い構成動作２を実行後に移動方向、移動距離、移動時間又は移動速度を変えて、矢印７４ｂで示す粗倣い構成動作２を実行し、ワーク接触前に矢印７２で示す粗倣い構成動作１を実行し、その後再び、矢印７４ａ、７４ｂで示す粗倣い構成動作２を行って、ツール移動方向を基準進行方向４４の逆方向にずらし、ワーク側に近付けていくようにしてもよい。
【００７６】
図２２に示すような窪み形状のワークにツールを倣わせる場合において、図２２（ａ）の左上方に示すように、矢印７２ａで示す粗倣い構成動作１の移動方向にツールを動かすとワークに接触し、粗倣い構成動作１を所定時間又は所定距離だけ行うことができず、故に反力を受けた方向に基づいて粗倣い構成動作１の移動方向をずらして、再度、矢印７２ｂで示す粗倣い構成動作１を行っている場合を考える。
【００７７】
このとき、図２２（ｂ）に示すように、矢印７２ｂで示す粗倣い構成動作１に続く粗倣い構成動作２（矢印７４）によってツールを基準進行方向に移動させると、ずっとその位置付近での移動を繰り返す状態に陥ることがある。
【００７８】
そこで、粗倣い構成動作１を繰り返し実行した後の位置が、基準押付方向４４と逆方向の位置について、その時点までで該逆方向に最も進んだ位置より手前の位置、又は、該最も進んだ位置から所定距離を超えて該逆方向の位置に存在しないなら、ツールの移動方向を、基準押付方向の逆方向かつ基準進行方向の逆方向にずらし、矢印７２ｃで示すような粗倣い構成動作１を行う。これにより、図２２のような窪み部分等において、ツールがある領域内での移動を繰り返して該領域から脱出できないという状況を避けることができる。
【００７９】
なお図２２（ａ）を用いて説明した方法を用いてもなお、粗倣い動作実行中においてある領域内での移動を繰り返す状態に陥っていると判断された場合は、以下のような処置を採ることができる。
（１）基準押付方向４６と逆方向（図２２では上向）に最も進んだ位置と比較するときの判定距離を大きくする。
（２）粗倣い構成動作１及び粗倣い構成動作２の移動時間又は移動距離を大きくする。
（３）移動中の位置をもとに、ツールの停滞状況が解消されるように粗倣い構成動作１又は粗倣い構成動作２の移動方向を変える。
【００８０】
図２３に示すように、矢印７２で示す粗倣い構成動作１の実行時に、押付方向成分の力が小さいため、基準押付方向について予め定めた閾値を超える力は検出されないが、基準進行方向について閾値を超える力が検出された場合、以下の方法が考えられる。
（１）矢印７４で示す粗倣い構成動作２に切り替え、ツールを基準進行方向と逆方向に移動させる。
（２）又は、ワークからの反力の大きさ、方向に基づき、基準進行方向とは逆方向の成分を持つ方向にツールを移動させる粗倣い構成動作１を行う。移動方向を変えた粗倣い構成動作１によって、ツールがワークと接触し、基準進行方向と逆方向について閾値を超える力が検出されたら、粗倣い構成動作１の移動方向を基準進行方向の成分を含む方向に変更し、移動時間又は移動距離を小さくする。
【００８１】
或いは、元の基本移動動作に切り替え、そのときの力をもとに、直ちに基準進行方向に障害となる形状がある（そのときの位置から基準進行方向に山形状の突起がある）場合に対応した、粗倣い動作に切り替えてもよい。
【００８２】
基準押付方向について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）が検出されず、かつ、基準進行方向について予め定めた閾値を超える力（基準進行方向の逆方向からかかる力）も検出されず、かつ、別の予め定めた閾値を超える反力が検出された場合、粗倣い構成動作１の移動する向きを該反力をもとに変え、粗倣い構成動作１を実行することが考えられる。例えば、図２４に示すように、矢印７２ａで示す粗倣い構成動作１の実行時に、ツールが本来倣うべきワーク２０ａから離れ、他のワーク又は障害物２０ｂに接触して、基準押付方向の逆方向について予め定めた閾値を超える反力（基準押付方向からかかる力）が検出された場合、該反力の方向、大きさをもとに、ツールの移動方向を変え、矢印７２ｂで示すような粗倣い構成動作１を行う。また粗倣い構成動作１の実行時に、ツールがワークと接触することなく所定時間又は所定距離動くことができたときは、粗倣い構成動作２に切り替えてもよい。このように動作を切り替えることによって、粗倣い構成動作１の実行中に、実際のワーク形状に応じた動きをさせることができる。
【００８３】
図２５に示すように、粗倣い動作実行中において粗倣い構成動作２を行っているときに、ツールとワークとの間に作用する力及びツールの位置をもとに、ワークになるべく近付け又は接触させてワーク形状を確認できるように動作を切り替えることができる。具体的には、粗倣い構成動作２の実行時に、所定時間又は所定距離だけツールを移動させてもツールがワークに接触しない場合、基準進行方向の逆方向への移動距離を大きくするために、先ず全体的な変更許容値と１回動作当たりの変更許容値を決めておき、粗倣い構成動作２の実行時の移動距離、移動時間又は移動速度を大きくする。但し、移動速度を過大にすると衝突する際の力が大きくなるので、移動速度はあまり大きくしないことが望ましい。
【００８４】
図２５（ａ）は、矢印７４ａ、７４ｂ及び７４ｃで示すように、粗倣い構成動作２の移動距離を、ある制限範囲内で徐々に大きくしている様子を示している。一方図２５（ｂ）は、矢印７４ａ、７４ｂ及び７４ｃで示すように、粗倣い構成動作２の移動距離を、ある制限範囲内で徐々に大きくするとともに、移動方向を基準進行方向とは逆方向に少しずつ変化させている様子を示している。このようにすれば、粗倣い構成動作２の実行時に、ツールがワークに接触しない場合に、ツールをワークに接触するように移動させ、よりワーク形状に沿って動かすことができる。
【００８５】
基準押付方向にワークがあることが確認できるまで図２５のような動作を続けることにより、基準押付方向に押し付けるような形状がない（そのときの位置から基準進行方向に谷がある）場合に、大きさや形状が未知のワーク又は障害物に対しても、迅速かつ、なるべくツールをワーク又は障害物の形状に倣わせて動かすことができる。
【００８６】
粗倣い動作において、粗倣い構成動作１や粗倣い構成動作２を繰り返し実行しても、ツールの位置があまり変わらず停滞している場合や、ツールの進み方が遅い場合に、粗倣い動作の動きを調整してこれらの問題を解消することができる。以下、具体的に説明する。
【００８７】
先ず、粗倣い動作時に粗倣い構成動作１又は粗倣い構成動作２を、ある回数以上又は所定時間以上繰り返し実行したときに、ツールが移動した位置やその変化を監視する。例えば、粗倣い構成動作１の実行中の開始位置又は粗倣い構成動作２の実行中の開始位置をもとに、粗倣い動作の開始時又はある時点の位置に対する位置の変化や、粗倣い動作実行中に通過したことのある位置との変化量（通過したことのある位置と予め定めた閾値とから領域を求め、その中に入っていないか）や、ある位置を基準としてある回数実行又はある時間経過したときの距離や、ある位置を基準としてある回数実行したとき又はある時間経過したときの移動距離の変化の様子等を監視する。その監視結果から、ツールが同じような位置付近に戻ってきていたり、図２２で示したように同じような位置で停滞していたり、ある時間間隔で比較したときの位置の変化量が小さかったりしたときは、動作を変える必要があると判断する。なおこれらの原因としては、粗倣い動作のパラメータが不適切であることや、ワーク形状が複雑であることが考えられる。
【００８８】
一例として、図２６（ａ）に示すように、ワークに対するツールの進み方が遅いと判断される場合、粗倣い構成動作１及び粗倣い構成動作２の一方又は双方の移動方向を変えることが考えられる。なお図２６（ｂ）に示す例では、矢印７２で示す粗倣い構成動作１の移動方向を変えている。或いは、粗倣い構成動作１及び粗倣い構成動作２の一方又は双方において、予め定めた制限値内で、ツールの移動時間、移動速度又は移動距離を大きくすることも考えられる。このようにすれば、ツールが同じ位置付近からあまり動けていないときや進み方が遅いときに、その停滞状況から脱出したり、ツールがワーク形状に倣う時間を短くしたりすることができる。但し、ワーク形状への倣い方が粗過ぎないようにしたい場合は、そのように上記制限値を調整しておけばよい。
【００８９】
本発明では、粗倣い動作によってツールがワークに対して意図しない方向に移動し続けた場合に、安全のため、ロボットの動作を停止させることができる。ここで「意図しない方向に移動し続けている」状況としては、例えば以下のような状況が考えられる。
（１）ツールが全く予期せぬワーク形状にずっと倣い続けてしまっている。
（２）ツールがある部分に嵌り込んでしまっている。すなわち、移動方向や移動量等を変えても、その位置から動くことができない。
（３）動作パラメータが不適切であり、ある部分をツールが適切に回避して動くことができない。
【００９０】
上述の「意図しない方向に移動し続けている」状況を判定する具体的方法としては、粗倣い動作時に以下のことを検出又は確認することが考えられる。
（１）粗倣い構成動作１又は粗倣い構成動作２を、ある回数以上繰り返している。
（２）粗倣い動作の実行時間が、所定時間を超えている。
（３）粗倣い動作の移動位置が、想定される位置から離れすぎている。
（４）基本移動動作から粗倣い動作へ切り替えたときの位置から現在のツールの位置までの距離又は総移動距離が、所定距離を超えている。
（５）基本移動動作が教示軌道に基づく動作である場合に、ツールが、教示軌道からの最短距離が閾値以上の位置にいる。
上記のような状況が検出又は確認されたら、ツールが「意図しない方向に移動し続けている」と判断し、安全のため、ロボットの動作を止めることが好ましい。これにより、粗倣い動作によって想定外の動作をした場合の危険な状況を回避できる。
【００９１】
本発明では、粗倣い動作時において粗倣い構成動作１又は粗倣い構成動作２の実行中に、ワークに対するツールの移動速度が大きく、ツールとワークとが過大な力で接触するような場合、移動速度を下げる（調整する）ことによって過大な力で衝突しないようにし、ツールやワークの破損等を回避することができる。さらに、最初は移動速度をある程度小さな値に設定し、回避時間を短くするために移動速度を少しずつ大きな値に変え、反力がある程度以上になったら速度を下げ、その後、その速度で動かすようにして速度を適切な値に調整するという、ツール及びワークの状況に応じた適切な速度の自動調整を行うこともできる。
【００９２】
本発明では、ツールをワーク形状に沿って動かしたときの動作をもとに、教示データを生成又は修正することができる。具体的には、粗倣い構成動作１及び粗倣い構成動作２によって、ワークに対してツールを接離させる動作（接触が望ましいが接触していなくともよい）を繰り返して動かすとした場合、ツールとワークとの離隔と接触とが繰り返されるため、その動作ピッチを短くしたとしても粗倣い動作はジグザグな動きをすることになる。そこで、教示データの生成を行うときは、ロボット制御装置の、制御周期毎に計測した、粗倣い構成動作１及び粗倣い構成動作２、或いは又はこれに加え、基本移動動作の位置に基づいて、例えば以下のような方法を用いて、所定の速度指令で動かす教示データを生成し又は教示軌道を修正することができる。
（１）粗倣い動作時には、粗倣い構成動作１及び粗倣い構成動作２の実行中に力検出手段によってツールとワークとの接触を検出したときのツールの位置や、粗倣い構成動作２の最終位置を位置データとして使用する。
（２）連続する位置データにおける位置間の距離が予め定めた閾値以下であるような、互いに近い位置が存在する場合には、それらを軌道生成のためのデータとして用いず、適当な代表点のみを使用する。
（３）例えば図２７に示すように、位置データとして使用する点の位置の変化を見たとき、ある点までとそれ以降の部分とで急にツールの移動方向が変わる部分は角部であると推定し、位置データとして適当な補間点７８、８０を入れ、その位置データをもとに軌道を生成する。
（４）最小自乗近似等により、軌道生成に用いる位置データに沿った滑らかな軌道を生成する。
（５）教示データをもとに位置の補間等を行いながら滑らかに動くように移動するとき、実際に通過する位置と、教示データによって与えられる位置とが、角部やコーナ部などにおいてずれることがある場合、それを考慮し、位置データの位置と実際に通過する位置とのずれが所定閾値以下になるように、位置データを補正し、教示データを生成する。
（６）教示データ上の移動速度はユーザが設定した値を用いる。又は、作業の目的や条件に応じて、予め用意したデータテーブルを参照して、適切な移動速度を設定する。さらに、移動速度によって、実際に通過する位置と、教示データによって与えられる位置とがずれることがある場合、そのずれが所定閾値以下になるように、前記のように与えられる速度をもとに、位置に応じて調整した移動速度を設定してもよい。
（７）教示データ作成時に押付方向も設定する場合には、生成される軌道をもとにその進行方向と直交する方向を押付方向とする。
以上のようにすれば、粗倣い動作実行中の位置を、粗倣い構成動作１及び粗倣い構成動作２の動作の特徴を考慮しながら、或いは又はこれに加え、基本移動動作実行中の位置を、ロボット制御装置の制御周期毎に取得し、その位置をもとに、動作中の速度には関係なく、また教示データには不要な動作中の位置は除き、適当な間隔で位置データを生成することができ、故にワーク形状に沿った軌道を移動させる教示データを生成又は修正することができる。
【００９３】
図９〜図２７を参照して説明したように、粗倣い構成動作１及び粗倣い構成動作２における移動方向、移動速度及び移動時間は、ワークに対するツールの位置や両者間に作用する力に基づき、状況に応じて変更されることが好ましい。また粗倣い構成動作１及び粗倣い構成動作２において、これらの動作を行ってもツールがワークに対して同じ位置付近に留まることがないように、粗倣い動作の移動中の位置を監視しながら移動方向、移動速度及び移動時間の少なくとも１つが調整される。さらに、粗倣い構成動作１及び粗倣い構成動作２において、安全の観点から、移動時間又は移動距離に制限を設けておくことが好ましい。
【００９４】
粗倣い構成動作１及び粗倣い構成動作２において、ツールがワークに衝突するときに発生する力が、検出を目的とする閾値以外の別の閾値を超えた場合に、移動速度を小さくすることによってツールやワークに過負荷がかかることを防止できる。粗倣い動作実行中の位置に基づき、粗倣い動作の進み具合が遅いと判断された場合には、速度を適切に大きくしたり、移動方向を基準進行方向又は基準押付方向とは逆方向に変えたり、移動時間又は移動距離を大きくすることが好ましい。但し、そのような調整によってワーク形状への倣いが過度に粗くなって問題となる場合には、適切な値に制限しておくことが望ましい。
【００９５】
粗倣い構成動作２の移動時間又は移動距離は、適切な距離だけ移動するように調整することが好ましく、具体的には、移動距離が短過ぎると進行方向（教示軌道によって与えられる進行方向）にワークが存在していることを検出することができず、逆に移動距離が長過ぎると進行方向にワークが存在しないにも関わらず不必要に移動を続けることを考慮して、調整される。但し基準進行方向にツールを進ませながら、大まかにワーク形状に沿わせることで十分である場合は、ツールをワークに接触するまで移動させ続ける必要はなく、制限距離内の移動で十分である。
【００９６】
上記粗倣い構成動作２の移動時間又は移動距離の調整方法としては、以下のようなものが考えられる。
（１）粗倣い動作中の移動位置に基づいて求められる、ワーク形状及びツールの現在位置に基づき、粗倣い構成動作２の移動距離、移動時間及び移動速度を調整する。例えば、粗倣い構成動作２の実行前の、粗倣い構成動作１の開始位置から、基準進行方向について所定距離だけ先に進ませるように、パラメータを定める。
（２）実験に基づいて、使用するワーク形状に適した移動時間又は移動距離を定め、その値を限度としてツールを動かす。
（３）粗倣い動作中に調整された、移動時間又は移動距離を限度としてツールを動かす。
【００９７】
以下、力制御により基本移動動作を行う種々の形態について説明する。先ず図２８は、矢印７６ａ、７６ｂ、７６ｃで示す基本移動動作では、教示点８２、８４により定められる教示軌道に沿って力制御で倣わせるときに参考とする進行方向及び位置が予め与えられており、ある位置におけるツール１８の進行方向は、実際の位置に基づく変化ではなく、教示軌道における進行ベクトルによって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行っているとしてもよい。
【００９８】
図２８の例では、ワーク２０に障害物となる凸状物８６が存在しているので、矢印７６ａで示す基本移動動作から粗倣い動作７２ａ、７４ａに切り替えられ、凸状物８６をツール１８が乗り越えたら基本移動動作７６ｂに移行し、次に押付方向について、倣う形状が存在しなくなるので、粗倣い動作７２ｂ、７４ｂが実行され、再度基本移動動作（７６ｃ）に戻ることになる。状況に応じて粗倣い動作を変えるため、力検出手段で検出される力をもとに、粗倣い構成動作１及び粗倣い構成動作２の動作が設定される。
【００９９】
図２８の例では、状況判断のために、粗倣い動作７２ａ、７４ａによって障害となる形状を回避できたかどうか、粗倣い動作７２ｂ、７４ｂによって倣うべき形状が現れたかどうかを確認する。また粗倣い動作７２ｂ、７４ｂを用いなくとも、押付力に応じて移動速度をかえるような基本移動動作を行って、接触を確認するまで移動速度を小さくするようにしてもよい。また基本移動動作は、教示軌道をもとに、力制御を行い、教示軌道とは異なった軌道を動いているとする。基本移動動作で力制御を行う場合は、教示軌道が与えられておらず、ワーク形状に沿って倣う形態としてもよい。
【０１００】
図２９は、図２８の凸状物８６とは異なる形状の凸状物８８がワーク２０に存在している例を示す。矢印７６ａと７６ｂで示される基本移動動作の間の領域について、検出される力に基づいて粗倣い構成動作１の動作方向を変えながら、粗倣い構成動作２で基準進行方向に動かし障害物がないかどうかを確認しながらツール１８が進められる。力をもとに、障害物がないと判断されたら、基本移動動作（７６ｂ）でツールを倣わせる。前方に倣わせる物体が急になくなったとき、再び粗倣い動作に切り替え、所定の移動時間又は移動距離を限度に、ワーク２０に対するツール１８の接離（但し接触しない場合もある）を繰り返しながら、ツールをワークに倣わせる。このとき所定方向について力を検出したら、基本移動動作（７６ｃ）に再び切り替えられる。
【０１０１】
図３０に示すように、ワーク２０に未知の形状物９０が存在していても該形状物が緩やかな山のような形状であり、基本移動動作の実行中において力制御のみでもツール１８を倣わせることができる場合を考える。しかしこの場合において、山形状の突起物９０によって、進行方向についての予め定めた閾値を超えるような力はツールにかからないが、教示点８２、８４間に設定された教示軌道９２とツール１８の位置との乖離（距離）が予め定めた閾値Ｓ１を超えるときは、ツールの軌道が教示軌道から大きく離れ、基本移動動作を続行したのでは移動時間が長くなる虞があるので、ツール１８の位置に基づいて粗倣い動作に切り替えてもよい。
【０１０２】
図３０の例では、現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、該最短距離が予め定めた閾値Ｓ１を超え、進行方向（教示軌道によって与えられる方向を意味する。ここでは、基準進行方向と同じ）についてツールとワークとの間に作用する力に応じて粗倣い動作に切り替えるための予め定めた閾値を超えるような力はツールにかからないが、進行方向（教示軌道によって与えられる方向）について別の予め定めた閾値を越える力はかかっているときに、基本移動動作７６から粗倣い動作７２、７４に切り替えられる。最初の粗倣い構成動作１及び粗倣い構成動作２の移動方向は、それまでに基本移動動作を実行していたときの力や位置に基づいて求められる。粗倣い構成動作２の移動方向は基準進行方向と同じ方向の成分を含む方向とし、粗倣い構成動作２の実行中に閾値を超える力が検出されたときは、粗倣い構成動作１に切り替える。このような粗倣い動作実行中に、粗倣い構成動作２において閾値を超える力を検出しなくなったときは、基本移動動作を再開する。粗倣い構成動作１と粗倣い構成動作２の移動方向は、これらからなる粗倣い動作によってツールが基準進行方向に進むように定められることが好ましい。また、ツールとワークとの間に作用する力が、該別の予め定めた閾値を超えているかどうかの確認をしなくとも、該最短距離が予め定めた閾値Ｓ１を超えている間、粗倣い動作を行うようにしてもよい。
【０１０３】
一方、例えば図３１に示すように、ワーク２０に未知の形状物９４が存在していても該形状物が谷のような形状であり、基本移動動作の実行中において力制御のみでもツール１８を倣わせることができる場合を考える。しかしこの場合において、谷形状の形状物９４によって、進行方向（教示軌道によって与えられる方向）についての予め定めた閾値を超えるような力はツールにかからず、かつ、押付方向についての別の予め定めた閾値を超えるような力がツールにかかるが、教示点８２、８４間に設定された教示軌道９２とツール１８の位置との乖離（距離）が予め定めた閾値Ｓ２を超えるときは、ツールの軌道が教示軌道から大きく離れ、基本移動動作を続行したのでは移動時間が長くなる虞があるので、ツール１８の位置に基づいて粗倣い動作に切り替えてもよい。
【０１０４】
図３１の例では、現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、該最短距離が予め定めた閾値Ｓ２を超えたときに基本移動動作７６から粗倣い動作に切り替えられる。この場合の粗倣い動作では、矢印７２で示す粗倣い構成動作１を先ず行い、粗倣い構成動作１によって所定距離又は所定時間だけツールを移動させた後、矢印７４で示す粗倣い構成動作２を行う。粗倣い構成動作２の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、粗倣い構成動作２によって所定距離又は所定時間だけツールを移動させた後、基本移動動作に切り替える。但し、粗倣い構成動作２の実行中に閾値を超える力が検出されたときは、粗倣い構成動作１に切り替える。再開された基本移動動作では、ツールを押付方向にも動かしながら移動させ、現在位置と教示軌道との最短距離が閾値Ｓ２を超えたとき再度粗倣い動作を行う。以上の動作を繰り返し、最終的には、基本移動動作によってツールをワークに倣わせて移動させる。
【０１０５】
図３１の場合において、粗倣い構成動作１においてツール位置が教示軌道から閾値を超えて離れた場合、ツールの移動方向を教示軌道へ向かう方向の成分を含むように変更することで、ツールを教示軌道に向かう方向に修正することができる。また図３１の例でも、粗倣い構成動作２では、ツールを基準進行方向に進めたときに、ワークが存在するかどうかを確認する。図３０では山のような形状がある場合について説明したが、図３１の場合でも、粗倣い構成動作２は他方（ワーク）の存在を確認する動作である。また、最初の粗倣い構成動作１において教示軌道と平行な方向に移動させ続け、接触後に、基本移動動作に切り替えるようにしてもよい。
【０１０６】
なお図９〜図２７を用いて説明した粗倣い動作の各例は、後述する基本移動動作が位置制御である場合にも同様に適用可能である。
【０１０７】
次に、基本移動動作が、指定された位置に基づいてワークに対してツールを移動させる位置制御に基づくものである場合について、図３２を参照しつつ説明する。図３２のように、教示点８２、８４に基づき定められる教示軌道９２に沿ってツール１８が移動させられ、該教示軌道上に障害物９６が存在しているとする。このように基本移動動作の軌道上に障害となる形状が存在する（山形状の突起がある）場合、基本的には、粗倣い動作によって、ツールをワークに対して停止させたり、ワークやツールに過負荷を与えたりすることなく、なるべくワーク形状に沿ってツールを倣わせる。以下、具体的に説明する。
【０１０８】
基準進行方向は、これまでの説明と同様に求めることができる。一方、基準押付方向は、予め定められた方向、各教示点に対して予め定められた方向、又は教示軌道上の進行方向との関係を設定しておくことによって決まる方向に基づき、粗倣い動作に切り替えたときの位置及び進行方向をもとに定める。このようにして定められた以降の基準押付方向は、あるときの位置から教示軌道に対して近づく方向や最短距離となる方向として求めてもよい。
【０１０９】
基本移動動作では、押付方向について力制御は行われないので、ツールが指定された位置を外れて押付方向に動かされることはないが、粗倣い動作では教示軌道から離れる。この場合、基準進行方向と基準押付方向とをもとに、これまでの説明と同様に、障害となる形状を回避する動作の移動方向や、障害となる部分に対してツールを近づけて形状を確認する動作の移動方向を定め、さらに、教示軌道に戻る方向を定める。
【０１１０】
粗倣い動作は、ワーク２０において基準進行方向に障害となる形状（山形状の突起）９６が存在する場合（図３２のＡ部）、或いは基準進行方向に障害物はないが、ある時点でのツール位置がもとの基本移動動作の軌道から外れており、もとの基本移動動作の軌道に復帰させようとする場合（図３２のＢ部）に実行される。但し、それぞれの場合でツールの動きは異なるので、力検出手段で検出される力、及び現在位置と教示軌道との位置関係によって、粗倣い動作を変える。
【０１１１】
先ず図３２のＡ部での動作方法について説明する。基本移動動作の実行中に、進行方向について閾値を超える力が検出されたときに、粗倣い動作に切り替えられ、先ず、第１の粗倣いパターン動作、すなわち粗倣い動作パターンＡ（第１の粗倣いパターン動作を、粗倣い動作パターンＡとも称することとする。）を実行する。粗倣い動作パターンＡでは、粗倣い構成動作２の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、粗倣い構成動作２の実行中に閾値を超える力が検出されたとき、粗倣い構成動作１を実行する。
【０１１２】
上記粗倣い動作パターンＡによる粗倣い動作の実行中に、粗倣い構成動作２において閾値を超える力を検出しなくなったとき、粗倣い構成動作１及び粗倣い構成動作２の動作を変えた粗倣い軌道復帰動作、すなわち粗倣い動作パターンＢ（粗倣い軌道復帰動作を、粗倣い動作パターンＢとも称することとする。）を実行する。粗倣い軌道復帰動作では、粗倣い構成動作１又は粗倣い構成動作２のどちらを先に実行しても構わないが、粗倣い構成動作２を先に実行することが好ましい。粗倣い軌道復帰動作における、粗倣い構成動作２の移動方向は、基準押付方向と同じ方向の成分を含む方向、又は教示軌道に向かう方向とする。粗倣い動作中に、ツールの現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、現在位置と教示軌道との最短距離が予め定めた閾値以内となったときに、基本移動動作を再開する。また粗倣い軌道復帰動作の実行中に、基準進行方向について予め定めた閾値を超える力が検出されたときは、粗倣い動作パターンＡを実行する。
【０１１３】
粗倣い軌道復帰動作における、粗倣い構成動作１及び粗倣い構成動作２の移動方向は、これらからなる粗倣い動作によってツールが基準進行方向に進むように定められる。また粗倣い構成動作１及び粗倣い構成動作２は、それらからなる動きによってツールの進み方が遅くならないように、或いはツールが同じ位置付近に留まらないように、粗倣い動作の移動中のツールの位置を監視しながら移動方向、移動速度及び移動時間の少なくとも１つを調整することが好ましい。また図３２のような場合では、粗倣い軌道復帰動作における、粗倣い構成動作１の移動方向は、基準進行方向と同じ方向の成分と、基準押付方向と逆方向の成分とを含む方向とすることが好ましい。
【０１１４】
なお、基本移動動作が力制御であっても位置制御であっても、その違いによらず、粗倣い構成動作１及び粗倣い構成動作２による粗倣い動作の動作は、これまで示したような直線的な動きを滑らかにつなげてもよいし、図３３に示すようにツールを曲線的に動かしてより滑らかに動くようにしてもよい。
【０１１５】
図３４は、上述の粗倣い動作パターンＡ及びＢを含む粗倣い動作を、図３０を用いて説明した、基本移動動作が力制御によるものである場合に適用した場合を示す図である。矢印７６で示す基本移動動作は、力制御によって押付力を制御しながら、ツール１８とワーク２０との間に作用する力が調整可能な目標値となるように、ツールをワーク形状に倣わせる動作であるとする。教示点８２、８４に基づく教示軌道９２によって、力制御で倣わせるときに参考とする位置が与えられており、ある位置におけるツールの進行方向は教示軌道によって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行ってもよい。
【０１１６】
力制御による基本移動動作の実行中に、未知の形状であっても力制御で適切に倣うことができ、進行方向（教示軌道によって与えられる方向）について予め定めた閾値を超えるような力はかからない状況であるが、教示軌道とは大きく異なる山のような形状物９０が存在する場合、ツール１８の位置に基づいて（例えば、教示軌道９２からの最短距離が閾値Ｓ１を超えるとき）、粗倣い動作に切り替えることが考えられる。このことは、ツールが倣う軌道が教示軌道から大きく離れるような場合、或いはツールをワーク形状にそのまま倣わせたくない場合に有用である。
【０１１７】
図３４の場合において、基本移動動作の実行中に、ツールの現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、該最短距離が予め定めた閾値Ｓ１を超えたとき、基本移動動作から粗倣い動作に切り替えられる。最初の粗倣い構成動作１及び粗倣い構成動作２の移動方向は、それまでに基本移動動作を実行していたときの力や位置に基づいて求められる。
【０１１８】
粗倣い動作に切り替えられたときは、先ず、図３２に関して説明したものと同等の粗倣い動作パターンＡを行う。この粗倣い動作パターンＡによる粗倣い動作の実行中に、粗倣い構成動作２において閾値を超える力を検出しなくなったとき、図３２に関して説明したものと同等の粗倣い軌道復帰動作、すなわち粗倣い動作パターンＢを実行する。
【０１１９】
粗倣い軌道復帰動作における、粗倣い構成動作１及び粗倣い構成動作２の移動方向は、これらからなる粗倣い動作によってツールが基準進行方向に進むように定められる。また粗倣い軌道復帰動作における粗倣い構成動作１の移動方向は、基準進行方向と同じ方向の成分と、基準押付方向と逆方向の成分とを含む方向にすることが好ましい。
【０１２０】
図３５は、図３１の場合における粗倣い動作に、上述の粗倣い軌道復帰動作（粗倣い動作パターンＢ）を適用した例を説明する図である。矢印７６で示す基本移動動作は、力制御によって押付力を制御しながら、ツール１８とワーク２０との間に作用する力が調整可能な目標値となるように、ツールをワーク形状に倣わせる動作であるとする。教示点８２、８４に基づく教示軌道９２によって、力制御で倣わせるときに参考とする位置が与えられており、ある位置におけるツールの進行方向は教示軌道によって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行ってもよい。
【０１２１】
力制御による基本移動動作の実行中に、未知の形状であっても力制御で適切に倣うことができ、進行方向（教示軌道によって与えられる方向）について予め定めた閾値を超えるような力はかからず、かつ、押付方向についての別の予め定めた閾値を超えるような力がツールにかかる状況であるが、教示軌道とは大きく異なる谷のような形状物９４が存在する場合、ツール１８の位置に基づいて（例えば、教示軌道９２からの最短距離が閾値Ｓ２を超えるとき）、粗倣い動作に切り替えることが考えられる。このことは、ツールが倣うべき軌道が教示軌道から大きく離れるような場合、或いはツールをワーク形状にそのまま倣わせたくない場合に有用である。
【０１２２】
図３５の例では、先ず粗倣い構成動作１を行う。粗倣い構成動作１によって所定距離又は所定時間だけツール１８を移動させた後、粗倣い構成動作２を行う。粗倣い構成動作２での移動方向は、基準進行方向と同じ方向の成分を含む方向とする。粗倣い構成動作２によって所定距離又は所定時間だけ移動させた後、上述の粗倣い軌道復帰動作（粗倣い動作パターンＢ）を行う。ツールが教示軌道９２から閾値Ｓ２以内の距離の位置に進んだら、基本移動動作を開始する。但し、図３５のように、基本移動動作の実行中に再びツール現在位置と教示軌道との最短距離が閾値Ｓ２を超えるときは、粗倣い動作を行う。続いて、粗倣い構成動作１の実行後に粗倣い構成動作２を実行し、粗倣い構成動作２の実行中に基準進行方向について閾値を超える力が検出されたら、障害物を含む領域を通過するための粗倣い動作を行う。障害物を越えて、ツールの現在位置と教示軌道との最短距離が閾値Ｓ２以下であれば、基本移動動作を再開する。
【０１２３】
図３６に示す例では、ワーク２０に未知の形状物９０が存在していても該形状物が緩やかな山のような形状であり、基本移動動作の実行中において力制御のみでもツール１８を倣わせることができる場合を考える。基本移動動作で倣う場合、山形状の上り部分では、進行方向（教示軌道によって与えられる方向を意味し、この場合、基準進行方向と同じ方向を意味する）と逆方向の力がかかり、その部分の先の下り部分では、進行方向（教示軌道によって与えられる方向）の力がかかるようになる。このとき、山形状の突起９０の頂点付近に到達し、進行方向と逆方向の力（教示軌道によって与えられる進行方向についての力）が所定閾値より小さくなる場合、ツールの位置に基づいて、例えば、教示軌道からの最短距離が予め定めた閾値Ｓ３を超えているとき、基本移動動作７６ａから粗倣い動作Ｂに切り替え、該最短距離が閾値Ｓ３を下回ったら基本移動動作７６ｂに戻す例が示されている。この例は、ツールが教示軌道から大きく離れ得る形状であって、上述のように、教示軌道から所定距離を超えて離れた位置から教示軌道方向に近づき下っていくような動作のとき、基本移動動作では時間がかかるので移動時間を短縮したい場合等、ツールをワーク形状にそのまま倣わせたくない場合に有用である。
【０１２４】
ここでは、現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、前記最短距離が予め定めた閾値を超えたときに粗倣い動作に切り替える。粗倣い動作Ｂにおける最初の粗倣い構成動作１及び粗倣い構成動作２の移動方向は、それまでに基本移動動作７６ａを実行していたときの力や位置に基づいて求める。図３６の場合、粗倣い構成動作２の移動方向は、基準進行方向と同じ方向の成分を含む方向、又は教示軌道に向かう方向とする。粗倣い構成動作２実行中に、予め定めた閾値を超える力が検出されたとき、粗倣い構成動作１を実行する。この粗倣い動作実行中に、教示軌道からの距離が閾値以内にあり、かつ、基準押付方向について予め定めた閾値を超える力が検出されたとき、基本移動動作７６ｂを再開する。また粗倣い動作の実行中に、基準進行方向について予め定めた閾値を超える力が検出されたときは、上述の障害物を越える方法に切り替えることが好ましい。また粗倣い構成動作１及び粗倣い構成動作２の移動方向は、それらの動作によってツールが基準進行方向に進むように決めることが好ましい。
【０１２５】
図３７は、力制御によりツール１８をワーク２０に倣わせている基本移動動作７６ａの実行中に、ツール１８の進行方向（教示軌道によって与えられる方向を意味し、この場合、基準進行方向と同じ方向を意味する。）について、過負荷を検出して粗倣い動作に切り替えるための予め定めた閾値を超えるような力は検出されない状況だが，別の閾値を超えるような力はかかっており、教示軌道とツール位置との最短距離が予め定めた閾値Ｓ４を超えているとき、上述の粗倣い動作パターンＡによる粗倣い動作に切り替え、矢印７６ｂで示すように基本移動動作を再開し、その後、進行方向（教示軌道によって与えられる方向を意味し、この場合、基準進行方向と同じ方向を意味する。）と逆方向について予め定めた閾値を超える力がかかっており、さらに教示軌道とツール位置との最短距離が該閾値Ｓ４を超えているときは、粗倣い動作パターンＢによる粗倣い動作に切り替えることも考えられる。図３７の例では、ワーク形状が教示軌道から予め定めた閾値を越えて異なることを検出し、進行方向（教示軌道によって与えられる方向）又はその逆方向について力がかかっておらず、基準進行方向がその部分でのワーク形状に沿っていると判断できる、すなわちツールを倣わせやすい状況では基本移動動作を行い、それ以外の部分では粗倣い動作を行うことによって、ワーク形状に応じた倣い動作を実行することができる。
【０１２６】
なお図３７のように、基本移動動作中のツールの押付方向が基準押付方向４６と同じ方向であり、基準進行方向４４と基準押付方向４６が直交しており、ワーク形状が基準進行方向と平行になっていない場合は、ツールの押付方向がワーク形状に対して直交しないため、力制御では押付方向が適切とならない場合がある。しかしこのような状況においては、ツールをワーク形状に沿ってそのまま倣わせず、粗く倣わせることによって、その形状部分にある程度沿わせつつ、好適にツールを通過させることができる。
【０１２７】
図３８は、図３５と同様の谷のような形状９４が存在しているが、ツール１８をワーク２０にある程度以上は倣わせたくない場合に、ツールの位置に基づいて、例えば教示軌道９２からの最短距離が予め定めた閾値Ｓ２を超えたときに、基本移動動作７６から粗倣い動作に切り替えることも考えられる。これは、倣うべき軌道が教示軌道から大きく離れるような場合に有用であり、想定外の軌道に倣ってツールが全く意図しない方向へ移動したり、ワークから離れすぎて停止させたりすることなく、教示軌道から大きくずれるような部位ではツールをワーク形状に沿って倣わせず、粗倣いによって進ませたい場合に有用である。図３８では、基本移動動作７６の実行中に、ツール１８の現在位置と教示軌道との最短距離が閾値Ｓ２を超えたとき、粗倣い動作を行うが、先ず粗倣い構成動作１を行い、粗倣い構成動作１での移動方向は基準進行方向と同じ方向の成分を含む方向とする。
【０１２８】
図３９は図３８の変形例を示す。図３５と同様の谷のような形状９４が存在しており、基本移動動作７６ａの実行中に、現在位置と教示軌道との最短距離が予め定めた閾値Ｓ２を超えたときに粗倣い動作を行うが、このとき、矢印７２ａで示す粗倣い構成動作１を先ず行い、粗倣い構成動作１での移動方向は、基準進行方向と同方向の成分を含む方向とする。次に所定距離又は所定時間だけツール１８を移動させた後、矢印７４で示す粗倣い構成動作２を実行する。粗倣い構成動作２において所定距離又は所定時間だけツール１８を移動させた後、矢印７２ｂで示す粗倣い構成動作１を実行する。この粗倣い構成動作１の実行中に、基準進行方向について予め定めた閾値を超える力は検出されないが、基準押付方向について予め定めた閾値を超える力が検出されたとき、基本移動動作７６ｂを再開する。
【０１２９】
図４０は、図３８の変形例を示す。図３５と同様の谷のような形状９４が存在しており、基本移動動作７６ａの実行中に、現在位置と教示軌道との最短距離が予め定めた閾値Ｓ２を超えたときに粗倣い動作を行うが、このとき、矢印７２で示す粗倣い構成動作１を先ず行い、粗倣い構成動作１での移動方向は、基準進行方向と同じ方向の成分を含む方向とする。この粗倣い構成動作１の実行中に、基準進行方向について予め定めた閾値を超える力が検出されたとき、上述の障害物を越える粗倣い動作に切り替える。障害物を越える粗倣い動作においては、矢印７４で示す粗倣い構成動作２によって所定距離又は所定時間だけツールを移動させ、粗倣い構成動作１と粗倣い構成動作２とを繰り返してツールが障害物を乗り越えたら、矢印７６ｂで示す基本移動動作を再開する。
【０１３０】
なお基本移動動作が力制御によるものである場合に、教示点に基づく教示軌道によってツールの進行方向が定められ、該進行方向に合わせてツールの押付方向を定める倣い動作を行っているとすると、例えば図４１に示すように、教示軌道上に、ツール（図示せず）をワーク２０に対して基準押付方向４６に押し付けるようなワーク形状がない（そのときの位置から基準進行方向に谷がある）場合、進行方向が基準進行方向と同じ方向、押付方向が基準押付方向と同じ方向であり、それらをもとに基本移動動作を継続すると、図４１に示すワークの窪み形状の部分に近づくことはなく、進行方向の速度を小さくしツールを押付方向に移動させたとしても、ツールがワークの該窪み形状から離れてしまうことになる。
【０１３１】
そこで、所定時間又は所定比較回数だけ、力の進行方向成分が予め定めた閾値より小さく、かつ、押付方向成分が予め定めた閾値より小さい場合は、ツールは押付方向に押し付けることができない部分に差し掛かり、ワークに倣うことができない状態になっていると判定する。この判定において、実際にそのような部分に差し掛かっているのか、或いは単に倣いの性能が悪いため倣えていないのかを、なるべく正確に判断できるように、押付方向についての反力が小さい場合に進行速度を下げて倣いの性能を相対的に向上させることもできる。
【０１３２】
そして、粗倣い構成動作１と、粗倣い構成動作２と、それらの動作切り替え判定とを繰り返し（切り替えをしない場合もあり、また順序は問わない）、つまりツールをワークから離す動作と、ワークに近付ける動作と、それらの動作中に倣うべきワーク形状が存在しているかを確認する動作とを繰り返して、基準押付方向及び基準進行方向をもとに、ツールをワーク形状になるべく沿うように動かす。
【０１３３】
ここで、粗倣い動作実行中に、状況に応じて、基準押付方向について力がかかっていなくともその後の基本移動動作の押付方向が基準押付方向とは異なる場合には任意の方向について、所定の閾値を超える力がかあれば、或いは、粗倣い動作を継続する場合には基準押付方向について、所定の閾値を超える力がかかれば、基本移動動作に切り替えることによって、図４１のように押付方向に形状物が急に存在しなくなった場合（そのときの位置から基準進行方向に谷がある）でも、迅速かつ、なるべくワーク形状に沿った倣い動作を行うことができる。さらに、それらの動作をより細かく（短時間又は短距離で）繰り返すことによって、ツールをワーク形状により正確に沿わせることができる。
【０１３４】
これまで述べてきた本発明の応用によって、複雑なワーク形状に対しても簡単な教示によってツールを倣わせることもできる。例えば、図４２に示すように、円周上に凹凸部９８が設けられているワーク形状に対して円を動くような簡単な教示軌道１００によってツールを倣わせたい場合を考える。教示軌道によって進行方向及び押付方向が与えられる通常の倣い動作では倣わせることは困難であるが、本発明では、凹凸部９８の各々において、そのときの形状・位置に応じたツールの進行方向及び押付方向をそれぞれ基準進行方向及び基準押付方向として、それをもとに回避動作をおこなうため、教示軌道１００とは異なる未知の形状・障害物に対しても、上述の基本移動動作と粗倣い動作とを切り替えながら、なるべくワーク形状に沿いつつ障害物等を回避しながら、迅速にツールを動かすことができる。
【０１３５】
なお、図４２に示されるような、教示軌道とは異なる実際のワークの複雑な凹凸形状や教示軌道とのサイズの違いなどによる、教示軌道と実際のワーク形状とのずれは、教示軌道上の点を実軌道上の点に射影させて、目標とする移動方向や位置を合わせたり補正したりすることによって所望の方向や位置を取得し、対応することができる。よって、力制御を行う基本移動動作や粗倣い動作実行中に、教示軌道を参考としながら、教示軌道とは異なる複雑な形状に沿ってツールを回避させながら、教示軌道が意図する位置へ移動させることが可能である。さらに、上述のように、ワーク形状になるべく沿った移動をさせたり、他の部分との干渉を避けたりするためには、粗倣い構成動作実行時に実際に移動する距離を短くしておくことが望ましい。
【０１３６】
また、ツールとワークとの間に作用する力や力制御による倣い動作中の移動位置をもとにツールの進行方向と押付方向を求め、ワークに対して連続的に倣わせる方法を行う場合でも、ワークに不連続な部分があったり非常に複雑な形状部分があったりする等、ツールをワークに連続的に倣わせ続けるよりも、本発明によって、ワーク形状を確認する動作とワークから局所的にツールを離す動作とを繰り返して、ワーク形状に粗く倣わせる方が望ましい場合もある。このように，ワーク形状が複雑な場合や動作時間をなるべく短縮したい場合等において、ワーク形状にツールを粗く倣わせればよいというときにも本発明は有効である。
【０１３７】
なお上述の実施形態では、ロボットアームの先端に取り付けたツールをワークに対して移動させて倣わせる場合について説明したが、本発明の実施に際し、ロボットアーム先端部分に取り付けたハンドにより把持されたワークを、固定された加工ツール等に対して移動させるようにしてもよい。また、ロボットアームの先端に取り付けたツール又は把持させたワークを作用させる、他方のワーク又はツールを固定させた部分は、移動動作が可能な装置によって移動させてもよい。
【０１３８】
以上述べたように、本願発明では、ロボットを用いてツールとワークとを相対的に移動させているときに、障害物、壁、穴、大きな凹凸等の未知な形状があった場合でも、「それまでの移動動作」と、「積極的に一方を他方から離す動作と、他方の存在を確認する（通常は他方に近付ける）動作とを繰り返す、粗倣い動作」とを、力検出手段で検出する力の情報、及びロボットが動かしている物体の位置情報の少なくとも一方に基づいて適宜切り替え、ロボット、作業ツール及びワークに対して過負荷を与えず、好適な回避動作を行いつつなるべく形状に沿った迅速な倣い制御が可能となる。本発明は、加工を伴わない教示作業のための倣い制御、バリ取り作業、研磨・研削作業、ロールヘム加工等の押付けによる倣い加工作業、さらにはツールをワークに対して移動させて異物があるか、ワークが不良品であるか等を確認する作業にも有用である。
【符号の説明】
【０１３９】
１０ ロボットシステム
１２ ロボット
１４ ロボット制御装置
１６ 力センサ
１８ 作業ツール
２０ ワーク
２２ 作業台
２４ 各軸位置検出部
２６ 位置、速度、加速度検出・算出部
２８ 作用力算出部
３０ 第１のパラメータ格納部
３２ 第２のパラメータ格納部
３４ 動作切り替え判定・動作パラメータ調整部
３６ 指令演算部
３８ 基準進行方向、基準押付方向算出・更新部
【技術分野】
【０００１】
本発明は、広くは倣い制御を行うロボット制御装置に関し、特には、ロボットによるバリ取り、研削、研磨、ロールヘム加工等の倣い作業の自動化、或いはロボットの移動動作中における障害物への対応の自動化を実現するためのロボット制御装置に関する。
【背景技術】
【０００２】
ロボットによりバリ取り、研磨、研削等の加工作業を行うため、或いはそのための教示データの生成のために、ロボットアーム先端部分に取り付けた作業ツールを適切な力でワークに押付けながら作業ツールを移動させる倣い動作が行われている。同様の目的のために、ロボットアーム先端部分に取り付けたハンドにより把持されたワークを適切な力で、固定された加工機械に押付けながらハンドを移動させる場合もある。このとき、ワークと作業ツールとの間に働く作用力が適切な値になるように、該作用力を、力覚センサを用いて検出し、又は、機械駆動部のアクチュエータの電流値を用いることにより該作用力を推定し、検出又は推定された作用力をフィードバックして作業ツールの位置が制御される。
【０００３】
このような倣い動作の方法として、従来、以下に述べるような様々な方法が実施され又は提案されている。
（１）倣い加工の方法としての、加工工具を教示された軌道に沿って動かし、該軌道上に存在するバリなどを削る方法
（２）倣い加工・教示の方法としての、特許文献１のように、ワークからの反力に基づいて、ロボットとワークとの接点の法線方向を求め、押付方向及び進行方向を算出し、ワーク表面に対して一定力を加えながら倣う方法
（３）特許文献２−４に記載のような、予めワークから離れた位置、押付方向の粗教示をしておき、それに基づいて、位置制御をしながら押付方向について力制御を行い、ワーク表面に倣う方法
【０００４】
上述のような倣い方法において、大きくかつ不連続な凸形状部や凹形状部が存在すると、ツールを適切に倣わして動かすことができないという問題が発生する。
【０００５】
これに対して特許文献５では、過負荷が発生した場合に、過負荷発生位置を基準として、過負荷発生要因部に対して進行方向側の背後に研削工具を移動させ、進行方向と逆側に移動させながら研削を行い、過負荷発生要因部を取り除き、その後、もとの研削作業を再開するという方法が提案されている。また特許文献６では、予め教示された軌道に沿ってグラインダを動かしているときに、過大バリに接触して負荷電流が一定の設定値を越えた場合、段階的に離れた複数の研削軌道を生成して、該研削軌道のうち教示軌道より離れたものから近いものに向かい順にグラインダを動かしながら段階的にバリのある方向に近づけて繰り返し研削するという方法が提案されている。
【０００６】
また特許文献７では、加工済みワークの加工面に対して粗教示点を設け、該粗教示点と１つ前の粗教示点とを結ぶ方向に加工工具を進めるとき、力の検出値が予め設定した値以上なら、次の粗教示点の方向に動かすことによって凹形状の角部に対応するという方法が提案されている。また特許文献８では、ツールの移動方向の力が設定値よりも大きいかどうかを判定し、予め設定した動作に切り替えることによって、ツールの押付方向と移動方向を変え、凹コーナー部の次の面に連続的に倣わせる方法が提案されている。さらに特許文献９では、エンドエフェクタにかかる反力が所定範囲にあるかどうかを判断し、凹コーナー部に対しては反力が所定範囲より大きいか、凸コーナー部に対しては反力が所定範囲より小さいかを見ることによって、反力が所定範囲にないとき、予め次に指定した動作命令を実行することによってコーナー部に倣わせるという方法が提案されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開平３−１８４７８６号公報
【特許文献２】特開平６−１０２９２４号公報
【特許文献３】特開平７−２６６２６９号公報
【特許文献４】特開平５−８８７４２号公報
【特許文献５】特開平４−３５８６０号公報
【特許文献６】実開平５−９２８０８号公報
【特許文献７】特開平６−２６２５６２号公報
【特許文献８】特開平３−１１１１５１号公報
【特許文献９】特開平８−１１８２７６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
上述のように特許文献１−４に記載の発明では、対象物において、教示軌道とは大きく異なる形状部が存在したり、サイズが大きくかつ不連続な形状部が存在したりすると、ツールを適切に倣わして動かすことができないため、ロボットやツールやワークに過負荷をかけたり、システムの稼動を止めたりするという問題が発生する。このとき特許文献２−４のように粗教示点をもとに進行方向を決め、押付方向に対して力制御を行う場合では、与えられる進行方向と大きく異なる凸形状部に衝突した場合や、凹形状部に嵌った場合、適切に回避するための移動指令を得ることができないため、該形状部を回避することができない。
【０００９】
特許文献５及び６の方法は、決められた軌道上を動きながら過負荷発生要因を削除することを目的としており、背後に回って削る動作や段階的に離れた複数の軌道を生成する動作において、予めワークや工具毎に過負荷要因部の大きさを予測し、予め冗長な軌道を用意しなければならなかったり、工具等が必要以上に大きく動いたりするため、未知形状のワークに対して好適な倣い動作を行うことが難しい。
【００１０】
また特許文献７−９の方法では、条件判定後の次の動作に切り替えるときに、次の部分に沿う動作を予め教示しておかねばならず、手間であり、またその動作が実際の形状と異なる場合は工具等を倣わすことができず、故に未知な形状に対しては倣わせることができない。また、ワークからの反力やワークの形状をもとに進行方向と押付方向を求め、ワークを連続的に倣わせる場合であっても、連続的に倣わせ続けるよりも、場所によっては形状の確認動作や対象物に対する離隔を繰り返して、ワーク形状に近い位置を素早く動かすことが望ましい場合もある。
【００１１】
そこで本発明は、形状が未知の部分や障害物等がワークに存在する場合でも、ロボット、作業ツール及びワークに対して過大な負荷をかけることなく、なるべくワークの形状に沿った迅速な倣い制御を可能とするロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【００１２】
上記目的を達成するために、本発明は、作業ツールとワークの一方に対して他方をロボットの手先部によって相対的に動かし、両者間に作用する力を制御するロボット制御装置において、前記両者間に作用する力を検出する力検出手段と、所定の基準進行方向と基準押付方向とに基づいて実行され、所定の方向に、所定移動時間又は所定移動距離を限度に移動する第１の粗倣い構成動作及び前記所定方向とは異なる方向に、所定移動時間又は所定移動距離を限度に移動する第２の粗倣い構成動作からなる粗倣い動作を実行する粗倣い動作実行手段と、前記ロボットの手先部を移動させる基本移動動作の実行中に検出された、前記両者間に作用する力における、所定方向の成分の評価結果、又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に基づいて基本移動動作から粗倣い動作に切り替える第１の動作切り替え手段と、粗倣い動作実行中の動作切り替え判定において、粗倣い動作中に検出された、前記両者間に作用する力の評価結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に基づいて、粗倣い動作から基本移動動作への切り替え、第１の粗倣い構成動作から第２の粗倣い構成動作への切り替え、第１の粗倣い構成動作から別の第１の粗倣い構成動作への切り替え、第２の粗倣い構成動作から第１の粗倣い構成動作への切り替え、及び、第２の粗倣い構成動作から別の第２の粗倣い構成動作への切り替えのいずれかを実行する、第２の動作切り替え手段と、を備えたことを特徴とするロボット制御装置を提供する。
【００１３】
また本発明のロボット制御装置は、前記基本移動動作は、前記両者間に作用する力が目標値と一致するように、前記一方に前記他方を接触し続けながら移動させる倣い動作であり、前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第１の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第２の粗倣い構成動作の移動方向は、基準進行方向と同じ方向の成分を含む方向であり、第２の粗倣い構成動作の実行中に、前記両者間に作用する力と予め定めた第２の閾値との比較結果に応じて第１の粗倣い構成動作又は基本移動動作に切り替え、第２の粗倣い構成動作の最後まで、第１の粗倣い構成動作への前記切り替えを行わないとき、粗倣い動作から基本移動動作に切り替えるものであることを特徴とする。
【００１４】
また本発明のロボット制御装置は、前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第１の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、粗倣い動作において、第２の粗倣い構成動作の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、前記第２の動作切り替え手段が、第２の粗倣い構成動作の実行中に、前記両者間に作用する力と予め定めた第２の閾値との比較結果に応じて第１の粗倣い構成動作に切り替える動作を、第１の粗倣いパターン動作とし、粗倣い動作において、第２の粗倣い構成動作の移動方向は、基準押付方向と同じ方向の成分を含む方向とし、又は、基本移動動作が教示軌道に基づいて移動する場合、前記教示軌道に近付く方向とし、前記第２の動作切り替え手段が、前記ツールまたはワークの位置と、前記指定された軌道の距離の評価結果に基づいて、粗倣い動作から基本移動動作に切り替える動作を粗倣い軌道復帰動作とし、前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第１の粗倣いパターン動作の実行中に、第２の粗倣い構成動作の最後まで、第１の粗倣い構成動作への動作切り替えを行わないとき、粗倣い軌道復帰動作に切り替え、粗倣い軌道復帰動作の実行中に、基準進行方向成分の力の大きさが予め定めた第３の閾値以上である場合、第１の粗倣いパターン動作に切り替えることを特徴とする。
【００１５】
また本発明のロボット制御装置は、前記基本移動動作は、該両者間に作用する力が目標値と一致するように、前記一方に前記他方を接触し続けながら移動させる倣い動作であり、前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第４の閾値との所定比較時間又は所定比較回数の比較結果、及び、前記両者間に作用する力における、前記ロボットの手先部の押付方向の成分と、予め定めた第５の閾値との所定比較時間又は所定比較回数の比較結果又は前記ロボットが動かしている前記作業ツール又はワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第２の粗倣い構成動作の移動方向は、基準進行方向と逆方向の成分を含む方向であり、粗倣い動作実行中に、前記両者間に作用する力における、所定方向の成分と、予め定めた第６の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、粗倣い動作から基本移動動作に切り替えるものであることを特徴とする。
【００１６】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、前記動作切り替え判定において、第１の粗倣い構成動作の実行時に、前記両者間に作用する力が、予め定めた第７の閾値を超えたとき、第１の粗倣い構成動作の移動する向きを、検出される力の大きさ及び方向に基づいて変更し、別の第１の粗倣い構成動作を行い、別の第１の粗倣い構成動作の実行時に、該別の第１の粗倣い構成動作が完了するまで、所定の条件を満たさないとき、第２の粗倣い構成動作に切り替えることを特徴とする。
【００１７】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、第１の粗倣い構成動作の実行後に、別の第１の粗倣い構成動作を行う場合において、別の第１の粗倣い構成動作で所定移動時間又は所定移動距離、移動したときの位置が、基準押付方向と逆方向の位置について、一連の第１の粗倣い構成動作における開始時からそれまでで、前記基準押付方向と逆方向に最も進んだ位置から所定距離以下なら、移動方向を基準押付方向の逆方向の成分を含む方向にし、又は基準進行方向の逆方向の成分を含む方向にし、別の第１の粗倣い構成動作を行うように動作を切り替えることを特徴とする。
【００１８】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、前記動作切り替え判定において、第１の粗倣い構成動作の実行時に、前記両者間に作用する力における、基準押付方向の成分が、予め定めた第８の閾値以上である場合、粗倣い動作から基本移動動作に切り替え、前記両者間に作用する力における、基準押付方向の成分が、前記第８の閾値以上でなく、かつ、前記両者間に作用する力における、基準進行方向の成分が、予め定めた第９の閾値以上である場合、第２の粗倣い構成動作、又は別の第１の粗倣い構成動作、又は基本移動動作に切り替え、前記両者間に作用する力における、基準押付方向の成分が前記第８の閾値以上でなく、かつ、前記両者間に作用する力における、基準進行方向の成分が、前記第９の閾値以上でなく、かつ、前記作業ツールと前記ワークとの間に作用する力が予め定めた第１０の閾値以上である場合、第１の粗倣い構成動作の移動する向きを、検出される力の大きさ及び方向に基づいて変更し、別の第１の粗倣い構成動作を行うように動作を切り替え、別の第１の粗倣い構成動作の実行時に、該別の第１の粗倣い構成動作が完了するまで、所定の条件を満たさなければ、第２の粗倣い構成動作に切り替えることを特徴とする。
【００１９】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、前記動作切り替え判定において、第２の粗倣い構成動作の実行時に、前記両者間に作用する力における、基準押付方向の成分が、予め定めた第１１の閾値以上である場合、粗倣い動作から基本移動動作に切り替え、前記両者間に作用する力における、基準押付方向の成分が、前記第１１の閾値未満、かつ、前記両者間に作用する力が、予め定めた第１２の閾値以上であるとき、第１の粗倣い構成動作に切り替え、第２の粗倣い構成動作の実行時に、該第２の粗倣い構成動作が完了するまで、所定の条件を満たさないとき、第１の粗倣い構成動作に切り替えることを特徴とする。
【００２０】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、第２の粗倣い構成動作の実行時に、前記所定移動時間又は前記所定移動距離だけ移動しても、前記両者に作用する力が、予め定めた第１３の閾値未満である場合、第２の粗倣い構成動作の所定移動時間、所定移動速度及び所定距離のうち少なくとも１つを大きくするか、又は、前記移動方向を基準進行方向の逆方向に所定量だけずらす、動作調整手段を含むことを特徴とする。
【００２１】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、粗倣い動作実行時に、第１の粗倣い構成動作若しくは第２の粗倣い構成動作を所定回数以上繰り返し実行しても、又は、所定時間が経過するまで繰り返し実行しても、第１の粗倣い構成動作の開始位置、又は第２の粗倣い構成動作の開始位置の少なくとも２つ以上のそれぞれの開始位置に基づく開始位置の変化量が所定値以下のとき、第１の粗倣い構成動作又は第２の粗倣い構成動作を行うときの、移動方向を変えるか、又は、第１の粗倣い構成動作の所定移動時間、所定移動速度及び所定移動距離、並びに第２の粗倣い構成動作の所定移動時間、所定移動速度及び所定移動距離のうち少なくとも１つを大きくする動作調整手段を含むことを特徴とする。
【００２２】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、前記粗倣い動作実行時に、第１の粗倣い構成動作若しくは第２の粗倣い構成動作を所定回数以上繰り返した場合若しくは所定時間経過した場合、粗倣い動作への切り替え時の開始位置からの距離が予め定めた第１４の閾値以上の場合、又は、基本移動動作が教示軌道に基づく場合かつ教示軌道からの最短距離が前記第１４の閾値以上の場合、前記ロボットの手先部の動作を止めることを特徴とする。
【００２３】
また本発明のロボット制御装置は、前記第２の動作切り替え手段は、粗倣い動作実行時の、第１の粗倣い構成動作又は第２の粗倣い構成動作の実行時に、前記両者間に作用する力が、予め定めた第１５の閾値以上である場合、第１の粗倣い構成動作又は第２の粗倣い構成動作における所定移動速度を、そのときの所定移動速度より小さくする動作調整手段を含むことを特徴とする。
【００２４】
また本発明のロボット制御装置は、粗倣い実行中又は基本移動動作の実行中に取得した位置データをもとに、所定の速度指令で動かす教示データを生成する教示データ生成手段を備えることを特徴とする。
【発明の効果】
【００２５】
本発明によれば、ロボットを用いてツールとワークとを相対的に移動させているときに、障害物、壁、穴、大きな凹凸等の未知な形状があった場合でも、基本移動動作と粗倣い動作とを、力検出手段で検出する力の情報、及びロボットが動かしている物体の位置情報の少なくとも一方に基づいて適宜切り替えることにより、ロボット、作業ツール及びワークに対して過負荷を与えず、好適な回避動作を行いつつなるべく形状に沿った迅速な動作を行うことが可能となる。
【図面の簡単な説明】
【００２６】
【図１】本発明に係るロボット制御装置を含むロボットシステムの概略構成を示す図である。
【図２】図１のロボット制御装置の機能ブロック図である。
【図３】ワークに予期せぬ凹凸形状がある場合に基準進行方向及び基準押付方向を規定する例を示す概略図である。
【図４】ワークに予期せぬ凸形状がある場合に基準進行方向及び基準押付方向を規定する他の例を示す概略図である。
【図５】力制御による倣い動作を説明する図である。
【図６】進行方向について閾値を超える力がかかった場合の処理について説明する図である。
【図７】本発明によって好適に倣うことができるワーク形状の例を説明する図である。
【図８】粗倣い構成動作１及び粗倣い構成動作２の移動方向の具体的求め方を説明する図である。
【図９】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１０】粗倣い構成動作１を含む粗倣い動作の具体例を示す図である。
【図１１】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１２】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１３】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１４】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１５】粗倣い構成動作１を含む粗倣い動作の具体例を示す図である。
【図１６】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１７】粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１８】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図１９】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２０】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２１】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２２】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２３】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２４】粗倣い構成動作１及び粗倣い構成動作２を含む粗倣い動作の具体例を示す図である。
【図２５】粗倣い構成動作２の移動距離又は移動方向を徐々に変化させた例を示す図である。
【図２６】粗倣い構成動作１及び粗倣い構成動作２の一方又は双方の移動方向を変えた例を示す図である。
【図２７】教示経路に適当な補間点を挿入した例を示す図である。
【図２８】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図２９】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３０】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３１】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３２】基本移動動作が位置制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３３】図３２の場合において、粗倣い動作を滑らかに行った例を説明する図である。
【図３４】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３５】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３６】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３７】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３８】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図３９】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図４０】基本移動動作が力制御によるものである場合の、該基本移動動作と粗倣い動作との切り替え例を説明する図である。
【図４１】教示軌道上に、ツールを基準押付方向に押し付けるようなワーク形状がない場合の、ツールの好適な移動例を説明する図である。
【図４２】本発明の応用例として、ワーク形状が円周上に凹凸部が設けられたものである例を示す図である。
【発明を実施するための形態】
【００２７】
以下、添付図面を参照しつつ、本願発明の好適な実施形態について説明する。なお特に断らない限り、「力」は、ツールとワークとの間に作用する力によってもたらされ、力検出手段によって得られたデータに基づく物理量とする。また「所定」又は「予め定めた」なる語は、関連する処理が実行されるまでに定められればよいという意味で用いられ、いつ定めるかについての特段の制約はないものとする。また、以降の説明や図においては、第１の粗倣い構成動作を粗倣い構成動作１、第２の粗倣い構成動作を粗倣い構成動作２とも記述する。
【００２８】
図１は、本発明に係るロボット制御装置を含むロボットシステムの構成例を示す図である。ロボットシステム１０は、ロボット１２と、ロボット１２の動作、具体的には該ロボットの各軸の位置を制御するロボット制御装置１４とを有する。なお図示例のロボットは６自由度の多関節型ロボットであるが、本願発明を実施するに際し６自由度の多関節型ロボットである必要はない。ロボット１２のアーム先端には力検出手段としての力センサ１６と、ワーク加工等の用途に供される作業ツール１８とが取り付けられ、力センサ１６はツール１８に作用する力を検出する。またツール１８が加工すべきワーク２０は、例えば作業台２２等の所定の位置に配置される。配置された該ワーク２０は、移動動作が可能な装置などによって移動させてもよい。
【００２９】
図２は、ロボット制御装置１４の処理機能と各機器等の関係を示すブロック図である。ロボット１２の各軸の位置を検出するエンコーダ等の各軸位置検出部２４からの検出データが、位置、速度、加速度検出・算出部２６に送られる。位置、速度、加速度検出・算出部２６は、各軸位置検出部２４からの所定時間間隔毎の各軸位置データ等に基づき、ロボット１２の位置、速度及び加速度を検出・算出する。或いは加速度は、図示しない加速度センサを用いて求めてもよい。
【００３０】
位置、速度、加速度検出・算出部２６の処理結果は、作用力算出部２８に送られる。作用力算出部２８は、力検出手段１６で検出した力又はモーメント、位置、速度、加速度検出・算出部２６からの位置、速度及び加速度に関するデータ、ロボットの各構成要素の質量・重心位置等のデータ、並びに作業ツール１８の進行方向、押付方向に関するデータに基づいて、ツール１８とワーク２０との間に作用する作用力、さらには、ツール１８とワーク２０との間に作用する作用力のうち、所定方向の成分を算出することができる。
【００３１】
作用力算出部２８での算出に使用されるパラメータは、第１のパラメータ格納部３０及び第２のパラメータ格納部３２に格納することができる。図２の例では、第１のパラメータ格納部３０には、後述する基本移動動作、粗倣い動作を行うための動作パラメータが格納される。一方第２のパラメータ格納部３２には、力センサがロボット手首に設けられ、その先にツール又はワークが取り付けられている場合に、ツールとワークとの間に作用する正味の力以外で、該力センサに作用する力を算出するために必要なパラメータが格納されており、例えばロボットの各構成要素の質量や重心位置のデータが格納されている。また特に示していないが、後述する算出方法において必要となるパラメータも、ロボット制御装置内部に格納することができる。
【００３２】
動作切り替え判定・動作パラメータ調整部３４は、ロボットの位置・速度・加速度と、ツールとワークとの間に作用する作用力、又は該作用力のうちの所定方向の成分と、基本移動動作、粗倣い構成動作１、粗倣い構成動作２のうち、現在行われている動作と、基本移動動作のパラメータ及び粗倣い動作のパラメータに基づいて、動作切り替えの判定を行うとともに、場合によっては第１のパラメータ格納部３０内のパラメータを変更し、基準進行方向及び基準押付方向等をもとに、指令演算部３６に対して、行うべき動作を指定する動作指令を作成する機能を備える。各動作の詳細については後述する。
【００３３】
指令演算部３６は、ロボットの位置・速度・加速度と、ツールとワークとの間に作用する作用力又は該作用力のうちの所定方向の成分と、動作切り替え判定・動作パラメータ調整部３４による動作指令に基づいて、ロボット１２に送る指令を演算する。なお本実施形態では、粗倣い動作実行手段、動作調整手段及び教示データ生成手段の各機能は、指令演算部３６が担うものとする。
【００３４】
なお基準進行方向、基準押付方向算出・更新部３８は、第１のパラメータ格納部３０に格納されているデータと、位置、速度、加速度検出・算出部２６の検出・算出結果とに基づいて、必要に応じて、ワークに対するツールの基準進行方向及び基準押付方向（後述）を算出し又は更新する。算出され又は更新された基準進行方向及び基準押付方向は、作用力算出部２８での処理、及び、粗倣い構成動作１又は粗倣い構成動作２の移動動作のパラメータの算出や調整等に使用される。
【００３５】
本実施形態における力検出手段１６は、Ｘ、Ｙ、Ｚ軸方向の並進力及び各軸回りのモーメントを検出できる力センサとする。該力センサは、ロボット１２の手首と作業ツール１８との間に配置される。好適な力センサは６軸の力センサであるが、目的とする方向に作用する力を検出できる力センサであれば、検出できる力及びモーメントの自由度や検出原理等は問われず、どのようなタイプの力センサであってもよい。また、力センサはロボット１２の手首部及びロボット１２が載置されている床面のいずれに設置されてもよく、要するにツール１８とワーク２０との間に作用する力を算出できるのであれば、その設置場所に制約はない。さらに、力センサを用いずに、ロボット１２を駆動するアクチュエータの電流値に基づいて力を推定する手段を力検出手段１６として用いることも可能である。
【００３６】
次に、ロボット制御装置１４からロボット１２に対して指令される各動作について説明する。先ず基本移動動作は、ロボット１２を用いて、ツール１８とワーク２０とを相対的に移動させる動作であり、具体的には以下の動作が挙げられる。
（１）予め教示された位置等に基づいて指定された軌道に沿って動作
（２）ツールとワークとの間に作用する力が目標値と一致するように力制御を行いながら、ワークの形状に沿ってツールを倣わせる動作
ここで（２）における力制御としては、以下のような具体的方法が考えられる。
（２ａ）教示軌道によって与えられる各位置における目標進行方向が、該教示軌道における進行ベクトルによって与えられており、該位置を参考として力制御を行う
（２ｂ）教示軌道を中心にばね成分を持たせた押付制御を行いながら、該教示軌道に基づいて力制御を行う
（２ｃ）ツールとワークとの間に作用する力、及び移動中のツール又はワークの位置に基づいて、ワークの形状を推定しながら力制御を行う
上記以外の方法であっても、ロボットを用いてツールとワークとを相対的に移動させる動作であれば、本発明の趣旨から外れないことは当業者には明らかである。
【００３７】
次に、粗倣い動作について説明する。粗倣い動作は、上述の粗倣い構成動作１及び粗倣い構成動作２から構成される。粗倣い構成動作１は、ツール及びワークの一方を他方から、各時点の位置よりも（通常は局所的に）遠ざける動作であって、予め定めた移動時間又は予め定めた移動距離を限度にツール又はワークを移動させる動作である。また粗倣い構成動作２は、ロボットが移動させている一方に対して他方の存在を確認しようとする動作であり、多くの場合、ツール及びワークの一方を他方から、各時点の位置よりも（通常は局所的に）近付ける動作であって、予め定めた移動時間又は予め定めた移動距離を限度にツール又はワークを移動させる動作である。
【００３８】
詳細は後述するが、粗倣い動作実行中は、力検出手段で検出する力の情報、或いは又はこれに加えて、ロボットが動かしている物体の粗倣い動作実行中の位置情報、基準進行方向及び基準押付方向に基づいて、粗倣い構成動作１又は粗倣い構成動作２の移動方向、移動速度、移動時間及び移動距離の少なくとも１つを、必要に応じて変化させながら、ワーク上の異物や障害物等に対してツールを適切に移動させることができる。
【００３９】
上述の基準進行方向及び基準押付方向とは、粗倣い動作実行中のある位置において、粗倣い構成動作１又は粗倣い構成動作２を行うときに、その移動方向を定めるために基準とする方向である。すなわち基準進行方向とは、予め又は動作実行中に定められる方向であって、粗倣い動作実行中に基準とする進行方向であり、より具体的には、粗倣い動作実行中にツールを進ませるために参考にする（通常は移動方向に概ね一致）方向である。一方基準押付方向とは、予め又は動作実行中に定められる方向であって、粗倣い動作実行中に基準とする押付方向であり、より具体的には、粗倣い動作実行中にワークに対して押付動作をするために参考にする（通常は押付方向に概ね一致）方向であり、上記基準進行方向に基づいてある領域を通過する際の押付方向である。但し、上記基本移動動作が位置制御に基づくものである場合は、該基準押付方向は教示軌道に近付く方向とする。
【００４０】
以下、図３及び図４を参照して、上記基準進行方向及び基準押付方向の定め方の具体例を説明する。
図３に示すように、ワーク２０の表面に沿ってツール１８を右方から左方へ移動させる場合に、ワーク２０に予期せぬ凹凸形状、すなわち凸状部分４０及び凹状部分４２が存在するとする。この場合、凸状部分４０はツールの通過動作の障害となるが、基準進行方向は、矢印４４で示すように凸状部分４０が存在しない（ワーク形状は破線のように平坦である）とした場合の基準進行方向（予め教示軌道等によって与えられる移動方向）を適用し、一方基準押付方向は、矢印４６で示すように、基準進行方向４４に垂直かつワークに向かう方向とする。
【００４１】
一方、凹状部分４２についても、基準進行方向は、矢印４８で示すように凹状部分４２が存在しない（ワーク形状は破線のように平坦である）とした場合の基準進行方向（予め教示軌道等によって与えられる移動方向）を適用し、一方基準押付方向は、矢印５０で示すように、基準進行方向４８に垂直かつワークに向かう方向とする。
【００４２】
他の例として、図４のようにワーク２０が凹面状部分５２を有し、凹面状部分５２に予期せぬ凸状部分５４が存在する場合を考える。この場合も、凸状部分５４はツールの通過動作の障害となるが、基準進行方向は、矢印５６で示すように凸状部分５４が存在しない（ワーク形状は破線のような滑らかな凹面形状である）とした場合の基準進行方向（予め教示軌道等によって与えられる移動方向）を適用し、一方基準押付方向は、矢印５８で示すように、基準進行方向５６に垂直かつワークに向かう方向とする。
【００４３】
図４のように教示軌道５６が曲がっていて，教示軌道上の位置に対応する進行方向及び押付方向が算出できる場合は、粗倣い動作実行中の位置から教示軌道に対して直交するように、或いは最短距離となる方向に線を引いたときの交点に対応する、進行方向や押付方向を用いてもよい。或いは押付方向として、対応する教示された進行方向をもとに算出した方向を用いてもよい。
【００４４】
このように、基準進行方向は、ワークに対してツールがある領域を通過する際に大局的に進ませたい方向とする。具体的には、障害となる部分を通過する方向、予期せぬ大きな凹凸形状部分を通過する方向、障害物がないとした場合に教示軌道によって与えられる移動方向である。また基準押付方向は、上記基準進行方向に基づいて、障害物がないとした場合に教示軌道に基づいて倣う場合のツールの押付方向、障害となる形状や予期せぬ大きな凹凸形状部分を通過する際の押付方向となる。但し、上記基本移動動作が位置制御である場合、基準押付方向は教示軌道に近付く方向とする。
【００４５】
図３及び図４は基準進行方向及び基準押付方向の定め方の一例であり、故に基準進行方向及び基準押付方向は、一連の作業を通して設定された方向等の、予め設定された方向を使用してもよい。
【００４６】
また基準進行方向及び基準押付方向は、基本移動動作から粗倣い動作に切り替えたときに予め設定されていた教示データによって与えられる方向としてもよい。
【００４７】
或いは基準進行方向及び基準押付方向は、基本移動動作から粗倣い動作に切り替えたときに実際に移動していた位置に基づいて算出した進行方向と、該進行方向に基づいて算出される押付方向としてもよい。
【００４８】
さらに或いは基準進行方向及び基準押付方向は、粗倣い動作中に更新された値を使用して求めてもよい。上述のように粗倣い動作の開始時（基本移動動作からの切替時）の進行方向及び押付方向に基づいて基準進行方向及び基準押付方向を設定した場合、障害となる形状が大きいと、参考とするツールの進行方向及び押付方向が、ワーク形状に合ったものでなくなることがある。このような場合には、基準進行方向及び基準押付方向を、粗倣い動作実行中に、教示軌道に基づいてワーク形状に沿った方向に更新することが望ましい。
【００４９】
本発明の主たる特徴は、ロボットを用いてツールとワークとを相対的に移動させているときに、障害物、壁、穴、大きな凹凸等の未知の形状を検出した場合、基本移動動作と粗倣い動作とを、力検出手段で検出する力の情報、或いは又はこれに加えて、ロボットが動かしている物体の位置情報に基づいて適宜切り替え、ロボット、作業ツール及びワークに対して過負荷を与えず、なるべく形状に沿って、必要に応じて回避しながら動かすことを迅速に行うことにある。なお粗倣い動作は、力や位置等に基づき、ワーク形状に応じて積極的にツール又はワークの一方を他方に対して現在の位置から局所的に遠ざける動作と、一方が他方の存在を確認する動作とを繰り返して行う、倣い動作を意味する。
【００５０】
但し、ワーク（他方）の形状が未知である場合、ある時点での移動動作が、他方の存在を確認するために近付く動作であるか離れる動作であるかは判別できない。そこで、一方が局所的に他方から離れることを目的とする動作を粗倣い構成動作１と称し、この動作の実行中には、基準進行方向及び基準押付方向に基づいて移動し、力検出手段を用いて検出された力によって移動方向が不適切であると判断された場合は移動方向等を変更しながら、一方が予め定めた（実行するまでに定められさえすればよい）時間又は距離を限度に、もとの位置から離れるような動作が行われる。また一方が他方の存在を確認することを目的とする粗倣い構成動作２では、障害となる物体を通過する際に基準となる基準進行方向及び基準押付方向に基づき、一方を他方が存在する方向に進ませるような動作が行われる。
【００５１】
以下、基本移動動作と粗倣い動作との切り替えの具体例について説明するが、基本移動動作には、力制御に基づくものと位置制御に基づくものとがあるので、先ずその両者に分けて説明を行う。基本移動動作が力制御に基づくものである場合、上記基本移動動作は、力制御によって押付力を制御しながら、ツールとワークとの間に作用する力が目標値（動作中に調整可能）となるようにツールをワーク形状に倣わせる動作であるとする。教示点に基づく教示軌道によって、力制御で倣わせるときに参考とする位置が与えられており、ある位置における進行方向は教示軌道によって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行うこともできる。また粗倣い動作は、ワーク形状に応じて積極的に、ツール又はワークの一方を現在の位置から遠ざける動作（局所的又は相対的に他方から離す動作）と、一方が他方の存在を確認する（通常は一方を他方に近付ける）動作とを繰り返す倣い動作である。また力制御に基づく基本移動動作を行っているときに粗倣い動作を行うべき場合としては、押付力を適宜調整しながらロボットを動かしているときに、障害物、壁、大きな凸凹等の、それまで倣ってきた形状とは異なり、適切に倣わせることが難しい形状物が存在し、それまでの力制御による倣い方法とは異なる方法に切り替えることが必要な場合が考えられる。
【００５２】
一方基本移動動作が位置制御に基づくものである場合は、上記基本移動動作はロボットを用いて作業ツールとワークとを相対移動させるにあたり、予め定めた教示点に基づき設定された教示経路に沿って作業ツールを移動させることを目的とする移動動作である。粗倣い動作については力制御の場合と同様である。また位置制御に基づく基本移動動作を行っているときに粗倣い動作を行うべき場合としては、以下のような場合が考えられる。
（１）ワークに対してツールを位置制御で動かし、バリ取り、研削、研磨作業等を行う場合。軌道を少しずつずらした位置制御を複数回行い、目的の作業を達成する場合もある。
（２）ワークの表面・形状状態や、ワークに存在する異物などを確認する検査等において、予め定めた範囲内でセンサ等（前記力検出手段に取り付けられた接触確認用の作業ツール等）の移動を行い、異物等の未知形状物があった場合に該未知形状になるべく沿ってセンサ等を動かしたい場合。
（３）ロボットの手先部を動かしているときに意図しない障害物があった場合、なるべく該障害物の形状に沿って該手先部を動かしたい場合。
【００５３】
図５は、力制御による倣い動作（基本移動動作）を説明する図である。教示点６０、６２に基づく教示軌道によってワーク２０に対するツール（図示せず）の進行方向が定められ、該進行方向に合わせて教示押付方向を決める倣い動作が行われる。目標進行方向６４に沿ってツール進めながら目標押付方向６６に対して力制御を行っているときに、軌道上に教示軌道とは異なる形状（例えば図５に示すような山形状の突起）が存在すると、該突起の表面が比較的滑らかであれば、押付方向６６について反力６８がかかるため好適な倣いが可能となることもある。しかし、押付方向成分に目標力がかかっているときに、その部分の傾斜角度や目標力によっては、進行方向（ここでは実際の位置に基づく進行方向ではなく、教示軌道によって与えられる進行方向を意味する。この場合、基準進行方向と同じ方向を意味しており、以降の説明においても、進行方向は、特に断わらない限り、同様の意味で使用する。）について過大な反力７０がかかり、特に進行方向の速度が速いときは好適な倣いができないことがある。このような場合、力に応じて進行速度を下げたり、目標押付力を下げたり、力制御の追従性能を向上させたりする等の工夫が必要であり、異物の形状によってはそれでも対応できない場合がある。
【００５４】
そこで本発明では、図６に示すように、ツール１８をワーク２０に対して目標力で押し付ける基本移動動作を実行しているときに、進行方向について閾値を超える力がかかった場合、上述の基準進行方向及び基準押付方向に基づき、粗倣い構成動作１と、粗倣い構成動作２と、それらの動作切り替え判定とを繰り返す（切り替えをしない場合もあり、また順序は問わない）。つまりツール１８をワーク２０から離す動作（矢印７２）と、ワーク形状を確認する動作（矢印７４）とを適宜切り替えて反復し、基準進行方向４４に障害物がないことが確認できたら、もとの基本移動動作に切り替えることによって、大きさや形状が未知な障害物に対しても、該障害物を回避しつつもなるべく該障害物に沿った移動が可能となる。また上述の動作切り替えをより細かく繰り返せば、ワーク形状により正確に沿った移動が可能となる。なお以降の説明において、基準進行方向は実線矢印４４、基準押付方向は破線矢印４６で表すものとする。
【００５５】
本発明によれば、図７（ａ）に示すようにツールの移動方向が基準進行方向に対して９０度以上変更させられる場合にも対応でき、また図７（ｂ）に示すようにコーナー部の倣い動作において、教示点が２点のみであっても好適に倣うことができる。
【００５６】
この場合の粗倣い動作の動作方法としては、種々考えられるが、粗倣い構成動作１及び粗倣い構成動作２の移動方向、移動速度及び動作時間を状況に応じて変えたり、粗倣い構成動作１、粗倣い構成動作２及び基本移動動作相互間での切り替えを適切に行ったりすることによって、状況に対応したより複雑な動作を実現できる。
【００５７】
次に、粗倣い構成動作１及び粗倣い構成動作２それぞれにおける移動方向の求め方の具体例を説明する。先ず基本移動動作から、粗倣い動作に切り替えたときの進行方向及び押付方向を、それぞれ基準進行方向及び基準押付方向として設定する。ここで、動作中の各位置に対応する教示軌道上の位置（例えば、その位置から最短距離にある教示軌道上の位置）で与えられる進行方向及び押付方向を、それぞれ基準進行方向及び基準押付方向としてもよい。
【００５８】
粗倣い構成動作１及び粗倣い構成動作２の移動方向は、基準進行方向及び基準押付方向の単位ベクトルに、実数（±の値、ゼロも含む）である係数をかけて、合成和をとることにより求める。具体的には、図８に示すように、粗倣い動作に切り替えたときの基準進行方向の単位ベクトルをＤrm、基準押付方向の単位ベクトルをＤrpとするとき、実数である係数αm1、βp1、αm2及びβp2を用いて、粗倣い構成動作１を実行するときの移動方向Ｄr1は、（αm1 Ｄrm ＋ βp1 Ｄrp）を正規化したベクトルとする。一方、粗倣い構成動作２を実行するときの移動方向Ｄr2は、（αm2 Ｄrm ＋ βp2 Ｄrp）を正規化したベクトルとする。なお、Ｄr1とＤr2とは互いに異なるものとし、また互いに反対方向の関係にもならないようにする。なお粗倣い構成動作２では、ツールがワークに接触するまで行わなくともよく、例えば接触前に粗倣い構成動作１に切り替えてもよい。
【００５９】
上述の係数は、ツールがワークに衝突したときは、そのときの反力に基づいて適切な移動方向を求めるべく調整されたり、粗倣い動作における粗倣い構成動作１又は粗倣い構成動作２の繰り返し動作での、粗倣い構成動作１又は粗倣い構成動作２の開始位置の変化量が小さいとき、移動方向を適切な方向にずらすように調整されたりすることが好ましい。また粗倣い構成動作１及び粗倣い構成動作２による粗倣い動作は、複数の直線的な動きを滑らかにつなげた移動や曲線的な移動を含むものとし、全体的により滑らかな動作とすることもできる。さらに、後述するような調整をおこなった場合、該調整をもとに、移動方向を算出するためのパラメータや基準進行方向、基準押付方向を変更し、移動方向を算出してもよい。
【００６０】
本発明の実施形態においては、粗倣い構成動作１及び粗倣い構成動作２の移動方向ベクトルは、上述のように基準進行方向ベクトルと基準押付方向ベクトルの線形結合和により求められる。それぞれのベクトルにかける係数は、ツールとワークとの間に作用する力や、ツール又はワークの粗倣い動作中の位置に基づき、状況に応じて変更可能であり、粗倣い構成動作１及び粗倣い構成動作２のそれぞれにおいて、適切な方向となるように係数を変えるものとする。また粗倣い動作中の基準進行方向ベクトル及び基準押付方向ベクトルは、粗倣い動作に切り替えたときの進行方向及び押付方向を使用するものとする。但し、問題となる部分を通過することができる方向であればその方向を使用してもよい。
【００６１】
粗倣い構成動作１、粗倣い構成動作２及び基本移動動作相互の切り替え方法としては、次のようなものが考えられる。
先ず図９に示すように、矢印７２で示す粗倣い構成動作１の実行中に、予め定めた閾値を超える力を検出せず、かつ、予め定めた移動時間が経過し又は予め定めた移動距離だけ移動したら、動作を矢印７４で示す粗倣い構成動作２に切り替える。
【００６２】
図１０（ａ）に示すように、矢印７２ａで示す粗倣い構成動作１の実行中に、予め定めた閾値を超える力が検出され、すなわちツールがワーク又は他の障害物に衝突したが、ツールが比較的狭い領域内にあってワークから離すために所定時間又は所定距離だけ動かすことができない場合は、ワークに対するツールの移動方向を変更して、再度、矢印７２ｂで示すような粗倣い構成動作１を実行するか、ツールをワークに対して停止させることができる。図１０（ｂ）は同様の考え方により、矢印７２ａで示す粗倣い構成動作１から矢印７２ｂで示す粗倣い構成動作１、さらに矢印７２ｃで示す粗倣い構成動作１に切り替えられていく様子を示している。
【００６３】
ここで粗倣い構成動作１の実行時に、予め定めた閾値を超える力を検出することなく所定時間又は所定距離だけツールを移動させることができれば、粗倣い構成動作２に切り替えてもよい。また次回以降の粗倣い構成動作１では、矢印７２ｂ、７２ｃで示すような変更された粗倣い構成動作１を用いてもよい。このようにすれば、実際のワーク形状に対して粗倣い構成動作１の移動方向が不適切であっても、所望の方向に所定時間又は所定距離だけツールを移動させることができる。
【００６４】
図１１に示すように、矢印７４で示す粗倣い構成動作２の実行中に、予め定めた閾値を超える力が検出され、すなわちツールがワーク又は他の障害物に衝突したときは、動作を矢印７２で示す粗倣い構成動作１に切り替える。
【００６５】
図１２に示すように、矢印７４ａで示す粗倣い構成動作２の実行後に、予め定めた閾値を超える力が検出されず、すなわちワーク又は障害等となる形状が確認できないとき、矢印７４ｂで示すように移動方向を変更し、粗倣い構成動作２を実行してもよい。これにより、粗倣い構成動作２によるワーク又は障害物等の確認が不十分であることを防止できる。
【００６６】
図１３に示すように、矢印７４で示す粗倣い構成動作２の実行中に、予め定めた閾値を超える力が検出されず、すなわちワーク又は障害物等となる形状が確認できないときに、矢印７６で示す基本移動動作を再開してもよい。なお図１３の基本移動動作のように、力制御による倣いでは、ツールを押付方向に動かしワークとの接触を確認するまでは進行方向に進ませないか、或いは押付力に応じて移動速度を変化させると、ワークとツールとが接触しない領域を小さくすることができる。
【００６７】
図１４に示すように、矢印７４で示す粗倣い構成動作２の実行中に、基準進行方向４４について予め定めた閾値を超える力が検出されず、一方基準押付方向４６については予め定めた閾値を超える力が検出された場合は、ワークは存在するが、その時点から基本移動動作で倣うことが可能であると判断されるので、矢印７６で示すような基本移動動作を再開してもよい。
【００６８】
図１５に示すように、矢印７２で示す粗倣い構成動作１の実行中に、基準押付方向４６について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）が検出され、基本移動動作を行うためにツールが倣えるような形状の存在が確認されたときは、基本移動動作７６を再開してもよい。
【００６９】
図１６に示すように、矢印７２ａで示す粗倣い構成動作１の実行中に、基準押付方向について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）は検出されないが、それ以外の方向について別の予め定めた閾値を超える力が検出され、すなわちツールとワーク又は障害物との衝突が確認されたときは、ツールの移動方向を変えて、再度、矢印７２ｂで示す粗倣い構成動作１を実行するか、或いはツールを停止させてもよい。再度の粗倣い構成動作１（７２ｂ）の後は、粗倣い構成動作２及び粗倣い構成動作１を繰り返している。なお図１６（ａ）はツールが連続しない他のワーク又は障害物に接触した場合であり、図１６（ｂ）は連続するワークの他の部分に接触した場合である。
【００７０】
なお図１６の例は、矢印７２ａで示した粗倣い構成動作１の実行時において衝突したワーク又は障害物にツールを倣わせたくない場合を示しているが、該衝突以降、粗倣い動作で該衝突した部分の形状に倣わせたいときであって、さらに、粗倣い構成動作１の実行時に、基準押付方向について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）は検出されないが、それ以外の方向について別の予め定めた閾値を超える力が検出され、ワークに衝突したことが確認されたときは、図１６（ｂ）において破線矢印で示すように、後述する基準進行方向に山形状の突起等の障害物が存在しているときの粗倣い動作に切り替え、ツールが障害物を乗り越えるような粗い動作を行ってもよい。なおこの場合において、力を検出したときの位置が粗倣い構成動作１の開始位置から予め定めた閾値を超えて離れているときは、粗倣い動作を停止し、障害物となる形状を越えるような粗倣い動作をさせてもよい。
【００７１】
図１７に示すように、矢印７４で示す粗倣い構成動作２の実行中に、基準押付方向について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）が検出され、基本移動動作を行うためにツールが倣えるような形状の存在が確認されたときは、基本移動動作７６に切り替え（再開）してもよい。
【００７２】
図１８に示すように、矢印７４で示す粗倣い構成動作２の実行中に予め定めた閾値を超える力が検出されず、所定の移動時間又は所定の移動距離を限度にツールが移動したら、ツールがワークに接触していなくとも、矢印７２で示す粗倣い構成動作１を実行することができる。図１８の例は、粗倣い動作においてツールを必要以上に基準進行方向と逆方向に進ませずに、ワーク形状に対して粗く倣わせたい場合に有効である。なお１回の粗倣い構成動作２において、基準進行方向と逆方向にどの程度を限度としてツールをワークに近付けるかは、各パラメータによって調整可能である。
【００７３】
図１９に示すように、矢印７４で示す粗倣い構成動作２の実行中に、予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）は検出されないが、それ以外の方向について、別の予め定めた閾値を超える力が検出され、すなわちツールがワークに衝突してワーク形状の存在が確認されたときは、矢印７２で示す粗倣い構成動作１を実行することができる。
【００７４】
図２０に示すように、矢印７４ａで示す粗倣い構成動作２の実行後に、ツールの移動方向、移動距離又は移動時間を変えて、再度、矢印７４ｂで示すような粗倣い構成動作２を実行してもよい。これによって、粗倣い構成動作２において、ツールがより確実にワーク形状に沿う（接触する）ようにすることができる。換言すれば矢印７４ｂで示す粗倣い構成動作２は、ツールがワークに接触するように、矢印７４ａで示す粗倣い構成動作２の実行後に、移動方向、移動距離、移動時間又は移動速度を変えるものである。
【００７５】
図２１に示すように、図２０のようにツールをワークに接触させずとも、矢印７４ａで示す粗倣い構成動作２を実行後に移動方向、移動距離、移動時間又は移動速度を変えて、矢印７４ｂで示す粗倣い構成動作２を実行し、ワーク接触前に矢印７２で示す粗倣い構成動作１を実行し、その後再び、矢印７４ａ、７４ｂで示す粗倣い構成動作２を行って、ツール移動方向を基準進行方向４４の逆方向にずらし、ワーク側に近付けていくようにしてもよい。
【００７６】
図２２に示すような窪み形状のワークにツールを倣わせる場合において、図２２（ａ）の左上方に示すように、矢印７２ａで示す粗倣い構成動作１の移動方向にツールを動かすとワークに接触し、粗倣い構成動作１を所定時間又は所定距離だけ行うことができず、故に反力を受けた方向に基づいて粗倣い構成動作１の移動方向をずらして、再度、矢印７２ｂで示す粗倣い構成動作１を行っている場合を考える。
【００７７】
このとき、図２２（ｂ）に示すように、矢印７２ｂで示す粗倣い構成動作１に続く粗倣い構成動作２（矢印７４）によってツールを基準進行方向に移動させると、ずっとその位置付近での移動を繰り返す状態に陥ることがある。
【００７８】
そこで、粗倣い構成動作１を繰り返し実行した後の位置が、基準押付方向４４と逆方向の位置について、その時点までで該逆方向に最も進んだ位置より手前の位置、又は、該最も進んだ位置から所定距離を超えて該逆方向の位置に存在しないなら、ツールの移動方向を、基準押付方向の逆方向かつ基準進行方向の逆方向にずらし、矢印７２ｃで示すような粗倣い構成動作１を行う。これにより、図２２のような窪み部分等において、ツールがある領域内での移動を繰り返して該領域から脱出できないという状況を避けることができる。
【００７９】
なお図２２（ａ）を用いて説明した方法を用いてもなお、粗倣い動作実行中においてある領域内での移動を繰り返す状態に陥っていると判断された場合は、以下のような処置を採ることができる。
（１）基準押付方向４６と逆方向（図２２では上向）に最も進んだ位置と比較するときの判定距離を大きくする。
（２）粗倣い構成動作１及び粗倣い構成動作２の移動時間又は移動距離を大きくする。
（３）移動中の位置をもとに、ツールの停滞状況が解消されるように粗倣い構成動作１又は粗倣い構成動作２の移動方向を変える。
【００８０】
図２３に示すように、矢印７２で示す粗倣い構成動作１の実行時に、押付方向成分の力が小さいため、基準押付方向について予め定めた閾値を超える力は検出されないが、基準進行方向について閾値を超える力が検出された場合、以下の方法が考えられる。
（１）矢印７４で示す粗倣い構成動作２に切り替え、ツールを基準進行方向と逆方向に移動させる。
（２）又は、ワークからの反力の大きさ、方向に基づき、基準進行方向とは逆方向の成分を持つ方向にツールを移動させる粗倣い構成動作１を行う。移動方向を変えた粗倣い構成動作１によって、ツールがワークと接触し、基準進行方向と逆方向について閾値を超える力が検出されたら、粗倣い構成動作１の移動方向を基準進行方向の成分を含む方向に変更し、移動時間又は移動距離を小さくする。
【００８１】
或いは、元の基本移動動作に切り替え、そのときの力をもとに、直ちに基準進行方向に障害となる形状がある（そのときの位置から基準進行方向に山形状の突起がある）場合に対応した、粗倣い動作に切り替えてもよい。
【００８２】
基準押付方向について予め定めた閾値を超える力（基準押付方向の逆方向からかかる力）が検出されず、かつ、基準進行方向について予め定めた閾値を超える力（基準進行方向の逆方向からかかる力）も検出されず、かつ、別の予め定めた閾値を超える反力が検出された場合、粗倣い構成動作１の移動する向きを該反力をもとに変え、粗倣い構成動作１を実行することが考えられる。例えば、図２４に示すように、矢印７２ａで示す粗倣い構成動作１の実行時に、ツールが本来倣うべきワーク２０ａから離れ、他のワーク又は障害物２０ｂに接触して、基準押付方向の逆方向について予め定めた閾値を超える反力（基準押付方向からかかる力）が検出された場合、該反力の方向、大きさをもとに、ツールの移動方向を変え、矢印７２ｂで示すような粗倣い構成動作１を行う。また粗倣い構成動作１の実行時に、ツールがワークと接触することなく所定時間又は所定距離動くことができたときは、粗倣い構成動作２に切り替えてもよい。このように動作を切り替えることによって、粗倣い構成動作１の実行中に、実際のワーク形状に応じた動きをさせることができる。
【００８３】
図２５に示すように、粗倣い動作実行中において粗倣い構成動作２を行っているときに、ツールとワークとの間に作用する力及びツールの位置をもとに、ワークになるべく近付け又は接触させてワーク形状を確認できるように動作を切り替えることができる。具体的には、粗倣い構成動作２の実行時に、所定時間又は所定距離だけツールを移動させてもツールがワークに接触しない場合、基準進行方向の逆方向への移動距離を大きくするために、先ず全体的な変更許容値と１回動作当たりの変更許容値を決めておき、粗倣い構成動作２の実行時の移動距離、移動時間又は移動速度を大きくする。但し、移動速度を過大にすると衝突する際の力が大きくなるので、移動速度はあまり大きくしないことが望ましい。
【００８４】
図２５（ａ）は、矢印７４ａ、７４ｂ及び７４ｃで示すように、粗倣い構成動作２の移動距離を、ある制限範囲内で徐々に大きくしている様子を示している。一方図２５（ｂ）は、矢印７４ａ、７４ｂ及び７４ｃで示すように、粗倣い構成動作２の移動距離を、ある制限範囲内で徐々に大きくするとともに、移動方向を基準進行方向とは逆方向に少しずつ変化させている様子を示している。このようにすれば、粗倣い構成動作２の実行時に、ツールがワークに接触しない場合に、ツールをワークに接触するように移動させ、よりワーク形状に沿って動かすことができる。
【００８５】
基準押付方向にワークがあることが確認できるまで図２５のような動作を続けることにより、基準押付方向に押し付けるような形状がない（そのときの位置から基準進行方向に谷がある）場合に、大きさや形状が未知のワーク又は障害物に対しても、迅速かつ、なるべくツールをワーク又は障害物の形状に倣わせて動かすことができる。
【００８６】
粗倣い動作において、粗倣い構成動作１や粗倣い構成動作２を繰り返し実行しても、ツールの位置があまり変わらず停滞している場合や、ツールの進み方が遅い場合に、粗倣い動作の動きを調整してこれらの問題を解消することができる。以下、具体的に説明する。
【００８７】
先ず、粗倣い動作時に粗倣い構成動作１又は粗倣い構成動作２を、ある回数以上又は所定時間以上繰り返し実行したときに、ツールが移動した位置やその変化を監視する。例えば、粗倣い構成動作１の実行中の開始位置又は粗倣い構成動作２の実行中の開始位置をもとに、粗倣い動作の開始時又はある時点の位置に対する位置の変化や、粗倣い動作実行中に通過したことのある位置との変化量（通過したことのある位置と予め定めた閾値とから領域を求め、その中に入っていないか）や、ある位置を基準としてある回数実行又はある時間経過したときの距離や、ある位置を基準としてある回数実行したとき又はある時間経過したときの移動距離の変化の様子等を監視する。その監視結果から、ツールが同じような位置付近に戻ってきていたり、図２２で示したように同じような位置で停滞していたり、ある時間間隔で比較したときの位置の変化量が小さかったりしたときは、動作を変える必要があると判断する。なおこれらの原因としては、粗倣い動作のパラメータが不適切であることや、ワーク形状が複雑であることが考えられる。
【００８８】
一例として、図２６（ａ）に示すように、ワークに対するツールの進み方が遅いと判断される場合、粗倣い構成動作１及び粗倣い構成動作２の一方又は双方の移動方向を変えることが考えられる。なお図２６（ｂ）に示す例では、矢印７２で示す粗倣い構成動作１の移動方向を変えている。或いは、粗倣い構成動作１及び粗倣い構成動作２の一方又は双方において、予め定めた制限値内で、ツールの移動時間、移動速度又は移動距離を大きくすることも考えられる。このようにすれば、ツールが同じ位置付近からあまり動けていないときや進み方が遅いときに、その停滞状況から脱出したり、ツールがワーク形状に倣う時間を短くしたりすることができる。但し、ワーク形状への倣い方が粗過ぎないようにしたい場合は、そのように上記制限値を調整しておけばよい。
【００８９】
本発明では、粗倣い動作によってツールがワークに対して意図しない方向に移動し続けた場合に、安全のため、ロボットの動作を停止させることができる。ここで「意図しない方向に移動し続けている」状況としては、例えば以下のような状況が考えられる。
（１）ツールが全く予期せぬワーク形状にずっと倣い続けてしまっている。
（２）ツールがある部分に嵌り込んでしまっている。すなわち、移動方向や移動量等を変えても、その位置から動くことができない。
（３）動作パラメータが不適切であり、ある部分をツールが適切に回避して動くことができない。
【００９０】
上述の「意図しない方向に移動し続けている」状況を判定する具体的方法としては、粗倣い動作時に以下のことを検出又は確認することが考えられる。
（１）粗倣い構成動作１又は粗倣い構成動作２を、ある回数以上繰り返している。
（２）粗倣い動作の実行時間が、所定時間を超えている。
（３）粗倣い動作の移動位置が、想定される位置から離れすぎている。
（４）基本移動動作から粗倣い動作へ切り替えたときの位置から現在のツールの位置までの距離又は総移動距離が、所定距離を超えている。
（５）基本移動動作が教示軌道に基づく動作である場合に、ツールが、教示軌道からの最短距離が閾値以上の位置にいる。
上記のような状況が検出又は確認されたら、ツールが「意図しない方向に移動し続けている」と判断し、安全のため、ロボットの動作を止めることが好ましい。これにより、粗倣い動作によって想定外の動作をした場合の危険な状況を回避できる。
【００９１】
本発明では、粗倣い動作時において粗倣い構成動作１又は粗倣い構成動作２の実行中に、ワークに対するツールの移動速度が大きく、ツールとワークとが過大な力で接触するような場合、移動速度を下げる（調整する）ことによって過大な力で衝突しないようにし、ツールやワークの破損等を回避することができる。さらに、最初は移動速度をある程度小さな値に設定し、回避時間を短くするために移動速度を少しずつ大きな値に変え、反力がある程度以上になったら速度を下げ、その後、その速度で動かすようにして速度を適切な値に調整するという、ツール及びワークの状況に応じた適切な速度の自動調整を行うこともできる。
【００９２】
本発明では、ツールをワーク形状に沿って動かしたときの動作をもとに、教示データを生成又は修正することができる。具体的には、粗倣い構成動作１及び粗倣い構成動作２によって、ワークに対してツールを接離させる動作（接触が望ましいが接触していなくともよい）を繰り返して動かすとした場合、ツールとワークとの離隔と接触とが繰り返されるため、その動作ピッチを短くしたとしても粗倣い動作はジグザグな動きをすることになる。そこで、教示データの生成を行うときは、ロボット制御装置の、制御周期毎に計測した、粗倣い構成動作１及び粗倣い構成動作２、或いは又はこれに加え、基本移動動作の位置に基づいて、例えば以下のような方法を用いて、所定の速度指令で動かす教示データを生成し又は教示軌道を修正することができる。
（１）粗倣い動作時には、粗倣い構成動作１及び粗倣い構成動作２の実行中に力検出手段によってツールとワークとの接触を検出したときのツールの位置や、粗倣い構成動作２の最終位置を位置データとして使用する。
（２）連続する位置データにおける位置間の距離が予め定めた閾値以下であるような、互いに近い位置が存在する場合には、それらを軌道生成のためのデータとして用いず、適当な代表点のみを使用する。
（３）例えば図２７に示すように、位置データとして使用する点の位置の変化を見たとき、ある点までとそれ以降の部分とで急にツールの移動方向が変わる部分は角部であると推定し、位置データとして適当な補間点７８、８０を入れ、その位置データをもとに軌道を生成する。
（４）最小自乗近似等により、軌道生成に用いる位置データに沿った滑らかな軌道を生成する。
（５）教示データをもとに位置の補間等を行いながら滑らかに動くように移動するとき、実際に通過する位置と、教示データによって与えられる位置とが、角部やコーナ部などにおいてずれることがある場合、それを考慮し、位置データの位置と実際に通過する位置とのずれが所定閾値以下になるように、位置データを補正し、教示データを生成する。
（６）教示データ上の移動速度はユーザが設定した値を用いる。又は、作業の目的や条件に応じて、予め用意したデータテーブルを参照して、適切な移動速度を設定する。さらに、移動速度によって、実際に通過する位置と、教示データによって与えられる位置とがずれることがある場合、そのずれが所定閾値以下になるように、前記のように与えられる速度をもとに、位置に応じて調整した移動速度を設定してもよい。
（７）教示データ作成時に押付方向も設定する場合には、生成される軌道をもとにその進行方向と直交する方向を押付方向とする。
以上のようにすれば、粗倣い動作実行中の位置を、粗倣い構成動作１及び粗倣い構成動作２の動作の特徴を考慮しながら、或いは又はこれに加え、基本移動動作実行中の位置を、ロボット制御装置の制御周期毎に取得し、その位置をもとに、動作中の速度には関係なく、また教示データには不要な動作中の位置は除き、適当な間隔で位置データを生成することができ、故にワーク形状に沿った軌道を移動させる教示データを生成又は修正することができる。
【００９３】
図９〜図２７を参照して説明したように、粗倣い構成動作１及び粗倣い構成動作２における移動方向、移動速度及び移動時間は、ワークに対するツールの位置や両者間に作用する力に基づき、状況に応じて変更されることが好ましい。また粗倣い構成動作１及び粗倣い構成動作２において、これらの動作を行ってもツールがワークに対して同じ位置付近に留まることがないように、粗倣い動作の移動中の位置を監視しながら移動方向、移動速度及び移動時間の少なくとも１つが調整される。さらに、粗倣い構成動作１及び粗倣い構成動作２において、安全の観点から、移動時間又は移動距離に制限を設けておくことが好ましい。
【００９４】
粗倣い構成動作１及び粗倣い構成動作２において、ツールがワークに衝突するときに発生する力が、検出を目的とする閾値以外の別の閾値を超えた場合に、移動速度を小さくすることによってツールやワークに過負荷がかかることを防止できる。粗倣い動作実行中の位置に基づき、粗倣い動作の進み具合が遅いと判断された場合には、速度を適切に大きくしたり、移動方向を基準進行方向又は基準押付方向とは逆方向に変えたり、移動時間又は移動距離を大きくすることが好ましい。但し、そのような調整によってワーク形状への倣いが過度に粗くなって問題となる場合には、適切な値に制限しておくことが望ましい。
【００９５】
粗倣い構成動作２の移動時間又は移動距離は、適切な距離だけ移動するように調整することが好ましく、具体的には、移動距離が短過ぎると進行方向（教示軌道によって与えられる進行方向）にワークが存在していることを検出することができず、逆に移動距離が長過ぎると進行方向にワークが存在しないにも関わらず不必要に移動を続けることを考慮して、調整される。但し基準進行方向にツールを進ませながら、大まかにワーク形状に沿わせることで十分である場合は、ツールをワークに接触するまで移動させ続ける必要はなく、制限距離内の移動で十分である。
【００９６】
上記粗倣い構成動作２の移動時間又は移動距離の調整方法としては、以下のようなものが考えられる。
（１）粗倣い動作中の移動位置に基づいて求められる、ワーク形状及びツールの現在位置に基づき、粗倣い構成動作２の移動距離、移動時間及び移動速度を調整する。例えば、粗倣い構成動作２の実行前の、粗倣い構成動作１の開始位置から、基準進行方向について所定距離だけ先に進ませるように、パラメータを定める。
（２）実験に基づいて、使用するワーク形状に適した移動時間又は移動距離を定め、その値を限度としてツールを動かす。
（３）粗倣い動作中に調整された、移動時間又は移動距離を限度としてツールを動かす。
【００９７】
以下、力制御により基本移動動作を行う種々の形態について説明する。先ず図２８は、矢印７６ａ、７６ｂ、７６ｃで示す基本移動動作では、教示点８２、８４により定められる教示軌道に沿って力制御で倣わせるときに参考とする進行方向及び位置が予め与えられており、ある位置におけるツール１８の進行方向は、実際の位置に基づく変化ではなく、教示軌道における進行ベクトルによって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行っているとしてもよい。
【００９８】
図２８の例では、ワーク２０に障害物となる凸状物８６が存在しているので、矢印７６ａで示す基本移動動作から粗倣い動作７２ａ、７４ａに切り替えられ、凸状物８６をツール１８が乗り越えたら基本移動動作７６ｂに移行し、次に押付方向について、倣う形状が存在しなくなるので、粗倣い動作７２ｂ、７４ｂが実行され、再度基本移動動作（７６ｃ）に戻ることになる。状況に応じて粗倣い動作を変えるため、力検出手段で検出される力をもとに、粗倣い構成動作１及び粗倣い構成動作２の動作が設定される。
【００９９】
図２８の例では、状況判断のために、粗倣い動作７２ａ、７４ａによって障害となる形状を回避できたかどうか、粗倣い動作７２ｂ、７４ｂによって倣うべき形状が現れたかどうかを確認する。また粗倣い動作７２ｂ、７４ｂを用いなくとも、押付力に応じて移動速度をかえるような基本移動動作を行って、接触を確認するまで移動速度を小さくするようにしてもよい。また基本移動動作は、教示軌道をもとに、力制御を行い、教示軌道とは異なった軌道を動いているとする。基本移動動作で力制御を行う場合は、教示軌道が与えられておらず、ワーク形状に沿って倣う形態としてもよい。
【０１００】
図２９は、図２８の凸状物８６とは異なる形状の凸状物８８がワーク２０に存在している例を示す。矢印７６ａと７６ｂで示される基本移動動作の間の領域について、検出される力に基づいて粗倣い構成動作１の動作方向を変えながら、粗倣い構成動作２で基準進行方向に動かし障害物がないかどうかを確認しながらツール１８が進められる。力をもとに、障害物がないと判断されたら、基本移動動作（７６ｂ）でツールを倣わせる。前方に倣わせる物体が急になくなったとき、再び粗倣い動作に切り替え、所定の移動時間又は移動距離を限度に、ワーク２０に対するツール１８の接離（但し接触しない場合もある）を繰り返しながら、ツールをワークに倣わせる。このとき所定方向について力を検出したら、基本移動動作（７６ｃ）に再び切り替えられる。
【０１０１】
図３０に示すように、ワーク２０に未知の形状物９０が存在していても該形状物が緩やかな山のような形状であり、基本移動動作の実行中において力制御のみでもツール１８を倣わせることができる場合を考える。しかしこの場合において、山形状の突起物９０によって、進行方向についての予め定めた閾値を超えるような力はツールにかからないが、教示点８２、８４間に設定された教示軌道９２とツール１８の位置との乖離（距離）が予め定めた閾値Ｓ１を超えるときは、ツールの軌道が教示軌道から大きく離れ、基本移動動作を続行したのでは移動時間が長くなる虞があるので、ツール１８の位置に基づいて粗倣い動作に切り替えてもよい。
【０１０２】
図３０の例では、現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、該最短距離が予め定めた閾値Ｓ１を超え、進行方向（教示軌道によって与えられる方向を意味する。ここでは、基準進行方向と同じ）についてツールとワークとの間に作用する力に応じて粗倣い動作に切り替えるための予め定めた閾値を超えるような力はツールにかからないが、進行方向（教示軌道によって与えられる方向）について別の予め定めた閾値を越える力はかかっているときに、基本移動動作７６から粗倣い動作７２、７４に切り替えられる。最初の粗倣い構成動作１及び粗倣い構成動作２の移動方向は、それまでに基本移動動作を実行していたときの力や位置に基づいて求められる。粗倣い構成動作２の移動方向は基準進行方向と同じ方向の成分を含む方向とし、粗倣い構成動作２の実行中に閾値を超える力が検出されたときは、粗倣い構成動作１に切り替える。このような粗倣い動作実行中に、粗倣い構成動作２において閾値を超える力を検出しなくなったときは、基本移動動作を再開する。粗倣い構成動作１と粗倣い構成動作２の移動方向は、これらからなる粗倣い動作によってツールが基準進行方向に進むように定められることが好ましい。また、ツールとワークとの間に作用する力が、該別の予め定めた閾値を超えているかどうかの確認をしなくとも、該最短距離が予め定めた閾値Ｓ１を超えている間、粗倣い動作を行うようにしてもよい。
【０１０３】
一方、例えば図３１に示すように、ワーク２０に未知の形状物９４が存在していても該形状物が谷のような形状であり、基本移動動作の実行中において力制御のみでもツール１８を倣わせることができる場合を考える。しかしこの場合において、谷形状の形状物９４によって、進行方向（教示軌道によって与えられる方向）についての予め定めた閾値を超えるような力はツールにかからず、かつ、押付方向についての別の予め定めた閾値を超えるような力がツールにかかるが、教示点８２、８４間に設定された教示軌道９２とツール１８の位置との乖離（距離）が予め定めた閾値Ｓ２を超えるときは、ツールの軌道が教示軌道から大きく離れ、基本移動動作を続行したのでは移動時間が長くなる虞があるので、ツール１８の位置に基づいて粗倣い動作に切り替えてもよい。
【０１０４】
図３１の例では、現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、該最短距離が予め定めた閾値Ｓ２を超えたときに基本移動動作７６から粗倣い動作に切り替えられる。この場合の粗倣い動作では、矢印７２で示す粗倣い構成動作１を先ず行い、粗倣い構成動作１によって所定距離又は所定時間だけツールを移動させた後、矢印７４で示す粗倣い構成動作２を行う。粗倣い構成動作２の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、粗倣い構成動作２によって所定距離又は所定時間だけツールを移動させた後、基本移動動作に切り替える。但し、粗倣い構成動作２の実行中に閾値を超える力が検出されたときは、粗倣い構成動作１に切り替える。再開された基本移動動作では、ツールを押付方向にも動かしながら移動させ、現在位置と教示軌道との最短距離が閾値Ｓ２を超えたとき再度粗倣い動作を行う。以上の動作を繰り返し、最終的には、基本移動動作によってツールをワークに倣わせて移動させる。
【０１０５】
図３１の場合において、粗倣い構成動作１においてツール位置が教示軌道から閾値を超えて離れた場合、ツールの移動方向を教示軌道へ向かう方向の成分を含むように変更することで、ツールを教示軌道に向かう方向に修正することができる。また図３１の例でも、粗倣い構成動作２では、ツールを基準進行方向に進めたときに、ワークが存在するかどうかを確認する。図３０では山のような形状がある場合について説明したが、図３１の場合でも、粗倣い構成動作２は他方（ワーク）の存在を確認する動作である。また、最初の粗倣い構成動作１において教示軌道と平行な方向に移動させ続け、接触後に、基本移動動作に切り替えるようにしてもよい。
【０１０６】
なお図９〜図２７を用いて説明した粗倣い動作の各例は、後述する基本移動動作が位置制御である場合にも同様に適用可能である。
【０１０７】
次に、基本移動動作が、指定された位置に基づいてワークに対してツールを移動させる位置制御に基づくものである場合について、図３２を参照しつつ説明する。図３２のように、教示点８２、８４に基づき定められる教示軌道９２に沿ってツール１８が移動させられ、該教示軌道上に障害物９６が存在しているとする。このように基本移動動作の軌道上に障害となる形状が存在する（山形状の突起がある）場合、基本的には、粗倣い動作によって、ツールをワークに対して停止させたり、ワークやツールに過負荷を与えたりすることなく、なるべくワーク形状に沿ってツールを倣わせる。以下、具体的に説明する。
【０１０８】
基準進行方向は、これまでの説明と同様に求めることができる。一方、基準押付方向は、予め定められた方向、各教示点に対して予め定められた方向、又は教示軌道上の進行方向との関係を設定しておくことによって決まる方向に基づき、粗倣い動作に切り替えたときの位置及び進行方向をもとに定める。このようにして定められた以降の基準押付方向は、あるときの位置から教示軌道に対して近づく方向や最短距離となる方向として求めてもよい。
【０１０９】
基本移動動作では、押付方向について力制御は行われないので、ツールが指定された位置を外れて押付方向に動かされることはないが、粗倣い動作では教示軌道から離れる。この場合、基準進行方向と基準押付方向とをもとに、これまでの説明と同様に、障害となる形状を回避する動作の移動方向や、障害となる部分に対してツールを近づけて形状を確認する動作の移動方向を定め、さらに、教示軌道に戻る方向を定める。
【０１１０】
粗倣い動作は、ワーク２０において基準進行方向に障害となる形状（山形状の突起）９６が存在する場合（図３２のＡ部）、或いは基準進行方向に障害物はないが、ある時点でのツール位置がもとの基本移動動作の軌道から外れており、もとの基本移動動作の軌道に復帰させようとする場合（図３２のＢ部）に実行される。但し、それぞれの場合でツールの動きは異なるので、力検出手段で検出される力、及び現在位置と教示軌道との位置関係によって、粗倣い動作を変える。
【０１１１】
先ず図３２のＡ部での動作方法について説明する。基本移動動作の実行中に、進行方向について閾値を超える力が検出されたときに、粗倣い動作に切り替えられ、先ず、第１の粗倣いパターン動作、すなわち粗倣い動作パターンＡ（第１の粗倣いパターン動作を、粗倣い動作パターンＡとも称することとする。）を実行する。粗倣い動作パターンＡでは、粗倣い構成動作２の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、粗倣い構成動作２の実行中に閾値を超える力が検出されたとき、粗倣い構成動作１を実行する。
【０１１２】
上記粗倣い動作パターンＡによる粗倣い動作の実行中に、粗倣い構成動作２において閾値を超える力を検出しなくなったとき、粗倣い構成動作１及び粗倣い構成動作２の動作を変えた粗倣い軌道復帰動作、すなわち粗倣い動作パターンＢ（粗倣い軌道復帰動作を、粗倣い動作パターンＢとも称することとする。）を実行する。粗倣い軌道復帰動作では、粗倣い構成動作１又は粗倣い構成動作２のどちらを先に実行しても構わないが、粗倣い構成動作２を先に実行することが好ましい。粗倣い軌道復帰動作における、粗倣い構成動作２の移動方向は、基準押付方向と同じ方向の成分を含む方向、又は教示軌道に向かう方向とする。粗倣い動作中に、ツールの現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、現在位置と教示軌道との最短距離が予め定めた閾値以内となったときに、基本移動動作を再開する。また粗倣い軌道復帰動作の実行中に、基準進行方向について予め定めた閾値を超える力が検出されたときは、粗倣い動作パターンＡを実行する。
【０１１３】
粗倣い軌道復帰動作における、粗倣い構成動作１及び粗倣い構成動作２の移動方向は、これらからなる粗倣い動作によってツールが基準進行方向に進むように定められる。また粗倣い構成動作１及び粗倣い構成動作２は、それらからなる動きによってツールの進み方が遅くならないように、或いはツールが同じ位置付近に留まらないように、粗倣い動作の移動中のツールの位置を監視しながら移動方向、移動速度及び移動時間の少なくとも１つを調整することが好ましい。また図３２のような場合では、粗倣い軌道復帰動作における、粗倣い構成動作１の移動方向は、基準進行方向と同じ方向の成分と、基準押付方向と逆方向の成分とを含む方向とすることが好ましい。
【０１１４】
なお、基本移動動作が力制御であっても位置制御であっても、その違いによらず、粗倣い構成動作１及び粗倣い構成動作２による粗倣い動作の動作は、これまで示したような直線的な動きを滑らかにつなげてもよいし、図３３に示すようにツールを曲線的に動かしてより滑らかに動くようにしてもよい。
【０１１５】
図３４は、上述の粗倣い動作パターンＡ及びＢを含む粗倣い動作を、図３０を用いて説明した、基本移動動作が力制御によるものである場合に適用した場合を示す図である。矢印７６で示す基本移動動作は、力制御によって押付力を制御しながら、ツール１８とワーク２０との間に作用する力が調整可能な目標値となるように、ツールをワーク形状に倣わせる動作であるとする。教示点８２、８４に基づく教示軌道９２によって、力制御で倣わせるときに参考とする位置が与えられており、ある位置におけるツールの進行方向は教示軌道によって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行ってもよい。
【０１１６】
力制御による基本移動動作の実行中に、未知の形状であっても力制御で適切に倣うことができ、進行方向（教示軌道によって与えられる方向）について予め定めた閾値を超えるような力はかからない状況であるが、教示軌道とは大きく異なる山のような形状物９０が存在する場合、ツール１８の位置に基づいて（例えば、教示軌道９２からの最短距離が閾値Ｓ１を超えるとき）、粗倣い動作に切り替えることが考えられる。このことは、ツールが倣う軌道が教示軌道から大きく離れるような場合、或いはツールをワーク形状にそのまま倣わせたくない場合に有用である。
【０１１７】
図３４の場合において、基本移動動作の実行中に、ツールの現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、該最短距離が予め定めた閾値Ｓ１を超えたとき、基本移動動作から粗倣い動作に切り替えられる。最初の粗倣い構成動作１及び粗倣い構成動作２の移動方向は、それまでに基本移動動作を実行していたときの力や位置に基づいて求められる。
【０１１８】
粗倣い動作に切り替えられたときは、先ず、図３２に関して説明したものと同等の粗倣い動作パターンＡを行う。この粗倣い動作パターンＡによる粗倣い動作の実行中に、粗倣い構成動作２において閾値を超える力を検出しなくなったとき、図３２に関して説明したものと同等の粗倣い軌道復帰動作、すなわち粗倣い動作パターンＢを実行する。
【０１１９】
粗倣い軌道復帰動作における、粗倣い構成動作１及び粗倣い構成動作２の移動方向は、これらからなる粗倣い動作によってツールが基準進行方向に進むように定められる。また粗倣い軌道復帰動作における粗倣い構成動作１の移動方向は、基準進行方向と同じ方向の成分と、基準押付方向と逆方向の成分とを含む方向にすることが好ましい。
【０１２０】
図３５は、図３１の場合における粗倣い動作に、上述の粗倣い軌道復帰動作（粗倣い動作パターンＢ）を適用した例を説明する図である。矢印７６で示す基本移動動作は、力制御によって押付力を制御しながら、ツール１８とワーク２０との間に作用する力が調整可能な目標値となるように、ツールをワーク形状に倣わせる動作であるとする。教示点８２、８４に基づく教示軌道９２によって、力制御で倣わせるときに参考とする位置が与えられており、ある位置におけるツールの進行方向は教示軌道によって与えられるとする。但し、教示軌道を中心にばね成分を持たせた押付制御を行ってもよい。
【０１２１】
力制御による基本移動動作の実行中に、未知の形状であっても力制御で適切に倣うことができ、進行方向（教示軌道によって与えられる方向）について予め定めた閾値を超えるような力はかからず、かつ、押付方向についての別の予め定めた閾値を超えるような力がツールにかかる状況であるが、教示軌道とは大きく異なる谷のような形状物９４が存在する場合、ツール１８の位置に基づいて（例えば、教示軌道９２からの最短距離が閾値Ｓ２を超えるとき）、粗倣い動作に切り替えることが考えられる。このことは、ツールが倣うべき軌道が教示軌道から大きく離れるような場合、或いはツールをワーク形状にそのまま倣わせたくない場合に有用である。
【０１２２】
図３５の例では、先ず粗倣い構成動作１を行う。粗倣い構成動作１によって所定距離又は所定時間だけツール１８を移動させた後、粗倣い構成動作２を行う。粗倣い構成動作２での移動方向は、基準進行方向と同じ方向の成分を含む方向とする。粗倣い構成動作２によって所定距離又は所定時間だけ移動させた後、上述の粗倣い軌道復帰動作（粗倣い動作パターンＢ）を行う。ツールが教示軌道９２から閾値Ｓ２以内の距離の位置に進んだら、基本移動動作を開始する。但し、図３５のように、基本移動動作の実行中に再びツール現在位置と教示軌道との最短距離が閾値Ｓ２を超えるときは、粗倣い動作を行う。続いて、粗倣い構成動作１の実行後に粗倣い構成動作２を実行し、粗倣い構成動作２の実行中に基準進行方向について閾値を超える力が検出されたら、障害物を含む領域を通過するための粗倣い動作を行う。障害物を越えて、ツールの現在位置と教示軌道との最短距離が閾値Ｓ２以下であれば、基本移動動作を再開する。
【０１２３】
図３６に示す例では、ワーク２０に未知の形状物９０が存在していても該形状物が緩やかな山のような形状であり、基本移動動作の実行中において力制御のみでもツール１８を倣わせることができる場合を考える。基本移動動作で倣う場合、山形状の上り部分では、進行方向（教示軌道によって与えられる方向を意味し、この場合、基準進行方向と同じ方向を意味する）と逆方向の力がかかり、その部分の先の下り部分では、進行方向（教示軌道によって与えられる方向）の力がかかるようになる。このとき、山形状の突起９０の頂点付近に到達し、進行方向と逆方向の力（教示軌道によって与えられる進行方向についての力）が所定閾値より小さくなる場合、ツールの位置に基づいて、例えば、教示軌道からの最短距離が予め定めた閾値Ｓ３を超えているとき、基本移動動作７６ａから粗倣い動作Ｂに切り替え、該最短距離が閾値Ｓ３を下回ったら基本移動動作７６ｂに戻す例が示されている。この例は、ツールが教示軌道から大きく離れ得る形状であって、上述のように、教示軌道から所定距離を超えて離れた位置から教示軌道方向に近づき下っていくような動作のとき、基本移動動作では時間がかかるので移動時間を短縮したい場合等、ツールをワーク形状にそのまま倣わせたくない場合に有用である。
【０１２４】
ここでは、現在位置と教示軌道との最短距離を所定時間間隔毎に算出しておき、前記最短距離が予め定めた閾値を超えたときに粗倣い動作に切り替える。粗倣い動作Ｂにおける最初の粗倣い構成動作１及び粗倣い構成動作２の移動方向は、それまでに基本移動動作７６ａを実行していたときの力や位置に基づいて求める。図３６の場合、粗倣い構成動作２の移動方向は、基準進行方向と同じ方向の成分を含む方向、又は教示軌道に向かう方向とする。粗倣い構成動作２実行中に、予め定めた閾値を超える力が検出されたとき、粗倣い構成動作１を実行する。この粗倣い動作実行中に、教示軌道からの距離が閾値以内にあり、かつ、基準押付方向について予め定めた閾値を超える力が検出されたとき、基本移動動作７６ｂを再開する。また粗倣い動作の実行中に、基準進行方向について予め定めた閾値を超える力が検出されたときは、上述の障害物を越える方法に切り替えることが好ましい。また粗倣い構成動作１及び粗倣い構成動作２の移動方向は、それらの動作によってツールが基準進行方向に進むように決めることが好ましい。
【０１２５】
図３７は、力制御によりツール１８をワーク２０に倣わせている基本移動動作７６ａの実行中に、ツール１８の進行方向（教示軌道によって与えられる方向を意味し、この場合、基準進行方向と同じ方向を意味する。）について、過負荷を検出して粗倣い動作に切り替えるための予め定めた閾値を超えるような力は検出されない状況だが，別の閾値を超えるような力はかかっており、教示軌道とツール位置との最短距離が予め定めた閾値Ｓ４を超えているとき、上述の粗倣い動作パターンＡによる粗倣い動作に切り替え、矢印７６ｂで示すように基本移動動作を再開し、その後、進行方向（教示軌道によって与えられる方向を意味し、この場合、基準進行方向と同じ方向を意味する。）と逆方向について予め定めた閾値を超える力がかかっており、さらに教示軌道とツール位置との最短距離が該閾値Ｓ４を超えているときは、粗倣い動作パターンＢによる粗倣い動作に切り替えることも考えられる。図３７の例では、ワーク形状が教示軌道から予め定めた閾値を越えて異なることを検出し、進行方向（教示軌道によって与えられる方向）又はその逆方向について力がかかっておらず、基準進行方向がその部分でのワーク形状に沿っていると判断できる、すなわちツールを倣わせやすい状況では基本移動動作を行い、それ以外の部分では粗倣い動作を行うことによって、ワーク形状に応じた倣い動作を実行することができる。
【０１２６】
なお図３７のように、基本移動動作中のツールの押付方向が基準押付方向４６と同じ方向であり、基準進行方向４４と基準押付方向４６が直交しており、ワーク形状が基準進行方向と平行になっていない場合は、ツールの押付方向がワーク形状に対して直交しないため、力制御では押付方向が適切とならない場合がある。しかしこのような状況においては、ツールをワーク形状に沿ってそのまま倣わせず、粗く倣わせることによって、その形状部分にある程度沿わせつつ、好適にツールを通過させることができる。
【０１２７】
図３８は、図３５と同様の谷のような形状９４が存在しているが、ツール１８をワーク２０にある程度以上は倣わせたくない場合に、ツールの位置に基づいて、例えば教示軌道９２からの最短距離が予め定めた閾値Ｓ２を超えたときに、基本移動動作７６から粗倣い動作に切り替えることも考えられる。これは、倣うべき軌道が教示軌道から大きく離れるような場合に有用であり、想定外の軌道に倣ってツールが全く意図しない方向へ移動したり、ワークから離れすぎて停止させたりすることなく、教示軌道から大きくずれるような部位ではツールをワーク形状に沿って倣わせず、粗倣いによって進ませたい場合に有用である。図３８では、基本移動動作７６の実行中に、ツール１８の現在位置と教示軌道との最短距離が閾値Ｓ２を超えたとき、粗倣い動作を行うが、先ず粗倣い構成動作１を行い、粗倣い構成動作１での移動方向は基準進行方向と同じ方向の成分を含む方向とする。
【０１２８】
図３９は図３８の変形例を示す。図３５と同様の谷のような形状９４が存在しており、基本移動動作７６ａの実行中に、現在位置と教示軌道との最短距離が予め定めた閾値Ｓ２を超えたときに粗倣い動作を行うが、このとき、矢印７２ａで示す粗倣い構成動作１を先ず行い、粗倣い構成動作１での移動方向は、基準進行方向と同方向の成分を含む方向とする。次に所定距離又は所定時間だけツール１８を移動させた後、矢印７４で示す粗倣い構成動作２を実行する。粗倣い構成動作２において所定距離又は所定時間だけツール１８を移動させた後、矢印７２ｂで示す粗倣い構成動作１を実行する。この粗倣い構成動作１の実行中に、基準進行方向について予め定めた閾値を超える力は検出されないが、基準押付方向について予め定めた閾値を超える力が検出されたとき、基本移動動作７６ｂを再開する。
【０１２９】
図４０は、図３８の変形例を示す。図３５と同様の谷のような形状９４が存在しており、基本移動動作７６ａの実行中に、現在位置と教示軌道との最短距離が予め定めた閾値Ｓ２を超えたときに粗倣い動作を行うが、このとき、矢印７２で示す粗倣い構成動作１を先ず行い、粗倣い構成動作１での移動方向は、基準進行方向と同じ方向の成分を含む方向とする。この粗倣い構成動作１の実行中に、基準進行方向について予め定めた閾値を超える力が検出されたとき、上述の障害物を越える粗倣い動作に切り替える。障害物を越える粗倣い動作においては、矢印７４で示す粗倣い構成動作２によって所定距離又は所定時間だけツールを移動させ、粗倣い構成動作１と粗倣い構成動作２とを繰り返してツールが障害物を乗り越えたら、矢印７６ｂで示す基本移動動作を再開する。
【０１３０】
なお基本移動動作が力制御によるものである場合に、教示点に基づく教示軌道によってツールの進行方向が定められ、該進行方向に合わせてツールの押付方向を定める倣い動作を行っているとすると、例えば図４１に示すように、教示軌道上に、ツール（図示せず）をワーク２０に対して基準押付方向４６に押し付けるようなワーク形状がない（そのときの位置から基準進行方向に谷がある）場合、進行方向が基準進行方向と同じ方向、押付方向が基準押付方向と同じ方向であり、それらをもとに基本移動動作を継続すると、図４１に示すワークの窪み形状の部分に近づくことはなく、進行方向の速度を小さくしツールを押付方向に移動させたとしても、ツールがワークの該窪み形状から離れてしまうことになる。
【０１３１】
そこで、所定時間又は所定比較回数だけ、力の進行方向成分が予め定めた閾値より小さく、かつ、押付方向成分が予め定めた閾値より小さい場合は、ツールは押付方向に押し付けることができない部分に差し掛かり、ワークに倣うことができない状態になっていると判定する。この判定において、実際にそのような部分に差し掛かっているのか、或いは単に倣いの性能が悪いため倣えていないのかを、なるべく正確に判断できるように、押付方向についての反力が小さい場合に進行速度を下げて倣いの性能を相対的に向上させることもできる。
【０１３２】
そして、粗倣い構成動作１と、粗倣い構成動作２と、それらの動作切り替え判定とを繰り返し（切り替えをしない場合もあり、また順序は問わない）、つまりツールをワークから離す動作と、ワークに近付ける動作と、それらの動作中に倣うべきワーク形状が存在しているかを確認する動作とを繰り返して、基準押付方向及び基準進行方向をもとに、ツールをワーク形状になるべく沿うように動かす。
【０１３３】
ここで、粗倣い動作実行中に、状況に応じて、基準押付方向について力がかかっていなくともその後の基本移動動作の押付方向が基準押付方向とは異なる場合には任意の方向について、所定の閾値を超える力がかあれば、或いは、粗倣い動作を継続する場合には基準押付方向について、所定の閾値を超える力がかかれば、基本移動動作に切り替えることによって、図４１のように押付方向に形状物が急に存在しなくなった場合（そのときの位置から基準進行方向に谷がある）でも、迅速かつ、なるべくワーク形状に沿った倣い動作を行うことができる。さらに、それらの動作をより細かく（短時間又は短距離で）繰り返すことによって、ツールをワーク形状により正確に沿わせることができる。
【０１３４】
これまで述べてきた本発明の応用によって、複雑なワーク形状に対しても簡単な教示によってツールを倣わせることもできる。例えば、図４２に示すように、円周上に凹凸部９８が設けられているワーク形状に対して円を動くような簡単な教示軌道１００によってツールを倣わせたい場合を考える。教示軌道によって進行方向及び押付方向が与えられる通常の倣い動作では倣わせることは困難であるが、本発明では、凹凸部９８の各々において、そのときの形状・位置に応じたツールの進行方向及び押付方向をそれぞれ基準進行方向及び基準押付方向として、それをもとに回避動作をおこなうため、教示軌道１００とは異なる未知の形状・障害物に対しても、上述の基本移動動作と粗倣い動作とを切り替えながら、なるべくワーク形状に沿いつつ障害物等を回避しながら、迅速にツールを動かすことができる。
【０１３５】
なお、図４２に示されるような、教示軌道とは異なる実際のワークの複雑な凹凸形状や教示軌道とのサイズの違いなどによる、教示軌道と実際のワーク形状とのずれは、教示軌道上の点を実軌道上の点に射影させて、目標とする移動方向や位置を合わせたり補正したりすることによって所望の方向や位置を取得し、対応することができる。よって、力制御を行う基本移動動作や粗倣い動作実行中に、教示軌道を参考としながら、教示軌道とは異なる複雑な形状に沿ってツールを回避させながら、教示軌道が意図する位置へ移動させることが可能である。さらに、上述のように、ワーク形状になるべく沿った移動をさせたり、他の部分との干渉を避けたりするためには、粗倣い構成動作実行時に実際に移動する距離を短くしておくことが望ましい。
【０１３６】
また、ツールとワークとの間に作用する力や力制御による倣い動作中の移動位置をもとにツールの進行方向と押付方向を求め、ワークに対して連続的に倣わせる方法を行う場合でも、ワークに不連続な部分があったり非常に複雑な形状部分があったりする等、ツールをワークに連続的に倣わせ続けるよりも、本発明によって、ワーク形状を確認する動作とワークから局所的にツールを離す動作とを繰り返して、ワーク形状に粗く倣わせる方が望ましい場合もある。このように，ワーク形状が複雑な場合や動作時間をなるべく短縮したい場合等において、ワーク形状にツールを粗く倣わせればよいというときにも本発明は有効である。
【０１３７】
なお上述の実施形態では、ロボットアームの先端に取り付けたツールをワークに対して移動させて倣わせる場合について説明したが、本発明の実施に際し、ロボットアーム先端部分に取り付けたハンドにより把持されたワークを、固定された加工ツール等に対して移動させるようにしてもよい。また、ロボットアームの先端に取り付けたツール又は把持させたワークを作用させる、他方のワーク又はツールを固定させた部分は、移動動作が可能な装置によって移動させてもよい。
【０１３８】
以上述べたように、本願発明では、ロボットを用いてツールとワークとを相対的に移動させているときに、障害物、壁、穴、大きな凹凸等の未知な形状があった場合でも、「それまでの移動動作」と、「積極的に一方を他方から離す動作と、他方の存在を確認する（通常は他方に近付ける）動作とを繰り返す、粗倣い動作」とを、力検出手段で検出する力の情報、及びロボットが動かしている物体の位置情報の少なくとも一方に基づいて適宜切り替え、ロボット、作業ツール及びワークに対して過負荷を与えず、好適な回避動作を行いつつなるべく形状に沿った迅速な倣い制御が可能となる。本発明は、加工を伴わない教示作業のための倣い制御、バリ取り作業、研磨・研削作業、ロールヘム加工等の押付けによる倣い加工作業、さらにはツールをワークに対して移動させて異物があるか、ワークが不良品であるか等を確認する作業にも有用である。
【符号の説明】
【０１３９】
１０ ロボットシステム
１２ ロボット
１４ ロボット制御装置
１６ 力センサ
１８ 作業ツール
２０ ワーク
２２ 作業台
２４ 各軸位置検出部
２６ 位置、速度、加速度検出・算出部
２８ 作用力算出部
３０ 第１のパラメータ格納部
３２ 第２のパラメータ格納部
３４ 動作切り替え判定・動作パラメータ調整部
３６ 指令演算部
３８ 基準進行方向、基準押付方向算出・更新部
【特許請求の範囲】
【請求項１】
作業ツールとワークの一方に対して他方をロボットの手先部によって相対的に動かし、両者間に作用する力を制御するロボット制御装置において、
前記両者間に作用する力を検出する力検出手段と、
所定の基準進行方向と基準押付方向とに基づいて実行され、所定の方向に、所定移動時間又は所定移動距離を限度に移動する第１の粗倣い構成動作及び前記所定方向とは異なる方向に、所定移動時間又は所定移動距離を限度に移動する第２の粗倣い構成動作からなる粗倣い動作を実行する粗倣い動作実行手段と、
前記ロボットの手先部を移動させる基本移動動作の実行中に検出された、前記両者間に作用する力における、所定方向の成分の評価結果、又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に基づいて基本移動動作から粗倣い動作に切り替える第１の動作切り替え手段と、
粗倣い動作実行中の動作切り替え判定において、粗倣い動作中に検出された、前記両者間に作用する力の評価結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に基づいて、粗倣い動作から基本移動動作への切り替え、第１の粗倣い構成動作から第２の粗倣い構成動作への切り替え、第１の粗倣い構成動作から別の第１の粗倣い構成動作への切り替え、第２の粗倣い構成動作から第１の粗倣い構成動作への切り替え、及び、第２の粗倣い構成動作から別の第２の粗倣い構成動作への切り替えのいずれかを実行する、第２の動作切り替え手段と、
を備えたことを特徴とするロボット制御装置。
【請求項２】
前記基本移動動作は、前記両者間に作用する力が目標値と一致するように、前記一方に前記他方を接触し続けながら移動させる倣い動作であり、
前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第１の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、
前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第２の粗倣い構成動作の移動方向は、基準進行方向と同じ方向の成分を含む方向であり、
第２の粗倣い構成動作の実行中に、前記両者間に作用する力と予め定めた第２の閾値との比較結果に応じて第１の粗倣い構成動作又は基本移動動作に切り替え、第２の粗倣い構成動作の最後まで、第１の粗倣い構成動作への前記切り替えを行わないとき、粗倣い動作から基本移動動作に切り替えるものであることを特徴とする、請求項１のロボット制御装置。
【請求項３】
前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第１の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、
粗倣い動作において、第２の粗倣い構成動作の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、
前記第２の動作切り替え手段が、第２の粗倣い構成動作の実行中に、前記両者間に作用する力と予め定めた第２の閾値との比較結果に応じて第１の粗倣い構成動作に切り替える動作を、第１の粗倣いパターン動作とし、
粗倣い動作において、
第２の粗倣い構成動作の移動方向は、基準押付方向と同じ方向の成分を含む方向とし、又は、基本移動動作が教示軌道に基づいて移動する場合、前記教示軌道に近付く方向とし、
前記第２の動作切り替え手段が、前記ツールまたはワークの位置と、前記指定された軌道の距離の評価結果に基づいて、粗倣い動作から基本移動動作に切り替える動作を粗倣い軌道復帰動作とし、
前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第１の粗倣いパターン動作の実行中に、第２の粗倣い構成動作の最後まで、第１の粗倣い構成動作への動作切り替えを行わないとき、粗倣い軌道復帰動作に切り替え、
粗倣い軌道復帰動作の実行中に、基準進行方向成分の力の大きさが予め定めた第３の閾値以上である場合、第１の粗倣いパターン動作に切り替えることを特徴とする、請求項１のロボット制御装置。
【請求項４】
前記基本移動動作は、該両者間に作用する力が目標値と一致するように、前記一方に前記他方を接触し続けながら移動させる倣い動作であり、
前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第４の閾値との所定比較時間又は所定比較回数の比較結果、及び、前記両者間に作用する力における、前記ロボットの手先部の押付方向の成分と、予め定めた第５の閾値との所定比較時間又は所定比較回数の比較結果又は前記ロボットが動かしている前記作業ツール又はワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、
前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、
第２の粗倣い構成動作の移動方向は、基準進行方向と逆方向の成分を含む方向であり、
粗倣い動作実行中に、前記両者間に作用する力における、所定方向の成分と、予め定めた第６の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、粗倣い動作から基本移動動作に切り替えるものであることを特徴とする、請求項１のロボット制御装置。
【請求項５】
前記第２の動作切り替え手段は、
前記動作切り替え判定において、第１の粗倣い構成動作の実行時に、前記両者間に作用する力が、予め定めた第７の閾値を超えたとき、第１の粗倣い構成動作の移動する向きを、検出される力の大きさ及び方向に基づいて変更し、別の第１の粗倣い構成動作を行い、
別の第１の粗倣い構成動作の実行時に、該別の第１の粗倣い構成動作が完了するまで、所定の条件を満たさないとき、第２の粗倣い構成動作に切り替えることを特徴とする、請求項１、２又は３のロボット制御装置。
【請求項６】
前記第２の動作切り替え手段は、
第１の粗倣い構成動作の実行後に、別の第１の粗倣い構成動作を行う場合において、
別の第１の粗倣い構成動作で所定移動時間又は所定移動距離、移動したときの位置が、
基準押付方向と逆方向の位置について、
一連の第１の粗倣い構成動作における開始時からそれまでで、前記基準押付方向と逆方向に最も進んだ位置から所定距離以下なら、移動方向を基準押付方向の逆方向の成分を含む方向にし、又は基準進行方向の逆方向の成分を含む方向にし、別の第１の粗倣い構成動作を行うように動作を切り替えることを特徴とする、請求項１、２、３又は５のロボット制御装置。
【請求項７】
前記第２の動作切り替え手段は、前記動作切り替え判定において、
第１の粗倣い構成動作の実行時に、前記両者間に作用する力における、基準押付方向の成分が、予め定めた第８の閾値以上である場合、粗倣い動作から基本移動動作に切り替え、前記両者間に作用する力における、基準押付方向の成分が、前記第８の閾値以上でなく、かつ、前記両者間に作用する力における、基準進行方向の成分が、予め定めた第９の閾値以上である場合、第２の粗倣い構成動作、又は別の第１の粗倣い構成動作、又は基本移動動作に切り替え、
前記両者間に作用する力における、基準押付方向の成分が前記第８の閾値以上でなく、かつ、前記両者間に作用する力における、基準進行方向の成分が、前記第９の閾値以上でなく、かつ、前記作業ツールと前記ワークとの間に作用する力が予め定めた第１０の閾値以上である場合、
第１の粗倣い構成動作の移動する向きを、検出される力の大きさ及び方向に基づいて変更し、別の第１の粗倣い構成動作を行うように動作を切り替え、
別の第１の粗倣い構成動作の実行時に、該別の第１の粗倣い構成動作が完了するまで、所定の条件を満たさなければ、第２の粗倣い構成動作に切り替えることを特徴とする、請求項１又は４のロボット制御装置。
【請求項８】
前記第２の動作切り替え手段は、前記動作切り替え判定において、
第２の粗倣い構成動作の実行時に、前記両者間に作用する力における、基準押付方向の成分が、予め定めた第１１の閾値以上である場合、粗倣い動作から基本移動動作に切り替え、
前記両者間に作用する力における、基準押付方向の成分が、前記第１１の閾値未満、かつ、前記両者間に作用する力が、予め定めた第１２の閾値以上であるとき、第１の粗倣い構成動作に切り替え、
第２の粗倣い構成動作の実行時に、該第２の粗倣い構成動作が完了するまで、所定の条件を満たさないとき、第１の粗倣い構成動作に切り替えることを特徴とする、請求項１、４又は７のロボット制御装置。
【請求項９】
前記第２の動作切り替え手段は、第２の粗倣い構成動作の実行時に、前記所定移動時間又は前記所定移動距離だけ移動しても、前記両者に作用する力が、予め定めた第１３の閾値未満である場合、第２の粗倣い構成動作の所定移動時間、所定移動速度及び所定距離のうち少なくとも１つを大きくするか、又は、
前記移動方向を基準進行方向の逆方向に所定量だけずらす、動作調整手段を含むことを特徴とする、請求項１、４、７又は８のロボット制御装置。
【請求項１０】
前記第２の動作切り替え手段は、粗倣い動作実行時に、
第１の粗倣い構成動作若しくは第２の粗倣い構成動作を所定回数以上繰り返し実行しても、又は、所定時間が経過するまで繰り返し実行しても、第１の粗倣い構成動作の開始位置、又は第２の粗倣い構成動作の開始位置の少なくとも２つ以上のそれぞれの開始位置に基づく開始位置の変化量が所定値以下のとき、
第１の粗倣い構成動作又は第２の粗倣い構成動作を行うときの、移動方向を変えるか、又は、第１の粗倣い構成動作の所定移動時間、所定移動速度及び所定移動距離、並びに第２の粗倣い構成動作の所定移動時間、所定移動速度及び所定移動距離のうち少なくとも１つを大きくする動作調整手段を含むことを特徴とする、請求項１〜９の内、何れか１項のロボット制御装置。
【請求項１１】
前記第２の動作切り替え手段は、
前記粗倣い動作実行時に、
第１の粗倣い構成動作若しくは第２の粗倣い構成動作を所定回数以上繰り返した場合若しくは所定時間経過した場合、粗倣い動作への切り替え時の開始位置からの距離が予め定めた第１４の閾値以上の場合、又は、基本移動動作が教示軌道に基づく場合かつ教示軌道からの最短距離が前記第１４の閾値以上の場合、
前記ロボットの手先部の動作を止めることを特徴とする、請求項１〜１０の内、何れか１項のロボット制御装置。
【請求項１２】
前記第２の動作切り替え手段は、
粗倣い動作実行時の、第１の粗倣い構成動作又は第２の粗倣い構成動作の実行時に、
前記両者間に作用する力が、予め定めた第１５の閾値以上である場合、
第１の粗倣い構成動作又は第２の粗倣い構成動作における所定移動速度を、そのときの所定移動速度より小さくする動作調整手段を含むことを特徴とする、請求項１〜１１の内、何れか１項のロボット制御装置。
【請求項１３】
粗倣い実行中又は基本移動動作の実行中に取得した位置データをもとに、所定の速度指令で動かす教示データを生成する教示データ生成手段を備えることを特徴とする、請求項１〜１２の内、何れか１項のロボット制御装置。
【請求項１】
作業ツールとワークの一方に対して他方をロボットの手先部によって相対的に動かし、両者間に作用する力を制御するロボット制御装置において、
前記両者間に作用する力を検出する力検出手段と、
所定の基準進行方向と基準押付方向とに基づいて実行され、所定の方向に、所定移動時間又は所定移動距離を限度に移動する第１の粗倣い構成動作及び前記所定方向とは異なる方向に、所定移動時間又は所定移動距離を限度に移動する第２の粗倣い構成動作からなる粗倣い動作を実行する粗倣い動作実行手段と、
前記ロボットの手先部を移動させる基本移動動作の実行中に検出された、前記両者間に作用する力における、所定方向の成分の評価結果、又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に基づいて基本移動動作から粗倣い動作に切り替える第１の動作切り替え手段と、
粗倣い動作実行中の動作切り替え判定において、粗倣い動作中に検出された、前記両者間に作用する力の評価結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に基づいて、粗倣い動作から基本移動動作への切り替え、第１の粗倣い構成動作から第２の粗倣い構成動作への切り替え、第１の粗倣い構成動作から別の第１の粗倣い構成動作への切り替え、第２の粗倣い構成動作から第１の粗倣い構成動作への切り替え、及び、第２の粗倣い構成動作から別の第２の粗倣い構成動作への切り替えのいずれかを実行する、第２の動作切り替え手段と、
を備えたことを特徴とするロボット制御装置。
【請求項２】
前記基本移動動作は、前記両者間に作用する力が目標値と一致するように、前記一方に前記他方を接触し続けながら移動させる倣い動作であり、
前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第１の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、
前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第２の粗倣い構成動作の移動方向は、基準進行方向と同じ方向の成分を含む方向であり、
第２の粗倣い構成動作の実行中に、前記両者間に作用する力と予め定めた第２の閾値との比較結果に応じて第１の粗倣い構成動作又は基本移動動作に切り替え、第２の粗倣い構成動作の最後まで、第１の粗倣い構成動作への前記切り替えを行わないとき、粗倣い動作から基本移動動作に切り替えるものであることを特徴とする、請求項１のロボット制御装置。
【請求項３】
前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第１の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、
粗倣い動作において、第２の粗倣い構成動作の移動方向は、基準進行方向と同じ方向の成分を含む方向とし、
前記第２の動作切り替え手段が、第２の粗倣い構成動作の実行中に、前記両者間に作用する力と予め定めた第２の閾値との比較結果に応じて第１の粗倣い構成動作に切り替える動作を、第１の粗倣いパターン動作とし、
粗倣い動作において、
第２の粗倣い構成動作の移動方向は、基準押付方向と同じ方向の成分を含む方向とし、又は、基本移動動作が教示軌道に基づいて移動する場合、前記教示軌道に近付く方向とし、
前記第２の動作切り替え手段が、前記ツールまたはワークの位置と、前記指定された軌道の距離の評価結果に基づいて、粗倣い動作から基本移動動作に切り替える動作を粗倣い軌道復帰動作とし、
前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、第１の粗倣いパターン動作の実行中に、第２の粗倣い構成動作の最後まで、第１の粗倣い構成動作への動作切り替えを行わないとき、粗倣い軌道復帰動作に切り替え、
粗倣い軌道復帰動作の実行中に、基準進行方向成分の力の大きさが予め定めた第３の閾値以上である場合、第１の粗倣いパターン動作に切り替えることを特徴とする、請求項１のロボット制御装置。
【請求項４】
前記基本移動動作は、該両者間に作用する力が目標値と一致するように、前記一方に前記他方を接触し続けながら移動させる倣い動作であり、
前記第１の動作切り替え手段は、前記両者間に作用する力における、前記ロボットの手先部の進行方向の成分と、予め定めた第４の閾値との所定比較時間又は所定比較回数の比較結果、及び、前記両者間に作用する力における、前記ロボットの手先部の押付方向の成分と、予め定めた第５の閾値との所定比較時間又は所定比較回数の比較結果又は前記ロボットが動かしている前記作業ツール又はワークの位置の評価結果に応じて、基本移動動作から粗倣い動作に切り替えるものであり、
前記第２の動作切り替え手段は、粗倣い動作実行中の動作切り替え判定において、
第２の粗倣い構成動作の移動方向は、基準進行方向と逆方向の成分を含む方向であり、
粗倣い動作実行中に、前記両者間に作用する力における、所定方向の成分と、予め定めた第６の閾値との比較結果又は前記ロボットが動かしている前記作業ツール若しくはワークの位置の評価結果に応じて、粗倣い動作から基本移動動作に切り替えるものであることを特徴とする、請求項１のロボット制御装置。
【請求項５】
前記第２の動作切り替え手段は、
前記動作切り替え判定において、第１の粗倣い構成動作の実行時に、前記両者間に作用する力が、予め定めた第７の閾値を超えたとき、第１の粗倣い構成動作の移動する向きを、検出される力の大きさ及び方向に基づいて変更し、別の第１の粗倣い構成動作を行い、
別の第１の粗倣い構成動作の実行時に、該別の第１の粗倣い構成動作が完了するまで、所定の条件を満たさないとき、第２の粗倣い構成動作に切り替えることを特徴とする、請求項１、２又は３のロボット制御装置。
【請求項６】
前記第２の動作切り替え手段は、
第１の粗倣い構成動作の実行後に、別の第１の粗倣い構成動作を行う場合において、
別の第１の粗倣い構成動作で所定移動時間又は所定移動距離、移動したときの位置が、
基準押付方向と逆方向の位置について、
一連の第１の粗倣い構成動作における開始時からそれまでで、前記基準押付方向と逆方向に最も進んだ位置から所定距離以下なら、移動方向を基準押付方向の逆方向の成分を含む方向にし、又は基準進行方向の逆方向の成分を含む方向にし、別の第１の粗倣い構成動作を行うように動作を切り替えることを特徴とする、請求項１、２、３又は５のロボット制御装置。
【請求項７】
前記第２の動作切り替え手段は、前記動作切り替え判定において、
第１の粗倣い構成動作の実行時に、前記両者間に作用する力における、基準押付方向の成分が、予め定めた第８の閾値以上である場合、粗倣い動作から基本移動動作に切り替え、前記両者間に作用する力における、基準押付方向の成分が、前記第８の閾値以上でなく、かつ、前記両者間に作用する力における、基準進行方向の成分が、予め定めた第９の閾値以上である場合、第２の粗倣い構成動作、又は別の第１の粗倣い構成動作、又は基本移動動作に切り替え、
前記両者間に作用する力における、基準押付方向の成分が前記第８の閾値以上でなく、かつ、前記両者間に作用する力における、基準進行方向の成分が、前記第９の閾値以上でなく、かつ、前記作業ツールと前記ワークとの間に作用する力が予め定めた第１０の閾値以上である場合、
第１の粗倣い構成動作の移動する向きを、検出される力の大きさ及び方向に基づいて変更し、別の第１の粗倣い構成動作を行うように動作を切り替え、
別の第１の粗倣い構成動作の実行時に、該別の第１の粗倣い構成動作が完了するまで、所定の条件を満たさなければ、第２の粗倣い構成動作に切り替えることを特徴とする、請求項１又は４のロボット制御装置。
【請求項８】
前記第２の動作切り替え手段は、前記動作切り替え判定において、
第２の粗倣い構成動作の実行時に、前記両者間に作用する力における、基準押付方向の成分が、予め定めた第１１の閾値以上である場合、粗倣い動作から基本移動動作に切り替え、
前記両者間に作用する力における、基準押付方向の成分が、前記第１１の閾値未満、かつ、前記両者間に作用する力が、予め定めた第１２の閾値以上であるとき、第１の粗倣い構成動作に切り替え、
第２の粗倣い構成動作の実行時に、該第２の粗倣い構成動作が完了するまで、所定の条件を満たさないとき、第１の粗倣い構成動作に切り替えることを特徴とする、請求項１、４又は７のロボット制御装置。
【請求項９】
前記第２の動作切り替え手段は、第２の粗倣い構成動作の実行時に、前記所定移動時間又は前記所定移動距離だけ移動しても、前記両者に作用する力が、予め定めた第１３の閾値未満である場合、第２の粗倣い構成動作の所定移動時間、所定移動速度及び所定距離のうち少なくとも１つを大きくするか、又は、
前記移動方向を基準進行方向の逆方向に所定量だけずらす、動作調整手段を含むことを特徴とする、請求項１、４、７又は８のロボット制御装置。
【請求項１０】
前記第２の動作切り替え手段は、粗倣い動作実行時に、
第１の粗倣い構成動作若しくは第２の粗倣い構成動作を所定回数以上繰り返し実行しても、又は、所定時間が経過するまで繰り返し実行しても、第１の粗倣い構成動作の開始位置、又は第２の粗倣い構成動作の開始位置の少なくとも２つ以上のそれぞれの開始位置に基づく開始位置の変化量が所定値以下のとき、
第１の粗倣い構成動作又は第２の粗倣い構成動作を行うときの、移動方向を変えるか、又は、第１の粗倣い構成動作の所定移動時間、所定移動速度及び所定移動距離、並びに第２の粗倣い構成動作の所定移動時間、所定移動速度及び所定移動距離のうち少なくとも１つを大きくする動作調整手段を含むことを特徴とする、請求項１〜９の内、何れか１項のロボット制御装置。
【請求項１１】
前記第２の動作切り替え手段は、
前記粗倣い動作実行時に、
第１の粗倣い構成動作若しくは第２の粗倣い構成動作を所定回数以上繰り返した場合若しくは所定時間経過した場合、粗倣い動作への切り替え時の開始位置からの距離が予め定めた第１４の閾値以上の場合、又は、基本移動動作が教示軌道に基づく場合かつ教示軌道からの最短距離が前記第１４の閾値以上の場合、
前記ロボットの手先部の動作を止めることを特徴とする、請求項１〜１０の内、何れか１項のロボット制御装置。
【請求項１２】
前記第２の動作切り替え手段は、
粗倣い動作実行時の、第１の粗倣い構成動作又は第２の粗倣い構成動作の実行時に、
前記両者間に作用する力が、予め定めた第１５の閾値以上である場合、
第１の粗倣い構成動作又は第２の粗倣い構成動作における所定移動速度を、そのときの所定移動速度より小さくする動作調整手段を含むことを特徴とする、請求項１〜１１の内、何れか１項のロボット制御装置。
【請求項１３】
粗倣い実行中又は基本移動動作の実行中に取得した位置データをもとに、所定の速度指令で動かす教示データを生成する教示データ生成手段を備えることを特徴とする、請求項１〜１２の内、何れか１項のロボット制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【図２９】
【図３０】
【図３１】
【図３２】
【図３３】
【図３４】
【図３５】
【図３６】
【図３７】
【図３８】
【図３９】
【図４０】
【図４１】
【図４２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【図２９】
【図３０】
【図３１】
【図３２】
【図３３】
【図３４】
【図３５】
【図３６】
【図３７】
【図３８】
【図３９】
【図４０】
【図４１】
【図４２】
【公開番号】特開２０１２−１３９７８９（Ｐ２０１２−１３９７８９Ａ）
【公開日】平成２４年７月２６日（２０１２．７．２６）
【国際特許分類】
【出願番号】特願２０１１−１１５（Ｐ２０１１−１１５）
【出願日】平成２３年１月４日（２０１１．１．４）
【出願人】（３９０００８２３５）ファナック株式会社 (1,110)
【Ｆターム（参考）】
【公開日】平成２４年７月２６日（２０１２．７．２６）
【国際特許分類】
【出願日】平成２３年１月４日（２０１１．１．４）
【出願人】（３９０００８２３５）ファナック株式会社 (1,110)
【Ｆターム（参考）】
[ Back to top ]