JPH09141582A - 力制御ロボットの接触位置検出装置 - Google Patents

Abstract

(57)【要約】 【課題】 安価な非接触式距離センサを用い高速で安全
に手先効果器を被検知物に接触して位置を検出できる力
制御ロボットの接触式位置検出装置を実現する。 【解決手段】 所定点ａにて停止し工具７とワーク６と
の距離を光学式変位計１２で測定し位置演算部４１がワ
ーク６の接触目標値及び低速移動点ｂを算出する。動作
制御指令部４４は演算部４１、パラメータ決定部４２の
演算が終了すると不感帯処理部２６に指令信号を供給し
不感帯幅を小さくして力制御モードとし力目標値ｆｒを
０に力目標値設定部２４に指令しパラメータ記憶部４３
の仮想バネ定数を取り出し位置力制御演算部２５に供給
する。指令部４４は演算部４１の位置検出結果によりｂ
点を通過したかを監視し通過したら制御状態を力一定モ
ードに切換え低速度ＶLで移動する。力センサ５の出力
を接触判定部４５が監視し力目標値と検出値とが等しく
なると工具７とワーク６とが接触したと判定する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、力制御ロボットの
接触位置検出装置に関し、特に、研削ロボットのような
多自由度力制御ロボットにおいて、アーム先部をワーク
に接触させることによりワークの実際の設置位置を正確
に検出する接触位置検出装置に関する。

【０００２】

【従来の技術】一般的な多自由度の力制御ロボットで、
そのアーム先部（手先効果器）が作業対象物に接近し、
この作業対象物に対して研削等の特定作業を行う場合、
作業対象物との間の距離関係を考慮して動作制御され
る。この力制御ロボットにおける距離関係を考慮した動
作制御は、例えば、ロボットの周辺部に非接触センサを
設け、この非接触センサによってアーム先部と作業対象
物との距離を検出し、その検出情報に基づいて、アーム
先部の軌道を制御する制御方式が知られている（例え
ば、特開昭６０−１０８２８５号公報）。

【０００３】この制御方式では、非接触式センサによっ
てアーム先部と作業対象物の距離が、予め設定された一
定値よりも小さくなったことが検出された場合に、制御
モードの切換または変更の処理を行うようにしている。

【０００４】また、従来の装置として、上記の非接触式
センサに加えて、接触式センサである力センサをアーム
の手首部に設け、ハイブリッド状態で制御方式を実現し
たものもある。このハイブリッド型制御方式のロボット
では、例えば、軸状ワークを他の固定ワークに組み付け
る場合、大きな衝撃の発生や低速移動の問題を避けるた
め、組付け直前の所定位置までは非接触センサを利用し
て位置制御モードで高速に移動して軸状ワークを搬送す
る。そして、組み付け直前の所定位置を検出した後は、
位置制御モードのままで低速移動に移り、軸状ワークと
固定ワークとの接触を力センサで検出して組付け作業を
実行していた。

【０００５】さらに、他の従来の装置として、第７回日
本ロボット学会学術講演会予稿集（平成元年１１月２〜
４日）の第１８９頁〜第１９２頁に記載されているもの
がある。この予稿集に記載されたロボットにおいては、
力センサと非接触型の渦電流式センサとを備え、位置制
御モードでの搬送移動中に非接触式距離センサがワーク
までの距離を測定し、その接近する距離情報に基づいて
適宜なタイミングで位置制御モードから力制御モードに
切換え、ワークとの接触時にはコンプライアンス制御を
行うように構成される。

【０００６】

【発明が解決しようとする課題】ところで、非接触式距
離センサを利用してワークとの距離または位置関係を検
出しながら所定区間の間ロボットのアーム先部の研削工
具等を高速に移動するようにした従来のロボットでは、
実際的な問題として、ロボットの制御手段は高速移動の
制御を行いながら距離センサで得られた距離情報を取込
み、この距離情報を利用して再び高速移動の制御のため
の指令を生成しなければならない。従って、距離センサ
の測定能力（距離検出とデータ処理）の限界を無視する
ことができなかった。

【０００７】すなわち、従来装置では、高速移動中に非
接触式の距離センサによってワークに至るまでの距離を
正確に測定することができない場合がある。このような
場合には、アーム先部に取り付けた研削工具が高速でワ
ークに衝突し、研削工具およびワークが共に破損すると
いう問題が起きる可能性がある。このため、組み付け直
前の所定位置に至るまでの間の移動は、距離センサの測
定能力の限界から、これ以上の高速化が困難であるとい
う問題があった。

【０００８】上記問題を解決する１つの方法としては、
距離データの取込み時間及びデータ処理時間が短い距離
センサを使用すればよい。しかし、このような非接触式
の距離センサは非常に高価格であり、上記位置検出装置
全体のコストアップとなるという問題が発生する。

【０００９】そこで、本発明の目的は、安価な非接触式
距離センサを使用して、高速にかつ安全に手先効果器を
被検知対象物に接触し、その位置を検出することのでき
る力制御ロボットの接触式位置検出装置を実現すること
である。

【００１０】

【課題を解決するための手段】本発明は、上記目的を達
成するため、次のように構成される。手先効果器に加わ
る力を検出する力検出手段と、手先効果器の位置を検出
するための位置検出手段と、力検出手段で得られる力デ
ータと位置検出手段で得られる位置データを入力し、各
データを用いて制御演算式に基づき制御指令のデータを
作成し、手先効果器の位置と力を制御する制御手段とを
備える力制御ロボットであり、制御手段を力制御モード
に設定し、力制御モードでの手先効果器の移動速度を力
目標値、位置目標値、特性補償係数、仮想バネ定数に基
づき算出し、手先効果器を力制御モードで移動させると
共に、手先効果器が対象物に接触した時、位置検出手段
で検出した位置データを対象物の位置として取り出す力
制御ロボットの接触位置検出装置において、手先効果器
の近くに設けられ、手先効果器と対象物の位置目標値と
の間の距離を測定する非接触式距離センサと、距離セン
サにより、手先効果が対象物の位置目標値との間の距離
が第１の距離の第１の位置となったときに、対象物の位
置目標値と第１の距離よりも短い第２の距離を有し、第
１の位置より対象物の位置目標値に近い第２の位置と、
手先効果器が第１の位置から第２の位置まで移動する第
１の移動速度と、手先効果器が第２の位置と対象物の位
置目標値との間を移動する移動速度であり、第１の移動
速度より小さい第２の移動速度とを設定する制御定数設
定手段とを備え、手先効果器は、第１の位置と第２の位
置との移動区間では、この第２の位置の目標値と仮想バ
ネ定数によるバネモードで移動し、第２の位置と対象物
との移動区間では、第２の移動速度を与える力目標値と
特性補償係数とによる力一定モードで移動する。

【００１１】好ましくは、上記力制御ロボットの接触位
置検出装置において、第２の位置と対象物との移動区間
の距離は、手先効果器の慣性による行過ぎ量と距離セン
サの測定誤差とのうちの少なくとも一方を考慮して設定
される。

【００１２】また、好ましくは、上記力制御ロボットの
接触位置検出装置において、対象物の位置目標値と第１
の位置との経路における対象物からの距離を△ｘとし、
仮想バネ係数をＫ、特性補償係数をＫｃとすると、手先
効果器は、第１の位置と第２の位置との移動区間の間の
所定の点から第２の位置までは、移動速度Ｖ＝Ｋｃ・Ｋ
・△ｘで表される式により算出された速度で移動し、第
１の位置から所定の点までは、上記式により算出される
所定点の移動速度の一定速度で移動する。

【００１３】また、好ましくは、上記力制御ロボットの
接触位置検出装置において、上記所定の点は、第１の位
置と対象物の位置目標値との２分の１の点である。ま
た、好ましくは、上記力制御ロボットの接触位置検出装
置において、手先効果器の先部で対象物に接触し、設定
された押し付け力を力検出手段が検出した時に、位置検
出手段で検出される位置データを対象物の位置であると
判定する判断手段を備える。

【００１４】また、好ましくは、上記力制御ロボットの
接触位置検出装置において、距離センサによる距離の測
定、および力制御モードでの移動のための設定は、ロボ
ット本体の動作を停止させて行う。

【００１５】本発明では、例えば、ティーチングプレイ
バック方式でかつ位置と力の制御が実行される制御手段
の制御の下で動作する研削ロボットにおいて、本来の研
削作業の開始前に、例えば研削工具を研削対象物に向か
って移動させ接触式で研削対象物の実際の設置位置を検
出するとき、研削対象物の設置誤差に約数ミリを付加し
た直前の位置まで研削工具を位置制御モードで移動さ
せ、そこで停止する。そして、その位置で、非接触式の
距離センサを用いて研削工具から研削対象物までの距離
を測定し、この測定距離に基づいて、その後の力制御モ
ードによる研削工具の移動に関し位置目標値を設定す
る。

【００１６】さらに、接触直前の低速移動区間を定め、
低速移動区間に入る前は適当な仮想バネ定数によるバネ
モードで移動し、低速移動区間では、仮想バネ定数を０
に設定し、力目標値と特性補償係数を適切に設定した力
一定モードで移動する。これによって、光学式変位計等
の安価な非接触式距離センサを利用して、衝突を生じる
ことなく高速にかつ安全に研削工具を研削対象物に接触
させ、その実際の設置位置を正確に検出できる。

【００１７】

【発明の実施の形態】以下に、本発明の実施形態を添付
図面に基づいて説明する。図１は、力制御ロボットの一
例である研削ロボットの全体システム構成を示す。ここ
で、力制御ロボットとは、制御部が位置と力の制御を行
い、この制御に従って力を伴う作業をワークに対して行
うロボットを意味する。また、この研削ロボットは、テ
ィーチングプレイバック方式で構成される。図１におい
て、１はロボット本体であり、複数の関節部を含むアー
ム２を備え、基台３の上に取り付けられる。

【００１８】各関節部にはモータ等の駆動装置が設けら
れ、各関節部は決められた範囲以内で稼働する機能を有
する。アーム２の各関節部の可動機能により、アームを
含むロボット本体１の全体の姿勢は、作業上必要な姿勢
に変化し、アーム２の先部は作業上必要とされる位置に
移動する。アーム２のリスト（手首）部４には６軸の力
センサ５が取り付けられる。力センサ５の先側部分に
は、ワーク６に対し研削作業を行う研削工具７が取り付
けられる。研削工具７は、例えばグラインダである。

【００１９】８はコントローラであり、コントローラ８
はコンピュータで構成される。コントローラ８は、位置
と力の制御を実行するため、予め定められた演算式等を
計算する制御プログラムを有し、この制御プログラム
と、与えられた条件に関するパラメータと、作業開始前
の教示内容とを用いて、ロボット本体１の動作を制御す
る。ロボット本体１の研削動作を制御するためのプログ
ラムは、コントローラ８の内部に設けられたメモリに格
納されている。

【００２０】コントローラ８は、信号ライン９で、ロボ
ット本体１に対して研削作業の動作を制御するための指
令を与える。また、コントローラ８は、力センサ５から
研削工具７に加わる力およびモーメントに関する検出信
号（力信号）１０を入力する。１１は教示用操作器であ
り、この教示用操作器１１は、研削ロボット１のコント
ローラ８に対し、実行すべき研削作業に関する諸条件を
設定し、そのメモリにセットする。教示用操作器１１
は、テンキーおよび各種の指令を与えるための操作スイ
ッチを有する。

【００２１】この教示用操作器１１によって、例えば、
研削作業前における基準ワークを用いた研削動作の教
示、研削作業条件の設定等の教示が行われる。この発明
の実施形態においては、教示用操作器１１を介して、後
述するような研削対象であるワークの実際の設置位置の
検出作業が行われる。

【００２２】なお、教示用操作器１１は、必要に応じ
て、研削作業を実行させるオペレータが所持する携帯用
のものとして構成できるし、また、固定設置用のものと
して構成することもできる。

【００２３】また、１２は非接触式の距離センサであ
り、力センサ５の先側部分に取り付けられる。この非接
触式の距離センサ１２には、安価なものが使用され、例
えば、光学式変位計が使用される。距離センサ１２の検
出部は、研削時に発生する火花の影響を受けにくい、研
削工具７の研削位置から離れた箇所に配置され、研削工
具７の刃先を含む領域に臨んでいる。

【００２４】距離センサ１２によって検出された、研削
工具７からワーク６までの距離に関するデータは、信号
ライン１３を介してコントローラ８に送給される。距離
センサ１２の距離検出の原理については、上述した光学
式のものの他に、例えば、磁気インピーダンスの変化を
利用するもの、発振周波数の変化を利用するもの、超音
波式のものなどが利用され、研削時に発生する火花の影
響を受けない離れた位置から距離を測定することができ
る各種のものを用いることができる。

【００２５】次に、図２を参照してコントローラ８の内
部の制御手段の詳細な構成について説明する。研削作業
を行うように設定されたロボット本体１に対して、この
研削作業の内容を決定する制御内容は、コントローラ８
の内部のメモリに格納されたプログラムによって実現さ
れる。

【００２６】図２は、コントローラ８の制御機能要素お
よび接続関係を示すブロック図であり、本発明の実施の
形態の要部を表す図であり、制御手段の構成を概念的に
示したものである。なお、ハード回路を利用して図２に
示す制御手段を構成することができるのは勿論である。

【００２７】また、ロボット本体１の内部には、アーム
２の各関節部を動作させるための駆動モータが複数内蔵
されており、これらの駆動モータには、それぞれの駆動
量を計測するための角度計２１が取り付けられる。これ
らの角度計２１によって、各関節部の軸角度データが測
定される。

【００２８】さて、図２において、基本構成である位置
・力制御演算系の構成は、次の通りである。位置・力制
御演算系は仮想コンプライアンス制御を実行する制御系
の構成を有する。ロボット本体１のアームリスト部４に
取り付けられた力センサ５で検出される力信号１０は、
力演算部２２に送られる。力演算部２２は、入力された
力信号をセンサ座標系からベース座標系に変換し、か
つ、入力された力信号から研削工具７の重力分を差し引
くことにより、重力補償を行う機能を有する。

【００２９】こうして、力演算部２２でワーク６と研削
工具７の接触点部分で発生する力〈ｆ〉が算出される。
ここで、記号「〈ｆ〉」はｆがベクトル量であることを
意味するものとする。以下において、記号「〈 〉」の
意味は同じであり、ベクトル量を示すこととする。

【００３０】力演算部２２で得られた力〈ｆ〉は、減算
器２３に入力され、この減算器２３で力〈ｆ〉と、力目
標値設定部２４に設定された力目標値〈ｆｒ〉との間の
減算が行われる。減算器２３の出力は、力〈ｆｄ〉とし
て位置・力制御演算部２５に供給される。上記力目標値
〈ｆｒ〉は、一般的に、研削工具７の押付け方向の力の
目標値を与えるための指令値である。

【００３１】また、研削工具７が非接触状態で、かつ力
制御モードで制御されて移動するとき、力目標値〈ｆ
ｒ〉は、研削工具７をワーク６に接近させ接触させるた
めの速度を決定する指令値となる。

【００３２】また、２６は不感帯処理部であり、減算器
２３の出力である力〈ｆｄ〉は、ここで不感帯処理を施
され、減算器３０に供給される。位置制御モードでは、
不感帯処理部２６で設定される不感帯設定領域を大きく
して、〈ｆｄ〉に関する信号を減算器３０へ供給される
ことを阻止し、位置フィードバック成分のみに基づいて
研削工具７の移動制御を実行するように制御される。

【００３３】角度計２１によって検出された各軸に関す
る角度データは、位置演算部２７に供給される。そし
て、位置演算部２７では、供給された角度データに基づ
いて、ベース座標系における研削工具７の位置〈ｘ〉
（姿勢データを含む）を算出する。算出された位置
〈ｘ〉は、位置偏差演算部２８において、位置目標値設
定部２９で設定された位置の目標値〈ｘｒ〉と比較さ
れ、それらの間の位置偏差〈△ｘ〉が求められる。

【００３４】上記のごとく、減算器２３で得られた力
〈ｆｄ〉と位置偏差演算部２８で得られた位置偏差〈△
ｘ〉とは、位置・力制御演算部２５に入力される。位置
・力制御演算部２５は、減算器３０と、バネ定数演算部
３１と、特性補償演算部３２とを有する。減算器２３か
ら出力された力〈ｆｄ〉は、不感帯処理部２６を介し
て、減算器３０に入力される。また、位置偏差演算部２
８から出力された位置偏差〈△ｘ〉は、バネ定数演算部
３１に入力される。

【００３５】バネ定数演算部３１には、仮想バネ定数行
列ｋが設定されている。そして、バネ定数演算部３１
は、入力された位置偏差〈△ｘ〉に対して、設定された
仮想バネ定数行列ｋを乗算することにより、Ｋ・〈△
ｘ〉を算出する。バネ定数演算部３１で算出されたＫ・
〈△ｘ〉は、減算器３０に対して出力される。そして、
減算器３０では、（〈ｆｄ〉−Ｋ・〈△ｘ〉）が演算さ
れる。この減算器３０で求められた演算結果（〈ｆｄ〉
−Ｋ・〈△ｘ〉）は、特性補償演算部３２に供給され
る。

【００３６】特性補償演算部３２にはコントローラゲイ
ン（特性補償係数）Ｋｃや速度の上限値が設定されてい
る。また、特性補償演算部３２に入力された上述の演算
値は、ここで制御上の特性補償を受け、それにより速度
指令値〈ｖ〉が算出される。位置・力制御演算部２５で
最終的に求められた速度指令値〈ｖ〉は、駆動指令部３
３に供給される。

【００３７】そして、この駆動指令部３３において、速
度指令値〈ｖ〉は、ロボット本体１の各駆動モータの駆
動指令値〈θ〉（角加速度である）に変換される。得ら
れた駆動指令値〈θ〉は、ロボット本体１の各駆動モー
タに供給され、各駆動モータの動作に基づき、ロボット
本体１は所望の位置および姿勢で動作する。

【００３８】上記において、研削工具７がワーク６に押
付けられている状態では、押付け方向について、速度指
令値ｖが０の状態で釣り合っており、 ｆ−ｆｒ＝Ｋ・△ｘ すなわち、ｆ＝ｆｒ＋Ｋ・△ｘ ……（１） となって、押付け力ｆは力目標値ｆｒと、仮想バネ力ｋ
△ｘの和となっている。

【００３９】また、接触前の空中においては、力ｆ＝０
であるから、速度指令値ｖは、 ｖ＝Ｋｃ・（−ｆｒ−Ｋ・△ｘ） ＝−Ｋｃ・（ｆｒ＋Ｋ・△ｘ） ……（２） となり、力目標値ｆｒ及びバネ力に比例した速度で空中
を移動する。

【００４０】上記移動を力制御モードでの移動と呼ぶこ
とにし、このうち、仮想バネ定数Ｋを０にした場合、す
なわち、 ｖ＝−Ｋｃ・ｆｒ ……（３） を力一定モードによる移動と呼び、力目標値ｆｒを０に
した場合、すなわち、 ｖ＝−Ｋｃ・Ｋ・△ｘ ……（４） をバネモードによる移動と呼ぶことにする。

【００４１】また、位置制御モードでの移動とは、不感
帯２６の不感帯幅を大きくして、力フィードバックを切
り、Ｋ及びＫｃを大きくした位置制御の状態で移動する
ことをいう。

【００４２】上述した仮想コンプライアンス制御を実現
する位置・力制御の基本構成に対して、さらに、本発明
の特徴部である制御定数設定部４０が付設される。この
制御定数設定部４０は、位置演算部４１と、パラメータ
決定部４２と、パラメータ記憶部４３と、動作制御指令
部４４とを備える。

【００４３】そして、位置演算部４１は、研削工具７が
ワーク６に向かって移動し、ワーク６に接触する過程に
おいて、非接触式の距離センサ１２による検出情報（研
削工具７の先端部からワーク６までの距離）に基づい
て、位置制御モードでの移動後における研削工具７のワ
ーク接触に至るまでの過程の初期の位置目標値及び低速
移動への切換え位置を演算する。

【００４４】また、パラメータ決定部４２は、上記位置
目標値の他に初期の高速移動時の仮想バネ定数や、速度
の上限値、低速移動時の移動速度を決定する力目標値な
どのパラメータを決定する。そして、パラメータ決定部
４２により決定されたパラメータがパラメータ記憶部４
３に記憶される。このパラメータ記憶部４３に記憶され
たパラメータは、動作制御指令部４４により必要に応じ
て、取り出され、力目標値設定部２４、位置・力制御演
算部２５、不感帯処理部２６、位置目標値設定部２９に
出力される。

【００４５】また、４５は接触判定部、４６は位置取込
み部である。接触判定部４５には、接続端子５０を介し
て、力演算部２２の出力信号である力〈ｆ〉を示す信号
が入力される。そして、接触判定部４５で、入力された
力〈ｆ〉の値と、力の設定値とが比較され、両者が等し
くなったときには接触検出信号が、位置取込み部４６に
供給される。

【００４６】位置取込み部４６は、接触判定部４５から
の接触検出信号が入力されると、このときの研削工具７
の接触箇所に関する位置データを位置演算部２７から取
り込む。これによって、ワーク６における定められた接
触箇所の位置を検出することが出来る。

【００４７】次に、上記構成を有する制御手段によって
実行される研削ロボットの特徴的な動作について説明す
る。上記特徴的動作は、ティーチングプレイバック方式
の研削ロボットに対して要求される基本動作であり、１
つ又はそれ以上の数のワークに対して、その被加工部を
研削する研削作業を実行する場合において、研削作業の
開始前に基準ワークについて被加工部の存在箇所に関す
る位置データを教示作業でコントローラ８のメモリに格
納する。そして、この状態において、実際の研削作業で
実際のワークが与えられたとき、その実際の設置位置を
検出し、基準ワークの設置位置とのずれを求める。これ
が上記基準的動作である。

【００４８】そして、上記特徴的動作の要点は、研削工
具７を実際のワーク６の所定の箇所に接近させ、研削工
具７の刃先を当該所定の箇所に接触させることによって
実際のワークの設置位置を検出するところにある。

【００４９】さらに詳しくは、最初、ロボット本体１の
動作を位置制御モードで制御し、研削工具７をワーク６
に位置制御モードで接近移動させる。そして、ワーク６
の近傍の所定位置で力制御モードに切換え、非接触式の
距離センサ１２で、その所定位置からワーク６までの距
離を測定する。続いて、その距離情報に基づいて、研削
工具７の刃先がワーク６の所定箇所に接触する位置目標
値を算出し、その他の仮想バネ定数や力目標値などのパ
ラメータを決定する。

【００５０】コントローラ８は、新たに得られたパラメ
ータによってロボット本体１およびアーム２の動作を力
制御モードにて制御し、この力制御モードで研削工具７
に接近移動および接触動作を行わせる。

【００５１】上記の動作において、位置制御モードによ
る移動の過程と、力制御モードおよび決められた速度パ
ターンによる移動の過程との間では、研削工具７の移動
動作を、一度停止することが望ましい。

【００５２】図３は、研削工具７の接触の対象である実
際のワーク６Ａを示す。このワーク６Ａの上面に被加工
部４７が形成されており、この被加工部４７を研削工具
７によって研削する作業が行われる。まず、実際のワー
ク６Ａの設置位置が検出される。ワーク６Ａの設置位置
を検出するには、基準面４８の上の一点４９に研削工具
７を接触させ、点４９の位置を検出することによりワー
ク６Ａの実際の設置位置を決定する。

【００５３】なお、教示作業では、基準ワークの基準面
における同一点についての位置データに基づいて、基準
ワークの設置位置が定められ、当該設置位置に基づき被
加工部を研削するための位置データが教示されている。
しかし、実際のワークと基準ワークの各設置位置は完全
に同じとは限らないので、実際のワークの設置位置を検
出し、基準ワークの設置位置とのずれを求め、教示され
た位置データを修正する必要が生じる。

【００５４】図３では、実際のワーク６Ａの設置位置を
検出するために、研削工具７をワーク６Ａに向けて移動
させる経路を示している。点ａに至る区間Ａでは、衝突
が起きない空間における移動であるので、研削工具７は
位置・力制御装置による位置制御モードで移動する。研
削工具７が点ａの位置に到達すると、一度停止する。そ
こで、位置・力制御装置を位置制御モードから力制御モ
ードに切換え、非接触式距離センサ１２で基準となる検
出点４９までの距離を測定し、測定した距離に基づいて
作成された制御情報によって研削工具７は移動を開始す
る。

【００５５】研削工具７は、点ａからの移動において最
初はバネモードにより高速移動し、その後、非接触式距
離センサ１２の測定誤差を考慮した距離だけ検出点４９
よりａ点側に存在するｂ点から検出点４９まで力一定モ
ードにより低速移動する。そして、研削工具７は、力制
御モードにて検出点４９に接触する。

【００５６】ワーク６Ａの設置誤差により検出点４９の
位置は、教示位置より変化している場合があるが、点ａ
において非接触式距離センサ１２で検出点４９までの距
離を測定し、基準点４９までの低速移動区間を、上述の
ようにして設定した点ｂから点４９までの一定距離とし
て、研削工具７を移動させている。従って、点ａから点
ｂまでの、研削工具７の高速移動中において、研削工具
７がワーク６Ａに衝突する等の誤動作が生じることがな
く、安全かつ高速に点ｂまで研削工具７を移動させるこ
とができる。

【００５７】研削工具７が検出点４９に接触することに
基づいて、ワーク６Ａの実際の設置位置が検出された
後、高速の力制御モードで研削工具７はワーク６Ａから
離れ、予め設定された位置に移動する。その後、ワーク
６Ａの実際の設定位置に基づいて修正された教示データ
に従って、被加工部４７を研削する作業が行われる。

【００５８】上述した例においては、位置検出の対象を
研削対象物である実際のワークとしたが、位置検出の対
象は、実際のワークに限定されず、他のものであっても
かまわない。また、上述の力制御モードによる移動は、
研削対象物である実際のワークの正確な位置検出のため
に行われたが、研削作業を開始するための接近・接触移
動においても同様に利用することができる。

【００５９】次に、図４のフローチャートおよび図５の
速度パターンを参照して、上述した位置検出のための移
動に関する制御動作を詳述する。以下の説明では、非接
触式距離センサ１２として安価な光学式変位計を用いる
ものとする。

【００６０】最初の設定状態では、ロボット本体１の動
作を位置制御モードで制御し、研削工具７の先端部を位
置制御モードで、目標停止位置点ａまで高速に移動させ
る（ステップＳ１）。このとき、ロボット本体１の高速
移動状態における慣性による行過ぎ量と光学式変位計１
２の測定誤差を考慮して、低速移動距離（ＸL）（図５
に示し、ｂ点から位置目標値に相当する）が位置演算部
４１に予め設定されている。研削工具７の先端が点ａの
位置に近づくと、ロボット本体１の動作を移動から停止
に切換え、研削工具７の先端を点ａで停止させる（ステ
ップＳ２）。

【００６１】次に、ステップＳ３において、ロボット本
体１の動作が停止した状態で、ワーク６Ａの位置検出点
４９までの距離を光学式変位計１２で測定する。そし
て、検出された距離に基づいて、位置演算部４１は、研
削工具７がワーク６Ａに接触する点の位置目標値及び低
速移動への切換点ｂの位置を算出する（ステップＳ
４）。

【００６２】この位置の目標値は、図３では点４９に相
当するが、ワーク６Ａの設置状態等で異なってくるの
で、ステップＳ３にて検出された距離に基づいて計算す
る。計算は現在の研削工具７の位置、工具７と距離セン
サ１２との位置関係及び距離センサ１２で検出した距離
から求められる。

【００６３】また、低速移動への切換点ｂの位置は、上
記で計算された位置目標値から予め設定された低速移動
距離（ロボット本体１の高速移動による慣性及び光学式
変位計の測定誤差を考慮した距離）だけ手前に来るよう
に計算される。

【００６４】そして、次のステップＳ５で、コントロー
ラ８内におけるロボット本体１に関する制御状態を、位
置制御モードから力制御モードのバネモードに切り換え
る。すなわち、動作制御指令部４４は、位置演算部４１
及びパラメータ決定部４２の演算が終了すると、不感帯
処理部２６に指令信号を供給して、不感帯幅を小さくし
て力のフィードバックが行われるようにして、力制御モ
ードとする。さらに、動作制御指令部４４は、力目標値
ｆｒを０にするように力目標値設定部２４に指令すると
ともに、パラメータ記憶部４３に記憶された仮想バネ定
数を取り出し、位置・力制御演算部２５に供給する。

【００６５】これによって、工具は位置目標値に向かっ
て、式（４）に示した速度で、バネモードによる移動を
行う（ステップＳ６）。この時の速度は、図５の点線で
示したように位置目標値と現在位置の偏差△ｘに比例す
る。すなわち、位置目標値に近づくにつれて高速→低速
へと変化する。実際には、初期の段階では速度の上限値
ＶHを設けて、それ以上の速度がでないようにする。こ
の動作は、動作制御指令部４４が、パラメータ記憶部４
３に記憶された速度上限値ＶHを取り出し、位置・力制
御演算部２５に指令する。

【００６６】従って、図５の実線で示すように、初めは
（点ａから点ｂまでの間）、研削工具７はＶHの速度で
高速に移動し、Ｋｃ・Ｋ△Ｘ＜ＶHとなる位置になる
と、△Ｘに比例して次第に低速になってゆく。そして、
ステップＳ７において、動作制御指令部４４は、位置演
算部４１からの位置検出結果に基づいて、ｂ点を通過し
たか否かを監視し、通過していなければ、ステップＳ６
に戻り、ｂ点を通過した場合には、ステップＳ８に進
み、制御状態を力一定モードに切換える。

【００６７】すなわち、動作制御指令部４４は、仮想バ
ネ定数Ｋを０にして位置・力制御演算部２５に指令し、
力目標値ｆｒを設定して力目標値設定部２４に指令す
る。このとき、力目標値ｆｒには低速移動速度ＶLに相
当する値が設定される。つまり、（３）式より、ｆｒ＝
−（１／Ｋｃ）・ＶLが設定される。ここで、Ｃ＝１／
Ｋｃとして、これを仮想粘性係数と呼ぶこともある。

【００６８】このようにして、ステップＳ９では、力一
定モードによる低速移動を行い、速度ＶLで移動する。
この速度ＶLは、工具７とワーク６Ａとが接触するまで
維持される速度であり、工具７がワーク６Ａに接触した
ときに、衝撃等の不具合が生じない速度にする。

【００６９】また、バネモードから力一定モードに変え
た時に、速度の急激な変動がないように、ｂ点でのバネ
モードでの移動速度がＶLになるようにする。すなわ
ち、ＶL＝Ｋc・Ｋ・ＸLになるように、パラメータを選
定する。これにより、図５のようになめらかな速度パタ
ーンを実現できる。

【００７０】ステップ１０では、力センサ５の出力信号
を接触判定部４５が監視し、力目標値と検出した力の値
が等しくなった時点で、研削工具７の先端部とワーク６
Ａとが接触したものと判定する。

【００７１】その判定の後、ロボット本体１およびアー
ム２の各関節部に設けた角度計２１によってワーク６Ａ
の位置を検出する。この時の接触を検出するための力目
標値は、研削作業時における押付け方向の力又はそれ以
上の力を設定しておく。そして、研削時の押付け力と力
検出値が等しいと判断された後に、ロボット本体１等の
各関節部に設けられた各角度計２１によってワーク６Ａ
の位置を検出する（ステップＳ１１）。

【００７２】なお、パラメータ決定部４２において、速
度上限値ＶHは、例えば、ａ点とｂ点との中間位置にお
ける直線式Ｖ＝Ｋｃ・Ｋ・△ｘ（図５の破線にて示す）
で表される速度となるように決定することができる。ま
た、低速移動速度ＶLは、工具７とワーク６Ａとの接触
時に、互いに衝撃が生じない程度の速度であり、例え
ば、速度上限値ＶHの３分の１の値とすることができ
る。そして、この低速移動速度ＶLに基づいて、仮想バ
ネ定数が決定される。

【００７３】

【発明の効果】本発明は、以上説明したように構成され
ているため、次のような効果がある。力制御ロボットの
接触位置検出装置において、手先効果器の近くに設けら
れ、手先効果器と対象物の位置目標値との間の距離を測
定する非接触式距離センサと、この距離センサにより、
手先効果が対象物の位置目標値との間の距離が第１の距
離の第１の位置となったときに、対象物の位置目標値と
第１の距離よりも短い第２の距離を有し、第１の位置よ
り対象物の位置目標値に近い第２の位置と、手先効果器
が第１の位置から第２の位置まで移動する第１の移動速
度と、手先効果器が第２の位置と対象物の位置目標値と
の間を移動する移動速度であり、第１の移動速度より小
さい第２の移動速度とを設定する制御定数設定とを備
え、手先効果器は、第１の位置と第２の位置との移動区
間では、この第２の位置の目標値と仮想バネ定数による
バネモードで移動し、第２の位置と対象物との移動区間
では、第２の移動速度を与える力目標値と特性補償係数
とによる力一定モードで移動するように構成される。

【００７４】したがって、安価な非接触式距離センサを
使用して、高速にかつ安全に手先効果器を被検知対象物
に接触し、その位置を検出することのできる力制御ロボ
ットの接触式位置検出装置を実現することができる。

【００７５】さらに、初期の高速移動から低速移動への
推移をバネモードを使って実現したため、簡単な構成で
なめらかに速度を変化させることができる。

【図面の簡単な説明】

【図１】研削ロボットの全体システムを示す構成図であ
る。

【図２】本発明の実施形態の要部構成を示すブロック図
である。

【図３】研削対象部を有する実際のワークの概略正面図
である。

【図４】図２の例の動作フローチャートである。

【図５】速度パターンの一例を示す図である。

【符号の説明】

１ ロボット本体 ２ アーム ５ 力センサ ６ ワーク ６Ａ 研削対象である実際のワーク ７ 研削工具 ８ コントローラ １２ 非接触式距離センサ ２２ 力演算部 ２３、２８、３０ 減算器 ２４ 力目標値設定部 ２５ 位置・力制御演算部 ２６ 不感帯処理部 ２７ 位置演算部 ２９ 位置目標値設定部 ３１ バネ定数演算部 ３２ 特性補償演算部 ３３ 駆動指令部 ４０ 制御定数設定部 ４１ 位置演算部 ４２ パラメータ決定部 ４３ パラメータ記憶部 ４４ 動作制御指令部

Claims (6)

【特許請求の範囲】
1. 【請求項１】 手先効果器に加わる力を検出する力検出
   手段と、手先効果器の位置を検出するための位置検出手
   段と、力検出手段で得られる力データと位置検出手段で
   得られる位置データを入力し、上記各データを用いて制
   御演算式に基づき制御指令のデータを作成し、手先効果
   器の位置と力を制御する制御手段とを備える力制御ロボ
   ットであり、上記制御手段を力制御モードに設定し、力
   制御モードでの手先効果器の移動速度を力目標値、位置
   目標値、特性補償係数、仮想バネ定数に基づき算出し、
   手先効果器を力制御モードで移動させると共に、手先効
   果器が対象物に接触した時、位置検出手段で検出した位
   置データを対象物の位置として取り出す力制御ロボット
   の接触位置検出装置において、 手先効果器の近くに設けられ、手先効果器と対象物の位
   置目標値との間の距離を測定する非接触式距離センサ
   と、 上記距離センサにより、手先効果器と上記対象物の位置
   目標値との間の距離が第１の距離の第１の位置となった
   ときに、上記対象物の位置目標値と第１の距離よりも短
   い第２の距離を有し、第１の位置より上記対象物の位置
   目標値に近い第２の位置と、手先効果器が第１の位置か
   ら第２の位置まで移動する第１の移動速度と、手先効果
   器が第２の位置と上記対象物の位置目標値との間を移動
   する移動速度であり、第１の移動速度より小さい第２の
   移動速度とを設定する制御定数設定手段と、 を備え、手先効果器は、第１の位置と第２の位置との移
   動区間では、この第２の位置の目標値と仮想バネ定数に
   よるバネモードで移動し、第２の位置と上記対象物との
   移動区間では、第２の移動速度を与える力目標値と特性
   補償係数とによる力一定モードで移動することを特徴と
   する力制御ロボットの接触位置検出装置。
2. 【請求項２】 請求項１記載の力制御ロボットの接触位
   置検出装置において、上記第２の位置と上記対象物との
   移動区間の距離は、手先効果器の慣性による行過ぎ量と
   上記距離センサの測定誤差とのうちの少なくとも一方を
   考慮して設定されることを特徴とする力制御ロボットの
   接触位置検出装置。
3. 【請求項３】 請求項１又は２記載の力制御ロボットの
   接触位置検出装置において、上記対象物の位置目標値と
   第１の位置との経路における上記対象物からの距離を△
   ｘとし、仮想バネ係数をＫ、特性補償係数をＫｃとする
   と、手先効果器は、上記第１の位置と第２の位置との移
   動区間の間の所定の点から第２の位置までは、移動速度
   Ｖ＝Ｋｃ・Ｋ・△ｘで表される式により算出された速度
   で移動し、第１の位置から上記所定の点までは、上記式
   により算出される上記所定点の移動速度の一定速度で移
   動することを特徴とした力制御ロボットの接触位置検出
   装置。
4. 【請求項４】 請求項３記載の力制御ロボットの接触位
   置検出装置において、上記所定の点は、第１の位置と対
   象物の位置目標値との２分の１の点であることを特徴と
   する力制御ロボットの接触位置検出装置。
5. 【請求項５】 請求項１〜４のいずれか１項記載の力制
   御ロボットの接触位置検出装置において、上記手先効果
   器の先部で対象物に接触し、設定された押し付け力を力
   検出手段が検出した時に、位置検出手段で検出される位
   置データを対象物の位置であると判定する判断手段を備
   えることを特徴とする力制御ロボットの接触位置検出装
   置。
6. 【請求項６】 請求項１〜５のいずれか１項記載の力制
   御ロボットの接触位置検出装置において、上記距離セン
   サによる距離の測定、および力制御モードでの移動のた
   めの設定は、ロボット本体の動作を停止させて行うこと
   特徴とする力制御ロボットの接触位置検出装置。