JP5411687B2 - ロボットの制御装置 - Google Patents

Description

本発明は、ロボットの制御装置、特に振動を抑制しながらロボットを制御することができるロボットの制御装置に関する。

従来より、ロボットが種々の分野で使用されている。このようなロボットは、複数のリンクと複数のモータとを有し、各モータが対応するリンクを回転させることに動作する。

ところが、近年の高速化および高精度化により、リンクの加減速時に該リンクに振動が発生するということが問題になっている。これは、モータとリンクとの間のバネ要素にねじれが生じることによる。

また、ロボットの動作中、特に高速動作中、各リンクの動作は他のリンクの動作の影響（干渉）を受ける。したがって、リンクの振動を抑制するためには、各モータと対応するリンクとの間のバネ要素それぞれのねじれを考慮するとともに、複数のリンク間の干渉も考慮しなければならない。

この問題に対処する方法が特許文献１に記載されている。特許文献１に記載する方法では、複数のモータそれぞれと対応するリンクとの間のバネ要素それぞれについて、状態観測器が設けられる。各状態観測器は、バネ要素に対応するモータの制御入力と該モータの回転角度（角度位置）とに基づき、バネ要素のねじれ角度を推定する。次に、各状態観測器が推定した対応するバネ要素のねじれ角度に基づいて、複数のリンク間の干渉（干渉力）を算出する。そして、算出した干渉力に基づいて、各バネ要素のねじれがゼロになるようなトルクを対応するモータが出力できるように、各モータの制御入力が補償される。これにより、バネ要素のねじれは解消され、リンクの振動が抑制される。

特許第３６１１１４７号

しかし、上述の特許文献１に記載する方法では、リンクの振動を十分に抑制することができない可能性がある。説明すると、特許文献１の方法では、複数の状態観測器それぞれが推定するバネ要素のねじれ角度は、対応するモータの制御入力と該モータの回転角度のみに基づいている。すなわち、バネ要素のねじれ角度を推定するにあたり、リンク間の干渉を考慮していない。

具体的に説明すると、例えばモータとリンクがＮ個ずつある場合、図５に示すように、特許文献１の方法は、１つのモータと対応する１つのリンクから構成される２慣性モデル（Ｎ個の２慣性モデル）に基づいている。そのため、１つのモータと対応する１つのリンクとの間のバネ要素のねじれ角度を推定するにあたり、当該リンクと他のリンクとの間の干渉を考慮していない。したがって、特許文献１の方法が推定するバネ要素のねじれ角度は、推定精度に欠ける。その結果、リンクの振動が十分に抑制されない可能性がある。

そこで、本発明は、複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットにおいて、リンクとモータとの間のバネ要素のねじれを原因として発生するリンクの振動を十分に抑制することを課題とする。

上述の課題を解決するために、本発明の請求項１に係る発明は、
複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットの制御装置であって、
対応するモータの回転角度または回転角速度を検出する複数のセンサと、
各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定した対応するバネ要素のねじれ角度およびねじれ角速度に基づいて、
対応するバネ要素のねじれが解消するトルクをモータが出力するように、対応するモータの制御入力に加算するトルク補償量を算出する複数のトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力を対応するモータに出力する複数のモータ制御手段とを有し、
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、各モータの制御入力から複数のバネ要素それぞれのねじれ角度に至るモータ数×バネ要素数個の伝達関数と、各モータの制御入力から複数のバネ要素それぞれのねじれ角速度に至るモータ数×バネ要素数個の伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とする。

さらに、本発明の請求項２に係る発明は、
リンクと、リンクを駆動するモータとを備えたロボットの制御装置であって、
モータの回転角度または回転角速度を検出するセンサと、
リンクとモータとの間のバネ要素のねじれ角度およびねじれ角速度を、モータへの制御入力とセンサが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定したバネ要素のねじれ角度およびねじれ角速度に基づいて、バネ要素のねじれが解消するトルクをモータが出力するように、モータの制御入力に加算するトルク補償量を算出するトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力をモータに出力するモータ制御手段とを有し、
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、モータの制御入力からバネ要素のねじれ角度に至る伝達関数と、モータの制御入力からバネ要素のねじれ角速度に至る伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とする。

本発明によれば、複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットにおいて、状態観測器は、各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する。そのため、各バネ要素のねじれ角度およびねじれ角速度を、これらを対応するモータへの制御入力と該モータの回転角度または回転角速度のみで推定する場合に比べて、高精度に推定することができる。すなわち、複数のリンク間の干渉を考慮して推定することができる。これにより、状態観測器が高精度に推定した各バネ要素のねじれ角度およびねじれ角速度に基づいて各ばね要素のねじれを解消でき、リンクの振動を十分に抑制することができる。

本発明の実施形態に係るロボットの一例の慣性モデルを示す図である。 図１の慣性モデルに対応するロボットの制御系を示す図である。 ４慣性モデルと２慣性モデルの共振周波数を示す図である。 極配置した場合と原点に零点を配置した場合の、モータ停止時のトルク補償量の変化を示す図である。 従来方法で用いられた２慣性モデルを説明するための図である。

本発明の一実施形態に係るロボットの制御装置について説明する。本発明は複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットに関するものであるが、理解を容易にするために、本明細書では、主に、２つのリンクと２つのモータを備えたロボットについて説明する。したがって、以下の説明は、本発明を２つのリンクと２つのモータを備えたロボットに限定するわけではない。

図１は、本発明の実施形態に係るロボットのモデルを示している。図に示すモデルは４慣性モデルであって、モータとアームとの間のバネ要素の剛性および減衰を考慮したモデルである。また、図５とは異なり、モータが、自身が駆動するリンク以外のリンクに取付けられていることを考慮したモデル、すなわち複数のリンク間の干渉を考慮したモデルである。なお、本明細書で言う「バネ要素」は、広義にはモータとリンクとを連結するものであって、モータシャフトやモータからリンクにトルクを伝達する減速機などのトルク伝達機構も含まれる。

図１の４慣性モデルは、第１および第２モータと、モータによって駆動される第１および第２リンクと、モータとリンクとの間の第１および第２バネ要素とから構成されている。

図１において、角度θ_Ｍ１，θ_Ｍ２は、第１および第２モータの回転角度（角度位置）である。角度θ_Ｌ１，θ_Ｌ２は、第１および第２リンクの角度である。第１および第２バネ要素のねじれ角度は、θ_Ｍ１−θ_Ｌ１、θ_Ｍ２−θ_Ｌ２で与えられる。また、Ｋ_Ｓ１，Ｋ_Ｓ２は第１および第２バネ要素の剛性であって、Ｄ_Ｓ１，Ｄ_Ｓ２は第１および第２バネ要素の減衰である。さらに、ｕ_１，ｕ_２は、第１および第２モータの出力トルクである。

図１に示す４慣性モデルの運動方程式は、遠心力・コリオリ力などの非線形な要素を無視すると、数式１〜４に示すように表現できる。

数式１〜４において、Ｊ_Ｌ１１，Ｊ_Ｌ１２，Ｊ_Ｌ２１，およびＪ_Ｌ２２は、リンクの質量、重心位置、長さなどのロボットパラメータとリンクの角度θ_Ｌ１，θ_Ｌ２から求まる係数である。また、Ｊ_Ｍ１，Ｊ_Ｍ２は、第１および第２モータの慣性である。さらに、

と

は、第１および第２バネ要素のねじれ角速度である。

この４つの運動方程式から数式５の次の状態方程式が得られる。

上述の４慣性モデルを適用した状態観測器を含む第１および第２モータの制御系、すなわち本発明に係るロボットの制御装置の制御系の一部のブロック図を図２に示す。

図２に示す状態観測器は、数式５の状態方程式と数式６の出力方程式に基づく観測器、すなわち第１リンクと第２リンクとの間の干渉を考慮した観測器である。行列Ａ，Ｂ，およびＣは４慣性モデルによって決定される行列である。行列Ｋは、詳細は後述するが、状態観測器の設計パラメータであって、フィードバックゲイン行列である。「∫」は、積分器である。

この状態観測器は、第１および第２モータそれぞれに対する制御入力（トルクｕ_１，ｕ_２に対応する入力電流）Ｉ_１，Ｉ_２と第１および第２モータそれぞれの回転角度θ_Ｍ１，θ_Ｍ２とに基づいて、状態ｘとして、第１バネ要素のねじれ角度θ_Ｍ１−θ_Ｌ１およびねじれ角速度

と第２バネ要素のねじれ角度θ_Ｍ２−θ_Ｌ２およびねじれ角速度

とを推定する。

厳密に言えば、状態観測器は、第１バネ要素のねじれ角度θ_Ｍ１−θ_Ｌ１およびねじれ角速度

を、第１モータの入力電流Ｉ_１および第２モータの入力電流Ｉ_２と、第１モータの回転角度θ_Ｍ１および第２のモータの回転角度θ_Ｍ２とから推定する。また、第２バネ要素のねじれ角度θ_Ｍ２−θ_Ｌ２およびねじれ角速度

を、第１モータの入力電流Ｉ_１および第２モータの入力電流Ｉ_２と、第１モータの回転角度θ_Ｍ１および第２のモータの回転角度θ_Ｍ２とから推定する。

そのため、この状態観測器は、第１バネ要素のねじれ角度θ_Ｍ１−θ_Ｌ１およびねじれ角速度

を、これらを第１モータの入力電流Ｉ_１と第１モータの回転角度θ_Ｍ１のみから推定する場合に比べて、高精度に推定することができる。また、第２バネ要素のねじれ角度θ_Ｍ２−θ_Ｌ２およびねじれ角速度

を、これらを第２モータの入力電流Ｉ_２と第２モータの回転角度θ_Ｍ２のみから推定する場合に比べて、高精度に推定することができる。

なお、第１および第２モータそれぞれの回転角度θ_Ｍ１，θ_Ｍ２の代りに、回転角速度

であってもよい。すなわち、回転角度または回転角速度のうち、実際に観測可能な方、例えばセンサ（エンコーダ）によって検出可能な方に基づいて状態観測器は状態ｘを推定する。

図２に示す第１および第２トルク補償量演算部は、状態観測器が推定した状態ｘに基づいて、数式７，８を用いて、トルク補償量Ｔ_ｕ１，Ｔ_ｕ２を算出する。

数式７，８において、Ｆ_１，Ｆ_２は、トルク補償量Ｔ_ｕ１，Ｔ_ｕ２を演算するためのゲイン行列である。具体的には、ゲイン行列Ｆ_１，Ｆ_２は、状態観測器が推定した状態ｘ（ねじれ角度とねじれ角速度）から、第１および第２バネ要素のねじれを解消できるトルクを第１および第２モータが出力するように、第１および第２モータの入力電流Ｉ_１，Ｉ_２に加算するトルク補償量Ｔ_ｕ１，Ｔ_ｕ２を算出するための行列である。

第１トルク補償量演算部が算出したトルク補償量Ｔ_ｕ１は、第１リンク位置／姿勢制御部から出力された第１モータへの入力電流Ｉ_１に加算される。第１リンク位置／姿勢制御部は、第１リンクの位置と姿勢とを制御するために、第１モータを制御する、ロボットの制御装置の構成要素である。

また、第２トルク補償量演算部が算出したトルク補償量Ｔ_ｕ２は、第２リンク位置／姿勢制御部から出力された第２モータへの入力電流Ｉ_２に加算される。第２リンク位置／姿勢制御部は、第２リンクの位置と姿勢とを制御するために、第２モータを制御する、ロボットの制御装置の構成要素である。

このような状態観測器と第１および第２トルク補償量演算部により、第１および第２バネ要素は、第１リンクと第２リンクとの間の干渉を考慮して、そのねじれが解消される。その結果、第１および第２リンクの振動が十分に抑制される。

また、別の効果として、第１リンクと第２リンクの間の干渉を考慮するため（すなわち４慣性モデルであるため）、複数の共振周波数を求めることが可能である。

具体的に説明する。図３（ａ）は、図１に示す４慣性モデルにおける、第１モータへの入力電流から該モータの回転角速度の推定値までのゲイン特性（実施例）を示す図である。一方、図３（ｂ）は、２慣性モデルにおける、モータへの入力電流から該モータの回転角速度の推定値までのゲイン特性（比較例）を示す図である。

図３（ａ）に示すように、４慣性モデルの場合、すなわち第１リンクと第２リンクとの間の干渉を考慮した場合、第１バネ要素および第２バネ要素それぞれについて、その挙動において２つの共振周波数を持つ。一方、図３（ｂ）に示すように、２慣性モデルの場合、モータとリンクとの間のバネ要素は、その挙動において１つの共振周波数を持つ。

したがって、モータとリンクとがそれぞれ複数あるにもかかわらず、２慣性モデルに基づく場合は、現実に存在する複数の共振周波数に対して考慮できず、各バネ要素について１つの共振周波数しか考慮できない。これにより、非考慮の共振周波数によるスピルオーバの問題が起こる。一方、リンク間の干渉を考慮すれば、４慣性モデルが各バネ要素について２つの共振周波数が考慮できるように、モータとリンクとがＮ個ずつある２Ｎ慣性モデルの場合、Ｎ個の共振周波数を考慮することができる。

また、本実施形態の状態観測器は、トルク補償量演算部のゲインを大きく設定することができるように、すなわち、それによって第１リンクや第２リンクの振動をさらに十分に抑制することができるように構成されている。

このことについて具体的に説明する。

図２に示す状態観測器の構成は、該状態観測器の状態ｘの推定値を

とすると、数式９のように表現することができる。

状態観測器が推定した状態

と実際の状態ｘとの誤差をｅ

とすると、数式９から数式１０が得られる。

誤差ｅの収束はフィードバックゲイン行列Ｋに依存し、フィードバックゲイン行列Ｋは、一般的には、極配置によって数式１０の（Ａ−ＫＣ）の極の実部が負となるように設計されている。この極の実部の値により、誤差ｅの収束度合いが決定される。

ところが、フィードバックゲイン行列Ｋを、極配置によって数式１０の（Ａ−ＫＣ）の極の実部が負になるように設計すると、モータの停止時に第１リンクや第２リンクに持続振動が発生する。

この持続振動の発生は、モータの特性を原因とする。通常、モータは非線形な静止摩擦があるため、その入力電流ゼロ付近で不感帯が存在し、モータ停止時に入力電流のオフセットが発生する。また、リンクには重力が作用するので該リンクを一定の姿勢で維持するためのトルクをモータは出力する必要があり、そのためにも入力電流のオフセットが発生する。

このようなオフセットが発生する入力電流を推定に使用すると、極配置によってフィードバックゲイン行列Ｋが設計されている状態観測器は、モータの停止時、実際の値との誤差が大きいねじれ角度やねじれ角速度を推定する。すなわち、モータの停止時、状態観測器の推定精度が低下する。

モータの停止時に状態観測器の推定精度が低下すると、トルク補償量演算部が出力するトルク補償量は、図４（ａ）に示すように、モータの停止時、持続振動する。なお、図４（ａ）は、数式１０に示す（Ａ−ＫＣ）の極の実部全てが（−１５０）になるように、フィードバックゲイン行列Ｋを設計した場合を示している。

図４（ａ）に示すようにトルク補償量が持続振動することを考えると、当然ながらトルク補償量演算部のゲインを大きく設定することはできない（ゲインを大きくすると、リンクが大きく振動する）。

したがって、トルク補償量演算部のゲインは、モータ停止時の入力電流のオフセットを原因とする、モータの停止時における状態観測器の推定精度の低下を考慮して、小さく設定される。

そこで、本実施形態の状態観測器では、モータ停止時に入力電流のオフセットが発生しても、モータ停止時において状態観測器の推定精度が低下しないように、すなわちトルク補償量演算部のゲインを大きく設定できるように、フィードバックゲイン行列Ｋが、各モータの入力電流から複数のバネ要素それぞれのねじれ角度およびねじれ角速度に至る複数の伝達関数全てが原点に零点を持つように設計されている。

具体的には、図１の４慣性モデルに対応する図２の状態観測器の場合、第１モータの入力電流Ｉ_１から第１バネ要素のねじれ角度θ_Ｍ１−θ_Ｌ１に至る伝達関数、入力電流Ｉ_１から第１バネ要素のねじれ角速度

に至る伝達関数、入力電流Ｉ_１から第２バネ要素のねじれ角度θ_Ｍ２−θ_Ｌ２に至る伝達関数、入力電流Ｉ_１から第２バネ要素のねじれ角速度

に至る伝達関数、第２モータの入力電流Ｉ_２から第１バネ要素のねじれ角度θ_Ｍ１−θ_Ｌ1に至る伝達関数、入力電流Ｉ_２から第１バネ要素のねじれ角速度

に至る伝達関数、入力電流Ｉ_２から第２バネ要素のねじれ角度θ_Ｍ２−θ_Ｌ２に至る伝達関数、および入力電流Ｉ_２から第２バネ要素のねじれ角速度

に至る伝達関数の全てが原点に零点を持つように、フィードバックゲイン行列Ｋが設計されている。言い換えると、８つの伝達関数全てにおいて、分子のｓ^０項（ラプラス演算子ｓの０乗の項）の係数がゼロになるように、フィードバックゲイン行列Ｋが設計されている。

このようなフィードバックゲイン行列Ｋの設計について具体的に説明する。なお、ここでは、説明を簡単にするために、１つのリンクと該リンクを駆動する１つのモータからなる２慣性モデルを例に挙げて説明する。

２慣性モデルの場合、状態方程式は、モータの回転角度をθ_Ｍ、リンクの角度をθ_Ｌ、
モータの慣性をＪ_Ｍ、リンクの慣性をＪ_Ｌ、モータとリンクとの間のバネ要素の剛性をＫ_Ｓ、バネ要素の減衰をＤ_Ｓとすると、数式１１のようになる。

ここで、フィードバックゲイン行列Ｋを

とし、モータの回転角度が観測可能（すなわち、

）とすると、モータの入力電流からバネ要素のねじれ角度θ_Ｍ−θ_Ｌの推定値に至る伝達関数は、数式１２のようになる。また、モータの入力電流からバネ要素のねじれ角速度

に至る伝達関数は、数式１３のようになる。なお、数式１２および１３における係数ａ_０〜ａ_４は、数式１４のようになる。

数式１２，１３に示す２つの伝達関数が原点に零点を持つためには、数式１２の分子のｓ^０項の係数Ｋ_２Ｄ_Ｓ−（Ｋ_３−Ｋ_４）Ｊ_Ｌと、数式１３の分子のＳ^０項の係数Ｋ_２Ｋ_Ｓの両方がゼロになるような値に、フィードバックゲイン行列Ｋの要素Ｋ_１〜Ｋ_４を決定しなければならない。

ここで、例えば、フィードバックゲイン行列Ｋを数式１５に示すように与える。

数式１５において、α，βは調整パラメータである。フィードバックゲイン行例Ｋを数式１５のように設計すれば、数式１２，１３に示す２つの伝達関数の分子のｓ^０項の係数がゼロになり、２つの伝達関数は原点に零点を持つ。なお、モータの回転角度ではなく回転角速度が観測可能な場合（すなわち、

）、調整パラメータβをゼロにすれば、同様に、２つの伝達関数は原点に零点を持つことができる。

数式１２，１３の伝達関数が原点に零点を持てば、図４（ｂ）に示すように、モータ停止時における、トルク補償量の持続振動の発生が抑制される。

これは、モータの入力電流からねじれ角度の推定値やねじれ角速度の推定値までの間の特性に微分が存在するため、低周波数域でのゲインが低下し、一定の入力値に対してはゲインがゼロになることによる。そのため、状態観測器は入力電流のオフセットの影響を受けにくくなり、その推定精度が低下しにくくなる。その結果、モータ停止時において、トルク補償量に持続振動が発生しにくくなり、トルク補償量演算部のゲインを大きく設定することができる。

当然ながら、モータの入力電流からバネ要素のねじれ角度およびねじれ角速度に至る複数の伝達関数が原点に零点を持つことにより、トルク補償量演算部のゲインを大きく設定することができることは、２慣性モデルに限らない。

例えば、図１の４慣性モデルにおいて第１モータが停止している場合、すなわち３慣性モデルの場合、状態方程式は数式１６のようになる。

ここで、フィードバックゲイン行列Ｋを

とし、モータの回転角度が観測可能とすると、第２モータの入力電流から第１バネ要素のねじれ角度θ_Ｍ１−θ_Ｌ１に至る伝達関数の分子のｓ^０項の係数は数式１７のように、第２モータの入力電流から第２バネ要素のねじれ角度θ_Ｍ２−θ_Ｌ２に至る伝達関数の分子のｓ^０項の係数は数式１８のように、第２モータの入力電流から第１バネ要素のねじれ角速度

に至る伝達関数の分子のｓ^０項の係数は数式１９のように、第２モータの入力電流から第２バネ要素のねじれ角速度

に至る伝達関数の分子のｓ^０項の係数は数式２０のようになる。

ここで、例えば、フィードバックゲイン行列Ｋを数式２１に示すように与える。

数式２１において、α，βは調整パラメータである。フィードバックゲイン行例Ｋを数式２１のように設計すれば、第２モータの入力電流から第１バネ要素のねじれ角度θ_Ｍ１−θ_Ｌ１に至る伝達関数、第２モータの入力電流から第２バネ要素のねじれ角度θ_Ｍ２−θ_Ｌ２に至る伝達関数、第２モータの入力電流から第１バネ要素のねじれ角速度

に至る伝達関数、および第２モータの入力電流から第２バネ要素のねじれ角速度

に至る伝達関数は、分子のｓ^０項の係数がゼロになり、４つの伝達関数は原点に零点を持つ。

さらに、図１に示す４慣性モデルにおいても、バックゲイン行列Ｋを数式２２のように設計すれば、第１モータの入力電流Ｉ_１から第１バネ要素のねじれ角度θ_Ｍ１−θ_Ｌ１に至る伝達関数、入力電流Ｉ_１から第１バネ要素のねじれ角速度

に至る伝達関数、入力電流Ｉ_１から第２バネ要素のねじれ角度θ_Ｍ２−θ_Ｌ２に至る伝達関数、入力電流Ｉ_１から第２バネ要素のねじれ角速度

に至る伝達関数、第２モータの入力電流Ｉ_２から第１バネ要素のねじれ角度θ_Ｍ１−θ_Ｌ1に至る伝達関数、入力電流Ｉ_２から第１バネ要素のねじれ角速度

に至る伝達関数、入力電流Ｉ_２から第２バネ要素のねじれ角度θ_Ｍ２−θ_Ｌ２に至る伝達関数、および入力電流Ｉ_２から第２バネ要素のねじれ角速度

に至る伝達関数の全てが原点に零点を持つ。

数式１５，２１，２２に示すように、慣性モデルに関わらず、トルク補償量演算部のゲインを大きく設定することができる、フィードバックゲイン行列Ｋは、

の形で表現することができる。したがって、複数モータのうち停止中のモータを特定することにより慣性モデルを決定し、決定した慣性モデルに基づく行列Ｂ、Ｃからフィードバックゲイン行列Ｋを算出することができる。

本実施形態によれば、複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットにおいて、状態観測器は、各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する。そのため、各バネ要素のねじれ角度およびねじれ角速度を、これらを対応するモータへの制御入力と該モータの回転角度または回転角速度のみで推定する場合に比べて、高精度に推定することができる。すなわち、複数のリンク間の干渉を考慮して推定することができる。これにより、状態観測器が高精度に推定した各バネ要素のねじれ角度およびねじれ角速度に基づいて各バネ要素のねじれを解消でき、リンクの振動を十分に抑制することができる。

また、本実施形態によれば、状態観測器に含まれるフィードバックゲイン行列Ｋが、各モータの制御入力から複数のバネ要素それぞれのねじれ角度に至るモータ数×バネ要素数個の伝達関数と、各モータの制御入力から複数のバネ要素それぞれのねじれ角速度に至るモータ数×バネ要素数個の伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計される。これにより、モータ停止時に入力電流のオフセットが発生しても、モータ停止時おける状態観測器の推定精度の低下が抑制される。そのため、状態観測器が推定した状態をモータの制御入力に大きくフィードバックすることができる（トルク補償量演算部のゲインを大きく設定することができる）。そして、その結果、リンクの振動をさらに十分に抑制することができる。

以上、上述の実施形態を挙げて、本発明を説明してきた。しかし、本発明は、上述の実施形態に限定されない。

例えば、上述の実施形態の場合、複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットにおいて、リンク間の干渉による影響と、モータ停止時の入力電流のオフセットの影響の両方に対処して状態観測器を構成することにより、リンクの振動を十分に抑制している。しかしながら、本発明は、この２つの影響の両方に状態観測器が対処することに限定しない。

例えば、ロボットが１つのリンクと該リンクを駆動する１つのモータのみで構成される場合、当然ながらリンク間の干渉の影響を考慮する必要はない。また、機構的にリンク間の干渉が起こらない場合も、リンク間の干渉を考慮しなくてもよい。このような場合、状態観測器において、フィードバックゲイン行列Ｋを、各モータへの制御入力から複数のバネ要素それぞれのねじれ角度およびねじれ角速度に至る複数の伝達関数が原点に零点を持つように設計して、モータ停止時の入力電流のオフセットの影響に対処すればよい。

また、例えば、ロボットが軽量に構成されているなどの理由から、モータの停止時の入力電流オフセット量が小さい場合、またトルク補償量演算部のゲインを大きくしてもトルク補償量が図４（ａ）に示すように大きく持続振動しない場合、モータ停止時の入力電流のオフセットの影響に対処する必要はない。

Claims (2)

1. 複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットの制御装置であって、
   対応するモータの回転角度または回転角速度を検出する複数のセンサと、
   各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
   状態観測器が推定した対応するバネ要素のねじれ角度およびねじれ角速度に基づいて、対応するバネ要素のねじれが解消するトルクをモータが出力するように、対応するモータの制御入力に加算するトルク補償量を算出する複数のトルク補償量算出手段と、
   トルク補償量算出手段が算出したトルク補償量を加算した制御入力を対応するモータに出力する複数のモータ制御手段とを有し、
   状態観測器はフィードバックゲイン行列を含み、
   フィードバックゲイン行列が、各モータの制御入力から複数のバネ要素それぞれのねじれ角度に至るモータ数×バネ要素数個の伝達関数と、各モータの制御入力から複数のバネ要素それぞれのねじれ角速度に至るモータ数×バネ要素数個の伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とするロボットの制御装置。
2. リンクと、リンクを駆動するモータとを備えたロボットの制御装置であって、
   モータの回転角度または回転角速度を検出するセンサと、
   リンクとモータとの間のバネ要素のねじれ角度およびねじれ角速度を、モータへの制御入力とセンサが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
   状態観測器が推定したバネ要素のねじれ角度およびねじれ角速度に基づいて、バネ要素のねじれが解消するトルクをモータが出力するように、モータの制御入力に加算するトルク補償量を算出するトルク補償量算出手段と、
   トルク補償量算出手段が算出したトルク補償量を加算した制御入力をモータに出力するモータ制御手段とを有し、
   状態観測器はフィードバックゲイン行列を含み、
   フィードバックゲイン行列が、モータの制御入力からバネ要素のねじれ角度に至る伝達関数と、モータの制御入力からバネ要素のねじれ角速度に至る伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とするロボットの制御装置。

Images