JP2011159210A - パラレルリンク機構の制御装置および制御方法 - Google Patents

Abstract

【課題】パラレルリンク機構の各リンク機構に備えたモータに指令値を出力する際に、事前に補正値を準備することなく、理想の移動軌跡に対する追従性を高めることができるパラレルリンク機構の制御装置および制御方法を提供する。
【解決手段】リンク機構によって変更されたエンドエフェクタの位置および姿勢を表す実位置の情報を取得し、リンク機構に出力した指令値と取得した実位置の情報とに基づいた偏差量に応じた補正量を次の指令値に加算し、該補正量を加算した指令値をリンク機構の各サーボモータに出力する指令値補正部、を備え、指令値補正部は、エンドエフェクタを目的の位置および姿勢に移動させるための最初の制御周期と、エンドエフェクタが目的の位置および姿勢に到達する最後の制御周期以外の制御周期のときに、補正した指令値をリンク機構の各サーボモータに出力する。
【選択図】図１

Description

本発明は、パラレルリンク機構の制御装置および制御方法に関する。

従来から、６自由度の相対運動が可能なパラレルリンク機構を用いた精密ステージ装置が知られている。この精密ステージ装置は、例えば、製品の組み立て調整機や加工機械などに用いられ、加工対象物などをエンドエフェクタ（ステージ）に搭載し、エンドエフェクタに機械的に可動可能に連結された各リンク機構を制御することによって、エンドエフェクタの位置・姿勢を決定している。それぞれのリンク機構には、モータが備えられており、パラレルリンク機構の制御部は、各リンク機構のモータを制御することによって、エンドエフェクタを移動させている。

従来のパラレルリンク機構の制御部では、エンドエフェクタの可動範囲を空間的に３次元の複数のブロックに分割し、この分割したブロック毎にそれぞれ荷重補正値を予め設定している。そして、パラレルリンク機構によってエンドエフェクタを移動させる際、エンドエフェクタが位置するブロックに設定されている荷重補正値と通常の位置指令値とを併せた指令値を各リンク機構のモータに出力する。これにより、エンドエフェクタの移動軌跡の追従性を向上させている（特許文献１参照）。

特開２００２−１７８２３７号公報

しかしながら、特許文献１で開示されたような、エンドエフェクタの移動軌跡の追従性を向上させるための制御方法では、例えば、エンドエフェクタの可動範囲を空間的に分割したブロックに対応したテーブルに荷重補正値を記憶している。このため、各ブロックに対応した荷重補正値を事前に算出してテーブルに記憶しておく必要がある。このとき、テーブルに記憶する荷重補正値は、精密ステージ装置に備えたリンク機構（軸）の数と、分割したブロックの数とに応じた数の荷重補正値を算出（事前準備）しておく必要がある。例えば、エンドエフェクタの可動範囲を３次元の各方向に６分割した場合、１つの軸（リンク機構）に対して６^３（＝２１６）個の荷重補正値を算出する。そして、６軸全ての荷重補正値は、６^３×６（＝１２９６）個という多大な数の荷重補正値を算出（事前準備）することとなる。この荷重補正値の事前の算出は、複雑な演算式を用いて行うため、多大な数の荷重補正値の算出には相当の時間を費やしてしまうという問題がある。

また、ブロックの分割数によってエンドエフェクタの位置・姿勢を補正することのできる分解能が決まるため、エンドエフェクタを移動させる位置と姿勢とを細かく調整するためには、ブロックの分割数を多くする必要がある。しかし、ブロックの分割数を多くすると、さらに、事前に準備しておく必要がある荷重補正値の算出に時間を費やしてしまう。

また、荷重補正値は、分割したブロック毎に１つの荷重補正値を記憶しているため、エンドエフェクタが同一のブロック内に位置しているときには、同一の荷重補正値に基づいてエンドエフェクタの位置・姿勢が補正される。このため、同一ブロック内において、エンドエフェクタの位置・姿勢に誤差が生じる箇所が存在してしまう。すなわち、同一ブロック内において、エンドエフェクタの位置や姿勢の細かい微調整を行うことができない。これにより、エンドエフェクタの理想的な移動軌跡に対する実際の移動軌跡に誤差が生じてしまうという問題がある。

本発明は、上記の課題認識に基づいてなされたものであり、パラレルリンク機構の各リンク機構に備えたモータに指令値を出力する際に、事前に補正値を準備することなく、理想の移動軌跡に対する追従性を高めることができるパラレルリンク機構の制御装置および制御方法を提供することを目的としている。

上記の課題を解決するため、本発明のパラレルリンク機構の制御装置は、ベースと、前記ベースに対して間隔をあけて配置されるエンドエフェクタと、前記ベースに固定されたサーボモータを有する駆動リンクが一端に連結され、前記エンドエフェクタに連結された可動リンクが他端に連結され、前記駆動リンクと前記可動リンクとの間隔を変更することによって前記ベースに対する前記エンドエフェクタの位置および姿勢を可変とする少なくとも３組のリンク機構とを備えたパラレルリンク機構の制御装置において、前記リンク機構によって変更された前記エンドエフェクタの位置および姿勢を表す実位置の情報を取得し、前記リンク機構に出力した指令値と取得した前記実位置の情報とに基づいた偏差量に応じた補正量を次の指令値に加算し、該補正量を加算した指令値を前記リンク機構の各サーボモータに出力する指令値補正部、を備え、前記指令値補正部は、前記エンドエフェクタを目的の位置および姿勢に移動させるための最初の制御周期と、前記エンドエフェクタが目的の位置および姿勢に到達する最後の制御周期以外の制御周期のときに、補正した指令値を前記リンク機構の各サーボモータに出力する、ことを特徴とする。

また、本発明の前記指令値補正部は、設定された前記エンドエフェクタの位置および姿勢に基づいて、前記リンク機構の各サーボモータに出力する設定指令値を生成する指令値生成部と、前記リンク機構の各サーボモータに出力した指令値と、前記リンク機構によって変更された前記エンドエフェクタの位置および姿勢を表す実位置の情報とに基づいて、前記設定指令値に対する前記実位置の偏差量を検出する偏差量検出部と、前記偏差量検出部が検出した偏差量に基づいて、前記リンク機構の各サーボモータに出力する指令値を補正するための補正量を算出する補正量算出部と、前記補正量算出部が算出した前記補正量を、前記指令値生成部によって生成された前記設定指令値に加算した補正指令値を生成する指令値補正量加算部と、前記指令値生成部によって生成された前記設定指令値または前記指令値補正量加算部によって生成された前記補正指令値を、前記リンク機構の各サーボモータに出力する指令値として、前記リンク機構の制御周期毎に出力する指令値出力部と、を備え、前記指令値出力部は、前記エンドエフェクタを設定された位置および姿勢に移動させるための最初の前記制御周期と、前記エンドエフェクタが設定された位置および姿勢に到達する最後の前記制御周期とにおいて、前記指令値生成部によって生成された前記設定指令値を前記リンク機構の各サーボモータに指令値として出力し、前記エンドエフェクタを設定された位置および姿勢に移動させるための最初の前記制御周期と、前記エンドエフェクタが設定された位置および姿勢に到達する最後の前記制御周期以外の制御周期において、前記指令値補正量加算部によって生成された前記補正指令値を前記リンク機構の各サーボモータに指令値として出力する、ことを特徴とする。

また、本発明の前記補正量算出部は、さらに、前記実位置の情報を前記リンク機構の制御周期毎に移動平均し、移動平均した情報と前記偏差量検出部が検出した偏差量に基づいて、前記リンク機構の各サーボモータに出力する指令値を補正するための補正量を算出する、ことを特徴とする。

また、本発明のパラレルリンク機構の制御方法は、ベースと、前記ベースに対して間隔をあけて配置されるエンドエフェクタと、前記ベースに固定されたサーボモータを有する駆動リンクが一端に連結され、前記エンドエフェクタに連結された可動リンクが他端に連結され、前記駆動リンクと前記可動リンクとの間隔を変更することによって前記ベースに対する前記エンドエフェクタの位置および姿勢を可変とする少なくとも３組のリンク機構とを備えたパラレルリンク機構の制御方法において、前記リンク機構によって変更された前記エンドエフェクタの位置および姿勢を表す実位置の情報を取得し、前記リンク機構に出力した指令値と取得した前記実位置の情報とに基づいた偏差量に応じた補正量を次の指令値に加算し、該補正量を加算した指令値を前記リンク機構の各サーボモータに出力する指令値補正手順、を含み、前記指令値補正手順において、前記エンドエフェクタを目的の位置および姿勢に移動させるための最初の制御周期と、前記エンドエフェクタが目的の位置および姿勢に到達する最後の制御周期以外の制御周期のときに、補正した指令値を前記リンク機構の各サーボモータに出力する、ことを特徴とする。

本発明によれば、パラレルリンク機構の各リンク機構に備えたモータに指令値を出力する際に、事前に補正値を準備することなく、理想の移動軌跡に対する追従性を高めたパラレルリンク機構の制御を行うことができるという効果が得られる。

本発明の第１の実施形態によるパラレルリンク機構によって制御される精密ステージ装置の概略構成を示した装置構成図である。 本発明の第１の実施形態におけるパラレルリンク機構の制御に係わる構成の一例を示したブロック図である。 本発明の第１の実施形態におけるパラレルリンク機構の制御手順を示したフローチャートである。 従来のパラレルリンク機構によって制御されるサーボモータに対する指令値とエンドエフェクタの実位置とを示す概念図である。 本発明の第１の実施形態におけるパラレルリンク機構によって制御されるサーボモータに対する補正指令値とエンドエフェクタの実位置とを示す概念図である。 本発明の第２の実施形態におけるパラレルリンク機構の制御に係わる構成の一例を示したブロック図である。

＜第１の実施形態＞
以下、本発明の実施形態について、図面を参照して説明する。図１は、本第１の実施形態によるパラレルリンク機構によって制御される精密ステージ装置の概略構成を示した装置構成図である。図１において、精密ステージ装置７は、６自由度に移動するステージであるエンドエフェクタ１と、エンドエフェクタ１に連結された可動リンク２と、精密ステージ装置７を外部の構造物に固定するためのベース６と、ベース６に固定されたサーボモータ５を備える駆動リンク４と、一端が駆動リンク４に連結され、他端が可動リンク２に連結された連結リンク３とを備えている。また、精密ステージ装置７は、コントローラ２０と、モータ駆動ドライバ１０とを備えている。また、サーボモータ５は、エンコーダ８を備えている。

なお、図１に示した精密ステージ装置７において、各符号の後の“＿：アンダーバー”に続く数字は、精密ステージ装置７内に備えたリンク機構の軸番号を示す。例えば、第１軸のリンク機構は、可動リンク２＿１と、連結リンク３＿１と、駆動リンク４＿１と、サーボモータ５＿１と、エンコーダ８＿１とによって構成される。また、精密ステージ装置７内に備えたリンク機構の軸番号を特定しない場合は、各符号の後の“＿：アンダーバー”およびそれに続く数字を表記しない。

精密ステージ装置７の使用者は、エンドエフェクタ１を移動させる位置・姿勢の情報をコントローラ２０に設定する。この設定された位置・姿勢の情報に基づいて、エンドエフェクタ１が指定された位置・姿勢に移動する。

コントローラ２０は、精密ステージ装置７の使用者によって設定されたエンドエフェクタ１の位置・姿勢の情報に基づいて、モータ駆動ドライバ１０に出力する指令値を算出する。また、コントローラ２０は、各サーボモータ５から入力された駆動リンク４の位置・姿勢の情報に基づいて、モータ駆動ドライバ１０に出力する指令値を補正する指令値補正部２００を備えている。そして、コントローラ２０は、算出した指令値または補正した指令値をモータ駆動ドライバ１０に出力する。

モータ駆動ドライバ１０は、コントローラ２０から入力された指令値に応じた駆動指令を各サーボモータ５に出力する。

各サーボモータ５は、モータ駆動ドライバ１０から入力された駆動指令に応じて駆動リンク４を駆動（移動）し、移動した駆動リンク４の位置・姿勢の情報をエンコーダ８によって検出する。各サーボモータ５は、エンコーダ８によって検出した駆動リンク４の位置・姿勢の情報をモータ駆動ドライバ１０およびコントローラ２０に出力する。

次に、本第１の実施形態の精密ステージ装置におけるリンク機構の制御に係わる構成について説明する。図２は、本第１の実施形態の精密ステージ装置７におけるリンク機構の制御に係わる構成の一例を示したブロック図である。なお、図２においては、精密ステージ装置７内の１つのリンク機構の構成要素である第１軸のサーボモータ５＿１とエンコーダ８＿１のみを示している。また、精密ステージ装置７内のその他のリンク機構については、図２に示した一例と同様に考えることができる。

指令値補正部２００は、指令値生成部２０１と、指令値出力部２０２と、偏差量検出部２０３と、補正量算出部２０４と、指令値・補正量加算部２０５とを備える。また、モータ駆動ドライバ１０は、従来のモータ駆動ドライバと同等の機能であり、例えば、位置制御部１０１と、速度制御部１０２と、電流制御部１０３と、位置−速度変換部１０４とを備えている。

指令値生成部２０１には、精密ステージ装置７の使用者によって設定されたエンドエフェクタ１を移動させる位置と姿勢との情報が入力される。そして、指令値生成部２０１は、入力（設定）されたエンドエフェクタ１の位置・姿勢情報（以下、「目標位置」という）に基づいて、サーボモータ５を動作させるための指令値を生成する。指令値生成部２０１は、生成した指令値を指令値出力部２０２および指令値・補正量加算部２０５に出力する。

指令値出力部２０２は、指令値生成部２０１から入力された指令値（以下、「設定指令値」という）または後述する指令値・補正量加算部２０５から入力された指令値（以下、「補正指令値」という）をモータ駆動ドライバ１０に出力する。また、指令値出力部２０２からモータ駆動ドライバ１０に出力する指令値は、偏差量検出部２０３にも出力される。

偏差量検出部２０３は、指令値出力部２０２がモータ駆動ドライバ１０に出力した指令値（指定指令値または補正指令値）と、その指令値に応じてエンドエフェクタ１が実際に移動したときの位置と姿勢との情報に基づいて、指令値に対する実位置の差である偏差量を検出する。なお、エンドエフェクタ１が実際に移動したときの位置と姿勢との情報（以下、「実位置」という）は、エンコーダ８＿１から入力される。偏差量検出部２０３は、検出した偏差量を補正量算出部２０４に出力する。

なお、指令値出力部２０２から出力される指令値の情報の形式と、エンコーダ８＿１から入力された実位置の情報の形式とが異なる場合が考えられる。例えば、指令値出力部２０２から出力される指令値の情報の形式がモータ駆動ドライバ１０から出力する電流値を表すデータであり、エンコーダ８＿１から入力された実位置の情報の形式がサーボモータ５＿１が回転したときの回転数を表すパルス数のデータであることが考えられる。このような場合、偏差量検出部２０３は、エンコーダ８＿１から入力されたパルス数のデータをモータ駆動ドライバ１０に出力する電流値のデータに変換するなど、指令値と実位置との情報の形式を統一する。そして、偏差量検出部２０３は、情報の形式を統一した指令値の情報と実位置の情報とに基づいて、偏差量を検出する。例えば、情報の形式を統一した指令値から情報の形式を統一した実位置を減算することによって、偏差量を検出することができる。

補正量算出部２０４は、偏差量検出部２０３から入力された偏差量に基づいて、サーボモータ５を動作させるための指令値の補正量（以下、「補正値」という）を算出する。補正量算出部２０４は、算出した補正値を指令値・補正量加算部２０５に出力する。

指令値・補正量加算部２０５は、指令値生成部２０１から入力された設定指令値と、補正量算出部２０４から入力された補正値とを加算し、補正指令値として指令値出力部２０２に出力する。

位置制御部１０１は、指令値補正部２００から入力された指令値に基づいて、サーボモータ５＿１を駆動してエンドエフェクタ１の位置と姿勢とを制御するための駆動指令を出力する。また、この位置と姿勢とを制御する駆動指令（以下、「位置指令」という）は、エンドエフェクタ１が実際に移動したときの実位置に応じて、位置指令の内容が逐次更新される。位置制御部１０１は、位置指令を速度制御部１０２に出力する。

速度制御部１０２は、位置制御部１０１から入力された位置指令に基づいて、サーボモータ５＿１を駆動してエンドエフェクタ１を移動する速度を制御するための駆動指令を出力する。また、この速度を制御する駆動指令（以下、「速度指令」という）は、後述する位置−速度変換部１０４から入力される、エンドエフェクタ１が実際に移動したときの速度に応じて、速度指令の内容が逐次更新される。速度制御部１０２は、速度指令を電流制御部１０３に出力する。

電流制御部１０３は、速度制御部１０２から入力された速度指令に基づいて、サーボモータ５＿１を駆動するための駆動指令（電流値）を、サーボモータ５＿１に出力する。また、電流制御部１０３は、出力した駆動指令（電流値）に応じて、次にサーボモータ５＿１に出力する駆動指令（電流値）を逐次更新する。

位置−速度変換部１０４は、エンコーダ８＿１から入力されたエンドエフェクタ１が実際に移動したときの実位置に基づいて、実位置の情報をエンドエフェクタ１の移動速度の情報に変換し、速度制御部１０２に出力する。

次に、本第１の実施形態の精密ステージ装置におけるリンク機構の制御方法について説明する。図３は、本第１の実施形態の精密ステージ装置７の指令値補正部２００におけるリンク機構の制御手順を示したフローチャートである。図３に示した制御手順によるリンク機構の制御は、精密ステージ装置７に備えた全てのリンク機構の制御が同時に並行して行われる。なお、図３の説明においては、説明を簡略化するため、精密ステージ装置７内に備えた第１軸のリンク機構を制御する場合について説明する。その他の軸のリンク機構の制御は、図３に示した制御手順と同様に考えることができる。

最初に、精密ステージ装置７の使用者によって、指令値補正部２００にエンドエフェクタ１を移動させる位置と姿勢との情報（目標位置）が設定されると、まず、ステップＳ１０において、指令値生成部２０１は、設定された目標位置に基づいて、サーボモータ５を動作させる制御周期（以下、「サーボ制御周期」という）毎の設定指令値を生成する。そして、生成したサーボ制御周期毎の設定指令値を指令値出力部２０２および指令値・補正量加算部２０５に出力する。また、指令値出力部２０２および指令値・補正量加算部２０５は、指令値生成部２０１から入力されたサーボ制御周期毎の設定指令値を一時記憶する。

続いて、ステップＳ２０において、指令値出力部２０２は、現在の時刻がサーボ制御周期のタイミングであるか否かを確認する。サーボ制御周期のタイミングである場合は、ステップＳ３０に進む。また、サーボ制御周期のタイミングでない場合は、ステップＳ２０を繰り返す。

続いて、ステップＳ３０において、指令値出力部２０２は、現在のサーボ制御周期が最後のサーボ制御周期のタイミング、すなわち、精密ステージ装置７の使用者によって設定された目標位置に応じた最後の指令値を出力するタイミングであるか否かを確認する。最後の指令値を出力するタイミングでない場合は、ステップＳ４０に進む。また、最後の指令値を出力するタイミングである場合は、ステップＳ１００に進み、指令値出力部２０２は、指令値生成部２０１から入力された設定指令値をモータ駆動ドライバ１０に出力し、処理を完了する。これにより、モータ駆動ドライバ１０から設定指令値に基づいた駆動指令が各サーボモータ５に出力され、精密ステージ装置７の使用者によって設定された目標位置にエンドエフェクタ１が移動する。

また、ステップＳ３０において、最後の指令値を出力するタイミングでない場合、ステップＳ４０において、指令値出力部２０２は、現在のサーボ制御周期が最初のサーボ制御周期のタイミング、すなわち、精密ステージ装置７の使用者によって設定された目標位置に応じた最初の指令値を出力するタイミングであるか否かを確認する。最初の指令値を出力するタイミングである場合、ステップＳ５０において、指令値出力部２０２は、指令値生成部２０１から入力された設定指令値をモータ駆動ドライバ１０に出力し、ステップＳ２０に戻る。これにより、モータ駆動ドライバ１０から設定指令値に基づいた駆動指令が各サーボモータ５に出力され、精密ステージ装置７の使用者によって設定された目標位置にエンドエフェクタ１の移動が開始する。

また、ステップＳ４０において、最初の指令値を出力するタイミングでない場合、ステップＳ６０において、偏差量検出部２０３は、エンコーダ８から入力されたエンドエフェクタ１の実位置を取得する。そして、偏差量検出部２０３は、指令値出力部２０２から入力された設定指令値とエンコーダ８から入力された実位置とに基づいて、偏差量を検出する。

続いて、ステップＳ７０において、補正量算出部２０４は、偏差量検出部２０３から入力された偏差量に基づいて、サーボモータ５の補正値を算出する。

続いて、ステップＳ８０において、指令値・補正量加算部２０５は、指令値生成部２０１から入力され一時記憶している設定指令値と補正量算出部２０４から入力された補正値とを加算した補正指令値を生成する。そして、ステップＳ９０において、指令値出力部２０２は、指令値・補正量加算部２０５から入力された補正指令値をモータ駆動ドライバ１０に出力し、ステップＳ２０に戻る。これにより、モータ駆動ドライバ１０から補正指令値に基づいた駆動指令が各サーボモータ５に出力され、エンドエフェクタ１が理想の移動軌跡、すなわち、指令値生成部２０１によって生成されたサーボ制御周期毎の設定指令値に沿って移動する。

上記に述べたとおり、本第１の実施形態によれば、エンドエフェクタ１が移動している途中の実位置をサーボ制御周期毎に取得し、エンドエフェクタ１の移動軌跡の偏差量を検出することができる。そして、検出した偏差量に基づいて、次のサーボ制御周期でモータ駆動ドライバ１０に出力する指令値を補正することができる。これにより、エンドエフェクタ１の移動軌跡が、理想の移動軌跡に近い軌跡となり、移動軌跡の追従性を高めることができる。

ここで、本第１の実施形態のパラレルリンク機構によって制御される精密ステージ装置の動作例について説明する。図４は、本第１の実施形態におけるパラレルリンク機構によって制御されていない場合、すなわち、従来のパラレルリンク機構によって制御される各軸のリンク機構に備えたサーボモータ５に対する指令値とエンドエフェクタ１の実位置とを示す概念図である。また、図５は、本第１の実施形態におけるパラレルリンク機構によって制御される各軸のリンク機構に備えたサーボモータ５に対する補正指令値とエンドエフェクタ１の実位置とを示す概念図である。なお、図４および図５においては、エンドエフェクタ１を８回のサーボ制御周期（Ｔ１〜Ｔ８）で地点Ａ（現在位置）から地点Ｂ（目標位置）に移動させる場合の一例を示している。この制御周期は、地点Ａから地点Ｂまでに移動させる間を、予め定められた所定時間毎に区切った形で構成される。また、地点Ａおよび地点Ｂの各符号の後の“＿：アンダーバー”に続く数字は、精密ステージ装置７内に備えたリンク機構の軸番号を示す。例えば、第１軸のリンク機構は、地点Ａ＿１から地点Ｂ＿１に移動する。

エンドエフェクタ１が地点Ａから地点Ｂに移動する場合、エンドエフェクタ１に搭載した加工対象物の重量や重心によって、各軸のリンク機構に掛かる負荷が異なる。この各軸のリンク機構の負荷によって、図４に示すように、設定指令値に応じたエンドエフェクタ１の実位置が異なり、各軸のリンク機構における偏差量も異なる。なお、図４に示した設定指令値に応じた実位置は、エンコーダ８によって検出したエンドエフェクタ１の実位置を表している。

そこで、指令値補正部２００は、エンコーダ８によって検出したエンドエフェクタ１の実位置に基づいて生成した、図５に示すような補正指令値を各軸のリンク機構のサーボモータ５に出力する。これにより、図５に示すように、エンドエフェクタ１の実位置が補正指令値に応じた実位置となる。なお、最初のサーボ制御周期Ｔ１においては、エンコーダ８がエンドエフェクタ１の実位置を検出していないため、各軸のリンク機構のサーボモータ５に出力する指令値は、設定指令値である。また、最後のサーボ制御周期Ｔ８においては、補正指令値によってエンドエフェクタ１の位置が精密ステージ装置７の使用者によって設定された目標位置と異なる位置に移動してしまう可能性を回避するため、各軸のリンク機構のサーボモータ５に出力する指令値は、設定指令値である。

上記に述べたとおり、本第１の実施形態によるパラレルリンク機構によれば、サーボ制御周期（Ｔ２〜Ｔ７）のタイミングにおいて、設定指令値に代わって補正指令値を各軸のリンク機構のサーボモータ５に出力することによって、エンドエフェクタ１の移動軌跡が理想の移動軌跡（図４および図５に示した設定指令値の軌跡）に近い軌跡となり、移動軌跡の追従性を高めることができる。これにより、パラレルリンク機構における高精度な移動軌跡の制御を行うことができる。

＜第２の実施形態＞
次に、本発明の第２の実施形態について説明する。図６は、本第２の実施形態によるパラレルリンク機構によって制御される精密ステージ装置の概略構成を示した装置構成図である。図６において、指令値補正部２１０は、指令値生成部２０１と、指令値出力部２０２と、偏差量検出部２０３と、補正量算出部２１４と、指令値・補正量加算部２０５とを備える。また、モータ駆動ドライバ１０は、従来のモータ駆動ドライバと同等の機能であり、例えば、位置制御部１０１と、速度制御部１０２と、電流制御部１０３と、位置−速度変換部１０４とを備えている。

なお、本第２の実施形態による精密ステージ装置の概略構成は、図１に示した第１実施形態による精密ステージ装置の概略構成と同様である。また、本第２の実施形態による精密ステージ装置の装置構成は、図２に示した第１実施形態による精密ステージ装置の装置構成の補正量算出部２０４が、補正量算出部２１４に変更されている。また、その他の構成要素は、図２に示した第１実施形態による精密ステージ装置の装置構成と同様であるため、同一の符号を付与し、説明を省略する。

補正量算出部２１４は、エンコーダ８＿１から入力されたエンドエフェクタ１の実位置を制御周期毎に記憶する。そして、記憶された実位置を移動平均して平滑化する。そして、平滑化した実位置と偏差量検出部２０３から入力された偏差量とに基づいて、サーボモータ５を動作させるための補正値を算出する。これにより、図２に示した第１実施形態による補正量算出部２０４が算出する補正値よりも滑らかに変化する補正値を算出することができる。なお、補正量算出部２１４が実位置を移動平均する際には、記憶された全ての制御周期における実位置を用いることもできるが、以前の制御周期で記憶された予め定められた数の実位置と現在のエンドエフェクタ１の実位置とを用いることもできる。

なお、本第２の実施形態の精密ステージ装置におけるリンク機構の制御方法は、図３に示した第１の実施形態の精密ステージ装置７の指令値補正部２１０におけるリンク機構の制御手順において、サーボモータ５の補正値を算出（ステップＳ７０）する際に、エンコーダ８から入力されたエンドエフェクタ１の実位置を移動平均して平滑化する処理が追加されるのみであるため、説明を省略する。

また、本第２の実施形態の精密ステージ装置の動作例も、図４および図５に示した本第１の実施形態の精密ステージ装置の動作例において、図５に示した各軸のリンク機構のサーボモータ５出力する補正指令値の変化が滑らかに変化しているのみであるため、説明を省略する。

上記に述べたとおり、本第２の実施形態によるパラレルリンク機構によれば、サーボ制御周期（Ｔ２〜Ｔ７）のタイミングにおけるエンドエフェクタ１の移動軌跡が滑らかに変化するため、より理想の移動軌跡に近い移動軌跡で追従性を高めることができる。

上記に述べたとおり、本発明を実施するための形態によれば、パラレルリンク機構によって制御される精密ステージ装置において、エンドエフェクタ１が移動している途中の実位置をパラレルリンク機構が制御するリンク機構の制御周期毎に取得することによって、次の制御周期における指令値を補正することができる。また、精密ステージ装置において、エンドエフェクタ１が移動する際、エンドエフェクタ１に搭載した加工対象物の重量や重心によって異なる各軸のリンク機構に掛かる負荷を考慮してエンドエフェクタ１の移動制御を行うことができる。これにより、エンドエフェクタ１の移動軌跡が、理想の移動軌跡に近い軌跡となり、移動軌跡の追従性を高めることができる。また、エンドエフェクタ１を高速に移動させる際にも有効であり、エンドエフェクタ１が移動中の位置・姿勢を安定させることができる。

また、本発明を実施するための形態によれば、パラレルリンク機構によって各リンク機構を制御する際に、事前にリンク機構の補正値を準備する必要がないため、補正値を記憶するテーブルなどの膨大な記憶領域を必要としない。また、事前に時間を費やして補正値を算出しておく必要がないため、パラレルリンク機構の補正などのキャリブレーションにかける時間をなくすことができる。

また、本発明を実施するための形態によれば、パラレルリンク機構が制御するリンク機構の制御周期毎にエンドエフェクタ１の実位置を取得しているため、急な外乱によって各軸のリンク機構に掛かる負荷が変動した場合などにおいても、次の制御周期で外乱によるエンドエフェクタ１の移動軌跡の変動をなくすことができる。

なお、本実施形態においては、サーボ制御周期（Ｔ２〜Ｔ７）のタイミングで補正指令値を各軸のリンク機構のサーボモータ５に出力する場合を一例として説明したが、補正指令値を各軸のリンク機構のサーボモータ５に出力するサーボ制御周期は、本発明を実施するための形態に限定されるものではなく、エンドエフェクタ１が移動している途中のいかなるタイミングでも補正指令値を各軸のリンク機構のサーボモータ５に出力する構成とすることができる。例えば、偏差量に対して予め閾値を設定し、偏差量検出部２０３が閾値以上の偏差量を検出した場合に、補正指令値を各軸のリンク機構のサーボモータ５に出力する構成としてもよい。また、例えば、偏差量の大きさに対応した複数の閾値を予め設定し、偏差量が小さい場合には偏差量をそのまま補正値とし、偏差量が大きい場合には予め設定した閾値毎の係数を偏差量に乗算し、その結果の偏差量を補正値とする構成としてもよい。

なお、図２に示した指令値補正部２００内の各構成要素による処理または図３に示した指令値補正部２００における各処理ステップを実現するためのプログラムを、コンピュータ読み取り可能な記録媒体に記録して、当該記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより、指令値補正部２００に係る上述した種々の処理を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものであってもよい。また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、フラッシュメモリ等の書き込み可能な不揮発性メモリ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。

さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリ（例えばＤＲＡＭ（Ｄｙｎａｍｉｃ Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ））のように、一定時間プログラムを保持しているものも含むものとする。また、上記プログラムは、このプログラムを記憶装置等に格納したコンピュータシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピュータシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。また、上記プログラムは、前述した機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であっても良い。

また、本発明の実施形態は、エンドエフェクタ１とベース６とを連結するリンク機構、つまり可動リンク２、連結リンク３、駆動リンク４、サーボモータ５およびエンコーダ８を、それぞれ６組備えた構成としたが、これはエンドエフェクタ１を精密に制御するために６組を設けたものであって、本発明は少なくとも３組のリンク機構を備えていればエンドエフェクタ１の制御を実施可能である。

以上、本発明の実施形態について、図面を参照して説明してきたが、具体的な構成はこの実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲においての種々の変更も含まれる。

１・・・エンドエフェクタ
２，２＿１，２＿２，２＿３，２＿４，２＿５，２＿６・・・可動リンク
３，３＿１，３＿２，３＿３，３＿４，３＿５，３＿６・・・連結リンク
４，４＿１，４＿２，４＿３，４＿４，４＿５，４＿６・・・駆動リンク
５，５＿１，５＿２，５＿３，５＿４，５＿５，５＿６・・・サーボモータ
６・・・ベース
７・・・精密ステージ装置
８，８＿１，８＿２，８＿３，８＿４，８＿５，８＿６・・・エンコーダ
１０・・・モータ駆動ドライバ
１０１・・・位置制御部
１０２・・・速度制御部
１０３・・・電流制御部
１０４・・・位置−速度変換部
２０・・・コントローラ（制御装置）
２００，２１０・・・指令値補正部
２０１・・・指令値生成部
２０２・・・指令値出力部
２０３・・・偏差量検出部
２０４，２１４・・・補正量算出部
２０５・・・指令値・補正量加算部（指令値補正量加算部）

Claims (4)

1. ベースと、前記ベースに対して間隔をあけて配置されるエンドエフェクタと、前記ベースに固定されたサーボモータを有する駆動リンクが一端に連結され、前記エンドエフェクタに連結された可動リンクが他端に連結され、前記駆動リンクと前記可動リンクとの間隔を変更することによって前記ベースに対する前記エンドエフェクタの位置および姿勢を可変とする少なくとも３組のリンク機構とを備えたパラレルリンク機構の制御装置において、
   前記リンク機構によって変更された前記エンドエフェクタの位置および姿勢を表す実位置の情報を取得し、前記リンク機構に出力した指令値と取得した前記実位置の情報とに基づいた偏差量に応じた補正量を次の指令値に加算し、該補正量を加算した指令値を前記リンク機構の各サーボモータに出力する指令値補正部、
   を備え、
   前記指令値補正部は、
   前記エンドエフェクタを目的の位置および姿勢に移動させるための最初の制御周期と、前記エンドエフェクタが目的の位置および姿勢に到達する最後の制御周期以外の制御周期のときに、補正した指令値を前記リンク機構の各サーボモータに出力する、
   ことを特徴とするパラレルリンク機構の制御装置。
2. 前記指令値補正部は、
   設定された前記エンドエフェクタの位置および姿勢に基づいて、前記リンク機構の各サーボモータに出力する設定指令値を生成する指令値生成部と、
   前記リンク機構の各サーボモータに出力した指令値と、前記リンク機構によって変更された前記エンドエフェクタの位置および姿勢を表す実位置の情報とに基づいて、前記設定指令値に対する前記実位置の偏差量を検出する偏差量検出部と、
   前記偏差量検出部が検出した偏差量に基づいて、前記リンク機構の各サーボモータに出力する指令値を補正するための補正量を算出する補正量算出部と、
   前記補正量算出部が算出した前記補正量を、前記指令値生成部によって生成された前記設定指令値に加算した補正指令値を生成する指令値補正量加算部と、
   前記指令値生成部によって生成された前記設定指令値または前記指令値補正量加算部によって生成された前記補正指令値を、前記リンク機構の各サーボモータに出力する指令値として、前記リンク機構の制御周期毎に出力する指令値出力部と、
   を備え、
   前記指令値出力部は、
   前記エンドエフェクタを設定された位置および姿勢に移動させるための最初の前記制御周期と、前記エンドエフェクタが設定された位置および姿勢に到達する最後の前記制御周期とにおいて、前記指令値生成部によって生成された前記設定指令値を前記リンク機構の各サーボモータに指令値として出力し、
   前記エンドエフェクタを設定された位置および姿勢に移動させるための最初の前記制御周期と、前記エンドエフェクタが設定された位置および姿勢に到達する最後の前記制御周期以外の制御周期において、前記指令値補正量加算部によって生成された前記補正指令値を前記リンク機構の各サーボモータに指令値として出力する、
   ことを特徴とする請求項１に記載のパラレルリンク機構の制御装置。
3. 前記補正量算出部は、さらに、
   前記実位置の情報を前記リンク機構の制御周期毎に移動平均し、移動平均した情報と前記偏差量検出部が検出した偏差量に基づいて、前記リンク機構の各サーボモータに出力する指令値を補正するための補正量を算出する、
   ことを特徴とする請求項２に記載のパラレルリンク機構の制御装置。
4. ベースと、前記ベースに対して間隔をあけて配置されるエンドエフェクタと、前記ベースに固定されたサーボモータを有する駆動リンクが一端に連結され、前記エンドエフェクタに連結された可動リンクが他端に連結され、前記駆動リンクと前記可動リンクとの間隔を変更することによって前記ベースに対する前記エンドエフェクタの位置および姿勢を可変とする少なくとも３組のリンク機構とを備えたパラレルリンク機構の制御方法において、
   前記リンク機構によって変更された前記エンドエフェクタの位置および姿勢を表す実位置の情報を取得し、前記リンク機構に出力した指令値と取得した前記実位置の情報とに基づいた偏差量に応じた補正量を次の指令値に加算し、該補正量を加算した指令値を前記リンク機構の各サーボモータに出力する指令値補正手順、
   を含み、
   前記指令値補正手順において、
   前記エンドエフェクタを目的の位置および姿勢に移動させるための最初の制御周期と、前記エンドエフェクタが目的の位置および姿勢に到達する最後の制御周期以外の制御周期のときに、補正した指令値を前記リンク機構の各サーボモータに出力する、
   ことを特徴とするパラレルリンク機構の制御方法。

Images