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

Description

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

従来のパラレルメカニズム機械の制御方法として、下記特許文献１に記載された方法が知られている。この方法では、パラレルメカニズム機械において、加工時における切削抵抗を求めることを目的とし、アクチュエータによって積極的に駆動される能動軸の送り抵抗の影響に加え、アクチュエータによって積極的に駆動されない受動軸に発生する回転抵抗及び慣性項や重力項をパラメータとして外乱をモデル化し、外乱オブザーバを用いて各パラメータを同定することで加工精度を向上する。ここで、この方法では全ての受動軸を対象とせず、一部についてのみモデル化を行っている。

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

又、従来の工作機械の制御装置として、下記特許文献２に記載されたものが知られている。この装置は、パラレルメカニズム機械に注目した制御をするものではなく、工作機械一般のアクチュエータの負荷を用いて診断する制御を行う。この制御では、工作機械が正常である際に、予めアクチュエータの負荷について正常時測定データを得ておき、この正常時測定データと診断時の測定データを比較することで診断を行う。

特開平５−２８５７８８号公報

特許文献１のような従来の方法では、能動軸の送り抵抗に加えて受動軸の回転抵抗を考慮する点である程度は精度が向上するものの、対象とならない受動軸の影響については考慮されないので、条件によってはその分誤差が大きくなる。又、前記従来の方法では、前記要因との組合せで回転抵抗が同定されるので、前記要因からの影響と受動軸の影響とが渾然一体となり、条件によって誤差が大きくなるし、受動軸における回転抵抗の実測値を用いた補償をしようとしても、前記要因との干渉が生じるために高精度の補償を得られない可能性がある。

そこで、請求項１、請求項５に記載の発明の課題は、全受動軸を考慮の対象として選択可能とし、慣性や重力等の影響から独立したモデルを構築することで、純粋に回転抵抗による影響のみを抜き出した補償をすることで、エンドエフェクタの位置や姿勢をより一層高精度に決定可能としたパラレルメカニズム機械の制御方法、制御装置を提供することにある。

一方、特許文献２のような従来の装置をパラレルメカニズム機械に単に適用したとしても、従来の装置では予め取得された正常時測定データに係る測定条件でしか診断を行うことができないため、パラレルメカニズム機械の状態によって診断時の測定条件が制限され、かつその測定条件が正常時測定データに係る測定条件と異なる場合に正確な診断を行うことが困難であるし、特定のストラットあるいはジョイントを対象とした詳細な診断を行う場合に、どのような測定条件での正常時測定データを得ておけば良いか明らかでなく、特定のストラットあるいはジョイントに対する正確な診断の実施も困難である。又、正常時のデータを比較して正常／異常の判別を行うことは可能でも、異常の発生したストラットあるいはジョイントの特定は困難である。

又、パラレルメカニズム機械を診断する場合、異常個所を特定する観点から、制御装置は各ジョイントの回転抵抗の増大を把握する必要があるが、ジョイントの回転抵抗値を得るにはジョイントを分解した上で実測する他ない。

そこで、請求項９、請求項１３に記載の発明の課題は、各ジョイントの回転抵抗値を容易かつ精度良く得ることのできるパラレルメカニズム機械の制御方法、制御装置を提供することにある。

又、請求項１２、請求項１６に記載の発明の課題は、診断に際し、得られた回転抵抗値を利用することで、より正確で異常個所の特定も可能なパラレルメカニズム機械の診断制御方法、診断制御装置を提供することにある。

上記課題を解決するために、請求項１に記載の発明は、外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続されるエンドエフェクタと、アクチュエータ指令値を与えて各アクチュエータを制御する制御手段と、制御手段に備えられていて第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を記憶可能な回転抵抗値記憶手段を備えたパラレルメカニズム機械の制御方法であって、パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値及び姿勢指令値に対する各アクチュエータ指令値を求める第１ステップと、第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を回転抵抗値記憶手段から取得する第２ステップと、回転抵抗値を用いて各第２ユニバーサルジョイントに作用する力及びモーメントを計算する第３ステップと、各第２ユニバーサルジョイントに作用する力及びモーメントからエンドエフェクタに作用する合力及び合モーメントを求める第４ステップと、エンドエフェクタに作用する合力及び合モーメントを用いて機構の弾性変形量を計算し、その値を用いてアクチュエータ指令値の補償量を計算する第５ステップと、第１ステップで求めたアクチュエータ指令値について、第５ステップで求めた補償量を加味して更新する第６ステップとを有することを特徴とするものである。

請求項２に記載の発明は、上記課題に加えて、少ない計算量で精度の良い効率的な補償を行う制御方法を提供する課題を達成するため、第３ステップにおいて、ストラット、第１ユニバーサルジョイント、第２ユニバーサルジョイントからなるリンクをそれぞれ独立したシリアルリンク機構とみなし、それぞれのシリアルリンク機構におけるユニバーサルジョイントの回転抵抗によってストラットとエンドエフェクタとの接続部に作用する力及びモーメントを計算することを特徴とするものである。

請求項３に記載の発明は、上記課題に加えて、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時においても精度の良い補償を行う制御方法を提供する課題を達成するため、第３ステップにおいて、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時における回転抵抗値の変化を線形関数で近似することを特徴とするものである。

請求項４に記載の発明は、上記課題に加えて、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時においても精度の良い補償をより少ない計算量で効率的に行う制御方法を提供する課題を達成するため、第３ステップにおいて、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時において反転する回転軸の回転抵抗値に対し移動平均処理を行うことを特徴とするものである。

上記課題を解決するために、請求項５に記載の発明は、外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続されるエンドエフェクタと、アクチュエータ指令値を与えて各アクチュエータを制御する制御手段と、制御手段に備えられていて第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を記憶可能な回転抵抗値記憶手段を備えたパラレルメカニズム機械の制御装置であって、パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値及び姿勢指令値に対する各アクチュエータ指令値を求めるアクチュエータ指令値計算手段と、回転抵抗値記憶手段から取得した回転抵抗値を用いて各第２ユニバーサルジョイントに作用する力及びモーメントを計算し、各第２ユニバーサルジョイントに作用する力及びモーメントからエンドエフェクタに作用する合力及び合モーメントを求める合力演算手段と、エンドエフェクタに作用する合力及び合モーメントを用いて機構の弾性変形量を計算し、その値を用いてアクチュエータ指令値の補償量を計算する補償量演算手段と、アクチュエータ指令値計算手段で求めたアクチュエータ指令値について、補償量演算手段で求めた補償量を加味して更新する前記制御手段とを有することを特徴とするものである。

請求項６に記載の発明は、上記課題に加えて、少ない計算量で精度の良い効率的な補償を行う制御装置を提供する課題を達成するため、合力演算手段は、ストラット、第１ユニバーサルジョイント、第２ユニバーサルジョイントからなるリンクをそれぞれ独立したシリアルリンク機構とみなし、それぞれのシリアルリンク機構におけるユニバーサルジョイントの回転抵抗によってストラットとエンドエフェクタとの接続部に作用する力及びモーメントを計算することを特徴とするものである。

請求項７に記載の発明は、上記課題に加えて、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時においても精度の良い補償を行う制御方法を提供する課題を達成するため、合力演算手段は、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時における回転抵抗値の変化を線形関数で近似することを特徴とするものである。

請求項８に記載の発明は、上記課題に加えて、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時においても精度の良い補償をより少ない計算量で効率的に行う制御方法を提供する課題を達成するため、合力演算手段は、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時において反転する回転軸の回転抵抗値に対し移動平均処理を行うことを特徴とするものである。

上記課題を解決するために、請求項９に記載の発明は、外部に固定されるベースと、それぞれ第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するそれぞれのアクチュエータと、それぞれ第２ユニバーサルジョイントを介して各ストラットと接続されるエンドエフェクタと、各アクチュエータの負荷を検出する負荷検出器とを備えたパラレルメカニズム機械についての制御方法であって、第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗によりエンドエフェクタに作用する内力に関し、各第１ユニバーサルジョイント、対応するストラット、対応する第２ユニバーサルジョイントを含む各脚軸系について、シリアルリンク機構とみなすことで、各脚軸系に係るヤコビ行列並びに第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗ベクトルを用いて表した式と、パラレルリンク機構の静力学的関係から、パラレルリンク機構のヤコビ行列及び各アクチュエータの負荷ベクトルを用いて表した式とが等しいことに基づいて、負荷検出器により検出される負荷より構成した負荷ベクトルから、第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗ベクトルを求めることで、当該回転抵抗ベクトルの構成要素としての第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を推定することを特徴とするものである。

上記課題を解決するために、請求項１０に記載の発明は、上記補償に係る制御方法にあって、上記推定に係る回転抵抗値を用いるものである。

請求項１１に記載の発明は、上記課題に加えて、パラレルメカニズム機械の状態に合わせた補償パラメータの自動的な更新を実現するため、制御手段は、回転抵抗値記憶手段における推定した回転抵抗値の記憶を更新することを特徴とするものである。

上記課題を解決するために、請求項１２に記載の発明は、推定した第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの各回転抵抗値について、所定の閾値を超えた場合には、その回転抵抗値に係る第１ユニバーサルジョイント又は第２ユニバーサルジョイントに異常が発生していると判定することを特徴とするものである。

上記課題を解決するために、請求項１３に記載の発明は、外部に固定されるベースと、それぞれ第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するそれぞれのアクチュエータと、それぞれ第２ユニバーサルジョイントを介して各ストラットと接続されるエンドエフェクタと、各アクチュエータの負荷を検出する負荷検出器とを備えたパラレルメカニズム機械における、アクチュエータを制御する制御装置であって、第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗によりエンドエフェクタに作用する内力に関し、各第１ユニバーサルジョイント、対応するストラット、対応する第２ユニバーサルジョイントを含む各脚軸系について、シリアルリンク機構とみなすことで、各脚軸系に係るヤコビ行列並びに第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗ベクトルを用いて表した式と、パラレルリンク機構の静力学的関係から、パラレルリンク機構のヤコビ行列及び各アクチュエータの負荷ベクトルを用いて表した式とが等しいことに基づいて、負荷検出器により検出される負荷より構成した負荷ベクトルから、第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗ベクトルを求めることで、当該回転抵抗ベクトルの構成要素としての第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を推定することを特徴とするものである。

上記課題を解決するために、請求項１４に記載の発明は、上記補償に係る制御装置にあって、上記推定に係る回転抵抗値を用いるものである。

請求項１５に記載の発明は、上記課題に加えて、パラレルメカニズム機械の状態に合わせた補償パラメータの自動的な更新を実現するため、回転抵抗値記憶手段における推定した回転抵抗値の記憶を更新するものである。

上記課題を解決するために、請求項１６に記載の発明は、推定した第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの各回転抵抗値について、所定の閾値を超えた場合には、その回転抵抗値に係る第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントに異常が発生していると判定することを特徴とするものである。

本発明によれば、全受動軸についての回転抵抗の影響を選択的に考慮した補償をし、慣性や重力等の影響から独立したモデルを構築して純粋に回転抵抗による影響のみを抜き出した補償をすることで、エンドエフェクタの位置や姿勢をより一層高精度に決定可能としたパラレルメカニズム機械の制御方法及び制御装置を提供することができる、という効果を奏する。

又、本発明によれば、キャリブレーションに係る機構パラメータの推定の計算の際、第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗による変形誤差を加味するので、回転抵抗による機構要素の変形誤差の影響を除去し、機構パラメータの推定精度をより一層向上することができる。

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

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

エンドエフェクタ６は、下面に、刃具取付部及び刃具回転機構を備えている。テーブル７は、加工物を固定する機構を備えている。そして、各サーボモータ３ａ〜３ｆには、基準状態からの回転角度ないし回転数に対応した数値を出力する図示しないエンコーダが取り付けられ、エンコーダが出力した情報は、制御手段１６（図３）にフィードバックされるようになっている。

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

第１ユニバーサルジョイント２は、回転２自由度である三重のリングを用いたジンバル構造となっている。最外方のリングはフレーム１に固定され、放射方向の軸を受ける軸受を内側に内包する。又、軸を有する中央のリングが、最外方のリングの軸受にこの軸を受けさせることでジョイントとして取り付けられている。更に、最外方及び中央のリングと同様にして、中央と最内方のリングがジョイントとして取り付けられている。ただし、最内方のリングの軸の方向は中央のリングの軸の方向と直交している。このように、２個のジョイントの組合わせで２自由度の第１ユニバーサルジョイント２が形成されている。

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

そして、パラレルメカニズム機械１の脚軸系は閉ループを形成し、又、各脚軸系は、次に示すように、１つの能動軸と５つの受動軸を持つ６自由度シリアルリンク機構とみなすことができる。即ち、各脚軸系は、サーボモータ３により能動軸となるボールねじ４と、回転２自由度の受動軸である第１ユニバーサルジョイント２と、回転３自由度の受動軸である第２ユニバーサルジョイント５を備えており、全体として１つの能動軸及び５つの受動軸を備えている。

図３はパラレルメカニズム機械の制御装置のブロック図である。制御装置は、ＣＰＵ（図示せず）と、ＣＰＵが制御のために実行するプログラムの各ステップ等を一時的に記憶するメモリ（図示せず）と、各種情報を表示する表示部（図示せず）と、各種入力を受付ける入力部（図示せず）と、プログラム等を記憶する記憶手段（図示せず）と、これらとＣＰＵとのインターフェイス（図示せず）と、各サーボモータ３（アクチュエータ）とＣＰＵとのインターフェイス１７と、サーボモータ３毎のドライブユニット（図示せず）とを備えている。記憶手段には、プログラムの一部としての、ジョイント回転軸角度演算手段１０、合力演算手段１１、回転抵抗値記憶手段１２、エンドエフェクタ指令値計算手段１３、補償量演算手段１４、アクチュエータ指令値計算手段１５が記憶されている。

このようになるパラレルメカニズム機械１の制御装置が実行する制御方法に係るフローチャートを図４に示す。まず、第１ステップとしてのステップＳ１において、制御装置のアクチュエータ指令値計算手段１５は、エンドエフェクタ６の位置及び姿勢の指令値Ｘ_ｒｅｆに基づき、各サーボモータ３の指令値ｌ_ｉを計算する。パラレルメカニズム機械１における機構パラメータは、第１ユニバーサルジョイント２の回転中心位置ベクトルＱ_ｉ（ｉ：１〜６、順に記号ａ〜ｆに係る部材に対応、以下同様）、エンドエフェクタ６に係る座標系での各第２ユニバーサルジョイント５の回転中心位置ベクトルＲ_ｉ、ストラット基準長ｌ_Ｂｉである。アクチュエータ指令値計算手段１５は、エンドエフェクタ６の位置指令値（ｘ，ｙ，ｚ）および姿勢指令値（ａ，ｂ，ｃ）をまとめた指令値Ｘ_ｒｅｆ（ｘ，ｙ，ｚ，ａ，ｂ，ｃ）から、下記［数１］によって、ストラット長指令値ｌ_ｉを求める。ここで、ＥはＸ_ｒｅｆに関する平行移動および回転移動を合成した行列である。

なお、ジョイント回転軸角度演算手段１０は、各脚軸系の第１，第２ユニバーサルジョイント２，５の回転軸角度θ_ｉを演算する。回転軸角度θ_ｉは、ボールねじ４の長さも表す。

次に、第２ステップとしてのステップＳ２において、受動軸の回転抵抗値記憶手段１２に記憶されている各受動軸の回転抵抗測定値である回転抵抗ｆ_ｉを取得する。任意のジョイントを考慮の対象から外す場合は、該当するｆ_ｉの成分を０とする。これにより、考慮するジョイントを任意に選択することができる。

続いて、パラレルメカニズム機械１の各脚軸系における受動軸の影響を解析するために、各脚軸系を前記の通り６自由度シリアルリンク機構と仮想し、次の［数２］により各受動軸の回転抵抗値を用いて第２ユニバーサルジョイント５に作用する力及びモーメントを計算する（第３ステップとしてのステップＳ３）。

更に、第４ステップとしてのステップＳ４において、制御装置の合力演算手段１１は、ステップＳ３で求めたリンク先端に作用する力及びモーメントから、エンドエフェクタ６へ作用する内力の合力及び合モーメント（Ｆ）を、次の［数３］に示す通りに求める。ここで、Ｔ_ｉは第２ユニバーサルジョイント５からエンドエフェクタ６への変換行列を表し、下記［数４］で定義される。

そして、制御装置の補償量演算手段１４は、ステップＳ４で求めたエンドエフェクタ６に作用する合力・合モーメントＦを用いて、本願出願人による先の出願に係る特開２００５−１８６２１０に記載された手法により各サーボモータ３の制御量の補正値を計算し、エンドエフェクタ６の変位量の補正を行う（ステップＳ５，Ｓ６）。より詳しくは、次の通りとなる。

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

更に、補償量演算手段１４は、上記のように算出したストラット軸方向の荷重を、両ユニバーサルジョイント２，５における個々のジョイントの軸方向（ユニバーサルジョイント軸の方向）に分解する演算を行う。図５は第２ユニバーサルジョイント５のモデルであり、第２ユニバーサルジョイント５の回転軸Ｐ_ａ，Ｐ_ｂ，Ｕ_ｉとは別に、エンドエフェクタ６に固定されたユニバーサルジョイント軸Ｐ_Ｉ，Ｐ_Ｊ，Ｐ_Ｋを定義する。ただしＵ_ｉはストラット軸方向（ボールねじ４の方向）の単位ベクトルである。ユニバーサルジョイント軸Ｐ_Ｉ〜Ｐ_Ｋは同一直交点において互いに直交して直交座標系を規定し、その直交点は回転軸Ｐ_ａ〜Ｐ_ｃの直交点と同一であるが、図中においてはわかりやすいようにずらして図示している。又、回転軸Ｐ_ａとユニバーサルジョイント軸Ｐ_Ｋを同一方向にとり、エンドエフェクタ６の下面と平行にユニバーサルジョイント軸Ｐ_Ｊをとり、エンドエフェクタ６の下面と垂直にユニバーサルジョイント軸Ｐ_Ｉをとる。

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

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

続いて、補償量演算手段１４は、パラレルメカニズム機械１における各要素の弾性変形量を算出する。制御装置はまず、各ストラット軸方向（アクチュエータ軸方向）の各要素の弾性変形量を推定する。各ストラット軸方向の各要素には、ストラット軸方向の荷重ｗ_ｉが働く。

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

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

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

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

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

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

そして、第６ステップとしてのステップＳ６において、制御装置の制御手段１６は、アクチュエータ指令値を更新する。即ち、下記［数１４］に示すように、ステップＳ５で求めた各ストラット長の変位Δｌ_ｉ，Δｌ_Ｊｉを、目標指令値ｌ_ｉから減算してストラット長指令値、即ちアクチュエータ指令値を補正し、インターフェイス１７へ伝える。

この補償されたアクチュエータ指令値を元にサーボモータ３を制御することにより、パラレルメカニズム機械１の弾性変形量が相殺される。即ち、第１，第２ユニバーサルジョイント２，５に関する弾性変形を、当初のアクチュエータ指令値に補償された指令値を与えることで相殺することができる。

図７はパラレルメカニズム機械１を用いて、Ｄｏｕｂｌｅ Ｂａｌｌ Ｂａｒ（ＤＢＢ）法による円弧軌跡の真円度を測定した結果を示している。図７（ａ）がいずれの補償をも用いない場合であり、図７（ｂ）が本発明に係る補償を用いた場合である。補償なしの場合、回転方向の違いにより大きなヒステリシスが見られ、又各方向においても波形が大きく歪んでいる。一方、本発明による補償ありの場合、ヒステリシスが大きく改善され、また各方向毎についても真円度が大幅に向上していることが確認できる。

但し、より詳細には、上記制御では所定の回転抵抗ｆ_ｉを用いており、回転方向を反転する際には回転抵抗ｆ_ｉの正負のみが変わって段差的に遷移するため（例えば図８参照）、これに基づく変形誤差推定値も段差状に変化し、結果的には動作方向の反転に対応して、ＤＢＢ法による円弧軌跡に係る変形誤差推定値においてジョイントの反転位相付近に段差状の半径変化が現れることになる。そして実際には回転抵抗ｆ_ｉの値は連続して変化するものであり、ＤＢＢ法の軌跡でも反転位相付近で推定値は連続的に変化するものであるから、ジョイント反転位相付近において推定値が実測値から離れることになる。

そこで、制御装置による変形誤差推定値の算出において、精度をより一層向上する制御としては、動作方向反転時の回転抵抗ｆ_ｉの値についての段差的変化の調整によるものがあることになる。そしてこの調整は、第１ユニバーサルジョイント２及び／又は第２ユニバーサルジョイント５の回転軸周りでエンドエフェクタ６に作用する力の関係に応じることになる。

これら力の関係においては、サーボモータ３の駆動力によりエンドエフェクタ６が駆動する際の、回転軸周りのトルクの存在が重要となる。このトルクが作用すると、パラレルメカニズム機構の各要素の弾性変形力と第１ユニバーサルジョイント２又は第２ユニバーサルジョイント５の回転抵抗の大小により、第１ユニバーサルジョイント２又は第２ユニバーサルジョイント５の挙動が変化する。ここでは、第２ユニバーサルジョイント５周りの挙動を例にとって説明する。この弾性変形力が第２ユニバーサルジョイント５の回転抵抗より小さい場合（弾性変形力＜回転抵抗）、第２ユニバーサルジョイント５は回転せず、エンドエフェクタ６の移動に伴って機構要素が変形する（図９参照、矢印Ｇはエンドエフェクタ６の移動方向を示し、矢印Ｈはその先方における回転抵抗力によるボールねじの変形を指摘するものである）。そして変形量が蓄積して弾性変形力が回転抵抗力と等しくなったところで（弾性変形力＝回転抵抗）、第２ユニバーサルジョイント５が回転し始め、以後第２ユニバーサルジョイント５が停止するまで弾性変形力と回転抵抗とが釣り合った状態で遷移する。

従って、エンドエフェクタ６に作用する力としては、第２ユニバーサルジョイント５が回転し始めるまでの領域ではボールねじ４の弾性変形量に比例した弾性力であり、第２ユニバーサルジョイント５が回転し始めてからは一定値である回転抵抗力である。よって、動作方向の反転後のエンドエフェクタ６に作用するトルクの推移は、例えば図８に対して示す図１０のように、反転直後は線形的に推移し、その後一定値となる。

そこで、制御装置による回転抵抗ｆ_ｉの値についての変化の調整としては、反転直後の回転抵抗ｆ_ｉの変化を線形関数で近似することが挙げられる。そして、この線形関数については、機構要素の弾性係数をパラメータとし、具体例としてはボールねじ４の弾性係数に対応する傾きを有するものとする。

回転抵抗ｆ_ｉの値を図８のように段差状に不連続で遷移させた場合におけるＤＢＢ軌跡の推定誤差を図１１（ａ）に示し、図１０のように線形近似を行った場合における、制御装置によって算出されたＤＢＢ軌跡の推定誤差を図１１（ｂ）に示す。これらより、線形近似の場合に推定誤差が小さくなり、実測値と推定値に関する一致の程度が高いことが分かる。又、回転抵抗ｆ_ｉの値を図８のように段差状に不連続で遷移させた場合におけるサーボモータ３の負荷を図１２（ａ）に示し、図１０のように線形近似を行った場合におけるサーボモータ３の負荷を図１２（ｂ）に示す。これらから、線形近似を行った場合、特に反転動作付近で推定値が実測値により近づくことが分かる。従って、動作方向の反転に伴う過渡的な領域においても、回転抵抗ｆ_ｉの変化について線形近似をすることで推定値の精度を向上できることが確認される。

なお、回転抵抗ｆ_ｉの変化についての線形関数への近似は、制御装置が、反転位相を基準として、回転抵抗ｆ_ｉが正から負へ変化したのかあるいはその逆の変化なのかで場合を分けて線形関数を切り替えることでなしたり、図８について移動平均処理を行えば図１０と同等の波形を得られることに着目して、機構要素の弾性係数の代わりに移動平均点数をパラメータとすることでなしたりすることが可能である。特に後者の場合は前者と比べて場合分け等の特殊な処理を要せず図１０に相当する遷移を得られるので、精度を維持しながら処理量をより低減することができる。

次に、ジョイントの回転抵抗によるパラレルメカニズム機構のキャリブレーションへの影響を例にとって説明する。上述したことから分かるように、第１ユニバーサルジョイント２及び／又は第２ユニバーサルジョイント５の回転抵抗ｆ_ｉに起因して、たとえエンドエフェクタ６が同位置・同姿勢にあったとしても、例えば図１３に示すように、動作方向によって変形誤差が変わり得る。よって、第１ユニバーサルジョイント２及び／又は第２ユニバーサルジョイント５の回転抵抗ｆ_ｉを考慮せずにパラレルメカニズム機械１のキャリブレーションを行うと、キャリブレーション時の動作方向によってキャリブレーション結果が変わってしまうし、キャリブレーションの目的である機構パラメータの推定値の算出にあって、精度がばらつき、あるいは比較的に悪くなる。従って、図１４に示すように、制御装置によって、第１ユニバーサルジョイント２及び／又は第２ユニバーサルジョイント５の回転抵抗ｆ_ｉを考慮したキャリブレーションを行うことで、この回転抵抗による外乱の影響を排除して、キャリブレーション精度を向上することができる。

即ち、制御装置は、まず一般のキャリブレーションと同様にして、キャリブレーション用の測定条件の設定を行う（ステップＳ１１）。例えば、測定位置や、姿勢、動作方向（ここでは反時計回り即ちＣＣＷ方向とするが逆の場合も以下と同様に考えて良い）といったものである。

次に、制御装置は、第１ユニバーサルジョイント２及び／又は第２ユニバーサルジョイントの回転抵抗ｆ_ｉに係る半径誤差量Δｒ_ｆｉについて、上述のストラット軸長さ変位Δｌ_ｉ，Δｌ_Ｊｉに基づき求めておく（ステップＳ１２）。即ち、制御装置は、キャリブレーションにおける測定条件に従い、各測定点におけるストラット軸長さ変位Δｌ_ｉ，Δｌ_Ｊｉを求め、これらの関係から各測定点における半径誤差量Δｒ_ｆｉを求めておく。続いて、制御装置は、上記測定条件に従った測定を実施する（ステップＳ１３）。

そして、制御装置は、以下のようにして、測定結果から、変形に係る半径誤差量Δｒ_ｆｉを取り除き、測定結果を修正する（ステップＳ１４）。即ち、ｎ個の機構パラメータを次に示す［数１５］のように表すと、ＤＢＢ軌跡の測定における２つの球の中心間距離ｒ_ｉとの関係は、一般的には次に示す［数１６］となり、これを線形近似すると、次に示す［数１７］となる。この［数１７］においては、第１ユニバーサルジョイント２及び／又は第２ユニバーサルジョイント５の回転抵抗による誤差成分が加味されていないので、これを加味した等式とすると、次に示す［数１８］の通りとなり、［数１８］によってより正確な半径誤差Δｒ_ｉを表すことができる。よって、制御装置は、この［数１８］にステップＳ１２で求めておいた半径誤差量Δｒ_ｆｉを代入することで、半径誤差量Δｒ_ｆｉをキャンセルした測定結果を得ることができる。

この後、制御装置は、引き続き［数１８］を機構パラメータΔｐ_ｊについて解くことで、回転抵抗ｆ_ｉに基づく半径誤差量Δｒ_ｆｉの影響を差し引いた機構パラメータΔｐ_ｊを得、更にこれに基づき、回転抵抗ｆ_ｉの影響を受けないキャリブレーションを行う（ステップＳ１５）。ＤＢＢ軌跡の測定点数がｍ点であるとき、ｍ本の［数１８］の連立方程式となり、この連立方程式は、次に［数１９］として示すように行列表記することができる。［数１９］は次に示す［数２０］のように最小自乗法で解くことができ、これによって制御装置は回転抵抗ｆ_ｉの影響を除いた機構パラメータ誤差ΔＰを得ることができる。

図１５（ａ）において回転抵抗ｆ_ｉに基づく半径誤差量Δｒ_ｆｉの影響を差し引かないでキャリブレーションを行った後のＤＢＢ軌跡の測定結果を示し、図１５（ｂ）において上述の通り回転抵抗ｆ_ｉに基づく半径誤差量Δｒ_ｆｉの影響を差し引いてキャリブレーションを行った後のＤＢＢ軌跡の測定結果を示す。図１５（ａ）では測定方向によって誤差が異なることで偏りが発生しているところ、図１５（ｂ）ではこのような偏りが比較的に少なく、ＤＢＢ軌跡の真円度の向上が見て取れる。又、図１５（ａ）では回転抵抗ｆ_ｉに基づく半径誤差量Δｒ_ｆｉが含められたままキャリブレーションされ、半径誤差量Δｒ_ｆｉをも機構パラメータ誤差ΔＰとして同定されていたため、全体としての真円度誤差が比較的大きくなっていたが、図１５（ｂ）では真円度誤差の絶対値も低減され、全体的な真円度向上も確認することができる。

ここで、上記キャリブレーションの少なくとも一部をまとめると、次の通りとなる。即ち、第２ユニバーサルジョイントを介してストラットと接続されるエンドエフェクタと、ストラットを駆動するアクチュエータと、アクチュエータ指令値を与えて各アクチュエータを制御する制御手段と、制御手段に備えられていて第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を記憶可能な回転抵抗値記憶手段を備えたパラレルメカニズム機械のエンドエフェクタを複数の位置及び姿勢に位置決めした際に、位置及び／又は姿勢の測定、又は固定点からの距離測定を行い、その測定値を元に、パラレルメカニズム機械の機構パラメータを推定しキャリブレーションする方法であって、制御手段による機構パラメータの推定の計算の際、第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗による変形誤差を加味することを特徴とするパラレルメカニズム機械のキャリブレーション方法である。

又、第２ユニバーサルジョイントを介してストラットと接続されるエンドエフェクタと、ストラットを駆動するアクチュエータと、アクチュエータ指令値を与えて各アクチュエータを制御する制御手段と、制御手段に備えられていて第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を記憶可能な回転抵抗値記憶手段を備えたパラレルメカニズム機械にあって、制御手段は、エンドエフェクタを複数の位置及び姿勢に位置決めした際に、位置及び／又は姿勢の測定、又は固定点からの距離測定を行い、その測定値を元に、パラレルメカニズム機械の機構パラメータを推定することでキャリブレーションし、当該キャリブレーションに係る機構パラメータの推定の計算の際、第１ユニバーサルジョイント２及び／又は第２ユニバーサルジョイントの回転抵抗による変形誤差を加味することを特徴とするパラレルメカニズム機械の制御装置である。

続いて、上記アクチュエータ指令値の補償に関する制御に好適に用いられる回転抵抗ｆ_ｉの推定について説明する。

ここで、第１ユニバーサルジョイント２及び第２ユニバーサルジョイント５については、既に述べている通りであるが、改めて説明する。図１６は第１ユニバーサルジョイント２の説明図であって、第１ユニバーサルジョイント２は、二重のリングを用いたジンバル構造となっている。外方のリング４１は、フレーム１に対し、放射方向の軸４２を介して回転自在に支えられる。又、軸４２に直交する軸４３を有する内側のリング４４が、外方のリング４１の軸受４５にこの軸を受けさせることでジョイントとして取り付けられている。このように、２個のジョイントの組合わせで２自由度の第１ユニバーサルジョイント２が形成され、第１ユニバーサルジョイント２は互いに直交する２個の受動回転軸６０，６１から構成されている。

図１７は第２ユニバーサルジョイント５の説明図であって、第２ユニバーサルジョイント５は、フランジ５１、及びシャフト５２を含む。フランジ５１は、エンドエフェクタ６（図１７では示さないがこの図で右後方の配置となる）の下面と直交する側面に垂直に固定されており、受動回転軸６４の周りで回転する。シャフト５２は、軸受５３を介してフランジ５１に取付けられており、エンドエフェクタ６の側面に平行な受動回転軸６３の周りに回転する。一方、シャフト５２における軸受５３と反対側の端部には、シャフト５２の軸と同等な受動回転軸６２の周りで回転する軸受５４が設置されており、この軸受５４にはボールねじ４（図１７では示さない）が入れられている。このように、３個の軸受（ジョイント）の組合わせで３自由度の第２ユニバーサルジョイント５が形成され、第２ユニバーサルジョイント５は、互いに直交する２個の受動回転軸６３，６４と、受動回転軸６３周りで揺動する受動回転軸６２とから構成されている。

このように、パラレルメカニズム機械の各脚軸系は５個の受動回転軸６０〜６４を有し、又脚軸系は６組存在するため、パラレルメカニズム機械全体で受動回転軸は３０個存在する。以下第１ユニバーサルジョイント２及び第２ユニバーサルジョイント５の各ジョイントについて、特に言及した場合を除き、互いに区別せずに受動ジョイントと称する。

図１８はパラレルメカニズム機械の制御装置、及び制御対象の一部としての機械系の特定要素を示すブロック図である。制御装置は、ＮＣ装置を用いて構成されており、軌道生成手段２０、補償パラメータ記憶手段２１、制御量演算手段２２、ジョイント抵抗値推定手段２６、ジョイント抵抗値履歴記憶手段２７、状態診断手段２８を含む。なお、これら手段の内の前４者は精度補償部に含まれ、後２者は状態診断部に含まれる。又、各手段においては、ＮＣ装置におけるプログラムが用いられている。

制御装置は、軌道生成手段２０において、エンドエフェクタ６の運動軌跡を生成する。又、制御装置は、制御量演算手段２２において、パラレルメカニズムの逆運動学計算を解くことにより各サーボモータ３の指令値を求める。制御装置は、これら指令値を、機械系に属するサーボコントローラ２３に与え、サーボコントローラ２３は、対応するサーボモータ３を、指令値に基づき駆動する。又、サーボモータ３には、駆動時における負荷（駆動動力値）を検出する負荷検出器２５が設置されており、これらの負荷は、制御装置のジョイント抵抗値推定手段２６において把握ないし記憶可能である。

一方、後述の通りジョイント抵抗値推定手段２６により、軌道生成手段２０によって生成される運動軌跡と、負荷検出器２５により測定されるサーボモータ３の負荷測定値を元に各受動ジョイントの回転抵抗値が算出される。

こうして得られた回転抵抗値は、エンドエフェクタ６の動作誤差を補償する際の新しいパラメータとして用いるために、補償パラメータ記憶手段２１の内容の更新に用いられる。なお、回転抵抗をパラメータとした誤差推定方法ないしは動作の補償については、上述のものを好適に用いることができる。

又、回転抵抗値は、ジョイント回転抵抗履歴記憶手段２７における内容の追加にも用いられる。制御装置は、状態診断手段２８において、ジョイント回転抵抗履歴記憶手段２７を参照し、回転抵抗値やその変化を把握することで、パラレルメカニズム機械の状態診断を実施する。

制御装置の状態診断手段２８においては、予め各受動ジョイントの回転抵抗値の閾値と、回転抵抗値の変化量の閾値とが格納されている。これら閾値としては、正常な状態（レベル１）、及び受動ジョイントに大きな負荷がかかり始めあるいはその急な変化が現れ始めているが制御装置によって誤差を補償することで精度を維持できる状態（レベル２）との境界を示すものと、当該レベル２、及び受動ジョイントに過大な負荷がかかりあるいは過大な負荷変化が発生している状態（レベル３）との境界を示すものを設定する。

このようになるパラレルメカニズム機械の制御装置が実行する制御方法に係るフローチャートを図１９及び図２０に示す。まず、制御装置のジョイント抵抗値推定手段２６は、次のように受動ジョイントの回転抵抗値を推定する（図１９のステップＳ２１）。

即ち、上述したように、パラレルメカニズム機械の各脚軸系における受動軸の影響を解析するために、各脚軸系を６自由度のシリアルリンク機構と仮想すると、前記［数２］に示すように、各受動ジョイントの回転抵抗値を用いて第２ユニバーサルジョイント５に作用する力及びモーメントを表すことができる。

一方、エンドエフェクタ６へ作用する内力の合力及び合モーメント（Ｆ）は、リンク先端に作用する力ないしはモーメントを用いて、前記［数３］に示す通りに表現することができる。ここで、Ｔ_ｉは第２ユニバーサルジョイント５からエンドエフェクタ６への変換行列を表し、前記［数４］で定義される。

そして［数２］と［数３］から、次の［数２１］を導くことができる。一方、パラレルリンク機構の静力学的関係から、次の［数２２］の関係が成り立つ。従って、［数２１］及び［数２２］から［数２３］という関係式が得られる。

更に、［数２３］を行列表記すると、次に示す［数２４］となる。そして、この［数２４］が各測定点について成り立つことを考慮して、これらの関係式を連立させると、次に示す［数２５］のように表現することができる。但し、ｎは測定点数である。［数２５］の右辺第１項をＪとすると、Ｊの疑似逆行列Ｊ^＋を用いて、次に示す［数２６］によって最小自乗近似的に回転抵抗ベクトルｆを求めることができる。

実際にパラレルメカニズム機械を駆動させて測定されるサーボモータ３の負荷には、各受動ジョイントの回転抵抗による成分の他に、自重による成分、慣性力による成分等が含まれる。よって、各受動ジョイントの回転抵抗の正確な推定のためには、［数２６］によって得た回転抵抗ベクトルｆから、回転抵抗による成分のみを抽出する必要がある。但し、測定時にパラレルメカニズム機械を低速で動作させることによって、慣性力の影響を微々たるものとすることができるので、慣性力による成分についてはないものとして除去を考慮しないものとする。

一方自重による成分については、この成分はエンドエフェクタ６の位置及び姿勢にのみに依存し、動作方向には依存しない。よって、エンドエフェクタ６をＣＣＷ方向（反時計回り方向）に移動しても、あるいはＣＷ方向（時計回り方向）に移動しても、同一軌跡上の同じ位置・姿勢同士で比べれば同じ自重による負荷がかかっている。従って、これら両方向についてサーボモータ３の負荷を測定し、それぞれの測定結果について差分をとることで、自重による成分を解消することができる。即ち、［数２５］についてこのような差分を考慮したものとすると、次の［数２７］のようになり、この［数２７］を［数２５］と同様にｆについて解くと［数２８］の通りとなる。この［数２８］によって、各脚軸系毎に、受動ジョイントの回転抵抗を表す回転抵抗ベクトルｆの同定を行える。ここで、任意のジョイントを同定の対象から除外する場合は［数２７］の右辺第２項から対応する成分を除外し、又［数２７］の右辺第１項について対応する列ベクトルを除外して行列を構成すれば良い。これにより、同定対象のジョイントを任意に選択することが可能である。

制御装置のジョイント抵抗値推定手段２６は、以上のことから、まず測定条件に応じて、各測定点における行列Ｊを算出する（図２０のステップＳ３１）。次に、ジョイント抵抗値推定手段２６は、ＣＣＷ方向、ＣＷ方向についてサーボモータ３（アクチュエータ）の負荷を測定する（ステップＳ３２）。続いて、ジョイント抵抗値推定手段２６は、これらの測定結果につき差分をとる（ステップＳ３３）。そして、ジョイント抵抗値推定手段２６は、予め［数２８］に相当する計算ができるように記憶されたプログラムに基づき、この差分及び行列Ｊを用いて［数２８］を解く（ステップＳ３４）。このようにして、図１９のステップＳ２１において、ジョイント抵抗値推定手段２６は、サーボモータ３の負荷から、受動ジョイントの回転抵抗を推定する。

そして、制御装置は、ステップＳ２２において、状態診断手段２８に格納された回転抵抗値に関するレベル１，２に係る閾値（回転抵抗値のレベル１の閾値）と、ジョイント回転抵抗履歴記憶手段２７に保持されている回転抵抗値とを比較する。その結果、回転抵抗値がこの閾値以下であれば（回転抵抗値≦回転抵抗値のレベル１の閾値）、制御装置は、処理をステップＳ２６へ進める。一方、回転抵抗値がこの閾値を超えていれば（回転抵抗値＞回転抵抗値のレベル１の閾値）、制御装置は、その回転抵抗値に係る受動ジョイントについて異常が発生していると判定して、処理をステップＳ２３へ進める。

制御装置は、ステップＳ２３において、状態診断手段２８に格納された回転抵抗値に関するレベル２，３に係る閾値（回転抵抗値のレベル２の閾値）と、ジョイント回転抵抗履歴記憶手段２７に保持されている回転抵抗値とを比較する。その結果、回転抵抗値がこの閾値以下であれば（回転抵抗値≦回転抵抗値のレベル２の閾値）、制御装置は、処理をステップＳ２４へ進め、回転抵抗値が通常のものより大きくなっている旨の警告や、このような回転抵抗値を発生している受動ジョイントあるいはこれの属する脚軸系の種別を表示する。一方、回転抵抗値がこの閾値を超えていれば（回転抵抗値＞回転抵抗値のレベル２の閾値）、制御装置は、処理をステップＳ２５へ進め、パラレルメカニズム機械の動作を停止する。なお、制御装置は、この動作停止時にその旨の表示を行う。

一方、制御装置は、ステップＳ２６において、状態診断手段２８に格納された回転抵抗値の変化（増分）に関するレベル１，２に係る閾値（回転抵抗値増分のレベル１の閾値）と、ジョイント回転抵抗履歴記憶手段２７における回転抵抗値の履歴から把握した回転抵抗値増分とを比較する。その結果、回転抵抗値増分がこの閾値以下であれば（回転抵抗値増分≦回転抵抗値増分のレベル１の閾値）、制御装置は、処理をステップＳ２８へ進め、診断の結果異常なしとして、通常の動作を続ける。一方、回転抵抗値増分がこの閾値を超えていれば（回転抵抗値増分＞回転抵抗値増分のレベル１の閾値）、制御装置は、その回転抵抗値増分に係る受動ジョイントについて異常が発生していると判定して、処理をステップＳ２７へ進める。

制御装置は、ステップＳ２７において、状態診断手段２８に格納された回転抵抗値増分に関するレベル２，３に係る閾値（回転抵抗値増分のレベル２の閾値）と、ジョイント回転抵抗履歴記憶手段２７を参照して得られる回転抵抗値増分とを比較する。その結果、回転抵抗値増分がこの閾値以下であれば（回転抵抗値増分≦回転抵抗値増分のレベル２の閾値）、制御装置は、処理をステップＳ２９へ進め、回転抵抗値増分が通常のものより大きくなっている旨の警告や、このような回転抵抗値増分を発生している受動ジョイントあるいはこれの属する脚軸系の種別を表示する。一方、回転抵抗値増分がこの閾値を超えていれば（回転抵抗値増分＞回転抵抗値増分のレベル２の閾値）、制御装置は、処理をステップＳ３０へ進め、パラレルメカニズム機械の動作を停止する。なお、制御装置は、この動作停止時にその旨の表示を行う。

以上の制御装置あるいは制御方法にあっては、アクチュエータであるサーボモータ３に係る負荷に基づき［数２８］の関係によって受動ジョイントの回転抵抗値を求めるので、容易に精度の良い推定を行うことができる。

図２１として、１つの脚軸系（第１ユニバーサルジョイント２ａ〜第２ユニバーサルジョイント５ａ）に関し、各受動ジョイント（図２１中の１１〜１５）における、［数２８］による回転抵抗値の推定値（図２１では黒色のバー）と、回転抵抗値の実測値（図２１では白色のバー）とを比較したグラフを示す。これより、［数１１］による精度の良好な推定が認められる。

又図２２の（ａ）〜（ｆ）として、それぞれの脚軸系（第１ユニバーサルジョイント２ａ〜第２ユニバーサルジョイント５ａに係るものが（ａ）であり、その他は順次同様である）に関する、各方向におけるサーボモータ３の負荷の差分τ_{ｉ，ＣＣＷ}−τ_ｉ，ＣＷの波形（［数１１］への入力波形、図２２では薄線）と、［数２８］により推定された各受動ジョイントの回転抵抗値から得た負荷の推定値の波形（図２２では実線）とを比較したグラフを示す。ここで、各グラフの縦軸は、対応するサーボモータ３の負荷であり、横軸は、ＤＢＢ（Ｄｏｕｂｌｅ Ｂａｌｌ Ｂａｒ）位相角（単位はdeg.、度）である。これにより、入力波形と推定結果との一致の度合いが高いことが分かる。

又、以上の制御装置あるいは制御方法にあっては、このように推定された回転抵抗値を用いて当該回転抵抗値が閾値を超えるか否かを判定し、回転抵抗値が閾値を超えた場合にはその回転抵抗値に係る受動ジョイントについて異常と判定するので、異常判定時にパラレルメカニズム機械の動作を停止して、受動ジョイントにおいて過大な回転抵抗やその変化を生じることにより発生する破損、変形や機能異常等を防止することができるし、過大な回転抵抗ないしその変化を生ずる前の異常判定時にその旨を表示して作業者に注意を促すこともできる。

そして、以上の制御装置あるいは制御方法において推定された回転抵抗値を用いて、上述の通り機構変形誤差を推定し補償することができる。以上のように回転抵抗値を高精度に推定できるため、この補償の精度も良好であるし、以上のように推定された回転抵抗値はアクチュエータ負荷から計算量の少ない状態で算出できるため、この補償も容易に実行できる。図２３にこの場合の回転抵抗値推定結果から推定したＤＢＢ軌跡（ＣＣＷ方向）と、ＤＢＢ軌跡（ＣＣＷ方向）の実測値との比較を示すグラフを示す。このグラフから、変形誤差補償の良好な精度が伺える。

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

又、エンドエフェクタに作用する合力からアクチュエータ指令値の補償量を求める際、特開２００５−１８６２１０記載の手法によらず、機構要素のコンプライアンスを用いて機構の弾性変形量を求め、得られた弾性変形量を加味して逆機構変換を行い、直接補償後のアクチュエータ指令値を求めても良い。

本発明の制御方法により制御されるパラレルメカニズム機械の一部透視斜視図である。 図１における脚軸を示す説明図である。 本発明の内のアクチュエータ指令値の補償に係る制御装置のブロック図である。 本発明のの内のアクチュエータ指令値の補償に係る制御方法に関する実施形態のフローチャートである。 図１における第２ユニバーサルジョイントに係る回転軸ないしユニバーサルジョイント軸の説明図である。 図１における第１及び第２ユニバーサルジョイントに係る回転中心変位とストラット軸ベクトルの関係の説明図である。 図１のパラレルメカニズム機械において、（ａ）本発明を適用しない場合、（ｂ）本発明を適用した場合における、ＤＢＢ法による真円度測定結果のグラフである。 線形近似を行わない場合における第２ユニバーサルジョイントの回転抵抗の遷移の一例を示すグラフである。 第２ユニバーサルジョイントの回転抵抗によるボールねじの変形を示す模式図である。 線形近似を行った場合における第１ユニバーサルジョイント又は第２ユニバーサルジョイントの回転抵抗の遷移の一例を示すグラフである。 （ａ）線形近似を行わない場合、（ｂ）線形近似を行った場合における、ＤＢＢ法による推定誤差値のグラフである。 （ａ）線形近似を行わない場合、（ｂ）線形近似を行った場合における、ＤＢＢ法におけるサーボモータ負荷に係るグラフである。 第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗によりＤＢＢ軌跡に現れる変形誤差量の一例を示すグラフである。 図１のパラレルメカニズム機械に対するキャリブレーションに係る実施形態のフローチャートである。 （ａ）図１４のキャリブレーションを行わない場合、（ｂ）図１４のキャリブレーションを行った場合における、ＤＢＢ法における真円度測定結果のグラフである。 図１における第１ユニバーサルジョイントを示す説明図である。 図１における第２ユニバーサルジョイントを示す説明図である。 本発明の内の回転抵抗値の推定に係る制御装置及び制御対象の一部のブロック図である。 本発明の内の回転抵抗値の推定に係る制御方法に関する実施形態のフローチャートである。 本発明の内の回転抵抗値の推定に係る制御方法に関する実施形態のフローチャートである。 図１のパラレルメカニズム機械における、ある脚軸系の回転抵抗値の測定値と本発明に係る推定値とを示すグラフである。 （ａ）〜（ｆ）は、対応するサーボモータにおける負荷の実測値と推定値とを示すグラフである。 ＤＢＢ軌跡の実測値と、本発明に係る推定値から求めたＤＢＢ軌跡とを示すグラフである。

符号の説明

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

Claims (16)

1. 外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続されるエンドエフェクタと、アクチュエータ指令値を与えて各アクチュエータを制御する制御手段と、制御手段に備えられていて第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を記憶可能な回転抵抗値記憶手段を備えたパラレルメカニズム機械の制御方法であって、
   パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値及び姿勢指令値に対する各アクチュエータ指令値を求める第１ステップと、
   第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を回転抵抗値記憶手段から取得する第２ステップと、
   回転抵抗値を用いて各第２ユニバーサルジョイントに作用する力及びモーメントを計算する第３ステップと、
   各第２ユニバーサルジョイントに作用する力及びモーメントからエンドエフェクタに作用する合力及び合モーメントを求める第４ステップと、
   エンドエフェクタに作用する合力及び合モーメントを用いて機構の弾性変形量を計算し、その値を用いてアクチュエータ指令値の補償量を計算する第５ステップと、
   第１ステップで求めたアクチュエータ指令値について、第５ステップで求めた補償量を加味して更新する第６ステップと
   を有することを特徴とするパラレルメカニズム機械の制御方法。
2. 第３ステップにおいて、ストラット、第１ユニバーサルジョイント、第２ユニバーサルジョイントからなるリンクをそれぞれ独立したシリアルリンク機構とみなし、それぞれのシリアルリンク機構におけるユニバーサルジョイントの回転抵抗によってストラットとエンドエフェクタとの接続部に作用する力及びモーメントを計算する
   ことを特徴とする請求項１に記載のパラレルメカニズム機械の制御方法。
3. 第３ステップにおいて、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時における回転抵抗値の変化を線形関数で近似する
   ことを特徴とする請求項１又は請求項２に記載のパラレルメカニズム機械の制御方法。
4. 第３ステップにおいて、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時において反転する回転軸の回転抵抗値に対し移動平均処理を行う
   ことを特徴とする請求項１又は請求項２に記載のパラレルメカニズム機械の制御方法。
5. 外部に固定されるベースと、第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するアクチュエータと、第２ユニバーサルジョイントを介して各ストラットと接続されるエンドエフェクタと、アクチュエータ指令値を与えて各アクチュエータを制御する制御手段と、制御手段に備えられていて第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を記憶可能な回転抵抗値記憶手段を備えたパラレルメカニズム機械の制御装置であって、
   パラレルメカニズム機械における所定の機構パラメータに基づいて、エンドエフェクタの位置指令値及び姿勢指令値に対する各アクチュエータ指令値を求めるアクチュエータ指令値計算手段と、
   回転抵抗値記憶手段から取得した回転抵抗値を用いて各第２ユニバーサルジョイントに作用する力及びモーメントを計算し、各第２ユニバーサルジョイントに作用する力及びモーメントからエンドエフェクタに作用する合力及び合モーメントを求める合力演算手段と、
   エンドエフェクタに作用する合力及び合モーメントを用いて機構の弾性変形量を計算し、その値を用いてアクチュエータ指令値の補償量を計算する補償量演算手段と、
   アクチュエータ指令値計算手段で求めたアクチュエータ指令値について、補償量演算手段で求めた補償量を加味して更新する前記制御手段と
   を有することを特徴とするパラレルメカニズム機械の制御装置。
6. 合力演算手段は、ストラット、第１ユニバーサルジョイント、第２ユニバーサルジョイントからなるリンクをそれぞれ独立したシリアルリンク機構とみなし、それぞれのシリアルリンク機構におけるユニバーサルジョイントの回転抵抗によってストラットとエンドエフェクタとの接続部に作用する力及びモーメントを計算する
   ことを特徴とする請求項５に記載のパラレルメカニズム機械の制御装置。
7. 合力演算手段は、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時における回転抵抗値の変化を線形関数で近似する
   ことを特徴とする請求項５又は請求項６に記載のパラレルメカニズム機械の制御装置。
8. 合力演算手段は、第１ユニバーサルジョイント又は第２ユニバーサルジョイントの反転時において反転する回転軸の回転抵抗値に対し移動平均処理を行う
   ことを特徴とする請求項５又は請求項６に記載のパラレルメカニズム機械の制御装置。
9. 外部に固定されるベースと、それぞれ第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するそれぞれのアクチュエータと、それぞれ第２ユニバーサルジョイントを介して各ストラットと接続されるエンドエフェクタと、各アクチュエータの負荷を検出する負荷検出器とを備えたパラレルメカニズム機械についての制御方法であって、
   第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗によりエンドエフェクタに作用する内力に関し、
   各第１ユニバーサルジョイント、対応するストラット、対応する第２ユニバーサルジョイントを含む各脚軸系について、シリアルリンク機構とみなすことで、各脚軸系に係るヤコビ行列並びに第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗ベクトルを用いて表した式と、
   パラレルリンク機構の静力学的関係から、パラレルリンク機構のヤコビ行列及び各アクチュエータの負荷ベクトルを用いて表した式と
   が等しいことに基づいて、
   負荷検出器により検出される負荷より構成した負荷ベクトルから、第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗ベクトルを求めることで、
   当該回転抵抗ベクトルの構成要素としての第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を推定する
   ことを特徴とするパラレルメカニズム機械の制御方法。
10. 請求項１ないしは請求項４のいずれかの制御方法にあって、請求項９によって推定される回転抵抗値を用いる
    ことを特徴とするパラレルメカニズム機械の制御方法。
11. 請求項１０に記載のパラレルメカニズム機械の制御方法にあって、
    制御手段は、回転抵抗値記憶手段における推定した回転抵抗値の記憶を更新する
    ことを特徴とするパラレルメカニズム機械の制御方法。
12. 推定した第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの各回転抵抗値について、所定の閾値を超えた場合には、その回転抵抗値に係る第１ユニバーサルジョイント又は第２ユニバーサルジョイントに異常が発生していると判定する
    ことを特徴とする請求項９ないしは請求項１１のいずれかに記載のパラレルメカニズム機械の制御方法。
13. 外部に固定されるベースと、それぞれ第１ユニバーサルジョイントを介してベースに接続される複数のストラットと、各ストラットを駆動するそれぞれのアクチュエータと、それぞれ第２ユニバーサルジョイントを介して各ストラットと接続されるエンドエフェクタと、各アクチュエータの負荷を検出する負荷検出器とを備えたパラレルメカニズム機械における、アクチュエータを制御する制御装置であって、
    第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗によりエンドエフェクタに作用する内力に関し、
    各第１ユニバーサルジョイント、対応するストラット、対応する第２ユニバーサルジョイントを含む各脚軸系について、シリアルリンク機構とみなすことで、各脚軸系に係るヤコビ行列並びに第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗ベクトルを用いて表した式と、
    パラレルリンク機構の静力学的関係から、パラレルリンク機構のヤコビ行列及び各アクチュエータの負荷ベクトルを用いて表した式と
    が等しいことに基づいて、
    負荷検出器により検出される負荷より構成した負荷ベクトルから、第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗ベクトルを求めることで、
    当該回転抵抗ベクトルの構成要素としての第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの回転抵抗値を推定する
    ことを特徴とするパラレルメカニズム機械の制御装置。
14. 請求項５ないしは請求項８のいずれかの制御装置にあって、請求項１３によって推定される回転抵抗値を用いる
    ことを特徴とするパラレルメカニズム機械の制御装置。
15. 請求項１４に記載のパラレルメカニズム機械の制御装置にあって、
    回転抵抗値記憶手段における推定した回転抵抗値の記憶を更新する
    ことを特徴とするパラレルメカニズム機械の制御装置。
16. 推定した第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントの各回転抵抗値について、所定の閾値を超えた場合には、その回転抵抗値に係る第１ユニバーサルジョイント及び／又は第２ユニバーサルジョイントに異常が発生していると判定する
    ことを特徴とする請求項１３ないしは請求項１５のいずれかに記載のパラレルメカニズム機械の制御装置。

Images