JPH03219307A - 産業機械の振動制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、産業機械の振動制御装置に関し、とりわけハ
ネ要素を有したトルク伝達機構を介してロボットアーム
を駆動するアクチュエータを具えたロボットにおいてロ
ボットアームの振動の低減するに好適な装置に関する。

〔従来の技術および発明か解決しようとする課題〕

ロボット、工作機械等の産業機械では一般的に第４図に
概念的に示すような制御系によって機械系の制御か行わ
れる。同図に示すようにこの制御系は、アーム等の制御
対象（機械系１００）を制御する場合に、該機械系１０
０の状態量（たとえばアーム先端の速度、加速度）を検
出、フィードバックするのではなく、この機械系１００
を駆動するサーボモータ等のアクチュエータ（駆動系２
００）の状態量（たとえばモータ回転速度、回転加速度
）を検出部３００で検出、フィードバックして、このフ
ィードバック量と目標値との偏差を制御要素４００に加
えて、アーム先端の速度、加速度を制御するようにした
セミクローズド方式と呼称される制御系である。

このセミクローズド方式の制御系は、駆動系２００の状
態量を検出しているので、検出方法が簡便であるという
利点があるものの、機械系１００の状態量を検出してこ
れをフィードバック量としていないので、機械系１００
がバネ要素を有するものであってこれに起因して振動し
てしまう場合にはこの振動を抑えた制御をなし得ないこ
ととなる。すなわちたとえばこの振動によりアーム先端
位置の位置決めに時間がかかり、作業効率が大幅に低下
するなどの不都合が招来することとなる。

しかしながら、アーム先端の速度、加速度等機械系１０
０の状態量をセンサ等によって検出することはセンサの
配設場所の選定等の困難を極め、採用、実施には至らな
いこととなっていた。

このため−船釣に使用されている上記セミクローズド方
式を基本にして機械系１００がバネ要素を有している場
合であっても振動を抑えた制御を行うことができる装置
の開発が望まれる。

そこで本出願人は、機械系１００を含む制御系全体の周
波数特性を予め計測しておき、機械系１００が望ましい
動作をするのに必要な制御入力、つまり第４図に示すよ
うに制御要素４００がら駆動系２００に加える駆動指令
値を上記計測した周波数応答特性データを用いて逆フー
リエ変換により求め、この求めた制御入力を駆動系２０
０に加え、機械系１００の振動を低減させる技術を特許
出願している（特願昭６２−８７４９３号）。

しかし、この技術では制御動作開始前に制御入力を予め
計算しておく必要があり、そのための前処理時間が膨大
なものとなり、作業効率を損なうという欠点を有してい
る。また、計算した制御入力を記憶しておく必要がある
が、機械系１００の動作パターンすべてに関する制御入
力を記憶する場合には、記憶容量の大きいメモリを用意
しなければならず、コスト上昇を招来するという欠点を
有している。

また、セミクローズド方式のままで機械系１００の振動
を低減させる技術として、アクチュエータ人力に対する
機械系１００の動きを記述した微分方程式をリアルタイ
ムで解くごとによって機械系１００の状態量（たとえば
バネ要素の歪み量、歪み量の時間変化率）を推定するオ
ブザーバ（状態観測器）を設け、このオブザーバによっ
て推定された機械系１００の状態量をフィードバックす
る技術かあり（「オブザーバを用いたロボット・アーム
の防振制御の開発」　；川崎重工技報・９３号、１９８
６年８月発行）、すてに特許出願されている（特開昭６
３−４６５２１号公報）。

しかし、上記微分方程式は複雑であり、オブザバをソフ
トウェアで構成した場合には演算処理に大幅な時間が費
やされるごとになってしまい、実用化が困難であるとい
う面がある。

本発明は、こうした実情に鑑みてなされたものであり、
前処理時間およびメモリの記憶容量の膨大化を回避する
ことができるとともに、リアルタイムで行う演算処理の
時間短縮を図ることができる産業機械の振動制御装置を
提供することをその目的としている。

〔課題を解決するための手段〕

そこでこの発明では、バネ要素を有したトルク伝達機構
を介して産業機械の可動部材を駆動するアクチュエータ
を具え、前記可動部材の先端が所望の軌跡を描くように
前記アクチュエータに駆動指令値を所定時間ごとに与え
るようにした産業機械の制御装置において、前記所定時
間ごとの駆動指令値と前回演算された前記伝達機構およ
び前記可動部材の予測状態量とに基づき前記所定時間後
の前記予測状態量を順次演算する予測状態量演算手段と
、前記所定時間ごとの駆動指令値と前記予測状態量演算
手段で演算された予測状態量に基づき補正駆動指令値を
前記所定時間ごとに演算する補正駆動指令値演算手段と
を具え、前記補正駆動指令値演算手段で演算された補正
駆動指令値を前記所定時間ごとに前記アクチュエータに
与えるようにしている。

〔作用〕

かかる構成によれば、所定時間ごとにアクチュエータに
与える駆動指令値と前回演算された伝達機構および可動
部材の予測状態量に基づき所定時間後の予測状態量が順
次演算される。これは具体的には入力（アクチュエータ
に与える駆動指令値）に対して機械系（伝達機構および
可動部材）の応答（伝達機構および可動部材の振動）を
実機計測または数学モデルにより離散時間系での状態方
程式の形で求めておき、この状態方程式に所定時間（離
散時間）ごとにアクチュエータに与える駆動指令値と前
回演算された伝達機構および可動部材の予測状態量を代
入するごとによりつぎの予測状態量を順次演算する。し
たがって、大きい記憶容量のメモリを必要することなく
計算をリアルタイムで素早く行゛うことができる。しか
して所定時間ごとの駆動指令値と演算された予測状態量
に基づき補正駆動指令値を所定時間ごとに演算し、この
演算された補正駆動指令値を所定時間ごとにアクチュエ
ータに与えるようにすれば、機械系（伝達機構および可
動部材）の状態量が加味されたアクチュエータの制御が
行われ、機械系（伝達機構および可動部材）の振動が低
減されることとなる。

〔実施例〕

以下、図面を参照して本発明の実施例について説明する
。なお、実施例では適用産業機械として産業用ロボット
を想定しており、この産業用ロボットの所定のアームの
回動軸の回転位置をツール先端位置が所望の軌跡を描く
ように制御する制御装置について説明する。

第１図に実施例の制御ブロック図を概念的に示す。同図
に示すようにこの制御系は先に第４図に示した制御系の
機械系１００１駆動系２００、検出部３００および制御
要素４００と同様な機械系１０、駆動系２０、検出部３
０および制御要素４０とから成るセミクロースト方式の
制御系であり、さらに離散時間系状態方程式演算部５０
（以下、演算部５０という）が付加されている。

駆動系２０は第２図に示すように入力駆動指令信号Ｖ　
（ｋ）を増幅する速度制御アンプ２１とこの速度制御ア
ンプ２１の増幅信号を入力して駆動されるサーボモータ
２２とから構成されている。

機械系２０は、サーボモータ２２の回転速度を減速する
減速機と、この減速機の最終段の軸トルクをアームに伝
達するトルク伝達機構と、このトルク伝達機構によって
伝達されたトルクに応じて回動軸が回転するアームとか
ら構成されており、これら減速機、°トルク伝達機構お
よびアームはバネ要素１１と、バネ要素１１を介して矢
印に示すごとく加速度Ｇを以て回転するアーム１２とに
単純モデル化することができる。検出部３０は、サーボ
モータ２２の回転速度を検出する速度検出器（たとえば
タコジェネレータが使用される）３１と同モータ２２の
回転角度（位置）を検出する位置検出器（たとえばエン
コーダか使用される）３２とから構成されている。速度
検出器３１の出力は速度フィードバック信号として速度
制御アンプ２１に加えられる。そして、制御要素４０は
、アム１２の目標回転角度に応じたサーボモータ２２の
目標回転位置（目標値）θｒ　（ｋ）と位置検出器３２
の出力θ（ｋ）との偏差ｅ　（ｋ）に所定の比例ゲイン
（位置フィードバックゲイン）Ｋｐを乗算する乗算部４
１と、演算部５０の出力に所定の比例ゲイン（加速度フ
ィードバックゲイン）Ｋｇを乗算する乗算部４２と、乗
算部４１の出力から乗算部４２の出力を減算する減算部
４３とから構成されており、この減算部４３の減算値Ｖ
（ｋ）か駆動指令信号として速度制御アンプ２１を介し
てサーボモータ２２に加えられるごとになる。なお、ハ
ードウェア上、第１図の破線で囲まれた部分はマイクロ
コンピュータ６０であり、乗算部４１．４２、減算部４
３および演算部５０等の処理はマイクロコンピュータ６
０て実行される。

以下、このマイクロコンピュータ６０て行う処理につい
て説明する。

この処理に先立って速度制御アンプ２１人力駆動指令信
号Ｖ　（ｋ）に対するアーム１２の加速度Ｇの周波数応
答特性か実機計測によって求められる。こうして計測が
行われると、下式（１）に示すような伝達関数Ｋ　（ｓ
）か求められる。

Ｋ　　（ｓ）＝Ｇ　　（ｓ）／Ｖ　　（ｓ）”　　（ｂ
　ＬＩＳ　”　　＋　ｂ　、、−Ｉ　　５−−２　　＋
・・・＋ｂ２　Ｓ＋ｔ）＋　　）／　　（Ｓ’　　＋ａ
、Ｓ’−’　　＋　ａｎ−Ｉ　　Ｓ”−２＋　＝・＋　
ａ　２　　Ｓ　＋　ａ　１）　　・・・（１）なお、Ｆ
ＦＴアナライザを用いると、伝達関数Ｋ　（ｓ）の極と
零点が求まり、上記（１）式の右辺のような多項式の形
が容易に得られるごとになる。

一般的に伝達関数から可観測または可制御正準形式によ
る最小実現の手法を用いて連続時間系における状態方程
式が得られる。いま、上記伝達関数Ｋ　（ｓ）について
可制御正準形式による最小実現の手法を用いた場合には
状態方程式は、下記（２）式のごとくなる。

（ｄ／ｄ　ｔ）Ｘ　（ｔ）−ＡＸ　（ｔ）、＋ＢＶ　（
ｔ）・・・（２） ここに、 である。Ｘ　（ｔ）はアーム１２の状態量を示す状態量
ベクトルＸ　（ｔ）−［ＸＩ　　（ｔ）　　Ｘ２　　（
ｔ）・　Ｘ、（ｔ））　”であり、このうちＸ、（ｔ）
がアーム１２の加速度Ｇに相当する。

上記（２）式をマイクロコンピュータ６ｏの処理用に離
散化すると、下記（３）式のような離散時間系での状態
方程式が得られる。

Ｘ　（ｋ＋１）−ＰＸ　（ｋ）＋ＱＶ　（ｋ）・・・（
３） ここに、 Ｐ−ｅＡＴ−１＋ＡＴ＋　　（１／２　　りＡ２　Ｔ２
　＋・・・であり、 Ｑ　−ｆ　Ｔ　ｅ　Ａ　ｒ　Ｂｄζ−Ｔ　（１＋（１／
２１）ＡＴ＋　　（１／３　りＡ２　Ｔ２　＋・・暑と
なる。Ｔはサンプリング周期であり、Ｘ　（ｋ）、Ｘ（
ｋ＋１）はに、に＋１１００サンプリングにおける状態
量ベクトル、Ｖ　（ｋ）はに回目のサンプリングにおけ
る速度制御アンプ２１への人力駆動指令値をそれぞれ示
す。上記（３）式は差分方程式表現となっており、この
式（３）を用いると行列演算のみで、簡単にアーム１２
の状態量を推定することができる。推定された状態量ベ
クトルへ をＸ　（ｋ）と表すと、（３）式は下記（４）式に書き
直される。

へ Ｘ　　（ｋ＋１）−ＰＸ　　（ｋ）＋ＱＶ　　（ｋ）−
（４）ただしここに Ｘ（ｋ）−（ＸＩ　　（ｋ）　　Ｘ２　　（ｋ）・・・
Ｘ７（ｋ）、）” であり、このうちＸ。（ｋ）がアーム１２の推定加速度
である。上記（４）式の演算が演算部５０で行われるも
のとする。

いま、ｋ回目のサンプリングにおいて、目標値θｒ　（
ｋ）か与えられたものとする。そこで、この目標値θｒ
　（ｋ）と位置検出器３２の出力θ（ｋ）との偏差ｅ　
（ｋ）が求められ、この偏差ｅ（ｋ）が乗算部４１に入
力される。乗算部４１では入力偏差ｅ　（ｋ）に位置フ
ィードバックゲインＫｐを乗算する処理を施し、乗算結
果Ｋｐ　　（θｒ（ｋ）−θ（ｋ））を減算部４３に出
力する。

方、演算部５０では前回（ｋ−１回目）のサンプリング
において減算部４３から出力された駆動指令値Ｖ　（ｋ
−１）と前回のサンプリングにおいてへ 推定されたアーム１２の推定加速度Ｘ。（ｋ−１）とに
基づき上記（４）式から今回（ｋ回目）のサンプリング
におけるアーム１２の推定加速度Ｘ、。

（ｋ）が演算され、乗算部４２に出力される。乗へ 鼻部４２では入力推定値Ｘ。（ｋ）に加速度フィードバ
ックゲインＫｇを乗算する処理を施し、乗算結果Ｋｇ−
Ｘ、（ｋ）を減算部４３に出力する。

しかして減算部４３では、アーム１２の加速度Ｇが低減
されるように乗算部４１の出力Ｋｐ　（θｒ　（ｋ）−
θ（ｋ））から乗算部４２の出力Ｋｇ・Ｘ、（ｋ）を減
算する処理を施し、ｋ回目のサンプリングにおける駆動
指令値 Ｖ　（ｋ）　＝Ｋｐ　（θｒ　（ｋ）−θ（ｋ））Ｋｇ
−Ｘｎ　　（ｋ）　　・・・　（５）が速度制御アンプ
２１に出力される。この駆動指令値Ｖ　（ｋ）に応じた
増幅信号がサーボモータ２２に加えられ、該モータ２２
はこの増幅信号（こ応じた駆動力で駆動される。この結
果、）く単要素１１、アーム１２は振動が低減された状
態で作動し、アーム１２は所望の回転角度に位置決めさ
れる。

以後、各サンプリング周期ごとに演算部５０では、推定
値Ｘ。（ｋ＋１）・・・か順次演算され、減算部４３か
ら上記（５）式に対応する駆動指令値Ｖ（ｋ＋１）・・
・が速度制御アンプ２１に出力される。

したがって、アーム１２は振動を伴うことなく、所望の
軌跡を描くごとになる。

さて、ロボット制御の最新技術として、アクチュエータ
であるモータに関して運動方程式をたて、この運動方程
式からモータに要求されるトルりを求め、この求めたト
ルクに応じた信号をモータに加え、制御する技術か知ら
れている。この技術（こよればアーム間に力学的干渉が
あったとしても、この要素を運動方程式の中にいれて考
慮することができるので、高速でも軌跡を高精度に保持
することができるという利点がある。

この技術に基づく本発明の実施例について第３図を参照
して説明する。

前述した実施例と同様に駆動系２０は入力駆動指令信号
τ（ｋ）を増幅する電流制御アンプ２３と、この電流制
御アンプ２３の増幅信号を入力して駆動されるサーボモ
ータ２４とから構成されている。機械系２０は、バネ要
素１３と、アーム１４とから構成されている。検出部３
０は、サーボモータ２４の回転位置θヮおよび回転速度
θ８を検出する位置・速度検出器３３から構成されてい
る。そして、制御要素４０は、アーム１４の目標回転角
度θ、に応じたサーボモータ２４の目標回転位置、目標
回転速度および目標回転加速度θｄ。

θｄおよびθｄと位置・速度検出器３３の出力θ、θと
に基づき現在のサーボモータ２４の加速度θをサーボ演
算するサーボ演算部４４と、サーボ演算部４４の出力θ
と位置・速度検出器３３の出力θ、θとに基づきサーボ
モータ２４に関する運動方程式からサーボモータ２４の
要求トルクτを演算する運動方程式演算部４５と、離散
時間系状態方程式演算部５２（以下、状態方程式演算部
５２という）で演算されたバネ１３の歪みθＳに所定の
状態フィードバックゲインＫｓｐを乗算する乗算部４６
と、状態方程式演算部５２で演算されたバネ１３の憤み
速度θＳに所定の状態フィードバックゲインＫｓｖを乗
算する乗算部４７と１乗算部４６．４７の各出力を加算
する加算部４８と、運動方程式演算部４５、加算部４８
の各出力を加算する加算部４９とから構成されている。

そしてこの加算部４９の加算値τ（ｋ）は駆動指令信号
として電流制御アンプ２３を介してサーボモータ２４に
加えられるごとになる。なお、ハードウェア上、サーボ
演算部４４、運動方程式演算部４５、乗算部４６．４７
、加算部４８．４９および状態方程式演算部５２の処理
はマイクロコンピュータ６０で行われるものとする。

この処理に先立って前述した実施例と同様に前記（２）
式に対応する連続時間系での状態方程式が下記（６）式
のごとく求められる。

（ｄ／ｄｔ）θｔ、（ｔ）−ＡｅＬ　（ｔ）＋Ｂｒ（１
）・・・（６） そして、この（６）式を離散化するごとにより、前記（
４）式に対応する離散時間系状態方程式が下記（７）式
のごとく得られる。

・・・　（７） ただし、θＬはアーム１４の状態量、つまり回転位置θ
し、回転速度θＬを示すベクトル［θ。

θＬ］”であるものとする。

さて、こうして得られるアーム１４の状態量推定ベクト
ルθＬ　　（ｋ＋１）と、サーボモータ２４の状態量、
つまり回転位置θ３、回転速度θ３を示すベクトルｅＭ
　（ｋ＋１）（−［０Ｍ　ｅ７Ｉ］Ｔ）とから、バネ要
素１３の歪みθｓ１歪み速度θＳを示す状態量ベクトル
ｅｓ（ｋ＋１）（−［θ８　θ５］Ｔ）か下記（８）式
のごとく得られる。

θｓ　　（ｋ＋１）＝θ＋、　　（ｋ＋１）−ｅＭ　（
ｋ＋１）・・・　（８） これら（７）、（８）式の演算が状態方程式演算部５２
で行われるものとする。

いま、ｋ回目のサンプリングにおいて、サーボモータ２
４の目標値としてθｄ、θｄおよびθｄが与えられたも
のとする。そこで、サーボ演算部４４ではこれら目標値
θｄ、ｉｄおよびθｄと位置・速度検出器３３の出力θ
、θとがら下記（９）式の演算を行い、アーム１４の現
在の加速度θを求める。

θ−θｄ＋Ｋｄ　（θｄ−δ）＋Ｋｐ（θｄ−θ）・・
・　（９） サーボ演算部４４の演算結果θは位置・速度検出器３３
の出力θ、ｄとともに運動方程式演算部４５に加えられ
、下記（１０）式に示すごとくサボモータ２４の要求す
るトルクτが演算される。

ここにＪはイナーシャであり、Ｄは粘性抵抗係数であり
、Ｔはクーロン摩擦である。そして、５ｉｎ（θ）は以
下のように定義する。

サーボモータ２４の演算結果τは加算部４９に出力され
る。

一方、状態方程式演算部５２では前回（ｋ−１回目）の
サンプリングにおいて加算部４９がら出力された駆動指
令値τ（ｋ−１）と前回のサンプリングにおいて推定さ
れたアーム１４の状態量θｔ（ｋｌ）とに基づき上記（
７）式がら今回（ｋ回目）のサンプリングにおけるアー
ム１４の状態量θＬ　　（ｋ）が推定される。さらに、
この状態量θＬ　　（ｋ）と、サーボモータ２４の現在
の状態量θ、Ａ　（ｋ）（これは位置・速度検出器３３
から入力される）とに基づき上記（８）式からバネ要素
１３の状態量ｅｓ　（ｋ）が得られる。こうして得られ
たバネ要素１３の状態量θｓ１θＳはそれぞれ乗算部４
６．４７に出力され、各乗算部４６．４７では状態量θ
ｓ１θＳに状態フィードバックゲインＫｓｐｓ　Ｋｓ　
ｖを乗算する処理を施す。

各乗算部４６．４７の乗算結果は加算部４８で加算処理
Ｋｓｐ・θｓ＋Ｋｓｖ・θＳがなされ、加算部４９に出
力される。

しかして加算部４９ではアーム１２の加速度が低減され
るように運動方程式演算部４５の出力τと加算部４８の
出力Ｋｓｐ・θｓ＋Ｋｓｖ・θＳとを加算する処理を施
し、ｋ回目のサンプリングにおける駆動指令値 τ　（ｋ）−Ｊｂ＋Ｄθ＋Ｔｓｉｎ（θ）＋Ｋｓｐ＊θ
ｓ＋Ｋｓｖ＊θｓ−・（１１）を電流制御アンプ２３に
出力する。この駆動指令値τ（ｋ）に応じた増幅信号が
サーボモータ２４に加えられ、該モータ２４はこの増幅
信号に応じた駆動力で駆動される。この結果、バネ要素
１３、アーム１４は振動が低減された状態で作動し、ア
ーム１４は所望の回転角度に位置決めされる。

以後、各サンプリング周期ごとに状態方程式演算部５０
では、バネ要素１３の状態量θｓ　（ｋ＋１）・・が順
次演算され、加算部４９がら上記（１１）式に対応する
駆動指令値τ（ｋ＋１）・・・が電流制御アンプ２３に
出力される。

したがって、アーム１４は振動を伴うことなく、所望の
軌跡を描くごとになる。

以上説明したように実施例によればバネ要素を有したア
ーム機構の状態量を直接検出することなく、サンプリン
グ周期ごとに状態方程式から状態量を推定して、推定し
た状態量を加味してサーボモータの制御を行うようにし
たので、アームの振動が低減されて、作業を精度よく行
うことができる。また、状態方程式から状態量を推定す
る演算は、簡単かつ大メモリ容量を要しないので、リア
ルタイムで行う演算処理の時間短縮とコスト低減を図る
ことができる。特に第２図に示す実施例の場合は従来の
制御ブロック構成に演算部５０を付加するだけでよいの
で、従来のものに大幅な改造を施すことなく、実施でき
るという利点がある。

なお、実施例の状態方程式で求めるべき機械系の状態量
は一例にすぎず、要は制御対象に応じた振動低減に有効
な状態量であれば任意である。

〔発明の効果〕

以上説明したように本発明によればバネ要素を有した機
械系の状態量を直接検出することなく、所定時間ごとに
状態量を予測演算して、予測した状態量を加味してアク
チュエータの制御を行うようにしたので、機械系の振動
が低減されて、作業を精度よく行うことかできる。また
、状態量の予測演算は簡単かつ大メモリ容量を要しない
ので、リアルタイムで行う演算処理の時間短縮とコスト
低減を図ることができる。

【図面の簡単な説明】

第１図は、本発明に係る産業機械の振動制御装置の実施
例の構成を概念的に示す制御ブロック図、第２図は、第
１図をより具体化して示す制御ブロック図、第３図は、
他の実施例を示す制御ブロック図、第４図は、セミクロ
ーズド方式の制御系を概念的に示す制御ブロック図であ
る。 １０・・・機械系、２０・・・駆動系、４０・・・制御
要素５０・・・離散時間系状態方程式演算部。 第１図 第２図

Claims (1)

1. 【特許請求の範囲】 バネ要素を有したトルク伝達機構を介して産業機械の可
   動部材を駆動するアクチュエータを具え、前記可動部材
   の先端が所望の軌跡を描くように前記アクチュエータに
   駆動指令値を所定時間ごとに与えるようにした産業機械
   の制御装置において、前記所定時間ごとの駆動指令値と
   前回演算された前記伝達機構および前記可動部材の予測
   状態量とに基づき前記所定時間後の前記予測状態量とを
   順次演算する予測状態量演算手段と、 前記所定時間ごとの駆動指令値と前記予測状態量演算手
   段で演算された予測状態量に基づき補正駆動指令値を前
   記所定時間ごとに演算する補正駆動指令値演算手段と を具え、前記補正駆動指令値演算手段で演算された補正
   駆動指令値を前記所定時間ごとに前記アクチュエータに
   与えるようにしたことを特徴とする産業機械の振動制御
   装置。