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

Abstract

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

Description

【発明の詳細な説明】 ［目次コ 概要 産業上の利用分野 従来の技術 発明が解決しようとする課題 課題を解決するための手段　（第１図）作用（第１図） 実施例 （ａ）ロボットと対象物の関係（第２図）（ｂ）ロボッ
ト倣い制御の概略（第３図）（Ｃ）倣い座標系の算出（
第４図、第５図）（１）始点での法線ベクトルの算出 （２）始点での移動方向ベクトルの算出（３）点Ｐｉで
の法線ベクトルの算出 （４）点Ｐｉでの移動方向ベクトルの算出（５）点Ｐｉ
′での移動方向ベクトルの算出（移動方向を切り換える
場合） （ｄ）倣い座標系切換タイミング （ｅ）倣い動作 （１）目標力の発生 （２）目標位置の発生 （３）倣い動作の終了 （ｆ）補足 （ｇ）本発明の一実施例（第６図） ・全体の構成 ・力検出部 ・位置制御部 ・法線ベクトル算出部 ・移動方向ベクトル算出部 ・制御指令生成部 ・力制御部（第７図） ・位置制御部（第７図） ・ハンド移動制御部 ・操作部 ・軌道平面切換部 （ｈ）全体の動作（第８図） （ｉ）本発明の他の実施例 発明の効果 ［概要］ 力覚センサをロボットハンドに取付け、力覚センサで検
出した対象物からの反力に基づいて対象物の法線方向を
求め、検出反力を設定値に維持しながら、ハンドを法線
方向に直交する方向（接線方向）に移動させて対象物表
面を倣わせるロボット倣い制御装置に関し。

教示作業や曲面の定義が不要であり、しかも作業条件を
設定するだけでロボットハンドをして対象物表面像わせ
ることができるロボット倣い制御装置を提供すること、
及び所定の条件が満たされた時、ロボットの倣い軌道平
面を切り換えることができるロボット倣い制御装置を提
供するを目的とし。

ロボットのハンドに取付けられ、該ハンドが対象物から
受ける反力を検出する力検出部と、ハンドの位置あるい
は移動量を検出する検出部と、力検出部により検出され
た反力に基づいて対象物の法線ベクトルを算出する法線
ベクトル算出部と、法線ベクトルに直交する方向であっ
て、予め特定されている倣い軌道平面上の移動方向ベク
トルを算出する移動方向ベクトル算出部と、法線方向の
目標力と検出反力との差に基づいて法線方向の速度指令
を出力する第１の速度指令出力部と、移動方向の目標位
置と検出位置との差、あるいは目標移動量と速度移動量
との差に基づいて移動方向の速度指令を出力する第２の
速度指令出力部と、第１、第２速度指令出力部から出力
される速度指令に基づいて前記各差が零となるようにハ
ンドを移動させるハンド移動制御部と、ハンドが移動し
て、予め設定されている倣い軌道平面切り換え条件が成
立した時、倣い軌道平面を設定されている別の軌道平面
に切り換える軌道平面切換部を有するように構成する。

［産業上の利用分野］ 本発明はロボット倣い制御装置に係わり、特に力覚セン
サをロボットハンドに取付け、力覚センサで検出した対
象物からの反力に基づいて対象物の法線方向を求め、検
出反力を設定値に維持しながら、ハンドを法線方向に直
交する方向（接線方向）に移動させて対象物表面を倣わ
せるロボット倣い制御装置に関する。

自由曲面を持った対象物の表面に沿ってツールを移動さ
せて該対象物に所定の作業、例えば塗装作業等を施した
い場合があり、最近、かかる作業をロボットに行わせる
ようになってきている。

［従来の技術］ 従来、ロボットにより自由曲面を持った対象物に対して
塗装等の作業を行わせるには、予め手動により対象物の
表面に沿ってロボットのハンドを移動させると共に適所
にロボット動作を指定し。

これによりロボット制御用のプログラムを作成しく教示
）、実際の作業に当たっては教示により作成したロボッ
ト制御用プログラムを用いてハンドを移動させて対象物
に対して所定の作業を行わせる。

また、教示によらず、オフラインでロボット制御用のプ
ログラムを作成する方法もある。この方法は予め対象物
の曲面を点列で定義し、該曲面上の点列をたどってハン
ドを移動させる移動通路を作成し、適所にロボット動作
指令を挿入してロボット制御用プログラムを作成する。

そして、実際の作業に当たっては作成したロボット制御
用プログラムを用いてハンドを移動させて対象物に対し
て所定の作業を行わせる。

［発明が解決しようとする課題］ しかし、教示による方法では、オペレータが実際の作業
に即して手動によりロボットハンドを移動させなくては
ならず、操作が面倒となる問題がある。

また、オフラインによる方法では、予め対象物の自由曲
面を定義しなくてはならないという問題がある。

以上から１本発明の目的は教示作業や曲面の定義が不要
であり、しかも作業条件を設定するだけでロボットハン
ドをして対象物表面像わせることができるロボット倣い
制御装置を提供することである。

本発明の別の目的は、所定の条件が満たされた時、ロボ
ットの倣い軌道平面を切り換えることができるロボット
倣い制御装置を提供することである。

［課題を解決するための手段］ 第１図は本発明の原理説明図である。

１は制御対象であるロボットのハンド、１′はマニピュ
レータ、２はロボットのハンドに取付けられ、該ハンド
が対象物から受ける反力（接触力）を検出する力検出部
、３はハンドの位置を検出する位置検出部、４は力検出
部により検出された反力に基づいて対象物の法線ベクト
ルを算出する法線ベクトル算出部、５は法線ベクトルに
直交する方向であって、予め特定されている倣い軌道平
面上の移動方向ベクトルを算出する移動方向ベクトル算
出部、６は法線方向の目標力ｏＦｒと移動方向の目標位
置または目標移動量１ｌＸｒを出力する制御指令生成部
、７は法線方向の目標力と検出反力との差に基づいて法
線方向の速度指令を出力する力制御部、８は移動方向の
目標位置と検出位置との差に基づいて移動方向の速度指
令を出力する位置制御部、９は各速度指令を合成する合
成部、１０は合成速度指令に基づいてハンドを移動させ
るハンド移動制御部、１１は軌道平面切換条件や切換後
の軌道平面等の設定や操作を行う操作部、１２は設定さ
れた各種データを記憶する設定データ記憶部、１３はハ
ンドが移動して、予め設定されている倣い軌道平面切り
換え条件が成立した時、倣い軌道平面を設定されている
別の軌道平面に切り換える軌道平面切換部、１４は移動
方向ベクトルを規定するための倣い軌道平面（移動方向
）を記憶する移動方向記憶部である。

［作用コ 予め、操作部１１から倣い軌道平面切換条件や切換後の
軌道平面データを設定して設定データ記憶部１２に記憶
しておく。

かかる状態において、ロボットのハンド１に取付けられ
た力検出部２を用いて、対象物からの接触力（反力）を
検出し、該反力に基づいて、法線ベクトル算出部４と移
動方向ベクトル算出部５において対象物の法線ベクトル
と、該法線ベクトルに直交する方向であって予め特定さ
れている倣い軌道平面上に存在する移動方向ベクトルを
算出する。制御指令生成部６は、法線ベクトルと移動方
向ベクトルを用いて法線方向における力指令１１Ｆｒと
移動方向における位置指令０又ｒを生成し、力制御部７
で力指令と検出反力との差に基づいて法線方向の速度指
令を出力し、位置制御部８で指令位置と検出位置との差
に基づいて移動方向の速度指令を出力する。ハンド移動
制御部１０は両速度指令を合成した合成速度指令に基づ
いて、前記各差が零となるようにハンドを移動させる。

以後、上記力制御、位置制御に基づいて、検出反力を設
定値に維持しながら、ハンドを法線方向に直交する方向
（接線方向）に移動させて対象物表面を倣わせる。

そして、ハンドが移動して倣い軌道平面切換条件が成立
すると軌道平面切換部１３は倣い軌道平面を、予め設定
してある別の軌道平面に切り換える。これにより以後、
切り換えられた軌道平面上であって、法線方向に直行す
る方向にハンドは移動する。

このように、力検出部と位置検出部を設け、検出値に基
づいて力制御、位置制御を行って、検出反力を設定値に
維持しながら、ハンドを法線方向に直交する方向（接線
方向）に移動させて対象物表面を倣わせるように構成し
たから、教示作業や曲面の定義が不要であり、しかも作
業条件を設定するだけでロボットハンドをして対象物・
表面を倣わせることができる。又、所定の条件が満たさ
れた時、ロボットの倣い軌道平面を自動的に切り換える
ようにして倣うように構成したから、所望の範囲全体を
自動的に倣わすことができる。

尚、倣い軌道平面を該平面上の１つのベクトルで、ある
いは倣い軌道平面と直交する平面上の１つのベクトルで
特定するようにすれば、軌道平面の設定を容易に行うこ
とができる。又、倣い軌道平面の切換を所定位置に到達
した時、あるいは検出反力が設定値になった時、あるい
は移動時間または移動距離が設定値になった時に行うよ
うにすれば、これら切換条件と切換後の倣い軌道平面を
設定しておくだけで自動的に倣い軌道平面を切り換えて
対象物表面を倣うことができる。

［実施例コ （ａ）ロボットと対象物の 第２図は本発明のロボット倣い制御におけるロボットと
対象物の関係図であり、ＲＢＴは関節型のロボット、Ｗ
Ｋはロボットの作業対象となる対象物である。ロボット
ＲＢＴにおいて、１１はハンド、１２はハンドを対象物
ＷＫに接触させた時の接触力（対象物からの反力）を検
出する力覚センサ（力検出部）、２１はマニピュレータ
、２２は関節軸であり、ロボットには原点をＯｏとする
ロボット基準座標系ＸｏＹｏＺｏが定められている。

Ｏｗ−ＸｗＹｗＺｗはロボットハンドと対象物の接触点
Ｏｗを原点とする倣い座標系（ワーク座標系）であり、
対象物に対するロボット先端部（ハンド）の位置、姿勢
により決定される座標系である。ベクトル百、！、Ｍは
それぞれ倣い座標系の座標軸Ｘｗ、Ｙｗ、Ｚｗについて
の単位ベクトルで、ベクトル臀は対象物ＷＫへカを加え
る時の押しつけ方向を示し、対象物の表面の法線ベクト
ルと同じ方向である。ベクトル６は、ベクトル百と直交
関係にあり、倣い動作時のロボット先端の移動方向を示
す。ベクトル百は、ベクトル百、６に直交するように定
められ、 百＝盲　ｘ６ で与えられる。

ベクトル盲、５．百のロボット基準座標系Ｘ。

−Ｙｏ−Ｚｏについての成分表示を行うと、百＝　（ｎ
ｘ　ｎｙ　ｎｚ）　” ’６＝　（ｏｘ　ｏｙ　ｏｚ）” ＝　　（ａｘ　　ａｙ　　ａｚ）　” となる。但し、Ｔは転置行列を示す。

（ｂｌｂ」１４Δ１整 第３図は本発明に係わるロボット倣い制御の概略説明図
である。本発明では、力覚センサ１２で検出した対象物
ＷＫからの接触力（反力）に基づいて、ハンド１１と対
象物ＷＫの接触点Ｐ１における法線方向百を求め、検出
反力が設定値と等しくなるように法線方向におけるハン
ド押しっけ力を制御しながら、ハンドを法線方向と直交
する方向（接線方向）５に移動させて対象物表面を倣う
ようにしている。

さて、ハンド１１が対象物ＷＫの曲面上を倣い、接触点
がＰ工からＰ２になると、該接触点Ｐ２での法線方向が
ポイントＰ工における法線方向（押しつけ方向）と一致
しなくなる。このため、ポイントＰ２において、面の法
線方向と押しつけ方向が一致しなくなり、対象物に対し
て発生する押しつけ力が指令値からずれ、正しく対象物
表面を倣えなくなる。そこで、正確な力を発生するため
には、倣い座標系をハンドの移動に従って切り換える必
要がある。すなわち、ハンドの移動に従って、倣い座標
系のＸｗ軸が法線方向を向くように、倣い座標系を切り
換える必要がある。倣い座標系を切り換えるタイミング
としては、 ■）所定時間毎に行う、 ２）対象物から受ける反力が設定力がらある程度以上ず
れた時に行う、 ３）ロボットが一定距離移動した時に行う等の方法があ
る。

本発明では、所定サンプリング時間ごとに法線方向を求
め、該方向に押しっけ力が発生するように、しかも検出
反力が設定値と等しくなるように力制御を行う。

又、本発明では、倣い動作時の各地点（サンプリング時
刻における位置）における移動方向ベクトル６が、オペ
レータが予め設定してある倣い軌道平面に平行となるよ
うに、しかも検出位置が目標位置と等しくなるように位
置制御を行う。

以上の力制御、位置制御に基づいて、検出反力を法線方
向の設定力と等しくなるように制御しながら、ハンドを
法線方向に直交する方向（接線方向）であって倣い軌道
平面上を移動させて対象物表面を倣わせる。

ハンドが移動して倣い軌道平面切換条件が成立すると倣
い軌道平面を、予め設定してある別の倣い軌道平面に切
り換え、以後、法線方向に直交する方向であって、切り
換えられた軌道平面上を移動するようにハンドを移動し
て倣いを行う。

以下、倣い座標系を切り換えながら対象物に対して倣い
動作を行う倣い制御方法と、倣い動作中に倣い軌道を切
り換えて再び倣いを再開する方法について説明する。

（Ｃ）飯ｆｆｌすＩど」雌 第４図は倣い座標系とロボットの軌道がどのように変化
するかを示す説明図であり、ＷＫは対象物、３１は最初
の倣い軌道平面、３２は軌道平面切換後の次の倣い軌道
平面、Ｐｂは倣い開始点（始点）、Ｐｉは倣い制御の中
間点、Ｐｉ’は倣い軌道切換点である。まず、第４図の
始点ｐｂでの倣い座標系の算出方法を説明する。

（１）　　　占での゛　　ベクトルの 始点ｐｂでの法線ベクトル盲すを次のようにして求める
。第５図（ａ）はハンドが対象物から受ける反力ＳＦと
、力覚センサ座標系０ｓ−ＸｓＹｓＺｓの関係を示した
図である。力覚センサでは、反力ＳＦのＸｓ、Ｙｓ、Ｚ
ｓ力方向それぞれの分力ｆｘ。

ｆｙ＋　ｆｚが検出される（力覚センサの検出するトル
ク成分についての説明は省略する）。

反力ＳＦをベクトル表示すると（Ｓは力覚センサ座標系
０ｓ−ＸｓＹｓＺｓで記述されていることを示す）、 ＳＦ：　（ｆｘ　　ｆｙ　ｆｚ）”　　　　（１）とな
る。力覚センサ座標系で表した法線ベクトル５ｒｆｂは
　ＳＦと逆向きのベクトルであり、成分表示すると。

Ｓ盲り＝−３Ｆ／ｌ　ＳＦ　＋ ＝−（ｆｘ　ｆｙ　ｆｚ）”／　　ｆｘ”＋ｆｙ２＋ｆ
ｚ”である。ただし、ｌＳＦ＋はベクトルの大きさであ
る。

次に、法線ベクトルＳＷｂをロボット基準座標系に変換
する。力覚センサ座標系の各座標軸Ｘｓ。

Ｙｓ、Ｚｓのロボット基準座標系に対する単位ベクトル
を　０ｉ″Ｉｓ、。′？５ｓ、’百Ｓとする（０はロボ
ット基準座標系で記述されていることを示す）。成分表
示すると、 ０盲ｓ＝　（ｎｓｘ　ｎｓｙ　ｎ５ｚ）　”’？５ｓ＝
　（ｏｓｘ　ｏｓｙ　ｏｓｚ）　Ｔ’Ｘｓ＝　（ａｓｘ
　ａｓｙ　ａｓｚ）　”　　　　　（３）である。この
とき、力覚センサ座標系から基準座標系への座標変換行
列’　Ａ　ｓは、 ’Ａｓ＝　（’？’ｌｓ　”？Ｊｓ　’Ｘｓ）　　　　
　　（４）で与えられる。従って、ロボット基準座標系
で表した法線ベクトル’ｒｉｂは、座標系変換行列″Ａ
ｓを用いて、 ’ｆｆ１ｂ＝’Ａｓ　Ｓ７”ｌｂ　　　　　（５）とな
る。法線ベクトル’ｒｉｂは、後述する法線ベクトル演
算部で算出される。又、始点以外の他の接触点での法線
ベクトルも同様に求めることができる。

（２）　　占での　　　向ベクトルの 次に、オペレータが予め設定した移動方向ベクトル？５
ｏｐｂ（倣い軌道平面３１上のベクトル）と。

求めた法線ベクトル’ｒ’＋ｂを用いて、始点ｐｂにお
ける実際の移動方向ベクトル？５ｂを算出する。但し、
ベクトル８　ｏｐｂとベクトル’ｒｒｂは。

５ｏｐｂ＝’ｎｂまたは１５ｏｐｂ＝−’ｒＴｂ　　　
（６）を満たさないものとする。実際の移動方向ベクト
ル６ｂは、法線ベクトル’ｒｒｂに直交し、移動方向ベ
クトル６　ｏｐｂと法線ベクトル’ｒｔｂのなす平面（
倣い軌道平面３１）上にあるベクトルである。このとき
、倣い座標系の座標軸を表す単位ベクトルの１つである
百すは、’ｒｔｂと５　ｏｐｂを用いて、’ｆｂ＝（’
ＷｂＸ６ｏｐｂ）／Ｉ’ｎｂｘ５ｏｐｂｌ　　　（７）
で表される。移動方向ベクトル’５ｂは、ベクトル６ｍ
ｂ、’ｌｂとの直交関係により。

’６ｂ＝’ｆｆ１ｂＸ’百ｂ（８） で求められる。

尚、上記の方法では、オペレータが倣い軌道平面３１上
の移動方向ベクトル５　ｏｐｂを設定した場合であるが
、倣い座標系の座標軸を表す単位ベクトルの１つである
百（倣い軌道平面に直交する平面上のベクトル）を設定
しても良い。設定するベクトルをＫ　ｏｐｂとすると、
移動方向ベクトル６ｂは。

’６ｂ＝（’ＭｏｐｂＸ’Ｗｂ）／ｌ’ｆｏｐｂＸ’？
’ｌｂｌ　　（９）のように表される。倣い座標系の座
標軸を表す単位ベクトルの１つであるＷｂは、ベクトル
’ｒｔｂ−’１５ｂとの直交関係により。

’ａｂ：’ｒ’ｌｂｘ’ｎｂ　　　　　（ｔｏ）で求め
られる。尚、この方法は倣い軌道平面を与えることと等
価である。移動方向ベクトル’Ｗｂは、後述する移動方
向ベクトル算出部で算出される。

次に、第４図の点ＰＬでの倣い座標系の算出方法につい
て説明する。

（３）占Ｐｉでの法線ベクトルの 点Ｐｉでの法線ベクトルｆｆ１ｉの算出方法は、始点Ｐ
ｓでの算出方法と同様であり、次式で表される。

但し、点Ｐｉでの反力ＳＦｉを、 ５Ｆｉ＝（ｆｘｉ　ｆｙｉ　ｆｚｉ）　”とする。

５ｒｆｉ＝−”Ｆｉ／　Ｉ　”Ｐｉ ＝−（ｆｘｉ　ｆｙｉ　ｆｚｉ）　”／　　ｆｘｉ２＋
ｆｙｉ”＋ｆｚｉ２’？’ｌｉ＝’Ａｓｉ　ＳＭｉ　　
　　　　（１２）”Ａｓｉは１点ＰＬの力覚センサ座標
系からロボット基準座標系への座標系変換行列である。

（４）占Ｐｉでの　　　向ベクトルの ロボットの先端位置は、ベクトル百方向にも制御される
ので、先端位置が常にベクトル晋、σがなす平面（倣い
軌道平面３１）上にあるように制御される。従って、対
象物の表面に描く軌跡は、始点ｐｂで与えられたベクト
ルＷｂ、５ｂのなす平面（ベクトル６　ｏｐｂを含む倣
い軌道平面３１）上の曲線となる。このことから、倣い
軌道を同一平面（倣い軌道平面）上に保つには、点Ｐｉ
での移動方向ベクトルｉ５ｉを、始点で求められたベク
トルｒｂに垂直に与えれば良いことが分かる。

移動方向ベクトル６１は、法線ベクトルＷｉにも直交す
るので、 ’５ｉ＝（’ｆｂＸ’バｉ）／　Ｉ’ｙｂｘ’バｉｔ　
　　（１３）のように表される。倣い座標系の座標軸を
表す単位ベクトルの１つであるＸｉは、ベクトルＯｒｌ
　１゜’ｉ５ｉとの直交関係により。

’ｆｉ＝＝”ＷｉＸ’？５ｉ　　　　　（１４）で求め
られる。

（５）占Ｐｉ’での　　　向ベクトルの移動方向を切り
換える場合の移動方向ベクトルの算出は、始点での移動
方向ベクトルの算出と同様の方法で行う、すなわち、オ
ペレータが与えた移動方向と、点Ｐｉ′の接点での法線
ベクトルから移動方向ベクトルを算出する。

オペレータが設定する点Ｐｉ′での倣い軌道平面３２上
の移動方向ベクトルをＢ　ｏｐｉとする。ただし、ベク
トル５　ｏｐｉとベクトル’Ｗｉ’　は、５ｏｐｉ＝’
Ｍｉ’または５ｏｐｉ＝−’ｆｆ１ｉ’　　　（１５）
を満たさないものとする。移動方向ベクトル６１は、法
線ベクトル’Ｗｉ’に直交し、移動方向ベクトル５　ｏ
ｐｉと法線ベクトルＪＴｉ’のなす平面（倣い軌道平面
３２）上にあるベクトルである。このとき、倣い座標系
の座標軸を表す単位ベクトルの１つであるＸｉは、’ｆ
ｆ１ｉと？５　ｏｐｉを用いて、０百ｉ’　＝（’ｒｌ
ｉ’　Ｘ５ｏｐｉ）／　ｌ’？Ｔｉ’　Ｘ５ｏｐｉｌテ
表される。移動方向ベクトル’６ｉ’は、ベクトル’ｆ
ｆ１ｉ、’百１′との直交関係により、’？５ｉ’＝’
百ｉ’　Ｘ’ｒ’ｆｉ’　　　　　（１７）で求められ
る。

尚、上記の方法では、オペレータが倣い軌道平面３２上
の移動方向ベクトル？５　ｏｐｉを設定した場合である
が、倣い座標系の座標軸を表す単位ベクトルの１つであ
る百（倣い軌道平面に直交する平面上のベクトル）を設
定しても良い。設定するベクトルをＭ　ｏｐｉとすると
、移動方向ベクトルｄ１は、 ”５ｉ’　　＝（’ｆｏｐｉＸ’？’ｌｉ’　　）／　
ｌ”１ｆｏｐｉＸ’ｆｆ１ｉ’のように表される。倣い
座標系の座標軸を表す単位ベクトルの１つであるＫｉは
、ベクトル’Ｗｉ’’６ｉ’　との直交関係により、 ｏｃｒｉ’　＝ｏｌ’Ｔｉ’　Ｘ’５ｉ’　　　　　（
１９）で求められる。この方法は倣い軌道平面を与える
ことと等価である。移動方向ベクトルは、後述する移動
方向ベクトル算出部で算出される。

（ｄ）　　い　　　の　　タイミング 倣い座標系の押しつけ方向を曲面の法線ベクトルに一致
させるための座標系切換のタイミングは、前述のように
、 ■）所定時間経過した時、 ２）対象物から受ける反力が設定力からある程度以上ず
れた時、 ３）ロボットが一定距離移動した時、 のいずれかがある。

又、倣い動作中の移動方向（倣い軌道平面）を変更する
ための座標系の切換タイミングは、１）設定した倣い動
作時間を経過した時、２）移動方向の反力がある値以上
になった時。

３）ロボットの先端が設定位置に到達した時、４）設定
した距離移動した時、 のいずれかである、すなわち、これらの条件のいずれか
を満たした時に、オペレータは予め登録しておいた移動
方向（倣い軌道平面）を用いて、前述の（ｃ）　（５）
の方法により、移動方向ベクトルを求め、倣い動作を再
開する。

（ｅ）負上りＥ生 本発明では、力制御に基づいて、検出反力「を法線方向
の目標力１”ｒと等しくなるように制御しながら、かつ
位置制御により法線方向に直交する方向であって、指定
された倣い軌道平面上を移動するようにハンドを移動さ
せ、対象物表面を倣わせる。そして、ハンドが移動し、
前記倣い軌道平面切換条件が成立すると倣い軌道平面を
、予め設定してある別の倣い軌道平面に切り換え、以後
。

法線方向に直交する方向であって、切り換えられた軌道
平面上を移動するようにハンドを移動して倣いを行う。

以下、力制御における目標力と位置制御における目標位
置の決定方法を説明する。

（１）目標力の発生 オペレータが設定した対象物に対する押しつけ力の大き
さをＦｒとすれば、始点ｐｂにおける目標力１ｌＦｒは
、大きさがＦｒで、方向・向きが法線方向ｒｔｂの力で
ある。従って、目標力ａ　Ｆ　ｒは、（２）式で求めた
法線ベクトル”ｒ’＋ｂを用いて次式％式％（２０） 尚、任意の点Ｐｉにおいても、誠意における法線方向０
阿ｊを求めれば（２０）式により目標力ａ　Ｆ　ｒを演
算できる。

（２）目標位置の発生 始点ｐｂでの移動方向ベクトル６ｂは（９）式により求
まる。従って、目標位置はこの移動方向ベクトル′？５
ｂで与えられる方向にあり、現在位置から目標位置まで
の相対位Ｗ（指令移動量）ｏＸｂは５ｂを用いて次式 ％式％（２１） により与えられる。αは倣い座標系切換条件により定ま
る適当な定数であり、予め設定される。

尚、任意の点Ｐｉにおいても、誠意における移動方向ベ
クトルｏ５１を求めれば（２１）式により目標位置０×
ｉを演算できる。

（３）倣い動作の終了 倣い制御動作は次の条件が満たされた時に終了する。す
なわち、 ｌ）オペレータから終了指示があった時、２）一定時間
経過した時、 ３）衝突した時、または対象物から離れた時、４）ロボ
ットの可動範囲を越えた時、 倣い動作を終了する。

（ｆ）藍 （７）　、　（１６）式は、’ｆｂ＝’ｎｉ　（’ｆ　
ｉ　＝’ｆｆ１ｉ）　＊たは０百ｂ＝−１ｌ盲ｉ（＠百
１＝−６百１）のときのは成立しない、この時、点Ｐｉ
の移動方向ベクトルにオペレータが与えた移動方向ベク
トル５ｏｐｂ（Ｗｏｐｉ）を用いると進行方向が維持で
きる。

（ｇ）生見豆東二哀１丘 全体の構成 第６図は本発明の一実施例構成図であり、第１図と同一
部分には同一符号を付している。

ｌは制御対象であるロボットのハンド、１′はハンドを
駆動するマニピュレータ、２はロボットのハンドに取付
けられ、該ハンドが対象物から受ける反力（接触力）を
検出する力検出部、３はハンドの位置を検出する位置検
出部、４は力検出部により検出された反力に基づいて対
象物の法線ベクトルを算出する法線ベクトル算出部、５
は法線ベクトルに直交する方向であって、予め特定され
ている倣い軌道平面上の移動方向ベクトルを算出する移
動方向ベクトル算出部、６は法線方向の目標力ＯＦ　ｒ
と移動方向の目標位置０×ｒ等を出力する制御指令生成
部、７は法線方向の目標力と検出反力「との差に基づい
て法線方向の速度指令Ｖｆを出力する力制御部、８は移
動方向の目標位！ｉ０ヌｒと検出位置Ｘとの差に基づい
て移動方向の速度指令Ｖｐを出力する位置制御部、９は
各速度指令を合成する合成部、１０は合成速度指令に・
基づいてハンドを移動させるハンド移動制御訊、１１は
軌道平面切換条件や切換後の軌道平面等の設定や操作を
行う操作部、１２は設定された各種データを記憶する設
定データ記憶部、１３はハンドが移動して、予め設定さ
れている倣い軌道平面切換条件が成立した時、倣い軌道
平面を設定されている別の軌道平面に切り換える軌道平
面切換部、１４は移動方向ベクトルを規定するための倣
い軌道平面（移動方向）を記憶する移動方向記憶部であ
る。

左支裏皿 力制御部２は、対象物に対する押し付けによりハンド１
が該対象物から受ける反力（接触力）の検出を行う力覚
センサ２ａと、力覚センサにより検出された反力１”ｈ
を変換マトリクスＴを用いてハンド座標系からロボット
基準座標系に変換する座標変換部２ｂを有している。

膨！喪番皿 位置検出部３は、マニピュレータ１′を駆動するモータ
の回転速度を検出するタコメータ３ａと、モータの所定
回転角度ごとにパルスを発生すると共に、該パルスを計
数して回転角度θＳを出力するエンコーダ＆カウンタ３
ｂと、マニピュレータ１′の回転角度（関節角度）θＳ
をロボット基準座標系での移動量Ｘに変換する変換部３
ｃを有している。

五龜べＬΣ玄算出部 法線ベクトル算出部４は、力検出部２により検出された
反力Ｆを用いて、倣い開始点では（２）〜（５）式によ
り、倣い動作の中間点では（１１）〜（１２）式を用い
てロボット基準座標系で表現した法線ベクトル０百を演
算して移動方向ベクトル算出部Ｓと制御指令生成部６に
出力する。

向ベクトル　出 移動方向ベクトル算出部５は、倣い開始点では法線ベク
トル１盲と倣い方向ベクトル６　ｏｐｂを用いて（７）
〜（８）式により、倣い動作の中間点では（１３）〜（
■４）式により、又倣い軌道平面切換時には（１６）〜
（１７）式により、ロボット座標系で表現した移動方向
ベクトルＯｒ５と８百を求めて制御指令生成部６に入力
する。

胤１１目しり良ｌ 制御指令生成部６は、法線方向の目標力１’　Ｆｒを（
２０）式により求めて力制御部７に入力すると共に。

力制御のパラメータとなる法線ベクトルｌＩｎとゲイン
Ｃｆを力制御部７に入力する。

又、制御指令生成部６は、（２１）式より移動方向ベク
トルが示す方向の目標位置（相対位置）”！ｒを演算し
て位置制御部８に入力すると共に、位置制御のパラメー
タとなる移動方向ベクトルｏ？５．ベクトル０百、ゲイ
ンＣｐを位置制御部８に入力する。

尚、制御指令生成部６は予め設定されている倣い速度■
。も出力する。

裏莢豊皿 力制御部７は、法線方向の目標力ＯＦ　ｒと検出反力Ｆ
との差が零となるように、該差に基づいて法線方向の第
１の速度指令Ｖｆを出力するものであり、目標力ＯＦｒ
と検出反力Ｆとの差０ΔＦを演算する算出部７ａと、前
記速度指令Ｖｆを発生する第１速度指令発生部７ｂを有
している。第１速度指令発生部７ｂは、第７図に示すよ
うに、前記力差０ΔＦをロボット基準座標系から倣い座
標系に変換する座標変換部７ｂ−１と、倣い座標系に変
換された力差ＳΔＦより法線方向成分を選択する選択部
７ｂ−２と、該法線方向成分をロボット基準座標系に変
換する座標変換部７ｂ−３と、法線方向の力差成分に所
定のゲインＣｆを乗算して第１速度指令Ｖｆを出力する
乗算部７ｂ−４で構成されている。尚、倣い座標系（Ｘ
ｗＹｗＺｗ）からロボット基準座標系（ＸｏＹｏＺｏ）
へ変換する直交座標変換行列Ｒは、ベクトル蒼、σ、百
を用いて次式 のように表される。従って、座標変換部７ｂ−１では変
換行列ＲＴを用いて力差０ΔＦを倣い座標系に変換でき
（’Ｂ＝−ＲＴ’ΔＦ）、　座１１変換部７ｂ−３は変
換行列Ｒを用いて法線方向成分をロボット基準座標系に
変換できる。

又、法線方向の力差成分を選択する選択行列Ｓｆは、倣
い座標系（ＸｗＹｗＺｖ）のＸｗ（法線方向）を力制御
方向、Ｙｗ、Ｚｗ力方向位置制御方向とすると（第２図
参照）、 で与えられる。従って、座標変換部７ｂ−３は上記選択
行列Ｓｆを用いて力差の法線方向成分を選択できる。

更に、ゲインＣｆは一定値であり１次式により表現され
る。

闘１級鼠１ 位置制御部８は、移動方向ベクトル６が示す方向の目標
位置１ＩＸｒと検出位置Ｘの差が零となるように、該差
に基づいて位置制御方向の第２の速度指令Ｖｐを出力す
るものであり、目標位置ａ×ｒと検出位置Ｘの差ΔＸを
演算する算出部８ａと、前記速度指令Ｖｐを発生する第
２速度指令発生部８ｂを有している。第２速度指令発生
部８ｂは第７図に示すように、前記位置偏差ΔＸをロボ
ット基準座標系から倣い座標系に変換する座標変換部８
ｂ−１と、倣い座標系に変換された位置偏差より位置制
御方向成分（ベクトルで、百の方向成分）を選択する選
択部８ｂ−２と１位置制御方向成分をロボット基準座標
系に変換する座標変換部８ｂ−３と１位置制御方向の位
置偏差成分に所定のゲインＣｐを乗算して第２速度指令
Ｖｐを出力する乗算部８ｂ−４で構成されている。尚、
位置制御方向成分を選択する選択行列Ｓｆ’は Ｓｆ’　＝Ｉ−Ｓｆ　　（但し、■は単位行列）で表さ
れ、 で与えられる。

又、ゲインＣｐは一定値であり、次式 により表現される。

すλ」１１１札婁皇− ハンド移動制御部１０は、合成部９により合成された合
成速度指令をマニピュレータ１′の各関節の速度δに変
換する逆ヤコビ変換部１０ａと、位置及び速度をフィー
ドバックされてモータを指令速度で回転させる速度制御
回路１０ｂと、実速度と指令速度との差に応じた速度偏
差をアナログに変換するＡＤ変換器１０ｃと、パワーア
ンプ１０ｄと、サーボモータ１０ｅを備えている。

１土鼠 操作部１１は、ロボットならい制御に先立って種々のパ
ラメータを入力したり、倣いの開始、停止等のオペレー
ションを実行するものである。

設定すべきパラメータとしては、倣い開始点における倣
い軌道平面を特定する移動方向ベクトル５０ρｂ、倣い
座標系切換時間（サンプリング時間）τ、倣い軌道平面
の切換条件Ｆｊ、ｔｊ、Ｐｊと切換後の倣い軌道平面？
５　ｏｐｊ、相対位置指令係数α。

目標設定力Ｆｒ、倣い指令速度Ｖｏ等があり、これらは
設定データ記憶部１２に記憶される。

１１王１隻１皿 軌道平面切換部１３は、設定された倣い軌道平面切換の
条件が成立したか監視し、例えば設定されている倣い軌
道平面切換位置に到達したか、あるいは検出反力が設定
値を越えたか監視しており、切換条件が成立すると、新
たな倣い軌道平面を特定するデータを制御指令生成部６
に入力すると共に、該倣い軌道平面を特定する移動方向
ベクトル６０ρｊを移動方向記憶部１４に記憶する。

金生血監生 以下、本発明の全体の倣い動作を第８図の流れ図に従っ
て説明する。尚、以下において、添字Ｂは倣い開始点を
、添字ｉは第１番目でのロボットと対象物との接触点を
、添字ｊは倣い軌道平面の切換・設定地点を表すものと
する。

まず、オペレータは操作部１１から、以下に示す（ｉ）
〜（ｖｉ）のデータを入力する、すなわち、（ｉ）倣い
軌道平面切換地点（始点を含む）における移動方向ベク
トル’５ｏｐｊ（あるいは軌道平面と直交するベクトル
０百ｏｐｊ）、 （ｎ）設定力Ｆｒ、 （ｉｉｉ）倣い座標系の切換時間τ、 （泣）倣い軌道平面切換条件である切換位置Ｐｊ（また
は力Ｆｊまたは時間ｔｊ）、 （Ｖ）位置制御における指令位置を特定するための係数
α、 （ｖｉ）指令倣い速度Ｖ。

を設定し、設定データ記憶部１２に記憶する（ステップ
１ｏ１）。

これにより、軌道平面切換部１３は、倣い開始点ｐｂに
おける移動方向ベクトル’　７５　ｏｐｂを移動方向記
憶部１４に記憶する（ステップ１０２）。

ついで、オペレータはハンドを移動して対象物に押しつ
けるために始点力ベクトルＦ？ｂを設定する（ステップ
１０３）。

始点力ベクトルが設定されると、制御指令生成部６は、
倣い開始点で力Ｆｂを発生するために力指令Ｆｂ、ゲイ
ンＣｆ等の力制御パラメータを発生して力制御部７に入
力する（ステップ１０４）。

力制御部７は入力された力指令等に基づいて対象物方向
にハンドを移動させるための速度指令Ｖｆを発生し、こ
れによりハンドは対象物方向に接近する（ステップ１０
５）。

力検出部２はハンドと対象物の接触により生じる反力戸
を検出する（ステップ１０６）。尚１反力が発生すると
法線ベクトル算出部４は法線ベクトル’ｒ’＋ｂの算出
が可能となる。

ハンドが対象物と接触せず反力「が生じなければ、制御
指令生成部６はハンドが対象物と接触して反力戸が発生
するまで力指令Ｆｂを出力して上記処理を繰り返す（ス
テップ１０７）。

ハンドが移動して対象物と接触し、該接触により反力Ｆ
が発生すると、法線ベクトル算出部４は該反力を用いて
倣い開始点における法線ベクトルＳｒ＋ｂ　（倣い座標
系）を（２）式に従って算出する（ステップ１０８）。

ついで、法線ベクトル算出部４は（４）式により倣い座
標系からロボット基準座標系に変換するための座標変換
行列’　Ａ　ｓを算出し、（５）式によりロボット座標
系における法線ベクトル’ｒｉｂを計算し、該法線ベク
トルを移動方向ベクトル算出部５と制御指令生成部６に
入力する（ステップ１０９，１１０）。

制御指令生成部６は軌道平面切換部１３から入力されて
いる倣い開始点ｐｂにおける移動方向ベクトル±’　５
　ｏｐｂと法線ベクトルａ百すが一致するかチエツクし
くステップ１１１．１１２）、一致していればその旨を
操作部１１の図示しない表示部に表示し、オペレータに
パラメータの設定しなおしを通知し、ステップ１０１以
降の処理を繰り返させる。

しかし、±’５ｏｐｂ＃’？Ｔｂであれば、移動方向ベ
クトル算出部５に移動方向ベクトルの演算を指示し、こ
れにより移動方向ベクトル算出部５は（７）式にしたが
ってベクトル’Ｗｂを演算し、ついで（８）式により移
動方向ベクトル０？５ｂを演算し、演算結果を制御指令
生成部６に入力する（ステップ１１３．１１４）。

制御指令生成部６は、法線ベクトル０盲すと設定力Ｆｒ
を用いて、（２０）式から目標力ベクトルＯＦ　ｒを生
成して力制御部７に入力すると共に、係数αと移動方向
ベクトル０？５ｂを用いて（２１）式から目標位置（目
標相対位置）０×ｒを生成して位置制御部８に入力する
（ステップ１１５．１１６）。

又、制御指令生成部６は倣い速度指令ｖＯを出力する（
ステップ１１７）。

力制御部７は目標力ＯＦｒと検出反力「の差に基づいて
法線方向の速度指令Ｖｆを出力し、位置制御部８は目標
位！！０ヌｒと検出位置Ｘの差に基づいて位置制御方向
の速度指令Ｖｐを出力し、合成部９はＶｏ、Ｖｆ、Ｖｐ
を合成してハンド移動制御部１０に入力してハンドを対
象物の表面に沿って移動させる（倣い動作）、・・・ス
テップ１１８倣い動作と並行して、一定時間（サンプリ
ング時間）で経過したか監視しており（ステップ１１９
）、経過しなければ、法線ベクトル及び移動方向ベクト
ルをステップ１１０，１１４で求めた値としく更新せず
）、倣い動作を継続する。

一定時間が経過すれば、制御指令生成部６は、ロボット
先端位Ｗ（ハンド位置）を取り込み、該位置がロボット
可動範囲内にあるかチエツクする（ステップ１２０，１
２１）。

ロボット可動範囲外であれば、次の動作へ移行し、可動
範囲内であれば、反力−を検出し、該反力が零か（対象
物から離れたか）、あるいは１より相当大きいか（壁と
衝突したか）判断し、「＝０または「）１であれば動作
を直ちに停止する（ステップ１２２〜１２４）。

「＝０でなく、戸）１でなければ、検出位置を第１番目
の位置として、以後開始点におけると同様の処理を行っ
てハンドを対象物に沿って移動させる。すなわち、 法線ベクトル算出部４は検出反力戸を用いて第１位ＩＥ
　Ｐ　ｉにおける法線ベクトルＳＷｉ　（倣い座標系）
を（１１）式により算出する（ステップ１２５）。

ついで、法線ベクトル算出部４は（４）式に従って倣い
座標系からロボット基準座標系に変換するための座標変
換行列’　Ａ　ｓを算出し、（１２）式によりロボット
座標系における法線ベクトル’？’ｉｉを計算し、該法
線ベクトルを移動方向ベクトル算出部５と制御指令生成
部６に入力する（ステップ１２６゜１２７）。

移動方向ベクトル算出部５は（１３）式にしたがって移
動方向ベクトル’５ｉを演算し、演算結果を制御指令生
成部６に入力する（ステップ１２８）・制御指令生成部
６は、法線ベクトルＪＴｉと設定力Ｆｒを用いて、（２
０）式から目標力ベクトルＯＦｒを生成して力制御部７
に入力すると共に、係数αと移動方向ベクトル’６ｉを
用いて（２１）式から目標位Ｗ（目標相対位置）０又ｒ
を生成して位！！制御部８に入力する（ステップ１２９
，１３０）。

又、制御指令生成部６は倣い速度指令ｖＯを出力する（
ステップ１３１）。

力制御部７は目標力と検出反力の差に基づいて法線方向
の速度指令Ｖｆを出力し、位置制御部８は目標位置と検
出位置の差に基づいて位置制御方向の速度指令Ｖｐを出
力し、合成部９はＶｏ、Ｖｆ。

ＶＰを合成してハンド移動制御部１０に入力してハンド
を対象物の表面に沿って移動させる（倣い動作）。・・
・ステップ１３２ 倣い動作と並行して、一定時間（サンプリング時間）τ
経過したか監視しており（ステップ１３３）、経過しな
ければ、法線ベクトル及び移動方向ベクトルをステップ
１２７，１２８で求めた値としく更新せず）、倣い動作
を継続する。

一定時間が経過すれば、制御指令生成部６は、ロボット
先端位置（ハンド位置）を取り込み、該位置がロボット
可動範囲内にあるかチエツクする（ステップ１３４，１
３５）。

ロボット可動範囲外であれば、次の動作に移行し、可動
範囲内であれば、反力「を検出し、該反力が零か（対象
物から離れたか）、あるいは１より相当大きいか（壁と
衝突したか）判断し、「＝０または戸）１であれば動作
を直ちに停止する（ステップ１３６〜１３８）。

Ｆ’＝Ｏでなく、Ｆ＞１でなければ、軌道平面切換部１
３に倣い軌道平面切り換え条件が成立しているかの判断
処理の実行を指示する。これにより。

軌道平面切換部１３は、倣い開始からの経過時間ｔ、あ
るいは倣い軌道平面を切り換えてからの経過時ｆａｔを
検出しくステップ１３９）、ついで倣い軌道平面切換条
件が成立したかチエツクする（ステップ１４０）。すな
わち、軌道平面切換部１３は、現在位置Ｐが倣い軌道平
面切換地点Ｐｊに一致するか、あるいは検出反力Ｆが設
°定値Ｆｊに一致するか、又は経過時間ｔが設定時間ｔ
ｊに一致するかチエツクする。

倣い条件が成立しなければ、オペレータによる倣い制御
終了の指示があるかチエツクしくステップ１４１）、あ
れば次の動作へ、指示がなければ現在位置をＰｉとして
ステップ１２５に飛び、以降の処理を繰り返す。

ステップ１４０において、倣い軌道平面の切換条件が成
立すれば、軌道平面切換部１３は切換地点における倣い
軌道平面を特定する移動方向ベクトル０σ０ρｊを移動
方向記憶部１４にセットしくステップ１４２）、法線ベ
クトル算出部４は検出反力「を用いて第ｉ位置Ｐｉにお
ける法線ベクトルＳｒ’ｒｉ　（倣い座標系）を（１１
）式に従って算出する（ステップ１４３）。

ついで、法線ベクトル算出部４は（４）式に従って倣い
座標系からロボット基準座標系に変換するための座標変
換行列’　Ａ　ｓを算出し、（１２）式によりロボット
座標系における法線ベクトル０百ｉを計算し、該法線ベ
クトルを移動方向ベクトル算出部５と制御指令生成部６
に入力する（ステップ１４４゜１４５）。

移動方向ベクトル算出部５は、入力された法線ベクトル
’ｒｒｉと移動方向記憶部１４から与えられる移動方向
ベクトル’　？５　ｏｐｊを用いて、（１６）　、　（
１７）式にしたがって移動方向ベクトル０？５１を演算
し、演算結果を制御指令生成部６に入力しくステップ１
４６．１４７）、ステップ１２９に飛び以降の処理を繰
り返す。

（ｉ）本発明の他の実施例 以上では、ロボット倣い制御装置を機能毎にブロック化
して説明したが、ホストコンピュータと、ロボットコン
トローラとで構成するようにもできる。例えば、法線ベ
クトル演算、移動方向ベクトル演算、制御指令生成、設
定データ記憶、軌道平面切換、移動方向記憶制御等をホ
スト側で行い、力制御部、位置制御部、ハンド移動制御
をロボットコントローラ側で行うようにする。

以上本発明を実施例により説明したが１本発明は請求の
範囲に記載した本発明の主旨に従い種々の変形が可能で
あり、本発明はこれらを排除するものではない。

［発明の効果コ 以上本発明によれば、力検出部と位置検出部を設け、検
出値に基づいて力制御１位置制御を行って、検出反力を
設定値に維持しながら、ハンドを法線方向に直交する方
向（接線方向）に移動させて対象物表面を倣わせるよう
に構成したから、教示作業や曲面の定義が不要であり、
しかも作業条件を設定するだけでロボットハンドをして
対象物の表面を倣わせることができる。

又、本発明によれば、所定の倣い軌道平面切換条件が満
たされた時、ロボットの倣い軌道平面を自動的に切り換
えるようにして倣うように構成したから、所望の範囲全
体を自動的に倣わすことができる。

更に、倣い軌道平面を、該平面上の１つのベクトルで、
あるいは倣い軌道平面と直交する平面上の１つのベクト
ルで特定するようにした力゛ら、軌道平面の設定を容易
に行うことができる。

又、倣い軌道平面の切換を、所定位置に到達した時、検
出反力が設定値を越えた時、移動時間または移動距離が
設定以上になった時に行うようにしたから、これら切換
条件と切換後の倣い軌道平面を設定しておくだけで自動
的に倣い軌道平面を切り換えて対象物の表面を倣うこと
ができる。

【図面の簡単な説明】

第１図は本発明の原理図、 第２図はロボットと対象物の関係図、 第３図は倣い制御の概略説明図、 第４図は倣い座標系とロボットの軌道関係図、第５図は
設定力の算出方向の説明図、 第６図は本発明の一実施例構成図、 第７図は位置制御部と力制御部の構成図、第８図は本発
明のロボットならい制御の流れ図である。 １・・制御対象（ロボット） ２・・力検出部 ３・・位置検出部 ４・・法線ベクトル算出部 ５・・移動方向ベクトル算出部 ６・・制御指令生成部 ７・・力制御部 ８・・位置制御部 １ｏ・・ハンド移動制御部 １３・・軌道平面切換部 １４　・ ・移動方向記憶部 外２各 Ｋ ×０ Ｒ８Ｔ−−、ロホヘント νｉ／Ｋ　　−Ｊ’ｊ袂孕敏 εＩＳ２ 図 ×０ ＰＩ、Ｐ２ ｍ−０４マνト６すｍｍ 々交し：飲Ｊ’ｄＨＪの４柏４ｆｉもα引Ｅ亭３ 図 Ｘ （ａ） （ｂ） 蔚１！θの算出す句のｗＡの 第５図 第７図

Claims (6)

【特許請求の範囲】
1. （１）ロボットのハンドに取付けられ、該ハンドが対象
   物から受ける反力を検出する力検出部と、ハンドの位置
   あるいは移動量を検出する検出部と、 力検出部により検出された反力に基づいて対象物の法線
   ベクトルを算出する法線ベクトル算出部と、 法線ベクトルに直交する方向であって、予め特定されて
   いる倣い軌道平面上の移動方向ベクトルを算出する移動
   方向ベクトル算出部と、 法線方向の目標力と検出反力との差に基づいて法線方向
   の速度指令を出力する第１の速度指令出力部と、 移動方向の目標位置と検出位置との差、あるいは目標移
   動量と速度移動量との差に基づいて移動方向の速度指令
   を出力する第２の速度指令出力部と、 第１、第２速度指令出力部から出力される速度指令に基
   づいて前記各差が零となるようにハンドを移動させるハ
   ンド移動制御部と、 ハンドが移動して、予め設定されている倣い軌道平面切
   り換え条件が成立した時、倣い軌道平面を設定されてい
   る別の軌道平面に切り換える軌道平面切換部を有するこ
   とを特徴とするロボット倣い制御装置。
2. （２）前記倣い軌道平面を、該平面上の１つのベクトル
   で特定することを特徴とする請求項１記載のロボット倣
   い制御装置。
3. （３）前記倣い軌道平面を、該平面と直交する平面上の
   １つのベクトルで特定することを特徴とする請求項１記
   載のロボット倣い制御装置。
4. （４）前記軌道平面切換部は、倣い軌道平面切換位置と
   切換後の倣い軌道平面を複数登録しておき、ハンドが登
   録してある倣い平面切換位置に到来したか監視し、到来
   した時、倣い軌道平面を登録してある軌道平面に切り換
   えることを特徴とする請求項１記載のロボット倣い制御
   装置。
5. （５）前記軌道平面切換部は、倣い軌道平面を切り換え
   るための反力と切換後の倣い軌道平面を特定するデータ
   を登録しておき、力検出部により検出された反力が設定
   値になった時、倣い軌道平面を登録してある軌道平面に
   切り換えることを特徴とする請求項１記載のロボット倣
   い制御装置。
6. （６）前記軌道平面切換部は、倣い軌道平面を切り換え
   るための移動時間または移動距離と切換後の倣い軌道平
   面を特定するデータを登録しておき、前記時間移動した
   時、または前記距離移動した時、倣い軌道平面を登録し
   てある軌道平面に切り換えることを特徴とする請求項１
   記載のロボット倣い制御装置。