(57)【要約】【課題】高速動作時においてもアームの振動が発生しにくい工作ロボットの制御方法を提供することを目的とする。【解決手段】エンドエフェクタ１３を出発地点Ｄ１から目標位置Ｄ２まで動かす際に、エンドエフェクタ１３の移動方向が第２アーム１１ａ、１１ａ、１１ｂ、１１ｂ、１１ｃ、１１ｃの並列方向のいづれかと一致している場合、エンドエフェクタ１３の目標位置までの経路を工作ロボット１の中心位置側に迂回する移動経路Ｔｒとすることを特徴とする。【選択図】図９
2019111638.tif 000002
【特許請求の範囲】
【請求項１】
ベースと、前記ベースに取り付けられた複数のアクチュエータと、前記複数のアクチュエータに取り付けられ、複数の並列に配置されたアームの一端にエンドエフェクタが取り付けられたパラレルリンク機構を備えた工作ロボットの制御方法において、
前記複数のアクチュエータを制御する制御手段を備え、
前記制御手段は、前記エンドエフェクタを出発地点から目標地点まで連続的に動かす際に、前記エンドエフェクタの移動方向が前記アームの並列方向と一致している場合、前記エンドエフェクタの出発地点から目標地点までの動作中の経路を、前記エンドエフェクタの中立地点側に迂回する移動経路とすることを特徴とする工作ロボットの制御方法。
【請求項２】
前記制御手段は、前記エンドエフェクタの移動方向が前記アームの並列方向と一致している場合には、前記工作ロボットの中立地点と前記目標地点を結んだ直線上に仮想経由地点を設定し、前記仮想経由地点を経由して前記エンドエフェクタを前記出発地点から目標地点まで迂回して移動させることを特徴とする工作ロボットの制御方法。
【請求項３】
前記制御手段は、前記仮想経由地点の座標情報をメモリに格納しており、前記エンドエフェクタの移動方向が前記アームの並列方向と一致している場合には、前記仮想経由地点の座標情報を前記メモリより呼び出し、前記エンドエフェクタを前記仮想経由地点を経由して前記出発地点から前記目標地点まで迂回して移動させることを特徴とする請求項２に記載の工作ロボットの制御方法。
【請求項４】
前記制御手段は、前記仮想経由地点近傍では曲線経路を設定し、前記エンドエフェクタが前記仮想経由地点に近づいた際には前記曲線経路を移動することを特徴とする請求項３に記載の工作ロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、製品の工作ロボットに関し、特にパラレルリンク機構を備えた工作装置の動作の高速化を行う制御方法に関するものである。
【背景技術】
【０００２】
従来より、パラレルリンク機構を備えた工作装置が多数提案されてきた。例えば、特許文献１に示すような工作機械である。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開平１１−４８０９１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１に開示されたようなパラレルリンク機構を有する工作機械（工作ロボット）は、トラベリングプレート（エンドエフェクタ）が目標位置に移動して停止する際に、パラレルリンク機構のアームに振動が生じやすい。このため、アームが振動しないように工作ロボットの動作速度を遅くしたり、エンドエフェクタを目標位置の手前から減速させてアームの振動を抑制するように、パラレルリンク機構の駆動モータを制御する必要があった。このような制御は、工作ロボットでのタイムロスとなり、工作ロボットのリードタイムを向上する際の障害となっていた。
【０００５】
本発明は、このような従来技術に鑑みてなされたもので、その目的は、高速動作時においてもアームの振動が発生しにくいパラレルリンク機構を用いた工作ロボットの制御方法を提供することにある。
【課題を解決するための手段】
【０００６】
本発明における工作ロボットの制御方法では、ベースと、前記ベースに取り付けられた複数のアクチュエータと、前記複数のアクチュエータに取り付けられ、複数の並列に配置されたアームの一端にエンドエフェクタが取り付けられたパラレルリンク機構を備えた工作ロボットの制御方法において、前記複数のアクチュエータを制御する制御手段を備え、前記制御手段は、前記エンドエフェクタを出発地点から目標地点まで連続的に動かす際に、前記エンドエフェクタの移動方向が前記アームの並列方向と一致している場合、前記エンドエフェクタの出発地点から目標地点までの動作中の経路を、前記エンドエフェクタの中立地点側に迂回する移動経路とすることを特徴とする。
【０００７】
上記の構成によれば、エンドエフェクタの移動に伴うパラレルリンク機構部の振動が抑えられ、工作ロボットの動作速度を上げることができる。
【０００８】
また本発明の工作ロボットの制御方法では、前記制御手段は、前記エンドエフェクタの移動方向が前記アームの並列方向と一致している場合には、前記工作ロボットの中心位置と前記目標位置を結んだ直線上に仮想経由地点を設定し、前記仮想経由地点を経由して前記エンドエフェクタを前記出発地点から目標地点まで迂回して移動させることを特徴とする。
【０００９】
上記の構成によれば、パラレルリンク機構の振動を抑制できる最短・最速経路でエンドエフェクタを移動させることが可能となる。
【００１０】
また本発明の工作ロボットの制御方法では、前記制御手段は、前記仮想経由地点の座標情報をメモリに格納しており、前記エンドエフェクタの移動方向が前記アームの並列方向と一致している場合には、前記仮想経由地点の座標情報を前記メモリより呼び出し、前記エンドエフェクタを前記仮想経由地点を経由して前記出発地点から前記目標地点まで迂回して移動させることを特徴とする。
【００１１】
上記の構成によれば、エンドエフェクタの移動の都度に迂回経路を計算で求める必要がなくなるので、安価なシステム構成にできる。
【００１２】
また本発明の工作ロボットの制御方法では、前記制御手段は、前記仮想経由地点近傍では曲線経路を設定し、前記エンドエフェクタが前記仮想経由地点に近づいた際には前記曲線経路を移動することを特徴とする。
【００１３】
上記の構成によれば、前記仮想経由地点近傍でのエンドエフェクタの移動がスムーズになり、パラレルリンク機構部の移動時の振動が抑制できる。
【発明の効果】
【００１４】
本発明によれば、エンドエフェクタを移動させた場合に発生するアームの振動を効果的に抑制することができ、工作ロボットの高速動作が可能となる。
【図面の簡単な説明】
【００１５】
【図１】本発明の第１の実施形態における工作ロボットの全体側面図である。
【図２】図１に示す工作ロボットのパラレルリンク機構部を下方向より見た平面図である。
【図３】図１に示す工作ロボットのコントロールパネル部と駆動モータの関係を示すブロック図である。
【図４】図１に示す工作ロボットにおいて、エンドエフェクタの位置と変位量の関係を測定するためのテンションメータを配置した側面図である。
【図５】テンションメータを用いて測定するエンドエフェクタの各測定点を示す図である。
【図６】図５に示す各測定点におけるエンドエフェクタの変位量の測定結果を示す図である。
【図７】図１に示す工作ロボットにおいて、エンドエフェクタの移動経路の計算過程の一部を説明する図である。
【図８】図７に示す工作ロボットにおいて、エンドエフェクタの移動経路の計算過程の一部を説明する図である。
【図９】図７および図８で説明した移動経路の計算結果に基づいた実際のエンドエフェクタの移動経路を示す図である。
【図１０】図７に示すエンドエフェクタの移動経路の計算方法を説明するフローチャートである。
【発明を実施するための形態】
【００１６】
以下、図面を参照して、本発明における工作ロボットの制御方法の具体的な実施形態について説明をする。なお、各図面の寸法は適宜変更して示している。尚、各図における方向は、Ｘ１を左、Ｘ２を右、Ｙ１を前、Ｙ２を後、Ｚ１を上、Ｚ２を下とする。
【００１７】
図１および図２に示すように、本発明の第１の実施形態おける工作ロボット１は、立方体のフレーム２と、フレーム２の内側に配置された作業台３と、フレーム２の内側上面の中央部から下方に延びる円柱状のベース４と、ベース４の下端に取り付けられたパラレルリンク機構部５と、フレーム２の側面に配置されたコントロールパネル部６からなる。
【００１８】
パラレルリンク機構部５は、ベース４の下端に取り付けられた略六角形の台座７と、台座７上には、パラレルリンク機構部５のアクチュエータとなる３つの駆動モータ８ａ、８ｂ、８ｃが１２０度間隔で配置され固定されている。この駆動モータ８ａ、８ｂ、８ｃの回転軸８ａ１、８ｂ１、８ｃ１には、棒状の第１アーム９ａ、９ｂ、９ｃの一端がそれぞれ取り付けられている。そして、回転軸８ａ１が回転すると第１アーム９ａは回転軸８ａ１を中心として回転する。同様に、回転軸８ｂ１、８ｃ１が回転すると第１アーム９ｂ、９ｃは、回転軸８ｂ１、８ｃ１を中心としてそれぞれ回転する。第１アーム９ａ、９ｂ、９ｃの他端の両側には、それぞれ任意の方向に回転可能なボールジョイント部１０ａ、１０ａ、１０ｂ、１０ｂ、１０ｃ、１０ｃを介して、それぞれの第１アーム９ａ、９ｂ、９ｃに対応して一対の同じ長さの棒状の第２アーム１１ａ、１１ａ、１１ｂ、１１ｂ、１１ｃ、１１ｃが取り付けられている。なお、ボールジョイント部１０ａを介して第２アーム１１ａ、１１ａの並んでいる方向（並列方向）を一点差線の線Ｖ１、ボールジョイント部１０ｂを介して第２アーム１１ｂ、１１ｂの並んでいる方向（並列方向）を線Ｖ２、ボールジョイント部１０ｃを介して第２アーム１１ｃ、１１ｃの並んでいる方向（並列方向）を線Ｖ３で示す。なお、パラルリンク機構においてはエンドエフェクタ１３に接続される１対の第２アーム１１ａ、１１ａは、常に動作中において平行を保つが、本明細書における“並列方向”は、平行を保って動作する１対の第２アーム１１に対して定義されるものである。また１対の第２アーム１１は平行を保って動作するものの、動作中においてはその平行な方向の向きは変わるが、本明細書における“並列方向”は第１のアーム９との接続部において並んでいる方向を示す。すなわち第１アーム９の動作する面と直行する方向と同じ方向を示す。
【００１９】
第２アーム１１ａ、１１ａ、１１ｂ、１１ｂ、１１ｃ、１１ｃの下端には、第２のボールジョイント部１２ａ、１２ａ、１２ｂ、１２ｂ、１２ｃ、１２ｃを介して、略六角形のエンドエフェクタ１３が取り付けられている。このエンドエフェクタ１３の中央には工具取付孔１４が形成されており、この工具取付孔１４に各種工具（例えば、部品搬送用の真空パッドや、液剤塗布用のスプレーノズル等）が取り付けられる。
なお、駆動モータ８ａ、第１アーム９ａ、ボールジョイント部１０ａ、１０ａ、一対の棒状の第２アーム１１ａ、１１ａ、第２のボールジョイント部１２ａ、１２ａのブロックと、駆動モータ８ｂ、第１アーム９ｂ、ボールジョイント部１０ｂ、１０ｂ、一対の棒状の第２アーム１１ｂ、１１ｂ、第２のボールジョイント部１２ｂ、１２ｂのブロックとは同じに構成され、また駆動モータ８ｃ、第１アーム９ｃ、ボールジョイント部１０ｃ、１０ｃ、一対の棒状の第２アーム１１ｃ、１１ｃ、第２のボールジョイント部１２ｃ、１２ｃのブロックも、同じように構成されている。
【００２０】
また図３に示すように、フレーム２の側面に配置されたコントロールパネル部６には、工作ロボット１全体を制御する制御部１５と、エンドエフェクタ１３を駆動する際に参照する軌道補正情報が格納されたメモリ部１６と、オペレータが操作するスイッチやキーボードからなる入力部１７を備えている。また、前述の駆動モータ８ａ、８ｂ、８ｃには、駆動状況（回転量・速度）を計測するセンサ部１８ａ、１８ｂ、１８ｃがそれぞれ設けられている。そして制御部１５が、エンドエフェクタ１３の現在位置から次の位置への移動経路を計算し、駆動モータ８ａ、８ｂ、８ｃの動作を適宜制御することにより、エンドエフェクタ１３をテーブル３上のＸ、Ｙ、Ｘ空間内で任意の座標位置・姿勢で駆動する。
【００２１】
次にパラレルリンク機構の動作について説明する。エンドエフェクタ１３を所定の位置とする際には、前述のように制御部１５が３つの駆動モータ８ａ、８ｂ、８ｃをそれぞれ動作させるだけでよい。駆動モータ８ａ、８ｂ、８ｃを動作させ回転軸８ａ１、８ｂ１、８ｃ１をそれぞれ所定角度回転させると、第１アーム９ａ、９ｂ、９ｃはそれぞれ、回転軸８ａ１、８ｂ１、８ｃ１の回りを所定角度回転する。すると、これらの回転した角度に応じて第２アーム１１ａ、１１ａ、１１ｂ、１１ｂ、１１ｃ、１１ｃは所定方向に、所定角度傾き、エンドエフェクタ１３は所定の位置に移動する事となる。なお、ボールジョイント部１０ａ、１０ａ、１０ｂ、１０ｂ、１０ｃ、１０ｃ及び第２のボールジョイント部１２ａ、１２ａ、１２ｂ、１２ｂ、１２ｃ、１２ｃによって１対の第２アーム１１ａ、１１ａ及びは、１対の第２アーム１１ｂ、１１ｂ及び１対第２アーム１１ｃ、１１ｃは、それぞれが平行な状態を保って傾き、エンドエフェクタ１３は面の向きを変えずに所定の位置に移動する。なお、これらの機構は周知のパラレルリンク機構の構成と同じであり、これ以上の説明は省略する。
【００２２】
次に、このようなパラレルリンク機構５において、エンドエフェクタ１３の位置と振動のし易さの関係を測定する方法について、図４および図５を用いて説明する。
【００２３】
エンドエフェクタ１３の振動は、その慣性力に起因して生ずるものであり、その振動のし易さは、エンドフェクタ１３の側面に一定の荷重をかけた場合のエンドエフェクタ１３の変位量と同義である。このため、図４に示すように、工作ロボット１のテーブル３にテンションメータ１９を配置し、同じくテーブル３上に配置したカメラ２０で、エンドエフェクタ１３のＸ−Ｙ平面での変位量を測定する。
【００２４】
また、エンドエフェクタ１３の変位量の測定に当たっては、図５に示すように、エンドエフェクタ１３の中立地点（中心位置Ｓ）を中心として第１の距離の円Ｌ１、第２の距離の円Ｌ２、第３の距離の円Ｌ３、第４の距離の円Ｌ４の各同心円上を、３０度間隔で区切って測定ポイントを設定する。第１の距離の円Ｌ１での測定ポイントは１２個の五角形点Ａ１〜Ａ１２で示しており、第２の距離の円Ｌ２での測定ポイントは１２個の円点Ｂ１〜Ｂ１２、第３の距離の円Ｌ３での測定ポイントは１２個の四角形点Ｃ１〜Ｃ１２、第４の距離の円Ｌ４での測定ポイントは１２個の三角形点Ｄ１〜Ｄ１２として示している。なお、円Ｌ４は、エンドエフェクタ１３の最大可動領域とほぼ同等であり、該最大可動領域は第１のアーム９ａ、９ｂ、９ｃの長さ、第２のアームの長さ１１ａ、１１ａ、１１ｂ、１１ｂ、１１ｃ、１１ｃ、ボールジョイント部１０ａ、１０ａ、１０ｂ、１０ｂ、１０ｃ、１０ｃ、第２のボールジョイント部１２ａ、１２ａ、１２ｂ、１２ｂ、１２ｃ、１２ｃの可動角度範囲等によって主に決まる。また、円Ｌ１の半径の２倍、３倍、４倍となるように、それぞれ円Ｌ２、円Ｌ３、円Ｌ４の半径は設定されている。なお、中立地点（中心位置Ｓ）はパラレルリンク機構５のＸＹ平面での中心位置であり、該状態においては、第１アーム９ａ、一対の棒状の第２アーム１１ａ、１１ａを、中心位置Ｓの回りに１２０度回転させると第１アーム９ｂ、一対の棒状の第２アーム１１ｂ、１１ｂと重なり、更に１２０度回転させると、第１アーム９ｃ、一対の棒状の第２アーム１１ｃ、１１ｃと重なる。
【００２５】
そして、各測定点に位置させたエンドエフェクタ１３と、作業台３上に配置したテンションメータ１９を接続し、エンドエフェクタ１３を一定の荷重（例えば２０Ｎ）で引っ張り、エンドエフェクタ１３の下面に配置したカメラ２０でエンドエフェクタ１３の変位量を計測する。この作業を、１つの測定点につき、Ｘ−Ｙ平面でエンドエフェクタ１３の周囲を３０°毎にズラして計１２回計測した。すなわち、１つの測定点につき、Ｘ−Ｙ平面上における３０°毎の１２の方向から、エンドエフェクタ１３に加重を加え、都度変位量を計測した。
【００２６】
図６は、各測定点での変位量の計測結果を示す図である。各測定点Ａ１〜Ａ１２、Ｂ１〜Ｂ１２、Ｃ１〜Ｃ１２、Ｄ１〜Ｄ１２を囲むように記載してある点線の円が、各測定点での変位量を示している。図６からわかるように、エンドエフェクタ１３が中心点Ｐから近い円Ｌ１、円Ｌ２上にある時は、各測定点の変位量を示す点線の円の直径は小さく、またほぼ真円となっている。つまり、エンドエフェクタ１３が第１の距離の円Ｌ１または第２の距離の円Ｌ２にある場合はパラレルリンク機構５の剛性が十分にあり、エンドエフェクタ１３を各測定点Ａ１〜Ａ１２、Ｂ１〜Ｂ１２に移動させるときには、パラレルリンク機構５は殆ど変位しないということがわかる。つまり、各測定点Ａ１〜Ａ１２、Ｂ１〜Ｂ１２を目標地点としてエンドエフェクタ１３を移動させた場合には、エンドエフェクタ１３は殆ど振動しない。
【００２７】
しかし、エンドエフェクタ１３が中心位置Ｓから離れた第３の距離の円Ｌ３や第４の距離の円Ｌ４に位置すると、各計測点Ｃ１〜Ｃ１２、Ｄ１〜Ｄ１２を囲む点線の円が大きくなる。これは、エンドエフェクタ５が中心位置Ｓより離れるにしたがって、パラレルリンク機構５が変位しやすくなっている（振動しやすくなっている）ことを示している。
【００２８】
特に、各測定点Ｄ２、Ｄ６、Ｄ１０の変位量を示す点線円Ｄ２ａ、Ｄ６ａ、Ｄ１０ａは、中心位置Ｓ方向を向く短径と、この短径と直交する長径の差が極めて大きい楕円形となっている。例えば点線円Ｄ２ａの長径の方向は、パラレルリンク機構５の第２アーム１１ｂ、１１ｂが並んでいる“Ｖ２方向”と平行な方向となっている。
【００２９】
つまり、エンドエフェクタ１３が測定点Ｄ２に位置するとき、パラレルリンク機構部５がＶ２方向に変位しやすく、振動しやすくいということがわかる。これは第１アーム９ｂと、第２アーム１１ｂ、１１ｂの成す角度がより鈍角となり直線に近づき、Ｖ２方向の加重に対して変形しやすく、また、その変形を第１アーム９ａ、第２アーム１１ａ、１１ａ及び第１アーム９ｃ、第２アーム１１ｃ、１１ｃで支えきれない為と考えられる。同様に、点線円Ｄ６ａの長径の方向は、パラレルリンク機構５の第２アーム１１ａ、１１ａが並んでいる“Ｖ１方向”と平行な方向であり、点線円Ｄ１０ａの長径の方向は、パラレルリンク機構５の第２アーム１１ｃ、１１ｃが並んでいる“Ｖ３方向”と平行な方向であり、エンドエフェクタ１３が測定点Ｄ６や測定点Ｄ１０に位置するとき、Ｖ１方向やＶ３方向への変位量が大きく、振動しやすいということがわかる。一方、第２アーム１１ａ、１１ａ、１１ｂ、１１ｂ、１１ｃ、１１ｃが並んでいる方向と直交する方向に移動したときは、エンドエフェクタ１３は殆ど振動しないということもわかる。なお、その詳細は省略するがエンドエフェクタ１３が高い位置（図４のＺ１方向）で変位量の計測を行うと変位量は小さくなる傾向となり、低い位置（図４のＺ２方向）で変位量の計測を行うと変位量は大きくなる傾向となったが、これは、第１アーム９ｂと、第２アーム１１ｂ、１１ｂの角度が鋭角となると剛性が高まり、直線に近く鈍角となると剛性が低くなる為と推測する。
【００３０】
そして、これらのエンドエフェクタ１３の各測定点における、前記３０°毎の１２の方向から取得した変位量データはメモリ部１６に格納しておく。
また、エンドエフェクタ１３に発生する変位量の閾値を設定して、メモリ部１６に格納しておく。
【００３１】
次に、本実施態様の工作ロボット１で、エンドエフェクタ１３を測定点Ｄ１から測定点Ｄ２に移動させる場合に所望の目標地点に移動、停止させる際の経路および動作について図３、図７乃至図１０を用いて説明する。
【００３２】
例えば、図７に示すようにエンドエフェクタ１３を出発地点である測定点Ｄ１から目標地点である測定点Ｄ２に移動・停止させる場合、メモリ部１６に格納されている測定点Ｄ１の座標データと測定点Ｄ２の座標データから、制御部１６がその最短距離となる直線経路Ｔｘを算出する（ステップＳ２）。そして、直線経路Ｔｘ上を移動して停止する際にエンドエフェクタ１３に発生する慣性力の大きさと方向、及びメモリ部１６に格納されている測定点Ｄ２における変位量データから、制御部１６が測定点Ｄ２で停止した場合のエンドエフェクタ１３の変位量（振動量）を計算する（ステップＳ３）。なお、エンドエフェクタ１３に発生する慣性力の大きさは、エンドエフェクタ１３及び取付けられる工具等の質量と、加速度から求める。
【００３３】
エンドエフェクタ１３が測定点Ｄ１から測定点Ｄ２に到達し停止したとき、エンドエフェクタ１３の移動に伴う慣性力の方向は、測定点Ｄ２での変位量が大きい方向（点線円Ｄ２ａの長径方向Ｖ２）に近い。このため、エンドエフェクタ１３に発生する慣性力のベクトルは測定点Ｄ２の長径方向が大きくなり、エンドエフェクタ１３が直線経路Ｔｘで測定点Ｄ２まで移動して停止した場合に、エンドエフェクタ１３が大きく変位する（振動する）計算結果となる。
【００３４】
次にステップＳ４にて、メモリ部１６に格納されている測定点Ｄ２に設定していた閾値と、ステップＳ３で制御部１６が計算した変位量の計算結果を比較する。このとき、先の計算結果が測定点Ｄ２に設定していた閾値を超えるため、新たな移動経路の設定フロー（ステップＳ５〜ステップＳ１０）に進む。
【００３５】
新たな経路の設定に当たっては、まず制御部１５が、エンドエフェクタ１３の目標地点である測定点Ｄ２に近く、且つ変位量が測定点Ｄ２より小さいエンドエフェクタの中立地点側の測定点Ｃ２を仮想経由地点として選択する（ステップＳ５）。
【００３６】
次に、図７に示すように、測定点Ｃ２の座標データをメモリ部１６から読み込み（ステップＳ６）、エンドエフェクタ１３の出発地点である測定点Ｄ１から測定点Ｃ２までの最短の経路である直線経路Ｔ１を算出する（ステップＳ７）。同様にして、測定点Ｃ２から目標地点の測定点Ｄ２までの最短経路である直線経路Ｔ２を算出する（ステップＳ８）。このとき直線経路Ｔ２の方向は、測定点Ｄ２の変位量を示す楕円形の短径方向に一致する。つまり、目標地点である測定点Ｄ２で最も変位しない（振動しない）方向から、エンドエフェクタ１３が移動してきて停止することになる。
【００３７】
次に、図８に示すように、エンドエフェクタ１３が直線経路Ｔ１から直線経路Ｔ２に軌道変更する場合に、エンドエフェクタ１３の動きを滑らかにするために、直線経路Ｔ１と直線経路Ｔ２を結ぶ半径Ｒの曲線経路Ｔ３を算出する（ステップＳ９）。
【００３８】
そして、図９に示すように、ステップＳ１０にて、直線経路Ｔ１、直線経路Ｔ２と曲線経路Ｔ３を合成して実効経路Ｔｒを設定し、ステップＳ１１にて実効経路Ｔｒ（エンドエフェクタの中立地点側に迂回する移動経路に相当）をエンドエフェクタ１３の移動経路として確定させる。
【００３９】
そして、制御部１５がパラレルリンク機構部５の各駆動モータ８ａ、８ｂ、８ｃを駆動・制御することにより、エンドエフェクタ１３が出発地点の測定点Ｄ１から実効経路Ｔｒの軌道を通って目標地点の測定点Ｄ２に到達して停止する。
【００４０】
この様に、エンドエフェクタ１３は、エンドエフェクタの中立地点側の変位量が小さい仮想経由地点を経由し、また目標位置で変位量が小さい方向から移動して停止することとなるため、エンドエフェクタ１３を目標地点である測定点Ｄ２に高速駆動させても、エンドエフェクタ１３の変位（振動）が殆ど起きず、工作ロボット１でのＬＴの短縮が可能となる。
【００４１】
なお、本実施例においては停止した位置での加工等を想定しているので、仮想経由地点で多少の振動などが発生したとしても、停止状態においては殆ど振動は停止しているので、その影響は少ない。
【００４２】
また、本実施例においては移動距離が増える分、目標地点までの到達時間は増える傾向にあるが、その対応として速度を増加させると振動が大きくなり振動停止までの時間が増えることから、速度及び振動が停止するまでの時間を考慮して、移動開始から振動停止までの時間が短くなるよう経路変更を行う閾値を設定している。よって工作ロボット１でのＬＴの短縮が可能となる。
【００４３】
また、エンドエフェクタ１３を測定点Ｄ１から目標地点である測定点Ｂ２や測定点Ｃ２に移動・停止させる場合は、メモリ部１６に格納されている測定点Ｄ１の座標データと測定点Ｂ２や測定点Ｃ２の座標データから、制御部１６がその最短距離となるそれぞれの直線経路を算出する（ステップＳ２）。そして、メモリ部１６に格納されている測定点Ｂ２や測定点Ｃ２における変位量データと、直線経路の移動時にエンドエフェクタ１３に発生する慣性力の方向から、測定点Ｂ２や測定点Ｃ２で停止した場合のエンドエフェクタ１３の変位量（振動量）を制御部１６が計算する（ステップＳ３）。
【００４４】
図６に示すように、測定点Ｂ２や測定点Ｃ２での変位量は測定点Ｄ２に比べて極めて小さく異方性も殆ど見られない。このため、制御部１６での変位量の計算結果は、先に規定していた閾値以下となり、図１０に示すフローチャートのステップＳ４からステップＳ１１に進み、測定点Ｄ１から測定点Ｂ２や測定点Ｃ２への直線経路がそのままエンドエフェクタ１３の移動経路として確定する。そして制御部１５がパラレルリンク機構部５の各駆動モータ８ａ、８ｂ、８ｃを駆動・制御することにより、エンドエフェクタ１３が測定点Ｄ１から直線経路を通って測定点Ｂ２または測定点Ｃ２に到達して停止することとなる。
【００４５】
この様に、エンドエフェクタ１３の座標位置毎での変位量データを測定して所定の閾値を設定し、エンドエフェクタ１３が出発地点から目標地点までを最短経路で移動したときの変位量が閾値よりも大きくなったときに、変位量が小さい仮想経由地点を設定して移動経路を演算・再設定することにより、高速稼動時でも殆ど振動することなく作業ＬＴが短い工作ロボット１が実現できる。
【００４６】
本発明は上記実施形態に限定されるものではなく、その他本発明の要旨を逸脱しない範囲での適宜変更が可能である。
【００４７】
例えば、第１の実施態様では中心位置Ｓから最も遠い第４の距離Ｌ４上にある測定点Ｄ１から測定点Ｄ２に移動する場合について述べたが、第１の距離Ｌ１〜第３の距離Ｌ３上の各測定点間の移動の場合においても同様に適用できる。
【００４８】
また、本実施例においてはエンドエフェクタ１３の停止位置はの変位量の測定点と一致する例を示したが、必ずしも一致していなくとも良い。その場合には、例えば停止位置に一番近い測定点の変位量を用いて計算しても良い。また、各測定店の実測値から座標に対する変位量の近似式を求めて保管しておき、計算によって求めるようにしても良い。
【００４９】
また、本実施例においては、ボールジョイント部１０、１２としたが任意の方向に回転可能な連結構造であればこれには限らない
【００５０】
さらに本実施例においては、それぞれの第１アーム９に対応して１対の第２アーム１１を設けるタイプのパラレルリンク機構の例で示したが、第２アーム１１に対応してそれぞれ第１アーム９と駆動モーター８を設けるタイプ、すなわち第１アーム９と駆動モータ８を合計６個設けるタイプのパラレルリンク機構に適用しても良い。また、中心位置Ｓの回りに１２０度毎に駆動モータ８と第１アーム９、第２アーム１１を設けたが９０度毎に４本設けるタイプのパラレルリンク機構に適用しても良い。
【符号の説明】
【００５１】
１  工作ロボット
２  フレーム
３  作業台３
４  ベース
５  パラレルリンク機構部
８ａ、８ｂ、８ｃ  駆動モータ（アクチュエータ）
９ａ、９ｂ、９ｃ  第１アーム
１１ａ、１１ｂ、１１ｃ  第２アーム
１３  エンドエフェクタ
１５  制御部（制御手段）
１６  メモリ部
Ｓ  中心位置（中立地点）
【図１】
2019111638.tif 000003
【図２】
2019111638.tif 000004
【図３】
2019111638.tif 000005
【図４】
2019111638.tif 000006
【図５】
2019111638.tif 000007
【図６】
2019111638.tif 000008
【図７】
2019111638.tif 000009
【図８】
2019111638.tif 000010
【図９】
2019111638.tif 000011
【図１０】
2019111638.tif 000012