(57)【特許請求の範囲】
【請求項１】
通過順序が予め設定された複数の通過点の位置を設定する設定部と、
制御対象が前記複数の通過点を連続的に通過する各前記通過点間の指令軌跡を第１変数に従う区分多項式で設定する軌跡設定部と、
対象とする指令軌跡に対応する前記区分多項式に基づいて制御周期毎の前記制御対象の目標指令値を算出する指令値算出部とを備え、
前記指令値算出部は、
前記制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部と、
前記算出された目標移動距離が一定となるように前記制御周期毎の第１変数の変動量を調整する変数調整部とを含む、コントローラシステム。
【請求項２】
前記第１変数は、０〜１の間で変化する、請求項１記載のコントローラシステム。
【請求項３】
前記区分多項式は、３次のエルミート曲線である、請求項２記載のコントローラシステム。
【請求項４】
前記変数調整部は、
前記算出された目標移動距離に基づいて前記第１変数の基準となる基準変動量を設定し、
前記基準変動量に基づいて前記区分多項式に基づく前記制御対象の座標を少なくとも１回算出し、
前回の制御周期における前記制御対象の座標と、前記基準変動量に基づく算出された前記制御対象の座標との間の距離を算出し、
算出された基準変動量に対する距離と前記目標移動距離との比較に基づいて、前記制御周期毎の第１変数の変動量を調整する、請求項２記載のコントローラシステム。
【請求項５】
前記変数調整部は、
前記基準変動量毎の前記区分多項式に基づく前記制御対象の座標を複数回算出し、
前記基準変動量毎の前記制御対象の座標間のそれぞれの距離を算出し、
前記基準変動量毎の前記制御対象の座標間の距離の合計と、前記目標移動距離とに基づいて前記第１変数の基準となる前記基準変動量を補正する、請求項４記載のコントローラシステム。
【請求項６】
前記変数調整部は、
前記基準変動量毎の前記制御対象の座標間の距離の合計と、前記目標移動距離とを比較し、
前記基準変動量毎の前記制御対象の座標間の距離の合計が前記目標移動距離以上の場合に前記目標移動距離に対する前記第１変数の変動量を算出する、請求項４記載のコントローラシステム。
【請求項７】
前記変数調整部は、前記対象とする指令軌跡の通過点間の距離と、前記目標移動距離と、分割係数とに基づいて前記基準変動量を設定する、請求項４記載のコントローラシステム。
【請求項８】
通過順序が予め設定された複数の通過点の位置を設定するステップと、
制御対象が前記複数の通過点を連続的に通過する各前記通過点間の指令軌跡を第１変数に従う区分多項式で設定するステップと、
対象とする指令軌跡に対応する前記区分多項式に基づいて制御周期毎の前記制御対象の目標指令値を算出するステップとを備え、
前記目標指令値を算出するステップは、
前記制御周期毎の指令制御速度に対する目標移動距離を算出するステップと、
前記算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整するステップとを含む、コントローラシステムの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本開示は、制御対象を制御するコントローラシステムおよびその制御方法に関する。
【背景技術】
【０００２】
各種設備および各設備に配置される各種装置の制御には、ＰＬＣ（プログラマブルロジックコントローラ）などの制御装置が用いられる。
【０００３】
例えば、特開２０００−１３７５０６号公報（特許文献１）には、ロボットの動作を制御する制御装置が指令軌跡に従う指令値を各サーボに出力してロボットの動作を制御する手法が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０００−１３７５０６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
複数の通過点を検査するロボットの動作を制御する場合、ロボットの動作を複数の通過点で停止させずに連続的に動作させることは検査効率を向上させる点で重要である。
【０００６】
一方で、ロボットの動作としては連続的に動作させるだけでなく、動作速度も適切に制御することが重要である。
【０００７】
本開示は、上記の課題を解決するためになされたものであって、制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能なコントローラシステムおよびその制御方法に関する。
【課題を解決するための手段】
【０００８】
ある局面に従うコントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を第１変数に従う区分多項式で設定する軌跡設定部と、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出する指令値算出部とを備える。指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部と、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する変数調整部とを含む。
【０００９】
この構成によれば、指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出し、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する。これにより制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。
【００１０】
好ましくは、第１変数は、０〜１の間で変化する。
【００１１】
この構成によれば、通過点間の距離に応じて軌跡移動速度が変動することを回避して、制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。
【００１２】
好ましくは、区分多項式は、３次のエルミート曲線である。
【００１３】
この構成によれば、３次のエルミート曲線を用いることにより滑らかな曲線軌跡を設定することが可能である。
【００１４】
好ましくは、変数調整部は、算出された目標移動距離に基づいて第１変数の基準となる基準変動量を設定し、基準変動量に基づいて区分多項式に基づく制御対象の座標を少なくとも１回算出し、前回の制御周期における制御対象の座標と、基準変動量に基づく算出された制御対象の座標との間の距離を算出し、算出された基準変動量に対する距離と目標移動距離との比較に基づいて、制御周期毎の第１変数の変動量を調整する。
【００１５】
この構成によれば、基準変動量を用いて試算点との距離を算出することにより、制御周期毎の第１変数の変動量を調整するため効率的に第１変数の変動量を調整することが可能である。
【００１６】
好ましくは、変数調整部は、基準変動量毎の区分多項式に基づく制御対象の座標を複数回算出し、基準変動量毎の制御対象の座標間のそれぞれの距離を算出し、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とに基づいて第１変数の基準となる基準変動量を補正する。
【００１７】
この構成によれば、基準変動量を補正して、補正した基準変動量を用いて試算点との距離を算出することにより、精度の高い制御周期毎の第１変数の変動量に調整することが可能である。
【００１８】
好ましくは、変数調整部は、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とを比較し、基準変動量毎の制御対象の座標間の距離の合計が目標移動距離以上の場合に目標移動距離に対する第１変数の変動量を算出する。
【００１９】
この構成によれば、基準変動量を用いて試算点との距離の合計を算出して、目標移動距離との比較により、制御周期毎の第１変数の変動量を調整するため効率的に第１変数の変動量を調整することが可能である。
【００２０】
好ましくは、変数調整部は、対象とする指令軌跡の通過点間の距離と、目標移動距離と、分割係数とに基づいて基準変動量を設定する。
【００２１】
この構成によれば、分割係数により基準変動量を調整することが可能であるため精度の高い制御周期毎の第１変数の変動量に調整することが可能である。
【００２２】
ある局面に従うコントローラシステムの制御方法は、通過順序が予め設定された複数の通過点の位置を設定するステップと、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を第１変数に従う区分多項式で設定するステップと、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出するステップとを備える。目標指令値を算出するステップは、制御周期毎の指令制御速度に対する目標移動距離を算出するステップと、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整するステップとを含む。
【００２３】
この構成によれば、指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出し、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する。これにより制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。
【００２４】
なお、本開示において、「部」及び「装置」とは、単に物理的手段を意味するものではなく、その「部」及び「装置」が有する機能をソフトウェアによって実現する構成も含む。また、１つの「部」及び「装置」が有する機能が２つ以上の物理的手段や装置によって実現されてもよく、或いは、２つ以上の「部」及び「装置」の機能が１つの物理的手段や装置によって実現されてもよい。さらに、「部」及び「装置」とは、例えば「手段」及び「システム」と言い換えることも可能な概念である。
【発明の効果】
【００２５】
本開示のコントローラシステムおよびその制御方法によれば、制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能である。
【図面の簡単な説明】
【００２６】
【図１】実施形態に従うレビュー制御システム１のアプリケーション例を示す模式図である。
【図２】実施形態に従う駆動装置２の具体的構成を説明する図である。
【図３】実施形態に従う制御装置１００のハードウェアの構成の一例を模式的に示す平面図である。
【図４】実施形態に従う制御装置１００の機能構成を説明する図である。
【図５】実施形態に従う速度ベクトルの設定について説明する図である。
【図６】実施形態に従う軌跡設定部３５とサーボ指令制御部４０との関係について説明する図である。
【図７】実施形態に従うサーボ指令制御部４０が出力する位置座標を説明する図である。
【図８】比較例として指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。
【図９】実施形態に従う指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。
【図１０】実施形態に従う１回目の試算処理を説明する図である。
【図１１】実施形態に従うパラメータｔの基準変動量の補正について説明する図である。
【図１２】実施形態に従う２回目の試算処理を説明する図である。
【図１３】実施形態に従う３回目の試算処理を説明する図である。
【図１４】実施形態に従う１４個の通過点を設定した場合のシミュレーション例について説明する図である。
【図１５】実施形態に従う通過座標および３次多項式の係数について説明する図である。
【図１６】実施形態に従うトレース軌跡を説明する図である。
【図１７】実施形態に従う指令速度の波形を説明する図である。
【発明を実施するための形態】
【００２７】
本発明の実施の形態について、図面を参照しながら詳細に説明する。なお、図中の同一または相当部分については、同一符号を付してその説明は繰り返さない。
【００２８】
以下では図１のような２軸（Ｘ軸、Ｙ軸）構成の同期グループ（「複数のサーボモータ」及びそれらに対応する「複数のサーボドライバ」を含む複数のサーボ制御デバイスに相当する。）を前提に説明するが、本実施形態の同期グループは、２軸以上のサーボ制御デバイスと１以上のトリガ制御デバイスを含む構成であればよい。以下では対象物が図１のような２軸（Ｘ軸、Ｙ軸）で移動するが、対象物の移動形態は、３軸以上であってもよい。
【００２９】
＜Ａ．適用例＞
まず、本発明が適用される場面の一例について説明する。
【００３０】
図１は、実施形態に従うレビュー制御システム１のアプリケーション例を示す模式図である。
【００３１】
図１には、フラットパネルディスプレイＦＰＤを検査するレビュー制御システム１が示されている。
【００３２】
レビュー制御システム１には、駆動装置２と、撮影装置２０とが設けられている。
【００３３】
駆動装置２は、各軸方向の任意の位置に移動可能に設けられている。なお、本例においては、Ｘ、Ｙ軸に移動する場合について説明する。
【００３４】
駆動装置２の先端には、撮影装置２０が設けられており、駆動装置２を駆動することにより予め検出したフラットパネルディスプレイＦＰＤの欠陥部分を撮影装置２０で再検査する。具体的には、当該位置で撮影装置２０で撮像する。
【００３５】
図２は、実施形態に従う駆動装置２の具体的構成を説明する図である。
【００３６】
図２に示されるように、駆動装置２は、３軸を構成する複数のサーボモータと、それぞれのサーボモータに対応するサーボドライバを含むサーボ制御デバイスとにより、撮像検査の対象物（フラットパネルディスプレイＦＰＤの欠陥部分）の位置に移動するように駆動し、当該位置の対象物（フラットパネルディスプレイＦＰＤの欠陥部分）をトリガ制御デバイスとしての撮影装置２０で撮影する。
【００３７】
再び図１を参照して、本例においては、フラットパネルディスプレイＦＰＤに４つの欠陥部分（通過点）が予め示されている。そして、欠陥部分（通過点）の通過順序が予め設定されているものとする。
【００３８】
レビュー制御システム１は、複数の欠陥部分（通過点）を通過する滑らかな指令曲線を設定する。撮影装置２０は、欠陥部分（通過点）を通過する際に当該位置を撮像する。
【００３９】
レビュー制御システム１は、設定された指令曲線に基づいて駆動装置２を制御することにより、止まらずに欠陥部分（通過点）を撮像することが可能となる。
【００４０】
これにより、フラットパネルディスプレイの検査速度の向上を図ることが可能である。
【００４１】
なお、図１に示すレビュー制御システム１は任意のアプリケーションに応用可能である。
【００４２】
本実施の形態に従うレビュー制御システム１は、複数のデバイスが組み合わされたアプリケーションに対しても、１つの制御装置で制御することができる。
【００４３】
なお、フラットパネルディスプレイに限られず、各種の半導体にも同様に適用可能である。
【００４４】
＜Ｂ．構成例＞
［ハードウェア構成］
次に、本実施形態に係る制御装置１００のハードウェア構成の一例について説明する。
【００４５】
図３は、実施形態に従う制御装置１００のハードウェアの構成の一例を模式的に示す平面図である。
【００４６】
図３を参照して、制御装置１００は、図１及び図２に例示した駆動装置２及び撮影装置２０に接続され、制御演算部１０１、通信インタフェース（Ｉ／Ｆ）部１０２、記憶部１０３、入力部１０４、及び出力部１０５を含み、各部はバスライン１０６を介して相互に通信可能に接続され得る。
【００４７】
制御演算部１０１は、ＣＰＵ（Central Processing Unit）、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）等を含み、情報処理に応じて各構成要素の制御及び各種演算を行う。
【００４８】
通信Ｉ／Ｆ部１０２は、例えば、有線又は無線により他の構成要素である「部」及び「装置」と通信するための通信モジュールである。通信Ｉ／Ｆ部１０２が通信に用いる通信方式は任意であり、例えば、ＬＡＮ（Local Area Network）やＵＳＢ（Universal Serial Bus）等が挙げられ、バスライン１０６と同等の適宜の通信線を適用することもできる。
【００４９】
駆動装置２及び撮影装置２０は、通信Ｉ／Ｆ部１０２を介して、制御演算部１０１等と通信可能に設けることが可能である。
【００５０】
記憶部１０３は、例えばハード・ディスク・ドライブ（ＨＤＤ）、ソリッド・ステート・ドライブ（ＳＳＤ）等の補助記憶装置であり、制御演算部１０１で実行される各種プログラム（各種処理を実行するための演算プログラム、並びに、駆動装置２及び撮影装置２０の動作の制御処理を行うための制御プログラム等）、校正条件、測定条件、画像処理条件（対象物の認識パラメータ等）を含むデータベース、撮影装置２０から出力される撮影画像（測定データ）、画像処理結果のデータ、対象物の３次元モデルデータ等を記憶する。このとおり、記憶部１０３に記憶された演算プログラム及び制御プログラムが制御演算部１０１で実行されることにより、後述する機能構成例における各種処理機能が実現される。
【００５１】
入力部１０４は、駆動装置２、撮影装置２０、及び制御装置１００を利用するユーザからの各種入力操作を受け付けるためのインタフェースデバイスであり、例えば、マウス、キーボード、タッチパネル、音声マイク等で実現し得る。出力部１０５は、駆動装置２、撮影装置２０、及び制御装置１００を利用するユーザ等へ、各種情報を、その表示、音声出力、印刷出力等により報知するためのインタフェースデバイスであり、例えば、ディスプレイ、スピーカ、プリンタ等で実現し得る。
【００５２】
［機能構成］
図４は、実施形態に従う制御装置１００の機能構成を説明する図である。
【００５３】
図４を参照して、制御装置１００の制御演算部１０１は、記憶部１０３に記憶された各種プログラム（制御プログラム及び演算プログラム等）をＲＡＭに展開し、それらの各種プログラムをＣＰＵにより解釈及び実行して、各構成要素を制御する。これにより、図４に例示の如く、本実施形態に係る制御装置１００は、設定部３０と、軌跡設定部３５と、サーボ指令制御部４０と、トリガ指令制御部５０とを備える構成を実現し得る。
【００５４】
設定部３０は、通過順序が予め設定された複数の通過点の位置を設定する。
【００５５】
軌跡設定部３５は、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を設定する。
【００５６】
サーボ指令制御部４０は、設定された指令軌跡に基づいて駆動装置２の動作を制御する指令信号を出力する。
【００５７】
トリガ指令制御部５０は、複数の通過点の位置に従って撮影装置２０に対して撮影指令信号を出力する。
【００５８】
なお、本実施形態では、制御装置１００で実現される各機能が汎用のＣＰＵによって実現される例について説明したが、以上の機能の一部又は全部が、１又は複数の専用のプロセッサ又は専用回路（例えば、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field-Programmable Gate Array）等）によって実現されてもよい。さらに、一部の処理をネットワーク接続された外部装置に担当させてもよい。また、制御装置１００の機能構成は、実施形態や構成例に応じて、適宜、機能の省略、置換、及び追加が行われてももちろんよい。また、「制御装置」とは、一般的な情報処理装置（例えば、コンピュータ、ワークステーション等）と解することができる。
【００５９】
［動作例］
本開示の一実施形態に係る制御装置１００の一例で実現される制御方法について説明する。
【００６０】
指令軌跡の算出方式について説明する。
【００６１】
一例として、順序が予め設定された複数の通過点を連続的に通過する指令軌跡の算出について説明する。
【００６２】
一例として、指定された通過点（P1、P2…Pn）の（P1−P2、P2−P3、・・・Pn-1−Pn）の点間の軌跡をｘ座標、ｙ座標の３次多項式で定義すると下記式（１）で表される。
【００６３】
【数１】
0006801802.tif 000002
【００６４】
そして、次の２つの条件を満たす３次の曲線式としてエルミート曲線で定義する。
【００６５】
具体的には、設定された通過点の座標を通ることと、各通過点の境界および曲線途中で軌跡が連続であることである。
【００６６】
上記条件に基づくエルミート曲線は、下記式（２）で表される。
【００６７】
【数２】
0006801802.tif 000003
【００６８】
エルミート曲線は、始点位置、始点速度ベクトル、終点位置、終点速度ベクトルの４つのパラメータに基づいて設定される。
【００６９】
したがって、t=0で始点位置、t=1で終点位置となるため、通過点の座標を始点位置、終点位置のパラメータとして設定する。
【００７０】
【数３】
0006801802.tif 000004
【００７１】
エルミート曲線を連続的に繋ぎ合わせる手法として、Catmull-Rom Spline（キャットムル・ロム曲線）を用いる。具体的には、本例においては、各通過点での位置および速度の連続性を保つために、速度ベクトルを通過点の前後で一致するように設定する。
【００７２】
図５は、実施形態に従う速度ベクトルの設定について説明する図である。
【００７３】
図５に示されるように、具体的には、通過点P2において、指令軌跡Q1の曲線式の終点速度ベクトルと、指令軌跡Q2の曲線式の始点速度ベクトルに同じベクトルを設定する。
【００７４】
これにより、順序が予め設定された複数の通過点を連続的に通過する指令軌跡を設定することが可能となる。
【００７５】
具体的には、各通過点間毎の区間多項式である３次のエルミート曲線が設定される。
【００７６】
図６は、実施形態に従う軌跡設定部３５とサーボ指令制御部４０との関係について説明する図である。
【００７７】
図６に示されるように、軌跡設定部３５は、通過点座標と、通過順序および速度ベクトルを入力することにより各通過点毎の３次のエルミート曲線を設定する。
【００７８】
軌跡設定部３５は、算出結果として３次のエルミート曲線の指令軌跡データをサーボ指令制御部４０に出力する。
【００７９】
サーボ指令制御部４０は、設定された３次のエルミート曲線の指令軌跡データに基づいて駆動装置２の動作を制御する指令信号を出力する。
【００８０】
具体的には、サーボ指令制御部４０は、駆動装置２の動作を制御する位置座標X,Yを出力する。
【００８１】
図７は、実施形態に従うサーボ指令制御部４０が出力する位置座標を説明する図である。
【００８２】
図７に示されるように、サーボ指令制御部４０は、軌跡設定部３５で設定された指令軌跡データに従って駆動装置２の動作位置を出力する。
【００８３】
具体的には、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の駆動装置２の目標指令値を算出する。
【００８４】
これによりサーボ指令制御部４０は、設定された指令軌跡に沿って駆動装置２を動作させることが可能となる。
【００８５】
図８は、比較例として指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。
【００８６】
図８（Ａ）には、各通過点間における軌跡移動速度が示されている。
【００８７】
図８（Ｂ）には、各指令軌跡のパラメータｔの変化が示されている。
【００８８】
当該図に示されるように、パラメータｔが一定の割合で０〜１に変化する場合には、一例として通過点間の距離が短い場合には、基準速度V0よりも軌跡移動速度は遅くなる場合が示されている。一方で、通過点間の距離が長い場合には、軌跡移動速度は、基準速度V0よりも軌跡移動速度は速くなる場合が示されている。
【００８９】
すなわち、パラメータｔを一定の割合で調整した場合には、駆動装置２の速度を一定に調整することはできない。
【００９０】
図９は、実施形態に従う指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。
【００９１】
図９（Ａ）には、各通過点間における軌跡移動速度が示されている。
【００９２】
図９（Ｂ）には、各指令軌跡のパラメータｔの変化が示されている。
【００９３】
当該図に示されるように、各通過点間における軌跡移動速度を基準速度V0で一定に制御するためには、各通過点間のパラメータｔの変動率を調整する必要がある。
【００９４】
実施形態においては、各通過点間における軌跡移動速度を調整する。
【００９５】
再び図７を参照してサーボ指令制御部４０は、距離算出部４２と、変数調整部４４とを含む。
【００９６】
距離算出部４２は、制御周期毎の指令制御速度に対する目標移動距離を算出する。
【００９７】
変数調整部４４は、距離算出部４２で算出された目標移動距離が一定となるように制御周期毎のパラメータｔの増分値を調整する。
【００９８】
＜Ｃ．設定方式＞
実施形態に従うパラメータｔの設定方式は、パラメータｔに所定の基準変動量を与える試算処理を実行する。
【００９９】
本例においては、一例として２回あるいは３回試算処理した場合に、制御周期毎の目標移動距離の近傍に試算点が到達するような基準変動量を設定する。
【０１００】
図１０は、実施形態に従う１回目の試算処理を説明する図である。
【０１０１】
図１０を参照して、本例においては、通過点P0(X0,Y0)と、通過点P1(X1,Y1)との間の指令軌跡Q0の曲線式に対してパラメータｔの変動量を設定する場合の処理について説明する。
【０１０２】
一例として、現在位置を通過点P0(X0,Y0)とした場合に、次の制御周期で移動する目標位置Pm(Xm,Ym)のパラメータｔの変動量Tmを算出する。
【０１０３】
［準備処理］
通過点P0(X0,Y0)までの積算移動距離をLsとする。
【０１０４】
制御周期毎の次の目標位置Pm(Xm,Ym)までの積算移動距離はLmとする。
【０１０５】
目標移動距離ΔLmは、Lm-Lsである。
【０１０６】
本例においては、パラメータｔを２回変化させて試算した場合に積算移動距離がLm近傍になる試算用の基準変動量ΔTをまず設定する。
【０１０７】
通過点P0(X0,Y0)と、通過点P1(X1,Y1)との間の直線距離ΔLを算出する。
【０１０８】
パラメータｔが０〜１まで変化することにより直線距離ΔL移動すると仮定すると、制御周期毎のパラメータｔの平均変化量は、ΔL/ΔLmである。
【０１０９】
したがって、パラメータｔをｋ回変化させて試算した場合に積算移動距離がLm近傍になる試算用の基準変動量ΔTは、ΔL/ΔLm/kにより設定する。
【０１１０】
本例においては、分割係数k=2に設定する。
【０１１１】
パラメータｔを１回変化させて試算した場合に積算移動距離がLm近傍になる試算用の基準変動量ΔTは、ΔL/ΔLmを設定する。
【０１１２】
パラメータｔを２回変化させて積算移動距離がLm近傍になる試算用の基準変動量ΔTは、ΔL/ΔLm/2により設定する。
【０１１３】
パラメータｔを３回変化させて積算移動距離がLm近傍になる試算用の基準変動量ΔTは、ΔL/ΔLm/3により設定する。
【０１１４】
なお、変化させる試算回数を増加させることにより精度の高いパラメータｔの変動量を算出することが可能であるが、演算量が増加する。
【０１１５】
基準変動量を用いて試算点との距離を算出することにより、パラメータｔの変動量を算出するため効率的にパラメータｔの変動量を調整することが可能である。
【０１１６】
［１回目の試算処理］
１回目の試算処理においては第１試算点SPiを算出する。
【０１１７】
iは、試算回数を示す。本例においては、i=1である。
【０１１８】
具体的には、第１試算点SPiとして、パラメータｔの値をTiに設定する。
【０１１９】
一例として、通過点P0(X0,Y0)のパラメータｔの値Tsは、０とする。
【０１２０】
Tiは、Ts+ΔTである。
【０１２１】
指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTiを入力することにより、通過点P0(X0,Y0)から基準変動量ΔT分、移動した第１試算点SPiの位置(Xi,Yi)が算出される。
【０１２２】
通過点P0(X0,Y0)から基準変動量ΔT分、移動した場合の第１試算点SPiの位置(Xi,Yi)の距離は、次式により算出される。
【０１２３】
指令軌跡の積算移動距離は、曲線式に制御周期毎にパラメータｔを与えて、線積分を各微小区間の直線補間に近似して積算する。
【０１２４】
【数４】
0006801802.tif 000005
【０１２５】
ここで、jは積算回数である。
【０１２６】
積算移動距離Liは、Ls+ΔLiとして算出される。
【０１２７】
次に、通過点P0(X0,Y0)からの目標である積算移動距離Lmと、１回目の試算点SPiまでの試算距離Liとを比較する。
【０１２８】
比較結果に基づいて、Lm＞Liである場合には、試算処理を継続する。すなわち、２回目の試算処理を実行する。
【０１２９】
［基準変動量の補正］
２回目の試算処理を実行する前にパラメータｔの基準変動量ΔTを補正する。
【０１３０】
図１１は、実施形態に従うパラメータｔの基準変動量の補正について説明する図である。
【０１３１】
図１１（Ａ）を参照して、パラメータｔと、積算移動距離Lとの関係が示されている。
【０１３２】
パラメータｔは、０〜１へと変化する。
【０１３３】
パラメータｔが０の位置の場合の通過点P0(X0,Y0)の積算移動距離は、Lsである。
【０１３４】
Tiは、TsにΔTを加算した値である。
【０１３５】
Liは、LsにΔLiを加算した値である。
【０１３６】
しかしながら、ΔLiは、試算点と通過点P0(X0,Y0)との間の直線距離であるため実際の曲線軌跡を移動した距離とは異なる。
【０１３７】
理想的には、１回目の試算処理の試算点における積算移動距離の理想値Ltは、目標とする積算移動距離Lmと、Lsとの間の中間値である。
【０１３８】
したがって、１回目の試算処理の試算点における積算移動距離が理想値Ltとなるように基準変動量ΔTを補正する。
【０１３９】
具体的には、距離の誤差を算出して、誤差分に応じた量だけ基準変動量ΔTを補正する。
【０１４０】
図１１（Ｂ）および（Ｃ）を参照して、補正後の基準変動量ΔTadj1は、次式により算出される。また、調整量Δadj1は、次式により算出される。
【０１４１】
【数５】
0006801802.tif 000006
【０１４２】
そして、補正後の基準変動量ΔTadj1を用いて２回目の試算処理を実行する。
【０１４３】
［２回目の試算処理］
図１２は、実施形態に従う２回目の試算処理を説明する図である。
【０１４４】
図１２に示されるように、２回目の試算処理においては第１試算点SPi-1と、第２試算点SPiとを算出する。
【０１４５】
iは、試算回数を示す。本例においては、i=2である。
【０１４６】
具体的には、第１試算点SPi-1として、パラメータｔの値をTi-1に設定する。
【０１４７】
Ti-1は、Ts+ΔTadj1である。
【０１４８】
指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTi-1を入力することにより、通過点P0(X0,Y0)から基準変動量ΔTadj1分、移動した第１試算点SPi-1の位置(Xi-1,Yi-1)が算出される。
【０１４９】
通過点P0(X0,Y0)と、基準変動量ΔTadj1分、移動した場合の試算した第１試算点SPi-1の位置(Xi-1,Yi-1)との距離ΔLi-1を算出する。
【０１５０】
積算移動距離Li-1は、LsにΔLi-1を加算した値である。
【０１５１】
次に、第２試算点SPiを算出する。
【０１５２】
具体的には、第２試算点SPiとして、パラメータｔの値をTiに設定する。
【０１５３】
Tiは、Ti-1+ΔTadj1である。
【０１５４】
指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTiを入力することにより、第１試算点SPi-1から基準変動量ΔTadj1分、移動した第２試算点SPiの位置(Xi,Yi)が算出される。
【０１５５】
第１試算点SPi-1の位置(Xi-1,Yi-1)と、基準変動量ΔTadj1分、移動した場合の試算した第２試算点SPiの位置(Xi,Yi)との距離ΔLiを算出する。
【０１５６】
積算移動距離Liは、Li-1にΔLiを加算した値である。
【０１５７】
次に、通過点P0(X0,Y0)からの目標である積算移動距離Lmと、第２試算点SPiまでの試算距離Liとを比較する。
【０１５８】
比較結果に基づいて、Lm≦Liである場合には、積算移動距離Lmに対応するTmを算出する。
【０１５９】
パラメータｔの変動量Tmは、次式により算出される。
【０１６０】
【数６】
0006801802.tif 000007
【０１６１】
そして、試算処理を終了する。
【０１６２】
すなわち、指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTmを入力して算出された目標位置Pm(Xm,Ym)が通過点P0(X0,Y0)から次の制御周期で移動する位置に設定される。
【０１６３】
本例の如く、基準変動量を補正して、補正した基準変動量を用いて試算点との距離を算出するため、精度の高いパラメータｔの変動量に調整することが可能である。
【０１６４】
一方、通過点P0(X0,Y0)からの目標である積算移動距離Lmと、第２試算点SPiまでの試算距離Liとを比較した比較結果に基づいて、Lm>Liである場合には、３回目の試算処理を実行する。
【０１６５】
［基準変動量の補正］
３回目の試算処理を実行する前にパラメータｔの基準変動量ΔTを補正する。
【０１６６】
具体的には、上記と同様の方式に従って基準変動量を補正する。
【０１６７】
理想的には、２回目の試算処理の第２試算点SPiにおける積算移動距離の理想値Ltは、目標とする積算移動距離Lmである。
【０１６８】
したがって、２回目の試算処理の第２試算点SPiにおける積算移動距離が理想値Ltとなるように基準変動量ΔTを補正する。
【０１６９】
具体的には、距離の誤差を算出して、誤差分に応じた量だけ基準変動量ΔTを補正する。
【０１７０】
補正後の基準変動量ΔTadj2は、次式により算出される。また、調整量Δadj2は、次式により算出される。また、調整量Δadj2は、次式により算出される。
【０１７１】
【数７】
0006801802.tif 000008
【０１７２】
そして、補正後の基準変動量ΔTadj2を用いて３回目の試算処理を実行する。
【０１７３】
［３回目の試算処理］
図１３は、実施形態に従う３回目の試算処理を説明する図である。
【０１７４】
図１３に示されるように、３回目の試算処理においては第１試算点SPi-2と、第２試算点SPi-1と、第３試算点SPiとを算出する。
【０１７５】
iは、試算回数を示す。本例においては、i=3である。
【０１７６】
具体的には、第１試算点SPi-2として、パラメータｔの値をTi-2に設定する。
【０１７７】
Ti-2は、Ts+ΔTadj2である。
【０１７８】
指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTi-2を入力することにより、通過点P0(X0,Y0)から基準変動量ΔTadj2分、移動した第１試算点SPi-2の位置(Xi-2,Yi-2)が算出される。
【０１７９】
通過点P0(X0,Y0)と、基準変動量ΔTadj2分、移動した場合の試算した第１試算点SPi-2の位置(Xi-2,Yi-2)との距離ΔLi-2を算出する。
【０１８０】
積算移動距離Li-2は、LsにΔLi-2を加算した値である。
【０１８１】
次に、第２試算点SPi-1を算出する。
【０１８２】
具体的には、第２試算点SPi-1として、パラメータｔの値をTi-1に設定する。
【０１８３】
Ti-1は、Ti-2+ΔTadj2である。
【０１８４】
指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTi-1を入力することにより、第１試算点SPi-2から基準変動量ΔTadj2分、移動した第２試算点SPi-1の位置(Xi-1,Yi-1)が算出される。
【０１８５】
第２試算点SPi-1の位置(Xi-1,Yi-1)と、基準変動量ΔTadj2分、移動した場合の試算した第２試算点SPi-1の位置(Xi-1,Yi-1)との距離ΔLi-1を算出する。
【０１８６】
積算移動距離Li-1は、Li-2にΔLi-1を加算した値である。
【０１８７】
次に、第３試算点SPiを算出する。
【０１８８】
具体的には、第３試算点SPiとして、パラメータｔの値をTiに設定する。
【０１８９】
Tiは、Ti-1+ΔTadj2である。
【０１９０】
指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTiを入力することにより、第２試算点SPi-1から基準変動量ΔTadj2分、移動した第３試算点SPiの位置(Xi,Yi)が算出される。
【０１９１】
第２試算点SPi-1の位置(Xi-1,Yi-1)と、基準変動量ΔTadj2分、移動した場合の試算した第３試算点SPiの位置(Xi,Yi)との距離ΔLiを算出する。
【０１９２】
積算移動距離Liは、Li-1にΔLiを加算した値である。
【０１９３】
次に、通過点P0(X0,Y0)からの目標である積算移動距離Lmと、第３試算点SPiまでの試算距離Liとを比較する。
【０１９４】
比較結果に基づいて、Lm≦Liである場合には、積算移動距離Lmに対応するTmを算出する。
【０１９５】
パラメータｔの変動量Tmは、次式により算出される。
【０１９６】
【数８】
0006801802.tif 000009
【０１９７】
そして、試算処理を終了する。
【０１９８】
すなわち、指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTmを入力して算出された目標位置Pm(Xm,Ym)が通過点P0(X0,Y0)から次の制御周期で移動する位置に設定される。
【０１９９】
そして、設定した位置を基点として、上記で説明した方式を用いて次の制御周期で移動する位置を算出する。
【０２００】
これにより、変数調整部４４は、距離算出部４２で算出された目標移動距離が一定となるように制御周期毎のパラメータｔの増分値を調整することが可能となる。通過点P0(X0,Y0)から通過点P1(X1,Y1)への指令軌跡Q0に沿って制御周期毎に移動する位置を設定する。
【０２０１】
一方で、通過点P1(X1,Y1)付近になった場合に、試算処理において、パラメータｔの値であるTiが１を超える場合がある。すなわち、通過点P1(X1,Y1)を跨ぐ場合である。
【０２０２】
パラメータｔの値であるTiが１を超える場合には、対象とする指令軌跡を変更する。通過点P1(X1,Y1)から通過点P2(X2,Y2)への指令軌跡Q1に対応する区分多項式に変更する。すなわち、通過点P1(X1,Y1)と通過点P2(X2,Y2)との間の区分多項式に変更する。
【０２０３】
そして、Ti-1をTmとして指令軌跡Q1に対応する区分多項式のパラメータｔに当該Tmを入力して算出された目標位置Pm(Xm,Ym)が次の制御周期で移動する位置に設定される。
【０２０４】
当該処理により、各通過点間のパラメータｔの変動率を調整することが可能である。
【０２０５】
これにより、各通過点間における軌跡移動速度を基準速度V0で一定に制御することが可能である。すなわち、制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。
【０２０６】
＜Ｄ．シミュレーション例＞
次に、実施形態に従う制御システム１のシミュレーション例について説明する。
【０２０７】
図１４は、実施形態に従う１４個の通過点を設定した場合のシミュレーション例について説明する図である。
【０２０８】
図１４に示されるように、１４個の通過点が示されており、数字は、通過順序を示している。
【０２０９】
図１５は、実施形態に従う通過座標および３次多項式の係数について説明する図である。
【０２１０】
図１５を参照して、本例においては、最初の始点座標および最後の終点座標は同じ（０，０）に設定されている。また、１４個のそれぞれの通過点の座標が設定されている。
【０２１１】
また、所定値Rは５０に設定している。
【０２１２】
上記で説明した方式に基づいて各区間のｘ座標の３次多項式の係数およびｙ座標の３次多項式の係数がそれぞれ算出される。
【０２１３】
図１６は、実施形態に従うトレース軌跡を説明する図である。
【０２１４】
図１６に示されるように、設定された順序に従って１４個の通過点を連続的にかつ滑らかに通過する指令軌跡が描かれている。
【０２１５】
図１７は、実施形態に従う指令速度の波形を説明する図である。
【０２１６】
図１７に示されるように、本例においては、指令速度として、500mm/sに設定した場合が示されている。
【０２１７】
なお、加速度および減速度は、1000mm/s2とした。また、ジャークは、10000mm/s3に設定した。
【０２１８】
当該図に示されるように、各通過点間における軌跡移動速度を基準速度V0で一定に制御することが可能であることを確認することが可能である。
【０２１９】
なお、本例においては、一例として平面上における通過順序が予め設定された複数の通過点を連続的に通過する各通過点間の指令軌跡として、区分多項式である３次のエルミート曲線で設定する方式について説明した。一方で、２次元の平面上の通過点に限られず３次元の空間上の通過点についても同様に適用可能である。すなわち、ｚ座標についてもエルミート曲線で定義し、３次元の空間上の通過点に対して通過点の速度ベクトルを３次元座標のベクトルとして処理すればよい。
【０２２０】
また、本例においては、各通過点間の指令軌跡の区分多項式として一例として、３次のエルミート曲線で設定する場合について説明したが、特にこれに限られず他の曲線等を用いることも可能である。すなわち、各通過点間の指令軌跡を１つの共通のパラメータｔで設定するスプライン関数であればどのようなものを採用しても良い。次数も３次に限られず、さらに高次でも良いし、２次あるいは１次の直線としてもよい。
【０２２１】
指令軌跡が直線の場合には、１次のスプライン関数として定義することが可能である。
【０２２２】
この場合であっても距離算出部４２で算出された目標移動距離が一定となるように各通過点間の制御周期毎のパラメータｔの増分値を調整することにより、軌跡移動速度を一定に制御することが可能である。
【０２２３】
＜Ｄ．付記＞
上述したような本実施の形態は、以下のような技術思想を含む。
【０２２４】
［構成１］
コントローラシステム（１００）は、通過順序が予め設定された複数の通過点の位置を設定する設定部（３０）と、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を区分多項式で設定する軌跡設定部（３５）と、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出する指令値算出部（４０）とを備える。区分多項式は、第１変数に従う３次のエルミート曲線で定義される。指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部（４２）と、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する変数調整部（４４）とを含む。
【０２２５】
［構成２］
第１変数は、０〜１の間で変化する。
【０２２６】
［構成３］
区分多項式は、３次のエルミート曲線である。
【０２２７】
［構成４］
変数調整部は、算出された目標移動距離に基づいて第１変数の基準となる基準変動量ΔTを設定し、基準変動量に基づいて区分多項式に基づく制御対象の座標を少なくとも１回算出し、前回の制御周期における制御対象の座標と、基準変動量に基づく算出された制御対象の座標との間の距離を算出し、算出された基準変動量に対する距離と目標移動距離との比較に基づいて、制御周期毎の第１変数の変動量を調整する。
【０２２８】
［構成５］
変数調整部は、基準変動量毎の区分多項式に基づく制御対象の座標を複数回算出し、基準変動量毎の制御対象の座標間のそれぞれの距離を算出し、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とに基づいて第１変数の基準となる基準変動量を補正する。
【０２２９】
［構成６］
変数調整部は、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とを比較し、基準変動量毎の制御対象の座標間の距離の合計が目標移動距離以上の場合に目標移動距離に対する第１変数の変動量を算出する。
【０２３０】
［構成７］
変数調整部は、対象とする指令軌跡の通過点間の距離と、目標移動距離と、分割係数ｋとに基づいて基準変動量を設定する。
【０２３１】
［構成８］
コントローラシステムの制御方法は、通過順序が予め設定された複数の通過点の位置を設定するステップと、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を区分多項式で設定するステップと、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出するステップとを備える。区分多項式は、第１変数に従う３次のエルミート曲線で定義され、目標指令値を算出するステップは、制御周期毎の指令制御速度に対する目標移動距離を算出するステップと、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整するステップとを含む。
【０２３２】
＜Ｅ．利点＞
本実施の形態に従うコントローラシステムおよびその制御方法によれば、制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能である。
【０２３３】
今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した説明ではなく、特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
【符号の説明】
【０２３４】
１  レビュー制御システム、２  駆動装置、２０  撮影装置、３０  設定部、３５  軌跡設定部、４０  サーボ指令制御部、４２  距離算出部、４４  変数調整部、５０  トリガ指令制御部、１００  制御装置、１０１  制御演算部、１０２  通信Ｉ／Ｆ部、１０３  記憶部、１０４  入力部、１０５  出力部、１０６  バスライン。
【要約】
【課題】制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能なコントローラシステムを提供する。
【解決手段】コントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を第１変数に従う区分多項式で設定する軌跡設定部と、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出する指令値算出部とを備える。指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部と、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する変数調整部とを含む。
【選択図】図１【図１】
0006801802.tif 000010
【図２】
0006801802.tif 000011
【図３】
0006801802.tif 000012
【図４】
0006801802.tif 000013
【図５】
0006801802.tif 000014
【図６】
0006801802.tif 000015
【図７】
0006801802.tif 000016
【図８】
0006801802.tif 000017
【図９】
0006801802.tif 000018
【図１０】
0006801802.tif 000019
【図１１】
0006801802.tif 000020
【図１２】
0006801802.tif 000021
【図１３】
0006801802.tif 000022
【図１４】
0006801802.tif 000023
【図１５】
0006801802.tif 000024
【図１６】
0006801802.tif 000025
【図１７】
0006801802.tif 000026