JP4962757B2 - 空間３自由度パラレル機構の制御方法および空間３自由度パラレル機構 - Google Patents

Description

本発明は、工作機械の主軸頭やテーブルを３自由度で移動位置決めさせるパラレル機構の制御方法に関し、特に航空機部品を加工するミーリング機械に適用するに好適な空間３自由度パラレル機構の制御方法に関する。

この種のパラレル機構としては、米国特許ＵＳ６，４３１，８０２Ｂに開示された図１０のようなものがある。これは、３本のボールスクリュー５でＺ軸方向に駆動される３つのキャリッジ３により主軸頭１２をパラレル駆動するものである。３つのキャリッジ３と主軸頭１２は３つの連結ロッド（８、９）により連結されている。キャリッジ３と連結ロッド８との連結手段はＺ軸に垂直なピン１０により、主軸頭１２と連結ロッド９との連結手段はユニバーサルジョイント１１によりなされている。この機構は簡略であるという利点がある。しかし、主軸頭の位置、角度を主軸頭から離れた３つのキャリッジ３の位置で検出し制御するため、連結ロッド８，９などの部品寸法に加工誤差があると直接位置決め誤差に響いてしまう。また、連結ロッド８，９等の熱変形による誤差が補正できない、といった問題点があった。このような誤差が出たまま制御すると、位置決め誤差が悪くなる、位置制御の制御性（応答性）が悪くなるという問題点があった。また、ピン１０による連結であるので、換言するとロッド８，９両端のジョイントの自由度の和が４であり５より少ないため、連結ロッド９に曲げ応力が掛かり機構としての剛性が弱くなる、という問題点があった。

また、ヨーロッパ特許ＥＰ１，２４５，３４９Ｂには、図１１に示すようなものが開示されている。これは、Ｚ方向の３本のガイドウエイ２にスライダ４を乗せ、各スライダ４にペアとなったアーム３を連結し６本のアーム３で主軸頭に相当するヘッドプラットホーム５を支承する。各アーム３とスライダ４及びヘッドプラットホーム５との連結には球面ジョイント６，７が用いられる。この装置は球面ジョイント６、７を使用しているのでアーム３に曲げ応力が掛からず剛性が高い。しかし、ヘッドプラットホーム５から離れたスライダ４の位置でヘッドプラットホーム５の位置姿勢を検出し制御するため、上記と同様に、アーム３などの部品寸法に加工誤差があると直接位置決め誤差に響いてしまう。また、アーム３等の熱変形による誤差が補正できない、といった問題点があった。このような誤差を残したまま制御すると、位置決め誤差が悪くなる、位置制御の制御性（応答性）が悪くなるという問題点がある。また、隣接したアーム３とスライダ４、ガイドウエイ２が干渉するため、ヘッドプラットホーム５の旋回範囲が小さくなるという問題点があった。

日本特許、特開平１１−１０５７５号には、図１２に示すようなものが開示されている。これは、エンドプレート６を駆動する３本のリンク７及びボールねじ１９の他に、エンドプレート６の位置姿勢を検出するためのマスト１を設け、このマスト１のベースプレート３位置付近での位置姿勢を検出するようにしたものである。これも、位置検出精度は上記２件のものより良くなるものの、エンドプレート６から離れた位置で検出しているため、上記と同様に、マスト１の部品寸法に加工誤差があると直接位置決め誤差に響いてしまう。また、マスト１等の熱変形による誤差が補正できない、といった問題点があった。このような誤差を残したまま制御すると、位置決め誤差が悪くなる、位置制御の制御性（応答性）が悪くなるという問題点がある。また、この装置では、検出されたマスト１の３軸（ｌ、θ、φ）を逆変換して各アクチュエーターの指令値にフィードバックをかけているが、逆変換関数の機構パラメータに製造誤差、熱変位等により実際の機械と誤差が生じた場合、いかんとも対処できないという問題点があった。

また、出願人はかって、特開２００２−２６３９７３号で、６本のロッドでトラベリングプレートを保持するパラレルリンク機構であって演算位置情報と測定位置情報から機構パラメータの校正を行うものを提案した。しかしながら、測定位置情報の基礎となる現在位置の検出器が６個のモータ位置検出用エンコーダと各ロッドの回転角を検出する６個の角度センサであるので、トラベリングプレートの位置を直接的に検出するものではない。このため、１２個の検出器により位置を検出し５自由度のトラベリングプレートの機構パラメータを推定しようとするものであり、演算負荷が大きいという問題点があった。また、トラベリングプレートから離れた箇所で現在位置を検出しフィードバックをかけて制御することになるから、機構パラメータに誤差があると、位置決め誤差が悪くなる、位置制御の制御性（応答性）が悪くなる、という問題点があった。さらに、検出軸が１２軸となって機構が複雑であるという問題点もあった。

米国特許ＵＳ６，４３１，８０２Ｂ ヨーロッパ特許ＥＰ１，２４５，３４９Ｂ 特開平１１−１０５７５号 特開２００２−２６３９７３号

本発明は、上記の問題点を解決するためなされたものであり、その目的とするところは、機構を構成する部品の加工誤差、熱変位等による誤差を排除することにより、位置決め精度を高めると共に、位置制御の制御性（応答性）の良い空間３自由度パラレル機構の制御方法および装置を提供することにある。

上記の目的を達成するため、第１の態様の発明では、相対的に静止した系に３つの自由度で支承された可動体を３つのアクチュエーターにより駆動し３自由度で移動させる空間３自由度パラレル機構の制御方法であって、前記３つの自由度に対応した３軸の座標系での可動体の座標位置を検出する３つの可動体位置検出器が、各軸の軸上若しくは軸と平行な線上であって前記可動体の近傍に配置され、前記可動体の３軸の指令位置を機構パラメータに基づいて逆変換し前記アクチュエーターの逆変換指令位置に変換する指令位置逆変換手順と、前記３つの可動体位置検出器で検出された前記可動体の実測位置を前記機構パラメータに基づいて逆変換し前記アクチュエーターの逆変換実測位置を求める実測位置逆変換手順と、前記逆変換指令位置に前記逆変換実測位置をフィードバックして前記可動体の位置を制御する制御手順と、複数の前記逆変換指令位置とそれにそれぞれ対応する複数の前記逆変換実測位置の値を記憶する記憶手順と、前記記憶された複数の逆変換指令位置及び対応する逆変換実測位置に基づいて前記指令位置から前記逆変換指令位置への逆変換式に含まれる前記機構パラメータを同定する機構パラメータの同定手順と、所定のキャリブレーションタイミングで前記記憶された逆変換指令位置及び逆変換実測位置に基づいて同定された機構パラメータに従って古い機構パラメータを補正するキャリブレーション手順と、を備えることを特徴とする空間３自由度パラレル機構の制御方法が提供される。

上記のように構成すると、複数の逆変換指令位置及び対応する逆変換実測位置に基づいて機構パラメータを同定する同定手順を有するから、設計値だけではなく機構部品の加工誤差等も考慮に入れた機構パラメータが同定される。さらに、その機構パラメータが刻々と得られる逆変換指令位置及び対応する逆変換実測位置に基づいて補正、キャリブレーションされるから機構部品の熱変位にも対応して現在の正確な機構パラメータにより制御され、より正確な位置決めがされる。さらに、より正確な最新の機構パラメータによる位置を基礎にフィードバック制御がなされるから位置制御の制御性（応答性）が格段に良くなり、動作速度や軌跡精度が高いという優れた効果を奏する。

ここで、第２の態様の発明として、前記記憶された逆変換指令位置及び逆変換実測位置に基づいて前記所定のキャリブレーションを行うタイミングが、前記可動体が工作物の加工を行っていないタイミングである、ことを特徴とすることができる。このように構成すると、一つの工作物を加工中に機構パラメータが変化することが無くなり、工作物に機構パラメータの変化（補正）による段差等が付かないという効果がある。

そして、第３の態様の発明として、前記複数の逆変換指令位置の基礎となる指令位置が、機構パラメータの同定に必要な特別な複数の位置である、ことを特徴とすることができる。このように構成すると、通常の切削加工ではめったに移動しない位置まで機械を移動位置決めし、バランスのとれた位置で同定情報を収集するから、機構パラメータ同定の正確さが増し、また、同定の収束計算が速いという効果がある。

第４の態様の発明として、前記実測位置逆変換手順における前記可動体の実測位置の検出を、前記可動体による加工物の加工中に行うことを特徴とすることができる。このように構成すると、特別の動作をさせなくても機構パラメータの同定ができるという効果がある。また、実際の切削加工中の機構部品の熱変位に基づいた同定ができるという効果がある。

第５の態様の発明として、前記機構パラメータの同定手順が、パラメータ誤差が所定許容値以下になるまで収束計算を行う収束計算手順を備える、ことを特徴とすることができる。このように構成すると、パラメータ誤差を確実に許容値以下にすることができ、加工精度が向上するという効果がある。

第６の態様の発明として、前記機構パラメータの同定手順が、前記機構パラメータの数より多い数の前記逆変換式が得られるような多数の前記指令位置での逆変換実測位置に基づいて最小２乗法により算定される最小２乗法手順を備える、ことを特徴とすることができる。このように構成すると、冗長度を持った指令位置情報、検出位置情報から正しい機構パラメータに高速で近づくことができるという効果がある。

以上述べた６つの態様の発明は、いずれも方法の発明として記載したが、これらは物の発明として定義することも可能である。そこで、第７の発明の態様として、相対的に静止した系に３つの自由度で支承された可動体を３つのアクチュエーターにより駆動し３自由度で移動させる制御装置を有する空間３自由度パラレル機構であって、前記３つの自由度に対応した３軸の座標系での可動体の座標位置を検出する３つの可動体位置検出器が、各軸の軸上若しくは軸と平行な線上であって前記可動体の近傍に配置されていることと、前記制御装置が、前記可動体の３軸の指令位置を機構パラメータに基づいて逆変換し前記アクチュエーターの逆変換指令位置を求める指令位置逆変換手段と、前記３つの可動体位置検出器で検出された前記可動体の実測位置を前記機構パラメータに基づいて逆変換し前記アクチュエーターの逆変換実測位置を求める実測位置逆変換手段と、前記逆変換指令位置に前記逆変換実測位置をフィードバックして前記３つのアクチュエーターを駆動し前記可動体の位置を制御する制御手段と、複数の前記逆変換指令位置にそれぞれ対応する複数の前記逆変換実測位置の値を記憶する逆変換指令位置逆変換実測位置記憶手段と、前記逆変換指令位置逆変換実測位置記憶手段に記憶された複数の逆変換指令位置及び対応する逆変換実測位置に基づいて前記指令位置から前記逆変換指令位置への逆変換式に含まれる前記機構パラメータを同定する機構パラメータの同定手段と、所定のキャリブレーションタイミングで前記記憶された逆変換指令位置及び逆変換実測位置に基づいて同定された機構パラメータに従って古い機構パラメータを補正するキャリブレーション手段と、を備えることを特徴とする空間３自由度パラレル機構、が提供される。

このように構成すると、可動体の座標位置を検出する３つの可動体位置検出器が可動体の近傍に配置されているため、可動体から位置検出器までの部材の熱変形等による位置検出誤差が生じにくく、可動体のより正確な位置を検出しフィードバックすることができる。このため、より正確な位置制御が可能になると共に同定される機構パラメータもより正確になるという効果がある。また、古い機構パラメータを補正するキャリブレーション手段を備えるから、より正確な最新の機構パラメータを用いてのフィードバック制御が可能になり、その結果、制御の応答性（速応性）に優れ、動作速度や軌跡精度が高いという優れた効果がある。

さらに、第８の態様の発明として、前記アクチュエーターの位置を検出するアクチュエーター位置検出器を備え、前記３つの可動体位置検出器がそれぞれ前記アクチュエーター位置検出器よりも前記可動体の近傍に配置されていることを特徴とすることができる。このように構成すると、３つの可動体位置検出器が可動体のより近傍に確実に配置されるため、より正確な位置を検出しフィードバックすることができる。このため、制御性がより良くなるという優れた効果を奏する。

以上説明したように、本発明によれば、複数の逆変換指令位置及び対応する逆変換実測位置に基づいて機構パラメータを同定する同定手順を有するから、設計値だけではなく機構部品の加工誤差等も考慮に入れた機構パラメータが同定される。さらに、その機構パラメータが刻々と得られる逆変換指令位置及び対応する逆変換実測位置に基づいて補正、キャリブレーションされるから機構部品の熱変位にも対応して現在の正確な機構パラメータにより制御され、より正確な位置決めがされると共に、より正確な機構パラメータによる位置を基礎にフィードバック制御がなされるから位置制御の制御性（応答性）が格段に良くなるという優れた効果を奏する。その結果、制御の応答性（速応性）に優れ、動作速度や軌跡精度が高いという優れた効果がある。

図１は、本発明の第１の実施の形態を示す空間３自由度パラレル機構の斜視図である。円筒状のハウジング（図示せず）に３本の直線ガイド２１が固定されている。図示しないハウジングは相対的に静止した系である。３本の直線ガイド２１は互いに平行でＺ軸方向を向き１２０°毎に配置されている。その３本の直線ガイド２１上に案内されＺ軸方向に移動自在に３つの従動スライダ２２が設けられている。その３つの従動スライダ２２に環状の第１の可動サポート体２３が固定されている。従って、第１の可動サポート体２３はＺ軸方向にのみ移動自在である。３つのうち１つの従動スライダ２２にリニアエンコーダ４５が組み込まれており、第１の可動サポート体２３のＺ軸位置を検出するＺ軸位置検出器４５をなしている。

その第１の可動サポート体２３に半径中心方向に突出して設けられた軸２４により、Ｂ軸方向に回転自在に第２の可動サポート体２５が支承されている。第２の可動サポート体２５は環状の部品である。第２の可動サポート体２５の回転位置は軸２４に設けられたロータリエンコーダ４７により検出される。このロータリエンコーダ４７は第２の可動サポート体２５の回転軸であるＢ軸位置を検出するＢ軸位置検出器４７をなしている。

第２の可動サポート体２５上の上記Ｂ軸と垂直な方向に、中心を通る中心軸２６がロータリージョイント２７，２７により回転自在に第２の可動サポート体２５に支承されている。一方のロータリージョイント２７にはロータリエンコーダ４６が組み込まれ中心軸２６の回転位置すなわちＡ軸位置を検出する。ロータリージョイント２７に組み込まれたロータリエンコーダ４６はＡ軸位置を検出するＡ軸位置検出器４６をなしている。上記の第１の可動サポート体２３、第２の可動サポート体２５、中心軸２６により可動体サポート体を構成する。

中心軸２６の中央部には主軸頭３０が一体に固定されている。主軸頭３０は可動体を成すものであり、Ｚ軸方向に移動可能であり、Ａ軸、Ｂ軸の回りを回転可能である。換言すれば、Ｘ軸、Ｙ軸方向の移動、Ｃ軸回りの回転を拘束されている。主軸頭３０には主軸モータが搭載され主軸に装着された工具３１を回転駆動し加工物を切削加工する。Ａ軸とＢ軸との交差点３２が主軸頭３０の回転中心である。

一方、３本の直線ガイド２１には従動スライダ２２とは別に駆動スライダ３３が配設され直線移動自在に支持されている。各駆動スライダ３３は、各直線ガイド２１に一端が固定された直線アクチュエーター３４の出力端に連結され、Ｚ軸方向に直線駆動され移動される。各直線アクチュエーター３４にはアクチュエーター軸モータ３８が内蔵されている。各駆動スライダ３３、３３、３３と主軸頭３０の底部は各ロッド３５、３５、３５により連結されている。すなわち、各駆動スライダ３３と各ロッド３５とは３自由度を有する球面ジョイント３６により連結され、各ロッド３５と主軸頭３０とは２自由度を有するユニバーサルジョイント３７により連結されている。

ここで、３つの駆動スライダ３３、３３、３３のＺ軸位置座標をそれぞれＵ_１ 、Ｕ_２ 、Ｕ_３ とし、その位置座標を検出するリニアエンコーダ４１、４２、４３はそれぞれの駆動スライダ３３、３３、３３に組み込まれている。また、主軸頭３０のＺ軸位置を検出するリニアエンコーダ４５は１つの従動スライダ２２の中に、Ａ軸の位置座標を検出するロータリエンコーダ４６はロータリージョイント２７の中に、Ｂ軸の位置座標を検出するロータリエンコーダ４７は軸２４の中に、それぞれ組み込まれていることは前述の通りである。このように、主軸頭３０の位置（ｚ，Ａ，Ｂ）を直接的にエンコーダからなる各位置検出器４５、４６、４７で検出できる点が、本実施の形態の利点である。このため主軸頭３０の現在位置検出精度が本質的に高くなる。

従って、３つの直線アクチュエーター３４，３４，３４を駆動し、３つの駆動スライダ３３，３３，３３のＺ軸上の位置（Ｕ_１ 、Ｕ_２ 、Ｕ_３ ）を制御することにより主軸頭３０のＺ軸、Ａ軸、Ｂ軸の位置を制御することができる。例えば、３つの駆動スライダ３３，３３，３３（Ｕ_１ 、Ｕ_２ 、Ｕ_３ ）を同時に同じ距離だけ負方向へ移動させると主軸頭３０はその姿勢を保ったまま−Ｚ軸方向に移動する。また、図面で上の直線ガイド２１上の駆動スライダ３３（Ｕ_１ ）を前進させ、下の２本の直線ガイド２１，２１上の駆動スライダ３３，３３（Ｕ_２ 、Ｕ_３ ）を後退させると、主軸頭３０はＢ軸の回りを回転する。３つの駆動スライダ３３，３３，３３の位置（Ｕ_１ 、Ｕ_２ 、Ｕ_３ ）により主軸頭３０の位置姿勢がどのように変わるかは、ロッド３５の長さ、主軸頭３０での連結の位置、等で決定される機構パラメータＰにより決定される。換言すれば、主軸頭３０のＺ軸、Ａ軸、Ｂ軸位置は３つの駆動スライダ３３，３３，３３の位置（Ｕ_１ 、Ｕ_２ 、Ｕ_３ ）と機構パラメータＰの関数として表現される。

次に、主軸頭３０の座標（ｚ、Ａ、Ｂ）から３つの直線アクチュエーター３４，３４，３４つまり３つの駆動スライダ３３，３３，３３のＺ軸上の位置（Ｕ_１ 、Ｕ_２ 、Ｕ_３ ）を求める逆変換式について説明する。
図２は、図面上方の駆動スライダ３３の位置Ｕ_１ を求める逆変換式を説明する空間３自由度パラレル機構の斜視図である。主軸頭３０には工具３１が装着されている。主軸頭３０の回転中心の座標がＯである。ロッド３５のジョイント位置は、トラベリングプレート座標系から見るとＳ_ＯＴであり、グローバル座標系から見るとＳ_Ｏ である。直線ガイド２１の前端の座標がＳ_１ 、後端の座標がＳ_２ 、駆動スライダ３３の座標がＵ_１ である。ロッド３５の長さをＲＬとする。

主軸頭３０の回転中心の座標Ｏは、数式１で表される。

トラベリングプレート座標系から見たロッド３５のジョイント位置Ｓ_ＯＴは、数式２で表される。

これにより、グローバル座標系におけるジョイント位置Ｓ_Ｏ が数式３のように表現される。

ここで、Ｒは数式４で示されるマトリックスである。

直線ガイド２１の両端の位置をＳ_１ 、Ｓ_２ とし、Ｓ_０ 、Ｓ_１ の辺と、ロッド３５の辺と、Ｓ_１ ，Ｕ_１ （駆動スライド３３の位置）の辺とからなる三角形に余弦定理を適用すれば、ロッド３５の長さをＲＬとして、数式５のようになる。

ここで、最後の余弦項が＋になっているのは、Ｓ_２１＝Ｓ_２ −Ｓ_１ と逆方向のベクトルとなっているためである。
数式５をアクチュエーター座標Ｕ_１ について解けば数式６が得られる。

このときアクチュエーター座標Ｕ_１ は、主軸頭３０の位置Ｘ＝（ｚ，Ａ、Ｂ）の関数として得られるので、数式７のように表現することができる。

ここで、数式８で表されるベクトルＰは、アクチュエーターの位置Ｕ_１ 、Ｕ_２ 、Ｕ_３ と主軸頭３０の位置及び姿勢Ｘ＝（ｚ，Ａ、Ｂ）との幾何学的な関係を定義する機構パラメータであり、たとえば、ロッド３５の長さや、主軸頭３０上のジョイント位置、駆動スライド３３の位置、角度、等を表す「機構パラメータ」である。ここで、ｎは機構パラメータの数である。

次に、機構パラメータの同定演算について説明する。主軸頭位置Ｘ_ｉ は数式９で表される。

ここで、ｉが１からｋまであるのは、ｋ箇所での主軸頭位置を示している。
主軸頭位置Ｘ_ｉ におけるアクチュエーター座標Ｕ_１ 、Ｕ_２ 、Ｕ_３ への逆変換式は、数式１０で表現することができる。

それぞれの機構パラメータについて全微分すると、数式１１が得られる。

ｋ個の主軸頭位置について上式がｋ組得られることになる。つまり、ｋ個の主軸頭位置について３ｋ個の逆変換式が得られることになる。機構パラメータの数ｎより多い数の逆変換式が得られるように（ｎ≧３ｋとなるように）、主軸頭位置の数ｋを決める。これらを行列表記すると、数式１２が得られる。

数式１２を書き直せば、数式１３のように表現できる。

したがって、パラメータ誤差ｄＰは、最小２乗法による解として、数式１４のようになる。

ここでＪ^＊ は、疑似逆行列であり、たとえば、数式１５で求められる。

パラメータ誤差ｄＰを求めるのに最小２乗法の手法が用いられる。
これにより、１回目の機構パラメータの推定値Ｐ１が、数式１６のように求められる。

関数ｆは非線形であるため、初回に求められたＰ１を新たな機構パラメータ値として、上記演算を数式１７のように繰り返し、パラメータ誤差ｄＰが所定の許容値ε以下になるまで、収束計算を実行する。

図３は、主軸頭位置（ｚ，Ａ、Ｂ）をアクチュエーター位置（Ｕ_１ 、Ｕ_２ 、Ｕ_３ ）に逆変換しフィードバック制御をする制御装置のブロック図である。主軸頭位置指令位置Ｘ（ｚ，Ａ、Ｂ）と、各エンコーダ５１（図１の４５、４６、４７）からの主軸頭位置エンコーダ値Ｘ’（ｚ’，Ａ’、Ｂ’）をそれぞれの逆変換手段５２、５３でアクチュエーター座標Ｕ（Ｕ_１ 、Ｕ_２ 、Ｕ_３ ）、Ｕ’（ Ｕ_１’、Ｕ_２’、Ｕ_３’）に逆変換する。逆変換された指令位置Ｕとエンコーダ値Ｕ’は各軸毎に加算器５４に入れられ減算されて差分が取られ（フィードバックされ）、位置誤差として出力される。位置誤差は位置比例ゲイン増幅器５５で位置ループゲインＫｐｐ倍されて出力される。位置比例ゲイン増幅器５５の出力は加算器５６に送られ、軸の速度出力との差分が取られ速度フィードバックがかけられる。加算器５６の出力は速度比例ゲイン増幅器５７で速度ループゲインＫｖｐ倍されて図示しないサーボアンプに送られ、アクチュエーター軸モータ５８（図１で３８、３８、３８）を駆動する。アクチュエーター軸モータ５８の速度出力は加算器５６にフィードバックされる。アクチュエーター軸モータ５８の出力により各軸が動きアクチュエーター軸エンコーダ値Ｕ_１ が変化する。つまり、速度出力の積分処理５９である。

図４は、制御装置の第２の実施例を示すブロック図である。図３と同じ部材には同じ符号を付して説明を省略する。上記の図３の例では、速度ループのフィードバック情報をアクチュエーター軸モータ５８の速度出力から取ったが、ここでは主軸頭位置エンコーダ値Ｘ’の逆変換値Ｕ’を微分器６０で微分することにより速度情報を得、速度フィードバック値として加算器５６に返している。

上記図３、図４のブロック図において加算器、増幅器等がハードとして存在するかのように記載して説明したが、これらの機能は全てＭＰＵの処理として実現されることは云うまでもない。

図５は、機構パラメータＰの同定作業を行う処理を示したフローチャートである。処理が始まると、まずステップＳ０１で、そのときの指令位置を記憶する。同時に、ステップＳ０２で、そのときのエンコーダ測定位置を記憶する。次に、ステップＳ０３で、現在が機構パラメータの同定を行う時機か否かが判断される。同定を行う時機か否かの判断は、使用する工作機械、工作物により異なるが、例えば、ある程度のデータが記憶蓄積されたとき、あるいは、数個の加工物の加工が終了したとき、等が考えられる。ＮＯであればステップＳ０１に戻り情報の収集を続け、ＹＥＳであればステップＳ０４に進み機構パラメータの同定作業を実行する。

ステップＳ０４では、数式１４にしたがってパラメータ誤差ｄＰが計算される。パラメータ誤差ｄＰを計算する際に、冗長度の高い情報から演算するため最小２乗法の手順が用いられる。次に、ステップＳ０５では、従前の機構パラメータＰに算出されたパラメータ誤差ｄＰを加えて機構パラメータＰの補正、キャリブレーションを行う。数式１６の作業である。次に、ステップＳ０６で、パラメータ誤差ｄＰが許容値εより小さいか否かが判断される。所定値εより大きくＮＯであればステップＳ０４に戻り、補正された機構パラメータＰに基づいて新たなパラメータ誤差ｄＰを算出する。そして、ステップＳ０５では、従前の機構パラメータＰに算出されたパラメータ誤差ｄＰを加えて機構パラメータＰの再補正、再キャリブレーションを行う。ステップＳ０４からステップＳ０６を何度も繰り返し、パラメータ誤差ｄＰが許容値εより小さくなるのを待つ。数式１７の収束計算の作業である。

収束計算の結果、パラメータ誤差ｄＰが許容値εより小さくなると、ステップＳ０７に進み、収束計算された新しい機構パラメータＰを記憶する。次に、ステップＳ０８で現在加工中の加工物の加工の終了を待つ。これは、１つの加工物の加工中に機構パラメータＰが変更されると、加工面に小さな段差が付いたりするおそれがあるので加工の終了を待つのである。加工が終了すると、ステップＳ０９に進み、機構パラメータＰを置き換え更新する。そして、処理を終了する。この処理は、工作機械の加工中も同定情報の収集が行われるから、加工中の熱変位等も機構パラメータに取り入れることができるという利点がある。

上記Ｓ０１〜Ｓ０９の処理では、加工物を加工中に位置情報等を収集することとしたが、加工をしていない状態で工作機械を所定の特別な複数の位置に位置決めし、そのときの位置情報等から機構パラメータＰの同定を行っても良い。特別な複数の位置としては、たとえば、Ｚ＝０、−１００、−２００、（単位はｍｍ）、Ａ＝−３０、−１５、０、＋１５、＋３０、（単位は度）、Ｂ＝−３０、−１５、０、＋１５、＋３０、（単位は度）、の組合せで、３×５×５＝７５の座標ができ、逆変換式が７５×３＝２２５組の位置情報が収集できる。これらの式から機構パラメータの同定を行う。通常、機構パラメータの数より式の数を多くし、最小２乗法により測定誤差の影響を最小化する。

図６は、機構パラメータＰの同定に必要な特別な複数の位置に機械を位置決めし、そのときのエンコーダ位置情報から機構パラメータＰの同定を行う処理を示すフローチャートである。特別な複数の位置としては上記の７５個の座標を採用するものとする。ステップＳ１１では、機構パラメータの同定時機を待つ。同定時機としては工作機械を動作させる最初の時期、同じ工作物の加工が終わって次の種類の工作物を加工する前、等が考えられる。ステップＳ１２では、７５個の座標のうち次に測定する座標に各軸を移動させる。そして、ステップＳ１３で、指令位置を記憶し、ステップＳ１４で、各軸エンコーダによる測定位置を記憶する。そして、ステップＳ１５で、７５の同定座標位置を全て終了したか否かを調べ、未だ終了していなければステップＳ１２に戻り、同定情報収集を繰り返す。同定情報収集が終了したら、ステップＳ１５からステップＳ１６に移る。

ステップＳ１６からステップＳ１９は、図５のステップＳ０４からステップＳ０７と全く同じである。すなわち、機構パラメータＰの誤差ｄＰを計算し、機構パラメータＰの補正、キャリブレーションを行い、誤差ｄＰが許容値ε以下になるまで収束計算を行う。そして、補正された新しい機構パラメータＰを記憶する。最後に、ステップＳ２０で、従前の機構パラメータＰを新しい機構パラメータＰと置き換え、処理を終了する。この処理は、通常の切削加工ではめったに移動しない位置まで機械を移動位置決めし、バランスのとれた位置で同定情報を収集するから、収束計算が速いという利点がある。

図７は、本発明の第２の実施の形態を示す斜視図である。図１と同じ部材には同じ符号を付け説明を省略する。ここでは、ロッド３５ではなく、３本の直線伸縮型アクチュエーター７３により主軸頭３０の位置姿勢が制御される。静止系のプラットフォーム７１に２自由度のユニバーサルジョイント７２により直線伸縮型アクチュエーター７３が３本取り付けられている。直線伸縮型アクチュエーター７３はエンコーダ付き駆動モータ７４を有し、エンコーダ付き駆動モータ７４の回転位置にしたがって出力ロッド７５が伸縮する。直線伸縮型アクチュエーター７３の出力ロッド７５は、３自由度の球面ジョイント７６により主軸頭３０の底部に連結されている。３本の直線伸縮型アクチュエーター７３を伸縮させることにより、主軸頭３０はその位置姿勢を制御される。この実施の形態は、空間３自由度パラレル機構を構成する部品点数が少なくなるという利点がある。

図８は、本発明の第３の実施の形態を示す斜視図である。図１と同じ部材には同じ符号を付け説明を省略する。静止系のプラットフォーム８１に、３つの回転型アクチュエーター８２が取り付けられている。回転型アクチュエーター８２はその回転位置を検出するロータリエンコーダが組み込まれている。回転型アクチュエーター８２の出力端には第１のレバー８３が固定され一体に回転する。第１のレバー８３の先端には２自由度のユニバーサルジョイント８４が連結され、そのユニバーサルジョイント８４に第２のレバー８５が連結されている。第２のレバー８５は球面ジョイント８６により主軸頭３０の底部に連結されている。３つの回転型アクチュエーター８２を駆動し３本の第１のレバー８３を旋回させることにより、主軸頭３０はその位置姿勢を制御される。この実施の形態は、回転型のアクチュエーターを用いて空間３自由度パラレル機構を構成することができるという利点がある。

図９は、図１で説明した第１の実施の形態の空間３自由度パラレル機構を横フライス盤に適用した例を、一部を透視して示す斜視図である。基台９１にコラム９２が立設されている。そのコラム９２のＹ軸方向の案内９３、９３に案内されてヘッドストック９４がＹ軸方向に摺動自在である。ヘッドストック９４はＹ軸モータ９５により駆動制御される。相対的に静止した系であるヘッドストック９４に、図１で説明した空間３自由度パラレル機構が組み込まれている。すなわち、ヘッドストック９４に３本の直線ガイド２１が固定されている。その直線ガイド２１に案内され直線アクチュエーターにより駆動される駆動スライダに連結された３本のロッド３５により、主軸頭３０の位置（ｚ，Ａ、Ｂ）及び工具３１の先端位置が制御される。

また、基台９１上にはテーブル９６が設置され、Ｘ軸方向の案内９７、９７によりテーブル９６はＸ軸方向に摺動自在である。テーブル９６はＸ軸モータ９８により駆動制御される。したがって、この工作機械は、Ｘ、Ｙ、Ｚ、Ａ、Ｂ、の５軸制御の工作機械となる。

本発明の第１の実施の形態を示す空間３自由度パラレル機構の斜視図である。 図面上方の駆動スライダの位置Ｕ１を求める逆変換式を説明する空間３自由度パラレル機構の斜視図である。 主軸頭位置（ｚ，Ａ、Ｂ）をアクチュエーター位置（Ｕ_１ 、Ｕ_２ 、Ｕ_３ ）に逆変換しフィードバック制御をする制御装置のブロック図である。 制御装置の第２の実施例を示すブロック図である。 機構パラメータＰの同定作業を行う処理を示したフローチャートである。 機構パラメータＰの同定に必要な特別な複数の位置に機械を位置決めし、そのときのエンコーダ位置情報から機構パラメータＰの同定を行う処理を示すフローチャートである。 本発明の第２の実施の形態を示す斜視図である。 本発明の第３の実施の形態を示す斜視図である。 図１で説明した第１の実施の形態の空間３自由度パラレル機構を横フライス機械に適用した例を、一部を透視して示す斜視図である。 第１の従来例を示す一部破断斜視図である。 第２の従来例を示す斜視図である。 第３の従来例を示す斜視図である。

符号の説明

２１ 直線ガイド
２２ 従動スライダ
２３ 第１の可動サポート体（可動体サポート体）
２５ 第２の可動体サポート体（可動体サポート体）
２６ 中心軸（可動体サポート体）
３０ 主軸頭（可動体）
３１ 工具
３３ 駆動スライダ
３４ 直線アクチュエーター
３５ ロッド
３６ 球面ジョイント
３７ ユニバーサルジョイント

Claims (8)

1. 相対的に静止した系に３つの自由度で支承された可動体を３つのアクチュエーターにより駆動し３自由度で移動させる空間３自由度パラレル機構の制御方法であって、
   前記３つの自由度に対応した３軸の座標系での可動体の座標位置を検出する３つの可動体位置検出器が、各軸の軸上若しくは軸と平行な線上であって前記可動体の近傍に配置され、
   前記可動体の３軸の指令位置を機構パラメータに基づいて逆変換し前記アクチュエーターの逆変換指令位置に変換する指令位置逆変換手順と、
   前記３つの可動体位置検出器で検出された前記可動体の実測位置を前記機構パラメータに基づいて逆変換し前記アクチュエーターの逆変換実測位置を求める実測位置逆変換手順と、
   前記逆変換指令位置に前記逆変換実測位置をフィードバックして前記可動体の位置を制御する制御手順と、
   複数の前記逆変換指令位置とそれにそれぞれ対応する複数の前記逆変換実測位置の値を記憶する記憶手順と、
   前記記憶された複数の逆変換指令位置及び対応する逆変換実測位置に基づいて前記指令位置から前記逆変換指令位置への逆変換式に含まれる前記機構パラメータを同定する機構パラメータの同定手順と、
   所定のキャリブレーションタイミングで前記記憶された逆変換指令位置及び逆変換実測位置に基づいて同定された機構パラメータに従って古い機構パラメータを補正するキャリブレーション手順と、
   を備えることを特徴とする空間３自由度パラレル機構の制御方法。
2. 前記記憶された逆変換指令位置及び逆変換実測位置に基づいて前記所定のキャリブレーションを行うタイミングが、前記可動体が工作物の加工を行っていないタイミングである、ことを特徴とする請求項１に記載の空間３自由度パラレル機構の制御方法。
3. 前記複数の逆変換指令位置の基礎となる指令位置が、機構パラメータの同定に必要な特別な複数の位置である、ことを特徴とする請求項１又は２に記載の空間３自由度パラレル機構の制御方法。
4. 前記実測位置逆変換手順における前記可動体の実測位置の検出を、前記可動体による加工物の加工中に行うことを特徴とする請求項１又は２に記載の空間３自由度パラレル機構の制御方法。
5. 前記機構パラメータの同定手順が、パラメータ誤差が所定許容値以下になるまで収束計算を行う収束計算手順を備える、ことを特徴とする請求項１乃至４のいずれかに記載の空間３自由度パラレル機構の制御方法。
6. 前記機構パラメータの同定手順が、前記機構パラメータの数より多い数の前記逆変換式が得られるような多数の前記指令位置での逆変換実測位置に基づいて最小２乗法により算定される最小２乗法手順を備える、ことを特徴とする請求項１乃至５のいずれかに記載の空間３自由度パラレル機構の制御方法。
7. 相対的に静止した系に３つの自由度で支承された可動体を３つのアクチュエーターにより駆動し３自由度で移動させる制御装置を有する空間３自由度パラレル機構であって、
   前記３つの自由度に対応した３軸の座標系での可動体の座標位置を検出する３つの可動体位置検出器が、各軸の軸上若しくは軸と平行な線上であって前記可動体の近傍に配置されていることと、
   前記制御装置が、
   前記可動体の３軸の指令位置を機構パラメータに基づいて逆変換し前記アクチュエーターの逆変換指令位置を求める指令位置逆変換手段と、
   前記３つの可動体位置検出器で検出された前記可動体の実測位置を前記機構パラメータに基づいて逆変換し前記アクチュエーターの逆変換実測位置を求める実測位置逆変換手段と、
   前記逆変換指令位置に前記逆変換実測位置をフィードバックして前記３つのアクチュエーターを駆動し前記可動体の位置を制御する制御手段と、
   複数の前記逆変換指令位置にそれぞれ対応する複数の前記逆変換実測位置の値を記憶する逆変換指令位置逆変換実測位置記憶手段と、
   前記逆変換指令位置逆変換実測位置記憶手段に記憶された複数の逆変換指令位置及び対応する逆変換実測位置に基づいて前記指令位置から前記逆変換指令位置への逆変換式に含まれる前記機構パラメータを同定する機構パラメータの同定手段と、
   所定のキャリブレーションタイミングで前記記憶された逆変換指令位置及び逆変換実測位置に基づいて同定された機構パラメータに従って古い機構パラメータを補正するキャリブレーション手段と、
   を備えることを特徴とする空間３自由度パラレル機構。
8. 前記アクチュエーターの位置を検出するアクチュエーター位置検出器を備え、前記３つの可動体位置検出器がそれぞれ前記アクチュエーター位置検出器よりも前記可動体の近傍に配置されていることを特徴とする請求項７に記載の空間３自由度パラレル機構。

Images