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

Abstract

【課題】従来構造よりも、リンク姿勢の変更に伴う演算処理負荷の低減を図り、かつ、位置決め精度を向上することができるリンク作動装置の制御装置を提供する。【解決手段】リンク作動装置１の制御装置４は、指令変換部４２と、アーム設定部５０と、アーム制御部３９とを備える。指令変換部４２は、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢を変更するとき、目標のリンク姿勢（θ、φ）に対応する各アーム１１ａ～１３ａの回転角を逆変換により算出する。アーム設定部５０は、算出された各アーム１１ａ～１３ａの回転角から、２つのアームを駆動する駆動アームに設定し、残りのアームをフリーアームとする。アーム制御部３９は、設定された２つの駆動アームのみを駆動し位置決めが完了した後、フリーアームを駆動し位置決めする。【選択図】図４

Description

本発明は、例えば、産業機器等の精密で広範な作動範囲を必要とする機器に用いられるリンク作動装置の制御装置に関する。

特許文献１には、３節連鎖のリンク機構を３組以上設け、３組以上のリンク機構のそれぞれにアクチュエータを設けたリンク作動装置が開示されている。前記リンク作動装置の各回転対偶部または機構部等に生じるガタを片側方向に寄せる力を付与し、そのときの各アクチュエータの動作量を各アクチュエータの原点位置にする方法が開示されている。これにより、どのような姿勢においても、回転対偶または機構部等に生じるガタを片側方向に寄せる力を付与した状態で姿勢制御を行うことができ、位置決め精度が向上する。

特許文献１の課題として、３節連鎖の３組のリンク機構を設けた２自由度のリンク機構を３個のモータ等のアクチュエータで駆動するリンク作動装置において、リンクの姿勢変更量が大きくなると、３つのアームは互いに連結されているため、３つのアームが過大に干渉し、より大きなトルクが必要になる。その結果、リンク機構の各回転対偶部または機構部等に荷重がかかり、リンク機構の組付け不具合および異常な摩耗等を与え、位置決め精度が悪化するという問題があった。それに対し、特許文献２のような技術が提案されている。

特許文献２では、３節連鎖のリンク機構を３組以上設け、その３組以上のリンク機構のそれぞれにアクチュエータを設けたリンク作動装置において、リンクの姿勢変更量が所定量を超える際に、始点姿勢から終点姿勢への姿勢変更経路の途中に１点以上の中継姿勢を設けている。設定された中継点を各アームが同時に通過することで、リンク機構の各部に過大な負荷を掛けずに駆動する。

特開２０１３－９６５２５号公報 特許第５９７６５８０号公報

特許文献２において、中継姿勢は、３つのアームのうち、２つのアームを選び、この選んだ２つのアームの回転経路を移動量が所定量以下になるように等分している。このとき、残り１つのアームは、他の２つのアームの回転角からリンク姿勢（θ、φ）を順変換によって算出し、算出されたリンク姿勢（θ、φ）を逆変換することで、残り１つのアームの回転角を算出している。

別の中継姿勢を求める方法として、始点姿勢（θａ、φａ）から終点姿勢（θｂ、φｂ）へ移動する際、移動距離が最短距離になる経路上に１つ以上の中継姿勢をとり、始点姿勢、中継姿勢、および終点姿勢のいずれかである各姿勢のうち、隣合う姿勢間の成す量が所定以下となる姿勢（θ、φ）を中継姿勢としている。各アームの回転角は、このリンク姿勢（θ、φ）に対応する逆変換により算出する。この方法では、始点姿勢（θａ、φａ）と終点姿勢（θｂ、φｂ）の位置情報が必要になる。

しかし、仮に非常停止ボタン押下等により、アームを回転させるモータへの電路が遮断されると、リンク姿勢が自重で変更するため、リンク姿勢（θ、φ）の現在位置情報が消失する。すると、その直後のリンク姿勢変更の際、始点姿勢（θａ、φａ）が不明になり、中継姿勢が算出できなくなる。このような場合、３つのアクチュエータのエンコーダ情報（各アームの現在位置）から、順変換により始点姿勢（θａ、φａ）を算出する必要がある。

特許文献２において、中継姿勢を求めるにはアーム回転角（β１、β２、β３）からリンク姿勢（θ、φ）を算出する順変換が必要になってくる。一般的に、本リンク機構を含むパラレルリンク機構においては、リンク姿勢（θ、φ）からアーム回転角（β１、β２、β３）を求める逆変換は容易に解ける。しかし、アーム回転角（β１、β２、β３）からリンク姿勢（θ、φ）を求める順変換の計算は複雑でかつ多くの計算量を必要とすることから、順変換式の算出は極めて困難という問題がある。

本発明の目的は、従来構造よりも、リンク姿勢の変更に伴う演算処理負荷の低減を図り、かつ、位置決め精度を向上することができるリンク作動装置の制御装置を提供することである。

本発明のリンク作動装置１の制御装置４は、基端側のリンクハブ１４に対し先端側のリンクハブ１５が３組以上のリンク機構１１，１２，１３を介して姿勢を変更可能に連結され、前記各リンク機構１１，１２，１３が、それぞれ前記基端側のリンクハブ１４および前記先端側のリンクハブ１５に一端が回転可能に連結された基端側および先端側の端部リンク部材１１ａ，１２ａ，１３ａ、１１ｂ，１２ｂ，１３ｂと、これら基端側および先端側の端部リンク部材１１ａ，１２ａ，１３ａ、１１ｂ，１２ｂ，１３ｂの他端に両端がそれぞれ回転可能に連結された中央リンク部材１１ｃ，１２ｃ，１３ｃとを有し、全ての前記リンク機構１１，１２，１３に、前記先端側のリンクハブ１５の姿勢を任意に制御するアクチュエータ３を備えたリンク作動装置１における、前記各アクチュエータ３を制御する制御装置４であって、
前記基端側のリンクハブ１４に対する前記先端側のリンクハブ１５の姿勢を変更するとき、目標のリンク姿勢（θ、φ）に対応する、前記基端側の端部リンク部材１１ａ，１２ａ，１３ａである各アームの回転角を逆変換により算出する指令変換部４２と、
前記指令変換部４２で算出された各アームの回転角から、定められた条件に従って２つのアームを駆動する駆動アームに設定し、残りのアームをフリーアームとするアーム設定部５０と、
前記アーム設定部５０で設定された前記２つの駆動アームのみを駆動し位置決めが完了した後、前記フリーアームを駆動し位置決めするアーム制御部３９と、を備える。
前記定められた条件は、設計等によって任意に定める条件であって、例えば、タクトタイムを優先するか、またはエンドエフェクタ等の慣性モーメントの大小を考慮するか、等の条件である。

この構成によると、指令変換部４２は、先端側のリンクハブ１５の姿勢を変更するとき、目標のリンク姿勢（θ、φ）に対応する各アームの回転角を逆変換により算出する。これにより、順変換を行う従来構造よりも演算処理負荷を低減し得る。アーム設定部５０は、各アームのうち２つのアームを駆動アームに設定し、残りのアームをフリーアームとする。本リンク作動装置１において、２つのアームの角度位置が決まると、残りのアームの角度位置は受動的に一意に決まる。このため、２つのアームのみを駆動して、残りのアームにつき駆動力を付与しないフリーにすると、２つのアームのみを駆動してリンク姿勢を変更している間は、各アームに過大な干渉つまり過大なトルクは発生しない。アーム制御部３９は、２つの駆動アームのみを駆動し位置決めが完了した後、フリーアームを駆動し位置決めする。これにより、機構部等に生じるガタを片側方向に寄せる力が付与される。よって、従来構造よりも、リンク姿勢の変更に伴う演算処理負荷の低減を図り、かつ、位置決め精度を向上することができる。

前記アクチュエータ３はモータであり、前記アーム制御部３９は、前記２つの駆動アームを駆動するとき、前記フリーアームに対応するモータへの通電を切ってもよい。この場合、さらに演算処理負荷の低減を図れる。

前記アクチュエータ３はモータであり、前記アーム制御部３９は、前記２つの駆動アームを駆動するとき、前記フリーアームに対応するモータ３への指令トルク値を零に設定したトルク制御を行ってもよい。この場合、各アームは互いに連結され、２つのアームの角度位置が決まると、残りのアームの角度位置は受動的に一意に決まる。このため、指令トルク値が零でトルク制御を行っているフリーアームは、駆動する２つの駆動アームの移動にならって移動する。

前記アーム設定部５０は、他のアームに対し移動量が相対的に少ない２つのアームを、前記駆動アームに設定してもよい。タクトタイムを優先する場合、駆動する２つのアームは、他のアームよりも移動量が少ない２つのアームを選択する。アームの移動量が少なくなるので、２つの駆動アームを駆動するときのアームの移動時間が短くなり、タクトタイムの短縮を図れる。

前記アーム設定部５０は、他のアームに対し移動量が相対的に大きい２つのアームを、前記駆動アームに設定してもよい。先端側のリンクハブに取り付けたワークまたはエンドエフェクタの慣性モーメントが大きい場合、駆動する２つのアームは、アームの移動量が大きい２つのアームを選択する。リンク姿勢を変更するとき、リンク姿勢の移動量が大きい場合でも、リンク姿勢によっては、１つのアームの移動量が略ゼロに近くなる場合がある。このとき、移動量が少ない２つのアームを選択すると、実質１つのアクチュエータでリンク姿勢の変更を実行することになる。そうすると、先端側のリンクハブに取り付けたワークまたはエンドエフェクタの慣性モーメントが大きい場合、駆動するアクチュエータにかかる負荷トルクが大きくなる。そこで、アームの移動量が大きい２つのアームを駆動アームとして選択すると、２つのアクチュエータ３，３でリンク姿勢の変更を行うことになる。よって、負荷が分散され、１つのアクチュエータ３にかかる負荷トルクを小さくできる。

本発明のリンク作動装置の制御装置は、アーム設定部で設定された２つの駆動アームを駆動し位置決めが完了した後、フリーアームを位置決めするため、従来構造よりも、リンク姿勢の変更に伴う演算処理負荷の低減を図り、かつ、位置決め精度を向上することができる。

本発明の第１の実施形態に係るリンク作動装置の斜視図である。 同リンク作動装置の一部を省略した正面図である。 図２Ａのリンク作動装置を姿勢変更した正面図である。 同リンク作動装置のリンク機構の一つを直線で表現した幾何学的モデル図である。 同リンク作動装置の制御装置のブロック図と、その制御対象となるリンク作動装置を一部省略した正面図とを組み合わせた説明図である。 同リンク作動装置が姿勢変更するときの過程を示すフローチャートである。

［第１の実施形態］
本発明の実施形態に係るリンク作動装置の制御装置を図１ないし図５と共に説明する。先ず、制御対象となるリンク作動装置を説明し、その後、制御装置を説明する。
＜リンク作動装置＞
図１のリンク作動装置１は、リンク作動装置本体であるパラレルリンク機構２と、パラレルリンク機構２における後述の先端側のリンクハブ１５の姿勢を任意に制御する図２Ａのアクチュエータ３とを備える。

＜パラレルリンク機構＞
図２Ａの例では、パラレルリンク機構２が、その基端側で、支持部材５にスペーサ６を介して吊り下げ状態に設置されている。パラレルリンク機構２の先端側には、先端取付部材７を介してエンドエフェクタ８が搭載されている。パラレルリンク機構２は、３節連鎖のリンク機構を３組備えている。図１に示す３組のリンク機構１１，１２，１３の形状につき、図２Ａ，図２Ｂ，図３では１組のリンク機構１１のみを表示している。図１に示す３組のリンク機構１１，１２，１３のそれぞれは幾何学的に互いに同一形状をなす。各リンク機構１１～１３は、後述の各リンク部材１１ａ～１３ａ，１１ｃ～１３ｃ，１１ｂ～１３ｂを直線で表現した幾何学モデルが、中央リンク部材１１ｃ～１３ｃの中央部に対する基端側部分と先端側部分が対称を成す形状である。

各リンク機構１１，１２，１３は、基端側の端部リンク部材１１ａ，１２ａ，１３ａ、中央リンク部材１１ｃ，１２ｃ，１３ｃ、および先端側の端部リンク部材１１ｂ，１２ｂ，１３ｂを有し、４つの回転対偶からなる３節連鎖のリンク機構をなす。基端側および先端側の端部リンク部材１１ａ，１２ａ，１３ａ、１１ｂ，１２ｂ，１３ｂはＬ字状をなし、基端がそれぞれ基端側のリンクハブ１４および先端側のリンクハブ１５に回転自在に連結されている。中央リンク部材１１ｃ，１２ｃ，１３ｃは、両端に基端側および先端側の端部リンク部材１１ａ，１２ａ，１３ａ、１１ｂ，１２ｂ，１３ｂの先端がそれぞれ回転自在に連結されている。基端側の端部リンク部材１１ａ，１２ａ，１３ａは、「アーム」と称する場合がある。

基端側のリンクハブ１４および先端側のリンクハブ１５は、例えば、六角柱状で、外周面を構成する６つの側面１６のうちの１つ置きに離れた３つの側面１６に、基端側および先端側の端部リンク部材１１ａ，１２ａ，１３ａ、１１ｂ，１２ｂ，１３ｂがそれぞれ回転自在に連結されている。３組のリンク機構１１，１２，１３の各基端側および先端側の端部リンク部材１１ａ，１２ａ，１３ａ、１１ｂ，１２ｂ，１３ｂは、球面リンク機構と呼ばれる構造であって、それぞれの球面リンク中心ＰＡ，ＰＢ（図４）は一致しており、球面リンク中心ＰＡ，ＰＢ（図４）からの距離も同じである。端部リンク部材１１ａ，１２ａ，１３ａ、１１ｂ，１２ｂ，１３ｂと中央リンク部材１１ｃ，１２ｃ，１３ｃとの連結部となる回転対偶軸は、ある交差角γをもっていてもよいし、平行であってもよい。

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

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

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

基端側の端部リンク部材１１ａ～１３ａと中央リンク部材１１ｃ～１３ｃの連結部も軸受を介して互いに回転自在に連結されている。先端側の端部リンク部材１１ｂ～１３ｂと中央リンク部材１１ｃ～１３ｃの連結部も、同様の構造である。

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

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

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

パラレルリンク機構２の構成によれば、基端側のリンクハブ１４に対する先端側のリンクハブ１５の可動範囲を広くとれる。例えば、基端側のリンクハブ１４の中心軸ＱＡと先端側のリンクハブ１５の中心軸ＱＢの折れ角θの最大値（最大折れ角）を約±９０°とすることができる。基端側のリンクハブ１４に対する先端側のリンクハブ１５の旋回角φを０°～３６０°の範囲で設定できる。

＜アクチュエータ等＞
図２Ａにおいて、複数のアクチュエータ３は、支持部材５の上に円周方向に等配で設置されている。アクチュエータ３の個数は、図１に示すリンク機構１１，１２，１３と同数の３個である。図４に示すように、アクチュエータ３はモータであり、その出力軸３ａにピニオン３０が設けられている。一方、図１の基端側の端部リンク部材１１ａ～１３ａにおける軸部１８との回転対偶部に、図４に示す連結部材３１が固定して取付けられ、連結部材３１に、ピニオン３０と噛み合う扇形ギア３２が設けられている。扇形ギア３２の中心軸は、軸部１８（図１）の中心軸と一致する。ピニオン３０と扇形ギア３２とで減速機３３を構成する。

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

＜制御装置＞
制御装置４は、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢を、現在の姿勢から目標のリンク姿勢（θ、φ）へ変更するように、各アクチュエータ３を制御する。目標のリンク姿勢である目標姿勢（θ、φ）は、外部指令手段４０から制御装置４に与えられる。制御装置４は、コンピュータによる数値制御式のものであり、姿勢変更制御手段４１を備える。

姿勢変更制御手段４１は、指令変換部４２と、アーム設定部５０と、アーム制御部３９とを有する。指令変換部４２は、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢を変更するとき、目標のリンク姿勢（θ、φ）に対応する、基端側の端部リンク部材である各アーム１１ａ～１３ａ（図１）の回転角を逆変換により算出する。指令変換部４２は、外部指令手段４０から与えられた目標姿勢（θ、φ）を各アームの回転角（β１n、β２n、β３n）に変換する。

アーム設定部５０は、指令変換部４２で算出された各アームの回転角から、定められた条件に従って２つのアームを駆動する駆動アームに設定し、残りのアームをフリーアームとする。アーム制御部３９は、アーム設定部５０で設定された２つの駆動アームのみを駆動し位置決めが完了した後、フリーアームを駆動し位置決めする。アーム制御部３９は、駆動する２つの駆動アームに対して同期制御を指令し、フリーにするフリーアームに対して指令トルク値を零でトルク制御を指令する制御指令部５３と、アクチュエータドライバである個別制御部４４とを有する。

指令変換部４２は、折れ角θと旋回角φで与えられる目標姿勢を各アームの回転角（β１n、β２n、β３n）に変換する。折れ角θと旋回角φから各アーム１１ａ～１３ａ（図１）の回転角βを求める演算は、以下の関係式（１）による逆変換で行われる。
cos（θ／２）sinβｎ－sin（θ／２）sin（φ＋δｎ）cosβｎ＋sin（γ／２）＝０ …式（１）

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

図４に示すように、アーム設定部５０は、３つのアーム１１ａ，１２ａ，１３ａ（図１）のうち、駆動する２つのアームと、フリーにするアームを設定する。アーム設定部５０は、比較部５０ａと設定部５０ｂとを含む。その設定方法は、タクトタイムを優先する場合、指令変換部４２で求められた各アームの回転角（β１n、β２n、β３n）から、他のアームに対し移動量が相対的に少ない２つのアームを駆動する駆動アームに設定し、それ以外のアームをフリーにするフリーアームに設定する。比較部５０ａにおいて、他のアームに対し移動量が相対的に少ないか否かを比較する。設定部５０ｂは、比較部５０ａにおける比較結果に基づいて駆動アームとフリーアームを設定する。この設定方法では、アームの移動量が少なくなるので、２つのアームを駆動するときの、アームの移動時間が短くなる。

別の設定方法として、リンク先端に取り付けたワークまたはエンドエフェクタの慣性モーメントが大きい場合、指令変換部４２で求められた各アームの回転角（β１n、β２n、β３n）から、他のアームに対し移動量が相対的に大きい２つのアームを駆動アームに設定し、それ以外のアームをフリーアームに設定する。この場合、１つのアクチュエータ３でリンク姿勢を変更することが無くなり、駆動するアクチュエータ３にかかる負荷トルクが低減する。

制御指令部５３は、駆動する２つの駆動アームに対して同期制御を指令し、フリーにするフリーアームに対してトルク制御を指令する。図４では、アーム１１ａとアーム１２ａを駆動アームに設定し、アーム１３ａをフリーアームに設定した場合の例を示している。駆動するアームをアーム１１ａとアーム１３ａに設定した場合、アーム１１ａとアーム１３ａのアクチュエータが同期制御を行い、アーム１２ａのアクチュエータがトルク制御を行う。駆動するアームをアーム１２ａとアーム１３ａに設定した場合、アーム１２ａとアーム１３ａのアクチュエータが同期制御を行い、アーム１１ａのアクチュエータがトルク制御を行う。

同期制御部４３は、駆動する２つのアーム回転の同期制御を行わせる手段であり、同期タイミングに合わせてアクチュエータドライバである個別制御部４４へ毎時毎の位置指令を送信する。
トルク制御部５１は、フリーアームのアクチュエータ３に対し、トルク指令値をゼロに設定したトルク制御を行う手段であり、アクチュエータドライバである個別制御部４４へ毎時毎のトルク値（ゼロ）を指令する。

個別制御部４４は、本実態形態ではアクチュエータドライバであり、制御指令部５３で指定した同期制御または、トルク制御に対応した動作を行う。
個別制御部４４は、同期制御の場合、駆動する２つのアームに対応するアクチュエータ３の動作を、アームの現在角度から目標角度へ回転させるように制御する手段であり、位置制御部４５および指令実行手段４６を有する。同期制御部４３より各制御パラメータ（例えば、加速時間、減速時間、指令速度）、およびアーム指令回転角度に従い、パルス払い出し等によって位置制御部４５へ動作指令を与え、指令実行手段４６でアクチュエータ３を駆動する。位置制御部４５は、その与えられた動作指令と回転角検出手段３５の検出値を用いてフィードバック制御を行う。

個別制御部４４は、トルク制御の場合、フリーにするアームに対応するアクチュエータ３の動作を、指定したトルク値になるように制御する手段であり、トルク制御部５２および指令実行手段５４を有する。トルク制御部５１より指定されたトルク値になるようにトルク制御部５２へ動作指令を与え、指令実行手段５４でアクチュエータ３を駆動する。トルク制御部５２は、与えられたトルク値とアクチュエータドライバに内蔵された電流検出手段（図示せず）の電流値を用いて、与えられたトルク値になるようにフィードバック制御を行う。

＜姿勢変更するときのフローチャート＞
次に、リンク作動装置が姿勢変更するときのフローチャートを図５に示す。図４も適宜参照しつつ説明する。本処理開始後、指令変換部４２は、外部指令手段４０が指定したリンクの目標姿勢（θ、φ）を各アームの回転角（β１、β２、β３）に変換する。本演算は、上述した関係式（１）による逆変換で行われる（ステップＳ１）。
次に、ステップＳ１で求めたアーム回転角（β１、β２、β３）をもとに、アーム設定部５０により、駆動する２つのアームとフリーにするアームを設定する（ステップＳ２）。

駆動アームおよびフリーアームを設定した後、アーム制御部３９は、駆動する２つのアームに対して同期制御を行い、同時にフリーにするアームに対してトルク指令ゼロでトルク制御を行う（ステップＳ３）。駆動する２つのアームの同期制御の位置決めが完了したら（ステップＳ４：Ｙｅｓ）、アーム制御部３９は、フリーにしたアームのトルク制御を停止し、フリーアームのアクチュエータに対してステップＳ１で求めたフリーアームのアーム回転角へ位置決めする（ステップＳ５）。
この位置決めが完了すると（ステップＳ６：Ｙｅｓ）、各回転対偶部および機構部などに生じるガタを片側方向に寄せる力が付与される。これをもって、リンク姿勢変更の一連の動作が完了する。

＜作用効果＞
以上説明したリンク作動装置１の制御装置４によると、指令変換部４２は、先端側のリンクハブ１５の姿勢を変更するとき、目標のリンク姿勢（θ、φ）に対応する各アーム１１ａ，１２ａ，１３ａの回転角を逆変換により算出する。これにより、順変換を行う従来構造よりも演算処理負荷を低減し得る。アーム設定部５０は、各アーム１１ａ，１２ａ，１３ａのうち２つのアームを駆動アームに設定し、残りのアームをフリーアームとする。本リンク作動装置１において、２つのアームの角度位置が決まると、残りのアームの角度位置は受動的に一意に決まる。このため、２つのアームのみを駆動して、残りのアームにつき駆動力を付与しないフリーにすると、２つのアームのみを駆動してリンク姿勢を変更している間は、各アームに過大な干渉つまり過大なトルクは発生しない。アーム制御部３９は、２つの駆動アームを駆動し位置決めが完了した後、フリーアームを位置決めする。これにより、機構部等に生じるガタを片側方向に寄せる力が付与される。よって、従来構造よりも、リンク姿勢の変更に伴う演算処理負荷の低減を図り、かつ、位置決め精度を向上することができる。本制御において、算出が困難な順変換を使用しないことから、制御ソフトウェアの開発工数の削減につながる。

アーム設定部５０は、他のアームに対し移動量が相対的に少ない２つのアームを駆動アームに設定した場合、アームの移動量が少なくなるので、２つの駆動アームを駆動するときのアームの移動時間が短くなり、タクトタイムの短縮を図れる。
アーム設定部５０は、他のアームに対し移動量が相対的に大きい２つのアームを駆動アームに設定してもよい。先端側のリンクハブ１５に取り付けたワークまたはエンドエフェクタの慣性モーメントが大きい場合、駆動する２つのアームは、アームの移動量が大きい２つのアームを選択する。リンク姿勢を変更するとき、リンク姿勢の移動量が大きい場合でも、リンク姿勢によっては、１つのアームの移動量が略ゼロに近くなる場合がある。このとき、移動量が少ない２つのアームを選択すると、実質１つのアクチュエータ３でリンク姿勢の変更を実行することになる。そうすると、先端側のリンクハブに取り付けたワークまたはエンドエフェクタの慣性モーメントが大きい場合、駆動するアクチュエータ３にかかる負荷トルクが大きくなる。そこで、アームの移動量が大きい２つのアームを駆動アームとして選択すると、２つのアクチュエータ３，３でリンク姿勢の変更を行うことになる。よって、負荷が分散され、１つのアクチュエータ３にかかる負荷トルクを小さくできる。

［第２の実施形態］
第１の実施形態では、リンク作動装置１のリンク姿勢を変更するのに２つのアームの同期制御を実行した後、フリーアームの位置決めを行っている。つまり、２ステップの位置決め動作を実行している。１ステップ目の２つのアームの同期制御が終了したときのフリーアームの位置は駆動アームの同期制御に倣った位置に移動しているため、その位置と２ステップ目のフリーアームの位置決め動作の目標位置はほぼ近い値になる。そのため、２ステップ目の位置決めにかかる時間は一瞬だが、最初から３つのアームの同期制御を実行する場合（例えば、特許文献２の制御方法）に比べると、本制御方法はリンク姿勢変更にかかる時間が長くなる。
そこで、特許文献２の課題としてあげた非常停止ボタン押下などにより、リンク姿勢（θ、φ）の現在位置情報が消失した場合のみ、本制御方法を実行する。
したがって、通常時において３つのアームの同期制御を実行し、非常停止ボタン押下などの緊急時のみ本制御方法を実行することで、リンク姿勢変更にかかる時間を短縮することが可能となる。

アクチュエータ３はモータであり、アーム制御部３９は、２つの駆動アームを駆動するとき、フリーアームに対応するモータへの通電を切るようにしてもよい。この場合、演算処理負荷の低減を図れる。
リンク機構の組数は４組以上であってもよい。

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

１…リンク作動装置、３…アクチュエータ、４…制御装置、１１，１２，１３…リンク機構、１１ａ，１２ａ，１３ａ…基端側の端部リンク部材、１１ｃ，１２ｃ，１３ｃ…中央リンク部材、１１ｂ，１２ｂ，１３ｂ…先端側の端部リンク部材、１４…基端側のリンクハブ、１５…先端側のリンクハブ、３９…アーム制御部、４２…指令変換部、５０…アーム設定部

Claims (5)

1. 基端側のリンクハブに対し先端側のリンクハブが３組以上のリンク機構を介して姿勢を変更可能に連結され、前記各リンク機構が、それぞれ前記基端側のリンクハブおよび前記先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とを有し、全ての前記リンク機構に、前記先端側のリンクハブの姿勢を任意に制御するアクチュエータを備えたリンク作動装置における、前記各アクチュエータを制御する制御装置であって、
   前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を変更するとき、目標のリンク姿勢（θ、φ）に対応する、前記基端側の端部リンク部材である各アームの回転角を逆変換により算出する指令変換部と、
   前記指令変換部で算出された各アームの回転角から、定められた条件に従って２つのアームを駆動する駆動アームに設定し、残りのアームをフリーアームとするアーム設定部と、
   前記アーム設定部で設定された前記２つの駆動アームのみを駆動し位置決めが完了した後、前記フリーアームを駆動し位置決めするアーム制御部と、を備えるリンク作動装置の制御装置。
2. 請求項１に記載のリンク作動装置の制御装置において、前記アクチュエータはモータであり、前記アーム制御部は、前記２つの駆動アームを駆動するとき、前記フリーアームに対応するモータへの通電を切るリンク作動装置の制御装置。
3. 請求項１に記載のリンク作動装置の制御装置において、前記アクチュエータはモータであり、前記アーム制御部は、前記２つの駆動アームを駆動するとき、前記フリーアームに対応するモータへの指令トルク値を零に設定したトルク制御を行うリンク作動装置の制御装置。
4. 請求項１に記載のリンク作動装置の制御装置において、前記アーム設定部は、他のアームに対し移動量が相対的に少ない２つのアームを、前記駆動アームに設定するリンク作動装置の制御装置。
5. 請求項１に記載のリンク作動装置の制御装置において、前記アーム設定部は、他のアームに対し移動量が相対的に大きい２つのアームを、前記駆動アームに設定するリンク作動装置の制御装置。
   

Images