(57)【要約】  多関節型のロボットアーム６の動作を制御するロボット制御装置１０は、各関節軸の動力学トルクを算出する動力学トルク算出部２１と、各関節軸の回転加速度が極大となる時点ｔａでの角度移動量Δθａｐと回転速度ωａｐと回転加速度αａｐとが加速時間Ｔａに関連付けられて保存されたテーブル２４を有する記憶部２０と、を備えている。動力学トルク算出部２１は、回転加速度αａｐに対応する慣性項トルクτａｐと残余トルクτｃａｐを算出するとともに、ｊ軸において、慣性項トルクτａｐｊが許容最大トルクτｊｍａｘと残余トルクτｃａｐとの差である利用可能トルクよりも大きい場合は加速時間Ｔａを補正する。
2019215980.tif 000002
【特許請求の範囲】
【請求項１】
モータと該モータによって回転駆動される関節軸とを複数組有するロボットアームの動作を制御するロボット制御装置であって、
各関節軸の回転速度と回転加速度と関節角度に基づいて、各関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部と、
各関節軸の回転加速度が極大及び極小となる時点での各関節軸における関節角度の移動量と回転速度と前記回転加速度の極大値及び極小値とが各関節軸を加減速動作させるための加減速時間に関連付けられたテーブルを有する記憶部と、を少なくとも備え、
前記動力学トルク算出部は、前記テーブル内の値に基づいて、前記回転加速度の極大値及び極小値に対応する動力学トルクを算出するとともに、前記回転加速度の極大値及び極小値に対応する慣性項トルクと、前記動力学トルクから前記慣性項トルクを差し引いた残余トルクとを算出し、
各関節軸のうち少なくとも１つの関節軸において、前記慣性項トルクが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルクよりも大きい場合は、各関節軸の回転動作における共通の加減速時間を補正することを特徴とするロボット制御装置。
【請求項２】
請求項１に記載のロボット制御装置において、
前記ロボットアームにおける隣り合う作業点間距離と目標移動速度とに基づいて、各関節軸に対する移動指令をそれぞれ生成して出力する移動指令生成部と、
一の前記関節軸が出力可能な許容最大トルクから前記残余トルクを差し引いた成分である前記利用可能トルクと、前記慣性項トルクとのトルク比を算出し、さらに各関節軸における当該トルク比をそれぞれ算出して、各関節軸における前記トルク比の中の最大値を補正係数として出力する補正処理部と、
前記補正係数が１以下の場合は、予め設定された前記加減速時間に基づいて前記移動指令を加減速処理し、前記補正係数が１を超える場合は、前記加減速時間に前記補正係数を乗じて補正された加減速時間に基づいて前記移動指令を加減速処理する加減速処理部と、をさらに備え、
前記加減速処理部で加減速処理された前記移動指令に基づいて各関節軸を回転駆動させて前記ロボットアームの動作を制御することを特徴とするロボット制御装置。
【請求項３】
請求項１または２に記載のロボット制御装置において、
前記テーブルに保存された値は、移動指令に基づいて算出されることを特徴とするロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの動作制御に係るロボット制御装置に関する。
【背景技術】
【０００２】
モータによって回転駆動される関節軸を複数組備えた多関節型ロボットにおいて、ロボットアームを教示点間で高速移動させる場合、急激な加減速によって関節軸を駆動するモータに過大な負荷がかかることがある。【０００３】
特許文献１には、各関節軸の最大または最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めたトルクが目標トルクになるように、基準関節軸の最大または最小加速度を修正する技術が開示されている。このようにすることで、加減速時のロボットアームの振動を抑えて、スムーズな加減速動作を行うことができる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１０−２６９３８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、関節軸に発生するトルクは、当該関節軸に連結されたモータの出力トルクに依存し、当該モータの許容最大トルクによって制限される。すなわち、複数の関節軸が同時に動作する場合に、関節軸を動作させるのに必要なトルクが不足する場合がある。【０００６】
例えば、加速時にトルクが不足したまま関節軸を回転させると、当該関節軸だけが遅れた動作となりアーム先端の軌道が予想外の軌道をたどる場合がある。すなわち、ロボットが周辺物と接触する恐れがある。また、減速時に必要なトルクが不足すると、目標教示点でオーバシュートする場合がある。その場合も、周辺物との接触の恐れがある。【０００７】
通常、これらのような場合は、ロボットアームの動作プログラムの修正を行う。具体的には、加減速時間を調整する命令を加えたり、目標動作速度を変更したりして対応する。【０００８】
しかし、この修正作業は、実際に動作プログラムを運転してカットアンドトライで行う必要があり、工数を要していた。【０００９】
また、特許文献１に示す構成では、各関節軸の最大または最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めたトルクが目標トルクになるように、基準関節軸の最大または最小加速度を修正している。【００１０】
しかし、この場合は、ロボットアームの動作は安定するものの、ロボットアームの動作時間であるタクトは増加してしまう。【００１１】
本発明は、かかる点に鑑みてなされたもので、ロボットのタクトが増加するのを抑えつつ、関節軸を動作させるのに必要なトルクがモータの許容最大トルクを超えないように、加減速時間を自動調整する仕組みを備えたロボット制御装置を提供することにある。
【課題を解決するための手段】
【００１２】
上記目的を達成するため、本発明に係るロボット制御装置は、モータと該モータによって回転駆動される関節軸とを複数組有するロボットアームの動作を制御するロボット制御装置であって、各関節軸の回転速度と回転加速度と関節角度に基づいて、各関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部と、各関節軸の回転加速度が極大及び極小となる時点での各関節軸における関節角度の移動量と回転速度と前記回転加速度の極大値及び極小値とが各関節軸を加減速動作させるための加減速時間に関連付けられたテーブルを有する記憶部と、を少なくとも備え、前記動力学トルク算出部は、前記テーブル内の値に基づいて、前記回転加速度の極大値及び極小値に対応する動力学トルクを算出するとともに、前記回転加速度の極大値及び極小値に対応する慣性項トルクと、前記動力学トルクから前記慣性項トルクを差し引いた残余トルクとを算出し、各関節軸のうち少なくとも１つの関節軸において、前記慣性項トルクが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルクよりも大きい場合は、各関節軸の回転動作における共通の加減速時間を補正することを特徴とする。【００１３】
この構成によれば、各関節軸の動作に必要なトルクの不足を予め検知でき、トルクの不足分に応じた値で加減速時間または移動時間を補正して、これらの値のいずれかを当初の値よりも大きくすることで、出力可能な範囲内のトルクで関節軸を回転駆動させて、ロボットアームの動作を制御することができる。このことにより、ロボットの動作プログラムの教示修正の手間を省くことができる。また、実際の加速度が極大または極小となる時刻、つまり、関節軸での加減速動作が最も急激に行われる時刻での動力学トルクに基づいて、加減速時間を補正するため、ロボットアームを安定して動作させつつ、ロボットアームの動作時間であるタクトが増加するのを抑えることができる。
【発明の効果】
【００１４】
以上説明したように、本発明に係るロボット制御装置によれば、ロボットアームを動作させるときの各関節軸で必要なトルクがその出力可能トルクを超えないように、加減速時間を自動調整することができ、ロボットの動作プログラムの教示修正の手間を省くことができる。また、ロボットのタクトが増加するのを抑制することができる。
【図面の簡単な説明】
【００１５】
【図１】図１は、本発明の一実施形態に係るロボット制御装置の機能ブロック図である。
【図２】図２は、異なる関節軸での関節角度、速度及び加速度の時間変化を示す図である。
【図３】図３は、２軸ロボットの構成を示す模式図である。
【図４】図４は、ｊ軸における加速時の速度、加速度及び動力学トルクの時間変化を示す図である。
【図５】図５は、本発明の一実施形態に係る加減速時間の補正手順を示すフローチャートである。
【図６Ａ】図６Ａは、加減速時間が１００ｍｓｅｃ場合の加速度、速度、角度移動量の時間変化を示す図である。
【図６Ｂ】図６Ｂは、加減速時間が１５０ｍｓｅｃ場合の加速度、速度、角度移動量の時間変化を示す図である。
【図６Ｃ】図６Ｃは、加減速時間が２００ｍｓｅｃ場合の加速度、速度、角度移動量の時間変化を示す図である。
【図７】図７は、テーブルの構成を示す図である。
【図８】図８は、角度移動量、速度及び加速度を導出するためのブロック線図である。
【図９】図９は、ｊ軸における加減速時の関節角度、速度、加速度及び動力学トルクの時間変化を示す図である。
【発明を実施するための形態】
【００１６】
以下、本発明の実施形態を図面に基づいて詳細に説明する。以下の好ましい実施形態の説明は、本質的に例示に過ぎず、本発明、その適用物或いはその用途を制限することを意図するものでは全くない。
【００１７】
（実施形態）
［ロボット制御装置の機能ブロック構成］
図１は、本実施形態に係るロボット制御装置の機能ブロック構成を示し、図２は、異なる関節軸であるｉ軸及びｊ軸における速度及び加速度の時間変化を示し、図３は、２軸ロボットの模式図を示す。なお、以降の説明において示す添字ｉ，ｊは整数で１≦ｉ，ｊ≦ｎかつｉ≠ｊであり、ｎはロボットアーム６に含まれる関節軸の個数である。なお、以降の説明において、回転軸であるｉ軸やｊ軸の回転速度や回転加速度を単にｉ軸やｊ軸の速度や加速度と呼ぶことがある。
【００１８】
ロボット制御装置１０は、動作プログラム１と、処理部２と、移動指令生成部３と、加減速処理部４と、サーボ５と、移動時間算出部７と、各軸速度算出部８と、加減速時間算出部９と、記録部２０と、動力学トルク算出部２１と、補正処理部２２と、演算部２３とを機能ブロックとして備えている。なお、図１に示す機能ブロックは、記録部２０に保存された、または外部から読み込まれたソフトウェアをロボット制御装置１０に設けられた図示しないＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）等の演算装置上で実行することで実現される。このソフトウェアに動作プログラム１が含まれていてもよい。また、説明の便宜上、ロボットアーム６に組み込まれた関節軸と、当該関節軸に連結されて関節軸を回転駆動するモータとで構成される複数の組については図示を省略している。
【００１９】
動作プログラム１には、ロボットアーム６の教示点位置と、教示点間を結ぶ軌跡の形状にあたる補間形態と、目標移動速度とが記録されている。教示点位置は、直交空間上の作業点位置またはロボットアーム６の関節角度である。目標移動速度は、教示点間を指示された補間形態で移動するときの目標速度である。
【００２０】
補間形態としては、直交空間上の作業点を、直線や円弧等の特定の形状を描くように補間するＣＰ（ｃｏｎｔｉｎｕａｓ  ｐａｔｈ）補間と、各関節軸を一定の速度で動かし、作業点の軌跡の形状は問わないＰＴＰ（ｐｏｉｎｔ  ｔｏ  ｐｏｉｎｔ）補間とがあるが、以降は、ＰＴＰ補間を行う場合について説明する。ＰＴＰ補間は教示点間を最短時間で移動する場合に適用する補間形態であり、ロボットアーム６の関節軸のいずれかもしくは複数が、その最大速度で回転するように動作させることが多い。
【００２１】
処理部２は、動作プログラム１に記録されている教示点位置が直交空間上の値である場合に、それをロボットアーム６の各関節軸の関節角度に変換し、また、教示点位置がロボットアーム６の関節角度の場合は直交空間上の値に変換する。なお、以降の説明において、動作プログラム１上に記録された任意の教示点をＰａとし、その次の教示点をＰｂとする。
【００２２】
移動指令生成部３は、隣り合う教示点間の移動量と目標移動速度とに基づいて移動指令を生成して出力する。この移動指令は単位時間当たりの関節角移動量（以下、単に角度移動量と呼ぶことがある。）である。各関節の角度移動量をまとめて１組とし、これを１つの移動指令とする。よって、移動指令はベクトル量である。移動指令生成部３は、後述する移動時間に対応する複数組の移動指令を時系列的に出力する。この時系列的に出力される複数組の移動指令を移動指令列と言う。なお、移動指令生成部３が出力する移動指令列には後述する加減速処理はなされていない。
【００２３】
加減速処理部４は、移動指令生成部３からの移動指令列に対して加減速処理を行って、加減速した移動指令を出力する。加減速処理部４は、後述する加減速時間算出部９が出力する加速時間で目標移動速度に達するように、移動指令生成部３からの移動指令列を加減速処理する。具体的には、図２に示すように、各関節軸における加速度を時間変化させて所定の目標移動速度に到達するようにする。また、教示点Ｐｂ手前で目標移動速度から減速を始めて、加減速時間算出部９が出力する減速時間が経過した後に教示点Ｐｂに到達するように、移動指令生成部３からの移動指令列を減速処理する。なお、以降の説明において、加速度の時間変化波形を加速プロファイルまたは減速プロファイルあるいは加減速プロファイルと呼ぶことがある。なお、図２に示すように、加減速プロファイルは、ロボットアーム６の振動を誘発しないように時間に対して速度がＳ字状に変化する形状とする。なお、加減速処理は後述する演算部２３が出力する補正後の加速時間または減速時間に基づいて行い、具体的な加減速処理の手順については後述する。
【００２４】
サーボ５は、加減速処理部４で処理された移動指令列に基づいて、各関節軸に連結されたモータを回転駆動させてロボットアーム６の動作を制御する。
【００２５】
移動時間算出部７は、ロボットアーム６の先端が教示点Ｐａから教示点Ｐｂへ加減速せずに目標移動速度で移動するのに要する所要時間を算出して出力する。当該所要時間が移動時間に相当する。
【００２６】
各軸速度算出部８は、ＰＴＰ補間時の各関節軸の目標速度を算出し出力する。
【００２７】
加減速時間算出部９は、教示点Ｐａから目標速度に加速するときの加速時間、教示点Ｐｂ手前で目標速度から減速して教示点Ｐｂに到達するときの減速時間を求め出力する。
【００２８】
記憶部２０は、後述するテーブル２４（図７参照）や動作プログラム１を保存しており、ロボット制御装置１０の各部からの要求に従って保存されたデータを対応するブロックに送信する。
【００２９】
動力学トルク算出部２１は、テーブル２４内のデータに基づいて、所定の時刻における各関節軸の動力学トルクをそれぞれ算出し出力する。
【００３０】
補正処理部２２は、動力学トルク算出部２１で算出された、所定の時刻における動力学トルク、慣性項トルク及び慣性項トルク以外のトルクに基づいて、後述する第１及び第２補正係数γａ，γｂを算出し出力する。
【００３１】
演算部２３は、加減速時間算出部９で算出された加速時間Ｔａまたは減速時間Ｔｂに対して、補正処理部２２で算出された第１または第２補正係数γａ，γｂを乗じて、補正後の加速時間Ｔａ’または減速時間Ｔｂ’を求め、加減速処理部４に出力する。
【００３２】
テーブル２４は、ロボットアーム６の各関節軸が駆動するときに、極大または極小となる実加速度及びそのときの速度と角度移動量が加減速時間に関連付けて配列されたテーブルである。
【００３３】
なお、加速時間は、例えば、教示点Ｐａから加速を始めて目標移動速度に達するまでの時間をいい、減速時間とは、例えば、教示点Ｐｂの手前で減速を始めて移動速度が零になるまでの時間をいい、加減速時間とは、これら両者を合わせた時間をいう。また、各関節軸について、各関節軸の許容最大加速度で目標速度まで加速したときの加速時間（各軸の最短加速時間と呼ぶことがある）を求め、これら各関節軸における最短加速時間の中で値が最大のものを選び、これを全軸共通の加速時間とする。図２に示すように、全ての関節軸はこの全軸共通の加速時間で加速する。また、減速時には、各関節軸における最短減速時間の中で値が最大のものを選び、これを全軸共通の減速時間とする。加速時と同様に、全ての関節軸はこの全軸共通の減速時間で減速する（図２参照）。
【００３４】
従って、テーブル２４における加減速時間は全軸共通の加減速時間である。なお、テーブル２４の構成及び作成手順については後述する。
【００３５】
［加減速時間の補正手順］
図４は、ｊ軸における加速時の速度、加速度及び動力学トルクの時間変化を示す。なお、図４では、ｊ軸の速度に相当する移動指令生成部３から出力された移動指令に基づいて算出された加速度やトルクを示している。また、図４において、実線は、移動指令に基づいて算出された動力学トルクを示し、破線は、実際にｊ軸に加わる動力学トルクを示す。
【００３６】
図４に示すように、動力学トルクは、加速度の上昇に伴い増加し、時刻ｔ２で最大値になった後、加速度の低下に伴い減少する。ここで、ｊ軸が出力可能な最大トルクをｊ軸に関する許容最大トルクτｊｍａｘとすると、時刻ｔ１からｔ３までの期間で、移動指令に基づいて算出された動力学トルクが許容最大トルクτｊｍａｘを上回っている。これは、関節軸を駆動させるトルクが足らないことを意味する。このような場合には、例えば、加速時間を増加させる補正を行って、動力学トルクが許容最大トルクτｊｍａｘ以下になるように調整する必要がある。
【００３７】
しかし、ロボットアーム６が接続された実際のロボット制御装置１０では、サーボ制御系の遅れ等により、加減速時の移動指令よりも実際の速度の時間変化（以下、速度プロファイルと呼ぶことがある。）が小さくなることが多く（図４中の破線参照）、これに伴い、破線で示す実際の動力学トルクが指令速度に基づいて算出された動力学トルクよりも小さくなる。
【００３８】
よって、図４に示すように、実際には動力学トルクが許容最大トルクτｊｍａｘを超えていないにも関わらず、加速時間を増加させる補正が必要と判断されてしまうことがある。この場合、タクトが増加するため、ロボットアーム６を用いた作業の生産性が低下するという問題があった。
【００３９】
そこで、本実施形態では、加速度が極大となるときの角度移動量や実速度を用いて動力学トルクを算出し、この動力学トルクに基づいて加減速時間の補正を行うことで、上記の不具合を解消している。
【００４０】
＜ＳＴＥＰ１：加減速時間の算出＞
図５は、本実施形態に係る加減速時間の補正手順を示し、以降で説明する５つのステップを順次実行して加減速時間の補正を行う。
【００４１】
まず、対象となる系において、加速時間Ｔａ及び減速時間Ｔｂを算出する（ＳＴＥＰ１）。
【００４２】
ＰＴＰ補間の場合、教示点Ｐａ、Ｐｂでの関節角度をそれぞれθａ、θｂとし、教示点間に対応する作業点間の移動時間をＴｍ、補間の単位時間をＴｈとするとき、移動指令Δθは、式（１）で表わされる。なお、移動時間Ｔｍは移動時間算出部７で算出される。
【００４３】
Δθ＝（θｂ−θａ）・Ｔｈ／Ｔｍ  ・・・（１）
【００４４】
ここで、θａ、θｂ各関節軸の関節角度を要素とするベクトル量である。また、Δθは各関節軸の角度移動量を要素とするベクトル量である。
【００４５】
教示点Ｐａ，Ｐｂに対応する作業点間の移動距離をＬ、目標移動速度をＶとするとき、移動時間Ｔｍは、式（２ａ）で表わされる。
【００４６】
Ｔｍ＝Ｌ／Ｖ  ・・・（２ａ）
【００４７】
なお、ＰＴＰ補間の場合、移動距離Ｌは、作業点間の直線距離に相当する。また、関節軸を加速または減速回転させてロボットアーム６を動作させる際に、自身の許容最大速度を超える関節軸がある場合は、当該関節軸の速度が許容最大速度に収まるように式（２ａ）から算出された移動時間Ｔｍは修正される。
【００４８】
また、ＰＴＰ補間において、教示点Ｐａ，Ｐｂに対応する作業点間を最短時間で移動する場合は、各関節軸の移動角度をその最大速度で除して得る値（当該軸の最短移動時間と呼ぶことがある）の中の最大値を移動時間Ｔｍとする。
【００４９】
すなわち、複数の関節軸のうちｊ軸の移動角度をΔθｊ、最大速度をωｊｍａｘとするとき、移動時間Ｔｍは式（２ｂ）で表わされる。
【００５０】
Ｔｍ＝ｍａｘ｛Δθｊ／ωｊｍａｘ｝  ・・・（２ｂ）
【００５１】
ここで、ｍａｘ｛｝は括弧内の要素（１個もしくは複数個）のうちの最大のものを選択することを意味する。
【００５２】
また、各軸速度算出部８で算出されたｊ軸に関する目標速度ωｊｃは、式（３）で表わされる。
【００５３】
ωｊｃ＝（θｂｊ−θａｊ）／Ｔｍ  ・・・（３）
【００５４】
ここで、θａｊ、θｂｊはそれぞれ、隣り合う教示点Ｐａ、Ｐｂでのｊ軸の関節角度であり、Ｔｍは上述した移動時間である。よって、目標速度ωｃは各関節軸の目標速度を要素とするベクトル量である。なお、ωｊｃはＰＴＰ補間前に予め値を求めることができる。
【００５５】
ＰＴＰ補間の場合は、各軸速度算出部８からの各関節軸の目標速度に基づいて、最短の加減速時間を算出し出力する。
【００５６】
まず、ｊ軸についての最短加速時間Ｔａｊを次のように求める。
【００５７】
Ｔａｊ＝｜ωｊｃ｜／αａｊｍａｘ  ・・・（４）
【００５８】
ここで、ωｊｃは各軸速度算出部８で算出されたｊ軸の目標速度、αａｊｍａｘは、教示点Ｐａにおいてｊ軸だけを単独で加速する場合のｊ軸の許容最大加速度である。
【００５９】
教示点Ｐａからロボットアーム６を移動させるときの加速時間Ｔａは、式（５）で表わされる。
【００６０】
Ｔａ＝ｍａｘ｛Ｔａｊ｝  ・・・（５）
【００６１】
ここで、ｍａｘ｛｝は括弧内の複数の要素から値が最大のものを選択することを意味する。
【００６２】
ｊ軸についての最短減速時間Ｔｂｊについても同様に式（６）で表わされる。
【００６３】
Ｔｂｊ＝｜ωｊｃ｜／αｂｊｍａｘ  ・・・（６）
【００６４】
ここで、αｂｊｍａｘは、教示点Ｐｂにおいてｊ軸だけを単独で減速する場合のｊ軸の許容最大加速度である。
【００６５】
従って、教示点Ｐｂへロボットアーム６を移動させるときの減速時間Ｔｂは、式（７）で表わされる。
【００６６】
Ｔｂ＝ｍａｘ｛Ｔｂｊ｝  ・・・（７）
【００６７】
＜ＳＴＥＰ２：テーブル作成＞
次に、実際の加速度が極大となるときの角度移動量や速度を求め、これらに基づいてテーブル２４を作成する（ＳＴＥＰ２）。
【００６８】
図６Ａは、加減速時間が１００ｍｓｅｃの場合の、図６Ｂは、加減速時間が１５０ｍｓｅｃの場合の、図６Ｃは、加減速時間が２００ｍｓｅｃの場合の加速度、速度、角度移動量の時間変化をそれぞれ示す。図６Ａ〜６Ｃにおいて、移動指令及びそれに基づいた加速度を破線で示し、実際の値を実線で示している。なお、図６Ａでは、１００ｍｓｅｃの間に移動指令を０から１まで増加させたときの加速度、速度、角度移動量を示している。また、１００ｍｓｅｃの間に移動指令を１から０まで減少させたときの角度移動量もあわせて図示している。同様に、図６Ｂでは、１５０ｍｓｅｃの間に移動指令を０から１まで増加させたときの加速度、速度、角度移動量を、図６Ｃでは、２００ｍｓｅｃの間に移動指令を０から１まで増加させたときの加速度、速度、角度移動量をそれぞれ示している。
【００６９】
図６Ａに示すように、加速中の回転軸の速度及び加速度は、移動指令及びそれに基づく加速度よりも小さくなっている。これは、前述したように、サーボ制御系の応答遅れ等に基づく。従って、実際の加減速プロファイルは台形形状とならずに、時刻ｔａで極大値を有する形状となる。時刻ｔａにおける実際の加速度α１００，速度ω１００、角度移動量Δθａ１００を加減速時間１００ｍｓｅｃに関連付けて配列する。また、図示しないが、１００ｍｓｅｃの間に移動指令を１から０まで減少させたときに、実際の加速度が極大となる時刻を時刻ｔｂ（図示せず）とし、時刻ｔｂにおける関節角度θｂまでの角度移動量Δθｂ１００も加減速時間１００ｍｓｅｃに関連付けて加速度α１００等と同様に配列する。
【００７０】
図６Ｂ，６Ｃに示すように、加減速時間を変化させて、それぞれ、加速度α１００に対応する値α１５０，α２００等を求めて、上記と同様に加減速時間に関連付けて配列する。このようにして図７に示すテーブル２４を作成し、テーブル２４は記憶部２０に保存される。
【００７１】
なお、加速度α１００，速度ω１００、角度移動量Δθａ１００等は、実際のロボットアーム６の回転軸に各種センサを取付けて実測して求めてもよいし、他の方法で求めてもよい。例えば、ロボットアーム６とロボット制御装置１０とで構成されるサーボ制御系のモデル及び各パラメータを用いて、図８に示すブロック線図に基づいて、シミュレーションによりこれらの値を導出するようにしてもよい。つまり、加減速時間を設定した後に、シミュレーションにより速度、加速度の時間変化を導出し、導出された加速度が極大となる値及びその時刻での速度、角度移動量を求めてテーブル２４に組み込むようにする。なお、各関節軸の駆動に用いられる制御定数は、全軸共通であるため、テーブル２４は１種類だけ準備される。ただし、６軸ロボット等では基本軸と手首軸とで異なる制御定数を用いる場合もある。このような場合は、図７には図示していないが、ロボットアーム６の関節軸のそれぞれについて加減速時間を共通とした上で、各データが取得されてテーブル２４が作成される。また、テーブル２４に含まれるデータの個数は、加減速時間の間隔により適宜決められる。
【００７２】
＜ＳＴＥＰ３：動力学トルク及び慣性項トルクの算出＞
ＳＴＥＰ２で作成したテーブル２４に基づいて、動力学トルク算出部２１で各関節軸の動力学トルク及び慣性項トルクをそれぞれ算出する（ＳＴＥＰ３）。
【００７３】
具体的には、テーブル２４内の以下のパラメータ、加速度が極大となる時刻ｔａにおける加速度αａｐ、速度ωａｐ、関節角度θａｐ、加速度が極小となる時刻ｔｂにおける加速度αｂｐ、速度ωｂｐ、関節角度θｂｐ、また、ロボットアーム６のリンク長、重心位置、重心質量、リンクの慣性モーメント、モータのイナーシャ等の定数（図３参照）から、時刻ｔａ及び時刻ｔｂでの動力学トルクを算出する。
【００７４】
この動力学トルクは、関節角度θの場合に、速度ω及び加速度αで動作するのに必要な各関節軸のトルクであり、関節角度θ、速度ω、加速度αは、各関節軸の成分からなるベクトル量である。
【００７５】
また、時刻ｔにおける動力学トルクは式（１５）で表わされる。
【００７６】
τ（ｔ）＝Ｈ（θ（ｔ））・α（ｔ）＋Ｄ・ω（ｔ）＋ｂ（ω（ｔ））＋ｃ（θ（ｔ））  ・・・（１５）
【００７７】
ここで、Ｈ（θ（ｔ））は関節角度θ（ｔ）における慣性行列であり、Ｄは粘性係数からなる粘性行列である。さらに、ｂ（ω（ｔ））は、時刻ｔでの関節角度θ（ｔ）における速度ω（ｔ）による遠心力、コリオリ力の和、ｃ（ω（ｔ））は、関節角度θ（ｔ）における重力トルクである。また、なお、式（１５）は、ラグランジュ法またはニュートンオイラー法によりロボットアーム６の運動方程式を立てて解くことで導出できる。
【００７８】
ここで、式（１５）の右辺を、第１項とそれ以外の部分とに分けて、式（１６）で表わされる形式とする。なお、式（１５）、（１６）の右辺の第１項は加速度のみに依存する成分（以下、慣性項トルクという。）である。また、式（１６）の第２項は速度ω（ｔ）に依存する成分（以下、速度依存項トルクという。）、第２項は重力に依存する成分（以下、重力項トルクという。）である。なお、以降の説明において、τｃ（ｔ）を残余トルクと呼ぶことがある。
【００７９】
τ（ｔ）＝τａ（ｔ）＋τｄ（ｔ）＋τｅ（ｔ）＝τａ（ｔ）＋τｃ（ｔ）  ・・・（１６）
【００８０】
ただし、
τａ（ｔ）＝Ｈ（θ（ｔ））・α（ｔ）  ・・・（１７）
τｄ（ｔ）＝Ｄ・ω（ｔ）＋ｂ（ω（ｔ））  ・・・（１８）
τｅ（ｔ）＝ｃ（（ω（ｔ））  ・・・（１９）
τｃ（ｔ）＝τｄ（ｔ）＋τｅ（ｔ）  ・・・（２０）
【００８１】
動力学トルク算出部２１では、上述した時刻ｔａにおける加速度αａｐ、速度ωａｐ、関節角度θａｐ、時刻ｔｂにおける加速度αｂｐ、速度ωｂｐ、関節角度θｂｐを式（１６）〜（１９）に代入して、時刻ｔａでの慣性項トルクτａｐ、速度依存項トルクτｄａｐ、重力項トルクτｅａｐ及び時刻ｔｂでの慣性項トルクτｂｐ、速度依存項トルクτｄｂｐ、重力項トルクτｅｂｐをそれぞれ算出する。
【００８２】
例えば、図３に示す２軸ロボットにおいて、第１軸の動力学トルクτ１（ｔ）及び第２軸の動力学トルクτ２（ｔ）は、それぞれ式（２１）、（２２）で表わされる。
【００８３】
τ１（ｔ）＝（Ｈ１１・α１＋Ｈ１２・α２）＋（Ｄ１１・ω１＋ｂ１）＋ｃ１  ・・・（２１）
τ２（ｔ）＝（Ｈ２１・α１＋Ｈ２２・α２）＋（Ｄ２２・ω２＋ｂ２）＋ｃ２  ・・・（２２）
【００８４】
なお、慣性行列Ｈ１１〜Ｈ２２は以下の式（２３）〜（２６）で表わされる。
【００８５】
Ｈ１１＝ｍ１・ｌｇ１２＋ｍ２・Ｌ１２＋ｍ２・ｌｇ２２＋Ｉ１＋Ｉ２＋２・ｍ２・Ｌ１・ｌｇ２・ｃｏｓθ２＋Ｊｍ１  ・・・（２３）
Ｈ１２＝ｍ２・ｌｇ２２＋Ｉ２＋ｍ２・Ｌ１・ｌｇ２・ｃｏｓθ２  ・・・（２４）
Ｈ２１＝ｍ２・ｌｇ２２＋Ｉ２＋ｍ２・Ｌ１・ｌｇ２・ｃｏｓθ２  ・・・（２５）
Ｈ２２＝ｍ２・ｌｇ２２＋Ｉ２＋Ｊｍ２  ・・・（２６）
【００８６】
また、粘性行列Ｄ１１，Ｄ２２は式（２７）、（２８）で表わされる。
【００８７】
Ｄ１１＝ｄ１  ・・・（２７）
Ｄ２２＝ｄ２  ・・・（２８）
【００８８】
また、速度依存項トルクであるｂ１，ｂ２はそれぞれ式（２９）、（３０）で表わされる。
【００８９】
ｂ１＝−ｍ２・Ｌ１・ｌｇ２・（２・ω１＋ω２）・ω２・ｓｉｎθ２  ・・・（２９）
ｂ２＝ｍ２・Ｌ１・ｌｇ２・ω１２・ｓｉｎθ２  ・・・（３０）
【００９０】
また、重力項トルクに対応するｃ１，ｃ２はそれぞれ式（３１）、（３２）で表わされる。
【００９１】
ｃ１＝（ｍ１・ｇ・ｌｇ１＋ｍ２・ｇ・Ｌ１）ｃｏｓθ１＋ｍ２・ｇ・ｌｇ２・ｃｏｓ（θ１＋θ２）  ・・・（３１）
ｃ２＝ｍ２・ｇ・ｌｇ２・ｃｏｓ（θ１＋θ２）  ・・・（３２）
【００９２】
ここで、式（２１）〜（３２）における各符号の内容は以下の通りである。
【００９３】
θ１：時刻ｔにおける第１軸の回転角（紙面における左回りを正方向とする。）
θ２：時刻ｔにおける第２軸の回転角（紙面における左回りを正方向とする。）
ω１：時刻ｔにおける第１軸の回転速度
ω２：時刻ｔにおける第２軸の回転速度
α１：時刻ｔにおける第１軸の回転加速度
α２：時刻ｔにおける第２軸の回転加速度
ｍ１：第１軸のリンクの重心質量
ｍ２：第２軸のリンクの重心質量、なお、ｍ２にはリンク先端の負荷の質量が含まれる。
【００９４】
ｌｇ１：第１軸の回転中心から重心位置までの距離
ｌｇ２：第２軸の回転中心から重心位置までの距離
Ｌ１：第１軸の回転中心から第２軸の回転中心までの距離
Ｉ１：第１軸の、重心位置での回転軸回りの主慣性モーメント
Ｉ２：第２軸の、重心位置での回転軸回りの主慣性モーメント
Ｊｍ１：第１軸のモータイナーシャのアーム回転軸換算値
Ｊｍ２：第２軸のモータイナーシャのアーム回転軸換算値
ｄ１：第１軸の粘性摩擦係数
ｄ２：第２軸の粘性摩擦係数
ｇ：重力加速度
である。
【００９５】
＜ＳＴＥＰ４：補正係数の算出＞
図９は、ｊ軸における加減速時の関節角度、速度、加速度及び動力学トルクの時間変化を示す。また、図９は、補正係数γを導出するための考え方を示す模式図でもある。
【００９６】
前述したように、補正処理部２２は、動力学トルク算出部２１で算出された、所定の時刻における動力学トルク、慣性項トルク及び慣性項トルク以外のトルク（速度依存項トルク、重力項トルク）に基づいて第１及び第２補正係数γａ，γｂを算出し出力する。
【００９７】
具体的な第１及び第２補正係数γａ，γｂの算出手順について以下に説明する。なお、ｊ軸の出力可能な最大トルクを許容最大トルクτｊｍａｘとする。また、本実施形態では、時刻ｔａ，ｔｂの前後における重力項トルクと速度依存項トルクの変化量は、慣性項トルクの変化量に比べて十分に小さいものとする。
【００９８】
まず、ｊ軸に関する許容最大トルクτｊｍａｘと、時刻ｔａにおけるｊ軸の残余トルクτｃａｐｊとを用いて、式（３３）に示すトルクτａｒｊを算出する。
【００９９】
τａｒｊ＝τｊｍａｘ−τｃａｐｊ  ・・・（３３）
【０１００】
なお、許容最大トルτｊｍａｘの符号は時刻ｔａにおける動力学トルクτｊ（ｔａ）と同符号に合わせる。  
【０１０１】
次に、動力学トルク算出部２１で算出された時刻ｔａにおけるｊ軸の慣性項トルクτａｐｊとトルクτａｒｊを用いて、ｊ軸に関する補正係数γａｊを算出する。
【０１０２】
γａｊ＝τａｐｊ／τａｒｊ  ・・・（３４）
【０１０３】
同様に、ｊ軸に関する許容最大トルクτｊｍａｘと、時刻ｔｂにおけるｊ軸の残余トルクτｃｂｐｊとを用いて、式（３５）に示すトルクτｂｒｊを算出する。
【０１０４】
τｂｒｊ＝τｊｍａｘ−τｃｂｐｊ  ・・・（３５）
【０１０５】
なお、許容最大トルτｊｍａｘの符号は時刻ｔｂにおける動力学トルクτｊ（ｔｂ）と同符号に合わせる。  
【０１０６】
次に、動力学トルク算出部２１で算出された時刻ｔｂにおけるｊ軸の慣性項トルクτｂｐｊとトルクτｂｒｊを用いて、ｊ軸に関する補正係数γｂｊを計算する。
【０１０７】
γｂｊ＝τｂｐｊ／τｂｒｊ  ・・・（３６）
【０１０８】
ここで、式（３３）に示すトルクτａｒｊ及び式（３５）中のトルクτｂｒｊは、ｊ軸における最大加速度での加減速回転動作を行う場合に、ｊ軸の許容最大トルクτｊｍａｘのうち利用可能なトルク（以下、利用可能トルクと呼ぶことがある。）に相当する。また、慣性項トルクτａｐｊ及びτｂｊは、ｊ軸における最大加速度での加減速回転動作を実現するのに必要なトルクである。
【０１０９】
従って、式（３４），（３６）に示すγａｊ，γｂｊが１以下であれば、ｊ軸は所定の最大加速度で加減速回転動作が可能となる。一方、γａｊ，γｂｊが１よりも大きければ、ｊ軸は所望の動作を実現するのに必要なトルクが足らないことを意味する。
【０１１０】
次に、ロボットアーム６の全関節軸について、式（３３）〜（３６）により補正係数をそれぞれ算出し、加速動作中に得られた補正係数の中の最大のものを選んで、第１補正係数γａとする。また、減速動作中に得られた補正係数の中の最大のものを選んで、第２補正係数γｂとする。なお、以降の説明において、γａ，γｂを総称して補正係数γと呼ぶことがある。
【０１１１】
γａ＝ｍａｘ｛γａｊ｝  ・・・（３７）
γｂ＝ｍａｘ｛γｂｊ｝  ・・・（３８）
【０１１２】
＜ＳＴＥＰ５：加減速時間の補正＞
演算部２３は、加減速時間算出部９で算出された加速時間Ｔａに対して、補正処理部２２で算出された第１補正係数γａを乗じて、補正後の加速時間Ｔａ’を求め、加減速処理部４に出力する。また、演算部２３は、加減速時間算出部９で算出された減速時間Ｔｂに対して、補正処理部２２で算出された第２補正係数γｂを乗じて、補正後の減速時間Ｔｂ’を求め、加減速処理部４に出力する。なお、Ｔａ’、Ｔｂ’はそれぞれ式（３９）、（４０）で表わされる。
【０１１３】
Ｔａ’＝Ｔａ・γａ  ・・・（３９）
Ｔｂ’＝Ｔｂ・γｂ  ・・・（４０）
【０１１４】
式（３３）〜（４０）より明らかなように、γａ，γｂ≦１の場合は、いずれの関節軸においてもトルク不足は生じていないことを意味する。この場合は、第１及び第２補正係数γａ，γｂを乗じて得る補正後の加速時間Ｔａ’及び減速時間Ｔｂ’は当初の加速時間Ｔａ及び減速時間Ｔｂと同じである。
【０１１５】
一方、γａ，γｂ＞１の場合は、の場合は、いずれかの関節軸においてトルク不足が生じていることを意味する。そこで、当初の加速時間Ｔａをγａ（＞１）倍した新たな加速時間Ｔａ’（＞Ｔａ）で関節軸を回転させる。これは、関節軸の当初の加速度を１／γａ倍に下げる、すなわち、慣性項トルクを１／γａ倍に下げるということである。従って、第１補正係数γａにより補正されたｊ軸の動力学トルクτｊは、その許容最大トルクτｊｍａｘを超えることはない。同様に、第２補正係数γｂにより補正されたｊ軸の動力学トルクτｊは、その許容最大トルクτｊｍａｘを超えることはない。
【０１１６】
加減速処理部４は、この補正後の加速時間Ｔａ’または減速時間Ｔｂ’を用いて、移動指令生成部３から出力された移動指令列に対して上述した加減速処理を行う。
【０１１７】
［効果等］
以上説明したように、本実施形態におけるロボット制御装置１０は、モータと該モータによって回転駆動される関節軸とを複数組有するロボットアーム６の動作を制御する。
【０１１８】
また、ロボット制御装置１０は、各関節軸の回転速度と回転加速度と関節角度に基づいて、各関節軸における動力学トルクをそれぞれ算出して出力する動力学トルク算出部２１と、各関節軸の回転加速度が極大及び極小となる時点ｔａ，ｔｂでの各関節軸における関節角度の移動量Δθａｐ，Δθｂｐと回転速度ωａｐ，ωｂｐと回転加速度の極大値αａｐ及び極小値αｂｐとが各関節軸を加減速動作させるための加減速時間Ｔａ，Ｔｂに関連付けられて保存されたテーブル２４と、これが格納された記憶部２０と、を少なくとも備えている。
【０１１９】
動力学トルク算出部２１は、テーブル２４内の値に基づいて、回転加速度の極大値αａｐ及び極小値αｂｐに対応する動力学トルクを算出するとともに、回転加速度の極大値αａｐ及び極小値αｂｐに対応する慣性項トルクτａｐ，τｂｐと、動力学トルクから慣性項トルクを差し引いた残余トルクτｃａｐ，τｃｂｐとを算出し、各関節軸のうち少なくとも１つの関節軸において、慣性項トルクτａｐが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルク（τｍａｘ−τｃａｐ）よりも大きいか、慣性項トルクτｂｐが各関節軸を所定の加速度で加減速動作させるに利用可能な利用可能トルク（τｍａｘ−τｃｂｐ）よりも大きい場合は、各関節軸の回転動作における共通の加減速時間ＴａまたはＴｂを補正する。
【０１２０】
本実施形態によれば、多関節ロボットにおいて、各関節軸での加減速に必要なトルクが不足しているかどうかを予め検知でき、トルクが不足している場合は、加速時間Ｔａまたは減速時間Ｔｂを補正して、出力可能な範囲内のトルクで関節軸の回転動作を加速して、ロボットアーム６の動作を制御することができる。このことにより、ロボットの動作プログラム１の教示修正の手間を省くことができる。
【０１２１】
また、ロボット制御装置１０は、ロボットアーム６における隣り合う作業点間距離Ｌと目標移動速度Ｖとに基づいて、各関節軸に対する移動指令Δθをそれぞれ生成して出力する移動指令生成部３と、一の関節軸であるｊ軸が出力可能な許容最大トルクτｊｍａｘから残余トルクτｃａｐまたはτｃｂｐを差し引いた成分である利用可能トルクと、慣性項トルクτａｐ，τｂｐとのトルク比を算出し、さらに各関節軸における当該トルク比をそれぞれ算出して、各関節軸におけるトルク比の中の最大値を補正係数γとして出力する補正処理部２２と、を備えている。また、ロボット制御装置１０は、補正係数γが１以下の場合は、予め設定された加速時間Ｔａまたは減速時間Ｔｂに基づいて移動指令Δθを加減速処理し、補正係数γが１を超える場合は、加速時間Ｔａまたは減速時間Ｔｂに補正係数γを乗じて補正された加速時間Ｔａ’または減速時間Ｔｂ’に基づいて移動指令Δθを加減速処理する加減速処理部４と、を備えている。
【０１２２】
加減速処理部４で加減速処理された移動指令Δθに基づいて各関節軸を回転駆動させてロボットアーム６の動作を制御する。
【０１２３】
本実施形態によれば、多関節ロボットにおいて、各関節軸での加速に必要なトルクが不足しているかどうかを予め検知でき、トルクが不足している場合は、不足分に応じた値である第１補正係数γａ（＞１）で加速時間Ｔａを補正して、加速時間Ｔａを当初の値よりも大きな時間Ｔａ’とすることで、出力可能な範囲内のトルクで関節軸の回転動作を加速して、ロボットアーム６の動作を制御することができる。このことにより、ロボットの動作プログラム１の教示修正の手間を省くことができる。同様に、トルクの不足分に応じた値である第２補正係数γｂ（＞１）で減速時間Ｔｂを補正して、減速時間Ｔｂを当初の値よりも大きな時間Ｔｂ’とすることで、出力可能な範囲内のトルクで関節軸の回転動作を減速して、ロボットアーム６の動作を制御することができる。このことにより、ロボットの動作プログラム１の教示修正の手間を省くことができる。
【０１２４】
また、実際の加速度が極大または極小となる時刻、つまり、関節軸での加減速動作が最も急激に行われる時刻での動力学トルクに基づいて、加速時間Ｔａ及び／または減速時間Ｔｂを補正するため、ロボットアーム６を安定して動作させつつ、ロボットアーム６の動作時間であるタクトが増加するのを抑えることができる。
【０１２５】
テーブル２４に保存された値は、移動指令に基づいて算出されるようにしてもよい。
【０１２６】
このようにすることで、テーブル２４の作成作業が簡素化されるとともに、作成時間を短縮することができる。
【０１２７】
なお、本実施形態では、加減速プロファイルを台形形状としたが、これ以外のプロファイルに対しても適用できることは言うまでもない。
【産業上の利用可能性】
【０１２８】
本発明のロボット制御装置は、タクトの増加を抑制しつつ、ロボットアームを移動させる際に各関節軸で必要なトルクがその出力可能トルクを超えないように、加減速時間を自動調整することができて、ロボットの動作プログラムの教示修正を少なくし、教示作業を効率的に行うことができて、複数軸からなるロボットに適用する上できわめて有用である。
【符号の説明】
【０１２９】
１    動作プログラム
２    処理部
３    移動指令生成部
４    加減速処理部
５    サーボ
６    ロボットアーム
７    移動時間算出部
８    各軸速度算出部
９    加減速時間算出部
１０  ロボット制御装置
２０  記憶部
２１  動力学トルク算出部
２２  補正処理部
２３  演算部
２４  テーブル
【図１】
2019215980.tif 000003
【図２】
2019215980.tif 000004
【図３】
2019215980.tif 000005
【図４】
2019215980.tif 000006
【図５】
2019215980.tif 000007
【図６Ａ】
2019215980.tif 000008
【図６Ｂ】
2019215980.tif 000009
【図６Ｃ】
2019215980.tif 000010
【図７】
2019215980.tif 000011
【図８】
2019215980.tif 000012
【図９】
2019215980.tif 000013
【国際調査報告】
2019215980.tif 000014
2019215980.tif 000015
2019215980.tif 000016
2019215980.tif 000017