WO2013180223A1

WO2013180223A1 - 多関節ロボットの弾性変形補償制御装置および制御方法

Info

Publication number: WO2013180223A1
Application number: PCT/JP2013/065057
Authority: WO
Inventors: 西田　吉晴; 尭和田; 芳英井上; 修一稲田
Original assignee: 株式会社神戸製鋼所
Priority date: 2012-05-30
Filing date: 2013-05-30
Publication date: 2013-12-05
Also published as: KR20150006017A; CN104349873B; US20150105905A1; JP2013248681A; EP2857151A4; US9505131B2; KR101612218B1; EP2857151A1; CN104349873A; JP5374613B2; EP2857151B1

Abstract

　弾性変形補償制御装置（１０）において、第１の動特性演算部（３００）は、モータ角度指令値計算部（６００）から出力されたモータ角度指令値（θｍｃ）をフィルタリング処理して処理後のモータ角度目標値（θｍｄ）を出力する。第２の動特性演算部（４００）は、第１の動特性演算部（３００）よりも低いカットオフ周波数を有する高周波遮断特性を備え、軸力トルク計算部（２００）からの出力をフィルタリング処理して、処理後の軸力トルク補償値（ｆｄ）を出力する。

Description

多関節ロボットの弾性変形補償制御装置および制御方法

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

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

　このような多関節ロボットでは、各軸単位でサーボ制御が行われる。しかし、固有振動数が低いため、振動抑制の観点から、速度フィードフォワードなどはほとんど適用されない。したがって、目標値に対して実際のフィードバック値の位相遅れは大きく、サーボ制御部の速度制御部の応答特性が軸毎に異なり、軌跡誤差に繋がっていた。また、このような多関節ロボットの各軸を動作させるモータは、減速器を介してアームに結合されている。この減速器の剛性不足などに起因する弾性変形を補正する場合、モータが指令値どおりに動作することが前提となっている。しかしながら、フィードフォワードなどが十分機能していないため、モータが指令値どおりに動作することはほぼ不可能であり、弾性変形補償は十分に機能していなかった。このような多関節ロボットの弾性変形補償制御について、以下のような技術が公知である。

　日本国特開昭６１－２０１３０４号公報（特許文献１）は、減速器等の関節群の機械的な剛性が低い場合でも、位置指令値に対してロボットアームを高精度に位置制御する方法を開示する。この位置制御方法においては、ロボットを構成する各アームの位置指令値と、それを１階微分して得た速度と、２階微分して得た加速度と、を、各アーム間の関節の機械的剛性を考慮したロボットアームの運動方程式に代入することにより、各関節に加わるトルクが算出される。そして、求められたトルクを定数あるいは関数あるいは制御装置内のテーブルとして与えられた各関節の機械的ばね剛性で除することによって、各関節の機械的剛性に因るたわみ角が求められる。求められたたわみ角を、各関節のたわみを打ち消すように、位置指令値と和することにより、新たな位置指令値が設定される。

　また、日本国特開２００５－１８６２３５号公報（特許文献２）は、干渉力が作用しても各軸が指令どおり動作するロボットの制御装置を開示する。この制御装置は、互いに干渉する複数軸から構成されるロボットの制御装置であって、ロボットは、モータと、モータに減速機等を介して結合されたアームと、モータの位置を検出するモータ位置検出器と、から構成された各軸を各軸毎の指令通りに動作させるための位置制御部および速度制御部を備える。この制御装置は、自軸の指令から他軸に作用する干渉力を計算で求める干渉力計算部と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータトルク指令信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉トルク信号作成部と、を備える。制御装置はさらに、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータ位置信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉位置信号作成部を備える。

日本国特開昭６１－２０１３０４号公報日本国特開２００５－１８６２３５号公報

　上述した特許文献１では、減速器等の剛性不足から生じる撓み（弾性変形）を関節角目標値等から算出し、その弾性変形を補償するようにモータへの角度指令値に弾性変形量を加算することによって、位置精度の改善が図られている。しかしながら、上述のように良好なフィードフォワード制御などが行われていないので、モータは指令値どおりに動作せず、弾性変形補償は十分に機能していない。

　また、特許文献２では、各軸間の干渉を非干渉化する手法が開示されている。しかしながら、この手法では、アーム加速度の１階微分値、２階微分値が必要であり、非常にノイズに弱い。また少し急峻な動作があるだけでアーム加速度の２階微分値は天文学的な値を示すなど、実現する上で非常に制約が大きい。

　すなわち、従来技術では、以下のような問題が解決されていない。
　（１）ロボットの固有振動が低い状態で、弾性変形補償および軸力トルク補償を有効に作用させることができないため、弾性変形の影響を補償できないことが、精度劣化につながっている。
　（２）サーボ制御部の位相遅れがある状態において、弾性変形補償制御を有効に作用せせることができないため、弾性変形の影響を補償できないことが、精度劣化につながっている。

　（３）軸毎にサーボ制御特性が異なるため、各軸の応答に差が発生することが、軌跡精度の劣化につながっている。
　（４）指令値の計算周期がサーボ制御部における計算周期よりも遅い場合に発生する位相差が、軌跡精度の劣化につながっている。
　（５）溶接ロボットのウィービング動作では、ウィービング周期での位相遅れおよびゲイン特性を各軸そろえることが非常に重要であるが、減速器による弾性変形によるサーボ特性変化や軸毎での特性の差異によって、位相・ゲイン特性を高周波ウィービング動作でそろえることが非常に困難である。

　本発明は、上述の問題に鑑みてなされたものであり、複数軸を備えた多関節ロボットにおいて、各軸の弾性変形の影響を補償して高い軌跡精度でウィービング等の動作を可能とすることができる、多関節ロボットの弾性変形補償制御装置および制御方法を提供することを目的とする。

　上記課題を解決するため、本発明に係る多関節ロボットの弾性変形補償制御装置は、以下の技術的手段を講じている。
　即ち、本発明に係る多関節ロボットの弾性変形補償制御装置は、多関節ロボットの関節軸を駆動するモータとアームとが弾性変形する減速器を介して結合された多関節ロボットに取り付けられたツールに所望の動作を行わせるように複数の関節軸を駆動させる。この弾性変形補償制御装置は、所望のツール動作を実現するための各関節軸の関節角度指令値θｌｃを算出して出力する関節角度指令値計算部と、前記関節角度指令値θｌｃ通りに動作した際に発生する各関節軸に作用する軸力トルクｆｃを、動力学モデルに基づいて関節角度指令値θｌｃから算出して出力する軸力トルク計算部と、関節軸の剛性パラメータを含むパラメータに基づいて、関節角度指令値θｌｃと軸力トルクｆｃとからモータ角度指令値θｍｃを算出して出力するモータ角度指令値計算部と、ロボットの固有振動周波数よりも低いカットオフ周波数を有する高周波遮断特性を備え、前記モータ角度指令値θｍｃをフィルタリング処理して、処理後のモータ角度目標値θｍｄを出力する第１の動特性演算部と、前記モータ角度目標値θｍｄが前記モータに対する目標値として入力されるモータ角度制御部と、前記第１の動特性演算部よりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備え、前記軸力トルク計算部への入力および前記軸力トルク計算部からの出力の少なくともいずれかをフィルタリング処理して、処理後の軸力トルク補償値ｆｄを出力する第２の動特性演算部と、前記モータ角度制御部から出力されるモータトルク指令値に、前記軸力トルク補償値ｆｄが加算された値が目標値として入力されるモータ電流制御部と、を含んで構成されていることを特徴とする。

　弾性変形補償制御装置は、好ましくは、前記モータ角度制御部の応答が遅い軸の第１の動特性演算部の位相遅れを、前記モータ角度制御部の応答が速い軸の第１の動特性演算部の位相遅れよりも短く与えるように構成することができる。
　弾性変形補償制御装置は、さらに好ましくは、各軸間の軸力がない状態でのモータ角度目標値θｍｄから実際のモータ角度θｍまでのフィードバック制御動特性を、位置フィードバックゲインおよび速度フィードバックゲインの少なくともいずれかを含むパラメータに基づいて算出するように構成され、前記第２の動特性演算部におけるカットオフ周波数を前記第１の動特性演算部におけるカットオフ周波数よりも低くするとともに、前記第２の動特性演算部における特性を前記フィードバック制御動特性で除した特性に対応するように、前記第１の動特性演算部の特性を与えるように構成することができる。

　弾性変形補償制御装置は、さらに好ましくは、前記モータ電流制御部の電流制御特性を考慮する場合、前記第２の動特性演算部における特性を前記フィードバック制御動特性で除すとともに、電流制御特性を乗じた特性に対応するように、前記第１の動特性演算部の特性を与えるように構成することができる。
　弾性変形補償制御装置は、さらに好ましくは、前記モータ角度指令値計算部における計算周期が、前記モータ角度制御部における制御周期よりも遅い場合、前記モータ角度指令値計算部において、軸力トルク計算値または弾性変形量計算値と関節角度指令値との位相差を合わせ、位相補正後の軸力トルク計算値または弾性変形量計算値と位相補正後の関節角度指令値とから、モータ角度指令値を算出するように構成することができる。

　また、本発明の別の形態に係る多関節ロボットの弾性変形補償制御方法は、所望のツール動作を実現するための各関節軸の関節角度指令値θｌｃを算出して出力する関節角度指令値計算ステップと、前記関節角度指令値θｌｃ通りに動作した際に発生する各関節軸に作用する軸力トルクｆｃを、動力学モデルに基づいて関節角度指令値θｌｃから算出して出力する軸力トルク計算ステップと、関節軸の剛性パラメータを含むパラメータに基づいて、関節角度指令値θｌｃと軸力トルクｆｃとからモータ角度指令値θｍｃを算出して出力するモータ角度指令値計算ステップと、ロボットの固有振動周波数よりも低いカットオフ周波数を有する高周波遮断特性を備え、前記モータ角度指令値θｍｃをフィルタリング処理して、処理後のモータ角度目標値θｍｄを出力する第１の動特性演算ステップと、前記モータ角度目標値θｍｄが前記モータに対する目標値として入力されるモータ角度制御ステップと、前記第１の動特性演算ステップよりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備え、前記軸力トルク計算ステップへの入力および前記軸力トルク計算ステップからの出力の少なくともいずれかをフィルタリング処理して、処理後の軸力トルク補償値ｆｄを出力する第２の動特性演算ステップと、前記モータ角度制御ステップから出力されるモータトルク指令値に、前記軸力トルク補償値ｆｄが加算された値が目標値として入力されるモータ電流制御ステップと、を含んで構成されていることを特徴とする。

　本発明に係る弾性変形補償制御装置または制御方法を用いることにより、複数軸を備えた多関節ロボットにおいて、各軸の弾性変形の影響を補償して高い軌跡精度でウィービング等の動作を可能とすることができる。

本発明の実施の形態に係る弾性変形補償制御装置が適用される多関節ロボットの全体構成を示す概略図である。本発明の第１の実施の形態に係る弾性変形補償制御装置の制御ブロック図である。図２に示す制御ブロック図により制御された多関節ロボットのウィービング軌跡を示す図である。従来技術に係る弾性変形補償制御装置の制御ブロック図である。図４に示す制御ブロック図により制御された多関節ロボットのウィービング軌跡を示す図である。従来技術に係る他の弾性変形補償制御装置の制御ブロック図である。図６に示す制御ブロック図により制御された多関節ロボットのウィービング軌跡を示す図である。本発明の第２の実施の形態に係る弾性変形補償制御装置（図２の動特性とは異なる特性）により制御された多関節ロボットのウィービング軌跡を示す図である。本発明の第２の実施の形態の変形例に係る弾性変形補償制御装置に係る制御ブロック図である。本発明の第３の実施の形態に係る弾性変形補償制御装置（図２の動特性とは異なる特性）により制御された多関節ロボットのウィービング軌跡を示す図である。本発明の第４の実施の形態に係る弾性変形補償制御装置（動特性を軸毎に変更）により制御された多関節ロボットのウィービング軌跡を示す図である。本発明の第５の実施の形態に係る弾性変形補償制御装置の制御ブロック図である。本発明の第５の実施の形態の変形例に係る弾性変形補償制御装置の制御ブロック図である。本発明の第６の実施の形態に係る弾性変形補償制御装置の制御ブロック図である。本発明の第６の実施の形態に係る弾性変形補償制御装置の他の制御ブロック図である。

　以下、本発明の実施の形態に係る多関節ロボットの弾性変形補償制御装置および制御方法を、図面に基づき詳しく説明する。なお、以下の説明では、同一の部品は同一の符号を付され、それらの名称および機能も同じである。したがって、それらについての詳細な説明は繰り返さない。また、以下においては、制御対象として溶接トーチを傾動動作（ウィービング動作）させる多関節ロボットを説明するが、これは一例に過ぎない。本発明に係る弾性変形補償制御装置は、多関節ロボットの関節軸を駆動するモータとアームとが弾性変形する減速器を介して結合された多関節ロボットに取り付けられたツールに所望の動作を行わせるように複数の関節軸を駆動するための制御に、広く適用が可能である。

＜第１の実施の形態＞
［全体構成］
　まず、本実施の形態に係る弾性変形補償制御装置が適用される垂直多関節ロボット（以下、単に多関節ロボットと記載する場合がある）の概要について説明する。
　図１は、溶接トーチを傾動動作（ウィービング動作）させるロボットの一例であって、本実施の形態に係る弾性変形補償制御装置が適用される多関節ロボット１の概要を示す図である。この多関節ロボット１は、垂直多関節型であってＪ１～Ｊ６の６個の関節を備え、Ｊ６の軸の先端に設けられた溶接トーチから送りだされる溶接ワイヤによりアーク溶接を行う。この多関節ロボット１は、予め定められた溶接開始点と溶接終了点との間を溶接作業区間として設定し、溶接開始点と溶接終了点とを結ぶ溶接線方向に移動しつつ、溶接ワイヤを予め定められた振幅および周波数で傾動する動作（ウィービング動作）を行うようにセットされている。

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

［制御ブロック］
　図２は、図１の多関節ロボット１を制御する弾性変形補償制御装置１０の制御ブロック図を示す。上述したように、この弾性変形補償制御装置１０は、上位ＣＰＵで実現される部分とサーボ制御部で実現される部分とを含んで構成されている。
　図２に示すように、この弾性変形補償制御装置１０は、多関節ロボット１に取り付けられたツール（ここでは溶接トーチ）に所望の動作（ここではウィービング動作）を行わせるように、複数の関節軸を駆動する。

　この弾性変形補償制御装置１０は、上位ＣＰＵで実現される、関節角度指令値計算部１００と、軸力トルク計算部（「軸力ＦＦ」として図示）２００と、モータ角度指令値計算部（「弾性変形補償」として図示）６００と、を備える。さらに、弾性変形補償制御装置１０は、サーボ制御部で実現される、第１の動特性演算部（「動特性（１）」として図示）３００と、第２の動特性演算部（「動特性（２）」として図示）４００と、フィードバック制御部（「サーボ制御ＦＢ特性」として図示）５００と、を備える。フィードバック制御部５００は、モータ角度制御部５１０およびモータ電流制御部（「電流制御」として図示）５２０を含んで構成されている。なお、以下において、制御ブロックにおける要素の特性は、動特性との記載がない場合でも、全て動特性である。また、記載「ＦＢ」はフィードバックを意味し、記載「ＦＦ」はフィードフォワードを意味する。

　関節角度指令値計算部１００は、溶接トーチのウィービング動作を実現するための各関節軸の関節角度指令値θｌｃを算出して出力する。
　軸力トルク計算部２００は、関節角度指令値計算部１００から出力された関節角度指令値θｌｃ通りに動作した際に発生する各関節軸に作用する軸力トルクｆｃを、動力学モデルに基づいて関節角度指令値θｌｃから算出し、出力する。

　モータ角度指令値計算部６００は、関節軸の剛性パラメータを含むパラメータに基づいて、関節角度指令値θｌｃと軸力トルクｆｃとからモータ角度指令値θｍｃを算出し、出力する。
　より詳しくは、軸力トルク計算部２００が、関節角度指令値θｌｃに基づいて指令値どおりに動作した際に各軸に作用する軸力トルクｆｃを算出する。モータ角度指令値計算部６００は、軸力トルクｆｃから軸剛性Ｋや粘性Ｂなどに基づいて（粘性は小さいため省略可能）弾性変形量ｅｃを算出する。そして、モータ角度指令値計算部６００は、関節角度指令値θｌｃと弾性変形量ｅｃとから、モータ角度指令値θｍｃを算出する。

　第１の動特性演算部３００は、モータ角度指令値計算部６００から出力されたモータ角度指令値θｍｃをフィルタリング処理して、処理後のモータ角度目標値θｍｄを出力する。この第１の動特性演算部３００は、多関節ロボット１の固有振動周波数よりも低いカットオフ周波数を有する高周波遮断特性を備える。
　第２の動特性演算部４００は、軸力トルク計算部２００への入力および軸力トルク計算部２００からの出力の少なくともいずれかをフィルタリング処理して、処理後の軸力トルク補償値ｆｄを出力する。なお、図２において、第２の動特性演算部４００は、軸力トルク計算部２００からの出力をフィルタリング処理している。この第２の動特性演算部４００は、第１の動特性演算部３００よりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備える。

　モータ角度制御部５１０には、モータに対する目標値として、モータ角度目標値θｍｄが入力される。
　モータ電流制御部５２０には、モータ角度制御部５１０から出力されるモータトルク指令値に、第２の動特性演算部４００から出力された軸力トルク補償値ｆｄが加算された値が、目標値として入力される。

　図２に示すブロック図で示される弾性変形補償制御装置１０は、以下のような特徴を備える。
　非線形項である軸力トルク計算部２００の前および／または後（ここでは後のみ）に、第２の動特性演算部４００が配置されている。この第２の動特性演算部４００により、第１の動特性演算部３００の高周波遮断特性以上の高周波帯域を遮断する特性が与えられている。ここで、第２の動特性演算部４００のカットオフ周波数は、第１の動特性演算部３００のカットオフ周波数よりも低いか同等である。

　このように構成することにより、第１の動特性演算部３００により、関節角度指令値θｌｃに含まれる固有振動成分を含む高周波を抑制することができる。さらに、第２の動特性演算部４００により、軸力トルクｆｃに含まれる固有振動成分を含む高周波を抑制することができる。これにより、多関節ロボット１に発生する高周波振動を抑制することができる。

　また、多関節ロボット１では、ＸＹＺ空間で低周波動作をさせても特異点近傍などのヤコビアン（Jacobian）が急峻に変化するところにおいて、関節角度に変化すると２倍や３倍成分の高周波が発生する。さらに、関節角度空間にて低周波動作をさせても、非線形項は速度の２乗項などを有しているため、関節角度の２倍や３倍成分の高周波が発生する。このため、非線形項である軸力トルク計算部２００からの出力である軸力トルクｆｃを、第１の動特性演算部３００の高周波遮断特性以上の高周波遮断特性を備えた第２の動特性演算部４００で処理して軸力トルク補償値ｆｄとする。これにより、多関節ロボット１に発生する高周波振動をさらに抑制している。

［制御特性（ウィービング軌跡）］
　以上のような構成を備えた弾性変形補償制御装置１０を用いて多関節ロボット１を制御した場合の制御特性（ウィービング軌跡）について説明する。
　図３は、第２の動特性演算部４００の高周波遮断特性として、第１の動特性演算部３００と同等の高周波遮断特性を与えた場合のウィービング軌跡を示す。

　これを評価するにあたり、まず従来技術（制御ブロック図およびウィービング軌跡）について説明する。
　図４は、最も一般的な多関節ロボットの制御ブロック図を示す。図４に示すように、この制御ブロックは、位置制御部（Ｇｐ）と速度制御部（Ｇｖ）と電流制御部とから構成される。位置制御部は、関節角度をフィードバック制御し、角度偏差を比例制御（Ｐ制御）し、速度指令として速度制御部に指令する。速度制御部は、関節角速度をフィードバック制御し、与えられた速度指令との偏差を比例積分制御（ＰＩ制御）し、電流制御指令として電流制御部に指令する。電流制御部は、与えられた電流制御指令に基づきモータ電流を制御する。

　なお、多関節ロボットは、機械の固有振動数が低いため、目標値に固有振動を励起する成分が含まれないように、図４に示す第１の動特性演算部（動特性１）などでのフィルタ処理にて、それらの成分を抑制している。
　ただし、多関節ロボットでは各軸間で干渉トルクが、重力項などを含めて非線形項ｃとして各リンクに作用する。バネ要素として作用する減速器を介してリンクとモータとが結合されているため、軸力がリンクとモータとに作用・反作用として作用する。特に溶接ロボットのウィービング動作では、上下にぶれることなく（上下方向の動きを発生させることなく）所望方向に所望の振幅で溶接トーチを揺動させることが必要であるために、非常に高精度な動的制御が必要とされる。そのため、この軸力および弾性変形の影響は甚大である。

　図５は、図４の制御ブロックで示される制御装置で多関節ロボットを制御した場合のウィービング軌跡を示す。この図５に示すように、非線形項、軸力および弾性変形が作用すると上下方向の動きが発生してしまい、ウィービング動作として全くふさわしくない。これを抑制するためには、目標値ベースで軸力を算出し、フィードフォワード補償することが考えられる。

　図６は、このような考えに基づいた目標値ベースの、従来技術に係る非線形フィードフォワード補償を実現する制御ブロック図を示す。
　図７は、図６の制御ブロックで示される制御装置で多関節ロボットを制御した場合のウィービング軌跡を示す。これは目標値ベースのフィードフォワード補償であるため、位相遅れの影響によりフィードフォワードタイミングがずれてしまい、却って、上下方向の動きを悪化させる結果となっている。

　一般に、このような従来技術に係る制御では、フィードバック制御部（サーボ制御フィードバック特性）における位相特性やゲイン特性が各軸毎に異なるため、フィードフォワード制御などの位相を合わせることが困難であった。このため、上記のような軸力トルク補償や弾性変形補償はほとんど実用化されておらず、弾性変形などの影響を抑制することは困難となっていた。

　なお、図６では、上位ＣＰＵが、非線形フィードフォワード計算を行っている。非線形項の計算が非常に複雑で計算量が多く、サーボ制御部で計算することが難しいため、上位ＣＰＵにより目標値ベースでおこなわれるのが一般的である。
　このような従来技術に係る制御による結果（ウィービング軌跡）を示す図５および図７と比較すると、本実施の形態に係る弾性変形補償制御装置１０による結果（ウィービング軌跡）は、図３に示すように、上下動成分は発生しているものの格段に上下動成分が抑制されている。なお、図３は、第２の動特性演算部４００における高周波遮断特性を、第１の動特性演算部３００における高周波遮断特性と同等とした場合の結果である。

　本実施の形態に係る弾性変形補償制御装置１０においては、第１の動特性演算部３００により、関節角度指令値θｌｃに含まれる固有振動成分を含む高周波が抑制される。さらに、第２の動特性演算部４００により、軸力トルクｆｃに含まれる固有振動成分を含む高周波が抑制される。このことにより、上述した従来技術における非線形項に起因する振動が抑制される。

　以上のようにして、本実施の形態に係る弾性変形補償制御装置によると、多関節ロボットにおいて、各軸の弾性変形の影響を補償して高い軌跡精度でウィービング等の動作を可能とすることができる。

＜第２の実施の形態＞
　以下、本発明の第２の実施の形態に係る弾性変形補償制御装置について説明する。なお、本実施の形態に係る弾性変形補償制御装置は、上述した第１の実施の形態に係る弾性変形補償制御装置１０と、第１の動特性演算部３００の与え方において異なる。それ以外は、第１の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

　上述した第１の実施の形態においては、第１の動特性演算部３００と同等以上の高周波遮断特性が第２の動特性演算部４００に与えられていた。本実施の形態においては、第２の動特性演算部４００が、第１の実施の形態と同じように非常に強い高周波遮断特性を備えるように与えられる。また、第１の動特性演算部３００が、第２の動特性演算部４００の動特性に各軸サーボ制御にて想定されるフィードバック特性の逆特性を乗じたものとして与えられる。これは、第２の動特性演算部４００における特性をフィードバック制御動特性で除した特性に対応するように、第１の動特性演算部３００の特性を与えるように構成されていることを示す。

　図２に示す制御ブロックの場合、各軸サーボフィードバック制御の動特性は、以下の式（１）で与えられる。なお、以下に示す式においては、「動特性１」は、第１の動特性演算部３００における動的特性を示し、「動特性２」は、第２の動特性演算部４００における動的特性を示している。

　したがって、第１の動特性演算部３００は、以下の式（２）で与えられる。

　ただし、式（２）がプロパー（proper）な伝達関数（分子多項式の次数が分母多項式のそれに比べて等しいかまたは低い）となるように、第２の動特性演算部４００には強い高周波遮断特性を与える必要がある。
　このようにして、第１の動特性演算部３００および第２の動特性演算部４００を与えた場合のウィービング軌跡を図８に示す。図８に示すように、本実施の形態に係る弾性変形補償制御装置によるウィービング軌跡では、第１の実施の形態におけるウィービング軌跡（図３）よりも、さらに上下方向の振動が抑制されていることがわかる。

　以上のようにして、本実施の形態に係る弾性変形補償制御装置は、多関節ロボットにおいて、各軸の弾性変形の影響を補償して、さらに高い軌跡精度でウィービング等の動作を可能とすることができる。

＜第２の実施の形態の変形例＞
　次に、第２の実施の形態の変形例に係る弾性変形補償制御装置について説明する。

　上述したように、第２の動特性演算部４００の特性は、所望の動特性（非常に強い高周波遮断特性と備える）として与えられていた。しかしながら、モータ電流制御部５２０の電流制御特性を無視できない場合には、所望の動特性を電流制御特性で除した特性として、第２の動特性演算部４００の特性が与えられる。これは、モータ電流制御部５２０の電流制御特性を考慮する場合、第２の動特性演算部４００における特性をフィードバック制御動特性で除すとともに、電流制御特性を乗じた特性に対応するように、第１の動特性演算部３００の特性を与えるように構成されていることを示す。

　この場合、第２の動特性演算部４００の特性は、以下の式（３）で与えられる。

　このため、上述した式（２）で表された第１の動特性演算部３００の特性は、以下の式（４）で与えられる。

　図９は、上述のようにして、第１の動特性演算部３００および第２の動特性演算部４００を与える場合の制御ブロック図を示す。図９に示すように、この弾性変形補償制御装置２０における第２の動特性演算部２４００の特性は、所望の動特性２４１０に電流制御特性の逆特性２４２０を乗じたものとなっている。
　以上のようにして、本変形例に係る弾性変形補償制御装置は、多関節ロボットにおいて、モータ電流制御部５２０の電流制御特性を考慮して、各軸の弾性変形の影響を補償して、さらに高い軌跡精度でウィービング等の動作を可能とすることができる。

＜第３の実施の形態＞
　以下、本発明の第３の実施の形態に係る弾性変形補償制御装置について説明する。なお、本実施の形態に係る弾性変形補償制御装置は、上述した第１の実施の形態に係る弾性変形補償制御装置１０と、第１の動特性演算部３００の与え方において異なる。それ以外は、第１の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

　電流制御特性は、非常に高速応答のため１として近似しても良い性能が得られることを考慮すると、式（１）は、以下の式（５）に変形される。

　そして、第１の動特性演算部３００を与える式（２）は、以下の式（６）に変形される。

　これは、第２の動特性演算部４００におけるカットオフ周波数を第１の動特性演算部３００におけるカットオフ周波数よりも低くするとともに、第２の動特性演算部４００における特性をフィードバック制御動特性（サーボＦＢ制御動特性）で除した特性に対応するように、第１の動特性演算部３００の特性を与えるように構成されていることを示す。
　なお、この場合、各軸間の軸力がない状態でのモータ角度目標値θｍｄから実際のモータ角度θｍまでのフィードバック制御動特性を、位置フィードバックゲインおよび速度フィードバックゲインの少なくともいずれかを含むパラメータに基づいて算出するように構成されている。

　図１０は、このようにして、第１の動特性演算部３００および第２の動特性演算部４００を与えた場合のウィービング軌跡を示す。図１０に示すように、本実施の形態に係る弾性変形補償制御装置においては、電流制御特性を１で近似しているため、当然ながら第２の実施の形態におけるウィービング軌跡（図８）の性能には及ばない。しかしながら、従来技術に比べて良好な、実用に十分耐え得る性能を発揮していることがわかる。このように電流制御特性を１で近似することにより、実用に十分耐え得る性能を満足しつつ、演算時間を短縮することができる。

＜第４の実施の形態＞
　以下、本発明の第４の実施の形態に係る弾性変形補償制御装置について説明する。なお、本実施の形態に係る弾性変形補償制御装置は、上述した第１の実施の形態に係る弾性変形補償制御装置１０と、第１の動特性演算部３００の与え方において異なる。それ以外は、第１の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

　本実施の形態に係る弾性変形補償制御装置は、モータ角度制御部５１０の応答が遅い軸の第１の動特性演算部３００の位相遅れを、モータ角度制御部５１０の応答が速い軸の第１の動特性演算部３００の位相遅れよりも短く与えるように構成されている。
　上述した第２および第３の実施の形態においては、各軸毎に所望の動特性が与えられていたが、実際のウィービング動作では各軸の動特性を揃える必要がある。このためには、全軸で同じ（共通の）所望の動特性が与えられればよい。上述した図８および図１０は、既に全軸で同じ所望の動特性が与えられている。

　図１１は、所望の動特性を軸毎に少し変更したときのウィービング軌跡を示す。図１１に示すように、軸毎に動特性を変更させることにより、各軸の応答を異ならせると、上下動が発生することがわかる。すなわち、上述した第１～第３の実施の形態（および従来技術）の結果は、各軸の応答をそろえた結果であり、各軸の応答を揃えない場合、ウィービング精度は悪化する。

　また、第１～第３の実施の形態においては、所望の動特性が全軸について同じである。式（１）および式（２）から明らかなように、サーボＦＢ制御動特性の逆特性を「動特性１」に乗じているため、サーボＦＢ制御動特性が遅い軸では、速い軸に比べて、「動特性１」の応答が速くなる。また、サーボＦＢ制御動特性が速い軸では、遅い軸に比べて、「動特性１」の応答が遅くなる。

　式（１）および式（２）により「動特性１」を与えることが一番良い方法であるが、関節角度指令値θｌｃから関節角度θまでの位相特性やゲイン特性は軸毎に異なる。本実施の形態に係る弾性変形補償制御装置は、サーボＦＢ制御動特性の遅い軸に比べて速い軸の「動特性１」を遅くすることにより、図１１の状態から、全軸で同じ動特性を「動特性１」として与えた図３、図８または図９の状態に徐々に近づけることができる。

＜第５の実施の形態＞
　以下、本発明の第５の実施の形態に係る弾性変形補償制御装置について説明する。なお、本実施の形態に係る弾性変形補償制御装置は、上述した第１の実施の形態に係る弾性変形補償制御装置１０が備えなかった速度フィードフォワード制御および／または加速度フィードフォワード制御が加えられている点において異なる。それ以外は、第１の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

　上述した実施の形態においては、速度フィードフォワード制御および／または加速度フィードフォワード制御を備えていなかった。しかしながら、本実施の形態に係る弾性変形補償制御装置３０は、速度フィードフォワード制御および加速度フィードフォワード制御を備える。
　図１２は、第５の実施の形態に係る弾性変形補償制御装置３０のブロック図を示す。弾性変形補償制御装置３０は、速度フィードフォワード制御および加速度フィードフォワード制御を備える。

　図１２に示すように、弾性変形補償制御装置３０は、モータ角度制御部５１０に代えて、モータ角度制御部３５１０を備える。
　この図１２に示す弾性変形補償制御装置３０では、式（１）は、以下の式（７）で与えられる。

　この式（７）に基づいて、式（２）、式（４）および式（６）を用いて「動特性１」を算出すればよい。電流制御特性を無視して「ゲイン＝１」としても、従来技術よりも良い性能が得られる。
　なお、ここで、ＧｄａおよびＧｄｖは、加速度フィードフォワードおよび速度フィードフォワードゲインであり、０～１の値をとる。また、ＪｄはＪｍの予測値である。

　また、上述したように、電流制御特性も、電流制御ゲインおよびモータパラメータ（インダクタンスや抵抗）から算出可能である。
　以上のようにして、本実施の形態に係る弾性変形補償制御装置によると、速度フィードフォワード制御および加速度フィードフォワード制御を加えて、各軸の弾性変形の影響を補償して、高い軌跡精度でウィービング等の動作を可能とすることができる。

＜第５の実施の形態の変形例＞
　次に、第５の実施の形態の変形例に係る弾性変形補償制御装置について説明する。
　図１３に示すように、本変形例では、非線形項である軸力トルク計算部２００の前後に、第２の動特性演算部４４００が配置される。より詳しくは、軸力トルク計算部２００の前に動特性（２１）４４１０が配置され、軸力トルク計算部２００の後に動特性（２２）４４２０が配置される。

　ここで、動特性（２１）×動特性（２２）が上述した動特性（２）と一致するように与えられれば、これまでの実施の形態と同等の効果が得られる。ここで、θｌｅは関節角度指令値θｌｃを動特性（２１）４４１０に入力したときの出力値である。このθｌｅに基づいて軸力トルク計算部２００にて算出された軸力トルクｆｃを動特性（２２）４４２０に入力したときの出力値が、軸力トルク補償値ｆｄである。

　以上のようにして、本変形例に係る弾性変形補償制御装置によると、多関節ロボットにおいて、分割して配置された第２の動特性演算部により、各軸の弾性変形の影響を補償して、高い軌跡精度でウィービング等の動作を可能とすることができる。

＜第６の実施の形態＞
　以下、本発明の第６の実施の形態に係る弾性変形補償制御装置について説明する。

　図１４および図１５は、本実施の形態に係る弾性変形補償制御装置の制御ブロック図を示す。図１４に示す制御ブロックと、図１５に示す制御ブロックとは、逆変換（逆ヤコビ）要素の数および位置において異なる。
　本実施の形態においては、「動特性２」（または「所望動特性」）によって目標値から実値への動特性が決定されるため、与えられた「動特性２」にしたがって、ウィービング動作時の周期におけるゲイン特性および位相特性を逆算することが可能となる。その特性に応じてウィービング指令を補正することにより、所望のウィービング動作を実現することができるようになる。

＜第７の実施の形態＞
　以下、本発明の第７の実施の形態に係る弾性変形補償制御装置について説明する。なお、本実施の形態に係る弾性変形補償制御装置は、上述した第１の実施の形態に係る弾性変形補償制御装置１０とは、軸力トルク計算値または弾性変形量計算値と関節角度指令値との位相差を合わせる点において異なる。それ以外は、第１の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

　本実施の形態に係る弾性変形補償制御装置においては、モータ角度指令値計算部における計算周期が、モータ角度制御部における制御周期よりも遅い場合には、以下のように処理する。モータ角度指令値計算部６００において、軸力トルク計算値または弾性変形量計算値と関節角度指令値との位相差を合わせ、位相補正後の軸力トルク計算値または弾性変形量計算値と位相補正後の関節角度指令値とから、モータ角度指令値が算出される。

　本実施の形態に係る弾性変形補償制御装置について、以下に詳しく説明する。軸力を算出する際、関節角度の速度および加速度が必要となるが、通常、これらには差分処理により算出された速度および加速度が用いられる。軸力を算出する軸力トルク計算部２００では、サーボ制御周期よりも非常に遅い周期ｄＴで処理が行われるため、差分による位相遅れの影響も非常に大きく、それらの位相を揃えるために位相補正が行なわれる。

　たとえば、加速度を以下の式（８）により導出した際には、速度を以下の式（９）により導出することにより、位相を揃えることが可能になる。

　また、位置は、以下の式（１０）または式（１１）で与えれば良い。

　このように、位相を合わせた関節角度指令値θｌｃを用いて弾性変形量ｅｃを算出し、さらにモータ指令値を算出する際は位相を合わせた関節角度指令値θｌｃに対して弾性変形量ｅｃで補正し、モータ角度指令値θｍｃを算出すればよい。

＜各実施の形態の作用効果＞
　上述した第１～第７の実施の形態のように、すなわち以下のように構成した弾性変形補償制御装置は、弾性変形する減速器を介してモータとアームとが結合された多関節ロボットにおいて、高い軌跡精度でウィービング等の動作を可能とすることができる。

　（１）目標値から固有振動周期を遮断するローパスフィルタ特性を与え、振動成分を目標角度から除去するとともに、軸力トルクについてもローパスフィルタ処理する。これにより、目標値と軸力トルクの位相を揃え、振動抑制しつつ弾性変形補償を行う。
　（２）サーボ制御部の位相遅れがある状態においても、軸力トルク補償に関するローパスフィルタの特性を遅らせる事で、目標値と軸力補償トルクの位相を揃え、振動抑制しつつ弾性変形補償を行う。

　（３）軸毎のサーボ制御特性の違いを、上記ローパスフィルタにて吸収することにより、軸毎の特性差を揃える。
　（４）モータ角度指令値計算部における計算周期が、モータ角度制御部における制御周期よりも遅い場合には、モータ角度指令値計算部において、軸力トルク計算値または弾性変形量計算値と関節角度指令値との位相差を合わせる。

　（５）上述した対応の上、ウィービング周期毎にウィービング振幅補正を行う。

　なお、今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。本出願は２０１２年５月３０日出願の日本特許出願（特願２０１２－１２３２３９）に基づくものであり、その内容はここに参照として取り込まれる。

１多関節ロボット
１０、２０、３０、４０　弾性変形補償制御装置
１００　関節角度指令値計算部
２００　軸力トルク計算部（軸力ＦＦ）
３００　第１の動特性演算部（動特性（１））
４００、２４００、４４００　第２の動特性演算部（動特性（２））
５００　フィードバック制御部（サーボ制御ＦＢ特性）
６００　モータ角度指令値計算部（弾性変形補償）
５１０　モータ角度制御部
５２０　モータ電流制御部（電流制御）

Claims

　弾性変形する減速器を介してモータとアームとが結合された多関節ロボットに取り付けられたツールに所望の動作を行わせるように複数の関節軸を前記モータにより駆動する多関節ロボットの弾性変形補償制御装置であって、
　前記ツールの所望の動作を実現するための各関節軸の関節角度指令値θｌｃを算出して出力する関節角度指令値計算部と、
　前記関節角度指令値θｌｃ通りに動作した際に発生する各関節軸に作用する軸力トルクｆｃを、動力学モデルに基づいて関節角度指令値θｌｃから算出して出力する軸力トルク計算部と、
　前記関節軸の剛性パラメータを含むパラメータに基づいて、関節角度指令値θｌｃと軸力トルクｆｃとからモータ角度指令値θｍｃを算出して出力するモータ角度指令値計算部と、
　前記多関節ロボットの固有振動周波数よりも低いカットオフ周波数を有する高周波遮断特性を備え、前記モータ角度指令値θｍｃをフィルタリング処理して、処理後のモータ角度目標値θｍｄを出力する第１の動特性演算部と、
　前記モータ角度目標値θｍｄが前記モータに対する目標値として入力されるモータ角度制御部と、
　前記第１の動特性演算部よりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備え、前記軸力トルク計算部への入力および前記軸力トルク計算部からの出力の少なくともいずれかをフィルタリング処理して、処理後の軸力トルク補償値ｆｄを出力する第２の動特性演算部と、
　前記モータ角度制御部から出力されるモータトルク指令値に、前記軸力トルク補償値ｆｄが加算された値が目標値として入力されるモータ電流制御部と、
　を含んで構成されていることを特徴とする多関節ロボットの弾性変形補償制御装置。
　前記モータ角度制御部の応答が遅い軸の第１の動特性演算部の位相遅れを、前記モータ角度制御部の応答が速い軸の第１の動特性演算部の位相遅れよりも短く与えるように構成されていることを特徴とする、請求項１に記載の多関節ロボットの弾性変形補償制御装置。
　各軸間の軸力がない状態でのモータ角度目標値θｍｄから実際のモータ角度θｍまでのフィードバック制御動特性を、位置フィードバックゲインおよび速度フィードバックゲインの少なくともいずれかを含むパラメータに基づいて算出するように構成され、
　前記第２の動特性演算部におけるカットオフ周波数を前記第１の動特性演算部におけるカットオフ周波数よりも低くするとともに、前記第２の動特性演算部における特性を前記フィードバック制御動特性で除した特性に対応するように、前記第１の動特性演算部の特性を与えるように構成されていることを特徴とする、請求項１に記載の多関節ロボットの弾性変形補償制御装置。
　各軸間の軸力がない状態でのモータ角度目標値θｍｄから実際のモータ角度θｍまでのフィードバック制御動特性を、位置フィードバックゲインおよび速度フィードバックゲインの少なくともいずれかを含むパラメータに基づいて算出するように構成され、
　前記第２の動特性演算部におけるカットオフ周波数を前記第１の動特性演算部におけるカットオフ周波数よりも低くするとともに、前記第２の動特性演算部における特性を前記フィードバック制御動特性で除した特性に対応するように、前記第１の動特性演算部の特性を与えるように構成されていることを特徴とする、請求項２に記載の多関節ロボットの弾性変形補償制御装置。
　前記モータ電流制御部の電流制御特性を考慮する場合、前記第２の動特性演算部における特性を前記フィードバック制御動特性で除すとともに、電流制御特性を乗じた特性に対応するように、前記第１の動特性演算部の特性を与えるように構成されていることを特徴とする、請求項３に記載の多関節ロボットの弾性変形補償制御装置。
　前記モータ電流制御部の電流制御特性を考慮する場合、前記第２の動特性演算部における特性を前記フィードバック制御動特性で除すとともに、電流制御特性を乗じた特性に対応するように、前記第１の動特性演算部の特性を与えるように構成されていることを特徴とする、請求項４に記載の多関節ロボットの弾性変形補償制御装置。
　前記モータ角度指令値計算部における計算周期が、前記モータ角度制御部における制御周期よりも遅い場合、
　前記モータ角度指令値計算部において、軸力トルク計算値または弾性変形量計算値と関節角度指令値との位相差を合わせ、位相補正後の軸力トルク計算値または弾性変形量計算値と位相補正後の関節角度指令値とから、モータ角度指令値を算出するように構成されていることを特徴とする、請求項１～請求項６のいずれか１項に記載の多関節ロボットの弾性変形補償制御装置。
　弾性変形する減速器を介してモータとアームとが結合された多関節ロボットに取り付けられたツールに所望の動作を行わせるように複数の関節軸を前記モータにより駆動する多関節ロボットの弾性変形補償制御方法であって、
　前記ツールの所望の動作を実現するための各関節軸の関節角度指令値θｌｃを算出して出力する関節角度指令値計算ステップと、
　前記関節角度指令値θｌｃ通りに動作した際に発生する各関節軸に作用する軸力トルクｆｃを、動力学モデルに基づいて関節角度指令値θｌｃから算出して出力する軸力トルク計算ステップと、
　前記関節軸の剛性パラメータを含むパラメータに基づいて、関節角度指令値θｌｃと軸力トルクｆｃとからモータ角度指令値θｍｃを算出して出力するモータ角度指令値計算ステップと、
　前記多関節ロボットの固有振動周波数よりも低いカットオフ周波数を有する高周波遮断特性を備え、前記モータ角度指令値θｍｃをフィルタリング処理して、処理後のモータ角度目標値θｍｄを出力する第１の動特性演算ステップと、
　前記モータ角度目標値θｍｄが前記モータに対する目標値として入力されるモータ角度制御ステップと、
　前記第１の動特性演算ステップよりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備え、前記軸力トルク計算ステップへの入力および前記軸力トルク計算ステップからの出力の少なくともいずれかをフィルタリング処理して、処理後の軸力トルク補償値ｆｄを出力する第２の動特性演算ステップと、
　前記モータ角度制御ステップから出力されるモータトルク指令値に、前記軸力トルク補償値ｆｄが加算された値が目標値として入力されるモータ電流制御ステップと、
　を含んで構成されていることを特徴とする多関節ロボットの弾性変形補償制御方法。