▶ 日本電産サンキョー株式会社の特許一覧 ▶ 国立大学法人長岡技術科学大学の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-08-08
(45)【発行日】2024-08-19
(54)【発明の名称】動力学トルク補償に基づく制御方法及び制御装置
(51)【国際特許分類】
G05B 11/32 20060101AFI20240809BHJP
G05B 11/36 20060101ALI20240809BHJP
H02P 29/00 20160101ALI20240809BHJP
【ＦＩ】
G05B11/32 F
G05B11/36 F
H02P29/00
【請求項の数】
15
(21)【出願番号】P 2020129350
(22)【出願日】2020-07-30
(65)【公開番号】P2022026069
(43)【公開日】2022-02-10
【審査請求日】2023-06-13
(73)【特許権者】
【識別番号】000002233
【氏名又は名称】ニデックインスツルメンツ株式会社
(73)【特許権者】
【識別番号】304021288
【氏名又は名称】国立大学法人長岡技術科学大学
(74)【代理人】
【識別番号】100123788
【弁理士】
【氏名又は名称】宮崎 昭夫
(74)【代理人】
【識別番号】100127454
【弁理士】
【氏名又は名称】緒方 雅昭
(72)【発明者】
【氏名】花岡 正志
(72)【発明者】
【氏名】桃澤 義秋
(72)【発明者】
【氏名】大石 潔
(72)【発明者】
【氏名】横倉 勇希
【審査官】田中 友章
(56)【参考文献】
【文献】特開２０１１－１５２００６（ＪＰ，Ａ）
【文献】特開２０１８－１９６２６６（ＪＰ，Ａ）
【文献】特開２００２－３３１４７７（ＪＰ，Ａ）
【文献】特開２００８－３１０６０３（ＪＰ，Ａ）
【文献】特開２０１９－１５９３８２（ＪＰ，Ａ）
【文献】国際公開第２０１４／１１２１７８（ＷＯ，Ａ１）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０５Ｂ １１／３２
Ｇ０５Ｂ １１／３６
Ｈ０２Ｐ ２９／００
(57)【特許請求の範囲】
【請求項1】
外部から入力する位置指令値に基づいてモータを制御する制御方法であって、前記位置指令値と前記モータの位置との差である位置偏差を算出し、前記位置偏差に基づく速度指令値と前記モータの速度との差である速度偏差を算出し、少なくとも前記速度偏差を用いて前記モータへの電流指令値を生成する工程と、
動力学トルクフィードフォワード補償により、プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する工程と、
前記負荷外乱トルク補償値を、前記プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、前記速度指令値に対して補償を行う速度指令補償値に変換する工程と、
前記速度指令補償値を、制御系において前記位置指令値の入力位置から前記電流指令値を生成するまでの区間であって速度偏差の積分補償器よりも前段にある加え合わせ点に入力する工程と、
を有し、
前記負荷外乱トルク補償値を算出する工程は、前記位置指令値から負荷状態を予測し、予測された前記負荷状態に基づいて、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求めて前記負荷外乱トルク補償値を算出する工程であることを特徴とする、制御方法。
【請求項2】
前記負荷外乱トルク補償値を前記速度指令補償値に変換するときに用いる伝達関数は、前記負荷側外乱入力から前記制御系内での前記速度指令補償値が入力される前記加え合わせ点までの伝達関数であって、分母に非零の定数項を含む、請求項１に記載の制御方法。
【請求項3】
前記加え合わせ点への前記速度指令補償値の入力は、前記速度指令値への前記速度指令補償値の加算、前記速度偏差への前記速度指令補償値の加算、及び、前記モータの速度からの前記速度指令補償値の減算、のいずれか１つによって行われる、請求項１または２に記載の制御方法。
【請求項4】
外部から入力する位置指令値に基づいてモータを制御する制御方法であって、前記位置指令値と前記モータの位置との差である位置偏差を算出し、少なくとも前記位置偏差を用いて前記モータに対する電流指令値を生成する工程と、
動力学トルクフィードフォワード補償により、プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する工程と、
前記負荷外乱トルク補償値を、前記プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、前記位置指令値に対して補償を行う位置指令補償値に変換する工程と、
前記位置指令補償値を、制御系において前記位置指令値の入力位置から前記電流指令値を生成するまでの区間であって位置偏差の積分補償器よりも前段にある加え合わせ点に入力する工程と、
を有し、
前記負荷外乱トルク補償値を算出する工程は、前記位置指令値から負荷状態を予測し、予測された前記負荷状態に基づいて、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求めて前記負荷外乱トルク補償値を算出する工程であることを特徴とする、制御方法。
【請求項5】
前記負荷外乱トルク補償値を前記位置指令補償値に変換するときに用いる伝達関数は、前記負荷側外乱入力から前記制御系内での前記位置指令補償値が入力される前記加え合わせ点までの伝達関数であって、分母に非零の定数項を含む、請求項４に記載の制御方法。
【請求項6】
前記加え合わせ点への前記位置指令補償値の入力は、前記位置指令値への前記位置指令補償値の加算、前記位置偏差への前記位置指令補償値の加算、前記位置偏差に比例ゲインを乗じたものへの前記位置指令補償値の加算、及び、前記位置指令補償値の前記モータの位置からの減算、のいずれか１つによって行われる、請求項４または５に記載の制御方法。
【請求項7】
外部から入力する位置指令値に基づいて電流指令値を生成してプラントに含まれるモータを制御する制御装置であって、前記位置指令値と前記モータの位置との差である位置偏差を算出する位置制御器と、
前記位置制御器の出力に基づく速度指令値と前記モータの速度との差である速度偏差を算出し、少なくとも前記速度偏差を用いて前記電流指令値を生成する速度制御器と、
前記速度偏差に対する積分動作を行う積分補償器と、
前記位置指令値から負荷状態を予測する負荷状態算出部と、
予測された前記負荷状態に基づく動力学トルクフィードフォワード補償の演算によって、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求め、前記プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する逆動力学計算部と、
前記負荷外乱トルク補償値を、前記プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、前記速度指令値に対して補償を行う速度指令補償値に変換する指令変換手段と、
を有し、
前記速度指令補償値が、前記速度制御器において前記積分補償器よりも前段にある加え合わせ点に入力されることを特徴とする、制御装置。
【請求項8】
前記指令変換手段において前記負荷外乱トルク補償値を前記速度指令補償値に変換するときに用いる伝達関数は、前記負荷側外乱入力から前記制御装置において前記速度指令補償値が入力される前記加え合わせ点までの伝達関数であって、分母に非零の定数項を含む、請求項７に記載の制御装置。
【請求項9】
前記速度指令値に前記速度指令補償値を加算する加算要素、前記速度偏差に前記速度指令補償値を加算する加算要素、及び、前記モータの速度から前記速度指令補償値を減算する減算要素、のいずれか１つを備える、請求項７または８に記載の制御装置。
【請求項10】
前記速度制御器は、前記位置制御器の出力値が入力されて前記速度指令値を生成する前置補償器を備える、請求項７乃至９のいずれか１項に記載の制御装置。
【請求項11】
外部から入力する位置指令値に基づいて電流指令値を生成してプラントに含まれるモータを制御する制御装置であって、
前記位置指令値と前記モータの位置との差である位置偏差を算出し、少なくとも前記位置偏差を用いて前記電流指令値を生成する位置速度制御器と、
前記位置偏差に対する積分動作を行う積分補償器と、
前記位置指令値から負荷状態を予測する負荷状態算出部と、
予測された前記負荷状態に基づく動力学トルクフィードフォワード補償の演算によって、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求め、前記プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する逆動力学計算部と、
前記負荷外乱トルク補償値を、前記プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、前記位置指令値に対して補償を行う位置指令補償値に変換する指令変換手段と、
を有し、
前記位置指令補償値が、前記位置速度制御器において前記積分補償器の前段にある加え合わせ点に入力されることを特徴とする、制御装置。
【請求項12】
前記指令変換手段において前記負荷外乱トルク補償値を前記位置指令補償値に変換するときに用いる伝達関数は、前記負荷側外乱入力から前記制御装置において前記位置指令補償値が入力される前記加え合わせ点までの伝達関数であって、分母に非零の定数項を含む、請求項１１に記載の制御装置。
【請求項13】
前記位置指令値に前記位置指令補償値を加算する加算要素、前記位置偏差に前記位置指令補償値を加算する加算要素、及び、前記モータの位置から前記位置指令補償値を減算する減算要素、のいずれか１つを備える、請求項１１または１２に記載の制御装置。
【請求項14】
外部から入力する位置指令値に基づいて電流指令値を生成してプラントに含まれるモータを制御する制御装置であって、
前記位置指令値と前記モータの位置との差である位置偏差を算出する位置制御器と、
前記位置制御器の出力に基づく速度指令値と前記モータの速度との差である速度偏差を算出し、少なくとも前記速度偏差を用いて前記電流指令値を生成する速度制御器と、
前記速度偏差に対する積分動作を行う積分補償器と、
前記位置指令値から負荷状態を予測する負荷状態算出部と、
予測された前記負荷状態に基づく動力学トルクフィードフォワード補償の演算によって、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求め、前記プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する逆動力学計算部と、
前記負荷外乱トルク補償値を、前記プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、前記位置指令値に対して補償を行う位置指令補償値に変換する指令変換手段と、
前記位置制御器の出力に対して前記位置指令補償値を加算する加算要素と、
を有し、
前記加算要素の出力が前記速度制御器に入力することを特徴とする制御装置。
【請求項15】
前記指令変換手段において前記負荷外乱トルク補償値を前記位置指令補償値に変換するときに用いる伝達関数は、前記負荷側外乱入力から前記加算要素までの伝達関数であって、分母に非零の定数項を含む、請求項１４に記載の制御装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、産業用ロボット（以下、「ロボット」とも称する）などの動作制御などに用いられる、動力学トルク補償に基づく制御方法及び制御装置に関する。
【背景技術】
【0002】
一般にロボットは複数の軸を有し、各軸のリンクが直列に接続されてマニピュレータを構成している。ロボットを高速で動かした場合には、マニピュレータの先端が振動しやすく、軸間干渉力によってマニピュレータ先端の軌跡に誤差が生じる。マニピュレータ先端の軌跡誤差を低減するために、お互いの動きによらずに位置指令に対する各軸の応答性を同じにすることが必要である。特許文献１は、動力学トルクフィードフォーワード補償により、各軸の駆動トルクを自軸分のノミナルトルクに近付け、全ての軸について位置制御応答性を同じにすることを開示している。特許文献１に記載された技術では、位置指令値から負荷速度までの伝達関数に基づいて、位置指令値を入力として負荷状態量（負荷の位置、速度及び加速度）を算出する負荷状態量算出部と、算出された負荷状態量を入力として負荷側のトルク補償値を算出する逆動力学計算部と、トルク補償値を、負荷側の外乱トルクから負荷側速度までの応答を相殺するための電流指令値補償値に変換する変換部と、を備え、ロボット制御装置における電流指令値に電流指令補償値を加算している。ここでトルク補償値は、他軸からの干渉力と、姿勢変化に伴う自軸の慣性モーメントのノミナルトルクからの変化分であるトルク変動量との和で表される。非特許文献１は、特許文献１などにおいて用いられる動力学トルクフィードフォワード補償について詳細に論じるとともに、軸ねじれ振動を抑圧する方法を開示している。特許文献２は、ロボットにおける動力学トルクフィードフォワード補償の応用の一例として、予期せぬ外乱力が作用したときにロボットが柔軟に動くことができるようにする制御を説明している。特許文献２は、通常のモータの位置、速度制御系において、制御偏差の積分補償器が設けられることも示している。
【先行技術文献】
【特許文献】
【0003】
【文献】特許第４００８２０７号公報
【文献】特開平１０－１８０６６３号公報
【非特許文献】
【0004】
【文献】宮崎 敏昌，大瀧 栄，ソムサワッス タンパタラタナウォン，大石 潔，“動力学トルク補償と２自由度制御系に基づく産業用ロボットの高速モーション制御法”，電気学会論文誌Ｄ，2003年，第123巻，第5号，p. 525-532
【発明の概要】
【発明が解決しようとする課題】
【0005】
特許文献１などに示される、動力学トルクフィードフォワード補償を用いて電流指令値補償値を算出し、算出された電流指令値補償値を電流指令値に加算するロボットの制御では、ロボットが位置決めを繰り返したときなどに、演算精度が低下することにより位置制御性能が失われるおそれがある。
【0006】
本発明の目的は、ロボットの制御などに用いられる制御方法及び制御装置であって、負荷側外乱トルクに対する動力学トルクフィードフォワード補償を行うときに、演算精度と位置制御性能とを維持できる制御方法及び制御装置を提供することにある。
【課題を解決するための手段】
【0007】
本発明の第１の制御方法は、外部から入力する位置指令値に基づいてモータを制御する制御方法であって、位置指令値とモータの位置との差である位置偏差を算出し、位置偏差に基づく速度指令値とモータの速度との差である速度偏差を算出し、少なくとも速度偏差を用いてモータへの電流指令値を生成する工程と、動力学トルクフィードフォワード補償により、プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する工程と、負荷外乱トルク補償値を、プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、速度指令値に対して補償を行う速度指令補償値に変換する工程と、速度指令補償値を、制御系において位置指令値の入力位置から電流指令値を生成するまでの区間であって速度偏差の積分補償器よりも前段にある加え合わせ点に入力する工程と、を有し、負荷外乱トルク補償値を算出する工程は、位置指令値から負荷状態を予測し、予測された負荷状態に基づいて、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求めて負荷外乱トルク補償値を算出する工程であることを特徴とする。
【0008】
本発明の第１の制御方法によれば、制御系において速度偏差の積分補償器よりも前段にある加え合わせ点にフィードフォワード補償値である速度指令補償値を入力することにより、フィードフォワード補償値に誤差が含まれることや、積分補償器の積分値が増大することが防がれ、負荷側外乱トルクの補償を行うときに、演算精度と位置制御性能とを維持できる。予測された負荷状態に基づいて、自軸の慣性モーメントのノミナルトルクからの変化分との和を求めて負荷外乱トルク補償値を算出することにより、負荷外乱トルク補償値を精度よく求めることが可能になる。フィードフォワード補償値として速度指令補償値を用いることにより、位置指令値に対応するフィードフォワード補償値を求める場合に比べて計算に用いる伝達関数の次数を下げられるので、演算負荷を軽減することができる。
【0009】
第１の制御方法において加え合わせ点への速度指令補償値の入力は、速度指令値への速度指令補償値の加算、速度偏差への速度指令補償値の加算、及び、モータの速度からの速度指令補償値の減算、のいずれか１つによって行なうことができる。このようにして制御系に対して速度指令補償値を入力することにより、速度指令補償値の入力の処理を容易なものとすることができる。
【0010】
本発明の第２の制御方法は、外部から入力する位置指令値に基づいてモータを制御する制御方法であって、位置指令値とモータの位置との差である位置偏差を算出し、少なくとも位置偏差を用いてモータに対する電流指令値を生成する工程と、動力学トルクフィードフォワード補償により、プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する工程と、負荷外乱トルク補償値を、プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、位置指令値に対して補償を行う位置指令補償値に変換する工程と、位置指令補償値を、制御系において位置指令値の入力位置から電流指令値を生成するまでの区間であって位置偏差の積分補償器よりも前段にある加え合わせ点に入力する工程と、を有し、負荷外乱トルク補償値を算出する工程は、位置指令値から負荷状態を予測し、予測された負荷状態に基づいて、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求めて負荷外乱トルク補償値を算出する工程であることを特徴とする。
【0011】
本発明の第２の制御方法によれば、制御系において速度偏差の積分補償器よりも前段にある加え合わせ点にフィードフォワード補償値である位置指令補償値を入力することにより、フィードフォワード補償値に誤差が含まれることや、積分補償器の積分値が増大することが防がれ、負荷側外乱トルクの補償を行うときに、演算精度と位置制御性能とを維持できる。予測された負荷状態に基づいて、自軸の慣性モーメントのノミナルトルクからの変化分との和を求めて負荷外乱トルク補償値を算出することにより、負荷外乱トルク補償値を精度よく求めることが可能になる。フィードフォワード補償値として位置指令補償値を用いることにより、速度指令値での補償が難しい制御系においても、負荷外乱トルクに対する動力学トルクフィードフォワード補償を行える。
【0012】
第２の制御方法において加え合わせ点への位置指令補償値の入力は、位置指令値への位置指令補償値の加算、位置偏差への位置指令補償値の加算、位置偏差に比例ゲインを乗じたものへの位置指令補償値の加算、及び、位置指令補償値のモータの位置からの減算、のいずれか１つによって行うことができる。このようにして制御系に対して位置指令補償値を入力することにより、位置指令補償値の入力の処理を容易なものとすることができる。
【0013】
第１及び第２の制御方法では、負荷外乱トルク補償値をフィードフォワード補償値（速度指令補償値、位置指令補償値）に変換するときに用いる伝達関数は、負荷側外乱入力から制御系内でのフィードフォワード補償値が入力される加え合わせ点までの伝達関数であって、分母に非零の定数項を含むものであることが好ましい。このような伝達関数を用いることにより、演算精度と位置制御性能とを維持しつつ、負荷外乱トルクの補償を十分に行うことできる。
【0015】
本発明の第１の制御装置は、外部から入力する位置指令値に基づいて電流指令値を生成してプラントに含まれるモータを制御する制御装置であって、位置指令値とモータの位置との差である位置偏差を算出する位置制御器と、位置制御器の出力に基づく速度指令値とモータの速度との差である速度偏差を算出し、少なくとも速度偏差を用いて電流指令値を生成する速度制御器と、速度偏差に対する積分動作を行う積分補償器と、位置指令値から負荷状態を予測する負荷状態算出部と、予測された負荷状態に基づく動力学トルクフィードフォワード補償の演算によって、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求め、プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する逆動力学計算部と、負荷外乱トルク補償値を、プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、速度指令値に対して補償を行う速度指令補償値に変換する指令変換手段と、を有し、速度指令補償値が、速度制御器において積分補償器よりも前段にある加え合わせ点に入力されることを特徴とする。
【0016】
本発明の第１の制御装置によれば、制御装置において制御偏差の積分補償器よりも前段にある加え合わせ点にフィードフォワード補償値である速度指令補償値を入力することにより、フィードフォワード補償値に誤差が含まれることや、積分補償器の積分値が増大することが防がれ、負荷側外乱トルクの補償を行うときに、演算精度と位置制御性能とを維持できる。また、負荷状態算出部と逆動力学計算部とを設けることによって、負荷外乱トルク補償値を精度よく求めることが可能になる。速度指令補償値をフィードフォワード補償値として用いることにより、位置指令値に対応するフィードフォワード補償値を求める場合に比べて計算に用いる伝達関数の次数を下げられるので、演算負荷を軽減することができる。
【0017】
第１の制御装置は、速度指令値に速度指令補償値を加算する加算要素、速度偏差に速度指令補償値を加算する加算要素、及び、モータの速度から速度指令補償値を減算する減算要素、のいずれか１つを備えるようにすることができる。このような構成によれば、加算要素あるいは減算要素を追加することで、簡単に速度指令補償値を入力できる。
【0018】
第１の制御装置では、速度制御器は、位置制御器の出力を入力して速度指令値を生成する前置補償器を備えてもよい。前置補償器を設けて速度制御系を２自由度制御系とすることにより、各軸の速度応答を一致させ、例えばロボットの軌道追従特性を改善することができる。
【0019】
本発明の第２の制御装置は、外部から入力する位置指令値に基づいて電流指令値を生成してプラントに含まれるモータを制御する制御装置であって、位置指令値とモータの位置との差である位置偏差を算出し、少なくとも位置偏差を用いて電流指令値を生成する位置速度制御器と、位置偏差に対する積分動作を行う積分補償器と、位置指令値から負荷状態を予測する負荷状態算出部と、予測された負荷状態に基づく動力学トルクフィードフォワード補償の演算によって、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求め、プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する逆動力学計算部と、負荷外乱トルク補償値を、プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、位置指令値に対して補償を行う位置指令補償値に変換する指令変換手段と、を有し、位置指令補償値が、位置速度制御器において積分補償器の前段にある加え合わせ点に入力されることを特徴とする。
【0020】
本発明の第２の制御装置によれば、制御装置において制御偏差の積分補償器よりも前段にある加え合わせ点にフィードフォワード補償値である位置指令補償値を入力することにより、フィードフォワード補償値に誤差が含まれることや、積分補償器の積分値が増大することが防がれ、負荷側外乱トルクの補償を行うときに、演算精度と位置制御性能とを維持できる。また、負荷状態算出部と逆動力学計算部とを設けることによって、負荷外乱トルク補償値を精度よく求めることが可能になる。位置指令補償値をフィードフォワード補償値として用いることにより、速度指令値での補償が難しい場合であっても、負荷外乱トルクに対する動力学トルクフィードフォワード補償を行える。
【0021】
第２の制御装置は、位置指令値に位置指令補償値を加算する加算要素、位置偏差に位置指令補償値を加算する加算要素、及び、モータの位置から位置指令補償値を減算する減算要素、のいずれか１つを備えるようにすることができる。このような構成によれば、加算要素あるいは減算要素を追加することで、簡単に位置指令補償値を入力できる。
【0022】
本発明の第３の制御装置は、外部から入力する位置指令値に基づいて電流指令値を生成してプラントに含まれるモータを制御する制御装置であって、位置指令値とモータの位置との差である位置偏差を算出する位置制御器と、位置制御器の出力に基づく速度指令値とモータの速度との差である速度偏差を算出し、少なくとも速度偏差を用いて電流指令値を生成する速度制御器と、速度偏差に対する積分動作を行う積分補償器と、位置指令値から負荷状態を予測する負荷状態算出部と、予測された負荷状態に基づく動力学トルクフィードフォワード補償の演算によって、他軸からの干渉力と、自軸の慣性モーメントのノミナルトルクからの変化分との和を求め、プラントに入力する負荷側外乱トルクを補償する補償値である負荷外乱トルク補償値を算出する逆動力学計算部と、負荷外乱トルク補償値を、プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するための、位置指令値に対して補償を行う位置指令補償値に変換する指令変換手段と、位置制御器の出力に対して位置指令補償値を加算する加算要素と、を有し、加算要素の出力が速度制御器に入力することを特徴とする。
【0023】
本発明の第３の制御装置によれば、制御装置において制御偏差の積分補償器よりも前段にある加え合わせ点にフィードフォワード補償値である位置指令補償値を入力することにより、フィードフォワード補償値に誤差が含まれることや、積分補償器の積分値が増大することが防がれ、負荷側外乱トルクの補償を行うときに、演算精度と位置制御性能とを維持できる。また、位置制御器の出力から後段の加え合わせ点（すなわち加算要素）に対して位置指令補償値を入力できる。
【0024】
第１乃至第３の制御装置では、指令変換手段において負荷外乱トルク補償値をフィードフォワード補償値（速度指令補償値、位置指令補償値）に変換するときに用いる伝達関数は、負荷側外乱入力から制御装置においてフィードフォワード補償値が入力される加え合わせ点までの伝達関数であって、分母に非零の定数項を含むものであることが好ましい。このような伝達関数を用いることにより、演算精度と位置制御性能とを維持しつつ、負荷外乱トルクの補償を十分に行うことできる。
【発明の効果】
【0025】
このように本発明によれば、負荷側外乱トルクの動力学トルクフィードフォワード補償を行うときに、演算精度と位置制御性能とを維持できる制御方法及び制御装置を提供することにある。
【図面の簡単な説明】
【0026】
【図1】本発明の第１の実施形態の制御装置を示すブロック線図である。
【図2】動力学トルクフィードフォワード補償により電流指令補償値を生成する従来の制御装置を示すブロック線図である。
【図3】第１の実施形態の制御装置の変形例を示すブロック線図である。
【図4】第１の実施形態の制御装置の別の変形例を示すブロック線図である。
【図5】第１の実施形態の制御装置のさらに別の変形例を示すブロック線図である。
【図6】本発明の第２の実施形態の制御装置を示すブロック線図である。
【図7】第２の実施形態の制御装置の変形例を示すブロック線図である。
【図8】第２の実施形態の制御装置の別の変形例を示すブロック線図である。
【図9】第２の実施形態の制御装置のさらに別の変形例を示すブロック線図である。
【発明を実施するための形態】
【0027】
次に、本発明の好ましい実施の形態について、図面を参照して説明する。図１は、本発明の第１の実施形態の制御装置をブロック線図により示すものであって、制御装置におけるロボットの軸ごとの構成を示している。この制御装置は、ロボットの軸ごとにモータ５３及び負荷（リンクまたはアーム）５９からなるプラント５０が構成されているとして、入力する位置指令値に基づいてプラント５０の位置を制御するために用いられるものであり、大別すると、位置指令値が入力して位置偏差に基づく指令を出力する位置制御器１０と、位置偏差に基づく指令に基づいて速度指令を求め、速度偏差を算出し、電流指令値を出力する速度制御器２０と、動力学トルクフィードフォワード補償を行ってフィードフォワード補償値を算出する動力学トルク補償値算出部４０と、を備えている。
【0028】
まず、制御装置による制御対象であるプラント５０について説明する。図示されるプラント５０は、モータ５３と負荷５８とからなる２慣性共振系として表されている。モータ５３と負荷５８との間には、減速比がＲgである減速機が設けられており、減速機のばね定数はＫsである。図においてＪmとＤmは、それぞれ、モータ５３の慣性モーメント及び粘性摩擦抵抗であり、ＪLとＤLは、それぞれ、負荷５８の慣性モーメント及び粘性摩擦抵抗である。負荷５８に対しては、負荷側外乱トルクも加わるものとする。負荷側外乱トルクには、他軸からの干渉と、姿勢変化に伴う自軸の慣性モーメントのノミナルトルクからの変化による寄与とが含まれる。図１に示すブロック線図によって説明すれば、電流指令値は、トルク定数Ｋtのゲイン要素５１によりトルクに変換され、トルクは、減速機に対応する減速要素６２からの出力を減算要素５２によって減じられてモータ５３に加えられる。図においてモータ５３の出力はモータ速度であり、それが積分要素５４を介することによってモータ位置に変換される。モータ５３の出力は、減速機に対応する減速要素５５と、減算要素５６と、積分要素６０と、ばね定数Ｋsに対応するゲイン要素６１とを介し、加算要素５７によって負荷側外乱トルクを加算されて負荷５８に加わる。負荷５８の出力である負荷速度は、積分要素５９を介することにより負荷側位置となる。ゲイン要素６１の出力は、減算要素６２に入力される。減算要素５６は、減速要素５５の出力から負荷速度を減算する。
【0029】
次に、本実施形態の制御装置の詳細について説明する。位置制御器１０には位置指令値が入力するともにモータ位置がフィードバックしている。位置制御器１０は、位置指令値からモータ位置を減算して位置偏差を算出する減算要素１１と、減算要素１１が出力する位置偏差に比例ゲインＫppを乗じて位置偏差に基づく指令とするゲイン要素１２とを備えている。
【0030】
速度制御器２０には、モータ速度が入力するとともに位置制御器１０から位置偏差に基づく指令が入力し、動力学トルク補償値算出部４０から速度指令補償値が入力する。動力学トルク補償値算出部４０と速度指令補償値については後述する。速度制御器２０は、位置偏差に基づく指令が入力して速度指令を出力する前置補償器２１と、前置補償器２１からの速度指令と速度指令補償値とを加算する加算要素３１と、加算要素３１の出力からモータ速度を減算して速度偏差を出力する減算要素２２と、速度偏差に対して速度ゲインＫvを乗ずるゲイン要素２３と、速度偏差が入力する積分要素２４と、積分要素２４の出力に積分ゲインＫiを乗ずるゲイン要素２５と、ゲイン要素２３の出力とゲイン要素２５の出力を加算する加算要素２６とを備えている。加算要素３１は、速度指令補償値を入力するための加え合わせ点である。さらに速度制御器２０は、モータ速度が入力する状態オブザーバ２７と、状態オブザーバ２７の出力に設けられたフィルタ２８と、加算要素２６の出力からフィルタ２８の出力とを減じて電流指令値とする減算要素２９と、を備えている。ゲイン要素２３，２５と積分要素２４とが設けられ、これらをまとめた伝達関数が（Ｋvｓ＋Ｋi）／ｓと表されることから、速度制御器２０はＰＩ（比例積分）制御を行っていることになる。積分要素２４とゲイン要素２５とは、制御偏差である速度偏差に対する積分補償器を構成している。また速度制御器２０は、状態オブザーバ２７に基づいた状態フィードバックによる制御も行っており、減算要素２９は、ＰＩ制御による制御量から状態フィードバックによる値を減算して電流指令値としている。一般的な制御理論から明らかなように、積分補償がないと、外乱によって位置指令値にモータ位置を一致させることはできないから、ＰＩ制御として位置偏差の算出から電流指令値の出力までの区間内に積分要素２４あるいは積分補償器を設けることが必要となる。
【0031】
前置補償器２１は、例えば双二次（ＦＦ（ｓ））の特性を有するものであり、非特許文献１に記載されるように、ＰＩ制御系と状態フィードバック制御系とを組み合わせて制振制御系を構成するために設けられており、２自由度制御系の構造を採用することにより各軸の速度応答を一致させ、ロボットの軌道追従特性を改善する。前置補償器２１は、必ずしも設けなくてもよいし、プラント５０の構成などに応じて双二次以外の特性のものであってもよい。前置補償器２１を設けない場合には、位置制御器１０からの位置偏差に基づく指令を速度指令として扱えばよい。
【0032】
次に、動力学トルク補償値算出部４０について説明する。動力学トルク補償値算出部４０は、位置指令値が入力して負荷状態の予測値を算出する負荷状態算出部４１と、負荷状態予測値から負荷外乱トルク補償値を生成する逆動力学計算部４２と、負荷外乱トルク補償値を速度指令補償値に変換する速度指令変換部４３とを備えている。速度指令補償値は、プラント５０における負荷側外乱トルクの入力から負荷側速度までの応答を相殺するためのフィードフォワード補償値の１つである。ロボットにおいて各軸の位置閉ループにおいて動力学補償がなされており、他軸からの干渉が完全に打ち消されているものとして、負荷状態算出部４１は、位置指令値を入力として、位置指令値から負荷速度までの系の伝達関数Ｗr（ｓ）に基づき、負荷５８の状態、すなわち負荷の位置、速度及び加速度を算出する。逆動力学計算部４２は、実際に動力学トルクフィードフォワード補償のための演算を行う部分であり、他軸からの干渉力と、姿勢変化に伴う自軸の慣性モーメントのノミナルトルクからの変化分であるトルク変動量とを計算し、他軸からの干渉力とトルク変動量の和として、負荷外乱トルク補償値を算出する。逆動力学計算部４２で行う演算については、例えば特許文献１や非特許文献１に記載されている。
【0033】
逆動力学計算部４２が出力する負荷外乱トルク補償値は、プラント５０に加わる負荷側外乱トルクに対応するものである。そこで本実施形態では速度指令変換部４３は、負荷側外乱トルクの入力から速度指令までの伝達関数Ｖtm（ｓ）に基づいて、負荷外乱トルク補償値を、負荷側外乱トルクによる負荷側速度の変化分を打ち消すために速度指令値に加えるべき速度指令補償値に変換する。本実施形態の制御装置によれば、負荷側外乱トルクによる負荷側速度の変化分を打ち消す速度指令補償値が速度指令に加算され、状態フィードバックと補償された速度指令とに基づいて電流指令値が生成してモータ５３が駆動されて負荷５８が動くので、負荷側外乱トルクの影響が低減されて、例えばマニピュレータの先端の軌跡誤差を低減することができる。この第１の実施形態では、動力学トルクフィードフォワード補償により速度指令補償値を生成するので、第１の実施形態における外乱トルクの補償方式を速度指令補償方式と呼ぶ。
【0034】
第１の実施形態の制御装置では、ロボットが位置決めを繰り返したときなどに、演算精度が低下したり位置制御性能が失われたりすることが防止される。以下、本実施形態の制御装置において演算精度の低下や位置制御性能の喪失が防がれることについて説明する。図１に示す制御装置では、負荷外乱トルク補償値を速度指令補償値に変換するときに速度指令変換部４３が使用する、負荷側外乱トルクの入力と速度指令の入力との間の伝達関数Ｖtm（ｓ）は、下記の式(1)のように定まる。式(1)において、ａv0～ａv5，ｂv0～ｂv3は、プラント５０などによって定まるパラメータである。ここでｂv0≠０である。
【0035】
【数1】
【0036】
式(1)で注目すべき点は、分母に非零の定数項ｂv0が存在することである。その結果、ｓ→０とする極限において、伝達関数Ｖtm（ｓ）は発散しない。すなわち、伝達関数Ｖtm（ｓ）の定常ゲインは有界である。なお速度指令変換部４３が用いる伝達関数Ｖtm（ｓ）は、プラント５０の構成や前置補償器２１の構成によっては必ずしも式(1)に示す形態となるわけではなく、分母、分子ともｓについての次数が式(1)に示すものとは異なることがある。しかしながら、補償値入力から負荷側速度までのフィードバック制御系において、ＰＩ制御のための積分要素２４が、補償側入力から負荷側速度までの前向き要素に含まれる限り、積分要素２４よりも位置指令値の入力側において速度指令値に加えられることとなる速度指令補償値を算出するための伝達関数Ｖtm（ｓ）の分母の定数項は、積分補償器のゲインの定数倍となり、非零となる。
【0037】
図２は、特許文献１あるいは非特許文献１に記載され、動力学トルクフィードフォワード補償により電流指令補償値を生成する制御装置の構成を示している。この制御装置の制御対象であるプラント５０は、図１に示したプラント５０と同じである。図２に示した制御装置は、図１に示した制御装置において速度指令補償値の代わりに電流指令補償値を発生するので、動力学トルク補償値算出部４０において速度指令変換部４３の代わりに電流指令変換部４９が設けられ、速度指令値に速度指令補償値を加算する加算要素３１の代わりに加算要素３９が設けられている点で、図１に示す制御装置とは異なっている。加算要素３９は、速度制御器２０とプラント５０の間に設けられて、速度制御器２０が出力する電流指令値に対して電流指令補償値を加算する。電流指令変換部４９は、負荷側外乱トルクの入力から電流指令値の入力までの伝達関数Ｔm（ｓ）に基づいて、負荷外乱トルク補償値を、負荷側外乱トルクを打ち消すために電流指令値に加えるべき電流指令補償値に変換する。このとき、伝達関数Ｔm（ｓ）は、下記の式(2)のように定まる。式(2)において、ａt0～ａv5，ｂt1～ｂt3は、プラント５０などによって定まるパラメータである。
【0038】
【数2】
【0039】
式(1)と式(2)とを比較すると、式(1)では伝達関数Ｖtm（ｓ）を表す式の分母に非零の定数項ｂs0が存在するのに対し、式(2)では伝達関数Ｔm（ｓ）を表す式の分母に非零の定数項が存在しない。伝達関数Ｔm（ｓ）は、ｓ→０の極限において収束せず、発散する。すなわち伝達関数Ｔm（ｓ）の定常ゲインは有界でない。伝達関数Ｔm（ｓ）の定常ゲインが有界でないのは、補償値入力から負荷側速度までのフィードバック制御系において、積分補償器（積分要素２４とゲイン要素２５）が、負荷側速度から補償値入力の加算点までフィードバック要素に含まれるため、積分補償器のゲインの定数倍が分母の定数項として現れないからである。伝達関数の定常ゲインが有界でないことが、図２に示すような制御装置において、ロボットが位置決めを繰り返したときなどに、演算精度が低下したり位置制御性能が失われたりすることの原因となる。
【0040】
定常ゲインが有界でないと、ロボットが位置決め動作を繰り返すたびに、電流指令変換部４９内の状態変数（積分値）が増大する。ロボットが停止し、かつ、重力がロボットの関節を回す方向には加わっていないときは外乱トルクは０であるべきであり、したがって、電流指令変換部４９からの電流指令補償値も０となるべきである。ここで、定常ゲインが有界でないことにより電流指令補償値が０とならなかった場合には、この電流指令補償値は、位置制御系にとっては外乱となる。この外乱を抑圧するために、速度制御器２０では、制御偏差の積分補償器（積分要素２４とゲイン要素２５）の積分値が増大する。電流指令変換部４９の内部の状態変数の値や、速度制御器２０の積分補償器の積分値は、制御装置を実現するソフトウェア上では有限のビット数を用いて保持される。そのため、状態変数の値や積分値が増大すると、演算精度の低下、制御信号の飽和、オーバーフローなどを引き起こし、最終的には位置制御性能が発揮できない状態となりうる。
【0041】
これに対し、図１に示した本実施形態の制御装置では、速度指令補償値の生成に用いられる伝達関数Ｖtm（ｓ）は、速度制御器２０内に設けられる積分補償器の寄与を含んでおり、有界な定常ゲインを有する。そのため、図２に示したような制御装置とは異なり、ロボットが停止しているときの外乱トルク補償値は０となり、速度指令補償値も０となり、速度指令変換部４３内の状態変数（積分値）は増大しない。また、速度制御器２０内の積分補償器の積分値が増大することも防がれる。その結果、本実施形態の制御装置では、ロボットの位置決め動作を繰り返した場合であっても、演算精度の低下、制御信号の飽和、オーバーフローなどが起こらず、位置制御性能が維持される。
【0042】
速度指令補償方式である第１の実施形態の制御装置には、図１に示したものには限定されない。図３は、第１の実施形態の制御装置の変形例の要部を示している。図３に示す装置は、基本的には図１に示すものと同様のものであるが、速度偏差を求めるための減算要素２２と、速度指令補償値を加算するための加算要素３１との配置の順番が、図１に示すものと反対になっている。当然のことながら、加算要素３１と減算要素２２の配置の順番を入れ替えても伝達関数としては等価であり、図４に示した制御装置によれば、図１に示した制御装置と同じ効果が得られる。
【0043】
図４は、第１の実施形態の制御装置の別の変形例の要部を示している。図４に示す制御装置では、速度偏差を求める減算要素２２に入力する前のモータ速度に対して速度指令補償値による補償を行っている。速度偏差の算出ではモータ速度は速度指令から減算されるものであるので、モータ速度から速度指令補償値を減算する減算要素３３を設け、モータ速度の代わりに減算要素３３の出力が減算要素２２に加えられる。減算要素３３も、速度指令補償値を入力するための加え合わせ点に含まれる。この構成は、伝達関数としては図１に示したものと等価であり、図１に示した制御装置と同じ効果を有する。
【0044】
図５は、第１の実施形態の制御装置のさらに別の変形例の要部を示している。この制御装置は、図３に示した制御装置から前置補償器２１を取り除いたものであり、位置制御器１０のゲイン要素１２から出力される位置偏差に基づく指令が、速度指令として、速度偏差を求める減算要素２２に直接供給されている。図５に示す制御装置では、速度偏差を求める減算要素２２と速度指令補償値を加算する加算要素３１との配置の順番を、図１に示すように逆にしてもよい。前置補償器２１を備えない場合であっても、速度指令補償方式による動力学トルクフィードフォワード補償を用いることによって、本発明の効果を達成することができる。図５に示す制御装置において速度指令変換部４３において用いる伝達関数Ｖtm（ｓ）は、負荷側外乱トルクの入力から減算要素２２の入力までの伝達関数であり、下記式(3)のように表される。式(1)と比べると、双二次の前置補償器２１を設けないことの影響が表れて、分母、分子とも次数が２ずつ低下している。ここでもｂv0≠０である。
【0045】
【数3】
【0046】
次に、本発明の第２の実施形態の制御装置について説明する。上述した第１の実施形態の制御装置は、速度指令補償方式として動力学トルクフィードフォワード補償により速度指令補償値を生成して、速度指令値や速度偏差に対する補償を行っている。しかしながら本発明はこれに限られるものでなく、位置指令補償方式として動力学トルクフィードフォワード補償により位置指令補償値を生成し、これに基づく補償を行うことも本発明に含まれる。速度指令補償値と同様に位置指令補償値も、フィードフォワード補償値の範疇に含まれるものである。以下、第２の実施形態の制御装置として、位置指令補償方式による制御装置を説明する。
【0047】
図６は本発明の第２の実施形態の制御装置を示している。この制御装置では、第１の実施形態の制御装置において位置制御器１０と速度制御器２０を設ける代わりに、位置偏差と状態フィードバックとに基づいて電流指令値を生成する位置速度制御器１５が設けられている。位置速度制御器１５は、位置指令値からモータ位置を減算して位置偏差を求めて出力する減算要素１１と、位置偏差が入力する積分要素２４と、積分要素２４の出力に積分ゲインＫiを乗ずるゲイン要素２５と、モータ速度が入力する状態オブザーバ２７と、状態オブザーバ２７の出力に設けられたフィルタ２８と、ゲイン要素２５の出力からフィルタ２８の出力とを減じて電流指令値とする減算要素２９と、を備えている。動力学トルク補償値算出部４０では、第１の実施形態における速度指令変換部４３の代わりに位置指令変換部４４が設けられている。位置指令変換部４４は、負荷側外乱トルクの入力と位置指令の入力との間の伝達関数Ｐtm（ｓ）に基づいて、負荷外乱トルク補償値を、負荷側外乱トルクを打ち消すために位置指令値に加えるべき位置指令補償値に変換する。位置速度制御器１５の入力側には、外部から与えられる位置指令値に対して位置指令補償値を加算する加算要素３２が設けられており、加算要素３２において位置指令補償値が加算された位置指令値が、位置速度制御器１５内の減算要素１１に与えられるようになっている。加算要素３２は、位置指令補償値を入力するための加え合わせ点である。
【0048】
図６に示す制御装置においても、負荷側外乱トルクを打ち消す位置指令補償値が位置指令に加算され、状態フィードバックと補償された位置指令とに基づいて電流指令値が生成してモータ５３が駆動されて負荷５８が動くので、負荷側外乱トルクの影響が低減されて、例えばマニピュレータの先端の軌跡誤差を低減することができる。位置指令変換部４４が用いる伝達関数Ｐtm（ｓ）は、下記の式(4)のように定まる。式(4)において、ａp0～ａp7，ｂp0～ｂv5は、プラント５０などによって定まるパラメータである。ここでｂp0≠０である。
【0049】
【数4】
【0050】
式(4)においても、分母に非零の定数項ｂp0が存在し、ｓ→０とする極限において伝達関数Ｐtm（ｓ）は発散しない。本実施形態の制御装置では、伝達関数Ｐtm（ｓ）は、積分補償器のゲインの定数倍が分母の定数項として現れ、有界な定常ゲインを有する。そのため、図２に示したような制御装置とは異なり、ロボットが停止しているときの位置指令補償値は０となり、位置指令変換部４４内の状態変数（積分値）が増大することが防がれる。また、位置速度制御器１５内の積分補償器の積分値が増大することも防がれる。その結果、本実施形態の制御装置では、ロボットの位置決め動作を繰り返した場合であっても、演算精度の低下、制御信号の飽和、オーバーフローなどが起こらず、位置制御性能が維持される。
【0051】
位置指令補償方式である第２の実施形態の制御装置には、図６に示したものには限定されない。図７は、第２の実施形態の制御装置の変形例の要部を示している。図７に示す装置は、基本的には図６に示すものと同様のものであるが、位置偏差を求めるための減算要素１１と、位置指令補償値を加算するための加算要素３２との配置の順番が、図７に示すものと反対になっており、加算要素３２が位置速度制御器１５内に組み込まれている。当然のことながら、加算要素３２と減算要素１１の配置の順番を入れ替えても伝達関数としては等価であり、図７に示した制御装置によれば、図６に示した制御装置と同じ効果が得られる。
【0052】
図８は、第２の実施形態の制御装置の別の変形例の要部を示している。図８に示す制御装置では、位置速度制御器１５において、位置偏差を求める減算要素１１に入力する前のモータ位置に対して位置指令補償値による補償を行っている。位置偏差の算出ではモータ位置は位置指令から減算されるものであるので、モータ位置から位置指令補償値を減算する減算要素３４を設け、モータ位置の代わりに減算要素３４の出力が減算要素１１に加えられる。減算要素３４も、位置指令補償値を入力するための加え合わせ点に含まれる。この構成は、伝達関数としては図６に示したものと等価であり、図６に示した制御装置と同じ効果を有する。
【0053】
図９は、第２の実施形態の制御装置のさらに変形例の要部を示している。図９に示す制御装置は、第１の実施形態のように位置制御器１０と速度制御器２０とが設けられ、速度制御器２０には前置補償器２１が設けられている場合に、どのように位置指令補償値による補償を行うかを示している。位置制御器１０の出力と速度制御器２０の入力との間に、位置制御器１０の出力（すなわち、位置偏差に比例ゲインＫppを乗じて得られた指令）に対して位置指令補償値を加算するように、加算要素３２が設けられている。この場合、位置指令変換部４４で用いる伝達関数Ｐtm（ｓ）は、負荷側外乱トルクの入力から速度制御器２０の入力までの伝達関数であり、伝達関数の経路内に前置補償器２１が含まれているので、伝達関数の次数は図６に示した制御装置での伝達関数の次数とは異なる。図９に示す制御装置では、位置指令補償値を加算するための加算要素３２の位置を、位置制御器１０の出力側でなく入力側とすることもでき、その場合の位置指令変換部４４で用いる伝達関数Ｐtm（ｓ）は、比例ゲインＫppを含むか含まないかだけの違いとなる。図９に示す制御装置においても、動力学トルクフィードフォワード補償によって生成した位置指令補償値に基づく補償を行うので、図６に示す制御装置と同様の効果を得ることができる。
【0054】
以上説明したように本発明の各実施形態の制御装置は、位置指令値に基づいてプラント５０を制御するものであって、動力学トルクフィードフォワード補償により負荷外乱トルク補償値を算出し、制御系内においてＰＩ制御を行うための制御偏差の積分補償器よりも位置指令値の入力側にある加え合わせ点（加算要素３１，３２及び減算要素３３，３４のいずれか）において補償を行うためのフィードフォワード補償値（速度指令補償値あるいは位置指令補償値）に、負荷外乱トルク補償値を変換するものである。負荷外乱トルク補償値のフィードフォワード補償値への変換には、負荷側外乱トルクの入力からフィードフォワード補償値による補償を行う加え合わせ点までの伝達関数を使用する。この伝達関数はその分母に非零の定数項を含むので、例えばロボットのマニピュレータ先端の軌跡誤差の低減を目的として動力学トルクフィードフォワード補償により負荷側外乱トルクの補償を行うときに、制御偏差の積分補償器における積分値が増大することが防がれ、また、ロボットが動作を停止しているときにフィードフォワード補償値が０となる。その結果、各実施形態に示される制御装置によれば、演算精度と位置制御性能とを維持できる。
【0055】
本発明に基づく制御装置を例えば、搬送用の多関節ロボットに適用した場合、搬送物の質量が切り替わるときに、逆動力学計算部４２は、ロボット手先の慣性モーメントを切り替えてトルク変動値を算出することができる。これにより、位置制御系の慣性モーメントノミナル値を切り替えるよりも容易かつ安定に多関節ロボットを制御することが可能になる。
【0056】
本発明に基づく制御装置が対象とするロボットは、２軸以上のものに限定されず、１軸のものであってもよい。１軸であってかつ位置指令値が与えられるという観点からすれば、例えば単軸のベルトコンベアも１軸のロボットの範疇に含まれ、ベルトコンベアにおけるベルト上の搬送物の速度をマニピュレータの先端の速度として扱うことができる。すなわち、本発明は単軸のサーボアンプにおける外乱トルクの補償にも使用することができる。単軸のベルトコンベアに対して本発明に基づく制御方法を適用するとすれば、搬送物の積み下ろしにより負荷慣性が変動する場合に、制御上の慣性モーメントを加速するための慣性力と実際の慣性モーメントを加速するための推力の変動分を負荷側力補償値とし、負荷側外力から負荷側速度への応答を相殺するための速度指令補償値に負荷側力補償値を変換し、速度指令値に加算する。これにより、位置制御系の慣性モーメントノミナル値を切り替えるよりも容易かつ安定に単軸のベルトコンベアを制御することが可能になる。
【符号の説明】
【0057】
１０…位置制御器、１５…位置速度制御器、２０…速度制御器、２１…前置補償器、２７…状態オブサーバ、２８…フィルタ、４０…動力学トルク補償値算出部、４１…負荷状態算出部、４２…逆動力学計算部、４３…速度指令変換部、４４…位置指令変換部、４９…電流指令変換部、５０…プラント、５３…モータ、５８…負荷。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】