JP2751967B2 - 倣い制御装置 - Google Patents

Description

【発明の詳細な説明】 〔概要〕 力制御ロボットで対象物の表面に沿って加工等を行う
倣い作業のための倣い制御方式に係り、さらに詳しく
は、例えば力覚センサによってロボットと作業対象物と
の間で検出された検出力を基にして倣い動作のための座
標系を設定しながら、曲面を有する対象物体の表面に沿
ってロボットを倣い動作させる倣い制御装置に関し、 ロボットの手首部とハンドとの間に取付けられた力覚
センサの検出力を利用して倣い座標系を自動的に生成し
ながら、未知形状曲面を持つ対象物にある一定の力を加
えながら倣い動作を行うことを目的とし、 ロボットと、該ロボットと作業対象物との間に働く力
を検出する力検出部と、前記ロボットの位置を検出する
位置検出部と有する力制御ロボットにおいて、該力検出
部の出力によって前記ロボットと作業対象物との接点に
おける法線ベクトルを算出する法線ベクトル算出手段
と、該法線ベクトル算出手段と、前記位置検出部との出
力により、前記ロボットの移動方向ベクトルを検出する
移動方向ベクトル算出手段と、該移動方向ベクトル算出
手段、および前記法線ベクトルの出力により、前記ロボ
ットへの力・位置指令を出力する制御指令生成手段と、
該制御指令生成手段と、前記力検出部、および位置検出
部の出力により、前記ロボットの力と位置とを制御する
力・位置制御手段とを備えるように構成する。

〔産業上の利用分野〕

本発明は、力制御ロボットで対象物の表面に沿って加
工等を行う倣い作業のための倣い制御方式に係り、さら
に詳しくは、例えば力覚センサによってロボットと作業
対象物との間で検出された検出力を基にして倣い動作の
ための座標系を設定しながら、曲面を有する対象物体の
表面に沿ってロボットを倣い動作させる倣い制御装置に
関する。

〔従来の技術〕

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

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

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

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

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

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

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

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

本発明は、例えばロボットの手首部とハンドとの間に
取付けられた力覚センサの検出力を利用して倣い座標系
を自動的に生成しながら、未知形状曲面を持つ対象物に
ある一定の力を加えながら倣い動作を行うことを目的と
する。

〔課題を解決するための手段および作用〕

第１図（ａ）は第１の発明の原理ブロック図、第１図
（ｂ）は第２の発明の機能ブロック図である。第１図
（ａ）はロボット10と作業対象物との間に働く力を検出
する力検出部11、例えば力覚センサと、ロボットの位
置、例えばハンド位置をロボット10のマニプレータを駆
動するモータに取付けられたエンコーダなどによって検
出する位置検出部12を有する力制御ロボットの倣い制御
位置の原理ブロック図である。

第１図（ａ）において、法線ベクトル算出手段13は力
検出部11、例えば力覚センサの出力からロボット10の先
端（ハンド）と対象物との接点における法線ベクトルを
算出する。移動方向ベクトル算出手段14は、法線ベクト
ル算出手段13の出力と、位置検出部12が出力するロボッ
ト先端の現在位置と、例えばあらかじめ入力される倣い
作業の終了位置からロボット10の移動方向ベクトルを算
出する。

制御指令生成手段15は、法線ベクトル算出手段13と移
動方向ベクトル算出手段14との出力からロボット10に対
する力および位置の指令値すなわち目標値と、例えば制
御パラメータとしてのフィードバックゲインなどを出力
する。ここで法線ベクトル算出手段13、移動方向ベクト
ル算出手段14、および制御指令生成手段15は、例えば１
つのパーソナルコンピュータによって実現される。

力・位置制御手段16は、制御指令生成手段15の出力す
る力指令値と力検出部11の検出力との偏差、および制御
指令生成手段15の出力する位置指令と位置検出部12の出
力するロボット先端位置との偏差に応じて、ロボット10
の力と位置とを制御する。

第１図（ｂ）は第２の発明、すなわち力制御ロボット
の倣い制御方法の機能ブロック図である。同図におい
て、まず20でロボットと作業対象物との間で検出された
力からその力と逆方向の単位ベクトルが求められ、21で
その逆方向単位ベクトルを倣い座標系から基準座標系記
述に交換してロボットと対象物との接点における法線ベ
クトルが求められる。

一方、22でロボット先端の現在位置から倣い動作終了
位置までの方向の単位ベクトルが求められ、23で法線
ベクトルと終了位置への方向単位ベクトルとの外積
から、その外積と同一方向の単位ベクトルとして接平面
上の単位ベクトルが求められ、24でさらにとの外
積からその方向、すなわちロボット先端の移動方向の単
位ベクトルが求められる。そして、25で倣い動作の対
象物が未知形状の曲面を持つものとして、例えば一定時
間毎に法線ベクトルとロボット先端の移動方向単位ベ
クトルとが設定し直され、倣い動作がその終了点まで
続行される。

すなわち本発明においては力検出部11、例えば力覚セ
ンサによって検出された、ロボット10と作業対象物との
間に作用する力の逆方向の単位ベクトルを基準座標系に
変換した法線ベクトルが法線ベクトル算出手段13によ
って算出される。次に移動方向ベクトル算出手段14によ
って法線ベクトルと終点位置方向の単位ベクトルと
からロボット先端と対象物との接平面上の単位ベクトル
が、またとからロボット先端の移動方向単位ベク
トルが求められる。これらのベクトル、を用い
て、制御指令生成手段15によってロボット10の制御を行
うための目標値として力および位置の制御指令が力・位
置制御手段16に与えられ、ロボット10の力と位置の制御
が行われる。そして対象物の面が未知曲面であるものと
して、例えばある時間毎に法線ベクトルと先端の移動
方向ベクトルとが設定し直され、倣い動作が行われ
る。

以上のように、本発明においては曲面を有する対象物
への倣い動作を自動的に行うことができる。

〔実施例〕

第２図は本発明における倣い座標系の設定法の実施例
である。同図において対象物表面上の始点P_Bから終点P_E
まで倣い動作が行われるものとし、まず倣い動作の開始
位置、すなわち始点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
で記述されることを示している。またここでは力覚セン
サが検出するトルク成分についての説明は省略する。

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

ここでベクトルＦの大きさは で与えられる。

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

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

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

^ｏ _Ｂ＝^oA_s ^s _Ｂ 次に求められた法線ベクトル^ｏ _Ｂを用いて、始点P_B
におけるロボットハンドの移動方向ベクトルの算出につ
いて説明する。移動方向ベクトル_Ｂは法線ベクトル
_Ｂに直交し、かつ始点P_Bと終点P_Eとを結ぶ直線と法線ベ
クトル_Ｂのなす平面上にあるベクトルである。始点P_B
と終点P_Eとを結ぶ直線の方向の単位ベクトル_Ｂは、基
準座標系における始点P_Bと終点P_Eの位置ベクトル
^ｏ _Ｂ、^ｏ _Ｅとを用いて次式で与えられている。^ｏ _Ｂ ＝（^ｏ _Ｅ−^ｏ _Ｂ）/|^o _Ｅ−^ｏ _B| この時始点P_Bにおいて対象物表面に接する接平面上に
あり、倣い座標系の座標軸方向の単位ベクトルの１つで
ある^ｏ _Ｂは^ｏ _Ｂと^ｏ _Ｂとを用いて次式で与えられ
ている。^ｏ _Ｂ ＝（^ｏ _Ｂ×^ｏ _Ｂ）/|^o _Ｂ×^ｏ _B| （ただし^ｏ _Ｂ≠^ｏ _Ｂ） さらにロボットハンドの移動方向ベクトル^ｏ _Ｂは、
同様に接平面上のベクトルとして次式で与えられる。

^ｏ _Ｂ＝^ｏ _Ｂ×^ｏ _Ｂ 力制御ロボットの倣い動作においては、第１図（ａ）
の制御指令生成手段15によって出力される力と位置の制
御指令、すなわち目標力と目標位置が力・位置制御手段
16に与えられロボットの制御が行われるが、まず目標力
_ｒの発生について説明する。倣い動作の始点P_Bで対象
物に対して設定した力F_r（スカラ量）を発生する時に
は、第３図（ｂ）に示すように力の大きさがF_rで、その
方向が_Ｂの力を発生すればよいことになる。従って設
定力ベクトル^ｏ _ｒは式で求められた^ｏ _Ｂを用いて
次式で与えられる。

^ｏ _ｒ＝F_r・^ｏ _Ｂ 次に始点P_Bでのロボットの移動目標位置は、始点P_Bか
らの相対位置指令として与えられる。始点P_Bからの移動
方向は式の移動方向ベクトル^ｏ _Ｂで与えられてお
り、これを用いて相対位置指令は次式で与えられる。

^ｏ _ｒ＝α・^ｏ _Ｂ ここでαは、第10図で説明したように、倣い座標系の
修正を行うべき距離に相当する適当な定数である。この
倣い座標系の変更の条件として、 （１）一定時間経過した時、 （２）対象物から受ける反力が設定力からある程度以上
にずれた時、 （３）ロボットが一定距離を移動した時、 等があげられるが本実施例では（１）の条件、すなわち
一定時間経過した時に倣い座標系の修正を行うものとす
る。なお（２）、（３）の条件においても発明の原理は
（１）と全く同一である。

第２図において倣い動作の途中点、例えばP₁における
法線ベクトル_１および移動方向ベクトル_１の設定
法、ならびに目標力_ｒ、目標移置の設定は始点P_Bに
おけると全く同様に行われる。すなわちから式を用
いて法線ベクトル_１、移動方向ベクトル_１が求めら
れ、これらを用いて設定力ベクトルと相対位置指令が
、式を用いて求められる。

本実施例では、上述のようにロボットのハンドと対象
物の接点における法線ベクトルと、接触点から倣い作
業の終点方向の単位ベクトルをとして接平面上のベク
トルが求められるが、特殊な場合としてベクトルと
とが一致する場合がある。第４図はそのような場合を
示す。同図において、接触点P₂における法線ベクトル
_２と終点P_E方向の単位ベクトル_２とが一致している。
この場合にはベクトル_２と_２との外積は求められ
ず、従って式を用いて接平面上のベクトル_２を求め
ることができない。そこで、接触点P₂に近い以前の接触
点P₁におけるハンドの移動方向ベクトル_１を利用し
て、接平面上のベクトル_２を次式によって求めること
にする。^ｏ _２ ＝（^ｏ _２×^ｏ _１）/|^o _２×^ｏ ₁| （ただし、^ｏ _２≠^ｏ _１） ここで点P₁は、例えばP₂での倣い座標系の修正の直前
の倣い座標系の設定点であるとする。移動方向ベクトル
^ｏ _２は２つの単位ベクトル^ｏ _２と^ｏ _２との外積と
して次式で与えられる。

^ｏ _２＝^ｏ _２×^ｏ _２ なお^ｏ _２＝^ｏ _１の時には式を用いて接平面上の
ベクトルを求めることができないが、これが成立するの
はロボットが対象物と衝突した場所であり、本発明では
このような場合を考えないことにする。

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

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

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

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

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

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

なお移動方向ベクトル算出部39にはオペレータから倣
い動作の始点_Ｂと終点_Ｅが指示される。ここで
_Ｂ、_Ｅの指示は制御指令生成部40を介して行われて
もよい。

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

さらに制御指令生成部40に力検出部29の出力である検
出力F_oが入力されるのは、後述のように対象物の反力を
検出することによって対象物への押付けが成功したか否
かを判別するためであり、また座標変換部30の出力であ
るハンドの現在位置X_oが入力されるのは、ハンドが倣い
作業の終点に達したか否かを判別するためである。

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

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

ここで行列Ｒは６行６列の正方行列であり、第１〜３
行の第１〜３列はＸ、Ｙ、Ｚの各軸の座標値ｘ、ｙ、ｚ
の変換に対応し、第４〜６行の第４〜６行はＸ、Ｙ、Ｚ
の各軸の回りの回転角度α、βγの変換に対応し、その
他の部分の要素は全て０である。

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

この場合、例えばネジ締め等のように押付け方向の軸
回りにトルクを与える場合には、第４行第４列のＳ＝１
とし、与えない場合にはＳ＝０とする。そして位置制御
部32内の選択行列演算部32bで用いられる選択行列Ｉ−S
_fは６次の単位行列Ｉから式のS_fを引いた行列として
与えられる。

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

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

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

第７図は倣い制御装置の実施例のシステム構成ブロッ
ク図である。同図においてホストコンピュータ37は法線
ベクトル算出部38、移動方向ベクトル算出部39、制御指
令生成部40、および通信制御部41を有する。ロボットコ
ントローラ42は、メモリ42a、通信制御部42b、および力
制御部、位置制御部、座標変換部、偏差部等42cを有
し、操作部27、位置検出部28および力覚センサ29aを介
してマニプレータ26の制御を行う。ホストコンピュータ
37とロボットコントローラ42はバス等の通信インタフェ
ースによって接続され、それぞれの信号の送受信のタイ
ミングを管理する通信制御部41、42bによって、メモリ4
2aと法線ベクトル算出部38、移動方向ベクトル算出部39
の間でデータ転送が行われる。

第８図は倣い動作の処理実施例のフローチャートであ
る。同図は未知形状の曲面を有する対象物への倣い動作
のフローチャートであり、ホストコンピュータ37の内部
の制御指令生成部40、法線ベクトル算出部38、移動方向
ベクトル算出部39、およびロボットコントローラで行わ
れる処理の流れを示している。なお第２図で示した倣い
動作の始点と終点はすでに与えられているものとし、倣
い動作の始点までのマニプレータの移動方法については
説明を省略する。また同図のなかで添字Ｂは始点を、添
字ｉは第ｉ番目の倣い座標設定時における処理を示して
いる。

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

ステップ48で反力のスカラ値が０でない場合には、
法線ベクトル算出部によってステップ49でセンサ座標系
の法線ベクトル^ｓ _Ｂが求められ、ステップ50で座標変
換行列^oA_sが算出され、ステップ51で式により基準座
標系で表示された法線ベクトル^ｏ _Ｂが算出され、それ
が制御指令生成部および移動方向ベクトル算出部に転送
される。

移動方向ベクトル算出部は、ステップ52で倣い作業の
始点と終点の位置ベクトル^ｏ _B,^o _Ｅより式で終点
方向の単位ベクトル^ｏ _Ｂを算出し、ステップ53で式
を用いて接平面上のベクトル^ｏ _Ｂを算出し、ステップ
54で式により移動方向ベクトル^oo_Bを算出し、それを
制御指令生成部へ転送する。

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

ステップ61で終点に達していない場合には、ステップ
62以降で新しい倣い座標系の設定とその座標系を用いた
倣い動作が繰返し行われる。すなわちステップ62でまず
反力が検出され、ステップ62で制御指令生成部により
のスカラ値が０か否かが判定される。のスカラ値が
０の場合には、何らかの原因でロボットと対象物とが離
れたことになるので動作を停止する。

ステップ63でのスカラ値が０でない場合には、ステ
ップ64から66で法線ベクトル算出部によってステップ49
から51と同様に法線ベクトルが算出され、ステップ67か
ら移動方向ベクトル算出部によって移動方向ベクトルが
算出される。ただし、この場合にはステップ52から54と
同様の処理が行われる前に、ステップ67でロボットの現
在の先端位置が検出され、それに基づいて移動方向ベク
トル算出される。続いて制御指令生成部によって、ステ
ップ71から73でステップ55から57と同様に目標力ベクト
ル、目標相対位置ベクトルおよび速度指令が生成され、
ステップ74でロボットコントローラによって倣い動作が
行われる。

制御指令生成部はステップ75で倣い動作の開始後一定
時間τが経過したか否かを監視し、経過していない場合
にはステップ74の倣い動作が続けられる。ステップ75で
一定時間が経過したと判定されると、ステップ76でロボ
ットの先端位置が検出され、ステップ77でその位置が終
点P_Eに一致するか否かが判定され、一致する場合には倣
い動作は終了し、次の動作に移行する。ステップ77で終
点に達していない場合には、再び新しい倣い座標系の設
定とその座標系の下での倣い動作がステップ62から続け
られる。以上の動作がステップ77でロボットが倣い動作
の終点に達したと判定されるまで繰返される。

〔発明の効果〕

以上詳細に説明したように、本発明によれば未知形状
の曲面を持つ対象物への倣い動作を自動的に行うことが
でき、従来ロボットのオペレータが行っていた倣い座標
系のロボットへの教示を行う必要がなくなり、オペレー
タの負担が大幅に軽減される。また対象物の曲面の接平
面と平行な移動方向ベクトルが算出されるために、対象
物への目標力と実際に対象物に対して発生される押付け
力とのずれが小さくなる。

【図面の簡単な説明】 第１図（ａ）は第１の発明の原理ブロック図、 第１図（ｂ）は第２の発明の機能ブロック図、 第２図は本発明における倣い座標系の設定法の実施例を
示す図、 第３図（ａ），（ｂ）は接触点における反力と設定力F_r
の算出方法を説明する図、 第４図はベクトルｎとＬが一致する場合のベクトルａの
設定法を示す図、 第５図は力制御ロボットの倣い制御装置の実施例の全体
構成のブロック図、 第６図は位置制御部と力制御部の詳細構成を示すブロッ
ク図、 第７図は倣い制御装置の実施例のシステム構成を示すブ
ロック図、 第８図は倣い動作の処理実施例のフローチャート、 第９図は倣い座標系を説明する図、 第10図は曲面を有する対象物と倣い座標系の関係を示す
図である。 １……基準原点、 ２、３、４、６……関節、 ５、26……マニプレータ、 ７……ハンド、 ８、29a……力覚センサ、 28……位置検出部、 29……力検出部、 32……位置制御部、 34……力制御部、 37……ホストコンピュータ．

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

Claims (2)

(57)【特許請求の範囲】
1. 【請求項１】ロボット（10）と、該ロボット（10）と作
   業対象物との間に働く力を検出する力検出部（11）と、
   前記ロボット（10）の位置を検出する位置検出部（12）
   とを有する力制御ロボットにおいて、 該力検出部（11）の出力によって前記ロボット（10）と
   作業対象物との接点における法線ベクトルを算出する法
   線ベクトル算出手段（13）と、 該法線ベクトル算出手段（13）と、前記位置検出部（1
   2）との出力により、前記ロボット（10）の移動方向ベ
   クトルを算出する移動方向ベクトル算出手段（14）と、 該移動方向ベクトル算出手段（14）、および前記法線ベ
   クトル算出手段（13）の出力により、前記ロボット（1
   0）への力・位置指令を出力する制御指令生成手段（1
   5）と、 該制御指令生成手段（15）と、前記力検出部（11）、お
   よび位置検出部（12）の出力により、前記ロボット（1
   0）の力と位置とを制御する力・位置制御手段（16）と
   を備えたことを特徴とする力制御ロボットの倣い制御装
   置。
2. 【請求項２】力制御ロボットの倣い制御装置において、 該ロボットと作業対象物との間で検出された力から該力
   と逆方向の単位ベクトルを求め（20）、 該逆方向単位ベクトルを基準座標系記述に変換して、法
   線ベクトルを求め（21）、 前記ロボットの現在位置から作業終了位置への方向の単
   位ベクトルを求め（22）、 該２つの単位ベクトルとの外積から前記ロボットと
   対象物の接平面上の単位ベクトルを求め（23）、 該ベクトルと前記ベクトルの外積から、前記ロボッ
   トの移動方向の単位ベクトルを求め（24）、 ベクトルとを設定し直しながら未知形状の曲面を持
   つ対象物の表面上で倣い動作をする（25）ことを特徴と
   する力制御ロボットの倣い制御方法。