(57)【要約】      （修正有）【課題】ユニバーサルジョイントの弾性変形量を考慮した高精度なアクチュエータ指令値の補正を、少ない計算量で実行可能とするパラレルメカニズム機械の制御方法および制御装置を提供する。【解決手段】ステップＳ１により機構パラメータに基づいて各アクチュエータ指令値を求め、ステップＳ２により機械の各構成要素の自重による荷重を計算し、ステップＳ３により各アクチュエータ軸方向荷重を求め、ステップＳ４により各ストラット軸方向荷重を各第１，第２ユニバーサルジョイント軸方向に分解し、ステップＳ５により各要素の弾性変形量を求め、ステップＳ６により各ユニバーサルジョイントの弾性変形量を各アクチュエータ軸方向変位に近似換算し、ステップＳ７により各アクチュエータ指令値をＳ５，Ｓ６で求めた各アクチュエータ軸方向変位で減算して更新し、更新された各アクチュエータ指令値を基に制御する。【選択図】図５
2005186210.tif 000002
【特許請求の範囲】
【請求項１】
外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続するエンドエフェクタと、アクチュエータ指令値を与えて各アクチュエータを制御する制御装置を備えたパラレルメカニズム機械の制御方法であって、
パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値および姿勢指令値に対する各アクチュエータ指令値を求める第１ステップと、
各ストラット、各アクチュエータ、各第１ユニバーサルジョイント、各第２ユニバーサルジョイント、またはエンドエフェクタのうち少なくともいずれかの自重による荷重を求める第２ステップと、
第２ステップで求めた荷重と、機構パラメータならびにエンドエフェクタの位置指令値および姿勢指令値とから、各ストラット軸方向荷重および各アクチュエータ軸方向荷重を求める第３ステップと、
第３ステップで求めた各ストラット軸方向荷重につき、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの構成要素の回転軸のいずれかと方向が同一の軸を含む軸座標系を規定する各ユニバーサルジョイント軸の各方向に分解する第４ステップと、
第３ステップで求めた、各ストラット軸方向荷重および各アクチュエータ軸方向荷重と、各方向における各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかのコンプライアンスとから、各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかの弾性変形量を求めるとともに、
各ユニバーサルジョイント軸の各方向におけるコンプライアンスと、第４ステップにおいて分解された各ストラット軸方向荷重とから、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの弾性変形量である各第１弾性変形量および／または各第２弾性変形量を求める第５ステップと、
第５ステップで求めた各弾性変形量、ならびに各第１弾性変形量および／または各第２弾性変形量を、各アクチュエータ軸方向および各ストラット軸方向の角度を考慮して各アクチュエータ軸方向に換算する第６ステップと、
第１ステップで求めた各アクチュエータ指令値を、第６ステップで求めた、換算された各弾性変形量、ならびに換算された各第１弾性変形量および／または換算された各第２弾性変形量を加味して更新する第７ステップと
を有することを特徴とするパラレルメカニズム機械の制御方法。
【請求項２】
第２ステップにおいて、各ストラット、各アクチュエータ、または各第２ユニバーサルジョイントのうち少なくともいずれかの自重による荷重を、各ストラット軸方向荷重と各ストラット軸直角方向荷重とに分解し、求めた各ストラット軸直角方向荷重を各第１ユニバーサルジョイントの回転中心周りのモーメント荷重として、エンドエフェクタに働く荷重に加味する
ことを特徴とする請求項１に記載のパラレルメカニズム機械の制御方法。
【請求項３】
第４ステップにおいて、各ユニバーサルジョイント軸を、第１ユニバーサルジョイントについてはベースに固定し、第２ユニバーサルジョイントについてはエンドエフェクタに固定する
ことを特徴とする請求項１または請求項２に記載のパラレルメカニズム機械の制御方法。
【請求項４】
第６ステップにおいて、各第１弾性変形量および／または各第２弾性変形量の変形方向に沿った各第１弾性変形量ベクトルおよび／または各第２弾性変形量ベクトルと、各アクチュエータ軸に沿った各アクチュエータ軸ベクトルの位置関係に基づいて、各第１弾性変形量および／または各第２弾性変形量を各アクチュエータ軸方向に近似する
ことを特徴とする請求項１ないし請求項３のいずれかに記載のパラレルメカニズム機械の制御方法。
【請求項５】
外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続するエンドエフェクタとを備えるパラレルメカニズム機械を、アクチュエータ指令値を与えることで制御する制御装置であって、
パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値および姿勢指令値に対する各アクチュエータ指令値を求めるアクチュエータ指令値演算部と、
各ストラット、各アクチュエータ、各第１ユニバーサルジョイント、各第２ユニバーサルジョイント、またはエンドエフェクタのうち少なくともいずれかの自重による荷重を求める荷重演算部と、
荷重演算部で求めた荷重と、機構パラメータならびにエンドエフェクタの位置指令値および姿勢指令値とから、各ストラット軸方向荷重および各アクチュエータ軸方向荷重を求める軸方向荷重演算部と、
軸方向荷重演算部で求めた各ストラット軸方向荷重につき、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの構成要素の回転軸のいずれかと方向が同一の軸を含む軸座標系を規定する各ユニバーサルジョイント軸の各方向に分解する分解演算部と、
軸方向荷重演算部で求めた、各ストラット軸方向荷重および各アクチュエータ軸方向荷重と、各方向における各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかのコンプライアンスとから、各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかの弾性変形量を求めるとともに、
各ユニバーサルジョイント軸の各方向におけるコンプライアンスと、分解演算部において分解された各ストラット軸方向荷重とから、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの弾性変形量である各第１弾性変形量および／または各第２弾性変形量を求める弾性変形量演算部と、
弾性変形量演算部で求めた各弾性変形量、ならびに各第１弾性変形量および／または各第２弾性変形量を、アクチュエータ軸方向およびストラット軸方向の角度を考慮して各アクチュエータ軸方向に換算する換算演算部と、
アクチュエータ指令値演算部で求めた各アクチュエータ指令値を、換算演算部で求めた、換算された各弾性変形量、ならびに換算された各第１弾性変形量および／または換算された各第２弾性変形量を加味して更新する更新演算部と
を有することを特徴とするパラレルメカニズム機械の制御装置。
【請求項６】
荷重演算部において、各ストラット、各アクチュエータ、または各第２ユニバーサルジョイントのうち少なくともいずれかの自重による荷重を、各ストラット軸方向荷重と各ストラット軸直角方向荷重とに分解し、求めた各ストラット軸直角方向荷重を各第１ユニバーサルジョイントの回転中心周りのモーメント荷重として、エンドエフェクタに働く荷重に加味する
ことを特徴とする請求項５に記載のパラレルメカニズム機械の制御装置。
【請求項７】
分解演算部において、各ユニバーサルジョイント軸を、第１ユニバーサルジョイントについてはベースに固定し、第２ユニバーサルジョイントについてはエンドエフェクタに固定する
ことを特徴とする請求項５または請求項６に記載のパラレルメカニズム機械の制御装置。
【請求項８】
換算演算部において、各第１弾性変形量および／または各第２弾性変形量の変形方向に沿った各第１弾性変形量ベクトルおよび／または各第２弾性変形量ベクトルと、各アクチュエータ軸に沿った各アクチュエータ軸ベクトルの位置関係に基づいて、各第１弾性変形量および／または各第２弾性変形量を各アクチュエータ軸方向に近似する
ことを特徴とする請求項５ないし請求項７のいずれかに記載のパラレルメカニズム機械の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、アクチュエータによって駆動される複数のストラットにより、これらに接続されたエンドエフェクタを移動可能なパラレルメカニズム機械の制御方法、およびこの方法を実行可能な制御装置に関する。
【背景技術】
【０００２】
従来のパラレルメカニズム機械の制御方法として、下記特許文献１に記載された方法が知られている。この方法は、ストラットとエンドエフェクタとが回転１自由度のジョイントを組合わせたユニバーサルジョイントで接続されている場合にあっても、各ジョイントの弾性変形量をも加味して各ストラットの長さに対応するアクチュエータ指令値を高精度に修正するものであり、エンドエフェクタの位置指令値および姿勢指令値と機構パラメータとから各ストラット長指令値を算出する逆機構変換を用いるものである。
【０００３】
具体的には、無負荷状態で逆機構変換を行って、修正前の各アクチュエータ指令値を求めた後、各ジョイントの角度を計算し、各ストラット等に作用する荷重からジョイントの構成要素に作用する荷重を求め、この荷重から各構成要素の弾性変形量を算出し、この弾性変形量に基づき機構パラメータを修正し、これを用い再び逆機構変換を行って修正された各アクチュエータ指令値を得る。
【０００４】
【特許文献１】特開２００２−９１５６８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
以上のような従来の方法では、各ジョイントの角度の計算、構成要素毎の荷重や弾性変形量の計算に加えて、逆機構変換を２度も行わなければならず、計算量が膨大になって、計算時間の長大化による制御性能低下を防ぐには、計算能力は極めて高いが高コストである制御装置を採用するしかない。
【０００６】
そこで、請求項１および請求項５に記載の発明は、エンドエフェクタの位置や姿勢を高精度に決定するための、ユニバーサルジョイントの弾性変形量を考慮したアクチュエータ指令値の補正を、少ない計算量で実行可能とすることを目的としたものである。
【課題を解決するための手段】
【０００７】
上記目的を達成するために、請求項１に記載の発明は、外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続するエンドエフェクタと、アクチュエータ指令値を与えて各アクチュエータを制御する制御装置を備えたパラレルメカニズム機械の制御方法であって、パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値および姿勢指令値に対する各アクチュエータ指令値を求める第１ステップと、各ストラット、各アクチュエータ、各第１ユニバーサルジョイント、各第２ユニバーサルジョイント、またはエンドエフェクタのうち少なくともいずれかの自重による荷重を求める第２ステップと、
第２ステップで求めた荷重と、機構パラメータならびにエンドエフェクタの位置指令値および姿勢指令値とから、各ストラット軸方向荷重および各アクチュエータ軸方向荷重を求める第３ステップと、第３ステップで求めた各ストラット軸方向荷重につき、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの構成要素の回転軸のいずれかと方向が同一の軸を含む軸座標系を規定する各ユニバーサルジョイント軸の各方向に分解する第４ステップと、第３ステップで求めた、各ストラット軸方向荷重および各アクチュエータ軸方向荷重と、各方向における各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかのコンプライアンスとから、各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかの弾性変形量を求めるとともに、各ユニバーサルジョイント軸の各方向におけるコンプライアンスと、第４ステップにおいて分解された各ストラット軸方向荷重とから、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの弾性変形量である各第１弾性変形量および／または各第２弾性変形量を求める第５ステップと、第５ステップで求めた各弾性変形量、ならびに各第１弾性変形量および／または各第２弾性変形量を、各アクチュエータ軸方向および各ストラット軸方向の角度を考慮して各アクチュエータ軸方向に換算する第６ステップと、第１ステップで求めた各アクチュエータ指令値を、第６ステップで求めた、換算された各弾性変形量、ならびに換算された各第１弾性変形量および／または換算された各第２弾性変形量を加味して更新する第７ステップとを有することを特徴とするものである。
【０００８】
請求項２に記載の発明は、上記目的に加えて、素早さを犠牲にせず更なる精度向上を図る目的を達成するため、更に、第２ステップにおいて、各ストラット、各アクチュエータ、または各第２ユニバーサルジョイントのうち少なくともいずれかの自重による荷重を、各ストラット軸方向荷重と各ストラット軸直角方向荷重とに分解し、求めた各ストラット軸直角方向荷重を各第１ユニバーサルジョイントの回転中心周りのモーメント荷重として、エンドエフェクタに働く荷重に加味することを特徴とするものである。
【０００９】
請求項３に記載の発明は、上記目的に加えて、ユニバーサルジョイントが取付けられる要素に注目して高精度の補正を素早く行うようにする目的を達成するため、更に、第４ステップにおいて、各ユニバーサルジョイント軸を、第１ユニバーサルジョイントについてはベースに固定し、第２ユニバーサルジョイントについてはエンドエフェクタに固定することを特徴とするものである。
【００１０】
請求項４に記載の発明は、上記目的に加えて、ユニバーサルジョイントに及ぼされる弾性変形による様々な方向の変位を素早く高精度にアクチュエータ指令値に換算する目的を達成するため、更に、第６ステップにおいて、各第１弾性変形量および／または各第２弾性変形量の変形方向に沿った各第１弾性変形量ベクトルおよび／または各第２弾性変形量ベクトルと、各アクチュエータ軸に沿った各アクチュエータ軸ベクトルの位置関係に基づいて、各第１弾性変形量および／または各第２弾性変形量を各アクチュエータ軸方向に近似することを特徴とするものである。
【００１１】
上記目的を達成するために、請求項５に記載の発明は、外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続するエンドエフェクタとを備えるパラレルメカニズム機械を、アクチュエータ指令値を与えることで制御する制御装置であって、パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値および姿勢指令値に対する各アクチュエータ指令値を求めるアクチュエータ指令値演算部と、各ストラット、各アクチュエータ、各第１ユニバーサルジョイント、各第２ユニバーサルジョイント、またはエンドエフェクタのうち少なくともいずれかの自重による荷重を求める荷重演算部と、荷重演算部で求めた荷重と、機構パラメータならびにエンドエフェクタの位置指令値および姿勢指令値とから、各ストラット軸方向荷重および各アクチュエータ軸方向荷重を求める軸方向荷重演算部と、軸方向荷重演算部で求めた各ストラット軸方向荷重につき、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの構成要素の回転軸のいずれかと方向が同一の軸を含む軸座標系を規定する各ユニバーサルジョイント軸の各方向に分解する分解演算部と、軸方向荷重演算部で求めた、各ストラット軸方向荷重および各アクチュエータ軸方向荷重と、各方向における各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかのコンプライアンスとから、各ストラット、各アクチュエータ、またはエンドエフェクタのうち少なくともいずれかの弾性変形量を求めるとともに、各ユニバーサルジョイント軸の各方向におけるコンプライアンスと、分解演算部において分解された各ストラット軸方向荷重とから、各第１ユニバーサルジョイントおよび／または各第２ユニバーサルジョイントの弾性変形量である各第１弾性変形量および／または各第２弾性変形量を求める弾性変形量演算部と、弾性変形量演算部で求めた各弾性変形量、ならびに各第１弾性変形量および／または各第２弾性変形量を、アクチュエータ軸方向およびストラット軸方向の角度を考慮して各アクチュエータ軸方向に換算する換算演算部と、アクチュエータ指令値演算部で求めた各アクチュエータ指令値を、換算演算部で求めた、換算された各弾性変形量、ならびに換算された各第１弾性変形量および／または換算された各第２弾性変形量を加味して更新する更新演算部とを有することを特徴とするものである。
【００１２】
請求項６に記載の発明は、上記目的に加えて、素早さを犠牲にせず更なる精度向上を図る目的を達成するため、更に、荷重演算部において、各ストラット、各アクチュエータ、または各第２ユニバーサルジョイントのうち少なくともいずれかの自重による荷重を、各ストラット軸方向荷重と各ストラット軸直角方向荷重とに分解し、求めた各ストラット軸直角方向荷重を各第１ユニバーサルジョイントの回転中心周りのモーメント荷重として、エンドエフェクタに働く荷重に加味することを特徴とするものである。
【００１３】
請求項７に記載の発明は、上記目的に加えて、ユニバーサルジョイントが取付けられる要素に注目して高精度の補正を素早く行うようにする目的を達成するため、更に、分解演算部において、各ユニバーサルジョイント軸を、第１ユニバーサルジョイントについてはベースに固定し、第２ユニバーサルジョイントについてはエンドエフェクタに固定することを特徴とするものである。
【００１４】
請求項８に記載の発明は、上記目的に加えて、ユニバーサルジョイントに及ぼされる弾性変形による様々な方向の変位を素早く高精度にアクチュエータ指令値に換算する目的を達成するため、更に、換算演算部において、各第１弾性変形量および／または各第２弾性変形量の変形方向に沿った各第１弾性変形量ベクトルおよび／または各第２弾性変形量ベクトルと、各アクチュエータ軸に沿った各アクチュエータ軸ベクトルの位置関係に基づいて、各第１弾性変形量および／または各第２弾性変形量を各アクチュエータ軸方向に近似することを特徴とするものである。
【発明の効果】
【００１５】
本発明によれば、ユニバーサルジョイントの弾性変形量をも考慮したアクチュエータ指令値の高精度な補正を、計算量が少なく、一般的な処理能力の制御装置においても素早く計算できる状態で実行できる、という効果を奏する。
【発明を実施するための最良の形態】
【００１６】
以下、本発明の実施形態に係るパラレルメカニズム機械につき、適宜図面に基づいて説明する。
【００１７】
図１は６自由度スチュワートプラットフォーム型のパラレルメカニズム機械１の説明図、図２はパラレルメカニズム機械１の制御装置１０のブロック図であって、パラレルメカニズム機械１は、床に固定されるベースとしてのフレーム１と、フレーム１に固定される６個の第１ユニバーサルジョイント２ａ〜２ｆと、各第１ユニバーサルジョイント２ａ〜２ｆに接続されるアクチュエータとしてのサーボモータ３ａ〜３ｆと、各サーボモータ３ａ〜３ｆにより駆動されるストラットとしてのボールねじ４ａ〜４ｆと、各ボールねじ４ａ〜４ｆの下端に接続される第２ユニバーサルジョイント５ａ〜５ｆと、第２ユニバーサルジョイント５ａ〜５ｆを有する１個のエンドエフェクタ６と、エンドエフェクタ６の対向位置においてフレーム１に固定されたテーブル７と、各サーボモータ３ａ〜３ｆに電気的に接続されアクチュエータ指令値を与える制御装置１０を備えている。
【００１８】
したがって、パラレルメカニズム機械１は、第１ユニバーサルジョイント２ａと、サーボモータ３ａと、ボールねじ４ａと、第２ユニバーサルジョイント５ａとからなる機構や、同様の機構を、合計６組備えている。以下、各組において同様である場合、主に記号のうちａ〜ｆを除いて示す代表例で説明する。
【００１９】
サーボモータ３は、基準状態からの回転角度ないし回転数に対応した数値を出力するエンコーダを備えている。エンドエフェクタ６は、下面に、刃具取付部および刃具回転機構を備えている。テーブル７は、加工物を固定する機構を備えている。
【００２０】
更に、第２ユニバーサルジョイント５は、図３に示すように、それぞれが回転１自由度のジョイントであるフランジ５０およびシャフト５１と、シャフト５１，５２と、ロッド５３とを含む。フランジ５０は、エンドエフェクタ６の下面と直交する側面に垂直に固定される。シャフト５１は、軸受を介してフランジ５０に取付けられており、エンドエフェクタ６の側面に垂直な回転軸Ｐａ周りに回転する。シャフト５２は、軸受を介してシャフト５１に取付けられており、回転軸Ｐｂ周りに回転する。ロッド５３は、シャフト５２に取付けられており、回転軸Ｐｃ周りに回転する軸受を内包していて、この軸受にボールねじ４が接続される。回転軸Ｐａ〜Ｐｃはそれぞれ互いに直交し、３個のジョイントの組合わせで３自由度の第２ユニバーサルジョイント５を形成している。
【００２１】
加えて、第１ユニバーサルジョイント２は、図４に示すように、それぞれが回転１自由度のジョイントであるリング２１，２２、ならびにリング２２，２３を含むジンバル構造となっている。リング２１はフレーム１に固定され、放射方向の軸を受ける軸受を内側に内包する。また、この軸を有するリング２２が、軸受を介してリング２１に取付けられ、回転軸Ｑａ周りに回転するようになっている。更に、同様にリング２３は、軸をリング２２の軸と直交する状態でリング２２に取付けられ、回転軸Ｑｂ周りに回転する。回転軸Ｑａ，Ｑｂはそれぞれ互いに直交し、２個のジョイントの組合わせで２自由度の第１ユニバーサルジョイント２を形成している。
【００２２】
また、制御装置１０は、ＣＰＵ１１と、ＣＰＵ１１が制御のために実行するプログラムの各ステップ等を一時的に記憶するメモリ１２と、各種情報を表示する表示部１３と、各種入力を受付ける入力部１４と、プログラム等を記憶する記憶手段１５と、これらとＣＰＵ１１とのインターフェイス１６と、アクチュエータ３とＣＰＵ１１とのインターフェイス１７と、アクチュエータ３毎のドライブユニット１８とを備えている。記憶手段１５には、プログラムの一部としての、アクチュエータ指令値演算部３０、荷重演算部３１、軸方向荷重演算部３２、分解演算部３３、弾性変形量演算部３４、換算演算部３５、および更新演算部３６が記憶されている。
【００２３】
このようになるパラレルメカニズム機械１の制御装置１０が実行する制御方法に係るフローチャートを図５に示す。第１ステップとしてのステップＳ１において、制御装置１０のアクチュエータ指令値演算部３０は、与えられたエンドエフェクタ６の位置および姿勢の指令値を、アクチュエータ指令値に、逆機構変換によって変換する。ここで、パラレルメカニズム機械１におけるストラット長は、第１ないし第２ユニバーサルジョイント２，５の回転中心間の距離である。また、パラレルメカニズム機械１において、アクチュエータ指令値はエンコーダ出力値と同様になり、アクチュエータ軸方向とストラット軸方向は同一である。そして、ボールねじ４は、ストラット長がサーボモータ３の回転角度ないし回転数（アクチュエータ指令値）に比例するように増減される。したがって、アクチュエータ指令値とストラット長指令値とは、エンコーダの基準位置におけるストラット長（ストラット基準長）を基準として完全に比例関係にあるため、ストラット長指令値をアクチュエータ指令値とみなして計算に用いることができる。
【００２４】
パラレルメカニズム機械１における機構パラメータは、第１ユニバーサルジョイント２の回転中心座標Ｑｉ（ｉ：１〜６、順に記号ａ〜ｆに係る部材に対応、以下同様）、エンドエフェクタ６に係る座標系での各第２ユニバーサルジョイント５の回転中心座標Ｒｉ、ストラット基準長ｌＢｉである。制御装置１０は、エンドエフェクタ６の位置指令値（ｘ，ｙ，ｚ）および姿勢指令値（ａ，ｂ，ｃ）をまとめた指令値Ｔ（ｘ，ｙ，ｚ，ａ，ｂ，ｃ）から、下記［数１］によって、ストラット長指令値ｌｉを求める。ここで、ＥはＴに関する平行移動および回転移動を合成した行列である。
【００２５】
【数１】
2005186210.tif 000003
【００２６】
次に、第２ステップとしてのステップＳ２において、制御装置１０の荷重演算部３１はパラレルメカニズム機械１の各要素の自重による荷重を計算する。サーボモータ３は、自重ｍＳＭｉを有するとすると、図６に示すように、自重ｍＳＭiによって第１ユニバーサルジョイント２の回転中心周りのモーメント荷重を発生し、エンドエフェクタ６に荷重ＦＳＭiとして働く。荷重ＦＳＭｉは下記［数２］で表される。ここで、Ｇは（０，０，−ｇ）というベクトルで、ｇは重力加速度であり、ｌＳＭｉは第１ユニバーサルジョイント２の回転中心とサーボモータ３の重心までの距離であり、ｌｉはストラット長指令値であり、Ｕｉはボールねじ４の軸方向（ストラット軸方向）の単位ベクトルである。
【００２７】
【数２】
2005186210.tif 000004
【００２８】
また、第２ユニバーサルジョイント５のロッド５３は軸受格納部が大きく、その自重ｍＰＣｉの影響は無視できない。この自重ｍＰＣｉにより、図７に示すように、第１ユニバーサルジョイント２の回転中心周りのモーメント荷重が発生し、エンドエフェクタ６に荷重ＦＰＣｉとして働く。荷重ＦＰＣｉは下記［数３］で表される。ここで、ｌＰＣＧは、第２ユニバーサルジョイント５の回転中心からロッド５３の重心までの距離である。
【００２９】
【数３】
2005186210.tif 000005
【００３０】
更に、ロッド５３の自重ｍＰＣｉがストラット軸方向に及ぼす荷重ＷＰＣｉは次の［数４］となる。
【００３１】
【数４】
2005186210.tif 000006
【００３２】
加えて、ボールねじ４の自重ｍＢＳｉにより、図８に示すように、第１ユニバーサルジョイント２の回転中心周りのモーメント荷重が発生し、エンドエフェクタ６に荷重ＦＢＳｉとして働く。そして、荷重ＦＢＳｉは下記［数５］で表される。ここで、ｌＢＳＷｉはボールねじ４の全長であり、ｌＰＣＬｉは第２ユニバーサルジョイント５とロッド−ボールねじ４接続部との間の距離である。
【００３３】
【数５】
2005186210.tif 000007
【００３４】
また、ボールねじ４の自重ｍＢＳｉがストラット軸方向に及ぼす荷重ＷＢＳｉは次の［数６］となる。
【００３５】
【数６】
2005186210.tif 000008
【００３６】
続いて、第３ステップとしてのステップＳ３において、制御装置１０の軸方向荷重演算部３２はボールねじ４やサーボモータ３のストラット軸方向に働く荷重の計算をする。エンドエフェクタ６には重力や外力の他にステップＳ２で求めた荷重が働く。したがって、それらを合計した荷重Ｆｘｙｚとモーメント荷重Ｎａｂｃがエンドエフェクタ６の重心に働く。ここで、荷重Ｆｘｙｚとモーメント荷重Ｎａｂｃを合わせて荷重ベクトルＦとし次の［数７］として表す。
【００３７】
【数７】
2005186210.tif 000009
【００３８】
パラレルメカニズム機械１における機構パラメータ、エンドエフェクタ６の位置および姿勢の指令値から公知の方式により決定される６行６列のヤコビアンマトリックスＪと、各ストラット軸方向の荷重ｗｉからなる荷重ベクトルＷおよび上記荷重ベクトルＦとには下記［数８］の関係があるため、ヤコビアンマトリックスＪの逆行列Ｊ−１を求めれば、下記［数９］となり、荷重ベクトルＷが求まり、各ストラット軸方向の荷重ｗｉが求まる。なお、荷重ベクトルＷは、下記［数１０］である。
【００３９】
【数８】
2005186210.tif 000010
【数９】
2005186210.tif 000011
【数１０】
2005186210.tif 000012
【００４０】
更に、第４ステップとしてのステップＳ４において、制御装置１０の弾性変形量演算部３４は、上記のように算出したストラット軸方向の荷重を、両ユニバーサルジョイント２，５における個々のジョイントの軸方向（ユニバーサルジョイント軸の方向）に分解する演算を行う。図１０は第２ユニバーサルジョイント５のモデルであり、回転軸Ｐａ〜Ｐｃとは別に、エンドエフェクタ６に固定されたユニバーサルジョイント軸ＰＩ，ＰＪ，ＰＫを定義する。ユニバーサルジョイント軸ＰＩ〜ＰＫは同一直交点において互いに直交して直交座標系を規定し、その直交点は回転軸Ｐａ〜Ｐｃの直交点と同一であるが、図中においてはわかりやすいようにずらして図示している。また、回転軸Ｐａとユニバーサルジョイント軸ＰＫを同一方向にとり、エンドエフェクタ６の下面と平行にユニバーサルジョイント軸ＰＪをとり、エンドエフェクタ６の下面と垂直にユニバーサルジョイント軸ＰＩをとる。
【００４１】
算出した各ストラット軸方向の荷重は、第２ユニバーサルジョイント５に働く。制御装置１０は、この荷重を、第２ユニバーサルジョイント５が取付けられるエンドエフェクタ６に固定されたユニバーサルジョイント軸ＰＩ〜ＰＫの各方向に分解する。この分解は、下記［数１１］によって行われる。ここで、ｗＰＩｉ，ｗＰＪｉ，ｗＰＫｉは順にユニバーサルジョイント軸ＰＩ，ＰＪ，ＰＫ方向に分解された荷重であり、ＰＩｉ，ＰＪｉ，ＰＫｉは順にユニバーサルジョイント軸ＰＩ，ＰＪ，ＰＫの単位ベクトルである。なお、ユニバーサルジョイント軸ＰＩ，ＰＪ，ＰＫは、エンドエフェクタ６の位置および姿勢により変化し、第２ユニバーサルジョイント５のエンドエフェクタ６における取付面毎（取付角度毎）に異なるものとなる。
【００４２】
【数１１】
2005186210.tif 000013
【００４３】
第１ユニバーサルジョイント２については、第２ユニバーサルジョイント５と同様に各ストラット軸方向荷重を各ユニバーサルジョイント軸方向に分解できる。ただし、ユニバーサルジョイント軸として、第１ユニバーサルジョイント２が取付けられるフレーム１に固定したものを用いる。第１ユニバーサルジョイント２にかかる荷重ｗｉ’は次の［数１２］となる。
【００４４】
【数１２】
2005186210.tif 000014
【００４５】
続いて、第５ステップとしてのステップＳ５において、制御装置１０の弾性変形量演算部３４は、パラレルメカニズム機械１における各要素の弾性変形量を算出する。制御装置１０はまず、各ストラット軸方向（アクチュエータ軸方向）の各要素の弾性変形量を推定する。各ストラット軸方向の各要素には、ステップＳ２で求めた荷重ＷＢＳｉ，ＷＰＣｉ、およびステップＳ３で求めたストラット軸方向の荷重ｗｉが働く。
【００４６】
そして、制御装置１０は、図９に示すように、各要素の位置関係によりかかる荷重が異なることを考慮した上で、各要素のストラット軸方向のコンプライアンス（剛性の逆数）と各荷重から、下記［数１３］によって、第３弾性変形量としての各ストラット軸方向の弾性変形量Δｌｉを求めることができる。ここで、ｌｉ’はエンドエフェクタ６の位置および姿勢の指令値により決定されるストラット長のうち、ボールねじ４が占める長さであり、ｃＬＯｉ，ｃＬＡｉ，ｃＬＢｉは、順に、ストラット長によって変化する、ボールねじ４の部材のコンプライアンス、ボールねじ４の部材のコンプライアンス、ボールねじ４の自重による自身のひずみの換算されたコンプライアンスであり、ｃＭＯｉ，ｃＭＡｉ，ｃＭＢｉは、順に、ストラット長に依存しない、フランジ５２，ロッド５３，各種軸受，ボールねじ４のナットおよびリング２２，２３のコンプライアンス、各種軸受，ボールねじ４のナット，リング２２，２３のコンプライアンス、各種軸受，ボールねじ４のナット，リング２２，２３のコンプライアンスである（一部省略）。
【００４７】
【数１３】
2005186210.tif 000015
【００４８】
次に、制御装置１０は、第２ユニバーサルジョイント５の弾性変形量を推定する。第２ユニバーサルジョイント５の各要素のコンプライアンスは、一般に、要素中の各軸受についての軸方向（アキシャル方向）とこれに垂直な方向（ラジアル方向）とに分けて与えられている。図１０に示されるように、ユニバーサルジョイント軸ＰＩ〜ＰＫを考えれば、シャフト５１に係る軸受のアキシャル方向はユニバーサルジョイント軸ＰＫ方向であり、ラジアル方向はユニバーサルジョイント軸ＰＩ，ＰＪの合成方向となる。なお、シャフト５２、ロッド５３に係る軸受については、ストラット軸方向荷重を考慮する必要がなく、あるいはストラット軸方向荷重しか働かないので、第１ないし第２ユニバーサルジョイントの弾性変形量（第１ないし第２弾性変形量）を
算出するためのストラット軸方向荷重の分解をしない。
【００４９】
したがって、各軸受についてのコンプライアンスをユニバーサルジョイント軸ＰＩ〜ＰＫ方向に分解したものは、所定のコンプライアンスから容易に得られる。また、ユニバーサルジョイント軸ＰＩ〜ＰＫ方向に分解したコンプライアンスと、ステップＳ４で求めたユニバーサルジョイント軸ＰＩ〜ＰＫ方向の荷重から、第２ユニバーサルジョイント５の弾性変形量が求められる。ただし、第２ユニバーサルジョイント５はエンドエフェクタ６に固定されており、第２ユニバーサルジョイント５に働く荷重によるエンドエフェクタ６の変形の影響を受けるため、この影響を線形近似して加味する。よって、次の［数１４］により、第２弾性変形量としての第２ユニバーサルジョイント５の弾性変形量ΔＰｉが推定される。ここで、ｃＰＩｉ，ｃＰＪｉ，ｃＰＫｉはそれぞれユニバーサルジョイント軸ＰＩ，ＰＪ，ＰＫ方向のコンプライアンスであり、ｊはｉ番目のボールねじ４等（ストラット）の隣の軸番号であり、ｋＰｊは比例定数としての隣接影響係数である。
【００５０】
【数１４】
2005186210.tif 000016
【００５１】
一方、第１弾性変形量としての、第１ユニバーサルジョイント２の弾性変形量も第２ユニバーサルジョイント５と同様に求めることができる。ただし、隣接に影響を与えるのはフレーム１である。即ち、第１ユニバーサルジョイント２のうちの一つにかかる荷重が、フレーム１における、隣接する第１ユニバーサルジョイント２の取付部分に影響を与え変形させる。このとき、フレーム１に固定した方向ユニバーサルジョイント軸Ｑｉ〜Ｑｋのうち、ユニバーサルジョイント軸Ｑｋ方向への影響が大きく、他の方向は無視できるので、［数１４］においてユニバーサルジョイント軸Ｑｋ方向のみに隣接影響係数を適用している。
【００５２】
続いて、第６ステップとしてのステップＳ６において、制御装置１０の換算演算部３５は、両ユニバーサルジョイントが弾性変形することによる変位を、アクチュエータ軸方向に換算する。即ち、ステップＳ５で求めた第１および第２ユニバーサルジョイント２，５の弾性変形を、各ユニバーサルジョイント２，５の回転中心の移動として捉える。図１１は第１，第２ユニバーサルジョイント２，５の回転中心変位のベクトルΔＰｉ，ΔＱｉとストラット軸ベクトルＬｉの関係を示している。ここで、Ｑｉ，Ｐｉはそれぞれ弾性変形前の第１，第２ユニバーサルジョイントの回転中心であり、Ｑｉ’，Ｐｉ’はそれぞれ弾性変形後の第１、第２ユニバーサルジョイントの回転中心であり、Ｌｉ’は弾性変形後のストラット軸ベクトルである。この関係より、各ユニバーサルジョイント回転中心の変位ベクトルとストラット軸ベクトルの関係は、次の［数１５］で表すことができる。
【００５３】
【数１５】
2005186210.tif 000017
【００５４】
更に、両ユニバーサルジョイント２，５の変位は微少であることから、ストラット軸長さ変位に近似することができる。即ち、回転中心変位ベクトルΔＰｉ，ΔＱｉのストラット軸方向成分をストラット軸長さ変位ΔｌＪｉに近似する観点から、次の［数１６］によって、各ユニバーサルジョイント２，５の変位をストラット軸長さ変位ΔｌＪｉに変換することができる。なお、パラレルメカニズム機械１において、ストラット軸方向（ボールねじ４の方向）とアクチュエータ軸方向（サーボモータ３の軸方向）は同一方向であるから、ストラット軸長さ変位ΔｌＪｉをもってアクチュエータ軸方向に換算した変位とすることができる。
【００５５】
【数１６】
2005186210.tif 000018
【００５６】
そして、第７ステップとしてのステップＳ７において、制御装置１０の更新演算部３６は、アクチュエータ指令値を更新する。即ち、下記［数１７］に示すように、ステップＳ５で求めた各ストラット長の変位Δｌｉ（第３変形量）と、ステップＳ６で求めたストラット長変位ΔｌＪｉ（第１および第２変形量）を減算してストラット長指令値、即ちアクチュエータ指令値を補正する。
【００５７】
【数１７】
2005186210.tif 000019
【００５８】
この補正されたアクチュエータ指令値を元にサーボモータ３を制御することにより、実際の機械の弾性変形量が相殺される。即ち、ストラットに関する弾性変形と、ユニバーサルジョイントに関する弾性変形とを、当初のアクチュエータ指令値に補正値を与えることのみによって相殺することができる。これにより、少ない計算量で、エンドエフェクタ６の位置および姿勢の指令値通りの位置および姿勢にエンドエフェクタが位置決めされるようになる。
【００５９】
図１２はパラレルメカニズム機械１を用いて、Ｄｏｕｂｌｅ  Ｂａｌｌ  Ｂａｒ（ＤＢＢ）法による円弧軌跡の真円度を測定した結果を示している。太線が本発明に係る制御方法を用いた場合であり、細線がいずれの補正制御方法をも用いない場合である。真円度が大幅に向上していることが確認できる。
【００６０】
なお、本発明は、ストラット軸とアクチュエータ軸とが同一（平行）でないパラレルメカニズム機械にも用いることができる。この場合、上記ステップＳ３においてアクチュエータ軸方向に働く荷重を算出し、更にストラット軸方向成分を求め、ステップＳ４において当該ストラット軸方向成分を加味した荷重をユニバーサルジョイントの要素に基づく各方向に分解し、ステップＳ６においてユニバーサルジョイントによるストラットの変位に対応するアクチュエータ変位量を求めることもできる。
【００６１】
加えて、ユニバーサルジョイント軸について、例えばユニバーサルジョイント軸Ｐｉ〜Ｐｋをシャフト５１上に固定し、回転軸Ｐｂとユニバーサルジョイント軸Ｐｊを一致させれば、シャフト５１に働く荷重をラジアル方向ないしアキシャル方向に分解でき、上記実施例では影響が少ないとして省略したシャフト５２のアキシャル方向の荷重を簡単に計算することができる。あるいは、ユニバーサルジョイント軸Ｐｉ〜Ｐｊを、極座標系を規定するものとすることもできる。
【００６２】
また、本発明により制御される機械は空間６自由度パラレルメカニズム工作機械に限るものではない。その自由度は６自由度に限るものではなくそれ以上もしくは以下の自由度でもよい。また、ユニバーサルジョイントは回転１自由度ジョイントの組み合わせたものに限らず、球面継手などその他の構造のものでもよい。さらにまた、機械として刃具を備えた工作機械に限らず、ロボット、産業機械、アミューズメント機械、建築機械等でもよい。
【図面の簡単な説明】
【００６３】
【図１】本発明の制御方法により制御されるパラレルメカニズム機械の一部透視斜視図である。
【図２】本発明に係る制御装置のブロック図である。
【図３】図１における第２ユニバーサルジョイントの斜視図である。
【図４】図１における第１ユニバーサルジョイントの斜視図である。
【図５】本発明の制御方法に係る実施形態のフローチャートである。
【図６】図１におけるサーボモータの自重が及ぼす荷重の説明図である。
【図７】図１におけるロッドの自重が及ぼす荷重の説明図である。
【図８】図１におけるボールねじの自重が及ぼす荷重の説明図である。
【図９】図１におけるボールねじの軸方向の荷重の説明図である。
【図１０】図１における第２ユニバーサルジョイントに係る回転軸ないしユニバーサルジョイント軸の説明図である。
【図１１】図１における第１および第２ユニバーサルジョイントに係る回転中心変位とストラット軸ベクトルの関係の説明図である。
【図１２】図１のパラレルメカニズム機械において本発明を適用した場合と適用しない場合のＤＢＢ法による真円度測定結果の説明図である。
【符号の説明】
【００６４】
１    フレーム
２    第１ユニバーサルジョイント
３    サーボモータ（アクチュエータ）
４    ボールねじ（ストラット）
５    第２ユニバーサルジョイント
６    エンドエフェクタ
７    テーブル
【図１】
2005186210.tif 000020
【図２】
2005186210.tif 000021
【図３】
2005186210.tif 000022
【図４】
2005186210.tif 000023
【図５】
2005186210.tif 000024
【図６】
2005186210.tif 000025
【図７】
2005186210.tif 000026
【図８】
2005186210.tif 000027
【図９】
2005186210.tif 000028
【図１０】
2005186210.tif 000029
【図１１】
2005186210.tif 000030
【図１２】
2005186210.tif 000031