JP2832903B2 - 倣い制御の座標切替方式 - Google Patents

Description

【発明の詳細な説明】 〔概要〕 未知形状の曲面を有する対象物体上を倣う場合に、対
象面の曲率によって倣い座標系を変更する時間間隔を変
化させる倣い制御の座標切替方式に関し、 未知形状の曲面を有する対象物体上を倣う場合に対象
物体表面の曲率によって倣い座標系の算出と再設定を行
う時間間隔を変化させて制御装置の負荷を軽減し、かつ
対象物体に加えられる力を一定に保つことを目的とし、 ロボットに対する力・位置指令およびパラメータ転送
を行う制御指令生成部と、該ロボットと作業対象物との
間に働く力を検出する力検出部と、該ロボットの位置を
検出する位置検出部と、該位置検出部によって検出され
た該ロボットの現在位置と前記制御指令生成部の指令と
に基づいてロボットの位置を制御する位置制御部と、前
記力検出部によって検出された力と前記制御指令生成部
の指令とに基づいてロボットの力を制御する力制御部と
を有し、ロボットの先端部の作業対象物に対する位置・
姿勢により決定される倣い座標系とロボットへの移動指
令に基づいて対象物表面に一定の力を加えながら倣い動
作を行う力制御ロボットにおいて、前記力検出部と位置
検出部との出力により、前記ロボットの先端と作業対象
物との接触点における対象物表面の法線方向の単位ベク
トルと、該ロボットの先端の移動方向の単位ベクトルと
で決定される倣い座標系を設定する倣い座標系設定手段
と、該倣い座標系設定手段と前記位置検出部の出力によ
り、該ロボットと対象物との接触点における未知形状対
象物表面の曲率を算出する曲率算出手段と、該曲率算出
手段の算出結果に応じて、前記倣い座標系の切替時間を
求め、該切替時間経過後に前記倣い座標系設定手段に、
制御指令生成部を介して新しい倣い座標系設定を指令す
る座標系切替指令手段とを備えるように構成する。

〔産業上の利用分野〕

本発明は力制御ロボットで作業対象物の表面に沿って
加工等を行う倣い作業のための倣い制御方式に係り、さ
らに詳しくは未知形状の曲面を有する対象物体上を倣う
場合に、対象面の曲率によって倣い座標系を変更する時
間間隔を変化させる倣い制御の座標切替方式に関する。

〔従来の技術〕

力制御ロボットでロボットの先端を作業対象物の表面
上に一定の力で押付けながら、作業開始点から終了点ま
でロボットの先端を移動させる倣い作業を行う際には、
ロボットの先端と対象物との接触点における対象物表面
の法線方向に一致する押付け方向ｎと、倣いをしながら
移動する移動方向ｏで決定される倣い座標系O_W−X_W,Y_W,
Z_Wをロボットのコントローラに与える必要がある。

第11図は倣い座標系の説明図である。同図において、
例えばロボットの本体底面の中心Ｏを基準座標系の原点
１とし、基準座標軸X₀,Y₀,およびZ₀がとられる。そして
座標軸Z₀上に関節２と３が、また関節３と４の間のアー
ムを含むマニプレータ５が、関節４の先の関節６とハン
ド７の間に力覚センサ８が設けられ、ハンド７が対象物
９に接した状態が示されている。ハンド７と対象物９と
の接触点が倣い座標系の原点O_W、接触点における対象物
の内向き法線方向が座標軸X_W、ハンドの移動方向がY_W、
２つの座標軸X_W,Y_Wに垂直な方向が座標軸Z_Wとなり、各
座標軸方向の単位ベクトルがそれぞれ、、および
で与えられる。

このように倣い座標系O_W−X_W,Y_W、Z_Wは対象物に対す
るロボット先端部（ハンド）の位置、および姿勢により
決定される座標系であって、単位ベクトルは対象物へ
力を加える場合の押付け方向を示し、倣い動作時のロボ
ット先端の移動方向を示す単位ベクトルはと直交関
係にあり、もう１つの単位ベクトルはとの外積と
して次式で与えられる。

＝× ここで３つの単位ベクトル、、およびは基準座
標系Ｏ−X₀,Y₀,Z₀で表示することも可能であり、一般に
次のような式で与えられる。ただしＴは転置行列を示し
ている。

第12図は曲面を有する対象物と倣い座標系の関係であ
る。ロボットは対象物９の側面で倣い動作を行っており
最初に倣い座標系は位置P₁を原点として与えられている
ものとする。この時ベクトルは対象物９の側面に垂直
であり、ベクトルとは接平面上にある。ロボットが
倣い動作を行ってベクトルの方向へ移動すると、点P₂
においては面の法線方向と押付け方向が一致しなくな
る。こうなると対象物に対してロボットが発生する押付
け力が設定した値からずれるために、正確な力で倣い動
作を行うためには、点P₁で設定された倣い座標系を点P₂
での倣い座標系に切り替える必要がある。

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

上述のように力制御ロボットで倣い制御を行うために
は、倣い座標系をロボットコントローラに与える必要が
あるが、従来この座標系はオペレータによるロボットへ
の教示によって設定されることが一般的であり、この場
合には作業対象物が第12図のように曲面を有する場合に
は教示が煩雑になるという問題点があった。

作業座標系をロボットコントローラに与える別の方法
として、ロボットの現在位置の情報を利用して移動方向
を推定する方法もあったが、この場合には移動方向ベク
トルが推定値となるために実際の移動位置とずれが生じ
るという問題点があった。

さらに作業対象物が曲面を有する場合には、例えばあ
る一定時間毎に倣い座標系を設定し直しながら倣い作業
を行うことも考えられるが、この方法では、例えば平面
を倣っている場合など倣い座標系を設定し直す必要が無
い場合でも座標系の切り替えが行われるために、制御装
置に常に計算の負荷がかかるという問題点があった。

本発明は、未知形状の曲面を有する対象物体上を倣う
場合に対象物体表面の曲率によって倣い座標系の算出と
再設定を行う時間間隔を変化させて制御装置の負荷を軽
減し、かつ対象物体に加えられる力を一定に保つことを
目的とする。

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

第１図は本発明の原理ブロック図である。同図はロボ
ット10に対する力・位置指令を与え、また力と位置を制
御するための各種パラメータの転送を行う制御指令生成
部11と、ロボット10と作業対象物との間に働く力を検出
する力検出部12と、ロボット10の位置を検出する位置検
出部13と、位置検出部13によって検出されたロボット10
の現在位置と制御指令生成部11の指令に基づいてロボッ
ト10の位置を制御する位置制御部14と、力検出部12によ
って検出された力と制御指令生成部11の指令に基づいて
ロボット10の力を制御する力制御部15とを有し、ロボッ
ト10の先端部の対象物に対する位置・姿勢により決定さ
れる倣い座標系とロボット10への移動指令に基づいて、
対象物の表面に一定の力を加えながら倣い作業を行う力
制御ロボットにおける倣い制御の座標切替方式の原理ブ
ロック図である。

第１図において、倣い座標系設定手段16はロボット10
の先端と作業対象物との接触点における対象物表面の法
線方向の単位ベクトルｎを、例えば倣い座標系のX_W軸、
ロボット10の先端の移動方向単位ベクトルをY_W軸、ま
たととの外積×＝をZ_W軸の単位ベクトルとす
る倣い座標系を設定する。倣い座標系設定手段16には力
検出部12と位置検出部13との出力が入力され、設定され
た倣い座標系は制御指令生成部11に出力される。

曲率算出手段17は位置検出部13の出力結果、例えば現
在のロボット先端位置と前回の倣い座標系設定時のロボ
ット先端位置、および倣い座標系設定手段16の出力によ
って、ロボット10と作業対象物との接触点における未知
形状の対象物表面の曲率を算出する。

座標系切替指令手段18は曲率算出手段17の曲率算出結
果に応じて、例えばメモリに格納されている曲率を座標
系切替時間との対応テーブル、または切替時間と曲率と
の関数関係に基づいて倣い座標系の切替時間を求め、そ
の切替時間経過後に制御指令生成部11を介して倣い座標
系設定手段16に新しい倣い座標系の設定を指令する。

〔作用〕

本発明においては、まず倣い作業の開始点において設
定される倣い座標系は、例えばオペレータから制御指令
生成部11にあらかじめ与えられる切替時間が経過するま
で用いられる。そしてその時間が経過すると制御指令生
成部11から倣い座標系設定手段16に新しい倣い座標系の
設定が指令される。また曲率算出手段17によって、例え
ば座標系切替時点での作業対象物表面の曲率が計算され
る。新しい倣い座標系、すなわち第２の倣い座標系は曲
率算出手段17の算出した曲率に応じた切替時間が経過す
るまで用いられ、その時間経過後に座標系切替指令手段
18から制御指令生成部11を介して再び倣い座標系の再設
定が倣い座標系設定手段16に指令される。これによって
新しい倣い座標系の設定と曲率の算出が繰り返される。

曲率算出手段17によって算出された曲率と座標系切替
時間との関係は、例えば関数として、または対応テーブ
ルとして与えられ、曲率が小さい場合には切替時間を長
く、大きい場合には短くすることにより未知形状を有す
る対象物に対する倣い動作を自動的に行うことができ
る。

〔実施例〕

第２図は本発明の座標切替方式を用いる制御系の全体
構成ブロック図である。同図において、制御系は第１図
のロボット10に対応する制御対象19、その操作部20、制
御指令生成部21、力検出部22、位置検出部23、位置制御
部14に相当する位置制御手段24、力制御部15に相当する
力制御手段25、倣い座標系設定手段16に対応する法線ベ
クトル算出部26と移動方向ベクトル算出部27、曲率算出
手段17に相当する曲率算出部28、座標系切替指令手段18
に対応する座標切替装置29から成り、座標切替装置29は
座標系の切替時間を算出する切替時間算出部29a、およ
びその算出結果に応じて座標切替指令を出力するタイミ
ングを生成するタイミング生成部29bから構成される。

第３図は倣い座標系設定の実施例である。本発明は、
倣い動作中のロボット先端位置の軌跡を成す各点におけ
る移動方向ベクトルが、倣い動作の開始地点（以下始点
という）における対象物表面への法線ベクトルとオペレ
ータが動作の開始時に与えた移動方向ベクトルの２つの
ベクトルが成す平面上に常に存在するように制御が行わ
れる。すなわち第３図において、始点P_Bにおける法線ベ
クトル▲▼と移動方向ベクトル▲▼とが成す
平面上に倣い動作中の各点における移動方向ベクトルが
存在するように制御が行われる。

まず倣い動作の開始位置、第３図の始点P_Bで倣い座標
系の算出方法について説明する。

始点P_Bでの法線ベクトル▲▼は、始点においてロ
ボットのハンドが対象物から受ける反力か求められ
る。第４図（ａ）はロボットのハンドが対象２から受け
る反力と力覚センサ座標系のO_S−X_S,Y_S,Z_Sの関係を示
す。力覚センサによれば反力の各センサ座標軸方向の
それぞれの分力f_X,f_Y,f_Zが検出されるために、反力^Ｓ
をベクトル表示すると次式で与えられる。

^Ｓ＝（f_x f_y f_z）^Ｔ ここでｓは反力が力覚センサ座標系O_S−X_S,Y_S,Z_Sで
記述されることを示している。またここでは力覚センサ
が検出するトルク成分についての発明は省略する。

力覚センサ座標系で表される法線ベクトル^ｓ _Ｂは^Ｓ
と逆向きのベクトルであって成分表示する次式で与え
られる。

ここでベクトルの大きさは |^s｜＝（f_x ²＋f_y ²＋f_z ²）^1/2 で与えられる。

力覚センサ座標系で表された法線ベクトル^ｓ _Ｂを基
準座標系で表すためには、力覚センサ座標系から基準座
標系への座標変換行列が必要である。力覚センサ座標系
の各座標軸X_S,Y_S,Z_Sの方向の単位ベクトルを基準座標系
で成分表示したものが次式で与えられるものとする。

ここで、例えば力覚センサ座標系の座標軸X_S方向の単
位ベクトル^ｏ _Ｓのｏは基準座標系で記述されているこ
とを示している。この時力覚センサ座標系から基準座標
系への変換行列^oA_Sは次式で与えられる。

⁰A_S＝（^０ _S ⁰ _S ⁰ _Ｓ） この座標変換行列を用いて基準座標系表示の法線ベク
トル^０ _Ｂは次式で与えられる。

^０ _Ｂ＝⁰A_S ^S _Ｂ 法線ベクトルは第２図の法線ベクトル算出部26によっ
て算出される。また始点以外は接触点での法線ベクトル
も同様に算出される。

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

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

倣い動作中の任意の接触点P_iでの法線ベクトル_ｉの
算出方法は、始点P_Bでの法線ベクトルの算出方法と同様
であり、次式で表される。ただし、点P_iでの反力^ｓ _ｉ
を、 ^ｓ _ｉ＝（f_Xi f_Yi f_Zi）^Ｔ とする。

⁰A_Siは、点P_iの力覚センサ座標系から基準座標系への
座標変換行列である。ここでこの変換行列は力覚センサ
座標系と基準座標系との位置関係によって変化するた
め、点P_iでの変換行が用いられる。

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

^０ _ｉ＝^０ _ｉ×^０ _ｉ なお^０ _Ｂ＝±^０ _ｉが成立すると式から_ｉを求
められないが、この場合には^０ _ｉとしてオベレータか
ら与えられた_OPを用いることにする。

次に倣い動作の制御方法を説明する。まず始点P_Sで、
対象物に対して設定された目標力_ｒを発生する時は、
力の大きさがF_rで、方向・向きが_Ｓの力を発生すれば
よい。したがって、設定力ベクトル^０ _ｒは、式で求
められた^０ _Ｂを用いて、 ^０ _ｒ＝F_r・^０ _Ｂ で与えられる。

始点P_Bでのロボットの進行方向は、始点P_Bからの相対
位置指令で与えられる。始点P_Bからの移動方向は、式
の移動方向ベクトル^０ _Ｂで与えられており、^０ _Ｂを
用いて目標位置を表すと、 ^０ _Ｂ＝α・^０ _Ｂ となる。ここでαをあまり小さく選ぶと対象物表面が平
坦な場合などは座標系の切り替えが行われないうちにロ
ボットが目標位置に達して停止してしまうため、始点か
ら充分離れた点が指令されるようにその値を選ぶ必要が
ある。

目標力および目標位置の設定は第２図の制御指令生成
部21によって行われる。

倣い動作は次の条件のいずれかが成立したときに終了
する。

１） オペレータから終了の指令があった時 ２） 一定時間を経過した時 ３） 衝突したとき、または、対象物から離れた時、 ４） ロボットの可動範囲を越えた時、 次に倣い座標系の切替について説明する。

前述のように、ロボットと対象物が接触する点で倣い
座標を設定して、正しい制御方向を指示すると、ロボッ
トと対象物との接触力を一定に保つ事が可能になる。こ
の倣い座標系の切替は、対象物体面の曲率が小さい場合
（対象面が平面に近い場合）は、座標系を切替なくても
設定値からの力のズレが少ないため、切替の時間間隔は
長くとってもよい。そこで、対象面が大きく変化する場
合、つまり、面の曲率が大きい時には、座標系の切替時
間を短くし、面の曲率が小さい時には座標系の切替時間
を長くするように制御を行う。第５図は曲率算出方法の
実施例を、また第６図は曲率と切替時間の関係の実施例
を示す。以下、曲率および切替時間の算出と座標系の切
替を第2,5,6図によって説明する。

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

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

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

以上の曲率の算出は、第２図の位置検出部23で検出さ
れたロボットの先端位置、法線ベクトル算出部26で算出
された法線ベクトル、第２図に図示しないメモリに格納
された１サンプリング前のロボット先端位置を用いて、
制御指令生成部21内の曲率算出部28によって行われる。

倣い座標系を切り換える切替時間は、式で求められ
た曲率と一定の関係で対応付けて求められる。曲率と切
替時間は一般的には、曲率が小さい時は切替時間を長
く、曲率が大きい時は切替時間を短くするように関係付
けられる。曲率と切替時間の関係は、（１）オペレータ
が与える場合と、（２）適当な関数を用いて算出する場
合の２通りが考えられる。

（１） オペレータが与える場合 倣い動作を開始する前に、オペレータが座標切替装置
の記憶領域（メモリ、ディスク装置等）に、あらかじめ
第６図（ａ）のような曲率と切替時間の対応表を作成し
ておく。第２図の切替時間算出部29aは、曲率ｋが与え
られた時にこの対応表を参照して、切替時間を出力す
る。表中のk_a、k_b、k_c・・・・r_a、r_b、r_c・・・は、オ
ペレータが制御指令生成ブロック21を介して入力する。

（２） 関数を用いる場合 曲率ｋと、切替時間ｒをある適当な関数ｆ（ｋ）によ
って関係付ける。

ｒ（ｋ）＝ｆ（ｋ） 第６図（ｂ）は、関数の例である。図のようにｋとｒ
の関係は、線形的であっても非線形的であってもよい。

また（１）（２）の何れの場合でも、制御装置の計算
時間の限界から定まる切替時間の最小値r_minが存在す
る。したがって、曲率がある値以上大きくなると切替時
間ｒは、一定値r_minとなる。

切替時間の算出は、第２図の切替時間算出部29aで行
われる。

第２図のタイミング生成部29bは、切替時間算出部29a
で算出部されたｒを基に時間τの経過後に、制御指令生
成部21へ倣い座標系切り換えの指令を出す。タイミング
生成部29bは、時間の経過を計るため、制御装置のサン
プリング時間と同期してサンプリング時間を積算する、
システムに内蔵されているタイマからの割り込みを受け
付ける、ウォッチドッグタイマを起動する（マルチタス
クの場合）などの方法を用いる。また、対象物体面の状
況を監視するため曲率算出をサンプリング時間毎に実行
しておくか、あるいは倣い座標系の切り換え時だけ実行
して、計算機の負荷を出来るだけ減らすかなどの制御も
行われる。制御指令生成部21は、タイミング生成部29b
が発生した切替の制御指令を受けると、倣い座標系の切
替や各算出部の状態制御を行う。

第７図はロボットの倣い制御装置の実施例の全体構成
ブロック図である。同図において、ホストコンピュータ
42を除く制御装置はロボットのマニプレータ31、マニプ
レータの操作部32、マニプレータの図示しないハンド先
端位置を検出する位置検出部33、マニプレータの力検出
部34、位置検出部33の出力としてのマニプレータ31の駆
動モータの回転角度θ_Ｓをロボットのハンド先端位置X₀
に変換する座標変換部35、ホストコンピュータ42から与
えられる目標位置と座標変換部35の出力との差をとる偏
差部36、偏差部36の出力とホストコンピュータ42から与
えられる位置制御パラメータを用いて、位置制御方向の
速度指令信号V_Pを出力する位置制御部37、ホストコンピ
ュータ42から与えられる力指令と力検出部34の出力F₀と
の差をとる偏差部38、偏差部38の出力とホストコンピュ
ータ42から与えられる力制御パラメータを用いて、力制
御方向の速度指令値V_fを出力する力制御部39、位置制御
部37の出力V_P、力制御部39の出力V_f、ホストコンピュー
タ42から与えられる速度指令V₀の和をとる加算部40、加
算部40の出力V_Kをマニプレータの各関節の回転速度θに
変換する逆ヤコビ変換部41から構成されている。ここで
逆ヤコビ行列J^-1は微小時間に対するロボット手先の微
小変位（速度Ｖ）とマニプレータの各関節の微小角変位
（角速度）とを関係づける式 Ｖ＝Ｊ において、各関節の構造で決まるヤコビ行列Ｊの逆行列
である。

またマニプレータ31の出力側にある偏差部31aは力覚
センサ34a、例えば歪みゲージへの入力としての変位を
与えるためのものである。すなわちマニプレータ31のハ
ンド先端部が対象物に接触した瞬間では力覚センサ34a
にはまだ変位が与えられずその出力はθであるか、さら
にハンド先端部が対象物に押しつけられるとコントロー
ラ内で保持されているハンド先端位置X_a（力覚センサの
変形が含まれない）と対象物の位置X_Eとに差ΔX_E＝X_a−
X_Eが生じ、この差に比例した反力F_hが力覚センサ34aか
ら出力される。

第７図において操作部32はマニプレータ31を駆動する
サーボモータ32a、パワーアンプ32b、D/Aコンバータ32
c、補償器32dを有し、また位置検出部33はエンコーダお
よびカウンタ33aとタコメータ33bから成り、力検出部34
は力覚センサ34a、およびセンサ座標系で表示された力
覚センサ34aの出力を基準座標系に変換する座標変換部3
4bから成っている。そして位置検出部33内のエンコーダ
およびカウンタ33aとタコメータ33bから、操作部32内の
補償器32dに補償演算のための信号が出力され、さらに
エンコーダおよびカウンタ33aの出力、すなわち各関節
の回転角度θ_Ｓが力検出部34内の座標変換部34bと逆ヤ
コビ変換部41に入力されている。

次にホストコンピュータ42は法線ベクトル算出部43、
移動方向ベクトル算出部44、制御指令生成部45、および
座標切替装置46から成る。法線ベクトル算出部43は力検
出部34の出力、すなわち基準座標系に変換された検出力
F₀からマニプレータと対象物の接触点での法線ベクトル
^０を算出し、これを移動方向ベクトル算出部44と制御
指令生成部45に出力する。

移動方向ベクトル算出部44は、座標変換部35の出力で
あるハンドの現在位置X₀と法線ベクトル算出部43から入
力される法線ベクトル^０等を用いて、移動方向ベクト
ル^０を算出し、制御指令生成部45に出力する。

制御指令生成部45は法線ベクトル算出部43、移動方向
ベクトル算出部44の出力、力検出部34の出力である検出
力F₀、および座標変換部35の出力であるハンドの現在位
置X₀を用いてロボットコントローラに目標位置X_r、力指
令F_rの制御指令と、倣い座標系の切替に伴う位置制御、
力制御パラメータを送信し、また法線ベクトル算出部43
と移動方向ベクトル算出部44へ状態制御の信号を発生す
る。

また制御指令生成部45からロボットコントローラに与
えられる信号は、偏差部36に与えられる目標位置X_r、位
置制御パラメータとして位置制御部37に与えられる倣い
座標系の各単位ベクトルと後述する位置フィードバック
ゲインC_P、偏差部38に与えられる力指令F_r、力制御パラ
メータとして力制御部39に与えられる倣い座標系の各単
位ベクトルと後述する力フィードバックゲインC_f、およ
び例えばハンド先端部を高速に移動させるために加算部
40に与えられる速度指令V₀がある。ここでV₀の値はオペ
レータによってその値が直接与えられる場合や、オペレ
ータによって設定された移動距離ｌと移動時間ｔを用い
てV₀＝l/tで与える場合等がある。

さらに制御指令生成部45に力検出部34の出力である検
出力F₀が入力されるのは、後述のように対象物の反力を
検出することによって対象物への押付けが成功したか否
かを判別するためでもあり、また座標変換部35の出力で
あるハンドの現在位置X₀が入力されるのは、ハンドの位
置を用いて対象物表面の曲率を算出するためでもある。

第７図の制御指令部45の内部には、法線ベクトル算出
部43で算出された法線ベクトルと、例えばサンプリン
グ時間毎に曲率を算出する場合には１サンプリング前の
ロボット先端位置とを用いて現在のロボット先端位置の
曲率を算出する曲率算出部47がある。また座標切替装置
46は曲率算出部47の算出した曲率から座標系の切替時間
を求める切替時間算出部48と、制御指令生成部45へ倣い
座標系の切替を指令するタイミング生成部49から成り、
タイミング生成部49は制御装置のサンプリング時間と同
期してサンプリング時間を積算するための例えばカウン
タ49a、第７図に図示しないタイマからの割り込みを受
け付ける割り込み処理ルーチン49b、および例えばマル
チタスクの場合などに起動されるウォッチドッグタイマ
49cから構成される。

第８図は第７図における位置制御部37と力制御部39の
詳細構成を示すブロック図である。同図において、位置
制御部37は偏差部36の出力、すなわち基準座標系で表さ
れた位置偏差を倣い座標系に変換する転置直交変換行列
（R^T）演算部37a、選択行列（Ｉ−S_f）演算部37b、直交
行列（Ｒ）演算部37c、および位置フィードバックゲイ
ン（c_P）演算部37dとから成り、また力制御部39は偏差
部38の出力である基準座標系で表された力偏差を倣い座
標系に変換する転置直交変換行列（R^T）演算部39a、選
択行列（S_f）演算部39b、直交行列（Ｒ）演算部39c、お
よび力フィードバックゲイン（c_f）演算部39dとを有す
る。

ベクトル、、およびを用いて、倣い座標系
（X_W,Y_W,Z_W）からロボット基準座標系（X₀,Y₀,Z₀）への
座標変換に用いられる直交座標変換行列Ｒは次の式のよ
うに与えられる。

ここで行列Ｒは６行６列の正方行列であり、第１〜３
行の第１〜３列はX,Y,Zの各軸の座標値x,y,zの変換に対
応し、第４〜６行の第４〜６列はX,Y,Zの各軸の回りの
回転角度のα，β，γの変換に対応し、その他の部分の
要素は全て０である。

第11図のように、倣い座標系のX_W方向を力制御方向、
Y_WおよびZ_W方向を位置制御方向とすると選択行列演算部
39bの選択行列S_fは次式で与えられる。

そして位置制御部37内の選択行列演算部37bで用いら
れる選択行列Ｉ−S_fは６次の単位行例Ｉから式のS_fを
引いた行列として与えられる。

位置制御部37内の位置フィードバックゲイン演算部37
dで用いられる位置フィードバックゲインc_Pは、一般に
基準座標系に関して次式で与えられる。

また力制御部39内の力フィードバックゲイン演算部39
dで用いられる力フィードバックゲインc_fは、基準座標
系に関して次式で与えられる。

以上のように、例えば位置制御部37内では偏差部36の
出力である基準座標系で表された位置偏差が転置直交変
換行列演算部37aによって倣い座標系に変換され、それ
に選択行列Ｉ−S_fが掛けられて位置制御方向の成分のみ
が取り出され、直交行列演算部37cによって再び基準座
標系に変換され、さらに位置フィードバックゲインc_pが
掛けられて位置制御方向の速度指令V_Pとして加算部40に
出力される。

第９図は倣い制御装置の実施例のシステム構成ブロッ
ク図である。同図においてホストコンピュータ42は法線
ベクトル算出部43、移動方向ベクトル算出部44、制御指
令生成部45、座標切替装置46、曲率算出部47、タイミン
グ生成部49内の割り込み処理ルーチン49bに座標系切替
の割り込みを行うためのタイマ50、メモリ51、および通
信制御部52を有する。ロボットコントローラ53はメモリ
53a、通信制御部53b、および力制御部、位置制御部、座
標変換部、偏差部等53cを有し、操作部32、位置制御検
出部33、および力覚センサ34aを介してマニプレータ31
の制御を行う。ホストコンピュータ42とロボットコント
ローラ53はバス等の通信インタフェースによって接続さ
れ、それぞれの信号送受信のタイミングを管理する通信
制御部52,53bによって、メモリ51と53aとの間でデータ
が転送される。制御指令生成部45、法線ベクトル算出部
43、移動方向ベクトル算出部44、曲率算出部47、および
座標切替装置46の計算に必要なデータはメモリ上のデー
タが参照される。

第10図は倣い動作の処理実施例のフローチャートであ
る。同図は未知形状の曲面を有する対象物への倣い動作
の処理フローチャートであり、ホストコンピュータ42の
内部の制御指令生成部45、法線ベクトル算出部43、移動
方向ベクトル算出部44、曲率算出部47、座標切替装置4
6、およびロボットコントローラで行われる処理の流れ
を示している。なお倣い動作のためのパラメータはオペ
レータによって設定されるものとし、倣い動作の始点ま
でのマニプレータの移動方法については説明を省略す
る。また図の中で添字Ｂは始点を、添字ｉは第ｉ番目の
倣い座標設定時における処理を示している。

まずオペレータはステップ（Ｓ）55で移動方向ベクト
ル^０▲▼、第６図に示したような曲率と座標系切
替時間の関係、設定力F_r、始点での倣い座標系の切換時
間τ_Ｂ、および相対位置を指定する係数αを設定し、さ
らにステップ56で始点での法線ベクトルを算出するため
に発生する力ベクトル_Ｂを設定する。制御指令生成部
は、ステップ57で始点で力_Ｂを発生するための力指令
_Ｂおよび力制御パラメータを生成し、ロボットコント
ローラへ転送する。ロボットコントローラはステップ58
で指令に基づいて力▲▼を発生し、対象物に押付け
動作を行う。制御指令生成部によりステップ59で検出さ
れる反力がステップ60で０と判定されると、ロボット
と対象物が離れていることになるのでステップ56に戻
り、再度力ベクトル▲▼の設定が行われる。

ステップ60で反力が０でない場合には、ステップ61
でロボット先端位置⁰P_Bがメモリに記憶された後に、法
線ベクトル算出部によってステップ62でセンサ座標系の
法線ベクトル^Ｓ _Ｂが求められ、ステップ63で座標変換
行列⁰A_sが算出され、ステップ64で基準座標系で表示さ
れた法線ベクトル^０ _Ｂが算出され、それが制御指令生
成部および移動方向ベクトル算出部に転送される。

移動方向ベクトル算出部は、ステップ65で算出された
法線ベクトルの方向がオペレータによりステップ55で設
定された移動方向ベクトルの方向と一致しないか否かを
判定し、一致する場合にはステップ55でのパラメータ設
定が不適当として、ステップ55からの処理が繰り返され
る。

両ベクトルの方向が一致しない場合には、ステップ66
で式を用いて接平面上のベクトル^０ _Ｂを算出して、
その結果をメモリに格納し、ステップ67で式により移
動方向ベクトル^０ _Ｂを算出し、それを制御指令生成部
へ転送する。

制御指令生成部は、ステップ68で式により目標力ベ
クトル^０ _ｒを生成しロボットコントローラへ転送し、
ステップ69で式を用いて目標相対位置ベクトル^０ _ｒ
を生成してロボットコントローラへ転送する。またステ
ップ70で、例えばロボットハンドを高速に動かすための
速度指令V₀をロボットコントローラに与え、ロボットコ
ントローラはステップ71で倣い動作を開始する。制御指
令生成部は倣い動作の開始後、ステップ55で設定された
切替時間（τ_Ｂ）が経過したか否かをステップ72で監視
し、経過するまではロボットコントローラによりステッ
プ71の倣い動作が続けられる。ステップ72で時間τ_Ｂが
経過したと判定された場合には、ステップ73で制御指令
生成部によりロボット先端位置Ｘが検出され、ステップ
74で先端位置がロボットの可動範囲内にあるか否かが判
定され、可動範囲内にない場合には倣い動作が終了し、
次の動作に移行する。

ステップ74で可動範囲内にある場合には、ステップ75
以降で新しい倣い座標系の設定とその座標系を用いた倣
い動作が繰り返し行われる。すなわちステップ75でまず
反力が検出され、ステップ76で制御指令生成部により
が０かまたは極端に大きい（Ｆ≫１）か否かが判定さ
れる。が０の場合には、何らかの原因でロボットと対
象物とが離れたことに、また極端に大きい場合には例え
ば壁に衝突したことになるので動作を停止する。

ステップ76でＦが０でなくまた極端に大きくもない場
合はステップ77でロボット先端位置がメモリに格納され
た後にステップ78から80で法線ベクトル算出部によって
ステップ62から64と同様に法線ベクトルが算出され、そ
の結果がメモリに格納される。そしてステップ81で移動
方向ベクトル算出部によって移動方向ベクトルが算出さ
れる。ここで法線ベクトルと移動方向ベクトルは倣い動
作を行う場合には必ず必要であるが、ベクトルは倣い
座標系を完全に決定する場合以外には必要ないので、ス
テップ66におけるようなベクトルの算出は行われな
い。

続いてステップ82と83で、制御指令生成部によってス
テップ68と69と同様にして目標力ベクトルと目標相対位
置ベクトルが生成され、ロボットコントローラに転送さ
れる。ステップ84で，式を用いて曲率算出部によっ
て曲率円の半径と曲率が算出され、ステップ85で切替時
間算出部によって倣い座標系の切替時間が算出され、タ
イミング生成部にその値が転送される。そしてステップ
86でステップ70と同様に速度V₀が指令され、ステップ87
で倣い動作が開始され、ステップ88でタイミング生成部
から倣い座標系の切替指令が出たか否かが制御指令生成
部によって監視される。

ステップ88でまだ切替指令が出ていない場合にはステ
ップ87以降の処理が繰り返され、タイミング生成部によ
ってステップ89で座標系の切替指令が出るとステップ88
でそれが判定され、ステップ90でオペレータの終了指示
があるか否かが判定され、終了指示がまだ出ていない場
合にはステップ73からの処理が繰り返され、終了指示が
出た場合には倣い動作は終了し、次の動作に移行する。

〔発明の効果〕

以上詳細に説明したように、本発明によれば曲面を有
する未知形状の対象物に対する倣い動作を自動的に行う
ことができ、対象物体の変更や対象物の位置ずれに伴う
ロボットへのティーチングを行う必要がなくなり、オペ
レータの負担が大幅に軽減される。また対象物体表面の
曲率によって倣い座標系の切替時間が自動的に変化する
ためにロボットの制御装置の計算の負荷が大いに軽減さ
れる。

【図面の簡単な説明】 第１図は本発明の原理ブロック図、 第２図は本発明の座標切替方式を用いる制御系の全体構
成を示すブロック図、 第３図は倣い座標系設定の実施例を示す図、 第４図は設定力F_rの算出の実施例を示す図、 第５図は曲率の算出方法の実施例を示す図、 第６図は曲率と切替時間の関係の実施例を示す図、 第７図はロボットの倣い制御装置の実施例の構成を示す
ブロック図、 第８図は位置制御部および力制御部の実施例の構成を示
すブロック図、 第９図はロボットの倣い制御装置のシステム構成を示す
ブロック図、 第10図は倣い動作の処理実施例のフローチャート、 第11図は倣い座標系を説明する図、 第12図は曲面を有する対象物と倣い座標系の関係を示す
図である。 １……基準原点、 2,3,4,6……関節、 5,31……マニプレータ、 ７……ハンド、 8,34a……力覚センサ、 33……位置検出部、 34……力検出部、 37……位置制御部、 39……力制御部、 42……ホストコンピュータ．

───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 平１−229306（ＪＰ，Ａ) 特開 昭63−62640（ＪＰ，Ａ) 特開 平１−115550（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁶，ＤＢ名) G05B 19/4093,19/408

Claims (3)

(57)【特許請求の範囲】
1. 【請求項１】ロボット（10）に対する力・位置指令およ
   びパラメータ転送を行う制御指令生成部（11）と、該ロ
   ボット（10）と作業対象物との間に働く力を検出する力
   検出部（12）と、該ロボット（10）の位置を検出する位
   置検出部（13）と、該位置検出部（13）によって検出さ
   れた該ロボット（10）の現在位置と前記制御指令生成部
   （11）の指令とに基づいてロボット（10）の位置を制御
   する位置制御部（14）と、前記力検出部（12）によって
   検出された力と前記制御指令生成部（11）の指令とに基
   づいてロボット（10）の力を制御する力制御部（15）と
   を有し、ロボット（10）の先端部の作業対象物に対する
   位置・姿勢により決定される倣い座標系とロボット（1
   0）への移動指令に基づいて対象物表面に一定の力を加
   えながら倣い動作を行う力制御ロボットにおいて、前記
   力検出部（12）と位置検出部（13）との出力により、前
   記ロボット（10）の先端と作業対象物との接触点におけ
   る対象物表面の法線方向の単位ベクトルと、該ロボット
   （10）の先端の移動方向の単位ベクトルとで決定される
   倣い座標系を設定する倣い座標系設定手段（16）と、該
   倣い座標系設定手段（16）と前記位置検出部（13）の出
   力により、該ロボット（10）と対象物との接触点におけ
   る未知形状対象物表面の曲率を算出する曲率算出手段
   （17）と、 該曲率算出手段（17）の算出結果に応じて、前記倣い座
   標系の切替時間を求め、該切替時間経過後に前記倣い座
   標系設定手段（16）に、制御指令生成部（11）を介して
   新しい倣い座標系設定を指令する座標系切替指令手段
   （18）とを備えたことを特徴とするロボットの倣い制御
   の座標切替方式。
2. 【請求項２】前記座標系切替指令手段（18）が、前記ロ
   ボット（10）の制御装置の記憶領域に格納された対象物
   表面の曲率と倣い座標系切替時間との対応表に基づき、
   曲率算出手段（17）の曲率算出結果に応じて前記倣い座
   標系の切替時間を求めることを特徴とする請求項１記載
   の倣い制御の座標切替方式。
3. 【請求項３】前記座標系切替指令手段（18）が前記曲率
   算出手段（17）の算出する曲率の関数として前記倣い座
   標系切替時間を求めることを特徴とする請求項１記載の
   倣い制御の座標切替方式。