JPH09230922A

JPH09230922A - 力制御ロボットの接触検出装置

Info

Publication number: JPH09230922A
Application number: JP3599396A
Authority: JP
Inventors: Yoshinori Furuno; 義紀古野; Tooru Kurenuma; 榑沼　　透
Original assignee: Hitachi Construction Machinery Co Ltd
Current assignee: Hitachi Construction Machinery Co Ltd
Priority date: 1996-02-23
Filing date: 1996-02-23
Publication date: 1997-09-05

Abstract

(57)【要約】【課題】力制御ロボットの手先が高速移動から低速移
動へ切り換えの際の作業対象物との接触誤判断を防止し
て接触検出精度が向上した力制御ロボットの接触検出装
置を実現する。【解決手段】ロボット手先加速時と減速時との振動波
形は略同パターンであることに着目し加速振動波形によ
り減速時付近の速度パターン補正をし減速時のロボット
手先の振動を制限する。速度パターン作成部41はセンサ
12の検出情報により工具7のワーク6迄の速度パターンを
演算し力計測値記憶部43は工具7の加速時の力計算値を
記憶する。速度パターン補正部42の閾値判定部51は記憶
部43の出力の閾値通過を判定する。力計測値取り込み部
52は判定部51からの指令信号により記憶部43から力計測
値を取り込み、補正パターン演算部53は取り込み部52の
出力信号に基づき速度補正パターンを演算し加算器54に
供給して速度パターン指令と加算する。加算器54の出力
はスイッチ44を介し駆動指令部33に供給される。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

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

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

【０００６】

【発明が解決しようとする課題】上記の従来の技術にお
いては、ロボットの手先がワークの付近に接近したとこ
ろで、非接触センサの出力を受け、減速して高速移動か
ら低速移動に切り換えると同時に、制御モードの切り換
えを行っている。そして、ロボットの手先がワークに接
触したことの検出は、ロボットの手先に加わる力を検出
することにより行われる。

【０００７】ところが、高速移動から低速移動への減速
時、ロボット本体や手先効果器の慣性力や弾性による振
動が発生する。この振動は、ロボットの手先に加わる力
となるため、この振動による力を、ロボットの手先とワ
ークとが接触したと、誤って判断してしまう可能性があ
る。ロボットの手先とワークとが接触していないにも拘
らず、接触したと判断されると、ロボットの手先は、ワ
ークとは接触していない状態で停止されてしまうため、
その後の、組み付け作業等の作業を適切に行うことがで
きなくなってしまう。

【０００８】本発明の目的は、力制御ロボットの手先が
高速移動から低速移動へ切り換わる際における作業対象
物との接触誤判断を防止して、作業対象物との接触検出
精度が向上した力制御ロボットの接触検出装置を実現す
ることである。

【０００９】

【課題を解決するための手段】上記目的を達成するた
め、本発明に係る力制御ロボットの接触検出装置は次の
ように構成される。手先効果器に加わる力を検出する力
検出手段と、手先効果器の位置を検出するための位置検
出手段と、力検出手段で得られる力データと位置検出手
段で得られる位置データとを入力し、これら力データ及
び位置データと力目標値及び位置目標値とを用いて制御
演算式に基づき制御指令のデータを作成し、手先効果器
の位置と力を制御する制御手段を備える力制御ロボット
の接触検出装置であり、手先効果器が移動して、対象物
に接触した時、力検出手段で検出した手先効果器の押し
付け方向の力データで対象物との接触を判断する力制御
ロボットの接触検出装置において、手先効果器の近くに
設けられ、手先効果器と対象物との間の距離を測定する
ための非接触式距離センサと、この非接触式距離センサ
の出力信号に基づき、手先効果器が対象物と第１の距離
に移動したとき、手先効果器を停止させ、停止した状態
から加速して、対象物に向かって、第１の移動速度で移
動させ、第１の距離より短い第２の距離まで対象物に接
近したとき、手先効果器を第１の移動速度より低い第２
の移動速度に減速して対象物に向かって移動させる速度
パターンを作成する速度パターン作成部と、手先効果器
が第２の移動速度に減速されるとき、速度振動波形信号
を速度パターンに付加する速度パターン補正部とを備え
る。

【００１０】手先効果器の減速時付近の速度パターンに
振動波形を付加して、速度パターンを補正することによ
り、減速時の手先効果器の振動が相殺され制限される。

【００１１】好ましくは、上記力制御ロボットの接触検
出装置において、手先効果器が停止した状態から加速し
て第１の移動速度で移動されるとき、力検出手段により
検出された手先手先効果器の振動を記憶する振動記憶部
をさらに備え、速度パターン補正部は、振動記憶部に記
憶された振動に基づいて作成した速度振動波形信号を、
手先効果器が第２の移動速度に減速されるとき、速度パ
ターンに付加する。

【００１２】手先効果器の加速時と減速時との振動波形
は略同パターンであることに着目し、加速時の振動波形
により減速時付近の速度パターンが補正され、補正され
た速度パターンにより、減速時のロボット手先の振動が
相殺され、制限される。

【００１３】また、好ましくは、上記力制御ロボットの
接触検出装置において、手先効果器が第２の移動速度に
減速されたときからの手先効果器の振動を力検出手段か
らの出力信号により監視し、検出された振動が所定振動
以下となったときから、手先効果器と対象物との接触判
断を開始させる接触力判定手段をさらに備える。

【００１４】減速時付近の速度パターンが補正され、補
正された速度パターンにより、減速時の手先効果器の振
動が制限されるが、振動が完全には防止されず、振動が
残留する可能性がある。そこで、残留振動が所定振動以
下となってから、手先効果器と対象物との接触検知を開
始すれば、接触誤判断を、より確実に防止することがで
きる。

【００１５】また、好ましくは、上記力制御ロボットの
接触検出装置において、速度パターン補正部は、振動記
憶部に記憶された振動波形の１／２周期分の振動波形デ
ータを速度パターンに付加する。

【００１６】また、好ましくは、上記力制御ロボットの
接触検出装置において、速度パターン補正部により補正
した速度パターンを、力目標値に変換する速度パラメー
タ変換部をさらに備える。

【００１７】

【発明の実施の形態】以下、本発明の実施形態を添付図
面に基づいて説明する。なお、以下に説明する本発明の
実施形態は、研削ロボットの接触検出装置に適用した場
合の例である。図１は、力制御ロボットの一例である研
削ロボットのシステム構成を示す。ここで、力制御ロボ
ットは、制御部が位置と力の制御を行い、この制御に従
って力を伴う作業をワークに対して行うロボットを意味
する。また、この研削ロボットは、ティーチングプレイ
バック方式で構成される。

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

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

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

【００２１】この操作器１１によって、例えば、研削作
業前における基準ワークを用いた研削動作の教示、研削
作業条件の設定等の教示が行われる。本実施例では、操
作器１１を介して、後述するような研削対象であるワー
クの実際の設置位置を検出するための教示点の設定が行
われる。なお、操作器１１は、必要に応じて、研削作業
を実行させるオペレータが所持する携帯用のものとして
構成できるし、また、固定設置用のものとして構成する
こともできる。

【００２２】また、１２は、非接触式距離センサであ
り、力センサ５の先側部分に取り付けられる。この非接
触式距離センサ１２には、安価な光学式距離センサが使
用される。この光学式距離センサとしては、反射光を光
スポットとして肉眼で直接確認できる形の例えば可視光
レーザを使用したものが適当である。距離センサ１２の
検出部は、研削時に発生する火花の影響を受けにくい、
研削工具７の研削位置から離れた箇所に配置され、研削
工具７の刃先を含む領域に臨んでいる。

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

【００２４】図２は、本発明の第１の実施形態であるロ
ボットの接触検出装置の構成図である。図２において、
研削作業を行うロボット本体１に対し、当該研削作業の
内容を決定する制御内容は、コントローラ８の内部のメ
モリに格納されたプログラムによって実現される。な
お、ハード回路を利用してコントローラを構成できるの
は勿論である。

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

【００２６】先ず、最初に基本構成である位置・力制御
演算系の構成を説明する。位置・力制御演算系は仮想コ
ンプライアンス制御を実行する制御系の構成を有する。
ロボット本体１のアーム２の先端部に取り付けられた力
センサ５で検出される力信号１０は、力演算部２２に供
給される。力演算部２２は、入力した力信号１０をセン
サ座標系から基準座標系（絶対座標系）に変換し、且
つ、力信号から研削工具７の重力分を差し引くことによ
り、重力補償を行う。こうして、力演算部２２で、ワー
ク６と研削工具７との接触点部分で発生する力＜ｆ＞が
算出される。ここで記号＜ｆ＞は、ｆがベクトル量であ
ることを意味する。以下の説明で、記号＜＞の意味は上
述と同様にベクトル量を表すものとする。

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

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

【００２９】上記のごとく、減算器２３で得られた力＜
ｆd＞と位置偏差演算部２８で得られた位置偏差＜Δｘ
＞とは、位置・力制御演算部２５に入力される。位置・
力制御演算部２５は、減算器３０と、バネ定数演算部３
１と、特性補償演算部３２とを有する。減算器２３から
出力された力＜ｆd＞は、減算器３０に入力される。

【００３０】また、位置偏差演算部２８から出力された
位置偏差＜Δｘ＞は、バネ定数演算部３１に入力され
る。バネ定数演算部３１には仮想バネ定数行列Ｋが設定
されている。バネ定数演算部３１は、入力された位置偏
差＜Δｘ＞に対し、設定された仮想バネ定数行列Ｋを乗
算することによりＫ＜Δｘ＞を算出する。バネ定数演算
部３１で算出されたＫ＜Δｘ＞は、減算器３０に出力さ
れる。そして、減算器３０においては、＜ｆd＞−Ｋ＜
Δｘ＞が演算される。減算器３０で求められた演算結果
は特性補償演算部３２に供給される。

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

【００３２】上記の仮想コンプライアンスを実現する位
置・力制御の基本構成に対して、さらに、本発明の特徴
部である速度パターン設定部４０が設けられている。こ
の速度パターン設定部４０は、速度パターーン作成部４
１と、速度パターン補正部４２と、力計測値記憶部４３
とを備えている。

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

【００３４】また、力計測値記憶部４３は、研削工具７
のワーク６への接触に至るまでの移動方向における力計
算値（力演算部２２から出力（１）(力＜ｆ＞を示す)）
を記憶する。また、速度パターン補正部４２は、力計測
値記憶部４３からの力計測データに基づき、速度パター
ンを補正する。この速度パターン補正部４２は、しきい
値判定部５１と、力計測値取り込み部５２と、補正パタ
ーン演算部５３と、加算器５４とを備えている。

【００３５】そして、しきい値判定部５１は、力計測値
記憶部４３からの出力値がしきい値を通過したことを判
定する。また、力計測値取り込み部５２は、しきい値判
定部５１からの指令信号に従って、力計測値記憶部４３
から力計測値を取り込む。

【００３６】また、補正パターン演算部５３は、力計測
値取り込み部５２からの出力信号に基づき、後述する演
算式によって、速度補正パターンを演算する。そして、
演算された速度補正パターンは、加算器５４に供給され
る。この加算器５４には、速度パターン作成部４１から
の速度パターン指令も供給されており、速度補正パター
ンと加算される。加算器５４の出力信号は、速度指令値
切り換えスイッチ４４を介して、駆動指令部３３に供給
される。

【００３７】４５は接触判定部、４６は位置取り込み部
である。接触判定部４５には、力演算部２２からの出力
（１）が供給される。接触判定部４５で、入力された力
の値を力目標値とが比較され、両者が等しくなった時に
は接触検出信号が位置取り込み部４６に供給される。位
置取り込み部４６は、接触検出信号が供給されると、こ
の時の研削工具７の接触箇所に関する位置データを位置
演算部２７から取り込む。これによって、ワーク６にお
ける定められた接触箇所の押し付け方向の位置を検出す
ることができる。また、接触判定部４５は、速度パター
ン指令発生後、一定期間において力演算部２２からの出
力（１）を受け付けない構成となっている。

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

【００３９】そして、この動作の要部は、研削工具７を
実際のワーク６の所定の箇所に接近させ、そして研削工
具７の刃先を当該所定箇所に接触させることによって実
際のワークの設置位置を検出する部分にある。さらに詳
しくは、最初、ロボット本体１の動作を位置制御モード
で制御し研削工具７をワーク６に位置制御モードで接近
移動させ、ワーク６の近傍の所定の位置で力制御モード
に切り換え、非接触式距離センサ１２でその所定位置か
らワーク６までの距離を測定し、その距離情報を基に、
研削工具７の刃先がワーク６の所定箇所に接触するまで
の速度を特定の速度パターンとして算出し、速度指令値
切り換えスイッチ４４を切り換えることによりその速度
パターンで研削工具７を接触にいたるまで移動させる。

【００４０】次に、この接触検出動作における本発明の
一実施形態の機能を図３及び図４を用いて説明する。

【００４１】図３では、実際のワーク６ａの設置位置を
検出するために、研削工具７をワーク６ａに向けて移動
させる経路を示している。点ａ（ワーク６ａから第１の
距離ＸH＋ＸLを有する位置）に至る区間Ａでは、衝突が
起きない空間における移動であるので、研削工具７は位
置・力制御演算部２５による位置制御モードで移動す
る。研削工具７が点ａの位置に到達すると、一度停止す
る。そこで、速度指令値切り換えスイッチ４４が特性補
償演算部３２側から速度パターン補正部４２側に切り換
えられる。

【００４２】そして、非接触式距離センサ１２で、基準
となる検出点４９までの距離を測定し、測定した距離に
基づき図４の（ａ）に示すような、検出点４９に至るま
での移動方向における速度パターンを作成する。その速
度パターンは、最高移動速度をＶH、高速移動距離をＸH
とする高速移動領域Ｂ１（点ａから点ｂに至る領域、た
だし、点ｂは上記第１の距離よりも小である第２の距離
ＸLをワークとの間に有する位置）と、接触速度をＶL、
低速移動距離をＸLとする低速移動領域Ｂ２（点ｂから
ワーク６ａに至る領域）とで構成されている。この速度
パターンに従って、ワーク６Ａの検出点４９の方向に移
動を開始するが、研削工具７やロボット本体１の慣性な
どの影響で、時点ｔ₂における減速時には、図４の
（ｂ）に示すような振動波形Ｖ２で研削工具７等が振動
する。この研削工具７等の振動を力センサ５が検出し、
接触判定部４５が、研削工具７がワークと接触する前に
接触と誤って判断してしまう可能性がある。

【００４３】そこで、点ａから検出点４９までの移動距
離が比較的短く、この移動距離においては、ロボット手
先の位置と姿勢がそれほど変化しないため、時点ｔ₁に
おける加速時の振動波形Ｖ１と、時点ｔ₂における減速
時の振動波形Ｖ２とは、ほぼ、同じパターンであること
に着目し、時点ｔ₁にて発生する振動波形Ｖ１を力計測
値として、力計測値記憶部４３に取り込み、この力計測
値を基に、速度パターン補正部４２で、振動パターンの
逆位相の振動波形Ｖ４（図４の（Ｃ））を加える等の、
速度パターンの補正を行い、駆動指令部３３に出力す
る。ただし、作成した速度パターンの低速移動領域Ｂ２
に入る前と振動が残っている間では、接触判定部４５は
接触の判断を行わないようにする。

【００４４】次に、速度パターンの補正方法を説明す
る。図４の（ａ）に示す速度パターンを駆動指令部３３
に出力する時点ｔ₁から、力計測値記憶部４３は図４の
（ｂ）に示す力波形Ｖ１を記憶する。そして、速度パタ
ーン補正部４２において、時点ｔ₁から一定時間経過し
た時点で、それまで記憶しておいた力計測値にゲイン
（係数）Ｇを掛け合わせる。

【００４５】このゲインＧを掛け合わせた力波形の１／
２周期分だけを図４の（ｃ）に示すように、減速を始め
る時点ｔ₂からロボット本体１の応答遅れを考慮した時
間ΔtOFSだけ手前から足し合わせることにより、速度パ
ターンの補正を行う。移動方向における速度パターンの
速度指令値をＶｒとすると、この速度指令値Ｖｒは、次
式（１）により演算される。Ｖｒ＝ＶＨ＋Ｇ・ｆ −−−（１）この補正された速度パターンで研削工具７は移動し、検
出点４９に接触することに基づいてワーク６Ａの研削工
具７の押し付け方向における実際の設置位置が検出され
た後、速度指令値スイッチ４４は、特性補償演算部３２
側に切り換えられる。

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

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

【００４８】最初の設定状態では、ロボット本体１の動
作を位置制御モードで制御し、研削工具７の先端部を位
置制御モードで点ａまで高速に移動させる（ステップＳ
１）。

【００４９】このとき、ロボット本体１の高速移動状態
における慣性による行き過ぎ量と非接触式距離センサ
（光学式変位計）１２の測定誤差と減速後の残留振動時
間分とを考慮して、低速移動距離（ＸL）と接触速度
（ＶL）とが速度パターン作成部４１に予め設定されて
いる。研削工具７の先端が点ａの位置に近づくと、ロボ
ット本体１の動作を移動から停止に切り換え、研削工具
７の先端を点ａで停止させる（ステップＳ２）。

【００５０】次に、ロボット本体１の動作が停止した状
態で、ワーク６Ａの検出点４９までの距離Ｘｄを距離セ
ンサ１２で測定する（ステップＳ３）。次に、検出され
た移動距離Ｘｄに基づいて、ワーク６Ａに接触するまで
の前区間における移動に関して、速度パターン作成部４
１が、高速移動距離ＸHを算出する。具体的には、距離
Ｘｄから距離ＸLを差引き得られた量を高速移動距離ＸH
とする。図４の（ａ）に示すような高速移動距離ＸHの
区間は、高速移動速度ＶH、接触移動する距離ＸLの区間
は接触速度ＶLからなる速度パターンが、速度パターン
作成部４１で作成される（ステップＳ４）。

【００５１】そして、次のステップＳ５で、速度パター
ン作成部４１は、指令信号を切り換えスイッチ４４に供
給し、このスイッチ４４を特性補償演算部３３側から速
度パターン設定部４０側に切り換える。ステップＳ６で
は、ロボット本体１の動作に関し、作成した速度パター
ンによる移動動作が開始される。つまり、速度パターン
作成部４１からの速度指令が加算器５４及びスイッチ４
４を介して駆動指令部３３に供給される。同時に、加速
時に発生する振動による力検出値の取り込みも力計測値
記憶部４３により開始され、サンプリング毎に取り込ま
れた力データが力計測値記憶部４３に格納される。

【００５２】ステップＳ７では、しきい値判定部５１
は、振動による力の値が検出され始めてから２度目に図
４（ｂ）に示すスレッシュホールドレベルｆｓを通過し
た時点を判定し、力計測値取り込み部５２による力計測
値記憶部４３からの力の取り込みを終了する（振動波形
の１／２周期分の取り込み）。また、この場合、タイマ
などでカウントして振動波形の１／２周期分になるまで
の力を取り込んでもよい。

【００５３】次に、補正パターン演算部５３は、１／２
周期の振動波形にゲインＧを掛けて、上述したＧ・ｆを
算出する。そして、補正パターン演算部５３は、振動波
形の１／２周期とロボット本体１の応答遅れを考慮した
時間ΔｔOFSとを、時点ｔ２から差し引いた時刻から、
上述したＧ・ｆを加算器５４に出力する（ステップＳ
８）。

【００５４】ステップＳ９では、この補正された速度パ
ターンに従って研削工具７が移動し、時点ｔ₂から低速
度ＶLで移動する。そして、振動波形Ｖ１を用いて振動
Ｖ２を補正したが、時点ｔ₂以降において、完全には振
動は補正されるとは限らず、微振動Ｖ３（図４の
（ｄ））が残る可能性があると考えられる。

【００５５】そこで、振動による力検出値がある一定の
値ｆcより小さくなったか否かを力演算部２２からの出
力信号から接触判定部４５で判定し、ｆcより小さくな
った時点で、接触の判定を始める（ステップＳ１０、ス
テップＳ１１）。望ましくは、この一定の値は、接触を
判定するための研削作業時の力目標値と等しいか、小さ
いほうが良い。

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

【００５７】この時の接触を検出するための力目標値
は、研削作業における押し付け方向の力又はそれ以上の
力を設定しておく。

【００５８】以上のように、本発明の第１の実施形態に
よれば、点ａからワーク６ａの移動距離が比較的短く、
この移動距離においては、ロボット手先の位置と姿勢が
それほど変化しないため、ａ点における加速時の振動波
形と、ｂ点における減速時の振動波形とは、ほぼ、同じ
波形パターンであることに着目し、ａ点にて発生する振
動波形を力計測値として、記憶部に取り込み、この記憶
した力計測値を基に、ｂ点付近の速度パターンの補正を
行い、減速時におけるロボット手先の振動を制限してい
る。したがって、ロボット手先の姿勢、形状、手先に取
り付けられる工具の種類等に関係無く、力制御ロボット
の手先が高速移動から低速移動へ切り換わる際における
作業対象物との接触誤判断を防止して、作業対象物との
接触検出精度が向上した力制御ロボットの接触検出装置
を実現することができる。

【００５９】上述した第１の実施形態では、速度パター
ンをステップ状のものにしたが、加速領域と減速領域の
ある速度パターン、つまり、徐々加速する領域と、徐々
に減速する領域を有する速度パターンでもよい。ただ
し、加速領域の加速度と減速領域の減速度との、互いの
絶対値がほぼ同じであることが条件である。

【００６０】また、第１の実施形態では、検出した振動
波形の連続値を用いて、順次、減速時の速度補正を行っ
たが、検出した振動波形のうち、ある時間間隔毎にサプ
リングした値を用いて、一度に減速時における速度パタ
ーンを補正することも可能である。

【００６１】上述した第１の実施形態においては、ａ点
にて速度指令値切り換えスイッチ４４により速度パター
ン設定部４０に切り換えていたが、他の方法により切り
換えることもできる。

【００６２】図６は、本発明の第２の実施形態であるロ
ボットの接触検出装置の構成図であり、上述したよう
に、切り換えスイッチ４４を用いずに、速度パターン設
定部４０に切り換える実施形態である。そして、図２の
例と図６の例との異なるところは、図６の例において
は、速度パラメータ変換部６１を速度パターン設定部４
０中であり、加算器５４と力目標値設定部との間に配置
し、速度指令値切り換えスイッチ４４が除去されている
ところと、新たにスイッチ４７を減算器２３と力演算部
２２との間に配置し、力の不感帯調整部２６を減算器２
３と位置・力制御部２５との間に配置したところであ
る。他の部分は同一であるので説明は省略する。速度パ
ラメータ変換部６１は、速度パターンを力目標値に変換
し、力目標値設定部２４に供給する。

【００６３】図７は、図６の例の動作フローチャートで
あり、このフローチャートを用いて、上述した力目標値
を用いた位置検出動作の詳細を説明する。図７におい
て、ステップＳ１からステップＳ４までの動作は、図５
のフローチャートで説明した位置検出動作と同じであ
る。

【００６４】ステップＳ２０において、位置・力制御モ
ードにおける位置制御モードから力制御モードに切り換
える。つまり、バネ定数演算部３１を零に設定し、不感
帯調整部２６の不感帯を小さくし、同時に振動による力
を検出しないようにスイッチ４７を図示したようにオフ
とする。

【００６５】次に、速度パラメータ変換部６１により、
高速移動速度ＶHを力目標値に変換し、力目標値設定部
２４に出力する（ステップＳ２１）。ステップＳ６以降
の処理は、図５のステップＳ６以降と同一である。ただ
し、補正された速度パターンはすべて、速度パラメータ
変換部６１で力目標値に変換され、力目標値設定部２４
に供給される。

【００６６】つまり、移動方向の速度指令値をＶｒとす
ると、この指令値Ｖｒは次式（２）にて表すことができ
る。Ｖｒ＝Ｋｃ・ｆｒ −−−（２）また、力目標値ｆｒは、次式（３）で表すことができ
る。ｆｒ＝Ｖｒ／Ｋｃ −−−（３）上記式（３）に従って、速度パラメータ変換部６１は、
速度指令値Ｖｒを力目標値ｆｒに変換する。

【００６７】また、速度パラメータ変換部６１は、力目
標値ｆｒだけでなく特性補償演算部３２に設定されてい
るＫｃにも変換することができる。ただし、この時の速
度パラメータ変換部６１の出力は、力目標値設定部２４
では無く、特性補償演算部３２に入力するように切り換
えられる。

【００６８】このとき、（２）式からＫｃは、次式
（４）にて表される。Ｋｃ＝Ｖｒ／ｆｒ −−−（４）ただし、このときの力目標値ｆｒは、一定値である。

【００６９】以上説明した本発明の第２の実施形態にお
いても、第１の実施形態と同様な効果を得ることができ
る。

【００７０】なお、上述した実施形態においては、位置
検出の対象を研削対象物である実際のワークとしたが、
位置検出の対象は、ワークに限定されず、他のものであ
っても構わない。

【００７１】また、力制御モードによる移動は、研削対
象物である実際のワークの正確な位置検出のために行わ
れたが、研削作業を開始するための接近・接触移動にお
いても同様に利用することができる。ただし、接触した
あと研削作業を開始するときには、第２の実施形態にあ
っては、スイッチ４７をオンにする必要がある。また、
第１の実施形態にあっては、速度指令値切り換えスイッ
チ４４を特性補償演算部３２に接続する必要がある。

【００７２】

【発明の効果】本発明は、以上説明したように構成され
ているため、次のような効果がある。手先効果器が対象
物と第１の距離で停止した状態から加速して、第１の移
動速度で移動させ、第２の距離まで対象物に接近したと
き、第２の移動速度に減速して移動させる速度パターン
を作成する速度パターン作成部と、手先効果器が第２の
移動速度に減速されるとき、速度振動波形信号を速度パ
ターンに付加する速度パターン補正部とを備える。した
がって、減速時の手先効果器の振動が相殺され制限さ
れ、力制御ロボットの手先が高速移動から低速移動へ切
り換わる際における作業対象物との接触誤判断を防止し
て、作業対象物との接触検出精度が向上した力制御ロボ
ットの接触検出装置を実現することができる。

【００７３】また、手先効果器が停止した状態から加速
して第１の移動速度で移動されるとき、力検出手段によ
り検出された手先手先効果器の振動を記憶する振動記憶
部をさらに備え、速度パターン補正部は、振動記憶部に
記憶された振動に基づいて作成した速度振動波形信号
を、手先効果器が第２の移動速度に減速されるとき、速
度パターンに付加するように構成される。したがって、
手先効果器の姿勢、形状、手先に取り付けられる工具の
種類等に関係無く、力制御ロボットの手先が高速移動か
ら低速移動へ切り換わる際における作業対象物との接触
誤判断を防止して、作業対象物との接触検出精度が向上
した力制御ロボットの接触検出装置を実現することがで
きる。

【００７４】また、手先効果器が第２の移動速度に減速
されたときからの手先効果器の振動を力検出手段からの
出力信号により監視し、検出された振動が所定振動以下
となったときから、手先効果器と対象物との接触判断を
開始させる接触力判定手段をさらに備えるように構成さ
れ、残留振動が所定振動以下となってから、手先効果器
と対象物との接触検知を開始すれば、接触誤判断を、よ
り確実に防止することができる。

【図面の簡単な説明】

【図１】本発明に係る力制御ロボットのシステム構成を
示す図である。

【図２】本発明の第１の実施形態の構成図である。

【図３】本発明の動作原理を説明するための図である。

【図４】研削工具の移動速度指令値である速度パターン
とその速度パターンを実行したときのロボット手先の振
動波形を示す図である。

【図５】本発明の第１の実施形態の動作フローチャート
である。

【図６】本発明の第１の実施形態の構成図である。

【図７】本発明の第２の実施形態の動作フローチャート
である。

【符号の説明】

１ロボット本体２アーム５力センサ６、６Ａワーク７研削工具８コントローラ１１操作器１２非接触式距離センサ３３駆動指令部４０速度パターン設定部４１速度パターン作成部４２速度パターン補正部４３力計測値記憶部４４速度指令値切り換えスイッチ４５接触判定部４７切り換えスイッチ５１しきい値判定部５２力計測値取り込み部５３補正パターン演算部５４加算器６１速度パラメータ変換部

Claims

【特許請求の範囲】

【請求項１】手先効果器に加わる力を検出する力検出
手段と、手先効果器の位置を検出するための位置検出手
段と、力検出手段で得られる力データと位置検出手段で
得られる位置データとを入力し、これら力データ及び位
置データと力目標値及び位置目標値とを用いて制御演算
式に基づき制御指令のデータを作成し、手先効果器の位
置と力を制御する制御手段を備える力制御ロボットの接
触検出装置であり、手先効果器が移動して、対象物に接
触した時、力検出手段で検出した手先効果器の押し付け
方向の力データで対象物との接触を判断する力制御ロボ
ットの接触検出装置において、手先効果器の近くに設けられ、手先効果器と対象物との
間の距離を測定するための非接触式距離センサと、この非接触式距離センサの出力信号に基づき、手先効果
器が対象物と第１の距離に移動したとき、手先効果器を
停止させ、停止した状態から加速して、対象物に向かっ
て、第１の移動速度で移動させ、第１の距離より短い第
２の距離まで対象物に接近したとき、手先効果器を第１
の移動速度より低い第２の移動速度に減速して対象物に
向かって移動させる速度パターンを作成する速度パター
ン作成部と、手先効果器が第２の移動速度に減速されるとき、速度振
動波形信号を上記速度パターンに付加する速度パターン
補正部と、を備えることを特徴とするロボットの接触検出装置。
【請求項２】請求項１記載の力制御ロボットの接触検
出装置において、手先効果器が停止した状態から加速し
て第１の移動速度で移動されるとき、上記力検出手段に
より検出された手先手先効果器の振動を記憶する振動記
憶部を、さらに備え、速度パターン補正部は、振動記憶
部に記憶された振動に基づいて作成した速度振動波形信
号を、手先効果器が第２の移動速度に減速されるとき、
上記速度パターンに付加することを特徴とする力制御ロ
ボットの接触検出装置。
【請求項３】請求項１又は２記載の力制御ロボットの
接触検出装置において、手先効果器が第２の移動速度に
減速されたときからの手先効果器の振動を力検出手段か
らの出力信号により監視し、検出された振動が所定振動
以下となったときから、手先効果器と対象物との接触判
断を開始させる接触力判定手段を、さらに備えることを
特徴とする力制御ロボットの接触検出装置。
【請求項４】請求項２記載の力制御ロボットの接触検
出装置において、速度パターン補正部は、振動記憶部に
記憶された振動波形の１／２周期分の振動波形データを
速度パターンに付加することを特徴とする力制御ロボッ
トの接触検出装置。
【請求項５】請求項２記載の力制御ロボットの接触検
出装置において、速度パターン補正部により補正した速
度パターンを、上記力目標値に変換する速度パラメータ
変換部を、さらに備えることを特徴とする力制御ロボッ
トの接触検出装置。