JP4762219B2

JP4762219B2 - メカニカルシステムの制御装置

Info

Publication number: JP4762219B2
Application number: JP2007267921A
Authority: JP
Inventors: 清石前川
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2007-10-15
Filing date: 2007-10-15
Publication date: 2011-08-31
Anticipated expiration: 2027-10-15
Also published as: JP2009098786A

Description

この発明は、各軸のモータ及び減速機などの伝達機構の制約を満たす範囲で最短の動作を行うための加減速パラメータを、動作毎に決定する機能を備えたメカニカルシステムの制御装置に関するものである。

従来のメカニカルシステムの制御装置について説明する（例えば、特許文献１参照）。まず、メカニカルシステムの各軸が同時に動作を開始し、かつ同時に動作を終了する場合の各軸の出しうる最高速度を演算する。次に、各軸の速度を台形状に加減速を行う速度指令パターンで動作させる場合の加速時間、減速時間の初期値を設定する。当該動作の開始地点、終了地点、算出した加速時間、減速時間、最高速度から加速開始地点、加速終了地点、減速開始地点、減速終了地点の各軸の位置、速度を算出し、算出した位置、速度を用いて、加速開始地点でモータ及び伝達機構の許容トルク最大値などの制約を満たす範囲で最短となる加速時間ｔ１ａと、加速終了地点でモータ及び伝達機構の許容トルク最大値などの制約を満たす範囲で最短となる加速時間ｔ１ｂを算出し、大きいほうを加速時間ｔ１とする。同様に、算出した位置、速度を用いて、減速開始地点でモータ及び伝達機構の許容トルク最大値などの制約を満たす範囲で最短となる減速時間ｔ２ａと、減速終了地点でモータ及び伝達機構の許容トルク最大値などの制約を満たす範囲で最短となる減速時間ｔ２ｂを算出し、大きいほうを減速時間ｔ２とする。算出した加速時間ｔ１、減速時間ｔ２と開始地点、終了地点、最高速度から加速開始地点、加速終了地点、減速開始地点、減速終了地点の各軸の位置、速度を算出し直し、再度、加速時間、減速時間を算出する。上記繰り返しを規定回数実行する。

特開平０７−２０００３３号公報（実施例３、４、図４）

上述したような従来のメカニカルシステムの制御装置では、高速域でモータの許容トルクが下がる場合、高速域に合わせて低速域でも加速度を下げる必要があるため、許容トルクを満たす範囲で十分な高速化が図れないという問題点があった。また、最高速度を上げると加速度が下がるために、かえって動作時間が長くなる場合があるという問題点があった。

この発明は、上述のような課題を解決するためになされたもので、その目的は、低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮でき、また、最高速度を高くしたため、かえって動作時間が伸びることを防止でき、さらに、各動作に応じた低速域、高速域の閾値が設定できるメカニカルシステムの制御装置を得るものである。

この発明に係るメカニカルシステムの制御装置は、メカニカルシステムの各軸のトルクを各軸の位置、速度、加速度を用いて算出し、時間−速度座標上の低速域の代表点でメカニカルシステムの動特性を表現した第１の運動方程式に基づいて、各軸のモータ及び伝達機構によって構成される駆動機構から定まる許容トルクを満たす範囲で最小の加速時間及び減速時間を演算する低速域加減速時間演算手段と、メカニカルシステムの各軸のトルクを各軸の位置、速度、加速度を用いて算出し、前記時間−速度座標上の高速域の代表点でメカニカルシステムの動特性を表現した第２の運動方程式に基づいて、各軸のモータ及び伝達機構によって構成される駆動機構から定まる許容トルクを満たす範囲で最小の加速時間及び減速時間を演算する高速域加減速時間演算手段と、前記低速域加減速時間演算手段により演算された低速域の加速時間及び減速時間、並びに前記高速域加減速時間演算手段により演算された高速域の加速時間及び減速時間に基づいて、低速域及び高速域で異なる加減速度の速度指令パターンを生成する指令生成手段とを設けたものである。

この発明に係るメカニカルシステムの制御装置は、低速域、高速域のそれぞれの区間でメカニカルシステムの動特性と駆動機構の出力制限値の双方を考慮して最小の加速時間、減速時間を算出するため、低速域と高速域で駆動機構の出力の制限値が異なり、かつ、各軸の変位により出しうる加減速度が異なるメカニカルシステムにおいて、動作毎に低速域と高速域のそれぞれの出力の制限を満たしながら短時間で動作できるという効果を奏する。

実施の形態１．
この発明の実施の形態１に係るメカニカルシステムの制御装置について図１から図６までを参照しながら説明する。図１は、この発明の実施の形態１に係るメカニカルシステムの制御装置の構成を示すブロック図である。なお、以降では、各図中、同一符号は同一又は相当部分を示す。

図１において、この発明の実施の形態１に係るメカニカルシステムの制御装置は、最高速度を演算する最高速度演算手段１と、低速域と高速域の境界を定める閾値を演算する閾値演算手段２と、低速域における加速時間及び減速時間を演算する低速域加減速時間演算手段３と、閾値を修正する閾値修正手段４と、高速域における加速時間及び減速時間を演算する高速域加減速時間演算手段５と、速度指令パターンを生成する指令生成手段６と、ロボット等のメカニカルシステム８の各軸を駆動するモータを制御するモータ制御手段７とが設けられている。なお、この発明に係る駆動機構は、各軸のモータ及び伝達機構から構成されている。

つぎに、この実施の形態１に係るメカニカルシステムの制御装置の動作について図面を参照しながら説明する。図２は、この発明の実施の形態１に係るメカニカルシステムの制御装置の動作を示すフローチャートである。

まず、最高速度演算手段１は、動作プログラムによる動作命令毎に、当該動作の動作開始点と動作終了点の情報を読み取り、当該動作でのメカニカルシステム８の各軸の出しうる最高速度（加減速時間を０とした場合の各軸の速度）を算出する。

メカニカルシステム８の軸数がｎ（自然数）で、各動作で全ての軸が同時に動作を開始し、同時に動作を終了する場合、第ｉ軸の許容最高速度をｖｍａｘｉ、第ｉ軸の動作開始点の値をｓｐｉ、第ｉ軸の動作終了点の値をｅｐｉとすれば、次の式（１）、（２）により、各軸の出しうる最高速度ｖｉが算出できる。ここで、ａｂｓ（）は絶対値、ｍａｘ（）は最大値を意味している。

ｍａｘｄｌ＝ｍａｘ（ａｂｓ（ｅｐ１−ｓｐ１）／ｖｍａｘ１，ａｂｓ（ｅｐ２−ｓｐ２）／ｖｍａｘ２，・・・ａｂｓ（ｅｐｎ−ｓｐｎ）／ｖｍａｘｎ）（１）

ｖｉ＝ａｂｓ（ｅｐｉ−ｓｐｉ）／ｍａｘｄｌ（２）

図３は、この発明の実施の形態１に係るメカニカルシステムの制御装置の制御対象であるモータの速度−トルク特性を示す図である。また、図４は、この発明の実施の形態１に係るメカニカルシステムの制御装置の低速域の速度指令パターンを示す図である。

次に、閾値演算手段２は、低速域と高速域の閾値ｍｉｎｒを以下の手順で算出する（ステップ１０１）。まず、図３に示すように、第ｉ軸のモータの速度−トルク特性において、速度ｖａｉ以上の領域では出せるトルクが下がっているとする。このとき、各軸の中間変数ｒｉを、次の式（３）により求め、次の式（４）により示す、各軸の中間変数ｒｉの最小値を低速域と高速域の閾値ｍｉｎｒとする。ただし、中間変数ｒｉ＞１となる場合には、中間変数ｒｉ＝１とする。

ｒｉ＝ｖａｉ／ｖｉ（３）

ｍｉｎｒ＝ｍｉｎ（ｒ１，ｒ２，・・・，ｒｎ）（４）

このように、動作に応じて閾値ｍｉｎｒを設定することにより、加減速度を高くできる区間（低速域）を動作に応じて延長することが可能になり、動作時間を短縮できる効果がある。

次に、低速域加減速時間演算手段３は、各軸の速度がｍｉｎｒ＊ｖｉ以下の領域を低速域として、低速域での加減速時間を算出する（ステップ１０２〜１０４）。また、高速域加減速時間演算手段５は、各軸の速度がｍｉｎｒ＊ｖｉより大きくなる領域を高速域として、高速域での加減速時間を算出する（ステップ１０６〜１０８）。さらに、指令生成手段６は、各軸の速度がｍｉｎｒ＊ｖｉ以下の領域を低速域として、低速域加減速時間演算手段３で算出された加減速時間に基づいた加減速の速度指令パターンを生成し、各軸の速度がｍｉｎｒ＊ｖｉより大きくなる領域を高速域として、高速域加減速時間演算手段５で算出された加減速時間に基づいた加減速の速度指令パターンを生成する。そして、モータ制御手段７は、低速域及び高速域の速度指令パターンに基づいてモータの加減速の制御を行う。

低速域加減速時間演算手段３は、次の式（５）により示す、各軸の最高速度ｖｌｉとして、図４のような台形状の速度指令パターンで動作させる場合に、各軸のモータ及び伝達機構の許容トルクの制約を満たす範囲で最短の加速時間と減速時間を算出する。

ｖｌｉ＝ｍｉｎｒ＊（ｅｐｉ−ｓｐｉ）／ｍａｘｄｌ（５）

ここで、加速時間及び減速時間は、低速域であっても、次の式（６）により示す、高速域も含めた最高速度ｖｍｉまで一定の加速度で加速する場合の加速及び減速に要する時間とする。

ｖｍｉ＝（ｅｐｉ−ｓｐｉ）／ｍａｘｄｌ（６）

まず、加速時間及び減速時間の初期値を設定する。次に、設定した加速時間及び減速時間の初期値と、各軸の低速域での最高速度ｖｌｉと、高速域も含めた最高速度ｖｍｉと、動作開始点と、動作終了点のデータから、図４に示すような、「加速開始点」、「加速終了点」、「減速開始点」、「減速終了点」の４つの代表点での各軸の位置と速度を算出する。算出した加速開始点の位置、速度と各軸のモータ及び伝達機構の許容トルクの制限値から加速開始点での加速時間ｔ１ａをメカニカルシステム８の運動方程式を用いて算出する。同様に、算出した加速終了点の位置、速度と各軸のモータ及び伝達機構の許容トルクの制限値から加速終了点での加速時間ｔ１ｂをメカニカルシステム８の運動方程式を用いて算出する。算出した加速時間ｔ１ａ、加速時間ｔ１ｂを比較し、大きいほうを加速時間ｔ１とする。

「加速開始点」での加速時間ｔ１ａは、以下のように算出する。まず、メカニカルシステム８の運動方程式が、次の式（７）により、表現できるとする。ここで、Ｍは慣性行列、ａは各軸の加速度から構成されるベクトル、ｈは各軸の遠心・コリオリ力、ｇは各軸の重力、ｆは各軸の摩擦力とする。

τ＝Ｍａ＋ｈ＋ｇ＋ｆ（７）

このとき、Ｍ、ｈ、ｇ、ｆはそれぞれ、各軸の位置で構成されるベクトルｐ、各軸の速度で構成されるベクトルｖの関数であり、Ｍ（ｐ）、ｈ（ｐ，ｖ）、ｇ（ｐ）、ｆ（ｖ）と表記できる。加速開始点での各軸位置から構成されるベクトルをｐ１ａ、加速開始点での各軸速度から構成されるベクトルをｖ１ａ、各軸の低速域の最高速度ｖｌｉから構成されるベクトルをｖｌ、高速域も含めた最高速度ｖｍｉから構成されるベクトルをｖｍとすれば、加速時間がｋｔのときの運動方程式は、次の式（８）となる。

τ＝Ｍ（ｐ１ａ）ｖｍ／ｋｔ＋ｈ（ｐ１ａ，ｖ１ａ）＋ｇ（ｐ１ａ）＋ｆ（ｖ１ａ）
（８）

従って、Ｍ（ｐ１ａ）ｖｍの第ｉ要素をｍｖｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（９）を満たすｋｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の加速時間ｔ１ａｉである。

τｍａｘｉ＝ｍｖｉ／ｋｔｉ＋ｈｉ＋ｇｉ＋ｆｉ（ｍｖｉ＞０の場合）、
−τｍａｘｉ＝ｍｖｉ／ｋｔｉ＋ｈｉ＋ｇｉ＋ｆｉ（ｍｖｉ＜０の場合）
（９）

ここで、ｈｉ、ｇｉ、ｆｉはそれぞれｈ（ｐ１ａ，ｖ１ａ）、ｇ（ｐ１ａ）、ｆ（ｖ１ａ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、加速開始点では速度０での許容トルクτｍａｘ１ｉが式（９）のτｍａｘｉとして用いられる。各軸の加速時間ｔ１ａｉの最大値をｔ１ａとする。

「加速終了点」での加速時間ｔ１ｂも同様に、加速終了点の位置ｐ１ｂ、加速終了点の速度ｖ１ｂから加速時間がｋｔのときの運動方程式が、次の式（１０）となる。

τ＝Ｍ（ｐ１ｂ）ｖｍ／ｋｔ＋ｈ（ｐ１ｂ，ｖ１ｂ）＋ｇ（ｐ１ｂ）＋ｆ（ｖ１ｂ）
（１０）

従って、Ｍ（ｐ１ｂ）ｖｍの第ｉ要素をｍｖｂｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（１１）を満たすｋｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の加速時間ｔ１ｂｉである。各軸の加速時間ｔ１ｂｉの最大値をｔ１ｂとする。

τｍａｘｉ＝ｍｖｂｉ／ｋｔｉ＋ｈｂｉ＋ｇｂｉ＋ｆｂｉ（ｍｖｂｉ＞０の場合）、
−τｍａｘｉ＝ｍｖｂｉ／ｋｔｉ＋ｈｂｉ＋ｇｂｉ＋ｆｂｉ（ｍｖｂｉ＜０の場合）（１１）

ここで、ｈｂｉ、ｇｂｉ、ｆｂｉはそれぞれｈ（ｐ１ｂ，ｖ１ｂ）、ｇ（ｐ１ｂ）、ｆ（ｖ１ｂ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、加速終了点では速度ｖｌｉでの許容トルクτｍａｘｂｉが式（１１）のτｍａｘｉとして用いられる。式（５）で用いるｍｉｎｒが初期値の場合には、τｍａｘｂｉは速度０での許容トルクτｍａｘ１ｉと同じになる。加速時間ｔ１ａ、ｔ１ｂの大きいほうを今回の繰り返し周期での低速域での加速時間ｔ１とする。

「減速開始点」での減速時間ｔ２ａは、下記の手順で算出する。まず、減速開始点での位置ｐ２ａ、減速開始点の速度ｖ２ａから減速時間がｇｔのときの運動方程式が、次の式（１２）となる。

τ＝−Ｍ（ｐ２ａ）ｖｍ／ｇｔ＋ｈ（ｐ２ａ，ｖ２ａ）＋ｇ（ｐ２ａ）＋ｆ（ｖ２ａ）
（１２）

従って、Ｍ（ｐ２ａ）ｖｍの第ｉ要素をｍｖ２ａｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（１３）を満たすｇｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の減速時間ｔ２ａｉである。各軸の減速時間ｔ２ａｉの最大値をｔ２ａとする。

τｍａｘｉ＝−ｍｖ２ａｉ／ｇｔｉ＋ｈ２ａｉ＋ｇ２ａｉ＋ｆ２ａｉ（ｍｖ２ａｉ＜０の場合）、
−τｍａｘｉ＝−ｍｖ２ａｉ／ｇｔｉ＋ｈ２ａｉ＋ｇ２ａｉ＋ｆ２ａｉ（ｍｖ２ａｉ＞０の場合）（１３）

ここで、ｈ２ａｉ、ｇ２ａｉ、ｆ２ａｉはそれぞれｈ（ｐ２ａ，ｖ２ａ）、ｇ（ｐ２ａ）、ｆ（ｖ２ａ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、減速開始点では速度ｖｌｉでの許容トルクτｍａｘ２ａｉが式（１３）のτｍａｘｉとして用いられる。式（５）で用いるｍｉｎｒが初期値の場合には、τｍａｘ２ａｉは速度０での許容トルクτｍａｘ１ｉと同じになる。

「減速終了点」での減速時間ｔ２ｂは、下記の手順で算出する。まず、減速終了点での位置ｐ２ｂ、減速開始点の速度ｖ２ｂから減速時間がｇｔのときの運動方程式が、次の式（１４）となる。

τ＝−Ｍ（ｐ２ｂ）ｖｍ／ｇｔ＋ｈ（ｐ２ｂ，ｖ２ｂ）＋ｇ（ｐ２ｂ）＋ｆ（ｖ２ｂ）
（１４）

従って、Ｍ（ｐ２ｂ）ｖｍの第ｉ要素をｍｖ２ｂｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（１５）を満たすｇｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の減速時間ｔ２ｂｉである。各軸の減速時間ｔ２ｂｉの最大値をｔ２ｂとする。

τｍａｘｉ＝−ｍｖ２ｂｉ／ｇｔｉ＋ｈ２ｂｉ＋ｇ２ｂｉ＋ｆ２ｂｉ（ｍｖ２ｂｉ＜０の場合）、
−τｍａｘｉ＝−ｍｖ２ｂｉ／ｇｔｉ＋ｈ２ｂｉ＋ｇ２ｂｉ＋ｆ２ｂｉ（ｍｖ２ｂｉ＞０の場合）（１５）

ここで、ｈ２ｂｉ，ｇ２ｂｉ，ｆ２ｂｉはそれぞれｈ（ｐ２ｂ，ｖ２ｂ）、ｇ（ｐ２ｂ）、ｆ（ｖ２ｂ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、減速終了点では速度０での許容トルクτｍａｘｌｉが式（１５）式のτｍａｘｉとして用いられる。減速時間ｔ２ａ、ｔ２ｂの大きいほうを今回の繰り返し周期の低速域での減速時間ｔ２とする。

次に、閾値修正手段４は、次回の繰り返しでの閾値ｍｉｎｒを下記の手順で算出する（ステップ１０５）。まず、各軸毎に加速終了点での使用トルクτｋｉ、減速開始点での使用トルクτｇｉをそれぞれ、次の式（１６）、（１７）により算出し、使用トルクτｋｉ、τｇｉの大きいほうを第ｉ軸の使用トルク使用率τｓｉとする。

τｋｉ＝ａｂｓ（ｍｖｂｉ／ｔ１＋ｈｂｉ＋ｇｂｉ＋ｆｂｉ）（１６）
τｇｉ＝ａｂｓ（−ｍｖ２ａｉ／ｔ２＋ｈ２ａｉ＋ｇ２ａｉ＋ｆ２ａｉ）（１７）

図５は、この発明の実施の形態１に係るメカニカルシステムの制御装置において閾値の修正値の算出方法を説明するための図である。また、図６は、この発明の実施の形態１に係るメカニカルシステムの制御装置の低速域及び高速域の速度指令パターンを示す図である。

次に、図５のように、各軸毎に、速度−トルク特性（折れ線）と直線τ＝τｓｉの交点の速度ｖ２ｉを算出する。ここで、使用トルク使用率τｓｉが許容トルクτｍａｘｌｉと一致する場合には、速度ｖ２ｉを速度−トルク特性の折れ点ｖａｉとする。さらに、算出した速度ｖ２ｉが最高速度ｖｉより大きくなっている場合には、速度ｖ２ｉを最高速度ｖｉとする。算出した各軸の速度ｖ２ｉから、次の式（１８）を算出し、中間変数ｒ２ｉの最小値を閾値ｍｉｎｒの修正値とし、次回の繰り返しでの低速域と高速域の閾値とする。

ｒ２ｉ＝ｖ２ｉ／ｖｉ（１８）

このようにして、動作に応じて低速域の範囲、つまり閾値を修正することにより、加減速度を高くできる区間を動作に応じて延長することが可能になり、動作時間を短縮できる効果がある。

２回目以降の繰り返しでは、修正した閾値ｍｉｎｒと、前回の周期の加速時間、減速時間を用いて、低速域での加速終了点、減速開始点を算出し直し、再度、式（１１）、式（１３）の演算を行い、加速終了点、減速開始点での加速時間ｔ１ｂ、減速時間ｔ２ａを求め直す。加速時間ｔ１ａ、ｔ１ｂの大きいほうを今回の繰り返し周期での低速域での加速時間ｔ１とする。また、減速時間ｔ２ａ、ｔ２ｂの大きいほうを今回の繰り返し周期での低速域での減速時間ｔ２とする。

規定回数をｎとするとき、閾値修正手段４による閾値ｍｉｎｒの修正をｎ−１回繰り返し、低速域加減速時間演算手段３での加速時間ｔ１、減速時間ｔ２の算出をｎ回繰り返すと、低速域での閾値ｍｉｎｒ、加速時間ｔ１、及び減速時間ｔ２の算出を終了し、高速域加減速時間演算手段５へ移行する。

また、ｎ回目の繰り返しで算出した加速時間ｔ１、減速時間ｔ２と、ｎ−１回目の繰り返しで算出した閾値ｍｉｎｒを、低速域における加速時間ｔ１、減速時間ｔ２及び閾値ｍｉｎｒとして高速域加減速時間演算手段５及び指令生成手段６へ出力する。

次に、高速域加減速時間演算手段５は、図６に示す「高速域開始点」、「高速域加速終了点」、「高速域減速開始点」、「高速域終了点」の４つの代表点でメカニカルシステム８の運動方程式の計算を行い、高速域の加速時間ｔ３ａ、ｔ３ｂと高速域の減速時間ｔ４ａ、ｔ４ｂを算出する。

まず、高速域の加速時間、減速時間の初期値をそれぞれ低速域の加速時間ｔ１、低速域の減速時間ｔ２とし、高速域加速終了点での各軸の位置ｐ３ｂ、各軸の速度ｖ３ｂと、高速域減速開始点での各軸の位置ｐ４ａ、各軸の速度ｖ４ａを算出する。

さらに、高速域開始点の位置、速度は、低速域加減速時間演算手段３による最後の繰り返しにおける加速終了点での位置ｐ１ｂ、速度ｖ１ｂと同一とする。また、高速域終了点での位置、速度は、低速域加減速時間演算手段３による最後の繰り返しにおける減速開始点での位置ｐ２ａ、速度ｖ２ａと同一とする。

「高速域開始点」の運動方程式は、式（１０）となるため、式（１１）を満たすｋｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の加速時間ｔ３ａｉである。ただし、低速域における計算と異なるのは、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、許容トルクτｍａｘｉとして高速域加速終了点での速度ｖ３ｂの第ｉ軸要素であるｖ３ｂｉでの許容トルクτｍａｘ３ｂｉを用いて算出する。算出した加速時間ｔ３ａｉの最大値を加速時間ｔ３ａとする。

「高速域終了点」の運動方程式は、式（１２）となるため、式（１３）を満たすｇｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の減速時間ｔ４ｂｉである。ただし、低速域における計算と異なるのは、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、許容トルクτｍａｘｉとして高速域減速開始点での速度ｖ４ａの第ｉ軸要素であるｖ４ａｉでの許容トルクτｍａｘ４ａｉを用いて算出する。算出した減速時間ｔ４ｂｉの最大値を減速時間ｔ４ｂとする。

すなわち、高速域加減速時間演算手段５は、高速域の代表点でメカニカルシステムの動特性を表現した運動方程式の演算の少なくとも１回を、低速域加減速時間演算手段３による、低速域の代表点でメカニカルシステムの動特性を表現した運動方程式の演算結果を用いて行うことにより、メカニカルシステムの動特性を表現した運動方程式の演算回数を少なくし、計算量を削減できる効果がある。

「高速域加速終了点」の運動方程式は、次の式（１９）となる。

τ＝Ｍ（ｐ３ｂ）ｖｍ／ｋｔ＋ｈ（ｐ３ｂ，ｖ３ｂ）＋ｇ（ｐ３ｂ）＋ｆ（ｖ３ｂ）
（１９）

従って、Ｍ（ｐ３ｂ）ｖｍの第ｉ要素をｍｖ３ｂｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（２０）を満たすｋｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の加速時間ｔ３ｂｉである。各軸の加速時間ｔ３ｂｉの最大値を加速時間ｔ３ｂとする。

τｍａｘｉ＝ｍｖ３ｂｉ／ｋｔｉ＋ｈ３ｂｉ＋ｇ３ｂｉ＋ｆ３ｂｉ（ｍｖ３ｂｉ＞０の場合）、
−τｍａｘｉ＝ｍｖ３ｂｉ／ｋｔｉ＋ｈ３ｂｉ＋ｇ３ｂｉ＋ｆ３ｂｉ（ｍｖ３ｂｉ＜０の場合）（２０）

ここで、ｈ３ｂｉ、ｇ３ｂｉ、ｆ３ｂｉはそれぞれｈ（ｐ３ｂ，ｖ３ｂ）、ｇ（ｐ３ｂ）、ｆ（ｖ３ｂ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、許容トルクτｍａｘｉとして高速域加速終了点での速度ｖ３ｂの第ｉ軸要素であるｖ３ｂｉでの許容トルクτｍａｘ３ｂｉが用いられる。

「高速域減速開始点」の運動方程式は、次の式（２１）となる。

τ＝−Ｍ（ｐ４ａ）ｖｍ／ｋｔ＋ｈ（ｐ４ａ，ｖ４ａ）＋ｇ（ｐ４ａ）＋ｆ（ｖ４ａ）
（２１）

従って、Ｍ（ｐ４ａ）ｖｍの第ｉ要素をｍｖ４ａｉとし、各軸のモータ及び伝達機構から定まる許容トルクをτｍａｘｉとすると、次の式（２２）を満たすｇｔｉが、第ｉ軸のモータ及び伝達機構から定まる許容トルクを満たす範囲で最短の減速時間ｔ４ａｉである。各軸の減速時間ｔ４ａｉの最大値を減速時間ｔ４ａとする。

τｍａｘｉ＝−ｍｖ４ａｉ／ｇｔｉ＋ｈ４ａｉ＋ｇ４ａｉ＋ｆ４ａｉ（ｍｖ４ａｉ＞０の場合）、
−τｍａｘｉ＝−ｍｖ４ａｉ／ｇｔｉ＋ｈ４ａｉ＋ｇ４ａｉ＋ｆ４ａｉ（ｍｖ４ａｉ＜０の場合）（２２）

ここで、ｈ４ａｉ、ｇ４ａｉ、ｆ４ａｉはそれぞれｈ（ｐ４ａ，ｖ４ａ）、ｇ（ｐ４ａ）、ｆ（ｖ４ａ）の第ｉ要素である。また、モータ及び伝達機構から定まる許容トルクがモータにより決まる場合、許容トルクτｍａｘｉとして高速域減速開始点での速度ｖ４ａの第ｉ軸要素であるｖ４ａｉでの許容トルクτｍａｘ４ａｉが用いられる。

加速時間ｔ３ａ、ｔ３ｂの大きいほうを高速域での加速時間ｔ３とし、減速時間ｔ４ａ、ｔ４ｂの大きいほうを高速域での減速時間ｔ４とする。高速域での加速時間、減速時間の算出も規定の繰り返し回数であるｍ（自然数）回実施する。ｋ回目の繰り返しでは高速域開始点、高速域加速終了点、高速域減速開始点、高速域終了点の位置、速度を前回の繰り返しにおける高速域での加速時間、減速時間を用いて算出し、加速時間ｔ３ａ、ｔ３ｂ、減速時間ｔ４ａ、ｔ４ｂを再度計算し、加速時間ｔ３ａ、ｔ３ｂの大きいほうを高速域での加速時間ｔ３とし、減速時間ｔ４ａ、ｔ４ｂの大きいほうを高速域での減速時間ｔ４とする。算出した加速時間ｔ３、減速時間ｔ４は、指令生成手段６に出力される。

なお、低速域、高速域いずれも繰り返し回数は１回でもよい。また、加速時間、減速時間ではなく、加速度、減速度を算出してもよい。

低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮できる。また、最高速度を高くしたため、かえって動作時間が伸びることを防止できる効果がある。さらに、各動作に応じた低速域と高速域の閾値が設定できる効果もある。

実施の形態２．
この発明の実施の形態２に係るメカニカルシステムの制御装置について説明する。

上記の実施の形態１では、高速域開始点の位置を、低速域加減速時間演算手段３による最終繰り返しの計算で使用した加速終了点の位置と同一としたが、この実施の形態２では、低速域加減速時間演算手段３による最後の繰り返しにおいて、加速時間ｔ１、減速時間ｔ２を用いて加速終了点を求め直し、求め直した加速終了点の位置を、高速域開始点の位置とする。

また、上記の実施の形態１では、高速域終了点の位置を、低速域加減速時間演算手段３による最終繰り返しの計算で使用した減速開始点の位置と同一としたが、この実施の形態２では、低速域加減速時間演算手段３による最後の繰り返しにおいて、加速時間ｔ１、減速時間ｔ２を用いて減速開始点を求め直し、求め直した減速開始点の位置を、高速域終了点の位置とする。

実施の形態３．
この発明の実施の形態３に係るメカニカルシステムの制御装置について図７を参照しながら説明する。図７は、この発明の実施の形態３に係るメカニカルシステムの制御装置における閾値の修正値の算出方法を説明するための図である。

上記の実施の形態１との差異は、閾値修正手段４の処理内容であるため、この閾値修正手段４の処理内容のみ説明する。上記の実施の形態１では、図５のように、速度−トルク特性（折れ線）と直線τ＝τｓｉとの交点を求めていたが、この実施の形態３では、図７のように、速度−トルク特性（折れ線）Ｃ１（破線）から、粘性摩擦力を差し引いた速度−トルク特性（折れ線）Ｃ２（実線）と直線τ＝τｓｉとの交点を求める。

実施の形態４．
この発明の実施の形態４に係るメカニカルシステムの制御装置について図８を参照しながら説明する。図８は、この発明の実施の形態４に係るメカニカルシステムの制御装置の構成を示すブロック図である。

上記の実施の形態１との差異は、閾値修正手段４がないことである。低速域加減速時間演算手段３では各繰り返しにおいて、閾値演算手段２で算出した閾値ｍｉｎｒを修正せずそのまま用いる。

実施の形態５．
この発明の実施の形態５に係るメカニカルシステムの制御装置について図９を参照しながら説明する。図９は、この発明の実施の形態５に係るメカニカルシステムの制御装置の構成を示すブロック図である。

上記の実施の形態４との差異は、閾値演算手段２がないことである。この実施の形態５では、各軸のｖａｉ／ｖｍａｘｉの最小値を閾値ｍｉｎｒとして、図示しないメモリに予め記憶しておき、記憶した閾値ｍｉｎｒを低速域加減速時間演算手段３、高速域加減速時間演算手段５、指令生成手段６で使用する。

低速域、高速域それぞれ独立してモータ及び伝達機構の制限を満たす範囲でできるだけ高い加減速度で動作できるため、低速域で必要以上に加減速を落とす必要がなくなり動作時間が短縮できる。また、最高速度を高くしたため、かえって動作時間が伸びることを防止できる効果がある。

実施の形態６．
この発明の実施の形態６に係るメカニカルシステムの制御装置について説明する。

上記の実施の形態１では、低速域加減速時間演算手段３では各繰り返しで加速時間ｔ１ａ、ｔ１ｂの大きいほうを加速時間ｔ１として採用したが、この実施の形態６では、第ｋ回目の繰り返しにおける加速時間ｔ１ｂをｔ１ｂ（ｋ）とし、次の式（２３）により、ｋ回目の繰り返しにおいて低速域の加速時間ｔ１を算出する。

ｔ１＝ｍａｘ（ｔ１ａ，ｔ１ｂ（１），ｔ１ｂ（２），・・・ｔ１ｂ（ｋ））
（２３）

同様に、ｋ回目の繰り返しにおける減速時間ｔ２ａをｔ２ａ（ｋ）とし、ｋ回目の繰り返しにおいて低速域の減速時間ｔ２を、次の式（２４）により、算出する。

ｔ２＝ｍａｘ（ｔ２ｂ，ｔ２ａ（１），ｔ２ａ（２），・・・ｔ２ａ（ｋ））
（２４）

また、高速域の加速時間ｔ３、減速時間ｔ４は、次の式（２５）、（２６）により、算出する。

ｔ３＝ｍａｘ（ｔ３ａ（１），ｔ３ａ（２），・・・ｔ３ａ（ｋ），ｔ３ｂ（１），ｔ３ｂ（２），・・・ｔ３ｂ（ｋ））（２５）

ｔ４＝ｍａｘ（ｔ４ａ（１），ｔ４ａ（２），・・・ｔ４ａ（ｋ），ｔ４ｂ（１），ｔ４ｂ（２），・・・ｔ４ｂ（ｋ））（２６）

実施の形態７．
この発明の実施の形態７に係るメカニカルシステムの制御装置について説明する。

上記の実施の形態１では、低速域加減速時間演算手段３は、各繰り返しで、加速開始点及び加速終了点から低速域の加速時間ｔ１を算出したが、この実施の形態７では、加速開始点、加速終了点、及び低速域の加速区間の中点（時間軸上の中点）から低速域の加速時間ｔ１を算出する。

同様に、この実施の形態７では、減速開始点、減速終了点、及び低速域の減速区間の中点から低速域の減速時間ｔ２を算出する。

さらに、この実施の形態７では、高速域加減速時間演算手段５は、各繰り返しで、高速域開始点、高速域加速終了点、及び高速域の加速区間の中点から高速域の加速時間ｔ３を算出し、高速域終了点、高速域減速開始点、及び高速域の減速区間の中点から高速域の減速時間ｔ４を算出する。

この発明の実施の形態１に係るメカニカルシステムの制御装置の構成を示すブロック図である。この発明の実施の形態１に係るメカニカルシステムの制御装置の動作を示すフローチャートである。この発明の実施の形態１に係るメカニカルシステムの制御装置の制御対象であるモータの速度−トルク特性を示す図である。この発明の実施の形態１に係るメカニカルシステムの制御装置の低速域の速度指令パターンを示す図である。この発明の実施の形態１に係るメカニカルシステムの制御装置における閾値の修正値の算出方法を説明するための図である。この発明の実施の形態１に係るメカニカルシステムの制御装置の低速域及び高速域の速度指令パターンを示す図である。この発明の実施の形態３に係るメカニカルシステムの制御装置における閾値の修正値の算出方法を説明するための図である。この発明の実施の形態４に係るメカニカルシステムの制御装置の構成を示すブロック図である。この発明の実施の形態５に係るメカニカルシステムの制御装置の構成を示すブロック図である。

符号の説明

１最高速度演算手段、２閾値演算手段、３低速域加減速時間演算手段、４閾値修正手段、５高速域加減速時間演算手段、６指令生成手段、７モータ制御手段、８メカニカルシステム。

Claims

メカニカルシステムの各軸のトルクを各軸の位置、速度、加速度を用いて算出し、時間−速度座標上の低速域の代表点でメカニカルシステムの動特性を表現した第１の運動方程式に基づいて、各軸のモータ及び伝達機構によって構成される駆動機構から定まる許容トルクを満たす範囲で最小の加速時間及び減速時間を演算する低速域加減速時間演算手段と、
メカニカルシステムの各軸のトルクを各軸の位置、速度、加速度を用いて算出し、前記時間−速度座標上の高速域の代表点でメカニカルシステムの動特性を表現した第２の運動方程式に基づいて、各軸のモータ及び伝達機構によって構成される駆動機構から定まる許容トルクを満たす範囲で最小の加速時間及び減速時間を演算する高速域加減速時間演算手段と、
前記低速域加減速時間演算手段により演算された低速域の加速時間及び減速時間、並びに前記高速域加減速時間演算手段により演算された高速域の加速時間及び減速時間に基づいて、低速域及び高速域で異なる加減速度の速度指令パターンを生成する指令生成手段と
を備えたことを特徴とするメカニカルシステムの制御装置。
前記高速域加減速時間演算手段は、前記第２の運動方程式の高速域開始点及び高速域終了点の演算の少なくとも１回を、前記低速域加減速時間演算手段による、前記第１の運動方程式の演算結果を用いて行う
ことを特徴とする請求項１記載のメカニカルシステムの制御装置。
前記駆動機構の速度−出力特性及び当該動作において前記駆動機構の出しうる最高速度に基づいて、低速域と高速域で異なる加減速度の速度指令パターンを生成する際の低速域と高速域の閾値を演算する閾値演算手段
をさらに備えたことを特徴とする請求項１又は２記載のメカニカルシステムの制御装置。
前記第１の運動方程式の演算結果に基づいて算出される加減速時間を用いた場合の各軸トルクを算出し、算出した各軸トルクに基づいて速度−トルク特性の折れ線を補正し、補正した折れ線に基づいて、前記閾値を修正する閾値修正手段
をさらに備えたことを特徴とする請求項３記載のメカニカルシステムの制御装置。