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

Description

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

〔産業上の利用分野〕

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

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

〔従来の技術と発明が解決しようとする課題〕

ロボットの力制御時に未知形状の曲面を有する対象物
体に対して倣い作業を行う際に、ロボットの先端と対象
物の接触点における対象物表面の法線方向に一致する押
しつける方向と、倣い作業をしながら移動する移動方
向で決定される倣い座標（O_W−X_WY_WZ_W）を対象物の形
状に合わせて逐次ロボットコントローラに与える必要が
ある。

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

ロボットと対象物がなすバネ系のバネ定数をｋとする
と、点P_A，_i+1、及び点P_B，_i+1で対象物に作用するベク
トルｎ方向の力はそれぞれ、Ｆ−ｋδ_Ａ及びＦ−ｋδ_Ｂ
となる。δ_Ａ＞δ_Ｂなので、本図に示すように、面の曲
率が大きいと、設定力と対象物への押しつけ力とのずれ
も大きくなる。このように、ロボット先端の移動速度を
一定のままにして異なった曲率を持つ対象物へ倣い作業
を行うと、曲率の大小により対象物への押しつけ力と設
定力との間に差が生じる。

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

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

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

〔作用〕

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

〔Ａ〕倣い動作の制御方法 第３図は倣い座標系の説明図である。倣い座標系O_W−
X_WY_WZ_Wは、対象物に対するロボット先端部の位置・姿勢
により決定される座標系で、第２図のように表される。
ベクトル，，はそれぞれ倣い座標系の座標軸X_W，
Y_W，Z_Wについての単位ベクトルである。は対象物へ力
を加える時の押し付け方向を示し、対象物の表面の法線
ベクトルと同じ方向である。は、と直交関係にあ
り、倣い動作時のロボット先端の移動方向を示す。
は、，に直交するように定められ、 ＝× で与えられる。

，，の基準座標系X₀Y₀Z₀についての成分表示を行
うと、 となる。ただし、Ｔは転置行列を示す。

第４図は対象物とマニプレータの位置関係の説明図で
ある。ロボットは、対象物のある面上で倣い動作を行っ
ており、このとき、倣い座標系は第４図の位置P₁の様に
与えられているものとする。図のように、ベクトルは
面に垂直であり、ベクトル，は接平面上にある。ロ
ボットが倣い動作を行ってベクトルの方向へ移動する
と、ロボットと対象物の接点での面の法線方向が位置P₁
での押しつけ方向と一致しなくなる（位置P₂）。面の
法線方向と押しつけ方向が一致しないと、対象物に対
して発生する押しつけ力が設定した値からずれる。従っ
て、正確な力を発生するためには、位置P₁の倣い座標系
を位置P₂の倣い座標系に切り換える必要がある。こうし
た倣い座標系を変更するか否かを判断するための条件と
して、 （１）一定時間経過したとき、 （２）対象物から受ける反力が設定力からある程度以上
ずれたとき、 （３）ロボットが一定距離を移動したとき、 等が挙げられる。尚、以下の説明では（１）の条件に従
って倣き座標を変更するものとする。また、（２），
（３）の場合でも、基本的な原理は、（１）と同一であ
る。

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

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

（１）始点での法線ベクトルの算出 始点P_Bでの法線ベクトル_Ｂを次のようにして求め
る。第６図（ａ），（ｂ）は、ハンドが対象物から受け
る反力と、力覚センサ座標系O_S−X_SY_SZ_Sの関係を示し
た図である。力覚センサでは、反力のX_S，Y_S，Z_S方向
のそれぞれの分力f_X，f_Y，f_Zが検出される。（以下、力
覚センの検出するトルク成分についての説明は省略す
る。）反力^Ｓをベクトル表示すると、（Ｓは力覚セン
サ座標系O_S−X_SY_SZ_Sで記述されていることを示す。）^Ｓ ＝(f_Xf_Yf_Z)^T …（１） となる。力覚センサ座標系で表した法線ベクトル^Ｓ _Ｓ は、^Ｓと逆向きのベクトルであり、成分表示す
ると、 ｜^Ｓ｜は、ベクトルの大きさ である。法線ベクトル^Ｓ _Ｂを基準座標系で表してみ
る。力覚センサ座標系の各座標軸X_S，Y_S，Z_Sの基準座標
系に対する単位ベクトルを^Ｏｎ，^Ｏ _Ｓ，^Ｏ _Ｓとす
る（Ｏは、基準座標系で記述されていることを示す）。
成分表示すると、 である。このとき、力覚センサ座標系から基準座標系ヘ
の座標変換行列^OA_Sは、^O A_S＝（^On_S ^Oo_S ^Oa_S） …（４） で与えられる。基準座標系で表した法線ベクトル^Ｏ _Ｂ
は、座標変換行列^OA_Sを用いて、^Ｏ _Ｂ ＝^OA_S ^Ｓ _Ｂ …（５） となる。法線ベクトルは、第５図の法線ベクトル算出部
で算出される。また、始点以外の他の接触点での法線ベ
クトルの算出も同様の操作で行われる。

（２）始点での移動方向ベクトルの算出 次に、オペレータが与えた移動方向ベクトル_OPと、
求めた法線ベクトル^Ｏ _Ｂを用いて、始点P_Bにおける移
動方向ベクトル_Ｂを算出する。ただし、ベクトル_OP
とベクトル^Ｏ _Ｂは、_OP ＝^Ｏ _Ｂまたは_OP＝−^Ｏ _Ｂ …（６） を満たさないものとする。移動方向ベクトル_Ｂは、法
線ベクトル^Ｏ _Ｂに直交し、移動方向ベクトル_OPと法
線ベクトル^Ｏ _Ｂの成す平面上にあるベクトルである。
このとき、倣い座標系の座標軸を表す単位ベクトルの１
つである_Ｂは、^Ｏ _Ｂと_OPを用いて、^Ｏ _Ｂ ＝（^Ｏ _Ｂ×_OP）/|^Ｏ _Ｂ×_OP｜ …（７） で表される。移動方向ベクトル^Ｏ _Ｂは、ベクトル^Ｏ
_Ｂ，^Ｏ _Ｂとの直交関係により、^Ｏ _Ｂ ＝^Ｏ _Ｂ×^Ｏ _Ｂ …（８） で求められる。移動方向ベクトルは、第１図の移動方向
ベクトル算出部で算出される。

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

（３）点P_iでの法線ベクトルの算出 点P_iでの法線ベクトル_ｉの算出方法は、始点P_Bでの
法線ベクトルの算出方法と同様であり、次式で表され
る。ただし、点P_iでの反力^Ｓ _ｉを、^Ｓ _ｉ ＝(f_Xif_Yif_Zi)^T とする。

^OA_Siは、点P_iの力覚センサ座標系から基準座標系への座
標変換行列である。

（４）点P_iでの移動方向ベクトルの算出 ロボットの先端位置は、ベクトル方向にも位置制御
されているので、先端位置が常にベクトル，が成す
平面上にあるように制御される。したがって、対象物の
表面に描く軌跡は、始点で与えられたベクトル_Ｂ，
_Ｂの成す平面上の曲線となる。このことから、点P_iでの
移動方向ベクトル_ｉは、始点で求められたベクトル
_Ｂに垂直に与えられればよいことがわかる。また、移動
方向ベクトル_ｉは、法線ベクトル_ｉにも直交するの
で、ベクトル^Ｏ _ｉは、^Ｏ _ｉ ＝（^Ｏ _Ｂ×^Ｏ _ｉ）/|^Ｏ _Ｂ×^Ｏ _ｉ｜ …（11） のように表される。倣い座標系の座標軸を表す単位ベク
トルの１つである_ｉは、ベクトル^Ｏ _ｉ，^Ｏ _ｉとの
直交関係により、^Ｏ _ｉ ＝^Ｏ _ｉ×^Ｏ _ｉ …（12） で求められる。

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

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

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

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

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

１）オペレータから終了の指令があったとき ２）一定時間を経過した時 ３）衝突したとき、または、対象物から離れた時 ４）ロボットの可動範囲を越えた時 式（11）は^Ｏ _Ｂ＝^Ｏ _ｉまたは^Ｏ _Ｂ＝−^Ｏ _ｉの
ときには成立しない。このとき、点P_iの移動方向ベクト
ルにオペレータが与えた移動方向ベクトル_OPを用いる
と進行方向が維持できる。

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

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

〔１〕曲率の算出 対象物体は未知形状なので、次のサンプリングが行わ
れる点までの曲率を、現在地点P_iと、１サンプリング前
の点P_i-1の情報を用いて推定する。ただし、P_i，P_i-1付
近の対象物体の形状が、球で近似できると仮定する。こ
のとき、球の半径をｒとすると、曲率κは1/rで与えら
れる。

第７図のように、点P_i，P_i-1の位置ベクトルを、^Ｏ
_ｉ，^Ｏ _i-1、点P_iの法線ベクトルを^Ｏ _ｉとおいたと
きの点P_iでの曲率を求める。点P_i付近の形状は球で表す
ことができると仮定したので、曲率円の中心は法線ベク
トル^Ｏ _ｉと線分P_i，P_i-1の垂直２等分線の交点とな
る。点P_iから曲率円の中心Ｒへ向かうベクトルは、法線
ベクトル^Ｏ _ｉを用いて、r_i ^O _ｉと表すことができる
（r_iは、点P_iの曲率円の半径）。第７図から次の関係が
成り立つことがわかる。

｛r_i ^O _ｉ−（^Ｏ _ｉ−^Ｏ _i-1）/2｝・（^Ｏ _ｉ−^Ｏ
_i-1）/2＝０ …（15） 従って、曲率円の半径r_iは、 のように求められる。式（16）から曲率κを求めると、 となる。

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

今、力の変位の許容範囲をΔＦとする。ロボットと対
象物が成すバネ系のバス定数ｋが与えられると許容条件
は、 ｋδ≦ΔＦ …（18） である。バネ定数が不定のときは、許容範囲を力の変位
ではなく、ベクトルｎ方向への変位Δδで許容範囲を与
える。このときの許容条件は、 δ≦Δδ …（19） である。第８図を用いて、上記の条件を満たすような指
令速度の算出方法を説明する。図において、現在のロボ
ットの先端位置は点P_iである。もし、点P₁からベクトル
方向へ指令速度｜_ｉ｜で進んだ場合、次のサンプリ
ング時間τ後にロボットの先端は、曲率円上の点P_i-1′
（次サンプリング時の推定位置）に達する。P_i-1′へ移
動したときに生じるベクトル方向の変位δは、式（1
6）で求めたr_iを用いて、 となる。式（18）（19）から、変位δは、 あるいは、 の条件を満足しなければならない。上式を満たすために
は、指令速度｜_ｉ｜を、 あるいは、 に設定すればよい。従って、指令速度をベクトルで表
すと、式（23）または（24）の｜_ｉ｜を用いて、^Ｏ _ｉ ＝｜_ｉ｜^Ｏ _ｉ …（23） となる。

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

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

〔実施例〕

第９図は本発明の倣い制御装置の一実施例ブロック構
成図である。制御装置はマニプレータ21の制御を行う操
作部22を備えている。この操作部22はサーボ・モータ22
aと、パワー・アンプ22bと、D/Aコンバータ22cと、補償
器22dとを有している。

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

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

更にまた、制御装置は倣い動作時に、力検出部23によ
り検出された力_ｏ、設定力（力指令F_r）及び力制御パ
ラメータに基づいて力制御方向の速度指令信号V_fを発す
る力制御部24と、位置検出部26に検出された位置X_O、目
標位置X_r及び位置パラメータに基づいて位置制御方向の
速度指令信号V_pを発する位置制御部27とを備えている。

第10図は位置制御部と力制御部の構成図である。この
位置制御部27の具体的構成は、転置直交変換行列（R^T）
演算部31と、選択行列（Ｉ−S_f）演算部32と、直交行列
（Ｒ）演算部33と、位置フィードバックゲイン（c_P）演
算部34とを有する。一方、力制御部24は転置直交変換行
列（R_T）演算部38と、選択行列（S_f）演算部37と、直交
行列（Ｒ）演算部36と、力フィードバックゲイン（c_f）
演算部35とを有する。尚、符号24a及び27aは偏差部であ
る。

ベクトル，，を用いてロボット基準座標系
（X_O，Y_O，Z_O）から倣い座標系（X_WY_WZ_W）への座標変換
する直交座標変換行列Ｒは、次のように表される。

第３図のように、倣い座標系（X_WY_WZ_W）のX_W方向を力
制御方向、Y_W，Z_W方向を位置制御方向とすると、選択行
列演算部32,37の選択行列S_fは、 で与えられる。

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

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

更に、制御装置は、力制御部24及び前記位置制御部27
から出力された速度についての加算を行う加算部30b
と、加算された速度をマニプレータ21の各関節の角速度
θに変換する逆ヤコビ変換部30aとを有している。

座標変換部20では、位置検出部26で検出されたマニプ
レータの関節角θ_Ｓを基準座標系での位置X_Oに変換す
る。

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

第11図は本発明システムの構成図である。ホストコン
ピュータ40は、制御指令生成部40a、法線ベクトル算出
部40b、移動方向ベクトル算出部40c、通信制御部40d、
曲率算出部40e、移動速度設定部40f、メモリ40gを有す
る。ロボットコントローラ10は、メモリ10a、通信制御
部10bと、制御部、座標変換部、偏差部等を有し、操作
部22、位置検出部26を介してマニプレータ21の制御を行
う。ホストコンピュータと、ロボットコントローラは、
バス等の通信インターフェースによって接続されてお
り、それぞれの信号の送・受信のタイミングを管理する
通信制御部10b,40dによって、メモリ10aとメモリ40g間
でデータが転送れる。制御生成指令部、法線ベクトル算
出部、移動方向ベクトル算出部、曲率算出部、移動速度
設定部の計算で必要なデータは、メモリ上のデータが参
照される。

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

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

まず、オペレータは移動方向ベクトル^Ｏ _OP、設定力
F_r、適当な値を持つ倣い座標系の切り換え時間τ、相対
位置を指定する係数α、始点で法線ベクトルを算出する
ために発生する力ベクトル_Ｂ、始点の移動速度指令^Ｏ
_Ｏを設定する。制御指令生成部では、始点で適当な力
_Ｂを発生するために力指令_Ｂ及び力制御パラメータ
を生成し、ロボットコントローラへ転送する。ロボット
コントローラは、指令に基づいて対象物に押しつけ動作
を行う。このとき、検出した反力が０のとき（ロボット
と対象物が離れた時）は、再度力ベクトル_Ｂの設定を
行う。

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

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

移動方向ベクトル算出部では、（７）式を用いてベク
トル^Ｏ _Ｂを算出し、^Ｏ _Ｂと^Ｏ _Ｂから移動方向ベク
トル^Ｏ _Ｂを求める（（８）式）。

制御指令生成部は、式（13），（14）と法線ベクトル
^Ｏ _Ｂ、移動方向ベクトル^Ｏ _Ｂから、目標力ベクトル
^Ｏ _ｒ、目標相対位置ベクトル^Ｏ _ｒを生成し、ロボッ
トコントローラへ転送する。始点^OP_Bでは、曲率を算出
することができないので、あらかじめオペレータによっ
て与えられた移動速度指令^Ｏ _Ｏを用いる。ロボットコ
ントローラは、制御指令生成部から速度指令^Ｏ _Ｏを受
けると倣い動作を開始する。

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

第１番目の接触点でも、目標相対位置ベクトル^Ｏ _ｒ
を生成し、ロボットコントローラへ転送するまで始点と
同様の操作で、法線ベクトル算出部で法線ベクトルを算
出し、移動方向ベクトル算出部で移動方向ベクトルを算
出する。制御指令生成部は、法線ベクトル^Ｏ _１、移動
方向ベクトル^Ｏ _１から、目標力ベクトル^Ｏ _ｒ、目標
相対位置ベクトル^Ｏ _ｒを生成し、ロボットコントロー
ラへ転送する。曲率算出部では、メモリに記憶してある
法線ベクトル^Ｏ _Ｂ、前回のロボット先端の位置^OP_B、
現在のロボット先端の位置^OP₁から曲率円の半径r₁を式
（17）によって算出する。移動速度設定部では、算出し
た曲率円形の半径r₁、オペレータによって設定された変
位の許容範囲Δδ、サンプリング時間τから、移動指令
速度^Ｏ _１を決定する。ロボットコントローラは、制御
指令生成部から速度指令^Ｏ _１を受けると倣い動作を開
始する。

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

〔発明の効果〕

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

【図面の簡単な説明】

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

Claims (1)

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