WO1987005721A1 - Robot controller - Google Patents

Description

明 細 睿

D ボ ツ ト 制御装置

技 術 分 野

本発明は、 ロ ボ ッ ト ア ー ム の負荷等価 ィ ) "一シ ャ を演 箕 し て サ ー ボ モ ー タ の ル ー プゲ イ ン を 調整する 専用の制 御 プ ロ セ ッ サ を備え た ロ ボ ッ ト 制御装置 に関す る 。

背 景 技 術

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

第 3 図は、 こ の よ う な水平関節型ロ ボ 卜 の一例の概 略外観図であ る 。 図 に示すよ う に 、 こ の マ ニ ピ ュ レ ー タ 5 は、 床上に 設置 された基台 1 1 上に、 駆動モ ー タ に よ リ 回動 自在な ポ ス ト 1 を設け、 該ボ ス ト の先端 に第 1 の ア ー ム 2 を水平方向 に 固着 し、 更に第 1 の ア ー ム 2 の先 端に も 水平方向に 回動 自在に第 2 のァー ム 3 を 設け、 こ の第 2 の ァ ー ム 3 の先端手首 （ハ ン ド） 4 に設け られ て い る 。 第 1 の ア ー ム 2 の中には、 第 2 の ア ー ム 3 を 回動 駆動す る サー ボモー タ が収納され、 第 2 の ア ー ム 3 の 中 に は手首 4 を駆動す る た め の サーボモ ー タ が収納 され て い る 。

6 は 、 こ の種の サ ー ポモー タ に給電す る電力 ケ一 ブ や各種セ ン サか ら の信号を伝える信号ケ ー ブル な どの各 種ケー ブルで あ り 、 2 2 , 3 2 は第 1 の ア ー ム 2 、 第 2 の ア ー ム 3 を 覆 う 蓋であ り 、 それぞれの ア ー ム ¾動系 の 保守、 点検時等に は 、 取付ビ ス にて蓋を 外 し て作業を行 な ラ 。

と こ ろ で、 こ の よ う な ロポ ッ ト アー ム 2 , 3 あ る い は 手首 4 を驟動す る サ ー ボモー タ の速度制御系は 、 第 4 図 に示す よ う に 、 い ずれ も 所定 の位置指令の入力に対 し て 、 位 置 、 速 度制 御 ル ー プ と 電流 ル ー プ と か ら な る フ ィ 一 ド、バ ッ ク 制御系 を構成 し ている。

図に おいて 、 図示 し ないメ イ ン C P U で形成 された位 置指令信号は 、 サー ボ制御回路で所定の位置制御信号 に 補正 され、 マ イ ナ一 ル ー プで構成される 電流制御ル ー プ の出力信号 と 共に サ ー ボモ ー タ に送出 される 。 各サー ボ 制御回路に は 、 コ ン ト ロ ー ラが接続 され、 各コ ン ト 口 一 ラ に は前記手首 4 に取付け られたハ ン ド の重量値や、 マ ニ ピュ レ ー タ の運動 に応 じて把持される種 々 の ワ ー ク の 重量値が適宜 メ イ ン C P Uか ら入力 され る 。 サ 一 ボモ ー タ は、 印加 された制御信号に基づいてマ ニ ピ ュ レ ー タ の 制御を行な う 。 こ の と き 、 負荷の回転位置は ロ ー タ リ エ ン コ ー ダで検出 され、 周波数一電圧変換器 （ F / V ) を 通 し て サ一 ポ制御回路に フ ィ ー ドパ ッ ク され る 。 こ こ で K 1 は積分ゲ イ ン 、 K ₂ は比例 ゲ イ ン を示 し てい る 。 サ ー ボ モ ー タ は 、 0 丄 ， 0 2 等各 ft毎 に設け られて お リ 、 各轴の 口 一 タ リ エ ンコー ダの出力信号は、 上記コ ン ト ロ 一 ラ に入力 され、 サ一ボモー タ の制御 に必要 と され る駆動 ト ル ク を計算 し 、 非線形 ト ル ク の項を サ 一 ボ制御 回路に フ ィ ー ド フ ォ ワ ー ド で補償する。 ろ で、 上記各 サ ー ボモー タ の ト ル ク 指令 u は 、 一 般 に次 の式 で表わ さ れ る 。

= ί( θ ) Θ + c( θ , Θ ) + f ( Θ ) + g( θ )

Π Θ θ + Τ( θ , θ ^ …（a) で、

J ( θ Θ 貝荷 の等価 イ ナ一 シ ャ 、

c ( θ , Θ 遠 心力、 コ リ オ リ カ、

f C Θ ) 摩擦力、

s C Θ ) 重 力

T ( θ , Θ ) 全体の非線形 ト ル ク で あ る 。

こ の ょ ラ に 従来の サ ー ボモー タ の制御 では 、 メ イ ン C

F U に 接铳 さ れ制御 され る各轴の負荷駆動用 サ 一 ボ モ ー タ に 、 そ れぞれサ ー ボ制御回路 を設け る と と も に 、 マ 二 ビ ユ レ ー タ の 変位 に 応 じ て変化する 非線形 ト ル ク を块定 す る た め の補助 C P U が設け られ、 そ こ で フ ィ ー ド フ ォ ワ ー ド 補償 に 必要な演箕ゃ制御信号の形成 を 行 な っ て い た。 こ の よ う な駆動 ト ル ク を演算す る補助 C P U が、 サ — ボ制御 回路毎に設 け られる位置指今の た め の演算手段 と は別 に必要 と な る た め、 制御装置の コ ス ト が高 く な り 、 し か も 各 C P u 間 の デー タ 制御が複雑化す る な どの 問 題が あ つ た。

ま た 、 各 サ ー ボ C P U では、 サー ポ モ ー タ の 制御 に 必 要 な 負荷の等価 イ ナ一シ ャ ゃ非線形 ト ル ク の演箕 を 前記 C a ) 式 に よ り 行 な う が、 こ の と き 、 他 の軸回 り の 回転 角等の情報 を 取込む 必要があ り 、 こ のた め 、 各 サ ー ボ C P U 毎 に 行 な つ て い る 電流ル ー プ の 処理が妨害 され、 サ一 ボ モー タ の電流制御が精度良 く 行な われな く な る と い ラ 問題があ っ た。

さ ら に各サ ーボモ ー タ のサー ボ制御回路毎に ¾動 ト ル ク 計算 を行な う ため に コ ン ト ロ ー ラ に入力 され る ハ ン ド お よび ワ ー ク の重量値は、 メ イ ン C P U の コ ン ト α— レ プ Q グ ラ ム に おいて予め設定されてい る の で 、 規 ¾ε重量 以外の ワ ー ク の処理 に あたっ て、 ォペ レ一タ は プ C2 グ ラ ム を変更 し な く ては な らず、 それに大 き な労力 を必要 と する な どの問題があ っ た。

発 明 の 開 示

本発明は、 こ の よ ラ な従来技術の問題点を解消 し 、 各 サー ボモー タ を制御す る サーボ C P U を 設け て 、 これ ら サ ー ボ C P U で 、 駆動 ト ル ク を 演算 す る よ う に し た α ボ ッ ト 制御装置を提供する こ と を 目 的 と し てい る 。

*発明の別 の 目 的 は 、 各サーボモー タ を制御する サ ー ボ C P U 間を接続 し てい るバ ス ラ イ ン に オ ペ レ ー タ が適 宜にハ ン ドゃ ワ ー ク の重畺値を入力 して駆動 ト ル ク を演 箕する よ う に し た ロ ボ ッ ト 制御装置を提供する こ と に あ る 。

に术発明 の別の 目 的は、 各轴毎の情報を共通の R A

Mな ど に記億 し て、 各サーボ C P Uか ら 共通に ア ク セ ス で き る よ う に した α ボ ッ ト 制御装置を提供する こ と に あ る 。

末発明 に よれば、 複数轴の ロ ボ ッ ト ア ー ム を媒介に し てマニ ピ ュ レ 一 タ の姿勢制御を行な う 口 ボ ッ ト 制御装置 におい て、 ロ ボ ッ ト ア ー ムを駆動する複数軸分の サー ボ モー タ と 、 各サー ポ モ ー タ の制御系の駆動 ト ル ク を演箕 す る複数の演箕手段 と 、 前記演箕手段を 相互に接統 し て 駆動 ト ル ク の演算に必要な他軸の情報を該演算手段か ら ア ク セ ス可能 と す る バ ス ラ イ ン と を具備する こ と を特徴 と する ロ ポ ッ ト 制御装置を提供でき る。

図面の簡単な説明

第 1 図は、 太発明 の一実施例を示すブ ロ ッ ク 図、 第 2 図は、 水平関節型 ロ ボ ッ ト の一例を示す機構図 、 第 3 図 は、 同 ロ ボ ッ ト の外観図、 第 4 図は、 従来装置の構成 を 示す プ ロ ッ ク 図であ る 。

発明 を実施するための最良の形態 以下 *発明 の実施例 を 、 図 面 を 用 い て詳細 に説明 す る 。

第 2 図は、 長 さ Α ι の リ ン ク L l 、 長 さ £ 2 の リ ン ク

L 2 、 長 さ £ s の ハ ン ド H を有する水平関節型 ロ ボ ッ ト の機構図であ る 。 リ ン ク L 1 は 0 ₁ 铀の 回 リ で 、 リ ン ク は 0 2 轴の 回 り で、 ハ ン ド Hは e ₅ 轴の回 り でそ れ ぞれ回転可能 に構成 されてい る。

で、 リ ン ク L 1 の全重量を M i ( M 1 = m 1 1 + m 1 2 + m i ₅ ) 、 リ ン ク L 2 の全重量 を M ₂ C M 2 = m 2 1 + m 2 2 + m 2 5 ) 、 ハ ン ド ' H及び ワ ー ク Wの重 量を M C M 5 = m h + M w ) 、 リ ン ク L i 、 L 2 及び ハ ン ド H の重心位置 を それぞれ , W 2 , W 5 と すれ ば、 マニ ピ ュ レ ー タ の各轴の ト ルク T i , T 2 , T s は 次の よ う に し て求め られる。

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

ハ ン ド H の 0 5 軸回 リ の ト ルク T ₅ は 、

T ₅ = K i 6 s + f i ( ^ 5 ) Θ 2.

+ f 2 ί 2. , ^ δ ) Θ 1 + f 5 ( Θ 5 ) Θ Z ^Z + f 4 Θ Z , 0 s ) Θ 1 ²

+ f 5 C θ _δ ) Θ i Θ 2.

= ( A ² M _S + J s yy) θ s

+ ( A ² M 5 + A M 3 Jl 2 cos ^ 5 + J s ") Θ 2 + { A M s ( A + i 2 cos 5

+ β ! co s ( ^ 2 + ^ 5 ) + J s yy) } & 1 + C A M 5 4 2 s in Θ s ) ^ 2 ^z

+ { A M 5 C δ 2 s in 0 s

+ C 2 A M _S fi z s i n & 5 ) ι ^" 2 ― ( 1 ) こ こ で、 係数 A は 、

A = fi _δ - S 5

-e ハ ン ド H の先端か ら ワ ー ク を含めたハ ン ド H の重心 位置ま での距離 S ₅ は、

S 5 = C ΰ s - δ 3 l ) ω. . / C m h + M w )

… C 2 ) と し て算出 され る 。

また 、 八 ン ド H の重心回 り の慣性をハ ン ド H に固定 さ れた座標系 で表わ し た J _δ 3?yは、

J 5 y y = m h ( Jl ₅ - fi s ι S _s ) ² + M w S ₅

( 3 ) と な る が 、 ハ ン ド H が ワ ー ク を 把持 し て い な い場合 に

11

は 、 上記各式 に お 21 け る M wを零にす る 。

ン ク L 2 の θ 2 c c軸回 の h ル ク τ 2 は 、

T 2 K 1 ' θ δ 22 + f 1 ( θ 3 ) θ 2

+ f 2 ' c θ 2 θ s ) θ 1

+ f 5 ' c θ 3 ) θ 2

2

+ f 4 ' ( θ 2 θ S ) 0 2

1

+ f 5 ' c θ 5 ) θ 1 & · 2

ζ

+ 2 θ s C 4 ) と な る

リ s 35

+

+

と な る

こ れ ら （ 1 ) , ( 4 ) , ( 5 ) 式 に お い て 、 の項 は 、 負荷の等価 イ ナ 一 シ ャ であ り 、 各轴 の 回転角 0 i の 変化 に 応 じ て変動す る 項であ る 。 第 1 図は、 本発明装置を適用 して制御 され る 口 ボ ッ ト ア ー ム の制御 シ ステ ム例を示すプロ ッ ク 図であ る 。

Θ 1 轴回 り に 負荷 を ¾動する サ一 ボ モ — タ 1 ， 0 2 軸 回 リ に 負荷を駆動す る サ一ボモー タ … の各制御ル ー プ に は、 それぞれの軸回 り の非線形 ト ル ク T i i θ , Θ ) ( i = 1 ， 2 ) を計算する サーボ C P U c ι ， c 2 が 設け られてお り 、 各サーボ C P Uが メ イ ン C P U a と バ ス ラ イ ン に ょ リ 結合 され、 メ イ ン C P U か ら の制御信号 に従 つ て各軸制御に必要な運動方程式を オ ン ラ ィ ン で 演 算が実行で き る 。 イ ンパータ <^ ₁ , d 2 は、 サ一ホ モ一 タ e i , e ₂ の電流を制御する。

*発明 に おいては 、 メ イ ン C P U a と 各サー ポ C P U c 1 ， C 2 '" と の間 に共有 （ S H A R E D ) R A M b が 設 け ら れ 、 か つ メ イ ン C P U a に よ リ 指令 される マ 二 ピ ユ レ ー タ の 制御指令 を 受 け て 、 ロ ボ ッ ト ァ — ム の 0

1 ， 0 2 …の各軸周 り で駆動される負荷 を制御す る複数 のサー ボ C F U C i ， C P U C 2 ."が、 バ ス ラ イ ン f を 介 し て相互に ア ク セ ス可能に接続されて い る 。 各サ一 ボ モ ー タ e i ， e ₂ は 、 それぞれサ一ボ C P U c 1 , C 2 イ ンパ― タ d i , d 2 と に よ り 、 所定の位置制御 と 電 流制御 を行な ラ 。 各サー ボ C P U c i , C 2 …で は 、 ¾ 記制御指今に基づ く 位置指今信号の応 じ た駆動 ト ル ク を 演算 してい る 。 即ち 、 各軸の駆動制御を行な う サ一 ボ C P U C ！ , C 2 '" に は、 他铀の回転角等 の情報が入力 さ れ、 それぞれ （ 1 ) ， （ 4 ) ， （ 5 ) 式の ト ル ク 指令 を 決定す る た め オ ン ラ イ ン処理を行なわせ 、 非線形 ト ル ク の項 を フ ィ ー ド、 フ ォ ワ ー ドで補償 させ る も の で あ る 。

ま た 、 上 記共 有 R A M b は 、 各 轴 回 り の情報 を 記憶 し 、 サ ー ボ C P U が電流ルー プ の制御 を 行 な わ な い と き に 、 該情報 を 読み出 し て ト ル ク 演算を行 な う た め に 、 各 サ ー ボ C P U c i , C 2 "' と か らの ア ク セ ス が可能 で あ リ 、 次 の よ う な機能 を 有する 。

即 ち 、 例 え ば、 0 1 軸回 り の ト ル ク Τ ι を サ ー ポ C P U c 1 に よ り 、 前記 （ 5 ) 式で演箕する 場合 に 、 他の ø 2 軸、 e _s 軸 に 関す る 情報が共有 R A M b に記憶 され 、 サ ー ボ C P U c i で は 、 電流ル ー プ の処理 を 行 な わ な い 時刻 に 、 共有 R A M b に ア ク セ ス し て他軸回 り の情報 を 読 み取 り 、 所定 の演算 を行な う 。 こ のた め 、 バ ス 競合 （C ONTENTION)で読み込 み の ロ スが生 じ て も 、 電流ルー プ の 処理 を 防 げ る こ と な く 、 サ一 ボ モー タ の 制御 に 必要な 非 線形 ト ル ク の演箕が確実に行なえ る 。

さ ら に こ の 共有 R A M b に は、 オ ペ レ ー タ が操作 し 、 かつ画像デ ー タ と し て マ ニ ピ ュ レ ー タ の動作確認が可能 な C R T / M D I g が接铳され、 こ の C R T Z M D I g に よ リ ハ ン ド、 お よ び ワ ー ク の重量を設定 で き る ょ ラ に し て お く 。

次 に 、 *発明装置 の作用について説明 す る 。

メ ィ ン C P U a が作動 して シ ス テ ム を 立 ち 上 げた後 、 オペ レ ー タ が 、 ロ ボ ッ ト ハ ン ド およ び ワ ー ク の 重量 を C R T M D I g か ら メ イ ン C P U a を介 し て共有 R A M b に入力する と と も に 、 メ イ ン C P U a か ら 、 各サー ボ モー タ e i ， β 2 ··· に対する位置指令を出力す る と 、 口 ボ 、 _y ト ア ー ム は始動 されて、 負荷の各轴回 り の情報を共 有 R A M b に記憶させ る。 次に、 各サー ボ C P U c i , c 2 … は、 他铀の轴回 り 情報を、 電流ルー プの制御を行 な わ な い 時刻 に 共有 R A M b か ら 読 み出 し て 、 それぞ れ、 前記 （ 1 ) ， （ 4 ) , ( 5 ) 式に ょ リ 非線形 ト ル ク T ( 6 , Θ ) を オ ン ラ イ ン で演算 し て、 サ一 ボ制御系 に 対 し て 、 フ ィ 一 ド フ ォ ワ ー ド補償を行な う 。

な'お、 ハ ン ド 自 体の重量値は通常変化 し ない ので ヮ ー ク重量値のみ を入力す る よ う に して も よ い。

また、 オ ペ レ ー タ は、 ロ ボ ッ ト ハ ン ド、が所定位置ま で 移動 し た こ と を C R T な どで確認 した と き 、 必要に応 じ て ワ ー ク重量値を変更入力する こ と が で き る 。

産業上の利用可能性

以上説明 し た よ う に 、 本発明の口 ボ ッ ト 制御装置に よ れば、 ロ ボ ッ ト ア ー ム の複数の駆動軸回 り で回動 され る 負荷を 、 マ ニ ピ ュ レ ー タ の運動に合せて 、 マニ ピユ レ一 タ 諶動用サ一 ボモ ー タ で題動制御する に あ た リ 、 各サ ー ポ C P U に て 、 サー ボモー タ の制御に必要 と され る駆動 ト ル ク の演算 を行わ せる の で、 各サー ボ制御ルー プ毎 に 別個に駆動 ト ル ク 演算用の補助 C P U を 設け る 必要が な く 、 制御ルー プの コ ス ト が低減でき る。

また 、 各サ ー ボモ ー タ を制御する サー ボ C P Uか ら 共 通に ア ク セ ス で き る R A Mを設け、 各サ ー ボ C P U が電 流ル ー プ処理 を 行 な わ ない時刻 に他轴回 リ の情報 を読 み 取 り 、 非線形 ト ル ク の演算を行な う の で 、 電流ル ー プ の 処理 を 妨害す る こ と な く 、 精度良 く サー ボ モ ー タ の制御 が行 な え る 。

さ ら に ロ ボ ッ ト ア ー ム に よ り 把持 され る ワ ー ク 重量値 を 各サ ー ボ C P U に 適宜必要な タ イ ミ ン グで変更設定 し て入力 し 、 サ 一 ボ モ ー タ の制御に必要 と され る 駆動 ト ル ク の演算 を行 わせて い る ので、 ワ ー ク の 重量値 の変動 に オ ペ レ ー タ が容易 に 対処でき 、 また種 々 の ワ ー ク 処理 を 行 な ラ ラ え で の シ ス テ' ム 変 更 が 容 易 に 行 な え る 。 し た が っ て ま た 、 本発明 の ロ ボ ッ ト 制御装置 を 水平関節型 口 ボ ッ ト に 適用 す る と き 、 一屠正確な位置制御が可能 に な り 、 高速 でか つ精度 の 良い ロ ボ ； ト 制御 がで き る 。

Claims

請 求 の 範 囲

C 1 ) 複数铀 の ロ ボ ッ ト ア ー ム を 媒介 に し てマニ ピ ュ レ ー タ の姿勢制御を行な ラ ロ ボ ッ ト 制御装置は、 次の手 段を含む ：

前記 ロ ボ ッ ト ア ー ム を駆動する複数軸分の サ ー ボモ ー タ ·，

各サ ー ボモ ー タ の制御系の駆動 ト ル ク を演算する複数 の演箕手段 ；

前記演算手段を相互に接練 して蘿動 ト ル ク の演算に必 要な他轴の情報を該演算手段か らア ク セ ス可能 と する バ ス ラ イ ン 。

( 2 ) 前記マ ニ ピ ュ レ ー タ の運動方程式の計算 に必要 な 他轴の情報を保持する と と も に各演算手段か ら共通に ァ ク セ ス可能な記憶手段を具備 したこ と を特徴 と す る前記 請求の範囲第 1 項に記載の σボ ッ ト 制御装置。

C 3 ) 前記 ロ ボ ッ ト ア ー ム先端のハ ン ド重量お よび該ハ ン ド に よ り 把持 され る ワ ー ク重量と を適宜所定の演箕手 に入力する デー タ 入力手段を具備 した こ と を特徵 と す る 前記請求の範囲第 1 項乃至第 2項に記載の ロ ボ ツ ト 制 御装置。

( ) 前記演箕手段 は 、 α ボ ッ ト ア ー ム の各轴毎に 、 サー ボ モー タ に対す る サーボ制御系の フ ィ ー ド フ ォ ヮ一 ド補償 を行な う サー ボマ イ ク ロ プロ セ ッ サ を含む こ と を 特徴 と す る前記請求の範囲第 1 項乃至第 3 項に記載の 口 ボ ッ ト 制御装置。

( 5 ) 前記複数轴の ロ ボ ッ ト ア ー ムは、 各ア ー ム が水平 面内で回動す る 水平関節型ロ ボ ッ ト のア ー ム で あ る こ と を特徴 と する 前記請求の範囲第 1 項乃至第 4 項 に記載の 口 ボ ツ ト 制御装置。