JPH0215956A - 倣い制御ロボット - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明は、ロボットの手先に設けた工具の被加工物（ワ
ーク）に対する反力を所望としながら作業する倣い制御
ロボットに関する。

［従来の技術］ 従来より、パリ取り、型磨き、トリミング、パフ仕上等
、アームに取り付けられた回転砥石等の工具の被加工物
（以下、ワークという）に対する反力を調整しながら、
作業を行うロボットが要求されている。

このようなロボットの−・つとして、ロボットの手先と
工具との間に、所定の軸方向に加えられる力と該軸方向
の変位量との間の関係が予め明らかである変位量−力変
換機構を設けるとともに、工具に対するワークの反力を
測定する力センサを備え、この力センサによって測定さ
れた反力が所定の値となるように、アームを微調整する
ものが提案されている（第３回日本ロボット学会学術講
演会　講演要旨集　３２０９　ｒ軽量多関節アームの力
制御方式」昭和６０年１１月２８．２９．３０日等）。

上記変位量−力変換機構は、−コンプライアンス機構と
も呼ばれる機構であり、バネ、ゴム等の弾性体により構
成される。そして、所定軸方向の変位量を調整すること
により、工具からワークに加えられる力を調整すること
ができる。

すなわち、上記従来のロボットでは、予め教示等によっ
て記憶された位置データにより得られる標準軌跡にそっ
て手先を移動させながら、手先に取り付けられた力セン
サから工具に対するワークからの反力を測定する。

そして、質点の運動方程式にこの測定された反力の値を
代入することによって、実際の手先位置と所望の反力と
なる手先位置との位置偏差量ΔＸを算出する。

さらに、前回の目標位置Ｘ１−１に前記Δｘ１を加えて
、次の手先の目標位置Ｘｉを求め、その位置になるよう
にアームを移動する。

この動作を繰り返し実行することにより、工具のワーク
に作用する力を所定値に制御しようとする。

［発明の解決しようとする課題］ 上述のように、従来は、目標位置Ｘｔの計算をＸ　ｉ　
”　Ｘ　ｉ　−１＋ΔＸｉ　という式で行っている。

しかし、力センサから反力を入力した位置と前回の目標
位置Ｘ＋−＋　とは、一般に一致しない。

これは、力センサによって反力を測定し、その測定結果
に基づいて、手先位置・姿勢の補正信号は数十ｍ５ｅｃ
毎に出力するが、実際にアームが移動し手先が目標位置
・姿勢となるまでに数百ｍ５ｅｃを要し、このためアー
ムの位置・姿勢の新たな補正信号が、アームが目標位置
・姿勢に達する前に出力されることによる。

この様子を第１４図に示す。

第１４図は、段差を有するワークを従来のロボットで加
工した際の手先の動きを示す図であり、ロボットの手先
の動きを実線で、手先の目標値を破線で、所望の反力と
なる理想的な手先の位置を一点鎖線で示している。また
、ロボットの手先は図の左方から右方に向かってワーク
を加工していく。

第１４図（ａ）は、上述のように力センサからの出力に
相当するコンプライアンス機構の変位量をそのまま、位
置偏差量ΔＸとして使用した場合である。

この時の手先の動きを順を追ってみていくと、Ｎ＝１の
時点までは、手先とワークとの間隔は一定であり、工具
に対するワークの反力は一定である。

Ｎ＝２の時点まで手先が移動すると、手先とワークとの
間隔が所定以上となってしまい、ワークに対する押付力
は低下してしまう。そのため、理想位置と実際の手先の
位置との差を前回（Ｎ＝２）の目標値に加えて、次回（
Ｎ＝３）の目標値とする。

Ｎ＝３の時点においても、手先とワークとの間隔は所定
以上であり、次回（Ｎ＝４）の目標ＩＱを補正する必要
がある。ところが、Ｎ＝３の時点では、手先は未だ目標
値に達しでいない。したがって、時点Ｎ＝３までに、解
消されていない位置偏差量が次回（Ｎ＝４）の目標値に
蓄積される。

以下の時点においても、同様に前回までに解消されない
位置偏差量が次々と蓄積されるため、第１４図（ａ）の
ように制御系はオーバーシュートし発振してしまう。

このような問題をさけるために、力センサからの出力に
相当するコンプライアンス機構の変位量に１より小さい
係数（例えば、１７３〜１／４）をかけたものを位置偏
差量として用いることが考えられる。

しかし、この場合には、第１４図（ｂ）に示すように、
オーバーシュートはおこらな１１ものの、工具の押付力
が所望の値となるのに時間がかかる。例えば、本図の例
では１０ステツプかかっている。

この工具の押付力が所望となっていない範囲では、当然
のことであるが、作業精度が低下する。

また、上記のように目標値の出力から実際にアームが目
標値に移動するまでの時間遅れによる問題を解決するた
めに、アームが目標値に達してから次の補正信号を出力
することも考えられる。

しかし、この場合には、前述のように目標値の出力から
アームの移動が完了するまでの時間が数百ｍ５ｅｃであ
ることから、制御の各ステップに要する時間も数百ｍ５
ｅｃとなり、作業速度が非常に遅くなり、実用的ではな
い。

また、前述した従来のロボットは、教示されたデータに
基づいた標準軌跡上を動作するのであるが、上記のよう
に位置の補正が必要となった場合には、挟持されたデー
タとは無関係に位置の補正を行うために、標準軌跡を大
きく外れてしまうことも考えられる。

本発明は、工具に対するワークの反力が所望の量となる
正確な軌跡目標位置を指示し、応答性がよく高精度に工
具の位置・姿勢を制御できる倣い制御ロボットを提供す
ることを課題とする。

［課題を解決するための手段］ 上記課題を解決するためになされた本発明の要旨は、第
１図に例示する如く、 手先Ｒ１に、工具Ｒ２を、所定の軸方向に加えられる力
と該軸方向の変位量との間の関係が予め明らかである変
位量−力変換機構Ｒ３を介して設けると共に、 予め記憶された位置データにより得られる標準軌跡上を
動作し、前記工具Ｒ２を予め定められた設定力で被加工
物Ｗに作用させる倣い制御ロボットであって、 前記手先Ｒ１の現在位置及び／又は現在姿勢を測定する
現在状態測定手段Ｒ４と、 該測定された現在位置及び／又は現在姿勢において、被
加工物Ｗより工具Ｒ２に加えられる反力を測定する反力
測定手段Ｒ５と、 該測定された反力と、前記測定された現在位置及び／又
は現在姿勢における予め定められた設定力との差を求め
ると共に、原基を前記工具Ｒ２の位置変位量に換算する
変位量換算手段Ｒ６と、該換算された変位量と前記手先
Ｒ１の現在位置及び／又は現在姿勢とから、現在の手先
Ｒ１の理想位置及び／又は理想姿勢を算出する理想状態
算出手段Ｒ７と、 該算出された手先Ｒ１の理想位置及び／又は理想姿勢と
、手先Ｒ１が次に移動する標準軌跡上の将来位置及び／
又は将来姿勢とを比較して、手先Ｒ１の位置及び／又は
姿勢の補正量を算出する補正量算出手段Ｒ８と、 該算出された補正量に基づいて、前記手先Ｒ１の位置及
び／又は姿勢を制御するロボット制御手段Ｒ９と を備え、 工具Ｒ２の被加工物Ｗに対する反力が予め定められた設
定力となるようリアルタイムで補正しながら作業するこ
とを特徴とする倣い制御ロボットにある。

ここで、予め記憶された位置データにより得られる標準
軌跡とは、例えば、予めティーチング動作によって記憶
された位置データを補間して得られる軌跡や、予めプロ
グラミング等によって定められた軌跡である。

上記理想状態算出手段Ｒ７で使用される理想状態とは、
被加工物Ｗより工具Ｒ２に加えられる反力と、予め設定
された反力とが等しい状態である。

また、上記予め定められた設定力は、一定量でもよいが
、作業位置毎に定められた量でもよい。

［作用］ 本発明の倣い制御ロボットは、 先ず、反力測定手段Ｒ５によって、工具Ｒ２に対する被
加工物（ワーク）Ｗの反力を測定し、同時に、現在状態
測定手段Ｒ４によって、手先Ｒ１の現在の位置会姿勢を
測定する。

続いて、変位量換算手段Ｒ６によって、測定されたワー
クＷからの反力と予め定められた設定力との差を工具Ｒ
２の位置変位量に換算する。

次ぎに、理想状態算出手段Ｒ７にて、現在状態測定手段
Ｒ４にて測定された手先Ｒ１の現在の状態と上記変位量
換算手段Ｒ６にて換算された変位量とを加算することに
より、反力が設定力と等しくなる手先Ｒ１の位置・姿勢
を理想状態として算出する。

さらに、補正量算出手段Ｒ８にて、上記のようにして算
出された手先Ｒ１の理想状態と、次ぎに手先Ｒ１の移動
する標準軌跡上の将来の位置・姿勢とを比較して、手先
Ｒ１の現在状態からの補正量を算出する。

そして、ロボット制御手段Ｒ９にて、上記算出された補
正量に基づいて手先Ｒ１の位置令姿勢を制御する。

以上のように、本発明の倣い制御ロボットは、現在位置
に補正量を加えて新たな目標位置を算出する際に、前回
の目標位置を全く使用していない。

そのため、手先Ｒ１の移動に時間がかかり、手先Ｒ１の
位置・Ｗｅが未だ前回の目標位置に達していないときに
、新たな目標位置を出力しても、新たな目標位置に時間
遅れによる誤差の蓄積等がない。

したがって、作業スピードを低下させることなく、高い
応答性と、正確な倣いが行える。

［実施例］ 本発明の一実施例を説明する。

本実施例は、工具として回転砥石を用い、工具のワーク
からの反力を一定にしながら、ワークの研削あるいは研
石作業する６軸多関節型のロボットに関する。

第２図の斜視図に、このロボット１０の構成を示す。

このロボット１０は、該ロボット１０の動作制御部であ
るロボ・ントコントローラ１２、該ロボ・ントコントロ
ーラ１２に設けられ作業者がロボット１０に仕事を教示
するティーチングボックス１４、ロボ・ントコントロー
ラ１２からの信号によって動作する６軸の動作部１６、
動作部１６の手先１８にコンプライアンス機構１９を介
して設けられた工具２０及び工具２０に対するワークＷ
の反力を検出するセンサ２２、該センサ２２からの信号
を処理するセンサコントローラ２４を備える。又、ロボ
ットコントローラ１２は人出力のためのターミナルＴ１
　プリンターＰを備える。

以下に、この日ボ・ント１０の各部について説明する。

まず、ロボッ）１０の動作部１６を第３図により詳細に
説明する。第３図（ａ）は平面図、（ｂ）は正面図、（
ｃ）は左側面図である。

この動作部１６は、第１軸から第６軸までの６個の軸を
有する。

そして、この内、第１軸から第３軸によって手先１日の
ワークＷに対する位置が制御され、第４軸から第６軸に
よって手先１８のワークに対する姿勢が制御される。

また、各軸には、各々の軸を中心として、回転駆動する
モータ、その回転を停止させるブレーキ、その回転量を
測定するエンコーダが設けられている。

すなわち、第１軸には第１軸用モータ３０．第１軸用ブ
レーキ３２．第１軸用エンコーダ３４が、第２軸には第
２軸用モータ４０．第２軸用ブレーキ４２゜第２軸用エ
ンコーダ４４が、第３軸には第３軸用モータ５０．第３
軸用ブレーキ５２．第３軸用エンコーダ５４が、第４軸
には第４軸用モータ６０．第４軸用ブレーキ６２．第４
軸用エンコーダ６４が、第５軸には第５軸用モータ７０
．第５軸用ブレーキ７２．第５軸用エンコーダ７４が、
第６軸には第６軸用モータ８０、第６軸用ブレーキ８２
．第６軸用エンコーダ８４が、各々設けられている。

なお、第２軸、第３軸は、各々そのモータ４０，５０の
回転運動をポールネジフィード９０　、９２により直線
運動に変換し、これを平行リンク機構の軸回転運動に変
換することにより、駆動される。他の軸は、その軸のモ
ータの回転をトルクチューブで伝え、平歯車、ハスバ歯
車等を用いて減速して駆動される。

つぎに、動作部１６の手先１８を第４図の斜視図により
説明する。

この手先１８には、ワークＷを加工する回転砥石２０と
、回転砥石２０と図示されない歯車によって連結され回
転砥石２０を回転させるモータ１００と、バネ１０２及
びダンパ１０４とから構成され図中の変位方向のみに、
ある反力をもってスライドするコンプライアンス機構１
９と、このコンプライアンス機構１９と手先１８との間
に設けられ回転砥石２０に加えられるワークＷの反力を
測定する力センサ２２とが配置されている。なお、この
コンプライアンス機構１９の変位量と力との関係は予め
既知である。

ここで、力センサ２２は、コンプライアンス機構１９の
変位軸方向、及び変位軸に垂直であり、かつ、互いに垂
直な２本の軸方向の各部と、この３つの軸回りのトルク
を検出する歪ゲージを用いた通常の力センサを使用する
。

この力センサ２２によ、って検出されるカデータＦｓｅ
ｎ（Ｋ）１１）は、力センサを基準位置とするセンサ座
標系０−Ｘｓ−Ｙｓ−Ｚｓの各軸方向の力および各軸回
りのモーメントであり、以下のような６次元のベクトル
で表される。

Ｆｓｅｎ　＝（ＦｘｓｓＦｖｈ＋Ｆｚｓ＋ＭＮｘｓｓ＋
ＭＮｖｓｓＭＮｚｓ）”として出力される。

ここで、Ｔは転置行列を示し、Ｆは力、ＭＮはモーメン
ト、各添え字Ｘｓ、Ｙｓ、Ｚｓは各軸を示す。

また、本実施例では、センサ座標系のＺｓ軸とコンプラ
イアンス機構１９の変位軸方向とは、一致させである。

続いて、第５図によって、６軸の動作部１６を制御する
とともにティーチング装置としても働くロボットコント
ローラ１２の構成を説明する。

このロボットコントローラ１２は、周知の第ｔｃｐｕ３
００、第２ＣＰＵ３０１．ＲＯＭ３０２．ＲＡＭ３０４
．バックアツプＲＡＭ３０６等を中心に、バス３０８に
より相互に接続された入出力ボートと共に算術論理演算
回路として構成されている。

第１ＣＰＵ３００．第２ＣＰＵ３０１等と共にロボット
コントローラ１２を構成する人出力ボートとしては、バ
ブルメモリ３１０にデータを記憶したり読み出すといっ
た処理を行うバブルメモリインターフェイス３１２、Ｃ
ＲＴデイスプレィとキーボードとを備えたターミナルＴ
とデータの授受を行うターミナル人出力ボート３１４、
プリンタＰにデータを出力するブリンク用出カポ−）　
３１６、ティーチングボックス１４の操作状態を人力す
るティーチングボックス人力ボート３１８、力センサ２
２の出力する電気信号をセンサコントローラ２４を介し
て人力し第２　ＣＰＵ３０１に対して割り込みを発生さ
せるセンサ人力ボート２３０、及び上述した６軸（第１
軸、第２軸、第３軸、第４軸、第５軸、第６軸）用のサ
ーボ回路３２０，３２２、３２４．３２６．３２８．３
３０が備えられている。

各軸周サーボ回路３２０ないし３３０は、何れも個々に
ＣＰＵを備えたインテリジェントタイプのものであり、
第１ＣＰＵ３００からバス３０８を介して、回転砥石２
０の作業目標位置及び目標姿勢のデータが与えられると
、第１軸ないし第６軸用エンコーダ３４．４４．５４，
６４．７４，８４から各軸の制御量を読み込み、第１軸
ないし第６軸用ドライバ３３２，３３４，３３６，３３
８゜３４０．３４２を介して第１軸ないし第６軸用モー
タ３０．４０，５０，６０，７０．８０を駆動し、回転
砥石２０の先端を目標位置まで移送し、更に目標姿勢に
同時に制御する。尚、第１軸ないし第６軸用サーボ回路
３２０゜３２２．３２４．３２６．３２８．３３０は、
各々第１軸ないし第６軸用ブレーキ３２，４２．５２．
６２，７２．８２も適時制御して、非常停止などでサー
ボ電源（モータ電Ｒ）が切られてもアームが自重で動作
又は、下へ垂れることを防いでいる。

このロボットコントローラ１２の動作を説明する。

このロボ・ントコントローラ１２の制御は第６図に示す
ように大きく分けて２つの部分からなる。

１つは、予めティーチングにより教示されたデータを補
間して得られた標準軌跡データＰＴｐａｔｈ（Ｋ）ＩＩ
）を他方のＣＰＬＩに渡し、そこで補正量を加味された
目標値ＰＴｍｏｄ（に旧）を再度受は取り、その目標値
ＰＴｍｏｄ（Ｋ旧）を基に実際の各部の目標値θｈｏｓ
ｔ（Ｋ旧）を算出し動作制御部（サーボ回路）に出力す
る部分であり、第１ＣＰＵ３００が受は持つ。

他の一つは、力センサ２２によって得られたカデータと
、その計測時における手先１８の姿勢データ及び第１Ｃ
ＰＵから渡されたＰＴｐａｔｈ（に旧）とから、後述の
補正量ΔＰＴ（に旧）を算出し、更にオフセット量を加
え目標値ＰＴｍｏｄ（に旧）を算出する部分であり、第
２ＣＰＵ３０１が受は持つ。

以下に、第１ＣＰＵ３００が受は持つ制御部分について
述べる。

まず、ロボット１０を再生動作させる前に従来のロボッ
トと同じくティーチングボックス１４を用いて、ワーク
Ｗに対して工具の作業点の通過すべき希望位置及び姿勢
をロボット１０に教示し、前述のＲＡＭ３０４中に確保
されたティーチング点メモリ４００に格納する。

このティーチング点データは、ロボットの基準座標から
工具の作業点までの埴であり、このｍ番目の教示データ
は次に示すマトリックスで表される。

・・・（１） ここで、 ＮＸ、ＮＹ、ＮＺは、ノーマルベクトルの各要素、ｏ　
ｘ、　ｏ　ｖ、　ｏ　ｚは、オリエントベクトルの各要
素、ＡＸ、Ａ’／、ＡＺは、アプローチベクトルの各要
素、Ｘ、Ｙ、Ｚ　は、教示点の位置ベクトルの各要素を
表すものであって、これらは何れもロボットの動作部１
６の動作の基準点を原点とするロボット基準座標系０−
ＸＹＺ　　（第２図参照）であられされたデータである
。

又、ノーマルベクトル、オリエントベクトル、アプロー
チベクトルとは、各々直交するベクトルであって、上記
マトリックスで表される教示点での工具の加工の方向を
表すものである。尚、以下の説明に用いられる各点を示
す変数もこのマトリックスと同様の形式である。

上記の教示が終了すると、作業開始信号により、ロボッ
ト１０は作業を開始する。

先ず、ティーチング点メモリ４００から出力されたＫＨ
Ｏ＠目のティーチング点データＰＴｔｃｈ（に）１０）
を軌跡補間回路４０２で補間して、回転砥石２０の作業
点の目標点データ（作業点目標データ）ＰＴｐａｔｈ（
にＨｌ）を算出する。この目標点データＰ　Ｔ　ｐａｔ
ｈ　（Ｋ　Ｈｌ）が、工具２０の移動すべき標準軌跡と
なる。

次いで、この目標点データＰＴｐａｔｈ（ＫＨＩ）を第
２ＣＰＵに渡し、第２ＣＰＵにて補正され新たな目標点
データとなった作業点（１正デ一タＰＴｍｏｄ（ＫＨ）
を受は取り、ジヨイント逆変換回路４０４に人力する。

ジヨイント逆変換回路４０４は、作業点１＠正データＰ
Ｔｍｏｄ（ＫＨＩ）を各軸の移動角度θｈｏｓｔ（ＫＨ
Ｉ）に変換し、各軸周サーボ回路３２０〜３３０に出力
し、各軸周ドライバー回路３３２〜３４２を介して各軸
周モータ３０．４０．５０．６０．Ｔｏ、８０を駆動し
て、回転砥石２０の作業点を、作業点修正データＰＴｍ
ｏｄ（に旧）に移動させるよう制御する。

この回転砥石２０．ワークＷ　、　ＰＴｐａｔｈ（に旧
）、ΔＰＴ（に）１１）、ＰＴｍｏｄ（ｋ旧）等の間の
位置関係を第７図に示す。

次に、作業点修正データＰＴｍｏｄ（に旧）を求める第
２ＣＰＵの部分について説明する。

この第２ＣＰＵ３０１は、第１ＣＰＵ３００と並列的に
動作しており、また、第１ＣＰＵが第２ＣＰＵにＰＴｐ
ａｔｈ（に旧）を渡す間隔と第２ＣＰＵが第１ＣＰ（Ｊ
からＰＴｐａｔｈ（Ｋ）ＩＩ）を受は取り、ＰＴｍｏｄ
（に旧）を算出し、第１ｃｐｕに渡す間隔は同一である
。

第２ＣＰＵでは第１ｃｐｕからＰＴｐａｔｈ（に旧）を
受は取ると処理が開始される。

先ず、各軸周エンコーダ３４，４４．５４．６４．７４
．８４からジョイント角データθｅｎｃ　（に旧）を人
力し、ジヨイント順変換回路４０６でフランジ端データ
ＰＦ（ＫＨｌ）に変換する。すなわち、以下の式を実行
する。

ＰＦ（ＫＨ）＝へ［θｅｎｃ（に旧）］−（２）ここで
、Ａはジヨイント順変換 ［］はベペクトを表す。

次いで、工具作業点座標◆センサ座標演算回路４０８で
、このフランジ端データＰＦ（に旧）からセンサ端デー
タＰＳＣＫ旧）および工具作業点実データＰＴｅｎｃ（
に旧）を以下の式（３）、（４）により求める。

ＰＳ（Ｋ）１１）　＝ＰＦ（に旧）　φ［１ｔ”ｓ”（
３）ＰＴｅｎｃ（ＫＨ１）＝ＰＦ（に８１）　φｎ　ｔ
”ｒ　　”（４）ここでｎｒ＋ｓはフランジ端に対する
センサ端の位置を示す位置・姿勢マトリックスであり、
ｎｒ→Ｔはフランジ端に対する工具作業点の位置を示す
位置・Ｗｅマトリックスである。

なお、この変換は、力センサ２２によって検出された各
軸方向の力、モーメントを工具２０位置における力、モ
ーメントに換算するために行うものである。

一方、上記のようにフランジ端データＰＦ（Ｋ）１１）
を人力すると同時に、力センサ２２からセンサコントロ
ーラ２４を介してカデータＦｓｅｎ（Ｋｌ（１）を人力
する。

このカデータＦｓｅｎ（ｋ）ＩＩ）は、前述のように力
センサを基準位置とするセンサ座標０−Ｘｓ−Ｖｓ−Ｚ
ｓの各軸方向の力および各軸回りのモーメントであり、
６次元のベクトルで表される。

このカデータＦｓｅｎ（に旧）は、工具である回転砥石
２０の重量Ｍｔｏｏｌ　およびワークＷからの反力Ｆｗ
の合成されたものとなる。なお、ここでは、以下の説明
のために、反力Ｆｗが予めＲＡＭ３０４中の目標カメモ
リ４１２に記憶される目標値Ｆ２０と等しいとする。

ところで、ロボット１０が自由空間を手先１８の姿勢を
変えながら動作すると、工具姿勢も変化しそれにともな
い、力センサ２２の各軸毎に受ける工具重量も変化する
。

ここでは、説明を容易にするため、工具２０の補正方向
をコンプライアンス機構１９のスライド軸方向、すなわ
ち変位方向（第４図の矢印方向）に限定して、工具姿勢
の変化に伴う工具重量変化を説明する。

また、前述のように、上記スライド軸方向は、センサ座
標系のＺｓ軸とコンプライアンス機構１９の変位方向を
一致している。

したがって、カデータＦｓｅｎのＺｓ軸方向成分Ｆ２Ｓ
は、第８図のように、工具重量のＺｓ軸方向成分Ｆｔ０
０１　とワークＷの反力Ｆｗとの差となる。

第８図のように、補正方向（Ｚｓ軸）と重力方向（図中
−点鎖線）とのなす角度がθであると、Ｆｔ００１＝Ｍ
ｔ００１ＸＣＯ８θとなり、工具２０に対するワークＷ
の反力をＦ％、Ｉとすると、カデータＦｓｅｎのＺｓ軸
方向成分Ｆｚｓは、 Ｆｚｓ＝Ｍｔｏｏ　ｌ　Ｘ　ｃｏｓθ−Ｆ−：Ｆｔｏｏ
ｌ−Ｆｗ　−（５） という関係になる。

第６図の工具重量演算回路４１０は、この各軸毎の工具
重量を算出するものである。

ここでは、前述のセンサ座標データＰＳ（に旧）から、
上記ＣＯ８θを演算し、予め記憶されている工具重量Ｍ
ｔｏｏｌを用いて、補正方向（Ｚｓ軸）の工具重量Ｆｔ
ｏｏｌを求める。

ロボット座標系のＸ−Ｙ平面１６が地面に水平におかれ
ている場合には、ロボ・ント基準座標系０−Ｘ−Ｙ−２
＜第２図）のＺ軸方向は重力方向と一致するので、ＣＯ
Ｓθはセンサ座標データＰＳ（に旧）の要素を取り出せ
ば求まる。

すなわち、ＰＳ（に旧）は、下記のようにロボット基準
座標ｏ−ｘ−ｖ−ｚからみたセンサ座標系の相対位置・
姿勢を表している。

・・・（６） よって、センサ座標系Ｚｓ軸とロボット座標系Ｚ軸との
なす角は ｃｏｓθ＝ＺＳ２　　・・・（７） また、ロボット座標系のＸ−Ｙ平面が水平におかれてい
ない場合は、ロボット基準座標系からみた重力方向ベク
トルＧ５　すなわち（Ｘ、Ｙ、Ｚ）＝（０，０，１）と
上記ＰＳ（ＫＨＩ）のＺｓベクトル（Ｚｓｘ、Ｚｓｙ、
Ｚｓｚ　）との内積で求まる。

次に、第９図を用いて、目標力Ｆ２０よりΔＦ大きい反
力が加わったときの、ΔＦの求め方について説明する。

なお、図中において、ｄＦｔｏ。１、ｄＦｌ、ｄΔＦは
、各々力Ｆｔｏｏｌ、　Ｆｗ、ΔＦに対応するコンプラ
イアンス機構１９の変位量である。

また、左側に描かれた手先１８は、ワークＷからの反力
が目標力Ｆ２０に等しい場合であり、右側に描かれた手
先１８は、この手先１８が図の右側へ移動したところワ
ークＷと手先１８との距離がｄｏだけ短くなり、そのた
めにワークＷからの反力Ｆ勤１ΔＦだけ増加した場合の
図である。

上述のようにワーク反力がＦｗであり、目標力Ｆ２０に
等しい時は、式（５）に表される関係であるが、ΔＦが
加わると、Ｆ２Ｓは次のようになる。

Ｆｚｓ＝Ｆｔｏｏｌ−（Ｆｗ＋ΔＦ　）−（９）よって
、Ｆｚｓ、Ｆｔｏｏ’ｌ　、Ｆ−は既に述べたように既
知なためΔＦは前式より求まる。

ΔＦ　＝−Ｆｚｓ＋Ｆｔｏｏ１−Ｆ−・・・（ｌＯ）こ
こで、補正したい力の量ΔＦが求まったので、この力を
０とするための位置偏差量Δｄを偏差量変換回路４１４
にて求める。

センサから先の工具重量Ｍｔｏｏｌ　　（以下、Ｍと略
して記す）を質点に見立てると、一般に次に示す運動方
程式により関係づけられる。

ΔＦ＝ＭＤ＋ｃＤ＋にΔｄ・・・（１１）ここで、Ｄは
センサからみたＺｓ軸方向の工具位置であり、ｃ、　　
Ｋはそれぞれコンプライアンス機構１９の粘性定数、バ
ネ定数を表している。

よって、Δｄは上記式を変形して、次のように表される
。

ここで、これらの演算は離散系である周期毎に実行する
ため、その周期をτとし、１周期前のΔｄをΔｄ　（Ｋ
）１１−１）、２周期前のΔｄをΔｄ　（ＭＨＩ−２）
として、これらΔｄ（にＨｌ−１）、　　Δｄ（に）Ｉ
ｆ−２）を用いて、Ｄ、　　Ｄを近似することにより、
Δｄ（Ｋ旧）を次式のように近似する。

Δｄ（に旧）＝−［ΔＦ 一−（Ｄ（に旧−１）−〇　（Ｋ旧））コ　・・・（１
３）τ である。

ここで、Ｍ、ｃ’；　０とおくと、Δｄ（Ｋｌｌｌ）＝
ΔＦ／にとなる。

これにより、慣性および粘性項に関しては、１周朋遅れ
ることになるが、１周期τを数十ｍ５ｅｃとすれば、Δ
ｄ　（ＭＨＩ）坤Δｄ（に旧−１）であるので、影響を
小さくしつつ、ΔＦに対する位置偏差量Δｄ（に旧）が
求まる。

また、静的なレベルで制御したり、物理的にＭ。

Ｃの定数が非常に小さい場合には、両者を無視し、この
計算から除外してもよい。

なお、本実施例では、工具位置りを近似するのに、位置
偏差量Δｄ（に旧）の１周期前Δｄ（に旧−１）および
２周期前Δｄ（に旧−２）を用いたが、１周期先Δｄ　
（ＫＨ１＋１）および２周期先Δｄ（に）１１＋２）を
用いて次式により、同様に工具位置を近似することも可
能である。

τ 上記のようにして求められたΔＦに対する位置偏差量Δ
ｄ（に旧）とセンサ座標データＰＳ（に旧）とから、セ
ンサ２２がどこにあれば、目標力Ｆｚｏ（ＫＨＩ）に一
致する反力を得られるかを理想位置演算回路４１６にて
求める。

この演算は次式による。

ＰＳａｉｍ　（ＫＨｌ）＝ＰＳ（に旧’）−Ｄ（ＫＨＩ
）−（１４）この理想位置ＰＳａｉｍ（に旧）は、セン
サ座標系のデータであるので、工具作業点変換回路４１
８にて以下の式を用い、このデータをロボット基準座標
系のデータＰＴａｉｍ（Ｋ旧）に変換する。

ＰＴａｉｍ（ｋ旧）＝ＰＳａ　ｉｍ（Ｋ旧）　◆ｎｓ→
ｒ・・・（１５）ここで、ｍ５−４−ｖはセンサ座標に
対する工具作業点座標の位置・姿勢を表すマトリックス
である。

この理想位置ＰＴａｉｍ（に旧）と前述の軌跡補間回路
４０２によって得られた作業点目標データＰＴｐａｔｈ
（Ｋ旧）との差Ｒ−ｏｆｓ（ＫＨＩ）を次式により求め
る。

Ｒ−ｏｆｓ（に旧）＝ＰＴｐａｔｈ（に旧）−’　舎Ｐ
Ｔａｉｍ（ＭＨＩ）・・・（１６） これは、教示時における工具進行方向（通常は補正方向
に対し直角方向）のスピードを維持しつつ、補正方向の
みの位置を修正させるためである。

なぜなら、第７図に示すように、指令値ＰＴｐａｔｈ（
Ｋｌ）に対して、実際の工具位置は遅れており、その遅
れた位置における工具作業点理想位置ＰＴａ　ｉ　ｍ（
Ｋｌ）は同様に遅れ、そのまま（１正しては、速度が変
化してしまうからである。

次に、補正要素選択回路４２０にて、力センサ２２が計
測した要素に対応するヌトリ・シクス成分のみにＲ−ｏ
ｆｓ（に旧）のデータを用いて、補正量ΔＰＴ（ＭＨＩ
）を以下のように求める。

・・・（１７） ここで、２はＲ−ｏｆｓ（に旧）の同一要素Ｚである。

この様にして求めた補正量ΔＰＴ（Ｋｌ）は、異常検出
回路４２２に人力され、所定値βと比較される。

ΔＰＴ（にＨｌ）の大きさＩＩΔＰＴ（ＫＨｌ）　１１
がβより大きいときはロボ・ントが暴走している可能性
があるので、例えば動作停止という非常処理を行う。逆
に小さい時はΔＰＴ（に旧）を用いて次の作業点１１！
正データＰＴρｍｏｄ　（に旧）を求める。

ＰＴｐｍｏｄ（にＨｌ） ＝ＰＴｐａｔｈ（ＫＨＩ）　φΔＰＴ（に旧）−（１Ｂ
）ついで、目標オフセット回路４２４の目標からのオフ
セラ）　０ｆｓ（に旧）を用いて、実際の作業点目標デ
ータＰＴｍｏｄ（に旧）とする。

ＰＴｍｏｄ（に旧） ＝ＰＴｐｍｏｄ（Ｋｌ）　◆０ｆｓ（ＫＨＩ）・・・（
１９）ここで、ＰＴｐｍｏｄ（Ｋｌ）は初段目標工具端
座標である。

そして、この作業点目標データＰＴｍｏｄ（に旧）は、
第１　ＣＰＵ３００に渡され、第２　ＣＰＵ３０１の処
理を終わる。

上述の処理を第１ＣＰＵ３００．第２ＣＰＵ３０１で実
行する流れ図を第１０図、第１１図に示す。

本実施例に用いているロボットコントローラ１２は、既
に述べたように、第６図に示す２つのＣＰＵ３００．３
０１を有し、この２つのＣＰＵ３００．３０１が並列的
に処理を行っている。第１Ｏ図に第１ｃＰＵ３００の動
作を示す流れ図を示し、第１１図に第２ＣＰＵ３０１の
動作を示す流れ図を示す。

作業開始信号により、ロボット１０が作業を開始すると
、第ｔｃｐｕは、以下のステップを順次実行する。

ステップ５００：メモリに記憶されているティーチング
点データＰＴｔｃｈ（Ｋｌ）を、補間して作業点目標デ
ータＰＴｐａｔｈ（に旧）を算出する。

ステップ５０２：作業点目標データＰＴｐａｔｈ（ＭＨ
Ｉ）とを、第２　ＣＰＵに送り、第２　ＣＰＵに１１正
された作業点目標データＰＴｍｏｄ（に旧）を、要求す
る。

ステップ５０４：第２　ＣＰＵから修正された作業点目
標データＰＴｍｏｄ　（に旧）を人力する。

ステ・シブ５０６：目標点データＰＴｍｏｄ（ＭＨＩ）
ヲ各軸の移動角度θｈｏｓｔ（に旧）に変換する。

ステップ５０８二〇ｈｏｓｔ（Ｋｌ（ｌ）を、各軸周サ
ーボ回路３２０〜３３０に出力する。

ステップ５１０：に旧を、１だけインクリメントし、上
記ステップ５００以降を繰り返して実行する。

第２ＣＰＵ３０１は以下の処理を行う。

ステップ６００：第１　ＣＰＵ３００から作業点目標デ
ータＰＴｐａｔｈ（に旧）を人力する。

ステップ６０２：センサコントローラ２４からカデータ
Ｆｓｅｎ（に旧）を人力する。

ステップ６０４：各軸のエンコーダ３４，４４，５４，
６４゜７４．８４から、力センサ２２が計測を開始した
瞬間のジヨイント角θｅｎｃ（に旧）を人力する。

ステップ６０６〜ステップ６１８：前記各式に従って、
Ｆｔｏｏｌ、ΔＦ、Δｄ（ＫＨＩ）、ＰＳａｉｍ（ＫＨ
Ｉ）、ＰＴａｉｍ　（ＭＨＩ）、Ｒｏｆｓ（にＨｌ）、
ΔＰＴ（に旧）を算出する。

ステ・シブ６２０：補正量ΔＰＴ（に旧）を、所定量β
と比較する。

１１ΔＰＴ（Ｋｌ）１１）＝βであれば、処理はステ・
ンブ６２８の異常処理へ移行し、 １１ΔＰＴ（に旧）１１＜βであれば、処理はステップ
６２２へ移行する。

ステップ６２２，６２４　：作業点ｆｒＭ正データＰＴ
ｐｍｏｄ（Ｋｌ）及び実際の作業点修正データＰＴｍｏ
ｄ（ＭＨＩ）を前記各式によって算出する。

ステップ６２６：作業点修正データＰＴｍｏｄ（ＫＨＩ
）を第１　ＣＰＵ３００に出力する。

ステップ６２８：異常処理としてロボット１０の動作を
停止して、処理を終わる。

以上の処理を繰り返すことによって、ロボットｌＯは、
回転砥石２０のワークＷに作用する力を目標力と一致さ
せながら、作業を行っていく。

第１２図は、第１４図にて従来のロボットで説明に使用
したものと同形状のワークＷを、本実施例によって加工
した際の手先１８の動きを説明する図である。本図にお
いて、ロボットの手先１８の動きを実線で、手先１８の
目標値を破線で、所望の反力となる理想的な手先１８の
位置を一点鎖線で示している。また、ロボットの手先は
図の左方から右方に向かってワークを加工していく。

第１２図から明らかなように、本実施例のロボットは、
従来と異なり、゛オーバーシュートすることなく、速や
かにワークＷの形状を倣う。

上述の如き構成を有する本実施例は次の効果を有する。

（１）目標点を求める際に、力センサ２２からの情報と
センシング時点の回転砥石２０の位置・姿勢の情報を基
に修正量を求めるため、作業速度を低下させることなく
、高精度な作業ができる。

（２）目標点を定める際に、力センサ２２からの情報だ
けを基にせず、基準軌跡であるティーチング点を参照し
ているので暴走することがない。

（３）例えば、数十ｍ５ｅｃ毎といった細かい周期で新
たな目標点を求め、制御するので、きめ細かい制御がで
きる。

（４）目標値を算出した後の実際の工具２０の位置・姿
勢制御は、従来と同じく各軸周のサーボ回路をそのまま
利用しているため、力の制御が不要の場合には、通常の
ティーチングプレイバックロボットとして動作できる。

（５）上記のように、実際の工具２０の位置・姿勢制御
は従来と同様なので、従来のティーチングブしイバック
型ロボットコントローラに、本実施例で必要とするＣＰ
Ｕボード、インターフェイスを加えるだけで、本実施例
を構成できる。そのため、安価に力の制御を行うロボッ
トを提供できる。

（６）加工作業だけでなく、作業者が力センサ２２の先
を持ち、ロボットが持つ重量物を軽い力で運搬するとき
にその方向をコントロールする、バランサーのような使
い方もできる。

（７）補正量ΔＰＴを算出する際に、各演算に使用する
マトリックスの要素を適切なものとすることにより、所
望の軸方向のみの力を制御できる。このとき、力制御を
しない軸に対しては、位置の制御だけが行われる。

（８）工具の自重補正を行っているため、重力に影響さ
れず任意の姿勢で加工作業ができる。

なお、本実施例では、回転砥石２０の軸に対して垂直の
方向にのみスライドするコンプライアンス機構１９（第
４図参照）を使用しているが、作業の種類によっては、
第１３図の斜視図のように回転砥石２０ａの軸に平行な
方向（図中矢印方向）のみにスライドするコンプライア
ンス機構１９ａを利用してもよい。

また、このコンプライアンス機構１９，１９ａとして、
一般にＲＣＣといわれるリモートセンターコンプライア
ンス装置を使用してもよい。

さらに、本実施例では、エンコーダ３４．４４．５４．
６４．７４．８４から人力された工具の位置を座標変換
回路４０８で、ロボット基準座標系からセンサ座標系に
変換し、センサ座標系の形で理想位置を算出し、座標変
換回路４１８にて、再びデータをセンサ座標系からロボ
・ント基準座標系に変換している。

しかし、センサ２４から人力されたセンサ座標系のカデ
ータを演算に使用する前に、ロボット基準座標系に変換
して、理想位置を算出してもよい。

すなわち、２つの座標変換回路４０８，４１８を取り除
き、センサコントローラ２４の後にカデータをセンサ座
標系からロボ・ント基準座標系に変換する座標変換回路
を設けるようにしてもよい。

この場合、容量がロボット基準座標系であられされてい
ることを除けば、上記した演算はそのまま使用できる。

［発明の効果］ 本発明は、以上のような構成を採用したことによって、
作業スピードを低下することなく、精度よくワークを加
工できる。

【図面の簡単な説明】

第１図は本発明の構成を例示する構成図、第２図は本発
明の一実施例の斜視図、 第３図はその動作部１６を説明する図であって、（ａ）
は平面図、（ｂ）は正面図、（ｃ）は左側面図、第４図
はその手先１８を説明する斜視図、第５図はそのロボッ
トコントローラ１２の構成図、第６図はロボットコント
ローラ１２０制御プロ・ンク図、 第７図各点の位置関係を示す説明図、 第８図は力センサ２２で検出されるカデータの説明図、 第９図は反力の変化検出の説明図、 第１Ｏ図、第１１図は各々第１ＣＰＵ３００、第２ＣＰ
Ｕ３０１で実行する流れ図、 第１２図は本実施例の手先１８の動きを示す説明図、第
１３図はコンプライアンス機構の他の例を示す斜視図、 第１４図は従来のロボットの手先の動きを示す説明図で
ある。 Ｒ１，１８・・・手先 Ｒ２，２０，２０ａ−・−工具（回転砥石）Ｒ３，１９
，１９ａ・・・変位量−力変換機構（コンプライアンス
機構） Ｒ４，３４，４４，５４，６４，７４，８／ｌ・・・現
在状態測定手段（エンコーダ） Ｒ５，２２・・・反力測定手段（力センサ）Ｒ６，４１
４・・・変位量換算手段（偏差量換算手段）Ｒ７，４１
６・・・理想状態算出手段（理想位置演算手段）Ｒ８，
４２０・・・補正量算出手段（補正要素選択手段）Ｒ９
，３２０〜３３０・・・ロボット制御手段（サーボ回路
）代理人　弁理士　定立　勉（他２名） 第４図 第１図 第 図 第１２図 第１３図 に 第１４図 （ａ） （ｂ）

Claims (1)

1. 【特許請求の範囲】 手先に、工具を、所定の軸方向に加えられる力と該軸方
   向の変位量との間の関係が予め明らかである変位量−力
   変換機構を介して設けると共に、予め記憶された位置デ
   ータにより得られる標準軌跡上を動作し、前記工具を予
   め定められた設定力で被加工物に作用させる倣い制御ロ
   ボットであって、 前記手先の現在位置及び／又は現在姿勢を測定する現在
   状態測定手段と、 該測定された現在位置及び／又は現在姿勢において、被
   加工物より工具に加えられる反力を測定する反力測定手
   段と、 該測定された反力と、前記測定された現在位置及び／又
   は現在姿勢における予め定められた設定力との差を求め
   ると共に、該差を前記工具の位置変位量に換算する変位
   量換算手段と、 該換算された変位量と前記手先の現在位置及び／又は現
   在姿勢とから、現在の手先の理想位置及び／又は理想姿
   勢を算出する理想状態算出手段と、該算出された手先の
   理想位置及び／又は理想姿勢と、手先が次に移動する標
   準軌跡上の将来位置及び／又は将来姿勢とを比較して、
   手先の位置及び／又は姿勢の補正量を算出する補正量算
   出手段と、 該算出された補正量に基づいて、前記手先の位置及び／
   又は姿勢を制御するロボット制御手段とを備え、 工具の被加工物に対する反力が予め定められた設定力と
   なるようリアルタイムで補正しながら作業することを特
   徴とする倣い制御ロボット。