JPWO2014155559A1 - ノッチフィルタ、外力推定器、モータ制御装置およびロボットシステム - Google Patents

Abstract

モータの回転に伴って発生する振動成分を減衰させることを課題とする。この課題を解決するために、実施形態に係るノッチフィルタは、フィルタリング部と、減衰制御部とを備える。フィルタリング部は、モータの回転に伴って発生する振動成分を含む信号を取得して、前記振動成分の減衰を行う。減衰制御部は、モータの回転速度に応じて減衰の減衰量を制御する。

Description

開示の実施形態は、ノッチフィルタ、外力推定器、モータ制御装置およびロボットシステムに関する。

従来、たとえばロボットの分野では、モータに加わる外力トルクを外力推定器を用いて推定している（特許文献１参照）。

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

しかしながら、外力推定器から出力される外力推定値には、モータの回転に伴って発生する振動成分が含まれている場合があり、外力推定値の精度低下の一因となっていた。このことは、外力推定値に限らずモータの回転に伴って発生する振動成分が含まれる他の信号においても同様に生じ得る。

実施形態の一態様は、モータの回転に伴って発生する振動成分を減衰させることのできるノッチフィルタ、外力推定器、モータ制御装置およびロボットシステムを提供することを目的とする。

実施形態の一態様に係るノッチフィルタは、フィルタリング部と、減衰制御部とを備える。フィルタリング部は、モータの回転に伴って発生する振動成分を含む信号を取得して、前記振動成分の減衰を行う。減衰制御部は、モータの回転速度に応じて減衰の減衰量を制御する。

実施形態の一態様によれば、モータの回転に伴って発生する振動成分を減衰させることができる。

図１は、第１の実施形態に係るロボットシステムが適用されたロボットの一例を示す図である。 図２は、第１の実施形態に係るロボットシステムの構成を示すブロック図である。 図３は、外力オブザーバの構成例を示すブロック図である。 図４は、第１の実施形態に係るノッチフィルタの構成を示すブロック図である。 図５Ａは、第１の実施形態に係るノッチフィルタの周波数特性を示す図である。 図５Ｂは、第１の実施形態に係るノッチフィルタの周波数特性を示す図である。 図６Ａは、ノッチ中心周波数とノッチ深さとの関係の一例を示す図である。 図６Ｂは、ノッチ中心周波数とノッチ深さとの関係の一例を示す図である。 図６Ｃは、ノッチ中心周波数とノッチ深さとの関係の一例を示す図である。 図７Ａは、第２の実施形態に係るノッチフィルタの周波数特性を示す図である。 図７Ｂは、第２の実施形態に係るノッチフィルタの周波数特性を示す図である。 図８は、第３の実施形態に係るロボットシステムの構成を示すブロック図である。 図９は、第４の実施形態に係るロボットシステムの構成を示すブロック図である。 図１０は、第４の実施形態に係る外力オブザーバの構成例を示すブロック図である。 図１１は、第５の実施形態に係るロボットシステムの構成を示すブロック図である。

以下、添付図面を参照して、本願の開示するノッチフィルタ、外力推定器、モータ制御装置およびロボットシステムの実施形態を詳細に説明する。なお、以下に示す実施形態によりこの発明が限定されるものではない。

（第１の実施形態）
図１は、第１の実施形態に係るロボットシステム１００が適用されたロボット１の一例を示す図である。

図１に示すように、ロボット１は、基台１０と、胴部１１と、第１アーム部１２と、第２アーム部１３と、リスト部１４とを備える。

基台１０は、設置面Ｇに固設される。胴部１１は、基台１０に対して旋回部２０を介して水平方向に旋回自在に取付けられる。第１アーム部１２は、胴部１１に対し、第１関節部２１を介して揺動自在に連結される。第２アーム部１３は、第１アーム部１２に対し、第２関節部２２を介して揺動自在に連結される。リスト部１４は、第２アーム部１３に対し、第３関節部２３を介して軸回りに回転自在に、かつ第４関節部２４を介して揺動自在に連結される。そして、リスト部１４の先端部には、用途に応じたエンドエフェクタ（不図示）が適宜連結される。

旋回部２０および第１〜第４関節部２１〜２４には、可動部である胴部１１、第１アーム部１２、第２アーム部１３およびリスト部１４を駆動するアクチュエータ５０が内蔵される。具体的には、アクチュエータ５０は、図１に示すようにモータ２と減速機３とを含んで構成される。

モータ２は、当該モータ２の駆動を制御するモータ制御装置８と電気的に接続され、モータ制御装置８から出力される指令に従って駆動する。減速機３は、モータ２の出力軸に接続され、モータ２の出力軸の回転を減じて第１アーム部１２等の可動部へ伝達する。モータ制御装置８は、たとえばサーボアンプ、サーボアンプを制御するコントローラ、または、サーボアンプとコントローラとを含む制御装置である。

第１の実施形態では、減速機３としてハーモニック減速機が用いられる。ハーモニック減速機は、楕円と真円の差動を利用した減速機（波動歯車装置）である。かかるハーモニック減速機は、モータ２の出力軸が１回転する毎に２回振動するという性質を有するが、かかる点については後述する。

次に、ロボットシステム１００の構成について図２を参照して具体的に説明する。図２は、第１の実施形態に係るロボットシステム１００の構成を示すブロック図である。図２では、第１関節部２１の構成を例に挙げて説明するが、旋回部２０および第２〜第４関節部２２〜２４についても同様の構成である。

図２に示すように、第１関節部２１は、前述のモータ２および減速機３に加えて、トルク検出部４と、速度検出部５と、位置検出器９と、外力推定器３０とを備える。

トルク検出部４は、減速機３と負荷（ここでは、第１アーム部１２）との間に設けられ、モータ２が駆動したときのトルク（Ｎ・ｍ）を検出する。

位置検出器９は、たとえばエンコーダであり、モータ２の出力軸の回転位置Ｐ_ｆｂを検出して速度検出部５へ出力する。なお、エンコーダは、絶対値エンコーダであるものとするが、これに限らず、インクリメンタルエンコーダであってもよい。また、エンコーダに代えて、レゾルバ等を位置検出器９として用いてもよい。

速度検出部５は、位置検出器９から入力される回転位置Ｐ_ｆｂを差分演算することによってモータ２の出力軸の回転速度（ｒａｄ／ｓ）を検出する。なお、トルク検出部４によるトルクの検出方法および速度検出部５による回転速度の検出方法については、いずれの公知技術を用いても構わない。

なお、ここでは、モータ２、減速機３、トルク検出部４、速度検出部５および位置検出器９がそれぞれ別体であるものとするが、たとえば減速機一体型モータや、センサ一体型モータ、あるいは、センサ一体型減速機などを採用してもよい。また、モータ２、減速機３、トルク検出部４、速度検出部５および位置検出器９が一体的に構成されたセンサ一体型アクチュエータを採用してもよい。

外力推定器３０は、たとえばロボットシステム１００の例では、第１アーム部１２や第２アーム部１３等に働く外力を推定する。具体的には、外力推定器３０は、外力オブザーバ６と、ノッチフィルタ７とを備える。外力オブザーバ６は、トルク検出部４から出力されるトルク検出値Ｔ_ｆｂと、速度検出部５から出力される速度検出値ｖ_ｆｂとに基づき、モータ２の出力軸回りに加えられた外力トルクを推定する。

ここで、外力オブザーバ６の具体的構成の一例について図３を参照して説明する。図３は、外力オブザーバ６の構成例を示すブロック図である。

図３に示すように、外力オブザーバ６は、非線形フィードバック項算出部６１と、一般化モーメント算出部６２と、減算部６３と、線形オブザーバ６４とを備える。

非線形フィードバック項算出部６１は、回転位置Ｐ_ｆｂおよび速度検出値ｖ_ｆｂを用いて非線形フィードバック項を算出する。ここで、非線形フィードバック項算出部６１によって算出される非線形フィードバック項は、

式（１）のように表される。ここで、ｑは回転位置Ｐ_ｆｂ相当し、ｄｑ／ｄｔは速度検出値ｖ_ｆｂに相当する。また、Ｃ（ｑ，ｄｑ／ｄｔ）は遠心力およびコリオリ力に関する行列、ｇ（ｑ）は重力項、Ｍ（ｑ）はリンクの質量行列である。非線形フィードバック項算出部６１は、算出した非線形フィードバック項を減算部６３へ出力する。

一般化モーメント算出部６２は、回転位置Ｐ_ｆｂおよび速度検出値ｖ_ｆｂを用いて一般化モーメントｐを算出して線形オブザーバ６４へ出力する。ここで、ｐ＝Ｍ（ｑ）ｄｑ／ｄｔである。

なお、ここでは、非線形フィードバック項算出部６１および一般化モーメント算出部６２が、速度検出部５から取得した速度検出値ｖ_ｆｂから回転位置Ｐ_ｆｂを算出するものとするが、非線形フィードバック項算出部６１および一般化モーメント算出部６２は、位置検出器９から回転位置Ｐ_ｆｂを取得してもよい。

減算部６３は、トルク検出値Ｔ_ｆｂから非線形フィードバック項を減算し、得られた値Ｔ’を線形オブザーバ６４へ出力する。

線形オブザーバ６４は、一般的な線形のオブザーバであり、一般化モーメント算出部６２から入力される一般化モーメントｐと、減算部６３から入力された値Ｔ’とを用いて外力推定値Ｔ_ｄを算出する。

ここで、上述したように、ハーモニック減速機である減速機３は、モータ２の出力軸が１回転する毎に２回振動し、かかる減速機３の振動は、トルク検出部４によってトルクとして検出される。これにより、トルク検出値Ｔ_ｆｂが振動し、かかるトルク検出値Ｔ_ｆｂの振動に伴って外力推定値Ｔ_ｄが振動することとなる。

このように、外力推定値Ｔ_ｄには、モータ２の回転に伴って発生する振動成分、具体的には、モータ２の回転に伴い減速機３が振動することによって生じる振動成分が含まれる。そこで、第１の実施形態に係るロボット１では、かかる振動成分をノッチフィルタ７を用いて減衰させることにより、外力推定値の精度を向上させることとした。

かかるノッチフィルタ７の構成について図４を参照して説明する。図４は、第１の実施形態に係るノッチフィルタ７の構成を示すブロック図である。

図４に示すように、ノッチフィルタ７は、第１の入力部７１と、第２の入力部７２と、フィルタリング部７３と、減衰制御部７４と、出力部７５とを備える。

第１の入力部７１は、外力推定値Ｔ_ｄを入力する。第２の入力部７２は、速度検出値ｖ_ｆｂを入力する。出力部７５は、後述するフィルタリング部７３によって振動成分が減衰された外力推定値Ｔ_ｄ’を出力する。なお、第１の入力部７１、第２の入力部７２および出力部７５は、たとえばポートや端子、ノード等に相当する。

フィルタリング部７３は、第１の入力部７１から入力される外力推定値Ｔ_ｄに含まれる振動成分を減衰させる。ノッチフィルタ７がデジタルフィルタである場合、フィルタリング部７３の伝達関数Ｇ（ｓ）は、

式（２）のようにあらわされる。ここで、δは振動成分の減衰量（以下、「ノッチ深さ」と記載する）を決めるパラメータ、ζは減衰帯域の幅（以下、「ノッチ幅」と記載する）を決めるパラメータ、ω_ｎは減衰帯域の中心周波数（以下、「ノッチ中心周波数」と記載する）を決めるパラメータである。

また、ノッチ深さを決めるパラメータであるδは、νをノッチ深さとすると、

式（３）のようにあらわされる。

減衰制御部７４は、第２の入力部７２から速度検出値ｖ_ｆｂを入力し、入力した速度検出値ｖ_ｆｂに応じてフィルタリング部７３のノッチ中心周波数ω_ｎを制御する。具体的には、減衰制御部７４は、速度検出値ｖ_ｆｂの増減に応じてフィルタリング部７３のノッチ中心周波数ω_ｎを増減させる。これにより、モータ２の回転速度に応じて周波数が変動する振動成分をフィルタリング部７３によって適切に減衰させることができる。

この点について図５Ａおよび図５Ｂを参照して説明する。図５Ａおよび図５Ｂは、第１の実施形態に係るノッチフィルタ７の周波数特性を示す図である。図５Ａに示すように、フィルタリング部７３は、入力信号の所定の周波数帯域を減衰させる。ここで、ω_ｎはノッチ中心周波数であり、νはノッチ深さである。

上述したように、ハーモニック減速機である減速機３は、モータ２の出力軸が１回転する毎に２回振動する、言い換えれば、モータ２の回転速度の２倍の周波数で振動する。したがって、外力推定値Ｔ_ｄに含まれる振動成分は、モータ２の回転速度が速くなるほど高周波となる。

そこで、減衰制御部７４は、図５Ｂに示すように、第２の入力部７２から入力される速度検出値ｖ_ｆｂが高くなるほどフィルタリング部７３のノッチ中心周波数ω_ｎを高くし、速度検出値ｖ_ｆｂが低くなるほどフィルタリング部７３のノッチ中心周波数ω_ｎを低くすることとした。具体的には、ノッチ中心周波数ω_ｎは、ω_ｎ＝２ｖ_ｆｂである。

このように、第１の実施形態では、減速機３の振動の周波数がモータ２の回転速度に応じて増減することに着目し、速度検出値ｖ_ｆｂに応じてフィルタリング部７３の減衰帯域を移動させることとした。具体的には、減速機３はモータ２の回転速度の２倍の周波数で振動する。このため、減衰制御部７４は、ノッチ中心周波数ω_ｎを速度検出値ｖ_ｆｂの２倍の周波数に変更することとした。これにより、外力推定値Ｔ_ｄに含まれる振動成分を適切に減衰させることができ、外力推定値の精度を向上させることができる。

さらに、減衰制御部７４は、第２の入力部７２から入力される速度検出値ｖ_ｆｂの増減に応じてノッチ深さνも増減させる。以下、この点について説明する。

上述したように減速機３の振動はモータ２の回転速度が増加するほど高周波となるが、振幅については、モータ２の回転速度に依らずほぼ一定である。これにもかかわらず、第１の実施形態に係るノッチフィルタ７は、モータ２の回転速度が遅いとき、つまり、減速機３の振動が低周波であるときには、ノッチ深さνを浅くする、すなわち、振動成分の減衰量を少なくする。

これは、外力推定値Ｔ_ｄの低い周波数帯域に有用な情報が集中しているためであり、低い周波数帯域において振動成分の減衰量を敢えて少なくすることで、外力推定値Ｔ_ｄに含まれる有用な情報を残しつつ不要な振動成分を減衰させることができる。

具体的には、図５Ｂに示すように、減衰制御部７４は、ノッチ中心周波数ω_ｎが低くなるほど、つまり、第２の入力部７２から入力される速度検出値ｖ_ｆｂが低くなるほどフィルタリング部７３のノッチ深さνを浅くする。

つづいて、ノッチ深さνの変化のさせ方の一例について図６Ａ〜図６Ｃを参照して説明する。図６Ａ〜図６Ｃは、ノッチ中心周波数ω_ｎとノッチ深さνとの関係の一例を示す図である。

なお、図６Ａ〜図６Ｃには、ノッチ中心周波数ω_ｎを横軸に、ノッチ深さνを決めるパラメータであるδを縦軸にとった場合のω_ｎとδとの関係を示している。上述した式（３）からわかるように、δが１のときにノッチ深さνが０となり、δが０のときにノッチ深さνが無限大となる。

たとえば、図６Ａに示すように、減衰制御部７４は、ω_ｎ＝０のときにδ＝１とし、ω_ｎの増加に伴って（すなわち、速度検出値ｖ_ｆｂの増加に伴って）δが曲線的に減少するようにフィルタリング部７３による減衰の減衰量を制御してもよい。図６Ａに示す曲線は、ω_ｎ＝ω１に変曲点Ｐを有する曲線（シグモイド曲線）であり、ω_ｎ＜ω１のときには上に凸、ω_ｎ＞ω１のときには下に凸となる。

なお、減衰制御部７４は、図６Ａに示した曲線に限らず、δが変曲点のない曲線（たとえば指数曲線など）に従って減少するようにフィルタリング部７３による減衰の減衰量を制御してもよい。

また、図６Ｂに示すように、２つの閾値ω２，ω３を設け、ω_ｎ≦ω_２のときにはδ＝１で一定とし、ω_ｎ≧ω_３のときにはδ＝α（＜１）で一定とし、ω_２＜ω_ｎ＜ω_３のときには、ω_ｎの増加に伴ってδが１からαへ直線的に減少するように、減衰制御部７４がフィルタリング部７３の減衰量を制御してもよい。

すなわち、減衰制御部７４は、ω_２／２（第１の閾値）以下の速度検出値ｖ_ｆｂが入力された場合には、ノッチ深さνが０となり、かつ、ω_３／２（第２の閾値）以上の速度検出値ｖ_ｆｂが入力された場合には、ノッチ深さνが０よりも大きい一定量となるようにフィルタリング部７３の減衰量を制御してもよい。

このように、減衰制御部７４は、所定の閾値（ここではω_３／２）以上の速度検出値ｖ_ｆｂが入力された場合のノッチ深さνを一定とする。これは、もともとモータ２の回転に伴う減速機３の振動の振幅がモータ２の回転速度に依らずほぼ一定であるためであり、このようにω_３／２以上の回転速度におけるノッチ深さνを一定とすることで、図６Ａに示す場合と比較して処理負荷を軽減することができる。

なお、ここでは、ω_２＜ω_ｎ＜ω_３のときに、ω_ｎの増加に伴ってδを直線的に減少させることとしたが、ω_ｎの増加に伴ってδを曲線的に減少させてもよい。また、ここでは、閾値を２つとしたが、閾値は３つ以上であってもよい。

また、図６Ｃに示すように、１つの閾値ω_４を設け、ω_ｎ＜ω_４のときにはδ＝１（すなわち、ν＝０）で一定となり、ω_ｎ≧ω_４のときにはδ＝αで一定となるように、減衰制御部７４がフィルタリング部７３の減衰量を制御してもよい。

また、たとえば図６Ｂの０≦ω_ｎ＜ω_３の部分を図６Ａに示すような曲線に置き換えてもよい。すなわち、０≦ω_ｎ＜ω_３のときには、δが図６Ａに示すような曲線に従って減少し、ω_ｎ≧ω_３のときにはδ＝αで一定となるように、減衰制御部７４がフィルタリング部７３の減衰量を制御してもよい。

また、図２に示すように、外力推定器３０から出力されるフィルタリング後の外力推定値Ｔ_ｄ’は、モータ制御装置８にフィードバックされる。そして、モータ制御装置８は、かかる外力推定値Ｔ_ｄ’に基づいてトルク指令を補正し、補正したトルク指令Ｔ_ｒｅｆをモータ２へ出力する。

たとえば、モータ制御装置８は、補正前のトルク指令から外力推定値Ｔ_ｄ’を減算したものをトルク指令Ｔ_ｒｅｆとして出力するポジティブフィードバックを行う。あるいは、モータ制御装置８は、外力推定値Ｔ_ｄ’の位相を反転させたうえで、補正前のトルク指令から位相反転後の外力推定値Ｔ_ｄ’を減算するネガティブフィードバックを行ってもよい。これにより、ロボット１の制御を精度良く行うことができる。

上述してきたように、第１の実施形態に係るロボットシステム１００は、ロボット１と、外力オブザーバ６と、ノッチフィルタ７とを備える。ロボット１は、各関節部２１〜２４がモータ２と減速機３とを含んで構成される。外力オブザーバ６は、モータ２のトルク検出値Ｔ_ｆｂと速度検出値ｖ_ｆｂとに基づいて外力推定値Ｔ_ｄを生成する。ノッチフィルタ７は、外力オブザーバ６から出力される外力推定値Ｔ_ｄに含まれる、モータ２の回転に起因する振動成分を減衰させる。また、ノッチフィルタ７は、フィルタリング部７３と減衰制御部７４とを備える。フィルタリング部７３は、外力推定値Ｔ_ｄを取得し、外力推定値Ｔ_ｄに含まれる振動成分の減衰を行う。減衰制御部７４は、モータ２の速度検出値ｖ_ｆｂを取得し、取得した速度検出値ｖ_ｆｂに応じてフィルタリング部７３による減衰の減衰量を制御する。

したがって、第１の実施形態に係るロボットシステム１００によれば、モータ２の回転に伴って発生する振動成分を減衰させることができる。

また、第１の実施形態に係るロボットシステム１００では、フィルタリング部７３が、モータ２の回転に伴って減速機３が発生させる振動成分を含む外力推定値Ｔ_ｄを取得することとした。これにより、モータ２の回転に伴って減速機３が発生させる振動成分を減衰させることができる。

なお、ここでは、モータ２の出力軸が１回転する毎に２回振動する性質を有する減速機３の場合に、ノッチ中心周波数ω_ｎを速度検出値ｖ_ｆｂの２倍とする場合の例を示した。これと同様に、モータ２の出力軸が１回転する毎にｎ回振動する性質を有する減速機３の場合には、ノッチ中心周波数ω_ｎを速度検出値ｖ_ｆｂのｎ倍（ｎは２以上の整数）とすればよい。また、上記のｎは、２以上の整数に限定されない。すなわち、減速機３が、モータ２の出力軸が２回転する毎に３回振動する性質やモータ２の出力軸が３回転する毎に１回振動する性質を有する場合には、ノッチ中心周波数ω_ｎを速度検出値ｖ_ｆｂのそれぞれ３／２倍、１／３倍とすればよい。このように、減衰制御部７４は、ノッチ中心周波数ω_ｎを速度検出値ｖ_ｆｂに比例した周波数に変更するようにしてもよい。

また、ここでは、減速機３が、モータ２の回転に応じて振動成分を発生させる減速機である場合の例を示したが、減速機３は、モータ２の回転とは無関係に振動成分を発生させる減速機であってもよい。このような場合であっても、減速機３の振動成分がモータ２の回転に応じて変化する場合には、上述してきたノッチフィルタ７を用いて振動成分を適切に減衰させることができる。

（第２の実施形態）
上述した第１の実施形態では、モータ２の回転速度の増減に応じて、ノッチ中心周波数ω_ｎおよびノッチ深さνの両方を増減させる場合の例について説明したが、ノッチフィルタ７は、ノッチ中心周波数ω_ｎを固定とし、ノッチ深さνのみを増減させる構成であってもよい。

かかる点について図７Ａおよび図７Ｂを参照して説明する。図７Ａおよび図７Ｂは、第２の実施形態に係るノッチフィルタ７の周波数特性を示す図である。

図７Ａおよび図７Ｂに示すように、第２の実施形態に係るノッチフィルタ７の減衰制御部７４は、第２の入力部７２から入力される速度検出値ｖ_ｆｂが変化した場合に、ノッチ中心周波数ω_ｎを変化させることなく、ノッチ深さνのみを変化させる。ノッチ深さνは、たとえば所定の係数ｋ（ｋは正の数）を用いてν＝ｋｖ_ｆｂとしてもよいし、速度検出値ｖ_ｆｂを変数とする所定の関数ν＝ｆ（ｖ_ｆｂ）としてもよい。

第１の実施形態では、減速機３が、ハーモニック減速機である場合を例に挙げて説明した。しかし、減速機３がハーモニック減速機以外の減速機である場合、減速機の種類によっては、モータ２の回転速度の増減に伴って振動成分の振幅が増減する場合もある。

このような場合には、第２の実施形態に係るノッチフィルタ７のように、速度検出値ｖ_ｆｂの増減に応じてノッチ深さνを増減させることで、モータ２の回転に伴って発生する振動成分を減衰させることができる。

（第３の実施形態）
ところで、上述してきた各実施形態では、外力推定器３０が旋回部２０および第１〜第４関節部２１〜２４に設けられる場合の例を示したが、外力推定器３０は、たとえばモータ制御装置８に設けられてもよい。以下では、モータ制御装置が、外力推定器３０に相当する処理部を備える場合の例について図８を参照して説明する。図８は、第３の実施形態に係るロボットシステムの構成を示すブロック図である。

図８に示すように、第３の実施形態に係るロボットシステム１００Ａにおいて、第１関節部２１Ａは、第１および第２の実施形態に係る第１関節部２１から外力推定器３０を除いた構成を有する。他の関節部についても同様に、外力推定器３０を除いた構成を有する。

第３の実施形態に係るモータ制御装置８Ａは、外力推定部３０Ａと、制御部８１とを備える。外力推定部３０Ａは、外力推定器３０に相当する処理部であり、外力推定器３０と同様、外力オブザーバ６とノッチフィルタ７とを備える。なお、モータ制御装置８Ａは、各関節部に対応する複数の外力推定部３０Ａを備えるが、ここでは、第１関節部２１Ａに対応する外力推定部３０Ａのみを示している。

トルク検出値Ｔ_ｆｂおよび速度検出値ｖ_ｆｂは、モータ制御装置８Ａに設けられた外力推定部３０Ａに入力される。具体的には、トルク検出値Ｔ_ｆｂは外力オブザーバ６に入力され、速度検出値ｖ_ｆｂは、外力オブザーバ６およびノッチフィルタ７にそれぞれ入力される。

外力推定部３０Ａでは、上述した外力推定器３０と同様、外力オブザーバ６が、トルク検出値Ｔ_ｆｂおよび速度検出値ｖ_ｆｂに基づいて外力推定値Ｔ_ｄを生成してノッチフィルタ７へ出力し、ノッチフィルタ７が、外力推定値Ｔ_ｄから振動成分を減衰させて、外力推定値Ｔ_ｄ’を制御部８１へ出力する。ノッチフィルタ７は、第１および第２の実施形態において説明した通り、フィルタリング部７３および減衰制御部７４を備え（図４参照）、減衰制御部７４が速度検出値ｖ_ｆｂに応じてフィルタリング部７３のノッチ中心周波数ω_ｎやノッチ深さνを変更する。これにより、外力推定値Ｔ_ｄに含まれる、モータ２の回転に伴って発生する振動成分を減衰させることができる。

そして、制御部８１は、外力推定部３０Ａから入力される外力推定値Ｔ_ｄ’に基づいてトルク指令を補正し、補正したトルク指令Ｔ_ｒｅｆをモータ２へ出力する。

このように、フィルタリング部７３および減衰制御部７４は、モータ制御装置８Ａに設けられていてもよい。

また、上述してきた各実施形態では、ノッチフィルタ７を外力推定器３０や外力推定部３０Ａに設ける場合の例について説明したが、ノッチフィルタ７は、外力オブザーバ６と別体で、図２に示す制御ループ内の任意の場所に設けられてもよい。

また、ノッチフィルタ７に入力される入力信号は、モータ２の回転に伴って発生する振動成分を含んだ信号であればよく、外力推定値Ｔ_ｄに限定されない。たとえば、ノッチフィルタ７をトルク検出部４の後段に設けて、トルク検出値Ｔ_ｆｂに含まれる振動成分をノッチフィルタ７によって減衰させるようにしてもよい。

（第４の実施形態）
次に、第４の実施形態に係るロボットシステムの構成について図９を参照して説明する。図９は、第４の実施形態に係るロボットシステムの構成を示すブロック図である。

図９に示すように、第４の実施形態に係るロボットシステム１００Ｂが備える第１関節部２１Ｂは、第１の実施形態に係る第１関節部２１（図２参照）から減速機３およびトルク検出部４を除外した構成を有する。

上述してきた各実施形態では、減速機３が振動成分を発生させる場合の例を示したが、振動成分は、減速機３だけではなく、たとえばモータ２自体の構造等に起因して発生する場合もある。つまり、第４の実施形態に係るロボットシステム１００Ｂのように減速機３を有していない系においても、モータ２の回転に伴って発生する振動成分が外力推定値Ｔ_ｄに含まれる可能性がある。したがって、このような系に対してロボットシステム１００Ｂを適用した場合であっても、モータ２の回転に伴って発生する振動成分を減衰させることができる。

また、第４の実施形態に係る外力オブザーバ６Ｂは、上述してきた外力オブザーバ６とは異なり、モータ制御装置８から出力されるトルク指令Ｔ_ｒｅfを用いて外力推定値Ｔ_ｄを推定する。かかる場合、外力オブザーバ６Ｂは、第１アーム部１２等に働く外力、摩擦力およびその他の力の総和、すなわち、外乱を「外力」として推定する。

ここで、第４の実施形態に係る外力オブザーバ６Ｂの構成例について図１０を参照して説明する。図１０は、第４の実施形態に係る外力オブザーバ６Ｂの構成例を示すブロック図である。図１０に示すように、外力オブザーバ６Ｂは、微分器６５と、慣性モーメント乗算部６６と、減算部６７と、ローパスフィルタ６８とを備える。

微分器６５は、速度検出値ｖ_ｆｂを微分することによって加速度検出値Ａ_ｆｂを算出し、算出した加速度検出値Ａ_ｆｂを慣性モーメント乗算部６６へ出力する。慣性モーメント乗算部６６は、微分器６５から入力された加速度検出値Ａ_ｆｂに対してモータ軸周りの慣性モーメントを乗算することによって加速トルク検出値ＴＡ_ｆｂを算出し、算出した加速トルク検出値ＴＡ_ｆｂを減算部６７へ出力する。

減算部６７は、加速トルク検出値ＴＡ_ｆｂからトルク指令Ｔ_ｒｅfを減算し、得られた値Ｔ’’をローパスフィルタ６８へ出力する。ローパスフィルタ６８は、Ｔ’’にローパスフィルタをかけた値を外力推定値Ｔ_ｄとして出力する。

このように、外力オブザーバ６Ｂは、トルク検出値Ｔ_ｆｂに代えてトルク指令Ｔ_ｒｅfを用いて外力推定値Ｔ_ｄを算出してもよい。

なお、ここでは、外力推定器３０Ｂが第１関節部２１Ｂに設けられる場合の例を示したが、第３の実施形態と同様、外力推定器３０Ｂに代えて、外力推定器３０Ｂに相当する処理部をモータ制御装置８に設けてもよい。

（第５の実施形態）
次に、第５の実施形態に係るロボットシステムの構成について図１１を参照して説明する。図１１は、第５の実施形態に係るロボットシステムの構成を示すブロック図である。

図１１に示すように、第５の実施形態に係るロボットシステム１００Ｃは、ノッチフィルタ７Ｃ１，７Ｃ２をさらに備える。ノッチフィルタ７Ｃ１は、第１関節部２１Ｃのノッチフィルタ７の後段に設けられ、ノッチフィルタ７Ｃ２は、第２関節部２２Ｃのノッチフィルタ７の後段に設けられる。

また、第１関節部２１Ｃおよび第２関節部２２Ｃは、上述した第１の実施形態に係る第１関節部２１と同様の構成である。以下では、第１関節部２１Ｃについてのトルク指令、回転位置、トルク検出値、速度検出値、外力推定値をそれぞれ「Ｔ_{ｒｅｆ＿１}」、「Ｐ_ｆｂ＿１」、「Ｔ_ｆｂ＿１」、「ｖ_ｆｂ＿１」、「Ｔ_ｄ＿１（Ｔ_ｄ＿１’）」とする。また、第２関節部２２Ｃについては、それぞれ「Ｔ_{ｒｅｆ＿２}」、「Ｐ_ｆｂ＿２」、「Ｔ_ｆｂ＿２」、「ｖ_ｆｂ＿２」、「Ｔ_ｄ＿２（Ｔ_ｄ＿２’）」とする。

ここで、第１関節部２１Ｃの信号には、ロボットシステム１００Ｃ内の他の系（たとえば、第２関節部２２Ｃ）において発生した振動成分も含まれ得る。第２関節部２２Ｃにおいても同様である。

そこで、第５の実施形態に係るロボットシステム１００Ｃでは、ノッチフィルタ７Ｃ１，７Ｃ２をさらに設け、他の系において発生した振動成分をノッチフィルタ７Ｃ１，７Ｃ２によって減衰させることとした。

たとえば、ノッチフィルタ７Ｃ１には、第１関節部２１Ｃのノッチフィルタ７から出力される外力推定値、すなわち、第１関節部２１Ｃの減速機３による振動成分が減衰された外力推定値と、第２関節部２２Ｃの速度検出部５から出力される速度検出値ｖ_ｆｂ＿２とが入力される。そして、ノッチフィルタ７Ｃ１は、速度検出値ｖ_ｆｂ＿２に応じたノッチ中心周波数ω_ｎおよびノッチ深さνで、第１関節部２１Ｃのノッチフィルタ７から出力される外力推定値をフィルタリングする。これにより、第１関節部２１Ｃのノッチフィルタ７から出力される外力推定値に含まれる、第２関節部２２Ｃにおいて発生した振動成分を減衰させることができる。フィルタリング後の外力推定値Ｔ_ｄ＿１’は、モータ制御装置８へ出力される。

また、ノッチフィルタ７Ｃ２には、第２関節部２２Ｃのノッチフィルタ７から出力される外力推定値、すなわち、第２関節部２２Ｃの減速機３による振動成分が減衰された外力推定値と、第１関節部２１Ｃの速度検出部５から出力される速度検出値ｖ_ｆｂ＿１とが入力される。そして、ノッチフィルタ７Ｃ２は、速度検出値ｖ_ｆｂ＿１に応じたノッチ中心周波数ω_ｎおよびノッチ深さνで、第２関節部２２Ｃのノッチフィルタ７から出力される外力推定値をフィルタリングする。これにより、第２関節部２２Ｃのノッチフィルタ７から出力される外力推定値に含まれる、第１関節部２１Ｃにおいて発生した振動成分を減衰させることができる。フィルタリング後の外力推定値Ｔ_ｄ＿２’は、モータ制御装置８へ出力される。

このように、第５の実施形態に係るロボットシステム１００Ｃによれば、ノッチフィルタ７Ｃ１，７Ｃ２を設けることで、他の系において発生した振動成分も減衰させることができる。

なお、ここでは、第１関節部２１Ｃのノッチフィルタ７の後段に、第２関節部２２Ｃにおいて発生した振動成分を減衰させるためのノッチフィルタ７Ｃ１を設ける場合の例を示した。しかし、これに限ったものではなく、ノッチフィルタ７の後段には、ノッチフィルタ７Ｃ１に加え、第２関節部２２Ｃ以外の関節部において発生した振動成分を減衰させるノッチフィルタがさらに設けられてもよい。

また、ここでは、ノッチフィルタ７Ｃ１，７Ｃ２が、たとえば第１関節部２１Ｃおよび第２関節部２２Ｃの外部に設けられる場合の例を示したが、ノッチフィルタ７Ｃ１，７Ｃ２は、それぞれ第１関節部２１Ｃおよび第２関節部２２Ｃの内部に設けられてもよいし、モータ制御装置８の内部に設けられてもよい。

また、第３の実施形態と同様、外力推定器３０を第１関節部２１Ｃおよび第２関節部２２Ｃから除外し、外力推定器３０に相当する処理部をモータ制御装置８に設けてもよい。

また、第４の実施形態と同様、外力オブザーバ６は、トルク検出値Ｔ_ｆｂに代えてトルク指令Ｔ_ｒｅfを用いて外力推定値Ｔ_ｄを算出してもよい。

また、モータ２は、回転型に限らず直動型のリニアモータであってもよい。かかる場合、並進力が上述したトルクに相当し、並進速度が上述した回転速度に相当する。また、モータ２は、電気モータに限らず流体圧アクチュエータなどであってもよい。

また、上述してきた各実施形態では、外力推定器３０をロボット１に適用した例について説明したが、外力推定器３０が適用されるロボットの構成は、図１に示したものに限定されない。また、外力推定器３０は、ロボット１に限らず、モータ２によって駆動されるあらゆるものに適用することができる。

さらなる効果や変形例は、当業者によって容易に導き出すことができる。このため、本発明のより広範な態様は、以上のように表しかつ記述した特定の詳細および代表的な実施形態に限定されるものではない。したがって、添付の特許請求の範囲およびその均等物によって定義される総括的な発明の概念の精神または範囲から逸脱することなく、様々な変更が可能である。

１ ロボット
２ モータ
３ 減速機
４ トルク検出部
５ 速度検出部
６ 外力オブザーバ
７ ノッチフィルタ
８ モータ制御装置
１０ 基台
１１ 胴部
１２ 第１アーム部
１３ 第２アーム部
１４ リスト部
２０ 旋回部
２１〜２４ 第１〜第４関節部
３０ 外力推定器
７１ 第１の入力部
７２ 第２の入力部
７３ フィルタリング部
７４ 減衰制御部
７５ 出力部
１００ ロボットシステム

Claims (12)

1. モータの回転に伴って発生する振動成分を含む信号を取得して、前記振動成分の減衰を行うフィルタリング部と、
   前記モータの回転速度に応じて前記減衰の減衰量を制御する減衰制御部と
   を備えることを特徴とするノッチフィルタ。
2. 前記減衰制御部は、
   前記モータの回転速度に応じて前記減衰の減衰帯域の中心周波数を制御すること
   を特徴とする請求項１に記載のノッチフィルタ。
3. 前記フィルタリング部は、
   前記モータの回転に伴って減速機が発生させる前記振動成分を含む信号を取得すること
   を特徴とする請求項１に記載のノッチフィルタ。
4. 前記フィルタリング部は、
   前記モータの回転速度に応じて変動する前記振動成分を含む信号を取得すること
   を特徴とする請求項３に記載のノッチフィルタ。
5. 前記フィルタリング部は、
   前記モータの回転に伴ってハーモニック減速機が発生させる前記振動成分を含む信号を取得すること
   を特徴とする請求項３に記載のノッチフィルタ。
6. 前記減衰制御部は、
   前記中心周波数を前記回転速度に比例した周波数に変更すること
   を特徴とする請求項２に記載のノッチフィルタ。
7. 前記減衰制御部は、
   前記中心周波数を前記回転速度のｎ倍（ｎは２以上の整数）の周波数に変更すること
   を特徴とする請求項２に記載のノッチフィルタ。
8. 前記減衰制御部は、
   前記回転速度が所定の閾値以上である場合には、前記回転速度に依らず前記減衰量を０よりも多い一定量とすること
   を特徴とする請求項１に記載のノッチフィルタ。
9. 前記減衰制御部は、
   前記回転速度が第１の閾値以下である場合には前記減衰量を０とし、かつ、前記回転速度が前記第１の閾値よりも大きい第２の閾値以上である場合には、前記回転速度に依らず前記減衰量を０よりも多い一定量とすること
   を特徴とする請求項１に記載のノッチフィルタ。
10. モータのトルクに関する情報と回転速度に関する情報とに基づいて外力推定値を生成する外力オブザーバと、
    前記外力オブザーバから出力される外力推定値に含まれる、前記モータの回転に伴って発生する振動成分を減衰させるノッチフィルタと
    を備え、
    前記ノッチフィルタは、
    前記外力推定値を取得して、前記振動成分の減衰を行うフィルタリング部と、
    前記モータの回転速度に応じて前記減衰の減衰量を制御する減衰制御部と
    を備えることを特徴とする外力推定器。
11. モータの回転に伴って発生する振動成分を含む信号を取得して、前記振動成分の減衰を行うフィルタリング部と、
    前記モータの回転速度に応じて前記減衰の減衰量を制御する減衰制御部と
    を備えることを特徴とするモータ制御装置。
12. 各関節部がモータを含んで構成されるロボットと、
    前記モータのトルクに関する情報と回転速度に関する情報とに基づいて外力推定値を生成する外力オブザーバと、
    前記外力オブザーバから出力される外力推定値に含まれる、前記モータの回転に伴って発生する振動成分を減衰させるノッチフィルタと
    を備え、
    前記ノッチフィルタは、
    前記外力推定値を取得して、前記振動成分の減衰を行うフィルタリング部と、
    前記モータの回転速度に応じて前記減衰の減衰量を制御する減衰制御部と
    を備えることを特徴とするロボットシステム。

Images