WO2020111020A1

WO2020111020A1 - リンク作動装置の制御装置

Info

Publication number: WO2020111020A1
Application number: PCT/JP2019/046051
Authority: WO
Inventors: 英樹松澤
Original assignee: Ntn株式会社
Priority date: 2018-11-29
Filing date: 2019-11-26
Publication date: 2020-06-04
Also published as: US11697204B2; US20210283772A1; EP3888858A4; CN113165185A; JP2020082310A; JP6648246B1; EP3888858A1

Abstract

リンク作動装置（５１）は、基端側リンクハブ（２）に対し先端側リンクハブ（３）が３組以上のリンク機構（４）を介して姿勢を変更可能に連結されたパラレルリンク機構（１）と、姿勢変更用のアクチュエータ（５３）とを備える。教示手段（５６）は、端部リンク部材（５）の回転角（βｎ；ｎ＝１，２，・・）から、先端側リンクハブ（３）の直交座標で示される先端側リンク中心の座標（Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ））を演算する変換手段（６１）を有する。この変換手段（６１）に、平面と球面の方程式に法線ベクトルを適用して整理した式を用いる。

Description

リンク作動装置の制御装置

関連出願

　この出願は、２０１８年１１月２９日出願の特願２０１８－２２３７８９の優先権を主張するものであり、その全体を参照により本願の一部をなすものとして引用する。

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

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

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

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

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

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

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

　前記直交座標は、
　前記基端側リンクハブ２の中心軸ＱＡとこの中心軸ＱＡと直交する２次元の直交座標系との交点を原点Ｏとし、前記基端側リンクハブ２の中心軸ＱＡに対して前記先端側リンクハブ３の中心軸ＱＢが傾斜する垂直角度である折れ角θが０°である前記先端側リンクハブ３の中心軸ＱＢの前記基端側リンクハブ２から離れる方向を正とする座標軸をＺ軸とし、このＺ軸に対し右手系でＸＹ軸を構成する座標である。
　前記変換手段は、前記基端側リンクハブの中心である基端側リンク中心から前記先端側リンク中心の軌道中心に向かうベクトルである法線ベクトルを、実施形態において説明する平面と球面の方程式に当てはめて使用する。

　前記法線ベクトルは、次式によって示される法線ベクトルＮｎ（Ａｎ，Ｂｎ．Ｃｎ)である。

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

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

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

　この発明の第２のリンク作動装置の制御装置５４は、前記各アクチュエータ５３を制御する制御手段５５の変換手段６１Ａに、前記基端側リンクハブの中心である基端側リンク中心から先端側リンク中心の軌道中心に向かうベクトルである法線ベクトルを、実施形態において説明する平面と球面の方程式に当てはめて使用する。
　リンク作動装置の構成は、第１のリンク作動装置の制御装置につき説明した構成と同じである。

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

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

　前記法線ベクトルは、次式によって示される法線ベクトルＮｎ（Ａｎ，Ｂｎ，Ｃｎ）である。

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

　請求の範囲および／または明細書および／または図面に開示された少なくとも２つの構成のどのような組合せも、この発明に含まれる。特に、請求の範囲の各請求項の２つ以上のどのような組合せも、この発明に含まれる。

　この発明は、添付の図面を参考にした以下の好適な実施形態の説明からより明瞭に理解されるであろう。しかしながら、実施形態および図面は単なる図示および説明のためのものであり、この発明の範囲を定めるために利用されるべきものではない。この発明の範囲は添付の請求の範囲によって定まる。添付図面において、複数の図面における同一の部品番号は、同一または相当部分を示す。
この発明の一実施形態に係るリンク作動装置のパラレルリンク機構の斜視図である。同リンク作動装置の制御装置の概念構成のブロック図である。同パラレルリンク機構の図１とは異なる動作状態を示す斜視図である。同パラレルリンク機構における一つのリンク機構の斜視図である。同リンク作動装置の一部を省略した正面図である。同パラレルリンク機構における一つのリンク機構の姿勢を示す正面図である。同パラレルリンク機構における一つのリンク機構の図５Ａとは異なる姿勢を示す正面図である。同パラレルリンク機構の一つのリンク機構を直線で表現したモデル図である。同リンク作動装置の基端側リンクハブと端部リンクとの連結部を示す断面図である。同パラレルリンク機構の直交座標系の斜視図である。同パラレルリンク機構の先端側リンク中心軌道の説明図である。同パラレルリンク機構の回転対称構成についての斜視図による説明図である。同パラレルリンク機構の鏡像対称構成についての斜視図による説明図である。同パラレルリンク機構の回転対称構成および鏡像対称構成についての斜視図による説明図である。

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

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

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

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

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

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

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

　また、基端側リンクハブ２に対する先端側リンクハブ３の旋回角φを０°～３６０°の範囲に設定できる。折れ角θは、基端側リンクハブ２の中心軸ＱＡに対して先端側リンクハブ３の中心軸ＱＢが傾斜した垂直角度のことであり、旋回角φは、基端側リンクハブ２の中心軸ＱＡに対して先端側リンクハブ３の中心軸ＱＢが傾斜した水平角度のことである。

　このパラレルリンク機構１において、以下の（１）～（３）を満たすとき、中央リンク部材７の対称面に対して、中央リンク部材７と端部リンク部材５，６との角度位置関係を基端側と先端側とで同じにすれば、幾何学的対称性から基端側リンクハブ２および基端側の端部リンク部材５と、先端側リンクハブ３および先端側の端部リンク部材６とは同じに動く。
（１）各リンク機構４の端部リンク部材５，６の軸部材１３（図７）の角度、および長さが等しい。
（２）基端側の端部リンク部材５と先端側の端部リンク部材６の幾何学的形状が等しい。
（３）中央リンク部材７についても基端側の先端側とで形状が等しい。

　例えば、基端側と先端側のリンクハブ２，３にそれぞれの中心軸ＱＡ，ＱＢと同軸に回転軸を設け、基端側から先端側へ回転伝達を行う場合、基端側と先端側は同じ回転角になって等速で回転する等速自在継手となる。この等速回転するときの中央リンク部材７の対称面を等速二等分面Ｅ（図３）という。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

　回転角βｎは、例えば、下記の式１を逆変換することで求められる。逆変換とは、基端側リンクハブ２の中心軸ＱＡと先端側リンクハブ３の中心軸ＱＢの折れ角θ（図１Ａ、図１Ｂ、図３）、および基端側リンクハブ２に対する出力側のリンクハブ３の旋回角φから基端側の端部リンク部材５の回転角βｎを算出する変換のことである。折れ角θおよび旋回角φと、回転角βｎとは相互関係があり、一方の値から他方の値を導くことができる。
　cos（θ／２）sinβｎ－sin（θ／２）sin（φ＋ηｎ）cosβｎ＋sin（γ／２）＝０；（ｎ＝１，２，３）　　　　　　　　　　　…（式１）

　ここで、軸角γ（図１Ａ、図１Ｂ、図３）は、基端側の端部リンク部材５に回転自在に連結された中央リンク部材７の連結中心軸と、先端側の端部リンク部材６に回転自在に連結された中央リンク部材７の連結中心軸とが成す角度である。ηｎ（図１Ａにおけるη１，η２，η３）は、基準となる基端側の端部リンク部材５に対する各基端側の端部リンク部材５の円周方向の離間角、つまり位相角である。

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

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

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

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

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

　＜数式＞
　前記数式（法線ベクトルＮｎ（Ａｎ、Ｂｎ，Ｃｎ）を示す式）は、以下の通りである。なお、法線ベクトルＮｎ（Ａｎ、Ｂｎ，Ｃｎ）については、後に説明する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

　＜数式＞
　前記数式（法線ベクトル法線ベクトルＮｎ（Ａｎ，Ｂｎ，Ｃｎ）を示す式）は、以下の通りである。

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

　以上のとおり、図面を参照しながら好適な実施形態を説明したが、この発明は、以上の実施形態に限定されるものでなく、この発明の要旨を逸脱しない範囲内で、種々の追加、変更または削除が可能である。よって、そのようなものもこの発明の範囲内に含まれる。

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

Claims

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

　　ｎ：各リンク機構の系統を示す番号（＝１，２，・・）
　　ｌｏ：基端側リンクハブの中心Ｃと先端側リンクハブの中心Ｂとの距離（固定値）
　　γ：中央リンク部材の軸角（中央リンク部材の両端の端部リンク部材との２本の連結中心軸が成す角度）
　　βｎ：各端部リンク部材の基端の回転角
　　ηｎ：位相角
　　Ａｎ，Ｂｎ，Ｃｎ：直交座標系の軸方向成分
　ここで、前記直交座標の３次元空間内での前記各リンク機構の先端側リンク中心軌道は下記の平面と球面の方程式を、アンド条件で満たすものである。
　平面：　Ａｎ（Ｘ－Ａｎ）＋Ｂｎ（Ｙ－Ｂｎ）＋Ｃｎ（Ｚ－Ｃｎ）＝０
　球面：　（Ｘ－Ａｎ）^２＋（Ｙ－Ｂｎ）^２＋（Ｚ－Ｃｎ）^２＝ｒ^２
　　ｒ：前記先端側リンク中心の軌道半径であり、次式で定まる。
　請求項１に記載のリンク作動装置の教示装置において、前記パラレルリンク機構の前記対称は、回転対称であるリンク作動装置の制御装置。
　請求項１または請求項２に記載のリンク作動装置の教示装置において、前記パラレルリンク機構の前記対称は、鏡像対称であるリンク作動装置の制御装置。
　基端側リンクハブに対し先端側リンクハブが、３組以上のリンク機構を介して姿勢を変更可能に連結され、前記各リンク機構は、それぞれ前記基端側リンクハブおよび前記先端側リンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とを有し、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルの、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であるパラレルリンク機構、および前記３組以上のリンク機構のうちの２組以上のリンク機構の前記基端側の端部リンク部材を回動させるアクチュエータを備えるリンク作動装置における、前記各アクチュエータを制御するリンク作動装置の制御装置であって、
　前記各アクチュエータを制御する制御手段は、
　前記先端側リンクハブの前記姿勢として直交座標で示される前記先端側リンクハブの中心である先端側リンク中心Ｗｔ（＝Ｘｔ，Ｙｔ，Ｚｔ）から、前記各アクチュエータが連結された少なくとも２つの前記端部リンク部材の回転角βｎ（ｎ＝１，２，・・）を演算する変換手段を有し、
　前記直交座標は、
　前記基端側リンクハブの中心軸とこの中心軸と直交する２次元の直交座標系との交点を原点とし、前記基端側リンクハブの中心軸に対して前記先端側リンクハブの中心軸が傾斜する垂直角度である折れ角が０°である前記先端側リンクハブの中心軸の前記基端側リンクハブから離れる方向を正とする座標軸をＺ軸とし、このＺ軸に対し右手系でＸＹ軸を構成する座標であり、
　前記変換手段は、前記基端側リンクハブの中心である基端側リンク中心から前記先端側リンク中心の軌道中心に向かうベクトルである法線ベクトルを、下記の平面と球面の方程式に当てはめて使用し、
　前記法線ベクトルは、次式によって示される法線ベクトルＮｎ（Ａｎ，Ｂｎ，Ｃｎ）であるリンク作動装置の制御装置。

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