JP5343081B2 - 経路計画器用の加加速度制限軌道計画システムおよび方法 - Google Patents

Description

本明細書に記載の主題は、一般に自動運動制御器に関し、より具体的には、加加速度制限軌道計画に関する。

経路計画器または経路生成器は、典型的には運動制御器を備える。経路計画器は、運動を必要とする機械装置を制御し、そうした機械装置の被制御要素の運動を決定するために使用される。経路計画を利用した機械装置が、製造、組立て、パッケージング、および他の生産能力など、様々な工業システムにおいて見受けられる。例えば、サーボモータは、かかる装置の一種である。他のモータでは、ドリルビットまたはロボットアームなどの要素の運動を制御することができる。

経路生成器は、様々な運動経路アルゴリズムを離散時間制御器と共に使用して、経路プロファイルを生成する。こうした経路プロファイルは、指定された運動パラメータに基づく。指定された運動パラメータには、終了位置、最大速度、最大加速度、最大減速度、および指令加加速度といった指令入力が含まれ得る。運動の初期状態は、経路計画器の初期状態によって規定され、したがって、初期位置、初期速度、初期加速度、および初期加加速度もやはり既知の値となる。

加加速度は、加速度または加速度傾斜の変化率として定義される。したがって、加加速度は加速度の微分であり、平滑な始動が求められる数多くの用途で重要な変数となる。加速度、減速度、および速度制約を条件とした、初期位置から最終位置までの一定加加速度経路を生成する運動方程式が周知である。したがって、上述の既知の値を運動方程式と併せると、所望の軌道を生成する解を得ることができる方程式の集合（ｅｑｕａｔｉｏｎ ｓｅｔ）が得られる
任意の運動を実施する前に、既知／未知の変数に基づいて相関した運動方程式を解くことができる。この方法によってソリューション（ｓｏｌｕｔｉｏｎ）が得られるが、この方法では複雑な計算（ｃｏｍｐｌｅｘ ｃａｌｃｕｌａｔｉｏｎ）が必要となり、それは実時間で実施するには容易には適合可能でない。さらに、多くの場合では、複雑なソリューション（ｃｏｍｐｌｅｘ ｓｏｌｕｔｉｏｎ）となり、それでは閉じた形（ｃｌｏｓｅｄ ｆｏｒｍ）が得られない。

米国特許第４，７６９，５８３号公報

したがって、プログラミングされた制限値を超えずに、初期状態および最終状態を満たす時間ベースのプロファイルを生成し、また、実時間制御器において解くことができるソリューションの集合（ｓｏｌｕｔｉｏｎ ｓｅｔ）を提供する手段が望まれる。

本発明の実施形態によれば、実時間で運動プロファイルを生成するシステムは、プロセッサを備える。このプロセッサは、運動を第１フェーズおよび第２フェーズに分割するように構成される。第１フェーズは、運動を定速セグメントに向けて指令することを含み、第２フェーズは、運動が進行中、首尾よく最終状態に到達するために必要となる加加速度値を計算することを含む。

本発明の別の実施形態では、実時間で運動プロファイルを生成するシステムは、プロセッサを備える。このプロセッサは、運動プロファイルに基づいて指令信号を送信し、運動を第１フェーズおよび第２フェーズに分割し、第２フェーズが目標位置に到達するために制御を行わなければならない時点を計算するように構成される。このシステムは、指令信号を受信するように構成された少なくとも１つの入出力モジュールと、運動が制御される装置とをさらに備える。

本発明の別の実施形態では、実時間で運動プロファイルを生成する方法は、運動を第１フェーズおよび第２フェーズに分割することを含む。第１フェーズは、運動を定速セグメントに向けて指令することを含み、第２フェーズは、運動が進行中、首尾よく最終状態に到達するために必要となる加加速度値を計算することを含む。

添付の図面を参照しながら、以下で詳細な説明を行う。

本発明の一実施形態による経路計画を実施する制御システムの概略図である。 本発明の一実施形態の図１の制御システムで利用する、基本運動型の７つのセグメントを示す積重ねグラフである。 ７つのセグメントにわたる共通時間尺度上の経路指令対時間を示す積重ねグラフである。 セグメント１〜３にわたる共通時間尺度上の経路指令対時間を示す積重ねグラフである。 セグメント２および６がない、共通時間尺度上の経路指令対時間を示す積重ねグラフである。 新たな運動指令を備えた、共通時間尺度上の経路指令対時間を示す積重ねグラフである。 プログラミングされた制限値に反する新たな運動指令を備えた、共通時間尺度上の経路指令対時間を示す積重ねグラフである。 セグメント６のない、セグメント５〜７にわたる共通時間尺度上の経路指令対時間を示すグラフである。

本発明の実施形態は、実時間で解くことができる時間ベースの運動プロファイルを生成し、また、プログラミングされた制限値を超えずに、初期状態および最終状態を満たすシステムおよび方法に関する。制限値とは、最小値もしくは最大値、またはそれらの組合せでもよい。

ここで図１を参照すると、本発明の一実施形態による経路計画用のプログラム可能自動制御器（ＰＡＣ−Ｐｒｏｇｒａｍｍａｂｌｅ Ａｕｔｏｍａｔｉｏｎ Ｃｏｎｔｒｏｌｌｅｒ）型運動システム１のブロック図が示されている。ＰＡＣシステム１は、ＰＡＣシステム型運動制御器９、ＰＡＣシステム型運動モジュール（ＰＭＭ）ファイバ端子ブロック５、およびマイクロプロセッササブシステム３からなる（ＰＭＭ）マザーボード２を含む。プロセッサシステムもまた、使用することができる。ＰＭＭマザーボード２は、経路生成器４を含むいくつかのサブシステムをさらに含む。経路生成器４は、経路計画専用マイクロプロセッサ３上で実行されるファームウェアサブシステムである。ＰＭＭマザーボード２の機能を支援する他のファームウェアサブシステムには、通信バス６が含まれ、ＰＣＩバックプレーンドライバ、状態機械、コマンド処理、コマンドインターフェイス、サーボ制御、および他の関連するハードウェア支援機能などがある。

ＰＡＣシステム１は、装置から様々なステータス、警告、または入力信号を受信する複数のＩ／Ｏモジュール７を含むことができる。Ｉ／Ｏモジュール７は、入出力装置に結合される。図１には１つのＩ／Ｏモジュールしか示されていないが、ＰＡＣシステムは、複数のＩ／Ｏモジュール、例えばモータと連係するＩ／Ｏモジュールを含んでもよい。

ＰＡＣ運動システムはまた、ＰＭＭサーボ制御ボード８を含む。このサーボ制御ボード８は、運動制御モジュール（ＰＭＭ）内部にホストされたドーターボードでよい。サーボ制御ボード８は、例えばロボットアームなどの機械装置の被制御要素の運動制御器として機能する。

プロセッサ３は、経路生成および経路追従を含めて、様々な主要機能を実施する。経路生成とは、初期位置と目標位置との間の、実時間における指令軌道の導出（ｄｅｒｉｖａｔｉｏｎ）を指す。経路追従とは、所望の軌道に従って、位置決め可能部材および被制御要素などの装置の運動を遂行するモータの制御を指す。

初期位置と目標位置との間の最適時間経路は、ユーザがプログラミングした指令加加速度値を利用したものである。しかし、最適時間経路の追従は、実時間用途には容易には適合可能でない。最適時間経路に追従するには、処理装置によって、目標位置に到達するために指令加加速度値を実施しなければならない正確な時期を経路に沿って計算しなければならない。正確な時間が計算されると、理想の軌道が分かり、追従することができる。運動全体に関する正確な時間を求めるには、複雑な計算を解く必要があり、この計算では多くの場合閉じた形のソリューションは得られない。

本発明の実施形態では、プロセッサ３は、運動を２つのフェーズに分割する２フェーズ軌道生成法を利用する。実時間で最適時間経路にできる限り近似して追従するために、プロセッサ３は、第２フェーズが開始するまで、ユーザが指定した指令加加速度値を使用する。任意の運動を指令する前に、プロセッサ３は、運動が進行している間のあらゆるサンプル期間について、最終状態に到達するために必要となる加加速度値を計算する。また、運動の進行中に、指令加加速度値を、現時点でのサンプル期間について、最終状態に到達するために必要となる計算済み加加速度値と比較し、また、次のサンプル期間について、最終状態に到達するために必要となる計算済み加加速度値と比較する。指令加加速度値Ｊ_ｃが、現時点でのサンプル期間について計算した加加速度値Ｊ_ｎと、次のサンプル期間について計算した加加速度値Ｊ_ｎ＋１との間にある場合、時間ｎで実施される加加速度値はＪ_ｎとなる。これは、運動の第２フェーズが開始する時期である。

その結果、運動プロファイルは、プログラミングされた制限値を超えることなく、またはユーザが指定した入力指令値に反することなく、実時間で最適時間経路にできる限り近似して追従することになる。この運動プロファイルは、既知の目標位置に到達する最短時間経路を示し、この目標位置は、サンプル期間で減速（または加速）が開始する位置である。最終状態に応じて、最終状態に到達するための加加速度値は、より低速への減速、またはより高速への加速を表すことがある。

運動開始前に生成される運動プロファイルに基づいて、プロセッサ３は、既知の初期位置と既知の目標位置との間で運動を実行するための指令信号を送信する。Ｉ／Ｏモジュール７が、プロセッサ３から指令信号を受信する。

次に図２の積重ねグラフを参照すると、２フェーズ法を用いて生成された、共通時間尺度上の時間に対する経路指令が示されている。図２は、運動が完了するために７つのセグメントが必要となった基本運動型をさらに示す。具体的には、７つのセグメントは、第１フェーズからの３つのセグメント、定速セグメント、および第２フェーズからの３つのセグメントを含む。

一定加加速度モードでは、加加速度曲線またはプロファイル２０は、セグメント１〜３のそれぞれで一定の指令値を維持している。運動の第２フェーズは、セグメント５の開始時から開始している。具体的には、運動は、指令加加速度値Ｊ_ｃが、現時点でのサンプル期間について計算した加加速度値Ｊ_ｎよりも大きいが、次のサンプル期間について計算した加加速度値Ｊ_ｎ＋１よりも小さくなるまで、一定加加速度セグメント（セグメント４）の間続く。時間ｎでの、の第２フェーズ（セグメント５）開始時に、プロセッサ３はＪ_ｎを加加速度値として実施する。

図２から分かるように、加加速度曲線２０は、サンプル時間ｎで値Ｊ_ｎを有し、セグメント５の持続時間にわたって保持する。セグメント５、６、および７の加加速度値は、常に指令値以下となる。この例示的なケースでは、最終状態に到達するのに必要となる加加速度値は、より低速への減速を表す。

図２では、加速度曲線２２は、加加速度曲線２０に対応する運動全体にわたる加速度を示し、セグメント２でその最大値に達している。速度曲線２４は、加加速度曲線２０および加速度曲線２２に対応する運動全体にわたる速度を示す。速度は、セグメント４でその最大値に達している。被制御装置の変位を変位曲線２６によって示し、この曲線は、加加速度２０、加速度２２、および速度２４のプロファイルに対応している。

定速セグメントを備えた２フェーズ運動は、ポイントツーポイント（ｐｏｉｎｔ−ｔｏ−ｐｏｉｎｔ）運動から生じるものであり、非被制御変数は全て最大値に達する。速度、加速度、および減速度は全て、それらの最大値に達している。表１に、各セグメントおよびその特有の側面を示す。

本発明の実施形態では、プロセッサ３は、一定加加速度モードを利用して運動プロファイルを計算する。この場合、加加速度が被制御変数であり、加速度および速度は、超えるべきでない最大値である。経路計画アルゴリズムを簡単にするために、プロセッサ３は、運動が進行中、首尾よく最終状態を満たすために必要となる加加速度値を計算する。これはまた、減速ソリューション（ｄｅｃｅｌｅｒａｔｉｏｎ ｓｏｌｕｔｉｏｎ）としても知られる。しかし、最終状態に応じて、最終状態に到達するために必要となる加加速度値は、より低速度への減速、またはより高速度への加速を示すことがある。

２フェーズ軌道生成法では、第１フェーズで、運動が定速セグメントに到達するように指令する。第２フェーズは、進行中の運動を常に監視して、目標位置または所望の最終状態に到達するように減速を開始しなければならない時点を決定する。第１フェーズを用いて、初期運動を計画する。したがって、新たな指令が到着するサンプル期間に対しては、第１フェーズ計算を実施するだけでよい。第２フェーズを実行して、最終状態に到達するように第２フェーズが制御を行うべき時点を決定する。

運動が指令される初期サンプル期間に対して、第１フェーズ計算および第２フェーズ計算の両方を実行することが望ましいが、これは必要条件ではない。処理負荷を平衡にするために、プロセッサ３は、サンプル期間にわたって、処理を第１フェーズ計算および第２フェーズ計算に分割することができる。処理を分割することによって、初期サンプル期間において、まさにその期間で運動の第２フェーズを開始する必要がある場合しか、所望の経路にペナルティが生じない。これは、非常に軽微なペナルティであり、多くの場合、結果として得られる性能の改善によって望ましくなる。しかし、初期状態が、第２フェーズによって制御を行わなければならない時点を既に過ぎている場合、経路計画器は運動の処理を行うことが可能でなければならないので、追加のファームウェアが求められる。

第１フェーズは、運動がセグメント４（定速セグメント）に到達するように指令する。プロセッサ３は、セグメント５〜７を第１フェーズとは考えない。したがって、プロセッサ３は指令された値を用いて、最大指令速度（セグメント４の開始時）で完了する運動を計算する。運動プロファイルは、第２フェーズが開始する前にセグメント４に到達することはできないが、それでもアルゴリズムの複雑さが大幅に低減する。

第２フェーズは、第１フェーズを監視して、所望の最終状態に到達するように減速または加速する必要がある時期を決定する。したがって、現時点で実行されている運動は、減速方程式の初期状態として機能する。このため、プロセッサはセグメント５、６、および７について解くだけでよいので、減速方程式の集合の複雑さが大幅に低減する。

非被制御変数および／または初期状態によって、ソリューションであって、その時間最適ソリューション（ｔｉｍｅ ｏｐｔｉｍａｌ ｓｏｌｕｔｉｏｎ）には、あるセグメントがもはや必要でなくなるソリューションが得られる場合、運動に必要となるセグメントは、７つ未満となる。したがって、プロセッサ３は、最少時間で運動を首尾よく完了させるために、全てのセグメントを計算または使用しなくてもよい。さらに、第２フェーズは、第１フェーズの開始後いつでも開始することができ、運動がセグメント１にあるような早い時期でも開始することができる。

加加速度、速度、加速度、および減速度の入力指令値は、絶対値としてユーザが指定する。しかし、指令値の大きさは、運動中全て正であるというわけではない。したがって、マイクロプロセッサ３は、入力指令値に正しい符号を付与する。

入力値に正しい符号を付与するために、プロセッサ３は、現時点での状態を指令位置と比較する。指令位置が初期位置よりも大きい場合、速度は正、加速度は正、減速度は負、初期加加速度は正となる。図２では、変位曲線２６が、指令位置が初期位置よりも大きいプロファイルを辿っている。一方、積重ねグラフ３は、変位曲線３６について、初期位置が最終位置よりも大きい例示的なケースを示している。プロセッサ３は、指令値に逆符号を割り当てている。したがって、速度は負、加速度は負、減速度は正、および初期加加速度は負となっている。

図３は、共通時間尺度上の経路指令対時間を示す積重ねグラフである。図３では、運動が完了するのに７つのセグメントが必要であった。図２と同様に、７つのセグメントは、第１フェーズからの３つのセグメント、定速セグメント、および第２フェーズからの３つのセグメントを含むが、セグメント１の加加速度指令値は、負である。

図３の第２フェーズは、セグメント５の開始時に開始する。具体的には、運動は、指令加加速度値Ｊ_ｃが、現時点でのサンプル期間について計算した加加速度値Ｊ_ｎよりも大きいが、次のサンプル期間について計算した加加速度値Ｊ_ｎ＋１よりも小さくなるまで、定速セグメント（セグメント４）にわたって続く。第２フェーズ（セグメント５）の開始時、すなわち時間ｎで、プロセッサ３はＪ_ｎを加加速度値として使用する。

図３から分かるように、加加速度曲線３０は、サンプル時間ｎで値Ｊ_ｎを有し、セグメント５の持続時間にわたって保持する。セグメント５、６、および７の加加速度値は、常に指令値以下となる。この例示的なケースでは、最終状態に到達するために必要となる加加速度値は、より高速への加速を表す。

図３でもやはり、加速度曲線３２は加加速度曲線３０に対応する加速度を示し、ここではセグメント１の加速度値は負である。速度曲線３４は、加加速度曲線３０および加速度曲線３２に対応する速度を示し、ここではセグメント１の速度値は負である。

プロセッサ３はまた、加加速度値に正しい符号を付与する。ゼロの初期状態から開始するあらゆる運動について、セグメント１の加加速度の符号は、セグメント３の符号とは逆になる。しかし、ゼロ以外の初期状態の場合は、セグメント１とセグメント３の加加速度の符号は同じでもよい。運動アルゴリズムを用いて、加加速度、速度、加速度、および減速度の正しい符号を決定することができる。

図４の例示的なケースは、運動の第１フェーズのセグメント１〜３の、共通時間尺度上の経路指令対時間を示す積重ねグラフである。加加速度曲線４０は、個々の各セグメントで一定値を維持している。セグメント１とセグメント３の加加速度の大きさは等しい。加速度曲線４２は、加加速度曲線４０に対応する加速度を示し、セグメント２でその最大値に達している。速度曲線４４は、セグメント１〜３の加加速度曲線４０および加速度曲線４２に対応する速度を示す。

図４に例示し、曲線４２によって示すように、初期加速度値は、指令加速度値よりも大きい。この場合、経路生成器の第１の動作は、最大加速度値への減速である。この動作によって、セグメント１および３の加加速度の符号が等しいという状態が得られる。

前述のように、経路計画器は、運動を首尾よく完了させるために、セグメント１〜７全てを必要としてもしなくてもよい。プロセッサ３は、運動を完了させるために必要となる離散時間セグメントの数を決定する。第１フェーズにおいて、経路計画器は、セグメント１〜３のいずれが必要となるか決定する。セグメント４は、第１フェーズの最終状態と考えられるので、計算されない。運動プロファイルがセグメント４に確かに達した場合も、プロセッサ３は通常通りサンプリングを続ける。最終状態に到達するために加加速度値を変える時点で、第２フェーズが引き継ぐ。

第１フェーズに戻り参照すると、既知の運動方程式を用いて、プロセッサ３は、最終状態（セグメント４の開始時）に到達するのに必要となるピーク加速度を解く。プロセッサ３は、ピーク加速度を、運動に許容される最大加速度と比較する。ピーク加速度が最大許容加速度よりも低い場合、セグメント２は必要でない。ピーク加速度が最大許容加速度よりも大きい場合、ピーク加速度を制限するために第２のセグメントが必要となる。

上述のように、任意の運動を指令する前に、プロセッサ３は、あらゆるサンプル期間について、運動の第１フェーズを完了させるために必要となるソリューションを計算する。経路計画アルゴリズムを簡単にするために、第１フェーズで、プロセッサ３は、運動がセグメント４に到達するように指令する。第１フェーズでは、セグメント４の開始時が、運動の最終状態と考えられる。

場合によっては、セグメント２は、運動の第１フェーズを完了させるために必要ではない。図５は、セグメント２が必要でない場合の例示的なケースを示す。図５は、共通時間尺度上の経路指令対時間を示す積重ねグラフである。図５の運動プロファイルは、第１フェーズからの２つのセグメント、定速セグメント、および第２フェーズからの２つのセグメントを含む。図５では、セグメント２は必要でない。その理由は、曲線５４の最大速度が低すぎて最大加速度に達することができず、したがって、所望の最大速度（セグメント４）に達し、維持するために、加速度をゼロまで低減させる必要がないからである。曲線５２では、到達ピーク加速度は、最大許容加速度よりも低い。

加速度曲線５２のセグメント１では、加速率は増大し、セグメント３では、加速率は低減している。したがって、加速度曲線５２は、セグメント１〜３で三角形の加速度プロファイルを辿っている。加速度曲線５２は、プロセッサ３がいかなる制約も超えずに最短経路を実施するので、三角形のプロファイルを辿っている。被制御装置の変位を変位曲線５６によって示し、この曲線は、加加速度５０、加速度５２、および速度５４のプロファイルに対応する。

図５からやはり分かるように、第２フェーズはセグメント５の開始時に開始する。具体的には、運動は、指令加加速度値Ｊ_ｃが、現時点でのサンプル期間について計算した加加速度値Ｊ_ｎよりも大きいが、次のサンプル期間について計算した加加速度値Ｊ_ｎ＋１よりも小さくなるまで、定速セグメント（セグメント４）にわたって続く。第２フェーズ（セグメント５）の開始時、すなわち時間ｎで、プロセッサ３はＪ_ｎを加加速度値として使用する。

加加速度曲線５０は、サンプル時間ｎで値Ｊ_ｎを有し、セグメント５の持続時間にわたって保持する。セグメント５、６、および７の加加速度値は、常に指令値以下となる。この場合、セグメント６は、運動が最終状態に到達するために必要ではないので、示されていない。図５では、最終状態に到達するために必要となる加加速度値は、より低い速度への減速を表す。

一方、最大速度が、最大加速度に達するほど十分に大きい場合、セグメント１、２、および３の全てが、運動の第１フェーズを完了させるために必要となる。言い換えれば、運動が指令速度に達するために、一定加速度セグメント（セグメント２）が必要となる。

図６によって示す例示的なケースでは、３つのセグメント１、２、および３は全て、運動の第１フェーズを完了させるために必要となる。図６は、共通時間尺度上の経路指令対時間の積重ねグラフである。この場合、進行中の運動が、ユーザが指定する新たな運動指令によって中断される。新たな運動指令は、プログラミングされた運動制限値に反することはない。被制御装置の変位を変位曲線６６によって示し、この曲線は、加加速度６０、加速度６２、および速度６４のプロファイルに対応する。

運動のセグメント２の間、加加速度を絶対値１００とした新たな入力指令値が受信される。加速度曲線６２では、そのセグメント２の加速度は、新しく指令された加速度よりも大きい値を有する。したがって、運動を新たな最大加速度まで減速させなければならない。さらに、運動を制限加加速度にし、所望の最大速度に到達させなければならない。

運動特性に対する新たな加加速度指令の効果が、加速度曲線６２および速度曲線６４から分かる。加速度曲線６２は減速して、新たな最大加速度に達している。プロセッサ３は、新たな指令加加速度値を実施し、別のセグメント１を実行して第１フェーズから再び開始している。上記によって、運動が加速度値２００から新たな最大加速度値１００まで減速することが可能となる。加速度曲線６２は一定の最大加速度を示すが、速度曲線６４は、所望の最大速度まで線形に増大している。

例示的な別の実施形態では、プロセッサ３は能動運動を行いながら加加速度を低減させることができる。最大加加速度を低減させる指令を実行し、最大加速度を現時点の値に留めるか、または低減させることができる。この場合、運動に問題が生じる。新たな加加速度値を使用するには、運動が最大速度を超える必要がある。プロセッサ３は、例えば、最大速度または他の最大値など、プログラミングされた制限値に反することはない。

したがって、現時点での加加速度値は、超過することなく最大速度に達する必要がある。プロセッサ３は警告を発し、運動は、加速が完了するまで、加加速度および加速度に関する現時点での指令値を用いて運動を続ける。加速が完了した時点で、プロセッサ３は新たな値を実施する。

図７は、上述のように警告が発せられた場合の例示的なケースである。図７は、共通時間尺度上の経路指令対時間を示す積重ねグラフである。加加速度曲線７０のセグメント２では、加加速度値を１００から１０に、最大加速度を２００から１００に低減させる入力が受信される。しかし、新たな指令加加速度値１０を用いるには、運動が指令最大速度を超える必要が生じる。言い換えれば、加加速度値を１０まで低減させる新たな運動指令では、加速度が最大速度を通過する前にゼロに達することが可能でなくなる。低速のため、加速度曲線７２は、指令最大速度に達するほど十分高速にゼロに達することがない。指令最大速度値は超過されることになり、したがって、プログラミングされた制限値に反することになる。プロセッサ３はこうした値に反することはないので、プロセッサ３は警告を発し、元の加加速度値１００を用いて運動を続ける。したがって、加加速度曲線７０、加速度曲線７２、および速度曲線７４は、セグメント５が開始するまで、加加速度および加速度に関する元の指令値を用いて運動を続ける。

図７に示す運動プロファイルのセグメント５では、運動の第２フェーズが開始し、プロセッサ３によって、加加速度曲線７０に新たな値１０が割り当てられている。加速度曲線７２は、新たな加加速度値に従ってセグメント５にわたり減速している。速度曲線７４は、加加速度曲線７０および加速度曲線７２に対応する速度を示す。さらに、被制御装置の変位を変位曲線７６によって示し、この曲線は、加加速度７０、加速度７２、および速度７４のプロファイルに対応する。

プロセッサ３の第１フェーズによって運動がセグメント４に到達するように指令する間、第２フェーズは、第１フェーズを監視して、所望の最終状態に到達するように減速または加速する必要がある時期を決定する。したがって、現時点で実行されている運動は、減速方程式の初期状態として機能する。第２フェーズでは、経路計画器は、セグメント５、６、および７について解く。セグメント５、６、および７の加加速度値は、常に指令値以下となる。

第２フェーズでは、経路計画器は、運動を首尾よく完了させるためにセグメント６を必要としてもしなくてもよい。まず、プロセッサ３は、指令による制限がないと想定したピーク減速度について計算を行う。次いで、プロセッサ３は、計算したピーク減速度を最大許容減速度と比較する。計算したピーク減速度が最大許容減速度よりも低い場合、セグメント６は必要でない。計算したピーク減速度が最大許容減速度よりも大きい場合、セグメント６が必要となる。その場合、別のアルゴリズムを用いて、セグメント６の使用を表すことができ、すなわち一定減速度セグメントを挿入することになる。第２フェーズでは、セグメント５開始時の初期速度が目標速度よりも低い場合、ピーク加速度もやはり計算することができる。

図８は、運動を完了させるためにセグメント６が必要でない場合の例示的なケースを示す。図８は、共通時間尺度上の経路指令対時間を示す。図８では、計算したピーク減速度が最大許容減速度よりも低いので、セグメント６は必要でない。

加速度曲線８２のセグメント５では加速率は低減し、セグメント７では加速率は増大している。したがって、加速度曲線８２は、セグメント５〜７で三角形の減速度プロファイルを辿っている。加速度曲線８２は、プロセッサ３がいかなる制約も超えずに最短経路を実施するので、三角形プロファイルを辿っている。

加加速度曲線８０は、加速度曲線８２の微分運動プロファイルを示し、個々のセグメント５および７で一定値を維持している。速度曲線８４は、セグメント５および７の加加速度曲線８０および加速度曲線８２に対応する速度を示す。被制御装置の変位を変位曲線８６によって示し、この曲線は、加加速度８０、加速度８２、および速度８４のプロファイルに対応する。

この場合も、減速が制限されない場合に三角形プロファイルが得られる。プロセッサ３は、サンプル期間で減速が開始する場合、いかなる制約も超えずに得られる最短減速度経路を実施する。したがって、曲線８２の経路は、三角形の減速度曲線を辿る。あるいは、運動を完了させるためにセグメント６が必要となる場合、プロセッサ３によって一定減速度セグメントを挿入する。

第２フェーズでは、プロセッサ３は、減速の最終状態を首尾よく満たすために必要となる加加速度を解く。最終状態に応じて、この減速ソリューションは、より低速への減速、またはより高速への加速を必要とし得る。加加速度／減速度傾斜が小さい場合でも、プロセッサ３は常にその加加速度を求めることができる。しかし、加加速度値はなおも、プロセッサ３が、いかなる指令パラメータまたは制約も超えずに完了する最短時間を表す経路プロファイルを生成することができるように、指令値にできる限り近似させなければならない。また、第２フェーズは、サンプル期間で開始し、したがって、プロセッサ３は最短時間経路を示す運動プロファイルを生成し、ここでは首尾よく最終状態を満たすために必要となる計算済み加加速度値が、サンプル期間のセグメント５の開始時に実施される。

プロセッサ３は、運動が最終状態に到達するように指令パラメータにできる限り近似して追従することによって経路プロファイルを生成する。経路生成器のこの要件を達成するために、プロセッサ３は、現時点でのサンプル期間について、また次のサンプル期間についても同様に、最終状態を満たす加加速度を解く。次いで経路計画器が、これらの値を指令加加速度値と比較する。指令加加速度値がこれら２つの値の間にある場合、時間最適ソリューションは、２つのサンプル期間の間にある。その場合、プロセッサ３は、現時点のサンプル期間についての加加速度を用いて運動の第２フェーズを開始する。というのは、この加加速度値によって、加加速度が指令値を超えずにできる限り大きくなることが可能となるからである。

加加速度を低減させることによって計算した軌道は、理想的な軌道ではないが、指令値を超えることはない。したがって、理想的な軌道は分かり、それは２つのサンプル期間の間のどこかに位置する。サンプル期間は、運動制御を改善するためにできる限り短いことが好ましい。

この場合もやはり、最終状態を満たす最終軌道は、減速軌道である必要はない。運動の最終状態に応じて、最終軌道は、加速軌道でもよい。具体的には、第２フェーズの開始時、すなわちセグメント５は、減速セグメントでも、加速セグメントでもよい。さらに、速度がゼロでない最終状態も許容可能である。速度がゼロでない最終状態は、ある運動を別の運動と組み合せた結果生じ得るので、運動の最大速度よりも大きい値を有することも可能である。

本明細書に記載し、添付の図面に示す経路計画器システムおよび方法の構成ならびに配置は、単なる例示にすぎない。本開示には本発明のいくつかの実施形態しか詳細には記載していないが、本開示を検討した当業者には、添付の特許請求の範囲に記載の主題の新規な教示および利点から実質的に逸脱せずに多くの改変形態（例えば、様々な要素のサイズ、寸法、構造、形状、および比率、パラメータ値、取り付け配置、材料の使用、向きなどの変形形態）が可能であることが容易に理解されよう。したがって、かかる改変形態は全て、添付の特許請求の範囲に定義される本発明の範囲内に含まれるものである。いかなる工程または方法ステップの順序またはシーケンスも、代替実施形態に従って変えること、または並べ直すことができる。特許請求の範囲では、いかなるミーンズプラスファンクション項も、記載の機能を実施するものとして本明細書に記載の構造、さらには構造的な均等物だけでなく同等の構造をも対象とするものである。添付の特許請求の範囲に示す本発明の実施形態の趣旨から逸脱することなく、好ましい実施形態、および他の例示的実施形態の設計、動作条件、および構成に関して他の置換え、改変、変形、および省略を行うことができる。したがって、本発明の技術範囲は、上述の実施形態だけでなく、添付の特許請求の範囲に含まれる全てのものを包含する。

１ プログラム可能自動制御器（ＰＡＣ）型運動システム
２ （ＰＭＭ）マザーボード
３ マイクロプロセッササブシステム
４ 経路生成器
５ （ＰＭＭ）ファイバ端子ブロック
６ 通信バス
７ Ｉ／Ｏモジュール
８ ＰＭＭサーボ制御ボード
９ ＰＡＣシステム型運動制御器

Claims (14)

1. 実時間で運動プロファイルを生成するシステムであって、
   運動を第１フェーズおよび第２フェーズに分割するように構成されたプロセッサを備え、前記第１フェーズが、前記運動を定速セグメントに向けて指令することを含み、
   前記第２フェーズが、前記第１フェーズを監視して、運動が進行中に首尾よく最終状態に到達するために必要となる加加速度値を適用することが必要となる時点を決定し、
   現時点でのサンプル期間について、首尾よく最終状態に到達するために必要となる加加速度値（Ｊｎ）を計算し、
   次のサンプル期間について、首尾よく最終状態に到達するために必要となる加加速度値（Ｊｎ＋１）を計算し、
   前記現時点でのサンプル期間について必要となる前記計算済み加加速度値（Ｊｎ）および前記次のサンプル期間について必要となる前記計算済み加加速度値（Ｊｎ＋１）を、指令加加速度値（Ｊｃ）と比較することを含む、システム。
2. 前記プロセッサが、
   前記第２フェーズが目標位置に到達するために必要となる加加速度値を適用することを含み、
   前記第２フェーズが制御を行わなければならない前記時点が、前記第１フェーズの開始後、いつでも生じ得る、請求項１記載のシステム。
3. 前記プロセッサが、サンプル期間にわたって、処理を前記第１フェーズ計算および前記第２フェーズ計算に分割するようにさらに構成される、請求項１記載のシステム。
4. 前記プロセッサが、プログラミングされた制限値を超えずに前記運動プロファイルを生成するようにさらに構成され、前記運動プロファイルが、前記目標位置に到達する最短時間経路を示し、首尾よく最終状態を満たすために必要となる前記加加速度値が、サンプル期間で実施される、請求項２記載のシステム。
5. 前記プロセッサが、前記運動を完了させるために必要となる離散時間セグメントの数を決
   定するようにさらに構成され、前記運動プロファイルが、前記初期位置と目標位置との間で複数の離散時間セグメントを含む、請求項２記載のシステム。
6. 前記第２フェーズが、前記複数の離散時間セグメントのうちの１つと同時に開始する、請求項５記載のシステム。
7. 前記プロセッサが、入力指令値に正しい符号を付与するようにさらに構成され、加加速度、速度、加速度、および減速度に関する入力指令値が、絶対値としてユーザが指定した値である、請求項１記載のシステム。
8. 前記プロセッサが、
   少なくとも１つのユーザが指定した新たな運動指令値を受信すると、進行中の前記運動を中断し、前記少なくとも１つの新たな運動指令値が、前記プログラミングされた制限値に反することがなく、
   前記少なくとも１つの新たな指令値を実施して前記運動を続けるようにさらに構成される、請求項４記載のシステム。
9. 前記プロセッサが、
   少なくとも１つのユーザが指定した新たな運動指令値を受信すると、警告を発し、前記少なくとも１つの新たな運動指令値が、前記プログラミングされた制限値に反し、
   現時点での指令値を用いて前記運動を続けるようにさらに構成される、請求項４記載のシステム。
10. 前記プロセッサが、前記指令加加速度値が、前記現時点でのサンプル期間について必要となる前記計算済み加加速度値と、前記次のサンプル期間に関する前記計算済み加加速度値との間にあるとき、前記現時点でのサンプル期間について必要となる前記計算済み加加速度値を実施して、前記運動の前記第２フェーズを開始するようにさらに構成される、請求項９記載のシステム。
11. 前記現時点でのサンプル期間について必要となる前記計算済み加加速度値が、前記現時点のサンプル期間時に実施される、請求項１乃至１０のいずれかに記載のシステム。
12. 実時間で運動プロファイル生成するシステムであって、
    指令信号を受信するように構成されたＩ／Ｏモジュールと、
    前記運動プロファイルに基づいて前記指令信号を送信し、
    運動を第１フェーズおよび第２フェーズに分割し、
    前記第２フェーズが目標位置に到達するために制御を行わなければならない時点を運動が進行中に計算し、
    現時点でのサンプル期間について、首尾よく最終状態に到達するために必要となる加加速度値（Ｊｎ）を計算し、
    次のサンプル期間について、首尾よく最終状態に到達するために必要となる加加速度値（Ｊｎ＋１）を計算し、
    前記現時点でのサンプル期間について必要となる前記計算済み加加速度値（Ｊｎ）および前記次のサンプル期間について必要となる前記計算済み加加速度値（Ｊｎ＋１）を、指令加加速度値（Ｊｃ）と比較するように構成されたプロセッサと、
    を備える、システム。
13. 前記プロセッサが、
    運動が進行中、首尾よく目標位置に到達するために必要となる加加速度値を計算し、前記第２フェーズが制御を行わなければならない時点で、前記計算済み加加速度値を実施する
    ようにさらに構成され、
    前記第２フェーズが制御を行わなければならない前記時点が、サンプル期間で生じ、
    前記第２フェーズが制御を行わなければならない前記時点が、前記第１フェーズの開始後、いつでも生じ得る、請求項１２記載のシステム。
14. 実時間で運動プロファイルを生成する方法であって、
    運動を第１フェーズおよび第２フェーズに分割するステップと、
    前記第１フェーズの間に、前記運動を定速セグメントに向けて指令するステップと、
    前記第１フェーズを監視して、運動が進行中に首尾よく最終状態に到達するために必要となる加加速度値を適用することが必要となる時点を決定するステップと、
    現時点でのサンプル期間について、首尾よく最終状態に到達するために必要となる加加速度値（Ｊｎ）を計算するステップと、
    次のサンプル期間について、首尾よく最終状態に到達するために必要となる加加速度値（Ｊｎ＋１）を計算するステップと、
    前記現時点でのサンプル期間について必要となる前記計算済み加加速度値（Ｊｎ）および前記次のサンプル期間について必要となる前記計算済み加加速度値（Ｊｎ＋１）を、指令加加速度値（Ｊｃ）と比較するステップと、
    を含む、方法。

Images