JPH03240105A - ロボットの倣い制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔概　　　要〕 未知形状の作業対象物の表面を滑らかに倣うために、作
業座標系をオンラインで自動的に設定するロボットの倣
い制御装置に関し、 未知形状の作業対象物の表面を滑らかに倣うために、作
業座標系をオンラインで自動的に設定することを目的と
し、 目標力ベクトルの方向の単位ベクトルｎを第１列、該ベ
クトルｎに直交し、かつロボットの手先ら基準座標系へ
の変換行列を用いて、前記ロボットの手先の目標位置と
現在位変換行列を用いて、前記ロボットの力の目標値と
手先の対象物への作用力との偏差とによって、該ロボッ
トの位置と手先の混成制御装置を備えたシステムにおい
て、前記ロボットの作業対象物表面上に指定される倣い
作業の開始点への位置決め制御による該ロボットの手先
の空間移動後に、該開始点において該対象物方向に仮の
目標力ベクトルを指令し、該仮の目標力ベクトル方向の
並進１自由度が拘束された状態で押付は制御を行い、該
開始点において検出される該ロボットの手先の対象物へ
の作用力の該並進ｌ自由度拘束方向成分が前記仮の目標
力ベクトルの大きさと等しくなった時に制御を完了する
押付は制御手段を有するように構成する。

〔産業上の利用分野〕

本発明はロボットの制御方式に係り、さらに詳しくは未
知形状の作業対象物の表面を滑らかに倣うために、作業
座標系をオンラインで自動的に設定するロボットの倣い
制御装置に関する。

〔従来の技術〕

ロボットの手先をある力で押し付けながら対象物表面に
沿って移動させる倣い作業においては、ロボット手先に
取り付けられたプローブと作業対象物との接触点におけ
る接平面に対して、その法線方向に力制御、また接平面
内で位置制御を行うように設定される。

第１２図は倣い作業のような位置と力の混成制御におけ
る作業座標系の設定の例である。同図において、ロボッ
トアーム１に力覚センサ２を介して取り付けられたプロ
ーブ３と作業対象物４との接触点において、力制御方向
は対象物４の表面の法線方向の単位ベクトルｎで、また
位置制御方向は単位ベクトル０と■によって示される。

本発明ではプローブ３の移動方向を単位ベクトルａの方
向に一致させるものとする。

第１３図はロボットの位置と力の混成制御装置の構成例
のブロック図である。同図においてＰ□はロボットの各
関節の変位から算出されるロボットの手先の現在位置、
Ｐ、、７は手先の目標位置、ｐ　ｃｒは作業座標系の原
点、Ｆｏはロボットに装着された力覚センサで検出され
るロボット手先と作業対象物との間の作用力、Ｆ　ｏｒ
は目標力、ｅは関節速度指令である。ここで例えばＰｌ
の添字“ｒ′は例えばロボット本体の固定位置を原点と
する基準座標系表示であることを示している。

ロボットの基準座標系で記述される位置偏差は位置偏差
生成器５によって、また力偏差は力偏差生成器６によっ
て生成され、それぞれ変換行列ＲＴによって、例えばロ
ボット手先位置を原点とし、手先と作業対象物との間の
押し付けなどの拘束関係で定義される作業座標系に変換
される。ここでＲ＝（ｎ　　　ｏ　　　ａ）　　　　　
　　　　　　　　　（１）ここでＲＴの“Ｔ′は位置行
列を示し、ｎ、　　ｏ。

ａは互いに右手系で直交する単位ベクトルを示す。

作業座標系に変換された位置偏差および力偏差からそれ
ぞれ位置補償器７、および力補償器８によって速度指令
が生成されるが、位置補償器７の出力には位置選択行列
Ｓｐ９が、また力補償器８の出力には力選択行列５ｒｌ
Ｏが掛けられて、位置制御方向と力制御方向が分離され
た速度指令としてＶ　ｐｗおよびＶｆｗが生成される。

ここで例えばことを示す。例えば並進ｎ方向を力制御、
並進０およびａ方向を位置制御する場合には位置選択行
列Ｓ、および力選択行列Ｓｔは次のように与えられる。

Ｓ、＝ｄ、□　（０１１）　　　　　　　（２）Ｓｔ＝
　　ｄｏｓｅ　　（１００）　　　　　　　（３）ここ
で°ｄ　ｔｍｇ′は括弧内を主対角線要素とする対角線
行列を示す。

手先を空間移動させる場合には、その移動を高速化する
ために、例えばロボットの出し得る最高速度■。Ｘをそ
の高さとする台形状の速度関数が速度発生器１１により
フィードフォワードの速度指令Ｖ　ｏｗとして生成され
る。そしてこの■。０は積分器１２を介して位置偏差生
成器５にも人力される。これは積分器１２の出力Ｉ　（
Ｖ。ｗ（ａ））、すなわち移動開始位置ｐ　ｃｒからの
移動すべき量によって時々刻々の手先のあるべき位置を
求め、それを現在位Ｒｐ、、と比較するためである。

加算部１３により３つの速度指令の和としてロボットの
手先が出すべき作業座標系記述の速度Ｖ。８＋　Ｖ　ｐ
　−＋　Ｖ　ｔ　ｗが生成され、変換行列Ｒ１４によっ
て基準座標系記述に変換される。そしてその速度から逆
ヤコビ行列Ｊ−’１５を用いて関節速度指令ｅが生成さ
れる。ここで逆ヤコビ行列Ｊ−１はロボットの各関節の
微小時間に対する微小角変位（関節速度）ｅとロボット
手先の微小変位（速度）■とを Ｖ　＝　Ｊ　ｅ　　　　　　　　　　　　　　　　　（
４）によって関係づけるヤコビ行列Ｊの逆行列であって
、ヤコビ行列Ｊの各要素はロボットの関節の構造によっ
て決定される。

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

第１２．１３図のような位置と力の混成制御において実
際のプローブ３の先端拘束状態と設定された作業座標系
との関係にずれが生じると接触力は目標清適りには制御
されないという問題点がある。例えばベクトルａが対象
物４との接線よりも内部に向いていると、ベクトルｎ方
向の位置ずれが修正されるまで過大な接触力が生じ、ま
たベクトルａが外部に向いていると逆に接触力が過小に
なるか、またはプローブ３が対象物４の表面から浮いた
状態になる。

従ってロボットによって対象物４の表面を安定した接触
力で滑らかに倣う場合には、プローブ３の先端拘束状態
に応じてベクトルｎ、ｏ、ａをオンラインで変化させる
必要があり、対象物表面の形状が判明していなければ作
業座標系を設定することができず、また判明していても
倣い動作中に刻々と変化する接平面、およびその法線を
オフラインで準備しておかねばならないという問題点が
あった。

本発明は、未知形状の作業対象物の表面を滑らかに倣う
ために、作業座標系をオンラインで自動的に設定するこ
とを目的とする。

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

第１図は本発明の原理ブロック図である。同図ベクトル
ｎを第１列、ベクトルｎに直交し、かつ種糸から基準座
標系への変換行列を用いて、ロボット１６の手先の目標
位置と現在位置との偏差と、ロボット１６の力の目標値
と手先の作業対象物への作用力との偏差とによって、ロ
ボット１６の位置と手先の混成制御装置１７を備えたロ
ボットシステムにおける倣い制御装置の原理ブロック図
である。

第１図（ａ）は第１の発明の原理ブロック図である。

同図において、押付は制御手段１９Ａはロボ・ント１６
の作業対象物の表面上に指定される倣い作業の開始点、
例えばＴ１への位置決め制御によるロボット手先の空間
移動後に、倣い作業開始点において作業対象物の方向に
仮の目標ベクトルを指令する。ここで仮の目標力ベクト
ルの方向は接触点における作業対象物表面の法線方向で
ある必要はなく、後に法線方向ベクトルを用いて目標力
ベクトルの修正が行われる。押付は制御手段１９Ａは、
次に仮の目標力ベクトル方向の並進１自由度が拘束され
た状態で押付は制御を行い、点ＴＩにおいて検出される
ロボット手先の対象物への作用力の拘束方向成分が仮の
目標力ベクトルの大きさと等しくなった時に、接触完了
として制御を終了する。

第１図（ｂ）は第２の発明の原理ブロック図である。

同図において、倣い経路設定手段１８はロボット１６の
作業対象物表面上に指定される倣い作業の開始点ＴＩと
その終了点Ｔ２、および点Ｔ＋　とＴ２との間の倣い作
業の経由点Ｔ３に対して、３点ＴＩ　％Ｔｚ、Ｔ□で決
定される平面Φと作業対象物表面との交線を倣い作業の
経路として設定する。平面Φに対して、その法線ベクト
ルφを３点Ｔ’＋　、　Ｔ２およびＴ３から求めること
により、倣い経路は法線ベクトルφをもち、かつ点ＴＩ
を含む平面Φ上に存在することになる。

次に押付は制御手段１９Ａの作用は第１図（ａ）にその
原理を示した第１の発明におけると同様である。

第１図（ハ）の倣い作業系設定手段１９Ｂはロボツ）１
６の手先が作業対象物に接触して停止した状態で、その
接触点を作業座標系の原点、例えばｐ　ｃｒとし、また
接触点で検出された手先の対象物への作用力の方向と逆
方向の単位ベクトルを作業座標系から基準座標系への変
換行列の第１列のベクトルｎとする。次にベクトルｎと
倣い作業の経路が存在する平面Φの法線方向ベクトルφ
との外積ｎ×φ＝ｍを方向ベクトルとして持ち、かつ接
触点を通る直線、例えばｌへの倣い作業の開始点と終了
点ＴＩ、Ｔ２からそれぞれ引いた垂線の足Ｐｔｌ、ｐｔ
ｚを求め、Ｐｔｌからｐｔｚへの方向の単位ベクトルを
前述の変換行列の第３列のベクトル■し、さらに目標力
ベクトルを前述の仮の目標力ベクトルと大きさが等しく
、ベクトルｎと同一方向のベクトルとしてその方向を修
正する。

第１図（Ｃ）は第３の発明の原理ブロック図である。

同図において、座標系適合性判定手段１９Ｃはロボット
１６の手先とロボット１６の作業対象物との接触点にお
ける接触力の単位ベクトルｎ方向の成分が充分に存在し
、かつ作業座標系の原点、例えばｐ　ｃｒからロボット
手先の現在位置、例えばＰ、、。

に至るベクトルと単位ベクトル■の成す角度があらかじ
め設定されている閾値を越えた時に、設定されている作
業系を不適と判定する。

〔作　　用〕

第１の発明の原理を示す第１図（ａ）では、押付は制御
手段１９Ａによって倣い作業の開始点におけるロボット
手先の作業対象物への押付は制御が行われる。この押付
けは作業対象物表面の倣い作業開始点における法線方向
に威されることが理想的であるが、一般にその方向は未
知であるので、倣い作業開始点では仮の目標力ベクトル
が対象物が存在する方向に指令され、検出される作用力
例えばＦ□の並進拘束方向の成分が仮の目標力ベクトル
の大きさと等しくなった時点で接触が完了したものとさ
れる。そして後述のように、法線方向ベクトルｎがも求
められた時点で目標力の修正が行われる。

第２の発明の原理を示す第１図（ｂ）では、平面Φと対
象物表面との交線が倣い経路設定手段１８によって倣い
作業の経路として設定された後に、ロボット手先、例え
ばプローブの先端が倣い作業の開始点ＴＩに接触するよ
うにロボットアームの移動が行われる。そして押付は制
御手段１９Ａにょって開始点Ｔ＋において仮の目標力ベ
クトル、例えばＦ。ｒ＋を用いて押付は制御が行われ、
対象物に作用する法線方向の力Ｆ１の仮の目標力ベクト
ル方向成分が仮の目標力ベクトルの大きさと等しくなっ
た時点で、押付は制御が終了する。

次に倣い作業系設定手段１９Ｂによって、例えばプロー
ブに装着された力覚センサによって検出される接触力の
方向から作業座標系の単位ベクトルｎが決定され、また
例えば倣い作業開始点ＴＩにおける接平面■と平面Φと
の交線ｌの方向のべ→　　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　→定され、第３の単位ベクトル０は■ｎとの外積
として求められる。そして、目標力ベクトルの修正が、
求められた単位ベクトルｎを用いて行われる。

第３の発明を示す第１図（Ｃ）においては、座標系適合
性判定手段１９Ｃによって接触力のｎ方向成分が十分大
きい時、すなわちプローブが作業対ｌ物を十分な力で押
しているという条件の下で、すでに設定されている座標
系の適合性が判定され、３１の作業系が不適と判定され
た時にはプローブの移動が停止され、作業座標系と目標
力ベクトルを再設定した後に再びロボット１６による倣
い作行が続行される。

以上のように本発明によれば、倣い作業の開始点におけ
る押付は制御、倣い作業系の設定、および倣い作業中の
作業系の再設定が自動的に行われる。

〔実　　施　　例〕

第２図は倣い経路設定の実施例である。同図において、
倣い作業の開始点ＴＩと終了点Ｔ２との間に、例えば経
由点Ｔ３１が設定されると、３点Ｔ＋。

Ｔ２．Ｔ３．によって決まる平面Φ１と作業対象物４の
表面との交線が倣い経路１として設定され、また倣い作
業の経由点としてＴ３２が指定されると、平面Φ２と作
業対象物４の表面との交線が倣い経路２として設定され
る。ここで平面Φに対する法線ベクトルΦは倣い作業の
経由点をＴ３として、次式で与えられる。

力を含んでいるが、第３図はプローブ先端が倣いすなわ
ち倣い経路はΦを法線ベクトルとして、かつ倣い作業の
開始点Ｔ＋を含む平面Φ上に存在することになる。

第３図は作業対象物への押し付は制御の実施例である。

同図は倣い作業の開始点Ｔ１における倣い作業のための
最初の接触状態を示している。まず開始点Ｔ＋へのプロ
ーブ先端の空間移動が行われ、次に点Ｔ１から作業対象
物が存在する方向に大きさがＩＦ、□　ｈの仮の目標力
ベクトルが指令され、プローブ先端の並進自由度の拘束
が１の状態で、押し付は制御が行われる。この時候の目
標ベクトルは接触点Ｔ１における法線方向と一致する必
要はない。

第３図に示すように最初の接触点における押し付けは、
力覚センサによって検出される接触力Ｆ、。

の仮の目標力ベクトル方向への成分がＩＦ、１．ｌ　１
ｔとなった状態で終了する。ここで添字“ｔｏは並進ベ
クトルの大きさを示す。

プローブ先端が移動している間の接触力は摩擦作業の開
始点ＴＩで停止した時には力覚センサによって検出され
る接触力Ｆ１．．は接触点ににおける接平面Ｈの法線方
向を表しており、対象物に作用する力の反作用力とみな
せるために、作業座標系第４図は倣い経路設定用平面Φ
と接平面ｎとの交線ｌの実施例である。同図において、
プローブの先端が開始点Ｔ＋から倣い経路上の任意の点
Ｐ１ｒに移動したものとし、この点を倣い作業座標系を
設定する接触点Ｐｃｒ（原点）として作業座標系の単位
ベクトルａの設定法について説明する。同図に示すよう
に、単位ベクトルａは平面Φと平面■との交線ｌの方向
のベクトルｍに平行となる。ここでｍは次式で与えられ
る。

ｍ＝ｎｘΦ　　　　　　　　　　　　　　　（７）（７
）式で与えられるベクトルｍが第４図のように倣作業座
標系の単位ベクトル■することができる。

しかしながら、例えば対象物表面が第２図のように上に
凸でなく、凹んでいるような場合には（５）式で求めら
れる法線ベクトルΦが逆向きとなり、その結果ベクトル
ｍもプローブ移動方向と逆向きとなり、その方向の単位
ベクトルを■することはできない。

第５図は作業座標系の単位ベクトルａの設定法の実施例
である。同図において、倣い作業の開始点Ｔ、と終点Ｔ
２の直線ｌへのＰｔｌ、　　Ｐｔ２が次式％式％（１０ また目標力ベクトルは単位ベクトルｎの方向のベクトル
として仮の目標ベクトルの大きさを用いて第６図は倣い
座標系の適合性判定の実施例である。

対象物が連続した曲面である場合、作業座標系は原理上
は連続して変化させることになるが、そうすると振動的
な動作となる恐れがある。本発明では第６図に示すよう
に、作業座標系の原点ｐ　ｃｒから手先の現在位置Ｐ１
ｒへ至るベクトルａｓとａベクトルとのなす角度θ、が
方向角誤差闇値θｔｈｄを越えたときに、既設作業座標
系は不適であると単位ベク トルａはｐｔｔからＰｔｚへ至る方向の単位なるａｓベ
クトルにおいて、 作業座標系の第３の単位ベク トル０は単位ベク が真のとき、 既設作業座標系は不適である。

また、何らかの原因で接触に不安定が生じたり、ｎ方向
の力制御の追従が遅れることにより、プローブ先端が対
象物から離れることがあり得るので、倣い作業座標系の
適合性判定には、ｎ方向の接触力が充分に存在している
ことを条件とする。

Ｆ、、’ｎ≧ξ（Ｆｏｒｌ　−ｎ）　、　　（０＜ξ〈
１）・　・　・０５） を満たしているときに限る。

第７図は本発明の倣い制御装置を用いるロボット制御シ
ステムの実施例の全体構成ブロック図である。同図にお
いて、システムはＣＰＵ２０．目標力で押し付は動作、
または目標位置への移動などの１動作を行うための目標
ベクトルＰ。ｒの生成、作業座標系から基準座標系への
変換行列Ｒの生成、選択行列Ｓ、、Ｓ、の生成、フィー
ドフォワードの移動速度Ｖ　ＩＩＩＩＸの生成を行う１
動作制御装置２１、第１３図の位置と力の混成制御装置
と関節サーボ制御装置を含むリアルタイム制御装置２２
、本発明の倣い動作制御装置２３、ロボットへの動作命
令などを格納する記憶装置、例えば磁気ディスク２４、
キーボード２５、倣い作業の開始点ＴＩ。

終了点Ｔ２および経由点Ｔ３などを記憶するメモリ２６
、各関節に取り付けられたエンコーダからの信号をディ
ジタル変換するＡ／Ｄ変換器２７、各関節のモータを駆
動するパワーアンプへの信号をアナログ変換するＤ／Ａ
変換器２８、ＣＲＴ２９、磁気ディスク２４のインタフ
ェース３ｏ、キーボード２５のインタフェース３１、図
示しない力覚センサのインタフェース３２、ＣＲＴ２９
のインタフェース３３、エンコーダ用の１１０３４、お
よびパワーアンプ用の１１０３５から構成されている。

第８図は本発明における倣い動作制御の全体処理実施例
のフローチャートである。同図において、処理がスター
トするとまずステップ（Ｓ）３６で第９図で説明する前
処理が行われる。前処理では倣い作業における倣い経路
の設定や、最初の接触点における仮の目標カベク・トル
などの各条件値の設定が行われる。続いてＳ３７で倣い
作業の開始点Ｔ＋への空間移動が、また３３８で開始点
Ｔ１での仮の目標力ベクトルＦ　ｏｒｌでの押し付けが
、第７図の１動作制御装？＆２１によって各ステップに
図示のマクロ命令によって実行される。なおＳ３７のマ
クロ命令における°ＴＩＭＥ’　は空間移動命令におけ
る移動時間の指定である。

続いてＳ３９で第１０図で説明する倣い作業座標系の設
定処理が行われる。この処理では前述のルｎ、ａ、およ
び０の設定などが行われる。Ｓ３９の処理が終了した時
点で倣い作業開始時の作業座標系の設定が完了したこと
になり、Ｓ４０で倣い作業が開始される。すなわち第７
図のメモリ２６に格納されている目標位置Ｐ０２を倣い
作業の終了点Ｔ２に、フィードフォワードの移動速度Ｖ
□８を変数５ｐｅｅｄに置換することによって、リアル
タイム制御装置２２を用いて位置と力の混成制御によっ
て倣い動作が実行される。

モしてＳ４１でプローブ先端位ｆＰ、、が目標位置Ｐ０
１、すなわち倣い作業の終了点Ｔ２に到着したか否かが
、例えばリアルタイム制御装置２２内の移動終了フラグ
を監視することによって判定される。３４１でプローブ
先端が倣い作業の終了点Ｔ２に到着していない場合には
、Ｓ４２で第１１図で説明する倣い作業座標系の適合性
判定処理が行われ、適と判定された場合には３４１から
の処理が繰り返される。

Ｓ４２で倣い座標系が不適と判定された場合には、３４
３でフィードフォワードの移動速度Ｖ。Ｘとサンプリン
グ周期毎の目標位置を求めるための積分値が共にＯとさ
れて、リアルタイム制御装置２２の位置と力の混成制御
による倣い動作が停止される。モしてＳ４４でプローブ
先端が停止した点において、Ｓ３９と同様にして倣い作
業座標系の設定処理が行われ、３４０以降の処理が繰り
返される。

Ｓ４１でプローブ先端位置ｐ　ｓｒが倣い作業の終了点
Ｔ２に達したと判定された時点で、３４５〜３４７の空
間移動命令によって点Ｅ２、および点Ｔ１とＥｚ　　・
・・を経由してプローブ先端の点Ｅｆｉ、例えばホ−ム
ポジションへの移動が行われ、処理が終了する。

第９図は第８図の３３６、すなわち前処理の実施例のフ
ローチャートである。同図においてまず３４Ｂで倣い作
業の開始点Ｔ２、および点Ｔ１と終了点Ｔ２、および経
由点Ｔ３がダイレクトティーチング、またはキーボード
２５からの座標値の入力、あるいはディスク装置２４か
らの座標値の入力により設定され、メモリ２６に格納さ
れる。

続いてＳ４９で（５）式を用いて平面Φの法線ベクトル
Φが算出され、メモリ２６に格納される。Ｓ５０では倣
い動作の終了後に、例えばホームポジションまでの空間
移動における経由点として任意の個数の点の座標がダイ
レクトティーチング、またはキーボード２５あるいはデ
ィスク装置２４からの座標値の入力によって設定され、
メモリ２６に格納される。

その後Ｓ５１で倣い作業の開始点Ｔ１における初期の接
触力Ｆ。２．が仮の大目標ベクトルとしてキーボード２
５、またはディスク装置Ｆ２４からの座標成分値の入力
によって設定され、メモリ２６に格納される。例えば第
３図に示す拘束状態の場合には、Ｚｒ方向に押し付けれ
ば作業対象物への接触ができるので、仮の大目標ベクト
ルとして次式を入力する。

Ｆｏｒｌ　＝〔ｏ　　Ｏ０，５Ｅ丁 この時のＩＦｏ−＋ｌｔが倣い動作における接触力の大
きさとして設定されることになる。

その後３５２で倣い動作の速度５ｐｅｅｄが、またＳ５
３．３５４ではそれぞれ座標系の適合判定処理に用いら
れる方向角誤差閾値θｔｈｄ　、および接触判定係数ξ
がキーボード２５、またはディスク装置２４からの入力
によって設定され、メモリ２６に格納されて処理を終了
する。

第１０図は第８図の３３９、すなわち倣い作業座標系設
定処理の実施例のフローチャートである。

同図において、まずＳ５５でプローブ先端の位置Ｐ１ｒ
が座標系の原点ｐ　ｅｒとされ、Ｓ５６で（６）式によ
って単位ベクトルｎが設定され、３５７で平面Φとｎと
の交線ｌの方向のベクトルｍが設定され、３５Ｂ、３５
９において（８）および（９）式を用いて倣い作業の開
始点ＴＩと終了点Ｔ２の直線ｌへの足ＰｔｌおよびＰｔ
ｇが求められる。

続いてＳ６０で０２）式に従って目標力ベクトルが単位
ベクトルｎの方向に設定され、３６１でプローブ先端の
移動方向単位ベクトル■作業座標系の第３の単位ベクト
ル０とが００および００式を用いて設定され、３６２で
力選択行列Ｓｒと位置選択行列Ｓ２との設定が行われる
。ここで力選択行列Ｓｒ？；！ｎ方向だけが１とされ、
位置選択行列Ｓ。

は■０方向とが１とされる。

第１１図は第８図におけるＳ４２、すなわち倣い作業座
標系の適合性判定処理の実施例のフローチャートである
。同図において、Ｓ６３で０３）式によって作業座標系
の原点ＰＣ，，からプローブ先端の現在位ｆＰ、、へ至
るベクトルａｓが求められ、Ｓ６４で０４）および０５
）式が共に成立しているか否かが判定され、共に成立し
ている場合には倣い作業座標系が不適なものとして第８
図において３４３の処理に移行し、（ロ）および０９式
の両方が成立している場合以外は作業座標系が適と判定
され、第８図において３４１からの処理が繰り返される
。

なお、本実施例では第１１図の３６４で（Ｉ５）式のみ
が成立しない場合、例えばプローブ先端が対象物から離
れたときには座標系はそのままで位置と力の混成制御が
続行されるが、０５）式が不成立になったときに移動速
度を零としてプローブを停止させ、ｎ方向への力制御に
よってプローブと対象物との接触が再び行われるのを待
つ処理を入れることも可能である。

〔発明の効果〕

以上詳細に説明したように、本発明によれば作業座標系
がオンラインで自動的に設定されることになり、未知形
状の作業対象物の表面を滑らかに倣うことが可能になり
、倣い作業におけるロボットの制御性の向上に寄与する
ところが大きい。

【図面の簡単な説明】

第１図（ａ）は第１の発明の原理ブロック図、第１図（
ロ）は第２の発明の原理ブロック図、第」図（Ｃ）は第
３の発明の原理ブロック図、第２図は倣い経路の設定の
実施例を示す図、第３図は作業対象物への押し付は制御
の実施例を示す図、 第４図は倣い経路設定平面Φと接平面■との交線ｌの実
施例を示す図、 第５図はベクトルａの設定法の実施例を示す図、第６図
は倣い座標系の適合性判定の実施例を示す図、 第７図は本発明の倣い制御装置を用いるロボット制御シ
ステムの実施例の全体構成を示すブロック図、 第８図は倣い制御の全体処理実施例のフローチャート、 第９図は前処理の実施例のフローチャート、第１０図は
倣い作業座標系設定処理の実施例のフローチャート、 第１１図は倣い作業座標系の適合性判定処理の実施例の
フローチャート、 第１２図は倣い作業における座標系設定の例を示す図、 第１３図は位Ｉと力の混成制御装置の構成例を示すブロ
ック図である。 １６・　・　・ロボット、 １７・・・位置と力の混成制御装置、 １８・・・倣い経路設定手段、 １９Ａ・・・押付は制御手段、 １９Ｂ・・・倣い作業系設定手段、 １９Ｃ・・・座標系適合性判定手段。

Claims (1)

1. 【特許請求の範囲】 １）目標力ベクトルの方向の単位ベクトル■を第１列、
   該ベクトル■に直交し、かつロボット（１６）の手先移
   動方向の単位ベクトル■を第３列、該２ベクトルの外積
   ■×■＝■を第２列とする作業座標系から基準座標系へ
   の変換行列を用いて、前記ロボット（１６）の手先の目
   標位置と現在位置との偏差と、該ロボット（１６）の力
   の目標値と手先の対象物への作用力との偏差とによって
   、該ロボット（１６）の位置と力を制御する位置と力の
   混成制御装置（１７）を備えたシステムにおいて、 前記ロボット（１６）の作業対象物表面上に指定される
   倣い作業の開始点への位置決め制御による該ロボット（
   １６）の手先の空間移動後に、該開始点において該対象
   物方向に仮の目標力ベクトルを指令し、該仮の目標力ベ
   クトル方向の並進１自由度が拘束された状態で押付け制
   御を行い、該開始点において検出される該ロボット（１
   ６）の手先の対象物への作用力の該並進１自由度拘束方
   向成分が前記仮の目標力ベクトルの大きさと等しくなっ
   た時に制御を完了する押付け制御手段（１９Ａ）を有す
   ることを特徴とするロボットの倣い制御装置。 ２）目標力ベクトルの方向の単位ベクトル■を第１列、
   該ベクトル■に直交し、かつロボット（１６）の手先移
   動方向の単位ベクトル■を第３列、該２ベクトルの外積
   ■×■＝■を第２列とする作業座標系から基準座標系へ
   の変換行列を用いて、前記ロボット（１６）の手先の目
   標位置と現在位置との偏差と、該ロボット（１６）の力
   の目標値と手先の対象物への作用力との偏差とによって
   、該ロボット（１６）の位置と力を制御する位置と力の
   混成制御装置（１７）を備えたシステムにおいて、 前記ロボット（１６）の作業対象物表面上に指定される
   倣い作業の開始点Ｔ＿１と終了点Ｔ＿２、および点Ｔ＿
   １とＴ＿２の間の該倣い作業の経由点Ｔ＿３に対して、
   該３点Ｔ＿１、Ｔ＿２、Ｔ＿３で決定される平面Φと前
   記作業対象物表面との交線を前記倣い作業の経路とする
   倣い経路設定手段（１８）と、前記ロボット（１６）の
   作業対象物表面上に指定される倣い作業の開始点への位
   置決め制御による該ロボット（１６）の手先の空間移動
   後に、該開始点において該対象物方向に仮の目標力ベク
   トルを指令し、該仮の目標力ベクトル方向の並進１自由
   度が拘束された状態で押付け制御を行い、該開始点にお
   いて検出される該ロボット（１６）手先の対象物への作
   用力の該並進１自由度拘束方向成分が前記仮の目標力ベ
   クトルの大きさと等しくなった時に制御を完了する押付
   け制御手段（１９Ａ）と、 前記ロボット（１６）の手先が前記対象物に接触して停
   止した状態において、該対象物との接触点を前記作業座
   標系の原点とし、該接触点において検出された該手先の
   対象物への作用力の方向と逆方向の単位ベクトルを前記
   ベクトル■とし、該ベクトル■と前記平面Φの、法前方
   向ベクトル■との外積■×■＝■を方向ベクトルとして
   持ち、かつ前記接触点を通る直線への前記開始点Ｔ＿１
   より引いた垂線の足Ｐ＿ｔ＿１から前記終了点Ｔ＿２よ
   り引いた垂線の足Ｐ＿ｔ＿２への方向の単位ベクトルを
   前記ベクトル■とし、また目標力ベクトルを前記仮の目
   標力ベクトルと大きさが等しく前記ベクトル■と同一方
   向のベクトルとする倣い作業系設定手段（１９Ｂ）とを
   有することを特徴とするロボットの倣い制御装置。 ３）目標力ベクトルの方向の単位ベクトル■を第１列、
   該ベクトル■に直交し、かつロボット（１６）の手先移
   動方向の単位ベクトル■を第３列、該２ベクトルの外積
   ■×■＝■を第２列とする作業座標系から基準座標系へ
   の変換行列を用いて、前記ロボット（１６）の手先の目
   標位置と現在位置との偏差と、該ロボット（１６）の力
   の目標値と手先の対象物への作用力との偏差とによって
   、該ロボット（１６）の位置と力を制御する位置と力の
   混成制御装置（１７）を備えたシステムにおいて、 前記ロボット（１６）の手先と該ロボット（１６）の作
   業対象物の接触点における接触力の前記単位ベクトル■
   の方向成分が存在し、かつ前記作業座標系の原点から該
   手先の現在位置に至るベクトルと前記単位ベクトル■と
   の成す角度があらかじめ設定された方向角誤差閾値を越
   えた時に既設作業系を不適と判定する座標系適合性判定
   手段（１９Ｃ）を備え、該座標系適合性判定手段（１９
   Ｃ）が既設座標系を不適と判定した時に前記手先の移動
   を停止させて前記作業座標系および目標力ベクトルを再
   設定した後に、再び前記ロボット（１６）に倣い作業を
   続行させることを特徴とするロボットの倣い制御装置。