JPH03184786A - ロボットの軌道生成方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔概　要〕 ロボットの力制御において、対象物の表面に沿って加工
等を行う倣い作業のための倣い制御装置の軌道生成方式
に関し、 未知形状曲面を持つ対象物の表面にある一定の力を加え
ながら倣い動作を行う力制御ロボットの軌道生成を容易
に行うことを目的とし、制御対象となるロボットの力制
御時における軌道生成方式において、ロボットと対象物
（１１）に作用する力を検出する力検出部と、ロボット
の現在位置を検出する位置検出部と、前記位置検出部の
位置座標に基づいてロボットの位置を制御する位置制御
部と、前記力検出部により検出した力に基づいてロボッ
トに与える力を制御する力制御部と、ロボットへ力・位
置指令及び各種パラメータの転送を行う制御指令生成部
と、ロボットと対象物の接点の法線ベクトルを算出し、
かつ倣い座標系を算出する法線ベクトル算出部と、倣い
座標系に沿ってロボットの移動方向ベクトルを算出する
移動方向ヘクトル算出部を備え、前記算出した倣い座標
系に基づいて未知形状の曲面を持つ対象物の表面に、前
記力制御部により一定の力を加えながら倣い動作を行う
ように構成する。

〔産業上の利用分野］ 本発明は、力制御ロボットにおいて、対象物の表面に沿
って加工等を行う倣い作業のための倣い制御装置の軌道
生成方式に関する。

倣い制御装置は基本的に、制御対象（ロボット）を作動
させる操作部と、制御対象の位置・姿勢等を検出する位
置検出部と、制御対象が受ける力を検出する力検出部に
より構成され、曲面を有する対象物体の表面に沿って制
御対象を倣い動作させる装置である。従って、上記の各
装置からのデータにより制御対象の軌道生成を迅速かつ
正確に行う必要がある。

〔従来の技術と発明が解決しようとする課題〕力制御ロ
ボットで倣い作業を行う際、ロボットの先端と対象物が
接触する接触点における対象物表面の法線方向に一致す
る押しつけ方向ｎと、倣い動作をしながら制御対象の移
動する方向０で決定される倣い座標系（Ｏｗ　　Ｘ　ｗ
　Ｙ　ｈ　Ｚ　ｗ　）をロボット制御装置に設定する必
要がある。

従来、この倣い座標系を設定する方法として、オペレー
タによる教示による方法が提案されている。

しかし、オペレータによるロボットへの教示は、対象物
表面が比較的平坦であればさほど複雑な操作は不要であ
るが、表面が複雑な曲面で構成されている場合には非常
に面倒な操作を伴うため、時間を要する。そのため、こ
の倣い座標系を設定する効率的方法が求められていた。

本発明の目的は、この倣い座標系の設定を容易に行い制
御対象の軌道生成を容易に行うことが可能なロボットの
力制御時における軌道生成方式を提供することにある。

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

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

〔作　用〕

第２図は倣い座標系（０ｗ　　Ｘ　ｗ　Ｙ　ｗ　Ｚ　ｈ
　）の説明図である。この座標系は対象物１１に対する
ロボット先端部１２（ハンド）の位置・姿勢により決定
される座標系である。なお、１３は力覚センサ、１４は
マニプレータである。ベクトルｎ、ｏ、ａはそれぞれ倣
い座標系の座標軸Ｘｗ　、　Ｙｔｎ　、　Ｚｕについて
の単位ベクトルである。

ヘクトルｎは対象物へ力を加えるときの押しつけ方向を
示し、対象物の表面の法線ベクトルと同じ方向である。

０はｎと直交関係にあり倣い動作時のロボット先端の移
動方向を示す。また、ａはｎ、ｏに直交するように定め
られ、ａ＝ｎＸｏで与えられる。

一方、座標（ＯＸｏ　、　Ｙｏ　、　Ｚｏ）は基準座標
系である。そこで、ベクトルｎ、ａ。

種糸について成分表示を行う。即ち、 ｎ　＝（ｎｘｎｙｎｇ）’ ｏ＝　（ｏ、ｏｙｏ、）’ ０を基準座 従って、倣い座標系（０，−Ｘ、４Ｙ、Ｚｌ、ｌ）から
基準座標系（０−Ｘ、、ｙｏ、　ＺＯ）に変換するとｎ
ａ、ｏは上式のように表すことができる。

第３図は基準座標系と倣い座標系におけるロボットの位
置関係の説明図である。ロボット（ハンド１２）は対象
物１１の表面で倣い動作を行っており、Ｐ、、Ｐ、はロ
ボットの位置である。Ｐｌにおいてベクトルｎは面に垂
直な押しつけ方向であり、ｏ、ａは接平面上にある。ロ
ボットが倣い動作を行ってベクトル０の方向に移動する
と、Ｐ２ではロボットと対象物の接点での面の法線方向
がＰでの押しつけ方向ｎと一致しなくなる。法線方向と
押しつけ方向が一致しないと、対象物に対して発生する
押しつけ力が設定値からずれる。従って、正確な力を発
生するためにはＰｌの倣い座標系をＰ２の倣い座標系に
切り換える必要がある。このような倣い座標系を変更す
るか否かを判断する条件として、 （１）一定時間経過したとき、 （２）対象物から受ける反力が設定力からある程度以上
ずれたとき、 （３）ロボットが一定距離を移動したとき、等がある。

なお、以下の説明では、（１）の条件に従って倣い座標
を変更するものとする。また、（２）、（３）の場合で
も基本的な原理は（１）と同様である。

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

第４図は倣い座標系とロボットの軌道の変化の説明図で
ある。また、第１．４、及び５図で示したような倣い動
作を力制御型ロボットで行うための制御系の原理構成図
である。第３．４図を参照しつつ曲面を有する対象物へ
倣い動作を行うための制御方法と軌道生成方法について
説明する。

倣梵生盗及生見班 まず、第４図図示の始点ｐＨにおける倣い座標系の算出
について説明する。

（１）始点における法線ベクトルｎ、の算出始点Ｐｌ、
での法線ベクトルｎ、ｌを次のようにして求める。第５
図（ａ）は、ハンドが対象物から受ける反力Ｆと力覚セ
ンサ座標系（０，−ＸＳＹｓＺＳ）の関係を示した図で
ある。力覚センサ１３では、反力Ｆのｘ、、ｙ、、ｚ、
方向のそれぞれの分力ｆＸ＋ｒｖ、ｒｚが検出される。

（以下、力覚センサの検出するトルク収骨についての説
明は省略する。）反力ＳＦをヘクトル表示すると、（こ
こで、Ｓは力覚センサ座標系（Ｏｓ　　Ｘ　ｓ　Ｙ　ｓ
　Ｚ　ｓ　）で記述されていることを示す。） ＳＦ＝　（ｆＫｆＹｆｚ）”　　　　　　　　　　−（
１）となる。力覚センサ座標系で表した法線ベクトルＳ
ｎＢは、　ＳＦと逆向きのベクトルであり、成分表示す
ると、 ・・・　（２） ここで１３Ｆ１は、ベクトルの大きさである。

法線ベクトルＳｎ、を基準座標系で表してみる。

力覚センサ座標系の各座標軸Ｘ、、Ｙ、、Ｚ、の基準座
標系に対する単位ベクトルを０ｎ、、　　。−；３０ａ
、とする（Ｏは、基準座標系で記述されていることを示
す）。成分表示すると、 である。このとき、力覚センサ座標系から基準座標系へ
の座標変換行列０Ａ、は、 ＯＡＳ　＝　（’ｎ、ｓ’ｏｓ’ａｓ）　　　　　　−
（４）で与えられる。基準座標系で表した法線ベクトル
Ｏｎ！ｌは、座標変換行列０Ａ、を用いて、’ｎ、Ｂ　
＝　ｏＡｓｓｎｓ　　　　　　　　　　　”・（５）と
なる。法線ベクトルは、第１図の法線ベクトル算出部８
で算出される。また、始点以外の他の接触点での法線ベ
クトルの算出も同様の操作で行われる。

（２）始点での移動方向ベクトルの算出部に、オペレー
タが与えた移動方向ベクトル０゜Ｐと、求めた法線ベク
トルＯｎ８を用いて、始点Ｐｌ＋における移動方向ベク
トルＯ６を算出する。ただを満たさないものとする。移
動方向ベクトルＯ，Ｉは、法線ベクトルＯｎ、に直交し
、移動方向ベクトル０゜、と法線ベクトルＯｎ、の或す
平面上にあるベクトルである。このとき、倣い座標系の
座標軸を表す単位ベクトルの１つであるａｌｌは、Ｏｎ
ＢとＯＯＰを用いて、 ・・・　（８） で表される。移動方向ベクトルＯｎ、は、ベクトル’ｎ
１’ａｍとの直交関係により、 Ｏｏ、＝　’ａ、Ｘ’ｎｓ　　　　　　　　　・＝　（
９）で求められる。移動方向ベクトルは、第１図の移動
方向ベクトル算出部９で算出される。

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

（３）点Ｐ、での法線ベクトルの算出 点Ｐ、での法線ベクトルｎ、の算出方法は、始点ＰＩｌ
での法線ベクトルの算出方法と同様であり、次式で表さ
れる。ただし、点Ｐ８での反力ＳＦ、を、 ’Ｆｔ　＝（ｆｘｔｆｖｔＬ＋）” とする。

・・・　（］０） Ｏｎ、　　＝　’Ａｓｔ’ｎｉ　　　　　　　　　　　
　　・・・　（１１）０Ａ５．は、点Ｐ、の力覚センサ
座標系から基準座標系への座標変換行列である。

（４）点Ｐ、での移動方向ベクトルの算出ロボットの先
端位置は、ヘクトルミ方向にも位置制御されているので
、先端位置が常にベクトルｎ、ｏが威す平面上にあるよ
うに制御される。したがって、対象物の表面に描く軌跡
は、始点で与えられたベクトルｎ１０８の成す平面上の
曲線となる。このことから、点Ｐ８での移動方向ヘク直
に与えられればよいことがわかる。また、移動するので
、ベクトル００．は、 ・・・　（１２） のように表される。倣い座標系の座標軸を表す単位ベク
トルの１つであるａｉ　は、ベクトルＯｎ　。

Ｏｏ　、との直交関係により、 Ｏａ、＝　Ｏｎ、×Ｏｏ、　　　　　　　　　・・・（
１３）で求められる。

倣□４■旨列性璽 第４図にある始点Ｐ、での倣い動作の制御方法について
説明する。

（１）目標力Ｆｒの発生 始点Ｐ８で、対象物に対して設定した力Ｆ１を発生する
時は、力の大きさがＦ、で、方向・向きがｎ、の力を発
生すればよい。従って、設定カベて、 で与えられる。

（２）目標位置の設定 始点Ｐヨてのロボットの進行方向は、始点Ｐ。

からの相対位置指令で与える。始点Ｐ、からの移動方向
は、式（９）の移動方向ベクトルＧｏ８で与えられてお
り、０ｏｌｌを用いて相対位置を表すと、 ＯＸＩ　＝α・００８　　　　　　　　　　・・・（１
５）となる。αは、２ページの倣い座標系の修正のため
の条件（１）〜（３）から決まる適当な定数である。

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

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

（１）オペレータから終了の指令があったとき（２）一
定時間を経過した時 （３）衝突したとき、または、対象物から離れた時 （４）ロボントの可動範囲を越えた時 代（１２）は０ａ、ｌ−００，または０ａｌｌ−−Ｏｎ
。

のときには成立しない。このとき、点Ｐ１の移動方向ベ
クトルにオペレータが与えた移動方向ベクトル０゜、を
用いると進行方向が維持できる。

〔実施例］ 第６図は本発明の一実施例倣い制御装置のブロック構成
図である。実施例に係わる制御装置は、図示のように、
マニプレータ２１の制御を行う操作部２２を備えている
。この操作部２２はサーボ・モータ２２ａと、パワー・
アンプ２２ｂと、Ｄ／Ａコンバータ２２ｃと、補償器２
２ｄとで構成される。

また、制御装置は、マニプレータ２１の図示しないハン
ド部の先端位置の検出を行う位置検出部２６を備えてお
り、この位置検出部２６はカウンタ及びエンコーダ２６
ａとタコ・メータ２６ｂとで構成される。

更に制御装置は、マニプレータ２１のハンド部が受ける
力の検出を行う力検出部２３を備えている。

この力検出部２３は上記と同様の力覚センサ２３ａとハ
ンド部座標系からロボット基準座標系への座標変換部２
３ｂとを備えている。

更に、また、制御装置は倣い動作時に、力検出部２３に
より検出された力Ｆ。、設定力（力指令Ｆ、）及び力制
御パラメータに基づいて力制御方向の速度指令信号■、
を発する力制御部２４と、位置検出部２６に検出された
位置Ｘ。、目標位置Ｘ、、及び位置パラメータに基づい
て位置制御方向の速度指令信号■、を発する位置制御部
２７とを備えている。

第７図は第６図の位置制御部２７の具体的構成図である
。図示のように、転置直交変換行列（ＲＴ）演算部３１
と、選択行列（ｒ−ｓｒ）演算部３２と、直交行列（Ｒ
）演算部３３と、位置フィードバックゲイン（ｃ＝　）
　演算部３４とを有する。一方、力制御部２４は転置直
交変換行列（Ｒ”　）演算部３８と、選択行列（Ｓ、）
演算部３７と、直交行列（Ｒ）ｉ算部３６と、カフィー
ドハックゲイン（ｃｆ）演ＩＥ部３５とを備える。尚、
符号２４ａ及び２７ａは偏差部である。

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

第２図のように、倣い座標系（Ｘｌ、ＩＹ、ＩＺｗ）の
Ｘｏ方向を力制御方向、ｙｌ、１．ｚｔ、、方向を位置
制御方向とすると、選択行列清算部３２　、３７の選択
行列Ｓｆは、 （１６） で与えられる。

カフィードバックゲインＣ２ は、 基準座標系に 関して、 （１７） で与えられる。

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

更に、制御装置は、力制御部２４及び前記位置制御部２
７から出力された速度についての加算を行う加算部３０
ｂと、加算された速度をマニプレータ２Ｉの各関節の角
速度θに変換する逆ヤコビ変換部３０ａとを備える。

座標変換部２０では、位置検出部２６で検出されたマニ
プレータの関節角θ、を基準座標系での位置Ｘｏに変換
する。

ホストコンピュータ４０は、（１）目標値ＷＸｒ、力指
令Ｆ１の制御指令、〔２）倣い座標系の切り換えに伴う
位置制御、力制御パラメータの送信、（３）法線ベクト
ル算出部、移動方向ベクトル算出部の状態制御の信号の
発生を行う制御指令生成部４０ａと、マニプレータと対
象物の接触点の法線ベクトルを算出する法線ヘクトル算
出部４０ｂと、マニプレータと対象物の接触点での移動
方向ベクトルを算出する移動方向ベクトル算出部４０ｃ
を備える。制御指令生成部４０ａには、オペレータによ
り倣い動作のためのパラメータが設定される。

第８図に、実施例に係わるハードウェアのシステム構成
を示す。ホストコンピュータ４０は、９Ｆ１１　Ｊ８指
令生収部４０ａ、法線ベクトル算出部４０ｂ、移動方向
ベクトル算出部４０ｃ、通信制御部４０ｄを備える。ロ
ボットコントローラ１０は、メモリ１０ａ、通信制御部
１０ｂと、制御部、座標変換部、偏差部等を備え、操作
部２２、位置検出部２６を介してマニプレータ２１の制
御を行う。ホストコンピュータ４０と、ロボットコント
ローラ１０は、バス等の通信インターフェースＩＦによ
って接続されており、それぞれの信号の送・受信のタイ
ミングを管理する通信制御部１０ｂ、４０ｄによって、
メモリ１０ａと法線ベクトル算出部４０ｂ、移動方向ベ
クトル算出部４０ｃ間でデータが転送される。

システム構成を第８図のようにした時に、曲面を有する
対象物に対して倣い動作を行う場合の処理の流れを、第
９図に示す。

第９図は、処理フローチャートであり、フローチャート
により前述のホストコンピュータ内の制御指令生成部４
０ａ、法線ベクトル算出部４０ｂ、移動方向ヘクトル算
出部４０ｃ、ロボットコントローラＩＯで行われる処理
の流れを示したものである。

尚、倣い動作のためのパラメータはオペレータによって
設定されるものとする。図中、添字Ｂは始点を、添字ｉ
は第ｉ番目でのロボットと対象物との接触点を表す。

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

次に、法線ヘクトル算出部４０ｂにおいて、検出した反
力Ｆから（２）式を用いて、５ｎｌＩを求める。

また、ヤコビ行列で力覚センサ座標系を計算し、（４）
弐から座標変換行列’　Ａ　ｇを算出する。算出したＯ
Ａ、を用いて、法線ベクトルＯｎ８を求め、制御指令生
成部４０ａと移動方向ベクトル算出部へＯｎ６を転送す
る。

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

移動方向ヘクトル算出部４０ｃでは、（８）式を用いて
ベクトルＯａｌ＋を算出し、ＯｎＢとＯａ６から移動方
向ベクトルＧｏ８を求める（（９）式）。

制御指令生成部４０ａは、式（１４Ｌ　（１５）と法線
ベクトル０ｎ８、移動方向ヘクトルＧｏ、から、目標力
ベクトル０Ｆｒ、目標相対位置ヘクトル０Ｘ、、を住成
し、ロボットコントローラへ転送する。ロボットコント
ローラ１０は、制御指令生成部４０ａから速度指令ｖ０
を受けると倣い動作を開始する。

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

第１番目の接触点でも、始点と同様の操作で、法線−・
クトル算出部４０ｂで法線ベクトルを算出する。また、
法線ベクトル算出部４０ｂでは、（１２）弐を用いて移
動方向ヘクトルを算出する。

制御指令生成部４０ａは、法線ヘクトルＯｎ　、、移動
方向ヘクトルＯｏ　、から、目標力ヘクトル０Ｆｒ、目
標相対位置ヘクトル０Ｘ、を生威し、ロボットコントロ
ーラへ転送する。ロボットコントローラは、制御指令生
成部４０ａから速度指令■。

を受けると倣い動作を開始する。

一定時間τが経過するとオペレータから終了指令が有る
かどうかを調べ、指令があれば倣い動作を停止し、次の
動作を行う。指令がなければ、反を計算する。以上の操
作を、オペレータが終了指令を出すまで繰り返す。

〔発明の効果〕

以上説明したように、本発明によれば、曲面を有する対
象物へ対する倣い動作を自動的に行うことができ、従来
ロボットの操作者が行っていた対象物体の変更や対象物
の位置ずれに伴うロボットへの教示を行う必要がなくな
り操作者の負担が軽減される。また、移動方向ベクトル
が常に同一平面にあるため、対象物への目標力と対象物
へ発生する力のずれを微小にできる。

【図面の簡単な説明】

第１図は本発明の制御系の原理構成図、第２図は倣い座
標系の説明図、 第３図は対象物とマニプレータの位置関係説明図、 第４図は各点における倣い座標系の説明図、第５図は設
定力の算出方向の説明図、 第６図は本発明の一実施例構成図、 第７図は位置制御部と力制御部の構成図、第８図は本発
明のシステム構成図、及び第９図（ａ）〜（Ｃ）は本発
明の処理手順図である。 （符号の説明） ｌ・・・制御対象、　　　　２・・・操作部、３・・・
位置制御部、　　４・・・力制御部、５・・・位置検出
部、　　　６・・・力検出部、７・・・制御指令生成部
、 ８・・・法線ヘクトル算出部、 ９・・・移動方向ヘクトル算出部、 １１・・・対象物、　　　　１２・・・ハント、１３・
・・力覚センサ、　　１４・・・マニプレータ。 特許出１頭人 冨士通株式会社 特許出願代理人

Claims (1)

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