JPS6235915A - バリ取りロボット用フィードバック制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は、カフィードバックおよび位置フィードバック
を必要とするバリ取り口がット用フィードバック制御方
式に関するものである。

〔発明の背景〕

これまで口がット用のカフィードバック制御方式として
は、ハンス　コリーｙ　（Ｈａｎｇ　Ｃｏ１１＠ｓｎ　
）にヨルギビング　ロがツッ　デノｆワー　ツー　ツー
ｆ（Ｇｌｖｉｎｇ　Ｒｏｂｏｔｓ　　ｔｈｅ　Ｐｏｗ＠
ｒ　ｔｏ　ＣＯＰ＠　）　（Ｓｐｒ１ｍｇ１９８０／Ｒ
ＯＢＯＴＩＣ８ＴＯＤＡＹ（ＩＦＳ社発行））ナル文献
に記載されているものが知られている。これによると力
センサとしては単純なオンオフセンサか、あるいは３〜
４段階のレベル検出センサまたはこれらセンサを組合せ
たものが使用されるようになっている。この力センサよ
シ得うレるカフィード・童ツク信号は一軸方向か、ある
いは多くても−３−−−頁 直交する３軸方向についての逃げ動作や、押付は動作の
制御方式に適用されていたのかせいぜいのところである
。

しかしながら、実際のバリ取りではある一方向には反力
を一定にしつつ他の方向には軌道上を移動させた少、場
合によっては倣うような制御が必要となっている。場合
によっては並進運動に限ることなく回転運動も考慮する
必要があるものとなっている。

〔発明の目的〕

本発明の目的は、力および位置をフィードバックするこ
とによってバリ取りを状態良好にして行ない得るバリ取
シロがット用フィードバック制御方式を供するにある。

〔発明の概要〕 この目的のため本発明は、アーム先端に力・トルクセン
サ、ハンドを介しバリ取り用ツールが取付されている場
合に、ハンドおよびバリ取り用ツールに仮想の質量、バ
ネ定数および粘性定数を運動特性を制御する・母うメー
タとして与え、これら・譬うメータや外部からの基準位
置姿勢指令、ロデットからの力・トルク信号および実際
の位置姿勢にもとづき、現すンノリング制御時での速度
指令値が得られるようにしたものである。

〔発明の実施例〕

以下、本発明を第１図から第８図により説明する０ 先ず本発明に係るバリ取り口がットの機構の概要につい
て説明する。第２図はその機構の概要をバリ取漫される
対象物とともに示したものである。

図示の如く口？ットのアーム１先端には力Ｏトルクセン
サ４を介しハンド２が取付されているが、ハンド２には
まだバリ取り用ツール（一般に回転砥石等を使用）３が
取付されるものとなっている。

ツール３を介し対象物５からの反力を検出しつつツール
３が対象物５に接触したー状態でバリ取りを実行するも
のである。

第３図は本発明に係る力・位置フィード・守ツク制御方
式の基本概念を示しだものである。ハンド２は実際には
既に述べた如く力・トルクセンサを５頁 介しアームに取付されるが、カートルクセンサおよびア
ームはその剛性がともに高いことから、ノ・ンド２ある
いはこれに取付されているバリ取υ用ツールに力あるい
はトルクを外部より加えても、ハンド２は静止したまま
である。このことはこれまでの位置制御の場合と同様で
ある。即ち、指定された軌道上を口？ットのハンドは移
動するが、その際ハンドに外力が作用したとしてもこの
作用自体によってはハンドは動くことはなく、ただ、指
定された軌道上を移動するだけであるというものである
。

しかしながら、バリ取りの実行には位置制御だけでは不
十分である。というのは、指定された軌道上を単に移動
するだけでは不規則に突出しているバリに対処し得ない
からである。よって、位置制御によって予め指定された
軌道（この軌道は予め予測されるバリの存在位置より決
定される）上を移動すると同時に、不測の突起状・奢り
に対してはバリ取り用ツールが適度の押付は力を対象物
に加えつつバリ取りを試みるよう、必要ならばバリ６−
頁 取り用ツールの位置を突起状バリに倣わせながら退避さ
せるといった具合に、高度な制御が必要となる。即ち、
力のフィードバック制御と位置のそれとが並行して行な
われる必要があるものである。

このうち位置制御についてはこれまでも行なわれてお）
、しかもそれで十分であることからこれ以上の説明は要
しない。しかしながら、力のフィードバック制御につい
ては何等かの対策が必要となる。既述のようにバリ取り
では突起状バリなど、不測のバリ形状が出現した場合に
はその形状に・々り取シ用ツールが倣う動作が必要であ
るからである。この倣い動作をソフトウェア的に実現し
ようというのが本発明なわけである。

第２図において述べたようにその剛性故にハンドあるい
はバリ取υ用ツールに力やモーメントを作用させてもそ
れらは動けず、これからすれば剛性は位置制御の精度向
上には有利に働くことになる。しかし、倣い動作には不
利に働くというものである。倣いには第３図に示したよ
うなバネ機構が必要であり、そのような機構によって初
めてノ・−７−１頁 ンドあるいはバリ取り用ツールに力やモーメントを作用
させることによって、それらの倣い動作が可能となるも
のである。しかしながら、このようなバネ機構は第２図
に示す機構では実現し得ないから、本発明はバネ機構を
ソフトウェア的に実現しようというものである。ここで
第３図に示す機構について簡単に説明すれば、ノ・ンド
２はフレームＩＯに並進バネ６や回転バネ８を介し吊り
下げ支持されたものとなっている。三次元空間内での位
置および姿勢が対象とされていることから、独立な並進
バネ、回転バネがそれぞれ３個必要であり、また、安定
化を図るべくそれら・ｆネには対としての並進ダン・４
７、回路ダンノ９９が組み込まれるようになっている。

第４図は位置フィードバック制御およびカフィード・守
ツク制御を行なう制御系の概要を示したものである。図
示の如く制御部１１には基準値（後述）とロピットから
の力および位置についてのフィードバック信号が入力さ
れるものとなっている。力のフィードバック信号は口？
ットのアーム１．７１２２２間に取付された力・トルク
センサ４より、また、位置のフィード・譬ツク信号はロ
ゲットの関節各々に取付されている位置センサ（例えば
ロータリエンコーダ）よ）得られるものとなっている。

制御部１１ではこれらフィードバック信号と基準値とに
もとづき速度指令値を算出するが、この指令値によって
口？ットにおけるサーＩモータ各々が駆動制御されると
ころとなるものである。

この制御部１１の詳細については後述するところである
が、それに先立ってその説明に必要とされる座標系につ
いて説明しておく。

第５図は６自由度関節形ロ？ットにその例を採った場合
での座標系を示したものである。これによるとロゲット
が設置されているベースに固定の座標系はベース（Ｂ）
座標系とされ、座標はその直交する３軸ｘ、、ｙｌ、ｚ
、で以て表されるものとなっている。また、ハンＰに固
定の座標系はハンド（６）座標系と称され、その座標は
直交３軸ｘＨ、Ｙ、、　Ｚｎで以て表されるようになっ
ている。このＨ座標系はその原点が主にハンド上でのあ
る点に定められ、９頁 ハンドの位置、姿勢が直感的に知れるようになっている
。しかし、ハンドはツール類を把持する場合が多くツー
ル上のある点近辺で口ｌットとしての重要な作業を行な
うことが多いことかＬ、口？クトが実際に作用を行なう
際での制御の便を考慮し補助（Ａ）座標系がＨ座標系に
対する相対的位置関係固定にして定められるようになっ
ている。Ａ座標系は場合によってはＨ座標系に同一であ
ってもよいが、Ｘム、Ｙム、ｚＡはその直交３軸となっ
ている。更に第３図に示すバネ機構において釣り合い平
衡状態での位置、姿勢が直感的に知れるように基準（８
）座標系が定められ、座標は直交３軸Ｘｒ、Ｙｒ。

ｚｒで以て表されるものとなっている。第３図に示すバ
ネ機構においてその平衡点にハンドが存在しない場合は
Ａ座標系がＲ座標系と異なっており、その平衡点に存在
する場合にはＡ座標系はＲ座標系に一致するようになっ
ている。なお、ｉ、はＢ座標系より見たＲ座標系の原点
の位置ベクトルを、また、ｆｒ＊ｇｒ＊’ｌｒはＸｒ、
　Ｙｒ、　Ｚｒの軸方向を表す単位ベクトルである。

ｌＯ−頁 次に上記座標系の表示方法について説明すれば、Ｂ座標
系より見たＨ座標系の原点の位置ベクトルをｉ、その直
交３軸ＸＨ、ｙＨ，ＺＨ各々の軸方向を表す単位ベクト
ルをＴｅ’ｌｌｅ”とすれば、Ｐ−Ｔｅｇｌｈはともに
３次元ベクトルであって、これらはロデットの関節各々
の角度θ０．θ８．・・・、θ６と関節間長さより定ま
るものとなっている。

また、Ｂ座標系より見たＡ座標系の原点の位置ベクトル
を１１、Ｘム、Ｙム、２ムの軸方向を表す単位ベクトル
をｔ＠ａｌｌ＠ａｈ＠とした場合、これら４クトルは３
次元ベクトルであって、式（１）より求められるものと
なっている。

・・・（１１ 但し、ｐ、、Ｈ座標系より見たＡ座標系原点の位置ベク
トル Ｔａ、ｇ、、村：Ｈ座標系よシ見たＸ、、　Ｙ、。

Ｚａの各軸方向を表す単位ベクトル １１−頁 である。

次に口♂ットによる作業を記述すべく適当な位置、姿勢
にＡ座標系を定めるが、本例では第６図。

第７図に示すようにバリ取シ作業を行なう関係上回転ツ
ール３の中心軸上であって、しかも切削点に近い点にＡ
座標系原点を設定し、まだ、軸Ｘム。

ＹＡｌｚムが適切に定められるようになっている。

さて、この人座標系により第３図に示すバネ機構の運動
を記述すれば以下のようになる。

但し、Ｔ、；ノ・ンドに加わる外力（並進力および回転
力）（６軸センサによって検出） ・・・６次元ベクトル Δ讐、；Ｒ座標系からのＡ座標系の偏差（位置および姿
勢の偏差）・・・６次元ベクトル ■、；ツールの速度（並進および回転の速度）・・・６
次元ベクトル Ｃｙａ）：仮想質量・・・６行６列の行列（Ｋａ）：仮
想バネ定数・・・６行６列の行列［：Ｃａ］：仮想粘性
定数用６行６列の行列である。（Ｍａ）　、　（：Ｋａ
）および〔Ｃ１〕はロゲットがもつ特性値ではなく、実
行しようとする運動特性よシ算出されるソフトウェア上
でのノｔラメータである。

ところで式（２）は式（３）に、更に式（３）は離散値
系表示によって式（４）のように変形され得る。

Ｖａ＝［：Ｍｇ３−’ｆ（Ｔａ　（Ｋａ〕Δｘａ　（Ｃ
ａ）Ｖａ）ｄｔ　”（３）ｖａ、ｔｔ−ｄｔ　（Ｍ、）
−’　（ｆ、、　ｎ−１−（ＫＪΔｌ、ｎ−１）＋（（
Ｉ）　ｊｔ（Ｍａ）”　（Ｃａ）　）　Ｖａ、　＊−ｓ
　　　”（４）但し、Δｔ；サンプリング周期 ＣＩ〕；単位行列 であシ、サフィックス−はサフィックストｌよりもΔｔ
ｅｎ後の値を示している。

この式（４）において右辺における各項の値は以下のよ
うに定められるようになっている。

即ち、ｄｔは計算所要時間などから適切に、また、（Ｍ
ｇ３．　［Ｋａ）および〔Ｃ１〕は運動特性よシ定めら
れるものとなっている。更にＴ１．ニー、についてはｌ
サン−１３−頁 ！リング前に力・トルクセンサよシ得られた信号がｖａ
、ｎ−１については１サンプリング前の速度指令値がそ
のまま、Δｉ＆、ｎ−１につ込ては同様に１サンプリン
グ前のΔｉ、がそのまま用いられ乙ようになっている。

ここでΔ気について説明すれば、これは式（５）より求
められるものとなっている。

・・・（５） 但し、Ｅ、θは以下のようである。

式（５）〜（７）よ）ΔＸ＆、１１−１が求められるわ
けであるが、結局式（４）の左辺におけるロゲットハン
ド（より正確にはそのハンドが把持するツール）の速度
−−−−一−−−！４−頁 指令値Ｖ、　ｎは、予め定められている定数と力および
位置のフィードバック信号とによって定まるものである
。このようにして求められた速度指令値ｖａ、　Ｉｌｌ
　は式（８）によってＢ座標系での速度指令値へに変換
された後、更に式（９）によって各関節角度θ（＝〔θ
３．θ３．・・・、Ｃ６〕）に変換され、これにより各
モータが駆動制御されるところとなるもので・・・（８
） τ θ＝　（Ｊ　］−１丸　　　　　　　　　　　　・・・
（９）但し、〔Ｊ〕；ヤコビアン行列 である。

第１図は本発明に係る制御部１１における演算制御の！
ロック構成をロゲットとともに示したものである。図中
ｉは力Ｏトルクセンサ２０　（４）からの歪−−１５−
頁 ｒ−ノミ圧を示しているが、これからも判るようにｃＭ
ａ〕、　［ａｌおよび〔Ｃ１〕と、ｐｍ　＋　ｆａ　ｇ
＊およびり。

は原則として定数として与えられ、また、Ｉ’ｒ＋’ｒ
＋ｆｒおよびに、は基準値として外部よシ与えられるも
のとなっている。この基準値は対象物の設計データおよ
び口♂ットとの相対的位置関数が知れていれば予め得て
おくことが可能であり、離散的、且つ更新可なものとし
て演算制御上のサンプリング周期毎に外部より与えられ
るものとなっている。

さて、次に以上述べたフィードバック制御方式をバリ取
り作業に実際に適用する場合について説明すれば、便宜
上Ｈ座標系はツールを交換しても、また、ツールが取付
されていなくても変化することがない点を原点として定
められるようになっている。Ａ座標系についてはツール
が対象物と接して切削を行なう点を考慮し第６図、第７
図に示すようにその原点と軸Ｘム、Ｙム　ｙ、方向が定
められるものとなっている。Ｒ座標系についても第６図
。

第７図に示すようにその原点と軸Ｘｒ　、　Ｙｒ　、　
Ｚｒ方向が定められるが、これによシ軸Ｘム、Ｘｒは同
一直線上に位置し、しかもその直線は第６図に示すよう
にＰ０→Ｐ、→Ｐ、→・・・・・・→Ｐ、→Ｐ０で示さ
れるバリ取り軌道と交わることになるものである。

この後はソフトウェア上の制御、ｆラメータであるとこ
ろの（Ｍａ）、　ＣＫ＆〕および〔Ｃ１〕が定められる
が、このうち〔Ｋ１〕は点Ｐ、　、　Ｐｒ間距離ととも
にツールの対象物への押付力に関し重要な７アクタとな
っている。［ａ　］が大きい程に、また、ツールの対象
物への接触点ｐ、とＲ座標系の原点Ｐ１との間の距離が
大である程に押付力が大となるからである。この状態で
ツールを回転させればＡ座標系はＲ座標系に近づこうと
することから、ツールはある押付力を以て対象物に接触
した状態でバリ取りを行なうことになるものである。し
かしながら、この段階ではツールをＰ、方向へ移動させ
ることは不可能である。ツールの移動はＲ座標系の原点
とその姿勢を順次更新されるものとして適当に設定する
ことによって、初めて可能になるというものである。

Ｒ座標系を適当に設定した場合は、ツールは対象物形状
に倣いつつ、しかもある押付力を以て対象−１７−頁 物上を移動し得るものである。例えば、対象物として第
８図に示すものを想定した場合、バリはＰＳＩ→Ｐ１１
−＋ｐ、、→Ｐ１４→ｐＨの曲線軌道上に存在するが、
これに対しＲ座標系をＱｌｌ→Ｑｌｔ→Ｑ１．→ＱＩ４
→Ｑ１１の曲線軌道上でその原点と姿勢を変化させて移
動してやれば、ツールは常時はぼ第７図に示す如くの姿
勢で対象物に接触したままｐＨ→ＰＩｌ−＋Ｐｉ１→Ｐ
Ｉ４→ｐＨといった具合に倣い移動することによって、
・守り取りを行ない得るものである。この場合倣い軌道
上に突起物が存在したとしても、ツールはその突起物を
避けつつも押付力大にして突起物形状を倣い得るもので
ある。なお、ツールが倣い移動している間ハンドはツー
ル中心軸回シに回転されなく、ハンドよりすれば単にツ
ールを前後、左右に振るだけである。また、Ｒ座標系の
移動は基本的には以上のようにして行なわれるが、場合
によってはその移動速度は適当に加減される必要がある
。直線部を倣っている間には速度大にして、コーナ部で
は速度小にして倣うようにするものである。更に必要に
応じてＲ座標系の原点の移動軌−−−１へ一−頁 跡に正弦波等の波状形状を与えたり、ツール移動中に〔
Ｋ、〕の値を変更することも可となっている。

更にまた〔Ｍ、〕や〔Ｃ１〕の値を変更することによっ
ては、応答性やダインをある範囲内で変化させることも
可となっている。

以上本発明を説明したが、本発明はバリ取シに限定され
ることなく場合によっては磨きや倣い、嵌め合いなど、
相手よシカを受ける作業にも広く適用し得る。しかも、
本発明はソフトウェア上の制御・ぐラメータを変更する
だけで各作業に対処し得ることになる。

〔発明の効果〕

以上説明したように本発明による場合は、ツールを対象
物に対して状態良好にして倣い移動させ得、たとえ突起
物が存在したとしても状態良好にしてバリ取りを行ない
得るという効果がある。

【図面の簡単な説明】

第１図は、本発明に係る演算制御の！ロック構成を示す
図、第２図は、本発明に係る・量り取り口♂ットの機構
の概要を示す図、第３図は、本発明＃［ による制御方式の基本概念を機械的に示す図、第４図は
、本発明に係る制御系の概要を示す図、第５図は、本発
明の説明上必要とされる各座標系を示す図、第６図、第
７図は、バリ堆りが如何にして行なわれるかを説明する
だめのバリ取り用ツールと対象物との間の関係を示す図
、第８図は、そのツールを倣い移動させるだめの特定座
標系の移動設定方法を説明するための図である。 １・・・アーム、２・・・ハンド、３・・・Ａ　！Ｊ　
取ｊＤ　用ツーツク信号変換部、１７・・・モータ、１
８・・・カウンタ、１９−・・・エンコーダ、２１・・
・サーがアンプ　　　　　　　　　　骸代理人　弁理士
　秋　本　正　実 ト　　　　　　　■　　　　　　　　０−　　　　　　
　　　　　　　ＯＪ 第２図 丁　３　ｒＡ 第６図 つ 第７図

Claims (1)

1. 【特許請求の範囲】 １、アーム先端に力・トルクセンサ、ハンドを介しバリ
   取り用ツールが把持され、該ツールに対象物上を接触倣
   い移動させることによって、該対象物に生じているバリ
   を連続的に切削するためのバリ取りロボット用フィード
   バック制御方式であって、ハンドおよびバリ取り用ツー
   ルに仮想の質量、バネ定数および粘性定数を与え、後に
   各アーム間関節角速度に変換される、現サンプリング制
   御時での速度指令値を上記各定数と、サンプリング制御
   周期と、力・トルクセンサからの１サンプリング前での
   力・トルクフィードバック信号と、１サンプリング前で
   の外部からの基準位置姿勢指令と実際のハンドおよびバ
   リ取り用ツールの位置姿勢との偏差とにもとづいて得る
   ことを特徴とするバリ取りロボット用フィードバック制
   御方式。 ２、バリ取り中制御パラメータとしての仮想の質量、バ
   ネ定数および粘性定数は可変とされる特許請求の範囲第
   １項記載のバリ取りロボット用フィードバック制御方式
   。