JP6453066B2 - リンク作動装置の制御方法 - Google Patents

Description

この発明は、産業機器等の精密で広範な作動範囲を必要とする機器に用いられるリンク作動装置の制御方法、特にエンドエフェクタを先端に設置して使用する場合に効果的に使用できる制御方法に関する。

従来、コンパクトな構成で先端側のリンクハブの可動範囲を広く取れるリンク作動装置のパラレルリンク機構１として、図１０，図１１に示すように、基端側のリンクハブ２に対し先端側のリンクハブ３を、３組以上のリンク機構４を介して姿勢を変更可能に連結したものが提案されている（例えば、特許文献１）。
このようなリンク作動装置において、２自由度のパラレルリンク機構１を３個以上のモータで駆動する装置は折れ角θと旋回角φにより姿勢が決定され、その折れ角θと旋回角φから各アーム回転角（β１n 、β２n 、β３n ）を求め、アームを駆動するアクチュエータに位置決めさせる。例えば、あるリンクハブ３の先端姿勢Ａ（θa 、φa ）とＢ（θb 、φb ）について、各々の先端姿勢に対応する各アーム回転角はリンクハブとアーム回転角との関係式によりＡ（β１a 、β２a 、β３a ）、Ｂ（β１b 、β２b 、β３b ）として求められる。ここで、先端姿勢Ａから先端姿勢Ｂへの移動は各アームの回転角がβ1aからβ1b、β2aからβ2b、β3aからβ3bへ移動することで実行される。なお、図１０，図１１には実施形態を示す図１〜図９に対応する部分に同じ符号を付した。

特許文献１では、リンク作動装置の姿勢延長状の直交座標１００から最小自乗法による収束演算を用いてリンクハブ３の先端姿勢（折れ角θ、旋回角φ）を取得している。これにより、エンドエフェクタが作業する作業面（直交座標平面）の任意の座標への位置決めが可能となる。

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

エンドエフェクタ１２２が作業する被作業対象ワークは、２次元形状のものだけではなく、３次元形状のワークに対して作業する用途も多い。一般に、エンドエフェクタが作業する３次元座標（Ｘ，Ｙ，Ｚ）は、ＣＡＤデータ等から容易に算出することができ、３次元ワークに作業する場合は、エンドエフェクタが作業するポジションを３次元の直交座標で指定できることが望ましい。

特許文献１のリンク作動装置の操作装置においは、指定する直交座標は２次元（Ｘ軸、Ｙ軸）であり、３次元ワークに作業する場合は、ある高さを基準としたＸＹ平面上に投影したＸ座標、Ｙ座標を折れ角と高さＺから算出するか、またはＸＹ軸のジョグボタン等の押し操作ボタンを使って各ポジションをティーチングする必要があった。
また、ジョグボタン操作においても、ＸＹ軸の２次元での操作しかできなかった為、３次元ワークの高さ方向（Ｘ軸方向）を調整したい場合に、操作が直感的に分かり辛いという課題があった。具体例を挙げると、立ち姿勢の円筒状ワークの側面に高さ方向に沿って塗布等の処理を行いたい場合があり、その場合、エンドエフェクタを上下方向にのみ動作させることになるが、このように高さ方向にのみ移動させる場合にも、ＸＹ軸の２次元での操作で入力することになるため、操作が分かり難い。

この発明は、先端姿勢を高さ方向にのみ変更させる動作の入力が、高さ方向の値で直接に行えて、高さ方向にのみ変更させる動作の指令の入力が簡単に行え、入力操作時間の短縮が図れるリンク作動装置の制御方法を提供することである。
この発明の他の目的は、Ｘ、Ｙ、Ｚの３次元の直交座標系で入力操作が行えるようにして、入力操作の簡易化、効率化を図ることである。

この発明のリンク作動装置の制御方法は、基端側のリンクハブ２に対し先端側のリンクハブ３を、３組以上のリンク機構４を介して姿勢を変更可能に連結し、前記各リンク機構４は、それぞれ前記基端側のリンクハブ２および先端側のリンクハブ３に一端が回転可能に連結された基端側および先端側の端部リンク部材５，６と、これら基端側および先端側の端部リンク部材５，６の他端に両端がそれぞれ回転可能に連結された中央リンク機構７７とでなり、前記各リンク機構４は、このリンク機構４を直線で表現した幾何学モデルが、前記中央リンク機構７の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構４のうち２組以上のリンク機構４に、前記基端側のリンクハブ２に対する前記先端側のリンクハブ３の姿勢である先端姿勢を変更させるアクチュエータ５３が設けられ、これらアクチュエータ５３を制御する制御装置５４が設けられ、前記先端側のリンクハブ３に３次元形状のワークに対し作業を行うエンドエフェクタ１２２が取付けられたリンク作動装置において、前記先端姿勢を変更させる目標値を入力する制御方法であって、
前記リンク作動装置には、
前記目標値を、前記基端側のリンクハブ２の中心軸上に原点が位置する３次元の直交座標系上の３次元直交座標により入力可能な目標値入力手段５７が設けられ、かつこの目標値入力手段５７により入力された値を用いて、前記基端側のリンクハブ２の中心軸に対して前記先端側のリンクハブ３の中心軸が傾斜した垂直角度である折れ角θと、前記基端側のリンクハブ２の中心軸に対して前記先端側のリンクハブ３の中心軸が傾斜した水平角度である旋回角φとによって表される前記リンク作動装置の前記先端姿勢を計算し、その計算結果から前記各アクチュエータ５３の指令動作量を計算して前記制御装置５４に入力する入力変換手段５８が設けられ、
前記制御方法の過程として、
前記３次元形状のワークのＣＡＤデータから前記エンドエフェクタ１２２が作業を行う前記３次元の直交座標系上の３次元直交座標を得てこの３次元直交座標を、前記目標値入力手段５７に前記目標値として入力する過程と、
前記入力された前記目標値である前記３次元直交座標から前記入力変換手段５７が前記先端姿勢の前記計算を行い前記各アクチュエータ５３の前記指令動作量を計算して前記制御装置５４に入力する過程とを含み、
前記先端姿勢の前記計算を行う過程は、
前記３次元直交座標から先端側の前記リンクハブ３の前記折れ角θと旋回角φを最小自乗法による収束演算で求める過程を含み、
この収束演算で前記折れ角θ求めるステップとして、
前記３次元の直交座標系における高さｙの平面の原点Ｏ’と、前記次元直交座標による前記目標値である目標座標Ｔ（ｘ，ｙ，ｚ）との距離ｒを、次式（１）

によって求めるステップと、
前記基端側のリンクハブ２の回転中心Ｐから前記目標座標Ｔまでの高さｈ’を、前記リンク作動装置のリンク球面中心間距離ｄ、基準平面（前記座標系の原点Ｏを含むｘｙ平面）から基端側リンク球面中心ＰＡまでの高さｈ、及び前記目標座標Ｔの高さｚを用いて、次式（２）

によって求めるステップとを含む。

この方法によると、例えば、目標値入力手段５７による人為操作で、目標とする先端姿勢を指定する。これにより、入力変換手段５８は、目標値入力手段５７から入力された各値を用いて、折れ角θと旋回角φとによって表される先端姿勢を求め、計算結果から前記各アクチュエータ５３の指令動作量を計算して制御装置５３に入力する。制御装置５３は、入力された指令動作量に従って各アクチュエータ５３を制御し、目標の先端姿勢になるように制御する。
また、前記入力変換手段５８は、前記目標値入力手段５７により入力された値を用いて、前記折れ角θと前記旋回角φとによって表される前記リンク作動装置の前記先端姿勢を計算するにつき、最小自乗法による収束演算を用いるため、先端姿勢を表す適切な折れ角θおよび旋回角φが簡単な演算で得られる。

この発明において、前記目標値入力手段５７は、前記基端側のリンクハブ２の中心軸上に原点が位置する３次元直交座標上における各方向の座標位置を入力する３次元直交座標入力部５７Ａを有するようにしても良い。なお、上記「リンクハブの中心軸上」とは、リンクハブの無限長さの軸心上を意味する。
先端側のリンクハブ３に取付けられたエンドエフェクタ１２２が作業する３次元ワークの３次元直交座標（Ｘ，Ｙ，Ｚ）は、ＣＡＤデータ等から容易に算出することができ、その３次元座標（Ｘ，Ｙ，Ｚ）を前記３次元直交座標入力部５７Ａで入力することで、エンドエフェクタ１２２はその入力されたポジションへ作業することになり、ジョグ送り等によるティーチング作業が不要になる。エンドエフェクタ１２２の位置決め精度にバラつきがある場合、微調整のティーチングが必要となるが、おおよその位置決めは出来ていることから、ティーチングにかかる時間は大幅に短縮できる。
ティーチングを行う際も、前記３次元直交座標におけるＸＹ軸方向の押し操作ボタン８１〜８４とＺ軸方向（前記高さ方向）の押し操作ボタン８５，８６があれば、直感的なティーチングが可能となる。

この発明において、前記入力変換手段５８は、前記基端側のリンクハブ２に対する前記基端側の端部リンク部材５の回転角をβｎ、前記基端側の端部リンク部材６に回転自在に連結された中央リンク機構７の連結端軸と、前記先端側の端部リンク部材６に回転自在に連結された中央リンク機構７の連結端軸とが成す角度をγ、基準となる基端側の端部リンク部材５に対する各基端側の端部リンク部材５の円周方向の離間角をδｎ、前記折れ角をθ、前記旋回角をφとした場合に、
cos （θ／２）sin βｎ−sin （θ／２）sin （φ＋δｎ）cos βｎ＋sin （γ／２）＝０
で表される式を逆変換することにより、目標とする前記先端姿勢における前記各基端側の端部リンク部材５の回転角を求め、この求められた回転角と、現在の前記先端姿勢における前記各基端側の端部リンク部材５の回転角との差分から、前記各アクチュエータ５３の前記指令動作量を算出するようにしても良い。
この手法によると、容易に指令動作量を求めることができ、アクチュエータ５３の制御が簡易となる。

また、この発明において、前記入力変換手段５８は、前記基端側のリンクハブ２に対する前記基端側の端部リンク部材５の回転角をβｎ、前記基端側の端部リンク部材５に回転自在に連結された中央リンク機構７の連結端軸と、前記先端側の端部リンク部材６に回転自在に連結された中央リンク機構７の連結端軸とが成す角度をγ、基準となる基端側の端部リンク部材５に対する各基端側の端部リンク部材５の円周方向の離間角をδｎ、前記折れ角をθ、前記旋回角をφとした場合に、
cos （θ／２）sin βｎ−sin （θ／２）sin （φ＋δｎ）cos βｎ＋sin （γ／２）＝０
で表される式を逆変換することにより、前記先端姿勢と前記各基端側の端部リンク部材５の回転角との関係を示すテーブルを作成しておき、このテーブルを用いて、目標とする前記先端姿勢における前記各基端側の端部リンク部材５の回転角を求め、この求められた回転角と、現在の前記各基端側の端部リンク部材５の回転角との差分から、前記各アクチュエータ５３の指令動作量を算出するようにしても良い。
この手法によると、事前に先端姿勢と基端側のリンクハブ２の各回転角との関係をテーブル化しておくことにより、上記式を用いた指令動作量の計算時間を短くすることができ、アクチュエータ５３の制御をより一層高速で行うことができる。

この発明において、前記目標値入力手段５７が前述のように３次元直交座標入力部５７Ａを有する場合に、前記目標値入力手段５７の前記３次元直交座標入力部５７Ａによる３次元直交座位置の指定は数値入力であっても良い。
３次元直交座位置の指定は、数値入力の他に、押し操作入力部５７Ｂ等による入力としても良いが、前述のようにエンドエフェクタ１２２が作業する３次元ワークの３次元直交座標（Ｘ，Ｙ，Ｚ）は、ＣＡＤデータ等から容易に算出することができるため、前記次元直交座位置の指定は数値入力で行うようにすることが、入力操作の簡易化、効率化の面で好ましい。

このように３次元直交座位置の指定を数値入力とする場合に、前記目標値入力手段５７は、前記３次元直交座標入力部５７Ａによる３次元直交座位置の指定を、定められた基準点に対する絶対座標で数値入力させることで行うようにしても良い。
３次元直交座位置の指定を、定められた基準点に対する絶対座標で数値入力させるようにすると、目標となる先端姿勢の絶対座標の座標位置が分かっている場合に、入力が容易となる。

また、３次元直交座位置の指定を数値入力とする場合に、前記目標値入力手段５７の前記３次元直交座標入力部５７Ａによる３次元直交座位置の指定を、現在の座標位置から目標とする座標位置までの差分を数値入力させることで行うようにしても良い。
３次元直交座位置の指定を、差分を数値入力で行うようにする場合は、現在の先端姿勢から少しの移動を行わせるような場合に、入力が直感的に行えて容易となる。

この発明において、前記入力変換手段５８は、前記目標値入力手段５７から与えられる前記先端姿勢の情報を、定められた変換式により前記アクチュエータ５３の動作量に変換し、前記制御手段５４に入力するようにしても良い。
前記目標値入力手段５７から行う入力は、先端姿勢の３次元直交座位置の座標位置や移動量であるが、先端姿勢の３次元直交座とアクチュエータ５３の動作量とはリンク作動装置の構成によって定まった関係があるため、定められた変換式でアクチュエータ５３の動作量に変換するようにすれば、目標値入力手段５７の入力から３次元直交座等を入力することで、アクチュエータ５３の正しい制御が行える。

この発明のリンク作動装置の制御方法は、先端姿勢を高さ方向にのみ変更させる動作の入力が、高さ方向の値で直接に行えて、高さ方向にのみ変更させる動作の指令の入力が簡単に行え、入力操作時間の短縮が図れる。

この発明の一実施形態に係るリンク作動装置の制御方法に用いる操作装置におけるパラレルリンク機構の斜視図および操作系，制御系の概念構成を示す説明図である。 （Ａ），（Ｂ）は、同リンク作動装置の操作装置における各座標のジョグ入力操作画面の説明図である。 同リンク作動装置におけるパラレルリンク機構の斜視図である。 同リンク作動装置におけるパラレルリンク機構の部分省略正面図である。 同リンク作動装置の動作説明図である。 同リンク作動装置の一つのリンク機構を直線で表現した模式図である。 同リンク作動装置のパラレルリンク機構の部分断面図である。 同リンク作動装置の操作装置における折れ角を求める計算のフローチャートである。 同リンク作動装置を用いた作業装置の一例の正面図である。 従来のリンク作動装置におけるパラレルリンク機構の斜視図である。 同パラレルリンク機構の動作説明図である。

この発明の一実施形態に係るリンク作動装置の制御方法に用いる操作装置を図１〜図９と共に説明する。図１は、同操作装置を備えたリンク作動装置を示す。このリンク作動装置５１は、パラレルリンク機構１と、このパラレルリンク機構１の先端姿勢を任意に変更させる複数（後記リンク機構４と同数）のアクチュエータ５３と、これらアクチュエータ５３を制御する制御装置５４と、この制御装置５４に操作指令を入力する操作装置５５とを備える。制御装置５４および操作装置５５は、共通の筐体に設けられて一つのコントローラ（図示せず）を構成していても、また互いに別個に離れて設けられていても良い。パラレルリンク機構１は、ベース部材５２に吊り下げ状態で設置されている。

パラレルリンク機構１から説明する。図５（Ａ），（Ｂ）はパラレルリンク機構１のそれぞれ異なる状態を示す正面図である。このパラレルリンク機構１は、基端側のリンクハブ２に対し先端側のリンクハブ３を３組のリンク機構４を介して姿勢変更可能に連結したものである。図５では、１組のリンク機構４のみが示されている。先端側のリンクハブ３には、例えば図９に例示するようにエンドエフェクタ１２２が設置される。

図３は、パラレルリンク機構１を三次元的に表わした斜視図である。なお、図３のパラレルリンク機構１は、図１０のパラレルリンク機構１と同じであるが、ここで具体的に説明する。各リンク機構４は、基端側の端部リンク部材５、先端側の端部リンク部材６、および中央リンク部材７で構成され、４つの回転対偶からなる３節連鎖のリンク機構をなす。基端側および先端側の端部リンク部材５，６はＬ字状をなし、基端がそれぞれ基端側のリンクハブ２および先端側のリンクハブ３にそれぞれ回転自在に連結されている。中央リンク部材７は、両端に基端側および先端側の端部リンク部材５，６の先端がそれぞれ回転自在に連結されている。

基端側および先端側の端部リンク部材５，６は球面リンク構造で、３組のリンク機構４における球面リンク中心ＰＡ，ＰＢ（図５）は一致しており、また、その球面リンク中心ＰＡ，ＰＢからの距離ｄも同じである。端部リンク部材５，６と中央リンク部材７との各回転対偶の中心軸は、ある交差角γをもっていてもよいし、平行であってもよい。

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

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

基端側のリンクハブ２と先端側のリンクハブ３と３組のリンク機構４とで、基端側のリンクハブ２に対し先端側のリンクハブ３が直交２軸方向に移動自在な２自由度機構が構成される。言い換えると、基端側のリンクハブ２に対して先端側のリンクハブ３を、回転が２自由度で姿勢変更自在な機構である。この２自由度機構は、基端側のリンクハブ２の中心軸ＱＡ、先端側のリンクハブ３の中心軸ＱＢ、および中央リンク部材７の中心線Ｃの交点Ｐを中心として、基端側のリンクハブ２に対して先端側のリンクハブ３が姿勢を変更する。

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

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

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

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

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

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

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

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

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

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

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

軸受１２を基端側のリンクハブ２および先端側のリンクハブ３に埋設状態で設けたことにより、パラレルリンク機構１全体の外形を大きくすることなく、基端側のリンクハブ２および先端側のリンクハブ３の外形を拡大することができる。そのため、基端側のリンクハブ２および先端側のリンクハブ３を他の部材に取付けるための取付スペースの確保が容易である。

図４において、パラレルリンク機構１は、基端側のリンクハブ２がベース部材５２の下面に固定され、先端側のリンクハブ３が垂下した状態となっている。ベース部材５２の上面には、モータまたは他のロータリアクチュエータからなるアクチュエータ５３が、リンク機構４と同数すなわち３個設置されている。アクチュエータ５３の出力軸５３ａはベース部材５２を貫通して下方に突出し、その出力軸５３ａに取付けたかさ歯車５７と基端側のリンクハブ２の軸部材１３（図６）に取付けた扇形のかさ歯車５８（図４）とが噛み合っている。

アクチュエータ５３を回転させると、その回転が一対のかさ歯車５７，５８を介して軸部材１３に伝達されて、基端側のリンクハブ２に対する基端側の端部リンク部材５の角度が変わる。各アクチュエータ５３の動作量を制御して、リンク機構４ごとに基端側の端部リンク部材５の角度を調整することにより、基端側のリンクハブ２に対する先端側のリンクハブ３の姿勢（以下、「先端姿勢」とする）が決まる。各アクチュエータ５３の動作は、図１の操作装置５５の操作指令に基づき、制御装置５４により制御される。

図１において、操作装置５５は、パネル型コンピュータで構成した例を示しており、同図には液晶表示装置等の画像を表示可能な表示装置５６を、操作装置５５を示すブロック部分の右側に図示している。表示装置５６は、タッチパネル形式の入力装置を兼ねる。操作装置５５は、先端位置の目標位置を入力する目標値入力手段５７と、入力変換手段５８とを有している。なお、入力変換手段５８は、目標値入力手段５７を構成するコンピュータに必ずしも設けなくても良く、制御装置５４を構成するコンピュータに設けても良い。

前記目標値入力手段５７は、３次元直交座標を指定して入力する構成とされ、各軸（Ｘ軸、Ｙ軸、Ｚ軸）の目標値をそれぞれ入力する横方向目標値入力部５７ｘ、縦方向目標値入力部５７ｙおよび高さ方向目標値入力部５７ｚを有している。前記目標値入力手段５７は、前記各軸の座標位置を数値で入力させる３次元直交座標入力部５７Ａと、押し操作ボタン（ジョグボタン）８１〜８６（図２）の押し時間または操作回数で指定する押し操作入力部５７Ｂ（図１）との２種類の入力形態を選択して操作可能とされている。これら３次元直交座標入力部５７Ａおよび押し操作入力部５７ＢにおけるＸ軸についての入力部を前記横方向目標値入力部５７ｘとして総称し、同じくＹ軸についての入力部を前記縦方向目標値入力部５７ｙとして総称し、同じくＺ軸についての入力部を前記高さ方向目標値入力部５７ｙとして総称する。

前記３次元直交座標入力部５７Ａにおける各軸の入力部は、前記表示装置５６の画面Ｇ上に入力ボックス等と呼ばれる目標値表示部６２と、テンキー等の数値入力ボタン６５とを表示させ、目標値表示部６２に表示させる数値を数値入力ボタン６５の画面タッチによる操作によって指定することで目標の座標位置を入力させるようにしている。数値入力は、前記数値入力ボタン６５による入力に代えて、ハードウェアキー等の入力装置（図示せず）から、目標の座標位置を入力させるようにしても良い。前記画面Ｇ上には、各数値入力ボックス６２と並んで、前記各軸の現在位置を座標位置の数値で表示する現在位置表示部６３と、いずれの軸方向であるかを示す「Ｘ座標」等の案内表示６３ａとが、前記３次元直交座標入力部５７Ａによって表示出力手段（図示せず）によって表示される。前記３次元直交座標は、基端側のリンクハブ２の中心軸上に原点が位置する任意の３次元直交座標であり、そのＺ軸方向を高さ方向と称する。

前記画面Ｇには、動作実行ボタン６４が表示され、この動作実行ボタン６４が画面タッチ等によりオン操作されると、３次元直交座標入力部５７Ａで入力された各入力値が、入力変換手段５８により各アクチュエータ５３の指令動作量に変換され、この指令動作量が制御装置５４に入力される。前記画面Ｇには、これらの他に、画面上の位置を示すカーソル（図示せず）の位置を変更させる矢印ボタンや、画面切換ボタン（いずれも図示せず）等が表示される。

制御装置５４は、前記指令動作量だけ移動するように各アクチュエータ５３を駆動する。制御装置５４は、各アクチュエータ５３に設けられた回転角センサ等の位置検出器（図示せず）の検出信号を用い、各アクチュエータ５３をフィードバック制御する。

なお、上記の例では、目標入力手段５７の３次元直交座標入力部５７Ａは、前記３次元直交座標上の定められた基準点（例えば原点Ｏ）に対する絶対座標を入力する方式としているが、現在の座標位置から目標とする座標位置までの差分を数値入力する方式としても良い。
また、前記目標値入力手段５７は、図１の画面Ｇで示された座標位置の入力を行う入力形成の他に、画面Ｇの表示内容を図２のように切り換えて押し操作ボタン８１〜８６による入力形式に切換可能とされている。この押し操作ボタンによる入力については、後に説明する。

図１において、前記入力変換手段５８は、前記目標値入力手段５７により入力された値を用いて、折れ角θと旋回角φとによって表されるリンク作動装置５１の姿勢を計算し、その計算結果から前記各アクチュエータ５３の指令動作量を計算して制御装置５４に入力する手段である。前記リンク作動装置５１の姿勢は、パラレルリンク機構１の姿勢を言う。前記折れ角θは、基端側のリンクハブ２の中心軸ＱＡ（図３）に対して先端側のリンクハブ３の中心軸ＱＢが傾斜した垂直角度を言う。前記旋回角φは、基端側のリンクハブ２の中心軸ＱＡに対して先端側のリンクハブ３の中心軸ＱＢが傾斜した水平角度を言う。

図８に、前記入力変換手段５８によって前記３次元直交座標（ＸＹＺ平面座標）から先端側のリンクハブ３の姿勢（折れ角θ、旋回角φ）を求める計算フローチャートと計算式を示す。同図の計算は、最小自乗法による収束演算で行うようにしている。
同図（Ｂ）の式（１）に示すように、高さＺ上の平面の原点Ｏ’と目標座標Ｔ（ｘ，ｙ，ｚ）との距離をｒとすると、ｒは目標座標Ｔのｘ，ｙ座標から求められる。

また、基端側のリンクハブ２の回転中心から目標座標Ｔまでの高さｈ’は、リンク球面中心間距離ｄ及び、基準平面から基端側リンク球面中心までの高さｈ、目標座標Ｔの高さｚで表すと、式（２）のように表される。

ここでｄ及びｈは、リンク寸法及び装置寸法により決定される固定値である。また、前述した式（２）を用いて、Ｏ’と目標座標Ｔとの距離は式（３）に示すように、θに関係する式からｒ’として求められる。よって、式（４）に示すようにｒ（目標座標Ｔのｘ，ｙから算出）とｒ’（折れ角θから算出）との差が最小となる折れθを探索することで折れ角θを得る。

として、drが最小となるθを探索する。
θの範囲は0°〜90°未満でリンクの仕様により最大角は変わる。

図８（Ａ）の計算フローチャートについて説明する。
まず、式（１）からｒを求める（ステップＳ１）。次に、計算フローの初期設定として、現在の繰返し回数ｊをゼロリセットし、折れ角θを探索する際の初期設定値θ’と、探索の繰返し回数Ｎを設定する。例えば、θ’は最大折れ角の半分の値に設定する。繰返し回数Ｎは、値が大きいほど最終的に得られる折れ角θの精度は良くなるが、計算フローの処理時間は長くなる。折れ角θの精度と、許容される処理時間の関係から最適なＮを決定する。

次に、θ＝θ’として、式（１）でｈ’、r ’、ｄｒを計算する（ステップＳ２）。
また、ここでθ’＝θ’／２とする（ステップＳ３）。

次にステップＳ４でｄｒの判定を行う。０＜ｄｒなら、θ＝θ＋θ’とし（ステップＳ５）、０＞ｄｒなら、θ＝θ−θ’とする（ステップＳ６）。この後、ｊ＝ｊ＋１とインクリメントする（ステップＳ７）。また、０＝ｄｒなら、解Ａｎｓθとして、θ＝θとする（ステップＳ９）。

ステップＳ８で、で現在の繰返し回数ｊが設定された繰返し回数Ｎに達していないときは、ステップＳ３へ戻る。このフローを繰返し回数Ｎまで繰り返し、最後に得られたＡｎｓθが、求める折れ角θとなる。折れ角を求めた後、式（５）、（６）に示すように旋回角を求める。

このように求められた折れ角θおよび旋回角φにより、目標とする先端姿勢が規定される。上記のように、最小二乗法による収束演算により、現在の座標位置を基準にその近辺から順に探索して折れ角θを求めると、演算回数を減らすことができる。

図１の入力変換手段５８は、上記のように求めた折れ角θおよび旋回角φから、各アクチュエータ５３を動作させる目標となる回転角βｎ（ｎ：１〜３）を求める。なお、回転角βｎは、図３と同じパラレルリンク機構１を示す図１０に図示した。
回転角βｎは、例えば、次の式（７）を逆変換することで求められる。逆変換とは、折れ角θおよび旋回角φから回転角βｎを算出する変換のことである。折れ角θおよび旋回角φと、回転角βｎとは相互関係があり、一方の値から他方の値を導くことができる。
cos （θ／２）sin βｎ−sin （θ／２）sin （φ＋δｎ）cos βｎ＋sin （γ／２）＝０ …（７）
ただし、ｎ＝１，２，３
ここで、γ（図１，図３）は、基端側の端部リンク部材５に回転自在に連結された中央リンク部材７の連結端軸と、先端側の端部リンク部材６に回転自在に連結された中央リンク部材７の連結端軸とが成す角度である。δｎは、基準となる基端側の端部リンク部材５に対する各基端側の端部リンク部材５の円周方向の離間角である。

指令ごとに式（７）を逆変換して回転角βｎを求めてもよいが、事前に、表１のように先端位置姿勢と回転角βｎとの関係を示すテーブルを作成しておいても良い。テーブル化されていると、先端姿勢変更の指令があれば直ぐにテーブルを用いて目標とする回転角βｎを求めることができる。そのため、アクチュエータ５３の制御をより一層高速で行うことが可能となる。また、予め指令パターンを登録して登録順に動作する場合は、表２のようにパターン登録時に先端位置姿勢と回転角βｎとの関係を示すテーブルを登録することで、テーブルの記憶領域を節約できる。

図２は押し操作ボタン８１〜８６により所定位置まで移動させる入力を行うようにした操作画面の例を示し、同図（Ａ）は図１の横方向目標値入力部５７ｘおよび縦方向入力部５７ｙの押し操作入力部５７Ｂの部分によってＸＹ軸方向に移動させるためのジョグ操作画面であり、画面Ｇには、押し操作ボタン８１〜８４と、現在の座標位置におけるＸ座標位置およびＹ座標位置をそれぞれ示す現在値表示部６７，６８とが表示されている。
同図（Ｂ）は、図１の高さ方向目標値入力部５７ｚによってＺ軸方向（高さ方向）に移動させるためのジョグ操作画面であり、画面Ｇには、押し操作ボタン８５，８６と、現在の座標位置におけるＺ座標位置を示す現在値表示部６９とが表示されている。
上記の各押し操作ボタン８１〜８６は、いずれも、操作時間または操作回数により操作量が変わる操作手段である。具体的には、これら押し操作ボタン８１〜８６は、押し続ける(画面タッチを続ける)ことによって操作量が増え続け、ごく短時間の押し操作(画面タッチ)であると１回の操作の操作量が一定であって、繰り返して操作して操作回数を増やすことで操作量が増える。

Ｘ，Ｙ軸の操作画面Ｇ（同図（Ａ））およびＺ軸の操作画面Ｇ（同図（Ｂ））のいずれも、前記押し操作ボタン８１〜８６は、各軸方向の移動方向の正逆の向き毎に別々に設けられ、かつ同じ軸方向で同じ方向の移動の入力用として、低速ボタン８１ａ〜８６ａと、高速ボタン８１ｂ〜８６ｂが設けられ、姿勢変更の指令の入力が低速（操作量の増加速度が低速）および高速（操作量の増加速度が高速）の２段階で行えるようにされている。また、前記各低速ボタン８１ａ〜８６ａと高速ボタン８１ｂ〜８６ｂにおける１回の短時間の場合の移動量をそれぞれ示す単位移動量表示７１，７２が、前記画面Ｇに設けられている。この画面Ｇには、さらに、各押し操作ボタン８１〜８６の表示部がＸ，Ｙ，Ｚのどの座標を示すか、または正逆のどの方向に対応するか等の案内の表示が、矢印やＸ，Ｙ，Ｚの文字等で表示されている。

図２（Ａ），（Ｂ）に示す押し操作ボタン８１〜８６で移動量を入力すると、図１の目標値入力手段５７の押し操作入力部５７Ｂは、その入力された移動量（操作量）だけ、現在の座標位置に加えた座標位置を目標の座標位置として、入力変換手段５８に与え、入力変換手段５８は、前記と同様に折れ角θと旋回角φによる先端姿勢を求め、各アクチュエータ５３の指令動作量を演算する。

具体的には、高さ方向（Ｚ軸）については、目標値入力手段５７は、押し操作ボタン８５が操作されたときは（８）式に従って、押し操作ボタン８６が操作されたときは（９）式に従って、リンクハブの目標座標Ｔからの高さｈ′の計算を行う。

（８）式における変数Ｚは、上向きの低速ボタン６５ａの押し下げの場合は、図２（Ｂ）の移動量表示部７１に表示された値とし、上向きの高速ボタン６５ｂの押し下げの場合は、同図の移動量表示部７２に表示された値とし、設定操作時間毎に計算を繰り返す。
（９）式における変数Ｚは、下向きの低速ボタン６６ａの押し下げの場合は、図２（Ｂ）の移動量表示部７１に表示された値とし、下向きの高速ボタン６６ｂの押し下げの場合は、同図の移動量表示部７２に表示された値とし、設定操作時間毎に計算を繰り返す。

このように、図２の押し操作ボタン８１〜８６による操作の場合は、押し操作ボタン８１〜８６の操作によりＸ，Ｙ，Ｘが逐次変更され、その都度、目標とする折れ角θおよび旋回角φを計算し、それに応じたアクチュエータ５３の動作量を決定する構成となっている。つまり、押し操作ボタン８１〜８６を押している間だけ、先端姿勢が変更し続ける。そのため、操作と座標位置との関係が感覚的に分かり易い。

なお、図２の前記押し操作ボタン８１〜８６は、ジョイスティックのような操作手段であっても良い。

この実施形態の作用、効果につき纏め直して説明すると、リンク作動装置５１の姿勢延長上の範囲にある任意の３次元直交座標を指定して、その３次元直交座標から演算により自動で先端側のリンクハブ１３の姿勢（折れ角θ、旋回角φ）を計算して位置決めする。この場合に、高さ方向（Ｚ方向）の押し操作ボタン（ジョグボタン）８５，８６とその移動量パラメータを追加し、現在値とそのＺ軸向押し操作ボタン８５，８６の操作による移動量から演算によりリンクハブ１３の姿勢（折れ角θ、旋回角φ）を計算して位置決めする。押し操作ボタン８５，８６の押下中、この位置決め動作を繰り返すことでＺ軸方向のジョグ機能を実現する。

エンドエフェクタ１２２（図９参照）が作業する３次元ワークの３次元直交座標（Ｘ，Ｙ，Ｚ）は、ＣＡＤデータ等から容易に算出することができ、その３次元座標（Ｘ，Ｙ，Ｚ）を登録することで、エンドエフェクタ１２２はその登録されたポジションへ作業することになり、ジョグ送り操作によるティーチング作業が不要になる。エンドエフェクタ１２２の位置決め精度にバラつきがある場合、微調整のティーチングが必要となるが、おおよその位置決めは出来ていることから、ティーチングにかかる時間は大幅に短縮できる。
ティーチングを行う際も、ＸＹ軸方向の押し操作ボタン８１〜８４とＺ軸方向の押し操作ボタン８５，８６により、直感的なティーチングが可能となる。

図９は、上記リンク作動装置５１を備えた作業装置を示す。この作業装置１２０は、作業室１２１の天井部を構成するベース部材５２に基端側のリンクハブ２を固定して、リンク機構本体１が吊り下げ状態で設置されており、このリンク機構本体１の先端側のリンクハブ３にエンドエフェクタ１２２が搭載されている。エンドエフェクタ１２２は、例えば塗装機である。

エンドエフェクタ１２２の下方には、被作業物ＷをＸＹ軸方向に移動させる移動機構１２３が設置されている。移動機構１２３は、床面に固定して設置されたＸ軸方向に沿って長いＸ軸レール１２４と、このＸ軸レール１２４に沿って進退自在でＹ軸方向に沿って長いＹ軸レール１２５と、このＹ軸レール１２５に沿って進退自在で上面に被作業物が載せられる作業台１２６とを備える。Ｙ軸レール１２５および作業台１２６は、それぞれ図示しない駆動源の駆動によりＸ軸方向およびＹ軸方向に移動させられる。

エンドエフェクタ１２２が塗装機である場合、移動機構１２３による作業台１２６をＸ軸およびＹ軸方向に進退させることで、被作業物Ｗの塗装箇所を塗装機の塗料噴射口１２２ａの先に位置させる。また、リンク作動装置５１の先端姿勢を変更してエンドエフェクタ１２２の向きを変えることで、常に前記塗料噴射口１２２ａが被作業物Ｗの塗装面に向くように調整する。

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

２…基端側のリンクハブ
３…先端側のリンクハブ
４…リンク機構
５…基端側の端部リンク部材
６…先端側の端部リンク部材
７…中央リンク部材
５１…リンク作動装置
５３…アクチュエータ
５４…制御装置
５６…表示装置
５７…目標値入力手段
５７Ａ…３次元直交座標入力部
５７Ｂ…押し操作入力部
５７ｘ…横方向目標値入力部
５７ｙ…縦方向目標値入力部
５７ｚ…高さ方向目標値入力部
６１…入力変換手段
６２…目標値表示部
６５…数値入力ボタン
８１〜８６…押し操作ボタン
Ｏ…原点
ＱＡ…基端側のリンクハブの中心軸
ＱＡ´…延長軸
ＱＢ…先端側のリンクハブの中心軸
θ…折れ角
φ…旋回角

Claims (4)

1. 基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構のうち２組以上のリンク機構に、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢である先端姿勢を変更させるアクチュエータが設けられ、これらアクチュエータを制御する制御装置が設けられ、前記先端側のリンクハブに３次元形状のワークに対し作業を行うエンドエフェクタが取付けられたリンク作動装置において、前記先端姿勢を変更させる目標値を入力する制御方法であって、
   前記リンク作動装置には、
   前記目標値を、前記基端側のリンクハブの中心軸上に原点が位置する３次元の直交座標系上の３次元直交座標により入力可能な目標値入力手段が設けられ、かつこの目標値入力手段により入力された値を用いて、前記基端側のリンクハブの中心軸に対して前記先端側のリンクハブの中心軸が傾斜した垂直角度である折れ角と、前記基端側のリンクハブの中心軸に対して前記先端側のリンクハブの中心軸が傾斜した水平角度である旋回角とによって表される前記リンク作動装置の前記先端姿勢を計算し、その計算結果から前記各アクチュエータの指令動作量を計算して前記制御装置に入力する入力変換手段が設けられ、
   前記制御方法の過程として、
   前記３次元形状のワークのＣＡＤデータから前記エンドエフェクタが作業を行う前記３次元の直交座標系上の３次元直交座標を得てこの３次元直交座標を、前記目標値入力手段に前記目標値として入力する過程と、
   前記入力された前記目標値である前記３次元直交座標から前記入力変換手段が前記先端姿勢の前記計算を行い前記各アクチュエータの前記指令動作量を計算して前記制御装置に入力する過程とを含み、
   前記先端姿勢の前記計算を行う過程は、
   前記３次元直交座標から先端側の前記リンクハブの前記折れ角θと旋回角φを最小自乗法による収束演算で求める過程を含み、
   この収束演算で前記折れ角θ求めるステップとして、
   前記３次元の直交座標系における高さｙの平面の原点Ｏ’と、前記３次元直交座標による前記目標値である目標座標Ｔ（ｘ，ｙ，ｚ）との距離ｒを、次式（１）
   

   

   によって求めるステップと、
   前記基端側のリンクハブの回転中心から前記目標座標Ｔまでの高さｈ’を、前記リンク作動装置のリンク球面中心間距離ｄ、基準平面（前記座標系の原点Ｏを含むｘｙ平面）から基端側リンク球面中心までの高さｈ、及び前記目標座標Ｔの高さｚを用いて、次式（２）
   

   

   によって求めるステップとを含む、
   リンク作動装置の制御方法。
2. 請求項１に記載のリンク作動装置の制御方法において、前記入力変換手段は、前記基端側のリンクハブに対する前記基端側の端部リンク部材の回転角をβｎ、前記基端側の端部リンク部材に回転自在に連結された中央リンク部材の連結端軸と、前記先端側の端部リンク部材に回転自在に連結された中央リンク部材の連結端軸とが成す角度をγ、基準となる基端側の端部リンク部材に対する各基端側の端部リンク部材の円周方向の離間角をδｎ、前記折れ角をθ、前記旋回角をφとした場合に、
   cos （θ／２）sin βｎ−sin （θ／２）sin （φ＋δｎ）cos βｎ＋sin （γ／２）＝０
   で表される式を逆変換することにより、目標とする前記先端姿勢における前記各基端側の端部リンク部材の回転角を求め、この求められた回転角と、現在の前記先端姿勢における前記各基端側の端部リンク部材の回転角との差分から、前記各アクチュエータの前記指令動作量を算出するリンク作動装置の制御方法。
3. 請求項１に記載のリンク作動装置の制御方法において、前記入力変換手段は、前記基端側のリンクハブに対する前記基端側の端部リンク部材の回転角をβｎ、前記基端側の端部リンク部材に回転自在に連結された中央リンク部材の連結端軸と、前記先端側の端部リンク部材に回転自在に連結された中央リンク部材の連結端軸とが成す角度をγ、基準となる基端側の端部リンク部材に対する各基端側の端部リンク部材の円周方向の離間角をδｎ、前記折れ角をθ、前記旋回角をφとした場合に、
   cos （θ／２）sin βｎ−sin （θ／２）sin （φ＋δｎ）cos βｎ＋sin （γ／２）＝０
   で表される式を逆変換することにより、前記先端姿勢と前記各基端側の端部リンク部材の回転角との関係を示すテーブルを作成しておき、このテーブルを用いて、目標とする前記先端姿勢における前記各基端側の端部リンク部材の回転角を求め、この求められた回転角と、現在の前記各基端側の端部リンク部材の回転角との差分から、前記各アクチュエータの指令動作量を算出するリンク作動装置の制御方法。
4. 請求項１ないし請求項３のいずれか１項に記載のリンク作動装置の制御方法において、前記目標値入力手段による３次元直交座位置の指定は数値入力であるリンク作動装置の制御方法。

Images