JPS59220806A - 工業用ロボツトの制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は、ロボットの動作に伴う慣性モーメントと重力
トルクの変動をフィードフォワード的に補償する方法に
係９、特に、ｉ’／ｌ＝の良い軌跡制御が可能な工業用
ロボットの制（財）方法に関する。

〔発明の背−駅〕

従来、この種の工業用ロボットの制御方法は、あらかじ
め教示されたロボット各軸の位置データから適当な補間
演算を行なうことによシ、各軸駆動系に対する時々刻々
の位（ｄ指令を求め、この位置指令値により、各軸駆動
系がザーボ制御されていた。このとき、各軸サーボ糸の
制御パラメータは、最適な位置サーボ特性葡示すように
ＷｒＡＩ整されたのち、一定で使用されていた。一方、
ロボットの制御系はロボットの動作に伴う姿勢の変化に
よって、慣性モーメントや重力トルクが本質的に変動す
る制御系である。従って、このような方法では、ロボッ
トの動作に伴い、各軸サーボ系の応答に差が生じるため
、ロボットの位置決め精度や軌跡制御性能が低下する。

これに対して、ロボットの姿勢変化に応じて、慣性や重
力トルクの変化分を補償する方法が提案されているが、
ロボットの動作に伴って実時間で慣性や重力トルクを演
算するためには、数多くの乗算、加減算を必要とするた
め、計算時間が犬となり、その分サンプリング周期を長
くせざるを得ない。

〔発明の目的〕

本発明の目的は演算時間を長くすることなく、精度のよ
い軌道制御を実現できる工業用ロボットの制御方法を提
供するにある。

〔発明の概要〕

本発明の要点は、ロボットの動作教示時に、教示点にお
ける慣性、重力トルクを演算してメモリに格納しておき
、ロボットの動作時に、これらのデータに基づき各軸サ
ーボ系のパラメータ補償を行なうようにしたものである
。

〔発明の実施例〕

以下、本発明の実施例を第１図以下を用いて説明する。

第１図は本発明による制御方法が適用される工業用ロボ
ット及びその制御装置の一実施例を示す斜視図で、５自
由度リンク機構の工業用ロボツ）１とその制御装置２及
びロボットに動作を教示するだめのティーチングボック
ス３から構成される。

図において、０１〜θ、はロボットの制御角であり、θ
、から順に本体１０１の旋回、上腕１０２の曲げ、前腕
１０３の曲げ、グリップ１０５の曲げ、グリップ１０５
のひねり角をそれぞれ示す。

本実施例では、ティーチングボックス３からの指令によ
シ、ロボットＦ計、目標とする姿勢になるよう動かし、
その教示点を制御装置２のメモリに格納することにより
、ロボットに動作を教示する。

次に、第２図に、ロボット制御装置２の構成を示す。ロ
ボット制御装置は、教示点のｌｆｆ１ｆデータのメモリ
への格納、ロボット教示点での慣性、重力トルクの演算
、更に、ロボット動作時における各軸サーボ系への時々
刻々の位置指令値の演算などを行なうだめの演算処理回
路と５つの駆動軸をそれぞれ個別にサーボ制御するだめ
の各軸サーボ糸とから構成される。ここで、演算処理回
路は、マイクロプロセッサ２０１とその周辺素子である
ＲＡＭ２０２．ＦＬＯＭ２０３％　及び教示点の位置デ
ータや教示点の慣性、重力トルク等のデータを格納する
だめの比較的大容量のメモ！７２０５などのディジタル
回路で構成される。更に、この演算処理回路はインター
フェース回路２０４を介してティーチングボックス３と
、インターフェース回路２０６を介して各軸サーボ系と
、それぞれ接続されている。ここで、演算処理回路で求
められた位置指令値は、Ｄ／Ａ変換器２０７によってア
ナログ量に変換され、各軸サーボ系への指令値となる。

各軸サーボ系はアナログ回路で構成され、位置制御回路
２０８（２１５）％速度制御回路２０９（２１６）、電
流制御回路２１０（２１７）％及びパ’７−７７７’２
１１　（２１８）カら成シ、ＤＣ％−１’２１２（２１
９）を駆動する。ここで、モータ電流、速度、位置はそ
れぞれ電流検出器２２２（２２３）、ｐ−ｙジェネレ１
１２１３（２２０）ポテンショメータ２１４　（２２１
）によって検出されて、各１軸サーボ系にフィードバッ
クされる。

このような各軸サーボ回路が駆動軸数だけ用意されてい
る。ここで、速度制御回路２０９　（２１６）の制御パ
ラメータは、教示点における慣性、重力トルクのデータ
に基づいて補償される。

さて、第１図に示す５自由度リンク機構のロボットｌに
おいて、慣性、重力トルクの変化の影響が特に顕著なの
は、本体の旋回（θ１軸）、上腕の曲げ（θ、軸）、前
腕の曲げ（θ３軸）の３軸である。そこで、本実施例で
は、この３軸について、慣性、重力トルクの補償を行な
うものとする。

更に１以下の実施例の説明け、この３軸についてのみ行
なう。

このときの、等価なモデルは第３図のようになる。ここ
で、ロボットの手先に相当するグリップの部分とグリッ
パがつかんだ荷重とは、ロボットの前腕１０３の先端に
集中しているとする。いま、第３図に示す直交座標系ｘ
ｙｚを古え、２軸を鉛直方向とする。各リンクの回転角
θ１．θ２゜θ３をｘｙｚ座標系に対して第３図のよう
に選ぶと、θ、軸の回転に対する慣性モーメントＪ１、
頂カトルクτ１３．θ２軸の回転に対する慣性モーメン
トＪ１％　重カトルクτｇｔ＋’Ｂ軸の回転に対する慣
性モーメン）Ｊｓ、取カトルクτ、３はそれぞれ次式で
与えられる。

Ｊ　１＝　８２８ｉ１１２０２　＋　２　ａｔ”ｉＩＩ
θ、５ｉｎ（θ２＋０３）＋ａｓｓｉｎ２（θ２＋θａ
　）　十Ｌ　　　　　　（１）τ、、＝ｏ（２） Ｊ２　＝３２　十ａ、　＋２　ａ４焦θｓ　十Ｉ２＋　
ＩＳ　　　　　（３）７Ｍ　２−（ａ　、、８ｉｎθ２
＋ａａ”（θ２＋θｓ　＞　）　ｇ　　　　　（４）Ｊ
ａ　＝ａｓ＋　Ｉｓ　　　　　　　　　　　　　　（５
）ｒ、　３＝−ａ６ｓｉｎ　（θ！十θｓ）ｇ　　　　
　　　　（６）ここで、ａｔ　ｒ−ａＩＩ　Ｊａ４　＊
　ａＩＩ　＊　ａａは、ａｔ　”ｍｔｌ−ｔ：十（”ｓ
＋ｍｔ）　１２　　　　　　　　　（７）ａ３　＝ｍ、
を八　十ｍ、ｔ”、　　　　　　　　　　　　　　　　
　　　　　　　　　　　　（ｓ）ａｔ　＝　（”ｓ　Ｌ
ｗｓ　＋ｍＷ　ｔｓ）　ｔｔ　　　　　　　　　（９）
ａａ　”ｍｔ４ｔ　＋　（ｍｓ　＋　ｍｗ　）　４　　
　　　　　（１０）ａ、　＝ｍｓ４．　＋ｍ、、ｚ、　
　　　　　　　　　　　（１１）である。

ただし、上述の各式において、 ｍｌ　：第３軸リンクの質量 ｔｌ　　Ｆ第３軸リンクの長さ ｔ１１８第、ｉ軸すンクの根元から第３軸リンクの重心
までの長さ Ｉ＋　　’第３軸リンクの重心に関する慣性モーメント ｍ、：第３軸リンクの先端の質量 ｇ　：重力加速度 である。

（１）〜（６）式に示すように、Ｊ８．τ、２．τ、３
はθ２゜θ８の関数、Ｊ２はθ、の関数として変化する
。

このため、各軸サーボ系の特性は、θ２．θ８の変化に
伴って変動する。本発明でｉｄ、　（ｉ）〜（６）式で
表わされる慣性、重力トルクの変化を補償することを目
的とする。以下、本実施例における慣性、重力トルクの
補償方法について述べる。

本実施例では、ロボットの動作教示時に、教示点におけ
るロボットの姿勢として、 ３つの関節角度θ１．θ２．θ、をテイーテンクデータ
記録用のメモリ２０５に格納する。これと同時に、０１
〜θ、の値から、（１）〜（６）式の計算式に従って、
教示点における各軸サーボ系の慣性モーメントＪ　ｌ　
％　ｋ、力トルクτｇ＋　（’　＝１〜３）をそれぞれ
計算する。この計算は、第２図に示す制御装置の構成要
素であるマイクロプロセッサ２０１によって行なわれる
。計算式（１）〜（６）は、同制御装置のＲＯＭ２０３
の中に、プログラムとして書込んでおく。三角関数の計
算には、同じＲＯＭ　２０３に格納しておいた８ＩＮ関
数テーブルを索引することによシ求める。これらの計算
には多くの乗算、加減算が必要となるが、ロボットの動
作教示はオフラインで行なわれるため、この演算に要す
る時間は問題にならない。

このようにして求めた慣性モーメント、重力トルクは、
第４図に示すように、教示点毎に関節角データと共に教
示データ用のメモリに格納する。

さて、ロボ、ットの動作時には、教示されたロボットの
位置を適当な関数で補間して、サンプリング周期毎に各
関節角θ厘を求め、これを各軸サーボ系の位置指令値と
する。このとき、前述した方法によシ求めた慣性モーメ
ント、重力トルクの値も同様にサンプリング周期毎に補
間し、この値によシ各軸サーボ系のパラメータを補償す
る。

第５図に、教示点Ｐ、とＰ２間の慣性モーメントと重力
トルクの補間の方法を、第ｉ軸について示す。」番目の
サンプリングにおける慣性モーメント、＋＋（ｊ）、ｉ
ｆカトルクτ、（ｊ）は次式で計算される。

ここで、ＪＩＩｌτ１１．は、それぞれ教示点Ｐ１　に
おける第ｉ軸の慣性モーメントと重力トルクであり、Δ
Ｊ　Ｉ（ｋ）、Δτ、、（ｋ）は、それらのサンプリン
グ周期毎の変化量を我わす。この変化酸０１次のように
計算される。

ここで、Δ山（ｋ）は、サンプリング間の関節角の変化
であり、次式で求められる。

Δ０１（ｋ＞＝θ＋（ｋ−１）ｉ必）　　　　　　　　
　（１６）又、Δθ、（ｋ）け、サンプリング周期毎の
各軸サーボ系に対する指令値からｎ」算する。

３）は、（１）〜（６）式から、次のように求まる。

（ｊ−１〜３）はθ、ｔｊ＝ｘ〜３）の関数となる力よ
、通常ロボットの制御では、教示点間における慣性モー
メント、重力トルクの変化は滑らかである。　　“（ｊ
−１〜３）の変化は小さく、はぼ一定と見做してよい。

そこで、第６図に示すように、サンプリング周期毎に、
ヱに±ｊ　・・・・・・２μｍ時と順次δθＩｙｄθ量
、　　　σθ量 くりかえし演算し、その計算毎に値を更新して、その間
では一定とする。この計算は、　　（１７）−４２２）
式をプログラムとしてＴＩ、　ＯＭに格納しておき実行
する。又、割算した外しｅ？ｔ、、・はメモリに格納し
ｂθｊ＋ｄθ。

ておき、ＪＩ、τ１１の計：惇に用いる。このようにし
δＪ＋　　ａｒ、。

て求めた一１石＋　ｗから（１４）・（１５）式を用“
てΔＪｌ（ｋ）、Δτ、、（ｋ）を計痒し、これらの値
から（１２）　。

（１３）式によシ、そのサンプリング時点の慣性モーメ
ントＪ＋（ｊ）とτ１（ｊ）とを求める。

以上のようにして求めだＪｌ（ｊ）、τ、＋（ｊ）を用
いて各軸ザーボ系のパラメータを補正する方法を第７図
により説明する。第７図において、Ｊｌ、τ１１を、ロ
ボットの動作に伴って変化する慣性モーメントと重力ト
ルクとする。ここで、位置制御回路２０８は単純なゲイ
ンＫｐとし、電流制御回路２１０は時定数Ｔｃの１次遅
れ系で近似した。又、ＫＴをトルク定数とする。Ｊ＋＋
τ１．の変化に伴い、速度制御回路２０９のゲインＫｓ
ｌとオフセラ）Ｋｏ。

を次式のように補償する。

Ｋｓ＋（Ｊ）＝Ｋｓｏ　−Ｊｓ（Ｊ）　　　　　　　　
　　　　（２３）これによって、各軸サーボ系の負荷と
して作用する重力トルク分を完全に補償することができ
、更に、酸性モーメントの変化を補うように速度制御系
のゲインを調整できるので、各軸サーボ系の応答特性を
一定に保つことができる。この結果、高精度の軌跡制御
が実現できる。

このように、本実施クリによれば、あらかじめ計算して
おいた教示点での慣性モーメントと車力トルクを用いて
、サンプリング周期毎の簡単な補間演算によシ、慣性モ
ーメントと重力トルクの変化を補償できるので、サンプ
リング周期毎に演算して補償する方法に比べて演算時間
を大幅に短縮できる。更に、各軸サーボ系のパラメータ
変化をフィードフォワード的に補償できるので、応答を
一様にすることができ、このため、精度の良い軌跡制御
が実現できる。

なお、上記実施例は、５自由度リンク機構を３自由度モ
デルに近似して、３軸について補償を行なう場合につい
て説明しだが、本発明はこれに限定されるもので（ｄ’
、　’ｘ　＜　、５軸全部にわたって慣性、重力、トル
ク補償をする場合にも、同様Ｖこ適用できる。更に、５
自由度以上の場合にも、本発明の制御方法を適用できる
ことは言うまでも゛ない。

〔発明の効果〕

本発明によれば、軌道指令に対する精度の良い制御性能
が得られ、又、補償量を格納しておくメモリの容敏をあ
まシ増すことなくパラメータ補償を実行でき、しかも、
サンプリング周期毎の補償量の計算に要する時間が短か
くてすむ。

【図面の簡単な説明】

第１図は本発明の一実施例の制御方法が適用される工業
用ロボットとその制御装置の斜視図、第２図は第１図の
制御装置の構成を示すブロック図、第３図は第１図にお
ける実施例の解析モデルを示す概略図、第４図は第１図
に示す実施例における教示データの蜂納テーブル、第５
図は第１図の実施例における補償パラメータの補間方法
を示す概略図、第９図は第１図の実施例におけるパラメ
ータ変化率を求めるタイミング図、第７図は第１図の実
施例におけるパラメータ補償の方法を示すブロック図で
ある。 ２・・・ロボット制御装置、３・・・ティーチングボッ
クス、２０１・・・マイクロプロセッサ、２０５・・・
教示データ格納用メモリ、２０８・・・位置制御回路、
警３ω を５区 第乙日 榴

Claims (1)

1. 【特許請求の範囲】 １、　複数のリンク機構から成シ、この各リンクが個別
   の軸サーボ系によシ駆動される多自由度運動機構の工業
   用ロボットの先端位置および姿勢が教示され、その教示
   データに基づいて制御されるものにおいて、 前記工業用ロボットの動作を教示する際に、教示点にお
   ける前記各軸サーボ系の慣性モーメントおよび重力トル
   クを演算によシ求め、他の教示データと共にメモリに格
   納しておき、前記工業用ロボットの動作時に、前記教示
   点における前記慣性モーメント、前記重力トルクのデー
   タから、前記工業用ロボットの動作時における前記各軸
   サーボ系の俳性モーメント、重力トルクを演算により求
   め、その値に基づき、前記各軸ザーボ系の制御パラメー
   タを補償することを特徴とする工業用ロボットの制御方
   法。