(57)【要約】【課題】搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができるパラレルリンク式搬送装置の制御装置及び制御方法を提供する。【解決手段】複数のモータドライバ２５，２６に与える各トルク指令値に、パラレルリンク機構の先端の位置目標値［Ｘ，Ｙ］と搬送物の質量ｍに基づいて求めた複数の補正トルク指令値Ｋｆｐ、Ｋｆｑをそれぞれ加算する。慣性力の影響を反映した補正トルク指令値Ｋｆｐ、Ｋｆｑを、モータドライバ２５，２６へ与えるトルク指令値に加算することによって、パラレルリンク機構を構成する各リンクの運動に対する補償制御を行うことができるので、搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができる。【選択図】図２
2008178945.tif 000002
【特許請求の範囲】
【請求項１】
複数のリンクの駆動により該複数のリンクが連結された先端の位置が決定されるパラレルリンク機構と、前記複数のリンクをそれぞれ駆動する複数の駆動モータと、トルク指令値に基づいて前記複数の駆動モータをそれぞれ駆動する複数のモータドライバとを備え前記パラレルリンク機構の駆動により搬送物を搬送するパラレルリンク式搬送装置の制御装置であって、
前記複数のモータドライバのそれぞれにトルク指令値を与える複数のモータ制御部と、
前記パラレルリンク機構の先端の位置目標値と搬送物の質量ｍに基づいて、前記トルク指令値をそれぞれ補正するための複数の補正トルク指令値を求め、前記複数のモータ制御部にそれぞれ与えられるトルク指令値に前記補正トルク指令値をそれぞれ加算するトルク補正手段と、を備えることを特徴とするパラレルリンク式搬送装置の制御装置。
【請求項２】
前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
前記トルク補正手段は、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項１記載のパラレルリンク式搬送装置の制御装置。
【請求項３】
前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
前記トルク補正手段は、搬送物の目標速度から求められる運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項１記載のパラレルリンク式搬送装置の制御装置。
【請求項４】
前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
前記トルク補正手段は、搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項１記載のパラレルリンク式搬送装置の制御装置。
【請求項５】
前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
前記トルク補正手段は、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の第１関数、前記運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第２関数、又は前記位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第３関数に基づいて前記複数の補正トルク指令値を求め、前記パラレルリンク式搬送装置の運転中に前記搬送物の位置及び／又は速度に応じて前記第１関数、前記第２関数又は前記第３関数のいずれに基づいて前記複数の補正トルク指令値を求めるかを切り替えることを特徴とする請求項１記載のパラレルリンク式搬送装置の制御装置。
【請求項６】
前記偏微分した関数がラグランジュの運動方程式と等価な式であることを特徴とする請求項２乃至５のいずれか記載のパラレルリンク式搬送装置の制御装置。
【請求項７】
前記トルク補正手段は、前記複数の補正トルク指令値を前記位置目標値の変化に応じて時々刻々と計算し、対応する前記複数のトルク指令値にそれぞれ加算することを特徴とする請求項１乃至６のいずれか記載のパラレルリンク式搬送装置の制御装置。
【請求項８】
複数のリンクの駆動により該複数のリンクが連結された先端の位置が決定されるパラレルリンク機構と、前記複数のリンクをそれぞれ駆動する複数の駆動モータと、トルク指令値に基づいて前記複数の駆動モータをそれぞれ駆動する複数のモータドライバとを備え前記パラレルリンク機構の駆動により搬送物を搬送するパラレルリンク式搬送装置の制御方法であって、
前記複数のモータドライバに与える各トルク指令値に、前記パラレルリンク機構の先端の位置目標値と搬送物の質量ｍに基づいて求めた複数の補正トルク指令値をそれぞれ加算することを特徴とするパラレルリンク式搬送装置の制御方法。
【請求項９】
前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項８記載のパラレルリンク式搬送装置の制御方法。
【請求項１０】
前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
搬送物の目標速度から求められる運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項８記載のパラレルリンク式搬送装置の制御方法。
【請求項１１】
前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項８記載のパラレルリンク式搬送装置の制御方法。
【請求項１２】
前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の第１関数、前記運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第２関数、又は前記位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第３関数に基づいて前記複数の補正トルク指令値を求め、前記パラレルリンク式搬送装置の運転中に前記搬送物の位置及び／又は速度に応じて前記第１関数、前記第２関数又は前記第３関数のいずれに基づいて前記複数の補正トルク指令値を求めるかを切り替えることを特徴とする請求項８記載のパラレルリンク式搬送装置の制御方法。
【請求項１３】
前記偏微分した関数がラグランジュの運動方程式と等価な式であることを特徴とする請求項９乃至１２のいずれか記載のパラレルリンク式搬送装置の制御方法。
【請求項１４】
前記複数の補正トルク指令値を前記位置目標値の変化に応じて時々刻々と計算し、対応する前記複数のトルク指令値にそれぞれ加算することを特徴とする請求項８乃至１３のいずれか記載のパラレルリンク式搬送装置の制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、パラレルリンク機構を駆動して搬送物を搬送するパラレルリンク式搬送装置の制御装置及び制御方法に関する。
【背景技術】
【０００２】
例えば、タンデムプレスなどのプレス間搬送装置として、パラレルリンク機構を駆動して搬送物を搬送するパラレルリンク式搬送装置が使用される（例えば、下記特許文献１参照）。この種の搬送装置では、搬送物の目標位置からパラレルリンク機構のモータ座標に目標位置を変換し、モータをフィードバック制御することによって位置決め制御を行う。しかし、パラレルリンク機構は、目標位置によっては搬送物の質量を支えるための釣り合い力がモータ軸ごとに著しく不均衡になり、リンク先端での位置決め精度が劣化することがあった。
【０００３】
このような問題に対し、下記特許文献２の方法では、目標位置から各モータ軸の釣り合い力を計算し、モータへの電流指令値に加算することで、各モータ軸の釣り合い力の不均衡を改善している。
【０００４】
【特許文献１】特開２００４−３４４８９９号公報
【特許文献２】特開２００２−１７８２３７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献２の方法では、釣り合い力のみを補正対象としているので、リンクが静止している場合の位置決め精度しか改善できない。搬送物を移動させている最中は、重力による釣り合い力だけでなく、加速度による慣性力の影響も受けることになる。搬送物を高速で移動させる場合には、その慣性力の影響が大きくなり、釣り合い力の補正だけでは位置精度が補償できなくなる。
【０００６】
本発明は上述した問題に鑑みてなされたものであり、搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができるパラレルリンク式搬送装置の制御装置及び制御方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記の課題を解決するため、本発明のパラレルリンク式搬送装置の制御装置及び制御方法は、以下の手段を採用する。
（１）すなわち、本発明のパラレルリンク式搬送装置の制御装置は、複数のリンクの駆動により該複数のリンクが連結された先端の位置が決定されるパラレルリンク機構と、前記複数のリンクをそれぞれ駆動する複数の駆動モータと、トルク指令値に基づいて前記複数の駆動モータをそれぞれ駆動する複数のモータドライバとを備え前記パラレルリンク機構の駆動により搬送物を搬送するパラレルリンク式搬送装置の制御装置であって、前記複数のモータドライバのそれぞれにトルク指令値を与える複数のモータ制御部と、前記パラレルリンク機構の先端の位置目標値と搬送物の質量ｍに基づいて、前記トルク指令値をそれぞれ補正するための複数の補正トルク指令値を求め、前記複数のモータ制御部にそれぞれ与えられるトルク指令値に前記補正トルク指令値をそれぞれ加算するトルク補正手段と、を備えることを特徴とする。
【０００８】
（２）また、上記パラレルリンク式搬送装置の制御装置において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、前記トルク補正手段は、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。
【０００９】
（３）また、上記パラレルリンク式搬送装置の制御装置において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、前記トルク補正手段は、搬送物の目標速度から求められる運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。
【００１０】
（４）また、上記パラレルリンク式搬送装置の制御装置において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、前記トルク補正手段は、搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。
【００１１】
（５）また、上記パラレルリンク式搬送装置の制御装置において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、前記トルク補正手段は、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の第１関数、前記運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第２関数、又は前記位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第３関数に基づいて前記複数の補正トルク指令値を求め、前記パラレルリンク式搬送装置の運転中に前記搬送物の位置及び／又は速度に応じて前記第１関数、前記第２関数又は前記第３関数のいずれに基づいて前記複数の補正トルク指令値を求めるかを切り替えることを特徴とする。
【００１２】
（６）また、上記パラレルリンク式搬送装置の制御装置において、前記偏微分した関数がラグランジュの運動方程式と等価な式であることを特徴とする。
【００１３】
（７）また、上記パラレルリンク式搬送装置の制御装置において、前記トルク補正手段は、前記複数の補正トルク指令値を前記位置目標値の変化に応じて時々刻々と計算し、対応する前記複数のトルク指令値にそれぞれ加算することを特徴とする。
【００１４】
（８）本発明のパラレルリンク式搬送装置の制御方法は、複数のリンクの駆動により該複数のリンクが連結された先端の位置が決定されるパラレルリンク機構と、前記複数のリンクをそれぞれ駆動する複数の駆動モータと、トルク指令値に基づいて前記複数の駆動モータをそれぞれ駆動する複数のモータドライバとを備え前記パラレルリンク機構の駆動により搬送物を搬送するパラレルリンク式搬送装置の制御方法であって、前記複数のモータドライバに与える各トルク指令値に、前記パラレルリンク機構の先端の位置目標値と搬送物の質量ｍに基づいて求めた複数の補正トルク指令値をそれぞれ加算することを特徴とする。
【００１５】
（９）また、上記パラレルリンク式搬送装置の制御方法において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。
【００１６】
（１０）また、上記パラレルリンク式搬送装置の制御方法において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、搬送物の目標速度から求められる運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。
【００１７】
（１１）また、上記パラレルリンク式搬送装置の制御方法において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。
【００１８】
（１２）また、上記パラレルリンク式搬送装置の制御方法において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の第１関数、前記運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第２関数、又は前記位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第３関数に基づいて前記複数の補正トルク指令値を求め、前記パラレルリンク式搬送装置の運転中に前記搬送物の位置及び／又は速度に応じて前記第１関数、前記第２関数又は前記第３関数のいずれに基づいて前記複数の補正トルク指令値を求めるかを切り替えることを特徴とする
【００１９】
（１３）また、上記パラレルリンク式搬送装置の制御方法において、前記偏微分した関数がラグランジュの運動方程式と等価な式であることを特徴とする。
【００２０】
（１４）また、上記パラレルリンク式搬送装置の制御方法において、前記複数の補正トルク指令値を前記位置目標値の変化に応じて時々刻々と計算し、対応する前記複数のトルク指令値にそれぞれ加算することを特徴とする。
【発明の効果】
【００２１】
上記の補正トルク指令値は、位置目標値と搬送物の質量ｍに基づくものであり、駆動軸の釣り合い力だけでなく、慣性力の影響を反映している。したがって、この補正トルク指令値を、モータモータドライバへ与えるトルク指令値に加算することによって、パラレルリンク機構を構成する各リンクの運動に対する補償制御を行うことができるので、搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができる。
【００２２】
また、補正トルク指令値により、先端重量部の変化や運動の影響を正確にキャンセルする分のトルク指令をモータドライバへ与えることができ、フィードバック制御系と組み合わせた場合、フィードバック部はパラレルリンク機構を構成する各リンクの運動に対する補償制御のみを行うことになる。したがって、フィードバック制御系のゲインの調整や性能補償が容易となる。
【００２３】
また、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を複数のスライドの移動座標でそれぞれ偏微分した複数の関数、搬送物の目標速度から求められる運動エネルギーを複数のスライドの移動座標でそれぞれ偏微分した複数の関数又は搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数（ラグランジュの運動方程式と等価な式）に基づいて、複数の補正トルク指令値を求めることにより、慣性力の影響を正確に反映した適切な補正トルク指令値を算出することができる。
【００２４】
また、補正トルク指令値の算出において、運動エネルギーのみ又は位置エネルギーのみを偏微分することにより、運動エネルギーと位置エネルギーの差を偏微分する場合と比較して、計算量が減少し、制御装置の負荷を抑えることができる。
また、運動エネルギーと位置エネルギーの差の偏微分、運動エネルギーのみの偏微分及び位置エネルギーのみの偏微分のいずれからも補正トルク指令値を求めることができるようにしておき、運転中に搬送物の位置や速度に応じて適宜切り替えることによっても、計算量が減少し、制御装置の負荷を抑えることができる。
【００２５】
また、複数の補正トルク指令値を位置目標値の変化に応じて時々刻々と計算し、対応する複数のトルク指令値にそれぞれ加算することで、その時の目標位置指令に応じた適切な補正トルク指令値が演算される。
【発明を実施するための最良の形態】
【００２６】
以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２７】
図１に、本発明の制御装置を備えたパラレルリンク式搬送装置１０の概略構成を示す。この搬送装置１０は、上記特許文献１（特開２００４−３４４８９９号公報）と同様の構成を有するものであり、上流側プレスステーション１４ａと、下流側プレスステーション１４ｂの間に設置され、上流側プレスステーション１４から成形された搬送物（パネル１）を受け取り、下流側プレスステーション１４へ搬送する。
【００２８】
図１に示すように、搬送装置１０は、パラレルリンク機構６と、パラレルリンク機構６の先端に接続されたパネル把持部１５と、パラレルリンク機構６の他端に接続された第１リンク駆動機構１１及び第２リンク駆動機構１２とを備える。
パネル把持部１５は、パネル１を吸着するバキュームカップ等のワーク把持具２と、該ワーク把持具２が取り付けられ搬送ラインと直角方向に延びるクロスバー３とを備えている。
パラレルリンク機構６は、第１リンク７と第２リンク８からなる。第１リンク７の他端に第１リンク駆動機構１１が接続され、第２リンクの他端に第２リンク駆動機構１２が接続されている。
【００２９】
第１リンク駆動機構１１及び第２リンク駆動機構１２は、Ｖ字型のベース部材１６に取り付けられている。第１リンク駆動機構１１及び第２リンク駆動機構１２は、それぞれ、ネジ軸で構成される第１スライド軸９及び第２スライド軸１３と、第１及び第２スライド軸９，１３を回転駆動するための第１駆動モータ１７及び第２駆動モータ１８と、第１及び第２スライド軸９，１３の回転によって直線駆動される第１スライド４及び第２スライド５とからなる。
第１駆動モータ１７及び第２駆動モータ１８は、後述するそれぞれのモータドライバ２７，２８により駆動される。
このような第１駆動モータ１７及び第２駆動モータ１８の駆動により、それぞれ第１スライド４及び第２スライド５が直線駆動され且つ数値制御等で正確に位置決めされるようになっている。
【００３０】
また、この図に示すように、第１スライド軸９及び第２スライド軸１３は、鉛直軸Ｙから所定の角度で互いに反対方向に傾斜している。
この構成により、第１スライド４は、その駆動領域のうち搬送ラインの上流側で低位置となり下流側で高位置となるように第１リンク駆動機構１１により水平面に対して傾斜して直線駆動されるようになっている。
また、第２スライド５は、その駆動領域のうち搬送ラインの上流側で高位置となり下流側で低位置となるように第２リンク駆動機構１２により水平面に対して傾斜して直線駆動されるようになっている。
【００３１】
なお、本実施形態において第１リンク駆動機構１１及び第２リンク駆動機構１２は、ボールネジ機構によって直動駆動機構を構成しているが、本発明はこれに限定されず、タイミングベルト、液圧シリンダ、ラックアンドピニオン、等によって直動駆動機構を構成してもよい。
【００３２】
上記の構成により、後述する制御装置２０により動作指令を与えることによって、第１駆動モータ１７及び第２駆動モータ１８を駆動させ、第１リンク７及び第２リンク８をそれぞれ独立して駆動させる。すると、第１リンク７及び第２リンク８の根元（第１スライド４及び第２スライド５）の相互位置関係によってパラレルリンク機構６の先端位置が決まる。しがたって、第１駆動モータ１７及び第２駆動モータ１８の駆動を独立に制御することにより、パラレルリンク機構６の先端に接続されたパネル把持部１５の位置を決定し、所望の搬送モーションを得ることができる。
【００３３】
図２に、本発明の実施形態にかかるパラレルリンク式搬送装置の制御装置２０の構成を示す。
図２に示すように、本発明の制御装置２０は、スライド目標位置演算手段２１と、モータ制御部２２，２３とトルク補正手段２４とを備える。
スライド目標位置演算手段２１は、リンク先端（パネル把持部１５）の位置目標値［Ｘ，Ｙ］に基づいて、第１スライド４及び第２スライド５の目標位置（ｐ，ｑ）を演算する。なお、以下、この目標位置（ｐ，ｑ）をスライド目標位置という。
【００３４】
モータ制御部２２，２３は複数あり、上述した第１駆動モータ１７及び第２駆動モータ１８を駆動する複数のモータドライバ２５，２６のそれぞれに、リンク先端を目標位置に移動させるためのトルク指令値を与える。本実施形態では、第１駆動モータ１７を駆動するモータドライバをｐ軸モータドライバ２５とし、第２駆動モータ１８を駆動するモータドライバをｑ軸モータドライバ２６とする。また、ｐ軸モータドライバ２５にトルク指令値を与えるモータ制御部をｐ軸モータ制御部２２とし、ｑ軸モータドライバ２６にトルク指令値を与えるモータ制御部をｑ軸モータ制御部２３とする。
【００３５】
第１駆動モータ１７、第２駆動モータ１８は、それぞれｐ軸モータドライバ２５、ｑ軸モータドライバ２６により駆動され、エンコーダ２７，２８によってその回転数が検出されて、その信号が、ｐ軸モータ制御部２２、ｑ軸モータ制御部２３にフィードバック信号として入力される。このように、制御装置２０は、駆動モータ１７，１８をフィードバック制御することによって位置決め制御を行う。
【００３６】
トルク補正手段２４は、パラレルリンク機構６の先端（パネル把持部１５）の位置目標値と搬送物の質量ｍ（パネル把持部１５とパネル１の合計質量）に基づいて、トルク指令値をそれぞれ補正するための複数の補正トルク指令値Ｋｆｐ、Ｋｆｑを求め、複数のモータ制御部２２，２３にそれぞれ与えられるトルク指令値に補正トルク指令値Ｋｆｐ、Ｋｆｑをそれぞれ加算する。この補正トルク指令値Ｋｆｐ、Ｋｆｑは、具体的には、第１スライド軸９及び第２スライド軸１３がそれぞれ第１スライド４及び第２スライド５を軸方向に移動させるために必要な駆動力ｆｐ、ｆｑに、調整ゲインＫを乗じて得た値である。なお、以下、ｆｐ，ｆｑをスライド駆動力という。
【００３７】
以下、上記のスライド目標位置（ｐ，ｑ）、スライド駆動力ｆｐ，ｆｑの算出方法を説明する。
【００３８】
図３に、図１に示したパラレルリンク式搬送装置１０の模式図を示す。図３において、パネル把持部１５の位置を（Ｘ，Ｙ）、リンク７，８の長さをそれぞれＬ、第１スライド軸９、第２スライド軸１３の駆動軸をそれぞれｐ軸、ｑ軸、リンク７，８とｐ軸及びｑ軸との接点（第１スライド４、第２スライド５）の位置をｐ，ｑ、ｐ軸とｑ軸の交点（ｐ軸、ｑ軸の原点）を（Ｘ０，Ｙ０）、Ｘ軸とｐ軸とのなす角をα、Ｘ軸とｑ軸とのなす角をβとする。
【００３９】
パネル把持部１５の位置（Ｘ，Ｙ）と、スライド位置（ｐ，ｑ）との間には、下記数１に示す関係が成立する。
【数１】
2008178945.tif 000003
【００４０】
したがって、リンク先端（パネル把持部１５）を（Ｘ，Ｙ）に位置決めしたい場合、上記数１の（Ａ−１式）及び（Ａ−２式）に基づいて、対応するスライド位置（ｐ，ｑ）を算出し、第１スライド４、第２スライド５を移動させればよい。
【００４１】
さて、パネル把持部１５とパネル１の合計質量がｍであったとする。質量ｍの物体を、時間関数［Ｘ（ｔ），Ｙ（ｔ）］の軌道で移動させた場合の運動エネルギーＩ、位置エネルギーＵは、下記数２の（式１）、（式２）で計算される。
【数２】
2008178945.tif 000004
【００４２】
運動エネルギーＩ、位置エネルギーＵが与えられた場合、必要なスライド駆動力ｆｐ，ｆｑは、ラグランジュの運動方程式を考えた場合、下記数３の（式３）、（式４）で計算される。
【数３】
2008178945.tif 000005
【００４３】
（式３）、（式４）では運動エネルギーＩと位置エネルギーＵの差をスライドの位置座標で偏微分しているが、運動エネルギーと比較して位置エネルギーがほとんど変化しない場合には運動エネルギーが支配的となるため、運動エネルギーのみを偏微分する（位置エネルギーは考慮しない）こととしてもよい。逆に、位置エネルギーと比較して運動エネルギーがほとんど変化しない場合には位置エネルギーが支配的となるため、位置エネルギーのみを偏微分する（運動エネルギーは考慮しない）こととしてもよい。
【００４４】
運動エネルギーと比較して位置エネルギーがほとんど変化しない場合としては、搬送物が水平座標面で移動するような構成とした場合（例えば、図１の装置を横倒しにした場合）が考えられる。また、図１の構成であったとしても、搬送物の軌道によっては（たとえば、搬送物が垂直方向にほとんど変位しない場合等）その位置エネルギーがほとんど時間変化せず、運動エネルギーが支配的となる。
【００４５】
位置エネルギーと比較して運動エネルギーがほとんど変化しない場合としては、搬送装置が停止している場合が考えられる。また、搬送装置が停止していない場合であっても、搬送物の速度によっては（たとえば、極めて低速の場合等）その速度エネルギーがほとんど時間変化せず、位置エネルギーが支配的となる。
【００４６】
運動エネルギーと位置エネルギーの両方を考慮するか運動エネルギーと位置エネルギーのいずれか一方を考慮するかについては、搬送装置の構成や運転方法に基づいて予め定めておき、搬送装置の運転を始めた後は運転が終了するまで設定を変えないとすることも可能である。あるいは、搬送装置の運転中であっても搬送物の位置や速度に基づいて適宜切り替えることとしても良い。このような構成とすることにより、計算量が減少し、制御装置２０の負荷を抑えることが可能となる。なお、このような切り替えは、図２に示した制御装置２０では、トルク補正手段２４によって実行される。
【００４７】
以下、運動エネルギーと位置エネルギーの双方を考慮した場合について説明する。
【００４８】
上記の（式３）、（式４）に、（式１）、（式２）を代入し整理すると、下記数４の（式５）、（式６）が得られる。
【数４】
2008178945.tif 000006
【００４９】
パラレルリンク式搬送装置１０を考えた場合、上記の（Ｂ−１式）、（Ｂ−２式）より、パネル把持部１５の位置（Ｘ，Ｙ）は、下記数５に示すようなスライド位置（ｐ，ｑ）の関数であると言える。
【数５】
2008178945.tif 000007
【００５０】
よって、下記数６のように表せる。
【数６】
2008178945.tif 000008
【００５１】
上記の（式９）〜（式１８）を（式５）、（式６）に代入すると、下記数７に示すようなスライド駆動力ｆｐ，ｆｑの計算式（式１９）、（式２０）が得られる。
【数７】
2008178945.tif 000009
【００５２】
（式１９）、（式２０）はリンク先端位置（Ｘ，Ｙ）の偏微分関数が含まれている。これは（Ｂ−１式）、（Ｂ−２式）を偏微分することによって求まる。偏微分の結果は、下記数８に示すようなｐとｑの関数で表すことができる。
【数８】
2008178945.tif 000010
【００５３】
（式２１）〜（式３０）は事前に（Ｂ−１式）、（Ｂ−２式）から求めておくことが可能であり、（式１９）、（式２０）からリンク先端の質量ｍに対するスライド駆動力ｆｐ，ｆｑは、以下のステップ１〜６で算出できる。
【００５４】
（ステップ１）
パネル把持部の位置目標値［Ｘ，Ｙ］を（Ａ−１式）、（Ａ−２式）に代入して、第１スライド及び第２スライドの位置目標値［ｐ，ｑ］を求める。
【００５５】
（ステップ２）
ステップ１で求めた［ｐ，ｑ］を上記数８のＦ１（ｐ，ｑ）〜Ｆ１０（ｐ，ｑ）に代入し、偏微分値を求める。
【００５６】
（ステップ３）
パネル把持部１５の位置目標値［Ｘ，Ｙ］と、第１スライド４及び第２スライド５の位置目標値［ｐ，ｑ］に微分演算を行い、下記数９の速度目標値を求める。
【数９】
2008178945.tif 000011
【００５７】
（ステップ４）
ステップ２とステップ３の結果から、下記数１０のＶｐ、Ｃｐ、Ｇｐ、Ｖｑ、Ｃｑ、Ｇｑを計算する。
【数１０】
2008178945.tif 000012
【００５８】
（ステップ５）
ステップ４で求めたＶｐとＶｑの微分値（下記数１１）を求める。
【数１１】
2008178945.tif 000013
【００５９】
（ステップ６）
ステップ４とステップ５の結果から、スライド駆動力ｆｐ，ｆｑを下記数１２の（式３７）、（式３８）により求める。
【数１２】
2008178945.tif 000014
【００６０】
上記ステップ１〜６の処理で求めたスライド駆動力ｆｐ，ｆｑに調整ゲインＫを乗じることにより、上述した補正トルク指令値Ｋｆｐ、Ｋｆｑを算出することができる。上記のトルク補正手段は、このような処理を実行して補正トルク指令値Ｋｆｐ、Ｋｆｑを算出する。
【００６１】
以下、上述した本発明のパラレルリンク式搬送装置の制御装置２０及び制御方法の作用効果について説明する。
上記の補正トルク指令値Ｋｆｐ、Ｋｆｑは、位置目標値［Ｘ，Ｙ］と搬送物の質量ｍに基づくものであり、駆動軸の釣り合い力だけでなく、慣性力の影響を反映している。したがって、この補正トルク指令値Ｋｆｐ、Ｋｆｑを、モータドライバ２５，２６へ与えるトルク指令値に加算することによって、パラレルリンク機構６を構成する各リンクの運動に対する補償制御を行うことができるので、搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができる。
【００６２】
また、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を複数のスライド４，５の位置座標でそれぞれ偏微分した複数の関数（ラグランジュの運動方程式）に基づいて、複数の補正トルク指令値を求めることにより、慣性力の影響を正確に反映した適切な補正トルク指令値を算出することができる。
【００６３】
また、補正トルク指令値Ｋｆｐ、Ｋｆｑの算出において、運動エネルギーのみ又は位置エネルギーのみを偏微分することにより、運動エネルギーと位置エネルギーの差を偏微分する場合と比較して、計算量が減少し、制御装置２０の負荷を抑えることができる。また、運動エネルギーと位置エネルギーの差の偏微分、運動エネルギーのみの偏微分及び位置エネルギーのみの偏微分のいずれからも補正トルク指令値を求めることができるようにしておき、運転中に搬送物の位置や速度に応じて適宜切り替えることによっても、計算量が減少し、制御装置２０の負荷を抑えることができる。
【００６４】
また、上記ステップ１〜６の処理は、トルク指令値を更新する制御サイクル毎に実行する。すなわち、複数の補正トルク指令値Ｋｆｐ、Ｋｆｑを位置目標値の変化に応じて時々刻々と計算し、対応する複数のトルク指令値にそれぞれ加算することで、その時の目標位置指令に応じた適切な補正トルク指令値Ｋｆｐ、Ｋｆｑが演算される。
【００６５】
また、生産するパネルサイズの変化や、パネル把持部のツール重量の変更があった場合は、それに応じて質量ｍを変更することで、搬送物の重量変更に対しても正確な補正トルク指令値Ｋｆｐ、Ｋｆｑを求めることができる。
【００６６】
また、補正トルク指令値Ｋｆｐ、Ｋｆｑにより、先端重量部の変化や運動の影響を正確にキャンセルする分のトルク指令をフィードフォワード的にモータドライバへ与えることができ、図２に示したフィードバック制御系と組み合わせた場合、フィードバック部はパラレルリンク機構６を構成する各リンクの運動に対する補償制御のみを行うことになる。これはすなわち、搬送物の変化に対するロバスト性を補償するものであり、フィードバック制御系のゲインの調整や性能補償が容易となる。
【００６７】
なお、上述した実施形態では、２自由度のパラレルリンク機構をもつパラレルリンク式搬送装置を対象としたが、本発明の適用範囲はこれに限られず、３自由度以上のパラレルリンク機構をもつパラレルリンク式搬送装置にも同様に適用可能である。
【００６８】
上記において、本発明の実施形態について説明を行ったが、上記に開示された本発明の実施の形態は、あくまで例示であって、本発明の範囲はこれら発明の実施の形態に限定されない。本発明の範囲は、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。
【図面の簡単な説明】
【００６９】
【図１】本発明の制御装置を備えたパラレルリンク式搬送装置の概略構成図である。
【図２】本発明の実施形態にかかるパラレルリンク式搬送装置の制御装置の構成図である。
【図３】図１に示したパラレルリンク式搬送装置の模式図である。
【符号の説明】
【００７０】
１  パネル
２  ワーク把持具
３  クロスバー
４  第１スライド
５  第２スライド
６  パラレルリンク機構
７  第１リンク
８  第２リンク
１０  パラレルリンク式搬送装置
１１  第１リンク駆動機構
１２  第２リンク駆動機構
１５  パネル把持部
１６  ベース部材
１７  第１駆動モータ
１８  第２駆動モータ
２０  制御装置
２１  スライド目標位置演算手段
２２  ｐ軸モータ制御部
２３  ｑ軸モータ制御部
２４  トルク補正手段
２５  ｐ軸モータドライバ
２６  ｑ軸モータドライバ
２７，２８  エンコーダ
【図１】
2008178945.tif 000015
【図２】
2008178945.tif 000016
【図３】
2008178945.tif 000017