JP2015170208A - 制御装置、制御方法及び制御プログラム - Google Patents

Abstract

【課題】制御対象を２つの駆動手段により駆動し共振相殺を行うこと。
【解決手段】制御装置は、制御対象を駆動する駆動側に設けられ、制御対象をトルク伝達手段を介して駆動する第１駆動手段と、制御対象側に設けられ、制御対象を駆動する第２駆動手段と、第１及び第２駆動手段を駆動するためのトルク指令値を生成する指令生成手段と、指令生成手段により生成されたトルク指令値に対して、駆動側及び制御対象側の慣性係数、粘性係数及びトルク伝達手段の減速比を含む第１ゲインを乗算し、トルク指令値に対して、駆動側及び制御対象側の慣性係数、粘性係数、及びトルク伝達手段の減速比を含む第２ゲインを乗算し、第１及び第２駆動手段に対するトルク指令値の比率を調整することで共振を相殺する共振相殺手段と、を備える。
【選択図】図６

Description

本発明は、制御対象を２つの駆動手段により駆動制御する制御装置、制御方法及び制御プログラムに関するものである。

近年、少子高齢化が深刻化しており、人に変わり得る労働力としてのロボットの利用に注目が集まっている。ところで、例えば、ヒューマノイドロボットなどの変速機構は低剛性であるために共振が低周波数に現れる。このため、制御帯域が上げられず、それ以上の運動性能の向上が困難となっている。したがって、そのようなロボットなどに共振抑制制御を導入することが重要となっている。

これに対し、駆動手段側及び制御対象側の情報を夫々検出する複数の検出手段を備え、各検出手段により検出された情報に基づいて駆動手段を駆動して共振を相殺する自己共振相殺制御を行う制御装置が知られている（例えば、非特許文献１参照）。

青木元伸、藤本博志、堀洋一、高橋太郎著、ｎ慣性系の自己共振相殺制御と自己共振相殺外乱オブザーバを用いたヒューマノイドロボットのロバスト共振抑制制御

しかしながら、上記制御装置は、制御対象を１つの駆動手段により駆動し共振相殺を行うものであり、制御対象を２つの駆動手段により駆動し共振相殺を行うものではない。

本発明は、このような問題点を解決するためになされたものであり、制御対象を２つの駆動手段により駆動し共振相殺を行うことができる制御装置、制御方法及び制御プログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、制御対象を駆動する駆動側に設けられ、該制御対象をトルク伝達手段を介して駆動する第１駆動手段と、前記制御対象側に設けられ、該制御対象を駆動する第２駆動手段と、前記第１及び第２駆動手段を駆動するためのトルク指令値を生成する指令生成手段と、前記指令生成手段により生成されたトルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数及び前記トルク伝達手段の減速比を含む第１ゲインを乗算し、前記トルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数、及び前記トルク伝達手段の減速比を含む第２ゲインを乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺する共振相殺手段と、を備える、ことを特徴とする制御装置である。
この一態様において、前記共振相殺手段は、前記指令生成手段により生成された前記トルク指令値に対して第１ゲイン（α＋β／ｓ）、および、第２ゲイン（γ＋δ／ｓ）を夫々乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺してもよい。
この一態様において、前記共振相殺手段は、前記トルク指令生成手段により生成されたトルク指令値から高周波成分を抽出する第１フィルタと、前記トルク指令生成手段により生成されたトルク指令値から低周波成分を抽出する第２フィルタと、を有しており、前記第１フィルタにより抽出された高周波のトルク指令値に対して、第１及び第２ゲインを夫々乗算し、該トルク指令値を調整することで共振を相殺して、前記第１及び第２駆動手段を制御し、前記第２フィルタにより抽出された低周波のトルク指令値に基づいて、前記第１駆動手段を制御してもよい。
この一態様において、前記共振相殺手段は、前記トルク指令生成手段により生成されたトルク指令値から高周波成分を抽出する第１フィルタと、前記トルク指令生成手段により生成されたトルク指令値から低周波成分を抽出する第２フィルタと、を有しており、前記第１フィルタにより抽出された高周波のトルク指令値に対して、第１ゲイン（α_Ｖ＋β_Ｖ／ｓ）、および、第２ゲイン（γ_Ｖ＋δ_Ｖ／ｓ）を夫々乗算し、該トルク指令値を調整することで共振を相殺して、前記第１及び第２駆動手段を制御すると共に、前記第２フィルタにより抽出された低周波のトルク指令値をも用いて、前記第１駆動手段を制御し、パラメータα_Ｖ（０≦α_Ｖ≦１）を調整し、該調整したパラメータα_Ｖに基づいてパラメータβ_Ｖ、γ_Ｖ、及びδ_Ｖを下記式を用いて設定してもよい。
この一態様において、前記第１フィルタは、ハイパスフィルタ（１−Ｆ（ｓ））であり、
前記第２フィルタは、ローパスフィルタ（Ｆ（ｓ））であってもよい。
この一態様において、前記共振相殺手段は、パラメータα_Ｖを設定後、該設定されたパラメータα_Ｖに基づいて前記ハイパスフィルタ及びローパスフィルタのカットオフ周波数を調整し設定してもよい。
この一態様において、前記制御対象は、ロボットの関節部であってもよい。
上記目的を達成するための本発明の一態様は、制御対象を駆動する駆動側に設けられ、該制御対象をトルク伝達手段を介して駆動する第１駆動手段と、前記制御対象側に設けられ、該制御対象を駆動する第２駆動手段と、を備える制御装置の制御方法であって、 前記第１及び第２駆動手段を駆動するためのトルク指令値を生成するステップと、前記生成されたトルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数及び前記トルク伝達手段の減速比を含む第１ゲインを乗算し、前記トルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数、及び前記トルク伝達手段の減速比を含む第２ゲインを乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺するステップと、を含む、ことを特徴とする制御装置の制御方法であってもよい。
この一態様において、前記生成された前記トルク指令値に対して第１ゲイン（α＋β／ｓ）、および、第２ゲイン（γ＋δ／ｓ）を夫々乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺してもよい。
この一態様において、前記生成されたトルク指令値から高周波成分を第１フィルタにより抽出するステップと、前記生成されたトルク指令値から低周波成分を第２フィルタにより抽出するステップと、を更に含み、 前記抽出された高周波のトルク指令値に対して、第１及び第２ゲインを夫々乗算し、該トルク指令値を調整することで共振を相殺して、前記第１及び第２駆動手段を制御すると共に、前記抽出された低周波のトルク指令値をも用いて、前記第１駆動手段を制御してもよい。
この一態様において、前記生成されたトルク指令値から高周波成分を第１フィルタにより抽出するステップと、前記生成されたトルク指令値から低周波成分を第２フィルタにより抽出するステップと、を更に含み、
前記抽出された高周波のトルク指令値に対して、第１ゲイン（α_Ｖ＋β_Ｖ／ｓ）、および、第２ゲイン（γ_Ｖ＋δ_Ｖ／ｓ）を夫々乗算し、該トルク指令値を調整することで共振を相殺して、前記第１及び第２駆動手段を制御し、前記抽出された低周波のトルク指令値に基づいて、前記第１駆動手段を制御し、パラメータα_Ｖ（０≦α_Ｖ≦１）を調整し、該調整したパラメータα_Ｖに基づいてパラメータβ_Ｖ、γ_Ｖ、及びδ_Ｖを下記式を用いて設定してもよい。
この一態様において、前記第１フィルタは、ハイパスフィルタ（１−Ｆ（ｓ））であり、
前記第２フィルタは、ローパスフィルタ（Ｆ（ｓ））であってもよい。
この一態様において、前記パラメータα_Ｖを設定後、該設定されたパラメータα_Ｖに基づいて前記ハイパスフィルタ及びローパスフィルタのカットオフ周波数を調整し設定してもよい。
この一態様において、前記制御対象は、ロボットの関節部であってもよい。
上記目的を達成するための本発明の一態様は、制御対象を駆動する駆動側に設けられ、該制御対象をトルク伝達手段を介して駆動する第１駆動手段と、前記制御対象側に設けられ、該制御対象を駆動する第２駆動手段と、を備える制御装置の制御プログラムであって、前記第１及び第２駆動手段を駆動するためのトルク指令値を生成する処理と、前記生成されたトルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数及び前記トルク伝達手段の減速比を含む第１ゲインを乗算し、前記トルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数、及び前記トルク伝達手段の減速比を含む第２ゲインを乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺する処理と、をコンピュータに実行させる、ことを特徴とする制御装置の制御プログラムであってもよい。

本発明によれば、制御対象を２つの駆動手段により駆動し共振相殺を行うことができる制御装置、制御方法及び制御プログラムを提供することができる。

本発明の実施の形態１に係る制御装置が制御するロボットの一例を示す図である。 各関節部の構成を示す図である。 本発明の実施の形態１に係る２慣性系の制御装置をモデル化した図である。 ２慣性系の制御装置を示すブロック線図である。 本発明の実施の形態１に係る制御装置のシステム構成を示すブロック図である。 本発明の実施の形態１に係る制御装置の共振相殺部のブロック線図である。 本発明の実施の形態２に係る制御装置の共振相殺部のブロック線図である。 伝達関数Ｐ_ＬＭ（ｓ）、Ｐ_{Ｌ、ＡＲＣ}（ｓ）、Ｐ_{ＦＳ−ＡＲＣ}（ｃ）のボーデ線図である。 ｆ_ＬＰＦを０（Ｈｚ）に固定しα_Ｖを０から１に変化させたときの、ナイキスト線図である。 ｆ_ＬＰＦを０に固定しα_Ｖを０から∞に変化させたときの、ナイキスト線図である。である。 ３つの条件を設定した場合のナイキスト線図である。 実験装置の一例を示す図である。 周波数特性の測定結果、及び、その測定結果による２慣性系モデルのボード線図である。 図１３の周波数特性に基づいて決定したプラントのノミナル値の一例を示す図である。 実験装置を用いて測定したナイキスト線図である。 負荷側モータの回転角度のステップ応答波形を示す図である。 ステップ応答におけるＣａｓｅ１及び２の負荷側モータのトルク波形を示す図である。 負荷側モータの回転角度の外乱応答波形を示す図である。

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。
図１は、本発明の実施の形態１に係る制御装置が制御するロボットの一例を示す図である。本実施の形態１に係る制御装置は、例えば、３つの関節部を有する脚式ロボット１００の駆動を制御する。脚式ロボット１００は、股関節１０１、膝関節１０２及び足首関節１０３を有している。図２に示すように、各関節部１０１〜１０３には、モータが設けられている。各モータは、例えば、ハーモニックギア１０４及びタイミングベルト１０５を介して各関節部１０１〜１０３に係合している。

本実施の形態１に係る制御装置は、制御対象である上記関節部１０１〜１０３を駆動制御する。制御装置は、例えば、タイミングベルト１０５などのバネ要素の特性を考慮して２慣性系としてモデル化されている。図３は、本実施の形態１に係る２慣性系の制御装置をモデル化した図である。

本実施の形態１に係る制御装置１は、回転センサ２と、駆動側モータ３と、負荷側モータ４と、バネ要素５と、減速機６と、制御装置７と、を備えている。
回転センサ２は、例えば、負荷（関節部）８あるいは負荷側モータ４に設けれたポテンショメータ、ロータリーエンコーダなどであり、負荷８の回転情報（回転角度、回転角速度、回転角加速度）を検出する。回転センサ２は、検出した負荷８の回転情報を制御装置７に出力する。

駆動側モータ３は、第１駆動手段の一具体例であり、負荷８を駆動する駆動側に設けられている。駆動側モータ３は、バネ要素５及び減速機６（トルク伝達手段の一具体例）を介して負荷８に係合している。駆動側モータ３は、バネ要素５及び減速機６を介して負荷８を駆動する。

負荷側モータ４は、第２駆動手段の一具体例であり、負荷側（制御対象側）に設けられている。負荷側モータ４は、減速機６に連結されている。負荷８は、減速機６を介して直接的に負荷側モータ４に連結されている。このように、駆動側モータ３のトルクは、減速機６を介して減速されて負荷８に伝達される。一方、負荷側モータ４のトルクは、減速機６によって減速されることなく直接的に負荷８に伝達される。

なお、図３において、Ｔはトルク、ωは角速度、θは回転角度、Ｊは慣性モーメント、Ｂは粘性摩擦係数、Ｋはバネ要素のバネ定数、ｒは減速機６の減速比を示している。各パラメータＴ、ω、θ、Ｊ、Ｂに付された添え字Ｍは、駆動側を示し、添え字Ｌは負荷側を示している。なお、本実施の形態において、減速機６が減速を行わない構成（ｒ＝１）であってもよい。

制御装置７は、例えば、回転センサ２から回転情報に基づいて、駆動側モータ３及び負荷側モータ４をフィードバック制御する。制御装置７は、例えば、演算処理、制御処理等と行うＣＰＵ（Central Processing Unit）７ａ、ＣＰＵ７ａによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ７ｂ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）７ｃ、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ７ａ、メモリ７ｂ、及びインターフェイス部７ｃは、データバスなどを介して相互に接続されている。

ここで、一般的な２慣性系の制御装置について説明する。図４は、２慣性系の制御装置を示すブロック線図である。図４に示す制御装置の伝達関数を下記（１）乃至（５）式で表すことができる。

但し、駆動側トルクＴ_Ｍからモータ回転角度θ_Ｍまでの伝達関数をＰ_ＭＭ、駆動側トルクＴ_Ｍから負荷回転角度θ_Ｌまでの伝達関数をＰ_ＬＭ、負荷側トルクＴ_Ｌからモータ回転角度θ_Ｍまでの伝達関数をＰ_ＭＬ、負荷側トルクＴ_Ｌから負荷回転角度θ_Ｌまでの伝達関数をＰ_ＬＬとする。

上記式より、２慣性系の制御装置には、分母多項式より下記（６）式に示す共振角周波数ω_ｐと、分子多項式より下記（７）式に示す反共振角周波数ω_ｚが存在し、共振が生じる。

ところで、例えば、ヒューマノイドロボットなどの変速機構は低剛性であるために共振が低周波数に現れる。このため、制御帯域が上げられず、それ以上の運動性能の向上が困難となっている。したがって、そのようなロボットなどに共振抑制制御を導入することが重要となっている。

これに対し、本出願人は、特願２０１３−０３４７０６において、１つのアクチュエータと複数のセンサを用いた１入力多出力の制御装置（ＳＩＭＯ：Single Input Multiple Output）による共振相殺制御（ＳＲＣ：Self Resonance Cancellation）を提案している。

これに双対となる制御系として、本実施の形態１に係る制御装置１は、多入力１出力（ＭＩＳＯ：Multiple Input Single Output）であり、複数のアクチュエータによる共振相殺制御（ＡＲＣ：Actuation Resonance Cancellation）を行う。これにより、ＳＲＣと同様に、振動抑制制御及びモデル化誤差に対してロバストな制御が実現できる。

本実施の形態１に係る制御装置１は、駆動側及び負荷側モータ３、４に対するトルク指令値に、駆動側及び負荷側の慣性係数、粘性係数及び減速比を含む第１ゲインを乗算し、トルク指令値に、駆動側及び負荷側の慣性係数、粘性係数、及び減速比を含む第２ゲインを乗算する。これにより、駆動側及び負荷側モータ３、４に対するトルク指令値の比率を調整し、バネ要素５によって生じる軸捩れ共振を相殺することができる。
通常、バネ要素５によって軸捩れ共振が生じると、上述の如く、その共振周波数以上に制御帯域を上げることができない。一方で、本実施の形態１に係る制御装置１によれば、軸捩れ共振を相殺することで制御帯域を上げることができ、制御性能を向上させることができる。

本実施の形態１に係る制御装置１は、図３に示す如く、駆動側モータ３に加えて、２慣性系の負荷側に負荷側モータ４を搭載し、共振相殺制御を行う。図５は、本実施の形態１に係る制御装置のシステム構成を示すブロック図である。本実施の形態１に係る制御装置７は、トルク指令値を生成する指令値生成部７１と、共振相殺制御を行う共振相殺部７２と、を有している。

指令値生成部７１は、指令生成手段の一具体例であり、回転センサ２からの回転情報に基づいて、駆動側モータ３及び負荷側モータ４を駆動するためのトルク指令値を生成する。指令値生成部７１は、生成したトルク指令値を入力トルクＴ_ｉｎとして共振相殺部７２に出力する。

共振相殺部７２は、共振相殺手段の一具体例であり、駆動側及び負荷側モータ３、４に対するトルク指令値に、慣性係数、粘性係数及び減速比を含む第１ゲイン（α＋β／ｓ）を乗算し、駆動側及び負荷側モータ３、４に対するトルク指令値に、慣性係数、粘性係数、及び減速比を含む第２ゲイン（γ＋δ／ｓ）を乗算することで、駆動側及び負荷側モータ３、４に対するトルク指令値の比率を調整し、共振を相殺する。

図６は、本実施の形態１に係る制御装置の共振相殺部のブロック線図である。
共振相殺部７２は、指令値生成部７１からの入力トルクＴ_ｉｎに第１ゲイン（α＋β／ｓ）を乗算し、負荷側トルクＴ_Ｌを算出する。
制御対象のモデルにおいては、負荷側モータ４の回転角度θ_Ｌに減速比ｒを乗算した値と、駆動側モータ３の回転角度θ_Ｍとの偏差が算出される。該算出した偏差にバネ定数Ｋ及び減速比ｒが乗算され、該算出した乗算値が、上記共振相殺部７２により算出された負荷側トルクＴ_Ｌに加算される。該加算値に１次遅れの伝達関数（１／（Ｊ_Ｌｓ＋Ｂ_Ｌ））が乗算され、積分処理が行われることで、負荷側モータ４の回転角度θ_Ｌが算出される。

同様に、共振相殺部７２は、指令値生成部７１からの入力トルクＴ_ｉｎに第２ゲイン（γ＋δ／ｓ）を乗算し、駆動側トルクＴ_Ｍを算出する。
制御対象のモデルにおいては、負荷側モータ４の回転角度θ_Ｌに減速比ｒを乗算した値と、駆動側モータ３の回転角度θ_Ｍとの偏差が算出される。該算出した偏差にバネ定数Ｋが乗算され、該算出した乗算値が、上記共振相殺部７２により算出された負荷側トルクＴ_Ｌから減算される。該減算値に１次遅れの伝達関数（１／（Ｊ_Ｍｓ＋Ｂ_Ｍ））が乗算され、積分処理が行われることで、駆動側モータ３の回転角度θ_Ｍが算出される。

図６に示す制御系の伝達関数を下記（８）式及び（９）式で表すことができる。
但し、入力トルクＴ_ｉｎから負荷側モータ４の回転角度θ_Ｌまでの伝達関数をＰ_Ｌ（ｓ）とし、入力トルクＴ_ｉｎから駆動側モータ３の回転角度θ_Ｍまでの伝達関数をＰ_Ｍ（ｓ）とする。

上記伝達関数Ｐ_Ｌ（ｓ）、Ｐ_Ｍ（ｓ）において共振項が消えるように係数α、β、γ、δを設定する。係数α、β、γ、δをバネ定数Ｋを用いずに下記（１０）乃至（１３）式に示すように設定できる。

上記式において、αは駆動側から見たときの見掛けの慣性比であり、０から１の値をとる。
上記（１０）乃至（１３）式に示すように係数α、β、γ、δを設定することで、入力トルクＴ_ｉｎから負荷側モータ４の回転角度θ_Ｌまでの伝達関数をＰ_Ｌ（ｓ）、および、入力トルクＴ_ｉｎから駆動側モータ３の回転角度θ_Ｍまでの伝達関数をＰ_Ｍ（ｓ）は、夫々、下記（１４）及び（１５）式で表すことができる。したがって、これら伝達関数Ｐ_Ｌ（ｓ）、Ｐ_Ｍ（ｓ）に共振項が存在しなくなる。

上述したように、共振相殺部７２は、トルク指令値に対して、上記（１０）式乃至（１３）式により設定されたα、β、γ、δに基づいた第１ゲイン（α＋β／ｓ）及び第２ゲイン（γ＋δ／ｓ）を夫々乗算することで、駆動側及び負荷側モータ３、４に対するトルク指令値の比率を調整し共振を相殺できる。

なお、本実施の形態１において、負荷側モータ４は、負荷８を直接的に駆動している。すなわち、負荷８を負荷側モータ４で直接的に駆動し減速機６による減速を必要とする構成にも、あるいは、負荷８を負荷側モータ４で直接的に駆動し減速機６による減速を必要としない構成にも適用できる。また、係数α、β、γ、δには、バネ定数Ｋが含まれていない。このため、本実施の形態１に係る制御装置１は、バネ定数Ｋに対してロバスト性を有している。

以上、本実施の形態１に係る制御装置１において、駆動側及び負荷側モータ３、４に対するトルク指令値に、駆動側及び負荷側の慣性係数、粘性係数及び減速比を含む第１ゲインを乗算し、トルク指令値に、駆動側及び負荷側の慣性係数、粘性係数、及び減速比を含む第２ゲインを乗算する。これにより、駆動側及び負荷側モータ３、４に対するトルク指令値の比率を調整し、共振を相殺することができる。すなわち、制御対象を２つのアクチュエータにより駆動し共振相殺を行うことができる。

実施の形態２
本発明の実施の形態２に係る制御装置は、駆動周波数を分離した共振相殺制御（ＦＳ−ＡＲＣ：Frequency Separation Actuation Resonance Cancellation）を行う。図７は、本実施の形態２に係る制御装置の共振相殺部を示すブロック線図である。本実施の形態２に係る共振相殺部２０は、上記実施の形態１に係る共振相殺部７２の構成に、ローパスフィルタ２１及びハイパスフィルタ２２を更に備える。

共振相殺部２０は、ハイパスフィルタ（１−Ｆ（ｓ））２２を用いて、指令値生成部７１からの入力トルクＴ’_ｉｎから高周波成分を抽出する。共振相殺部２０は、ハイパスフィルタ２２により抽出された高周波の入力トルクＴ_ｉｎに第１ゲイン（α＋β／ｓ）及び第２ゲイン（γ＋δ／ｓ）を夫々乗算し、負荷側トルクＴ_Ｌ及び駆動側トルクＴ_Ｍを算出することで、共振を相殺しつつ負荷側及び駆動側モータを制御する。

共振相殺部２０は、ローパスフィルタ（Ｆ（ｓ））２１を用いて、指令値生成部７１からの入力トルクＴ’_ｉｎから低周波成分を抽出する。共振相殺部２０は、ローパスフィルタ２１により抽出された低周波の入力トルクに基づいて駆動側トルクＴ_Ｍを算出することで、駆動側モータ３を制御する。

図７において、Ｆ（ｓ）はカットオフ周波数ｆ_ＬＰＦ［Ｈｚ］の１次ローパスフィルタであり、下記（１６）式で表すことができる。

なお、上記ローパスフィルタＦ（ｓ）及びハイパスフィルタ（１−Ｆ（ｓ））は一例であり、これに限らない。上記ローパスフィルタＦ（ｓ）の代わりに、例えば、２次のバターワースフィルタなど用いてもよく、上記ローパスフィルタＦ（ｓ）及びハイパスフィルタ（１−Ｆ（ｓ））と同様にトルク指令値から低周波成分及び高周波成分を抽出する機能を有する任意のフィルタを用いることができる。

入力トルクＴ’_ｉｎから負荷側モータ４の回転角度θ_Ｌまでの伝達関数Ｐ_{ＦＳ-ＡＲＣ}（ｓ）を下記（１７）式で表すことができる。

図７に示すようにローパスフィルタ２１及びハイパスフィルタ２２を構成する。これにより、共振の存在しない低周波領域では、ローパスフィルタ２１を介して駆動側モータ３のモータトルクにより負荷８を駆動する。一方、共振の存在する比較的高い周波数領域では、ハイパスフィルタ２２を介して駆動側モータ３及び負荷側モータ４のモータトルクにより共振相殺制御を行いつつ、負荷８を駆動する。

本実施の形態２において、駆動側モータ３は比較的低い周波数の入力トルクによって駆動し、負荷側モータ４は比較的高い周波数の入力トルクによって駆動する。比較的高い周波数成分のトルクは振動を励起する可能性があるトルクであり、比較的低い周波数成分は振動を励起する可能性が低いトルクであるため、負荷側モータ４は振動を励起する可能性があるトルクのみに対して駆動するため、負荷側モータ４に定格出力の小さいモータを用いることができる。例えば、負荷側モータ４の出力を小さく抑えることで、大型モータを設ける必要がないため、ロボット自体の小型化に繋がる。
なお、本実施の形態２において、上記実施の形態１と同一部分には同一符号を付して詳細な説明は省略する。

実施の形態３
上記実施の形態１に係る共振相殺制御（以下、ＡＲＣ）では、共振を相殺し、入力トルクから負荷側の回転角度までの共振モードを不可観測化している。これに対し、上記実施の形態２に係る共振相殺制御（以下、ＦＳ−ＡＲＣ）において、共振を完全に相殺しないことで、共振モードを可観測化し、外乱による振動を抑制することも可能である。これにより、システムに外乱が入力された場合でも、予期しない振動が発生するのを防止できる。

本実施の形態３においては、上記（１７）式に用いるαを、０から１までの間で変化する可変制御パラメータα_Ｖ（０≦α_Ｖ≦１）とする。これにより、入力から応答および外乱応答の振動抑制効果を調整できる。可変制御パラメータα_Ｖを用いて、β_Ｖ、γ_Ｖ、δ_Ｖを下記（１８）式乃至（２０）式のように表すことができる。

なお、可変制御パラメータα_Ｖ＝α及びカットオフ周波数ｆ_ＬＰＦ＝０（Ｈｚ）としたとき、本実施の形態３に係るＦＳ−ＡＲＣは上記実施の形態１に係るＡＲＣと等価となる。
図８は、伝達関数Ｐ_ＬＭ（ｓ）、Ｐ_{Ｌ、ＡＲＣ}（ｓ）、Ｐ_{ＦＳ−ＡＲＣ}（ｃ）のボーデ線図である。ＦＳ−ＡＲＣ（ｓ）において、ローパスフィルタＦ（ｓ）のカットオフ周波数ｆ_ＬＦＰは、例えば、共振の５５（Ｈｚ）より小さい５（Ｈｚ）に設定されている。

図８に示す如く、ＡＲＣを適用することで２慣性系の共振が存在しなくなっていることが分かる。駆動周波数を分離したＦＳ−ＡＲＣにおいても、共振は略存在しないことが分かる。従がって、ＡＲＣ及びＦＳ−ＡＲＣを適用することで共振を消去できることが分かる。さらに、ＡＲＣ及びＦＳ−ＡＲＣの最終位相は、−１８０（ｄｅｇ）であり、制御し易いシステムと言える。

次に、図７に示すＦＳ−ＡＲＣのシステムに対して、フィードバック制御系を構築する。フィードバックコントローラＣ（ｓ）は、剛体化されたＰ_{Ｌ、ＡＲＣ}（ｓ）に対して設計する。フィードバックコントローラＣ（ｓ）は、感度関数Ｓ_ＡＲＣ（ｓ）＝（１＋Ｐ_{Ｌ、ＡＲＣ}（ｓ）Ｃ（ｓ））^−１が安定となるように設計されている。

フィードバックコントローラＣ（ｓ）は、例えば、ＰＩＤコントローラとし、極配置により設計されている。この設計に際して、感度関数Ｓ_ＡＲＣ（ｓ）の極を２０Ｈｚに４重根配置している。本実施の形態３に係るＦＳ−ＡＲＣは、剛体モードに対してコントローラを設計するればよいので、容易にコントローラのパラメータを決定できる。

次に、本実施の形態３に係るＦＳ−ＡＲＣの制御系の安定性について解析を行う。コントローラのパラメータである、可変制御パラメータα_Ｖ、カットオフ周波数ｆ_ＬＰＦを変化させたときのＦＳ−ＡＲＣの制御系の安定性について解析を行う。

図９は、カットオフ周波数ｆ_ＬＰＦを０（Ｈｚ）に固定し、可変制御パラメータα_Ｖを０から１に変化させたときの、ナイキスト線図である。図１０は、カットオフ周波数ｆ_ＬＰＦを０に固定し、可変制御パラメータα_Ｖを０から∞に変化させたときの、ナイキスト線図である。図９及び図１０において、実線は可変制御パラメータα_Ｖ＝０及びカットオフ周波数ｆ_ＬＰＦ＝０（Ｈｚ）とした場合を示している。この場合、Ｐ_{ＦＳ−ＡＲＣ}（ｓ）の共振モードはＰ_ＬＬ（ｓ）の共振モードと一致している。図９において、破線は可変制御パラメータα_Ｖ＝１及びカットオフ周波数ｆ_ＬＰＦ＝０（Ｈｚ）とした場合を示している。この場合、Ｐ_{ＦＳ−ＡＲＣ}（ｓ）の共振モードは、Ｐ_ＬＭ（ｓ）の共振モードと一致している。図１０において、一点鎖線は可変制御パラメータα_Ｖ＝１及びカットオフ周波数ｆ_ＬＰＦ＝∞とした場合を示している。この場合、Ｐ_{ＦＳ−ＡＲＣ}（ｓ）の共振モードは、Ｐ_ＬＭ（ｓ）の共振モードと一致している。

以上から、図９及び図１０に示すナイキスト線図上において、Ｐ_{ＦＳ−ＡＲＣ}（ｓ）の共振モードが描く円の大きさ及び方向は、Ｐ_ＬＬ（ｓ）とＰ_ＬＭ（ｓ）の共振モード間で自由に設定できることが分かる。すなわち、共振抑制効果と共振モードの位相安定化を可変制御パラメータα_Ｖ及びカットオフ周波数ｆ_ＬＰＦという２つのパラメータを用いて容易に調整できる。

例えば、最初に振動抑制効果を、可変制御パラメータα_Ｖにより調整する。次に、カットオフ周波数ｆ_ＬＰＦを閉ループ系の位相余裕を最大にとるように選択することで、共振モードの位相安定化を図ることができる。より具体的には、共振モードを点−１＋ｊ０から可能な限り遠ざけるようにカットオフ周波数ｆ_ＬＰＦを選択する。

次に、上述したＡＲＣ及びＦＳ−ＡＲＣの周波数特性をシミュレーションにより評価する。本シミュレーションにおいて、下記３つの条件（Ｃａｓｅ１−３）を設定している。
・Ｃａｓｅ１：
ＡＲＣを適用し、可変制御パラメータα_Ｖ＝α、及びカットオフ周波数ｆ_ＬＰＦ＝０（Ｈｚ）に設定し、感度関数Ｓ_ＡＲＣ（ｓ）の極を３０（Ｈｚ）に設定する。
このＣａｓｅ１は、トルク指令値に対する応答の振動抑制に重点を置いた設計となっている。
・Ｃａｓｅ２：
ＦＳ−ＡＲＣを適用し、可変制御パラメータα_Ｖ＝α、及びカットオフ周波数ｆ_ＬＰＦ＝１．０（Ｈｚ）に設定し、感度関数Ｓ_ＡＲＣ（ｓ）の極を２５（Ｈｚ）に設定する。
このＣａｓｅ２は、負荷側モータ４の小型化に重点を置いた設計となっている。
・Ｃａｓｅ３：
ＦＳ−ＡＲＣを適用し、可変制御パラメータα_Ｖ＝０．１、及びカットオフ周波数ｆ_ＬＰＦ＝５．０（Ｈｚ）に設定し、感度関数Ｓ_ＡＲＣ（ｓ）の極を３２（Ｈｚ）に設定する。
このＣａｓｅ３は、外乱抑制性能に重点を置いた設計となっている。
なお、上記Ｃａｓｅ１の場合、位相余裕が４３．５°となることから、Ｃａｓｅ２とＣａｓｅ３の位相余裕が等しくなるように、感度関数Ｓ_ＡＲＣ（ｓ）の極とカットオフ周波数ｆ_ＬＰＦを設計している。

図１１は、上記３つの条件を設定した場合のナイキスト線図である。図１１に示す如く、Ｃａｓｅ１及び２の場合、共振が相殺されていることが分かる。一方、Ｃａｓｅ３の場合、共振モードが−１＋ｊ０から離れるように配置され、位相安定化が図られていることが分かる。

次に、ＡＲＣ及びＦＳ−ＡＲＣの周波数特性および時間応答を、実験装置を用いて評価する。図１２は実験装置の一例を示す図である。負荷側には、外乱トルクなどの負荷（関節部）を再現するためのモータ１０６が設けれている。

図１３は、膝関節部の駆動側トルクＴ_Ｍから駆動側モータの角速度θ_Ｍドットまでの周波数特性の測定結果、及び、その測定結果に基づいてモデルフィッティングを行った２慣性系モデル（図５）のボード線図である。図１４は、図１３の周波数特性に基づいて決定したプラントのノミナル値の一例を示す図である。ここで、低域のゲインにおいて非線形摩擦による誤差が生じている。

上記実験装置を用いた実験において、上記シミュレーションと同様の３つの条件（Ｃａｓｅ１−３）が設定されている。
（１）周波数応答
図１５は、実験装置を用いて測定したナイキスト線図である。図１５に示す如く、Ｃａｓｅ１及び２の場合、共振が略相殺されていることが分かる。一方、Ｃａｓｅ３の場合、共振モードを点−１＋ｊ０から遠ざけて配置されており、位相安定化が図られていることが分かる。Ｃａｓｅ１乃至３で上記シミュレーション結果と同様の傾向が見られる。

（２）時間応答
ステップ状指令及びステップ外乱の時間応答により、ＡＲＣ及びＦＳ−ＡＲＣの評価を行う。

（２−１）ステップ応答
ＡＲＣ及びＦＳ−ＡＲＣのステップ応答を評価する。図１６は、負荷側モータの回転角度のステップ応答波形を示す図である。図１６に示す如く、Ｃａｓｅ１の場合、振動が抑制されていることが分かる。Ｃａｓｅ２の場合も、振動が抑制されていることが分かる。しかし、感度関数Ｓ_ＡＲＣ（ｓ）の極を遅くしているため、オーバーシュートがＣａｓｅ１より大きくなっている。

Ｃａｓｅ３の場合も、振動が抑制されていることが分かる。なお、そのオーバーシュートはＣｓａｅ２より小さくなっているが、可変制御パラメータα_Ｖ≠αとしているため、振動が僅かに残存している。

図１７は、ステップ応答におけるＣａｓｅ１及び２の負荷側モータのトルク波形を示す図である。図１７に示す如く、Ｃａｓｅ２のトルクピークがＣａｓｅ１のトルクピークより２５％低くなっていることが分かる。従がって、Ｃａｓｅ２のようにＦＳ−ＡＲＣを適用することで負荷側モータ４を小型化できることが分かる。

（２−２）外乱応答
ＡＲＣ及びＦＳ−ＡＲＣの外乱応答を評価する。負荷側に０．０２（Ｎ・ｍ）のステップ状外乱を０．１（ｓ）与える。このとき、負荷側モータ４の回転角度θ_Ｌは０（ｒａｄ）となっている。図１８は、負荷側モータの回転角度の外乱応答波形を示す図である。図１８に示す如く、Ｃａｓｅ１及び２の応答は、外乱により振動していることが分かる。一方、可変制御パラメータα_Ｖ≠αとしたＣａｓｅ３の場合、外乱抑制効果が高く振動が抑制できていることが分かる。

以上の実験結果により、Ｃａｓｅ１は、トルク指令値に対する応答の振動抑制に重点を置いた設計、Ｃａｓｅ２は、負荷側モータ４の小型化に重点を置いた設計、Ｃａｓｅ３は、外乱抑制性能に重点を置いた設計、となっていることが確認できる。
ここで、上記（１７）式に用いるα_Ｖを、下記（ａ）及び（ｂ）のトレードオフのバランスを取ることで決定できる。
（ａ）トルク指令値に対する追従性及び振動特性を重視する。
（ｂ）外乱に対する抑圧特性（外乱抑圧特性）及び振動特性を重視する。

「外乱」とは、例えば、外部からロボットの動作を妨げるような力が作用することで、「外乱抑圧特性」とはそのような力が作用しても影響を受けないようにトルク指令値に追従し続ける性能を指す。
上記（ａ）を最重視する場合は、上記のように、可変制御パラメータα_Ｖ＝αとする。一方、上記（ｂ）を少し重視した場合、可変制御パラメータα_Ｖにより調整（０≦α_Ｖ≦１）を行う。

例えば、図１６のステップ応答波形からトルク指令値に対する追従性及び振動特性を、図１８の外乱応答波形から外乱抑圧特性及び振動特性を、考慮しつつトレードオフのバランスが最適となるα_Ｖを設定する。なお、横軸が時間を表す応答波形を示す図１６や図１８を用いてα_Ｖを設定しているが、これに限らない。例えば、負荷側外乱から出力（回転角度）までの伝達関数をプロットした応答波形や感度関数をプロットした応答波形を用いてα_Ｖを設定してもよい。
本実施の形態３において、上記実施の形態１及び２と同一部分には同一符号を付して詳細な説明は省略する。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

上記実施の形態において、制御装置１はヒューマノイドロボットの共振相殺制御（ＡＲＣ及びＦＳ−ＡＲＣ）を行っているが、これに限らない。本実施の形態に係る制御装置１は、例えば、工作機械の共振相殺制御を行ってもよい。これにより、複数のモータを用いて、大トルク、高速、かつ高精度な制御が実現できる。また、制御装置１は、半導体製造装置の共振相殺制御を行ってもよい。これにより、超精密位置決め時に問題となる機械的な弾性の影響を低減できる。さらに、制御装置１は、車両のエンジンと電気モータとのハイブリッド制御に対して共振相殺制御を行っても良い。これにより、エンジンと電気モータ間で生じる軸捩れ共振を抑えて高速かつ高精度な制御が実現できる。

また、本発明は、例えば、上記共振相殺部７２の処理を、ＣＰＵ７ａにコンピュータプログラムを実行させることにより実現することも可能である。
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１ 制御装置、２ 回転センサ、３ 駆動側モータ、４ 負荷側モータ、５ バネ要素、６ 減速機、７ 制御装置、７１ 指令値生成部、７２ 共振相殺部

Claims (15)

1. 制御対象を駆動する駆動側に設けられ、該制御対象をトルク伝達手段を介して駆動する第１駆動手段と、
   前記制御対象側に設けられ、該制御対象を駆動する第２駆動手段と、
   前記第１及び第２駆動手段を駆動するためのトルク指令値を生成する指令生成手段と、
   前記指令生成手段により生成されたトルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数及び前記トルク伝達手段の減速比を含む第１ゲインを乗算し、前記トルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数、及び前記トルク伝達手段の減速比を含む第２ゲインを乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺する共振相殺手段と、
   を備える、ことを特徴とする制御装置。
2. 請求項１記載の制御装置であって、
   前記共振相殺手段は、前記指令生成手段により生成された前記トルク指令値に対して第１ゲイン（α＋β／ｓ）、および、第２ゲイン（γ＋δ／ｓ）を夫々乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺する、ことを特徴とする制御装置。
   但し、
   Ｊ_Ｍは、前記駆動側の慣性係数であり、Ｂ_Ｍは前記駆動側の粘性係数であり、Ｊ_Ｌは、前記制御対象側の慣性係数であり、Ｂ_Ｌは前記制御対象側の粘性係数であり、ｒは前記トルク伝達手段の減速比である。
3. 請求項１又は２記載の制御装置であって、
   前記共振相殺手段は、
   前記トルク指令生成手段により生成されたトルク指令値から高周波成分を抽出する第１フィルタと、
   前記トルク指令生成手段により生成されたトルク指令値から低周波成分を抽出する第２フィルタと、
   を有しており、
   前記第１フィルタにより抽出された高周波のトルク指令値に対して、第１及び第２ゲインを夫々乗算し、該トルク指令値を調整することで共振を相殺して、前記第１及び第２駆動手段を制御すると共に、
   前記第２フィルタにより抽出された低周波のトルク指令値をも用いて、前記第１駆動手段を制御する、ことを特徴とする制御装置。
4. 請求項１記載の制御装置であって、
   前記共振相殺手段は、
   前記トルク指令生成手段により生成されたトルク指令値から高周波成分を抽出する第１フィルタと、
   前記トルク指令生成手段により生成されたトルク指令値から低周波成分を抽出する第２フィルタと、
   を有しており、
   前記第１フィルタにより抽出された高周波のトルク指令値に対して、第１ゲイン（α_Ｖ＋β_Ｖ／ｓ）、および、第２ゲイン（γ_Ｖ＋δ_Ｖ／ｓ）を夫々乗算し、該トルク指令値を調整することで共振を相殺して、前記第１及び第２駆動手段を制御し、
   前記第２フィルタにより抽出された低周波のトルク指令値に基づいて、前記第１駆動手段を制御し、
   パラメータα_Ｖ（０≦α_Ｖ≦１）を調整し、該調整したパラメータα_Ｖに基づいてパラメータβ_Ｖ、γ_Ｖ、及びδ_Ｖを下記式を用いて設定する、ことを特徴とする制御装置。
   
5. 請求項３又は４記載の制御装置であって、
   前記第１フィルタは、ハイパスフィルタ（１−Ｆ（ｓ））であり、
   前記第２フィルタは、ローパスフィルタ（Ｆ（ｓ））である、ことを特徴とする制御装置。
6. 請求項４記載の制御装置であって、
   前記共振相殺手段は、パラメータα_Ｖを設定後、該設定されたパラメータα_Ｖに基づいて前記第１及び第２フィルタのカットオフ周波数を調整し設定する、ことを特徴とする制御装置。
7. 請求項１乃至６のうちいずれか１項記載の制御装置であって、
   前記制御対象は、ロボットの関節部である、ことを特徴とする制御装置。
8. 制御対象を駆動する駆動側に設けられ、該制御対象をトルク伝達手段を介して駆動する第１駆動手段と、
   前記制御対象側に設けられ、該制御対象を駆動する第２駆動手段と、を備える制御装置の制御方法であって、
   前記第１及び第２駆動手段を駆動するためのトルク指令値を生成するステップと、
   前記生成されたトルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数及び前記トルク伝達手段の減速比を含む第１ゲインを乗算し、前記トルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数、及び前記トルク伝達手段の減速比を含む第２ゲインを乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺するステップと、
   を含む、ことを特徴とする制御装置の制御方法。
9. 請求項８記載の制御装置の制御方法であって、
   前記生成された前記トルク指令値に対して第１ゲイン（α＋β／ｓ）、および、第２ゲイン（γ＋δ／ｓ）を夫々乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺する、ことを特徴とする制御装置の制御方法。
   但し、
   Ｊ_Ｍは、前記駆動側の慣性係数であり、Ｂ_Ｍは前記駆動側の粘性係数であり、Ｊ_Ｌは、前記制御対象側の慣性係数であり、Ｂ_Ｌは前記制御対象側の粘性係数であり、ｒは前記トルク伝達手段の減速比である。
10. 請求項８又は９記載の制御装置の制御方法であって、
    前記生成されたトルク指令値から高周波成分を第１フィルタにより抽出するステップと、
    前記生成されたトルク指令値から低周波成分を第２フィルタにより抽出するステップと、
    を更に含み、
    前記抽出された高周波のトルク指令値に対して、第１及び第２ゲインを夫々乗算し、該トルク指令値を調整することで共振を相殺して、前記第１及び第２駆動手段を制御すると共に、
    前記抽出された低周波のトルク指令値をも用いて、前記第１駆動手段を制御する、ことを特徴とする制御装置の制御方法。
11. 請求項８記載の制御装置の制御方法であって、
    前記生成されたトルク指令値から高周波成分を第１フィルタにより抽出するステップと、
    前記生成されたトルク指令値から低周波成分を第２フィルタにより抽出するステップと、
    を更に含み、
    前記抽出された高周波のトルク指令値に対して、第１ゲイン（α_Ｖ＋β_Ｖ／ｓ）、および、第２ゲイン（γ_Ｖ＋δ_Ｖ／ｓ）を夫々乗算し、該トルク指令値を調整することで共振を相殺して、前記第１及び第２駆動手段を制御し、
    前記抽出された低周波のトルク指令値に基づいて、前記第１駆動手段を制御し、
    パラメータα_Ｖ（０≦α_Ｖ≦１）を調整し、該調整したパラメータα_Ｖに基づいてパラメータβ_Ｖ、γ_Ｖ、及びδ_Ｖを下記式を用いて設定する、ことを特徴とする制御装置の制御方法。
    
12. 請求項１０又は１１記載の制御装置の制御方法であって、
    前記第１フィルタは、ハイパスフィルタ（１−Ｆ（ｓ））であり、
    前記第２フィルタは、ローパスフィルタ（Ｆ（ｓ））である、ことを特徴とする制御装置の制御方法。
13. 請求項１１記載の制御装置の制御方法であって、
    前記パラメータα_Ｖを設定後、該設定されたパラメータα_Ｖに基づいて前記第１及び第２フィルタのカットオフ周波数を調整し設定する、ことを特徴とする制御装置の制御方法。
14. 請求項８乃至１３のうちいずれか１項記載の制御装置の制御方法であって、
    前記制御対象は、ロボットの関節部である、ことを特徴とする制御装置の制御方法。
15. 制御対象を駆動する駆動側に設けられ、該制御対象をトルク伝達手段を介して駆動する第１駆動手段と、
    前記制御対象側に設けられ、該制御対象を駆動する第２駆動手段と、を備える制御装置の制御プログラムであって、
    前記第１及び第２駆動手段を駆動するためのトルク指令値を生成する処理と、
    前記生成されたトルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数及び前記トルク伝達手段の減速比を含む第１ゲインを乗算し、前記トルク指令値に対して、前記駆動側及び前記制御対象側の慣性係数、粘性係数、及び前記トルク伝達手段の減速比を含む第２ゲインを乗算し、前記第１及び第２駆動手段に対する前記トルク指令値の比率を調整することで共振を相殺する処理と、
    をコンピュータに実行させる、ことを特徴とする制御装置の制御プログラム。