特許 6565622 ロボットシステム及びロボット制御方法

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

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

(11)【特許番号】6565622

(24)【登録日】2019年8月9日

(45)【発行日】2019年8月28日

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

(51)【国際特許分類】

   B25J 13/00 20060101AFI20190819BHJP

【ＦＩ】

   B25J13/00 Z

【請求項の数】8

【全頁数】14

(21)【出願番号】特願2015-223412(P2015-223412)

(22)【出願日】2015年11月13日

(65)【公開番号】特開2017-87375(P2017-87375A)

(43)【公開日】2017年5月25日

【審査請求日】2018年2月2日

(73)【特許権者】

【識別番号】000006622

【氏名又は名称】株式会社安川電機

(74)【代理人】

【識別番号】110000154

【氏名又は名称】特許業務法人はるか国際特許事務所

(72)【発明者】

【氏名】中西 光章

【審査官】 松田 長親

(56)【参考文献】

【文献】 特開平０１−２７４９８０（ＪＰ，Ａ）

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

Ｂ２５Ｊ １／００−２１／０２

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

【請求項1】

複数の駆動軸を備えた多関節アームを有するロボットと、
前記アーム先端に取り付けられ、１のエンドエフェクタ軸に沿って駆動し、所定の動作を行う可動部材を備えたエンドエフェクタと、
前記複数の駆動軸を制御するロボットコントローラとを有し、
前記ロボットコントローラは、
前記可動部材の前記所定の動作に伴って前記エンドエフェクタに生じる前記エンドエフェクタ軸方向の慣性力を繰り返し演算し取得する慣性力取得部と、
前記慣性力取得部において繰り返し取得される前記慣性力より、前記複数の駆動軸の各軸に作用する外乱トルクを求める外乱トルク取得部と、
前記複数の駆動軸の各軸についての制御目標値から得られるトルク指令と前記外乱トルクとに基いて、前記複数の駆動軸を制御する駆動軸制御部と、
を有するロボットシステム。

【請求項2】

前記駆動軸制御部は、フィードフォワード制御により、前記外乱トルクに対する補償を行う請求項１に記載のロボットシステム。

【請求項3】

前記ロボットコントローラは、前記可動部材の前記エンドエフェクタ軸に沿った駆動を制御するエンドエフェクタ軸制御部を有する請求項１又は２に記載のロボットシステム。

【請求項4】

前記ロボットコントローラは、前記複数の駆動軸の駆動により前記エンドエフェクタに生じる慣性力である逆慣性力を繰り返し演算し取得する逆慣性力取得部を有し、
前記エンドエフェクタ軸制御部は、前記可動部材の前記エンドエフェクタ軸に沿った駆動における目標力と前記逆慣性力とに基いて、前記エンドエフェクタ軸を制御する請求項３に記載のロボットシステム。

【請求項5】

前記ロボットコントローラは、前記可動部材の前記エンドエフェクタ軸に沿った駆動に際して、前記エンドエフェクタの慣性質量を推定する慣性質量推定部を有する請求項３又は４に記載のロボットシステム。

【請求項6】

前記ロボットコントローラは、少なくとも前記エンドエフェクタの慣性質量に関する情報を含むエンドエフェクタ情報を読み込むエンドエフェクタ情報読み込み部を有する請求項１〜５のいずれか１項に記載のロボットシステム。

【請求項7】

前記多関節アームは少なくとも６軸以上の前記駆動軸を有し、
前記駆動軸制御部は、前記アームの基台側の少なくとも３軸について、ヤコビ転置行列を用いて前記慣性力より前記外乱トルクを求め、前記ロボットの先端側の少なくとも３軸について、幾何学的関係を用いて前記慣性力より前記外乱トルクを求める請求項１〜６のいずれか１項に記載のロボットシステム。

【請求項8】

複数の駆動軸を備えた多関節アームを有するロボットの前記アーム先端に取り付けられ、１のエンドエフェクタ軸に沿って駆動し、所定の動作を行う可動部材を備えたエンドエフェクタにおける、前記可動部材の前記エンドエフェクタ軸に沿った駆動により生じる慣性力を繰り返し求め、
繰り返し求められる前記慣性力より前記複数の駆動軸の各軸に作用する外乱トルクを求め、
前記複数の駆動軸の各軸についての制御目標値から得られるトルク指令と前記外乱トル
クとに基いて、前記複数の駆動軸を制御する、
ロボット制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

多関節アームを有するロボットにおいては、ある軸の動作により生じる慣性力が他の軸に干渉力として影響を及ぼし、位置決めや追従の精度に悪影響を及ぼす場合がある。例えば、特許文献１には、多慣性系干渉モデルを用いて非干渉トルク信号を求めトルク信号に加算することで、かかる干渉力の影響を排除し、各軸が指令通り動作するように制御するロボットの制御装置が記載されている。

【0003】

一方で、ロボットアームの先端に取り付けられるいわゆるエンドエフェクタもまた、干渉力の発生源となりうるが、これまで、エンドエフェクタによる干渉力を効果的に低減しうる制御は、特許文献２、特許文献３に記載されているように、実測定により得た加工反力を補償するものが知られている程度である。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２００５−１８６２３５号公報

【特許文献2】特開２０１１−２１６０５０号公報

【特許文献3】特開２０１２−１７６４７８号公報

【発明の概要】

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

【0005】

上述の通り、多関節ロボットのアーム先端に取り付けられたエンドエフェクタによる干渉力を効果的に低減しうる一般的に適用可能な制御はこれまで知られていない。そのため、エンドエフェクタとして、ロボットの可搬重量内の重さであっても、可動部の質量が大きい等、エンドエフェクタの動作時に生じる慣性力が大きいものを用いると、エンドエフェクタの動作時の干渉力が無視できないものとなり、振動が生じるなど、位置決めに時間を要したり、精度が低下する原因となる。

【0006】

本発明の解決しようとする課題は、多関節アームを有するロボットにおいて、エンドエフェクタによる干渉力を効果的に低減しうるロボットシステム及びロボット制御方法を提供することである。

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

【0007】

本発明の一側面に係るロボットシステムは、複数の駆動軸を備えた多関節アームを有するロボットと、前記アーム先端に取り付けられ、駆動軸であるエンドエフェクタ軸を備えたエンドエフェクタと、前記複数の駆動軸を制御するロボットコントローラとを有し、前記ロボットコントローラは、前記エンドエフェクタ軸の駆動により生じる慣性力を求める慣性力取得部と、前記慣性力より前記複数の駆動軸の各軸に作用する外乱トルクを求める外乱トルク取得部と、前記複数の駆動軸の各軸についての制御目標値から得られるトルク指令と前記外乱トルクとに基いて、前記複数の駆動軸を制御する駆動軸制御部と、を有する。

【0008】

また、本発明の一側面に係るロボットシステムでは、前記駆動軸制御部は、フィードフォワード制御により、前記外乱トルクに対する補償を行ってよい。

【0009】

また、本発明の一側面に係るロボットシステムでは、前記ロボットコントローラは、前記エンドエフェクタ軸を制御するエンドエフェクタ軸制御部を有してよい。

【0010】

また、本発明の一側面に係るロボットシステムでは、前記ロボットコントローラは、前記複数の駆動軸の駆動により前記エンドエフェクタに生じる慣性力である逆慣性力を求める逆慣性力取得部を有し、前記エンドエフェクタ軸制御部は、前記エンドエフェクタ軸についての目標力と前記逆慣性力とに基いて、前記エンドエフェクタ軸を制御してよい。

【0011】

また、本発明の一側面に係るロボットシステムでは、前記ロボットコントーラは、前記エンドエフェクタ軸を駆動し、前記エンドエフェクタの慣性質量を推定する慣性質量推定部を有してよい。

【0012】

また、本発明の一側面に係るロボットシステムでは、前記ロボットコントローラは、少なくとも前記エンドエフェクタの慣性質量に関する情報を含むエンドエフェクタ情報を読み込むエンドエフェクタ情報読み込み部を有してよい。

【0013】

また、本発明の一側面に係るロボットシステムでは、前記多関節アームは少なくとも６軸以上の前記駆動軸を有し、前記駆動軸制御部は、前記アームの基台側の少なくとも３軸について、ヤコビ転置行列を用いて前記慣性力より前記外乱トルクを求め、前記ロボットの先端側の少なくとも３軸について、幾何学的関係を用いて前記慣性力より前記外乱トルクを求めてよい。

【0014】

また、本発明の一側面に係るロボット制御方法は、複数の駆動軸を備えた多関節アームを有するロボットの前記アーム先端に取り付けられ、駆動軸であるエンドエフェクタ軸を備えたエンドエフェクタの前記エンドエフェクタ軸の駆動により生じる慣性力を求め、前記慣性力より前記複数の駆動軸の各軸に作用する外乱トルクを求め、前記複数の駆動軸の各軸についての制御目標値から得られるトルク指令と前記外乱トルクとに基いて、前記複数の駆動軸を制御する。

【図面の簡単な説明】

【0015】

【図1】本発明の実施形態に係るロボットシステムを示す概略図である。

【図2】本実施形態に係るロボットシステムの機能ブロック図である。

【図3】制御ループの例を示す図である。

【図4】フランジをＴ軸の軸方向から見た模式図である。

【図5A】フランジをＴ軸の軸方向から見た模式図である。

【図5B】フランジをＢ軸の軸方向から見た模式図である。

【図6A】フランジをＴ軸の軸方向から見た模式図である。

【図6B】フランジをＢ軸の軸方向から見た模式図である。

【図7】ロボットコントローラの動作を示すフロー図である。

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

【0016】

図１は本発明の実施形態に係るロボットシステムを示す概略図である。ロボットシステムは、多関節のアーム１０を備え、アームの先端にエンドエフェクタ１１が取り付けられたロボット１と、ロボット１を制御するロボットコントローラ２を含む。

【0017】

ここで、ロボット１は、一般的な産業用ロボットでよく、本実施形態では６軸の垂直多関節型ロボットであるが、ロボット１の形式や軸数に限定はなく、５軸のものや、７軸以上のものであってもよく、また、水平多関節型等の他の形式のものであってもよい。この例では、アーム１０に設けられている駆動軸を、基台側から順に、Ｓ軸、Ｌ軸、Ｕ軸、Ｒ軸、Ｂ軸及びＴ軸と呼び、それぞれの駆動軸の回転方向は図１に示した通りである。

【0018】

また、本明細書では、アーム１０の基台側からの３軸である、Ｓ軸、Ｌ軸及びＵ軸を総称して基本軸と呼び、アーム１０の先端側からの３軸である、Ｔ軸、Ｂ軸及びＲ軸を総称して手首軸と呼ぶものとする。アーム１０が７軸以上を有する場合には、冗長軸の配置により当該軸を基本軸に含めるか、手首軸に含めるかするとよい。いずれにせよ、基本軸はアーム１０の基台側からの少なくとも３軸を含み、手首軸はアーム１０の手首側からの少なくとも３軸を含む。

【0019】

エンドエフェクタ１１は、駆動軸であるエンドエフェクタ軸を少なくとも１軸有し、このエンドエフェクタ軸が駆動されることにより、エンドエフェクタ１１の動作がなされる。以降、単に駆動軸というときには、アーム１０が備える各軸（Ｓ軸〜Ｔ軸）を指し、エンドエフェクタ１１が有する軸（Ｚ軸）はエンドエフェクタ軸と呼び両者を区別する。

【0020】

エンドエフェクタ１１の用途や機能、型式に特に限定はないが、この例では、エンドエフェクタ１１はスポット溶接ガンであり、エンドエフェクタ軸は図中Ｚ軸として示されている。この例のエンドエフェクタ１１は、Ｚ軸を駆動することにより顎状の部材を開閉させ、顎状の部材の先端に互いに向き合うように設けられた溶接電極で対象部材を挟み込み、高電圧を印加することによりスポット溶接を行うものである。したがって、エンドエフェクタ１１の可動部材（この場合は顎状の部材）はＺ軸に沿って動き、かかる動作に伴って、Ｚ軸方向の慣性力を発生する。エンドエフェクタ１１自体はロボット１の可搬重量内の重さであるが、可動部材の質量が大きく、又サイズも大きいため、エンドエフェクタ１１の動作により生じる慣性力はロボット１に対する干渉力として無視できない程度の大きさとなる。

【0021】

ロボットコントローラ２は、駆動軸及びエンドエフェクタ軸を制御する。駆動軸及びエンドエフェクタ軸の駆動源は特に限定されるものではないが、本実施形態ではいずれもサーボモータを用いている。各サーボモータは、ロボットコントローラ２から出力される電流によりその位置及び速度が所望のものとなるよう制御される。

【0022】

さらに、以降の説明で、ロボットの基台を原点とし、鉛直情報をＺ方向にとる座標をロボット座標Ｏｒｏｂｏｔ、アーム１０先端に設けられ、エンドエフェクタ１１が取り付けられるフランジの中心を原点とし、フランジ法線方向をＺ方向にとる座標をフランジ座標Ｏｆｌａｎｇｅ（図示せず）、エンドエフェクタ１１の作用基準点であるＰ点を原点とし、エンドエフェクタ軸の駆動方向をＺ方向にとる座標をツール座標Ｏｔｏｏｌ（図示せず）とする。

【0023】

図２は、本実施形態に係るロボットシステムの機能ブロック図である。ロボットコントローラ２は、駆動軸制御部２０及びエンドエフェクタ軸制御部２１を有し、それぞれ制御目標値が入力されることで、駆動軸１３（図１のＳ軸〜Ｔ軸）及びエンドエフェクタ軸１４（図１のＺ軸）に対し制御出力、ここでは各サーボモータに対する電流を出力し、駆動軸１３及びエンドエフェクタ軸１４を制御する。

【0024】

さらに、ロボットコントローラ２は、エンドエフェクタ１１の動作によりアーム１０に作用する干渉力を補償する干渉力補償のための構成と、アーム１０の動作によりエンドエフェクタ１１に作用する干渉力である、逆干渉力を補償する逆干渉力補償のための構成を有する。以降、エンドエフェクタ１１の動作によりアーム１０に生じる干渉力を単に干渉力と呼び、アーム１０の動作によりエンドエフェクタ１１に生じる干渉力を逆干渉力と呼ぶ。

【0025】

[干渉力補償のための構成]
慣性力取得部２２は、エンドエフェクタ軸制御部２１からエンドエフェクタ軸１４の駆動情報、例えば、現在の加速度を取得し、既知のエンドエフェクタ１１の慣性質量から、生じる慣性力を演算し、取得する。この慣性力は、アーム１０の各駆動軸１３に対し、干渉力として作用する。

【0026】

さらに、外乱トルク取得部２３は、慣性力から、各駆動軸１３に生じる外乱トルクを演算し、取得する。得られた外乱トルクは、駆動軸制御部２０における制御に用いられ、補償される。より具体的には、駆動軸制御部２０において、制御目標値から得られる各駆動軸１３に対するトルク指令に対し、外乱トルクが打ち消されるように制御する。

【0027】

サーボモータの一般的な位置・速度制御では、図３に示すような制御ループが用いられる。すなわち、制御目標である位置目標ｕと、電動機の現在位置Ｘとの差ｅから速度指令ｖ、加速度指令ａを順に求め、電動機に対するトルク指令Ｔに変換し、かかるトルク指令Ｔに従った電流出力が電動機に対しなされる。電動機の現在位置Ｘ及び演算により推定された電動機速度ｖ_ｅは、図のようにフィードバックされる。

【0028】

本実施形態では、このような一般的な制御ループに加え、さらに、トルク指令Ｔに対し、外乱トルク取得部２３により取得された外乱トルクＴ_ｔを逆符号で加算する。これにより、エンドエフェクタ１１の動作によりアーム１０に生じる干渉力は打ち消される。したがって、外乱トルクＴ_ｔに関して言えば、これは電動機に対し、フィードフォワード制御がなされていることになる。あるいは、図２の慣性力取得部２２及び外乱トルク取得部２３は、駆動軸制御部２０に対し、一種の外乱オブザーバとして機能しているともいえる。

【0029】

この構成により、エンドエフェクタ１１による干渉力は効果的に低減され、エンドエフェクタ１１の動作に起因する振動が抑制されるから、位置決め精度の向上や高速化がなされる。また、干渉力の低減をフィードフォワード制御により行うため、これをフィードバック制御により行う場合に比して即応性に優れ、速やかに振動抑制がなされる。

【0030】

[逆干渉力補償のための構成]
逆慣性力取得部２４は、駆動軸制御部２０から各駆動軸１３の駆動情報、例えば、現在の各駆動軸に生じるトルクを取得し、既知のエンドエフェクタ１１の慣性質量から、エンドエフェクタ１１に生じる逆慣性力を演算し、取得する。この演算は、先ほどの外乱トルクＴ_ｔを求める演算に対し、各駆動軸に生じるトルクＴを用いて逆演算をするものである。この逆慣性力は、エンドエフェクタ１１のエンドエフェクタ軸１４に対し、逆干渉力として作用する。

【0031】

得られた逆慣性力は、エンドエフェクタ軸制御部２１における制御に用いられ、補償される。より具体的には、エンドエフェクタ軸制御部２１において、制御目標値から得られる目標力に対し、逆慣性力が打ち消されるように制御する。かかる制御を行うには、図３において外乱トルクＴ_ｔを逆慣性力によりエンドエフェクタ軸１４に生じるトルクとして読み替えられれば良く、すなわち、逆慣性力に関して、エンドエフェクタ軸１４の電動機に対し、フィードフォワード制御がなされていることになる。また、逆慣性力取得部２４は、エンドエフェクタ軸制御部２１に対し、一種の外乱オブザーバとして機能することになる。

【0032】

この構成により、アーム１０の動作による逆干渉力は効果的に低減され、エンドエフェクタ１１に生じる振動が抑制されるから、やはり位置決め精度の向上や高速化がなされる。また、逆干渉力の低減をフィードフォワード制御により行うため、これをフィードバック制御により行う場合に比して即応性に優れ、速やかに振動抑制がなされる。

【0033】

[慣性質量を取得するための構成]
以上の制御を行うためには、エンドエフェクタ１１のエンドエフェクタ軸に関する慣性質量が既知である必要がある。そのため、慣性質量推定部２５は、慣性質量が既知でない場合、例えば、後述するエンドエフェクタ情報が利用できない場合等に、エンドエフェクタ軸制御部２１に指令して、エンドエフェクタ軸１４に対し既知の動作をさせる。この時生じた干渉力により、各駆動軸１３にはかかる干渉力を打ち消すための電流が発生するが、かかる電流値（又は指令トルク）の値から、慣性質量を演算し推定する。

【0034】

この演算は、各駆動軸１３に生じた電流値（又は指令トルク）より推定される外乱トルクより、前述した逆干渉力補償のための演算によってエンドエフェクタ１１において生じた慣性力を求め、エンドエフェクタ軸１４に対し指令した加速度で除算すればよい。これにより、あらかじめ慣性質量を測定することなく、任意のエンドエフェクタ１１についての干渉力補償及び逆干渉力補償が可能である。

【0035】

あるいは、慣性質量の値は、エンドエフェクタ１１に固有のものであることから、エンドエフェクタ１１毎にあらかじめ測定し、エンドエフェクタ情報として用意しておいてもよい。エンドエフェクタ情報読み込み部２６は、かかるエンドエフェクタ情報が利用可能な場合に、エンドエフェクタ情報を例えば外部の機器から読み込み、慣性質量を得るものである。なお、エンドエフェクタ情報には、慣性質量以外の情報、例えば、フランジ１２に対するエンドエフェクタ１１の取り付け角度に関する情報等を含んでいてよい。これにより、エンドエフェクタ１１を異なるものに交換した場合などにも適切な慣性質量を用いて干渉力補償及び逆干渉力補償が可能である。

【0036】

なお、以上の説明では、ロボットコントローラ２がエンドエフェクタ軸制御部２１を有し、エンドエフェクタ軸１４をも制御するものとして説明した。かかる構成では、ロボットコントローラ２が直接エンドエフェクタ軸１４の駆動情報を得ることができるので、上述した干渉力補償や逆干渉力補償が容易に行える利点がある。しかしながらこれに限定されるものでなく、エンドエフェクタ軸制御部２１をロボットコントローラ２に対し別機器として用意し、両者が互いに通信することにより同期制御するものとしてもよいが、この場合には、エンドエフェクタ軸制御部２１とロボットコントーラ２との間で必要な情報を送受信できるよう通信回線を用意するとともに、通信によるラグの影響に注意を払う必要がある。

【0037】

また、上述の例では、慣性力を、エンドエフェクタ軸１４の駆動情報及び既知のエンドエフェクタ１１の慣性質量から演算することによって適時求めているが、これをあらかじめ既知のものとして与えておき、エンドエフェクタ軸１４の駆動の際に、所与の慣性力を参照することによって求めてもよい。これは、エンドエフェクタ軸１４の動作があらかじめ決められたものに限定されており、生じる慣性力が限られている場合などに有効である。この時、慣性力取得部２２は、エンドエフェクタ軸１４の動作に応じた慣性力を参照するとよい。慣性力を既知のものとして与える場合には、想定されるエンドエフェクタ１１の動作毎に、慣性力をあらかじめ測定しておいてもよいし、上述したように慣性力取得部２２が慣性力を演算により求めた後、その結果を記憶しておき、同条件によるエンドエフェクタ軸１４の動作の際に、記憶しておいた慣性力を参照するようにすることも可能である。既知のものとして与えられる慣性力は、例えば、上述したエンドエフェクタ情報に含まれており、エンドエフェクタ情報読み込み部２６により読み込まれてよい。

【0038】

[干渉力補償の演算]
つづいて、干渉力補償をする際の具体的な演算手法を説明する。

【0039】

本実施形態では、外乱トルク取得部２３による演算を、基本軸についての演算と、手首軸についての演算とに分けて行い、基本軸については、ヤコビ転置行列を用いて慣性力より各軸に作用する外乱トルクを求めており、手首軸については、幾何学的関係を用いて慣性力より各軸に作用する外乱トルクを求めている。

【0040】

一般にヤコビ転置行列を用いた演算は、その演算量が多く、また、ヤコビ転置行列の次数が大きくなると演算量は急激に増大する。そこで、一部の軸について演算量の少ない幾何学的関係による演算を行うことで、ヤコビ転置行列による演算を必要とする軸数を減らし、又その次数を減じることができるので、演算負荷を小さくできる。

【0041】

[基本軸についての演算]
まず、フランジ１２に対するエンドエフェクタ１１の取り付け角度を、フランジ座標ＯｆｌａｎｇｅのＸ軸、Ｙ軸、Ｚ軸に対する回転角度Ｒｘ、Ｒｙ、Ｒｚとした際の、フランジ座標Ｏｆｌａｎｇｅからツール座標Ｏｔｏｏｌへの回転行列ｒｏｔ_{ｆｌａｎｇｅ}は次の通りである。

【0042】

【数1】

【0043】

ここで、ツール座標Ｏｔｏｏｌはフランジ座標Ｏｆｌａｎｇｅに対し、Ｚ軸、Ｙ軸、Ｘ軸の順番で回転させるものとする。

【0044】

また、ツール座標Ｏｔｏｏｌからフランジ座標Ｏｆｌａｎｇｅへの回転行列ｒｏｔ_ｔｏｏｌは、次の通りである。

【0045】

【数2】

【0046】

さらに、ロボット座標Ｏｒｏｂｏｔ原点からみたＰ点の姿勢を示す行列をＩｎｒｅｐｏｓ、フランジ１２の姿勢を示す行列をｒｏｔとすると、ロボット座標Ｏｒｏｂｏｔからツール座標Ｏｔｏｏｌへの回転行列ｒｏｔ_{ｒｏｂｏｔ}は、次の通りとなる。

【0047】

【数3】

【0048】

この回転行列ｒｏｔ_{ｒｏｂｏｔ}を用い、ツール座標ＯｔｏｏｌのＺ方向に生じる慣性力をＦｚ_ｔｏｏｌとすると、ロボット座標Ｏｒｏｂｏｔにおいて生じる力は、次の通りである。

【0049】

【数4】

【0050】

さらに、ヤコビ転置行列Ｊ^Ｔを用いて、ロボット座標Ｏｒｏｂｏｔの外力をＳ軸、Ｌ軸、Ｕ軸に対する外乱トルクτ_Ｓ、τ_Ｌ、τ_Ｕに変換する。

【0051】

【数5】

【0052】

以上の演算により、慣性力Ｆｚ＿ｔｏｏｌにより生じるＳ軸、Ｌ軸、Ｕ軸に対する外乱トルクτ_Ｓ、τ_Ｌ、τ_Ｕがそれぞれ求められる。

【0053】

[手首軸についての演算]
手首軸については、幾何学的関係に基いて、Ｔ軸、Ｂ軸、Ｒ軸にそれぞれ作用する外乱トルクτ_Ｔ、τ_Ｂ、τ_Ｒを求める。

【0054】

このとき、ツール座標ＯｔｏｏｌのＺ方向に生じる慣性力Ｆｚ_ｔｏｏｌにより、フランジ座標Ｏｆｌａｎｇｅにおいて生じる力は、次の通りである。

【0055】

【数6】

【0056】

また、フランジ座標Ｏｆｌａｎｇｅにおいて、Ｐ点の座標を（Ｌｘ，Ｌｙ，Ｌｚ）とおく。

【0057】

Ｔ軸について、図４はフランジをＴ軸の軸方向から見た模式図である。ここで、Ｔ軸の回転中心は、フランジの中心に一致し、フランジ座標Ｏｆｌａｎｇｅの原点である。図より明らかなように、Ｔ軸に作用する外乱トルクτ_Ｔは、次の通りである。

【0058】

【数7】

【0059】

Ｂ軸について、図５Ａは、フランジをＴ軸の軸方向から見た模式図であり、θ_ＴはＴ軸の回転角である。また、図５Ｂは、フランジをＢ軸の軸方向から見た模式図であり、ｄ６は、Ｂ軸の回転中心からフランジ中心までの距離である。

【0060】

図より明らかなように、Ｂ軸に作用する外乱トルクτ_Ｂは、次の通りである。

【0061】

【数8】

【0062】

Ｒ軸について、図６Ａは、フランジをＴ軸の軸方向から見た模式図であり、θ_ＴはＴ軸の回転角である。また、図６Ｂは、フランジをＢ軸の軸方向から見た模式図であり、θ_ＢはＢ軸の回転角である。この時、図６Ｂの紙面垂直方向に作用する力は、図６Ａに紙面水平方向の力として現れるから、図より、Ｒ軸に作用する外乱トルクτ_Ｒは、次の通りである。

【0063】

【数9】

【0064】

このようにして、手首軸に作用する外乱トルクτ_Ｔ、τ_Ｂ、τ_Ｒは、幾何学的関係を用いて簡単に求めることができる。

【0065】

[逆干渉力補償の演算]
つづいて、逆干渉力補償をする際の具体的な演算手法を説明する。

【0066】

この演算においては、駆動軸１３により発生するトルクを、外乱トルクとおいて、前述の干渉力補償において行ったものと逆の演算をすればよいが、ここでは、エンドエフェクタ１１に比較的近い位置にある手首軸の駆動による逆干渉力による影響は、相対的に遠い位置にある基本軸の駆動による逆干渉力による影響に比べ小さいため、基本軸の駆動による逆干渉力の影響のみを考慮するものとする。もちろん、手首軸の駆動による逆干渉力の影響をも考慮するものとしてもよい。

【0067】

ここで、基本軸Ｓ軸、Ｌ軸、Ｕ軸において生じるトルクをそれぞれτ_Ｓ、τ_Ｌ、τ_Ｕとすると、これらトルクにより生じるロボット座標Ｏｒｏｂｏｔにおける逆慣性力は、ヤコビ転置行列Ｊ^Ｔの逆行列（Ｊ^Ｔ）^−１を用いて、次の通りとなる。

【0068】

【数10】

【0069】

これにさらに、前出の回転行列ｒｏｔ_{ｒｏｂｏｔ}の転置行列ｒｏｔ_{ｒｏｂｏｔ}^Ｔを掛けることにより、ツール座標Ｏｔｏｏｌにおけるｚ軸方向の逆慣性力Ｆｚ_ｔｏｏｌを次のように求めることができる。

【0070】

【数11】

【0071】

[ロボットコントローラの動作]
ロボットコントローラの動作を、図７に示すフロー図に従って説明する。

【0072】

まず、ステップＳＴ１において、エンドエフェクタ１１の慣性質量が既知であるか否かを判定する。既知である（肯定）場合にはステップＳＴ５へと進み、そうでなければ（否定）、ステップＳＴ２へと進み、エンドエフェクタ情報が利用可能であるか否かを判定する。

【0073】

ステップＳＴ２で肯定、すなわち、エンドエフェクタ情報が利用可能である場合には、ステップＳＴ３へと進み、エンドエフェクタ情報読込部２６によりエンドエフェクタ情報を読み込み、エンドエフェクタ情報に含まれる慣性質量を取得する。ステップＳＴ２で否定、すなわち、エンドエフェクタ情報が利用できない場合には、ステップＳＴ４へと進み、慣性質量推定部２５により、エンドエフェクタ軸１４を駆動することによって慣性質量を推定する。いずれの場合でも、慣性質量を取得後、ステップＳＴ５へと進む。

【0074】

ステップＳＴ５以降は、ロボット１の通常運転時の制御となるから、一般的なモーションプログラムや、上位コントローラからの指令に基いて、ロボット１のアーム１０及びエンドエフェクタ１１が所望の動作をするよう制御がなされる。図７では、特に、これら通常の制御と平行してなされる干渉力及び、逆干渉力補償に関する動作が示されている。

【0075】

ステップＳＴ５で、干渉力補償が有効であるか否かを判定する。本実施形態に係るロボットシステムでは、ユーザの指定等により、干渉力補償及び逆干渉力補償の有無を切り替えることができるものとし、ステップＳＴ５では、干渉力補償の有無を切り替えるための判断をしている。

【0076】

ステップＳＴ５で肯定、すなわち、干渉力補償が有効である場合には、ステップＳＴ６へ進み、慣性力取得部２２により慣性力を取得する。さらにステップＳＴ７へ進み、外乱トルク取得部２３により、慣性力より各駆動軸１３に対する外乱トルクを取得する。そして、ステップＳＴ８へ進み、得られた外乱トルクをトルク指令に対する修正値として駆動軸１３にフィードフォワード制御を行い、干渉力の補償を行う。

【0077】

ステップＳＴ５で否定、すなわち、干渉力補償が無効である場合及び、ステップＳＴ８の終了後は、ステップＳＴ９へと進み、逆干渉力補償が有効であるか否かを判定する。ステップＳＴ９では、逆干渉力補償の有無を切り替えるための判断をしていることになる。

【0078】

ステップＳＴ９で肯定、すなわち、逆干渉力補償が有効である場合には、ステップＳＴ１０へと進み、逆慣性力取得部２４により逆慣性力を取得する。さらにステップＳＴ１１に進み、得られた逆慣性力を駆動力指令に対する修正値とし、或いは逆慣性力をトルクに変換した上でトルク指令に対する修正値としてエンドエフェクタ軸１４にフィードフォワード制御を行い、逆干渉力の補償を行う。

【0079】

一連の動作の終了後、すなわち、ステップＳＴ９で否定の場合及び、ステップＳＴ１１の終了後は、ステップＳＴ５へと戻り、以降繰り返す。これにより、本願発明では、ロボットシステム１の動作に応じて、時々刻々と変化する干渉力及び逆干渉力がリアルタイムに取得され補償される。そのため、エンドエフェクタ１１において生じる慣性力や逆慣性力をあらかじめ測定しておく必要が無く、ロボットシステム１の動作を新規に作成したり変更したりした場合にも、直ちに干渉力及び逆干渉力に対する補償を行うことができ、位置決めの精度を確保するとともに、動作を高速化することができる。

【0080】

以上、本発明に係る実施形態について説明したが、この実施形態に示した具体的な構成は一例として示したものであり、本発明の技術的範囲をこれに限定することは意図されていない。当業者は、これら開示された実施形態を適宜変形してもよく、本明細書にて開示される発明の技術的範囲は、そのようになされた変形をも含むものと理解すべきである。

【符号の説明】

【0081】

１ ロボット、２ ロボットコントローラ、１０ アーム、１１ エンドエフェクタ、１２ フランジ、１３ 駆動軸、１４ エンドエフェクタ軸、２０ 駆動軸制御部、２１ エンドエフェクタ軸制御部、２２ 慣性力取得部、２３ 外乱トルク取得部、２４ 逆慣性力取得部、２５ 慣性質量推定部、２６ エンドエフェクタ情報読込部。

【図1】

【図2】

【図3】

【図4】

【図5A】

【図5B】

【図6A】

【図6B】

【図7】