JPH07195287A

JPH07195287A - 力制御ロボットの接触式位置検出装置

Info

Publication number: JPH07195287A
Application number: JP35444493A
Authority: JP
Inventors: Yoshinori Furuno; 義紀古野; Tooru Kurenuma; 榑沼　　透
Original assignee: Hitachi Construction Machinery Co Ltd
Current assignee: Hitachi Construction Machinery Co Ltd
Priority date: 1993-12-29
Filing date: 1993-12-29
Publication date: 1995-08-01

Abstract

(57)【要約】【目的】力制御ロボットの接触式位置検出装置で安価
な非接触式距離センサで高速、安全に研削工具を対象物
に接触し、位置を検出する。【構成】研削工具７に加わる力を検出する力検出手段
5,22と、研削工具７の位置を検出する位置検出手段21,2
7 と、研削工具の位置と力を制御する制御手段8,25を備
える研削ロボットで、研削工具を力制御モードで移動
し、研削工具が対象物に接触した時の位置データを実際
の位置として取り出す。研削工具と対象物の間の距離を
測定する非接触式距離センサ12と、研削工具の力制御モ
ードでの移動で、距離センサで得られた距離に基づき距
離区間における初期の高速度移動区間と対象物直前の低
速度移動区間を含む速度パターンを生成する演算処理手
段40とを備える。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は力制御ロボットの接触式
位置検出装置に関し、特に、研削ロボットのごとき多自
由度力制御ロボットにおいてアーム先部をワークに接触
させることによりワークの実際の設置位置を正確に検出
する接触式位置検出装置に関するものである。

【０００２】

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

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

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

【０００５】

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

【０００６】上記問題を解決する１つの方法としては、
距離データの取込み時間およびデータ処理時間が短い距
離センサを使用すればよい。しかしこの場合には、非接
触式の距離センサの使用コストが高くなるという問題を
提起する。

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

【０００８】

【課題を解決するための手段】本発明に係る力制御ロボ
ットの接触式位置検出装置は次のように構成される。

【０００９】例えば研削工具等の手先効果器に加わる力
を検出する力検出手段と、手先効果器の位置を検出する
ための位置検出手段と、力検出手段で得られる力データ
と位置検出手段で得られる位置データを入力し、各デー
タを用いて制御演算式に基づき制御指令のデータを作成
し、手先効果器の位置と力を制御する制御手段を備える
例えば研削ロボット等の力制御ロボットであり、制御手
段を力制御モードに設定し、力制御モードでの手先効果
器の移動速度を力目標値、位置目標値、仮想粘性係数、
仮想バネ定数に基づき算出し、手先効果器を力制御モー
ドで移動させると共に、手先効果器が対象物に接触した
時、位置検出手段で検出した位置データを対象物の位置
として取り出す前記力制御ロボットにおいて適用される
接触式位置検出装置において、手先効果器の近くに設け
られ、手先効果器と対象物の間の距離を測定するための
非接触式距離センサと、手先効果器が対象物の近くにお
いて対象物に接触すべく当該対象物に向かって力制御モ
ードで移動するとき、距離センサによる測定で得られた
距離に基づいて当該距離区間における初期の高速度移動
区間と対象物直前の低速度移動区間を含む速度パターン
を生成する演算処理手段とを備え、制御手段の移動指令
に基づき手先効果器は速度パターンに従って力制御モー
ドによって移動するように構成される。

【００１０】前記低速度移動区間の距離は、手先効果器
の慣性による行過ぎ量と距離センサの測定誤差のうち少
なくとも一方を考慮して設定される。低速度移動区間が
最初に設定されることが好ましい。

【００１１】また移動初期の高速度移動区間の距離は、
距離センサで測定された距離と低速度移動区間の距離と
の差として設定される。

【００１２】前記の構成において、好ましくは、演算処
理手段は、高速度移動区間と低速度移動区間における速
度切換え時間を各区間の距離データを基に算出する時間
演算手段と、各区間の移動速度を算出する速度演算手段
と、速度データを制御手段内の制御用係数に変換する変
換手段と、係数を記憶する記憶手段と、係数をサンプリ
ングタイム毎に制御手段に入力する入力手段とを含むよ
うに構成される。

【００１３】前記の構成において、さらに、手先効果器
の先部で対象物に接触し、設定された押付け力を力検出
手段が検出した時に、位置検出手段で検出される位置デ
ータを対象物の位置であると判定する判断手段を備え
る。上記押付け力を、研削作業時の押付け力に設定する
と、研削作業時における研削工具のたわみ分を含んで対
象物の位置を検出することができるので、精度が向上す
る。

【００１４】前記の各構成において、距離センサによる
距離の測定、および力制御モードでの移動のための設定
は、ロボット本体の動作を停止させて行うことが好まし
い。

【００１５】

【作用】本発明では、例えばティーチングプレイバック
方式でかつ位置と力の制御が実行される制御手段の制御
の下で動作する研削ロボットにおいて本来の研削作業の
開始前に、例えば研削工具を研削対象物に向かって移動
させ接触式で研削対象物の実際の設置位置を検出すると
き、研削対象物の設置誤差に約数ミリを付加した直前の
位置まで研削工具を位置制御モードで移動させ、そこで
停止する。そしてその位置で、非接触式の距離センサを
用いて研削工具から研削対象物までの距離を測定し、こ
の測定距離に基づいて、その後の力制御モードによる研
削工具の移動に関し速度パターンを作成する。速度パタ
ーンは、初期の高速度移動区間と接触直前の低速度移動
区間を含むもので、低速度移動区間を優先的に決定し
て、全体の速度パターンを作成する。研削対象物に接触
する前の段階で、かかる速度パターンを作成し、この速
度パターンに従ってかつ力制御モードで移動することに
よって、光学式変位計等の安価な非接触式距離センサを
利用して、衝突を生じることなく高速にかつ安全に研削
工具を研削対象物に接触させ、その実際の設置位置を正
確に検出できる。

【００１６】

【実施例】以下に、本発明の実施例を添付図面に基づい
て説明する。

【００１７】図１は、力制御ロボットの一例である研削
ロボットのシステム構成を示す。ここで力制御ロボット
は、制御部が位置と力の制御を行い、この制御に従って
力を伴う作業をワークに対して行うロボットを意味す
る。またこの研削ロボットは、ティーチングプレイバッ
ク方式で構成される。図１で、１はロボット本体であ
り、複数の関節部を含むアーム２を備え、基台３の上に
取り付けられる。各関節部にはモータ等の駆動装置が設
けられ、各関節部は決められた範囲内で可動する機能を
有する。アーム２の各関節部の可動機能により、アーム
を含むロボット本体１の全体の姿勢は作業上必要な姿勢
に変化し、アーム２の先部は作業上必要とされる位置に
移動する。アーム２のリスト（手首）部４には６軸の力
センサ５が取り付けられる。力センサ５の先側部分に
は、ワーク６に対し研削作業を行う研削工具７が取り付
けられる。研削工具７は、例えばグラインダである。

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

【００１９】コントローラ８は、信号ライン９で、ロボ
ット本体１に対して研削作業の動作を制御するための指
令を与える。またコントローラ８は、力センサ５から研
削工具７に加わる力およびモーメントに関する検出信号
（力信号）１０を入力する。１１は、研削ロボットのコ
ントローラ８に対し、実行すべき研削作業に関する諸条
件を付与し、そのメモリにセットするための教示用操作
器である。操作器１１は、テンキーおよび各種の指令を
与えるための操作スイッチを有する。この操作器１１に
よって、例えば、研削作業前における基準ワークを用い
た研削動作の教示、研削作業条件の設定等の教示が行わ
れる。本実施例では、操作器１１を介して、後述するよ
うな研削対象であるワークの実際の設置位置の検出作業
が行われる。なお操作器１１は、必要に応じて、研削作
業を実行させるオペレータが所持する携帯用のものとし
て構成できるし、また固定設置用のものとして構成する
こともできる。

【００２０】また１２は非接触式の距離センサであり、
力センサ５の先側部分に取り付けられる。この非接触式
の距離センサ１２には、安価なものが使用され、例えば
光学式変位計が使用される。距離センサ１２の検出部
は、研削時に発生する火花の影響を受けにくい、研削工
具７の研削位置から離れた箇所に配置され、研削工具７
の刃先を含む領域に臨んでいる。距離センサ１２によっ
て検出された、研削工具７からワーク６までの距離に関
するデータは、信号ライン１３を介してコントローラ８
に送給される。距離センサ１２の距離検出の原理につい
ては、前述の光学式のものの他に、例えば、磁気インピ
ーダンスの変化を利用するもの、発振周波数の変化を利
用するもの、超音波式のものなどが利用され、研削時に
発生する火花の影響を受けない離れた位置から距離を測
定することができる各種のものを用いられる。

【００２１】次に、図２を参照してコントローラ８で実
現される制御手段の詳細な構成について説明する。研削
作業を行うように設定されたロボット本体１に対して、
当該研削作業の内容を決定する制御内容は、コントロー
ラ８の内部のメモリに格納されたプログラムによってソ
フト的に実現される。図２は、その制御機能要素および
接続関係を示すブロック図であり、制御手段の構成を概
念的に示したものである。なお、ハード回路を利用して
制御手段を構成することができるのは勿論である。

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

【００２３】基本構成である位置・力制御演算系の構成
は、次の通りである。位置・力制御演算系は仮想コンプ
ライアンス制御を実行する制御系の構成を有する。ロボ
ット本体１のアームリスト部に取り付けられた力センサ
５で検出される力信号１０は、力演算部２２に送られ
る。力演算部２２は、入力した力信号をセンサ座標系か
らベース座標系に変換しかつ力信号から研削工具７の重
力分を差し引くことにより、重力補償を行う機能を有す
る。こうして力演算部２２でワーク６と研削工具７の接
触点部分で発生する力〈ｆ〉が算出される。ここで記号
「〈ｆ〉」はｆがベクトル量であることを意味するもの
とする。以下において、記号「〈〉」の意味は同じで
ある。

【００２４】力演算部２２で得られた力〈ｆ〉は減算器
２３に入力され、この減算器２３で力〈ｆ〉と力目標値
設定部２４に設定された力目標値〈ｆｒ〉との間の減算
が行われる。減算器２３の出力は、力〈ｆ′〉として位
置・力制御演算部２５に送られる。上記の力目標値〈ｆ
ｒ〉は、一般的に研削工具７の押付け方向の力の目標値
を与えるための指令値である。また研削工具７が、非接
触状態でかつ力制御モードで制御されて移動するとき、
力目標値〈ｆｒ〉は、研削工具７をワーク６に接近させ
接触させるための速度を決定する指令値となる。

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

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

【００２７】上記のごとく、減算器２３で得られた力
〈ｆ′〉と位置偏差演算部２８で得られた位置偏差〈Δ
ｘ〉は、位置・力制御演算部２５に入力される。

【００２８】位置・力制御演算部２５は、減算器３０
と、バネ定数演算部３１と、特性補償演算部３２を有す
る。減算器２３から出力された力〈ｆ′〉は、減算器３
０に入力される。位置偏差演算部２８から出力された位
置偏差〈Δｘ〉は、バネ定数演算部３１に入力される。
バネ定数演算部３１には仮想バネ定数行列Ｋが設定され
る。バネ定数演算部３１は、入力された位置偏差〈Δ
ｘ〉に対し、設定された仮想バネ定数行列Ｋを乗算する
ことによりＫ〈Δｘ〉を算出する。バネ定数演算部３１
で算出されたＫ〈Δｘ〉は減算器３０に対して出力され
る。減算器３０では〈ｆ′〉−Ｋ〈Δｘ〉が演算され
る。減算器３０で求められた演算結果は特性補償演算部
３２に与えられる。

【００２９】特性補償演算部３２にはコントローラゲイ
ンＫｃが設定される。特性補償演算部３２に入力された
前述の演算値は、ここで制御上の特性補償を受け、それ
により速度指令値〈ｖ〉が算出される。位置・力制御演
算部２５で最終的に求められた速度指令値〈ｖ〉は駆動
指令部３３に供給され、この駆動指令部３３において速
度指令値〈ｖ〉は、ロボット本体１の各駆動モータの駆
動指令値〈θ〉（角加速度である）に変換される。得ら
れた駆動指令値〈θ〉は、ロボット本体１の各駆動モー
タに供給され、各駆動モータの動作に基づきロボット本
体１は所望の位置および姿勢で動作する。

【００３０】上記においてワークの実際の設置位置を接
触式にて検出する場合の動作で、研削工具７をワーク６
に向かって接近させる方向については、仮想バネ定数Ｋ
を０として位置のフィードバックをなくすようにする。
これにより、研削工具７をワーク６に接近させる方向に
ついては、位置・力制御演算部２５に入力されるのは力
成分〈ｆ′〉のみとなり、速度指令値〈ｖ〉は力
〈ｆ′〉に比例する関係を有し、力制御モードで研削工
具７の移動が行われる。研削工具７の移動速度は、力目
標値と粘性係数に基づいて決定される。

【００３１】前述の仮想コンプライアンス制御を実現す
る位置・力制御の基本構成に対して、さらに、本発明の
特徴部である制御定数設定部４０が付設される。この制
御定数設定部４０は、研削工具７がワーク６に向かって
移動しワーク６に接触する過程において、非接触式の距
離センサ１２による検出情報（研削工具７の先端部から
ワーク６までの距離）に基づいて、位置制御モードでの
移動後における研削工具７のワーク接触に至るまでの速
度パターンを演算する速度演算部４１と、この速度パタ
ーンを研削工具７の移動速度を決定する力目標値に変換
する速度パラメータ変換部４２と、この速度パラメータ
を記憶する速度パラメータ記憶部４３とで構成される。
速度パラメータ変換部４２と速度パラメータ記憶部４３
のそれぞれは、力目標設定部２４に接続され得る状態に
構成される。

【００３２】４５は接触判定部、４６は位置取込み部で
ある。接触判定部４５には、接続端子（１）を介した接
続関係に基づき力演算部２２の出力が入力される。接触
判定部４５で、入力された力の値と力目標値とが比較さ
れ、両者が等しくなったときには接触検出信号が位置取
込み部４６に送られる。位置取込み部４６は接触検出信
号を入力すると、このときの研削工具７の接触箇所に関
する位置データを位置演算部２７から取り込む。これに
よって、ワーク６における定められた接触箇所の位置を
検出することができる。

【００３３】次に、上記構成を有する制御手段によって
実行される研削ロボットの特徴的な動作について説明す
る。

【００３４】特徴的動作は、ティーチングプレイバック
方式の研削ロボットに対し要求される基本的動作であ
り、１つまたはそれ以上の数のワークに対してその被加
工部を研削する研削作業を実行することにおいて、研削
作業の開始前に基準ワークについて被加工部の存在箇所
に関する位置データを教示作業でコントローラ８のメモ
リに格納した状態において、実際の研削作業で実際のワ
ークが与えられたとき、その実際の設置位置を検出し、
基準ワークの設置位置とのずれを求める動作である。そ
して特徴的動作の要部は、研削工具７を実際のワーク６
の所定の箇所に接近させ、そして研削工具７の刃先を当
該所定の箇所に接触させることによって実際のワークの
設置位置を検出する部分にある。さらに詳しくは、最
初、ロボット本体１の動作を位置制御モードで制御し研
削工具７をワーク６に位置制御モードで接近移動させ、
ワーク６の近傍の所定位置で力制御モードに切換え、非
接触式の距離センサ１２でその所定位置からワーク６ま
での距離を測定し、その距離情報に基づいて、研削工具
７の刃先がワーク６の所定箇所に接触するまでの速度を
特定の速度パターンとして算出し、算出した速度を力目
標値や粘性係数等の速度パラメータに変換する動作であ
る。コントローラ８は、新たに得られた速度パタメータ
によって、ロボット本体１およびアーム２の動作を力制
御モードにて制御し、この力制御モードで研削工具７に
接近移動および接触動作を行わせる。

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

【００３６】図３は、接触の対象である実際のワークを
示す。このワーク６Ａの上面に被加工部４７が形成され
ており、この被加工部４７を研削工具７によって研削す
る作業が行われる。まず、実際のワーク６Ａの設置位置
が検出される。ワーク６Ａの設置位置を検出するには、
基準面４８の上の一点４９に研削工具７を接触させ、点
４９の位置を検出することによりワーク６Ａの実際の設
置位置を決定する。なお教示作業では、基準ワークの基
準面における同一点についての位置データに基づいて基
準ワークの設置位置が定められ、当該設置位置に基づき
被加工部を研削するための位置データが教示されてい
る。しかし、実際のワークと基準ワークの各設置位置は
完全に同じとは限らないので、実際のワークの設置位置
を検出し、基準ワークの設置位置とのずれを求め、教示
された位置データを修正する必要が生じる。

【００３７】図３では、実際のワーク６Ａの設置位置を
検出するために、研削工具７をワーク６Ａに向けて移動
させる経路を示している。点ａに至る区間Ａでは、衝突
が起きない空間における移動であるので、研削工具７は
位置・力制御装置による位置制御モードで移動する。研
削工具７が点ａの位置に到達すると、一度停止する。そ
こで、位置・力制御装置を位置制御モードから力制御モ
ードに切換え、非接触式距離センサ１２で基準となる検
出点４９までの距離を測定し、測定した距離に基づいて
作成された制御情報によって研削工具７は移動を開始す
る。研削工具７は、点ａからの移動において最初は高速
移動し、その後検出点４９まで低速移動するという速度
パターンに従って移動し、検出点４９に力制御モードに
て接触する。ワーク６Ａの設置誤差により検出点４９の
位置は教示位置より変化しているが、点ａにおいて非接
触式距離センサ１２で点４９までの距離を測定し、点ｂ
から基準点４９までの低速移動距離を一定に保つように
研削工具７を移動させるので、高速移動中において研削
工具７がワーク６Ａに衝突する等の誤動作が生じること
がなく、安全かつ高速にワーク６Ａに接触させることが
可能になる。

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

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

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

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

【００４２】ステップＳ３では、ロボット本体１の動作
が停止した状態で、ワーク６Ａの位置検出点４９までの
距離（Ｘｄ）を光学式変位計１２で測定する。次に、検
出された距離Ｘｄに基づいて、ワーク６Ａに接触するま
での全区間における移動に関して、高速移動距離Ｘ_H を
算出した後、加速時間、一定速度時間、および減速時間
が算出される（ステップＳ４）。具体的には、距離Ｘｄ
から距離Ｘ_L を差し引き、得られた量を高速移動の距離
Ｘ_H とする。力制御モードにおける高速移動時の一定速
度をＶ_H 、力制御モードにおける加速度をα_A ，α_D と
すると、高速移動距離Ｘ_H は、次式で与えられる。ただ
し０＜ｔ＜ｔ₁ を加速時間、ｔ₁ ＜ｔ＜ｔ₂ を一定速度
時間、ｔ₂ ＜ｔ＜ｔ₃ を減速時間とする。この速度に関
する条件で作られる速度パターンを図５に示す。図５に
おいて、Ｂ１は加速区間、Ｂ２は一定高速度区間、Ｂ３
は減速区間、Ｂ４は一定低速度区間である。

【００４３】

【数１】Ｘ_H ＝1/2・α_A・ｔ₁ ²＋Ｖ_H・（ｔ₂−ｔ₁）＋1/2・｜α_D｜・（ｔ₃−ｔ₂）² ＋Ｖ_L（ｔ₃−ｔ₂） …(1) ここで、ｔ₁＝Ｖ_H／α_A …(2) ｔ₃＝（Ｖ_H−Ｖ_L）／｜α_D｜＋ｔ₂ …(3)

【００４４】式（２）と式（３）を式（１）に代入し、
一定終了時間ｔ₂ を求めると、次式が得られる。

【００４５】

【数２】ｔ₂ ＝｛Ｘ_H＋Ｖ_H ²／２α_A−（Ｖ_H−Ｖ_L）² ／２｜α_D｜ −Ｖ_L（Ｖ_H−Ｖ_L）／｜α_D｜｝／Ｖ_H …(4)

【００４６】そして次のステップＳ５で、コントローラ
８内におけるロボット本体１に関する制御状態を、位置
制御モードから力制御モードに切り換える。すなわち、
前述したバネ定数演算部３１の仮想バネ定数を０にセッ
トし、不感帯処理部２６の不感帯幅を狭くする。ステッ
プＳ６では、ロボット本体１の動作に関し、力制御モー
ドによる移動動作が開始される。このとき、速度パラメ
ータ変換部４２内で加速区間Ｂ１における力目標値Ｆｒ
が、次式のように設定される。

【００４７】

【数３】Ｆｒ＝−ＣＶ（ｔ） …(5) （Ｃは特性補償演算部３２で設定された速度ゲイン）Ｖ（ｔ）＝α_A ｔ …(6)

【００４８】式（６）を式（５）に代入し、離散時間制
御式に変換すると、次式のようになり、力目標値Ｆｒn
が求まる。

【００４９】

【数４】Ｆｒn ＝−Ｃ（Ｖn-1 ＋α_A Δｔ） …(7) （Δｔはサンプリングタイム）

【００５０】ステップＳ７においては、サンプリングタ
イム毎に力目標値Ｆｒが力目標値設定部２４に出力さ
れ、研削工具７は、ワーク６Ａの基準面における検出点
４９に向かって移動を行う。移動開始と同時にディジタ
ルカウンタによって経過時間を計測し、ｔ＝ｔ₁ に到達
したとき一定高速度区間Ｂ２に切り換わり、Ｆｒn ＝
−ＣＶＨとなるように力目標値が算出され、力目標値
設定部２４に出力され設定される（ステップＳ８，Ｓ
９）。

【００５１】次に、ｔ＝ｔ₂ で、Ｆｒn ＝−Ｃ（Ｖn-1
−α_D Δｔ）のように力目標値が算出され、力目標値設
定部２４に設定される（ステップＳ１０，Ｓ１１）。

【００５２】ｔ＝ｔ₃ の後は力目標値設定部２４に設定
される力目標値がＦｒn ＝−ＣＶＬのように切り換わ
り、一定低速度区間Ｂ４で研削工具７は一定低速度Ｖ_L
（すなわち接触速度）の移動状態でワーク６Ａに向かっ
て移動する（ステップＳ１２，Ｓ１３）。

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

【００５４】ロボット本体１のアーム先部の移動におい
て、前述の加速や減速を必要としない場合もあり得る。
このような場合には、図５に示した速度パターンにおい
て加速区間Ｂ１や減速区間Ｂ３を特に設ける必要がな
い。従って、図６に示すようなフローチャートになる。
図６のフローチャートにおいて、図４に示したフローチ
ャートと同一のステップについては同一のステップ番号
を付し、その説明を省略する。図６のフローチャートの
特徴的な部分は、ロボット本体１の動作が停止している
間に、一定高速度Ｖ_H の移動時間内の力目標値と一定低
速度Ｖ_L の移動時間内の力目標値を計算し、速度パラメ
ータ記憶部４３に記憶することである（ステップＳ２
１）。そしてステップＳ２２，Ｓ２３，Ｓ２４によっ
て、一定高速度および一定低速度の移動が行われる。

【００５５】上記の実施例では、位置および力の制御に
おいて位置制御モードと力制御モードとの切換え点
（ａ）でロボットの動作を停止させ、検出点４９までの
距離を測定するように構成したが、図３に示すワーク６
Ａに対してかなり近づいた点ｂで再度ロボットを停止さ
せ、検出点４９までの距離を光学式変位計１２で測定
し、速度パターンを作成し、この速度パターンに基づく
前述の接触動作を行わせるようにすることもできる。ま
た点ｂで、接触後の力目標値パターンを作成することも
できる。

【００５６】上記実施例では、力目標値設定部２４に制
御定数設定部４０の出力を入力するようにしたが、速度
パラメータ変換部２４において目標速度を仮想粘性係数
に変換し、このデータを特性補償部３２に設定するよう
に構成することもできる。ただしこのときの力目標値
は、研削時の押付け力の目標値を設定する。

【００５７】

【発明の効果】以上の説明で明らかなよう本発明によれ
ば、研削ロボット等の力制御ロボットにおいて本来の作
業を行う前に研削工具等の手先効果器を実際の対象物に
接触させて実際の設置位置を検出し、検出された位置デ
ータに基づいて教示された内容を補正する場合の位置検
出作業で、力制御モードでの手先効果器の移動の動作
で、対象物に接触する前に手先効果器と対象物との距離
を光学式変位計等の接触式距離センサで検出し、当該距
離に基づいて初期の高速度移動区間と接触直前の低速度
移動区間からなる速度パターンを作成し、この速度パタ
ーンに従って移動を行うように構成したため、距離セン
サの測定誤差の影響を受けず、高速の移動を行うことが
でき、さらに衝突を避け、安全かつ確実な接触を行うこ
とができ、正確な位置検出を行うことができる。

【図面の簡単な説明】

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

【図２】コントローラに設定される制御手段の詳細な構
成を示すブロック図である。

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

【図４】第１の動作例を示すフローチャートである。

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

【図６】第２の動作例を示すフローチャートである。

【符号の説明】

１ロボット本体２アーム５力センサ６ワーク６Ａ研削対象である実際のワーク７研削工具８コントローラ１２非接触式距離センサ２２力演算部２４力目標値設定部２５位置・力制御演算部２６不感帯処理部２７位置演算部２９位置目標値設定部４０制御定数設定部４１速度演算部４２速度パラメータ変換部４３速度パラメータ記憶部

Claims

【特許請求の範囲】

【請求項１】手先効果器に加わる力を検出する力検出
手段と、前記手先効果器の位置を検出するための位置検
出手段と、前記力検出手段で得られる力データと前記位
置検出手段で得られる位置データを入力し、前記各デー
タを用いて制御演算式に基づき制御指令のデータを作成
し、前記手先効果器の位置と力を制御する制御手段を備
える力制御ロボットであり、前記制御手段を力制御モー
ドに設定し、力制御モードでの前記手先効果器の移動速
度を力目標値、位置目標値、仮想粘性係数、仮想バネ定
数に基づき算出し、前記手先効果器を力制御モードで移
動させると共に、前記手先効果器が対象物に接触した
時、前記位置検出手段で検出した位置データを前記対象
物の位置として取り出す力制御ロボットの接触式位置検
出装置において、前記手先効果器の近くに設けられ、前記手先効果器と前
記対象物の間の距離を測定するための非接触式距離セン
サと、前記手先効果器が前記対象物の近くにおいて前記
対象物に接触すべく前記対象物に向かって前記力制御モ
ードで移動するとき、前記距離センサによる測定で得ら
れた距離に基づいて当該距離区間における初期の高速度
移動区間と対象物直前の低速度移動区間を含む速度パタ
ーンを生成する演算処理手段とを備え、前記制御手段の
移動指令に基づき前記手先効果器は前記速度パターンに
従って前記力制御モードで移動することを特徴とする力
制御ロボットの接触式位置検出装置。
【請求項２】請求項１記載の力制御ロボットの接触式
位置検出装置において、前記低速度移動区間の距離は、
前記手先効果器の慣性による行過ぎ量と前記距離センサ
の測定誤差のうち少なくとも一方を考慮して設定される
ことを特徴とする力制御ロボットの接触式位置検出装
置。
【請求項３】請求項２記載の力制御ロボットの接触式
位置検出装置において、前記高速度移動区間の距離は、
前記距離センサで測定された前記距離と前記低速度移動
区間の距離との差として設定されることを特徴とする力
制御ロボットの接触式位置検出装置。
【請求項４】請求項１記載の力制御ロボットの接触式
位置検出装置において、前記演算処理手段は、前記高速
度移動区間と前記低速度移動区間における速度切換え時
間を前記各区間の距離データを基に算出する時間演算手
段と、前記各区間の移動速度を算出する速度演算手段
と、前記速度データを前記制御手段内の制御用係数に変
換する変換手段と、前記係数を記憶する記憶手段と、前
記係数をサンプリングタイム毎に前記制御手段に入力す
る入力手段とを含むことを特徴とする力制御ロボットの
接触式位置検出装置。
【請求項５】請求項１〜４のいずれか１項に記載の力
制御ロボットの接触式位置検出装置において、前記手先
効果器の先部で前記対象物に接触し、設定された押付け
力を前記力検出手段が検出した時に、前記位置検出手段
で検出される位置データを前記対象物の位置であると判
定する判断手段を備えることを特徴とする力制御ロボッ
トの接触式位置検出装置。
【請求項６】請求項１〜５のいずれか１項に記載の力
制御ロボットの接触式位置検出装置において、前記距離
センサによる前記距離の測定、および力制御モードでの
移動のための設定は、ロボット本体の動作を停止させて
行うこと特徴とする力制御ロボットの接触式位置検出装
置。