特許 5976580 リンク作動装置の制御装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5976580

(24)【登録日】2016年7月29日

(45)【発行日】2016年8月23日

(54)【発明の名称】リンク作動装置の制御装置

(51)【国際特許分類】

   B25J 13/00 20060101AFI20160809BHJP

   F16H 21/50 20060101ALI20160809BHJP

【ＦＩ】

   B25J13/00 Z

   F16H21/50

【請求項の数】10

【全頁数】18

(21)【出願番号】特願2013-63948(P2013-63948)

(22)【出願日】2013年3月26日

(65)【公開番号】特開2014-188602(P2014-188602A)

(43)【公開日】2014年10月6日

【審査請求日】2015年9月18日

(73)【特許権者】

【識別番号】000102692

【氏名又は名称】ＮＴＮ株式会社

(74)【代理人】

【識別番号】100086793

【弁理士】

【氏名又は名称】野田 雅士

(74)【代理人】

【識別番号】100087941

【弁理士】

【氏名又は名称】杉本 修司

(72)【発明者】

【氏名】西尾 幸宏

(72)【発明者】

【氏名】磯部 浩

(72)【発明者】

【氏名】山田 裕之

【審査官】 木原 裕二

(56)【参考文献】

【文献】 特開２０１２−８２９３７（ＪＰ，Ａ）

【文献】 特開平１０−２８６７８８（ＪＰ，Ａ）

【文献】 特開２００５−３０９９９０（ＪＰ，Ａ）

【文献】 国際公開第２００６／１２１０９１（ＷＯ，Ａ１）

【文献】 特開平８−１１８２７０（ＪＰ，Ａ）

【文献】 国際公開第２０１１／００４５８４（ＷＯ，Ａ１）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００ − ２１／０２

Ｆ１６Ｈ ２１／５０

(57)【特許請求の範囲】

【請求項1】

基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構のそれぞれに、前記基端側の端部リンク部材であるアームを回転させることにより、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータを設けたリンク作動装置において、
前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を、始点姿勢から指令された終点姿勢へ変更させるように、前記各アクチュエータを制御するリンク作動装置の制御装置であって、
前記各アクチュエータを、前記始点姿勢となるときの前記アームの回転角である始点から前記終点姿勢となるときの回転角である終点へポイントツーポイントで互いに同期して駆動する姿勢変更制御手段と、
前記始点姿勢から終点姿勢へ姿勢変更するときの前記先端側のリンクハブの姿勢変更量を所定量と比較し、所定量以上である場合に、前記始点姿勢から終点姿勢へ変更する姿勢変更経路の途中に、定められた規則によって１つ以上の中継姿勢を設定し、この中継姿勢になるときの前記各アームの回転経路の中継点となる回転角を求めて設定する中継姿勢設定手段とを設け、
前記姿勢変更制御手段は、前記中継姿勢設定手段で設定された前記中継点を前記各アームが同時に通過するように位置制御する、
ことを特徴とするリンク作動装置の制御装置。

【請求項2】

請求項１に記載のリンク作動装置の制御装置において、前記中継姿勢設定手段は、前記リンクハブの前記始点姿勢から終点姿勢への姿勢変更量を所定量以下に分割した数をもとに前記各アームの回転量を分割するリンク作動装置の制御装置。

【請求項3】

請求項１または請求項２に記載のリンク作動装置の制御装置において、前記リンク機構が３組であり、前記中継姿勢設定手段により前記中継点を設定する前記定められた規則は、前記リンクハブの前記始点姿勢から終点姿勢へ姿勢変更する過程で動作する３つのアームのうち、２つのアームを選び、この選んだ２つのアームの回転経路を等分し、残り１つのアームは、他の２つのアームとの相対位置関係から一意に決まる位置とするリンク作動装置の制御装置。

【請求項4】

請求項３に記載のリンク作動装置の制御装置において、前記３つのアームのうち２つのアームを選ぶ基準として、回転量の大きいアームを選択するリンク作動装置の制御装置。

【請求項5】

請求項３または請求項４に記載のリンク作動装置の制御装置において、残り一つのアームの回転角を定めるについては、選択された２つのアームの回転角から、前記先端側のリンクハブの姿勢（θ，φ）を準変換の変換式で求め、前記リンクハブの姿勢（θ，φ）から逆変換の変換式により前記残り一つのアームの回転角を定めるリンク作動装置の制御装置。

【請求項6】

請求項５に記載のリンク作動装置の制御装置において、前記先端側のリンクハブの姿勢（θ，φ）は、前記３つのアームのうちの２つのアームの回転角と変換式とを用いて収束演算で求めるリンク作動装置の制御装置。

【請求項7】

請求項１または請求項２に記載のリンク作動装置の制御装置において、前記中継姿勢設定手段は、前記各アームの中継点を求める定められた規則として、前記先端側のリンクハブが最短距離で移動する経路上に一つ以上の中継姿勢をとり、前記経路上の、前記始点姿勢、中継姿勢、および終点姿勢のいずれかである各姿勢のうち、隣合う姿勢間の成す量が所定量以下となるリンクハブの姿勢（θ，φ）に対応する各アームの回転角を前記中継点とするリンク作動装置の制御装置。

【請求項8】

請求項７において、前記リンクハブの姿勢（θ，φ）から逆変換の変換式により各アームの回転角を求めるリンク作動装置の制御装置。

【請求項9】

請求項１または請求項２または請求項７に記載のリンク作動装置の制御装置において、前記リンク機構が３組であり、前記先端側のリンクハブの前記始点姿勢（θａ，φａ）と終点姿勢（θｂ，φｂ）から、それぞれ折れ角θの移動量Δθと旋回角φの移動量Δφが所定の移動量以下となるように等分した各リンクハブ姿勢（θ１，θ２，…θｎ，φ１，φ２，…φｎ）（ただし、ｎ：分割数−１）を逆変換の変換式により求め、この求めたリンクハブ姿勢（θ１，θ２，…θｎ，φ１，φ２，…φｎ）を前記中継姿勢とするリンク作動装置の制御装置。

【請求項10】

請求項１ないし請求項９のいずれか１項に記載のリンク作動装置の制御装置において、前記姿勢変更制御手段は、前記先端側のリンクハブの前記始点姿勢から終点姿勢への位置決め制御を、前記始点姿勢から姿勢変更を開始するときの加速区間と、前記姿勢に達する手前で減速する減速区間とを除いて、姿勢変更経路の全区間で加減速無しで行うリンク作動装置の制御装置。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

パラレルリンク機構を具備する作業装置の一例が、特許文献２に開示されている。この作業装置のパラレルリンク機構は、各リンクの作動角が小さいため、トラベリングプレートの作動範囲を大きく設定するためには、リンク長さを長くする必要がある。それにより、機構全体の寸法が大きくなって、装置が大型になってしまうという問題があった。また、リンク長さを長くすると、機構全体の剛性の低下を招く。そのため、トラベリングプレートに搭載されるツールの重量、つまりトラベリングプレートの可搬重量も小さいものに制限されるという問題もあった。

【0003】

上記問題点を解消するものとして、３節連鎖のリンク機構を３組以上設けたリンク作動装置が提案されている（例えば特許文献２，３）。この構成であると、コンパクトでありながら、精密で広範な作動範囲の動作が可能である。

【0004】

図８〜図９は、３節連鎖のリンク機構を３組以上設けたリンク作動装置の一例を示す。このリンク作動装置１は、基端側のリンクハブ１４と、先端側のリンクハブ１５と、これらリンクハブ１４，１５を互いに連結する３組のリンク機構１１，１２，１３とでなる。各リンク機構１１，１２，１３は、基端側の端部リンク部材１１ａ，１２ａ，（１３ａ）、先端側の端部リンク部材１１ｂ，１２ｂ，１３ｂ、および中央リンク１１ｃ，１２ｃ，１３ｃで構成され、４つの回転対偶からなる３節連鎖のリンク機構をなす。なお、図８，図９では、基端側の端部リンク部材１３ａは図示されていないが、区別のために符号を用いて説明する。また、前記基端側の端部リンク部材１１ａ，１２ａ，１３ａを、以下、アーム１１ａ，１２ａ，１３ａと称する。

【0005】

このリンク作動装置１の構成によると、基端側のリンクハブ１４と、先端側のリンクハブ１５と、３組のリンク機構１１，１２，１３とで、基端側のリンクハブ１４に対し先端側のリンクハブ１５が直交２軸方向に移動自在な２自由度機構が構成される。この２自由度機構は、コンパクトでありながら、先端側のリンクハブ１５の可動範囲を広くとれる。例えば、基端側のリンクハブ１４の中心軸ＱＡと先端側のリンクハブ１５の中心軸ＱＢの最大折れ角は約±９０°であり、基端側のリンクハブ１４に対する先端側のリンクハブ１５の旋回角を０°〜３６０°の範囲に設定できる。

【先行技術文献】

【特許文献】

【0006】

【特許文献1】特開２０００−９４２４５号公報

【特許文献2】米国特許第５，８９３，２９６号明細書

【特許文献3】特開２０１１−２４０４４０号公報

【発明の概要】

【発明が解決しようとする課題】

【0007】

図８，図９の例のような３節連鎖の３組のリンク機構１１，１２，１３を設けたリンク作動装置において、２自由度のリンク機構１１，１２，１３を３個のモータ等のアクチュエータ（図示せず）で駆動する装置は、折れ角θと旋回角φにより姿勢が決定される。折れ角θは、基端側のリンクハブ１４の中心軸ＱＡに対する先端側のリンクハブ１５の中心軸ＱＢの傾斜角度であり、旋回角φは、基端側のリンクハブ１４の中心軸ＱＡに対する先端側のリンクハブ１５の中心軸ＱＢの旋回角度である。その折れ角θと旋回角φとから、前記基端側の端部リンク部材である各アーム１１ａ，１２ａ，１３ａの回転角（以下「アーム回転角」と称す）（β１n 、β２n 、β３n ）を求め、アーム１１ａ，１２ａ，１３ａを駆動するアクチュエータに位置決めさせる。

【0008】

例えば、先端側のリンクハブ１５のある姿勢Ａ（θa 、φa ）とＢ（θb 、φb ）について、各々の姿勢に対応する各アーム回転角は、リンクハブ１４，１５とアーム回転角との関係式によりＡ（β１a 、β２a 、β３a ）、Ｂ（β１ｂ、β２ｂ、β３ｂ）として求められる。ここで、姿勢Ａから姿勢Ｂへの移動は、各アーム１１ａ，１２ａ，１３ａの回転角がβ1aからβ1b、β2aからβ2b、β3aからβ3bへ移動することで実行される。

【0009】

このとき、図１０にアーム回転角βの指令値と折れ角θの関係を点線の曲線Ｌ１〜Ｌ３で示すように、例えば、旋回角を１５°に固定した状態で折れ角θを−６０°から６０°に変化させ姿勢変更する場合、各アーム１１ａ，１２ａ，１３ａは始点Ａから終点Ｂへ３つのアーム１１ａ，１２ａ，１３ａがポイントツーポイント（Point-to-Point）制御で同期して等速運動で制御される。
しかし、折れ角θを例えば-60 °、-45 °、-30 °と15°毎に分割し、各先端側リンクハブ１５の姿勢から先端側リンクハブ１５とアーム回転角との関係式を用いてそれぞれのアーム回転角を求めると、図１０の実線で示す曲線Ｍ１〜Ｍ３を通過する経路で各姿勢が決まる。同図から分かるように、特に曲線Ｌ１と曲線Ｍ１とでは折れ角０°付近で経路の乖離が大きい。

【0010】

このように、曲線Ｌ１〜Ｌ３の経路で駆動の指令をする場合、曲線Ｍ１〜Ｍ３の経路で駆動の指令をする場合に比べて各アーム回転角β１，β２，β３は、始点と終点を除く途中の経路では各アーム１１ａ，１２ａ，１３ａの位置が、リンクハブとアーム回転角との関係式に対して大きく異なる位置を通過して位置決め指令されることになる。

【0011】

このとき各アーム１１ａ，１２ａ，１３ａは互いに連結され、先端側リンクハブ１５の姿勢により３つのアーム回転角の相対位置は一意に決まるため、アーム回転角の相対位置が崩れ、３つのアーム１１ａ，１２ａ，１３ａが過大に干渉する。すなわち、３つのアーム１１ａ，１２ａ，１３ａが、それぞれのアクチュエータへの指令値に対して偏差を生じたアーム回転角を採る。そのため、曲線Ｌ１〜Ｌ３の経路ではリンクの駆動に、より大きなトルクが必要になる。

【0012】

これは、１度の駆動における姿勢変更量が大きい程、経路途中で、リンクハブ１４，１５とアーム回転角βとの関係式から定まるアーム回転角と大きく異なる位置に位置決め指令されるため、各アーム１１ａ，１２ａ，１３ａ間に干渉が生じ、リンク機構１１，１２，１３の駆動に過大なトルクが必要になることを意味する。このときリンク機構１１，１２，１３にも荷重がかかり、リンク機構１１，１２，１３の組付け不具合や、異常な摩耗等を与え、位置決め精度悪化の原因になる。これを解消するためには図１０の曲線Ｍ１〜Ｍ３に近い経路を通過するように各アームを駆動する必要がある。
しかし、先端側リンクハブ１５の姿勢Ａから姿勢Ｂまでの全経路において干渉（すなわち、指令値に対して３つのリンク機構１１，１２，１３の関係で定まる位置との偏差）を生じないように各アーム１１ａ，１２ａ，１３ａの回転角を制御すると、先端側リンクハブ１５の姿勢から各アーム各アーム１１ａ，１２ａ，１３ａの回転角への変換演算時間を要するため移動時間が長くなる。また、予め全経路の座標を記憶して制御する場合は、より大きな記憶メモリが必要になるという問題がある。

【0013】

この発明の目的は、先端側リンクハブの姿勢を大きく変更するときに、複数のリンク機構の相互の関係で一義的に定まる各アーム回転角から大きく外れることなく各アームのアクチュエータの位置決め制御が行えて、リンク機構の各部に過大な負荷を掛けずに駆動でき、かつ演算時間による遅れが回避できて高速移動が図れ、制御のための記憶メモリの容量も小さくて済むリンク作動装置の制御装置を提供することである。

【課題を解決するための手段】

【0014】

この発明のリンク作動装置の制御装置は、基端側のリンクハブ１４に対し先端側のリンクハブ１５を、３組以上のリンク機構１１〜１３を介して姿勢を変更可能に連結し、前記各リンク機構１１〜１３は、それぞれ前記基端側のリンクハブ１４および先端側のリンクハブ１５に一端が回転可能に連結された基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと、これら基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの他端に両端がそれぞれ回転可能に連結された中央リンク部材１１ｃ〜１３ｃとでなり、前記各リンク機構１１〜１３は、このリンク機構１１〜１３を直線で表現した幾何学モデルが、前記中央リンク部材１１ｃ〜１３ｃの中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構１１〜１３のそれぞれに、前記基端側の端部リンク部材であるアーム１１ａ〜１３ａを回転させることにより、前記基端側のリンクハブ１４に対する前記先端側のリンクハブ１５の姿勢を任意に変更させるアクチュエータ３を設けたリンク作動装置において、
前記基端側のリンクハブ１４に対する前記先端側のリンクハブ１５の姿勢を、始点姿勢Ａから指令された終点姿勢Ｂへ変更させるように、前記各アクチュエータ３を制御するリンク作動装置の制御装置１であって、
前記各アクチュエータ３を、前記始点姿勢Ａとなるときの前記アーム１１ａ〜１３ａの回転角である始点から前記終点姿勢Ｂとなるときの回転角である終点へポイントツーポイントで互いに同期して駆動する姿勢変更制御手段４１と、
前記始点姿勢から終点姿勢へ姿勢変更するときの前記先端側のリンクハブ１５の姿勢変更量を所定量と比較し、所定量以上である場合に、前記始点姿勢から終点姿勢へ変更する姿勢変更経路の途中に、定められた規則によって１つ以上の中継姿勢を設定し、この中継姿勢になるときの前記各アーム１１ａ〜１３ａの回転経路の中継点となる回転角を求めて設定する中継姿勢設定手段４２とを設け、
前記姿勢変更制御手段４１は、前記中継姿勢設定手段４２で設定された前記中継点を前記各アーム１１ａ〜１３ａが同時に通過するように位置制御する、
ことを特徴とする。

【0015】

この構成によると、中継姿勢設定手段４２により、先端側のリンクハブ１５の姿勢変更量が所定量以上である場合に、中継姿勢を設定し、この中継姿勢になるときの前記各アーム１１ａ〜１３ａの回転経路の中継点を設定する。姿勢変更制御手段は、この設定された中継点を前記各アーム１１ａ〜１３ａが同時に通過するように位置制御する。
このように先端側リンクハブ１５の中継姿勢を設定して各アーム１１ａ〜１３ａがその中継姿勢となる中継点を同時に通過するように制御する。そのため、先端側リンクハブ１５の姿勢を大きく変更するときに、複数のリンク機構１１〜１３の相互の関係で一義的に定まる各アーム回転角から大きく外れることなく各アーム１１ａ〜１３ａのアクチュエータ３の位置決め制御が行える。したがって、リンク機構１１〜１３の各部に過大な負荷を掛けずに駆動できる。また、中継点を求める演算は必要であるが、基本的にはポイントツーポイント制御であるため、先端側リンクハブ１５の姿勢変更の全経路において干渉を生じないように回転角を制御するものと異なり、制御に必要な演算時間が短くて済んで、演算時間による遅れが生じず、高速移動が図れる。また、予め全経路の座標を記憶して制御するものと異なり、記憶メモリの容量が小さくて済む。

【0016】

なお、上記の「先端側のリンクハブ１５の姿勢変更量を所定量と比較し、」とある姿勢変更量および所定量は、折れ角θや旋回角φによって直接に定めても良いが、ポイントツーポイント制御した場合に前記アクチュエータ３に生じる最大のトルクが所定値となる折れ角θまたは旋回角φまたはこれら折れ角θと旋回角φの両方の値であっても良い。

【0017】

この発明において、前記中継姿勢設定手段４２は、前記リンクハブ１５の前記始点姿勢から終点姿勢への姿勢変更量を所定量以下に分割した数をもとに前記各アーム１１ａ〜１３ａの回転量を分割する構成であっても良い。
前記「所定量」は、例えば、リンクハブ１５を駆動する前記各アクチュエータ３をポイントツーポイント制御で所定の値以下で駆動できる移動量としても良く、これは、リンク装置の幾何学的な寸法及びリンクに与える初期与圧によって決まる。また、駆動するアクチュエータ３についての所定の値とは、リンク装置の入力トルク許容値以下の値を示す。リンク装置が入力部に減速機を有する場合は、その減速機の入力トルク許容値以下の値を示す。
このように姿勢変更時の経路を分割することで、各軸のアーム１１ａ〜１３ａの干渉を緩和し、リンクの組付け不具合や、異常な摩耗を与えないような適正なアクチュエータトルクで駆動することができる。始点姿勢から終点姿勢への先端側のリンクハブ１５の移動経路は近似して求めれば良い。

【0018】

この発明において、前記リンク機構が３組であり、前記中継姿勢設定手段４２により前記中継点を設定する前記定められた規則は、前記リンクハブ１５の前記始点姿勢から終点姿勢へ姿勢変更する過程で動作する３つのアーム１１ａ〜１３ａのうち、２つのアームを選び、この選んだ２つのアームの回転経路を等分し、残り１つのアームは、他の２つのアームとの相対位置関係から一意に決まる位置としても良い。
この場合に、前記３つのアーム１１ａ〜１３ａのうち２つのアームを選ぶ基準として、回転量の大きいアームを選択しても良い。この制御では回転量の大きい２つのアームが各軸の最大速度で等速に移動できるため、最短の移動時間で駆動できる。また、回転時間を考慮しなければ３つのアーム１１ａ〜１３ａのうちの２つのアームの選択は、任意に選んでもよい。ただし、このときリンク先端の軌跡は最短経路の通過を保証しない。

【0019】

上記のように３つのアーム１１ａ〜１３ａのうち２つのアームを選ぶ場合に、残り一つのアームの回転角を定めるについては、選択された２つのアームの回転角から、前記先端側のリンクハブ１５の姿勢（θ，φ）を準変換の変換式で求め、前記リンクハブ１５の姿勢（θ，φ）から逆変換の変換式により前記残り一つのアームの回転角を定めるようにしても良い。
これにより、残り一つのアームの回転角を適切に求めることができる。

【0020】

この場合に、前記先端側のリンクハブ１５の姿勢（θ，φ）は、前記３つのアーム１１ａ〜１３ａのうちの２つのアームの回転角と、順変換等の変換式とを用いて収束演算で求めるようにしても良い。収束演算を行うことで、先端側のリンクハブ１５の姿勢（θ，φ）が、２つのアームの回転角から容易にかつ適切に求まる。

【0021】

この発明において、前記中継姿勢設定手段４２は、前記各アームの中継点を求める定められた規則として、前記先端側のリンクハブ１５が最短距離で移動する経路上に一つ以上の中継姿勢をとり、前記経路上の、前記始点姿勢、中継姿勢、および終点姿勢のいずれかである各姿勢のうち、隣合う姿勢間の成す距離等の量が所定量以下となるリンクハブ１５の姿勢（θ，φ）に対応する各アームの回転角を前記中継点とする構成としても良い。
この場合に、前記リンクハブ１５の姿勢（θ，φ）から逆変換の変換式により各アームの回転角を求めるようにしても良い。
すなわち、先端側のリンクハブ１５のある始点姿勢（折れ角、旋回角）Ａ（θa 、φa ）と終点姿勢Ｂ（θb 、φb ）について、姿勢Ａ、Ｂ間を移動する先端側のリンクハブ１５の姿勢変更経路が、できるだけ最短になるように求める。このとき必要に応じて適切な近似を行うことで、経路を求める時間を短縮することができる。姿勢Ａから姿勢Ｂへの経路を複数の姿勢に分割し、各リンクハブ１５の姿勢Ｎ（θn 、φn)から、逆変換アルゴリズムにより各リンクハブ１５の姿勢に対するアーム軸の回転角Ｎ（β１n 、β2n、β3n）を得る。リンク姿勢制御は各軸の位置を同期して移動するように行う。

【0022】

この発明において、前記リンク機構が３組であり、前記リンクハブ１５の前記始点姿勢から終点姿勢への姿勢変更量を所定量以下に分割する場合に、前記先端側のリンクハブ１５の前記始点姿勢Ａ（θａ，φａ）と終点姿勢Ｂ（θｂ，φｂ）から、それぞれ折れ角θの移動量Δθと旋回角φの移動量Δφが所定の移動量以下となるように等分した先端側の各リンクハブ１５の姿勢（θ１，θ２，…θｎ，φ１，φ２，…φｎ）（ただし、ｎ：分割数−１）を逆変換の変換式により求め、この求めたリンクハブ１５の姿勢（θ１，θ２，…θｎ，φ１，φ２，…φｎ）を前記中継姿勢としても良い。
このように、折れ角θの移動量Δθと旋回角φの移動量Δφが共に所定の移動量以下となるように等分することで、より一層干渉が少なくて過大な負荷が生じない駆動を行うことができる。

【0023】

この発明において、前記姿勢変更制御手段４１は、前記先端側のリンクハブ１５の前記始点姿勢Ａから終点姿勢Ｂへの位置決め制御を、前記始点姿勢Ａから姿勢変更を開始するときの加速区間と、前記姿勢Ｂに達する手前で減速する減速区間とを除いて、姿勢変更経路の全区間で加減速無しで行うようにしても良い。
このように途中の点を加減速なしで位置決め制御することで、始点姿勢から終点姿勢までを一定速度で滑らかに駆動することができる。

【発明の効果】

【0024】

この発明のリンク作動装置の制御装置は、基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構のそれぞれに、前記基端側の端部リンク部材であるアームを回転させることにより、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータを設けたリンク作動装置において、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を、指令された始点姿勢から終点姿勢へ変更させるように、前記各アクチュエータを制御するリンク作動装置の制御装置であって、前記各アクチュエータを、前記始点姿勢となるときの前記アームの回転角である始点から前記終点姿勢となるときの回転角である終点へポイントツーポイントで互いに同期して駆動する姿勢変更制御手段と、前記始点姿勢から終点姿勢へ姿勢変更するときの前記先端側のリンクハブの姿勢変更量を所定量と比較し、所定量以上である場合に、前記始点姿勢から終点姿勢へ変更する姿勢変更経路の途中に、定められた規則によって１つ以上の中継姿勢を設定し、この中継姿勢になるときの前記各アームの回転経路の中継点を求めて設定する中継姿勢設定手段とを設け、前記姿勢変更制御手段は、前記中継姿勢設定手段で設定された前記中継点を前記各アームが同時に通過するように位置制御するため、先端側リンクハブの姿勢を大きく変更するときに、複数のリンク機構の相互の関係で一義的に定まる各アーム回転角から大きく外れることなく各アームのアクチュエータの位置決め制御が行えて、リンク機構の各部に過大な負荷を掛けずに駆動でき、かつ演算時間による遅れが回避できて高速移動が図れ、制御のための記憶メモリの容量も小さくて済む。

【図面の簡単な説明】

【0025】

【図1】この発明の一実施形態に係るリンク作動装置の制御装置のブロック図とその制御対象となるリンク作動装置の一部省略正面図とを組み合わせた説明図である。

【図2】同リンク作動装置の図１と異なる動作状態を示す一部省略正面図である。

【図3】同リンク作動装置のリンク作動装置本体の斜視図である。

【図4】同リンク作動装置のリンク機構の一つを直線で表現した幾何学的モデル図である。

【図5】同リンク作動装置の基端側のリンクハブ、基端側の端部リンク部材、および中央リンク部材の断面図である。

【図6】同リンク作動装置の制御装置における座標変換の処理の流れ図である。

【図7】リンク作動装置の制御装置による制御の一例を示す時間と速度を関係のグラフである。

【図8】従来の３節連鎖のリンク機構を３組以上設けたリンク作動装置の一例の斜視図である。

【図9】同リンク作動装置の姿勢変更動作の説明図である。

【図10】同リンク作動装置を姿勢変更する場合の折れ角と基端側の端部リンクの回転角との関係を示す図である。

【発明を実施するための形態】

【0026】

この発明の実施形態を図面と共に説明する。まず、制御対象となるリンク作動装置を、図１ないし図５と共に説明する。図１および図２に示すように、このリンク作動装置１は、リンク作動装置本体２と、このリンク作動装置本体２を作動させる複数の姿勢制御用のアクチュエータ３とを備え、これら姿勢制御用のアクチュエータ３が制御装置４により制御される。図の例では、リンク作動装置本体２が、その基端側で、支持部材５にスペーサ６を介して吊り下げ状態に設置されている。リンク作動装置本体２の先端側には、先端取付部材７を介してエンドエフェクタ８が搭載されている。

【0027】

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

【0028】

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

【0029】

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

【0030】

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

【0031】

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

【0032】

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

【0033】

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

【0034】

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

【0035】

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

【0036】

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

【0037】

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

【0038】

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

【0039】

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

【0040】

次に、図１と共に制御装置４につき説明する。制御装置４は、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢を、現在の姿勢である始点姿勢から、この制御装置４に対する外部の指令手段４０から与えられる終点姿勢へ変更するように、各アクチュエータ３を制御する装置である。制御装置４は、コンピュータによる数値制御式のものであり、主に基本的な制御を行う姿勢変更制御手段４１と、特徴的な制御に係る中継姿勢設定手段４２とを備える。

【0041】

姿勢変更制御手段４１は、前記各アクチュエータ３を、前記始点姿勢となるときの前記アーム１１ａ〜１３ａの回転角である始点から、前記終点姿勢となるときの回転角である終点へポイントツーポイントで駆動する手段である。姿勢変更制御手段４１は、各アクチュエータ３が互いに同時に始点から移動を開始し、互いに同時に終点に到着を行うように同期制御を行い、かつ互いに同時に中継点を通過するように制御する機能を備える。

【0042】

姿勢変更制御手段４１は、具体的には各リンク機構１１〜１３のアクチュエータ３をそれぞれ制御する複数の個別制御部４３と、指令変換部４４と、同期制御部４５と、個別指令部４８でなる。

【0043】

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

【0044】

指令変換部４４は、指令手段４０から折れ角θと旋回角φとで与えられる終点姿勢の指令Ｂ（θｂ，φｂ）を、各リンク機構１１〜１３のアーム１１ａ〜１３ａの回転角βに変換する手段であり、その変換された回転角βが各アーム回転角の終点となる。指令変換部４４は、この変換した各アーム１１ａ〜１３ａの回転角βを、対応する個別指令部４８へ終点の位置として与える。折れ角θと旋回角φから各アーム１１ａ〜１３ａの回転角βを求める演算は、後述の関係式（１）による逆変換で行われる。

【0045】

なお、始点姿勢は現在の姿勢であり、各個別指令部４８に与えられる始点は、現在の各アーム１１ａ〜１３ａの位置である。この始点は、前回移動時の終点とされ、または各回転角検出手段３５により検出された値と減速機構３３の減速比との値から見積られる現在位置、または定められた基準位置等とされる。

【0046】

同期制御部４５は、各アーム１１ａ〜１３ａが、同時に始点から回転を始め、同時に終点で止まるように、各個別制御部４３における前記台形速度制御における等速移動時の速度、および加減速時の加速度（または加減速時定数）を設定することで同期制御を行う手段である。加速度の代わりに、加速の終了時刻および減速の開始時刻を定めるようにしても良い。同期制御部４５は、この他に、前記中継点を各アーム１１ａ〜１３ａが同時に通過するように位置・速度制御されるように前記速度曲線を定める。

【0047】

中継姿勢設定手段４２は、先端側のリンクハブ１５が前記始点姿勢から終点姿勢へ姿勢変更するときの先端側リンクハブ１５の姿勢変更量を、設定部４６に定められた所定量と比較部４７によって比較し、所定量以上である場合に、姿勢変更経路の途中に、定められた規則によって１つ以上の中継姿勢を設定する手段である。中継姿勢設定手段４２は、この中継姿勢になるときの前記各アーム１１ａ〜１３ａの回転経路の中継点となる回転角を求めて設定する。なお、先端側のリンクハブ１５の姿勢変更の経路は、例えば、このリンクハブ１５の中心の経路とする。また、先端側のリンクハブ１５は、任意の経路で姿勢変更可能であるが、中継姿勢は、例えば最短経路となる姿勢変更経路を通る場合の中継姿勢とする。

【0048】

中継姿勢設定手段４２は、この例では、先端側のリンクハブ１５の前記始点姿勢から終点姿勢への姿勢変更量を所定量以下に分割した数をもとに、前記各アーム１１ａ〜１３ａの回転量を分割する。前記の「定められた規則」は、例えば、前記始点姿勢から終点姿勢へ姿勢変更する過程で動作する３つのアーム１１ａ〜１３ａのうち、２つのアームを選び、この選んだ２つのアームの回転経路を等分し、残り１つのアームは、他の２つのアームとの相対位置関係から一意に決まる位置としても良い。この３つのアーム１１ａ〜１３ａのうち２つのアームを選ぶ基準としては、回転量の大きいアームを選択することが好ましい。

【0049】

前記同期制御部４５は、上記のように中継姿勢設定手段４２で定めた各アーム１１ａ〜１３ａの中継点を、各アーム１１ａ〜１３ａが同時に通過するように、各個別制御部４３に前記速度曲線を定めさせる。各個別制御部４３は、この定められた速度曲線で各アーム１１ａ〜１３ａのアクチュエータ３を制御する。これにより、姿勢変更制御手段４１は、各アクチュエータ３が互いに同時に始点から移動を開始し、互いに同時に終点に到着を行い、かつ互いに同時に中継点を通過するように同期制御する。

【0050】

上記構成の動作を説明を説明する。制御対象となるリンク作動装置１は、前記折れ角θおよび旋回角φと、各基端側の端部リンク１１ａ，１２ａ，１３ａの回転角βｎ（β１，β２，β３）とが次式（１）で表わされる関係にある。
cos （θ／２）sin βｎ−sin （θ／２）sin （φ＋δｎ）cos βｎ＋sin （γ／２）＝０ …式（１）
ここで、γは、アーム１１ａ，１２ａ，１３ａに回転自在に連結された中央リンク部材１１ｃ，１２ｃ，１３ｃの連結端軸と、先端側の端部リンク部材１１ｂ，１２ｂ，１３ｂに回転自在に連結された中央リンク部材１１ｃ，１２ｃ，１３ｃの連結端軸とが成す角度である。δｎ（δ１，δ２，δ３）（図示せず）は、基準となるアーム１１ａに対する各基端側の端部リンク部材１１ａ，１２ａ，１３ａの円周方向の離間角である。リンク機構１１，１２，１３の数が３組で、各リンク機構１１，１２，１３が円周方向に等配である場合、各アーム１１ａ，１２ａ，１３ａの離間角δ１，δ２，δ３はそれぞれ０°，１２０°，２４０°となる。

【0051】

この関係より、先端側のリンクハブ１５のある始点姿勢（折れ角、旋回角）Ａ（θa 、φa ）と終点姿勢Ｂ（θb 、φb ）について、これらの姿勢Ａ、Ｂに対応するアーム回転角は、リンクハブ１４，１５とアーム回転角との前記関係式（１）より、それぞれ回転角Ａ（β１a 、β２a 、β３a ）、回転角Ｂ（β1b、β2b、β3b）として関係が成り立つ。

【0052】

このとき、中継姿勢設定手段４２は、先端側のリンクハブ１５の始点姿勢Ａから終点姿勢Ｂへの先端側のリンクハブ１５の移動量を求め、求めた移動量が所定の移動量より大きい場合、所定の移動量以下になるように始点姿勢Ａから終点姿勢Ｂの間を複数の姿勢に分割する。ここで所定の移動量とは、例えば、モータからなる各アクチュエータ３のモータトルクが所定の値以下で駆動できる移動量とする。この移動量は、リンク作動装置１の幾何学的な寸法及びリンクに与える初期与圧によって決まる。また、駆動するモータトルクについての所定の値とは、例えば、リンク作動装置１の入力部となる前記減速機３３等の入力トルク許容値以下の値を示す。

【0053】

このように姿勢変更時の経路を分割し、その分割部である中継点を各軸のアーム１１ａ〜１１ｃが同時に通過するように制御することで、各軸のアーム１１ａ〜１１ｃの干渉を緩和し、リンクの組付け不具合や、異常な摩耗等を与えないような適正なモータトルクで駆動することができる。ここで言う「干渉」は、上記のようにアクチュエータ３への指令値に対して偏差を生じたアーム回転角となることを言う。始点姿勢Ａから終点姿勢Ｂへの先端側リンクハブ１５の移動経路は近似して求める。
また、中継点を求める演算は必要であるが、基本的にはポイントツーポイント制御であるため、先端側リンクハブ１５の姿勢変更の全経路において干渉を生じないように回転角を制御するものと異なり、制御に必要な演算時間が短くて済んで、演算時間による遅れが生じず、高速移動が図れる。また、予め全経路の座標を記憶して制御するものと異なり、記憶メモリの容量が小さくて済む。

【0054】

中継姿勢設定手段４２により中継点を求める分割方法としては、次の第１〜第３の例が採用できる。
まず、分割方法の第１の例を表１に示す。

【表1】

【0055】

始点姿勢Ａから終点姿勢Ｂへの移動においてアーム軸β１、β２、β3 の回転角移動量を求め、移動量の大きい２つのアームを選ぶ。このとき移動量の大きいアーム軸をβ１、β２とすると、β１、β２の移動量を先に求めた分割数ｎで等分割し、各アーム１１〜１３の分割された回転角位置を決める。このとき分割数ｎは、先端側リンクハブ１５の始点姿勢Ａから終点姿勢Ｂへの移動量をＬ、所定の移動量をｄとし、Ｌ÷ｄ＝ｍ（商）…ａ（剰余）として、分割数ｎを、ｎ＝ｍ＋１とする。

【0056】

アーム軸β３の回転角は、２軸のアーム軸β１、β２の回転角と順変換アルゴリズムから求められる先端側リンクハブ１５の姿勢（θｎ，φｎ）を用いて、逆変換によりβ３のアーム回転角を得る（図６参照）。順変換について、リンクハブとアーム回転角との前記関係式（１）よりβ1a、β2a、θ、φの４つの角度は次式（２）、（３）を満足する。
ｆ：cos θ sinβ_1a− sinθ sinφ cosβ_1a＋ sinγ＝０≡ｆ（θ，φ） …式（２）
ｇ：cos θ sinβ_2a− sinθ sin（φ＋σ) cosβ_2a＋sin γ＝０≡ｇ（θ，φ）
…式（３）

【0057】

この２式はθ，φの非線形方程式であるため、数値的に解を求める。この２軸のアーム軸β１、β２の回転角から順変換の変形式を用いて先端側リンクハブ１５の姿勢（θｎ，φｎ）を求める演算は、例えば収束演算で行う。ここでγはリンクの設計構造によって決まる軸角度、σは３つアームの配列位置によって決まる位相角度とする。また、逆変換についてはリンクハブとアーム回転角との前述の関係式（１）より、あるリンクハブ姿勢（θ、φ）におけるβ１、β2 、β3 のアーム回転角は一意に求まる。表２に３軸のアームの回転角移動量を分割した位置を示す。

【0058】

【表2】

【0059】

リンク姿勢制御は、表１に示す各軸の回転角位置を、同期制御部４５の制御により同期して移動するように行う。

【0060】

この制御では移動量の大きい２つのアーム１１ａ，１１ｂが各軸の最大速度で等速に移動できるため、最短の移動時間で駆動できる。また、移動時間を考慮しなければβ１、β２、β３のうち２つのアームの選択は任意に選んでもよい。ただし、このときリンク先端の軌跡は最短経路の通過を保証しない。

【0061】

第２の分割方法を示す。この方法では、先端側のリンクハブ１５が最短距離で移動する経路上に一つ以上の中継姿勢をとり、前記経路上の、前記始点姿勢、中継姿勢、および終点姿勢のいずれかである各姿勢のうち、隣合う姿勢間の成す距離等の量が所定量以下となるリンクハブ１５の姿勢（θ，φ）に対応する各アーム１１ａ〜１３ａの回転角を前記中継点とする構成とする。この場合に、前記リンクハブ１５の姿勢（θ，φ）から逆変換の変換式により各アーム１１ａ〜１３ａの回転角を求めるようにしても良い。

【0062】

第２の分割方法は、より具体的には、先端側リンクハブ１５のある始点姿勢（折れ角、旋回角）Ａ（θa 、φa ）と終点姿勢Ｂ（θb 、φb ）について、姿勢Ａ、Ｂ間を移動する先端側リンクハブ１５の経路が、できるだけ最短になるように求める。このとき必要に応じて適切な近似を行うことで経路を求める時間を短縮することができる。始点姿勢Ａから終点姿勢Ｂへの経路を複数の姿勢に分割し、各先端側のリンクハブ１５の姿勢Ｎ（θn ，φn)から、逆変換アルゴリズムにより各リンクハブの姿勢に対するアーム軸の回転角Ｎ（β１n 、β2n、β3n）を得る。なお、前記逆変換アルゴリズムは、例えば、前述の関係式（１）に従って、折れ角θおよび旋回角φから回転角βｎを算出する変換である。
リンク姿勢制御は、同期制御部４５の制御により、各軸のアクチュエータ３の位置を同期して移動するように行う。

【0063】

第３の分割方法を示す。この方法では、先端側のリンクハブ１５のある始点姿勢Ａ（θａ，φａ）と終点姿勢Ｂ（θｂ，φｂ）から、それぞれ折れ角θの移動量Δθと旋回角φの移動量Δφが所定の移動量以下となるように等分した先端側の各リンクハブ１５の姿勢（θ１，θ２，…θｎ，φ１，φ２，…φｎ）（ただし、ｎ：分割数−１）を逆変換の変換式により求め、この求めたリンクハブ１５の姿勢（θ１，θ２，…θｎ，φ１，φ２，…φｎ）を前記中継姿勢とする。

【0064】

第３の分割方法は、具体的には、先端側のリンクハブ１５のある始点姿勢（折れ角、旋回角）Ａ（θa 、φa ）と終点姿勢Ｂ（θb 、φb ）の間にて、折れ角移動量Δθと旋回角移動量Δφを各々等間隔に分割し、先端側のリンクハブ１５の複数の姿勢を求める。このとき分割した移動量が所定の移動角度以下となるようにする。次に先端側のリンクハブ１５の各姿勢Ｎ( θa + n ×Δθ / M、φa + n ×Δφ / M) から、逆変換アルゴリズムによりアーム軸の回転角Ｎ（β１n 、β2n、β3n）を得る。ここでＮ，ｎは分割した各姿勢のＮ（またはｎ）番目を示し、Ｍは分割数とする。
リンク姿勢制御は、同期制御部４５の制御により、各軸の位置を同期して移動するように行う。このように、折れ角θの移動量Δθと旋回角φの移動量Δφが共に所定の移動量以下となるように等分することで、より一層干渉が少なくて過大な負荷が生じない駆動を行うことができる。

【0065】

上記第１，第２，第３のいずれの分割方法の場合においても、姿勢変更制御手段４１の同期制御部４５は、これらの方法で得た先端側のリンクハブ１５のある始点姿勢Ａ（θa 、φa ）から終点姿勢Ｂ（θb 、φb ）への移動経路について、図７に示すように各アーム軸β１〜β３の回転角βＡ（β１a 、β２a 、β３a ）、回転角βＢ（β1b、β2b、β3b）への制御は、途中の中継点を加減速なしで位置決め制御することが好ましい。この場合、表３に示す各軸の回転角位置を、同期制御部４５の制御により同期して移動するように行う。すなわち、全てのアーム１１ａ〜１３ａの軸β１〜β３につき、台形速度制御の加速を行う時間（時刻(1) 〜(2) ）、および減速を行う時間（時刻(4) 〜(5) ）を揃え、例えば時刻(3) 等のように、等速動作を行う間（時刻(2) 〜(4) ）に、各中継点を通過するように制御する。

【0066】

【表3】

【0067】

このように中継点を加減速なしで位置決め制御することにより、始点姿勢Ａ点から終点姿勢Ｂまでを一定速度で滑らかに駆動することができる。

【0068】

以上の各実施形態によると、リンク作動装置１を広角に姿勢変更させる場合、駆動途中の各アーム回転角の相対位置関係が、複数のリンク機構の相互の関係で一義的に定まる式（１）を満たす位置から大きく外れることなく位置決め制御させることができ、これにより、リンクに過大荷重をかけず、かつ高速移動が可能になる。

【符号の説明】

【0069】

１…リンク作動装置
３…アクチュエータ
５…基端側の支持部材
７…先端取付部材
８…エンドエフェクタ
１４…基端側のリンクハブ
１５…先端側のリンクハブ
１１〜１３…リンク機構
１１ａ，１２ａ，１３ａ…基端側の端部リンク部材（アーム）
１１ｂ，１２ｂ，１３ｂ…先端側の端部リンク部材
１１ｃ，１２ｃ，１３ｃ…中央リンク部材
３３…減速機
４１…姿勢変更制御手段
４２…中継姿勢設定手段
４３…個別制御部
４４…指令変換部
４５…同期制御部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】