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

Description

本発明は、ロボットの動作制御に係るロボット制御装置に関する。

モータによって回転駆動される関節軸を複数組備えた多関節型ロボットにおいて、ロボットアームを教示点間で高速移動させる技術が従来知られている。

特許文献１には、ロボットアームの各軸の位置関係と、ロボットアームの最大負荷姿勢とに基づいて許容最大加速度を算出し、この許容最大加速度から加速時間または減速時間を決定して、ロボットアームの移動時間を短縮する技術が開示されている。

また、特許文献２には、モータにかかる負荷イナーシャと重力トルクとからモータの加減速時間を可変にする技術が開示されている。

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

ところで、特許文献１に示す構成は、複数ある関節軸の中の１軸だけが主として動作することを想定している。

しかし、複数の関節軸が同程度に動作する場合は、加減速時に各関節軸で必要となるトルクがその最大値を超えてしまう場合がある。例えば、ロボットアームの関節軸のうち１つの軸に注目してみた場合、隣接する軸が加減速動作するときにその反力が加わることで、隣接軸が動作しない、またはその動作量が小さい場合に比べて、当該１つの軸での必要なトルクが増加してしまう。

一方、関節軸に発生するトルクは、当該軸に連結されたモータの出力トルクに依存し、当該モータの出力可能トルクによって制限される。すなわち、複数軸が同時に動作する場合に、関節軸を動作させるのに必要なトルクが不足する場合がある。

例えば、加速時にトルクが不足したまま関節軸を回転させると、当該軸だけが遅れた動作となりアーム先端の軌道が予想外の軌道をたどる場合がある。すなわち、ロボットが周辺物と接触する恐れがある。また、減速時に必要なトルクが不足すると、目標教示点でオーバシュートする場合がある。その場合も、周辺物との接触の恐れがある。

また、使用されるモータによっては、回転速度の上昇とともにその出力可能トルクが減少するものがあり、この場合にも、関節軸を動作させるのに必要なトルクが不足することがある。

通常、これらのような場合は、ロボットアームの動作プログラムの修正を行う。具体的には、加減速時間を調整する命令を加えたり、目標動作速度を変更したりして対応する。

しかし、この修正作業は、実際に動作プログラムを運転してカットアンドトライで行う必要があり、工数を要していた。

本開示は、かかる点に鑑みてなされたもので、複数の関節軸を有するロボットアームの各関節軸を動作させるのに必要なトルクが、モータの出力可能トルクを超えないようにしたロボット制御装置を提供することにある。

上記目的を達成するため、本開示の一態様に係るロボット制御装置は、モータと前記モータによって回転駆動される関節軸とを複数組有するロボットアームの動作を制御するロボット制御装置であって、前記ロボットアームにおける隣り合う作業点間の移動距離と目標移動速度とに基づいて、各前記関節軸に対する移動指令をそれぞれ生成して出力する移動指令生成部と、各前記関節軸の回転速度と回転加速度と関節角度に基づいて、各前記関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部と、前記移動指令を加減速処理する加減速処理部と、を備え、各前記関節軸のうち少なくとも１つの前記関節軸において、前記関節軸の前記動力学トルクのうち前記回転加速度に依存する慣性項トルクが、前記関節軸を加減速動作させるために利用可能な利用可能トルクよりも大きい場合は、各前記関節軸の回転動作における共通の加減速時間が相対的に大きくなるように前記加減速時間を補正し、補正された前記加減速時間に基づいて前記加減速処理部により前記移動指令が前記加減速処理され、各前記関節軸のうち少なくとも１つの前記関節軸において、前記関節軸の前記動力学トルクが、前記関節軸の出力可能トルクよりも大きい場合は、前記ロボットアームが前記作業点間の移動距離を前記目標移動速度で移動するのに要する移動時間が相対的に大きくなるように前記移動時間を補正し、補正した前記移動時間に基づいて前記移動指令生成部により移動指令が生成されることを特徴とする。

この構成によれば、本開示の一態様に係るロボット制御装置は、各関節軸の動作に必要なトルクの不足を予め検知できる。そのため、このロボット制御装置は、トルクの不足分に応じた値で加減速時間または移動時間を補正して、これらの値のいずれかを当初の値よりも大きくすることで、出力可能な範囲内のトルクで関節軸を回転駆動させて、ロボットアームの動作を制御することができる。このことにより、本開示の一態様に係るロボット制御装置は、ロボットの動作プログラムの教示修正の手間を省くことができる。

以上説明したように、本開示の一態様に係るロボット制御装置によれば、ロボットアームを動作させるときの各関節軸で必要なトルクがその出力可能トルクを超えないように、加減速時間または移動時間を自動調整し得る。そのため、ロボットの動作プログラムの教示修正の手間を省くことができる。

［図１］実施形態１に係るロボット制御装置の機能ブロック図である。
［図２］異なる関節軸での回転速度及び回転加速度の時間変化を示す図である。
［図３］ｊ軸における加速時の関節角度、回転速度及び回転加速度の時間変化を示す図である。
［図４］ｊ軸における減速時の関節角度、回転速度及び回転加速度の時間変化を示す図である。
［図５］２軸ロボットの構成を示す模式図である。
［図６Ａ］２軸ロボットにおける第１軸動作時のトルクの各成分の時間変化を示す図である。
［図６Ｂ］２軸ロボットにおける第１及び第２軸動作時の第１軸のトルクの各成分の時間変化を示す図である。
［図６Ｃ］第１及び第２軸動作時に加速時間を補正した場合の第１軸のトルクの各成分の時間変化を示す図である。
［図７］実施形態２に係るロボット制御装置の機能ブロック図である。
［図８］ｊ軸における加速時の関節角度、回転速度、回転加速度及びトルクの時間変化を示す図である。
［図９Ａ］モータ回転速度とモータ出力トルクとの関係を示す図である。
［図９Ｂ］移動時間を補正する前の、モータ回転速度とモータ出力トルク及びｊ軸の動力学トルクとの関係を示す図である。
［図９Ｃ］移動時間を補正した後の、モータ回転速度とモータ出力トルク及びｊ軸の動力学トルクとの関係を示す図である。

以下、本発明の実施形態を図面に基づいて詳細に説明する。以下の好ましい実施形態の説明は、本質的に例示に過ぎず、本発明、その適用物或いはその用途を制限することを意図するものでは全くない。

（実施形態１）
［ロボット制御装置の機能ブロック構成］
図１は、本実施形態に係るロボット制御装置１０の機能ブロック構成を示す。また、図２は、異なる関節軸であるｉ軸及びｊ軸における回転速度及び回転加速度の時間変化を示す。また、図３，４は、それぞれｊ軸における加速時及び減速時の関節角度と回転速度と回転加速度との時間変化を示す。また、図５は、２軸ロボットの模式図を示す。なお、以降の説明において示す添字ｉ，ｊは整数で１≦ｉ，ｊ≦ｎかつｉ≠ｊであり、ｎはロボットアーム６に含まれる関節軸の個数である。

ロボット制御装置１０は、動作プログラム１と、処理部２と、移動指令生成部３と、加減速処理部４と、サーボ５と、移動時間算出部７と、各軸速度算出部８と、加減速時間算出部９と、パラメータ演算部２０と、動力学トルク算出部２１と、補正処理部２２（第１補正処理部）と、演算部２３とを機能ブロックとして備えている。なお、図１に示す機能ブロックは、図示しない記録部に保存された、または外部から読み込まれたソフトウェアをロボット制御装置１０に設けられた図示しないＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等の演算装置上で実行することで実現される。このソフトウェアに動作プログラム１が含まれていてもよい。また、説明の便宜上、ロボットアーム６に組み込まれた関節軸と、当該関節軸に連結されて関節軸を回転駆動するモータとで構成される複数の組については図示を省略している。

動作プログラム１には、ロボットアーム６の教示点位置と、教示点間を結ぶ軌跡の形状にあたる補間形態と、目標移動速度とが記録されている。教示点位置は、直交空間上の作業点位置またはロボットアーム６の関節角度である。目標移動速度は、教示点間を指示された補間形態で移動するときの目標速度である。

補間形態としては、直交空間上の作業点を、直線や円弧等の特定の形状を描くように補間するＣＰ（ｃｏｎｔｉｎｕｏｕｓ ｐａｔｈ）補間と、各関節軸を一定の回転速度で動かし、作業点の軌跡の形状は問わないＰＴＰ（ｐｏｉｎｔ ｔｏ ｐｏｉｎｔ）補間とがあるが、以降は、ＰＴＰ補間を行う場合について説明する。ＰＴＰ補間は教示点間を最短時間で移動する場合に適用する補間形態であり、ロボットアーム６の関節軸のいずれかもしくは複数が、その最大速度で回転するように動作させることが多い。

処理部２は、動作プログラム１に記録されている教示点位置が直交空間上の値である場合に、それをロボットアーム６の各関節軸の関節角度に変換する。また、処理部２は、教示点位置がロボットアーム６の関節角度の場合は直交空間上の値に変換する。なお、以降の説明において、動作プログラム１上に記録された任意の教示点をＰａとし、その次の教示点をＰｂとする。

移動指令生成部３は、隣り合う教示点間の移動量と目標移動速度とに基づいて移動指令を生成して出力する。この移動指令は単位時間当たりの関節角移動量である。各関節の関節角移動量をまとめて１組とし、これを１つの移動指令とする。よって、移動指令はベクトル量である。移動指令生成部３は、後述する移動時間に対応する複数組の移動指令を時系列的に出力する。この時系列的に出力される複数組の移動指令を移動指令列と言う。なお、移動指令生成部３が出力する移動指令列には後述する加減速処理はなされていない。

ＰＴＰ補間の場合、教示点Ｐａ、Ｐｂでの関節角度をそれぞれθａ、θｂとし、教示点間に対応する作業点間の移動時間をＴｍ、補間の単位時間をＴｈとするとき、移動指令Δθは、式（１）で表わされる。なお、移動時間Ｔｍは後述する移動時間算出部７で算出される。

Δθ＝（θｂ－θａ）・Ｔｈ／Ｔｍ ・・・（１）
ここで、θａ、θｂは、各関節軸の関節角度を要素とするベクトル量である。また、Δθは各関節軸の単位時間当たりの関節角移動量を要素とするベクトル量である。

加減速処理部４は、移動指令生成部３からの移動指令列に対して加減速処理を行って、加減速した移動指令を出力する。なお、加減速処理は後述する演算部２３が出力する補正後の加速時間または減速時間に基づいて行い、具体的な加減速処理の手順については後述する。

また、加速時間とは、例えば、教示点Ｐａから加速を始めて目標移動速度に達するまでの時間をいい、減速時間とは、例えば、教示点Ｐｂの手前で減速を始めて移動速度が零になるまでの時間をいい、加減速時間とは、これら両者を合わせた時間をいう。また、各関節軸について、各関節軸の許容最大加速度で目標回転速度まで加速したときの加速時間（各軸の最短加速時間と呼ぶことがある）を求め、これら各関節軸における最短加速時間の中で値が最大のものを選び、これを全軸共通の加速時間とする。図２，３に示すように、全ての関節軸はこの全軸共通の加速時間で加速する。また、減速時には、各関節軸における最短減速時間の中で値が最大のものを選び、これを全軸共通の減速時間とする。加速時と同様に、全ての関節軸はこの全軸共通の減速時間で減速する（図２参照）。

加減速処理部４は、後述する加減速時間算出部９が出力する加速時間で目標移動速度に達するように、移動指令生成部３からの移動指令列を加速処理する。具体的には、図２，３に示すように、各関節軸における回転加速度（以下、単に加速度ともいう）を時間変化させて所定の目標移動速度に到達するようにする。また、教示点Ｐｂ手前で目標移動速度から減速を始めて、加減速時間算出部９が出力する減速時間が経過した後に教示点Ｐｂに到達するように、移動指令生成部３からの移動指令列を減速処理する。なお、以降の説明において、加速度の時間変化波形を加速プロファイルまたは減速プロファイルあるいは加減速プロファイルと呼ぶことがある。なお、図２に示すように、加減速プロファイルは、ロボットアーム６の振動を誘発しないように時間に対して速度がＳ字状に変化する形状とする。

サーボ５は、加減速処理部４で処理された移動指令列に基づいて、各関節軸に連結されたモータを回転駆動させてロボットアーム６の動作を制御する。

移動時間算出部７は、ロボットアーム６の先端が教示点Ｐａから教示点Ｐｂへ加減速せずに目標移動速度で移動するのに要する所要時間を算出して出力する。当該所要時間が前述した移動時間Ｔｍに相当する。

教示点Ｐａ，Ｐｂに対応する作業点間の移動距離をＬ、目標移動速度をＶとするとき、移動時間Ｔｍは、式（２ａ）で表わされる。

Ｔｍ＝Ｌ／Ｖ ・・・（２ａ）
なお、ＰＴＰ補間の場合、移動距離Ｌは、作業点間の直線距離に相当する。また、関節軸を加速または減速回転させてロボットアーム６を動作させる際に、自身の許容最大回転速度を超える軸がある場合は、当該軸の回転速度が許容最大速度に収まるように式（２ａ）から算出された移動時間Ｔｍは修正される。

また、ＰＴＰ補間において、教示点Ｐａ，Ｐｂに対応する作業点間を最短時間で移動する場合は、各関節軸の移動角度をその最大回転速度で除して得る値（当該軸の最短移動時間と呼ぶことがある）の中の最大値を移動時間Ｔｍとする。

すなわち、複数の関節軸のうちｊ軸の移動角度をΔθｊ、関節軸ｊの最大回転速度をωｊｍａｘとするとき、移動時間Ｔｍは式（２ｂ）で表わされる。

Ｔｍ＝ｍａｘ｛Δθｊ／ωｊｍａｘ｝ ・・・（２ｂ）
ここで、ｍａｘ｛｝は括弧内の要素（１個もしくは複数個）のうちの最大のものを選択することを意味する。

各軸速度算出部８は、ＰＴＰ補間時の各関節軸の目標回転速度を算出し出力する。

また、ｊ軸に関する目標回転速度ωｊｃは、式（３）で表わされる。

ωｊｃ＝（θｂｊ－θａｊ）／Ｔｍ ・・・（３）
ここで、θａｊ、θｂｊはそれぞれ、隣り合う教示点Ｐａ、Ｐｂでのｊ軸の関節角度であり、Ｔｍは上述した移動時間である。よって、目標回転速度ωｃは各関節軸の目標回転速度を要素とするベクトル量である。なお、ωｊｃはＰＴＰ補間前に予め値を求めることができる。

加減速時間算出部９は、教示点Ｐａから目標速度に加速するときの加速時間、教示点Ｐｂ手前で目標速度から減速して教示点Ｐｂに到達するときの減速時間を求め出力する。

ＰＴＰ補間の場合は、各軸速度算出部８からの各関節軸の目標回転速度に基づいて、最短の加減速時間を算出し出力する。

まず、ｊ軸についての最短加速時間Ｔａｊを次のように求める。

Ｔａｊ＝｜ωｊｃ｜／αａｊｍａｘ ・・・（４）
ここで、ωｊｃは各軸速度算出部８で算出されたｊ軸の目標回転速度、αａｊｍａｘは、教示点Ｐａにおいてｊ軸だけを単独で加速する場合のｊ軸の許容最大加速度である。

教示点Ｐａからロボットアーム６を移動させるときの加速時間Ｔａは、式（５）で表わされる。

Ｔａ＝ｍａｘ｛Ｔａｊ｝ ・・・（５）
ここで、ｍａｘ｛｝は括弧内の複数の要素から値が最大のものを選択することを意味する。

ｊ軸についての最短減速時間Ｔｂｊについても同様に式（６）で表わされる。

Ｔｂｊ＝｜ωｊｃ｜／αｂｊｍａｘ ・・・（６）
ここで、αｂｊｍａｘは、教示点Ｐｂにおいてｊ軸だけを単独で減速する場合のｊ軸の許容最大加速度である。

従って、教示点Ｐｂへロボットアーム６を移動させるときの減速時間Ｔｂは、式（７）で表わされる。

Ｔｂ＝ｍａｘ｛Ｔｂｊ｝ ・・・（７）
＜＜関節軸の加速度の最大値、回転速度及び関節角度の算出手順＞＞
パラメータ演算部２０は、関節軸が加速または減速回転中の加速度の絶対値が最大となる時刻ｔ１、ｔ２における、各関節軸の加速度の最大値と回転速度と関節角度とを求め出力する。なお、加速度の最大値とは関節軸が加速または減速回転中の加速度の絶対値が最大のものをいう。よって、正方向に回転する場合は、加速度の最大値は、加速時は正値を、減速中は負値を採る。

具体的には、処理部２から出力される教示点Ｐａでのｊ軸の関節角度θａｊと、各軸速度算出部８から出力される教示点Ｐａから教示点Ｐｂへ移動するときのｊ軸の目標回転速度ωｊｃと、加減速時間算出部９から出力される教示点Ｐａから移動を開始するときの加速時間Ｔａとに基づいて、以下を算出する。すなわち、ｊ軸が加速して回転する時間帯での、ｊ軸の加速度の最大値αａｊと、加速度が最大値αａｊになる時刻ｔ１での回転速度ωｊ（ｔ１）及び関節角度θｊ（ｔ１）と、加速度が最大値αａｊから減少し始める時刻ｔ２での回転速度ωｊ（ｔ２）及び関節角度θｊ（ｔ２）と、を算出する。なお、図３，４の下側のグラフにそれぞれ示すように加減速プロファイルは時間に対して台形形状としている。

図３の下側のグラフに示すように、時刻ｔ１，ｔ２はそれぞれ以下の式（８）、（９）で表わされる。

ｔ１＝ｒ・Ｔａ （ただし、０＜ｒ＜１） ・・・（８）
ｔ２＝（１－ｓ）・Ｔａ （ただし、０＜ｓ＜１，ｒ＋ｓ≦１） ・・・（９）
例えば、図３の下側のグラフに示す加速度の時間積分値が目標回転速度ωｊｃであるから（図３の中段のグラフ参照）、加速度の最大値αａｊは式（１０）で表わされる。

αａｊ＝２・ωｊｃ／｛Ｔａ・（２－ｒ－ｓ）｝ ・・・（１０）
式（１０）を用いて、時刻ｔ１におけるｊ軸の回転速度ωｊ（ｔ１）および関節角度θｊ（ｔ１）は、式（１１）および式（１２）に示すように導出される。

ωｊ（ｔ１）＝αａｊ・ｔ１／２ ・・・（１１）
θｊ（ｔ１）＝θａｊ＋αａｊ・ｔ１^２／６ ・・・（１２）
ここで、θａｊは、教示点Ｐａでのｊ軸の関節角度である。

また、時刻ｔ２におけるｊ軸の回転速度ωｊ（ｔ２）および関節角度θｊ（ｔ２）は、式（１３）および式（１４）に示すように導出される。

ωｊ（ｔ２）＝ωｊ（ｔ１）＋αａｊ・（ｔ２－ｔ１） ・・・（１３）
θｊ（ｔ２）＝θｊ（ｔ１）＋（ωｊ（ｔ１）＋ωｊ（ｔ２））・（ｔ２－ｔ１）／２ ・・・（１４）
＜＜動力学トルク及び慣性項トルクの算出手順＞＞
動力学トルク算出部２１は、上述した時刻ｔ１、ｔ２における各関節軸の動力学トルクをそれぞれ算出し出力する。

具体的には、パラメータ演算部２０で算出された以下のパラメータ、加速度の最大値αａ、時刻ｔ１における回転速度ω（ｔ１）、関節角度θ（ｔ１）、時刻ｔ２における回転速度ω（ｔ２）、関節角度θ（ｔ２）、また、ロボットアーム６のリンク長、重心位置、重心質量、リンクの慣性モーメント、モータのイナーシャ等の定数（図５参照）から、時刻ｔ１および時刻ｔ２での動力学トルクを算出する。

この動力学トルクは、関節角度θの場合に、回転速度ω及び加速度αで動作するのに必要な各関節軸のトルクであり、関節角度θ、回転速度ω、加速度αは、各関節軸の成分からなるベクトル量である。

また、時刻ｔにおける動力学トルクτ（ｔ）は式（１５）で表わされる。

τ（ｔ）＝Ｈ（θ（ｔ））・α（ｔ）＋Ｄ・ω（ｔ）＋ｂ（ω（ｔ），θ（ｔ）） ・・・（１５）
ここで、Ｈ（θ（ｔ））は関節角度θ（ｔ）における慣性行列であり、Ｄは粘性係数からなる粘性行列である。さらに、ｂ（ω（ｔ），θ（ｔ））は、時刻ｔでの関節角度θ（ｔ）における速度ω（ｔ）による遠心力、コリオリ力、及び関節角度θ（ｔ）における重力トルクの和である。また、なお、式（１５）は、ラグランジュ法またはニュートンオイラー法によりロボットアーム６の運動方程式を立てて解くことで導出できる。

ここで、式（１５）の右辺を、第１項とそれ以外の部分とに分けて、式（１６）で表わされる形式とする。なお、式（１５）、（１６）の右辺の第１項は加速度のみに依存する成分（以下、慣性項トルクという）である。また、式（１５）、（１６）の右辺の第１項以外の部分を「残余トルク」と呼ぶことがある。

τ（ｔ）＝τａ（ｔ）＋τｄ（ｔ） ・・・（１６）
ただし、
τａ（ｔ）＝Ｈ（θ（ｔ））・α（ｔ） ・・・（１７）
τｄ（ｔ）＝Ｄ・ω（ｔ）＋ｂ（ω（ｔ），θ（ｔ）） ・・・（１８）
動力学トルク算出部２１では、上述した最大加速度αａ、時刻ｔ１における回転速度ω（ｔ１）、関節角度θ（ｔ１）、および、時刻ｔ２における回転速度ω（ｔ２）、関節角度θ（ｔ２）を式（１６）～（１８）に代入して、以下を算出する。すなわち、動力学トルク算出部２１は、時刻ｔ１での慣性項トルクτａ（ｔ１）、慣性項トルク以外のトルクτｄ（ｔ１）、動力学トルクτ（ｔ１）、および、時刻ｔ２での慣性項トルクτａ（ｔ２）、慣性項トルク以外のトルクτｄ（ｔ２）、動力学トルクτ（ｔ２）を算出する。

例えば、図５に示す２軸ロボットにおいて、第１軸の動力学トルクτ１（ｔ）及び第２軸の動力学トルクτ２（ｔ）は、それぞれ式（１９）、（２０）で表わされる。

τ１（ｔ）＝（Ｈ１１・α１＋Ｈ１２・α２）＋Ｄ１１・ω１＋ｂ１ ・・・（１９）
τ２（ｔ）＝（Ｈ２１・α１＋Ｈ２２・α２）＋Ｄ２２・ω２＋ｂ２ ・・・（２０）
なお、慣性行列Ｈ１１～Ｈ２２は以下の式（２１）～（２４）で表わされる。

Ｈ１１＝ｍ１・ｌｇ１^２＋ｍ２・Ｌ１^２＋ｍ２・ｌｇ２^２＋Ｉ１＋Ｉ２＋２・ｍ２・Ｌ１・ｌｇ２・ｃｏｓθ２＋Ｊｍ１ ・・・（２１）
Ｈ１２＝ｍ２・ｌｇ２^２＋Ｉ２＋ｍ２・Ｌ１・ｌｇ２・ｃｏｓθ２ ・・・（２２）
Ｈ２１＝ｍ２・ｌｇ２^２＋Ｉ２＋ｍ２・Ｌ１・ｌｇ２・ｃｏｓθ２ ・・・（２３）
Ｈ２２＝ｍ２・ｌｇ２^２＋Ｉ２＋Ｊｍ２ ・・・（２４）
また、粘性行列Ｄ１１，Ｄ２２は式（２５）、（２６）で表わされる。

Ｄ１１＝ｄ１ ・・・（２５）
Ｄ２２＝ｄ２ ・・・（２６）
また、遠心力、コリオリ力及び重力トルクの和であるｂ１，ｂ２はそれぞれ式（２７）、（２８）で表わされる。

ｂ１＝－ｍ２・Ｌ１・ｌｇ２・（２・ω１＋ω２）・ω２・ｓｉｎθ２＋（ｍ１・ｇ・ｌｇ１＋ｍ２・ｇ・Ｌ１）ｃｏｓθ１＋ｍ２・ｇ・ｌｇ２・ｃｏｓ（θ１＋θ２） ・・・（２７）
ｂ２＝ｍ２・Ｌ１・ｌｇ２・ω１^２・ｓｉｎθ２＋ｍ２・ｇ・ｌｇ２・ｃｏｓ（θ１＋θ２） ・・・（２８）
ここで、式（１９）～（２８）における各符号の内容は以下の通りである。

θ１：時刻ｔにおける第１軸の回転角（紙面における左回りを正方向とする。）
θ２：時刻ｔにおける第２軸の回転角（紙面における左回りを正方向とする。）
ω１：時刻ｔにおける第１軸の回転速度
ω２：時刻ｔにおける第２軸の回転速度
α１：時刻ｔにおける第１軸の回転加速度
α２：時刻ｔにおける第２軸の回転加速度
ｍ１：第１軸のリンクの重心質量
ｍ２：第２軸のリンクの重心質量、なお、ｍ２にはリンク先端の負荷の質量が含まれる。

ｌｇ１：第１軸の回転中心から重心位置までの距離
ｌｇ２：第２軸の回転中心から重心位置までの距離
Ｌ１：第１軸の回転中心から第２軸の回転中心までの距離
Ｉ１：第１軸の、重心位置での回転軸回りの主慣性モーメント
Ｉ２：第２軸の、重心位置での回転軸回りの主慣性モーメント
Ｊｍ１：第１軸のモータイナーシャのアーム回転軸換算値
Ｊｍ２：第２軸のモータイナーシャのアーム回転軸換算値
ｄ１：第１軸の粘性摩擦係数
ｄ２：第２軸の粘性摩擦係数
ｇ：重力加速度
である。

＜＜第１補正係数の算出手順＞＞
補正処理部２２（第１補正処理部）は、動力学トルク算出部２１で算出された、時刻ｔ１およびｔ２における、動力学トルクτ（ｔ１）、τ（ｔ２）、慣性項トルクτａ（ｔ１）、τａ（ｔ２）及び慣性項トルク以外のトルクτｄ（ｔ１）、τｄ（ｔ２）に基づいて第１補正係数γを算出し出力する。

具体的な第１補正係数γの算出手順について以下に説明する。なお、各関節軸の出力可能な最大トルク（出力可能トルク）を許容最大トルクτｍａｘとする。

まず、ｊ軸に関する許容最大トルクτｊｍａｘと、動力学トルク算出部２１で算出された時刻ｔ１におけるｊ軸の動力学トルクτｊ（ｔ１）とを用いて、式（２９）よりｊ軸の補正値γｊ１を算出する。

なお、許容最大トルクτｊｍａｘの符号は動力学トルクτｊ（ｔ１）と同符号に合わせる。

γｊ１＝（τｊ（ｔ１）－τｄｊ（ｔ１））／（τｊｍａｘ－τｄｊ（ｔ１））
＝τａｊ（ｔ１）／（τｊｍａｘ－τｄｊ（ｔ１）） ・・・（２９）
ここで、τｊ（ｔ１）、τａｊ（ｔ１）、τｄｊ（ｔ１）は前記式（１６）～（１８）で表される値のｊ軸の成分である。

また、許容最大トルクτｊｍａｘの符号を動力学トルクτｊ（ｔ１）と同符号にするので、式（２９）により算出される補正値γｊ１は正値となることに注意する。

式（２９）により算出された補正値γｊ１について、この値が１よりも小さい場合は、γｊ１を１に置き換える。すなわち、
ｉｆ γｊ１＜１ ⇒ γｊ１＝１ ・・・（３０）
同様に、時刻ｔ２におけるｊ軸の動力学トルクτｊ（ｔ２）を用いて、式（３１）より補正値γｊ２を算出する。

γｊ２＝（τｊ（ｔ２）－τｄｊ（ｔ２））／（τｊｍａｘ－τｄｊ（ｔ２））
＝τａｊ（ｔ２）／（τｊｍａｘ－τｄｊ（ｔ２）） ・・・（３１）
ただし、
ｉｆ γｊ２＜１ ⇒ γｊ２＝１ ・・・（３２）
ここで、式（２９）における（τｊｍａｘ－τｄｊ（ｔ１））は、ｊ軸の許容最大トルクτｊｍａｘのうち加減速回転に利用可能なトルク（利用可能トルク）に相当する。

よって、γｊ１＜１、及びγｊ２＜１である場合は、式（２９）または式（３１）より明らかなように、ｊ軸において加減速回転動作を実現するのに必要なトルクである慣性項トルクτａｊ（ｔ１）及びτａｊ（ｔ２）が、ｊ軸の許容最大トルクτｊｍａｘのうち加減速運動に利用可能なトルクよりも小さいこと、つまり、時刻ｔ１及びｔ２において、ｊ軸は所望の動作が実現可能であるということを意味する。

逆に、γｊ１＞１、またはγｊ２＞１である場合は、時刻ｔ１またはｔ２においてｊ軸は所望の動作を実現するのに必要なトルクが足らないことを意味する。

次に、ロボットアーム６のｊ軸以外の残りの関節軸について、式（２９）～（３２）により補正値をそれぞれ算出し、得られた補正値の中の最大のものを選んで、第１補正係数γとする。

γ＝ｍａｘ｛γｊ１、γｊ２｝ ・・・（３３）
演算部２３は、加減速時間算出部９で算出された加速時間Ｔａに対して、補正処理部２２で算出された第１補正係数γを乗じて、補正後の加速時間Ｔａ’を求め、加減速処理部４に出力する。なお、Ｔａ’は式（３４）で表わされる。

Ｔａ’＝Ｔａ・γ ・・・（３４）
式（２９）～（３３）より明らかなように、γ≦１の場合は、いずれの関節軸においてもトルク不足は生じていないことを意味する。この場合は、第１補正係数γを乗じて得る補正後の加速時間Ｔａ’は当初の加速時間Ｔａと同じである。

一方、γ＞１の場合は、いずれかの関節軸においてトルク不足が生じていることを意味する。そこで、当初の加速時間Ｔａをγ（＞１）倍した新たな加速時間Ｔａ’（＞Ｔａ）で関節軸を回転させる。これは、関節軸の当初の加速度を１／γ倍に下げる、すなわち、慣性項トルクを１／γ倍に下げるということである。従って、第１補正係数γにより補正されたｊ軸の動力学トルクτｊは、その許容最大トルクτｊｍａｘを超えることはない。

加減速処理部４は、この補正後の加速時間Ｔａ’を用いて、移動指令生成部３から出力された移動指令列に対して上述した加減速処理を行う。

＜＜２軸ロボットにおける第１軸のトルクの各成分の時間変化＞＞
図６Ａ～６Ｃは、図５に示す２軸ロボットにおける第１軸のトルクの各成分の時間変化を示す。図６Ａは、第２軸は動かさずに第１軸のみを上方向（振り上げ方向）に動作させた場合の各トルクの時間変化を示す。また、図６Ｂは、第１軸に加えて第２軸も上方向（振り上げ方向）に動作させた場合の各トルクの時間変化を示す。また、図６Ｃは、第１軸に加えて第２軸も上方向（振り上げ方向）に動作させた場合に、第１補正係数γで加速時間を補正した場合の各トルクの時間変化を示している。なお、図６Ａ～６Ｃにおける第１軸のトルクは、シミュレーションにより算出している。また、図６Ａ～６Ｃにおいて、実線は第１軸に働く動力学トルクτ１を、破線は慣性項トルクτａ１を、点線は動力学トルクτ１における慣性項トルクτａ１以外のトルクτｄ１（残余トルク）を示す。

図６Ａ，６Ｂから明らかなように、第２軸を動作させることにより、その干渉力を受けて、第１軸の動力学トルクτ１は、第２軸を動作させない場合に比べて大きくなっている。

ここで、第１軸に働くトルクの許容最大値を１２００Ｎ・ｍとした場合、図６Ａに示すように、第１軸のみを動作させた場合には、動力学トルクτ１は許容最大値以内に収まっている。しかし、第１軸と第２軸をともに動作させた場合には、図６Ｂに示すように、動力学トルクτ１は許容最大値を超えた状態となってしまう。この場合、実際のロボットでは、トルクの振り切れが発生して、意図した動作が実現できない。

一方、図６Ｃに示すように、上述した手順に従い第１補正係数γ（＞１）で加速時間を補正した場合には、慣性項トルクτａ１を１／γ倍に低減でき、結果として動力学トルクτ１を許容最大値以内に抑えることが可能となる。

以上説明したように、本実施形態によれば、多関節ロボットにおいて、各関節軸での加速に必要なトルクが不足しているかどうかを予め検知できる。そのため、トルクが不足している場合は、不足分に応じた値である第１補正係数γ（＞１）で加速時間Ｔａを補正して、加速時間Ｔａを当初の値よりも大きな時間Ｔａ’とすることで、出力可能な範囲内のトルクで関節軸の回転動作を加速して、ロボットアーム６の動作を制御することができる。このことにより、ロボットの動作プログラムの教示修正の手間を省くことができる。

すなわち、本開示の一態様に係るロボット制御装置によれば、各関節軸の加減速動作に必要なトルクの不足を予め検知できる。そのため、トルクの不足分に応じた値で加減速時間を補正して、この値を当初の値よりも大きくすることで、このロボット制御装置は、出力可能な範囲内のトルクで関節軸を回転駆動させて、ロボットアームの動作を制御することができる。このことにより、ロボットの動作プログラムの教示修正の手間を省くことができる。

なお、本実施形態では、動力学計算におけるロボットアーム６の各定数の誤差は十分に小さく無視できると仮定している。しかし、ロボットアーム６の各定数の誤差が大きく無視できない場合は、式（２９）、（３１）の右辺に１より少し大きい値を乗じて、γｊ１、γｊ２に余裕を持たせてもよい。その場合、式（３３）に示す第１補正係数γが修正され、式（３４）において、補正後の加速時間Ｔａ’が増加する、つまり、慣性項トルクτａｊがより低減されることとなる。

また、本実施形態では、加減速プロファイルを台形形状としたが、これ以外のプロファイルに対しても、式（８）～（１４）に従って、プロファイルの合わせ込みを行うことにより、関節軸のトルクτを抑制することができる。

また、本実施形態では、図３における時刻ｔ１、ｔ２の２箇所で、それぞれの補正値γｊ１、γｊ２を求めたが、ｔ１≒ｔ２であれば、γｊ１またはγｊ２のいずれか一つだけを求めて、第１補正係数γを得てもよい。

また、図３における時刻ｔ１、ｔ２以外の時刻、例えば時刻ｔ１と時刻ｔ２の中間の時刻においても補正値γｊを算出して、得られた補正値の中の最大のものを第１補正係数γと決定してもよい。

なお、本実施形態は減速時にも適用できることは言うまでもない。

すなわち、上述したのと同様の方法で、図４における時刻ｔ４、ｔ５での関節軸の加速度の最大値αｂ、回転速度ω（ｔ４）、ω（ｔ５）及び関節角度θ（ｔ４）、θ（ｔ５）を算出する。さらに、時刻ｔ４、ｔ５での動力学トルクτ（ｔ４）、τ（ｔ５）、慣性項トルクτａ（ｔ４）、τａ（ｔ５）、慣性項トルク以外のトルク（残余トルク）τｄ（ｔ４）、τｄ（ｔ５）を得ることができる。これらのトルクから、式（２９）～（３３）により第１補正係数γを算出して、減速時のトルク不足を解消するための、補正後の減速時間Ｔｂ’を得ることができる。

（実施形態２）
図７は、本実施形態に係るロボット制御装置１０Ａの機能ブロック構成を示す。また、図８はｊ軸における加速時の関節角度、回転速度、回転加速度及びトルクの時間変化を示す。なお、図８において、上から順にｊ軸の関節角度、回転速度、回転加速度及びトルクの時間変化を示している。また、図８の一番下側のグラフにおいて、実線はｊ軸の動力学トルクτｊ（ｔ）を示し、破線は動力学トルクτｊ（ｔ）のうちの慣性項トルクτａｊ（ｔ）を示している。図９Ａは、モータ回転速度とモータ出力トルクとの関係を示す。また、図９Ｂは、移動時間を補正する前のモータ出力トルク及びｊ軸の動力学トルクを示す。また、図９Ｃは、移動時間を補正後のモータ出力トルク及びｊ軸の動力学トルクを示す。なお、図９Ａ～９Ｃにおいて、モータ回転速度及びモータ出力トルクともに、ロボットアーム６の関節軸と同じ値で表している。

なお、実施形態１で説明した構成と同様の箇所については、同一の符号を付して詳細な説明は省略する。

本実施形態と実施形態１とで異なる点は、パラメータ演算部３０と許容速度算出部３１と補正処理部３２（第２補正処理部）及び演算部３３を設けた点にある。

パラメータ演算部３０は、実施形態１で説明したパラメータ演算部２０と同様の機能に加えて、図８に示す時刻ｔ２からｔ３における任意の時刻ｕでの各軸の、加速度αｊ（ｕ）、回転速度ωｊ（ｕ）、関節角度θｊ（ｕ）をそれぞれ算出して出力する。

パラメータ演算部３０は、まず、図８における時刻ｔ１でのｊ軸の加速度αｊ（ｔ１）、回転速度ωｊ（ｔ１）、関節角度θｊ（ｔ１）をそれぞれ式（８）、（１０）～（１２）より求める。

さらに、式（９）、（１０）、（１３）、（１４）より、図８における時刻ｔ２でのｊ軸の各量αｊ（ｔ２）、ωｊ（ｔ２）、θｊ（ｔ２）を求める。

次に、図８における時刻ｕでの各量αｊ（ｕ）、ωｊ（ｕ）、θｊ（ｕ）を次のように求める。

なお、時刻ｕは、時刻ｔ２，ｔ３（＝Ｔａ＞ｔ２）間の任意の時刻であり、
ｔ２＜ｕ＜ｔ３ ・・・（３５）
とする。

図８から明らかなように、時刻ｕにおけるｊ軸の加速度αｊ（ｕ）は、
αｊ（ｕ）＝αａｊ・（ｔ３－ｕ）／（ｔ３－ｔ２） ・・・（３６）
ここで、αａｊは式（１０）に示す通りである。

また、時刻ｕにおけるｊ軸の回転速度ωｊ（ｕ）は、
ωｊ（ｕ）＝ωｊ（ｔ２）＋（αａｊ＋αｊ（ｕ））・（ｕ－ｔ２）／２ ・・・（３７）
そして、時刻ｕにおけるｊ軸の関節角度θｊ（ｕ）は、
θｊ（ｕ）＝θｊ（ｔ２）＋ωｊ（ｔ２）・（ｕ－ｔ２）＋αａｊ・（ｕ－ｔ２）^２／２－αａｊ（ｕ－ｔ２）^３／｛６・（ｔ３－ｔ２）｝ ・・・（３８）
である。

なお、時刻ｔ３においては、
αｊ（ｔ３）＝０ ・・・（３９）
ωｊ（ｔ３）＝ωｊｃ ・・・（４０）
であり、式（３８）より、
θｊ（ｔ３）＝θｊ（ｔ２）＋ωｊ（ｔ２）・（ｔ３－ｔ２）＋αａｊ・（ｔ３－ｔ２）^２／３ ・・・（４１）
となる。

このように、パラメータ演算部３０は、ｊ軸についての、時刻ｔ２における各量αｊ（ｔ２）、ωｊ（ｔ２）、θｊ（ｔ２）、時刻ｕにおける各量αｊ（ｕ）、ωｊ（ｕ）、θｊ（ｕ）、時刻ｔ３における各量αｊ（ｔ３）、ωｊ（ｔ３）、θｊ（ｔ３）をそれぞれ算出して出力する。

なお、後述する動力学トルク及び許容最大回転速度の算出にあたっては、時刻ｕを式（４２）で表わされる値とする。

ｕ＝ｔ２３＝（ｔ２＋ｔ３）／２ ・・・（４２）
動力学トルク算出部２１は、パラメータ演算部３０から出力された、時刻ｔ２，ｔ２３，ｔ３における各量αｊ，ωｊ，θｊと式（１５）とから、時刻ｔ２，ｔ２３，ｔ３における動力学トルクτｊ（ｔ２）、τｊ（ｔ２３）、τｊ（ｔ３）をそれぞれ算出して出力する。

＜＜モータの速度－トルク特性に基づく許容最大回転速度の算出手順＞＞
ところで、関節軸を回転駆動させるモータの種類によっては、図９Ａに示すように、モータを力行モード（モータのトルクの方向と回転方向とが同じ動作モード）で動作させる場合、ある回転速度から出力可能最大トルク（出力可能トルク）が低下し始めるものがある。

これは、モータの回転速度の上昇とともに誘起電圧が上昇し、モータに電流を流すための電圧が低下するからである。

以下、図９Ａに示す曲線を速度－トルク特性またはＳ－Ｔ曲線と呼ぶことがある。

許容速度算出部３１は、図９Ａに示すモータの速度－トルク特性に基づいて、トルクに対する許容最大回転速度を算出し出力する。

図９Ａに示す曲線は、ｊ軸が出力可能な、あるトルクτｊに対する最大回転速度と見ることができる。なお、図９Ａに示す曲線上の点ｑ１、ｑ２を結ぶ区間は１次直線で近似している。

この最大回転速度は、トルクτｊの関数として表すことができ、これを許容最大回転速度ωｐ（τｊ）とすれば、ωｐ（τｊ）は式（４３）、（４４）で表わされる。

すなわち、０≦τｊ≦τｅにおいて、
ωｐ（τｊ）＝ωｊｍａｘ ・・・（４３）
また、τｅ＜τｊ≦τｍａｘにおいて、
ωｐ（τｊ）＝ωｊｍａｘ－τｊ・（ωｊｍａｘ－ωｊｆ）／（τｊｍａｘ－τｊｅ） ・・・（４４）
ここで、ωｊｆはトルクτｊが低下し始める回転速度、τｊｅは最大回転速度ωｊｍａｘにおいて出力可能なトルクである（図９Ａ参照）。

なお、回転方向が負方向の場合は、ωｊｍａｘ、ωｊｆ、τｊｍａｘ、τｊｅのいずれも負値を採る。

許容速度算出部３１は、動力学トルク算出部２１から出力された、時刻ｔ２，ｔ２３，ｔ３における動力学トルクτｊ（ｔ２）、τｊ（ｔ２３）、τｊ（ｔ３）より、式（４３）、（４４）を用いて、時刻ｔ２，ｔ２３，ｔ３における許容最大回転速度ωｐ（τｊ（ｔ２））、ωｐ（τｊ（ｔ２３））、ωｐ（τｊ（ｔ３））を算出して出力する。

＜＜第２補正係数の算出手順＞＞
補正処理部３２（第２補正処理部）は、パラメータ演算部３０から出力された、時刻ｔ２，ｔ２３，ｔ３における回転速度と、許容速度算出部３１から出力された、時刻ｔ２，ｔ２３，ｔ３における許容最大回転速度とに基づいて第２補正係数λを算出して出力する。

具体的な第２補正係数λの算出手順について以下に説明する。

まず、パラメータ演算部３０から、時刻ｔ２，ｔ２３，ｔ３におけるｊ軸の回転速度ωｊ（ｔ２）、ωｊ（ｔ２３）、ωｊ（ｔ３）を出力する。なお、以降の説明において、これらを要求回転速度という。

また、許容速度算出部３１から、時刻ｔ２，ｔ２３，ｔ３におけるｊ軸の許容最大回転速度ωｊｐ（τｊ（ｔ２））、ωｊｐ（τｊ（ｔ２３））、ωｊｐ（τｊ（ｔ３））を出力する。

次に、式（４５）～（４８）より、ｊ軸についての許容最大回転速度に対する要求回転速度の比率λｊを算出する。

λｊ（ｔ２）＝ωｊ（ｔ２）／ωｊｐ（τｊ（ｔ２）） ・・・（４５）
λｊ（ｔ２３）＝ωｊ（ｔ２３）／ωｊｐ（τｊ（ｔ２３）） ・・・（４６）
λｊ（ｔ３）＝ωｊ（ｔ３）／ωｊｐ（τｊ（ｔ３）） ・・・（４７）
λｊ＝ｍａｘ｛λｊ（ｔ２），λｊ（ｔ２３），λｊ（ｔ３）｝ ・・・（４８）
このようにして、各軸についてλｊを求めてから、第２補正係数λを式（４９）より算出する。

λ＝ｍａｘ｛λｊ｝ ・・・（４９）
ここで、
ｉｆ λ＜１ ⇒ λ＝１ ・・・（５０）
として算出する。

＜＜移動時間の補正手順＞＞
以下、図９Ａ～図９Ｃを用いて、第２補正係数λによるロボットアーム６の移動時間の補正手順について説明する。

図９Ｂに示す曲線ａは、図８に示すｊ軸の回転速度ωｊ（ｔ）を横軸に、ｊ軸の動力学トルクτｊ（ｔ）を縦軸にして表したものである。以下、曲線ａをｊ軸の動力学トルク曲線という。また、図９Ｂに示す曲線ｂは、図９Ａと示すのと同じ曲線、つまり、ｊ軸のモータのＳ－Ｔ曲線である。

図９Ｂには、時刻ｔ２，ｔ２３，ｔ３に対応する、ｊ軸の動力学トルク曲線上の点［ωｊ（ｔ），τｊ（ｔ）］（ｔ＝ｔ１、ｔ２３、ｔ３）を”○”で示し、また、ｊ軸のＳ－Ｔ曲線上の点［ωｊｐ（τｊ（ｔ）），τｊ（ｔ）］（ｔ＝ｔ２、ｔ２３、ｔ３）を”●”で示している。

ｊ軸の動力学トルク曲線はｊ軸のＳ－Ｔ曲線の内側にあるのが望ましく、その場合は、ｊ軸の回転動作時に必要なトルクが確保される。

このとき、式（４５）～（４７）で表わされるλｊ（ｔ２）、λｊ（ｔ２３）、λｊ（ｔ３）の値はいずれも１より小さくなる。

一方、図９Ｂに示すケースでは、ｊ軸の動力学トルク曲線上の点［ωｊ（ｔ２），τｊ（ｔ２）］及び［ωｊ（ｔ３），τｊ（ｔ３）］は、ｊ軸のＳ－Ｔ曲線よりも内側にあるが、ｊ軸のＳ－Ｔ曲線における区間［ｑ１，ｑ２］において、ｊ軸の動力学トルク曲線の一部の区間がｊ軸のＳ－Ｔ曲線の外側に出ている。例えば、ｊ軸の動力学トルク曲線上の点［ωｊ（ｔ２３），τｊ（ｔ２３）］は、ｊ軸のＳ－Ｔ曲線よりも外側にある。

これは、ｊ軸について、当該区間で所望の動作に必要なトルクが確保されないということを意味し、式（４５）～（４７）においては、λｊ（ｔ２）とλｊ（ｔ３）は１より小さいが、λｊ（ｔ２３）は１より大きいということになる。

このように、上述した特性を有するモータにｊ軸が連結されている場合に、回転速度によっては動作に必要なトルクが不足する場合がある。

そこで、本実施形態では、第２補正係数λを用いてロボットアーム６の移動時間を補正することでこの課題を解決するようにしている。

まず、ｊ軸の動力学トルク曲線をｊ軸のＳ－Ｔ曲線より内側にするには、例えば、ｊ軸の目標回転速度ωｊｃを低下させればよく、図９Ｂに示す場合であれば、低下後の目標回転速度ωｊｃ’を、
ωｊｃ’＝ωｊｃ／λｊ（ｔ２３） ・・・（５１）
となるようにすればよい。

図９Ｃに示す曲線ｃは、このようにして得られたｊ軸の動力学トルク曲線である。なお、図９Ｃに示す曲線ｄ，ｅは、それぞれ、図９Ｂに示す曲線ａ，ｂと同じである。

目標回転速度ωｊｃを式（５１）に示すように低下させるには、移動時間算出部７が出力する移動時間Ｔｍを大きくしてＴｍ’とし、これを、
Ｔｍ’＝Ｔｍ・λｊ ・・・（５２）
とすればよい。

また、λｊが１より大きくなるのは１軸だけとは限らない。そこで、式（４９）に示すように、全関節軸に関し算出されたλｊの中から値の最大のものを第２補正係数λとする。

演算部３３は、移動時間算出部７からの移動時間Ｔｍに対して、式（５３）に示すように、補正処理部３２から出力された第２補正係数λを乗じて、補正後の移動時間Ｔｍ’を求め、移動指令生成部３に渡す。移動指令生成部３はこの補正後の移動時間Ｔｍ’を用いて移動指令を生成する。

Ｔｍ’＝Ｔｍ・λ ・・・（５３）
ここで、式（４９）、（５０）から明らかなように、式（５３）に代入される第２補正係数λは１以上の値となる。

以上説明したように、本実施形態によれば、いずれの軸においても、その動力学トルク曲線を当該軸のＳ－Ｔ曲線の内側に収めることができる。よって、関節軸を駆動するモータとして、高速域でトルクが低下する速度－トルク特性を有したモータを備えた場合においても、関節軸を駆動するのに必要なトルクの不足を予め検知できる。従って、そのトルクの不足分に応じた値で移動時間を補正して、移動時間を当初の値よりも大きくすることで、出力可能な範囲内のトルクで関節軸を回転駆動させて、ロボットアームの動作を制御することができる。このことにより、ロボットの動作プログラムの教示修正の手間を省くことができる。

すなわち、本開示の一態様に係るロボット制御装置によれば、関節軸を駆動するモータとして高速域でトルクが低下する速度－トルク特性を有したモータを備えた場合において、関節軸を動作させるのに必要なトルクの不足を予め検知できる。そのため、そのトルクの不足分に応じた値で移動時間を補正して、移動時間を当初の値よりも大きくすることで、このロボット制御装置は、出力可能な範囲内のトルクで関節軸を回転駆動させて、ロボットアームの動作を制御することができる。このことにより、ロボットの動作プログラムの教示修正の手間を省くことができる。

なお、本実施形態では、時刻ｔ２とｔ３の間の時刻ｔ２３を１点のみ選択してλｊ（ｔ２３）を求めたが、ｔ２～ｔ３の間をＮ等分した各時刻におけるλｊ（ｕ）を求めて、第２補正係数λを求めてもよい。

また、本実施形態では、式（４２）に示すように、時刻ｔ２３を時刻ｔ２とｔ３の間の中間時刻としたが、時刻ｔ２とｔ３の間を適当な比率で分割した時刻としてもよい。

また、図９Ａ～９ＣにおけるＳ－Ｔ曲線の区間［ｑ１，ｑ２］を直線で近似したが、曲線で近似してもよい。あるいは、区間［ｑ１，ｑ２］の間の複数の代表点をテーブル値として持たせて、その隣り合う２点間を直線または曲線で補間して、区間［ｑ１，ｑ２］の間の任意の点を求めてもよい。さらには、Ｓ－Ｔ曲線の区間［ｑ１，ｑ２］を実際のそれよりも内側、つまり、座標原点寄りに設けてもよい。

また、式（４５）～（４７）の左辺に１より少し大きい値を乗じて、λｊ（ｔ２）、λｊ（ｔ２３）、λｊ（ｔ３）にマージンを設けてもよい。その場合、式（４９）に示す第２補正係数λが修正され、式（５３）において、補正後の移動時間Ｔｍ’が増加する、つまり、動力学トルクτｊがより低減されて、ｊ軸の動作に必要なトルクが確保されることとなる。

なお、実施形態１に示す加減速時間の補正がなされる場合は、この加減速時間の補正を先に行ってから、実施形態２に示す移動時間の補正がなされる。

また、実施形態１に示す加減速時間の補正がなされない場合においても、本実施形態が適用できることは言うまでもない。

本開示の一態様に係るロボット制御装置は、ロボットアームを移動させる際に各関節軸で必要なトルクがその出力可能トルクを超えないように、加減速時間および移動時間を自動調整することができる。従って、ロボットの動作プログラムの教示修正を少なくし、教示作業を効率的に行うことができて、複数軸からなるロボットに適用する上できわめて有用である。

１ 動作プログラム
２ 処理部
３ 移動指令生成部
４ 加減速処理部
５ サーボ
６ ロボットアーム
７ 移動時間算出部
８ 各軸速度算出部
９ 加減速時間算出部
１０ ロボット制御装置
１０Ａ ロボット制御装置
２０ パラメータ演算部
２１ 動力学トルク算出部
２２ 補正処理部（第１補正処理部）
２３ 演算部
３０ パラメータ演算部
３１ 許容速度算出部
３２ 補正処理部（第２補正処理部）
３３ 演算部

Claims (4)

1. モータと前記モータによって回転駆動される関節軸とを複数組有するロボットアームの動作を制御するロボット制御装置であって、
   前記ロボットアームにおける隣り合う作業点間の移動距離と目標移動速度とに基づいて、各前記関節軸に対する移動指令をそれぞれ生成して出力する移動指令生成部と、
   各前記関節軸の回転速度と回転加速度と関節角度に基づいて、各前記関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部と、
   前記移動指令を加減速処理する加減速処理部と、を備え、
   各前記関節軸のうち少なくとも１つの前記関節軸において、前記関節軸の前記動力学トルクのうち前記回転加速度に依存する慣性項トルクが、前記関節軸を加減速動作させるために利用可能な利用可能トルクよりも大きい場合は、各前記関節軸の回転動作における共通の加減速時間が相対的に大きくなるように前記加減速時間を補正し、補正された前記加減速時間に基づいて前記加減速処理部により前記移動指令が前記加減速処理され、
   各前記関節軸のうち少なくとも１つの前記関節軸において、前記関節軸の前記動力学トルクが、前記関節軸の出力可能トルクよりも大きい場合は、前記ロボットアームが前記作業点間の移動距離を前記目標移動速度で移動するのに要する移動時間が相対的に大きくなるように前記移動時間を補正し、補正された前記移動時間に基づいて前記移動指令生成部により前記移動指令が生成されることを特徴とするロボット制御装置。
2. 請求項１に記載のロボット制御装置において、
   前記動力学トルク算出部は、各前記関節軸について、一の前記関節軸の前記回転加速度が最大となる任意の時刻での前記一の関節軸の前記回転速度と前記回転加速度と前記関節角度とに基づいて、前記動力学トルクと、前記慣性項トルクと、前記動力学トルクから前記慣性項トルクを差し引いた残余トルクとを算出し、さらに前記一の関節軸以外の他の前記関節軸における前記動力学トルクと前記慣性項トルクと前記残余トルクとをそれぞれ算出して出力し、
   前記一の関節軸が出力可能な許容最大トルクから前記残余トルクを差し引いた成分である前記利用可能トルクと、前記慣性項トルクとのトルク比を算出し、さらに前記他の関節軸における前記トルク比をそれぞれ算出して、各前記関節軸における前記トルク比の中の最大値を第１補正係数として出力する第１補正処理部と、をさらに備え、
   前記加減速処理部は、前記第１補正係数が１以下の場合は、予め設定された前記加減速時間に基づいて前記移動指令を前記加減速処理し、前記第１補正係数が１を超える場合は、前記加減速時間に前記第１補正係数を乗じて補正された前記加減速時間に基づいて前記移動指令を前記加減速処理し、
   前記加減速処理部で前記加減速処理された前記移動指令に基づいて各前記関節軸を回転駆動させて前記ロボットアームの動作を制御することを特徴とするロボット制御装置。
3. 請求項１に記載のロボット制御装置において、
   前記モータのうち少なくとも一つは、所定の回転速度を超えると出力可能トルクが低下する速度－トルク特性を有しており、
   前記速度－トルク特性の出力可能トルク低下領域において、各前記関節軸について、任意の複数時刻それぞれにおける一の前記関節軸の前記回転速度を要求回転速度として抽出し、さらに前記一の関節軸以外の他の前記関節軸における前記要求回転速度をそれぞれ抽出するパラメータ演算部と、
   前記動力学トルクと前記速度－トルク特性とに基づいて、各前記関節軸について、前記関節軸の前記複数時刻における許容最大回転速度をそれぞれ算出して出力する許容速度算出部と、
   前記複数時刻における前記要求回転速度と前記許容最大回転速度との速度比をそれぞれ算出して、各前記関節軸における前記速度比の最大値を第２補正係数として出力する第２補正処理部と、をさらに備え、
   前記移動指令生成部は、前記第２補正係数が１以下の場合は、前記移動時間に基づいて前記移動指令を生成する一方、前記第２補正係数が１を超える場合は、前記移動時間に前記第２補正係数を乗じて補正された前記移動時間に基づいて前記移動指令を生成し、
   前記移動指令生成部で生成された前記移動指令に基づいて各前記関節軸を回転駆動させて前記ロボットアームの動作を制御することを特徴とするロボット制御装置。
4. 請求項２に記載のロボット制御装置において、
   前記モータのうち少なくとも一つは、所定の回転速度を超えると出力可能トルクが低下する速度－トルク特性を有しており、
   前記速度－トルク特性の出力可能トルク低下領域において、各前記関節軸について、任意の複数時刻それぞれにおける一の前記関節軸の前記回転速度を要求回転速度として抽出し、さらに前記一の関節軸以外の他の前記関節軸における前記要求回転速度をそれぞれ抽出するパラメータ演算部と、
   前記動力学トルクと前記速度－トルク特性とに基づいて、各前記関節軸について、前記関節軸の前記複数時刻における許容最大回転速度をそれぞれ算出して出力する許容速度算出部と、
   前記複数時刻における前記要求回転速度と前記許容最大回転速度との速度比をそれぞれ算出して、各前記関節軸における前記速度比の最大値を第２補正係数として出力する第２補正処理部と、をさらに備え、
   前記移動指令生成部は、前記第２補正係数が１以下の場合は、前記移動時間に基づいて前記移動指令を生成する一方、前記第２補正係数が１を超える場合は、前記移動時間に前記第２補正係数を乗じて補正された前記移動時間に基づいて前記移動指令を生成し、
   前記移動指令生成部で生成された前記移動指令に基づいて各前記関節軸を回転駆動させて前記ロボットアームの動作を制御することを特徴とするロボット制御装置。

Images