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

Abstract

【課題】簡易な構成でロボットアーム先端の振動抑制効果を高めること。
【解決手段】本実施形態のロボット制御装置は、弾性機構を有するロボットアームを制御対象とし、モータの角速度を比例積分制御してモータへ電流指令値を出力する角速度制御系を有し、ロボットアームの非線形動力学モデルに設定する慣性モーメント、摩擦係数およびバネ係数を含む物理パラメータセットをロボットアームの手先負荷の質量の区分によって切り替える物理パラメータ切替部２０４と、モータの角速度と電流指令値とを入力とし、非線形動力学モデル２０３を持ち、角速度制御系と等価なゲイン比例積分制御されるモータ角速度制御系のシミュレーションモデルからリンクの角速度を推定するオブザーバ部２００と、モータの角速度とリンクの角速度との差から軸ねじり角速度を算出し、角速度制御系にフィードバックする状態フィードバック部３０１，３０２とを有する。
【選択図】図３

Description

この発明の実施形態は、例えば、多リンクロボットアームを制御するロボット制御装置に関する。

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

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

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

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

小林，他：ロボット制御の実際，第３章ロボットの同定，コロナ社，１９９７年，ｐ．６２−８５ 杉本，他：ＡＣサーボシステムの理論と設計の実際，第７章速度制御系および位置制御系の設計，総合電子出版社，１９９０年，ｐ．１５３−１７９

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

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

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

本実施形態に係るロボット制御装置の一態様は、モータの回転軸とリンクの回転軸との間に弾性機構を有するロボットアームを制御対象とし、前記モータの角速度を比例積分制御して前記モータへ電流指令値を出力する角速度制御系を有するロボット制御装置であって、前記ロボットアームの非線形動力学モデルに設定する慣性モーメント、摩擦係数およびバネ係数を含む物理パラメータセットを前記ロボットアームの手先負荷の質量の区分によって切り替える物理パラメータ切替部と、前記モータの角速度と前記電流指令値とを入力とし、前記非線形動力学モデルを持ち、前記角速度制御系と等価なゲイン比例積分制御されるモータ角速度制御系のシミュレーションモデルから前記リンクの角速度を推定するオブザーバ部と、前記モータの角速度と前記リンクの角速度との差から軸ねじり角速度を算出し、前記角速度制御系にフィードバックする状態フィードバック部とを具備する。

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

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

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

図２は、このロボットアームの１リンク分を示す模式図である。図１及び図２を参照して、このロボットアームは、弾性関節を持つシリアル２リンクアームとして非線形動力学モデル化できる。図２に１リンク分の非線形動力学モデルの記述に必要な物理パラメータ（慣性モーメント、摩擦係数およびバネ係数など）を第１軸を代表として示す。弾性関節を持つシリアル２リンクアームの非線形動力学モデルは、次のようなモータ側とリンク側の２組の式（１）で与えられる。

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

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

モータ角速度制御系については、ＰＩ（比例・積分）制御をＦＦ−Ｉ−Ｐ（フィードフォワード−積分−比例）制御として２自由度ＰＩ制御にした式（４）を基本構成とする。

この２自由度ＰＩ速度制御系は、図９に示すように、位置制御系（Ｐ制御）の内側にカスケード接続された制御系として構成される。本実施形態では位置制御系を除いたモータ角速度制御系に焦点を当て、制御周期が十分短いものとして連続系のまま説明を行う。

例えば、図１の２リンクアームの手先負荷１２を５ｋｇとし、式（１）、（２）、（３）に現れる物理パラメータ（以下、基底パラメータも含む）を非特許文献１やロボットモデルの関節剛性を考慮した特許文献３などの同定法で推定しておく。これらの物理パラメータを式（４）で使用しているモータ速度制御ゲインと共に、動力学モデルの式（１）、（２）と速度フィードバック制御則の式（３）に代入することによって、シリアル２リンクアームの速度制御系の時間応答シミュレータを構築し、ロボット実機の時間応答と良く合うかどうかを確認する。上記の同定法によって物理パラメータが精度よく推定されていれば、これらの時間応答は良く合うはずである。もし、少しずれているようであれば、上記シミュレータを用いた非線形最小２乗法（マルカート法など）による最適化計算を用いて、物理パラメータをファインチューニングする。例えば、図１０（ａ）に最適化前、図１０（ｂ）に最適化後の角速度ステップ応答を示し、横軸を時間、縦軸を角速度とし、実線はロボット実機、破線はシミュレータを示す。図１０（ｂ）に示すように、物理パラメータを最適化することで、シミュレータとロボット実機の角速度ステップ応答を良く合わせることができる。

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

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

しかし、実際のところ種々のノイズやモデル化誤差があるオブザーバでは、そのようなハイゲインにはできない。実機の応答を見ながらオブザーバの極配置を調整すると、結局、状態フィードバックの極配置に近くなることもある。ここでカルマンフィルタを用いれば、オブザーバの設計をシステマティックに行えるが、仮定するノイズモデルのパラメータ設定が必要である。さらに、オブザーバと状態フィードバックという２階建てを無くしたＨ∞制御（Ｈ無限大制御）などのロバスト制御では、陽にノイズやモデル化誤差を考慮した制御系設計ができる。しかし、いずれの方法にせよ、ＣＡＥ（Computer-Aided Engineering）ツールが不可欠なので現場での手調整は難しい。

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

（実施例１）
図３は、実施例１のオブザーバを示すブロック図である。オブザーバ部２００は、ＰＩ制御器２０１，２０２と、２リンクアームの非線形動特性モデル２０３と、物理パラメータ切替部２０４と、非線形動力学モデル２０３を２回積分する積分器群２０５，２０６とを有する。すなわち、図３に示すオブザーバ部２００は、オブザーバ部２００は、ロボットアームの非線形動特性モデル２０３を丸ごと内蔵し、ＰＩ制御器２０１，２０２のオブザーバゲインはロボットアーム各軸の既存の速度制御系のＰＩ制御ゲインをそのまま使用する、というシミュレータそのものである。また、積分制御が入っているので出力推定値の定常偏差を零にする機能も内蔵されている。そして、オブザーバ実装における近似やゲイン調整にかかるエンジニアリングコストが全くない。

この非線形動力学モデルに基づくＰＩ制御型オブザーバは、式（１）、（２）、（３）を変形した式（５）のような２階微分形式で表される。なお、式（５）において、＾は推定値を表す。

ここでＰＩ制御系２０１，２０２のオブザーバゲインは、実機の速度ループのＦＦ−Ｉ−Ｐ制御（２自由度ＰＩ制御）のＰとＩのゲインと同じものを選んであり、この場合、オブザーバの追従性能の１自由度だけを考えれば良いので、ＦＦ＝ＰのようにＰＩ制御として設定する。このシステムは、ゲインが一定の非線形オブザーバの一種である。また、積分制御が入っているので出力推定値の定常偏差は零になり、外乱除去オブザーバとしての機能も内蔵されている。式（５）をロボット制御装置１３に実装し、リアルタイムで図３の積分器群２０５，２０６のように２回積分（実際には数値積分）することによって状態推定ができる。

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

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

後述のシミュレーションや実験で明らかなように、この軸ねじれ角速度推定値のフィードバック制御ゲインｋ_ＴＶｉは、時間応答波形を見ながらの手調整が容易である。また、弾性関節を持つロボットアームの正確な非線形動力学モデルに基づいたオブザーバを用いているので、シリアルリンクで構成されるロボットアームの振動抑制制御では、第１軸だけの状態フィードバックだけでも十分な効果がある。

手先負荷変動とそれに伴う摩擦力の変動に対しては、図３に示すように、物理パラメータ切替部２０４によって非線形動力学モデル２０３の物理パラメータセットを切り替え（ゲインスケジューリング）を行い、振動抑制性能がロボットの手先負荷・摩擦力の変動によらずにロバストになるようにする。物理パラメータセットを陽に切り替えるためには、例えば、“Payload(5kg)”のようなロボット言語を実装し、手先負荷が変化するタイミングで用いれば良い。

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

図５の左側に、手先負荷５ｋｇの場合の軸ねじれフィードバック制御の有無による第１軸の速度ステップ応答波形の比較を示す。図５（ａ）がリンク角速度、図５（ｂ）がモータ角速度である。本実施形態の場合（オブザーバあり）は、ＦＦ−Ｉ−Ｐ制御だけの場合（オブザーバなし）に比べて、リンク角速度の応答の改善が著しいことがわかる。

図５の右側に、手先負荷２ｋｇの場合の軸ねじれフィードバック制御の有無による第１軸の速度ステップ応答波形の比較を示す。図５（ｃ）がリンク角速度、図５（ｄ）がモータ角速度である。手先負荷に応じて、物理パラメータセットを切り替えているので、５ｋｇの場合と同様、本実施形態の場合（オブザーバあり）は、ＦＦ−Ｉ−Ｐ制御だけの場合（オブザーバなし）に比べて、リンク角速度の応答の改善が著しいことがわかる。

（実施例２）
図６に、実施例２のオブザーバを示す。実施例２のオブザーバ部２００Ａでは、手先負荷の取り付け位置にオフセットがある場合を考慮する。この場合、手先負荷の慣性モーメントが大きく変化するので、図６のように、物理パラメータ切替部２０４Ａは、オフセット量（例えば、０．０ｍ，０．１ｍ，０．２ｍの３種類など）を加味した物理パラメータのスケジューリングを行う。例えば、“Payload(5kg,0.1m)”のようなロボット言語を実装し、手先負荷が変化するタイミングで用いれば良い。

（実施例３）
図７に、実施例３のオブザーバを示す。実施例３のオブザーバ部２００Ｂは、上記実施例１のオブザーバ部２００を応用したものである。物理パラメータ切替部２０４Ｂは、重力がかかる軸を備えたロボットアームを対象にし、手先負荷情報を自動的に取得する。図７のように、ロボットが静止する毎に、アーム先端の上下軸（スカラロボットの第３軸など）の重力保持トルクを利用して手先負荷変動を計測して得る。例えば、“EnableAutoPayload”、“DisableAutoPayload”のようなロボット言語を実装し、自動的に物理パラメータセットを切り替えるか否かを行うようにしてもよい。

（実施例４）
図８に、実施例４のオブザーバを示す。実施例４のオブザーバ部２００Ｃは、上記実施例２のオブザーバ部２００Ａを応用したものである。手先負荷の取り付けにオフセットがある場合慣性モーメントが大きく変化するが、重力保持トルクだけでは手先負荷情報が得られない。そこで図８のように、物理パラメータ切替部２０４Ｃは、同じ手先負荷の質量で、オフセットの異なる場合（例えば、０．０ｍ，０．１ｍ，０．２ｍの３種類など）の物理パラメータ群を用意しておき、複数の非線形動力学モデル２０３を並行動作させる。状態フィードバック部３０１，３０２は、オブザーバのＰＩ制御器２０１，２０２の積分器出力（この値があるということは動力学モデルに誤差があることを示している）の絶対値が小さいモデルを選択して、状態フィードバックに使用する。

ここで、状態フィードバック部３０１，３０２は、積分器出力が似たような値になってモデルを１つに絞れない場合は、複数のモデルに基づくオブザーバの軸ねじれ角速度推定値に重みをつけてフィードバックすることが可能である。

（実施例５）
上記摩擦力は、手先負荷によって変動するが、温度によっても変動する。実施例５では、例えば、外気温度０℃、１０℃、２０℃、３０℃の環境で物理パラメータセットを推定しておき、ロボットアーム付近に設置した温度計の値によって、切り替えるようにする。上記の手先負荷やそのオフセット量の切り替えにも同時に対応するためには、手先負荷、オフセット量および温度による３次元テーブルを作成すれば良い。

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

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

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

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

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

なお、この発明は、上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合せにより種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態に亘る構成要素を適宜組み合せてもよい。

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

Claims (6)

1. モータの回転軸とリンクの回転軸との間に弾性機構を有するロボットアームを制御対象とし、前記モータの角速度を比例積分制御して前記モータへ電流指令値を出力する角速度制御系を有するロボット制御装置であって、
   前記ロボットアームの非線形動力学モデルに設定する慣性モーメント、摩擦係数およびバネ係数を含む物理パラメータセットを前記ロボットアームの手先負荷の質量の区分によって切り替える物理パラメータ切替部と、
   前記モータの角速度と前記電流指令値とを入力とし、前記非線形動力学モデルを持ち、前記角速度制御系と等価なゲイン比例積分制御されるモータ角速度制御系のシミュレーションモデルから前記リンクの角速度を推定するオブザーバ部と、
   前記モータの角速度と前記リンクの角速度との差から軸ねじり角速度を算出し、前記角速度制御系にフィードバックする状態フィードバック部と
   を具備することを特徴とするロボット制御装置。
2. 前記物理パラメータ切替部は、前記手先負荷の質量の区分と手先負荷位置のオフセット量の区分とによって前記物理パラメータセットを切り替えることを特徴とする請求項１に記載のロボット制御装置。
3. 前記モータの重力保持トルクを計測するトルク計測部をさらに具備し、
   前記物理パラメータ切替部は、前記トルク計測部の計測値にしたがって前記物理パラメータセットを切り替えることを特徴とする請求項１に記載のロボット制御装置。
4. 前記オブザーバ部は、前記オフセット量の区分に対応する複数の非線形動力学モデルを動作させ、
   前記状態フィードバック部は、前記比例積分制御の積分器出力が小さい非線形動力学モデルを選択して前記角速度制御系にフィードバックすることを特徴とする請求項２に記載のロボット制御装置。
5. 前記状態フィードバック部は、前記複数の非線形動力学モデルに基づく前記軸ねじり角速度に重みをつけてフィードバックすることを特徴とする請求項４に記載のロボット制御装置。
6. 前記ロボットアームの環境温度を計測する温度計測部をさらに具備し、
   前記物理パラメータ切替部は、さらに前記温度計測部により計測された温度によって前記物理パラメータセットを切り替えることを特徴とする請求項１乃至５のいずれか１項に記載のロボット制御装置。

Images