(57)【要約】  モータの回転に伴って発生する振動成分を減衰させることを課題とする。この課題を解決するために、実施形態に係るノッチフィルタは、フィルタリング部と、減衰制御部とを備える。フィルタリング部は、モータの回転に伴って発生する振動成分を含む信号を取得して、前記振動成分の減衰を行う。減衰制御部は、モータの回転速度に応じて減衰の減衰量を制御する。
2014155559.tif 000002
【特許請求の範囲】
【請求項１】
モータの回転に伴って発生する振動成分を含む信号を取得して、前記振動成分の減衰を行うフィルタリング部と、
前記モータの回転速度に応じて前記減衰の減衰量を制御する減衰制御部と
を備えることを特徴とするノッチフィルタ。
【請求項２】
前記減衰制御部は、
前記モータの回転速度に応じて前記減衰の減衰帯域の中心周波数を制御すること
を特徴とする請求項１に記載のノッチフィルタ。
【請求項３】
前記フィルタリング部は、
前記モータの回転に伴って減速機が発生させる前記振動成分を含む信号を取得すること
を特徴とする請求項１に記載のノッチフィルタ。
【請求項４】
前記フィルタリング部は、
前記モータの回転速度に応じて変動する前記振動成分を含む信号を取得すること
を特徴とする請求項３に記載のノッチフィルタ。
【請求項５】
前記フィルタリング部は、
前記モータの回転に伴ってハーモニック減速機が発生させる前記振動成分を含む信号を取得すること
を特徴とする請求項３に記載のノッチフィルタ。
【請求項６】
前記減衰制御部は、
前記中心周波数を前記回転速度に比例した周波数に変更すること
を特徴とする請求項２に記載のノッチフィルタ。
【請求項７】
前記減衰制御部は、
前記中心周波数を前記回転速度のｎ倍（ｎは２以上の整数）の周波数に変更すること
を特徴とする請求項２に記載のノッチフィルタ。
【請求項８】
前記減衰制御部は、
前記回転速度が所定の閾値以上である場合には、前記回転速度に依らず前記減衰量を０よりも多い一定量とすること
を特徴とする請求項１に記載のノッチフィルタ。
【請求項９】
前記減衰制御部は、
前記回転速度が第１の閾値以下である場合には前記減衰量を０とし、かつ、前記回転速度が前記第１の閾値よりも大きい第２の閾値以上である場合には、前記回転速度に依らず前記減衰量を０よりも多い一定量とすること
を特徴とする請求項１に記載のノッチフィルタ。
【請求項１０】
モータのトルクに関する情報と回転速度に関する情報とに基づいて外力推定値を生成する外力オブザーバと、
前記外力オブザーバから出力される外力推定値に含まれる、前記モータの回転に伴って発生する振動成分を減衰させるノッチフィルタと
を備え、
前記ノッチフィルタは、
前記外力推定値を取得して、前記振動成分の減衰を行うフィルタリング部と、
前記モータの回転速度に応じて前記減衰の減衰量を制御する減衰制御部と
を備えることを特徴とする外力推定器。
【請求項１１】
モータの回転に伴って発生する振動成分を含む信号を取得して、前記振動成分の減衰を行うフィルタリング部と、
前記モータの回転速度に応じて前記減衰の減衰量を制御する減衰制御部と
を備えることを特徴とするモータ制御装置。
【請求項１２】
各関節部がモータを含んで構成されるロボットと、
前記モータのトルクに関する情報と回転速度に関する情報とに基づいて外力推定値を生成する外力オブザーバと、
前記外力オブザーバから出力される外力推定値に含まれる、前記モータの回転に伴って発生する振動成分を減衰させるノッチフィルタと
を備え、
前記ノッチフィルタは、
前記外力推定値を取得して、前記振動成分の減衰を行うフィルタリング部と、
前記モータの回転速度に応じて前記減衰の減衰量を制御する減衰制御部と
を備えることを特徴とするロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
開示の実施形態は、ノッチフィルタ、外力推定器、モータ制御装置およびロボットシステムに関する。
【背景技術】
【０００２】
従来、たとえばロボットの分野では、モータに加わる外力トルクを外力推定器を用いて推定している（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００１−３５３６８７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、外力推定器から出力される外力推定値には、モータの回転に伴って発生する振動成分が含まれている場合があり、外力推定値の精度低下の一因となっていた。このことは、外力推定値に限らずモータの回転に伴って発生する振動成分が含まれる他の信号においても同様に生じ得る。【０００５】
実施形態の一態様は、モータの回転に伴って発生する振動成分を減衰させることのできるノッチフィルタ、外力推定器、モータ制御装置およびロボットシステムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
実施形態の一態様に係るノッチフィルタは、フィルタリング部と、減衰制御部とを備える。フィルタリング部は、モータの回転に伴って発生する振動成分を含む信号を取得して、前記振動成分の減衰を行う。減衰制御部は、モータの回転速度に応じて減衰の減衰量を制御する。
【発明の効果】
【０００７】
実施形態の一態様によれば、モータの回転に伴って発生する振動成分を減衰させることができる。
【図面の簡単な説明】
【０００８】
【図１】図１は、第１の実施形態に係るロボットシステムが適用されたロボットの一例を示す図である。
【図２】図２は、第１の実施形態に係るロボットシステムの構成を示すブロック図である。
【図３】図３は、外力オブザーバの構成例を示すブロック図である。
【図４】図４は、第１の実施形態に係るノッチフィルタの構成を示すブロック図である。
【図５Ａ】図５Ａは、第１の実施形態に係るノッチフィルタの周波数特性を示す図である。
【図５Ｂ】図５Ｂは、第１の実施形態に係るノッチフィルタの周波数特性を示す図である。
【図６Ａ】図６Ａは、ノッチ中心周波数とノッチ深さとの関係の一例を示す図である。
【図６Ｂ】図６Ｂは、ノッチ中心周波数とノッチ深さとの関係の一例を示す図である。
【図６Ｃ】図６Ｃは、ノッチ中心周波数とノッチ深さとの関係の一例を示す図である。
【図７Ａ】図７Ａは、第２の実施形態に係るノッチフィルタの周波数特性を示す図である。
【図７Ｂ】図７Ｂは、第２の実施形態に係るノッチフィルタの周波数特性を示す図である。
【図８】図８は、第３の実施形態に係るロボットシステムの構成を示すブロック図である。
【図９】図９は、第４の実施形態に係るロボットシステムの構成を示すブロック図である。
【図１０】図１０は、第４の実施形態に係る外力オブザーバの構成例を示すブロック図である。
【図１１】図１１は、第５の実施形態に係るロボットシステムの構成を示すブロック図である。
【発明を実施するための形態】
【０００９】
以下、添付図面を参照して、本願の開示するノッチフィルタ、外力推定器、モータ制御装置およびロボットシステムの実施形態を詳細に説明する。なお、以下に示す実施形態によりこの発明が限定されるものではない。
【００１０】
（第１の実施形態）
図１は、第１の実施形態に係るロボットシステム１００が適用されたロボット１の一例を示す図である。
【００１１】
図１に示すように、ロボット１は、基台１０と、胴部１１と、第１アーム部１２と、第２アーム部１３と、リスト部１４とを備える。
【００１２】
基台１０は、設置面Ｇに固設される。胴部１１は、基台１０に対して旋回部２０を介して水平方向に旋回自在に取付けられる。第１アーム部１２は、胴部１１に対し、第１関節部２１を介して揺動自在に連結される。第２アーム部１３は、第１アーム部１２に対し、第２関節部２２を介して揺動自在に連結される。リスト部１４は、第２アーム部１３に対し、第３関節部２３を介して軸回りに回転自在に、かつ第４関節部２４を介して揺動自在に連結される。そして、リスト部１４の先端部には、用途に応じたエンドエフェクタ（不図示）が適宜連結される。
【００１３】
旋回部２０および第１〜第４関節部２１〜２４には、可動部である胴部１１、第１アーム部１２、第２アーム部１３およびリスト部１４を駆動するアクチュエータ５０が内蔵される。具体的には、アクチュエータ５０は、図１に示すようにモータ２と減速機３とを含んで構成される。
【００１４】
モータ２は、当該モータ２の駆動を制御するモータ制御装置８と電気的に接続され、モータ制御装置８から出力される指令に従って駆動する。減速機３は、モータ２の出力軸に接続され、モータ２の出力軸の回転を減じて第１アーム部１２等の可動部へ伝達する。モータ制御装置８は、たとえばサーボアンプ、サーボアンプを制御するコントローラ、または、サーボアンプとコントローラとを含む制御装置である。
【００１５】
第１の実施形態では、減速機３としてハーモニック減速機が用いられる。ハーモニック減速機は、楕円と真円の差動を利用した減速機（波動歯車装置）である。かかるハーモニック減速機は、モータ２の出力軸が１回転する毎に２回振動するという性質を有するが、かかる点については後述する。
【００１６】
次に、ロボットシステム１００の構成について図２を参照して具体的に説明する。図２は、第１の実施形態に係るロボットシステム１００の構成を示すブロック図である。図２では、第１関節部２１の構成を例に挙げて説明するが、旋回部２０および第２〜第４関節部２２〜２４についても同様の構成である。
【００１７】
図２に示すように、第１関節部２１は、前述のモータ２および減速機３に加えて、トルク検出部４と、速度検出部５と、位置検出器９と、外力推定器３０とを備える。
【００１８】
トルク検出部４は、減速機３と負荷（ここでは、第１アーム部１２）との間に設けられ、モータ２が駆動したときのトルク（Ｎ・ｍ）を検出する。
【００１９】
位置検出器９は、たとえばエンコーダであり、モータ２の出力軸の回転位置Ｐｆｂを検出して速度検出部５へ出力する。なお、エンコーダは、絶対値エンコーダであるものとするが、これに限らず、インクリメンタルエンコーダであってもよい。また、エンコーダに代えて、レゾルバ等を位置検出器９として用いてもよい。
【００２０】
速度検出部５は、位置検出器９から入力される回転位置Ｐｆｂを差分演算することによってモータ２の出力軸の回転速度（ｒａｄ／ｓ）を検出する。なお、トルク検出部４によるトルクの検出方法および速度検出部５による回転速度の検出方法については、いずれの公知技術を用いても構わない。
【００２１】
なお、ここでは、モータ２、減速機３、トルク検出部４、速度検出部５および位置検出器９がそれぞれ別体であるものとするが、たとえば減速機一体型モータや、センサ一体型モータ、あるいは、センサ一体型減速機などを採用してもよい。また、モータ２、減速機３、トルク検出部４、速度検出部５および位置検出器９が一体的に構成されたセンサ一体型アクチュエータを採用してもよい。
【００２２】
外力推定器３０は、たとえばロボットシステム１００の例では、第１アーム部１２や第２アーム部１３等に働く外力を推定する。具体的には、外力推定器３０は、外力オブザーバ６と、ノッチフィルタ７とを備える。外力オブザーバ６は、トルク検出部４から出力されるトルク検出値Ｔｆｂと、速度検出部５から出力される速度検出値ｖｆｂとに基づき、モータ２の出力軸回りに加えられた外力トルクを推定する。
【００２３】
ここで、外力オブザーバ６の具体的構成の一例について図３を参照して説明する。図３は、外力オブザーバ６の構成例を示すブロック図である。
【００２４】
図３に示すように、外力オブザーバ６は、非線形フィードバック項算出部６１と、一般化モーメント算出部６２と、減算部６３と、線形オブザーバ６４とを備える。
【００２５】
非線形フィードバック項算出部６１は、回転位置Ｐｆｂおよび速度検出値ｖｆｂを用いて非線形フィードバック項を算出する。ここで、非線形フィードバック項算出部６１によって算出される非線形フィードバック項は、
【数１】
2014155559.tif 000003
式（１）のように表される。ここで、ｑは回転位置Ｐｆｂ相当し、ｄｑ／ｄｔは速度検出値ｖｆｂに相当する。また、Ｃ（ｑ，ｄｑ／ｄｔ）は遠心力およびコリオリ力に関する行列、ｇ（ｑ）は重力項、Ｍ（ｑ）はリンクの質量行列である。非線形フィードバック項算出部６１は、算出した非線形フィードバック項を減算部６３へ出力する。
【００２６】
一般化モーメント算出部６２は、回転位置Ｐｆｂおよび速度検出値ｖｆｂを用いて一般化モーメントｐを算出して線形オブザーバ６４へ出力する。ここで、ｐ＝Ｍ（ｑ）ｄｑ／ｄｔである。
【００２７】
なお、ここでは、非線形フィードバック項算出部６１および一般化モーメント算出部６２が、速度検出部５から取得した速度検出値ｖｆｂから回転位置Ｐｆｂを算出するものとするが、非線形フィードバック項算出部６１および一般化モーメント算出部６２は、位置検出器９から回転位置Ｐｆｂを取得してもよい。
【００２８】
減算部６３は、トルク検出値Ｔｆｂから非線形フィードバック項を減算し、得られた値Ｔ’を線形オブザーバ６４へ出力する。
【００２９】
線形オブザーバ６４は、一般的な線形のオブザーバであり、一般化モーメント算出部６２から入力される一般化モーメントｐと、減算部６３から入力された値Ｔ’とを用いて外力推定値Ｔｄを算出する。
【００３０】
ここで、上述したように、ハーモニック減速機である減速機３は、モータ２の出力軸が１回転する毎に２回振動し、かかる減速機３の振動は、トルク検出部４によってトルクとして検出される。これにより、トルク検出値Ｔｆｂが振動し、かかるトルク検出値Ｔｆｂの振動に伴って外力推定値Ｔｄが振動することとなる。
【００３１】
このように、外力推定値Ｔｄには、モータ２の回転に伴って発生する振動成分、具体的には、モータ２の回転に伴い減速機３が振動することによって生じる振動成分が含まれる。そこで、第１の実施形態に係るロボット１では、かかる振動成分をノッチフィルタ７を用いて減衰させることにより、外力推定値の精度を向上させることとした。
【００３２】
かかるノッチフィルタ７の構成について図４を参照して説明する。図４は、第１の実施形態に係るノッチフィルタ７の構成を示すブロック図である。
【００３３】
図４に示すように、ノッチフィルタ７は、第１の入力部７１と、第２の入力部７２と、フィルタリング部７３と、減衰制御部７４と、出力部７５とを備える。
【００３４】
第１の入力部７１は、外力推定値Ｔｄを入力する。第２の入力部７２は、速度検出値ｖｆｂを入力する。出力部７５は、後述するフィルタリング部７３によって振動成分が減衰された外力推定値Ｔｄ’を出力する。なお、第１の入力部７１、第２の入力部７２および出力部７５は、たとえばポートや端子、ノード等に相当する。
【００３５】
フィルタリング部７３は、第１の入力部７１から入力される外力推定値Ｔｄに含まれる振動成分を減衰させる。ノッチフィルタ７がデジタルフィルタである場合、フィルタリング部７３の伝達関数Ｇ（ｓ）は、
【数２】
2014155559.tif 000004
式（２）のようにあらわされる。ここで、δは振動成分の減衰量（以下、「ノッチ深さ」と記載する）を決めるパラメータ、ζは減衰帯域の幅（以下、「ノッチ幅」と記載する）を決めるパラメータ、ωｎは減衰帯域の中心周波数（以下、「ノッチ中心周波数」と記載する）を決めるパラメータである。
【００３６】
また、ノッチ深さを決めるパラメータであるδは、νをノッチ深さとすると、
【数３】
2014155559.tif 000005
式（３）のようにあらわされる。
【００３７】
減衰制御部７４は、第２の入力部７２から速度検出値ｖｆｂを入力し、入力した速度検出値ｖｆｂに応じてフィルタリング部７３のノッチ中心周波数ωｎを制御する。具体的には、減衰制御部７４は、速度検出値ｖｆｂの増減に応じてフィルタリング部７３のノッチ中心周波数ωｎを増減させる。これにより、モータ２の回転速度に応じて周波数が変動する振動成分をフィルタリング部７３によって適切に減衰させることができる。
【００３８】
この点について図５Ａおよび図５Ｂを参照して説明する。図５Ａおよび図５Ｂは、第１の実施形態に係るノッチフィルタ７の周波数特性を示す図である。図５Ａに示すように、フィルタリング部７３は、入力信号の所定の周波数帯域を減衰させる。ここで、ωｎはノッチ中心周波数であり、νはノッチ深さである。
【００３９】
上述したように、ハーモニック減速機である減速機３は、モータ２の出力軸が１回転する毎に２回振動する、言い換えれば、モータ２の回転速度の２倍の周波数で振動する。したがって、外力推定値Ｔｄに含まれる振動成分は、モータ２の回転速度が速くなるほど高周波となる。
【００４０】
そこで、減衰制御部７４は、図５Ｂに示すように、第２の入力部７２から入力される速度検出値ｖｆｂが高くなるほどフィルタリング部７３のノッチ中心周波数ωｎを高くし、速度検出値ｖｆｂが低くなるほどフィルタリング部７３のノッチ中心周波数ωｎを低くすることとした。具体的には、ノッチ中心周波数ωｎは、ωｎ＝２ｖｆｂである。
【００４１】
このように、第１の実施形態では、減速機３の振動の周波数がモータ２の回転速度に応じて増減することに着目し、速度検出値ｖｆｂに応じてフィルタリング部７３の減衰帯域を移動させることとした。具体的には、減速機３はモータ２の回転速度の２倍の周波数で振動する。このため、減衰制御部７４は、ノッチ中心周波数ωｎを速度検出値ｖｆｂの２倍の周波数に変更することとした。これにより、外力推定値Ｔｄに含まれる振動成分を適切に減衰させることができ、外力推定値の精度を向上させることができる。
【００４２】
さらに、減衰制御部７４は、第２の入力部７２から入力される速度検出値ｖｆｂの増減に応じてノッチ深さνも増減させる。以下、この点について説明する。
【００４３】
上述したように減速機３の振動はモータ２の回転速度が増加するほど高周波となるが、振幅については、モータ２の回転速度に依らずほぼ一定である。これにもかかわらず、第１の実施形態に係るノッチフィルタ７は、モータ２の回転速度が遅いとき、つまり、減速機３の振動が低周波であるときには、ノッチ深さνを浅くする、すなわち、振動成分の減衰量を少なくする。
【００４４】
これは、外力推定値Ｔｄの低い周波数帯域に有用な情報が集中しているためであり、低い周波数帯域において振動成分の減衰量を敢えて少なくすることで、外力推定値Ｔｄに含まれる有用な情報を残しつつ不要な振動成分を減衰させることができる。
【００４５】
具体的には、図５Ｂに示すように、減衰制御部７４は、ノッチ中心周波数ωｎが低くなるほど、つまり、第２の入力部７２から入力される速度検出値ｖｆｂが低くなるほどフィルタリング部７３のノッチ深さνを浅くする。
【００４６】
つづいて、ノッチ深さνの変化のさせ方の一例について図６Ａ〜図６Ｃを参照して説明する。図６Ａ〜図６Ｃは、ノッチ中心周波数ωｎとノッチ深さνとの関係の一例を示す図である。
【００４７】
なお、図６Ａ〜図６Ｃには、ノッチ中心周波数ωｎを横軸に、ノッチ深さνを決めるパラメータであるδを縦軸にとった場合のωｎとδとの関係を示している。上述した式（３）からわかるように、δが１のときにノッチ深さνが０となり、δが０のときにノッチ深さνが無限大となる。
【００４８】
たとえば、図６Ａに示すように、減衰制御部７４は、ωｎ＝０のときにδ＝１とし、ωｎの増加に伴って（すなわち、速度検出値ｖｆｂの増加に伴って）δが曲線的に減少するようにフィルタリング部７３による減衰の減衰量を制御してもよい。図６Ａに示す曲線は、ωｎ＝ω１に変曲点Ｐを有する曲線（シグモイド曲線）であり、ωｎ＜ω１のときには上に凸、ωｎ＞ω１のときには下に凸となる。
【００４９】
なお、減衰制御部７４は、図６Ａに示した曲線に限らず、δが変曲点のない曲線（たとえば指数曲線など）に従って減少するようにフィルタリング部７３による減衰の減衰量を制御してもよい。
【００５０】
また、図６Ｂに示すように、２つの閾値ω２，ω３を設け、ωｎ≦ω２のときにはδ＝１で一定とし、ωｎ≧ω３のときにはδ＝α（＜１）で一定とし、ω２＜ωｎ＜ω３のときには、ωｎの増加に伴ってδが１からαへ直線的に減少するように、減衰制御部７４がフィルタリング部７３の減衰量を制御してもよい。
【００５１】
すなわち、減衰制御部７４は、ω２／２（第１の閾値）以下の速度検出値ｖｆｂが入力された場合には、ノッチ深さνが０となり、かつ、ω３／２（第２の閾値）以上の速度検出値ｖｆｂが入力された場合には、ノッチ深さνが０よりも大きい一定量となるようにフィルタリング部７３の減衰量を制御してもよい。
【００５２】
このように、減衰制御部７４は、所定の閾値（ここではω３／２）以上の速度検出値ｖｆｂが入力された場合のノッチ深さνを一定とする。これは、もともとモータ２の回転に伴う減速機３の振動の振幅がモータ２の回転速度に依らずほぼ一定であるためであり、このようにω３／２以上の回転速度におけるノッチ深さνを一定とすることで、図６Ａに示す場合と比較して処理負荷を軽減することができる。
【００５３】
なお、ここでは、ω２＜ωｎ＜ω３のときに、ωｎの増加に伴ってδを直線的に減少させることとしたが、ωｎの増加に伴ってδを曲線的に減少させてもよい。また、ここでは、閾値を２つとしたが、閾値は３つ以上であってもよい。
【００５４】
また、図６Ｃに示すように、１つの閾値ω４を設け、ωｎ＜ω４のときにはδ＝１（すなわち、ν＝０）で一定となり、ωｎ≧ω４のときにはδ＝αで一定となるように、減衰制御部７４がフィルタリング部７３の減衰量を制御してもよい。
【００５５】
また、たとえば図６Ｂの０≦ωｎ＜ω３の部分を図６Ａに示すような曲線に置き換えてもよい。すなわち、０≦ωｎ＜ω３のときには、δが図６Ａに示すような曲線に従って減少し、ωｎ≧ω３のときにはδ＝αで一定となるように、減衰制御部７４がフィルタリング部７３の減衰量を制御してもよい。
【００５６】
また、図２に示すように、外力推定器３０から出力されるフィルタリング後の外力推定値Ｔｄ’は、モータ制御装置８にフィードバックされる。そして、モータ制御装置８は、かかる外力推定値Ｔｄ’に基づいてトルク指令を補正し、補正したトルク指令Ｔｒｅｆをモータ２へ出力する。
【００５７】
たとえば、モータ制御装置８は、補正前のトルク指令から外力推定値Ｔｄ’を減算したものをトルク指令Ｔｒｅｆとして出力するポジティブフィードバックを行う。あるいは、モータ制御装置８は、外力推定値Ｔｄ’の位相を反転させたうえで、補正前のトルク指令から位相反転後の外力推定値Ｔｄ’を減算するネガティブフィードバックを行ってもよい。これにより、ロボット１の制御を精度良く行うことができる。
【００５８】
上述してきたように、第１の実施形態に係るロボットシステム１００は、ロボット１と、外力オブザーバ６と、ノッチフィルタ７とを備える。ロボット１は、各関節部２１〜２４がモータ２と減速機３とを含んで構成される。外力オブザーバ６は、モータ２のトルク検出値Ｔｆｂと速度検出値ｖｆｂとに基づいて外力推定値Ｔｄを生成する。ノッチフィルタ７は、外力オブザーバ６から出力される外力推定値Ｔｄに含まれる、モータ２の回転に起因する振動成分を減衰させる。また、ノッチフィルタ７は、フィルタリング部７３と減衰制御部７４とを備える。フィルタリング部７３は、外力推定値Ｔｄを取得し、外力推定値Ｔｄに含まれる振動成分の減衰を行う。減衰制御部７４は、モータ２の速度検出値ｖｆｂを取得し、取得した速度検出値ｖｆｂに応じてフィルタリング部７３による減衰の減衰量を制御する。
【００５９】
したがって、第１の実施形態に係るロボットシステム１００によれば、モータ２の回転に伴って発生する振動成分を減衰させることができる。
【００６０】
また、第１の実施形態に係るロボットシステム１００では、フィルタリング部７３が、モータ２の回転に伴って減速機３が発生させる振動成分を含む外力推定値Ｔｄを取得することとした。これにより、モータ２の回転に伴って減速機３が発生させる振動成分を減衰させることができる。
【００６１】
なお、ここでは、モータ２の出力軸が１回転する毎に２回振動する性質を有する減速機３の場合に、ノッチ中心周波数ωｎを速度検出値ｖｆｂの２倍とする場合の例を示した。これと同様に、モータ２の出力軸が１回転する毎にｎ回振動する性質を有する減速機３の場合には、ノッチ中心周波数ωｎを速度検出値ｖｆｂのｎ倍（ｎは２以上の整数）とすればよい。また、上記のｎは、２以上の整数に限定されない。すなわち、減速機３が、モータ２の出力軸が２回転する毎に３回振動する性質やモータ２の出力軸が３回転する毎に１回振動する性質を有する場合には、ノッチ中心周波数ωｎを速度検出値ｖｆｂのそれぞれ３／２倍、１／３倍とすればよい。このように、減衰制御部７４は、ノッチ中心周波数ωｎを速度検出値ｖｆｂに比例した周波数に変更するようにしてもよい。
【００６２】
また、ここでは、減速機３が、モータ２の回転に応じて振動成分を発生させる減速機である場合の例を示したが、減速機３は、モータ２の回転とは無関係に振動成分を発生させる減速機であってもよい。このような場合であっても、減速機３の振動成分がモータ２の回転に応じて変化する場合には、上述してきたノッチフィルタ７を用いて振動成分を適切に減衰させることができる。
【００６３】
（第２の実施形態）
上述した第１の実施形態では、モータ２の回転速度の増減に応じて、ノッチ中心周波数ωｎおよびノッチ深さνの両方を増減させる場合の例について説明したが、ノッチフィルタ７は、ノッチ中心周波数ωｎを固定とし、ノッチ深さνのみを増減させる構成であってもよい。
【００６４】
かかる点について図７Ａおよび図７Ｂを参照して説明する。図７Ａおよび図７Ｂは、第２の実施形態に係るノッチフィルタ７の周波数特性を示す図である。
【００６５】
図７Ａおよび図７Ｂに示すように、第２の実施形態に係るノッチフィルタ７の減衰制御部７４は、第２の入力部７２から入力される速度検出値ｖｆｂが変化した場合に、ノッチ中心周波数ωｎを変化させることなく、ノッチ深さνのみを変化させる。ノッチ深さνは、たとえば所定の係数ｋ（ｋは正の数）を用いてν＝ｋｖｆｂとしてもよいし、速度検出値ｖｆｂを変数とする所定の関数ν＝ｆ（ｖｆｂ）としてもよい。
【００６６】
第１の実施形態では、減速機３が、ハーモニック減速機である場合を例に挙げて説明した。しかし、減速機３がハーモニック減速機以外の減速機である場合、減速機の種類によっては、モータ２の回転速度の増減に伴って振動成分の振幅が増減する場合もある。
【００６７】
このような場合には、第２の実施形態に係るノッチフィルタ７のように、速度検出値ｖｆｂの増減に応じてノッチ深さνを増減させることで、モータ２の回転に伴って発生する振動成分を減衰させることができる。
【００６８】
（第３の実施形態）
ところで、上述してきた各実施形態では、外力推定器３０が旋回部２０および第１〜第４関節部２１〜２４に設けられる場合の例を示したが、外力推定器３０は、たとえばモータ制御装置８に設けられてもよい。以下では、モータ制御装置が、外力推定器３０に相当する処理部を備える場合の例について図８を参照して説明する。図８は、第３の実施形態に係るロボットシステムの構成を示すブロック図である。
【００６９】
図８に示すように、第３の実施形態に係るロボットシステム１００Ａにおいて、第１関節部２１Ａは、第１および第２の実施形態に係る第１関節部２１から外力推定器３０を除いた構成を有する。他の関節部についても同様に、外力推定器３０を除いた構成を有する。
【００７０】
第３の実施形態に係るモータ制御装置８Ａは、外力推定部３０Ａと、制御部８１とを備える。外力推定部３０Ａは、外力推定器３０に相当する処理部であり、外力推定器３０と同様、外力オブザーバ６とノッチフィルタ７とを備える。なお、モータ制御装置８Ａは、各関節部に対応する複数の外力推定部３０Ａを備えるが、ここでは、第１関節部２１Ａに対応する外力推定部３０Ａのみを示している。
【００７１】
トルク検出値Ｔｆｂおよび速度検出値ｖｆｂは、モータ制御装置８Ａに設けられた外力推定部３０Ａに入力される。具体的には、トルク検出値Ｔｆｂは外力オブザーバ６に入力され、速度検出値ｖｆｂは、外力オブザーバ６およびノッチフィルタ７にそれぞれ入力される。
【００７２】
外力推定部３０Ａでは、上述した外力推定器３０と同様、外力オブザーバ６が、トルク検出値Ｔｆｂおよび速度検出値ｖｆｂに基づいて外力推定値Ｔｄを生成してノッチフィルタ７へ出力し、ノッチフィルタ７が、外力推定値Ｔｄから振動成分を減衰させて、外力推定値Ｔｄ’を制御部８１へ出力する。ノッチフィルタ７は、第１および第２の実施形態において説明した通り、フィルタリング部７３および減衰制御部７４を備え（図４参照）、減衰制御部７４が速度検出値ｖｆｂに応じてフィルタリング部７３のノッチ中心周波数ωｎやノッチ深さνを変更する。これにより、外力推定値Ｔｄに含まれる、モータ２の回転に伴って発生する振動成分を減衰させることができる。
【００７３】
そして、制御部８１は、外力推定部３０Ａから入力される外力推定値Ｔｄ’に基づいてトルク指令を補正し、補正したトルク指令Ｔｒｅｆをモータ２へ出力する。
【００７４】
このように、フィルタリング部７３および減衰制御部７４は、モータ制御装置８Ａに設けられていてもよい。
【００７５】
また、上述してきた各実施形態では、ノッチフィルタ７を外力推定器３０や外力推定部３０Ａに設ける場合の例について説明したが、ノッチフィルタ７は、外力オブザーバ６と別体で、図２に示す制御ループ内の任意の場所に設けられてもよい。
【００７６】
また、ノッチフィルタ７に入力される入力信号は、モータ２の回転に伴って発生する振動成分を含んだ信号であればよく、外力推定値Ｔｄに限定されない。たとえば、ノッチフィルタ７をトルク検出部４の後段に設けて、トルク検出値Ｔｆｂに含まれる振動成分をノッチフィルタ７によって減衰させるようにしてもよい。
【００７７】
（第４の実施形態）
次に、第４の実施形態に係るロボットシステムの構成について図９を参照して説明する。図９は、第４の実施形態に係るロボットシステムの構成を示すブロック図である。
【００７８】
図９に示すように、第４の実施形態に係るロボットシステム１００Ｂが備える第１関節部２１Ｂは、第１の実施形態に係る第１関節部２１（図２参照）から減速機３およびトルク検出部４を除外した構成を有する。
【００７９】
上述してきた各実施形態では、減速機３が振動成分を発生させる場合の例を示したが、振動成分は、減速機３だけではなく、たとえばモータ２自体の構造等に起因して発生する場合もある。つまり、第４の実施形態に係るロボットシステム１００Ｂのように減速機３を有していない系においても、モータ２の回転に伴って発生する振動成分が外力推定値Ｔｄに含まれる可能性がある。したがって、このような系に対してロボットシステム１００Ｂを適用した場合であっても、モータ２の回転に伴って発生する振動成分を減衰させることができる。
【００８０】
また、第４の実施形態に係る外力オブザーバ６Ｂは、上述してきた外力オブザーバ６とは異なり、モータ制御装置８から出力されるトルク指令Ｔｒｅfを用いて外力推定値Ｔｄを推定する。かかる場合、外力オブザーバ６Ｂは、第１アーム部１２等に働く外力、摩擦力およびその他の力の総和、すなわち、外乱を「外力」として推定する。
【００８１】
ここで、第４の実施形態に係る外力オブザーバ６Ｂの構成例について図１０を参照して説明する。図１０は、第４の実施形態に係る外力オブザーバ６Ｂの構成例を示すブロック図である。図１０に示すように、外力オブザーバ６Ｂは、微分器６５と、慣性モーメント乗算部６６と、減算部６７と、ローパスフィルタ６８とを備える。
【００８２】
微分器６５は、速度検出値ｖｆｂを微分することによって加速度検出値Ａｆｂを算出し、算出した加速度検出値Ａｆｂを慣性モーメント乗算部６６へ出力する。慣性モーメント乗算部６６は、微分器６５から入力された加速度検出値Ａｆｂに対してモータ軸周りの慣性モーメントを乗算することによって加速トルク検出値ＴＡｆｂを算出し、算出した加速トルク検出値ＴＡｆｂを減算部６７へ出力する。
【００８３】
減算部６７は、加速トルク検出値ＴＡｆｂからトルク指令Ｔｒｅfを減算し、得られた値Ｔ’’をローパスフィルタ６８へ出力する。ローパスフィルタ６８は、Ｔ’’にローパスフィルタをかけた値を外力推定値Ｔｄとして出力する。
【００８４】
このように、外力オブザーバ６Ｂは、トルク検出値Ｔｆｂに代えてトルク指令Ｔｒｅfを用いて外力推定値Ｔｄを算出してもよい。
【００８５】
なお、ここでは、外力推定器３０Ｂが第１関節部２１Ｂに設けられる場合の例を示したが、第３の実施形態と同様、外力推定器３０Ｂに代えて、外力推定器３０Ｂに相当する処理部をモータ制御装置８に設けてもよい。
【００８６】
（第５の実施形態）
次に、第５の実施形態に係るロボットシステムの構成について図１１を参照して説明する。図１１は、第５の実施形態に係るロボットシステムの構成を示すブロック図である。
【００８７】
図１１に示すように、第５の実施形態に係るロボットシステム１００Ｃは、ノッチフィルタ７Ｃ１，７Ｃ２をさらに備える。ノッチフィルタ７Ｃ１は、第１関節部２１Ｃのノッチフィルタ７の後段に設けられ、ノッチフィルタ７Ｃ２は、第２関節部２２Ｃのノッチフィルタ７の後段に設けられる。
【００８８】
また、第１関節部２１Ｃおよび第２関節部２２Ｃは、上述した第１の実施形態に係る第１関節部２１と同様の構成である。以下では、第１関節部２１Ｃについてのトルク指令、回転位置、トルク検出値、速度検出値、外力推定値をそれぞれ「Ｔｒｅｆ＿１」、「Ｐｆｂ＿１」、「Ｔｆｂ＿１」、「ｖｆｂ＿１」、「Ｔｄ＿１（Ｔｄ＿１’）」とする。また、第２関節部２２Ｃについては、それぞれ「Ｔｒｅｆ＿２」、「Ｐｆｂ＿２」、「Ｔｆｂ＿２」、「ｖｆｂ＿２」、「Ｔｄ＿２（Ｔｄ＿２’）」とする。
【００８９】
ここで、第１関節部２１Ｃの信号には、ロボットシステム１００Ｃ内の他の系（たとえば、第２関節部２２Ｃ）において発生した振動成分も含まれ得る。第２関節部２２Ｃにおいても同様である。
【００９０】
そこで、第５の実施形態に係るロボットシステム１００Ｃでは、ノッチフィルタ７Ｃ１，７Ｃ２をさらに設け、他の系において発生した振動成分をノッチフィルタ７Ｃ１，７Ｃ２によって減衰させることとした。
【００９１】
たとえば、ノッチフィルタ７Ｃ１には、第１関節部２１Ｃのノッチフィルタ７から出力される外力推定値、すなわち、第１関節部２１Ｃの減速機３による振動成分が減衰された外力推定値と、第２関節部２２Ｃの速度検出部５から出力される速度検出値ｖｆｂ＿２とが入力される。そして、ノッチフィルタ７Ｃ１は、速度検出値ｖｆｂ＿２に応じたノッチ中心周波数ωｎおよびノッチ深さνで、第１関節部２１Ｃのノッチフィルタ７から出力される外力推定値をフィルタリングする。これにより、第１関節部２１Ｃのノッチフィルタ７から出力される外力推定値に含まれる、第２関節部２２Ｃにおいて発生した振動成分を減衰させることができる。フィルタリング後の外力推定値Ｔｄ＿１’は、モータ制御装置８へ出力される。
【００９２】
また、ノッチフィルタ７Ｃ２には、第２関節部２２Ｃのノッチフィルタ７から出力される外力推定値、すなわち、第２関節部２２Ｃの減速機３による振動成分が減衰された外力推定値と、第１関節部２１Ｃの速度検出部５から出力される速度検出値ｖｆｂ＿１とが入力される。そして、ノッチフィルタ７Ｃ２は、速度検出値ｖｆｂ＿１に応じたノッチ中心周波数ωｎおよびノッチ深さνで、第２関節部２２Ｃのノッチフィルタ７から出力される外力推定値をフィルタリングする。これにより、第２関節部２２Ｃのノッチフィルタ７から出力される外力推定値に含まれる、第１関節部２１Ｃにおいて発生した振動成分を減衰させることができる。フィルタリング後の外力推定値Ｔｄ＿２’は、モータ制御装置８へ出力される。
【００９３】
このように、第５の実施形態に係るロボットシステム１００Ｃによれば、ノッチフィルタ７Ｃ１，７Ｃ２を設けることで、他の系において発生した振動成分も減衰させることができる。
【００９４】
なお、ここでは、第１関節部２１Ｃのノッチフィルタ７の後段に、第２関節部２２Ｃにおいて発生した振動成分を減衰させるためのノッチフィルタ７Ｃ１を設ける場合の例を示した。しかし、これに限ったものではなく、ノッチフィルタ７の後段には、ノッチフィルタ７Ｃ１に加え、第２関節部２２Ｃ以外の関節部において発生した振動成分を減衰させるノッチフィルタがさらに設けられてもよい。
【００９５】
また、ここでは、ノッチフィルタ７Ｃ１，７Ｃ２が、たとえば第１関節部２１Ｃおよび第２関節部２２Ｃの外部に設けられる場合の例を示したが、ノッチフィルタ７Ｃ１，７Ｃ２は、それぞれ第１関節部２１Ｃおよび第２関節部２２Ｃの内部に設けられてもよいし、モータ制御装置８の内部に設けられてもよい。
【００９６】
また、第３の実施形態と同様、外力推定器３０を第１関節部２１Ｃおよび第２関節部２２Ｃから除外し、外力推定器３０に相当する処理部をモータ制御装置８に設けてもよい。
【００９７】
また、第４の実施形態と同様、外力オブザーバ６は、トルク検出値Ｔｆｂに代えてトルク指令Ｔｒｅfを用いて外力推定値Ｔｄを算出してもよい。
【００９８】
また、モータ２は、回転型に限らず直動型のリニアモータであってもよい。かかる場合、並進力が上述したトルクに相当し、並進速度が上述した回転速度に相当する。また、モータ２は、電気モータに限らず流体圧アクチュエータなどであってもよい。
【００９９】
また、上述してきた各実施形態では、外力推定器３０をロボット１に適用した例について説明したが、外力推定器３０が適用されるロボットの構成は、図１に示したものに限定されない。また、外力推定器３０は、ロボット１に限らず、モータ２によって駆動されるあらゆるものに適用することができる。
【０１００】
さらなる効果や変形例は、当業者によって容易に導き出すことができる。このため、本発明のより広範な態様は、以上のように表しかつ記述した特定の詳細および代表的な実施形態に限定されるものではない。したがって、添付の特許請求の範囲およびその均等物によって定義される総括的な発明の概念の精神または範囲から逸脱することなく、様々な変更が可能である。
【符号の説明】
【０１０１】
１  ロボット
２  モータ
３  減速機
４  トルク検出部
５  速度検出部
６  外力オブザーバ
７  ノッチフィルタ
８  モータ制御装置
１０  基台
１１  胴部
１２  第１アーム部
１３  第２アーム部
１４  リスト部
２０  旋回部
２１〜２４  第１〜第４関節部
３０  外力推定器
７１  第１の入力部
７２  第２の入力部
７３  フィルタリング部
７４  減衰制御部
７５  出力部
１００  ロボットシステム
【図１】
2014155559.tif 000006
【図２】
2014155559.tif 000007
【図３】
2014155559.tif 000008
【図４】
2014155559.tif 000009
【図５Ａ】
2014155559.tif 000010
【図５Ｂ】
2014155559.tif 000011
【図６Ａ】
2014155559.tif 000012
【図６Ｂ】
2014155559.tif 000013
【図６Ｃ】
2014155559.tif 000014
【図７Ａ】
2014155559.tif 000015
【図７Ｂ】
2014155559.tif 000016
【図８】
2014155559.tif 000017
【図９】
2014155559.tif 000018
【図１０】
2014155559.tif 000019
【図１１】
2014155559.tif 000020
【国際調査報告】
2014155559.tif 000021
2014155559.tif 000022
2014155559.tif 000023
2014155559.tif 000024
2014155559.tif 000025
2014155559.tif 000026