JPH10329063A - ロボット制御装置 - Google Patents

Abstract

(57)【要約】 【課題】各軸ごとに電動機とロボットアームとの間にバ
ネ要素を有する機構が設けられた多軸ロボットを制御す
る際に、軸間の干渉による振動や電動機の高周波振動を
起こすことなく、ツール先端の軌跡精度を向上させる。 【解決手段】各電動機に対する位置指令Ｘ_{ref_L},Ｘ
_{ref_U}を入力とし、各電動機及び各ロボットアームを駆
動、制御するフィードバック制御系に対して、モデル電
動機位置指令θ_{Mm_L},θ_{Mm_U}、モデル電動機速度指令 【外１】 及びモデルフィードフォワード指令Ｕ_{FF_F},Ｕ_{FF_L}を出
力するモデル制御器１を設ける。モデル制御器１の内部
に、軸間に作用する他軸からの干渉力を考慮してこの干
渉力を相殺するような補正量（補正トルク）を算出する
補正量算出部を設け、補正量を加算してからモデルフィ
ードフォワード指令を出力する。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

【従来の技術】一般に、電動機により各軸が制御される
産業用ロボットでは、電動機の力不足を補ったり負荷側
からの外乱力の影響を極力小さくしたりするために、減
速比の大きなハーモニックドライブ等のギアを介して、
負荷側のアームを駆動している。したがって、従来は、
制御軸間の力学的干渉はそれほど問題にされてこなかっ
た。しかしながら、近時の高速化、高精度化の要求の高
まりから、ＰＩ制御では補償しきれない力学的影響や、
高減速比駆動であっても無視できない外乱が生じ、問題
となってきている。各軸の電動機とロボットアームとの
間にそれぞれ減速機などのバネ要素を有する機構が設け
られた多軸ロボットにおいて、軸干渉による振動を抑制
する方法として、本出願人は、既に、特願平８−３２２
３０を出願している。特願平８−３２２３０に示される
発明では、各軸に状態観測器を配置して電動機と負荷の
間のねじれ角度を推定し、推定されたねじれ角度を用い
て干渉力を計算し、その干渉力に基づいて補正トルクを
求め、電動機へのトルクに加算して出力している。

【０００３】

【発明が解決しようとする課題】しかしながら、上述し
た特願平８−３２２３０に開示される制御方法では、状
態観測器により推定されたねじれ角度の値を微分して補
正トルクを算出しており、このため、補正トルクにノイ
ズが重畳しやすく、電動機が高周波振動を起こしたり、
動作時の音が大きくなるおそれがある。本発明の目的
は、軸間の干渉による振動や電動機の高周波振動を起こ
すことなく、ツール先端の軌跡精度を向上させることが
できるロボット制御装置を提供することにある。

【０００４】

【課題を解決するための手段】本発明のロボット制御装
置は、各軸ごとに電動機とロボットアームとの間にバネ
要素を有する機構が設けられた多軸ロボットの制御に使
用され、前記各電動機に対する位置指令を入力として、
前記各軸ごとに、モデル電動機位置指令、モデル電動機
速度指令及びモデルフィードフォワード指令を出力する
モデル制御器と、前記モデル制御器から出力される前記
各指令に基づき、前記各電動機及び前記各ロボットアー
ムを駆動、制御するフィードバック制御器と、を備える
ロボット制御装置において、前記モデル制御器内に、軸
間に作用する他軸からの干渉による干渉トルクを求め、
前記干渉トルクを相殺するモデル補正トルクを算出する
補正量算出部を有し、前記モデル補正トルクを加算され
た前記モデルフィードフォワード指令が前記モデル制御
器から出力する。

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

【０００６】

【発明の実施の形態】次に、本発明の実施の形態につい
て図面を参照して説明する。図１は、本発明の実施の一
形態のロボット制御装置の構成を示す制御ブロック図で
あり、図２は、モデル制御器の構成を示す制御ブロック
図である。ここでは、説明を容易にするために２軸ロボ
ットの場合を取り上げるが、３軸以上の多軸ロボットに
おいても、該当する軸数の場合に以下の説明を拡張する
ことにより、本発明を適用することができる。図１は、
各軸ごとに２慣性系（電動機＋減速機等のバネ＋アー
ム）に近似したロボット制御系の基本構成を示してい
る。２軸をＬ軸、Ｕ軸とおき、Ｌ軸に関する指令や量に
ついてはそのサフィックスに"_L"を付加し、Ｕ軸に関す
る指令や量についてはそのサフィックスに"_U"を付加し
ている。

【０００７】このロボット制御装置は、各軸ごとの電動
機位置指令Ｘ_{ref_L},Ｘ_{ref_U}に基づいてＬ軸及びＵ軸の
制御を行うものであり、２軸の制御系（フィードバック
制御器）と制御対象（電動機、減速機、ロボットアー
ム）との擬似モデルであるモデル制御器１を有してい
る。モデル制御器１は、電動機位置指令Ｘ_{ref_L},Ｘ
_{ref_U}を入力とし、ロボットのダイナミクスを考慮して
各軸のフィードフォワード指令Ｕ_{FF_L},Ｕ_{FF_U}を計算
し、各軸の電動機の加速度項へのフィードフォワード補
償を行うとともに、規範となるモデルにおける各軸の電
動機位置θ_{Mm_L},θ_{Mm_U}、電動機速度

【０００８】

【外１】 ねじれ角θ_{Ms_L},θ_{Ms_U}、ねじれ角速度

【０００９】

【外２】 を算出して出力する。以下、モデル制御器１の出力であ
る各要素には、接頭語「モデル」を付加する。一方、フ
ィードバック制御系（フィードバック制御器）におい
て、各軸の位置ゲインはＫ_{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}であり、電動機
の実際の速度は

【００１０】

【外３】 であり、電動機の実際の加速度は

【００１１】

【外４】 である。さらに減速機での減速比がＮ_{_L},Ｎ_{_U}、減速機
のバネ定数がＫ_{c_L},Ｋ_{c_U}、アームの慣性モーメントが
Ｊ_{L_L},Ｊ_{L_U}、アームの実際の位置がθ_{L_L},θ_{L_U}、アー
ムの加速度（負荷加速度）が

【００１２】

【外５】 で表わされている。そして、両軸間の干渉を表わすため
に、ロボットアームの質量や両軸間の角度により決定さ
れるＭ_LU／Ｍ_o ²の値がＬ軸の負荷トルクに乗じられてＵ
軸の負荷加速度に作用し、同様に、Ｍ_UL／Ｍ_o ²の値がＵ
軸の負荷トルクに乗じられてＬ軸の負荷加速度に作用す
る構成となっている。このロボット制御系には、Ｌ軸、
Ｕ軸のそれぞれごとに、電動機への最終的な加速度指令
Ｕ_{ref_L},Ｕ_{ref_U}と電動機の実際の位置θ_{m_L},θ_{m_U}を入
力として、実際のねじれ角θ_{s_L},θ_{s_U}とその角速度

【００１３】

【外６】 とを推定する状態観測器２_L,２_Uが設けられている。さ
て、このフィードバック制御器では、モデル制御器１か
ら出力されるモデル電動機位置θ_{Mm_L},θ_{Mm_U}と実際の
電動機位置θ_{m_L},θ_{m_U}との偏差に対して位置ゲインＫ
_{p_L},Ｋ_{p_U}を乗算して、それぞれの軸の速度指令として
いる。このように得られた各速度指令に、モデル電動機
速度

【００１４】

【外７】 と実際の電動機速度

【００１５】

【外８】 との偏差を加算し、速度ゲインＫ_{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)モデルねじれ角速度

【００１６】

【外９】 と状態観測器２_L,２_Uから出力されるねじれ角速度

【００１７】

【外１０】 との偏差にフィードバックゲインＫ_{2_L},Ｋ_{2_U}を乗じた
ものとを加算し、それぞれ、各軸に対する最終的な電動
機加速度指令Ｕ_{ref_L},Ｕ_{ref_U}としている。このロボッ
ト制御系では、このように得られた最終的な電動機加速
度指令Ｕ_{re f_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}を乗じて各軸のモ
デル速度指令としている。そして、これらのモデル速度
指令から各軸のモデル電動機速度

【００２０】

【外１１】 を減算した値にモデル速度ゲインＫ_{vM_L},Ｋ_{vM_U}を乗算
して、それぞれ各軸のモデル加速度指令としている。こ
のモデル加速度指令に、モデル減速機から得られるモデ
ルねじれ角θ_{Ms_L},θ_{Ms_U}にモデルフィードバックゲイ
ンＫ_{1M_L},Ｋ_{1M_U}を乗じて得られる値と、モデルねじれ
角θ_{Ms_L},θ_{Ms_U}を微分して得られるモデルねじれ角速
度

【００２１】

【外１２】 にモデルフィードバックゲインＫ_{2M_L},Ｋ_{2M_U}を乗じて
得られる値とを減算し、モデル加速度指令Ｕ_{Mref_L},Ｕ
_{Mref_L}としている。このモデル加速度指令Ｕ_{Mref_ L},Ｕ
_{Mref_U}が、モデル電動機に入力している。したがって、
モデル加速度指令Ｕ _{Mref_L},Ｕ_{Mref_U}は、

【００２２】

【数１】 と表わすことができる。しかし、ここで想定している２
軸ロボットでは、干渉により、Ｌ軸の負荷トルクが、ロ
ボットアームの質量や両軸間の角度により決定される値
Ｍ_LUM／Ｍ_oM ²が乗じられて、Ｕ軸の負荷加速度に作用す
る。同様に、Ｕ軸の負荷トルクが、値Ｍ _ULM／Ｍ_oM ²が乗
じられて、Ｌ軸の負荷加速度に作用する。このような干
渉をアーム側に作用する外乱としてとらえると、Ｌ軸に
関しては、下式に示すモデル補正トルクＴ_{comp_L}をモデ
ル電動機加速度指令Ｕ_{Mref_L}に加算すれば、干渉による
アームの振動を軽減することができる。

【００２３】

【数２】 ここで、Ｄ_{is_UL}は、Ｕ軸からＬ軸に作用する干渉トル
クであって、

【００２４】

【数３】 と表わすことができる。よって、最終的なモデル電動機
加速度指令Ｕ_{FF_L}を

【００２５】

【数４】 Ｕ_{FF_L} ＝ Ｕ_{Mref_L} ＋ Ｔ_{comp_L} (5) として、電動機の加速度項へフィードフォワード補償を
行う。このモデル制御器１には、モデルねじれ角θ_{Ms_U}
を入力として上述の式(3),(4)に基づいてモデル補正ト
ルクＴ_{comp_L}を算出する補正量算出部３_Lが設けられて
おり、補正量算出部３_Lからのモデル補正トルクＴ
_{comp_L}がモデル電動機加速度指令Ｕ_{Mref_L}に加算され
る。同様に、Ｕ軸についても、モデル補正トルクＴ
_{comp_U}を

【００２６】

【数５】 のように定める。ここでＤ_{is_LU}は、Ｌ軸からＵ軸に作
用する干渉トルクであって、

【００２７】

【数６】 である。モデルねじれ角θ_{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} モデルねじれ角

Claims (2)

【特許請求の範囲】
1. 【請求項１】 各軸ごとに電動機とロボットアームとの
   間にバネ要素を有する機構が設けられた多軸ロボットの
   制御に使用され、 前記各電動機に対する位置指令を入力として、前記各軸
   ごとに、モデル電動機位置指令、モデル電動機速度指令
   及びモデルフィードフォワード指令を出力するモデル制
   御器と、 前記モデル制御器から出力される前記各指令に基づき、
   前記各電動機及び前記各ロボットアームを駆動、制御す
   るフィードバック制御器と、を備えるロボット制御装置
   において、 前記モデル制御器内に、軸間に作用する他軸からの干渉
   による干渉トルクを求め、前記干渉トルクを相殺するモ
   デル補正トルクを算出する補正量算出部を有し、前記モ
   デル補正トルクを加算された前記モデルフィードフォワ
   ード指令が前記モデル制御器から出力することを特徴と
   するロボット制御装置。
2. 【請求項２】 前記補正量算出部は、前記干渉トルク
   を、モデル電動機とモデルロボットアームとの位置偏差
   であるモデルねじり角により算出する、請求項１に記載
   のロボット制御装置。