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

Abstract

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

Description

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

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

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

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

第４図は、このような水平関節型ロボットを駆動するサ
ーボモータの駆動制御系を示すブロー２り図である。図
において、Ｏｌ　、θ２・・・の各軸回りのアームやハ
ンドを駆動するサーボモータ１０゜２０・・・は、それ
ぞれの制御系として設けたサーボＣＰＵとインバータと
により、指令信号にフィードフォワード補償を行なって
所定の位置制御、電流制御が実行される。これら各制御
系に共通に設けたメインＣＰＵは、ロボットシステム全
体の制御信号を出力している。

（究明が解決しようとする問題点） ところで、上記各サーボモータ１０．２０・・・のトル
ク指令Ｕは、一般に次の式で表わされる。

ｕ＝Ｊ（０）　Ｏ＋ｃ（０、０）　＋ｒ（θ）＋ｇ（０
）＝　Ｊ（０）　θ＋τ（０，０）　　　　　　　　・
・・（ａ）ここで、 Ｊ（０）θ　：負荷の等価慣性イナーシャ、Ｃ（０，δ
）：遠心力、コリオリカ。

ｆ（０）　　　　：摩擦力、 ｇ（θ）　　　：重力、 Ｔ（θ、０）：全体の非線形トルクである。

各サーボＣＰＵでは、サーボモータの制御に必要な負荷
の慣性イナーシャや非線形Ｉ・ルクのて（算を前記（ａ
）式により行なうが、このとき、他の軸回りの回転角等
の情報を取込む必要があり、このため、各サーボＣＰＵ
毎に行なっている主流ルーズの処理が妨害され、サーボ
モータの主流制御が精度良く行なわれなくなるという問
題があった。

本発明は、このような従来技術の問題点を解消し、各軸
毎の情報を、共通のＲＡＭなどに記憶〇して、各サーボ
ＣＰｔＪから共通にアクセスできるようにしたロボット
アームのＪＩＪＩ装置を提供することを目的としている
。

（問題点を解決するための手段） 本発明は、ロボットアームによりマニピュレータを駆動
する複数軸分のサーボモータと、各サーボモータの制御
系に設けられ制御系のフィードフォワード補償を行なう
サーボマイクロプロセッサ（ｃｐｕ）と、これらサーボ
ＣＰＵでのマニピュレータの運動方程式の計算に必要な
他軸の情報を保持するとともに各サーボＣＰＵから共通
にアクセス可能な記憶手段とを具備したことを特徴とす
るロボットアームの制御装置を提供することにより、従
来技術の問題点を解消するものである。

（作用） 本発明の制御装置は、ロボットアームの複数の駆動軸回
りで負荷を駆動制御する各サーボモータの制御ループに
設けたサーボＣＰＵが、それぞれ共通にアクセスできる
共有ＲＡＭに記憶された他軸回りの情報を読み出し、ｉ
′ｔｔ流ループループを行なわない時刻に、サーボ制御
に必要な非線形トルクの演算を行なう、このため、電流
ループの処理が妨害されず、精度良くサーボモータの制
御が行なえる。

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

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

ここで、リンクＬｌの全１３）をＭＩ　（Ｍ１＝ｍｔ＋
＋ｍ＋２＋ｍ＋ｓ）、リンクＬ２の全重量をＭ２　ＣＭ
２　＝ｍ２１＋ｍ２２　＋ｍ２５）、　ハンドＨ及びワ
ークＷ　ｃｒ）　ｑ　量をＭ　−５（Ｍ　ｓ　＝　ｍ　
ｈ　＋　ＭＷ）、リンクＬＬ、Ｌ２及びハンドＨの重心
位とをそれぞれｗｔ　、ｗ２　、Ｗ３とすれば、マニピ
ュレータの各軸のトルクＴ、、Ｔ２．Ｔ５は次のように
して求められる。

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

ハンドＨの０５軸回りのトルクＴ５は、Ｔ、＝に、０５ ＋ｆｉ　　（（＋５）０２ ＋ｆ２　（θｚ、Ｏｇ）　　δ゛１ ＋ｆ＝ｓ　　（０５）Ｏ２ｚ ＋ｆ、、（０２、Ｏｓ）δ１ｚ ＋ｆ５　　（０５）　　０＋　　０２ ＝　　（Ａ２　＋５　　＋　＋５　　ｙｙ）　　Ｏｓ＋
　　（Ａｚ　＋５　　＋ＡＭ５　　＋２　　Ｃａ５Ｏｙ
。

＋Ｊ５　ｙり　　０２ ＋　　（ＡＭ５　　（Ａ＋Ｉ１２　　ｃｏｓθ３＋Ｑｔ
　ｃｏｓ（０２＋０５））＋ＪｇＹＹ　　］　　ＯＨ＋
　（ＡＭ５　１１２　５ｉｎ７７５）　　δ２ｚ＋　　
（ＡＭ５　　（１２ｓｉｎ０ｇ ＋Ｑｌ　　５ｉｎ（＋７２　　＋（＋５　）））　　Ｏ
Ｈｚ＋　　（２置Ｍ５　１２　５ｉｎ（１５）　　０１
０２・・・　（１） ここで、係数Ａは、 Ａ＝　９．−Ｓ３ で、ハン、１：Ｈの先端からワークを含めたハンドＨの
改心位置までの距離Ｓ３は、 Ｓ５　＝　Ｃａ５　ｎｓ　ｔ　）ｍｈ／　（ｍｈ＋Ｍｗ
）として算出される。　　　　　　　　　・・・（２）
また、ハンドＨの重心回りの慣性をハンドＨに固定され
た座標系で表わしたＪｓＹＹは、Ｊ　３　ｙｙ＝ｍｈ　
　（９５＋５　　＋　　−５３）ｚ＋Ｍｗ５ｙ５　　・
・・　（３） となるが、ハンドＨがワークを把持していない場合には
、上記各式におけるＭｗを零にする。

リンクＬ２の０２袖回りのトルクＴ２は、Ｔ２＝に、’
０３ ＋ｆ＋　　’　Ｃ０３）０２ ＋ｆ２’　（（ｌＪｚ　、＋５）０＋ ＋ｆ５　　’　　Ｃ０３）　　δ２ｚ ”ｆ４　　′　（０２，０５）Ｏｌ” ＋ｆｓ　　′　（Ｏｇ）δ１０゜ ＋ｆ６　Ｃ０３）（δ５ｚ＋２０２　ｊ３＋２０３６１
　） ・・・（４） となる。

リンクＬ、の０１軸回りのトルクＴ１は。

Ｔ＋　＝ｆ７　ＣＯ２，Ｃ５） ・　（δ１　＋０２＋６３）２ ＋ｆｓ　　ＣＯ２，０５） ・ＣＯＩ　＋０２）　ｚ ”ｆ９　　（１）ｚ　　、Ｏｓ） ・　（Ｏｌ　＋０２　＋　０３　） ”ｆｌ　　Ｏ（０２、（’り ・　（０１＋０２） ＋ｆｌ　　□　（０２，０５）　　・δ“１＋ｆ　ｌ　
２　　ＣＯ２＋　Ｏｓ　）　　・δ１ｚ・・・　（５） となる。

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

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

Ｏ１１置りに負荷を駆動するサーボモータｌ。

０２軸回りに負荷を駆動するサーボモータ２・・・の各
制御ループには、それぞれの軸回りの非線形トルクＴｉ
　　Ｃ０，０）（ｉ＝１，２・・・）を計算するサーボ
ＣＰＵｃｌ、ｃ２が設けられており、各サーボＣＰＵが
メインＣＰＵａとパスラインにより結合され、メインＣ
ＰＵからの制御信号に従って各軸制御に必要な運動方程
式をオンラインで演算が実行できる。インバータｄ、、
ｄ２は、サーボモータｅｌ、ｅ２の°電流を制御する。

こうした演算制御のための構成は、従来９置と同様の構
成であるあが、本発明においては、メインＣＰＵａと各
サーボＣＰＵＣ＋　　、Ｃ２・・・どの間に共有（ＳＨ
ＡＲＥＤ）ＲＡＭｂが設けられたことを特徴とする。こ
の共有ＲＡＭｂは、メインＣＰＵａと。

各サーボＣＰＵｃ１　　、Ｃ２・・・とからのアクセス
が可能であり、次のような機１駐を有する。即ち１例え
ば、ＯＩ軸回りのトルクＴ１をサーボＣＰＵｃ１により
、前記（５）式で演算する場合に、他の０２軸、０５軸
に関する情報が共有ＲＡＭｂに記憶され、サーボＣＰ　
Ｕ　’Ｃ１では、電流ループの処理を行なわない時刻に
、共有ＲＡＭｂにアクセスして他軸回りの情報を読み取
り、所定の演算を行なう、このため、バス競合（Ｃ；０
ＮＴＥＮＴＩＯＮ）で読み込みのロスが生じても、電流
ループの処理を防げることなく、サーボモータの制御に
必要な非線形トルクの演算が確実に行なえる。

（発明の効果） 以上説す１したように、本発明のロボットアームの制御
装置によれば、ロボットアームの複数の駆動軸回りで回
動される負荷を、対応する複数のサーボモータで駆動制
御し、各サーボモータを制御するサーボＣＰＵから共通
にアクセスできるＲＡＭを設け、各サーボＣＰＵが電流
ループ処理を行なわない時刻に他軸回りの情報を読み取
り、非線形トルクの演算を行なうので、電流ループの処
理を妨害することなく、精度良くサーボモータの制御が
行なえる。

【図面の簡単な説明】

第１図は、本発明の一実施例を示すブロック図、第２図
は、水ｆ型ロボットの一例を示す機構図、第３図は、同
ロボットの外観図、第４図は、従来装置のブロック図で
ある。 ■・・・ポスト、２・・・第１のアーム、３・・・第２
のアーム、４・・・手首、１０．２０・・・サーボモー
タ、ａ・・・メインＣＰＵ、ｂ−共有ＲＡＭ、Ｃ１、Ｃ
２・・・サーボＣＰＵ。

Claims (1)

【特許請求の範囲】
1. ロボットアームによりマニピュレータを駆動する複数軸
   分のサーボモータと、各サーボモータの制御系に設けら
   れ制御系のフィードフォワード補償を行なうサーボマイ
   クロプロセッサ（ＣＰＵ）と、これらサーボＣＰＵでの
   マニピュレータの運動方程式の計算に必要な他軸の情報
   を保持するとともに各サーボＣＰＵから共通にアクセス
   可能な記憶手段とを具備したことを特徴とするロボット
   アームの制御装置。