ロボットの制御装置
【課題】ロボットのリンクの振動を十分に抑制する。
【解決手段】複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットの制御装置であって、対応するモータの回転角度または回転角速度を検出する複数のセンサと、各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、状態観測器が推定した対応するバネ要素のねじれ角度およびねじれ角速度に基づいて、対応するバネ要素のねじれが解消するトルクをモータが出力するように、対応するモータの制御入力に加算するトルク補償量を算出する複数のトルク補償量算出手段と、トルク補償量算出手段が算出したトルク補償量を加算した制御入力を対応するモータに出力する複数のモータ制御手段とを有する。
【解決手段】複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットの制御装置であって、対応するモータの回転角度または回転角速度を検出する複数のセンサと、各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、状態観測器が推定した対応するバネ要素のねじれ角度およびねじれ角速度に基づいて、対応するバネ要素のねじれが解消するトルクをモータが出力するように、対応するモータの制御入力に加算するトルク補償量を算出する複数のトルク補償量算出手段と、トルク補償量算出手段が算出したトルク補償量を加算した制御入力を対応するモータに出力する複数のモータ制御手段とを有する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの制御装置、特に振動を抑制しながらロボットを制御することができるロボットの制御装置に関する。
【背景技術】
【０００２】
従来より、ロボットが種々の分野で使用されている。このようなロボットは、複数のリンクと複数のモータとを有し、各モータが対応するリンクを回転させることに動作する。
【０００３】
ところが、近年の高速化および高精度化により、リンクの加減速時に該リンクに振動が発生するということが問題になっている。これは、モータとリンクとの間のバネ要素にねじれが生じることによる。
【０００４】
また、ロボットの動作中、特に高速動作中、各リンクの動作は他のリンクの動作の影響（干渉）を受ける。したがって、リンクの振動を抑制するためには、各モータと対応するリンクとの間のバネ要素それぞれのねじれを考慮するとともに、複数のリンク間の干渉も考慮しなければならない。
【０００５】
この問題に対処する方法が特許文献１に記載されている。特許文献１に記載する方法では、複数のモータそれぞれと対応するリンクとの間のバネ要素それぞれについて、状態観測器が設けられる。各状態観測器は、バネ要素に対応するモータの制御入力と該モータの回転角度（角度位置）とに基づき、バネ要素のねじれ角度を推定する。次に、各状態観測器が推定した対応するバネ要素のねじれ角度に基づいて、複数のリンク間の干渉（干渉力）を算出する。そして、算出した干渉力に基づいて、各バネ要素のねじれがゼロになるようなトルクを対応するモータが出力できるように、各モータの制御入力が補償される。これにより、バネ要素のねじれは解消され、リンクの振動が抑制される。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特許第３６１１１４７号
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかし、上述の特許文献１に記載する方法では、リンクの振動を十分に抑制することができない可能性がある。説明すると、特許文献１の方法では、複数の状態観測器それぞれが推定するバネ要素のねじれ角度は、対応するモータの制御入力と該モータの回転角度のみに基づいている。すなわち、バネ要素のねじれ角度を推定するにあたり、リンク間の干渉を考慮していない。
【０００８】
具体的に説明すると、例えばモータとリンクがＮ個ずつある場合、図５に示すように、特許文献１の方法は、１つのモータと対応する１つのリンクから構成される２慣性モデル（Ｎ個の２慣性モデル）に基づいている。そのため、１つのモータと対応する１つのリンクとの間のバネ要素のねじれ角度を推定するにあたり、当該リンクと他のリンクとの間の干渉を考慮していない。したがって、特許文献１の方法が推定するバネ要素のねじれ角度は、推定精度に欠ける。その結果、リンクの振動が十分に抑制されない可能性がある。
【０００９】
そこで、本発明は、複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットにおいて、リンクとモータとの間のバネ要素のねじれを原因として発生するリンクの振動を十分に抑制することを課題とする。
【課題を解決するための手段】
【００１０】
上述の課題を解決するために、本発明の請求項１に係る発明は、
複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットの制御装置であって、
対応するモータの回転角度または回転角速度を検出する複数のセンサと、
各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定した対応するバネ要素のねじれ角度およびねじれ角速度に基づいて、対応するバネ要素のねじれが解消するトルクをモータが出力するように、対応するモータの制御入力に加算するトルク補償量を算出する複数のトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力を対応するモータに出力する複数のモータ制御手段とを有することを特徴とする。
【００１１】
また、本発明の請求項２に係る発明は、
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、各モータの制御入力から複数のバネ要素それぞれのねじれ角度に至るモータ数×バネ要素数個の伝達関数と、各モータの制御入力から複数のバネ要素それぞれのねじれ角速度に至るモータ数×バネ要素数個の伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とする。
【００１２】
さらに、本発明の請求項３に係る発明は、
リンクと、リンクを駆動するモータとを備えたロボットの制御装置であって、
モータの回転角度または回転角速度を検出するセンサと、
リンクとモータとの間のバネ要素のねじれ角度およびねじれ角速度を、モータへの制御入力とセンサが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定したバネ要素のねじれ角度およびねじれ角速度に基づいて、バネ要素のねじれが解消するトルクをモータが出力するように、モータの制御入力に加算するトルク補償量を算出するトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力をモータに出力するモータ制御手段とを有し、
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、モータの制御入力からバネ要素のねじれ角度に至る伝達関数と、モータの制御入力からバネ要素のねじれ角速度に至る伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とする。
【発明の効果】
【００１３】
本発明によれば、複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットにおいて、状態観測器は、各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する。そのため、各バネ要素のねじれ角度およびねじれ角速度を、これらを対応するモータへの制御入力と該モータの回転角度または回転角速度のみで推定する場合に比べて、高精度に推定することができる。すなわち、複数のリンク間の干渉を考慮して推定することができる。これにより、状態観測器が高精度に推定した各バネ要素のねじれ角度およびねじれ角速度に基づいて各ばね要素のねじれを解消でき、リンクの振動を十分に抑制することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態に係るロボットの一例の慣性モデルを示す図である。
【図２】図１の慣性モデルに対応するロボットの制御系を示す図である。
【図３】４慣性モデルと２慣性モデルの共振周波数を示す図である。
【図４】極配置した場合と原点に零点を配置した場合の、モータ停止時のトルク補償量の変化を示す図である。
【図５】従来方法で用いられた２慣性モデルを説明するための図である。
【発明を実施するための形態】
【００１５】
本発明の一実施形態に係るロボットの制御装置について説明する。本発明は複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットに関するものであるが、理解を容易にするために、本明細書では、主に、２つのリンクと２つのモータを備えたロボットについて説明する。したがって、以下の説明は、本発明を２つのリンクと２つのモータを備えたロボットに限定するわけではない。
【００１６】
図１は、本発明の実施形態に係るロボットのモデルを示している。図に示すモデルは４慣性モデルであって、モータとアームとの間のバネ要素の剛性および減衰を考慮したモデルである。また、図５とは異なり、モータが、自身が駆動するリンク以外のリンクに取付けられていることを考慮したモデル、すなわち複数のリンク間の干渉を考慮したモデルである。なお、本明細書で言う「バネ要素」は、広義にはモータとリンクとを連結するものであって、モータシャフトやモータからリンクにトルクを伝達する減速機などのトルク伝達機構も含まれる。
【００１７】
図１の４慣性モデルは、第１および第２モータと、モータによって駆動される第１および第２リンクと、モータとリンクとの間の第１および第２バネ要素とから構成されている。
【００１８】
図１において、角度θＭ１，θＭ２は、第１および第２モータの回転角度（角度位置）である。角度θＬ１，θＬ２は、第１および第２リンクの角度である。第１および第２バネ要素のねじれ角度は、θＭ１−θＬ１、θＭ２−θＬ２で与えられる。また、ＫＳ１，ＫＳ２は第１および第２バネ要素の剛性であって、ＤＳ１，ＤＳ２は第１および第２バネ要素の減衰である。さらに、ｕ１，ｕ２は、第１および第２モータの出力トルクである。
【００１９】
図１に示す４慣性モデルの運動方程式は、遠心力・コリオリ力などの非線形な要素を無視すると、数式１〜４に示すように表現できる。
【数１】
【数２】
【数３】
【数４】
【００２０】
数式１〜４において、ＪＬ１１，ＪＬ１２，ＪＬ２１，およびＪＬ２２は、リンクの質量、重心位置、長さなどのロボットパラメータとリンクの角度θＬ１，θＬ２から求まる係数である。また、ＪＭ１，ＪＭ２は、第１および第２モータの慣性である。さらに、
と
は、第１および第２バネ要素のねじれ角速度である。
【００２１】
この４つの運動方程式から数式５の次の状態方程式が得られる。
【数５】
【００２２】
上述の４慣性モデルを適用した状態観測器を含む第１および第２モータの制御系、すなわち本発明に係るロボットの制御装置の制御系の一部のブロック図を図２に示す。
【００２３】
図２に示す状態観測器は、数式５の状態方程式と数式６の出力方程式に基づく観測器、すなわち第１リンクと第２リンクとの間の干渉を考慮した観測器である。行列Ａ，Ｂ，およびＣは４慣性モデルによって決定される行列である。行列Ｋは、詳細は後述するが、状態観測器の設計パラメータであって、フィードバックゲイン行列である。「∫」は、積分器である。
【数６】
【００２４】
この状態観測器は、第１および第２モータそれぞれに対する制御入力（トルクｕ１，ｕ２に対応する入力電流）Ｉ１，Ｉ２と第１および第２モータそれぞれの回転角度θＭ１，θＭ２とに基づいて、状態ｘとして、第１バネ要素のねじれ角度θＭ１−θＬ１およびねじれ角速度
と第２バネ要素のねじれ角度θＭ２−θＬ２およびねじれ角速度
とを推定する。
【００２５】
厳密に言えば、状態観測器は、第１バネ要素のねじれ角度θＭ１−θＬ１およびねじれ角速度
を、第１モータの入力電流Ｉ１および第２モータの入力電流Ｉ２と、第１モータの回転角度θＭ１および第２のモータの回転角度θＭ２とから推定する。また、第２バネ要素のねじれ角度θＭ２−θＬ２およびねじれ角速度
を、第１モータの入力電流Ｉ１および第２モータの入力電流Ｉ２と、第１モータの回転角度θＭ１および第２のモータの回転角度θＭ２とから推定する。
【００２６】
そのため、この状態観測器は、第１バネ要素のねじれ角度θＭ１−θＬ１およびねじれ角速度
を、これらを第１モータの入力電流Ｉ１と第１モータの回転角度θＭ１のみから推定する場合に比べて、高精度に推定することができる。また、第２バネ要素のねじれ角度θＭ２−θＬ２およびねじれ角速度
を、これらを第２モータの入力電流Ｉ２と第２モータの回転角度θＭ２のみから推定する場合に比べて、高精度に推定することができる。
【００２７】
なお、第１および第２モータそれぞれの回転角度θＭ１，θＭ２の代りに、回転角速度
であってもよい。すなわち、回転角度または回転角速度のうち、実際に観測可能な方、例えばセンサ（エンコーダ）によって検出可能な方に基づいて状態観測器は状態ｘを推定する。
【００２８】
図２に示す第１および第２トルク補償量演算部は、状態観測器が推定した状態ｘに基づいて、数式７，８を用いて、トルク補償量Ｔｕ１，Ｔｕ２を算出する。
【数７】
【数８】
【００２９】
数式７，８において、Ｆ１，Ｆ２は、トルク補償量Ｔｕ１，Ｔｕ２を演算するためのゲイン行列である。具体的には、ゲイン行列Ｆ１，Ｆ２は、状態観測器が推定した状態ｘ（ねじれ角度とねじれ角速度）から、第１および第２バネ要素のねじれを解消できるトルクを第１および第２モータが出力するように、第１および第２モータの入力電流Ｉ１，Ｉ２に加算するトルク補償量Ｔｕ１，Ｔｕ２を算出するための行列である。
【００３０】
第１トルク補償量演算部が算出したトルク補償量Ｔｕ１は、第１リンク位置／姿勢制御部から出力された第１モータへの入力電流Ｉ１に加算される。第１リンク位置／姿勢制御部は、第１リンクの位置と姿勢とを制御するために、第１モータを制御する、ロボットの制御装置の構成要素である。
【００３１】
また、第２トルク補償量演算部が算出したトルク補償量Ｔｕ２は、第２リンク位置／姿勢制御部から出力された第２モータへの入力電流Ｉ２に加算される。第２リンク位置／姿勢制御部は、第２リンクの位置と姿勢とを制御するために、第２モータを制御する、ロボットの制御装置の構成要素である。
【００３２】
このような状態観測器と第１および第２トルク補償量演算部により、第１および第２バネ要素は、第１リンクと第２リンクとの間の干渉を考慮して、そのねじれが解消される。その結果、第１および第２リンクの振動が十分に抑制される。
【００３３】
また、別の効果として、第１リンクと第２リンクの間の干渉を考慮するため（すなわち４慣性モデルであるため）、複数の共振周波数を求めることが可能である。
【００３４】
具体的に説明する。図３（ａ）は、図１に示す４慣性モデルにおける、第１モータへの入力電流から該モータの回転角速度の推定値までのゲイン特性（実施例）を示す図である。一方、図３（ｂ）は、２慣性モデルにおける、モータへの入力電流から該モータの回転角速度の推定値までのゲイン特性（比較例）を示す図である。
【００３５】
図３（ａ）に示すように、４慣性モデルの場合、すなわち第１リンクと第２リンクとの間の干渉を考慮した場合、第１バネ要素および第２バネ要素それぞれについて、その挙動において２つの共振周波数を持つ。一方、図３（ｂ）に示すように、２慣性モデルの場合、モータとリンクとの間のバネ要素は、その挙動において１つの共振周波数を持つ。
【００３６】
したがって、モータとリンクとがそれぞれ複数あるにもかかわらず、２慣性モデルに基づく場合は、現実に存在する複数の共振周波数に対して考慮できず、各バネ要素について１つの共振周波数しか考慮できない。これにより、非考慮の共振周波数によるスピルオーバの問題が起こる。一方、リンク間の干渉を考慮すれば、４慣性モデルが各バネ要素について２つの共振周波数が考慮できるように、モータとリンクとがＮ個ずつある２Ｎ慣性モデルの場合、Ｎ個の共振周波数を考慮することができる。
【００３７】
また、本実施形態の状態観測器は、トルク補償量演算部のゲインを大きく設定することができるように、すなわち、それによって第１リンクや第２リンクの振動をさらに十分に抑制することができるように構成されている。
【００３８】
このことについて具体的に説明する。
【００３９】
図２に示す状態観測器の構成は、該状態観測器の状態ｘの推定値を
とすると、数式９のように表現することができる。
【数９】
【００４０】
状態観測器が推定した状態
と実際の状態ｘとの誤差をｅ
とすると、数式９から数式１０が得られる。
【数１０】
【００４１】
誤差ｅの収束はフィードバックゲイン行列Ｋに依存し、フィードバックゲイン行列Ｋは、一般的には、極配置によって数式１０の（Ａ−ＫＣ）の極の実部が負となるように設計されている。この極の実部の値により、誤差ｅの収束度合いが決定される。
【００４２】
ところが、フィードバックゲイン行列Ｋを、極配置によって数式１０の（Ａ−ＫＣ）の極の実部が負になるように設計すると、モータの停止時に第１リンクや第２リンクに持続振動が発生する。
【００４３】
この持続振動の発生は、モータの特性を原因とする。通常、モータは非線形な静止摩擦があるため、その入力電流ゼロ付近で不感帯が存在し、モータ停止時に入力電流のオフセットが発生する。また、リンクには重力が作用するので該リンクを一定の姿勢で維持するためのトルクをモータは出力する必要があり、そのためにも入力電流のオフセットが発生する。
【００４４】
このようなオフセットが発生する入力電流を推定に使用すると、極配置によってフィードバックゲイン行列Ｋが設計されている状態観測器は、モータの停止時、実際の値との誤差が大きいねじれ角度やねじれ角速度を推定する。すなわち、モータの停止時、状態観測器の推定精度が低下する。
【００４５】
モータの停止時に状態観測器の推定精度が低下すると、トルク補償量演算部が出力するトルク補償量は、図４（ａ）に示すように、モータの停止時、持続振動する。なお、図４（ａ）は、数式１０に示す（Ａ−ＫＣ）の極の実部全てが（−１５０）になるように、フィードバックゲイン行列Ｋを設計した場合を示している。
【００４６】
図４（ａ）に示すようにトルク補償量が持続振動することを考えると、当然ながらトルク補償量演算部のゲインを大きく設定することはできない（ゲインを大きくすると、リンクが大きく振動する）。
【００４７】
したがって、トルク補償量演算部のゲインは、モータ停止時の入力電流のオフセットを原因とする、モータの停止時における状態観測器の推定精度の低下を考慮して、小さく設定される。
【００４８】
そこで、本実施形態の状態観測器では、モータ停止時に入力電流のオフセットが発生しても、モータ停止時において状態観測器の推定精度が低下しないように、すなわちトルク補償量演算部のゲインを大きく設定できるように、フィードバックゲイン行列Ｋが、各モータの入力電流から複数のバネ要素それぞれのねじれ角度およびねじれ角速度に至る複数の伝達関数全てが原点に零点を持つように設計されている。
【００４９】
具体的には、図１の４慣性モデルに対応する図２の状態観測器の場合、第１モータの入力電流Ｉ１から第１バネ要素のねじれ角度θＭ１−θＬ１に至る伝達関数、入力電流Ｉ１から第１バネ要素のねじれ角速度
に至る伝達関数、入力電流Ｉ１から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、入力電流Ｉ１から第２バネ要素のねじれ角速度
に至る伝達関数、第２モータの入力電流Ｉ２から第１バネ要素のねじれ角度θＭ１−θＬ1に至る伝達関数、入力電流Ｉ２から第１バネ要素のねじれ角速度
に至る伝達関数、入力電流Ｉ２から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、および入力電流Ｉ２から第２バネ要素のねじれ角速度
に至る伝達関数の全てが原点に零点を持つように、フィードバックゲイン行列Ｋが設計されている。言い換えると、８つの伝達関数全てにおいて、分子のｓ０項（ラプラス演算子ｓの０乗の項）の係数がゼロになるように、フィードバックゲイン行列Ｋが設計されている。
【００５０】
このようなフィードバックゲイン行列Ｋの設計について具体的に説明する。なお、ここでは、説明を簡単にするために、１つのリンクと該リンクを駆動する１つのモータからなる２慣性モデルを例に挙げて説明する。
【００５１】
２慣性モデルの場合、状態方程式は、モータの回転角度をθＭ、リンクの角度をθＬ、
モータの慣性をＪＭ、リンクの慣性をＪＬ、モータとリンクとの間のバネ要素の剛性をＫＳ、バネ要素の減衰をＤＳとすると、数式１１のようになる。
【数１１】
【００５２】
ここで、フィードバックゲイン行列Ｋを
とし、モータの回転角度が観測可能（すなわち、
）とすると、モータの入力電流からバネ要素のねじれ角度θＭ−θＬの推定値に至る伝達関数は、数式１２のようになる。また、モータの入力電流からバネ要素のねじれ角速度
に至る伝達関数は、数式１３のようになる。なお、数式１２および１３における係数ａ０〜ａ４は、数式１４のようになる。
【数１２】
【数１３】
【数１４】
【００５３】
数式１２，１３に示す２つの伝達関数が原点に零点を持つためには、数式１２の分子のｓ０項の係数Ｋ２ＤＳ−（Ｋ３−Ｋ４）ＪＬと、数式１３の分子のＳ０項の係数Ｋ２ＫＳの両方がゼロになるような値に、フィードバックゲイン行列Ｋの要素Ｋ１〜Ｋ４を決定しなければならない。
【００５４】
ここで、例えば、フィードバックゲイン行列Ｋを数式１５に示すように与える。
【数１５】
【００５５】
数式１５において、α，βは調整パラメータである。フィードバックゲイン行例Ｋを数式１５のように設計すれば、数式１２，１３に示す２つの伝達関数の分子のｓ０項の係数がゼロになり、２つの伝達関数は原点に零点を持つ。なお、モータの回転角度ではなく回転角速度が観測可能な場合（すなわち、
）、調整パラメータβをゼロにすれば、同様に、２つの伝達関数は原点に零点を持つことができる。
【００５６】
数式１２，１３の伝達関数が原点に零点を持てば、図４（ｂ）に示すように、モータ停止時における、トルク補償量の持続振動の発生が抑制される。
【００５７】
これは、モータの入力電流からねじれ角度の推定値やねじれ角速度の推定値までの間の特性に微分が存在するため、低周波数域でのゲインが低下し、一定の入力値に対してはゲインがゼロになることによる。そのため、状態観測器は入力電流のオフセットの影響を受けにくくなり、その推定精度が低下しにくくなる。その結果、モータ停止時において、トルク補償量に持続振動が発生しにくくなり、トルク補償量演算部のゲインを大きく設定することができる。
【００５８】
当然ながら、モータの入力電流からバネ要素のねじれ角度およびねじれ角速度に至る複数の伝達関数が原点に零点を持つことにより、トルク補償量演算部のゲインを大きく設定することができることは、２慣性モデルに限らない。
【００５９】
例えば、図１の４慣性モデルにおいて第１モータが停止している場合、すなわち３慣性モデルの場合、状態方程式は数式１６のようになる。
【数１６】
【００６０】
ここで、フィードバックゲイン行列Ｋを
とし、モータの回転角度が観測可能とすると、第２モータの入力電流から第１バネ要素のねじれ角度θＭ１−θＬ１に至る伝達関数の分子のｓ０項の係数は数式１７のように、第２モータの入力電流から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数の分子のｓ０項の係数は数式１８のように、第２モータの入力電流から第１バネ要素のねじれ角速度
に至る伝達関数の分子のｓ０項の係数は数式１９のように、第２モータの入力電流から第２バネ要素のねじれ角速度
に至る伝達関数の分子のｓ０項の係数は数式２０のようになる。
【数１７】
【数１８】
【数１９】
【数２０】
【００６１】
ここで、例えば、フィードバックゲイン行列Ｋを数式２１に示すように与える。
【数２１】
【００６２】
数式２１において、α，βは調整パラメータである。フィードバックゲイン行例Ｋを数式２１のように設計すれば、第２モータの入力電流から第１バネ要素のねじれ角度θＭ１−θＬ１に至る伝達関数、第２モータの入力電流から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、第２モータの入力電流から第１バネ要素のねじれ角速度
に至る伝達関数、および第２モータの入力電流から第２バネ要素のねじれ角速度
に至る伝達関数は、分子のｓ０項の係数がゼロになり、４つの伝達関数は原点に零点を持つ。
【００６３】
さらに、図１に示す４慣性モデルにおいても、バックゲイン行列Ｋを数式２２のように設計すれば、第１モータの入力電流Ｉ１から第１バネ要素のねじれ角度θＭ１−θＬ１に至る伝達関数、入力電流Ｉ１から第１バネ要素のねじれ角速度
に至る伝達関数、入力電流Ｉ１から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、入力電流Ｉ１から第２バネ要素のねじれ角速度
に至る伝達関数、第２モータの入力電流Ｉ２から第１バネ要素のねじれ角度θＭ１−θＬ1に至る伝達関数、入力電流Ｉ２から第１バネ要素のねじれ角速度
に至る伝達関数、入力電流Ｉ２から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、および入力電流Ｉ２から第２バネ要素のねじれ角速度
に至る伝達関数の全てが原点に零点を持つ。
【数２２】
【００６４】
数式１５，２１，２２に示すように、慣性モデルに関わらず、トルク補償量演算部のゲインを大きく設定することができる、フィードバックゲイン行列Ｋは、
の形で表現することができる。したがって、複数モータのうち停止中のモータを特定することにより慣性モデルを決定し、決定した慣性モデルに基づく行列Ｂ、Ｃからフィードバックゲイン行列Ｋを算出することができる。
【００６５】
本実施形態によれば、複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットにおいて、状態観測器は、各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する。そのため、各バネ要素のねじれ角度およびねじれ角速度を、これらを対応するモータへの制御入力と該モータの回転角度または回転角速度のみで推定する場合に比べて、高精度に推定することができる。すなわち、複数のリンク間の干渉を考慮して推定することができる。これにより、状態観測器が高精度に推定した各バネ要素のねじれ角度およびねじれ角速度に基づいて各バネ要素のねじれを解消でき、リンクの振動を十分に抑制することができる。
【００６６】
また、本実施形態によれば、状態観測器に含まれるフィードバックゲイン行列Ｋが、各モータの制御入力から複数のバネ要素それぞれのねじれ角度に至るモータ数×バネ要素数個の伝達関数と、各モータの制御入力から複数のバネ要素それぞれのねじれ角速度に至るモータ数×バネ要素数個の伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計される。これにより、モータ停止時に入力電流のオフセットが発生しても、モータ停止時おける状態観測器の推定精度の低下が抑制される。そのため、状態観測器が推定した状態をモータの制御入力に大きくフィードバックすることができる（トルク補償量演算部のゲインを大きく設定することができる）。そして、その結果、リンクの振動をさらに十分に抑制することができる。
【００６７】
以上、上述の実施形態を挙げて、本発明を説明してきた。しかし、本発明は、上述の実施形態に限定されない。
【００６８】
例えば、上述の実施形態の場合、複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットにおいて、リンク間の干渉による影響と、モータ停止時の入力電流のオフセットの影響の両方に対処して状態観測器を構成することにより、リンクの振動を十分に抑制している。しかしながら、本発明は、この２つの影響の両方に状態観測器が対処することに限定しない。
【００６９】
例えば、ロボットが１つのリンクと該リンクを駆動する１つのモータのみで構成される場合、当然ながらリンク間の干渉の影響を考慮する必要はない。また、機構的にリンク間の干渉が起こらない場合も、リンク間の干渉を考慮しなくてもよい。このような場合、状態観測器において、フィードバックゲイン行列Ｋを、各モータへの制御入力から複数のバネ要素それぞれのねじれ角度およびねじれ角速度に至る複数の伝達関数が原点に零点を持つように設計して、モータ停止時の入力電流のオフセットの影響に対処すればよい。
【００７０】
また、例えば、ロボットが軽量に構成されているなどの理由から、モータの停止時の入力電流オフセット量が小さい場合、またトルク補償量演算部のゲインを大きくしてもトルク補償量が図４（ａ）に示すように大きく持続振動しない場合、モータ停止時の入力電流のオフセットの影響に対処する必要はない。
【技術分野】
【０００１】
本発明は、ロボットの制御装置、特に振動を抑制しながらロボットを制御することができるロボットの制御装置に関する。
【背景技術】
【０００２】
従来より、ロボットが種々の分野で使用されている。このようなロボットは、複数のリンクと複数のモータとを有し、各モータが対応するリンクを回転させることに動作する。
【０００３】
ところが、近年の高速化および高精度化により、リンクの加減速時に該リンクに振動が発生するということが問題になっている。これは、モータとリンクとの間のバネ要素にねじれが生じることによる。
【０００４】
また、ロボットの動作中、特に高速動作中、各リンクの動作は他のリンクの動作の影響（干渉）を受ける。したがって、リンクの振動を抑制するためには、各モータと対応するリンクとの間のバネ要素それぞれのねじれを考慮するとともに、複数のリンク間の干渉も考慮しなければならない。
【０００５】
この問題に対処する方法が特許文献１に記載されている。特許文献１に記載する方法では、複数のモータそれぞれと対応するリンクとの間のバネ要素それぞれについて、状態観測器が設けられる。各状態観測器は、バネ要素に対応するモータの制御入力と該モータの回転角度（角度位置）とに基づき、バネ要素のねじれ角度を推定する。次に、各状態観測器が推定した対応するバネ要素のねじれ角度に基づいて、複数のリンク間の干渉（干渉力）を算出する。そして、算出した干渉力に基づいて、各バネ要素のねじれがゼロになるようなトルクを対応するモータが出力できるように、各モータの制御入力が補償される。これにより、バネ要素のねじれは解消され、リンクの振動が抑制される。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特許第３６１１１４７号
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかし、上述の特許文献１に記載する方法では、リンクの振動を十分に抑制することができない可能性がある。説明すると、特許文献１の方法では、複数の状態観測器それぞれが推定するバネ要素のねじれ角度は、対応するモータの制御入力と該モータの回転角度のみに基づいている。すなわち、バネ要素のねじれ角度を推定するにあたり、リンク間の干渉を考慮していない。
【０００８】
具体的に説明すると、例えばモータとリンクがＮ個ずつある場合、図５に示すように、特許文献１の方法は、１つのモータと対応する１つのリンクから構成される２慣性モデル（Ｎ個の２慣性モデル）に基づいている。そのため、１つのモータと対応する１つのリンクとの間のバネ要素のねじれ角度を推定するにあたり、当該リンクと他のリンクとの間の干渉を考慮していない。したがって、特許文献１の方法が推定するバネ要素のねじれ角度は、推定精度に欠ける。その結果、リンクの振動が十分に抑制されない可能性がある。
【０００９】
そこで、本発明は、複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットにおいて、リンクとモータとの間のバネ要素のねじれを原因として発生するリンクの振動を十分に抑制することを課題とする。
【課題を解決するための手段】
【００１０】
上述の課題を解決するために、本発明の請求項１に係る発明は、
複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットの制御装置であって、
対応するモータの回転角度または回転角速度を検出する複数のセンサと、
各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定した対応するバネ要素のねじれ角度およびねじれ角速度に基づいて、対応するバネ要素のねじれが解消するトルクをモータが出力するように、対応するモータの制御入力に加算するトルク補償量を算出する複数のトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力を対応するモータに出力する複数のモータ制御手段とを有することを特徴とする。
【００１１】
また、本発明の請求項２に係る発明は、
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、各モータの制御入力から複数のバネ要素それぞれのねじれ角度に至るモータ数×バネ要素数個の伝達関数と、各モータの制御入力から複数のバネ要素それぞれのねじれ角速度に至るモータ数×バネ要素数個の伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とする。
【００１２】
さらに、本発明の請求項３に係る発明は、
リンクと、リンクを駆動するモータとを備えたロボットの制御装置であって、
モータの回転角度または回転角速度を検出するセンサと、
リンクとモータとの間のバネ要素のねじれ角度およびねじれ角速度を、モータへの制御入力とセンサが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定したバネ要素のねじれ角度およびねじれ角速度に基づいて、バネ要素のねじれが解消するトルクをモータが出力するように、モータの制御入力に加算するトルク補償量を算出するトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力をモータに出力するモータ制御手段とを有し、
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、モータの制御入力からバネ要素のねじれ角度に至る伝達関数と、モータの制御入力からバネ要素のねじれ角速度に至る伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とする。
【発明の効果】
【００１３】
本発明によれば、複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットにおいて、状態観測器は、各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する。そのため、各バネ要素のねじれ角度およびねじれ角速度を、これらを対応するモータへの制御入力と該モータの回転角度または回転角速度のみで推定する場合に比べて、高精度に推定することができる。すなわち、複数のリンク間の干渉を考慮して推定することができる。これにより、状態観測器が高精度に推定した各バネ要素のねじれ角度およびねじれ角速度に基づいて各ばね要素のねじれを解消でき、リンクの振動を十分に抑制することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態に係るロボットの一例の慣性モデルを示す図である。
【図２】図１の慣性モデルに対応するロボットの制御系を示す図である。
【図３】４慣性モデルと２慣性モデルの共振周波数を示す図である。
【図４】極配置した場合と原点に零点を配置した場合の、モータ停止時のトルク補償量の変化を示す図である。
【図５】従来方法で用いられた２慣性モデルを説明するための図である。
【発明を実施するための形態】
【００１５】
本発明の一実施形態に係るロボットの制御装置について説明する。本発明は複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットに関するものであるが、理解を容易にするために、本明細書では、主に、２つのリンクと２つのモータを備えたロボットについて説明する。したがって、以下の説明は、本発明を２つのリンクと２つのモータを備えたロボットに限定するわけではない。
【００１６】
図１は、本発明の実施形態に係るロボットのモデルを示している。図に示すモデルは４慣性モデルであって、モータとアームとの間のバネ要素の剛性および減衰を考慮したモデルである。また、図５とは異なり、モータが、自身が駆動するリンク以外のリンクに取付けられていることを考慮したモデル、すなわち複数のリンク間の干渉を考慮したモデルである。なお、本明細書で言う「バネ要素」は、広義にはモータとリンクとを連結するものであって、モータシャフトやモータからリンクにトルクを伝達する減速機などのトルク伝達機構も含まれる。
【００１７】
図１の４慣性モデルは、第１および第２モータと、モータによって駆動される第１および第２リンクと、モータとリンクとの間の第１および第２バネ要素とから構成されている。
【００１８】
図１において、角度θＭ１，θＭ２は、第１および第２モータの回転角度（角度位置）である。角度θＬ１，θＬ２は、第１および第２リンクの角度である。第１および第２バネ要素のねじれ角度は、θＭ１−θＬ１、θＭ２−θＬ２で与えられる。また、ＫＳ１，ＫＳ２は第１および第２バネ要素の剛性であって、ＤＳ１，ＤＳ２は第１および第２バネ要素の減衰である。さらに、ｕ１，ｕ２は、第１および第２モータの出力トルクである。
【００１９】
図１に示す４慣性モデルの運動方程式は、遠心力・コリオリ力などの非線形な要素を無視すると、数式１〜４に示すように表現できる。
【数１】
【数２】
【数３】
【数４】
【００２０】
数式１〜４において、ＪＬ１１，ＪＬ１２，ＪＬ２１，およびＪＬ２２は、リンクの質量、重心位置、長さなどのロボットパラメータとリンクの角度θＬ１，θＬ２から求まる係数である。また、ＪＭ１，ＪＭ２は、第１および第２モータの慣性である。さらに、
と
は、第１および第２バネ要素のねじれ角速度である。
【００２１】
この４つの運動方程式から数式５の次の状態方程式が得られる。
【数５】
【００２２】
上述の４慣性モデルを適用した状態観測器を含む第１および第２モータの制御系、すなわち本発明に係るロボットの制御装置の制御系の一部のブロック図を図２に示す。
【００２３】
図２に示す状態観測器は、数式５の状態方程式と数式６の出力方程式に基づく観測器、すなわち第１リンクと第２リンクとの間の干渉を考慮した観測器である。行列Ａ，Ｂ，およびＣは４慣性モデルによって決定される行列である。行列Ｋは、詳細は後述するが、状態観測器の設計パラメータであって、フィードバックゲイン行列である。「∫」は、積分器である。
【数６】
【００２４】
この状態観測器は、第１および第２モータそれぞれに対する制御入力（トルクｕ１，ｕ２に対応する入力電流）Ｉ１，Ｉ２と第１および第２モータそれぞれの回転角度θＭ１，θＭ２とに基づいて、状態ｘとして、第１バネ要素のねじれ角度θＭ１−θＬ１およびねじれ角速度
と第２バネ要素のねじれ角度θＭ２−θＬ２およびねじれ角速度
とを推定する。
【００２５】
厳密に言えば、状態観測器は、第１バネ要素のねじれ角度θＭ１−θＬ１およびねじれ角速度
を、第１モータの入力電流Ｉ１および第２モータの入力電流Ｉ２と、第１モータの回転角度θＭ１および第２のモータの回転角度θＭ２とから推定する。また、第２バネ要素のねじれ角度θＭ２−θＬ２およびねじれ角速度
を、第１モータの入力電流Ｉ１および第２モータの入力電流Ｉ２と、第１モータの回転角度θＭ１および第２のモータの回転角度θＭ２とから推定する。
【００２６】
そのため、この状態観測器は、第１バネ要素のねじれ角度θＭ１−θＬ１およびねじれ角速度
を、これらを第１モータの入力電流Ｉ１と第１モータの回転角度θＭ１のみから推定する場合に比べて、高精度に推定することができる。また、第２バネ要素のねじれ角度θＭ２−θＬ２およびねじれ角速度
を、これらを第２モータの入力電流Ｉ２と第２モータの回転角度θＭ２のみから推定する場合に比べて、高精度に推定することができる。
【００２７】
なお、第１および第２モータそれぞれの回転角度θＭ１，θＭ２の代りに、回転角速度
であってもよい。すなわち、回転角度または回転角速度のうち、実際に観測可能な方、例えばセンサ（エンコーダ）によって検出可能な方に基づいて状態観測器は状態ｘを推定する。
【００２８】
図２に示す第１および第２トルク補償量演算部は、状態観測器が推定した状態ｘに基づいて、数式７，８を用いて、トルク補償量Ｔｕ１，Ｔｕ２を算出する。
【数７】
【数８】
【００２９】
数式７，８において、Ｆ１，Ｆ２は、トルク補償量Ｔｕ１，Ｔｕ２を演算するためのゲイン行列である。具体的には、ゲイン行列Ｆ１，Ｆ２は、状態観測器が推定した状態ｘ（ねじれ角度とねじれ角速度）から、第１および第２バネ要素のねじれを解消できるトルクを第１および第２モータが出力するように、第１および第２モータの入力電流Ｉ１，Ｉ２に加算するトルク補償量Ｔｕ１，Ｔｕ２を算出するための行列である。
【００３０】
第１トルク補償量演算部が算出したトルク補償量Ｔｕ１は、第１リンク位置／姿勢制御部から出力された第１モータへの入力電流Ｉ１に加算される。第１リンク位置／姿勢制御部は、第１リンクの位置と姿勢とを制御するために、第１モータを制御する、ロボットの制御装置の構成要素である。
【００３１】
また、第２トルク補償量演算部が算出したトルク補償量Ｔｕ２は、第２リンク位置／姿勢制御部から出力された第２モータへの入力電流Ｉ２に加算される。第２リンク位置／姿勢制御部は、第２リンクの位置と姿勢とを制御するために、第２モータを制御する、ロボットの制御装置の構成要素である。
【００３２】
このような状態観測器と第１および第２トルク補償量演算部により、第１および第２バネ要素は、第１リンクと第２リンクとの間の干渉を考慮して、そのねじれが解消される。その結果、第１および第２リンクの振動が十分に抑制される。
【００３３】
また、別の効果として、第１リンクと第２リンクの間の干渉を考慮するため（すなわち４慣性モデルであるため）、複数の共振周波数を求めることが可能である。
【００３４】
具体的に説明する。図３（ａ）は、図１に示す４慣性モデルにおける、第１モータへの入力電流から該モータの回転角速度の推定値までのゲイン特性（実施例）を示す図である。一方、図３（ｂ）は、２慣性モデルにおける、モータへの入力電流から該モータの回転角速度の推定値までのゲイン特性（比較例）を示す図である。
【００３５】
図３（ａ）に示すように、４慣性モデルの場合、すなわち第１リンクと第２リンクとの間の干渉を考慮した場合、第１バネ要素および第２バネ要素それぞれについて、その挙動において２つの共振周波数を持つ。一方、図３（ｂ）に示すように、２慣性モデルの場合、モータとリンクとの間のバネ要素は、その挙動において１つの共振周波数を持つ。
【００３６】
したがって、モータとリンクとがそれぞれ複数あるにもかかわらず、２慣性モデルに基づく場合は、現実に存在する複数の共振周波数に対して考慮できず、各バネ要素について１つの共振周波数しか考慮できない。これにより、非考慮の共振周波数によるスピルオーバの問題が起こる。一方、リンク間の干渉を考慮すれば、４慣性モデルが各バネ要素について２つの共振周波数が考慮できるように、モータとリンクとがＮ個ずつある２Ｎ慣性モデルの場合、Ｎ個の共振周波数を考慮することができる。
【００３７】
また、本実施形態の状態観測器は、トルク補償量演算部のゲインを大きく設定することができるように、すなわち、それによって第１リンクや第２リンクの振動をさらに十分に抑制することができるように構成されている。
【００３８】
このことについて具体的に説明する。
【００３９】
図２に示す状態観測器の構成は、該状態観測器の状態ｘの推定値を
とすると、数式９のように表現することができる。
【数９】
【００４０】
状態観測器が推定した状態
と実際の状態ｘとの誤差をｅ
とすると、数式９から数式１０が得られる。
【数１０】
【００４１】
誤差ｅの収束はフィードバックゲイン行列Ｋに依存し、フィードバックゲイン行列Ｋは、一般的には、極配置によって数式１０の（Ａ−ＫＣ）の極の実部が負となるように設計されている。この極の実部の値により、誤差ｅの収束度合いが決定される。
【００４２】
ところが、フィードバックゲイン行列Ｋを、極配置によって数式１０の（Ａ−ＫＣ）の極の実部が負になるように設計すると、モータの停止時に第１リンクや第２リンクに持続振動が発生する。
【００４３】
この持続振動の発生は、モータの特性を原因とする。通常、モータは非線形な静止摩擦があるため、その入力電流ゼロ付近で不感帯が存在し、モータ停止時に入力電流のオフセットが発生する。また、リンクには重力が作用するので該リンクを一定の姿勢で維持するためのトルクをモータは出力する必要があり、そのためにも入力電流のオフセットが発生する。
【００４４】
このようなオフセットが発生する入力電流を推定に使用すると、極配置によってフィードバックゲイン行列Ｋが設計されている状態観測器は、モータの停止時、実際の値との誤差が大きいねじれ角度やねじれ角速度を推定する。すなわち、モータの停止時、状態観測器の推定精度が低下する。
【００４５】
モータの停止時に状態観測器の推定精度が低下すると、トルク補償量演算部が出力するトルク補償量は、図４（ａ）に示すように、モータの停止時、持続振動する。なお、図４（ａ）は、数式１０に示す（Ａ−ＫＣ）の極の実部全てが（−１５０）になるように、フィードバックゲイン行列Ｋを設計した場合を示している。
【００４６】
図４（ａ）に示すようにトルク補償量が持続振動することを考えると、当然ながらトルク補償量演算部のゲインを大きく設定することはできない（ゲインを大きくすると、リンクが大きく振動する）。
【００４７】
したがって、トルク補償量演算部のゲインは、モータ停止時の入力電流のオフセットを原因とする、モータの停止時における状態観測器の推定精度の低下を考慮して、小さく設定される。
【００４８】
そこで、本実施形態の状態観測器では、モータ停止時に入力電流のオフセットが発生しても、モータ停止時において状態観測器の推定精度が低下しないように、すなわちトルク補償量演算部のゲインを大きく設定できるように、フィードバックゲイン行列Ｋが、各モータの入力電流から複数のバネ要素それぞれのねじれ角度およびねじれ角速度に至る複数の伝達関数全てが原点に零点を持つように設計されている。
【００４９】
具体的には、図１の４慣性モデルに対応する図２の状態観測器の場合、第１モータの入力電流Ｉ１から第１バネ要素のねじれ角度θＭ１−θＬ１に至る伝達関数、入力電流Ｉ１から第１バネ要素のねじれ角速度
に至る伝達関数、入力電流Ｉ１から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、入力電流Ｉ１から第２バネ要素のねじれ角速度
に至る伝達関数、第２モータの入力電流Ｉ２から第１バネ要素のねじれ角度θＭ１−θＬ1に至る伝達関数、入力電流Ｉ２から第１バネ要素のねじれ角速度
に至る伝達関数、入力電流Ｉ２から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、および入力電流Ｉ２から第２バネ要素のねじれ角速度
に至る伝達関数の全てが原点に零点を持つように、フィードバックゲイン行列Ｋが設計されている。言い換えると、８つの伝達関数全てにおいて、分子のｓ０項（ラプラス演算子ｓの０乗の項）の係数がゼロになるように、フィードバックゲイン行列Ｋが設計されている。
【００５０】
このようなフィードバックゲイン行列Ｋの設計について具体的に説明する。なお、ここでは、説明を簡単にするために、１つのリンクと該リンクを駆動する１つのモータからなる２慣性モデルを例に挙げて説明する。
【００５１】
２慣性モデルの場合、状態方程式は、モータの回転角度をθＭ、リンクの角度をθＬ、
モータの慣性をＪＭ、リンクの慣性をＪＬ、モータとリンクとの間のバネ要素の剛性をＫＳ、バネ要素の減衰をＤＳとすると、数式１１のようになる。
【数１１】
【００５２】
ここで、フィードバックゲイン行列Ｋを
とし、モータの回転角度が観測可能（すなわち、
）とすると、モータの入力電流からバネ要素のねじれ角度θＭ−θＬの推定値に至る伝達関数は、数式１２のようになる。また、モータの入力電流からバネ要素のねじれ角速度
に至る伝達関数は、数式１３のようになる。なお、数式１２および１３における係数ａ０〜ａ４は、数式１４のようになる。
【数１２】
【数１３】
【数１４】
【００５３】
数式１２，１３に示す２つの伝達関数が原点に零点を持つためには、数式１２の分子のｓ０項の係数Ｋ２ＤＳ−（Ｋ３−Ｋ４）ＪＬと、数式１３の分子のＳ０項の係数Ｋ２ＫＳの両方がゼロになるような値に、フィードバックゲイン行列Ｋの要素Ｋ１〜Ｋ４を決定しなければならない。
【００５４】
ここで、例えば、フィードバックゲイン行列Ｋを数式１５に示すように与える。
【数１５】
【００５５】
数式１５において、α，βは調整パラメータである。フィードバックゲイン行例Ｋを数式１５のように設計すれば、数式１２，１３に示す２つの伝達関数の分子のｓ０項の係数がゼロになり、２つの伝達関数は原点に零点を持つ。なお、モータの回転角度ではなく回転角速度が観測可能な場合（すなわち、
）、調整パラメータβをゼロにすれば、同様に、２つの伝達関数は原点に零点を持つことができる。
【００５６】
数式１２，１３の伝達関数が原点に零点を持てば、図４（ｂ）に示すように、モータ停止時における、トルク補償量の持続振動の発生が抑制される。
【００５７】
これは、モータの入力電流からねじれ角度の推定値やねじれ角速度の推定値までの間の特性に微分が存在するため、低周波数域でのゲインが低下し、一定の入力値に対してはゲインがゼロになることによる。そのため、状態観測器は入力電流のオフセットの影響を受けにくくなり、その推定精度が低下しにくくなる。その結果、モータ停止時において、トルク補償量に持続振動が発生しにくくなり、トルク補償量演算部のゲインを大きく設定することができる。
【００５８】
当然ながら、モータの入力電流からバネ要素のねじれ角度およびねじれ角速度に至る複数の伝達関数が原点に零点を持つことにより、トルク補償量演算部のゲインを大きく設定することができることは、２慣性モデルに限らない。
【００５９】
例えば、図１の４慣性モデルにおいて第１モータが停止している場合、すなわち３慣性モデルの場合、状態方程式は数式１６のようになる。
【数１６】
【００６０】
ここで、フィードバックゲイン行列Ｋを
とし、モータの回転角度が観測可能とすると、第２モータの入力電流から第１バネ要素のねじれ角度θＭ１−θＬ１に至る伝達関数の分子のｓ０項の係数は数式１７のように、第２モータの入力電流から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数の分子のｓ０項の係数は数式１８のように、第２モータの入力電流から第１バネ要素のねじれ角速度
に至る伝達関数の分子のｓ０項の係数は数式１９のように、第２モータの入力電流から第２バネ要素のねじれ角速度
に至る伝達関数の分子のｓ０項の係数は数式２０のようになる。
【数１７】
【数１８】
【数１９】
【数２０】
【００６１】
ここで、例えば、フィードバックゲイン行列Ｋを数式２１に示すように与える。
【数２１】
【００６２】
数式２１において、α，βは調整パラメータである。フィードバックゲイン行例Ｋを数式２１のように設計すれば、第２モータの入力電流から第１バネ要素のねじれ角度θＭ１−θＬ１に至る伝達関数、第２モータの入力電流から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、第２モータの入力電流から第１バネ要素のねじれ角速度
に至る伝達関数、および第２モータの入力電流から第２バネ要素のねじれ角速度
に至る伝達関数は、分子のｓ０項の係数がゼロになり、４つの伝達関数は原点に零点を持つ。
【００６３】
さらに、図１に示す４慣性モデルにおいても、バックゲイン行列Ｋを数式２２のように設計すれば、第１モータの入力電流Ｉ１から第１バネ要素のねじれ角度θＭ１−θＬ１に至る伝達関数、入力電流Ｉ１から第１バネ要素のねじれ角速度
に至る伝達関数、入力電流Ｉ１から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、入力電流Ｉ１から第２バネ要素のねじれ角速度
に至る伝達関数、第２モータの入力電流Ｉ２から第１バネ要素のねじれ角度θＭ１−θＬ1に至る伝達関数、入力電流Ｉ２から第１バネ要素のねじれ角速度
に至る伝達関数、入力電流Ｉ２から第２バネ要素のねじれ角度θＭ２−θＬ２に至る伝達関数、および入力電流Ｉ２から第２バネ要素のねじれ角速度
に至る伝達関数の全てが原点に零点を持つ。
【数２２】
【００６４】
数式１５，２１，２２に示すように、慣性モデルに関わらず、トルク補償量演算部のゲインを大きく設定することができる、フィードバックゲイン行列Ｋは、
の形で表現することができる。したがって、複数モータのうち停止中のモータを特定することにより慣性モデルを決定し、決定した慣性モデルに基づく行列Ｂ、Ｃからフィードバックゲイン行列Ｋを算出することができる。
【００６５】
本実施形態によれば、複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットにおいて、状態観測器は、各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する。そのため、各バネ要素のねじれ角度およびねじれ角速度を、これらを対応するモータへの制御入力と該モータの回転角度または回転角速度のみで推定する場合に比べて、高精度に推定することができる。すなわち、複数のリンク間の干渉を考慮して推定することができる。これにより、状態観測器が高精度に推定した各バネ要素のねじれ角度およびねじれ角速度に基づいて各バネ要素のねじれを解消でき、リンクの振動を十分に抑制することができる。
【００６６】
また、本実施形態によれば、状態観測器に含まれるフィードバックゲイン行列Ｋが、各モータの制御入力から複数のバネ要素それぞれのねじれ角度に至るモータ数×バネ要素数個の伝達関数と、各モータの制御入力から複数のバネ要素それぞれのねじれ角速度に至るモータ数×バネ要素数個の伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計される。これにより、モータ停止時に入力電流のオフセットが発生しても、モータ停止時おける状態観測器の推定精度の低下が抑制される。そのため、状態観測器が推定した状態をモータの制御入力に大きくフィードバックすることができる（トルク補償量演算部のゲインを大きく設定することができる）。そして、その結果、リンクの振動をさらに十分に抑制することができる。
【００６７】
以上、上述の実施形態を挙げて、本発明を説明してきた。しかし、本発明は、上述の実施形態に限定されない。
【００６８】
例えば、上述の実施形態の場合、複数のリンクと対応するリンクを駆動する複数のモータとを有するロボットにおいて、リンク間の干渉による影響と、モータ停止時の入力電流のオフセットの影響の両方に対処して状態観測器を構成することにより、リンクの振動を十分に抑制している。しかしながら、本発明は、この２つの影響の両方に状態観測器が対処することに限定しない。
【００６９】
例えば、ロボットが１つのリンクと該リンクを駆動する１つのモータのみで構成される場合、当然ながらリンク間の干渉の影響を考慮する必要はない。また、機構的にリンク間の干渉が起こらない場合も、リンク間の干渉を考慮しなくてもよい。このような場合、状態観測器において、フィードバックゲイン行列Ｋを、各モータへの制御入力から複数のバネ要素それぞれのねじれ角度およびねじれ角速度に至る複数の伝達関数が原点に零点を持つように設計して、モータ停止時の入力電流のオフセットの影響に対処すればよい。
【００７０】
また、例えば、ロボットが軽量に構成されているなどの理由から、モータの停止時の入力電流オフセット量が小さい場合、またトルク補償量演算部のゲインを大きくしてもトルク補償量が図４（ａ）に示すように大きく持続振動しない場合、モータ停止時の入力電流のオフセットの影響に対処する必要はない。
【特許請求の範囲】
【請求項１】
複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットの制御装置であって、
対応するモータの回転角度または回転角速度を検出する複数のセンサと、
各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定した対応するバネ要素のねじれ角度およびねじれ角速度に基づいて、対応するバネ要素のねじれが解消するトルクをモータが出力するように、対応するモータの制御入力に加算するトルク補償量を算出する複数のトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力を対応するモータに出力する複数のモータ制御手段とを有することを特徴とするロボットの制御装置。
【請求項２】
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、各モータの制御入力から複数のバネ要素それぞれのねじれ角度に至るモータ数×バネ要素数個の伝達関数と、各モータの制御入力から複数のバネ要素それぞれのねじれ角速度に至るモータ数×バネ要素数個の伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とする請求項１に記載のロボットの制御装置。
【請求項３】
リンクと、リンクを駆動するモータとを備えたロボットの制御装置であって、
モータの回転角度または回転角速度を検出するセンサと、
リンクとモータとの間のバネ要素のねじれ角度およびねじれ角速度を、モータへの制御入力とセンサが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定したバネ要素のねじれ角度およびねじれ角速度に基づいて、バネ要素のねじれが解消するトルクをモータが出力するように、モータの制御入力に加算するトルク補償量を算出するトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力をモータに出力するモータ制御手段とを有し、
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、モータの制御入力からバネ要素のねじれ角度に至る伝達関数と、モータの制御入力からバネ要素のねじれ角速度に至る伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とするロボットの制御装置。
【請求項１】
複数のリンクと、対応するリンクを駆動する複数のモータとを有するロボットの制御装置であって、
対応するモータの回転角度または回転角速度を検出する複数のセンサと、
各リンクと対応するモータとの間のバネ要素それぞれについて、そのねじれ角度およびねじれ角速度を、複数のモータそれぞれへの制御入力と複数のセンサそれぞれが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定した対応するバネ要素のねじれ角度およびねじれ角速度に基づいて、対応するバネ要素のねじれが解消するトルクをモータが出力するように、対応するモータの制御入力に加算するトルク補償量を算出する複数のトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力を対応するモータに出力する複数のモータ制御手段とを有することを特徴とするロボットの制御装置。
【請求項２】
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、各モータの制御入力から複数のバネ要素それぞれのねじれ角度に至るモータ数×バネ要素数個の伝達関数と、各モータの制御入力から複数のバネ要素それぞれのねじれ角速度に至るモータ数×バネ要素数個の伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とする請求項１に記載のロボットの制御装置。
【請求項３】
リンクと、リンクを駆動するモータとを備えたロボットの制御装置であって、
モータの回転角度または回転角速度を検出するセンサと、
リンクとモータとの間のバネ要素のねじれ角度およびねじれ角速度を、モータへの制御入力とセンサが検出した回転角度または回転角速度とに基づいて推定する状態観測器と、
状態観測器が推定したバネ要素のねじれ角度およびねじれ角速度に基づいて、バネ要素のねじれが解消するトルクをモータが出力するように、モータの制御入力に加算するトルク補償量を算出するトルク補償量算出手段と、
トルク補償量算出手段が算出したトルク補償量を加算した制御入力をモータに出力するモータ制御手段とを有し、
状態観測器はフィードバックゲイン行列を含み、
フィードバックゲイン行列が、モータの制御入力からバネ要素のねじれ角度に至る伝達関数と、モータの制御入力からバネ要素のねじれ角速度に至る伝達関数とにおいて、分子のラプラス演算子ｓの０乗項の係数がゼロになるように設計されていることを特徴とするロボットの制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１１−１３６３９１（Ｐ２０１１−１３６３９１Ａ）
【公開日】平成２３年７月１４日（２０１１．７．１４）
【国際特許分類】
【出願番号】特願２００９−２９７５３５（Ｐ２００９−２９７５３５）
【出願日】平成２１年１２月２８日（２００９．１２．２８）
【出願人】（００００００９７４）川崎重工業株式会社 (1,710)
【Ｆターム（参考）】
【公開日】平成２３年７月１４日（２０１１．７．１４）
【国際特許分類】
【出願日】平成２１年１２月２８日（２００９．１２．２８）
【出願人】（００００００９７４）川崎重工業株式会社 (1,710)
【Ｆターム（参考）】
[ Back to top ]