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

Abstract

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

Description

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

〔産業上の利用分野〕

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

〔従来の技術〕

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

Ｙｗ、Ｚｖをロボットのコントローラに与える必要があ
る。

第１１図は倣い座標系の説明図である。同図において、
例えばロボットの本体底面の中心○を基準座標系の原点
１とし、基準座標軸Ｘｏ、Ｙｏ、およびＺｏがとられる
。そして座標軸Ｚｏ上に関節２と３が、また関節３と４
の間のアームを含むマニプレータ５が、関節４の先の関
節６とハンド７の間に力覚センサ８が設けられ、ハンド
７が対象物９に接した状態が示されている。ハンド７と
対象物９との接触点が倣い座標系の原点○Ｗ、接触点に
おける対象物の内向き法線方向が座標軸ＸＷ、ハンドの
移動方向がＹｗ、２つの座標軸Ｘｗ。

Ｙｗに垂直な方向が座標軸Ｚｗとなり、各座標軸方向の
単位ベクトルがそれぞれｎ　Ｗ　＋　　ＯＷ　、および
ａ。で与えられる。

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

ａｗ　　＝ｌ’ｌ、　　ＸｏｌｌＩ ここで、３つの単位ベクトルｎい＋　　ｏＷ、およびａ
。は基準座標系ＯＸｏ　、Ｙｏ　、Ｚｏで表示すること
も可能であり、−ｉに次のような式で与えられる。ただ
しＴば転置行列を示している。

ｎＷ　−（ｎｘ　ｎｙｎ２）”　　Ｏ，−（ｏＸＯｙＯ
２）’ｒｎｙ　Ｏ２″″　ｎ２０ｙ ａ、＝　　ｎ、ｏ、−ｎＸｏ２　＝　（ａＸａｙａ２）
ＴｎＸ　Ｏｙ　−ｎｙ　ＯＸ 第１２図は曲面を有する対象物と倣い座標系の関係であ
る。ロボットは対象物９の側面で倣い動作を行っており
最初に倣い座標系は位置Ｐ１を原点として与えられてい
るものとする。この時ベクトルｎ、１は対象物９の側面
に垂直であり、ベクトルｏ８とａ。は接平面上にある。

ロボットが倣い動作を行ってベクトルｏＷの方向へ移動
すると、点Ｐ２においては面の法線方向ｎ９と押付は方
向が一致しなくなる。こうなると対象物に対してロボッ
トが発生する押付は力が設定した値からずれるために正
確な力で倣い動作を行うためには、点Ｐ、で設定された
倣い座標系を点Ｐ２での倣い座標系に切り替える必要が
ある。

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

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

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

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

第１３図において対象物体上の点Ｐｉで倣い座標系の単
位ベクトルイ８と１４とが設定され、盲□方向にオペレ
ータによって設定された力Ｆが加わっているものとする
。ロボットの先端は、次のサンプリング時間τまでの間
にベクトル０゜方向へは一定速度ＶでＶ・τまで移動し
、ベクトルｎ。

方向には、ロボットと対象物体の成すバネ系の釣り合い
を満たす位置まで変位する。このとき、ロボットの先端
は面Ａについては点ＰＡ、１＋ｌ、面Ｂについては点Ｐ
Ｒ，□１まで移動する。点ＰＡ、＋＋、点Ｐ　Ｒ＋　ｉ
□までのベクトルｎ。方向への変位量をそれぞれδヶ、
δ８とし、ロボットと対象物が威すバネ系のハネ定数を
ｋとすると点ＰＡ。

、＋＋、点Ｐ　Ｒ＋　＋＋１で対象物に働くベクトルｎ
、１方向の力はそれぞれＦ−にδ、　、Ｆ−にδおとな
る。　δ０〉δ８であり、図かられかるように面の曲率
が大きいと、設定力と対象物への押しっけ力とのずれも
大きくなる。すなわち、点Ｐ工で正確な押しつけ力を発
生するように倣い座標系を設定すると、過渡時の対象面
の曲率によって次のサンプリング点で発生する押しつけ
力に差がでることがわかる。

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

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

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

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

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

１ ２ 〔作　　　用〕 本発明においては、ある接触点で倣い座標系が設定され
てから次の倣い座標系が設定されるまでの時間、すなわ
ち座標系切替時間は一定であり、その値はオペレータか
ら与えられるものとする。

まず倣い作業の開始点においては、例えば法線ベクトル
算出手段１７によって算出された法線ベクトルが倣い座
標系の押付は方向の単位ベクトルイ。

とされる。そして倣い座標系の第３の単位ベクトルａ８
ばｎＷと例えばオペレータから与えられるロボット先端
の移動方向ベクトルＯ８，との外積として求められ、倣
い座標系の第２の単位ベクトル、すなわち移動方向ベク
トル０．１はａヮとｎ。との外積として求められる。

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

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

〔実　　施　　例〕

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

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

始点ＰＲでの法線ベクトルｎＩ＋は、始点においてロボ
ットのハンドが対象物から受ける反力Ｆから求められる
。第３図（ａ）はロボットのハンドが対象物から受ける
反力Ｆと力覚センサ座標系の０ｓｘＳ、ｙｓ、ｚＳの関
係を示す。力覚センサによれば反力Ｆの各センサ座標軸
方向のそれぞれの分力ｆＸ、ｆＹ、ｆ２が検出されるた
めに、反力″Ｆをベクトル表示すると次式で与えられる
。

５Ｆ−（ｆＸ　　ｆＶ　ｆ２）、”　　　　・・・・・
■ここでＳは反力Ｆが力覚センサ座標系○、−ＸＳｙ、
、ｚ、で、記述されることを示している。またここでは
力覚センサが検出するトルク成分についての説明は省略
する。

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

（ｒｘ　　ｆ、ｒｚ）”　　　・　・　・　・　・■こ
こでベクトルＦの大きさは Ｓｐ　１−（ｒｘ２＋　ｆ、２　＋　ｆ２２　）　Ｉ／
Ｚで与えられる。

力覚センサ座標系で表された法線ベクトルＳ　ｎＢを基
準座標系で表すためには、力覚センサ座標系から基準座
標系への座標変換行列が必要である。

力覚センサ座標系の各座標軸Ｘｓ　、Ｙｓ　、Ｚｓの５ ６ 方向の単位ベクトルを基準座標系で成分表示したものが
次式で与えられるものとする。

・　・　・　・　・■ ここで、例えば力覚センサ座標系の座標軸Ｘ。

方向の単位ベクトルｔａｎＳの０は基準座標系で記述さ
れていることを示している。この時力覚センサ座標系か
ら基準座標系への変換行列’　Ａ　３は次式で与えられ
る。

０Ａｓ−（０ｎ、。Ｏ３０ａ３）・・・・・■この座標
変換行列を用いて基準座標系表示の法線ベクトルＯｎＢ
は次式で与えられる。

０ｎＢ−０Ａｓ　ｓｎＢ　　　　　　・・・・・■始点
ＰＲにおける押付は方向ベクトル０ｎＨＢはこの法線ベ
クトルＯｎＩ］　と一致するように設定され、次式で与
えられる。

０→　　　　　。→ ｎ、Ｂ＝　　ｎＢ　　　　　　　　　−＠なお、法線ベ
クトルは第１図の法線ベクトル算出手段１７によって算
出される。

次に、オペレータから与えられた移動方向ベクトル０゜
Ｐと、求められた押付は方向ベクトル’ｎＷＢを用いて
、始点ＰＲにおける移動方向ベクトルＯ１，ＩＢが算出
される。ただし、ベクトルＯ８Ｐとムク１−ル’ｎ１ｌ
ｌＢは、 ＯＯＦ　””　’　ｎ　ＷＢまたばＱ。、−’ｎＷＢ・
　・　・　・　・■ を満たさないものとする。移動方向ベクトルｏ、ＩＩ］
は、押付は方向ベクトル’ｎＷＢに直交し、移動方向ベ
クトル７゜Ｐと押付は方向ベクトル’ｎＷＢの威す平面
上にあるベクトルである。このとき、倣い座標系の座標
軸を表す単位ベクトルの１つであるａＷＢは、’ｎＷＢ
とＯＯＰを用いて、０ａＷＢ−（’ｎ、、ｘ　０ｏｐ）
　／　ｌ　　’ｎＷＥＸ　ＯＯＰ・　・　・　・　・■ で表される。移動方向ベクトル’ＯＷＢは、ベクトル０
ｎＷＩ＋、’ａＷＢとの直交関係により、７ ８ １図の接線ベクトル算出手段１８によって算出さされる
。

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

点Ｐ８での法線ベクトルｎ、の算出方法は、始点ＰＢで
の法線ベクトルの算出方法と同様であり、ベクトルｎ、
は次式で表される。ただし、点Ｐ。

での反力Ｓ　Ｆ　、を、 ｓＦ、　−（ｒ、、　　ｆＹｉ　　ｆｚ＋）”とする。

（ｆＸｌ　　ｆＹ、　　ｆ、）Ｔ　　・　・　・　・　
・［相］０ｎｉ　　−’Ａｓｉ　　ｓｎ４　　　　　　
　　　　＋　　−、、・＠’ＡＳｉは、点Ｐ、の力覚セ
ンサ座標系から基準座標系への座標変換行列である。こ
の変換行列は力覚センサ座標系と基準座標系との位置関
係によって変化するため、点Ｐ、での変換行列が用いら
れる。

対象物体は未知形状のために次のサンプリングが行われ
る点が現在地点Ｐ、付近の曲率円上にあると仮定する。

現在地点Ｐ、付近の曲率を、現在地点Ｐ１と、１サンプ
リング前の点Ｐ、−４の位置を用いて求める。ただし、
Ｐｉ　、　　Ｐｉ−、付近の対象物体の形状が、球で近
似できると仮定する。このとき、球の半径をｒとすると
、曲率には１／ｒで与えられる。

第４図は、点Ｐ、での曲率算出方法の実施例を示したも
のである。第４図のように、点Ｐｉ＋Ｐ、の法線ベクト
ルをＯｎ　、とおいたときの点Ｐ。

での曲率を求める。点Ｐ、付近の形状は球で表すことが
できると仮定したので、曲率円の中心は法線ベクトル’
ｎｉ　と線分Ｐｉ　Ｐｉ−、の垂直２等分線の交点とな
る。点Ｐ、から曲率円の中心Ｒへ向かうベクトルは、法
線ベクトル’ｎｉを用いて、ｒｌ　。ｎ、と表すことが
できる（ｒｌは、点Ｐユの曲率円の半径）。第４図から
次の関係が成立する。

９ ０ 従って、曲率円の半径ｒ、は、 ・　・　・　・　・［相］ ただし、。ｎ、・　（’Ｐニー’Ｐｉ−１）はベクトル
の内積を示す。

のように求められる。弐〇から曲率ｋを求めると、 ・　・　・　・　・■ となる。

ロボットの先端位置は、ベクトルａ、１の方向へは移動
しないように位置制御されているので、先端位置が常に
ベクトルｎＷ、ｏｌ＋Ｉが威す平面上にあるように制御
される。したがって、対象物の表面に描く軌跡は、始点
で与えられたベクトルｎ　Ｗ　Ｂ　＋２．１Ｂの威す平
面上の曲線となる。このことから、点Ｐ１での接線ベク
トル０１もベクトルｎＷＢ＋　　ｏＷＢの成す平面上に
あるように求める。接線ベクトル０工は始点で求められ
たヘクトルａ、ｌＢに垂直に与えられればよい。接線ベ
クトルｏ１は、法線ベクトルｎ１にも直交するので、ベ
クトルＯｏ　、は、００１−（０ａＩＩＩＢ×０ｎ、）
／］　０ａ、１Ｂ×０ｎ工・・・・・［相］ のように表される。接線ベクトルは、第１図の接線ベク
トル算出手段１８によって算出される。

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

が或す曲率円上にあると考えて移動方向ベクトルを推定
する。

第１３図で示したように力の押付は方向を面の１ ２ 法線方向とすると、次のサンプリング点へ移動するまで
法線方向への制御指令は変更されないため、次のサンプ
リング点でベクトルｎ方向へδだけ変位したとき、接触
力は設定力からにδ　（ただし、ｋはロボットと対象物
が威すバネ系のハネ定数）だけ変化する。また、本発明
のように、移動方向ベクトルＯＷｉを直線Ｐ＝　、Ｐ１
＋＋上のベクトルとした場合も、押付は方向ベクトル０
ｎ、ｌｉが点Ｐ、＋１の法線ベクトル０ｎｉ□と一致し
ていないため接触力は設定力からずれる。そこで、まず
、ペクト→ ル’ｎｉ　、　　。Ｏｉを倣い座標系とするよりも、ベ
クトル’　ｎＩ’ｌｉ＋　　’　ＯＷｉを倣い座標系と
した方が接触力と設定力のずれが小さくなることを第６
図で示す。

第６図で、点Ｐ、はロボット先端の現在位置、・７８．
　・ｏ８はそれぞれ点Ｐ１の法線ベクトル、接線ベクト
ルであり、　　ｎＷｉ＋　　。Ｏヮエは、倣い座標系の
単位ベクトルである。また、点Ｐ、＋１は、次のサンプ
リング点であり、ベクトルＯｎ　、方向の点Ｐ、からの
変位をδ、法線ベクトルをＯｎ１＋Ｏｎｉ、、と　ｎＷ
ｉが威す角をα、。ｎｚ＋１　と０ｎ、が或す角をβ、
ベクトル０ｎＷｉ＋　　。６．、、を倣い座標系とした
ときの００，１方向への押付力をＦ、１８、ベクトルＯ
ｎ　、　、　　Ｏｏ工を倣い座標系としたときの’ｎｉ
＋１方向への押付力をＦＮｉとする。ベクトルＯｎｉ　
、　　００ｉを倣い座標系としたときは、前述したよう
に、変位δのためＯｎ　ｉ方向の力は、Ｆ　　→　　Ｆ
−ｋ　δ のように変化する。従って法線Ｏｎ　、。、方向の力Ｆ
　Ｎｉは、’ｎｉ。１と０ｎ１が威す角βを用いて、Ｆ
Ｎ□ｌ　−（Ｆ−にδ）　ｃｏｓ　　β　　・　・　・
　・［相］と表される。ベクトル０ｎＷ、＋　。Ｏｗｉ
を倣い座標系としたときの’ｎｉ＋１方向への押付力Ｆ
　ｗｉば、Ｏｎ工。１と０ｎ８１が威す角αを用いて、
Ｆ　ｗｉ　ｌ　＝　Ｆ　ＣＯＳ　α　　　　　　◆・・
・・０となる。角α、βの間には、 α十（π−β）／２−π／２ 変形して、 α−β／２ のような関係がある。式［相］、Ｏ２ ［相］から、 ・　・［相］ Ｆ＞Ｆ□〉Ｆ旧 となることが分かる。したがって、点Ｐ、。１では、ベ
クトル　ｎｗｉ＋　　００ｗ１を倣い座標系とした方が
、接触力と設定力のずれが小さくなる。

第５図において、ロボットの先端位置がベクトルＯｎｗ
ｉ方向へ■。で進むとすると、サンプリング時間τ後の
位置は、ＩＶｏ　）τである。次のサンプリング点Ｐ１
＋１は曲率円上の点と仮定しているので、点Ｐ　ｉｌｌ
　の位置は、曲率円と、ＩＶ。

τを半径とする円の交点となる。従って、点Ｐ１．。

を、位置ベクトルＰ２、ベクトル０ｎ、、　　°Ｚ、を
用いて表すと、次式が設立する。

・［相］ は第５図で点０Ｐ、を通る曲率円の接線に点ｏ　Ｐ　、
。

から引いた垂線の足をＱ９点ｏｐ、。１を通る直径と曲
率円とのもう一方の交点をＳとするときΔｏｐ、、、３
　ｏｐ、とΔｏｐ、。、’Ｐ、Ｑとが相似となることか
ら求められる。［相］式から移動方向ベクトルは次式で
与えられる。

’０ｗ１＝に／ｌｋ ・　・　・　・　・［相］ 次に、移動方向ベクトル００８１に直交する倣い座標系
のもう１つの座標軸を表すベクトルである０マ、、を求
める。ベクトル０７８．は、点Ｐ、の法線ベクトル０ｎ
１、法線ベクトル０６　、と直交するように定められる
。従って、ベクトル０ａ８．はの位置ベクトルである。

［相］式の右辺の第２．３項で００．と０ｎｉの係数式
〇［相］を用いて、 ’ａ、、＝　ｃ’ｎ、Ｘ　００Ｈ・−・、、■のように
求められる。

６ 倣い座標系の押付方向ベクトル０ｎ８．は、式［相］。

■で算出されたベクトル００ｗ１ｔ　　’ａｗｉとの直
交関係から、次式で表される。

０ｎ、ｌ、＝　００．Ｉｉｘ　’ａｗｉ　　　　　・−
−・−＠以上の曲率、倣い座標系の計算は、第１図の位
置検出部１４で検出されたロボットの先端位置、法線ベ
クトル算出手段１７により算出された法線ベクトル、接
線ベクトル算出手段１８により算出された移動方向ベク
トルを用いて制御指令生成部１２内に設けられた曲率算
出手段１９及び倣い座標設定手段２０によって行われる
。

次に倣い動作の制御方法を説明する。始点ＰＥを含む任
意の接触点Ｐ、で、対象物に対して設定された目標力Ｆ
、を発生する時は、力の大きさがＦ、で、方向・向きが
ｎ、ｉの力を発生すればよい。

従って、設定力ベクトルＱＦｒは、０式で求められた０
ｎ８、を用いて、 ’Ｆ、＝Ｆ、　　・　’ｎ、ｌ、　　　　・・・・・０
で与えられる。

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

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

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

１）　オペレータから終了の指令があったとき２）　一
定時間を経過した時 ３）　衝突したとき、または、対象物から離れた７ ８ 時 ４）　ロボットの可動範囲を越えた時 第７図は力制御ロボットの倣い装置の実施例の全体構成
ブロック図である。同図において、倣い装置はロボット
のマニプレータ２１、マニプレータの操作部２２、マニ
プレータの位置検出部２３、マニプレータの力検出部２
４、位置検出部２３の出力としてのマニプレータ２１の
駆動モータの回転角度θ８をロボットのハンド先端位置
Ｘ０に変換する座標変換部２５、ホストコンピュータ３
２から与えられる目標位置と座標変換部２５の出力との
差をとる偏差部２６、偏差部２６の出力とホストコンピ
ュータ３２から与えられる位置制御パラメータを用いて
、位置制御方向の速度指令信号Ｖｐを出力する位置制御
部２７、ホストコンピュータ３２から与えられる力指令
と力検出部２４の出力Ｆ。との差をとる偏差部２８、偏
差部２８の出力とホストコンピュータ３２から与えられ
る力制御パラメータを用いて、力制御方向の速度指令（
Ｉｉ！Ｖ５を出力する力制御部２９、位置制御部２７の
出力Ｖい、力制御部２９の出力Ｖ５、ホストコンピュー
タ３２から与えられる速度指令Ｖ。の和をとる加算部３
０、加算部３０の出力Ｖｋをマニプレータの各関節の回
転速度θに変換する逆ヤコビ変換部３１から構成されて
いる。ここで逆ヤコビ行列Ｊ−１は微小時間に対するロ
ボット手先の微小変位（速度Ｖ）とマニプレータの各関
節の微小角変位（角速度）θとを関係づける式 ％式％［］ において、各関節の構造で決まるヤコビ行列Ｊの逆行列
である。

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

第７図において操作部２２はマニプレータ２１を駆動す
るサーボモータ２２ａ、パワーアンプ２２ｂ、Ｄ／Ａコ
ンバータ２２ｃ、補償器２２ｄを有し、また位置検出部
２３はエンコーダおよびカウンタ２３．ｉとタコメーク
２３ｂからなり、力検出部２４は力覚センサ２４　ａ、
およびセンサ座標系で表示された力覚センサ２４ａの出
力を基準座標系に変換する座標変換部２４ｂから戒って
いる。

そして位置検出部２３内のエンコーダおよびカウンタ２
３ａとタコメータ２３ｂから、操作部２２内の補償器２
２ｄに補償演算のための信号が出力され、さらにエンコ
ーダおよびカウンタ２３ａの出力、すなわち各関節の回
転角度θ５が力検出部２４内の座標変換部２４ｂと逆ヤ
コビ変換部３１に人力されている。

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

接線ベクトル算出部３４は、法線ベクトル算出部３３か
ら入力される法線ベクトル０ｎ等を用いて接線ベクトル
０ｏを算出し、制御指令生成部３５に出力する。

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

また制御指令生成部３５からロボットコントロ１ ２ −ラに与えられる信号は、偏差部２６に与えられる目標
位置Ｘｒ、位置制御パラメータとして位置制御部２７に
与えられる倣い座標系の各単位ベクトルと、後述する位
置フィートハックゲインＣｐ、偏差部２８に与えられる
力指令Ｆｒ、力制御パラメータとして力制御部２９に与
えられる倣い座標系の各単位ベクトルと後述するカフィ
ードハックゲインＣ２、および例えばハンド先端部を高
速に移動させるために加算部３９に与えられる速度指令
ｖ０がある。ここでＶ。の値はオペレータによってその
値が直接与えられる場合や、オペレータによって設定さ
れた移動距離ｌと移動時間ｔを用いてＶ０＝ｎ／ｌで与
える場合等がある。

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

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

第８図は第７図における位置制御部２７と力制御部２９
の詳細構成を示すブロック図である。同図において、位
置制御部２７は偏差部２６の出力、すなわち基準座標系
で表された位置偏差を倣い座標系に変換する転置直交行
列（Ｒ７）演算部２７ａ、選択行列Ｎ　−３ｆ　）演算
部２７ｂ、直交行列（Ｒ）演算部２７ｃ、および位置フ
ィードハックゲイン（Ｃｐ）演算部２７ｄとから成り、
また力制御部２９は偏差部２８の出力である基準座標系
で表された力偏差を倣い座標系に変換する転置直交変換
行列（Ｒ”　）演算部２９ａ、選択行列（Ｓ、）演算部
２９ｂ、直交行列（Ｒ）演算部２９Ｃおよびカフィード
バンクゲイン（Ｃｆ）演算３ ４ 部２９ｄとを有する。

ベクトルｎ、ｏ、およびａを用いて、倣い座標系（Ｘ、
、Ｙ、、Ｚえ）からロボット基準座標系（Ｘ、、Ｙ、、
Ｚ、）への座標変換に用いられる直交座標変換行列Ｒは
次のように与えられる。

・　・　・　・　・［相］ ここで行列Ｒは６行６列の正方行列であり、第１〜３行
の第１〜３列は、ｘ、　ｙ、　　ｚの各軸の座標値ｘ、
ｙ、ｚの変換に対応し、第４〜６行の第４〜６列はｘ、
ｙ、ｚの各軸の回りの回転角度α。

β、γの変換に対応し、その他の部分の要素は全て０で
ある。

第１１図のように、倣い座標系のＸ８方向を力制御方向
、Ｙ８およびＺ８方向を位置制御方向とすると選択行列
演算部３４ｃの選択行列Ｓ、は次式で与えられる。

そして、位置制御部２７内の選択行列演算部２７ｂで用
いられる選択行列Ｔ−３ｆは６次の単位行列Ｉから０式
のＳｆを引いた行列として与えられる。

位置制御部２７内の位置フィードバックゲイン演算部２
７ｄで用いられる位置フィートノ＼・ツクゲインＣｐは
、一般に基準座標系に関して次式で与えられる。

５ ６ ・　・　・　・　・［相］ また、力制御部２９内のカフィードバックゲイン演算部
２９ｄで用いられるカフィードバンクゲインＣｆは、基
準座標系に関して次式で与えられる。

・　・　・　・　・［相］ 以上のように、例えば位置制御部２７内では偏差部２６
の出力である基準座標系で表された位置偏差が転置直交
変換行列演算部２７ａによって倣い座標系に変換され、
それに選択行列ＩＳｒが掛けられて位置制御方向の成分
のみが取り出され、直交行列演算部２７Ｃによって再び
基準座標系に変換され、さらに位置フィードバックゲイ
ンＣｐが掛けられて位置制御方向の速度指令Ｖｐとして
加算部３０に出力される。

第９図は倣い装置の実施例のシステム構成ブロック図で
ある。同図においてホストコンピュータ３２は法線ベク
トル算出部３３、接線ベクトル算出部３４、制御指令生
成部３５、メモリ３８および通信制御部３９を有する。

ロボットコントローラ４０はメモリ４０ａ、通信制御部
４０ｂ、及び力制御部、位置制御部、座標変換部、偏差
部等４０ｃを有し、操作部２２、位置検出部２３、およ
び力覚センサ２４ａを介してマニプレータ２１の制御を
行う。ホストコンピュータ３２とロボットコントローラ
４０はバス等の通信インターフェイスによって接続され
、それぞれの信号の送受信のタイくングを管理する通信
制御部３９，４０ｂによって３８と４０２との間でデー
タが転送される。

制御指令生成部３５、法線ベクトル算出部３３、接線ベ
クトル算出部３４、曲率算出部３６および倣い座標設定
部３７の計算に必要なデータはメモリ上のデータが参照
される。

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

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

ステップ４６で反力Ｆが０でない場合には、ステップ４
７でロボット先端位置がメモリに記憶された後に、法線
ベクトル算出部によってステップ４８で■式によりセン
サ座標系の法線ヘクトルＳｎＢが求められ、ステップ４
９で■式により座標変換行列０ＡＳが算出され、ステッ
プ５０で■式により基準座標系表示の法線ベクトルＯｎ
Ｂが算出され、メモリに格納される。またステップ５１
で倣い座標設定部により■式を用いて押イ」方向ベクト
ル０ｎい８が算出され、メモリに格納される。

算出されたヘクトル’ｎＷＢがオペレータにより与えら
れた移動方向ベクトルとステップ５２で比較され、両者
が一致（逆方向を含む）する場合にはステップ４１のパ
ラメータ設定からの処理が繰９ ０ り返される。両者が一致しない場合にはステップ５３．
５４で接線ヘクトル算出部によって■式を用いてヘクト
ルＯａ　、、■式を用いて移動方向ベクトル００オ８が
算出され、ともにメモリに格納される。

制御指令生成部は、ステップ５５で０式により目標力ベ
クトルｏＦｒを生威しロボットコントローラへ転送し、
ステップ５６で［相］式を用いて目標相対位置ヘクトル
ＯＸｒを生成してロボットコントローラへ転送する。ま
たステップ５７でζ例えばロボットハンドを高速に動か
すための速度指令Ｖｏをロボットコントローラに与え、
ロボットコントローラはステップ５８で倣い動作を開始
する。

制御指令生成部は倣い動作の開始後一定時間（τ）が経
過したか否かをステップ５９で監視し、経過するまでは
ロボットコントローラによりステップ５８の倣い動作が
続けられる。ステップ５９で一定時間が経過したと判定
された場合には、ステップ６０で制御指令生成部により
ロボット先端位置Ｘが検出され、ステップ６１でそれが
ロボットの可動範囲内にあるか否かが判定され、可動範
囲外のときには次の動作に移行する。

ステップ６１で可動範囲内にある場合にはステップ６２
以降で新しい倣い座標系の設定と、その座標系を用いた
倣い動作が繰り返し行われる。すなわちステップ６２で
まず反力Ｆが検出され、ステップ６３で制御指令生成部
によりＦがＯか、または極端に大きい（Ｆ＞１）か否か
が判定される。

Ｆが０の場合には何らかの原因でロボットが対象物から
離れたことに、また極端に大きい場合には、例えば壁に
衝突したことになるので動作を停止する。

ステップ６３でＦが０でなく、また極端に大きくもない
場合には、ステップ６４でロボット先端位置がメモリに
格納された後に、ステップ６５から６７でステップ４８
から５０と同様に法線ベクトル算出部によって法線ベク
トルが算出され、その結果がメモリに格納される。次に
ステップ６８で曲率算出部によって曲率円の半径の算出
とそのメモリへの格納が行われ、ステップ６９で法線べ
１ ２ クトル算出部によって接線ベクトルの算出とメモリへの
格納が行われる。続いて倣い座標設定部によりステップ
７０で移動方向ヘクトル、７１でベクトルａＷｉ、ステ
ップ７２で押付は方向ベクトルｎ、ｉが算出され、移動
方向ヘクトルと押付は方向ベクトルはメモリに格納され
る。

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

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

〔発明の効果〕

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

【図面の簡単な説明】

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

Claims (1)

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