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

Abstract

【課題】直線補間や円弧補間の最高速度上限値を、モータあるいは伝達機構の許容値を考慮して動作毎に決定できるようにし動作の高速化を図る。
【解決手段】基準速度演算手段で、現在動作の各軸もしくは直交座標系における各方向の基準速度を算出する。加減速時間・速度上限パラメータ決定手段は、加速開始地点と減速終了地点の直交座標値と関節変位に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、減速時間を算出する。動作パラメータ修正手段は、加速時間、減速時間を修正する。指令曲線生成手段は、修正されたデータに基づいて指令曲線を生成する。
【選択図】図４

Description

この発明は、ロボットの各軸を駆動するモータの動作速度を制御するロボット制御装置に関するものである。

ロボットを動作させる際には加速度が同一でもロボットの姿勢に応じて必要なトルクは変化するため、動作毎に加減速時間などのパラメータを最適化する方法がとられている。この方法を適用した従来のロボット制御装置（例えば、特許文献１参照）では、次のような構成で動作する。
ロボットの動作開始点（加速開始地点）と動作終了点（減速終了地点）の座標を教示点として記憶しておく教示点記憶手段を備え、また、各軸もしくは各方向の最高速度パラメータ値等を予め記憶させているパラメータ記憶手段を備えている。教示点記憶手段で移動命令を受信すると、その移動命令を解析してどの点からどの点までロボットを移動させるかの判断を行う。例えば、その移動命令がＡ点（加速開始地点）からＢ点（減速終了地点）への直線補間命令である場合、この教示点記憶手段は、Ａ点およびＢ点の直交座標値および関節変位のデータを出力する。次に、最高速度演算手段により、この直交座標値とパラメータ記憶手段の最高速度パラメータ値に基づいて、その動作における各方向の最高速度を算出する。次に、教示点記憶手段から出力された直交座標値および関節変位と、パラメータ記憶手段に記憶されているトルクもしくは力の許容値等のパラメータと、最高速度演算手段で算出された最高速度に基づいて、加減速時間決定手段で、その動作における最適な加減速時間を決定する。最高速度と加減速時間が算出されると、これらの値に基づいて指令曲線生成手段で速度指令曲線を生成する。この場合、速度指令曲線が速度台形指令曲線になるように位置指令を生成し、その位置指令に対して移動平均フィルタおよび１次遅れフィルタなどによるスムージング処理が行われる。結果として、モータ制御手段は、生成された指令曲線にロボットが追従する動きをとるように各軸の駆動モータを制御する。

ここで、加減速時間決定手段が行う加減速時間決定の動作は、次のようにして行っている。
まず、加速時間と減速時間の初期値を算出する。次に、加速終了地点および減速開始地点での位置と速度を求め、加速開始地点および加速終了地点で加速時間を算出し直す。そして、加速開始地点で算出した加速時間と加速終了地点で算出した加速時間を比較し、大きい方を加速時間とする。減速時間に関しても同様に、減速開始地点および減速終了地点で減速時間を算出し直す。そして、減速開始地点で算出した減速時間と減速終了地点で算出した減速時間を比較し、大きい方を減速時間とする。上記一連の処理の繰り返し回数が、指定した回数に達したときは、現在の算出結果を加速時間および減速時間として出力する。繰り返し回数が指定した回数に達していなければ、上記一連の処理を繰り返し、現在の加速時間および減速時間に基づいて加速終了地点および減速開始地点の位置と速度を求めて、再度加減速時間の算出を行う。加速開始地点および加速終了地点では以下のようにして加速時間を算出する。

ロボットの運動方程式は、各関節の駆動トルクから構成される駆動トルクベクトルをτ、慣性行列をＭ、各関節の遠心・コリオリ力、重力、摩擦力の和から構成されるベクトルをｈ、各関節の加速度から構成されるベクトルをａとすると、
τ＝Ｍａ＋ｈ （１）
となる。ここで、各方向の速度指令が台形速度指令の場合、加速終了地点の各軸速度から構成されるベクトルをｖ_k，加速時間をｔ_kとすれば、加速区間におけるロボットの運動方程式は、
τ＝Ｍｖ_k／ｔ_k＋ｈ （２）
となる。式（２）のＭ、ｈを加速開始地点の位置と速度に基づいて算出した結果が、いずれの軸においてもそれぞれの軸の許容最大トルク以下となる制約を満たす範囲で最短となる加速時間ｔ_k1を算出するとともに、式（２）のＭ、ｈを加速終了地点の位置と速度に基づいて算出した結果が、いずれの軸においてもそれぞれの軸の許容最大トルク以下となる制約を満たす範囲で最短となる加速時間ｔ_k2を算出する。この算出したｔ_k1とｔ_k2を比較し、大きい方を加速時間ｔ_kとして用いる。一方、減速区間の運動方程式は、減速時間をｔ_g、減速開始地点の各軸速度から構成されるベクトルをｖ_gとすると、
τ＝−Ｍｖ_g／ｔ_g＋ｈ （３）
となるので、式（３）に基づいて減速開始地点および減速終了地点の位置と速度から、加速時間の算出と同様の方法で減速時間を算出する。

特開平７−２０００３３号公報

従来のロボット制御装置は、以上のように構成されているので、ロボットの手先を直線に沿って動作させる直線補間や円弧に沿って動作させる円弧補間を行う場合、等速区間であっても関節レベルでは加減速を行うため、等速区間の速度が高すぎると、等速動作を継続するだけでモータあるいは伝達機構の許容値を超えてしまう場合があり、動作毎に自動的に手先速度の上限値を定めることができなかった。また、連続動作を行う際には、特定の軸のモータのみ温度上昇量が大きくなる場合があるが、これを防ぐために各軸毎の加速度補正割合、各軸毎の速度補正割合、各軸毎の待ち時間補正割合などの予め指定した割合と各軸のモータの負荷に応じて自動的に加速時間、減速時間などの動作パラメータを修正することができないなどの課題があった。

この発明は上記のような課題を解決するためになされたもので、第１の目的は、新規な方法により動作の高速化を図ることができるロボット制御装置を得るものである。

この発明に係るロボット制御装置は、ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速時間決定手段と、算出された最短の加速時間に基づいて各軸の加速区間のトルク使用率を算出し、最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出するトルク使用率演算手段と、ロボットの各軸の補正率を予め記憶する補正率記憶手段と、各軸のトルク使用率を各軸の補正率の設定値で除算して得られる各軸の補正候補値を算出し、算出した補正候補値の最大値が１より大きくなる場合に、加速時間及び減速時間を補正候補値の最大値を乗じた値に修正する動作パラメータ修正手段と、算出された基準速度、加速時間、減速時間と修正された加速時間、減速時間に基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたものである。

この発明は、予め指定した割合と各軸のモータの負荷に応じて動作パラメータを修正できるため、指定されたモータの負荷を抑えながらロボットの高速化が実現できる効果がある。

実施の形態１．
図１はこの発明の実施の形態１によるロボット制御装置の構成を示すブロック図である。図において、教示点記憶手段１は、ロボットの加速開始地点および減速終了地点の直交座標を記憶しており、例えばＡ点からＢ点に移動する移動命令を受信したときにＡ点およびＢ点の直交座標値および関節変位を出力する手段である。ロボットパラメータ記憶手段６は、ロボットの機種毎に定められている各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値等の制約条件と、質量、重心位置等のロボットの動力学特性を表現するためのパラメータ値を予め記憶している手段である。基準速度演算手段２は、ロボットパラメータ記憶手段６に記憶されているロボットの各軸毎の許容最高速度もしくは直交座標系における基準最高速度に基づいて、ロボットの現在動作についての各軸もしくは直交座標系における各方向の動作開始から加速時間だけ経過したときに達する基準速度を算出する手段である。

加減速時間・速度上限パラメータ決定手段７は、動作毎にロボットパラメータ記憶手段６に記憶されているモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、最短の減速時間および最大の速度上限パラメータを算出する手段である。指令曲線生成手段３は、基準速度演算手段２で算出されたロボットの現在動作の基準速度、加減速時間・速度上限パラメータ決定手段７で算出された当該動作の加速時間、減速時間および速度上限パラメータに基づいて指令曲線を生成する手段である。モータ制御手段４は、指令曲線生成手段３で生成された指令曲線に従ってロボット５の各軸を駆動するモータを制御する。

次に動作について説明する。
教示点記憶手段１が、例えばＡ点（加速開始地点）からＢ点（減速終了地点）まで直線補間で移動する命令を受け取ると、教示点記憶手段１からは、Ａ点およびＢ点の直交座標値および関節変位が読み出され、基準速度演算手段２と加減速時間・速度上限パラメータ決定手段７に与えられる。基準速度演算手段２では、これらデータとロボットパラメータ記憶手段６に記憶されている直交座標系の各方向における基準最高速度に基づいて、当該動作における各方向の基準速度が算出され、加減速時間・速度上限パラメータ決定手段７と指令曲線生成手段３に与えられる。

加減速時間・速度上限パラメータ決定手段７の内部では、図２に示される処理手順に従って、モータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータが決定される。
まず、当該動作の加速時間と減速時間の初期値と、基準速度に乗じる倍率許容値、すなわち速度上限パラメータの初期値を設定する（ステップＳＴ１１）。次に、繰り返し回数ｎを初期値１に設定する（ステップＳＴ１２）。教示点記憶手段１から与えられたＡ点およびＢ点の直交座標値と、基準速度演算手段２で算出された当該動作における各方向の基準速度と、現在動作における加減速時間および速度上限パラメータとから、加速終了地点および減速開始地点における直交座標値、関節変位、各方向での速度、各軸の速度を算出する（ステップＳＴ１３）。

次に、加速開始地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の加速時間ｔ_1ai（ｉ＝１〜ｊ、ｊはロボットの軸数）をそれぞれ算出し、得られたｔ_1aiの最大値をｔ_1aとする。同様に、加速終了地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の加速時間ｔ_1biをそれぞれ算出し、ｔ_1biの最大値をｔ_1bとする（ステップＳＴ１４）。これらの算出された最大値ｔ_1a、ｔ_1bの中で、大きい方の値を今回の加速時間ｔ₁として決定する（ステップＳＴ１５）。
同様にして減速時間も求める。まず、減速開始地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の減速時間ｔ_2ai（ｉ＝１〜ｊ、ｊはロボットの軸数）をそれぞれ算出し、得られたｔ_2aiの最大値をｔ_2aとする。同様に減速終了地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の減速時間ｔ_2biをそれぞれ算出し、得られたｔ_2biの最大値をｔ_2bとする（ステップＳＴ１６）。これらの算出された最大値ｔ_2a、ｔ_2bの中で、大きい方の値を今回の減速時間ｔ₂として決定する（ステップＳＴ１７）。

次に、加速終了地点の関節変位、各方向の速度から、各軸のモータおよび伝達機構の許容トルクの制約を満たす範囲で等速運動を続けることができる最大の速度上限パラメータｖ_aを算出する。また、減速開始地点の関節変位、各方向の速度から、各軸のモータおよび伝達機構の許容トルクの制約を満たす範囲で等速運動を続けることができる最大の速度上限パラメータｖ_bを算出する（ステップＳＴ１８）。現在の速度上限パラメータｖｍａｘ（ｎ−１）、算出された最大の速度上限パラメータｖ_a、ｖ_bの中で最小のものを今回の繰り返しにおける速度上限パラメータｖｍａｘ（ｎ）として決定する（ステップＳＴ１９）。

繰り返し回数ｎが規定値Ｋを超えていれば、今回の加速時間、減速時間、および速度上限パラメータを加減速時間・速度上限パラメータ決定手段７で最終的に求めた加速時間ｋｔ、最終的に求めた減速時間ｇｔ、最終的に求めた速度上限パラメータｖｍａｘとして指令曲線生成手段３に出力する（ステップＳＴ２０）。繰り返し回数ｎが規定値Ｋを超えていない場合は、繰り返し回数ｎを１だけ増加させ（ステップＳＴ２１）、ステップＳＴ１３からステップＳＴ１９の演算を再度行い、加速時間、減速時間および速度上限パラメータの算出を行う。

指令曲線生成手段３では、基準速度演算手段２から出力される基準速度と、加減速時間・速度上限パラメータ決定手段７から出力される加速時間、減速時間および速度上限パラメータに基づいた時々刻々の指令曲線を生成し、モータ制御手段４に出力する。モータ制御手段４は、指令曲線生成手段３で生成された時々刻々の指令曲線に従ってロボット５を駆動するモータを制御する。

次に、上記ステップＳＴ１３における加速終了地点および減速開始地点での速度および変位の算出、ステップＳＴ１４、ＳＴ１６における加速時間、減速時間の算出と、ステップＳＴ１８における速度上限パラメータの算出の詳細について説明する。
まず、ステップＳＴ１３では、基準速度ｖ₀、速度上限パラメータｖｍａｘから、この動作の直線速度ｖ_lを式（４）で算出する。
ｖ_l＝ｖｍａｘ^*ｖ₀ （４）
次に、直線速度ｖ_l、動作開始地点の直交座標値および動作終了地点の直交座標値、現在の加速時間、減速時間から、加速終了地点（Ｃ点）および減速開始地点（Ｄ点）での直交座標値を算出する。算出した直交座標値から加速終了地点の関節変位ｑ_cと減速開始地点での関節変位ｑ_dを算出する。加速終了地点から微小時間Δｔだけ、直線速度ｖ_lで動作終了地点Ｂへ進んだ地点（Ｃ２点）の直交座標値と関節変位ｑ_c2を算出する。また、減速開始地点から微小時間Δｔだけ直線速度ｖ_lで動作開始地点Ａへ戻った地点（Ｄ２点）の直交座標値と関節変位ｑ_d2を算出する。

さらに、加速開始地点（Ａ点）の関節変位をｑ_a、加速開始地点から微小時間Δｔだけ、直線速度ｖ_lで進んだ地点（Ａ２）点の関節変位をｑ_a2、減速終了地点（Ｂ点）の関節変位をｑ_b、減速終了地点から微小時間Δｔだけ直線速度ｖ_lで動作開始地点Ａへ戻った地点（Ｂ２点）の関節変位をｑ_b2とする。このとき、加速開始地点、加速終了地点、減速開始地点、減速終了地点での関節速度ベクトルｖ_1a、ｖ_1b、ｖ_2a、ｖ_2bをそれぞれ式（５）〜式（８）によりそれぞれ算出する。
ｖ_1a＝（ｑ_a2−ｑ_a）／Δｔ （５）
ｖ_1b＝（ｑ_c2−ｑ_c）／Δｔ （６）
ｖ_2a＝（ｑ_d−ｑ_d2）／Δｔ （７）
ｖ_2b＝（ｑ_b−ｑ_b2）／Δｔ （８）

ステップＳＴ１４では加速開始地点および加速終了地点から加速時間を算出する。まず、ロボットの運動方程式の慣性行列からモータの慣性を除いた行列をＭ_l、遠心・コリオリ力および重力の和から構成されるベクトルをｈ_lとする。このとき各関節の減速機に作用するトルクτ_lは式（９）となる。
τ_l＝Ｍ_lａ＋ｈ_l （９）
式（１）、（９）で計算されるモータおよび減速機のトルクが許容値以下となるように加速開始地点、加速終了地点で加速時間を算出する。速度ｖまで加速時間ｔｋで加速する場合は、式（１）、（９）においてａ＝ｖ／ｔｋとすればよい。加速開始地点では、関節変位がｑ_a、現在の速度を０とし、速度ｖ_1aまで等加速度で加速を行うとしたときの式（１）、（９）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最短となる加速時間ｔ_1aを算出する。加速終了地点では、関節変位がｑ_c、現在の速度をｖ_1bとし、速度ｖ_1bまで等加速度で加速を行うとしたときの式（１）、（９）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最短となる加速時間ｔ_1bを算出する。

ステップＳＴ１６では、減速開始地点および減速終了地点で減速時間を算出する。速度ｖから減速時間ｔｇで減速する場合は、式（１）、（９）においてａ＝−ｖ／ｔｇとすればよい。減速開始地点では、関節変位がｑ_d、現在の速度をｖ_2aとし、速度ｖ_2aから等加速度で減速を行うとしたときの式（１）、（９）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最短となる減速時間ｔ_2aを算出する。減速終了地点では、関節変位がｑ_b、現在の速度を０とし、速度ｖ_2bから等加速度で減速を行うとしたときの式（１）、（９）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最短となる減速時間ｔ_2bを算出する。

ステップＳＴ１８では、まず、加速終了地点から微小時間Δｔだけ、直線速度ｖ_lで動作開始点（加速開始地点）Ａへ戻った地点（Ｃ３点）の直交座標値と関節変位ｑ_c3を算出する。また、減速開始地点から微小時間Δｔだけ直線速度ｖ_lで動作終了点（減速終了地点）Ｂへ進んだ地点（Ｄ３点）の直交座標値と関節変位ｑ_d3を算出する。次に速度ｖ_1c、ｖ_2cをそれぞれ式（１０）、（１１）により算出する。
ｖ_1c＝（ｑ_c−ｑ_c3）／Δｔ （１０）
ｖ_2c＝（ｑ_d3−ｑ_d）／Δｔ （１１）
さらに加速終了地点および減速開始地点での加速度ａ_c、ａ_dは、それぞれ式（１２）、（１３）により算出できる。
ａ_c＝（ｖ_1b−ｖ_1c）／Δｔ （１２）
ａ_d＝（ｖ_2c−ｖ_2b）／Δｔ （１３）
したがって、加速終了地点でのモータトルクτ_c、減速機トルクτ_lcはそれぞれ式（１４）、（１５）となる。
τ_c＝Ｍ_cａ_c＋ｈ_c （１４）
τ_lc＝Ｍ_lcａ_c＋ｈ_lc （１５）
ここでＭ_c、ｈ_c、Ｍ_lc、ｈ_lcはそれぞれ加速終了地点の関節変位ｑ_cおよび関節速度ｖ_1bから算出した式（１）のＭ、ｈ、式（９）のＭ_l、ｈ_lである。

次に、加速終了地点での直線速度が、同じ場所で速度のみｋ倍されると、式（６）、（１０）の速度および式（１２）の加速度もｋ倍される。簡略化のため、速度が変化したことによる遠心・コリオリ力の変化は無視し、ｈ_cのうち、粘性摩擦力をｆ_vc、残りをｈ_2cとすると、加速終了地点での直線速度が、同じ場所で速度のみｋ倍されたときには上記式（１４）、（１５）はそれぞれ次式（１６）、（１７）のように変形される。
τ_c＝ｋ^*（Ｍ_cａ_c＋ｆ_vc）＋ｈ_2c （１６）
τ_lc＝ｋ^*Ｍ_lcａ_c＋ｈ_lc （１７）

式（１６）、（１７）の算出結果がいずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大のｋであるｋ_aを算出する。式（１６）、（１７）はｋに関する１次式なのでｋ_aは容易に算出できる。例えば、第ｉ軸のモータトルクの制限τ_mmaxiを満たすｋの最大値ｋ_miは、（Ｍ_cａ_c＋ｆ_vc）、ｈ_2cの第ｉ行要素をそれぞれｍ_i、ｈ_iとすると、ｍ_i＞０の場合は式（１８）となる。
ｋ_mi＝（τ_mmaxi−ｈ_i）／ｍ_i （１８）
また、ｍ_i＜０の場合は（１９）となる。
ｋ_mi＝（−τ_mmaxi−ｈ_i）／ｍ_i （１９）
また、ｍ_i＝０の場合は任意のｋでモータトルクの制限τ_mmaxiを満たすので、予め定めた正の大きな値とする。全ての軸のモータトルク制限を満たすｋの最大値と減速機のトルク制限を満たすｋの最大値の中から最小値を速度倍率ｋ_aとして算出する。
減速開始地点でも減速開始地点での加速度、速度、関節変位から、加速開始地点での演算と全く同様にして、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大のｋである速度倍率ｋ_bを算出する。

次に、現在の速度上限パラメータｖｍａｘと速度倍率ｋ_aから加速終了地点での速度上限パラメータｖ_aを次式（２０）により算出する。
ｖ_a＝ｖｍａｘ^*ｋ_a （２０）
さらに現在の速度上限パラメータｖｍａｘとｋ_bから減速開始地点での速度上限パラメータを式（２１）で算出する。
ｖ_b＝ｖｍａｘ^*ｋ_b （２１）

以上のように、この実施の形態１によれば、ロボットパラメータ記憶手段６に、ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しておき、基準速度演算手段２により、入力される加速開始地点と減速終了地点の直交座標値と関節変位、および各軸の許容最高速度もしくは直交座標系における基準最高速度に基づいて、ロボットの現在動作についての各軸もしくは直交座標系における各方向の動作開始から加速時間経過したときに達する基準速度を算出し、加減速時間・速度上限パラメータ決定手段７において、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値、各軸の許容最高速度の少なくとも１つの制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出し、指令曲線生成手段３により、算出された当該現在動作の基準速度、加速時間、減速時間および最大の速度上限パラメータに基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成するようにしている。したがって、動作毎に最適な速度上限値および加減速パラメータを設定できるため、ロボットの高速化が図れる効果が得られる。

また、そのための具体性として、この実施の形態１では、加減速時間・速度上限パラメータ決定手段７は、規定の繰り返し回数で演算を行い、与えられた加速開始地点および減速終了地点の直交座標値と、基準速度演算手段２で演算された当該動作における各方向の基準速度と、現在の加速時間、減速時間および速度上限パラメータとに基づいて加速終了地点および減速開始地点における直交座標値、関節変位、各方向での速度、各軸の速度を算出し、加速開始地点と加速終了地点のそれぞれにおいて、モータおよび伝達機構の許容トルクの制約を満たす範囲で各軸の最短の加速時間をそれぞれ算出し、得られた各地点での最短の加速時間の最大値ｔ_1a、ｔ_1bをそれぞれ抽出し、抽出されたこれらの最大値ｔ_1a、ｔ_1bの中で、大きい方の値を今回の加速時間として決定し、また、減速開始地点と減速終了地点のそれぞれにおいて、モータおよび伝達機構の許容トルクの制約を満たす範囲で各軸の最短の減速時間をそれぞれ算出し、得られた各地点での最短の減速時間の最大値ｔ_2a、ｔ_2bをそれぞれ抽出し、抽出されたこれらの最大値ｔ_2a、ｔ_2bの中で、大きい方の値を今回の減速時間として決定し、加速終了地点と減速開始地点のそれぞれの関節変位、各方向の速度から、各軸のモータおよび伝達機構の許容トルクの制約を満たす範囲で等速運動を続けることができる各地点の最大の速度上限パラメータｖ_a、ｖ_bをそれぞれ算出し、算出されたこれら最大の速度上限パラメータｖ_a、ｖ_bおよび現在の速度上限パラメータの中で、最小の値を今回の速度上限パラメータとして決定するようにしたことを特徴としている。

実施の形態２．
実施の形態２によるロボット制御装置の構成も図１で表され、加減速時間・速度上限パラメータ決定手段７の内部における加速時間、減速時間および速度上限パラメータ決定までの処理の流れも図２のフローチャートで表される。実施の形態１とは、ステップＳＴ１８における速度上限パラメータの算出方法のみが異なるので、ステップＳＴ１８の動作について説明する。
ステップＳＴ１８の内部でも加速終了地点での加速度ａ_c、減速開始地点での加速度ａ_dの算出までは実施の形態１と同一である。実施の形態１では速度がｋ倍されたときに遠心・コリオリ力が変化する影響は無視していたが、実施の形態２の場合は、遠心・コリオリ力の変化も考慮してモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率を算出するようにしたものである。

まず、加速終了地点での関節変位ｑ_c、関節速度ｖ_1bから算出した遠心・コリオリ力をｃ_c、重力をｇ_c、クーロン摩擦力をｆ_ccとすると、上記式（１４）、（１５）は、それぞれ次式（２２）、（２３）のように変形できる。
τ_c＝ｋ^2*ｃ_c＋ｋ^*（Ｍ_cａ_c＋ｆ_vc）＋（ｇ_c＋ｆ_cc） （２２）
τ_lc＝ｋ^2*ｃ_c＋ｋ^*Ｍ_lcａ_c＋ｇ_c （２３）
式（２２）、（２３）は速度倍率ｋの２次式なので、ｋ²およびｋの係数の符号毎に場合わけを行い、各軸のモータのトルク許容値を満たす範囲で最大の速度倍率ｋと、各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋを算出し、その中の最小値をｋ_aとすれば、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大の速度倍率ｋ_aが算出される。

同様に減速開始地点での関節変位ｑ_d、関節速度ｖ_2aから算出した遠心・コリオリ力をｃ_d、重力をｇ_d、クーロン摩擦力をｆ_cdとし、減速開始地点での関節変位ｑ_d、関節速度ｖ_2aから算出した式（１）のＭ、ｈ、式（９）のＭ_l、ｈ_lをそれぞれＭ_d、ｈ_d、Ｍ_ld、ｈ_ldとすれば、減速開始地点でのモータトルクおよび減速機トルクはそれぞれ次式（２４）、（２５）のようになる。
τ_d＝ｋ^2*ｃ_d＋ｋ^*（Ｍ_dａ_d＋ｆ_vd）＋（ｇ_d＋ｆ_cd） （２４）
τ_ld＝ｋ^2*ｃ_d＋ｋ^*Ｍ_ldａ_d＋ｇ_d （２５）
式（２４）、（２５）は速度倍率ｋの２次式なので、ｋ²およびｋの係数の符号毎に場合わけを行い、各軸のモータトルク許容値を満たす範囲で最大の速度倍率ｋと、各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋを算出し、その中の最小値を速度倍率ｋ_aとすれば、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大の速度倍率ｋ_bが算出される。
以上のようにして算出された速度倍率ｋ_a、ｋ_bからは、実施の形態１と同様に式（２０）、（２１）を用いて加速終了地点での速度上限パラメータｖ_aと減速開始地点での速度上限パラメータｖ_bが算出される。

以上のように、この実施の形態２によれば、加減速時間・速度上限パラメータ決定手段７は、上記実施の形態１の速度上限パラメータを求める部分に代わって、加速終了地点と減速開始地点のそれぞれの関節変位、関節速度から算出した遠心・コリオリ力、重力、クーロン摩擦力に基づいて、各地点（加速終了地点と減速開始地点）で各軸のモータの許容トルクの制約を満たす範囲で最大となる速度倍率と各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率をそれぞれ算出し、算出された各地点での両速度倍率の中で、最小の速度倍率ｋ_a、ｋ_bをそれぞれ抽出し、抽出された各地点での最小値の速度倍率ｋ_a、ｋ_bに基づいて、加速終了地点での最大の速度上限パラメータｖ_aと減速開始地点での速度上限パラメータｖ_bを算出し、算出されたこれら最大の速度上限パラメータｖ_a、ｖ_bおよび現在の速度上限パラメータの中で、最小の値を今回の速度上限パラメータとして決定するようにしたことを特徴としている。したがって、遠心・コリオリ力の変化も考慮してモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率を算出できるため、実施の形態１と同様に、動作毎に最適な速度上限パラメータを設定できる効果が得られる。

実施の形態３．
実施の形態３によるロボット制御装置の構成も図１で表される。加減速時間・速度上限パラメータ決定手段７の内部における加速時間、減速時間および速度上限パラメータ決定までの処理の流れは図３のフローチャートで表される。図３において、この実施の形態３は、実施の形態１および実施の形態２の場合とはステップＳＴ１８における速度上限パラメータの算出方法が異なり、また、ステップＳＴ２２およびステップＳＴ２３が新たに追加されている点で異なっている。したがって、異なる点を中心にして説明する。
まず、ステップＳＴ１８では、加速終了地点で全てのモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋ_aを算出し、減速開始地点で全てのモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋ_bを算出する。これら速度倍率ｋ_aおよびｋ_bの算出方法は、実施の形態２と全く同様とする。

次に、加速終了地点の速度ｖ_1bからモータ速度の制限を満たす範囲で最大となる倍率ｋ_a2を算出する。ｖ_1bの第ｉ要素をｖ_1bi、第ｉ軸のモータ許容最高速度をｖｅｌ_i、とすると第ｉ軸の速度の最高速度に対する比率ｖｒ_iは式（２６）により算出できる。
ｖｒ_i＝ａｂｓ（ｖ_1bi）／ｖｅｌ_i （２６）
ここで、ａｂｓ（ ）は絶対値を意味する。加速開始地点でのｖｒ_iの最大値をｖｒｍａｘ_aとすれば、モータ速度の制限を満たす範囲で最大となる倍率ｋ_a2は次式（２７）により算出できる。
ｋ_a2＝１／ｖｒｍａｘ_a （２７）
同様に、減速開始地点の速度ｖ_2aからモータ速度の制限を満たす範囲で最大となる倍率ｋ_b2を算出する。
ｋ_aとｋ_a2の小さい方をｋ_a3、またｋ_bとｋ_b2の小さい方をｋ_b3とし、加速終了地点での速度上限パラメータｖ_a、ｖ_bをそれぞれ現在の速度上限パラメータｖｍａｘから次式（２８）、（２９）で算出する。
ｖ_a＝ｖｍａｘ^*ｋ_a3 （２８）
ｖ_b＝ｖｍａｘ^*ｋ_b3 （２９）

次にステップＳＴ２２およびＳＴ２３について説明する。
まず、ステップＳＴ２０までで算出した加速時間、減速時間および速度上限値を用いた場合に、等速区間があるかどうかの判別を行う（ステップＳＴ２２）。この等速区間の有無の判別では、加速区間と減速区間の移動距離の和がその動作の移動距離よりも短い場合は等速区間が有り、一方、長い場合は等速区間が無いと判別できる。

ステップＳＴ２２の判定で、「等速区間が有り」の場合、等速区間の中点で速度上限パラメータを修正する（ステップＳＴ２３）。この場合、等速区間の中点、すなわち加速終了地点と減速開始地点の中点（Ｍ１点）を求め、中点の関節変位ｑ_mを算出する。次に等速区間の中点（Ｍ１点）から等速区間の速度で微小時間Δｔだけ動作終了地点Ｂ点へ進んだ地点（Ｍ２点）の関節変位ｑ_m2と、等速区間の中点（Ｍ１点）から等速区間の速度で微小時間Δｔだけ動作開始地点Ａ点へ戻った地点（Ｍ３点）の関節変位ｑ_m3を算出する。さらに中点（Ｍ１点）での速度ｖ_mおよび加速度ａ_mを式（３０）、（３１）、（３２）により算出する。
ｖ_m＝（ｑ_m2−ｑ_m）／Δｔ （３０）
ｖ_m2＝（ｑ_m−ｑ_m3）／Δｔ （３１）
ａ_m＝（ｖ_m−ｖ_m2）／Δｔ （３２）

中点（Ｍ１点）で、全ての軸のモータおよび減速機の許容トルクの制約を満たす範囲で最大の速度倍率ｋ_maを、ステップＳＴ１８における加速終了地点で全ての軸のモータおよび減速機の許容トルクの制約を満たす範囲で最大の速度倍率ｋ_aの算出と全く同様にて求める。さらに中点でのモータ最高速度の制限を満たす範囲で最大の速度倍率ｋ_mvを、ステップＳＴ１８における加速終了地点でのモータ最高速度の制限を満たす範囲で最大の速度倍率ｋ_a2の算出と全く同様にして求める。次に、算出された速度倍率ｋ_maおよびｋ_mvの小さい方をｋ_mとし、ｋ_m＜１の場合は速度上限パラメータをｋ_m ^*ｖｍａｘに書き換える。また、ｋ_m≧１の場合には速度上限パラメータは書き換えない。
なお、ステップＳＴ２２で「等速区間は無し」との判定がなされた場合には、パラメータの修正は必要なく動作を終了する。

以上のように、この実施の形態３によれば、加減速時間・速度上限パラメータ決定手段７は、上記実施の形態１および実施の形態２の速度上限パラメータを求める部分に代わって、加速終了地点と減速開始地点のそれぞれの関節変位、関節速度から算出した遠心・コリオリ力、重力、クーロン摩擦力に基づいて、各地点で各軸のモータの許容トルクの制約を満たす範囲で最大となる速度倍率と各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率をそれぞれ算出し、算出された各地点での両速度倍率の中で、最小の速度倍率ｋ_a、ｋ_bをそれぞれ抽出し、加速終了地点および減速開始地点におけるそれぞれの速度ｖ_1b、ｖ_2aからモータ速度の制限を満たす範囲で最大となる速度倍率ｋ_a2、ｋ_b2をそれぞれ算出し、最小の速度倍率ｋ_a、ｋ_bと最大となる速度倍率ｋ_a2、ｋ_b2の中で小さい方の値ｋ_a3、ｋ_b3に基づいて、加速終了地点と減速開始地点での速度上限パラメータｖ_a、ｖ_bをそれぞれ算出している。また、加速時間、減速時間および速度上限パラメータを用いた場合に、等速区間があるかどうかの判別を行い、等速区間が有りの場合、等速区間の中点で速度上限パラメータを修正し、中点の関節変位ｑ_mを算出し、次に、等速区間の中点から等速区間の速度で微小時間Δｔだけ動作終了地点側へ進んだ地点の関節変位ｑ_m2と、等速区間の中点から等速区間の速度で微小時間Δｔだけ動作開始地点側へ戻った地点の関節変位ｑ_m3を算出し、さらに中点での速度ｖ_mおよび加速度ａ_mを算出し、中点で、全ての軸のモータおよび減速機の許容トルクの制約を満たす範囲で最大の速度倍率ｋ_maを算出し、さらに中点でのモータ最高速度の制限を満たす範囲で最大の速度倍率ｋ_mvを算出し、算出された速度倍率ｋ_maおよびｋ_mvの中で小さい方の値ｋ_mが、１より小さい場合のみ、その小さい方の値（速度倍率ｋ_m）に基づいて、速度上限パラメータをｋ_m ^*ｖｍａｘにそれぞれ書き換えるようにしたことを特徴としている。したがって、等速区間が存在する場合にも、実施の形態１と同様に、動作毎に最適な速度上限パラメータを設定できる効果が得られる。

実施の形態４．
図４はこの発明の実施の形態４によるロボット制御装置の構成を示すブロック図である。図において、図１と同一または相当する部分には同一符号を付し、原則としてその説明を省略する。
この実施の形態４における基準速度演算手段２０は、図１の基準速度演算手段２と同様に、ロボットの現在動作についての各軸もしくは直交座標系における各方向の基準速度を演算する手段であるが、Ａ点からＢ点に移動する移動命令が各関節を同期して動作させる関節補間の場合には、ロボットの各軸毎の許容最高速度に基づいて当該動作についての各軸における基準速度を演算する。また、Ａ点からＢ点に移動する移動命令が直線上を動作する直線補間の場合には、直交座標系における基準最高速度に基づいて当該動作についての直交座標系における各方向の基準速度を算出する。指令曲線生成手段３０は、図１の指令曲線生成手段３と同様に指令曲線を生成する手段であるが、この場合には基準速度演算手段２０で算出された当該動作の最高速度と、動作パラメータ補正手段１２で補正された動作パラメータに基づいて指令曲線の生成を行う。

加減速時間・速度上限パラメータ決定手段７０は、Ａ点からＢ点に移動する移動命令が関節補間の場合には、動作毎にロボットパラメータ記憶手段６に記憶されているモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間および減速時間を算出する。また、Ａ点からＢ点に移動する移動命令が直線補間の場合は、モータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出する。関節補間の場合の加速時間および減速時間の算出方法は、上記特許文献１の加減速時間決定手段による方法と同じである。また、直線補間の場合の加速時間、減速時間および速度上限パラメータの算出方法は、上記実施の形態１の加減速時間・速度上限パラメータ決定手段７による方法と同じである。

トルク使用率演算手段１０は、加減速時間・速度上限パラメータ決定手段７０の内部で計算した各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔ_miおよび加減速時間・速度上限パラメータ決定手段７０で最終的に求めた加速時間ｋｔから各軸の加速区間のトルク使用率ｔｒａｔｅｋ_iを次式（３３）で算出する。
ｔｒａｔｅｋ_i＝ｋｔ_mi／ｋｔ （３３）
同様に、各軸のモータトルク制限を満たす範囲で最短となる減速時間ｇｔ_mi、加減速時間・速度上限パラメータ決定手段７０で最終的に求めた減速時間ｇｔから各軸の減速区間のトルク使用率ｔｒａｔｅｇ_iを次式（３４）で算出する。
ｔｒａｔｅｇ_i＝ｇｔ_mi／ｇｔ （３４）

補正率記憶手段１１には、ロボット動作命令が記載されておりロボット制御装置で処理されるプログラム中に記載された各軸毎の補正率の設定値ｈｏｓｅ_iが記憶されている。動作パラメータ補正手段１２は、トルク使用率演算手段１０で演算したトルク使用率と補正率記憶手段１１に記憶された各軸の補正率を用いて、加減速時間・速度上限パラメータ決定手段７０で算出された加速時間および減速時間を次のようにして補正し、指令曲線生成手段３０に出力する。

動作パラメータ補正手段１２において、まず、トルク使用率演算手段１０で求めた各軸の加速区間のトルク使用率と補正率記憶手段１１に記憶されている各軸の補正率の設定値から補正候補値ａｒａｔｅｋ_iを次式（３５）で算出する。
ａｒａｔｅｋ_i＝ｔｒａｔｅｋ_i ^*１００／ｈｏｓｅ_i （３５）
算出された補正候補値ａｒａｔｅｋ_iの中から最大値ａｒａｔｅｋｍａｘを求める。ここで、ａｒａｔｅｋｍａｘ≦１の場合は加速時間を補正せず、ａｒａｔｅｋｍａｘ＞１の場合は加速時間ｋｔをｋｔ^*ａｒａｔｅｋｍａｘに書き換える。
同様に、減速区間の各軸のトルク使用率と補正率の設定値から補正候補値ａｒａｔｅｇ_iを次式（３６）で算出する。
ａｒａｔｅｇ_i＝ｔｒａｔｅｇ_i ^*１００／ｈｏｓｅ_i （３６）
算出された補正候補値ａｒａｔｅｇ_iの中から最大値ａｒａｔｅｇｍａｘを求める。ａｒａｔｅｇｍａｘ≦１の場合、減速時間は補正しない。ａｒａｔｅｇｍａｘ＞１の場合は減速時間ｇｔをｇｔ^*ａｒａｔｅｇｍａｘに書き換える。なお、直線補間の場合に加減速時間・速度上限パラメータ決定手段７０が算出する速度上限パラメータは補正せずそのまま指令曲線生成手段に出力する。

以上のように、この実施の形態４によれば、加減速時間・速度上限パラメータ決定手段７０で、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、関節補間の場合には、動作毎にロボットパラメータ記憶手段６に記憶されている各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出し、また、直線補間の場合には、モータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出し、トルク使用率演算手段１０で、加減速時間・速度上限パラメータ決定手段７０により算出された最短の加速時間および最短の減速時間に基づいて各軸の加速区間のトルク使用率および各軸の減速区間のトルク使用率を算出し、算出された各軸の加速区間および減速区間の各トルク使用率および補正率記憶手段１１に記憶された各軸毎の補正率に基づいて、動作パラメータ補正手段１２により、加減速時間・速度上限パラメータ決定手段７０で算出された加速時間および減速時間を補正するようにしたので、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。なお、ここで動作パラメータの修正については加速時間および減速時間に限定される必要はない。また、加減速時間・速度上限パラメータの決定については、速度上限パラメータの算出も行う場合に限定される必要はない。速度上限パラメータの算出を行わない場合は、加減速時間・速度上限パラメータ決定手段７０は、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速決定手段に置き換えればよい。

また、具体的処理としてこの実施の形態４では、トルク使用率演算手段１０は、加減速時間・速度上限パラメータ決定手段７０で算出した各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔ_mi、減速時間ｇｔ_miおよび最終的に求めた加速時間ｋｔ、減速時間ｇｔに基づいて各軸の加速区間および減速区間のトルク使用率を算出し、動作パラメータ補正手段１２により、トルク使用率演算手段１０で求めた各軸の加速区間および減速区間の各トルク使用率と補正率記憶手段１１に記憶されている各軸の補正率の設定値に基づいてそれぞれの区間の補正候補値を算出し、算出された補正候補値の中から最大値をそれぞれ求め、求めた最大値が１より大きい場合のみ、最終的に求めた加速時間ｋｔおよび減速時間ｇｔをその１より大きい補正候補値の最大値に基づいてそれぞれｋｔ^*ａｒａｔｅｋｍａｘおよびｇｔ^*ａｒａｔｅｇｍａｘに置き換えるようにしたことを特徴としている。

実施の形態５．
実施の形態５によるロボット制御装置の構成も図４で表される。実施の形態４とは動作パラメータ補正手段１２と指令曲線生成手段３０の動作が異なるので、その異なる部分についてのみ説明する。
まず、動作パラメータ補正手段１２では、加速区間のトルク使用率と補正率の設定値から補正候補値ｖｒａｔｅｋ_iを次式（３７）で算出する。
ｖｒａｔｅｋ_i＝ｔｒａｔｅｋ_i ^*１００／ｈｏｓｅ_i （３７）
算出された補正候補値ｖｒａｔｅｋ_iの中から最大値ｖｒａｔｅｋｍａｘを求める。
同様に減速区間のトルク使用率と補正率の設定値から補正候補値ｖｒａｔｅｇ_iを次式（３８）で算出する。
ｖｒａｔｅｇ_i＝ｔｒａｔｅｇ_i ^*１００／ｈｏｓｅ_i （３８）
算出された補正候補値ｖｒａｔｅｇ_iの中から最大値ｖｒａｔｅｇｍａｘを求める。

次に、２つの補正候補値の最大値ｖｒａｔｅｋｍａｘとｖｒａｔｅｇｍａｘの中で大きい方を最終補正候補値ｖｒａｔｅｍａｘとする。ｖｒａｔｅｍａｘ＞１の時に、基準速度修正率ｏｖｒｄを次式（３９）で算出する。
ｏｖｒｄ＝１／ｖｒａｔｅｍａｘ （３９）
また、ｖｒａｔｅｍａｘ≦１のときはｏｖｒｄ＝１とする。指令曲線生成手段３０へは、基準速度修正率ｏｖｒｄと加減速時間・速度上限パラメータ決定手段７０で算出した加速時間、減速時間および速度上限パラメータを出力する。
指令曲線生成手段３０では、基準速度演算手段２０で演算した基準速度に動作パラメータ補正手段１２で算出した基準速度修正率ｏｖｒｄを乗じた値を基準速度として指令曲線を生成する。

以上のように、この実施の形態５によれば、トルク使用率演算手段１０では、加減速時間・速度上限パラメータ決定手段７０で算出された各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔ_mi、減速時間ｇｔ_miおよび最終的に求めた加速時間ｋｔ、減速時間ｇｔに基づいて各軸の加速区間および減速区間のトルク使用率をそれぞれ算出し、動作パラメータ補正手段１２により、算出された各軸の加速区間および減速区間のトルク使用率と補正率の設定値に基づいて補正候補値をそれぞれ算出し、算出された補正候補値の中からそれぞれ各区間での補正候補値の最大値を抽出し、抽出された各区間での最大値の中で、大きい方の値に基づいて基準速度修正率を算出し、指令曲線生成手段では、基準速度演算手段２０で算出した基準速度に算出された基準速度修正率を乗じて修正した基準速度とし、指令曲線を生成するようにしたことを特徴としている。したがって、実施の形態４と同様にして、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。

実施の形態６．
実施の形態６によるロボット制御装置の構成も図４で表される。実施の形態４とは動作パラメータ補正手段１２および指令曲線生成手段３０の動作が異なるので、その異なる部分についてのみ説明する。
まず、動作パラメータ補正手段１２では、加速区間のトルク使用率と補正率の設定値から補正候補値ｄｒａｔｅｋ_iを次式（４０）から算出する。
ｄｒａｔｅｋ_i＝ｔｒａｔｅｋ_i ^*１００／ｈｏｓｅ_i （４０）
算出された補正候補値ｄｒａｔｅｋ_iの最大値ｄｒａｔｅｋｍａｘを求める。同様に減速区間のトルク使用率と補正率の設定値から次式（４１）から補正候補値ｄｒａｔｅｇ_iを算出する。
ｄｒａｔｅｇ_i＝ｔｒａｔｅｇ_i ^*１００／ｈｏｓｅ_i （４１）
算出された補正候補値ｄｒａｔｅｇ_iの最大値ｄｒａｔｅｇｍａｘを求める。
次に、ｄｒａｔｅｋｍａｘ、ｄｒａｔｅｇｍａｘの中で大きい方をｄｒａｔｅｍａｘとする。ｄｒａｔｅｍａｘ＞１の時に、待ち時間修正率ｒｄｅｌａｙを
ｒｄｅｌａｙ＝ｄｒａｔｅｍａｘ （４２）
で算出する。また、ｄｒａｔｅｍａｘ≦１のときはｒｄｅｌａｙ＝１とする。

動作パラメータ補正手段１２は、待ち時間修正率ｒｄｅｌａｙを算出すると、その値を加減速時間・速度上限パラメータ決定手段７０で算出した加速時間、減速時間および速度上限パラメータと共に指令曲線生成手段３０へ出力する。指令曲線生成手段３０では、ロボット動作命令が記載されておりロボット制御装置で処理されるプログラム上に動作と動作の間で待ち時間が指定されているとき、その指定された待ち時間に待ち時間修正率ｒｄｅｌａｙを乗じて修正し、修正された待ち時間だけ停止してから次の動作に移行する。

以上のように、この実施の形態６によれば、トルク使用率演算手段１０では、加減速時間・速度上限パラメータ決定手段７０で算出された各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔ_mi、減速時間ｇｔ_miおよび最終的に求めた加速時間ｋｔ、減速時間ｇｔに基づいて各軸の加速区間および減速区間のトルク使用率をそれぞれ算出し、動作パラメータ補正手段１２により、算出された各軸の加速区間および減速区間のトルク使用率と補正率の設定値に基づいて補正候補値をそれぞれ算出し、算出された補正候補値の中からそれぞれ各区間での補正候補値の最大値を抽出し、抽出された各区間での補正候補値の最大値の中で、大きい方の値に基づいて待ち時間修正率を算出し、指令曲線生成手段３０では、動作と動作の間で待ち時間が指定されているとき、ロボット動作命令が記載されておりロボット制御装置で処理されるプログラム上に記載された待ち時間に算出された待ち時間修正率を乗じて修正し、修正された待ち時間に応じた動作停止を行ってから次の動作に移行する指令曲線を生成するようにしたことを特徴としている。したがって、実施の形態４と同様に、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。

実施の形態７．
実施の形態７によるロボット制御装置の構成も図４で表わされる。実施の形態４とはトルク使用率演算手段１０での動作が異なるので、その異なる部分についてのみ説明する。
トルク使用率演算手段１０は、加減速時間・速度上限パラメータ決定手段７０内部で計算した加速終了地点における慣性行列Ｍ_c、関節速度ｖ_1b、遠心・コリオリ力、重力、摩擦力の和ｈ_cと加速時間ｋｔから、加速終了地点でのモータトルクτ_mkを次式（４３）で算出する。
τ_mk＝Ｍ_cｖ_1b／ｋｔ＋ｈ_c （４３）
加速終了地点でのモータトルクτ_mkの第ｉ軸要素をτ_mkiとし、各軸の加速区間のトルク使用率ｔｒａｔｅｋ_iを次式（４４）から算出する。
ｔｒａｔｅｋ_i＝ａｂｓ（τ_mki）／τ_maxmi （４４）
ここで、τ_maxmiは各軸モータの許容最大トルクである。同様に、加減速時間・速度上限パラメータ決定手段７０内部で計算した減速開始地点における慣性行列Ｍ_d、関節速度ｖ_2a、遠心・コリオリ力、重力、摩擦力の和ｈ_dと減速時間ｇｔから、減速開始地点でのモータトルクτ_mgを次式（４５）で算出する。
τ_mg＝−Ｍ_dｖ_2a／ｇｔ＋ｈ_d （４５）
減速開始地点でのモータトルクτ_mgの第ｉ軸要素をτ_mgiとし、各軸の減速区間のトルク使用率ｔｒａｔｅｇ_iを次式（４６）から算出する。
ｔｒａｔｅｇ_i＝ａｂｓ（τ_mgi）／τ_maxmi （４６）
算出された各軸の加速区間のトルク使用率および減速区間のトルク使用率は、動作パラメータ補正手段１２に出力され、動作パラメータ補正手段１２において実施の形態４のように加速時間および減速時間の修正に用いられる。

以上のように、この実施の形態７によれば、トルク使用率演算手段１０では、加減速時間・速度上限パラメータ決定手段７０で計算した加速終了地点および減速開始地点における慣性行列、関節速度、遠心・コリオリ力、重力、摩擦力の和と加速時間ｋｔ、減速時間ｇｔに基づいて各地点でのモータトルクτ_mk、τ_mgを算出し、これらのモータトルクτ_mk、τ_mgと各軸のモータの許容最大トルクに基づいて各軸の加速区間および減速区間のトルク使用率を算出し、動作パラメータ補正手段１２では、算出された各軸の加速区間および減速区間の各トルク使用率と補正率記憶手段に記憶されている各軸の補正率の設定値に基づいてそれぞれの区間の補正候補値を算出し、加速時間および減速時間の修正に用いられることを特徴としている。したがって、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。

この発明の実施の形態１乃至実施の形態３によるロボット制御装置の構成を示すブロック図である。 この発明の実施の形態１および実施の形態２に係る加減速時間・速度上限パラメータ決定手段の処理手順を示すフローチャートである。 この発明の実施の形態３に係る加減速時間・速度上限パラメータ決定手段の処理手順を示すフローチャートである。 この発明の実施の形態４乃至実施の形態７によるロボット制御装置の構成を示すブロック図である。

符号の説明

１ 教示点記憶手段、２，２０ 基準速度演算手段、３，３０ 指令曲線生成手段、４ モータ制御手段、５ ロボット、６ ロボットパラメータ記憶手段、７，７０ 加減速時間・速度上限パラメータ決定手段、１０ トルク使用率演算手段、１１ 補正率記憶手段、１２ 動作パラメータ補正手段。

Claims (3)

1. ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、
   入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、前記ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、前記直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、
   入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に前記各軸を駆動するモータの駆動トルクの許容最大値、前記各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速時間決定手段と、
   算出された前記最短の加速時間に基づいて各軸の加速区間のトルク使用率を算出し、前記最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出するトルク使用率演算手段と、
   ロボットの各軸の補正率を予め記憶する補正率記憶手段と、
   各軸のトルク使用率を各軸の補正率の設定値で除算して得られる各軸の補正候補値を算出し、算出した補正候補値の最大値が１より大きくなる場合に、加速時間及び減速時間を補正候補値の最大値を乗じた値に修正する動作パラメータ修正手段と、
   算出された基準速度、加速時間、減速時間と修正された加速時間、減速時間に基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたロボット制御装置。
2. ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、
   入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、前記ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、前記直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、
   入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に前記各軸を駆動するモータの駆動トルクの許容最大値、前記各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速時間決定手段と、
   算出された前記最短の加速時間に基づいて各軸の加速区間のトルク使用率を算出し、前記最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出するトルク使用率演算手段と、
   ロボットの各軸の補正率を予め記憶する補正率記憶手段と、
   各軸のトルク使用率を各軸の補正率の設定値で除算して得られる各軸の補正候補値を算出し、算出した補正候補値の最大値が１より大きくなる場合に、基準速度に乗じる基準速度修正率を補正候補値の最大値で除算した値に修正する動作パラメータ修正手段と、
   算出された基準速度、加速時間、減速時間と修正された基準速度修正率に基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたロボット制御装置。
3. ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、
   入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、前記ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、前記直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、
   入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に前記各軸を駆動するモータの駆動トルクの許容最大値、前記各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速時間決定手段と、
   算出された前記最短の加速時間に基づいて各軸の加速区間のトルク使用率を算出し、前記最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出するトルク使用率演算手段と、
   ロボットの各軸の補正率を予め記憶する補正率記憶手段と、
   各軸のトルク使用率を各軸の補正率の設定値で除算して得られる各軸の補正候補値を算出し、算出した補正候補値の最大値が１より大きくなる場合に、待ち時間修正率を補正候補値の最大値に修正する動作パラメータ修正手段と、
   算出された基準速度、加速時間、減速時間と修正された待ち時間修正率に基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたロボット制御装置。

Images