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

Abstract

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

Description

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

〔産業上の利用分野〕

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

〔従来の技術〕

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

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

第１１図は倣い座標系の説明図である。同図において、
例えばロボットの本体底面の中心Ｏを基準座標系の原点
１とし、基準座標軸Ｘ、、Ｙｏ　。

およびＺｏがとられる。そして座標軸Ｚｏ上に関節２と
３が、また関節３と４の間のアームを含むマニプレータ
５が、関節４の先の関節６とハンド７の間に力覚センサ
８が設けられ、ハンド７が対象物９に接した状態が示さ
れている。ハンド７と対象物９との接触点が倣い座標系
の原点Ｏｈｌ、接触点における対象物の内向き法線方向
が座標軸Ｘ−１ハンドの移動方向がＹ。、２つの座標軸
Ｘ。＋ＹＷえられる。

このように倣い座標系Ｏ８−Ｘｗ、Ｙｗ、Ｚｗは対象物
に対するロボット先端部（ハンド）の位置、および姿勢
により決定される座標系であって、単位ベクトルｎは対
象物へ力を加える場合の押付ａ＝ｎＸ。

ここで３つの単位ベクトルｎ、ｏ、およびａは基準座標
系ＯＸｏ、Ｙｏ、Ｚｏで表示することも可能であり、一
般に次のような式で与えられる。

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

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

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

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

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

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

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

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

第１図において、倣い座標系設定手段１６はロボット１
０の先端と作業対象物との接触点における対象物表面の
法線方向の単位ベクトルｎを、側段１６に新しい倣い座
標系の設定を指令する。

倣い座標系を設定する。倣い座標系設定手段１６には力
検出部１２と位置検出部１３との出力が入力され、設定
された倣い座標系は制御指令生成部１１に出力される。

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

座標系切替指令手段１８は曲率算出手段１７の曲率算出
結果に応じて、例えばメモリに格納されている曲率を座
標系切替時間との対応テーブル、または切替時間と曲率
との関数関係に基づいて倣い座標系の切替時間を求め、
その切替時間経過後に制御指令生成部１１を介して倣い
座標系設定子〔作　　　用〕 本発明においては、まず倣い作業の開始点において設定
される倣い座標系は、例えばオペレータから制御指令生
成部１１にあらかじめ与えられる切替時間が経過するま
で用いられる。そしてその時間が経過すると制御指令生
成部１１から倣い座標系設定手段１６に新しい倣い座標
系の設定が指令される。また曲率算出手段１７によって
、例えば座標系切替時点での作業対象物表面の曲率が計
算される。新しい倣い座標系、すなわち第２の倣い座標
系は曲率算出手段１７の算出した曲率に応じた切替時間
が経過するまで用いられ、その時間経過後に座標系切替
指令手段１８から制御指令生成部１１を介して再び倣い
座標系の再設定が倣い座標系設定手段１６に指令される
。これによって新しい倣い座標系の設定と曲率の算出が
繰り返される。

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

〔実　　施　　例〕

第２図は本発明の座標切替方式を用いる制御系の全体構
成ブロック図である。同図において、制御系は第１図の
ロボット１０に対応する制御対象１９、その操作部２０
、制御指令生成部２１、力検出部２２、位置検出部２３
、位置制御部１４に相当する位置制御手段２４、力制御
部１５に相当する力制御手段２５、倣い座標系設定手段
１６に対応する法線ベクトル算出部２６と移動方向ベク
トル算出部２７、曲率算出手段１７に相当する曲率算出
部２８、座標系切替指令手段１８に対応する座標切替装
置２９から戒り、座標切替装置２９は座標系の切替時間
を算出する切替時間算出部２９ａ、およびその算出結果
に応じて座標切替指令を出力するタイミングを生成する
タイ〔ング生成部２９ｂから構成される。

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

面上に倣い動作中の各点における移動方向ベクトルが存
在するように制御が行われる。

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

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

Ｆ＝　（ｆ、ｆ、ｆ、）’　　　　　　　　　■ここで
Ｓは反力Ｆが力覚センサ座標系Ｏｓ　　Ｘｓ。

Ｙｓ、Ｚｓで記述されることを示している。またここで
は力覚センサが検出するトルク成分についての発明は省
略する。

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

ここでベクトルＦの大きさは ｓＦ　１＝、（ｆｘ′＋ｆｙｚ＋ｆＪ）１′２　　　　
　　■で与えられる。

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

力覚センサ座標系の各座標軸Ｘｓ　、Ｙｓ　、Ｚｓの方
向の単位ベクトルを基準座標系で成分表示したここで、
例えば力覚センサ座標系の座標軸Ｘ、方向の単位ベクト
ルＯｎ　ｓの０は基準座標系で記述されていることを示
している。この時力覚センサ座標系から基準座標系への
変換行列’　Ａ　ｓは次式％式％ この座標変換行列を用いて基準座標系表示の法線’ｎｌ
　　＝　　’Ａｓ　　ｓｎｍ　　　　　　　　　　　　
　　■法線ベクトルは第２図の法線ベクトル算出部２６
によって算出される。また始点以外は接触点での法線ベ
クトルも同様に算出される。

トル０゜、と、求められた法線ベクトルＯｎ　ｓを用い
て、始点Ｐ、における移動方向ベクトルｏｓをは、 ００Ｆ＝’ｎｌ　　または　０ＯＰ＝　　Ｏｎ、　　　
■を満たさないものとする。移動方向ベクトル０゜トル
０゜と法線ベクトルＯｎＩ＋の威す平面上にあるベクト
ルである。このとき、倣い座標系の座標とＯＯＰを用い
て、 ’Ｏｎ　　＝　　’ａＢ　　Ｘ　　ｎ＊　　　　　　　
　　　　　　■で求められる。移動方向ベクトルは、第
２図の移動方向ベクトル算出部２７で算出される。

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

倣い動作中の任意の接触点Ｐｉでの法線ベクトルｎｉの
算出方法は、始点Ｐ＠での法線ベクトルの算出方法と同
様であり、次式で表される。ただし、点Ｐｉでの反力Ｓ
　Ｆ　、を、 ”Ｆ１＝　（ｆｘｔ　　ｆｖｉ　　ｆｚｉ）↑とする。

’ｎｉ　＝　　’Ａｓｉ　　Ｓｎ、　　　　　　　　　
　　　　　■ＯＡ　ｓｌは、点Ｐｔの力覚センサ座標系
から基準座標系への座標変換行列である。ここでこの変
換行列は力覚センサ座標系と基準座標系との位置関係に
よって変化するため、点Ｐｔでの変換行が用いられる。

ロボットの先端位置は、ベクトルａ方向にも位ｎ、０が
威す平面上にあるように制御される。したがって、対象
物の表面に描かれる軌跡は、始点で与えられたベクトル
ｎ５ｓＯ５の威す平面上のベクトルｏｉは、始点で求め
られたベクトルａ。

に垂直に与えられればよいことがわかる。また移動方向
ベクトルＯ東は法線ベクトルｎｉにも直交で与えられる
。

始点ｐＨでのロボットの進行方向は、始点Ｐ。

からの相対位置指令で与えられる。始点Ｐ１がらの移動
方向は、式■の移動方向ベクトルＤｏＩＩで与えられて
おり、。ＯＢを用いて目標位置を表す位ベクトルの１つ
であるａｌは、ベクトルＯｎＬ、を求められないが、こ
の場合にはＯｏｌとしてオペレータから与えられたＯｏ
Ｐを用いることにする。

次に倣い動作の制御方法を説明する。まず始点Ｐｓで、
対象物に対して設定された目標力Ｆ１を発生する時は、
力の大きさがＦ、、で、方向・向きとなる。ここでαを
あまり小さく選ぶと対象物表面が平坦な場合などは座標
系の切り替えが行われないうちにロボットが目標位置に
達して停止してしまうため、始点から光分離れた点が指
令されるようにその値を選ぶ必要がある。

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

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

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

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

対象物体は未知形状なので、次のサンプリングが行われ
る点までの曲率を、現在地点Ｐ１と、１サンプリング前
の点Ｐｉ−１の情報を用いて推定する。ただし、Ｐｉ　
％　Ｐｔ−を付近の対象物体の形状が、球で近似できる
と仮定する。このとき、球のルを、ｏｐｌ、。Ｐｉ−１
、点Ｐ１の法線ベクトルをＯｎｉとおいたときの点Ｐｉ
での曲率を求める。

点Ｐ、付近の形状は球で表すことができると仮定したの
で、曲率円の中心は法線ベクトルＯｎｉと線分ＰｉＰ＋
−ｉの垂直２等分の交点となる。点Ｐできる（ｒｉは、
点Ｐ、の曲率円の半径）。第５（’Ｐ＊　　　’Ｐｉ−
ｉ　　） ・　・　・＠ ・　・　・［株］ となる。

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

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

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

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

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

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

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

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

制御指令生成部２１は、タイミング生成部２９ｂが発生
した切替の制御指令を受けると、倣い座標系の切替や各
算出部の状態制御を行う。

第７図はロボットの倣い制御装置の実施例の全体構成ブ
ロック図である。同図において、ホストコンピュータ４
２を除く制御装置はロボットのマニプレータ３１、マニ
プレータの操作部３２、マニプレータの図示しないハン
ド先端位置を検出する位置検出部３３マニプレータの力
検出部３４、位置検出部３３の出力としてのマニプレー
タ３１の駆動モータの回転角度θＳをロボットのハンド
先端位置Ｘｏに変換する座標変換部３５、ホストコンピ
ュータ４２から与えられる目標位置と座標変換部３５の
出力との差をとる偏差部３６、偏差部３６の出力とホス
トコンピュータ４２から与えられる位置制御パラメータ
を用いて、位置制御方向の速度指令信号ＶＰを出力する
位置制御部３７、ホストコンピュータ４２から与えられ
る力指令と力検出部３４の出力Ｆｏとの差をとる偏差部
３８、偏差部３８の出力とホストコンピュータ４２から
与えられる力制御パラメータを用いて、力制御方向の速
度指令値Ｖｆを出力する力制御部３９、位置制御部３７
の出力ｖＰ、力制御部３９の出力Ｖｒ、ホストコンピュ
ータ４２から与えられる速度指令Ｖｏの和をとる加算部
４０．加算部４０の出力Ｖｔをマニプレータの各関節の
回転速度θに変換する逆ヤコビ変換部４１から構成され
ている。ここで逆ヤコビ行列Ｊ−１は微小時間に対する
ロボット手先の微小変位（速度Ｖ）とマニプレータの各
関節の微小角変位（角速度）θとを関係づける式％式％
［］ において、各関節の構造で決まるヤコビ行列Ｊの逆行列
である。

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

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

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

次にホストコンピュータ４２は法線ベクトル算出部４３
、移動方向ベクトル算出部４４、制御指令生成部４５、
および座標切替装置４６から戒る。

法線ベクトル算出部４３は力検出部３４の出力、すなわ
ち基準座標系に変換された検出力Ｆｏからマニプレータ
と対象物の接触点での法線ベクトル０ｎを真出し、これ
を移動方向ベクトル算出部４４と制御指令生成部４５に
出力する。

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

制御指令生成部４５は法線ベクトル算出部４３、移動方
向ベクトル算出部４４の出力、力検出部３４の出力であ
る検出力ＦＯ１および座標変換部３５の出力であるハン
ドの現在位置Ｘｏを用いてロボットコントローラに目標
位置Ｘｒ、力指令Ｆ、。

の制御指令と、倣い座標系の切替に伴う位置制御、力制
御パラメータを送信し、また法線ベクトル算出部４３と
移動方向ベクトル算出部４４へ状態制御の信号を発生す
る。

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

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

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

第８図は第７図における位置制御部３７と力制御部３９
の詳細構成を示すブロック図である。同図において、位
置制御部３７は偏差部３６の出力、すなわち基準座標系
で表された位置偏差を倣い座標系に変換する転置直交変
換行列（Ｒ”　）演算部３７ａ、選択行列（Ｉ　　Ｓｒ
）演算部３７ｂ、直交行列（Ｒ）演算部３７ｃ、および
位置フィードバックゲイン（Ｃ，）演算部３７ｄとから
威り、また力制御部３９は偏差部３８の出力である基準
座標系で表された力偏差を倣い座標系に変換する転置直
交変換行列（Ｒ７）演算部３９ａ、選択行列Ｃ８ｔ　）
演算部３９ｂ、直交行列（Ｒ）演算部３９ｃ、およびカ
フィードバックゲイン（Ｃ７）ベクトルｎ、ｏ、および
ａを用いて、倣い座標系（Ｘｗ　、　Ｙｗ　、　　Ｚｗ
　）からロボット基準座標系（Ｘｏ　、　Ｙｏ　、　　
Ｚｏ　）への座標変換に用いられるここで行列Ｒは６行
６列の正方行列であり、第１〜３行の第１〜３列はｘ、
ｙ、ｚの各軸の座標値ｘ、ｙ、ｚの変換に対応し、第４
〜６行の第４〜６列はｘ、ｙ、ｚの各軸の回りの回転角
度のα。

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

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

・　・　・　＠ また力制御部３９内のカフィードバックゲイン演算部３
９ｄで用いられるカフィードバックゲインｃｇは、基準
座標系に関して次式で与えられる。

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

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

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

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

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

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

ステップ６０で反力ＦがＯでない場合には、ステップ６
１でロボット先端位置０Ｐｌがメモリに記憶された後に
、法線ベクトル算出部によってステップ６２でセンサ座
標系の法線ベクトルＳｎ璽が求められ、ステップ６３で
座標変換行列’　Ａ　□が算出され、ステップ６４で基
準座標系で表示された法線ベクトルＯｎＢが算出され、
それが制御指令生成部および移動方向ベクトル算出部に
転送される。

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

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

制御指令生成部は、ステップ６８で［相］式により目標
力ベクトルＯＦ、を生成しロボットコントローラへ転送
し、ステップ６９で０式を用いて目標相対位置ベクトル
ＯＸ　、を生成してロボットコントローラへ転送する。

またステップ７０で、例えばロボットハンドを高速に動
かすための速度指令■ｏをロボットコントローラに与え
、ロボットコントローラはステップ７１で倣い動作を開
始する。

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

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

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

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

続いてステップ８２と８３で、制御指令生成部によって
ステップ６８と６９と同様にして目標力ベクトルと目標
相対位置ベクトルが生成され、ロボットコントローラに
転送される。ステップ８４で■、［株］式を用いて曲率
算出部によって曲率円の半径と曲率が算出され、ステッ
プ８５で切替時間算出部によって倣い座標系の切替時間
が算出され、タイミング生成部にその値が転送される。

そしてステップ８６でステップ７０と同様に速度Ｖｏが
指令され、ステップ８７で倣い動作が開始され、ステッ
プ８８でタイミング生成部から倣い座標系の切替指令が
出たか否かが制御指令生成部によって監視される。

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

〔発明の効果〕

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

【図面の簡単な説明】

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

Claims (1)

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