(57)【要約】      （修正有）【課題】  直線補間や円弧補間の最高速度上限値を、モータあるいは伝達機構の許容値を考慮して動作毎に決定できるようにし動作の高速化を図る。【解決手段】  基準速度演算手段２で、現在動作の各軸もしくは直交座標系における各方向の基準速度を算出し、加減速時間・速度上限パラメータ決定手段７が加速開始地点と減速終了地点の直交座標値と関節変位に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出し、算出したデータに基づいて指令曲線を生成する。【選択図】        図１
2005044230.tif 000002
【特許請求の範囲】
【請求項１】
ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位、および前記各軸の許容最高速度もしくは前記直交座標系における基準最高速度に基づいて、ロボットの現在動作についての各軸もしくは直交座標系における各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に前記各軸を駆動するモータの駆動トルクの許容最大値、前記各軸の伝達機構の作用トルクおよびモーメントの許容最大値、前記各軸の許容最高速度の少なくとも１つの制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出する加減速時間・速度上限パラメータ決定手段と、
算出された当該現在動作の基準速度、加速時間、減速時間および最大の速度上限パラメータに基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたロボット制御装置。
【請求項２】
ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、前記ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、前記直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に前記各軸を駆動するモータの駆動トルクの許容最大値、前記各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速時間決定手段と、
算出された前記最短の加速時間に基づいて各軸の加速区間のトルク使用率算出し、前記最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出するトルク使用率演算手段と、
ロボットの各軸の補正率を予め記憶する補正率記憶手段と、
算出された各軸の加速区間および減速区間の各トルク使用率および前記各軸毎の補正率に基づいて動作パラメータを修正する動作パラメータ修正手段と、
算出された基準速度、加速時間、減速時間と修正された動作パラメータに基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、ロボットの各軸を駆動するモータの動作速度を制御するロボット制御装置に関するものである。
【背景技術】
【０００２】
ロボットを動作させる際には加速度が同一でもロボットの姿勢に応じて必要なトルクは変化するため、動作毎に加減速時間などのパラメータを最適化する方法がとられている。この方法を適用した従来のロボット制御装置（例えば、特許文献１参照）では、次のような構成で動作する。
ロボットの動作開始点（加速開始地点）と動作終了点（減速終了地点）の座標を教示点として記憶しておく教示点記憶手段を備え、また、各軸もしくは各方向の最高速度パラメータ値等を予め記憶させているパラメータ記憶手段を備えている。教示点記憶手段で移動命令を受信すると、その移動命令を解析してどの点からどの点までロボットを移動させるかの判断を行う。例えば、その移動命令がＡ点（加速開始地点）からＢ点（減速終了地点）への直線補間命令である場合、この教示点記憶手段は、Ａ点およびＢ点の直交座標値および関節変位のデータを出力する。次に、最高速度演算手段により、この直交座標値とパラメータ記憶手段の最高速度パラメータ値に基づいて、その動作における各方向の最高速度を算出する。次に、教示点記憶手段から出力された直交座標値および関節変位と、パラメータ記憶手段に記憶されているトルクもしくは力の許容値等のパラメータと、最高速度演算手段で算出された最高速度に基づいて、加減速時間決定手段で、その動作における最適な加減速時間を決定する。最高速度と加減速時間が算出されると、これらの値に基づいて指令曲線生成手段で速度指令曲線を生成する。この場合、速度指令曲線が速度台形指令曲線になるように位置指令を生成し、その位置指令に対して移動平均フィルタおよび１次遅れフィルタなどによるスムージング処理が行われる。結果として、モータ制御手段は、生成された指令曲線にロボットが追従する動きをとるように各軸の駆動モータを制御する。
【０００３】
ここで、加減速時間決定手段が行う加減速時間決定の動作は、次のようにして行っている。
まず、加速時間と減速時間の初期値を算出する。次に、加速終了地点および減速開始地点での位置と速度を求め、加速開始地点および加速終了地点で加速時間を算出し直す。そして、加速開始地点で算出した加速時間と加速終了地点で算出した加速時間を比較し、大きい方を加速時間とする。減速時間に関しても同様に、減速開始地点および減速終了地点で減速時間を算出し直す。そして、減速開始地点で算出した減速時間と減速終了地点で算出した減速時間を比較し、大きい方を減速時間とする。上記一連の処理の繰り返し回数が、指定した回数に達したときは、現在の算出結果を加速時間および減速時間として出力する。繰り返し回数が指定した回数に達していなければ、上記一連の処理を繰り返し、現在の加速時間および減速時間に基づいて加速終了地点および減速開始地点の位置と速度を求めて、再度加減速時間の算出を行う。加速開始地点および加速終了地点では以下のようにして加速時間を算出する。
【０００４】
ロボットの運動方程式は、各関節の駆動トルクから構成される駆動トルクベクトルをτ、慣性行列をＭ、各関節の遠心・コリオリ力、重力、摩擦力の和から構成されるベクトルをｈ、各関節の加速度から構成されるベクトルをａとすると、
τ＝Ｍａ＋ｈ                  （１）
となる。ここで、各方向の速度指令が台形速度指令の場合、加速終了地点の各軸速度から構成されるベクトルをｖｋ，加速時間をｔｋとすれば、加速区間におけるロボットの運動方程式は、
τ＝Ｍｖｋ／ｔｋ＋ｈ          （２）
となる。式（２）のＭ、ｈを加速開始地点の位置と速度に基づいて算出した結果が、いずれの軸においてもそれぞれの軸の許容最大トルク以下となる制約を満たす範囲で最短となる加速時間ｔｋ１を算出するとともに、式（２）のＭ、ｈを加速終了地点の位置と速度に基づいて算出した結果が、いずれの軸においてもそれぞれの軸の許容最大トルク以下となる制約を満たす範囲で最短となる加速時間ｔｋ２を算出する。この算出したｔｋ１とｔｋ２を比較し、大きい方を加速時間ｔｋとして用いる。一方、減速区間の運動方程式は、減速時間をｔｇ、減速開始地点の各軸速度から構成されるベクトルをｖｇとすると、
τ＝−Ｍｖｇ／ｔｇ＋ｈ        （３）
となるので、式（３）に基づいて減速開始地点および減速終了地点の位置と速度から、加速時間の算出と同様の方法で減速時間を算出する。
【０００５】
【特許文献１】特開平７−２０００３３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
従来のロボット制御装置は、以上のように構成されているので、ロボットの手先を直線に沿って動作させる直線補間や円弧に沿って動作させる円弧補間を行う場合、等速区間であっても関節レベルでは加減速を行うため、等速区間の速度が高すぎると、等速動作を継続するだけでモータあるいは伝達機構の許容値を超えてしまう場合があり、動作毎に自動的に手先速度の上限値を定めることができなかった。また、連続動作を行う際には、特定の軸のモータのみ温度上昇量が大きくなる場合があるが、これを防ぐために各軸毎の加速度補正割合、各軸毎の速度補正割合、各軸毎の待ち時間補正割合などの予め指定した割合と各軸のモータの負荷に応じて自動的に加速時間、減速時間などの動作パラメータを修正することができないなどの課題があった。
【０００７】
この発明は上記のような課題を解決するためになされたもので、第１の目的は、新規な方法により動作の高速化を図ることができるロボット制御装置を得るものである。
【課題を解決するための手段】
【０００８】
この発明に係るロボット制御装置は、ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値をロボットパラメータ記憶手段に予め記憶しておき、基準速度演算手段により、直線補間で移動する際に入力される加速開始地点と減速終了地点の直交座標値と関節変位、および各軸の許容最高速度もしくは直交座標系における基準最高速度に基づいて、ロボットの現在動作についての各軸もしくは直交座標系における各方向の動作開始から加速時間経過したときに達する基準速度を算出し、加減速時間・速度上限パラメータ決定手段により、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値、前記各軸の許容最高速度の少なくとも１つの制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出し、算出された当該現在動作の基準速度、加速時間、減速時間および最大の速度上限パラメータに基づいて、指令曲線生成手段によりロボットの各軸を駆動するモータを制御する指令曲線を生成するものである。
【０００９】
この発明に係るロボット制御装置は、ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を、ロボットパラメータ記憶手段に予め記憶しておき、基準速度演算手段により、入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出し、加減速時間決定手段により、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出し、トルク使用率演算手段により、算出された最短の加速時間に基づいて各軸の加速区間のトルク使用率算出し、最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出し、また、ロボットの各軸の補正率を補正率記憶手段に予め記憶しておき、動作パラメータ修正手段により、算出された各軸の加速区間および減速区間の各トルク使用率および各軸毎の補正率に基づいて動作パラメータを修正し、算出された基準速度、加速時間、減速時間と修正された動作パラメータに基づいて、指令曲線生成手段によりロボットの各軸を駆動するモータを制御する指令曲線を生成するようにしたものである。
【発明の効果】
【００１０】
この発明は、動作毎にモータおよび伝達機構の許容トルクの制約を満たす範囲で最大の速度上限値および加減速パラメータを設定できるため、ロボットの動作の高速化を実現できる効果がある。
【００１１】
この発明は、予め指定した割合と各軸のモータの負荷に応じて動作パラメータを修正できるため、指定されたモータの負荷を抑えながらロボットの高速化が実現できる効果がある。
【発明を実施するための最良の形態】
【００１２】
実施の形態１．
図１はこの発明の実施の形態１によるロボット制御装置の構成を示すブロック図である。図において、教示点記憶手段１は、ロボットの加速開始地点および減速終了地点の直交座標を記憶しており、例えばＡ点からＢ点に移動する移動命令を受信したときにＡ点およびＢ点の直交座標値および関節変位を出力する手段である。ロボットパラメータ記憶手段６は、ロボットの機種毎に定められている各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値等の制約条件と、質量、重心位置等のロボットの動力学特性を表現するためのパラメータ値を予め記憶している手段である。基準速度演算手段２は、ロボットパラメータ記憶手段６に記憶されているロボットの各軸毎の許容最高速度もしくは直交座標系における基準最高速度に基づいて、ロボットの現在動作についての各軸もしくは直交座標系における各方向の動作開始から加速時間だけ経過したときに達する基準速度を算出する手段である。
【００１３】
加減速時間・速度上限パラメータ決定手段７は、動作毎にロボットパラメータ記憶手段６に記憶されているモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、最短の減速時間および最大の速度上限パラメータを算出する手段である。指令曲線生成手段３は、基準速度演算手段２で算出されたロボットの現在動作の基準速度、加減速時間・速度上限パラメータ決定手段７で算出された当該動作の加速時間、減速時間および速度上限パラメータに基づいて指令曲線を生成する手段である。モータ制御手段４は、指令曲線生成手段３で生成された指令曲線に従ってロボット５の各軸を駆動するモータを制御する。
【００１４】
次に動作について説明する。
教示点記憶手段１が、例えばＡ点（加速開始地点）からＢ点（減速終了地点）まで直線補間で移動する命令を受け取ると、教示点記憶手段１からは、Ａ点およびＢ点の直交座標値および関節変位が読み出され、基準速度演算手段２と加減速時間・速度上限パラメータ決定手段７に与えられる。基準速度演算手段２では、これらデータとロボットパラメータ記憶手段６に記憶されている直交座標系の各方向における基準最高速度に基づいて、当該動作における各方向の基準速度が算出され、加減速時間・速度上限パラメータ決定手段７と指令曲線生成手段３に与えられる。
【００１５】
加減速時間・速度上限パラメータ決定手段７の内部では、図２に示される処理手順に従って、モータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータが決定される。
まず、当該動作の加速時間と減速時間の初期値と、基準速度に乗じる倍率許容値、すなわち速度上限パラメータの初期値を設定する（ステップＳＴ１１）。次に、繰り返し回数ｎを初期値１に設定する（ステップＳＴ１２）。教示点記憶手段１から与えられたＡ点およびＢ点の直交座標値と、基準速度演算手段２で算出された当該動作における各方向の基準速度と、現在動作における加減速時間および速度上限パラメータとから、加速終了地点および減速開始地点における直交座標値、関節変位、各方向での速度、各軸の速度を算出する（ステップＳＴ１３）。
【００１６】
次に、加速開始地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の加速時間ｔ１ａｉ（ｉ＝１〜ｊ、ｊはロボットの軸数）をそれぞれ算出し、得られたｔ１ａｉの最大値をｔ１ａとする。同様に、加速終了地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の加速時間ｔ１ｂｉをそれぞれ算出し、ｔ１ｂｉの最大値をｔ１ｂとする（ステップＳＴ１４）。これらの算出された最大値ｔ１ａ、ｔ１ｂの中で、大きい方の値を今回の加速時間ｔ１として決定する（ステップＳＴ１５）。
同様にして減速時間も求める。まず、減速開始地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の減速時間ｔ２ａｉ（ｉ＝１〜ｊ、ｊはロボットの軸数）をそれぞれ算出し、得られたｔ２ａｉの最大値をｔ２ａとする。同様に減速終了地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の減速時間ｔ２ｂｉをそれぞれ算出し、得られたｔ２ｂｉの最大値をｔ２ｂとする（ステップＳＴ１６）。これらの算出された最大値ｔ２ａ、ｔ２ｂの中で、大きい方の値を今回の減速時間ｔ２として決定する（ステップＳＴ１７）。
【００１７】
次に、加速終了地点の関節変位、各方向の速度から、各軸のモータおよび伝達機構の許容トルクの制約を満たす範囲で等速運動を続けることができる最大の速度上限パラメータｖａを算出する。また、減速開始地点の関節変位、各方向の速度から、各軸のモータおよび伝達機構の許容トルクの制約を満たす範囲で等速運動を続けることができる最大の速度上限パラメータｖｂを算出する（ステップＳＴ１８）。現在の速度上限パラメータｖｍａｘ（ｎ−１）、算出された最大の速度上限パラメータｖａ、ｖｂの中で最小のものを今回の繰り返しにおける速度上限パラメータｖｍａｘ（ｎ）として決定する（ステップＳＴ１９）。
【００１８】
繰り返し回数ｎが規定値Ｋを超えていれば、今回の加速時間、減速時間、および速度上限パラメータを加減速時間・速度上限パラメータ決定手段７で最終的に求めた加速時間ｋｔ、最終的に求めた減速時間ｇｔ、最終的に求めた速度上限パラメータｖｍａｘとして指令曲線生成手段３に出力する（ステップＳＴ２０）。繰り返し回数ｎが規定値Ｋを超えていない場合は、繰り返し回数ｎを１だけ増加させ（ステップＳＴ２１）、ステップＳＴ１３からステップＳＴ１９の演算を再度行い、加速時間、減速時間および速度上限パラメータの算出を行う。
【００１９】
指令曲線生成手段３では、基準速度演算手段２から出力される基準速度と、加減速時間・速度上限パラメータ決定手段７から出力される加速時間、減速時間および速度上限パラメータに基づいた時々刻々の指令曲線を生成し、モータ制御手段４に出力する。モータ制御手段４は、指令曲線生成手段３で生成された時々刻々の指令曲線に従ってロボット５を駆動するモータを制御する。
【００２０】
次に、上記ステップＳＴ１３における加速終了地点および減速開始地点での速度および変位の算出、ステップＳＴ１４、ＳＴ１６における加速時間、減速時間の算出と、ステップＳＴ１８における速度上限パラメータの算出の詳細について説明する。
まず、ステップＳＴ１３では、基準速度ｖ０、速度上限パラメータｖｍａｘから、この動作の直線速度ｖｌを式（４）で算出する。
ｖｌ＝ｖｍａｘ＊ｖ０            （４）
次に、直線速度ｖｌ、動作開始地点の直交座標値および動作終了地点の直交座標値、現在の加速時間、減速時間から、加速終了地点（Ｃ点）および減速開始地点（Ｄ点）での直交座標値を算出する。算出した直交座標値から加速終了地点の関節変位ｑｃと減速開始地点での関節変位ｑｄを算出する。加速終了地点から微小時間Δｔだけ、直線速度ｖｌで動作終了地点Ｂへ進んだ地点（Ｃ２点）の直交座標値と関節変位ｑｃ２を算出する。また、減速開始地点から微小時間Δｔだけ直線速度ｖｌで動作開始地点Ａへ戻った地点（Ｄ２点）の直交座標値と関節変位ｑｄ２を算出する。
【００２１】
さらに、加速開始地点（Ａ点）の関節変位をｑａ、加速開始地点から微小時間Δｔだけ、直線速度ｖｌで進んだ地点（Ａ２）点の関節変位をｑａ２、減速終了地点（Ｂ点）の関節変位をｑｂ、減速終了地点から微小時間Δｔだけ直線速度ｖｌで動作開始地点Ａへ戻った地点（Ｂ２点）の関節変位をｑｂ２とする。このとき、加速開始地点、加速終了地点、減速開始地点、減速終了地点での関節速度ベクトルｖ１ａ、ｖ１ｂ、ｖ２ａ、ｖ２ｂをそれぞれ式（５）〜式（８）によりそれぞれ算出する。
ｖ１ａ＝（ｑａ２−ｑａ）／Δｔ            （５）
ｖ１ｂ＝（ｑｃ２−ｑｃ）／Δｔ              （６）
ｖ２ａ＝（ｑｄ−ｑｄ２）／Δｔ              （７）
ｖ２ｂ＝（ｑｂ−ｑｂ２）／Δｔ              （８）
【００２２】
ステップＳＴ１４では加速開始地点および加速終了地点から加速時間を算出する。まず、ロボットの運動方程式の慣性行列からモータの慣性を除いた行列をＭｌ、遠心・コリオリ力および重力の和から構成されるベクトルをｈｌとする。このとき各関節の減速機に作用するトルクτｌは式（９）となる。
τｌ＝Ｍｌａ＋ｈｌ                        （９）
式（１）、（９）で計算されるモータおよび減速機のトルクが許容値以下となるように加速開始地点、加速終了地点で加速時間を算出する。速度ｖまで加速時間ｔｋで加速する場合は、式（１）、（９）においてａ＝ｖ／ｔｋとすればよい。加速開始地点では、関節変位がｑａ、現在の速度を０とし、速度ｖ１ａまで等加速度で加速を行うとしたときの式（１）、（９）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最短となる加速時間ｔ１ａを算出する。加速終了地点では、関節変位がｑｃ、現在の速度をｖ１ｂとし、速度ｖ１ｂまで等加速度で加速を行うとしたときの式（１）、（９）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最短となる加速時間ｔ１ｂを算出する。
【００２３】
ステップＳＴ１６では、減速開始地点および減速終了地点で減速時間を算出する。速度ｖから減速時間ｔｇで減速する場合は、式（１）、（９）においてａ＝−ｖ／ｔｇとすればよい。減速開始地点では、関節変位がｑｄ、現在の速度をｖ２ａとし、速度ｖ２ａから等加速度で減速を行うとしたときの式（１）、（９）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最短となる減速時間ｔ２ａを算出する。減速終了地点では、関節変位がｑｂ、現在の速度を０とし、速度ｖ２ｂから等加速度で減速を行うとしたときの式（１）、（９）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最短となる減速時間ｔ２ｂを算出する。
【００２４】
ステップＳＴ１８では、まず、加速終了地点から微小時間Δｔだけ、直線速度ｖｌで動作開始点（加速開始地点）Ａへ戻った地点（Ｃ３点）の直交座標値と関節変位ｑｃ３を算出する。また、減速開始地点から微小時間Δｔだけ直線速度ｖｌで動作終了点（減速終了地点）Ｂへ進んだ地点（Ｄ３点）の直交座標値と関節変位ｑｄ３を算出する。次に速度ｖ１ｃ、ｖ２ｃをそれぞれ式（１０）、（１１）により算出する。
ｖ１ｃ＝（ｑｃ−ｑｃ３）／Δｔ              （１０）
ｖ２ｃ＝（ｑｄ３−ｑｄ）／Δｔ              （１１）
さらに加速終了地点および減速開始地点での加速度ａｃ、ａｄは、それぞれ式（１２）、（１３）により算出できる。
ａｃ＝（ｖ１ｂ−ｖ１ｃ）／Δｔ              （１２）
ａｄ＝（ｖ２ｃ−ｖ２ｂ）／Δｔ              （１３）
したがって、加速終了地点でのモータトルクτｃ、減速機トルクτｌｃはそれぞれ式（１４）、（１５）となる。
τｃ＝Ｍｃａｃ＋ｈｃ                      （１４）
τｌｃ＝Ｍｌｃａｃ＋ｈｌｃ                  （１５）
ここでＭｃ、ｈｃ、Ｍｌｃ、ｈｌｃはそれぞれ加速終了地点の関節変位ｑｃおよび関節速度ｖ１ｂから算出した式（１）のＭ、ｈ、式（９）のＭｌ、ｈｌである。
【００２５】
次に、加速終了地点での直線速度が、同じ場所で速度のみｋ倍されると、式（６）、（１０）の速度および式（１２）の加速度もｋ倍される。簡略化のため、速度が変化したことによる遠心・コリオリ力の変化は無視し、ｈｃのうち、粘性摩擦力をｆｖｃ、残りをｈ２ｃとすると、加速終了地点での直線速度が、同じ場所で速度のみｋ倍されたときには上記式（１４）、（１５）はそれぞれ次式（１６）、（１７）のように変形される。
τｃ＝ｋ＊（Ｍｃａｃ＋ｆｖｃ）＋ｈ２ｃ      （１６）
τｌｃ＝ｋ＊Ｍｌｃａｃ＋ｈｌｃ              （１７）
【００２６】
式（１６）、（１７）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大のｋであるｋａを算出する。式（１６）、（１７）はｋに関する１次式なのでｋａは容易に算出できる。例えば、第ｉ軸のモータトルクの制限τｍｍａｘｉを満たすｋの最大値ｋｍｉは、（Ｍｃａｃ＋ｆｖｃ）、ｈ２ｃの第ｉ行要素をそれぞれｍｉ、ｈｉとすると、ｍｉ＞０の場合は式（１８）となる。
ｋｍｉ＝（τｍｍａｘｉ−ｈｉ）／ｍｉ        （１８）
また、ｍｉ＜０の場合は（１９）となる。
ｋｍｉ＝（−τｍｍａｘｉ−ｈｉ）／ｍｉ      （１９）
また、ｍｉ＝０の場合は任意のｋでモータトルクの制限τｍｍａｘｉを満たすので、予め定めた正の大きな値とする。全ての軸のモータトルク制限を満たすｋの最大値と減速機のトルク制限を満たすｋの最大値の中から最小値を速度倍率ｋａとして算出する。
減速開始地点でも減速開始地点での加速度、速度、関節変位から、加速開始地点での演算と全く同様にして、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大のｋである速度倍率ｋｂを算出する。
【００２７】
次に、現在の速度上限パラメータｖｍａｘと速度倍率ｋａから加速終了地点での速度上限パラメータｖａを次式（２０）により算出する。
ｖａ＝ｖｍａｘ＊ｋａ                      （２０）
さらに現在の速度上限パラメータｖｍａｘとｋｂから減速開始地点での速度上限パラメータを式（２１）で算出する。
ｖｂ＝ｖｍａｘ＊ｋｂ                      （２１）
【００２８】
以上のように、この実施の形態１によれば、ロボットパラメータ記憶手段６に、ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しておき、基準速度演算手段２により、入力される加速開始地点と減速終了地点の直交座標値と関節変位、および各軸の許容最高速度もしくは直交座標系における基準最高速度に基づいて、ロボットの現在動作についての各軸もしくは直交座標系における各方向の動作開始から加速時間経過したときに達する基準速度を算出し、加減速時間・速度上限パラメータ決定手段７において、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値、各軸の許容最高速度の少なくとも１つの制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出し、指令曲線生成手段３により、算出された当該現在動作の基準速度、加速時間、減速時間および最大の速度上限パラメータに基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成するようにしている。したがって、動作毎に最適な速度上限値および加減速パラメータを設定できるため、ロボットの高速化が図れる効果が得られる。
【００２９】
また、そのための具体性として、この実施の形態１では、加減速時間・速度上限パラメータ決定手段７は、規定の繰り返し回数で演算を行い、与えられた加速開始地点および減速終了地点の直交座標値と、基準速度演算手段２で演算された当該動作における各方向の基準速度と、現在の加速時間、減速時間および速度上限パラメータとに基づいて加速終了地点および減速開始地点における直交座標値、関節変位、各方向での速度、各軸の速度を算出し、加速開始地点と加速終了地点のそれぞれにおいて、モータおよび伝達機構の許容トルクの制約を満たす範囲で各軸の最短の加速時間をそれぞれ算出し、得られた各地点での最短の加速時間の最大値ｔ１ａ、ｔ１ｂをそれぞれ抽出し、抽出されたこれらの最大値ｔ１ａ、ｔ１ｂの中で、大きい方の値を今回の加速時間として決定し、また、減速開始地点と減速終了地点のそれぞれにおいて、モータおよび伝達機構の許容トルクの制約を満たす範囲で各軸の最短の減速時間をそれぞれ算出し、得られた各地点での最短の減速時間の最大値ｔ２ａ、ｔ２ｂをそれぞれ抽出し、抽出されたこれらの最大値ｔ２ａ、ｔ２ｂの中で、大きい方の値を今回の減速時間として決定し、加速終了地点と減速開始地点のそれぞれの関節変位、各方向の速度から、各軸のモータおよび伝達機構の許容トルクの制約を満たす範囲で等速運動を続けることができる各地点の最大の速度上限パラメータｖａ、ｖｂをそれぞれ算出し、算出されたこれら最大の速度上限パラメータｖａ、ｖｂおよび現在の速度上限パラメータの中で、最小の値を今回の速度上限パラメータとして決定するようにしたことを特徴としている。
【００３０】
実施の形態２．
実施の形態２によるロボット制御装置の構成も図１で表され、加減速時間・速度上限パラメータ決定手段７の内部における加速時間、減速時間および速度上限パラメータ決定までの処理の流れも図２のフローチャートで表される。実施の形態１とは、ステップＳＴ１８における速度上限パラメータの算出方法のみが異なるので、ステップＳＴ１８の動作について説明する。
ステップＳＴ１８の内部でも加速終了地点での加速度ａｃ、減速開始地点での加速度ａｄの算出までは実施の形態１と同一である。実施の形態１では速度がｋ倍されたときに遠心・コリオリ力が変化する影響は無視していたが、実施の形態２の場合は、遠心・コリオリ力の変化も考慮してモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率を算出するようにしたものである。
【００３１】
まず、加速終了地点での関節変位ｑｃ、関節速度ｖ１ｂから算出した遠心・コリオリ力をｃｃ、重力をｇｃ、クーロン摩擦力をｆｃｃとすると、上記式（１４）、（１５）は、それぞれ次式（２２）、（２３）のように変形できる。
τｃ＝ｋ２＊ｃｃ＋ｋ＊（Ｍｃａｃ＋ｆｖｃ）＋（ｇｃ＋ｆｃｃ）  （２２）
τｌｃ＝ｋ２＊ｃｃ＋ｋ＊Ｍｌｃａｃ＋ｇｃ  （２３）
式（２２）、（２３）は速度倍率ｋの２次式なので、ｋ２およびｋの係数の符号毎に場合わけを行い、各軸のモータのトルク許容値を満たす範囲で最大の速度倍率ｋと、各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋを算出し、その中の最小値をｋａとすれば、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大の速度倍率ｋａが算出される。
【００３２】
同様に減速開始地点での関節変位ｑｄ、関節速度ｖ２ａから算出した遠心・コリオリ力をｃｄ、重力をｇｄ、クーロン摩擦力をｆｃｄとし、減速開始地点での関節変位ｑｄ、関節速度ｖ２ａから算出した式（１）のＭ、ｈ、式（９）のＭｌ、ｈｌをそれぞれＭｄ、ｈｄ、Ｍｌｄ、ｈｌｄとすれば、減速開始地点でのモータトルクおよび減速機トルクはそれぞれ次式（２４）、（２５）のようになる。
τｄ＝ｋ２＊ｃｄ＋ｋ＊（Ｍｄａｄ＋ｆｖｄ）＋（ｇｄ＋ｆｃｄ）  （２４）
τｌｄ＝ｋ２＊ｃｄ＋ｋ＊Ｍｌｄａｄ＋ｇｄ  （２５）
式（２４）、（２５）は速度倍率ｋの２次式なので、ｋ２およびｋの係数の符号毎に場合わけを行い、各軸のモータトルク許容値を満たす範囲で最大の速度倍率ｋと、各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋを算出し、その中の最小値を速度倍率ｋａとすれば、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大の速度倍率ｋｂが算出される。
以上のようにして算出された速度倍率ｋａ、ｋｂからは、実施の形態１と同様に式（２０）、（２１）を用いて加速終了地点での速度上限パラメータｖａと減速開始地点での速度上限パラメータｖｂが算出される。
【００３３】
以上のように、この実施の形態２によれば、加減速時間・速度上限パラメータ決定手段７は、上記実施の形態１の速度上限パラメータを求める部分に代わって、加速終了地点と減速開始地点のそれぞれの関節変位、関節速度から算出した遠心・コリオリ力、重力、クーロン摩擦力に基づいて、各地点（加速終了地点と減速開始地点）で各軸のモータの許容トルクの制約を満たす範囲で最大となる速度倍率と各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率をそれぞれ算出し、算出された各地点での両速度倍率の中で、最小の速度倍率ｋａ、ｋｂをそれぞれ抽出し、抽出された各地点での最小値の速度倍率ｋａ、ｋｂに基づいて、加速終了地点での最大の速度上限パラメータｖａと減速開始地点での速度上限パラメータｖｂを算出し、算出されたこれら最大の速度上限パラメータｖａ、ｖｂおよび現在の速度上限パラメータの中で、最小の値を今回の速度上限パラメータとして決定するようにしたことを特徴としている。したがって、遠心・コリオリ力の変化も考慮してモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率を算出できるため、実施の形態１と同様に、動作毎に最適な速度上限パラメータを設定できる効果が得られる。
【００３４】
実施の形態３．
実施の形態３によるロボット制御装置の構成も図１で表される。加減速時間・速度上限パラメータ決定手段７の内部における加速時間、減速時間および速度上限パラメータ決定までの処理の流れは図３のフローチャートで表される。図３において、この実施の形態３は、実施の形態１および実施の形態２の場合とはステップＳＴ１８における速度上限パラメータの算出方法が異なり、また、ステップＳＴ２２およびステップＳＴ２３が新たに追加されている点で異なっている。したがって、異なる点を中心にして説明する。
まず、ステップＳＴ１８では、加速終了地点で全てのモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋａを算出し、減速開始地点で全てのモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋｂを算出する。これら速度倍率ｋａおよびｋｂの算出方法は、実施の形態２と全く同様とする。
【００３５】
次に、加速終了地点の速度ｖ１ｂからモータ速度の制限を満たす範囲で最大となる倍率ｋａ２を算出する。ｖ１ｂの第ｉ要素をｖ１ｂｉ、第ｉ軸のモータ許容最高速度をｖｅｌｉ、とすると第ｉ軸の速度の最高速度に対する比率ｖｒｉは式（２６）により算出できる。
ｖｒｉ＝ａｂｓ（ｖ１ｂｉ）／ｖｅｌｉ            （２６）
ここで、ａｂｓ（  ）は絶対値を意味する。加速開始地点でのｖｒｉの最大値をｖｒｍａｘａとすれば、モータ速度の制限を満たす範囲で最大となる倍率ｋａ２は次式（２７）により算出できる。
ｋａ２＝１／ｖｒｍａｘａ              （２７）
同様に、減速開始地点の速度ｖ２ａからモータ速度の制限を満たす範囲で最大となる倍率ｋｂ２を算出する。
ｋａとｋａ２の小さい方をｋａ３、またｋｂとｋｂ２の小さい方をｋｂ３とし、加速終了地点での速度上限パラメータｖａ、ｖｂをそれぞれ現在の速度上限パラメータｖｍａｘから次式（２８）、（２９）で算出する。
ｖａ＝ｖｍａｘ＊ｋａ３                （２８）
ｖｂ＝ｖｍａｘ＊ｋｂ３                （２９）
【００３６】
次にステップＳＴ２２およびＳＴ２３について説明する。
まず、ステップＳＴ２０までで算出した加速時間、減速時間および速度上限値を用いた場合に、等速区間があるかどうかの判別を行う（ステップＳＴ２２）。この等速区間の有無の判別では、加速区間と減速区間の移動距離の和がその動作の移動距離よりも短い場合は等速区間が有り、一方、長い場合は等速区間が無いと判別できる。
【００３７】
ステップＳＴ２２の判定で、「等速区間が有り」の場合、等速区間の中点で速度上限パラメータを修正する（ステップＳＴ２３）。この場合、等速区間の中点、すなわち加速終了地点と減速開始地点の中点（Ｍ１点）を求め、中点の関節変位ｑｍを算出する。次に等速区間の中点（Ｍ１点）から等速区間の速度で微小時間Δｔだけ動作終了地点Ｂ点へ進んだ地点（Ｍ２点）の関節変位ｑｍ２と、等速区間の中点（Ｍ１点）から等速区間の速度で微小時間Δｔだけ動作開始地点Ａ点へ戻った地点（Ｍ３点）の関節変位ｑｍ３を算出する。さらに中点（Ｍ１点）での速度ｖｍおよび加速度ａｍを式（３０）、（３１）、（３２）により算出する。
ｖｍ＝（ｑｍ２−ｑｍ）／Δｔ            （３０）
ｖｍ２＝（ｑｍ−ｑｍ３）／Δｔ          （３１）
ａｍ＝（ｖｍ−ｖｍ２）／Δｔ            （３２）
【００３８】
中点（Ｍ１点）で、全ての軸のモータおよび減速機の許容トルクの制約を満たす範囲で最大の速度倍率ｋｍａを、ステップＳＴ１８における加速終了地点で全ての軸のモータおよび減速機の許容トルクの制約を満たす範囲で最大の速度倍率ｋａの算出と全く同様にて求める。さらに中点でのモータ最高速度の制限を満たす範囲で最大の速度倍率ｋｍｖを、ステップＳＴ１８における加速終了地点でのモータ最高速度の制限を満たす範囲で最大の速度倍率ｋａ２の算出と全く同様にして求める。次に、算出された速度倍率ｋｍａおよびｋｍｖの小さい方をｋｍとし、ｋｍ＜１の場合は速度上限パラメータをｋｍ＊ｖｍａｘに書き換える。また、ｋｍ≧１の場合には速度上限パラメータは書き換えない。
なお、ステップＳＴ２２で「等速区間は無し」との判定がなされた場合には、パラメータの修正は必要なく動作を終了する。
【００３９】
以上のように、この実施の形態３によれば、加減速時間・速度上限パラメータ決定手段７は、上記実施の形態１および実施の形態２の速度上限パラメータを求める部分に代わって、加速終了地点と減速開始地点のそれぞれの関節変位、関節速度から算出した遠心・コリオリ力、重力、クーロン摩擦力に基づいて、各地点で各軸のモータの許容トルクの制約を満たす範囲で最大となる速度倍率と各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率をそれぞれ算出し、算出された各地点での両速度倍率の中で、最小の速度倍率ｋａ、ｋｂをそれぞれ抽出し、加速終了地点および減速開始地点におけるそれぞれの速度ｖ１ｂ、ｖ２ａからモータ速度の制限を満たす範囲で最大となる速度倍率ｋａ２、ｋｂ２をそれぞれ算出し、最小の速度倍率ｋａ、ｋｂと最大となる速度倍率ｋａ２、ｋｂ２の中で小さい方の値ｋａ３、ｋｂ３に基づいて、加速終了地点と減速開始地点での速度上限パラメータｖａ、ｖｂをそれぞれ算出している。また、加速時間、減速時間および速度上限パラメータを用いた場合に、等速区間があるかどうかの判別を行い、等速区間が有りの場合、等速区間の中点で速度上限パラメータを修正し、中点の関節変位ｑｍを算出し、次に、等速区間の中点から等速区間の速度で微小時間Δｔだけ動作終了地点側へ進んだ地点の関節変位ｑｍ２と、等速区間の中点から等速区間の速度で微小時間Δｔだけ動作開始地点側へ戻った地点の関節変位ｑｍ３を算出し、さらに中点での速度ｖｍおよび加速度ａｍを算出し、中点で、全ての軸のモータおよび減速機の許容トルクの制約を満たす範囲で最大の速度倍率ｋｍａを算出し、さらに中点でのモータ最高速度の制限を満たす範囲で最大の速度倍率ｋｍｖを算出し、算出された速度倍率ｋｍａおよびｋｍｖの中で小さい方の値ｋｍが、１より小さい場合のみ、その小さい方の値（速度倍率ｋｍ）に基づいて、速度上限パラメータをｋｍ＊ｖｍａｘにそれぞれ書き換えるようにしたことを特徴としている。したがって、等速区間が存在する場合にも、実施の形態１と同様に、動作毎に最適な速度上限パラメータを設定できる効果が得られる。
【００４０】
実施の形態４．
図４はこの発明の実施の形態４によるロボット制御装置の構成を示すブロック図である。図において、図１と同一または相当する部分には同一符号を付し、原則としてその説明を省略する。
この実施の形態４における基準速度演算手段２０は、図１の基準速度演算手段２と同様に、ロボットの現在動作についての各軸もしくは直交座標系における各方向の基準速度を演算する手段であるが、Ａ点からＢ点に移動する移動命令が各関節を同期して動作させる関節補間の場合には、ロボットの各軸毎の許容最高速度に基づいて当該動作についての各軸における基準速度を演算する。また、Ａ点からＢ点に移動する移動命令が直線上を動作する直線補間の場合には、直交座標系における基準最高速度に基づいて当該動作についての直交座標系における各方向の基準速度を算出する。指令曲線生成手段３０は、図１の指令曲線生成手段３と同様に指令曲線を生成する手段であるが、この場合には基準速度演算手段２０で算出された当該動作の最高速度と、動作パラメータ補正手段１２で補正された動作パラメータに基づいて指令曲線の生成を行う。
【００４１】
加減速時間・速度上限パラメータ決定手段７０は、Ａ点からＢ点に移動する移動命令が関節補間の場合には、動作毎にロボットパラメータ記憶手段６に記憶されているモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間および減速時間を算出する。また、Ａ点からＢ点に移動する移動命令が直線補間の場合は、モータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出する。関節補間の場合の加速時間および減速時間の算出方法は、上記特許文献１の加減速時間決定手段による方法と同じである。また、直線補間の場合の加速時間、減速時間および速度上限パラメータの算出方法は、上記実施の形態１の加減速時間・速度上限パラメータ決定手段７による方法と同じである。
【００４２】
トルク使用率演算手段１０は、加減速時間・速度上限パラメータ決定手段７０の内部で計算した各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔｍｉおよび加減速時間・速度上限パラメータ決定手段７０で最終的に求めた加速時間ｋｔから各軸の加速区間のトルク使用率ｔｒａｔｅｋｉを次式（３３）で算出する。
ｔｒａｔｅｋｉ＝ｋｔｍｉ／ｋｔ              （３３）
同様に、各軸のモータトルク制限を満たす範囲で最短となる減速時間ｇｔｍｉ、加減速時間・速度上限パラメータ決定手段７０で最終的に求めた減速時間ｇｔから各軸の減速区間のトルク使用率ｔｒａｔｅｇｉを次式（３４）で算出する。
ｔｒａｔｅｇｉ＝ｇｔｍｉ／ｇｔ              （３４）
【００４３】
補正率記憶手段１１には、ロボット動作命令が記載されておりロボット制御装置で処理されるプログラム中に記載された各軸毎の補正率の設定値ｈｏｓｅｉが記憶されている。動作パラメータ補正手段１２は、トルク使用率演算手段１０で演算したトルク使用率と補正率記憶手段１１に記憶された各軸の補正率を用いて、加減速時間・速度上限パラメータ決定手段７０で算出された加速時間および減速時間を次のようにして補正し、指令曲線生成手段３０に出力する。
【００４４】
動作パラメータ補正手段１２において、まず、トルク使用率演算手段１０で求めた各軸の加速区間のトルク使用率と補正率記憶手段１１に記憶されている各軸の補正率の設定値から補正候補値ａｒａｔｅｋｉを次式（３５）で算出する。
ａｒａｔｅｋｉ＝ｔｒａｔｅｋｉ＊１００／ｈｏｓｅｉ      （３５）
算出された補正候補値ａｒａｔｅｋｉの中から最大値ａｒａｔｅｋｍａｘを求める。ここで、ａｒａｔｅｋｍａｘ≦１の場合は加速時間を補正せず、ａｒａｔｅｋｍａｘ＞１の場合は加速時間ｋｔをｋｔ＊ａｒａｔｅｋｍａｘに書き換える。
同様に、減速区間の各軸のトルク使用率と補正率の設定値から補正候補値ａｒａｔｅｇｉを次式（３６）で算出する。
ａｒａｔｅｇｉ＝ｔｒａｔｅｇｉ＊１００／ｈｏｓｅｉ      （３６）
算出された補正候補値ａｒａｔｅｇｉの中から最大値ａｒａｔｅｇｍａｘを求める。ａｒａｔｅｇｍａｘ≦１の場合、減速時間は補正しない。ａｒａｔｅｇｍａｘ＞１の場合は減速時間ｇｔをｇｔ＊ａｒａｔｅｇｍａｘに書き換える。なお、直線補間の場合に加減速時間・速度上限パラメータ決定手段７０が算出する速度上限パラメータは補正せずそのまま指令曲線生成手段に出力する。
【００４５】
以上のように、この実施の形態４によれば、加減速時間・速度上限パラメータ決定手段７０で、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、関節補間の場合には、動作毎にロボットパラメータ記憶手段６に記憶されている各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出し、また、直線補間の場合には、モータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出し、トルク使用率演算手段１０で、加減速時間・速度上限パラメータ決定手段７０により算出された最短の加速時間および最短の減速時間に基づいて各軸の加速区間のトルク使用率および各軸の減速区間のトルク使用率を算出し、算出された各軸の加速区間および減速区間の各トルク使用率および補正率記憶手段１１に記憶された各軸毎の補正率に基づいて、動作パラメータ補正手段１２により、加減速時間・速度上限パラメータ決定手段７０で算出された加速時間および減速時間を補正するようにしたので、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。なお、ここで動作パラメータの修正については加速時間および減速時間に限定される必要はない。また、加減速時間・速度上限パラメータの決定については、速度上限パラメータの算出も行う場合に限定される必要はない。速度上限パラメータの算出を行わない場合は、加減速時間・速度上限パラメータ決定手段７０は、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速決定手段に置き換えればよい。
【００４６】
また、具体的処理としてこの実施の形態４では、トルク使用率演算手段１０は、加減速時間・速度上限パラメータ決定手段７０で算出した各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔｍｉ、減速時間ｇｔｍｉおよび最終的に求めた加速時間ｋｔ、減速時間ｇｔに基づいて各軸の加速区間および減速区間のトルク使用率を算出し、動作パラメータ補正手段１２により、トルク使用率演算手段１０で求めた各軸の加速区間および減速区間の各トルク使用率と補正率記憶手段１１に記憶されている各軸の補正率の設定値に基づいてそれぞれの区間の補正候補値を算出し、算出された補正候補値の中から最大値をそれぞれ求め、求めた最大値が１より大きい場合のみ、最終的に求めた加速時間ｋｔおよび減速時間ｇｔをその１より大きい補正候補値の最大値に基づいてそれぞれｋｔ＊ａｒａｔｅｋｍａｘおよびｇｔ＊ａｒａｔｅｇｍａｘに置き換えるようにしたことを特徴としている。
【００４７】
実施の形態５．
実施の形態５によるロボット制御装置の構成も図４で表される。実施の形態４とは動作パラメータ補正手段１２と指令曲線生成手段３０の動作が異なるので、その異なる部分についてのみ説明する。
まず、動作パラメータ補正手段１２では、加速区間のトルク使用率と補正率の設定値から補正候補値ｖｒａｔｅｋｉを次式（３７）で算出する。
ｖｒａｔｅｋｉ＝ｔｒａｔｅｋｉ＊１００／ｈｏｓｅｉ    （３７）
算出された補正候補値ｖｒａｔｅｋｉの中から最大値ｖｒａｔｅｋｍａｘを求める。
同様に減速区間のトルク使用率と補正率の設定値から補正候補値ｖｒａｔｅｇｉを次式（３８）で算出する。
ｖｒａｔｅｇｉ＝ｔｒａｔｅｇｉ＊１００／ｈｏｓｅｉ    （３８）
算出された補正候補値ｖｒａｔｅｇｉの中から最大値ｖｒａｔｅｇｍａｘを求める。
【００４８】
次に、２つの補正候補値の最大値ｖｒａｔｅｋｍａｘとｖｒａｔｅｇｍａｘの中で大きい方を最終補正候補値ｖｒａｔｅｍａｘとする。ｖｒａｔｅｍａｘ＞１の時に、基準速度修正率ｏｖｒｄを次式（３９）で算出する。
ｏｖｒｄ＝１／ｖｒａｔｅｍａｘ                        （３９）
また、ｖｒａｔｅｍａｘ≦１のときはｏｖｒｄ＝１とする。指令曲線生成手段３０へは、基準速度修正率ｏｖｒｄと加減速時間・速度上限パラメータ決定手段７０で算出した加速時間、減速時間および速度上限パラメータを出力する。
指令曲線生成手段３０では、基準速度演算手段２０で演算した基準速度に動作パラメータ補正手段１２で算出した基準速度修正率ｏｖｒｄを乗じた値を基準速度として指令曲線を生成する。
【００４９】
以上のように、この実施の形態５によれば、トルク使用率演算手段１０では、加減速時間・速度上限パラメータ決定手段７０で算出された各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔｍｉ、減速時間ｇｔｍｉおよび最終的に求めた加速時間ｋｔ、減速時間ｇｔに基づいて各軸の加速区間および減速区間のトルク使用率をそれぞれ算出し、動作パラメータ補正手段１２により、算出された各軸の加速区間および減速区間のトルク使用率と補正率の設定値に基づいて補正候補値をそれぞれ算出し、算出された補正候補値の中からそれぞれ各区間での補正候補値の最大値を抽出し、抽出された各区間での最大値の中で、大きい方の値に基づいて基準速度修正率を算出し、指令曲線生成手段では、基準速度演算手段２０で算出した基準速度に算出された基準速度修正率を乗じて修正した基準速度とし、指令曲線を生成するようにしたことを特徴としている。したがって、実施の形態４と同様にして、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。
【００５０】
実施の形態６．
実施の形態６によるロボット制御装置の構成も図４で表される。実施の形態４とは動作パラメータ補正手段１２および指令曲線生成手段３０の動作が異なるので、その異なる部分についてのみ説明する。
まず、動作パラメータ補正手段１２では、加速区間のトルク使用率と補正率の設定値から補正候補値ｄｒａｔｅｋｉを次式（４０）から算出する。
ｄｒａｔｅｋｉ＝ｔｒａｔｅｋｉ＊１００／ｈｏｓｅｉ      （４０）
算出された補正候補値ｄｒａｔｅｋｉの最大値ｄｒａｔｅｋｍａｘを求める。同様に減速区間のトルク使用率と補正率の設定値から次式（４１）から補正候補値ｄｒａｔｅｇｉを算出する。
ｄｒａｔｅｇｉ＝ｔｒａｔｅｇｉ＊１００／ｈｏｓｅｉ      （４１）
算出された補正候補値ｄｒａｔｅｇｉの最大値ｄｒａｔｅｇｍａｘを求める。
次に、ｄｒａｔｅｋｍａｘ、ｄｒａｔｅｇｍａｘの中で大きい方をｄｒａｔｅｍａｘとする。ｄｒａｔｅｍａｘ＞１の時に、待ち時間修正率ｒｄｅｌａｙを
ｒｄｅｌａｙ＝ｄｒａｔｅｍａｘ                      （４２）
で算出する。また、ｄｒａｔｅｍａｘ≦１のときはｒｄｅｌａｙ＝１とする。
【００５１】
動作パラメータ補正手段１２は、待ち時間修正率ｒｄｅｌａｙを算出すると、その値を加減速時間・速度上限パラメータ決定手段７０で算出した加速時間、減速時間および速度上限パラメータと共に指令曲線生成手段３０へ出力する。指令曲線生成手段３０では、ロボット動作命令が記載されておりロボット制御装置で処理されるプログラム上に動作と動作の間で待ち時間が指定されているとき、その指定された待ち時間に待ち時間修正率ｒｄｅｌａｙを乗じて修正し、修正された待ち時間だけ停止してから次の動作に移行する。
【００５２】
以上のように、この実施の形態６によれば、トルク使用率演算手段１０では、加減速時間・速度上限パラメータ決定手段７０で算出された各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔｍｉ、減速時間ｇｔｍｉおよび最終的に求めた加速時間ｋｔ、減速時間ｇｔに基づいて各軸の加速区間および減速区間のトルク使用率をそれぞれ算出し、動作パラメータ補正手段１２により、算出された各軸の加速区間および減速区間のトルク使用率と補正率の設定値に基づいて補正候補値をそれぞれ算出し、算出された補正候補値の中からそれぞれ各区間での補正候補値の最大値を抽出し、抽出された各区間での補正候補値の最大値の中で、大きい方の値に基づいて待ち時間修正率を算出し、指令曲線生成手段３０では、動作と動作の間で待ち時間が指定されているとき、ロボット動作命令が記載されておりロボット制御装置で処理されるプログラム上に記載された待ち時間に算出された待ち時間修正率を乗じて修正し、修正された待ち時間に応じた動作停止を行ってから次の動作に移行する指令曲線を生成するようにしたことを特徴としている。したがって、実施の形態４と同様に、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。
【００５３】
実施の形態７．
実施の形態７によるロボット制御装置の構成も図４で表わされる。実施の形態４とはトルク使用率演算手段１０での動作が異なるので、その異なる部分についてのみ説明する。
トルク使用率演算手段１０は、加減速時間・速度上限パラメータ決定手段７０内部で計算した加速終了地点における慣性行列Ｍｃ、関節速度ｖ１ｂ、遠心・コリオリ力、重力、摩擦力の和ｈｃと加速時間ｋｔから、加速終了地点でのモータトルクτｍｋを次式（４３）で算出する。
τｍｋ＝Ｍｃｖ１ｂ／ｋｔ＋ｈｃ                    （４３）
加速終了地点でのモータトルクτｍｋの第ｉ軸要素をτｍｋｉとし、各軸の加速区間のトルク使用率ｔｒａｔｅｋｉを次式（４４）から算出する。
ｔｒａｔｅｋｉ＝ａｂｓ（τｍｋｉ）／τｍａｘｍｉ    （４４）
ここで、τｍａｘｍｉは各軸モータの許容最大トルクである。同様に、加減速時間・速度上限パラメータ決定手段７０内部で計算した減速開始地点における慣性行列Ｍｄ、関節速度ｖ２ａ、遠心・コリオリ力、重力、摩擦力の和ｈｄと減速時間ｇｔから、減速開始地点でのモータトルクτｍｇを次式（４５）で算出する。
τｍｇ＝−Ｍｄｖ２ａ／ｇｔ＋ｈｄ                  （４５）
減速開始地点でのモータトルクτｍｇの第ｉ軸要素をτｍｇｉとし、各軸の減速区間のトルク使用率ｔｒａｔｅｇｉを次式（４６）から算出する。
ｔｒａｔｅｇｉ＝ａｂｓ（τｍｇｉ）／τｍａｘｍｉ  （４６）
算出された各軸の加速区間のトルク使用率および減速区間のトルク使用率は、動作パラメータ補正手段１２に出力され、動作パラメータ補正手段１２において実施の形態４のように加速時間および減速時間の修正に用いられる。
【００５４】
以上のように、この実施の形態７によれば、トルク使用率演算手段１０では、加減速時間・速度上限パラメータ決定手段７０で計算した加速終了地点および減速開始地点における慣性行列、関節速度、遠心・コリオリ力、重力、摩擦力の和と加速時間ｋｔ、減速時間ｇｔに基づいて各地点でのモータトルクτｍｋ、τｍｇを算出し、これらのモータトルクτｍｋ、τｍｇと各軸のモータの許容最大トルクに基づいて各軸の加速区間および減速区間のトルク使用率を算出し、動作パラメータ補正手段１２では、算出された各軸の加速区間および減速区間の各トルク使用率と補正率記憶手段に記憶されている各軸の補正率の設定値に基づいてそれぞれの区間の補正候補値を算出し、加速時間および減速時間の修正に用いられることを特徴としている。したがって、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。
【図面の簡単な説明】
【００５５】
【図１】この発明の実施の形態１乃至実施の形態３によるロボット制御装置の構成を示すブロック図である。
【図２】この発明の実施の形態１および実施の形態２に係る加減速時間・速度上限パラメータ決定手段の処理手順を示すフローチャートである。
【図３】この発明の実施の形態３に係る加減速時間・速度上限パラメータ決定手段の処理手順を示すフローチャートである。
【図４】この発明の実施の形態４乃至実施の形態７によるロボット制御装置の構成を示すブロック図である。
【符号の説明】
【００５６】
１  教示点記憶手段、２，２０  基準速度演算手段、３，３０  指令曲線生成手段、４  モータ制御手段、５  ロボット、６  ロボットパラメータ記憶手段、７，７０  加減速時間・速度上限パラメータ決定手段、１０  トルク使用率演算手段、１１  補正率記憶手段、１２  動作パラメータ補正手段。
【図１】
2005044230.tif 000003
【図２】
2005044230.tif 000004
【図３】
2005044230.tif 000005
【図４】
2005044230.tif 000006