特許 5916583 多関節ロボットのウィービング制御装置

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

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

(11)【特許番号】5916583

(24)【登録日】2016年4月15日

(45)【発行日】2016年5月11日

(54)【発明の名称】多関節ロボットのウィービング制御装置

(51)【国際特許分類】

   B25J 9/10 20060101AFI20160422BHJP

   B23K 9/12 20060101ALI20160422BHJP

【ＦＩ】

   B25J9/10 A

   B23K9/12 331K

   B23K9/12 350D

【請求項の数】6

【全頁数】20

(21)【出願番号】特願2012-231871(P2012-231871)

(22)【出願日】2012年10月19日

(65)【公開番号】特開2014-83605(P2014-83605A)

(43)【公開日】2014年5月12日

【審査請求日】2014年9月1日

(73)【特許権者】

【識別番号】000001199

【氏名又は名称】株式会社神戸製鋼所

(74)【代理人】

【識別番号】100061745

【弁理士】

【氏名又は名称】安田 敏雄

(74)【代理人】

【識別番号】100120341

【弁理士】

【氏名又は名称】安田 幹雄

(72)【発明者】

【氏名】和田 尭

(72)【発明者】

【氏名】西田 吉晴

(72)【発明者】

【氏名】井上 芳英

(72)【発明者】

【氏名】稲田 修一

【審査官】 木原 裕二

(56)【参考文献】

【文献】 特開２００６−０５９０３７（ＪＰ，Ａ）

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

【文献】 特開２００７−２１６２３２（ＪＰ，Ａ）

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

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

【文献】 特開平０７−０６０６６７（ＪＰ，Ａ）

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

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

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

【文献】 特開２０１３−２４８６８１（ＪＰ，Ａ）

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

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

Ｂ２３Ｋ ９／１２

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

【請求項1】

多関節ロボットに取り付けられたツールが所望のウィービング軌跡を描くように複数の関節軸を駆動させる多関節ロボットのウィービング制御装置において、
前記ウィービング軌跡に基づいて、前記多関節ロボットの各軸の目標位置信号を演算する信号演算部と、
前記演算された目標位置信号に対して、前記多関節ロボットの固有振動成分を除去するためのローパスフィルタ処理を行い、前記多関節ロボットの各軸についてフィルタ処理された目標指令信号を演算するフィルタ演算部と、
フィルタ処理された目標指令信号を入力として、前記多関節ロボットの各軸を駆動するモータ制御部とを含んで構成され、
前記モータ制御部におけるゲインの周波数特性が略フラットに構成され、
前記信号演算部は、ゲイン補正された目標位置信号を演算することを特徴とする、制御装置。

【請求項2】

前記モータ制御部は、位置フィードバック部と速度フィードバック部と速度フィードフォワード部とを含み、
前記信号演算部は、前記フィルタ演算部におけるフィルタ特性および前記ウィービング軌跡に基づいて、ゲイン補正された目標位置信号を演算することを特徴とする、請求項１に記載の制御装置。

【請求項3】

前記ローパスフィルタ処理で用いるフィルタは全軸で同様とし、
前記信号演算部は、前記フィルタ演算部のフィルタ特性から得られるウィービング周波数におけるフィルタゲインの逆数を目標位置信号に乗算することにより、ゲイン補正された目標位置信号を演算することを特徴とする、請求項１または請求項２に記載の制御装置。

【請求項4】

前記信号演算部は、前記フィルタ演算部のフィルタの特性およびウィービング周波数に基づいて、ゲイン補正に加えて、位相補償された目標位置信号を演算することを特徴とする、請求項１または請求項２に記載の制御装置。

【請求項5】

前記モータ制御部は、自軸が加減速することにより自軸に作用する慣性力、重力および摩擦力を予測し、前記速度フィードバック部から出力されるトルク指令値に加えるフィードフォワード制御部をさらに含むことを特徴とする、請求項２〜請求項４のいずれかに記載の制御装置。

【請求項6】

前記モータ制御部は、他軸が加減速することにより自軸に作用する慣性力および遠心コリオリ力を予測し、前記速度フィードバック部から出力されるトルク指令値に加えるフィードフォワード制御部をさらに含むことを特徴とする、請求項５に記載の制御装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、たとえば、アーク溶接に用いられる多関節ロボットの制御に関し、特に、高い軌跡精度でウィービング動作を可能とする多関節ロボットの制御に関する。

【背景技術】

【0002】

アーク溶接により複数の母材の溶接を行う際には、溶接電極を溶接方向に進ませつつ、溶接線の左右方向に正弦波ウィービング動作をさせながら溶接するウィービング溶接が採用される。このウィービング溶接は、従来から、溶接トーチ自体を左右に揺動させるか、または溶接トーチ自体を中心として左右に傾動させることにより行っている。このようなウィービング溶接を多関節ロボットに行わせる場合、高い軌跡精度が要求される。

【0003】

このようなウィービング動作について、以下のような技術が公知である。
特開２００６−５９０３７号公報（特許文献１）は、予め教示された振幅で確実にかつ正確にウィービング動作させることのできる溶接ロボット制御装置を開示する。
溶接ロボットにウィービング動作を行わせる際、指令されたウィービング周波数や振幅に基づいて、各関節のモータの角度指令値が生成され、その指令値に基づいて制御される。ただし、モータの動特性（フィードバック系の制御時定数）に依存して、モータは角度指令値通りに動作することができず、結果として、指令された振幅でウィービングすることができない。そこで、この特許文献１に開示された技術では、以下のように、この問題を解決している。まず、ウィービング動作しない場合とウィービング動作する場合との各関節の角度指令値の差をウィービング信号として算出する。そして、モータのフィードバック系の制御時定数とウィービング周波数とに基づいて、モータのフィードバック系のゲイン（出力振幅／指令振幅）を算出し、算出されたゲインの逆数を、ウィービング信号に乗じ、その値をウィービング動作しない場合の各関節の角度指令値に加算することにより、最終的な各関節の角度指令値を算出する。

【0004】

また、特開平６−２２２８１７号公報（特許文献２）は、ウィービング溶接が行われるロボットに適用されてウィービング波形を精度よく描かせることができる技術を開示する。
この特許文献２に開示された技術では、モータの動特性に依存してウィービング振幅が変動する問題点を解決するため、ロボットの姿勢に基づいて決まるモータ駆動系のモデル（伝達関数Ｇ（ｓ））を求め、ウィービング動作時の各軸の角度指令値に１／Ｇ（ｓ）を乗算された角度指令値をモータに対して入力する。

【先行技術文献】

【特許文献】

【0005】

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

【特許文献2】特開平６−２２２８１７号公報

【発明の概要】

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

【0006】

多関節ロボットの各軸を動作されるモータは、ウィービング動作のような周期信号を指令値として入力した場合、出力角度は指令値に対して振幅・位相が変化するという特性を有している。このため、ロボット先端位置を正確に動かすためには、この変化特性を予測して角度指令値を補正する必要がある。また、この振幅・位相の変化に関与する要因は、モータ自身の動特性（フィードバック系の制御時定数）と、外部からの外力（ロボットの場合では、他軸が動くことによる影響）とがある。問題点を整理すると、以下のように纏められる。

【0007】

（１）モータのフィードバック系の制御時定数により、指令値に対してモータ角度の振幅が変化する。
（２）各モータのフィードバック系の制御時定数が異なるため、指令値に対して各モータ角度の位相が異なる。
（３）他軸の動作影響により、指令値に対してモータ角度の振幅が変化する。

【0008】

（４）他軸の動作影響により、指令値に対して各モータ角度の位相が異なる。
しかしながら、上述した特許文献１では、これらの問題点のなかの（１）のみを解決した技術であって、（２）〜（４）については何ら考慮されていない。
さらに、上述したこのような問題点を解決するための方法として、角度指令値を補正することが挙げられるが（特許文献１もこの手法を採用）、補正を行うためには、角度指令値に対する出力角度の変化特性をモデル化し、どの程度変化するのかを正確に予測する必要がある。補正する量が大きくなったり、モデルが複雑になったりすると、当然ながら誤差も増える。この誤差を低減させるためには、そもそも、上述した変化特性による振幅・位相の変化量を極力小さくするための制御を行う必要もある。

【0009】

また、特許文献２では、上述した４つの問題点のなかの（１）および（２）を解決した技術であり、（３）および（４）については何ら考慮されていない。
さらに、特許文献１に開示された技術と同じように、特許文献２に開示された技術も、モータの特性をモデル化し、振幅・位相の変化量を予測し、角度指令値を補正することにより問題を解決するため、根本的に補正量は小さくならず、補正する量が大きくなったり、モデルが複雑になると誤差が大きくなったりする。

【0010】

すなわち、これらの特許文献に開示された技術では、上述した４つ全ての問題点を解決して、高い軌跡精度でのウィービング動作を実現することができていない。
本発明は、上述の問題点に鑑みてなされたものであり、複数軸を備えた多関節ロボットにおいて、多関節ロボットの軸を動作させるモータ自身の動特性（上述した問題点（１）、（２））に起因するウィービング動作の誤差、および、他軸が動作することによる影響（上述した問題点（３）、（４））に起因するウィービング動作の誤差、が生じることを抑制して、高い軌跡精度でウィービング動作を可能とすることができる、多関節ロボットのウィービング制御装置を提供することを目的とする。

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

【0011】

上記課題を解決するため、本発明に係る多関節ロボットのウィービング制御装置は、以下の技術的手段を講じている。
即ち、本発明に係る多関節ロボットのウィービング制御装置は、多関節ロボットに取り付けられたツールが所望のウィービング軌跡を描くように複数の関節軸を駆動させる。このウィービング制御装置は、前記ウィービング軌跡に基づいて、前記多関節ロボットの各軸の目標位置信号を演算する信号演算部と、前記演算された目標位置信号に対して、前記多関節ロボットの固有振動成分を除去するためのローパスフィルタ処理を行い、前記多関節ロボットの各軸についてフィルタ処理された目標指令信号を演算するフィルタ演算部と、フィルタ処理された目標指令信号を入力として、前記多関節ロボットの各軸を駆動するモータ制御部とを含んで構成される。前記モータ制御部におけるゲインの周波数特性が、略フラットに構成され、前記信号演算部は、ゲイン補正された目標位置信号を演算することを特徴とする。

【0012】

好ましくは、前記モータ制御部は、位置フィードバック部と速度フィードバック部と速度フィードフォワード部とを含むように構成することができ、前記信号演算部は、前記フィルタ演算部におけるフィルタ特性および前記ウィービング軌跡に基づいて、ゲイン補正された目標位置信号を演算するように構成することができる。
さらに好ましくは、前記ローパスフィルタ処理で用いるフィルタは全軸で同様とするように構成することができ、前記信号演算部は、前記フィルタ演算部のフィルタ特性から得られるウィービング周波数におけるフィルタゲインの逆数を目標位置信号に乗算することにより、ゲイン補正された目標位置信号を演算するように構成することができる。

【0013】

さらに好ましくは、前記信号演算部は、前記フィルタ演算部のフィルタ特性およびウィービング周波数に基づいて、ゲイン補正に加えて、位相補償された目標位置信号を演算するように構成することができる。
さらに好ましくは、前記モータ制御部は、自軸が加減速することにより自軸に作用する慣性力、重力および摩擦力を予測し、前記速度フィードバック部から出力されるトルク指令値に加えるフィードフォワード制御部をさらに含むように構成することができる。

【0014】

さらに好ましくは、前記モータ制御部は、他軸が加減速することにより自軸に作用する慣性力および遠心コリオリ力を予測し、前記速度フィードバック部から出力されるトルク指令値に加えるフィードフォワード制御部をさらに含むように構成することができる。

【発明の効果】

【0015】

本発明に係るウィービング制御装置を用いることにより、複数軸を備えた多関節ロボットにおいて、多関節ロボットの軸を動作させるモータ自身の動特性に起因するウィービング動作の誤差、および、他軸が動作することによる影響に起因するウィービング動作の誤差、が生じることを抑制して、高い軌跡精度でのウィービング動作が可能となる。

【図面の簡単な説明】

【0016】

【図1】本発明の実施の形態に係るウィービング制御装置が適用される多関節ロボットの全体構成を示す概略図である。

【図2】本発明の第１の実施の形態に係るウィービング制御装置の制御ブロック図である。

【図3】図２のフィルタ演算部およびモータ制御部の詳細な制御ブロック図である。

【図4】図３に示した制御ブロック図の簡略版である。

【図5】図４に示す制御ブロック図により制御された多関節ロボットの出力波形を示す図である。

【図6】図２に示した制御ブロック図の簡略版である。

【図7】図６に示す制御ブロック図により制御された多関節ロボットの出力波形を示す図（その１）である。

【図8】図６に示す制御ブロック図により制御された多関節ロボットの出力波形を示す図（その２）である。

【図9】図６に示す制御ブロック図により制御された多関節ロボットの出力波形を示す図（その３）である。

【図10】本発明の第２の実施の形態に係るウィービング制御装置の制御ブロック図である。

【図11】２リンクロボットの模式図である。

【図12】図１０に示す制御ブロック図（ただし速度ＦＦのみ）により制御された多関節ロボットの出力波形を示す図である。

【図13】図１０に示す制御ブロック図（速度ＦＦ＋３種ＦＦ）により制御された多関節ロボットの出力波形を示す図である。

【図14】図１０に示した制御ブロック図の簡略版である。

【図15】図１４に示す制御ブロック図により制御された多関節ロボットの出力波形を示す図である。

【図16】本発明の第３の実施の形態に係るウィービング制御装置の制御ブロック図である。

【図17】図１６に示す制御ブロック図（速度ＦＦ＋３種ＦＦ）により制御された多関節ロボットの出力波形を示す図である。

【図18】図１６に示す制御ブロック図（速度ＦＦ＋５種ＦＦ）により制御された多関節ロボットの出力波形を示す図である。

【図19】図１６に示した制御ブロック図の簡略版である。

【図20】図１９に示す制御ブロック図により制御された多関節ロボットの出力波形を示す図である。

【図21】本発明の実施の形態の変形例における、基本位置信号とウィービング信号の生成方法を示す図である。

【図22】本発明の実施の形態の変形例に係るウィービング制御装置の制御ブロック図である。

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

【0017】

以下、本発明の実施の形態に係る多関節ロボットのウィービング制御装置を、図面に基づき詳しく説明する。なお、以下の説明では、同一の部品には同一の符号を付してある。それらの名称および機能も同じである。したがって、それらについての詳細な説明は繰り
返さない。
＜第１の実施の形態＞
［全体構成］
まず、第１の実施の形態に係るウィービング制御装置が適用される垂直多関節ロボット（以下、単に多関節ロボットと記載する場合がある）の概要について説明する。

【0018】

図１は、溶接トーチを傾動動作（ウィービング動作）させるロボットの一例であって、本実施の形態に係るウィービング制御装置が適用される多関節ロボット１の概要を示す図である。この多関節ロボット１は、垂直多関節型であってＪ１〜Ｊ６の６関節を備え、Ｊ６軸の先端に溶接トーチが設けられ、溶接トーチから送りだされる溶接ワイヤによりアーク溶接が行われる。この多関節ロボット１は、予め定められた溶接開始点と溶接終了点との間が溶接作業区間であって、溶接開始点と溶接終了点とを結ぶ溶接線方向に移動しつつ、溶接ワイヤを予め定められた振幅および周波数で傾動する動作（ウィービング動作）を行うようにセットされている。

【0019】

このような多関節ロボット１は、図示した多関節ロボット１の本体に加えて、教示ペンダントを備え各軸をサーボ制御する制御装置（サーボ制御部）と、上位コンピュータ（上位ＣＰＵ）とを含む。これらの制御装置および上位コンピュータにより、第１の形態に係るウィービング制御装置１０が実現されている。
制御装置（サーボ制御部）は、多関節ロボット１に設けられた溶接トーチを、予め教示したプログラムに従って、上述した溶接線に倣ってウィービング動作して移動するように制御する。教示プログラムは、制御装置に接続された教示ペンダントを使用して作成する場合や、上位コンピュータを利用したオフライン教示システムを使用して作成する場合がある。いずれの場合であっても、教示プログラムは、実際の動作の前に予め作成される。

【0020】

［制御ブロック］
図２に、図１の多関節ロボット１を制御するウィービング制御装置１０の制御ブロック図を示す。このウィービング制御装置１０は、多関節ロボット１に取り付けられたツール（ここでは溶接トーチ）に所望の動作（ここではウィービング動作）を行わせるように複数の関節軸を駆動させる。図２は、複数ある関節軸（図１の例示では６軸）のなかの１つの関節軸に対応している。

【0021】

このウィービング制御装置１０は、ウィービング軌跡に基づいて、多関節ロボット１の各軸の目標位置信号を演算する信号演算部（ウィービング信号生成部１００、ウィービング信号補正部２００、基本位置信号生成部３００）と、演算された目標位置信号に対して、多関節ロボット１の固有振動成分を除去するためのローパスフィルタ処理を行い、多関節ロボット１の各軸についてフィルタ処理された目標指令信号を演算するフィルタ演算部４００と、フィルタ処理された目標指令信号を入力として、多関節ロボット１の各軸を駆動するモータ制御部５００とを含む。ここで、モータ制御部５００は、位置フィードバック部５１０と速度フィードバック部と速度フィードフォワード部５２０とを含み、モータ制御部５００におけるゲインの周波数特性が、略フラットに構成され、信号演算部のウィービング信号補正部２００は、ゲイン補正された目標位置信号を演算することが特徴である。ここで、略フラットとは、モータ制御部５００におけるゲインの周波数特性が、良好なウィービング動作に影響を与えない程度に、フラットに構成されていることを意味する。言い換えれば、モータ制御部５００の特性をボード線図で示した場合、ボード線図上のゲイン曲線が平坦、または、良好なウィービング動作に影響を与えない程度に平坦になっていることを意味する。

【0022】

なお、以下において、制御ブロックにおける要素の特性は（動特性との記載がなくても）全て動特性である。また、ＦＢの記載はフィードバックを意味し、ＦＦの記載はフィードフォワードを意味する。以下に、さらに詳しく各ブロックについて説明する。
基本位置信号生成部３００は、ウィービング動作しない場合の多関節ロボット１の先端軌跡を描くための多関節ロボット１の各軸の角度位置として基本位置信号を生成する。

【0023】

ウィービング信号生成部１００は、ウィービング動作する場合の多関節ロボット１の先端軌跡を描くための多関節ロボット１の各軸の角度位置から、基本位置信号生成部３００
で生成された角度位置を引いた角度位置としてウィービング信号を生成する。この角度位置は、基本的に、指定されるウィービング周波数の周期信号になる。
ウィービング信号補正部２００は、ウィービング信号生成部１００で生成されたウィービング信号を補正し、ウィービング補正信号を生成する。第１の実施の形態においては、ウィービング信号補正部２００における補正方法が、フィルタ演算部４００で使用するフィルタに基づいて行うことが特徴である。この補正方法の詳細については、後述する。

【0024】

フィルタ演算部４００は、基本位置信号とウィービング補正信号とを足し合わせた目標位置信号に対して、多関節ロボット１の固有振動成分を除去するためのローパスフィルタ処理を行い、フィルタ後目標位置信号を生成する。ここで生成されるフィルタ後目標位置信号はモータ制御部５００での制御方法に応じて、角度位置に加え、角速度、角加速度も生成する。フィルタ処理方法の詳細については、後述する。

【0025】

モータ制御部５００は、フィルタ後目標位置信号に基づいて、モータ５５０を制御する。最も一般的なモータ制御方法として、図２の詳細図である図３に示すように、エンコーダ５６０の検出角度に基づいた位置（フィードバック）制御部５１０、速度（フィードバック）制御部５３０、電流制御部５４０による制御が行われる。第１の実施の形態においては、これらに加えて、フィルタ後目標速度信号に速度フィードフォワードゲインを乗算することによる速度フィードフォワード制御を行う速度フィードフォワード部５２０を加えることを特徴とする。モータ制御方法の詳細については、後述する。

【0026】

第１の実施の形態に係るウィービング制御装置１０は、モータ制御部５００での制御方法に速度フィードフォワード制御を加えることにより、モータ５５０の動特性による遅れを低減し、目標位置信号からエンコーダで検出される現在位置信号までの動特性をフィルタ演算部４００で使用されるフィルタで近似できることを特徴とする。
そして、目標位置信号から現在位置信号までの動特性を任意設計したフィルタに近似できることを利用して、ウィービング信号をこのフィルタ特性を考慮して補正し、ウィービング補正信号を生成することを特徴とする。

【0027】

これにより、多関節ロボット１の各軸でのモータ５５０の動特性による遅れによって乱れるウィービング波形を、設定したフィルタに基づいて補正するだけで、良好なウィービング波形を得ることができる。
［フィルタ処理方法およびモータ制御方法］
ここでは、図２または図３に示す、第１の実施の形態に係るウィービング制御装置１０のフィルタ演算部４００、および、モータ制御部５００について説明する。

【0028】

ここで、Ｔはフィルタ時定数、Ｋｘｐは位置制御部５１０の比例ゲイン、ＫｖｐとＫｖｉとは速度制御部５３０の比例ゲインと積分ゲイン、Ｋｖｆは速度フィードフォワード部５２０のフィードフォワードゲイン、ｓはラプラス演算子を示す。
なお、Ｔ、Ｋｘｐ、Ｋｖｐ、Ｋｖｉは、多関節ロボット１の動特性に応じて設定し、Ｋｖｆは０〜１の値で設定される。また、ここでは、フィルタ演算部４００では１次のローパスフィルタを用いてフィルタ後目標位置信号として、角度位置、角速度を計算する。そして、角度位置、角速度に基づいて図３に示すモータ制御が行われる。なお、フィルタ演算部４００のローパスフィルタは、必要に応じて２次や３次のフィルタを用いてもよい。

【0029】

図２で示した制御方法によりモータ５５０の動特性による遅れを低減し、図３に示す目標位置信号θから現在位置信号θｒまでの特性を、フィルタ演算部４００で使用される１次のローパスフィルタ（Ｆ（ｓ）＝１／（Ｔ・ｓ＋１））で近似できることを、図４を参照して説明する。
図４は、目標位置信号θをある正弦波信号とした場合の各出力波形を表している。実線が目標位置信号、破線がフィルタ後目標位置信号、一点鎖線が速度フィードフォワード制御を使用した時のモータ５５０の現在位置信号、点線が速度フィードフォワード制御を使用していない時のモータ５５０の現在位置信号である。これを見ると、速度フィードフォワード制御使用時の現在位置信号はフィルタ後目標位置信号に近く、目標位置信号θから現在位置信号θｒまでの特性をフィルタ特性に近似できていることがわかる。また、一方で、速度フィードフォワード制御を使用していない時には、大きく振幅が減少しており、
補正を行う際には大きな補正を行う必要があることがわかる。

【0030】

［ウィービング信号補正方法］
上述したフィルタ処理方法およびモータ制御方法を用いることにより、目標位置信号θから現在位置信号θｒまでの動特性を、任意に設計したフィルタの伝達関数で近似することができる。ウィービング信号補正部２００では、このことを利用して、ウィービング信号を補正する。以下、２通りの補正方法（ゲイン補正方法、ゲイン位相補正方法）を説明する。

【0031】

・ゲイン補正方法
このゲイン補正方法では、ウィービング信号生成部１００で生成されたウィービング信号（ウィービング周波数はω[Ｈｚ]）にフィルタ演算部４００で使用されるフィルタＦ（ｓ）の周波数ωでのゲイン（フィルタゲイン）の逆数を乗算することにより、ウィービング補正信号を生成する。このときのウィービング補正信号の算出式は、以下の式（１）で表される。

【0032】

【数1】

【0033】

これにより、モータ５５０に対して、周期信号を入力した際の振幅変化を補正することができる。なお、ここでの補正は、モータ５５０の動特性に依存せず、任意設計したフィルタにのみ基づいて補正を行うことができる。
ただし、ここでは位相変化に対しては補正を行っていないため、各軸のフィルタ演算部４００で異なるフィルタを設定すると、各軸の位相変化量が異なり、良好なウィービング波形が得られない。そのため、このゲイン補正方法を用いる際はフィルタ演算部４００で使用するフィルタを多関節ロボット１の全軸で同じものとし、全軸の位相変化量を同等にすることが好ましい。

【0034】

このようなゲイン補正方法（補正方法１）を使用する制御ブロック（図６）を用いた場合の出力波形を図７に示す。
図７には、補正方法１による出力波形と、比較のために従来技術１（特許文献１による技術、以下では従来技術１と記載）による補正を実施した際の出力波形を表している。実線がウィービング信号、一点鎖線が補正方法１によるモータ５５０の現在位置信号、点線が従来技術１によるモータ５５０の現在位置信号である。なお、ここでは単軸のみ駆動するものとしているため、他軸が動作することによる影響はでていない。

【0035】

これを見ると、どちらのモータ５５０の現在位置信号の振幅もウィービング信号の振幅に近く適切に補正できていることがわかる。ただ、従来技術１による補正は、完全にモータモデルが分かっているとして補正を行っている。
図８に、このような従来技術１において、モータモデルがずれた際の結果を示す。この図８を見ると、従来技術１によるモータ５５０の現在位置信号の振幅は大きくずれていることがわかる。すなわち、従来技術１による補正でモータモデルがずれると、好ましい制御が実現できないことを示す。

【0036】

・ゲイン位相補正方法
このゲイン位相補正方法では、ウィービング信号生成部１００で生成されたウィービング信号（ウィービング周波数はω[Ｈｚ]）にフィルタ演算部４００で使用されるフィルタＦ（ｓ）およびウィービング周波数に基づいて、ウィービング信号の振幅変化および位相遅れを推測して補償を行う。たとえば、位相遅れは以下の式（２）で、振幅変化は以下の式（３）で推測し、これらの値に基づき、位相進み補償およびゲイン補正を行う。

【0037】

これにより、モータ５５０に対して、周期信号を入力した際の振幅変化および位相変化を補正することができる。

【0038】

【数2】

【0039】

【数3】

【0040】

図９に、このようなゲイン位相補正方法（補正方法２）を使用する制御ブロック（図６）を用いた場合の出力波形を示す。補正方法２は、補正方法１に対して、位相の遅れも補償する。このため、図９を見ると、補正方法１よりも、ウィービング信号と、補正方法２によるモータ５５０の現在位置信号との位相のずれが少ないことがわかる。
以上のようにして、第１の実施の形態に係るウィービング制御装置によると、モータ制御部において速度フィードフォワード制御を実施することにより、目標位置からモータ出力角度までの特性をローパスフィルタ処理で使用するフィルタに近似することができる。このフィルタに基づき、目標位置の振幅の補正を行うようにしたので、多関節ロボットにおいて、多関節ロボットの軸を動作させるモータ自身の動特性に起因するウィービング動作の誤差が生じることを抑制して、高い軌跡精度でウィービング等の動作を可能とすることができる。なお、各軸の位相のずれは全軸同じフィルタを用いることで解決している。その結果、従来技術では、制御装置も含めたモータのモデル化を行ってモデルに基づいて目標位置の補正を行ってために発生していた問題点（補正にかかる計算量が多く、モデルの誤差が大きい場合には上手く補正できない）を解決できるようになった。

【0041】

＜第２の実施の形態＞
以下、本発明の第２の実施の形態に係るウィービング制御装置２０について説明する。なお、本実施の形態に係るウィービング制御装置は、上述した第１の実施の形態に係るウィービング制御装置１０のモータ制御部の制御特性の与え方が異なる。それ以外は、第１の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

【0042】

上述した第１の実施の形態においては、モータ制御部５００で速度フィードフォワード部５２０を設けることにより、目標位置信号から現在位置信号までの動特性を任意設計したフィルタに近似できるようにした。しかしながら、この近似精度では十分でない場合がある。
この理由としては、多関節ロボット１の各関節は、多関節ロボット１の姿勢に応じて変化する慣性力または／および重力の影響、速度等に依存している摩擦力の影響を受けているため、速度フィードフォワード制御のみでは、目標位置信号から現在位置信号までの動特性を十分に任意設計したフィルタに近似させることができない。

【0043】

第２の実施の形態に係るウィービング制御装置２０は、目標位置信号から現在位置信号までの動特性を任意設計したフィルタにより厳密に近似できるようにするための制御を加える。このようにすると、任意設計したフィルタに基づいてウィービング信号の補正を行うだけでウィービング精度をさらに向上できることになる。
図１０は、第２の実施の形態に係るウィービング制御装置２０の制御ブロック図である。多関節ロボット１のある関節軸に対応している。図１０も図２と同様に、複数ある関節軸のなかの１つの関節軸に対応している。

【0044】

図２に示したウィービング制御装置１０との差は、ウィービング制御装置２０は、モータ制御部５００に代えてモータ制御部１５００を備える点、そのモータ制御部１５００に他軸の情報を与える他軸目標位置信号出力部６００をさらに備える点である。
モータ制御部１５００は、ウィービング制御装置１０のモータ制御部５００の構成に加えて、慣性項フィードフォワード部１５１０、重力項フィードフォワード部１５２０および摩擦項フィードフォワード部１５３０を備える。他軸目標位置信号出力部６００から出力される、フィルタ後の目標位置信号（他軸）は、これらの慣性項フィードフォワード部１５１０、重力項フィードフォワード部１５２０および摩擦項フィードフォワード部１５３０に入力される。３種類のフィードフォワード部では、フィルタ後の目標位置信号（他軸）に基づいて、多関節ロボット１の姿勢がわかり、自軸の状態がわかるので、自軸の慣性等がわかる。以下に、各ブロックについて詳しく説明する。

【0045】

他軸目標位置信号出力部６００は、他軸で同様に生成されるフィルタ演算部４００から
の出力であるフィルタ後目標位置信号（角度、角速度、角加速度）を出力する。たとえば、多関節ロボット１が６軸の場合には、対象としている軸以外の５軸分のフィルタ後目標位置信号をモータ制御部１５００へ出力する。
慣性項フィードフォワード部１５１０は、多関節ロボット１の姿勢により決まる自軸に働く慣性力を予測し、電流制御部５４０のフィードフォワード制御のトルク指令値として出力する。

【0046】

重力項フィードフォワード部１５２０は、多関節ロボット１の姿勢により決まる自軸に働く重力影響を予測し、電流制御部５４０のフィードフォワード制御のトルク指令値として出力する。
摩擦項フィードフォワード部１５３０は、自軸に働く摩擦力を予測し、電流制御部５４０のフィードフォワード制御のトルク指令値として出力する。

【0047】

第２の実施の形態に係るウィービング制御装置２０の特徴は、ウィービング制御装置１０のモータ制御部５００に３種のフィードフォワード制御部を加えることにより、目標位置信号からエンコーダ５６０で検出される現在位置信号までの動特性を、より厳密にフィルタ演算部４００で使用されるフィルタに近似できることである。
これにより、第１の実施の形態にて説明した、フィルタに基づいてウィービング信号の補正を行うだけで、良好なウィービング波形を得ることができる。

【0048】

なお、ここで実施している３種のフィードフォワード制御は、多関節ロボット１のモデル（運動方程式）に基づいて設計される。ここでは、図１１に示す２リンクロボットを対象として、各種フィードフォワード制御（慣性項、重力項、摩擦項）の計算方法について説明する。
図１１に示す２リンクロボットにおける運動方程式を求めると、各関節に作用するトルクτ_１、τ_２は、以下の式（４）および式（５）でそれぞれ表される。そして、以下の式（６）〜式（１３）に示すように計算されて、１軸目に関する３種類のフィードフォワード制御（慣性項、重力項、摩擦項）の出力が式（１４）に示すように求まる。

【0049】

【数4】

【0050】

【数5】

【0051】

【数6】

【0052】

【数7】

【0053】

【数8】

【0054】

【数9】

【0055】

【数10】

【0056】

【数11】

【0057】

【数12】

【0058】

【数13】

【0059】

【数14】

【0060】

上記の３種類のフィードフォワード制御を実施すれば、目標位置信号からエンコーダ５６０で検出される現在位置信号までの動特性を、フィルタ演算部４００で使用されるフィルタに一致させることができる。なお、第２の実施の形態においては、他軸の動作の影響に関しては考慮していないので、単軸動作する場合に限る。他軸が動作することによる影響を考慮した場合は後述の第３の実施の形態で検討する。

【0061】

また、上記の３種類のフィードフォワード制御を部分的に実施するだけでも、目標位置信号からエンコーダ５６０で検出される現在位置信号までの動特性を、フィルタ演算部４００で使用されるフィルタ特性に（第１の実施の形態よりも）さらに近づけることができる。
図１２および図１３は、上記の２リンクロボットの２軸目に、ある正弦波信号を目標位置信号として入力した場合の出力波形を表している（１軸目の目標位置信号はゼロ）。実線が目標位置信号、破線がフィルタ後目標位置信号、一点鎖線がモータ５５０の現在位置信号である。

【0062】

また、図１２が第１の実施の形態に対応するものであって、速度フィードフォワード制御のみを使用した場合の出力波形を示し、図１３が第２の実施の形態に対応するものであって、速度フィードフォワード制御、慣性項フィードフォワード制御、重力項フィードフォワード制御および摩擦項フィードフォワード制御を使用した場合の出力波形を示している。

【0063】

これらを見ると、速度フィードフォワード制御に加えて３種類のフィードフォワード制御を実施することにより、速度フィードフォワード制御のみの場合よりも、現在位置信号がフィルタ後目標位置信号にさらに近づき、目標位置信号から現在位置信号までの動特性をフィルタ特性に近似する際の精度を向上できることがわかる。
これにより、第１の実施の形態において説明したように、フィルタに基づいてウィービング信号の補正を行うだけで、良好なウィービング波形を得ることができる。

【0064】

第１の実施の形態にて説明したゲイン補正方法（補正方法１）を使用する制御ブロック（図１４）を用いた場合の出力波形を図１５に示す。図１５には、補正方法１を第２の実施の形態に適用した場合の出力波形と、比較のために、補正方法１を第１の実施の形態に適用した場合の出力波形とを表す。
図１５は、２軸目に、ある正弦波信号をウィービング信号として入力した場合に、補正方法１（ゲイン補正）を行った場合の２軸目の出力波形を表している。実線がウィービング信号、一点鎖線が第２の実施の形態（実施例２）にて補正方法１を行った場合のモータ
５５０の現在位置信号、点線が第１の実施の形態（実施例１）にて補正方法１を行った場合のモータ５５０の現在位置信号である。なお、ここでは、単軸のみ駆動するものとしているため、他軸が動作することによる影響はでていない。これを見ると、第２の実施の形態の方がモータ５５０の現在位置信号の振幅がウィービング信号の振幅に近く、適切に補正できていることがわかる。

【0065】

以上のようにして、第２の実施の形態に係るウィービング制御装置によると、第１の実施の形態に係るウィービング制御装置における制御に加えて、各軸に働く慣性力、重力および摩擦力を予め予測し、適切なトルク指令値としてモータへ入力することにより、目標位置からモータ出力角度までの動特性をフィルタへ近似する精度を向上させることができる。このフィルタに基づき、目標位置の振幅の補正を行うようにしたので、多関節ロボットにおいて、多関節ロボットの軸を動作させるモータ自身の動特性に起因するウィービング動作の誤差、および、他軸が動作することによる自軸の影響に起因するウィービング動作の誤差、が生じることを抑制して、高い軌跡精度でウィービング等の動作を可能とすることができる。

【0066】

＜第３の実施の形態＞
以下、本発明の第３の実施の形態に係るウィービング制御装置３０について説明する。なお、本実施の形態に係るウィービング制御装置は、上述した第１の実施の形態に係るウィービング制御装置１０のモータ制御部の制御特性の与え方が異なる。それ以外は、第１の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

【0067】

上述した第２の実施の形態においては、第１の実施の形態で実装した速度フィードフォワード部５２０に３種類のフィードフォワード制御部を設けることにより、目標位置信号から現在位置信号までの動特性を任意設計したフィルタにさらに近似できるようにした。しかしながら、第２の実施の形態においては、他軸が動作することによる影響に関しては考慮していない。このため、他軸の影響を考慮して近似しなければならない場合もある。

【0068】

第３の実施の形態に係るウィービング制御装置３０は、目標位置信号から現在位置信号までの動特性を任意設計したフィルタによりさらに厳密に近似できるようにするための制御を加える。このようにすると、任意設計したフィルタに基づいてウィービング信号の補正を行うだけでウィービング精度をさらに向上できることになる。
図１６は、第３の実施の形態に係るウィービング制御装置３０の制御ブロック図である。多関節ロボット１のある関節軸に対応している。図１６も、図２および図１０と同様に、複数ある関節軸のなかの１つの関節軸に対応している。

【0069】

図１０に示したウィービング制御装置２０との差は、ウィービング制御装置３０は、モータ制御部１５００に代えてモータ制御部２５００を備える点である。
モータ制御部２５００は、ウィービング制御装置２０のモータ制御部１５００の構成に加えて、干渉慣性項フィードフォワード部２５１０および遠心コリオリ項フィードフォワード部２５２０を備える。他軸目標位置信号出力部６００から出力される、フィルタ後の目標位置信号（他軸）は、これらの干渉慣性項フィードフォワード部２５１０および遠心コリオリ項フィードフォワード部２５２０に入力される。なお、他軸目標位置信号出力部６００から、慣性項フィードフォワード部１５１０、重力項フィードフォワード部１５２０および摩擦項フィードフォワード部１５３０にフィルタ後の目標位置信号（他軸）が入力される点は、ウィービング制御装置２０と同じである。以下に、各ブロックについて詳しく説明する。

【0070】

干渉慣性項フィードフォワード部２５１０は、他軸が動作することによる自軸のアームに働く慣性力を予測し、電流制御部５４０のフィードフォワード制御のトルク指令値として出力する。
遠心コリオリ項フィードフォワード部２５２０は、他軸が動作することによる自軸のアームに働く遠心コリオリ力を予測し、電流制御部５４０のフィードフォワード制御のトルク指令値として出力する。

【0071】

第３の実施の形態に係るウィービング制御装置３０の特徴は、第２の実施の形態に係る
ウィービング制御装置２０のモータ制御部１５００に、さらに、他軸が動作した際の影響を補償するための２種類のフィードフォワード制御部を加えることにより、目標位置信号からエンコーダ５６０で検出される現在位置信号までの動特性を、より厳密にフィルタ演算部４００で使用されるフィルタに近似できることである。

【0072】

これにより、第１および第２の実施の形態にて説明した、フィルタに基づいてウィービング信号の補正を行うだけで、良好なウィービング波形を得ることができる。
なお、ここで実施している２種のフィードフォワード制御は、多関節ロボット１のモデル（運動方程式）に基づいて設計される。ここでは、図１１に示す２リンクロボットを対象として、各種フィードフォワード制御（干渉慣性項、遠心コリオリ項）の計算方法について説明する。

【0073】

図１１に示す２リンクロボットにおける運動方程式を求めると、各関節に作用するトルクτ_１、τ_２は、以下の式（１５）および式（１６）でそれぞれ表される。そして、以下の式（１７）〜式（１９）に示すように計算されて、１軸目に関する２種類のフィードフォワード制御（干渉慣性項、遠心コリオリ項）の出力が式（２０）に示すように求まる。なお、以下の式では、干渉慣性項および遠心コリオリ項に関係のない部分は記載していない。

【0074】

【数15】

【0075】

【数16】

【0076】

【数17】

【0077】

【数18】

【0078】

【数19】

【0079】

【数20】

【0080】

上記の２種類のフィードフォワード制御を、第２の実施の形態に係る３種類のフィードフォワード制御に加えて実施すれば、他軸が動作した場合でも、目標位置信号からエンコーダ５６０で検出される現在位置信号までの動特性を、フィルタ演算部４００で使用されるフィルタに一致させることができる。
また、上記の２種類のフィードフォワード制御を部分的に実施するだけでも、目標位置信号からエンコーダ５６０で検出される現在位置信号までの動特性を、フィルタ演算部４００で使用されるフィルタ特性に（第１および第２の実施の形態よりも）さらに近づけることができる。

【0081】

図１７および図１８は、上記の２リンクロボットの各関節に、ある正弦波信号を目標位置信号として入力した場合の２軸目の出力波形を表している。実線が目標位置信号、破線がフィルタ後目標位置信号、一点鎖線がモータ５５０の現在位置信号である。
また、図１７が第２の実施の形態に対応するものであって、速度フィードフォワード制御、慣性項フィードフォワード制御、重力項フィードフォワードおよび摩擦項フィードフ
ォワード制御を使用した場合の出力波形を示し、図１８が第３の実施の形態に対応するものであって、干渉慣性項フィードフォワード制御および遠心コリオリ項フィードフォワード制御を使用した場合の出力波形を示している。

【0082】

これらを見ると、干渉慣性項フィードフォワード制御および遠心コリオリ項フィードフォワード制御を追加することにより、他軸が動作した場合でも、現在位置信号がフィルタ後目標位置信号にさらに近づき、目標位置信号から現在位置信号までの動特性をフィルタ特性に近似する際の精度を向上できることがわかる。
これにより、第１および第２の実施の形態において説明したように、フィルタに基づいてウィービング信号の補正を行うだけで、良好なウィービング波形を得ることができる。

【0083】

第１の実施の形態にて説明したゲイン補正方法（補正方法１）を使用する制御ブロック（図１９）を用いた場合の出力波形を図２０に示す。図２０には、補正方法１を第３の実施の形態に適用した場合の２軸目の出力波形と、比較のために、補正方法１を第２の実施の形態に適用した場合の２軸目の出力波形とを表す。
図２０は、各関節に、ある正弦波信号をウィービング信号として入力した場合に、補正方法１（ゲイン補正）を行った場合の２軸目の出力波形を表している。実線がウィービング信号、一点鎖線が第３の実施の形態（実施例３）にて補正方法１を行った場合のモータ５５０の現在位置信号、点線が第２の実施の形態（実施例２）にて補正方法１を行った場合のモータ５５０の現在位置信号である。これを見ると、第３の実施の形態の方がモータ５５０の現在位置信号の振幅がウィービング信号の振幅に近く、適切に補正できていることがわかる。

【0084】

以上のようにして、第３の実施の形態に係るウィービング制御装置によると、第２の実施の形態に係るウィービング制御装置における制御に加えて、ｉ軸（自軸）以外の軸が加減速することによりｉ軸に働く慣性力および遠心コリオリ力を予め予測し、適切なトルク指令値としてモータへ入力することにより、他軸が動作することによる影響を抑え、目標位置からモータ出力角度までの動特性をフィルタへ近似する精度を向上させることができる。このフィルタに基づき、目標位置の振幅の補正を行うようにしたので、多関節ロボットにおいて、多関節ロボットの軸を動作させるモータ自身の動特性に起因するウィービング動作の誤差、および、他軸が動作することによる影響に起因するウィービング動作の誤差、が生じることを抑制して、高い軌跡精度でウィービング等の動作を可能とすることができる。

【0085】

＜変形例＞
以下、本発明の実施の形態の変形例に係るウィービング制御装置について説明する。なお、本変形例は、上述した第１〜第３の実施の形態に係るウィービング制御装置に適用でき、フィルタ演算部４００に入力される目標位置信号の生成方法に関するものである。それ以外は、第１〜第３の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

【0086】

上述した実施の形態においては、ウィービング信号の補正を多関節ロボット１の各軸座標系で行っていた。このために、図２１に示すように、各軸座標系で基本位置信号とウィービング信号とを分離して算出する必要がある。しかしながら、このようにすると、ウィービングする場合としない場合とでそれぞれ各軸の角度指令値を算出するために逆変換を行う必要があり、計算負荷が大きくなる。

【0087】

そこで、本変形例においては、より簡易に補正を行うことができるようにする。図２２を参照して、本変形例について説明する。
図２２に示すように、本変形例においては、上述した実施の形態（図２、図１０、図１６）の制御ブロックにおいて、逆変換演算部７００を備える。以下に、各ブロックについて詳しく説明する。

【0088】

ウィービング波形生成部１１００は、指令された振幅および周波数でウィービングを行うために、上述した基本位置波形に加えられる波形を、ウィービング波形として生成する。このウィービング波形は、先端位置を表すための３次元位置と先端姿勢を表すための角度とで表現される。
ウィービング波形補正部１２００は、ウィービング波形生成部１１００で生成されたウィービング波形を補正し、ウィービング補正波形を生成する。補正方法に関しては、第１の実施の形態にて説明したものと同様である。

【0089】

基本波形生成部１３００は、ウィービングをしない場合の多関節ロボット１の先端軌跡としての基本位置波形を生成する。この基本位置波形は、先端位置を表すための３次元位置と先端姿勢を表すための角度とで表現される。
逆変換演算部７００は、基本位置波形にウィービング補正波形が加えられた目標波形に対して逆変換を行い、各関節の目標位置信号を生成する。この目標波形は、先端位置を表すための３次元位置と先端姿勢を表すための角度とで表現される。

【0090】

このように、本変形例によると、逆変換は１回のみでよく、計算負荷を増やすことなく本実施の形態に係る補正方法を適用することができる。
なお、今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

【符号の説明】

【0091】

１ 多関節ロボット
１０、２０、３０ ウィービング制御装置
１００ ウィービング信号生成部
２００ ウィービング信号補正部
３００ 基本位置信号生成部
４００ フィルタ演算部
５００、１５００、２５００ モータ制御部
５１０ 位置制御部
５２０ 速度フィードフォワード部
５３０ 速度制御部
５４０ 電流制御部
５５０ モータ
５６０ エンコーダ
６００ 他軸目標位置信号出力部
７００ 逆変換演算部
１１００ ウィービング波形生成部
１２００ ウィービング波形補正部
１３００ 基本波形生成部
１５１０ 慣性項フィードフォワード部
１５２０ 重力項フィードフォワード部
１５３０ 摩擦項フィードフォワード部
２５１０ 干渉慣性項フィードフォワード部
２５２０ 遠心コリオリ項フィードフォワード部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】

【図22】