JPH04164585A - 力制御装置及びそれを用いたロボット - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［発明の目的］ （産業上の利用分野） 本発明は、手先部に取り付けられた手先効果器の被加工
部材への押し付方を検出して、ロボットを制御する力制
御装置及び力制御ロボットに関する。

（従来の技術） ロボットの手先に、工具等の手先効果器を取り付けて、
ワークへ所定の力で押圧してワークを作業する力制御ロ
ボットと、力制御ロボットを制御する力制御装置が提案
されている。

力制御ロボットとして例えば、グラインダ作業ロボット
等においては、手先効果器であるグラインダをワークへ
所定の押し付は力で任意の方向に押圧するため、工具と
ロボットの手先部の間に６軸力トルクセンサが固着され
ている。

この６軸カトルクセンサによって検出された各軸方向の
力及び各軸回りのモーメントまたはこれらの合成力が所
定の値になるように力制御装置が制御している。

つまり、検出したい方向の力又はモーメントをそのまま
直接検出し、その検出された力やモーメントを力制御ロ
ボットの制御用の制御ループに組込むことにより、力制
御やコンプライアンス制御、ハイブリッド制御等を行っ
ている。

ところが、上記のようなロボットでは、力トルクセンサ
は、その先に比較的重量のある工具を取り付けているた
め、工具に加速度が働くと、この加速度によって生じる
慣性力も当然この力トルクセンナによって検出すること
になる。

さらに、力トルクセンサによって検出される力は、押付
は力や慣性力の合成力であり、押付は力や慣性力を区別
して検出することができない。

従って、上記のような力制御装置や力制御ロボットの構
成では、手先効果器のワークへの押付は力だけを正確に
測定することは困難であった。

また、力制御などを行う際には、従来の力検出方法では
、ワークと被接触時でもロボットアームの駆動による工
具の慣性力やロボットアームの振動などによる工具の慣
性力を押付は力と区別なく制御系にフィードバックして
しまう。そして、この慣性力は、制御しようとする押付
は力に対して同レベルあるいはそれ以上の大きさがある
ため無視することが出来なかった。仮にこの慣性力を無
視して作業を行うとワークへの所望の作業をすることが
出来なかった。

また微少の工具の振動でも、その慣性力を制御系にフィ
ードバックしてしまうため、さらに大きな振動となって
発振の原因となっていた。

従って、カルーブのゲインを上げることが出来ず、応答
性の良い制御を行うことが出来ないので、精度の高い加
工を行うことが出来なかった。

このような問題を解決する方法として、あらかじめロボ
ットの動作によって慣性力を計算しておき、その分を除
去する方法が提案されているが、動作ごとにあらかじめ
計算しておく必要があるし、また、アームの振動のよう
に、予測できない慣性力に対しては全く対応できない。

他にフィルタリング手法を用いる方法が提案されている
が、この方法も、十分に効果があるとはいえないし、多
くの計算を必要とする。

また、常に、工具の姿勢によって工具の６軸力トルクセ
ンサに対する自重補償の計算もしなければならなかった
。従って、計算機の負担が増える等の問題があった。

他の問題点として、加工作業されるワークの形状がわか
っている場合にはそれに応じてワークの法線方向に工具
を押付けながらかつ手先効果器の姿勢を変化させ常に手
先効果器をワークに対して所定の姿勢を保ちながら、作
業を行うことができるが、ワークの形状が予めわからな
いような場合には、どの様に姿勢を変化させればよいの
かロボットは自ら判断することが出来ないので、未知形
状のワークの加工に対応することが出来なかった。

またワークの形状が予めわかったとしても、その形状を
ロボットに教示したり、制御装置に形状に対応したデー
タを入力する作業は、ワークの形状、ワークの仕上形状
が複雑になればなるほて真人な労力を必要とする。

一方、研究段階のものとして、特殊な形状の砥石や専用
の特殊な力センサを工具に組込んで、未知の形状のワー
クを研削するロボットが提案されているが、汎用性に乏
しく一般のグラインダ作業に適用することは出来ない。

また、特殊な形状の砥石や専用の特殊な力センサを工具
に組込んでいるのでコストが割高である。

（発明が解決しようとする課題） このように、従来の力制御ロボットでは、手先効果器で
ある工具の慣性力を検出するため、工具とワークの間の
正確な押付は力を検出することができず、慣性力などを
検出してしまうため力ループのゲインを上げることが出
来ず、力制御の応答性が悪く、加工を精度良く行うこと
が困難であるなどの問題があった。また、工具の６軸ト
ルクセンサに対する自重補償の計算もしなければならな
かったなどの問題もあった。

さらに、従来の力制御ロボット及び力制御装置では、未
知の形状の被加工部材に適用することが出来ない。この
ため被加工部材の形状をロボットに教示したり、インプ
ットするのに真人な労力を必要とするという問題があっ
た。

また、研究段階のものとして、未知の形状の被加工部材
を研削するロボットが提案されているが、汎用性に乏し
く、一般のグラインダ作業に適用することが出来ない。

またコスト的にも割高であるという問題がある。

そこで本発明は、重力、慣性力の影響を簡単に消去し、
手先効果器のワークへの押付は力を正確に検出して、力
制御等の応答性を向上することが出来、加工精度を向上
することが出来、計算機の負担を軽減させることが出来
る力制御装置及び力制御ロボットの提供が第１の目的で
ある。

さらに、予め被加工部材の形状をロボットに教示する必
要がなく、未知の形状の被加工部材に対しても、被加工
部材の法線方向に押付けながら、かつ、手先効果器の姿
勢を変化させ常に手先効果器を被加工部材に対して所定
の姿勢を保ちながら作業することが出来、通常の工具や
一般的なセンサを用いて作業を行うことが出来る力制御
装置及び力制御ロボットの提供が第２の目的である。

［発明の構成］ （課題を解決するための手段） 上記第１の目的を達成するため請求項（１）記載の発明
では、手先効果器へ付与された押付は力の反力を検出す
る検出手段と、この検出手段の検出位置を手先効果器の
重心へ移動して、この重心回りのモーメントを求めて、
手先効果器の被加工部材への押付は力を演算する演算手
段とを設けたことを特徴としている。

請求項（２）記載の発明では、前記手先効果器へ付与さ
れた押付は力の反力を検出する検出手段の検出結果から
、前記手先効果器の重心回りのモーメントを求めて、こ
の重心回りのモーメントから手先効果器の押付は力を補
正する補正手段を設けたことを特徴としている。

請求項（３）記載の発明では、補正手段が、検出手段の
検出位置を手先効果器の重心へ移動して、この重心回り
のモーメントを求めて、手先効果器の被加工部材への押
付は力を演算する演算手段であることを特徴としている
。

請求項（４）記載の発明では、補正手段が、手先効果器
にカウンタウェイトを取り付けることにより、手先効果
器の重心位置と前記検出手段の検出位置とを一致させた
ことである。

上記第２の目的を達成するため請求項（５）記載の発明
では、手先効果器へ付与された押付は力の反力を検出手
段で検出し、この検出手段の結果が所定の値に対して変
化しないように手先効果器の姿勢を変化させる姿勢変化
手段と、前記手先効果器の固定された方向に、前記手先
効果器の押付けや移動がなされる駆動手段とを設けたこ
とを特徴としている。

請求項（６）記載の発明では、前記手先効果器の姿勢制
御の回転中心を、前記手先効果器と被加工部材の接触点
付近としたことを特徴としている。

請求項（７）記載の発明では、手先効果器の移動軌跡か
ら被加工部材の形状を記憶する被加工部材形状記憶手段
と、前記被加工部材の仕上形状を記憶する仕上形状記憶
手段と、前記被加工部材形状記憶手段と前記仕上形状記
憶手段に記憶されている形状から手先効果器の目標位置
と目標姿勢を演算する演算手段とを備えたことを特徴と
している。

（作用） 請求項（１）の発明では、演算手段によって、検出され
た手先効果器の重心回りのモーメントから、手先効果器
の被加工部材への正確な押付は力が検出される。

手先効果器は、被加工部材に対して一定の姿勢で一定の
方向に押付ける場合が多く、また、被加工部材と接触す
る部分もほぼ決まっている場合が多い。

このため、手先効果器の被加工部材への押付は力をＦと
すると、検出手段によって検出される手先効果器の重心
回りのモーメントＭＧと、重心からの垂直距離ｒを用い
て Ｆ−Ｍ　ｃ　／　ｒ　　　　　　　　　　・・・（１）
によって押付は力を算出することが出来る。

また、押付は力を制御するために、手先効果器に慣性力
が働いた場合でも、つまり手先効果器の押付は方向に加
速度αが生じた場合でも、それによって生じる慣性力ｍ
α（ｍは手先効果器の重量）は、重心回りのモーメント
ＭＧには、まったく検出されない。

従って、慣性力等の影響を受けずに正確な押付は力を検
出することが出来るので、検出された押付は力に基づい
て、手先効果器を被加工部材へ押付けることが出来る。

従って、力制御等の応答性が良く、精度の良い力制御装
置を提供することが出来る。

また、手先効果器の重心回りのモーメントＭＧで検出し
ているため、ロボットの姿勢などが変化してもそれによ
る手先効果器の自重の影響は生じないので、自重補償の
演算も必要としない。

請求項（２）の発明では、補正手段によって、手先効果
器の被加工部材への押付は力を求めて、検出手段の結果
を補正する。

これにより、正確な押付は力を検出することが出来るの
で、検出された押付は力に基づいて、手先効果器を被加
工部材へ押付けることが出来る。

請求項（３）の発明では、力制御ロボットの補正手段が
、請求項（１）記載の演算手段であり、この演算手段に
よって正確な押付は力を検出することにより、手先効果
器の被加工部材への正確な押付は力が検出される。

請求項（４）の発明では、カウンタウェイトを手先効果
器へ取り付けることにより、検出された手先効果器の押
付は力を特徴する 請求項（５）の発明では、検出手段の検出結果が所定の
値に対して変化しないように手先効果器の姿勢が、姿勢
変化手段によって変化される。

また、手先効果器の固定された方向に、駆動手段によっ
て被加工部材の押付けや、移動がなされる。

請求項（６）記載の発明では、手先効果器の姿勢制御の
回転中心を、手先効果器と被加工部材の接触部付近とし
たことにより、この接触部付近を中心に姿勢制御される
。

請求項（７）記載の発明では、被加工部材形状記憶手段
に、手先効果器の移動軌跡から被加工部材の形状が記憶
される。また被加工部材の仕上げ形状は仕上げ形状記憶
手段に記憶される。これらの被加工部材形状記憶手段と
仕上げ形状記憶手段に記憶されている形状から、手先効
果器の目標位置と目標姿勢が演算手段によって演算され
る。

力制御ロボットにおいて、被加工部材に加工を行う際、
手先効果器の押付は方向や移動方向、被加工部材に対す
る姿勢は、およそ決まっている場合が多い。例えば、デ
ィスクグラインダ（アングルグラインダ）による研削作
業では、押付は方向は被加工部材の法線方向、移動方向
は接線方向であるし、被加工部材に対する手先効果器の
姿勢については、ピッチ角は２０〜３０＠、ロール角は
９０°である。したがって、作業を行う場合、既知の被
加工部材に対して、請求項（１）乃至請求項（４）記載
の力制御装置で力制御ロボットの方向、姿勢の制御を行
う。

しかし、未知の被加工部材の形状に対しては、方向、姿
勢などを決定することができない。

そこで、請求項（５）乃至請求項（６）の発明の力制御
装置で力制御ロボットを制御する。すなわち手先効果器
の押付は方向や移動方向を被加工部材によって決めるの
ではなく、手先効果器に対して決定している。つまり、
手先効果器に対して常に一定の方向へ押付け、その直角
方向に手先効果器を送ることにより、被加工部材に倣う
ようにロボットを制御する。

ところが、上記だけでは、未知の形状の被加工部材に対
して、積極的に姿勢を変化させて手先効果器の被加工部
材に対する。姿勢を保つことはできない。

そこで、上記請求項（５）、（６）、（７）の発明では
、例えば、手先効果器に対して常に一定の方向へ押付け
るが、その押付は方向が被加工部材の法線と一致してい
ない場合に、押付は方向の直角方向に手先効果器を送っ
た場合、押付は力や押付けによって生じるモーメントは
、法線と一致している場合と比較して大きく変化する。

逆に法線と一致している場合には、押付は力や押付けに
よって生じるモーメントはあまり変化しない。

従って、この変化を検出して、その変化が無くなるよう
に手先効果器の姿勢を変化させることにより、押付は方
向と被加工部材の法線と一致させることが出来る。

そして、常にその変化が無くなるように手先効果器の姿
勢を変化させることが出来れば、被加工部材の形状が３
次元曲面のような場合でも、被加工部材に対する手先効
果器の姿勢を保つことが出来る。あらかじめ被加工部材
の形状を教示することなく一定の押付は力で、姿勢を変
化させながら被加工部材に倣うことが出来る。

さらに、請求項（７）の発明によって、未知の被加工部
材を倣ったときのデータを記憶しておくことにより、被
加工部材の形状を認識することが出来る。またその被加
工部材を任意の形状に仕上げることが出来る。

（実施例） 次に本発明に係る力制御装置及び力制御ロボットの実施
例について説明する。

第１実施例 第２図には第１実施例の力制御装置及びこの力制御装置
によって制御される力制御ロボット１が示されている。

この力制御ロボット１は６軸円筒座標形のロボットであ
り、ＺＳＲ，θ、α、β、γ方向の６軸を有している。

この力制御ロボット１の手先部３には手先効果器として
グラインダ５が取り付けられている。このグラインダ５
は、被加工部材であるワーク７（第１図参照）を研削加
工する。

力制御ロボット１のアーム９の端部とグラインダ５の間
には、６軸カセンサ】１が配置されており、この６軸カ
センサ１１とグラインダ５との間には、グラインダ５の
高周波の振動遮断するためのゴムダンパ１３が配置され
ている。

第１図に示されるように、手先部３のグラインダ５は、
円板状の砥石１５をモータ１７で回転し、ワーク７の表
面を研削する。この時砥石１５はワーク７の表面に対し
て所定の角度φｃ６で傾斜して、このワーク７の表面上
を移動する。

なお、第１図において、ワーク７の表面に沿った座標系
ΣＣ（ワーク７の法線方向Ｚ。、グラインダ送り方向Ｘ
　、グラインダ横方向Ｙ。）、センサ座標系ΣＳ、ΣＳ
をグラインダの重心に平行移動させたグラインダ座標系
ΣＧを設定する。また、第１図において、ＯＧはグライ
ンダ５の重心を示し、砥石１５とワーブ−の表面との当
接部から重心０゜までの距離をｒとする。

ワーク７の表面をグラインダ作業する場合は、−船釣に
は、ワーク７に対して法線Ｚ。方向にグラインダ５を押
付ける。またワーク７に対してグラインダ５は一定の接
触角φ。６を保ちながら作業を行う。

従って、グラインダ５の押付は反力Ｆｗｚとセンサー１
によって検出する重心００回り（この場合Ｙ　）のモー
メントＭＧＹおよび、グラインダ５の重心０　からＦｗ
Ｚまでの垂直距離「との関係はＭ　ｃｙ””　Ｆ　ｖｚ
Ｘ　ｒ　　　　　　　”’　（２）となる。式（２）よ
り重心００回りのモーメントＭＧＶを検出することによ
ってグラインダ５のワ−り７への押付は力の反力Ｆｖｚ
を計測することが出来る。また、第３図は、力制御ロボ
ット１の記号図であるが、ベース固定部に固定された絶
対座標系Σ０を設定する。

次に、力制御ロボット１が制御される力制御装置１９に
ついて第４図を用いて説明する。第４図は力制御装置１
９の構成を示すブロック図である。

力制御装置１９はコンピュータ２１からグラインダ５の
ワーク７への目標押付は力が入力されて、この目標押付
は力を後述する駆動用座標変換器へ出力する目標押付は
力出力部２３と、ジョイステック等の操作部２５から目
標位置が入力されて、この目標位置を出力する目標位置
出力部２７と、操作部２９から目標姿勢位置が出力され
てこの目標姿勢位置を出力する目標姿勢位置出力部３１
と、が設けられている。

さらに、力制御装置１９には、目標押付は力出力部２３
から目標押付は出力信号が入力され、目標位置出力部２
７からは目標位置信号が入力され、目標姿勢位置出力部
３１からは目標姿勢位置信号がそれぞれ出力されて、目
標角度θ１．がサーボドライブ、モータへ出力される座
標変換器３３力（設けられている。

また、６軸力センサー１によって検出された６軸方向の
力信号からグラインダ５の重心回りのモーメントＭＧを
求め、さらに重心まわりのモーメントから所定の押付は
力が出力される座標変換器３５が設けられている。

グラインダ５の重心回りのモーメントを検出するために
は６軸カセンサー１の原点０８とグラインダ５の重心０
　との各軸方向のオフセ・ットと、センサによって検出
できる各軸方向の力と、角軸回りのモーメントを用いて
座標変換器３５によって座標変換することにより、セン
サの原点を見掛上グラインダの重心へ移動させることが
出来る。

この場合、重心００回りのモーメントＭＧＹは、Ｍｓｙ
”　ｓｚ”　ｓｘ’　Ｚｃｓ’　Ｘｃｓ’こよって１Ｍ
ＧＹ＝ＭＳＹ＋ＦＳＺ’　ＸＧＳ−ＦＳＸ”　ｚＧＳ　
　”゛（３）と表せる。なお、ＭＳＹ”　ＳＺ”　ＳＸ
はセンサ座標系でのＹ軸回りのモーメント、Ｚ１Ｘ方向
の力、Ｚ　１Ｘ　は、センサ原点とグラインダ５の重心
ＧＳ　　　Ｇ５ ０ｏの各軸方向の距離を示している。また、符号も座標
系の取り方によって変わる。また、他の軸回りのモーメ
ントも同様にして、 Ｍ　　−Ｍ　　＋Ｆ　　−Ｚ　　−Ｆ　　−Ｙ　　　・
・・（４）ＧＸ　　　ＳＸ　　　ＳＹ　　　ＧＳ　　　
ＳＺ　　　ＧＳＭ　　−Ｍ　　＋Ｆ　　−Ｙ　　−Ｆ　
　−Ｘ　　　・・・（５）ＧＺ　　　ＳＺ　　　ＳＸ　
　　ＧＳ　　　ＳＹ　　　ＧＳにより求めることが出来
る。

ワーク７の法線方向に一定の押付は力で押付け、ワーク
７の表面を一様に研削するような場合、グラインダ５は
、ワーク７の法線方向に一定の力で押付け、表面に沿っ
た方向へ移動させれば良い。

ワーク７の表面に沿ったワーク座標系ΣＣを設定し、力
トルクセンサによって検出したワーク７の法線方向の押
付は力Ｆｗｚが、目標の押付は力出力部２３から出力さ
れた目標の押付は力Ｆ　　とＷＺｄ なるようにＺ　方向に目標位置Ｚ。、を与える。つまり
、 ＝　Ｋ　　（Ｆ　　　　Ｆ　　）　＋　Ｚ　Ｃｄ（ｎ−
１）ＺＣｄ（ｎ）　　　　ｆ　　　ＷＺｄ　　　Ｖｚ・
・・（６） と表される。なお、Ｚ　　−は、１サンプリンＣｄ（ｎ
　１） グ前の目標位置である。当然、ここでの、ＦｗＺは、グ
ラインダ５の重心Ｏｃ回りに働（モーメントから検出し
ている。

ワーク７に沿った方向Ｘ　１Ｙ　については、Ｃ ジョイスティックや計算によって、またグラインダ５の
姿勢α　、βｃ１γＣについては、ワーク７の表面に一
定の角度を保つように、目標位置を与える。

従って、検出される押付は力Ｆ　が目標の押付ｚ け力Ｆ　　に満たない場合には、さらにワーク７Ｚｄ の方へ目標位置が与えられ、目標の押付は力になるまで
ワーク７の方へ押付けられる。

この間、他の方向については、位置制御が成されており
、グラインダ５は押付は方向に並進移動される。

ワーク７の表面に沿った目標位置Ｘ　１Ｙ　をＣｄ　　
　Ｃｄ 固定している場合には、位置を保持した状態で、押付け
が行うことが出来る。また順当適当に計算機やジョイス
ティックにより目標位置Ｘ、ＹＣｄ　　　　　Ｃｄ を与えることで、ワーク７の表面に任意の送り速度で、
グラインダ５を進めることが出来る。

また、グランダ５の姿勢の目標値α　、β　、Ｃｄ　　
　　　Ｃｄ γＣｄについては、ワークに対して、一定に保つように
目標位置をあたえる。平板てあれば、α。１、β　、γ
　は一定であり、曲面の場合には、そのＣｄ　　　　　
Ｃｄ 曲面に応じてαＣｄ’　β。６、γ。、を変化させる。

例えば、マニュアルでのグラインダ作業時には、ジョイ
スティックの座標軸をワーク表面に沿ったワーク座標系
に対応させることにより、種々の傾きに対しても、ジョ
イスティックの二次元的な操作だけで、ワークに対して
一定の力で押付けながら作業が行える。

また、自動研削では、ワーク７の表面上の軌道Ｘ０１Ｙ
ｃを作成しておくことにより、自動的に任意の領域を研
削が出来る。

次に、上記力制御装置１９を用いた力制御ロボット１を
制御する制御方法について以下説明する。

コンピューター２１から目標押付は力Ｆ　　がｚｄ 目標押付は力出力部２３へ出力される。目標押付は力出
力部から出力された目標押付は力ＦＷＺｄは加算器によ
って座標変換器３５から出力される押付は力の反力ＦＷ
ｚと比較される。このとき反力Ｆの差をなくすように、
目標位置Ｚ。、を変化させる。

この場合、座標変換器３５内では、６軸カセンサ１１か
らの出力すなわち、検出された重心００回りのモーメン
トＭＧが入力される。この重心００回りの鷺−メントを
検出するためには、６軸力センサーｌの原点Ｏとグライ
ンダ５の重心ＯＧとの各軸方向のオフセットと、６軸力
センサー１によって検出される各軸方向の力と、各軸回
りのモーメントを用いて上述した式（３）〜（５）によ
って座標変換する。これにより６軸力センサー１の原点
を見掛上グラインダ５の重心０゜へ移動させることが出
来る。

このように、グラインダ５をワーク７への目標押付は力
で押付けて加工するように、押付は力が駆動用座標変換
器３３へ出力される。この目標押付は力に基づいて、サ
ーボドライブが図示しない関節駆動機構のモータを作動
させ、グラインダ５をワーク７へ押付ける。

従って、グラインダ５のワーク７への押付は力を目標押
付は力Ｆ　　て押付けることが出来、力Ｚｄ 制御等の応答性を向上することが出来る。さらにグライ
ンダ５によるワーク７の加工精度を向上することが出来
る。

本実施例の実験結果について第８図及び第９図を用いて
説明する。力の検出方法の違いによる慣性力の検出の影
響を調べた。

第１図に示す力制御ロボットを用いて、非接触状態で、
単に上下軸の速度入力にステップ入力を与えたときの６
軸カセンサ１１によって検出した押付は方向の反力Ｆ、
２を表した図である。

このため、非接触なので、グラインダ５には押付は力は
働かないが、上下方向の速度ステップ入力により、グラ
インダ５の慣性力が生じている。

第８図は、上下軸方向に働く力をそのまま検出した場合
である。当然この場合は、上下軸方向の力をそのまま検
出しているので、速度ステップ入力によりグラインダ５
の慣性力を検出してしまっている。

実際には、グラインダ作業の場合には、１〜２Ｋｇ程度
の押付は力を制御しようとしているのに対して、はるか
に大きい慣性力を検出してしまっている。

第９図はセンサの原点０　まわりに働くモータントから
押付は力を検出した場合である。この場合第９図に示さ
れるように、センサの原点０３とグラインダの重心ＯＧ
はずれているので、このずれによって生しる回転慣性力
を若干検出しているが、第８図に比較して慣性力の影響
は少ない。

さらに、本実施例によれば第１０図に示されるように、
センサの原点Ｏ８とグラインダ５の重心Ｏｏを一致させ
ているので、慣性力の影響を受けていない。

第１０図は、カウンタウェイト３７を用いないでグライ
ンダ５の重心００回りのモーメントを式（３）によって
検出した結果である。この場合にも慣性力の影響は受け
ていないが、カウンタウェイトをつけていないので、グ
ラインダ部の重量が大幅に軽い。

第１２図と第１３図は、第８図と第１０図の場合の検出
方法で押付は力Ｆ　　を検出し、その押Ｚｄ 付は力Ｆ　が目標押付は力Ｆ　　となるように、ｗｚ　
　　　　　　　　ｖｚｄ 重心を並進移動させることにより力制御を行いグライン
ダ作業を行った結果である。下方向へＩＫｇで押付ける
ように非接触状態から接触してＩＫｇになるまでの押付
は力を比較した図である。ともに発振しない程度まで力
のゲインを上げているが、約１０倍程度第１３図の方が
高い。明らかに第１３図のほうが応答性が良いことが判
る。第１２図程度の応答性では、研削面が少しでも傾い
ていたら、その面に対して十分に追従することができな
い。逆に第１３図程度の応答性があれば、研削面が多少
類いていても、十分追従することが出来る。

第２実施例 次に第５図（１１）及び第５図（ｂ）を用いて説明する
。第２実施例は手先部３のグラインダ５にカウンタウェ
イト３７を取り付け、２８軸とＺ６軸とを一致させて、
グラインダ５の重心０６回りのモーメントＭ６゜を直接
検出する例である。あるいは、カウンタウェイトを取り
付けずに、直接グラインダ５の重心ＯＧがＺｓ軸と一致
するように固着してもよい。

すなわち、グラインダ５の重心位置は第１図に示される
ように、２８軸方向に対して、砥石１５側に位置してい
る。このためＺｓ軸よりモータ１７の尻部側にカウンタ
ウェイト３７を取り付けることにより、グラインダ５の
重心は、モータ１７の尻部側へ移動し、カウンタウェイ
ト３７の重量を適当な重さに設定することにより、２８
軸　と２６軸とを同軸とすることが出来る。

本実施例によれば、グラインダ５のワーク７への押付は
力の反力の方向として、ＦＶＺを考慮しているので、こ
の方向のグラインダの重心回りのモーメントＭＧｚが検
出されれば良い。このため、１軸のみ一致させている。

他の場合で、複数の軸回りのモーメントが生じる場合に
は、１軸だけでなく、原点を完全に一致させなければな
らない場合もある。

このように、センサ座標系の軸とグラインダ５座標系の
軸を、カウンタウェイト３７やその取り付は位置の調整
により一致させることが出来れば、直接グラインダ５の
重心回りのモーメントＭＧを検出することが出来る。

この場合、−船釣な６軸カセンサの場合では、グライン
ダ５の取り付は方法が限定されたり、カウンタウェイト
３７により、重量が増加するが、センサ原点の平行移動
などの座標変換の演算が不要となる。

以上のように、本実施例においてもグラインダ５の重心
回りのモーメントＭＧを計測しているので、任意の押付
は力にするために、力制御によて押しつけ方向に駆動し
たとしても、グラインダ５の送りによる慣性力やアーム
の振動による慣性力等も検出することがない。

なお、アームの振動はグラインダ５に対して並進方向に
振動し、グラインダ５に対して回転方向にはほとんど振
動しない。従って、アームの振動によって、グラインダ
５の重心まわりのモーメントＭＧが振動することは、は
とんどない。

従って、グラインダ５の押付は力Ｆｖ２を精度良く検出
することが出来、制御応答性を向上することが出来る。

次に第１１図には本実施例の実験結果が示されている。

第１１図は、センサの原点０８回りにカウンタウェイト
３７でバランスさせた場合で、慣性力の影響をほとんど
受けていないことが確認される。

第３実施例 次に第３実施例について第６図＜８）乃至ＣＣ＞を用い
て説明する。第３実施例は、上記第１実施例、第２実施
例が研削作業（グラインダ作業）の例であるの対して、
平板を切断する例である。

１１１６図（ｂ）に示されるように、手先部３のグライ
ンダ５の砥石１５は、平板３９を切断する。

この切断する際には、グラインダ５の切り込み深さや、
送り方向などによって異なるが、グラインダ５を切断す
る時、グラインダ５の砥石１５は反力として力Ｆが付与
される。この力Ｆは、平板３９の厚み方向の分力Ｆ　、
平板３９の面方向の分力Ｆ。Ｙに分圧することが出来る
。

グラインダ５の重心０゜から砥石１５の垂直距離ｒは常
に一定であるので、これを利用して、グラインダ５のＸ
　　、Ｙ　　回りのモーメントによっＧ て、押付は力を求めることが出来る。

反力Ｆは Ｆ−Ｊ（Ｆ　　、”＋Ｆ　ｚ） ＧＸ　　　　　ＧＹ −Ｊ　（（ＭＧｘ／ｒ）’　＋　（ＭＧＹ／ｒ）　２）
・・　（７） によって、計算することが出来る。この場合も慣性力の
影響を受けないので、研削の場合と同様の制御応答が向
上する。なお、モーター７のトルクを考慮する必要があ
る場合には、あらかじめその値を計測しておき、除法す
ればよい。

適用例として、上記各実施例では、円筒座標形ロボット
について示しているが、とくにこのロボットに限らず、
直交座標形、極座棟形、多関節座標形、ロボットに適用
することが出来る。

また、上記各実施例では、押付は力検出用センサとして
６軸カセンサを用いたが、これに限らず必要な方向の力
を検出することが出来れば、一方向のみ検出する力セン
サでも良い。

これにより、小形化、低コスト化が出来る。６軸カセン
サの場合には、押付は方向以外に過大な力が加わった場
合に非常停止などの処置が可能となる。

第４実施例 次に第７図（ａ）及び第７図（ｂ）を用いて第４実施例
について説明する。本実施例はグリッパ等に作用する力
を本発明を適用して検出する例である。

グリッパ部に働く力Ｆ　　ＳＦ　　、Ｆ　　は、グリＧ
Ｘ　　　ＧＹ　　　ＧＺ ツバ部の作用点Ｐから、センサ原点までの距離（Ｘ　　
　Ｙ ＣＰ’　　ＧＰ’　ＺＧＰ）とグリッパの重心回りに働
くモーメントＭ。Ｘ’　ＭＧＸ”　ＧＹと一方向の力成
分、例えば、ＦＧｚなどが分かっていれば、次式（８）
〜式（１０）を解くことにより、検出することが出来る
。

グリッパに働く力の各軸方向の成分Ｆ　　、Ｆ　　、Ｇ
Ｘ　　　　　ＧＹ Ｆ　Ｇ　Ｙ　ト、グリッパの重心回りのモーメントＭｃ
ｘ’ＭＧＺ”　ＧＹと、グリッパに働く力の重心までの
各軸方向の垂直距離ｘ　　、ｙ　　、ｚ　　の関係式は
ＧＰ　　　　ＧＰ　　　　　ＧＰ Ｍ　　−Ｆ　　・Ｙ　　−Ｆ　　−Ｚ ＯＸ　　　ＧＺ　　　ＧＳ　　　ＧＶ　　　ＧＳ　　　
　””８）Ｍ　　−Ｆ　　−Ｚ　　−Ｆ　　−Ｘ ＧＹ　　　ＧＸ　　　ＧＳ　　　ＧＺ　　　ＧＳ　　　
　””９）Ｍ　　−Ｆ　　・Ｘ　　−Ｆ　　−Ｙ ＧＺ　　　ＧＹ　　　ＧＳ　　　ＧＸ　　　ＧＳ　　　
　””０）であるため、これらの式から並進方向に働く
力Ｆ　　ＳＦ　　ＳＦ　　などを、ＭＧＸ’　ＭＧＺ、
ＭＧＹ、ＯＸ　　　ＧＹ　　　ＧＺ ｘ　　、ｙ　　、ｚ　　などから求めることが出来る。

ＧＰ　　　　ＧＰ　　　　ＧＰ ただ、全方向の力ＦＧＸ”ＧＶ、ＦＧＹをＭｃｘ’　Ｍ
ｃｚ’Ｍ　　、Ｘ　　ＳＹ　　、Ｚ　　だけがら求める
ことは出ＧＹ　　　ＧＰ　　　ＧＰ　　　ＧＰ 来ないが、何か条件のある場合や、１軸方向については
直接、軸方向の力を用いた場合には、求めることが出来
る。

この場合、１方向（例えばＦＧＺ）については、軸方向
の働く力を検出しているので、その方向の力については
慣性力の影響を消去することは出来ない。

しかし、ロボットアームの場合、比較的アームの振動し
やすい方向が決まっている場合が多いので、アームの振
動しにくい方向については、直接その軸方向の力を検出
し、他方向の力については、クリッパの重心図りに働く
モーメントにより検出することによって、慣性力の影響
を極力除去した形でグリッパに働く力を検出することが
出来る。

また、直接検出する方向は、アームの姿勢などにより、
その姿勢ごとに最も影響の少ない方向をまた、直接検出
する方向は、アームの姿勢などにより、その姿勢ごとに
最も影響の少ない方向を選択することによって、慣性力
の少ない検出が出来る。当然、グリッパの重心にセンサ
原点を平行移動させるだけでなく、適当に座標軸を回転
させることなども必要に応じて行うほうが良い。

第５実施例 次に第５実施例について第１４図を用いて説明する。本
実施例は、第１図乃至第３図て示される力制御ロボット
１で未知の形状のワーク７を加工する際に制御するため
の、力制御装［４１の他の実施例である。

なお、第１図において、グラインダ座標系ΣＧ１をＹ　
軸まわりにφ６ｖ度回転させた座標系ΣＷを設定する。

センサ座標系ΣＳはセンサに、ΣＧ５ΣＷはグラインダ
５にそれぞれ固定された座標系である。またＯｃは、グ
ラインダ５の重心を示し、砥石１５とワーク７の表面と
の当接部から重心Ｏまでの距離をｒとする。またφ。、
をグラインダ５のワーク７に対するピッチ角とする。ま
た、第３図において、ベース固定部に固定された絶対座
標系Σ０を設定する。

さらに、Ｚｖをグラインダ５のワーク７への押し付は方
向、Ｘｖをグラインダ５の前後進行方向、Ｙｖをグライ
ンダ５の横送り方向とする。つまり、グラインダ５の姿
勢によってそれぞれの方向が決定される。

したがって、ＺｖとＺｃが平行であれば、グラインダ５
は、ワーク７の法線方向に砥石１５を押し付けることが
出来る。さらに、Ｘｗ、Ｙｖ力方向グラインダ５を移動
させることにより、ワーク７の接線方向に移動させるこ
とが出来る。φ６ｖは、ワーク７に対するグラインダ５
の目標ピッチ角になる。

そして、研削作業時において、６軸力センサー１の検出
値の変化を検出して、常にＺ　とＺｖが■ 平行になるように、グラインダ５の姿勢を変化させるこ
とか出来れば、未知の平面や曲面のワークについて常に
ワーブ−に対する所定の姿勢を保つた状態で、かつ、任
意の押し付は力で研削作業をすることが出来る。

第１４図に示されるように、力制御装置４１は、ホスト
コンピュータ４３から、グラインダ５の目標モーメント
Ｍ　　、目標押し付は方Ｆ　　１目ＧＸｄ　　　　　　
　　　　ＶＺｄ 標ヨー角ψ　、目標修正量ΔＸ　、ΔＹＧｄ　　　　　
　　　　Ｖｄ　　　　Ｖｄ（ΔＸ　１Δ”ｌｄφＧｄは
、マニュアル操作では、Ｏｄ ジョイスティックなどの操作部から）が入力される。

そして、６軸力センサー１の検出値を力センサ座標変換
器４５によって、グラインダ５の重心Ｏｏに座標変換す
る。次に、横送り時、進行、停止時に応じて、Ｆ　また
はＭＧｘから目標ロール角ｚ θ　を計算する。次に、ＦＶｚからグラインダ目標ｄ ピッチ角φ。、を計算する。なお、積分項は図示してい
ない。以上の目標ロール角θ　、目標ピッチｄ 角φ　および予め設定している目標ヨー角ψ。、かＯｄ ら、グラインダ姿勢座標変換器４７によって、例えば力
制御ロボット１のベースに固定された絶対座標系でオイ
ラー角表示されるような目標姿勢α　、β　、γ　を計
算する。

Ｏｄ　　　Ｏｄ　　　Ｏｄ 具体的には、−回の座標変換で、同時に目標ロール角、
ピッチ角などを作成することはできないので、たとえば
、まずロール方向の修正のみを考慮した目標姿勢を作成
し、それに対しピッチ方向の修正を考慮した目標姿勢を
作成することにより、ロール方向、ピッチ方向の修正を
考慮した目標姿勢を作成している。（必要に応じヨ一方
向の修正を考慮する） そして、Ｆｖ７から押し付は方向の目標修正量ΔＺ　を
計算し、目標修正量Δ２　、および予め１１１ｄ　　　
　　　　　　　　　　ｗｄ段設定ている進行方向、横方
向の目標修正量ΔＸＶｄ、ΔＹ　から、目標姿勢α　、
β　、γ　によりｄ　　　　　　　　　　　　　　　　
Ｏｄ　　　　　Ｏｄ　　　　　Ｏｄって決定されるグラ
インダ位置座標変換器４９を用いて、絶対座標系で表さ
れる目標位置Ｘ　　、Ｙｄ 。６、ｚｏｄを計算する。

さらに、上記によって計算された絶対座標系で表される
目標位置、姿勢ｘ　　、ｙ　　、ｚ　　、Ｏｄ　　　Ｏ
ｄ　　　Ｏｄ α　、β　、γ　を駆動用座標変換器５１により、Ｏｄ
　　　Ｏｄ　　　Ｏｄ １〜６軸の各関節軸の目標角度を計算する。そして、位
置、速度ループによって構成された各軸を駆動する。

次に、上記力制御ロボット１を力制御装置４１で制御す
る制御方法について説明する。

先ず、押付は方向Ｚ、を制御するためには、第１実施例
の式（２）乃至（５）により各軸回りのモーメントを求
める。そして式（２）によって計算されるワークに対す
る押付は力の反力Ｆ　が目Ｚ 樟の押付は力Ｆ　　となるように制御を行えば良１Ｚｄ い。

反力Ｆ　が目標の押付は力Ｆ　　となるように、Ｗｚ　
　　　　　　　　　ＶＺｄ グラインダ５の押付は方向の目標位置を修正する。

押付は方向の修正量ΔＺｌｌｄは次式で与える。

ΔＺ−Ｋ（Ｆ　　　　−Ｆ）　　　　　・・・　（］　
１）Ｖｄ　　　　ｆｚ　　　　ＶＺｄ　　　　　ＶＺこ
こて、Ｋｒｚはカゲインである。１サンプルごとに式（
１１）により、２ｗ方向の目標位置が修正される。例え
ば、目標の力に対して検出した力が小さい場合、ワーク
７の目標位置は押し付は方向に修正される。

次に、グラインダ５の前後進行方向Ｘｖおよび横送り方
向Ｙｖを制御するには、グラインダ５の前後進行方向お
よび横送り方向Ｙｖの１サンプリングごとの目標移動量
ΔＸ　、Ｙ　、たとえば、Ｗｄ　　　Ｖｄ 計算機によってあらかじめグラインダ５のワーク７上の
移動方法を作成しておくことにより、ワークの任意の領
域を自動的に研削しておくことが出来る。

あるいは、ジョイスティックがら与えることにより、マ
ニュアルによるグラインダ作業が可能である。

上述の、押付は方向、進行方向、横送り方向の修正量、
移動量、ΔＸ　　ΔＹ　１ΔＺＩｌｄを以下ｖｄ’　　
　　　　Ｗｄ に示すグラインダ５の目標姿勢を用いて座標変換し、グ
ラインダ５の目標位置を作成する。

次に、グラインダ５の姿勢の制御方法について説明する
。まずグラインダ５のピッチ方向（φ０Ｙ、軸回り）の
制御方法について説明する。

ワーク７に対するグラインダ５のピッチ角φＣＧがφ。

Ｗと一致している状態で、グラインダ５を移動させた場
合には、式（１１）によって制御している押付は力に、
誤差はほとんど生じない。

しかし、第１６図に示すように、φＣＧがφＧｖより大
きい状態で、グラインダ５を前進（−Ｘ、方向）させた
場合、グラインダ５は、押付は方向（図示矢印方向）に
進もうとするので、押付は力が増加する。一定速度で送
った場合などでは、押付は力Ｆ　は目標押付は力Ｆ　　
に対して定常的ｖｚ　　　　　　　　　ｖｚｄ に誤差を含んだ状態になる。

逆に、φＣＧがφＧｗより小さい状態で、グラインダを
前ａ　（−Ｘ　　方向）させた場合、グラインダ５は、
離れようとする方向に進もうとするので、押付は力が減
少する。また、グラインダを後進させる場合には、逆に
なる。

したがって押付は力ＦｗＺが目標押付は力Ｆｚｖｄにな
るように、グラインダ５のピッチ角φＧを変化させるこ
とにより、グラインダ５のワーク７に対するピッチ角を
常にφＧＷに保つことが出来る。

つまり式（１１）と同様に、 φＧｄ（ｎ）　　　　ｆφ　　ＶＺｄ −Ｋ　　　　（Ｆ　　　−ＦｖＺ） １φＧｄ（ｎ−１）　　　　　　　　　　−（１２）と
表される。なおＫ　　はゲイン、φ。ｄ（。〜１）ｒφ は、１サンプリング前の目標ピッチ角である。

なお、重心ＯＧを固定した状態でＹｃ軸回りに回転させ
ると、姿勢の変化によって押付はカを変化させることに
なるため、ピッチ方向の回転は、砥石先端部つまり砥石
１５とワーク７の接触点付近を中心に回転させる。

したがって、式（１１）によってワーク７に対するピッ
チ角は変化するが、押付は方向の制御に悪影響を与える
ことはない。

実際には、グラインダは、前進、後進するので必ずしも
接触点を中心に回転させた場合に最もスムーズに曲面に
追従できるとは限らない。前進時には接触点に対しグラ
インダ側、後進時は逆に接触点より先側を回転中心とし
た方がスムーズに曲面に追従できる。必要に応じ適宜変
化させる。

平面などを研削する場合には、式（１１）によって、最
終的にワークとのピッチ角φＧｃをφＧＷにすることが
出来るが、ゲインに、φを上げられずに追従性が悪く曲
面などに対応出来ない場合などには、第１項の比例制御
だけでなく、積分項を加え、式０１）を −Ｋ　　　（Ｆ　　−Ｆ） φＧｄ（ｎ）　　　　ｆφｗＺｄ　　　　Ｖｄ＋Ｋ　　
Σ（Ｆ−Ｆ） ｒ（１１１１Ｚｄ　　　ＷＺ ＋φ。ｄ（。−１）　　　　　　・・・（１３）とする
ことにより追従性を上げても良い。基本的にはピッチ角
φｃＧをφＧｖにするように制御すればよい。

式（１２）または式（１３）では、円弧状の曲面や連続
的に曲率が変化するような曲面に対しても、グラインダ
が前後方向に進行中のときは、常にワーク７とのピッチ
角φｃＧをφＧｗにするように制御されているので、追
従することが出来る。

なお、前進の場合と後進の場合では、回転させるべき方
向が異なるので、式（１２）または式（１３）での符号
は適宜変える必要がある。

上記ピッチ角の制御方法と前述のグラインダ５の押付は
方向、前後進行方向の制御方法を組み合わせることによ
り、例えば第１７図に示されるような曲面を、曲率の変
化などを予め教示することなく、図中■の方向へ研削す
ることが出来る。但し、グラインダ５のワークに対する
ロール角やヨー角を与えるため曲面の傾き角は予め必要
である。

次に、グラインダ５のロール方向（θ軸：ＸＧ軸まわり
）の＠脚力法について説明する。

グラインダ５を横方法つまりＹｗ方向に移動させる時は
、前述のピッチ方向の制御と全く同様に考えることが出
来る。グラインダ５のワークに対するロール角は９０°
　（Ｘ　ｗ軸とＸｃ軸が一致）の方向でないとスムーズ
なグラインダ作業を行うことが出来ない。つまり、ワー
ク７に対するグラインダのロール角が９０’と一致して
いる状態でグラインダ５を横方向に移動させた場合には
、式（１２）によって制御している押付は力の誤差はほ
とんど生じない。

しかし、ピッチ方向の時と同様に、ワークに対するグラ
インダ５のロール角が９０″からずれている状態で、グ
ラインダ５を横方向に移動させた場合には、押付は方向
あるいは離れようとする方向に進もうとするので、押付
は力に増減が生しる。

したがって、式（１２）あるいは式（１３）と同様に、
押付は力ＦｗＺが目標押付は力ＦＶｄになるように、グ
ラインダ５のロール角θ。を変化させることによって、
グラインダ５のワーク７に対するロール角を常に９０”
に保つことが出来る。つまり −Ｋ　　　　（Ｆ−Ｆ） ０Ｇｄ（ｎ）　　　　ｒｅ　ｌ　　　　ＶＺｄ　　　’
ｉＺ＋θＧｄ（ｎ−１＞　　　　　　　　　　・＝　（
１４）と表される。あるいは、積分を付加してθＧｄ（
ｎ）　　　　ｒｅｌ　　　　Ｖｚｄ　　　’１ｚ−Ｋ　
　　　（Ｆ−Ｆ） ＋Ｋ　　、θｌ　Σ　（Ｆ　　　　−ＦＷＺｄ　　　Ｉ
ｆｚ ＋θＧｄ（ｎ−１）　　　　　　　−（１５）と表され
る。

上記式（１４）、式（１５）によって、グラインダ５を
横方向つまりＹｙ力方向移動させる時には、ワーク７に
対するグラインダ５のロール角を９０°に保つことが出
来る。

横移動時に削り過ぎないようにするため、横方向に送り
の時の目標押付は力ＦＷＺを前後進行時の目標押付は力
Ｆ　　を前後進行時の目標押付は力ｚｄ と比較して小さくしても良い。

上記ロール角の制御方法と前述のグラインダ５の押付は
方向、前後進行方向の制御方法を組み合わせることによ
り、第１７図に示すような２次曲線を曲率の変化などを
あらかじめ教示することなく図中■の方向に研削するこ
とが出来る。ただし、グラインダ５のワーク７に対する
ピッチ角やヨー角を与えるため曲面の傾きはあらかじめ
必要である。

上記の方法では、グラインダ５の前後方向時や停止時な
どにロール角をを修正することは出来ない。そこで、次
にグラインダの前後進行時や停止時のロール角の制御方
法について説明する。

例えば第１８図に示すように、ワーク７に対するロール
角が９０°からずれている場合にはＺｖとｚｃはずれて
いるのて、傾いた方向に押付けようとするのて、Ｘ　軸
回りのモーメントＭ。Ｘが変化する。研削を行っていな
い状態では、ワーク７に対するロール角が９０°の状態
で押付けた場合には、モーメントＭＧｘが働かないので
、目標モーメントＭＧＸｄはゼロであるが、研削を行っ
ている状態では、切削抵抗やモータのトルク等が働くた
めゼロにはならない。また、砥石やワークの種類、押付
は力によっても異なる。したがって、予めワーク７に対
するロール角が９０＃の時のモーメントＭ　が目標モー
メントＭＧＸｄとなるように目標Ｘ ロール角を与えることにより、グラインダ５の前後進行
時や停止時などのロール角を抑制する。つまり、 −Ｋ　　　　　（Ｍ−Ｍ） θＧｄ（ｎ）　　　　ｆ　４９２　　　ＧＸｄ　　　Ｇ
Ｘ＋０Ｇｄ（ｎ−１）　　　　　　””　１”　）と表
せる。押付は等から目標モーメントＭＧＸｄを予め求め
ておく必要があるが、グラインダの条件（ワークの種類
により砥石の種類や押付は力、進行速度、横送りピッチ
量）なども、あらかじめ調べておく必要があることを考
慮すると、とくに負担にはならない。

ロール方向の回転は、ピッチ方向の回転と同様に、砥石
先端部つまり砥石とワークの接触点付近を中心に回転さ
せる。

次にヨ一方向（ψ　：Ｘｏ軸まわり）の制御力法につい
て説明する。

ヨ一方向を変化させると、グラインダ５の進行方向が変
化する。したがって、研削領域によって、あらかじめ設
定しておく。研削中は特に変化させる必要はないが、研
削方向や研削領域を変化させる必要があるときは、必要
に応じて変化させる。

姿勢の変化方法については、座標系ΣＧの各軸まわりに
回転させる方法について説明したが、座標系ΣＷの各軸
まわりに回転させても同様に姿勢を変化させることが出
来る。

上記の方法で、グラインダの押付は方向、前後進行方向
、横送り方向の並進方向および、ロール、ピッチ、ヨー
の姿勢の制御方法を行うことにより３次元曲面などにつ
いても、あらかじめその形状を詳細に教示すること無く
、ワークに対するグラインダ姿勢を保つように自動的に
姿勢を変化させ、なおかつ、任意の押付は力でのグライ
ンダ作業が可能となる。

例えば、特殊環境下などで、非定形なグラインダ作業を
行うような場合には、ワークの形状やロボットに対する
ワークの位置などをあらかじめ設定しておくことが出来
ないような作業においても、その場で、モニタなどを通
しておおよその位置関係やグラインダの姿勢を与えるこ
とができれば、作業は容易に、また効率的に行える。

ジョイスティックによるマニュアル作業では、作業者は
、おおよその初期姿勢を与えられるだけで、後は、グラ
インダ５の移動を指令するためのジョイスティックの２
次元的な操作だけでよ（、押付は力、グラインダ姿勢を
自動的に制御しながら研削することが出来る。またグラ
インダ５の進行方向や横方向の移動パターンなどをあら
かじめ設定しておくことにより、任意の領域の研削を容
易に自動的に研削することが出来る。

このように、上記制御方法により、予めワーク形状を教
示すること無く研削作業を行った実験結果の一例を示す
。第１９図に示すような断面形状のワークを図中矢印方
向に、目標押付は力ＦＶＺｄ−１．２ｋｇｆ、グライン
ダ移動速度ΔＸ　ｖ　ｚ　−２。

５ｃｍ／ｓ、ワークに対する目標ピッチ角φＧｖ−２５
″そして、初期のワークに対するグラインダのピッチ角
４０１　（従って誤差は１５°）で、研削を行ったとき
の、グラインダのワークへの押付は力、ピッチ角（水平
からの角度を表している）、１〜６軸の関節角から算出
した砥石先端部の軌跡をｊＩ２０ｖ！ｆｆ乃至第２２図
に示す。これらの図がら、ワークに対するピッチ角が、
２５°となるように姿勢を変化させ、かつ押付は力が一
定の状態で研削しているのが確認できる。

第６実施例 次に第１５図を用いて説明する。本実施例は、力制御装
置の他の実施例である。上記第５実施例では、未知形状
のワークを一定の力で押付けながら研削作業を行う方法
を示したが、場合によっては、未知形状のワークを任意
の形状に仕上げたい場合がある。その場合について以下
説明する。第１５図に示されるブロックは、第１４図に
示される力制御装置４１と異なる部分のみ図示した。

まず、最初の研削では、力制御ｌｃ７ポツトはワーク形
状を確認していないので、第４実施例の力制御装置によ
り未知の形状のワークを一定の力で押付けながら研削作
業を行う。そしてその際に、１〜６軸の関節角を逆座標
変換器５３により、絶対座標系で表される実際のグライ
ンダの位置、姿勢ｘ　　、ｙ　　、ｚ　　、α　、β　
、γ　を計算し、ｏｏｏｏｏ。

それをワーク形状データ記憶袋ｗ５５に記憶する。

これにより、力１ｌｉｉＩＩ１１０ボットは、ワークの
形状を認識することが出来る。

そして、以降の研削では、コンピュータ４３から与えら
れるワーク仕上げ形状データ記憶袋Ｗ５９の記憶データ
とワーク形状データ記憶装置５５の記憶データから、グ
ラインダ目標位置、姿勢作成装ｆ５７によって絶対座標
系で表される目標位置゛姿勢Ｘｏｐ”　ｏｐ’　Ｚｏｐ
・　ＯＰ・βＯＰ・γＯＰを作成する。

そして、同様に、上記の各データを駆動用座標変換器５
１に入力し、各軸を駆動する。これにより、未知の形状
のワーク７を任意の形状に仕上げることが出来る。

なお、グラインダ目標位置・姿勢作成装置５７では、仕
上げ形状データと記憶データに形状が異なる時には、ワ
ーク７の法線方向には力制御を、移動方向には移動速度
に応した目標位置を与え、形状に差が無い場合、あるい
は、小さい場合には、位置制御を行うようにしても良い
。いずれにしても、ワークの形状データは、この時点で
は、既知となっているので、従来の制御方法を適用して
も良い。

また、特に工具などにより作業を行わない場合には、未
知の形状を確認する手段としても利用することが出来る
。

本発明の適用例として、上記各実施例では、円筒座標型
ロボットについて説明したが、特にこの形式のロボット
に限らず、直交座標型、極座標型、多関節座標型のロボ
ットに適用することが出来る。

また、上記各実施例では、押付は力検出用センサとして
、６軸カセンサを用いたが、これに限らず、必要な方向
の力を検出することができれば良い。

他に、押付は力の検出にグラインダ５の重心まわりに働
くモーメントＭｖＧを用いているが、小型のグラインダ
で慣性力の影響をあまり受けないような場合には、直接
押し付は力を検出しても良い。

［発明の効果］ 以上説明したように本発明に係る力ｆＩＩｉ御装置及び
力ｌ！ｌ１Ｉ１１ロボットによれば、重力、慣性力を簡
単に消去し、手先効果器の被加工部材への正確な押付は
力を検出することが出来、力制御等の応答性が良く、精
度のよい力制御ロボットを提供することが出来るという
優れた効果が得られる。

また、予めワーク形状を教示すること無く、未知の形状
のワークに対しても、ワークの法線方向に押し付けなが
らかつ手先効果器の姿勢を変化させ常に手先効果器をワ
ークに対して所定の姿勢を保ちながら作業を行うことが
出来るという優れた効果が得られる。

【図面の簡単な説明】

第１図は手先部を示す側面図、第２図は力制御ロボット
の外観を示す斜視図、第３図は力制御ロボットをモデル
化したモデル図、第４図は力制御装置の構成を示すブロ
ック図、第５図（ａ）は第２実施例の手先部を示す平面
図、第５図（ｂ）は側面図、第６図は第３実施例の手先
部を示す平面、側面、正面図、第７図（ａ）は第４実施
例を示しグリッパを示す平面図、第７図（ｂ）は側面図
、第８図は乃至第１３図は押付は力の実験結果を示す図
、第１４図は第５実施例を示し力制御装置を示すブロッ
ク図、第１５図は第６実施例の力＃ｑｍ装置の一部を示
すブロック図、第１６図は第５実施例の手先部を示す側
面図、第１７ｖ！Ｊは２次元平面のワークの例を示す斜
視図、第１８図（ａ）は手先部を示す側面図、第１８図
（ｂ）ワークに対するロール角が９０°ずれている場合
の手先部を示す側面図、第１９図はワークの断面形状を
示す側面図、第２０図乃至第２３図は実験結果を示し１
１２０図は時間とグラインダ押付は力の関係を示す線図
、第２１図は時間とピッチ角の関係を示す線図、第２２
図は時間と砥石先端部軌跡の関係を示す線図である。 １・・・力制御ロボット ３・・・手先部 ５・・・グラインダ ７・・・ワーク（被加工部材） １１・・・６軸力トルクセンサ １９．４１・・力制御装置 ３５・・座標変換器 ３７・・・カウンタウェイト ４５・・・センサ座標変換器 ４７・・・グラインダ座標変換器 ４９・グラインダ位置座標変換器 ５１・・駆動用座標変換器 ５３・・・逆座標変換器 ５５・・・ワーク形状データ記憶装置

Claims (7)

【特許請求の範囲】
1. （１）ロボットの手先部に取り付けられた手先効果器の
   被加工部材への押付け力を検出して、検出された押付け
   力が目標押付け力となるようにロボットを制御する力制
   御装置であって、 前記手先効果器へ付与された押付け力の反力を検出する
   検出手段と、この検出手段の検出位置を手先効果器の重
   心へ移動して、この重心回りのモーメントを求めて、手
   先効果器の被加工部材への押付け力を演算する演算手段
   とを設けたことを特徴とする力制御装置。
2. （２）手先部に取り付けられた手先効果器の被加工部材
   への押付け力を検出して、この検出された押付け力が目
   標押付け力となるように制御される力制御ロボットであ
   って、 前記手先効果器へ付与された押付け力の反力を検出する
   検出手段の検出結果から、前記手先効果器の重心回りの
   モーメントを求めて、この重心回りのモーメントから前
   記押付け力の反力を求めて前記検出手段の結果を補正す
   る補正手段を設けたことを特徴とする力制御ロボット。
3. （３）前記補正手段が、検出手段の検出位置を手先効果
   器の重心へ移動して、この重心回りのモーメントを求め
   て、手先効果器の被加工部材への押付け力を演算する演
   算手段である請求項（２）記載の力制御ロボット。
4. （４）前記補正手段が、手先効果器にカウンタウェイト
   を取り付けることにより、手先効果器の重心位置と前記
   検出手段の検出位置とを一致させたことである請求項（
   ２）記載の力制御ロボット。
5. （５）ロボットの手先部に取り付けられた手先効果器の
   被加工部材への押付け力を検出して、検出された押付け
   力が目標押付け力となるように力制御ロボットを制御す
   る制御装置であって、前記手先効果器へ付与された押付
   け力の反力を検出手段で検出し、この検出手段の結果が
   所定の値に対して変化しないように手先効果器の姿勢を
   変化させる姿勢変化手段と、前記手先効果器の固定され
   て方向に、前記手先効果器の押付けや移動がなされる駆
   動手段とを設けたことを特徴とする力制御装置。
6. （６）前記手先効果器の姿勢制御の回転中心を、前記手
   先効果器と被加工部材の接触点付近としたことを特徴と
   する請求項（５）記載の力制御装置。
7. （７）ロボットの手先部に取り付けられた手先効果器の
   被加工部材への押付け力を検出して、検出された押付け
   力が目標押付け力となるようにロボットを制御する力制
   御装置であって、前記手先効果器の移動軌跡から被加工
   部材の形状を記憶する被加工部材形状記憶手段と、前記
   被加工部材の仕上形状を記憶する仕上形状記憶手段と、
   前記被加工部材形状記憶手段と前記仕上形状記憶手段に
   記憶されている形状から手先効果器の目標位置と目標姿
   勢を演算する演算手段とを備えたことを特徴とする力制
   御装置。