JP2008178945A - パラレルリンク式搬送装置の制御装置及び制御方法 - Google Patents

Abstract

【課題】搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができるパラレルリンク式搬送装置の制御装置及び制御方法を提供する。
【解決手段】複数のモータドライバ２５，２６に与える各トルク指令値に、パラレルリンク機構の先端の位置目標値［Ｘ，Ｙ］と搬送物の質量ｍに基づいて求めた複数の補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑをそれぞれ加算する。慣性力の影響を反映した補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑを、モータドライバ２５，２６へ与えるトルク指令値に加算することによって、パラレルリンク機構を構成する各リンクの運動に対する補償制御を行うことができるので、搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができる。
【選択図】図２

Description

本発明は、パラレルリンク機構を駆動して搬送物を搬送するパラレルリンク式搬送装置の制御装置及び制御方法に関する。

例えば、タンデムプレスなどのプレス間搬送装置として、パラレルリンク機構を駆動して搬送物を搬送するパラレルリンク式搬送装置が使用される（例えば、下記特許文献１参照）。この種の搬送装置では、搬送物の目標位置からパラレルリンク機構のモータ座標に目標位置を変換し、モータをフィードバック制御することによって位置決め制御を行う。しかし、パラレルリンク機構は、目標位置によっては搬送物の質量を支えるための釣り合い力がモータ軸ごとに著しく不均衡になり、リンク先端での位置決め精度が劣化することがあった。

このような問題に対し、下記特許文献２の方法では、目標位置から各モータ軸の釣り合い力を計算し、モータへの電流指令値に加算することで、各モータ軸の釣り合い力の不均衡を改善している。

特開２００４−３４４８９９号公報 特開２００２−１７８２３７号公報

しかしながら、特許文献２の方法では、釣り合い力のみを補正対象としているので、リンクが静止している場合の位置決め精度しか改善できない。搬送物を移動させている最中は、重力による釣り合い力だけでなく、加速度による慣性力の影響も受けることになる。搬送物を高速で移動させる場合には、その慣性力の影響が大きくなり、釣り合い力の補正だけでは位置精度が補償できなくなる。

本発明は上述した問題に鑑みてなされたものであり、搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができるパラレルリンク式搬送装置の制御装置及び制御方法を提供することを目的とする。

上記の課題を解決するため、本発明のパラレルリンク式搬送装置の制御装置及び制御方法は、以下の手段を採用する。
（１）すなわち、本発明のパラレルリンク式搬送装置の制御装置は、複数のリンクの駆動により該複数のリンクが連結された先端の位置が決定されるパラレルリンク機構と、前記複数のリンクをそれぞれ駆動する複数の駆動モータと、トルク指令値に基づいて前記複数の駆動モータをそれぞれ駆動する複数のモータドライバとを備え前記パラレルリンク機構の駆動により搬送物を搬送するパラレルリンク式搬送装置の制御装置であって、前記複数のモータドライバのそれぞれにトルク指令値を与える複数のモータ制御部と、前記パラレルリンク機構の先端の位置目標値と搬送物の質量ｍに基づいて、前記トルク指令値をそれぞれ補正するための複数の補正トルク指令値を求め、前記複数のモータ制御部にそれぞれ与えられるトルク指令値に前記補正トルク指令値をそれぞれ加算するトルク補正手段と、を備えることを特徴とする。

（２）また、上記パラレルリンク式搬送装置の制御装置において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、前記トルク補正手段は、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。

（３）また、上記パラレルリンク式搬送装置の制御装置において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、前記トルク補正手段は、搬送物の目標速度から求められる運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。

（４）また、上記パラレルリンク式搬送装置の制御装置において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、前記トルク補正手段は、搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。

（５）また、上記パラレルリンク式搬送装置の制御装置において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、前記トルク補正手段は、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の第１関数、前記運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第２関数、又は前記位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第３関数に基づいて前記複数の補正トルク指令値を求め、前記パラレルリンク式搬送装置の運転中に前記搬送物の位置及び／又は速度に応じて前記第１関数、前記第２関数又は前記第３関数のいずれに基づいて前記複数の補正トルク指令値を求めるかを切り替えることを特徴とする。

（６）また、上記パラレルリンク式搬送装置の制御装置において、前記偏微分した関数がラグランジュの運動方程式と等価な式であることを特徴とする。

（７）また、上記パラレルリンク式搬送装置の制御装置において、前記トルク補正手段は、前記複数の補正トルク指令値を前記位置目標値の変化に応じて時々刻々と計算し、対応する前記複数のトルク指令値にそれぞれ加算することを特徴とする。

（８）本発明のパラレルリンク式搬送装置の制御方法は、複数のリンクの駆動により該複数のリンクが連結された先端の位置が決定されるパラレルリンク機構と、前記複数のリンクをそれぞれ駆動する複数の駆動モータと、トルク指令値に基づいて前記複数の駆動モータをそれぞれ駆動する複数のモータドライバとを備え前記パラレルリンク機構の駆動により搬送物を搬送するパラレルリンク式搬送装置の制御方法であって、前記複数のモータドライバに与える各トルク指令値に、前記パラレルリンク機構の先端の位置目標値と搬送物の質量ｍに基づいて求めた複数の補正トルク指令値をそれぞれ加算することを特徴とする。

（９）また、上記パラレルリンク式搬送装置の制御方法において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。

（１０）また、上記パラレルリンク式搬送装置の制御方法において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、搬送物の目標速度から求められる運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。

（１１）また、上記パラレルリンク式搬送装置の制御方法において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする。

（１２）また、上記パラレルリンク式搬送装置の制御方法において、前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の第１関数、前記運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第２関数、又は前記位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第３関数に基づいて前記複数の補正トルク指令値を求め、前記パラレルリンク式搬送装置の運転中に前記搬送物の位置及び／又は速度に応じて前記第１関数、前記第２関数又は前記第３関数のいずれに基づいて前記複数の補正トルク指令値を求めるかを切り替えることを特徴とする

（１３）また、上記パラレルリンク式搬送装置の制御方法において、前記偏微分した関数がラグランジュの運動方程式と等価な式であることを特徴とする。

（１４）また、上記パラレルリンク式搬送装置の制御方法において、前記複数の補正トルク指令値を前記位置目標値の変化に応じて時々刻々と計算し、対応する前記複数のトルク指令値にそれぞれ加算することを特徴とする。

上記の補正トルク指令値は、位置目標値と搬送物の質量ｍに基づくものであり、駆動軸の釣り合い力だけでなく、慣性力の影響を反映している。したがって、この補正トルク指令値を、モータモータドライバへ与えるトルク指令値に加算することによって、パラレルリンク機構を構成する各リンクの運動に対する補償制御を行うことができるので、搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができる。

また、補正トルク指令値により、先端重量部の変化や運動の影響を正確にキャンセルする分のトルク指令をモータドライバへ与えることができ、フィードバック制御系と組み合わせた場合、フィードバック部はパラレルリンク機構を構成する各リンクの運動に対する補償制御のみを行うことになる。したがって、フィードバック制御系のゲインの調整や性能補償が容易となる。

また、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を複数のスライドの移動座標でそれぞれ偏微分した複数の関数、搬送物の目標速度から求められる運動エネルギーを複数のスライドの移動座標でそれぞれ偏微分した複数の関数又は搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数（ラグランジュの運動方程式と等価な式）に基づいて、複数の補正トルク指令値を求めることにより、慣性力の影響を正確に反映した適切な補正トルク指令値を算出することができる。

また、補正トルク指令値の算出において、運動エネルギーのみ又は位置エネルギーのみを偏微分することにより、運動エネルギーと位置エネルギーの差を偏微分する場合と比較して、計算量が減少し、制御装置の負荷を抑えることができる。
また、運動エネルギーと位置エネルギーの差の偏微分、運動エネルギーのみの偏微分及び位置エネルギーのみの偏微分のいずれからも補正トルク指令値を求めることができるようにしておき、運転中に搬送物の位置や速度に応じて適宜切り替えることによっても、計算量が減少し、制御装置の負荷を抑えることができる。

また、複数の補正トルク指令値を位置目標値の変化に応じて時々刻々と計算し、対応する複数のトルク指令値にそれぞれ加算することで、その時の目標位置指令に応じた適切な補正トルク指令値が演算される。

以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１に、本発明の制御装置を備えたパラレルリンク式搬送装置１０の概略構成を示す。この搬送装置１０は、上記特許文献１（特開２００４−３４４８９９号公報）と同様の構成を有するものであり、上流側プレスステーション１４ａと、下流側プレスステーション１４ｂの間に設置され、上流側プレスステーション１４から成形された搬送物（パネル１）を受け取り、下流側プレスステーション１４へ搬送する。

図１に示すように、搬送装置１０は、パラレルリンク機構６と、パラレルリンク機構６の先端に接続されたパネル把持部１５と、パラレルリンク機構６の他端に接続された第１リンク駆動機構１１及び第２リンク駆動機構１２とを備える。
パネル把持部１５は、パネル１を吸着するバキュームカップ等のワーク把持具２と、該ワーク把持具２が取り付けられ搬送ラインと直角方向に延びるクロスバー３とを備えている。
パラレルリンク機構６は、第１リンク７と第２リンク８からなる。第１リンク７の他端に第１リンク駆動機構１１が接続され、第２リンクの他端に第２リンク駆動機構１２が接続されている。

第１リンク駆動機構１１及び第２リンク駆動機構１２は、Ｖ字型のベース部材１６に取り付けられている。第１リンク駆動機構１１及び第２リンク駆動機構１２は、それぞれ、ネジ軸で構成される第１スライド軸９及び第２スライド軸１３と、第１及び第２スライド軸９，１３を回転駆動するための第１駆動モータ１７及び第２駆動モータ１８と、第１及び第２スライド軸９，１３の回転によって直線駆動される第１スライド４及び第２スライド５とからなる。
第１駆動モータ１７及び第２駆動モータ１８は、後述するそれぞれのモータドライバ２７，２８により駆動される。
このような第１駆動モータ１７及び第２駆動モータ１８の駆動により、それぞれ第１スライド４及び第２スライド５が直線駆動され且つ数値制御等で正確に位置決めされるようになっている。

また、この図に示すように、第１スライド軸９及び第２スライド軸１３は、鉛直軸Ｙから所定の角度で互いに反対方向に傾斜している。
この構成により、第１スライド４は、その駆動領域のうち搬送ラインの上流側で低位置となり下流側で高位置となるように第１リンク駆動機構１１により水平面に対して傾斜して直線駆動されるようになっている。
また、第２スライド５は、その駆動領域のうち搬送ラインの上流側で高位置となり下流側で低位置となるように第２リンク駆動機構１２により水平面に対して傾斜して直線駆動されるようになっている。

なお、本実施形態において第１リンク駆動機構１１及び第２リンク駆動機構１２は、ボールネジ機構によって直動駆動機構を構成しているが、本発明はこれに限定されず、タイミングベルト、液圧シリンダ、ラックアンドピニオン、等によって直動駆動機構を構成してもよい。

上記の構成により、後述する制御装置２０により動作指令を与えることによって、第１駆動モータ１７及び第２駆動モータ１８を駆動させ、第１リンク７及び第２リンク８をそれぞれ独立して駆動させる。すると、第１リンク７及び第２リンク８の根元（第１スライド４及び第２スライド５）の相互位置関係によってパラレルリンク機構６の先端位置が決まる。しがたって、第１駆動モータ１７及び第２駆動モータ１８の駆動を独立に制御することにより、パラレルリンク機構６の先端に接続されたパネル把持部１５の位置を決定し、所望の搬送モーションを得ることができる。

図２に、本発明の実施形態にかかるパラレルリンク式搬送装置の制御装置２０の構成を示す。
図２に示すように、本発明の制御装置２０は、スライド目標位置演算手段２１と、モータ制御部２２，２３とトルク補正手段２４とを備える。
スライド目標位置演算手段２１は、リンク先端（パネル把持部１５）の位置目標値［Ｘ，Ｙ］に基づいて、第１スライド４及び第２スライド５の目標位置（ｐ，ｑ）を演算する。なお、以下、この目標位置（ｐ，ｑ）をスライド目標位置という。

モータ制御部２２，２３は複数あり、上述した第１駆動モータ１７及び第２駆動モータ１８を駆動する複数のモータドライバ２５，２６のそれぞれに、リンク先端を目標位置に移動させるためのトルク指令値を与える。本実施形態では、第１駆動モータ１７を駆動するモータドライバをｐ軸モータドライバ２５とし、第２駆動モータ１８を駆動するモータドライバをｑ軸モータドライバ２６とする。また、ｐ軸モータドライバ２５にトルク指令値を与えるモータ制御部をｐ軸モータ制御部２２とし、ｑ軸モータドライバ２６にトルク指令値を与えるモータ制御部をｑ軸モータ制御部２３とする。

第１駆動モータ１７、第２駆動モータ１８は、それぞれｐ軸モータドライバ２５、ｑ軸モータドライバ２６により駆動され、エンコーダ２７，２８によってその回転数が検出されて、その信号が、ｐ軸モータ制御部２２、ｑ軸モータ制御部２３にフィードバック信号として入力される。このように、制御装置２０は、駆動モータ１７，１８をフィードバック制御することによって位置決め制御を行う。

トルク補正手段２４は、パラレルリンク機構６の先端（パネル把持部１５）の位置目標値と搬送物の質量ｍ（パネル把持部１５とパネル１の合計質量）に基づいて、トルク指令値をそれぞれ補正するための複数の補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑを求め、複数のモータ制御部２２，２３にそれぞれ与えられるトルク指令値に補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑをそれぞれ加算する。この補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑは、具体的には、第１スライド軸９及び第２スライド軸１３がそれぞれ第１スライド４及び第２スライド５を軸方向に移動させるために必要な駆動力ｆｐ、ｆｑに、調整ゲインＫを乗じて得た値である。なお、以下、ｆ_ｐ，ｆ_ｑをスライド駆動力という。

以下、上記のスライド目標位置（ｐ，ｑ）、スライド駆動力ｆ_ｐ，ｆ_ｑの算出方法を説明する。

図３に、図１に示したパラレルリンク式搬送装置１０の模式図を示す。図３において、パネル把持部１５の位置を（Ｘ，Ｙ）、リンク７，８の長さをそれぞれＬ、第１スライド軸９、第２スライド軸１３の駆動軸をそれぞれｐ軸、ｑ軸、リンク７，８とｐ軸及びｑ軸との接点（第１スライド４、第２スライド５）の位置をｐ，ｑ、ｐ軸とｑ軸の交点（ｐ軸、ｑ軸の原点）を（Ｘ_０，Ｙ_０）、Ｘ軸とｐ軸とのなす角をα、Ｘ軸とｑ軸とのなす角をβとする。

パネル把持部１５の位置（Ｘ，Ｙ）と、スライド位置（ｐ，ｑ）との間には、下記数１に示す関係が成立する。

したがって、リンク先端（パネル把持部１５）を（Ｘ，Ｙ）に位置決めしたい場合、上記数１の（Ａ−１式）及び（Ａ−２式）に基づいて、対応するスライド位置（ｐ，ｑ）を算出し、第１スライド４、第２スライド５を移動させればよい。

さて、パネル把持部１５とパネル１の合計質量がｍであったとする。質量ｍの物体を、時間関数［Ｘ（ｔ），Ｙ（ｔ）］の軌道で移動させた場合の運動エネルギーＩ、位置エネルギーＵは、下記数２の（式１）、（式２）で計算される。

運動エネルギーＩ、位置エネルギーＵが与えられた場合、必要なスライド駆動力ｆ_ｐ，ｆ_ｑは、ラグランジュの運動方程式を考えた場合、下記数３の（式３）、（式４）で計算される。

（式３）、（式４）では運動エネルギーＩと位置エネルギーＵの差をスライドの位置座標で偏微分しているが、運動エネルギーと比較して位置エネルギーがほとんど変化しない場合には運動エネルギーが支配的となるため、運動エネルギーのみを偏微分する（位置エネルギーは考慮しない）こととしてもよい。逆に、位置エネルギーと比較して運動エネルギーがほとんど変化しない場合には位置エネルギーが支配的となるため、位置エネルギーのみを偏微分する（運動エネルギーは考慮しない）こととしてもよい。

運動エネルギーと比較して位置エネルギーがほとんど変化しない場合としては、搬送物が水平座標面で移動するような構成とした場合（例えば、図１の装置を横倒しにした場合）が考えられる。また、図１の構成であったとしても、搬送物の軌道によっては（たとえば、搬送物が垂直方向にほとんど変位しない場合等）その位置エネルギーがほとんど時間変化せず、運動エネルギーが支配的となる。

位置エネルギーと比較して運動エネルギーがほとんど変化しない場合としては、搬送装置が停止している場合が考えられる。また、搬送装置が停止していない場合であっても、搬送物の速度によっては（たとえば、極めて低速の場合等）その速度エネルギーがほとんど時間変化せず、位置エネルギーが支配的となる。

運動エネルギーと位置エネルギーの両方を考慮するか運動エネルギーと位置エネルギーのいずれか一方を考慮するかについては、搬送装置の構成や運転方法に基づいて予め定めておき、搬送装置の運転を始めた後は運転が終了するまで設定を変えないとすることも可能である。あるいは、搬送装置の運転中であっても搬送物の位置や速度に基づいて適宜切り替えることとしても良い。このような構成とすることにより、計算量が減少し、制御装置２０の負荷を抑えることが可能となる。なお、このような切り替えは、図２に示した制御装置２０では、トルク補正手段２４によって実行される。

以下、運動エネルギーと位置エネルギーの双方を考慮した場合について説明する。

上記の（式３）、（式４）に、（式１）、（式２）を代入し整理すると、下記数４の（式５）、（式６）が得られる。

パラレルリンク式搬送装置１０を考えた場合、上記の（Ｂ−１式）、（Ｂ−２式）より、パネル把持部１５の位置（Ｘ，Ｙ）は、下記数５に示すようなスライド位置（ｐ，ｑ）の関数であると言える。

よって、下記数６のように表せる。

上記の（式９）〜（式１８）を（式５）、（式６）に代入すると、下記数７に示すようなスライド駆動力ｆ_ｐ，ｆ_ｑの計算式（式１９）、（式２０）が得られる。

（式１９）、（式２０）はリンク先端位置（Ｘ，Ｙ）の偏微分関数が含まれている。これは（Ｂ−１式）、（Ｂ−２式）を偏微分することによって求まる。偏微分の結果は、下記数８に示すようなｐとｑの関数で表すことができる。

（式２１）〜（式３０）は事前に（Ｂ−１式）、（Ｂ−２式）から求めておくことが可能であり、（式１９）、（式２０）からリンク先端の質量ｍに対するスライド駆動力ｆ_ｐ，ｆ_ｑは、以下のステップ１〜６で算出できる。

（ステップ１）
パネル把持部の位置目標値［Ｘ，Ｙ］を（Ａ−１式）、（Ａ−２式）に代入して、第１スライド及び第２スライドの位置目標値［ｐ，ｑ］を求める。

（ステップ２）
ステップ１で求めた［ｐ，ｑ］を上記数８のＦ_１（ｐ，ｑ）〜Ｆ_１０（ｐ，ｑ）に代入し、偏微分値を求める。

（ステップ３）
パネル把持部１５の位置目標値［Ｘ，Ｙ］と、第１スライド４及び第２スライド５の位置目標値［ｐ，ｑ］に微分演算を行い、下記数９の速度目標値を求める。

（ステップ４）
ステップ２とステップ３の結果から、下記数１０のＶ_ｐ、Ｃ_ｐ、Ｇ_ｐ、Ｖ_ｑ、Ｃ_ｑ、Ｇ_ｑを計算する。

（ステップ５）
ステップ４で求めたＶ_ｐとＶ_ｑの微分値（下記数１１）を求める。

（ステップ６）
ステップ４とステップ５の結果から、スライド駆動力ｆ_ｐ，ｆ_ｑを下記数１２の（式３７）、（式３８）により求める。

上記ステップ１〜６の処理で求めたスライド駆動力ｆ_ｐ，ｆ_ｑに調整ゲインＫを乗じることにより、上述した補正トルク指令値Ｋｆｐ、Ｋｆｑを算出することができる。上記のトルク補正手段は、このような処理を実行して補正トルク指令値Ｋｆｐ、Ｋｆｑを算出する。

以下、上述した本発明のパラレルリンク式搬送装置の制御装置２０及び制御方法の作用効果について説明する。
上記の補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑは、位置目標値［Ｘ，Ｙ］と搬送物の質量ｍに基づくものであり、駆動軸の釣り合い力だけでなく、慣性力の影響を反映している。したがって、この補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑを、モータドライバ２５，２６へ与えるトルク指令値に加算することによって、パラレルリンク機構６を構成する各リンクの運動に対する補償制御を行うことができるので、搬送物を移動させている最中においてもリンク先端の位置精度を向上させることができる。

また、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を複数のスライド４，５の位置座標でそれぞれ偏微分した複数の関数（ラグランジュの運動方程式）に基づいて、複数の補正トルク指令値を求めることにより、慣性力の影響を正確に反映した適切な補正トルク指令値を算出することができる。

また、補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑの算出において、運動エネルギーのみ又は位置エネルギーのみを偏微分することにより、運動エネルギーと位置エネルギーの差を偏微分する場合と比較して、計算量が減少し、制御装置２０の負荷を抑えることができる。また、運動エネルギーと位置エネルギーの差の偏微分、運動エネルギーのみの偏微分及び位置エネルギーのみの偏微分のいずれからも補正トルク指令値を求めることができるようにしておき、運転中に搬送物の位置や速度に応じて適宜切り替えることによっても、計算量が減少し、制御装置２０の負荷を抑えることができる。

また、上記ステップ１〜６の処理は、トルク指令値を更新する制御サイクル毎に実行する。すなわち、複数の補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑを位置目標値の変化に応じて時々刻々と計算し、対応する複数のトルク指令値にそれぞれ加算することで、その時の目標位置指令に応じた適切な補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑが演算される。

また、生産するパネルサイズの変化や、パネル把持部のツール重量の変更があった場合は、それに応じて質量ｍを変更することで、搬送物の重量変更に対しても正確な補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑを求めることができる。

また、補正トルク指令値Ｋｆ_ｐ、Ｋｆ_ｑにより、先端重量部の変化や運動の影響を正確にキャンセルする分のトルク指令をフィードフォワード的にモータドライバへ与えることができ、図２に示したフィードバック制御系と組み合わせた場合、フィードバック部はパラレルリンク機構６を構成する各リンクの運動に対する補償制御のみを行うことになる。これはすなわち、搬送物の変化に対するロバスト性を補償するものであり、フィードバック制御系のゲインの調整や性能補償が容易となる。

なお、上述した実施形態では、２自由度のパラレルリンク機構をもつパラレルリンク式搬送装置を対象としたが、本発明の適用範囲はこれに限られず、３自由度以上のパラレルリンク機構をもつパラレルリンク式搬送装置にも同様に適用可能である。

上記において、本発明の実施形態について説明を行ったが、上記に開示された本発明の実施の形態は、あくまで例示であって、本発明の範囲はこれら発明の実施の形態に限定されない。本発明の範囲は、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。

本発明の制御装置を備えたパラレルリンク式搬送装置の概略構成図である。 本発明の実施形態にかかるパラレルリンク式搬送装置の制御装置の構成図である。 図１に示したパラレルリンク式搬送装置の模式図である。

符号の説明

１ パネル
２ ワーク把持具
３ クロスバー
４ 第１スライド
５ 第２スライド
６ パラレルリンク機構
７ 第１リンク
８ 第２リンク
１０ パラレルリンク式搬送装置
１１ 第１リンク駆動機構
１２ 第２リンク駆動機構
１５ パネル把持部
１６ ベース部材
１７ 第１駆動モータ
１８ 第２駆動モータ
２０ 制御装置
２１ スライド目標位置演算手段
２２ ｐ軸モータ制御部
２３ ｑ軸モータ制御部
２４ トルク補正手段
２５ ｐ軸モータドライバ
２６ ｑ軸モータドライバ
２７，２８ エンコーダ

Claims (14)

1. 複数のリンクの駆動により該複数のリンクが連結された先端の位置が決定されるパラレルリンク機構と、前記複数のリンクをそれぞれ駆動する複数の駆動モータと、トルク指令値に基づいて前記複数の駆動モータをそれぞれ駆動する複数のモータドライバとを備え前記パラレルリンク機構の駆動により搬送物を搬送するパラレルリンク式搬送装置の制御装置であって、
   前記複数のモータドライバのそれぞれにトルク指令値を与える複数のモータ制御部と、
   前記パラレルリンク機構の先端の位置目標値と搬送物の質量ｍに基づいて、前記トルク指令値をそれぞれ補正するための複数の補正トルク指令値を求め、前記複数のモータ制御部にそれぞれ与えられるトルク指令値に前記補正トルク指令値をそれぞれ加算するトルク補正手段と、を備えることを特徴とするパラレルリンク式搬送装置の制御装置。
2. 前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
   前記トルク補正手段は、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項１記載のパラレルリンク式搬送装置の制御装置。
3. 前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
   前記トルク補正手段は、搬送物の目標速度から求められる運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項１記載のパラレルリンク式搬送装置の制御装置。
4. 前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
   前記トルク補正手段は、搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項１記載のパラレルリンク式搬送装置の制御装置。
5. 前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
   前記トルク補正手段は、搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の第１関数、前記運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第２関数、又は前記位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第３関数に基づいて前記複数の補正トルク指令値を求め、前記パラレルリンク式搬送装置の運転中に前記搬送物の位置及び／又は速度に応じて前記第１関数、前記第２関数又は前記第３関数のいずれに基づいて前記複数の補正トルク指令値を求めるかを切り替えることを特徴とする請求項１記載のパラレルリンク式搬送装置の制御装置。
6. 前記偏微分した関数がラグランジュの運動方程式と等価な式であることを特徴とする請求項２乃至５のいずれか記載のパラレルリンク式搬送装置の制御装置。
7. 前記トルク補正手段は、前記複数の補正トルク指令値を前記位置目標値の変化に応じて時々刻々と計算し、対応する前記複数のトルク指令値にそれぞれ加算することを特徴とする請求項１乃至６のいずれか記載のパラレルリンク式搬送装置の制御装置。
8. 複数のリンクの駆動により該複数のリンクが連結された先端の位置が決定されるパラレルリンク機構と、前記複数のリンクをそれぞれ駆動する複数の駆動モータと、トルク指令値に基づいて前記複数の駆動モータをそれぞれ駆動する複数のモータドライバとを備え前記パラレルリンク機構の駆動により搬送物を搬送するパラレルリンク式搬送装置の制御方法であって、
   前記複数のモータドライバに与える各トルク指令値に、前記パラレルリンク機構の先端の位置目標値と搬送物の質量ｍに基づいて求めた複数の補正トルク指令値をそれぞれ加算することを特徴とするパラレルリンク式搬送装置の制御方法。
9. 前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
   搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項８記載のパラレルリンク式搬送装置の制御方法。
10. 前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
    搬送物の目標速度から求められる運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項８記載のパラレルリンク式搬送装置の制御方法。
11. 前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
    搬送物の目標軌道から求められる位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の関数に基づいて、前記複数の補正トルク指令値を求めることを特徴とする請求項８記載のパラレルリンク式搬送装置の制御方法。
12. 前記複数のリンクは、それぞれ、直線移動する複数のスライドを介して前記駆動モータにより駆動されるものであり、
    搬送物の目標軌道と目標速度から求められる運動エネルギーと位置エネルギーの差を前記複数のスライドの移動座標でそれぞれ偏微分した複数の第１関数、前記運動エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第２関数、又は前記位置エネルギーを前記複数のスライドの移動座標でそれぞれ偏微分した複数の第３関数に基づいて前記複数の補正トルク指令値を求め、前記パラレルリンク式搬送装置の運転中に前記搬送物の位置及び／又は速度に応じて前記第１関数、前記第２関数又は前記第３関数のいずれに基づいて前記複数の補正トルク指令値を求めるかを切り替えることを特徴とする請求項８記載のパラレルリンク式搬送装置の制御方法。
13. 前記偏微分した関数がラグランジュの運動方程式と等価な式であることを特徴とする請求項９乃至１２のいずれか記載のパラレルリンク式搬送装置の制御方法。
14. 前記複数の補正トルク指令値を前記位置目標値の変化に応じて時々刻々と計算し、対応する前記複数のトルク指令値にそれぞれ加算することを特徴とする請求項８乃至１３のいずれか記載のパラレルリンク式搬送装置の制御方法。

Images