以下、実施形態について、図面を参照しつつ詳細に説明する。説明において、同一要素又は同一機能を有する要素には同一の符号を付し、重複する説明を省略する。
〔ロボットシステム〕
ロボットシステム1は、ロボットに予め設定された制御目標に従って動作させるシステムである。図1に例示するように、ロボットシステム1は、制御対象の一例であるロボット10と、コントローラ100とを備える。
(ロボット)
ロボット10は、例えば6軸の垂直多関節ロボットであり、基部11と、旋回部12と、第1アーム13と、第2アーム14と、手首部15と、先端部16と、アクチュエータ41,42,43,44,45,46と、加速度センサ51,52,53,54,55,56とを有する。
基部11は、床面又は台車等に固定される。旋回部12は基部11上に設けられており、鉛直な軸線21まわりに旋回可能である。すなわちロボット10は、軸線21まわりに旋回部12を旋回可能とする関節31を有する。
第1アーム13は、旋回部12から延出しており、軸線21に交差(例えば直交)する軸線22まわりに揺動可能である。すなわちロボット10は、軸線22まわりに第1アーム13を揺動可能とする関節32を有する。なお、ここでの交差は、所謂立体交差のように互いにねじれの関係にある状態での交差を含む。以下においても同様である。
第2アーム14は、第1アーム13の先端部から延出しており、軸線21に交差(例えば直交)する軸線23まわりに揺動可能である。すなわちロボット10は、軸線23まわりに第2アーム14を揺動可能とする関節33を有する。軸線23は軸線22と平行であってもよい。
第2アーム14の先端部は、第2アーム14の延出方向に沿って軸線23に交差(例えば直交)する軸線24まわりに旋回可能である。すなわちロボット10は、軸線24まわりに第2アーム14の先端部を旋回可能とする関節34を有する。
手首部15は、第2アーム14の先端部から延出しており、軸線24に交差(例えば直交)する軸線25まわりに揺動可能である。すなわちロボット10は、軸線25まわりに手首部15を揺動可能とする関節35を有する。
先端部16は、手首部15の先端部に設けられており、手首部15の延出方向に沿って軸線25に交差(例えば直交)する軸線26まわりに旋回可能である。すなわちロボット10は、軸線26まわりに先端部16を旋回可能とする関節36を有する。先端部16には、作業用の様々なツールが設けられる。ツールの具体例としては、ワークを保持するためのハンド、塗料を吐出する塗装ガン、及び溶接トーチ等が挙げられる。
アクチュエータ41,42,43,44,45,46は、関節31,32,33,34,35,36をそれぞれ駆動する。例えばアクチュエータ41は軸線21まわりに旋回部12を旋回させ、アクチュエータ42は軸線22まわりに第1アーム13を揺動させ、アクチュエータ43は軸線23まわりに第2アーム14を揺動させ、アクチュエータ44は軸線24まわりに第2アーム14の先端部を旋回させ、アクチュエータ45は軸線25まわりに手首部15を揺動させ、アクチュエータ46は軸線26まわりに先端部16を旋回させる。
加速度センサ51,52,53,54,55,56のそれぞれは、駆動対象部分に取り付けられている。加速度センサ51,52,53,54,55,56は、アクチュエータ41,42,43,44,45,46による駆動対象部分(以下、単に「駆動対象部分」という。)の加速度をそれぞれ検出する。
なお、加速度センサ51,52,53,54,55,56は、加速度に相関する何らかの物理量を検出するセンサであればよく、ロボット10は、必ずしも加速度自体を検出するセンサを有していなくてよい。例えば加速度センサ51,52,53,54,55,56は、アクチュエータ41,42,43,44,45,46の駆動対象の駆動力をそれぞれ検出する力センサ(例えばトルクセンサ)であってもよい。
また、ロボット10は、アクチュエータ41,42,43,44,45,46ごとに加速度センサを有するのに代えて、アクチュエータ41,42,43,44,45,46のいずれによっても駆動される先端部16に加速度センサを有していてもよい。例えばロボット10は、先端部16に6軸(並進3軸及び回転3軸)の加速度センサを有していてもよい。
図2に示すように、アクチュエータ41,42,43,44,45,46のそれぞれは、モータ60と、速度センサ70と、減速機80とを有する。モータ60は、ロータ61と、電力の供給に応じてロータ61を回転駆動する電機子62とを有する。速度センサ70は、ロータ61の回転速度を検出する。速度センサ70の具体例としては、ロータリーエンコーダ等のパルスジェネレータ、及びタコジェネレータ等が挙げられる。
減速機80は、ロータ61に接続される入力軸81と、駆動対象部分に接続される出力軸82と、入力軸81から出力軸82に駆動力を伝達する伝達要素83とを有する。伝達要素83は、例えば少なくとも一つのギヤにより構成されており、入力軸81の回転速度に所定の減速比を乗算して出力軸82に伝達し、入力軸81の回転トルクに上記減速比の逆数を乗算して出力軸82に伝達する。
ロータ61と駆動対象部分との間には、伝達要素83の変形等に起因する撓みが生じる。そこで、アクチュエータ41,42,43,44,45,46及びその駆動対象部分は、ロータ61を主体とする第1慣性体91と、電力の供給に応じて第1慣性体91を駆動する電機子62と、駆動対象部分を主体とする第2慣性体92と、第1慣性体91から第2慣性体92に力を伝達する伝達要素83とによって近似可能である。
このように近似されたモデルにおいては、図3の伝達関数TF01で示されるように、電機子62からの電流Iqに所定のトルク定数KTを乗算した大きさの駆動力(例えばトルクTM)が第1慣性体91に生じる。これに応じ、第1慣性体91は、伝達関数TF02で示されるように、トルクTMを第1慣性体91のイナーシャJMで除算した大きさの加速度aMで加速される。更に、伝達関数TF03で示されるように、加速度aMが積分されて第1慣性体91が速度ωMで動作する。
第1慣性体91の速度ωMは、伝達関数TF04で示されるように、減速比Rgで除算された大きさの速度ωMtとして第2慣性体92側に伝わる。第1慣性体91から第2慣性体92側に伝わる速度ωMtと、第2慣性体92の速度ωLとの間に差(以下、この差を「差速度ωs」という。)が生じると、伝達関数TF05で示されるように差速度ωsが積分されて撓み角θsが生じる。更に、伝達関数TF06で示されるように、撓み角θsに所定のばね定数Ksを乗算した大きさの駆動力(例えばトルクTs)が第2慣性体92に伝わる。
また、第2慣性体92には、外乱のトルクTdisも伝わる。このため、加え合わせ点AP01で示されるように、第2慣性体92には、トルクTsからトルクTdisを減算したトルクTLが伝わる。
これに応じ、第2慣性体92は、伝達関数TF07で示されるように、トルクTLを第2慣性体92のイナーシャJLで除算した大きさの加速度aLで加速される。更に、伝達関数TF08で示されるように、加速度aLが積分されて第2慣性体92が速度ωLで動作する。
上述した第1慣性体91から第2慣性体92への力、加速度及び速度の伝達成分に加えて、第1慣性体91と第2慣性体92との間には複数のフィードバック成分が生じる。例えば、伝達関数TF09で示されるように、上記トルクTsを減速比Rgで除算した大きさのトルクTstが反力として伝達要素83から第1慣性体91に伝わる。このため、加え合わせ点AP02で示されるように、第1慣性体91には、トルクTMからトルクTstを減算したトルクTRが伝わる。また、第2慣性体92の速度ωLが、上記差速度ωsを定める速度成分として第2慣性体92から伝達要素83に伝わる。
更に、第1慣性体91の速度ωMに比例する粘性抵抗が第1慣性体91に生じ、第2慣性体92の速度ωLに比例する粘性抵抗が第2慣性体92に生じる。具体的には、伝達関数TF11で示されるように、速度ωMに粘性係数DMを乗算した大きさのトルクTdmが第1慣性体91に作用する。このため、加え合わせ点AP02で示されるように、上記トルクTRは、トルクTMからトルクTstを減算し、更にトルクTdmを減算した値となる。
また、伝達関数TF12で示されるように、速度ωLに粘性係数DLを乗算した大きさのトルクTdlが第2慣性体92に作用する。このため、加え合わせ点AP01で示されるように、第2慣性体92には、トルクTsからトルクTdisを減算し、更にトルクTdlを減算したトルクTLが伝わる。
このように、第1慣性体91と第2慣性体92との間に生じるフィードバック成分の具体例としては、伝達要素83から第1慣性体91に入力される反力であるフィードバック成分FB01と、第2慣性体92から伝達要素83に入力される第2慣性体92の動作速度であるフィードバック成分FB02と、第1慣性体91に生じる粘性抵抗であるフィードバック成分FB03と、第2慣性体92に生じる粘性抵抗であるフィードバック成分FB04とが挙げられる。
コントローラ100(制御装置)は、電機子62に対し、第2慣性体92の駆動力を制御目標に追従させるための駆動電力を出力することと、制御対象(例えばロボット10)内で生じ第2慣性体92の動作に影響するフィードバック成分を打ち消す補償成分を制御対象の動作状態に基づいて算出し、電機子に出力する駆動電力に加算することと、を実行するように構成されている。
図4は、コントローラ100の少なくとも一部と機能的に等価な構成を示す図である。図4に示すように、コントローラ100は、機能上の構成(以下、「機能ブロック」という。)として、加速度制御部111と、電流制御部112と、瞬時状態オブザーバ113と、補償部114と、電流検出部119とを有する。
加速度制御部111は、第2慣性体92の加速度aLを目標加速度とするトルク目標値(制御目標)に第2慣性体92の駆動トルク(駆動力)を近付けるように、第1慣性体91の駆動トルクの目標値を算出することを所定の制御周期で繰り返し実行する。
電流制御部112(制御部)は、加速度制御部111がトルク目標値を算出する度に、第1慣性体91の駆動トルクを当該トルク目標値とするための電流を電機子62に出力する。電流検出部119は、電流制御部112が電機子62に出力する電流を検出する。
瞬時状態オブザーバ113は、第2慣性体92の加速度に相関する物理量に基づいて制御対象の状態を推定する。例えば瞬時状態オブザーバ113は、加速度センサ51,52,53,54,55,56による検出値と、速度センサ70による検出値と、電流検出部119による検出値とに基づいて、第2慣性体92の速度及び撓み角等を推定する。
補償部114は、制御対象内で生じ第2慣性体92の動作に影響するフィードバック成分を打ち消す補償成分を制御対象の動作状態に基づいて算出し、電流制御部112が電機子62に出力する駆動電力に加算する。
補償部114は、伝達要素83から第1慣性体91に入力されるフィードバック成分を打ち消す第1補償成分と、第2慣性体92から伝達要素83に入力されるフィードバック成分を打ち消す第2補償成分とを制御対象の動作状態に基づいて算出し、電流制御部112が電機子62に出力する駆動電力に加算してもよい。例えば補償部114は、伝達要素83から第1慣性体91に入力される反力である上記フィードバック成分FB01を打ち消すように第1補償成分を算出し、第2慣性体92から伝達要素83に入力される第2慣性体92の動作速度であるフィードバック成分FB02を打ち消すように第2補償成分を算出する。
補償部114は、第1慣性体91に生じる粘性抵抗であるフィードバック成分FB03を打ち消す第3補償成分と、第2慣性体92に生じる粘性抵抗であるフィードバック成分FB04を打ち消す第4補償成分とを、電流制御部112が電機子62に出力する駆動電力に更に加算してもよい。
補償部114は、第2慣性体92の加速度に相関する物理量の同時点の検出値に基づいて第1補償成分及び第2補償成分を算出してもよい。また、補償部114は、上記物理量の同時点の検出値に基づいて第1補償成分、第2補償成分、第3補償成分及び第4補償成分を算出してもよい。
補償部114は、第1補償成分及び第2補償成分の少なくとも一方を瞬時状態オブザーバ113により算出してもよい。例えば補償部114は、各制御周期において、加速度センサ51,52,53,54,55,56が検出した加速度と、速度センサ70が検出した第1慣性体91の動作速度と、電流検出部119が検出した電流とに基づき瞬時状態オブザーバ113が推定した制御対象の動作状態に基づいて上記第1補償成分、第2補償成分、第3補償成分及び第4補償成分を算出する。
〔制御手順〕
続いて、制御方法の一例として、コントローラ100が実行する制御手順を具体的に例示する。この制御手順は、電機子62に対し、第2慣性体92の駆動力を制御目標に追従させるための駆動電力を出力することと、制御対象内で生じ第2慣性体92の動作に影響するフィードバック成分を打ち消す補償成分を制御対象の動作状態に基づいて算出し、電機子62に出力する駆動電力に加算することとを含む。
上記第1補償成分と、上記第2補償成分とを制御対象の動作状態に基づいて算出し、電機子62に出力する駆動電力に加算してもよい。例えば上記フィードバック成分FB01を打ち消すように第1補償成分を算出し、上記フィードバック成分FB02を打ち消すように第2補償成分を算出してもよい。第2慣性体92の加速度に相関する物理量の同時点の検出値に基づいて第1補償成分及び第2補償成分を算出してもよい。第1補償成分及び第2補償成分の少なくとも一方を瞬時状態オブザーバ113により算出してもよい。
この制御手順は、瞬時状態オブザーバ113による状態推定手順と、加速度制御部111による第1慣性体91のトルク目標値の算出手順と、電流制御部112による駆動電力の出力手順と、補償部114による補償成分の加算手順とを含む。以下、各手順を詳細に例示する。
(状態推定手順)
図5に示すように、電流制御部112が電機子62に出力する電流Iqは、伝達関数TF21で示される遅れをもって電流検出部119により検出され、その検出結果である電流検出値Iq_sensが瞬時状態オブザーバ113に入力される。
また、第2慣性体92の加速度aLは、伝達関数TF22で示される遅れをもって加速度センサ51,52,53,54,55,56により検出され、その検出結果である加速度検出値aL_sensが瞬時状態オブザーバ113に更に入力される。
第1慣性体91の速度ωMは、伝達関数TF23で示される遅れをもって速度センサ70により検出され、その検出結果である速度検出値ωM_sensが瞬時状態オブザーバ113に更に入力される。
瞬時状態オブザーバ113は、電流検出値Iq_sens、加速度検出値aL_sens及び速度検出値ωM_sensに基づいて、第2慣性体92の速度推定値ωL_ob、第1慣性体91の速度推定値ωM_ob、及び伝達要素83の撓み角推定値θs_obを算出する。なお、速度推定値ωM_obは、速度検出値ωM_sensと同じである。
(トルク目標値の算出手順)
図6に示すように、加速度制御部111は、第2慣性体92の駆動トルク(上記トルクTL)の目標値であるトルク目標値TL_refと、第2慣性体92の駆動トルクのフィードバック値であるトルクフィードバック値TL_fbとを算出し、これらの偏差(以下、「トルク偏差」という。)を算出する。例えば伝達関数TF31で示されるように、加速度制御部111は、加速度目標値aL_refにイナーシャモデル値JLnを乗算してトルク目標値TL_refを算出する。イナーシャモデル値JLnは、第2慣性体92のイナーシャのモデル値であり、コントローラ100が保持している。
伝達関数TF32で示されるように、加速度制御部111は、第2慣性体92の加速度検出値aL_sensにイナーシャモデル値JLnを乗算してトルクフィードバック値TL_fbを算出する。加え合わせ点AP31で示されるように、加速度制御部111は、トルク目標値TL_refからトルクフィードバック値TL_fbを減算して上記トルク偏差を算出する。
次に加速度制御部111は、伝達要素83に入力する差速度ωsの目標値である差速度目標値ωs_refと、差速度ωsのフィードバック値である差速度フィードバック値ωs_fbとを算出し、これらの偏差(以下、「差速度偏差」という。)を算出する。例えば伝達関数TF33,TF34で示されるように、加速度制御部111は、加速度ゲインgaとばね定数モデル値Ksnの逆数とをトルク偏差に乗算して差速度目標値ωs_refを算出する。加速度ゲインgaは予め設定された定数であり、ばね定数モデル値Ksnは伝達要素83のばね定数Ksのモデル値であり、いずれもコントローラ100が保持している。
伝達関数TF35で示されるように、加速度制御部111は、第1慣性体91の速度検出値ωM_sensに減速比モデル値Rgnを乗算して理想速度ωL_idを算出する。減速比モデル値Rgnは、伝達要素83の減速比のモデル値であり、コントローラ100が保持している。理想速度ωL_idは、差速度ωsがゼロである場合の速度ωLである。
加え合わせ点AP32で示されるように、加速度制御部111は、第2慣性体92の速度推定値ωL_obから理想速度ωL_idを減算して差速度フィードバック値ωs_fbを算出する。
加え合わせ点AP33で示されるように、加速度制御部111は、差速度目標値ωs_refから差速度フィードバック値ωs_fbを減算して上記差速度偏差を算出する。
次に、加速度制御部111は、差速度偏差に基づいて、第1慣性体91の駆動トルクの目標値であるトルク目標値TM_refを算出する。例えば加速度制御部111は、伝達関数TF37,TF38で示されるように差速度偏差に差速度ゲインgvを乗算して加速度目標値aM_refを算出し、加速度目標値aM_refにイナーシャモデル値JMn及び減速比モデル値Rgnを乗算してトルク目標値TM_refを算出する。差速度ゲインgvは予め設定された定数であり、イナーシャモデル値JMnは第1慣性体91のイナーシャのモデル値であり、いずれもコントローラ100が保持している。以上で、1制御周期におけるトルク目標値の算出手順が完了する。
(駆動電力の出力手順)
図7に示すように、電流制御部112は、トルク目標値TM_refに基づいて電流目標値Iq_refを算出し、電流目標値Iq_refに応じた電流Iqを電機子62に出力する。例えば伝達関数TF41で示されるように、電流制御部112は、トルク定数モデル値Ktnの逆数をトルク目標値TM_refに乗算して電流目標値Iq_refを算出する。トルク定数モデル値Ktnは、上記トルク定数KTのモデル値であり、コントローラ100が保持している。
電流制御部112は、電流目標値Iq_refに対し、伝達関数TF42で示される遅れをもって電流目標値Iq_refに従う電流Iqを算出し、電機子62に出力する。
(補償成分の加算手順)
図6において加え合わせ点AP34で示されるように、補償部114は、上記加速度目標値aM_refに加速度検出値aL_sensを加算する。ここで加算された加速度検出値aL_sensは、伝達関数TF38,TF41,TF42を経て上記第2補償成分となる。換言すると、補償部114は上記第2補償成分を算出して電流Iqに加算する。第2補償成分は、伝達関数TF01,TF02,TF03,TF04を経て差速度ωsに相当する値となり、フィードバック成分FB02を打ち消す。
加速度検出値aL_sensの加算箇所は必ずしも加え合わせ点AP34に限られない。例えば補償部114は、加速度検出値aL_sensにイナーシャモデル値JMn及び減速比モデル値Rgnを乗算した値をトルク目標値TM_refに加算してもよい。
伝達関数TF51で示されるように、補償部114は、速度推定値ωL_obに粘性係数モデル値DLnを乗算して第2慣性体92に生じる粘性抵抗を算出し、これを加え合わせ点AP31でトルク目標値TL_refに加算する。粘性係数モデル値DLnは上記粘性係数DLのモデル値であり、コントローラ100が保持している。
ここで加算された粘性抵抗は、伝達関数TF33,TF34,TF37,TF38,TF41,TF42を経て上記第4補償成分となる。換言すると、補償部114は上記第4補償成分を算出して電流Iqに加算する。第4補償成分は、伝達関数TF01,TF02,TF03,TF04,TF05,TF06を経て第2慣性体92の粘性抵抗に相当する値となり、フィードバック成分FB04を打ち消す。
粘性抵抗の加算箇所は必ずしも加え合わせ点AP31に限られない。例えば補償部114は、粘性抵抗に加速度ゲインgaとばね定数モデル値Ksnの逆数とを乗算して差速度目標値ωs_refに加算してもよい。
図7の伝達関数TF52,TF53で示されるように、補償部114は、撓み角推定値θs_obにばね定数モデル値Ksnを乗算し、更にこれに減速比モデル値Rgnの逆数を乗算してトルク推定値Ts_obを算出する。トルク推定値Ts_obは、伝達要素83から第2慣性体92に伝わる上記トルクTsの推定値である。加え合わせ点AP35で示されるように、補償部114は、トルク目標値TM_refにトルク推定値Ts_obを加算する。
ここで加算されたトルク推定値Ts_obは、伝達関数TF41,TF42を経て上記第1補償成分となる。換言すると、補償部114は第1補償成分を算出して電流Iqに加算する。第1補償成分は、伝達関数TF01を経てトルクTsに相当する値となり、フィードバック成分FB01を打ち消す。
トルク推定値Ts_obの加算箇所は必ずしも加え合わせ点AP35に限られない。例えば補償部114は、トルク推定値Ts_obにトルク定数モデル値Ktnの逆数を乗算して電流目標値Iq_refに加算してもよい。
伝達関数TF54で示されるように、補償部114は、速度検出値ωM_sensに粘性係数モデル値DMnを乗算して第1慣性体91に生じる粘性抵抗を算出し、これを加え合わせ点AP35でトルク目標値TM_refに加算する。粘性係数モデル値DMnは上記粘性係数DMのモデル値であり、コントローラ100が保持している。
ここで加算された粘性抵抗は、伝達関数TF41,TF42を経て上記第3補償成分となる。換言すると、補償部114は上記第3補償成分を算出して電流Iqに加算する。第3補償成分は、伝達関数TF01を経て第1慣性体91の粘性抵抗に相当する値となり、フィードバック成分FB03を打ち消す。
粘性抵抗の加算箇所は必ずしも加え合わせ点AP35に限られない。例えば補償部114は、粘性抵抗にトルク定数モデル値Ktnの逆数を乗算して電流目標値Iq_refに加算してもよい。
このように、補償部114が第1補償成分、第2補償成分、第3補償成分、及び第4補償成分を電流Iqに加算することによって、フィードバック成分FB01,FB02,FB03,FB04が打ち消され、図3のブロック線図が図8のように変形される。これにより、第1慣性体91及び第2慣性体92の挙動が一つの遅れ要素に近付けられる。
図9は、コントローラ100のハードウェア構成を例示するブロック図である。図9に示すように、コントローラ100は、回路120を有する。回路120は、一つ又は複数のプロセッサ121と、メモリ122と、ストレージ123と、ドライバ124と、入出力ポート125と、を含む。ストレージ123は、例えば不揮発性の半導体メモリ等、コンピュータによって読み取り可能な記憶媒体を有する。ストレージ123は、電機子62に対し、第2慣性体92の駆動力を制御目標に追従させるための駆動電力を出力することと、制御対象(例えばロボット10)内で生じ第2慣性体92の動作に影響するフィードバック成分を打ち消す補償成分を制御対象の動作状態に基づいて算出し、電機子62に出力する駆動電力に加算することと、をコントローラ100に実行させるプログラムを記憶している。例えばストレージ123は、上述したコントローラ100の機能ブロックを構成するためのプログラムを記憶している。
メモリ122は、ストレージ123の記憶媒体からロードしたプログラム及びプロセッサ121による演算結果を一時的に記憶する。プロセッサ121は、メモリ122と協働して上記プログラムを実行することで、コントローラ100の各機能ブロックを構成する。ドライバ124は、プロセッサ121からの指令に従って、モータ60を駆動する。入出力ポート125は、プロセッサ121からの指令に従って、速度センサ70及び加速度センサ51,52,53,54,55,56との間で電気信号の入出力を行う。なお、回路120は、必ずしもプログラムにより各機能を構成するものに限られない。例えば回路120は、専用の論理回路又はこれを集積したASIC(Application Specific Integrated Circuit)により少なくとも一部の機能を構成してもよい。
〔応用例〕
第2慣性体92の駆動力を制御目標に追従させる構成は、第2慣性体92の駆動力の制御を必要とする様々な制御に応用可能である。以下に応用例を示す。
図10に示すコントローラ101は、第2慣性体92の駆動力を制御目標に追従させる構成を利用して、第2慣性体92の動作速度を制御する。例えばコントローラ101は、上述したコントローラ100に速度制御部115を付加したものである。
図11に示すように、速度制御部115は、第2慣性体92の速度ωLの目標値である速度目標値ωL_refと、速度推定値ωL_obとの偏差(以下、「速度偏差」という。)に応じた加速度目標値aL_refを算出し、加速度制御部111に出力する。例えば加え合わせ点AP61で示されるように、速度制御部115は、速度目標値ωL_refから速度推定値ωL_obを減算して速度偏差を算出する。
伝達関数TF61で示されるように、速度制御部115は、速度偏差に速度ゲインgvpを乗算して加速度目標値aL_refを算出する。速度ゲインgvpは予め設定された定数であり、コントローラ100が保持している。以上で速度制御部115による加速度目標値aL_refの算出手順が完了する。加速度目標値aL_refに加速度aLを追従させる制御についてはコントローラ100と同様である。
図12に示すコントローラ102は、第2慣性体92の駆動力を制御目標に追従させる構成を利用して、第2慣性体92の位置を制御する。例えばコントローラ102は、上述したコントローラ101に位置制御部116を付加したものである。
図13に示すように、位置制御部116は、第2慣性体92の位置θLの目標値である位置目標値θL_refと、位置推定値θL_obとの偏差(以下、「位置偏差」という。)に応じた速度目標値ωL_refを算出し、速度制御部115に出力する。例えば加え合わせ点AP62で示されるように、位置制御部116は、第2慣性体92の位置目標値θL_ref(角度目標値)から第2慣性体92の位置推定値θL_obを減算して位置偏差を算出する。位置推定値θL_obは、例えば速度推定値ωL_obの積分により算出される。
伝達関数TF62で示されるように、位置制御部116は、位置偏差に位置ゲインgppを乗算して速度目標値ωL_refを算出する。位置ゲインgppは予め設定された定数であり、コントローラ100が保持している。以上で位置制御部116による速度目標値ωL_refの算出手順が完了する。速度目標値ωL_refに速度ωLを追従させる制御についてはコントローラ101と同様である。
図14に示すコントローラ103は、第2慣性体92の駆動力を制御目標に追従させる構成を利用して、第2慣性体92が外部環境に対して付与する力を制御する。例えばコントローラ103は、上述したコントローラ100の加速度制御部111を力制御部117に置き換えたものである。
力制御部117は、第2慣性体92から外部環境に対して付与する外部トルクTeをトルク目標値(制御目標)に近付けるように、第1慣性体91の駆動トルクの目標値を算出することを所定の制御周期で繰り返し実行する。
図15は、第1慣性体91、伝達要素83及び第2慣性体92に加えて、上記外部環境をモデル化して示すブロック線図である。図15において、外部環境は、弾性及び粘性を有するものとしてモデル化されている。すなわち図15においては、外部環境から第2慣性体92に弾性反発力及び粘性抵抗が作用する。
例えば伝達関数TF71,TF72で示されるように、外部環境においては、速度ωLを積分した変位角度に外部環境のばね定数KEを乗算した大きさの弾性反発力が生じる。また、伝達関数TF73で示されるように、速度ωLに外部環境の粘性成分DEを乗算した大きさの粘性抵抗が生じる。そして、加え合わせ点AP71で示されるように、上記弾性反発力と上記粘性抵抗とが加算された外部トルクTeが第2慣性体92に伝わる。このため、加え合わせ点AP01で示されるように、第2慣性体92には、トルクTsから外部トルクTeを減算したトルクTLが伝わる。
図16に示すように、力制御部117は、第2慣性体92が外部環境に対して付与する力(上記外部トルクTe)のフィードバック値であるトルクフィードバック値Te_fbを算出し、外部トルクTeの目標値であるトルク目標値Te_refと上記トルクフィードバック値Te_fbとの偏差(以下、「トルク偏差」という。)を算出する。
例えば加え合わせ点AP64で示されるように、力制御部117は、外部トルクTeの検出値であるトルク検出値Te_sensと、加速度aLに応じた上記トルクフィードバック値TL_fbとを合算してトルクフィードバック値Te_fbを算出する。加え合わせ点AP65で示されるように、力制御部117は、トルク目標値Te_refからトルクフィードバック値Te_fbを減算して上記トルク偏差を算出する。トルク偏差を縮小させるための制御手順はコントローラ100と同じである。
なお、コントローラ103において、補償部114は、速度推定値ωL_obに粘性係数モデル値DLnを乗算して算出した粘性抵抗を加え合わせ点AP65でトルク目標値Te_refに加算する。
図17に示すコントローラ104は、第2慣性体92の駆動力を制御目標に追従させる構成を利用して、第2慣性体92の位置と、第2慣性体92が外部環境に対して付与する力とのハイブリッド制御を行う。例えばコントローラ104は、上述したコントローラ100の加速度制御部111をハイブリッド制御部118に置き換えたものである。
ハイブリッド制御部118は、第2慣性体92の位置と、第2慣性体92が外部環境に付与する力との両方を制御目標に近付けるように第1慣性体91の駆動トルクの目標値を算出することを所定の制御周期で繰り返し実行する。
図18は、ハイブリッド制御部118が実行する第1慣性体91のトルク目標値の算出手順を例示するブロック線図である。上述したように、ロボット10は、アクチュエータ41,42,43,44,45,46ごとに加速度センサを有するのに代えて、アクチュエータ41,42,43,44,45,46のいずれによっても駆動される先端部16に加速度センサを有していてもよい。
図18は、先端部16の加速度センサ57及び力センサ90に基づいてアクチュエータ41,42,43,44,45,46を制御する場合を例示している。図18において、先端部16の位置、速度、加速度及び力等は、いずれも直交3軸方向に沿う並進成分と、直交3軸まわりの回転成分との6成分を含む。
図18に示すように、ハイブリッド制御部118は、先端部16の位置のフィードバック値である位置フィードバック値Pt_fbを算出し、先端部16の位置目標値Pt_refと位置フィードバック値Pt_fbとの偏差に応じた先端部16の速度目標値Vt_refを算出する。
例えばハイブリッド制御部118は、伝達関数TF81,TF82で示されるように、関節31,32,33,34,35,36の速度推定値ωL_obを積分し、積分結果に順運動学演算を施して位置フィードバック値Pt_fbを算出する。ハイブリッド制御部118は、加え合わせ点AP81で示されるように、位置目標値Pt_refから位置フィードバック値Pt_fbを減算して偏差(以下、「位置偏差」という。)を算出する。伝達関数TF83で示されるように、ハイブリッド制御部118は、位置偏差に位置ゲインgppを乗算して速度目標値Vt_refを算出する。位置ゲインgppは予め設定された定数であり、コントローラ100が保持している。
ハイブリッド制御部118は、先端部16の速度のフィードバック値である速度フィードバック値Vt_fbを算出し、先端部16の速度目標値Vt_refと速度フィードバック値Vt_fbとの偏差に応じた先端部16の加速度目標値at_refを算出する。
例えばハイブリッド制御部118は、伝達関数TF84で示されるように、関節31,32,33,34,35,36の速度推定値ωL_obに所定の変換行列Jを乗算して先端部16の速度フィードバック値Vt_fbを算出する。ハイブリッド制御部118は、加え合わせ点AP82で示されるように、速度目標値Vt_refから速度フィードバック値Vt_fbを減算して偏差(以下、「速度偏差」という。)を算出する。伝達関数TF85で示されるように、ハイブリッド制御部118は、速度偏差に速度ゲインgvpを乗算して加速度目標値at_refを算出する。速度ゲインgvpは予め設定された定数であり、コントローラ100が保持している。
次に、ハイブリッド制御部118は、加速度目標値at_refに基づいて、第2慣性体92の位置を制御するための駆動トルクの目標値であるトルク目標値TL_refをアクチュエータ41,42,43,44,45,46ごとに算出する。
例えばハイブリッド制御部118は、伝達関数TF86で示されるように、加速度目標値at_refに逆変換行列J-1を乗算してアクチュエータ41,42,43,44,45,46ごとの加速度目標値aL_refを算出する。この際に、ハイブリッド制御部118は、加速度目標値at_refに選択行列Sを更に乗算してもよい。選択行列Sは、第2慣性体92の位置の制御方向の成分を抽出するための行列であり、コントローラ100が保持している。更にハイブリッド制御部118は、伝達関数TF87で示されるように、加速度目標値aL_refにイナーシャモデル値JLnを乗算してトルク目標値TL_refを算出する。
また、ハイブリッド制御部118は、第2慣性体92が外部環境に付与する力を制御するためのトルク目標値Te_refを算出する。例えばハイブリッド制御部118は、伝達関数TF88で示されるように、先端部16が外部環境に付与する力の目標値である力目標値Ft_refに変換行列JTを乗算してアクチュエータ41,42,43,44,45,46ごとのトルク目標値Te_refを算出する。この際に、ハイブリッド制御部118は、力目標値Ft_refに選択行列I-Sを更に乗算してもよい。選択行列I-Sは、第2慣性体92が外部環境に付与する力の制御方向の成分を抽出するための行列であり、コントローラ100が保持している。
このように、第2慣性体92の位置を制御するためのトルク目標値TL_refと、第2慣性体92が外部環境に付与する力を制御するためのトルク目標値Te_refとを算出した後、ハイブリッド制御部118は、加え合わせ点AP83で示されるようにこれらを合算してトルク目標値Ts_refを算出する。トルク目標値Ts_refは、伝達要素83から第2慣性体92に伝わるトルクの目標値である。
ハイブリッド制御部118は、伝達要素83から第2慣性体92に伝わるトルクのフィードバック値であるトルクフィードバック値Ts_fbを算出し、上記トルク目標値Ts_refとトルクフィードバック値Ts_fbとの偏差(以下、「トルク偏差」という。)を算出する。
例えば伝達関数TF91,TF92で示されるように、ハイブリッド制御部118は、先端部16の加速度検出値at_sensに逆変換行列J-1を乗算し、更にこれにイナーシャモデル値JLnを乗算してトルクフィードバック値TL_fbを算出する。また、伝達関数TF93で示されるように、ハイブリッド制御部118は、先端部16が外部環境に付与する力の検出値である力検出値Ft_sensに変換行列JTを乗算してトルクフィードバック値Te_fbを算出する。この際に、ハイブリッド制御部118は、力検出値Ft_sensに上述した選択行列I-Sを更に乗算してもよい。
その後ハイブリッド制御部118は、加え合わせ点AP84で示されるように、トルクフィードバック値TL_fbとトルクフィードバック値Te_fbとを合算してトルクフィードバック値Ts_fbを算出する。
次に、ハイブリッド制御部118は、加え合わせ点AP85で示されるように、トルク目標値Ts_refからトルクフィードバック値Ts_fbを減算してトルク偏差を算出する。トルク偏差を縮小させるための制御手順はコントローラ100と同じである。
なお、コントローラ104において、補償部114は、速度推定値ωL_obに粘性係数モデル値DLnを乗算して算出した粘性抵抗を加え合わせ点AP85でトルク目標値Ts_refに加算する。
〔本実施形態の効果〕
以上に説明したように、コントローラ100は、第1慣性体91と、電力の供給に応じて第1慣性体91を駆動する電機子62と、第2慣性体92と、第1慣性体91から第2慣性体92に動力を伝達する伝達要素83とを有する制御対象の電機子62に対し、第2慣性体92の駆動力を制御目標に追従させるための駆動電力を出力する電流制御部112と、制御対象内で生じ第2慣性体92の動作に影響するフィードバック成分を打ち消す補償成分を制御対象の動作状態に基づいて算出し、電流制御部112が電機子62に出力する駆動電力に加算する補償部114と、を備える。
制御対象内で生じるフィードバック成分が打ち消されることで、第1慣性体91及び第2慣性体92の挙動が一つの遅れ要素に近付けられる。これにより、第1慣性体91と第2慣性体92との間に生じる振動が抑制されるので、制御目標に対する第2慣性体92の駆動力の追従性が向上する。従って、制御の安定性向上に有効である。
補償部114は、伝達要素83から第1慣性体91に入力されるフィードバック成分を打ち消す第1補償成分と、第2慣性体92から伝達要素83に入力されるフィードバック成分を打ち消す第2補償成分とを制御対象の動作状態に基づいて算出し、電流制御部112が電機子62に出力する駆動電力に加算してもよい。この場合、第1慣性体91と第2慣性体92との間に生じる振動がより確実に抑制される。
補償部114は、伝達要素83から第1慣性体91に入力される反力を打ち消すように第1補償成分を算出し、第2慣性体92から伝達要素83に入力される第2慣性体92の動作速度を打ち消すように第2補償成分を算出してもよい。
補償部114は、第2慣性体92の加速度に相関する物理量の同時点の検出値に基づいて第1補償成分及び第2補償成分を算出してもよい。この場合、第1補償成分及び第2補償成分の位相ずれが抑制される。このため、第1慣性体91と第2慣性体92との間に生じる振動がより確実に抑制される。
補償部114は、第1補償成分及び第2補償成分の少なくとも一方を瞬時状態オブザーバ113により算出してもよい。この場合、第1補償成分及び第2補償成分を容易に導出することができる。
以上、実施形態について説明したが、本開示は必ずしも上述した実施形態に限定されるものではなく、その要旨を逸脱しない範囲で様々な変更が可能である。例えば、上述の実施形態では、ロボットが制御対象であるロボットシステムを例示したがこれに限られない。本開示の制御装置は、第1慣性体と、電力の供給に応じて第1慣性体を駆動する電機子と、第2慣性体と、第1慣性体から第2慣性体に動力を伝達する伝達要素とを有するあらゆる制御対象の制御において、第2慣性体の駆動力の追従性を向上させるのに有効である。