JPS63301305A

JPS63301305A - マニピュレ−タ型ロボットの軌道制御方法

Info

Publication number: JPS63301305A
Application number: JP13483787A
Authority: JP
Inventors: Akihiko Yabuki; 彰彦矢吹; Katsushi Nishimoto; 西本　克史; Yutaka Yoshida; 豊吉田
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1987-06-01
Filing date: 1987-06-01
Publication date: 1988-12-08

Abstract

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

Description

【発明の詳細な説明】〔概　要〕マニピュレータ型ロボットの軌道制御方法において、マ
ニピュレータへの目標速度指令を経由点間の速度ベクト
ルで与え、経路からのずれを修正する速度ベクトルをハ
ンドと経路との距離に比例して設定し、経由点間の速度
ベクトルおよび目標経由点の切り換えを行なうときは、
ハンドから経由点間の直線経路上に下した垂線の足が線
路延長線上で目標としている経由点を越えたときに行な
うようにしたものである。

〔産業上の利用分野〕

本発明はロボットの軌道制御方法に関し、特にマニピュ
レータ型ロボットのマニピュレータの軌道について無駄
な停止時間を除去し、作業効率の改善とエネルギ消費の
改善を図ったものである。

〔従来の技術および発明が解決しようとする問題点〕

第９図は従来のマニピュレータの軌道制御を説明する図
である。図において、ｌは組立等に用し＼る部品、２は
部品１が格納されているパーツマガジン又はトレイ、３
は部品１が組み付けられる他の部品、４は部品１が挿入
される穴である。Ｐｏは部品１が図示しないロボットハ
ンドによって把持される位置であり、作業の開始点であ
る。Ｐｌは部品１が穴４に挿入されてハンドが部品ｌを
離す位置であり作業の目標位置に相当する。又、Ｐｌお
よびＢ２は本作業の軌道における途中の経由点で、Ｐｌ
はＰｏの上方の点、Ｂ２はＰ、の上方の点である。

このような作業において、従来では、ハンドはＰｏで部
品１を把持し、制御部によってＰ、に向う軌跡を生成し
てＰ、まで位置制御により到達し、この点Ｐ１で一旦停
正した後、同様に制御部によって軌跡を生成してＰ、お
よびＰ、に到達し、Ｂ３で精密位置決めをした後、部品
１を穴４に挿入している。このようなマニピュレータの
軌道制御においてはＰｌおよびＢ２で停止して、各点で
次の点への軌跡を発生するための演算を停止している間
に行い、それが終了した後火の点ｐ２　、Ｂ３へ向って
いた。

しかしながら、ＰｌおよびＢ２は途中の経由点にすぎず
、停止する必要のない点でもある。また、大部分の場合
がその点で精密に位置決めされる必要がない。このよう
に従来は、本来の作業からすると停止する必要のない点
に停止しているためそれだけ経路の移動に時間を要し、
又、その位置で加減速を行なうためエネルギを余分に必
要とする、などからロボットにおける作業効率を下げる
といった問題を住じていた。

本発明の目的は、上述の問題点を解消することにあり、
目標点以外の経由点間の経路では速度制御により移動さ
せ、経由点からのずれを容認した滑らかな軌道をたどる
ことができるマニピュレータの軌道制御方法を堤供する
ことにある。

〔問題点を解決するための手段および作用〕第１図は、
本発明に係るマニピュレータ型ロボットの軌道制御方法
の原理図である。図において、Ｐｏは作業前の物品の位
置、Ｐ、は作業後の位置であり、ＰｌおよびＢ２は経由
点である。また、ＨｌおよびＨ２はハンドの異なる時点
での位置を示す。３次元座標ｘｙｚにおいて、ｐｏ　　
＋　ＰＩ　　＋Ｐ２の座標は各々Ｐ６　＝　（ｐ、、、
Ｐｏｙ、ＰＯ−）、ＰＩｍ（Ｐｌ、、Ｐｌｙ、Ｂ１ヨ）
　、Ｐｔ　＝　（Ｐｔ−。

Ｂ２９．Ｂ２−）で示され、Ｂ＋＝（Ｂ＋、＋ＢＩ９＋
Ｂ　、ｔ）はハンド位置Ｈ１から経路Ｐ。−Ｐ、上へ下
した垂線の足であり、Ｂｚ　＝　（Ｂ２−　、　Ｂｚ、
。

Ｂ□）はＨ２から経路Ｐ０−Ｐｌ上へ下した垂線の足で
あり、Ｂｚ−＝　（Ｂ□、＋　Ｂ　ｔ、ｙ　　＋　Ｂ　
ｔ□）はＨ２から経路Ｐ　Ｉ−Ｐ　を上へ下した垂線の
足である。また、Ｐ、からＰ、へ向かう速度をｖ０１＝
（Ｖｌｌｌｌｌ　　＋　ＶＬＩｙ　　＋　Ｖ６１１１　
）と設定し、ＰｌがらＰ、へ向かう速度をＶ＋ｚ＝　（
Ｖ＋ｔ＊　　＊　ＶＩ２ｙ　　ｌＶ＋ｚ＊　）　、Ｐｇ
からＰ、へ向がう速度をＶ２３＝（■！ｆｆ。＋　Ｖ！
３ｙ　　＋　７１１Ｍ　）と設定する。尚、がっこ内は
各速度の各座標成分を示している。

本発明の軌道制御方法ではハンド位置から経路上へ下し
た垂線の足が、経由点よりも経路延長上へ越えたときハ
ンドはその経由点を越えたと判定する。即ち、ハンドの
進行方向を速度のある座標成分の正負で決定し、経由点
の通過を垂線の足と経由点とにおける同じ座標系の大小
で判定する。

例えば、ハンドがＨｌにあるとき、Ｘ座標成分では■。

□〉０であり、Ｂ　＋＊＜　Ｐ　ＩＭなので、ハンドは
経路Ｐ、−Ｐ、を進行すべく速度Ｖ□を指令する。また
、ハンドがＨ２にあるとき、■。ＩＸ＞０でＢｚＸ＞Ｐ
Ｉｍなので、ハンドは経由点Ｐ、を通過したとみなし、
経路Ｐ１→Ｐ２を進行すべく速度■、を指令されると同
時に垂線の足は経路Ｐｏ−１１Ｐ２上に下したＢｚ１１
へ切り換える。いま、Ｖｌ！ｘ＝０のとき、通過の判断
はＸ座標成分に切り換へＢ□、とＢ２アと比較する。

〔実施例〕

第２図は本発明に係るマニピュレータ型口ポソトの軌道
制御方法を適用した装置のブロック図である。図におい
て、ブロックＡはロボット本体を示し、基本的機構とし
てハンドを移動させるマニピュレータＡ１と、位置座標
等を検出するエンコーダＡ２と、ハンド及びマニピュレ
ータの動作を制御するサーボ機構Ａ３とを有する。マニ
ピュレータＡ１は各関節ごとにエンコーダＡ２をもち、
サーボ機構Ａ３により速度制御される。

ブロックＢは制御のための演算を行なう高速演算プロセ
ッサ部であって、基準座標系での速度ベクトルをハンド
座標系の速度ベクトルに変換演算する座標変換演算部Ｂ
ｌ、後述するヤコビ行列により速度ベクトルを関節座標
系の速度ベクトルに変換演算する関節速度演算部Ｂ２、
関節の速度ベクトルに対し加減速パターンで修飾し、か
つ目標点と現在位置とによって目標点に近づいたことを
検出し、位置制御のための減速カーブを演算し、サーボ
機構Ａ３に与える関数発生演算部Ｂ３、を包含し、さら
にハンドの現在位置を演算するハンド位置演算部Ｂ４、
ホストプロセッサの経路生成部Ｃ１から与えられた通過
点座標と現在位置とを比較しハンドが指定された通過点
を通過したかを判定する経路判定部Ｂ５、軌道をはずれ
た後の復帰を制御する経路復帰速度演算部Ｂ６、および
速度ベクトルと力制御速度ベクトルを合成して合成速度
ベクトルを演算する合成演算部Ｂ７を包含する。

ブロックＣはホストプロセッサであり、例えばパーソナ
ルコンピュータで構成され、経路生成部Ｃ１においてロ
ボットの動きを指示するティーチングデータ（目標点、
途中の通過点等）を受け、各点間の速度ベクトルを演算
し、更に基準座標系で与えられた位置決め目標点を関節
座標系の目標座標に変換し、通過点、速度ベクトル、最
終目標座標を高速演算プロセッサＢに与えるものである
。

このような構成において、マニピュレータ先端のハンド
（図示せず）の移動位置（Ｈ）および移動速度（Ｖ）は
操作員に理解し易いように直交基準座標系で指令し、高
速演算プロセッサＢ内ではハンドに固定されたハンド座
標系の速度（Ｖ’　）で取り扱う。ここで、Ｈ，Ｖ、Ｖ
”等はすべてベクトル表示である。そして、ハンド座標
系が基準座標系に対して第４図に示す如くベクトル表示
ス。

ずで表わされるとき、ハンド座標系の速度■”は、によ
り求められる。π　？、７は、マニピュレータの各関節
の構成で決定される後述する関節座標変換行列Ａ８．・
・・、Ａ６による演算式（４）と、関節変位■および最
終関節座標がらハンド先端へ至るベクトルπとで決定さ
れ、（１）式は座標変換演算部Ｂ１で演算される。

速度■８と関節速度心との関係はＶ’＝Ｊ■　　　　　　　　　　　　・・・・・・　（
２）で表わされ、ここでＪはマニピュレータのヤコビ行
列である。（２）式から特異点以外では関節速度は ■−Ｊ　−Ｉ　Ｖ　Ｈ・・・・・・（３）で与えられ、
この演算は関節速度演算部Ｂ２で行われる。更に、関数
発生部Ｂ３によって介の加速、減速を計画されてサーボ
Ａ３へ出力される。

ホストプロセッサＣではティーチングデータ等によるコ
マンドを受けて経路生成部ＣＩにおいて経路演算を行い
、第１図に示すような経由点Ｐａ。

ＰＩ＋・・・　ｐ７を生成し、各経由点間を移動する速
度Ｖｏ＋　＊　Ｖｌｚ　＊　”’　＋　Ｖ　（ＩＩ−１
１１１を目標速度指令として高速演算プロセッサ４に出
力する。また、後述する経路復帰速度ゲインα。５．α
、、・・・。

α。−＋１を出力する。

ハンド位置演算部Ｂ４では次の（４）式で与えられるハ
ンドの位置Ｈを演算する。

ここで、盲は最終関節座標からハンド先端へ至るベクト
ルである。ハンド先端の経路からのずれを修正する経路
復帰速度は演算部Ｂ６にて次の（６）式により算出する
。即ち、位置Ｈから経路Ｐ僅−Ｐ！、１へ下した垂線の
足の座標Ｂは、で与えられるので、経路復帰速度Ｖ、を
位置Ｈと経路ｐ　、　−ｐ　、　、　、　との距離に比
例して与えるものとすると、速度Ｖ、は、Ｖ、＝αｉ　　（Ｂ−Ｈ）・・・・・・（６）で与えられる。

線路ｐ　、　、　ｐ　ｉｌ＋の選択は、先に説明した原
理および後述する第３図の経路判定アルゴリズムに従っ
て経路判定部Ｂ５にて行われる。従って、指令速度■は
、目標速度指令ＶｉＨ１，）と経路復帰速度■、とを合
成演算部Ｂ７にて加算して与えられ、Ｖ＝Ｖ、。、、、
＋Ｖ、　　　　　　　　　　・・・・・・（７）となる
。

第３図は経路判定部Ｂ５における経路判定のアルゴリズ
ムを示している。ハンド位置Ｈの算出は（４）式にもと
づき行われる。また、経路に下した垂線の足の座標Ｂは
（５）式により与えられる。

座標Ｂと経由点Ｐ、との大小関係がＸ座標あるいはＸ座
標について求められ、第１図からも明らかなようにＰ、
がＢ、より大であれば、指令速度■が（６）および（７
）式から求められる。

第５図はスカラ型ロボットを模式的に示したものであり
、ハンドの初期位置をＰｏとみなして第６図に示す経路
にて制御を行うもので、その結果を第７，８図に示しで
ある。第５図において、１および２は屈曲関節、３は直
動関節、４は回転関節を示し、５，６および７はそれぞ
れアームであり、８はハンドを示している。第６図に示
すように、各経由点の座標はＰｏ　　＋Ｐｌ　　、Ｐｚ
　　、Ｐｔの如く与えられるものとする。

明らかな様に本実施例ではＸ軸は一定座標であるから、
目標速度指令■。Ｉ＋Ｖ。２．■。、におけるＸ方向成
分は“０”である。経由点通過の判定は垂線の足のＸ座
標（！　Ｂ　ｙとハンド位置のＹ座標値Ｈｙとの大小関
係で行なう。

第７図はＹ方向の制御におけるｖ　ｙ　　、　Ｂ　ｙ　
　、Ｈｙの時間変化を示したものである。曲線Ｂ、にお
けるＡ、Ｂ、Ｃは、ぞれぞれ経由点Ｐ＋　　、Ｐｔ　　
、Ｐｉの通過判定時点を示しており、初速■。、、＋Ｖ
、。

で走行し、Ｐ、、Ｐｔ　　、Ｂ３を通過するに伴って、
■、がそれぞれＶ＋ｚｙ　＋Ｖｈｙ、Ｖ２３Ｖ　＋Ｖｋ
ｌ＋　０に切り換えられる。第８図は第７図がＹ方向の
制御を示したのに対しＸ、Ｚ方向についても示したもの
である。

〔発明の効果〕

以上説明したように本発明によれば、速度ベクトルによ
って速度制御して、目標点に到達すると位置制御してい
るので、通過点での移動停止を行なわないようにできる
から、高速の経路移動が可能となり作業効率が向上する
という効果を奏すると共に移動に要するエネルギを節約
できるという効果もある。

【図面の簡単な説明】

第１図は本発明に係る軌道制御方法の原理図、第２図は
本発明に係る軌道制御方法を適用した装置のブロック図
、第３図は第２図の経路判定部における経路判定アルゴリ
ズムを示す図、第４図は基準座標系とハンド座標系との関係を示す図、第５図はスカラ型ロボットを模式的に示した図、第６図
は軌道の一例を示す図、第７，８図は各軸方向の制御の時間的変化を示したグラ
フ、および第９図は従来の軌道制御経路を示す図である。（符号の説明）Ａ１・・・マニピュレータ、Ａ２・・・エンコーダ、Ａ３・・・サーボ機構、Ｂｌ・・・座標変換演算部、Ｂ２・・・関節速度演算部、Ｂ３・・・関数発生部、Ｂ４・・・ハンド位置演算部、Ｂ５・・・経路判定部、Ｂ６・・・経路復帰速度演算部、Ｃ１・・・経路生成部。

Claims

【特許請求の範囲】１、ロボットのマニピュレータの移動経路として経由点
と目標点が与えられ、前記マニピュレータを経由点を経
て移動させて目標点に位置決めする軌道制御方法におい
て、前記マニピュレータへの目標速度指令を経由点間の速度
ベクトルにより与え、経路からのずれを修正する速度ベ
クトルを前記マニピュレータと前記経路との間の垂線の
距離に比例して設定し、経由点間の速度ベクトルおよび
目標経由点の切り換えは、前記マニピュレータから経由
点間の直線経路上に下した垂線の足が経路延長線上で目
標としている経由点を越えたことを判定して行なうよう
にしたことを特徴とするマニピュレータ型ロボットの軌
道制御方法。