JPH01191209A - 多関節アーム制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 え肌Ω旦釣 ［産業上の利用分野コ 本発明は多関節アーム制御装置に関し、特に−連のアー
ムの先端振動を抑制し、同時にアーム先端の位置決め精
度の向上を図るものである。

［従来の技術］ 作業用アームを備えた産業用ロボットの制御システムと
して、例えばアームの各関節毎のサーボ系においては、
電動機等の出力軸の回転角度を検出するエンコーダの出
力と、回転速度を検出するタコジェネレータの出力とを
フィードバックするシステムが過密実施されている。し
かし、高負荷重量のロボットにおいては、アームの慣性
モーメントが大きく、減速機、トルク伝達機構及びアー
ム等のはね要素により、アームの振動に関する固有周波
数が低下するため、アーム先端に低周期で振幅の大きな
振動が発生しやすくなりアーム先端の位置決め精度が低
下するといった問題カ生シ、いた。

この課題に関しては、既に系の内部モデルを設計し、こ
のモデルからの推定出力値と実際の出力値との偏差を零
とするようにモデルの人力を修正して、修正後のモデル
出力値と人力値と参ロボ・ントの位置指令値にフィード
バックすることにより振動を防止しようとする提案がな
されている（特開昭６１−２５５４１５号）。

［発明が解決しようとする課題］ しかし、この様なシステムでは、複数あるアーム毎に独
立にモデルが設定されているが、実際にはアーム同士が
干渉する。このため、各アームが独立とは見なせずモデ
ルが不適切なものとなる。

従って、真に正確な制御が実施されいるとはいい難く、
振動の防止や位置決め精度が不完全であった。ただ、一
連のアームで−モデルを設計すると、非線形になり、上
記手法をそのまま適用することはできなかった。

発夏駆ｌｌ成 本発明は、上記問題点を解決することを目的とするもの
であり、元来非線形であるアームの数式モデルを各アー
ムの角度毎に線形として取り扱うことにより、アーム振
動の効果的防止及び高精度制御を実現する多関節アーム
制ｔＭＪ装置を提供するものである。

［課題を解決するための手段］ 即ち、本発明の要旨とするところは、第１図に例示する
ごとく、 一連に配設された複数のアームＭ１と、この各アームＭ
１を動作させるアクチュエータＭ２と、 アクチュエータＭ２の位置を検出する位置検出手段Ｍ３
と、 上記アクチュエータＭ２を制御しアームＭ１に所定の動
作をさせる制御手段Ｍ４と、 を備えた多関節アーム制御装置において、上記制御手段
Ｍ４が、上記アクチュエータＭ２及びアームＭ１によっ
て構成される系をはねと質量とからなる系と仮定して設
定された数式モデルＭ５であって、アクチュエータ駆動
指令値から少なくともアクチュエータ変位またはアーム
変位を推定する非線形の数式モデルＭ５を用いるととも
に、上記アクチュエータＭ２の変位毎に上記数式モデル
Ｍ５を線形として取り扱うことにより、アクチュエータ
駆動指令値及びアクチュエータ変位の実際値に応じてフ
ィードバック補償量を求め、このフィードバック補償量
にて上記アクチュエータ駆動指令値を補償することを特
徴とする多関節アーム制御装置にある。

［作用コ 本発明の多関節アーム制御装置は多関節であるために、
アクチュエータ変位またはアーム変位が推定できる数式
モデルＭ５が非線形となっているが、各アームがある変
位にある場合は、その変位での線形のモデルＭ５と見な
すことができる。従って、非線形な数式モデルＭ５も、
アクチュエータＭ２またはアームＭ１の変位に応じた線
形な数式モデルが存在すると考えることができる。

このことから現代制御理論にて数式モデルＭ５を解くこ
とができるので、制御手段Ｍ４はこの数式モデルＭ５に
より、アクチュエータ駆動指令値及びアクチュエータ変
位の実際値からフィードバック補償量を求め、上記アク
チュエータ駆動指令値を補償する。このことにより、停
止した際の一連のアームＭ１の先端振動を抑制できると
ともに、その振動を考慮してアームＭ１の先端を正確な
位置に制御できる。

次に、本発明の詳細な説明する。本発明はこれらに限ら
れるものではなく、その要旨を逸脱しない範囲の種々の
態様のものが含まれる。

［実施例コ 第２図、第３図に本発明多関節アーム制御装置の一実施
例を示す。第２図はそのアーム部分の概略構成図である
。基台１上には第１アーム３、第２アーム５及び第３ア
ーム７が各揺動軸９．１１゜１３を介して揺動自在に連
結されている。この各軸９，１１．１３には後述するサ
ーボモータの出力軸がギヤの刊合せからなる減速機構を
介して結合されており、上記アーム３．　５．　７は、
後述する電子制御回路によりサーボモータが駆動される
と所望の回転位置に位置し得るように構成されている。

また第３アーム７の先端には溶接用の電極１５が配置さ
れ、所望の位置で電気溶接できるように構成されている
。

第３図に本実施例の全体のブロック図を示す。

上記各軸９，１１．１３には各アーム３，５゜７の揺動
角度を検出するためのエンコーダ１７゜１９．２１が設
けられ、電子制御回路２３にその信号が人力されるでい
る。また各軸９．１１．１３に設けられた上記サーボモ
ータ２５．　２７．　２９は上記電子制御回路２３にて
その回転角度が制御されている。上記電子制御回路２３
の構成の内、ＣＰＵ２３ａは人力される信号に基づいて
サーボモータ２５．２７．２９の駆動量等の演算及びそ
の制御を行う。また読み出し専用メモリであるＲＯＭ２
３ｂにはＣＰＵ２３ａの処理において使用されるプログ
ラム、定数あるいは後述する数式モデルのデータ等が格
納されている。読み出し書き込み可能なメモリであるＲ
ＡＭ２３ｃにはＣＰＵ２３ａで求められた演算結果や各
エンコーダ１７゜１９．２１からの検出データ等が一時
記憶される。

人力部２３ｄは、各エンコーダ１７．　１９．　２１や
指示人力用キーボード３１等からの信号を受信するとと
もに、それらの信号に対して波形成形等の信号処理を実
行する。出力部２３ｅは、サーボモータ２５．２７．２
９の駆動回路を含み、ＣＰＵ２３ａで実行された処理結
果に基づき、サーボモータ２５．２７．２９あるいは図
示しない溶接用高電圧回路等を必要に応じて駆動させる
ための信号を出力する。コモンバス２３ｆは、ＣＰＵ２
３ａ、ＲＯＭ２３ｂ、ＲＡＭ２３ｃ、人力部２３ｄ、出
力部２３ｅ間を結び、データを相互伝達する。

ここで、上記一連のアーム３．　５．　７の数式モデル
を設定するために座標系を第４図のように衷す。ただし
第１アーム３と第２アーム５とはｘｙ平面上で揺動動作
するものとし、その揺動の間は第３７−ム７は揺動して
いないものとする。Ｉ２１゜ρ２．ρ３は各アーム３．
　５．　７の長さ、ｄｉ、ｄ２゜ｄ３は軸９．１１．１
３から各アーム３．　５．　７の重心までの距離、ＤＡ
Ｉ、　　θＡ２は第１，２アーム３．５の揺動角を表し
ている。更に、以後に出て来るｍｌ、ｍ２．ｍ３は各ア
ーム３．　５．　７の質量、ＤＡＩ、　　ＤＡ２．　　
ＤＡ３は各アーム３．　５．　７の粘性抵抗（例えば摺
動摩擦等）を表すものとする。

また第３アームの先端での振動として、２次元のものを
考え、第２７−ム５の延長方向の振動成分とそれと垂直
な振動成分とに分ける。そのため、第５図に示すごとく
、第２アーム５の延長線ｐ１とＺ軸とのなす平面αに第
３アーム７を投影した線ｑ１と、第２アーム５に対する
ｘｙ平面上での垂線ｐ２と２軸のなす平面βに第３アー
ム７を投影した線ｑ２とを考え、第３アーム７の角度を
、上記線ｑ１と２軸との角θＡ３１及び上記線ｑ２と２
軸との角θＡ３２で表すものとする。

上記アーム３．　５．　７の運動方程式は、ラグランジ
ェ力学により以下の通りにして導出される。

ラグランシアンＬはシステムの総運動エネルギにと総位
置エネルギｐｃ７）差として、次式のように定義される
。

Ｌ＝に−Ｐ　　　　　　　・・・（１，１）そして運動
方程式は、運動・位置エネルギを衷した座標によって、
次式のように与えられる。

ｄ　　　　２３Ｌ　　　　　ＢＬ　　　　ＥＩＤτ１　
＝　□◆　−７−−　　＋　　−Ｔ−ｄｔ　　　Ｅｌｑ
ｉ　　　　　ａｑｉ　　　　ａｑｉ・・・（１，２） ここでｑｌは運動・位置エネルギを表す座標、ｉｌはそ
の速度、τｉは対応するトルク、Ｄは散逸エネルギであ
る 上記運動方程式を上記一連のアーム３．　５．　７　　
、に適用するために、次のような処理をする。

（１）まず各アーム３．　５．　７の重心の位置を求め
ると、次のように表される。

■第１アーム３の重心座標 ｘ１＝ｄ１４ＣＯ５θＡｌ ｙｌ＝ｄｌ◆ＳＩＮθＡｌ ｚ　ｌ＝　Ｏ−（１，３ａ） ■第２アーム５の重心庫標 ｘ２＝ρ１◆Ｃ０５ＢＡＩ＋ｄ２・Ｃ０５（θＡ１＋θ
Ａ２）ｙ２＝＝ρ１・ＳＩＮθ八１＋へ２・５ＩＮ（θ
Ａ１十〇Ａ２）Ｚ２＝０　　　　　　　　　　　　　　
　　　　−・・（１，３ｂ）■第３アーム７の重心座標 ｘ３＝ρ１・ＣＯＳθＡ１＋ρ２・Ｃ０５（θＡ１＋θ
Ａ２）＋ｄ３・ＳＩＮθＡ３１◆Ｃ０５（θＡ１十〇Ａ
２）−Ｃ３・ＳＩＮθＡ３シ・５ＩＮ（ｅＡｌ十〇Ａ２
’）ｙ３＝＝ρ１◆ＳＩＮθＡｌ−１２◆５ＩＮ（θＡ
１＋θＡ２）＋ｄ３◆ＳＩＮθＡ３１・５ＩＮ（θＡ１
＋θＡ２）＋ｄ３・ＳＩＮθＡ３２・Ｃ０５（θＡ１＋
θＡ２）ｚ３＝　−Ｃ３（１−５ＩＮ２θＡ３１−５Ｉ
Ｎ”’θＡ３２）　１′２・・・（１，３ｃ） （２）各アーム３．　５．　７の重心の速度は（１゜３
）式を微分することにより、次のように表される。

■第１アーム３の重心速度 １１＝−ｄｉ◆ＳＩＮθＡ１・ＥＩＡＩシ１＝ｄ１・Ｃ
Ｏ５θＡ１・ＯＡ１ シ１＝０　　　　　　　　　　　・・・（１，４ａ）■
第２アーム５の重心速度 、Ｑ２＝＝−ρ１・ＳＩＮθＡ１◆υＡ１−Ｃ２・５Ｉ
Ｎ（θＡ１十〇Ａ２）・ｃ　？ｒ　Ａ１＋　？ｒ　Ａ２
）Ω２＝ｇ１・ＣＯ５θＡ１・ｂＡ１ ＋ｄ２・Ｃ０５（θＡ１＋θＡ２）◆＜　０　Ａ１＋　
６　Ａ２）乏２＝０　　　　　　　　　　　　　　　　
　・・・（１，４ｂ）■第３アーム７の重心速度 交３＝−４１−５ＩＮθＡ１・ａＡｌ −ρ２・５ＩＮ（θへ１十〇Ａ２）・（ｂへ１＋　９　
Ａ２）−Ｃ３・ＳＩＮθＡ３１・５ＩＮ（θＡ１＋θＡ
２）・（ｂＡｌ＋υＡ２）−Ｃ３・５ＩＮ８Ａ３２・Ｃ
０５（θＡ１＋θＡ２）・＜　？ｒ　Ａ１＋　９　Ａ２
）＋ｄ３◆ＣＯＳθＡ３１◆Ｃ０５（θＡ１＋θＡ２）
◆ｂＡ３１−ｄ３◆ＣＯＳθＡ　３’２◆５ＩＮ（θＡ
１＋θＡ２）◆ｂＡ３２シ３＝ρ１・Ｃ０５ＢＡＩ・６
Ａｔ 十ρ２・Ｃ０５（θＡ１＋θＡ２）・（ａ　Ａ１＋　？
ｒ　Ａ２）＋　ｄ　３−５＋ｘｅ　Ａ３１−ｃｏｓ（θ
Ａｘ＋θＡ２）−（６Ａ１＋ｏ　Ａ２）−Ｃ３・ＳＩＮ
θＡ３２・５ＩＮ（θＡ１十〇Ａ２）◆（ａＡ１＋υＡ
２）＋　ｄ　３◆ＣＯＳθＡ３１◆５ＩＮ（θＡ１＋θ
Ａ２）・ｂＡ３１＋ｄ３◆ＣＯＳθＡ３２争Ｃ０５（θ
　Ａ１＋θ　八２）◆　ｂ　Ａ３２シ３＝ Ｃ３・［ｇ（θＡ３１）　＋ｇ　（θＡ３２）コ（１−
５ＩＮ２θＡ３１−５ＩＮ２θＡ３２）１／２・・・（
１、４ｃ） ただし、ｇ（θ）　＝ＳＩＮθ・ＣＯＳθ・ｂである。

（３）上述の関係から各アーム３．　５．　７の運動エ
ネルギに１．に２．に３は、次のように表される。

■第１アーム３の運動エネルギＫＩ Ｋｌ　＝（１／２）ｍｌ（ｘ１２＋ｙ１２＋シ１２）＝
　（１／２）ｍｌ　ｄ　１２・６　Ａ１２・（１，５ａ
）■第２アーム５の運動エネルギに２ Ｋ　２　＝　（１／２）ｍ２・（ｘ　２２＋　ｙ　２”
　＋　Ｍ　２”’）＝（１／２）◆ｍ訃 （、ζ？１２・６Ａ１２＋ｄ２２・（ｂへ１＋　９　Ａ
２）２＋２ｇ１・Ｃ２・ＣＯ５θＡ２・ＯＡｌ・（υＡ
１＋？ｉ　Ａ２））・・・（１，５ｂ） ■第３アーム７の運動エネルギに３ に３　＝（１／２）ｍ３・（ｘ　３２＋　＊３”＋　ｚ
　３２）二ｍ３・（ρ１・ｇｌ・ＣＯ５θＡ２＋　１１
−　ｄ　３−（ＣＯ５θＡ２−５ｔＮθＡ３１−５ＩＮ
θＡ２◆ＳＩＮθＡ３２）十ρ２◆ｄ３◆ＳＩＮθＡ３
１＋（１／２）・　［ｇ１２十ｇ２２　＋　　ｄ　３”
・（ＳＩＮ２θ八３１＋５ＩＮ２θへ３２）］　）　・
６ＡＩ”＋ｍ３・Ｃ２・Ｃ３・ＳＩＮθＡ３１＋　（ｍ
３／２）・（ρ２２　＋　ｄ　３２・（ＳＩＮ２θＡ３
１＋５ＩＮ２θＡ３２））　・０Ａ２２＋　（ｍ３／２
）　ｄ　３”−ＣＯ５２θＡ３１−　［１＋５ＩＮ２θ
Ａ３１／（１−５ＩＮ２θＡ３１−５ＩＮ２θＡ３２）
コ　−ｏ　Ａ３１２　＋　（ｍ３／２）Ｃ３２・ＣＯ５
２θＡ３２・［１＋５ＩＮ２θＡ３２／（１−５ＩＮ２
θＡ３１−５ＩＮ２　θＡ３２）］　　◆　υ　Ａ３２
２＋ｍ３争　（ρ　１◆ρ２争ＣＯ５θ　Ａ２十４１◆
ｄ　３−（ＣＯ５θＡ２◆５ＩＮｅＡ３１−５ＩＮθＡ
２−５ＩＮ＆　Ａ３２）＋２ρ２・Ｃ３・５ＩＮ２θＡ
３１＋　４２２　＋　ｄ　３２・（ＳＩＮ２θＡ３１＋
５ＩＮ２θＡ３２））　・６　Ａ１・０　Ａ２＋　ｍ３
・ｄ　３◆ＣＯ５θＡ３１−（ｇｌ−５ＩＮθＡ２−　
ｄ　３−５ＩＮθＡ３２）６Ａ１・６Ａ３１＋ｍ３・Ｃ
３・ＣＯ５θＡ３２・１４２１・ＣＯ５θＡ２＋ρ２＋
ｄ３・ＳＩＮθＡ３１）　？Ｊ　Ａ１◆０　Ａ３２−　
ｍ３◆ｄ　３２◆ＣＯ５θＡ３１◆ＳＩＮθＡ３２◆ｂ
Ａ２φ６　Ａ３１＋　ｍ３◆ｄ　３◆ＣＯ５θＡ３２◆
（ｄ２＋　ｄ　３−５ＩＮθＡ３１）◆υＡ２・？３　
Ａ３２＋（ｍ３◆ｄ　３２◆ＳＩＮ　８　Ａ３１令ＣＯ
５θ八３１◆ＳＩ　Ｎ　θＡ３２−ＣＯ５ｅ　Ａ３２−
０　Ａ３１−υＡ３２）／（１−５＋１１１２　／３　
Ａ３１−５ＩＮ２θＡ３２） ・・・（１，５ｃ） （４）上述の関係から各アーム３．　５．　７の位置エ
ネルギＰＬ、Ｐ２．Ｐ３は、次のように表される。

■第１アーム３の位置エネルギＰ１ Ｐ　１＝　Ｏ・・・（１，６ａ） ■第２アーム５の位置エネルギＰ２ Ｐ　２＝　０　　　　　　　　　　　　−（１，６ｂ）
■第３アーム７の位置エネルギＰ３ Ｐ３＝ −ｍ３・ｄ３・ｇ・（１−５ＩＮ２θＡ３１−５ＩＮ”
θＡ３２）”　２・・・（１，６ｃ） （５）全体の散逸エネルギＤは次のように表される。

Ｄ＝ １／２争　（ＤＡｌ令ａＡ１２＋ＤＡ２φｂＡ２２＋　
Ｄ　Ａ３争（ＯＡ３１２＋　ＯＡ３２２））・・・（１
，７） ラグランジアンＬは（１，１）式に基づき下式のごとく
表される。

Ｌ＝に１＋に２＋に３−Ｐｌ−Ｐ２−Ｐ３・・・（１，
８） この式を用いて（１，２）式より各アーム３，５゜７で
のトルクを求めた後、以下の操作により簡略化を行う。

即ち、θＡ３ｉ、　　Ｍ３ｉ　（ｉ＝１．　２）は微小
と考えると、 ＳＩＮθＡ３１＝θＡ３１ ＣＯ５θＡ３１＝１ θＡ３１２＝０ ６Ａ３ｉ”　　＝０　　　　　　　　　・・・（１，９
）と近似できる。

このようにして簡略化したトルクは以下の式で表される
。

ｒ１＝　（ｔｎｌ◆ｄ１２＋ｍ２◆ｇ１２＋ｍ２◆ｄ２
２＋２ｍ２−６１−ｄ　２−ＣＯ５θＡ２＋ｍ３・ｇｌ
”＋ｍ３・６２２＋２ｍ３※ｇ１◆＃　２−ＣＯ５θＡ
２＋　２　ｍ３令ｄ　３◆（Ｉ２２＋　１１・ＣＯ５θ
Ａ２）・θＡ３１−２ｍ３・６１・ｄ３＊θＡ３２◆Ｓ
ＩＮθＡ２）・ｔＡｌ＋（ｍ２・ｄ２２＋ｍ２・ρ１・
ｄ２・ＣＯ５θＡ２＋ｍ３・ρ２２＋ｍ３・ρｌ◆ρ２
・ＣＯ５θＡ２＋ｍ３◆ｄ３◆（２ρ２＋！２１◆ＣＯ
５θＡ２）　−θＡ３１−ｍ３・ρ１・ｄ３・θＡ３２
−５ＩＮθＡ２）会？ｊＡ２＋ｍ３◆ρ１−ｄ３争ＳＩ
ＮθＡ２◆ｂ　Ａ３１＋ｍ３◆ｄ３◆　（、ζ？ｌ◆Ｃ
Ｏ５θＡ２＋！２２）　　◆＃Ａ３２−　　（（ｍ２争
、（？トｄ２＋ｍ３−６１◆ｇ２）−５ＩＮθＡ２千ｍ
３φＩ２１◆ｄ３φ（θＡ３１−５ＩＮθＡ２＋θＡ３
２・ＣＯＳθＡ２））・（ｂＡ２２＋２０ＡＩ・０　Ａ
２）＋　２　ｍ：ｂ　ｄ　３・（４？　１・ＣＯＳθＡ
２＋ρ２）　−（ａＡｌ−ａＡ３１＋１９Ａ２・６Ａ３
１）　−２ｍ３・＃１・ｄ３・ＳＩＮθＡ２−（ａＡｌ
−？ｔ　Ａ３２＋　０　Ａ２・ａＡ３２）　＋ＤＡ１・
ｂＡｌ・・・（１，１０） ｒ　２＝　　（ｍ２φｄ　　２”　＋　ｍ２◆４　１◆
ｄ　　２争ＣＯ５θＡ２＋ｍ３◆ｇ２２＋ｍ３．ρ１◆
ρ２φＣＯ５θＡ２＋ｍ３◆ｄ３◆（ρ１φＣＯ５θＡ
２＋：１１２）　　會θＡ３１−ｍ３・Ｉ２１舎ｄ３・
θＡ３２・ＳＩＮθＡ２）　◆ｂ　Ａ１＋　（ｍ２・ｄ
　２２＋ｍ３◆ρ２２＋　２ｍ３争ρ２・ｄ３◆θＡ３
１）◆ｔＡ２＋ｍ３◆ρ２◆ｄ３・ｂＡ３２＋（（ｍ２
◆ρ１争ｄ２＋ｍ３争ｇ１４ρ２）　舎ＳＩＮθＡ２＋
ｍ３◆ρ１争ｄ３争　（θＡ３１　◆ＳＩＮ　θＡ２＋
θＡ３２−ＣＯ５θＡ２）　　）　　争６Ａ１２＋２ｍ
３・ρ２◆ｄ３・＜ｅＡｔ・０　Ａ３１＋　６　Ａ２・
υＡ３１）　＋　ＤＡ２◆ｂＡ２　　　　　　　　　　
　　　　　　　　　　　・・・（１゜１１）ｒ３１＝　
　（ｍ３◆＃１争ｄ３φｓＩＮθＡ２−ｍ３◆ｄ３◆θ
Ａ３２）・ｄＡｌ−ｍ３◆ｄ３２◆θＡ３２◆６　Ａ２
＋　ｍ３◆ｄ　３”◆？３　Ａ３１−ｍ３・ｄ３争　（
ρ２＋ρ１−ＣＯ５θＡ２＋ｄ３・θＡ３１）　　◆ｅ
　　Ａｔ２−ｍ３・ｄ３・（ρ２＋ｄ３・θＡ３１）　
◆（ａＡ２２＋２６Ａ１φ６Ａ２）−２ｍ３◆ｄ３２◆
（ｆ５Ａ１◆６Ａ３２＋　６Ａ２φｊＡ３２）　＋ＤＡ
３◆　０Ａ３１＋ｍ３・ｄ３φｇ・θＡ３１・・・（１
，１２） Ｔ　３２＝　ｍ３・ｄ　３・（４？　２＋　（２１・Ｃ
Ｏ５θＡ２＋　ｄ　３−θＡ３１）　・？ｊ　Ａ１＋ｍ
３・ｄ　３・（＃２＋　ｄ　３・θＡ３１）　・？Ｊ　
Ａ２＋　ｍ３・ｄ３２・ｖＡ３２＋ｍ３・ｄ３◆（ρ１
・ＳＩＮθＡ２−ｄ３−θＡ３２）　・６Ａ１２−ｍ３
・ｄ３２・θＡ３２◆（ｆ）　Ａ２２＋　２６　ＡＩ−
６Ａ２）　＋　２ｍ３◆ｄ３”・（６ＡＩ・６Ａ３１＋
　６Ａ２◆６　Ａ３１）　＋ＤＡ３φ０Ａ３２＋ｍ３◆
ｄ３◆ｇ◆θＡ３２・・・（１，１３） ここで（１，１０）−（１，１３）式に基づき、・・・
（１，１４） とおくと、 ・・・（１，１５） ここで、 Ｃ１１＝　ａ３３（ａｌｌ・ａ４４−　ａｌｌ・ａ４２
）／ＶＣ１２＝（−ａ　１２・ａ３３会ａ４４＋　ａ　
１３・ａ３２・ａ４４＋　ａ　１４・ａ　３３・ａ　４
２）／　Ｖ Ｃ１３＝　ａ１３令（ａ２４◆ａ４２−　　ａ２２会ａ
４４）／　ＶＣ１４＝（ａ１２・ａ２４◆ａ３３−　　
ａ１４＋ａ２２◆ａ３３−　　ａ１３・ａ　２４◆ａ　
３２）／　Ｖ Ｃ２１＝　　ａ３３（ａ２４令ａ　４１−　　ａ２１◆
ａ４４）／　ＶＣ２２＝（ａｌｌ中ａ３３・ａ４４−　
ａ１３＋ａ３１・ａ４４−　ａ１４◆ａ　３３・ａ　４
１）／　Ｖ Ｃ２３＝　　ａ　１３令（ａ２１◆ａ４４−　　ａ２４
令ａ４１）／　ＶＣ２４＝（−ａ　１１・ａ２４◆ａ３
３＋　ａ　１３◆ａ２４◆ａ３１＋　ａ　１４◆ａ２１
◆ａ３３）／Ｖ Ｃ３１＝［ａ３１◆（ａ２４◆ａ４２−　ａ２２◆ａ４
４）＋　ａ３２◆（ａ２１争ａ　４４−　ａ　２４◆ａ
　４１）］／　ＶＣ３２＝［−ａ　　１１・　ａ３３◆
　ａ４４＋　　ａ１２令　ａ３１◆　ａ４４−　　ａ　
　１４（ａ３１・ａ４２−　　ａ３２争ａ４１）］／Ｖ
Ｃ３３＝［ａｌｌ・（ａｌｌ・ａ４４−　ａｌｌ・ａ４
２）＋　ａ　１２・（ａｌｌ・ａｌｌ　−ａｌｌ・ａ４
４）＋　ａ１４・（ａ２１＋ａ４２−　ａｌｌ・ａｌｌ
）］／Ｖ Ｃ３４＝［ａｌｌ・ａｌｌ・ａ３２−　ａ１２◆ａ２４
◆ａ３１＋　ａ１４◆（ａｌｌ・ａ３１−ａ２１令ａ３
２）コ／ＶＣ４１＝　ａ３３◆（ａ２１◆ａ４２−　ａ
２２◆ａ４１）／ＶＣ４２＝［−ａｌｌ・ａ３３・ａ４
２＋ａ１２・ａ３３・ａ４１＋ａ１３＊（ａ　３１−ａ
　４２−　ａ　３２・ａ　４１）コ／ＶＣ４３＝　　ａ
　１３◆（ａ２２令ａ４１−　　ａ２１◆ａ４２）／　
ＶＣ４４＝［ａｌｌ・ａｌｌ・ａ３３−　ａ　１２◆ａ
２１・ａ３３＋　ａ１３・（ａ　２１−ａ　３２−　　
ａ　２２・ａ　３１）コ／Ｖただし、Ｖ＝ａｌｌ令ａ３
３・（ａｌｌ・ａ４４−ａｌｌ・ａ４２）＋ａ１２・ａ
３３◆（ａｌｌ・ａｌｌ−ａｌｌ・ａ　４４）＋　ａ　
１３◆ａ３１◆（ａｌｌ・ａ４２−ａｌｌ・ａ　４４）
＋　ａ　１３・ａ３２・（ａｌｌ・ａ４４−ａｌｌ・ａ
ｌｌ）＋　ａ　１４・ａ３３・（ａｌｌ・ａ４２−ａｌ
ｌ・ａｌｌ） また、第１．第２アーム３，５のサーボモータ２５．２
７と出力部２３ｅ中の駆動回路とを含む駆動部は、第６
図のブロック線図にて表される。

図においてＥｉは各サーボモータ２５．２７にかかる電
圧、Ｔ１〜Ｔ３は各アーム３．　５．　７のばねによる
反力トルク、ＩＭｉは各サーボモータ２５゜２７に流れ
る電流、θ旧は各サーボモータ２５゜２７の回転角度、
−旧は各サーボモータ２５．２７の回転角速度、θ「１
は電子制御回路２３からの指示角度（目標角度）（直、
θＡ１〜θＡ３はアーム３゜５．７の実際の角度、ＲＭ
ｉはサーボモータ２５゜２７の電機子抵抗、Ｇ１．Ｇ２
はギヤ比、ＪＭｉはサーボモータ２５．２７のイナーシ
ャ、ＤＭｉはサーボモータ２５．２７の粘性抵抗、ＫＳ
Ｉ〜ＫＳ３は各アーム３．　５．　７のばね定数、Ｇ２
ｉ、　　Ｋｌｉ、　　Ｋ２ｉ、　　ＫＥｉ、　　ＫＡｉ
、　　ＫＶｉ、　　Ｋｌｉ、　　ＫＳ１〜ＫＳ３．　　
ＫＴｉ。

ＫＰＷＭｉは所定の係数を表している。ただし、ｌ＝１
．２である。このブロック線図より次式を得る。

Ｅｉ＝　　（［Ｇ２ｉ◆Ｋｌｉ舎　（θ「１−ＤＭｉ）
　　−ＫＶｉ◆ωＭｉ］争に２ｉ−Ｋｌｉ◆Ｉ　Ｍｉ）
　　◆ＫＡｉ◆ＫＰＩＪＭｉ・・・（２，１） Ｅｉ−ＫＥｉ◆ωＭｉ＝ＲＭｉ◆ＩＭｉ　　　　　　　
・・・（２，２）＝ωＭ１　　　　　・・・（２，３） 上記（２，１）、（２，２）式より、下式が導かれる。

（ＲＭｉ＋Ｋｌｉ−ＫＡｉ−ＫＰＷＭｉ）・ＩＭｉ＋　
（Ｉ（Ｅｉ＋Ｉ（Ｖｌ・Ｋ２ｉφＫＡｉφＫＰＷＭｉ）
◆ωＭ１＝Ｇ２ｉ＋Ｋｌｉ＊に２ｉ−ＫＡｉ・ＫＰＷＭ
ｉ・（θｒｉ−θＭｉ）・・・（２，５） 上記（２，３）、（２，４）式より、下式が導かれる。

ＫＴｉ争ＩＭｉ＝ 十ＪＭｉ−山Ｍｉ十ＤＭｉ・ωＭｉ 上記（２，５）、（２，６）式より、下式が導かれる。

小山＝ ＪＭｉ　　　　　　Ｇ１２１（Ａｉ・Ｉ（Ｉｉ・ＫＰＷ
Ｍｉ＋１和倉ＪＭｉ　　　　　　　　　　　　ＫＡｉ・
Ｉ（ｌｉＩＩＫＰ闘ｉ　＋　ＲＭｉＩ　　　　　　　Ｋ
Ｓｉ 十−命　　−一　−θＡｉ ＪＭｉ　　　　　　Ｇｉ ＫＴｉ　　　　　Ｋｌｉψに２Ｉ＃Ｇ２ＩＩＩＩ（Ａ１
・Ｋ　ＰＷＭ　ｉ＋□争□□・θｒ１ ＪＭｉ　　　　　Ｊ（ＡｉφＫｌｉ−ＫＰＷＭｉ＋ＲＭ
ｉ・・・（２，７） 上記（２，７）式は次のように置き換える。

ゐＭ１＝ ｄ２１◆θＭ１＋ｄ２２◆ωＭ１＋ｄ２５◆θＡ１＋ｄ
　ｒｌ争θ「１ゐ贋＝ ｄ　４３◆θＭ２＋　ｄ　４４・ｃ、＋Ｍ２＋　ｄ　４
７◆θＡ２＋　ｄ’ｒ訃θ「２・・・（３，１） 各アーム３．　５．　７のトルクは第６図のプロ・ツク
線図より、 τ１＝ＫＳ１◆（θＭｌ／Ｇｌ−θＡｔ）τ２＝ＫＳ２
・（０Ｍ２／　Ｇ　２−θＡ２）τ３１＝−ＫＳ３◆θ
Ａ３１ τ３２＝−ＫＳ３争θ　Ａ３２ ・・・（３，２） と衷され、　（１，１５）、　（３，１）、　（３，２
）式より非線形の運動方程式は次のように表される。

・・・（３，３） ここで、 ｄ６１＝Ｋｓ１／Ｇｌ舎Ｃ１ｌ　　　ｄ６３＝Ｋｓ２／
Ｇ２◆Ｃｌ２ｄ６５＝−ＫＳＩ◆Ｃ１ｌ　　　　　ｄ６
６＝−ＤＡＩ◆Ｃ１１ｄ６７＝−ＫＳ２◆Ｃ１２ｄ６Ｂ
＝−ＤＡ２◆Ｃｌ２ｄ６９＝−ＫＳ３１・Ｃ１３ｄ６Ａ
＝−ＤＡ３◆Ｃ１３ｄ６Ｂ＝−ＫＳ３２◆Ｃ１４ｄ６Ｃ
＝−ＤＡ３◆Ｃ１４ｄ８１＝Ｋｓｌ／Ｇｌ◆Ｃ２１ｄ８
３＝ＫＳ２／Ｇ２◆Ｃ２２ｄ８５＝−ＫＳＩφＣ２１ｄ
８６＝−ＤＡＩ令Ｃ２１ｄＢ？＝−ＫＳ２◆Ｃ２２ｄ８
Ｂ＝−ＤＡ２◆Ｃ２２ｄ　８９＝−ＫＳ３１◆Ｃ２３ｄ
８Ａ＝−ＤＡ３・Ｃ２３ｄ８Ｂ＝−ＫＳ３２◆Ｃ２４ｄ
８ｃ＝−ＤＡ３・Ｃ２４ｄＡ１＝Ｋｓ１／Ｇｌ◆Ｃ３１
ｄＡ３＝Ｋｓ２／Ｇ２◆Ｃ３２ｄＡ５＝−ＫＳＩ◆Ｃ３
１ｄＡ６＝−ＤＡＩ・Ｃ３１ｄＡ７＝−ＫＳ２φＣ３２
ｄＡ８＝−ＤＡ２◆Ｃ３２ｄＡ９＝−ＫＳ３１◆Ｃ３３
ｄＡＡ＝−ＤＡ３◆Ｃ３３ｄＡＢ＝−ＫＳ３２φＣ３４
ｄＡＣ＝−ＤＡ３令Ｃ３４ｄｃ１＝Ｋｓ１／Ｇｌ◆Ｃ４
１ｄＣ３＝ＫＳ２／Ｇ２◆Ｃ４２ｄＣ５＝−ＫＳＩ◆Ｃ
４１ｄＣ６＝−ＤＡＩ◆Ｃ４１ｄｃ７＝−ＫＳ２◆Ｃ４
２ｄＣ８＝−ＤＡ２φＣ４２ｄＣ９＝−ＫＳ３１◆Ｃ４
３ｄＣＡ＝−ＤＡ３◆Ｃ４３ｄ　　Ｃ１］＝　　−Ｉ（
５３２中Ｃ４４ｄＣＣ＝−ＤＡ３◆Ｃ４４である。

ここで、 上記（３，３）式は、 ｉ＝Ａχ＋Ｂｕ ・・・（３，４） と近似できる。

上記行列Ａの要素ｄ２１〜ｄＣＣは、前述のごとくＫＳ
Ｉ〜Ｋ　Ｓ３２、Ｇ１、Ｇ２、Ｃ１ｌ〜Ｃ４４、ＤＡＩ
〜ＤＡ３から構成され、このＫＳＩ〜Ｋ　Ｓ３２はばね
定数であり、Ｇ１は第１アーム３とそのサーボモータ２
５との間のギヤ比であり、Ｇ２は第２アーム５とそのサ
ーボモータ２７との間のギヤ比であり、Ｃ１ｌ〜Ｃ４４
はａｌｌ〜ａ４４から構成され（１，１０）〜（１，１
３）式と（１，１４）式との関係から判るようにθＡ１
〜θＡ３２をパラメータとする関数であり、更にＤＡＩ
〜ＤＡ３はアーム３．　５．　７の粘性抵抗である。

ここで振動角度θＡ３１．θＡ３２は小さいので０と見
なすと、下式のごとくＡはＤＡＩ、　　θＡ２のみの関
数と考えることができる。

Ａ＝ｆ（ＤＡＩ、　　θＡ２） 従って、第１アーム３の角度θＡ１、第２アーム５の角
度θＡ２ｍに、行列Ａが一定の値と考えることができ、
上記運動方程式（ｉ＝Ａχ＋Ｂｕ）は角度毎に線形と考
えることができ、角度毎に現代制御理論が適用できる。

上記θＡｌ、　　θＡ２の値は、アーム３．　５．　７
が静止状態であると仮定して、エンコーダ１７．１９の
出力値にギヤ比を掛けれは求められる。

一般的な現代制御理論では、第７図で表されるような制
御ｊ対象を線形の式で、 ｉ＝Ａχ＋Ｂｕ ｙ＝Ｃχ と表す。ここで、フィードバックゲインＦとオブザーバ
ゲインにとを、極配置法にて、目的とする制御結果が得
られるように決め、ホールド付離散化により、下式の係
数Ｐ、　　Ｑ、　　Ｒを求めると、第８図に示すような
オブザーバ５０が設計できる。

ｐ　＝　ｅＴＡ−ＫＣ）丁 Ｑ　：　１Ｔ　ｅ（Ｉｌｌ−ＫＣＩＴでＢｄ　τＲ＝１
１ｅ（Ａ−にＣＩＴｒＫｄτ ここで、Ｔはサンプリングタイム（例えば１　ｍ５ｅｃ
）であり、τは積分用の変数である。上記（３，４）式
は同型の式であることから、現代制御理論が適用でき同
様なオブザーバが設計できる。

ただし、 第８図のオブザーバの処理を計算式で表すと、次式のご
とくになる。

Ｚ（ｎ＋１）＝Ｐ◆Ｚ（ｎ）＋Ｑ◆ｕ（ｎ）＋Ｒ◆ｙ（
ｎ）ｕ　（ｎ）＝　　ｕ　（ｎ）＋　　Ｆ　　−Ｚ　（
ｎ）本オブザーバ５０は、一連のアーム３．　５．　７
の状態変数θＡｔ、　　θＡ２．　　θＡ３１．θＡ３
２が推定でき、この値からアーム３．　５．　７の振動
を考慮して第３アーム７の先端を所望の位置へ移動制御
することができる。

このオブザーバ５０の処理を前記電子制御回路２３にて
実現したフローチャートを第９図に示す。

まず、Ｚ　（ｎ）の初期値が与えられる（ステップ１０
０）。次に前回の指令値ｕ　（ｎ）と、ｙ　（ｎ）即ち
エンコーダ１７，１９．２１の出力値とが取り込まれる
（ステップ１１０）。次に行列Ａの算出がなされる（ス
テップ１１２）。即ちエンコーダ１７．１９の出力値を
アーム３．５の静止状態でのθＡｌ、　　θＡ２として
用いて、その他前述の各種定数等を使用してＡを算出す
る。次に上記゛した式からＰ、　　Ｑ、　　Ｒを算出す
る（ステップ１１４）。

次に前回算出されたＺ　（ｎ）と上記ｕ（ｎ）、ｙ（ｎ
）とを用いて、上記式のごとく次回に用いるＺ　（ｎ＋
１）を前記式に従って求める（ステップ１２０）。

次に前回求めたＺ　（ｎ）から上記式に従って、新たな
指令値ｕ　（ｎ）を求める（ステップ１３０）。次にこ
の新たな指令値ｕ　（ｎ）をサーボモータ２５゜２７．
２９側に出力する（ステップ１４０）。

次に処理の終了条件が満足されたか否かが判定される（
ステップ１５０）。終了条件とは、指令が終わった状態
であり、例えば、溶接等の一連の作業が終了した状態等
をいう。

継続であれは、ステップ１５０にては否定判定されて、
再度ステップ１１０〜ステツプ１５０の処理を繰り返し
、第３アーム７の先端を所定の位置に正確に移動する。

この繰り返しは上記サンプリングタイムＴ（例えば１　
＋ｎ５ｅｃ）の周期で行われ、測定されるｙ　（ｎ）の
内のエンコーダ１７．１９の出力を、上記のごとくアー
ム３，５の静止状態でのθＡｔ、　　θＡ２として、前
記式Ａ＝ｆ（θＡ１．　　θＡ２）に基づきＡが求めら
れることから、現代制御理論に従った制御が実現される
こととなる。

ステップ１５０にて肯定判定されれば、他の処理に移る
か、あるいはこのまま処理を終了する。

上述したごとく、本実施例°の多関節アーム制御装置は
、予め多関節アームの数式モデルを作成し、そのモデル
にしたがって、角度のパラメータが一定ならば線形とな
る運動方程式を求め、オブザーバとしての各種演算式を
設計している。そのため、実際の制御に際して、第１、
第２アーム３，５の角度毎に現代制御理論が適用でき、
アーム３，５゜７がばね系として振動するのを防止でき
るとともに、正確に第３アーム７の先端を所定の位置に
制御でき、ロボット等に精度の高い作業をさせることが
できる。

上記実施例において、サーボモータ２５．　２７゜２９
がアクチュエータＭ２に該当し、エンコーダ１７．１９
．２１が位置検出手段Ｍ３に該当し、電子制御回路２３
が制御手段Ｍ４に該当する。

光肌少苅盟 本発明の多関節アーム制御装置は、非線形な数式モデル
Ｍ５をアームＭ１の角度毎に線形として取り扱うことに
より、現代制御理論を適用でき、アームＭ１の振動を考
慮してアクチュエータ駆動指令値を補償することができ
る。

従って、アームＭ１がばね系として振動しようとしても
、その振動を防止すると共に、正確にアームＭ１の先端
を所定の位置に制御でき、ロボット等に精度の高い作業
をさせることができる。

【図面の簡単な説明】

第１図は本発明の基本的構成を例示するブロック図、第
２図は本発明の一実施例のアーム部分を示す概略斜視図
、第３図はその電子制御装置のブロック図、第４図及び
第５図は各アームの動作説明図、第６図はサーボモータ
駆動部のブロック線図、第７図は一般化した制御対象の
ブロック線図、第８図は本実施例のブロック線図、第９
図は電子制御回路にて行われる処理のフローチャートで
ある。 Ｍｌ・・・アーム　　　　Ｍ２・・・アクチュエータＭ
３・・・位置検出手段　　Ｍ４・・・制御手段Ｍ５・・
・数式モデル ３、　５．　７・・・アーム １７．１９．２１・・・エンコーダ ２３・・・電子制御回路 ２５．２７．２９・・・サーボモータ

Claims (1)

1. 【特許請求の範囲】 １　一連に配設された複数のアームと、 この各アームを動作させるアクチュエータと、アクチュ
   エータの位置を検出する位置検出手段と、 上記アクチュエータを制御しアームに所定の動作をさせ
   る制御手段と、 を備えた多関節アーム制御装置において、 上記制御手段が、上記アクチュエータ及びアームによっ
   て構成される系をばねと質量とからなる系と仮定して設
   定された数式モデルであって、アクチュエータ駆動指令
   値から少なくともアクチュエータ変位またはアーム変位
   を推定する非線形の数式モデルを用いるとともに、上記
   アクチュエータの変位毎に上記数式モデルを線形として
   取り扱うことにより、アクチュエータ駆動指令値及びア
   クチュエータ変位の実際値に応じてフィードバック補償
   量を求め、このフィードバック補償量にて上記アクチュ
   エータ駆動指令値を補償することを特徴とする多関節ア
   ーム制御装置。