ホーム > 特許ランキング > 国立大学法人 東京大学
知財求人 - 知財ポータルサイト「IP Force」
▶ 国立大学法人 東京大学の特許一覧 ▶ トヨタ自動車株式会社の特許一覧
特許5947732制御システム、外乱推定システム、制御方法、制御プログラム及び設計方法
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】5947732
(24)【登録日】2016年6月10日
(45)【発行日】2016年7月6日
(54)【発明の名称】制御システム、外乱推定システム、制御方法、制御プログラム及び設計方法
(51)【国際特許分類】
G05B 13/02 20060101AFI20160623BHJP
G05D 3/12 20060101ALI20160623BHJP
【ＦＩ】
G05B13/02 C
G05D3/12 305V
【請求項の数】21
【全頁数】29
(21)【出願番号】特願2013-34706(P2013-34706)
(22)【出願日】2013年2月25日
(65)【公開番号】特開2014-164498(P2014-164498A)
(43)【公開日】2014年9月8日
【審査請求日】2015年4月9日
【新規性喪失の例外の表示】特許法第３０条第２項適用 （１）平成２４年９月４日開催の「産業計測制御研究会」にて発表 （２）平成２４年９月４日一般社団法人 電気学会発行の「電気学会研究会資料，産業計測制御研究会，ＩＩＣ−１２−１７１〜１７９」に掲載
(73)【特許権者】
【識別番号】504137912
【氏名又は名称】国立大学法人 東京大学
(73)【特許権者】
【識別番号】000003207
【氏名又は名称】トヨタ自動車株式会社
(74)【代理人】
【識別番号】100103894
【弁理士】
【氏名又は名称】家入 健
(72)【発明者】
【氏名】藤本 博志
(72)【発明者】
【氏名】青木 元伸
(72)【発明者】
【氏名】高橋 太郎
【審査官】
谷治 和文
(56)【参考文献】
【文献】
特開昭６１−０３２１２０（ＪＰ，Ａ）
【文献】
特開２００５−１７４０１１（ＪＰ，Ａ）
【文献】
特開平１１−０３１０１４（ＪＰ，Ａ）
【文献】
国際公開第２００７／１０５５２７（ＷＯ，Ａ１）
【文献】
国際公開第２０１２／１０２０６０（ＷＯ，Ａ１）
【文献】
特開２００６−１５８０２６（ＪＰ，Ａ）
(58)【調査した分野】（Int.Cl.，ＤＢ名）
Ｇ０５Ｂ １３／０２
Ｇ０５Ｄ ３／１２
(57)【特許請求の範囲】
【請求項1】
制御対象を駆動する駆動手段と、
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、
前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、前記第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺する自己共振相殺制御手段と、
を備え、
前記自己共振相殺制御手段は、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システム。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、
前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、前記第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺する自己共振相殺制御手段と、
を備え、
前記自己共振相殺制御手段は、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システム。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
【請求項2】
請求項１記載の制御システムであって、
前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、前記第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺した加算結果に基づいて、外乱推定値を算出する外乱オブザーバを更に備える、ことを特徴とする制御システム。
前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、前記第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺した加算結果に基づいて、外乱推定値を算出する外乱オブザーバを更に備える、ことを特徴とする制御システム。
【請求項3】
請求項２記載の制御システムであって、
前記自己共振相殺制御手段は、前記外乱オブザーバにより算出された外乱推定値に基づいて前記駆動手段に対するトルク指令値を生成する、ことを特徴とする制御システム。
前記自己共振相殺制御手段は、前記外乱オブザーバにより算出された外乱推定値に基づいて前記駆動手段に対するトルク指令値を生成する、ことを特徴とする制御システム。
【請求項4】
請求項２又は３記載の制御システムであって、
前記自己共振相殺制御手段は、下記式に基づいて、前記共振成分を含まない角速度を算出し、前記外乱オブザーバに出力する、ことを特徴とする制御システム。
但し、上記式において、θＭは前記第１検出手段により検出された前記角度情報、θＬは前記第２検出手段により検出された前記角度情報、である。
前記自己共振相殺制御手段は、下記式に基づいて、前記共振成分を含まない角速度を算出し、前記外乱オブザーバに出力する、ことを特徴とする制御システム。
【数1】
【請求項5】
請求項２乃至４のうちいずれか１項記載の制御システムであって、
前記外乱オブザーバは、下記式に基づいて前記外乱推定値を算出する、ことを特徴とする制御システム。
但し、上記式において、Ｑはフィルタであり、ΔＰＭＭ、ΔＰＭＬ、ΔＰＬＭ、ΔＰＬＬは、モデル化誤差である。
前記外乱オブザーバは、下記式に基づいて前記外乱推定値を算出する、ことを特徴とする制御システム。
【数2】
【請求項6】
請求項２乃至５のうちいずれか１項記載の制御システムであって、
前記制御対象の角度情報または位置情報を検出する少なくとも１つ以上の前記第２検出手段を備え、
前記自己共振相殺制御手段は、前記制御対象及び駆動手段に関する複数の慣性系に対して、夫々、運動方程式を算出し該算出した運動方程式を足し合わせて生成した共振成分を含まない式に基づいて生成したフィードバック制御器を含む、ことを特徴とする制御システム。
前記制御対象の角度情報または位置情報を検出する少なくとも１つ以上の前記第２検出手段を備え、
前記自己共振相殺制御手段は、前記制御対象及び駆動手段に関する複数の慣性系に対して、夫々、運動方程式を算出し該算出した運動方程式を足し合わせて生成した共振成分を含まない式に基づいて生成したフィードバック制御器を含む、ことを特徴とする制御システム。
【請求項7】
請求項６記載の制御システムであって、
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の前記第２検出手段により検出されフィードバックされた角度情報または位置情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対するトルク指令値を生成する、または、入力される角度指令値と第２検出手段により検出されフィードバックされた角度情報の偏差に第１制御ゲインを夫々乗算し、それらを加算した値に基づいて、前記駆動手段に対するトルク指令値を生成する、ことを特徴とする制御システム。
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の前記第２検出手段により検出されフィードバックされた角度情報または位置情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対するトルク指令値を生成する、または、入力される角度指令値と第２検出手段により検出されフィードバックされた角度情報の偏差に第１制御ゲインを夫々乗算し、それらを加算した値に基づいて、前記駆動手段に対するトルク指令値を生成する、ことを特徴とする制御システム。
【請求項8】
請求項６記載の制御システムであって、
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の前記第２検出手段により検出されフィードバックされた角度情報または位置情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対する第１トルク指令値を生成し、
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の第２検出手段により検出された角度情報または位置情報に第３制御ゲインを夫々乗算し、該各乗算値を加算した値を、前記外乱オブザーバに出力し、
前記外乱オブザーバは、前記自己共振相殺制御手段から出力された値に基づいて外乱推定値を生成する、ことを特徴とする制御システム。
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の前記第２検出手段により検出されフィードバックされた角度情報または位置情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対する第１トルク指令値を生成し、
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の第２検出手段により検出された角度情報または位置情報に第３制御ゲインを夫々乗算し、該各乗算値を加算した値を、前記外乱オブザーバに出力し、
前記外乱オブザーバは、前記自己共振相殺制御手段から出力された値に基づいて外乱推定値を生成する、ことを特徴とする制御システム。
【請求項9】
請求項８記載の制御システムであって、
前記自己共振相殺制御手段は、前記外乱オブザーバからの外乱推定値と、前記生成した第１トルク指令値と、に基づいて第２トルク指令値を生成する、ことを特徴とする制御システム。
前記自己共振相殺制御手段は、前記外乱オブザーバからの外乱推定値と、前記生成した第１トルク指令値と、に基づいて第２トルク指令値を生成する、ことを特徴とする制御システム。
【請求項10】
請求項９記載の制御システムであって、
前記自己共振相殺制御手段は、前記生成した第２トルク指令値を前記外乱オブザーバに出力し、
前記外乱オブザーバは、前記自己共振相殺制御手段からの第２トルク指令値と、前記各乗算値を加算した値と、に基づいて、前記外乱推定値を生成する、ことを特徴とする制御システム。
前記自己共振相殺制御手段は、前記生成した第２トルク指令値を前記外乱オブザーバに出力し、
前記外乱オブザーバは、前記自己共振相殺制御手段からの第２トルク指令値と、前記各乗算値を加算した値と、に基づいて、前記外乱推定値を生成する、ことを特徴とする制御システム。
【請求項11】
請求項６乃至１０のうちいずれか１項記載の制御システムであって、
下記式に示す慣性系のｎ個の運動方程式を足し合わせて前記共振成分を含まない式を生成する、ことを特徴とする制御システム。
但し、上記式において、Ｊｎはｎ番目の慣性モーメント、Ｋｎはｎ番目のギア比、Ｂｎはｎ番目の粘性減衰係数、θｎはｎ番目の角度センサにより検出される角度情報、である。
下記式に示す慣性系のｎ個の運動方程式を足し合わせて前記共振成分を含まない式を生成する、ことを特徴とする制御システム。
【数3】
【請求項12】
請求項１乃至１１のうちいずれか１項記載の制御システムであって、
前記制御対象の加速度情報を検出する加速度センサ及び／又はジャイロセンサを更に備え、
前記加速度センサ及び／又はジャイロセンサにより検出された加速度情報及び／又は角速度情報に基づいて、前記制御対象の角度情報又は位置情報を算出する、ことを特徴とする制御システム。
前記制御対象の加速度情報を検出する加速度センサ及び／又はジャイロセンサを更に備え、
前記加速度センサ及び／又はジャイロセンサにより検出された加速度情報及び／又は角速度情報に基づいて、前記制御対象の角度情報又は位置情報を算出する、ことを特徴とする制御システム。
【請求項13】
請求項１乃至１２のうちいずれか１項記載の制御システムであって、
前記制御対象は、ロボットの可動部である、ことを特徴とする制御システム。
前記制御対象は、ロボットの可動部である、ことを特徴とする制御システム。
【請求項14】
制御対象を駆動する駆動手段と、
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数を乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数を乗算し、該乗算結果を加算することで共振を相殺した加算結果に基づいて、外乱推定値を算出する外乱オブザーバと、
を備え、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする外乱推定システム。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数を乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数を乗算し、該乗算結果を加算することで共振を相殺した加算結果に基づいて、外乱推定値を算出する外乱オブザーバと、
を備え、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする外乱推定システム。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
【請求項15】
制御対象を駆動する駆動手段と、
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、
前記第１及び第２検出手段により検出された角度情報または位置情報に対して所定係数を乗算し該乗算結果を加算することで共振を相殺する自己共振相殺制御手段と、
を備える制御システムであって、
前記自己共振相殺制御手段は、前記制御対象及び駆動手段に関する複数の慣性系に対して、夫々、運動方程式を算出し該算出した運動方程式を足し合わせて生成した共振成分を含まない式に基づいて生成したフィードバック制御器を含み、
前記自己共振相殺制御手段は、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システム。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、
前記第１及び第２検出手段により検出された角度情報または位置情報に対して所定係数を乗算し該乗算結果を加算することで共振を相殺する自己共振相殺制御手段と、
を備える制御システムであって、
前記自己共振相殺制御手段は、前記制御対象及び駆動手段に関する複数の慣性系に対して、夫々、運動方程式を算出し該算出した運動方程式を足し合わせて生成した共振成分を含まない式に基づいて生成したフィードバック制御器を含み、
前記自己共振相殺制御手段は、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システム。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
【請求項16】
請求項１５記載の制御システムであって、
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の第２検出手段により検出されフィードバックされた角度情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対するトルク指令値を生成する、ことを特徴とする制御システム。
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の第２検出手段により検出されフィードバックされた角度情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対するトルク指令値を生成する、ことを特徴とする制御システム。
【請求項17】
請求項１５記載の制御システムであって、
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の第２検出手段により検出されフィードバックされた角度情報または位置情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対する第１トルク指令値を生成し、
前記自己共振相殺制御手段は、前記第１及び複数の前記第２検出手段により検出された角度情報または位置情報に第３制御ゲインを夫々乗算し、該各乗算値を加算した値を、出力し、
前記自己共振相殺制御手段から出力された値に基づいて外乱推定値を生成する外乱オブザーバを更に備える、ことを特徴とする制御システム。
前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の第２検出手段により検出されフィードバックされた角度情報または位置情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対する第１トルク指令値を生成し、
前記自己共振相殺制御手段は、前記第１及び複数の前記第２検出手段により検出された角度情報または位置情報に第３制御ゲインを夫々乗算し、該各乗算値を加算した値を、出力し、
前記自己共振相殺制御手段から出力された値に基づいて外乱推定値を生成する外乱オブザーバを更に備える、ことを特徴とする制御システム。
【請求項18】
請求項１７記載の制御システムであって、
前記自己共振相殺制御手段は、前記外乱オブザーバからの外乱推定値と、前記生成した第１トルク指令値と、に基づいて第２トルク指令値を生成する、
ことを特徴とする制御システム。
前記自己共振相殺制御手段は、前記外乱オブザーバからの外乱推定値と、前記生成した第１トルク指令値と、に基づいて第２トルク指令値を生成する、
ことを特徴とする制御システム。
【請求項19】
制御対象を駆動する駆動手段と、
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、を備える制御システムの制御方法であって、
前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺し、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システムの制御方法。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、を備える制御システムの制御方法であって、
前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺し、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システムの制御方法。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
【請求項20】
制御対象を駆動する駆動手段と、
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、を備える制御システムの制御プログラムであって、
前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺する、処理をコンピュータに実行させ、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システムの制御プログラム。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、を備える制御システムの制御プログラムであって、
前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺する、処理をコンピュータに実行させ、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システムの制御プログラム。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
【請求項21】
制御対象を駆動する駆動手段と、
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する複数の第２検出手段と、を備える制御システムの設計方法であって、
前記第１及び第２検出手段により検出された角度情報または位置情報に対して所定係数を乗算し、加算することで共振を相殺する自己共振相殺制御を行う制御システムの設計方法であって、
前記制御対象及び駆動手段に関する複数の慣性系に対して、夫々、運動方程式を算出するステップと、
該算出した運動方程式を足し合わせ、共振成分を含まない式を生成するステップと、
前記生成した式に基づいてフィードバック制御器を生成するステップと、
を含み、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システムの設計方法。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、
前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する複数の第２検出手段と、を備える制御システムの設計方法であって、
前記第１及び第２検出手段により検出された角度情報または位置情報に対して所定係数を乗算し、加算することで共振を相殺する自己共振相殺制御を行う制御システムの設計方法であって、
前記制御対象及び駆動手段に関する複数の慣性系に対して、夫々、運動方程式を算出するステップと、
該算出した運動方程式を足し合わせ、共振成分を含まない式を生成するステップと、
前記生成した式に基づいてフィードバック制御器を生成するステップと、
を含み、
前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、
前記所定係数α、β、γ、δを、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない下記式に基づいて設定する、ことを特徴とする制御システムの設計方法。
α＝ＪＭｎ
β＝ＢＭｎ
γ＝ＪＬｎ／ｎ
δ＝ＢＬｎ／ｎ
但し、上記式において、ＢＭｎは前記駆動手段の粘性減衰係数のノミナル値、ＪＭｎは前記駆動手段の慣性モーメントのノミナル値、ＪＬｎは前記制御対象の慣性モーメントのノミナル値、ＢＬｎは前記制御対象の粘性係数のノミナル値、ｎは前記制御対象内のギア比である。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、ロボットなどの制御対象を共振を抑制しつつ制御を行う制御システム、外乱推定システム、制御方法、制御プログラム及び設計方法に関するものである。
【背景技術】
【0002】
近年、少子高齢化が深刻化しており、人に変わり得る労働力としてのロボットの利用に注目が集まっている。ところで、例えば、ヒューマノイドロボットなどの変速機構は低剛性であるために共振が低周波数に現れる。このため、制御帯域が上げられず、それ以上の運動性能の向上が困難となっている。したがって、そのようなロボットなどに共振抑制制御を導入することが重要となっている。これに対し、制御対象の操作点から計測点までを剛体としたときに現れる共振を相殺する駆動装置が知られている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】国際公開公報第２０１２／１０２０６０号
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、上記特許文献１に示す駆動装置においては、上記共振相殺を行う際に、粘性抵抗を無視している。このため、例えば、摩擦力が特に小さくなるように設計された駆動装置等においては上記粘性抵抗を無視してもその誤差は小さくてすむ。一方、ロボットなどのハーモニック減速機を含む駆動装置等においては、上記のように粘性抵抗を無視して共振相殺を行うと、誤差が大きくなり、共振が残存する虞がある。
【0005】
本発明は、このような問題点を解決するためになされたものであり、制御対象に生じる共振をより確実に低減できる制御システム、外乱推定システム、制御方法、制御プログラム及び設計方法を提供することを主たる目的とする。
【課題を解決するための手段】
【0006】
上記目的を達成するための本発明の一態様は、制御対象を駆動する駆動手段と、前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、前記第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺する自己共振相殺制御手段と、を備えることを特徴とする制御システムである。この一態様において、前記自己共振相殺制御手段は、前記第１検出手段により検出された角度情報または位置情報に対して所定係数α及びβを夫々乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数γ及びδを夫々乗算し、該乗算結果を加算しており、前記所定係数α、β、γ、δは、前記制御対象における伝達関数を剛体モードと共振モードの積の形式に分離しない式に基づいて、設定してもよい。
この一態様において、前記自己共振相殺制御手段は、前記所定係数α、β、γ、δを、下記（１３）式に基づいて設定してもよい。
この一態様において、前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、前記第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺した加算結果に基づいて、外乱推定値を算出する外乱オブザーバを更に備えていてもよい。
この一態様において、前記自己共振相殺制御手段は、前記外乱オブザーバにより算出された外乱推定値に基づいて前記駆動手段に対するトルク指令値を生成してもよい。
この一態様において、前記自己共振相殺制御手段は、下記（１５）式に基づいて、前記共振成分を含まない角速度を算出し、前記外乱オブザーバに出力してもよい。
この一態様において、前記外乱オブザーバは、下記（１６）及び（１７）式に基づいて前記外乱推定値を算出してもよい。
この一態様において、前記制御対象の角度情報または位置情報を検出する少なくとも１つ以上の前記第２検出手段を備え、前記自己共振相殺制御手段は、前記制御対象及び駆動手段に関する複数の慣性系に対して、夫々、運動方程式を算出し該算出した運動方程式を足し合わせて生成した共振成分を含まない式に基づいて生成したフィードバック制御器を含んでいても良い。
この一態様において、前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の前記第２検出手段により検出されフィードバックされた角度情報または位置情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対するトルク指令値を生成する、または、入力される角度指令値と第２検出手段により検出されフィードバックされた角度情報の偏差に第１制御ゲインを夫々乗算し、それらを加算した値に基づいて、前記駆動手段に対するトルク指令値を生成してもよい。
この一態様において、前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の前記第２検出手段により検出されフィードバックされた角度情報または位置情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対する第１トルク指令値を生成し、前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の第２検出手段により検出された角度情報または位置情報に第３制御ゲインを夫々乗算し、該各乗算値を加算した値を、前記外乱オブザーバに出力し、前記外乱オブザーバは、前記自己共振相殺制御手段から出力された値に基づいて外乱推定値を生成してもよい。
この一態様において、前記自己共振相殺制御手段は、前記外乱オブザーバからの外乱推定値と、前記生成した第１トルク指令値と、に基づいて第２トルク指令値を生成してもよい。
この一態様において、前記自己共振相殺制御手段は、前記生成した第２トルク指令値を前記外乱オブザーバに出力し、前記外乱オブザーバは、前記自己共振相殺制御手段からの第２トルク指令値と、前記各乗算値を加算した値と、に基づいて、前記外乱推定値を生成してもよい。
この一態様において、下記（２９）式に示す慣性系のｎ個の運動方程式を足し合わせて前記共振成分を含まない式を生成してもよい。
この一態様において、前記制御対象の加速度情報を検出する加速度センサ及び／又はジャイロセンサを更に備え、前記加速度センサ及び／又はジャイロセンサにより検出された加速度情報及び／又は角速度情報に基づいて、前記制御対象の角度情報又は位置情報を算出してもよい。
この一態様において、前記制御対象は、ロボットの可動部であってもよい。
上記目的を達成するための本発明の一態様は、制御対象を駆動する駆動手段と、前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、前記第１検出手段により検出された角度情報または位置情報に対して所定係数を乗算し、前記第２検出手段により検出された角度情報または位置情報に対して所定係数を乗算し、該乗算結果を加算することで共振を相殺した加算結果に基づいて、外乱推定値を算出する外乱オブザーバと、を備えることを特徴とする外乱推定システムであってもよい。
上記目的を達成するための本発明の一態様は、制御対象を駆動する駆動手段と、前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、前記第１及び第２検出手段により検出された角度情報または位置情報に対して所定係数を乗算し該乗算結果を加算することで共振を相殺する自己共振相殺制御手段と、を備える制御システムであって、前記自己共振相殺制御手段は、前記制御対象及び駆動手段に関する複数の慣性系に対して、夫々、運動方程式を算出し該算出した運動方程式を足し合わせて生成した共振成分を含まない式に基づいて生成したフィードバック制御器を含む、ことを特徴とする制御システムであってもよい。
この一態様において、前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の第２検出手段により検出されフィードバックされた角度情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対するトルク指令値を生成してもよい。
この一態様において、前記自己共振相殺制御手段は、前記第１及び少なくとも１つ以上の第２検出手段により検出されフィードバックされた角度情報または位置情報に第１制御ゲインを夫々乗算した各値と、入力される角度指令値に第２制御ゲインを乗算した値と、の偏差を夫々算出し、該各偏差を加算した値に基づいて、前記駆動手段に対する第１トルク指令値を生成し、前記自己共振相殺制御手段は、前記第１及び複数の前記第２検出手段により検出された角度情報または位置情報に第３制御ゲインを夫々乗算し、該各乗算値を加算した値を、前記外乱オブザーバに出力し、前記外乱オブザーバは、前記自己共振相殺制御手段から出力された値に基づいて外乱推定値を生成してもよい。
この一態様において、前記自己共振相殺制御手段は、前記外乱オブザーバからの外乱推定値と、前記生成した第１トルク指令値と、に基づいて第２トルク指令値を生成してもよい。
上記目的を達成するための本発明の一態様は、制御対象を駆動する駆動手段と、前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、を備える制御システムの制御方法であって、前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺する、ことを特徴とする制御システムの制御方法であってもよい。
上記目的を達成するための本発明の一態様は、制御対象を駆動する駆動手段と、前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する第２検出手段と、を備える制御システムの制御プログラムであって、前記第１検出手段により検出された角度情報または位置情報に対して前記駆動手段の慣性及び粘性に関する所定係数を乗算し、第２検出手段により検出された角度情報または位置情報に対して前記制御対象の慣性及び粘性に関する所定係数を乗算し、該乗算結果を加算することで共振を相殺する、処理をコンピュータに実行させる、ことを特徴とする制御システムの制御プログラムであってもよい。
上記目的を達成するための本発明の一態様は、制御対象を駆動する駆動手段と、前記駆動手段側に設けられ該駆動手段側の角度情報または位置情報を検出する第１検出手段と、前記制御対象側に設けられ該制御対象側の角度情報または位置情報を検出する複数の第２検出手段と、を備える制御システムの設計方法であって、前記第１及び第２検出手段により検出された角度情報または位置情報に対して所定係数を乗算し、加算することで共振を相殺する自己共振相殺制御を行う制御システムの設計方法であって、前記制御対象及び駆動手段に関する複数の慣性系に対して、夫々、運動方程式を算出するステップと、該算出した運動方程式を足し合わせ、共振成分を含まない式を生成するステップと、前記生成した式に基づいてフィードバック制御器を生成するステップと、を含む、ことを特徴とする制御システムの設計方法であってもよい。
【発明の効果】
【0007】
本発明によれば、制御対象に生じる共振をより確実に低減できる制御システム、外乱推定システム、制御方法、制御プログラム及び設計方法を提供することができる。
【図面の簡単な説明】
【0008】
【図1】本発明の実施の形態１に係る制御システムの概略的なシステム構成を示すブロック図である。
【図2】ＤＯＢのブロック線図である。
【図3】本発明の実施の形態１に係る制御装置の自己共振相殺制御部の制御ブロック線図である。
【図4】３関節脚ロボットに適用した一例を示す図である。
【図5A】プラントＰｍｍの周波数特性の実測値を示す図である。
【図5B】プラントＰｍｍの周波数特性の実測値を示す図である。
【図6】実測した周波数特性に基づいて決定したプラントのノミナル値を示す図である。
【図7】本発明の実施の形態１に係る自己共振相殺制御と、従来の自己共振相殺制御とのシミュレーション結果を示す図である。
【図8】本発明の実施の形態２に係る制御システムの概略的なシステム構成を示すブロック図である。
【図9】本発明の実施の形態２に係る自己共振相殺制御外乱オブザーバの制御ブロック線図である。
【図10A】自己共振相殺制御外乱オブザーバを比較したボーデ線図である。
【図10B】自己共振相殺制御外乱オブザーバを比較したボーデ線図である。
【図11A】自己共振相殺制御外乱オブザーバを比較したボーデ線図である。
【図11B】自己共振相殺制御外乱オブザーバを比較したボーデ線図である。
【図12】実施の形態１に係る自己共振相殺制御部と実施の形態２に係る自己共振相殺制御外乱オブザーバとを併用した制御ステムを示すブロック線図である。
【図13】本発明の実施の形態２に係る制御方法と従来の制御方法とによるシミュレーション結果を示す図である。
【図14】３慣性系のブロック線図である。
【図15A】股関節における３慣性系モデルのボーデ線図である。
【図15B】股関節における３慣性系モデルのボーデ線図である。
【図16】プラントのモデルパラメータのノミナル値の一例を示す図である。
【図17】３慣性系の自己共振相殺制御のブロック線図である。
【図18】３慣性系の自己共振相殺制御外乱オブザーバのブロック線図である。
【図19】ｎ慣性系の自己共振相殺制御外乱オブザーバの制御ブロック図である。
【図20A】１／ｓ２の伝達関数のボーデ線図である。
【図20B】モデル化誤差が生じたときのＰＳＲＣ、及びＰＳＲＣを自己共振相殺制御外乱オブザーバでノミナル化したときの伝達関数のボーデ線図である。
【発明を実施するための形態】
【0009】
実施の形態１．以下、図面を参照して本発明の実施の形態について説明する。本発明の実施の形態１に係る制御システムは、例えば、ロボットの足首関節部、膝関節部、股関節部などの各関節部の駆動を制御するものである。
【0010】
図１は本実施の形態１に係る制御システムの概略的なシステム構成を示すブロック図である。本実施の形態１に係る制御システム１は、制御対象である関節部２を駆動するモータ３と、モータ３側に設けられモータ３側の角度情報を検出する第１角度センサ４と、関節部２側に設けられ関節部２側の角度情報を検出する第２角度センサ５と、第１及び第２角度センサ４、５により検出された角度情報に基づいてモータ３を制御する制御装置６と、を備える。
【0011】
モータ３は、駆動手段の一具体例であり、関節部２に設けられており、関節部２を回転駆動する。第１角度センサ４は、第１検出手段の一具体例であり、モータ３側の角度情報（回転角度、回転角速度、回転角加速度など）を検出する。第２角度センサは、第２検出手段の一具体例であり、関節部２側の角度情報（回転角度、回転角速度、回転角加速度など）を検出する。第１及び第２角度センサ４、５は、例えば、エンコーダ、ポテンショメータなどにより構成されている。なお、本実施の形態において、関節部２は回転関節部に適用されているが、これに限らず、例えば、並進可動するなどの並進可動部などでもよく、ロボットの任意の可動部に適用可能である。
【0012】
制御装置６は、第１及び第２角度センサ４、５から出力される角度情報に基づいてモータ３を回転駆動する、所謂フィードバック制御を行っている。制御装置６は、例えば、演算処理、制御処理等と行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。
【0013】
ロボットの各関節部の制御を行う際に、その計算量と、パラメータの同定誤差、などの問題が生じる、これに対して、外乱オブザーバ（ＤＯＢ：Disturbance Observer）を用いることで、遠心力・コリオリ力・重力・摩擦力・同定誤差を全て外乱とみなし、プラントをノミナル化させるロバストなモーションコントロールを実現できる。
【0014】
ここで、外乱オブザーバについて、詳細に説明する。図２は、ＤＯＢのブロック線図である。なお、Ｐを実プラント、Ｐｎをノミナルプラント、Ｔを入力トルク、ｄをトルク外乱、Ｑをフィルタとすると、その出力は下記（１）式のように表すことができる。【数1】
【0015】
上記のようにプラントをノミナル化できるが、このノミナル化は１慣性系のみの適用となる。これは、２以上の慣性系に適用すると負荷慣性とばねによる大きな振動を誘発し、プラントをノミナル化することが困難となるからである。
【0016】
これに対し、本実施の形態１に係る制御装置６は、２慣性系の共振抑制手法である、いわゆる自己共振相殺制御（ＳＲＣ：Self Resonance Cancellation Control）を用いて、ロボットの関節部２の制御を行っている。
【0017】
ロボットの関節部２の駆動を制御する場合、その関節部を構成するギアやベルトの撓み、軸の捻れなどに起因して関節角度誤差が発生する。そこで、本実施の形態１において、この関節角度誤差を抑制するために、モータ３側の第１角度センサ４に加えて、負荷側（関節部２のギア出力段）にも第２角度センサ５を設置し、１入力２出力の制御対象を構成している。本実施の形態１に係る制御装置６は、モータ３側の第１角度センサ４及び関節部２（負荷）側の第２角度センサ５から夫々出力されフィードバックされた角度情報に対して所定係数を乗算し足合わせることで、開ループ伝達関数の見かけのプラントの共振を相殺する上記自己共振相殺制御を行う。なお、自己共振相殺制御は、ばね定数Ｋを用いずに設計できるので、ばね定数Ｋの変動にロバストである。
【0018】
さらに、従来の自己共振相殺制御においては、上記共振相殺時に粘性項については近似などを行って無視をしていた為、共振が残存する虞があった。そこで、本実施の形態１に係る制御装置６においては、共振を相殺した角度情報を生成する際に無視をしていた粘性項を厳密に考慮して上記所定係数を決定し、自己共振相殺制御を行う。
【0019】
すなわち、本実施の形態１に係る制御装置６は、第１角度センサ４により検出された角度情報に対してモータ３の慣性及び粘性に関する所定係数を乗算し、第２角度センサ５により検出された角度情報に対して関節部２の慣性及び粘性に関する所定係数を乗算し、その乗算結果を加算することで共振を相殺する自己共振相殺制御部（自己共振相殺制御手段の一具体例）６１を有している。
【0020】
これにより、ロボットの関節部２に生じる共振をより確実に低減することができる。特に、ヒューマノイドロボットや産業用ロボットなどの関節部においては、ハーモニックギアなどを用いているため、その粘性摩擦がより大きくなり問題となる。これに対し、上述の如く、粘性項を厳密に考慮して上記所定係数を決定し自己共振相殺制御を行うことで、より高精度な共振抑制効果が期待できる。
【0021】
次に、本実施の形態１に係る制御装置６において、粘性項を厳密に考慮して上記所定係数を決定する方法について、具体的に説明する。
【0022】
まず、モータ３の慣性モーメント、モータ３の粘性減衰係数、第１角度センサ４から出力されるモータ３の角度情報、関節部２の慣性モーメント、関節部２の粘性係数、第２角度センサ５から出力される関節部２（負荷）側の角度情報、及び、ばね定数を夫々、ＪＭ、ＢＭ、θＭ、ＪＬ、ＢＬ、θＬ、Ｋ、とする。また、モータ３の慣性モーメント、モータ３の粘性減衰係数、関節部２側の慣性モーメント、関節部２の粘性係数、及びばね定数のノミナル値を夫々、ＪＭｎ、ＢＭｎ、ＪＬｎ、ＢＬｎ、Ｋｎとし、関節部２のギア比をｎとする。以下、添え字のｎが付いたものは全てノミナル値とする。ＴＭからθＭ（図３参照）までの伝達関数ＰＭＭ、ＴＭからθＬまでの伝達関数ＰＭＬ、ｄＬからθＭまでの伝達関数ＰＬＭ、ｄＬからθＬまでの伝達関数ＰＬＬを下記（３）式から（８）式で表すことができる。【数2】
【0023】
なお、本実施の形態１に係る自己共振相殺制御においては、フィードバック制御の設計と、開ループ伝達関数の見かけのプラントの共振相殺と、を別々に行う。これにより、制御装置を自由に設計できる。
【0024】
図３は、本実施の形態１に係る制御装置の自己共振相殺制御部の制御ブロック線図である。ここで、Ｆをモデル応答とすると、関節部２側（負荷側）の角度指令値θＬｒｅｆからθＬまでの閉ループ伝達関数ＧＣ、開ループ伝達関数ＧＯ、それぞれのノミナル値ＧＣｎ、ＧＯｎ、は、下記（９）乃至（１２）式のように表すことができる。【数3】
【0025】
さらに、上記（１２）式に示すＧＯｎの見かけのプラントの共振の項が相殺されるように所定係数α、β、γ、δを、下記（１３）式に示すように決定できる。特に、従来無視されていたβ及びδの項を考慮することで、粘性項を厳密に考慮することができる。
【0026】
従来は、制御対象における伝達関数を「剛体モード」と「共振モード」の積の形式で表しており、粘性項を積の形式で分離できないため、近似などを行い無視されていた。一方、本実施の形態１においては、伝達関数を「剛体モード」と「共振モード」の積の形式で分離しない下記（１３）式を用いることで、粘性項を厳密に考慮することができる。【数4】
【0027】
このとき、下記（１４）式が成立する。ＧＯｎ＝ＰＳＲＣｎＣＦＢ
ＰＳＲＣｎ＝１／ＪＭｎｓ２ （１４）
【0028】
上記（１４）式に示す如く、見かけのプラントＰＳＲＣｎに共振成分の粘性項が存在しない。したがって、従来、近似などを行って無視していた粘性項が存在しないことから、共振が残存することがない。
【0029】
本実施の形態１に係る制御装置６の自己共振相殺制御部６１は、上記（１３）式に基づいて所定係数α、β、γ、δを設定する。そして、自己共振相殺制御部６１は、図３に示す如く、第１角度センサ４からの角度情報θＭ及び第２角度センサ５からの角度情報θＬに対して、設定した所定係数α、β、γ、δを夫々乗算する。さらに、自己共振相殺制御部６１は、その乗算した各乗算結果を加算器で加算し、その加算結果に基づいて、モータトルク指令値を生成する。
【0030】
より具体的には、図３に示す如く、自己共振相殺制御部６１は、入力された角度指令値θＬｒｅｆに基づいたモデル応答値と、フィードバックされた第２角度センサ５の角度情報θＬと、の偏差を算出し、その偏差に所定係数γ、δを夫々乗算する。さらに、自己共振相殺制御部６１は、偏差に所定係数δを乗算して求めた値のみに積分処理を行う。
【0031】
また、自己共振相殺制御部６１は入力された角度指令値θＬｒｅｆに制御ゲインを乗算し、その乗算値と、フィードバックされた第１角度センサの角度情報と、の偏差を算出し、その偏差に所定係数α、βを夫々乗算する。さらに、自己共振相殺制御部６１は、偏差に所定係数βを乗算して求めた値のみに積分処理を行う。自己共振相殺制御部６１は、上述のように所定係数α、β、γ、δを夫々乗算して算出した値を加算し、その加算値に基づいてモータトルク指令値ＴＭを算出する。
【0032】
このように、モータ３及び関節部２の粘性係数を厳密考慮しつつ、制御対象の逆相となる所定係数を決定でき、共振を大きく低減できる。また、フィードバック制御を独立に設計できるため、制御装置６のフィードバック制御を自由に設計できる。
【0033】
次に、本実施の形態に係る自己共振相殺制御のシミュレーション結果について説明する。図４に示す如く、３関節脚ロボットに適用した一例について説明する。３関節脚ロボットの股関節まわりを２慣性系でモデル化し、本実施の形態１に係る自己共振相殺制御と、従来の自己共振相殺制御との比較を行う。
【0034】
図５Ａ及びＢは、プラントＰｍｍの周波数特性の実測値を示す図である。この周波数特性の実測は、膝関節部を９０度に屈曲させた状態と、股関節部を地面に対して垂直にした状態と、で行っている。図６は、実測した周波数特性に基づいて決定したプラントのノミナル値を示している。なお、Ｑフィルタは下記（２４）式に示すように設定し、極を共に２００Ｈｚとし、各関節部に対する位置指令値を０［ｒａｄ］としている。開始１ｓｅｃ後に１０［Ｎｍ］のステップ外乱を負荷側に加えている。Ｑ＝ωＱ／（ｓ＋ωＱ） （２４）式
ωＱ＝１５０×２π［ｒａｄ／ｓｅｃ］
【0035】
図７は、本実施の形態１に係る自己共振相殺制御と、従来の自己共振相殺制御とのシミュレーション結果を示す図である。図７に示す如く、従来の自己共振相殺制御を行った場合に（実線）、残存する共振によって制御が発散してしまうことが分かる。一方、本実施の形態１に係る自己共振相殺制御を行った場合（点線）、共振が残存することなく、制御が収束していることが分かる。
【0036】
以上、本実施の形態１に係る制御システム１においては、第１角度センサ４により検出された角度情報に対してモータ３の慣性及び粘性に関する所定係数を乗算し、第２角度センサ５により検出された角度情報に対して関節部２の慣性及び粘性に関する所定係数を乗算し、その乗算結果を加算することで共振を相殺する。これにより、従来無視されていた粘性項を厳密に考慮することができるため、ロボットの関節部２に生じる共振をより確実に低減することができる。
【0037】
実施の形態２．図８は、本実施の形態２に係る制御システムの概略的なシステム構成を示すブロック図である。本実施の形態２に係る制御システム１０の制御装置６２は、上記実施の形態１に係る２慣性系の自己共振相殺制御部と外乱オブザーバとを組み合わせた自己共振相殺制御外乱オブザーバ（ＳＲＣＤＯＢ：Self Resonance Cancellation Control Disturbance Observer）６３を備える点を特徴とする。これにより、従来の外乱オブザーバではノミナル化が１慣性系に限られ、さらに、自己共振相殺制御では慣性モーメントと粘性係数のモデル化誤差に弱いという２つの問題を同時に解決できる。すなわち、本実施の形態２に係る制御システムにおいて、２慣性系のシステムにおいてモデル変動しても外乱を抑制し制御帯域を向上させることができる。
【0038】
まず、共振相殺を行い剛体モードとなるロボットの関節部２の角速度（以下、関節角速度と称す）を以下の（１５）式で定義することができる。なお、下記（１５）式において、所定係数α、β、γ、δは上記（１３）式を用いて求めることができる。【数5】
【0039】
本実施の形態２においては、上記（１５）式に示す出力に対して外乱オブザーバを設計することで自己共振相殺制御外乱オブザーバ６３を構成する。すなわち、従来、外乱オブザーバを設計する場合、関節部側の第２角度センサの角度情報とモータ側の第１角度センサの角度情報との２種類が存在し、少なくとも一方が外乱オブザーバに入力される。しかしながら、いずれの角度情報も共振によるノイズが含まれるため、外乱を完全に抑制できない。
【0040】
一方、本実施の形態２に係る自己共振相殺制御外乱オブザーバ６３において、自己共振相殺制御部は上記（１３）式で決定した所定係数α、β、γ、δ及び上記（１５）式を用いて、共振を完全に相殺した関節角速度を算出し、その算出した関節角速度を外乱オブザーバに入力する。したがって、外乱オブザーバは、その共振を完全に相殺した関節角速度に基づいて、外乱推定値を高精度に推定できるため、外乱をより確実に抑制できる。
【0041】
図９は、本実施の形態２に係る自己共振相殺制御外乱オブザーバの制御ブロック線図である。図９において、外乱推定値ｄＳＲＣハットは下記（１６）式で表すことができる。なお、下記（１６）式において、モデル化誤差ΔＰＭＭ、ΔＰＭＬ、ΔＰＬＭ、ΔＰＬＬを下記（１７）式で表すことができる。【数6】
【0042】
上記（１６）式に示す外乱推定値において、共振成分はモデル化誤差により発生する項のみに依存している。従来の外乱オブザーバにおいては、モデル化誤差が存在しない場合でも負荷側外乱ｄＬの係数に共振成分が存在していた。一方、本実施の形態２に係る自己共振相殺制御外乱オブザーバ６３においては、外乱推定開始直後はモデル化誤差により共振が相殺されないものの、次第にプラントがノミナル化されて共振が相殺され、安定化していくことが分かる。
【0043】
さらに、Ｑ≒約１として見かけ上のモータトルク指令値Ｔ'ＭからθＳＲＣの微分値（関節角速度）までの伝達関数を下記（１８）式で表すことができる。【数7】
【0044】
上記（１８）式は、上記（１４）式の見かけ上のプラントと一致する。ばね定数Ｋの変動に対して、ロバストな自己共振相殺制御と、この自己共振相殺制御の見かけのプラントをノミナル化できる外乱オブザーバと、を併用すれば、全パラメータ変動にロバストな２慣性系フィードバック制御が構成できる。このとき、見かけのモータトルク指令値Ｔ'ＭからθＭまでの伝達関数を下記（１９）式で表すことができる。【数8】
【0045】
さらに、モデル化誤差を下記（２１）式及び（２２）式で表すことができるため、プラントの剛体モードがノミナル化されていることが確認できる。【数9】
【0046】
また、上記（１９）式に基づいて、共振周波数ωｒと反共振周波数ωａｒとの比である共振比Ｈを下記（２３）式で表すことでき、共振比がノミナル化されることがわかる。【数10】
【0047】
次に、上述した本実施の形態２に係る自己共振相殺制御外乱オブザーバ６３のノミナル化について具体例を挙げて説明する。例えば、負荷（関節部２）の慣性モーメントＪＬが変動した際に、自己共振相殺制御外乱オブザーバによりプラントがノミナル化されることをボーデ線図で確認できる。なお、ＪＭ、ＢＭ、ＢＬのモデル化誤差に関しても、ＪＬと同様にノミナル化できるため、詳細な説明は省略する。
【0048】
図１０Ａ及びＢは、ＪＬ＝ＪＬｎの場合、ＪＬ＝２ＪＬｎの場合、及びＪＬ＝２ＪＬｎに自己共振相殺制御外乱オブザーバ（ＳＲＣＤＯＢ）を適用した場合、を比較したボーデ線図である。なお、Ｑフィルタは下記（２５）式に示すように設定されている。Ｑ＝ωＱ／（ｓ＋ωＱ） （２５）式
ωＱ＝１５０×２π［ｒａｄ／ｓｅｃ］
【0049】
図１０Ａ及びＢに示す如く、本実施の形態２に係る自己共振相殺制御外乱オブザーバ６３を用いることで剛体モードがノミナル化されていることが分かる。さらに、図１１Ａ及びＢに示す如く、Ｔ'ＭからθＳＲＣの微分値までの伝達関数ＰＳＲＣがノミナル化され、見かけのプラントの共振が相殺できていることが確認できる。
【0050】
次に、上述した本実施の形態２に係る自己共振相殺制御外乱オブザーバ６３における制御のシミュレーションについて、詳細に説明する。ここでは、上記実施の形態１に係る自己共振相殺制御部６１と本実施の形態２に係る自己共振相殺制御外乱オブザーバ６３とを併用した制御システムと、従来のＰＩＤ制御と外部オブザーバとを併用した制御システムと、の比較を行う。
【0051】
図１２は、上記実施の形態１に係る自己共振相殺制御部と本実施の形態２に係る自己共振相殺制御外乱オブザーバとを併用した制御ステムを示すブロック線図である。各制御システムは、膝関節部を９０度に屈曲させた状態のノミナルプラントＰＳＣＲｎを制御している。なお、本実施の形態２に係る制御方法は、ωＣ＝−２００×２πとし、Ｑフィルタのカットオフ周波数をωＱ＝１５０×２πとして設定している。一方、従来の制御方法はωＣ＝−１００×２πとして、Ｑフィルタのカットオフ周波数をωＱ＝７５×２πとして設定している。このように、各パラメータを制御が発散しない帯域に設定している。以上の条件で、ＪＬ＝２ＪＬｎとして、１ｓｅｃ後に１０Ｎｍのステップ外乱を入力し、本実施の形態２に係る制御方法と従来の制御方法の応答を比較する。
【0052】
図１３は、本実施の形態２に係る制御方法と従来の制御方法とによるシミュレーション結果を示す図である。図１３に示す如く、従来の制御方法においては、負荷側外乱により振動が発生していることが分かる。一方、本実施の形態２に係る制御方法によれば、帯域が上がっており、さらに、振動が発生しないことが分かる。
【0053】
以上、本実施の形態２に係る制御システム１０において、２慣性系の自己共振相殺制御部と外乱オブザーバとを組み合わせた自己共振相殺制御外乱オブザーバ６３を備える。これにより、２慣性系においてもノミナル化が可能となり、さらに、自己共振相殺制御における慣性モーメントと粘性係数のモデル化誤差を大きく改善できる。
【0054】
実施の形態３．本発明の実施の形態３は、上記実施の形態１に係る自己共振相殺制御部６１および上記実施の形態２に係る自己共振相殺制御外乱オブザーバ６３を、ｎ慣性系（ｎは３以上）に適用させた点を特徴とする。例えば、ヒューマノイドロボットの股関節部においては、低周波数に共振が複数表れている。このため、多慣性系のプラントのモデル化誤差にロバストな共振抑制制御が必要となる。これに対し、本実施の形態３に係る自己共振相殺制御部及び自己共振相殺制御外乱オブザーバは、モータ３側の第１角度センサ４だけでなく、負荷側にもｎ−１個（複数個）の第２角度センサ５を設け、各角度センサ４、５により検出される角度情報に所定係数を乗算し足し合わせることで、ｎ−１の共振を相殺することができる。これにより、軸ねじれや撓みなどが複数個所で発生する場合でも、常時、共振を相殺できるため、制御性能を向上させることができる。
【0055】
なお、上述の如く、複数の第２角度センサを設ける場合、例えば、ロボットのハードウェア制限により、３つ目（或いは４つ目以上）の慣性を有する部位の変位をエンコーダなどで計測することが困難となることがある。この場合、加速度センサ及び／又はジャイロセンサにより検出された加速度情報に基づいてその慣性を有する部位の変位を算出してもよい。
【0056】
次に、本実施の形態３に係る自己共振相殺制御部および自己共振相殺制御外乱オブザーバを３関節脚ロボットとその３慣性系モデルに適用させた一例を説明する。まず、３関節脚ロボットの基本構成について説明する。
【0057】
３関節脚ロボットは、股関節部、膝関節部及び足首関節部の３つの関節部を備えている。図４に示すように、各モータ３は各関節部２のタイミングベルトを介してハーモニックギアに連結されている。各モータ３が回転駆動することで、各関節部２は回転駆動する。ここで、このタイミングベルトが持つばね特性の影響で１つの共振が発生している。また、股関節部においてはさらにもう１つ共振が発生している。したがって、股関節部においては３慣性系でのモデル化が必要になっている。
【0058】
図１４は、上述した３慣性系のブロック線図である。ｎ番目の慣性モーメント及び粘性減衰係数を夫々、Ｊｎ、Ｂｎと定義する。また、ｎ番目の慣性とｎ＋１番目の慣性との間のばね定数、及びギア比を夫々Ｋｎ,ｎ＋１、ｒｎ,ｎ＋１とする。また、ノミナル値は文字の右上にＮを付するものとする。なお、本実施の形態３においては、３関節のうち股関節部に適用した場合について説明を行うが、膝関節部及び足首関節部についても股関節部と同様に適用可能である。
【0059】
ここで、股関節部周りの周波数特性について、説明する。図１５Ａ及びＢは、股関節部における、１慣性目へのトルク指令値Ｔ１から関節角度θ１までの周波数特性Ｐ１１の計測結果と、その計測結果に基づく３慣性系モデルのボーデ線図である。３関節脚ロボットの大腿部および下腿部は地面に垂直にし、足先は地面に水平にして計測を行っている。図１５Ａ及びＢは示す如く、６０Ｈｚに生じている共振は、タイミングベルトに依るものである。図１６は、上記周波数特性に基づいて決定したプラントのモデルパラメータのノミナル値を示している。
【0060】
次に上述したような３慣性系のプラントに対する自己共振相殺制御、及び自己共振相殺制御外乱オブザーバの理論を説明する。３慣性系のプラントにおける運動方程式は、下記（２６）式で表すことができる。【数11】
【0061】
さらに、上記３つの運動方程式を全て足し合わせると、下記（２７）式が導出される。【数12】
【0062】
上記（２７）式におけるＰＳＲＣには共振成分は存在しなくなるので、このＰＳＲＣに対しフィードバック制御器を構成すればよいこととなる。実際にはノミナル値に対し制御器は構成されるため、下記（２８）式となるようにフィードバック制御器を構成すればよいこととなる。【数13】
【0063】
上述したように、本実施の形態３に係る自己共振相殺制御においては、関節部２及びモータ３に関する３個の慣性系に対して、夫々、運動方程式を算出する。そして、その算出した運動方程式を足し合わせて生成した共振成分を含まない式に基づいてフィードバック制御器を生成する。
【0064】
これにより、元々Ｐ１、１やＰ２、１は共振を持っているにも関わらず、３つの信号に所定係数をかけて足し合わせることでフィードバック制御における見かけのプラントの共振が相殺される。これが３慣性系の自己共振相殺制御である。上記一例では、３慣性系について説明したが、４以上の慣性系についても同様に導出できる。このように、慣性系毎の運動方程式を単純に足し合わせた式に基づいて、自ずと共振成分を相殺したフィードバック制御器を構成できる。
【0065】
図１７は、上述した３慣性系の自己共振相殺制御のブロック線図である。図１７に示す如く、自己共振相殺制御部は、第１乃至第３角度センサにより検出された角度情報θ１、θ２、θ３に第１制御ゲインを夫々乗算した各値と、入力される角度指令値θ１ｒｅｆ、θ２ｒｅｆ、θ３ｒｅｆに第２制御ゲインを乗算した値と、の偏差ｅ１、ｅ２、ｅ３を夫々算出する。そして、自己共振相殺制御部は、算出した各偏差ｅ１、ｅ２、ｅ３を加算した値ｅＳＲＣに基づいて、モータに対するトルク指令値Ｔ１を生成する。
【0066】
図１８は、上述した３慣性系の自己共振相殺制御外乱オブザーバのブロック線図である。図１８に示す如く、自己共振相殺制御部は、第１乃至第３角度センサにより検出された角度情報θ１、θ２、θ３に第１制御ゲインを夫々乗算した各値と、入力される角度指令値θ１ｒｅｆ、θ２ｒｅｆ、θ３ｒｅｆに第２制御ゲインを乗算した値と、の偏差ｅ１、ｅ２、ｅ３を夫々算出する。そして、自己共振相殺制御部は、算出した各偏差ｅ１、ｅ２、ｅ３を加算した値ｅＳＲＣに基づいて、第１トルク指令値Ｔ１を生成する。さらに、自己共振相殺制御部は、第１乃至第３角度センサにより検出された角度情報θ１、θ２、θ３に第３制御ゲインを夫々乗算した各値を加算して関節角速度（θＳＲＣの微分値）を算出し、外乱オブザーバに出力する。外乱オブザーバは、自己共振制御部からの関節角速度θＳＲＣに基づいて外乱推定値ｄＳＲＣハットを生成し、自己共振相殺制御部に出力する。自己共振相殺制御部は、外乱オブザーバからの外乱推定値ｄＳＲＣハットと、生成した第１トルク指令値Ｔ１'と、の偏差を求め、その偏差を第２トルク指令値Ｔ１とする。なお、上述した図１７及び１８に示す自己共振相殺制御及び自己共振相殺制御外乱オブザーバは一例であり、これに限らず、例えば、図１７及び１８に示すブロック線図を等価変換あるいは、同等の数式変形に基づく制御システムであってもよい。
【0067】
なお、上述の如く、３慣性系モデルに適用させた一例を説明したがｎ（４≦ｎ）慣性系モデルについても同様に適用させることができる。この場合、下記（２９）式に示すようにｎ個の運動方程式を全て足し合わせる。【数14】
【0068】
下記（３０）式は、上記（２９）式を足し合わせた結果である。【数15】
【0069】
上記（３０）式におけるＰＳＲＣには共振成分が存在しなくなる。実際にはノミナル値に対し制御器は構成されるため、開ループ伝達関数のノミナル値が下記（３１）式に示すように制御器を構成する。【数16】
【0070】
図１９は、上述のように構成したｎ慣性系の自己共振相殺制御外乱オブザーバの制御ブロック図である。
【0071】
次に、上述した本実施の形態３に係る３慣性系の自己共振相殺制御部と自己共振相殺制御外乱オブザーバとを併用した制御のシミュレーションについて、詳細に説明する。本シミュレーションにおいて、従来のＰＩＤ制御及び外乱オブザーバの制御と、本実施の形態３に係る３慣性系の自己共振相殺制御と自己共振相殺制御外乱オブザーバとを併用した制御との比較を行う。ここで、負荷の慣性モーメントＪＬ＝２ＪＬｎなるモデル化誤差を与えてシミュレーションを行う。
【0072】
負荷の慣性モーメントにＪ３＝２ＪＮ3なるモデル化誤差が生じたときに、自己共振相殺制御外乱オブザーバによってＰＳＲＣがノミナル化される。図２０Ａは、１／ｓ２の伝達関数のボーデ線図である。図２０Ｂは、モデル化誤差が生じたときのＰＳＲＣ（Ｊ２＝２Ｊ２ｎ）、及びモデル化誤差が生じたときのＰＳＲＣ（Ｊ２＝２Ｊ２ｎ）を自己共振相殺制御外乱オブザーバでノミナル化したときの伝達関数のボーデ線図である。図２０Ａ及びＢに示す如く、自己共振相殺制御外乱オブザーバを用いることで、負荷の慣性モーメントＪＬに変動が生じた場合でも、ＳＲＣプラントＰＳＲＣがノミナル化され、共振がほとんど相殺されていることがわかる。なお、若干共振が残っているが、これはＱ≠１であるためで、ωＱ→∞となるとき、完全に共振が相殺される。
【0073】
以上、本実施の形態３において、関節部及びモータに関する複数の慣性系に対して、夫々、運動方程式を算出する。そして、その算出した運動方程式を足し合わせて生成した共振成分を含まない式に基づいてフィードバック制御器を生成する。これにより、軸ねじれや撓みなどが複数個所で発生する場合でも、共振を相殺できるため、制御性能を向上させることができる。
【0074】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、上記実施の形態において、制御対象としてロボットの関節部に適用されているが、これに限らず、任意の可動部に適用可能である。
【0075】
また、本発明は、例えば、上記自己共振相殺制御及び外乱オブザーバの処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
【0076】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【0077】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【0078】
１ 制御システム２ 関節部
３ モータ
４ 第１角度センサ
５ 第２角度センサ
６ 制御装置