JP6648246B1 - リンク作動装置の制御装置 - Google Patents

Abstract

【課題】 端部リンク部材の回転角から、直交座標で示される先端側リンク中心の座標を求める計算が、近似解によらずに行えて算出時間が短縮でき、かつ精度良く求めることができるリンク作動装置の制御装置を提供する。【解決手段】リンク作動装置５１は、基端側リンクハブ２に対し先端側リンクハブ３が３組以上のリンク機構４を介して姿勢を変更可能に連結されたパラレルリンク機構１と、姿勢変更用のアクチュエータ５３を備える。教示手段５６は、端部リンク部材５の回転角βｎ（ｎ＝１，２，・・）から、先端側リンクハブ３の直交座標で示される先端側リンク中心の座標Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）を演算する変換手段６１を有する。この変換手段６１に、平面と球面の方程式に法線ベクトルを適用して整理した式を用いる。【選択図】 図１

Description

この発明は、産業機器や医療機器等の精密で広範な作動範囲を必要とする機器に用いられるパラレルリンク機構型のリンク作動装置に対して、教示および制御を行うリンク作動装置の制御装置に関する。

パラレルリンク機構を用いたリンク作動装置は、極座標系で表現できる２自由度の装置である。その先端側リンクハブの中心である先端側リンクハブの球面中心と基端側リンクハブの球面中心との関係式は、特許文献１に示されており、先端側リンクハブの球面中心である先端側リンク中心の座標から、基端側端部リンク部材を回動させるモータの回転角へは、逆変換で求められている。これと同様に、順変換は、基端側２軸の回転角からの算出を、ニュートン・ラフソン法による近似解で求めることが示されている。

特許第５８６４３２号公報

リンク作動装置に教示方法の一つとして、先端側リンクハブを手動で目標位置に動作させ、そのときの基端側の端部リンク部材の回転角から、直交座標で示される先端側リンク中心の座標を求める方法がある。
しかし、特許文献１では、基端側２軸の回転角から先端側リンク中心の座標を求める方法として、ニュートン・ラフソン法による近似解を用いており、繰り返し演算で近似解を求める計算を行うため、時間が掛かると言う問題がある。
上記のような２自由度のパラレルリンク機構において、基端側２軸による先端部座標を求める関係式は、前記の近似解以外に無くて、前記近似解を使わざるを得ず、教示の計算に時間が掛かっていた。
また、近似解によるため、先端部座標の位置の精度に劣り、教示された先端部座標の位置を用いてリンク作動装置を動作させる場合、特に多点高速移動させる場合に、機構的な遊びによるがたつきを抑えるトルク付加制御の精度が十分に得られず、また位置決め精度や、軌道制度がいま一つ十分に得られないと言う課題があった。

この発明は、上記課題を解消するものであり、その目的は、２自由度のパラレルリンク機構を備えるリンク作動装置につき、端部リンク部材の回転角から、直交座標で示される先端側リンク中心の座標を求める計算が、近似解によらずに行えて算出時間が短縮でき、かつ精度良く求めることができるリンク作動装置の制御装置を提供することである。
この発明の他の目的は、２自由度のパラレルリンク機構を備えるリンク作動装置を制御するにつき、直交座標で示される先端側リンク中心の座標から、端部リンク部材の回転角を求める計算が短時間で精度良く行えて、位置決め精度や、軌道制度を向上させることができるリンク作動装置の制御装置を提供することである。

この発明の第一のリンク作動装置の制御装置５４は、
基端側リンクハブに対し先端側リンクハブが、３組以上のリンク機構を介して姿勢を変更可能に連結され、前記各リンク機構は、それぞれ前記基端側リンクハブおよび前記先端側リンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とを有し、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルの、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であるパラレルリンク機構、および前記３組以上のリンク機構のうちの２組以上のリンク機構の前記基端側の端部リンク部材を回動させるアクチュエータを備えるリンク作動装置における、前記各アクチュエータを制御する装置であって、
前記先端側リンクハブの姿勢を教示する教示手段５６を有する。

この教示手段５６は、
前記各アクチュエータ５３が連結された少なくとも２つの前記端部リンク部材５の回転角βｎ（ｎ＝１，２，・・）から、前記先端側リンクハブ３の前記姿勢として直交座標で示される前記先端側リンクハブ３の中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）（Ｂ）を演算する変換手段６１を有する。
前記直交座標は、
前記基端側リンクハブ２の中心軸ＱＡとこの中心軸ＱＡと直交する２次元の直交座標系との交点を原点Ｏとし、前記基端側リンクハブ２の中心軸ＱＡに対して前記先端側リンクハブ３の中心軸ＱＢが傾斜する垂直角度である折れ角θが０°である前記先端側リンクハブ３の中心軸ＱＢの前記基端側リンクハブ２から離れる方向を正とする座標軸をＺ軸とし、このＺ軸に対し右手系でＸＹ軸を構成する座標である。
前記変換手段は、前記基端側リンクハブの中心である基端側リンク中心から前記先端側リンク中心の軌道中心に向かうベクトルである法線ベクトルを、実施形態において説明する平面と球面の方程式に当てはめて使用する。

前記法線ベクトルは、次式によって示される法線ベクトルNn(An,Bn,Cn)である。

ｎ：各リンク機構の系統を示す番号（＝１，２，・・）
ｌ_０：基端側リンクハブの中心Ｃと先端側リンクハブの中心Ｂとの距離（固定値）
γ：中央リンク部材の軸角（中央リンク部材の両端の端部リンク部材との２本の連結中心軸が成す角度）
βn ：各端部リンク部材の基端の回転角
ηｎ：位相角
An,Bn,Cn：直交座標系の軸方向成分

この構成によると、２自由度のパラレルリンク機構１を備えるリンク作動装置５１につき、基端側の端部リンク部材５の回転角βｎから、直交座標で示される先端側リンク中心Ｂを求める計算が、近似解によらずに行える。そのため、近似解を求める場合のような多くの繰り返し計算が不要で、算出時間が短縮でき、かつ精度良く求めることができる。

この発明において、前記パラレルリンク機構の前記対称は、回転対称であってもよい。
この発明において、前記パラレルリンク機構の前記対称は、鏡像対称であってもよい。
前記パラレルリンク機構が回転対称と鏡像対称とのいずれの対称構成であっても、前記の各式は成り立つ。

この発明の第２のリンク作動装置の制御装置５４は、前記各アクチュエータ５３を制御する制御手段５５の変換手段６１Ａに、前記基端側リンクハブの中心である基端側リンク中心から先端側リンク中心の軌道中心 に向かうベクトルである法線ベクトルを、実施形態において説明する平面と球面の方程式に当てはめて使用する。
リンク作動装置の構成は、第１のリンク作動装置の制御装置につき説明した構成と同じである。
前記各アクチュエータ５３を制御する制御手段５５は、
前記先端側リンクハブ３の前記姿勢として直交座標で示される前記先端側リンクハブ２の中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）（Ｂ）から、前記各アクチュエータ５３が連結された少なくとも２つの前記基端側の端部リンク部材５の回転角βｎ（ｎ＝１，２，・・）を、以下の数式を用いて演算する変換手段６１Ａを有する。
前記直交座標および各数式は、第１のリンク作動装置の制御装置につき説明した事項と同じであるが、再度掲載する。

前記直交座標は、
前記基端側リンクハブの中心軸とこの中心軸と直交する２次元の直交座標系との交点を原点とし、前記基端側リンクハブの中心軸に対して前記先端側リンクハブの中心軸が傾斜する垂直角度である折れ角が０°である前記先端側リンクハブの中心軸の前記基端側リンクハブから離れる方向を正とする座標軸をＺ軸とし、このＺ軸に対し右手系でＸＹ軸を構成する座標である。

前記法線ベクトルは、次式によって示される法線ベクトルNn(An,Bn,Cn)
である。

ｎ：各リンク機構の系統を示す番号（＝１，２，・・）
ｌ_０：基端側リンクハブの中心Ｃと先端側リンクハブの中心Ｂとの距離（固定値）
γ：中央リンク部材の軸角（中央リンク部材の両端の端部リンク部材との２本の連結中心軸が成す角度）
βn ：各端部リンク部材の基端の回転角
ηn ：位相角
An,Bn,Cn：直交座標系の軸方向成分

この構成の場合、２自由度のパラレルリンク機構を備えるリンク作動装置につき、直交座標で示される先端側リンク中心から端部リンク部材の回転角を求める計算が、近似解によらずに行える。そのため、近似解を求める場合のような多くの繰り返し計算が不要で、算出時間が短縮でき、かつ精度良く求めることができる。
そのため、多点移動を行う場合に、高速移動により設備タクトタイムの短縮が行える。また、正しい回転角が算出されるため、機構的な遊びによるぶれを抑える一定トルク付加制御の精度が上がり、位置決め精度や、軌道制度が向上する。

この発明の第１のリンク作動装置の制御装置は、教示手段が、各アクチュエータが連結された少なくとも２つの基端側の端部リンク部材の回転角βｎ（ｎ＝１，２，・・）から、先端側リンクハブの姿勢として直交座標で示される前記先端側リンクハブの中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）（Ｂ）を上記の法線ベクトルを用いて演算する変換手段を有するため、端部リンク部材の回転角から、直交座標で示される先端側リンク中心を求める計算が、近似解によらずに行えて算出時間が短縮でき、かつ精度良く求めることができる。

この発明の第２のリンク作動装置の制御装置は、先端側リンクハブの姿勢として直交座標で示される前記先端側リンクハブの中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ））（Ｂ）から、各アクチュエータが連結された少なくとも２つの端部リンク部材の回転角βｎ（ｎ＝１，２，・・）を、上記の法線ベクトルを用いて演算する変換手段を有するため、直交座標で示される先端側リンク中心から、端部リンク部材の回転角の計算を短時間で精度良く行えて、位置決め精度や、軌道制度を向上させることができる。

この発明の一実施形態に係るリンク作動装置の制御装置の概念構成のブロック図とパラレルリンク機構の斜視図とを組み合わせた図である。 同パラレルリンク機構の図１とは異なる動作状態を示す斜視図である。 同パラレルリンク機構における一つのリンク機構の斜視図である。 同リンク作動装置の一部を省略した正面図である。 同パラレルリンク機構における一つのリンク機構の各姿勢を示す正面図である。 同パラレルリンク機構の一つのリンク機構を直線で表現したモデル図である。 同リンク作動装置の基端側リンクハブと端部リンクとの連結部を示す断面図である。 同パラレルリンク機構の直交座標系の斜視図である。 同パラレルリンク機構の先端側リンク中心軌道の説明図である。 同パラレルリンク機構の２種の対称構成についての斜視図による説明図である。

この発明の一実施形態にかかるリンク作動装置の制御装置を図面と共に説明する。図１に示すように、リンク作動装置５１は、パラレルリンク機構１と、このパラレルリンク機構１を動作させる複数のアクチュエータ５３とを備え、制御装置５４によって制御および教示がなされる。制御装置５４に、前記アクチュエータ５３を制御する制御手段５５、教示手段５６、手動操作手段５７、および記憶手段５８備える。

＜パラレルリンク機構１＞
パラレルリンク機構１は、基端側リンクハブ２に対し先端側リンクハブ３を、３組のリンク機構４（４_１,４_２,４₃）を介して姿勢変更可能に連結したものである。基端側リンクハブ２は、図４に示すように基台５２に設置されている。図１、図２は、パラレルリンク機構１の異なる動作状態を示し、図３〜図５はパラレルリンク機構１の１組のリンク機構４を示す。

図５（Ａ）（Ｂ）は、１組のリンク機構４のそれぞれ異なる状態を示す正面図である。 各リンク機構４は、基端側の端部リンク部材５、先端側の端部リンク部材６、および中央リンク部材７で構成され、４つの回転対偶からなる３節連鎖のリンク機構をなす。基端側および先端側の端部リンク部材５，６はＬ字状をなし（図１〜図３参照）、基端がそれぞれ基端側リンクハブ２および先端側リンクハブ３に回転自在に連結されている。中央リンク部材７は、両端に基端側および先端側の端部リンク部材５，６の先端がそれぞれ回転自在に連結されている。なお、以下の説明において、先端側の端部リンク部材６は、「基端側アーム」と称することがある。

基端側および先端側の端部リンク部材５，６は球面リンク構造であり、３組のリンク機構４における基端側および先端側の球面リンク中心は、基端側リンクハブ２の中心である基端側リンク中心ＰＡおよび先端側リンクハブ３の中心である先端側リンク中心ＰＢにそれざれ一致している。したがって、３組のリンク機構４の球面リンク中心
間の距離は、基端側リンク中心ＰＡと先端側リンク中心ＰＢの間の距離ｌ_０となる。端部リンク部材５，６と中央リンク部材７との各回転対偶の中心軸は、ある交差角をもっていてもよいし、平行であってもよい。

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

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

基端側リンクハブ２と先端側のリンクハブ３と３組のリンク機構４とで、基端側リンクハブ２に対し先端側リンクハブ３が直交２軸方向に移動自在な２自由度機構が構成される。言い換えると、基端側リンクハブ２に対して先端側リンクハブ３を、回転が２自由度で姿勢変更自在な機構である。この２自由度機構は、コンパクトでありながら、基端側リンクハブ２に対する先端側リンクハブ３の可動範囲を広くとれる。例えば、基端側リンクハブ２の中心軸ＱＡと先端側リンクハブ３の中心軸ＱＢの折れ角θ（図１、図３）の最大値（最大折れ角）を約±９０°とすることができる。また、基端側リンクハブ２に対する先端側リンクハブ３の旋回角φを０°〜３６０°の範囲に設定できる。折れ角θは、基端側リンクハブ２の中心軸ＱＡに対して先端側リンクハブ３の中心軸ＱＢが傾斜した垂直角度のことであり、旋回角φは、基端側リンクハブ２の中心軸ＱＡに対して先端側リンクハブ３の中心軸ＱＢが傾斜した水平角度のことである。

このパラレルリンク機構１において、各リンク機構４の端部リンク部材５，６の軸部材１３（図７）の角度、および長さが等しく、かつ基端側の端部リンク部材５と先端側の端部リンク部材６の幾何学的形状が等しく、かつ中央リンク部材７についても基端側の先端側とで形状が等しいとき、中央リンク部材７の対称面に対して、中央リンク部材７と端部リンク部材５，６との角度位置関係を基端側と先端側とで同じにすれば、幾何学的対称性から基端側リンクハブ２および基端側の端部リンク部材５と、先端側リンクハブ３および先端側の端部リンク部材６とは同じに動く。例えば、基端側と先端側のリンクハブ２，３にそれぞれの中心軸ＱＡ，ＱＢと同軸に回転軸を設け、基端側から先端側へ回転伝達を行う場合、基端側と先端側は同じ回転角になって等速で回転する等速自在継手となる。この等速回転するときの中央リンク部材７の対称面を等速二等分面Ｅ（図３）という。

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

基端側リンクハブ２および先端側リンクハブ３は、この実施形態では、その中心部に貫通孔１０が軸方向に沿って形成され、外形が球面状をしたドーナツ形状をしている。貫通孔１０の中心はリンクハブ２，３の中心軸ＱＡ，ＱＢと一致している。これら基端側リンクハブ２および先端側リンクハブ３の外周面の円周方向に等間隔の位置に、基端側の端部リンク部材５および先端側の端部リンク部材６がそれぞれ回転自在に連結されている。

図７は、基端側リンクハブ２と基端側の端部リンク部材５の回転対偶、および基端側の端部リンク部材５と中央リンク部材７の回転対偶を示す断面図である。基端側リンクハブ２は、前記軸方向の貫通孔１０と外周側とを連通する半径方向の連通孔１１が円周方向３箇所に形成され、各連通孔１１内に設けた二つの軸受１２により軸部材１３がそれぞれ回転自在に支持されている。軸部材１３の外側端部は基端側リンクハブ２から突出し、その突出ねじ部１３ａに基端側の端部リンク部材５が結合され、ナット１４によって締付け固定されている。

前記軸受１２は、例えば深溝玉軸受等の転がり軸受であり、その外輪（図示せず）が前記連通孔１１の内周に嵌合し、その内輪（図示せず）が前記軸部材１３の外周に嵌合している。外輪は止め輪１５によって抜け止めされている。また、内輪と基端側の端部リンク部材５の間には間座１６が介在し、ナット１４の締付力が基端側の端部リンク部材５および間座１６を介して内輪に伝達されて、軸受１２に所定の予圧を付与している。

基端側の端部リンク部材５と中央リンク部材７の回転対偶は、中央リンク部材７の両端に形成された連通孔１８に二つの軸受１９が設けられ、これら軸受１９により、基端側の端部リンク部材５の先端の軸部２０が回転自在に支持されている。軸受１９は、間座２１を介して、ナット２２によって締付け固定されている。

前記軸受１９は、例えば深溝玉軸受等の転がり軸受であり、その外輪（図示せず）が前記連通孔１８の内周に嵌合し、その内輪（図示せず）が前記軸部２０の外周に嵌合している。外輪は止め輪２３によって抜け止めされている。軸部２０の先端ねじ部２０ａに螺着したナット２２の締付力が間座２１を介して内輪に伝達されて、軸受１９に所定の予圧を付与している。

以上、基端側リンクハブ２と基端側の端部リンク部材５の回転対偶、および基端側の端部リンク部材５と中央リンク部材７の回転対偶について説明したが、先端側リンクハブ３と先端側の端部リンク部材６の回転対偶、および先端側の端部リンク部材６と中央リンク部材７の回転対偶も基端側と同じ構成である（図示省略）。

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

この軸受１２，１９を設けた構造では、軸受１２，１９に予圧を付与することにより、ラジアル隙間とスラスト隙間をなくし、回転対偶のがたつきを抑えることができ、基端側リンクハブ２側と先端側リンクハブ３側間の回転位相差がなくなり等速性を維持できると共に振動や異音の発生を抑制できる。特に、前記軸受１２，１９の軸受隙間を負すきまとすることにより、入出力間に生じるバックラッシュを少なくすることができる。

＜アクチュエータ５３＞
図４において、基台５２の上面にパラレルリンク機構１の基端側リンクハブ２が固定されている。基台５２の上部の外周にはつば状のアクチュエータ取付台５５が設けられ、このアクチュエータ取付台５５に前記アクチュエータ５３が垂下状態で取付けられている。アクチュエータ５３の数は、リンク機構４と同数の３個であるが、２個であってもよい。アクチュエータ５３はロータリアクチュエータからなり、その出力軸に取付けたかさ歯車５６と基端側の端部リンク部材５の軸部材１３（図７）に取付けた扇形のかさ歯車５７とが噛み合っている。アクチュエータ５３は、電動モータであってもよい。

各アクチュエータ５３に、回転角度検出手段５９（図１参照）が設けられている。回転角度検手段５９は、直接にはアクチュエータ５３の出力軸の回転角度を検出するものであるが、制御装置５４側、例えば教示手段５６または制御手段５５において、前記出力軸と基端側の端部リンク部材５の軸部材１３との回転比（かさ歯車５６，５７の歯数比）を乗算することで、基端側の端部リンク部材５の回転角βｎ（β１、β２、・・）（図１参照）を検出する手段として用いられる。

＜リンク作動装置５１の動作の概要＞
このリンク作動装置５１は、制御装置５４の制御手段５５による制御、または手動操作手段５７による操作に従い、各アクチュエータ５３を回転駆動することで、パラレルリンク機構１を作動させる。詳しくは、アクチュエータ５３が回転駆動すると、その回転が一対のかさ歯車５６，５７を介して軸部材１３に伝達されて、基端側リンクハブ２に対する基端側の端部リンク部材５の回転角度βｎが変更する。それにより、基端側リンクハブ２に対する先端側リンクハブ３の位置（以下、「先端位置」と称する）が決まる。ここでは、かさ歯車５６、５７を用いて基端側の端部リンク部材５の角度を変更しているが、その他の機構（例えば、平歯車やウォーム機構）でも良い。

制御装置５４は、コンピュータによる数値制御式のものであり、記憶手段５８に記憶されたプログラム、および先端側リンクハブ３の中心位置である先端側リンク中心Ｂ（以下、「先端位置」と称することがある）のデータに従って、制御手段５５により制御する。前記先端位置のデータは、後述の直交座標Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）、極座標（θ、φ）、各基端側の端部リンク部材５の回転角βｎのいずれで指定されていてもよい。

先端位置が極座標で指定されている場合、姿勢制御は以下のように行う。まず、指令された先端位置に応じて、各基端側の端部リンク部材５の回転角βｎ（ｎ＝１，２、・・）（図１、図３）を求める。ここで言う回転角βｎは、指令された先端位置に対応する各基端側の端部リンク部材５の回転角（水平面からの角度）のことである。

回転角βｎは、例えば、下記の式１を逆変換することで求められる。逆変換とは、基端側リンクハブ２の中心軸ＱＡと先端側リンクハブ３の中心軸ＱＢの折れ角θ（図１、図３）、および基端側リンクハブ２に対する出力側のリンクハブ３の旋回角φから基端側の端部リンク部材５の回転角βｎを算出する変換のことである。折れ角θおよび旋回角φと、回転角βｎとは相互関係があり、一方の値から他方の値を導くことができる。
cos（θ／２）sinβｎ−sin（θ／２）sin（φ＋δｎ）cosβｎ＋sin（γ／２）＝０
；（ｎ＝１，２，３） …（式１）
ここで、軸角γ（図１、図３）は、基端側の端部リンク部材５に回転自在に連結された中央リンク部材７の連結中心軸と、先端側の端部リンク部材６に回転自在に連結された中央リンク部材７の連結中心軸とが成す角度である。δｎ（図１におけるδ１，δ２，δ３）は、基準となる基端側の端部リンク部材５に対する各基端側の端部リンク部材５の円周方向の離間角である。

前記直交座標は、図１に示すように、基端側リンクハブ２の中心軸ＱＡの延長軸と直交し、前記延長軸上の任意の位置に原点Ｏが定められたＸＹ直交座標である。目標とする先端姿勢は、先端側リンクハブ３の中心軸ＱＢがＸＹ直交座標系と交わる点の座標である目標座標ｔ（Ｘｔ，Ｙｔ）で表わされる。ここでは、基端側リンク中心Ｃに原点Ｏを定めた例を説明する。

＜手動操作手段５７＞
手動操作手段５７は、各アクチュエータ５３を手動入力装置（図示せず）からの入力に従って回転駆動させ、パラレルリンク機構１を姿勢変更させる手段である。前記手動入力装置は、例えば制御装置５４に設けられた液晶表示装置の画面に、アクチュエータ５３毎に表示された各送りボタンをタッチする毎に微小角度だけ、対応するアクチュエータ５３を動作させる指令を与える構成とされる。前記手動入力装置は、この他に、前記液晶表示装置の画面に設けられた入力ボックスに、前記端部リンク部材５の回転角βｎ等を数値で入力する構成であってもよい。

＜教示手段５６＞
教示手段５６は、パラレルリンク機構１の先端位置を任意位置に位置させたときの、
先端側リンク中心の座標Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）を、基端側の各端部リンク部材５の回転角βｎ（ｎ＝１，２、またはｎ＝１，２，３）から求める装置である。パラレルリンク機構１を任意位置に位置させる動作は、前記手動操作手段５７により手動入力で行っても、各アクチュエータ５３を回転自在状態としておいて、パラレルリンク機構１を手で動かせて行ってもよい。

この教示手段５６は、前記各アクチュエータ５３が連結された少なくとも２つの前記端部リンク部材５の回転角βｎ（ｎ＝１，２，・・）から、前記先端側リンクハブ３の前記姿勢として直交座標で示される前記先端側リンクハブ３の中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）（Ｂ）を以下の法線ベクトル、数式を用いて演算する変換手段６１を備える。

＜直交座標＞
前記直交座標は、 前記基端側リンクハブ２の中心軸ＱＡとこの中心軸ＱＡと直交する２次元の直交座標系との交点Ｃを原点とし、前記基端側リンクハブ２の中心軸ＱＡに対して前記先端側リンクハブ３の中心軸ＱＢが傾斜する垂直角度である折れ角θが０°である前記先端側リンクハブ３の中心軸ＱＢの前記基端側リンクハブ２から離れる方向を正とする座標軸をＺ軸とし、このＺ軸に対し右手系でＸＹ軸を構成する座標である。

＜数式＞
前記数式は、以下の通りである。
法線ベクトルNn(An,Bn,Cn)を示す式
（なお、法線ベクトルNn(An,Bn,Cn)については、後に説明する。）

ｎ：各リンク機構の系統を示す番号（＝１，２，・・）
ｌ_０：基端側リンクハブの中心Ｃと先端側リンクハブの中心Ｂとの距離（固定値）
γ：中央リンク部材の軸角（中央リンク部材の両端の端部リンク部材との連結中心軸が成す角度）
βn ：各端部リンク部材の基端の回転角
ηn ：位相角

法線ベクトルNn(An,Bn,Cn)と先端側リンクハブの中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）（Ｂ）との関係式は、上記の関係から適宜得られる。

この構成によると、２自由度のパラレルリンク機構１を備えるリンク作動装置につき、端部リンク部材２の回転角βｎから、直交座標で示される先端側リンク中心Ｂを求める計算が、近似解によらずに行える。そのため、近似解を求める場合のような多くの繰り返し計算が不要で、算出時間が短縮でき、かつ精度良く求めることができる。

＜上記の式が成り立つ理由＞
上記の式により各端部リンク部材の回転角βｎ（ｎ＝１，２、またはｎ＝１，２，３）から、前記先端側リンク中心の座標Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）（Ｂ）が求まる理由を説明する。

パラレルリンク機構１の構造を図１に、ジョイントの姿勢を図３に示す。
図３では、基端側リンク中心線ＱＡと先端側リンク中心線ＱＢの交点を座標系基準として、先端側リンク中心Ｂを、θ・φで表現している。
前記変換手段６１(図１)では、第１のリンク機構４_１、第２のリンク機構４_２、及び第３のリンク機構４₃の３リンク系統のうち、２系統の関係式により、基端側リンク中心Ｃを座標系基準とし、先端側リンク中心Ｂの位置の関係式を導出する。

先ず一つのリンク系を考える（図３）。このリンク機構４は、長さが同じ一点鎖線の直線の線分ａ，ｂの２本とその交点Ａで理論的に示すことができる。線分ａ，ｂは、中央リンク部材７の両端の前記連結中心軸となる直線のうち、前記交点Ａから基端側リンク中心Ｃおよび先端側リンク中心Ｂまでの部分である。線分ａ，ｂをそれぞれ半径とする、基端側リンク中心Ｃを中心とする基端部球面Ｓ_Ｃと、先端側リンク中心Ｂを中心とする先端部球面Ｓ_Ｂの交点が、Ａ点の軌道円ＣＩになる。このＡ点軌道円ＣＩを含む平面を前記等速二等分面Ｅとして示してある。この等速二等分面Ｅに対して、パラレルリンク機構１の先端部側部分と基端部側部分は面対象の関係にある。

軸角γはこのパラレルリンク機構１において一定であるので、線分ａ，ｂの点Ａとは逆の端点同士間の距離は一定であるため、基端側リンク中心Ｃと先端側リンク中心Ｂ間の距離ｌ０は一定である。よって先端側リンク中心Ｂは、基端部側の線分ａ（ＡＣ）を回転中心とし、半径を基端側リンク中心Ｃと先端側リンク中心Ｂ間の距離ｌ０とした円軌道ＴＢ（図９参照）を描く。このパラレルリンク機構１において、各系統のリンク機構４の円軌道ＴＢは先端側リンク中心Ｂで交わる。この関係を用いて、パラレルリンク機構１の先端位置Ｂ（先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）（Ｂ）として導出する。

図８に、この関係式で示すワールド座標系を示す。座標系の原点Ｏは、基端側リンク中心Ｃである。

図９に先端側リンク中心軌道Ｔ_Ｂ（先端側リンク中心Ｂの軌道）を示す。
図９の先端側リンク中心軌道Ｔ_Ｂは、第１のリンク機構４_１に関して表示している。
３つのリンク機構４の構成において、各リンク機構４の先端側軌道Ｔ_Ｂの交点が、先端中心位置（先端側リンク中心Ｂ）である。また、ある２系統のリンク機構４のリンク交点（軌道円の交点（換言すれば先端側軌道Ｔ_Ｂの交点）に、もう１系統のリンク機構４の軌道円が交わるため、２系統のリンク機構４のリンク交点を求める。つまり、２自由度での先端側リンク中心Ｂを導出できる。

３次元空間内での各リンク機構４の先端側リンク中心軌道ＴＢは下記２式を、アンド条件で満たすものである。
平面： Ａｎ（Ｘ−Ａｎ）＋Ｂｎ（Ｙ−Ｂｎ）＋ Ｃｎ（Ｚ−Ｃｎ）＝０
球面： （Ｘ−Ａｎ）^２＋（Ｙ−Ｂｎ）^２＋（Ｚ−Ｃｎ）^２＝ｒ^２
ベクトル Ｎｎを法線ベクトル( Ａｎ, Ｂｎ, ・・) とする。
ｎ：各リンク機構の系統を示す番号（＝１，２，・・）

＜法線ベクトル＞
法線ベクトルNn(An,Bn,Cn)は、図９において原点（基端側リンク中心Ｃ）から、先端側リンク中心軌道Ｔ_Ｂの中心に向かうベクトルである。
なお、上記法線ベクトルNnのＡｎ、Ｂｎ、Ｃｎは、それぞれ上記直交座標系の軸方向成分であることを示す。

法線ベクトルを下記に示す。

ｎ：各リンク機構の系統を示す番号（＝１，２，・・）
ｌ _０ ：基端側リンクハブの中心Ｃと先端側リンクハブの中心Ｂとの距離（固定値）
γ：中央リンク部材の軸角（中央リンク部材の両端の端部リンク部材との２本の連結中心軸が成す角度）
βn ：各端部リンク部材の基端の回転角
ηn ：位相角
An,Bn,Cn：直交座標系の軸方向成分

先端軌道半径を下記に示す。

上記、平面と球面の方程式に法線ベクトルn1，n2を当てはめて整理する。
先端中心座標( 先端側リンク中心の座標) をＷｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）（Ｂ）とする。

上記は、基端部リンク中心をワールド座標系原点としたときの、先端側リンク中心座標である。
なお、このリンク作動装置を、先端側リンクハブ３に作業機（図示せず）を取り付けてワークに対して作業を行う構成とし、ワークの作業点で先端側リンクハブ３の位置を指定や制御する実機構においては、先端側リンク中心の座標とワーク座標などの関係式を用いる。

上記関係式の正当性を示すため、従来制御式を下記に示す

θ：折れ角、φ：旋回角、γ：中央リンクの軸角、ηn：基端側アーム（基端側の端部リンク部材）の離間隔 、α：アーム角、εn：基端側アームと中央リンクの回転対偶部の位相、n：リンク系の番号

上記関係式にあるように、本手法においては、アーム角αは算出手順に必要ない為、本手法では考慮する必要が無い。つまり、各軸回転角は、アーム角αとは無関係である。

以上のように、この実施形態における、上記の平面と球面の方程式に法線ベクトルを適用した数式を用いる数式が成り立ち、この数式を教示に用いることで、端部リンク部材２の回転角βｎから、直交座標で示される先端側リンク中心Ｂを求める計算が、近似解によらずに行える。そのため、近似解を求める場合のような多くの繰り返し計算が不要で、算出時間が短縮でき、かつ精度良く求めることができる。

なお、上記実施形態では、パラレルリンク機構１が回転対称タイプである場合につき説明したが、鏡像対称タイプである場合にも適用することができる。
図１０の（Ａ）と（Ｂ）は、パラレルリンク機構の構成が２種存在することを示している。同図（Ａ）は回転対称タイプ、（Ｂ）は鏡像対称タイプを示す。その両方は、図１０（Ｃ）の様に、基端側リンク中心Ｃから先端側リンク中心Ｂへの理論的表現が可能である。線分Ｈは、直線モデルを示す(図６参照)。
図１０の（Ａ）と（Ｂ）)には、中心線に対してリンク機構４が逆の回転系も存在するが、これも上記と同様に図１０（Ｃ）のように表すことが可能である。

＜制御手段５５＞
上記の説明は、教示手段５６に上記数式を適用する場合につき説明したが、制御手段５５に上記数式を適用することもできる。

この場合、前記制御手段５５は、前記先端側リンクハブ３の前記姿勢として直交座標で示される前記先端側リンクハブ２の中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）（Ｂ）から、前記各アクチュエータ５３が連結された少なくとも２つの前記端部リンク部材２の回転角βｎ（ｎ＝１，２，・・）を、以下の法線ベクトル、数式を用いて演算する変換手段６１Ａを有する。
前記直交座標および各数式は、教示手段５６につき説明した事項と同じであるが、再度掲載する。

＜直交座標＞
前記直交座標は、
前記基端側リンクハブの中心軸とこの中心軸と直交する２次元の直交座標系との交点を原点とし、前記基端側リンクハブの中心軸に対して前記先端側リンクハブの中心軸が傾斜する垂直角度である折れ角が０°である前記先端側リンクハブの中心軸の前記基端側リンクハブから離れる方向を正とする座標軸をＺ軸とし、このＺ軸に対し右手系でＸＹ軸を構成する座標である。

＜数式＞
前記数式は、以下の通りである。
法線ベクトル法線ベクトルNn(An,Bn,Cn)を示す式

ｎ：各リンク機構の系統を示す番号（＝１，２，・・）
ｌ_０：基端側リンクハブの中心Ｃと先端側リンクハブの中心Ｂとの距離（固定値）
γ：中央リンク部材の軸角（中央リンク部材の両端の端部リンク部材との日本の連結中心軸が成す角度）
βn ：各端部リンク部材の基端の回転角
ηn ：位相角
An,Bn,Cn：直交座標系の軸方向成分
法線ベクトルNn(An,Bn,Cn)と先端側リンクハブの中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）との関係式は、上記の関係から適宜得られる。

この構成の場合、２自由度のパラレルリンク機構を備えるリンク作動装置につき、直交座標で示される先端側リンク中心から端部リンク部材の回転角を求める計算が、近似解によらずに行える。そのため、近似解を求める場合のような多くの繰り返し計算が不要で、算出時間が短縮でき、かつ精度良く求めることができる。
そのため、多点移動を行う場合に、高速移動により設備タクトタイムの短縮が行える。 また、正しい回転角が算出されるため、機構的な遊びによるぶれを抑える一定トルク付加制御の精度が上がり、位置決め精度や、軌道制度が向上する。

以上、実施形態に基づいて本発明を実施するための形態を説明したが、ここで開示した実施の形態はすべての点で例示であって制限的なものではない。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

１…パラレルリンク機構、２…基端側リンクハブ、３…先端側リンクハブ、４（４_１〜４_３）…リンク機構、５…基端側の端部リンク部材、６…先端側の端部リンク部材、７…中央リンク部材、５１…リンク作動装置、５３…アクチュエータ、５４…制御装置、５５…制御手段、５６…教示手段、５７…手動操作手段、５９…回転角検出手段、６１，６１Ａ…変換手段、Ａ…交点、Ｂ…先端側シンク中心、Ｃ…基端側リンク中心、ＰＡ…基端側リンク中心、ＰＢ…先端側リンク中心、ＱＡ…基端側リンクハブの中心軸、ＱＢ…先端側リンクハブの中心軸

Claims (4)

1. 基端側リンクハブに対し先端側リンクハブが、３組以上のリンク機構を介して姿勢を変更可能に連結され、前記各リンク機構は、それぞれ前記基端側リンクハブおよび前記先端側リンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とを有し、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルの、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であるパラレルリンク機構、および前記３組以上のリンク機構のうちの２組以上のリンク機構の前記基端側の端部リンク部材を回動させるアクチュエータを備えるリンク作動装置における、前記各アクチュエータを制御するリンク作動装置の制御装置であって、
   前記先端側リンクハブの姿勢を教示する教示手段を有し、
   この教示手段は、
   前記各アクチュエータが連結された少なくとも２つの前記端部リンク部材の回転角βｎ（ｎ＝１，２，・・）から、前記先端側リンクハブの前記姿勢として直交座標で示される前記先端側リンクハブの中心である先端側リンク中心の座標Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）を演算する変換手段を有し、
   前記直交座標は、
   前記基端側リンクハブの中心軸とこの中心軸と直交する２次元の直交座標系との交点を原点とし、前記基端側リンクハブの中心軸に対して前記先端側リンクハブの中心軸が傾斜する垂直角度である折れ角が０°である前記先端側リンクハブの中心軸の前記基端側リンクハブから離れる方向を正とする座標軸をＺ軸とし、このＺ軸に対し右手系でＸＹ軸を構成する座標であり、
   前記変換手段は、前記基端側リンクハブの中心である基端側リンク中心から前記先端側リンク中心の軌道中心に向かうベクトルである法線ベクトルを、下記の平面と球面の方程式に当てはめて使用し、
   前記法線ベクトルは、次式によって示される法線ベクトルNn(An,Bn,Cn)である、
   リンク作動装置の制御装置。
   

   

   ｎ：各リンク機構の系統を示す番号（＝１，２，・・）
   ｌ _０ ：基端側リンクハブの中心Ｃと先端側リンクハブの中心Ｂとの距離（固定値）
   γ：中央リンク部材の軸角（中央リンク部材の両端の端部リンク部材との２本の連結中心軸が成す角度）
   βn ：各端部リンク部材の基端の回転角
   ηn ：位相角
   An,Bn,Cn：直交座標系の軸方向成分
   前記直交座標の３次元空間内での前記各リンク機構の先端側リンク中心軌道は下記の平面と球面の方程式を、アンド条件で満たすものである。
   平面： Ａｎ（Ｘ−Ａｎ）＋ Ｂｎ（Ｙ−Ｂｎ）＋ Ｃｎ（Ｚ−Ｃｎ）＝０
   球面： （Ｘ−Ａｎ） ^２ ＋ （Ｙ−Ｂｎ） ^２ ＋（Ｚ−Ｃｎ） ^２ ＝ｒ ^２
   ｒ：前記先端側リンク中心の軌道半径であり、次式で定まる
   

   
2. 請求項１に記載のリンク作動装置の教示装置において、前記パラレルリンク機構の前記対称は、回転対称であるリンク作動装置の制御装置。
3. 請求項１に記載のリンク作動装置の教示装置において、前記パラレルリンク機構の前記対称は、鏡像対称であるリンク作動装置の制御装置。
4. 基端側リンクハブに対し先端側リンクハブが、３組以上のリンク機構を介して姿勢を変更可能に連結され、前記各リンク機構は、それぞれ前記基端側リンクハブおよび前記先端側リンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とを有し、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルの、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であるパラレルリンク機構、および前記３組以上のリンク機構のうちの２組以上のリンク機構の前記基端側の端部リンク部材を回動させるアクチュエータを備えるリンク作動装置における、前記各アクチュエータを制御するリンク作動装置の制御装置であって、
   前記各アクチュエータを制御する制御手段は、
   前記先端側リンクハブの前記姿勢として直交座標で示される前記先端側リンクハブの中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）から、前記各アクチュエータが連結された少なくとも２つの前記端部リンク部材の回転角βｎ（ｎ＝１，２，・・）を演算する変換手段を有し、
   前記直交座標は、
   前記基端側リンクハブの中心軸とこの中心軸と直交する２次元の直交座標系との交点を原点とし、前記基端側リンクハブの中心軸に対して前記先端側リンクハブの中心軸が傾斜する垂直角度である折れ角が０°である前記先端側リンクハブの中心軸の前記基端側リンクハブから離れる方向を正とする座標軸をＺ軸とし、このＺ軸に対し右手系でＸＹ軸を構成する座標であり、
   前記変換手段は、前記基端側リンクハブの中心である基端側リンク中心から前記先端側リンク中心の軌道中心に向かうベクトルである法線ベクトルを、下記の平面と球面の方程式に当てはめて使用し、
   前記法線ベクトルは、次式によって示される法線ベクトルNn(An,Bn,Cn)である、
   リンク作動装置の制御装置。
   

   

   ｎ：各リンク機構の系統を示す番号（＝１，２，・・）
   ｌ _０ ：基端側リンクハブの中心Ｃと先端側リンクハブの中心Ｂとの距離（固定値）
   γ：中央リンク部材の軸角（中央リンク部材の両端の端部リンク部材との２本の連結中心軸が成す角度）
   βn ：各端部リンク部材の基端の回転角
   ηn ：位相角
   An,Bn,Cn：直交座標系の軸方向成分
   前記直交座標３次元空間内での前記各リンク機構の先端側リンク中心軌道は下記の平面と球面の方程式を、アンド条件で満たすものである。
   平面： Ａｎ（Ｘ−Ａｎ）＋ Ｂｎ（Ｙ−Ｂｎ）＋ Ｃｎ（Ｚ−Ｃｎ）＝０
   球面： （Ｘ−Ａｎ） ^２ ＋ （Ｙ−Ｂｎ） ^２ ＋ （Ｚ−Ｃｎ） ^２ ＝ｒ ^２
   ｒ：前記先端側リンク中心の軌道半径であり、次式で定まる
   
   

   

Images