特開 2024-17883 動力学トルク補償に基づく制御方法及び制御装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024017883

(43)【公開日】2024-02-08

(54)【発明の名称】動力学トルク補償に基づく制御方法及び制御装置

(51)【国際特許分類】

   G05B 11/32 20060101AFI20240201BHJP

   G05B 11/36 20060101ALI20240201BHJP

   B25J 9/16 20060101ALI20240201BHJP

【ＦＩ】

G05B11/32 F

G05B11/36 F

G05B11/36 D

G05B11/36 507H

B25J9/16

【審査請求】未請求

【請求項の数】10

【出願形態】ＯＬ

(21)【出願番号】P 2022120828

(22)【出願日】2022-07-28

(71)【出願人】

【識別番号】000002233

【氏名又は名称】ニデックインスツルメンツ株式会社

(74)【代理人】

【識別番号】100123788

【弁理士】

【氏名又は名称】宮崎 昭夫

(74)【代理人】

【識別番号】100127454

【弁理士】

【氏名又は名称】緒方 雅昭

(72)【発明者】

【氏名】桃澤 義秋

(72)【発明者】

【氏名】花岡 正志

【テーマコード（参考）】

3C707

5H004

【Ｆターム（参考）】

3C707KS21

3C707KS24

3C707KS35

3C707LW01

5H004GA13

5H004GA21

5H004GB16

5H004HA07

5H004HA09

5H004HA10

5H004HB07

5H004HB09

5H004HB10

5H004JA21

5H004KB02

5H004KB04

5H004KB06

5H004KB32

5H004KB33

5H004KC55

5H004KC56

(57)【要約】

【課題】動力学トルクフィードフォワード補償を行うときに、負荷外乱トルク補償値を算出する逆動力学計算を行う前に実行される軸ごとの負荷状態量の算出の演算負荷を低減する。
【解決手段】プラントが複数の軸を有するときに、軸ごとに、ノミナルな２慣性系を対象としたときのその軸の位置指令値から負荷側位置までの伝達関数を分母が４次、分子が０次である希望伝達関数で表現し、この希望伝達関数を使用してその軸の負荷状態量を予測する。プラントの複数の軸のそれぞれの予測された負荷状態量に基づいてプラントに関する逆動力学演算を実施し、各軸の負荷外乱トルク補償値を算出する。
【選択図】 図６

【特許請求の範囲】

【請求項1】

複数の軸を有するプラントにおいて軸ごとの位置指令値に基づいて各軸のモータを制御する制御方法であって、
軸ごとの負荷外乱トルク補償値を算出するステップと、
軸ごとに、当該軸の前記負荷外乱トルク補償値を、前記プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するためのフィードフォワード補償値に変換するステップと、
軸ごとに、当該軸の前記フィードフォワード補償値を、制御系において前記位置指令値の入力位置から前記モータへの電流指令値を出力する位置までの区間にある加え合わせ点に入力するステップと、
を有し、
前記軸ごとの負荷外乱トルク補償値が、ノミナルな２慣性系を対象としたときの当該軸の前記位置指令値から負荷側位置までの伝達関数を分母が４次、分子が０次である希望伝達関数で表現し、前記希望伝達関数を使用して当該軸の負荷状態量を予測し、前記複数の軸について予測された前記負荷状態量に基づいて前記プラントに関する逆動力学演算を実施することによって算出される、制御方法。

【請求項2】

軸ごとに、前記位置指令値に対する前置補償を実行し、
前記前置補償の伝達関数は、前記希望伝達関数を、前記前置補償を行わないとしたときの当該軸の位置指令値から負荷側位置までの伝達関数によって除したものである、請求項１に記載の制御方法。

【請求項3】

軸ごとに、前記モータの位置偏差の算出の前に前記前置補償を実行する、請求項２に記載の制御方法。

【請求項4】

前記加え合わせ点は制御偏差の積分補償器より前段にある、請求項１乃至３のいずれか１項に記載の制御方法。

【請求項5】

前記位置指令値と前記モータの位置との差である位置偏差を算出し、前記位置偏差に基づく速度指令値と前記モータの速度との差である速度偏差を算出し、少なくとも前記速度偏差を用いて前記電流指令値を生成し、
前記フィードフォワード補償値は、前記速度指令値に対して補償を行う速度指令補償値である、請求項４に記載の制御方法。

【請求項6】

複数の軸を備えるプラントにおいて軸ごとの位置指令値に基づいて電流指令値を生成し、各軸のモータを制御する制御装置であって、
軸ごとの負荷外乱トルク補償値を算出する負荷外乱トルク補償値算出手段と、
軸ごとに設けられ、当該軸の前記負荷外乱トルク補償値を、前記プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するためのフィードフォワード補償値に変換する指令変換手段と、
を有し、
前記負荷外乱トルク補償値算出手段は、
軸ごとに設けられ、ノミナルな２慣性系を対象としたときの当該軸の前記位置指令値から負荷側位置までの伝達関数を分母が４次、分子が０次である希望伝達関数で表現して前記希望伝達関数を使用して当該軸の負荷状態量を予測する負荷状態算出手段と、
前記複数の軸について予測された前記負荷状態量に基づいて前記プラントに関する逆動力学演算を実施して各軸の前記負荷外乱トルク補償値を算出する逆動力学計算手段と、
を有し、
前記フィードフォワード補償値が、前記制御装置の入力から前記電流指令値を出力する位置までの区間にある加え合わせ点に入力される、制御装置。

【請求項7】

前記制御装置は、前記複数の軸に共通に設けられる全軸指令算出基板と、軸ごとに設けられるモータ駆動制御基板とを有し、
前記負荷外乱トルク補償値算出手段は前記全軸指令算出基板に設けられ、
前記指令変換手段は、対応する軸のモータ駆動制御基板に設けられている、請求項６に記載の制御装置。

【請求項8】

前記モータ駆動制御基板に設けられ、対応する軸の前記位置指令値に対する前置補償を実行する前置補償器をさらに備え、
前記前置補償の伝達関数は、前記希望伝達関数を、前記前置補償器が設けられないとしたときの当該軸の位置指令値から負荷側位置までの伝達関数によって除したものである、請求項７に記載の制御装置。

【請求項9】

前記位置指令値と前記モータの位置との差である位置偏差を算出する位置制御器を備え、
前記前置補償器は、前記位置制御器よりも前記位置指令値の入力位置の側に設けられている、請求項８に記載の制御装置。

【請求項10】

前記位置制御器の出力に基づく速度指令値と前記モータの速度との差である速度偏差を算出し、少なくとも前記速度偏差を用いて前記電流指令値を生成する速度制御器と、
制御偏差に対する積分動作を行う積分補償器と、
を有し、
前記フィードフォワード補償値は、前記速度指令値に対して補償を行う速度指令補償値であって、前記積分補償器の前段にある前記加え合わせ点に入力する、請求項９に記載の制御装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、産業用ロボット（以下、「ロボット」とも称する）などの動作制御などに用いられる、動力学トルク補償に基づく制御方法及び制御装置に関する。

【0002】

一般にロボットは複数の関節軸（以下、単に「軸」とも称する）を有し、各軸のリンクが直列に接続されてマニピュレータを構成している。関節軸に減速機を有するロボットを高速で動かした場合には、関節軸ねじれによりマニピュレータの先端が振動しやすく、軸間干渉力と関節軸ねじれとによってマニピュレータ先端の軌跡に誤差が生じる。マニピュレータ先端の軌跡誤差を低減するために、お互いの動きによらずに各関節軸モータの位置指令に対する各関節軸角度（すなわち負荷の位置）の応答性を同じにすることが必要である。特許文献１は、動力学トルクフィードフォーワード補償により、各軸の駆動トルクを自軸分のノミナルトルクに近付け、全ての軸について位置制御応答性を同じにすることを開示している。特許文献１に記載された制御装置では、モータの位置指令値から関節軸ねじれよりも負荷側の速度（関節角速度）までの伝達関数に基づいて、モータの位置指令値を入力として関節軸ねじれよりも負荷側の状態量（関節の角度、角速度及び角加速度）を算出する負荷状態算出部と、算出された状態量を入力として負荷側の外乱トルク補償値を算出する逆動力学計算部と、外乱トルク補償値を、負荷側の外乱トルクから負荷側速度までの応答を相殺するための電流指令補償値に変換する電流指令変換部と、が設けられ、制御装置における電流指令値に電流指令補償値が加算されている。ここでトルク補償値は、他軸からの干渉力と、姿勢変化に伴う自軸の慣性モーメントのノミナルトルクからの変化分であるトルク変動量との和で表される負荷外乱トルク補償値である。

【0003】

特許文献１などに示される、動力学トルクフィードフォワード補償を用いて電流指令値補償値を算出し、算出された電流指令値補償値を電流指令値に加算する制御方法では、ロボットが位置決めを繰り返したときなどに、演算精度が低下することにより位置制御性能が失われるおそれがある。動力学トルクフィードフォワード補償におけるこのような課題を解決するために、特許文献２は、負荷外乱トルク補償値を、プラントにおける負荷側外乱から負荷側速度までの応答を相殺するためのフィードフォワード補償値（例えば速度指令補償値）に変換し、そのフィードフォワード補償値を、制御系において外部からの指令の入力位置からモータへの電流指令値を生成するまでの区間であって制御偏差の積分補償器よりも前段にある加え合わせ点に入力することを開示している。

【0004】

ところで、ロボットなどのプラントに対する動作制御を行なうときに用いられる制御装置（コントローラ）は、プラントにおいて複数の軸が設けられているとして、上位装置からの動作指令に基づいて各軸のモータに対する位置指令値を生成し、軸ごとの位置指令値に基づいて当該軸のモータに対するサーボ制御を行なう。モータのサーボ制御を行なう部分には、モータを駆動するインバータ回路などの電力系の回路も含まれる。そこで制御装置の内部では、モータの軸ごとにそのモータのサーボ制御などに必要な回路を集積したモータ駆動制御基板と、上位装置からの動作指令などに基づいて軸ごとの位置指令値などを生成するために用いられる回路などを集積した全軸指令算出基板とを別個に配置し、モータ駆動制御基板と全軸指令算出基板の各々にマイクロプロセッサなどを設けて制御に必要な演算を行わせることが一般的である。特許文献３は、複数のリンクをベース側から順次接続させた型の機構を有するロボットを制御するコントローラにおいて、軸ごとに設けられるプロセッサにおいて軸ごとの干渉トルクを演算するときの演算負荷を平準化するために、ロボットのベース側に相対的に近いリンクを駆動する軸のうちの少なくとも１つの軸に関し、その軸に対する干渉トルク計算処理をその軸で駆動されるリンクよりもベース側から相対的に遠いリンクを駆動する軸に対応して設けられたプロセッサにおいて実行することを開示している。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特許第４００８２０７号公報

【特許文献2】特開２０２２－２６０６９号公報

【特許文献3】特開平８－１０６３１２号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

特許文献１，２に記載された技術は、いずれも、位置指令値から負荷側速度までの伝達関数に基づいて、位置指令値を入力として負荷状態量を算出する負荷状態算出部と、算出された負荷状態量を入力として負荷側のトルク補償値（負荷外乱トルク補償値）を軸ごとに算出する逆動力学計算部と、負荷側の外乱トルクから負荷側速度までの応答を相殺するために、逆動力学計算部で得られた負荷外乱トルク補償値を何らかのフィードフォワード補償値に変換する指令変換部を備え、得られたフィードフォワード補償値を対応する軸の対応する指令値に加算するものである。指令変換部での演算では他軸のフィードフォワード補償値を使用しないため、指令変換部を軸ごとのモータ駆動制御基板に設けることができる。これに対して逆動力学演算は全ての軸における指令値を必要とするため、逆動力学演算部は全軸指令算出基板に設けられる必要がある。負荷状態算出部は軸ごとに負荷状態量を算出するが、逆動力学演算部よりも前段に設けられる必要があるので、これも全軸指令算出基板に設けられる必要がある。なお、特許文献１，２に記載された技術では、位置指令値から負荷側速度までの伝達関数の代わりに位置指令値から負荷側位置までの伝達関数を用いてもよい。

【0007】

プラントの各軸は減速機を介してモータが負荷を駆動する構成であることが一般的であるので、一例として負荷状態算出部は、ノミナルな２慣性共振系を制御対象としたときの、各軸の位置制御系における位置指令値から負荷側位置までの伝達関数Ｗ_r（ｓ）として設計される。積分補償器を１つ含む制御ループを２慣性共振系に組み込んで構成された位置速度制御系においては、状態オブザーバの状態推定精度が十分であって誤差を無視すると、伝達関数Ｗ_r（ｓ）の分母の次数は最小で５次であり、分子の次数は最大で５次である。全軸指令算出基板ではこのような伝達関数Ｗ_r（ｓ）の計算を関節軸の数だけ行う必要があるので、全軸指令算出基板における演算負荷が大きくなる。制御装置において全軸指令算出基板とモータ駆動制御基板とが構造的に別個のものとなっていないとしても、計算負荷の分散のために、全軸を対象とした制御の計算を行うプロセッサと軸ごとにモータのサーボ制御のための計算を行うプロセッサとが分離されていることが一般的であり、伝達関数Ｗ_r（ｓ）の計算のために前者のプロセッサの演算負荷が大きくなることは同様である。

【0008】

本発明の目的は、動力学トルクフィードフォワード補償を行う制御方法及び制御装置であって、トルク補償値を算出する逆動力学計算を行う前に実行される軸ごとの負荷状態量の算出の演算負荷を低減できる制御方法及び制御装置を提供することにある。

【課題を解決するための手段】

【0009】

本発明の一態様によれば、複数の軸を有するプラントにおいて軸ごとの位置指令値に基づいて各軸のモータを制御する制御方法は、軸ごとの負荷外乱トルク補償値を算出するステップと、軸ごとその軸の負荷外乱トルク補償値を、プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するためのフィードフォワード補償値に変換するステップと、軸ごとに、その軸の前記フィードフォワード補償値を、制御系において位置指令値の入力位置からモータへの電流指令値を出力する位置までの区間にある加え合わせ点に入力するステップと、を有する。この制御方法では、軸ごとの負荷外乱トルク補償値は、ノミナルな２慣性系を対象としたときのその軸の前記位置指令値から負荷側位置までの伝達関数を分母が４次、分子が０次である希望伝達関数で表現し、希望伝達関数を使用してその軸の負荷状態量を予測し、複数の軸について予測された負荷状態量に基づいてプラントに関する逆動力学演算を実施することによって算出される。

【0010】

上記態様の制御方法では、動力学トルクフィードフォワード補償を行うときに必要となる負荷外乱トルク補償値の算出において、分母が４次、分子が０次である希望伝達関数を使用して負荷状態量を予測することにより、逆動力学計算を行う前に実行される軸ごとの負荷状態量の算出の演算負荷を低減できる。

【0011】

一態様の制御方法では、軸ごとに、位置指令値に対する前置補償を実行することが好ましい。前置補償で用いる伝達関数は、前置補償を行わないとしたときの位置指令値から負荷側位置までの伝達関数によって希望伝達関数を除したものとすることが好ましい。このような前置補償を行うことによって、位置指令値から負荷側位置までの実際の伝達関数と希望伝達関数との差異を吸収できる。前置補償を実行するときは、軸ごとに、モータの位置偏差の算出の前に前置補償を実行することが好ましい。位置偏差の算出の前に位置フィードフォワード補償として前置補償を行うことにより、制御の精度を高めつつ演算負荷の増加を抑えることができる。

【0012】

一態様の制御方法では、加え合わせ点は制御偏差の積分補償器より前段にあることが好ましい。このように構成することにより、フィードフォワード補償値に誤差が含まれることや、積分補償器の積分値が増大することが防がれ、負荷側外乱トルクの補償を行うときに、演算精度と位置制御性能とを維持できる。この場合、位置指令値とモータの位置との差である位置偏差を算出し、位置偏差に基づく速度指令値とモータの速度との差である速度偏差を算出し、少なくとも速度偏差を用いて電流指令値を生成することでき、このように電流指令値を生成するときは、速度指令値に対して補償を行う速度指令補償値をフィードフォワード補償値とすることができる。速度指令補償値をフィードフォワード補償値とすることにより、負荷側外乱トルクの補償のために位置指令値に対応するフィードフォワード補償値を求める場合に比べて計算に用いる伝達関数の次数を下げることができるので、演算負荷を低減できる。

【0013】

本発明の別の態様によれば、複数の軸を備えるプラントにおいて軸ごとの位置指令値に基づいて電流指令値を生成し、各軸のモータを制御する制御装置は、軸ごとの負荷外乱トルク補償値を算出する負荷外乱トルク補償値算出手段と、軸ごとに設けられ、その軸の負荷外乱トルク補償値を、プラントにおける負荷側外乱入力から負荷側速度までの応答を相殺するためのフィードフォワード補償値に変換する指令変換手段と、を有する。ここで負荷外乱トルク補償値算出手段は、軸ごとに設けられた、ノミナルな２慣性系を対象としたときのその軸の位置指令値から負荷側位置までの伝達関数を分母が４次、分子が０次である希望伝達関数で表現して希望伝達関数を使用してその軸の負荷状態量を予測する負荷状態算出手段と、複数の軸について予測された負荷状態量に基づいてプラントに関する逆動力学演算を実施して各軸の負荷外乱トルク補償値を算出する逆動力学計算手段と、を有し、フィードフォワード補償値は、制御装置の入力から電流指令値を出力する位置までの区間にある加え合わせ点に入力される。

【0014】

上記態様の制御装置では、動力学トルクフィードフォワード補償を行うときに必要となる負荷外乱トルク補償値を算出する負荷外乱トルク補償値算出手段において、分母が４次、分子が０次である希望伝達関数を使用して負荷状態量を予測することにより、逆動力学計算を行う前に実行される軸ごとの負荷状態量の算出の演算負荷を低減できる。

【0015】

一態様の制御装置では、制御装置が複数の軸に共通に設けられる全軸指令算出基板と、軸ごとに設けられるモータ駆動制御基板とを有するときに、負荷外乱トルク補償値算出手段を全軸指令算出基板に設け、指令変換手段を対応する軸のモータ駆動制御基板に設けることができる。このように構成することにより、一般に各モータ駆動制御基板に比べて演算負荷が大きい全軸指令算出基板において、負荷状態量の算出に要する演算負荷を低減することができる。

【0016】

一態様の制御装置では、対応する軸の位置指令値に対する前置補償を実行する前置補償器をモータ駆動制御基板に設けることが好ましく、その場合、前置補償器が設けられないとしたときの位置指令値から負荷側位置までの伝達関数によって希望伝達関数を除したものを前置補償の伝達関数とすることが好ましい。このように前置補償器を設けることにより、位置指令値から負荷側位置までの実際の伝達関数と希望伝達関数との差異を吸収できる。制御装置が位置指令値とモータの位置との差である位置偏差を算出する位置制御器を備えるときは、前置補償器は、位置制御器よりも位置指令値の入力位置の側に設けられることが好ましい。位置制御器の前段に前置補償器を設けることにより、制御の精度を高めつつ演算負荷の増加を抑えることができる。

【0017】

一態様の制御装置は、位置制御器の出力に基づく速度指令値とモータの速度との差である速度偏差を算出し、少なくとも速度偏差を用いて電流指令値を生成する速度制御器と、制御偏差に対する積分動作を行う積分補償器と、を備えていてもよく、その場合、フィードフォワード補償値は、速度指令値に対して補償を行う速度指令補償値であって、積分補償器の前段にある加え合わせ点に入力されることが好ましい。速度指令補償値をフィードフォワード補償値とすることにより、負荷側外乱トルクの補償のために位置指令値に対応するフィードフォワード補償値を求める場合に比べて計算に用いる伝達関数の次数を下げることができるので、演算負荷を低減できる。

【発明の効果】

【0018】

本発明によれば、動力学トルクフィードフォワード補償を行う際に分母が４次、分子が０次である希望伝達関数を用いて各軸の負荷状態量を予測するので、トルク補償値を算出する逆動力学計算を行う前に行う負荷状態量算出の演算負荷を低減することができる。

【図面の簡単な説明】

【0019】

【図1】ロボットシステムのハードウェア構成を示す図である。

【図2】プラントであるロボットを示すブロック線図である。

【図3】従来の制御装置の構成の一例を示すブロック線図である。

【図4】本発明の第１の実施形態の制御装置の構成を示すブロック線図である。

【図5】本発明の原理を説明するブロック線図である。

【図6】１軸分の制御系を示すブロック線図である。

【図7】従来の制御装置の構成の別の例を示すブロック線図である。

【図8】第２の実施形態の制御装置での１軸分の制御系を示すブロック線図である。

【発明を実施するための形態】

【0020】

次に、本発明の好ましい実施の形態について、図面を参照して説明する。以下では、制御対象であるプラントが多軸のロボットであり、このロボットを制御する制御装置（ロボットコントローラ）において動力学フィードフォワード補償を実行する場合を例に挙げて、本発明を実施するための各形態を説明する。まず、制御装置と制御対象であるロボットからなるロボットシステムについて説明する。図１はロボットシステムのハードウェア構成の一例を示している。

【0021】

ロボット１０は、３リンク構成のものであり、ベース（不図示）に対して関節１を介して第１リンクの一端が接続し、第１リンクの他端に対して関節２を介して第２リンクの一端が接続し、第２リンクの他端に対して関節３を介して手先の一端が接続しているものである。ロボット１０において第１リンク、第２リンク及び手先からなる部分をマニピュレータ１１と総称する。関節１～３の各々は、モータ１２から減速機１３を介して駆動される。各軸のモータ１２には、そのモータ１２の現在位置を検出するエンコーダ１４が取り付けられている。軸ごとに、モータ１２が出力するトルクが減速機１３を介してその軸の関節に伝達される。以下の説明において、関節１に係る軸を第１軸、関節２に係る軸を第２軸のように呼ぶこととする。

【0022】

一方、制御装置１５は、マニピュレータ１１の手先位置などを指令する動作指令が上位装置から入力されて各軸のモータ１２に対する位置指令値と各軸の負荷外乱トルク値とを算出する回路が設けられた全軸指令算出基板１６と、ロボット１０の軸ごとに対応して設けられたモータ駆動制御基板１７とを備えている。各軸のモータ駆動制御基板１７には、その軸のモータ１２に接続するエンコーダ１４からモータ位置検出値がフィードバックされている。各軸のモータ駆動制御基板１７は、全軸指令算出基板１６から受け取った位置指令値とフィードバックされたモータ位置検出値とに基づいてその軸のモータ１２のサーボ制御を行なうものであるが、サーボ制御の演算を行うときに、全軸指令算出基板１６から受け取った負荷外乱トルク補償値に基づいて、その軸についての負荷外乱トルクの補償も行う。さらに各軸のモータ駆動制御基板１７には、サーボ制御のための演算結果に基づいてその軸のモータ１２に対する駆動電流を生成するインバータ回路なども設けられている。

【0023】

図２は、プラントであるロボット１０の力学系の構成を示すブロック線図である。図２においてロボット１０は、第１軸、第２軸などの軸ごとにモータ１２と負荷２８とから２慣性系として表されている。モータ１２を駆動する電流の代わりに電流指令値がモータ１２に入力されるものとしている。図では、伝達関数を構成する要素を表現するときに第ｉ軸（ｉ＝１，２，…）の要素に対して添え字ｉを用いることによって、どの軸に対する要素であるかを明示しているが、ブロック線図上では各軸についての構成は同一であるので、以下では、添え字ｉを用いることなくすなわち軸の違いを明示せずに説明を行う。各軸において、減速比がＲ_gである減速機１３が設けられており、減速機１３のばね定数はＫ_sである。図においてＪ_mとＤ_mは、それぞれ、モータ１２の慣性モーメント及び粘性摩擦抵抗であり、Ｊ_LとＤ_Lは、それぞれ、負荷２８の慣性モーメント及び粘性摩擦抵抗である。負荷２８に対しては、負荷側外乱トルクも加わるものとする。負荷側外乱トルクには、他軸からの干渉と、姿勢変化に伴う自軸の慣性モーメントのノミナルトルクからの変化による寄与とが含まれる。

【0024】

図２に示すブロック線図によって説明すれば、制御装置１５から出力された電流は、トルク定数Ｋ_tのゲイン要素２１によりトルクに変換され、トルクは、減速機１３に対応する減速要素３２からの出力を減算要素２２によって減じられてモータ１２に加えられる。図においてモータ１２の出力はモータ速度であり、それが積分要素２４を介することによってモータ位置（すなわちモータ１２の回転角度）に変換される。モータ１２の出力は、減速機に対応する減速要素２５と、減算要素２６と、積分要素３０と、ばね定数Ｋ_sに対応するゲイン要素３１とを介し、加算要素２７によって負荷側外乱トルクを加算されて負荷２８に加わる。負荷２８の出力である負荷側速度は、積分要素２９を介することにより負荷側位置となる。ゲイン要素３１の出力は、減速要素３２にも入力される。減算要素２６は、減速要素２５の出力から負荷側速度を減算する。

【0025】

負荷２８において、粘性摩擦抵抗Ｄ_Lに対応するゲイン要素３６に対して負荷側速度が入力し、減算要素３７によって負荷２８への入力からゲイン要素３６の出力を減算したものが、負荷側位置θ_Lによって変化する慣性モーメントＪ_Lに対応するゲイン要素３８に入力する。慣性モーメントＪ_Lに対応するゲイン要素３８の出力が、関節角加速度である。この関節角加速度を積分要素３９によって積分したものが、負荷２８の出力である負荷側速度となる。負荷２８における関節角加速度は、他軸への干渉トルクの原因となる。Ｎ個の軸を有するロボットにおいて、各軸の関節角加速度とこれらの関節角加速度から発生して各軸に作用する干渉トルクとの関係はＮ×Ｎ行列を含む式で表すことができるので、この式を図において逆動力学３５として示している。逆動力学３５は、各軸の関節角加速度が入力されて軸ごとにその軸に作用する干渉トルクを発生する。軸ごとの干渉トルクは負荷側外乱トルクとしてその軸の加算要素２７に入力される。

【0026】

［第１の実施形態］
本発明の第１の実施形態の制御装置を説明する。まず、第１の実施形態の制御装置を説明する前に、従来の制御装置の構成について説明する。図３は、特許文献２に記載される制御装置を用いて図２に示すロボット１０に対する動力学トルクフィードフォワード補償を行うときの構成を示している。特許文献２に記載される制御装置では、負荷外乱トルク補償値を、プラントにおける負荷側外乱から負荷側速度までの応答を相殺するためのフィードフォワード補償値に変換し、そのフィードフォワード補償値を、制御系において外部からの指令の入力位置からモータへの電流指令値を出力するまでの区間であって制御偏差の積分補償器よりも前段にある加え合わせ点に入力するが、ここでは、変換されるフィードフォワード補償値が速度指令を補償する速度指令補償値であるものとする。

【0027】

上述したように制御装置１５は、ロボット１０の複数の軸に共通に設けられる全軸指令算出基板１６と、ロボット１０の軸ごとに用意されるモータ駆動制御基板１７とを備えている。全軸指令算出基板１６は、上位装置から入力される動作指令に基づいて、軸ごとの位置指令値などを算出する回路を備えている。図３に示す制御装置１５では、全軸指令算出基板１５は、ロボット１０の軸ごとに設けられてその軸の負荷状態量を予測する負荷状態算出部４１と、全ての負荷状態算出部４１から負荷状態量の予測値（すなわち負荷状態予測値）が入力されてそれぞれの軸の負荷外乱トルク補償値を算出する逆動力学計算部４２とをさらに備えている。逆動力学計算部４２が出力する負荷外乱トルク補償値は、ロボット１０の軸ごとに加わる負荷側外乱トルクに対応するものである。他軸からの干渉トルクがないと仮定して、前置補償器が設けられないときに軸ごとにその軸の位置指令値からロボット１０におけるその軸の負荷側位置までの伝達関数をＧ（ｓ）とすれば、軸ごとに設けられる負荷状態算出部４１が負荷状態量を予測するときの伝達関数Ｗ_r（ｓ）はＧ（ｓ）と等しく設計される。具体的には負荷状態算出部４１は、ロボット１０において各軸の位置閉ループにおいて動力学補償がなされており、他軸からの干渉が完全に打ち消されているものとして、位置指令値を入力として、位置指令値から負荷側位置までの系の伝達関数Ｗ_r（ｓ）に基づき、負荷２８の状態、すなわち負荷の位置、速度及び加速度を算出する。逆動力学計算部４２は、実際に動力学トルクフィードフォワード補償のための演算を行う部分であり、他軸からの干渉力と、姿勢変化に伴う自軸の慣性モーメントのノミナルトルクからの変化分であるトルク変動量とを計算し、他軸からの干渉力とトルク変動量の和として、負荷外乱トルク補償値を算出する。逆動力学計算部４２で行う演算については、例えば特許文献１に記載されている。

【0028】

一方、軸ごとに設けられるモータ駆動制御基板１７には、その軸の位置指令値が入力されるとともにモータ位置がフィードバックされる位置制御器５１と、その軸に対する速度指令値が入力されるとともにモータ速度がフィードバックされて電流指令値を生成する速度制御器５２と、速度指令補償値を出力する速度指令変換部５３と、位置制御器の出力と速度指令補償値とを加算して速度指令値として速度制御器５２に供給する加算要素５４とを備えている。速度指令変換部５３は、負荷側外乱トルクの入力から速度指令までの伝達関数Ｖ_tm（ｓ）に基づいて、全軸指令算出基板１６において算出されたその軸の負荷外乱トルク補償値を、負荷側外乱トルクによる負荷側速度の変化分を打ち消すために速度指令値に加えるべき速度指令補償値に変換する。伝達関数伝達関数Ｖ_tm（ｓ）の詳細は、特許文献２に記載されている。図３に示す制御系では、負荷状態算出部４１、逆動力学計算部４２及び速度指令変換部５３によって動力学トルク補償値が算出される。なお、図３及び図４では、説明の都合上、速度制御器５２の出力に基づいてモータ１２への電流を発生する部分の詳細は描かれていない。

【0029】

ここで図３に示した制御系における負荷状態算出部４１の伝達関数Ｗ_r（ｓ）について検討する。Ｗ_r（ｓ）は、ノミナルな２慣性系を制御対象としたときに位置指令値θ_ref（ｓ）から負荷側位置θ_L（ｓ）までの伝達関数である。後述するように速度制御器５２において状態オブザーバ７７（図６～図８参照）を設ける場合には、状態オブザーバ７７は、モータ速度を入力として、モータ速度と負荷速度と積分要素３０とから出力される各ねじれ角の推定値を出力する。モータ速度は、通常、モータ角度の検出値の疑似微分によって算出される。負荷速度やねじれ角度を検出するセンサが設けられていて負荷速度やねじれ角度を直接検出できる場合には、状態オブザーバ７７を使用せずに、モータ速度と負荷速度とねじれ角度とをゲイン７８（図６～図８参照）に直接入力して、減算要素７９（図６～図８参照）へ状態を直接フィードバックすることができる。このように状態の直接フィードバックを行う場合、Ｗ_r（ｓ）の分母の次数は、プラントであるロボット１０での電流指令値から負荷側位置までの積分要素（図２における積分要素２９，３０，３９とモータ１２の伝達関数に含まれる積分要素）の数の４に、速度制御器５２において比例積分制御を行なうための積分要素（例えば、図６における積分要素７４）の数である１を足した５次となる。このときの分母の次数の５次は、Ｗ_r（ｓ）の分子の最大次数である。速度制御器５２において状態オブザーバ７７を用いてプラントの状態を推定するときは、状態オブザーバが通常、３次であるので、その分、Ｗ_r（ｓ）の分母と分子の次数が大きくなる。負荷状態算出部４１は軸の数だけ全軸指令算出基板１６に設けられるので、Ｗ_r（ｓ）の分母や分子の次数が大きいことは、全軸指令算出基板１６における演算負荷を大きくする。

【0030】

図４は、図３に示す従来の制御装置に改良を加えた本発明の第１の実施形態の制御装置を示している。第１の実施形態の制御装置１５は、図３に示した従来の制御装置１５と比べ、モータ駆動制御基板１７において位置制御器５１の入力側に位置フィードフォワード補償を行う位置ＦＦ補償部５６が設けられ、また、伝達関数がＷ_r（ｓ）で表される負荷状態算出部４１の代わりに伝達関数がＷ_rh（ｓ）で表される負荷状態算出部４３が設けられている点で異なっている。各軸の位置指令値は、前置補償器である位置ＦＦ補償部５６を介して位置制御器５１に入力される。負荷状態算出部４３における伝達関数Ｗ_rh（ｓ）は、式（１）に示すように、分母が４次、分子が０次である。
Ｗ_rh（ｓ）＝ｂ₀／（ａ₄ｓ⁴＋ａ₃ｓ³＋ａ₂ｓ²＋ａ₁ｓ＋ａ₀） （１）
式（１）において、ａ₀，ａ₁，ａ₂，ａ₃，ａ₄及びｂ₀は定数である。伝達関数Ｗ_rh（ｓ）のことを希望伝達関数とも呼ぶ。本実施形態では、実際のプラントの状態を反映する伝達関数Ｗ_r（ｓ）を用いる代わりに、このＷ_r（ｓ）よりも次数の小さい希望伝達関数Ｗ_rh（ｓ）によりＷ_r（ｓ）を近似することによって、負荷状態量の演算負荷を低減させる。

【0031】

位置指令値から負荷側位置までの伝達関数を式（１）で示すＷ_rh（ｓ）とするために、位置ＦＦ補償部５６を設けないとしたときの位置指令値から負荷側位置までの伝達関数をＧ（ｓ）として、前置補償器である位置ＦＦ補償部５６での伝達関数ＦＦ_p（ｓ）は、式（２）に示すように定められる。
ＦＦ_p（ｓ）＝Ｗ_rh（ｓ）／Ｇ（ｓ） （２）

【0032】

全軸指令算出基板１６に設けられる負荷状態算出部４３で用いる伝達関数Ｗ_rh（ｓ）は分母が４次であって分子が０次（すなわち定数）であるので、第１の実施形態の制御装置によれば、全軸指令算出基板１６での演算負荷を大幅に低減させることができる。位置指令ＦＦ補償部５６を設けたことによりモータ駆動制御基板１７での演算負荷は増大するが、全軸指令算出基板１６での演算負荷の低減分よりも各モータ駆動制御基板１７の増加分は小さい。

【0033】

図５は本発明の原理を説明する図であり、位置指令値θ_cmd（ｓ）から負荷側位置θ_L（ｓ）までの伝達関数Ｇ（ｓ）と、位置指令値θ_cmd（ｓ）から算出される負荷状態予測値θ_est（ｓ）との関係を示している。Ｇ（ｓ）に直接与えられることとなる位置指令値を制御指令値θ_cmd（ｓ）としている。図５（ａ）は、図３に示した従来の制御装置では、分母が５次以上、分子が分母次数以下の伝達関数Ｗ_r（ｓ）（＝Ｇ（ｓ））である負荷状態算出部４１によって負荷状態予測値θ_est（ｓ）が算出されることを示している。このような図５（ａ）に示す系において、負荷状態算出部４１の前段と伝達関数Ｇ（ｓ）の前段にいずれも伝達関数がＦＦ_p（ｓ）で表される位置ＦＦ補償部４６，５６をそれぞれ配置した状態を示したものが図５（ｂ）である。伝達関数ＦＦ_p（ｓ）の選択により、位置ＦＦ補償部５６と伝達関数Ｇ_s（ｓ）とからなるブロック４８の全体としての伝達関数の分母を４次、分子を０次とすることができる。そこで図５（ｂ）に示す構成において位置ＦＦ補償部４６と負荷状態算出部４１とをまとめて改めて負荷状態算出部４３とすれば、図５（ｃ）に示すように、負荷状態算出部４３の伝達関数Ｗ_rh（ｓ）は、分母が４次であり、分子は０次となる。その結果、全軸指令算出基板１６に設けられる負荷状態算出部４３の演算量を大幅に小さくすることができる。

【0034】

図６は、図４に示す制御装置１５を備えたロボットシステムでの１軸分の制御系の構成を示している。図６では軸を特定しないので、伝達関数を構成する要素の表示に関し、どの軸であるかを示す添え字は用いられていない。また図６では、速度指令に速度指令補償値を加算する加算要素５４は速度制御器５２の内部に設けられるものとしている。以下、図６を使用して、モータ駆動制御基板１７に設けられる位置制御器５１や速度制御器５２の構成の一例を説明する。

【0035】

全軸指令算出基板１６において生成された位置指令値は、モータ駆動制御基板１７に送られ、上述した位置ＦＦ補償部５６に入力される。位置制御器５１には、位置ＦＦ補償部５６において位置フィードフォワード補償がなされた位置指令値が入力するともにモータ位置がフィードバックしている。位置制御器５１は、位置ＦＦ補償部５６から出力される位置指令値からモータ位置を減算して位置偏差を算出する減算要素６１と、減算要素６１が出力する位置偏差に比例ゲインＫ_ppを乗じて位置偏差に基づく指令とするゲイン要素６２とを備えている。

【0036】

速度制御器５２には、モータ速度が入力するとともに位置制御器５１から位置偏差に基づく指令が入力し、速度指令変換部５３から速度指令補償値が入力する。速度制御器５２は、位置制御器５１からの速度指令値と速度指令補償値とを加算する上述した加算要素５４と、加算要素５４の出力からモータ速度を減算して速度偏差を出力する減算要素７２と、速度偏差に対して速度ゲインＫ_vを乗ずるゲイン要素７３と、速度偏差が入力する積分要素７４と、積分要素７４の出力に積分ゲインＫ_iを乗ずるゲイン要素７５と、ゲイン要素７３の出力とゲイン要素７５の出力を加算する加算要素７６とを備えている。加算要素５４は、速度指令補償値を入力するための加え合わせ点である。さらに速度制御器５２は、モータ速度が入力する状態オブザーバ７７と、状態オブザーバ７７の出力に設けられたゲイン７８と、加算要素７６の出力からゲイン７８の出力とを減じて電流指令値とする減算要素７９と、を備えている。ゲイン要素７３，７５と積分要素７４とが設けられ、これらをまとめた伝達関数が（Ｋ_vｓ＋Ｋ_i）／ｓと表されることから、速度制御器５２はＰＩ（比例積分）制御を行っていることになる。積分要素７４とゲイン要素７５とは、制御偏差である速度偏差に対する積分補償器を構成している。また速度制御器５２は、状態オブザーバ７７に基づいた状態フィードバックによる制御も行っており、減算要素７９は、ＰＩ制御による制御量から状態フィードバックによる値を減算して電流指令値としている。電流指令値は、インバータ回路などを含む電流制御部８０により、モータ１２を駆動する電流に変換されてモータ１２に出力される。一般的な制御理論から明らかなように、積分補償がないと、外乱によって位置指令値にモータ位置を一致させることはできないから、ＰＩ制御として位置偏差の算出から電流指令値の出力までの区間内に積分要素７４あるいは積分補償器を設けることが必要となる。

【0037】

以上説明した第１の実施形態の制御装置では、負荷外乱トルク補償値の算出するために軸ごとの負荷状態量を算出する負荷状態算出部４３で用いる伝達関数を分母が４次、分子が０次であるＷ_rh（ｓ）とすることにより、全軸指令算出基板１６における演算負荷を低減できる。また、この実施形態では、負荷外乱トルク補償値から変換されたフィードフォーワード補償値を、制御系において制御偏差の積分補償器よりも前段にある加え合わせ点で入力することにより、フィードフォワード補償値に誤差が含まれることや、積分補償器の積分値が増大することが防がれ、負荷側外乱トルクの補償を行うときに、演算精度と位置制御性能とを維持できる。

【0038】

［第２の実施形態］
次に本発明の第２の実施形態について説明する。第１の実施形態の制御装置１５は、特許文献２に記載された制御装置に対して本発明を適用したものであるが、特許文献１に記載された制御装置に対しても本発明を適用できる。第２の実施形態の制御装置を説明する前に、特許文献１に記載された制御装置を含むロボットシステムの構成について、図７を用いて説明する。図７は、特許文献１に記載された制御装置を含むロボットシステムでの１軸分の制御系の構成を示している。図７に示す制御装置１５は、図３に示した制御装置１５と同様にものであるが、軸ごとの負荷外乱トルク補償値を電流指令値に加算される電流指令補償値に変換するようにした点で、図３に示した制御装置１５とは異なっている。そのため、速度指令変換部５３と加算要素５４とが設けられておらず、その代わりにモータ駆動制御基板１７に電流指令変換部５７と加算要素５８が設けられている。加算要素５８の出力は、モータ駆動制御基板１７においてロボット１０側に電流指令値を出力する端子に接続している。位置制御器５１や速度制御器５２の構成は、図６に示したものと同様である。電流指令変換部５７は、負荷側外乱トルクの入力から電流指令値の入力までの伝達関数Ｔ_m（ｓ）に基づいて、負荷外乱トルク補償値を、負荷側外乱トルクを打ち消すために電流指令値に加えるべき電流指令補償値に変換する。伝達関数Ｔ_m（ｓ）の詳細は、特許文献１，２に記載されている。加算要素５８は、速度制御器５２とプラントであるロボット１０との間に設けられた加え合わせ点であって、速度制御器５２が出力する電流指令値に対し、電流指令変換部５７が出力する電流指令補償値を加算する。加算要素５８が出力する電流指令値は、インバータ回路などを含む電流制御部８０により、モータ１２を駆動する電流に変換されてモータ１２に出力される。図７に示す制御装置１５においても、図３に示すものと同様に、全軸指令算出基板１６おいてモータ１２の軸ごとに対応して設けられる負荷状態計算部４１の伝達関数Ｗ_r（ｓ）は、分母が５次以上、分子の次数が分母の次数以下であり、全軸指令算出基板１６における演算負荷が大きい。

【0039】

図８は、第２の実施形態の制御装置１５を備えるロボットシステムでの１軸分の制御系の構成を示している。図８に示す第２の実施形態での制御系は、図７に示す制御系に対して位置ＦＦ補償部５６を付加して位置指令値が位置ＦＦ補償部５６を介して位置制御器５１に入力されるようにし、さらに、負荷状態算出部４１の代わりに伝達関数がＷ_rh（ｓ）で表される負荷状態算出部４３を設けたものである。第２の実施形態で用いられる負荷状態算出部４３及び位置ＦＦ補償部５６は、第１の実施形態で用いられるものと同様のものである。伝達関数Ｗ_rh（ｓ）は式（１）に示すように分母が４次であり分子が０次であり、位置ＦＦ補償部５６を示す伝達関数ＦＦ_p（ｓ）は式（２）で表される。伝達関数Ｗ_rh（ｓ）の分母と分子の次数が小さいので、第２の実施形態の制御装置１５においても全軸指令算出基板１６における演算負荷を低減できる。

【符号の説明】

【0040】

１０…ロボット；１１…マニピュレータ；１２…モータ；１３…減速機；１４…エンコーダ；１５…制御装置；１６…全軸指令算出基板；１７…モータ駆動制御基板；４１，４３…負荷状態算出部；４２…逆動力学計算部；５１…位置制御器；５２…速度制御器；５３…速度指令変換部；５６…位置ＦＦ補償部；５７…電流指令変換部；８０…電流制御部。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】