JP3807547B2

JP3807547B2 - パラレルメカニズムの制御装置

Info

Publication number: JP3807547B2
Application number: JP2002155042A
Authority: JP
Inventors: 公一大須賀; 陽一中村
Original assignee: Murata Machinery Ltd
Current assignee: Murata Machinery Ltd
Priority date: 2002-05-29
Filing date: 2002-05-29
Publication date: 2006-08-09
Anticipated expiration: 2022-05-29
Also published as: JP2003345441A

Description

【０００１】
【発明の利用分野】
この発明はパラレルメカニズムの制御装置に関する。
【０００２】
【従来技術】
パラレルメカニズムは、モーションベースなどのエンドエフェクタを固定のベースに複数の並列なリンクで接続した機構であり、関節の一部が受動関節で、他が駆動関節である。パラレルメカニズムを用いると、従来からのシリアルメカニズムでは困難な、高剛性かつ高精度で、現在位置から目標位置へと直線的に高速運動ができる、と期待されている。しかしながらパラレルメカニズムでは受動関節が存在するため、順運動学的な計算は困難で、作業座標変数での制御はほとんど行われていない。これは特に、駆動関節座標変数を作業座標変数の推定値に変換する際に、ヤコビ行列Ｊの逆行列Ｊ^-1 が必要になるからである。周知のようにヤコビ行列の各要素は、定数ではなく、作業座標変数の関数であり、この逆行列の値を作業座標変数の値毎に評価することは、計算上の負担が大きい。このような計算上の負担は、パラレルメカニズムの実用化での障害となる。
【０００３】
【発明の課題】
この発明の基本的課題は、作業座標変数を求めるに際して、ヤコビ行列の逆行列Ｊ^-1 を不要にすることにある。
請求項２の発明での追加の課題は、駆動関節へのフィードバックに際して、ヤコビ行列の転置行列の逆行列Ｊ^-Tを不要にすることにある。
請求項３の発明での追加の課題は、エンドエフェクタの姿勢を一定に保ちながら、高剛性でエンドエフェクタを運動させることができる、パラレルメカニズムを提供することにある。
【０００４】
【発明の構成】
この発明は、エンドエフェクタを複数の駆動関節と複数の受動関節とを用いて運動させるパラレルメカニズムを、逆運動学変換手段により、エンドエフェクタの作業座標変数の推定値を、駆動関節座標変数の推定値に変換し、順運動学変換手段により、推定した駆動関節座標変数と実際の駆動関節座標変数との偏差を解消するように、作業座標変数の推定値を更新し、制御手段により、目標作業座標変数と作業座標変数の推定値が一致するように、駆動関節をフィードバック制御するようにした、パラレルメカニズムの制御装置であって、前記順運動学変換手段では、前記偏差に、作業座標変数の微分を駆動関節座標変数の微分に変換するためのヤコビ行列の転置行列と、ゲインとを乗算し、積分して、作業座標変数の推定値を更新するようにしたことを特徴とする（請求項１）。
【０００５】
好ましくは、前記制御手段では、目標作業座標変数と作業座標変数の推定値との偏差に、前記ヤコビ行列とゲインとを乗算して、駆動関節に対する制御量を求める(請求項２)。
【０００６】
また好ましくは、制御対象のパラレルメカニズム本体が、固定ベースに対してエンドエフェクタの姿勢を一定に保つための機構と、エンドエフェクタの位置を変化させるための直動機構とからなり、該直動機構を駆動関節として制御する(請求項３)。
【０００７】
【発明の作用と効果】
この発明のパラレルメカニズムの制御装置では、順運動学変換で、駆動関節座標変数から作業座標変数の推定値を求める過程で、ヤコビ行列の逆行列を用いる必要が無く、ヤコビ行列の転置行列を用いればよい。ヤコビ行列の転置行列はヤコビ行列が求まれば容易に求まるので、駆動関節座標を作業座標変数に変換する際の、計算上の負担を小さくできる(請求項１)。またこの構成で作業座標変数を推定し得ることを、発明者は実施例に記載の補題やシミュレーションで確認した。
【０００８】
作業座標変数の推定値が得られ、目標作業座標変数との偏差が判明すると、駆動関節にフィードバックする必要がある。駆動関節に加える制御量を決定するには、ヤコビ行列の転置行列の逆行列が必要であるが、逆行列を求めるのは計算上の負担が大きい。そこで請求項２の発明では、偏差にヤコビ行列とゲインとを乗算して制御量を求めるので、ヤコビ行列の転置行列の逆行列を求める必要がない。なおこの構成でパラレルメカニズムを目標軌道に沿って制御し得ることを、発明者はシミュレーションで確認した。
【０００９】
【実施例】
図１〜図１３を参照して、実施例のパラレルメカニズムの制御装置２を説明する。パラレルメカニズムの制御装置２は、大別して、駆動関節座標変数ｑdからエンドエフェクタの作業座標変数の推定値Ｘcを求めるための順運動学処理部４と、求めた作業座標変数Ｘcと目標作業座標変数Ｘrとの偏差に応じて、パラレルメカニズム本体２０へとフィードバック制御を行うための制御系１８とからなっている。
【００１０】
駆動関節座標変数ｑdとその推定値との偏差にヤコビ行列Ｊの逆行列Ｊ^-1を乗算し、適当なゲインＫを乗算した後、積分(積算)を施せば、作業座標変数ＸEの推定値Ｘcを得ることができる。このこと自体は公知である。ここで発明者は、ヤコビ行列の逆行列Ｊ^-1ではなく、ヤコビ行列の転置行列Ｊ^Tを用いても、作業座標変数の推定値Ｘcを得られることを見い出した。
【００１１】
図１において、６は逆運動学処理部で、作業座標変数の推定値Ｘcを駆動関節座標変数の推定値ｑcへ変換して出力する。減算器８では、エンコーダなどから求めた実際の駆動関節座標変数ｑdとの差分を求め、ヤコビ転置行列処理部１０でヤコビ行列の転置行列Ｊ^Tを乗算し、ゲイン処理部１２で定数あるいは行列のゲインＫと乗算し、積分器１４で積算して、作業座標変数の推定値Ｘc(エンドエフェクタの座標変数の推定値)を得る。
【００１２】
順運動学処理部４は、仮想的なパラレルメカニズムに対して、駆動関節座標変数の推定値ｑcが実際の駆動関節座標変数ｑdに一致するように、フィードバック制御を施しているものと見なすことができる。減算器８の出力ｕは駆動関節座標での偏差であり、ヤコビ行列の転置行列を乗算した後の出力Ｆはエンドエフェクタに加わる仮想的な力やモーメントであり、これにゲインを乗算して積分すると、仮想的なエンドエフェクタの位置が定まる。順運動学処理部４では、作業座標変数の推定値Ｘcの初期値に適宜の値を入力し、この値を逆運動学計算により、駆動関節座標変数の推定値に変換し、実際の駆動関節座標変数からの偏差に応じて、ヤコビ行列の転置行列(以下ヤコビ転置行列という)を乗算する。これによって、仮想的なエンドエフェクタに加わる力やモーメントＦを求め、これにゲインを乗算して、仮想的な制御量Ｗに変換し、積分して、作業座標変数の推定値Ｘcの値を更新する。
【００１３】
一般にエンドエフェクタの作業座標変数をＸEとし、駆動関節座標変数をｑとすると、ｑ＝ｆ(ＸE) との関係が成り立つ。これを時間微分すると、(1)式が得られる。
ｑ'＝ＪＸE', Ｊ＝σｆ／σＸE (1)
なおここで、'は微分記号を表し、実装上は差分を表している。ここで行列Ｊはヤコビ行列と呼ばれ、正方行列であるが、各要素は定数ではなく、作業座標変数ＸEの関数である。またこの明細書で座標や座標変数という時、単なる３次元座標をいうのではなく、一般にベクトル量で、例えば作業座標変数の場合はエンドエフェクタの位置と姿勢を特定する変数のセットを意味する。駆動関節座標変数ｑの場合は、複数の駆動関節の状態を指定し得る変数のセットからなるベクトルである。さらに式(1)において、ヤコビ行列Ｊが正則行列ではなくなる点を特異点という。
【００１４】
エンドエフェクタに加わる力やモーメントをＦとすると、各関節に加わる駆動力をｕとして、次式が成り立つ。
Ｆ＝Ｊ^T(ＸE)ｕ (2)
ここで、図１の順運動学処理部４では、作業座標変数の推定値の速度Ｘ'cが制御入力Ｗによって(3)式のように制御できるとものとする。するとＸ'cの積分で、作業座標変数の推定値が得られる。
Ｘ'c＝Ｗ (3)
すると次の補題が成り立つ。
【００１５】
【補題】
制御入力Ｗを式(4)に従って定める。
Ｗ＝−ＫＪ(Ｘc)^T(ｑc−ｑd) (4)
この時、ある正数Ｎ,ｃに対して(5)式が成立すると、
Ｎ＜σn(Ｊ(Ｘc)・Ｊ(Ｘc)^T)、 ‖ｑ'd‖＜ｃ (5)
任意のε0＞０と、Ｋ＞Ｃ／(ε0・Ｎ)となるＫに対して、あるＴ≧０が存在し、(6)式が成立する。
‖ｑc−ｑd‖＜ε0、(for all t≧Ｔ) (6)
ここでσnは最小特異解を表す。(6)式は、仮想的なエンドエフェクタに(4)式に従って制御入力を加えながら、時間Ｔ以上の間、仮想的なエンドエフェクタを運動させると、駆動関節座標の推定値は実際の駆動関節座標に収束することを意味している。ここで順運動学処理部４が実際のパラレルメカニズムと同一の機構を表現しているものと仮定できれば、即ちヤコビ行列Ｊや逆運動学処理部６が実際のパラレルメカニズムを表現していれば、エンドエフェクタの作業座標変数の推定値Ｘcはｑ＝ｆ(ＸE)の１つの解となる。
【００１６】
図１の１６は減算器で、作業座標変数の目標値Ｘrと作業座標変数の推定値Ｘcとの偏差を求めて、制御系１８によりＰＤ制御やＰＤＩ制御などを加えて、制御量（制御入力）を発生させて、パラレルメカニズム本体２０を駆動する。実施例での記号を表１に示す。
【００１７】
【表１】

【００１８】
図１の順運動学処理部による、作業座標変数Ｘの推定の妥当性を検証するため、図２のパラレルメカニズムをモデルとして、シミュレーションを行った。このパレルメカニズムは、図１１に示すパラレルメカニズムをモデル化したもので、固定ベース３０に対して、エンドエフェクタ３２を、その姿勢が一定になるように受動関節を介して取り付ける。ここではエンドエフェクタ３２の姿勢が、常に固定ベース３０に平行になるように、受動関節により規制する。３３〜３５はサーボモータで、駆動関節の例であり、３６〜３８はボールネジで、サーボモータ３３〜３５とエンドエフェクタ３２間のボールネジ３６〜３８の長さを、変数θ1〜θ3とする。またサーボモータ３３〜３５は、固定ベース３０に２以上の揺動自由度のジョイントで支承され、ボールネジ３６〜３８の先端は、エンドエフェクタ３２に２以上の揺動自由度で支承されているものとする。
【００１９】
エンドエフェクタ３２は姿勢が拘束されているので、自由度はＸ,Ｙ,Ｚの３つの座標である。エンドエフェクタの中心位置が図３〜図５の破線のように運動し、３つのボールネジの長さθ1〜θ3が式(7)のように変化するものとする。エンドエフェクタ３２の中心位置の初期位置(時刻０での実線位置)を適当に推定し、例えばここでは(１,１,１)とし、図１の順運動学処理部４により、エンドエフェクタ３２の位置をシミュレーションした。結果を図３〜図５に示し、実線がエンドエフェクタ座標の推定値である。なおこのシミュレーションではゲインＫを50とした。図３〜図５から明らかなように、エンドエフェクタの座標変数の推定値は１秒程度で実際の値に収束し、以下はリアルタイムでエンドエフェクタの位置を求めることができた。
θ1＝ sinπ/2 t + √10
θ2＝ sinπ/2 t + √10, Ｋ＝50 (7)
θ3＝ sinπ/2 t + √10
【００２０】
以上のように、図１の順運動学処理部４を用いると、ヤコビ行列の逆行列Ｊ^-1を用いずに、ヤコビ行列の転置行列Ｊ^Tを用いて、エンドエフェクタの作業座標変数を推定できる。そこでこの値を推定できれば、作業座標変数の目標値Ｘrとの差を解消するように制御系１８でフィードバック制御を行うことにより、目標の位置へあるいは目標の軌跡に沿って、エンドエフェクタを運動させることができる。
【００２１】
図６にパラレルメカニズムの制御装置２での制御系１８を示す。ここでは制御系１８でＰＤ制御を行うので、入力は目標座標変数Ｘrとその微分値Ｘr'並びに作業座標変数の推定値Ｘcとその微分値Ｘc'とである。ＰＤ制御に代えてＰＩＤ制御などでも良く、制御の手法自体は任意である。
【００２２】
運動学からすると、ＸrとＸcとの変位などにゲインＫPなどを乗算し、これにヤコビ転置行列の逆行列Ｊ^-Tを乗算することにより、サーボモータなどへの制御量（制御入力）が得られる。しかしながらＪ^-Tを用いると、ヤコビ行列の逆行列Ｊ^-1を求めるのと同程度の計算量が必要になり、処理上の負担が著しい。そこで発明者は、ゲインＫPやゲインＫDに代えて、Ｊ・ＫP・Ｊ^TやＪ・ＫD・Ｊ^Tを用いれば、 J^-T・Ｊ^Tが恒等変換Ｉとなり、単にＪ・ＫPやＪ・ＫDなどの乗算で処理できることを見出した。ここにゲインＫP，ＫDは行列であるが、定数でも良い。
【００２３】
図６の制御系１８はこのような着想に基づくもので、減算器１６,１７により、目標座標変数Ｘrと作業座標変数の推定値Ｘcとの差分や、目標座標変数の微分値Ｘr'と推定作業座標変数の微分値Ｘc'との差分を求め、比例制御部２２で、Ｊ・ＫPを乗算し、微分制御部２３でＪ・ＫDを乗算し、加算器２４でこれらの制御量を加算して、恒等変換部２５で形式的に恒等変換して、パラレルメカニズム本体２０に制御を加える。なお恒等変換部２５は実装上は不要である。
【００２４】
このような制御を加えても良い根拠として、ゲインＫPやＫDは基本的に正定行列であり、これに両側からヤコビ行列Ｊとその転置行列Ｊ^Tを乗算しても、やはり正定行列で、基本的な性質が変わらないことがある。
【００２５】
Ｋの値を50000、ＫPを200、ＫDを50とし、図２のモデルを用いて、図６の制御系１８での制御結果をシミュレーションした。結果を図７〜図９に示し、破線は目標軌道を表し、実線はシミュレーション結果である。このシミュレーションでは、時刻０において目標軌道上の位置と、作業座標変数の推定値を一致させてある。図７〜図９から明らかなように、Ｙ座標において、目標軌道からの僅かな誤差が生じる他は、目標軌道を追従して制御できている。このように図６の制御系１８を用いても、パラレルメカニズム本体２０を安定に制御できる。
【００２６】
図１０に、パラレルメカニズムの制御装置４０を示す。図１,図６の制御装置２との違いは、微分信号を得るための差分器４２,４３の存在を明示し、順運動学処理部４で作業座標変数の推定値が収束するまで、制御の開始を待つ、あるいはオープンループ制御するためのスイッチ４４,４５を明示した点である。加算器２４の出力は、サーボモータ３３〜３５の駆動用のサーボドライバアンプ５０〜５２に加えられ、出力τ1〜τ3としてサーボモータへ加えられる。モータ３３〜３５の回転に伴うボールネジの位置あるいはモータの実際の回転角度やその積算値などは、駆動関節座標変数ｑdの成分θ1〜θ3として制御装置４０へフィードバックされる。
【００２７】
作業座標変数系での制御を行うことの利点を以下に示す。関節座標系で制御を行うと、それぞれの関節での目標値に対する誤差、即ち制御誤差が累積し、作業座標系でのエンドエフェクタの目標値に対する誤差を補償できない。それに対し、作業座標系で制御を行うと、エンドエフェクタの目標値に対する誤差を直接フィードバックできるため、それぞれの関節での誤差が累積することがなく、エンドエフェクタの運動を高精度に制御できる。
【００２８】
図１１に、実施例の制御装置４０の制御対象としての、パラレルメカニズム本体２０の例を示す。Ｇはグラウンドで、Ｈは例えば３本の支柱であり、固定ベース３０が固定されている。回転軸６０は例えば固定ベース３０の表面に平行に取り付けられ、回転軸６０に接続された第１リンク６１の他端には、２自由度以上の関節６３を介して、揺動自在に第２リンク６２が取り付けてある。第２リンク６２は平行な２本のアームからナリ、第２リンク６２の先端は、２自由度以上の関節６４を介して、揺動自在にエンドエフェクタ３２に取り付けてある。回転軸６０や関節６３,６４は受動関節で、関節６３,６４は２自由度に限らず、それ以上の自由度の関節であればよい。６６はボールネジ３６〜３８の先端をエンドエフェクタ３２に取り付けるためのジョイントで、ボールネジを２自由度以上の揺動運動が自在に取り付ける。
【００２９】
図１２に示すように、回転軸６０が固定ベース３０の表面に平行で、関節６３,６３を結ぶ直線も固定ベース３０の表面に平行になり、その結果、関節６４,６４を結ぶ直線も固定ベース３０の表面に平行になる。エンドエフェクタ３２では、第２リンク６２の先端の関節６４，６４を結ぶ３本の直線がいずれも固定ベース３０に平行なので、絶えず固定ベース３０に平行な姿勢をとるようになる。この結果、エンドエフェクタ３２の表面は絶えず固定ベース３０の表面に平行になる。このため半導体ウエハや液晶基板などの姿勢に敏感なものを搬送したり、正確な姿勢で加工や組立などの作業を行う場合に便利である。なおエンドエフェクタ３２には適宜のハンドなどを取り付けるものとする。
【００３０】
エンドエフェクタ３２の位置は、ボールネジ３６〜３８の伸縮で定まり、これはサーボモータ３３〜３５の回転で定まる。そしてサーボモータ３３〜３５は、図１３に示すように、固定ベース３０の表面に対して２軸方向に揺動自在である。中間部材７４は回転軸７２,７２を介して固定ベース３０に取り付けられ、サーボモータ３３は回転軸７６,７６を介して中間部材７４に取り付けてある。この結果、サーボモータ３３は固定ベース３０に対して、２自由度で揺動自在である。他のサーボモータ３４,３５の取り付けも同様である。
【００３１】
図１１のパラレルメカニズム本体２０では、エンドエフェクタ３２の姿勢が絶えず一定に保たれ、その位置はボールネジ３６〜３８で調整され、直線運動型（直動型）の駆動関節を用いるので、高剛性で高精度に高速で運動できる。従ってエンドエフェクタ３２の運動は、高速かつ高剛性でしかも高精度である。なおボールネジ３６〜３８に代えて、油圧や空圧のシリンダや、リニアモータなどで伸縮するロッドを用いても良い。
【００３２】
実施例では、特定のパラレルメカニズム本体２０の制御を例としたが、ヤコビ行列の逆行列Ｊ^-1や、ヤコビ転置行列の逆行列Ｊ^-Tなどの計算を不要にできるので、より複雑でかつより駆動関節の多いパラレルメカニズムでも容易に制御することができる。
【図面の簡単な説明】
【図１】実施例のパラレルメカニズムの制御装置を、順運動学処理部を中心に示すブロック図
【図２】実施例で用いたパラレルメカニズムを模式的に示す図
【図３】図１の順運動学処理部を用いた作業座標変数Ｘのシミュレーション結果を示す図
【図４】図１の順運動学処理部を用いた作業座標変数Ｙのシミュレーション結果を示す図
【図５】図１の順運動学処理部を用いた作業座標変数Ｚのシミュレーション結果を示す図
【図６】実施例のパラレルメカニズムの制御装置の制御系を示すブロック図
【図７】実施例での、パラレルメカニズムの運動制御のシミュレーション結果を作業座標変数Ｘについて示す図
【図８】実施例での、パラレルメカニズムの運動制御のシミュレーション結果を作業座標変数Ｙについて示す図
【図９】実施例での、パラレルメカニズムの運動制御のシミュレーション結果を作業座標変数Ｚについて示す図
【図１０】実施例のパラレルメカニズムの制御装置の全体構成を示すブロック図
【図１１】実施例で用いたパラレルメカニズムの斜視図
【図１２】図１１のパラレルメカニズムでの、固定ベースに対するエンドエフェクタの姿勢の保持機構を示す斜視図
【図１３】図１１のパラレルメカニズムでの、ボールネジ駆動用サーボモータの姿勢の自由度を示す図
【符号の説明】
２パラレルメカニズムの制御装置
４順運動学処理部
６逆運動学処理部
８減算器
１０ヤコビ転置行列処理部
１２ゲイン処理部
１４積分器
１６減算器
１８制御系
２０パラレルメカニズム本体
２２比例制御部
２３微分制御部
２４加算器
２５恒等変換部
３０固定ベース
３２エンドエフェクタ
３３〜３５サーボモータ
３６〜３８ボールネジ
４０制御装置
４２,４３差分器
４４,４５スイッチ
５０〜５２サーボドライバアンプ
５４〜５６エンコーダ
６０回転軸
６１第１リンク
６２第２リンク
６３２自由度の関節
６４２自由度の関節
７２,７６回転軸
７４中間部材
Ｇグラウンド
Ｈ支柱

Claims

エンドエフェクタを複数の駆動関節と複数の受動関節とを用いて運動させるパラレルメカニズムを、
逆運動学変換手段により、エンドエフェクタの作業座標変数の推定値を、駆動関節座標変数の推定値に変換し、順運動学変換手段により、推定した駆動関節座標変数と実際の駆動関節座標変数との偏差を解消するように、作業座標変数の推定値を更新し、制御手段により、目標作業座標変数と作業座標変数の推定値が一致するように、駆動関節をフィードバック制御するようにした、パラレルメカニズムの制御装置であって、
前記順運動学変換手段では、前記偏差に、作業座標変数の微分を駆動関節座標変数の微分に変換するためのヤコビ行列の転置行列と、ゲインとを乗算し、積分して、作業座標変数の推定値を更新するようにしたことを特徴とする、パラレルメカニズムの制御装置。
前記制御手段では、目標作業座標変数と作業座標変数の推定値との偏差に、前記ヤコビ行列とゲインとを乗算して、駆動関節に対する制御量を求めるようにしたことを特徴とする、請求項１のパラレルメカニズムの制御装置。
制御対象のパラレルメカニズム本体が、固定ベースに対してエンドエフェクタの姿勢を一定に保つための機構と、エンドエフェクタの位置を変化させるための直動機構とからなり、該直動機構を駆動関節として制御するようにしたことを特徴とする、請求項１または２のパラレルメカニズムの制御装置。