(57)【特許請求の範囲】
【請求項１】
メカニカルシステムの各軸のトルクを各軸の位置、速度、加速度を用いて算出し、時間−速度座標上の低速域の代表点でメカニカルシステムの動特性を表現した第１の運動方程式に基づいて、各軸のモータ及び伝達機構によって構成される駆動機構から定まる許容トルクを満たす範囲で最小の加速時間及び減速時間を演算する低速域加減速時間演算手段と、
メカニカルシステムの各軸のトルクを各軸の位置、速度、加速度を用いて算出し、前記時間−速度座標上の高速域の代表点でメカニカルシステムの動特性を表現した第２の運動方程式に基づいて、各軸のモータ及び伝達機構によって構成される駆動機構から定まる許容トルクを満たす範囲で最小の加速時間及び減速時間を演算する高速域加減速時間演算手段と、
前記低速域加減速時間演算手段により演算された低速域の加速時間及び減速時間、並びに前記高速域加減速時間演算手段により演算された高速域の加速時間及び減速時間に基づいて、低速域及び高速域で異なる加減速度の速度指令パターンを生成する指令生成手段と
を備えたことを特徴とするメカニカルシステムの制御装置。
【請求項２】
前記高速域加減速時間演算手段は、前記第２の運動方程式の高速域開始点及び高速域終了点の演算の少なくとも１回を、前記低速域加減速時間演算手段による、前記第１の運動方程式の演算結果を用いて行う
ことを特徴とする請求項１記載のメカニカルシステムの制御装置。
【請求項３】
前記駆動機構の速度−出力特性及び当該動作において前記駆動機構の出しうる最高速度に基づいて、低速域と高速域で異なる加減速度の速度指令パターンを生成する際の低速域と高速域の閾値を演算する閾値演算手段
をさらに備えたことを特徴とする請求項１又は２記載のメカニカルシステムの制御装置。
【請求項４】
前記第１の運動方程式の演算結果に基づいて算出される加減速時間を用いた場合の各軸トルクを算出し、算出した各軸トルクに基づいて速度−トルク特性の折れ線を補正し、補正した折れ線に基づいて、前記閾値を修正する閾値修正手段
をさらに備えたことを特徴とする請求項３記載のメカニカルシステムの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、各軸のモータ及び減速機などの伝達機構の制約を満たす範囲で最短の動作を行うための加減速パラメータを、動作毎に決定する機能を備えたメカニカルシステムの制御装置に関するものである。
【背景技術】
【０００２】
従来のメカニカルシステムの制御装置について説明する（例えば、特許文献１参照）。まず、メカニカルシステムの各軸が同時に動作を開始し、かつ同時に動作を終了する場合の各軸の出しうる最高速度を演算する。次に、各軸の速度を台形状に加減速を行う速度指令パターンで動作させる場合の加速時間、減速時間の初期値を設定する。当該動作の開始地点、終了地点、算出した加速時間、減速時間、最高速度から加速開始地点、加速終了地点、減速開始地点、減速終了地点の各軸の位置、速度を算出し、算出した位置、速度を用いて、加速開始地点でモータ及び伝達機構の許容トルク最大値などの制約を満たす範囲で最短となる加速時間ｔ１ａと、加速終了地点でモータ及び伝達機構の許容トルク最大値などの制約を満たす範囲で最短となる加速時間ｔ１ｂを算出し、大きいほうを加速時間ｔ１とする。同様に、算出した位置、速度を用いて、減速開始地点でモータ及び伝達機構の許容トルク最大値などの制約を満たす範囲で最短となる減速時間ｔ２ａと、減速終了地点でモータ及び伝達機構の許容トルク最大値などの制約を満たす範囲で最短となる減速時間ｔ２ｂを算出し、大きいほうを減速時間ｔ２とする。算出した加速時間ｔ１、減速時間ｔ２と開始地点、終了地点、最高速度から加速開始地点、加速終了地点、減速開始地点、減速終了地点の各軸の位置、速度を算出し直し、再度、加速時間、減速時間を算出する。上記繰り返しを規定回数実行する。
【０００３】
【特許文献１】特開平０７−２０００３３号公報（実施例３、４、図４）
【発明の開示】
【発明が解決しようとする課題】
【０００４】
上述したような従来のメカニカルシステムの制御装置では、高速域でモータの許容トルクが下がる場合、高速域に合わせて低速域でも加速度を下げる必要があるため、許容トルクを満たす範囲で十分な高速化が図れないという問題点があった。また、最高速度を上げると加速度が下がるために、かえって動作時間が長くなる場合があるという問題点があった。
【０００５】
この発明は、上述のような課題を解決するためになされたもので、その目的は、低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮でき、また、最高速度を高くしたため、かえって動作時間が伸びることを防止でき、さらに、各動作に応じた低速域、高速域の閾値が設定できるメカニカルシステムの制御装置を得るものである。
【課題を解決するための手段】
【０００６】
この発明に係るメカニカルシステムの制御装置は、メカニカルシステムの各軸のトルクを各軸の位置、速度、加速度を用いて算出し、時間−速度座標上の低速域の代表点でメカニカルシステムの動特性を表現した第１の運動方程式に基づいて、各軸のモータ及び伝達機構によって構成される駆動機構から定まる許容トルクを満たす範囲で最小の加速時間及び減速時間を演算する低速域加減速時間演算手段と、メカニカルシステムの各軸のトルクを各軸の位置、速度、加速度を用いて算出し、前記時間−速度座標上の高速域の代表点でメカニカルシステムの動特性を表現した第２の運動方程式に基づいて、各軸のモータ及び伝達機構によって構成される駆動機構から定まる許容トルクを満たす範囲で最小の加速時間及び減速時間を演算する高速域加減速時間演算手段と、前記低速域加減速時間演算手段により演算された低速域の加速時間及び減速時間、並びに前記高速域加減速時間演算手段により演算された高速域の加速時間及び減速時間に基づいて、低速域及び高速域で異なる加減速度の速度指令パターンを生成する指令生成手段とを設けたものである。
【発明の効果】
【０００７】
この発明に係るメカニカルシステムの制御装置は、低速域、高速域のそれぞれの区間でメカニカルシステムの動特性と駆動機構の出力制限値の双方を考慮して最小の加速時間、減速時間を算出するため、低速域と高速域で駆動機構の出力の制限値が異なり、かつ、各軸の変位により出しうる加減速度が異なるメカニカルシステムにおいて、動作毎に低速域と高速域のそれぞれの出力の制限を満たしながら短時間で動作できるという効果を奏する。
【発明を実施するための最良の形態】
【０００８】
実施の形態１．
この発明の実施の形態１に係るメカニカルシステムの制御装置について図１から図６までを参照しながら説明する。図１は、この発明の実施の形態１に係るメカニカルシステムの制御装置の構成を示すブロック図である。なお、以降では、各図中、同一符号は同一又は相当部分を示す。
【０００９】
図１において、この発明の実施の形態１に係るメカニカルシステムの制御装置は、最高速度を演算する最高速度演算手段１と、低速域と高速域の境界を定める閾値を演算する閾値演算手段２と、低速域における加速時間及び減速時間を演算する低速域加減速時間演算手段３と、閾値を修正する閾値修正手段４と、高速域における加速時間及び減速時間を演算する高速域加減速時間演算手段５と、速度指令パターンを生成する指令生成手段６と、ロボット等のメカニカルシステム８の各軸を駆動するモータを制御するモータ制御手段７とが設けられている。なお、この発明に係る駆動機構は、各軸のモータ及び伝達機構から構成されている。
【００１０】
つぎに、この実施の形態１に係るメカニカルシステムの制御装置の動作について図面を参照しながら説明する。図２は、この発明の実施の形態１に係るメカニカルシステムの制御装置の動作を示すフローチャートである。
【００１１】
まず、最高速度演算手段１は、動作プログラムによる動作命令毎に、当該動作の動作開始点と動作終了点の情報を読み取り、当該動作でのメカニカルシステム８の各軸の出しうる最高速度（加減速時間を０とした場合の各軸の速度）を算出する。
【００１２】
メカニカルシステム８の軸数がｎ（自然数）で、各動作で全ての軸が同時に動作を開始し、同時に動作を終了する場合、第ｉ軸の許容最高速度をｖｍａｘｉ、第ｉ軸の動作開始点の値をｓｐｉ、第ｉ軸の動作終了点の値をｅｐｉとすれば、次の式（１）、（２）により、各軸の出しうる最高速度ｖｉが算出できる。ここで、ａｂｓ（）は絶対値、ｍａｘ（）は最大値を意味している。
【００１３】
ｍａｘｄｌ＝ｍａｘ（ａｂｓ（ｅｐ１−ｓｐ１）／ｖｍａｘ１，ａｂｓ（ｅｐ２−ｓｐ２）／ｖｍａｘ２，・・・ａｂｓ（ｅｐｎ−ｓｐｎ）／ｖｍａｘｎ）            （１）
【００１４】
ｖｉ＝ａｂｓ（ｅｐｉ−ｓｐｉ）／ｍａｘｄｌ                              （２）
【００１５】
図３は、この発明の実施の形態１に係るメカニカルシステムの制御装置の制御対象であるモータの速度−トルク特性を示す図である。また、図４は、この発明の実施の形態１に係るメカニカルシステムの制御装置の低速域の速度指令パターンを示す図である。
【００１６】
次に、閾値演算手段２は、低速域と高速域の閾値ｍｉｎｒを以下の手順で算出する（ステップ１０１）。まず、図３に示すように、第ｉ軸のモータの速度−トルク特性において、速度ｖａｉ以上の領域では出せるトルクが下がっているとする。このとき、各軸の中間変数ｒｉを、次の式（３）により求め、次の式（４）により示す、各軸の中間変数ｒｉの最小値を低速域と高速域の閾値ｍｉｎｒとする。ただし、中間変数ｒｉ＞１となる場合には、中間変数ｒｉ＝１とする。
【００１７】
ｒｉ＝ｖａｉ／ｖｉ                                                      （３）
【００１８】
ｍｉｎｒ＝ｍｉｎ（ｒ１，ｒ２，・・・，ｒｎ）                            （４）
【００１９】
このように、動作に応じて閾値ｍｉｎｒを設定することにより、加減速度を高くできる区間（低速域）を動作に応じて延長することが可能になり、動作時間を短縮できる効果がある。
【００２０】
次に、低速域加減速時間演算手段３は、各軸の速度がｍｉｎｒ＊ｖｉ以下の領域を低速域として、低速域での加減速時間を算出する（ステップ１０２〜１０４）。また、高速域加減速時間演算手段５は、各軸の速度がｍｉｎｒ＊ｖｉより大きくなる領域を高速域として、高速域での加減速時間を算出する（ステップ１０６〜１０８）。さらに、指令生成手段６は、各軸の速度がｍｉｎｒ＊ｖｉ以下の領域を低速域として、低速域加減速時間演算手段３で算出された加減速時間に基づいた加減速の速度指令パターンを生成し、各軸の速度がｍｉｎｒ＊ｖｉより大きくなる領域を高速域として、高速域加減速時間演算手段５で算出された加減速時間に基づいた加減速の速度指令パターンを生成する。そして、モータ制御手段７は、低速域及び高速域の速度指令パターンに基づいてモータの加減速の制御を行う。
【００２１】
低速域加減速時間演算手段３は、次の式（５）により示す、各軸の最高速度ｖｌｉとして、図４のような台形状の速度指令パターンで動作させる場合に、各軸のモータ及び伝達機構の許容トルクの制約を満たす範囲で最短の加速時間と減速時間を算出する。
【００２２】
ｖｌｉ＝ｍｉｎｒ＊（ｅｐｉ−ｓｐｉ）／ｍａｘｄｌ                        （５）
【００２３】
ここで、加速時間及び減速時間は、低速域であっても、次の式（６）により示す、高速域も含めた最高速度ｖｍｉまで一定の加速度で加速する場合の加速及び減速に要する時間とする。
【００２４】
ｖｍｉ＝（ｅｐｉ−ｓｐｉ）／ｍａｘｄｌ                                  （６）
【００２５】
まず、加速時間及び減速時間の初期値を設定する。次に、設定した加速時間及び減速時間の初期値と、各軸の低速域での最高速度ｖｌｉと、高速域も含めた最高速度ｖｍｉと、動作開始点と、動作終了点のデータから、図４に示すような、「加速開始点」、「加速終了点」、「減速開始点」、「減速終了点」の４つの代表点での各軸の位置と速度を算出する。算出した加速開始点の位置、速度と各軸のモータ及び伝達機構の許容トルクの制限値から加速開始点での加速時間ｔ１ａをメカニカルシステム８の運動方程式を用いて算出する。同様に、算出した加速終了点の位置、速度と各軸のモータ及び伝達機構の許容トルクの制限値から加速終了点での加速時間ｔ１ｂをメカニカルシステム８の運動方程式を用いて算出する。算出した加速時間ｔ１ａ、加速時間ｔ１ｂを比較し、大きいほうを加速時間ｔ１とする。
【００２６】
「加速開始点」での加速時間ｔ１ａは、以下のように算出する。まず、メカニカルシステム８の運動方程式が、次の式（７）により、表現できるとする。ここで、Ｍは慣性行列、ａは各軸の加速度から構成されるベクトル、ｈは各軸の遠心・コリオリ力、ｇは各軸の重力、ｆは各軸の摩擦力とする。
【００２７】
τ＝Ｍａ＋ｈ＋ｇ＋ｆ                                                    （７）
【００２８】
このとき、Ｍ、ｈ、ｇ、ｆはそれぞれ、各軸の位置で構成されるベクトルｐ、各軸の速度で構成されるベクトルｖの関数であり、Ｍ（ｐ）、ｈ（ｐ，ｖ）、ｇ（ｐ）、ｆ（ｖ）と表記できる。加速開始点での各軸位置から構成されるベクトルをｐ１ａ、加速開始点での各軸速度から構成されるベクトルをｖ１ａ、各軸の低速域の最高速度ｖｌｉから構成されるベクトルをｖｌ、高速域も含めた最高速度ｖｍｉから構成されるベクトルをｖｍとすれば、加速時間がｋｔのときの運動方程式は、次の式（８）となる。
【００２９】
τ＝Ｍ（ｐ１ａ）ｖｍ／ｋｔ＋ｈ（ｐ１ａ，ｖ１ａ）＋ｇ（ｐ１ａ）＋ｆ（ｖ１ａ）
（８）
【００３０】
従って、Ｍ（ｐ１ａ）ｖｍの第ｉ要素をｍｖｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（９）を満たすｋｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の加速時間ｔ１ａｉである。
【００３１】
τｍａｘｉ＝ｍｖｉ／ｋｔｉ＋ｈｉ＋ｇｉ＋ｆｉ      （ｍｖｉ＞０の場合）、
−τｍａｘｉ＝ｍｖｉ／ｋｔｉ＋ｈｉ＋ｇｉ＋ｆｉ      （ｍｖｉ＜０の場合）
（９）
【００３２】
ここで、ｈｉ、ｇｉ、ｆｉはそれぞれｈ（ｐ１ａ，ｖ１ａ）、ｇ（ｐ１ａ）、ｆ（ｖ１ａ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、加速開始点では速度０での許容トルクτｍａｘ１ｉが式（９）のτｍａｘｉとして用いられる。各軸の加速時間ｔ１ａｉの最大値をｔ１ａとする。
【００３３】
「加速終了点」での加速時間ｔ１ｂも同様に、加速終了点の位置ｐ１ｂ、加速終了点の速度ｖ１ｂから加速時間がｋｔのときの運動方程式が、次の式（１０）となる。
【００３４】
τ＝Ｍ（ｐ１ｂ）ｖｍ／ｋｔ＋ｈ（ｐ１ｂ，ｖ１ｂ）＋ｇ（ｐ１ｂ）＋ｆ（ｖ１ｂ）
（１０）
【００３５】
従って、Ｍ（ｐ１ｂ）ｖｍの第ｉ要素をｍｖｂｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（１１）を満たすｋｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の加速時間ｔ１ｂｉである。各軸の加速時間ｔ１ｂｉの最大値をｔ１ｂとする。
【００３６】
τｍａｘｉ＝ｍｖｂｉ／ｋｔｉ＋ｈｂｉ＋ｇｂｉ＋ｆｂｉ      （ｍｖｂｉ＞０の場合）、
−τｍａｘｉ＝ｍｖｂｉ／ｋｔｉ＋ｈｂｉ＋ｇｂｉ＋ｆｂｉ      （ｍｖｂｉ＜０の場合）                                                                      （１１）
【００３７】
ここで、ｈｂｉ、ｇｂｉ、ｆｂｉはそれぞれｈ（ｐ１ｂ，ｖ１ｂ）、ｇ（ｐ１ｂ）、ｆ（ｖ１ｂ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、加速終了点では速度ｖｌｉでの許容トルクτｍａｘｂｉが式（１１）のτｍａｘｉとして用いられる。式（５）で用いるｍｉｎｒが初期値の場合には、τｍａｘｂｉは速度０での許容トルクτｍａｘ１ｉと同じになる。加速時間ｔ１ａ、ｔ１ｂの大きいほうを今回の繰り返し周期での低速域での加速時間ｔ１とする。
【００３８】
「減速開始点」での減速時間ｔ２ａは、下記の手順で算出する。まず、減速開始点での位置ｐ２ａ、減速開始点の速度ｖ２ａから減速時間がｇｔのときの運動方程式が、次の式（１２）となる。
【００３９】
τ＝−Ｍ（ｐ２ａ）ｖｍ／ｇｔ＋ｈ（ｐ２ａ，ｖ２ａ）＋ｇ（ｐ２ａ）＋ｆ（ｖ２ａ）
（１２）
【００４０】
従って、Ｍ（ｐ２ａ）ｖｍの第ｉ要素をｍｖ２ａｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（１３）を満たすｇｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の減速時間ｔ２ａｉである。各軸の減速時間ｔ２ａｉの最大値をｔ２ａとする。
【００４１】
τｍａｘｉ＝−ｍｖ２ａｉ／ｇｔｉ＋ｈ２ａｉ＋ｇ２ａｉ＋ｆ２ａｉ      （ｍｖ２ａｉ＜０の場合）、
−τｍａｘｉ＝−ｍｖ２ａｉ／ｇｔｉ＋ｈ２ａｉ＋ｇ２ａｉ＋ｆ２ａｉ      （ｍｖ２ａｉ＞０の場合）                                                          （１３）
【００４２】
ここで、ｈ２ａｉ、ｇ２ａｉ、ｆ２ａｉはそれぞれｈ（ｐ２ａ，ｖ２ａ）、ｇ（ｐ２ａ）、ｆ（ｖ２ａ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、減速開始点では速度ｖｌｉでの許容トルクτｍａｘ２ａｉが式（１３）のτｍａｘｉとして用いられる。式（５）で用いるｍｉｎｒが初期値の場合には、τｍａｘ２ａｉは速度０での許容トルクτｍａｘ１ｉと同じになる。
【００４３】
「減速終了点」での減速時間ｔ２ｂは、下記の手順で算出する。まず、減速終了点での位置ｐ２ｂ、減速開始点の速度ｖ２ｂから減速時間がｇｔのときの運動方程式が、次の式（１４）となる。
【００４４】
τ＝−Ｍ（ｐ２ｂ）ｖｍ／ｇｔ＋ｈ（ｐ２ｂ，ｖ２ｂ）＋ｇ（ｐ２ｂ）＋ｆ（ｖ２ｂ）
（１４）
【００４５】
従って、Ｍ（ｐ２ｂ）ｖｍの第ｉ要素をｍｖ２ｂｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（１５）を満たすｇｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の減速時間ｔ２ｂｉである。各軸の減速時間ｔ２ｂｉの最大値をｔ２ｂとする。
【００４６】
τｍａｘｉ＝−ｍｖ２ｂｉ／ｇｔｉ＋ｈ２ｂｉ＋ｇ２ｂｉ＋ｆ２ｂｉ      （ｍｖ２ｂｉ＜０の場合）、
−τｍａｘｉ＝−ｍｖ２ｂｉ／ｇｔｉ＋ｈ２ｂｉ＋ｇ２ｂｉ＋ｆ２ｂｉ      （ｍｖ２ｂｉ＞０の場合）                                                          （１５）
【００４７】
ここで、ｈ２ｂｉ，ｇ２ｂｉ，ｆ２ｂｉはそれぞれｈ（ｐ２ｂ，ｖ２ｂ）、ｇ（ｐ２ｂ）、ｆ（ｖ２ｂ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、減速終了点では速度０での許容トルクτｍａｘｌｉが式（１５）式のτｍａｘｉとして用いられる。減速時間ｔ２ａ、ｔ２ｂの大きいほうを今回の繰り返し周期の低速域での減速時間ｔ２とする。
【００４８】
次に、閾値修正手段４は、次回の繰り返しでの閾値ｍｉｎｒを下記の手順で算出する（ステップ１０５）。まず、各軸毎に加速終了点での使用トルクτｋｉ、減速開始点での使用トルクτｇｉをそれぞれ、次の式（１６）、（１７）により算出し、使用トルクτｋｉ、τｇｉの大きいほうを第ｉ軸の使用トルク使用率τｓｉとする。
【００４９】
τｋｉ＝ａｂｓ（ｍｖｂｉ／ｔ１＋ｈｂｉ＋ｇｂｉ＋ｆｂｉ）              （１６）
τｇｉ＝ａｂｓ（−ｍｖ２ａｉ／ｔ２＋ｈ２ａｉ＋ｇ２ａｉ＋ｆ２ａｉ）    （１７）
【００５０】
図５は、この発明の実施の形態１に係るメカニカルシステムの制御装置において閾値の修正値の算出方法を説明するための図である。また、図６は、この発明の実施の形態１に係るメカニカルシステムの制御装置の低速域及び高速域の速度指令パターンを示す図である。
【００５１】
次に、図５のように、各軸毎に、速度−トルク特性（折れ線）と直線τ＝τｓｉの交点の速度ｖ２ｉを算出する。ここで、使用トルク使用率τｓｉが許容トルクτｍａｘｌｉと一致する場合には、速度ｖ２ｉを速度−トルク特性の折れ点ｖａｉとする。さらに、算出した速度ｖ２ｉが最高速度ｖｉより大きくなっている場合には、速度ｖ２ｉを最高速度ｖｉとする。算出した各軸の速度ｖ２ｉから、次の式（１８）を算出し、中間変数ｒ２ｉの最小値を閾値ｍｉｎｒの修正値とし、次回の繰り返しでの低速域と高速域の閾値とする。
【００５２】
ｒ２ｉ＝ｖ２ｉ／ｖｉ                                                  （１８）
【００５３】
このようにして、動作に応じて低速域の範囲、つまり閾値を修正することにより、加減速度を高くできる区間を動作に応じて延長することが可能になり、動作時間を短縮できる効果がある。
【００５４】
２回目以降の繰り返しでは、修正した閾値ｍｉｎｒと、前回の周期の加速時間、減速時間を用いて、低速域での加速終了点、減速開始点を算出し直し、再度、式（１１）、式（１３）の演算を行い、加速終了点、減速開始点での加速時間ｔ１ｂ、減速時間ｔ２ａを求め直す。加速時間ｔ１ａ、ｔ１ｂの大きいほうを今回の繰り返し周期での低速域での加速時間ｔ１とする。また、減速時間ｔ２ａ、ｔ２ｂの大きいほうを今回の繰り返し周期での低速域での減速時間ｔ２とする。
【００５５】
規定回数をｎとするとき、閾値修正手段４による閾値ｍｉｎｒの修正をｎ−１回繰り返し、低速域加減速時間演算手段３での加速時間ｔ１、減速時間ｔ２の算出をｎ回繰り返すと、低速域での閾値ｍｉｎｒ、加速時間ｔ１、及び減速時間ｔ２の算出を終了し、高速域加減速時間演算手段５へ移行する。
【００５６】
また、ｎ回目の繰り返しで算出した加速時間ｔ１、減速時間ｔ２と、ｎ−１回目の繰り返しで算出した閾値ｍｉｎｒを、低速域における加速時間ｔ１、減速時間ｔ２及び閾値ｍｉｎｒとして高速域加減速時間演算手段５及び指令生成手段６へ出力する。
【００５７】
次に、高速域加減速時間演算手段５は、図６に示す「高速域開始点」、「高速域加速終了点」、「高速域減速開始点」、「高速域終了点」の４つの代表点でメカニカルシステム８の運動方程式の計算を行い、高速域の加速時間ｔ３ａ、ｔ３ｂと高速域の減速時間ｔ４ａ、ｔ４ｂを算出する。
【００５８】
まず、高速域の加速時間、減速時間の初期値をそれぞれ低速域の加速時間ｔ１、低速域の減速時間ｔ２とし、高速域加速終了点での各軸の位置ｐ３ｂ、各軸の速度ｖ３ｂと、高速域減速開始点での各軸の位置ｐ４ａ、各軸の速度ｖ４ａを算出する。
【００５９】
さらに、高速域開始点の位置、速度は、低速域加減速時間演算手段３による最後の繰り返しにおける加速終了点での位置ｐ１ｂ、速度ｖ１ｂと同一とする。また、高速域終了点での位置、速度は、低速域加減速時間演算手段３による最後の繰り返しにおける減速開始点での位置ｐ２ａ、速度ｖ２ａと同一とする。
【００６０】
「高速域開始点」の運動方程式は、式（１０）となるため、式（１１）を満たすｋｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の加速時間ｔ３ａｉである。ただし、低速域における計算と異なるのは、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、許容トルクτｍａｘｉとして高速域加速終了点での速度ｖ３ｂの第ｉ軸要素であるｖ３ｂｉでの許容トルクτｍａｘ３ｂｉを用いて算出する。算出した加速時間ｔ３ａｉの最大値を加速時間ｔ３ａとする。
【００６１】
「高速域終了点」の運動方程式は、式（１２）となるため、式（１３）を満たすｇｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の減速時間ｔ４ｂｉである。ただし、低速域における計算と異なるのは、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、許容トルクτｍａｘｉとして高速域減速開始点での速度ｖ４ａの第ｉ軸要素であるｖ４ａｉでの許容トルクτｍａｘ４ａｉを用いて算出する。算出した減速時間ｔ４ｂｉの最大値を減速時間ｔ４ｂとする。
【００６２】
すなわち、高速域加減速時間演算手段５は、高速域の代表点でメカニカルシステムの動特性を表現した運動方程式の演算の少なくとも１回を、低速域加減速時間演算手段３による、低速域の代表点でメカニカルシステムの動特性を表現した運動方程式の演算結果を用いて行うことにより、メカニカルシステムの動特性を表現した運動方程式の演算回数を少なくし、計算量を削減できる効果がある。
【００６３】
「高速域加速終了点」の運動方程式は、次の式（１９）となる。
【００６４】
τ＝Ｍ（ｐ３ｂ）ｖｍ／ｋｔ＋ｈ（ｐ３ｂ，ｖ３ｂ）＋ｇ（ｐ３ｂ）＋ｆ（ｖ３ｂ）
（１９）
【００６５】
従って、Ｍ（ｐ３ｂ）ｖｍの第ｉ要素をｍｖ３ｂｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（２０）を満たすｋｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の加速時間ｔ３ｂｉである。各軸の加速時間ｔ３ｂｉの最大値を加速時間ｔ３ｂとする。
【００６６】
τｍａｘｉ＝ｍｖ３ｂｉ／ｋｔｉ＋ｈ３ｂｉ＋ｇ３ｂｉ＋ｆ３ｂｉ      （ｍｖ３ｂｉ＞０の場合）、
−τｍａｘｉ＝ｍｖ３ｂｉ／ｋｔｉ＋ｈ３ｂｉ＋ｇ３ｂｉ＋ｆ３ｂｉ      （ｍｖ３ｂｉ＜０の場合）                                                            （２０）
【００６７】
ここで、ｈ３ｂｉ、ｇ３ｂｉ、ｆ３ｂｉはそれぞれｈ（ｐ３ｂ，ｖ３ｂ）、ｇ（ｐ３ｂ）、ｆ（ｖ３ｂ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、許容トルクτｍａｘｉとして高速域加速終了点での速度ｖ３ｂの第ｉ軸要素であるｖ３ｂｉでの許容トルクτｍａｘ３ｂｉが用いられる。
【００６８】
「高速域減速開始点」の運動方程式は、次の式（２１）となる。
【００６９】
τ＝−Ｍ（ｐ４ａ）ｖｍ／ｋｔ＋ｈ（ｐ４ａ，ｖ４ａ）＋ｇ（ｐ４ａ）＋ｆ（ｖ４ａ）
（２１）
【００７０】
従って、Ｍ（ｐ４ａ）ｖｍの第ｉ要素をｍｖ４ａｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（２２）を満たすｇｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の減速時間ｔ４ａｉである。各軸の減速時間ｔ４ａｉの最大値を減速時間ｔ４ａとする。
【００７１】
τｍａｘｉ＝−ｍｖ４ａｉ／ｇｔｉ＋ｈ４ａｉ＋ｇ４ａｉ＋ｆ４ａｉ      （ｍｖ４ａｉ＞０の場合）、
−τｍａｘｉ＝−ｍｖ４ａｉ／ｇｔｉ＋ｈ４ａｉ＋ｇ４ａｉ＋ｆ４ａｉ      （ｍｖ４ａｉ＜０の場合）                                                          （２２）
【００７２】
ここで、ｈ４ａｉ、ｇ４ａｉ、ｆ４ａｉはそれぞれｈ（ｐ４ａ，ｖ４ａ）、ｇ（ｐ４ａ）、ｆ（ｖ４ａ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、許容トルクτｍａｘｉとして高速域減速開始点での速度ｖ４ａの第ｉ軸要素であるｖ４ａｉでの許容トルクτｍａｘ４ａｉが用いられる。
【００７３】
加速時間ｔ３ａ、ｔ３ｂの大きいほうを高速域での加速時間ｔ３とし、減速時間ｔ４ａ、ｔ４ｂの大きいほうを高速域での減速時間ｔ４とする。高速域での加速時間、減速時間の算出も規定の繰り返し回数であるｍ（自然数）回実施する。ｋ回目の繰り返しでは高速域開始点、高速域加速終了点、高速域減速開始点、高速域終了点の位置、速度を前回の繰り返しにおける高速域での加速時間、減速時間を用いて算出し、加速時間ｔ３ａ、ｔ３ｂ、減速時間ｔ４ａ、ｔ４ｂを再度計算し、加速時間ｔ３ａ、ｔ３ｂの大きいほうを高速域での加速時間ｔ３とし、減速時間ｔ４ａ、ｔ４ｂの大きいほうを高速域での減速時間ｔ４とする。算出した加速時間ｔ３、減速時間ｔ４は、指令生成手段６に出力される。
【００７４】
なお、低速域、高速域いずれも繰り返し回数は１回でもよい。また、加速時間、減速時間ではなく、加速度、減速度を算出してもよい。
【００７５】
低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮できる。また、最高速度を高くしたため、かえって動作時間が伸びることを防止できる効果がある。さらに、各動作に応じた低速域と高速域の閾値が設定できる効果もある。
【００７６】
実施の形態２．
この発明の実施の形態２に係るメカニカルシステムの制御装置について説明する。
【００７７】
上記の実施の形態１では、高速域開始点の位置を、低速域加減速時間演算手段３による最終繰り返しの計算で使用した加速終了点の位置と同一としたが、この実施の形態２では、低速域加減速時間演算手段３による最後の繰り返しにおいて、加速時間ｔ１、減速時間ｔ２を用いて加速終了点を求め直し、求め直した加速終了点の位置を、高速域開始点の位置とする。
【００７８】
また、上記の実施の形態１では、高速域終了点の位置を、低速域加減速時間演算手段３による最終繰り返しの計算で使用した減速開始点の位置と同一としたが、この実施の形態２では、低速域加減速時間演算手段３による最後の繰り返しにおいて、加速時間ｔ１、減速時間ｔ２を用いて減速開始点を求め直し、求め直した減速開始点の位置を、高速域終了点の位置とする。
【００７９】
低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮できる。また、最高速度を高くしたため、かえって動作時間が伸びることを防止できる効果がある。さらに、各動作に応じた低速域と高速域の閾値が設定できる効果もある。
【００８０】
実施の形態３．
この発明の実施の形態３に係るメカニカルシステムの制御装置について図７を参照しながら説明する。図７は、この発明の実施の形態３に係るメカニカルシステムの制御装置における閾値の修正値の算出方法を説明するための図である。
【００８１】
上記の実施の形態１との差異は、閾値修正手段４の処理内容であるため、この閾値修正手段４の処理内容のみ説明する。上記の実施の形態１では、図５のように、速度−トルク特性（折れ線）と直線τ＝τｓｉとの交点を求めていたが、この実施の形態３では、図７のように、速度−トルク特性（折れ線）Ｃ１（破線）から、粘性摩擦力を差し引いた速度−トルク特性（折れ線）Ｃ２（実線）と直線τ＝τｓｉとの交点を求める。
【００８２】
低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮できる。また、最高速度を高くしたため、かえって動作時間が伸びることを防止できる効果がある。さらに、各動作に応じた低速域と高速域の閾値が設定できる効果もある。
【００８３】
実施の形態４．
この発明の実施の形態４に係るメカニカルシステムの制御装置について図８を参照しながら説明する。図８は、この発明の実施の形態４に係るメカニカルシステムの制御装置の構成を示すブロック図である。
【００８４】
上記の実施の形態１との差異は、閾値修正手段４がないことである。低速域加減速時間演算手段３では各繰り返しにおいて、閾値演算手段２で算出した閾値ｍｉｎｒを修正せずそのまま用いる。
【００８５】
低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮できる。また、最高速度を高くしたため、かえって動作時間が伸びることを防止できる効果がある。さらに、各動作に応じた低速域と高速域の閾値が設定できる効果もある。
【００８６】
実施の形態５．
この発明の実施の形態５に係るメカニカルシステムの制御装置について図９を参照しながら説明する。図９は、この発明の実施の形態５に係るメカニカルシステムの制御装置の構成を示すブロック図である。
【００８７】
上記の実施の形態４との差異は、閾値演算手段２がないことである。この実施の形態５では、各軸のｖａｉ／ｖｍａｘｉの最小値を閾値ｍｉｎｒとして、図示しないメモリに予め記憶しておき、記憶した閾値ｍｉｎｒを低速域加減速時間演算手段３、高速域加減速時間演算手段５、指令生成手段６で使用する。
【００８８】
低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮できる。また、最高速度を高くしたため、かえって動作時間が伸びることを防止できる効果がある。
【００８９】
実施の形態６．
この発明の実施の形態６に係るメカニカルシステムの制御装置について説明する。
【００９０】
上記の実施の形態１では、低速域加減速時間演算手段３では各繰り返しで加速時間ｔ１ａ、ｔ１ｂの大きいほうを加速時間ｔ１として採用したが、この実施の形態６では、第ｋ回目の繰り返しにおける加速時間ｔ１ｂをｔ１ｂ（ｋ）とし、次の式（２３）により、ｋ回目の繰り返しにおいて低速域の加速時間ｔ１を算出する。
【００９１】
ｔ１＝ｍａｘ（ｔ１ａ，ｔ１ｂ（１），ｔ１ｂ（２），・・・ｔ１ｂ（ｋ））
（２３）
【００９２】
同様に、ｋ回目の繰り返しにおける減速時間ｔ２ａをｔ２ａ（ｋ）とし、ｋ回目の繰り返しにおいて低速域の減速時間ｔ２を、次の式（２４）により、算出する。
【００９３】
ｔ２＝ｍａｘ（ｔ２ｂ，ｔ２ａ（１），ｔ２ａ（２），・・・ｔ２ａ（ｋ））
（２４）
【００９４】
また、高速域の加速時間ｔ３、減速時間ｔ４は、次の式（２５）、（２６）により、算出する。
【００９５】
ｔ３＝ｍａｘ（ｔ３ａ（１），ｔ３ａ（２），・・・ｔ３ａ（ｋ），ｔ３ｂ（１），ｔ３ｂ（２），・・・ｔ３ｂ（ｋ））                                        （２５）
【００９６】
ｔ４＝ｍａｘ（ｔ４ａ（１），ｔ４ａ（２），・・・ｔ４ａ（ｋ），ｔ４ｂ（１），ｔ４ｂ（２），・・・ｔ４ｂ（ｋ））                                        （２６）
【００９７】
低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮できる。また、最高速度を高くしたため、かえって動作時間が伸びることを防止できる効果がある。さらに、各動作に応じた低速域と高速域の閾値が設定できる効果もある。
【００９８】
実施の形態７．
この発明の実施の形態７に係るメカニカルシステムの制御装置について説明する。
【００９９】
上記の実施の形態１では、低速域加減速時間演算手段３は、各繰り返しで、加速開始点及び加速終了点から低速域の加速時間ｔ１を算出したが、この実施の形態７では、加速開始点、加速終了点、及び低速域の加速区間の中点（時間軸上の中点）から低速域の加速時間ｔ１を算出する。
【０１００】
同様に、この実施の形態７では、減速開始点、減速終了点、及び低速域の減速区間の中点から低速域の減速時間ｔ２を算出する。
【０１０１】
さらに、この実施の形態７では、高速域加減速時間演算手段５は、各繰り返しで、高速域開始点、高速域加速終了点、及び高速域の加速区間の中点から高速域の加速時間ｔ３を算出し、高速域終了点、高速域減速開始点、及び高速域の減速区間の中点から高速域の減速時間ｔ４を算出する。
【０１０２】
低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮できる。また、最高速度を高くしたため、かえって動作時間が伸びることを防止できる効果がある。さらに、各動作に応じた低速域と高速域の閾値が設定できる効果もある。
【図面の簡単な説明】
【０１０３】
【図１】この発明の実施の形態１に係るメカニカルシステムの制御装置の構成を示すブロック図である。
【図２】この発明の実施の形態１に係るメカニカルシステムの制御装置の動作を示すフローチャートである。
【図３】この発明の実施の形態１に係るメカニカルシステムの制御装置の制御対象であるモータの速度−トルク特性を示す図である。
【図４】この発明の実施の形態１に係るメカニカルシステムの制御装置の低速域の速度指令パターンを示す図である。
【図５】この発明の実施の形態１に係るメカニカルシステムの制御装置における閾値の修正値の算出方法を説明するための図である。
【図６】この発明の実施の形態１に係るメカニカルシステムの制御装置の低速域及び高速域の速度指令パターンを示す図である。
【図７】この発明の実施の形態３に係るメカニカルシステムの制御装置における閾値の修正値の算出方法を説明するための図である。
【図８】この発明の実施の形態４に係るメカニカルシステムの制御装置の構成を示すブロック図である。
【図９】この発明の実施の形態５に係るメカニカルシステムの制御装置の構成を示すブロック図である。
【符号の説明】
【０１０４】
１  最高速度演算手段、２  閾値演算手段、３  低速域加減速時間演算手段、４  閾値修正手段、５  高速域加減速時間演算手段、６  指令生成手段、７  モータ制御手段、８  メカニカルシステム。
【図１】
0004762219.tif 000002
【図２】
0004762219.tif 000003
【図３】
0004762219.tif 000004
【図４】
0004762219.tif 000005
【図５】
0004762219.tif 000006
【図６】
0004762219.tif 000007
【図７】
0004762219.tif 000008
【図８】
0004762219.tif 000009
【図９】
0004762219.tif 000010