JP2773778B2 - 力制御ロボットの倣い装置 - Google Patents

Description

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

〔産業上の利用分野〕

本発明は力制御ロボットで作業対象物の表面に沿って
加工等を行う倣い作業のための倣い制御方式に係り、さ
らに詳しくは未知形状の曲面を有する対象物体上を倣う
場合に対象面の曲率によって倣い座標系を設定し、対象
物体に加える力を一定に保つ力制御ロボットの倣い装置
に関する。

〔従来の技術〕

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

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

このように倣い座標系Ow−Xw,Yw,Zwは対象物に対する
ロボット先端部（ハンド）の位置、および姿勢により決
定される座標系であって、単位ベクトル_Ｗは対象物へ
力を加える場合の押付け方向を示し、倣い動作時のロボ
ット先端の移動方向を示す単位ベクトル_Ｗは_Ｗと直
交関係にあり、もう１つの単位ベクトル_Ｗは_Ｗと
_Ｗの外積として次式で与えられる。_Ｗ ＝_Ｗ×_Ｗ ここで、３つの単位ベクトル_W,_Ｗ、および_Ｗは
基準座標系Ｏ−X_o,Y_o,Z_oで表示することも可能であり、
一般に次のような式で与えられる。ただしＴは転置行列
を示している。

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

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

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

また、作業対象物が曲面を有する場合には、例えば対
象物表面の法線方向を押付け方向として、ある一定時間
毎に倣い座標系を設定して直しながら倣い作業を行うこ
とも考えられるが、この方法では一度倣い座標系を設定
してから次に倣い座標系を設定するまでの過渡時に曲率
の大きい対象面を倣うと対象物体面の変化に追従できな
くなり、設定力を保てなくなるという問題点がある。

第13図は接触力の設定力からのずれの説明図であり、
曲率の大きい対象面Ａと曲率の小さい対象面Ｂを倣う時
の状況を示している。

第13図において対象物体上の点Piで倣い座標系の単位
ベクトル_Ｗと_Ｗとが設定され、_Ｗ方向にオペレー
タによって設定された力が加わっているものとする。
ロボットの先端は、次のサンプリング時間τまでの間に
ベクトル_Ｗ方向へは一定速度ｖでｖ・τまで移動し、
ベクトル_Ｗ方向には、ロボットと対象物体の成すバネ
系の釣り合いを満たす位置まで変位する。このとき、ロ
ボットの先端は面Ａについては点P_A,_i+1、面Ｂについて
は点P_B,_i+1まで移動する。点P_A,_i+1、点P_B,_i+1までのベ
クトル_Ｗ方向への変位位置をそれぞれδ_A,δ_Ｂとし、
ロボットと対象物が成すバネ系のバネ定数をｋとすると
点P_A,_i+1、点P_B,_i+1で対象物に働くベクトル_Ｗ方向の
力はそれぞれＦ−ｋδ_Ａ、Ｆ−ｋδ_Ｂとなる。δ_Ａ＞δ
_Ｂであり、図からわかるように面の曲率が大きいと、設
定力と対象物への押しつけ力とのずれも大きくなる。す
なわち、点Piで正確な押しつけ力を発生するように倣い
座標系を設定すると、過渡時の対象面の曲率によって次
のサンプリング点で発生する押しつけ力に差がでること
がわかる。

本発明は、未知形状の曲面を有する対象物体上を倣う
場合に、対象物表面の曲率に応じて倣い座標系を設定
し、倣い作業中に対象物体に加える力を一定に保つこと
を目的とする。

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

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

第１図において法線ベクトル算出手段17は、例えば力
覚センサを用いた力検出部13の出力によって、ロボット
10の、例えばハンド先端と作業対象物との接触点におけ
る、例えば対象物の内側向きの法線方向の単位ベクト
ル、すなわち法線ベクトルを算出する。また接線ベクト
ル算出手段18は、法線ベクトル算出手段17の出力と例え
ばオペレータから制御指令生成部を介して与えられる移
動方向ベクトルとによって前述の接触点における接線ベ
クトルを算出する。例えば倣い作業の始点と終点の間の
任意の接触点で接線ベクトルは、その点での法線ベクト
ルと倣い作業の始点でのベクトル_Ｗとの両方に直交す
るベクトルとして求められる。

第１図の曲率算出手段19は、位置検出部14と法線ベク
トル算出手段17との出力によって、ロボット10のハンド
先端部と作業対象物との接触点における未知形状の対象
物表面の曲率を算出する。また倣い座標設定手段20は、
法線ベクトル算出手段17、接線ベクトル算出手段18、お
よび曲率算出手段19の出力によって、ロボット10のハン
ド先端部と対象物との接触点におけるハンド先端部の押
付け方向の単位ベクトルとロボット先端の移動方向の単
位ベクトルとで決定される倣い座標を設定する。この倣
い座標系におけるもう１つの方向の単位ベクトルは、押
付け方向単位ベクトルと移動方向単位ベクトルとの両方
に垂直なものとして求められる。

〔作用〕

本発明においては、ある接触点で倣い座標系が設定さ
れてから次の倣い座標系が設定されるまでの時間、すな
わち座標系切替時間は一定であり、その値はオペレータ
から与えられるものとする。まず倣い作業の開始点にお
いては、例えば法線ベクトル算出手段17によって算出さ
れた法線ベクトルが倣い座標系の押付け方向の単位ベク
トル_Ｗとされる。そして倣い座標系の第３の単位ベク
トル_ｗは_Ｗと例えばオペレータから与えられるロボ
ット先端の移動方向ベクトル▲▼との外積として
求められ、倣い座標系の第２の単位ベクトル、すなわち
移動方向ベクトル_Ｗは_Ｗと_Ｗとの外積として求め
られる。

倣い作業の始点において設定された倣い座標系はあら
かじめ与えられた切替時間が経過するまで用いられる。
そしてその時間が経過すると倣い座標設定手段20によっ
て新しい倣い座標が設定される。倣い座標の始点以外の
接触点での倣い座標系の設定時には、まずその接触点で
の法線ベクトルが算出される。続いて接線ベクトル
が、例えばその法線ベクトルと倣い作業の始点でのベ
クトル▲▼との外積として求められる。そして次の
倣い座標系設定時の接触点における接触力と設定力との
差を小さくするために、例えば曲率算出手段19によって
算出された曲率を用いて次の倣い座標系設定時の接触点
が推定され、移動方向ベクトル▲▼が現在の接触点
から次の接触点推定位置への方向のベクトルとして設定
される。倣い座標系の第３の単位ベクトル_Ｗは法線ベ
クトルと接線ベクトルとの外積として設定され、ま
た倣い座標系の押付け方向ベクトル▲▼は移動方向
ベクトル▲▼とベクトル▲▼との外積として求
められる。

以上のように、本発明では倣い作業の始点以外の任意
の接触点における倣い座標系の設定時に、押付け方向ベ
クトルが法線ベクトルそのものでなく、次の倣い座標系
設定点推定位置への移動方向ベクトルに垂直なものとし
て設定されるために、次の倣い座標系設定点における接
触力の設定力からのずれが小さくなる。

〔実施例〕

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

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

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

^ｓ＝（f_Xf_Yf_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は次式で与えられる。

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

^ｏ _Ｂ＝^oA_S ^s _Ｂ …… 始点P_Bにおける押付け方向ベクトル^ｏ _WBはこの法線
ベクトル^ｏ _Ｂと一致するように設定され、次式で与え
られる。

^ｏ _WB＝^ｏ _Ｂ …… なお、法線ベクトルは第１図の法線ベクトル算出手段
17によって算出される。

次に、オペレータから与えられた移動方向ベクトル
_OPと、求められた押付け方向ベクトル^ｏ _WBを用いて、
始点P_Bにおける移動方向ベクトル_WBが算出される。た
だし、ベクトル_OPとベクトル^ｏ _WBは、 _OP＝^ｏ _WBまたは_OP＝−^ｏ _WB …… を満たさないものとする。移動方向ベクトル_WBは、押
付け方向ベクトル^ｏ _WBに直交し、移動方向ベクトル
_OPと押付け方向ベクトル^ｏ _WBの成す平面上にあるベク
トルである。このとき、倣い座標系の座標軸を表す単位
ベクトルの１つである_WBは、^ｏ _WBと_OPを用いて、 ^ｏ _WB＝（^ｏ _WB×_OP）/|^o _WB×_OP| …… で表される。移動方向ベクトル^ｏ _WBは、ベクトル^ｏ
_WB、^ｏ _WBとの直交関係により、 ^ｏ _WB＝^ｏ _WB×^ｏ _WB …… で求められる。始点での移動方向ベクトルは、第１図の
接線ベクトル算出手段18によって算出される。

以上により、始点での倣い座標系が求まる。次に、第
２図の倣い動作中の任意の接触点Piでの倣い座標系の算
出方法について説明する。

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

^ｏ _ｉ＝^oA_si ^s _ｉ ……^o A_siは、点Piの力覚センサ座標系から基準座標系への座
標変換行列である。この変換行列は力覚センサ座標系と
基準座標系との位置関係によって変化するため、点Piで
の変換行列が用いられる。

対象物体は未知形状のために次のサンプリングが行わ
れる点が現在地点Pi付近の曲率円上にあると仮定する。
現在地点Pi付近の曲率を、現在地点Piと、１サンプリン
グ前の点P_i-1の位置を用いて求める。ただし、P_i,P_i-1
付近の対象物体の形状が、球で近似できると仮定する。
このとき、球の半径をｒとすると、曲率ｋは1/rで与え
られる。

第４図は、点P_iでの曲率算出方法の実施例を示したも
のである。第４図のように、点P_i,P_i-1の位置ベクトル
を、^ｏ _i,^o _i-1、点Piの法線ベクトルを^ｏ _ｉとおい
たときの点Piでの曲率を求める。点P_i付近の形状は球で
表すことができると仮定したので、曲率円の中心は法線
ベクトル^ｏ _ｉと線分P_iP_i-1の垂直２等分線の交点とな
る。点P_iから曲率円の中心Ｒへ向かうベクトルは、法線
ベクトル^ｏ _ｉを用いて、r_i ^o _ｉと表すことができる
（r_iは、点P_iの曲率円の半径）。第４図から次の関係が
成立する。

｛r_i ^o _ｉ−（^ｏ _ｉ−^ｏ _i-1）/2｝・ （^ｏ _ｉ−^ｏ _i-1）/2＝０ …… 従って、曲率円の半径r_iは、 ただし、^ｏ _ｉ・（^ｏ _ｉ−^ｏ _i-1）はベクトルの
内積を示す。

のように求められる。式から曲率ｋを求めると、 となる。

ロボットの先端位置は、ベクトル_Ｗの方向へは移動
しないように位置制御されているので、先端位置が常に
ベクトル_Ｗ、_Ｗが成す平面上にあるように制御され
る。したがって、対象物の表面に描く軸跡は、始点で与
えられたベクトル_WB,_WBの成す平面上の曲線とな
る。このことから、点P_iでの接線ベクトル_ｉもベクト
ル_WB,_WBの成す平面上にあるように求める。接線ベ
クトル_ｉは始点で求められたベクトル_WBに垂直に与
えられればよい。接線ベクトル_ｉは、法線ベクトル
_ｉにも直交するので、ベクトル^ｏ _ｉは、 ^ｏ _ｉ＝（^ｏ _WB×^ｏ _ｉ）/|^o _WB×^ｏ _i| …… のように表される。接線ベクトルは、第１図の接線ベク
トル算出手段18によって算出される。

第５図は、ロボット先端の移動方向ベクトルの算出方
法の実施例を示したものである。移動方向ベクトル_Wi
は現在位置P_iと次のサンプリング点P_i+1を結ぶ直線上の
ベクトルで、第４図で求められた曲率円と同一平面上に
ある。対象物体は未知形状であり、次のサンプリング点
P_i+1の正確な位置は不明なので、点P_i+1は、P_i付近の形
状が球で近似できると仮定したとき、P_i,P_i-1が成す曲
率円上にあると考えて移動方向ベクトルを推定する。

第13図で示したように力の押付け方向を面の法線方向
とすると、次のサンプリング点へ移動するまで法線方へ
の制御指令は変更されないため、次のサンプリング点で
ベクトル方向へδだけ変位したとき、接触力は設定力
からｋδ（ただし、ｋはロボットと対象物が成すバネ系
のバネ定数）だけ変化する。また、本発明のように、移
動方向ベクトル_Wiを直線P_i,P_i+1上のベクトルとした
場合も、押付け方向ベクトル^ｏ _Wiが点P_i+1の法線ベク
トル^ｏ _i+1と一致していないために接触力は設定力か
らずれる。そこで、まず、ベクトル^ｏ _i,^o _ｉを倣い
座標系とするよりも、ベクトル^ｏ _Wi,^o _Wiを倣い座標
系とした方が接触力と設定力のずれが小さくなることを
第６図で示す。

第６図で、点P_iはロボット先端の現在位置、^ｏ _i,^o
_ｉはそれぞれ点P_iの法線ベクトル、接線ベクトルであ
り、^ｏ _Wi,^o _Wiは、倣い座標系の単位ベクトルであ
る。また、点P_i+1は、次のサンプリング点であり、ベク
トル^ｏ _ｉ方向の点P_iからの変位をδ、法線ベクトルを
^ｏ _i+1、^ｏ _i+1と^ｏ _Wiが成す角をα、^ｏ _i+1と^ｏ
_ｉが成す角をβ、ベクトル^ｏ _wi,^o _wiを倣い座標系
としたときの^ｏ _i+1方向への押付力を_Wi、ベクトル
^ｏ _i,^o _ｉを倣い座標系としたときの^ｏ _i+1方向への
押付力を_Niとする。ベクトル^ｏ _i,^o _ｉを倣い座標
系としたときは、前述したように、変位δのため^ｏ _ｉ
方向の力は、 Ｆ→Ｆ−ｋδ のように変化する。従って法線^ｏ _i+1方向の力
_Niは、^ｏ _i+1と^ｏ _ｉが成す角βを用いて、 |F_Ni|＝（Ｆ−ｋδ）cosβ …… と表される。ベクトル^ｏ _Wi,^o _Wiを倣い座標系とした
ときの^ｏ _i+1方向への押付力_Wiは、^ｏ _i+1と^ｏ _Wi
が成す角αを用いて、 |F_wi|＝F cos α …… となる。角α、βの間には、 α＋（π−β）/2＝π/2 変形して、 α＝β/2 …… のような関係がある。式，，から、 Ｆ＞F_Wi＞F_Ni となることが分かる。したがって、点P_i+1では、ベクト
ル^ｏ _wi,^o _wiを倣い座標系とした方が、接触力と設定
力のずれが小さくなる。

第５図において、ロボットの先端位置がベクトル^ｏ
_wi方向へ_ｏで進むとすると、サンプリング時間τ後の
位置は、｜_o|τである。次のサンプリング点P_i+1は曲
率円上の点と仮定しているので、点P_i+1の位置は、曲率
円と、｜_o|τを半径とする円の交点となる。従って、
点P_i+1を、位置ベクトルP_i、ベクトル^ｏ _i,^o _ｉを用
いて表すと、次式が設立する。

ただし、^ｏ _i,^o _i+1はそれぞれ点P_i,P_i+1の位置ベ
クトルである。

式の右辺の第２、３項で^ｏ _ｉと^ｏ _ｉの係数は第５
図で点^oP_iを通る曲率円の接線に点^oP_i+1から引いた垂線
の足をQ,点^oP_i+1を通る直径と曲率円とのもう一方の交
点をＳとするときΔ^oP_i+1S^oP_iとΔ^oP_i+1 ^oP_iQとが相似と
なることから求められる。式から移動方向ベクトルは
次式で与えられる。

次に、移動方向ベクトル^ｏ _wiに直交する倣い座標系
のもう１つの座標軸を表すベクトルである^ｏ _wiを求め
る。ベクトル^ｏ _wiは、点P_iの法線ベクトル^ｏ _ｉ、法
線ベクトル^ｏ _ｉと直交するように定められる。従っ
て、ベクトル^ｏ _wiは式を用いて、 ^ｏ _wi＝^ｏ _ｉ×^ｏ _ｉ …… のように求められる。

倣い座標系の押付方向ベクトル^ｏ _wiは、式，で
算出されたベクトル^ｏ _wi,^o _wiとの直交関係から、次
式で表される。

^ｏ _wi＝^ｏ _wi×^ｏ _wi …… 以上の曲率、倣い座標系の計算は、第１図の位置検出
部14で検出されたロボットの先端位置、法線ベクトル算
出手段17により算出された法線ベクトル、接線ベクトル
算出手段18により算出された移動方向ベクトルを用いて
制御指令生成部12内に設けられた曲率算出手段19及び倣
い座標設定手段20によって行われる。

次に倣い動作の制御方法を説明する。始点P_Bを含む任
意の接触点P_iで、対象物に対して設定された目標力F_rを
発生する時は、力の大きさがF_rで、方向・向きが_wiの
力を発生すればよい。従って、設定力ベクトル飲^ｏ _ｒ
は、式で求められた^ｏ _wiを用いて、 ^ｏ _ｒ＝F_r・^ｏ _wi …… で与えられる。

点P_iでのロボットの進行方向は、点P_iからの相対位置
指令で与えられる。点P_iからの移動方向は、式の移動
方向ベクトル^ｏ _wiで与えられており、^ｏ _wiを用いて
相対位置を表すと、 ^ｏ _ｉ＝α・^ｏ _wi …… となる。ここでαは倣い座標系の修正条件から決まる定
数で、その条件には （１） 一定時間経過したとき、 （２） 対象物から受ける反力が設定力からある程度以
上ずれたとき、 （３） ロボットが一定距離を移動したとき、 等があげられるが、本発明では（１）の条件に従うもの
とする。（２），（３）の場合でも基本的な考え方は
（１）と同様である。

目標力、目標位置の設定は第１図の制御指令生成部12
によって行われる。

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

１） オペレータから終了の指令があったとき ２） 一定時間を経過した時 ３） 衝突したとき、または、対象物から離れた時 ４） ロボットの可動範囲を越えた時 第７図は力制御ロボットの倣い装置の実施例の全体構
成ブロック図である。同図において、倣い装置はロボッ
トのマニプレータ21、マニプレータの操作部22、マニプ
レータの位置検出部23、マニプレータの力検出部24、位
置検出部23の出力としてのマニプレータ21の駆動モータ
の回転角度θ_ｓをロボットのハンド先端位置X_oに変換す
る座標変換部25、ホストコンピュータ32から与えられる
目標位置と座標変換部25の出力との差をとる偏差部26、
偏差部26の出力とホストコンピュータ32から与えられる
位置制御パラメータを用いて、位置制御方向の速度指令
信号V_pを出力する位置制御部27、ホストコンピュータ32
から与えられる力指令と力検出部24の出力F_oとの差をと
る偏差部28、偏差部28の出力とホストコンピュータ32か
ら与えられる力制御パラメータを用いて、力制御方向の
速度指令値V_sを出力する力制御部29、位置制御部27の出
力V_p、力制御部29の出力V_s、ホストコンピュータ32から
与えられる速度指令V_oの和をとる加算部30、加算部30の
出力V_kをマニプレータの各関節の回転速度に変換する
逆ヤコビ変換部31から構成されている。ここで逆ヤコビ
行列J^-1は微小時間に対するロボット手先の微小変位
（速度Ｖ）とマニプレータの各関節の微小角変位（角速
度）とを関係づける式 Ｖ＝Ｊ …… において、各関節の構造で決まるヤコビ行列Ｊの逆行列
である。

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

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

次にホストコンピュータ32は法線ベクトル算出部33、
接線ベクトル算出部34、および制御指令生成部35から成
る。法線ベクトル算出部33は力検出部24の出力、すなわ
ち基準座標系に変換された検出力F_oからマニプレータと
対象物の接触点での法線ベクトル^ｏを算出し、これを
接線ベクトル算出部34と制御指令生成部35に出力する。

接線ベクトル算出部34は、法線ベクトル算出部33から
入力される法線ベクトル^ｏ等を用いて接線ベクトル^ｏ
を算出し、制御指令生成部35に出力する。

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

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

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

さらに制御指令生成部45の内部には、法線ベクトル算
出部33で算出された法線ベクトルと、例えば１サンプリ
ング前のロボット先端位置とを用いて対象物表面の曲率
を算出する曲率算出部36と、曲率算出部の算出結果、法
線ベクトル、および法線ベクトル等を用いて倣い座標系
の単位ベクトルを求める倣い座標設定部37がある。

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

ベクトル，、およびを用いて、倣い座標系
（X_w,Y_w,Z_w）からロボット基準座標系（X_o,Y_o,Z_o）への
座標変換に用いられる直交座標変換行列Ｒは次のように
与えられる。

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

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

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

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

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

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

第９図は倣い装置の実施例のシステム構成ブロック図
である。同図においてホストコンピュータ32は法線ベク
トル算出部33、接線ベクトル算出部34、制御指令生成部
35、メモリ38および通信制御部39を有する。ロボットコ
ントローラ40はメモリ40a、通信制御部40b、及び力制御
部、位置制御部、座標変換部、偏差部等40cを有し、操
作部22、位置検出部23、および力覚センサ24aを介して
マニプレータ21の制御を行う。ホストコンピュータ32と
ロボットコントローラ40はバス等の通信インターフェイ
スによって接続され、それぞれの信号の送受信のタイミ
ングを管理する通信制御部39,40bによって38と40aとの
間でデータが転送される。制御指令生成部35、法線ベク
トル算出部33、接線ベクトル算出部34、曲率算出部36お
よび倣い座標設定部37の計算に必要なデータはメモリ上
のデータが参照される。

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

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

ステップ46で反力が０でない場合には、ステップ47
でロボット先端位置がメモリに記憶された後に、法線ベ
クトル算出部によってステップ48で式によりセンサ座
標系の法線ベクトル^ｓ _Ｂが求められ、ステップ49で
式により座標変換行列^oA_sが算出され、ステップ50で
式により基準座標系表示の法線ベクトル^ｏ _Ｂが算出さ
れ、メモリに格納される。またステップ51で倣い座標設
定部により式を用いて押付方向ベクトル^ｏ _WBが算出
され、メモリに格納される。

算出されたベクトル^ｏ _WBがオペレータにより与えら
れた移動方向ベクトルとステップ52で比較され、両者が
一致（逆方向を含む）する場合にはステップ41のパラメ
ータ設定からの処理が繰り返される。両者が一致しない
場合にはステップ53,54で接線ベクトル算出部によって
式を用いてベクトル^ｏ _Ｗ、式を用いて移動方向ベ
クトル^ｏ _WBが算出され、ともにメモリ格納される。

制御指令生成部は、ステップ55で式により目標力ベ
クトル^ｏ _ｒを生成しロボットコントローラへ転送し、
ステップ56で式を用いて目標相対位置ベクトル^ｏ _ｒ
を生成してロボットコントローラへ転送する。またステ
ップ57で、例えばロボットハンドを高速に動かすための
速度指令V_Oをロボットコントローラに与え、ロボットコ
ントローラはステップ58で倣い動作を開始する。制御指
令生成部は倣い動作の開始後一定時間（τ）が経過した
か否かをステップ59で監視し、経過するまではロボット
コントローラによりステップ58の倣い動作が続けられ
る。ステップ59で一定時間が経過したと判定された場合
には、ステップ60で制御指令生成部によりロボット先端
位置Ｘが検出され、ステップ61でそれがロボットの可動
範囲内にある否かが判定され、可動範囲外のときには次
の動作に移行する。

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

ステップ63でが０でなく、また極端に大きくもない
場合には、ステップ64でロボット先端位置がメモリに格
納された後に、ステップ65から67でステップ48から50と
同様に法線ベクトル算出部によって法線ベクトルが算出
され、その結果がメモリに格納される。次にステップ68
で曲率算出部によって曲率円の半径の算出とそのメモリ
への格納が行われ、ステップ69で法線ベクトル算出部に
よって接線ベクトルの算出とメモリへの格納が行われ
る。続いて倣い座標設定部によりステップ70で移動方向
ベクトル、71でベクトル_wi、ステップ72で押付け方向
ベクトル_wiが算出され、移動方向ベクトルと押付け方
向ベクトルはメモリに格納される。

続いてステップ73と74で、制御指令生成部によってス
テップ55と56と同様にして目標力ベクトルと目標相対位
置ベクトルが生成され、ロボットコントローラに転送さ
れる。そしてステップ75で速度V_Oが指令され、ステップ
76で倣い動作が開始され、ステップ77で倣い座標系の切
替時間τが経過したか否かが監視される。

ステップ77で時間τが経過していない場合にはステッ
プ76以降の処理が繰り返され、時間τが経過するとステ
ップ78で再びロボット先端位置が制御指指令生成部によ
って検出され、ステップ79で可動範囲内にあるか否かが
判定され、可動範囲内にある場合にはステップ80,81で
ステップ62,63と同様に反力の検出とその値が０また
は極端に大きいか否かが判定され、が０でもなく、ま
た極端に大きくもない場合にはステップ82でオペレータ
の終了指示があるか否かが判定され、終了指示がまだ出
ていない場合にはステップ64からの処理が繰り返され、
終了指示が出た場合には倣い動作は終了し、次の動作に
移行する。

〔発明の効果〕

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

【図面の簡単な説明】

第１図は本発明の原理ブロック図、 第２図は倣い座標系設定の実施例を示す図、 第３図は設定力F_rの算出の実施例を示す図、 第４図は曲率の算出方法の実施例を示す図、 第５図は移動方向ベクトルの算出方法の実施例を示す
図、 第６図は次の倣い座標系設定時の接触点における押付け
力の推定方法の実施例を示す図、 第７図はロボットの倣い装置の実施例の構成を示すブロ
ック図、 第８図は位置制御部および力制御部の実施例の構成を示
すブロック図、 第９図はロボットの倣い装置のシステム構成を示すブロ
ック図、 第10図は倣い動作の処理実施例のフローチャートを示す
図、 第11図は倣い座標系を説明する図、 第12図は曲面を有する対象物と倣い座標系の関係を示す
図、 第13図は接触力の設定力からのずれを説明する図であ
る。 １……基準原点、 2,3,4,6……関節、 5,21……マニプレータ、 ７……ハンド、 8,24a……力覚センサ、 23……位置検出部、 24……力検出部、 27……位置制御部、 29……力制御部、 32……ホストコンピュータ．

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

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】ロボットの操作部と、該ロボットに対する
   力・位置指令およびパラメータ転送を行う制御指令生成
   部と、該ロボットと対象物との間に働く力を検出する力
   検出部と、前記操作部の出力から該ロボットの位置を検
   出する位置検出部と、該位置検出部によって検出された
   該ロボットの現在位置と前記制御指令生成部の指令とに
   基づいてロボットの位置を制御する位置制御部と、前記
   力検出部によって検出された力と前記制御指令生成部の
   指令とに基づいてロボットの力を制御する力制御部とを
   有し、該ロボットの対象物に対する位置・姿勢により決
   定される倣い座標系と該ロボットへの移動指令に基づい
   て、対象物表面に一定の力を加えながら倣い動作を行う
   力制御ロボットにおいて、 前記力検出部の出力により前記ロボットと対象物との接
   触点における法線ベクトルを算出する法線ベクトル算出
   手段と、 該法線ベクトル算出手段の出力を用いて該接触点におけ
   る接線ベクトルを算出する接線ベクトル算出手段と、 前記位置検出部と該法線ベクトル算出手段との出力によ
   り該接触点における未知形状対象物表面の曲率を算出す
   る曲率算出手段と、 前記法線ベクトル算出手段、接線ベクトル算出手段およ
   び曲率算出手段の出力により、前記ロボットと対象物と
   の接触点におけるロボット先端の押付け方向の単位ベク
   トルと、該ロボット先端の移動方向の単位ベクトルとで
   決定される倣い座標を設定する倣い座標設定手段とを備
   えたことを特徴とする力制御ロボットの倣い装置。