JPH01146645A - ならい制御方式 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔概　要〕 本発明は産業用ロボット等を用いて対象物表面に沿って
加工するならい制御方式に関し、対象物表面の異常に対
して容易に対応し得、対象物表面あるいはワークに損傷
を与えることのないならい制御を提供することを目的と
し、本発明によれば、力検知手段により検出された力を
座標変換して得られる検出力と押しつけ力を示す力指令
との偏差に基づいて、ならい方向に垂直な法線方向に速
度指令を出力する力制御手段と、前記力制御手段内に設
けられ、位置制御方向に作用する監視力を演算する手段
と、前記監視力について所定の許容範囲を設定する不感
帯演算部とを備える不感帯演算手段と、現在位置座標と
目標位置との偏差に基づいて、ならい方向に速度指令を
出力する位置制御手段とを備え、前記不感帯演算手段に
おいて前記監視力が前記許容範囲を越えたときに、前記
速度指令に前記不感帯演算手段の出力を加えるように構
成する。

〔産業上の利用分野〕

本発明はならい制御方式に関し、特に産業用ロボットに
おいて、ワークを対象物表面の形状になぞって加工する
必要のある作業に利用するならい制御方式に関する。

〔従来の技術及び発明が解決しようとする問題点〕対象
物表面の加工、例えば、パリ取り、拭き取り等の作業に
ロボットを使用することはすでに知られており、このと
きに用いるならい制御方式もすでに提案されている。

ならい制御の基本的な方法は、第４図に示すようにワー
クを構成するアームＡに回転機構Ｒを介して取り付けら
れたハンドＨと、対象物Ｏとの接触点Ｐにおける拘束方
向（Ｘ方向）を力制御方向とし、他の方向（ＹおよびＺ
方向）を位置制御方向として各々完全に分離して制御す
るものである。

既ち、このならい制御では対象物表面の接触点Ｐにおけ
る法線方向（Ｘ方向）に力指令を与え、対象物の接平面
内の任意の方向に移動指令を与えるようにしている。と
ころで、上述の力および移動指令を正しくハンドＨに与
えるためには対象物の表面形状が正確に数値的に記述さ
れることが必要であり、さらに、表面形状の変化に微妙
に追従可能でかつリアルタイム処理の可能な高精度の視
覚装置が必要である。

一方、上述の如きならい制御方式では、位置制御の方向
は作業時の外乱や位置決め上の誤差あるいは対象物表面
の異常形状によって非常に影響を受けやすいものである
ため、例えば移動指令が対象物の接平面外に指向して与
えられることがある。

第５図はこれを説明する図であり、対象物表面が図示の
如く異常に変化している場合に、点Ｐ、からＰｂにハン
ドＡが移動するときに表面にならって正確に追従可能な
制御であれば、ｘ０方向に力指令を与えＹ０方向に移動
指令を与えて正確にならい動作をすることができる。し
かし、このときに点線で示すようにＸ、方向に力指令を
与え、Ｙ、方向に移動指令を与えてしまうとＹ、方向の
力、さらにはＹｂ方向の力によって対象物表面をかじる
ように作用するので表面やワークを損傷することになる
。

この場合に対象物表面を出来るだけ微視的にとらえて数
値化し、Ｘ方向の力指令とＹ方向の移動指令を出来るだ
け微細に制御すればある程度は回避することはできるが
、ある瞬間をとらえると上述と同様の問題を生ずること
になる。第８図は従来の構成例であり、力制御部２１で
は後述する検出力Ｆ、と押しつけ力を示す力指令Ｆ０か
ら所定の演算を経て速度指令■、を出力し、また、位置
制御部２２ではロボット座標系の現在位置Ｘ、と目標位
置Ｘ０とから速度指令Ｖ、を出力し、これらを加算して
速度指令■を出力している。図からも明らかなように、
力制御部においても、位置制御部においてもいかなるフ
ィードバック制御もなされていないことがわかる。この
ために対象物表面の異常に応じて速度指令Ｖに異常が生
じかじり等の損傷が生じていた。これについては後述す
る。

本発明の目的は、対象物表面の異常に対して容易に対応
し、対象物表面あるいはワークに損傷を与えることのな
いならい制御方式を提供することにある。

〔問題点を解決するための手段〕

第１図は本発明の原理構成図である。図において、Ａは
対象物表面の接平面方向の移動指令を制御する位置制御
手段、Ｂは対象物表面の法線方向の力指令を制御する力
制御手段、Ｃは前記力制御手段内に設けられ所定範囲の
力に対して不感帯として作用する不感帯演算手段である
。

〔作　用〕 本発明では力検知手段により検出された力を座標変換し
て得られる検出力と、押しつけ力を示す力指令との偏差
を求め、力制御手段によりならい方向に対して垂直な方
向に速度指令を与え、かつ力制御手段内に備えられる不
感帯演算手段によって位置制御方向に作用する監視力を
演算し、さらにこの監視力が許容範囲内にあるか否かを
判定する。一方、位置制御手段により現在位置座標と目
標位置との偏差を求め、これをならい方向に速度指令と
して出力する。そして不感帯演算手段において、監視力
が所定の許容範囲を越えたときは速度指令に不感帯演算
手段の出力を加えるようにする。

〔実施例〕

第２図は、本発明に係るならい制御方式の一実施例ブロ
ック構成図である。図において、２１ａは力制御部、２
２は位置制御部である。このような構成において、なら
い動作ではならい方向の移動速度指令ｖ０と、押し付は
力を示す力指令Ｆ０とロボット座標系で示した検出力Ｆ
、との偏差にもとづいて力制御部２１ａにて生成された
速度指令■、と、ある時点での目標位置Ｘ０とロボット
ハンドの現在位置Ｘ、との偏差にもとづいて位置制御部
２２にて生成された速度指令Ｖ、と、に基づいてこれら
の和ｖ、＋ｖ、＋ｙ、を速度指令Ｖとして出力する。

この場合、ロボット座標系等の基準座標系にて速度指令
Ｖを生成するためにマニピュレータの各関節の動作速度
々は後述する演算部２ｏにて算出され、マニピュレータ
の逆ヤコビ行列をＪ−１とすると、δ＝Ｊ−１・Ｖと表
わされる。ここで、第７図に示すように、対象物への押
しっけカを示す力指令Ｆ０で与えられる力制御方向の単
位ベクトルを“ｎ”、ならい方向の移動速度指令Ｖ０で
与えられる位置制御方向の単位ベクトル“０”で表わし
たとき、位置制御されるさらにもう１つの直交ベクトル
“ａ″は、 ａ＝ｎｘ。

で与えられる。

これをそれぞれ基準座標系による成分表示すると、 ｎ＝　　（ｎｘ　　　ｎｖ　　　ｎｚ　）”ｏ＝　　（
ｏＸ　　　Ｇｙ　　　Ｏｚ　）ｒとなる。ここで、Ｔは
転置行列を示す。一方、演算部１の座標変換Ｒは、 で与えられる。

そして、演算部２は基準座標系で表示された力の偏差を
座標変換してならい座標系にて表示するもので、第７図
ではならい座標系Ｘ’Ｙ’Ｚ″において、Ｘ′方向を力
制御方向、Ｙ′およびＺ′方向を位置制御方向とするこ
とで、選択マドＩノクス演算部３の選択マトリクスＳｆ
は、 で与えられる。この場合、一般に、例え巳よねじ寒帝め
、缶の蓋締め、等押し付は方向の軸まわりＧこトルクを
与えるのでトルクを与える場合にはｓ＝１とし、与えな
い場合にはｓ＝ｏとする。

カフィードバックゲイン演算部４ではカフィードバック
ゲインＣ１は基準座標系に関して、で与えられる。

また、位置フィードバックゲイン演算部６では、位置フ
ィードバックゲインＣ２は、同様にしてで与えられる。

また、選択マトリクス演算部５の“Ｉ”は単位行列を表
わしている。

以上の各式から、力制御部２１ａがら出力される速度指
令Ｖ、は、式（１）、（２）、（３）および検出力Ｆ、
と力指令Ｆ０との偏差によって、■ｆ　冨Ｃｒ　　ＲＳ
ｔ　　Ｒ丁　　（Ｆｒ−Ｆ、”）　　　・・・　（５）
また、位置制御部２２から出力される速度指令■９は、
式（１）、（２）、（４）および現在位置Ｘ、。

と目標位置Ｘ０との偏差によって、 Ｖ、＝ＣｐＲ（１−３ｆ　”）Ｒ”　　（Ｘｒ−Ｘ、）
・・・（６） で与えられる。

この場合に、本発明では力制御部２１ａにおい、て、位
置制御方向に作用する力も監視するために、選択マトリ
クス演算手段５と不感帯演算手段７を設ける。これらの
手段によって、ならい動作中に位置制御方向が対象物接
触点における接平面に一致していない場合に、対象物表
面に多大な摩擦力が生じて表面をかじり、対象物表面あ
るいはワークに損傷を与えるのを防止することができる
ものである。既ち、対象物の接触点に許容範囲以上の力
が作用したときには位置制御方向にもカフィードバック
を作用させるためである。

第２図において、位置制御方向に作用する力としての監
視力Ｆ　ｅｒは、 ＦＣ，＝　（１−３ｒ　）Ｒ’　　（Ｆｒ−Ｆ、）　　
・　（７）で与えられ、ならい座標系で表示した力の許
容範囲Ｕｔ　　（＞Ｏ）を不感帯中と設定した不感帯演
算手段７において、 ＩＦｃ、ｌ≦Ｕ。

のとき、既ち、監視力が力の許容範囲より小さいときは
、式（５）および（６）を加算した速度指令Ｖが得られ
、 ｌ　Ｆ　ｃｒ　ｌ　＞　Ｕ　ｔ のとき、既ち、監視力が力の許容範囲より大なるときは
、力制御部２１ａの速度指令Ｖｆは、Ｖｔ　＝Ｃｔ　Ｒ
（Ｓｒ　Ｒ”　　（Ｆｏ　　Ｆｒ　）　＋ＦＣ，，，）
　　　　　　　　　　　　　・・・（８）で与えられる
。この場合Ｆ　ｃｒａは、力の許容範囲と監視力との大
小に応じて、 Ｆｃｒｍ　＝Ｆｃｒ　　Ｕｔ　　（Ｆｃｒ＞　０）＝Ｆ
ｃｒ＋Ｕｒ　　（Ｆｃｒ＜　Ｏ） で与えられる。当然、位置制御部２２における速度指令
Ｖｐは式（６）でよい。

第３図は本発明の適用される制御装置の一実施例ブロッ
ク図である。図において、１０はロボットのマニピュレ
ータ、１１はサーボモータ、工２はタコメータ、１３は
カウンタおよびエンコーダ、１４は増幅器、１５はＤ／
Ａコンバータ、１６は補償器、１７は力覚センサ、１８
は座標変換部、１９は位置姿勢演算部、２０は逆ヤコビ
行列演算部である。１１〜１６によって構成される系２
３はマニピュレータ１０の自由度の数だけ備えられる速
度サーボ系である。

マニピュレータＩＯは先端のハンドの一点に作用する力
は、ハンドの位置と、マニピュレータの自然長時の位置
とのオフセットにマニピュレータ対象物系の剛性を掛け
た値として表わされ、この作用力はマニピュレータの手
首部分に装着した力覚センサ１７により検出される。第
６図に示すようなハンド座標系（Ｘｈ　　、　Ｙｈ　　
、　Ｚｈ　）で検出した力Ｆｈは座標変換部１８におい
てロボット基準座標系（Ｘｒ、、Ｙ、　　、Ｚ、、）の
検出力Ｆ、に変換されて力制御部２１ａに入力される。

ハンドの位置姿勢Ｘｒはエンコーダおよびカウンタ１３
により検出された各関節角θ３から、位置姿勢演算部工
９にて算出される。ここでＰｆは力制御パラメータ、Ｐ
ｐは位置制御パラメータである。

前述の力制御部２１ａおよび位置制御部２２から得られ
た速度指令Ｖは逆ヤコビ行列演算部２０において、前述
の式θ＝Ｊ−１・Ｖにもとづき、関節速度θが求められ
、この関節速度は補償器１６に入力され、Ｄ／Ａ変換さ
れた後増幅されてサーボモータを駆動する。

〔発明の効果〕

以上説明したように、本発明によれば、対象物表面の異
常によって、ならい方向に対象物やワークを損傷する恐
れのある異常な力が作用したときに、これを容易に回避
することができる。

【図面の簡単な説明】

第１図は本発明の原理構成図、 第２図は本発明の一実施例構成図、 第３図は本発明を適用した制御装置ブロック図、第４図
は対象物表面の力方向を説明する図、第５図は対象物表
面の異常形状における力方向を説明する図、 第６図はロボット座標系とハンド座標系を説明する図、 第７図はロボット座標系における単位ベクトルを説明す
る図、および 第８図は従来の構成図である。 （符号の説明） １．２・・・演算部、 ３・・・選択マトリクス演算部、 ４・・・カフィードバックゲイン演算部、５・・・選択
マトリクス演算手段、 ６・・・位置フィードバックゲイン演算部、７・・・不
感帯演算手段、　　２１　、２１　ａ・・・力制御部、
２２・・・位置制御部。 、２２３ 本発明を適用した制御装置ブロック図 第３ｙＪ Ｘ 対象物表面の力方向を説明する図 第４１ 人Ｃ 対象物表面の異常形状における力方向を説明する図第５
５Ｊ 入ｒ ロボット座標系とハンド座標系を説明する図＠６図 ｒ ロボット座標系における単位ベクトル全説明する図＄７
図

Claims (1)

1. 【特許請求の範囲】 １、対象物表面に沿って加工するならい制御方式におい
   て、 力検知手段により検出された力を座標変換して得られる
   検出力（Ｆ＿ｒ）と押しつけ力を示す力指令（Ｆ＿ｏ）
   との偏差に基づいて、ならい方向に垂直な法線方向に速
   度指令（Ｖ＿ｆ）を出力する力制御手段（Ｂ）と、 前記力制御手段内に設けられ、位置制御方向に作用する
   監視力（Ｆ＿ｃ＿ｒ）を演算する手段（５）と、前記監
   視力（Ｆ＿ｃ＿ｒ）について所定の許容範囲を設定する
   不感帯演算部（７）とを備える不感帯演算手段（Ｃ）と
   、 現在位置座標（Ｘ＿ｒ）と目標位置（Ｘ＿ｏ）との偏差
   に基づいて、ならい方向に速度指令（Ｖ＿ｐ）を出力す
   る位置制御手段（Ａ）とを備え、 前記不感帯演算手段において前記監視力が前記許容範囲
   を越えたときに、前記速度指令（Ｖ＿ｆ）に前記不感帯
   演算手段の出力を加えるようにしたことを特徴とするな
   らい制御方式。