JP2008132595A - ロボット制御装置 - Google Patents
ロボット制御装置 Download PDFInfo
- Publication number
- JP2008132595A JP2008132595A JP2008029595A JP2008029595A JP2008132595A JP 2008132595 A JP2008132595 A JP 2008132595A JP 2008029595 A JP2008029595 A JP 2008029595A JP 2008029595 A JP2008029595 A JP 2008029595A JP 2008132595 A JP2008132595 A JP 2008132595A
- Authority
- JP
- Japan
- Prior art keywords
- acceleration
- axis
- speed
- deceleration
- time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
【課題】直線補間や円弧補間の最高速度上限値を、モータあるいは伝達機構の許容値を考慮して動作毎に決定できるようにし動作の高速化を図る。
【解決手段】基準速度演算手段で、現在動作の各軸もしくは直交座標系における各方向の基準速度を算出する。加減速時間・速度上限パラメータ決定手段は、加速開始地点と減速終了地点の直交座標値と関節変位に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、減速時間を算出する。動作パラメータ修正手段は、加速時間、減速時間を修正する。指令曲線生成手段は、修正されたデータに基づいて指令曲線を生成する。
【選択図】図４
【解決手段】基準速度演算手段で、現在動作の各軸もしくは直交座標系における各方向の基準速度を算出する。加減速時間・速度上限パラメータ決定手段は、加速開始地点と減速終了地点の直交座標値と関節変位に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、減速時間を算出する。動作パラメータ修正手段は、加速時間、減速時間を修正する。指令曲線生成手段は、修正されたデータに基づいて指令曲線を生成する。
【選択図】図４
Description
この発明は、ロボットの各軸を駆動するモータの動作速度を制御するロボット制御装置に関するものである。
ロボットを動作させる際には加速度が同一でもロボットの姿勢に応じて必要なトルクは変化するため、動作毎に加減速時間などのパラメータを最適化する方法がとられている。この方法を適用した従来のロボット制御装置（例えば、特許文献１参照）では、次のような構成で動作する。
ロボットの動作開始点（加速開始地点）と動作終了点（減速終了地点）の座標を教示点として記憶しておく教示点記憶手段を備え、また、各軸もしくは各方向の最高速度パラメータ値等を予め記憶させているパラメータ記憶手段を備えている。教示点記憶手段で移動命令を受信すると、その移動命令を解析してどの点からどの点までロボットを移動させるかの判断を行う。例えば、その移動命令がＡ点（加速開始地点）からＢ点（減速終了地点）への直線補間命令である場合、この教示点記憶手段は、Ａ点およびＢ点の直交座標値および関節変位のデータを出力する。次に、最高速度演算手段により、この直交座標値とパラメータ記憶手段の最高速度パラメータ値に基づいて、その動作における各方向の最高速度を算出する。次に、教示点記憶手段から出力された直交座標値および関節変位と、パラメータ記憶手段に記憶されているトルクもしくは力の許容値等のパラメータと、最高速度演算手段で算出された最高速度に基づいて、加減速時間決定手段で、その動作における最適な加減速時間を決定する。最高速度と加減速時間が算出されると、これらの値に基づいて指令曲線生成手段で速度指令曲線を生成する。この場合、速度指令曲線が速度台形指令曲線になるように位置指令を生成し、その位置指令に対して移動平均フィルタおよび１次遅れフィルタなどによるスムージング処理が行われる。結果として、モータ制御手段は、生成された指令曲線にロボットが追従する動きをとるように各軸の駆動モータを制御する。
ロボットの動作開始点（加速開始地点）と動作終了点（減速終了地点）の座標を教示点として記憶しておく教示点記憶手段を備え、また、各軸もしくは各方向の最高速度パラメータ値等を予め記憶させているパラメータ記憶手段を備えている。教示点記憶手段で移動命令を受信すると、その移動命令を解析してどの点からどの点までロボットを移動させるかの判断を行う。例えば、その移動命令がＡ点（加速開始地点）からＢ点（減速終了地点）への直線補間命令である場合、この教示点記憶手段は、Ａ点およびＢ点の直交座標値および関節変位のデータを出力する。次に、最高速度演算手段により、この直交座標値とパラメータ記憶手段の最高速度パラメータ値に基づいて、その動作における各方向の最高速度を算出する。次に、教示点記憶手段から出力された直交座標値および関節変位と、パラメータ記憶手段に記憶されているトルクもしくは力の許容値等のパラメータと、最高速度演算手段で算出された最高速度に基づいて、加減速時間決定手段で、その動作における最適な加減速時間を決定する。最高速度と加減速時間が算出されると、これらの値に基づいて指令曲線生成手段で速度指令曲線を生成する。この場合、速度指令曲線が速度台形指令曲線になるように位置指令を生成し、その位置指令に対して移動平均フィルタおよび１次遅れフィルタなどによるスムージング処理が行われる。結果として、モータ制御手段は、生成された指令曲線にロボットが追従する動きをとるように各軸の駆動モータを制御する。
ここで、加減速時間決定手段が行う加減速時間決定の動作は、次のようにして行っている。
まず、加速時間と減速時間の初期値を算出する。次に、加速終了地点および減速開始地点での位置と速度を求め、加速開始地点および加速終了地点で加速時間を算出し直す。そして、加速開始地点で算出した加速時間と加速終了地点で算出した加速時間を比較し、大きい方を加速時間とする。減速時間に関しても同様に、減速開始地点および減速終了地点で減速時間を算出し直す。そして、減速開始地点で算出した減速時間と減速終了地点で算出した減速時間を比較し、大きい方を減速時間とする。上記一連の処理の繰り返し回数が、指定した回数に達したときは、現在の算出結果を加速時間および減速時間として出力する。繰り返し回数が指定した回数に達していなければ、上記一連の処理を繰り返し、現在の加速時間および減速時間に基づいて加速終了地点および減速開始地点の位置と速度を求めて、再度加減速時間の算出を行う。加速開始地点および加速終了地点では以下のようにして加速時間を算出する。
まず、加速時間と減速時間の初期値を算出する。次に、加速終了地点および減速開始地点での位置と速度を求め、加速開始地点および加速終了地点で加速時間を算出し直す。そして、加速開始地点で算出した加速時間と加速終了地点で算出した加速時間を比較し、大きい方を加速時間とする。減速時間に関しても同様に、減速開始地点および減速終了地点で減速時間を算出し直す。そして、減速開始地点で算出した減速時間と減速終了地点で算出した減速時間を比較し、大きい方を減速時間とする。上記一連の処理の繰り返し回数が、指定した回数に達したときは、現在の算出結果を加速時間および減速時間として出力する。繰り返し回数が指定した回数に達していなければ、上記一連の処理を繰り返し、現在の加速時間および減速時間に基づいて加速終了地点および減速開始地点の位置と速度を求めて、再度加減速時間の算出を行う。加速開始地点および加速終了地点では以下のようにして加速時間を算出する。
ロボットの運動方程式は、各関節の駆動トルクから構成される駆動トルクベクトルをτ、慣性行列をＭ、各関節の遠心・コリオリ力、重力、摩擦力の和から構成されるベクトルをｈ、各関節の加速度から構成されるベクトルをａとすると、
τ＝Ｍａ＋ｈ （１）
となる。ここで、各方向の速度指令が台形速度指令の場合、加速終了地点の各軸速度から構成されるベクトルをｖk，加速時間をｔkとすれば、加速区間におけるロボットの運動方程式は、
τ＝Ｍｖk／ｔk＋ｈ （２）
となる。式（２）のＭ、ｈを加速開始地点の位置と速度に基づいて算出した結果が、いずれの軸においてもそれぞれの軸の許容最大トルク以下となる制約を満たす範囲で最短となる加速時間ｔk1を算出するとともに、式（２）のＭ、ｈを加速終了地点の位置と速度に基づいて算出した結果が、いずれの軸においてもそれぞれの軸の許容最大トルク以下となる制約を満たす範囲で最短となる加速時間ｔk2を算出する。この算出したｔk1とｔk2を比較し、大きい方を加速時間ｔkとして用いる。一方、減速区間の運動方程式は、減速時間をｔg、減速開始地点の各軸速度から構成されるベクトルをｖgとすると、
τ＝−Ｍｖg／ｔg＋ｈ （３）
となるので、式（３）に基づいて減速開始地点および減速終了地点の位置と速度から、加速時間の算出と同様の方法で減速時間を算出する。
τ＝Ｍａ＋ｈ （１）
となる。ここで、各方向の速度指令が台形速度指令の場合、加速終了地点の各軸速度から構成されるベクトルをｖk，加速時間をｔkとすれば、加速区間におけるロボットの運動方程式は、
τ＝Ｍｖk／ｔk＋ｈ （２）
となる。式（２）のＭ、ｈを加速開始地点の位置と速度に基づいて算出した結果が、いずれの軸においてもそれぞれの軸の許容最大トルク以下となる制約を満たす範囲で最短となる加速時間ｔk1を算出するとともに、式（２）のＭ、ｈを加速終了地点の位置と速度に基づいて算出した結果が、いずれの軸においてもそれぞれの軸の許容最大トルク以下となる制約を満たす範囲で最短となる加速時間ｔk2を算出する。この算出したｔk1とｔk2を比較し、大きい方を加速時間ｔkとして用いる。一方、減速区間の運動方程式は、減速時間をｔg、減速開始地点の各軸速度から構成されるベクトルをｖgとすると、
τ＝−Ｍｖg／ｔg＋ｈ （３）
となるので、式（３）に基づいて減速開始地点および減速終了地点の位置と速度から、加速時間の算出と同様の方法で減速時間を算出する。
従来のロボット制御装置は、以上のように構成されているので、ロボットの手先を直線に沿って動作させる直線補間や円弧に沿って動作させる円弧補間を行う場合、等速区間であっても関節レベルでは加減速を行うため、等速区間の速度が高すぎると、等速動作を継続するだけでモータあるいは伝達機構の許容値を超えてしまう場合があり、動作毎に自動的に手先速度の上限値を定めることができなかった。また、連続動作を行う際には、特定の軸のモータのみ温度上昇量が大きくなる場合があるが、これを防ぐために各軸毎の加速度補正割合、各軸毎の速度補正割合、各軸毎の待ち時間補正割合などの予め指定した割合と各軸のモータの負荷に応じて自動的に加速時間、減速時間などの動作パラメータを修正することができないなどの課題があった。
この発明は上記のような課題を解決するためになされたもので、第１の目的は、新規な方法により動作の高速化を図ることができるロボット制御装置を得るものである。
この発明に係るロボット制御装置は、ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速時間決定手段と、算出された最短の加速時間に基づいて各軸の加速区間のトルク使用率を算出し、最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出するトルク使用率演算手段と、ロボットの各軸の補正率を予め記憶する補正率記憶手段と、各軸のトルク使用率を各軸の補正率の設定値で除算して得られる各軸の補正候補値を算出し、算出した補正候補値の最大値が１より大きくなる場合に、加速時間及び減速時間を補正候補値の最大値を乗じた値に修正する動作パラメータ修正手段と、算出された基準速度、加速時間、減速時間と修正された加速時間、減速時間に基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたものである。
この発明は、予め指定した割合と各軸のモータの負荷に応じて動作パラメータを修正できるため、指定されたモータの負荷を抑えながらロボットの高速化が実現できる効果がある。
実施の形態１．
図１はこの発明の実施の形態１によるロボット制御装置の構成を示すブロック図である。図において、教示点記憶手段１は、ロボットの加速開始地点および減速終了地点の直交座標を記憶しており、例えばＡ点からＢ点に移動する移動命令を受信したときにＡ点およびＢ点の直交座標値および関節変位を出力する手段である。ロボットパラメータ記憶手段６は、ロボットの機種毎に定められている各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値等の制約条件と、質量、重心位置等のロボットの動力学特性を表現するためのパラメータ値を予め記憶している手段である。基準速度演算手段２は、ロボットパラメータ記憶手段６に記憶されているロボットの各軸毎の許容最高速度もしくは直交座標系における基準最高速度に基づいて、ロボットの現在動作についての各軸もしくは直交座標系における各方向の動作開始から加速時間だけ経過したときに達する基準速度を算出する手段である。
図１はこの発明の実施の形態１によるロボット制御装置の構成を示すブロック図である。図において、教示点記憶手段１は、ロボットの加速開始地点および減速終了地点の直交座標を記憶しており、例えばＡ点からＢ点に移動する移動命令を受信したときにＡ点およびＢ点の直交座標値および関節変位を出力する手段である。ロボットパラメータ記憶手段６は、ロボットの機種毎に定められている各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値等の制約条件と、質量、重心位置等のロボットの動力学特性を表現するためのパラメータ値を予め記憶している手段である。基準速度演算手段２は、ロボットパラメータ記憶手段６に記憶されているロボットの各軸毎の許容最高速度もしくは直交座標系における基準最高速度に基づいて、ロボットの現在動作についての各軸もしくは直交座標系における各方向の動作開始から加速時間だけ経過したときに達する基準速度を算出する手段である。
加減速時間・速度上限パラメータ決定手段７は、動作毎にロボットパラメータ記憶手段６に記憶されているモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、最短の減速時間および最大の速度上限パラメータを算出する手段である。指令曲線生成手段３は、基準速度演算手段２で算出されたロボットの現在動作の基準速度、加減速時間・速度上限パラメータ決定手段７で算出された当該動作の加速時間、減速時間および速度上限パラメータに基づいて指令曲線を生成する手段である。モータ制御手段４は、指令曲線生成手段３で生成された指令曲線に従ってロボット５の各軸を駆動するモータを制御する。
次に動作について説明する。
教示点記憶手段１が、例えばＡ点（加速開始地点）からＢ点（減速終了地点）まで直線補間で移動する命令を受け取ると、教示点記憶手段１からは、Ａ点およびＢ点の直交座標値および関節変位が読み出され、基準速度演算手段２と加減速時間・速度上限パラメータ決定手段７に与えられる。基準速度演算手段２では、これらデータとロボットパラメータ記憶手段６に記憶されている直交座標系の各方向における基準最高速度に基づいて、当該動作における各方向の基準速度が算出され、加減速時間・速度上限パラメータ決定手段７と指令曲線生成手段３に与えられる。
教示点記憶手段１が、例えばＡ点（加速開始地点）からＢ点（減速終了地点）まで直線補間で移動する命令を受け取ると、教示点記憶手段１からは、Ａ点およびＢ点の直交座標値および関節変位が読み出され、基準速度演算手段２と加減速時間・速度上限パラメータ決定手段７に与えられる。基準速度演算手段２では、これらデータとロボットパラメータ記憶手段６に記憶されている直交座標系の各方向における基準最高速度に基づいて、当該動作における各方向の基準速度が算出され、加減速時間・速度上限パラメータ決定手段７と指令曲線生成手段３に与えられる。
加減速時間・速度上限パラメータ決定手段７の内部では、図２に示される処理手順に従って、モータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータが決定される。
まず、当該動作の加速時間と減速時間の初期値と、基準速度に乗じる倍率許容値、すなわち速度上限パラメータの初期値を設定する（ステップＳＴ１１）。次に、繰り返し回数ｎを初期値１に設定する（ステップＳＴ１２）。教示点記憶手段１から与えられたＡ点およびＢ点の直交座標値と、基準速度演算手段２で算出された当該動作における各方向の基準速度と、現在動作における加減速時間および速度上限パラメータとから、加速終了地点および減速開始地点における直交座標値、関節変位、各方向での速度、各軸の速度を算出する（ステップＳＴ１３）。
まず、当該動作の加速時間と減速時間の初期値と、基準速度に乗じる倍率許容値、すなわち速度上限パラメータの初期値を設定する（ステップＳＴ１１）。次に、繰り返し回数ｎを初期値１に設定する（ステップＳＴ１２）。教示点記憶手段１から与えられたＡ点およびＢ点の直交座標値と、基準速度演算手段２で算出された当該動作における各方向の基準速度と、現在動作における加減速時間および速度上限パラメータとから、加速終了地点および減速開始地点における直交座標値、関節変位、各方向での速度、各軸の速度を算出する（ステップＳＴ１３）。
次に、加速開始地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の加速時間ｔ1ai（ｉ＝１〜ｊ、ｊはロボットの軸数）をそれぞれ算出し、得られたｔ1aiの最大値をｔ1aとする。同様に、加速終了地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の加速時間ｔ1biをそれぞれ算出し、ｔ1biの最大値をｔ1bとする（ステップＳＴ１４）。これらの算出された最大値ｔ1a、ｔ1bの中で、大きい方の値を今回の加速時間ｔ1として決定する（ステップＳＴ１５）。
同様にして減速時間も求める。まず、減速開始地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の減速時間ｔ2ai（ｉ＝１〜ｊ、ｊはロボットの軸数）をそれぞれ算出し、得られたｔ2aiの最大値をｔ2aとする。同様に減速終了地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の減速時間ｔ2biをそれぞれ算出し、得られたｔ2biの最大値をｔ2bとする（ステップＳＴ１６）。これらの算出された最大値ｔ2a、ｔ2bの中で、大きい方の値を今回の減速時間ｔ2として決定する（ステップＳＴ１７）。
同様にして減速時間も求める。まず、減速開始地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の減速時間ｔ2ai（ｉ＝１〜ｊ、ｊはロボットの軸数）をそれぞれ算出し、得られたｔ2aiの最大値をｔ2aとする。同様に減速終了地点でモータおよび伝達機構の許容トルクの制約を満たす範囲で最短の減速時間ｔ2biをそれぞれ算出し、得られたｔ2biの最大値をｔ2bとする（ステップＳＴ１６）。これらの算出された最大値ｔ2a、ｔ2bの中で、大きい方の値を今回の減速時間ｔ2として決定する（ステップＳＴ１７）。
次に、加速終了地点の関節変位、各方向の速度から、各軸のモータおよび伝達機構の許容トルクの制約を満たす範囲で等速運動を続けることができる最大の速度上限パラメータｖaを算出する。また、減速開始地点の関節変位、各方向の速度から、各軸のモータおよび伝達機構の許容トルクの制約を満たす範囲で等速運動を続けることができる最大の速度上限パラメータｖbを算出する（ステップＳＴ１８）。現在の速度上限パラメータｖｍａｘ（ｎ−１）、算出された最大の速度上限パラメータｖa、ｖbの中で最小のものを今回の繰り返しにおける速度上限パラメータｖｍａｘ（ｎ）として決定する（ステップＳＴ１９）。
繰り返し回数ｎが規定値Ｋを超えていれば、今回の加速時間、減速時間、および速度上限パラメータを加減速時間・速度上限パラメータ決定手段７で最終的に求めた加速時間ｋｔ、最終的に求めた減速時間ｇｔ、最終的に求めた速度上限パラメータｖｍａｘとして指令曲線生成手段３に出力する（ステップＳＴ２０）。繰り返し回数ｎが規定値Ｋを超えていない場合は、繰り返し回数ｎを１だけ増加させ（ステップＳＴ２１）、ステップＳＴ１３からステップＳＴ１９の演算を再度行い、加速時間、減速時間および速度上限パラメータの算出を行う。
指令曲線生成手段３では、基準速度演算手段２から出力される基準速度と、加減速時間・速度上限パラメータ決定手段７から出力される加速時間、減速時間および速度上限パラメータに基づいた時々刻々の指令曲線を生成し、モータ制御手段４に出力する。モータ制御手段４は、指令曲線生成手段３で生成された時々刻々の指令曲線に従ってロボット５を駆動するモータを制御する。
次に、上記ステップＳＴ１３における加速終了地点および減速開始地点での速度および変位の算出、ステップＳＴ１４、ＳＴ１６における加速時間、減速時間の算出と、ステップＳＴ１８における速度上限パラメータの算出の詳細について説明する。
まず、ステップＳＴ１３では、基準速度ｖ0、速度上限パラメータｖｍａｘから、この動作の直線速度ｖlを式（４）で算出する。
ｖl＝ｖｍａｘ*ｖ0 （４）
次に、直線速度ｖl、動作開始地点の直交座標値および動作終了地点の直交座標値、現在の加速時間、減速時間から、加速終了地点（Ｃ点）および減速開始地点（Ｄ点）での直交座標値を算出する。算出した直交座標値から加速終了地点の関節変位ｑcと減速開始地点での関節変位ｑdを算出する。加速終了地点から微小時間Δｔだけ、直線速度ｖlで動作終了地点Ｂへ進んだ地点（Ｃ２点）の直交座標値と関節変位ｑc2を算出する。また、減速開始地点から微小時間Δｔだけ直線速度ｖlで動作開始地点Ａへ戻った地点（Ｄ２点）の直交座標値と関節変位ｑd2を算出する。
まず、ステップＳＴ１３では、基準速度ｖ0、速度上限パラメータｖｍａｘから、この動作の直線速度ｖlを式（４）で算出する。
ｖl＝ｖｍａｘ*ｖ0 （４）
次に、直線速度ｖ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）／Δｔ （８）
ｖ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を算出する。
τ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である。
ｖ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 （１７）
τ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を算出する。
ｋmi＝（τmmaxi−ｈi）／ｍi （１８）
また、ｍi＜０の場合は（１９）となる。
ｋmi＝（−τmmaxi−ｈi）／ｍi （１９）
また、ｍi＝０の場合は任意のｋでモータトルクの制限τmmaxiを満たすので、予め定めた正の大きな値とする。全ての軸のモータトルク制限を満たすｋの最大値と減速機のトルク制限を満たすｋの最大値の中から最小値を速度倍率ｋaとして算出する。
減速開始地点でも減速開始地点での加速度、速度、関節変位から、加速開始地点での演算と全く同様にして、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大のｋである速度倍率ｋbを算出する。
次に、現在の速度上限パラメータｖｍａｘと速度倍率ｋaから加速終了地点での速度上限パラメータｖaを次式（２０）により算出する。
ｖa＝ｖｍａｘ*ｋa （２０）
さらに現在の速度上限パラメータｖｍａｘとｋbから減速開始地点での速度上限パラメータを式（２１）で算出する。
ｖb＝ｖｍａｘ*ｋb （２１）
ｖa＝ｖｍａｘ*ｋa （２０）
さらに現在の速度上限パラメータｖｍａｘとｋbから減速開始地点での速度上限パラメータを式（２１）で算出する。
ｖb＝ｖｍａｘ*ｋb （２１）
以上のように、この実施の形態１によれば、ロボットパラメータ記憶手段６に、ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しておき、基準速度演算手段２により、入力される加速開始地点と減速終了地点の直交座標値と関節変位、および各軸の許容最高速度もしくは直交座標系における基準最高速度に基づいて、ロボットの現在動作についての各軸もしくは直交座標系における各方向の動作開始から加速時間経過したときに達する基準速度を算出し、加減速時間・速度上限パラメータ決定手段７において、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値、各軸の許容最高速度の少なくとも１つの制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出し、指令曲線生成手段３により、算出された当該現在動作の基準速度、加速時間、減速時間および最大の速度上限パラメータに基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成するようにしている。したがって、動作毎に最適な速度上限値および加減速パラメータを設定できるため、ロボットの高速化が図れる効果が得られる。
また、そのための具体性として、この実施の形態１では、加減速時間・速度上限パラメータ決定手段７は、規定の繰り返し回数で演算を行い、与えられた加速開始地点および減速終了地点の直交座標値と、基準速度演算手段２で演算された当該動作における各方向の基準速度と、現在の加速時間、減速時間および速度上限パラメータとに基づいて加速終了地点および減速開始地点における直交座標値、関節変位、各方向での速度、各軸の速度を算出し、加速開始地点と加速終了地点のそれぞれにおいて、モータおよび伝達機構の許容トルクの制約を満たす範囲で各軸の最短の加速時間をそれぞれ算出し、得られた各地点での最短の加速時間の最大値ｔ1a、ｔ1bをそれぞれ抽出し、抽出されたこれらの最大値ｔ1a、ｔ1bの中で、大きい方の値を今回の加速時間として決定し、また、減速開始地点と減速終了地点のそれぞれにおいて、モータおよび伝達機構の許容トルクの制約を満たす範囲で各軸の最短の減速時間をそれぞれ算出し、得られた各地点での最短の減速時間の最大値ｔ2a、ｔ2bをそれぞれ抽出し、抽出されたこれらの最大値ｔ2a、ｔ2bの中で、大きい方の値を今回の減速時間として決定し、加速終了地点と減速開始地点のそれぞれの関節変位、各方向の速度から、各軸のモータおよび伝達機構の許容トルクの制約を満たす範囲で等速運動を続けることができる各地点の最大の速度上限パラメータｖa、ｖbをそれぞれ算出し、算出されたこれら最大の速度上限パラメータｖa、ｖbおよび現在の速度上限パラメータの中で、最小の値を今回の速度上限パラメータとして決定するようにしたことを特徴としている。
実施の形態２．
実施の形態２によるロボット制御装置の構成も図１で表され、加減速時間・速度上限パラメータ決定手段７の内部における加速時間、減速時間および速度上限パラメータ決定までの処理の流れも図２のフローチャートで表される。実施の形態１とは、ステップＳＴ１８における速度上限パラメータの算出方法のみが異なるので、ステップＳＴ１８の動作について説明する。
ステップＳＴ１８の内部でも加速終了地点での加速度ａc、減速開始地点での加速度ａdの算出までは実施の形態１と同一である。実施の形態１では速度がｋ倍されたときに遠心・コリオリ力が変化する影響は無視していたが、実施の形態２の場合は、遠心・コリオリ力の変化も考慮してモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率を算出するようにしたものである。
実施の形態２によるロボット制御装置の構成も図１で表され、加減速時間・速度上限パラメータ決定手段７の内部における加速時間、減速時間および速度上限パラメータ決定までの処理の流れも図２のフローチャートで表される。実施の形態１とは、ステップＳＴ１８における速度上限パラメータの算出方法のみが異なるので、ステップＳＴ１８の動作について説明する。
ステップＳＴ１８の内部でも加速終了地点での加速度ａc、減速開始地点での加速度ａdの算出までは実施の形態１と同一である。実施の形態１では速度がｋ倍されたときに遠心・コリオリ力が変化する影響は無視していたが、実施の形態２の場合は、遠心・コリオリ力の変化も考慮してモータおよび減速機の許容トルクの制約を満たす範囲で最大となる速度倍率を算出するようにしたものである。
まず、加速終了地点での関節変位ｑc、関節速度ｖ1bから算出した遠心・コリオリ力をｃc、重力をｇc、クーロン摩擦力をｆccとすると、上記式（１４）、（１５）は、それぞれ次式（２２）、（２３）のように変形できる。
τc＝ｋ2*ｃc＋ｋ*（Ｍcａc＋ｆvc）＋（ｇc＋ｆcc） （２２）
τlc＝ｋ2*ｃc＋ｋ*Ｍlcａc＋ｇc （２３）
式（２２）、（２３）は速度倍率ｋの２次式なので、ｋ2およびｋの係数の符号毎に場合わけを行い、各軸のモータのトルク許容値を満たす範囲で最大の速度倍率ｋと、各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋを算出し、その中の最小値をｋaとすれば、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大の速度倍率ｋaが算出される。
τc＝ｋ2*ｃc＋ｋ*（Ｍcａc＋ｆvc）＋（ｇc＋ｆcc） （２２）
τlc＝ｋ2*ｃc＋ｋ*Ｍlcａc＋ｇc （２３）
式（２２）、（２３）は速度倍率ｋの２次式なので、ｋ2およびｋの係数の符号毎に場合わけを行い、各軸のモータのトルク許容値を満たす範囲で最大の速度倍率ｋと、各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋを算出し、その中の最小値をｋ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 （２５）
式（２４）、（２５）は速度倍率ｋの２次式なので、ｋ2およびｋの係数の符号毎に場合わけを行い、各軸のモータトルク許容値を満たす範囲で最大の速度倍率ｋと、各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋを算出し、その中の最小値を速度倍率ｋaとすれば、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大の速度倍率ｋbが算出される。
以上のようにして算出された速度倍率ｋa、ｋbからは、実施の形態１と同様に式（２０）、（２１）を用いて加速終了地点での速度上限パラメータｖaと減速開始地点での速度上限パラメータｖbが算出される。
τd＝ｋ2*ｃd＋ｋ*（Ｍdａd＋ｆvd）＋（ｇd＋ｆcd） （２４）
τld＝ｋ2*ｃd＋ｋ*Ｍldａd＋ｇd （２５）
式（２４）、（２５）は速度倍率ｋの２次式なので、ｋ2およびｋの係数の符号毎に場合わけを行い、各軸のモータトルク許容値を満たす範囲で最大の速度倍率ｋと、各軸の減速機の許容トルクの制約を満たす範囲で最大となる速度倍率ｋを算出し、その中の最小値を速度倍率ｋaとすれば、いずれの軸においてもモータおよび減速機の許容トルク以下となる制約を満たす範囲で最大の速度倍率ｋbが算出される。
以上のようにして算出された速度倍率ｋa、ｋbからは、実施の形態１と同様に式（２０）、（２１）を用いて加速終了地点での速度上限パラメータｖ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 （２９）
ｖｒ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）／Δｔ （３２）
ｖ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＝ｋｔmi／ｋｔ （３３）
同様に、各軸のモータトルク制限を満たす範囲で最短となる減速時間ｇｔmi、加減速時間・速度上限パラメータ決定手段７０で最終的に求めた減速時間ｇｔから各軸の減速区間のトルク使用率ｔｒａｔｅｇiを次式（３４）で算出する。
ｔｒａｔｅｇi＝ｇｔmi／ｇｔ （３４）
補正率記憶手段１１には、ロボット動作命令が記載されておりロボット制御装置で処理されるプログラム中に記載された各軸毎の補正率の設定値ｈｏｓｅiが記憶されている。動作パラメータ補正手段１２は、トルク使用率演算手段１０で演算したトルク使用率と補正率記憶手段１１に記憶された各軸の補正率を用いて、加減速時間・速度上限パラメータ決定手段７０で算出された加速時間および減速時間を次のようにして補正し、指令曲線生成手段３０に出力する。
動作パラメータ補正手段１２において、まず、トルク使用率演算手段１０で求めた各軸の加速区間のトルク使用率と補正率記憶手段１１に記憶されている各軸の補正率の設定値から補正候補値ａｒａｔｅｋiを次式（３５）で算出する。
ａｒａｔｅｋi＝ｔｒａｔｅｋi *１００／ｈｏｓｅi （３５）
算出された補正候補値ａｒａｔｅｋiの中から最大値ａｒａｔｅｋｍａｘを求める。ここで、ａｒａｔｅｋｍａｘ≦１の場合は加速時間を補正せず、ａｒａｔｅｋｍａｘ＞１の場合は加速時間ｋｔをｋｔ*ａｒａｔｅｋｍａｘに書き換える。
同様に、減速区間の各軸のトルク使用率と補正率の設定値から補正候補値ａｒａｔｅｇiを次式（３６）で算出する。
ａｒａｔｅｇi＝ｔｒａｔｅｇi *１００／ｈｏｓｅi （３６）
算出された補正候補値ａｒａｔｅｇiの中から最大値ａｒａｔｅｇｍａｘを求める。ａｒａｔｅｇｍａｘ≦１の場合、減速時間は補正しない。ａｒａｔｅｇｍａｘ＞１の場合は減速時間ｇｔをｇｔ*ａｒａｔｅｇｍａｘに書き換える。なお、直線補間の場合に加減速時間・速度上限パラメータ決定手段７０が算出する速度上限パラメータは補正せずそのまま指令曲線生成手段に出力する。
ａｒａｔｅｋi＝ｔｒａｔｅｋi *１００／ｈｏｓｅi （３５）
算出された補正候補値ａｒａｔｅｋiの中から最大値ａｒａｔｅｋｍａｘを求める。ここで、ａｒａｔｅｋｍａｘ≦１の場合は加速時間を補正せず、ａｒａｔｅｋｍａｘ＞１の場合は加速時間ｋｔをｋｔ*ａｒａｔｅｋｍａｘに書き換える。
同様に、減速区間の各軸のトルク使用率と補正率の設定値から補正候補値ａｒａｔｅｇiを次式（３６）で算出する。
ａｒａｔｅｇi＝ｔｒａｔｅｇi *１００／ｈｏｓｅi （３６）
算出された補正候補値ａｒａｔｅｇiの中から最大値ａｒａｔｅｇｍａｘを求める。ａｒａｔｅｇｍａｘ≦１の場合、減速時間は補正しない。ａｒａｔｅｇｍａｘ＞１の場合は減速時間ｇｔをｇｔ*ａｒａｔｅｇｍａｘに書き換える。なお、直線補間の場合に加減速時間・速度上限パラメータ決定手段７０が算出する速度上限パラメータは補正せずそのまま指令曲線生成手段に出力する。
以上のように、この実施の形態４によれば、加減速時間・速度上限パラメータ決定手段７０で、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、関節補間の場合には、動作毎にロボットパラメータ記憶手段６に記憶されている各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出し、また、直線補間の場合には、モータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間、減速時間および最大の速度上限パラメータを算出し、トルク使用率演算手段１０で、加減速時間・速度上限パラメータ決定手段７０により算出された最短の加速時間および最短の減速時間に基づいて各軸の加速区間のトルク使用率および各軸の減速区間のトルク使用率を算出し、算出された各軸の加速区間および減速区間の各トルク使用率および補正率記憶手段１１に記憶された各軸毎の補正率に基づいて、動作パラメータ補正手段１２により、加減速時間・速度上限パラメータ決定手段７０で算出された加速時間および減速時間を補正するようにしたので、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。なお、ここで動作パラメータの修正については加速時間および減速時間に限定される必要はない。また、加減速時間・速度上限パラメータの決定については、速度上限パラメータの算出も行う場合に限定される必要はない。速度上限パラメータの算出を行わない場合は、加減速時間・速度上限パラメータ決定手段７０は、入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速決定手段に置き換えればよい。
また、具体的処理としてこの実施の形態４では、トルク使用率演算手段１０は、加減速時間・速度上限パラメータ決定手段７０で算出した各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔmi、減速時間ｇｔmiおよび最終的に求めた加速時間ｋｔ、減速時間ｇｔに基づいて各軸の加速区間および減速区間のトルク使用率を算出し、動作パラメータ補正手段１２により、トルク使用率演算手段１０で求めた各軸の加速区間および減速区間の各トルク使用率と補正率記憶手段１１に記憶されている各軸の補正率の設定値に基づいてそれぞれの区間の補正候補値を算出し、算出された補正候補値の中から最大値をそれぞれ求め、求めた最大値が１より大きい場合のみ、最終的に求めた加速時間ｋｔおよび減速時間ｇｔをその１より大きい補正候補値の最大値に基づいてそれぞれｋｔ*ａｒａｔｅｋｍａｘおよびｇｔ*ａｒａｔｅｇｍａｘに置き換えるようにしたことを特徴としている。
実施の形態５．
実施の形態５によるロボット制御装置の構成も図４で表される。実施の形態４とは動作パラメータ補正手段１２と指令曲線生成手段３０の動作が異なるので、その異なる部分についてのみ説明する。
まず、動作パラメータ補正手段１２では、加速区間のトルク使用率と補正率の設定値から補正候補値ｖｒａｔｅｋiを次式（３７）で算出する。
ｖｒａｔｅｋi＝ｔｒａｔｅｋi *１００／ｈｏｓｅi （３７）
算出された補正候補値ｖｒａｔｅｋiの中から最大値ｖｒａｔｅｋｍａｘを求める。
同様に減速区間のトルク使用率と補正率の設定値から補正候補値ｖｒａｔｅｇiを次式（３８）で算出する。
ｖｒａｔｅｇi＝ｔｒａｔｅｇi *１００／ｈｏｓｅi （３８）
算出された補正候補値ｖｒａｔｅｇiの中から最大値ｖｒａｔｅｇｍａｘを求める。
実施の形態５によるロボット制御装置の構成も図４で表される。実施の形態４とは動作パラメータ補正手段１２と指令曲線生成手段３０の動作が異なるので、その異なる部分についてのみ説明する。
まず、動作パラメータ補正手段１２では、加速区間のトルク使用率と補正率の設定値から補正候補値ｖｒａｔｅｋiを次式（３７）で算出する。
ｖｒａｔｅｋi＝ｔｒａｔｅｋi *１００／ｈｏｓｅi （３７）
算出された補正候補値ｖｒａｔｅｋiの中から最大値ｖｒａｔｅｋｍａｘを求める。
同様に減速区間のトルク使用率と補正率の設定値から補正候補値ｖｒａｔｅｇiを次式（３８）で算出する。
ｖｒａｔｅｇi＝ｔｒａｔｅｇi *１００／ｈｏｓｅi （３８）
算出された補正候補値ｖｒａｔｅｇiの中から最大値ｖｒａｔｅｇｍａｘを求める。
次に、２つの補正候補値の最大値ｖｒａｔｅｋｍａｘとｖｒａｔｅｇｍａｘの中で大きい方を最終補正候補値ｖｒａｔｅｍａｘとする。ｖｒａｔｅｍａｘ＞１の時に、基準速度修正率ｏｖｒｄを次式（３９）で算出する。
ｏｖｒｄ＝１／ｖｒａｔｅｍａｘ （３９）
また、ｖｒａｔｅｍａｘ≦１のときはｏｖｒｄ＝１とする。指令曲線生成手段３０へは、基準速度修正率ｏｖｒｄと加減速時間・速度上限パラメータ決定手段７０で算出した加速時間、減速時間および速度上限パラメータを出力する。
指令曲線生成手段３０では、基準速度演算手段２０で演算した基準速度に動作パラメータ補正手段１２で算出した基準速度修正率ｏｖｒｄを乗じた値を基準速度として指令曲線を生成する。
ｏｖｒｄ＝１／ｖｒａｔｅｍａｘ （３９）
また、ｖｒａｔｅｍａｘ≦１のときはｏｖｒｄ＝１とする。指令曲線生成手段３０へは、基準速度修正率ｏｖｒｄと加減速時間・速度上限パラメータ決定手段７０で算出した加速時間、減速時間および速度上限パラメータを出力する。
指令曲線生成手段３０では、基準速度演算手段２０で演算した基準速度に動作パラメータ補正手段１２で算出した基準速度修正率ｏｖｒｄを乗じた値を基準速度として指令曲線を生成する。
以上のように、この実施の形態５によれば、トルク使用率演算手段１０では、加減速時間・速度上限パラメータ決定手段７０で算出された各軸のモータの許容トルクの制約を満たす範囲で最短となる加速時間ｋｔmi、減速時間ｇｔmiおよび最終的に求めた加速時間ｋｔ、減速時間ｇｔに基づいて各軸の加速区間および減速区間のトルク使用率をそれぞれ算出し、動作パラメータ補正手段１２により、算出された各軸の加速区間および減速区間のトルク使用率と補正率の設定値に基づいて補正候補値をそれぞれ算出し、算出された補正候補値の中からそれぞれ各区間での補正候補値の最大値を抽出し、抽出された各区間での最大値の中で、大きい方の値に基づいて基準速度修正率を算出し、指令曲線生成手段では、基準速度演算手段２０で算出した基準速度に算出された基準速度修正率を乗じて修正した基準速度とし、指令曲線を生成するようにしたことを特徴としている。したがって、実施の形態４と同様にして、予め指定した割合と各軸のモータの負荷に応じて自動的に動作パラメータを修正できる効果が得られる。
実施の形態６．
実施の形態６によるロボット制御装置の構成も図４で表される。実施の形態４とは動作パラメータ補正手段１２および指令曲線生成手段３０の動作が異なるので、その異なる部分についてのみ説明する。
まず、動作パラメータ補正手段１２では、加速区間のトルク使用率と補正率の設定値から補正候補値ｄｒａｔｅｋiを次式（４０）から算出する。
ｄｒａｔｅｋi＝ｔｒａｔｅｋi *１００／ｈｏｓｅi （４０）
算出された補正候補値ｄｒａｔｅｋiの最大値ｄｒａｔｅｋｍａｘを求める。同様に減速区間のトルク使用率と補正率の設定値から次式（４１）から補正候補値ｄｒａｔｅｇiを算出する。
ｄｒａｔｅｇi＝ｔｒａｔｅｇi *１００／ｈｏｓｅi （４１）
算出された補正候補値ｄｒａｔｅｇiの最大値ｄｒａｔｅｇｍａｘを求める。
次に、ｄｒａｔｅｋｍａｘ、ｄｒａｔｅｇｍａｘの中で大きい方をｄｒａｔｅｍａｘとする。ｄｒａｔｅｍａｘ＞１の時に、待ち時間修正率ｒｄｅｌａｙを
ｒｄｅｌａｙ＝ｄｒａｔｅｍａｘ （４２）
で算出する。また、ｄｒａｔｅｍａｘ≦１のときはｒｄｅｌａｙ＝１とする。
実施の形態６によるロボット制御装置の構成も図４で表される。実施の形態４とは動作パラメータ補正手段１２および指令曲線生成手段３０の動作が異なるので、その異なる部分についてのみ説明する。
まず、動作パラメータ補正手段１２では、加速区間のトルク使用率と補正率の設定値から補正候補値ｄｒａｔｅｋ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 （４６）
算出された各軸の加速区間のトルク使用率および減速区間のトルク使用率は、動作パラメータ補正手段１２に出力され、動作パラメータ補正手段１２において実施の形態４のように加速時間および減速時間の修正に用いられる。
実施の形態７によるロボット制御装置の構成も図４で表わされる。実施の形態４とはトルク使用率演算手段１０での動作が異なるので、その異なる部分についてのみ説明する。
トルク使用率演算手段１０は、加減速時間・速度上限パラメータ決定手段７０内部で計算した加速終了地点における慣性行列Ｍ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)
- ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、前記ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、前記直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に前記各軸を駆動するモータの駆動トルクの許容最大値、前記各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速時間決定手段と、
算出された前記最短の加速時間に基づいて各軸の加速区間のトルク使用率を算出し、前記最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出するトルク使用率演算手段と、
ロボットの各軸の補正率を予め記憶する補正率記憶手段と、
各軸のトルク使用率を各軸の補正率の設定値で除算して得られる各軸の補正候補値を算出し、算出した補正候補値の最大値が１より大きくなる場合に、加速時間及び減速時間を補正候補値の最大値を乗じた値に修正する動作パラメータ修正手段と、
算出された基準速度、加速時間、減速時間と修正された加速時間、減速時間に基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたロボット制御装置。 - ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、前記ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、前記直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に前記各軸を駆動するモータの駆動トルクの許容最大値、前記各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速時間決定手段と、
算出された前記最短の加速時間に基づいて各軸の加速区間のトルク使用率を算出し、前記最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出するトルク使用率演算手段と、
ロボットの各軸の補正率を予め記憶する補正率記憶手段と、
各軸のトルク使用率を各軸の補正率の設定値で除算して得られる各軸の補正候補値を算出し、算出した補正候補値の最大値が１より大きくなる場合に、基準速度に乗じる基準速度修正率を補正候補値の最大値で除算した値に修正する動作パラメータ修正手段と、
算出された基準速度、加速時間、減速時間と修正された基準速度修正率に基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたロボット制御装置。 - ロボットの各軸の許容最高速度、直交座標系における基準最高速度、各軸を駆動するモータの駆動トルクの許容最大値、各軸の伝達機構の作用トルクおよびモーメントの許容最大値を予め記憶しているロボットパラメータ記憶手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位に応答して、加速開始地点から減速終了地点への移動が関節補間の場合には、前記ロボットの各軸の許容最高速度に基づいて現在動作の各軸の動作開始から加速時間経過したときに達する基準速度を算出し、また、加速開始地点から減速終了地点への移動が直線補間の場合には、前記直交座標系における基準最高速度に基づいて現在動作の直交座標系の各方向の動作開始から加速時間経過したときに達する基準速度を算出する基準速度演算手段と、
入力される加速開始地点と減速終了地点の直交座標値と関節変位の少なくとも一方に応答して、動作毎に前記各軸を駆動するモータの駆動トルクの許容最大値、前記各軸の伝達機構の作用トルクおよびモーメントの許容最大値の少なくとも１つの制約を満たす範囲で最短の加速時間および減速時間を算出する加減速時間決定手段と、
算出された前記最短の加速時間に基づいて各軸の加速区間のトルク使用率を算出し、前記最短となる減速時間に基づいて各軸の減速区間のトルク使用率を算出するトルク使用率演算手段と、
ロボットの各軸の補正率を予め記憶する補正率記憶手段と、
各軸のトルク使用率を各軸の補正率の設定値で除算して得られる各軸の補正候補値を算出し、算出した補正候補値の最大値が１より大きくなる場合に、待ち時間修正率を補正候補値の最大値に修正する動作パラメータ修正手段と、
算出された基準速度、加速時間、減速時間と修正された待ち時間修正率に基づいてロボットの各軸を駆動するモータを制御する指令曲線を生成する指令曲線生成手段とを備えたロボット制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2008029595A JP2008132595A (ja) | 2008-02-08 | 2008-02-08 | ロボット制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2008029595A JP2008132595A (ja) | 2008-02-08 | 2008-02-08 | ロボット制御装置 |
Related Parent Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2003279304A Division JP2005044230A (ja) | 2003-07-24 | 2003-07-24 | ロボット制御装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2008132595A true JP2008132595A (ja) | 2008-06-12 |
Family
ID=39557793
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2008029595A Pending JP2008132595A (ja) | 2008-02-08 | 2008-02-08 | ロボット制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2008132595A (ja) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2018120543A (ja) * | 2017-01-27 | 2018-08-02 | ファナック株式会社 | 数値制御装置 |
| CN111051010A (zh) * | 2017-09-04 | 2020-04-21 | 松下知识产权经营株式会社 | 机器人控制装置 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS62251906A (ja) * | 1986-04-25 | 1987-11-02 | Mitsubishi Electric Corp | 速度自動発生機能付き制御装置 |
| JPH05138563A (ja) * | 1991-11-13 | 1993-06-01 | Mitsubishi Electric Corp | ロボツトの制御装置 |
| JPH07200033A (ja) * | 1993-12-28 | 1995-08-04 | Mitsubishi Electric Corp | ロボット制御装置 |
| JPH09204216A (ja) * | 1996-01-25 | 1997-08-05 | Nachi Fujikoshi Corp | 多関節ロボットの加減速制御方法 |
| JPH09265313A (ja) * | 1996-01-24 | 1997-10-07 | Mitsubishi Electric Corp | ロボットの速度演算装置、およびロボットの速度演算方法 |
| JPH1039916A (ja) * | 1996-07-26 | 1998-02-13 | Nachi Fujikoshi Corp | 多関節ロボットの加減速制御方法 |
| JPH11184512A (ja) * | 1997-12-19 | 1999-07-09 | Yaskawa Electric Corp | ロボットの制御ゲイン決定方法 |
| JPH11198072A (ja) * | 1998-01-14 | 1999-07-27 | Yaskawa Electric Corp | ロボットの最短時間速度制御装置 |
| JPH11259108A (ja) * | 1998-03-13 | 1999-09-24 | Mitsubishi Electric Corp | ロボット制御装置 |
| JP2001306118A (ja) * | 2000-04-21 | 2001-11-02 | Mitsubishi Electric Corp | メカニカルシステムの制御装置及び制御方法 |
-
2008
- 2008-02-08 JP JP2008029595A patent/JP2008132595A/ja active Pending
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS62251906A (ja) * | 1986-04-25 | 1987-11-02 | Mitsubishi Electric Corp | 速度自動発生機能付き制御装置 |
| JPH05138563A (ja) * | 1991-11-13 | 1993-06-01 | Mitsubishi Electric Corp | ロボツトの制御装置 |
| JPH07200033A (ja) * | 1993-12-28 | 1995-08-04 | Mitsubishi Electric Corp | ロボット制御装置 |
| JPH09265313A (ja) * | 1996-01-24 | 1997-10-07 | Mitsubishi Electric Corp | ロボットの速度演算装置、およびロボットの速度演算方法 |
| JPH09204216A (ja) * | 1996-01-25 | 1997-08-05 | Nachi Fujikoshi Corp | 多関節ロボットの加減速制御方法 |
| JPH1039916A (ja) * | 1996-07-26 | 1998-02-13 | Nachi Fujikoshi Corp | 多関節ロボットの加減速制御方法 |
| JPH11184512A (ja) * | 1997-12-19 | 1999-07-09 | Yaskawa Electric Corp | ロボットの制御ゲイン決定方法 |
| JPH11198072A (ja) * | 1998-01-14 | 1999-07-27 | Yaskawa Electric Corp | ロボットの最短時間速度制御装置 |
| JPH11259108A (ja) * | 1998-03-13 | 1999-09-24 | Mitsubishi Electric Corp | ロボット制御装置 |
| JP2001306118A (ja) * | 2000-04-21 | 2001-11-02 | Mitsubishi Electric Corp | メカニカルシステムの制御装置及び制御方法 |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2018120543A (ja) * | 2017-01-27 | 2018-08-02 | ファナック株式会社 | 数値制御装置 |
| CN111051010A (zh) * | 2017-09-04 | 2020-04-21 | 松下知识产权经营株式会社 | 机器人控制装置 |
| CN111051010B (zh) * | 2017-09-04 | 2023-03-14 | 松下知识产权经营株式会社 | 机器人控制装置 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4697139B2 (ja) | サーボ制御装置 | |
| JP5896789B2 (ja) | ロボット制御装置、ロボット装置、ロボット制御方法、プログラム及び記録媒体 | |
| JP2005056171A (ja) | 制御装置 | |
| Duchaine et al. | Computationally efficient predictive robot control | |
| JP5096019B2 (ja) | サーボモータ制御装置 | |
| JPWO2017047009A1 (ja) | ロボットの衝突検出方法 | |
| JP2005044230A (ja) | ロボット制御装置 | |
| JP2019013984A (ja) | 速度一定が要求されるアプリケーションにおいて学習制御を行うロボット及びその制御方法 | |
| JP4453526B2 (ja) | サーボ制御装置 | |
| JP2015051469A (ja) | ロボット制御装置、ロボット装置、ロボット制御方法、プログラム及び記録媒体 | |
| CN107544247B (zh) | 一种抑制机械运动结构振动的方法及系统 | |
| JP2016055404A (ja) | 軌道生成方法、軌道生成装置、ロボット装置、プログラム及び記録媒体 | |
| JP5268974B2 (ja) | 数値制御装置および生産システム | |
| JP2008132595A (ja) | ロボット制御装置 | |
| WO2019117201A1 (ja) | 制御システム | |
| JP2017213631A (ja) | ロボットアーム制御装置、ロボットアーム制御方法、およびプログラム | |
| WO2002025390A1 (fr) | Procede de servocommande | |
| WO1997001801A1 (fr) | Procede de regulation des operations d'acceleration et de deceleration d'un robot | |
| JP4183057B2 (ja) | 数値制御システム | |
| CN101785717B (zh) | 肘关节驱动安装结构及其优化设计方法 | |
| JP3805476B2 (ja) | モータ制御装置 | |
| JP6046994B2 (ja) | 制御装置 | |
| JP2015058520A (ja) | ロボット制御装置およびロボット制御方法 | |
| JP4762219B2 (ja) | メカニカルシステムの制御装置 | |
| JP2010269385A (ja) | Ｓ字加減速軌道生成方法および多関節型ロボットシステム |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| RD04 | Notification of resignation of power of attorney |
Effective date: 20080604 Free format text: JAPANESE INTERMEDIATE CODE: A7424 |
|
| A977 | Report on retrieval |
Effective date: 20100617 Free format text: JAPANESE INTERMEDIATE CODE: A971007 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20100622 |
|
| A521 | Written amendment |
Effective date: 20100820 Free format text: JAPANESE INTERMEDIATE CODE: A523 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20110308 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20110705 |