JP2014117787A

JP2014117787A - 制御装置

Info

Publication number: JP2014117787A
Application number: JP2012276389A
Authority: JP
Inventors: Hiroto Sakahara; 洋人坂原
Original assignee: Daihen Corp
Current assignee: Daihen Corp
Priority date: 2012-12-19
Filing date: 2012-12-19
Publication date: 2014-06-30

Abstract

【課題】外乱トルクを推定する際に、ノイズを適切に除去できない。
【解決手段】モータの指令トルクを生成する指令トルク生成部１１、モータの速度を用いて推定トルクを算出する推定トルク算出部１２、推定トルクとモータに入力される指令トルクの差である基本推定外乱トルクを算出する外乱トルク算出部１３、基本推定外乱トルク、モータの速度、指令トルクを用いてフィルタ処理を行い、ノイズを除去した最終推定外乱トルクを生成するフィルタ部１４、基本推定外乱トルクとモータの速度を用いてフィルタ処理のパラメータを更新する更新部１５、生成された指令トルクから最終推定外乱トルク成分を補償した指令トルクを生成し、モータに出力する外乱補償部１６を備える。フィルタ処理のパラメータを更新することによって、マニピュレータ２の動作状況に応じたフィルタを構成でき、ノイズを適切に除去することができる。
【選択図】図１

Description

本発明は、推定された外乱トルクからノイズを除去するフィルタを備えた制御装置に関する。

複数のアームを有するマニピュレータの制御時には、アームの自重、搬送対象の重量、関節での摩擦等に起因する種々の外乱が発生する。そのため、例えば、特許文献１には、外乱トルクを算出し、その算出した外乱トルクを指令トルクから引くことで外乱を補償し、高い精度の軌道制御を行う手法が記載されている。また、外乱のうち、特に摩擦を補償する方法として、特許文献２には、速度反転判別器と位置指令の微分値により、摩擦補償の有無と向きを決定する方法であって、動作に合わせて摩擦を補償する方法が記載されている。

特開平８−５２６７５号公報特開平１１−１４３５４８号公報

しかしながら、特許文献１の方法では、速度により推定されるトルクと、電流値から計測されるトルクの差分を一次遅れフィルタに通すことで外乱トルクを算出しているが、その一次遅れフィルタが外乱の推定確度を下げてしまうことになる。具体的には、遅れ時定数を小さくすることで遅れ時間が小さくなるが、ノイズが多くなるため精度が下がり、適切な補償ができない場合がある。一方、精度を上げるためには、遅れ時定数を大きくする必要があり、それだけフィードバックのタイムラグが大きくなるという問題があった。

また、特許文献２の方法では、動摩擦補償の方向と有無について考慮しているが、補償量については別途考慮する必要がある。そのため、同じ現象を２個のモジュールで取り扱うことで、動摩擦補償の方向と補償量との不一致が起こる可能性がある。
一般的に言えば、アームの自重や摩擦等に起因する外乱を補償する際に、より精度の高い外乱の推定を行いたいという要望があった。

本発明は、上記事情に応じてなされたものであり、推定された外乱トルクから適切にノイズを除去することができる制御装置を提供することを目的とする。

上記目的を達成するため、本発明による制御装置は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータを制御する制御装置であって、モータの指令トルクを生成する指令トルク生成部と、モータの動作情報を用いて推定トルクを算出する推定トルク算出部と、推定トルクと、モータに入力される指令トルクとの差に応じた基本推定外乱トルクを算出する外乱トルク算出部と、基本推定外乱トルクを少なくとも用いてフィルタ処理を行うことにより、ノイズを除去した最終推定外乱トルクを生成するフィルタ部と、基本推定外乱トルクを少なくとも用いてフィルタ処理のパラメータを更新する更新部と、指令トルク生成部が生成した指令トルクから、フィルタ部が生成した最終推定外乱トルク成分を補償した指令トルクを生成し、モータに出力する外乱補償部と、を備えたものである。
このような構成により、更新部によってフィルタ処理のパラメータが更新され、マニピュレータの動作状況に応じた適切なパラメータをフィルタ部に設定することができる。その結果、ノイズが適切に除去された精度の高い最終推定外乱トルクを生成することができ、その最終推定外乱トルクを用いることによって、マニピュレータをより高精度に動作させることができる。

また、本発明による制御装置では、フィルタ部は、基本推定外乱トルクと、モータの速度と、指令トルク生成部が生成した指令トルクとを用いて時間発展モデルのフィルタ処理を行い、更新部は、モータの速度と、基本推定外乱トルクとを用いて、速度と外乱トルクとの関係を示す関数のパラメータを更新してもよい。
このような構成により、時間発展モデルのフィルタを適切に更新することができ、精度の高い最終推定外乱トルクの生成が可能となる。

また、本発明による制御装置では、更新部は、速度と外乱トルクとの関係を示す関数を決定できるだけのモータの速度と速度に応じた基本推定外乱トルクとの組が取得された場合に、フィルタ処理のパラメータを更新してもよい。
このような構成により、フィルタのパラメータが適宜、更新されることになる。その結果、マニピュレータの動作状況に応じた適切なフィルタを構成することができる。

本発明による制御装置によれば、ノイズの除去された精度の高い最終推定外乱トルクの生成が可能となる。その結果、マニピュレータの精度の高い制御が可能となる。

本発明の実施の形態１による産業用ロボットの構成を示す図同実施の形態における速度と外乱トルクとの関係の一例を示す図同実施の形態による制御装置の動作を示すフローチャート

以下、本発明による制御装置について、実施の形態を用いて説明する。なお、以下の実施の形態において、同じ符号を付した構成要素及びステップは同一または相当するものであり、再度の説明を省略することがある。

（実施の形態１）
本発明の実施の形態１によるマニピュレータの制御装置について、図面を参照しながら説明する。本実施の形態によるマニピュレータの制御装置は、フィルタのパラメータを適宜、更新することによって、精度の高い最終推定外乱トルクの生成を行うものである。

図１は、本実施の形態による産業用ロボット１００の構成を示すブロック図である。本実施の形態による産業用ロボット１００は、制御装置１と、マニピュレータ２とを備える。制御装置１は、モータによって駆動される関節によって連結された複数のアームを有するマニピュレータ２を制御する。そのマニピュレータ２において、各モータとアームとは減速機を介して接続されている。また、直列に接続されたアームの先端に、手先効果器（エンドエフェクタ）が設けられていてもよい。マニピュレータ２は、例えば、垂直多関節ロボットのマニピュレータであってもよく、水平多関節ロボットのマニピュレータであってもよい。また、そのマニピュレータ２の軸数は問わない。マニピュレータ２の軸数は、例えば、４軸、５軸、６軸であってもよく、または、その他の軸数であってもよい。また、制御装置１は、ティーチングプレイバック方式によりマニピュレータ２を制御してもよく、または、自律方式によりマニピュレータ２を制御してもよい。本実施の形態による産業用ロボット１００は、例えば、搬送ロボットであってもよく、溶接ロボットであってもよく、組立ロボットであってもよく、塗装ロボットであってもよく、あるいは、その他の用途のロボットであってもよい。

図１で示されるように、制御装置１は、指令トルク生成部１１と、推定トルク算出部１２と、外乱トルク算出部１３と、フィルタ部１４と、更新部１５と、外乱補償部１６とを備える。

指令トルク生成部１１は、マニピュレータ２が有するモータの指令トルクを生成する。指令トルク生成部１１は、例えば、教示情報等を用いることによって、モータの位置を算出し、モータがその位置となるように、指令トルクを生成してもよい。また、指令トルク生成部１１は、図示しない他の構成要素からモータの位置指令を受け取り、モータがその指令に応じた位置となるように、指令トルクを生成してもよい。また、指令トルク生成部１１は、図示しない他の構成要素からモータの速度指令や電流指令を受け取り、モータがそれらの指令に応じた速度や電流となるように、指令トルクを生成してもよい。なお、指令トルク生成部１１は、これらの処理を行う際に、モータの現在位置や、現在速度、現在電流等のフィードバックを受け取り、フィードバック制御を行うことによって、指令トルクを生成してもよい。生成された指令トルクは、フィルタ部１４と、外乱補償部１６とに入力される。

推定トルク算出部１２は、マニピュレータ２からモータの動作情報を受け取り、そのモータの動作情報を用いて推定トルクを算出する。そのモータの動作情報は、モータの現在速度、モータの現在電流、モータのトルク指令の少なくとも１以上のものである。なお、本実施の形態では、モータの動作情報がモータの現在速度である場合について主に説明する。そのモータの速度は、マニピュレータ２においてエンコーダ等を用いて取得された位置を時間微分したものであり、厳密に言えば、角速度である。推定トルク算出部１２は、例えば、バネマス系程度の簡単なモデルを用いて推定トルクを算出してもよい。なお、推定トルク算出部１２が、これら以外の方法を用いて推定トルクを算出してもよいことは言うまでもない。算出された推定トルクは、外乱トルク算出部１３に入力される。

外乱トルク算出部１３は、推定トルク算出部１２が算出した推定トルクと、マニピュレータ２のモータに入力される指令トルクとの差に応じた基本推定外乱トルクを算出する。なお、基本推定外乱トルクとは、ノイズの除去前の推定外乱トルクのことであり、後述する最終推定外乱トルクとは、ノイズの除去後の推定外乱トルクのことである。外乱トルク算出部１３が算出する基本推定外乱トルクは、推定トルクと指令トルクとの差であってもよく、または、その差と正の相関を有する値であってもよい。基本推定外乱トルクが推定トルクと指令トルクとの差である場合には、外乱トルク算出部１３は、その差を算出する減算器であってもよい。なお、この基本推定外乱トルクの算出で用いられる指令トルクは、外乱トルクの補償後の指令トルク、すなわち外乱補償部１６が出力した指令トルクであることが好適であるが、外乱トルクの補償前の指令トルク、すなわち、指令トルク生成部１１が出力した指令トルクであってもよい。算出された基本推定外乱トルクは、フィルタ部１４と、更新部１５とに渡される。

フィルタ部１４は、外乱トルク算出部１３が算出した基本推定外乱トルクを少なくとも用いてフィルタ処理を行うことにより、ノイズを除去した最終推定外乱トルクを生成する。なお、このフィルタ処理の種類は問わない。このフィルタ処理は、例えば、特許文献１に記載の一次遅れフィルタ処理であってもよく、時間発展モデルのフィルタ処理であってもよく、または、ノイズを除去可能なその他のフィルタ処理であってもよい。なお、一次遅れフィルタ処理を行う場合には、フィルタ部１４は、基本推定外乱トルクの入力を受け付け、その基本推定外乱トルクを用いてフィルタ処理を行い、ノイズを除去した最終推定外乱トルクを生成する。一方、時間発展モデルのフィルタ処理を行う場合には、フィルタ部１４は、基本推定外乱トルクと、モータの速度と、指令トルク生成部１１が生成した指令トルクとの入力を受け付け、それらを用いて時間発展モデルのフィルタ処理を行い、ノイズを除去した最終推定外乱トルクを生成する。なお、時間発展モデルのフィルタ処理は、例えば、カルマンフィルタの処理であってもよく、パーティクルフィルタの処理であってもよい。なお、そのカルマンフィルタは、通常、線形のカルマンフィルタであるが、非線形のカルマンフィルタであってもよい。非線形のカルマンフィルタは、例えば、拡張カルマンフィルタであってもよく、Ｕｎｓｃｅｎｔｅｄカルマンフィルタであってもよい。本実施の形態では、フィルタ部１４が、カルマンフィルタの処理を行う場合について主に説明する。また、カルマンフィルタの時間発展モデルや観測モデルについては後述する。ノイズの除去された最終推定外乱トルクは、外乱補償部１６に入力される。

更新部１５は、基本推定外乱トルクを少なくとも用いてフィルタ処理のパラメータを更新する。フィルタ部１４が、一次遅れフィルタである場合には、更新部１５は、基本推定外乱トルクを用いてパラメータである遅れ時定数を更新する。一方、フィルタ部１４が、時間発展モデルのフィルタである場合には、更新部１５は、モータの速度と、基本推定外乱トルクとを用いて、速度と外乱トルクとの関係を示す関数のパラメータを更新する。なお、フィルタ部１４が一次遅れフィルタである場合には、更新部１５は、基本推定外乱トルクが所定の条件をみたす場合に、フィルタ処理のパラメータを更新してもよい。フィルタ部１４が、時間発展モデルのフィルタである場合には、更新部１５は、速度と外乱トルクとの関係を示す関数を決定できるだけのモータの速度と、その速度に応じた基本推定外乱トルクとの組が取得された場合に、フィルタ処理のパラメータを更新してもよい。なお、パラメータの更新については、後述する。

外乱補償部１６は、指令トルク生成部１１が生成した指令トルクから、フィルタ部１４が生成した最終推定外乱トルク成分を補償した指令トルクを生成し、モータに出力する。ここで、指令トルクから最終推定外乱トルク成分を補償するとは、その補償後の指令トルクがモータに入力され、さらに外乱が加わることによって、その外乱が相殺されるようにすることである。その結果として、モータは、指令トルク生成部１１が生成した指令トルクで指示されるように動作することになる。この最終推定外乱トルクの補償は、指令トルクから、最終推定外乱トルクを減算することによって行われてもよい。その場合には、外乱補償部１６は、その差を算出する減算器であってもよい。外乱の補償された指令トルクは、マニピュレータ２のモータと、外乱トルク算出部１３とに入力される。

なお、図１では、マニピュレータ２に一の指令トルクを出力し、マニピュレータ２から一の速度を受け取る場合について示しているが、これは説明の便宜のためであり、制御装置１は、マニピュレータ２が有する軸数、すなわちモータの数だけ指令トルクを出力し、その数だけ速度を受け取ってもよい。すなわち、制御装置１の処理は、モータごとに行われてもよい。

ここで、フィルタ部１４が用いるフィルタ処理として、（１）カルマンフィルタの例１、（２）カルマンフィルタの例２、（３）一次遅れフィルタの例について説明する。

（１）カルマンフィルタの例１
カルマンフィルタの時間発展モデルと、観測モデルとについて説明する。ここでは、システム時間発展モデルの例として、動摩擦を利用する。また、カルマンフィルタの内部パラメータとして、外乱トルクτと、モータの速度ｖと、モータの加速度αとを使用する。以下、τ_ｋは、時刻ｋにおける基本推定外乱トルクであり、ｖ_ｋは、時刻ｋにおける角速度であり、α_ｋは、時刻ｋにおける角加速度であり、ｕ_ｋは、時刻ｋにおける指令トルクであり、ｄｔは、更新間隔、すなわち、時刻ｋと、時刻ｋ＋１との時間間隔を示すものとする。

速度ｖと、外乱トルクτとの関係は、例えば、図２のように示され、次式のように表すことができる。なお、この式において、ｇ_ａは、速度が０の近傍である場合の動摩擦の傾きに関係するパラメータであり、ｇ_ｉは、速度の絶対値が大きい場合の動摩擦の傾きであり、ｇ_０は、速度が０の近傍である場合の動摩擦の大きさであり、ｇ_ｂは、アームの自重や搬送対象の重量等に応じたオフセット値である。なお、以下の式は、速度の絶対値の大きい場合に傾きがｇ_ｉとなり、速度の０近傍で傾きがｇ_ｉに応じたものとなり、両傾きの切り替わりが２ｇ_０程度の幅を持っており、オフセットがｇ_ｂとなるように、関数を選択したものである。

この外乱トルクと速度との関係式を用いると、時刻ｋにおけるτ_ｋ、ｖ_ｋ、α_ｋは、次のようになる。なお、Ｍは、慣性モーメントである。

したがって、時刻ｋの状態ベクトルｘ_ｋを、

とすると、状態方程式は、次式のようになる。

ただし、ヤコビアン（状態遷移行列）Ｆ_ｋは、次式の通りであり、Ｒ_ｋは、システム誤差（システム雑音）である。このシステム誤差Ｒ_ｋは、任意に設定してもよい。

また、時刻ｋの観測ベクトルｚ_ｋを、

とすると、観測方程式は、次式のようになる。

ただし、ヤコビアン（観測行列）Ｈ_ｋは、次式の通りであり、Ｑ_ｋは、観測誤差（観測雑音）である。この観測誤差Ｑ_ｋは、任意に設定してもよい。

フィルタ部１４は、このようなシステムを有するカルマンフィルタを使用することで、指令値ｕ_ｋと観測値ｚ_ｋとを利用して、ノイズの除去されたτ_ｋ、ｖ_ｋ、α_ｋを推定できる。フィルタ部１４は、その推定値のうち、τ_ｋを最終推定外乱トルクとして出力する。なお、カルマンフィルタは公知のアルゴリズムであるため、その詳細な説明を省略する。

なお、この場合には、ｇ_ａ、ｇ_ｉ、ｇ_０がフィルタ処理のパラメータである。したがって、更新部１５は、角速度ｖと、基本推定外乱トルクτとの組を順次、図示しない記録媒体に蓄積する。その蓄積のタイミングは、例えば、更新間隔ｄｔごとであってもよい。そして、更新部１５は、速度と外乱トルクとの関係を示す関数を決定できるだけの組が取得された場合に、それらの組を用いて、最小二乗法等により、パラメータｇ_ａ、ｇ_ｉ、ｇ_０、ｇ_ｂを決定して、フィルタ部１４のパラメータｇ_ａ、ｇ_ｉ、ｇ_０を更新する。フィルタ部１４は、それらのパラメータが更新されると、その更新後のパラメータを用いて、カルマンフィルタの処理を行う。なお、最小二乗法は計算量が多いため、速度の０近傍と、傾きがｇ_ｉとなる位置とにおいて線形近似を行い、各パラメータの近似値を算出してもよい。

更新部１５は、例えば、速度があらかじめ決められた値−ｖ_ｈからｖ_ｈまで変化したときに（ｖ_ｈは正または負の実数である）、速度と外乱トルクとの関係を示す関数を決定できるだけの組が取得されたと判断してもよい。また、更新部１５は、例えば、速度があらかじめ決められた値−ｖ_ｈからｖ_ｈ／Ａまで変化したとき、または、その逆のときに、速度と外乱トルクとの関係を示す関数を決定できるだけの組が取得されたと判断してもよい。ここで、Ａは、１より大きい実数であり、大きすぎない値であることが好適である。また、更新部１５は、モータにおいて回転方向の変化が発生した場合、例えば、モータが正回転から逆回転になった場合、またはその逆の場合に、速度と外乱トルクとの関係を示す関数を決定できるだけの組が取得されたと判断してもよい。また、更新部１５は、その他のタイミングでパラメータを更新してもよい。例えば、ティーチングプレイバック方式の制御を行う場合には、更新部１５は、教示情報のステップごとにパラメータを更新してもよい。教示情報のステップとは、ある教示点から他の教示点までの移動に応じた教示情報の単位である。また、例えば、更新部１５は、あらかじめ決められた時間ごとに、パラメータを更新してもよい。

（２）カルマンフィルタの例２
上述した（１）カルマンフィルタの例１において、時刻ｋにおけるτ_ｋ、ｖ_ｋ、α_ｋを次式のようにしてもよい。

この場合には、状態方程式は、次式のようになる。

なお、ヤコビアンＦ_ｋは、次のようになる。

状態方程式やヤコビアンが異なる以外は、上述した（１）カルマンフィルタの例１と同様であり、その説明を省略する。

（３）一次遅れフィルタの例
この場合には、フィルタ部１４、及び更新部１５に入力されるのは、基本推定外乱トルクのみである。そして、フィルタ部１４は、一次遅れフィルタによってノイズを除去する。その処理については、例えば、特許文献１を参照されたい。なお、そのフィルタのパラメータは、遅れ時定数であるため、更新部１５は、その遅れ時定数を更新する。具体的には、更新部１５は、入力された基本推定外乱トルクを蓄積し、あらかじめ決められた期間ｄｔごと（例えば、１秒ごと等）に、基本推定外乱トルクの最大値と最小値との差である変化量を算出する。この変化量が大きい場合にはノイズが大きく、この変化量が小さい場合にはノイズが小さいと考えることができる。そして、更新部１５は、ノイズが大きい場合には、遅れ時定数が大きくなるように更新し、ノイズが小さい場合には、遅れ時定数が小さくなるように更新する。なお、例えば、更新部１５は、期間ｄｔにおける変化量が第１のしきい値より大きい場合には、遅れ時定数を一段階大きい値に更新し、変化量が第２のしきい値より小さい場合には、遅れ時定数を一段階小さい値に更新してもよい。ここで、第１のしきい値は、第２のしきい値と同じ値であるか、あるいは、第２のしきい値よりも大きい値である。なお、この更新は、そのあらかじめ決められた期間ｄｔよりも長い周期Ｔで行われてもよい。例えば、ある更新の周期Ｔにおいて、変化量が第１のしきい値より大きかった期間ｄｔの数Ｎ１と、変化量が第２のしきい値より小さかった期間ｄｔの数Ｎ２とを比較し、Ｎ１＞Ｎ２であれば、遅れ時定数を一段階大きい値に更新し、Ｎ１＜Ｎ２であれば、遅れ時定数を一段階小さい値に更新してもよい。このように遅れ時定数の更新を行うことによって、ノイズの除去と、応答の遅れとのバランスが最適になるようにフィルタを設定することができる。

次に、制御装置１の動作について図３のフローチャートを用いて説明する。
（ステップＳ１０１）指令トルク生成部１１は、指令トルクを生成するタイミングかどうか判断する。そして、指令トルクを生成するタイミングである場合には、ステップＳ１０２に進み、そうでない場合には、指令トルクを生成するタイミングとなるまでステップＳ１０１の処理を繰り返す。

（ステップＳ１０２）指令トルク生成部１１は、指令トルクｕ_ｋを生成し、フィルタ部１４と、外乱補償部１６とに渡す。

（ステップＳ１０３）フィルタ部１４は、それまでに受け取っている基本推定外乱トルクτ_ｋ−１と、角速度ｖ_ｋ−１と、角速度の時間微分である角加速度α_ｋ−１と、指令トルク生成部１１から受け取った指令トルクｕ_ｋとを用いて、カルマンフィルタにより最終推定外乱トルクτ_ｋを算出し、外乱補償部１６、及び外乱トルク算出部１３に出力する。なお、初期の動作時であって、まだ基本推定外乱トルク等を受け取っていない場合には、あらかじめ設定されている初期値を用いて最終推定外乱トルクの算出が行われてもよい。

（ステップＳ１０４）外乱補償部１６は、指令トルクｕ_ｋから最終推定外乱トルクτ_ｋの成分を補償した指令トルクｕ_ｋ'を生成し、マニピュレータ２のモータと、外乱トルク算出部１３とに出力する。その出力に応じて、モータが動作し、それに応じて角速度ｖ_ｋが推定トルク算出部１２、フィルタ部１４、更新部１５に渡される。

（ステップＳ１０５）推定トルク算出部１２は、角速度ｖ_ｋを受け取ると、その角速度ｖ_ｋを用いて推定トルクを算出し、外乱トルク算出部１３に渡す。なお、角速度ｖ_ｋから角加速度α_ｋを算出して推定トルクを算出する場合には、推定トルク算出部１２は、次式により角加速度を算出してもよい。
α_ｋ＝（ｖ_ｋ−ｖ_ｋ−１）／ｄｔ

（ステップＳ１０６）外乱トルク算出部１３は、外乱補償部１６から受け取った指令トルクｕ_ｋ'と、推定トルク算出部１２から受け取った推定トルクとを用いて基本推定外乱トルクを算出し、フィルタ部１４と、更新部１５とに渡す。

（ステップＳ１０７）更新部１５は、角速度ｖ_ｋと、それに対応する基本推定外乱トルクとの組を図示しない記録媒体に蓄積する。

（ステップＳ１０８）更新部１５は、フィルタ部１４のパラメータを更新するかどうか判断する。そして、更新する場合には、ステップＳ１０９に進み、そうでない場合には、ステップＳ１０１に戻る。

（ステップＳ１０９）更新部１５は、それまでに蓄積した角速度と基本推定外乱トルクとの組を用いてフィルタ処理のパラメータを算出し、フィルタ部１４のパラメータを更新する。なお、この処理の後に、それまでに蓄積した角速度と基本推定外乱トルクとの組を削除してもよく、または、そうでなくてもよい。そして、ステップＳ１０１に戻る。
なお、上記フローチャートの説明において、ステップＳ１０１からＳ１０９までのループが一周するごとに、添字のｋが１だけインクリメントされることになる。また、図３のフローチャートにおいて、電源オフや処理終了の割り込みにより処理は終了する。

以上のように、本実施の形態による制御装置１によれば、更新部１５がフィルタ部１４のパラメータを更新することにより、マニピュレータ２の動作状況に応じたフィルタ処理を行うことができるようになる。その結果、より適切にノイズの除去された、遅れが最小限となる滑らかな推定外乱トルクを用いてマニピュレータ２を制御することができ、より精度の高い制御が可能となる。なお、外乱を正確に推定するためには、ロボットの逆システムによる推定を行えばよいが、非線形システムの逆システムを実現するのは困難である。そのため、本実施の形態による制御装置１では、粗い外乱推定と、ロボットの外乱モデルを学習するフィルタとを組み合わせることによって、精度の高い外乱推定を実現している。

なお、本実施の形態で説明した速度と外乱トルクとの関係を示す関数は一例であり、他の関数を用いて外乱トルクの推定を行ってもよい。また、本実施の形態で説明したカルマンフィルタや一次遅れフィルタ等は、フィルタ部１４の一例であり、他のフィルタによって基本推定外乱トルクからノイズを除去し、最終推定外乱トルクを生成してもよい。すなわち、状態ベクトルや観測値、状態方程式、観測方程式等は、上述の説明に限定されるものではない。

また、上記実施の形態において、各処理または各機能は、単一の装置または単一のシステムによって集中処理されることによって実現されてもよく、あるいは、複数の装置または複数のシステムによって分散処理されることによって実現されてもよい。

また、上記実施の形態において、各構成要素間で行われる情報の受け渡しは、例えば、その情報の受け渡しを行う２個の構成要素が物理的に異なるものである場合には、一方の構成要素による情報の出力と、他方の構成要素による情報の受け付けとによって行われてもよく、あるいは、その情報の受け渡しを行う２個の構成要素が物理的に同じものである場合には、一方の構成要素に対応する処理のフェーズから、他方の構成要素に対応する処理のフェーズに移ることによって行われてもよい。

また、上記実施の形態において、各構成要素が実行する処理に関係する情報、例えば、各構成要素が受け付けたり、取得したり、選択したり、生成したり、送信したり、受信したりした情報や、各構成要素が処理で用いるしきい値や数式、アドレス、設定値等の情報等は、上記説明で明記していなくても、図示しない記録媒体において、一時的に、あるいは長期にわたって保持されていてもよい。また、その図示しない記録媒体への情報の蓄積を、各構成要素、あるいは、図示しない蓄積部が行ってもよい。また、その図示しない記録媒体からの情報の読み出しを、各構成要素、あるいは、図示しない読み出し部が行ってもよい。

また、上記実施の形態において、各構成要素等で用いられる情報、例えば、各構成要素が処理で用いるしきい値やアドレス、各種の設定値等の情報がユーザによって変更されてもよい場合には、上記説明で明記していなくても、ユーザが適宜、それらの情報を変更できるようにしてもよく、あるいは、そうでなくてもよい。それらの情報をユーザが変更可能な場合には、その変更は、例えば、ユーザからの変更指示を受け付ける図示しない受付部と、その変更指示に応じて情報を変更する図示しない変更部とによって実現されてもよい。その図示しない受付部による変更指示の受け付けは、例えば、入力デバイスからの受け付けでもよく、通信回線を介して送信された情報の受信でもよく、所定の記録媒体から読み出された情報の受け付けでもよい。

また、上記実施の形態において、制御装置１に含まれる２以上の構成要素が通信デバイスや入力デバイス等を有する場合に、２以上の構成要素が物理的に単一のデバイスを有してもよく、あるいは、別々のデバイスを有してもよい。

また、上記実施の形態において、各構成要素は専用のハードウェアにより構成されてもよく、あるいは、ソフトウェアにより実現可能な構成要素については、プログラムを実行することによって実現されてもよい。例えば、ハードディスクや半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。その実行時に、プログラム実行部は、記憶部や記録媒体にアクセスしながらプログラムを実行してもよい。なお、そのプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体に記録されたプログラムが読み出されることによって実行されてもよい。また、そのプログラムは、プログラムプロダクトを構成するプログラムとして用いられてもよい。また、そのプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、あるいは分散処理を行ってもよい。

また、本発明は、以上の実施の形態に限定されることなく、種々の変更が可能であり、それらも本発明の範囲内に包含されるものであることは言うまでもない。

以上より、本発明による制御装置によれば、ノイズを適切に除去した外乱トルクを推定できるという効果が得られ、マニピュレータの制御装置として有用である。

１制御装置
２マニピュレータ
１１指令トルク生成部
１２推定トルク算出部
１３外乱トルク算出部
１４フィルタ部
１５更新部
１６外乱補償部

Claims

モータによって駆動される関節によって連結された複数のアームを有するマニピュレータを制御する制御装置であって、
前記モータの指令トルクを生成する指令トルク生成部と、
前記モータの動作情報を用いて推定トルクを算出する推定トルク算出部と、
前記推定トルクと、前記モータに入力される指令トルクとの差に応じた基本推定外乱トルクを算出する外乱トルク算出部と、
前記基本推定外乱トルクを少なくとも用いてフィルタ処理を行うことにより、ノイズを除去した最終推定外乱トルクを生成するフィルタ部と、
前記基本推定外乱トルクを少なくとも用いて前記フィルタ処理のパラメータを更新する更新部と、
前記指令トルク生成部が生成した指令トルクから、前記フィルタ部が生成した最終推定外乱トルク成分を補償した指令トルクを生成し、前記モータに出力する外乱補償部と、を備えた制御装置。
前記フィルタ部は、前記基本推定外乱トルクと、前記モータの速度と、前記指令トルク生成部が生成した指令トルクとを用いて時間発展モデルのフィルタ処理を行い、
前記更新部は、前記モータの速度と、前記基本推定外乱トルクとを用いて、速度と外乱トルクとの関係を示す関数のパラメータを更新する、請求項１記載の制御装置。
前記更新部は、前記速度と外乱トルクとの関係を示す関数を決定できるだけの前記モータの速度と当該速度に応じた前記基本推定外乱トルクとの組が取得された場合に、前記フィルタ処理のパラメータを更新する、請求項２記載の制御装置。