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

Description

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

この種のリンク作動装置として、基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結したものが提案されている（例えば特許文献１）。前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなる。各リンク機構は、このリンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状である。前記３組以上のリンク機構のうち２組以上のリンク機構に、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータが設けられる。

このリンク作動装置を制御する制御装置は、アクチュエータの全てが同時に動作を開始して同時に動作を完了するように制御する同期制御により、前記アクチュエータごとの動作を制御して、前記先端側のリンクハブを任意の姿勢に変更する姿勢制御を行う構成とされる。

特開２０００−９４２４５号公報 米国特許第５，８９３，２９６号明細書

前記リンク作動装置の制御装置として、前記全てのアクチュエータの減速時間を、前記リンク作動装置の持つ共振周波数の１周期付近に設定して、前記同期制御および姿勢制御を行うことが試みられている。なお、ここで言う共振周波数は、先端側のリンクハブに搭載物を設置した状態での共振周波数のことである。

前記構成のリンク作動装置においては、直線加減速制御で先端部を駆動する場合、特に先端部の負荷が大きい場合、リンク部の剛性により先端部の振動が収まる整定時間が長くなるという問題がある。その対策の一つとしてリンクハブの姿勢変更時のアクチュエータの加速時間と減速時間を、リンク作動装置のもつ共振周波数（リンク作動装置の先端側リンクハブに搭載物を設置した状態での共振周波数）の１周期付近に設定することで、先端部が共振周波数の約半周期後に加速を打ち消す方向に振動し、整定時間を短縮させる方法がある。
しかし、リンク姿勢変更時の移動量が小さく、指令速度が大きい条件では、リンクの動作時間が加速時間と減速時間の合計以下になり、リンク先端部となる先端側リンクハブが振動して整定時間が伸び、結果として動作時間が長くなる場合がある。

この発明の目的は、リンク姿勢変更時の移動量が小さく、指令速度が大きい条件において、自動で制御条件を変更して先端側リンクハブの整定時間を短くすることができるリンク作動装置の制御装置を提供することである。

この発明のリンク作動装置の制御装置は、基端側のリンクハブ１４に対し先端側のリンクハブ１５を、３組以上のリンク機構１１〜１３を介して姿勢を変更可能に連結し、前記各リンク機構１１〜１３は、それぞれ前記基端側のリンクハブ１４および先端側のリンクハブ１５に一端が回転可能に連結された基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂと、これら基端側および先端側の端部リンク部材１１ａ〜１３ａ，１１ｂ〜１３ｂの他端に両端がそれぞれ回転可能に連結された中央リンク部材１１ｃ〜１３ｃとでなり、前記各リンク機構１１〜１３は、このリンク機構１１〜１３を直線で表現した幾何学モデルが、前記中央リンク部材１１ｃ〜１３ｃの中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構１１〜１３のそれぞれに、前記基端側の端部リンク部材であるアーム１１ａ〜１３ａを回転させることにより、前記基端側のリンクハブ１４に対する前記先端側のリンクハブ１５の姿勢を任意に変更させるアクチュエータ３を設けたリンク作動装置において、
前記基端側のリンクハブ１４に対する前記先端側のリンクハブ１５の姿勢を、現在姿勢から指令された目標姿勢へ変更させるように、前記各アクチュエータ３を制御するリンク作動装置の制御装置４であって、
次の基本構成を備える。

この基本構成は、前記各アクチュエータ３を、前記現在姿勢であるときの前記アーム１１ａ〜１３ａの回転角から前記目標姿勢となるときの前記アーム１１ａ〜１３ａの回転角までの指令アーム回転角度だけ、アーム回転制御の制御パラメータである加速時間、減速時間、および指令速度に従って、互いに同期して駆動する姿勢変更制御手段４１を備え、
前記制御パラメータのうち、加速時間Ｔacc と減速時間Ｔdcc の合計を前記リンク作動装置１の共振周波数の１周期に設定し、
前記アクチュエータ３の加速時間Ｔacc 、減速時間Ｔdcc 、指令速度Ｖ０、および指令アーム回転角度から推定動作時間Ｔｅを推定する動作時間推定手段４９と、
この推定された推定動作時間Ｔｅと加速時間Ｔacc および減速時間Ｔdcc の合計とを比較する条件選定手段５０と、
この条件選定手段５０による比較の結果に応じて定められた規則に従い前記制御パラメータを変更するパラメータ変更手段５１とを設けた構成である。

この構成の制御装置４によると、初期の制御パラメータのうち加速時間Ｔacc と減速時間Ｔdcc を共振周波数の１周期で設定し、かつ直線加減速制御で駆動させる制御装置４において、アクチュエータ３の加速時間Ｔacc 及び減速時間Ｔdcc 、指令速度Ｖ０、指令アーム回転角度から推定動作時間Ｔｅを推定する動作時間推定手段４９により推定動作時間を求める。この求めた推定動作時間Ｔｅと加速時間Ｔacc 及び減速時間Ｔdcc の合計とを比較する条件選定手段５０の結果をもとに制御パラメータを変更するパラメータ変更手段５１を用いて、その動作時の制御パラメータを変更し、姿勢変更制御手段４１で駆動する。これにより、先端側リンクハブ１５の整定時間が短くなり、この整定時間を含むリンク姿勢変更の動作時間を短縮することができる。

前記基本構成において、前記パラメータ変更手段５１は、前記条件選定手段５０による比較の結果、前記推定動作時間Ｔｅが前記加速時間Ｔacc および減速時間Ｔdcc の合計以下であって、かつ前記推定動作時間Ｔｅがある所定の時間範囲にある場合に前記制御パラメータの変更を行う。具体的には、前記指令速度を低下させる。このように指令速度を低下させることにより、加速時間Ｔaccと減速時間Ｔdccの合計をリンク作動装置が持つ共振周波数の１周期付近に設定すれば整定時間が短くなり、通常制御ではリンク先端の振動が大きく整定時間が長くなる場合と比較して、この整定時間を含むリンク姿勢変更動作時間を短縮することができる。なお、駆動条件によっては加速時間と減速時間の合計が共振周波数の１周期でなくても振動が大きくならない場合がある（例えば、低速時や１周期近辺等。）。よって、パラメータ変更条件は所定の時間範囲内とすることが好ましい。

前記「ある所定の時間」は、例えば、前記共振周波数の１周期とは時間が異なり、かつその所定時間範囲内の時間で加速時間と減速時間を設定すると位置決め完了後のリンク先端の整定時間悪化に影響を及ぼすことになる時間とする。

また、前記基本構成の制御装置において、前記推定動作時間が加速時間と減速時間との合計以上となる場合に、前記パラメータ変更手段５１は、制御パラメータのうちの指令速度Ｖ１を、初期設定した指令速度Ｖ０よりも速い速度に変更しても良い。
これにより、例えば、前記加速時間と減速時間を維持したままで、リンク姿勢変更の動作時間を短縮することができる。

この場合に、前記パラメータ変更手段は、前記推定動作時間が加速時間と減速時間との合計と等しくなるように、前記指令速度を速く設定しても良い。
この場合、前記加速時間Ｔacc と減速時間Ｔdcc を維持したままで、リンク姿勢変更の動作時間が最短になるように短縮することができる。

この発明の制御装置において、前記推定動作時間が加速時間と減速時間との合計よりも小さい場合、前記パラメータ変更手段５１は、前記制御パラメータのうちの前記指令速度を、前記アクチュエータ３の動作が所定の推定動作時間Ｔｅとなるような指令速度に低下させる。
この場合、リンク先端の振動を抑制でき、整定時間を含む動作時間を短縮できるという効果が得られる。
この場合に、前記所定の推定動作時間Ｔｅは、加速時間Ｔacc と減速時間Ｔdcc の合計としても良い。
一般に移動距離が短い場合、モータは加速した後、中間地点で減速に切替わる。動作時間は加速時間と減速時間の合計となり、この合計時間がリンク装置の共振周波数の１周期分から大きく乖離するとリンク先端は位置決め完了後に大きく振動する。しかし、振動を抑制する動作時間（加速時間と減速時間の合計）をリンク装置の共振周波数の１周期に合わせると振動は抑制できる。

この発明の制御装置において、前記パラメータ変更手段５１は、前記指令速度を、この指令速度の初期速度と移動量である指令アーム回転角度から求めた推定動作時間に比例した割合で減速した速度に変更しても良い。

この発明の制御装置において、前記パラメータ変更手段５１は、前記指令速度を、初期速度に対して所定の割合で減速した速度に変更するようにしても良い。

この発明の制御装置において、前記パラメータ変更手段５１は、制御パラメータの変更により、前記姿勢変更制御手段４１による前記直線加減速制御をＳ字加減速制御に変更するようにしても良い。
Ｓ字加減速制御であると、制御は複雑となるが、加減速を急加速度で行うようにしても、加減速の完了時における整定時間がより一層短縮される。

この発明の制御装置において、前記パラメータ変更手段５１は、制御パラメータの変更により、前記姿勢変更制御手段による前記直線加減速制御を指数関数型の加減速制御にするようにしても良い。
指数関数型の加減速制御にした場合も、加減速を急加速度で行うようにしても、加減速の完了時における整定時間がより一層短縮される。

この発明の制御装置において、前記パラメータ変更手段５１は、加減速制御の時定数を変える場合に、加減速制御の時定数を推定動作時間に比例した割合で設定した時定数としても良い。

この発明の制御装置４において、前記パラメータ変更手段５１は、加速時間または減速時間が固定で設定されている場合、指令速度を変更する。

この発明の制御装置４において、前記パラメータ変更手段５１は、前記指令速度の代わりに加減速度を変更して各条件を満たす制御を行うようにしても良い。
なお、この発明で必要なことは加速時間と減速時間の合計が共振周波数の１周期となるように制御して、位置決め完了後のリンク先端の振動を抑制することである。そのための速度パラメータは加速・減速時間と（到達）速度、加速度・減速度と（到達）速度のいずれであっても問題がない。

この発明のリンク作動装置の制御装置は、基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構のそれぞれに、前記基端側の端部リンク部材であるアームを回転させることにより、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータを設けたリンク作動装置において、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を、現在姿勢から指令された目標姿勢へ変更させるように、前記各アクチュエータを制御するリンク作動装置の制御装置であって、前記各アクチュエータを、前記現在姿勢であるときの前記アームの回転角から前記目標姿勢となるときの前記アームの回転角までの指令アーム回転角度だけ、アーム回転制御の制御パラメータである加速時間、減速時間、および指令速度に従って、互いに同期して駆動する姿勢変更制御手段を備え、前記制御パラメータのうち、加速時間と減速時間の合計を前記リンク作動装置の共振周波数の１周期に設定し、前記アクチュエータの加速時間、減速時間、指令速度、および指令アーム回転角度から動作時間を推定する動作時間推定手段と、この推定された推定動作時間と加速時間および減速時間の合計とを比較する条件選定手段と、この条件選定手段による比較の結果に応じて定められた規則に従い前記制御パラメータを変更するパラメータ変更手段とを設け、 前記パラメータ変更手段は、前記条件選定手段による比較の結果、前記推定動作時間が前記加速時間および減速時間の合計以下であって、かつ前記推定動作時間がある所定の時間範囲内にある場合に、前記定められた規則に従う変更として、前記制御パラメータにおける前記指令速度を、変更後の指令速度で前記アクチュエータを動作させた場合の前記推定動作時間が前記共振周波数の１周期付近となるように遅く変更し、前記所定の時間は、前記共振周波数の１周期とは時間が異なり、かつその所定時間範囲内の時間で加速時間と減速時間を設定すると位置決め完了後のリンク先端の整定時間悪化に影響を及ぼすことになる時間とするため、リンク姿勢変更時の移動量が小さく、指令速度が速い条件において、自動で制御条件を変更して先端側リンクハブの整定時間を短くすることができる。

この発明の一実施形態に係るリンク作動装置の制御装置のブロック図とその制御対象となるリンク作動装置の一部省略正面図とを組み合わせた説明図である。 同リンク作動装置のリンク作動装置本体の斜視図である。 同リンク作動装置の動作説明図である。 同リンク作動装置のリンク機構の一つを直線で表現した幾何学的モデル図である。 同リンク作動装置の基端側のリンクハブ、基端側の端部リンク部材、および中央リンク部材の断面図である。 リンク作動装置の制御装置による制御パラメータの変更の過程を示す流れ図である。 同リンク作動装置のアーム回転の動作時間と速度との関係を示すグラフである。 同リンク作動装置の制御装置における制御パラメータの変更前後におけるアーム回転の動作時間と速度との関係例を示すグラフである。 同リンク作動装置の制御装置における制御パラメータの変更前後におけるアーム回転の動作時間と速度との他の関係例を示すグラフである。

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

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

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

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

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

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

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

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

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

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

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

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

図１において、前記複数のアクチュエータ３は、支持部材５の上に円周方向に等配で設置されている。アクチュエータ３の個数は、リンク機構１１，１２，１３と同数の３個である。この実施形態では、アクチュエータ３はモータからなり、その出力軸の回転が減速機構３３（図５）を介してアーム１１ａ〜１３ａに伝達される。

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

つぎに、図１と共に制御装置４につき説明する。制御装置４は、基端側のリンクハブ１４に対する先端側のリンクハブ１５の姿勢を、現在の姿勢から、この制御装置４に対する外部の外部指令手段４０から与えられる目標姿勢へ変更するように、各アクチュエータ３を制御する装置である。外部指令手段４０は、このリンク作動装置１を組み込んだ機器（図示せず）や、操作盤（図示せず）等に設けられる。制御装置４は、コンピュータによる数値制御式のものであり、主に姿勢変更制御手段４１により構成される。
なお、姿勢変更制御手段４１は、先端側のリンクハブ１５を、外部指令手段４０から与えられた目標姿勢である最終目標姿勢へ現在の姿勢から姿勢変更させるにつき、通常は、途中で通過させる複数のポイント（ポイント間毎に移動量が異なる）を所定の演算により求めて登録しておき、各登録ポイントへ順次移動させるように、指令を順次行う。以下で言う「現在の姿勢」および「目標姿勢」は、個々のポイント間の移動における移動前のポイントと移動先のポイントのことである。前記複数のポイントを演算により求めて登録するポイント演算手段（図示せず）は、制御装置４における姿勢変更制御手段４１の内部または外部で、後述の指令変換部４２の上位に設けられる。この他に、外部指令手段４０が各ポイントを指令するようにしても良い。

姿勢変更制御手段４１は、各アクチュエータ３を、現在姿勢であるときのアーム１１ａ〜１３ａの回転角から目標姿勢となるときのアーム１１ａ〜１３ａの回転角までの指令アーム回転角度だけ駆動する手段である。姿勢変更制御手段４１は、具体的には、各リンク機構１１〜１３のアクチュエータ３をそれぞれ制御するアクチュエータドライバである複数の個別制御部４４と、指令変換部４２と、同期制御部４３とでなる。

指令変換部４２は、前記外部指令手段４０から前記ポイント演算手段（図示せず）を介して、または直接に折れ角θと旋回角φとで与えられる目標姿勢の指令Ｂ（θｂ，φｂ）を、各リンク機構１１〜１３のアーム１１ａ〜１３ａの回転角βに変換することを基本の機能とする手段であり、その変換された各アーム１１ａ〜１３ａの回転角β（β１〜β３）と、現在の各アーム１１ａ〜１３ａの回転角αとの差が、それぞれ請求項で言う指令アーム回転角度となる。折れ角θと旋回角φから各アーム１１ａ〜１３ａの回転角βを求める演算は、後述の関係式（１）による逆変換で行われる。
指令変換部４２は、上記基本の機能を持つ手段に加えて、後述の機能を持つ初期パラメータ生成部４７および判断・変更部４８を有する。
指令変換部４２で変換して得られた指令アーム回転角度は、同期制御部４３を介して、各個別制御部４４に与えられる。

概要を説明すると、前記指令変換部４２内で初期パラメータ生成、所定の判断・変更をして、各ポイント間の速度や各アクチュエータ（モータ位置）座標を予め計算する。
同期制御部４３は、各アーム１１ａ〜１３ａの回転の同期制御を行わせる手段であり、同期タイミングに合わせて個別制御部（アクチュエータドライバ）４４へ毎時毎の位置指令を送信する。個別制御部４４では同期制御部４３の指令に応じてアクチュエータ３を駆動する。同期制御部４３は、各個別制御部４４へ指定するパラメータおよび位置をそれぞれ設定するパラメータ設定部５２および位置設定部５３を有し、パラメータ設定部５２には、指令変換部４２により初期生成された制御パラメータを、変更したときはその変更した制御パラメータを設定する。位置設定部５３には指令アーム回転角度を設定する。

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

指令変換部４２の初期パラメータ生成部４７は、上位から与えられた各アーム１１ａ〜１３ａの指令アーム回転角度だけ、各アーム１１ａ〜１３ａが同時に回転を始めて同時に回転が止まるように制御する制御パラメータを、定められた規則に従って制御パラメータの初期値として生成する。この初期値として生成する制御パラメータには、例えば加速時間、減速時間、および指令速度を含み、加速，減速の時定数は一定値として直線加減速制御を行わせる。
初期パラメータ生成部４７は、制御パラメータのうち、加速時間と減速時間を、リンク作動装置の共振周波数の１周期に設定する。ここで言う共振周波数は、先端側のリンクハブ１５にエンドエフェクタ８等の、搭載された全ての搭載物を設置した状態での共振周波数のことである。

判断・変更部４８は、初期パラメータ生成部４７で生成された制御パラメータを、定められた基準で判断し、変更すべきと判断した場合に変更する手段である。
判断・変更部４８は、図６に流れ図に示した処理を行う手段であって、図１の動作時間推定手段４９、条件選定手段５０、およびパラメータ変更手段５１を有する。
動作時間推定手段４９は、前記アクチュエータ３の加速時間、減速時間、指令速度、および指令アーム回転角度から動作時間を推定する。
条件選定手段５０は、この推定された推定動作時間と、加速時間および減速時間の合計とを比較する手段である。
パラメータ変更手段５１は、条件選定手段５０による比較の結果により、前記制御パラメータの初期値を変更する手段である。パラメータ変更手段５１によりどの制御パラメータをどのように変更するかは、次の制御動作および作用の説明欄で説明する。

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

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

姿勢変更の制御を説明する。先端側のリンクハブ１５の姿勢変更は、それぞれのアーム１１ａ〜１３ａが直線加減速制御で同期してＡのアーム回転角からＢのアーム回転角へ駆動することで行われる。ここで図６に制御パラメータを決めるフローチャートを示す。

概要を説明すると、各アーム１１ａ〜１３ａ毎に、これらアーム１１ａ〜１３ａを駆動するアクチュエータ３の加速時間と減速時間、指令速度、指令アーム回転角度から、動作時間推定手段４９により推定動作時間を得る（Ｓ１）。次に条件選定手段（５０）により、先に推定した動作時間からパラメータ変更の有無を判定する（Ｓ２）。

図７にアーム１１ａ〜１３ａを回転させるアクチュエータ３の推定動作時間Ｔｅと、加速時間Ｔacc 、減速時間Ｔdcc との関係を示す。図７(a) は推定動作時間Ｔｅが加速時間Ｔacc 及び減速時間Ｔdcc の合計よりも長い場合を示し、図７(b) は推定動作時間Ｔｅが加速時間Ｔacc 及び減速時間Ｔdcc の合計よりも短い場合を示す。同図に斜線を施した部分の面積が移動量ｄ（現在姿勢であるときのアームの回転角から目標姿勢となるときのアームの回転角までの指令アーム回転角度）である。条件選定手段５０は、図７に示すような関係からパラメータ変更の有無を判断する（Ｓ２）。
判断の結果、パラメータ変更が必要な場合、パラメータ変更手段５１により制御パラメータを変更する（Ｓ４）。

条件選定手段５０において推定動作時間Ｔｅと加速時間Ｔacc 及び減速時間Ｔdcc の合計とを比較し、図８（ａ）のように推定動作時間Ｔｅの方が長い場合は、パラメータ変更手段５１は、図８（ｂ）のようにアーム１１ａ〜１３ａを駆動する指令速度Ｖ１を、初期値である指令速度Ｖ０よりも速くする。このときの指令速度Ｖ１は、変更後の動作時間Ｔｆが加速時間Ｔacc 及び減速時間Ｔdcc の合計未満とならないようにする。また、速度の上限Ｖmax が設定されている場合、図８（ｃ）のように上限速度Ｖmax での台形駆動になる。なお、変更後の動作時間Ｔｆが加速時間Ｔacc 及び減速時間Ｔdcc の和になるようにして変更前と同じ動作量だけ動作させるように変更した場合に、変更後の指令速度Ｖ１が上限速度Ｖmax 達しない場合は、図８（ｂ）と同様の三角形移動になる。

前記条件選定手段５０（図６）で推定動作時間Ｔｅと加速時間Ｔacc 及び減速時間Ｔdcc の合計とを比較したときに（Ｓ２）、図９(a) に示すように推定動作時間Ｔｅの方が短い場合は、図９(b) のように、パラメータ変更手段５１により、加速時間Ｔacc 及び減速時間Ｔdcc の合計が推定動作時間Ｔｅと等しくなる指令速度Ｖ１、または加速度に変更する。なお、ここでは、このように変更する変更規則を、手法（１）となる定められた規則と称する。

この構成のリンク作動装置の制御装置によると、上記のように条件判定し、制御パラメータを変更するため、リンク姿勢変更時の移動量が小さく、指令速度Ｖ０が速い条件において、自動で指令速度を変更して先端側のリンクハブ部１５の整定時間を短くすることができる。

パラメータ変更手段５１による制御パラメータを変更する定められた規則は、上記手法（１）による他に、次の手法（２）〜（６）のいずれかを用いる規則としても良い。
手法（２）では、図９(a) に示すように推定動作時間Ｔｅの方が短い場合に、手法（１）の代わりに、パラメータ変更手段５１により、初期設定された指令速度Ｖ０と移動量ｄから求めた推定動作時間Ｔｅに比例した割合で減速した速度に変更する。

手法（３）では、パラメータ変更手段５１により、初期に設定した指令速度Ｖ０を所定の割合で減速した速度に変更する。

手法（４）では、パラメータ変更手段５１により、直線加減速制御からＳ字加減速制御に変更する。Ｓ字加減速制御は、直線加減速制御における加速および減速を、直線に代えてＳ字状の速度曲線となるように、時定数を変えながら加減速する制御である。
なお、時定数を変える場合、各個別制御部４４は、設定された時定数を用いて制御を行う構成とする。
手法（５）では、パラメータ変更手段５１により、直線加減速制御から指数関数型の加減速制御に変更する。指数関数型の加減速制御は、直線加減速制御における加速および減速を、直線に代えて指数関数型の速度曲線となるように、時定数を変えながら加減速する制御である。
Ｓ字加減速制御や指数関数型の加減速制御であると、制御は複雑となるが、加減速を急加速度で行うようにしても、加減速の完了時における整定時間がより一層短縮される。
前記手法（４）と手法（５）については、加減速制御の時定数を推定動作時間Ｔｅに比例した設定割合で設定した時定数としてもよい。

手法（６）では、パラメータ変更手段５１により、加速時間Ｔacc 及び減速時間Ｔdcc を変更する。なお、前記手法（２）〜（６）については、特に変更を記載した制御パラメータ以外の他の制御パラメータは初期値のままとする。

前記条件選定手段５０（ステップＳ２）において、制御パラメータを変更する条件は、アーム１１ａ〜１３ａの推定動作時間Ｔｅが、加速時間Ｔacc 及び減速時間Ｔdcc との合計以下で、かつ所定の動作時間以上の場合としてもよい。例えば閾値となる所定の動作時間を３０msec、加速時間を４０msec、減速時間を４０msecと設定し、移動量や速度等の諸条件によりアーム１１ａ〜１３ａの推定動作時間が２０msecとなる場合では制御パラメータは変更せず、初期値のパラメータで駆動する。ここで閾値となる所定の動作時間はリンク作動装置１の機械的特性により決定され、推定動作時間Ｔｅをゼロから徐々に増加させ、位置決め完了後のリンク先端の整定時間の増加が見られる時の時間とする。

また、条件選定手段５０およびパラメータ変更手段５１における扱う速度に関する制御パラメータを、速度の代わりに加速度または減速度とし、加速時間Ｔacc および減速時間Ｔdcc が共振周波数の一周期になるように制御しても良い。

なお、上記実施形態では、個々のアーム１１ａ〜１３ａ毎に制御パラメータの変更を説明したが、３つのアクチュエータ３を例に、相互の同期につき説明する。３つのアクチュエータ３の各速度をＶ１、Ｖ２、Ｖ３とすると３つのアクチュエータ３の合成速度ＶはＶ＝√(Ｖ１×Ｖ１＋Ｖ２×Ｖ２＋Ｖ３×Ｖ３)となり、Ｖ１、Ｖ２、Ｖ３は各アクチュエータの移動量Ｌ１、Ｌ２、Ｌ３に比例した値になる。今回、ある１軸について変更する速度を計算するが、他の２軸についても距離に応じて同じ割合で速度変更するため、同期は崩れない。

上記実施形態のリンク作動装置の制御装置につき、要点を纏め直して以下に説明する。この実施形態は、リンク作動装置１を高速動作で位置決め時の先端側リンクハブ１５の振動抑制技術である。主に、リンク駆動時の加速時間と減速時間の合計がリンク作動装置１の共振周波数の１周期付近になるように制御することで振動を抑制できることを利用する。移動距離が微小の場合、ある指令速度で駆動すると、駆動時間が共振周波数の１周期以下で駆動（この場合、加速した後減速する三角駆動のため、駆動時間＝加速時間＋減速時間となる）し、先端側リンクハブ１５が位置決め完了後に大きく振動する。この実施形態では、移動距離が微小の場合、予め設定された速度よりも速度を低下させて駆動し、駆動時間（加速時間と減速時間の合計）が共振周波数の１周期付近になるようにして、位置決め完了後の先端側リンクハブ１５の振動を抑制するものである。一般にこの装置は、複数のポイント（ポイント毎に移動量が異なる）を登録し、連続して登録ポイントへ移動して作業をする。
これより移動量が大きく加速・減速時間が十分にとれる（台形駆動）ポイント間動作では指令速度で駆動し、微小移動量のポイント間のみ速度を落とし、設定した加速時間と減速時間が十分確保できる駆動パターンで動作することで、先端の振動を抑制しながら、複数設定したポイント間をできるだけ高速で駆動することができる。

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

Claims (6)

1. 基端側のリンクハブに対し先端側のリンクハブを、３組以上のリンク機構を介して姿勢を変更可能に連結し、前記各リンク機構は、それぞれ前記基端側のリンクハブおよび先端側のリンクハブに一端が回転可能に連結された基端側および先端側の端部リンク部材と、これら基端側および先端側の端部リンク部材の他端に両端がそれぞれ回転可能に連結された中央リンク部材とでなり、前記各リンク機構は、このリンク機構を直線で表現した幾何学モデルが、前記中央リンク部材の中央部に対する基端側部分と先端側部分とが対称を成す形状であり、前記３組以上のリンク機構のそれぞれに、前記基端側の端部リンク部材であるアームを回転させることにより、前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を任意に変更させるアクチュエータを設けたリンク作動装置において、
   前記基端側のリンクハブに対する前記先端側のリンクハブの姿勢を、現在姿勢から指令された目標姿勢へ変更させるように、前記各アクチュエータを制御するリンク作動装置の制御装置であって、
   前記各アクチュエータを、前記現在姿勢であるときの前記アームの回転角から前記目標姿勢となるときの前記アームの回転角までの指令アーム回転角度だけ、アーム回転制御の制御パラメータである加速時間、減速時間、および指令速度に従って、互いに同期して駆動する姿勢変更制御手段を備え、
   前記制御パラメータのうち、加速時間と減速時間の合計を前記リンク作動装置の共振周波数の１周期に設定し、
   前記アクチュエータの加速時間、減速時間、指令速度、および指令アーム回転角度から動作時間を推定する動作時間推定手段と、
   この推定された推定動作時間と加速時間および減速時間の合計とを比較する条件選定手段と、
   この条件選定手段による比較の結果に応じて定められた規則に従い前記制御パラメータを変更するパラメータ変更手段とを設け、
   前記パラメータ変更手段は、前記条件選定手段による比較の結果、前記推定動作時間が前記加速時間および減速時間の合計以下であって、かつ前記推定動作時間がある所定の時間範囲内にある場合に、前記定められた規則に従う変更として、前記制御パラメータにおける前記指令速度を、変更後の指令速度で前記アクチュエータを動作させた場合の前記推定動作時間が前記共振周波数の１周期付近となるように遅く変更し、
   前記所定の時間は、前記共振周波数の１周期とは時間が異なり、かつその所定時間範囲内の時間で加速時間と減速時間を設定すると位置決め完了後のリンク先端の整定時間悪化に影響を及ぼすことになる時間とする
   ことを特徴とするリンク作動装置の制御装置。
2. 請求項１に記載のリンク作動装置の制御装置において、前記パラメータ変更手段は、初期設定された前記加速時間、減速時間、指令速度、および指令アーム回転角度から前記動作時間推定手段が推定した前記推定動作時間が加速時間と減速時間との合計以上となる場合に、制御パラメータのうちの指令速度を、初期設定した指令速度よりも速い速度に変更する機能を有するリンク作動装置の制御装置。
3. 請求項２に記載のリンク作動装置の制御装置において、前記パラメータ変更手段は、前記指令速度を速い速度に変更する場合に、前記推定動作時間が加速時間と減速時間の合計と等しくなるように、前記指令速度を速く変更するリンク作動装置の制御装置。
4. 請求項１に記載のリンク作動装置の制御装置において、前記パラメータ変更手段は、前記制御パラメータの変更に加えて、前記姿勢変更制御手段による直線加減速制御をＳ字加減速制御に変更するリンク作動装置の制御装置。
5. 請求項１に記載のリンク作動装置の制御装置において、前記パラメータ変更手段は、前記制御パラメータの変更に加えて、前記姿勢変更制御手段による直線加減速制御を指数関数型の加減速制御にするリンク作動装置の制御装置。
6. 請求項１に記載のリンク作動装置の制御装置において、前記パラメータ変更手段は、各ポイント位置における指令速度を変更するリンク作動装置の制御装置。

Images