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

Description

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

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

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

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

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

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

ｕ＝Ｊ（θ）＋ｃ（θ，）＋ｆ（）＋ｇ（θ） ＝Ｊ（θ）＋Ｔ（θ，） ……（ａ） ここで、 Ｊ（θ）：負荷の等価慣性イナーシャ、 ｃ（θ，）：遠心力、コリオリ力、 ｆ（θ）：摩擦力、 ｇ（θ）：重力、 Ｔ（θ，）：全体の非線形トルクである。

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

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

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

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

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

第２図は、長さl₁のリンクL1、長さl₂のリンクL2、長さ
l₃のハンドＨを有する水平関節型ロボットの構成図であ
る。リンクL1はθ_１軸の回りで、リンクL2はθ_２軸の回
りで、ハンドＨはθ_３軸の回りでそれぞれ回転可能に構
成されている。

ここで、リンクL1の全重量をM₁（M₁＝m₁₁＋m₁₂＋
m₁₃）、リンクL2の全重量をM₂（M₂＝m₂₁＋m₂₂＋m₂₃）、
ハンドＨ及びワークＷの重量をM₃（M₃＝mh＋Mw）、リン
クL₁、L₂及びハンドＨの重心位置をそれぞれW₁,W₂,W₃と
すれば、マニピュレータの各軸のトルクT₁,T₂,T₃は次の
ようにして求められる。

但し、mijは、リンクL₁,L₂の重量やサーボモータの重量
等を質点系として表示している。

ハンドＨのθ_３軸回りのトルクT₃は、 T₃＝K_{1 ３} ＋f₁（θ_３）_２ ＋f₂（θ₂,θ_３）_１ ＋f₃（θ_３）₂ ² ＋f₄（θ₂,θ_３）₁ ² ＋f₅（θ_３）_{１ ２} ＝（A²M₃＋J₃yy）_３ ＋（A²M₃＋AM₃l₂ cosθ_３ ＋J₃yy）_２ ＋〔AM₃｛Ａ＋l₂ cosθ_３ ＋l₁ cos（θ_２＋θ_３）｝＋J₃yy〕_１ ＋（AM₃l₂ sinθ_３）₂ ² ＋｛AM₃（l₂ sinθ_３ ＋l₁ sin（θ_２＋θ_３））｝₁ ² ＋（2AM₃l₂ sinθ_３）_{１ ２} ……（１） ここで、係数Ａは、 Ａ＝l₃−S₃ で、ハンドＨの先端からワークを含めたハンドＨの重心
位置までの距離S₃は、 S₃＝（l₃−l₃₁）mh/（mh＋Mw） ……（２） として算出される。

また、ハンドＨの重心回りの慣性をハンドＨに固定され
た座標系で表わしたJ₃yyは、 J₃yy＝mh（l₃−l₃₁−S₃）^２＋MwS₃ ……（３） となるが、ハンドＨがワークを把持していない場合に
は、上記各式におけるMwを零にする。

リンクL₂のθ_２軸回りのトルクT₂は、 T₂＝K₁′_３ ＋f₁′（θ_３）_２ ＋f₂′（θ₂,θ_３）_１ ＋f₃′（θ_３）₂ ² ＋f₄′（θ₂,θ_３）₁ ² ＋f⁵′（θ_３）_{１ ２} ＋f₆（θ_３）（₃ ²＋２_{２ ３}＋２_{３ １}） ……
（４） となる。

リンクL₁のθ_１軸回りのトルクT₁は、 T₁＝f₇（θ₂,θ_３）・（_１＋_２＋_３）^２ ＋f₈（θ₂,θ_３）・（_１＋_２）^２ ＋f₉（θ₂,θ_３）・（_１＋_２＋_３） ＋f₁₀（θ₂,θ_３）・（_１＋_２） ＋f₁₁（θ₂,θ_３）・_１ ＋f₁₂（θ₂,θ_３）・₁ ² ……（５） となる。

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

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

θ_１軸回りに負荷を駆動するサーボモータ1,θ_２軸回り
に負荷を駆動するサーボモータ２…の各制御ループに
は、それぞれの軸回りの非線形トルクTi（θ，θ）（ｉ
＝1,2…）を計算するサーボCPUc₁,c₂が設けられてお
り、各サーボCPUがメインCPUaとバスラインにより結合
され、メインCPUからの制御信号に従って各軸制御に必
要な運動方程式をオンラインで演算が実行できる。イン
バータd₁,d₂は、サーボモータe₁,e₂の電流を制御する。
こうした演算制御のための構成は、従来装置と同様の構
成であるあが、本発明においては、メインCPUaと各サー
ボCPUc₁,c₂…との間に共有（SHARED）RAMbが設けられた
ことを特徴とする。この共有RAMbは、メインCPUaと、各
サーボCPUc₁,c₂…とからのアクセスが可能であり、次の
ような機能を有する。即ち、例えば、θ_１軸回りのトル
クT₁をサーボCPUc₁により、前記（５）式で演算する場
合に、他のθ_２軸、θ_３軸に関する情報が共有RAMbに記
憶され、サーボCPUc₁では、電流ループの処理を行なわ
ない時刻に、共有RAMbにアクセスして他軸回りの情報を
読み取り、所定の演算を行なう。このため、バス競合
（CONTENTION）で読み込みのロスが生じても、電流ルー
プの処理を防げることなく、サーボモータの制御に必要
な非線形トルクの演算が確実に行なえる。

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

【図面の簡単な説明】

第１図は、本発明の一実施例を示すブロック図、第２図
は、水平型ロボットの一例を示す機構図、第３図は、同
ロボットの外観図、第４図は、従来装置のブロック図で
ある。 １……ポスト、２……第１のアーム、３……第２のアー
ム、４……手首、10,20……サーボモータ、ａ……メイ
ンCPU、ｂ……共有RAM、c₁,c₂……サーボCPU。

───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 昭58−5804（ＪＰ，Ａ) 特開 昭60−263206（ＪＰ，Ａ) 特開 昭60−218113（ＪＰ，Ａ) 特開 昭56−42804（ＪＰ，Ａ)

Claims (1)

【特許請求の範囲】
1. 【請求項１】ロボットのアームによりマニピュレータを
   駆動する複数軸分のサーボモータと、各サーボモータの
   制御系に設けられ制御系のフィードフォワード補償を行
   うサーボマイクロプロセッサ（CPU）と、これらサーボC
   PUでのマニピュレータの運動方程式の計算に必要な他軸
   の情報を保持すると共に各サーボCPUから共通にアクセ
   ス自在な記憶手段と、電流ループの処理演算の合間にて
   干渉トルクの演算を実行する手段とを具備してなること
   を特徴とするロボットアームの制御装置。