JP2009028865A - 多関節型ロボットの制御方法 - Google Patents

Abstract

【課題】従来のオブザーバ制御演算装置では、ロボット自身の姿勢やアームに取り付ける負荷の変化に対応して制御性能の劣化を防ぐには、負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータのリアルタイムでの演算が必要であり、演算処理の負荷が非常に多くなる。
【解決手段】アームに取り付けられる複数の負荷に応じて予め調整されたパラメータを持つ複数の状態オブザーバと状態ＦＢの演算を同時に行い、アームに取り付けられる負荷情報に基づいてそれぞれの状態ＦＢ値のモータ電流指令への加算量を調整する際に、各姿勢におけるアーム回転半径の最大値に対する２乗比の１次関数として定義する姿勢ゲインを乗じることにより、リアルタイムで負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算を行う必要はなく、ロボットの姿勢変化に応じた振動抑制効果が得られる状態で演算時間を削減できる。
【選択図】図１

Description

本発明は、減速機を介してモータにより駆動されるロボットの位置制御における多関節型ロボットの制御方法に関するものである。

近年、溶接やハンドリングに用いられる垂直多関節ロボットにおいて、作業精度の向上が求められている。しかし、垂直多関節ロボットは、合成アーム長が１ｍ以上、構成軸が６軸以上の構成が一般的であり、アームのしなりや各軸に搭載されている減速機のバネ成分によりアーム先端は振動しやすい構造になっている。特に、減速機のバネ成分による振動は、大型ロボットでは１０Ｈｚ以下の低い周波数となり、位置制御性能に大きな影響を与える。

図２は、ロボットにおけるモータと減速機をモデル化したものであり、モータ取り付けベースとなる第１アーム（１）にモータ（２）、減速機（３）、ベアリング（４）が固定され、減速機２次側（７）の回転部に結合された負荷である第２アーム（９）を駆動する。

減速機１次側（６）はモータ回転軸（１０）でモータ内のロータに結合され、モータ回転速度ωＭ（１１）で回転する。減速機（３）は減速比Ｒｇで、モータ回転速度ωＭ（１１）を負荷回転速度ωＬ（１２）に減速する。

しかし、減速機（３）は減速機１次側（６）と減速機２次側（７）の間にバネ成分が存在するので、（数１）の式が成立するのは、バネの伸びが一定となった定常状態のみである。このバネ成分のバネ定数をＫｓとして、図２のモデルをブロック線図で表したものが図３である。

図３において、電流指令ｉｃｏｍ（１３）はモータ（２）を駆動するモータ電流指令、Ｋｔ（１４）はモータ（２）のトルク定数、１／Ｒｇ（１５、１６）は減速比の逆数、（１７）はモータ伝達関数、（１８）は負荷伝達関数、バネ定数 Ｋｓ（１９）は減速機（３）のバネ定数、θｓ（２０）は減速機１次側（６）と減速機２次側（７）間に発生するねじれ角、（２１）は積分要素、Ｔｄ（２２）は負荷（第２アーム（９））に加わる外力である。

モータ伝達関数（１７）において、ＪＭはモータロータ（５）と減速機１次側（６）を合わせた回転軸（１０）回りの慣性モーメント、ＤＭは粘性摩擦係数である。また、負荷伝達関数（１８）においても、ＪＬは負荷（第２アーム（９））と減速機２次側（７）を合わせた回転軸（１０）回りの慣性モーメント、ＤＬは粘性摩擦係数である。

図３の負荷に対し、通常用いられる位置決め制御ループを示したものが図４である。

図４において、位置制御ブロック（２７）は、モータに接続されたエンコーダ等によって検出されるモータ回転速度ωＭを積分要素（２５）で積分したモータ位置フィードバックθＭ（２４）を、モータ位置指令θｃｏｍ（２３）から減算し、位置比例ゲインＫＰＰ（２６）を乗じてモータ速度指令ωｃｏｍ（２８）を生成する。

速度制御ブロック（３１）は、速度指令ωｃｏｍ（２８）とモータ回転速度ωＭより下記式で、モータ電流指令ｉｃｏｍ０（３２）を生成する。

ただし、

図４で示す通常の位置制御ループでは、負荷速度ωＬ（１２）や減速機ねじれ角θｓ（２０）を無視し、モータ速度ωＭ（１１）のみを観測して位置制御を行うため、減速機のバネ成分による負荷速度ωＬ（１２）の振動を抑制することは出来なかった。

そこで、負荷速度ωＬ（１２）や減速機ねじれ角θｓ（２０）を計測し、その計測値を用いてフィードバック（以下ＦＢと省略）制御を行うことで、減速機のバネ成分による振動を抑制する方式が考えられる。しかし、負荷速度ωＬ（１２）や減速機ねじれ角θｓ（２０）の計測機能を付加することにより、装置コストや負荷質量の増大を招くので、製品化するにはデメリットが大きい。

上記課題を解決するために、図５では、図４で示した通常の位置制御ループに、状態ＦＢブロック（３９）を加えている。

状態ＦＢブロック（３９）においては、電流指令ｉｃｏｍ（１３）とモータ速度ωＭ（１１）の値を用いて、状態オブザーバ（３３）により、負荷速度推定値ωＬｏ（３４）や減速機ねじれ角推定値θｓｏ（３５）を推定する。これらの推定値とモータ回転速度ωＭ（１１）にゲイン（３６）〜（３８）を乗じ、速度制御ブロックより生成された電流指令ｉｃｏｍ０（３２）から減じて、新たに電流指令ｉｃｏｍ（１３）を生成する。

このように構成することにより、状態ＦＢゲイン（３６）〜（３８）を調整することで、減速機のバネ成分による負荷速度ωＬ（１２）の振動を抑制する制御特性の実現が可能になる。

図６は図５における状態オブザーバ（３３）を最小次元オブザーバで構成した場合の内部構造を示したもので、状態オブザーバパラメータ６１〜７０を以下の式で計算することにより構成する。

（数５）において、ｋ１、ｋ２（図６では６９、７０）は、オブザーバの応答性と安定性を決定するゲインで、オブザーバの応答性と安定性を決定する極をα１、α２と指定すると以下の式で計算される。

一方、状態ＦＢゲインＫｆ１〜３（３６）〜（３８）の決定方法は最適レギュレータ等様々あるが、速度指令ωｃｏｍ（２３）からモータ速度ωＭ（１１）までの伝達関数の極を指定する方式を図５の制御系で採用した場合は以下のようになる。
指定する極をβ１〜４とすると

ただし、

ところで、垂直多間接ロボットにおいては、ロボット自身の姿勢や、アームに取り付けられる負荷が変化すると、負荷イナーシャＪＬは変動する。そこで、負荷イナーシャＪＬを計算し、その値に基づいて、（数５）〜（数７）の式（６）〜（９）で示す状態オブザーバと状態ＦＢのパラメータを再計算すれば良いことが公知技術として知られている。

しかし、近年の高応答性を要求されるロボットにおいては、図５で示す位置ループ制御の全軸分の演算を数ミリ秒以内のサンプリングで行う必要があり、数値演算が速いデジタルシグナルプロセッサ（以後ＤＳＰと略す）を用いたとしても、負荷イナーシャＪＬの演算と（数５）〜（数７）の式（６）〜（９）の演算を位置制御ループサンプリングの余り時間で処理する事は困難である。また、（数５）〜（数７）の式（６）〜（９）にはＤＳＰが不得意とする除算が多く含まれている。

以上の問題に対し、従来は状態方程式の離散化を１次で終了させて演算時間を短縮する方式が示されている（例えば、特許文献１参照。）。

しかし、従来のオブザーバ制御演算装置では短縮したとは言え、ロボット自身の姿勢やアームに取り付ける負荷に変化が発生すると、依然として負荷イナーシャＪＬ、状態オブザーバ、状態ＦＢパラメータの演算が必要であり、また、ＤＳＰが不得意とする除算が含まれていることには変わりない。

最近のロボットにおいては、溶接や力制御のアプリケーションに適応した制御や、センサレス衝突検出に代表される安全性を高める制御等にも演算時間を割かねばならず、ますます演算処理の負荷が重くなってきている。

そこで、減速機のバネ成分によるロボット先端部の振動が、ロボットアームが伸長した姿勢で大きくなることに着目し、この姿勢における状態オブザーバと状態ＦＢのパラメータをアームに取り付けられる複数の負荷に対して予め準備し、その数に対応した複数の状態オブザーバ並びに状態ＦＢの演算を同時に行い、それぞれの状態ＦＢ値のモータ電流指令への加算量を調整する方法が示されている（例えば、特許文献２参照。）。

図７（ａ）は制御対象とする垂直多関節ロボットを示している。通常の垂直多関節ロボットは基本３軸と手首３軸で構成されており、図７（ａ）は基本３軸のみを示している。

図５等に示す、各軸の負荷速度ωＬ（１２）をアーム先端の線速度ｖＬに換算する式は以下の様になる。

ただし、
回転半径ｒ：各軸の回転中心軸とアーム先端までの距離
手首３軸はアーム長が基本３軸に比べ短いため、回転半径ｒが小さく、軸回りの速度ωＬに振動成分が乗っていても、アーム先端速度ｖＬへの影響は少ないので無視することができる。

図７（ａ）において、（１０１）は水平面で回転する第１軸、（１０２）は第１軸に取り付けられ垂直面で回転する第２軸、（１０３）は第２軸に取り付けられ垂直面で回転する第３軸である。（１０４）は第１軸の角速度ωＬ１、（１０５）は第２軸の角速度ωＬ２、（１０６）は第３軸の角速度ωＬ３である。

図７（ｂ）は（数８）の式（１０）が最大となる姿勢を示している。ただし、第３軸については回転中心軸とアーム先端までの距離は手首軸を無視すれば、姿勢によらず一定である。

（１０７）と（１０８）は、それぞれ第１軸と第２軸の回転半径ｒ１、ｒ２であり、第１軸と第２軸の回転中心がオフセットしていなければ一致し、図７（ｂ）の姿勢で最大となる。

また、この姿勢では、アームも含めた負荷イナーシャＪＬが最大となるので、振動周波数が最も低く、制御性能に悪影響を与える。さらに、図２で示す減速機（３）の減速機バネ（８）から先の負荷が大きくなるので、振動を始めると静止させにくくなる。

逆に図７（ｃ）（ｄ）はアームをおりたたんだ姿勢であり、図７（ｃ）に第１軸の回転半径ｒ１（１０７）、図７（ｄ）に第２軸の回転半径ｒ２（１０８）を示している。

いずれにしても、図７（ｂ）に比べると回転半径ｒが小さく、アームも含めた負荷イナーシャＪＬも小さくなるので、振動周波数が高くなり、振動の抑制も容易である。

つまり、図７（ｂ）で示す姿勢近傍が最も振動が大きく、止めにくいことが考えられるので、図７（ｂ）近傍で状態オブザーバと状態ＦＢのパラメータを予め調整しておく。

しかし、アームに取り付けられる負荷は一定ではなく、移載作業等ではロボットの動作中にも変わる。そこで、数種の負荷に対応したパラメータを図７（ｂ）近傍で、状態オブザーバと状態ＦＢのパラメータを予め調整しておく。

図８は、２種類の負荷に対応した場合の位置制御ループを示すものである。

図８においては２種類の負荷に対応した状態ＦＢブロック１、２（４７、５７）と状態ＦＢ比率ゲインＲＫ１、ＲＫ２（４８、５８）を有する。

状態ＦＢブロック１、２（４７、５７）においては、ＤＳＰが得意とする積和演算のみで構成される。

状態ＦＢブロック１、２（４７、５７）中の状態オブザーバと状態ＦＢのパラメータは、それぞれの負荷に対応し、図７（ｂ）の姿勢の近傍で予め調整された値を用いる。

ロボット動作プログラムにより、アームに取り付けられる負荷が、状態ＦＢブロック１（４７）で調整した負荷から状態ＦＢブロック２（５７）で調整した負荷へ切り替わったことが知らされると、図８の状態ＦＢ比率ゲインＲＫ１、ＲＫ２（４８、５８）は図９で示すグラフで変化させる。

状態ＦＢ比率ゲインＲＫ１、ＲＫ２（４８、５８）の関係は以下の式で定める。

図９では、時刻０ではＲＫ１＝１、ＲＫ２＝０であった状態ＦＢ比率ゲインは、時刻０．２で負荷切替が知らされると、ＲＫ１を０になるまで減少させる。ＲＫ２は（数９）の式（１１）の関係を保つので、時刻０．７でＲＫ２＝１に増加し、負荷切替対応を完了する。

この方法では、リアルタイムで負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算を行う必要はなく、ＤＳＰが得意とする積和演算で構成される複数の状態オブザーバ、状態ＦＢの演算をするのみで良いので、十分な振動抑制効果が得られる状態で、演算時間を削減することができる。
特開平８−１２３５０８号公報 特開２００６−１１６６３１号公報

しかし、従来例の２例目で示した方法では、状態オブザーバと状態ＦＢのパラメータは、図７（ｂ）で示すアーム伸長姿勢における負荷イナーシャをＪＬとして計算し、アーム姿勢には関係なく固定値を用いている。

アームをおりたたんだ姿勢の図７（ｃ）（ｄ）においては、回転半径ｒが小さく、アームも含めた負荷イナーシャＪＬも小さくなるので、振動周波数が高くなり、状態ＦＢによる振動抑制制御は元来不要であるにも関わらず、図７（ｂ）の伸長姿勢で調整された整合しないパラメータで状態ＦＢによる振動抑制制御が実施されることになる。

つまり、状態ＦＢによる振動抑制制御を実施しない場合より、パラメータ不一致に起因する振動が増加する可能性がある。

本発明は、上記課題を解決するものであり、ロボットアームが伸長した姿勢で大きくなる減速機バネ成分による振動の抑制と、アーム縮退時のパラメータ不一致に起因する振動増加の防止の両立を、現状の制御周期内の演算時間で可能とするロボットの制御方法の提供を目的とする。

上記課題を解決するために、本発明の多関節型ロボットの制御方法は、減速機を介してモータで駆動され回転軸を中心に回転するアームを複数有し、予め記憶された動作プログラムによって動作し、前記モータの電流および回転位置情報を基に前記モータのフィードバック制御ループを構成し、状態オブザーバで推定した前記モータに接続された前記アームの回転速度推定値と前記減速機のねじれ推定値および前記モータの回転速度を前記フィードバック制御ループに付加させる状態フィードバック値を出力する状態フィードバックブロックを備えた前記モータの動作を制御する多関節型ロボットの制御方法であって、前記多関節型ロボットの各姿勢における前記回転軸を中心としたアーム回転半径のアーム回転半径最大値に対する比の関数とする姿勢ゲインを乗じた状態フィードバック値をモータ電流指令から減算して新たなモータ電流指令とするものである。

また、本発明の多関節型ロボットの制御方法は、上記に加えて、姿勢ゲインは、回転軸を中心としたアーム回転半径のアーム回転半径最大値に対する２乗比の１次関数として決定されるものである。

また、本発明の多関節型ロボットの制御方法は、上記に加えて、姿勢ゲインは１以下であり、多関節型ロボットの各姿勢におけるアーム回転半径のアーム回転半径最大値に対する２乗比が０から第１の所定値までは０であり、前記第１の所定値から第２の所定値までは直線状に増加する一次関数であらわされ、前記第２の所定値から１までは１としたものである。

また、本発明の多関節型ロボットの制御方法は、上記に加えて、アームには負荷の装着と離脱が可能であり、前記アームへの負荷の装着と離脱も動作プログラムに含まれ、負荷に応じた状態フィードバックブロックを複数備え、前記動作プログラムによる動作中にアームへの負荷の装着または離脱が実行されて負荷変動が発生する負荷変動時に、負荷に応じて状態フィードバックブロックを切り替えるものである。

以上のように、本発明の多関節型ロボットの制御方法においては、ロボットアームが伸長した姿勢で大きくなる減速機バネ成分による振動の抑制と、アーム縮退時のパラメータ不一致に起因する振動増加の防止を実現できる。

（実施の形態１）
以下、本発明の実施の形態について、主に図１と図１０と図１１を用いて説明する。なお、上述した背景技術や発明が解決しようとする課題で説明した図２から図９と同様の箇所については同一の符号を付して詳細な説明を省略する。

図１は多関節型ロボットの制御方法における１つの軸についての位置制御ループを示す図である。背景技術で説明した図８と異なる主な点は、状態ＦＢ比率ゲインＲＫ１（４８）、ＲＫ２（５８）のそれぞれの出力に、姿勢ゲインＪ１＿ＫＬ１（７１）、Ｊ１＿ＫＬ２（７２）を乗じるようにした点である。

図１において、状態ＦＢ比率ゲインＲＫ１（４８）、ＲＫ２（５８）のそれぞれの出力に、姿勢ゲインＪ１＿ＫＬ１（７１）、Ｊ１＿ＫＬ２（７２）を乗じ、これらを速度制御ブロック３１より生成された電流指令ｉｃｏｍ０（３２）から減じて、新たに電流指令ｉｃｏｍ（１３）を生成する。

なお、姿勢ゲインＪ１＿ＫＬ１（７１）、Ｊ１＿ＫＬ２（７２）は、以下の手順で定める。

上述の背景技術で記した（数４）〜（数７）にわたって状態オブザーバ、状態ＦＢのパラメータを求めるが、ここで用いられる負荷イナーシャＪＬは、図７（ｂ）に示すような伸長姿勢の近傍の値を用いている。

一方、アームが縮退すると、負荷イナーシャＪＬが小さくなり、伸長姿勢で求めた状態オブザーバ、状態ＦＢのパラメータの誤差が大きくなるが、反面状態ＦＢによる振動抑制の必要性は低くなるので、負荷イナーシャＪＬが小さくなるにつれて状態ＦＢを効かせないようにすれば良い。

姿勢ゲインＪ１＿ＫＬ１（７１）、Ｊ１＿ＫＬ２（７２）は、アームに取り付けられる負荷に応じて状態オブザーバと状態ＦＢのパラメータ計算に用いられた負荷イナーシャＪＬの最大値ＪＬＭＡＸに対する各姿勢での負荷イナーシャＪＬのイナーシャ比の１次関数として定義する。

ただし、負荷イナーシャＪＬを動力学演算等で正確に求めようとすると、演算時間が長くなる。そこで、負荷イナーシャＪＬの大きさは一般的に質量あるいは回転中心からの距離の２乗に比例するので、アーム回転半径のアーム回転半径最大値に対する２乗比Ｐｘ（ｘ＝１，２，３）を求め、イナーシャ比の代用とする。

図１０はイナーシャ比を求める概念を示すための図であり、６軸の多関節ロボットにおける基本３軸と手首３軸のうちの主に基本３軸を示している。図１０において、第１軸は第１アーム（１）が回動する回転軸、第２軸は第２アーム（９）が回動する回転軸、第３軸は第３アーム（７３）が回動する回転軸である。そして、Ｌ１は第１軸から第２軸までの水平距離であり、Ｌ２は第２軸から第３軸までの距離であり、Ｌ３は第３軸から第３アーム（７３）の先端までの距離である。また、Ｒ１は多関節ロボットがある姿勢をとっている時の第１軸から第３アーム（７３）の先端までの水平距離、すなわち、第１軸の回転半径であり、Ｒ１の最大値であるＲ１ＭＡＸはＬ１＋Ｌ２＋Ｌ３となる。また、Ｒ２は第２軸から第３アーム（７３）の先端までの距離、すなわち、第２軸の回転半径であり、Ｒ２の最大値であるＲ２ＭＡＸはＬ２＋Ｌ３となる。また、Ｒ３は第３軸から第３アーム（７３）の先端までの距離、すなわち、第３軸の回転半径であり、Ｒ３の最大値であるＲ３ＭＡＸはＬ３となる。各軸において図１０に基づいてアーム回転半径のアーム回転半径最大値に対する２乗比Ｐｘ（ｘ＝１，２，３）をあらわすと（数１０）のようになる。

図１１は、第１軸における回転半径２乗比Ｐ１と姿勢ゲインＪ１＿ＫＬ１（７１）の１次関数の関係を示した例であり、数式で表すと以下の様になる。

ただし、

上記（数１１）の（式１３）を用いて姿勢ゲインＪ１＿ＫＬ１（７１）を計算し、状態オブザーバによるフィードバックに乗じる。

なお、姿勢ゲインＪ１＿ＫＬ２（７２）も姿勢ゲインＪ１＿ＫＬ１（７１）と同様であり、下記の（数１２）のように表される。

ただし、

また、第２軸および第３軸の姿勢ゲインについても（数１１）や（数１２）で表した第１軸と同様であり、第２軸については下記の（数１３）と（数１４）のように表され、第３軸については下記の（数１５）と（数１６）のように表される。

ただし、

ただし、

ただし、

ただし、

ここで、上記した、ａ１１，ｂ１１，ｃ１１，ａ２１，ｂ２１，ｃ２１と、ａ２１，ｂ２１，ｃ２１，ａ２２，ｂ２２，ｃ２２と、ａ３１，ｂ３１，ｃ３１，ａ３２，ｂ３２，ｃ３２とは、例えば実験等により予め求めておくことができる。

また、多関節型ロボットの各姿勢におけるＲ１やＲ２は、多関節型ロボットの動作プログラムから求めることができ、あるいは、第１アーム（１），第２アーム（９），第３アーム（７３）の形状寸法情報は予めわかっているので、これらアームの形状寸法情報と図１０に示す角度情報θ２やθ３とから求めることができる。

上記の構成をとることにより、振動抑制の必要がないアーム縮退時には、姿勢ゲインＫＬｘが０となり、状態オブザーバのフィードバックは無効となる。そして、速度制御ブロックより生成された電流指令ｉｃｏｍ０（３２）がそのまま電流指令ｉｃｏｍ（１３）として用いられる。その結果、パラメータ誤差による悪影響を防止できる。

ところで、回転半径が小さく振動への影響が少ない手首３軸の動作を無視した場合、第３軸の回転半径Ｒ３は姿勢によらず一定で、イナーシャ変動は無く、回転半径２乗比Ｐ３も一定（＝１）である。アーム縮体姿勢で第２軸の回転半径２乗比Ｐ２が０に近づいた場合でもＰ３＝１であり、つまり第２軸の姿勢ゲインＪ２＿ＫＬ１が０に近づいた場合でも第３軸の姿勢ゲインＪ３＿ＫＬ１＝１であり、状態ＦＢ量に大きな差が発生する。

第３軸の回転中心は第２アーム（９）の先端にあり、第２アーム（９）が動作すると第３軸の回転中心が移動する従属関係にあるので、出来るだけ制御系の応答は一致させたい。図１２は、ロボット設置面に水平に置かれた直方体のワーク（１１１）を、ロボット先端に装着した溶接トーチ（１１０）で、図１２（ａ）の姿勢から図１２（ｂ）の姿勢へロボットが縮対する方向（１１４）に等速で溶接する例を示している。この時、第２軸（１０２）は時計方向（１１２）に回転し、第３軸（１０３）は反時計方向（１１３）に回転する。第２軸（１０２）の動作により、第３軸（１０３）の回転中心はアーム先端動作方向（１１４）へ動かされると同時に上方へ動作させられる。溶接トーチ（１１０）をワーク（１１１）上で並行に等速動作させるためには、第２軸（１０２）と第３軸（１０３）を同期して制御する必要がある。しかし、第２軸（１０２）と第３軸（１０３）の角度指令θｃｏｍが上記軌跡を描くように正しく出力されたとしても、位置制御ループの応答性が軸毎で異なると実際の位置であるθＭの追従遅れにも差が生じ、軌跡にズレが生じることとなる。例えば第２軸（１０２）の応答性が第３軸（１０３）より速ければ、第２軸（１０２）の回転動作はアーム全体を持ち上げる方向（第３軸を持ち上げる方向）へより速く動作させるので、溶接トーチ（１１０）先端は上向き（１１５）に軌跡がずれることになる。

上記の、軸間の応答差による軌跡ズレを抑制するために、出来るだけ軸間の応答性を一致させる手段の一例として、（数７）の式（８）、（９）で速度指令ωｃｏｍ（２３）からモータ速度ωＭ（１１）までの伝達関数の極を指定する方式を背景技術で示したが、第２軸（１０２）の状態ＦＢ量だけが０に近づき、状態ＦＢが無い時の伝達関数に戻ると第３軸（１０３）との応答差が広がる可能性が有るので、第３軸（１０３）の姿勢ゲインとして第２軸のＪ２＿ＫＬ１を用いても良い。

以上のように、本実施の形態の関節型ロボットの制御方法によれば、減速機のバネ成分による振動が発生しやすい姿勢で、アームに取り付けられる複数の負荷に対応して予め調整されたパラメータを持つ複数の状態オブザーバ並びに状態ＦＢの演算を同時に行い、アームに取り付けられる負荷情報に基づいて、それぞれの状態ＦＢ値のモータ電流指令への加算量の調整に関し、各姿勢におけるアーム回転半径の最大値に対する２乗比の１次関数として定義する姿勢ゲインを乗じることを特徴とする。これにより、姿勢ゲインは伸長時には１、縮退時には０に近づくので、各姿勢で最適な状態ＦＢ量を得ることが出来、リアルタイムで負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算を行う必要はなく、ロボットの姿勢変化に対応した十分な振動抑制効果が得られる状態で、演算時間を削減することができる。

本発明の多関節型ロボットの制御方法は、従来のオブザーバ制御演算装置法が有していた、負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算に要する時間を、十分な振動抑制効果が得られる状態でさらに削減することができ、溶接や力制御のアプリケーションに適応した制御や、センサレス衝突検出に代表される安全性を高める制御等にも演算時間を割くことができ、産業上有用である。

本発明の実施の形態における位置制御ループを示すブロック図 ロボットの減速機のバネ成分を示す概略構成図 ロボットの減速機のバネ成分をモデル化したブロック図 １つ目の従来技術における位置制御ループを示すブロック図 １つ目の従来技術におけるオブザーバ制御を用いた位置制御ループを示すブロック図 状態オブザーバの構成を示すブロック図 ロボット姿勢とアーム先端振動の関係を示す概略図 ２つ目の従来技術における位置制御ループを示すブロック図 ２つ目の従来技術における状態ＦＢ比率ゲインの切替方法を示す図 本発明の実施の形態におけるアーム回転半径を示す概略図 本発明の実施の形態における回転半径２乗比Ｐ１と姿勢ゲインＪ１＿ＫＬ１の関係を示す図 第２軸と第３軸の応答差による軌跡ズレを説明するための概略図

符号の説明

１ 第１アーム（モータ取り付けベース）
２ モータ
３ 減速機
４ ベアリング
５ モータロータ
６ 減速機１次側
７ 減速機２次側
８ 減速機バネ
９ 第２アーム（負荷）
１０ モータ回転軸
１１ モータ回転速度ωＭ
１２ 負荷回転速度ωＬ
１３ 電流指令ｉｃｏｍ
１４ Ｋｔ（モータトルク定数）
１５ 減速比逆数（１／Ｒｇ）
１６ 減速比逆数（１／Ｒｇ）
１７ モータ伝達関数
１８ 負荷伝達関数
１９ バネ定数 Ｋｓ
２０ 減速機ねじれ角 θｓ
２１ 積分要素
２２ Ｔｄ（負荷（第２アーム）に加わる外力）
２３ モータ位置指令 θｃｏｍ
２４ モータ位置フィードバックθＭ
２５ 積分要素
２６ 位置比例ゲイン ＫＰＰ
２７ 位置制御ブロック
２８ モータ速度指令 ωｃｏｍ
２９ 速度比例ゲイン ＫＰ
３０ 速度積分ゲイン ＫＩ
３１ 速度制御ブロック
３２ モータ電流指令 ｉｃｏｍ０（速度制御ブロック出力）
３３ 状態オブザーバ
３４ 負荷速度推定値 ωＬＯ
３５ 減速機ねじれ角推定値 θｓｏ
３６ ゲイン（状態ＦＢゲイン１ Ｋｆ１）
３７ ゲイン（状態ＦＢゲイン２ Ｋｆ２）
３８ ゲイン（状態ＦＢゲイン３ Ｋｆ３）
３９ 状態ＦＢブロック
４０ 状態ＦＢ量 ＳＦＢ
４１ 状態オブザーバブロック１
４２ モータ回転速度推定値 ωＭＯ １
４３ 減速機ねじれ角推定値 θｓｏ１
４４ 状態ＦＢゲイン１ Ｋｆ１１
４５ 状態ＦＢゲイン２ Ｋｆ２１
４６ 状態ＦＢゲイン３ Ｋｆ３１
４７ 状態ＦＢブロック１
４８ 状態ＦＢ比率ゲインＲＫ１
４９ 状態ＦＢ量 ＳＦＢ１
５１ 状態オブザーバブロック２
５２ モータ回転速度推定値 ωＭＯ ２
５３ 減速機ねじれ角推定値 θｓｏ２
５４ 状態ＦＢゲイン１ Ｋｆ１２
５５ 状態ＦＢゲイン２ Ｋｆ２２
５６ 状態ＦＢゲイン３ Ｋｆ３２
５７ 状態ＦＢブロック２
５８ 状態ＦＢ比率ゲインＲＫ２
５９ 状態ＦＢ量 ＳＦＢ２
６１〜７０ 状態オブザーバパラメータ
７１ Ｊ１＿ＫＬ１（姿勢ゲイン）
７２ Ｊ１＿ＫＬ２（姿勢ゲイン）
７３ 第３アーム

Claims (4)

1. 減速機を介してモータで駆動され回転軸を中心に回転するアームを複数有し、予め記憶された動作プログラムによって動作し、前記モータの電流および回転位置情報を基に前記モータのフィードバック制御ループを構成し、状態オブザーバで推定した前記モータに接続された前記アームの回転速度推定値と前記減速機のねじれ推定値および前記モータの回転速度を前記フィードバック制御ループに付加させる状態フィードバック値を出力する状態フィードバックブロックを備えた前記モータの動作を制御する多関節型ロボットの制御方法であって、
   前記多関節型ロボットの各姿勢における前記回転軸を中心としたアーム回転半径のアーム回転半径最大値に対する比の関数とする姿勢ゲインを乗じた状態フィードバック値をモータ電流指令から減算して新たなモータ電流指令とする多関節型ロボットの制御方法。
2. 姿勢ゲインは、回転軸を中心としたアーム回転半径のアーム回転半径最大値に対する２乗比の１次関数として決定される請求項１記載の多関節型ロボットの制御方法。
3. 姿勢ゲインは１以下であり、多関節型ロボットの各姿勢におけるアーム回転半径のアーム回転半径最大値に対する２乗比が０から第１の所定値までは０であり、前記第１の所定値から第２の所定値までは直線状に増加する一次関数であらわされ、前記第２の所定値から１までは１である請求項２記載の多関節型ロボットの制御方法。
4. アームには負荷の装着と離脱が可能であり、前記アームへの負荷の装着と離脱も動作プログラムに含まれ、負荷に応じた状態フィードバックブロックを複数備え、前記動作プログラムによる動作中に前記アームへの負荷の装着または離脱が実行されて負荷変動が発生する負荷変動時に、負荷に応じて状態フィードバックブロックを切り替える請求項１から３のいずれか１項に記載の多関節型ロボットの制御方法。

Images