(57)【特許請求の範囲】
【請求項１】
通過順序が予め設定された複数の通過点の位置を設定する設定部と、
制御対象が前記複数の通過点を連続的に通過する各前記通過点間の指令曲線軌跡を設定する軌跡設定部とを備え、
前記軌跡設定部は、
（i−１）番目（i≧２）の通過点とi番目の通過点との間の（i−１）番目の曲線軌跡は、前記（i−１）番目の通過点の位置と、前記（i−１）番目の通過点の速度ベクトルと、前記i番目の通過点の位置と、前記i番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義され、
前記i番目の通過点と（i＋１）番目の通過点との間のi番目の曲線軌跡は、前記i番目の通過点の位置と、前記i番目の通過点の速度ベクトルと、前記（i＋１）番目の通過点の位置と、前記（i＋１）番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義され、
前記i番目の通過点の速度ベクトルは、前記i番目の通過点を基点とした場合の前記（i−１）番目の通過点への第１方向ベクトルと、前記i番目の通過点を基点とした場合の前記（i＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定され、
前記軌跡設定部は、
前記第１方向ベクトルと前記第２方向ベクトルとで成す角度が所定角度以上である場合には、前記i番目の通過点の速度ベクトルの方向は、前記（i−１）番目の通過点から前記（i＋１）番目の通過点への方向に設定され、
前記第１方向ベクトルと前記第２方向ベクトルとで成す角度が所定角度未満である場合には、前記i番目の通過点の速度ベクトルの方向は、前記i番目の通過点から前記（i＋１）番目の通過点への方向に直交する方向かつ、（i＋２）番目の通過点から遠ざかる方向に設定される、コントローラシステム。
【請求項２】
前記軌跡設定部は、
前記第１方向ベクトルと前記第２方向ベクトルとで成す角度が所定角度以上であり、鈍角である場合には、前記i番目の通過点の速度ベクトルの大きさは、所定値に設定され、
前記第１方向ベクトルと前記第２方向ベクトルとで成す角度が所定角度以上であり、鋭角である場合には、前記i番目の通過点の速度ベクトルの大きさは、前記所定値に前記第１方向ベクトルと前記第２方向ベクトルとで成す角度とに応じた第１係数を加算した値に設定する、請求項１記載のコントローラシステム。
【請求項３】
前記軌跡設定部は、
前記第１方向ベクトルと前記第２方向ベクトルとで成す角度が所定角度未満である場合には、前記i番目の通過点の速度ベクトルの大きさは、前記所定値に前記第１方向ベクトルと前記第２方向ベクトルとで成す角度とに応じた前記第１係数を加算した値に設定する、請求項２記載のコントローラシステム。
【請求項４】
前記曲線軌跡は、前記複数の通過点の２次元座標のそれぞれのパラメータに対する３次の第１エルミート曲線および第２エルミート曲線で定義される、請求項１記載のコントローラシステム。
【請求項５】
通過順序が予め設定された複数の通過点の位置を設定するステップと、
制御対象が前記複数の通過点を連続的に通過する各前記通過点間の指令曲線軌跡を設定するステップとを備え、
前記指令曲線軌跡を設定するステップは、
（i−１）番目（i≧２）の通過点とi番目の通過点との間の（i−１）番目の曲線軌跡は、前記（i−１）番目の通過点の位置と、前記（i−１）番目の通過点の速度ベクトルと、前記i番目の通過点の位置と、前記i番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義するステップと、
前記i番目の通過点と（i＋１）番目の通過点との間のi番目の曲線軌跡は、前記i番目の通過点の位置と、前記i番目の通過点の速度ベクトルと、前記（i＋１）番目の通過点の位置と、前記（i＋１）番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義するステップとを含み、
前記i番目の通過点の速度ベクトルは、前記i番目の通過点を基点とした場合の前記（i−１）番目の通過点への第１方向ベクトルと、前記i番目の通過点を基点とした場合の前記（i＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定され、
前記第１方向ベクトルと前記第２方向ベクトルとで成す角度が所定角度以上である場合には、前記i番目の通過点の速度ベクトルの方向は、前記（i−１）番目の通過点から前記（i＋１）番目の通過点への方向に設定され、
前記第１方向ベクトルと前記第２方向ベクトルとで成す角度が所定角度未満である場合には、前記i番目の通過点の速度ベクトルの方向は、前記i番目の通過点から前記（i＋１）番目の通過点への方向に直交する方向かつ、（i＋２）番目の通過点から遠ざかる方向に設定される、コントローラシステムの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本開示は、制御対象を制御するコントローラシステムおよびその制御方法に関する。
【背景技術】
【０００２】
各種設備および各設備に配置される各種装置の制御には、ＰＬＣ（プログラマブルロジックコントローラ）などの制御装置が用いられる。
【０００３】
例えば、特開２０００−１３７５０６号公報（特許文献１）には、ロボットの動作を制御する制御装置が指令軌跡に従う指令値を各サーボに出力してロボットの動作を制御する手法が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０００−１３７５０６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
一方で、複数の通過点を検査するロボットの動作を制御する場合、ロボットの動作を複数の通過点で停止させずに連続的に動作させることは検査効率を向上させる点で重要である。
【０００６】
本開示は、上記の課題を解決するためになされたものであって、制御対象が複数の通過点を連続的に通過する指令曲線軌跡を設定することが可能なコントローラシステムおよびその制御方法に関する。
【課題を解決するための手段】
【０００７】
ある局面に従うコントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令曲線軌跡を設定する軌跡設定部とを備える。軌跡設定部は、（i−１）番目（i≧２）の通過点とi番目の通過点との間の（i−１）番目の曲線軌跡は、（i−１）番目の通過点の位置と、（i−１）番目の通過点の速度ベクトルと、i番目の通過点の位置と、i番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義される。i番目の通過点と（i＋１）番目の通過点との間のi番目の曲線軌跡は、i番目の通過点の位置と、i番目の通過点の速度ベクトルと、（i＋１）番目の通過点の位置と、（i＋１）番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義される。i番目の通過点の速度ベクトルは、i番目の通過点を基点とした場合の（i−１）番目の通過点への第１方向ベクトルと、i番目の通過点を基点とした場合の（i＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定される。
【０００８】
この構成によれば（i−１）番目の曲線軌跡と、i番目の曲線軌跡とは、同一のｉ番目の通過点の速度ベクトルは同一に設定され、当該速度ベクトルは、i番目の通過点を基点とした場合の（i−１）番目の通過点への第１方向ベクトルと、i番目の通過点を基点とした場合の（i＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定される。これにより（i−１）番目の曲線軌跡とi番目の曲線軌跡との連続性が維持され、ｉ番目の通過角度に基づいて設定されるため滑らかな指令曲線軌跡が設定される。
【０００９】
好ましくは、軌跡設定部は、第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度以上である場合には、i番目の通過点の速度ベクトルの方向は、（i−１）番目の通過点から（i＋１）番目の通過点への方向に設定される。第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度未満である場合には、i番目の通過点の速度ベクトルの方向は、i番目の通過点から（i＋１）番目の通過点への方向に直交する方向かつ、（i＋２）番目の通過点から遠ざかる方向に設定される。
【００１０】
この構成によれば、ｉ番目の通過点の速度ベクトルは、ｉ番目の通過角度が所定角度以上である場合には、i番目の通過点の速度ベクトルの方向は、（i−１）番目の通過点から（i＋１）番目の通過点への方向に設定される。所定角度未満である場合には、i番目の通過点の速度ベクトルの方向は、i番目の通過点から（i＋１）番目の通過点への方向に直交する方向かつ、（i＋２）番目の通過点から遠ざかる方向に設定される。これによりｉ番目の通過角度が所定角度未満すなわち折り返すような条件の場合であっても滑らかな指令曲線軌跡が設定される。
【００１１】
好ましくは、軌跡設定部は、第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度以上であり、鈍角である場合には、i番目の通過点の速度ベクトルの大きさは、所定値に設定される。第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度以上であり、鋭角である場合には、i番目の通過点の速度ベクトルの大きさは、所定値に第１方向ベクトルと第２方向ベクトルとで成す角度とに応じた第１係数を加算した値に設定する。
【００１２】
この構成によれば、ｉ番目の通過点の速度ベクトルは、ｉ番目の通過角度が所定角度以上である場合には、i番目の通過点の速度ベクトルの方向は、（i−１）番目の通過点から（i＋１）番目の通過点への方向に設定される。所定角度未満である場合には、i番目の通過点の速度ベクトルの方向は、i番目の通過点から（i＋１）番目の通過点への方向に直交する方向かつ、（i＋２）番目の通過点から遠ざかる方向に設定される。これによりｉ番目の通過角度が所定角度未満すなわち折り返すような条件の場合であっても滑らかな指令曲線軌跡が設定される。
【００１３】
好ましくは、軌跡設定部は、第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度未満である場合には、i番目の通過点の速度ベクトルの大きさは、所定値に第１方向ベクトルと第２方向ベクトルとで成す角度とに応じた第１係数を加算した値に設定する。
【００１４】
この構成によれば、ｉ番目の通過点の速度ベクトルは、ｉ番目の通過角度が所定角度未満である場合には、i番目の通過点の速度ベクトルの大きさは、所定値に通過角度に応じた第１係数を加算した値に設定される。ｉ番目の通過角度が所定角度未満すなわち折り返すような条件の場合であっても速度ベクトルの大きさを適切に設定することにより滑らかな指令曲線軌跡が設定される。
【００１５】
好ましくは、曲線軌跡は、複数の通過点の２次元座標のそれぞれのパラメータに対する３次の第１エルミート曲線および第２エルミート曲線で定義される。
【００１６】
この構成によればＸ軸およびＹ軸のそれぞれにおいてエルミート曲線が定義されることにより任意の通過座標に対して連続的で滑らかな指令曲線軌跡を設定することが可能である。
【００１７】
ある局面に従うコントローラシステムの制御方法は、通過順序が予め設定された複数の通過点の位置を設定するステップと、制御対象が複数の通過点を連続的に通過する各通過点間の指令曲線軌跡を設定するステップとを備える。指令曲線軌跡を設定するステップは、（i−１）番目（i≧２）の通過点とi番目の通過点との間の（i−１）番目の曲線軌跡は、（i−１）番目の通過点の位置と、（i−１）番目の通過点の速度ベクトルと、i番目の通過点の位置と、i番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義するステップと、i番目の通過点と（i＋１）番目の通過点との間のi番目の曲線軌跡は、i番目の通過点の位置と、i番目の通過点の速度ベクトルと、（i＋１）番目の通過点の位置と、（i＋１）番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義するステップとを含む。i番目の通過点の速度ベクトルは、i番目の通過点を基点とした場合の（i−１）番目の通過点への第１方向ベクトルと、i番目の通過点を基点とした場合の（i＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定される。
【００１８】
この構成によれば（i−１）番目の曲線軌跡と、i番目の曲線軌跡とは、同一のｉ番目の通過点の速度ベクトルは同一に設定され、当該速度ベクトルは、i番目の通過点を基点とした場合の（i−１）番目の通過点への第１方向ベクトルと、i番目の通過点を基点とした場合の（i＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定される。これにより（i−１）番目の曲線軌跡とi番目の曲線軌跡との連続性が維持され、ｉ番目の通過角度に基づいて設定されるため滑らかな指令曲線軌跡が設定される。
【００１９】
なお、本開示において、「部」及び「装置」とは、単に物理的手段を意味するものではなく、その「部」及び「装置」が有する機能をソフトウェアによって実現する構成も含む。また、１つの「部」及び「装置」が有する機能が２つ以上の物理的手段や装置によって実現されてもよく、或いは、２つ以上の「部」及び「装置」の機能が１つの物理的手段や装置によって実現されてもよい。さらに、「部」及び「装置」とは、例えば「手段」及び「システム」と言い換えることも可能な概念である。
【発明の効果】
【００２０】
本開示のコントローラシステムおよびその制御方法によれば、制御対象が複数の通過点を連続的に通過する指令曲線軌跡を設定することが可能である。
【図面の簡単な説明】
【００２１】
【図１】実施形態に従うレビュー制御システム１のアプリケーション例を示す模式図である。
【図２】実施形態に従う駆動装置２の具体的構成を説明する図である。
【図３】実施形態に従う制御装置１００のハードウェアの構成の一例を模式的に示す平面図である。
【図４】実施形態に従う制御装置１００の機能構成を説明する図である。
【図５】実施形態に従う速度ベクトルの設定（その１）について説明する図である。
【図６】実施形態に従う速度ベクトルの設定（その２）について説明する図である。
【図７】実施形態に従う速度ベクトルの設定（その３）について説明する図である。
【図８】実施形態に従う制御システムの一例における処理手順を示すフローチャートである。
【図９】実施形態に従う通過点間の指令曲線軌跡の設定処理について説明するフロー図である。
【図１０】実施形態に従う１４個の通過点を設定した場合のシミュレーション例について説明する図である。
【図１１】実施形態に従う通過座標および３次多項式の係数について説明する図である。
【図１２】実施形態に従うトレース軌跡を説明する図である。
【発明を実施するための形態】
【００２２】
本発明の実施の形態について、図面を参照しながら詳細に説明する。なお、図中の同一または相当部分については、同一符号を付してその説明は繰り返さない。
【００２３】
以下では図１のような２軸（Ｘ軸、Ｙ軸）構成の同期グループ（「複数のサーボモータ」及びそれらに対応する「複数のサーボドライバ」を含む複数のサーボ制御デバイスに相当する。）を前提に説明するが、本実施形態の同期グループは、２軸以上のサーボ制御デバイスと１以上のトリガ制御デバイスを含む構成であればよい。以下では対象物が図１のような２軸（Ｘ軸、Ｙ軸）で移動するが、対象物の移動形態は、３軸以上であってもよい。
【００２４】
＜Ａ．適用例＞
まず、本発明が適用される場面の一例について説明する。
【００２５】
図１は、実施形態に従うレビュー制御システム１のアプリケーション例を示す模式図である。
【００２６】
図１には、フラットパネルディスプレイＦＰＤを検査するレビュー制御システム１が示されている。
【００２７】
レビュー制御システム１には、駆動装置２と、撮影装置２０とが設けられている。
【００２８】
駆動装置２は、各軸方向の任意の位置に移動可能に設けられている。なお、本例においては、Ｘ、Ｙ軸に移動する場合について説明する。
【００２９】
駆動装置２の先端には、撮影装置２０が設けられており、駆動装置２を駆動することにより予め検出したフラットパネルディスプレイＦＰＤの欠陥部分を撮影装置２０で再検査する。具体的には、当該位置で撮影装置２０で撮像する。
【００３０】
図２は、実施形態に従う駆動装置２の具体的構成を説明する図である。
【００３１】
図２に示されるように、駆動装置２は、３軸を構成する複数のサーボモータと、それぞれのサーボモータに対応するサーボドライバを含むサーボ制御デバイスとにより、撮像検査の対象物（フラットパネルディスプレイＦＰＤの欠陥部分）の位置に移動するように駆動し、当該位置の対象物（フラットパネルディスプレイＦＰＤの欠陥部分）をトリガ制御デバイスとしての撮影装置２０で撮影する。
【００３２】
再び図１を参照して、本例においては、フラットパネルディスプレイＦＰＤに４つの欠陥部分（通過点）が予め示されている。そして、欠陥部分（通過点）の通過順序が予め設定されているものとする。
【００３３】
レビュー制御システム１は、複数の欠陥部分（通過点）を通過する滑らかな指令曲線を設定する。撮影装置２０は、欠陥部分（通過点）を通過する際に当該位置を撮像する。
【００３４】
レビュー制御システム１は、設定された指令曲線に基づいて駆動装置２を制御することにより、止まらずに欠陥部分（通過点）を撮像することが可能となる。
【００３５】
これにより、フラットパネルディスプレイの検査速度の向上を図ることが可能である。
【００３６】
なお、図１に示すレビュー制御システム１は任意のアプリケーションに応用可能である。
【００３７】
本実施の形態に従うレビュー制御システム１は、複数のデバイスが組み合わされたアプリケーションに対しても、１つの制御装置で制御することができる。
【００３８】
なお、フラットパネルディスプレイに限られず、各種の半導体にも同様に適用可能である。
【００３９】
＜Ｂ．構成例＞
［ハードウェア構成］
次に、本実施形態に係る制御装置１００のハードウェア構成の一例について説明する。
【００４０】
図３は、実施形態に従う制御装置１００のハードウェアの構成の一例を模式的に示す平面図である。
【００４１】
図３を参照して、制御装置１００は、図１及び図２に例示した駆動装置２及び撮影装置２０に接続され、制御演算部１０１、通信インタフェース（Ｉ／Ｆ）部１０２、記憶部１０３、入力部１０４、及び出力部１０５を含み、各部はバスライン１０６を介して相互に通信可能に接続され得る。
【００４２】
制御演算部１０１は、ＣＰＵ（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）の点間の軌跡をｘ座標、ｙ座標の３次多項式で定義すると下記式（１）で表される。
【００５８】
【数１】
0006863497.tif 000002
【００５９】
そして、次の２つの条件を満たす３次の曲線式としてエルミート曲線で定義する。
【００６０】
具体的には、設定された通過点の座標を通ることと、各通過点の境界および曲線途中で軌跡が連続であることである。
【００６１】
上記条件に基づくエルミート曲線は、下記式（２）で表される。
【００６２】
【数２】
0006863497.tif 000003
【００６３】
エルミート曲線は、始点位置、始点速度ベクトル、終点位置、終点速度ベクトルの４つのパラメータに基づいて設定される。
【００６４】
したがって、t=0で始点位置、t=1で終点位置となるため、通過点の座標を始点位置、終点位置のパラメータとして設定する。
【００６５】
【数３】
0006863497.tif 000004
【００６６】
エルミート曲線を連続的に繋ぎ合わせる手法として、Catmull-Rom Spline（キャットムル・ロム曲線）を用いる。具体的には、本例においては、各通過点での位置および速度の連続性を保つために、速度ベクトルを通過点の前後で一致するように設定する。
【００６７】
図５は、実施形態に従う速度ベクトルの設定（その１）について説明する図である。
【００６８】
図５に示されるように、具体的には、通過点P2において、指令曲線軌跡Q1の曲線式の終点速度ベクトルと、指令曲線軌跡Q2の曲線式の始点速度ベクトルに同じベクトルを設定する。
【００６９】
そして、下記の方式に基づいて速度ベクトルを設定する。
【００７０】
具体的には、ある任意の通過点とその前後の通過点とを直線で結んだときの角度（θ[deg]）によって速度ベクトルを算出する。
【００７１】
具体的には、i番目の通過点の速度ベクトルは、i番目の通過点を基点とした場合の（i−１）番目の通過点への第１方向ベクトルと、i番目の通過点を基点とした場合の（i＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定される。
【００７２】
図６は、実施形態に従う速度ベクトルの設定（その２）について説明する図である。
【００７３】
図６に示されるように、通過点P2および通過点P3の速度ベクトルの設定について説明する。
【００７４】
通過点P2の１つ前の通過点は、通過点P1、１つ後の通過点は、通過点P3である。
【００７５】
通過点P3の１つ前の通過点は、通過点P2、１つ後の通過点は、通過点P4である。
【００７６】
通過点P2を基点とした場合の通過点P1への第１方向ベクトルと、通過点P2を基点とした場合の通過点P3への第２方向ベクトルとで成す角度θ2を算出する。
【００７７】
通過点P3を基点とした場合の通過点P2への第１方向ベクトルと、通過点P3を基点とした場合の通過点P4への第２方向ベクトルとで成す角度θ3を算出する。
【００７８】
本例においては、第１方向ベクトルと、第２方向ベクトルとで成す角度θに応じて速度ベクトルを設定する。
【００７９】
具体的には、第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度以上（θ≧1）である場合について説明する。
【００８０】
第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度以上（θ≧1）である場合で、鈍角（θ≧90）の場合には、速度ベクトルの大きさを所定値Rに設定する。速度ベクトルの方向は、１つ前の通過点から１つ後の通過点へ向かう方向に設定する。
【００８１】
第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度以上（θ≧1）である場合で、鋭角（90＞θ≧1）の場合には、速度ベクトルの大きさは、R(1+cosθ)に設定する。また、速度ベクトルの方向は、１つ前の通過点から１つ後の通過点へ向かう方向に設定する。
【００８２】
本例においては、通過点P2を基点とした場合の通過点P1への第１方向ベクトルと、通過点P2を基点とした場合の通過点P3への第２方向ベクトルとで成す角度θ2は、鋭角である。
【００８３】
したがって、通過点P2の速度ベクトルの大きさは、R(1+cosθ)に設定される。また、通過点P2の速度ベクトルの方向は、通過点P1から通過点P3の方向に設定される。
【００８４】
本例においては、通過点P3を基点とした場合の通過点P2への第１方向ベクトルと、通過点P3を基点とした場合の通過点P4への第２方向ベクトルとで成す角度θ3は、鈍角である。
【００８５】
したがって、通過点P3の速度ベクトルの大きさは、Rに設定される。また、通過点P3の速度ベクトルの方向は、通過点P2から通過点P3の方向に設定される。
【００８６】
図７は、実施形態に従う速度ベクトルの設定（その３）について説明する図である。
【００８７】
図７に示されるように、通過点P2の速度ベクトルの設定について説明する。
【００８８】
通過点P2の１つ前の通過点は、通過点P1、１つ後の通過点は、通過点P3である。また、その１つ後の通過点は、通過点P4である。
【００８９】
通過点P2を基点とした場合の通過点P1への第１方向ベクトルと、通過点P2を基点とした場合の通過点P3への第２方向ベクトルとで成す角度θ2を算出する。
【００９０】
本例においては、第１方向ベクトルと、第２方向ベクトルとで成す角度θに応じて速度ベクトルを設定する。
【００９１】
具体的には、第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度未満（1>θ)である場合について説明する。すなわち、現在の通過点から次の通過点が折り返す方向となる場合である。
【００９２】
第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度未満（1>θ)である場合には、速度ベクトルの大きさをR(1+cosθ)に設定する。速度ベクトルの方向は、現在の通過点から１つ後の通過点へ向かう方向と直交し、かつ、２つ後の通過点から遠ざかる方向に設定する。
【００９３】
本例においては、通過点P2を基点とした場合の通過点P1への第１方向ベクトルと、通過点P2を基点とした場合の通過点P3への第２方向ベクトルとで成す角度θ2は、所定角度未満である。
【００９４】
したがって、通過点P2の速度ベクトルの大きさは、R(1+cosθ)に設定される。また、通過点P2の速度ベクトルの方向は、通過点P2から通過点P3の方向と直交し、かつ、通過点P4から遠ざかる方向に設定される。
【００９５】
本例においては、通過点P3を基点とした場合の通過点P2への第１方向ベクトルと、通過点P3を基点とした場合の通過点P4への第２方向ベクトルとで成す角度θ3は、鈍角である。
【００９６】
したがって、通過点P3の速度ベクトルの大きさは、Rに設定される。また、通過点P3の速度ベクトルの方向は、通過点P2から通過点P3の方向に設定される。
【００９７】
図８は、実施形態に従う制御システムの一例における処理手順を示すフローチャートである。なお、以下で説明する処理手順は一例に過ぎず、各処理は、本開示の技術思想の範囲内において可能な限り変更されてよい。また、以下で説明する処理手順は、実施形態や各構成例に応じて、適宜、ステップの省略、置換、及び追加が可能である。
【００９８】
図８を参照して、制御装置１００は、通過点を設定する（ステップＳ２）。
【００９９】
具体的には、設定部３０は、通過順序が予め設定された複数の通過点の位置を設定する。
【０１００】
次に、制御装置１００は、通過点間の指令曲線軌跡を設定する（ステップＳ４）。
【０１０１】
具体的には、軌跡設定部３５は、駆動装置２が移動する複数の通過点を連続的に通過する各通過点間の指令曲線軌跡を設定する。当該通過点間の指令曲線軌跡の設定処理の詳細については後述する。
【０１０２】
そして、処理を終了する（エンド）。
【０１０３】
図９は、実施形態に従う通過点間の指令曲線軌跡の設定処理について説明するフロー図である。
【０１０４】
図９を参照して、軌跡設定部３５は、初期パラメータを設定する（ステップＳ１０）。具体的には、初期パラメータとして、始点座標、終点座標および所定値Rを設定する。また、最初の区間の前回の通過点の速度ベクトルの方向は、始点座標から最初の通過点のベクトルの向きとし、大きさは所定値Rに設定する。
【０１０５】
次に、軌跡設定部３５は、最初の区間を算出する区間に設定する（ステップＳ１１）。
【０１０６】
一例として、始点座標から第１番目の通過点との間の区間が最初の区間に設定される。
【０１０７】
第１番目の通過点から第２番目の通過点との間の区間が２番目の区間に設定される。第２番目の通過点から第３番目の通過点との間の区間が３番目の区間に設定される。なお、第１番目の通過点を始点とした場合には、第１番目の通過点から第２番目の通過点との間の区間が最初の区間に設定される。第２番目の通過点から第３番目の通過点との間の区間が２番目の区間に設定される。第３番目の通過点から第４番目の通過点との間の区間が３番目の区間に設定される。
【０１０８】
次に、軌跡設定部３５は、算出する区間の前回の通過点の座標を座標(x0,y0)、今回の通過点の座標を座標(x1,y1)として取得する（ステップＳ１２）。
【０１０９】
具体的には、算出する区間が最初の区間の場合、前回の通過点の座標(x0,y0)は、始点座標である。また、今回の通過点の座標(x1,y1)は、第１番目の通過点P1の位置座標である。
【０１１０】
次に、軌跡設定部３５は、今回の通過点の通過角度を算出する（ステップＳ１４）。
【０１１１】
具体的には、最初の区間の場合、第１番目の通過点P1を基点とした場合の始点座標への第１方向ベクトルと、第１番目の通過点P1を基点とした場合の第２番目の通過点P2への第２方向ベクトルとの成す角度θ1を算出する。
【０１１２】
次に、軌跡設定部３５は、今回の通過点の速度ベクトルを設定する（ステップＳ１６）。具体的には、算出した角度θに基づいて速度ベクトルを設定する。
【０１１３】
図６および図７で説明したように第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度以上（θ≧1）である場合で、鈍角（θ≧90）の場合には、速度ベクトルの大きさを所定値Rに設定する。速度ベクトルの方向は、１つ前の通過点から１つ後の通過点へ向かう方向に設定する。
【０１１４】
第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度以上（θ≧1）である場合で、鋭角（90＞θ≧1）の場合には、速度ベクトルの大きさは、R(1+cosθ)に設定する。また、速度ベクトルの方向は、１つ前の通過点から１つ後の通過点へ向かう方向に設定する。
【０１１５】
第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度未満（1>θ)である場合には、速度ベクトルの大きさをR(1+cosθに設定する。速度ベクトルの方向は、現在の通過点から１つ後の通過点へ向かう方向と直交し、かつ、２つ後の通過点から遠ざかる方向に設定する。
【０１１６】
次に、軌跡設定部３５は、パラメータをエルミート曲線式に代入する（ステップＳ１８）。具体的には、前回の通過点の座標(x0,y0)、今回の通過点の座標(x1,y1)、前回の通過点の速度ベクトル(vx0,vy0)、今回の通過点の速度ベクトル(vx1,vy1)を式（２）に代入する。
【０１１７】
次に、軌跡設定部３５は、今回の通過点の曲線式の算出結果として３次多項式の係数を算出する（ステップＳ２０）。
【０１１８】
具体的には、エルミート曲線式のｘ座標の３次多項式の係数（a1,b1,c1,d1）およびｙ座標の３次多項式の係数（e1,f1,g1,h1）を算出する。
【０１１９】
次に、軌跡設定部３５は、全区間を計算したか否かを判断する（ステップＳ２２）。
【０１２０】
ステップＳ２２において、軌跡設定部３５は、全区間を計算したと判断した場合（ステップＳ２２においてＹＥＳ）には、処理を終了する（リターン）。
【０１２１】
一方、ステップＳ２２において、軌跡設定部３５は、全区間を計算していないと判断した場合（ステップＳ２２においてＮＯ）には、今回通過点の速度ベクトルを次の区間の前回通過点の速度ベクトルに設定する（ステップＳ２４）。
【０１２２】
具体的には、今回の通過点の速度ベクトル(vx1,vy1)を次の区間の通過点の前回の速度ベクトル(vx0,vy0)に設定する。
【０１２３】
次に、軌跡設定部３５は、次の区間を算出する区間に設定する（ステップＳ２６）。
【０１２４】
具体的には、算出する区間が２番目の区間の場合、前回の通過点の座標(x0,y0)は、第１番目の通過点P1の位置座標である。また、今回の通過点の座標(x1,y1)は、第２番目の通過点P2の位置座標である。
【０１２５】
そして、軌跡設定部３５は、ステップＳ１２に戻り、上記処理を繰り返す。
【０１２６】
以降、上記で説明したのと同様の処理を繰り返すことにより、各区間のエルミート曲線式の３次多項式の係数を算出することが可能となる。
【０１２７】
なお、算出する区間が最後の区間の場合、すなわち、今回の通過点の座標(x1,y1)が終点座標である場合には、次の通過点が存在しない。この場合の速度ベクトルの方向は、前回の通過点の座標から今回の通過点の座標（終点座標）へのベクトルの向きとし、大きさは所定値Rに設定する。当該方式により、複数の通過点を連続的にかつ滑らかに通過する指令曲線軌跡を設定することが可能となる。
【０１２８】
＜Ｃ．動作例＞
次に、実施形態に従う制御システム１のシミュレーション例について説明する。
【０１２９】
図１０は、実施形態に従う１４個の通過点を設定した場合のシミュレーション例について説明する図である。
【０１３０】
図１０に示されるように、１４個の通過点が示されており、数字は、通過順序を示している。
【０１３１】
図１１は、実施形態に従う通過座標および３次多項式の係数について説明する図である。
【０１３２】
図１１を参照して、本例においては、最初の始点座標および最後の終点座標は同じ（０，０）に設定されている。また、１４個のそれぞれの通過点の座標が設定されている。
【０１３３】
また、所定値Rは５０に設定している。
【０１３４】
上記で説明した方式に基づいて各区間のｘ座標の３次多項式の係数およびｙ座標の３次多項式の係数がそれぞれ算出される。
【０１３５】
図１２は、実施形態に従うトレース軌跡を説明する図である。
【０１３６】
図１２に示されるように、設定された順序に従って１４個の通過点を連続的にかつ滑らかに通過する指令曲線軌跡が描かれている。
【０１３７】
なお、本例においては、一例として平面上における通過順序が予め設定された複数の通過点を連続的に通過する各通過点間の指令曲線軌跡として、区分多項式である３次のエルミート曲線で設定する方式について説明した。一方で、２次元の平面上の通過点に限られず３次元の空間上の通過点についても同様に適用可能である。すなわち、ｚ座標についてもエルミート曲線で定義し、３次元の空間上の通過点に対して通過点の速度ベクトルを３次元座標のベクトルとして処理すればよい。上記と同様の方式に従って第１方向ベクトルと、第２方向ベクトルとで形成される平面上において、第１方向ベクトルと、第２方向ベクトルとで成す角度θに基づいて速度ベクトルの大きさを設定することが可能である。
【０１３８】
＜Ｄ．付記＞
上述したような本実施の形態は、以下のような技術思想を含む。
【０１３９】
［構成１］
コントローラシステム（１００）は、通過順序が予め設定された複数の通過点の位置を設定する設定部（３０）と、制御対象が複数の通過点を連続的に通過する各通過点間の指令曲線軌跡を設定する軌跡設定部（３５）とを備える。軌跡設定部は、（i−１）番目（i≧２）の通過点Ｐとi番目の通過点との間の（i−１）番目の曲線軌跡Ｑは、（i−１）番目の通過点の位置と、（i−１）番目の通過点の速度ベクトルと、i番目の通過点の位置と、i番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義される。i番目の通過点と（i＋１）番目の通過点との間のi番目の曲線軌跡は、i番目の通過点の位置と、i番目の通過点の速度ベクトルと、（i＋１）番目の通過点の位置と、（i＋１）番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義される。i番目の通過点の速度ベクトルは、i番目の通過点を基点とした場合の（i−１）番目の通過点への第１方向ベクトルと、i番目の通過点を基点とした場合の（i＋１）番目の通過点への第２方向ベクトルとで成す角度θに基づいて設定される。
【０１４０】
［構成２］
軌跡設定部は、第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度以上である場合（θ≧１）には、i番目の通過点の速度ベクトルの方向は、（i−１）番目の通過点から（i＋１）番目の通過点への方向に設定される。第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度未満である場合（１＞θ）には、i番目の通過点の速度ベクトルの方向は、i番目の通過点から（i＋１）番目の通過点への方向に直交する方向かつ、（i＋２）番目の通過点から遠ざかる方向に設定される。
【０１４１】
［構成３］
軌跡設定部は、第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度以上であり、鈍角である場合（θ≧９０）には、i番目の通過点の速度ベクトルの大きさは、所定値Ｒに設定される。第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度以上であり、鋭角である場合（θ＜９０）には、i番目の通過点の速度ベクトルの大きさは、所定値に第１方向ベクトルと第２方向ベクトルとで成す角度とに応じた第１係数を加算した値に設定する。
【０１４２】
［構成４］
軌跡設定部は、第１方向ベクトルと第２方向ベクトルとで成す角度が所定角度未満である場合には、i番目の通過点の速度ベクトルの大きさは、所定値に第１方向ベクトルと第２方向ベクトルとで成す角度とに応じた第１係数を加算した値に設定する。
【０１４３】
［構成５］
曲線軌跡は、複数の通過点の２次元座標のそれぞれのパラメータに対する３次の第１エルミート曲線および第２エルミート曲線で定義される。
【０１４４】
［構成６］
コントローラシステムの制御方法は、通過順序が予め設定された複数の通過点の位置を設定するステップ（Ｓ２）と、制御対象が複数の通過点を連続的に通過する各通過点間の指令曲線軌跡を設定するステップ（Ｓ４）とを備える。指令曲線軌跡を設定するステップは、（i−１）番目（i≧２）の通過点とi番目の通過点との間の（i−１）番目の曲線軌跡は、（i−１）番目の通過点の位置と、（i−１）番目の通過点の速度ベクトルと、i番目の通過点の位置と、i番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義するステップと、i番目の通過点と（i＋１）番目の通過点との間のi番目の曲線軌跡は、i番目の通過点の位置と、i番目の通過点の速度ベクトルと、（i＋１）番目の通過点の位置と、（i＋１）番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義するステップとを含む。i番目の通過点の速度ベクトルは、i番目の通過点を基点とした場合の（i−１）番目の通過点への第１方向ベクトルと、i番目の通過点を基点とした場合の（i＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定される。
【０１４５】
＜Ｅ．利点＞
本実施の形態に従うコントローラシステムおよびその制御方法によれば、制御対象が複数の通過点を連続的に通過する指令曲線軌跡を設定することが可能である。
【０１４６】
今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した説明ではなく、特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
【符号の説明】
【０１４７】
１  レビュー制御システム、２  駆動装置、２０  撮影装置、３０  設定部、３５  軌跡設定部、４０  サーボ指令制御部、５０  トリガ指令制御部、１００  制御装置、１０１  制御演算部、１０２  通信Ｉ／Ｆ部、１０３  記憶部、１０４  入力部、１０５  出力部、１０６  バスライン。
【要約】
【課題】制御対象が複数の通過点を連続的に通過する指令曲線軌跡を設定することが可能なコントローラシステムを提供する。
【解決手段】コントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令曲線軌跡を設定する軌跡設定部とを備える。軌跡設定部は、（i−１）番目（i≧２）の通過点とi番目の通過点との間の（i−１）番目の曲線軌跡は、（i−１）番目の通過点の位置と、（i−１）番目の通過点の速度ベクトルと、i番目の通過点の位置と、i番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義される。
【選択図】図１【図１】
0006863497.tif 000005
【図２】
0006863497.tif 000006
【図３】
0006863497.tif 000007
【図４】
0006863497.tif 000008
【図５】
0006863497.tif 000009
【図６】
0006863497.tif 000010
【図７】
0006863497.tif 000011
【図８】
0006863497.tif 000012
【図９】
0006863497.tif 000013
【図１０】
0006863497.tif 000014
【図１１】
0006863497.tif 000015
【図１２】
0006863497.tif 000016