JP2011056622A

JP2011056622A - ロボット

Info

Publication number: JP2011056622A
Application number: JP2009208986A
Authority: JP
Inventors: Masaki Motoyoshi; 正樹元吉
Original assignee: Seiko Epson Corp
Current assignee: Seiko Epson Corp
Priority date: 2009-09-10
Filing date: 2009-09-10
Publication date: 2011-03-24
Anticipated expiration: 2029-09-10
Also published as: JP5353581B2

Abstract

【課題】角速度センサーのバイアスドリフトの影響を除去するとともに外乱等によるノイズの影響を除去し、高精度な制御を行うことが可能なロボットを提供する。
【解決手段】基体に対して回転運動可能なリンクと、リンクを駆動するアクチュエーターと、アクチュエーターのトルクをリンクに所定の減速比で伝達するトルク伝達機構と、アクチュエーターの回転角度を検出する角度センサーと、基体に対する前記リンクの角速度を検出する角速度センサーと、所定の減速比と回転角度との積のうちカットオフ周波数以下の低周波数成分及び角速度の積分値のうちカットオフ周波数以上の高周波数成分を用いるとともに高周波数成分において特定の周波数特性を有するフィルタを用いてリンクの角度を算出する演算部と、を備える。
【選択図】図２

Description

本発明は、ロボットに関するものである。

多リンク構造を有するロボットには、各リンクを駆動させるアクチュエーター（例えば、電動モーター）と、各アクチュエーターの回転角度を検出する角度センサー（例えば、エンコーダ）とを備えて構成されている。従来、アクチュエーターの回転角度を制御して、間接的に各リンクの回転角度を制御する技術が用いられている。しかしながら、アクチュエーターのトルクをリンクへ伝達するトルク伝達機構は、一般に剛体ではなくバネ特性を有する。よって、多リンク機構を有するロボットにおいては、アクチュエーターやリンクが機械的な共振特性を有し、互いに振動してしまうという問題がある。

このような問題点を解決するための技術が各種開発されており、例えば、特許文献１では、２次モード以上の高周波数成分を除去し１次モード周波数成分及び直流成分を通過させるローパスフィルタを用いている。すなわち、ローパスフィルタにより２次モード以上の加速度信号をカットし、２次モード以上の振動が励起されてアームが振動しまうことを抑制してアームの正確な位置決めを可能にしている。
また、特許文献２では、ワークの掴み部（チャック部）近傍に慣性センサーを設けてチャック部の動作（振れ状態）を検出している。そして、慣性センサー及び各アームの関節となる駆動体に接続された制御回路に指令信号が入力されることにより、各アームの駆動を制御してチャック部の振れの制振を行っている。

これら特許文献１及び２では、アクチュエーターやリンクが互いに振動することを防止することができるが、慣性センサーのバイアスドリフトの影響によって速度偏差や位置偏差などの定常偏差（オフセット）が生じる問題がある。これに対して、特許文献３では、角速度の積分値のうちカットオフ周波数以上の高周波数成分と、アクチュエーターの回転角度とトルク伝達機構の所定の減速比との積のうちカットオフ周波数以下の低周波数成分とを用いることで、角速度センサーのバイアスドリフトの影響を除去している。

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

しかしながら、特許文献１〜３では、慣性センサーのバイアスドリフトの影響以外のその他外乱等によるノイズの影響が考慮されていない。このため、外乱等によるノイズの影響によって定常偏差（オフセット）が生じ、高精度な制御を行うことができなくなる。

本発明はこのような事情に鑑みてなされたものであって、角速度センサーのバイアスドリフトの影響を除去するとともに外乱等によるノイズの影響を除去し、高精度な制御を行うことが可能なロボットを提供することを目的とする。

上記の課題を解決するため、本発明のロボットは、基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに所定の減速比で伝達するトルク伝達機構と、前記アクチュエーターの回転角度を検出する角度センサーと、前記基体に対する前記リンクの角速度を検出する角速度センサーと、所定の減速比と前記回転角度との積のうちカットオフ周波数以下の低周波数成分及び前記角速度の積分値のうち前記カットオフ周波数以上の高周波数成分を用いるとともに前記高周波数成分において特定の周波数特性を有するフィルタを用いて前記リンクの角度を算出する演算部と、を備えることを特徴とする。

本発明のロボットによれば、角速度の積分値のうちカットオフ周波数以上の高周波数成分と、アクチュエーターの回転角度とトルク伝達機構の所定の減速比との積のうちカットオフ周波数以下の低周波数成分とを用いているので、角速度センサーのバイアスドリフトの影響を除去することができる。また、角速度の積分値のうちカットオフ周波数以上の高周波数成分において特定の周波数特性を有するフィルタを用いているので、外乱等によるノイズの影響を除去することができる。したがって、角速度センサーのバイアスドリフトの影響を除去するとともに外乱等によるノイズの影響を除去し、高精度な制御を行うことが可能なロボットが提供できる。

また、このロボットにおいては、前記フィルタが前記高周波数成分において前記リンクと前記アクチュエーターと前記トルク伝達機構とからなる系の固有周波数のうち少なくとも最も低い周波数の固有周波数よりも高い周波数を減衰させる特性を有していてもよい。

この構成によれば、リンク、アクチュエーター、トルク伝達機構からなる系の固有周波数のうち少なくとも最も低い周波数の固有周波数よりも高い周波数が減衰されるので、制御に必要な系の固有周波数帯の信号をとりつつ、制御に必要のない外乱等によるノイズの影響の大きい周波数成分を除去することができる。したがって、ノイズの影響の少ない高精度の制御を行うことができる。

また、このロボットにおいては、前記フィルタが前記高周波数成分において前記系の固有周波数のうち少なくとも測定対象となる周波数の固有周波数のゲインが相対的に大きい特性を有していてもよい。

この構成によれば、リンク、アクチュエーター、トルク伝達機構からなる系の固有周波数のうち少なくとも測定対象となる固有周波数のゲインが相対的に大きいので、系の測定したい部分においてセンサーの感度を高める効果があり、リンクやアクチュエーターの振動を強調して測定を行うことが可能となる。したがって、強調して測定された振動のデータを基に制御を行うことで、リンクやアクチュエーターの振動を確実に抑制することができる。

また、このロボットにおいては、前記フィルタが前記高周波数成分において電気的な外乱ノイズのうち少なくとも最もエネルギー強度の大きい周波数の外乱ノイズのゲインが相対的に小さい特性を有していてもよい。

この構成によれば、電気的な外乱ノイズのうち少なくとも最もエネルギー強度の大きい周波数の外乱ノイズのゲインが相対的に小さくなる。このため、センサーで検出したくない部分においてセンサーの感度が低くされるので、外乱ノイズなどの制御に必要のない信号を確実に除去することができる。したがって、格段に高精度な制御を行うことが可能となる。

また、このロボットにおいては、前記フィルタが前記高周波数成分において外部からの物理的な振動のうち少なくとも最もエネルギー強度の大きい周波数の物理的な振動のゲインが相対的に小さい特性を有していてもよい。

この構成によれば、外部からの物理的な振動のうち少なくとも最もエネルギー強度の大きい周波数の物理的な振動のゲインが相対的に小さくなる。このため、センサーで検出したくない部分においてセンサーの感度が低くされる効果があるので、物理的な振動などの制御に必要のない信号を確実に除去することが可能となる。したがって、格段に高精度な制御を行うことが可能となる。

また、このロボットにおいては、前記フィルタが前記高周波数成分において前記ロボットの筐体または設置台の固有周波数のうち少なくとも最もエネルギー強度の大きい周波数の固有周波数のゲインが相対的に小さい特性を有していてもよい。

この構成によれば、ロボットの筐体または設置台の固有周波数のうち少なくとも最もエネルギー強度の大きい周波数の固有周波数のゲインが相対的に小さくなる。このため、センサーで検出したくない部分においてセンサーの感度が低くされる効果があるので、筐体または設置台の固有周波数などの制御に必要のない信号を確実に除去することが可能となる。したがって、格段に高精度な制御を行うことが可能となる。

また、このロボットにおいては、前記角速度センサーに振動型ジャイロを用いる場合であって、前記フィルタが前記高周波数成分において前記振動型ジャイロの駆動周波数、検出周波数、離調周波数、または素子の固有周波数のうち少なくとも最もエネルギー強度の大きい周波数の駆動周波数、検出周波数、離調周波数、または素子の固有周波数のゲインが相対的に小さい特性を有していてもよい。

この構成によれば、角速度センサーに振動型ジャイロを用いる場合、振動型ジャイロの駆動周波数、検出周波数、離調周波数、または素子の固有周波数のうち少なくとも最もエネルギー強度の大きい周波数の駆動周波数、検出周波数、離調周波数、または素子の固有周波数のゲインが相対的に小さくなる。このため、センサーで検出したくない部分においてセンサーの感度が低くされる効果があるので、振動型ジャイロの駆動周波数、検出周波数、離調周波数、または素子の固有周波数などの制御に必要のない信号を確実に除去することが可能となる。したがって、格段に高精度な制御を行うことが可能となる。

本発明に係るロボットを模式的に示す図である。リンクの角度を算出する過程を示すフローチャート図である。本発明の第１実施形態に係るフィルタの周波数特性を示すグラフである。本発明の第２実施形態に係るフィルタの周波数特性を示すグラフである。本発明の第３実施形態に係るフィルタの周波数特性を示すグラフである。図５に続くフィルタの周波数特性を示すグラフである。リンクの角度を算出する過程の変形例を示すフローチャート図である。

以下、図面を参照して、本発明の実施の形態について説明する。かかる実施の形態は、本発明の一態様を示すものであり、この発明を限定するものではなく、本発明の技術的思想の範囲内で任意に変更可能である。また、以下の図面においては、各構成をわかりやすくするために、実際の構造と各構造における縮尺や数等が異なっている。

(第１実施形態)
図１は、本発明の第１実施形態における角速度センサー１２，１３が設けられたロボット１００の模式断面図である。図１に示すように、ロボット１００は、架台（基体）１と、リンク３，８と、モーター（アクチュエーター）４，９と、減速機（トルク伝達機構）５，１１と、角度センサー６，１０と、角速度センサー１２，１３と、演算部１５と、を備えて構成されている。

架台１の上部にはリンク３の一端が取り付けられており、リンク３の他端にはリンク８の一端が取り付けられている。リンク３は、軸２を中心として架台１に対して相対的に回転運動可能になっている。リンク８は、軸７を中心としてリンク３に対して相対的に回転運動可能になっている。

モーター４は、減速機５を介してリンク３を駆動するものである。モーター９は、減速機１１を介してリンク８を駆動するものである。ここで、減速機５における所定の減速比をＮ１、減速機１１における所定の減速比をＮ２とする。

角度センサー６は、モーター４の頂部に設けられている。角度センサー６は、軸２の回りにおけるモーター４の回転角度Θｍ１を検出する機能を有する。角度センサー１０は、モーター９の頂部に設けられている。角度センサー１０は、軸７の回りにおけるモーター９の回転角度Θｍ２を検出する機能を有する。

角速度センサー１２は、リンク３の他端に設けられている。角速度センサー１２は、架台１に対するリンク３の角速度ｗ１を検出する機能を有する。角速度センサー１３は、リンク８の他端に設けられている。角速度センサー１３は、架台１に対するリンク８の角速度ｗ２を検出する機能を有する。なお、架台１に対するリンク３の角速度ｗ１は、リンク３の回転面内の角速度成分である。また、架台１に対するリンク８の角速度ｗ２は、リンク８の回転面内の角速度成分である。

演算部１５は、架台１の近傍に設けられている。演算部１５は、モーター４，９の回転角度Θｍ１，Θｍ２と所定の減速比Ｎ１,Ｎ２との積Θｍ１・Ｎ１,Θｍ２・Ｎ２のうちカットオフ周波数以下の低周波数成分及び角速度ｗ１,（ｗ２−ｗ１）の積分値のうちカットオフ周波数以上の高周波数成分を用いるとともに高周波数成分において特定の周波数特性を有するフィルタを用いてリンクの角度ΘＡ１,ΘＡ２を算出する機能を有する。

なお、本実施形態においては、リンク、モーター、減速機、角度センサー、角速度センサーがそれぞれ２つ設けられているが、これに限らない。例えば、リンク、モーター、減速機、角度センサー、角速度センサーがそれぞれ１つずつ設けられていてもよく、あるいは３つ以上など任意の数が設けられていてもよい。

また、角速度センサー１２，１３は、それぞれリンク３，８の先端に配置されているが、これに限らない。例えば、角速度センサー１２，１３は、それぞれリンク３，８の任意の位置に配置されていてもよい。また、演算部１５は、架台１の近傍に配置されているが、これに限らない。例えば、演算部１５は、ロボット１００の外部など任意の位置に配置されていてもよい。

図２は、リンク３の角度ΘＡ１、リンク８の角度ΘＡ２を算出する過程を示すフローチャート図である。ステップＳ１及びＳ６以外の演算処理は、ロボット１００内に配置された演算部１５が実行してもよく、または、ロボット１００の外部に設けられた演算部１５が実行してもよい。

先ず、角速度センサー１２によって架台１に対するリンク３の角速度ｗ１が検出される。また、角速度センサー１３によって架台１に対するリンク８の角速度ｗ２が検出される（Ｓ１）。

次に、演算部１５によって、リンク３の単独の角速度ｄΘＡ１／ｄｔが算出される。また、演算部１５によってリンク８の単独の角速度ｄΘＡ２／ｄｔが算出される。角速度ｗ１及びｗ２は、それぞれ架台１に対するリンク３及び８の角速度であるので、角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔは、以下の式（１）のように表すことができる。

ｄΘＡ１／ｄｔ＝ｗ１, ｄΘＡ２／ｄｔ＝ｗ２−ｗ１ ……（１）

次に、演算部１５によって、リンクの角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔが積分される（Ｓ３）。

次に、これらの積分結果がハイパスフィルタＦＨに通過されるとともに特定の周波数成分を有するフィルタに通過される（Ｓ４）。これにより、リンクの角度ΘＡ１及びΘＡ２のうちカットオフ周波数１／Ｔｆ以上の高周波数成分が得られる。

ここで、リンクの角度ΘＡ１及びΘＡ２のうち低周波数成分を除去する理由は、リンクの角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔの積分ではバイアスドリフトの影響によりリンクの角度ΘＡ１及びΘＡ２を正確に求めることができないので、このバイアスドリフトの影響を除去するためである。また、リンクの角度ΘＡ１及びΘＡ２が特定の周波数成分を有するフィルタに通過される理由は、リンクの角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔの積分では外乱等によるノイズの影響によりリンクの角度ΘＡ１及びΘＡ２を正確に求めることができないので、この外乱等によるノイズの影響を除去するためである。

一方、ロボット１００が停止あるいは極低周波帯域で動作している場合には、減速機５,１１はほとんど捩れないので以下の式（２）が成り立つ。

ΘＡ１＝Θｍ１・Ｎ１,ΘＡ２＝Θｍ２・Ｎ２ ……（２）

そこで、角度センサー６によってモーター角度Θｍ１が検出される。また、角度センサー１０によってモーター角度Θｍ２が検出される（Ｓ６）。

次に、演算部１５によって、所定の減速比Ｎ１（Ｎ１＜１）がモーター角度Θｍ１に乗算され、Θｍ１・Ｎ１が算出される。また、演算部１５によって、所定の減速比Ｎ２（Ｎ２＜１）がモーター角度Θｍ２に乗算され、Θｍ２・Ｎ２が算出される（Ｓ７）。

次に、これらの算出結果がローパスフィルタＦＬに通過される（Ｓ８）。これにより、リンクの角度ΘＡ１及びΘＡ２のうちカットオフ周波数１／Ｔｆ以下の低周波数成分が得られる。ここで、リンクの角度ΘＡ１及びΘＡ２の高周波数成分を除去する理由は、高周波数成分では減速機５,１１が捩れるため式（２）が成立しなくなるからである。

ハイパスフィルタＦＨのフィルタ係数をＨＦとし、ローパスフィルタＦＬのフィルタ係数をＬＦとすると、以下の式（３）が成り立つ。

ＨＦ＝１−ＬＦ ……（３）

このように、式（３）の関係を有するハイパスフィルタＦＨ及びローパスフィルタＦＬを用いることによって、ステップＳ１〜Ｓ４により得られたリンクの角度ΘＡ１及びΘＡ２の高周波数成分と、ステップＳ６〜Ｓ８により得られたリンクの角度ΘＡ１及びΘＡ２の低周波数成分と、を加算することができる（Ｓ１０）。この演算によって、架台１に対するリンク３の角度ΘＡ１及びリンク３に対するリンク８の角度ΘＡ２を算出することができる。

さらに、ステップＳ２において得られた角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔを１回近似微分することによって、リンク３及び８の角加速度ｄ^２ΘＡ１／ｄｔ^２及びｄ^２ΘＡ２／ｄｔ^２を算出することができる（Ｓ５）。また、ステップＳ６において得られたモーター角度Θｍ１及びΘｍ２を１回近似微分することによって、モーター４及び１０のモーター角速度ｄΘｍ１／ｄｔ及びｄΘｍ２／ｄｔを算出することができる（Ｓ９）。

このように、１回積分または１回微分によって、モーター角度Θｍ１及びΘｍ２、モーター角速度ｄΘｍ１／ｄｔ及びｄΘｍ２／ｄｔ、リンクの角度ΘＡ１及びΘＡ２、リンクの角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔ、リンクの角加速度ｄ^２ΘＡ１／ｄｔ^２及びｄ^２ΘＡ２／ｄｔ^２を求めることができる。すなわち、バイアスドリフトの影響を受けやすくなる２回積分または２回微分を必要としないので、正確な制御を行うことができる。

図３は、本実施形態に係るフィルタの周波数特性を示すグラフである。図３（ａ）は、リンクとモーターと減速機とからなる系の応答特性、図３（ｂ）は、本実施形態に係るフィルタ特性を示している。図３は、横軸に周波数、縦軸にゲインを示したグラフである。図３において、符号ｆ０はリンクとモーターと減速機とからなる系の固有周波数帯のうちの測定対象部、符号ｆ１は第１カットオフ周波数、符号ｆ２は第２カットオフ周波数である。なお、第１カットオフ周波数ｆ１及び第２カットオフ周波数ｆ２はｆ１＜ｆ２の関係を有している。

図３（ａ）に示すように、リンクとモーターと減速機とからなる系の固有周波数が３箇所に生じている。図３（ｂ）に示すように、本実施形態に係るフィルタは、３箇所に生じている系の固有周波数のうち少なくとも最も低い周波数の固有周波数よりも高い周波数を減衰させる特性を有する。これにより、振動への寄与の大きい最も低い固有振動数を測定しつつ、制御に必要のない外乱等によるノイズの影響の大きい周波数成分を除去することができる。

また、第１カットオフ周波数ｆ１と第２カットオフ周波数ｆ２との間の帯域には、制御に必要のない外乱等によるノイズの影響が除かれた系の固有周波数帯となっている。測定対象部ｆ０は第１カットオフ周波数ｆ１と第２カットオフ周波数ｆ２との間の帯域に含まれている。このため、第１カットオフ周波数ｆ１と第２カットオフ周波数ｆ２との間において、制御に必要な系の固有周波数帯の信号をとることができる。

本発明のロボット１００によれば、リンクの角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔの積分値のうちカットオフ周波数１／Ｔｆ以上の高周波数成分と、モーターの回転角度と減速機の所定の減速比との積Θｍ１・Ｎ１及びΘｍ２・Ｎ２のうちカットオフ周波数１／Ｔｆ以下の低周波数成分とを用いているので、角速度センサー１２,１３のバイアスドリフトの影響を除去することができる。また、リンクの角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔの積分値のうちカットオフ周波数１／Ｔｆ以上の高周波数成分において特定の周波数特性を有するフィルタを用いているので、外乱等によるノイズの影響を除去することができる。したがって、角速度センサー１２,１３のバイアスドリフトの影響を除去するとともに外乱等によるノイズの影響を除去し、高精度な制御を行うことが可能なロボット１００が提供できる。

また、この構成によれば、リンクとモーターと減速機とからなる系の固有周波数のうち少なくとも最も低い周波数の固有周波数よりも高い周波数が減衰されるので、制御に必要な系の固有周波数帯の信号をとりつつ、制御に必要のない外乱等によるノイズの影響の大きい周波数成分を除去することができる。したがって、ノイズの影響の少ない高精度の制御を行うことができる。

（第２実施形態）
図４は、本発明の第２実施形態に係るフィルタの周波数特性を示すグラフである。図４は、図３（ｂ）に対応した、第２実施形態におけるフィルタ特性を示したグラフである。図４に示すように、本実施形態のフィルタは特定の帯域においてゲインを大きくする特性を有する点で、上述の第１実施形態で説明したフィルタと異なっている。その他の点は第１実施形態と同様であるので、図３（ｂ）と同様の要素には同一の符号を付し、詳細な説明は省略する。

図４に示すように、本実施形態に係るフィルタは、第１カットオフ周波数ｆ１以上の高周波数成分において系の固有周波数のうち少なくとも測定対象となる周波数の固有周波数のゲインが相対的に大きい特性を有する。言い換えると、フィルタは第１カットオフ周波数ｆ１以上の高周波数成分における特定の帯域においてゲインを相対的に大きくし、このゲインが相対的に大きくなった帯域に測定対象部ｆ０が含まれるようにしている。なお、特定の帯域においてゲインを大きくすることは、特定の帯域でセンサーの感度を高くすることと等価である。

本実施形態の構成によれば、系の固有周波数のうち少なくとも測定対象となる固有周波数のゲインが相対的に大きいので、系の測定したい部分においてセンサーの感度を高める効果があるため、リンク３,８やモーター４,１０の振動を強調して測定を行うことが可能となる。したがって、強調して測定された振動のデータを基に制御を行うことで、リンク３,８やモーター４,１０の振動を確実に抑制することができる。

（第３実施形態）
図５及び図６は、本発明の第３実施形態に係るフィルタの周波数特性を示すグラフである。図５（ａ）は、ノイズのエネルギー強度、図５（ｂ）は、本実施形態に係るフィルタ特性を示している。図５（ｂ）及び図６は、図３（ｂ）に対応した、第３実施形態におけるフィルタ特性を示したグラフである。図５及び図６に示すように、本実施形態のフィルタは特定の帯域においてゲインを小さくする特性を有する点で、上述の第１実施形態で説明したフィルタと異なっている。その他の点は第１実施形態と同様であるので、図３（ｂ）と同様の要素には同一の符号を付し、詳細な説明は省略する。

図５（ｂ）及び図６に示すように、本実施形態に係るフィルタは、第１カットオフ周波数ｆ１以上の高周波数成分において電気的な外乱ノイズのうち少なくとも最もエネルギー強度の大きい周波数の外乱ノイズのゲインが相対的に小さい特性を有する。本実施形態では、フィルタは第２カットオフ周波数ｆ２の帯域においてゲインを相対的に小さくし、このゲインが相対的に小さくなった帯域に外乱ノイズが含まれるようにしている。なお、特定の帯域においてゲインを小さくすることは、特定の帯域でセンサーの感度を低くすることと等価である。

つまり、図５に示すように、本実施形態のフィルタにより、電気的な外乱ノイズはゲインが相対的に小さくされた第２カットオフ周波数ｆ２の帯域近傍に含まれる。これにより、電気的な外乱ノイズをゲインが相対的に小さくなった帯域にもっていくことができる。

本実施形態の構成によれば、電気的な外乱ノイズのうち少なくとも最もエネルギー強度の大きい周波数の外乱ノイズのゲインが相対的に小さくなる。このため、センサーで検出したくない部分においてセンサーの感度が低くされるので、外乱ノイズなどの制御に必要のない信号を確実に除去することができる。したがって、格段に高精度な制御を行うことが可能となる。

なお、本実施形態においては、フィルタの電気的な外乱ノイズのゲインが相対的に小さい例を挙げて説明したが、これに限らない。例えば、電気的な外乱ノイズに替えて、外部からの物理的な振動、ロボットの筐体または設置台の固有周波数、などのセンサーで検出したくない信号においても適用可能である。また、角速度センサーに振動型ジャイロを用いる場合にあっては、振動型ジャイロの駆動周波数、検出周波数、離調周波数、または素子の固有周波数、などのセンサーで検出したくない信号においても適用可能である。

（変形例）
図７は、リンクの角度を算出する過程の変形例を示すフローチャート図である。図７は、図２に対応した、リンクの角度の算出過程の変形例を示したフローチャート図である。図７に示すように、本変形例のリンクの角度の算出過程は、リンクの角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔの積分値のうちカットオフ周波数１／Ｔｆ以上の高周波数成分と、モーターの回転角度と減速機の所定の減速比との積Θｍ１・Ｎ１及びΘｍ２・Ｎ２のうちカットオフ周波数１／Ｔｆ以下の低周波数成分とを用いた後に、特定の周波数特性を持ったフィルタを用いる点で、上述の第１実施形態で説明したリンクの角度の算出過程と異なっている。その他の点は第１実施形態と同様であるので、図２と同様の要素には同一の符号を付し、詳細な説明は省略する。

本変形例のステップＳ４においては、バイアスドリフトを除去するために、演算部１５によって、リンクの角速度ｄΘＡ１／ｄｔ及びｄΘＡ２／ｄｔの積分結果がハイパスフィルタＦＨに通過される。これにより、リンクの角度ΘＡ１及びΘＡ２のうちカットオフ周波数１／Ｔｆ以上の高周波数成分が得られる。ステップＳ１〜Ｓ４により得られたリンクの角度ΘＡ１及びΘＡ２の高周波数成分と、ステップＳ６〜Ｓ８により得られたリンクの角度ΘＡ１及びΘＡ２の低周波数成分とが加算され、外乱等によるノイズの影響を除去するために、特定の周波数特性を持ったフィルタに通過される（Ｓ１０）。この演算によって、架台１に対するリンク３の角度ΘＡ１及びリンク３に対するリンク８の角度ΘＡ２が算出される（Ｓ１１）。

このように、ハイパスフィルタＦＨ及びローパスフィルタＦＬを用いることによって、ステップＳ１〜Ｓ４により得られたリンクの角度ΘＡ１及びΘＡ２の高周波数成分と、ステップＳ６〜Ｓ８により得られたリンクの角度ΘＡ１及びΘＡ２の低周波数成分と、が加算された後に、特定の周波数特性を持ったフィルタを用いる場合であっても、角速度センサー１２,１３のバイアスドリフトの影響を除去するとともに外乱等によるノイズの影響を除去し、高精度な制御を行うことができる。

１…架台（基体）、３，８…リンク、４，９…モーター（アクチュエーター）、５,１１…減速機（トルク伝達機構）、６，１０…角度センサー、１２，１３…角速度センサー、１００…ロボット、ｗ１,ｗ２…リンクの角速度、ΘＡ１,ΘＡ２…リンクの角度、Θｍ１，Θｍ２…モーターの回転角度、１／Ｔｆ…カットオフ周波数、Ｎ１,Ｎ２…減速比

Claims

基体に対して回転運動可能なリンクと、
前記リンクを駆動するアクチュエーターと、
前記アクチュエーターのトルクを前記リンクに所定の減速比で伝達するトルク伝達機構と、
前記アクチュエーターの回転角度を検出する角度センサーと、
前記基体に対する前記リンクの角速度を検出する角速度センサーと、
前記所定の減速比と前記回転角度との積のうちカットオフ周波数以下の低周波数成分及び前記角速度の積分値のうち前記カットオフ周波数以上の高周波数成分を用いるとともに前記高周波数成分において特定の周波数特性を有するフィルタを用いて前記リンクの角度を算出する演算部と、を備えることを特徴とするロボット。
前記フィルタが前記高周波数成分において前記リンクと前記アクチュエーターと前記トルク伝達機構とからなる系の固有周波数のうち少なくとも最も低い周波数の固有周波数よりも高い周波数を減衰させる特性を有することを特徴とする請求項１に記載のロボット。
前記フィルタが前記高周波数成分において前記系の固有周波数のうち少なくとも測定対象となる周波数の固有周波数のゲインが相対的に大きい特性を有することを特徴とする請求項２に記載のロボット。
前記フィルタが前記高周波数成分において電気的な外乱ノイズのうち少なくとも最もエネルギー強度の大きい周波数の外乱ノイズのゲインが相対的に小さい特性を有することを特徴とする請求項１〜３のいずれか１項に記載のロボット。
前記フィルタが前記高周波数成分において外部からの物理的な振動のうち少なくとも最もエネルギー強度の大きい周波数の物理的な振動のゲインが相対的に小さい特性を有することを特徴とする請求項１〜４のいずれか１項に記載のロボット。
前記フィルタが前記高周波数成分において前記ロボットの筐体または設置台の固有周波数のうち少なくとも最もエネルギー強度の大きい固有周波数のゲインが相対的に小さい特性を有することを特徴とする請求項１〜５のいずれか１項に記載のロボット。
前記角速度センサーに振動型ジャイロを用いる場合であって、前記フィルタが前記高周波数成分において前記振動型ジャイロの駆動周波数、検出周波数、離調周波数、または素子の固有周波数のうち少なくとも最もエネルギー強度の大きい駆動周波数、検出周波数、離調周波数、または素子の固有周波数のゲインが相対的に小さい特性を有することを特徴とする請求項１〜６のいずれか１項に記載のロボット。