JPS63149707A - ロボツトの速度指令生成方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔目　次〕 概要 産業上の利用分野 従来の技術 発明が解決しようとする問題点 問題点を解決するための手段（第１図）作用 実施例 （ａ）　　一実施例の説明 （第２図、第３図、第４図） （ｂ）　　他の実施例の説明 発明の効果 〔概　要〕 目標速度指令にセンサからの速度指令を合成して９合成
速度指令を生成し、センサーフィードバックでロボット
を制御する速度指令生成方法において、センナ出力を基
準座標系に変換した後、基準座標系でのパラメータを用
いて速度指令を生成し、基準座標系で目標速度と合成す
るととくよって、ロボットの姿勢に応じたパラメータ設
定を行なわなくて済むようにしたもの。

〔産業上の利用分野〕

本発明は、センサーフィードバックによってロボットの
運動制御を行なうため、目標速度指令とセンナ座標系で
のセンナ出力によシ得た速度指令とを合成して合成速度
指令を生成するロボットの速度指令生成方法に関する。

近年のロボット技術の発達に伴ない、１：ロボットに力
覚、視覚等のセンサを取付け、センサーフィードバック
によってロボットの外的環境を把握し。

適応制御することが行なわれている。

このようなセンサーフィードバック制御においては、セ
ンサの出力から修正速度指令を発生し。

目標速度指令に合成して、ロボットの運動を制御するも
のである。

この修正速度指令の発生には、速度変換パラメータを用
いて行なうことから、速度変換パラメータの教示が簡易
にできる方法が求められている。

〔従来の技術〕

第５図は従来技術の説明図である。

この図においては、多関節型口ボッ）Ｋセンサ１１とし
て力覚センサ１１ａ、視覚センサ１１ｂ。

距離センナ等の他のセンナ１１Ｃが設けられ、制御部で
あるプロセッサ２が演算によってマニピユレータ（０ボ
ツト）の関節座標系の速度θを発生し、マニピユレータ
の各関節を制御するものである０ プロセッサ２は、プルツクで示された演算を行ない、各
センナｌｌａ、ｌｌｂ、ＩＩＣのセンナ信号、即ち力信
号Ｆ　（Ｈ）　、視覚信号ｅ（Ｂ）、センナ信号Ｓ　（
Ｓ）が各々のセンナ座標系で入力される。

プロセッサ２は、先づセンサ信号によシ速度指令を演算
する。即ちセンサ信号Ｆ、　（Ｈ）　、　　ｅ　（Ｅ）
　、　８（８）に各センナ座標系で作業者が設定したパ
ラメータ（速度生成ゲイン）を用いて各センナ座標系テ
表示すし；ｂ　速ｌＫ　指令Ｖｔ　（Ｈ）　−Ｖｅ　（
Ｅ）　−Ｖｓ　（Ｓ）　を演算する。

次に、これらをマニピユレータの姿勢から求められるハ
ンド座標系へ各々変換し、全てハンド座標系表示Ｏ速度
指令Ｖｔ　（Ｈ）　−Ｖｅ　（Ｈ）　−Ｖｓ　（Ｈ）と
する０ 更に９作業者が与えた作業者基準の座標系表示の目標速
度ｖｄ（Ｒ）をｖｄ（Ｈ）としてハンド座標系に変換し
、すベテノ速度指令ｖｄ（Ｈ）　、　Ｖｌ　（Ｈ）　。

Ｖ＠（Ｈ）、　Ｖ、（Ｈ）をハンド座標系で加算し１合
成速度指令Ｖ　（Ｈ）を発生する◇ このハンド座標系の合成速度指令Ｖ　（Ｈ）を、マニピ
ユレータの関節座標系の速度すに次のようにして変換す
る。

即ち、ハンドの直交座標系の速度Ｖ　（Ｈ）と、マニピ
ユレータの関節座標系の速度すの関係は、マニピユレー
タのヤコビ行列Ｊを用いて次式で与えられる。

ｖ（ｕ）＝ｒ−４・・・・・・・・・・・・・・・・・
・・・・（１）従って。

ｅ＝ｓ″１・Ｖ　（Ｈ）　　　　　　　・・・・・・・
・・・・・・・・・・・・・・（２）によって関節速度
−を生成する。

このように、各センサｌｌａ〜１１Ｃのロボットに取付
けられる位置に起因した各センナ座標系でセンサ出力が
表示されるため、センサ出力から生成する速度指令と目
標速度の座標系を一致させて加算するようＫしている。

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

このような従来の速度指令生成方法においては。

各センナ信号から速度指令を演算するのに、各センサの
座標系で行っていることから１作業者は速度生成のパラ
メータを各々センナ座標系で入力教示しなければならな
い。

このため、第６図に示すように１作業者基準（ロボット
の設置点）の座標系ｘｏ、ｙｏ、ｚｏに対し、センサ座
標系ｘｓ、ｙｓ、ｚｓは異なるため、そのパラメータｋ
ｘ　、　ｋｙ　、　ｋｚもマニピユレータの姿勢を考慮
して教示する必要がアリ、その手間がわずられしいとい
う問題がある他に、マニピユレータの姿勢は一定でない
から、第６図囚、（ハ）に示すように、マニピユレータ
の姿勢に応じて変化するセンサ座標系毎のパラメータの
設定を要し、一層教示作業を面到なものとするという問
題が生じていた０ 本発明は、マニピユレータの姿勢を考慮せずに。

速度生成用パラメータの教示ができるロボットの速度指
令生成方法を提供することを目的とする。

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

第１図は本発明の原理説明図である。

図中、第５図で示したものと同一のものは同一の記号で
示しである。

本発明では、各センサｌｌａ〜１１Ｃのセンサ座標系の
出力Ｆ　（Ｈ）、　　ｅ　（Ｅ）　、　８　（Ｓ）を先
づ、基準座標系（例えば、ロボットの設置点の直交座標
系）表示Ｆ　（Ｒ）、　　ｅ　（Ｒ）、　　Ｓ　（Ｒ）
に座標変換する。

次に、基準座標系で設定したパラメータを用ｈテセンサ
出力Ｆ　（Ｒ）、　　ｅ　（Ｒ）、　　Ｓ　（Ｒ）から
速度指令Ｖｆ（Ｒ）　−Ｖｅ（Ｒ）　−Ｖｓ（Ｒ）　ｔ
”生成ｔ、、　基ｅ座ｓ系での目標速度指令Ｖ＋（Ｒ）
と合成し２合成速度指令Ｖ　（Ｒ）を生成する。

これをハンド座標系Ｖ　（Ｈ）　Ｋ座標変換し、関節型
マニピユレータであれば、前述の（２）式によシ。

関節速度６に変換する。

〔作　用〕

本発明では、センサ出力を基準座標系に座標変換後、速
度生成を行りているので、速度生成パラメータは作業者
の教示し易い基準座標系で行りておシ、シかもマニピユ
レータの姿勢に影響されない０ 従って速度生成パラメータの教示が容易で且つｗ＝ビニ
レータの姿勢に応じて個々に設定する必要がない。

〔実施例〕

（ａ）　　一実施例の説明 第２図は本発明の詳細な説明図である。

図中、第１図及び第５図で示したものと同一のものは同
一の記号で示してあシ、１２はアクチェエータでアシ、
モータで構成され、マニピュレータ１の各関節を駆動す
るものでアシ、この例で拡マニビエレータｌが６軸の多
関節凰で構成されていることから６軸分のアクチェエー
タを備えるもの、１３はエンコーダであシ、各軸のアク
チェエータ１２による関節変位θ（θ１．σ３．σ３．
θ１．θ。、σ、）を検出するもの、１４はサーボ制御
部であり、後述する演算プロセッサから与えられる各軸
の指令速度に追従する様に各軸のアクチェエータ１２を
サーボ制御するものである。

従りて、マニピユレータ（ロボット）１は６軸の多関節
型で構成され、ハンドと手首部の間に力センサ（力検出
装置）１１が設けられている。

２は前述の演算プロセッサでアシ、例えばＤＳＰ（デジ
タルシグナルプロセッサ）で構成され。

後述する如くホストプロセッサから最終目標関節変位０
０１　ｍ　目標速度指令ベクトルｖｄｉ　ｅ復帰ゲイン
α１１通過位置乃、目標速度指令ベクトルＶｔＨＯ内積
値の逆数（ＶａｌｌｌＶｄｌ）−’が入力され、マニピ
ユレータ１から現在の関節変位θを取り込み、力センサ
１１の検出出力ＦＨ（ハンド座標系での直交座標成分）
を取り込み２回避速度演算、現在位置演算、経路復帰速
度演算等を行ない、各サンプリングタイム毎に各軸の指
令速度をサーボ制御部１４に与えるものである。

２０は座標変換演算部であり、力センサ１１の出力（反
力）ＦＨを基準座標系の反力ＦＨに座標変換演算するも
の、２１は回避速度演算部であシ。

反力ＦＲを基準座標系表示のパラメータＫによって反力
方向の回避速度指令ベクトルＶｔ　（基準座標系）を演
算するもの、２２は現在位置演算部であり、各関節の現
在の変位θかも基準座標系でのハンドの現在位置Ｐ！Ｉ
を演算するもの、２３は経路復帰速度演算部２３でアル
、後述する如く、基準座標系表示の目標速度指令ベクト
ルＶｄｌを発するとともに、後述の如＜　＊　”ｌｉ　
ｅ復帰ゲインαｈ内積Ｏ逆数（ｖｄｔ　−ｖｄｔ　）−
”ｙ　ｉ布位置Ｐｓ、　Ａ過点ｒｔカ’）を基準座標系
表示の経路復帰速度指令ベクトルＶ、を演算し、出力す
るもの、２４は合成速度ベクトル演算部であシｅ　ｖｄ
ｔ　ｔ　Ｖｋ　＊　Ｖｆ　を加算して基準座標系での合
成速度指令ベクトル■！を演算するもの。

２５は座標変換演算部であシ、基準座標系での合成速度
指令ベクトルＶｉをハンド座標系の速度指令ベクトル隻
に変換するもの、２６は関節速度指令演算部であり、ハ
ンド座標系の速度指令ベクトルＶＨを前述の（２）弐に
よシ各関節の速度指令ベクトル６に変換するもの、２７
は関数発生演算部でｓｂ。

各関節の速度指令ベクトルｂの立上り、立下シを予定の
加速、減速カーブで規定して各関節の指令速度を演算す
るものである。

尚、各演算部２０〜２７は演算プロセッサ２が行なう演
算をブロックとして示したものである０３はホストプロ
セッサであシ、与えられたティーチングデータから通過
点ＰＩ　、　Ｐｌ＋１・・・Ｐｌｕｍｓ各経路の各棟路
度指令ベクトルｖｄｉ　ｅ　Ｖｄｌ＋ｓ・・・ｖ（１１
４１！Ｉ　ｍ各経路の復帰ゲインαｌ、内積の逆数（ｖ
ｄｌ・Ｖａｔγ１゜・・・（Ｖｌｍ・Ｖｄｍ　）−ｓを
演算し、これらと最終目標関節変位θ（１１を予じめ演
算プロセッサ２に定数としてセットシ且つマニピユレー
タの現在の関節変位θを取シ込んで各関節の現在位置を
把握するものであり、経路演算部３０を持つものである
０従って、この実施例では、ハンドの移動位置。

各指令速度は２作業゛者に理解、し易い直交基準座標系
（ＰｓｐＶａｔ　ｐＶｋ　ｅＶｔ　ｅａｔ　）　テ扱イ
、　とｔＬヲノｓ　ｙ　）’座標系に変換し・（ＶＨ）
、更に関節座標系での各関節速度すに変換することによ
って多関節マニピユレータを速度制御するものである。

これらの関係は、第３図の座標系関係説明図に示す如く
、ロボットの基準座標系（設置点）のＸ。

−Ｙｏ　−ｚｏ直交座標に対し、ハンド１０のハンド座
標系は直交座標としてベクトルｎ、ｏ、ａで表わされ、
１番目の関節山による座標変換行列ＡＩは。

関節占の変位らの関数として表わされる。この時。

基準座標系でのハンド１０の位置はＰｌｍ（Ｘｆｉ−Ｙ
ｎ　−２ｆｉ）であシ、後述する（３）式で示す如くＡ
・、Ａ１９人雪。

・・・、Ａ＠トマニピエレータの最後のアームからのハ
ンド１０の方向ベクトル百で表わされる。

そして、このロボットは、第４図に示す如く。

マニピユレータのハンド１０に設けられた力センサ１１
ａＫよりて障害物４の存在を検出し、力センサｌｌａの
出力ＦＨから回避速度ベクトルＶ、を発生し、目標速度
ベクトルＶｄｌとの合成によって障害物回避運動し、障
害物回避運動によって予定の経路ＲＰをはずれたものを
、目標速度ベクトルｖｄｉを変化せずに、経路復帰速度
ベクトルＶ、の発生によって障害物を回避しつつ予定の
経路ＲＰＫ復帰制御するものである。

次に、第２図実施例の動作について説明する。

ホストプロセッサ３は、ティーチングデータを受けると
１通過点ｈ・・・Ｐｌｕｍｓ　目標速度指令ペクトｋ　
Ｖ＋・・・Ｖｔ＋ｍｔ　内ＷＲＯ逆数（Ｗａｓ　・Ｖｄ
ｔ　）−”　・・・（ｖｄｍ”Ｖｄｎ　）−’を演算し
、これらと復帰ゲインα鳳、最終目標関節変位θｏ１を
演算プロセッサ２に定数としてセットし、起動する。

演算プロセッサ２では、現在位置演算部２２で各関節変
位θから基準座標系でのハンド１０の現在位置ｐＨを次
式によシ演算する。

尚ｇ　Ａ４ｇ　Ａｌ　＠・・・、Ａｓは各々基準座標系
から関節Ｊｌの座標系へ、関節Ｊ１の座標系からＪ２の
座標系へ、・・・関節Ｊ５の座標系から関節Ｊ６の座標
系への変換行列であシ、各関節角度θにより定義される
。

経路復帰速度演算部２３遺、与えられた目標速度ベクト
ルＶａｔを演算部２４へ送シ込むとともに。

次式によシ、現在位置Ｐ１から経路復帰速度ベクトルＶ
ｈを演算する。

先づ、第４図に示す如く２回避速度ベクトルによりて障
害物４を回避し、予定の経路ＲＰから位置ａにずれたも
のとする。位置ａから通過点ＰＩ→Ｐｌ＋１の予定経路
ＲＰ上に下した垂線の足の座標Ｐｒｎは。

である。

又ｔ　’にはＰｆｉと経路ＲＰとの距離！即ち、経路Ｒ
Ｐからのずれに比例して与えるから、復帰ゲインα蔦を
用いて２位置Ｐ、から経路方向のｈは。

Ｖｌ　＝　ａｌ・（Ｐ□−Ｐｎ） となる。

（５）式中、ａｌ　ｍ　Ｐｉ　＊　Ｖｄｉ　ｅ　（Ｖｄ
ｌ　ｅ　Ｖｄｉ　）−’は定数ア与えられているから、
現在位置Ｐｆｉを得るこ・とによって（５）式は簡単に
演算できる。

又、力センサｌｌａの出力（反力）ＰＨを取シ込み、座
標変換演算部２０で２次式によ〕基準座標系表示の反力
ＦＨに変換する。

次に反力ＰＲを回避速度演算部２１がパラメータＫを用
いて次式によシ演算して２反力方向の回避速度ベクトル
Ｖｆを求める。

Ｖｆ＝に−ＦＲ・・・・・・・・・・・・・・・・・・
・・・（７）合成速度ベクトル演算部２４は、各速度ベ
クトルｖａｔ　ｅ　Ｖｋ　ｔ　ｖｆを合成し、速度指令
ベクトルｈをえる。

この速度指令ベクトル−の演算は基準座標系で行なわれ
るため、ハンド座標系の速度指令ベクトルｖＨに座標変
換演算部２５で次式によシ得られる。

尚、（６）式、（８）式のｎ、ｏ、ａは、マニピユレー
タの各関節の構成で決定される前述の座標変換行列ん、
・・・んと、関節変位θで求められる。

更に、ハンド座標系の速度指令ベクトル−と関節速度６
の関係は、第（１）式で表わされ、特異点以外では、第
（２）式が成立するから、関節速度指令演算部２６は、
（２）式により関節速度ｄ（１９ｓ、１９ｓ・・・ｄ・
）を演算する。

この関節速度６は、関数発生演算部２７に与えられ、立
上〕、立下シを所定の加減速カーブで変換した各関節の
指令速度を演算し、サーボ制御部１４へ与える０これを
各サンプリングタイム毎に演算プロセッサ２は実行し、
リアルタイムにマニピユレータ１を速度制御する。

又、現在位置ａがＰＩ＋１に達すると１次の目標速度ベ
クトルＶ、Ｈ＋ｔ＊復帰ゲインα１十１・・・を用いて
演算を行なう。

このようにして、経路ＲＰでは、目標速度ベクトルＶｄ
１は一定とし、ｍ害物が存在しないとｔ　Ｖｆ＝ｖｋ＝
＝ｏであ少、障害物に当たシ、カセンサ１１ａによる反
力ＰＨが発生すると２回避速度ベクトルｖｆが演算によ
シ発生し、これとともに経路ＲＰからの位置ずれによシ
第（５）式によって経路方向の復帰速度ベクトル−が演
算され、これらの速度べ″クトルｖｄｉ　ｅ　’ｆ　ｐ
　Ｖｋの合成で速度制御が行なわれる。この場合、　ｖ
ｋを求める第（５）式は、単なるベクトル乗算でアシ、
平方根演算を含んでいないので。

リアルタイムＫ　Ｖｈを演算できる。

従って、力センサｌｌａの出力ＦＨによって回避速度ベ
クトルｖｆを発生し、障害物４の回避を行いつつ、経路
復帰速度ベクトルＶ−によって、目標速度ベクトルＶｄ
ｌを変えずに、経路復帰制御ができる。

この場合、力センサｌｌａの出力ＦＨはセンサ座標系、
この例ではハンドＩＯＫ取付けられているからハンド座
標系で表示され、これを（６）式によって基準座標系に
変換し、（７）弐によりて回避速度ベクトルｖｆを演算
することから、速度生成パラメータには２作業者の教示
し易い基準座標系で与えればよく、又マニピユレータの
姿勢に影響されないから１つのパラメータＫを教示すれ
ばよい。

Φ）他の実施例の説明 上述の実施例では、センナとして力センサを用いている
が、他の周知の種々のセンサにも適用出来、センナの数
も１つに限られず、複数であってもよい。

又、マニピユレータも関節型に限ることなく。

スカ？−Ｗ１．円筒座標型、直交座標型等周知のものに
適用でき、外的環境適応制御も障害物回避に限らず、対
象物への位置決め制御等周知のものく適用しうる。

以上本発明を実施例によシ説明したが９本発明は本発明
の主旨に従い種々の変形が可能であシ。

本発明からこれらを排除するものではない。

〔発明の効果〕

以上説明した様に２本発明によれば、センサ座標系のセ
ンナ出力を基準座標系に変換後、速度生成しているので
、速度生成パラメータは作業者の教示し易い基準座標系
で教示できるという効果を奏し、マニピユレータの姿勢
に影響されないため。

速度生成パラメータをマニピユレータの姿勢に応じて個
々に教示する必要もないという効果を奏し。

センサーフィードバック制御のロボットの教示を容易に
する０

【図面の簡単な説明】

第１図は本発明の原理説明図。 第２図は本発明の詳細な説明図。 第３図は第２図における座標系関係説明図。 第４図は第２図における障害物回避動作説明図。 第５図は従来技術の説明図。 第６図は従来技術の問題点説明図でらる０図中、１・・
・マニピユレータ（ロボット）。 ２・・・プロセッサ。 １１、ｌｌａ　〜ＩＩＧ・・・センサ。 警出願人　富士通株式会社

Claims (1)

1. 【特許請求の範囲】 センサ（１１）の出力を速度指令に変換し、目標速度指
   令と合成して、合成速度指令に基いてロボットを運動制
   御するロボットの速度指令生成方法において、 センサ座標系における該センサ（１１）の出力を基準座
   標系に変換し、 変換した該センサ（１１）の出力から該基準座標系で設
   定したパラメータを用いて速度指令を生成し、該生成し
   た速度指令と該基準座標系での目標速度指令とを合成し
   て、合成速度指令を生成することを特徴とするロボット
   の速度指令生成方法。