特許 7392159 制御装置及びロボットシステム

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

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-11-27

(45)【発行日】2023-12-05

(54)【発明の名称】制御装置及びロボットシステム

(51)【国際特許分類】

   B25J 9/10 20060101AFI20231128BHJP

【ＦＩ】

B25J9/10 A

【請求項の数】 4

(21)【出願番号】P 2022539504

(86)(22)【出願日】2021-07-27

(86)【国際出願番号】 JP2021027780

(87)【国際公開番号】W WO2022025072

(87)【国際公開日】2022-02-03

【審査請求日】2023-02-07

(31)【優先権主張番号】P 2020130903

(32)【優先日】2020-07-31

(33)【優先権主張国・地域又は機関】JP

(73)【特許権者】

【識別番号】390008235

【氏名又は名称】ファナック株式会社

(74)【代理人】

【識別番号】100099759

【弁理士】

【氏名又は名称】青木 篤

(74)【代理人】

【識別番号】100123582

【弁理士】

【氏名又は名称】三橋 真二

(74)【代理人】

【識別番号】100112357

【弁理士】

【氏名又は名称】廣瀬 繁樹

(74)【代理人】

【識別番号】100169856

【弁理士】

【氏名又は名称】尾山 栄啓

(72)【発明者】

【氏名】有田 創一

(72)【発明者】

【氏名】小栗 健一郎

(72)【発明者】

【氏名】澤田 和輝

【審査官】牧 初

(56)【参考文献】

【文献】特開２０００－７８５（ＪＰ，Ａ）

【文献】特開平３－２８１０８３（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ２５Ｊ １／００－２１／０２

Ｇ０５Ｂ １９／１８－１９／４１６

Ｇ０５Ｂ １９／４２－１９／４６

Ｇ０５Ｄ ３／００－ ３／２０

(57)【特許請求の範囲】

【請求項1】

複数の軸を備える機械であって該複数の軸のうち一の軸の回転が他の一の軸の回転を生じさせる連れ回り関係を有する機械の動作を制御する制御装置において、
前記複数の軸の各々の位置を検出する軸位置検出器と、
前記複数の軸をそれぞれ駆動する複数のモータの各々の位置を検出するモータ位置検出器と、
動作プログラムに従って前記複数の軸の各々に対する位置指令を算出する位置指令算出部と、
前記位置指令と、前記複数の軸の各々の検出位置とに基づいて、前記複数の軸の各々に対応する前記モータの速度指令を出力する位置制御部と、
前記速度指令に基づいて各々の前記モータを制御する速度制御部と、
前記複数の軸のうち前記連れ回り関係に依存した回転をする軸である制御対象軸の前記モータに対する前記速度指令を、該連れ回り関係に基づいて補正するための補正値を算出する補正値算出部と、を備え、
前記制御対象軸に対応する前記速度制御部は、前記制御対象軸の前記モータに対する前記速度指令を前記補正値に基づいて補正して前記制御対象軸の前記モータの制御に適用する、制御装置。

【請求項2】

前記補正値算出部は、前記連れ回り関係に依存した回転をする前記制御対象軸の位置と、前記制御対象軸に連れ回りを生じさせる少なくとも一つの軸に対応する前記モータの位置との間の対応関係に基づいて前記補正値を算出する、請求項１に記載の制御装置。

【請求項3】

前記機械は６軸ロボットであり、
前記制御対象軸を第６軸とし、該第６軸の位置が第４軸及び第５軸にそれぞれ対応する前記モータの位置に依存する連れ回り関係がある場合において、
当該連れ回り関係を、

【数1】

ただし、θ_J6は第６軸の位置、
θ_m4、θ_m5、θ_m6は、それぞれ第４から第６軸に対応するモータの位置、
α、β、Ｆは係数、
と表すとき、
前記補正値算出部は、前記補正値を、

【数2】

により求める、請求項１又は２に記載の制御装置。

【請求項4】

請求項１から３のいずれか一項に記載の制御装置と、
前記機械としての構成を有する多関節ロボットと、
を備えるロボットシステム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、制御装置及びロボットシステムに関する。

【背景技術】

【0002】

多関節ロボットは、各関節軸に配置されたサーボモータをサーボ制御することによって動作制御される。このような多関節ロボットの制御装置の例が、特許文献１に記載されている。

【先行技術文献】

【特許文献】

【0003】

【文献】国際公開第２０１９／０１２９４２号

【発明の概要】

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

【0004】

多関節ロボットの各関節軸に配置したサーボモータのエンコーダからのモータ位置・速度情報をフィードバック情報として用いる制御方式は、セミクローズド制御方式とも称される。さらに精度の高い動作制御を行うため、各関節軸にアーム（軸）の位置（角度）を検出するエンコーダを配置し、このエンコーダからのアーム（軸）位置情報をフィードバック情報として用いる制御方式は、フルクローズド制御方式とも称される。ところで、多関節ロボット等の複数の軸を備えた機械では、複数の軸の一部が、ある軸の動作に伴い他の軸が連れまわる連れ回り関係にある場合がある。連れ回り関係にある軸がある場合においても、適切なフルクローズド制御方式による制御が可能な制御装置及びロボットシステムが求められている。

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

【0005】

本開示の一態様は、複数の軸を備える機械であって該複数の軸のうち一の軸の回転が他の一の軸の回転を生じさせる連れ回り関係を有する機械の動作を制御する制御装置において、前記複数の軸の各々の位置を検出する軸位置検出器と、前記複数の軸をそれぞれ駆動する複数のモータの各々の位置を検出するモータ位置検出器と、動作プログラムに従って前記複数の軸の各々に対する位置指令を算出する位置指令算出部と、前記位置指令と、前記複数の軸の各々の検出位置とに基づいて、前記複数の軸の各々に対応する前記モータの速度指令を出力する位置制御部と、前記速度指令に基づいて各々の前記モータを制御する速度制御部と、前記複数の軸のうち前記連れ回り関係に依存した回転をする軸である制御対象軸の前記モータに対する前記速度指令を、該連れ回り関係に基づいて補正するための補正値を算出する補正値算出部と、を備え、前記制御対象軸に対応する前記速度制御部は、前記制御対象軸の前記モータに対する前記速度指令を前記補正値に基づいて補正して前記制御対象軸の前記モータの制御に適用する、制御装置である。

【0006】

本開示の別の態様は、上記制御装置と、上記機械としての構成を有する多関節ロボットと、を備えるロボットシステムである。

【発明の効果】

【0007】

上記構成によれば、複数の軸に連れ回り関係がある場合であっても、制御対象軸の制御に連れ回り関係を考慮した制御を適用することができ、機械の位置制御の性能を向上させるこができる。

【0008】

添付図面に示される本発明の典型的な実施形態の詳細な説明から、本発明のこれらの目的、特徴および利点ならびに他の目的、特徴および利点がさらに明確になるであろう。

【図面の簡単な説明】

【0009】

【図1】一実施形態に係るロボット制御装置を含むロボットシステムの構成を表す図である。

【図2】ロボット制御装置及びロボットの機能ブロック図である。

【図3】実施形態にかかるフルクローズド制御方式によるフィードバック制御回路のブロック線図である。

【図4A】参考例として一般的なセミクローズド制御方式によるフィードバック制御回路のブロック線図である。

【図4B】参考例として一般的なフルクローズド制御方式によるフィードバック制御回路のブロック線図である。

【図5】参考例として、連れ回り関係があるロボットアームの構成例を示す図である。

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

【0010】

次に、本開示の実施形態について図面を参照して説明する。参照する図面において、同様の構成部分または機能部分には同様の参照符号が付けられている。理解を容易にするために、これらの図面は縮尺を適宜変更している。また、図面に示される形態は本発明を実施するための一つの例であり、本発明は図示された形態に限定されるものではない。

【0011】

図１は、一実施形態に係るロボット制御装置２０を含むロボットシステム１００の構成を表す図である。図２は、ロボット制御装置２０及びロボット１０の機能ブロック図である。以下で詳細に説明するように、ロボット制御装置２０は、フルクローズド制御方式によるフィードバック制御において、ロボット１０の関節軸に連れ回り関係がある場合を考慮した適切な制御を実行する。

【0012】

図１に示すように、ロボットシステム１００は、ロボット１０とロボット制御装置２０とを含む。ロボット１０は、本実施形態では６軸の垂直多関節ロボットであるものとする。ロボット１０として他のタイプのロボットが用いられても良い。ロボット制御装置２０は、動作プログラムにしたがってロボット１０の動作を制御する。なお、ロボット制御装置２０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、記憶装置、操作部、表示部、入出力インタフェース、ネットワークインタフェース等を有する一般的なコンピュータとしての構成を有していても良い。ロボット制御装置２０には、教示装置（不図示）が接続されていても良い。

【0013】

図１に示すように、ロボット１０は６軸ロボットであり、６つの関節軸（以下、単に軸とも記載する）Ｊ１軸－Ｊ６軸を有する。Ｊ１軸－Ｊ６軸は、それぞれ図１中の矢印で示すように回転可能である。また、ロボット１０は、各軸の位置（角度）を検出する検出器としてエンコーダを各軸に備えている。これにより、ロボット１０は、各軸の検出位置（検出角度）に基づくフルクローズド制御方式によるフィードバック制御を行うことができる。

【0014】

複数の軸を含むロボットでは、アームの構造上、複数の軸に、ある軸の回転が他の軸の回転を生じさせる連れ回りの関係がある場合がある。なお、本明細書において、連れ回り関係とは、複数の軸のうち少なくとも一つの軸の回転が、他の軸のうちの少なくとも一つの回転を生じさせるあらゆる場合を含むものとする。参考例として、連れ回り関係があるロボットアームの構成例を図５を参照して説明する。図５に示すロボットアームは、第１アーム３３１と第２アーム３３２とを含む。このようなロボットアームの機構が、例えば多関節ロボットのアーム先端部分の機構として用いられる。サーボモータ３０１はＢ軸（連れ回り要因軸）を駆動し、サーボモータ３０２はＡ軸（連れ回り軸）を駆動する。サーボモータ３０１が回転すると、ギヤ３１１ａ、３１１ｂを介して第１アーム３３１が回転する。すなわちＢ軸はサーボモータ３０１の回転によって回転制御される。サーボモータ３０２が回転すると、ギヤ３１２ａ、３１２ｂを介して軸３２１が回転し、さらに傘歯ギヤ３２２ａ、３２２ｂを介して軸３２３が回転し、第２アーム３３２が回転する。すなわちＡ軸はサーボモータ３０２の回転によって回転制御される。

【0015】

ここで、第１アーム３３１と第２アーム３３２は機構上接続されているため、第１アーム３３１がＢ軸回りに回転すると、傘歯ギヤ３２２ｂの回転を生じ、第２アーム３３２にもＡ軸回りの回転（すなわち、連れ回り）を生じる。このため、サーボモータ３０１が回転すると、Ｂ軸が回転して第１アーム３３１が回転し、第２アーム３３２がＡ軸回りで連れ回る。

【0016】

本実施形態に係る連れ回りを考慮したフルクローズド制御方式による各軸制御の理解の便宜のため、参考例として、モータ位置（回転角度）・速度を検出するエンコーダを用いたセミクローズド制御方式と、アーム（各軸）の位置を検出するエンコーダを用いた一般的なフルクローズド制御方式について図４Ａ、図４Ｂを参照して説明する。

【0017】

図４Ａにセミクローズド制御方式によるフィードバック制御回路のブロック線図を示す。図４Ａに示されるように、セミクローズド制御方式のフィードバック制御回路には、指令としてモータ位置指令が入力され、モータ位置指令は位置制御部４１（Ｇｐ（ｓ））によりモータ速度指令に変換され、さらに、速度制御部４２（Ｇｖ（ｓ））は速度指令にしたがってモータ４３を制御する指令（電流指令等）を出力する。なお、図４Ａ（他のブロック線図においても同様）においてロボット機構を「ねじれ系」と記載している。図４Ａのフィードバック制御回路では、モータ速度がマイナーループとしてフィードバック制御され、モータ位置のフィードバック信号とモータ位置指令との偏差を用いて位置制御部４１が制御を実行する。

【0018】

図４Ｂは、一般的なフルクローズド制御方式によるフィードバック制御回路を示す。図４Ｂのフィードバック制御回路には、指令としてアーム（軸）位置指令が与えられ、位置制御部５１（Ｇｐ（ｓ））はアーム位置指令をモータ速度指令に変換する。速度制御部５２（Ｇｖ（ｓ））は、モータ速度指令にしたがってモータ５３を制御する指令（電流指令等）を出力する。図４Ｂのフィードバック制御回路では、モータ速度がマイナーループとしてフィードバック制御され、アーム（軸）位置のフィードバック信号とアーム（軸）位置指令との偏差を用いて位置制御部５１が制御を実行する。図４Ｂの一般的なフルクローズド制御方式では、制御対象軸に配置された単一のモータ５３のモータ速度のみに基づき制御を行う構成であるため、制御対象軸の位置が他の軸にも依存する連れ回り関係が考慮されていないことが理解される。

【0019】

以下、図２及び図３を参照し、本実施形態に係るロボット制御装置２０による、連れ回りを考慮したフルクローズド制御方式について説明する。図２に示すように、ロボット制御装置２０は、動作プログラム及びその他ロボット１０の制御に係わる各種情報を記憶する記憶部３２と、動作プログラムにしたがって運動学的な計算によりロボット１０の動作を制御するための各軸の位置（角度）指令を算出する位置指令算出部２７と、Ｊ１軸－Ｊ６軸に対してフルクローズド制御方式によるフィードバック制御をそれぞれ実行するフィードバック制御部２１－２６とを備える。

【0020】

ロボット制御装置２０は、さらに、連れ回り関係に基づいて、連れ回り軸の速度指令に対する補正値を計算する補正値算出部２８を備えている。なお、ロボット制御装置２０における、位置指令算出部２７、フィードバック制御部２１－２６、補正値算出部２８等の機能ブロックは、ロボット制御装置２０のＣＰＵ３１が、各種ソフトウェアを実行することで実現されても良く、或いは、ＡＳＩＣ（Application Specific Integrated Circuit）等のハードウェアを主体とした構成により実現されても良い。

【0021】

ロボット１０は、Ｊ１軸１１１、Ｊ２軸１１２、Ｊ３軸１１３、Ｊ４軸１１４、Ｊ５軸１１５、及びＪ６軸１１６を有し、これら６つの軸にはそれぞれ軸位置（角度）を検出する軸位置検出器としてエンコーダ１２１－１２６が設けられている。エンコーダ１２１－１２６は、例えば光学式のロータリエンコーダであっても良い。エンコーダ１２１－１２６はそれぞれ軸の位置情報をフィードバック制御部２１－２６にフィードバックする。Ｊ１軸からＪ６軸には、サーボモータであるモータ２１１－２１６が配置されている。モータ２１１－２１６には、回転位置を検出するモータ位置検出器としてエンコーダ２２１－２２６がそれぞれ設けられている。エンコーダ２２１－２２６は、例えば光学式のロータリエンコーダであっても良い。エンコーダ２２１－２２６は、モータの位置情報及び速度情報をそれぞれフィードバック制御部２１－２６にフィードバックする。

【0022】

ロボット１０の軸に連れ回り関係がある場合に、連れ回り関係を考慮してフルクローズド制御方式を実現することについて検討する。６軸ロボットにおいてアーム（軸）位置をθ_Jn（ｎ＝１，…，６）、モータ位置をθ_mn（ｎ＝１，…，６）とする。この場合、連れ回りがない場合のアーム位置θ_Jnは、以下の数式（１）の様に表される。なお、数式（１）では、モータ位置θ_mn（ｎ＝１，…，６）をアーム位置θ_Jn（ｎ＝１，…，６）に変換する係数をそれぞれＡ，Ｂ，Ｃ，Ｄ，Ｅ，Ｆとする。

【数1】

【0023】

上記数式（１）では、右辺の行列は対角成分のみが非ゼロであり対角成分以外はゼロである。よって、各軸の位置は、当該軸が有するモータ位置のみに依存することが理解できる。よって、数式（１）は、軸に連れ回りの関係がない場合を表す。

【0024】

連れ回り関係がある場合のアーム位置θ_Jnとモータ位置をθ_mnとの関係は下記数式（２）のように表すことができる。数式（２）の右辺の行列では、対角成分以外も非ゼロであるため、ある軸の位置が当該軸に設けられたモータ位置に加え、当該軸以外の軸に設けられたモータ位置にも依存する。なお、数式（２）の右辺の行列において、対角成分以外の成分のうち少なくとも一つに非ゼロの成分がある場合に、連れ回り関係があることとなる。

【数2】

【0025】

一例として、ロボット１０において、Ｊ６軸の位置がＪ４軸及びＪ５軸のモータ位置に依存する連れ回りの関係がある場合を想定する。この場合、Ｊ６軸の位置は、下記の数式（３）の様に表される。ここで、Ｊ６軸の位置に対するＪ４軸、Ｊ５軸のそれぞれのモータ位置による影響を表す係数をそれぞれα、βとする。

【数3】

【0026】

すなわち、上記例の場合、Ｊ６軸の位置は下記の数式（４）により表される。

【数4】

【0027】

上記数式（４）を変形し、Ｊ６軸のモータ２１６の位置（角度）を表す下記の数式（５）を得る。

【数5】

【0028】

上記数式（５）の両辺を微分することでＪ６軸のモータ２１６の速度は下記数式（６）のように表される。

【数6】

【0029】

数式（６）において右辺の第２項及び第３項が、数式（４）で表される連れ回り関係がある場合に、Ｊ６軸のモータ２１６の速度指令に対して適用するべき補正値（下記数式（７））である。

【数7】

【0030】

数式（４）で示した連れ回り関係がある場合、補正値算出部２８が、数式（７）のように補正値を算出し、算出した補正値を制御対象軸のフィードバック制御部（この場合はフィードバック制御部２６）に適用する。なお、補正値算出部２８は、連れ回り関係が上述の例以外の場合にも、上述の例に沿って、連れ回り関係に依存した回転をする制御対象軸の位置と、制御対象軸に連れ回りを生じさせる少なくとも一つの軸に対応するモータの位置との間の対応関係（上記例において数式（４））に基づいて補正値を算出する。

【0031】

図３は、Ｊ６軸に関するフルクローズド制御方式によるフィードバック制御をブロック線図として表している。なお、他の軸のフルクローズド制御方式も図３と同様である。図４Ｂに示した構成と同様に、図３のフィードバック制御回路は、アーム（軸）位置指令に基づいてモータ２１６の速度指令を算出する位置制御部２６１と、速度指令にしたがってモータ２１６を駆動するための駆動信号を出力する速度制御部２６２とを備える。図３における位置制御部２６１及び速度制御部２６２が、図２におけるＪ６軸のフィードバック制御部２６に対応している。

【0032】

図３のフィードバック制御回路では、モータ速度がマイナーループとしてフィードバック制御され、軸位置がフィードバックされてアーム位置指令との偏差を用いて位置制御部２６１が制御を実行する。Ｊ６軸が上記数式（４）で示す連れ回り関係にある場合、補正値算出部２８によって算出された補正値（数式（７））としてのモータ速度補正値が、位置制御部２６１が出力する速度指令値に加算されることで速度指令の補正が行われる。したがって、Ｊ６軸に対し、連れ回り関係を考慮した速度制御が実現される。このような構成により、Ｊ６軸のフィードバック制御部２６は、補正値算出部２８が算出した補正値に基づいて補正が成された速度指令に基づいて速度制御を行うことができる。

【0033】

以上述べたように、本実施形態によれば、ロボットの軸に連れ回り関係がある場合であっても、制御対象軸の制御に連れ回り関係を考慮した制御を適用することができ、ロボットアームの位置制御の性能を向上させることができる。特に、連れ回り関係にある軸を含む複数の軸を制御しながらロボットアームを動作させる場合の動的性能が向上する。

【0034】

以上、典型的な実施形態を用いて本発明を説明したが、当業者であれば、本発明の範囲から逸脱することなしに、上述の各実施形態に変更及び種々の他の変更、省略、追加を行うことができるのを理解できるであろう。

【0035】

上述の実施形態で示した各軸位置とモータ位置との間の係数Ａ－Ｆ、α、β等は、ロボット制御装置２０の記憶部３２に予め記憶されていても良く、或いは、ロボット制御装置２０の操作部（不図示）を介してユーザが設定可能であっても良い。或いは、これらの係数は、外部装置からロボット制御装置２０に対して入力されても良い。

【0036】

上述の実施形態に構成は、連れ回り関係にある軸を含む各種の機械に適用することができる。

【0037】

図２に示したロボット制御装置２０の機能構成は例示であり、様々な変形を行っても良い。例えば、図２の構成例では、ロボット制御装置２０が一つの補正値算出部２８を有し、この補正値算出部２８が制御対象軸の制御に適用すべき補正値を算出して制御対象軸のフィードバック制御部に適用する構成となっているが、このような構成に替えて、フィードバック制御部２１－２６の各々がその内部に補正値を算出する補正値算出部の機能を有する構成も有り得る。

【符号の説明】

【0038】

１０ ロボット
２０ ロボット制御装置
２１－２６ フィードバック制御部
２７ 位置指令算出部
２８ 補正値算出部
３１ ＣＰＵ
３２ 記憶部
１００ ロボットシステム
１１１ Ｊ１軸
１１２ Ｊ２軸
１１３ Ｊ３軸
１１４ Ｊ４軸
１１５ Ｊ５軸
１１６ Ｊ６軸
１２１－１２６ エンコーダ
２１１－２１６ モータ
２２１－２２６ エンコーダ
２６１ 位置制御部
２６２ 速度制御部

【図1】

【図2】

【図3】

【図4A】

【図4B】

【図5】