JPH03251376A - 制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔概要〕 制御対象が受ける力を検出して力制御を行な−）力制御
部と、前記制御対象の位置を検出Ａる位置検出部と、前
記力制御部に対して制御指令を発生する制御指令発生部
とを備えて成る制御装置に関し、 操作者が指示する任意の位置で動作を切換え可能にする
ことを目的とし、 操作者より与えられる位置指令を格納可能な記憶手段を
設け、前記制御指令発生部は前記位置検出部の検出位置
と前記記憶手段に格納された位置指令とを比較する手段
を有し、比較結果に応じた制御を行なうように構成する
。

〔産業上の利用分野〕

本発明は、制御対象が受ける力を検出して力制御を行な
う力制御部と、前記制御対象の位置を検出する位置検出
部と、前記力制御部に対して制御指令を発生ずる制御指
令発生部と、を備えて成る制御装置に関する。

力制御ロボットで組立・はめあい作業をする際には、制
御対象が受ける反力をフィードバックして反力がゼロあ
るいは一定値になるように制御対象を制御する。力制御
ロボットが空中にあるときは、制御対象に接触している
物体がないため力検出部では反力は検出されず、慣性力
のみが検出される。力制御部へ一定力を発生するように
指令した場合、その設定した反力を検出するまでは、力
制御ロボットは動作を続ける。また、力制御中は、位置
のフィードハックは行わないようになっている。

また、一般に力制御ロボットは、制御装置内のメモリに
予しめ格納された教示データ（動作プログラム及び座標
情報）に従って組立作業等の動作を行なうように構成さ
れている。

〔発明が解決しようとする課題〕

前述したように、ロボットの力制御を行なう従来の制御
装置では、制御対象であるロボットアームの位置が不適
当な領域に移動したとしても反力が予じめ設定した値に
達しない間は、その制御動作を停止あるいは回避させる
ことができないという問題があった。

また、予じめ教示しておいた作業をわずかに変更させる
場合でも再作成した教示データをメモリに格納させる必
要があった。

本発明の目的は、前述した従来の問題に鑑み操作者が指
定する任意の位置で動作を切換えることが可能な制御装
置を提供することにある。

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

そして、この目的は、制御対象が受ける力を検出して力
制御を行なう力制御部と、前記制御対象の位置を検出す
る位置検出部と、前記力制御部に対して制御指令を発生
する制御指令発生部と、操作者より与えられる位置指令
を格納可能な記憶手段と、を備えると共に、前記制御指
令発生部は前記位置検出部の検出位置と前記記憶手段に
格納された位置指令とを比較する手段を有し、比較結果
に応じた制御を行なうよ・うに構成されて成ること、を
特徴とする制御装置により達成される。

〔作用〕

すなわち、本発明では、操作者より与えられる位置指令
と制御対象の位置とを比較し、比較結果に応じた動作を
行なわせることができるので、制御対象が非所望な位置
に移動した場合にその動作を停止又は回避させることが
でき、また、簡単な作業の変更にも対処することが可能
となる。

〔実施例〕

以上、本発明に係る制御装置の実施例を図面を用いて説
明する。

第１図は一実施例の説明図、第２図は位置制御部及び力
制御部の構成説明図、第３図は座標系の説明図、第４図
は動作フローチャートである。

図に示すように、操作部２２は、マユブレーク２１の制
御を行なうものであり、サーボ・モータ２２ａと、パワ
ー・アンプ２２ｂと、Ｄ／Ａコンバータ２２ｃと、補償
器２２ｄとを有している。

また、位置検出部２６は、マユブレーク２１の図示しな
いハンド部の先端位置の検出を行なうものであり、この
位置検出部２６はカウンタ及びエンコーダ２６ａとタコ
・メータ２６ｂとを有している。

更に、力検出部２３は、マユブレーク２１のハンド部が
受ける力の検出を行なうものであり、この力検出部２３
は力覚センサ２３ａとハンド座標系からロボット基準座
標系への座標変換部２３ｂとを有している。

更にまた、力制御部゛２４は、力制御動作時に、力検出
部２３により検出された力Ｆと設定力（力指令Ｆ。）と
の偏差を求める加算部２４ａの出力及びフィードバック
ゲインＣｆに基づいて速度指令■、を発するものであり
、位置制御部２７は、位置検出部２６で検出された位置
Ｘと位置指令Ｘ。

との偏差を求める加算部２７ａの出力及び位置フィード
バンクゲインＣｐに基づいて速度指令信号■２を発する
ものである。

更に、加算部３０ｂは、力制御部及び前記位置制御部か
ら出力された速度についての加算を行なうもの、逆ヤコ
ビ変換部３０ａは、加算された速度をマニプレータ２１
の各関節の角度θに変換するものである。

位置制御部２７は、第２図に示すように、転置直交変換
行列（ＲＴ）演算部３１と、選択行列（＋−５ｒ）演算
部３２と、直交行列（Ｒ）演算部３３と、位置フィード
ハックゲイン（ｃｐ）演算部３４とを有する。一方、力
制御部２４は、転置直交行列（ＲＴ）演算部３８と、選
択行列（ｓｒ）演算部３７と、直交行列演算部３６と、
カフィードバックゲイン（Ｃ７）演算部３５とを有する
。

ベクトルｎ、ａ、ｏを用いてロボット基準座標系（Ｘｏ
、Ｙｏ＋　　Ｚ、）からハンド座標系（Ｘ、、。

Ｙ、、Ｚ、）への座標変換を表示する直交座標変換行列
Ｒは次のように表されることになる。

３７の選択行列は次の式で表示される。

この場合、一般に例えばねじ締め、缶の蓋締めなど押付
方向の軸まわりにトルクを与える場合にはＳ＝１とし、
与えない場合にはＳ−０とする。

カフィードバックゲインＣ２は基準座標系に関して、 第１図において、このハンド座標系（Ｘ、、Ｙ、。

Ｚ、）のＹｗ及びＺ８を位置制御方向、Ｘ、方向を力制
御方向とすることで選択行列演算部３２゜で与えられる
。

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

座標変換部２０では、位置検出部２６で検出されたマニ
プレータの関節角θ、を基準座標系での位置Ｘ。に変換
する。

制御指令生成部４０は、 ■　目標位置Ｘ１や力指令Ｆｒ等の制御指令、■　倣い
座標系の切換えに伴う位置制御パラメータや力制御パラ
メータの送信、 ■　位置記憶部への位置記憶命令や検出した力Ｆ、のモ
ニタリング等のタイミングの発生、− を行なう。

また、制御指令生成部４０は、操作者が繰作可能な入力
手段であるコンソール５０から与えられる位置座標及び
その座標での動作指令（例えば、停止）を受は取り、メ
モリ６０のテーブル６１に格納する。尚、コンソール５
０は、英数字キーを有するキーボード５１が設けられて
おり操作者はこのキーボード５１を操作する。

また、メモリ６０には、予じめ作成された教示データ６
２や動作プログラムが格納されている。

つまり、例えば、マユブレーク２１により物品の嵌合作
業を行なわせる場合、マニプレータ２１の位置精度の低
さにより力制御開始前の位置決めが所望の位置からずれ
ており、その状態からずれていたとすると、力制御動作
を行なっている際には、成金時の反力が得られないため
、ずれたまま別の所へ押付けてしまい、そして設定され
た反力が得られた時迄で作業終了と判断してしまう。従
って、操作者は、予じめずれ量を考慮して設定をコンソ
ール５０から入力することで、誤動作を内ケ抄ドルへの
積層枚数の変更、ねじ締めトルクの変更、力付与方向の
変更を行なう場合等において、設定位置及びカイ］与方
向変更命令等をコンソール５０のキーボード５１から入
力する。そして、制御指令生成部４０はこの入力された
設定位置Ｘ。ｐ及び変更命令をテーブル６１に格納する
。

更に、制御指令生成部４０は、動作開始時の位ＷＸＩｌ
あるいは、オペレータが指令した位置Ｘ。。

と現在位置Ｘを比較して位置指令・力指令を生成する条
件が成立したか否かを判断する。

一方、力覚センサ２３ａで検出された力Ｆｈは、ハンド
座標系で記述されており、座標変換部２３ｂにより基準
座標系での力Ｆに変換される。ハンド座標系を直交座標
系とし、ハンドの押しっけを行なう力制御方向の単位ベ
クトルをｎとし、単位ベクトルｎに直交し、反力をゼロ
に保つように力制御を行なう力制御方向の単位ベクトル
をｏ、　　ａとする。ただし、ｎ、ｏ、ａは、 ａ　　＝　ｎｘ。

を満たすものとする。ｎ 記述すると、 ｎ−［ｎｘ　＋　　ｎｙ ｏ＝　［ｏＸ、ｏｙ ｎ　、　　］　　Ｔ ｏ２　］　　Ｔ ｏ、ａを基準座標系で で表される。Ｔは、転置行列を示す。各ベクトルの成分
は、関節角θ５を同次変換やＤｅｎａｖｉｔ−Ｈａｒｔ
ｅｎｂｅｒｇ記法で変換して求めることができる。これ
らのベクトルを用いて、力覚センサ２３ａで検出された
力Ｆｈを基準座標系での力Ｆへ変換する座標変換行列を
表すと、 Ｔ＝［ｎ　　ｏ　　ａｌ となる。（但し、トクル成分については省略する。）前
述した構成についてその動作を第４図の動作フローチャ
ートを用いて説明する。

■　まず制御指令生成部４０は、オペレータが設定した
位置Ｘ。Ｉ−をテーブル６１へ格納する。

■　次いで、位置フィードバックゲインＣ２を１ ゼロとして位置制御ループをカットし、力制御を行うた
め、適当なカフィードバックゲインＣｆ（例えば、「１
」）を力制御を力制御部２４へ送る。

■　押しつけ方向ｎの力を発生ずる力命令Ｆ。

を生成し、加算部２４ａへ送る。

■　位置検出部２６で検出した力制御中のロボットの先
端（ハンド位置）の現在位置Ｘを読取ると共に、テーブ
ル６１に格納されている設定位置Ｘ　ｏｐを読出ず。

■　そして、現在位置Ｘとテーブル６１に格納し一ζあ
るオペレータの指示した停止位置Ｘ。ｐを比較する。

そして、条件が成立したか否かを判断し、成立していな
ければステップ■へ移り、成立していればステップ■へ
移る。

■　制御指令生成部４０は力検出部２６で検出した力Ｆ
を読取り、そして、指令した力Ｆ。と−致したか否かを
チエツクする。

一致しておれば、処理を終了、一致していなけ２ れば、ステップ■へ戻る。

■　力指令Ｆ。を全でゼロとし、押しつ＆−１動作を停
止する。

コンソール５０等を用いて操作者に対してエラー報告を
する。

前述した動作では、ステップ■で条件が一致すると、そ
の動作を停止する場合について説明したが、例えば、カ
フィードバックゲインＣ１をゼロとし、力制御ループを
カントし、位置フィードバックゲインＣ２を適当な値に
設定すると共に、例えば、ロボット（マニプレータ）２
１を原点に戻す位置指令ｘ、”’を生成して位置制御部
へ送り、ロボットの動作を位置制御モードで行なうよう
にしても良い。

また、マニプレータ２１を原点に戻すことなく、位置制
御モード（カフィードハックゲインＣ２は全て「０」１
位置フィードバックゲインＣＰはある値に設定）で停止
（位置指令Ｘ。は停止位置の座標）させておいても良い
。

更には、磁気ディスク円板のスピンドルへの挿大作業後
のバランス取り作業を不要にすべく、磁気ディスク円板
をスピンドルに対して力制御で挿入させた後、スピンド
ルの軸と直交する方向に円板を移動させ、ディスク円板
の内周面をスピンドル軸面につき当てる作業にも適用で
きる。

つまり、この作業は、複数の磁気ディスク円板の押付方
向を各円板毎に１８０°異ならせて積層（２枚目と３枚
目は９０度とし、３枚目と４枚目は１８０度してもよい
。） この場合は、予じめディスク円板を力制御でスピンドル
に嵌合させる教示データがある場合にこれを変更する例
である。

つまり、複数のディスク円板をスペーサを介在させてス
ピンドルに挿入する教示データ６２はメモリ６０に格納
されており、オペレータは、動作の開始前に、この教示
データ６２による作業を変更すべく、コンソール５０の
キーボード５１から、各ディスク円板の積層位置を入力
する。

また、この積層位置と共に、押付方向ｎ及び力指令Ｆ。

も与える。

この時の押付方向は、前述したように、１枚目はＸ軸と
平行で且つ原点０に向う方向（第１図を参照）、２枚目
はこの方向と１８０°異なる方向、３枚目は２枚目を９
０度、４枚目は３枚目を１８０度、５枚目は１枚目と同
じで４枚目と９０度異な円板挿入作業のために、位置フ
ィードバックゲインｃ、、のうちの各要素ＣＩ）Ｘ＋　
　Ｃｐｌ’ｌ　　ＣＰα＋ＣＩ）β。

Ｃ，、γの値を全て「１」とし、要点Ｃｐ□のみを「０
」とし、積層すべき円板の位置よりもわずかに下方の位
置指令Ｘ。を与えると共に、カフィードバックゲインＣ
２の全要素Ｃｒｘ−Ｃｒ　ｒを「１」とし、力指令Ｆ。

を与える。この時、鉛直となるように立てられたスピン
ドル軸に磁気ディスク円板を挿入するものとする。ロボ
ットのＺ軸が鉛直と一致し、Ｘ、Ｙ軸が水平と一致して
いるものとする。

これにより、例えば、スカラ型ロボット等のマニプレー
タは、Ｚ軸方向にのみ力制御され、その５ 他のｘ、ｙ、　　α〜Ｔ軸はコンプライアンス制御され
る。そして、磁気ディスク円板がロボットによりスピン
ドルに対して挿入される。

そして、この時、第４図のステップ■〜■の処理が実行
され、条件が一致したならば（ここで、位置Ｘ。２はＦ
。＞Ｆとなる円板積層位置を与えているため、ステップ
■では条件一致がなく、必らずステップ■で一致する迄
ループする）、制御指令生成部４０は、力指令Ｆ０及び
位置指令Ｘ０を１０」とする。

尚、制御指令生成部４０は、ステップ■において、図示
しないカウンタを用いて円板積層枚数をカウントしてお
り、現在積層している円板に対応する設定位置Ｘ。２と
の比較を行なうようにしてい第ｎ枚目の押付方向に対応
するゲインＣ２のみを「１」とし、コンソール５０から
入力された方向の力指令Ｆ０を当てる。

これにより、第ｎ枚目の磁気ディスク円板の内６ 周面がスピンドル軸面に所望の方向から押し当てられる
。

そして、押付力が指令Ｆ。に一致すると、制御指令生成
部４０は、いわゆる割込み処理（サブルーチン相当）が
終了したものとして、予じめメモリ６０に格納されてい
る教示データ６２に従って前述した挿込時と同様のゲイ
ン及び指令を与えてそれ以降の作業を実行する。つまり
、制御指令生成部４０は、ステップ■で条件が一致した
時点で位置制御部２７、力制御部２４に設定していた値
をメモリ６０のワークエリアに移す−・方、この時の教
示データのステップ位置も格納しており、これらの値に
基づいてそれ以降の作業に復帰することができる。

このように、予じめ教示したデータも任意に変更するこ
とが可能となる。

〔発明の効果〕

以上説明したように本発明によれば、操作者が指定する
任意の位置で動作を切換えることが可能な制御装置が得
られる。

【図面の簡単な説明】

第１図は一実施例の説明図、 第２図は位置制御部及び力制御部の構成説明図、第３図
は座標系の説明図 第４図は動作フローチャートである。 図において、２１はマユブレーク、２２は操作部、２３
は力検出部、２４は力制御部、２６は位置検出部、２７
は位置制御部、４０は制御指令生成部、５０はコンソー
ル、６０はメモリである。

Claims (1)

1. 【特許請求の範囲】 制御対象が受ける力を検出して力制御を行なう力制御部
   と、 前記制御対象の位置を検出する位置検出部と、前記力制
   御部に対して制御指令を発生する制御指令発生部と、 操作者より与えられる位置指令を格納可能な記憶手段と
   、を備えると共に、 前記制御指令発生部は前記位置検出部の検出位置と前記
   記憶手段に格納された位置指令とを比較する手段を有し
   、比較結果に応じて制御を行なうように構成されて成る
   こと、 を特徴とする制御装置。