JP2737326B2 - ロボットの軌道探索方式 - Google Patents

Description

【発明の詳細な説明】 〔概要〕 ロボットの軌道探索方式に関し、 倣い動作中の未知形状の不連続面を持つ対象物の表面
からロボットが離れた時に、元の軌道への復帰を容易に
行うことを目的とし、 制御対象となるロボットの軌道探索方式において、ロ
ボットと対象物（11）に作用する力を検出する力検出部
と、ロボットの現在位置を検出する位置検出部と、前記
位置検出部の位置座標値に基づきロボットの位置を制御
する位置制御部と、前記力検出部により力に基づいてロ
ボットの力を制御する力制御部と、ロボットへ力・位置
指令及びパラメータの転送を行う制御指令生成部と、ロ
ボットと対象物の接点の法線ベクトルを算出し、倣い座
標系を算出する法線ベクトル算出部と、倣い座標系に沿
ってロボットの移動方向を算出する移動方向ベクトル算
出部と、対象物からロボットの先端が離れた時に元の軌
道への復帰を行う軌道探索部を備え、前記算出した倣い
座標系及び軌道に基づいて未知形状の不連続面を持つ対
象物の表面に、前記力制御部により一定の力を加えなが
ら倣い動作をするように構成する。

〔産業上の利用分野〕

本発明はロボットの力制御時において、対象物の表面
に沿って加工等を行う倣い作業のための倣い制御装置の
軌道探索方式に関する。

倣い制御装置は基本的に制御対象（ロボット）を作動
させる操作部と、制御対象の位置・姿勢等を検出する位
置検出部と、制御対象が受ける力の検出を行う力検出部
により構成され、曲面を有する対象物体の表面に沿って
制御対象を倣い動作させる装置である。従って、任意形
状の不連続面を有する対象物体の倣い動作中に、その表
面からロボットが離れた時に、表面への復帰動作を迅速
に行う必要がある。

〔従来の技術と発明が解決しようとする課題〕

ロボットの力制御時に倣い作業を行う際、ロボットの
先端と対象物の接触点における対象物表面の法線方向に
一致する押しつけ方向ｎと、倣い動作をしながら移動す
る移動方向ｏで決定される倣い座標系（O_W-X_WY_WZ_W）を
ロボットコントローラ（10）に与える必要がある。ま
た、対象物体が不連続面を有する場合に、ロボットの先
端が対象物表面から離れた時に元の軌道への復帰と、新
しい倣い座標系の生成が必要となる。

従来、このような制御を行う方法として、オペレータ
による教示方式や直角・矩形物体への倣い制御方式、等
が提供されている。

しかし、前者の方式ではオペレターによるロボットへ
の教示が必要なため操作が非常に煩雑である、後者の方
式では任意形状の対象物に対しては倣い動作ができな
い、等の問題があった。

本発明の目的は、任意形状の不連続面を有する対象物
体の表面に沿って倣い動作を行うときに、制御対象が対
象物体の表面から離れた時に、迅速に元の軌道に復帰
し、再度倣い動作を行うための軌道探索方式を提供する
ことにある。

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

第１図は本発明の原理構成図である。本発明は、制御
対象となるロボット（１）の力制御時における軌道探索
方式であって、ロボットと対象物（11）に作用する力を
検出する力検出部（６）と、ロボットの現在位置を検出
する位置検出部（５）と、前記位置検出部の位置座標値
に基づきロボットの位置を制御する位置制御部（３）
と、前記力検出部により力に基づいてロボットの力を制
御する力制御部（４）と、ロボットへ力・位置指令及び
パラメータの転送を行う制御指令生成部（７）と、ロボ
ットと対象物の接点の法線ベクトルを算出し、倣い座標
系を算出する法線ベクトル算出部（８）と、倣い座標系
に沿ってロボットの移動方向を算出する移動方向ベクト
ル算出部（９）と、対象物からロボットの先端が離れた
時に元の軌道への復帰を行う軌道探索部（10）と備え、
前記算出した倣い座標系及び軌道に基づいて未知形状の
不連続面を持つ対象物の表面に、前記力制御部により一
定の力を加えながら倣い動作をするようにしたことを特
徴とする。

〔作用〕

一般に、不連続面を有する対象物体に対して倣い動作
を行う場合に、ロボットは下記の２通りの動作状態とな
る。即ち、 （Ａ）不連続点においてロボットの先端と対象物体の表
面が接触している場合、 （Ｂ）不連続点においてロボットの先端と対象物体の表
面が離れている場合、 である。（Ａ）の場合には、対象物体が曲面のときの制
御方式と同様の倣い動作が可能である。また、（Ｂ）の
場合は、離脱点に戻って対象物との接触を回復し、再
度、軌道及び倣い座標系を設定し直す必要がある。そし
て、対象物との接触を回復した後の動作は（Ａ）の制御
と同様である。

そこで、まず、（Ａ）の場合について説明し、次に
（Ｂ）の場合について説明する。

（Ａ）接触が保たれている場合 第２図は倣い座標系の説明図である。倣い座標系O_W-X
_WY_WZ_Wは、対象物に対するロボット先端部の位置・姿勢
により決定される座標系である。ベクトル，，は
それぞれ倣い座標系の座標軸X_W,Y_W,Z_Wについての単位ベ
クトルである。は対象物へ力を加える時の押し付け方
向を示し、対象物の表面の法線ベクトルと同じ方向であ
る。は、と直交関係にあり、倣い動作時のロボット
の先端の移動方向を示す。は、，に直交するよう
に定められ、 ＝× で与えられる。

，，の基準座標系X_OY_OZ_Oについての成分表示を行
うと、 となる。ただし、Ｔは転置行列を示す。

第３図は対象物とマニプレーターの位置関係の説明図
である。ロボットは、対象物のある面上で倣い動作を行
っており、このとき、倣い座標系は第３の位置P₁の様に
与えられているものとする。図示のように、ベクトル
は面に垂直であり、ベクトル，は接平面上にある。
ロボットが倣い動作を行ってベクトルのの方向へ移動
すると、ロボットと対象物の接点での面の法線方向が位
置P₁での押しつけ方向と一致しなくなる（位置P₂）。
不連続面を有する対象物への倣い動作では、面の法線方
向と押しつけ方向が不連続点を境にステップ状に変化
するため、対象物に対して発生する押しつけ力も設定し
た値からステップ状にずれる。従って、位置P₂において
も正確な力を発生するためには、不連続点に達した瞬間
に位置P₁の倣い座標系を位置P₂の倣い座標系に切り換え
ればよい。倣い座標系を変更するか否かを判断するため
の条件は、曲面に対して倣いを行う場合には、 （１）一定時間（制御系のサンプリング周期等）を経過
したとき、 （２）対象物から受ける反力が設定力からある程度以上
ずれたとき、 （３）ロボットが一定距離を移動したとき、等がある。
不連続面に対して倣いを行う場合でも、接触が保たれて
いれば上記の条件は有効である。そこで、以下の説明で
は、（１）の条件に従って倣い座標を変更するものとす
る。また、（２），（３）の場合でも、基本的な原理
は、（１）と同一である。

本発明では、倣い動作時の各地点における移動方向ベ
クトルが、倣い動作の開始地点（以下、始点）の法線ベ
クトルとオペレータが動作の開始時に与えた移動方向ベ
クトルの２つのベクトルが成す平面上に常に存在するよ
うに制御を行う。

第４図は各点における倣い座標系の説明図であり、倣
い座標系とロボットの軌道がどのように変化するかを示
したものである。第１図、第４図を用いて、不連続面を
有する対象物へ倣い動作を行うための制御方法について
説明する。

〔１〕倣い座標系の算出 まず、第４図にある始点P_Bでの倣い座標系の算出方法
について説明する。

（１）始点での法線ベクトルの算出 始点P_Bでの法線ベクトル_Ｂを次のようにして求め
る。第５図（ａ）は、ハンドが対象物から受ける反力
と、力覚センサ座標系O_S-X_SY_SZ_Sの関係を示した図であ
る。力覚センサでは、反力のX_S,Y_S,Z_S方向のそれぞれ
の分力f_X,f_Y,f_Zが検出される。（以下、力覚センサの検
出するトルク成分についての説明は省略する。）反力^Ｓ
をベクトル表示とすると、（Ｓは力覚センサ座標系O_S
-X_SY_SZ_Sで記述されていることを示す。）^Ｓ ＝（f_Xf_Yf_Z）^Ｔ …（１） となる。力覚センサ座標系で表した法線ベクトル^Ｓ _Ｂ
は、^Ｓと逆向きのベクトルであり、成分表示すると、 ｜^Ｓ｜は、ベクトルの大きさ である。法線ベクトル^Ｓ _Ｂを基準座標系で表してみ
る。力覚センサ座標系の各座標軸X_S,Y_S,Z_Sの基準座標系
に対する単位ベクトルを^Ｏ _Ｓ，^Ｏ _Ｓ，^Ｏ _Ｓとする
（Ｏは、基準座標系で記述されていることを示す）。成
分表示すると、 である。このとき、力覚センサ座標系か基準座標系への
座標変換行列^OA_Sは、^O A_S＝（^On_S ^OO_S ^Oa_S） …（４） で与えられる。基準座標系で表した法線ベクトル^Ｏ _Ｂ
は、座標変換行列^OA_Sを用いて、^Ｏ _Ｂ ＝^OA_S ^Ｓ _Ｂ …（５） となる。法線ベクトルは、第１図の法線ベクトル算出部
７で算出される。また、始点以外の他の接触点での法線
ベクトルの算出も同様の操作で行われる。

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

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

（３）点P₁での法線ベクトルの算出 点P₁での法線ベクトル_ｉの算出方法は、始点P_Bでの
法線ベクトルの算出方法と同様であり、次式で表され
る。ただし、点P₁での反力^Ｓ _ｉを、^Ｓ _ｉ ＝（f_Xif_Yif_Zi)^T …（９） とする。

^OA_Siは、点P₁の力覚センサ座標系から基準座標系への座
標変換行列である。

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

〔２〕倣い動作の制御 第４図にある始点P_Bでの倣い動作の制御方法について
説明する。

（１）目標力_ｒの発生 始点P_Bにおいて、対象物に対して設定した力_ｒを発
生する時は、力の大きさがF_rで、方向・向きが_Ｂの力
を発生すればよい。従って、設定力ベクトル^Ｏ _ｒは、
（２）式で求めた^Ｏ _Ｂを用いて、^Ｏ _ｒ ＝F_r・^Ｏ _Ｂ …（14） で与えられる。

（２）目標位置の設定 始点P_Bでのロボットの進行方向は、始点P_Bからの相対
位置として指令される。始点P_Bからの移動方向は、式
（８）の移動方向ベクトル^Ｏ _Ｂで与えられており、^Ｏ
_Ｂを用いて相対位置を表すと、^Ｏ _Ｂ ＝α・^Ｏ _Ｂ …（15） となる。αは、２ページの倣い座標系を修正のための条
件（１）〜（３）から決まる適当な定数である。

目標力、目標位置の設定は第１図に示す制御指令生成
部７で行う。

（３）倣い動作の終了 次のような条件を満たした時に倣い動作を終了する。

１）オペレータから終了の指令があったとき ２）一定時間を経過した時 ３）ロボットの可動範囲を越えた時 式（12）は^Ｏ _Ｂ＝^Ｏ _ｉまたは^Ｏ _Ｂ＝−^Ｏ _ｉの
ときに成立しない。このとき、点P_iの移動方向ベクトル
にオペレータが与えた移動方向ベクトル_OPを用いると
進行方向が維持できる。

（Ｂ）対象物から離れた場合 第６図は軌道探索の説明図であり、対象物の断面を上
から見た図であり、ロボットが不連続点付近でどのよう
に動作するかを示したものである。第６図（ａ）（ｂ）
のように対象物の形状により軌道探索の方法が異なる。

第７図は軌道探索時の時間応答の説明図であり、第６
図（ａ）（ｂ）それぞれの軌道探索時の押しつけ力を示
したものである。t₀,t₁,t₂,t₃,t₄,t₅,t₆はそれぞれ、第
６図でロボットが位置P_i0,P_i1,P_i2,P_i3,P_i4,P_i5,P_i6，
に達した時刻である。以下、第6,7図と第１図の制御系
の原理ブロック図を用いて軌道探索の方法について説明
する。

〔１〕第６図（ａ）の場合 第ｉ番目の倣い座標系の設定点P_i（＝P_i0）では、前
述した（Ａ）の制御方法で倣い動作を行っているものと
する。第６図のようにロボットがP_i0→P_i1のように倣い
動作を行い時刻t₁に位置P_i1に達したする。位置P_i1より
も右側では対象物は存在しないが、この時点では制御指
令が変更されていないのでロボットは_ｉ方向へ押しつ
け力を発生しようとして_ｉ方向へ移動する。また、
_ｉ方向への移動成分もあるため、結局、ロボットは位置
P_i1から位置P_i2へ進むように動作する。このように対象
物の形状とロボットの移動速度の関係で、不連続点にお
いて対象物から離脱する場合がある。このとき第１図の
力検出部で検出される_ｉ方向の押しつけ力_niは第７
図（ａ）のような時間応答を示す。つまり、対象物と接
触している間（t₀〜t₁）は、一定の力で押しつけ力を発
生しているが不連続点P_i1に達して対象物が無くなると
反力がなくなるため押しつけ力はゼロとなる。第１図の
制御指令生成部では、第７図（ａ）のような力の変化
（_ni＝０）を検出すると １）位置P_i1のハンドの座標値（不連続点）の記憶 ２）ロボットへの停止指令 を、直ちに行う。第１図の位置制御手段・力制御手段
は、２）の停止指令が送られてくると、ロボットの動作
を停止する。このとき、ロボットの位置はP_i2である。

次に、ハンドの姿勢を90°回転し、ベクトル_ｉの延
長上の位置P_i3へ移動する。ロボットの姿勢をP_i2からP
_i3にするには、現在のハンド座標系O_H-X_HY_HZ_HのZ_H軸回
りに、−π/2ハンドを回転すると良い（Z_H軸は紙面下向
き）。ハンド座標系は、ハンドのマニプレータに対する
位置、姿勢から決定される座標系であり、各座標軸X_H,Y
_H,Z_Hに対して、単位ベクトルｎ_Ｈ，，_{Ｈ Ｈ}が定め
られる。位置P_i2でのハンド座標系_H2，_HZ，
_H2を、位置P_i3でのハンド座標系_H3，_H3，_H3に変
化する変換式は、回転座標行列E^K（−π/2）を用いて次
式のように表される。

_i1，_i2，_i3をそれぞれ点Ｐ_i1,P_i2,P_i3の位置ベク
トルとすると、点P_i3は、ベクトル_ｉ上の点なので、_i3 ＝_i1＋ｒ・_ｉ …（17） ただし、ｒ＝_i2−_i1）・_ｉ で与えられる。ハンドの位置・姿勢をＰ_i3に変えた後、
ベクトル_ｉと逆方向へ押しつけを行う。ロボットは対
象物に接して設定力に達するまで対象物への押しつけを
行う。ロボットが対象物と接触する位置はＰ_i4（＝Ｐ
_i+1）である。このときの力応答は第７図（ａ）のよう
に、時刻t₄で、設定力を発生する。対象物と接触が回復
すると、（Ａ）の制御方法で倣い動作が可能となるの
で、再び、（Ａ）のアルゴリズムに戻る。

〔２〕第６図（ｂ）の場合 対象物の形状が第６図（ｂ）のような場合、〔１〕で
行ったように、ハンドの位置・姿勢をP_i3に変えた後、
ベクトル_ｉの逆方向に押しつけ力を発生しただけでは
面Ｂと接しないので、面Ｂに対する倣い動作を行うこと
ができない。そこで、P_i3から押しつけ動作を行った後
の制御を次のように行って、面Ｂへ倣い動作をする。

第６図（ｂ）からわかるように、面Ｂへの押しつけ力
を発生した場合の力の方向が、ベクトル_ｉの反対方向
の成分を持つのは、面Ｂが、不連続点を原点とし_ｉが
Ｘ軸_ｉがＹ軸を成す座標系の第４現象にある場合であ
る。P_i3からP_i4での押しつけ動作の際、ロボットの先端
が上記の第４象限に入るのは、先端が_ｉ−_ｉ平面を
通過する時である。従って、ロボットの先端が_ｉ−
_ｉ平面を通過しても押しつけ力が感知されないときは、
面Ｂが第４象限にあると考えられる。

不連続点から倣い動作を再開するため、ハンドの先端
が_ｉ−_ｉ平面を通過した瞬間にP_i3から行っていた
押しつけ動作を停止する。このときのロボットの先端位
置をP_i5とする。点P_i5は、点P_i1と一致する。次に、ロ
ボットの先端をP_i5に固定してハンドをベクトル_ｉの
左ねじの方向（第６図では反時計回り）に回転動作で押
しつけを行う。ロボットの対象物に接して設定力に達す
るまで対象物への押しつけを行う。ロボットが対象物と
接触する位置は、P_i6（＝P_i+1）である。このときの力
応答は第７図（ｂ）のように、時刻t₆で、設定力を発生
するような応答となる。対象物と接触が回復すると、
〔２〕と応用に（Ａ）の制御方法で倣い動作が可能とな
るので、再び、（Ａ）のアルゴリズムに戻る。

〔実施例〕

第８図は本発明の一実施例制御装置ブロック構成図で
ある。実施例に係わる制御装置は、本図に示すように、
マニプレータ21の制御を行う操作部22を備えている。こ
の操作部22はサーボ・モータ22aと、パワー・アンプ22b
と、D/Aコンバータ22cと、補償器22dとを有している。

また、制御装置はマニプレータ21の図示しないハンド
部の先端位置の検出を行う位置検出部26を備えており、
この位置検出部26はカウンタ及びエンコーダ26aとタコ
・メータ26bとを有している。

更に、制御装置はマニプレータ21のハンド部が受ける
力の検出を行う力検出部23を備えている。この力検出部
23は上記と同様の力覚センサ23aとハンド部座標系から
ロボット基準座標への座標変換部23bとを有している。

更にまた、制御装置は倣い動作時に、力検出部23によ
り検出された力_ｏ、設定力（力指令F_r）及び力制御パ
ラメータに基づいて力制御方向の速度指令信号V_fを発す
る力制御部24と、位置検出部26に検出された位置X_o、目
標位置X_r及び位置パラメータに基づいて位置制御方向の
速度指令信号V_Pを発する位置制御部27とを備えている。

第９図は位置制御部及び力制御部の構成図である。位
置制御部27の具体的構成は、転置直交変換行列（R^T）演
算部31と、選択行列（I-S_f）演算部32と、直交行列
（Ｒ）演算部33と、位置フィードバックゲイン（c_P演算
部34とを有する。一方、力制御部24は転置直交変換行列
（R_T）演算部38と、選択行列（S_f）演算部37と、直交行
列（Ｒ）演算部36と、力フィードバックゲイン（c_f）演
算部35とを有する。尚、符号24a及び27aは偏差部であ
る。

ベクトル，，を用いてロボット基準座標系（X_O
Y_OZ_O）から倣い座標系（X_WY_WZ_W）への座標変換する直交
座標変換行列Ｒは、次のように表される。

第２図のように、倣い座標系（X_WY_WZ_W）のX_W方向を力
制御方向、Y_W,Z_W方向を位置制御方向とすると、選択行
列演算部32,37の選択行列S_fは、 で与えられる。

力フィードバックゲインc_fは、基準座標系に関して、 で与えられる。

また、位置フィードバックゲインc_Pは同様にして、 で与えられる。

更に、制御装置は、力制御部24及び前記位置制御部27
から出力されら速度についての加算を行う加算部30b
と、加算された速度をマニプレータ21の各関節の角速度
θに変換する逆ヤコビ変換部30aとを有している。

座標変換部20では、位置検出部26で検出されたマニプ
レータの関節角θ_Ｓを基準座標系での位置X_Oに変換す
る。

ホストコンピュータ40は、（１）目標位置X_r、力指令
F_rの制御指令、（２）倣い座標系の切り換えに伴う位置
制御、力制御パラメータの送信、（３）法線ベクトル算
出部、移動方向ベクトル算出部の状態制御の信号の発生
を行う制御指令生成部40aと、マニプレータと対象物の
接触点の法線ベクトルを算出する法線ベクトル算出部40
bと、マニプレータと対象物の接触点での移動方向ベク
トルを算出する移動方向ベクトル算出部40cと、対象物
から離れた時に軌道探索を行う軌道探索部40dを有す
る。オペレータは、倣い動作のためのパラメータを制御
方向指令部に設定する。

第10図は実施例に係わるハードウェアのシステム構成
図である。ホストコンピュータ40は、制御指令生成部40
a、法線ベクトル算出部40b、移動方向ベクトル算出部40
c、軌道探索部40d、通信制御部40eを有する。ロボット
コントローラ10は、メモリ10a、通信制御部10bと、制御
部、座標変換部、偏差部等を有し、操作部22、位置検出
部26を介してマニプレータ21の制御を行う。ホストコン
ピュータと、ロボットコントローラは、バス等の通信イ
ンタフェースIFによって接続されており、それぞれの信
号の送・受信のタイミングを管理する通信制御部10b,40
eによって、メモリ10aと法線ベクトル算出部40b、移動
方向ベクトル算出部40c、軌道探索部40d間でデータが転
送される。

第11,12図は本発明の処理フローチャートである。

システム構成を第10図のようにした時に、不連続面を
有する対象物に対して倣い動作を行う場合の処理の流れ
であり、第11図は対象物との接触が保たれている場合、
第12図は対象物から離れた場合である。図中、添字Ｂは
始点を、添字ｉは第ｉ番目でのロボットと対象物との接
触点を表す。

まず、オペレータは移動方向ベクトル^Ｏ _OP、設定力
F_r、適当な値を持つ倣い座標系の切り換え時間ｒ、相対
位置を指定する係数α、始点で法線ベクトルを算出する
ために発生する力ベクトル_Ｂを設定する。制御指令生
成部では、始点で適当な力_Ｂを発生するために力指令
_Ｂ及び力制御パラメータを生成し、ロボットコントロ
ーラ10へ転送する。ロボットコントローラ10は、指令に
基づいて対象物に押しつけ動作を行う。このとき、検出
した反力が０のとき（ロボットと対象物が離れた時）
は、再度力ベクトル_Ｂの設定を行う。

次に、法線ベクトル算出部40bにおいて、検出した反
力から（２）式を用いて、^Ｓ _Ｂを求める。また、ヤ
コビ行列で力覚センサ座標系を計算し、（４）式から座
標変換行列^OA_Sを算出する。算出した、^OA_Sを用いて、法
線ベクトル^Ｏ _Ｂを求め、制御指令生成部40aと移動方
向ベクトル算出部へ^Ｏ _Ｂを転送する。

ここで、法線ベクトル^Ｏ _Ｂと移動方向ベクトル^Ｏ
_OPの方向が一致した場合は、再度パラメータの設定を行
う。

移動方向はベクトル算出部40cでは、（７）式を用い
てベクトル^Ｏ _Ｂを算出し、^Ｏ _Ｂと^Ｏ _Ｂから移動方
向ベクトル^Ｏ _Ｂを求める（（８）式）。

制御指令生成部40aは、式（14,15）と法線ベクトル^Ｏ
_Ｂ、移動ベクトル^Ｏ _Ｂから、目標力ベクトル
^Ｏ _ｒ、目標相対位置ベクトル^Ｏ _ｒを生成し、ロボッ
トコントローラへ転送する。ロボットコントローラは、
制御指令生成部40aから速度指令V_Oを受けると倣い動作
を開始する。

一定時間ｒが経過するとロボットの可動範囲にあるか
どうかを調べ、可動範囲内であれば、第１番目の接触点
での反応力を検出する。

第１番目の接触点でも、始点と同様の操作で、法線ベ
クトル算出部で法線ベクトルを算出する。また、法線ベ
クトル算出部では、（12）式を用いて移動方向へベクト
ルを算出する。

制御指令生成部40aは、法線ベクトル^Ｏ _１、移動方
向ベクトル^Ｏ _１から、目標力ベクトル^Ｏ _ｒ、目標相
対位置ベクトル^Ｏ _ｒを生成し、ロボットコントローラ
へ転送する。ロボットコントローラは、制御指令生成部
から速度指令V_Oを受けると倣い動作を開始する。

一定時間ｒが経過するとオペレータから終了指令が有
るかどうかを調べ、指令があれば倣い動作を停止し、次
の動作を行う。指令がなければ反力から第１番目の接
触点の法線ベクトル^Ｓ _２の計算を行う。このとき、検
出した反力が０のとき、つまり、ロボットと対象物が離
れたときは、第12図の処理へ移る。

倣い動作中に反力が０になったとき、軌道探索部では
ロボット先端位置を検出し、その座標値を記憶する。ま
た、制御指令生成部はロボットコントローラへ停止指令
を出し、ロボットの動作を直ちに停止する。つぎに、停
止したロボットの先端位置を検出し、P_i3の姿勢・位置
を算出し（16,17式）、位置P_i3へ移動する。

位置P_i3へ移動すると、制御指令部では、ベクトル
_ｉの逆方向への力指令、パラメータを生成して、−_ｉ
方向への押しつけ動作を行う。このとき、ロボットの先
端位置が_ｉ−_ｉ平面を通過した場合は、第12図の＊
６の処理に移る。ロボットの先端位置が_ｉ−_ｉ平面
を通過する前に反力の絶対値｜｜が正になれば、第11
図の＊５の処理に戻り、反力からの次の接触点の法線
ベクトル^Ｓ _i+1の計算を行う。

ロボットの先端位置が_ｉ−_ｉ平面を通過した場合
（＊６の処理）、まず、ロボットの動作を停止する。次
に、ベクトル_ｉの左ねじの方向への回転動作を、反力
の絶対値｜｜が正になるまで行う。反力の絶対値｜
｜が正になれば、第11図の＊５の処理に戻り、反力か
ら次の接触点の法線ベクトル^Ｓ _i+1の計算を行う。

以上の操作を、オペレータが終了指令を出すまで繰り
返す。

〔発明の効果〕

以上説明したように、本発明によれば、不連続面を有
する対象物へ対する倣い動作に、対象物からロボットが
離れが場合、元の軌道への復帰と、新しい倣い座標系の
生成を自動的に行うことができ、従来ロボットの操作者
が行っていた対象物体の変更や対象物の位置ずれに伴う
ロボットへの教示を行う必要がなくなり操作者の負担が
軽減される。また、軌道探索時に、接触力の有無により
対象物との接触点を調べるため、対象物が任意の形状で
あっても倣い動作を行うことができる。

【図面の簡単な説明】 第１図は本発明の制御系の原理構成図、 第２図は倣い座標系の説明図、 第３図は対象物とマニプレータの位置関係の説明図、 第４図は各点における倣い座標系の説明図、 第５図（ａ），（ｂ）は設定力の算出方向の説明図、 第６図（ａ），（ｂ）は軌道探索の説明図、 第７図は軌道探索時の時間応答のタイムチャート、 第８図は本発明の一実施例制御装置のブロック構成図、 第９図は位置制御部及び力制御部の構成図、 第10図は本発明のシステム構成図、及び 第11,12図は本発明の処理手順図である。 （符号の説明） １……制御対象、２……操作部、３……位置制御部、４
……力制御部、５……位置検出部、６……力検出部、７
……制御指令生成部、８……法線ベクトル算出部、９…
…移動方向ベクトル算出部。

───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 平３−104577（ＪＰ，Ａ) 特開 平１−229306（ＪＰ，Ａ)

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】制御対象となるロボット（１）の力制御時
   における軌道探索方式において、 ロボットと対象物（11）に作用する力を検出する力検出
   部（６）と、 ロボットの現在位置を検出する位置検出部（５）と、 前記位置検出部の位置座標値に基づきロボットの位置を
   制御する位置制御部（３）と、 前記力検出部により力に基づいてロボットの力を制御す
   る力制御部（４）と、 ロボットへ力・位置指令及びパラメータの転送を行う制
   御指令生成部（７）と、 ロボットと対象物の接点の法線ベクトルを算出し、倣い
   座標系を算出する法線ベクトル算出部（８）と、 倣い座標系に沿ってロボットの移動方向を算出する移動
   方向ベクトル算出部（９）と、 対象物からロボットの先端が離れた時に元の軌道への復
   帰を行う軌道探索部（10）を備え、 前記算出した倣い座標系及び軌道に基づいて未知形状の
   不連続面を持つ対象物の表面に、前記力制御部により一
   定の力を加えながら倣い動作をするようにしたことを特
   徴とするロボットの力制御時における軌道探索方式。