(57)【特許請求の範囲】
【請求項１】
各軸ごとに電動機とロボットアームとの間にバネ要素を有する減速機が設けられた多軸ロボットの制御に使用され、前記電動機、前記減速機及び前記ロボットアームを制御対象とするロボット制御装置において、
フィードバック制御器と、
前記制御対象と前記フィードバック制御器との擬似モデルであるモデル制御器であって、擬似モデルにおける軸間に作用する他軸からの干渉による干渉トルクを求め前記干渉によるアームの振動を軽減するトルクであるモデル補正トルクを算出する補正量算出部を備え、前記各軸ごとの前記各電動機に対する位置指令が入力され、前記入力された位置指令に対する前記擬似モデルの応答を求めて、前記各軸ごとに、それぞれ前記擬似モデルにおける電動機位置及び電動機速度であるモデル電動機位置指令及びモデル電動機速度指令と、前記擬似モデルでの電動機と前記擬似モデルでのロボットアームとの位置偏差であるモデルねじれ角と、該モデルねじれ角を微分した値であるモデルねじれ角速度と、を計算して出力し、前記擬似モデルにおける電動機加速度指令であるモデル電動機加速度指令を計算するモデル制御器と、
を備え、
各軸ごとに、当該軸の前記電動機に対する位置指令をＸref_Lとし、当該軸のモデル電動機位置、モデル電動機速度、モデルねじれ角及びモデルねじれ角速度をそれぞれ
0003981773.tif 000024
とし、前記擬似モデルにおける当該軸の位置ゲイン及び速度ゲインをそれぞれモデル位置ゲインＫpM_L及びモデル速度ゲインＫvM_Lとし、前記擬似モデルにおける当該軸での第１及び第２のフィードバックゲインをそれぞれ第１及び第２のモデルフィードバックゲインＫ1M_L，Ｋ2M_Lとし、前記擬似モデルにおける当該軸のアーム及び電動機の慣性モーメントをそれぞれモデルアーム慣性モーメントＪLM_L及びモデル電動機慣性モーメントＪmM_Lとし、前記擬似モデルにおける当該軸及び当該軸に干渉する他軸での減速機のバネ定数であるモデル減速機バネ定数をそれぞれＫcM_L及びＫcM_Uとし、前記擬似モデルにおける当該軸の減速機の減速比をモデル減速比ＮM_Lとし、前記他軸のモデルねじれ角をθMs_Uとし、前記他軸から当該軸に対する干渉トルクと該干渉トルクの１次微分及び２次微分とをそれぞれ
0003981773.tif 000025
とし、前記擬似モデルにおけるロボットアームの質量と当該軸及び前記他軸の間の角度により決定される値をＭULM／ＭoM2としたときに、前記補正量算出部は、当該軸についての前記干渉トルクＤis_ULを
0003981773.tif 000026
によって算出し、当該軸についての前記モデル補正トルクＴcomp_Lを
0003981773.tif 000027
によって算出し、前記モデル制御器は、当該軸の前記モデル電動機加速度指令ＵMref_Lを
0003981773.tif 000028
によって算出し、該モデル電動機加速度指令ＵMref_Lに当該軸のモデル補正トルクＴcomp_Lを加算した当該軸に対する最終的なモデルフィードフォワード指令ＵFF_Lを計算して出力し、
前記フィードバック制御器は、前記モデル制御器から出力された各軸ごとの前記モデル電動機位置指令及びモデル電動機速度指令と各軸の電動機の実際の位置及び速度とに基づいて電動機加速度指令を生成し、該電動機加速度指令に対して、当該軸に対する前記モデルフィードフォワード指令を加算し、前記モデルねじれ角と前記多軸ロボットの各軸に配置された状態観測器で推定されたねじれ角との差に第１のゲインを乗じたものを加算し、前記モデルねじれ角速度と前記状態観測器で推定されたねじれ角速度との差に第２のゲインを乗じたものを加算して、各軸ごとの最終的な電動機加速度指令を生成し、該最終的な電動機加速度指令に基づいて前記電動機を駆動することを特徴とするロボット制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、産業用ロボットなどの多軸ロボットを制御するロボット制御装置に関し、特に、互いの軸間に作用する力学的な干渉により生じる振動を抑制できるロボット制御装置に関する。
【０００２】
【従来の技術】
一般に、電動機により各軸が制御される産業用ロボットでは、電動機の力不足を補ったり負荷側からの外乱力の影響を極力小さくしたりするために、減速比の大きなハーモニックドライブ等のギアを介して、負荷側のアームを駆動している。したがって、従来は、制御軸間の力学的干渉はそれほど問題にされてこなかった。しかしながら、近時の高速化、高精度化の要求の高まりから、ＰＩ制御では補償しきれない力学的影響や、高減速比駆動であっても無視できない外乱が生じ、問題となってきている。
各軸の電動機とロボットアームとの間にそれぞれ減速機などのバネ要素を有する機構が設けられた多軸ロボットにおいて、軸干渉による振動を抑制する方法として、本出願人は、既に、特願平８−３２２３０を出願している。特願平８−３２２３０に示される発明では、各軸に状態観測器を配置して電動機と負荷の間のねじれ角度を推定し、推定されたねじれ角度を用いて干渉力を計算し、その干渉力に基づいて補正トルクを求め、電動機へのトルクに加算して出力している。
【０００３】
【発明が解決しようとする課題】
しかしながら、上述した特願平８−３２２３０に開示される制御方法では、状態観測器により推定されたねじれ角度の値を微分して補正トルクを算出しており、このため、補正トルクにノイズが重畳しやすく、電動機が高周波振動を起こしたり、動作時の音が大きくなるおそれがある。
本発明の目的は、軸間の干渉による振動や電動機の高周波振動を起こすことなく、ツール先端の軌跡精度を向上させることができるロボット制御装置を提供することにある。
【０００４】
【課題を解決するための手段】
本発明のロボット制御装置は、各軸ごとに電動機とロボットアームとの間にバネ要素を有する減速機が設けられた多軸ロボットの制御に使用され、前記電動機、前記減速機及び前記ロボットアームを制御対象とするロボット制御装置において、フィードバック制御器と、前記制御対象と前記フィードバック制御器との擬似モデルであるモデル制御器であって、擬似モデルにおける軸間に作用する他軸からの干渉による干渉トルクを求め前記干渉によるアームの振動を軽減するトルクであるモデル補正トルクを算出する補正量算出部を備え、前記各軸ごとの前記各電動機に対する位置指令が入力され、前記入力された位置指令に対する前記擬似モデルの応答を求めて、前記各軸ごとに、それぞれ前記擬似モデルにおける電動機位置及び電動機速度であるモデル電動機位置指令及びモデル電動機速度指令と、前記擬似モデルでの電動機と前記擬似モデルでのロボットアームとの位置偏差であるモデルねじれ角と、該モデルねじれ角を微分した値であるモデルねじれ角速度と、を計算して出力し、前記擬似モデルにおける電動機加速度指令であるモデル電動機加速度指令を計算するモデル制御器と、を備え、各軸ごとに、当該軸の前記電動機に対する位置指令をＸref_Lとし、当該軸のモデル電動機位置、モデル電動機速度、モデルねじれ角及びモデルねじれ角速度をそれぞれ
0003981773.tif 000002
とし、前記擬似モデルにおける当該軸の位置ゲイン及び速度ゲインをそれぞれモデル位置ゲインＫpM_L及びモデル速度ゲインＫvM_Lとし、前記擬似モデルにおける当該軸での第１及び第２のフィードバックゲインをそれぞれ第１及び第２のモデルフィードバックゲインＫ1M_L，Ｋ2M_Lとし、前記擬似モデルにおける当該軸のアーム及び電動機の慣性モーメントをそれぞれモデルアーム慣性モーメントＪLM_L及びモデル電動機慣性モーメントＪmM_Lとし、前記擬似モデルにおける当該軸及び当該軸に干渉する他軸での減速機のバネ定数であるモデル減速機バネ定数をそれぞれＫcM_L及びＫcM_Uとし、前記擬似モデルにおける当該軸の減速機の減速比をモデル減速比ＮM_Lとし、前記他軸のモデルねじれ角をθMs_Uとし、前記他軸から当該軸に対する干渉トルクと該干渉トルクの１次微分及び２次微分とをそれぞれ
0003981773.tif 000003
とし、前記擬似モデルにおけるロボットアームの質量と当該軸及び前記他軸の間の角度により決定される値をＭULM／ＭoM2としたときに、前記補正量算出部は、当該軸についての前記干渉トルクＤis_ULを
0003981773.tif 000004
によって算出し、当該軸についての前記モデル補正トルクＴcomp_Lを
0003981773.tif 000005
によって算出し、前記モデル制御器は、当該軸の前記モデル電動機加速度指令ＵMref_Lを
0003981773.tif 000006
によって算出し、該モデル電動機加速度指令ＵMref_Lに当該軸のモデル補正トルクＴcomp_Lを加算した当該軸に対する最終的なモデルフィードフォワード指令ＵFF_Lを計算して出力し、前記フィードバック制御器は、前記モデル制御器から出力された各軸ごとの前記モデル電動機位置指令及びモデル電動機速度指令と各軸の電動機の実際の位置及び速度とに基づいて電動機加速度指令を生成し、該電動機加速度指令に対して、当該軸に対する前記モデルフィードフォワード指令を加算し、前記モデルねじれ角と前記多軸ロボットの各軸に配置された状態観測器で推定されたねじれ角との差に第１のゲインを乗じたものを加算し、前記モデルねじれ角速度と前記状態観測器で推定されたねじれ角速度との差に第２のゲインを乗じたものを加算して、各軸ごとの最終的な電動機加速度指令を生成し、該最終的な電動機加速度指令に基づいて前記電動機を駆動する。
【０００５】
本発明では、他軸から受ける干渉力を干渉トルクとして求めてモデル制御器で補正し、この補正量（補正トルク）を基に規範となる各状態量をフィードバック制御器に入力する。干渉力を相殺するような補正量の算出には、モデルでの量、例えばモデルねじれ角を使用するので、微分演算を行ったとしても、補正量にノイズ成分が重畳することがない。したがって、電動機の高周波振動や動作時の音が大きくなることが防がれ、ロボットツールの軌跡精度が向上する。
【０００６】
【発明の実施の形態】
次に、本発明の実施の形態について図面を参照して説明する。図１は、本発明の実施の一形態のロボット制御装置の構成を示す制御ブロック図であり、図２は、モデル制御器の構成を示す制御ブロック図である。ここでは、説明を容易にするために２軸ロボットの場合を取り上げるが、３軸以上の多軸ロボットにおいても、該当する軸数の場合に以下の説明を拡張することにより、本発明を適用することができる。
図１は、各軸ごとに２慣性系（電動機＋減速機等のバネ＋アーム）に近似したロボット制御系の基本構成を示している。２軸をＬ軸、Ｕ軸とおき、Ｌ軸に関する指令や量についてはそのサフィックスに"_L"を付加し、Ｕ軸に関する指令や量についてはそのサフィックスに"_U"を付加している。
【０００７】
このロボット制御装置は、各軸ごとの電動機位置指令Ｘref_L,Ｘref_Uに基づいてＬ軸及びＵ軸の制御を行うものであり、２軸の制御系（フィードバック制御器）と制御対象（電動機、減速機、ロボットアーム）との擬似モデルであるモデル制御器１を有している。モデル制御器１は、電動機位置指令Ｘref_L,Ｘref_Uを入力とし、ロボットのダイナミクスを考慮して各軸のフィードフォワード指令ＵFF_L,ＵFF_Uを計算し、各軸の電動機の加速度項へのフィードフォワード補償を行うとともに、規範となるモデルにおける各軸の電動機位置θMm_L,θMm_U、電動機速度
【０００８】
【外１】
0003981773.tif 000007
ねじれ角θMs_L,θMs_U、ねじれ角速度
【０００９】
【外２】
0003981773.tif 000008
を算出して出力する。以下、モデル制御器１の出力である各要素には、接頭語「モデル」を付加する。
一方、フィードバック制御系（フィードバック制御器）において、各軸の位置ゲインはＫp_L,Ｋp_U、速度ゲインはＫv_L,Ｋv_U、トルク定数はＫt_L,Ｋt_Uであり、電動機に与えられる最終的な加速度指令はそれぞれＵref_L,Ｕref_Uである。また、各軸の電動機の慣性モーメントはそれぞれＪm_L,Ｊm_L、電流指令はＩref_L,Ｉref_Uである。電動機の実際の位置はθm_L,θm_Uであり、電動機の実際の速度は
【００１０】
【外３】
0003981773.tif 000009
であり、電動機の実際の加速度は
【００１１】
【外４】
0003981773.tif 000010
である。さらに減速機での減速比がＮ_L,Ｎ_U、減速機のバネ定数がＫc_L,Ｋc_U、アームの慣性モーメントがＪL_L,ＪL_U、アームの実際の位置がθL_L,θL_U、アームの加速度（負荷加速度）が
【００１２】
0003981773.tif 000011
で表わされている。そして、両軸間の干渉を表わすために、ロボットアームの質量や両軸間の角度により決定されるＭLU／Ｍo2の値がＬ軸の負荷トルクに乗じられてＵ軸の負荷加速度に作用し、同様に、ＭUL／Ｍo2の値がＵ軸の負荷トルクに乗じられてＬ軸の負荷加速度に作用する構成となっている。
このロボット制御系には、Ｌ軸、Ｕ軸のそれぞれごとに、電動機への最終的な電動機加速度指令Ｕref_L,Ｕref_Uと電動機の実際の位置θm_L,θm_Uを入力として、実際のねじれ角θs_L,θs_Uとその角速度
【００１３】
【外６】
0003981773.tif 000012
とを推定する状態観測器２L,２Uが設けられている。
さて、このフィードバック制御器では、モデル制御器１から出力されるモデル電動機位置θMm_L,θMm_Uと実際の電動機位置θm_L,θm_Uとの偏差に対して位置ゲインＫp_L,Ｋp_Uを乗算して、それぞれの軸の速度指令としている。このように得られた各速度指令に、モデル電動機速度
【００１４】
【外７】
0003981773.tif 000013
と実際の電動機速度
【００１５】
0003981773.tif 000014
との偏差を加算し、速度ゲインＫv_L,Ｋv_Uを乗算して、それぞれ、加速度指令としている。このような加速度指令に対して、(a)モデル制御器１からのモデルフィードフォワード指令ＵFF_L,ＵFF_Uと、(b)モデルねじれ角θMs_L,θMs_Uと状態観測器２L,２Uから出力されるねじれ角θs_L,θs_Uとの偏差にフィードバックゲインＫ1_L,Ｋ1_Uを乗じたものと、(c)モデルねじれ角速度
【００１６】
【外９】
0003981773.tif 000015
と状態観測器２L,２Uから出力されるねじれ角速度
【００１７】
【外１０】
0003981773.tif 000016
との偏差にフィードバックゲインＫ2_L,Ｋ2_Uを乗じたものとを加算し、それぞれ、各軸に対する最終的な電動機加速度指令Ｕref_L,Ｕref_Uとしている。
このロボット制御系では、このように得られた最終的な電動機加速度指令Ｕref_L,Ｕref_Uにより各軸の電動機が駆動され、減速比がそれぞれＮ_L,Ｎ_Uである減速機を介して、各軸のアームが駆動される。その際、上述したように、軸間の干渉力が各軸の負荷加速度に作用する。
【００１８】
次に、モデル制御器１の構成を説明する。図２は、フィードバック制御系や電動機、減速機、ロボットアームを表す擬似モデルからなるモデル制御器１の構成の詳細を示している。擬似モデルにおける各軸の位置ゲインと速度ゲインがそれぞれモデル位置ゲインＫpM_L,ＫpM_Uとモデル速度ゲインＫvM_L,ＫvM_Uである。同様に、モデル電動機慣性モーメントＪmM_L,ＪmM_U、モデルアーム慣性モーメントＪLM_L,ＪLM_U、モデル減速比ＮM_L,ＮM_U、モデル減速機バネ定数ＫcM_L,ＫcM_Uが、モデルパラメータとして定められている。
【００１９】
モデル制御器１は、各軸の電動機位置指令Ｘref_L,Ｘref_Uを入力とし、この電動機位置指令Ｘref_L,Ｘref_Uとモデル電動機位置θMm_L,θMm_Uとの偏差にモデル位置ゲインＫpM_L,ＫpM_Uを乗じて各軸のモデル速度指令としている。そして、これらのモデル速度指令から各軸のモデル電動機速度
【００２０】
【外１１】
0003981773.tif 000017
を減算した値にモデル速度ゲインＫvM_L,ＫvM_Uを乗算して、それぞれ各軸のモデル加速度指令としている。このモデル加速度指令に、モデル減速機から得られるモデルねじれ角θMs_L,θMs_UにモデルフィードバックゲインＫ1M_L,Ｋ1M_Uを乗じて得られる値と、モデルねじれ角θMs_L,θMs_Uを微分して得られるモデルねじれ角速度
【００２１】
0003981773.tif 000018
にモデルフィードバックゲインＫ2M_L,Ｋ2M_Uを乗じて得られる値とを減算し、モデル電動機加速度指令ＵMref_L,ＵMref_Lとしている。このモデル電動機加速度指令ＵMref_L,ＵMref_Uが、モデル電動機に入力している。したがって、モデル電動機加速度指令ＵMref_L,ＵMref_Uは、
【００２２】
【数１】
0003981773.tif 000019
と表わすことができる。
しかし、ここで想定している２軸ロボットでは、干渉により、Ｌ軸の負荷トルクが、ロボットアームの質量や両軸間の角度により決定される値ＭLUM／ＭoM2が乗じられて、Ｕ軸の負荷加速度に作用する。同様に、Ｕ軸の負荷トルクが、値ＭULM／ＭoM2が乗じられて、Ｌ軸の負荷加速度に作用する。このような干渉をアーム側に作用する外乱としてとらえると、Ｌ軸に関しては、下式に示すモデル補正トルクＴcomp_Lをモデル電動機加速度指令ＵMref_Lに加算すれば、干渉によるアームの振動を軽減することができる。
【００２３】
【数２】
0003981773.tif 000020
ここで、Ｄis_ULは、Ｕ軸からＬ軸に作用する干渉トルクであって、
【００２４】
【数３】
0003981773.tif 000021
と表わすことができる。
よって、最終的なモデルフィードフォワード指令ＵFF_Lを
【００２５】
【数４】
ＵFF_L ＝ ＵMref_L ＋ Ｔcomp_L                        (5)
として、電動機の加速度項へフィードフォワード補償を行う。このモデル制御器１には、モデルねじれ角θMs_Uを入力として上述の式(3),(4)に基づいてモデル補正トルクＴcomp_Lを算出する補正量算出部３Lが設けられており、補正量算出部３Lからのモデル補正トルクＴcomp_Lがモデル電動機加速度指令ＵMref_Lに加算される。
同様に、Ｕ軸についても、モデル補正トルクＴcomp_Uを
【００２６】
【数５】
0003981773.tif 000022
のように定める。ここでＤis_LUは、Ｌ軸からＵ軸に作用する干渉トルクであって、
【００２７】
【数６】
0003981773.tif 000023
である。モデルねじれ角θMs_Lを入力として式(6),(7)に基づきモデル補正トルクＴcomp_Uを求める補正量算出部３Uが設けられている。その結果、最終的なモデルフィードフォワード指令ＵFF_Uは、
【００２８】
【数７】
ＵFF_U ＝ ＵMref_U ＋ Ｔcomp_U                        (8)
となる。
なお、ここで求めたモデル補正トルクＴcomp_L,Ｔcomp_Uは、フィードバック制御系の速度ループを比例制御とした場合であり、比例積分制御として補正トルクを求め補償することも可能である。
本実施の形態では、このようにモデル制御器１により干渉トルク補正を行った上で、規範となる各状態量を指令としてフィードバック制御系に入力するので、各ロボットアームは、他軸からの干渉による影響を受けなくなる。
【００２９】
【発明の効果】
以上説明したように本発明は、各軸ごとに電動機とロボットアームとの間にバネ要素を有する機構が設けられた多軸ロボットの制御に使用されるロボット制御装置において、他軸から受ける干渉力をモデル制御器で補正し、それを基に規範となる各状態量をフィードバック制御器に入力することにより、補正トルクにノイズ成分が重畳することがなくなり、ロボットアームは干渉による振動を受けずにツール先端の軌跡精度が向上するという効果がある。
【図面の簡単な説明】
【図１】本発明の実施の一形態のロボット制御装置の構成を示す制御ブロック図である。
【図２】モデル制御器の構成を示す制御ブロック図である。
【符号の説明】
１    モデル制御器
２L,２U     状態観測器
３L,３U     補正量演算部
Ｔcomp_L,Ｔcomp_U     モデル補正トルク
ＵFF_L,ＵFF_U モデルフィードフォワード指令
Ｘref_L,Ｘref_U     位置指令
θMm_L,θMm_U     モデル電動機位置指令
θMs_L,θMs_U     モデルねじれ角
【図１】
0003981773.tif 000029
【図２】
0003981773.tif 000030