JP6863497B1

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

Info

Publication number: JP6863497B1
Application number: JP2020042302A
Authority: JP
Inventors: 宏次森野; 晃一坂田; 優大小林
Original assignee: Omron Corp
Current assignee: Omron Corp
Priority date: 2020-03-11
Filing date: 2020-03-11
Publication date: 2021-04-21
Anticipated expiration: 2040-03-11
Also published as: JP2021144437A; CN113391597A

Abstract

【課題】制御対象が複数の通過点を連続的に通過する指令曲線軌跡を設定することが可能なコントローラシステムを提供する。【解決手段】コントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令曲線軌跡を設定する軌跡設定部とを備える。軌跡設定部は、（i−１）番目（i≧２）の通過点とi番目の通過点との間の（i−１）番目の曲線軌跡は、（i−１）番目の通過点の位置と、（i−１）番目の通過点の速度ベクトルと、i番目の通過点の位置と、i番目の通過点の速度ベクトルとに基づく３次のエルミート曲線で定義される。【選択図】図１

Description

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

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

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

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

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

本開示は、上記の課題を解決するためになされたものであって、制御対象が複数の通過点を連続的に通過する指令曲線軌跡を設定することが可能なコントローラシステムおよびその制御方法に関する。

ある局面に従うコントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令曲線軌跡を設定する軌跡設定部とを備える。軌跡設定部は、（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）等）によって実現されてもよい。さらに、一部の処理をネットワーク接続された外部装置に担当させてもよい。また、制御装置１００の機能構成は、実施形態や構成例に応じて、適宜、機能の省略、置換、及び追加が行われてももちろんよい。また、「制御装置」とは、一般的な情報処理装置（例えば、コンピュータ、ワークステーション等）と解することができる。

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

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

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

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

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

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

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

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

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

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

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

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

そして、下記の方式に基づいて速度ベクトルを設定する。

具体的には、ある任意の通過点とその前後の通過点とを直線で結んだときの角度（θ[deg]）によって速度ベクトルを算出する。

具体的には、i番目の通過点の速度ベクトルは、i番目の通過点を基点とした場合の（i−１）番目の通過点への第１方向ベクトルと、i番目の通過点を基点とした場合の（i＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定される。

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

図６に示されるように、通過点P₂および通過点P₃の速度ベクトルの設定について説明する。

通過点P₂の１つ前の通過点は、通過点P₁、１つ後の通過点は、通過点P₃である。

通過点P₃の１つ前の通過点は、通過点P₂、１つ後の通過点は、通過点P₄である。

通過点P₂を基点とした場合の通過点P₁への第１方向ベクトルと、通過点P₂を基点とした場合の通過点P₃への第２方向ベクトルとで成す角度θ₂を算出する。

通過点P₃を基点とした場合の通過点P₂への第１方向ベクトルと、通過点P₃を基点とした場合の通過点P₄への第２方向ベクトルとで成す角度θ₃を算出する。

本例においては、第１方向ベクトルと、第２方向ベクトルとで成す角度θに応じて速度ベクトルを設定する。

具体的には、第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度以上（θ≧1）である場合について説明する。

第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度以上（θ≧1）である場合で、鈍角（θ≧90）の場合には、速度ベクトルの大きさを所定値Rに設定する。速度ベクトルの方向は、１つ前の通過点から１つ後の通過点へ向かう方向に設定する。

第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度以上（θ≧1）である場合で、鋭角（90＞θ≧1）の場合には、速度ベクトルの大きさは、R(1+cosθ)に設定する。また、速度ベクトルの方向は、１つ前の通過点から１つ後の通過点へ向かう方向に設定する。

本例においては、通過点P₂を基点とした場合の通過点P₁への第１方向ベクトルと、通過点P₂を基点とした場合の通過点P₃への第２方向ベクトルとで成す角度θ₂は、鋭角である。

したがって、通過点P₂の速度ベクトルの大きさは、R(1+cosθ)に設定される。また、通過点P₂の速度ベクトルの方向は、通過点P₁から通過点P₃の方向に設定される。

本例においては、通過点P₃を基点とした場合の通過点P₂への第１方向ベクトルと、通過点P₃を基点とした場合の通過点P₄への第２方向ベクトルとで成す角度θ₃は、鈍角である。

したがって、通過点P₃の速度ベクトルの大きさは、Rに設定される。また、通過点P₃の速度ベクトルの方向は、通過点P₂から通過点P₃の方向に設定される。

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

図７に示されるように、通過点P₂の速度ベクトルの設定について説明する。

通過点P₂の１つ前の通過点は、通過点P₁、１つ後の通過点は、通過点P₃である。また、その１つ後の通過点は、通過点P₄である。

具体的には、第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度未満（1>θ)である場合について説明する。すなわち、現在の通過点から次の通過点が折り返す方向となる場合である。

第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度未満（1>θ)である場合には、速度ベクトルの大きさをR(1+cosθ)に設定する。速度ベクトルの方向は、現在の通過点から１つ後の通過点へ向かう方向と直交し、かつ、２つ後の通過点から遠ざかる方向に設定する。

本例においては、通過点P₂を基点とした場合の通過点P₁への第１方向ベクトルと、通過点P₂を基点とした場合の通過点P₃への第２方向ベクトルとで成す角度θ₂は、所定角度未満である。

したがって、通過点P₂の速度ベクトルの大きさは、R(1+cosθ)に設定される。また、通過点P₂の速度ベクトルの方向は、通過点P₂から通過点P₃の方向と直交し、かつ、通過点P₄から遠ざかる方向に設定される。

図８は、実施形態に従う制御システムの一例における処理手順を示すフローチャートである。なお、以下で説明する処理手順は一例に過ぎず、各処理は、本開示の技術思想の範囲内において可能な限り変更されてよい。また、以下で説明する処理手順は、実施形態や各構成例に応じて、適宜、ステップの省略、置換、及び追加が可能である。

図８を参照して、制御装置１００は、通過点を設定する（ステップＳ２）。

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

次に、制御装置１００は、通過点間の指令曲線軌跡を設定する（ステップＳ４）。

具体的には、軌跡設定部３５は、駆動装置２が移動する複数の通過点を連続的に通過する各通過点間の指令曲線軌跡を設定する。当該通過点間の指令曲線軌跡の設定処理の詳細については後述する。

そして、処理を終了する（エンド）。

図９は、実施形態に従う通過点間の指令曲線軌跡の設定処理について説明するフロー図である。

図９を参照して、軌跡設定部３５は、初期パラメータを設定する（ステップＳ１０）。具体的には、初期パラメータとして、始点座標、終点座標および所定値Rを設定する。また、最初の区間の前回の通過点の速度ベクトルの方向は、始点座標から最初の通過点のベクトルの向きとし、大きさは所定値Rに設定する。

次に、軌跡設定部３５は、最初の区間を算出する区間に設定する（ステップＳ１１）。

一例として、始点座標から第１番目の通過点との間の区間が最初の区間に設定される。

第１番目の通過点から第２番目の通過点との間の区間が２番目の区間に設定される。第２番目の通過点から第３番目の通過点との間の区間が３番目の区間に設定される。なお、第１番目の通過点を始点とした場合には、第１番目の通過点から第２番目の通過点との間の区間が最初の区間に設定される。第２番目の通過点から第３番目の通過点との間の区間が２番目の区間に設定される。第３番目の通過点から第４番目の通過点との間の区間が３番目の区間に設定される。

次に、軌跡設定部３５は、算出する区間の前回の通過点の座標を座標(x₀,y₀)、今回の通過点の座標を座標(x₁,y₁)として取得する（ステップＳ１２）。

具体的には、算出する区間が最初の区間の場合、前回の通過点の座標(x₀,y₀)は、始点座標である。また、今回の通過点の座標(x₁,y₁)は、第１番目の通過点P₁の位置座標である。

次に、軌跡設定部３５は、今回の通過点の通過角度を算出する（ステップＳ１４）。

具体的には、最初の区間の場合、第１番目の通過点P₁を基点とした場合の始点座標への第１方向ベクトルと、第１番目の通過点P₁を基点とした場合の第２番目の通過点P₂への第２方向ベクトルとの成す角度θ₁を算出する。

次に、軌跡設定部３５は、今回の通過点の速度ベクトルを設定する（ステップＳ１６）。具体的には、算出した角度θに基づいて速度ベクトルを設定する。

図６および図７で説明したように第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度以上（θ≧1）である場合で、鈍角（θ≧90）の場合には、速度ベクトルの大きさを所定値Rに設定する。速度ベクトルの方向は、１つ前の通過点から１つ後の通過点へ向かう方向に設定する。

第１方向ベクトルと、第２方向ベクトルとで成す角度θが所定角度未満（1>θ)である場合には、速度ベクトルの大きさをR(1+cosθに設定する。速度ベクトルの方向は、現在の通過点から１つ後の通過点へ向かう方向と直交し、かつ、２つ後の通過点から遠ざかる方向に設定する。

次に、軌跡設定部３５は、パラメータをエルミート曲線式に代入する（ステップＳ１８）。具体的には、前回の通過点の座標(x₀,y₀)、今回の通過点の座標(x₁,y₁)、前回の通過点の速度ベクトル(v_x0,v_y0)、今回の通過点の速度ベクトル(v_x1,v_y1)を式（２）に代入する。

次に、軌跡設定部３５は、今回の通過点の曲線式の算出結果として３次多項式の係数を算出する（ステップＳ２０）。

具体的には、エルミート曲線式のｘ座標の３次多項式の係数（a₁,b₁,c₁,d₁）およびｙ座標の３次多項式の係数（e₁,f₁,g₁,h₁）を算出する。

次に、軌跡設定部３５は、全区間を計算したか否かを判断する（ステップＳ２２）。

ステップＳ２２において、軌跡設定部３５は、全区間を計算したと判断した場合（ステップＳ２２においてＹＥＳ）には、処理を終了する（リターン）。

一方、ステップＳ２２において、軌跡設定部３５は、全区間を計算していないと判断した場合（ステップＳ２２においてＮＯ）には、今回通過点の速度ベクトルを次の区間の前回通過点の速度ベクトルに設定する（ステップＳ２４）。

具体的には、今回の通過点の速度ベクトル(v_x1,v_y1)を次の区間の通過点の前回の速度ベクトル(v_x0,v_y0)に設定する。

次に、軌跡設定部３５は、次の区間を算出する区間に設定する（ステップＳ２６）。

具体的には、算出する区間が２番目の区間の場合、前回の通過点の座標(x₀,y₀)は、第１番目の通過点P₁の位置座標である。また、今回の通過点の座標(x₁,y₁)は、第２番目の通過点P₂の位置座標である。

そして、軌跡設定部３５は、ステップＳ１２に戻り、上記処理を繰り返す。

以降、上記で説明したのと同様の処理を繰り返すことにより、各区間のエルミート曲線式の３次多項式の係数を算出することが可能となる。

なお、算出する区間が最後の区間の場合、すなわち、今回の通過点の座標(x₁,y₁)が終点座標である場合には、次の通過点が存在しない。この場合の速度ベクトルの方向は、前回の通過点の座標から今回の通過点の座標（終点座標）へのベクトルの向きとし、大きさは所定値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＋１）番目の通過点への第２方向ベクトルとで成す角度に基づいて設定される。

＜Ｅ．利点＞
本実施の形態に従うコントローラシステムおよびその制御方法によれば、制御対象が複数の通過点を連続的に通過する指令曲線軌跡を設定することが可能である。

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

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

Claims

通過順序が予め設定された複数の通過点の位置を設定する設定部と、
制御対象が前記複数の通過点を連続的に通過する各前記通過点間の指令曲線軌跡を設定する軌跡設定部とを備え、
前記軌跡設定部は、
（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＋２）番目の通過点から遠ざかる方向に設定される、コントローラシステムの制御方法。