JP6271288B2 - リンク作動装置の制御装置および制御方法 - Google Patents

Description

この発明は、産業機器等の精密で広範な作動範囲を必要とする機器に用いられるリンク作動装置の制御装置および制御方法に関する。

三次元空間における複雑な加工や処理等の作業を高速かつ精密に行うリンク作動装置として、例えば特許文献１〜３に、４節連鎖のリンク機構を３組以上有し、２自由度の動作を可能とした装置が開示されている。これらのリンク作動装置は、例えば図９に示すように、基端側のリンクハブ１４と、先端側のリンクハブ１５と、これらリンクハブ１４，１５を互いに連結する３組のリンク機構１１，１２，１３とでなる。各リンク機構１１，１２，１３は、基端側の端部リンク部材１１ａ，１２ａ，（１３ａ）、先端側の端部リンク部材１１ｂ，１２ｂ，１３ｂ、および中央リンク部材１１ｃ，１２ｃ，１３ｃで構成されている。なお、図９では、基端側の端部リンク部材１３ａは図示されていないが、区別のために符号を用いて説明する。また、前記基端側の端部リンク部材１１ａ，１２ａ，１３ａを、以下、アーム１１ａ，１２ａ，１３ａと称する。

この種のリンク作動装置は、３組以上のリンク機構４を２個以上のモータ等のアクチュエータ（図示せず）により駆動することで、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢（以下「リンクハブ先端姿勢」または単に「姿勢」と称す）を変更する。リンクハブ先端姿勢は、折れ角θと旋回角φにより決定される。折れ角θは、基端側のリンクハブ１４の中心軸ＱＡに対する先端側のリンクハブ１５の中心軸ＱＢの傾斜角度であり、旋回角φは、基端側のリンクハブ１４の中心軸ＱＡに対する先端側のリンクハブ１５の中心軸ＱＢの旋回角度である。

リンクハブ先端姿勢の制御は、具体的には、前記折れ角θおよび旋回角φから、各アーム１１ａ，１２ａ，１３ａの回転角（以下「アーム回転角」と称す）（β１_ｎ，β２_ｎ，β３_ｎ）を求め、アーム１１ａ，１２ａ，１３ａを駆動するアクチュエータに位置決めさせる。例えば、図１０（Ａ），（Ｂ）に示すある姿勢Ａ（θ_ａ，φ_ａ）と異なる姿勢Ｂ（θ_ｂ，φ_ｂ）について、各々の姿勢Ａ，Ｂに対応する各アーム回転角は、下記の折れ角θおよび旋回角φとアーム回転角βとの関係を示す式（１）によりＡ（β１_ａ，β２_ａ，β３_ａ）、Ｂ（β１_ｂ，β２_ｂ，β３_ｂ）として求められる。

cos （θ／２）sin βｎ−sin （θ／２）sin （φ＋δｎ）cos βｎ＋sin （γ／２）＝０ …式（１）
γは、アーム１１ａ，１２ａ，１３ａに回転自在に連結された中央リンク部材１１ｃ，１２ｃ，１３ｃの連結端軸と、先端側の端部リンク部材１１ｂ，１２ｂ，１３ｂに回転自在に連結された中央リンク部材１１ｃ，１２ｃ，１３ｃの連結端軸とが成す角度である。δｎ（δ１，δ２，δ３）（図示せず）は、基準となるアーム１１ａに対する各基端側の端部リンク部材１１ａ，１２ａ，１３ａの円周方向の離間角である。リンク機構１１，１２，１３の数が３組で、各リンク機構１１，１２，１３が円周方向に等配である場合、各アーム１１ａ，１２ａ，１３ａの離間角δ１，δ２，δ３はそれぞれ０°，１２０°，２４０°となる。

ここで、姿勢Ａから姿勢Ｂへの姿勢変更は、各アーム１１ａ，１２ａ，１３ａの回転角がβ１ａからβ１_ｂ、β２ａからβ２_ｂ、β３_ａからβ３_ｂへそれぞれ変化することで実行される。

また、特許文献４では、基端側のリンクハブ１４の中心軸ＱＡの延長上に設定された基端側のリンクハブ１４の中心軸ＱＡと垂直な直交座標から、最小自乗法による収束演算を用いてリンクハブの姿勢（折れ角θ、旋回角φ）を取得している。これにより、エンドエフェクタが作業する被作業面（直交座標平面）の任意の座標への位置決めが可能となる。

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

リンクハブ先端姿勢につき姿勢Ａ（θ_ａ、φ_ａ）から姿勢Ｂ（θ_ｂ、φ_ｂ）への姿勢変更量が大きい場合、姿勢Ａから姿勢Ｂまで単にポイントツーポイント（Point-to-Point）制御で同期して等速運動で制御すると、特に姿勢Ａと姿勢Ｂ間の経路の中間部付近において、ポイントツーポイント制御で決定されるアーム回転角βが、前記式（１）から定まるアーム回転角βと比べて大きく異なる位置に位置決め指令される。そのため、各アーム１１ａ，１２ａ，１３ａ間に干渉が生じ、リンク機構１１，１２，１３の駆動に過大なトルクが必要になる。

そこで、姿勢変更量が大きい場合にリンク作動装置の各部に過大な負荷をかけずに駆動するために、姿勢Ａから姿勢Ｂの間を複数の姿勢に分割し、分割された各区間をポイントツーポイント制御で姿勢変更する制御装置を先に提案した（特願２０１３−０６３９４８号）。例えば、旋回角φを１５°に固定した状態で折れ角θを−６０°から６０°に姿勢変更する際、Ａ，Ｂ間を移動するリンクハブ先端姿勢の経路が最短となるようにＡ，Ｂ間の折れ角θを分割する。

具体例を示すと、旋回角を１５°に固定した状態で、折れ角θを−６０°から６０°に姿勢変更する際、Ａ，Ｂ間を移動するリンクハブ先端姿勢の経路が最短となるように折れ角θを分割した場合、アーム回転角βと折れ角θの関係が、前記式（１）より、図１１の実線Ｍ１〜Ｍ３で表わされる。旋回角φを１５°に固定した状態での、各折れ角θ（−６０°，−４５°，−３０°，…）における各アーム回転角βが実線Ｍ１〜Ｍ３上にプロットされる。
一方、旋回角φを１５°に固定した状態で折れ角θを−６０°から６０°に一気にポイントツーポイント制御で姿勢変更する場合のアーム回転角βと折れ角θの関係は、図１１の点線Ｌ１〜Ｌ３で表わされる。

つまり、旋回角φを１５°に固定した状態で折れ角θを−６０°から６０°に姿勢変更する例において、ポイントツーポイント制御で姿勢変更する場合は、全てのアームにつきアーム回転角βの経路が直線（Ｌ１〜Ｌ３）になるのに対し、折れ角θを分割した場合のアーム回転角βは、曲線（Ｍ１〜Ｍ３）を通過する経路で各姿勢が決まる。ここでは、姿勢変更の一例を挙げたが、全ての姿勢変更において、折れ角θを分割した場合に各アーム回転角βが直線になるとは限らない。これは、姿勢Ａから姿勢Ｂの区間において、リンクハブ先端姿勢を等速で変化させるためには、各アーム回転角βを非線形に動作させる必要があることを示す。例えば、先端側のリンクハブ１５に設置されたエンドエフェクタの被作業面を球面とした場合において、姿勢Ａから姿勢Ｂまでを複数の区間に分割し、分割された各区間を連続的に変化させると、姿勢Ａが被作業面上に指す位置から姿勢Ｂが被作業面上に指す位置までの移動速度は一定にならない。

したがって、図９、図１０に示すような、４節連鎖のリンク機構を３組以上有するリンク作動装置を用い、レーザによる加工や、ディスペンサ、インクジェットによる塗布、または溶接等を行う場合、被作業面上でのエンドエフェクタの不等速動作により、加工ムラや塗装ムラ、および溶接の際の肉厚ムラ等が発生する。

この発明の目的は、上記課題を解消し、４節連鎖のリンク機構を３組以上有するリンク作動装置を用いてエンドエフェクタで作業を行うときに、エンドエフェクタが被作業面上を実質的に等速で移動しながら作業を行うことができるリンク作動装置の制御装置および制御方法を提供することである。

この発明のリンク作動装置の制御装置４は、次のリンク作動装置１を制御対象とする。このリンク作動装置１は、基端側のリンクハブ１４に対し先端側のリンクハブ１５を、３組以上のリンク機構１１〜１３を介して姿勢を変更可能に連結し、前記各リンク機構１１〜１３は、それぞれ前記基端側のリンクハブ１４および先端側のリンクハブ１５に一端が回転可能に連結された基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと、これら基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの他端に両端がそれぞれ回転可能に連結された中央リンク部材１１ｃ〜１３ｃとでなり、前記３組以上のリンク機構１１〜１３のうち２組以上のリンク機構に、前記基端側の端部リンク部材であるアーム１１ａ〜１３ａを回転させることにより、前記基端側のリンクハブ１４に対する前記先端側のリンクハブ１５の姿勢を任意に変更させるアクチュエータ３を設け、前記先端側のリンクハブ１５にエンドエフェクタ８を設置してなる。前記各リンク機構１１〜１３は、前記リンク作動装置１が如何なる姿勢をとっていても、このリンク機構１１〜１３を直線で表現した幾何学モデルが、前記中央リンク部材１１ｃ〜１３ｃの中央部に対する基端側部分と先端側部分とが対称を成す形状である。

前記制御装置４は、
前記エンドエフェクタ８が作業する被作業面６０上の前記エンドエフェクタ８が移動する始点から終点までの経路を通過点で複数の区間に分割し、前記通過点の位置を設定する分割区間設定手段４２と、
指定された目標移動速度Ｖおよび前記各区間の距離から定まる各区間内の移動の時間と、前記各区間における前記各アーム１１ａ，１２ａ、１３ａの回転角移動量とから、前記各区間における前記各アーム１１ａ，１２ａ，１３ａが等速回転する回転速度を計算するアーム回転速度計算手段４３と、
前記各区間内での前記各アーム１１ａ〜１３ａの回転速度を前記回転速度計算手段４３で計算された値として、前記各区間を加減速無しで連続して前記各アームを回転させるように前記各アクチュエータ３を位置決め制御する姿勢変更制御手段４１と、
を備える。

この構成によると、エンドエフェクタ８の始点から終点までの移動、つまり先端側のリンクハブ１５の始点姿勢Ａ（θ_a 、φ_a ）から終点姿勢Ｂ（θ_b 、φ_b ）への移動において、分割区間設定手段４２に区間を複数に分割して設定する際に、分割する区間はエンドエフェクタ８が作業する被作業面６０上での経路を基準に区間設定する。
アーム回転速度計算手段４３は、エンドエフェクタ８の被作業面６０上を移動する速度が始点から終点まで一定の前記目標移動速度となるように、各区間の移動ごとに各アーム１１ａ，１２ａ，１３ａの等速回転する回転速度（β１軸、β２軸、β３軸の移動速度）を計算する。この回転速度の計算は、指定された目標移動速度および前記各区間の距離から定まる各区間内の移動の時間と、前記各区間における前記各アーム１１ａ，１２ａ，１３ａの回転角移動量とから行う。例えば、回転速度＝（回転角移動量）÷（移動時間）とする。前記目標移動速度はオペレータが設定する速度である。
姿勢変更制御手段４１は、前記各区間内での前記各アーム１１ａ〜１３ａの回転速度を、前記の計算された回転速度に設定し、この設定された回転速度で、前記各区間内を加減速無しで連続して回転させるように前記各アクチュエータ３を位置決め制御する。

これにより、エンドエフェクタが作業する被作業平面（直交座標）上の複数点を実質的に等速で動作することが可能となる。すなわち、始点から終点への位置決め制御が、途中の被作業面６０上の任意の点を加減速無しで通過することになる。そのため、エンドエフェクタとして、例えばレーザ加工ヘッド、ディスペンサ、インクジェットノズル、溶接器具等をリンクに付加した際、レーザの加工ムラやディスペンサ、インクジェットによる塗布ムラ、溶接による溶接ムラをなくすことができる。

なお、区間の分割数は エンドエフェクタ８の被作業面６０上を移動する速度が実質的に等速となるように設定する。「実質的に等速」とは、換言すれば「近似的に等速」であり、始点付近での動作開始時と終点付近での動作終了時を除き、エンドエフェクタ８による作業の観点から等速であると見なせる程度に速度の変化が生じないことを言う。
分割区間設定手段４２における区間の設定は、指令入力手段４０から各通過点を入力することで設定しても、また分割区間設定手段４２が各通過点を計算により求めて設定するようにしても良い。
また、アーム１１ａ〜１３ａに着目すると、ある区間のアーム回転速度と、次の区間のアーム回転速度とでは差が生じ、区間の隣合う箇所でアーム回転の加減速が生じるが、被作業面６０でのエンドエフェクタ８の移動は、前記アーム１１ａ〜１３ａの加減速が無視できる程度の近似的な等速になる。

前記被作業面６０は、平面であっても、球面であっても、またその他の面であっても良い。前記被作業面６０が平面である場合は、前記アーム回転速度計算手段は、前記始点、終点、および各通過点のいずれかの点である各ポイントへの前記各アームの回転速度を計算するものであって、この回転速度を、前記被作業面６０を示す直交座標系の任意の２ポイント間の距離と前記目標移動速度から求められる時間と、前記２ポイント間の前記各アームの回転角移動量とから計算するようにしても良い。
これにより、被作業面６０が平面にある場合に、前記各区間における前記各アームの回転速度を的確に計算できる。

前記被作業面６０が球面である場合は、前記アーム回転速度計算手段は、前記始点、終点、および各通過点のいずれかの点である各ポイントへの前記各アームの回転速度を、球面三角法より得られる任意の２ポイント間の移動量と前記目標移動速度から求められる時間と、前記２ポイント間の前記各アームの回転角移動量とから、前記各区間における前記各アームの回転速度を計算するようにしても良い。
これにより、被作業面６０が球面である場合に、前記各アームの回転速度を正確にかつ簡単に計算できる。

この発明において、前記各区間における、前記各通過点の位置と、前記各区間における前記各アーム１１ａ，１２ａ，１３ａの回転速度につき、リンク作動装置１の実使用の前に計算した値を記憶する区間対応設定情報記憶手段４６を設け、前記姿勢変更制御手段４１は、実使用時は、前記区間対応設定情報記憶手段４６に記憶された前記各通過点の位置と、前記各アーム１１ａ，１２ａ，１３ａの回転速度を読み出して制御するようにしても良い。
リンク作動装置１を用いて同じ作業を繰り返して行う場合がある。そのような場合、各区間における前記各アーム１１ａ，１２ａ，１３ａの回転速度を予め区間対応設定情報記憶手段４６に記憶させておき、前記姿勢変更制御手段４１はその記憶内容を読み出して制御を行うようにすれば、区間の設定や回転速度の計算を毎回行う必要がなく、制御が簡単な装置で迅速に行える。一度だけの作業であっても、予め区間およびアーム回転速度を予め計算して設定しておくことが便利な場合がある。
なお、この構成の場合、分割区間設定手段４２およびアーム回転速度計算手段４３は、姿勢変更制御手段４１とは互いに別のコンピュータ等に設けられていても良い。また、上記の「実使用時」とは、実際にエンドエフェクタ８で加工や処理を行うときを言う。

この発明のリンク作動装置の制御方法は、この発明の制御装置と同じ構成のリンク作動装置１を制御する方法であり、次のように制御する。
すなわち、このリンク作動装置の制御方法は、
前記エンドエフェクタ８が作業する被作業面６０上の前記エンドエフェクタ８が移動する始点から終点までの経路を通過点で複数の区間に分割し、
指定された目標移動速度および前記各区間の距離から定まる各区間内の移動の時間と、前記各区間における前記各アーム１１ａ，１２ａ，１３ａの回転角移動量とから、前記各区間における前記各アーム１１ａ，１１ｂ１２ａ，１１ｃ１３ａの回転速度を計算し、
前記各区間内での前記各アームの回転速度を前記の計算された値として、前記各区間を加減速無しで連続して前記各アーム１１ａ，１２ａ，１３ａを回転させるように前記各アクチュエータ８を位置決め制御することで、
前記作業面６０上の分割された複数点間を始点から終点まで実質的に一定速度に動作させる方法である。
この制御方法によると、この発明の制御装置につき前述したと同様に、エンドエフェクタ８が被作業面６０上を実質的に等速で移動しながら作業を行うことができる。

この発明のリンク作動装置の制御装置は、基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記３組以上のリンク機構のうち２組以上のリンク機構に、前記基端側の端部リンク部材であるアームを回転させることにより、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータを設け、前記先端側のリンクハブにエンドエフェクタを設置したリンク作動装置、を制御する制御装置であって、
前記リンク作動装置の前記各リンク機構は、前記リンク作動装置が如何なる姿勢をとっていても、前記リンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、
前記制御装置は、前記エンドエフェクタが作業する被作業面上の前記エンドエフェクタが移動する始点から終点までの経路を通過点で複数の区間に分割し、前記通過点の位置を設定する分割区間設定手段と、指定された一定速度の目標移動速度および前記各区間の距離から定まる各区間内の移動の時間と、前記各区間における前記各アームの回転角移動量とから、前記各区間における前記各アームが等速回転する回転速度を計算するアーム回転速度計算手段と、前記各区間内での前記各アームの回転速度を前記回転速度計算手段で計算された値として、前記各区間を加減速無しで連続して前記各アームを回転させるように前記各アクチュエータを位置決め制御する姿勢変更制御手段とを備えるため、エンドエフェクタが作業する被作業平面上を実質的に等速で動作することが可能となり、エンドエフェクタとして、例えばレーザ加工ヘッド、ディスペンサ、インクジェットノズル、溶接器具等をリンクに付加した際、レーザの加工ムラやディスペンサ、インクジェットによる塗布ムラ、溶接による溶接ムラをなくすことができる。

この発明のリンク作動装置の制御方法は、基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記３組以上のリンク機構のうち２組以上のリンク機構に、前記基端側の端部リンク部材であるアームを回転させることにより、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータを設け、前記先端側のリンクハブにエンドエフェクタを設置したリンク作動装置、を制御する制御方法であって、
前記各リンク機構は、前記リンク作動装置が如何なる姿勢をとっていても、前記リンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、
前記制御方法は、前記エンドエフェクタが作業する被作業面上の前記エンドエフェクタが移動する始点から終点までの経路を通過点で複数の区間に分割し、指定された目標移動速度および前記各区間の距離から定まる各区間内の移動の時間と、前記各区間における前記各アームの回転角移動量とから、前記各区間における前記各アームの回転速度を計算し、前記各区間内での前記各アームの回転速度を前記の計算された値として、前記各区間を加減速無しで連続して前記各アームを回転させるように前記各アクチュエータを位置決め制御することで、前記作業面上の分割された複数点間を始点から終点まで実質的に一定速度に動作させるため、エンドエフェクタが作業する被作業平面上を実質的に等速で動作することが可能となり、エンドエフェクタとして、例えばレーザ加工ヘッド、ディスペンサ、インクジェットノズル、溶接器具等をリンクに付加した際、レーザの加工ムラやディスペンサ、インクジェットによる塗布ムラ、溶接による溶接ムラをなくすことができる。

この発明の第１の実施形態に係るリンク作動装置の制御装置のブロック図とその制御対象となるリンク作動装置の一部省略正面図とを組み合わせた説明図である。 同リンク作動装置の図１と異なる動作状態を示す一部省略正面図である。 同リンク作動装置のリンク作動装置本体の斜視図である。 同リンク作動装置のリンク機構の一つを直線で表現した幾何学的モデル図である。 同リンク作動装置の基端側のリンクハブ、基端側の端部リンク部材、および中央リンク部材の断面図である。 同リンク作動装置に搭載されたエンドエフェクトが作業する平面からなる被作業面上の移動経路の簡易化例を表す直交座標系の図である。 同リンク作動装置に搭載されたエンドエフェクトが作業する平面からなる被作業面上の移動経路の一般化例を表す直交座標系の図である。 同リンク作動装置に搭載されたエンドエフェクトが作業する球面からなる被作業面上の移動経路の例を表す曲座標系の図である。 従来の４節連鎖のリンク機構を３組以上設けたリンク作動装置の一例の斜視図である。 同リンク作動装置の姿勢変更動作の説明図である。 同リンク作動装置を姿勢変更する場合の折れ角と基端側の端部リンクの回転角との関係を示す図である。

＜第１の実施形態＞
この発明の第１の実施形態に係るリンク作動装置の制御装置および制御方法を図面と共に説明する。まず、制御対象となるリンク作動装置を、図１ないし図５と共に説明する。図１および図２に示すように、このリンク作動装置１は、リンク作動装置本体２と、このリンク作動装置本体２を作動させる複数の姿勢制御用のアクチュエータ３とを備え、これら姿勢制御用のアクチュエータ３が制御装置４により制御される。図の例では、リンク作動装置本体２が、その基端側で、支持部材５にスペーサ６を介して吊り下げ状態に設置されている。リンク作動装置本体２の先端側には、先端取付部材７を介してエンドエフェクタ８が搭載されている。アクチュエータ３は例えばサーボモータであり、位置検出器３５を有している。エンドエフェクタ８は、例えばレーザ加工ヘッド、ディスペンサ、インクジェットノズル、溶接トーチ等であり、テーブル装置５０（図２）上のワークＷの表面である作業面６０上で作業を行う。

リンク作動装置本体２は、背景技術の欄で説明した図９のリンク作動装置１と基本的に同じ構成である。このため一部が繰り返しになるが、各部の構造をより詳しく説明する。図３に示すように、リンク作動装置本体２は、３組のリンク機構１１，１２，１３（以下、「１１〜１３」と表記する）を具備する。なお、図１および図２では、リンク機構１１の形状につき１組のみを表示し、図１では他の２組のリンク機構１２，１３について、制御の説明のためにブロックで示している。これら３組のリンク機構１１〜１３のそれぞれは幾何学的に互いに同一形状をなす。すなわち、各リンク機構１１〜１３は、後述の各リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂ，１１ｃ〜１３ｃを直線で表現した幾何学モデルが、中央リンク部材１１ｃ〜１３ｃの中央部に対する基端側部分と先端側部分が対称を成す形状である。

各リンク機構１１，１２，１３は、基端側の端部リンク部材１１ａ，１２ａ，１３ａ（以下、「１１ａ〜１３ａ」と表記する）、中央リンク部材１１ｃ，１２ｃ，１３ｃ（以下、「１１ｃ〜１３ｃ」と表記する）、および先端側の端部リンク部材１１ｂ，１２ｂ，１３ｂ（以下、「１１ｂ〜１３ｂ」と表記する）で構成され、４つの回転対偶からなる４節連鎖のリンク機構をなす。基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂはＬ字状をなし、基端がそれぞれ基端側のリンクハブ１４および先端側のリンクハブ１５に回転自在に連結されている。中央リンク部材１１ｃ〜１３ｃは、両端に基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの先端がそれぞれ回転自在に連結されている。基端側のリンクハブ１４および先端側のリンクハブ１５は六角柱状で、外周面を構成する６つの側面１６のうちの１つ置きに離れた３つの側面１６に、基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂがそれぞれ回転自在に連結されている。なお、基端側の端部リンク部材１１ａ〜１３ａは、請求項で言う「アーム」であり、以下の説明において、「アーム１１ａ〜１３ａ」と称する場合がある。

リンク作動装置本体２は、２つ球面リンク機構を組み合わせた構成であって、リンクハブ１４，１５と端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの各回転対偶、および端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと中央リンク部材１１ｃ〜１３ｃの各回転対偶の中心軸が、基端側と先端側において、それぞれの球面リンク中心ＰＡ，ＰＢ（図１、図２）で交差している。また、基端側と先端側において、リンクハブ１４，１５と端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの各回転対偶とそれぞれの球面リンク中心ＰＡ，ＰＢからの距離も同じであり、端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと中央リンク部材１１ｃ〜１３ｃの各回転対偶とそれぞれの球面リンク中心ＰＡ，ＰＢからの距離も同じである。端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと中央リンク部材１１ｃ〜１３ｃとの回転対偶の中心軸は、ある交差角αをもっていてもよいし、平行であってもよい。

つまり、３組のリンク機構１１〜１３は、幾何学的に同一形状をなす。幾何学的に同一形状とは、各リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂ，１１ｃ〜１３ｃを直線で表現した幾何学モデル、すなわち各回転対偶と、これら回転対偶間を結ぶ直線とで表現したモデルが、中央リンク部材１１ｃ〜１３ｃの中央部に対する基端側部分と先端側部分が対称を成す形状であることを言う。図４は、一つのリンク機構１１を直線で表現した図である。

この実施形態のリンク機構１１〜１３は回転対称タイプで、基端側のリンクハブ１４および基端側の端部リンク部材１１ａ〜１３ａと、先端側のリンクハブ１５および先端側の端部リンク部材１１ｂ〜１３ｂとの位置関係が、中央リンク部材１１ｃ〜１３ｃの中心線Ｃに対して回転対称となる位置構成になっている。図１は、基端側のリンクハブ１４の中心軸ＱＡと先端側のリンクハブ１５の中心軸ＱＢとが同一線上にある状態を示し、図２は、基端側のリンクハブ１４の中心軸ＱＡに対して先端側のリンクハブ１５の中心軸ＱＢが所定の作動角をとった状態を示す。各リンク機構１１〜１３の姿勢が変化しても、基端側と先端側の球面リンク中心ＰＡ，ＰＢ間の距離Ｈは変化しない。

図５は、基端側のリンクハブ１４と基端側の端部リンク部材１１ａ〜１３ａの連結部を示す断面図である。基端側のリンクハブ１４の側面１６から軸部１８が突出し、この軸部１８に複列の軸受１７の内輪（図示せず）が外嵌し、基端側の端部リンク部材１１ａ〜１３ａの基端側のリンクハブ側の端部に軸受１７の外輪（図示せず）が内嵌している。つまり、内輪は基端側のリンクハブ１４に固定され、外輪が基端側の端部リンク部材１１ａ〜１３ａと共に回転する構造である。軸受１７は、例えば深溝玉軸受、アンギュラ玉軸受等の玉軸受であって、ナット１９による締付けでもって所定の予圧量を付与して固定されている。軸受１７としては、図示例のように玉軸受を複列で配列する以外に、ローラ軸受や滑り軸受を用いてもよい。先端側のリンクハブ１５と先端側の端部リンク部材１１ｂ〜１３ｂの連結部も、同様の構造である。

また、基端側の端部リンク部材１１ａ〜１３ａと中央リンク部材１１ｃ〜１３ｃの連結部も複列の軸受２０を介して互いに回転自在に連結されている。すなわち、基端側の端部リンク部材１１ａ〜１３ａに軸受２０の外輪（図示せず）が外嵌し、中央リンク部材１１ｃ〜１３ｃに設けた軸部２１に軸受２０の内輪（図示せず）が外嵌している。軸受２０は、例えば深溝玉軸受、アンギュラ玉軸受等の玉軸受であって、ナット２２による締付けでもって所定の予圧量を付与して固定されている。軸受２０としては、図示例のように玉軸受を複列で配列する以外に、ローラ軸受や滑り軸受を用いてもよい。先端側の端部リンク部材１１ｂ〜１３ｂと中央リンク部材１１ｃ〜１３ｃの連結部も、同様の構造である。

前記リンク機構１１〜１３において、端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの軸部１８の角度、長さ（球面リンク中心ＰＡ，ＰＢからの長さ）、および端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの幾何学的形状が基端側と先端側で等しく、また、中央リンク部材１１ｃ〜１３ｃについても基端側と先端側で形状が等しいとき、中央リンク部材１１ｃ〜１３ｃの対称面に対して中央リンク部材１１ｃ〜１３ｃと、入先端側のリンクハブ１４，１５と連結される端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂとの角度位置関係を基端側と先端側で同じにすれば、幾何学的対称性から基端側のリンクハブ１４および基端側の端部リンク部材１１ａ〜１３ａと、先端側のリンクハブ１５および先端側の端部リンク部材１１ｂ〜１３ｂとは同じに動き、基端側と先端側は同じ回転角になって等速で回転することになる。この等速回転するときの中央リンク部材１１ｃ〜１３ｃの対称面を等速二等分面という。

このため、基端側のリンクハブ１４および先端側のリンクハブ１５を共有する同じ幾何学形状のリンク機構１１〜１３を円周上に複数配置させることにより、複数のリンク機構１１〜１３が矛盾なく動ける位置として中央リンク部材１１ｃ〜１３ｃが等速二等分面上のみの動きに限定され、これにより基端側と先端側は任意の作動角をとっても等速回転が得られる。

各リンク機構１１〜１３における４つの回転対偶の回転部、つまり、基端側のリンクハブ１４と基端側の端部リンク部材１１ａ〜１３ａとの連結部分、先端側のリンクハブ１５と先端側の端部リンク部材１１ｂ〜１３ｂとの連結部分、および基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと中央リンク部材１１ｃ〜１３ｃとの２つの連結部分を軸受構造とすることにより、その連結部分での摩擦抵抗を抑えて回転抵抗の軽減を図ることができ、滑らかな動力伝達を確保できると共に耐久性を向上できる。

このリンク作動装置本体２の構成によれば、基端側のリンクハブ１４に対する先端側のリンクハブ１５の可動範囲を広くとれる。背景技術の欄で説明したように、例えば、基端側のリンクハブ１４の中心軸ＱＡと先端側のリンクハブ１５の中心軸ＱＢの折れ角θの最大値（最大折れ角）を約±９０°とすることができる。また、基端側のリンクハブ１４に対する先端側のリンクハブ１５の旋回角φを０°〜３６０°の範囲で設定できる。

図１および図２において、前記複数のアクチュエータ３は、支持部材５の上に円周方向に等配で設置されている。アクチュエータ３の個数は、リンク機構１１，１２，１３と同数の３個である。ただし、アクチュエータ３の個数が２個であっても、リンク作動装置本体２の動作を規定できる。この実施形態では、アクチュエータ３はモータからなり、その出力軸３ａにピニオン３０が設けられている。一方、前記端部側の端部リンク部材１１ａ〜１３ａにおける前記軸部１８との回転対偶部に連結部材３１（図５）が固定して取付けられ、この連結部材３１に、前記ピニオン３０と噛み合う扇形ギア３２が設けられている。扇形ギア３２の中心軸は、軸部１８（図５）の中心軸と一致する。前記ピニオン３０と扇形ギア３２とで減速機３３を構成する。

各アクチュエータ３を回転駆動すると、その回転が減速機構３３を介して軸部１８（図５）に伝達されて、基端側のリンクハブ１４に対する基端側の端部リンク部材であるアーム１１ａ〜１３ａの角度が変更される。それにより、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢が決まる。この姿勢は、折れ角θ（図３）および旋回角φ（図３）により規定される。アーム１１ａ〜１３ａの回転角β１，β２，β３は、回転角検出手段３５により検出された値と減速機構３３の減速比との値から見積もられる。

次に、図１と共に制御装置４について説明する。制御装置４は、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢を始点姿勢から終点姿勢へ変更すると共に、その際、被作業面６０上の経路を作業点が指令入力手段４０で設定された目標移動速度Ｖで移動して姿勢変更するように、各アクチュエータ３を制御する装置である。前記作業点は、例えば前記エンドエフェクタ８の先端部となる点である。
前記支点姿勢は、現在の姿勢であり、個々のアーム１１ａ〜１３ａの始点は、現在の各アーム１１ａ〜１３ａの位置である。この始点は、前回移動時の終点とされ、または各回転角検出手段３５により検出された値と減速機構３３の減速比との値から見積られる現在位置、または定められた基準位置等とされる。支点姿勢は、この制御装置４に対する外部の指令入力手段４０から与えるようにしても良い。
指令入力手段４０は、キーボードやマウス等のオペレータの操作により入力を行う機器であっても、またこの制御装置４に対する上位の制御装置や、その他の情報機器類から情報を得る通信手段や記憶媒体のドライブ手段であっても良い。

制御装置４は、コンピュータによる数値制御式のものであり、分割区間設定手段４２、アーム回転速度計算手段４３、区間対応設定情報記憶手段４６、および姿勢変更制御手段４１を備える。

分割区間設定手段４２は、エンドエフェクタ８が作業する被作業面６０上の前記エンドエフェクタ８が移動する始点から終点までの経路を複数または一つの通過点で複数の区間に分割し、前記通過点の位置を設定する手段である。被作業面６０上の前記始点から終点までの経路は、前記指令入力手段４０から直交座標で入力され、また前記各通過点も、分割区間設定手段４２では、直交座標で示される。ここで、区間の分割数は エンドエフェクタ８の被作業面６０上を移動する速度が実質的に等速となるよう設定する。前記経路を通過点で複数の区間に分割する処理は、指令入力手段４０からオペレータが入力するようにしても良く、また分割区間設定手段４２に分割機能を持たせて自動で分割するようにしても良い。オペレータが入力するように構成する場合、前記分割区間設定手段４２は、単に前記始点、終点、および通過点の座標を記憶する手段となる。

前記分割区間設定手段４２が各通過点を計算して区分を設定する構成とする場合、定められた規則により、前記始点から終点までの経路を、例えば一定の分割数で等分割し、アーム回転速度計算手段４３に回転速度の計算を行わせて、始点から終点までを実質的に等速となるように制御できるか否かを判定し、できない場合は分割数を変更して再度アーム回転速度計算手段４３に回転速度の計算を行わせる処理を繰り返し、始点から終点までを実質的に等速となるように制御できると判定する処理によって定めても良い。

アーム回転速度計算手段４３は、オペレータにより指令入力手段４０から入力して指定された目標移動速度、および前記各区間の距離から定まる各区間内の移動の時間と、前記各区間における前記各アーム１１ａ，１２ａ，１３ａの回転角移動量とから、前記各区間における前記各アーム１１ａ，１２ａ，１３ａが等速回転する回転速度を計算する手段である。

区間対応設定情報記憶手段４６は、各区間毎に、前記アーム回転速度計算手段４３で計算された各アーム１１ａ，１２ａ，１３ａの、前記各区間に対応する前記始点、通過点、または終点であるポイントの位置と、アーム回転速度を記憶する手段である。

姿勢変更制御手段４１は、前記各区間内での前記各アーム１１ａ〜１３ａの回転速度を前記回転速度計算手段４３で計算された値として、前記各区間を加減速無しで連続して回転させるように前記各アクチュエータ３を位置決め制御する手段である。姿勢変更制御手段４１は、指令読み出し部５１、同期制御部５２、および前記アクチュエータ３と同数の個別制御部５３からなる。指令読み出し部５１は、前記区間対応設定情報記憶手段４６に記憶された各アーム１１ａ，１２ａ，１３ａの各区間毎の、各ポイントの位置とアーム回転速度を読み出す手段である。指令読み出し部５１は、前記区間対応設定情報記憶手段４６から読み出す代わりに、アーム回転速度計算手段４３から各アーム１１ａ，１２ａ，１３ａの各区間毎の、各ポイントの位置とアーム回転速度を読み出すようにしても良い。

各個別制御部５３は、対応するアクチュエータ３を、与えられたポイントからからポイントへポイントツーポイントで位置制御する手段である。個別制御部５３は、具体的には、例えばアーム回転角をアクチュエータ移動量に変換する動作量変換部（図示せず）と動作指令生成部（図示せず）とサーボコントローラ部（図示せず）とからなり、動作指令生成部は前記台形速度制御の速度曲線に従ってパルス払い出し等によってサーボコントローラ部へ動作指令を与え、サーボコントローラ部は、その与えられた動作指令と前記回転角検出手段３５の検出値を用いてフィードバック制御を行う。
同期制御部５２は、各アーム１１ａ〜１３ａが、同時に始点から回転を始め、同時に終点で止まるように、各個別制御部５３に対して同期制御を行わせる手段である。

前記アーム回転速度計算手段４３で用いる具体的な回転速度の計算方法を説明する。まず簡単な例で説明する。
図６にエンドエフェクタ８が作業する平面の被作業面６０上の任意の３つのポイント（Ａ、Ｂ、Ｃ）を示す。前記各ポイント（Ａ、Ｂ、Ｃ）は直交座標で示された点である。ポイントＡとＢの間の距離をＬ_ａｂ、ポイントＢとＣの間の距離をＬ_ｂｃとすると、Ｌ_ａｂとＬ_ｂｃは次の式となる。

被作業平面６０上でのエンドエフェクタ８の速度（なぞる速度）をＶとすると、Ｌ_ａｂの距離を移動する時間Ｔ_ａｂと、Ｌ_ｂｃの距離を移動する時間Ｔ_ｂｃは以下の式となる。速度Ｖは前記目標移動速度である。加減速時間はゼロとして計算する。
Ｔ_ａｂ＝Ｌ_ａｂ／Ｖ
Ｔ_ｂｃ＝Ｌ_ｂｃ／Ｖ

ポイントＡ、Ｂ、Ｃにおけるリンクハブの姿勢（折れ角、旋回角）を姿勢Ａ（θ_ａ，φ_ａ）、姿勢Ｂ（θ_ｂ、φ_ｂ）、姿勢Ｃ（θ_ｃ、φ_ｃ）とすると、姿勢Ａ、Ｂ、Ｃに対応するアーム回転角は、リンクハブとアーム回転角との前記関係式（１）からそれぞれ回転角Ａ（β１_ａ，β２_ａ、β３_ａ）、回転角Ｂ（β１_ｂ、β２_ｂ、β３_ｂ）、回転角Ｃ（β１_ｃ、β２_ｃ、β３_ｃ）として関係が成り立つ。ポイントＡからＢへ移動する時のβ１軸、β２軸、β３軸の各回転角移動量をΔβ１_ａｂ、Δβ２_ａｂ、Δβ３_ａｂとすると、各軸の回転速度である移動速度Ｖ１_ａｂ、Ｖ２_ａｂ、Ｖ３_ａｂは以下の式で表せる。なお、前記β１軸、β２軸、β３軸は、それぞれ前記各アーム１１ａ〜１３ａの回転中心回りの回転について示す。

上記と同様に、ポイントＢからＣへ移動する時のβ１軸、β２軸、β３軸の各移動量をΔβ１_ｂｃ、Δβ２_ｂｃ、Δβ３_ｂｃとすると、各軸の移動速度Ｖ１_ｂｃ、Ｖ２_ｂｃ、Ｖ３_ｂｃは次の式となる。

アーム回転速度計算手段４３は、先端側のリンクハブ１５に付加したエンドエフェクタ８が平面の被作業面６０上の３つのポイントＡ→Ｂ→Ｃをなぞる動作の際、β１軸、β２軸、β３軸の各移動量と、上記式より求めた速度を各区間毎に設定し、各区間内は加減速なしで連続位置決め制御する。なお、「加減速なしで連続位置決め」とは、止まらずに連続して回転させる意味である。

図７に一般化して示す。同図に示すように、エンドエフェクタ８が平面の被作業面６０上の各ポイントを指定された目標移動速度Ｖでなぞる動作において、ｎポイント目の直交座標を（Ｘ_ｎ、Ｙ_ｎ）とし、ｎ−１ポイント目の直交座標を（Ｘ_ｎ−１、Ｙｎ−１）とし、ｎ−１ポイント目からｎポイント目へのβ１軸、β２軸、β３軸の各移動量をΔβ１、Δβ２、Δβ３とすると、Ｎポイント目に移動する際の各軸の移動速度Ｖ１_ｎ、Ｖ２_ｎ、Ｖ３_ｎは次の式で表せる。

アーム回転速度計算手段４３は、これらの式（２）〜（５）に従って、各区間の移動時間Ｔと、これら各区間における各アーム回転速度である各軸の移動速度Ｖ１_ｎ、Ｖ２_ｎ、Ｖ３_ｎを計算する。また、アーム回転速度計算手段４３は、上記の式（２）〜（５）による計算に先立って、このアーム回転速度計算手段４３が有する指令変換部４４により、前記被作業面６０上の直交座標で示された各ポイントの座標位置を、前記のリンクハブとアーム回転角との関係式（１）に従って各アーム１１ａ〜１１ｃの回転に変換し、上記各軸の移動量Δβ１、Δβ２、Δβ３を求める。

この実施形態の構成によると、このようにエンドエフェクタ８の始点から終点までの移動、つまり先端側のリンクハブ１５の始点姿勢Ａ（θ_a 、φ_a ）から終点姿勢Ｂ（θ_b 、φ_b ）への移動において、分割区間設定手段４２に区間を複数に分割して設定する際に、分割する区間はエンドエフェクタ８が作業する被作業面６０上での経路を基準に区間設定する。
アーム回転速度計算手段４３は、エンドエフェクタ８の作業面上を移動する速度が始点から終点まで一定の前記目標移動速度Vとなるように、各区間の移動ごとに各アーム１１ａ，１２ａ，１３ａの等速回転する回転速度（β１軸、β２軸、β３軸の移動速度）を計算する。この回転速度の計算は、指定された目標移動速度Vおよび前記各区間の距離から定まる各区間内の移動の時間と、前記各区間における前記各アーム１１ａ，１２ａ，１３ａの回転角移動量とから行う。
姿勢変更制御手段４１は、前記各区間内での前記各アーム１１ａ〜１３ａの回転速度を、前記の計算された回転速度に設定し、この設定された回転速度で、前記各区間内を加減速無しで連続して回転させるように前記各アクチュエータ３を位置決め制御する。

これにより、エンドエフェクタ８が作業する平面の被作業面（直交座標）上の複数点を実質的に等速で動作することが可能となる。すなわち、始点から終点への位置決め制御が、途中の被作業面上の任意の点を加減速無しで通過することになる。そのため、エンドエフェクタとして、例えばレーザ加工ヘッド、ディスペンサ、インクジェットノズル、溶接器具等をリンクに付加した際、レーザの加工ムラやディスペンサ、インクジェットによる塗布ムラ、溶接による溶接ムラをなくすことができる。

＜第２の実施形態＞
この発明の第２の実施形態を、図８を参照して説明する。この実施形態において、特に説明した事項の他は、第１の実施形態と同様である。この実施形態は、図１における分割区間設定手段４２、アーム回転速度計算速度計算手段４３、および区間対応設定情報記憶手段４６の機能につき、第１の実施形態とは一部を異ならせたものである。区間対応設定情報記憶手段４６は、記憶する値が第１の実施形態とは異なる。

第１の実施形態では、エンドエフェクタ８の被作業面６０が平面である場合の等速移動について示したが、これと同様に被作業面６０が球面である場合に、この球面状の被作業面６０上でのエンドエフェクタ８の先端の動作を等速にすることもできる。これは、先端側のリンクハブ１５の動作を等速にすることになる。

ここでは、先端側のリンクハブ１５の動作を等速にする時の各軸（β１軸、β２軸、β３軸）の移動速度について示す。図８に示すように、あるポイント（ｎ−1 ）とｎにおける先端側リンクハブ１５の姿勢（折れ角、旋回角）を姿勢Ａ（θ_ｎ−１、φ_ｎ−１）、姿勢Ｂ（θ_Ｎ、φ_ｎ）とする。姿勢Ａ、Ｂに対応するアーム回転角は、前記リンクハブとアーム回転角との関係式（１）より、それぞれ回転角Ａ（β１_ｎ−1 、β２_ｎ−1 、β３_ｎ−1 ）、回転角Ｂ（β１_ｎ、β２_ｎ、β３_ｎ）として関係が成り立つ。また、姿勢Ａ（θ_ｎ−1 、φ_ｎ−1 ）から姿勢B （θ_ｎ、φ_ｎ）へ移動したときの回転角移動量γ（rad ）は、球面三角法より以下の式で近似して表せる。

先端側リンクハブ１５の指定された目標移動速度をＶ（rad/s)とすると、移動量γ（rad)にかかる時間は次の式となる。
Ｔ＝γ／Ｖ
あるポイント（ｎ−１）からポイントｎへ移動するときの各軸の回転角の移動速度である回転速度Ｖ１_ｎ′、Ｖ２_ｎ′、Ｖ３_ｎ′は、次の式となる。

β１軸、β２軸、β３軸の各移動量と、上記式より求めた速度を各区画毎、つまり各ポイント毎に設定し、各ポイントを加減速なしで連続位置決め制御する。

図１と共に説明すると、図１の分割区間設定手段４２は、球面からなる被作業面６０上の前記エンドエフェクタ８が移動する始点から終点までの経路を通過点で複数の区間に分割する。すなわち、前記各ポイントを定める。前記アーム回転速度計算手段４３は、指定された目標移動速度Ｖおよび前記各区間の距離から定まる各区間内の移動の時間Ｔと、前記各区間における前記各アーム１１ａ，１２ａ，１３ａの回転角移動量γとから、前記各区間における前記各アーム１１ａ，１２ａ，１３ａが等速回転する回転角の移動速度である回転速度Ｖ１_ｎ′、Ｖ２_ｎ′、Ｖ３_ｎ′を、上記の式（６）〜（８）によって計算する。前記姿勢変更制御手段４１は、前記ポイント間の各区間内での前記各アーム１１ａ，１２ａ，１３ａの回転速度Ｖ１_ｎ′、Ｖ２_ｎ′、Ｖ３_ｎ′を前記回転速度計算手段４３で計算された値として、前記各区間を加減速無しで連続して回転させるように前記各アクチュエータ３を位置決め制御する。

このように、被作業面６０が球面である場合も、エンドエフェクタ８が被作業面６０上を実質的に等速で移動しながら作業をすることができる。

＜第３の実施形態＞
この発明の第３の実施形態を説明する。この実施形態は、図２に想像線で示すように、テーブル装置５０の移動によって対象ワークＷがリンク作動装置１のエンドエフェクタ８に対して相対移動する場合に、その相対移動の速度を等速とする例である。テーブル装置５０は、テーブル面が直交２軸方向に移動するＸＹテーブルであり、同図の例では表面が平面の被作業面６０となる対象ワークＷを移動させるが、リンク作動装置１をテーブル装置５０に搭載し、ワークＷを位置固定としても良い。この実施形態において、特に説明する事項の他は、第１の実施形態と同様である。この実施形態は、図１における分割区間設定手段４２、アーム回転速度計算速度計算手段４３、および区間対応設定情報記憶手段４６の機能につき、第１の実施形態とは一部が異なっている。区間対応設定情報記憶手段４６は、記憶する値が第１の実施形態とは異なる。

第１の実施形態においては、リンク作動装置１と被作業面６０上の対象ワークWの位置は固定であったが、実際にレーザ加工や塗布、溶接等を行う場合、対象ワークWをＸＹテーブル（すなわち、テーブル可動面が直交２軸方向に移動可能なテーブル装置）上に搭載するか、またはリンク作動装置１をＸＹテーブルに搭載する構成が一般的となる。
ここでは、前記リンク作動装置１の３軸（β1 軸、β2 軸、β3 軸）と、ＸＹテーブルからなるテーブル装置５０（図２）の２軸（Ｘ軸、Ｙ軸）とが可動であるテーブル併用リンク作動装置のシステム構成において、エンドエフェクタ８が平面状の被作業平面６０上の各ポイントを指定された速度Ｖで等速移動する際の合成速度Ｖ_ｎ″について示す。

対象ワークWの直交座標は、図７と同様に、ｎポイント目の直交座標を（Ｘ_ｎ、Ｙ_ｎ）とし、ｎ−１ポイント目の直交座標を（Ｘ_ｎ−１、Ｙ_ｎ−１）とする。ｎ−１ポイント目からｎポイント目へのβ１軸、β２軸、β３軸、Ｘ軸、Ｙ軸の各移動量をΔβ１、Δβ２、Δβ３、Δｘ、Δｙとすると、ｎポイント目に移動する際の各軸の移動速度Ｖ１_ｎ″、Ｖ２_ｎ”、Ｖ３_ｎ”、Ｖ_ｘｎ”、Ｖ_ｙｎ”は次の式で表せる。β１軸、β２軸、β３軸の移動速度は、回転角の移動速度である。

アーム回転速度計算手段４３は、先端側リンクハブ１５に付加したエンドエフェクタ８が平面状の被作業面６０上の各ポイントをなぞる動作の際、β１軸、β２軸、β３軸、Ｘ軸、Ｙ軸の各移動量と、上記式より求めた速度を各区間毎、つまり各ポイント毎に設定し、各ポイントを加減速なしで連続位置決め制御する。

図１と共に説明すると、図１の分割区間設定手段４２は、ＸＹ方向に相対移動する平面からなる被作業面６０上の、前記エンドエフェクタ８がその相対移動する始点から終点までの経路を通過点で複数の区間に分割する。すなわち前記各ポイントを定める。前記アーム回転速度計算手段４３は、指定された目標移動速度Ｖおよび前記各区間の距離から定まる各区間内の移動の時間Ｔと、前記各区間における前記各アーム１１ａ，１２ａ，１３ａの回転角移動量Δβ１、Δβ２、Δβ３とから、前記各区間における前記各アーム１１ａ，１２ａ，１３ａが等速回転する回転角の移動速度である回転速度Ｖ１_ｎ″、Ｖ２_ｎ”、Ｖ３_ｎ”を、上記の式（９）〜（１１）（１４）によって計算する。アーム回転速度計算手段４３は、上記各軸の回転速度の他に、Ｘ軸、Ｙ軸の移動速度Ｖ_ｘｎ”、Ｖ_ｙｎ”を上記の式（１2）(１３) （１４）で計算する機能を付加した構成としても良い。

前記姿勢変更制御手段４１は、前記ポイント間の各区間内での前記各アーム８の回転速度Ｖ１_ｎ″、Ｖ２_ｎ”、Ｖ３_ｎ”を前記回転速度計算手段４３で計算された値として、前記各区間を加減速無しで連続して回転させるように前記各アクチュエータ３を位置決め制御する。Ｘ軸、Ｙ軸の制御手段（図示せず）は、上記のように計算したＸ軸、Ｙ軸の移動速度Ｖ_ｘｎ”、Ｖ_ｙｎ”を設定してその速度でテーブル装置５０の各軸を移動させる。

このように、リンク作動装置１がテーブル装置５０によって相対移動する場合に、エンドエフェクタ８がその相対移動を加味して被作業面６０上を実質的に等速で移動しながら作業をするように制御することができる。

なお、前記各実施形態では、いずれも、実使用時に制御装置４が各通過点の位置（各区間）の設定、および各区間におけるアーム回転速度の計算を行うようにしたが、これら各通過点の位置と、前記各区間における前記各アーム１１ａ，１２ａ，１３ａの回転速度につき、リンク作動装置１の実使用の前に計算した値を区間対応設定情報記憶手段４６に記憶させておき、前記姿勢変更制御手段４１は、実使用時は、前記区間対応設定情報記憶手段４６に記憶された前記各通過点の位置と、前記各アーム１１ａ，１２ａ，１３ａの回転速度を読み出して制御するようにしても良い。この場合に、各通過点の位置と、前記各区間における前記各アーム１１ａ，１２ａ，１３ａの回転速度を設定し、計算する装置は、姿勢変更制御手段４１を持つ制御装置４とは別のコンピュータに設けられていても良い。

リンク作動装置１を用いて同じ作業を繰り返して行う場合がある。そのような場合、各区間における前記各アーム１１ａ，１２ａ，１３ａの回転速度を予め区間対応設定情報記憶手段４６に記憶させておき、前記姿勢変更制御手段４１はその記憶内容を読み出して制御を行うようにすれば、区間の設定や回転速度の計算を毎回行う必要がなく、制御が簡単な装置で迅速に行える。一度だけの作業であっても、予め区間およびアーム回転速度を予め計算して設定しておくことが便利な場合がある。
なお、上記の「実使用時」とは、試し運転を除く実際にエンドエフェクタ８で加工や処理を行うときを言う。

１…リンク作動装置
３…アクチュエータ
４…制御装置
８…エンドエフェクタ
１４…基端側のリンクハブ
１５…先端側のリンクハブ
１１〜１３…リンク機構
１１ａ，１２ａ，１３ａ…基端側の端部リンク部材（アーム）
１１ｂ，１２ｂ，１３ｂ…先端側の端部リンク部材
１１ｃ，１２ｃ，１３ｃ…中央リンク部材
４０…指令入力手段
４１…姿勢変更制御手段
４２…分割区間設定手段
４３…アーム回転速度計算手段
４４…指令変換部
４６…区間対応設定情報記憶手段
５０…テーブル装置
６０…被作業面
６１…経路

Claims (7)

1. 基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記３組以上のリンク機構のうち２組以上のリンク機構に、前記基端側の端部リンク部材であるアームを回転させることにより、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータを設け、前記先端側のリンクハブにエンドエフェクタを設置したリンク作動装置、を制御する制御装置であって、
   前記リンク作動装置の前記各リンク機構は、前記リンク作動装置が如何なる姿勢をとっていても、前記リンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、
   前記制御装置は、
   前記エンドエフェクタが作業する被作業面上の前記エンドエフェクタが移動する始点から終点までの経路を通過点で複数の区間に分割し、前記通過点の位置を設定する分割区間設定手段と、
   指定された一定速度の目標移動速度および前記各区間の距離から定まる各区間内の移動の時間と、前記各区間における前記各アームの回転角移動量とから、前記各区間における前記各アームが等速回転する回転速度を計算するアーム回転速度計算手段と、
   前記各区間内での前記各アームの回転速度を前記回転速度計算手段で計算された値として、前記各区間を加減速無しで連続して前記各アームを回転させるように前記各アクチュエータを位置決め制御する姿勢変更制御手段と、
   を備えることを特徴とするリンク作動装置の制御装置。
2. 請求項１に記載のリンク作動装置の制御装置において、前記エンドエフェクタが作業する前記被作業面は平面であるリンク作動装置の制御装置。
3. 請求項１に記載のリンク作動装置の制御装置において、前記エンドエフェクタが作業する前記被作業面は球面であるリンク作動装置の制御装置。
4. 請求項２に記載のリンク作動装置の制御装置において、前記アーム回転速度計算手段は、前記始点、終点、および各通過点のいずれかの点である各ポイントへの前記各アームの回転速度を計算するものであって、この回転速度を、前記被作業面を示す直交座標系の任意の２ポイント間の距離と前記目標移動速度から求められる時間と、前記２ポイント間の前記各アームの回転角移動量とから計算するリンク作動装置の制御装置。
5. 請求項３に記載のリンク作動装置の制御装置において、前記アーム回転速度計算手段は、前記始点、終点、および各通過点のいずれかの点である各ポイントへの前記各アームの回転速度を、球面三角法より得られる任意の２ポイント間の移動量と前記目標移動速度から求められる時間と、前記２ポイント間の前記各アームの回転角移動量とから、前記各区間における前記各アームの回転速度を計算するリンク作動装置の制御装置。
6. 請求項１ないし請求項５のいずれか１項に記載のリンク作動装置の制御装置において、前記各区間における、前記各通過点の位置と、前記各区間における前記各アームの回転速度につき、リンク作動装置の実使用の前に計算した値を記憶する区間対応設定情報記憶手段を有し、前記姿勢変更制御手段は、実使用時は、前記区間対応設定情報記憶手段に記憶された前記各通過点の位置と、前記各アームの回転速度を読み出して制御するリンク作動装置の制御装置。
7. 基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記３組以上のリンク機構のうち２組以上のリンク機構に、前記基端側の端部リンク部材であるアームを回転させることにより、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータを設け、前記先端側のリンクハブにエンドエフェクタを設置したリンク作動装置、を制御する制御方法であって、
   前記リンク作動装置の前記各リンク機構は、前記リンク作動装置が如何なる姿勢をとっていても、前記リンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、
   前記制御方法は、
   前記エンドエフェクタが作業する被作業面上の前記エンドエフェクタが移動する始点から終点までの経路を通過点で複数の区間に分割し、
   指定された目標移動速度および前記各区間の距離から定まる各区間内の移動の時間と、前記各区間における前記各アームの回転角移動量とから、前記各区間における前記各アームの回転速度を計算し、
   前記各区間内での前記各アームの回転速度を前記の計算された値として、前記各区間を加減速無しで連続して前記各アームを回転させるように前記各アクチュエータを位置決め制御することで、
   前記被作業面上の分割された複数点間を始点から終点まで実質的に一定速度に動作させることを特徴とするリンク作動装置の制御方法。

Images