JP2012125844A - サーボアンプの制御ループゲイン調整方法、プログラム及びロボット制御装置 - Google Patents

Abstract

【課題】ロボット動作中のサーボアンプの電源電圧の変動に対して簡易な制御で制御ループを安定化させる。
【解決手段】ロボットの各関節に設けられたサーボモータに対して外部からの制御指令に応じた該サーボモータの制御量を帰還させて制御ループを形成するサーボアンプの制御ループゲイン調整方法であって、前記制御ループゲインのデフォルト値及び前記サーボアンプの電源電圧のデフォルト値に基づいて、前記制御ループを形成するとともに前記サーボモータの駆動を開始する工程と、前記サーボアンプの電源電圧を検出する工程と、前記制御ループゲインを構成する制御ゲインのうち前記サーボアンプの電源電圧と相関して変化する制御ループゲインを、検出した前記サーボアンプの電源電圧の変化に対して逆方向に変化するように調整する工程と、を備える。
【選択図】 図２

Description

本発明は、サーボアンプの制御ループゲイン調整方法、プログラム及びロボット制御装置に関する。

ロボットは、ロボットアームの先端部位にツール部材が取り付けられており、ロボットアームを構成する複数のアーム部材の各回転軸周りの回転を互いに独立して行い得るように構成されている。ロボットの動作を制御するロボット制御装置は、複数のアーム部材の各回転軸の位置サーボ制御によってツール部材を所望の位置に所望の姿勢で停止させたり、複数のアーム部材の各回転軸の速度サーボ制御によりツール部材を所望の経路に沿って所望の姿勢で、かつ所望の速度で移動させるように構成されている。さらに、ロボット制御装置は、各軸に設けられたサーボモータの各相に流れる電流量を各々制御する電流サーボ制御を行うように構成されている。これらのサーボ制御の具体例として、例えば以下に示す特許文献１〜３に記載された技術がある。

特許文献１には、アクティブフィルタにより３相交流を整流し、その整流波形の電力を平滑コンデンサにより直流に平滑化して出力するロボットの電源装置と、上記平滑コンデンサの両端子間電圧が一定となるように制御する該電源装置の制御回路とが開示されている。

特許文献２には、位置指令信号に基づいて速度指令信号を出力する位置制御部と、該位置制御部の速度指令信号に基づいて電流値の設定指令信号を出力する速度制御部と、該速度制御部の設定指令信号に基づいてサーボモータの電流を制御する電流制御部と、を備えるとともに、上記サーボモータの電流、回転速度及び回転位置をそれぞれ検出してこれらの検出値をフィードバックし、ロボットを駆動制御するロボットの制御装置において、上記位置制御部、速度制御部及び電流制御部のゲインを必要に応じて切換指令する切換指令信号を出力するゲイン切替部を備えることが開示されている。

特許文献３には、サーボアンプへ入力される電源電圧を測定する入力電圧測定方法と、この測定した入力電圧に基づいてサーボアンプの制御ゲインを設定する制御ゲイン設定方法とが開示されている。

特開平８−２５１９２２号公報 特開平１−２５５０１５号公報 特開平８−２０５５６９号公報

ところで、ロボットアームの加速や該ロボットアーム先端のツール部材によってワークを持ち上げる場合など、多くの電力消費を伴うサーボ制御が行われるときに、商用電源から供給される電力が各軸のサーボモータで消費される電力に間に合わず、ロボットの電源部からモータ駆動回路に供給される電源電圧（例えば、特許文献１の平滑コンデンサ間電圧）が低下するおそれがある。なお、このように電源電圧が低下すると、電流サーボ制御における応答が遅くなるという問題が生じる。

一方、ロボットアームの減速や該ロボットアーム先端のツール部材によってワークを降ろす場合など、サーボモータが発電機として作動する回生動作を伴うサーボ制御が行われるときに、回生抵抗によって回生電力を消費しきれず、上記の電源電圧が上昇するおそれがある。なお、このように電源電圧が上昇すると、電流サーボ制御の電流ループゲインが上昇するため、サーボモータの振動や異音が発生するという問題が生じる。

上記の問題は、電流ループゲインに限らず、サーボアンプの制御ループゲイン（位置速度ループゲイン、速度ループゲインなど）全般に該当するものである。

特許文献１では、ロボットの負荷変動を考慮して電源装置の平滑コンデンサ間電圧を一定にしようとするものであるが、この制御のために新たなハードウェア（制御回路）を追加する必要がある。

特許文献２では、位置制御部、速度制御部及び電流制御部のゲインを、ワーク間を移動するときには高い値に切り替え、ワークとハンド間に引付力あるいは押付力が作用するときには低い値に切り替えようとするものであるが、上記のようにロボット動作中に電源電圧が変動することについて示唆も開示もない。

特許文献３では、サーボアンプの入力許容電圧にはある程度の幅があることを考慮して、測定した入力電圧に基づいてサーボアンプの制御ゲインを設定するものであるが、上記のようにロボット動作中の電源電圧の変動を考慮したものではない。また、サーボアンプの制御ゲインを設定するにあたり、サーボモータの相電流の無効電流成分比から推定された電源電圧が用いられており、複雑な制御が必要である。

本発明は、上記のような従来の課題を解決するためになされたものであり、ロボット動作中のサーボアンプの電源電圧の変動に対して簡易な制御で制御ループを安定化させることを目的とする。

上記目的を達成するために、本発明に係るサーボアンプの制御ループゲイン調整方法は、ロボットの各関節に設けられたサーボモータに対して外部からの制御指令に応じた該サーボモータの制御量を帰還させて制御ループを形成する、サーボアンプの制御ループゲイン調整方法であって、前記制御ループゲインのデフォルト値及び前記サーボアンプの電源電圧のデフォルト値に基づいて、前記制御ループを形成するとともに前記サーボモータの駆動を開始する工程と、前記サーボアンプの電源電圧を検出する工程と、検出した前記サーボアンプの電源電圧の変化に対して逆方向に変化するように前記制御ループゲインを調整する工程と、を備えるものである。

この方法によれば、ロボットの各関節に設けられたサーボモータの駆動中（ロボット動作中）にサーボアンプの電源電圧が変動しても、サーボアンプの電源電圧の変化に対して逆方向に制御ループゲインが変化するので、制御ループの安定化が図られる。

上記のサーボアンプの制御ループゲイン調整方法において、記制御ループは、前記サーボモータの電流を帰還させて形成される電流ループを含み、前記制御ループゲインを調整する工程は、前記電流ループ全体のゲインである電流ループゲインに含まれる比例ゲインを調整する、としてもよい。

この方法によれば、サーボアンプの電源電圧の変動に対して、簡易な制御で電流ループを安定化させることができる。

上記のサーボアンプの制御ループゲイン調整方法において、検出した前記サーボアンプの電源電圧を上限値から下限値までの範囲内にリミット処理する工程を備える、としてもよい。

この方法によれば、簡易な制御で電流ループをさらに安定化させることができる。

上記のサーボアンプの制御ループゲイン調整方法において、調整した前記制御ループゲインを上限値から下限値までの範囲内にリミット処理する工程を備える、としてもよい。

この方法によれば、簡易な制御で電流ループをさらに安定化させることができる。

上記のサーボアンプの制御ループゲイン調整方法において、検出した前記サーボアンプの電源電圧又は調整した前記制御ループゲインをフィルタ処理する工程を備える、としてもよい。

この方法によれば、簡易な制御で電流ループをさらに安定化させることができる。
上記目的を達成するために、その他の本発明に係るサーボアンプの制御ループゲイン調整プログラムは、ロボットの各関節に設けられたサーボモータに対して外部からの制御指令に応じた該サーボモータの制御量を帰還させて制御ループを形成する、サーボアンプの制御ループゲイン調整プログラムであって、コンピュータに、前記制御ループゲインのデフォルト値及び前記サーボアンプの電源電圧のデフォルト値に基づいて、前記制御ループを形成するとともに前記サーボモータの駆動を開始する工程と、前記サーボアンプの電源電圧を検出する工程と、検出した前記サーボアンプの電源電圧の変化に対して逆方向に変化するように前記制御ループゲインを調整する工程と、を遂行させる、ものである。

上記目的を達成するために、その他の本発明に係るロボット制御装置は、ロボットの各関節に設けられたサーボモータに対して、外部からの制御指令に応じた該サーボモータの制御量を帰還させて制御ループを形成するサーボアンプを備えたロボット制御装置において、前記サーボアンプは、前記サーボモータを駆動する駆動部と、前記サーボアンプ全体の制御を司る制御部と、商用電源を整流して得られる直流の電源電圧を前記制御部に供給する電源部と、を備え、前記制御部は、前記制御ループの制御ループゲインのデフォルト値及び前記電源部の前記電源電圧のデフォルト値に基づいて、前記制御ループを形成するとともに、前記駆動部による前記サーボモータの駆動を開始する手段と、前記電源部の前記電源電圧を検出する手段と、検出した前記電源電圧が上昇する場合には減少させ、前記の電源電圧が下降する場合には増大させるように前記制御ループゲインを調整する手段と、を備えるものである。

本発明によれば、ロボット動作中のサーボアンプの電源電圧の変動に対して簡易な制御で制御ループを安定化させることができる。

図１は、本発明の実施の形態に係るサーボアンプを含むロボット制御システムの全体構成を示した図である。 図２は、図１に示すロボット制御装置の詳細な構成を示した図である。 図３は、図２に示す電流サーボ制御系の概略のブロック線図である。 図４は、図１、図２に示すロボット制御装置の電流ループゲインの調整動作を説明するフローチャートである。 図５は、図４に示すフローチャートを遂行したときの電流ループゲインＫＣＰ及びＰＮ間電圧ＰＮｖｏｌｔａｇｅの時間的推移を表したグラフである。

以下、本発明の好ましい実施の形態を、図面を参照しながら説明する。なお、以下では全ての図を通じて同一又は相当する要素には同一の参照符号を付して、特に言及しない場合にはその重複する説明を省略する。

[ロボット制御システムの構成]
図１は、本発明の実施の形態に係るサーボアンプを含むロボット制御システムの全体構成を示した図である。

多関節ロボット１００は、基台２が作業スペース下側の床面に設置される所謂床置き型の６軸の垂直ロボットとして構成されている。つまり、多関節ロボット１００は、先端に設けられた手首と所定の基端から当該手首に向かって順に設けられた６つの関節ＪＴ１〜ＪＴ６とを具備し６つの関節ＪＴ１〜ＪＴ６が第１乃至第６回転軸Ａ１〜Ａ６をそれぞれ有し、かつ各回転軸Ａ１〜Ａ６の周りに次の関節を回転させるように構成されている。なお、上記の６つの関節及び６つの回転軸に付与された符号（ＪＴ１〜ＪＴ６、Ａ１〜Ａ６）は便宜上付したものであり、６つの関節及び６つの回転軸それぞれを識別可能であれば如何なる符号であってもよい。

基台２が床面に適正に設置されると、第１乃至第６回転軸Ａ１〜Ａ６は、全ての互いに隣り合う関節の回転軸が互いに垂直となるように配置されている。基台２には、旋回台３、アーム部材（リンク）４、５、６、７及びアタッチメント９がこの順に連設されている。なお、アタッチメント９の先端を成すフランジ面には、各種の作業内容に応じて適宜選択されたツール部材１１（図示せず）が着脱可能に取り付けられている。基台２からアタッチメント９までの連設された部材２〜７，９は互いに相対回転可能となるよう連結されている。以下、このように６つの関節ＪＴ１〜ＪＴ６によって互いに連接された部材群２〜７，９を、ロボットアームと定義する。

アーム部材６，７、及びアタッチメント９は、アタッチメント９に取り付けられるツール部材１１に微細な動作を行わせるための人間の手首に似せた構造体１０（以下、手首装置という。）を成している。第１回転軸Ａ１、第２回転軸Ａ２及び第３回転軸Ａ３は、ツール部材１１を手首装置１０とともに水平旋回させたり揺動させたりするための回転軸として用いられ、多関節ロボット１００の主軸を成している。第４回転軸Ａ４、第５回転軸Ａ５及び第６回転軸Ａ６は、手首装置１０に設定される回転軸である。

第１乃至第６関節ＪＴ１〜ＪＴ６にはサーボモータＭ１〜Ｍ６及び位置検出器Ｅ１〜Ｅ６がそれぞれ設けられている。位置検出器Ｅ１〜Ｅ６は、例えば、ロータリーエンコーダで構成されている。上記の各サーボモータＭ１〜Ｍ６を駆動することにより、第１乃至第６関節ＪＴ１〜ＪＴ６においてそれぞれ許容される第１乃至第６回転軸Ａ１〜Ａ６周りの回転が行われる。なお、各サーボモータＭ１〜Ｍ６は互いに独立して駆動することが可能である。また、上記の各サーボモータＭ１〜Ｍ６が駆動されると、上記の各位置検出器Ｅ１〜Ｅ６によって上記の各サーボモータＭ１〜Ｍ６の第１乃至第６回転軸Ａ１〜Ａ６周りの回転位置の検出が行われる。

ロボット制御装置２００は、多関節ロボット１００の基台２の周辺に配置されている。なお、多関節ロボット１００と遠隔に配置されていてもよいし、多関節ロボット１００と物理的に着脱可能な形態で接続されてもよい。ロボット制御装置２００は、サーボアンプ２１０を備えている。サーボアンプ２１０は、多関節ロボット１００の第１乃至第６関節ＪＴ１〜ＪＴ６が具備するサーボモータＭ１〜Ｍ６それぞれに対し、アタッチメント９に取り付けられたツール部材１１を任意の位置及び姿勢に任意の経路に沿って移動させるサーボ制御を行うものである。

サーボアンプ２１０は、駆動部２３０、制御部２４０及び電源部２５０を備えている。なお、制御部２４０及び電源部２５０は、サーボアンプ２１０の外部に設けられてもよく、換言すると、サーボアンプ２１０は、駆動部２３０のみで構成されていてもよい。

駆動部２３０は、多関節ロボット１００のサーボモータＭ１〜Ｍ６を駆動する直流−交流変換器であり、例えば、フルブリッジ回路又はハーフブリッジ回路から成るインバータで構成されている。なお、サーボアンプ２１０はサーボモータＭ１〜Ｍ６それぞれに対して個別のサーボループを形成するように複数設けられているが、１つのサーボアンプ２１０によってサーボモータＭ１〜Ｍ６が統括して駆動されるように構成されてもよい。この場合、駆動部２３０のみがサーボモータＭ１〜Ｍ６毎に設けられる。

制御部２４０は、サーボアンプ２１０を含めたロボット制御装置２００全体の制御を司るものであり、例えば、マイクロコントローラ、ＣＰＵ、ＭＰＵ、ＰＬＣ、ＤＳＰ、ＡＳＩＣ又はＦＰＧＡなどで構成されている。なお、制御部２４０は、互いに協働して分散制御する複数の制御器によって構成されていてもよい。例えば、上位制御装置とのインタフェース処理をＣＰＵが担当し、サーボアンプ２１０の基本演算（位置ループ演算、速度ループ演算及び電流ループ演算）をＤＳＰが担当するようにシステム構成されてもよい。

電源部２５０は、商用電源２２０（一次電源）を直流に整流して得られる電源電圧（後述のＰＮ間電圧ＰＮｖｏｌｔａｇｅ）を駆動部２３０に供給する交流−直流変換器であり、例えば、サイリスタ整流器、スイッチング電源によって構成されている。

ロボット制御装置２００は、ティーチペンダントなどの周辺装置（図示せず）と通信可能に接続可能となっている。ロボット制御装置２００は、この周辺装置のオペレータの操作によって記憶された位置情報に応じて多関節ロボット１００を並進又は回転運動させるための位置指令（制御指令）が入力される。すると、ロボット制御装置２００は、制御時間毎に入力された上記の位置指令に基づいて、ツール部材１１が当該制御時間の経過後に位置すべき目標位置を算出する。かかる目標位置は、制御時間と予め定められたツール部材１１の制限移動速度などから求まる移動距離に基づいて算出された後、制御部２４０が演算可能となるようにツール座標系（直交座標系）において定義される座標データという形式に変換される。そして、制御部２４０は、目標位置の座標データの逆変換処理を行い、制御時間の経過後にツール部材１１を目標位置に移動させるために必要となる関節角θ１〜θ６をそれぞれ演算し、これらの関節角θ１〜θ６と上記の位置検出器Ｅ１〜Ｅ６により検出される電源オン時の回転位置との偏差に基づき、第１乃至第６関節ＪＴ１〜ＪＴ６に設けられたサーボモータＭ１〜Ｍ６の動作量の指令値をそれぞれ演算し、各サーボモータＭ１〜Ｍ６に供給する。これにより、ツール部材１１が制御時間の経過毎に目標位置に移動される。

[ロボット制御装置の構成]
図２は、図１に示すロボット制御装置の詳細な構成を示した図である。なお、同図に示す参照符号Ｍ及びＥは、図１に示す各軸Ａ１〜Ａ６のサーボモータＭ１〜Ｍ６及び位置検出器Ｅ１〜Ｅ６のうちのいずれか一つを表している。駆動部２３０及び制御部２４０は、各軸Ａ１〜Ａ６それぞれに設けられるものである。また、同図に示す太線は、電流サーボ系の電流ループを表している。

電源部２５０は、商用電源２２０から供される三相交流又は単相交流を全波整流する全波整流器２５１と、全波整流器２５１の出力を直流に平滑化する平滑コンデンサ２５２と、サーボモータＭが発電機として作動する回生時に用いられる回生抵抗２５３及び回生電力放電用トランジスタ２５４とを備えている。平滑コンデンサ２５２の両端子間の直流電圧は、後述の駆動部２３０のソーストランジスタＴ１及びシンクトランジスタＴ２の電源電圧として用いられる。以下では、平滑コンデンサ２５２の両端子間の電圧のことを、ＰＮ間電圧ＰＮｖｏｌｔａｇｅと呼ぶ。このＰＮ間電圧ＰＮｖｏｌｔａｇｅを検出するために、電圧検出器２５５が設けられている。

駆動部２３０は、サーボモータＭの各相それぞれに対して、ＰＮ間電圧ＰＮｖｏｌｔａｇｅを規定する正極Ｐと負極Ｎとの間に、例えば、ソーストランジスタＴ１とシンクトランジスタＴ２とが直列に接続されており、この接続点からサーボモータＭの対応したコイルにコイル電流を供給するように構成されている。また、駆動部２３０は、電流サーボ制御のために、駆動部２３０からサーボモータＭのコイルに供給されるコイル電流を検出する電流検出器２３１が設けられている。さらに、駆動部２３０は、無効電流及びサーボモータＭが発電機として作動する回生時における電流の経路を形成するために、ソーストランジスタＴ１とシンクトランジスタＴ２それぞれに対して回生ダイオードＤ１，Ｄ２を並列に設けている。なお、図２では、図面の簡略化のために、サーボモータＭの１相分の駆動部２３０の構成のみを図示している。

制御部２４０は、位置制御部２４１と、速度制御部２４２と、電流制御部２４３と、パルス幅変調部２４４と、回生制御部２４５と、ゲイン調整部２４６とを備えている。位置制御部２４１は、ロボット制御装置２００からの位置指令とサーボモータＭの位置検出器Ｅにより検出された回転位置との偏差に応じた速度指令を出力する。速度制御部２４２は、位置制御部２４１から出力された速度指令と位置検出器Ｅにより検出された回転位置の微分値（回転速度）との偏差に応じた電流指令（トルク指令）を出力する。電流制御部２４３は、速度制御部２４２から出力された電流指令と電流検出器２３１により検出されたコイル電流との偏差に応じた電圧指令を出力する。パルス幅変調部２４４は、電流制御部２４３から出力された電圧指令を三角波信号と比較して、駆動部２３０のソーストランジスタＴ１及びシンクトランジスタＴ２をオンオフさせるＰＷＭ信号を生成して出力する。

回生制御部２４５は、電圧検出器２５５により検出されたＰＮ間電圧ＰＮｖｏｌｔａｇｅを所定の閾値電圧（例えば、後述の上限値ＰＮＶＯＬＴ＿ＭＡＸ以上の値）と比較した結果に基づいて、回生電力放電用トランジスタ２５４をオンオフさせるスイッチング制御信号を出力する。詳しくは、回生制御部２４５は、ＰＮ間電圧ＰＮｖｏｌｔａｇｅが閾値電圧を上回るとき、回生電力放電用トランジスタ２５４をオンさせ、ＰＮ間電圧ＰＮｖｏｌｔａｇｅが閾値電圧を下回るとき、回生電力放電用トランジスタ２５４をオフさせるように、スイッチング制御信号を出力する。

ゲイン調整部２４６は、電圧検出器２５５により検出されたＰＮ間電圧ＰＮｖｏｌｔａｇｅのレベルに基づいて、電流制御部２４３の電流ループゲインを調整するものである。

図３は、図２に示す電流サーボ制御系の概略のブロック線図である。同図に示すように、電流制御部２４３は、速度制御部２４２から出力される電流指令ｉと電流検出器２３１により検出されたコイル電流との偏差が入力され、該偏差を入力とする比例ゲインＫＣＰの伝達関数の出力を出力する比例要素２４７と、比例要素２４７の出力を入力とする積分ゲインＫＣＩの伝達関数の出力を出力する積分要素２４８と、を備え、比例要素２４７の出力と積分要素２４８の出力とを加算した結果を電圧指令ｖとして出力するものである。電流制御部２４３から出力された電圧指令ｖは電子回路上でパルス幅変調部２４４によりスイッチングされた後に抵抗成分Ｒとインダクタンス成分Ｌを有するサーボモータＭに与えられ、その応答としてのコイル電流ｉが電流制御部２４３の入力へとフィードバックされる。

ここで、電流制御部２４３のブロック構成では、積分要素２４８は単なる時定数要素として作用するので、比例要素２４７の作用が支配的となっている。従って、図３に示す電流制御部２４３の電流ループゲインは、比例要素２４７の比例ゲインＫＣＰのみで取り扱っても差し支えなく、電流制御部２４３では、比例要素２４７の比例ゲインＫＣＰをＰＮ間電圧ＰＮｖｏｌｔａｇｅのレベルに応じて可変なゲインとしている。

[ロボット制御装置の動作（電流ループゲイン調整）]
図４は、図１、図２に示すロボット制御装置の電流ループゲインの調整動作を説明するフローチャートである。

まず、商用電源２２０の交流電力が全波整流器２５１によって全波整流され、平滑コンデンサ２５２には直流かつデフォルト値ＰＮＶＯＬＴ＿ＤＥＦ（例えば、実効値２１０Ｖ、整流値２９７Ｖ）のＰＮ間電圧ＰＮｖｏｌｔａｇｅが充電されており、このＰＮ間電圧ＰＮｖｏｌｔａｇｅが駆動部２３０のブリッジ回路の電源電圧として出力されている。また、ゲイン調整部２４６は、電流制御部２４３の電流ループゲイン（比例ゲイン）ＫＣＰをデフォルト値ＫＣＰ＿Ｄｅｆａｕｌｔに初期設定している（ステップＳ４００）。

つぎに、ロボット制御装置２００は、多関節ロボット１００のサーボモータＭ１〜Ｍ６に対して位置サーボ制御、速度サーボ制御及び電流サーボ制御を開始する。すると、ロボットアームの加速や該ロボットアーム先端のツール部材１１によってワークを持ち上げる場合など、多くの電力消費を伴うサーボ制御が行われるときに、商用電源２２０から供給される電力が各軸のサーボモータＭ１〜Ｍ６で消費される電力に間に合わず、電源部２５０から駆動部２３０のブリッジ回路に供給されるＰＮ間電圧ＰＮｖｏｌｔａｇｅが低下することがある。あるいは、ロボットアームの減速や該ロボットアーム先端のツール部材１１によってワークを降ろす場合など、サーボモータＭ１〜Ｍ６が発電機として作動する回生動作を伴うサーボ制御が行われるときに、回生抵抗２５３によってサーボモータＭ１〜Ｍ６が発生した回生電力を消費しきれず、ＰＮ間電圧ＰＮｖｏｌｔａｇｅが上昇することがある。

そこで、ゲイン調整部２４６は、ＰＮ間電圧ＰＮｖｏｌｔａｇｅがデフォルト値ＰＮＶＯＬＴ＿ＤＥＦのα倍になれば、電流制御部２４３の電流ループゲインＫＣＰをデフォルト値ＫＣＰ＿Ｄｅｆａｕｌｔの１／α倍に変更する。これにより、ロボットアームの動作状況によってＰＮ間電圧ＰＮｖｏｌｔａｇｅが変動したとしても、電流サーボ制御の電流ループ特性を一定に保つことができる。

具体的には、ゲイン調整部２４６は、電圧検出器２５５によって検出されたＰＮ間電圧ＰＮｖｏｌｔａｇｅを取得して所定の変数に代入し（ステップＳ４０１）、このＰＮ間電圧ＰＮｖｏｌｔａｇｅの変数値が上限値ＰＮＶＯＬＴ＿ＭＡＸ（例えば３５０Ｖ）から下限値ＰＮＶＯＬＴ＿ＭＩＮ（例えば２５０Ｖ）までの範囲内に収まっている否かを判定する（ステップＳ４０２）。ＰＮ間電圧ＰＮｖｏｌｔａｇｅの変数値が上限値ＰＮＶＯＬＴ＿ＭＡＸから下限値ＰＮＶＯＬＴ＿ＭＩＮまでの範囲内に収まっていれば（ステップＳ４０２：ＹＥＳ）、電流制御部２４３の電流ループゲインＫＣＰを次式により演算して所定のバッファに書き込む（ステップＳ４０３）。

ＫＣＰ＝（ＰＮＶＯＬＴ＿ＤＥＦ[Ｖ]／ＰＮｖｏｌｔａｇｅ[Ｖ]）＊ＫＣＰ＿ｄｅｆａｕｌｔ ・・・（式１）
（式１）はＰＮ間電圧ＰＮｖｏｌｔａｇｅと電流ループゲインＫＣＰとの間には反比例の関係が成立することを表している。

ＰＮ間電圧ＰＮｖｏｌｔａｇｅの変数値が上限値ＰＮＶＯＬＴ＿ＭＡＸ以上であれば（ステップＳ４０２：ＮＯ）、ＰＮ間電圧ＰＮｖｏｌｔａｇｅの変数値を上限値ＰＮＶＯＬＴ＿ＭＡＸに維持し、下限値ＰＮＶＯＬＴ＿ＭＩＮ以下であれば（ステップＳ４０２：ＮＯ）、ＰＮ間電圧ＰＮｖｏｌｔａｇｅの変数値を下限値ＰＮＶＯＬＴに維持する（ステップＳ４０４）。この様にする理由は、電流ループゲインＫＣＰの急激な変化に伴う電流サーボ制御の不安定性を抑止するためである。そして、上記のステップＳ４０３に移行して、（式１）に基づいて電流ループゲインＫＣＰが演算される。つまり、電流ループゲインＫＣＰの上限値ＫＣＰ＿ＭＡＸ及び下限値ＫＣＰ＿ＭＩＮは、それぞれ次式で表される。

ＫＣＰ＿ＭＡＸ＝（ＰＮＶＯＬＴ＿ＤＥＦ[Ｖ]／ＰＮＶＯＬＴ＿ＭＩＮ[Ｖ]）＊ＫＣＰ＿ｄｅｆａｕｌｔ ・・・（式２）
ＫＣＰ＿ＭＩＮ＝（ＰＮＶＯＬＴ＿ＤＥＦ[Ｖ]／ＰＮＶＯＬＴ＿ＭＡＸ[Ｖ]）＊ＫＣＰ＿ｄｅｆａｕｌｔ ・・・（式３）
つぎに、ゲイン調整部２４６は、バッファに書き込まれた電流ループゲインＫＣＰの変数値に対してローパスフィルタ処理を実行し（ステップＳ４０５）、上限値ＫＣＰ＿ＭＡＸから下限値ＫＣＰ＿ＭＩＮの範囲内に収まるようにリミット処理を実行する（ステップＳ４０６）。電流ループゲインＫＣＰの急激な変化に伴う電流サーボ制御の不安定性をさらに抑止するためである。

つぎに、ゲイン調整部２４６は、電流制御部２４３に初期設定されているデフォルト値ＫＣＰ＿Ｄｅｆａｕｌｔを、ステップＳ４０６で求められた電流ループゲインＫＣＰに変更する（ステップＳ４０７）。以降の処理として、ゲイン調整部２４６は、ステップＳ４０１〜Ｓ４０７を繰り返し実行する。

図５は、図４に示すフローチャートに従って電流ループゲインＫＣＰを調整したときのＰＮ間電圧ＰＮｖｏｌｔａｇｅ及び電流ループゲインＫＣＰの時間的推移を表したグラフである。詳しくは、ＰＮ間電圧ＰＮｖｏｌｔａｇｅが上限値ＰＮＶＯＬＴ＿ＭＡＸ以上となるときの電流ループゲインＫＣＰの時間的推移を表したグラフである。以下、同図に現れた状況を説明する。

ロボットアームの減速や該ロボットアーム先端のツール部材１１によってワークを降ろすことで、ＰＮ間電圧ＰＮｖｏｌｔａｇｅはデフォルト値ＰＮＶＯＬＴ＿ＤＥＦから上限値ＰＮＶＯＬＴ＿ＭＡＸまで上昇する。このとき、ＰＮ間電圧ＰＮｖｏｌｔａｇｅは上限値ＰＮＶＯＬＴ＿ＭＡＸから下限値ＰＮＶＯＬＴ＿ＭＩＮまでの範囲内に収まっているので、ゲイン調整部２４６は、上記の（式１）に従って、ＰＮ間電圧ＰＮｖｏｌｔａｇｅの上昇に反比例して電流制御部２４３の電流ループゲインＫＣＰを下げるようにする。

ＰＮ間電圧ＰＮｖｏｌｔａｇｅが上限値ＰＮＶＯＬＴ＿ＭＡＸ以上となれば、ゲイン調整部２４６は、電流制御部２４３の電流ループゲインＫＣＰを下限値ＫＣＰ＿ＭＩＮに維持する。なお、ＰＮ間電圧ＰＮｖｏｌｔａｇｅの上昇に伴ってサーボモータＭは発電機として作動している。すると、回生制御部２４５は、ＰＮ間電圧ＰＮｖｏｌｔａｇｅが上限値ＰＮＶＯＬＴ＿ＭＡＸ以上の閾値電圧となったことを契機として、回生電力放電用トランジスタ２５４をオフからオンに切り替えるので、回生抵抗２５３によって回生電力が消費され、ＰＮ間電圧ＰＮｖｏｌｔａｇｅは下降することになる。なお、回生動作は継続しているため、ＰＮ間電圧ＰＮｖｏｌｔａｇｅは上限値ＰＮＶＯＬＴ＿ＭＡＸまで下降すると、回生制御部２４５は、回生電力放電用トランジスタ２５４をオンからオフに切り替え、ＰＮ間電圧ＰＮｖｏｌｔａｇｅは再び上昇する。このように、回生電力放電用トランジスタ２５４のオンオフにより、ＰＮ間電圧ＰＮｖｏｌｔａｇｅは上昇と下降とを繰り返すことになる。

回生動作が終了したとき、ＰＮ間電圧ＰＮｖｏｌｔａｇｅは上限値ＰＮＶＯＬＴ＿ＭＡＸからデフォルト値ＰＮＶＯＬＴ＿ＤＥＦまで徐々に下降する。このとき、ＰＮ間電圧ＰＮｖｏｌｔａｇｅは上限値ＰＮＶＯＬＴ＿ＭＡＸから下限値ＰＮＶＯＬＴ＿ＭＩＮまでの範囲内に収まっているので、ゲイン調整部２４６は、上記の（式１）に従って、ＰＮ間電圧ＰＮｖｏｌｔａｇｅの下降に反比例して電流制御部２４３の電流ループゲインＫＣＰを下限値ＫＣＰ＿ＭＩＮから上げるようにする。なお、ＰＮ間電圧ＰＮｖｏｌｔａｇｅはデフォルト値ＰＮＶＯＬＴ＿ＤＥＦとなった後では、電流ループゲインＫＣＰはデフォルト値ＫＣＰ＿Ｄｅｆａｕｌｔに維持される。

[変形例]
ＰＮ間電圧ＰＮｖｏｌｔａｇｅのレベルに応じて可変させる電流制御ループゲインは、電流制御部２４３の比例ゲインＫＣＰ及び積分ゲインＫＣＩの両方であってもよい。また、電流制御部２４３はＰＩＤ制御を行うように構成されてもよく、この場合、ＰＮ間電圧ＰＮｖｏｌｔａｇｅのレベルに応じて可変させる電流制御ループゲインは、比例ゲイン、比例ゲイン＋積分ゲイン、比例ゲイン＋微分ゲイン、又は比例ゲイン＋積分ゲイン＋微分ゲインのうちのいずれか一つとしてもよい。
電流ループゲインＫＣＰに対して実行したローパスフィルタ処理は、電圧検出器２５５から取得したＰＮ間電圧ＰＮｖｏｌｔａｇｅに対して実行してもよい。
電流ループゲインＫＣＰに対して実行したローパスフィルタ処理は、バンドパスフィルタ処理などの他のフィルタ処理であってもよい。

なお、ロボット制御装置２００の制御対象であるロボットは、上述の６軸の多関節ロボット１００には限定されず、任意のロボットであってよい。例えば、６軸以外の多軸の関節ロボット、関節ロボット以外のタイプのロボットであってもよい。

また、駆動部２３０のブリッジ回路を構成するスイッチング素子として、上述のトランジスタ以外に、ＧＴＯ、ＩＧＢＴ、ＳＩサイリスタ等を用いることができる。

上記説明から、当業者にとっては、本発明の多くの改良や他の実施形態が明らかである。従って、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び／又は機能の詳細を実質的に変更できる。

本発明は、サーボアンプの制御ループゲイン調整方法、プログラム及びロボット制御装置に係り、ロボット動作中のサーボアンプの電源電圧の変動に対して簡易な制御で制御ループを安定化させる上で有用である。

１００…多関節ロボット
Ｍ１〜Ｍ６…サーボモータ
Ｅ１〜Ｅ６…位置検出器
Ａ１〜Ａ６…回転軸
ＪＴ１〜ＪＴ６…関節
２…基台
３…旋回台
４〜７…アーム部材
９…アタッチメント
１０…手首装置
１１…ツール部材
２００…ロボット制御装置
２１０…サーボアンプ
２２０…商用電源
２３０…駆動部
２３１…電流検出器
Ｔ１…ソーストランジスタ
Ｔ２…シンクトランジスタ
Ｄ１、Ｄ２…回生ダイオード
２４０…制御部
２４１…位置制御部
２４２…速度制御部
２４３…電流制御部
２４４…パルス幅変調部
２４５…回生制御部
２４６…ゲイン調整部
２４７…比例要素
２４８…積分要素
２５０…電源部
２５１…全波整流器
２５２…平滑コンデンサ
２５３…回生抵抗
２５４…回生電力放電用トランジスタ
２５５…電圧検出器

Claims (7)

1. ロボットの各関節に設けられたサーボモータに対して外部からの制御指令に応じた該サーボモータの制御量を帰還させて制御ループを形成する、サーボアンプの制御ループゲイン調整方法であって、
   前記制御ループゲインのデフォルト値及び前記サーボアンプの電源電圧のデフォルト値に基づいて、前記制御ループを形成するとともに前記サーボモータの駆動を開始する工程と、
   前記サーボアンプの電源電圧を検出する工程と、
   検出した前記サーボアンプの電源電圧の変化に対して逆方向に変化するように前記制御ループゲインを調整する工程と、
   を備えるサーボアンプの制御ループゲイン調整方法。
2. 前記制御ループは、前記サーボモータの電流を帰還させて形成される電流ループを含み、
   前記制御ループゲインを調整する工程は、前記電流ループ全体のゲインである電流ループゲインに含まれる比例ゲインを調整する、請求項１に記載のサーボアンプの制御ループゲイン調整方法。
3. 検出した前記サーボアンプの電源電圧を上限値から下限値までの範囲内にリミット処理する工程を備える、請求項１に記載のサーボアンプの制御ループゲイン調整方法
4. 調整した前記制御ループゲインを上限値から下限値までの範囲内にリミット処理する工程を備える、請求項１に記載のサーボアンプの制御ループゲイン調整方法。
5. 検出した前記サーボアンプの電源電圧又は調整した前記制御ループゲインをフィルタ処理する工程を備える、請求項１に記載のサーボアンプの制御ループゲイン調整方法。
6. ロボットの各関節に設けられたサーボモータに対して外部からの制御指令に応じた該サーボモータの制御量を帰還させて制御ループを形成する、サーボアンプの制御ループゲイン調整プログラムであって、
   コンピュータに、
   前記制御ループゲインのデフォルト値及び前記サーボアンプの電源電圧のデフォルト値に基づいて、前記制御ループを形成するとともに前記サーボモータの駆動を開始する工程と、
   前記サーボアンプの電源電圧を検出する工程と、
   検出した前記サーボアンプの電源電圧の変化に対して逆方向に変化するように前記制御ループゲインを調整する工程と、
   を遂行させる、サーボアンプの制御ループゲイン調整プログラム。
7. ロボットの各関節に設けられたサーボモータに対して、外部からの制御指令に応じた該サーボモータの制御量を帰還させて制御ループを形成するサーボアンプを備えたロボット制御装置において、
   前記サーボアンプは、
   前記サーボモータを駆動する駆動部と、
   前記サーボアンプ全体の制御を司る制御部と、
   商用電源を整流して得られる直流の電源電圧を前記制御部に供給する電源部と、を備え、
   前記制御部は、
   前記制御ループの制御ループゲインのデフォルト値及び前記電源部の前記電源電圧のデフォルト値に基づいて、前記制御ループを形成するとともに、前記駆動部による前記サーボモータの駆動を開始する手段と、
   前記電源部の前記電源電圧を検出する手段と、
   検出した前記電源電圧が上昇する場合には減少させ、前記の電源電圧が下降する場合には増大させるように前記制御ループゲインを調整する手段と、
   を備えるロボット制御装置。
   

Images