JPS62212802A - ロボツトア−ムの制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、ロボットアームの制御装置に関する。

（従来の技術） 最近、半導体装置の組立てや、簡単なパレタイジングの
作業では、６腕が二次元的に移動する水平関節型ロボッ
トが使用されてきている。

第３図は、このような水平１！Ｉｆｊｉ型ロボツトの一
例の概略外観図である０図に示すように、このマニピュ
レータ５は、床上に設置された基台ｌｌ上に、駆動モー
タにより回動自在なポストｌを設け、該ポストの先端に
第１のアーム２を水平方向に固着し、更に第１のアーム
２の先端にも水平方向に回動自在に第２のアーム３を設
け、この第２のアーム３の先端手首４に設けられている
。第１のアーム２の中には、第２のアーム３を回動駆動
するサーボモータが収納され、第２のアーム３の中には
手首４を駆動するためのサーボモータが収納されている
。

６は、この種のサーボモータに給電する電カケープルや
各種センサからの信号を伝える信号ケーブルなどの各種
ケーブルであり、２２．３２は第１のアーム２、第２の
アーム３を覆う蓋であり、それぞれのアーム駆動系の保
守、点検時等には、取付ビスにて蓋を外して作業を行な
う。

ところで、このようなロボットアーム２．３あるいは手
首４を駆動するサーボモータの速度制御系は、第４図に
示すように、いずれも所定の位置指間の入力に対して、
位置、速度制御ループと゛屯流ループとからなるフィー
ドバック制御系を構成している。

１くにおいて、Ｋ１は積分ゲ・イン、Ｋ２は比例ゲイン
　ＫＬは電力増幅器ゲイン、Ｊｍはモータロータイナー
シャ、ＪＬは負荷等価イナーシャ、Ｃはパラメータであ
る。

ここで、′心力増幅器ゲインＫｔがある程度大きければ
、第４　［３は簡単化して第５図のように表わすことが
できる。

（発明が解決しようとする問題点） 第５図において、ロボットアームが最適制御されるため
の最適フィードバック行列のへターワースパターンの極
配置を計算するとき、積分ゲインに＋、Ｌｔ例ゲイソゲ
インそれぞれ次のように決定される。

Ｋ　＋　＝　（（Ｊｍ÷ｊＬ）／Ｋｔ）Ｘ（２／　ｃｒ
”　）・・・（ａ） Ｋ２＝＋（Ｊ■＋ＪＬ　）／Ｋｔｌ　Ｘ　（２／σ）・
・・（ｂ） （ａ）、（ｂ）式において、サーボモータのロータイナ
ーシャＪｍは一定値であるが、負荷等価イナーシャＪＬ
はリンクやハント等マニピュレータの連動に合せて変化
するので、マニピュレータに振動が生じない最適のゲイ
ンに、、に２をオンラインで決定する必要があるが、従
来、各サーボモータのサーボ系では時々刻々に変化する
マニピュレータの状態を推定してレギュレータによる最
適ゲインを決定していたため、精度の良い制御が実現で
きないという問題があった。

本発明は、このような従来技術の問題点の解消し、負荷
等価イナーシャの変化をオンラインで計算し、算出され
た負荷等価イナーシャの値を用いてサーボモータの制御
ループのゲインをｉＡ幣するようにしたロボットアーム
の制御装置を提供することを目的としている。

（問題点を解決するための手段） 本発明は、ロボットアームを媒介にしてマニピュレータ
を駆動する複数のサーボモータと、各サーボモータの制
御系での位置情報に基づき負荷等価イナーシャの変化を
演算する演算手段と、演算された各サーボモータの負荷
等価イナーシャにより調整可能なループゲイン設定手段
とを具備したことを特徴とするロボットアームの制御装
置を提供することにより、従来技術の問題点を解消する
ものである。

（作用） 本発明は、負荷等価イナーシャの変化をオンラインで計
算し、得られた負荷等価イナーシャの値により、サーボ
モータ制御ループのゲインを調整するので、各時点での
最適ゲインが正確に算定される。

（実施例） 以下、図により本発明の一実施例について説明する。

第２図は、長さ交１のリンクＬ１、長さ文２のリンクＬ
２、長さ交３のハンドＨを有する水平関節型ロボットの
機構図である。リンクＬＬは０１軸の回りで、リングＬ
２は０２軸の回りで、ハンドＨは０５軸の回りでそれぞ
れ回転可能に構成されている。

ここで、リンクＬ１の全重量をＭ、　　（Ｍ、　＝ｒＨ
１１＋ｍ＋　２　”ｍｌ　５）、　リンクＬ２の全重力
をＭ２　（Ｍ２　＝ｍ２　ｌ＋ｍ２２　＋ｍ２Ｋ　）、
　ハントＨ及びワークＷ（７）重ｆｆｉをＭ５　（Ｍ５
＝ｍｈ＋ＭＷ）、リンクＬ、、Ｌ２及びハンドＨの重心
位置をそれぞれＷＬ　、Ｗ２　、Ｗ５とすれば、マニピ
ュレータの各軸のトルク”ｒ、、Ｔ２　、Ｔｇは次のよ
うにして求められる。

但し、ｍｉｊは、リンクＬ１＋Ｌ２の重量やサーボモー
タの重量等を質点系として表示している。

ハンドＨの０３軸回りのトルクＴ３は。

Ｔ３＝に、０５ ＋ｆ１　（δ５）δ°２ ＋ｆ２　　（Ｏｚ　　、δ５）δ°１ 十ｆ５（δ５）０２ｚ ＋ｆ４　　（０２，０５）　　δ１ｚ ＋ｆ５　（δ３）δ１　δ２ −（Ａ２Ｍｇ　＋　Ｊ　ｇ　ｒｙ）　δ゛３＋　　（Ａ
２　Ｍ５　　＋　ＡＭ３　ｎ　２　　ｃｏｓθ３＋　Ｊ
−、ｙｙ）　　δ２ ＋　　（ＡＭ３　（Ａ＋ｕ２　　ｃａｓθ３＋　Ｄ　１
ｃｏｓ（０２＋ｂ ＋　（ＡＭ３１２　　ｓｉｎθ３）δ２２＋　　（ＡＭ
５　　（１２ｓｉｎθ３ ”　Ｕ　１ｓｉｎ（θ２＋０３））１５Ｊｔｚ＋　（２
ＡＭ５１２　　ｓｉｎθ３）δ１６゜・・・　（１） ここで、係数Ａは、 Ａ＝皇３−５３ で、ハンドＨの先端からワークを含めたハンドＨの重心
位置までの距離Ｓ５は、 Ｓ３＝　（Ｉｇ−１ｓ　ｔ　）ｍｈ／　（ｍｈ＋Ｍｗ）
として算出される。　　　　　　　　　・・・（２）ま
た、ハンドＨの重心回りの慣性をハンドＨに固定された
座標系で表わしたＪ３！！は、Ｊ３ｙｙ＝ｍｈ　（Ｉｇ
　　１５　ｔ　　Ｓｇ）　ｚ＋　ＭｗＳ５　・＝　（３
） となるが、ハンドＨがワークを把持していない場合には
、上記各式におけるＭｗを零にする。

リンクＬ２の０２軸回りのトルクＴ２は、Ｔ２　＝Ｋｌ
　　δ′５ ＋ｆ＋　　′　（δ３）δ°２ ＋ｆ２　’　（θ２．０５）δ１ ＋ｆ５　　′（δ３）δ２ｚ ＋ｆ４　　′（θ２．０５）０１２ ＋ｆ！５　　′　（δ５）δ１　δ２ ＋２θ３θ１） ・・・（４） となる。

リンクＬ１のθｌ軸回りのトルクＴｌは、Ｔ、＝ｆ７　
（θｚ、＜Ｊｇ） ・（０１＋０２　＋　０５　）　２ ＋ｆａ　　（０２，δ３） ・　（δＬ＋０２）ｚ ＋ｆ９　（θｚ、Ｏｇ） ・　（δ１＋０２＋０３） 十ｆｌＯ（θ２．θ３） ・　（δ“ｒ　　＋０２） ＋ｆｒｔ（δ２　、δ３　）　・　θＬ＋ｆ１２（δ２
　、δ３）・δ１２ ・・・　（５） となる。

これら（１）、（４）、（５）式において、θｉの項は
、負荷の等価イナーシャであり、各軸の回転角θｉの変
化に応じて変動する項である。

上記各サーボモータに対するトルク指令Ｕは。

一般に次の式で表わされる。

＝Ｊ（θ）θ＋丁（θ、０）　　　　　　・・・（ｃ）
ここで、 Ｊ（θ）θ　：負荷の等価慣性イナーシャ。

ｃ（０，０）：遠心力、コリオリカ、 ｆ（１：ｆ！ｌ擦力、 ｇｃＯ）　　　　：重力、 Ｔ（θ、δ）：全体の非線形トルクである。

第１図は、本発明装置を適用して制御されるロポントア
ームの制御システム例を示すブロック図である。

本発明は、これらの負荷等価イナーシャを、ロボットハ
ンド及びリンクの回転角が変化する毎に、オンラインで
計算し、これに基づいて、速度制御ループの比例ゲイン
と積分ゲインを調整するものである。第１図は、本発明
の概略のブロック図であり、任意の１輌、例えばｏｌ軸
の回りを回動するマニピュレータの駆動用サーボモータ
の制御回路を示している。このサーボモータは、サーボ
制御回路ａに設けられた電流制御のマイナーループで電
流制御を行なうと共に、ロータリエンコーダで得られた
パルス信号により位置制御を行ない、周波数−電圧変換
器（Ｆ／Ｖ）を通して得られた信号で速度制御を行なう
、コントローラｂは、予め入力されているリンク及びハ
ンドの長さ、重量と、ロータリエンコーダからのパルス
信号によりオンラインで求めた各軸の回転角により、各
軸のトルクを計算する。このときに得られる負荷等価イ
ナーシャの値により、（ａ）。

（ｂ）式で最適の積分ゲインに１と比例ゲインに２を演
算して、それぞれのゲインを調整する。

なお、コントローラｂにおける各軸毎のトルクエ」算は
、それらを快適に演算処理するＣＰＵで行なわせるよう
にしてもよく、未発明はト記実施例に限定されるもので
はないことは、言うまでもない。

（発明の効果） 以に説明したように、未発明のロボットアームの制御装
ηによれば、ロボットアームの複数の駆動軸回りで回動
される負荷を、マニピュレータの運動に合セて、マニピ
ュレータ駆動用サーボモータで駆動制御するにあたり、
各軸に加わる負荷等価イナーシャの変化をオンラインで
計算し、得られた負荷等価イナーシャの値を用いて、該
サーボモータの制御ループのゲインを変更しているので
、マニピュレータ動作の各時点のゲインを最適に制御で
息、マニピュレータの振動を除去した精密な制御が行な
える。

【図面の簡単な説明】

第１図は、本発明の一実施例を示すブロック図、第２図
は、水上関節型ロボットの−・例を示す機構図、第３図
は、同ロボットの外観図、第４図、第５図は、速度制御
から見たサーボ系のブロック図である。 １・・・ポスト、２・・・第１のアーム、３・・・第２
のアーム、４・・・ｆ首、ａ・・・サーボ制御回路、ｂ
・・・コントローラ。

Claims (1)

【特許請求の範囲】
1. ロボットアームを媒介にしてマニピュレータを駆動する
   複数のサーボモータと、各サーボモータの制御系での位
   置情報に基づき負荷等価イナーシャの変化を演算する演
   算手段と、演算された各サーボモータの負荷等価イナー
   シャにより調整可能なループゲイン設定手段とを具備し
   たことを特徴とするロボットアームの制御装置。