JPH0349885A - 力制御ロボットの倣い制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔概要〕 力制御ロボットで対象物の表面に沿って加工等を行う倣
い作業のための倣い制御方式に係わり、未知形状の鋭角
を持つ対象物に成る一定の力を加えながら倣い動作を行
う力制御ロボットの倣い制御方式を提供することを目的
とし、 対象物の面Ａ上の１点の位置ＰＩを記憶した後に面Ａを
倣うと共に押付力を監視し、押付力がゼロになった時に
はその時点の位？ｔｉｐ、を記憶し、ロボットのハンド
をＺ軸（押付力の方向をＸ、倣い方向をＹとする）回り
に反時計方向に９０度回転させ、ロボットを−Ｙ方向に
移動させ、対象物の他の面Ｂと衝突した時の位ＥＰ６を
記憶し、位置Ｐ＋　、Ｐｚ　、Ｐｂ　！：基づいて他の
面Ｂに対する倣い座標系を生成し、生成した倣い座標系
に基づいて他の面Ｂに対する倣い動作を行うものである
。

〔産業上の利用分野〕

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

〔従来の技術〕

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

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

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

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

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

（ｉ！ｌ！Ｗを解決するための手段〕 第１図は倣い座標系を説明する図である。第１図におい
て、０．ＩＸｗ　Ｙｔ＝　Ｚ、１は倣い座標系、０゜−
Ｘ。ＹｏＺｏは基準座標系、式１名、；は倣い座標系Ｏ
ｔ＝　　Ｘｔ＝　Ｙｓ＝　Ｚｔ−の単位ベクトルをそれ
ぞれ示している。

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

ベクトルｎ、ｏ、ａは、それぞれ倣い座標系の座標軸Ｘ
ｗ　、Ｙｗ　、Ｚｗの単位ベクトルである。古は対象物
へ力を加えたときの押付方向を示し、対象物の表面の法
線ベクトルと同じである。

８は杏と直交関係にあり、倣い動作時のロボット先端の
移動方向を示す。富は、古とδに直交するように定めら
れ、 奮＝式×才 で与えられる。

漬、７．言を基準座標系Ｘ０ 成分表示を行うと、 ｎ＝　（ｎＸｎ、ｎ、）〒 ３＝　（ｏ、ｏ、ｏ・）゛ Ｙ。

Ｚ。

について となる。ただし、Ｔは転置行列を示す。

第２図は対象物とマニプレータの位置関係を示す図であ
る。同図において、Ｐ＋　、Ｐｚ、Ｐｓはロボットの位
置を示す。

第２図を用いて、鋭角を持つ対象物へ倣い動作を如何に
して行うかを示す。同図において、面Ａと面Ｂの交錯す
る角が鋭角である。

制御対象は対象物の成る面（例えば面Ａ）に倣い動作を
行っており、このとき倣い座標系は第２図の位置Ｐ、の
ように与えられているものとする。

図示するように、ベクトル貧は面に垂直、ベクトル８と
倉は面に平行である。

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

第３図は倣い動作を説明する図である。第３図は対象物
を上方（面Ａ、Ｂに対して垂直方向）から見た図であり
、ロボットが位ｔｐｚ付近でどのような動作をするかを
示したものである。

第４図は倣い動作時の力９位置の時間応答を示す図であ
る。第４図は、面Ａに対する倣い動作時の負方向への押
付力Ｆ２と、ロボットのハンド位ｔ　（Ｘｈ　、Ｙｈ　
）の時間履歴を示したものである。

なお、ｚ８軸の座標値は省略している。ｔ（ｐ＋）。

ｔ（ｐｇ）、　　ｔ（Ｐａ）、　　ｔ（Ｐｓ）、　　ｔ
（Ｐｈ）は、それぞれロボットが位置Ｐ＋　、Ｐｇ、Ｐ
ａ、Ｐｓ、Ｐｈに達した時刻である。

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

第５図は、位置Ｐ１°で記述される倣い座標系を位置Ｐ
３での倣い座標系に切り換え、第３図のように、ロボッ
トの姿勢をＰ２→Ｐ７→Ｐ、に変えるだめの力制御ロボ
ットの制御系の原理ブロック図である。以下、第３．４
．５図を用いて鋭角を有する対象物へ倣い動作を行うた
めの制御方法について説明する。

先ず、面Ａでの倣い動作開始点位置Ｐ１のハンドの位置
（Ｘｈ　、　Ｙｈ　）を第５図の位置記憶部７に記憶す
る。第３図に示すように、ロボットが面ＡをＰｌ−＋Ｐ
２のように倣い動作を行い、時刻Ｌ（Ｐ２）に位置Ｐ２
に達したとする。位置Ｐ２よりも右側では対象物が存在
しないが、この時点では制御指令は変更されていないの
で、ロボットは負方向への押付力を発生しようとして負
方向へ移動する。また、８方向への移動成分もあるため
、結局、ロボットは位置Ｐ！から位置Ｐ、へ進むように
動作する。このとき、第５図の力検出部６で検出される
ｎ方向の押付力Ｆ　４は、第４図（ａ）のような時間応
答を示す。すなわち、面Ａを倣っている間（ｔ　（Ｐ＋
）〜ｔ（ｐｇ）　）は、一定の力で押付力を発生してい
るが、Ｐ２に達して対象物がな（なると、反力がなくな
るため押付力Ｆ才は零となる。また、第５図の位置検出
部５で検出されるロボットのハンドの位置（Ｘｈ、Ｙｈ
　）の時間応答は、第４図（５）および（Ｃ）のように
なり、位？ｆｆｐ、に達すると、Ｘ、方向へも移動を行
う。ただし、ここではＸ１Ｙｈ方向への移動は全て等速
で行われているものと仮定する。

第５図の制御指令生成部−８ａでは、第４図（ａ）のよ
うな力の変化（ＦＴ＝Ｏ）を検出すると、（ａ）　　位
置Ｐ２のハンド座標値（第４図（ｂ）、　（Ｃ）の黒丸
で示すｘ、、ｙｈＯ値）の記憶 （ｂ）　　ロボットへの停止指令 を直ちに行う。位置制御手段３及び力制御手段４は、（
ｂ）の停止指令が送られて来ると、ロボットの動作を停
止する。この時のロボットの位置はＰ４である。

ロボットの姿勢をＰ４からＰ、にするには、ハンド座標
系０．４　ＸＨＹＨＺｏのＺ８軸回りに−π／２ハンド
を回転する。ハンド座標系はハンドのマニプレータに対
する位置および姿勢から決定される座標系であり、各座
標軸ＸＭ、ＹＨ，Ｚ。

に対して単位ベクトルｎ　Ｈ＋　　ｆｆＨ＊　　Ｍ　Ｈ
が定められる。第３図においては、位置Ｐ４でのハンド
座標系をｎ　ｌＰｉ　）　ＴＨＰ４　＊　　ａＨＰ４と
し、位’ｉｎ　ｐ　ｓでのハンド座標系を′？ｉＭＰ５
　＋　？’ＨＰＳ　Ｈ３エ７．と取っている。ただし、
３　ｌＰｉ　＋　ｆＨＰｓは紙面垂直下向きのベクトル
である。

ハンドの姿勢をハンドＰ４の姿勢からＰ、の姿勢に変化
する変換式はハンド座標系にＯＮ　　Ｘ。

ＹＨ２Ｈに関する回転座標変換行列Ｅ’（−予）を用い
て次式のように表される。

〔五ＨＰＳ　ＯＨＰＳ言□、〕

、＝Ｅｋ（Ｔ＞［れ、４８□４言、Ｐ４］・・・■ ハンド座標系をＰ５に変えた後、−Ｙ、方向への押付け
を行う。ロボ・ントは、対象物に接して設定力に達する
まで対象物への押付けを行う。この時の位置はＰ、であ
る。オ、方向の力の応答は第４図（ａ）のようになり、
時刻ｔ　（Ｐ６）以降はＦマー−定に保たれる。

第５図の制御指令生成部８ａでは、時刻ｔ　（ｐｈ）で
設定力に達したことを検出すると、位Ｔｘ　ｐ　ｂのハ
ンドの座標値（第３図、第４図（ｂ）および第４図（Ｃ
）の黒三角で示すｘ、、ｙｈＯ値）の記憶指令を第５図
の位置記憶部７に送り、位置記憶部７はＰ６の位置を記
憶する。

以上のようにして第５図の位置記憶部７に記憶した位Ｉ
Ｐ、、Ｐ、、Ｐ、の座標値から面Ｂに対する倣い座標系
を求める方法について述べる。ここで、ハンドの位置の
位置ベクトルおよび座標値Ｉｐ　Ｉ　　（Ｐ　ＩＸ、　
　Ｐ　＋ｙ、　　Ｐ　＋り。

’Ｐ　ｚ　　（Ｐ　ｚｘ、Ｐ　２１／＋　　Ｐ　ｚｊ　
。

ｌＰｉ　　（Ｐ６Ｘ、　　Ｐ６Ｆ、　　Ｐｉｌｌ）とす
る。第３図および４図ではＸｈ−ｙｈ平面でしか考えて
いないため、Ｚ座標は与えられていないが、一般的に扱
うために以下の説明ではＺ座標の値も含めて述べる。

第３図で位置Ｐｌの倣い座標系をＰ３の倣い座標系に変
換するためには、面Ａと面Ｂの成す角φを求めなければ
ならない。

角φを位置ベクトルＩＰ　＋　、　ＩＰｚ　、　ｌＰｉ
を用いて表すと、 ・・・■ ただし、上式において・は内積を示し、１１　は絶対値
を表す。

Ｐ、での倣い座標系〔式、１ｔ、言Ｐ１）がらＰ３での
倣い座標系（Ｌ、Ｌ、百Ｐ３〕へ切り換えるためには、
第６図のＰｌとＰ３の倣い座標系の関係から判るように
、〔芥□、　？６．　ｆＰｌ）を軸言、。

回りにπ−φだけ回転すると良い。Ｐ、での倣い座標系
からＰ３での倣い座標系を求めるとＰｌの倣い座標系に
関する回転座標変換行列Ｅｋ′）を用いて、 ・・・■ で表される。

ロボットのハンドの姿勢を位置Ｐ２の状態から位ＩＰ、
の状態に変化させるときも０式で用いた回転座標変換行
列Ｅｂｏｃ−０を、Ｐ２のハンド座標系（貧□２古□ｐ
ｇ　１ｏｒｚ　）にかけてＰ７のハンド座標系〔π□Ｐ
？　５ＨＰ７　ｊｓｐｔ　）を求めると良い。

また、ロボットのハンド位置をＰ６からＰフヘ移動する
ときには、ハンドの大きさ（第３図のｌ）を考慮して面
Ｂの延長方向へ！たけずらして移動を行う。このときハ
ンドの座標値および姿勢は次式で表される。

時＝ＩＰ、−＋４（叱−ＩＰ、）／ｌ　　職−ｐｔ　ｌ
　　　　・・・■（ＴＬｐ−１７３，ｒ？　ｍｓｒり　
）　　　＝　　Ｅ　　ｋ　”−”　　　（ｘ、、　　ｉ
、、　　　ｆｓｐ　　Ｅ・・・■ 式■〜■で用いる座標変換行列、ベクトルの算出は、第
５図に示す座標変換行列生成部８ｂで行う。

Ｐ、での倣い座標系、Ｐ７でのロボットの位置及びハン
ド座標系が求まると、第５図の制御指令生成部８ａから
制御指令のパラメータが位置制御手段３および力制御手
段４に設定される。ロボットは、位置Ｐ６から位置Ｐｑ
へ姿勢を変えながら移動し、面Ｂへの押付動作を行う。

このときの押付方向は位置Ｐ、で示されている倣い座標
系に従う。位置Ｐ、で設定された力に達すると、再び面
Ｂに対する倣い動作を再開する。

なお、以上の説明では簡略化するためＸｈＹ、平面で説
明したが、式■〜■の一般式を用い、後に述べるような
倣い座標系の基準座標系への座標変換を行うと、基準座
標系でも同様に説明できる。〔実施例〕 第７図は制御装置の１実施例の機能ブロック図である。

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

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

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

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

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

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

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

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

制御指令生成部４０ｂは、 ■　目標位置ｘ、や力指令Ｆｒ等の制御指令、■　倣い
座標系の切換えに伴う位置制御パラメータや力制御パラ
メータの送信、 ■　位置記憶部への位置記憶命令や検出した力Ｆ０のモ
ニタリング等のタイミングの発生、を行う。制御指令生
成部４０ｂには、座標変換行列生成部４０ｃが付随して
おり、倣い座標系やハンド座標系の座標変換を行う座標
変換行列やベクトルを算出する。

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

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

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

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

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

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

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

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

第９図は本発明を実施するためのシステムの構成例を示
す図である。同図において、１０はロボット・コントロ
ーラ、１０ａはメモリ、１０ｂは通信制御部、１０ｃは
ロボット制御部、２１はマニプレータ、２２は操作部、
２３ａは力覚センサ、２６は位置検出部、４０はホスト
・コンピュータ、４０ａは位置記憶部、４０ｂは制御指
令生成部、４０ｃは座標変換行列生成部、４０ｄは通信
制御部、４０ｅはメモリをそれぞれ示している。

ホスト・コンピュータ４０は、制御指令生成部４０ｂ、
座標変換行列生成部４０ｃ、メモリ４゜ｅおよび通信制
御部４０ｄを備え、メモリ４０ｅ内に位置記憶部４０ａ
が存在する。ロボット・コントローラ１０は、メモリ１
０ａ９通信制御部１０ｂおよびロボット制御部１０ｃを
有している。

ロボット制御部１０ｃは、力制御部や位置制御部座標変
換部、偏差部などから構成されている。また、ロボット
・コントローラ１０は、操作部２２や位置検出部２６を
介してマニプレータ２１の制御を行う。ホスト・コンピ
ュータ４０とロボット・コントローラ１０はバス等の通
信インタフェースによって接続されており、それぞれの
送受信のタイミングを管理する通信制御部１０ｂ、４０
ｃによって、メモリ１０ａと４０ｃ間でデータが転送さ
れる。

第１０図（ａ）、　（ｂ）は処理の流れを示す図である
。

第９図のようなシステム構成で第３図で説明したような
鋭角を有する対象物に対して倣い動作を行う場合の処理
の流れを、第１０図を用いて説明する。第１０図はフロ
ーチャート形式で、ホスト・コンピュータ内の制御指令
生成部５０１位置記憶部５１．座標変換行列生成部５２
およびロボット・コントローラ５３での処理の流れを示
したものである。同図において、実線の矢印は処理の流
れを示し、点線の矢印は各動作時に転送される指令及び
データの流れを表している。以下の説明では、第３図の
位置Ｐ２から位置ｆｐ、への切換え方法について詳述し
、ハンドを位？ａ　Ｐ　＋までどのように移動させて面
Ａに対する倣いを行わせるかについての説明は省く。

ハンドが対象物の面Ａに接した時点で、制御指令生成部
５０は処理５０ａにより、位置記憶命令をロボット・コ
ントローラ５３に送り、処理５３ａで位置、Ｐｌの座標
値を検出して位置記憶部５１の処理５１ａで位置を記憶
する。処理５０ｂは、面Ａへの倣い動作指令であり、ロ
ボット・コントローラ５３は処理５３ｂにより面Ａへの
倣い動作を行う０面Ａへの倣い動作の説明の詳細は省略
する。

制御指令生成部５０では、ハンドが面Ａの倣い動作を行
っている間、処理５３ｂによって送られてくるハンドの
押付力Ｆのモニタを行い、処理５０ｄのようにＦ７がゼ
ロ、すなわち、ハンドの位置がＰ２に達して対象物から
の反力がなくなったか否かを判定する。この時の判定は
誤差を考慮してＦ、、ｔ　（＜１）よりも、Ｆ７の絶対
値ＩＦ。

が小さいか否かで行っても良い。制御指令生成部５０は
反力がなくなって位置ｐｚへ達したと判断する（Ｆ、、
＝Ｏ）と、処理５０ｅによって、ロボット・コントロー
ラの位置検出部２６．座標変換部２０で検出した位置Ｐ
、の座標値（χ、、、Ｙ、。

Ｚゎ）を位置記憶部へ転送する０位置記憶部５１では、
処理５１ｂのように位置Ｐ２の座標値を処理５３ｄの操
作で受は取り、記憶する。次に、制御指令生成部５０は
、処理５０ｆにより力指令Ｆ、＝０．速度指令■。＝０
．目標位置Ｘ、＜＝現在位置）を指令し、マニプレータ
を停止させる。

停止後、処理５０ｇにより、ハンド座標系のＺ軸回りに
−π／２回転したハンドの姿勢を式■を用いて算出し、
目標位置Ｘ、、速度指令■。をロボット・コントローラ
５３に送る。ロボ・ント・コントーラ５３では、送られ
てきたＸ、および■。に従ってマニプレータ、ハンドの
姿勢を変える（処理５３ｆ）、姿勢変化後、第３図で示
したＹ、軸の負方向へ押付指令を発生しく処理５０ｈ）
、力指令Ｆ、および速度指令■。をロボット・コントロ
ーラ５３へ送る。ロボット・コントローラ５３は、−Ｙ
ｈ力方向の押付は動作を行う（５３ｇ）。

制御指令生成部５０は、指令後、力をモニタするため、
ロボット・コントローラ５３で検出した力（５３ｈ）を
受信する（５０ｉ）。処理５０ｊの判定で、押付は力Ｆ
ヶ、が設定力Ｆ、に達したと判断すると、ハンドと面Ｂ
への接触位置Ｐ、に対する位置記憶命令をロボット・コ
ントローラ５３に送る（５０ｋ）。ロボット・コントロ
ーラ５３は、Ｐ６の座標値を検出し、位置記憶部５１へ
値を送る（５３ｉ）。位置記憶部５１では、送られてき
た値を受信し、記憶する。処理５０ゑで、倣い座標系の
座標変換を行うための行列を生成する指令を座標変換行
列生成部５２に送る。同時に、位置記憶部５１に記憶し
である位？＆Ｐ＋　、Ｐｇ　、Ｐｈの座標値を座標変換
行列生成部５２に送るための指令も位置記憶部５１へ送
る。座標変換行列生成部５２は、位置記憶部５１から送
られてきた位置ｐ、、Ｐｚ、Ｐ＆の座標値を基に、面Ａ
と面Ｂの交錯する角度φを式■に従って算出する（５２
ａ）。同様に、位置ｐｓでの倣い座標系の算出（５２ｂ
）、位置Ｐ、のハンド座標値及びＰ、での姿勢の算出（
５２ｃ）を行う。処理５２ｂおよび５２ｃでは、算出の
度に計算結果と計算終了フラグを制御指令生成部５０へ
送る。制御指令生成部５０は、終了フラグが送られて来
ると、次の処理に進む（５０ｍ、５０ｎ、５０ｏ、５０
ｐ）。

次に、面Ｂに正しい姿勢で接するため、制御指令生成部
５０から位置Ｐ７への移動指令がロボ・ント・コントロ
ーラ５３に送られる（５０ｑ）。ロボット・コントロー
ラ５３では、送られてきた目標位置、姿勢ｘｒ及び速度
指令■。に従ってＰ６からＰ、への移動及び姿勢変化を
行う（５３ｊ）。

移動後、倣い座標系を処理５２ｂで求めた座標系に切り
換えて（５０ｒ、５３ｋ）、新しい座標系のベクトル賓
方向への押付けを行う（５０ｓ、５３２）。制御指令生
成部５０では、力Ｆ７が設定力になったか否かを判断し
く５０ｔ、５０ｕ）、設定力に達していれば、再び倣い
動作の指令をロボット・コントローラ５３に送る（５０
ｖ）。ロボット・コントローラ５３は、新しい倣い座標
系を用いて、面Ｂへ一定ツＪＦ、、を発生しながら倣う
。

〔発明の効果〕

以上の説明から明らかなように、本発明によれば、鋭角
を有する対象物に対する倣い動作を自動的に行うことが
出来、従来、ロボットの操作者が行っていた対象物体の
変更や対象物の位置ずれに伴うロボットへの教示を行う
必要がなくなり、操作者の負担が軽減される。

【図面の簡単な説明】

第１図は倣い座標系を説明する図、第２図は対象物とマ
ニプレータの位置関係を示す図、第３図は倣い動作を説
明する図、第４図は倣い動作時の力と位での時間応答を
示す図、第５図は制御系の原理ブロック図、第６図は第
３図の位ＲＰ１の倣い座標系と位置Ｐ３の倣い座標系の
関係を示す図、第７図は制御装置の１実施例の機能ブロ
ック図、第８図は位置制御部および力制御部の機能ブロ
ック図、第９図はシステムの構成例を示す図、第１Ｏ図
（ａ）、　（ｂ）は処理の流れを示す図である。 １・・・制御対象、２・・・操作部、３・・・位置制御
手段、４・・・力制御手段、５・・・位置検出部、６・
・・力検出部、７・・・位置記憶部、８ａ・・・制御指
令生成部、８ｂ・・・座標変換行列生成部、２０・・・
座標変換部、２１・・・マニプレータ、２２・・・操作
部、２２ａ・・・モータ、２２ｂ・・・パワーアンプ、
２２Ｃ・・・Ｄ／Ａコンバータ、２２ｄ・・・補償器、
２３・・・力検出部、２３ａ・・・力覚センサ、２３ｂ
・・・座標変換部、２４・・・力制御部、２４ａ・・・
偏差部、２６・・・位置検出部、２６ａ・・・エンコー
ダ＆カウンタ、２６ｂ・・・タコメーク、２７・・・位
置制御部、２１ａ・・・偏差部、３０ａ・・・逆ヤコビ
行列、３０ｂ・・・加算部、４０・・・ホスト・コンピ
ュータ、４０ａ・・・位置記憶部、４０ｂ・・・制御指
令生成部、４０ｃ・・・座標変換行列生成部。

Claims (1)

【特許請求の範囲】
1. （１）ロボットに操作量を与える操作部（２）と、ロボ
   ットと対象物に働く力を検出する力検出部（６）と、 ロボットの位置を検出する位置検出部（５）と、検出し
   た位置を記憶する位置記憶部（７）と、位置検出部（５
   ）の位置座標値に基づいてロボットの位置を制御する位
   置制御手段（３）と、 力検出部（６）によって検出した力に基づいてロボット
   の力を制御する力制御手段（４）と、 位置記憶部（７）に記憶されている位置情報からロボッ
   トの倣い座標系を算出する座標変換行列生成部（８ｂ）
   と、 ロボットへ力指令及び位置指令並びにパラメータの転送
   を行う制御指令生成部（８ａ）と、を有し、 制御指令生成部（８ａ）は、ロボットのハンドが対象物
   の面（Ａ）に接している時点で、位置検出部（５）によ
   って検出された位置（Ｐ＿１）を位置記憶部（７）に記
   憶させ、 位置（Ｐ＿１）を記憶した後、面（Ａ）への倣い動作を
   位置制御手段（３）および力制御手段（４）に行わせ、
   力検出部（６）によって検出された押付力がゼロ又は略
   ほゼロになったか否かを監視し、 押付力がゼロになった時には、位置検出部（５）によっ
   て検出された位置（Ｐ＿２）を位置記憶部（７）に記憶
   させ、 ロボットを停止させる指令を、位置制御手段（３）およ
   び力制御手段（４）に与え、 ハンドをＺ軸（押付力の方向をＸ、倣い方向をＹとする
   ）の回りに反時計方向に９０度回転させるための指令を
   、位置制御手段（３）および力制御手段（４）に与え、 −Ｙ方向への押付け指令を、位置制御手段（３）および
   力制御手段（４）に与え、 力検出部（６）によって検出された押付力が設定値に等
   しいか否かを監視し、 ハンドが対象物の他の面（Ｂ）に衝突して押付力が設定
   値に等しくなった時点で、位置検出部（５）によって検
   出された位置（Ｐ＿６）を位置記憶部（７）に記憶させ
   、 倣い座標系の座標変換を行うための行列を生成する指令
   および位置記憶部（７）に記憶されている位置（Ｐ＿１
   、Ｐ＿２、Ｐ＿６）を、座標変換行列生成部（８ｂ）に
   与えるように構成され、 座標変換行列生成部（８ｂ）は、位置（Ｐ＿１、Ｐ＿２
   、Ｐ＿６）に基づいて他の面（Ｂ）に対する倣い座標系
   を生成するように構成されている ことを特徴とする力制御ロボットの倣い制御方式。