JP3089027B2 - ロボット倣い制御装置 - Google Patents
ロボット倣い制御装置Info
- Publication number
- JP3089027B2 JP3089027B2 JP02273904A JP27390490A JP3089027B2 JP 3089027 B2 JP3089027 B2 JP 3089027B2 JP 02273904 A JP02273904 A JP 02273904A JP 27390490 A JP27390490 A JP 27390490A JP 3089027 B2 JP3089027 B2 JP 3089027B2
- Authority
- JP
- Japan
- Prior art keywords
- plane
- vector
- force
- robot
- unit
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Manipulator (AREA)
Description
【発明の詳細な説明】 ［目次］ 概要 産業上の利用分野 従来の技術 発明が解決しようとする課題 課題を解決するための手段（第１図） 作用（第１図） 実施例 （ａ）ロボットと対象物の関係（第２図） （ｂ）ロボット倣い制御の概略（第３図） （ｃ）倣い座標系の算出（第４図、第５図） （１）始点での法線ベクトルの算出 （２）始点での移動方向ベクトルの算出 （３）点Piでの法線ベクトルの算出 （４）点Piでの移動方向ベクトルの算出 （５）点Pi′での移動方向ベクトルの算出（移動方
向を切り換える場合） （ｄ）倣い座標系切換タイミング （ｅ）倣い動作 （１）目標力の発生 （２）目標位置の発生 （３）倣い動作の終了 （ｆ）補足 （ｇ）本発明の一実施例（第６図） ・全体の構成 ・力検出部 ・位置制御部 ・法線ベクトル算出部 ・移動方向ベクトル算出部 ・制御指令生成部 ・力制御部（第７図） ・位置制御部（第７図） ・ハンド移動制御部 ・操作部 ・軌道平面切換部 （ｈ）全体の動作（第８図） （ｉ）本発明の他の実施例 発明の効果 ［概要］ 力覚センサをロボットハンドに取付け、力覚センサで
検出した対象物からの反力に基づいて対象物の法線方向
を求め、検出反力を設定値に維持しながら、ハンドを法
線方向に直交する方向（接線方向）に移動させて対象物
表面を倣わせるロボット倣い制御装置に関し、 教示作業や曲面の定義が不要であり、しかも作業条件
を設定するだけでロボットハンドをして対象物表面倣わ
せることができるロボット倣い制御装置を提供するこ
と、及び所定の条件が満たされた時、ロボットの倣い軌
道平面を切り換えることができるロボット倣い制御装置
を提供するを目的とし、 ロボットのハンドに取付けられ、該ハンドが対象物か
ら受ける反力を検出する力検出部と、ハンドの位置ある
いは移動量を検出する検出部と、力検出部により検出さ
れた反力に基づいて対象物の法線ベクトルを算出する法
線ベクトル算出部と、法線ベクトルに直交する方向であ
って、予め特定されている倣い軌道平面上の移動方向ベ
クトルを算出する移動方向ベクトル算出部と、法線方向
の目標力と検出反力との差に基づいて法線方向の速度指
令を出力する第１の速度指令出力部と、移動方向の目標
位置と検出位置との差、あるいは目標移動量と速度移動
量との差に基づいて移動方向の速度指令を出力する第２
の速度指令出力部と、第１、第２速度指令出力部から出
力される速度指令に基づいて前記各差が零となるように
ハンドを移動させるハンド移動制御部と、ハンドが移動
して、予め設定されている倣い軌道平面切り換え条件が
成立した時、倣い軌道平面を設定されている別の軌道平
面に切り換える軌道平面切換部を有するように構成す
る。
向を切り換える場合） （ｄ）倣い座標系切換タイミング （ｅ）倣い動作 （１）目標力の発生 （２）目標位置の発生 （３）倣い動作の終了 （ｆ）補足 （ｇ）本発明の一実施例（第６図） ・全体の構成 ・力検出部 ・位置制御部 ・法線ベクトル算出部 ・移動方向ベクトル算出部 ・制御指令生成部 ・力制御部（第７図） ・位置制御部（第７図） ・ハンド移動制御部 ・操作部 ・軌道平面切換部 （ｈ）全体の動作（第８図） （ｉ）本発明の他の実施例 発明の効果 ［概要］ 力覚センサをロボットハンドに取付け、力覚センサで
検出した対象物からの反力に基づいて対象物の法線方向
を求め、検出反力を設定値に維持しながら、ハンドを法
線方向に直交する方向（接線方向）に移動させて対象物
表面を倣わせるロボット倣い制御装置に関し、 教示作業や曲面の定義が不要であり、しかも作業条件
を設定するだけでロボットハンドをして対象物表面倣わ
せることができるロボット倣い制御装置を提供するこ
と、及び所定の条件が満たされた時、ロボットの倣い軌
道平面を切り換えることができるロボット倣い制御装置
を提供するを目的とし、 ロボットのハンドに取付けられ、該ハンドが対象物か
ら受ける反力を検出する力検出部と、ハンドの位置ある
いは移動量を検出する検出部と、力検出部により検出さ
れた反力に基づいて対象物の法線ベクトルを算出する法
線ベクトル算出部と、法線ベクトルに直交する方向であ
って、予め特定されている倣い軌道平面上の移動方向ベ
クトルを算出する移動方向ベクトル算出部と、法線方向
の目標力と検出反力との差に基づいて法線方向の速度指
令を出力する第１の速度指令出力部と、移動方向の目標
位置と検出位置との差、あるいは目標移動量と速度移動
量との差に基づいて移動方向の速度指令を出力する第２
の速度指令出力部と、第１、第２速度指令出力部から出
力される速度指令に基づいて前記各差が零となるように
ハンドを移動させるハンド移動制御部と、ハンドが移動
して、予め設定されている倣い軌道平面切り換え条件が
成立した時、倣い軌道平面を設定されている別の軌道平
面に切り換える軌道平面切換部を有するように構成す
る。
［産業上の利用分野］ 本発明はロボット倣い制御装置に係わり、特に力覚セ
ンサをロボットハンドに取付け、力覚センサで検出した
対象物からの反力に基づいて対象物の法線方向を求め、
検出反力を設定値に維持しながら、ハンドを法線方向に
直交する方向（接線方向）に移動させて対象物表面を倣
わせるロボット倣い制御装置に関する。
ンサをロボットハンドに取付け、力覚センサで検出した
対象物からの反力に基づいて対象物の法線方向を求め、
検出反力を設定値に維持しながら、ハンドを法線方向に
直交する方向（接線方向）に移動させて対象物表面を倣
わせるロボット倣い制御装置に関する。
自由曲面を持った対象物の表面に沿ってツールを移動
させて該対象物に所定の作業、例えば塗装作業等を施し
たい場合があり、最近、かかる作業をロボットに行わせ
るようになってきている。
させて該対象物に所定の作業、例えば塗装作業等を施し
たい場合があり、最近、かかる作業をロボットに行わせ
るようになってきている。
［従来の技術］ 従来、ロボットにより自由曲面を持った対象物に対し
て塗装等の作業を行わせるには、予め手動により対象物
の表面に沿ってロボットのハンドを移動させると共に適
所にロボット動作を指定し、これによりロボット制御用
のプログラムを作成し（教示）、実際の作業に当たって
は教示により作成したロボット制御用プログラムを用い
てハンドを移動させて対象物に対して所定の作業を行わ
せる。
て塗装等の作業を行わせるには、予め手動により対象物
の表面に沿ってロボットのハンドを移動させると共に適
所にロボット動作を指定し、これによりロボット制御用
のプログラムを作成し（教示）、実際の作業に当たって
は教示により作成したロボット制御用プログラムを用い
てハンドを移動させて対象物に対して所定の作業を行わ
せる。
また、教示によらず、オフラインでロボット制御用の
プログラムを作成する方法もある。この方法は予め対象
物の曲面を点列で定義し、該曲面上の点列をたどってハ
ンドを移動させる移動通路を作成し、適所にロボット動
作指令を挿入してロボット制御用プログラムを作成す
る。そして、実際の作業に当たっては作成したロボット
制御用プログラムを用いてハンドを移動させて対象物に
対して所定の作業を行わせる。
プログラムを作成する方法もある。この方法は予め対象
物の曲面を点列で定義し、該曲面上の点列をたどってハ
ンドを移動させる移動通路を作成し、適所にロボット動
作指令を挿入してロボット制御用プログラムを作成す
る。そして、実際の作業に当たっては作成したロボット
制御用プログラムを用いてハンドを移動させて対象物に
対して所定の作業を行わせる。
［発明が解決しようとする課題］ しかし、教示による方法では、オペレータが実際の作
業に即して手動によりロボットハンドを移動させなくて
はならず、操作が面倒となる問題がある。
業に即して手動によりロボットハンドを移動させなくて
はならず、操作が面倒となる問題がある。
また、オフラインによる方法では、予め対象物の自由
曲面を定義しなくてはならないという問題がある。
曲面を定義しなくてはならないという問題がある。
以上から、本発明の目的は教示作業や曲面の定義が不
要であり、しかも作業条件を設定するだけでロボットハ
ンドをして対象物表面倣わせることができるロボット倣
い制御装置を提供することである。
要であり、しかも作業条件を設定するだけでロボットハ
ンドをして対象物表面倣わせることができるロボット倣
い制御装置を提供することである。
本発明の別の目的は、所定の条件が満たされた時、ロ
ボットの倣い軌道平面を切り換えることができるロボッ
ト倣い制御装置を提供することである。
ボットの倣い軌道平面を切り換えることができるロボッ
ト倣い制御装置を提供することである。
［課題を解決するための手段］ 第１図は本発明の原理説明図である。
１は制御対象であるロボットのハンド、１′はマニピ
ュレータ、２はロボットのハンドに取付けられ、該ハン
ドが対象物から受ける反力（接触力）を検出する力検出
部、３はハンドの位置を検出する位置検出部、４は力検
出部により検出された反力に基づいて対象物の法線ベク
トルを算出する法線ベクトル算出部、５は法線ベクトル
に直交する方向であって、予め特定されている倣い軌道
平面上の移動方向ベクトルを算出する移動方向ベクトル
算出部、６は法線方向の目標力０ｒと移動方向の目標
位置または目標移動量０ｒを出力する制御指令生成
部、７は法線方向の目標力と検出反力との差に基づいて
法線方向の速度指令を出力する力制御部、８は移動方向
の目標位置と検出位置との差に基づいて移動方向の速度
指令を出力する位置制御部、９は各速度指令を合成する
合成部、10は合成速度指令に基づいてハンドを移動させ
るハンド移動制御部、11は軌道平面切換条件や切換後の
軌道平面等の設定や操作を行う操作部、12は設定された
各種データを記憶する設定データ記憶部、13はハンドが
移動して、予め設定されている倣い軌道平面切り換え条
件が成立した時、倣い軌道平面を設定されている別の軌
道平面に切り換える軌道平面切換部、14は移動方向ベク
トルを規定するための倣い軌道平面（移動方向）を記憶
する移動方向記憶部である。
ュレータ、２はロボットのハンドに取付けられ、該ハン
ドが対象物から受ける反力（接触力）を検出する力検出
部、３はハンドの位置を検出する位置検出部、４は力検
出部により検出された反力に基づいて対象物の法線ベク
トルを算出する法線ベクトル算出部、５は法線ベクトル
に直交する方向であって、予め特定されている倣い軌道
平面上の移動方向ベクトルを算出する移動方向ベクトル
算出部、６は法線方向の目標力０ｒと移動方向の目標
位置または目標移動量０ｒを出力する制御指令生成
部、７は法線方向の目標力と検出反力との差に基づいて
法線方向の速度指令を出力する力制御部、８は移動方向
の目標位置と検出位置との差に基づいて移動方向の速度
指令を出力する位置制御部、９は各速度指令を合成する
合成部、10は合成速度指令に基づいてハンドを移動させ
るハンド移動制御部、11は軌道平面切換条件や切換後の
軌道平面等の設定や操作を行う操作部、12は設定された
各種データを記憶する設定データ記憶部、13はハンドが
移動して、予め設定されている倣い軌道平面切り換え条
件が成立した時、倣い軌道平面を設定されている別の軌
道平面に切り換える軌道平面切換部、14は移動方向ベク
トルを規定するための倣い軌道平面（移動方向）を記憶
する移動方向記憶部である。
［作用］ 予め、操作部11から倣い軌道平面切換条件や切換後の
軌道平面データを設定して設定データ記憶部12に記憶し
ておく。
軌道平面データを設定して設定データ記憶部12に記憶し
ておく。
かかる状態において、ロボットのハンド１に取付けら
れた力検出部２を用いて、対象物からの接触力（反力）
を検出し、該反力に基づいて、法線ベクトル算出部４と
移動方向ベクトル算出部５において対象物の法線ベクト
ルと、該法線ベクトルに直交する方向であって予め特定
されている倣い軌道平面上に存在する移動方向ベクトル
を算出する。制御指令生成部６は、法線ベクトルと移動
方向ベクトルを用いて法線方向における力指令０ｒと
移動方向における位置指令０ｒを生成し、力制御部７
で力指令と検出反力との差に基づいて法線方向の速度指
令を出力し、位置制御部８で指令位置と検出位置との差
に基づいて移動方向の速度指令を出力する。ハンド移動
制御部10は両速度指令を合成した合成速度指令に基づい
て、前記各差が零となるようにハンドを移動させる。
れた力検出部２を用いて、対象物からの接触力（反力）
を検出し、該反力に基づいて、法線ベクトル算出部４と
移動方向ベクトル算出部５において対象物の法線ベクト
ルと、該法線ベクトルに直交する方向であって予め特定
されている倣い軌道平面上に存在する移動方向ベクトル
を算出する。制御指令生成部６は、法線ベクトルと移動
方向ベクトルを用いて法線方向における力指令０ｒと
移動方向における位置指令０ｒを生成し、力制御部７
で力指令と検出反力との差に基づいて法線方向の速度指
令を出力し、位置制御部８で指令位置と検出位置との差
に基づいて移動方向の速度指令を出力する。ハンド移動
制御部10は両速度指令を合成した合成速度指令に基づい
て、前記各差が零となるようにハンドを移動させる。
以後、上記力制御、位置制御に基づいて、検出反力を
設定値に維持しながら、ハンドを法線方向に直交する方
向（接線方向）に移動させて対象物表面を倣わせる。
設定値に維持しながら、ハンドを法線方向に直交する方
向（接線方向）に移動させて対象物表面を倣わせる。
そして、ハンドが移動して倣い軌道平面切換条件が成
立すると軌道平面切換部13は倣い軌道平面を、予め設定
してある別の軌道平面に切り換える。これにより以後、
切り換えられた軌道平面上であって、法線方向に直行す
る方向にハンドは移動する。
立すると軌道平面切換部13は倣い軌道平面を、予め設定
してある別の軌道平面に切り換える。これにより以後、
切り換えられた軌道平面上であって、法線方向に直行す
る方向にハンドは移動する。
このように、力検出部と位置検出部を設け、検出値に
基づいて力制御、位置制御を行って、検出反力を設定値
に維持しながら、ハンドを法線方向に直交する方向（接
線方向）に移動させて対象物表面を倣わせるように構成
したから、教示作業や曲面の定義が不要であり、しかも
作業条件を設定するだけでロボットハンドをして対象物
表面を倣わせることができる。又、所定の条件が満たさ
れた時、ロボットの倣い軌道平面を自動的に切り換える
ようにして倣うように構成したから、所望の範囲全体を
自動的に倣わすことができる。
基づいて力制御、位置制御を行って、検出反力を設定値
に維持しながら、ハンドを法線方向に直交する方向（接
線方向）に移動させて対象物表面を倣わせるように構成
したから、教示作業や曲面の定義が不要であり、しかも
作業条件を設定するだけでロボットハンドをして対象物
表面を倣わせることができる。又、所定の条件が満たさ
れた時、ロボットの倣い軌道平面を自動的に切り換える
ようにして倣うように構成したから、所望の範囲全体を
自動的に倣わすことができる。
尚、倣い軌道平面を該平面上の１つのベクトルで、あ
るいは倣い軌道平面と直交する平面上の１つのベクトル
で特定するようにすれば、軌道平面の設定を容易に行う
ことができる。又、倣い軌道平面の切換を所定位置に到
達した時、あるいは検出反力が設定値になった時、ある
いは移動時間または移動距離が設定値になった時に行う
ようにすれば、これら切換条件と切換後の倣い軌道平面
を設定しておくだけで自動的に倣い軌道平面を切り換え
て対象物表面を倣うことができる。
るいは倣い軌道平面と直交する平面上の１つのベクトル
で特定するようにすれば、軌道平面の設定を容易に行う
ことができる。又、倣い軌道平面の切換を所定位置に到
達した時、あるいは検出反力が設定値になった時、ある
いは移動時間または移動距離が設定値になった時に行う
ようにすれば、これら切換条件と切換後の倣い軌道平面
を設定しておくだけで自動的に倣い軌道平面を切り換え
て対象物表面を倣うことができる。
［実施例］ （ａ）ロボットと対象物の関係 第２図は本発明のロボット倣い制御におけるロボット
と対象物の関係図であり、RBTは関節型のロボット、WK
はロボットの作業対象となる対象物である。ロボットRB
Tにおいて、11はハンド、12はハンドを対象物WKに接触
させた時の接触力（対象物からの反力）を検出する力覚
センサ（力検出部）、21はマニピュレータ、22は関節軸
であり、ロボットには原点をＯとするロボット基準座標
系XoYoZoが定められている。
と対象物の関係図であり、RBTは関節型のロボット、WK
はロボットの作業対象となる対象物である。ロボットRB
Tにおいて、11はハンド、12はハンドを対象物WKに接触
させた時の接触力（対象物からの反力）を検出する力覚
センサ（力検出部）、21はマニピュレータ、22は関節軸
であり、ロボットには原点をＯとするロボット基準座標
系XoYoZoが定められている。
Ow−XwYwZwはロボットハンドと対象物の接触点Owを原
点とする倣い座標系（ワーク座標系）であり、対象物に
対するロボット先端部（ハンド）の位置、姿勢により決
定される座標系である。ベクトル，，はそれぞれ
倣い座標系の座標軸Xw,Yw,Zwについての単位ベクトル
で、ベクトルは対象物WKへ力を加える時の押しつけ方
向を示し、対象物の表面の法線ベクトルと同じ方向であ
る。ベクトルは、ベクトルと直交関係にあり、倣い
動作時のロボット先端の移動方向を示す。ベクトル
は、ベクトル，に直交するように定められ、 ＝× で与えられる。
点とする倣い座標系（ワーク座標系）であり、対象物に
対するロボット先端部（ハンド）の位置、姿勢により決
定される座標系である。ベクトル，，はそれぞれ
倣い座標系の座標軸Xw,Yw,Zwについての単位ベクトル
で、ベクトルは対象物WKへ力を加える時の押しつけ方
向を示し、対象物の表面の法線ベクトルと同じ方向であ
る。ベクトルは、ベクトルと直交関係にあり、倣い
動作時のロボット先端の移動方向を示す。ベクトル
は、ベクトル，に直交するように定められ、 ＝× で与えられる。
ベクトル，，のロボット基準座標系Xo−Yo−Zo
についての成分表示を行うと、 ＝（nx ny nz）Ｔ ＝（ox oy oz）Ｔ となる。但し、Ｔは転置行列を示す。
についての成分表示を行うと、 ＝（nx ny nz）Ｔ ＝（ox oy oz）Ｔ となる。但し、Ｔは転置行列を示す。
（ｂ）倣い制御の概略 第３図は本発明に係わるロボット倣い制御の概略説明
図である。本発明では、力覚センサ12で検出した対象物
WKからの接触力（反力）に基づいて、ハンド11と対象物
WKの接触点P1における法線方向を求め、検出反力が設
定値と等しくなるように法線方向におけるハンド押しつ
け力を制御しながら、ハンドを法線方向と直交する方向
（接線方向）に移動させて対象物表面を倣うようにし
ている。
図である。本発明では、力覚センサ12で検出した対象物
WKからの接触力（反力）に基づいて、ハンド11と対象物
WKの接触点P1における法線方向を求め、検出反力が設
定値と等しくなるように法線方向におけるハンド押しつ
け力を制御しながら、ハンドを法線方向と直交する方向
（接線方向）に移動させて対象物表面を倣うようにし
ている。
さて、ハンド11が対象物WKの曲面上を倣い、接触点が
P1からP2になると、該接触点P2での法線方向がポイント
P1における法線方向（押しつけ方向）と一致しなくな
る。このため、ポイントP2において、面の法線方向と押
しつけ方向が一致しなくなり、対象物に対して発生する
押しつけ力が指令値からずれ、正しく対象物表面を倣え
なくなる。そこで、正確な力を発生するためには、倣い
座標系をハンドの移動に従って切り換える必要がある。
すなわち、ハンドの移動に従って、倣い座標系のXw軸が
法線方向を向くように、倣い座標系を切り換える必要が
ある。倣い座標系を切り換えるタイミングとしては、 １）所定時間毎に行う、 ２）対象物から受ける反力が設定力からある程度以上ず
れた時に行う、 ３）ロボットが一定距離移動した時に行う等の方法があ
る。
P1からP2になると、該接触点P2での法線方向がポイント
P1における法線方向（押しつけ方向）と一致しなくな
る。このため、ポイントP2において、面の法線方向と押
しつけ方向が一致しなくなり、対象物に対して発生する
押しつけ力が指令値からずれ、正しく対象物表面を倣え
なくなる。そこで、正確な力を発生するためには、倣い
座標系をハンドの移動に従って切り換える必要がある。
すなわち、ハンドの移動に従って、倣い座標系のXw軸が
法線方向を向くように、倣い座標系を切り換える必要が
ある。倣い座標系を切り換えるタイミングとしては、 １）所定時間毎に行う、 ２）対象物から受ける反力が設定力からある程度以上ず
れた時に行う、 ３）ロボットが一定距離移動した時に行う等の方法があ
る。
本発明では、所定サンプリング時間ごとに法線方向を
求め、該方向に押しつけ力が発生するように、しかも検
出反力が設定値と等しくなるように力制御を行う。
求め、該方向に押しつけ力が発生するように、しかも検
出反力が設定値と等しくなるように力制御を行う。
又、本発明では、倣い動作時の各地点（サンプリング
時刻における位置）における移動方向ベクトルが、オ
ペレータが予め設定してある倣い軌道平面に平行となる
ように、しかも検出位置が目標位置と等しくなるように
位置制御を行う。
時刻における位置）における移動方向ベクトルが、オ
ペレータが予め設定してある倣い軌道平面に平行となる
ように、しかも検出位置が目標位置と等しくなるように
位置制御を行う。
以上の力制御、位置制御に基づいて、検出反力を法線
方向の設定力と等しくなるように制御しながら、ハンド
を法線方向に直交する方向（接線方向）であって倣い軌
道平面上を移動させて対象物表面を倣わせる。
方向の設定力と等しくなるように制御しながら、ハンド
を法線方向に直交する方向（接線方向）であって倣い軌
道平面上を移動させて対象物表面を倣わせる。
ハンドが移動して倣い軌道平面切換条件が成立すると
倣い軌道平面を、予め設定してある別の倣い軌道平面に
切り換え、以後、法線方向に直交する方向であって、切
り換えられた軌道平面上を移動するようにハンドを移動
して倣いを行う。
倣い軌道平面を、予め設定してある別の倣い軌道平面に
切り換え、以後、法線方向に直交する方向であって、切
り換えられた軌道平面上を移動するようにハンドを移動
して倣いを行う。
以下、倣い座標系を切り換えながら対象物に対して倣
い動作を行う倣い制御方法と、倣い動作中に倣い軌道を
切り換えて再び倣いを再開する方法について説明する。
い動作を行う倣い制御方法と、倣い動作中に倣い軌道を
切り換えて再び倣いを再開する方法について説明する。
（ｃ）倣い座標系の算出 第４図は倣い座標系とロボットの軌道がどのように変
化するかを示す説明図であり、WKは対象物、31は最初の
倣い軌道平面、32は軌道平面切換後の次の倣い軌道平
面、Pbは倣い開始点（始点）、Piは倣い制御の中間点、
Pi′は倣い軌道切換点である。まず、第４図の始点Pbで
の倣い座標系の算出方向を説明する。
化するかを示す説明図であり、WKは対象物、31は最初の
倣い軌道平面、32は軌道平面切換後の次の倣い軌道平
面、Pbは倣い開始点（始点）、Piは倣い制御の中間点、
Pi′は倣い軌道切換点である。まず、第４図の始点Pbで
の倣い座標系の算出方向を説明する。
（１）始点での法線ベクトルの算出 始点Pbでの法線ベクトルｂを次のようにして求め
る。第５図（ａ）はハンドが対象物から受ける反力Ｓ
と、力覚センサ座標系Os−XsYsZsの関係を示した図であ
る。力覚センサでは、反力ＳのXs,Ys,Zs方向のそれぞ
れの分力fx,fy,fzが検出される（力覚センサの検出する
トルク成分についての説明は省略する）。
る。第５図（ａ）はハンドが対象物から受ける反力Ｓ
と、力覚センサ座標系Os−XsYsZsの関係を示した図であ
る。力覚センサでは、反力ＳのXs,Ys,Zs方向のそれぞ
れの分力fx,fy,fzが検出される（力覚センサの検出する
トルク成分についての説明は省略する）。
反力Ｓをベクトル表示すると（ｓは力覚センサ座標
系Os−XsYsZsで記述されていることを示す）、 Ｓ＝（fxfyfz）Ｔ （１） となる。力覚センサ座標系で表した法線ベクトルＳｂ
は、Ｓと逆向きのベクトルであり、成分表示すると、 である。ただし、|S｜はベクトルの大きさである。
系Os−XsYsZsで記述されていることを示す）、 Ｓ＝（fxfyfz）Ｔ （１） となる。力覚センサ座標系で表した法線ベクトルＳｂ
は、Ｓと逆向きのベクトルであり、成分表示すると、 である。ただし、|S｜はベクトルの大きさである。
次に、法線ベクトルＳｂをロボット基準座標系に変
換する。力覚センサ座標系の各座標軸Xs,Ys,Zsのロボッ
ト基準座標系に対する単位ベクトルを０s,0s,0ｓ
とする（Ｏはロボット基準座標系で記述されていること
を示す）。成分表示すると、 ０ｓ＝（n sx n sy n sz）Ｔ ０ｓ＝（o sx o sy o sz）Ｔ ０ｓ＝（a sx a sy a sz）Ｔ （３） である。このとき、力覚センサ座標系から基準座標系へ
の座標変換行列0Asは、 0As＝（０s 0s 0ｓ） （４） で与えられる。従って、ロボット基準座標系で表した法
線ベクトル０ｂは、座標系変換行列0Asを用いて、 ０ｂ＝0As Sｂ （５） となる。法線ベクトル０ｂは、後述する法線ベクトル
演算部で算出される。又、始点以外の他の接触点での法
線ベクトルも同様に求めることができる。
換する。力覚センサ座標系の各座標軸Xs,Ys,Zsのロボッ
ト基準座標系に対する単位ベクトルを０s,0s,0ｓ
とする（Ｏはロボット基準座標系で記述されていること
を示す）。成分表示すると、 ０ｓ＝（n sx n sy n sz）Ｔ ０ｓ＝（o sx o sy o sz）Ｔ ０ｓ＝（a sx a sy a sz）Ｔ （３） である。このとき、力覚センサ座標系から基準座標系へ
の座標変換行列0Asは、 0As＝（０s 0s 0ｓ） （４） で与えられる。従って、ロボット基準座標系で表した法
線ベクトル０ｂは、座標系変換行列0Asを用いて、 ０ｂ＝0As Sｂ （５） となる。法線ベクトル０ｂは、後述する法線ベクトル
演算部で算出される。又、始点以外の他の接触点での法
線ベクトルも同様に求めることができる。
（２）始点での移動方向ベクトルの算出 次に、オペレータが予め設定した移動方向ベクトル
opb（倣い軌道平面31上のベクトル）と、求めた法線ベ
クトル０ｂを用いて、始点Pbにおける実際の移動方向
ベクトルｂを算出する。但し、ベクトルopbとベク
トル０ｂは、 opb＝０ｂまたはopb＝−０ｂ （６） を満たさないものとする。実際の移動方向ベクトルｂ
は、法線ベクトル０ｂに直交し、移動方向ベクトル
opbと法線ベクトル０ｂのなす平面（倣い軌道平面3
1）上にあるベクトルである。このとき、倣い座標系の
座標軸を表す単位ベクトルの１つであるｂは、０ｂ
とopbを用いて、０ ｂ＝（０ｂ×opb）/|0ｂ×opb| （７） で表される。移動方向ベクトル０ｂは、ベクトル０
ｂ、０ｂとの直交関係により、 ０ｂ＝０ｂ×０ｂ （８） で求められる。
opb（倣い軌道平面31上のベクトル）と、求めた法線ベ
クトル０ｂを用いて、始点Pbにおける実際の移動方向
ベクトルｂを算出する。但し、ベクトルopbとベク
トル０ｂは、 opb＝０ｂまたはopb＝−０ｂ （６） を満たさないものとする。実際の移動方向ベクトルｂ
は、法線ベクトル０ｂに直交し、移動方向ベクトル
opbと法線ベクトル０ｂのなす平面（倣い軌道平面3
1）上にあるベクトルである。このとき、倣い座標系の
座標軸を表す単位ベクトルの１つであるｂは、０ｂ
とopbを用いて、０ ｂ＝（０ｂ×opb）/|0ｂ×opb| （７） で表される。移動方向ベクトル０ｂは、ベクトル０
ｂ、０ｂとの直交関係により、 ０ｂ＝０ｂ×０ｂ （８） で求められる。
尚、上記の方法では、オペレータが倣い軌道平面31上
の移動方向ベクトルopbを設定した場合であるが、倣
い座標系の座標軸を表す単位ベクトルの１つである
（倣い軌道平面に直交する平面上のベクトル）を設定し
ても良い。設定するベクトルをopbとすると、移動方
向ベクトルｂは、０ ｂ＝（０opb×０ｂ）/|0opb ×０b| （９） のように表される。倣い座標系の座標軸を表す単位ベク
トルの１つであるｂは、ベクトル０ｂ、０ｂとの
直交関係により、 ０ｂ＝０ｂ×０ｂ （10） で求められる。尚、この方法は倣い軌道平面を与えるこ
とと等価である。移動方向ベクトル０ｂは、後述する
移動方向ベクトル算出部で算出される。
の移動方向ベクトルopbを設定した場合であるが、倣
い座標系の座標軸を表す単位ベクトルの１つである
（倣い軌道平面に直交する平面上のベクトル）を設定し
ても良い。設定するベクトルをopbとすると、移動方
向ベクトルｂは、０ ｂ＝（０opb×０ｂ）/|0opb ×０b| （９） のように表される。倣い座標系の座標軸を表す単位ベク
トルの１つであるｂは、ベクトル０ｂ、０ｂとの
直交関係により、 ０ｂ＝０ｂ×０ｂ （10） で求められる。尚、この方法は倣い軌道平面を与えるこ
とと等価である。移動方向ベクトル０ｂは、後述する
移動方向ベクトル算出部で算出される。
次に、第４図の点Piでの倣い座標系の算出方法につい
て説明する。
て説明する。
（３）点Piでの法線ベクトルの算出 点Piでの法線ベクトルｉの算出方法は、始点Psでの
算出方法と同様であり、次式で表される。但し、点Piで
の反力Ｓｉを、 Ｓｉ＝（f xi f yi f zi）Ｔ とする。
算出方法と同様であり、次式で表される。但し、点Piで
の反力Ｓｉを、 Ｓｉ＝（f xi f yi f zi）Ｔ とする。
０ｉ＝0Asi Sｉ （12） 0Asiは、点Piの力覚センサ座標系からロボット基準座
標系への座標系変換行列である。
標系への座標系変換行列である。
（４）点Piでの移動方向ベクトルの算出 ロボットの先端位置は、ベクトル方向にも制御され
るので、先端位置が常にベクトル，がなす平面（倣
い軌道平面31）上にあるように制御される。従って、対
象物の表面に描く軌跡は、始点Pbで与えられたベクトル
b,ｂのなす平面（ベクトルopbを含む倣い軌道平
面31）上の曲線となる。このことから、倣い軌道を同一
平面（倣い軌道平面）上に保つには、点Piでの移動方向
ベクトルｉを、始点で求められたベクトルｂに垂直
に与えれば良いことが分かる。
るので、先端位置が常にベクトル，がなす平面（倣
い軌道平面31）上にあるように制御される。従って、対
象物の表面に描く軌跡は、始点Pbで与えられたベクトル
b,ｂのなす平面（ベクトルopbを含む倣い軌道平
面31）上の曲線となる。このことから、倣い軌道を同一
平面（倣い軌道平面）上に保つには、点Piでの移動方向
ベクトルｉを、始点で求められたベクトルｂに垂直
に与えれば良いことが分かる。
移動方向ベクトルｉは、法線ベクトルｉにも直交
するので、０ ｉ＝（０ｂ×０ｉ）/|0ｂ ×０i| （13） のように表される。倣い座標系の座標軸を表す単位ベク
トルの１つであるｉは、ベクトル０ｉ、０ｉとの
直交関係により、 ０ｉ＝０ｉ×０ｉ （14） で求められる。
するので、０ ｉ＝（０ｂ×０ｉ）/|0ｂ ×０i| （13） のように表される。倣い座標系の座標軸を表す単位ベク
トルの１つであるｉは、ベクトル０ｉ、０ｉとの
直交関係により、 ０ｉ＝０ｉ×０ｉ （14） で求められる。
（５）点Pi′での移動方向ベクトルの算出（移動方向を
切り換える場合） 移動方向を切り換える場合の移動方向ベクトルの算出
は、始点での移動方向ベクトルの算出と同様の方法で行
う。すなわち、オペレータが与えた移動方向と、点Pi′
の接点での法線ベクトルから移動方向ベクトルを算出す
る。
切り換える場合） 移動方向を切り換える場合の移動方向ベクトルの算出
は、始点での移動方向ベクトルの算出と同様の方法で行
う。すなわち、オペレータが与えた移動方向と、点Pi′
の接点での法線ベクトルから移動方向ベクトルを算出す
る。
オペレータが設定する点Pi′での倣い軌道平面32上の
移動方向ベクトルをopiとする。ただし、ベクトルo
piとベクトル０ｉ′は、 opi＝０ｉ′またはopi＝−０ｉ′ （15） を満たさないものとする。移動方向ベクトルｉは、法
線ベクトル０ｉ′に直交し、移動方向ベクトルopi
と法線ベクトル０ｉ′のなす平面（倣い軌道平面32）
上にあるベクトルである。このとき、倣い座標系の座標
軸を表す単位ベクトルの１つであるｉとopiを用い
て、０ ｉ′＝（０ｉ′×opi）/|0ｉ′ ×opi| （16） で表される。移動方向ベクトル０ｉ′は、ベクトル０
ｉ、０ｉ′との直交関係により、 ０ｉ′＝０ｉ′×０ｉ′ （17） で求められる。
移動方向ベクトルをopiとする。ただし、ベクトルo
piとベクトル０ｉ′は、 opi＝０ｉ′またはopi＝−０ｉ′ （15） を満たさないものとする。移動方向ベクトルｉは、法
線ベクトル０ｉ′に直交し、移動方向ベクトルopi
と法線ベクトル０ｉ′のなす平面（倣い軌道平面32）
上にあるベクトルである。このとき、倣い座標系の座標
軸を表す単位ベクトルの１つであるｉとopiを用い
て、０ ｉ′＝（０ｉ′×opi）/|0ｉ′ ×opi| （16） で表される。移動方向ベクトル０ｉ′は、ベクトル０
ｉ、０ｉ′との直交関係により、 ０ｉ′＝０ｉ′×０ｉ′ （17） で求められる。
尚、上記の方法では、オペレータが倣い軌道平面32上
の移動方向ベクトルopiを設定した場合であるが、倣
い座標系の座標軸を表す単位ベクトルの１つである
（倣い軌道平面に直交する平面上のベクトル）を設定し
ても良い。設定するベクトルをopiとすると、移動方
向ベクトルｉ′は、０ ｉ′＝（０opi×０ｉ′）/|0opi ×０ｉ′｜ （18） のように表される。倣い座標系の座標軸を表す単位ベク
トルの１つであるｉは、ベクトル０ｉ′、０ｉ′
との直交関係により、 ０ｉ′＝０ｉ′×０ｉ′ （19） で求められる。この方法は倣い軌道平面を与えることと
等価である。移動方向ベクトルは、後述する移動方向ベ
クトル算出部で算出される。
の移動方向ベクトルopiを設定した場合であるが、倣
い座標系の座標軸を表す単位ベクトルの１つである
（倣い軌道平面に直交する平面上のベクトル）を設定し
ても良い。設定するベクトルをopiとすると、移動方
向ベクトルｉ′は、０ ｉ′＝（０opi×０ｉ′）/|0opi ×０ｉ′｜ （18） のように表される。倣い座標系の座標軸を表す単位ベク
トルの１つであるｉは、ベクトル０ｉ′、０ｉ′
との直交関係により、 ０ｉ′＝０ｉ′×０ｉ′ （19） で求められる。この方法は倣い軌道平面を与えることと
等価である。移動方向ベクトルは、後述する移動方向ベ
クトル算出部で算出される。
（ｄ）倣い座標系の切換タイミング 倣い座標系の押しつけ方向を曲面の法線ベクトルに一
致させるための座標系切換のタイミングは、前述のよう
に、 １）所定時間経過した時、 ２）対象物から受ける反力が設定力からある程度以上ず
れた時、 ３）ロボットが一定距離移動した時、 のいずれかがある。
致させるための座標系切換のタイミングは、前述のよう
に、 １）所定時間経過した時、 ２）対象物から受ける反力が設定力からある程度以上ず
れた時、 ３）ロボットが一定距離移動した時、 のいずれかがある。
又、倣い動作中の移動方向（倣い軌道平面）を変更す
るための座標系の切換タイミングは、 １）設定した倣い動作時間を経過した時、 ２）移動方向の反力がある値以上になった時、 ３）ロボットの先端が設定位置に到達した時、 ４）設定した距離移動した時、 のいずれかである。すなわち、これらの条件のいずれか
を満たした時に、オペレータは予め登録しておいた移動
方向（倣い軌道平面）を用いて、前述の（ｃ）（５）の
方法により、移動方向ベクトルを求め、倣い動作を再開
する。
るための座標系の切換タイミングは、 １）設定した倣い動作時間を経過した時、 ２）移動方向の反力がある値以上になった時、 ３）ロボットの先端が設定位置に到達した時、 ４）設定した距離移動した時、 のいずれかである。すなわち、これらの条件のいずれか
を満たした時に、オペレータは予め登録しておいた移動
方向（倣い軌道平面）を用いて、前述の（ｃ）（５）の
方法により、移動方向ベクトルを求め、倣い動作を再開
する。
（ｅ）倣い動作 本発明では、力制御に基づいて、検出反力を法線方
向の目標力ｒと等しくなるように制御しながら、かつ
位置制御により法線方向に直交する方向であって、指定
された倣い軌道平面上を移動するようにハンドを移動さ
せ、対象物表面を倣わせる。そして、ハンドが移動し、
前記倣い軌道平面切換条件が成立すると倣い軌道平面
を、予め設定してある別の倣い軌道平面に切り換え、以
後、法線方向に直交する方向であって、切り換えられた
軌道平面上を移動するようにハンドを移動して倣いを行
う。
向の目標力ｒと等しくなるように制御しながら、かつ
位置制御により法線方向に直交する方向であって、指定
された倣い軌道平面上を移動するようにハンドを移動さ
せ、対象物表面を倣わせる。そして、ハンドが移動し、
前記倣い軌道平面切換条件が成立すると倣い軌道平面
を、予め設定してある別の倣い軌道平面に切り換え、以
後、法線方向に直交する方向であって、切り換えられた
軌道平面上を移動するようにハンドを移動して倣いを行
う。
以下、力制御における目標力と位置制御における目標
位置の決定方法を説明する。
位置の決定方法を説明する。
（１）目標力の発生 オペレータが設定した対象物に対する押しつけ力の大
きさをFrとすれば、始点Pbにおける目標力０ｒは、大
きさがFrで、方向・向きが法線方向ｂの力である。従
って、目標力０ｒは、（２）式で求めた法線ベクトル
０ｂを用いて次式 ０ｒ＝Fr・０ｂ （20） で与えられる。
きさをFrとすれば、始点Pbにおける目標力０ｒは、大
きさがFrで、方向・向きが法線方向ｂの力である。従
って、目標力０ｒは、（２）式で求めた法線ベクトル
０ｂを用いて次式 ０ｒ＝Fr・０ｂ （20） で与えられる。
尚、任意の点Piにおいても、該点における法線方向０
ｉを求めれば（20）式により目標力０ｒを演算でき
る。
ｉを求めれば（20）式により目標力０ｒを演算でき
る。
（２）目標位置の発生 始点Pbでの移動方向ベクトルｂは（９）式により求
まる。従って、目標位置はこの移動方向ベクトルｂで
与えられる方向にあり、現在位置から目標位置までの相
対位置（指令移動量）０ｂはｂを用いて次式 ０ｂ＝α・０ｂ （21） により与えられる。αは倣い座標系切換条件により定ま
る適当な定数であり、予め設定される。
まる。従って、目標位置はこの移動方向ベクトルｂで
与えられる方向にあり、現在位置から目標位置までの相
対位置（指令移動量）０ｂはｂを用いて次式 ０ｂ＝α・０ｂ （21） により与えられる。αは倣い座標系切換条件により定ま
る適当な定数であり、予め設定される。
尚、任意の点Piにおいても、該点における移動方向ベ
クトル０ｉを求めれば（21）式により目標位置０ｉ
を演算できる。
クトル０ｉを求めれば（21）式により目標位置０ｉ
を演算できる。
（３）倣い動作の終了 倣い制御動作は次の条件が満たされた時に終了する。
すなわち、 １）オペレータから終了指示があった時、 ２）一定時間経過した時、 ３）衝突した時、または対象物から離れた時、 ４）ロボットの可動範囲を越えた時、 倣い動作を終了する。
すなわち、 １）オペレータから終了指示があった時、 ２）一定時間経過した時、 ３）衝突した時、または対象物から離れた時、 ４）ロボットの可動範囲を越えた時、 倣い動作を終了する。
（ｆ）補足 （７），（16）式は、０ｂ＝０ｉ（０ｉ＝０
ｉ）または０ｂ＝−０ｉ（０ｉ＝−０ｉ）のと
きのは成立しない。この時、点Piの移動方向ベクトルに
オペレータが与えた移動方向ベクトルopb（opi）を
用いると進行方向が維持できる。
ｉ）または０ｂ＝−０ｉ（０ｉ＝−０ｉ）のと
きのは成立しない。この時、点Piの移動方向ベクトルに
オペレータが与えた移動方向ベクトルopb（opi）を
用いると進行方向が維持できる。
（ｇ）本発明の一実施例 全体の構成 第６図は本発明の一実施例構成図であり、第１図と同
一部分には同一符号を付している。
一部分には同一符号を付している。
１は制御対象であるロボットのハンド、１′はハンド
を駆動するマニピュレータ、２はロボットのハンドに取
付けられ、該ハンドが対象物から受ける反力（接触力）
を検出する力検出部、３はハンドの位置を検出する位置
検出部、４は力検出部により検出された反力に基づいて
対象物の法線ベクトルを算出する法線ベクトル算出部、
５は法線ベクトルに直交する方向であって、予め特定さ
れている倣い軌道平面上の移動方向ベクトルを算出する
移動方向ベクトル算出部、６は法線方向の目標力０ｒ
と移動方向の目標位置０ｒ等を出力する制御指令生成
部、７は法線方向の目標力と検出反力との差に基づい
て法線方向の速度指令Vfを出力する力制御部、８は移動
方向の目標位置０ｒと検出位置Ｘとの差に基づいて移
動方向の速度指令Vpを出力する位置制御部、９は各速度
指令を合成する合成部、10は合成速度指令に基づいてハ
ンドを移動させるハンド移動制御部、11は軌道平面切換
条件や切換後の軌道平面等の設定や操作を行う操作部、
12は設定された各種データを記憶する設定データ記憶
部、13はハンドが移動して、予め設定されている倣い軌
道平面切換条件が成立した時、倣い軌道平面を設定され
ている別の軌道平面に切り換える軌道平面切換部、14は
移動方向ベクトルを規定するための倣い軌道平面（移動
方向）を記憶する移動方向記憶部である。
を駆動するマニピュレータ、２はロボットのハンドに取
付けられ、該ハンドが対象物から受ける反力（接触力）
を検出する力検出部、３はハンドの位置を検出する位置
検出部、４は力検出部により検出された反力に基づいて
対象物の法線ベクトルを算出する法線ベクトル算出部、
５は法線ベクトルに直交する方向であって、予め特定さ
れている倣い軌道平面上の移動方向ベクトルを算出する
移動方向ベクトル算出部、６は法線方向の目標力０ｒ
と移動方向の目標位置０ｒ等を出力する制御指令生成
部、７は法線方向の目標力と検出反力との差に基づい
て法線方向の速度指令Vfを出力する力制御部、８は移動
方向の目標位置０ｒと検出位置Ｘとの差に基づいて移
動方向の速度指令Vpを出力する位置制御部、９は各速度
指令を合成する合成部、10は合成速度指令に基づいてハ
ンドを移動させるハンド移動制御部、11は軌道平面切換
条件や切換後の軌道平面等の設定や操作を行う操作部、
12は設定された各種データを記憶する設定データ記憶
部、13はハンドが移動して、予め設定されている倣い軌
道平面切換条件が成立した時、倣い軌道平面を設定され
ている別の軌道平面に切り換える軌道平面切換部、14は
移動方向ベクトルを規定するための倣い軌道平面（移動
方向）を記憶する移動方向記憶部である。
力制御部 力制御部２は、対象物に対する押し付けによりハンド
１が該対象物から受ける反力（接触力）の検出を行う力
覚センサ2aと、力覚センサにより検出された反力ｈを
変換マトリクスＴを用いてハンド座標系からロボット基
準座標系に変換する座標変換部2bを有している。
１が該対象物から受ける反力（接触力）の検出を行う力
覚センサ2aと、力覚センサにより検出された反力ｈを
変換マトリクスＴを用いてハンド座標系からロボット基
準座標系に変換する座標変換部2bを有している。
位置検出部 位置検出部３は、マニピュレータ１′を駆動するモー
タの回転速度を検出するタコメータ3aと、モータの所定
回転角度ごとにパルスを発生すると共に、該パルスを計
数して回転角度θｓを出力するエンコーダ＆カウンタ3b
と、マニピュレータ１′の回転角度（関節角度）θｓを
ロボット基準座標系での移動量Ｘに変換する変換部3cを
有している。
タの回転速度を検出するタコメータ3aと、モータの所定
回転角度ごとにパルスを発生すると共に、該パルスを計
数して回転角度θｓを出力するエンコーダ＆カウンタ3b
と、マニピュレータ１′の回転角度（関節角度）θｓを
ロボット基準座標系での移動量Ｘに変換する変換部3cを
有している。
法線ベクトル算出部 法線ベクトル算出部４は、力検出部２により検出され
た反力を用いて、倣い開始点では（２）〜（５）式に
より、倣い動作の中間点では（11）〜（12）式を用いて
ロボット基準座標系で表現した法線ベクトル０を演算
して移動方向ベクトル算出部５と制御指令生成部６に出
力する。
た反力を用いて、倣い開始点では（２）〜（５）式に
より、倣い動作の中間点では（11）〜（12）式を用いて
ロボット基準座標系で表現した法線ベクトル０を演算
して移動方向ベクトル算出部５と制御指令生成部６に出
力する。
移動方向ベクトル算出部 移動方向ベクトル算出部５は、倣い開始点では法線ベ
クトル０と倣い方向ベクトルopbを用いて（７）〜
（８）式により、倣い動作の中間点では（13）〜（14）
式により、又倣い軌道平面切換時には（16）〜（17）式
により、ロボット座標系で表現した移動方向ベクトル０
と０を求めて制御指令生成部６に入力する。
クトル０と倣い方向ベクトルopbを用いて（７）〜
（８）式により、倣い動作の中間点では（13）〜（14）
式により、又倣い軌道平面切換時には（16）〜（17）式
により、ロボット座標系で表現した移動方向ベクトル０
と０を求めて制御指令生成部６に入力する。
制御指令生成部 制御指令生成部６は、法線方向の目標力０ｒを（2
0）式により求めて力制御部７に入力すると共に、力制
御のパラメータとなる法線ベクトル０とゲインCfを力
制御部７に入力する。
0）式により求めて力制御部７に入力すると共に、力制
御のパラメータとなる法線ベクトル０とゲインCfを力
制御部７に入力する。
又、制御指令生成部６は、（21）式より移動方向ベク
トルが示す方向の目標位置（相対位置）０ｒを演算し
て位置制御部８に入力すると共に、位置制御のパラメー
タとなる移動方向ベクトル０、ベクトル０、ゲイン
Cpを位置制御部８に入力する。
トルが示す方向の目標位置（相対位置）０ｒを演算し
て位置制御部８に入力すると共に、位置制御のパラメー
タとなる移動方向ベクトル０、ベクトル０、ゲイン
Cpを位置制御部８に入力する。
尚、制御指令生成部６は予め設定されている倣い速度
V0も出力する。
V0も出力する。
力制御部 力制御部７は、法線方向の目標力０ｒと検出反力
との差が零となるように、該差に基づいて法線方向の第
１の速度指令Vfを出力するものであり、目標力０ｒと
検出反力との差０Δを演算する算出部7aと、前記速
度指令Vfを発生する第１速度指令発生部7bを有してい
る。第１速度指令発生部7bは、第７図に示すように、前
記力差０Δをロボット基準座標系から倣い座標系に変
換する座標変換部7b−１と、倣い座標系に変換された力
差ＳΔより法線方向成分を選択する選択部7b−２と、
該法線方向成分をロボット基準座標系に変換する座標変
換部7b−３と、法線方向の力差成分に所定のゲインCfを
乗算して第１速度指令Vfを出力する乗算部7b−４で構成
されている。尚、倣い座標系（XwYwZw）からロボット基
準座標系（XoYoZo）へ変換する直交座標変換行列Ｒは、
ベクトル，，を用いて次式 のように表される。従って、座標変換部7b−１では変換
行列RTを用いて力差０Δを倣い座標系に変換でき（０
Δ＝RT0Δ）、座標変換部7b−３は変換行列Ｒを用
いて法線方向成分をロボット基準座標系に変換できる。
との差が零となるように、該差に基づいて法線方向の第
１の速度指令Vfを出力するものであり、目標力０ｒと
検出反力との差０Δを演算する算出部7aと、前記速
度指令Vfを発生する第１速度指令発生部7bを有してい
る。第１速度指令発生部7bは、第７図に示すように、前
記力差０Δをロボット基準座標系から倣い座標系に変
換する座標変換部7b−１と、倣い座標系に変換された力
差ＳΔより法線方向成分を選択する選択部7b−２と、
該法線方向成分をロボット基準座標系に変換する座標変
換部7b−３と、法線方向の力差成分に所定のゲインCfを
乗算して第１速度指令Vfを出力する乗算部7b−４で構成
されている。尚、倣い座標系（XwYwZw）からロボット基
準座標系（XoYoZo）へ変換する直交座標変換行列Ｒは、
ベクトル，，を用いて次式 のように表される。従って、座標変換部7b−１では変換
行列RTを用いて力差０Δを倣い座標系に変換でき（０
Δ＝RT0Δ）、座標変換部7b−３は変換行列Ｒを用
いて法線方向成分をロボット基準座標系に変換できる。
又、法線方向の力差成分を選択する選択行列Sfは、倣
い座標系（XwYwZw）のXw（法線方向）を力制御方向、Y
w,Zw方向を位置制御方向とすると（第２図参照）、 で与えられる。従って、座標変換部7b−３は上記選択行
列Sfを用いて力差の法線方向成分を選択できる。
い座標系（XwYwZw）のXw（法線方向）を力制御方向、Y
w,Zw方向を位置制御方向とすると（第２図参照）、 で与えられる。従って、座標変換部7b−３は上記選択行
列Sfを用いて力差の法線方向成分を選択できる。
更に、ゲインCfは一定値であり、次式 により表現される。
位置制御部 位置制御部８は、移動方向ベクトルが示す方向の目
標位置０ｒと検出位置Ｘの差が零となるように、該差
に基づいて位置制御方向の第２の速度指令Vpを出力する
ものであり、目標位置０ｒと検出位置Ｘの差ΔＸを演
算する算出部8aと、前記速度指令Vpを発生する第２速度
指令発生部8bを有している。第２速度指令発生部8bは第
７図に示すように、前記位置偏差ΔＸをロボット基準座
標系から倣い座標系に変換する座標変換部8b−１と、倣
い座標系に変換された位置偏差より位置制御方向成分
（ベクトル，の方向成分）を選択する選択部8b−２
と、位置制御方向成分をロボット基準座標系に変換する
座標変換部8b−３と、位置制御方向の位置偏差成分に所
定のゲインCpを乗算して第２速度指令Vpを出力する乗算
部8b−４で構成されている。尚、位置制御方向成分を選
択する選択行列Sf′は Sf′＝Ｉ−Sf（但し、Ｉは単位行列） で表され、 で与えられる。
標位置０ｒと検出位置Ｘの差が零となるように、該差
に基づいて位置制御方向の第２の速度指令Vpを出力する
ものであり、目標位置０ｒと検出位置Ｘの差ΔＸを演
算する算出部8aと、前記速度指令Vpを発生する第２速度
指令発生部8bを有している。第２速度指令発生部8bは第
７図に示すように、前記位置偏差ΔＸをロボット基準座
標系から倣い座標系に変換する座標変換部8b−１と、倣
い座標系に変換された位置偏差より位置制御方向成分
（ベクトル，の方向成分）を選択する選択部8b−２
と、位置制御方向成分をロボット基準座標系に変換する
座標変換部8b−３と、位置制御方向の位置偏差成分に所
定のゲインCpを乗算して第２速度指令Vpを出力する乗算
部8b−４で構成されている。尚、位置制御方向成分を選
択する選択行列Sf′は Sf′＝Ｉ−Sf（但し、Ｉは単位行列） で表され、 で与えられる。
又、ゲインCpは一定値であり、次式 により表現される。
ハンド移動制御部 ハンド移動制御部10は、合成部９により合成された合
成速度指令をマニピュレータ１′の各関節の速度に変
換する逆ヤコビ変換部10aと、位置及び速度をフィード
バックされてモータを指令速度で回転させる速度制御回
路10bと、実速度と指令速度との差に応じた速度偏差を
アナログに変換するAD変換器10cと、パワーアンプ10d
と、サーボモータ10eを備えている。
成速度指令をマニピュレータ１′の各関節の速度に変
換する逆ヤコビ変換部10aと、位置及び速度をフィード
バックされてモータを指令速度で回転させる速度制御回
路10bと、実速度と指令速度との差に応じた速度偏差を
アナログに変換するAD変換器10cと、パワーアンプ10d
と、サーボモータ10eを備えている。
操作部 操作部11は、ロボットならい制御に先立って種々のパ
ラメータを入力したり、倣いの開始、停止等のオペレー
ションを実行するものである。
ラメータを入力したり、倣いの開始、停止等のオペレー
ションを実行するものである。
設定すべきパラメータとしては、倣い開始点における
倣い軌道平面を特定する移動方向ベクトルopb、倣い
座標系切換時間（サンプリング時間）τ、倣い軌道平面
の切換条件Fj,tj,Pjと切換後の倣い軌道平面opj、相
対位置指令係数α、目標設定力Fr、倣い指令速度Vo等が
あり、これらは設定データ記憶部12に記憶される。
倣い軌道平面を特定する移動方向ベクトルopb、倣い
座標系切換時間（サンプリング時間）τ、倣い軌道平面
の切換条件Fj,tj,Pjと切換後の倣い軌道平面opj、相
対位置指令係数α、目標設定力Fr、倣い指令速度Vo等が
あり、これらは設定データ記憶部12に記憶される。
軌道平面切換部 軌道平面切換部13は、設定された倣い軌道平面切換の
条件が成立したか監視し、例えば設定されている倣い軌
道平面切換位置に到達したか、あるいは検出反力が設定
値を越えたか監視しており、切換条件が成立すると、新
たな倣い軌道平面を特定するデータを制御指令生成部６
に入力すると共に、該倣い軌道平面を特定する移動方向
ベクトルopjを移動方向記憶部14に記憶する。
条件が成立したか監視し、例えば設定されている倣い軌
道平面切換位置に到達したか、あるいは検出反力が設定
値を越えたか監視しており、切換条件が成立すると、新
たな倣い軌道平面を特定するデータを制御指令生成部６
に入力すると共に、該倣い軌道平面を特定する移動方向
ベクトルopjを移動方向記憶部14に記憶する。
全体の動作 以下、本発明の全体の倣い動作を第８図の流れ図に従
って説明する。尚、以下において、添字Ｂは倣い開始点
を、添字ｉは第ｉ番目でのロボットと対象物との接触点
を、添字ｊは倣い軌道平面の切換・設定地点を表すもの
とする。
って説明する。尚、以下において、添字Ｂは倣い開始点
を、添字ｉは第ｉ番目でのロボットと対象物との接触点
を、添字ｊは倣い軌道平面の切換・設定地点を表すもの
とする。
まず、オペレータは操作部11から、以下に示す（ｉ）
〜（vi）のデータを入力する、すなわち、 （ｉ）倣い軌道平面切換地点（始点を含む）における移
動方向ベクトル０opj（あるいは軌道平面と直交する
ベクトル０opj）、 （ii）設定力Fr、 （iii）倣い座標系の切換時間τ、 （iv）倣い軌道平面切換条件である切換位置Pj（または
力Fjまたは時間tj）、 （ｖ）位置制御における指令位置を特定するための係数
α、 （vi）指令倣い速度Vo を設定し、設定データ記憶部12に記憶する（ステップ10
1）。
〜（vi）のデータを入力する、すなわち、 （ｉ）倣い軌道平面切換地点（始点を含む）における移
動方向ベクトル０opj（あるいは軌道平面と直交する
ベクトル０opj）、 （ii）設定力Fr、 （iii）倣い座標系の切換時間τ、 （iv）倣い軌道平面切換条件である切換位置Pj（または
力Fjまたは時間tj）、 （ｖ）位置制御における指令位置を特定するための係数
α、 （vi）指令倣い速度Vo を設定し、設定データ記憶部12に記憶する（ステップ10
1）。
これにより、軌道平面切換部13は、倣い開始点Pbにお
ける移動方向ベクトル０opbを移動方向記憶部14に記
憶する（ステップ102）。
ける移動方向ベクトル０opbを移動方向記憶部14に記
憶する（ステップ102）。
ついで、オペレータはハンドを移動して対象物に押し
つけるために始点力ベクトルｂを設定する（ステップ
103）。
つけるために始点力ベクトルｂを設定する（ステップ
103）。
始点力ベクトルが設定されると、制御指令生成部６
は、倣い開始点で力ｂを発生するために力指令Fb、ゲ
インCf等の力制御パラメータを発生して力制御部７に入
力する（ステップ104）。
は、倣い開始点で力ｂを発生するために力指令Fb、ゲ
インCf等の力制御パラメータを発生して力制御部７に入
力する（ステップ104）。
力制御部７は入力された力指令等に基づいて対象物方
向にハンドを移動させるための速度指令Vfを発生し、こ
れによりハンドは対象物方向に接近する（ステップ10
5）。
向にハンドを移動させるための速度指令Vfを発生し、こ
れによりハンドは対象物方向に接近する（ステップ10
5）。
力検出部２はハンドと対象物の接触により生じる反力
を検出する（ステップ106）。尚、反力が発生すると
法線ベクトル算出部４は法線ベクトル０ｂの算出が可
能となる。
を検出する（ステップ106）。尚、反力が発生すると
法線ベクトル算出部４は法線ベクトル０ｂの算出が可
能となる。
ハンドが対象物と接触せず反力が生じなければ、制
御指令生成部６はハンドが対象物と接触して反力が発
生するまで力指令Fbを出力して上記処理を繰り返す（ス
テップ107）。
御指令生成部６はハンドが対象物と接触して反力が発
生するまで力指令Fbを出力して上記処理を繰り返す（ス
テップ107）。
ハンドが移動して対象物と接触し、該接触により反力
が発生すると、法線ベクトル算出部４は該反力を用い
て倣い開始点における法線ベクトルＳｂ（倣い座標
系）を（２）式に従って算出する（ステップ108）。
が発生すると、法線ベクトル算出部４は該反力を用い
て倣い開始点における法線ベクトルＳｂ（倣い座標
系）を（２）式に従って算出する（ステップ108）。
ついで、法線ベクトル算出部４は（４）式により倣い
座標系からロボット基準座標系に変換するための座標変
換行列0Asを算出し、（５）式によりロボット座標系に
おける法線ベクトル０ｂを計算し、該法線ベクトルを
移動方向ベクトル算出部５と制御指令生成部６に入力す
る（ステップ109、110）。
座標系からロボット基準座標系に変換するための座標変
換行列0Asを算出し、（５）式によりロボット座標系に
おける法線ベクトル０ｂを計算し、該法線ベクトルを
移動方向ベクトル算出部５と制御指令生成部６に入力す
る（ステップ109、110）。
制御指令生成部６は軌道平面切換部13から入力されて
いる倣い開始点Pbにおける移動方向ベクトル±０opb
と法線ベクトル０ｂが一致するかチェックし（ステッ
プ111、112）、一致していればその旨を操作部11の図示
しない表示部に表示し、オペレータにパラメータの設定
しなおしを通知し、ステップ101以降の処理を繰り返さ
せる。
いる倣い開始点Pbにおける移動方向ベクトル±０opb
と法線ベクトル０ｂが一致するかチェックし（ステッ
プ111、112）、一致していればその旨を操作部11の図示
しない表示部に表示し、オペレータにパラメータの設定
しなおしを通知し、ステップ101以降の処理を繰り返さ
せる。
しかし、±０opb≠０ｂであれば、移動方向ベク
トル算出部５に移動方向ベクトルの演算を指示し、これ
により移動方向ベクトル算出部５は（７）式にしたがっ
てベクトル０ｂを演算し、ついで（８）式により移動
方向ベクトル０ｂを演算し、演算結果を制御指令生成
部６に入力する（ステップ113、114）。
トル算出部５に移動方向ベクトルの演算を指示し、これ
により移動方向ベクトル算出部５は（７）式にしたがっ
てベクトル０ｂを演算し、ついで（８）式により移動
方向ベクトル０ｂを演算し、演算結果を制御指令生成
部６に入力する（ステップ113、114）。
制御指令生成部６は、法線ベクトル０ｂの設定力Fr
を用いて、（20）式から目標力ベクトル０ｒを生成し
て力制御部７に入力すると共に、係数αと移動方向ベク
トル０ｂを用いて（21）式から目標位置（目標相対位
置）０ｒを生成して位置制御部８に入力する（ステッ
プ115、116）。
を用いて、（20）式から目標力ベクトル０ｒを生成し
て力制御部７に入力すると共に、係数αと移動方向ベク
トル０ｂを用いて（21）式から目標位置（目標相対位
置）０ｒを生成して位置制御部８に入力する（ステッ
プ115、116）。
又、制御指令生成部６は倣い速度指令Voを出力する
（ステップ117）。
（ステップ117）。
力制御部７は目標力０ｒと検出反力の差に基づい
て法線方向と速度指令Vfを出力し、位置制御部８は目標
位置０ｒと検出位置Ｘの差に基づいて位置制御方向の
速度指令Vpを出力し、合成部９はVo,Vf,Vpを合成してハ
ンド移動制御部10に入力してハンドを対象物の表面に沿
って移動させる（倣い動作）。・・・ステップ118 倣い動作と並行して、一定時間（サンプリング時間）
τ経過したか監視しており（ステップ119）、経過しな
ければ、法線ベクトル及び移動方向ベクトルをステップ
110、114で求めた値とし（更新せず）、倣い動作を継続
する。
て法線方向と速度指令Vfを出力し、位置制御部８は目標
位置０ｒと検出位置Ｘの差に基づいて位置制御方向の
速度指令Vpを出力し、合成部９はVo,Vf,Vpを合成してハ
ンド移動制御部10に入力してハンドを対象物の表面に沿
って移動させる（倣い動作）。・・・ステップ118 倣い動作と並行して、一定時間（サンプリング時間）
τ経過したか監視しており（ステップ119）、経過しな
ければ、法線ベクトル及び移動方向ベクトルをステップ
110、114で求めた値とし（更新せず）、倣い動作を継続
する。
一定時間が経過すれば、制御指令生成部６は、ロボッ
ト先端位置（ハンド位置）を取り込み、該位置がロボッ
ト可動範囲内にあるかチェックする（ステップ120,12
1）。
ト先端位置（ハンド位置）を取り込み、該位置がロボッ
ト可動範囲内にあるかチェックする（ステップ120,12
1）。
ロボット可動範囲外であれば、次の動作へ移行し、可
動範囲内であれば、反力を検出し、該反力が零か（対
象物から離れたか）、あるいは１より相当大きいか（壁
と衝突したか）判断し、＝０または≫１であれば動
作を直ちに停止する（ステップ122〜124）。
動範囲内であれば、反力を検出し、該反力が零か（対
象物から離れたか）、あるいは１より相当大きいか（壁
と衝突したか）判断し、＝０または≫１であれば動
作を直ちに停止する（ステップ122〜124）。
＝０でなく、≫１でなければ、検出位置を第ｉ番
目の位置として、以後開始点におけると同様の処理を行
ってハンドを対象物に沿って移動させる。すなわち、 法線ベクトル算出部４は検出反力を用いて第ｉ位置
Piにおける法線ベクトルＳｉ（倣い座標系）を（11）
式により算出する（ステップ125）。
目の位置として、以後開始点におけると同様の処理を行
ってハンドを対象物に沿って移動させる。すなわち、 法線ベクトル算出部４は検出反力を用いて第ｉ位置
Piにおける法線ベクトルＳｉ（倣い座標系）を（11）
式により算出する（ステップ125）。
ついで、法線ベクトル算出部４は（４）式に従って倣
い座標系からロボット基準座標系に変換するための座標
変換行列0Asを算出し、（12）式によりロボット座標系
における法線ベクトル０ｉを計算し、該法線ベクトル
を移動方向ベクトル算出部５と制御指令生成部６に入力
する（ステップ126,127）。
い座標系からロボット基準座標系に変換するための座標
変換行列0Asを算出し、（12）式によりロボット座標系
における法線ベクトル０ｉを計算し、該法線ベクトル
を移動方向ベクトル算出部５と制御指令生成部６に入力
する（ステップ126,127）。
移動方向ベクトル算出部５は（13）式にしたがって移
動方向ベクトル０ｉを演算し、演算結果を制御指令生
成部６に入力する（ステップ128）。
動方向ベクトル０ｉを演算し、演算結果を制御指令生
成部６に入力する（ステップ128）。
制御指令生成部６は、法線ベクトル０ｉと設定力Fr
を用いて、（20）式から目標力ベクトル０ｒを生成し
て力制御部７に入力すると共に、係数αと移動方向ベク
トル０ｉを用いて（21）式から目標位置（目標相対位
置）０ｒを生成して位置制御部８に入力する（ステッ
プ129、130）。
を用いて、（20）式から目標力ベクトル０ｒを生成し
て力制御部７に入力すると共に、係数αと移動方向ベク
トル０ｉを用いて（21）式から目標位置（目標相対位
置）０ｒを生成して位置制御部８に入力する（ステッ
プ129、130）。
又、制御指令生成部６は倣い速度指令Voを出力する
（ステップ131）。
（ステップ131）。
力制御部７は目標力と検出反力の差に基づいて法線方
向の速度指令Vfを出力し、位置制御部８は目標位置と検
出位置の差に基づいて位置制御方向の速度指令Vpを出力
し、合成部９はVo,Vf,Vpを合成してハンド移動制御部10
に入力してハンドを対象物の表面に沿って移動させる
（倣い動作）。・・・ステップ132 倣い動作と並行して、一定時間（サンプリング時間）
τ経過したか監視しており（ステップ133）、経過しな
ければ、法線ベクトル及び移動方向ベクトルをステップ
127,128で求めた値とし（更新せず）、倣い動作を継続
する。
向の速度指令Vfを出力し、位置制御部８は目標位置と検
出位置の差に基づいて位置制御方向の速度指令Vpを出力
し、合成部９はVo,Vf,Vpを合成してハンド移動制御部10
に入力してハンドを対象物の表面に沿って移動させる
（倣い動作）。・・・ステップ132 倣い動作と並行して、一定時間（サンプリング時間）
τ経過したか監視しており（ステップ133）、経過しな
ければ、法線ベクトル及び移動方向ベクトルをステップ
127,128で求めた値とし（更新せず）、倣い動作を継続
する。
一定時間が経過すれば、制御指令生成部６は、ロボッ
ト先端位置（ハンド位置）を取り込み、該位置がロボッ
ト可動範囲内にあるかチェックする（ステップ134,13
5）。
ト先端位置（ハンド位置）を取り込み、該位置がロボッ
ト可動範囲内にあるかチェックする（ステップ134,13
5）。
ロボット可動範囲外であれば、次の動作に移行し、可
動範囲内であれば、反力を検出し、該反力が零か（対
象物から離れたか）、あるいは１より相当大きいか（壁
と衝突したか）判断し、＝０または≫１であれば動
作を直ちに停止する（ステップ136〜138）。
動範囲内であれば、反力を検出し、該反力が零か（対
象物から離れたか）、あるいは１より相当大きいか（壁
と衝突したか）判断し、＝０または≫１であれば動
作を直ちに停止する（ステップ136〜138）。
＝０でなく、≫１でなければ、軌道平面切換部13
に倣い軌道平面切り換え条件が成立しているかの判断処
理の実行を指示する。これにより、軌道平面切換部13
は、倣い開始からの経過時間ｔ、あるいは倣い軌道平面
を切り換えてからの経過時間ｔを検出し（ステップ13
9）、ついで倣い軌道平面切換条件が成立したかチェッ
クする（ステップ140）。すなわち、軌道平面切換部13
は、現在位置Ｐが倣い軌道平面切換地点Pjに一致する
か、あるいは検出反力Ｆが設定値Fjに一致するか、又は
経過時間ｔが設定時間tjに一致するかチェックする。
に倣い軌道平面切り換え条件が成立しているかの判断処
理の実行を指示する。これにより、軌道平面切換部13
は、倣い開始からの経過時間ｔ、あるいは倣い軌道平面
を切り換えてからの経過時間ｔを検出し（ステップ13
9）、ついで倣い軌道平面切換条件が成立したかチェッ
クする（ステップ140）。すなわち、軌道平面切換部13
は、現在位置Ｐが倣い軌道平面切換地点Pjに一致する
か、あるいは検出反力Ｆが設定値Fjに一致するか、又は
経過時間ｔが設定時間tjに一致するかチェックする。
倣い条件が成立しなければ、オペレータによる倣い制
御終了の指示があるかチェックし（ステップ141）、あ
れば次の動作へ、指示がなければ現在位置をPiとしてス
テップ125に飛び、以降の処理を繰り返す。
御終了の指示があるかチェックし（ステップ141）、あ
れば次の動作へ、指示がなければ現在位置をPiとしてス
テップ125に飛び、以降の処理を繰り返す。
ステップ140において、倣い軌道平面の切換条件が成
立すれば、軌道平面切換部13は切換地点における倣い軌
道平面を特定する移動方向ベクトル０opjを移動方向
記憶部14にセットし（ステップ142）、法線ベクトル算
出部４は検出反力を用いて第ｉ位置Piにおける法線ベ
クトルＳｉ（倣い座標系）を（11）式に従って算出す
る（ステップ143）。
立すれば、軌道平面切換部13は切換地点における倣い軌
道平面を特定する移動方向ベクトル０opjを移動方向
記憶部14にセットし（ステップ142）、法線ベクトル算
出部４は検出反力を用いて第ｉ位置Piにおける法線ベ
クトルＳｉ（倣い座標系）を（11）式に従って算出す
る（ステップ143）。
ついで、法線ベクトル算出部４は（４）式に従って倣
い座標系からロボット基準座標系に変換するための座標
変換行列0Asを算出し、（12）式によりロボット座標系
における法線ベクトル０ｉを計算し、該法線ベクトル
を移動方向ベクトル算出部５と制御指令生成部６に入力
する（ステップ144,145）。
い座標系からロボット基準座標系に変換するための座標
変換行列0Asを算出し、（12）式によりロボット座標系
における法線ベクトル０ｉを計算し、該法線ベクトル
を移動方向ベクトル算出部５と制御指令生成部６に入力
する（ステップ144,145）。
移動方向ベクトル算出部５は、入力された法線ベクト
ル０ｉと移動方向記憶部14から与えられる移動方向ベ
クトル０opjを用いて、（16），（17）式にしたがっ
て移動方向ベクトル０ｉを演算し、演算結果を制御指
令生成部６に入力し（ステップ146、147）、ステップ12
9に飛び以降の処理を繰り返す。
ル０ｉと移動方向記憶部14から与えられる移動方向ベ
クトル０opjを用いて、（16），（17）式にしたがっ
て移動方向ベクトル０ｉを演算し、演算結果を制御指
令生成部６に入力し（ステップ146、147）、ステップ12
9に飛び以降の処理を繰り返す。
（ｉ）本発明の他の実施例 以上では、ロボット倣い制御装置を機能毎にブロック
化して説明したが、ホストコンピュータと、ロボットコ
ントローラとで構成するようにもできる。例えば、法線
ベクトル演算、移動方向ベクトル演算、制御指令生成、
設定データ記憶、軌道平面切換、移動方向記憶制御等を
ホスト側で行い、力制御部、位置制御部、ハンド移動制
御をロボットコントローラ側で行うようにする。
化して説明したが、ホストコンピュータと、ロボットコ
ントローラとで構成するようにもできる。例えば、法線
ベクトル演算、移動方向ベクトル演算、制御指令生成、
設定データ記憶、軌道平面切換、移動方向記憶制御等を
ホスト側で行い、力制御部、位置制御部、ハンド移動制
御をロボットコントローラ側で行うようにする。
以上本発明を実施例により説明したが、本発明は請求
の範囲に記載した本発明の主旨に従い種々の変形が可能
であり、本発明はこれらを排除するものではない。
の範囲に記載した本発明の主旨に従い種々の変形が可能
であり、本発明はこれらを排除するものではない。
［発明の効果］ 以上本発明によれば、力検出部と位置検出部を設け、
検出値に基づいて力制御、位置制御を行って、検出反力
を設定値に維持しながら、ハンドを法線方向に直交する
方向（接線方向）に移動させて対象物表面を倣わせるよ
うに構成したから、教示作業や曲面の定義が不要であ
り、しかも作業条件を設定するだけでロボットハンドを
して対象物の表面を倣わせることができる。
検出値に基づいて力制御、位置制御を行って、検出反力
を設定値に維持しながら、ハンドを法線方向に直交する
方向（接線方向）に移動させて対象物表面を倣わせるよ
うに構成したから、教示作業や曲面の定義が不要であ
り、しかも作業条件を設定するだけでロボットハンドを
して対象物の表面を倣わせることができる。
又、本発明によれば、所定の倣い軌道平面切換条件が
満たされた時、ロボットの倣い軌道平面を自動的に切り
換えるようにして倣うように構成したから、所望の範囲
全体を自動的に倣わすことができる。
満たされた時、ロボットの倣い軌道平面を自動的に切り
換えるようにして倣うように構成したから、所望の範囲
全体を自動的に倣わすことができる。
更に、倣い軌道平面を、該平面上の１つのベクトル
で、あるいは倣い軌道平面と直交する平面上の１つのベ
クトルで特定するようにしたから、軌道平面の設定を容
易に行うことができる。
で、あるいは倣い軌道平面と直交する平面上の１つのベ
クトルで特定するようにしたから、軌道平面の設定を容
易に行うことができる。
又、倣い軌道平面の切換を、所定位置に到達した時、
検出反力が設定値を越えた時、移動時間または移動距離
が設定以上になった時に行うようにしたから、これら切
換条件と切換後の倣い軌道平面を設定しておくだけで自
動的に倣い軌道平面を切り換えて対象物の表面を倣うこ
とができる。
検出反力が設定値を越えた時、移動時間または移動距離
が設定以上になった時に行うようにしたから、これら切
換条件と切換後の倣い軌道平面を設定しておくだけで自
動的に倣い軌道平面を切り換えて対象物の表面を倣うこ
とができる。
第１図は本発明の原理図、 第２図はロボットと対象物の関係図、 第３図は倣い制御の概略説明図、 第４図は倣い座標系とロボットの軌道関係図、 第５図は設定力の算出方向の説明図、 第６図は本発明の一実施例構成図、 第７図は位置制御部と力制御部の構成図、 第８図は本発明のロボットならい制御の流れ図である。 １……制御対象（ロボット） ２……力検出部 ３……位置検出部 ４……法線ベクトル算出部 ５……移動方向ベクトル算出部 ６……制御指令生成部 ７……力制御部 ８……位置制御部 10……ハンド移動制御部 13……軌道平面切換部 14……移動方向記憶部
フロントページの続き (56)参考文献 特開 平１−229306（ＪＰ，Ａ) 特開 平１−222311（ＪＰ，Ａ) 特開 平２−45806（ＪＰ，Ａ) 特開 昭62−140751（ＪＰ，Ａ) (58)調査した分野(Int.Cl.7，ＤＢ名) B25J 13/08 B25J 9/10
Claims (6)
- 【請求項１】ロボットのハンドに取付けられ、該ハンド
が対象物から受ける反力を検出する力検出部と、 ハンドの位置あるいは移動量を検出する検出部と、 力検出部により検出された反力に基づいて対象物の法線
ベクトルを算出する法線ベクトル算出部と、 法線ベクトルに直交する方向であって、予め特定されて
いる倣い軌道平面上の移動方向ベクトルを算出する移動
方向ベクトル算出部と、 法線方向の目標力と検出反力との差に基づいて法線方向
の速度指令を出力する第１の速度指令出力部と、 移動方向の目標位置と検出位置との差、あるいは目標移
動量と速度移動量との差に基づいて移動方向の速度指令
を出力する第２の速度指令出力部と、 第１、第２速度指令出力部から出力される速度指令に基
づいて前記各差が零となるようにハンドを移動させるハ
ンド移動制御部と、 ハンドが移動して、予め設定されている倣い軌道平面切
り換え条件が成立した時、倣い軌道平面を設定されてい
る別の軌道平面に切り換える軌道平面切換部を有するこ
とを特徴とするロボット倣い制御装置。 - 【請求項２】前記倣い軌道平面を、該平面上の１つのベ
クトルで特定することを特徴とする請求項１記載のロボ
ット倣い制御装置。 - 【請求項３】前記倣い軌道平面を、該平面と直交する平
面上の１つのベクトルで特定することを特徴とする請求
項１記載のロボット倣い制御装置。 - 【請求項４】前記軌道平面切換部は、倣い軌道平面切換
位置と切換後の倣い軌道平面を複数登録しておき、ハン
ドが登録してある倣い平面切換位置に到来したか監視
し、到来した時、倣い軌道平面を登録してある軌道平面
に切り換えることを特徴とする請求項１記載のロボット
倣い制御装置。 - 【請求項５】前記軌道平面切換部は、倣い軌道平面を切
り換えるための反力と切換後の倣い軌道平面を特定する
データを登録しておき、力検出部により検出された反力
が設定値になった時、倣い軌道平面を登録してある軌道
平面に切り換えることを特徴とする請求項１記載のロボ
ット倣い制御装置。 - 【請求項６】前記軌道平面切換部は、倣い軌道平面を切
り換えるための移動時間または移動距離と切換後の倣い
軌道平面を特定するデータを登録しておき、前記時間移
動した時、または前記距離移動した時、倣い軌道平面を
登録してある軌道平面に切り換えることを特徴とする請
求項１記載のロボット倣い制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP02273904A JP3089027B2 (ja) | 1990-10-12 | 1990-10-12 | ロボット倣い制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP02273904A JP3089027B2 (ja) | 1990-10-12 | 1990-10-12 | ロボット倣い制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH04152082A JPH04152082A (ja) | 1992-05-26 |
| JP3089027B2 true JP3089027B2 (ja) | 2000-09-18 |
Family
ID=17534195
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP02273904A Expired - Fee Related JP3089027B2 (ja) | 1990-10-12 | 1990-10-12 | ロボット倣い制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3089027B2 (ja) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2009034957A1 (ja) * | 2007-09-14 | 2009-03-19 | Kabushiki Kaisha Yaskawa Denki | ロボットの力制御方法、制御装置およびロボットシステム |
| JP6381200B2 (ja) * | 2012-11-22 | 2018-08-29 | キヤノン株式会社 | ロボット装置及びロボットの制御方法 |
-
1990
- 1990-10-12 JP JP02273904A patent/JP3089027B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH04152082A (ja) | 1992-05-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR20180059888A (ko) | 로봇교시방법 및 로봇 암 제어장치 | |
| JPH079606B2 (ja) | ロボット制御装置 | |
| JP3089027B2 (ja) | ロボット倣い制御装置 | |
| JP2786874B2 (ja) | 可動位置制御装置 | |
| JP2791030B2 (ja) | 多自由度作業機械の曲面倣い制御装置 | |
| JP3208722B2 (ja) | マニピュレータ用追従装置及び追従制御方法 | |
| JPH11191005A (ja) | ロボット制御装置 | |
| JP2737325B2 (ja) | ロボットの軌道生成方式 | |
| JPS5857762B2 (ja) | ロボットにおける移動速度の修正方法とその装置 | |
| JP2691591B2 (ja) | 力制御ロボットの制御方式 | |
| JP2830205B2 (ja) | ロボットの制御装置 | |
| JP3668821B2 (ja) | ロボットコントローラおよびロボット制御方法 | |
| JP3350687B2 (ja) | ロボット制御方法およびロボット制御装置 | |
| JPH07205068A (ja) | ロボットの座標系設定方法 | |
| JP2922617B2 (ja) | 力制御ロボットにおける押付力方向の指定方法 | |
| JP3191341B2 (ja) | ロボット制御装置 | |
| JP2868343B2 (ja) | 三次元レーザ加工機のオフライン教示方法 | |
| JP3078884B2 (ja) | 倣い制御装置 | |
| JPH01175004A (ja) | ロボット制御装置 | |
| JP2592636B2 (ja) | 倣い教示制御方式 | |
| JP3001211B2 (ja) | 任意曲面の倣い制御装置 | |
| JP2773778B2 (ja) | 力制御ロボットの倣い装置 | |
| JP2993708B2 (ja) | 工業用ロボット及びその軌道修正方法 | |
| de Graaf et al. | Real-time trajectory generation for sensor-guided robotic laser welding | |
| JPS63184806A (ja) | ロボツトによる相対座標系情報テイ−チング方式 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |