JP6237039B2 - ロボット制御装置およびロボット制御方法 - Google Patents

Description

本発明は、複数のアームが関節軸を介して連結されたロボットの動作を制御するロボット制御装置およびロボット制御方法に関する。

従来、多関節型のロボットを利用したアプリケーションの仕様としては、その動作の速度および精度のうち、いずれか一方だけが重要視されるものが多かった。すなわち、速度に対する要求は甘いが精度に対する要求は厳しい（低速でもよいが精度が高くないといけない）もの、精度に対する要求は甘いが速度に対する要求は厳しい（精度は低くてもよいが高速でないといけない）もの、といった具合である。

しかし、精度だけが重要視されていたアプリケーションにおいても、生産性向上を図るべく、高速化の要求がなされるようになってきている。また、速度だけが重要視されていたアプリケーションにおいても、チャックミスなどが起こると実稼働時における速度を上げることが難しくなるため、精度に対する要求が厳しくなってきている。ロボットの動作について、速度および精度の双方の向上、つまり高速化および高精度化の双方を実現しようとすると、ロボットの各軸間における干渉トルク、コリオリ力、遠心力など、複数の駆動要素が連動して影響する事象（干渉）を無視することができなくなる。

例えば、特許文献１には、疑似モデルを用いて、上述したような干渉の影響を補償する技術が開示されている。しかし、この方法では、通常のフィードバック制御に関する演算に加え、疑似モデルに関する演算を行う必要があるため、その計算量が膨大になってしまう。

一方、特許文献２には、自軸の指令から他軸に作用する干渉力を求め、他軸からの干渉力があったとしても、自軸を指令値通りに動作させるように、モータトルク指令および位置指令を補正する技術が開示されている。この場合、疑似モデルを用いることなく、軸毎に他軸への干渉力を求めることで干渉の影響を補償するようにしているため、特許文献１に記載の技術に比べると、計算量を低減するとともに計算を高速化することができると考えられる。

特開平１０−３２９０６３号公報 特開２００５−１８６２３５号公報

しかしながら、干渉の影響係数を求める計算は、軸数に比例して増加する。これは、６軸ロボットの場合、例えば２軸目および５軸目の影響係数を計算するためには、３〜６軸目の関節角度を用いなければ重心位置となす角を計算できないからである。従って、干渉する軸数の組合せについては、二次関数的に増加することになる。そのため、特許文献２に記載の技術では、その第１実施例に挙げられている２軸ロボットなどの軸数が比較的少ないロボットに適用する場合ではなく、軸数が比較的多いロボットに適用した場合には、上述した効果を得ることはできない。

例えば、特許文献２に記載の技術を６軸ロボットに適用した場合、乗算および加算ともに１０００回以上必要になるため、計算量が多くなり、その計算を高速化するという効果を得ることはできない。干渉トルクによる影響を抑制するための計算を高速化できなければ、たとえロボットの動作について高精度化を達成することができたとしても、高速化を実現することはできない。さらに、特許文献２に記載の技術では、軸毎の動きに対して別の軸の干渉トルクだけを計算しているため、コリオリ力、遠心力などによる影響を考慮することができない。つまり、特許文献２に記載の技術（以下、従来の手法とも呼ぶ）では、ロボットの動作について高速化および高精度化の双方を実現することはできない。

本発明は上記事情に鑑みてなされたものであり、その目的は、計算量が大幅に増加することなく、複数の駆動要素が動いている状態で働く影響を抑制することができるロボット制御装置およびロボット制御方法を提供することにある。

請求項１に記載のロボット制御方法および請求項２に記載のロボット制御装置（以下、これらをまとめて本手段と称す）は、複数のアームが関節軸（以下、単に軸とも称す）を介して連結されたロボット、つまり多関節型のロボットの動作を制御するためのものである。前述したように、このようなロボットの動作について、高速化および高精度化の双方を実現しようとすると、各軸間における干渉トルク、コリオリ力、遠心力などの複数の駆動要素が連動して影響する事象（干渉）を無視できなくなる。

ここで、従来技術のように、自軸の動作および他軸の動作といった具合に、軸毎に区分して干渉を抑制するための計算を行うと、各計算において重複する部分が多くなり、その結果、その計算量が膨大なものとなってしまう。そこで、本手段では、ロボット全体の動作に着目し、まず全体動作に係る計算（必要関節トルクＴ_Ａを求める演算）が行われる。詳細は後述するが、この全体動作に係る計算には上記干渉を抑制するための計算が含まれている。そして、その全体動作に係る計算の結果を用い、軸毎の動作に係る計算が行われる。この場合、全体動作に係る計算において干渉を抑制するための計算が含まれているので、軸毎に干渉を抑制するための計算を行う必要はない。このようにすることで、計算の重複が抑えられ、その結果、その計算量が低減されることになる。本手段では、このような考えに基づいて、次のようにしてロボットの動作の制御が行われる。

すなわち、本手段では、アームが指令どおりに動作するために必要となる必要関節トルクＴ_Ａが各軸に発生するように、アームを駆動するためのモータの駆動が制御されるようになっている（駆動制御処理または駆動制御部）。ロボットが所定の動作を行う際に必要となる関節トルク（必要関節トルクＴ_Ａ）は、ニュートン・オイラー法によってロボット物理モデルを元に計算を行うことで、容易に導出が可能である（例えば、「コロナ社 ロボット制御基礎論 吉川恒夫著」など参照）。このようして導出される関節トルクには、コリオリ力や遠心力も含まれており、重力を考慮することも可能である。

このようなことから、本手段では、アームの回転角度の指令値であるアーム角度指令Ｙ_Ａ、アームの回転速度の指令値であるアーム速度指令Ｖ_Ａおよびアームの回転加速度の指令値であるアーム加速度指令Ａ_Ａを用いてニュートン・オイラー法による運動方程式に基づく演算を行うことで必要関節トルクＴ_Ａを求める（必要関節トルク演算ステップまたは必要関節トルク演算部）。このようにして求められた必要関節トルクＴ_Ａは、他軸からの干渉トルクおよび自軸の駆動トルクの双方が含まれたものである。つまり、本手段では、軸毎に干渉トルクを導出することなく、自軸への駆動トルクをも含めた計算を行うようになっている。

本手段では、このようにして求められた必要関節トルクＴ_Ａを活用した制御系が以下のように構築される。すなわち、必要関節トルクＴ_Ａおよびアーム角度指令Ｙ_Ａを用い、次式により、モータの回転角度の指令値であるモータ角度指令Ｙ_Ｍが求められる（モータ角度指令演算ステップまたはモータ角度指令演算部）。ただし、Ｇはモータおよびアームの間に介在する減速機のギア比であり、Ｋは減速機のバネ定数である。
Ｙ_Ｍ＝Ｇ・（Ｙ_Ａ＋（Ｔ_Ａ／Ｋ））
このようして求められるモータ角度指令Ｙ_Ｍに基づいてモータを駆動すれば、適切にねじれが生じて、アームが指令どおりの動きをすることになる。

また、必要関節トルクＴ_Ａおよびモータ角度指令Ｙ_Ｍを用い、次式により、モータが指令どおりに動作するために必要となる必要モータトルクＴ_Ｍが求められる（必要モータトルク演算ステップまたは必要モータトルク演算部）。ただし、Ｊ_Ｍはモータの慣性モーメントであり、Ｄ_Ｍはモータの粘性制動係数であり、ｓはラプラス演算子である。
Ｔ_Ｍ＝（Ｊ_Ｍｓ^２＋Ｄ_Ｍｓ）・Ｙ_Ｍ＋（Ｔ_Ａ／Ｇ）
そして、モータ角度指令Ｙ_Ｍにモータの回転角度の検出値が追従するようにモータトルク指令が生成され（フィードバック制御ステップまたはフィードバック制御部）、その生成されたモータトルク指令に対して必要モータトルクＴ_Ｍを加えたトルク指令に基づいてモータが駆動される（モータ駆動ステップまたはモータ駆動部）。つまり、本手段では、必要モータトルクＴ_Ｍによるフィードフォワード制御が実施される。このようにすれば、複数の駆動要素が動いている状態で働く影響（干渉トルク、コリオリ力、遠心力など）を抑制することができる。

しかも、本手段では、従来の手法のように軸毎に（個別に）干渉トルクを導出することがない。そのため、本手段によれば、従来の手法に比べ、比較的軸数が多いロボットの場合でも計算量を少なく抑えることができる。例えば、６軸のロボットの場合であれば、乗算が８８２回、加算が７６８回で計算することができる。従って、本手段は、従来の手法に比べると、およそ１割以上の計算時間の短縮が可能となる。

このように、本手段では、大幅な計算量の増加を招くことなく、各軸間における干渉トルク、コリオリ力、遠心力などの影響を抑制することができる。そのため、本手段を用いて多関節型のロボットの動作を制御すれば、そのロボットは、速度および精度の双方が重要視されるアプリケーションにも適用することができる。従って、本手段によれば、ロボットの動作について高速化および高精度化の双方の実現に寄与することができるという優れた効果が得られる。なお、本手段は、モータが減速機を介してアームを駆動する構成に限らず、モータが直接アームを駆動する構成（ダイレクトドライブ）にも適用することができる。ただし、その場合、バネ定数Ｋを∞とし、ギア比Ｇを１とすればよい。

本発明の一実施形態を示すもので、ロボットシステムの全体構成図 モータの駆動を制御するための制御系の構成を示すブロック図 演算部の構成を示すブロック図 軸数および計算量の関係を示す図

以下、本発明の一実施形態について図面を参照しながら説明する。
図１は、一般的な産業用ロボットのシステム構成を示している。図１に示すように、ロボットシステム１は、ロボット２、ロボット２の動作を制御するコントローラ３（ロボット制御装置に相当）およびコントローラ３に接続されたティーチングペンダント４により構成されている。

ロボット２は、例えば６軸の垂直多関節型ロボットとして構成されている。ロボット２は、ベース５、ベース５に水平方向に回転可能に支持されたショルダ部６、ショルダ部６に上下方向に回転可能に支持された下アーム７、下アーム７に上下方向に回転可能に支持された第１の上アーム８、第１の上アーム８に捻り回転可能に支持された第２の上アーム９、第２の上アーム９に上下方向に回転可能に支持された手首１０および手首１０に捻り回転可能に支持されたフランジ１１から構成されている。

ベース５、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０およびフランジ１１は、ロボット２のアームとして機能し、アーム先端であるフランジ１１には、図示はしないが、エンドエフェクタ（手先）が取り付けられる。ロボット２に設けられる複数の軸Ｊ１〜Ｊ６（関節軸に相当）、ひいては各アームは、それぞれに対応して設けられるモータ（図２に符号１２を付して示す）により駆動される。各モータの近傍には、それぞれの回転軸の回転角度を検出するための位置検出器（図２に符号１３を付して示す）が設けられている。

ティーチングペンダント４は、例えば使用者が携帯あるいは手に所持して操作可能な程度の大きさで、例えば薄型の略矩形箱状に形成されている。ティーチングペンダント４には、各種のキースイッチが設けられており、使用者は、それらキースイッチにより種々の入力操作を行う。ティーチングペンダント４は、ケーブルを経由してコントローラ３に接続され、通信インターフェイスを経由してコントローラ３との間で高速のデータ転送を実行するようになっており、上記キースイッチの操作により入力された操作信号等の情報はティーチングペンダント４からコントローラ３へ送信される。

図２は、ロボット２の各アームを駆動するためのモータ１２の駆動を制御する制御系の構成を概略的に示している。なお、図２では、１つの軸（アーム）に対応する構成だけを抽出して示しているが、他の軸に対応する構成についても同様になっている。モータ１２は、例えばブラシレスＤＣモータであり、減速機１４を介してアーム１５を駆動する。モータ１２は、コントローラ３に設けられた駆動制御部１６により駆動される。駆動制御部１６は、外部から与えられるアーム１５の回転角度の指令値（目標値）であるアーム角度指令Ｙ_Ａ、位置検出器１３から与えられるモータ１２の回転角度の検出値であるモータ角度検出値Ｙ_Ｄなどに基づいて、モータ１２の駆動を制御する。

駆動制御部１６は、演算部１７、減算器１８、フィードバック制御部１９、加算器２０および電流制御部２１（モータ駆動部に相当）を備えている。演算部１７は、アーム角度指令Ｙ_Ａを用いて各種の演算を行うことにより、必要モータトルクＴ_Ｍおよびモータ角度指令Ｙ_Ｍを求める（詳細は後述する）。減算器１８は、モータ角度指令Ｙ_Ｍからモータ角度検出値Ｙ_Ｄを減算して角度偏差ΔＹを求める。フィードバック制御部１９は、角度偏差ΔＹを用いた演算を行うことにより、モータトルク指令Ｔ_Ｘを生成する。加算器２０は、モータトルク指令Ｔ_Ｘに対して必要モータトルクＴ_Ｍを加算したものをトルク指令Ｔ_Ｙとして出力する。電流制御部２１は、加算器２０から与えられるトルク指令Ｔ_Ｙに基づいてモータ１２の巻線に対する通電を行うことにより、モータ１２を駆動する。

演算部１７の具体的な構成は、図３に示すとおりである。なお、図３では、６つの軸の全てに対応する構成を示している。図３に示すように、演算部１７は、必要関節トルク演算部２２、モータ角度指令演算部２３および必要モータトルク演算部２４を備えている。必要関節トルク演算部２２は、アームが指令どおりに動作するために必要となる必要関節トルクＴ_Ａを求める。このような必要関節トルクＴ_Ａは、ニュートン・オイラー法によってロボット物理モデルを元に計算（逆動力学問題を解く計算）を行えば、容易に導出することができる。なお、ニュートン・オイラー法の計算方法は、公知の計算手法であり、例えば「コロナ社 ロボット制御基礎論 吉川恒夫著」（以下、参考文献とも称す）などを参考にする。

この場合、必要関節トルク演算部２２には、アーム角度指令Ｙ_Ａ１〜Ｙ_Ａ６が与えられている。そこで、必要関節トルク演算部２２は、アーム角度指令Ｙ_Ａ１〜Ｙ_Ａ６を１階微分および２階微分することにより、アームの回転速度の指令値であるアーム速度指令Ｖ_Ａ１〜Ｖ_Ａ６およびアームの回転加速度の指令値であるアーム加速度指令Ａ_Ａ１〜Ａ_Ａ６をそれぞれ求める。そして、必要関節トルク演算部２２は、軸毎のアーム角度指令Ｙ_Ａ１〜Ｙ_Ａ６、アーム速度指令Ｖ_Ａ１〜Ｖ_Ａ６およびアーム加速度指令Ａ_Ａ１〜Ａ_Ａ６を用いて、ニュートン・オイラー法による運動方程式（参考文献の式（３・８７）〜式（３・９５）など）に基づく演算Ｆ_ＮＥを行うことにより、必要関節トルクＴ_Ａ１〜Ｔ_Ａ６を求める（必要関節トルク演算ステップに相当）。そうすると、必要関節トルクＴ_Ａ１〜Ｔ_Ａ６は、下記（１）式のように表すことができる。
［Ｔ_Ａ１…Ｔ_Ａ６］＝Ｆ_ＮＥ（Ｙ_Ａ１…Ｙ_Ａ６，Ｖ_Ａ１…Ｖ_Ａ６，Ａ_Ａ１…Ａ_Ａ６） …（１）

このようにして求められる必要関節トルクＴ_Ａ１〜Ｔ_Ａ６には、コリオリ力や遠心力も含まれている。これは、参考文献の式（３・８８）、式（３・９０）などにおいて、速度の二乗に相当する項が含まれていることなどから明らかである。また、必要関節トルクＴ_Ａ１〜Ｔ_Ａ６は、重力を考慮することも可能である（参考文献の９１ページなど参照）。なお、摩擦要素については、一般的には、上記計算中には含まれていない。そのため、関節軸における摩擦を考慮する場合には、式（３・９５）の右辺に摩擦力の項を付け加えるなどの必要がある（参考文献の式（３・９６）などを参照）。

モータ角度指令演算部２３は、必要関節トルクＴ_Ａ１〜Ｔ_Ａ６およびアーム角度指令Ｙ_Ａ１〜Ｙ_Ａ６を用い、下記（２）式に基づいて、６つのモータ１２の回転角度の指令値であるモータ角度指令Ｙ_Ｍ１〜Ｙ_Ｍ６を求める（モータ角度指令演算ステップに相当）。ただし、Ｇは減速機１４のギア比であり、Ｋは減速機のバネ定数である。また、添え字ｎは、各軸（第１軸〜第６軸）を表すための数字（１〜６）を一般化したものである。また、このようにして求められるモータ角度指令Ｙ_Ｍ１〜Ｙ_Ｍ６に基づいて各モータ１２を駆動すれば、適切にねじれが生じて、アームが指令どおりの動きをすることになる。
Ｙ_Ｍｎ＝Ｇ_ｎ・（Ｙ_Ａｎ＋（Ｔ_Ａｎ／Ｋ_ｎ）） …（２）

モータ角度指令演算部２３は、上記（２）式に基づいて、６つのモータ１２のそれぞれについてのモータ角度指令Ｙ_Ｍ１〜Ｙ_Ｍ６を求めるため、次のような構成を備えている。すなわち、モータ角度指令演算部２３は、補償器２５ａ〜２５ｆ、加算器２６ａ〜２６ｆおよび補償器２７ａ〜２７ｆを備えている。補償器２５ａ〜２５ｆは、必要関節トルクＴ_Ａ１〜Ｔ_Ａ６を、それぞれバネ定数Ｋ_１〜Ｋ_６で除算する。加算器２６ａ〜２６ｆは、アーム角度指令Ｙ_Ａ１〜Ｙ_Ａ６に対し、それぞれ補償器２５ａ〜２５ｆの出力（アームねじれ角度に相当）を加算する。補償器２７ａ〜２７ｆは、加算器２６ａ〜２６ｆの出力に対し、それぞれギア比Ｇ_１〜Ｇ_６を乗算する。補償器２７ａ〜２７ｆの出力は、各モータ１２のモータ角度指令Ｙ_Ｍ１〜Ｙ_Ｍ６として後段に出力される。

必要モータトルク演算部２４は、必要関節トルクＴ_Ａ１〜Ｔ_Ａ６およびモータ角度指令Ｙ_Ｍ１〜Ｙ_Ｍ６を用い、下記（３）式に基づいて、６つのモータ１２が指令どおりに動作するために必要となる必要モータトルクＴ_Ｍ１〜Ｔ_Ｍ６を求める（必要モータトルク演算ステップに相当）。ただし、Ｊ_Ｍはモータ１２の慣性モーメントであり、Ｄ_Ｍはモータ１２の粘性制動係数であり、ｓはラプラス演算子である。
Ｔ_Ｍｎ＝（Ｊ_Ｍｎｓ^２＋Ｄ_Ｍｎｓ）・Ｙ_Ｍｎ＋（Ｔ_Ａｎ／Ｇ_ｎ） …（３）

必要モータトルク演算部２４は、上記（３）式に基づいて、６つのモータ１２のそれぞれについての必要モータトルクＴ_Ｍ１〜Ｔ_Ｍ６を求めるため、次のような構成を備えている。すなわち、必要モータトルク演算部２４は、補償器２８ａ〜２８ｆ、補償器２９ａ〜２９ｆおよび加算器３０ａ〜３０ｆを備えている。補償器２８ａ〜２８ｆは、必要関節トルクＴ_Ａ１〜Ｔ_Ａ６を、それぞれギア比Ｇ_１〜Ｇ_６で除算する。補償器２９ａ〜２９ｆは、モータ角度指令Ｙ_Ｍ１〜Ｙ_Ｍ６に対し、それぞれ慣性モーメントＪ_Ｍ１〜Ｊ_Ｍ６および粘性制動係数Ｄ_Ｍ１〜Ｄ_Ｍ６に関する項（Ｊ_Ｍｎｓ^２＋Ｄ_Ｍｎｓ）を乗算する。加算器３０ａ〜３０ｆは、補償器２８ａ〜２８ｆの出力に対し、それぞれ補償器２９ａ〜２９ｆの出力を加算する。加算器３０ａ〜３０ｆの出力は、各モータ１２の必要モータトルクＴ_Ｍ１〜Ｔ_Ｍ６として後段に出力される。

以上説明したように、本実施形態では、ロボット２全体の動作に着目し、まず全体動作に係る計算（ロボット２のアームが指令どおりに動作するために必要となる必要関節トルクＴ_Ａを求める演算）が行われる。必要関節トルクＴ_Ａは、他軸からの干渉トルクおよび自軸の駆動トルクの双方が含まれたものである。つまり、本実施形態では、軸毎に干渉トルクを導出することなく、自軸への駆動トルクをも含めた計算を行うため、計算の重複が抑えられるとともに、その計算が容易となる。そして、求めた必要関節トルクＴ_Ａが各関節軸に発生するように、アームを駆動するためのモータ１２の駆動を制御する。具体的には、次のような制御系を構築することで、上記制御を実現する。

すなわち、必要関節トルクＴ_Ａおよびアーム角度指令Ｙ_Ａなどを用いて上記（２）式によりモータ角度指令Ｙ_Ｍが求められる。また、必要関節トルクＴ_Ａおよびモータ角度指令Ｙ_Ｍなどを用いて上記（３）式により必要モータトルクＴ_Ｍが求められる。そして、モータ角度指令Ｙ_Ｍにモータ角度検出値Ｙ_Ｄが追従するようにモータトルク指令Ｔ_Ｘが生成され、その生成されたモータトルク指令Ｔ_Ｘに対して必要モータトルクＴ_Ｍを加えたトルク指令Ｔ_Ｙに基づいてモータ１２が駆動される。つまり、本実施形態では、必要モータトルクＴ_Ｍによるフィードフォワード制御が実施される。

このようにすれば、複数の駆動要素が動いている状態で働く影響（干渉トルク、コリオリ力、遠心力など）を抑制することができる。しかも、本実施形態では、従来の手法のように軸毎に個別に干渉トルクを導出していないため、計算の重複が削減される。そのため、従来の手法に比べ、比較的軸数が多いロボットの動作を制御する場合でも、計算量を少なく抑えることができる。図４に示すように、例えば、６軸のロボットの場合、従来の手法では「乗算＝１０４２回、加算＝１０２０回」であるのに対し、本実施形態では「乗算＝８８２回、加算＝７６８回」である。従って、この場合、本実施形態は、従来の手法に比べると、１割以上の計算量および計算時間の短縮が可能となる。

図４に示すように、本実施形態は、従来の手法に比べ、制御対象となるロボットの軸数が増えるほど有利となる。具体的には、軸数が「４」までは、従来の手法のほうが本実施形態よりも計算量が少ないものの、軸数が「５」からは、その関係が逆転する。ただし、従来の手法では、そもそもコリオリ力、遠心力などによる影響を考慮することができない。これに対し、本実施形態では、大幅な計算量の増加を招くことなく、各軸間における干渉トルク、コリオリ力、遠心力などの影響を全て抑制（補償）することができる。そのため、本実施形態のロボットシステム１によれば、高速動作時における精度を向上するなど、ロボット２の動作について高速化および高精度化を実現することができる。従って、本実施形態のロボットシステム１は、速度および精度の双方が重要視されるアプリケーションに用いることができる。

さらに、本実施形態では、各式中にアーム慣性に相当する項が含まれていないため、次のような効果も得られる。すなわち、複数の軸を有するロボットにおいて、アーム慣性は、その姿勢に応じて変動する。例えば、ロボット２のような６軸のロボットにおいて、アームを真上に伸ばした状態では第１軸Ｊ１のアーム慣性が最小となる。そして、このようにアームを真上に伸ばした状態から第２軸Ｊ２を９０度回転させると、アーム慣性は最大となる。この場合、アーム慣性は、最小値と最大値とで倍以上の差がある。このようなことから、動作位置を問わずに性能を発揮させるためには、アーム慣性の計算を逐次行う必要がある。そのため、従来の手法のように、アーム慣性を定数としたものでは、アーム慣性の変動による影響を考慮することができず、制御性能が劣化するおそれがある。

例えば、従来の手法における式（１３）には、アーム慣性Ｊ_Ｌ１が存在する。この場合、アーム慣性Ｊ_Ｌ１が定数であればよいが、時間で変化する値とした場合、アーム慣性Ｊ_Ｌ１および１軸目位置指令ｘ_ｒｅｆ１の積が周波数領域においては単純な積であったにもかかわらず、時間領域において単純な積では書き表せず、畳み込み積分の形となる。しかし、実際の挙動は、時間領域において単純な積の形を成している。そのため、従来の手法では、軸数が「３」以上のロボットの場合に生じるアーム慣性の変動による影響を補償することができず、アーム慣性が大きく変動する場合には、その制御性能が著しく劣化するおそれがある。これに対し、本実施形態では、前述したように自軸の駆動トルクをも考慮した計算を行うことにより、アーム慣性の変動が必要関節トルクＴ_Ａに含まれている。そのため、本実施形態によれば、アーム慣性を逐次更新するといった処理を行わなくても、動作位置に関係なく性能を発揮する（前述したような効果を得る）ことができる。

なお、本発明は上記し且つ図面に記載した実施形態に限定されるものではなく、次のような変形または拡張が可能である。
上記実施形態では、本発明を６軸の垂直多関節型のロボット２の制御に適用した例を説明したが、本発明は、例えば４軸の水平多関節型のロボットなど、複数の軸を有する多関節型のロボット全般の動作制御について適用可能である。
上記実施形態では、モータ１２が減速機１４を介してアームを駆動する構成を例に説明したが、これに限らずともよく、モータ１２が直接アームを駆動する構成（ダイレクトドライブ）でもよい。ただし、その場合、（２）式および（３）式に存在するバネ定数Ｋに「∞」を代入するとともにギア比Ｇに「１」を代入すればよい。

図面中、２はロボット、３はコントローラ（ロボット制御装置）、１２はモータ、１４は減速機、１５はアーム、１６は駆動制御部、１９はフィードバック制御部、２１は電流制御部（モータ駆動部）、２２は必要関節トルク演算部、２３はモータ角度指令演算部、２４は必要モータトルク演算部、Ｊ１〜Ｊ６は軸（関節軸）を示す。

Claims (2)

1. 複数のアームが関節軸を介して連結されたロボットの動作を制御するロボット制御方法であって、
   前記アームが指令どおりに動作するために必要となる必要関節トルクＴ_Ａが前記関節軸に発生するように、前記アームを駆動するためのモータの駆動を制御する駆動制御処理を行い、
   前記駆動制御処理には、
   前記アームの回転角度の指令値であるアーム角度指令Ｙ_Ａ、前記アームの回転速度の指令値であるアーム速度指令Ｖ_Ａおよび前記アームの回転加速度の指令値であるアーム加速度指令Ａ_Ａを用いてニュートン・オイラー法による運動方程式に基づく演算を行うことで前記必要関節トルクＴ_Ａを求める必要関節トルク演算ステップと、
   前記必要関節トルクＴ_Ａおよび前記アーム角度指令Ｙ_Ａを用い、次式により前記モータの回転角度の指令値であるモータ角度指令Ｙ_Ｍを求めるモータ角度指令演算ステップと、
   Ｙ_Ｍ＝Ｇ・（Ｙ_Ａ＋（Ｔ_Ａ／Ｋ））
   前記必要関節トルクＴ_Ａおよび前記モータ角度指令Ｙ_Ｍを用い、次式により前記モータが指令どおりに動作するために必要となる必要モータトルクＴ_Ｍを求める必要モータトルク演算ステップと、
   Ｔ_Ｍ＝（Ｊ_Ｍｓ^２＋Ｄ_Ｍｓ）・Ｙ_Ｍ＋（Ｔ_Ａ／Ｇ）
   前記モータ角度指令Ｙ_Ｍに前記モータの回転角度の検出値が追従するようにモータトルク指令を生成するフィードバック制御ステップと、
   前記モータトルク指令に対して前記必要モータトルクＴ_Ｍを加えたトルク指令に基づいて前記モータを駆動するモータ駆動ステップと、
   が含まれていることを特徴とするロボット制御方法。
   ただし、Ｇは前記モータおよび前記アームの間に介在する減速機のギア比であり、Ｋは前記減速機のバネ定数であり、Ｊ_Ｍは前記モータの慣性モーメントであり、Ｄ_Ｍは前記モータの粘性制動係数であり、ｓはラプラス演算子である。
2. 複数のアームが関節軸を介して連結されたロボットの動作を制御するロボット制御装置であって、
   前記アームが指令どおりに動作するために必要となる必要関節トルクＴ_Ａが前記関節軸に発生するように、前記アームを駆動するためのモータの駆動を制御する駆動制御部を備え、
   前記駆動制御部は、
   前記アームの回転角度の指令値であるアーム角度指令Ｙ_Ａ、前記アームの回転速度の指令値であるアーム速度指令Ｖ_Ａおよび前記アームの回転加速度の指令値であるアーム加速度指令Ａ_Ａを用いてニュートン・オイラー法による運動方程式に基づく演算を行うことで前記必要関節トルクＴ_Ａを求める必要関節トルク演算部と、
   前記必要関節トルクＴ_Ａおよび前記アーム角度指令Ｙ_Ａを用い、次式により前記モータの回転角度の指令値であるモータ角度指令Ｙ_Ｍを求めるモータ角度指令演算部と、
   Ｙ_Ｍ＝Ｇ・（Ｙ_Ａ＋（Ｔ_Ａ／Ｋ））
   前記必要関節トルクＴ_Ａおよび前記モータ角度指令Ｙ_Ｍを用い、次式により前記モータが指令どおりに動作するために必要となる必要モータトルクＴ_Ｍを求める必要モータトルク演算部と、
   Ｔ_Ｍ＝（Ｊ_Ｍｓ^２＋Ｄ_Ｍｓ）・Ｙ_Ｍ＋（Ｔ_Ａ／Ｇ）
   前記モータ角度指令Ｙ_Ｍに前記モータの回転角度の検出値が追従するようにモータトルク指令を生成するフィードバック制御部と、
   前記モータトルク指令に対して前記必要モータトルクＴ_Ｍを加えたトルク指令に基づいて前記モータを駆動するモータ駆動部と、
   を備えていることを特徴とするロボット制御装置。
   ただし、Ｇは前記モータおよび前記アームの間に介在する減速機のギア比であり、Ｋは前記減速機のバネ定数であり、Ｊ_Ｍは前記モータの慣性モーメントであり、Ｄ_Ｍは前記モータの粘性制動係数であり、ｓはラプラス演算子である。

Images