本発明は、搬送装置に関する。

下記特許文献1には、一方のプレス装置から他方のプレス装置へ搬送対象物を搬送する搬送装置が開示されている。この搬送装置は、パラレルリンク機構の先端に搬送対象物を支持する支持部が設けられており、当該パラレルリンク機構を駆動して搬送対象物を支持部により搬送するパラレルリンク式の搬送装置である。

ところで、一方のプレス装置から他方のプレス装置へ搬送対象物を搬送するストローク(以下、「搬送ストローク」という。)を長くしたいという要望がある。しかしながら、上記パラレルリンク式の搬送装置において、搬送ストロークを長くするにはパラレルリンク機構を構成する各アームを長くする必要があり、搬送装置が大型化する。

特開2008−142724号公報

そこで、本願の出願人は、搬送ストロークを長くしながら搬送装置の大型化を抑制することを目的として、シリアルリンク機構の先端に支持部を設け、当該シリアルリンク機構の姿勢をパラレルリンク機構で制御可能な搬送装置を既に出願している(特願2017−087598)。

ただし、パラレルリンク機構とシリアルリンク機構とを組み合わせた上記搬送装置に対して、従来のようなパラレルリンク式の搬送装置の制御方法をそのまま適用すると、シリアルリンク機構が考慮されていないため、支持部の位置精度が悪化する場合がある。

本発明は、このような事情に鑑みてなされたもので、その目的は、パラレルリンク機構とシリアルリンク機構とを組み合わせた搬送装置であって、支持部の精度を向上させる搬送装置及びその制御方法を提供することである。

本発明の一態様は、旋回アームとメインアームとが自由関節で接続され、前記メインアームの先端に搬送対象物を支持する支持部を有するシリアルリンク機構と、前記メインアームに対して摺動可能に接続された摺動部が先端に設けられているパラレルリンク機構と、前記パラレルリンク機構を駆動する第1の駆動部と、前記第1の駆動部を制御して前記摺動部を目標位置に摺動させる制御装置と、を備え、前記制御装置は、前記シリアルリンク機構のみを抽出して前記自由関節に第2の駆動部があると仮定した場合に前記第2の駆動部で前記メインアームを駆動するために必要な仮想駆動力を算出し、前記仮想駆動力を前記パラレルリンク機構で補償するように前記第1の駆動部を制御することを特徴とする、搬送装置である。

本発明の一態様は、上述の搬送装置であって、前記パラレルリンク機構は、一端部が前記摺動部に対して回転可能に接続されかつ他端部が前記搬送対象物の搬送方向下流側に向かって移動されるに連れて下降されるスイングアームと、一端部が前記摺動部に対して回転可能に接続されかつ他端部が前記搬送対象物の搬送方向下流側に向かって移動されるに連れて上昇されるサポートアームと、を備え、前記制御装置は、前記仮想駆動力に相当するモーメントを、前記摺動部を介して前記スイングアームと前記サポートアームとの合力で前記メインアームに与えるように前記第1の駆動部を制御してもよい。

本発明の一態様は、上述の搬送装置であって、前記制御装置は、前記第1の駆動部を駆動して前記スイングアームと前記サポートアームとを駆動して前記摺動部を目標位置に制御するために必要な駆動力をスライド駆動力として、前記パラレルリンク機構の運動方程式に基づいて算出する第1の算出部と、前記仮想駆動力を前記シリアルリンク機構の運動方程式に基づいて算出する第2の算出部と、前記スライド駆動力及び前記仮想駆動力に基づいて、前記第1の駆動部のトルクをフィードフォワード制御する制御部と、を備えてもよい。

本発明の一態様は、上述の搬送装置であって、前記制御部は、前記第1の駆動部のトルク指令値に、前記スライド駆動力及び前記仮想駆動力に基づいて演算されたフィードフォワードトルク指令値を加算した値に従って、前記第1の駆動部を制御してもよい。

本発明の一態様は、上述の搬送装置であって、前記第1の駆動部は、前記スイングアームを駆動するスイングモータと、前記サポートアームを駆動するサポートモータと、を有し、前記制御部は、前記仮想駆動力に相当するモーメントを、前記スイングモータのスイングトルクと、前記サポートモータのサポートトルクとに変換するトルク変換部と、前記スライド駆動力における前記スイングモータの駆動力と、前記スイングトルクと、に基づいて前記スイングモータのフィードフォワードトルク指令値を演算し、当該フィードフォワードトルク指令値を前記スイングモータのトルク指令値に加算した値に基づいて前記スイングモータを制御するスイングモータ制御部と、前記スライド駆動力における前記サポートモータの駆動力と、前記サポートトルクと、に基づいて前記サポートモータのフィードフォワードトルク指令値を演算し、当該フィードフォワードトルク指令値を前記サポートモータのトルク指令値に加算した値に基づいて前記サポートモータを制御するサポートモータ制御部と、を備えてもよい。

本発明の一態様は、上述の搬送装置であって、前記旋回アームを駆動する旋回モータを備え前記第2の算出部は、前記シリアルリンク機構のみを抽出して前記自由関節に第2の駆動部があると仮定した場合における前記シリアルリンク機構の運動方程式を求め、当該運動方程式に基づいて前記旋回アームを駆動するために必要な旋回駆動力を算出し、前記制御部は、前記旋回駆動力に基づいて前記旋回モータのトルクをフィードフォワード制御する。

以上説明したように、本発明によれば、パラレルリンク機構とシリアルリンク機構とを組み合わせた搬送装置であって、支持部の精度を向上させることができる。

本発明の一実施形態に係る搬送装置の概略構成図であり、(a)が側断面図であり、(b)が正断面図である。

本発明の一実施形態に係る搬送装置の模式図であり、動作を説明するための説明図である。

本発明の一実施形態に係る搬送装置の模式図であり、動作を説明するための説明図である。

本発明の一実施形態に係る搬送装置の模式図であり、動作を説明するための説明図である。

本発明の一実施形態に係る制御装置10の制御ブロックの一例を示す図である。

本発明の一実施形態に係る駆動力fp及び駆動力fqの算出方法を説明する図である。

本発明の一実施形態に係る駆動力fp及び駆動力fqを算出する流れを示す図である。

本発明の一実施形態に係る駆動力τ

₂の算出方法を説明する図である。

本発明の一実施形態に係るメインアーム6を駆動するためのスイング軸、サポート軸トルクの算出方法を説明する図である。

本発明の一実施形態に係る駆動力τ

₂に相当するモーメント力を分解する様子を示す模式図である。

本発明の一実施形態に係るスイングアーム9cの方向の力f

_rpとサポートアーム9eの方向の力f

_rqとの合力を示す図である。

本発明の一実施形態に係るf

_rp及びf

_rqのベクトルの方向と、f

_τ2のベクトルの方向との関係を示す図である。

図10の摺動ブロック9fで分解した力ベクトルをスイング軸、サポート軸に移動させた図である。

以下、本発明の一実施形態に係る搬送装置を、図面を用いて説明する。

図1は、本実施形態の搬送装置1の概略構成図であり、(a)が側断面図であり、(b)が正断面図である。なお、図1(a)は、図1(b)のA−A断面図である。また、図1(b)は、図1(a)のB−B断面図である。

本実施形態の搬送装置1は、四輪自動車のドアパネルP(搬送対象物)をプレス加工する複数のプレス装置(図1(a)におけるプレス装置X1とプレス装置X2)の間に配置されており、一方のプレス装置X1でプレス加工されたドアパネルPを他方のプレス装置X2へ搬送する。なお、本実施形態においては、搬送装置1の搬送対象物をドアパネルPとするが、本発明における搬送対象物はドアパネルPに限られるものでない。

本実施形態の搬送装置1は、図1(a)及び図1(b)に示すように、架台2、旋回モータ3、旋回軸部4、旋回アーム5、メインアーム6、クロスバー7、吸着部8、及び傾斜調整部9を備えている。なお、旋回アーム5及びメインアーム6は、本発明の「シリアルリンク機構」を構成する。また、傾斜調整部9は、本発明の「パラレルリンク機構」を有している。

架台2は、プレス装置X1とプレス装置X2との間にて床部に立設されており、旋回モータ3、旋回軸部4、旋回アーム5(第1のアーム)、メインアーム6(メインアーム)、クロスバー7、吸着部8、傾斜調整部9及び制御装置10を直接あるいは間接的に支持している。

架台2は、メインアーム6が移動する場合にも移動及び変形せず、本実施形態の搬送装置1の強度部材である。なお、本実施形態の搬送装置1において架台2は、プレス装置X1及びプレス装置X2と別体として設けられている。しかしながら、架台2をプレス装置X1あるいはプレス装置X2と一体化する構成や、プレス装置X1あるいはプレス装置X2のフレームを架台2とする構成を採用することも可能である。

本実施形態において、架台2は、図1(b)に示すように、ドアパネルPの搬送領域を挟んで立設される複数の柱部2aと、ドアパネルPの搬送領域の上方にて柱部2aから水平方向に延出される梁部2bとを有している。梁部2bは、内部が中空とされた長尺状の箱形状とされており、メインアーム6を挟むように一対設けられている。これら一対の梁部2bは、ドアパネルPの搬送方向と直交する水平方向から見て重ねて配置されており、互いに対向する側の端部が開口端とされている。

旋回モータ3は、旋回アーム5を旋回駆動させるための動力を生成する駆動部であって、例えば、サーボモータである。この旋回モータ3は、制御装置10の制御の下に動力を生成する。旋回モータ3は、一対の梁部2bに各々設けられている。つまり、本実施形態の搬送装置1においては、2つの旋回モータ3が設置されている。これらの旋回モータ3は、制御装置10により同期制御される。

旋回軸部4は、旋回モータ3と旋回アーム5とを接続する軸部材であり、旋回モータ3から伝達される回転動力によって回転される。旋回軸部4は、2つの旋回モータ3の出力軸の各々に設けられている。つまり、本実施形態の搬送装置1においては、2つの旋回軸部4が設けられている。これらの旋回軸部4は、図1(b)に示すように、梁部2bの端部よりもメインアーム6側の位置に配置されている。また、これらの旋回軸部4は、ドアパネルPの搬送方向と直交する水平方向に沿う単一の旋回軸芯L1を中心として旋回される。なお、旋回モータ3と旋回軸部4との間には、必要に応じて減速機等の装置が設置されても良い。

旋回アーム5は、一端部が旋回軸部4に固定されると共に他端部がメインアーム6の上端部に回転可能に接続されたアーム部材である。旋回アーム5は、2つの旋回軸部4の各々に設けられている。つまり、本実施形態の搬送装置1においては、2つの旋回アーム5が設けられている。

各旋回アーム5は、同一の長さ寸法に設定されており、ドアパネルPの搬送方向と直交する水平方向から見て重なる姿勢で旋回軸部4に固定されている。各旋回アーム5は、旋回軸部4が回転されることにより、旋回軸芯L1を中心として回転される。つまり、各旋回アーム5は、旋回モータ3によって旋回軸芯L1を中心として同期して回転駆動可能とされている。 さらに、旋回アーム5の他端部は、ドアパネルPの搬送方向と直交する水平方向に沿う回転軸芯を中心とする回転方向に回転可能となるように、メインアーム6に接続されている。つまり、メインアーム6は、上端部が旋回アーム5の他端部に軸支されている。したがって、旋回アーム5の他端部とメインアーム6の上端部とが自由関節で接続されていることになる。

また、旋回アーム5の他端部がメインアーム6の上端部に接続されているため、旋回アーム5の他端部の位置がメインアーム6の上端部の位置となる。本実施形態では、後述のように旋回アーム5の他端部が旋回軸部4の真下に位置する場合に、メインアーム6が最も寝かされた姿勢となる。このため、旋回アーム5の長さ寸法は、最も寝かされた状態のメインアーム6の上端位置を規定し、最も寝かされた状態のメインアーム6の傾斜角度がプレス装置X1及びプレス装置X2に対してドアパネルPを出し入れ可能な角度となるように設定されている。例えば、本実施形態の搬送装置1において旋回アーム5の長さ寸法は、先端部が最下位置となった場合に、高さ方向において先端部がプレス装置(プレス装置X1及びプレス装置X2)の出入口(高さ方向に離間されたダイとパンチとの間)と重なる位置となるように設定されている。

メインアーム6は、上端部が上述のように旋回アーム5の他端部に自由関節で回転可能に接続されると共に先端部である下端部にドアパネルPを支持する支持部(クロスバー7及び吸着部8等)が接続されている。

メインアーム6は、上端部から下端部に向けて直線状に延伸する長尺部材であり、長手方向と直交する断面形状が略矩形状とされている。また、メインアーム6は、旋回アーム5の他端部が最上部に位置する場合に、下端部が傾斜調整部9よりも下方に位置するように長さ寸法(長手方向の寸法)が設定されている。また、メインアーム6の周面は、傾斜調整部9の後述する摺動ブロック9fがスライドする摺動面となっている。このようなメインアーム6は、旋回モータ3から旋回軸部4及び旋回アーム5を介して伝達される動力によって移動可能とされ、さらに傾斜調整部9によって移動中の姿勢が変更される。

クロスバー7は、ドアパネルPの搬送方向と直交する水平方向に直線状に延伸する長尺部材であり、図1(b)に示すようにドアパネルPの搬送方向から見てメインアーム6と直交するように接続されている。このクロスバー7は、長手方向における中央部がメインアーム6の下端部に接続されており、ドアパネルPの搬送方向と直交する水平方向に沿った回転軸芯を中心として回転可能にメインアーム6の下端部に軸支されている。

吸着部8は、ドアパネルPの上面に吸着することによりドアパネルPを支持する部位であり、クロスバー7から下方に垂下するようにしてクロスバー7の長手方向に配列されて複数設けられている。これらの吸着部8では、制御装置10の制御の下で不図示の真空発生装置(ポンプ、インジェクタ等)により形成される負圧によりドアパネルPを吸着支持する。

なお、メインアーム6とクロスバー7との接続部分には、クロスバー7の回転角度を調整する不図示のチルト調整機構が収容されている。このチルト調整機構は、サーボモータ等を備えており、吸着部8に支持されたドアパネルPの姿勢が安定するようにクロスバー7の回転姿勢を調整する。本実施形態の搬送装置1においては、上述のクロスバー7と吸着部8と不図示のチルト調整機構等により、本発明の支持部が構成されている。ただし、チルト調整機構を設けずに本発明の支持部を構成することも可能である。

傾斜調整部9は、旋回モータ3の動力により移動されるメインアーム6の姿勢を調整する機構であり、ベース部9a(図1(a)においては不図示)、第1直動機構9b、スイングアーム9c(第1リンク棒)、第2直動機構9d、サポートアーム9e(第2リンク棒)、及び摺動ブロック9fを備えている。なお、スイングアーム9c及びサポートアーム9eは、本発明の「パラレルリンク機構」を構成する。

ベース部9aは、架台2の梁部2bに固定されており、第1直動機構9bや第2直動機構9dを支持する強度部材である。図1(b)に示すように、本実施形態の搬送装置1では、第1直動機構9bを支持するベース部9aと、第2直動機構9dを支持するベース部9aとの合計2つのベース部9aが設けられている。

第1直動機構9bは、スイングアーム9cの上端部(他端部)を直動させるユニットである。具体的には、第1直動機構9bは、第1直動ガイド9b1、第1直動用モータ9b2、及び第1ボールネジ9b3を備えている。

第1直動ガイド9b1は、図1(a)に示すように、ドアパネルPの搬送方向において上端部が下端部よりも下流側に位置するようにして傾斜して配置されたガイドであり、ドアパネルPの搬送方向下流側に向かうに連れて下方に向かうようにスイングアーム9cの上端部を直線状に案内する。

第1直動用モータ(スイングモータ)9b2は、スイングアーム9cの上端部を移動させるための動力を生成する駆動部であって、例えば、サーボモータである。第1直動用モータ9b2は、第1直動ガイド9b1に固定されている。この第1直動用モータ9b2は、制御装置10の制御の下に、動力を生成する。

第1ボールネジ9b3は、第1直動用モータ9b2で生成された動力をスイングアーム9cの上端部に伝達することで、スイングアーム9cの上端部を第1直動ガイド9b1に沿って直線運動させる。

スイングアーム9cは、上端部が第1直動機構9bの第1直動ガイド9b1に対して摺動可能に取り付けられ、下端部(一端部)が摺動ブロック9fの外周面に対して回転可能に接続されたリンク棒である。このスイングアーム9cの下端部は、ドアパネルPの搬送方向と直交する水平方向に沿った回転軸芯を中心として回転可能に摺動ブロック9fと接続されている。

第2直動機構9dは、サポートアーム9eの上端部(他端部)を直動させるユニットである。具体的には、第2直動機構9dは、第2直動ガイド9d1、第2直動用モータ9d2、及び第2ボールネジ9d3を備えている。

第2直動ガイド9d1は、図1(a)に示すように、ドアパネルPの搬送方向において上端部が下端部よりも上流側に位置するようにして傾斜して配置されたガイドであり、ドアパネルPの搬送方向下流側に向かうに連れて上方に向かうようにサポートアーム9eの上端部を直線状に案内する。

第2直動用モータ(サポートモータ)9d2は、サポートアーム9eの上端部を移動させるための動力を生成する駆動部であって、例えば、サーボモータである。第2直動用モータ9d2は、第2直動ガイド9d1に固定されている。この第2直動用モータ9d2は、制御装置10の制御の下に、動力を生成する。

第2ボールネジ9d3は、第2直動用モータ9d2で生成された動力をサポートアーム9eの上端部に伝達することで、サポートアーム9eの上端部を第2直動ガイド9d1に沿って直線運動させる。

サポートアーム9eは、上端部が第2直動機構9dの第2直動ガイド9d1に対して摺動可能に取り付けられ、下端部(一端部)が摺動ブロック9fの外周面に対して回転可能に接続されたリンク棒である。このサポートアーム9eの下端部は、ドアパネルPの搬送方向と直交する水平方向に沿った回転軸芯を中心として回転可能に摺動ブロック9fと接続されている。なお、サポートアーム9eの下端部の回転軸芯と、上述のスイングアーム9cの下端部の回転軸芯とは、ドアパネルPの搬送方向と直交する水平方向から見て重なるように配置されている。

摺動ブロック9fは、メインアーム6の長手方向に沿う方向から見て中央部にメインアーム6が挿通される開口がある筒部であり、メインアーム6に対してスライド可能に外装されている。この摺動ブロック9fは、パラレルリンク機構の先端(スイングアーム9cの一端部とサポートアーム9eの一端部)に接続されており、内周面がメインアーム6の外周面に対して摺動可能とされている。摺動ブロック9fは、スイングアーム9c及びサポートアーム9eの移動によってメインアーム6に沿って移動することでメインアーム6を支える位置を変更し、これによってメインアーム6の姿勢、すなわち支持部の位置を変更する。

このような傾斜調整部9は、メインアーム6に対してスライド可能に接続された摺動ブロック9fを備え、この摺動ブロック9fに対して2本のリンク棒(スイングアーム9c及びサポートアーム9e)が接続されたパラレルリンク構造を有している。つまり、本実施形態の搬送装置1において傾斜調整部9は、メインアーム6に対してメインアーム6の長手方向にスライド可能に接続されると共にパラレルリンク機構を有し、メインアーム6の傾きを調整する。

制御装置10は、旋回モータ3の駆動を制御して旋回アーム5の旋回角度を制御する。さらに、制御装置10は、第1直動用モータ9b2及び第2直動用モータ9d2の駆動を制御することでパラレルリンク機構を介してシリアルリンク機構に設けられた支持部を所望の位置に制御する。すなわち、制御装置10は、パラレルリンク機構によってシリアルリンク機構の先端の姿勢角を補償している。 なお、制御装置10は、CPU又はMPUなどのマイクロプロセッサ、MCUなどのマイクロコントローラなどにより構成されてよい。

次に、本実施形態の搬送装置1におけるシリアルリンク機構及びパラレルリンク機構の動作について、図2〜図4を参照して説明する。なお、図2〜図4においては、各構成要素を模式的に示すと共に、上述の架台2及び旋回モータ3を省略して図示し、傾斜調整部9の第1直動用モータ9b2、第1ボールネジ9b3、第2直動用モータ9d2及び第2ボールネジ9d3も省略して図示している。

また、以下の説明においては、プレス装置X1(図2〜図4における左側)で成形されたドアパネルPをプレス装置X2(図2〜図4における右側)に搬送するまでの動作を説明する。また、図2〜図4に示す各図は、プレス装置X1からプレス装置X2までドアパネルPを連続的に搬送する過程の途中の瞬間を図示したものであり、図2〜図4の各図に示す状態でドアパネルPの搬送が一旦停止されることを意味するものではない。

まず、制御装置10は、旋回モータ3を駆動することにより、図2(a)に示すように、旋回モータ3の先端部、自由関節が最下位置から僅かにプレス装置X1側(図2(a)における左側)に位置するように旋回アーム5の旋回角度を設定する。同時に、制御装置10は、第1直動用モータ9b2を駆動することにより、スイングアーム9cの上端部を第1直動ガイド9b1の中央部に配置し、第2直動用モータ9d2を駆動することにより、サポートアーム9eの上端部を第2直動ガイド9d1の下端近傍に配置する。これにより、クロスバー7は、最もプレス装置X1側であって下降された位置に配置され、吸着部8がドアパネルPに当接する。ここで、吸着部8は、ドアパネルPを上面側から吸着支持する。これにより、ドアパネルPの移動が可能となる。

さらに、制御装置10は、旋回モータ3を駆動し続けることにより、図2(b)に示すように、自由関節が最下位置に近づくように旋回アーム5を旋回させる。同時に、制御装置10は、第1直動用モータ9b2を駆動することにより、スイングアーム9cの上端部を第1直動ガイド9b1の上端に近づけ、第2直動用モータ9d2を駆動することにより、サポートアーム9eの上端部を第2直動ガイド9d1の上端に近づける。これにより、クロスバー7は、図2(a)の状態よりも上昇される。このため、吸着部8に吸着されたドアパネルPが持ち上げられる。

さらに、制御装置10は、旋回モータ3を駆動し続けることにより、図2(c)に示すように、自由関節が最下位置を通過してプレス装置X2側(図2(c)における右側)に位置するように旋回アーム5を旋回させる。同時に、制御装置10は、第1直動用モータ9b2を駆動することにより、スイングアーム9cの上端部を第1直動ガイド9b1の上端にさらに近づけ、第2直動用モータ9d2を駆動することにより、サポートアーム9eの上端部を第2直動ガイド9d1の上端にさらに近づける。これにより、メインアーム6がプレス装置X2側に引かれると共に摺動ブロック9fに摺動支持されて立ち上げられ、クロスバー7はプレス装置X2側に略水平移動される。このため、吸着部8に吸着されたドアパネルPは、プレス装置X2に向けて略水平移動される。

さらに、制御装置10は、旋回モータ3を駆動し続けることにより、図3(a)に示すように、自由関節がプレス装置X2側(図3(a)における右側)であって高さ方向において旋回軸部4よりも高くなる位置となるように旋回アーム5を旋回させる。同時に、制御装置10は、第1直動用モータ9b2を駆動することにより、スイングアーム9cの上端部を第1直動ガイド9b1の上端にさらに近づけ、第2直動用モータ9d2を駆動することにより、サポートアーム9eの上端部を第2直動ガイド9d1の上端にさらに近づける。これにより、さらにメインアーム6がプレス装置X2側に引かれると共に摺動ブロック9fに摺動支持されて立ち上げられ、クロスバー7はプレス装置X2側に略水平移動される。このため、吸着部8に吸着されたドアパネルPは、さらにプレス装置X2に向けて略水平移動される。

さらに、制御装置10は、旋回モータ3を駆動し続けることにより、図3(b)に示すように、自由関節が最上位置となるように旋回アーム5を旋回させる。同時に、制御装置10は、第1直動用モータ9b2を駆動することにより、スイングアーム9cの上端部を第1直動ガイド9b1の下端に近づけ、第2直動用モータ9d2を駆動することにより、サポートアーム9eの上端部を第2直動ガイド9d1の上端にさらに近づけ、スイングアーム9cの上端部とサポートアーム9eの上端部との高さ位置が同じとする。これにより、メインアーム6が鉛直姿勢となり、クロスバー7はプレス装置X1とプレス装置X2との間に位置することになる。このため、吸着部8に吸着されたドアパネルPは、プレス装置X1とプレス装置X2との間に配置されることとなる。

さらに、制御装置10は、旋回モータ3を駆動し続けることにより、図3(c)に示すように、自由関節がプレス装置X1側(図3(c)における左側)であって高さ方向において旋回軸部4よりも高くなる位置となるように旋回アーム5を旋回させる。同時に、制御装置10は、第1直動用モータ9b2を駆動することにより、スイングアーム9cの上端部を第1直動ガイド9b1の下端に近づけ、第2直動用モータ9d2を駆動することにより、サポートアーム9eの上端部を第2直動ガイド9d1の上端に近づける。これにより、さらにメインアーム6がプレス装置X1側に引かれると共に摺動ブロック9fに摺動支持されて寝かされ、クロスバー7はプレス装置X2側に略水平移動される。このため、吸着部8に吸着されたドアパネルPは、さらにプレス装置X2に向けて略水平移動される。

さらに、制御装置10は、旋回モータ3を駆動し続けることにより、図4(a)に示すように、自由関節が最下位置よりもプレス装置X1側(図4(a)における左側)であって高さ方向において旋回軸部4よりも低くなる位置となるように旋回アーム5を旋回させる。同時に、制御装置10は、第1直動用モータ9b2を駆動することにより、スイングアーム9cの上端部を第1直動ガイド9b1の下端にさらに近づけ、第2直動用モータ9d2を駆動することにより、サポートアーム9eの上端部を第2直動ガイド9d1の下端に近づける。これにより、メインアーム6がプレス装置X1側に押されると共に摺動ブロック9fに摺動支持されてさらに寝かされ、クロスバー7はプレス装置X2側にさらに略水平移動される。このため、吸着部8に吸着されたドアパネルPは、プレス装置X2に向けてさらに略水平移動される。

さらに、制御装置10は、旋回モータ3を駆動し続けることにより、図4(b)に示すように、自由関節が最下位置から僅かにプレス装置X2側(図4(b)における右側)に配置されるように旋回アーム5を旋回させる。同時に、制御装置10は、第1直動用モータ9b2を駆動することにより、スイングアーム9cの上端部を第1直動ガイド9b1のさらに下端に近づけ、第2直動用モータ9d2を駆動することにより、サポートアーム9eの上端部を第2直動ガイド9d1の下端にさらに近づける。これにより、クロスバー7は、プレス装置X2のドアパネルPの載置位置の上方に配置される。このため、吸着部8に吸着されたドアパネルPもプレス装置X2のドアパネルPの載置位置の上方に配置される。

さらに、制御装置10は、旋回モータ3を駆動し続けることにより、図4(c)に示すように、自由関節が最下位置からさらにプレス装置X2側(図4(c)における右側)に位置するように旋回アーム5を旋回させる。同時に、制御装置10は、第1直動用モータ9b2を駆動することにより、スイングアーム9cの上端部を第1直動ガイド9b1のさらに下端に近づけ、第2直動用モータ9d2を駆動することにより、サポートアーム9eの上端部を第2直動ガイド9d1の下端にさらに近づける。これにより、クロスバー7は、最もプレス装置X2側であって図4(b)で示された位置よりも下降された位置に配置され、吸着部8に吸着されたドアパネルPがプレス装置X2に載置される。この後、吸着部8からドアパネルPを解離させることにより、ドアパネルPのプレス装置X1からプレス装置X2への搬送が完了する。

なお、クロスバー7をプレス装置X2からプレス装置X1に戻す場合には、図2〜図4を用いて説明した上述の動作の反対の動作を行う。これによって、クロスバー7が連続的に略水平移動されてプレス装置X2からプレス装置X1まで移動される。

また、上述の動作説明における旋回アーム5の旋回角度、スイングアーム9cの上端部の位置、及び、サポートアーム9eの上端部の位置は、各部材を図1に示す寸法や形状とした場合のあくまでも一例である。各部材の寸法や形状は特に限定されるものではない。 このため、各部材の寸法や形状が変更になった場合には、その寸法や形状に沿って、旋回アーム5の旋回角度、スイングアーム9cの上端部の位置、及び、サポートアーム9eの上端部の位置も変更される。実際には、メインアーム6の先端位置(すなわちクロスバー7の位置)が所望の位置となるように、旋回アーム5の旋回角度、スイングアーム9cの上端部の位置、及び、サポートアーム9eの上端部の位置が設定される。

次に、本実施形態に係る制御装置10の構成について、図5を用いて説明する。図5は、本実施形態に係る制御装置10の制御ブロックの一例を示す図である。

制御装置10は、クロスバー目標値取得部11、第1の制御部12A、及び第2の制御部12Bを備える。

クロスバー目標値取得部11は、クロスバー7を動かす目標位置(以下、「クロスバー目標位置」という。)である(X_cb,Y_cb)を取得する。なお、本実施形態のクロスバー目標位置(X_cb,Y_cb)の取得方法は、特に限定されないが、例えば、予め搬送装置1内の記憶部に記憶されており、クロスバー目標値取得部11が当該記憶部からクロスバー目標位置(X_cb,Y_cb)を読み取ることで取得してもよい。クロスバー目標値取得部11は、取得したクロスバー目標位置(X_cb,Y_cb)を、第1の制御部12A及び第2の制御部12Bのそれぞれに出力する。

第1の制御部12Aは、旋回モータ3の駆動を制御する。本実施形態では、第1の制御部12Aは、旋回モータ3のトルクをフィードフォワード制御する。ただし、本発明はこれに限定されず、第1の制御部12Aは、フィードフォワード制御以外の制御方法で旋回モータ3のトルクを制御してもよい。例えば、第1の制御部12Aは、旋回モータ3に対して通常のモータ位置によるフィードバック制御(例えば、PID制御)を行ってもよい。

第2の制御部12Bは、スイングモータ9b2及びサポートモータ9d2の駆動をそれぞれ独立して制御する。本実施形態では、第2の制御部12Bは、スイングモータ9b2及びサポートモータ9d2のそれぞれのトルクを独立してフィードフォワード制御する。ただし、本発明はこれに限定されず、第2の制御部12Bは、フィードフォワード制御以外の制御方法でスイングモータ9b2及びサポートモータ9d2のそれぞれのトルクを独立してもよい。例えば、第2の制御部12Bは、スイングモータ9b2及びサポートモータ9d2のそれぞれに対して通常のモータ位置によるフィードバック制御(例えば、PID制御)を行ってもよい。 まず、本実施形態に係る第1の制御部12Aの機能部について、説明する。

第1の制御部12Aは、目標角度算出部13、旋回モータ位置・速度制御部14、シリアルリンク駆動力算出部(第2の算出部)15、乗算器16,17、加算器18、旋回モータトルク・電流制御部19、トルク変換部20、及び乗算器21,22を備える。

目標角度算出部13は、クロスバー目標値取得部11からクロスバー目標位置(X_cb,Y_cb)を取得し、その取得したクロスバー目標位置(X_cb,Y_cb)に基づいて、リフト軸及び旋回アーム5のなす角度の目標角度θ_Cと、旋回アーム5及びメインアーム6のなす角の目標角度θ_Fとを、求める。なお、目標角度算出部13は、シリアルリンク機構の構造上、クロスバー目標位置(X_cb,Y_cb)が決定すれば一意に目標角度θ_C及び目標角度θ_Fを算出することが可能である。

旋回モータ位置・速度制御部14は、目標角度算出部13から目標角度θ_Cを取得する。そして、旋回モータ位置・速度制御部14は、目標角度θ_Cから得られる旋回モータ3の回転位置の目標値と、旋回モータ3の現在の回転位置との差から速度指令を、例えばフィードバック制御により求める。そして、旋回モータ位置・速度制御部14は、この速度指令を、旋回モータ3に対するトルク指令値に変換して、加算器18に出力する。

シリアルリンク駆動力算出部15は、搬送装置1のシリアルリンク機構のみを抽出して、自由関節にアクチュエータ等のメインアーム6を駆動する駆動部(以下、「仮想駆動部」という。)があると仮定する。この仮定において、まず、シリアルリンク駆動力算出部15は、リフト軸及び旋回アーム5のなす角度が目標角度θ_Cとなり、且つ、旋回アーム5及びメインアーム6のなす角が目標角度θ_Fとなるために必要な旋回アーム5の駆動トルクτ₁と仮想駆動部の駆動力τ₂とを、例えば、シリアルリンク機構の運動方程式に基づいて求める。 なお、駆動力τ₂は、本発明の「仮想駆動力」に相当する。

ただし、本実施形態では、自由関節に仮想駆動部が設けられていると仮定して駆動トルクτ₁も算出しているため、駆動力τ₁に駆動力τ₂の反力が含まれている。そのため、駆動力τ₂から駆動トルクτ₁を差し引いた値を、旋回アーム5を駆動するためのフィードフォワードトルクτ_C(旋回駆動力)として用いる。

シリアルリンク駆動力算出部15は、算出したフィードフォワードトルクτ_Cを乗算器16に出力する。また、シリアルリンク駆動力算出部15は、算出した駆動力τ₂をトルク変換部20に出力する。

乗算器16は、所定の減速比をフィードフォワードトルクτ_Cに乗算して乗算器17に出力する。 乗算器17は、所定の減速比が乗算されたフィードフォワードトルクτ_Cに対して、フィードバックゲインkを乗算することで、旋回モータ3のフィードフォワードトルク指令値を生成する。そして、乗算器17は、生成した旋回モータ3のフィードフォワードトルク指令値を加算器18に出力する。

加算器18は、トルク指令値Tに、旋回モータ3のフィードフォワードトルク指令値を加算することで、旋回モータ3の加算トルク指令を生成する。そして、加算器18は、生成した加算トルク指令を旋回モータトルク・電流制御部19に出力する。

旋回モータトルク・電流制御部19は、加算器18から出力された加算トルク指令に基づいて、旋回モータ3をトルク制御する。すなわち、旋回モータトルク・電流制御部19は、加算トルク指令に応じた電流指令値を演算し、その演算した電流指令値に旋回モータ3に流れる電流が一致するように旋回モータ3の駆動を制御する。

トルク変換部20は、シリアルリンク駆動力算出部15で算出した駆動力τ₂を、第1直動機構9bの駆動軸(以下、「スイング軸」という。)のトルクτ_2p(スイングトルク)と、第2直動機構9dの駆動軸(以下、「サポート軸」という。)のトルクτ_2q(サポートトルク)と、に変換する。これは、トルクτ_2p及びトルクτ_2qは、駆動力τ₂をパラレルリンク機構で補償させるためのモータトルクである。すなわち、駆動力τ₂に相当するモーメントを、摺動ブロック9fを介してスイングアーム9c及びサポートアーム9eの合力でメインアーム6に与えるために必要なパラレルリンク機構用のモータ(第1の駆動部)のモータトルクである。なお、このスイング軸のトルクτ_2pとは、スイングモータ9b2のトルクを示す。サポート軸のトルクτ_2qは、サポートモータ9d2のトルクを示す。

乗算器21は、トルクτ_2pに対してフィードフォワードゲインλを乗算してスイングモータ9b2の第1のフィードフォワードトルク指令値T1spを生成する。

乗算器22は、トルクτ_2qに対してフィードフォワードゲインλを乗算してサポートモータ9d2の第1のフィードフォワードトルク指令値T1sqを生成する。

第2の制御部12Bは、摺動ブロック目標位置算出部23、スライド目標値算出部24、スイングモータ位置・速度制御部25、サポートモータ位置・速度制御部26、パラレルリンク駆動力算出部(第1の算出部)27、乗算器28〜31、加算器32,33、スイングモータトルク・電流制御部34、及びサポートモータトルク・電流制御部35を備える。

なお、スイングモータ位置・速度制御部25、乗算器28、乗算器29、加算器32、及びスイングモータトルク・電流制御部34は、本発明の「スイングモータ制御部」を構成する。また、サポートモータ位置・速度制御部26、乗算器30、乗算器31、加算器33、及びサポートモータトルク・電流制御部35は、本発明の「サポートモータ制御部」を構成する。

摺動ブロック目標位置算出部23は、クロスバー目標値取得部11からクロスバー目標位置(X_cb,Y_cb)を取得して摺動ブロック9fの目標位置(X_p,Y_p)を算出する。例えば、摺動ブロック目標位置算出部23は、X_cbに比例定数k_pb(0

pb<1)を乗算することでX

_pを算出する。また、摺動ブロック目標位置算出部23は、Y

_cb+(1−k

_pb)・X

_cb・(1/tanΦ

_F)=Y

_pを演算することでY

_pを算出する。なお、Φ

_Fは、リフト軸及びメインアーム6のなす角度である。

スライド目標値算出部24は、摺動ブロック目標位置算出部23で算出された(X_p,Y_p)に基づいて、スイングアーム9cの上端部(以下、「スイングスライドブロック」という。)及びサポートアーム9eの上端部(以下、「サポートスライドブロック」という。)の目標位置(p,q)を演算する。なお、以下、この目標位置(p,q)をスライド目標位置という。

スライド目標値算出部24は、算出したスイングスライドブロックの目標位置pをスイングモータ位置・速度制御部25に出力する。また、スライド目標値算出部24は、算出したサポートスライドブロックの目標位置qをサポートモータ位置・速度制御部26に出力する。

スイングモータ位置・速度制御部25は、目標位置pから得られるスイングモータ9b2の回転位置の目標値と、スイングモータ9b2の現在の回転位置との差から速度指令を例えば、フィードバック制御により求める。そして、スイングモータ位置・速度制御部25は、この速度指令を、スイングモータ9b2に対するトルク指令値Tpに変換して、加算器32に出力する。

サポートモータ位置・速度制御部26は、目標位置qから得られるサポートモータ9d2の回転位置の目標値と、サポートモータ9d2の現在の回転位置との差から速度指令を例えば、フィードバック制御により求める。そして、サポートモータ位置・速度制御部26は、この速度指令を、サポートモータ9d2に対するトルク指令値Tqに変換して、加算器33に出力する。

パラレルリンク駆動力算出部27は、摺動ブロック9fを目標位置(X_p,Y_p)で動かすために必要なスイングアーム9cの駆動力fp及びサポートアーム9eの駆動力fqを求める。例えば、パラレルリンク駆動力算出部27は、スイング軸における運動方程式を解くことで、スイングアーム9cの駆動力fpを求める。また、パラレルリンク駆動力算出部27は、サポート軸における運動方程式を解くことで、サポートアーム9eの駆動力fqを求める。なお、駆動力fp及び駆動力fqは、本発明の「スライド駆動力」に相当する。

乗算器28は、パラレルリンク駆動力算出部27で算出された駆動力fpにトルク変換ゲインを乗算することで、駆動力fpに相当するスイングモータ9b2のトルクを算出する。

乗算器29は、乗算器28で算出された、駆動力fpに相当するスイングモータ9b2のトルクにフィードフォワードゲインγを乗算して、スイングモータ9b2の第2のフィードフォワードトルク指令値T2spを生成する。そして、乗算器29は、生成した第2のフィードフォワードトルク指令値T2spを、加算器32に出力する。

乗算器30は、パラレルリンク駆動力算出部27で算出された駆動力fqにトルク変換ゲインを乗算することで、駆動力fqに相当するサポートモータ9d2のトルクを算出する。

乗算器31は、乗算器30で算出された、駆動力fqに相当するサポートモータ9d2のトルクにフィードフォワードゲインγを乗算して、サポートモータ9d2の第2のフィードフォワードトルク指令値T2sqを生成する。そして、乗算器31は、生成した第2のフィードフォワードトルク指令値T2sqを、加算器33に出力する。

加算器32は、スイングモータ位置・速度制御部25からトルク指令Tpを取得し、このトルク指令Tpに、乗算器21から出力された第1のフィードフォワードトルク指令値T1spと、乗算器29から出力された第2のフィードフォワードトルク指令値T2spとを加算することで、スイングモータ9b2の加算トルク指令を生成する。そして、加算器32は、生成した加算トルク指令をスイングモータトルク・電流制御部34に出力する。

加算器33は、サポートモータ位置・速度制御部26からトルク指令Tqを取得し、このトルク指令Tqに、乗算器22から出力された第1のフィードフォワードトルク指令値T1sqと、乗算器31から出力された第2のフィードフォワードトルク指令値T2sqとを加算することで、サポートモータ9d2の加算トルク指令を生成する。そして、加算器33は、生成した加算トルク指令をサポートモータトルク・電流制御部35に出力する。

スイングモータトルク・電流制御部34は、加算器32から出力された加算トルク指令に基づいて、スイングモータ9b2をトルク制御する。すなわち、スイングモータトルク・電流制御部34は、加算トルク指令に応じた電流指令値を演算し、その演算した電流指令値にスイングモータ9b2に流れる電流が一致するようにスイングモータ9b2の駆動を制御する。

サポートモータトルク・電流制御部35は、加算器33から出力された加算トルク指令に基づいて、サポートモータ9d2をトルク制御する。すなわち、サポートモータトルク・電流制御部35は、加算トルク指令に応じた電流指令値を演算し、その演算した電流指令値にサポートモータ9d2に流れる電流が一致するようにサポートモータ9d2の駆動を制御する。

このように、制御装置10は、摺動ブロック9fを目標位置(Xp,Yp)で動かすために必要なスイングアーム9cの駆動力fp及びサポートアーム9eの駆動力fqを求める。さらに、制御装置10は、シリアルリンク機構のみを抽出して自由関節に仮想駆動部があると仮定した場合にその仮想駆動部でメインアーム6を駆動するために必要な駆動力τ₂を算出し、この駆動力τ2をスイングアーム及びサポートアームの合力として補償させる。例えば、制御装置10は、駆動力τ₂を、摺動ブロック9fを介してメインアーム6に作用させる作用力(パラレルリンク機構から摺動ブロック9fへの作用力)に変換し、その作用力相当のモータトルクをパラレルリンク駆動用モータ(スイングモータ9b2及びサポートモータ9d2)のフィードフォワード指令値として当該各パラレルリンク駆動用モータをフィードフォワード制御する。これにより、シリアルリンク姿勢角の変化による支持部の位置精度の悪化を軽減することができる。

(駆動力fp及び駆動力fqの算出方法) 以下に、本実施形態に係るスイングアーム9cの駆動力fp及びサポートアーム9eの駆動力fqの算出方法を、図6を用いて説明する。図6は、本実施形態に係る駆動力fp及び駆動力fqの算出方法を説明する図であって、パラレルリンク機構の模式図である。

図6において、スイングアーム9c及びサポートアーム9eのそれぞれの長さをL、第1直動機構9bの駆動軸をスイング軸、第2直動機構9dのサポート軸とする。また、スイングアーム9cとスイング軸との接点(スイングスライドブロックの位置)をp、サポートアーム9eとサポート軸との接点(サポートスライドブロックの位置)をqとする。また、スイング軸とサポート軸の交点(スイング軸,サポート軸の原点)を(X₀,Y₀)、フィード軸とスイング軸とのなす角をα、フィード軸とサポート軸とのなす角をβとする。

まず、制御装置10は、摺動ブロック9fの目標位置(Xp,Yp)に基づいて、スイングスライドブロック及びサポートスライドブロックの位置、すなわちスライド目標位置(p,q)を演算する。

ここで、目標位置(X_p,Y_p)とスライド目標位置(p,q)との間には、以下に示す関係式が成立する。

なお、搬送装置1の構造からβ=π−αであるので、この式を式(A1)〜式(A4)に代入すると、以下の式を得る。

なお、以下の式(A9)〜(A52)において、記述の簡略化のため、目標位置(Xp,Yp)を単に目標位置(X,Y)と記述する。さらに、スイングアーム9cの駆動力fp及びサポートアーム9eの駆動力fqの計算式を簡単にするため、摺動ブロック9f以外の慣性を無視する。つまり、スイングアーム9c、サポートアーム9e、スイングスライドブロック、及びサポートスライドブロックの質量や慣性モーメントをゼロと仮定する。

この場合において、スイングアーム9c及びサポートアーム9eの運動エネルギーI及びポテンシャルエネルギーUは、式(A9)及び式(A10)となる。

摺動ブロック9fを駆動するのに必要なスイングアーム9cの駆動力fp及びサポートアーム9eの駆動力fqは、ラグランジュの運動方程式から式(A11)及び式(A12)で表される。

したがって、上記式(A9)及び式(A10)を式(A11)及び式(A12)に代入して整理すると、以下に示す式を得る。

式(A5)及び式(A6)式より、X及びYは、(p,q)=(0,0)の点を除いて全微分可能である。よって、全微分可能な領域では、以下に示す関係式が成立する。

したがって、上記式(A15)及び式(A16)より、以下に示す式が導出される。

上記式(A17)〜式(A24)を式(A13)及び(A14)式に代入すると、以下に示す運動方程式を得る。

上記の運動方程式である式(A25)及び式(A26)の左辺を計算することで、リンク先端部を駆動するために必要な駆動力fp及び駆動力fqが求まる。

なお、本実施形態では、説明の便宜上、重力補償を省略するため、重力補償分の力を除外して説明する。ただし、重力補償分の力を求めるには、搬送装置1が停止している状態(Xの1階微分値,Yの1階微分値,pの1階微分値,qの1階微分値)=(0,0,0,0)とすると、重力補償分の力として下記示す式(A25a)及び式(A26a)が得られる。

式(A25)、式(A26)式から重力補償分である式(A25a)、式(A26a)を差し引くと、重力補償分以外のトルクを示す式(A25b)、式(A26b)を得る。

ここで、式(A25b)及び式(A26b)の値を計算するためには、下記に示す関数値F₁〜F₁₀が必要となる。

これら関数値F₁〜F₁₀を求めるための準備として、式(A5)及び式(A6)を、以下に示す式(A27)及び式(A28)のように再定義する。

ここで、上記式(A27)及び式(A28)を偏微分することにより以下に示す関数値の式が求まる。

式(A33)〜式(A42)式の関数値を求めるためには、以下に示す関数値F₁₁〜F₂₀が必要となる。

したがって、上記式(A29)及び式(A20)を偏微分することにより、以下に示す関数値F₁₁〜F₂₀を求める式が求まる。

以上の計算式から、制御装置10は、摺動ブロック9fを動かすために必要なスイングアーム9cの駆動力fp及びサポートアーム9eの駆動力fqを求めることができる。例えば、制御装置10は、以下に示すステップS101からS105を順に実行することで駆動力fp及び駆動力fqを算出する。図7は、駆動力fp及び駆動力fqを算出する流れを示す図である。

より具体的には、ステップS101として、制御装置10は、摺動ブロック9fの目標位置(X,Y)を式(A7)及び式(A8)に代入してスライド目標位置(p,q)を算出する。次に、制御装置10は、ステップS102として、ステップS101で算出したスライド目標位置(p,q)を式(A29)〜(32)に代入して、(R,sinθ,cosθ)を算出する。

制御装置10は、ステップS103として、ステップS101及びステップS10で算出した(p,q,R)を、式(A43)〜式(A52)に代入してF₁₁〜F₂₀を算出する。そして、制御装置10は、ステップS104として、ステップS103で算出したF₁₁〜F₂₀を式(A33)〜式(A42)に代入してF₁〜F₁₀を算出する。 制御装置10は、ステップS104でF₁〜F₁₀を算出すると、ステップS105として、F₁〜F₁₀と、(X,Y,p,q)の微分値と、を式(A25b)及び式(A26b)に代入することで駆動力fp及び駆動力fqを算出する。

(駆動力τ₂の算出方法) 次に、本実施形態に係る駆動力τ₂の算出方法を、図8を参照して説明する。図8は、本実施形態に係る駆動力τ₂の算出方法を説明する図であって、シリアルリンク機構の模式図である。

図8において、旋回アーム5の長さをL_C、メインアーム6の長さをL_Fとする。また、リフト軸と旋回アーム5とのなす角をθ₁=θ_C=ψ_Cとする。さらに、旋回アーム5とメインアーム6とのなす角をθ_Fとする。

まず、図8に示す2リンクアームのシリアルリンク機構の運動エネルギーを示す式(B1)及び位置エネルギーを示す式(B2)からラグランジュの運動方程式は、以下に示す式(B3)及び式(B4)で表される。

ここで、旋回アーム5の重心位置を(X_1G,Y_1G),メインアーム6の重心位置を(X_2G,Y_2G)とすると、各重心位置は、以下の式で表される。

なお、(X_C,Y_C)は、旋回アーム5の回転位置であって、旋回アーム5の一端部の位置である。また、a₁は旋回アーム5の根本(回転軸)から重心位置までの距離であり、a₂は、メインアーム6の根本(回転軸)から重心までの距離である。

ここで、θ₁及びθ₂は、以下に示す式で表される。

さらに、仮想仕事の原理より、以下の関係式が成立する。

したがって、上記式(B11)に示す一般化座標のトルクuのベクトルと、上記式(B12)に示す実際のアクチュエータトルクτのベクトルとの関係は、以下の式で表される。

さらに、旋回アーム5の重心位置(X_1G,Y_1G),と、メインアーム6の重心位置(X_2G,Y_2G)とのそれぞれの微分値(速度)は、以下の式で表される。

そして、式(B16)及び式(B17)から下記の式(B18)を求め、式(14)及び式B(B15)から下記の式(B19)を求める。

上記式(18)及び式(19)を式(B1)及び式(B2)に代入すると、以下の式が求まる。

これにより、上記式(B20)及び式(B21)から下記の式(B22)〜式(B25)が導出される。

したがって、式(B22)〜式(B25)及び式(B13)を、式(B3)式に代入して下記の運動方程式を得る。

同様に、上記式(B20)及び式(B21)から下記の式(B27)〜式(B30)が導出される。

上記式(B27)〜式(B30)及び式(B13)を、式(B4)に代入すると、以下の運動方程式が求まる。

ここで、φ_C及びφ_F及び、φ_C及びφ_Fの各微分値(1階微分値、2階微分値)は、下記に示す式で表される。

したがって、式(B31)は、下記の式で表される。

また、式(B26)は、下記の式で表される。

ここで、メインアーム6の自由間接にアクチュエータが設けられているという仮定をしたため、旋回アーム5の駆動力τ₁に、メインアーム6の駆動力の反力成分であるτ₂が含まれている。ただし、実際には、自由間接のためトルク反力は不要である。よって、旋回アーム5を駆動するためのフィードフォワードトルクτ_Cは、以下の式で表される。

なお、説明の便宜上、重力補償分を省略すると、上記式(B34)に示すフィードフォワードトルクτ_Cは、以下の式で表される。

同様に、式(B32)も重力項を除いて、以下の式で表される。

以上のように、上記式を用いることで、制御装置10は、フィードフォワードトルクτ_C及び駆動力τ₂を算出することができる。

(メインアーム6を駆動するためのスイング軸、サポート軸トルクの算出方法) 次に、本実施形態に係るメインアーム6を駆動するためのスイング軸、サポート軸トルクの算出方法を図9〜図12を用いて説明する。

まず、自由関節の位置座標(X_F,Y_F)を求める。例えば、図9において、自由関節の位置座標(X_F,Y_F)は、摺動ブロック9fの目標位置(X_P,Y_P)と旋回アーム5の角度θ_Cが与えられれば、以下の式で求まる。

また、自由関節から摺動ブロック9fまでの距離L_Pは、以下の式で表される。

ここで、水平軸とメインアーム6とのなす角Ψ_Fは、以下の式で表される。

ただし、θ_Cは、ゼロではないという条件である。θ_C=0の場合には、X_F=X_Pであり、tan⁻¹(x)のxが無限大になるので、その場合には、Ψ_F=(π/2)とする。

スイングスライドブロックの位置座標を(X_SP,Y_SP)、サポートスライドブロックの位置座標を(X_SQ,Y_SQ)とすると、各位置座標は、下記の式で表される。

次に、駆動力τ₂による摺動ブロック9fでのモーメント力を求める。ここで、式(B36)で示す駆動力τ₂はスカラ量である。そこで、対応する力のモーメントのベクトル成分は、作用点を(X_P,Y_P)とした場合において、以下に示す式で表される。なお、式(C8)式の駆動力τ₂は、右ねじの方向が正である。

次に、式(C9)で表されたモーメント力を、スイングアーム9c及びサポートアーム9eの各方向に分解する。図10は、式(C9)で表されたモーメント力を、スイングアーム9c及びサポートアーム9eの各方向に分解する様子を示す模式図である。

図10に示すように、スイングアーム9cとサポートアーム9eの成す角をθ_Vとすると、θ_Vは余弦定理より、以下の式で表される。

また、スイングアーム9cと水平軸の成す角Ψ_Pは、以下の式で表される。

さらに、サポートアーム9eと水平軸の成す角Ψ_Qは、以下の式で表される。

なお、図10より明らかなように、θ_V、θ_u、及びθ_wは以下に示す関係式が成り立つ。

ここで、スイングアーム9cの方向の力f_rpの絶対値を|f_rp|の上に→を付した文字で表記し、サポートアーム9eの方向の力f_rqの絶対値を|f_rq|の上に→を付した文字に表記すると、図10の合力と平行四辺形の関係、および正弦定理より、以下に示す関係式が成り立つ。

よって、式(C16)により、以下の式が成立する。

なお、図10は、スイングアーム9cの方向の力f_rpとサポートアーム9eの方向の力f_rqとの合力がスイングアーム9cとサポートアーム9eの成す角の外側に伸びる場合としたが、当該合力がスイングアーム9cとサポートアーム9eの内側に伸びる場合を、図11を用いて説明する。

図11のf_τ2、f_rp、及びf_rqの各ベクトルが成す平行四辺形の関係より、正弦定理から、以下に示す関係式が成立する。

よって、式(C19)により、以下の式が成立する。

上記式(C17)及び式(C18)と、式(C20)及び式(C21)とを比較すると、力ベクトルの絶対値の大きさが同じであることがわかる。

なお、各軸の力の絶対値は、式(C17)及び式(C18)で求めることができるが、力ベクトルの方向(上向き/下向き)は別途求める必要がある。

図10及び図11に示すように、f_τ2のベクトルがスイングアーム9cとサポートアーム9eの成す角の外側を向いている場合には、f_rp及びf_rqの各ベクトルは、符号が逆転する。したがって、f_τ2のベクトルが、スイングアーム9cとサポートアーム9eとの成す角の内側の場合は、f_rp及びf_rqの各ベクトルの符号は一致する。

ここで、f_rp及びf_rqのベクトルの方向と、f_τ2のベクトルの方向との関係を図12に示す。

図12に示すように、f_rp及びf_rqのベクトルの方向は、以下に示す(1)から(4)のルールで確認できる。 (1)ΨQ>Ψτ≧ΨPの場合の場合には、f_rp及びf_rqのベクトルは共に上向きベクトルである。 (2)ΨP>Ψτ≧0、または、2π>Ψτ≧ΨQ+πの場合には、f_rpのベクトルが上向きベクトルであり、f_rqのベクトルの向きが下向きベクトルである。 (3)ΨP+π>Ψτ≧ΨQの場合には、f_rpのベクトルが下向きベクトルであり、f_rqのベクトルが上向きベクトルである。 (4)ΨQ+π>Ψτ≧ΨP+πの場合には、f_rp及びf_rqのベクトルは共に下向きベクトルである。

ただし、上記ルールを適用するにあたっては、π≧Ψ_F≧0[rad]の条件を満たしていることが必要である。 ここで、Ψτは、f_τ2のベクトルと水平線との成す角度をであって、2π>Ψτ≧0である。さらに、Ψτ=Ψ_F+(π/2)、又はΨτ=Ψ_F+(3π/2)である。このΨτは、式(C8)から下記に示す式(C22)及び式(C23)を満たすように一意に算出され得る。

次に、式(C17)及び式(C18)で計算されるf_rp及びf_rqのベクトルの大きさをスイング軸、サポート軸に投影して、スイングモータ9b2及びサポートモータ9d2のフィードフォワードトルクを求める。図13は、図10の摺動ブロック9fで分解した力ベクトルをスイング軸、サポート軸に移動させた図である。

図13に示すように、力ベクトルをスイング軸及びサポート軸に投射したベクトル成分のうち、スイング軸に投射したベクトル成分|f_p|がスイングモータ9b2で発生させる必要がある力であり、サポート軸に投射したベクトル成分|f_q|がサポートモータ9d2で発生させる必要がある力である。この|f_p|及び|f_q|は、以下で示す式で表される。

なお、f_pのベクトルの方向は、f_τPの方向と同じである。また、f_qのベクトルの方向は、f_τqの方向と同じである。 ここで、式(C24)及び式(C25)は、力[N]であるため、トルクに変換する場合は、適切なリード長と減速比とを乗ずることが望ましい。リード長をL_Lead、減速比をN_Sとすると、スイングモータ9b2のトルク|τ_2p|と、サポートモータ9d2のトルク|τ_2q|とは、下記の式で与えられる。

以上、この発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計等も含まれる。

上述したように、本実施形態に係る搬送装置1は、第1の駆動部(スイングモータ9b2及びサポートモータ9d2)を制御して摺動ブロック(摺動部)9fを目標位置に摺動させる制御装置10を備える。そして、制御装置10は、シリアルリンク機構のみを抽出して自由関節に仮想駆動部(第2の駆動部)があると仮定した場合にその仮想駆動部でメインアーム6を駆動するために必要な駆動力τ₂(仮想駆動力)を算出し、駆動力τ₂をパラレルリンク機構で補償するように第1の駆動部を制御する。

これにより、パラレルリンク機構とシリアルリンク機構とを組み合わせた搬送装置1において、パラレルリンク機構を用いてシリアル機構の姿勢を精度よく制御できる。これにより、搬送装置1における支持部の精度を向上させることができる。

1 搬送装置 5 旋回アーム 6 メインアーム 9b2 スイングモータ(第1の駆動部) 9d2 サポートモータ(第1の駆動部) 9f 摺動ブロック(摺動部) 10 制御装置 12A 第1の制御部(制御部) 12B 第2の制御部(制御部) 15 シリアルリンク駆動力算出部(第2の算出部) 20 トルク変換部 27 パラレルリンク駆動力算出部(第1の算出部)