(57)【要約】  【課題】  従来のオブザーバ制御演算装置では、ロボット自身の姿勢や、アームに取り付ける負荷に変化に対応し、制御性能の劣化を防ごうとすると、負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータのリアルタイムでの演算が必要であり、演算処理の負荷が非常に多くなるという課題を有していた。【解決手段】  アームに取り付けられる複数の負荷に対応して予め調整されたパラメータを持つ複数の状態オブザーバ並びに状態ＦＢの演算を同時に行い、アームに取り付けられる負荷情報に基づいて、それぞれの状態ＦＢ値のモータ電流指令への加算量を調整する手段を備えることにより、リアルタイムで負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算を行う必要はなく、十分な振動抑制効果が得られる状態で、演算時間を削減することができる。【選択図】  図１
2006116631.tif 000002
【特許請求の範囲】
【請求項１】
モータで駆動する回転軸を中心に回転し、負荷の装着と離脱が可能なアームを複数有し、アームへの負荷の装着と離脱の動作を含む予め記憶された動作プログラムによって動作し、前記モータの電流、および回転位置情報を元に前記モータのフィードバック制御ループを構成し、状態オブザーバで推定した前記モータに接続する前記アームの速度およびねじれ量の推定量を前記フィードバック制御ループに付加させる状態フィードバックブロックを負荷に応じて複数個備えた前記モータの動作を制御する多関節型ロボットの制御方法であって、前記プログラムによる動作中に前記アームへの負荷の装着または離脱が実行されて負荷変動が発生する負荷変動発生時に、負荷に応じて状態フィードバックブロックを切り替えることを特徴とする多関節型ロボットの制御方法。
【請求項２】
複数個の状態フィードバックブロックから得られた状態フィードバック量に変更可能な乗算係数を乗算し、前記モータのフィードバック制御ループに付加させる状態フィードバック量を決定する請求項１記載の多関節型ロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、減速機を介してモータにより駆動されるロボットの位置制御における多関節型ロボットの制御方法に関するものである。
【背景技術】
【０００２】
近年、溶接やハンドリングに用いられる垂直多関節ロボットにおいて、作業精度の向上が求められている。しかし、垂直多関節ロボットは、合成アーム長が１ｍ以上、構成軸が６軸以上の構成が一般的であり、アームのしなりや、各軸に搭載されている減速機のバネ成分により、アーム先端は振動しやすい構造になっている。
【０００３】
特に、減速機のバネ成分による振動は、大型ロボットでは１０Ｈｚ以下の低い周波数となり、位置制御性能に大きな影響を与える。
【０００４】
図２は、ロボットにおけるモータと減速機をモデル化したものを示してもので、モータ取り付けベースとなるアーム１（１）にモータ（２）、減速機（３）、ベアリング（４）が固定され、減速機２次側（７）の回転部に結合された負荷であるアーム２（９）を駆動する。
【０００５】
減速機１次側（６）はモータ回転軸（１０）でモータ内のロータに結合され、モータ回転速度ωＭ（１１）で回転する。減速機（３）は減速比Ｒｇで、モータ回転速度ωＭ（１１）を負荷回転速度ωＬ（１２）に減速する。
【０００６】
【数１】
2006116631.tif 000003
【０００７】
しかし、減速機（３）は減速機１次側（６）と減速機２次側（７）の間にバネ成分が存在するので、（数１）の式が成立するのは、バネの伸びが一定となった定常状態のみである。
【０００８】
このバネ成分のバネ定数をＫｓとして、図２のモデルをブロック線図で表したものが図３である。
【０００９】
図３において、ｉｃｏｍ（１３）はモータ（２）を駆動するモータ電流指令、Ｋｔ（１４）はモータ（２）のトルク定数、１／Ｒｇ（１５、１６）は減速比の逆数、（１７）はモータ伝達関数、（１８）は負荷伝達関数、Ｋｓ（１９）は減速機（３）のバネ定数、θｓ（２０）は減速機１次側（６）と減速機２次側（７）間に発生するねじれ角、（２１）は積分、Ｔｄ（２２）は負荷（アーム２）に加わる外力である。
【００１０】
モータ伝達関数（１７）において、ＪＭはモータロータ（５）と減速機１次側（６）を合わせた回転軸（１０）回りの慣性モーメント、ＤＭは粘性摩擦係数である。
【００１１】
負荷伝達関数（１８）においても、ＪＬは負荷（アーム２）（９）と減速機２次側（７）を合わせた回転軸（１０）回りの慣性モーメント、ＤＬは粘性摩擦係数である。
【００１２】
図３の負荷に対し、通常用いられる位置決め制御ループを示したものが図４である。
【００１３】
図４において、位置制御ブロック（２７）は、モータに接続されたエンコーダ等によって検出されるモータ回転速度ωＭを積分要素（２５）で積分したモータ位置θＭ（２４）を、位置指令θｃｏｍ（２３）から減算し、位置ゲインＫＰＰ（２６）を乗じて速度指令ωｃｏｍ（２８）を生成する。
【００１４】
【数２】
2006116631.tif 000004
【００１５】
速度制御ブロック（３１）は、速度指令ωｃｏｍ（２８）とモータ回転速度ωＭより下記式で、モータ電流指令ｉｃｏｍ０（３２）を生成する。
【００１６】
【数３】
2006116631.tif 000005
【００１７】
図４で示す通常の位置制御ループでは、負荷速度ωＬ（１２）や減速機ねじれ角θｓ（２０）を無視し、モータ速度ωＭ（１１）のみを観測して位置制御を行うため、減速機のバネ成分による負荷速度ωＬ（１２）の振動を抑制することは出来なかった。
【００１８】
そこで、負荷速度ωＬ（１２）や減速機ねじれ角θｓ（２０）を計測し、その計測値を用いてフィードバック（以下ＦＢと省略）制御を行うことで、減速機のバネ成分による振動を抑制する方式が考えられるが、負荷速度ωＬ（１２）や減速機ねじれ角θｓ（２０）の計測機能を付加することにより、装置コストや負荷質量の増大を招くので、製品化するにはデメリットが大きい。
【００１９】
上記課題を解決するために、図５では図４で示した通常の位置制御ループに、状態ＦＢブロック（３９）を加えている。
【００２０】
状態ＦＢブロックにおいては、電流指令ｉｃｏｍ（１３）とモータ速度ωＭ（１１）の値を用いて、状態オブザーバ（３３）により、負荷速度推定値ωＬｏ（３４）や減速機ねじれ角推定値θｓｏ（３５）を推定する。これらの推定値とモータ回転速度ωＭ（１１）にゲイン（３６）〜（３８）を乗じ、速度制御ブロックより生成された電流指令ｉｃｏｍ０（３２）から減じて、新たに電流指令ｉｃｏｍ（１３）を生成する。
【００２１】
【数４】
2006116631.tif 000006
【００２２】
このように構成することにより、状態ＦＢゲイン（３６）〜（３８）を調整することで、減速機のバネ成分による負荷速度ωＬ（１２）振動を抑制する制御特性の実現が可能になる。
【００２３】
図６は図５における状態オブザーバ（３３）を最小次元オブザーバで構成した場合の内部構造を示したもので、パラメータ６１〜７０を以下の式で計算することにより構成する。
【００２４】
【数５】
2006116631.tif 000007
【００２５】
において、ｋ１、ｋ２（図６では６９、７０）は、オブザーバの応答性と安定性を決定するゲインで、オブザーバの応答性と安定性を決定する極をα１、α２と指定すると以下の式で計算される。
【００２６】
【数６】
2006116631.tif 000008
【００２７】
一方、状態ＦＢゲインＫｆ１〜３（３６）〜（３８）の決定方法は最適レギュレータ等様々あるが、速度指令ωｃｏｍ（２３）からモータ速度ωＭ（１１）までの伝達関数の極を指定する方式を図５の制御系で採用した場合は以下のようになる。
【００２８】
指定する極をβ１〜４とすると
【００２９】
【数７】
2006116631.tif 000009
【００３０】
ところで、垂直多間接ロボットにおいては、ロボット自身の姿勢や、アームに取り付けられる負荷が変化すると、負荷イナーシャＪＬは変動する。
【００３１】
そこで、負荷イナーシャＪＬを計算し、その値に基づいて、（数５）〜（数７）の式（６）〜（９）で示す状態オブザーバと状態ＦＢのパラメータを再計算すれば良いことが公知技術として知られている。
【００３２】
しかし、近年の高応答性を要求されるロボットにおいては、図５で示す位置ループ制御の全軸分の演算を数ミリ秒以内のサンプリングで行う必要があり、数値演算が速いデジタルシグナルプロセッサ（以後ＤＳＰと略す）を用いたとしても、負荷イナーシャＪＬの演算と（数５）〜（数７）の式（６）〜（９）の演算を位置制御ループサンプリングの余り時間で処理する事は困難である。また、（数５）〜（数７）の式（６）〜（９）にはＤＳＰが不得意とする除算が多く含まれている。
【００３３】
以上の問題に対し、従来は状態方程式の離散化を１次で終了させて演算時間を短縮する方式が示されている（例えば特許文献１参照）。
【特許文献１】特開平８−１２３５０８号公報
【発明の開示】
【発明が解決しようとする課題】
【００３４】
しかし、従来のオブザーバ制御演算装置では短縮したとは言え、ロボット自身の姿勢や、アームに取り付ける負荷に変化が発生すると、依然として負荷イナーシャＪＬ、状態オブザーバ、状態ＦＢパラメータの演算が必要であり、また、ＤＳＰが不得意とする除算が含まれていることには変わりない。
【００３５】
最近のロボットにおいては、溶接や力制御のアプリケーションに適応した制御や、センサレス衝突検出に代表される安全性を高める制御等にも演算時間を割かねばならず、ますます演算処理の負荷が重くなってきている。
本発明は、上記課題を解決するものであり、従来のオブザーバ制御演算装置法が有していた、負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算に要する時間を、十分な振動抑制効果が得られる状態で、削減することができるロボットの制御方法の提供を目的とする。
【課題を解決するための手段】
【００３６】
上記目的を達成するために本発明の多関節型ロボットの制御方法は、減速機のバネ成分による振動が発生しやすい姿勢で、アームに取り付けられる複数の負荷に対応して予め調整されたパラメータを持つ複数の状態オブザーバ並びに状態ＦＢの演算を同時に行い、アームに取り付けられる負荷情報に基づいて、それぞれの状態ＦＢ値のモータ電流指令への加算量を調整する手段を備えることを特徴とする。
【発明の効果】
【００３７】
以上のように、本発明の多関節型ロボットの制御方法においては、リアルタイムで負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算を行う必要はなく、ＤＳＰが得意とする積和演算で構成される複数の状態オブザーバ、状態ＦＢの演算をするのみで良いので、十分な振動抑制効果が得られる状態で、演算時間を削減することができる。
【発明を実施するための最良の形態】
【００３８】
（実施の形態）
以下、本発明の実施の形態について、図１〜３及び図７〜８を用いて説明する。
【００３９】
図７（ａ）は本発明の実施の形態での制御対象とする垂直多関節ロボットを示している。
【００４０】
通常の垂直多関節ロボットは基本３軸と手首３軸で構成されており、図７（ａ）は基本３軸のみを示している。
【００４１】
図５等に示す、角軸の負荷速度ωＬ（１２）をアーム先端の線速度ｖＬに換算する式は以下の様になる。
【００４２】
【数８】
2006116631.tif 000010
【００４３】
手首３軸はアーム長が基本３軸に比べ短いため、回転半径ｒが小さく、軸回りの速度ωＬに振動成分が乗っていても、アーム先端速度ｖＬへの影響は少ないので無視することができる。
図７（ａ）において、（１０１）は水平面で回転する第１軸、（１０２）は第１軸に取り付けられ垂直面で回転する第２軸、（１０３）は第２軸に取り付けられ垂直面で回転する第３軸である。（１０４）は第１軸の角速度ωＬ１、（１０５）は第２軸の角速度ωＬ２、（１０６）は第３軸の角速度ωＬ３である。
図７（ｂ）は（数８）の式（１０）が最大となる姿勢を示している。ただし、第３軸は回転中心軸とアーム先端までの距離は手首軸を無視すれば、姿勢によらず一定である。
【００４４】
（１０７）と（１０８）は、それぞれ第１軸と第２軸の回転半径ｒ１、ｒ２であり、第１軸と第２軸の回転中心がオフセットしていなければ一致し、図７（ｂ）の姿勢で最大となる。
【００４５】
また、この姿勢では、アームも含めた負荷イナーシャＪＬが最大となるので、振動周波数が最も低く、制御性能に悪影響を与える。さらに、図２で示す減速機（３）のバネ（８）から先の負荷が大きくなるので、振動が始めると静止させにくくなる。
【００４６】
逆に図７（ｃ）（ｄ）はアームをおりたたんだ姿勢であり、図７（ｃ）に第１軸の回転半径ｒ１（１０７）、図７（ｄ）に第２軸の回転半径ｒ２（１０８）を示している。
【００４７】
いずれにしても、図７（ｂ）に比べると回転半径ｒが小さく、アームも含めた負荷イナーシャＪＬも小さくなるので、振動周波数が高くなり、振動の抑制も容易である。
【００４８】
つまり、図７（ｂ）で示す姿勢近傍が最も振動が大きく、止めにくいことが考えられるので、図７（ｂ）近傍で状態オブザーバと状態ＦＢのパラメータを予め調整しておく。
【００４９】
しかし、アームに取り付けられる負荷は一定ではなく、移載作業等ではロボットが動作中にも変わる。そこで、数種の負荷に対応したパラメータを図７（ｂ）近傍で状態オブザーバと状態ＦＢのパラメータを予め調整しておく。
【００５０】
以下の説明では、説明を簡単にするために２種類の負荷に対応した場合を用いる。
【００５１】
図１は本発明の実施の形態における位置制御ループを示すものである。
【００５２】
図５の従来の位置制御ループにおいて状態ＦＢブロック（３９）が単一だったことに対し、図１においては２種類の負荷に対応した状態ＦＢブロック１、２（４７、５７）と状態ＦＢ比率ゲインＲＫ１、ＲＫ２（４８、５８）を有する。
【００５３】
状態ＦＢブロック１、２（４７、５７）においては、ＤＳＰが得意とする積和演算のみで構成される。
【００５４】
状態ＦＢブロック１、２（４７、５７）中の状態オブザーバと状態ＦＢのパラメータは、それぞれの負荷に対応し、図７（ｂ）の姿勢の近傍で予め調整された値を用いる。
【００５５】
ロボット動作プログラムにより、アームに取り付けられる負荷が、状態ＦＢブロック１（４７）で調整した負荷から状態ＦＢブロック２（５７）で調整した負荷へ切り替わったことが知らされると、図１の状態ＦＢ比率ゲインＲＫ１、ＲＫ２（４８、５８）は図８で示すグラフで変化させる。
【００５６】
状態ＦＢ比率ゲインＲＫ１、ＲＫ２（４８、５８）の関係は以下の式で定める。
【００５７】
【数９】
2006116631.tif 000011
【００５８】
図８では、時刻０ではＲＫ１＝１、ＲＫ２＝０であった状態ＦＢ比率ゲインは、時刻０．２で負荷切替が知らされると、ＲＫ１を０になるまで減少させる。ＲＫ２は（数９）の式（１１）の関係を保つので、時刻０．７でＲＫ２＝１に増加し、負荷切替対応を完了する。
【００５９】
もし、図８で示す切替シーケンスを行わずに、時刻０．２でＲＫ１＝０、ＲＫ２＝１として切り替えると、状態ＦＢ量が不連続となり、電流指令ｉｃｏｍ（１３）も不連続となって、アームに振動を発生させる可能性がある。
【００６０】
その理由は、以下の通りである。
【００６１】
状態オブザーバ１（４１）と状態オブザーバ２（５１）はそれぞれの負荷に対応して負荷速度ωＬＯ１、ωＬＯ２（４２、５２）、減速機ねじれ角θＬＯ１、θＬＯ２（４３、５３）を演算しており、状態ＦＢゲインＫｆ１１〜Ｋｆ３１（４４〜４６）とＫｆ１２〜Ｋｆ３２（５４〜５６）も異なるので、それぞれの状態ＦＢ量ＳＦＢ１、ＳＦＢ２（４９、５９）も異なる。
【００６２】
つまり、時刻０．２で時間経過無しに状態ＦＢ比率ゲインＲＫ１、ＲＫ２（４８、５８）を切り替えると、状態ＦＢ量が不連続となり、電流指令ｉｃｏｍ（１３）も不連続となってしまう。
【００６３】
一方、状態オブザーバは（数６）の式（７）で指定した極α１、α２で決定される推測の収束時定数を持つ。図５の状態ＦＢブロック（３９）が単一である従来の位置制御ループにおいて、状態オブザーバ（３３）内のパラメータを不連続に切り替えると、収束遅れの影響で負荷速度ωＬＯ（３４）、減速機ねじれ角θＬＯ（３５）に乱れが生じる。
【００６４】
また、状態ＦＢゲインＫｆ１〜Ｋｆ３（３６〜３８）も不連続となるので、状態ＦＢ量ＳＦＢ（４０）も不連続となる。
【００６５】
つまり、図５の状態ＦＢブロック（３９）が単一である従来の位置制御ループにおいては、状態ＦＢブロック３９内のパラメータを連続的に変化させる必要があり、従来のオブザーバ制御演算装置の様に、負荷イナーシャＪＬ、状態オブザーバ、状態ＦＢのパラメータ演算を常に行う必要がある。
本発明の説明では、２種類のアームに取り付けられる負荷に対応した場合を説明したが、３種類以上でも同様の手法を用いることが出来るのは言うまでもない。
【産業上の利用可能性】
【００６６】
本発明の多関節型ロボットの制御方法は、従来のオブザーバ制御演算装置法が有していた、負荷イナーシャ、状態オブザーバ、状態ＦＢのパラメータ演算に要する時間を、十分な振動抑制効果が得られる状態で、さらに削減することができ、溶接や力制御のアプリケーションに適応した制御や、センサレス衝突検出に代表される安全性を高める制御等にも演算時間を割くことができるので産業上有用である。
【図面の簡単な説明】
【００６７】
【図１】本発明の実施の形態における位置制御ループを示すブロック図
【図２】ロボットの減速機のバネ成分を示す概略構成図
【図３】ロボットの減速機のバネ成分をモデル化したブロック図
【図４】従来技術の形態における位置制御ループを示すブロック図
【図５】従来技術の形態におけるオブザーバ制御を用いた位置制御ループを示すブロック図
【図６】状態オブザーバの構成を示すブロック図
【図７】ロボット姿勢とアーム先端振動の関係を示す概略図
【図８】本発明の実施の形態における状態ＦＢ比率ゲインの切替方法を示すグラフ
【符号の説明】
【００６８】
１  モータ取り付けベース（アーム１）
２  モータ
３  減速機
４  ベアリング
５  モータ内ロータ
６  減速機１次側
７  減速機２次側
８  減速機バネ
９  負荷（アーム２）
１０  モータ回転軸
１１  モータ回転速度  ωＭ
１２  負荷回転速度  ωＬ
１３  モータ電流指令  ｉｃｏｍ
１４  モータトルク定数  Ｋｔ
１５  減速比逆数
１６  減速比逆数
１７  モータ伝達関数
１８  負荷伝達関数
１９  バネ定数  Ｋｓ
２０  減速機ねじれ角  θｓ
２１  積分要素
２２  負荷（アーム２）に加わる外力  Ｔｄ
２３  モータ位置指令  θｃｏｍ
２４  モータ位置ＦＢ  θＭ
２５  積分要素
２６  位置比例ゲイン  ＫＰＰ
２７  位置制御ブロック
２８  モータ速度指令  ωｃｏｍ
２９  速度比例ゲイン  ＫＰ
３０  速度積分ゲイン  ＫＩ
３１  速度ループブロック
３２  電流指令（速度ループブロック出力）  ｉｃｏｍ０
３３  状態オブザーバブロック
３４  モータ回転速度推定値  ωＭＯ
３５  減速機ねじれ角推定値  θｓｏ
３６  状態ＦＢゲイン１  Ｋｆ１
３７  状態ＦＢゲイン２  Ｋｆ２
３８  状態ＦＢゲイン３  Ｋｆ３
３９  状態ＦＢブロック
４０  状態ＦＢ量  ＳＦＢ
４１  状態オブザーバブロック１
４２  モータ回転速度推定値  ωＭＯ１
４３  減速機ねじれ角推定値  θｓｏ１
４４  状態ＦＢゲイン１  Ｋｆ１１
４５  状態ＦＢゲイン２  Ｋｆ２１
４６  状態ＦＢゲイン３  Ｋｆ３１
４７  状態ＦＢブロック１
４８  状態ＦＢ比率ゲイン１
４９  状態ＦＢ量  ＳＦＢ１
５１  状態オブザーバブロック２
５２  モータ回転速度推定値  ωＭＯ２
５３  減速機ねじれ角推定値  θｓｏ２
５４  状態ＦＢゲイン１  Ｋｆ１２
５５  状態ＦＢゲイン２  Ｋｆ２２
５６  状態ＦＢゲイン３  Ｋｆ３２
５７  状態ＦＢブロック２
５８  状態ＦＢ比率ゲイン２
５９  状態ＦＢ量  ＳＦＢ２
６１〜７０  状態オブザーバパラメータ
【図１】
2006116631.tif 000012
【図２】
2006116631.tif 000013
【図３】
2006116631.tif 000014
【図４】
2006116631.tif 000015
【図５】
2006116631.tif 000016
【図６】
2006116631.tif 000017
【図７】
2006116631.tif 000018
【図８】
2006116631.tif 000019