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

Description

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

近年、溶接やハンドリングに用いられる垂直多関節ロボットにおいて、作業精度の向上が求められている。しかし、垂直多関節ロボットは、合成アーム長が１ｍ以上、構成軸が６軸以上の構成が一般的であり、アームのしなりや、各軸に搭載されている減速機のバネ成分により、アーム先端は振動しやすい構造になっている。

特に、減速機のバネ成分による振動は、大型ロボットでは１０Ｈｚ以下の低い周波数となり、位置制御性能に大きな影響を与える。

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

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

しかし、減速機（３）は減速機１次側（６）と減速機２次側（７）の間にバネ成分が存在するので、（数１）の式が成立するのは、バネの伸びが一定となった定常状態のみである。

このバネ成分のバネ定数をＫｓとして、図２のモデルをブロック線図で表したものが図３である。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

指定する極をβ１〜４とすると

ところで、垂直多間接ロボットにおいては、ロボット自身の姿勢や、アームに取り付けられる負荷が変化すると、負荷イナーシャＪ_Ｌは変動する。

そこで、負荷イナーシャＪ_Ｌを計算し、その値に基づいて、（数５）〜（数７）の式（６）〜（９）で示す状態オブザーバと状態ＦＢのパラメータを再計算すれば良いことが公知技術として知られている。

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

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

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

最近のロボットにおいては、溶接や力制御のアプリケーションに適応した制御や、センサレス衝突検出に代表される安全性を高める制御等にも演算時間を割かねばならず、ますます演算処理の負荷が重くなってきている。
本発明は、上記課題を解決するものであり、従来のオブザーバ制御演算装置法が有していた、負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算に要する時間を、十分な振動抑制効果が得られる状態で、削減することができるロボットの制御方法の提供を目的とする。

上記目的を達成するために本発明の多関節型ロボットの制御方法は、減速機のバネ成分による振動が発生しやすい姿勢で、アームに取り付けられる複数の負荷に対応して予め調整されたパラメータを持つ複数の状態オブザーバ並びに状態ＦＢの演算を同時に行い、アームに取り付けられる負荷情報に基づいて、それぞれの状態ＦＢ値のモータ電流指令への加算量を調整する手段を備えることを特徴とする。

以上のように、本発明の多関節型ロボットの制御方法においては、リアルタイムで負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算を行う必要はなく、ＤＳＰが得意とする積和演算で構成される複数の状態オブザーバ、状態ＦＢの演算をするのみで良いので、十分な振動抑制効果が得られる状態で、演算時間を削減することができる。

（実施の形態）
以下、本発明の実施の形態について、図１〜３及び図７〜８を用いて説明する。

図７（ａ）は本発明の実施の形態での制御対象とする垂直多関節ロボットを示している。

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

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

手首３軸はアーム長が基本３軸に比べ短いため、回転半径ｒが小さく、軸回りの速度ω_Ｌに振動成分が乗っていても、アーム先端速度ｖ_Ｌへの影響は少ないので無視することができる。
図７（ａ）において、（１０１）は水平面で回転する第１軸、（１０２）は第１軸に取り付けられ垂直面で回転する第２軸、（１０３）は第２軸に取り付けられ垂直面で回転する第３軸である。（１０４）は第１軸の角速度ω_Ｌ１、（１０５）は第２軸の角速度ω_Ｌ２、（１０６）は第３軸の角速度ω_Ｌ３である。
図７（ｂ）は（数８）の式（１０）が最大となる姿勢を示している。ただし、第３軸は回転中心軸とアーム先端までの距離は手首軸を無視すれば、姿勢によらず一定である。

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

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

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

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

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

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

以下の説明では、説明を簡単にするために２種類の負荷に対応した場合を用いる。

図１は本発明の実施の形態における位置制御ループを示すものである。

図５の従来の位置制御ループにおいて状態ＦＢブロック（３９）が単一だったことに対し、図１においては２種類の負荷に対応した状態ＦＢブロック１、２（４７、５７）と状態ＦＢ比率ゲインＲＫ１、ＲＫ２（４８、５８）を有する。

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

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

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

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

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

もし、図８で示す切替シーケンスを行わずに、時刻０．２でＲＫ１＝０、ＲＫ２＝１として切り替えると、状態ＦＢ量が不連続となり、電流指令ｉｃｏｍ（１３）も不連続となって、アームに振動を発生させる可能性がある。

その理由は、以下の通りである。

状態オブザーバ１（４１）と状態オブザーバ２（５１）はそれぞれの負荷に対応して負荷速度ω_ＬＯ１、ω_ＬＯ２（４２、５２）、減速機ねじれ角θ_ＬＯ１、θ_ＬＯ２（４３、５３）を演算しており、状態ＦＢゲインＫｆ１１〜Ｋｆ３１（４４〜４６）とＫｆ１２〜Ｋｆ３２（５４〜５６）も異なるので、それぞれの状態ＦＢ量ＳＦＢ１、ＳＦＢ２（４９、５９）も異なる。

つまり、時刻０．２で時間経過無しに状態ＦＢ比率ゲインＲＫ１、ＲＫ２（４８、５８）を切り替えると、状態ＦＢ量が不連続となり、電流指令ｉｃｏｍ（１３）も不連続となってしまう。

一方、状態オブザーバは（数６）の式（７）で指定した極α１、α２で決定される推測の収束時定数を持つ。図５の状態ＦＢブロック（３９）が単一である従来の位置制御ループにおいて、状態オブザーバ（３３）内のパラメータを不連続に切り替えると、収束遅れの影響で負荷速度ω_ＬＯ（３４）、減速機ねじれ角θ_ＬＯ（３５）に乱れが生じる。

また、状態ＦＢゲインＫｆ１〜Ｋｆ３（３６〜３８）も不連続となるので、状態ＦＢ量ＳＦＢ（４０）も不連続となる。

つまり、図５の状態ＦＢブロック（３９）が単一である従来の位置制御ループにおいては、状態ＦＢブロック３９内のパラメータを連続的に変化させる必要があり、従来のオブザーバ制御演算装置の様に、負荷イナーシャＪ_Ｌ、状態オブザーバ、状態ＦＢのパラメータ演算を常に行う必要がある。
本発明の説明では、２種類のアームに取り付けられる負荷に対応した場合を説明したが、３種類以上でも同様の手法を用いることが出来るのは言うまでもない。

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

本発明の実施の形態における位置制御ループを示すブロック図 ロボットの減速機のバネ成分を示す概略構成図 ロボットの減速機のバネ成分をモデル化したブロック図 従来技術の形態における位置制御ループを示すブロック図 従来技術の形態におけるオブザーバ制御を用いた位置制御ループを示すブロック図 状態オブザーバの構成を示すブロック図 ロボット姿勢とアーム先端振動の関係を示す概略図 本発明の実施の形態における状態ＦＢ比率ゲインの切替方法を示すグラフ

符号の説明

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

Claims (2)

1. モータで駆動する回転軸を中心に回転し、負荷の装着と離脱が可能なアームを複数有し、アームへの負荷の装着と離脱の動作を含む予め記憶された動作プログラムによって動作し、前記モータの電流、および回転位置情報を元に前記モータのフィードバック制御ループをソフトウエアで構成し、モータ回転速度および状態オブザーバで推定した前記モータに接続する前記アームの速度およびねじれ量の推定量から得られる状態フィードバック量を前記フィードバック制御ループの電流指令から減じるために出力する状態フィードバックブロックをソフトウエア上に備えた前記モータの動作を制御する多関節型ロボットの制御方法であって、負荷イナーシャが最大となる姿勢でアームに装着または離脱される複数の負荷毎に予め演算した負荷イナーシャに応じて状態オブザーバのパラメータおよび状態フィードバックのパラメータを予め演算した状態フィードバックブロックをソフトウエア上に複数個備え、前記プログラムによる動作中に前記アームへの負荷の装着または離脱を記述したプログラム命令が実行された時に、前記フィードバック制御ループの電流指令から減じるための状態フィードバック量が、前記プログラム命令が実行された負荷に応じた状態フィードバックブロックからの出力となるように状態フィードバックブロックの出力を切り替えることを特徴とする多関節型ロボットの制御方法。
2. 複数個の状態フィードバックブロックから各々出力される状態フィードバック量各々に０以上１以下の範囲で変更可能であり加算すれば１となる乗算係数を乗算して、フィードバック制御ループの電流指令から減じるための状態フィードバック量を決定することにより、状態フィードバックブロックの出力を切り替える時に前記状態フィードバック量が不連続となることを防止する請求項１記載の多関節型ロボットの制御方法。

Images