WO1998053962A1 - Procede et dispositif de commande de robot - Google Patents

Description

明 細 書

ロボット制御方法及び装置

技術分野

本発明は、 産業用ロボットなどの多軸ロボットを制御するロボット制御装置に 関し、 特に、 互いの軸間に作用する力学的な干渉により生じる振動を抑制できる ロボット制御方法及び装置に関する。

背景技術

一般に、 電動機により各軸が制御される産業用ロボットでは、 電動機の力不足 を補ったり負荷側からの外乱力の影響を極力小さくしたりするために、 減速比の 大きなハーモニックドライブ等のギアを介して、 負荷側のアームを駆動している。 減速比の大きなギアが介在するため、 従来は、 制御軸間の力学的干渉はそれほど 問題にされてこなかった。 しかしながら、 近時の高速化、 高精度化の要求の高ま りから、 P I (比例積分） 制御では補償しきれない力学的影響や、 高減速比駆動 であっても無視できない外乱が生じ、 問題となってきている。

各軸の電動機とロボットアームとの間にそれぞれ減速機などのパネ要素を有す る機構が設けられた多軸ロボッ卜において、 軸干渉による振動を抑制する方法と して、 本出願人は、 既に、 日本国特許公開平成 9年第 2 2 2 9 1 0号 UP. A, 09222910) に開示する発明を提案している。 日本国特許公開平成 9年第 2 2 2 9 1 0号公報に開示する発明では、 多軸ロボットの各軸に状態観測器を配置して電 動機と負荷の間のねじれ角度を推定し、 推定されたねじれ角度を用いて干渉力を 計算し、 その干渉力に基づいて補正トルクを求め、 電動機へのトルクに加算して 出力している。

しかしながら、 上述した日本国特許公開平成 9年第 2 2 2 9 1 0号公報に開示 される制御方法では、 状態観測器により推定されたねじれ角度の値を微分して補 正トルクを算出しており、 このため、 補正トルクにノイズが重畳しやすく、 電動 機が高周波振動を起こしたり、 動作時の音が大きくなるおそれがある。

発明の開示

本発明の目的は、 軸間の干渉による振動や電動機の高周波振動を起こすことな く、 ツール先端の軌跡精度を向上させることができるロボット制御方法を提供す る とにめる。

本発明の別の目的は、 軸間の干渉による振動や電動機の高周波振動を起こすこ となく、 ツール先端の軌跡精度を向上させることができるロボット制御装置を提 供することにある。

本発明の目的は、 各軸ごとに電動機とロボットアームとの間にパネ要素を有す る機構が設けられた多軸ロボットを制御対象とするロボッ ト制御方法であって、 制御対象と制御対象に対するフィ一ドバック制御系との擬似モデルを用意し、 擬 似モデルを用いて、 各電動機に対する位置指令を入力として、 各軸ごとに、 モデ ル電動機位置指令、 モデル電動機速度指令及びモデルフイードフォヮ一ド指令を 算出し、 擬似モデルを用いて、 軸間に作用する他軸からの干渉による干渉トルク を求め、 干渉トルクを相殺するモデル補正トルクを算出し、 モデルフィードフォ ーヮ一ド指令に対してモデル補正トルクを加算して最終的なモデル電動機加速度 指令とし、 モデル電動機位置指令、 モデル電動機速度指令及び最終的なモデル電 動機加速度指令に応じて各軸ごとのフィードバック制御を実行する、 ロボッ ト制 御方法によって達成される。

本発明の別の目的は、 各軸ごとに電動機とロボットアームとの間にパネ要素を 有する機構が設けられた多軸ロボッ トの制御に使用されるロボット制御装置であ つて、 各電動機に対する位置指令を入力として、 各軸ごとに、 モデル電動機位置 指令、 モデル電動機速度指令及びモデルフイードフォヮ一ド指令を出力するモデ ル制御器と、 モデル制御器から出力される各指令に基づき、 各電動機及び各ロボ ットアームを駆動、 制御するフィードバック制御器と、 を備え、 モデル制御器内 に、 軸間に作用する他軸からの干渉による干渉トルクを求め、 干渉トルクを相殺 するモデル;！正トルクを算出する補正量算出部が設けられ、 モデル補正トルクを 加算されたモデルフィ一ドフォヮ一ド指令がモデル制御器から出力するロボット 制御装置によって達成される。

本発明においては、 補正量を算出する際に、 干渉トルクは、 モデル電動機とモ デルロボットアームとの位置偏差であるモデルねじり角により算出することがで ぎる。

本発明では、 ロボッ 卜のある軸が他軸から受ける干渉力を干渉トルクとして求 めてモデル制御器で補正し、 この補正量 （補正トルク） を基に規範となる各状態 量をフィ一ドバック制御器に入力する。 干渉力を相殺するような補正量の算出に は、 モデルでの量、 例えばモデルねじれ角を使用するので、 微分演算を行ったと しても、 補正量にノイズ成分が重畳することがない。 したがって、 電動機の高周 波振動や動作時の音が大きくなることが防止され、 ロボッ トツールの軌跡精度が 向上する。

図面の簡単な説明

第 1図は、 本発明の実施の一形態のロボッ ト制御装置の構成を示す制御ブロッ ク図である。

第 2図は、 モデル制御器の構成を示す制御プロック図である。

発明を実施するための最良の形態

次に、 本発明の実施の形態について図面を参照して説明する。 ここでは、 説明 を容易にするために 2軸ロボットの場合を取り上げるが、 3軸以上の多軸ロボッ トにおいても、 当業者には容易に理解できるように、 以下の説明を該当する軸数 の場合に拡張することにより、 本発明を適用することができる。 ここでは、 2つ の軸をそれぞれ L軸、 U軸とおき、 L軸に関する指令や量については、 その指令 や量を表わす変数にサフィ ックスとして "_L" を付加し、 U軸に関する指令や量 については、 同様に変数に対してサフィ ックスとして "― U" を付加している。 第 1図は、 各軸ごとに 2慣性系に近似したロポッ ト制御系の基本構成を示して いる。 2慣性系は、 電動機 1 1 _L , 1 1 と、 減速機 1 2 _L , 1 2 uなどのパネ要 素と、 ロボッ トアーム 1 3い 1 3 uとによって構成される。 ここで、 電動機 1 1い 1 l uのトルク定数を K_t レ K_t uとし、 各軸の電動機 1 1 _L， 1 1ひの 実際の位置をそれぞれ e_{m L}， S_{m U}とする。 電動機の実際の速度をそれぞれ m—い ^m— Uとし、 電動機の実際の加速度をそれぞれ^ ^L . とする。 減速機

1 ²い 1 ² uでの減速比をそれぞれ N _L， N ひとし、 減速機のパネ定数をそれ ぞれ K _L， K_c— uとする。 アーム 1 3い 1 3 uの慣性モーメントをそれぞれ J_{L L}, J_L— uとし、 アームの実際の位置をそれぞれ e_Lい uとし、 アームの加 速度つまり負荷の加速度をそれぞれ^ュ， uで表わしている。

そして、 U軸と L軸との間の干渉を表わすために、 ロボッ トアームの質量ゃ両 軸間の角度により決定される M_LU/M。²の値が L軸の負荷トルクに乗じられて U 軸の負荷加速度に作用し、 同様に、 M_UL/M。²の値が U軸の負荷トルクに乗じら れて L軸の負荷加速度に作用するものとしている。

- 1 _L, 1 luにはそれぞれセンサ （不図示） が取り付けられおり、 電 L, 1 luの実際の位置がこのセンサから出力されるようになっている。 ロボット制御装置は、 各軸の電動機位置指令 X_{ref L}， X_ref uに基づいて、 それ ぞれ L軸及び U軸の制御を行うものである。 ロボッ ト制御装置には、 U軸及び L 軸のフィードバック制御をそれぞれ行うためのフィ一ドバック制御器 10レ 10リが設けられるとともに、 フィードバック制御器 10_L, 10uと制御対象 との擬似モデルであるモデル制御器 1が設けられている。 モデル制御器 1は、 電 動機位置指令 X_{ref L}, X_{ref U}を入力とし、 ロボッ トのダイナミクスを考慮して各 軸のフィードフォヮ一ド指令 U_{FF L}. U_FF uを計算し、 各軸の電動機 1 1レ

1 l uの加速度項へのフィードフォワード補償を行うとともに、 規範となるモデ ルにおける各軸の電動機位置 _Mmい ½_m u, 電動機速度 _{Mm L}. Θ_Μ ひ、 ね じれ角 e_{Ms L}. 0_Ms u, ねじれ角速度 _Msい ^_Ms uを算出して出力する。 以下、 モデル制御器 1の出力である各要素には、 接頭語 「モデル」 を付加する。

フィードバック制御器 10_L， Ι Ουにおいて、 各軸の位置ゲインは K_pい

Κ_π υであり、 速度ゲインは Κ_νい K_{v U}である。 フィードバック制御器 10_L, Ι Ουは、 それぞれ電動機 1 1い 1 luに与えるための最終的な加速度指令

U_{ref L}, U_ref— uを出力し、 この加速度指令 U_ref—い U_ref_uはそれぞれ駆動回路 14い 14uによって電流指令 I_ref い I_ref uに変換される。 電動機 1 1い 1 luは、 それぞれ電流指令 I_{ref L}, I_ref „によって電流駆動されることになる。 さらにこのロボッ ト制御装置には、 L軸、 U軸のそれぞれごとに、 電動機への 最終的な加速度指令 U_ref— _L, U_ref uと電動機の実際の位置 _L， e_{m U}を入力と して、 実際のねじれ角 _い uとその角速度 _L, —υとを推定する状態観測 器 2_L, 2 uが設けられている。 状態観測器 2し， 2uとしては、 例えば、 日本 国特許出願公開平成 9年第 222910号公報に開示されたものを好ましく使用 することができる。 状態観測器 2_L， 2uで推定された値は、 それぞれ、 フィー ドバック制御器 10 _L， 10uに出力されている。

フィ一ドバック制御器 10 _L, 10 uでは、 それぞれ、 モデル制御器 1から出 力されるモデル電動機位置^ ^ _L， e_Mm_uと実際の電動機位置 e_mい _{m U}との 偏差に対して位置ゲイン K_P__L, K_P— uを乗算して、 それぞれの軸の速度指令とし ている。 このように得られた各速度指令に、 モデル電動機速度 _Mmい ^_{Mm U}と 実際の電動機速度 m^, _m_uとの偏差を加算し、 速度ゲイン K_V—い K_{V U}を乗算 して、 それぞれ、 加速度指令としている。 このような加速度指令に対して、 （a)モ デル制御器 1からのフィードフォワード指令 U_FFい U_{FF TT}と、 （b)モデルねじれ 角 ^ls L. e_{Ms U}と状態観測器 ²い ²uから出力されるねじれ角 し， uと の偏差にフィードバックゲイン い K_{L ;}を乗じたものと、 （C)モデルねじれ角 速度^! _Sい ^_{Ms U}と状態観測器 ²い ²uから出力されるねじれ角速度 L, Uとの偏差にフィードバックゲイン K₂い Κ₂ υを乗じたものとを加算し、 そ れそれ、 各軸に対する最終的な電動機加速度指令 U_ref L. U_{ref u}としている。 このロボッ ト制御系では、 このように得られた最終的な電動機加速度指令

U_{ref L}. U_ref— uに基づいて各軸の電動機 1 1 L. 1 1 uが駆動され、 減速比がそ れぞれ Nい ₁；でぁる減速機 1 2し， 12 uを介して、 各軸のアーム 13い 13 uが駆動される。 その際、 上述したように、 軸間の干渉力が各軸の負荷加速 度に作用する。

次に、 モデル制御器 1の構成を説明する。 第 2図は、 フィードバック制御器や 電動機、 減速機、 ロボットアームを表す擬似モデルからなるモデル制御器 1の構 成の詳細を示している。 擬似モデルにおける各軸の位置ゲインと速度ゲインがそ れぞれモデル位置ゲイン K_PM__L， K_{PM U}とモデル速度ゲイン K_VM ， K_VM— uであ る。 同様に、 モデル電動機慣性モーメント J_MMい J_{MM U}、 モデルアーム慣性モ —メント JLM— L， JLM_U> モデル減速比 N_M— _L， N_{m u}、 モデル減速機パネ定数 K_{CM L}, K_CM u力 モデルパラメータとして定められている。 モデル制御器 1は、 各軸の電動機位置指令 X_ref__L, X_ref_uを入力とし、 この電 動機位置指令 X_{ref L}， X_{ref u}とモデル電動機位置 _m—い S_{Mm U}との偏差にモデ ル位置ゲイン K_{pM L}， K_pM uを乗じて各軸のモデル速度指令としている。 そして、 これらのモデル速度指令から各軸のモデル電動機速度 _Mm__L， ½_m_uを減算した 値にモデル速度ゲイン K_{vM L}， K_{vM U}を乗算して、 それぞれ各軸のモデル加速度 指令としている。 このモデル加速度指令に、 モデル減速機から得られるモデルね じれ角^ is—い e_Ms— uにモデルフィードバックゲイン K_1M—い K_1M— uを乗じて得 られる値と、 モデルねじれ角 _{s L}, e_Ms uを微分して得られるモデルねじれ角速 度 s L， _Ms_Uにモデルフィードバックゲイン _M—い uを乗じて得られ る値とを減算し、 モデル加速度指令 U_Mref L, U_{Mref u}としている。 このモデル加 速度指令 U_Mref い U_Mw u力、 モデル電動機に入力している。 したがって、 モデ ル加速度指令 U_Mref い U_{Mref U}は、

Ujvlref— L - KpM— L . vM— i Xreし L— ^Mm— L)— kvM_L . Mm— L

— KIM L ^-t7Ms L ^_^2M L ' ^Ms L (り

Uhlref— U = K_pM一 U · K_VM— U(Xref— U - ^Mm_U ) - ^vM_U · ^Mm_U

- K!M— U Ms一 U - K₂M_U Ms— U (²) と表わすことができる。

しかし、 ここで想定している 2軸ロボットでは、 上述したように、 干渉により、

L軸の負荷トルクが、 ロボッ トアームの質量ゃ両軸間の角度により決定される値 M J_M/M。_m ²が乗じられて、 U軸の負荷加速度に作用する。 同様に、 U軸の負荷 トルク力、 値 M_ULM/M。_M ²が乗じられて、 L軸の負荷加速度に作用する。 このよ うな干渉をアーム側に作用する外乱としてとらえると、 L軸に関しては、 下式に 示すモデル補正トルク T_{CQmp L}をモデル電動機加速度指令 U_{Mref L}に加算すれば、 干渉によるアームの振動を軽減することができる。

^ M L' _VM L.NM L ^Dis UL ^KvM L '^U L D is UL ^IM L'Di_s UL

X comp L

Κ Μ— L — L

^K2M じ ^Dis UL ^Dis UL

K_CM L JmM L 'NM L

ここで、 D_{is UL}は、 U軸から L軸に作用する干渉トルクであって、

MUL_M

^Dis UL = ^JLM L ^{- K}cM U ~ ~~ ^Ms U (4)

― ― 一 M_OM ^Z - と表わすことができる。

よって、 最終的なモデル電動機加速度指令 U_FF__Lを

UFF— L - U ref L ⁺T_COmp_L (5)

として、 電動機の加速度項へフィードフォワード補償を行う。 このモデル制御器

1には、 モデルねじれ角 e_Ms uを入力として上述の式 (3)，（4)に基づいてモデル補 正トルク T_C。m_P_Lを算出する補正量算出部 ³ Lが設けられており、 補正量算出部

3 _Lからのモデル補正トルク T_c。_{mp L}がモデル電動機加速度指令 U_{Mref L}に加算さ れる。

同様に、 U軸についても、 モデル補正トルク

を 丁 _

!comp— U

^K2M U ' ^Dis LU ^Dis LU N_M u ' ^Dis LU , 、

= = = = = ~ ( 6 ) k_cM— U JmM U " ^ u 上 cM— U のように定める。 ここで D_is— _LUは、 L軸から U軸に作用する干渉トルクであって、

である。 モデルねじれ角 e_{Ms L}を入力として式 (6), (7)に基づきモデル補正トルク

T_comp一 uを求める補正量算出部 3 uが設けられている。 その結果、 最終的なモデル 電動機加速度指令 U_FF uは、

UFF— U = UMref一 U + 丄' (：。 mp— U (⁸)

となる。

なお、 ここで求めたモデル補正トルク T_c。_mpい T_c。_mp— uは、 フィードバック制 御器 1 0 _L , 1 O uの速度ループを比例 （P ) 制御とした場合であり、 比例積分 (P I ) 制御として補正トルクを求め補償することも可能である。

この実施態様では、 このようにモデル制御器 1により干渉トルク補正を行った 上で、 規範となる各状態量を指令としてフィードバック制御器 1 0い 1 0 ₁；に 入力するので、 各ロボットアームは、 他軸からの干渉による影響を受けなくなる。

産業上の利用の可能性

以上説明したように本発明は、 各軸ごとに電動機とロボットアームとの間にバ ネ要素を有する機構が設けられた多軸ロボットの制御に使用されるロボット制御 装置において、 他軸から受ける干渉力をモデル制御器で補正し、 それを基に規範 となる各状態量をフィードバック制御器に入力することにより、 補正トルクにノ ィズ成分が重畳することがなくなり、 ロボットアームは干渉による振動を受けず にツール先端の軌跡精度が向上するという効果がある。

Claims

請 求 の 範 囲

1 . 各軸ごとに電動機とロボットアームとの間にパネ要素を有する機構が 設けられた多軸ロボットを制御対象とするロボット制御方法であって、

前記制御対象と前記制御対象に対するフィ一ドバック制御系との擬似モデルを 用 ¾s、し、

前記擬似モデルを用いて、 前記各電動機に対する位置指令を入力として、 前記 各軸ごとに、 モデル電動機位置指令、 モデル電動機速度指令及びモデルフィード フォワード指令を算出し、

前記擬似モデルを用いて、 軸間に作用する他軸からの干渉による干渉トルクを 求め、 前記干渉トルクを相殺するモデル補正トルクを算出し、

前記モデルフィードフォーヮ一ド指令に対して前記モデル補正トルクを加算し て最終的なモデル電動機加速度指令とし、

前記モデル電動機位置指令、 前記モデル電動機速度指令及び前記最終的なモデ ル電動機加速度指令に応じて各軸ごとのフィードバック制御を実行する、 ロボッ ト制御方法。

2. 前記干渉トルクを、 モデル電動機とモデルロボッ トアームとの位置偏 差であるモデルねじり角により算出する、 請求の範囲第 1項に記載のロボッ ト制 御方法。

3. 各軸ごとに電動機とロボットアームとの間にパネ要素を有する機構が 設けられた多軸ロボッ トの制御に使用されるロボット制御装置であって、 前記各電動機に対する位置指令を入力として、 前記各軸ごとに、 モデル電動機 位置指令、 モデル電動機速度指令及びモデルフイードフォヮ一ド指令を出力する モデル制御器と、

前記モデル制御器から出力される前記各指令に基づき、 前記各電動機及び前記 各ロボットアームを駆動、 制御するフィードバック制御器と、 を備え、

前記モデル制御器内に、 軸間に作用する他軸からの干渉による干渉トルクを求 め、 前記干渉トルクを相殺するモデル補正トルクを算出する補正量算出部が設け られ、 前記モデル補正トルクを加算された前記モデルフイードフォヮ一ド指令が 前記モデル制御器から出力するロボット制御装置。

4. 前記補正量算出部は、 前記干渉トルクを、 モデル電動機とモデルロボ ットアームとの位置偏差であるモデルねじり角により算出する、 請求の範囲第 3 項に記載のロボッ ト制御装置。

5. 各軸ごとに、 実際の電動機位置と前記フィードバック制御器から出力 される電動機加速度指令とに基づいて、 ねじれ角とねじれ角速度を推定する状態 観測器がさらに設けられ、

前記モデル制御器は、 モデルねじれ角及びねじれ角速度も算出し、

前記フィ一ドバック制御器が、 推定されたねじれ角と前記モデルねじれ角の偏 差、 及び推定されたねじれ角速度と前記モデルねじれ角速度との偏差を加味して、 前記電動機加速度指令を出力する、請求の範囲第 3項に記載のロボッ ト制御装置。