JP6801802B1 - コントローラシステムおよびその制御方法 - Google Patents

Abstract

【課題】制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能なコントローラシステムを提供する。【解決手段】コントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を第１変数に従う区分多項式で設定する軌跡設定部と、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出する指令値算出部とを備える。指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部と、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する変数調整部とを含む。【選択図】図１

Description

本開示は、制御対象を制御するコントローラシステムおよびその制御方法に関する。

各種設備および各設備に配置される各種装置の制御には、ＰＬＣ（プログラマブルロジックコントローラ）などの制御装置が用いられる。

例えば、特開２０００−１３７５０６号公報（特許文献１）には、ロボットの動作を制御する制御装置が指令軌跡に従う指令値を各サーボに出力してロボットの動作を制御する手法が開示されている。

特開２０００−１３７５０６号公報

複数の通過点を検査するロボットの動作を制御する場合、ロボットの動作を複数の通過点で停止させずに連続的に動作させることは検査効率を向上させる点で重要である。

一方で、ロボットの動作としては連続的に動作させるだけでなく、動作速度も適切に制御することが重要である。

本開示は、上記の課題を解決するためになされたものであって、制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能なコントローラシステムおよびその制御方法に関する。

ある局面に従うコントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を第１変数に従う区分多項式で設定する軌跡設定部と、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出する指令値算出部とを備える。指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部と、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する変数調整部とを含む。

この構成によれば、指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出し、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する。これにより制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。

好ましくは、第１変数は、０〜１の間で変化する。

この構成によれば、通過点間の距離に応じて軌跡移動速度が変動することを回避して、制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。

好ましくは、区分多項式は、３次のエルミート曲線である。

この構成によれば、３次のエルミート曲線を用いることにより滑らかな曲線軌跡を設定することが可能である。

好ましくは、変数調整部は、算出された目標移動距離に基づいて第１変数の基準となる基準変動量を設定し、基準変動量に基づいて区分多項式に基づく制御対象の座標を少なくとも１回算出し、前回の制御周期における制御対象の座標と、基準変動量に基づく算出された制御対象の座標との間の距離を算出し、算出された基準変動量に対する距離と目標移動距離との比較に基づいて、制御周期毎の第１変数の変動量を調整する。

この構成によれば、基準変動量を用いて試算点との距離を算出することにより、制御周期毎の第１変数の変動量を調整するため効率的に第１変数の変動量を調整することが可能である。

好ましくは、変数調整部は、基準変動量毎の区分多項式に基づく制御対象の座標を複数回算出し、基準変動量毎の制御対象の座標間のそれぞれの距離を算出し、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とに基づいて第１変数の基準となる基準変動量を補正する。

この構成によれば、基準変動量を補正して、補正した基準変動量を用いて試算点との距離を算出することにより、精度の高い制御周期毎の第１変数の変動量に調整することが可能である。

好ましくは、変数調整部は、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とを比較し、基準変動量毎の制御対象の座標間の距離の合計が目標移動距離以上の場合に目標移動距離に対する第１変数の変動量を算出する。

この構成によれば、基準変動量を用いて試算点との距離の合計を算出して、目標移動距離との比較により、制御周期毎の第１変数の変動量を調整するため効率的に第１変数の変動量を調整することが可能である。

好ましくは、変数調整部は、対象とする指令軌跡の通過点間の距離と、目標移動距離と、分割係数とに基づいて基準変動量を設定する。

この構成によれば、分割係数により基準変動量を調整することが可能であるため精度の高い制御周期毎の第１変数の変動量に調整することが可能である。

ある局面に従うコントローラシステムの制御方法は、通過順序が予め設定された複数の通過点の位置を設定するステップと、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を第１変数に従う区分多項式で設定するステップと、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出するステップとを備える。目標指令値を算出するステップは、制御周期毎の指令制御速度に対する目標移動距離を算出するステップと、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整するステップとを含む。

この構成によれば、指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出し、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する。これにより制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。

なお、本開示において、「部」及び「装置」とは、単に物理的手段を意味するものではなく、その「部」及び「装置」が有する機能をソフトウェアによって実現する構成も含む。また、１つの「部」及び「装置」が有する機能が２つ以上の物理的手段や装置によって実現されてもよく、或いは、２つ以上の「部」及び「装置」の機能が１つの物理的手段や装置によって実現されてもよい。さらに、「部」及び「装置」とは、例えば「手段」及び「システム」と言い換えることも可能な概念である。

本開示のコントローラシステムおよびその制御方法によれば、制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能である。

実施形態に従うレビュー制御システム１のアプリケーション例を示す模式図である。 実施形態に従う駆動装置２の具体的構成を説明する図である。 実施形態に従う制御装置１００のハードウェアの構成の一例を模式的に示す平面図である。 実施形態に従う制御装置１００の機能構成を説明する図である。 実施形態に従う速度ベクトルの設定について説明する図である。 実施形態に従う軌跡設定部３５とサーボ指令制御部４０との関係について説明する図である。 実施形態に従うサーボ指令制御部４０が出力する位置座標を説明する図である。 比較例として指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。 実施形態に従う指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。 実施形態に従う１回目の試算処理を説明する図である。 実施形態に従うパラメータｔの基準変動量の補正について説明する図である。 実施形態に従う２回目の試算処理を説明する図である。 実施形態に従う３回目の試算処理を説明する図である。 実施形態に従う１４個の通過点を設定した場合のシミュレーション例について説明する図である。 実施形態に従う通過座標および３次多項式の係数について説明する図である。 実施形態に従うトレース軌跡を説明する図である。 実施形態に従う指令速度の波形を説明する図である。

本発明の実施の形態について、図面を参照しながら詳細に説明する。なお、図中の同一または相当部分については、同一符号を付してその説明は繰り返さない。

以下では図１のような２軸（Ｘ軸、Ｙ軸）構成の同期グループ（「複数のサーボモータ」及びそれらに対応する「複数のサーボドライバ」を含む複数のサーボ制御デバイスに相当する。）を前提に説明するが、本実施形態の同期グループは、２軸以上のサーボ制御デバイスと１以上のトリガ制御デバイスを含む構成であればよい。以下では対象物が図１のような２軸（Ｘ軸、Ｙ軸）で移動するが、対象物の移動形態は、３軸以上であってもよい。

＜Ａ．適用例＞
まず、本発明が適用される場面の一例について説明する。

図１は、実施形態に従うレビュー制御システム１のアプリケーション例を示す模式図である。

図１には、フラットパネルディスプレイＦＰＤを検査するレビュー制御システム１が示されている。

レビュー制御システム１には、駆動装置２と、撮影装置２０とが設けられている。

駆動装置２は、各軸方向の任意の位置に移動可能に設けられている。なお、本例においては、Ｘ、Ｙ軸に移動する場合について説明する。

駆動装置２の先端には、撮影装置２０が設けられており、駆動装置２を駆動することにより予め検出したフラットパネルディスプレイＦＰＤの欠陥部分を撮影装置２０で再検査する。具体的には、当該位置で撮影装置２０で撮像する。

図２は、実施形態に従う駆動装置２の具体的構成を説明する図である。

図２に示されるように、駆動装置２は、３軸を構成する複数のサーボモータと、それぞれのサーボモータに対応するサーボドライバを含むサーボ制御デバイスとにより、撮像検査の対象物（フラットパネルディスプレイＦＰＤの欠陥部分）の位置に移動するように駆動し、当該位置の対象物（フラットパネルディスプレイＦＰＤの欠陥部分）をトリガ制御デバイスとしての撮影装置２０で撮影する。

再び図１を参照して、本例においては、フラットパネルディスプレイＦＰＤに４つの欠陥部分（通過点）が予め示されている。そして、欠陥部分（通過点）の通過順序が予め設定されているものとする。

レビュー制御システム１は、複数の欠陥部分（通過点）を通過する滑らかな指令曲線を設定する。撮影装置２０は、欠陥部分（通過点）を通過する際に当該位置を撮像する。

レビュー制御システム１は、設定された指令曲線に基づいて駆動装置２を制御することにより、止まらずに欠陥部分（通過点）を撮像することが可能となる。

これにより、フラットパネルディスプレイの検査速度の向上を図ることが可能である。

なお、図１に示すレビュー制御システム１は任意のアプリケーションに応用可能である。

本実施の形態に従うレビュー制御システム１は、複数のデバイスが組み合わされたアプリケーションに対しても、１つの制御装置で制御することができる。

なお、フラットパネルディスプレイに限られず、各種の半導体にも同様に適用可能である。

＜Ｂ．構成例＞
［ハードウェア構成］
次に、本実施形態に係る制御装置１００のハードウェア構成の一例について説明する。

図３は、実施形態に従う制御装置１００のハードウェアの構成の一例を模式的に示す平面図である。

図３を参照して、制御装置１００は、図１及び図２に例示した駆動装置２及び撮影装置２０に接続され、制御演算部１０１、通信インタフェース（Ｉ／Ｆ）部１０２、記憶部１０３、入力部１０４、及び出力部１０５を含み、各部はバスライン１０６を介して相互に通信可能に接続され得る。

制御演算部１０１は、ＣＰＵ（Central Processing Unit）、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）等を含み、情報処理に応じて各構成要素の制御及び各種演算を行う。

通信Ｉ／Ｆ部１０２は、例えば、有線又は無線により他の構成要素である「部」及び「装置」と通信するための通信モジュールである。通信Ｉ／Ｆ部１０２が通信に用いる通信方式は任意であり、例えば、ＬＡＮ（Local Area Network）やＵＳＢ（Universal Serial Bus）等が挙げられ、バスライン１０６と同等の適宜の通信線を適用することもできる。

駆動装置２及び撮影装置２０は、通信Ｉ／Ｆ部１０２を介して、制御演算部１０１等と通信可能に設けることが可能である。

記憶部１０３は、例えばハード・ディスク・ドライブ（ＨＤＤ）、ソリッド・ステート・ドライブ（ＳＳＤ）等の補助記憶装置であり、制御演算部１０１で実行される各種プログラム（各種処理を実行するための演算プログラム、並びに、駆動装置２及び撮影装置２０の動作の制御処理を行うための制御プログラム等）、校正条件、測定条件、画像処理条件（対象物の認識パラメータ等）を含むデータベース、撮影装置２０から出力される撮影画像（測定データ）、画像処理結果のデータ、対象物の３次元モデルデータ等を記憶する。このとおり、記憶部１０３に記憶された演算プログラム及び制御プログラムが制御演算部１０１で実行されることにより、後述する機能構成例における各種処理機能が実現される。

入力部１０４は、駆動装置２、撮影装置２０、及び制御装置１００を利用するユーザからの各種入力操作を受け付けるためのインタフェースデバイスであり、例えば、マウス、キーボード、タッチパネル、音声マイク等で実現し得る。出力部１０５は、駆動装置２、撮影装置２０、及び制御装置１００を利用するユーザ等へ、各種情報を、その表示、音声出力、印刷出力等により報知するためのインタフェースデバイスであり、例えば、ディスプレイ、スピーカ、プリンタ等で実現し得る。

［機能構成］
図４は、実施形態に従う制御装置１００の機能構成を説明する図である。

図４を参照して、制御装置１００の制御演算部１０１は、記憶部１０３に記憶された各種プログラム（制御プログラム及び演算プログラム等）をＲＡＭに展開し、それらの各種プログラムをＣＰＵにより解釈及び実行して、各構成要素を制御する。これにより、図４に例示の如く、本実施形態に係る制御装置１００は、設定部３０と、軌跡設定部３５と、サーボ指令制御部４０と、トリガ指令制御部５０とを備える構成を実現し得る。

設定部３０は、通過順序が予め設定された複数の通過点の位置を設定する。

軌跡設定部３５は、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を設定する。

サーボ指令制御部４０は、設定された指令軌跡に基づいて駆動装置２の動作を制御する指令信号を出力する。

トリガ指令制御部５０は、複数の通過点の位置に従って撮影装置２０に対して撮影指令信号を出力する。

なお、本実施形態では、制御装置１００で実現される各機能が汎用のＣＰＵによって実現される例について説明したが、以上の機能の一部又は全部が、１又は複数の専用のプロセッサ又は専用回路（例えば、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field-Programmable Gate Array）等）によって実現されてもよい。さらに、一部の処理をネットワーク接続された外部装置に担当させてもよい。また、制御装置１００の機能構成は、実施形態や構成例に応じて、適宜、機能の省略、置換、及び追加が行われてももちろんよい。また、「制御装置」とは、一般的な情報処理装置（例えば、コンピュータ、ワークステーション等）と解することができる。

［動作例］
本開示の一実施形態に係る制御装置１００の一例で実現される制御方法について説明する。

指令軌跡の算出方式について説明する。

一例として、順序が予め設定された複数の通過点を連続的に通過する指令軌跡の算出について説明する。

一例として、指定された通過点（P₁、P₂…P_n）の（P₁−P₂、P₂−P₃、・・・P_n-1−P_n）の点間の軌跡をｘ座標、ｙ座標の３次多項式で定義すると下記式（１）で表される。

そして、次の２つの条件を満たす３次の曲線式としてエルミート曲線で定義する。

具体的には、設定された通過点の座標を通ることと、各通過点の境界および曲線途中で軌跡が連続であることである。

上記条件に基づくエルミート曲線は、下記式（２）で表される。

エルミート曲線は、始点位置、始点速度ベクトル、終点位置、終点速度ベクトルの４つのパラメータに基づいて設定される。

したがって、t=0で始点位置、t=1で終点位置となるため、通過点の座標を始点位置、終点位置のパラメータとして設定する。

エルミート曲線を連続的に繋ぎ合わせる手法として、Catmull-Rom Spline（キャットムル・ロム曲線）を用いる。具体的には、本例においては、各通過点での位置および速度の連続性を保つために、速度ベクトルを通過点の前後で一致するように設定する。

図５は、実施形態に従う速度ベクトルの設定について説明する図である。

図５に示されるように、具体的には、通過点P₂において、指令軌跡Q1の曲線式の終点速度ベクトルと、指令軌跡Q2の曲線式の始点速度ベクトルに同じベクトルを設定する。

これにより、順序が予め設定された複数の通過点を連続的に通過する指令軌跡を設定することが可能となる。

具体的には、各通過点間毎の区間多項式である３次のエルミート曲線が設定される。

図６は、実施形態に従う軌跡設定部３５とサーボ指令制御部４０との関係について説明する図である。

図６に示されるように、軌跡設定部３５は、通過点座標と、通過順序および速度ベクトルを入力することにより各通過点毎の３次のエルミート曲線を設定する。

軌跡設定部３５は、算出結果として３次のエルミート曲線の指令軌跡データをサーボ指令制御部４０に出力する。

サーボ指令制御部４０は、設定された３次のエルミート曲線の指令軌跡データに基づいて駆動装置２の動作を制御する指令信号を出力する。

具体的には、サーボ指令制御部４０は、駆動装置２の動作を制御する位置座標X,Yを出力する。

図７は、実施形態に従うサーボ指令制御部４０が出力する位置座標を説明する図である。

図７に示されるように、サーボ指令制御部４０は、軌跡設定部３５で設定された指令軌跡データに従って駆動装置２の動作位置を出力する。

具体的には、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の駆動装置２の目標指令値を算出する。

これによりサーボ指令制御部４０は、設定された指令軌跡に沿って駆動装置２を動作させることが可能となる。

図８は、比較例として指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。

図８（Ａ）には、各通過点間における軌跡移動速度が示されている。

図８（Ｂ）には、各指令軌跡のパラメータｔの変化が示されている。

当該図に示されるように、パラメータｔが一定の割合で０〜１に変化する場合には、一例として通過点間の距離が短い場合には、基準速度V₀よりも軌跡移動速度は遅くなる場合が示されている。一方で、通過点間の距離が長い場合には、軌跡移動速度は、基準速度V₀よりも軌跡移動速度は速くなる場合が示されている。

すなわち、パラメータｔを一定の割合で調整した場合には、駆動装置２の速度を一定に調整することはできない。

図９は、実施形態に従う指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。

図９（Ａ）には、各通過点間における軌跡移動速度が示されている。

図９（Ｂ）には、各指令軌跡のパラメータｔの変化が示されている。

当該図に示されるように、各通過点間における軌跡移動速度を基準速度V₀で一定に制御するためには、各通過点間のパラメータｔの変動率を調整する必要がある。

実施形態においては、各通過点間における軌跡移動速度を調整する。

再び図７を参照してサーボ指令制御部４０は、距離算出部４２と、変数調整部４４とを含む。

距離算出部４２は、制御周期毎の指令制御速度に対する目標移動距離を算出する。

変数調整部４４は、距離算出部４２で算出された目標移動距離が一定となるように制御周期毎のパラメータｔの増分値を調整する。

＜Ｃ．設定方式＞
実施形態に従うパラメータｔの設定方式は、パラメータｔに所定の基準変動量を与える試算処理を実行する。

本例においては、一例として２回あるいは３回試算処理した場合に、制御周期毎の目標移動距離の近傍に試算点が到達するような基準変動量を設定する。

図１０は、実施形態に従う１回目の試算処理を説明する図である。

図１０を参照して、本例においては、通過点P₀(X₀,Y₀)と、通過点P₁(X₁,Y₁)との間の指令軌跡Q0の曲線式に対してパラメータｔの変動量を設定する場合の処理について説明する。

一例として、現在位置を通過点P₀(X₀,Y₀)とした場合に、次の制御周期で移動する目標位置P_m(X_m,Y_m)のパラメータｔの変動量T_mを算出する。

［準備処理］
通過点P₀(X₀,Y₀)までの積算移動距離をL_sとする。

制御周期毎の次の目標位置P_m(X_m,Y_m)までの積算移動距離はL_mとする。

目標移動距離ΔL_mは、L_m-L_sである。

本例においては、パラメータｔを２回変化させて試算した場合に積算移動距離がL_m近傍になる試算用の基準変動量ΔTをまず設定する。

通過点P₀(X₀,Y₀)と、通過点P₁(X₁,Y₁)との間の直線距離ΔLを算出する。

パラメータｔが０〜１まで変化することにより直線距離ΔL移動すると仮定すると、制御周期毎のパラメータｔの平均変化量は、ΔL/ΔL_mである。

したがって、パラメータｔをｋ回変化させて試算した場合に積算移動距離がL_m近傍になる試算用の基準変動量ΔTは、ΔL/ΔL_m/kにより設定する。

本例においては、分割係数k=2に設定する。

パラメータｔを１回変化させて試算した場合に積算移動距離がL_m近傍になる試算用の基準変動量ΔTは、ΔL/ΔL_mを設定する。

パラメータｔを２回変化させて積算移動距離がL_m近傍になる試算用の基準変動量ΔTは、ΔL/ΔL_m/2により設定する。

パラメータｔを３回変化させて積算移動距離がL_m近傍になる試算用の基準変動量ΔTは、ΔL/ΔL_m/3により設定する。

なお、変化させる試算回数を増加させることにより精度の高いパラメータｔの変動量を算出することが可能であるが、演算量が増加する。

基準変動量を用いて試算点との距離を算出することにより、パラメータｔの変動量を算出するため効率的にパラメータｔの変動量を調整することが可能である。

［１回目の試算処理］
１回目の試算処理においては第１試算点SP_iを算出する。

iは、試算回数を示す。本例においては、i=1である。

具体的には、第１試算点SP_iとして、パラメータｔの値をT_iに設定する。

一例として、通過点P₀(X₀,Y₀)のパラメータｔの値T_sは、０とする。

T_iは、T_s+ΔTである。

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにT_iを入力することにより、通過点P₀(X₀,Y₀)から基準変動量ΔT分、移動した第１試算点SP_iの位置(X_i,Y_i)が算出される。

通過点P₀(X₀,Y₀)から基準変動量ΔT分、移動した場合の第１試算点SP_iの位置(X_i,Y_i)の距離は、次式により算出される。

指令軌跡の積算移動距離は、曲線式に制御周期毎にパラメータｔを与えて、線積分を各微小区間の直線補間に近似して積算する。

ここで、jは積算回数である。

積算移動距離L_iは、L_s+ΔL_iとして算出される。

次に、通過点P₀(X₀,Y₀)からの目標である積算移動距離L_mと、１回目の試算点SP_iまでの試算距離L_iとを比較する。

比較結果に基づいて、L_m＞L_iである場合には、試算処理を継続する。すなわち、２回目の試算処理を実行する。

［基準変動量の補正］
２回目の試算処理を実行する前にパラメータｔの基準変動量ΔTを補正する。

図１１は、実施形態に従うパラメータｔの基準変動量の補正について説明する図である。

図１１（Ａ）を参照して、パラメータｔと、積算移動距離Lとの関係が示されている。

パラメータｔは、０〜１へと変化する。

パラメータｔが０の位置の場合の通過点P₀(X₀,Y₀)の積算移動距離は、L_sである。

T_iは、T_sにΔTを加算した値である。

L_iは、L_sにΔL_iを加算した値である。

しかしながら、ΔL_iは、試算点と通過点P₀(X₀,Y₀)との間の直線距離であるため実際の曲線軌跡を移動した距離とは異なる。

理想的には、１回目の試算処理の試算点における積算移動距離の理想値L_tは、目標とする積算移動距離L_mと、L_sとの間の中間値である。

したがって、１回目の試算処理の試算点における積算移動距離が理想値L_tとなるように基準変動量ΔTを補正する。

具体的には、距離の誤差を算出して、誤差分に応じた量だけ基準変動量ΔTを補正する。

図１１（Ｂ）および（Ｃ）を参照して、補正後の基準変動量ΔT_adj1は、次式により算出される。また、調整量Δ_adj1は、次式により算出される。

そして、補正後の基準変動量ΔT_adj1を用いて２回目の試算処理を実行する。

［２回目の試算処理］
図１２は、実施形態に従う２回目の試算処理を説明する図である。

図１２に示されるように、２回目の試算処理においては第１試算点SP_i-1と、第２試算点SP_iとを算出する。

iは、試算回数を示す。本例においては、i=2である。

具体的には、第１試算点SP_i-1として、パラメータｔの値をT_i-1に設定する。

T_i-1は、T_s+ΔT_adj1である。

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにT_i-1を入力することにより、通過点P₀(X₀,Y₀)から基準変動量ΔT_adj1分、移動した第１試算点SP_i-1の位置(X_i-1,Y_i-1)が算出される。

通過点P₀(X₀,Y₀)と、基準変動量ΔT_adj1分、移動した場合の試算した第１試算点SP_i-1の位置(X_i-1,Y_i-1)との距離ΔL_i-1を算出する。

積算移動距離L_i-1は、L_sにΔL_i-1を加算した値である。

次に、第２試算点SP_iを算出する。

具体的には、第２試算点SP_iとして、パラメータｔの値をT_iに設定する。

T_iは、T_i-1+ΔT_adj1である。

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにT_iを入力することにより、第１試算点SP_i-1から基準変動量ΔT_adj1分、移動した第２試算点SP_iの位置(X_i,Y_i)が算出される。

第１試算点SP_i-1の位置(X_i-1,Y_i-1)と、基準変動量ΔT_adj1分、移動した場合の試算した第２試算点SP_iの位置(X_i,Y_i)との距離ΔL_iを算出する。

積算移動距離L_iは、L_i-1にΔL_iを加算した値である。

次に、通過点P₀(X₀,Y₀)からの目標である積算移動距離L_mと、第２試算点SP_iまでの試算距離L_iとを比較する。

比較結果に基づいて、L_m≦L_iである場合には、積算移動距離L_mに対応するT_mを算出する。

パラメータｔの変動量T_mは、次式により算出される。

そして、試算処理を終了する。

すなわち、指令軌跡Qのエルミート曲線で示される数式のパラメータｔにT_mを入力して算出された目標位置P_m(X_m,Y_m)が通過点P₀(X₀,Y₀)から次の制御周期で移動する位置に設定される。

本例の如く、基準変動量を補正して、補正した基準変動量を用いて試算点との距離を算出するため、精度の高いパラメータｔの変動量に調整することが可能である。

一方、通過点P₀(X₀,Y₀)からの目標である積算移動距離L_mと、第２試算点SP_iまでの試算距離L_iとを比較した比較結果に基づいて、L_m>L_iである場合には、３回目の試算処理を実行する。

［基準変動量の補正］
３回目の試算処理を実行する前にパラメータｔの基準変動量ΔTを補正する。

具体的には、上記と同様の方式に従って基準変動量を補正する。

理想的には、２回目の試算処理の第２試算点SP_iにおける積算移動距離の理想値L_tは、目標とする積算移動距離L_mである。

したがって、２回目の試算処理の第２試算点SP_iにおける積算移動距離が理想値L_tとなるように基準変動量ΔTを補正する。

具体的には、距離の誤差を算出して、誤差分に応じた量だけ基準変動量ΔTを補正する。

補正後の基準変動量ΔT_adj2は、次式により算出される。また、調整量Δ_adj2は、次式により算出される。また、調整量Δ_adj2は、次式により算出される。

そして、補正後の基準変動量ΔT_adj2を用いて３回目の試算処理を実行する。

［３回目の試算処理］
図１３は、実施形態に従う３回目の試算処理を説明する図である。

図１３に示されるように、３回目の試算処理においては第１試算点SP_i-2と、第２試算点SP_i-1と、第３試算点SP_iとを算出する。

iは、試算回数を示す。本例においては、i=3である。

具体的には、第１試算点SP_i-2として、パラメータｔの値をT_i-2に設定する。

T_i-2は、T_s+ΔT_adj2である。

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにT_i-2を入力することにより、通過点P₀(X₀,Y₀)から基準変動量ΔT_adj2分、移動した第１試算点SP_i-2の位置(X_i-2,Y_i-2)が算出される。

通過点P₀(X₀,Y₀)と、基準変動量ΔT_adj2分、移動した場合の試算した第１試算点SP_i-2の位置(X_i-2,Y_i-2)との距離ΔL_i-2を算出する。

積算移動距離L_i-2は、L_sにΔL_i-2を加算した値である。

次に、第２試算点SP_i-1を算出する。

具体的には、第２試算点SP_i-1として、パラメータｔの値をT_i-1に設定する。

T_i-1は、T_i-2+ΔT_adj2である。

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにT_i-1を入力することにより、第１試算点SP_i-2から基準変動量ΔT_adj2分、移動した第２試算点SP_i-1の位置(X_i-1,Y_i-1)が算出される。

第２試算点SP_i-1の位置(X_i-1,Y_i-1)と、基準変動量ΔT_adj2分、移動した場合の試算した第２試算点SP_i-1の位置(X_i-1,Y_i-1)との距離ΔL_i-1を算出する。

積算移動距離L_i-1は、L_i-2にΔL_i-1を加算した値である。

次に、第３試算点SP_iを算出する。

具体的には、第３試算点SP_iとして、パラメータｔの値をT_iに設定する。

T_iは、T_i-1+ΔT_adj2である。

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにT_iを入力することにより、第２試算点SP_i-1から基準変動量ΔT_adj2分、移動した第３試算点SP_iの位置(X_i,Y_i)が算出される。

第２試算点SP_i-1の位置(X_i-1,Y_i-1)と、基準変動量ΔT_adj2分、移動した場合の試算した第３試算点SP_iの位置(X_i,Y_i)との距離ΔL_iを算出する。

積算移動距離L_iは、L_i-1にΔL_iを加算した値である。

次に、通過点P₀(X₀,Y₀)からの目標である積算移動距離L_mと、第３試算点SP_iまでの試算距離L_iとを比較する。

比較結果に基づいて、L_m≦L_iである場合には、積算移動距離L_mに対応するT_mを算出する。

パラメータｔの変動量T_mは、次式により算出される。

そして、試算処理を終了する。

すなわち、指令軌跡Qのエルミート曲線で示される数式のパラメータｔにT_mを入力して算出された目標位置P_m(X_m,Y_m)が通過点P₀(X₀,Y₀)から次の制御周期で移動する位置に設定される。

そして、設定した位置を基点として、上記で説明した方式を用いて次の制御周期で移動する位置を算出する。

これにより、変数調整部４４は、距離算出部４２で算出された目標移動距離が一定となるように制御周期毎のパラメータｔの増分値を調整することが可能となる。通過点P₀(X₀,Y₀)から通過点P₁(X₁,Y₁)への指令軌跡Q0に沿って制御周期毎に移動する位置を設定する。

一方で、通過点P₁(X₁,Y₁)付近になった場合に、試算処理において、パラメータｔの値であるT_iが１を超える場合がある。すなわち、通過点P₁(X₁,Y₁)を跨ぐ場合である。

パラメータｔの値であるT_iが１を超える場合には、対象とする指令軌跡を変更する。通過点P₁(X₁,Y₁)から通過点P₂(X₂,Y₂)への指令軌跡Q1に対応する区分多項式に変更する。すなわち、通過点P₁(X₁,Y₁)と通過点P₂(X₂,Y₂)との間の区分多項式に変更する。

そして、Ti-1をT_mとして指令軌跡Q1に対応する区分多項式のパラメータｔに当該T_mを入力して算出された目標位置P_m(X_m,Y_m)が次の制御周期で移動する位置に設定される。

当該処理により、各通過点間のパラメータｔの変動率を調整することが可能である。

これにより、各通過点間における軌跡移動速度を基準速度V₀で一定に制御することが可能である。すなわち、制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。

＜Ｄ．シミュレーション例＞
次に、実施形態に従う制御システム１のシミュレーション例について説明する。

図１４は、実施形態に従う１４個の通過点を設定した場合のシミュレーション例について説明する図である。

図１４に示されるように、１４個の通過点が示されており、数字は、通過順序を示している。

図１５は、実施形態に従う通過座標および３次多項式の係数について説明する図である。

図１５を参照して、本例においては、最初の始点座標および最後の終点座標は同じ（０，０）に設定されている。また、１４個のそれぞれの通過点の座標が設定されている。

また、所定値Rは５０に設定している。

上記で説明した方式に基づいて各区間のｘ座標の３次多項式の係数およびｙ座標の３次多項式の係数がそれぞれ算出される。

図１６は、実施形態に従うトレース軌跡を説明する図である。

図１６に示されるように、設定された順序に従って１４個の通過点を連続的にかつ滑らかに通過する指令軌跡が描かれている。

図１７は、実施形態に従う指令速度の波形を説明する図である。

図１７に示されるように、本例においては、指令速度として、500mm/sに設定した場合が示されている。

なお、加速度および減速度は、1000mm/s²とした。また、ジャークは、10000mm/s³に設定した。

当該図に示されるように、各通過点間における軌跡移動速度を基準速度V₀で一定に制御することが可能であることを確認することが可能である。

なお、本例においては、一例として平面上における通過順序が予め設定された複数の通過点を連続的に通過する各通過点間の指令軌跡として、区分多項式である３次のエルミート曲線で設定する方式について説明した。一方で、２次元の平面上の通過点に限られず３次元の空間上の通過点についても同様に適用可能である。すなわち、ｚ座標についてもエルミート曲線で定義し、３次元の空間上の通過点に対して通過点の速度ベクトルを３次元座標のベクトルとして処理すればよい。

また、本例においては、各通過点間の指令軌跡の区分多項式として一例として、３次のエルミート曲線で設定する場合について説明したが、特にこれに限られず他の曲線等を用いることも可能である。すなわち、各通過点間の指令軌跡を１つの共通のパラメータｔで設定するスプライン関数であればどのようなものを採用しても良い。次数も３次に限られず、さらに高次でも良いし、２次あるいは１次の直線としてもよい。

指令軌跡が直線の場合には、１次のスプライン関数として定義することが可能である。

この場合であっても距離算出部４２で算出された目標移動距離が一定となるように各通過点間の制御周期毎のパラメータｔの増分値を調整することにより、軌跡移動速度を一定に制御することが可能である。

＜Ｄ．付記＞
上述したような本実施の形態は、以下のような技術思想を含む。

［構成１］
コントローラシステム（１００）は、通過順序が予め設定された複数の通過点の位置を設定する設定部（３０）と、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を区分多項式で設定する軌跡設定部（３５）と、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出する指令値算出部（４０）とを備える。区分多項式は、第１変数に従う３次のエルミート曲線で定義される。指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部（４２）と、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する変数調整部（４４）とを含む。

［構成２］
第１変数は、０〜１の間で変化する。

［構成３］
区分多項式は、３次のエルミート曲線である。

［構成４］
変数調整部は、算出された目標移動距離に基づいて第１変数の基準となる基準変動量ΔTを設定し、基準変動量に基づいて区分多項式に基づく制御対象の座標を少なくとも１回算出し、前回の制御周期における制御対象の座標と、基準変動量に基づく算出された制御対象の座標との間の距離を算出し、算出された基準変動量に対する距離と目標移動距離との比較に基づいて、制御周期毎の第１変数の変動量を調整する。

［構成５］
変数調整部は、基準変動量毎の区分多項式に基づく制御対象の座標を複数回算出し、基準変動量毎の制御対象の座標間のそれぞれの距離を算出し、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とに基づいて第１変数の基準となる基準変動量を補正する。

［構成６］
変数調整部は、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とを比較し、基準変動量毎の制御対象の座標間の距離の合計が目標移動距離以上の場合に目標移動距離に対する第１変数の変動量を算出する。

［構成７］
変数調整部は、対象とする指令軌跡の通過点間の距離と、目標移動距離と、分割係数ｋとに基づいて基準変動量を設定する。

［構成８］
コントローラシステムの制御方法は、通過順序が予め設定された複数の通過点の位置を設定するステップと、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を区分多項式で設定するステップと、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出するステップとを備える。区分多項式は、第１変数に従う３次のエルミート曲線で定義され、目標指令値を算出するステップは、制御周期毎の指令制御速度に対する目標移動距離を算出するステップと、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整するステップとを含む。

＜Ｅ．利点＞
本実施の形態に従うコントローラシステムおよびその制御方法によれば、制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能である。

今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した説明ではなく、特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

１ レビュー制御システム、２ 駆動装置、２０ 撮影装置、３０ 設定部、３５ 軌跡設定部、４０ サーボ指令制御部、４２ 距離算出部、４４ 変数調整部、５０ トリガ指令制御部、１００ 制御装置、１０１ 制御演算部、１０２ 通信Ｉ／Ｆ部、１０３ 記憶部、１０４ 入力部、１０５ 出力部、１０６ バスライン。

Claims (8)

1. 通過順序が予め設定された複数の通過点の位置を設定する設定部と、
   制御対象が前記複数の通過点を連続的に通過する各前記通過点間の指令軌跡を第１変数に従う区分多項式で設定する軌跡設定部と、
   対象とする指令軌跡に対応する前記区分多項式に基づいて制御周期毎の前記制御対象の目標指令値を算出する指令値算出部とを備え、
   前記指令値算出部は、
   前記制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部と、
   前記算出された目標移動距離が一定となるように前記制御周期毎の第１変数の変動量を調整する変数調整部とを含む、コントローラシステム。
2. 前記第１変数は、０〜１の間で変化する、請求項１記載のコントローラシステム。
3. 前記区分多項式は、３次のエルミート曲線である、請求項２記載のコントローラシステム。
4. 前記変数調整部は、
   前記算出された目標移動距離に基づいて前記第１変数の基準となる基準変動量を設定し、
   前記基準変動量に基づいて前記区分多項式に基づく前記制御対象の座標を少なくとも１回算出し、
   前回の制御周期における前記制御対象の座標と、前記基準変動量に基づく算出された前記制御対象の座標との間の距離を算出し、
   算出された基準変動量に対する距離と前記目標移動距離との比較に基づいて、前記制御周期毎の第１変数の変動量を調整する、請求項２記載のコントローラシステム。
5. 前記変数調整部は、
   前記基準変動量毎の前記区分多項式に基づく前記制御対象の座標を複数回算出し、
   前記基準変動量毎の前記制御対象の座標間のそれぞれの距離を算出し、
   前記基準変動量毎の前記制御対象の座標間の距離の合計と、前記目標移動距離とに基づいて前記第１変数の基準となる前記基準変動量を補正する、請求項４記載のコントローラシステム。
6. 前記変数調整部は、
   前記基準変動量毎の前記制御対象の座標間の距離の合計と、前記目標移動距離とを比較し、
   前記基準変動量毎の前記制御対象の座標間の距離の合計が前記目標移動距離以上の場合に前記目標移動距離に対する前記第１変数の変動量を算出する、請求項４記載のコントローラシステム。
7. 前記変数調整部は、前記対象とする指令軌跡の通過点間の距離と、前記目標移動距離と、分割係数とに基づいて前記基準変動量を設定する、請求項４記載のコントローラシステム。
8. 通過順序が予め設定された複数の通過点の位置を設定するステップと、
   制御対象が前記複数の通過点を連続的に通過する各前記通過点間の指令軌跡を第１変数に従う区分多項式で設定するステップと、
   対象とする指令軌跡に対応する前記区分多項式に基づいて制御周期毎の前記制御対象の目標指令値を算出するステップとを備え、
   前記目標指令値を算出するステップは、
   前記制御周期毎の指令制御速度に対する目標移動距離を算出するステップと、
   前記算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整するステップとを含む、コントローラシステムの制御方法。

Images