(57)【要約】【課題】パラレルメカニズム工作機械の位置制御装置において、プラットフォームの動作に関する運動方程式を得ることで、運動状態の解析を可能にし、この結果を用いて、プラットフォームの位置や姿勢及び制御状態を、高精度に制御、且つ状態検出する。【解決手段】順変換関数（順運動学問題の解析解に相当する）の１階及び２階偏微分値などを、位置指令演算部の各リンク位置指令値などからリアルタイムに求め、各リンク推力を演算する推力演算部を具備する。【選択図】図１
2006095621.tif 000002
【特許請求の範囲】
【請求項１】
プラットフォームを支持する複数本のリンクの長さを上位装置からの指令値に従って制御することで、前記プラットフォームの位置及び姿勢を制御するパラレルメカニズム工作機械の位置制御装置であって、
前記プラットフォームの座標値をリンク長で表した順変換関数の１階微分と２階微分と１階微分の時間微分値とを、リンク長をプラットフォームの座標値で表した逆変換関数の解析解と各リンク位置指令値から導出し、前記１階微分と２階微分と１階微分の時間微分値に基づいて各リンクのリアルタイムのプラットフォーム推力を求める推力演算部を有することを特徴とする位置制御装置。
【請求項２】
請求項１に記載の位置制御装置であって、
前記推力演算部から出力される各リンクのリアルタイムのプラットフォーム推力を、各リンクの位置を制御する位置制御部から出力されるリンク推力指令値にフィードフォワード加算することを特徴とする位置制御装置。
【請求項３】
請求項１または２に記載の位置制御装置であって、
各リンクのリンク軸方向の総合剛性と、前記推力演算部から出力される各リンクのリアルタイムのプラットフォーム推力と、から各リンクのリンク軸方向の撓み量を求め、
当該撓み量分を加算した各リンクの位置指令値を作成することを特徴とする位置制御装置。
【請求項４】
請求項１から３のいずれか１項に記載の位置制御装置であって、
位置制御部から出力される各リンクの推力と、前記推力演算部から出力される各リンクのリアルタイムのプラットフォーム推力と、から負荷反力ベクトルを演算することで負荷状態を検出することを特徴とする位置制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、パラレルメカニズム工作機械におけるプラットフォームの位置及び姿勢を制御する位置制御装置に関するものである。
【背景技術】
【０００２】
図５は、パラレルメカニズム工作機械の全体を表した図である。テーブル１０１には、加工ワーク（図示せず）が搭載される。６個ある自在継手１０２は、ベース１０５（天板）に固定されて、６本のリンク１０４の方向を自由に取れるように構成された継手である。同様に６個ある自在継手１０３は、主軸頭１０６とリンク１０４の先端に直結されて、両者の成す角度を自由に取れる役割を果している。サーボモータ１０９は、ＮＣ装置１０８の内部に含まれる位置制御装置（図示しない）を介して、位置指令値に従って、リンク１０４をリンク軸方向に動作させ、自在継手１０２から自在継手１０３の間のリンク長を長さ制御する。６本のリンク長の長さ制御により、主軸頭１０６（以下、「プラットフォーム」という）に搭載された回転工具１０７の位置と姿勢が制御される。なお、サーボモータ１０９は、リンク長を検出する位置検出器（図示せず）を内部に含んだ構成をとっている。
【０００３】
図６は、パラレルメカニズム工作機械におけるプラットフォームの位置及び姿勢を制御するメカニズムを簡単にモデル化したものである。図５におけるベース１０５は、破線の正三角形でモデル化し、各頂点を１，２，３とする。同様に、プラットフォーム１０６も、破線の正三角形でモデル化し、各頂点を１’，２’，３’とする。更に、プラットフォーム側では、リンクＬ１とＬ２が頂点１’でジョイントされ、ベース側では、リンクＬ４とＬ５が頂点１でジョイントされている。つまり、６本のリンク１０４は、ベース側とプラットフォーム側で異なる２本ずつの組になってジョイントされるモデルになっている。
【０００４】
今、空間上に固定されたベースの重心Ｏを原点として、重心Ｏから頂点１の逆方向にＸ軸を、頂点２と頂点３を結ぶ線と平行にＹ軸を、この右ネジ方向にＺ軸をとる。このＯ−ＸＹＺ座標系をパラレルメカニズム工作機械における基準座標系とする。一方で、プラットフォームの重心Ｏ’を原点として、重心Ｏ’から頂点１’の方向にＸ’軸を、頂点３’と２’とを結ぶ線と平行にＹ’軸を、この右ネジ方向にＺ’軸をとる。このＯ’−Ｘ’Ｙ’Ｚ’座標系は、プラットフォームに固定された座標系とする。更に、プラットフォームの初期位置と初期姿勢は、ベースに対して空間的に平行で、ＯとＯ’が共にＺ軸上に存在する場合を選ぶ。
【０００５】
以上の前提条件の下で、プラットフォームの位置と姿勢は、各リンク１０４のリンク長Ｌｊ（ｊ＝１，・・・，６）により制御される。また本モデルにおいて、プラットフォームの位置は、Ｏ−ＸＹＺ座標系から見たプラットフォームの重心Ｏ’の座標値［ｘ，ｙ，ｚ］’（但し、［  ］’は転置を示す）で表す。プラットフォームの姿勢は、良く知られたＺ’−Ｙ’−Ｘ’オイラー角［φ，θ，ψ］’で表す。（初期姿勢で、φ＝０，θ＝０，ψ＝０である。）更に、Ｏ’−Ｘ’Ｙ’Ｚ’座標系で見た位置ベクトルの座標値を、Ｏ−ＸＹＺ座標系で見た座標値に変換する変換行列：Ｅ：を、次のように定義する。なお、以下では、行列を示す記号は、：（コロン）で囲む。
［数１］  ：Ｅ：＝ｒｏｔ（Ｚ,φ）・ｒｏｔ（Ｙ,θ）・ｒｏｔ（Ｚ,ψ）
ここで、ｒｏｔ（Ｚ,φ）等は、良く知られた回転行列である。
【０００６】
以上の準備の下、パラレルメカニズム工作機械の逆運動学問題、すなわち、基準座標系における座標値（ｘ，ｙ，ｚ，φ，θ，ψ）からリンク長（Ｌ１，・・・，Ｌ６）を求める問題について考える。重心Ｏ’から頂点１’，２’，３’へ到る位置ベクトルに関して、基準座標系（Ｏ−ＸＹＺ座標系）で見た場合を、：ｄ１：，：ｄ２：，：ｄ３：と示し、プラットフォーム座標系で見た場合を、：ｄ１’：，：ｄ２’：，：ｄ３’：と示すと、変換行列：Ｅ：により、：ｄｉ：＝：Ｅｄｉ’：（ｉ＝１，２，３）となる。さらに、重心Ｏから重心Ｏ’に到る位置ベクトルを：ｒ：、各リンクＬｊのベース側先端からプラットフォーム側先端に至る位置ベクトルを：Ｌｊ：とおく（いずれも基準座標系から見た場合）。この場合、図６のモデルに関して、次の関係式が成立する。
［数２］
：Ｌ１：＝：ｒ：−：ｂ２：＋：Ｅｄ１’：
：Ｌ２：＝：ｒ：−：ｂ３：＋：Ｅｄ１’：
：Ｌ３：＝：ｒ：−：ｂ３：＋：Ｅｄ２’：
：Ｌ４：＝：ｒ：−：ｂ１：＋：Ｅｄ２’：
：Ｌ５：＝：ｒ：−：ｂ１：＋：Ｅｄ３’：
：Ｌ６：＝：ｒ：−：ｂ２：＋：Ｅｄ３’：
ここで、これらの関係式を、：Ｌｊ：＝［ｘｌｊ，ｙｌｊ，ｚｌｊ］’（ｊ＝１，・・・，６）と基準座標系で見た座標値で表現すれば、
［数３］  Ｌｊ＝｜：Ｌｊ：｜＝（ｘｌｊ２＋ｙｌｊ２＋ｚｌｊ２）1／２  （ｊ＝１，・・・，６）
がパラレルメカニズム工作機械の逆運動学問題の解析解になる。
【０００７】
図７は、従来のパラレルメカニズム工作機械の位置制御装置の構成を示すブロック図である。位置制御装置はＮＣ装置の内部に含まれ、実座標系（ｘ，ｙ，ｚ，φ，θ，ψ）で表現されたパラレルメカニズム工作機械の動作軌跡が、ある一定周期毎に、位置指令値ＰＤとして上位装置（図示しない）より入力される。位置指令演算部の加減速演算部では、位置指令値ＰＤが補間加減速処理され、微小時間ΔＴ毎のパラレルメカニズム工作機械の動作軌跡を示す実座標値（ｘ，ｙ，ｚ，φ，θ，ψ）が出力される。次に、この実座標値は、逆変換部に送られ、数２，３式に従って逆変換処理され、各リンクのリンク長に相当する位置指令値Ｌｊ（ｊ＝１，・・・，６）が出力される。各リンク位置指令値Ｌｊは、独立した同一構成を有する各リンク位置制御部に送られる。
【０００８】
次に、第１リンクを例に、リンク位置制御部の動作について説明する。サーボモータ５７に締結された位置検出器５８は、リンクの軸方向の変位量、つまりはリンク長を検出する位置検出器であり、その出力であるリンク位置検出値Ｌ１ｄは、減算器５０により、リンク位置指令値Ｌ１から減算される。減算器５０の出力は、位置偏差増幅器Ｇｐで増幅される。一方、リンク位置指令値Ｌ１は、微分器５３で微分されて、位置偏差増幅器Ｇｐの出力と加算器５１で加算され、リンク速度指令値Ｖｃとなる。
【０００９】
リンク位置検出値Ｌ１ｄは、微分器５５で時間微分され、リンク速度検出値ｖｄとなる。減算器５２では、リンク速度指令値Ｖｃからリンク速度検出値ｖｄが減算される。減算器５２の出力は、速度偏差増幅器Ｇｖで通常、比例積分増幅される。一方、前記微分器５３の出力は、微分器５４で再度微分され、増幅器Ｍ１でＭ１倍される。ここでＭ１は、サーボモータ５７のリンク軸方向可動部の質量を示すため、増幅器Ｍ１出力は、リンク位置指令値Ｌ１の加速度成分に対する、サーボモータ５７分の加減速推力を意味している。増幅器Ｍ１の出力は、前記速度偏差増幅器Ｇｖの出力と、加算器５６で加算され、リンク推力指令値Ｆｃとなる。
【００１０】
パワー増幅部は、リンク推力指令値Ｆｃを電力増幅し、サーボモータ５７に供給する。これにより、サーボモータ５７は、リンク推力指令値Ｆｃ通りの推力を発生する。以上説明したリンク位置制御部の動作により、各リンク長は、各リンク位置指令値Ｌｊに一致する様に制御され、結果的にパラレルメカニズム工作機械のプラットフォームの位置と姿勢が制御される。
【００１１】
次に、パラレルメカニズム工作機械の順運動学問題、すなわち、リンク長（Ｌ１，・・・，Ｌ６）から座標値（ｘ，ｙ，ｚ，φ，θ，ψ）を求める問題について考える。図６において、プラットフォームの頂点１’，２’，３’の基準座標系で見た座標値を、Ｌ１〜Ｌ６の各リンク長で表現できれば、リンク長からプラットフォームの重心位置（ｘ，ｙ，ｚ）と姿勢（φ，θ，ψ）が算出できる。重心Ｏから各頂点までの長さをｂとおくと、ベースの頂点２の座標値は、［ｂ／２，−√３ｂ／２，０］’となる。また、頂点１’の座標値を［ｘ１，ｙ１，ｚ１］’（頂点２’，３’も同様）とおく。この場合、リンク長Ｌ１について、次の式が成立する。
［数４］  Ｌ１２＝（ｘ１−ｂ／２）２＋（ｙ１＋√３ｂ／２）２＋ｚ１２
他のＬ２〜Ｌ６についても同様な関係式が成立する。一方、プラットフォームの重心Ｏ’から各頂点１’，２’，３’までの距離をｄとおくと、プラットフォームの辺１’−２’の長さは、√３ｄと表せる。さらに、この辺１’−２’について、次の式が成立する。
［数５］  （√３ｄ）２＝（ｘ１−ｘ２）２＋（ｙ１−ｙ２）２＋（ｚ１−ｚ２）２
他の辺についても同様な関係式が成立する。以上の事から、結局、９本の９元連立２次方程式が構成できる。これは、解析的に求まる形式であるが、変数を消去していく過程で、残る変数が高次に成り過ぎるため、実際問題として解析的に解くことは非常に困難になる。これは解法の一例であるが、一般にパラレルメカニズム工作機械の順運動学問題を解析的に解く方法は知られておらず、通常は、探索的に解を求める方法が採られる。
【発明の開示】
【発明が解決しようとする課題】
【００１２】
以上説明してきた様に、パラレルメカニズム工作機械では、逆運動学問題は解析的に解けるが、順運動学問題は探索的にしか解けないため、シリアルリンク型ロボットの様に、解析的な運動方程式を得ることができない。このため、運動状態が解析できず、従来の位置制御装置では、動的なプラットフォームの位置や姿勢を高精度に制御することができなかった。本発明が解決しようとする課題は、逆運動学問題の解析解から、プラットフォームの動作に関するリアルタイムの運動方程式を得ることで、運動状態のリアルタイム解析を可能にし、この結果を用いて、プラットフォームの位置や姿勢を高精度に制御、且つ、制御状態を検出できる位置制御装置を提供することである。
【課題を解決するための手段】
【００１３】
本発明の位置制御装置は、プラットフォームを支持する複数本のリンクの長さを上位装置からの指令値に従って制御することで、前記プラットフォームの位置及び姿勢を制御するパラレルメカニズム工作機械の位置制御装置であって、前記プラットフォームの座標値をリンク長で表した順変換関数の１階微分と２階微分と１階微分の時間微分値とを、リンク長をプラットフォームの座標値で表した逆変換関数の解析解と各リンク位置指令値から導出し、前記１階微分と２階微分と１階微分の時間微分値に基づいて各リンクのリアルタイムのプラットフォーム推力を求める推力演算部を有することを特徴とする。
【発明の効果】
【００１４】
本発明によれば、各リンク推力をリアルタイムに得ることができる。これにより、パラレルメカニズム工作機械の運動状態の解析ができ、プラットフォームの動的な位置や姿勢を高精度に制御、且つ、制御状態を検出できる。
【発明を実施するための最良の形態】
【００１５】
はじめに、本実施形態の基本となるプラットフォームの推力の求め方について説明する。基準座標系から見た、プラットフォーム重心Ｏ’の初期位置における座標値を［ｘ０，ｙ０，ｚ０］’、初期姿勢を［φ０，θ０，ψ０］’、この時の逆運動学問題の解析解として求まる各リンク長をＬｊ０とする。ここで、パラレルメカニズム工作機械の一般化座標として、初期リンク長からのリンク長変化ｌｊを次の式で定義する。
［数６］  ｌｊ＝Ｌｊ−Ｌｊ０
さらに、実座標系（ｘ，ｙ，ｚ，φ，θ，ψ）を一般化座標系（ｌ１，・・・，ｌ６）により、ｘ＝ｆ（ｌ１，・・・，ｌ６），ｙ＝ｆ（ｌ１，・・・，ｌ６），・・・の多変数関数で表し、これらの１階と２階の偏微分が存在すると仮定する。
【００１６】
以上の準備の下、プラットフォームの並進エネルギーＫｐｌについて考える。基準座標系の原点Ｏから、プラットフォーム重心Ｏ’への位置ベクトル：ｒ：は、：ｒ：＝［ｘ，ｙ，ｚ］’である。よって、速度ｖは、：ｖ：＝ｄ：ｒ：／ｄｔ＝［ｄｘ／ｄｔ，ｄｙ／ｄｔ，ｄｚ／ｄｔ］’となる。さらに、プラットフォームの質量Ｍｐを用いると、並進エネルギーＫｐｌは、次式で表せられる。
［数７］
Ｋｐｌ＝（１／２）Ｍｐ｜：ｖ：｜２＝（１／２）Ｍｐ｛（ｄｘ／ｄｔ）２＋（ｄｙ／ｄｔ）２＋（ｄｚ／ｄｔ）２｝
さらに仮定より、
［数８］
ｄｘ／ｄｔ＝Σ｛ａ１ｋ・（ｄｌｋ／ｄｔ）｝
ｄｙ／ｄｔ＝Σ｛ａ２ｋ・（ｄｌｋ／ｄｔ）｝
ｄｚ／ｄｔ＝Σ｛ａ３ｋ・（ｄｌｋ／ｄｔ）｝
と表現する。ここで、Σの範囲は、ｋ＝１〜６であり、ａ１ｋ＝∂ｘ／∂ｌｋ，ａ２ｋ＝∂ｙ／∂ｌｋ，ａ３ｋ＝∂ｚ／∂ｌｋとする。この数８式を数７式に代入すると、並進エネルギーＫｐｌは、次のように記述できる。
［数９］
Ｋｐｌ＝（１／２）Ｍｐ［｛Σａ１ｋ（ｄｌｋ／ｄｔ）｝２＋｛Σａ２ｋ（ｄｌｋ／ｄｔ）｝２＋｛Σａ３ｋ（ｄｌｋ／ｄｔ）｝２］
よって、
［数１０］
∂Ｋｐｌ／∂（ｄｌｊ／ｄｔ）＝（１／２）Ｍｐ［２｛Σａ１ｋ（ｄｌｋ／ｄｔ）｝ａ１ｊ＋２｛Σａ２ｋ（ｄｌｋ／ｄｔ）｝ａ２ｊ＋２｛Σａ３ｋ（ｄｌｋ／ｄｔ）｝ａ３ｊ｝］
＝ＭｐΣ｛（ａ１ｊ・ａ１ｋ＋ａ２ｊ・ａ２ｋ＋ａ３ｊ・ａ３ｋ）（ｄｌｋ／ｄｔ）｝
＝ＭｐΣ｛Ｂｊｋ（ｄｌｋ／ｄｔ）｝
となる。さらに、
［数１１］
ｄ｛∂Ｋｐｌ／∂（ｄｌｊ／ｄｔ）｝／ｄｔ＝ＭｐΣ｛Ｂｊｋ（ｄ２ｌｋ／ｄｔ２）＋（ｄＢｊｋ／ｄｔ）（ｄｌｋ／ｄｔ）｝
となる。
【００１７】
一方で、
［数１２］
∂Ｋｐｌ／∂ｌｊ＝（１／２）Ｍｐ［２｛Σａ１ｋ（ｄｌｋ／ｄｔ）｝｛Σ（ｄｌｋ／ｄｔ）（∂ａ１ｋ／∂ｌｊ）｝＋２｛Σａ２ｋ（ｄｌｋ／ｄｔ）｝｛Σ（ｄｌｋ／ｄｔ）（∂ａ２ｋ／∂ｌｊ）｝＋２｛Σａ３ｋ（ｄｌｋ／ｄｔ）｝｛Σ（ｄｌｋ／ｄｔ）（∂ａ３ｋ／∂ｌｊ）｝］
＝Ｍｐ｛ΣΣＣｊｍｎ（ｄｌｍ／ｄｔ）（ｄｌｎ／ｄｔ）｝
（但し、前のΣの範囲は、ｍ＝１〜６であり、後のΣの範囲は、ｎ＝ｍ〜６である。）
となる。
【００１８】
次に、プラットフォームの回転エネルギーＫｐｒについて考える。一般的にパラレルメカニズム工作機械では、回転工具の限定から、ψ≡０で制御される。そこで、この限定の下で、Ｏ’−Ｘ’Ｙ’Ｚ’座標系で見た角速度ベクトル：ω’：は、次式で示される。
［数１３］  ：ω’：＝［−Ｓθ（ｄφ／ｄｔ），ｄθ／ｄｔ，Ｃθ（ｄφ／ｄｔ）］’
ここで、Ｓθは、ｓｉｎθ，Ｃθは、ｃｏｓθを示す。また、プラットフォームの形状より、慣性乗積は全て０と見なせるから、プラットフォームの慣性テンソル行列：Ｈ：を用いると、回転エネルギーＫｐｒは、次式のようになる。
［数１４］  
Ｋｐｒ＝（１／２）［：ω’：］’：Ｈ：： ω’：＝（１／２）｛Ｉｘ（Ｓθ）２（ｄφ／ｄｔ）２＋Ｉｙ（ｄθ／ｄｔ）２＋Ｉｚ（Ｃθ）２（ｄφ／ｄｔ）２｝
＝（１／２）［Ｉｘ（Ｓθ）２｛Σａ４ｋ（ｄｌｋ／ｄｔ）｝２＋Ｉｙ｛Σａ５ｋ（ｄｌｋ／ｄｔ）｝２＋Ｉｚ（Ｃθ）２｛Σａ４ｋ（ｄｌｋ／ｄｔ）｝２］
（但し、Σの範囲は、ｋ＝１〜６である。なお、Ｉｘは、Ｘ’軸周りのプラットフォームの慣性モーメントを示しており、Ｉｙ，Ｉｚも同様である。）
となる。よって、
［数１５］  
∂Ｋｐｒ／∂（ｄｌｊ／ｄｔ）＝Σ｛Ｉｘ（Ｓθ）２ａ４ｊ・ａ４ｋ＋Ｉｙ・ａ５ｊ・ａ５ｋ＋Ｉｚ（Ｃθ）２ａ４ｊ・ａ４ｋ｝（ｄｌｋ／ｄｔ）
＝ΣＰｊｋ（ｄｌｋ／ｄｔ）
更に、
［数１６］
ｄ｛∂Ｋｐｒ／∂（ｄｌｊ／ｄｔ）｝／ｄｔ＝Σ｛Ｐｊｋ（ｄ２ｌｋ／ｄｔ２）＋（ｄＰｊｋ／ｄｔ）（ｄｌｋ／ｄｔ）｝
となる。
【００１９】
一方で、
［数１７］
∂Ｋｐｒ／∂ｌｊ＝Ｉｘ｛Σ（Ｓθ）ａ４ｋ（ｄｌｋ／ｄｔ）｝［Σ｛Ｃθ・ａ５ｊ・ａ４ｋ＋Ｓθ（∂ａ４ｋ／∂ｌｊ）｝（ｄｌｋ／ｄｔ）］＋Ｉｙ｛Σａ５ｋ（ｄｌｋ／ｄｔ）｝［Σ（∂ａ５ｋ／∂ｌｊ）（ｄｌｋ／ｄｔ）］＋Ｉｚ｛Σ（Ｃθ）ａ４ｋ（ｄｌｋ／ｄｔ）｝［Σ｛−Ｓθ・ａ５ｊ・ａ４ｋ＋Ｃθ（∂ａ４ｋ／∂ｌｊ）｝（ｄｌｋ／ｄｔ）］
＝ΣΣＱｊｍｎ（ｄｌｍ／ｄｔ）（ｄｌｎ／ｄｔ）
（但し、前のΣの範囲は、ｍ＝１〜６であり、後のΣの範囲は、ｎ＝ｍ〜６である。）
となる。
【００２０】
次に、プラットフォームのポテンシャルエネルギーＶｐについて考える。Ｖｐ＝Ｍｐ・ｇ（ｚ−ｚ０）だから、
［数１８］  ∂Ｖｐ／∂ｌｊ＝Ｍｐ・ｇ（∂ｚ／∂ｌｊ）＝Ｍｐ・ｇ・ａ３ｊ
（但し、ｇは重力加速度である。）
となる。
【００２１】
以上、数１１，１２，１６，１７，１８式の結果を、ラグランジェの運動方程式に適用することで、プラットフォームの運動に対して、各リンクのサーボモータが発生する推力Ｆｊ（ｊ＝１，・・・，６）は、
［数１９］
Ｆｊ＝Σ（Ｍｐ・Ｂｊｋ＋Ｐｊｋ）（ｄ２ｌｋ／ｄｔ２）
＋Σ｛Ｍｐ（ｄＢｊｋ／ｄｔ）＋（ｄＰｊｋ／ｄｔ）｝（ｄｌｋ／ｄｔ）
−ΣΣ｛Ｍｐ・Ｃｊｍｎ＋Ｑｊｍｎ｝（ｄｌｍ／ｄｔ）（ｄｌｎ／ｄｔ）
＋Ｍｐ・ｇ・ａ３ｊ
（但し、右辺第１，２項のΣの範囲は、ｋ＝１〜６であり、第３項のΣの範囲は、前が、ｍ＝１〜６であり、後が、ｎ＝ｍ〜６である。）
と表現できる。ここで、本式の右辺第１項はパラレルメカニズム工作機械における慣性力項、第２，３項は遠心力やコリオリ力の項、第４項は重力項と見なすことができる。
【００２２】
数１９式の運動方程式を導出する前提条件として、最初に、実座標系（ｘ，ｙ，ｚ，φ，θ，ψ）の一般化座標系ｌｊに関する偏微分の存在性を仮定した。しかし、前述の如く、パラレルメカニズム工作機械では、リンク長（Ｌ１，・・・，Ｌ６）から座標値（ｘ，ｙ，ｚ，φ，θ，ψ）を求める順運動学問題は、解析的に求まらない。そのため、これらの偏微分を解析的に得ることができない。そこで、次の様な方法により、リアルタイムで実量として偏微分を求め、リアルタイムで各リンクのサーボモータの推力Ｆｊを求める。
【００２３】
パラレルメカニズム工作機械では、前述の如く、座標値（ｘ，ｙ，ｚ，φ，θ，ψ）からリンク長（Ｌ１，・・・，Ｌ６）を求める逆運動学問題は、解析的に求まる。そのため、実座標系で与えられるプラットフォームの位置や姿勢に対する各リンクの位置指令値Ｌｊは、数２，３式で示した逆変換で求められる。そこで、便宜上、この逆変換関数を次の一般的表現
［数２０］  Ｌｊ＝Ｌｊ（ｘ，ｙ，ｚ，φ，θ，ψ）  （ｊ＝１，・・・，６）
で置き換えておく。ここで、本式の全微分をとると、
［数２１］
ΔＬｊ＝Δｌｊ＝（∂Ｌｊ／∂ｘ）Δｘ＋（∂Ｌｊ／∂ｙ）Δｙ＋・・・＋（∂Ｌｊ／∂ψ）Δψ
となるため、
【数２２】
2006095621.tif 000003
の表現が成立する。
【００２４】
任意時刻ｔにおける実座標値（ｘ，ｙ，ｚ，φ，θ，ψ）を用いれば、数２２式の右辺の行列は、定数行列となり、その逆行列：Ａ：を数２２式の左から乗ずれば、
【数２３】
2006095621.tif 000004
の関係式が得られる。ここにおいて、定数行列：Ａ：の要素ａｉｊは、実座標値（ｘ，ｙ，ｚ，φ，θ，ψ）の一般化座標値（ｌ１，・・・，ｌ６）に関する偏微分値、つまり、ａ１ｊ＝∂ｘ／∂ｌｊ，ａ２ｊ＝∂ｙ／∂ｌｊ，・・・，ａ６ｊ＝∂ψ／∂ｌｊを示している。この演算を、ある単位時間ΔＴ毎に実行すれば、リアルタイムでのａｉｊが導出できる。これにより、数１１式におけるＢｊｋ，数１６式におけるＰｊｋ，数１８式におけるａ３ｊが求まる。
【００２５】
次に、今サイクルで求めたＢｊｋをＢｊｋ（ｎ），先サイクルで求めたＢｊｋをＢｊｋ（ｎ−１）とすると、数１１式におけるｄＢｊｋ／ｄｔは、近似的に、
［数２４］  ｄＢｊｋ／ｄｔ＝｛Ｂｊｋ（ｎ）−Ｂｊｋ（ｎ−１）｝／ΔＴ
として求まる。数１６式におけるｄＰｊｋ／ｄｔも同様である。
【００２６】
リアルタイムでの２階偏微分値∂ａｉｊ／∂ｌｋは、次の様に導出する。今サイクルのａｉｊ（ｎ）に対応する第ｋリンク位置指令値をＬｋ（ｎ）とする。ここで、ΔＴが微小時間ならば近似的に、
［数２５］  
ａｉｊ（ｎ−１）−ａｉｊ（ｎ）＝Σ｛∂ａｉｊ／∂ｌｋ｝｛ｌｋ（ｎ−１）−ｌｋ（ｎ）｝
＝Σ｛∂ａｉｊ／∂ｌｋ｝｛Ｌｋ（ｎ−１）−Ｌｋ（ｎ）｝
ａｉｊ（ｎ−２）−ａｉｊ（ｎ）＝Σ｛∂ａｉｊ／∂ｌｋ｝｛Ｌｋ（ｎ−２）−Ｌｋ（ｎ）｝
以下、ａｉｊ（ｎ−３）−ａｉｊ（ｎ），・・・についても同様な関係が成立するため、ａｉｊ（ｎ−６）−ａｉｊ（ｎ）まで用いて、
【数２６】
2006095621.tif 000005
と表現すれば、右辺の定数行列の逆行列：Ｄ：を数２６式の左から乗ずることで、リアルタイムの２階偏微分値∂ａｉｊ／∂ｌｋが導出できる。以上、一連の方法を合わせて、数１２式におけるＣｊｍｎ，数１７式におけるＱｊｍｎが求まる。
【００２７】
なお、ｄｌｋ／ｄｔ，ｄ２ｌｋ／ｄｔ２  は、ｄＬｋ／ｄｔ，ｄ２Ｌｋ／ｄｔ２  と等価であるから、Ｌｋ（ｎ），Ｌｋ（ｎ−１），Ｌｋ（ｎ−２）を用いて、数２４式同様に、
［数２７］  ｄｌｋ／ｄｔ＝｛Ｌｋ（ｎ）−Ｌｋ（ｎ−１）｝／ΔＴ
［数２８］  ｄ２ｌｋ／ｄｔ２＝｛Ｌｋ（ｎ）−２Ｌｋ（ｎ−１）＋Ｌｋ（ｎ−２）｝／（ΔＴ）２
として求まる。以上で各リンクのサーボモータの推力Ｆｊが、数１９式によって、リアルタイムで演算できる。
【実施例１】
【００２８】
次に、上述の推力算出方式を利用した位置制御装置について説明する。図１は、本発明による位置制御装置に、共通して含まれる本発明の推力演算部の構成を示すブロック図である。なお、位置指令演算部の構成と動作は、従来同様なので説明は省略する。数２０式は解析的に求まるから、予め、その偏微分の関数形式を推力演算部に設定しておく。ａｉｊ演算部は、前記偏微分の関数形式にリアルタイムの実座標位置指令値（ｘ，・・・，ψ）を代入して、数２２式右辺の行列を定数行列化し、その逆行列：Ａ：を求めることで、リアルタイムの１階偏微分ａｉｊを演算出力する。なお、数２２式右辺の定数行列が正則でなく、逆行列：Ａ：が存在しない時は、先サイクルのａｉｊを今サイクルのａｉｊとして出力する。Ｂｊｋ，Ｐｊｋ演算部は、ａｉｊを用いて、数１１式におけるＢｊｋと数１６式におけるＰｊｋ、及びこれらの時間微分値ｄＢｊｋ／ｄｔとｄＰｊｋ／ｄｔを数２４式に基づいて演算出力する。
【００２９】
∂ａｉｊ／∂ｌｋ演算部は、逆変換部出力であるリアルタイムの各リンク位置指令値（Ｌ１，・・・，Ｌ６）から、数２６式右辺の定数行列の逆行列：Ｄ：を求め、ａｉｊから求めた、数２６式左辺の定数ベクトルの左から：Ｄ：を乗ずることで、リアルタイムの２階偏微分値：∂ａｉｊ／∂ｌｋを演算出力する。なお、数２６式右辺の定数行列が正則でなく、逆行列：Ｄ：が存在しない時は、前記ａｉｊ演算部と同様に、先サイクルの∂ａｉｊ／∂ｌｋを出力する。Ｃｊｍｎ，Ｑｊｍｎ演算部は、加減速演算部出力であるリアルタイムの実座標位置指令値（ｘ，・・・，ψ）と、ａｉｊ演算部及び∂ａｉｊ／∂ｌｋ演算部の出力であるａｉｊ及び∂ａｉｊ／∂ｌｋを用いて、数１２式に基づいてＣｊｍｎを、数１７式に基づいてＱｊｍｎを演算出力する。
【００３０】
速度・加速度演算部は、リアルタイムの各リンク位置指令値（Ｌ１，・・・，Ｌ６）から、数２７式及び数２８式に基づいてｄｌｋ／ｄｔとｄ２ｌｋ／ｄｔ２を演算出力する。最後に、リンク推力合成部は、前記、各演算部で求めた諸量を用いて、数１９式に基づいて各リンクのサーボモータの推力：Ｆｊ（ｊ＝１，・・・，６）をリアルタイムで演算出力する。以上により、本発明の推力演算部から出力される、各リンクのサーボモータの推力Ｆｊは、プラットフォームの動作に関するリアルタイムの運動方程式になっている。
【実施例２】
【００３１】
次に本発明による推力演算部を適用して構成した、本発明の位置制御装置の一例を図２に示す。なお、本ブロック図において、図７（従来例）と同一の部分については、同一の名称及び番号を付けて説明を省略する。更に、各リンク位置制御部は同一構成を有するため、第１リンクを例に、本発明におけるリンク位置制御部の動作を説明する。図２では、加算器１０により、従来のリンク推力指令値Ｆｃに、本発明の推力演算部出力Ｆ１が加算される形態になっている。つまり、本発明では、プラットフォームの動作に関して発生する推力をリアルタイム解析して、リンク推力指令値Ｆｃに、フィードフォワード加算することで、プラットフォームの動作に要する推力に起因した位置偏差（Ｌ１−Ｌ１ｄ）の発生を抑制し、動的な位置制御性能の向上を指向する構成になっている。
【実施例３】
【００３２】
本発明による位置制御装置の他の一例を図３に示す。増幅器１／Ｋ（Ｌ１）は、本発明の推力演算部出力Ｆ１を１／Ｋ（Ｌ１）倍する。ここで、Ｋ（Ｌ１）は、図５における自在継手１０２，１０３と、リンク１０４のリンク軸方向の総合剛性を示している。これは、リンク長Ｌ１の関数として、予め各リンク位置制御部に設定されている。これにより、増幅器１／Ｋ（Ｌ１）の出力ε１は、プラットフォームの動作に関して発生する、リンク駆動系の撓み量を示す。リンク撓み量ε１は、リンク位置指令値Ｌ１と加算器１３で加算され、減算器５０で、リンク位置検出値Ｌ１ｄとの差分がとられる。つまり、本発明におけるリンク位置制御部では、プラットフォームの動作に起因して発生する駆動系の撓み量を考慮したリンク位置指令値が内部的に発生する。
【００３３】
リンク撓み速度量εｖ１は、リンク撓み量ε１を微分器１１で時間微分した出力で、加算器１２により、従来のリンク速度指令値Ｖｃに、本発明のリンク撓み速度量εｖ１が加算される形態になっており、撓み量を考慮したリンク位置指令値通りに、リンク位置が制御されるのを支援する構成になっている。以上のことから、本発明では、プラットフォームの動作に関して発生する推力をリアルタイム解析して、この推力に起因して発生する駆動系の撓み量をリアルタイムで求め、これを考慮したリンク位置制御を行なうことで、動的なプラットフォームの位置及び姿勢の高精度制御化を指向する構成になっている。
【実施例４】
【００３４】
本発明による位置制御装置の他の一例を図４に示す。図４では、第１リンク位置制御部の速度偏差増幅器Ｇｖ出力をリンク推力Ｆｃ１として取り出している。リンク推力Ｆｃ１は、リンク推力全体を意味するリンク推力指令値Ｆｃから、サーボモータ５７分の加減速推力を差し引いたものである。なお、各リンクにおけるＦｃ２〜Ｆｃ６も同様である。本発明の衝突検出部において、減算器１４はリンク推力Ｆｃ１から、推力演算部出力Ｆ１を差し引いているため、その出力は、ほぼ、パラレルメカニズム工作機械の切削加工における第１リンク方向の負荷力と考えることができる。
【００３５】
増幅器：Ｌ１：／｜Ｌ１｜は、減算器１４出力を：Ｌ１：／｜Ｌ１｜倍する。これは、大きさ１倍で、第１リンク方向の負荷力に方向付けを行なう増幅器であり、その出力は、リンク負荷力：Ｆｅｘｔ１：になる。同様に演算される第２リンクから第６リンクまでの各リンク方向の負荷力：Ｆｅｘｔ２：〜：Ｆｅｘｔ６：は、本発明の負荷反力検出部で、全て−１倍された後、ベクトル合成され、基準座標上で見た負荷反力ベクトルになる。この大きさが、衝突検出部に予め設定された基準値と比較して大きい時、負荷反力検出部は衝突（過大負荷）を検出して、負荷反力方向の情報を持った衝突検出信号ＣＲＤを上位装置（図示しない。）に出力する。上位装置は、負荷反力方向の動作軌跡を持つ実座標値（ｘ，・・・，ψ）を位置指令値ＰＤとして、位置指令演算部に入力し、プラットフォームの退避動作を実行する。
【００３６】
本実施例では、速度偏差増幅器Ｇｖ出力をリンク推力Ｆｃ１として取り出しているが、実施例２（図２）のリンク位置制御部における速度偏差増幅器Ｇｖ出力を取り出して、本実施例の衝突検出部の増幅器：Ｌ１：／｜Ｌ１｜に直接入力する構成をとってもよい。以上、本発明による位置制御装置の実施例では、プラットフォームの動作に関して発生する推力をリアルタイム解析することで、負荷反力ベクトルが検出でき、その大きさと方向から衝突現象と退避方向を検出できる。
【００３７】
なお、本実施例では、負荷反力検出部に衝突を検出する機能を持たせたが、検出される負荷反力ベクトルから、負荷表示を行なったり、負荷反力が大きい時は加工速度を落とすなど、想像される制御状態に応じた機能を搭載することが可能である。
【００３８】
以上説明した様に本発明によれば、パラレルメカニズム工作機械のプラットフォームの動作に関する各リンク推力をリアルタイムに求めることができるから、これを搭載した本発明の位置制御装置では、動的なプラットフォームの位置及び姿勢の位置制御に関して、位置制御の高性能化や高精度化が指向できる。更に、負荷反力ベクトルが検出できるから、プラットフォームの制御状態を想像でき、衝突及び退避方向の検出などが可能になる。
【図面の簡単な説明】
【００３９】
【図１】本発明の推力演算部の構成を示すブロック図である。
【図２】本発明による位置制御装置の一例を示すブロック図である。
【図３】本発明による位置制御装置の他の一例を示すブロック図である。
【図４】本発明による位置制御装置の他の一例を示すブロック図である。
【図５】パラレルメカニズム工作機械の全体図である。
【図６】パラレルメカニズム工作機械をモデル化した図である。
【図７】従来の位置制御装置の一例を示すブロック図である。
【符号の説明】
【００４０】
１，２，３  ベース各頂点、１’，２’，３’  プラットフォーム各頂点、１０，１２，１３，５１，５６  加算器、１４，１５，１６，５０，５２  減算器、１１，５３，５４，５５  微分器、５７，１０９  サーボモータ、５８  位置検出器、１０１  テーブル、１０２，１０３  自在継手、１０４  リンク、１０５  ベース（天板）、１０６  プラットフォーム、１０７  回転工具、１０８  ＮＣ装置。
【図１】
2006095621.tif 000006
【図２】
2006095621.tif 000007
【図３】
2006095621.tif 000008
【図４】
2006095621.tif 000009
【図５】
2006095621.tif 000010
【図６】
2006095621.tif 000011
【図７】
2006095621.tif 000012