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

Abstract

【課題】モータとアームとが弾性変形する減速器を介して結合された多関節ロボットにおいて、各軸の弾性変形の影響を補償して高い軌跡精度でウィービング動作させるとともに、外乱の振動を速やかに収束させる。
【解決手段】弾性変形補償制御装置５０は、関節角度指令値計算部１００、軸力トルク計算部２００、動特性演算部３００、４００、４１０、オブザーバ５０００を含むフィードバック制御部５５００およびモータ角度指令値計算部６００で構成される。第３の動特性演算部４１０は、オブザーバ５０００で推定された関節側の状態量（関節角度）の目標値である関節角度指令値計算部１００からの出力θｌｃを、ローパスフィルタ処理することにより振動成分を除外して状態フィードバック制御の目標値として与える。
【選択図】図１２

Description

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

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

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

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

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

特開昭６１−２０１３０４号公報 特開２００５−１８６２３５号公報

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

また、特許文献２では、各軸間の干渉を含めた弾性変形補償補償について記載されている。しかしながら、アーム加速度の１階微分値、２階微分値が必要であり、非常にノイズに弱く、また少し急峻な動作をさせただけで、アーム加速度の２階微分値は天文学的な値を示すなど、実現する上で非常に制約が大きい。
すなわち、従来技術では、以下のような問題を解決できていない。

（１）ロボットの固有振動が低い状態で、弾性変形補償および軸力トルク補償を有効に作用させることができないため、弾性変形の影響を補償できず、精度劣化につながっている。
（２）溶接ロボットのウィービング動作では、ウィービング周期での位相遅れおよびゲイン特性を各軸そろえることが非常に重要であるが、減速器による弾性変形によるサーボ特性変化や軸毎での特性の差異によって、位相・ゲイン特性を高周波ウィービング動作でそろえることが非常に困難である。さらに、外乱として溶接ロボットのアームが障害物に衝突したりして加振力が加わったときに、速やかにその外乱に基づく振動を早期に収束できず、高精度なウィービング動作を実現できていない。

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

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

好ましくは、前記第１の動特性演算部よりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備え、前記軸力トルク計算部への入力および前記軸力トルク計算部からの出力の少なくともいずれかをフィルタリング処理して、処理後の軸力トルク補償値ｆｄを出力する第３の動特性演算部をさらに含んで構成され、前記モータ電流制御部には、
前記目標値に、前記軸力トルク補償値ｆｄが加算された値が目標値として入力されるように構成することができる。

さらに好ましくは、前記第２の動特性演算部は、前記第３の動特性演算部と同じように構成することができる。
さらに好ましくは、前記観測器は、軸力トルク補償値ｆを推定して出力するように構成され、前記モータ電流制御部には、前記目標値に、前記軸力トルク補償値ｆが加算された値が目標値として入力されるように構成することができる。

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

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

本発明の実施の形態に係る弾性変形補償制御装置が適用される多関節ロボットの全体構成を示す概略図である。 本発明の第１の実施の形態に係る弾性変形補償制御装置の制御ブロック図である。 図２に示す制御ブロック図により制御された多関節ロボットのウィービング軌跡を示す図である。 従来技術（その１）に係る弾性変形補償制御装置の制御ブロック図である。 図４に示す制御ブロック図により制御された多関節ロボットのウィービング軌跡を示す図である。 従来技術（その２）に係る弾性変形補償制御装置の制御ブロック図である。 図６に示す制御ブロック図により制御された多関節ロボットのウィービング軌跡を示す図である。 本発明の第２の実施の形態に係る弾性変形補償制御装置の制御ブロック図である。 図８に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボットのウィービング軌跡を示す図である。 本発明の第３の実施の形態に係る弾性変形補償制御装置に関係する装置の制御ブロック図である。 図１０に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボットのウィービング軌跡を示す図である。 本発明の第３の実施の形態に係る弾性変形補償制御装置の制御ブロック図である。 図１２に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボットのウィービング軌跡を示す図である。 本発明の第３の実施の形態の第１の変形例に係る弾性変形補償制御装置の制御ブロック図である。 本発明の第３の実施の形態の第２の変形例に係る弾性変形補償制御装置の制御ブロック図である。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

以上のようにして、本実施の形態に係る弾性変形補償制御装置によると、多関節ロボットにおいて、各軸の弾性変形の影響を補償して高い軌跡精度でウィービング等の動作を可能とすることができる。
＜第２の実施の形態＞
以下、本発明の第２の実施の形態に係る弾性変形補償制御装置について説明する。なお、本実施の形態に係る弾性変形補償制御装置は、上述した第１の実施の形態に係る弾性変形補償制御装置１０が備えなかった速度フィードフォワード制御および／または加速度フィードフォワード制御が加えられている点が異なる。それ以外は、第１の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

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

図１２に示すように、弾性変形補償制御装置３０は、モータ角度制御部５１０に代えてモータ角度制御部３５１０を備える。
この図１２に示す制御ブロックの場合、各軸サーボフィードバック制御の動特性（サーボＦＢ制御動特性）は、以下の式（１）で与えられる。なお、「動特性１」は、第１の動特性演算部３００における動的特性を、「動特性２」は、第２の動特性演算部４００における動的特性をそれぞれ示している。

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

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

なお、非線形項である軸力トルク計算部２００の前後に、第２の動特性演算部を配置す
るようにしても構わない。より詳しくは、軸力トルク計算部２００の前に動特性（２１）を、軸力トルク計算部２００の後に動特性（２２）を、それぞれ配置した。ここで、動特性（２１）×動特性（２２）が上述した動特性（２）と一致するように与えれば、これまでの実施の形態と同等の効果が得られる。ここで、θｌｅは関節角度指令値θｌｃを動特性（２１）に入力したときの出力値であり、θｌｅに基づいて軸力トルク計算部２００にて算出された軸力トルクｆｃを動特性（２２）に入力したときの出力値が軸力トルク補償値ｆｄである。このような弾性変形補償制御装置によると、多関節ロボットにおいて、各軸の弾性変形の影響を分割して配置された第２の動特性演算部により補償して、高い軌跡精度でウィービング等の動作を可能とすることができる。

＜第３の実施の形態＞
以下、本発明の第３の実施の形態に係る弾性変形補償制御装置について説明する。なお、本実施の形態に係る弾性変形補償制御装置は、上述した第２の実施の形態に係る弾性変形補償制御装置１０が備えなかったオブザーバ（観測器）およびＰＩＤ制御器が加えられている点が異なる。これらの加えられた機器により状態フィードバック制御が実現されている。それ以外は、第２の実施の形態と同じであるので、上述した説明と重複する部分についてはここでは繰り返さない。

上述した第２の実施の形態に係る弾性変形補償制御装置３０においては、速度フィードフォワード制御および加速度フィードフォワード制御を備えている。このような弾性変形補償制御装置３０において、多関節ロボット１のアーム等が障害物に衝突したなど（外乱）により加振力が加わった場合について考える。
図９に、第２の実施の形態に係る弾性変形補償制御装置の制御ブロック図（図８）により制御された、加振力が加わった場合の多関節ロボット１のウィービング軌跡を示す。この図９に示すように、時間０で加わった加振力は、０．５秒後の上下方向においても減衰しないで、振動を持続している。これは、第２の実施の形態に係る弾性変形補償制御装置３０においては、上述したようにフィードフォワード制御をベースにしているため、関節側に外乱などの加振力が加わった場合、振動を速やかに抑制することができない。このような上下方向の大きな振幅の振動が持続するという現象は、ウィービング動作における高精度な動的精度を確保することができていないことを示す。

このような場合、発生した振動を早期に減衰させるために、関節側の状態（関節角度、関節角速度）をオブザーバで推定して、状態フィードバック制御が行われる。すなわち、外乱などの加振力によって振動が発生した場合、それを素早く抑制するには、関節側の情報（関節角度、関節角速度、または軸捩れ量＝関節角度−モータ角度）を状態フィードバックする必要がある。関節側の情報（ここでは関節角度）をオブザーバにて推定し、関節角度指令値θｌｃとの偏差に基づいて、状態フィードバックする場合の制御ブロック図を図１０に示す。

図１０に示すように、オブザーバ５０００で推定されて出力された関節側の状態量である関節角度の状態量が、関節角度θｌｃから減算されて、その演算結果（偏差）がＰＩＤ制御器５０１０へ入力されて、ＰＩＤ制御器５０１０での演算結果が、モータ角度制御部３５１０から出力されるモータトルク指令値に加算される。
図１１に、図１０に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボット１のウィービング軌跡を示す。この図１１に示すように、オブザーバ５０００で推定されて出力された関節側の状態量と目標値との偏差に基づいて状態フィードバック制御すると、上下方向の振幅が異常に大きな大鋸歯状に悪化し、上下方向の振動がウィービング振幅よりも大きくなってしまう。このように、ウィービング波形ではウィービング振幅の５倍以上の上下動が発生するなど、非常に好ましくないウィービング波形となっている。これは、偏差を演算する場合の目標値が適正に与えられていないことが原因と考えられる。

このような観点から、第３の実施の形態に係る弾性変形補償制御装置５０においては、図１２に示すように、関節角度指令値計算部１００からの出力（関節角度指令値θlc）をフィルタリング処理して、処理後の関節角度目標値θｌｄを出力する第３の動特性演算部
４１０を設けた。それ以外の構成は図１０と同じく、関節側の状態量を推定するオブザーバ５０００、オブザーバ５０００で推定されて出力された関節側の状態量と関節角度目標値θｌｄとの偏差に基づいてＰＩＤ制御するＰＩＤ制御器５０１０とを備え、ＰＩＤ制御器５０１０での演算結果が、モータ角度制御部３５１０から出力されるモータトルク指令値に加算される。

第３の動特性演算部４１０は、第１の動特性演算部３００よりも低いまたは同等のカットオフ周波数を有する高周波遮断特性を備える。以下の説明では、第２の動特性演算部４００と同じ動特性を備えるものとして説明する。なお、請求項１における「第２の動特性演算部」は第３の動特性演算部４１０に対応し、請求項２における「第３の動特性演算部」は第２の動特性演算部４００に対応する。

図１３に、図１２に示す制御ブロック図により制御された、加振力が加わった場合の多関節ロボット１のウィービング軌跡を示す。この図１３に示すように、オブザーバ５０００で推定されて出力された関節側の状態量とフィルタ処理された後の目標値との偏差に基づいて状態フィードバック制御すると、上下方向の振幅が速やかに収束している。すなわち、図９と比較して、図１３においては、時間０で加わった振動は、０．３秒程度で収束しており、その後の上下方向の振動は減衰して持続していない。このような上下方向の大きな振幅の振動が速やかに収束するという現象は、ウィービング動作における高精度な動的精度を確保することができていることを示す。

これは、偏差を演算する場合の目標値が適正に与えられていることが理由である。すなわち、目標値から多関節ロボット１の固有振動周期を遮断するローパスフィルタ特性を備えた第３の動特性演算部４１０を設けて、振動成分を目標値（ここでは関節角度）から除去する。このように除去された目標値（ここでは微分値である関節角度）が、関節側状態量の目標値として与えられ偏差が演算されて、ＰＩＤ制御された結果が、状態フィードバック量として処理される。

このように処理することにより、関節側の状態量の目標値が適正に与えられることになり、振動を抑制しつつ、多関節ロボット１の先端における高精度の動作が実現できる。
以上のようにして、本実施の形態に係る弾性変形補償制御装置によると、関節側の状態量をオブザーバで推定して、その状態量の目標値をローパスフィルタ処理して振動成分を目標値から除外してから与えることにより、外乱として多関節ロボットのアームが障害物に衝突したり加振力が加わったりしても、速やかに振動を収束させることができ、各軸の弾性変形の影響を補償して、高い軌跡精度でウィービング等の動作を可能とすることができる。

＜第３の実施の形態の変形例＞
次に、第３の実施の形態の２つの変形例に係る弾性変形補償制御装置について説明する。
１つめの変形例である第１の変形例に係る弾性変形補償制御装置６０の制御ブロック図を図１４に示す。図１２に示した制御ブロック図との差は、（１）オブザーバ５０００への入力側に微分演算を備える点、（２）オブザーバ５０００からの出力である関節側の状態量が関節角度ではなく関節角速度である点、（３）これら２点に対応して、第３の動特性演算部４１０の出力後に微分演算を備える点、である。

このため、第１の変形例に係る弾性変形補償制御装置６０においては、状態フィードバックされる状態量は関節角速度となり、それが関節角度である第３の実施の形態と異なる。第１の変形例に係る弾性変形補償制御装置６０のように、関節角速度を用いて振動抑制した方が、抑制性能は高く好ましい。
次に、２つめの変形例である第２の変形例に係る弾性変形補償制御装置７０の制御ブロック図を図１５に示す。図１４に示した制御ブロック図との差は、（１）第２の動特性演算部４００を備えずこれによりフィルタリング処理された軸力トルク補償値ｆｄにより軸力補償されていない点、（２）オブザーバ５０００からの出力により軸力補償している点、である。

このため、第２の変形例に係る弾性変形補償制御装置７０においては、軸力補償するの
はオブザーバ５０００からの出力となり、それが軸力トルク計算部２００からの出力である第３の実施の形態と異なる。弾性変形補償制御装置５０および弾性変形補償制御装置６０では、軸力をフィードフォワード的に補償しているが、第２の変形例に係る弾性変形補償制御装置７０のようにオブザーバ５０００などで推定しフィードバック補償することも可能である。ただし、フィードバック補償のため、応答遅れがありウィービング精度は若干悪くなるが、従来の補償しない場合に比べて断然良い精度および振動抑制が可能となっている。

＜実施の形態の作用効果＞
上述した第１〜第３の実施の形態のように、以下のように弾性変形補償制御装置を構成したため、モータとアームとが弾性変形する減速器を介して結合された多関節ロボットにおいて、高い軌跡精度でウィービング等の動作を可能とすることができる。
（１）目標値から固有振動周期を遮断するローパスフィルタ特性を与え、振動成分を目標角度から除去するとともに、軸力トルクについてもローパスフィルタ処理し、目標値と軸力トルクの位相を揃え、振動抑制しつつ弾性変形補償を行う。

（２）状態フィードバック制御（オブザーバ、ＰＩＤ制御器）を付加して、その状態量の目標値をフィルタリング処理して振動成分を目標値から除外して、外乱として多関節ロボットのアームが障害物に衝突したりして加振力が加わっても、速やかに振動を収束させる。
なお、今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

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

Claims (6)

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

Images