JP6097174B2 - ロボット制御装置 - Google Patents

Description

本発明の実施形態は、ロボット制御装置に関する。

多リンクロボットのアーム先端の振動を抑制するためには、各軸を駆動するモータ内蔵エンコーダで計測したモータ角速度から軸ねじれ角速度、すなわちリンク角速度とモータ角速度の差を推定し、モータ角速度制御系にフィードバックしなければならない。その推定には、弾性関節やリンク間の非線形干渉力を考慮に入れた非線形動力学モデルに基づく非線形のオブザーバが必要である。

上記のような多入出力かつ非線形のオブザーバの実現においては、正確な動力学モデルの構築や、先端負荷および摩擦力の変動へのロバスト性を要求される。従来は、制御の演算量削減の観点から「１リンク毎の弾性関節モデルにおいて他リンクからの外乱推定と補償」を行う１入出力線形モデルにおいて＋αの処理を行うことに基づく近似的なオブザーバが主であった。

また、近年のＣＰＵやメモリの性能向上によれば、ロボット制御装置においても豊富な演算能力や記憶容量の恩恵にあずかることができるようになってきた。

ところが、上記のようなオブザーバでは、近似に基づいているので振動抑制効果がロバストでない上に、かえって制御則の実装や制御ゲインの調整にかかるエンジニアリングコストが多大であった。

また、最近では角速度センサの入手が容易になりつつあるため、この角速度センサをロボットアームの各リンクに搭載し、軸ねじれ角速度を直接計測してフィードバックすることによって振動抑制することも考えられる。しかし、この手法では配線が増えることによるコスト面などの問題が生じることもある。

さらには、状態オブザーバによる推定にせよ、直接計測にせよ、軸ねじれ角速度をフィードバックする振動抑制制御系では、アーム先端の負荷質量が軽いとき、すなわち低慣性時に振動抑制制御の減衰効果が小さくなってしまう、という課題がある。

特公平７−１２０２１２号公報 特許第４２８９２７５号公報 特許第２７７２０６４号公報

小林、他：ロボット制御の実際、第３章ロボットの同定、コロナ社、１９９７年、ｐ．６２−８５ 杉本、他：ＡＣサーボシステムの理論と設計の実際、第７章速度制御系および位置制御系の設計、総合電子出版社、１９９０年、ｐ．１５３−１７９ 武居、他：二慣性系の慣性比と振動減衰性に関する一考察、電気学会論文誌Ｄ、１２１巻、２号、２００１年、ｐ．２８３−２８４

本実施形態は、簡易な構成でロボットアーム先端の振動抑制効果を高めることができるロボット制御装置を提供する。

本実施形態のロボット制御装置は、モータの回転軸とリンクの回転軸との間に弾性機構を有するロボットアームを制御対象とし、前記モータの角速度を比例積分制御して前記モータへ電流指令値を出力する角速度制御系を有するロボット制御装置である。このロボット制御装置は、前記モータの角速度と前記電流指令値とを入力とし、前記ロボットアームの非線形動力学モデルを持ち、前記角速度制御系と等価なゲインを用いて比例積分制御される前記モータの角速度制御系のシミュレーションモデルから前記リンクの角加速度と、前記リンクの角速度と、前記モータの角速度とを推定するオブザーバと、前記オブザーバによって推定された前記リンクの角速度と、前記オブザーバによって推定された前記モータの角速度との差から軸ねじり角速度を算出し、前記角速度制御系にフィードバックする第１フィードバック部と、前記オブザーバによって推定された前記リンクの角加速度を前記角速度制御系にフィードバックする第２フィードバック部と、前記第２フィードバック部において、前記非線形動力学モデルで先端負荷が低慣性の場合に先端負荷質量を補償して高慣性化する第１フィードバック定数算出部と、を備えている。

第１実施形態によるロボット制御装置の制御対象の一例であるロボットアームの断面図。 図１に示すロボットアームの１リンク分を示す模式図。 一般的な位置および速度制御系を示すブロック図。 非線形最適化を用いた物理パラメータのファインチューニングを示す図。 第１実施形態によるロボット制御装置のオブザーバを示すブロック図。 第１実施形態によるロボット制御装置を示すブロック図。 第１実施形態による振動抑制制御の一例を示す図。 第１実施形態による振動抑制制御の一例を示す図。 第１実施形態による振動抑制制御の効果を示す図。 第２実施形態によるロボット制御装置のオブザーバを示すブロック図。 第３実施形態によるロボット制御装置のオブザーバを示すブロック図。 第４実施形態によるロボット制御装置のオブザーバを示すブロック図。 第５実施形態によるロボット制御装置のオブザーバを示すブロック図。

以下、図面を参照しながら実施形態を詳細に説明する。

各実施形態を説明するまえに、実施形態に至った経緯について説明する。

図１は、各実施形態に係るロボット制御装置の制御対象の一例である２リンクロボットアームの断面図である。このロボットアームは、架台１、第１リンク３、第１モータ４、第１減速機５、第１エンコーダ６、第２リンク８、第２モータ９、第２減速機１０、第２エンコーダ１１を有する。架台１の上部に第１リンク３の一端が取り付けられており、第１リンク３の他端には第２リンク８が取り付けられている。第２リンク８の先端に負荷１２が付加される。

制御装置１３は、第１モータ４及び第１エンコーダ６と、バネ特性を持つ第１減速機５との組合せにより、第１軸２を中心として架台１に対して第１リンク３を水平旋回させる。また、制御装置１３は、第２モータ９及び第２エンコーダ１１と、バネ特性を持つ第２減速機１０との組合せにより、第２軸７を中心として第１リンク３に対して第２リンク８を水平旋回させる。

図２は、このロボットアームの１リンク分を示す模式図である。これは、２慣性系と呼ばれる。図１及び図２を参照して、このロボットアームは、弾性関節を持つシリアル２リンクアームとして非線形動力学モデル化することができる。図２では、１リンク分の非線形動力学モデルの記述に必要な物理パラメータ、慣性モーメント、摩擦係数、およびバネ係数などについて第１軸を代表として示す。この図２においては、モータ２０によってリンク３０が減速機２５を介して駆動制御される。第１軸に関する物理パラメータとしては、モータ２０のトルク入力がｕ_１で表され、モータ２０の慣性モーメントがｍ_Ｍ１で表され、モータ２０の回転角、すなわちエンコーダによって検出された出力がθ_Ｍ１で表され、モータ２０の粘性摩擦係数がｄ_Ｍ１で表され、モータ２０のクーロン動摩擦係数がｆ_Ｍ１で表され、減速機２５の減衰係数がｄ_Ｇ１で表され、減速機２５のバネ係数がｋ_Ｇ１で表され、リンク３０の慣性モーメントがｍ_Ｌ１で表され、リンク３０の粘性摩擦係数がｄ_Ｌ１で表され、リンク３０の回転角がθ_Ｌ１で表される。

弾性関節を持つシリアル２リンクアームの非線形動力学モデルは、次のようなモータ側が式（１）で与えられ、リンク側が式（２）で与えられる。

ただし、
θ_Ｍ＝[θ_Ｍ１，θ_Ｍ２]^Ｔ：モータの回転角（サフィックス１、２は軸番号）
θ_Ｌ＝[θ_Ｌ１，θ_Ｌ２]^Ｔ：リンクの回転角
α＝[α_１，α_２]^Ｔ：リンクの並進加速度
Ｍ_Ｌ（θ_Ｌ）εＲ^２×２：リンク慣性行列
ｃ_Ｌ（ｄθ_Ｌ／ｄｔ，θ_Ｌ）εＲ^２×１：遠心力およびコリオリ力ベクトル
Ｍ_Ｍ＝ｄｉａｇ（ｍ_Ｍ１，ｍ_Ｍ２）：モータおよび減速機高速段慣性
Ｄ_Ｍ＝ｄｉａｇ（ｄ_Ｍ１，ｄ_Ｍ２）：モータ軸粘性摩擦係数
Ｄ_Ｌ＝ｄｉａｇ（ｄ_Ｌ１，ｄ_Ｌ２）：リンク軸粘性摩擦係数
Ｋ_Ｇ＝ｄｉａｇ（ｋ_Ｇ１，ｋ_Ｇ２）：減速機バネ係数
Ｄ_Ｇ＝ｄｉａｇ（ｄ_Ｇ１，ｄ_Ｇ２）：減速機減衰係数
Ｎ_Ｇ＝ｄｉａｇ（ｎ_Ｇ１，ｎ_Ｇ２）：減速比（ｎ_Ｇ１，ｎ_Ｇ２≦１）
ｆ_Ｍ＝[ｆ_Ｍ１，ｆ_Ｍ２]^Ｔ：モータ軸のクーロン動摩擦トルク
Ｅ＝ｄｉａｇ（ｅ_１，ｅ_２）：トルク／電圧（電流指令値）定数
ｕ＝[ｕ_１，ｕ_２]^Ｔ：入力電圧（モータの電流制御系への指令値）である。ここで，式（１）中のｓｇｎ（ａ）は符号関数を表し、ａが正、負、零の値に応じてｓｇｎ（ａ）はそれぞれ１、−１、０の値を取る。また、ｄｉａｇ（ａ、ｂ）は、ａおよびｂを対角要素とする対角行列を表す。

ここで、α、β、γをリンクの長さ、重心位置、質量、慣性より構成される基底パラメータとすると、リンクの慣性行列は式（３）のように表される。

上記基底パラメータの詳細は、以下の式（４）にように表される。
α＝ｍ_１ｌ_ｇ１ ^２＋Ｉ_ｚ１＋ｍ_２ｌ_１ ^２
β＝ｍ_２ｌ_ｇ２ ^２＋ｌ_ｚ２ （４）
γ＝ｍ_２ｌ_１ｌ_ｇ１
ただし、
ｌ_ｉ：各リンクの長さ（ｉは軸番号）
ｍ_ｉ：各リンクの質量
ｌ_ｇｉ：各リンクの重心位置（リンクは長手方向に左右対称）
ｌ_ｚｉ：各リンクの重心周りの慣性モーメント
である。先端負荷１２が変化すると、この先端負荷が付加されているリンクの質量ｍ_２が変化し、基底パラメータα、β、γがともに変化する。

遠心力およびコリオリ力ベクトルについては、式（５）のようになる。

モータ角速度制御系については、ＰＩ（比例および積分）制御をＦＦ−Ｉ−Ｐ（フィードフォワード−積分−比例）制御として２自由度ＰＩ制御にした式（６）を基本構成とする。
ただし、
ｄθ_ＭＲｉ／ｄｔ：モータの角速度目標値（ｉは軸番号）
ｄθ_Ｍｉ／ｄｔ：モータの角速度
ｋ_ＦＶｉ：モータの角速度目標値フィードフォワード制御ゲイン
ｋ_ＩＶｉ：モータの角速度偏差積分フィードバック制御ゲイン
ｋ_ＰＶｉ：モータの角速度比例フィードバック制御ゲイン
ｕ_ｉ：入力電圧（モータの電流制御系への指令値（トルク入力））
である。

この２自由度ＰＩ速度制御系は、図３に示すように、位置制御系（Ｐ制御）の内側にカスケード接続された制御系として構成される。なお、図３においては、第１軸に関する２自由度ＰＩ速度制御系を示している。

以下においては、位置制御系を除いたモータの角速度制御系に焦点を当てて、制御周期が十分短いものと仮定しこれによりモータの角度制御系が連続系であるとして説明を行う。

まず、図１に示す２リンクアームの先端負荷１２を例えば５ｋｇとし、式（１）に現れる物理パラメータ（以下においては、基底パラメータα、β、γも含むものとする）を公知の同定法を用いて推定する。この同定法としては、例えば非特許文献１に記載の同定法またはロボットモデルの関節剛性を考慮した特許文献３などの同定法が用いられる。

次に、これらの推定された物理パラメータをモータ速度制御ゲインと共に、動力学モデルを表す式（１）および速度フィードバック制御則を表す式（６）に代入する。これにより、シリアル２リンクアームの速度制御系の時間応答シミュレータを構築し、ロボット実機の時間応答と良く合うかどうかを確認する。

上記の同定法によって物理パラメータが精度よく推定されていれば、これらの時間応答は良く合うはずである。もし、少しずれているようであれば、上記時間応答シミュレータを用いた非線形最小２乗法（レーベンバーグ−マルカート法など）による最適化計算を用いて、物理パラメータをファインチューニングする。例えば、図４（ａ）に最適化前、図４（ｂ）に最適化後の角速度ステップ応答を示す。図４（ａ）、４（ｂ）において、横軸は時間を示し、縦軸は角速度を示し、実線はロボット実機の応答を示し、破線はシミュレータによる応答結果を示す。図４（ｂ）からわかるように、物理パラメータを最適化することで、シミュレータとロボット実機の角速度ステップ応答を良く合わせることができる。

そこで、本実施形態においては、上記エンコーダで計測したモータの角速度からリンクの角速度を推定するオブザーバを構成し、モータの角速度の計測値だけでなく、リンクの角速度の推定値もフィードバックすることによってロボットアーム先端の振動を抑制しながら精度良く動作させる。

ここで、オブザーバの推定値を用いた状態フィードバックの場合、オブザーバ自体のフィードバックゲインと、状態フィードバックのゲインの設計および調整が必要になる。オブザーバにおいて状態変数を再現してから、フィードバック制御するという順番から、通常、オブザーバの極は状態フィードバックの極より複素平面上でずっと左半平面（実数部が負の領域）寄りに配置する、というのが現代制御理論の立場である。

しかし、実際のところ種々のノイズやモデル化誤差があるオブザーバでは、そのようなハイゲインにはできない。実機の応答を見ながらオブザーバの極の配置を調整すると、結局、状態フィードバックの極の配置に近くなることもある。

ここで、カルマンフィルタを用いれば、オブザーバの設計をシステマティックに行える。しかしこの場合、仮定するノイズモデルのパラメータの設定が必要である。さらに、オブザーバおよび状態フィードバックを無くしたＨ^∞制御（Ｈ無限大制御）などのロバスト制御では、ノイズやモデル化誤差を実際に考慮した制御系の設計ができる。しかし、いずれの方法にせよ、ＣＡＥ（Computer-Aided Engineering）ツールが不可欠なので現場での手調整は難しい。

そこで、以下の実施形態では、上述したように、速度制御されたロボット実機とシミュレーションの時間応答とが良く合うことに着目し、物理パラメータの推定精度の良さを生かしたオブザーバを構成する。以下、各実施形態に係るオブザーバを説明する。

（第１実施形態）
第１実施形態によるロボット制御装置のオブザーバを図５に示す。この第１実施形態のオブザーバ２００は、第１および第２軸のＰＩ制御器２０１、２０２と、２リンクアームの非線形動力学モデル２０３と、非線形動力学モデル２０３の出力を積分する積分器２０４ａ、２０４ｂ、２０４ｃ、２０４ｄ、２０５ａ、２０５ｂ、２０５ｃ、２０５ｄとを有する。ＰＩ制御器２０１は、第１軸を駆動するモータの速度ｄθ_Ｍ１／ｄｔと第１軸を駆動するモータの回転角の推定値との偏差に基づいてＰＩ制御する。ＰＩ制御器２０２は、第２軸を駆動するモータの速度ｄθ_Ｍ２／ｄｔと第２軸を駆動するモータの回転角の推定値との偏差に基づいてＰＩ制御する。２リンクアームの非線形動力学モデル２０３は、ＰＩ制御器２０１の出力と第１軸の操作量ｕ_１との和となる第１入力τ_１およびＰＩ制御器２０２の出力と第２軸の操作量ｕ_２との和となる第２入力τ_２に基づいて非線形動力学モデルに基づいて第１および第２リンクの角加速度の推定を行うとともに第１および第２リンクをそれぞれ駆動するモータの角加速度の推定を行い、推定した角加速度を出力する。

積分器２０４ａは、非線形動力学モデル２０３から出力される第１リンクの角加速度の推定値を積分し、角速度の推定値を出力する。積分器２０４ｂは、積分器２０４ａの出力を積分し、第１リンクの回転角の推定値を出力する。積分器２０４ｃは、非線形動力学モデル２０３から出力される第１リンクを駆動するモータの角加速度の推定値を積分し、上記モータの角速度の推定値を出力する。積分器２０４ｄは、積分器２０４ｃの出力を積分し、第１リンクを駆動するモータの回転角の推定値を出力する。

積分器２０５ａは、非線形動力学モデル２０３から出力される第２リンクの角加速度の推定値を積分し、角速度の推定値を出力する。積分器２０４ｂは、積分器２０４ａの出力を積分し、第２リンクの回転角の推定値を出力する。積分器２０４ｃは、非線形動力学モデル２０３から出力される第２リンクを駆動するモータの角加速度の推定値を積分し、上記モータの角速度の推定値を出力する。積分器２０４ｄは、積分器２０４ｃの出力を積分し、第２リンクを駆動するモータの回転角の推定値を出力する。

これらの第１および第２リンクの角加速度の推定値、第１および第２リンクの角速度の推定値、第１および第２リンクを駆動するモータの角速度の推定値、およびモータの回転角の推定値は、それぞれオブザーバ２００から出力される。

また、このオブザーバ２００において、第１リンクの角速度の推定値、第１リンクとこの第１リンクを駆動するモータとの間に設けられる減速機の減速比ｎ_Ｇ１、および第１リンクを駆動するモータの角速度の推定値に基づいて、第１リンクのねじれ角速度の推定値を演算し、この推定値が出力される。同様に、第２リンクの角速度の推定値、第２リンクとこの第２リンクを駆動するモータとの間に設けられる減速機の減速比ｎ_Ｇ２、および第２リンクを駆動するモータの角速度の推定値に基づいて、第２リンクのねじれ角速度の推定値を演算し、この推定値が出力される。

この第１実施形態のオブザーバ２００は、ロボットアームの非線形動力学モデル２０３を丸ごと内蔵し、ＰＩ制御器２０１、２０２のオブザーバゲインはロボットアームの各軸の既存の速度制御系のＰＩ制御ゲインをそのまま使用する、というシミュレータそのものである。すなわち、ＰＩ制御器２０１、２０２のオブザーバゲインは速度制御と等価なオブザーバゲインである。また、積分制御が入っているので出力推定値の定常偏差を零にする機能も内蔵されている。そして、オブザーバの実装における近似、およびゲイン調整にかかるエンジニアリングコストが全くない。

非線形動力学モデルに基づくＰＩ制御型オブザーバは、式（１）を変形した式（７）に示す２階微分形式で表される。なお、式（７）において、記号「＾」は推定値を表す。
ただし、
ｄθ_Ｍ／ｄｔ＝[ｄθ_Ｍ１／ｄｔ, ｄθ_Ｍ２／ｄｔ]^Ｔ：オブザーバへのモータ角速度入力
ｕ＝[ｕ_１,ｕ_２]^Ｔ：オブザーバへの入力（モータ電流指令値）
Ｋ_ＰＶ＝ｄｉａｇ（ｋ_ＰＶ１，ｋ_ＰＶ２）：速度偏差比例制御ゲイン
Ｋ_ＩＶ＝ｄｉａｇ（ｋ_ＩＶ１，ｋ_ＩＶ２）：速度偏差積分制御ゲイン
τ＝[τ_１,τ_２]^Ｔ：オブザーバへの入力（モータ電流指令値）
である。

ここで、ＰＩ制御系２０１、２０２のオブザーバゲインは、ロボット実機の速度ループのＦＦ−Ｉ−Ｐ制御（２自由度ＰＩ制御）のＰとＩのゲインと同じものを選んでいる。この場合、オブザーバの追従性能の１自由度だけを考えれば良いので、ＦＦ＝Ｐ、すなわちフィードフォワード系を比例系としたＰＩ制御として設定する。このシステムは、ゲインが一定の非線形オブザーバの一種である。また、積分制御が入っているので出力推定値の定常偏差は零になり、外乱除去オブザーバとしての機能も内蔵されている。

式（７）で表される状態オブサーバをロボット制御装置１３に実装し、リアルタイムで図５に示す積分器を用いて２回積分することによって状態の推定ができる。なお、実際の積分は数値積分によって行われる。

さて、２慣性系の角速度制御においては、モータの角速度とリンクの角速度、モータとリンクとの軸ねじれ角速度の３状態量をフィードバックすれば、任意の極の配置が可能となり、速応性（制御系の目標値に追従する速さ）と減衰率を自在に設定できる。しかし、既存のＰＩ制御系の再調整を伴うので移行するのは困難である。そこで、ＰＩ制御系への入力として、まず、オブザーバ２００から出力される軸ねじれ角速度（リンク角速度とモータ角速度の差）の推定値だけを状態フィードバックする。この軸ねじれ角速度のフィードバックでは、ＰＩ制御のゲイン交差周波数を変化させずに減衰率だけを増加させる効果が見込まれる（例えば、非特許文献２）。このため、現場での手調整が容易なので、特に産業用ロボットの制御装置には導入しやすい。

図６に、図５に示すオブザーバ２００を用いた状態フィードバック制御のブロック図を示す。オブザーバ２００には、モータ角速度（エンコーダ差分）とモータドライバへの入力電圧（電流指令値）が入力される。このＰＩ制御型のオブザーバ２００による、第１および第２リンクに関する軸ねじれ角速度の推定値の状態フィードバック部３０１、３０２、３０３、３０４は、式（７）に示す２自由度ＰＩ制御（ＦＦ−Ｉ−Ｐ制御）にプラグインする形で実現可能であり、式（８）に示す制御則になる。

この軸ねじれ角速度の推定値の状態フィードバック制御ゲインｋ_ＴＶｉ（ｉ＝１，２）は、時間応答波形を見ながらの手調整が容易である。また、弾性関節を持つロボットアームの正確な非線形動力学モデルに基づいたオブザーバを用いているので、シリアルリンクで構成されるロボットアームの振動抑制制御では、第１軸だけの状態フィードバックだけでも十分な効果がある。なお、図６には、高慣性化のためのリンク角加速度のフィードバック定数算出部３０５が示されているが、このフィードバック定数算出部３０５の機能については、後述する。

まず、先端負荷１２が５ｋｇの場合の軸ねじれ角速度のフィードバック制御が無い場合および有る場合における第１軸の速度ステップ応答波形を図７（ａ）、７（ｂ）にそれぞれ示す。なお、図７（ａ）、７（ｂ）においては、リンクの応答とモータの応答の波形は、減速比を考慮して同じ時間軸となるように調整して表示している。また、オブザーバでは、先端負荷５ｋｇでの動力学モデルを用いている。図７（ａ）からわかるように、ＰＩ制御のみ（オブザーバなし）の応答は、振動の振幅が大きく、減衰が遅い。これに対して、図７（ｂ）からわかるように、軸ねじれ角速度のフィードバックを付加した制御（オブザーバあり）の応答は、振動の振幅が小さく、減衰が速くなる。すなわち、図７（ａ）に示すＰＩ制御のみの応答に比べて、図７（ｂ）に示す軸ねじれ角速度のフィードバックを付加した制御の応答は、リンク角速度の応答の改善が著しいことがわかる。

次に、無負荷すなわち先端負荷１２が０ｋｇの場合の軸ねじれ角速度のフィードバック制御が無い場合および有る場合における第１軸の速度ステップ応答波形を図８（ａ）、８（ｂ）にそれぞれ示す。なお、図８（ａ）、８（ｂ）においては、リンクの応答とモータの応答の波形は、減速比を考慮して同じ時間軸となるように調整して表示している。オブザーバでは、先端負荷１２が０ｋｇでの動力学モデルを用いている。図８（ａ）からわかるように、ＰＩ制御のみ（オブザーバなし）の応答は、振動の振幅が大きく、減衰が遅い。これに対して、図８（ｂ）からわかるように、軸ねじれ角速度のフィードバックを付加した（オブザーバあり）応答は、振動の振幅が小さく、立ち上がりの減衰が悪くなる。すなわち、図８（ａ）のＰＩ制御のみの応答に比べて、図８（ｂ）に示す軸ねじれ角速度のフィードバックを付加した応答は、リンク角速度の応答の振幅は小さくなるが、立ち上がりの減衰が悪いことがわかる。

これは、アーム先端の負荷質量が軽いとき、すなわち低慣性時に軸ねじり角速度のフィードバックによる振動抑制制御の減衰効果が小さくなってしまうからである。つまり、２慣性系におけるリンク側とモータ側の慣性モーメントの比、すなわち慣性比が小さくなることによる減衰制御性能の劣化であると考えられる（非特許文献３）。

そこで本実施形態では、手先負荷が軽い場合にリンク側の慣性を大きく見せかけるための高慣性化制御について示す。その制御則は、式(８)にリンク角加速度の推定値のネガティブフィードバックを加えたものであり（図６の状態フィードバック部３０３、３０４）、式（９）に示すようになる。

ここで、角加速度の推定値の状態フィードバック制御ゲインｋ_ＡＶｉは、リンクの慣性モーメントの次元を持っており、ロボットアームの先端負荷の慣性変動分Δｍを補償できると考えられる。例えば、式（３）の慣性行列がΔｍの変動（減少）を受けると、基底パラメータα、β、γがそれぞれ式（１０）に示すように、α’、β’、γ’に変化する。
α’＝α―Δｍ
β’＝β−Δｍ （１０）
γ’＝γ−Δｍ
ここで、式（３）の慣性行列の（１、１）成分に、式（１０）に示すロボットアームの先端負荷の慣性変動後のα’、β’、γ’を代入し、減速比ｎ_Ｇ１を考慮すると、式（１１）に示すようになる。
ｋ_ＡＶ１＝（２＋２ｃｏｓ（θ_Ｌ２））Δｍ×ｎ_Ｇ１ （１１）
この式（１１）により、第１軸の角加速度の推定値の状態フィードバック制御ゲインを算出することができる。式（１１）が、図６に示す高慣性化のためのリンク角加速度のフィードバック定数算出部３０５の機能である。

次に、先端負荷１２が０ｋｇの場合の軸ねじれ角速度のフィードバックおよびリンク角加速度のフィードバックが無い場合および有る場合の第１軸の速度ステップ応答波形を図９（ａ）、９（ｂ）にそれぞれ示す。なお、図９（ａ）、９（ｂ）においては、リンクの応答とモータの応答の波形は、減速比を考慮して同じ時間軸となるように調整して表示している。また、オブザーバでは、先端負荷１２が０ｋｇでの動力学モデルを用いている。図９（ａ）からわかるように、ＰＩ制御のみ（オブザーバなし）の応答は、振動の振幅は小さく、立ち上がりの減衰が悪い。これに対して、図９（ｂ）からわかるように、軸ねじれ角速度のフィードバックおよびリンク角加速度のフィードバックが有る場合の波形は、振動の振幅が小さく減衰も速くなる。すなわち、図８（ａ）に示すＰＩ制御のみの場合は当然のこと、図８（ｂ）に示す軸ねじれ角速度のフィードバックだけを付加した場合と比較しても、図９（ｂ）に示す軸ねじれ角速度のフィードバックおよびリンク角加速度のフィードバックを付加した制御の場合は、リンク角速度の応答の改善が著しく、高慣性化制御の効果は明らかである。

（第２実施形態）
第２実施形態によるロボット制御装置を図１０に示す。この第２実施形態の制御装置は、高慣性化および定慣性化によるリンク角加速度のフィードバック制御およびそのゲイン算出機能を備えている。

上述した慣性の低下は、アーム先端の負荷の質量が軽いときだけでなく、ロボットアームの姿勢変化でも生じる。２リンクロボットアームでは、例えば、第２軸の角度が大きくなると第１軸の周りの慣性が低下することになる。そこで、姿勢変化による慣性の低下についても、リンク角加速度のフィードバックによって補償することを考える。

２リンクロボットアームの場合、式（３）に示す慣性行列の（１、１）成分の最大値からの慣性低下分を考慮すると、式（１２）に示すようになる。
ｋ_ＡＶ１＝２γ（１−ｃｏｓ（θ_Ｌ２））ｎ_Ｇ１ （１２）
この式（１２）により、第１軸のリンク角加速度のフィードバック制御ゲインを算出できる。このフィードバックによって、アームの姿勢によらずに慣性を一定に保つこと、すなわち、定慣性化制御が実現され、上述の高慣性化制御と同等の振動抑制効果が得られる。

この定慣性化制御と、第１実施形態の高慣性化制御は併用することが可能であり、式（１１）と式（１２）を融合すると、式（１３）に示すようになる。
ｋ_ＡＶ１＝[（２＋２ｃｏｓ（θ_２Ｌ））Δｍ＋２γ（１−ｃｏｓ（θ_Ｌ２））]ｎ_Ｇ１ （１３）
この式（１３）により、第１軸の高慣性化および定慣性化制御によるリンク角加速度のフィードバック制御ゲインを算出することができる。これが、図１０に示す高慣性化および定慣性化制御によるリンク角加速度のフィードバック定数算出部３０５Ａの機能である。

さらに、式（３）に示す慣性行列の非対角要素に注目すると、（１、２）成分と（２、１）成分についても、高慣性化および定慣性化制御によるリンク角加速度のフィードバック制御ゲインｋ_ＢＶ１、ｋ_ＢＶ２はそれぞれ、式（１４）、（１５）のように算出することができる。
ｋ_ＢＶ１＝[（１＋ｃｏｓ（θ_２Ｌ））Δｍ＋γ（１−ｃｏｓ（θ_Ｌ２））]ｎ_Ｇ１ （１４）
ｋ_ＢＶ２＝[（１＋ｃｏｓ（θ_２Ｌ））Δｍ＋γ（１−ｃｏｓ（θ_Ｌ２））]ｎ_Ｇ１ （１５）
これらｋ_ＢＶ１、ｋ_ＢＶ２を用いて、第１軸に第２軸の角加速度のフィードバック、第２軸に第１軸の角加速度のフィードバックを式（９）に加えると、式（１６）に示すようになる。

このようにして、一般化した高慣性化および定慣性化制御を加えた振動抑制制御系が得られる。図１０は、高慣性化および定慣性化制御を加えた振動抑制制御系全体のブロック図である。

（第３実施形態）
第３実施形態によるロボット制御装置のオブザーバを図１１に示す。この第３実施形態のオブザーバ２００Ａは、図５に示す第１実施形態のオブザーバ２００において、物理パラメータ切り替え部２０６を新たに設けた構成を有している。この第３実施形態のオブザーバ２００Ａは、先端負荷の変動とそれに伴う摩擦力の変動に対しては、図１１に示すように、物理パラメータ切り替え部２０６によって非線形動力学モデル２０３の物理パラメータセットの切り替え（ゲインスケジューリング）を行い、振動抑制性能がロボットの先端負荷および摩擦力の変動によらずにロバストになるようにする。物理パラメータセットを陽に切り替えるためには、例えば、“Ｐａｙｌｏａｄ（５ｋｇ）”のようなロボット言語を実装し、先端負荷が変化するタイミングで用いれば良い。なお、物理パラメータセットとしては、例えば質量、慣性モーメント、摩擦係数、バネ係数等が挙げられる。

図１１に示す例では、物理パラメータ切り替え部２０６は、先端負荷が５、４、３、２、１、０ｋｇ、つまり６通りの物理パラメータセットを必要とする。しかし、５ｋｇの物理パラメータセットさえ精度よく推定しておけば、上述した非線形最小２乗法による最適化の手法によって、先端負荷４ｋｇの物理パラメータセットを生成することができる。同じ要領で４ｋｇから３ｋｇ、３ｋｇから２ｋｇ、２ｋｇから１ｋｇ、２ｋｇから０ｋｇの物理パラメータセットを生成することができる。

（第４実施形態）
第４実施形態によるロボット制御装置のオブザーバを図１２に示す。この第４実施形態のオブザーバ２００Ｂは、図５に示す第１実施形態のオブザーバ２００において、物理パラメータ切り替え部２０６Ａを新たに設けた構成を有している。この第４実施形態のオブザーバ２００Ｂは、先端負荷の取り付け位置にオフセットがある場合を考慮する。この場合、先端負荷の慣性モーメントが大きく変化するので、図１２に示すように、物理パラメータ切り替え部２０６Ａは、オフセット量（例えば、０．０ｍ、０．１ｍ、０．２ｍの３種類など）を加味した物理パラメータのスケジューリングを行う。例えば、“Ｐａｙｌｏａｄ（５ｋｇ、０．１ｍ）”のようなロボット言語を実装し、先端負荷が変化するタイミングで用いれば良い。物理パラメータとしては、図１１に示す第３実施形態と同様に、例えば質量、慣性モーメント、摩擦係数、バネ係数等が挙げられる。

（第５実施形態）
第５実施形態によるロボット制御装置のオブザーバを図１３に示す。この第５実施形態のオブザーバ２００Ｃは、図５に示す第１実施形態のオブザーバ２００において、テーブル記憶部２０７と、ステップ応答比較部２０８と、物理パラメータ最適化部２０９とを新たに設けた構成を有している。この第５実施形態のオブザーバ２００Ｃは、先端負荷が５ｋｇ、４ｋｇ、３ｋｇ、２ｋｇ、１ｋｇ、０ｋｇといった数値からずれていても良い。図１３では、予め取得しておいた先端負荷の質量毎（５ｋｇ、４ｋｇ、３ｋｇ、２ｋｇ、１ｋｇ、０ｋｇ）の速度ステップ応答データと、その時の物理パラメータセットを格納するテーブル記憶部２０７が用意されている。

まず、未知の先端負荷を持つ実機の速度ステップ応答データを取得する。次に、取得した速度ステップ応答データと、テーブル記憶部２０７に格納された、先端負荷毎の速度ステップ応答データとをステップ応答データ比較部において比較する。そして、この比較結果に基づいて、実機の速度ステップ応答データに対応する先端負荷が、テーブル記憶部２０７に格納された先端負荷、例えば５ｋｇ、４ｋｇ、３ｋｇ、２ｋｇ、１ｋｇ、０ｋｇのうちから最も近い先端負荷を選択し、この選択した先端負荷に対応する物理パラメータセットを初期値として選択する。実機の速度ステップ応答データに対応する先端負荷に最も近い先端負荷の選択は、例えば、実機の速度ステップ応答データと、テーブル記憶部２０７に格納された各先端負荷の速度ステップ応答データとの差の２乗の和が最小となる先端負荷を選択する。

さらに、上記初期値として選択された物理パラメータセットを用いて物理パラメータ最適化部２０９によって最適化計算を行う。この最適化計算は、上述した非線形最小２乗法による手法と全く同様である。以上によって、未知の先端負荷に対しても、精度の良い物理パラメータセットが得られるので、精度の良いオブザーバを構築でき、効果的な振動抑制制御を供することができる。

以上述べたように、各実施形態によれば、多リンクロボットアーム先端の振動抑制制御技術において、モータ角速度からリンク角速度を推定するオブザーバのエンジニアリングコストを大きく削減することが可能となる。さらに、ロボットの先端負荷および摩擦力の変動があってもロバストな振動抑制性能を発揮させることができる。

既存の速度ＰＩ制御系を再チューニングしなくて良く、振動抑制制御や高慣性化制御などの付加フィードバックを切り離せば、元のＰＩ制御系に戻るという仕様は、現場のニーズに良くマッチしている。

本実施形態の非線形動特性モデルに基づくシミュレータを丸ごと制御装置に内蔵するという構成は、時間軸がロボットの１００倍以上のプロセス制御の分野で用いられることがある。しかし、この方法は未来の（液面や圧力、温度などの）セットポイント制御を省エネルギーなどの観点から最適に行うことを目的としていて、ロボットなどリアルタイムでの目標値追従性能を出すためのサーボ制御には適さない。

特許文献１などに記載された手法は、非線形動力学モデルにおいて何らかの近似に基づいたオブザーバを用いているため、チャンピオンデータは出せても、広い動作範囲でロバストな振動制御性能は得られない。また、演算量削減やオブザーバゲインの調整など、エンジニアリングコストがかさむ。

その他、モータドライブメーカーも振動抑制制御方式やその適応制御およびオートチューニング方式など、多くの提案がされている。しかし、モータの負荷として考えた１リンク毎の弾性関節モデル＋外乱推定とした汎用的なオブザーバに基づいているため、多リンクロボットアームの動力学モデルを陽に考慮した方式からは性能が劣ることになるのは当然である。

これに対し、本実施形態では、非線形動力学モデルにも基づく近似のないオブザーバを用いており、先端負荷および摩擦力の変動に対応した物理パラメータのゲインスケジューリングも行っているので、広い動作範囲でロバストな振動制御性能が得られる。さらに、先端負荷変動に伴う摩擦力の変動も考慮に入れており、質量や慣性に加えて摩擦係数もセットでゲインスケジューリングする。また、オブザーバゲインの設計が不要なので、エンジニアリングコストがかからないという大きな利点を有する。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これらの実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これらの実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

１…架台、２…第１軸、３…第１リンク、４…第１モータ、５…第１減速機、６…第１エンコーダ、７…第２軸、８…第２リンク、９…第２モータ、１０…第２減速機、１１…第２エンコーダ、１２…先端負荷、１３…制御装置、２００…オブザーバ、２０１、２０２…ＰＩ制御器、２０３…２リンクアーム非線形動力学モデル、２０４ａ、２０４ｂ、２０４ｃ、２０４ｄ、２０５ａ、２０５ｂ、２０５ｃ、２０５ｄ…積分器、３０１、３０２、３０３、３０４…状態フィードバック部。

Claims (6)

1. モータの回転軸とリンクの回転軸との間に弾性機構を有するロボットアームを制御対象とし、前記モータの角速度を比例積分制御して前記モータへ電流指令値を出力する角速度制御系を有するロボット制御装置であって、
   前記モータの角速度と前記電流指令値とを入力とし、前記ロボットアームの非線形動力学モデルを持ち、前記角速度制御系と等価なゲインを用いて比例積分制御される前記モータの角速度制御系のシミュレーションモデルから前記リンクの角加速度と、前記リンクの角速度と、前記モータの角速度とを推定するオブザーバと、
   前記オブザーバによって推定された前記リンクの角速度と、前記オブザーバによって推定された前記モータの角速度との差から軸ねじり角速度を算出し、前記角速度制御系にフィードバックする第１フィードバック部と、
   前記オブザーバによって推定された前記リンクの角加速度を前記角速度制御系にフィードバックする第２フィードバック部と、
   前記第２フィードバック部において、前記非線形動力学モデルで先端負荷が低慣性の場合に先端負荷質量を補償して高慣性化する第１フィードバック定数算出部と、
   を備えたロボット制御装置。
2. 前記第２フィードバック部において、前記ロボットアームの姿勢変化で生じる慣性の低下を補償して前記姿勢変化に依らず定慣性化する第２フィードバック定数算出部を更に備えた請求項１記載のロボット制御装置。
3. 前記非線形動力学モデルに設定する慣性モーメント、摩擦係数およびバネ係数を含む物理パラメータセットを前記ロボットアームの先端負荷の質量の区分によって切り替える物理パラメータ切り替え部を更に備えた請求項１または２記載のロボット制御装置。
4. 前記非線形動力学モデルに設定する慣性モーメント、摩擦係数およびバネ係数を含む物理パラメータセットを、前記ロボットアームの先端負荷の質量の区分と、前記先端負荷の位置のオフセット量の区分とに基づいて切り替える物理パラメータ切り替え部を更に備えた請求項１乃至３のいずれかに記載のロボット制御装置。
5. 前記物理パラメータ切り替え部は、
   予め用意しておいた先端負荷の質量毎の速度ステップ応答データと、前記速度ステップ応答データを得た時の前記物理パラメータとを格納するテーブル記憶部と、
   前記ロボットアームの速度ステップ応答データと、前記テーブル記憶部に格納された前記速度ステップ応答データとを比較し、この比較結果に基づいて前記物理パラメータの初期値を選択するステップ応答データ比較部と、
   前記ロボットアームの速度ステップ応答データを取得して、この取得した速度ステップ応答データと、前記テーブル記憶部に格納された、先端負荷の質量毎の前記速度ステップ応答データとを比較し、この比較結果に基づいて物理パラメータの初期値を推定し、この推定した物理パラメータを初期値として用いて前記ロボットのシミュレーションステップ応答波形が前記ロボットの実ステップ応答波形に合うように非線形最適化計算を行い、物理パラメータの最適化を行う物理パラメータ最適化部と、
   を更に備えた請求項１乃至４のいずれかに記載のロボット制御装置。
6. 前記物理パラメータ最適化部は、前記テーブル記憶部に格納された先端負荷のなかから、前記取得した速度ステップ応答データと前記先端負荷の質量毎の前記速度ステップ応答データとの差の２乗の和が最小となる先端負荷を選択し、この選択した先端負荷に対応する物理パラメータを前記物理パラメータの初期値として推定する請求項５記載のロボット制御装置。