JP2005186210A - パラレルメカニズム機械の制御方法および制御装置 - Google Patents

Abstract

【課題】ユニバーサルジョイントの弾性変形量を考慮した高精度なアクチュエータ指令値の補正を、少ない計算量で実行可能とするパラレルメカニズム機械の制御方法および制御装置を提供する。
【解決手段】ステップＳ１により機構パラメータに基づいて各アクチュエータ指令値を求め、ステップＳ２により機械の各構成要素の自重による荷重を計算し、ステップＳ３により各アクチュエータ軸方向荷重を求め、ステップＳ４により各ストラット軸方向荷重を各第１，第２ユニバーサルジョイント軸方向に分解し、ステップＳ５により各要素の弾性変形量を求め、ステップＳ６により各ユニバーサルジョイントの弾性変形量を各アクチュエータ軸方向変位に近似換算し、ステップＳ７により各アクチュエータ指令値をＳ５，Ｓ６で求めた各アクチュエータ軸方向変位で減算して更新し、更新された各アクチュエータ指令値を基に制御する。
【選択図】図５

Description

本発明は、アクチュエータによって駆動される複数のストラットにより、これらに接続されたエンドエフェクタを移動可能なパラレルメカニズム機械の制御方法、およびこの方法を実行可能な制御装置に関する。

従来のパラレルメカニズム機械の制御方法として、下記特許文献１に記載された方法が知られている。この方法は、ストラットとエンドエフェクタとが回転１自由度のジョイントを組合わせたユニバーサルジョイントで接続されている場合にあっても、各ジョイントの弾性変形量をも加味して各ストラットの長さに対応するアクチュエータ指令値を高精度に修正するものであり、エンドエフェクタの位置指令値および姿勢指令値と機構パラメータとから各ストラット長指令値を算出する逆機構変換を用いるものである。

具体的には、無負荷状態で逆機構変換を行って、修正前の各アクチュエータ指令値を求めた後、各ジョイントの角度を計算し、各ストラット等に作用する荷重からジョイントの構成要素に作用する荷重を求め、この荷重から各構成要素の弾性変形量を算出し、この弾性変形量に基づき機構パラメータを修正し、これを用い再び逆機構変換を行って修正された各アクチュエータ指令値を得る。

特開２００２−９１５６８号公報

以上のような従来の方法では、各ジョイントの角度の計算、構成要素毎の荷重や弾性変形量の計算に加えて、逆機構変換を２度も行わなければならず、計算量が膨大になって、計算時間の長大化による制御性能低下を防ぐには、計算能力は極めて高いが高コストである制御装置を採用するしかない。

そこで、請求項１および請求項５に記載の発明は、エンドエフェクタの位置や姿勢を高精度に決定するための、ユニバーサルジョイントの弾性変形量を考慮したアクチュエータ指令値の補正を、少ない計算量で実行可能とすることを目的としたものである。

上記目的を達成するために、請求項１に記載の発明は、外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続するエンドエフェクタと、アクチュエータ指令値を与えて各アクチュエータを制御する制御装置を備えたパラレルメカニズム機械の制御方法であって、パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値および姿勢指令値に対する各アクチュエータ指令値を求める第１ステップと、各ストラット、各アクチュエータ、各第１ユニバーサルジョイント、各第２ユニバーサルジョイント、またはエンドエフェクタのうち少なくともいずれかの自重による荷重を求める第２ステップと、
第２ステップで求めた荷重と、機構パラメータならびにエンドエフェクタの位置指令値および姿勢指令値とから、各ストラット軸方向荷重および各アクチュエータ軸方向荷重を求める第３ステップと、第３ステップで求めた各ストラット軸方向荷重につき、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの構成要素の回転軸のいずれかと方向が同一の軸を含む軸座標系を規定する各ユニバーサルジョイント軸の各方向に分解する第４ステップと、第３ステップで求めた、各ストラット軸方向荷重および各アクチュエータ軸方向荷重と、各方向における各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかのコンプライアンスとから、各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかの弾性変形量を求めるとともに、各ユニバーサルジョイント軸の各方向におけるコンプライアンスと、第４ステップにおいて分解された各ストラット軸方向荷重とから、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの弾性変形量である各第１弾性変形量および／または各第２弾性変形量を求める第５ステップと、第５ステップで求めた各弾性変形量、ならびに各第１弾性変形量および／または各第２弾性変形量を、各アクチュエータ軸方向および各ストラット軸方向の角度を考慮して各アクチュエータ軸方向に換算する第６ステップと、第１ステップで求めた各アクチュエータ指令値を、第６ステップで求めた、換算された各弾性変形量、ならびに換算された各第１弾性変形量および／または換算された各第２弾性変形量を加味して更新する第７ステップとを有することを特徴とするものである。

請求項２に記載の発明は、上記目的に加えて、素早さを犠牲にせず更なる精度向上を図る目的を達成するため、更に、第２ステップにおいて、各ストラット、各アクチュエータ、または各第２ユニバーサルジョイントのうち少なくともいずれかの自重による荷重を、各ストラット軸方向荷重と各ストラット軸直角方向荷重とに分解し、求めた各ストラット軸直角方向荷重を各第１ユニバーサルジョイントの回転中心周りのモーメント荷重として、エンドエフェクタに働く荷重に加味することを特徴とするものである。

請求項３に記載の発明は、上記目的に加えて、ユニバーサルジョイントが取付けられる要素に注目して高精度の補正を素早く行うようにする目的を達成するため、更に、第４ステップにおいて、各ユニバーサルジョイント軸を、第１ユニバーサルジョイントについてはベースに固定し、第２ユニバーサルジョイントについてはエンドエフェクタに固定することを特徴とするものである。

請求項４に記載の発明は、上記目的に加えて、ユニバーサルジョイントに及ぼされる弾性変形による様々な方向の変位を素早く高精度にアクチュエータ指令値に換算する目的を達成するため、更に、第６ステップにおいて、各第１弾性変形量および／または各第２弾性変形量の変形方向に沿った各第１弾性変形量ベクトルおよび／または各第２弾性変形量ベクトルと、各アクチュエータ軸に沿った各アクチュエータ軸ベクトルの位置関係に基づいて、各第１弾性変形量および／または各第２弾性変形量を各アクチュエータ軸方向に近似することを特徴とするものである。

上記目的を達成するために、請求項５に記載の発明は、外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続するエンドエフェクタとを備えるパラレルメカニズム機械を、アクチュエータ指令値を与えることで制御する制御装置であって、パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値および姿勢指令値に対する各アクチュエータ指令値を求めるアクチュエータ指令値演算部と、各ストラット、各アクチュエータ、各第１ユニバーサルジョイント、各第２ユニバーサルジョイント、またはエンドエフェクタのうち少なくともいずれかの自重による荷重を求める荷重演算部と、荷重演算部で求めた荷重と、機構パラメータならびにエンドエフェクタの位置指令値および姿勢指令値とから、各ストラット軸方向荷重および各アクチュエータ軸方向荷重を求める軸方向荷重演算部と、軸方向荷重演算部で求めた各ストラット軸方向荷重につき、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの構成要素の回転軸のいずれかと方向が同一の軸を含む軸座標系を規定する各ユニバーサルジョイント軸の各方向に分解する分解演算部と、軸方向荷重演算部で求めた、各ストラット軸方向荷重および各アクチュエータ軸方向荷重と、各方向における各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかのコンプライアンスとから、各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかの弾性変形量を求めるとともに、各ユニバーサルジョイント軸の各方向におけるコンプライアンスと、分解演算部において分解された各ストラット軸方向荷重とから、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの弾性変形量である各第１弾性変形量および／または各第２弾性変形量を求める弾性変形量演算部と、弾性変形量演算部で求めた各弾性変形量、ならびに各第１弾性変形量および／または各第２弾性変形量を、アクチュエータ軸方向およびストラット軸方向の角度を考慮して各アクチュエータ軸方向に換算する換算演算部と、アクチュエータ指令値演算部で求めた各アクチュエータ指令値を、換算演算部で求めた、換算された各弾性変形量、ならびに換算された各第１弾性変形量および／または換算された各第２弾性変形量を加味して更新する更新演算部とを有することを特徴とするものである。

請求項６に記載の発明は、上記目的に加えて、素早さを犠牲にせず更なる精度向上を図る目的を達成するため、更に、荷重演算部において、各ストラット、各アクチュエータ、または各第２ユニバーサルジョイントのうち少なくともいずれかの自重による荷重を、各ストラット軸方向荷重と各ストラット軸直角方向荷重とに分解し、求めた各ストラット軸直角方向荷重を各第１ユニバーサルジョイントの回転中心周りのモーメント荷重として、エンドエフェクタに働く荷重に加味することを特徴とするものである。

請求項７に記載の発明は、上記目的に加えて、ユニバーサルジョイントが取付けられる要素に注目して高精度の補正を素早く行うようにする目的を達成するため、更に、分解演算部において、各ユニバーサルジョイント軸を、第１ユニバーサルジョイントについてはベースに固定し、第２ユニバーサルジョイントについてはエンドエフェクタに固定することを特徴とするものである。

請求項８に記載の発明は、上記目的に加えて、ユニバーサルジョイントに及ぼされる弾性変形による様々な方向の変位を素早く高精度にアクチュエータ指令値に換算する目的を達成するため、更に、換算演算部において、各第１弾性変形量および／または各第２弾性変形量の変形方向に沿った各第１弾性変形量ベクトルおよび／または各第２弾性変形量ベクトルと、各アクチュエータ軸に沿った各アクチュエータ軸ベクトルの位置関係に基づいて、各第１弾性変形量および／または各第２弾性変形量を各アクチュエータ軸方向に近似することを特徴とするものである。

本発明によれば、ユニバーサルジョイントの弾性変形量をも考慮したアクチュエータ指令値の高精度な補正を、計算量が少なく、一般的な処理能力の制御装置においても素早く計算できる状態で実行できる、という効果を奏する。

以下、本発明の実施形態に係るパラレルメカニズム機械につき、適宜図面に基づいて説明する。

図１は６自由度スチュワートプラットフォーム型のパラレルメカニズム機械１の説明図、図２はパラレルメカニズム機械１の制御装置１０のブロック図であって、パラレルメカニズム機械１は、床に固定されるベースとしてのフレーム１と、フレーム１に固定される６個の第１ユニバーサルジョイント２ａ〜２ｆと、各第１ユニバーサルジョイント２ａ〜２ｆに接続されるアクチュエータとしてのサーボモータ３ａ〜３ｆと、各サーボモータ３ａ〜３ｆにより駆動されるストラットとしてのボールねじ４ａ〜４ｆと、各ボールねじ４ａ〜４ｆの下端に接続される第２ユニバーサルジョイント５ａ〜５ｆと、第２ユニバーサルジョイント５ａ〜５ｆを有する１個のエンドエフェクタ６と、エンドエフェクタ６の対向位置においてフレーム１に固定されたテーブル７と、各サーボモータ３ａ〜３ｆに電気的に接続されアクチュエータ指令値を与える制御装置１０を備えている。

したがって、パラレルメカニズム機械１は、第１ユニバーサルジョイント２ａと、サーボモータ３ａと、ボールねじ４ａと、第２ユニバーサルジョイント５ａとからなる機構や、同様の機構を、合計６組備えている。以下、各組において同様である場合、主に記号のうちａ〜ｆを除いて示す代表例で説明する。

サーボモータ３は、基準状態からの回転角度ないし回転数に対応した数値を出力するエンコーダを備えている。エンドエフェクタ６は、下面に、刃具取付部および刃具回転機構を備えている。テーブル７は、加工物を固定する機構を備えている。

更に、第２ユニバーサルジョイント５は、図３に示すように、それぞれが回転１自由度のジョイントであるフランジ５０およびシャフト５１と、シャフト５１，５２と、ロッド５３とを含む。フランジ５０は、エンドエフェクタ６の下面と直交する側面に垂直に固定される。シャフト５１は、軸受を介してフランジ５０に取付けられており、エンドエフェクタ６の側面に垂直な回転軸Ｐ_ａ周りに回転する。シャフト５２は、軸受を介してシャフト５１に取付けられており、回転軸Ｐ_ｂ周りに回転する。ロッド５３は、シャフト５２に取付けられており、回転軸Ｐ_ｃ周りに回転する軸受を内包していて、この軸受にボールねじ４が接続される。回転軸Ｐ_ａ〜Ｐ_ｃはそれぞれ互いに直交し、３個のジョイントの組合わせで３自由度の第２ユニバーサルジョイント５を形成している。

加えて、第１ユニバーサルジョイント２は、図４に示すように、それぞれが回転１自由度のジョイントであるリング２１，２２、ならびにリング２２，２３を含むジンバル構造となっている。リング２１はフレーム１に固定され、放射方向の軸を受ける軸受を内側に内包する。また、この軸を有するリング２２が、軸受を介してリング２１に取付けられ、回転軸Ｑａ周りに回転するようになっている。更に、同様にリング２３は、軸をリング２２の軸と直交する状態でリング２２に取付けられ、回転軸Ｑｂ周りに回転する。回転軸Ｑａ，Ｑｂはそれぞれ互いに直交し、２個のジョイントの組合わせで２自由度の第１ユニバーサルジョイント２を形成している。

また、制御装置１０は、ＣＰＵ１１と、ＣＰＵ１１が制御のために実行するプログラムの各ステップ等を一時的に記憶するメモリ１２と、各種情報を表示する表示部１３と、各種入力を受付ける入力部１４と、プログラム等を記憶する記憶手段１５と、これらとＣＰＵ１１とのインターフェイス１６と、アクチュエータ３とＣＰＵ１１とのインターフェイス１７と、アクチュエータ３毎のドライブユニット１８とを備えている。記憶手段１５には、プログラムの一部としての、アクチュエータ指令値演算部３０、荷重演算部３１、軸方向荷重演算部３２、分解演算部３３、弾性変形量演算部３４、換算演算部３５、および更新演算部３６が記憶されている。

このようになるパラレルメカニズム機械１の制御装置１０が実行する制御方法に係るフローチャートを図５に示す。第１ステップとしてのステップＳ１において、制御装置１０のアクチュエータ指令値演算部３０は、与えられたエンドエフェクタ６の位置および姿勢の指令値を、アクチュエータ指令値に、逆機構変換によって変換する。ここで、パラレルメカニズム機械１におけるストラット長は、第１ないし第２ユニバーサルジョイント２，５の回転中心間の距離である。また、パラレルメカニズム機械１において、アクチュエータ指令値はエンコーダ出力値と同様になり、アクチュエータ軸方向とストラット軸方向は同一である。そして、ボールねじ４は、ストラット長がサーボモータ３の回転角度ないし回転数（アクチュエータ指令値）に比例するように増減される。したがって、アクチュエータ指令値とストラット長指令値とは、エンコーダの基準位置におけるストラット長（ストラット基準長）を基準として完全に比例関係にあるため、ストラット長指令値をアクチュエータ指令値とみなして計算に用いることができる。

パラレルメカニズム機械１における機構パラメータは、第１ユニバーサルジョイント２の回転中心座標Ｑ_ｉ（ｉ：１〜６、順に記号ａ〜ｆに係る部材に対応、以下同様）、エンドエフェクタ６に係る座標系での各第２ユニバーサルジョイント５の回転中心座標Ｒ_ｉ、ストラット基準長ｌ_Ｂｉである。制御装置１０は、エンドエフェクタ６の位置指令値（ｘ，ｙ，ｚ）および姿勢指令値（ａ，ｂ，ｃ）をまとめた指令値Ｔ（ｘ，ｙ，ｚ，ａ，ｂ，ｃ）から、下記［数１］によって、ストラット長指令値ｌ_ｉを求める。ここで、ＥはＴに関する平行移動および回転移動を合成した行列である。

次に、第２ステップとしてのステップＳ２において、制御装置１０の荷重演算部３１はパラレルメカニズム機械１の各要素の自重による荷重を計算する。サーボモータ３は、自重ｍ_ＳＭｉを有するとすると、図６に示すように、自重ｍ_ＳＭiによって第１ユニバーサルジョイント２の回転中心周りのモーメント荷重を発生し、エンドエフェクタ６に荷重Ｆ_ＳＭiとして働く。荷重Ｆ_ＳＭｉは下記［数２］で表される。ここで、Ｇは（０，０，−ｇ）というベクトルで、ｇは重力加速度であり、ｌ_ＳＭｉは第１ユニバーサルジョイント２の回転中心とサーボモータ３の重心までの距離であり、ｌ_ｉはストラット長指令値であり、Ｕ_ｉはボールねじ４の軸方向（ストラット軸方向）の単位ベクトルである。

また、第２ユニバーサルジョイント５のロッド５３は軸受格納部が大きく、その自重ｍ_ＰＣｉの影響は無視できない。この自重ｍ_ＰＣｉにより、図７に示すように、第１ユニバーサルジョイント２の回転中心周りのモーメント荷重が発生し、エンドエフェクタ６に荷重Ｆ_ＰＣｉとして働く。荷重Ｆ_ＰＣｉは下記［数３］で表される。ここで、ｌ_ＰＣＧは、第２ユニバーサルジョイント５の回転中心からロッド５３の重心までの距離である。

更に、ロッド５３の自重ｍ_ＰＣｉがストラット軸方向に及ぼす荷重Ｗ_ＰＣｉは次の［数４］となる。

加えて、ボールねじ４の自重ｍ_ＢＳｉにより、図８に示すように、第１ユニバーサルジョイント２の回転中心周りのモーメント荷重が発生し、エンドエフェクタ６に荷重Ｆ_ＢＳｉとして働く。そして、荷重Ｆ_ＢＳｉは下記［数５］で表される。ここで、ｌ_ＢＳＷｉはボールねじ４の全長であり、ｌ_ＰＣＬｉは第２ユニバーサルジョイント５とロッド−ボールねじ４接続部との間の距離である。

また、ボールねじ４の自重ｍ_ＢＳｉがストラット軸方向に及ぼす荷重Ｗ_ＢＳｉは次の［数６］となる。

続いて、第３ステップとしてのステップＳ３において、制御装置１０の軸方向荷重演算部３２はボールねじ４やサーボモータ３のストラット軸方向に働く荷重の計算をする。エンドエフェクタ６には重力や外力の他にステップＳ２で求めた荷重が働く。したがって、それらを合計した荷重Ｆ_ｘｙｚとモーメント荷重Ｎ_ａｂｃがエンドエフェクタ６の重心に働く。ここで、荷重Ｆ_ｘｙｚとモーメント荷重Ｎ_ａｂｃを合わせて荷重ベクトルＦとし次の［数７］として表す。

パラレルメカニズム機械１における機構パラメータ、エンドエフェクタ６の位置および姿勢の指令値から公知の方式により決定される６行６列のヤコビアンマトリックスＪと、各ストラット軸方向の荷重ｗ_ｉからなる荷重ベクトルＷおよび上記荷重ベクトルＦとには下記［数８］の関係があるため、ヤコビアンマトリックスＪの逆行列Ｊ^−１を求めれば、下記［数９］となり、荷重ベクトルＷが求まり、各ストラット軸方向の荷重ｗ_ｉが求まる。なお、荷重ベクトルＷは、下記［数１０］である。

更に、第４ステップとしてのステップＳ４において、制御装置１０の弾性変形量演算部３４は、上記のように算出したストラット軸方向の荷重を、両ユニバーサルジョイント２，５における個々のジョイントの軸方向（ユニバーサルジョイント軸の方向）に分解する演算を行う。図１０は第２ユニバーサルジョイント５のモデルであり、回転軸Ｐ_ａ〜Ｐ_ｃとは別に、エンドエフェクタ６に固定されたユニバーサルジョイント軸Ｐ_Ｉ，Ｐ_Ｊ，Ｐ_Ｋを定義する。ユニバーサルジョイント軸Ｐ_Ｉ〜Ｐ_Ｋは同一直交点において互いに直交して直交座標系を規定し、その直交点は回転軸Ｐ_ａ〜Ｐ_ｃの直交点と同一であるが、図中においてはわかりやすいようにずらして図示している。また、回転軸Ｐ_ａとユニバーサルジョイント軸Ｐ_Ｋを同一方向にとり、エンドエフェクタ６の下面と平行にユニバーサルジョイント軸Ｐ_Ｊをとり、エンドエフェクタ６の下面と垂直にユニバーサルジョイント軸Ｐ_Ｉをとる。

算出した各ストラット軸方向の荷重は、第２ユニバーサルジョイント５に働く。制御装置１０は、この荷重を、第２ユニバーサルジョイント５が取付けられるエンドエフェクタ６に固定されたユニバーサルジョイント軸Ｐ_Ｉ〜Ｐ_Ｋの各方向に分解する。この分解は、下記［数１１］によって行われる。ここで、ｗ_ＰＩｉ，ｗ_ＰＪｉ，ｗ_ＰＫｉは順にユニバーサルジョイント軸Ｐ_Ｉ，Ｐ_Ｊ，Ｐ_Ｋ方向に分解された荷重であり、Ｐ_Ｉｉ，Ｐ_Ｊｉ，Ｐ_Ｋｉは順にユニバーサルジョイント軸Ｐ_Ｉ，Ｐ_Ｊ，Ｐ_Ｋの単位ベクトルである。なお、ユニバーサルジョイント軸Ｐ_Ｉ，Ｐ_Ｊ，Ｐ_Ｋは、エンドエフェクタ６の位置および姿勢により変化し、第２ユニバーサルジョイント５のエンドエフェクタ６における取付面毎（取付角度毎）に異なるものとなる。

第１ユニバーサルジョイント２については、第２ユニバーサルジョイント５と同様に各ストラット軸方向荷重を各ユニバーサルジョイント軸方向に分解できる。ただし、ユニバーサルジョイント軸として、第１ユニバーサルジョイント２が取付けられるフレーム１に固定したものを用いる。第１ユニバーサルジョイント２にかかる荷重ｗ_ｉ’は次の［数１２］となる。

続いて、第５ステップとしてのステップＳ５において、制御装置１０の弾性変形量演算部３４は、パラレルメカニズム機械１における各要素の弾性変形量を算出する。制御装置１０はまず、各ストラット軸方向（アクチュエータ軸方向）の各要素の弾性変形量を推定する。各ストラット軸方向の各要素には、ステップＳ２で求めた荷重Ｗ_ＢＳｉ，Ｗ_ＰＣｉ、およびステップＳ３で求めたストラット軸方向の荷重ｗ_ｉが働く。

そして、制御装置１０は、図９に示すように、各要素の位置関係によりかかる荷重が異なることを考慮した上で、各要素のストラット軸方向のコンプライアンス（剛性の逆数）と各荷重から、下記［数１３］によって、第３弾性変形量としての各ストラット軸方向の弾性変形量Δｌ_ｉを求めることができる。ここで、ｌ_ｉ’はエンドエフェクタ６の位置および姿勢の指令値により決定されるストラット長のうち、ボールねじ４が占める長さであり、ｃ_ＬＯｉ，ｃ_ＬＡｉ，ｃ_ＬＢｉは、順に、ストラット長によって変化する、ボールねじ４の部材のコンプライアンス、ボールねじ４の部材のコンプライアンス、ボールねじ４の自重による自身のひずみの換算されたコンプライアンスであり、ｃ_ＭＯｉ，ｃ_ＭＡｉ，ｃ_ＭＢｉは、順に、ストラット長に依存しない、フランジ５２，ロッド５３，各種軸受，ボールねじ４のナットおよびリング２２，２３のコンプライアンス、各種軸受，ボールねじ４のナット，リング２２，２３のコンプライアンス、各種軸受，ボールねじ４のナット，リング２２，２３のコンプライアンスである（一部省略）。

次に、制御装置１０は、第２ユニバーサルジョイント５の弾性変形量を推定する。第２ユニバーサルジョイント５の各要素のコンプライアンスは、一般に、要素中の各軸受についての軸方向（アキシャル方向）とこれに垂直な方向（ラジアル方向）とに分けて与えられている。図１０に示されるように、ユニバーサルジョイント軸Ｐ_Ｉ〜Ｐ_Ｋを考えれば、シャフト５１に係る軸受のアキシャル方向はユニバーサルジョイント軸Ｐ_Ｋ方向であり、ラジアル方向はユニバーサルジョイント軸Ｐ_Ｉ，Ｐ_Ｊの合成方向となる。なお、シャフト５２、ロッド５３に係る軸受については、ストラット軸方向荷重を考慮する必要がなく、あるいはストラット軸方向荷重しか働かないので、第１ないし第２ユニバーサルジョイントの弾性変形量（第１ないし第２弾性変形量）を
算出するためのストラット軸方向荷重の分解をしない。

したがって、各軸受についてのコンプライアンスをユニバーサルジョイント軸Ｐ_Ｉ〜Ｐ_Ｋ方向に分解したものは、所定のコンプライアンスから容易に得られる。また、ユニバーサルジョイント軸Ｐ_Ｉ〜Ｐ_Ｋ方向に分解したコンプライアンスと、ステップＳ４で求めたユニバーサルジョイント軸Ｐ_Ｉ〜Ｐ_Ｋ方向の荷重から、第２ユニバーサルジョイント５の弾性変形量が求められる。ただし、第２ユニバーサルジョイント５はエンドエフェクタ６に固定されており、第２ユニバーサルジョイント５に働く荷重によるエンドエフェクタ６の変形の影響を受けるため、この影響を線形近似して加味する。よって、次の［数１４］により、第２弾性変形量としての第２ユニバーサルジョイント５の弾性変形量ΔＰ_ｉが推定される。ここで、ｃ_ＰＩｉ，ｃ_ＰＪｉ，ｃ_ＰＫｉはそれぞれユニバーサルジョイント軸Ｐ_Ｉ，Ｐ_Ｊ，Ｐ_Ｋ方向のコンプライアンスであり、ｊはｉ番目のボールねじ４等（ストラット）の隣の軸番号であり、ｋ_Ｐｊは比例定数としての隣接影響係数である。

一方、第１弾性変形量としての、第１ユニバーサルジョイント２の弾性変形量も第２ユニバーサルジョイント５と同様に求めることができる。ただし、隣接に影響を与えるのはフレーム１である。即ち、第１ユニバーサルジョイント２のうちの一つにかかる荷重が、フレーム１における、隣接する第１ユニバーサルジョイント２の取付部分に影響を与え変形させる。このとき、フレーム１に固定した方向ユニバーサルジョイント軸Ｑ_ｉ〜Ｑ_ｋのうち、ユニバーサルジョイント軸Ｑ_ｋ方向への影響が大きく、他の方向は無視できるので、［数１４］においてユニバーサルジョイント軸Ｑ_ｋ方向のみに隣接影響係数を適用している。

続いて、第６ステップとしてのステップＳ６において、制御装置１０の換算演算部３５は、両ユニバーサルジョイントが弾性変形することによる変位を、アクチュエータ軸方向に換算する。即ち、ステップＳ５で求めた第１および第２ユニバーサルジョイント２，５の弾性変形を、各ユニバーサルジョイント２，５の回転中心の移動として捉える。図１１は第１，第２ユニバーサルジョイント２，５の回転中心変位のベクトルΔＰ_ｉ，ΔＱ_ｉとストラット軸ベクトルＬ_ｉの関係を示している。ここで、Ｑ_ｉ，Ｐ_ｉはそれぞれ弾性変形前の第１，第２ユニバーサルジョイントの回転中心であり、Ｑ_ｉ’，Ｐ_ｉ’はそれぞれ弾性変形後の第１、第２ユニバーサルジョイントの回転中心であり、Ｌ_ｉ’は弾性変形後のストラット軸ベクトルである。この関係より、各ユニバーサルジョイント回転中心の変位ベクトルとストラット軸ベクトルの関係は、次の［数１５］で表すことができる。

更に、両ユニバーサルジョイント２，５の変位は微少であることから、ストラット軸長さ変位に近似することができる。即ち、回転中心変位ベクトルΔＰ_ｉ，ΔＱ_ｉのストラット軸方向成分をストラット軸長さ変位Δｌ_Ｊｉに近似する観点から、次の［数１６］によって、各ユニバーサルジョイント２，５の変位をストラット軸長さ変位Δｌ_Ｊｉに変換することができる。なお、パラレルメカニズム機械１において、ストラット軸方向（ボールねじ４の方向）とアクチュエータ軸方向（サーボモータ３の軸方向）は同一方向であるから、ストラット軸長さ変位Δｌ_Ｊｉをもってアクチュエータ軸方向に換算した変位とすることができる。

そして、第７ステップとしてのステップＳ７において、制御装置１０の更新演算部３６は、アクチュエータ指令値を更新する。即ち、下記［数１７］に示すように、ステップＳ５で求めた各ストラット長の変位Δｌ_ｉ（第３変形量）と、ステップＳ６で求めたストラット長変位Δｌ_Ｊｉ（第１および第２変形量）を減算してストラット長指令値、即ちアクチュエータ指令値を補正する。

この補正されたアクチュエータ指令値を元にサーボモータ３を制御することにより、実際の機械の弾性変形量が相殺される。即ち、ストラットに関する弾性変形と、ユニバーサルジョイントに関する弾性変形とを、当初のアクチュエータ指令値に補正値を与えることのみによって相殺することができる。これにより、少ない計算量で、エンドエフェクタ６の位置および姿勢の指令値通りの位置および姿勢にエンドエフェクタが位置決めされるようになる。

図１２はパラレルメカニズム機械１を用いて、Ｄｏｕｂｌｅ Ｂａｌｌ Ｂａｒ（ＤＢＢ）法による円弧軌跡の真円度を測定した結果を示している。太線が本発明に係る制御方法を用いた場合であり、細線がいずれの補正制御方法をも用いない場合である。真円度が大幅に向上していることが確認できる。

なお、本発明は、ストラット軸とアクチュエータ軸とが同一（平行）でないパラレルメカニズム機械にも用いることができる。この場合、上記ステップＳ３においてアクチュエータ軸方向に働く荷重を算出し、更にストラット軸方向成分を求め、ステップＳ４において当該ストラット軸方向成分を加味した荷重をユニバーサルジョイントの要素に基づく各方向に分解し、ステップＳ６においてユニバーサルジョイントによるストラットの変位に対応するアクチュエータ変位量を求めることもできる。

加えて、ユニバーサルジョイント軸について、例えばユニバーサルジョイント軸Ｐ_ｉ〜Ｐ_ｋをシャフト５１上に固定し、回転軸Ｐ_ｂとユニバーサルジョイント軸Ｐ_ｊを一致させれば、シャフト５１に働く荷重をラジアル方向ないしアキシャル方向に分解でき、上記実施例では影響が少ないとして省略したシャフト５２のアキシャル方向の荷重を簡単に計算することができる。あるいは、ユニバーサルジョイント軸Ｐ_ｉ〜Ｐ_ｊを、極座標系を規定するものとすることもできる。

また、本発明により制御される機械は空間６自由度パラレルメカニズム工作機械に限るものではない。その自由度は６自由度に限るものではなくそれ以上もしくは以下の自由度でもよい。また、ユニバーサルジョイントは回転１自由度ジョイントの組み合わせたものに限らず、球面継手などその他の構造のものでもよい。さらにまた、機械として刃具を備えた工作機械に限らず、ロボット、産業機械、アミューズメント機械、建築機械等でもよい。

本発明の制御方法により制御されるパラレルメカニズム機械の一部透視斜視図である。 本発明に係る制御装置のブロック図である。 図１における第２ユニバーサルジョイントの斜視図である。 図１における第１ユニバーサルジョイントの斜視図である。 本発明の制御方法に係る実施形態のフローチャートである。 図１におけるサーボモータの自重が及ぼす荷重の説明図である。 図１におけるロッドの自重が及ぼす荷重の説明図である。 図１におけるボールねじの自重が及ぼす荷重の説明図である。 図１におけるボールねじの軸方向の荷重の説明図である。 図１における第２ユニバーサルジョイントに係る回転軸ないしユニバーサルジョイント軸の説明図である。 図１における第１および第２ユニバーサルジョイントに係る回転中心変位とストラット軸ベクトルの関係の説明図である。 図１のパラレルメカニズム機械において本発明を適用した場合と適用しない場合のＤＢＢ法による真円度測定結果の説明図である。

符号の説明

１ フレーム
２ 第１ユニバーサルジョイント
３ サーボモータ（アクチュエータ）
４ ボールねじ（ストラット）
５ 第２ユニバーサルジョイント
６ エンドエフェクタ
７ テーブル

Claims (8)

1. 外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続するエンドエフェクタと、アクチュエータ指令値を与えて各アクチュエータを制御する制御装置を備えたパラレルメカニズム機械の制御方法であって、
   パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値および姿勢指令値に対する各アクチュエータ指令値を求める第１ステップと、
   各ストラット、各アクチュエータ、各第１ユニバーサルジョイント、各第２ユニバーサルジョイント、またはエンドエフェクタのうち少なくともいずれかの自重による荷重を求める第２ステップと、
   第２ステップで求めた荷重と、機構パラメータならびにエンドエフェクタの位置指令値および姿勢指令値とから、各ストラット軸方向荷重および各アクチュエータ軸方向荷重を求める第３ステップと、
   第３ステップで求めた各ストラット軸方向荷重につき、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの構成要素の回転軸のいずれかと方向が同一の軸を含む軸座標系を規定する各ユニバーサルジョイント軸の各方向に分解する第４ステップと、
   第３ステップで求めた、各ストラット軸方向荷重および各アクチュエータ軸方向荷重と、各方向における各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかのコンプライアンスとから、各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかの弾性変形量を求めるとともに、
   各ユニバーサルジョイント軸の各方向におけるコンプライアンスと、第４ステップにおいて分解された各ストラット軸方向荷重とから、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの弾性変形量である各第１弾性変形量および／または各第２弾性変形量を求める第５ステップと、
   第５ステップで求めた各弾性変形量、ならびに各第１弾性変形量および／または各第２弾性変形量を、各アクチュエータ軸方向および各ストラット軸方向の角度を考慮して各アクチュエータ軸方向に換算する第６ステップと、
   第１ステップで求めた各アクチュエータ指令値を、第６ステップで求めた、換算された各弾性変形量、ならびに換算された各第１弾性変形量および／または換算された各第２弾性変形量を加味して更新する第７ステップと
   を有することを特徴とするパラレルメカニズム機械の制御方法。
2. 第２ステップにおいて、各ストラット、各アクチュエータ、または各第２ユニバーサルジョイントのうち少なくともいずれかの自重による荷重を、各ストラット軸方向荷重と各ストラット軸直角方向荷重とに分解し、求めた各ストラット軸直角方向荷重を各第１ユニバーサルジョイントの回転中心周りのモーメント荷重として、エンドエフェクタに働く荷重に加味する
   ことを特徴とする請求項１に記載のパラレルメカニズム機械の制御方法。
3. 第４ステップにおいて、各ユニバーサルジョイント軸を、第１ユニバーサルジョイントについてはベースに固定し、第２ユニバーサルジョイントについてはエンドエフェクタに固定する
   ことを特徴とする請求項１または請求項２に記載のパラレルメカニズム機械の制御方法。
4. 第６ステップにおいて、各第１弾性変形量および／または各第２弾性変形量の変形方向に沿った各第１弾性変形量ベクトルおよび／または各第２弾性変形量ベクトルと、各アクチュエータ軸に沿った各アクチュエータ軸ベクトルの位置関係に基づいて、各第１弾性変形量および／または各第２弾性変形量を各アクチュエータ軸方向に近似する
   ことを特徴とする請求項１ないし請求項３のいずれかに記載のパラレルメカニズム機械の制御方法。
5. 外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続するエンドエフェクタとを備えるパラレルメカニズム機械を、アクチュエータ指令値を与えることで制御する制御装置であって、
   パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値および姿勢指令値に対する各アクチュエータ指令値を求めるアクチュエータ指令値演算部と、
   各ストラット、各アクチュエータ、各第１ユニバーサルジョイント、各第２ユニバーサルジョイント、またはエンドエフェクタのうち少なくともいずれかの自重による荷重を求める荷重演算部と、
   荷重演算部で求めた荷重と、機構パラメータならびにエンドエフェクタの位置指令値および姿勢指令値とから、各ストラット軸方向荷重および各アクチュエータ軸方向荷重を求める軸方向荷重演算部と、
   軸方向荷重演算部で求めた各ストラット軸方向荷重につき、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの構成要素の回転軸のいずれかと方向が同一の軸を含む軸座標系を規定する各ユニバーサルジョイント軸の各方向に分解する分解演算部と、
   軸方向荷重演算部で求めた、各ストラット軸方向荷重および各アクチュエータ軸方向荷重と、各方向における各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかのコンプライアンスとから、各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかの弾性変形量を求めるとともに、
   各ユニバーサルジョイント軸の各方向におけるコンプライアンスと、分解演算部において分解された各ストラット軸方向荷重とから、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの弾性変形量である各第１弾性変形量および／または各第２弾性変形量を求める弾性変形量演算部と、
   弾性変形量演算部で求めた各弾性変形量、ならびに各第１弾性変形量および／または各第２弾性変形量を、アクチュエータ軸方向およびストラット軸方向の角度を考慮して各アクチュエータ軸方向に換算する換算演算部と、
   アクチュエータ指令値演算部で求めた各アクチュエータ指令値を、換算演算部で求めた、換算された各弾性変形量、ならびに換算された各第１弾性変形量および／または換算された各第２弾性変形量を加味して更新する更新演算部と
   を有することを特徴とするパラレルメカニズム機械の制御装置。
6. 荷重演算部において、各ストラット、各アクチュエータ、または各第２ユニバーサルジョイントのうち少なくともいずれかの自重による荷重を、各ストラット軸方向荷重と各ストラット軸直角方向荷重とに分解し、求めた各ストラット軸直角方向荷重を各第１ユニバーサルジョイントの回転中心周りのモーメント荷重として、エンドエフェクタに働く荷重に加味する
   ことを特徴とする請求項５に記載のパラレルメカニズム機械の制御装置。
7. 分解演算部において、各ユニバーサルジョイント軸を、第１ユニバーサルジョイントについてはベースに固定し、第２ユニバーサルジョイントについてはエンドエフェクタに固定する
   ことを特徴とする請求項５または請求項６に記載のパラレルメカニズム機械の制御装置。
8. 換算演算部において、各第１弾性変形量および／または各第２弾性変形量の変形方向に沿った各第１弾性変形量ベクトルおよび／または各第２弾性変形量ベクトルと、各アクチュエータ軸に沿った各アクチュエータ軸ベクトルの位置関係に基づいて、各第１弾性変形量および／または各第２弾性変形量を各アクチュエータ軸方向に近似する
   ことを特徴とする請求項５ないし請求項７のいずれかに記載のパラレルメカニズム機械の制御装置。

Images