JPH03190687A - ロボットの倣い速度制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔概　要〕 ロボットの力制御時における倣い速度制御方式未知形状
の曲面を有する対象物体の表面を倣い動作するときに、
ロボットの先端の移動速度指令を変化させることにより
、倣い速度制御を容易に行うことを目的とし、 ロボットの力制御時における倣い速度制御方式において
、ロボットと対象物に作用する力を検出する力検出部と
、ロボットの位置を検出する位置検出部と、前記位置検
出部の位置座標値に基づきロボットの位置を制御する位
置制御部と、前記力検出部により検出した力に基づいて
ロボットの力を制御する力制御部と、ロボットヘカ・位
置指令及びパラメータの転送を行うため、ロボットの対
象物の接点の曲率を算出する曲率算出部と、曲率によっ
てロボット先端の移動速度を自動的に変化させる移動速
度設定部を有する制御指令生成部と、ロボットと対象物
の接点の法線ベクトルを算出し、かつ倣い座標系を算出
する法線ベクトル算出部と、ロボットの移動方向ベクト
ルを算出する移動方向ベクトル算出部とを備え、前記算
出した倣い座標系及び移動指令に基づいて未知形状の曲
面を持つ対象物の表面に、前記力制御部により一定の力
を加え、かつ自動的に移動速度を変化させながら倣い動
作を行うように構成する。

［産業上の利用分野〕 本発明はロボットの力制御時における倣い速度制御方式
に関し、力制御ロボットにより対象物体の表面に沿って
倣い加工を行うための倣い制御装置における倣い速度制
御方式に関する。

倣い制御装置は基本的に制御対象を作動させる操作部と
、制御対象の位置・姿勢を検出する位置検出部と、制御
対象が受ける力の検出を行う力検出部により構成される
。ロボットの先端が未知形状の曲面を有する対象物体の
表面を倣い動作するときに、ロボット先端の移動速度指
令を変化させて対象物体に加える力を一定に保持させる
必要がある。倣い制御装置はこのような制御を行うため
のものである。

［従来の技術と発明が解決しようとする課題〕ロボット
の力制御時に未知形状の曲面を有する対象物体に対して
倣い作業を行う際に、ロボットの先端と対象物の接触点
における対象物表面の法線方向に一致する押しつけ方向
ｎと、倣い作業をしながら移動する移動方向０で決定さ
れる倣い座標（○い−Ｘ　ｗ　Ｙ　ｗ　Ｚ　ｗ　）を対
象物の形状に合わせて逐次ロボットコントローラに与え
る必要がある。

第２図は曲率の大きい対象面Ａと、曲率の小さい対象面
Ｂをロボットの先端が一定の移動速度で倣うときの状態
を説明する図である。図示のように、対象物体上の点Ｐ
、で倣い座標系ｎ、ｏが設定され、オペレータにより設
定された力Ｆが加わっているとする。ロボットの先端は
次のサンプリング時間τ迄の間に、ベクトル０方向へは
一定速度■・ｒ迄移動し、ベクトルｎ方向にはロボット
と対象物体のなすバネ系の釣り合いを満たす位置まで変
位する。このとき、ロボットの先端は面Ａについては点
ＰＡ、ｉ。１、面Ｂについては点Ｐ７１．。

まで移動する。点ｐｔｔ、ｔ。４、及び点ＰＲ５ｉ。、
までのベクトルｎ方向への変位料をそれぞれδ８．δ６
とする。

ロボットと対象物がなすバネ系のバネ定数をｋとすると
、点Ｐ　Ａ＋　＝＋＋　、及び点ＰＲ９ｉ。１で対象物
に作用するベクトルｎ方向の力はそれぞれ、Ｆｋδ１及
びＦ−にδ８となる。δ、〉δ８なので、本図に示すよ
うに、面の曲率が大きいと、設定力と対象物への押しつ
け力とのずれも大きくなる。

このように、ロボット先端の移動速度を一定のままにし
て異なった曲率を持つ対象物へ倣い作業を行うと、曲率
の大小により対象物への押しつけ力と設定力との間に差
が生じる。

本発明の目的は未知形状の曲面を有する対象物体の表面
を倣い動作するときに、対象面の曲率によってロボット
の先端の移動速度指令を変化させて対象物体に加える力
を一定に保つための倣い制御装置の倣い速度制御方式を
提供することにある。

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

第１図は本発明の原理構成図である。本発明は、ロボッ
トの力制御時における倣い速度制御方式であって、−ロ
ボット（１）と対象物（１１）に作用する力を検出する
力検出部（６）と、ロボットの位置を検出する位置検出
部（５）と、前記位置検出部の位置座標値に基づきロボ
ットの位置を制御する位置制御部（３）と、前記力検出
部により検出した力に基づいてロボットの力を制御する
力制御部（４）と、ロボットヘカ・位置指令及びバラメ
−夕の転送を行うため、ロボットの対象物の接点の曲率
を算出する曲率算出部（７１）と、曲率によってロボッ
ト先端の移動速度を自動的に変化させる移動速度設定部
（７２）を有する制御指令生成部（７）と、ロボットと
対象物の接点の法線ベクトルを算出し、かつ倣い座標系
を算出する法線ベクトル算出部（８）と、ロボットの移
動方向ベクトルを算出する移動方向ベクトル算出部（９
）とを備え、前記算出した倣い座標系及び移動指令に基
づいて未知形状の曲面を持つ対象物の表面に、前記力制
御部により一定の力を加え、かつ自動的に移動速度を変
化させながら倣し）動作を行うようにしたことを特徴と
する。

〔作　用〕

以下に倣い動作の制御方法と、対象物の曲率によりロボ
ット先端の移動速度を変化させる速度制御方法について
詳細に説明する。

一υ（Ｌ欧見勤■イｌ唱」か汰 第３図は倣い座標系の説明図である。倣い座標系０８−
Ｘ　ｗ　Ｙ　ｗ　Ｚ　ｕは、対象物に対するロボット先
端部の位置・姿勢により決定される座標系で、第２図の
ように表される。ベクトルｎ、ｏ、ａはそれぞれ倣い座
標系の座標軸Ｘ。、ｙ、４　、ｚ、についての単位ベク
トルである。ｎは対象物へ力を加える時の押し付は方向
を示し、対象物の表面の法線ベクトルと同じ方向である
。０は、ｎと直交関係にあり、倣い動作時のロボット先
端の移動方向を示す。ａは、ｎ、ｏに直交するように定
められ、ａ＝ｎＸ。

で与えられる。

ｎ、ｏ、ａの基準座標系Ｘ。ＹｏＺ。

分表示を行うと、 ｎ　＝（ｎｘｎｙｎＪ” ０　＝（Ｏｘ　Ｏｙ　Ｏｇ）　’ についての成 となる。ただし、Ｔは転置行列を示す。

第４図は対象物とマニプレータの位置関係の説明図であ
る。ロボットは、対象物のある面上で倣い動作を行って
おり、このとき、倣い座標系は第４図の位置Ｐ１の様に
与えられているものとする。

図のように、ベクトルｎは面に垂直であり、ベクトルＯ
，ａは接平面上にある。ロボットが倣い動作を行ってベ
クトル０の方向へ移動すると、ロボットと対象物の接点
での面の法線方向が位置Ｐでの押しつけ方向ｎと一致し
なくなる（位置Ｐｚ）。

面の法線方向と押しつけ方向ｎが一致しないと、対象物
に対して発生する押しつけ力が設定した値からずれる。

従って、正確な力を発生するためには、位置Ｐ１の倣い
座標系を位置Ｐ２の倣い座標系に切り換える必要がある
。こうした倣い座標系を変更するか否かを判断するため
の条件として、（１）一定時間経過したとき、 （２）対象物から受ける反力が設定力からある程度以上
ずれたとき、 （３）ロボットが一定距離を移動したとき、等が挙げら
れる。尚、以下の説明では（１）の条件に従って倣い座
標を変更するものとする。また、（２）、（３）の場合
でも、基本的な原理は、（１）と同一である。

第５図は各点における倣い座標系の説明図であり、倣い
座標系とロボットの軌道がどのように変化するかを示し
たものである。第１図、第５図を用いて、曲面を有する
対象物へ倣い動作を行うための制御方法について説明す
る。

１　　い座・系の まず、第５図にある始点Ｐ、での倣い座標系の算出方法
について説明する。

（１）始点での法線ベクトルの算出 始点Ｐｇでの法線ベクトルｎｌｌを次のようにして求め
る。第６図（ａ）、（ｂ）は、ハンドが対象物から受け
る反力Ｆと、力覚センサ座標系０゜Ｘ、Ｙ、Ｚ、の関係
を示した図である。力覚センサでは、反力ＦのＸｓ　　
、Ｙｓ　　、Ｚｓ力方向それぞれの分力ｆＸ　、ｆ、、
ｆ、が検出される。（以下、力覚センサの検出するトル
ク成分についての説明は省略する。）反力３Ｆをベクト
ル表示すると、（Ｓは力覚センサ座標系Ｏｓ　　Ｘ５Ｙ
ｓＺｓで記述されていることを示す、） ’Ｆ＝　（ｆｘｒｙｒｚ）’　　　　　　　　　　　・
・・（１）となる。力覚センサ座標系で表した法線ベク
トルＳｎ、ば、！Ｆと逆向きのベクトルであり、成分表
示すると、 ・・・　（２） １３Ｆ１は、ベクトルの大きさ である。法線ベクトルＳｎｌを基準座標系で表してみる
。力覚センサ座標系の各座標軸Ｘｓ、Ｙｓ。

Ｚ、の基準座標系に対する単位ベクトルをＯｎ。

Ｇｏｓ　＋　　Ｏａ、とする（０は、基準座標系で記述
されていることを示す）。成分表示すると、である。こ
のとき、力覚センサ座標系から基準座標系への座標変換
行列０Ａ３は、 ０Ａｓ　＝　（０ｎｓ。ｏｓ。ａｓ＞　　　　　　−（
４）で与えられる。基準座標系で表した法線ベクトルＯ
ｎ、は、座標変換行列’　Ａ　３を用いて、’ｎ＋＋　
＝　ｏＡ、５ｎｌｌ・＋　（５）となる。法線ベクトル
は、第５図の法線ベクトル算出部で算出される。また、
始点以外の他の接触点での法線ベクトルの算出も同様の
操作で行われる。

（２）始点での移動方向ベクトルの算出部に、オペレー
タが与えた移動方向ベクトル０゜Ｐと、求めた法線ベク
トルＯｎ、を用いて、始点ＰＩｌにおける移動方向ベク
トル０．を算出する。ただし、ベクトルＯＯＦとベクト
ルＯｎ、は、００Ｐ−ＯｎＢまたは０ＯＰ＝　　’ｎｉ
　　−（６）を満たさないものとする。移動方向ベクト
ルＯ８は、法線ベクトルＯｎ、ｌに直交し、移動方向ベ
クトル０゜、と法線ベクトルＯｎＩ＋の成す平面上にあ
るベクトルである。このとき、倣い座標系の座標軸を表
す単位ベクトルの１つであるａ、は、０ｎｌｌとＱＯＦ
を用いて、 ’ａ　Ｂ　＝　（’ｎＨＸ　０ｏｒ）　／　ｌ　’ｎｓ
Ｘ　ＯＯＦ・・・　（７） で表される。移動方向ベクトル００．は、ベクトル０ｎ
Ｂ＋。ａ＠との直交関係により、ＧｏＩＩ＝　’ａ、Ｘ
’ｎ、　　　　　　　　　−（８）で求められる。移動
方向ベクトルは、第１図の移動方向ベクトル算出部で算
出される。

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

（３）点Ｐ、での法線ベクトルの算出 点Ｐ、での法線ベクトルｎ１の算出方法は、始点Ｐ、で
の法線ベクトルの算出方法と同様であり、次式で表され
る。ただし、点Ｐ、での反力３Ｆ、を、・・・　（９） ’ｎ（＝０Ａｓ＝Ｓｎ、　　　　　　　　　　　　　　
　−（１０）’　Ａ　Ｂ　ｉは、点Ｐ、の力覚センサ座
標系から基準座標系への座標変換行列である。

（４）点Ｐ、での移動方向ベクトルの算出ロボットの先
端位置は、ベクトルａ方間にも位置制御されているので
、先端位置が常にベクトルｎ、ｏが成す平面上にあるよ
うに制御される。したがって、対象物の表面に描く軌跡
は、始点で与えられたベクトルｎｌ＋ｏｌの成す平面上
の曲線となる。このことから、点Ｐ、での移動方向ベク
直に与えられればよいことがわかる。また、移動方向ベ
クトル０．は、法線ベクトルｎ、にも直交するので、ベ
クトルＯｏ　、は、 ００ｉ−（’ａＢｘ’ｎｔ）／ｌ　’ａＢＸ’ｎｔのよ
うに表される。

・・・　（１１） 倣い座標系の座標軸を表す単 Ｏｏ　、との直交関係により、 Ｏａ、＝　０ｎｉＸ００゜ （１２） で求められる。

２　　　　い　　　の　　′ 第５図にある始点Ｐ、での倣い動作の制御方法について
説明する。

（１）目標力Ｆ１の発生 始点Ｐ、で、対象物に対して設定した力Ｆ１を発生する
時は、力の大きさがＦ、、で、方向・向きがｎＢの力を
発生すればよい。従って、設定力ベクトルＯＦｒは、（
２）式で求めたＯｎＢを用いて、０Ｆｒ＝Ｆｒ”ｎＢ　
　　　　　　　　　　・　（１３）で与えられる。

（２）目標位置の設定 始点Ｐ、でのロボットの進行方向は、始点Ｐ、ｌからの
相対位置指令で与える。始点Ｐ８からの移動方向は、式
（９）の移動方向々クトル００１１で与られており、Ｏ
ｏ８を用いて相対位置を表すと、ＯＸＢ　＝α・００ｓ
　　　　　　　　　　　＋・＋　（１４）となる。αは
、２ページの倣い座標系の修正のための条件（１）〜（
３）から決まる適当な定数である。

目標力、目標位置の設定は第１図に示す制御指令生成部
で行う。

（３）倣い動作の終了 次のような条件を満たした時に倣い動作を終了する。

１）オペレータから終了の指令があったとき２）一定時
間を経過した時 ３）衝突したとき、または、対象物から離れた時 ４）ロボットの可動範囲を越えた時 代（１１）はＯａ　、−Ｏｎ　、またはＯａ　ｌ＝＝　
　Ｏｎ　。

のときには成立しない。このとき、点Ｐ、の移動方向ベ
クトルにオペレータが与えた移動方向ベクトル０゜、を
用いると進行方向が維持できる。

Ｂ　′−′ 前述のように、各サンプリング毎にロボットと対象物が
接触する点で倣い座標を設定して正しい制御方向を指示
すると、サンプリング毎のロボットと対象物との接触力
を一定に保つ事が可能になる。しかし、サンプリングが
行われないサンプリング点間で対象面が大きく変化する
と、ロボットと対象物との接触力が設定した力からずれ
てしまう。そこで、対象面が大きく変化する場合、つま
り、面の曲率が大きい時にはロボット先端の移動速度を
落として、接触力と設定力のずれが大きくならないよう
に制御する。

第７図は、点Ｐ、での曲率を算出する方法について示し
たものである。また、第８図は、ロボット先端の指令移
動速度の決定方法について示したものである。以下、第
１．７．８図を用いてロボットと対象物との接触力を一
定に保つための速度制御方法について説明する。

利土二典圭■ｌ■ 対象物体は未知形状なので、次のサンプリングが行われ
る点までの曲率を、現在地点Ｐ、と、１サンプリング前
の点Ｐ、−１の情報を用いて推定する。ただし、Ｐｉ、
Ｐｉ−、付近の対象物体の形状が、球で近似できると仮
定する。このとき、球の半径をｒとすると、曲率χは１
　／　ｒで与えられる。

第７図のように、点Ｐｉ、Ｐ、−，の位置ベクトルを、
　Ｐ　ｉ　　＋　　ＯＰ　ｉ−１、点Ｐ８の法線ベクト
ルをＯｎ　、とおいたときの点Ｐ、での曲率を求める。

点Ｐ、付近の形状は球で表すことができると仮定したの
で、曲率円の中心は法線ベクトルＯｎ　、と線分ｐ、、
ｐ、Ｉの垂直２等分線の交点となる。

点Ｐ、から曲率円の中心Ｒへ向かうベクトルは、ことが
できる（ｒ、は、点Ｐ、の曲率円の半径）。

第７図から次の関係が成り立つことがわかる。

（’Ｐ４−’Ｐｉ−＋）　／２　＝　。

従って、曲率円の半径ｒ１は、 ・・・　（１５） の内積を示す。

のように求められる。式（１６）から曲率にを求めとな
る。

２゛庁′法 第２図で示したように、次のサンプリング点へ移動する
まで法線方向への制御指令は変更されないので、次のサ
ンプリング点でのベクトルｎ方向への変位をδとすると
、設定力からにδ（ただし、ｋはロボットと対象物が成
すバネ系のバネ定数）だけ変化する。Ｐ　ｌ　　＊　Ｐ
　ｉ−１付近の対象物体の形状が球で近似できると仮定
して求めたサンプリング時間τ後に生ずる力の変位の推
定（Ｊｋδが常に許容範囲にある様に、移動ベクトル方
向の速度指令を制御する方法を示す。

今、力の変位の許容範囲をΔＦとする。ロボットと対象
物が成すバネ系のバス定数ｋが与えられると許容条件は
、 ｋδ≦ΔＦ　　　　　　　　　　　　・・・（１８）で
ある。バネ定数が不定のときは、許容範囲を力の変位で
はなく、ベクトルｎ方向への変位Δδで許容範囲を与え
る。このときの許容条件は、δ≦Δδ　　　　　　　　
　　　　　・・・（１９）である。第８図を用いて、上
記の条件を満たすような指令速度の算出方法を説明する
。図において、現在のロボットの先端位置は点Ｐ、であ
る。もし、点Ｐ１からベクトル。方向へ指令速度１ｖ＝
Ｉで進んだ場合、次のサンプリング時間τ後にロボット
の先端は、曲率円上の点Ｐ、。、′（次サンプリング時
の推定位置）に達する。ｐ、、、ｌへ移動したときに生
じるベクトルｎ方向の変位δは、式（１６）で求めたｒ
ｉを用いて、 δ＝　ｒｉ−７ｒｉ　−ｖ　ｒ　　　　　　　−（１９
）となる。弐（１８）（１９）から、変位δは、ｒ、７
７ゴτゴ丁”≦ΔＦ／に−（２０）あるいは、 ｒｔ、／−Ｙ７：ゴ■〒Ｐ−≦Δδ　　　・（２１）の
条件を満足しなければならない。上式を満たすためには
６、指令速度１ｖ、１を、 １ｖ、１≦　’ｉ　　　　ｒｉ　　　　　　　／ｒある
いは、 Ｖｉ　　ｌ≦　ｒｔ　−ｒ、　−／ｒ：　　・＝　（２
２）に設定すればよい。従って、指令速度子をベクトル
で表すと、式（２３）または（２４）のｌｖｔ＋を用い
て、 となる。

式（２３）で求めた指令速度Ｏｖ　、を用いて倣いを行
うと、サンプリングとサンプリングの間でも対象物とロ
ボット先端の間に働く接触力と設定力のずれが大きくな
らないように制御できる。

以上の計算は、第１図の位置検出部で検出したロボット
の先端位置、法線ベクトル算出部で算出した法線ベクト
ル、移動方向ベクトル算出部で算出した移動方向ベクト
ルを用いて、制御指令生成部内に設けられた曲率算出部
、及び移動速度設定部によって行う。

〔実施例］ 第９図は本発明の倣い制御装置の一実施例ブロック構成
図である。制御装置はマニプレータ２１の制御を行う操
作部２２を備えている。この操作部２２はサーボ・モー
タ２２ａと、パワー・アンプ２２ｂと、Ｄ／Ａコンバー
タ２２ｃと、補償器２２ｄとを有している。

また、制御装置は、マニプレータ２１の図示しないハン
ド部の先端位置の検出を行う位置検出部２６を備えてお
り、この位置検出部２６はカウンタ及びエンコーダ２６
ａとタコ・メータ２６ｂとを有している。

更に、制御装置は、マニプレータ２Ｉのハンド部が受け
る力の検出を行う力検出部２３を備えている。

この力検出部２３は上記と同様の力覚センサ２３ａとハ
ンド部座標系からロボット基準座標系への座標変換部２
３ｂとを有している。

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

第１０図は位置制御部と力制御部の構成図である。

この位置制御部２７の具体的構成は、転置直交変換行列
（Ｒ７）演算部３１と、選択行列Ｎ−５ｒ）演算部３２
と、直交行列（Ｒ）演算部３３と、位置フィ−ドパツク
ゲイン（Ｃ７）演算部３４とを有する。

一方、力制御部２４は転置直交変換行列（Ｒ”）演算部
３８と、選択行列（Ｓｒ）演算部３７と、直交行列（Ｒ
）演算部３６と、カフィードバックゲイン（ｃ　ｒ）演
算部３５とを有する。尚、符号２４ａ及び２７ａは偏差
部である。

ベクトルｎ、ｏ、ａを用いてロボット基準座標系（Ｘ、
、Ｙｏ、Ｚｏ）から倣い座標系（Ｘ、ＩＹｔ４２ｔ＝）
への座標変換する直交座標変換行列Ｒは、次のように表
される。

で与えられる。

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

また、位置フィードバックゲインＣ４は同様にして、 第３図のように、倣い座標系（Ｘ　ｗ　Ｙ　ｗ　Ｚ　ｕ
　）のＸｌ、１方向を力制御方向、Ｙい、Ｚい方向を位
置制御方向とすると、選択行列演算部３２　、３７の選
択行列Ｓｆは、 ・・・　（２４） ・・・　（２６） で与えられる。

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

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

ホストコンピュータ４０は、（１）目標位置Ｘｒ、力指
令Ｆ、の制御指令、（２）倣い座標系の切り換えに伴う
位置制御、力制御パラメータの送信、（３）法線ベクト
ル算出部、移動方向ヘクトル算出部の状態制御の信号の
発生を行う制御指令生成部４０ａと、マニプレータと対
象物の接触点の法線ベクトルを算出する法線ベクトル算
出部４０ｂと、マニプレータと対象物の接触点での移動
方向ベクトルを算出する移動方向ベクトル算出部４０ｃ
を有し、座標変換部２０で変換されたロボットの先端位
置、法線ヘクトル算出部４０ｂで算出した法線ベクトル
、移動方向ベクトル算出部４０ｃで算出した移動方向ベ
クトルを用いて、現在のロボットの先端位置の曲率を算
出する曲率算出部４０ｅ、及び倣いを行う際のコントロ
ーラへの移動速度指令を算出する移動速度設定部４０ｆ
が制御指令生成部４０ａ内に設けられている。また、制
御指令生成部には、オペレータにより倣い動作のための
パラメータが設定される。

第１１図は本発明システムの構成図である。ホストコン
ピュータ４０は、制御指令生成部４０ａ、法線ベクトル
算出部４０ｂ、移動方向ヘクトル算出部４０ｃ、通信制
御部４０ｄ、曲率算出部４０ｅ、移動速度設定部４０ｆ
、メモＵ４０ｇを有する。ロボットコントローラ１０は
、メモリ１０ａ、通信制御部Ｊｏｂと、制御部、座標変
換部、偏差部等を有し、操作部２２、位置検出部２６を
介してマニプレータ２１の制御を行う。ホストコンピュ
ータと、ロボットコントローラは、ハス等の通信インタ
ーフェースによって接続されており、それぞれの信号の
送・受信のタイミングを管理する通信制御部１０ｂ、４
０ｄによって、メモ１月Ｏａとメモリ４０ｇ間でデータ
が転送される。

制御生成指令部、法線ヘクトル算出部、移動方向ベクト
ル算出部、曲率算出部、移動速度設定部の計算で必要な
データは、メモリ上のデータが参照される。

システムを第１１図のように構成した時に、曲面を有す
る対象物に対して倣い動作を行う場合の処理の流れを第
１２図に示す。

第１２図は本発明の処理手順図であり、ホストコンピュ
ータ内の制御指令生成部、法線ベクトル算出部、移動方
向ヘクトル算出部、曲率算出部、移動速度設定部、ロボ
ットコントローラで行われる処理の流れを示したもので
ある。尚、倣い動作のためのパラメータはオペレータに
よって設定されるものとする。また、図中添字Ｂは始点
を、添字ｉは第１番目でのロボットと対象物との接触点
を表す。

まず、オペレータは移動方向ベクトル００ｏ４、設定力
Ｆｒ、適当な値を持つ倣い座標系の切り換え時間τ、相
対位置を指定する係数α、始点で法線ベクトルを算出す
るために発生する力ベクトルＦＢ、始点の移動速度指令
Ｏｖ’　　を設定する。制御指令生成部では、始点で適
当な力Ｆｌｌを発生するために力指令Ｆ、及び力制御パ
ラメータを生成し、ロボットコントローラへ転送する。

ロボットコントローラは、指令に基づいて対象物に押し
つけ動作を行う。このとき、検出した反力が０のとき（
ロボットと対象物が離れた時）は、再度力ベクトルＦ３
の設定を行う。

次に、コントローラの位置検出部で検出したロボットの
先端位置ｏｐ８をメモリに記憶する。さらに、法線ベク
トル算出部において、検出した反力Ｆから（２）式を用
いて、Ｓｎ８を求める。また、ヤコビ行列で力覚センサ
座標系を計算し、（４）式から座標変換行列０ＡＳを算
出する。算出した０Ａ３を用いて、法線ベクトルＯｎＢ
を求め、制御指令生成部と移動方向ベクトル算出部へＯ
ｎｌ＋を転送する。

ここで、法線ベクトルＯｎＢと移動方向ベクトル００ｏ
、の方向が一致した場合は、再度パラメータの設定を行
う。

移動方向ベクトル算出部では、（７）式を用いてヘクト
ルＯａ！＋を算出し、Ｏｎ８とＯａＢから移動方向ベク
トル００．を求める（（８）式）。

制御指令生成部は、式（１３）、　（１４）と法線ベク
トルｕｎＢ、斗多動万同ヘクトルＵｏＢから、目標力ヘ
クトル０Ｆ１、目標相対位置ベクトルＯＸｒを生成し、
ロボットコントローラへ転送する。始点ｏｐＢでは、曲
率を算出することができないので、あらかじめオペレー
タによって与えられた移動速度指令Ｏｖｏを用いる。ロ
ボットコントローラは、制御指令生成部から速度指令Ｏ
ｖ０を受けると倣い動作を開始する。

一定時間τが経過するとロボットの可動範囲にあるかど
うかを調べ、可動範囲内であれば、第１番目の接触点で
の反力Ｆを検出する。

第１番目の接触点でも、目標相対位置ベクトル０マ、を
生成し、ロボ・ントコントローラへ転送するまで始点と
同様の操作で、法線ベクトル算出部で法線ベクトルを算
出し、移動方向ベクトル算出部で移動方向ベクトルを算
出する。制御指令生成部は、法線ベクトル０ｎ９、移動
方向ベクトル°τ１から、目標力ヘクトル０Ｆｒ、目標
相対位置ベクトル０Ｘｒを生成し、ロボットコントロー
ラへ転送する。曲率算出部では、メモリに記憶しである
法線ベクトル０ｎＢ、前回のロボ・ント先端の位置０Ｐ
Ｂ、現在のロボット先端の位Ｊ’Ｐ、。

から曲率円の半径ｒ１を式（１７）によって算出する。

移動速度設定部では、算出した曲率円の半径ｒ７、オペ
レータによって設定された変位の許容範囲Δδ、サンプ
リング時間τから、移動指令速度Ｏｖ１を決定する。ロ
ボットコントローラは、制御指令生成部から速度指令Ｏ
ｖ１を受けると倣い動作を開始する。

一定時間τが経過するとオペレータから終了指令が有る
かどうかを調べ、指令があれば倣い動作を停止し、次の
動作を行う。指令がなければ、反力Ｆから第２番目の接
触点の法線ベクトルＳｎ２を計算する。以上の操作を、
オペレータが終了指令を出すまで繰り返す。

［発明の効果］ 以上説明したように、本発明によれば、曲面を有する対
象物へ対する倣い動作を自動的に行うことができ、従来
ロボットの操作者が行っていた対象物体の変更や対象物
の位置ずれに伴うロポ・ントへの教示を行う必要がなく
なり操作者の負担が軽減される。また、対象面の曲率に
よってロボット先端の移動速度が自動的に変化するため
、対象物への目標力と対象物へ発生する力のずれを一定
に保って倣い動作を行うことができる。

【図面の簡単な説明】

第１図は本発明の制御系の原理構成図、第２図は接触力
のずれの説明図、 第３図は倣い座標系の説明図、 第４図は対象物とマニプレータの位置関係説明図、 第５図は各点における倣い座標系の説明図、第６図（ａ
）、（ｂ）は設定力の算出方法の説明図、 第７図は曲率の算出方法の説明図、 第８図は移動速度指令の算出方法の説明図、第９図は本
発明の一実施例構成図、 第１０図は位置制御部及び力制御部の構成図、第１１図
は本発明のシステム構成図、及び第１２図（ａ）〜（ｃ
）は本発明の処理手順図、（符号の説明） １・・・制御対象、 ３・・・位置制御部、 ５・・・位置検出部、 ７・・・制御指令生成部、 ８・・・法線ベクトル算出部、 ９・・・移動方向ベクトル算出部、 ７１・・・曲率算出部、　　７２・・・移動速度設定部
。 ２・・・操作部、 ４・・・力制御部、 ６・・・力検出部、

Claims (1)

1. 【特許請求の範囲】 １、ロボットの力制御時における倣い速度制御方式にお
   いて、 ロボット（１）と対象物（１１）に作用する力を検出す
   る力検出部（６）と、 ロボットの位置を検出する位置検出部（５）と、前記位
   置検出部の位置座標値に基づきロボットの位置を制御す
   る位置制御部（３）と、 前記力検出部により検出した力に基づいてロボットの力
   を制御する力制御部（４）と、 ロボットへ力・位置指令及びパラメータの転送を行うた
   め、ロボットの対象物の接点の曲率を算出する曲率算出
   部（７１）と、曲率によってロボット先端の移動速度を
   自動的に変化させる移動速度設定部（７２）を有する制
   御指令生成部（７）と、ロボットと対象物の接点の法線
   ベクトルを算出し、かつ倣い座標系を算出する法線ベク
   トル算出部（８）と、 ロボットの移動方向ベクトルを算出する移動方向ベクト
   ル算出部（９）とを備え、 前記算出した倣い座標系及び移動指令に基づいて未知形
   状の曲面を持つ対象物の表面に、前記力制御部により一
   定の力を加え、かつ自動的に移動速度を変化させながら
   倣い動作を行うようにしたことを特徴とするロボットの
   力制御時における倣い速度制御方式。