JPH02303780A - 倣い制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔概要〕 矩形対象物に対する倣い制御方式の改良に関し、オペレ
ータによる教示なしに、未知の矩形状の対象物に成る一
定の力を加えながら倣い動作を行うことを目的とし、 力検出部から出力される力がゼロ又は略ぼゼロになった
時にその時点の位置を位置記憶部に記憶し、ロボットを
停止させ、旧の倣い座標系をＺ。

軸（倣い方向＝Ｙい、押付力の方向＝Ｘ、）の回りに反
時計方向に９０度回転することにより新しい倣い座標系
を求め、新しい倣い座標系に基づくパラメータをロボッ
ト・コントローラに設定し、新しい倣い座標系にロボッ
トの姿勢を一致させ、位置記憶部に記憶されている位置
にロボットを移動させ、ロボットを対象物に押し付け、
押付力＝設定力になったときにロボットに倣い動作を行
わせることを構成要件としている。

〔産業上の利用分野〕

本発明は、力制御ロボットで対象物の表面に沿って加工
等を行う倣い作業のための倣い制御方式に係わり、特に
、制御対象を作動させる操作部と、制御対象の位置・姿
勢を検出する位置検出部と、制御対象が受ける力の検出
を行う力検出部とを備え、制御対象を矩形の対象物の表
面に沿って倣い動作させる倣い制御方式に関する。

〔従来の技術〕

力制御ロボットで倣い作業をする際には、ロボットの先
端と対象物との接触点における対象物表面の法線方向に
一致する押付方向ｎと、倣いを行いながら移動する移動
方向Ｏで決定される倣い座標系（Ｏｗ　　Ｘｗ　Ｙｗ　
Ｚｉｎ　）を、ロボット・コントローラへ与える必要が
ある。

従来、この倣い座標系を作る方法として、■　オペレー
タによる教示（特願昭６３−２１３５７号、特願昭６３
−５９５０４号）■　ロボットの位置情報による未知拘
束面の推定（例　吉川、須蒔：未知拘束面に対する動的
ハイブリッド制御、第５回ロボット学会（１等があった
。

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

上記の■の方法ではオペレータによるロボットへの教示
が必要なため操作が煩雑であり、■の方法では矩形や多
角形などの不連続面を有する物体への倣いが出来ないと
言った問題があった。

本発明は、この点に鑑みて創作されたものであって、オ
ペレータによる教示なしに、未知の矩形状の対象物に成
る一定の力を加えながら倣い動作を行う力制御ロボット
の倣い制御方式を提供することを目的としている。

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

第１図は倣い座標系を説明する図である。第１図におい
て、Ｏ，−ｘｗ　Ｙ、Ｚ、は倣い座標系、ＯＸｏ　Ｙｏ
　Ｚｏは基準座標系、ｎ、ｏ、ａは倣い座標系Ｏｗ　’
　Ｘｈ　Ｙｗ　Ｚｗの単位ベクトルをそれぞれ示してい
る。

倣い座標系０−　　Ｘｗ　Ｙｗ　Ｚｗは、対象物に対す
るロボット先端の位置・姿勢により決定される座標系で
、第１図に示すような位置関係にある。

ベクトルｎ、ｏ、ａは倣い座標軸Ｘ。ＹｗＺｓ、ｌそれ
ぞれについて単位ベクトルである。λは対象物へ力を加
えるときの押付方向を示し、対象物の表面の法線ベクト
ルと同じ方向である。０はｎと直交関係にあり、倣い動
作時のロボット先端の移動方向を示す。ａはｎ、ｏに直
交するように定められ、 ａ＝ｎＸ。

で与えられる。

ｎ、、ｏ、ａの基準座標系Ｘ。ｙｏｚ、についての成分
表示を行うと、 ■＝（ｎ貢ｎ、ｎ、）” ｏ＝（ｏつｏ、ｏ、）” となる。但し、Ｔは転置行列を示す。

第２図は対象物とマニプレータの関係を示す図である。

同図において、Ｐ＋　、Ｐｚ　、Ｐ３はロボットの位置
を示す。

第２図を用いて矩形対象物への倣い動作をどのように行
うかを示す。矩形対象物の成る面Ａに対して倣い動作を
行う場合、倣い座標系は第２図のＰ、のように与えられ
゛る。ベクトルπは面に垂直、ベクトルＯ，ａは面に平
行である。

第２図の位置Ｐ、のように与えられた倣い座標系は、面
Ａに対する倣い動作を行っている間は変更する必要がな
い。しかし、ロボットが位Ｗ　ｐ　ｔから位置Ｐ２まで
移動すると、倣い拘束面が面Ａから面Ｂへ変わるため、
倣い座標系も位置Ｐ、で与えられる座標系へ切り換える
必要がある。

第３図は倣い動作を説明する図である。同図において、
ａは紙面に下向き垂直である。同図は対象物をト方（面
Ａ及びＢに対して垂直方向）から見た図であり、ロボッ
トが位置Ｐ２付近でどのように動作するかを示したもの
である。

第４図は倣い動作時の力および位置の時間応答を示す図
である。同図において、ｔ（Ｐ、）、ｔ（ｐｚ　）、　
　ｔ、　（Ｐｌ　）はそれぞれロボットが位置Ｐ＝　、
Ｐ２　、Ｐａに達した時刻を示す。

同図は、面Ａに対する倣い動作時のπ方向への押付力Ｆ
１とロボットの先端位”ｔ　（Ｘｏｗ、　Ｙｏ、１）の
時間履歴を示したものである（Ｚ、軸の座標値は省略し
ている）。

第５図は制御系の原理ブロック図である。同図において
、１は制御対象、２は操作部、３は位置制御手段、４は
力制御手段、５は位置検出部、６は力検出部、７は位置
記憶部、８は制御指令生成部をそれぞれ示している。

第５図は、位置Ｐ＋で記述される倣い座標系を位ＦＩ　
Ｐ　３での倣い座標系に切り換え、第３図のように、ロ
ボット姿勢をＰ８からＰ５に変えるための力制御ロボッ
トの制御系の原理ブロック図である。以下、第３図ない
し第５図を用いて矩形対象物への倣い動作を行うための
制御方法について説明する。

第３図に示すよ・うに、ロボットが面Ａ’＆Ｐ、→Ｐ、
のように倣い動作を行い、時刻ｔ　（Ｐ２　）で位Ｂ　
ｐ　ｚに達したとする。位置Ｐ２よりも右側では対象物
が存在しないが、この時点では制御指令は変更されてい
ないので、ロボットはｎ方向への押付力を発生しようと
して、π方向に移動する。

また、才方向への移動成分もあるため、結局、ロボット
は位Ｗｐｚから位ＩＰ、へ進むように動作する。このと
き、第５図の力検出部６で検出される五方向の押付力Ｆ
７は、第４図（ａ）のような時間応答を示す。つまり、
面Ａを倣っている間（Ｐｉ〜Ｐ２まで移動する期間）は
、一定の力で押付力を発生しているが、Ｐ２へ達して対
象物がなくなると、反力がなくなるため、押付力Ｆ７は
セロとなる。また、第５図の位置検出部５で検出される
ロボットの先端位”ｔＯ−（Ｘｏｗ、　Ｙｏｗ）の時間
応答は第４図（ｂ）、　（Ｃ）のようになり、位置Ｐ、
に達すると、Ｘ１方向へも移動する。但し、Ｘｗ、Ｙｗ
力方向の移動は全て等速度で行われているものとする。

第５図の制御指令生成部８では第４図（ａ）のような力
の変化を検出すると、 １）位置Ｐ２の座標値（第４図（ｔ））、（Ｃ）の黒丸
で示すＸ。、１．　ｙｏ、）の記憶 ２）０ボツトへの停止指令 を直ちに行う。位置制御手段３．力制御手段４は２）の
停止指令が送られてくると、ロボットの動作を停止する
。この時のロボットの位置はＰｌである。

倣い座標系をＰｌでの座標系からＰ、での座標系に切り
換え、且つ、ロボットの姿勢をＰ２からＰ、にするには
、ベクトルオの回りに一９０度回転すると良い。従って
、倣い座標系Ｘｗ、Ｙｗ。

ハに関する回転座標変換行列ｐ、　ｋ　（−ｃｚｚ＋　
をＰ。

の倣い座標系［ｎ□＋　　０９１＋　　ａｌ１１］にか
けてやると、Ｐ３の倣い座標系Ｅ式、、宕Ｐ３＋　？ａ
ｐ３］が求まる。

［ｉ、、、、ｇ、ｚ、れ、］−Ｅに（−防）　げＫ　Ｐ
Ｉ＋　れ６．れ、〕・・・　■ Ｐ２からＰ、への姿勢の変化も、ロボットの先端を記述
するハンド座標について同様な操作を行うことにより求
まる。

Ｐ３での倣い座標系、Ｐ５のロボットの姿勢が求まると
、第５図の制御指令生成部８から必要なパラメータが位
置制御手段３．力制御手段４に設定される。次に、ロボ
ットの姿勢を新しく設定した座標系に一致するように変
化させ、ロボットのＸ、方向の位置が位置記憶部７に記
憶しである第４図（ｂ）の黒点のＸ。０に一致するまで
移動する（位置Ｐ６）。次に、−Ｙ、方向への押付けを
行い、設定力に達したら（位置ｐｓ）、再び面Ｂに対す
る倣い動作を再開する。

以上のような操作を他の角に対して行うと、対象物の４
つの面に対する倣い動作を行うことが出来る。また、以
上の説明では簡単のため、全て倣い座標系Ｏｗ　　Ｘｗ
　Ｙｗ　Ｚ＝ｖを基準にして行ったが、後述するように
、倣い座標系を基準座標系に座標変換すると、基準座標
系でも同様に説明できる。

〔実施例〕

第６図は制御装置の１実施例の機能ブロツク図である。

同図において、２０は座標変換部、２１はマユブレーク
、２２は操作部、２２ａはモータ、２２ｂはパワーアン
プ、２２ｃはＤ／Ａコンバータ、２２ｄは補償器、２３
は力検出部、２３ａは力覚センサ、２３ｂは座標変換部
、２４は力制御部、２４ａは偏差部、２６は位置検出部
、２６ａはエンコーダ＆カウンタ、２６ｂはタコメータ
、２７は位置制御部、２７ａは偏差部、３０ａは逆ヤコ
ビ行列、３０ｂは加算部、４０′はホスト・コンピュー
タ、４０ａは位置記憶部、４０ｂは制御指令生成部をそ
れぞれ示している。

第６図に示す制御装置は、マユブレーク２１の制御を行
う操作部２２を備えている。この操作部２２は、サーボ
・モータ２２ａと、パワー・アンプ２２ｂと、Ｄ／Ａコ
ンバータ２２ｃと、補償器２２ｄとを有している。また
、制御装置は、マユブレーク２１のハンド部（図示せず
）の先端位置の検出を行う位置検出部２６を備えており
、この位置検出部２６はカウンタ＆エンコーダ２６ａと
タコ・メータ２６ｂとを有している。

更に、制御装置は、マユブレーク２１のハンドが受ける
力の検出をけう力検出部２３を備えている。この力検出
部２３は、力覚センサ２３ａと、ハンド座標系からロボ
ー７）の基準座標系への座標変換部２３ｂとを有してい
る。

更に、制御装置は、倣い動作時に力検出部２３により検
出された力Ｆ０．設定力（力指令Ｆ、）及び力制御パラ
メータに基づいて力制御方向の速度指令信号■、を発す
る力制御部２４を有すると共に、位置検出部２Ｇにより
検出された位置Ｘ。

、目標位置Ｘ、及び位置パラメータに基づいて位置制御
方向の速度指令信号■。を発する位置制御部２７を有し
ている。

第７図は位置制御部および力制御部の機能ブロック図で
ある。同図において、３１は転置直交行列演算部、３２
は選択行列演算部、３３は直交行列演算部、３４は位置
フィードバック・ゲイン演算部、３５はカフィードバッ
ク・ゲイン演算部、３６は直交行列演算部、３７は選択
行列演算部、３８は転置直交行列演算部をそれぞれ示し
ている。

位置制御部２７の具体的構成は、第７図に示すように、
転置直交変換行列ＣＲＴ”）演算部３１と、選択行列（
Ｉ　　Ｓｒ）演算部３２と、直交行列（Ｒ）演算部３３
と、位置フィードバンクゲイン（Ｃ９）演算部３４とを
有する。一方、力制御部、２４は、転置直交行列（Ｒｔ
　）演算部３８と、選択行列（Ｓ、）演算部３７と、直
交行列演算部３６と、カフィードバックゲイン（Ｃ２）
演算部３５とを有する。

ベクトルｎ、ａ、ｏを用いてロボット基準座標系（ｘ、
、ｙ、、ｚｏ）から倣い座標系（Ｘ、。

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

第１図において、この倣い座標系（ｘ、、ｙ。

、２．）のＹ。及びＺ、Ａを位置制御方向、Ｘｏ力方向
力制御方向とすることで選択行列演算部３２゜３７の選
択行列は次の弐で表示される。

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

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

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

更に、制御装置は、力制御部２４及び位置制御部２７か
ら出力された速度についての加算を行う加算部３０ｂと
、加算された速度をマユブレーク２１の各関節の角速度
シに変換する逆ヤコビ変換部３０ａとを有している。

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

ホスト・コンピュータ４０は、座標変換部２０で変換さ
れた位置を記憶する位置記憶部４０ａを有すると共に、
制御指令生成部４０ｂを有する。

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

第８図はハードウェアのシステム構成例を示す歯である
。同図において、１０はロボ・ント・コントローラ、１
０ａばメモリ、１０ｂは通信制御部、１０ｃはロボット
制御部、２１はマニプレータ、２２は操作部、２３ａは
力覚センサ、２６は位置検出部、４０はホスト・コンピ
ュータ、４０ａは位置記憶部、４０ｂは制御指令生成部
、４０ｃは通信制御部、４０ｄはメモリをそれぞれ示す
。

ホスト・コンビエータ４０は、制御指令生成部４０ｂ、
メモリ４０ｄ１通信制御部４０ｅを備え、メモリ４０ｄ
内に位置記憶部４０ａを有する。ロボット・コントーラ
１０は、メモリ１０ａ１通信制御部１０ｂ、　ロボ７）
制御部１０ｃ等を有している。ロボット・コントローラ
１０は、操作部２２を介してマユブレーク２１を動かし
、マニプレータ２１の位置を位置検出部２６から取り込
み、マニプレータ２１に加わる力を力覚センサ２３ａか
ら取り込む。ホスト・コンピュータ４０とロボット・コ
ントローラ１０はバス等の通信インタフェースによって
接続されており、それぞれの信号の送受信のタイミング
を管理する通信制御部１０ｂ、４０ｃによって、メモリ
１０ａと４０ｄの間でデータが転送される。

このようなシステム構成で、第３図で説明したような矩
形対象物に対して倣い動作を行う場合の処理の流れを第
９図を用いて説明する。

第９図は、フローチャートの形式で、ホスト・コンピュ
ータ内の制御指令生成部５０と、位置記憶部５１と、ロ
ボット・コントローラ５２での処理の流れを示したもの
である。同図において、実線の矢印は処理の流れを示し
、点線の矢印は各動作時に転送される指令及びデータの
流れを表している９以下の説明では、第３図の位置Ｐ２
から位’１１　Ｐ　ｓの切り換え方法について詳述し、
ハンドを位置Ｐ１までどのように移動させて面、ｌど対
する倣いを行わせるか、についての説明は省く。

マニプレータに取り付けられたハンドが第３図きょうに
位置Ｐ１で凹入への倣い動作を行っている状態では、５
０ａの処理は既に終了しており、制御指令生成部５０で
は５０ｂ、５０ｃの処理を行っており、ロボット・コン
トローラでは５２ａ。

５２ｂの処理を行っている。

制御指令生成部５０では、ハンドが面Ａの倣い動作を行
っている間、処理５２ｂによって送られて来るハンドの
押付力Ｆ。のモニタを行い、処理５０ｃのように反力が
ゼロ、つまりハンドの位置がＰ２に達して対象物からの
反力がなくなったか否かを判定する。このときの判定は
、誤差を考慮してＦ□ｔ　（＜１）よりもＦｏの絶対値
Ｆ０が小さいか否かで行っても良い。制御指令生成部５
０は、反力がなくなって位置Ｐ２へ達したと判断する（
Ｆ０＝０が成立）と、処理５０ｄによって、ロボット・
コントローラの位置検出部２６及び座標変換部２０で検
出した位置Ｐ２の座標（ｉ（ｘｏｗ。

Ｙｏｗ、　　ＺＯ−、’）を位置記憶部５１へ転送する
ｆ位置記憶部５１では、処理５１ａのように、処理５２
ｃの操作で位置Ｐ７の座標値を受は取り、記憶する。次
に、制御指令生成部は、処理５０ｅにより、力指令Ｆ、
＝Ｏ，速度指令■。−０，目標値Ｈｘ、＝現在位置を指
令し、マニプレータを停止させる。停止後、処理５０ｆ
の座標変換行列Ｒの切り換えを式■によって行い、ロボ
ット・コントローラ５２へ転送する。ロボット・コント
ローラ５２では、処理５２ｅにより、位置制御部２７及
び力制御部２４の中のＲ，Ｒ’を変更する。また、処理
５０ｇにより新しく設定した倣い座標系に従うハンドの
姿勢を算出し−、目標位置Ｘｒ及び速度指令■。をロボ
ット・コントローラ５２に送る９０ボツト・コントロー
ラ５２では、送られてきた目標位置Ｘｒ及び速度指令■
。に従って、マニプレータ及びハンドの姿勢を変える。

次に、処理５ｏｈにより、処理５１ａで記憶していた位
置Ｐ２の座標値（Ｘｏｗ、　Ｙｏｗ、　　Ｚｏ、ｉ）を
受は取り、位置Ｐ、へ移動するように、ロボット・コン
トローラ５２へ指令を送る。移動停止後、処理５０ｉに
示すように、設定力Ｆ、で面Ｂを押し付けるよう、力指
令Ｆ、及び速度指令■。をロボット・コントローラ５２
に送る。指令後、制御指令生成部５０は、力のモニタを
行い（５０ｊ）、処理５０にの判定で押付力Ｆ０が設定
力Ｆ、に迷したと判断すると、再び倣い動作の指令を行
って（５０ｆ）、面Ｂへ倣う。

上記の操作を繰り返し行うことによって、他の面への倣
い動作も実現できる。

〔発明の効果〕

以上の説明から明らかなように、本発明によれば、未知
形状で不連続面を有する矩形対象物に対する倣い動作を
自動的に行うことが出来、従来。

ロボットの操作者が行っていた対象物体の変更及び対象
物の位置ずれに伴うロボットへの教示を行う必要がなく
なり、操作者のｆ！、担が軽減される。

【図面の簡単な説明】

第１図は倣い座標系を説明する図、第２図は対象物とマ
ニプレータの位置関係を示す図、第３図は倣い動作を説
明する図、第４図は倣い動作時の力２位置の時間応答を
示す図、第５図は制御系の原理ブロック図、第６図は制
御装置の１実施例の機能ブロック図、第７図は位置制御
部及び力制御部の機能ブロック図、第８図はハードウェ
アのシステム構成例を示す図、第９図は処理の流れを示
す図である。 １・・・制御対象、２・・・操作部、３・・・位置制御
手段、４・・・力制御手段、５・・・位置検出部、６・
・・力検出部、７・・・位置記憶部、８・・・制御指令
生成部、２０・・・座標変換部、２１・・・マニプレー
タ、２２・・・操作部、２２ａ・・・モータ、２２ｂ・
・・パワーアンプ、２２ｃ・・・Ｄ／Ａコンバータ、２
２ｄ・・・補償器、２３・・・力検出部、２３ａ・・・
力覚センサ、２３ｂ・・・座標変換部、２４・・・力制
御部、２４ａ・・・偏差部、２６・・・位置検出部、２
６ａ・・・エンコーダ＆カウンタ、２６ｂ・・・タコメ
ータ、２７・・・位置制御部、２７ａ・・・偏差部、３
０ａ・・・逆ヤコビ行列、３０ｂ・・・加算部、４０・
・・ホスト・コンピュータ、４０ａ・・・位置記憶部、
４０ｂ・・・制御指令生成部。 せ象物 第１図 入Ｏ 対象物ヒマ；プレーグの４１１関係 第２図 俄！−）ｔｙ１作ｔｊ克明Ｔ５図弗３図襖１動作哨哨力
、伎ｌの吟間之層−第４図帝ｌ穐甲禾の虜Ｊ！ブロッグ
図 第５図 制部夛１ｒ）１笑た例　第６図

Claims (1)

1. 【特許請求の範囲】 ロボットと対象物に働く力を検出する力検出部（６）と
   、ロボットの位置を検出する位置検出部（５）と、検出
   した位置を記憶する位置記憶部（７）と、位置検出部（
   ５）の位置座標値に基づいてロボットの位置を制御する
   位置制御手段（３）と、力検出部（６）によって検出さ
   れた力に基づいてロボットの力を制御する力制御手段（
   ４）と、制御指令生成部（８）とを具備し、制御指令生
   成部（８）は、矩形対象物の表面に一定力を加えながら
   倣い動作を実行している過程において、 力検出部（６）から出力される力がゼロ又は略ぼゼロに
   なった時、位置検出部（５）によって検出された位置を
   位置記憶部（７）に記憶させ、 ロボットを停止させる指令を、位置制御手段（３）およ
   び力制御手段（４）に与え、 旧の倣い座標系を、Ｚ＿ｗ軸（倣い方向をＹ＿ｗ、押付
   力の方向をＸ＿ｗとする）の回りに反時計方向に９０度
   回転することにより、新しい倣い座標系を求め、新しい
   倣い座標系に基づくパラメータ（Ｒ、Ｒ＾Ｔ）を位置制
   御手段（３）および力制御手段（４）に設定し、 新しい倣い座標系にロボットの姿勢を一致させるための
   指令を、位置制御手段（３）および力制御手段（４）に
   与え、 位置記憶部（７）に記憶されている位置にロボットを移
   動させる指令を、位置制御手段（３）および力制御手段
   （４）に与え、 ロボットを対象物に押し付ける指令を、位置制御手段（
   ３）および力制御手段（４）に与え、押付力が設定力に
   なったときに、倣い動作指令を、位置制御手段（３）お
   よび力制御手段（４）に与えるように構成されている ことを特徴とする倣い制御方式。