JP3558179B2 - サーボ制御装置及びサーボ制御方法並びにこれらを用いたロボット制御装置及びロボット制御方法 - Google Patents

Description

【０００１】
【産業上の利用分野】
本発明は、複数の駆動軸が力学的に干渉作用を及ぼしながら運動する制御対象をあたかも各駆動軸が力学的に干渉しない系であるかのように制御することよって、制御の高速化や安定化を図ることを目的とした新規なサーボ制御装置及びサーボ制御方法、そしてロボット制御装置及び制御方法を提供しようとするものである。
【０００２】
【従来の技術】
一般に、２以上の自由度を有する機構は、複数の駆動要素（摺動や回動、その他、部材間の相対運動を可能とする構成要素）の組み合わせによって構成されており、例えば、駆動軸のうちの２軸が互いに独立又は略独立した力学系（以下、「力学的非干渉系」という。）とされている場合には、各軸についての制御を分離して取り扱うをことができるため、制御が簡単である。
【０００３】
しかしながら、駆動軸同士が相互に干渉し合うように構成されている力学系（以下、「力学的干渉系」という。）では、ある駆動軸に対する制御に他の駆動軸に係る運動制御が関与するため、系の運動状態は一般に複雑な様相を呈することとなる。
【０００４】
例えば、多関節型ロボットの場合には、ロボット本体が複数のアームやアームの手先位置に取着されるツール等を有し、アーム間あるいはアームとツール間で力学的な干渉作用を及ぼし合いながら運動することになる。
【０００５】
図１２は水平関節型ロボットの構成を簡略的に示すものである。
【０００６】
ロボットａは、基軸部ｂと、第１アームｃ、第２アームｄとを備え、第１アームｃの一端部が回動し得る状態で基軸部ｂに取り付けらている。そして、第１アームｃの他端部には第２アームｄの一端部が回動し得る状態で取り付けられており、第２アームｄの他端部にはツール搭載軸ｅが設けられている。
【０００７】
基軸部ｂには第１アームｃを回動させるためにモータｆとハーモニック減速機ｇとが設けられており、モータｆの駆動力がハーモニック減速機ｇを介して第１アームｃの回動力として伝達される。
【０００８】
第１アームｃには、第２アームｄを回動させるための機構が設けられている。例えば、図示するように第１アームｃに固定された支持部ｈにモータｉ及びハーモニック減速機ｊが取り付けられており、ハーモニック減速機ｊの出力軸にプーリｋが取り付けられている。プーリｌはプーリｋと対をなすものであり、第２アームｄの回動軸に固定されている。そして、プーリｋとプーリｌとの間にベルトｍが掛け渡されている。モータｉの駆動力はハーモニック減速機ｊを介してプーリｋの回動力となり、これがベルトｍによってプーリｌに伝達され、第２アームｄの回動力となる。
【０００９】
図１３はロボットａを平面で見たときの構成を略線的に表現したものであり、第１アームｃの回動中心を原点Ｏとする２次元の位置座標系（Ｘ，Ｙ）を基軸部ｂに設定している。
【００１０】
図において点Ｃは第２アームｄの回動中心を示し、点Ｅは手先位置を示しており、線分ＯＣ（その長さを「Ｌ１」とする。）によって第１アームｃを表し、線分ＣＥ（その長さを「Ｌ２」とする。）が第２アームｄを表している。また。角度「θ１」は線分ＯＣがＸ軸に対してなす角度を示し、角度「θ２」は線分ＣＥが線分ＯＣの延長線に対してなす角度を示している。
【００１１】
点Ｐ１は第１アームｃの重心位置、点Ｐ２は第２アームｄの重心位置をそれぞれ示しており、線分ＯＰ１の長さが「ｌ１」とされ、線分ＣＰ２の長さが「ｌ２」とされている。
【００１２】
このように簡略化された力学系モデルについての運動方程式を導出するにあたっては、Ｘ軸を実数軸とし、Ｙ軸を虚数軸とする複素平面座標系を用いるようにすると、位置や速度等を、複素表示で表現することができる。
【００１３】
例えば、各アームの重心位置やその１階の時間微分は複素量に拡張されて下式のように表わされる。
【００１４】
【数１】

【００１５】
尚、ここで「ｊ」は虚数単位である。
【００１６】
解析にあたって必要な物理量の記号を表形式にまとめたものが、下表１、２である。
【００１７】
【表１】

【００１８】
【表２】

【００１９】
第１アームｃの回転角がθ１である時のモータｆの回転角はＲ１・θ１であり、第２アームｄの回転角がθ２である時のモータｉの回転角はＲ２・θ２であるので、ロボット系（ロボット本体の他、負荷等を含む系）の運動エネルギーを「Ｅｋ」とすると、下式のようになる。
【００２０】
【数２】

【００２１】
速度の２乗値は複素数の絶対値として求められるので、Ｅｋは下式のようになる。
【００２２】
【数３】

【００２３】
重力に係るポテンシャルエネルギーを無視した場合に、ラグランジアンがＥｋに等しいことに注意して、オイラー−ラグランジュ方程式に従って各アームに係る運動方程式（［数４］式）に［数３］式を適用すると、ハーモニック減速機の出力トルクが［数５］式のように求められる。
【００２４】
【数４】

【００２５】
【数５】

【００２６】
尚、上式において摩擦等の粘性項の影響は無視している。
【００２７】
［数５］式において、トルクＴ１に係る第１式の右辺第１項は慣性項を示し、第２項は第２アームｄから受ける慣性力（トルク）を示しており、また、第３項はコリオリ力（トルク）を示し、第４項は第２アームｄの回転による遠心力が第１アームｃに及ぼすトルクを示している。
【００２８】
また、トルクＴ２に係る第２式の右辺第１項は慣性項を示し、第２項は第１アームｃから受ける慣性力（トルク）を示しており、また、第３項は第１アームｃの回転による遠心力が第２アームｄに及ぼすトルクを示している。
【００２９】
各アームの運動についてはアーム自身の慣性項の他に干渉項が存在し、該干渉項の絶対値は慣性項と略同程度の大きさをもっているため無視することができない。
【００３０】
【発明が解決しようとする課題】
上述のように力学的干渉系に係る制御は、駆動要素間の干渉項の存在を充分考慮して制御を行う必要があるため、運動の高速化や安定化を図ることが難しいという問題がある。
【００３１】
例えば、上記のようなロボットでは、第１アームｃと第２アームｄとが互いに力学的な干渉作用を及ぼし合いながら運動しているため、運動状態によっては、ロボットの手先位置について位置決めを行う際に静定が悪化したり、タクトタイム（作業時間）が長くなるといった不都合が生じることになる。
【００３２】
これは、例えば、以下の（１）乃至（３）に示す事項が要因となっているからである。
（１）第１アームｃの運動に関して慣性項（［数５］式の第１式右辺第１項）が第２アームｄの姿勢に影響される（慣性項における２・ｂ・ｃｏｓθ２の項に注意）。
（２）第１アームｃは、第２アームｄの運動によって生じる慣性力や、コリオリ力、遠心力（［数５］式の第１式右辺第２項乃至第４項）の影響を受ける。
（３）第２アームｄは、第１アームｃの運動によって生じる慣性力や遠心力（［数５］式の第２式右辺第２項、第３項）の影響を受ける。
【００３３】
このように第１アームと第２アームとの間の複雑な干渉作用のため、位置や速度のフィードバック制御における最適ゲインがロボットの姿勢によって変化してしまい制御が難しくなってしまう。
【００３４】
尚、このような干渉作用が生じないようにロボットの構造自体を力学的非干渉系又はこれに近い系に改変する方法も考えられるが、そのためにはロボットの設計に比較的強い制約が課せられてしまうという危惧が生じるので、ロボットの構造の如何によって制御の仕方が著しく変化しない汎用性の高い制御装置や制御方法が求められている。
【００３５】
【課題を解決するための手段】
そこで、本発明は、サーボ系に係る運動方程式において、ある駆動軸が他の駆動軸から受ける力学的干渉項（コリオリ力や遠心力等）を打ち消すための補正項の値を、加減速パターン生成部により生成される加減速パターンに係る制御変数を力学的干渉項に代入することで計算し、計算結果と加減速パターンに係る制御指令とをサーボ系に送出して、サーボ系において力学的干渉項に起因する外乱を除去することができるようにしたものである。
【００３６】
【作用】
本発明によれば、補正項を負の要因としてサーボ系に送出することによって力学的干渉項が打ち消され又は干渉作用の影響が弱められるので、各駆動要素について略独立した運動系であるかのように取り扱うことができる。
【００３７】
【実施例】
以下に、本発明の詳細を図示した実施例に従って説明する。尚、図示した実施例は本発明をロボット制御装置に適用したものである。
【００３８】
図１はロボット制御装置１のハードウェア構成の要部を概略的に示すものである。
【００３９】
ロボット制御装置１は、ＣＰＵ（中央演算装置）２を核とし、そのバス３に直接又は間接的に接続されるプログラム記憶部４を有している。
【００４０】
プログラム記憶部４には、ロボットの動作手順の記述に係るロボットプログラムや、ロボットシステムの記述（例えば、ロボットの各軸の動作を決定する座標変換等の記述）に係るシステムプログラムが記憶されるようになっている。
【００４１】
サーボユニット５、５、・・・は、制御対象たるロボット６の駆動軸毎に独立しており、ＣＰＵ２から発せられた命令に従ってロボット６の軸駆動を行なうアクチュエータ（モータやシリンダー等）を制御するために設けられている。
【００４２】
ロボット６の動作指示に係るティーチングペンダント７は、Ｔ．Ｐ．（ティーチングペンダントの略）インターフェース８を介してバス３に接続されている。図２は、制御対象となるロボット６の構成例として、４軸構成の水平多関節型ロボットを示すものである。
【００４３】
即ち、ロボット６は、基軸部９と、第１アーム１０、第２アーム１１とを備え、第１アーム１０の一端部が回動し得る状態で基軸部９に取り付けらている。そして、第１アーム１０の他端部には第２アーム１１の一端部が回動し得る状態で取り付けられており、第２アーム１１の他端部にはツール搭載軸１２が設けられている。
【００４４】
基軸部９には第１アーム１０を回動させるためにモータ１３が設けられており、該モータ１３の駆動力がハーモニック減速機１４を介して第１アーム１０の回動力として伝達される。
【００４５】
第１アーム１０には、第２アーム１１を回動させるための機構が設けられている。即ち、第１アーム１０の一端部に固定された支持部１５にモータ１６及びハーモニック減速機１７が取り付けられており、ハーモニック減速機１７の出力軸にプーリ１８が取り付けられている。このプーリ１８と対をなすプーリ１９が第２アーム１１の回動軸１１ａに固定されており、プーリ１８とプーリ１９との間にベルト２０が掛け渡されている。つまり、モータ１６の駆動力はハーモニック減速機１７を介してプーリ１８の回動力となり、これがベルト２０によってプーリ１９に伝達されて第２アーム１１の回動力となる。
【００４６】
ツール搭載軸１２は、第２アーム１１の回動端部の上面に設けられたアクチュエータ２１によって鉛直方向に摺動されるとともに、第２アーム１１の回動軸１１ａ寄りの端部に設けられたモータ２２の回転力がベルト等によってツール搭載軸１２に伝達されることによって軸回りに回動されるようになっている。そして、ツール搭載軸１２の下端部には、ツール搭載部２３が固定されており、ハンドや工具等の効果器を取り付けることができるようになっている。
【００４７】
図３はロボット６を平面で見たときの構成を略線的に表現したものであり、第１アーム１０の回動中心Ｏ_１を原点とする２次元の位置座標系（Ｘ，Ｙ）を基軸部９に設定している。
【００４８】
図において、点Ｏ_２は第２アーム１１の回動中心、点Ｏ３はツール搭載軸１２の回動中心、点Ｅはツール搭載部２３の回動端点をそれぞれ示している。そして、線分Ｏ_１Ｏ_２（その長さを「Ｌ_１」とする。）が第１アーム１０を表し、線分Ｏ_２Ｏ_３（その長さを「Ｌ_２」とする。）が第２アーム１１を表し、線分Ｏ_３Ｅ（その長さを「Ｌ_４」とする。）がツール搭載部２３を表している。尚、ツール搭載軸１２の長さを「Ｌ_３」とする。
【００４９】
また、「θ_ｉ（ｉ＝１、２、３、４）」は、各駆動軸に係る関節変位量を表しており、「θ_ｉ（ｉ＝１、２、４）」は角度変位、「θ_３」が鉛直方向の摺動変位を表している。つまり、角度「θ_１」は線分Ｏ_１Ｏ_２がＸ軸に対してなす角度、角度「θ_２」は線分Ｏ_２Ｏ_３が線分Ｏ_１Ｏ_２の延長線に対してなす角度、角度「θ_４」は線分Ｏ_３Ｅが線分Ｏ_２Ｏ_３の延長線に対してなす角度をそれぞれ示しており、「θ_３」はツール搭載軸１２の鉛直方向における変位を示している。
【００５０】
点Ｐ_１は第１アーム１０の重心位置、点Ｐ_２は第２アーム１１の重心位置、点Ｐ３はツール搭載軸１２の重心位置、点Ｐ４はツール搭載部２３の重心位置をそれぞれ示しており、線分Ｏ_１Ｐ_１の長さが「ｌ_１」、線分Ｏ_２Ｐ_２の長さが「ｌ_２」、線分Ｏ_３Ｐ_４の長さが「ｌ_４」とされている。
【００５１】
しかして、図２及び図３に示すように、ロボット６は、点Ｏ_１の回りの回動軸（以下、「θ_１軸」という。）、点Ｏ_２の回りの回動軸（以下、「θ_２軸」という。）、点Ｏ３を通って鉛直方向に延びる摺動軸（以下、「Ｒｚ軸」という。）、点Ｏ３の回りの回動軸（以下、「Ｒθ軸」という。）に係る４つの駆動軸が関節要素として組み合わされることで構成されている。
【００５２】
尚、以下では、ある定義量「Ｘ」に添え字「ｉ」を付すことによって各駆動軸に関する量を示すものとし、ｉ＝１がθ_１軸、ｉ＝２がθ_２軸、ｉ＝３がＲｚ軸、ｉ＝４がＲθ軸に係る量をそれぞれ表すものとする。つまり、各駆動軸に係る減速比を「Ｇ_ｉ」とし、アーム等の駆動中心に関する慣性モーメントを「Ｊ_ａｉ」、駆動モータのロータの慣性モーメントを「Ｊ_ｍｉ」とし、駆動モータの位相角を「θ_ｍｉ」とすると、例えば、Ｇ_１がハーモニック減速機１４の減速比、Ｊ_ａ１が第１アーム１０の慣性モーメント、Ｊ_ｍ１がモータ１３のロータの慣性モーメント、θ_ｍ１がモータ１３の位相角をそれぞれ示し、また、Ｇ_３がアクチュエータ２１に係る減速比、Ｊ_ａ３がツール搭載軸１２の慣性モーメント、Ｊ_ｍ３がアクチュエータ２１の駆動源であるモータのロータの慣性モーメント、θ_ｍ３がモータの位相角をそれぞれ示すことになる。
【００５３】
以上の定義量を表形式にまとめると下表３のようになる。
【００５４】
【表３】

【００５５】
このように簡略化された力学系モデルについての運動方程式を導出するにあたっては、Ｘ軸を実数軸とし、Ｙ軸を虚数軸とする複素平面座標系と、両軸に直交するＺ軸とからなる座標系を設定すると、アーム等の重心位置やその１階の時間微分等を複素量に拡張して下式のように表わすことができる。即ち、点Ｐ_ｉの位置を示す複素量「Ｐ_ｉ」は、［数６］式のように表され、また、それらについての時間による１次微分が、［数７］式のように表される。
【００５６】
【数６】

【００５７】
【数７】

【００５８】
尚、ここで「ｊ」は虚数単位である。
【００５９】
ロボット６の基軸部９が設置される地面に固定された基準座標系に対するロボット系の運動エネルギー（正確にはラグランジアン）を「Ｌｇ」とし、各駆動軸に係るラグランジアンを「Ｌｇ_ｉ」（ｉ＝１、２、３、４）とすると、これらは下式に示すようになる。
【００６０】
【数８】

【００６１】
【数９】

【００６２】
【数１０】

【００６３】
【数１１】

【００６４】
即ち、θ_１軸に係るラグランジアンＬｇ_１は、［数８］式の右辺に示すように、並進エネルギー（第１項）と第１アーム１０の回転エネルギー（第２項）と駆動モータ１３のロータの回転エネルギー（第３項）の和であり、また、θ_２軸に係るラグランジアンＬｇ_２も同様に並進エネルギーと、第２アーム１１や駆動モータ１６のロータの回転エネルギーの和として、［数９］式の右辺に示すように表される。尚、Ｊ_ｍ２の公転分についての並進エネルギーや回転エネルギーはラグランジアンＬｇ_１に含まれるものとする。
【００６５】
Ｒｚ軸に係るラグランジアンＬｇ_３は、［数１０］式の右辺に示すように、駆動モータの回転エネルギー（第１項）と、ツール搭載軸１２の鉛直方向における運動エネルギー（第２項）と、ツール搭載軸１２にかかる重力のポテンシャルエネルギー（第３項）との和である。尚、アクチュエータ２１を構成するモータや、ボールネジ等の機構部分、負荷等の公転分についてのエネルギーはラグランジアンＬｇ_２に含まれる。
【００６６】
Ｒθ軸に係るラグランジアンＬｇ_４は、［数１１］式に示すように、負荷の並進エネルギー（第１項）と公転エネルギー（第２項）と、モータ２２の回転エネルギーの和である。
【００６７】
尚、以上の解析にあたっては、説明を簡単にするために、運動方程式における粘性項や非線形項、熱エネルギーの散逸を無視するとともに、アーム等がサーボ指令値通りに運動するものと仮定している。
【００６８】
しかして、全系のラグランジアンＬｇはＬｇ_１乃至Ｌｇ_４の和となるが、下式［数１２］の関係式を用いると、最終的に下式［数１３］のようになる。
【００６９】
【数１２】

【００７０】
【数１３】

【００７１】
但し、［数１３］式における力学定数「Ａ_ｉ（ｉ＝１、２、３、４）」、「Ｂ_ｉ（ｉ＝１、４）」、「Ｃ_１」、「Ｄ_ｉ（ｉ＝１、２、３、４）」、「Ｅ_ｉ（ｉ＝１、４）」、「Ｆ_ｉ（ｉ＝１、３、４）」、「Ｈ_ｉ（ｉ＝０、１）」は下式に示す通りである。
【００７２】
【数１４】

【００７３】
【数１５】

【００７４】
【数１６】

【００７５】
尚、［数１３］式の導出に係る式変形にあたっては、上記［数６］式乃至［数１１］式や複素数の絶対値の計算等を用いれば良い。また、［数１４］式乃至［数１６］式において定義される１８個の力学定数については、ロボット６の設計図に基づいて計算することができるが、これらの力学定数を精度良く同定するためには、実験によって決定する方が好ましい（例えば、モータ電流や、位相角、角速度、角加速度等を実測すれば良い。）。
【００７６】
さて、以上のようにラグランジアンが求まると、ロボット系の運動を記述することができる。即ち、各駆動軸についてモータの発生するトルクを「τ_ｉ」（ｉ＝１、２、３、４）とすると、下式［数１７］に示すオイラー−ラグランジュ方程式に従って、各駆動軸に係る運動方程式（［数１８］式乃至［数２１］式）を導出することができる。
【００７７】
【数１７】

【００７８】
【数１８】

【００７９】
【数１９】

【００８０】
【数２０】

【００８１】
【数２１】

【００８２】
尚、［数１８］、［数１９］、［数２１］式では「ｃｏｓθ＝Ｃ（θ）」、「ｓｉｎθ＝Ｓ（θ）」とおいて、記述の簡略化を図った。
【００８３】
［数２０］式に示すＲｚ軸を除いて、各駆動軸の間には複雑な力学的干渉作用が働いていることが、［数１８］、［数１９］、［数２１］式から明らかとなる。
【００８４】
図４は、本発明に係るサーボ制御装置２４の機能的な構成を示すものであり、指令部２５からの信号が加減速パターン生成部２６に入力され、加減速パターン生成部２６の出力はサーボ系２７に送出されるとともに、非干渉化計算部２８を介してサーボ系２７に送出される。尚、ここにいう「サーボ系」とは、制御対象である駆動源及び／又は駆動機構の他、サーボ回路等を含む系を意味している。また、サーボ系を除く部分は制御装置においてソフトウェア処理により実現される部分であるが、図では各部の機能をブロック要素で視覚化して表現している。また、「加減速パターン」とは、加減速曲線によって直接に記述される状態量の変化は勿論、該状態量の時間微分や積分値の変化等を含む広義の概念である。
【００８５】
加減速パターン生成部２６は、指令部２５からの信号に基づいて状況に応じた加減速パターンを生成するものであり、例えば、本願出願人が特開平３−１４７１０３号公報にて開示したように、サーボ系２７の応答特性を基本とし、この応答特性を再現する手段に指令値を与えたときの応答出力結果として、加速パターンを生成し、減速時には、加速パターンに対して時間的な対称化操作を施すことによって減速パターンを得ることを制御の基礎としている。
【００８６】
例えば、サーボ系２７にステップ入力信号を与えた場合には、理想的なサーボ系を除いてこれに完全に追従した振る舞いをすることはなく、サーボ系２７の応答の遅れにより、実際の立ち上がり特性では、ステップ入力信号の定常値に達するまでに時間がかかることになる。そこで、運動方程式の可逆性（つまり、運動方程式が時間反転に関して対称性を有するという性質。）を利用して、サーボ系２７が追従可能な加速パターンに対して時間的な対称化を行う（例えば、スタックを用いた操作等による。）ことによって減速パターンが得られるように制御すると、サーボ系の円滑な制御が可能となる。例えば、図５（ｂ）に示すように、速度ωの制御では、速度ωがそのピーク値ωｐを示す時間を「Ｔｐ」とした時、ｔ＝Ｔｐを通りω軸に平行な直線ＬＮに関して加速時のグラフ曲線２９を折り返した曲線によって減速時のグラフ曲線３０が得られるように操作し、または、これと同じ事を位置θの制御について言及すれば、図５（ａ）に示すように、加速時のグラフ曲線３１に対して、点Ｐ（ｔ＝Ｔｐ）に関する点対称操作を施すことによって減速時のグラフ曲線３２が得られるように操作する。尚、最適な加減速パターンを決める上で主要な要素はサーボ系の応答特性であり、この点に関して設計者の能力や経験等が介入する余地は皆無であるため、原理的に明確なアルゴリズムに従ってサーボ系にとっての最適制御パターンを得ることができる。また、図５（ａ）、（ｂ）では、加速パターンに対して、時間ｔ＝一定の線に関する線対称性化の操作や、時間ｔ＝一定の線上の点に関する点対称化の操作によって減速パターンを得るようにしたが、これらはほんの一例に過ぎず、例えば、図５（ｃ）に示すように、加速時のグラフ曲線２９′を、時間軸上の点Ｑ（ｔ＝Ｔｐ）に関して時計回り方向に９０゜だけ回転させることによって、グラフ曲線２９′に対して回転対称性を有する減速パターン３０′を形成する等、各種の対称化の操作が可能である。
【００８７】
そして、例えば、ＰＴＰ（Ｐｏｉｎｔ Ｔｏ Ｐｏｉｎｔ）動作に係る制御については、特開平４−１５７５０８号公報に示すように、速度のピーク時間Ｔｐと移動距離との間の関係を予め指定したり、あるいは、特開平４−３０６７１１号公報に示すように、駆動軸間の力学的な干渉作用について、エネルギー式や駆動源のパワー及びパワー配分に基づいてサーボ系を理論的にモデル化して速度のピーク時間Ｔｐの最適値を算出して、上記のような加減速パターンを生成する方法を本願出願人が提案している。
【００８８】
即ち、図６に示すように、制御装置３３は、指令部２５からの指令がパターン生成部３４やＴｐ値設定／決定部３５に送出され、パターン生成部３４の出力がサーボ系２７に送出されるように構成される。
【００８９】
Ｔｐ値設定／決定部３５は、加減速パターンの形状を指定するための情報等を入力部３６から受け取り、軸駆動量と速度のピーク時間との関数関係やエネルギー演算式に基づいて速度のピーク時間Ｔｐを求めるために設けられており、Ｔｐ値に係る情報をパターン生成部３４に送出する。
【００９０】
パターン生成部３４は指令部２５からの情報やＴｐ値設定／決定部３５からのＴｐ値に係る情報に基づいて加減速パターンを生成するものである。つまり、ここでは、加減速パターンを生成する上で基準となる関数を用意して、これに時間的なスケール変換操作や図５で示したような時間的な対称化の操作が基本となる。尚、「時間的なスケール変換操作」とは、軸駆動量についての保存を前提とした時間軸に関する変倍操作を意味する。図７は、速度ωに関する時間的なスケール変換操作について示すものであり、変換パラメータｋを導入して、グラフ曲線３７（ｔ＝Ｔｐでの速度値＝ωｐ、時間軸方向の幅２・Ｔｐ）に対して、時間軸をｋ倍に伸縮するとともに速度ピーク値をｋ分の１倍にすることでグラフ曲線３８を得る純粋に理論的な操作である。
【００９１】
パターン生成部３４は、このような時間的なスケール変換操作をＴｐ値に応じて施すことにより加速パターンの元を生成して、該加速パターンに対して時間的な対称化操作を施すことによって減速パターンを得ることができるように構成されている。
【００９２】
尚、パターン生成方法としては、例えば、下記に示す２通りの方法を挙げることができる。
（１）関数計算による方法（図８（ａ）参照。）
現実のサーボ系の応答特性が、理論的に明快に解析されているような場合に、サーボ系の制御変数を関数式として表現することができれば、パターン生成の対象となる量を予め数式によって用意することができる。例えば、図８（ａ）に簡単に示すように、パターン生成部３４内に関数演算部３４ａと時間軸操作部３４ｂとを設け、関数演算部３４ａにおいて制御変数に係る基準関数群を予め決定しておけば、時間軸操作部３４ｂによって上記のような時間的スケール変換や対称化操作を基準関数に対して容易に施すことができる。即ち、基準関数の関数値の計算とこれに四則演算を組みあわせた計算レベルで済むことになる。
（２）仮想サーボ系を用いる方法（図８（ｂ）参照。）
ここにいう「仮想サーボ系」とは、現実のサーボ系に対応した構成をソフトウェア処理によって模擬的に構築して制御装置内に用意される系であり、これを計算手段として用いることによって各種の推定量の算出に利用することができる。
例えば、モータの制御回路には、モータの制御に直接に関係する回路部分や制御の安定化のための動的補償に係る部分等がサーボ系内に設けられるが、これらに対応した構成を有する仮想モータ系を、ソフトウェア処理による内部モデルとして制御装置内に用意すれば、仮想モータ系に対して指令を与えた時の応答特性を得て、これをそのまま実際のサーボ系の制御に利用することができる。
【００９３】
また、サーボ系の構成が比較的簡単な場合、即ち、サーボ系の応答特性が解析式によって表現することができ、計算が簡単な場合には、上記（１）のように関数計算による方法が有効であるが、一般には、サーボ系の応答特性は解析式によって容易に表わされるとは限らないので、現実のサーボ系を摸した仮想サーボ系を導入することは、実際的な見地から有用である。
【００９４】
例えば、図８（ｂ）に示すように、仮想サーボ系３４ｃ（図ではシグナルフロー線図で表現している。）を用意するとともに、時間軸操作部３４ｂを設け、該仮想サーボ系３４ｃの各ノードから得られる諸量に対して時間軸操作部３４ｂによる時間的なスケール変換操作や対称化操作を施すことができるようにプログラム処理により系を構成すれば良い。
【００９５】
また、ロボットの種類や構造等が異なる場合でも、ロボットの駆動に係るサーボ系の構成に応じて仮想サーボ系３４ｃの構成を改変すれば容易に制御の変更が可能であるため、汎用性が高い。
【００９６】
尚、これまでの説明はＰＴＰ動作に係る制御について行ったが、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐａｔｈ）動作に関する制御については、本願出願人が、特開平６−１９５２８号公報で示した方法を用いれば、サーボ系の構造に依存することのない汎用的なアルゴリズムに従った制御を実現することができる。
【００９７】
即ち、ＰＴＰ動作にあっては、ロボットの移動経路上の重要な点のみを指定し、その点間の経路が不問とされるのに対し、後者のＣＰ動作ではロボットの移動経路（以下、その指定曲線を「ＣＰ曲線」という。）を指定するとともに、ＣＰ曲線に沿ってロボットの手先位置を如何に正確にトレースさせるかが問題となるため、軸駆動量の概念をＣＰ曲線の曲線長で置き換えるだけでは不充分であり、ＣＰ曲線における作用点の位置制御が重要項目として浮上してくる。
【００９８】
その制御の大要を簡単にまとめると、以下の（ａ）乃至（ｅ）に示すようになる。
【００９９】
（ａ）ＣＰ曲線の計算時間間隔の決定
（ｂ）ＣＰ曲線の曲線長に応じた加減速パターンの生成及び線素列の算出
（ｃ）ＣＰ曲線上に指定される点列と線素列とに基づく関節角列の算出
（ｄ）ＣＰ制御の精度を評価量とした速度ピーク値の最適値の決定
（ｅ）関節角列の算出に係る再計算処理及び計算結果のサーボ系への出力
上記の「線素列」とは、ＣＰ曲線上に設定される多数の点列間の長さを制御始点から制御終点に向かって順次に配列したものであり、線素列や点列から関節角列を求めることは、ＣＰ曲線に沿ってロボットの作用点を運動させようとするＣＰ動作にとって基本的な事項であり、ロボットアームの関節角列は逆運動学問題（手先位置が与えられたときにロボットの関節変位や姿勢を求める問題）の解として求められる。また、（ｃ）の速度ピーク値については、理想的には大きい値程好ましいが、制御精度の如何によって制限を受けるため、その適正値を規定する必要が生じる。
【０１００】
尚、逆運動学問題の解析にあたっては、順運動学問題（ロボットの関節変位や姿勢からロボットの手先位置を求める問題）の解法に係る演算要素（同次変換行列）を利用してこれにロボット構造を反映させ、該演算要素を含むネガティブフィードバックフィルターによる計算処理を通して解を求めるようにしたフィルタリング法を挙げることができ、また、ロボットの作用点を指定されたＣＰ曲線に沿って精度良く移動させるために標本化に際して時間軸補正を行い、加減速パターンの生成や速度制御等に要する計算処理上の負担を減らして処理の高速化を図るとともに、この時間軸補正を介在させることによってＣＰ動作に係る制御のアルゴリズムの基本部分には何等変更を加えることなく単にＣＰ曲線の曲線長を駆動軸の軸駆動量へと置換するだけでＰＴＰ動作の制御を可能とし、ＰＴＰ動作とＣＰ動作との間で制御の統一化を図ることができることは特筆すべき事項である（その詳細については説明を省略する。）。
【０１０１】
非干渉化計算部２８は、加減速パターン生成部２６によって得られる諸量に基づいて各駆動軸に関する力学的な干渉項を打ち消すためのトルク補正計算を行うものであり、力学系の非干渉化にとって本質的な部分である。
【０１０２】
今、ある駆動軸に係る状態量を「θ」とし、その時間によるｎ次（ｎは自然数）の導関数を「θ^（ｎ）」で表すことにする。例えば、サーボ系がモータを含むものとして、該モータの位相角を「θ」とすると、「θ^（１）」は角速度、「θ^（２）」は角加速度をそれぞれ示すことになる。
【０１０３】
また、各駆動軸に係る状態量を、「θ」に下付の添え字「ｉ」（ｉ＝１、２、・・・）を付すことによって表して、これらの列（θ_１、θ_２、・・・）をベクトル「Θ」とし、その時間によるｎ次（ｎは自然数）の導関数θ^（ｎ）に下付の添え字「ｉ」（ｉ＝１、２、・・・）を付したもので構成される列（θ_１ ^（ｎ）、θ_２ ^（ｎ）、・・・）をベクトル「Θ^（ｎ）」で表すことにする。
【０１０４】
駆動軸の発生トルクに係るベクトル量をＴ（＝（Ｔ_１、Ｔ_２、・・・））、慣性モーメントに係るベクトル量をΘの関数として「Ｊ（Θ）」とし、また、ある駆動軸を対象とした時これとは異なる駆動軸が当該駆動軸に及ぼす外乱トルクに係るベクトル量を、Θ、Θ^（１）、Θ^（２）のベクトル関数として「ＴＨ（Θ、Θ^（１）、Θ^（２））」（ＴＨ＝（ＴＨ_１、ＴＨ_２、・・・）。但し、ｉ番目の駆動軸に係る量ＴＨ_ｉはＴＨ_ｉ＝ＴＨ_ｉ（Θ、Θ^（１）、Θ^（２））である。）とし、駆動軸間の干渉作用に依らない純粋にランダムな外乱に係るベクトル量を時間ｔの関数として「ｄ（ｔ）」でそれぞれ表すことにすると、ロボット系の運動方程式は、下式に示すベクトル方程式として一般化することができる。
【０１０５】
【数２２】

【０１０６】
パターン生成部３４により加減速パターンとして得られる、ある駆動軸についての状態量を「θ_ｄ（ｔ）」とし、その時間によるｎ次（ｎは自然数）の導関数を「θ_ｄ ^（ｎ）」で表すことにし、各駆動軸に係る状態量を、「θ_ｄ」に下付の添え字「ｉ」（ｉ＝１、２、・・・）を付すことによって表して、これらの列（θ_ｄ１、θ_ｄ２、・・・）をベクトル「Θ_ｄ」とし、その時間によるｎ次（ｎは自然数）の導関数θ_ｄ ^（ｎ）に下付の添え字「ｉ」（ｉ＝１、２、・・・）を付してたもので構成される列（θ_ｄ１ ^（ｎ）、θ_ｄ２ ^（ｎ）、・・・）をベクトル「Θ_ｄ ^（ｎ）」で表すことにする。
【０１０７】
そして、加減速パターンに沿って各駆動軸を運動させた場合に、ある駆動軸を対象としてこれとは異なる駆動軸が当該駆動軸に及ぼす外乱トルクに係るベクトル量を「ＴＨ_ｄ」と定義すると、これは、Θ_ｄ、Θ_ｄ ^（１）、Θ_ｄ ^（２）のベクトル関数として「ＴＨ_ｄ（Θ、Θ^（１）、Θ^（２））」（ＴＨ_ｄ＝（ＴＨ_ｄ１、ＴＨ_ｄ２、・・・）。但し、ｉ番目の駆動軸に係る量ＴＨ_ｄｉはＴＨ_ｄｉ＝ＴＨ_ｉ（Θ_ｄ、Θ_ｄ ^（１）、Θ_ｄ ^（２））である。）と表すことができる。このベクトル量ＴＨ_ｄは加減速パターンの生成にあたってΘ_ｄ、Θ_ｄ ^（１）、Θ_ｄ ^（２）が知られていることから明らかなように事前に計算可能な量であり、具体的には、［数１８］、［数１９］、［数２１］式における「τ_ＩＦｉ」（ｉ＝１、２、４）の「θ_ｍｉ」、「θ_ｍｉ ^（ｎ）」に「θ_ｄｉ」、「θ_ｄｉ ^（ｎ）」をそれぞれ代入することによって下式のように求めることができる。
【０１０８】
【数２３】

【０１０９】
【数２４】

【０１１０】
【数２５】

【０１１１】
但し、加減速曲線については、そのθ_ｄｉやθ_ｄｉ ^（ｎ）がサーボ系にとって実現可能なものである必要があり、三角波のようにその頂点で加速度が不連続となるものは避ける必要がある。
【０１１２】
ところで、［数２２］式の右辺にベクトルＴＨ_ｄに負符号を掛けたものを加えると下式が得られる。
【０１１３】
【数２６】

【０１１４】
上式の右辺第２項及び第３項の「ＴＨ−ＴＨ_ｄ」は、ロボット系のモデル化や力学的定数の同定が完全であって、サーボ系が加減速パターンに追従して制御される場合には理論的にゼロとなるので、結局、［数２６］式は下式のようになる
。
【数２７】

【０１１５】
上式は、各駆動軸がランダムな外乱の影響を受けながらも、他の駆動軸からの干渉から完全に解放されている状態を示している。即ち、他の駆動軸からの干渉項ＴＨがＴＨ_ｄによって打ち消されることで、各駆動軸に係る運動方程式が互いに独立した系を記述することになり、これによって多関節型ロボットをあたかも直交型ロボットのように力学的非干渉系として取り扱うことの可能性が開けることになる。勿論、現実のロボット系では各種の誤差要因によって「ＴＨ−ＴＨ_ｄ」が完全にゼロになるとは限らないが、サーボ系が上記のような加減速パターンにほぼ追従して制御されている場合には、「ＴＨ−ＴＨ_ｄ」という外乱項が小さな値となる。
【０１１６】
非干渉化計算部２８は、加減速パターンに基づいて補正項ＴＨ_ｄを計算して、これによって干渉項ＴＨを打ち消すための出力をサーボ系２７に送出するように構成されている。例えば、パターン生成方法として、上述した仮想サーボ系を用いる方法を採用した場合には、図９に示すように、仮想サーボ系３４ｃのノードから直接又は間接的にΘ_ｄ、Θ_ｄ ^（１）、Θ_ｄ ^（２）を得て、補正項ＴＨ_ｄの値を［数２３］乃至［数２５］式に基づく演算により求めて、これに負の符号をつけたものを、非干渉化計算部２８が現実のサーボ系２７におけるトルクノードＴにそのまま送出するように構成すれば良い。また、関数計算による方法では、Θ_ｄ、Θ_ｄ ^（１）、Θ_ｄ ^（２）や干渉項ＴＨに係る関数形は既知であるため、単に関数値を求めれば良い。
【０１１７】
例えば、ＰＴＰ動作に係る制御において、指令信号としてランプ信号を用いた場合には、Θ_ｄ、Θ_ｄ ^（１）、Θ_ｄ ^（２）が下式のように表されるので、これらを補正項ＴＨ_ｄに代入すれば、ＴＨ_ｄ＝ＴＨ（Θ_ｄ、Θ_ｄ ^（１）、Θ_ｄ ^（２））からＴＨ_ｄの値を計算することができる。
【０１１８】
【数２８】

【０１１９】
尚、上式において、「ΔΘ」は軸駆動量、「Ｔ_ｆ」はランプ信号の折れ時間、「Ｔ_１」、「Ｔ_２」、「Ｔ_３」は仮想モータ系として３次のフィルタ構造を有する場合の制御ループにおける極の逆数をそれぞれ示している。また、関数ｕ（ｔ）は、Θ_ｄ等の表現の便宜上導入される関数であり、Θ_ｄの時間微分によってΘ_ｄ ^（１）やΘ_ｄ ^（２）が直接に得られる訳ではないことに注意を要する。
【０１２０】
本発明に係る制御の手順をまとめると、以下の（Ｓ１）乃至（Ｓ５）及び図１０に示すようになる。
【０１２１】
（Ｓ１）制御対象についての力学的なモデル化
即ち、図３及び［数６］乃至［数１６］式を用いて説明したように、制御対象であるロボット系についての力学的なモデル化に基づいてエネルギー解析を行う。
【０１２２】
（Ｓ２）運動方程式における力学的干渉項の算定
［数１７］式乃至［数２１］式に示すように、各駆動軸についての運動方程式に基づいて、駆動軸が他の駆動軸から受ける力学的干渉項を導出する。
【０１２３】
（Ｓ３）指令値に応じた加減速パターンの生成
指令部２５の発する指令値に応じて、加速度の連続性が保証されかつ加速パターンと減速パターンとが時間軸について対称性を有する加減速パターン（図５参照。）を生成する。尚、ここでいう「時間軸についての対称性」には、前述したように、時間ｔ＝一定の線に関する線対称性、時間ｔ＝一定の線上の点に関する点対称性、時間軸上の点に関する回転対称性等が含まれる。
【０１２４】
（Ｓ４）非干渉化のための補正項ＴＨ_ｄの計算
加減速パターンに沿う制御変数を（Ｓ２）の力学的干渉項に代入して、補正項ＴＨ_ｄの値を［数２３］式乃至［数２５］式に示すように算出する。
【０１２５】
（Ｓ５）サーボ系への制御出力の送出
加減速パターンに沿う制御出力と、補正項ＴＨ_ｄ又は補正項ＴＨ_ｄによる力学的非干渉化作用と同等の効果を得るための出力をサーボ系２７に送出する。
【０１２６】
尚、本発明に係る非干渉化のためのトルク補正は、図４及び図９から明らかなように、パターン生成部３４からサーボ系２７へと至るフィードフォワード法であるため、制御の安定性の面で有利である。
【０１２７】
図１１は、上記ロボット６に関する制御装置の機能的構成の要部を各駆動軸に対して視覚化して示したものである。
【０１２８】
各加減速パターン生成部２６_ｉ（ｉ＝１、２、３、４）には、指令部２５からの指令信号、例えば、図示するような所定の傾斜をもったランプ信号等がそれぞれ入力され、これに応じた加減速パターンが生成される。そして、加減速パターンに沿う制御変数Θ_ｄｉやΘ_ｄｉ ^（１）、Θ_ｄｉ ^（２）が非干渉化計算部２８に送られ、ここで、補正項ＴＨ_ｄの値が各駆動軸に対して計算される（但し、Ｒｚ軸については、［数２０］式から明らかなように、干渉項がないので計算から除外される。）。 非干渉化計算部２８_ｉ（ｉ＝１、２、４）は、サーボ系２７に対して加減速パターンに沿う制御変数Θ_ｄｉやΘ_ｄｉ ^（ｎ）、補正項ＴＨ_ｄに係るトルク計算値（［数２３］式乃至［数２５］式参照。）を、サーボ系２７においてこれらに対応するノードに制御指令としてそれぞれ送出する。
【０１２９】
以上に説明したように、本発明によれば機構的には力学的干渉系である制御対象をあたかも力学的非干渉系であるかのように、つまり、ロボット６について言えば、多関節型ロボットを直交型ロボットであるかのように擬制することが可能となる。
【０１３０】
尚、上記実施例では、本発明をロボット系の制御に適用したが、ロボット系を構成する各駆動軸に対して本発明を適用することができることから明らかなように、駆動軸が互いに力学的に干渉し合っている系であれば、その構造が如何なるものであっても本発明を適用することが可能である。
【０１３１】
【発明の効果】
以上に記載したところから明らかなように、請求項１、請求項２に係る発明によれば、ある駆動要素が他の駆動要素から受ける力学的干渉項を打ち消すための補正計算を行い、これによってサーボ系における力学的干渉項による外乱を除去して、構造上は力学的干渉系である制御対象をあたかも力学的非干渉系であるかのように制御することができるので、サーボ系に対する制御性の向上によって、運動の高速化や安定化を図ることができる。
【０１３２】
また、請求項３、請求項４に係る発明によれば、請求項１や請求項２に係るサーボ制御をロボット系の制御に適用することによって、アームとアームとの間やアームとツール搭載部との間の力学的干渉作用がアーム又はツール搭載部に与える影響を、ロボットの構造の改変に拠ることなくその制御において除去することで、例えば、多関節型ロボットをあたかも直交型ロボットであるかのように制御することができ、ロボットの特定の構造に依存しない汎用性の高い制御を行うことができる。
【図面の簡単な説明】
【図１】図２乃至図１１とともに本発明について説明するための図であり、本図はロボット制御装置の構成例を示す図である。
【図２】制御対象であるロボットの構成例を示す概略図である。
【図３】平面で見た場合の図２のロボットを略線的に示す図である。
【図４】本発明に係る制御装置の機能的な構成を示すブロック図である。
【図５】加減速パターンの生成法について説明するためのグラフ図であり、（ａ）は駆動軸の移動量ΔΘについての時間的な対称化操作を示し、（ｂ）は速度ωについての時間的な対称化操作を示し、（ｃ）は時間軸上の点に関する回転対称性についてそれぞれ示す。
【図６】加減速パターン部の構成例を概略的に示すブロック図である。
【図７】加減速パターンの生成にあたって基準関数に対する時間的なスケール変換操作を施した様子を示すグラフ図である。
【図８】加減速パターンの生成法に係るパターン生成部について説明するための概念図であり、（ａ）は関数計算を用いる方法、（ｂ）は仮想サーボ系を用いる方法についてそれぞれ示す。
【図９】非干渉化計算部における補正項の計算について説明するための概略図である。
【図１０】制御手順について説明するための図である。
【図１１】図２のロボットに対する制御装置の機能的な構成を示すブロック図である。
【図１２】図１３とともに従来の問題点について説明するための図であり、本図はロボットの構成例を示す概略図である。
【図１３】平面で見た場合の図１２のロボットを略線的に示す図である。
【符号の説明】
１ ロボット制御装置
６ ロボット
１０、１１ アーム
２３ ツール搭載部
２４ サーボ制御装置
２５ 指令部
２６ 加減速パターン生成部
２７ サーボ系
２８ 非干渉化計算部

Claims (4)

1. 各駆動要素が互いに力学的な干渉作用を及ぼし合いながら運動するように構成された力学系の制御を行うために、駆動要素とその制御手段を含むサーボ系に対して制御信号を送出するサーボ制御装置であって、
   サーボ制御変数について指令を発する指令部と、
   加速度の連続性が保証されかつ加速パターンと減速パターンとが時間軸について対称性を有する加減速パターンを生成してサーボ系に送出する加減速
   パターン生成部と、
   サーボ系に係る運動方程式においてある駆動要素に対して他の駆動要素から受ける力学的干渉項を打ち消すための補正項の値を、加減速パターン生成部からの加減速パターンに係る制御変数を力学的干渉項に代入して計算し、計算結果を駆動要素間の力学的干渉項を打ち消すための出力としてサーボ系に送出する非干渉化計算部とを備えた
   ことを特徴とするサーボ制御装置。
2. 各駆動要素が互いに力学的な干渉作用を及ぼし合いながら運動するように構成された力学系の制御を行うためのサーボ制御方法であって、
   （１）駆動要素とその制御手段を含むサーボ系についての力学的なモデル化を行った後、
   （２）サーボ系に係る運動方程式において、ある駆動要素が他の駆動要素から受ける力学的な干渉項を算定し、
   （３）サーボ制御変数についての指令値に応じて、加速度の連続性が保証されかつ加速パターンと減速パターンとが時間軸について対称性を有する加減速パターンを生成し、
   （４）（３）の加減速パターンに基づいて（２）の力学的干渉項を打ち消して非干渉化するための補正項の値を、加減速パターンに係るサーボ制御変数を（２）の力学的干渉項に代入することで計算し、
   （５）（３）の加減速パターンに沿う制御指令及び（４）の補正項に係る出力を略同時にサーボ系に送出するようにした、
   ことを特徴とするサーボ制御方法。
3. アームとアームとの間又はアームとツール搭載部との間で力学的な干渉作用を及ぼしながら運動するように構成されたロボットの制御を行うために、ロボットのアーム又はツール搭載部の駆動制御に係る駆動源やサーボ回路を含むサーボ系に対して制御信号を送出するロボット制御装置であって、
   ロボットの運動状態に係る制御変数について指令を発する指令部と、
   指令部からの指令に応じて、加速度の連続性が保証されかつ加速パターンと減速パターンとが時間軸について対称性を有する加減速パターンを生成してサーボ系に送出する加減速パターン生成部と、
   ロボットの駆動軸に係る運動方程式においてある駆動軸が他の駆動軸から受ける力学的な干渉項を打ち消すための補正項の値を、加減速パターン生成部からの加減速パターンに係る制御変数を力学的干渉項に代入することで計算し、計算結果を駆動軸間の力学的干渉項を打ち消すための出力としてサーボ系に送出する非干渉化計算部とを備えた、
   ことを特徴とするロボット制御装置。
4. アームとアームとの間又はアームとツール搭載部との間で力学的な干渉作用を及ぼしながら運動するように構成されたロボットの制御を行うためのロボット制御方法であって、
   （１）ロボットのアーム又はツール搭載部の駆動制御に係る駆動源やサーボ回路を含むサーボ系についての力学的なモデル化を行った後、
   （２）ロボットの各駆動軸に係る運動方程式において、あるアーム又はツール搭載部が他のアーム又はツール搭載部から受ける力学的な干渉項を算定し、
   （３）ロボットの運動状態に係る制御変数の指令値に応じて、加速度の連続性が保証されかつ加速パターンと減速パターンとが時間軸について対称性を有する加減速パターンを生成し、
   （４）（３）の加減速パターンに基づいて（２）の力学的干渉項を打ち消してすための補正項の値を、加減速パターンに係る制御変数を（２）の力学的干渉項に代入することで計算し、
   （５）（３）の加減速パターンに沿う制御指令及び（４）の補正項に係る出力を略同時にサーボ系に送出するようにした、
   ことを特徴とするロボット制御方法。

Images