ロボット制御装置、ロボットの制御方法
【課題】ロボットの制御を可能にするロボット制御装置を提供する。
【解決手段】ロボット制御装置は、基体１に対して回転運動可能なリンク３と、リンク３を駆動するモーター４と、モーター４のトルクをリンク３に減速比Ｎで伝達する減速機５と、モーター４の回転角θM1を検出する角度センサー６と、基体１に対するリンク３の角速度ωL1を検出する角速度センサー１２と、角速度ωL1の積分値のうち第１の周波数ｆH以上の高周波成分、及びθM1＊Ｎのうち第２の周波数ｆL以下の低周波成分を用いリンク３の回転角を算出する演算部１５と、を備え、第１の周波数ｆHと第２の周波数ｆLとが異なる。このことによって、角速度センサー１２のバイアスドリフトの影響を排除し、特定の周波数を強調したり除去した角度の信号を得ることができる。
【解決手段】ロボット制御装置は、基体１に対して回転運動可能なリンク３と、リンク３を駆動するモーター４と、モーター４のトルクをリンク３に減速比Ｎで伝達する減速機５と、モーター４の回転角θM1を検出する角度センサー６と、基体１に対するリンク３の角速度ωL1を検出する角速度センサー１２と、角速度ωL1の積分値のうち第１の周波数ｆH以上の高周波成分、及びθM1＊Ｎのうち第２の周波数ｆL以下の低周波成分を用いリンク３の回転角を算出する演算部１５と、を備え、第１の周波数ｆHと第２の周波数ｆLとが異なる。このことによって、角速度センサー１２のバイアスドリフトの影響を排除し、特定の周波数を強調したり除去した角度の信号を得ることができる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの制御装置、及びロボットの制御方法に関する。
【背景技術】
【０００２】
従来、ロボットを駆動する際に、エンコーダーでモーターの回転角を制御し、リンクの制御を行っている。しかし、モーターとリンクとの間のトルク伝達機構やリンクは剛体ではないため動作時に振動が発生し、正確なリンクの作動を制御できないという課題を有していた。
【０００３】
そこで、リンク（例えば、アーム等）に慣性センサーを取り付け、この慣性センサーの出力信号を用いてリンクの動作制御を行うロボット制御装置が提案されている（例えば、特許文献１、特許文献２参照）。
【０００４】
また、エンコーダーの低周波成分と慣性センサーの高周波成分とを合成し、リンクの回転角度を制御するロボット制御装置も提案されている（例えば、特許文献３）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平１−１７３１１６号公報
【特許文献２】特開平７−９７３４号公報
【特許文献３】特許第３８８３５４４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
このような特許文献１や特許文献２では、慣性センサーのバイアスドリフトに起因する定常の速度偏差や位置偏差をもつという課題を有している。
【０００７】
この課題に対して、特許文献３では、エンコーダーの低周波成分と慣性センサーの高周波成分とを合成することで、慣性センサーのバイアスドリフトの影響を抑制することを可能としている。
【０００８】
しかしながら、エンコーダーの低周波成分を取り出すためのローパスフィルターと、慣性センサーの高周波成分を取り出すためのハイパスフィルターの遮断周波数（カットオフ周波数）を一致させているために、低周波成分と高周波成分とを合成させた場合に周波数特性が平坦になる。つまり、ゲインが平坦になる。従って、合成感度が平坦になることから、強調したい周波数帯、または除去したい周波数帯がある場合には別に外付けのフィルターを設けなければならないという課題を有している。
【課題を解決するための手段】
【０００９】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【００１０】
［適用例１］本適用例に係るロボット制御装置は、基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、前記アクチュエーターの回転角θM1を検出する角度センサーと、前記基体に対する前記リンクの角速度ωL1を検出する角速度センサーと、ωL1の積分値のうち第１の周波数ｆH以上の高周波成分とθM1＊Ｎのうち第２の周波数ｆL以下の低周波成分とを用いて前記リンクの回転角を算出する演算部と、を備え、前記第１の周波数ｆHと前記第２の周波数ｆLとが異なることを特徴とする。
【００１１】
本適用例によれば、リンクに角速度センサーを取り付け、この角速度センサーの出力信号を用いてリンクの動作制御を行うことにより、アクチュエーターとリンクとの間の弾性成分に起因する動作時の振動を抑制することができる。
【００１２】
また、第１の周波数ｆHと第２の周波数ｆLとを一致させないことにより、角速度センサーのバイアスドリフトの影響を排除することができる。
なお、第１の周波数ｆH、第２の周波数ｆLはそれぞれ、低周波成分及び高周波成分を取り出すために用いるフィルターの遮断周波数（カットオフ周波数）に相当する。
【００１３】
従って、特許文献３のように強調したい周波数帯、または除去したい周波数帯がある場合に設けられる外付けのフィルターを必要としないため、コストの低減や演算部の演算量を減らすことができる。
【００１４】
［適用例２］上記適用例に係るロボット制御装置は、前記第１の周波数ｆHと前記第２の周波数ｆLとが、ｆH＜ｆLであることが好ましい。
【００１５】
［適用例３］上記適用例に係るロボット制御装置は、前記リンクと前記アクチュエーターと前記トルク伝達機構からなる系の固有振動数のうちの少なくとも一つの固有振動数ｆLNが、ｆH＜ｆLN＜ｆLであることが望ましい。
【００１６】
このように、第１の周波数ｆH、第２の周波数ｆLとをｆH＜ｆLとすること、また、系の固有振動数ｆLNを、ｆH＜ｆLN＜ｆLとすることにより、系の固有振動のような測定したい部分において感度を高めることができ、振動を強調することが可能となる。従って、この測定した情報を基に制御を行うことで振動を抑制することができる。
【００１７】
［適用例４］上記適用例に係るロボット制御装置は、前記第１の周波数ｆHと前記第２の周波数ｆLとが、ｆH＞ｆLであることが好ましい。
【００１８】
このようにすれば、特定の周波数帯域において検出するリンクの角度の感度を小さくすることができ、検出したくない特定周波数のノイズを除去することができる。
【００１９】
［適用例５］上記適用例に係るロボット制御装置は、電気的なノイズの周波数のうちの少なくと一つの周波数ｆnoが、ｆH＞ｆno＞ｆLであることが望ましい。
【００２０】
［適用例６］上記適用例に係るロボット制御装置は、外部からの物理的な振動の周波数のうちの少なくとも一つの周波数ｆphが、ｆH＞ｆph＞ｆLであることが望ましい。
【００２１】
［適用例７］上記適用例に係るロボット制御装置は、前記基体、または前記アクチュエーターと前記トルク伝達機構を保持する筐体の固有周波数のうち、少なくとも一つの固有周波数ｆrbがｆH＞ｆrb＞ｆLであることが望ましい。
【００２２】
前述したように、第１の周波数ｆHと第２の周波数ｆLとをｆH＞ｆLとした場合、特定の周波数帯域において角速度センサーの感度を小さくすることができることから、電気的なノイズの周波数ｆnoをｆH＞ｆno＞ｆLとし、外部からの物理的な振動の周波数ｆphをｆH＞ｆph＞ｆLとし、基体または筐体の固有周波数のどちらか一方の固有周波数ｆrbをｆH＞ｆrb＞ｆLとすれば、リンクの角度として検出したくない外乱信号が入る場合に、その信号を除去するノッチフィルターの役割を兼ねることが可能になる。
【００２３】
さらに、上述した外乱ノイズを除去して測定した情報を基に制御を行うことにより、より正確な制御を行うことができる。
【００２４】
［適用例８］本適用例に係るロボットの制御方法は、基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、前記アクチュエーターの回転角θM1を検出する角度センサーと、前記基体に対する前記リンクの角速度ωL1を検出する角速度センサーと、ωL1の積分値のうち第１の周波数ｆH以上の高周波成分とθM1＊Ｎのうち第２の周波数ｆL以下の低周波成分と、を用いて前記リンクの回転角を算出する演算部と、を備え、前記第１の周波数ｆHと前記第２の周波数ｆLとが異なるロボットの制御方法であって、前記アクチュエーターが前記リンクを駆動するステップと、前記角度センサーが、前記アクチュエーターの回転角θM1を検出するステップと、前記角速度センサーが、前記基体に対する前記リンクの角速度ωL1を検出するステップと、前記演算部が、ωL1の積分値のうちの第１の周波数ｆH以上の高周波成分と、θM1＊Ｎのうちの第２の周波数ｆL以下の低周波成分を用いて前記リンクの角度を算出するステップと、を含むことを特徴とする。
【００２５】
このような制御方法によれば、角速度センサーの出力信号を用いてリンクの動作制御を行うことにより、アクチュエーターとリンクとの間の弾性成分に起因する動作時の振動を抑制することができ、角速度センサーのバイアスドリフトの影響を排除することができる。
【００２６】
さらに、第１の周波数ｆH、第２の周波数ｆLはそれぞれ、低周波成分及び高周波成分を取り出すために用いるフィルターの遮断周波数（カットオフ周波数）に相当し、従来技術のように、強調したい周波数帯または除去したい周波数帯がある場合に設けられる外付けのフィルターを必要としないため、コストの低減や演算部の演算量を減らすことができる。
【図面の簡単な説明】
【００２７】
【図１】実施形態１に係るロボットの概略構成を示す正面断面図。
【図２】リンクの角度を算出する過程を示すフロー図。
【図３】実施例１に係る周波数とゲインの関係を示すグラフ。
【図４】実施例２に係る周波数とゲインの関係を示すグラフ。
【図５】実施例２において２次のフィルターを用いた周波数とゲインの関係を示すグラフ。
【図６】モーター角度の応答とリンク角度の応答からみた周波数とゲインの関係を示すグラフ。
【発明を実施するための形態】
【００２８】
以下、本発明の実施形態を図面に基づいて説明する。
（実施形態１）
【００２９】
図１は、実施形態１に係るロボットの概略構成を示す正面断面図である。ロボット１００は、基体１と、リンク３，８と、アクチュエーターとしてのモーター４，９と、トルク伝達機構としての減速機５，１１と、角度センサー６，１０と、角速度センサー１２，１３とから構成されている。
【００３０】
基体１の上部にリンク３の一端が取付けられており、リンク３の他端にはリンク８が取付けられている。リンク３は、軸２を回転軸として基体１に対して相対的に回転運動することができる。また、リンク８は、軸７を回転軸としてリンク３に対して相対的に回転運動することができる。
【００３１】
モーター４は減速機５を介してリンク３を駆動し、モーター９は減速機１１を介してリンク８を駆動する。減速機５及び減速機１１の減速比をそれぞれＮ1、Ｎ2とする。角度センサー６はモーター４の頂部に設けられ、軸２回りのモーター回転角θM1を検出する。角度センサー１０はモーター９の頂部に設けられ、軸７回りのモーター回転角θM2を検出する。
【００３２】
角速度センサー１２はリンク３に設けられ、角速度センサー１３はリンク８に設けられている。角速度センサー１２は基体１に対するリンク３の角速度ωL1を検出し、角速度センサー１３はリンク３に対するリンク８の角速度ωL2を検出する。なお、リンク８の先端方向には回転装置３０が設けられ、回転装置３０には昇降装置３１が軸止されている。そして、昇降装置３１の先端部にはワークのチャック部３２が設けられている。
【００３３】
角速度センサー１２，１３は、それぞれリンク３，８の先端部に配置されている。なお、リンク３，８を剛体とみなすのに十分な剛性を有する場合、リンク３，８のそれぞれにおいて角速度は一定であるので、角速度センサー１２，１３はリンク３，８それぞれの任意の位置に配設することができる。リンク３，８が十分な剛性を有しない場合、角速度センサー１２，１３は、それぞれリンク３，８の先端部に配置する方が望ましい。なお、角速度ωL1、ωL2は、リンク３，８の回転面内の角速度成分である。
【００３４】
なお、モーター４及び減速機５は筐体２０によって保持されており、モーター９及び減速機１１は筐体２１によって保持されている。
続いて、本実施形態によるリンクの角度を算出する過程について説明する。
【００３５】
図２は、本実施形態に係るリンクの角度を算出する過程を示すフロー図である。なお、基体１に対するリンク３の角度と、リンク３に対するリンク８の角度の算出はリンク３に対するリンク８の角速度をωL1−ωL2とすることと同じ考え方で可能なため、基体１に対するリンク３の角度θA1の算出方法を例示して説明する。図１も参照する。
【００３６】
図２おいて、ステップＳ１０及びステップＳ６０以外の演算処理は演算部１５（図１、参照）によって実行される。なお、演算部１５は、ロボット１００内の任意の位置に配置されるＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、またはロボット１００の外部に設けられるＣＰＵによって実行してもよい。
【００３７】
まず、角速度センサー１２が角速度ω1を検出する（Ｓ１０）。次に、リンク３の単独の角速度を算出する（Ｓ２０）。リンク３の角速度はｄθA1／ｄｔで算出される。角速度ω1は基体１に対する角速度である。次に、ｄθA1／ｄｔを積分する（Ｓ３０）。
【００３８】
さらに、この積分結果をハイパスフィルターＨFに通過させる（Ｓ４０）。これにより、角度θA1のうち第１の周波数ｆH以上の高周波成分が得られる。なお、第１の周波数ｆHは、ハイパスフィルターＨFの遮断周波数（カットオフ周波数）のため、以降、遮断周波数ｆHまたはカットオフ周波数ｆHと表す。ここで、角度θA1の低周波成分を除去する理由は、リンク３の角速度ｄθA1／ｄｔの積分ではバイアスドリフトの影響によりリンク３の角度θA1を正確に求められないので、このバイアスドリフトの影響を除去するためである。
【００３９】
ロボット１００が停止または極低周波帯域で動作している場合には、減速機５はほとんど捩れない。そこで、角度センサー６でモーター回転角θM1を検出する（Ｓ６０）。次に、減速比Ｎ1をモーター回転角θM1に乗算し、θA1＝θM1＊Ｎ1を算出する（Ｓ７０）。
【００４０】
続いて、θM1＊Ｎ1をローパスフィルターＬFに通過させる（Ｓ８０）。これにより、リンク３の角度θA1のうち第２の周波数ｆL以下の低周波成分が得られる。なお、第２の周波数ｆHは、ローパスフィルターＬFの遮断周波数（カットオフ周波数）のため、以降、遮断周波数ｆLまたはカットオフ周波数ｆL表す。ここで、角度θA1の高周波成分を除去する理由は、高周波成分では減速機５が捩れるためθA1＝θM1＊Ｎ1が成立しなくなるからである。
【００４１】
このように、ハイパスフィルターＨFとローパスフィルターＬFを用いることにより、ステップＳ１０〜Ｓ４０により得られた角度θA1の高周波成分と、ステップＳ６０〜Ｓ８０によって得られた角度θA1の低周波成分とを加算する（Ｓ１００）。この演算によって、基体１に対するリンク３の角度θA1を算出することができる。
【００４２】
また、ステップＳ２０において得られた角速度ω1（ｄθA1／ｄｔ）を１回近似微分することによって、リンク３の角加速度ｄ2θA1／ｄｔ2を算出することができる（Ｓ５０）。また、ステップＳ６０において得られたモーター回転角θM1を１回近似微分することによって、モーター４のモーター角速度ｄθM1／ｄｔを算出する（Ｓ９０）。
【００４３】
従って、このようにすれば、角速度センサー１２のバイアスドリフトのリンク角度θA1に対する影響を抑制することができる。
【００４４】
なお、本実施形態では、ハイパスフィルターＨF、ローパスフィルターＬFそれぞれの遮断周波数ｆH，ｆLをｆH≠ｆLとしている。
次に、遮断周波数ｆH，ｆLが、ｆH≠ｆLとする場合について実施例１〜実施例３で詳しく説明する。
（実施例１）
【００４５】
実施例１では、遮断周波数ｆH，ｆLが、ｆH＜ｆL、１次のフィルターの場合について図面を参照して説明する。
図３は、実施例１に係る周波数とゲインの関係を示すグラフである。図３は、遮断周波数が、ｆL＝１５Ｈｚ、ｆH＝５Ｈｚの場合の周波数ｆ（Ｈｚ）とゲイン（ｄｂ）の関係を示している。図３において、周波数ｆが、ｆH＜ｆ＜ｆLの条件では、ハイパスフィルターＨFのゲインとローパスフィルターＬFのゲインとを合成（加算）して表した場合、合成ゲインは、ハイパスフィルターＨFのゲイン曲線とローパスフィルターＬFのゲイン曲線との交差域において高くなる。これは、特定の周波数帯域において角度センサー６と角速度センサー１２の出力を基に演算されたリンクの角度の感度を高くしていることを意味している。
【００４６】
従って、モーター４、減速機５、リンク３からなる系の固有振動のような測定したい部分において感度を高めることができる。つまり、これらの系の少なくとも一つの固有振動数ｆLNをｆH＜ｆLN＜ｆLとすれば、測定したい部分において感度を高めることで振動を強調して角度θA1の測定を行うことができる。従って、この測定結果に基づき制御を行うことで効果的にリンク３の振動を抑制することができる。
【００４７】
また、測定したい部分において感度を高めることで振動を強調するための外付けのフィルターを必要としないため、コストの低減、及び演算部１５の演算量を減らすことができる。
（実施例２）
【００４８】
続いて、遮断周波数ｆH，ｆLが、ｆH＞ｆL、１次のフィルターの場合について図面を参照して説明する。
図４は、実施例２に係る周波数とゲインの関係を示すグラフである。図４は、遮断周波数が、ｆL＝５Ｈｚ、ｆH＝１５Ｈｚの場合の周波数ｆ（Ｈｚ）とゲイン（ｄｂ）の関係を示している。
【００４９】
図４において、周波数ｆが、ｆH＞ｆ＞ｆLの条件では、ハイパスフィルターＨFのゲインとローパスフィルターＬFのゲインとを合成（加算）した場合、合成ゲインは、ハイパスフィルターＨFのゲイン曲線とローパスフィルターＬFのゲイン曲線との交差域においてゲインが低くなる。これは、特定の周波数帯域において角度センサー６と角速度センサー１２の出力を基に演算されたリンクの角度の感度を低くしていることを意味している。
【００５０】
そのことから、遮断周波数ｆH，ｆLを、ｆH＞ｆLとしたとき、特定の周波数をもった電気的なノイズが外部から入る場合に電気的なノイズの周波数ｆnoをｆH＞ｆno＞ｆLとし、周囲の装置等の外部からの特定の周波数をもった物理的な振動が発生している場合に、この物理的な振動の周波数ｆphをｆH＞ｆph＞ｆLとし、基体１または筐体２０自体が特定の周波数で振動する場合、この固有周波数のどちらか一方の固有周波数ｆrbをｆH＞ｆrb＞ｆLとしても、角速度センサー１２で検出したくない外乱信号が入る場合に、その外乱信号を除去することができる。このことから外乱信号を除去するための外付けのノッチフィルターとしての役割を兼ねることが可能になる。
【００５１】
従って、外乱ノイズを除去して測定したリンク３の角度θA1の情報を基に制御を行うことにより、ロボットをより正確に制御することができる。
【００５２】
さらに、外乱信号を除去するための外付けのフィルターを必要としないことから、コストの低減や演算部１５の演算量を減らすことができる。
なお、前述した実施例１、実施例２では、１次のフィルターを例示したが、２次以上のフィルターであっても同様な効果が得られる。
【００５３】
図５は、実施例２において２次のフィルターを用いた周波数とゲインの関係を示すグラフである。図５は、遮断周波数が、ｆL＝５Ｈｚ、ｆH＝１５Ｈｚの場合の周波数ｆ（Ｈｚ）とゲイン（ｄｂ）の関係を示すグラフである。図５に示すように、２次のフィルターを用いた場合、１次の場合と同様な効果が得られる。つまり、次数が高くなるに従い特定の周波数帯域において角度センサー６と角速度センサー１２の出力を基に演算されたリンクの角度の感度をより低くしている。
【００５４】
この場合、ｆL＞ｆHとした場合には、測定したい部分において感度を高めることで振動をより強調して角度θA1の測定を行うことができる。従って、この測定結果に基づき制御を行うことでより効果的にリンク３の振動を抑制することができる。
【００５５】
同様に、２次のフィルターを用いたときの周波数とゲインの関係をモーター角度の応答とリンク角度の応答から説明する。
図６は、実施例２において２次のフィルターを用いたときのモーター角度の応答とリンク角度の応答からみた周波数とゲインの関係を示すグラフである。なお、フィルターのカットオフ周波数は、ローパスフィルターが５Ｈｚ、ハイパスフィルターが１５Ｈｚとした。
【００５６】
図６に示すように、モーター角度の応答信号と、リンク角度の応答信号を合成した合成角度は、５Ｈｚ〜１５Ｈｚの周波数帯でゲイン（振幅）が下がっている。従って、リンクの角度として検出したくない外乱信号が入る場合に、その信号を除去するノッチフィルターの役割を兼ねることが可能になる。
【符号の説明】
【００５７】
１…基体、３…リンク、４…アクチュエーターとしてのモーター、５…トルク伝達機構としての減速機、６…角度センサー、１２…角速度センサー、１５…演算部、１００…ロボット。
【技術分野】
【０００１】
本発明は、ロボットの制御装置、及びロボットの制御方法に関する。
【背景技術】
【０００２】
従来、ロボットを駆動する際に、エンコーダーでモーターの回転角を制御し、リンクの制御を行っている。しかし、モーターとリンクとの間のトルク伝達機構やリンクは剛体ではないため動作時に振動が発生し、正確なリンクの作動を制御できないという課題を有していた。
【０００３】
そこで、リンク（例えば、アーム等）に慣性センサーを取り付け、この慣性センサーの出力信号を用いてリンクの動作制御を行うロボット制御装置が提案されている（例えば、特許文献１、特許文献２参照）。
【０００４】
また、エンコーダーの低周波成分と慣性センサーの高周波成分とを合成し、リンクの回転角度を制御するロボット制御装置も提案されている（例えば、特許文献３）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平１−１７３１１６号公報
【特許文献２】特開平７−９７３４号公報
【特許文献３】特許第３８８３５４４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
このような特許文献１や特許文献２では、慣性センサーのバイアスドリフトに起因する定常の速度偏差や位置偏差をもつという課題を有している。
【０００７】
この課題に対して、特許文献３では、エンコーダーの低周波成分と慣性センサーの高周波成分とを合成することで、慣性センサーのバイアスドリフトの影響を抑制することを可能としている。
【０００８】
しかしながら、エンコーダーの低周波成分を取り出すためのローパスフィルターと、慣性センサーの高周波成分を取り出すためのハイパスフィルターの遮断周波数（カットオフ周波数）を一致させているために、低周波成分と高周波成分とを合成させた場合に周波数特性が平坦になる。つまり、ゲインが平坦になる。従って、合成感度が平坦になることから、強調したい周波数帯、または除去したい周波数帯がある場合には別に外付けのフィルターを設けなければならないという課題を有している。
【課題を解決するための手段】
【０００９】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【００１０】
［適用例１］本適用例に係るロボット制御装置は、基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、前記アクチュエーターの回転角θM1を検出する角度センサーと、前記基体に対する前記リンクの角速度ωL1を検出する角速度センサーと、ωL1の積分値のうち第１の周波数ｆH以上の高周波成分とθM1＊Ｎのうち第２の周波数ｆL以下の低周波成分とを用いて前記リンクの回転角を算出する演算部と、を備え、前記第１の周波数ｆHと前記第２の周波数ｆLとが異なることを特徴とする。
【００１１】
本適用例によれば、リンクに角速度センサーを取り付け、この角速度センサーの出力信号を用いてリンクの動作制御を行うことにより、アクチュエーターとリンクとの間の弾性成分に起因する動作時の振動を抑制することができる。
【００１２】
また、第１の周波数ｆHと第２の周波数ｆLとを一致させないことにより、角速度センサーのバイアスドリフトの影響を排除することができる。
なお、第１の周波数ｆH、第２の周波数ｆLはそれぞれ、低周波成分及び高周波成分を取り出すために用いるフィルターの遮断周波数（カットオフ周波数）に相当する。
【００１３】
従って、特許文献３のように強調したい周波数帯、または除去したい周波数帯がある場合に設けられる外付けのフィルターを必要としないため、コストの低減や演算部の演算量を減らすことができる。
【００１４】
［適用例２］上記適用例に係るロボット制御装置は、前記第１の周波数ｆHと前記第２の周波数ｆLとが、ｆH＜ｆLであることが好ましい。
【００１５】
［適用例３］上記適用例に係るロボット制御装置は、前記リンクと前記アクチュエーターと前記トルク伝達機構からなる系の固有振動数のうちの少なくとも一つの固有振動数ｆLNが、ｆH＜ｆLN＜ｆLであることが望ましい。
【００１６】
このように、第１の周波数ｆH、第２の周波数ｆLとをｆH＜ｆLとすること、また、系の固有振動数ｆLNを、ｆH＜ｆLN＜ｆLとすることにより、系の固有振動のような測定したい部分において感度を高めることができ、振動を強調することが可能となる。従って、この測定した情報を基に制御を行うことで振動を抑制することができる。
【００１７】
［適用例４］上記適用例に係るロボット制御装置は、前記第１の周波数ｆHと前記第２の周波数ｆLとが、ｆH＞ｆLであることが好ましい。
【００１８】
このようにすれば、特定の周波数帯域において検出するリンクの角度の感度を小さくすることができ、検出したくない特定周波数のノイズを除去することができる。
【００１９】
［適用例５］上記適用例に係るロボット制御装置は、電気的なノイズの周波数のうちの少なくと一つの周波数ｆnoが、ｆH＞ｆno＞ｆLであることが望ましい。
【００２０】
［適用例６］上記適用例に係るロボット制御装置は、外部からの物理的な振動の周波数のうちの少なくとも一つの周波数ｆphが、ｆH＞ｆph＞ｆLであることが望ましい。
【００２１】
［適用例７］上記適用例に係るロボット制御装置は、前記基体、または前記アクチュエーターと前記トルク伝達機構を保持する筐体の固有周波数のうち、少なくとも一つの固有周波数ｆrbがｆH＞ｆrb＞ｆLであることが望ましい。
【００２２】
前述したように、第１の周波数ｆHと第２の周波数ｆLとをｆH＞ｆLとした場合、特定の周波数帯域において角速度センサーの感度を小さくすることができることから、電気的なノイズの周波数ｆnoをｆH＞ｆno＞ｆLとし、外部からの物理的な振動の周波数ｆphをｆH＞ｆph＞ｆLとし、基体または筐体の固有周波数のどちらか一方の固有周波数ｆrbをｆH＞ｆrb＞ｆLとすれば、リンクの角度として検出したくない外乱信号が入る場合に、その信号を除去するノッチフィルターの役割を兼ねることが可能になる。
【００２３】
さらに、上述した外乱ノイズを除去して測定した情報を基に制御を行うことにより、より正確な制御を行うことができる。
【００２４】
［適用例８］本適用例に係るロボットの制御方法は、基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、前記アクチュエーターの回転角θM1を検出する角度センサーと、前記基体に対する前記リンクの角速度ωL1を検出する角速度センサーと、ωL1の積分値のうち第１の周波数ｆH以上の高周波成分とθM1＊Ｎのうち第２の周波数ｆL以下の低周波成分と、を用いて前記リンクの回転角を算出する演算部と、を備え、前記第１の周波数ｆHと前記第２の周波数ｆLとが異なるロボットの制御方法であって、前記アクチュエーターが前記リンクを駆動するステップと、前記角度センサーが、前記アクチュエーターの回転角θM1を検出するステップと、前記角速度センサーが、前記基体に対する前記リンクの角速度ωL1を検出するステップと、前記演算部が、ωL1の積分値のうちの第１の周波数ｆH以上の高周波成分と、θM1＊Ｎのうちの第２の周波数ｆL以下の低周波成分を用いて前記リンクの角度を算出するステップと、を含むことを特徴とする。
【００２５】
このような制御方法によれば、角速度センサーの出力信号を用いてリンクの動作制御を行うことにより、アクチュエーターとリンクとの間の弾性成分に起因する動作時の振動を抑制することができ、角速度センサーのバイアスドリフトの影響を排除することができる。
【００２６】
さらに、第１の周波数ｆH、第２の周波数ｆLはそれぞれ、低周波成分及び高周波成分を取り出すために用いるフィルターの遮断周波数（カットオフ周波数）に相当し、従来技術のように、強調したい周波数帯または除去したい周波数帯がある場合に設けられる外付けのフィルターを必要としないため、コストの低減や演算部の演算量を減らすことができる。
【図面の簡単な説明】
【００２７】
【図１】実施形態１に係るロボットの概略構成を示す正面断面図。
【図２】リンクの角度を算出する過程を示すフロー図。
【図３】実施例１に係る周波数とゲインの関係を示すグラフ。
【図４】実施例２に係る周波数とゲインの関係を示すグラフ。
【図５】実施例２において２次のフィルターを用いた周波数とゲインの関係を示すグラフ。
【図６】モーター角度の応答とリンク角度の応答からみた周波数とゲインの関係を示すグラフ。
【発明を実施するための形態】
【００２８】
以下、本発明の実施形態を図面に基づいて説明する。
（実施形態１）
【００２９】
図１は、実施形態１に係るロボットの概略構成を示す正面断面図である。ロボット１００は、基体１と、リンク３，８と、アクチュエーターとしてのモーター４，９と、トルク伝達機構としての減速機５，１１と、角度センサー６，１０と、角速度センサー１２，１３とから構成されている。
【００３０】
基体１の上部にリンク３の一端が取付けられており、リンク３の他端にはリンク８が取付けられている。リンク３は、軸２を回転軸として基体１に対して相対的に回転運動することができる。また、リンク８は、軸７を回転軸としてリンク３に対して相対的に回転運動することができる。
【００３１】
モーター４は減速機５を介してリンク３を駆動し、モーター９は減速機１１を介してリンク８を駆動する。減速機５及び減速機１１の減速比をそれぞれＮ1、Ｎ2とする。角度センサー６はモーター４の頂部に設けられ、軸２回りのモーター回転角θM1を検出する。角度センサー１０はモーター９の頂部に設けられ、軸７回りのモーター回転角θM2を検出する。
【００３２】
角速度センサー１２はリンク３に設けられ、角速度センサー１３はリンク８に設けられている。角速度センサー１２は基体１に対するリンク３の角速度ωL1を検出し、角速度センサー１３はリンク３に対するリンク８の角速度ωL2を検出する。なお、リンク８の先端方向には回転装置３０が設けられ、回転装置３０には昇降装置３１が軸止されている。そして、昇降装置３１の先端部にはワークのチャック部３２が設けられている。
【００３３】
角速度センサー１２，１３は、それぞれリンク３，８の先端部に配置されている。なお、リンク３，８を剛体とみなすのに十分な剛性を有する場合、リンク３，８のそれぞれにおいて角速度は一定であるので、角速度センサー１２，１３はリンク３，８それぞれの任意の位置に配設することができる。リンク３，８が十分な剛性を有しない場合、角速度センサー１２，１３は、それぞれリンク３，８の先端部に配置する方が望ましい。なお、角速度ωL1、ωL2は、リンク３，８の回転面内の角速度成分である。
【００３４】
なお、モーター４及び減速機５は筐体２０によって保持されており、モーター９及び減速機１１は筐体２１によって保持されている。
続いて、本実施形態によるリンクの角度を算出する過程について説明する。
【００３５】
図２は、本実施形態に係るリンクの角度を算出する過程を示すフロー図である。なお、基体１に対するリンク３の角度と、リンク３に対するリンク８の角度の算出はリンク３に対するリンク８の角速度をωL1−ωL2とすることと同じ考え方で可能なため、基体１に対するリンク３の角度θA1の算出方法を例示して説明する。図１も参照する。
【００３６】
図２おいて、ステップＳ１０及びステップＳ６０以外の演算処理は演算部１５（図１、参照）によって実行される。なお、演算部１５は、ロボット１００内の任意の位置に配置されるＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、またはロボット１００の外部に設けられるＣＰＵによって実行してもよい。
【００３７】
まず、角速度センサー１２が角速度ω1を検出する（Ｓ１０）。次に、リンク３の単独の角速度を算出する（Ｓ２０）。リンク３の角速度はｄθA1／ｄｔで算出される。角速度ω1は基体１に対する角速度である。次に、ｄθA1／ｄｔを積分する（Ｓ３０）。
【００３８】
さらに、この積分結果をハイパスフィルターＨFに通過させる（Ｓ４０）。これにより、角度θA1のうち第１の周波数ｆH以上の高周波成分が得られる。なお、第１の周波数ｆHは、ハイパスフィルターＨFの遮断周波数（カットオフ周波数）のため、以降、遮断周波数ｆHまたはカットオフ周波数ｆHと表す。ここで、角度θA1の低周波成分を除去する理由は、リンク３の角速度ｄθA1／ｄｔの積分ではバイアスドリフトの影響によりリンク３の角度θA1を正確に求められないので、このバイアスドリフトの影響を除去するためである。
【００３９】
ロボット１００が停止または極低周波帯域で動作している場合には、減速機５はほとんど捩れない。そこで、角度センサー６でモーター回転角θM1を検出する（Ｓ６０）。次に、減速比Ｎ1をモーター回転角θM1に乗算し、θA1＝θM1＊Ｎ1を算出する（Ｓ７０）。
【００４０】
続いて、θM1＊Ｎ1をローパスフィルターＬFに通過させる（Ｓ８０）。これにより、リンク３の角度θA1のうち第２の周波数ｆL以下の低周波成分が得られる。なお、第２の周波数ｆHは、ローパスフィルターＬFの遮断周波数（カットオフ周波数）のため、以降、遮断周波数ｆLまたはカットオフ周波数ｆL表す。ここで、角度θA1の高周波成分を除去する理由は、高周波成分では減速機５が捩れるためθA1＝θM1＊Ｎ1が成立しなくなるからである。
【００４１】
このように、ハイパスフィルターＨFとローパスフィルターＬFを用いることにより、ステップＳ１０〜Ｓ４０により得られた角度θA1の高周波成分と、ステップＳ６０〜Ｓ８０によって得られた角度θA1の低周波成分とを加算する（Ｓ１００）。この演算によって、基体１に対するリンク３の角度θA1を算出することができる。
【００４２】
また、ステップＳ２０において得られた角速度ω1（ｄθA1／ｄｔ）を１回近似微分することによって、リンク３の角加速度ｄ2θA1／ｄｔ2を算出することができる（Ｓ５０）。また、ステップＳ６０において得られたモーター回転角θM1を１回近似微分することによって、モーター４のモーター角速度ｄθM1／ｄｔを算出する（Ｓ９０）。
【００４３】
従って、このようにすれば、角速度センサー１２のバイアスドリフトのリンク角度θA1に対する影響を抑制することができる。
【００４４】
なお、本実施形態では、ハイパスフィルターＨF、ローパスフィルターＬFそれぞれの遮断周波数ｆH，ｆLをｆH≠ｆLとしている。
次に、遮断周波数ｆH，ｆLが、ｆH≠ｆLとする場合について実施例１〜実施例３で詳しく説明する。
（実施例１）
【００４５】
実施例１では、遮断周波数ｆH，ｆLが、ｆH＜ｆL、１次のフィルターの場合について図面を参照して説明する。
図３は、実施例１に係る周波数とゲインの関係を示すグラフである。図３は、遮断周波数が、ｆL＝１５Ｈｚ、ｆH＝５Ｈｚの場合の周波数ｆ（Ｈｚ）とゲイン（ｄｂ）の関係を示している。図３において、周波数ｆが、ｆH＜ｆ＜ｆLの条件では、ハイパスフィルターＨFのゲインとローパスフィルターＬFのゲインとを合成（加算）して表した場合、合成ゲインは、ハイパスフィルターＨFのゲイン曲線とローパスフィルターＬFのゲイン曲線との交差域において高くなる。これは、特定の周波数帯域において角度センサー６と角速度センサー１２の出力を基に演算されたリンクの角度の感度を高くしていることを意味している。
【００４６】
従って、モーター４、減速機５、リンク３からなる系の固有振動のような測定したい部分において感度を高めることができる。つまり、これらの系の少なくとも一つの固有振動数ｆLNをｆH＜ｆLN＜ｆLとすれば、測定したい部分において感度を高めることで振動を強調して角度θA1の測定を行うことができる。従って、この測定結果に基づき制御を行うことで効果的にリンク３の振動を抑制することができる。
【００４７】
また、測定したい部分において感度を高めることで振動を強調するための外付けのフィルターを必要としないため、コストの低減、及び演算部１５の演算量を減らすことができる。
（実施例２）
【００４８】
続いて、遮断周波数ｆH，ｆLが、ｆH＞ｆL、１次のフィルターの場合について図面を参照して説明する。
図４は、実施例２に係る周波数とゲインの関係を示すグラフである。図４は、遮断周波数が、ｆL＝５Ｈｚ、ｆH＝１５Ｈｚの場合の周波数ｆ（Ｈｚ）とゲイン（ｄｂ）の関係を示している。
【００４９】
図４において、周波数ｆが、ｆH＞ｆ＞ｆLの条件では、ハイパスフィルターＨFのゲインとローパスフィルターＬFのゲインとを合成（加算）した場合、合成ゲインは、ハイパスフィルターＨFのゲイン曲線とローパスフィルターＬFのゲイン曲線との交差域においてゲインが低くなる。これは、特定の周波数帯域において角度センサー６と角速度センサー１２の出力を基に演算されたリンクの角度の感度を低くしていることを意味している。
【００５０】
そのことから、遮断周波数ｆH，ｆLを、ｆH＞ｆLとしたとき、特定の周波数をもった電気的なノイズが外部から入る場合に電気的なノイズの周波数ｆnoをｆH＞ｆno＞ｆLとし、周囲の装置等の外部からの特定の周波数をもった物理的な振動が発生している場合に、この物理的な振動の周波数ｆphをｆH＞ｆph＞ｆLとし、基体１または筐体２０自体が特定の周波数で振動する場合、この固有周波数のどちらか一方の固有周波数ｆrbをｆH＞ｆrb＞ｆLとしても、角速度センサー１２で検出したくない外乱信号が入る場合に、その外乱信号を除去することができる。このことから外乱信号を除去するための外付けのノッチフィルターとしての役割を兼ねることが可能になる。
【００５１】
従って、外乱ノイズを除去して測定したリンク３の角度θA1の情報を基に制御を行うことにより、ロボットをより正確に制御することができる。
【００５２】
さらに、外乱信号を除去するための外付けのフィルターを必要としないことから、コストの低減や演算部１５の演算量を減らすことができる。
なお、前述した実施例１、実施例２では、１次のフィルターを例示したが、２次以上のフィルターであっても同様な効果が得られる。
【００５３】
図５は、実施例２において２次のフィルターを用いた周波数とゲインの関係を示すグラフである。図５は、遮断周波数が、ｆL＝５Ｈｚ、ｆH＝１５Ｈｚの場合の周波数ｆ（Ｈｚ）とゲイン（ｄｂ）の関係を示すグラフである。図５に示すように、２次のフィルターを用いた場合、１次の場合と同様な効果が得られる。つまり、次数が高くなるに従い特定の周波数帯域において角度センサー６と角速度センサー１２の出力を基に演算されたリンクの角度の感度をより低くしている。
【００５４】
この場合、ｆL＞ｆHとした場合には、測定したい部分において感度を高めることで振動をより強調して角度θA1の測定を行うことができる。従って、この測定結果に基づき制御を行うことでより効果的にリンク３の振動を抑制することができる。
【００５５】
同様に、２次のフィルターを用いたときの周波数とゲインの関係をモーター角度の応答とリンク角度の応答から説明する。
図６は、実施例２において２次のフィルターを用いたときのモーター角度の応答とリンク角度の応答からみた周波数とゲインの関係を示すグラフである。なお、フィルターのカットオフ周波数は、ローパスフィルターが５Ｈｚ、ハイパスフィルターが１５Ｈｚとした。
【００５６】
図６に示すように、モーター角度の応答信号と、リンク角度の応答信号を合成した合成角度は、５Ｈｚ〜１５Ｈｚの周波数帯でゲイン（振幅）が下がっている。従って、リンクの角度として検出したくない外乱信号が入る場合に、その信号を除去するノッチフィルターの役割を兼ねることが可能になる。
【符号の説明】
【００５７】
１…基体、３…リンク、４…アクチュエーターとしてのモーター、５…トルク伝達機構としての減速機、６…角度センサー、１２…角速度センサー、１５…演算部、１００…ロボット。
【特許請求の範囲】
【請求項１】
基体に対して回転運動可能なリンクと、
前記リンクを駆動するアクチュエーターと、
前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、
前記アクチュエーターの回転角θM1を検出する角度センサーと、
前記基体に対する前記リンクの角速度ωL1を検出する角速度センサーと、
ωL1の積分値のうち第１の周波数ｆH以上の高周波成分とθM1＊Ｎのうち第２の周波数ｆL以下の低周波成分と、を用いて前記リンクの回転角を算出する演算部と、を備え、
前記第１の周波数ｆHと前記第２の周波数ｆLとが異なることを特徴とするロボット制御装置。
【請求項２】
請求項１に記載のロボット制御装置において、
前記第１の周波数ｆHと前記第２の周波数ｆLとが、ｆH＜ｆLであることを特徴とするロボット制御装置。
【請求項３】
請求項２に記載のロボット制御装置において、
前記リンクと前記アクチュエーターと前記トルク伝達機構からなる系の固有振動数のうちの少なくとも一つの固有振動数ｆLNが、ｆH＜ｆLN＜ｆLであることを特徴とするロボット制御装置。
【請求項４】
請求項１に記載のロボット制御装置において、
前記第１の周波数ｆHと前記第２の周波数ｆLとが、ｆH＞ｆLであることを特徴とするロボット制御装置。
【請求項５】
請求項４に記載のロボット制御装置において、
電気的なノイズの周波数のうちの少なくと一つの周波数ｆnoが、ｆH＞ｆno＞ｆLであることを特徴とするロボット制御装置。
【請求項６】
請求項４に記載のロボット制御装置において、
外部からの物理的な振動の周波数のうちの少なくとも一つの周波数ｆphが、ｆH＞ｆph＞ｆLであることを特徴とするロボット制御装置。
【請求項７】
請求項４に記載のロボット制御装置において、
前記基体、または前記アクチュエーターと前記トルク伝達機構を保持する筐体の固有周波数のうち、少なくとも一つの固有周波数ｆrbが、ｆH＞ｆrb＞ｆLであることを特徴とするロボット制御装置。
【請求項８】
基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、前記アクチュエーターの回転角θM1を検出する角度センサーと、前記基体に対する前記リンクの角速度ωL1を検出する角速度センサーと、ωL1の積分値のうち第１の周波数ｆH以上の高周波成分と、θM1＊Ｎのうち第２の周波数ｆL以下の低周波成分と、を用いて前記リンクの回転角を算出する演算部と、を備え、前記第１の周波数ｆHと前記第２の周波数ｆLとが異なるロボットの制御方法であって、
前記アクチュエーターが前記リンクを駆動するステップと、
前記角度センサーが、前記アクチュエーターの回転角θM1を検出するステップと、
前記角速度センサーが、前記基体に対する前記リンクの角速度ωL1を検出するステップと、
前記演算部が、ωL1の積分値のうちの第１の周波数ｆH以上の高周波成分と、θM1＊Ｎのうちの第２の周波数ｆL以下の低周波成分と、を用いて前記リンクの角度を算出するステップと、を含むことを特徴とするロボットの制御方法。
【請求項１】
基体に対して回転運動可能なリンクと、
前記リンクを駆動するアクチュエーターと、
前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、
前記アクチュエーターの回転角θM1を検出する角度センサーと、
前記基体に対する前記リンクの角速度ωL1を検出する角速度センサーと、
ωL1の積分値のうち第１の周波数ｆH以上の高周波成分とθM1＊Ｎのうち第２の周波数ｆL以下の低周波成分と、を用いて前記リンクの回転角を算出する演算部と、を備え、
前記第１の周波数ｆHと前記第２の周波数ｆLとが異なることを特徴とするロボット制御装置。
【請求項２】
請求項１に記載のロボット制御装置において、
前記第１の周波数ｆHと前記第２の周波数ｆLとが、ｆH＜ｆLであることを特徴とするロボット制御装置。
【請求項３】
請求項２に記載のロボット制御装置において、
前記リンクと前記アクチュエーターと前記トルク伝達機構からなる系の固有振動数のうちの少なくとも一つの固有振動数ｆLNが、ｆH＜ｆLN＜ｆLであることを特徴とするロボット制御装置。
【請求項４】
請求項１に記載のロボット制御装置において、
前記第１の周波数ｆHと前記第２の周波数ｆLとが、ｆH＞ｆLであることを特徴とするロボット制御装置。
【請求項５】
請求項４に記載のロボット制御装置において、
電気的なノイズの周波数のうちの少なくと一つの周波数ｆnoが、ｆH＞ｆno＞ｆLであることを特徴とするロボット制御装置。
【請求項６】
請求項４に記載のロボット制御装置において、
外部からの物理的な振動の周波数のうちの少なくとも一つの周波数ｆphが、ｆH＞ｆph＞ｆLであることを特徴とするロボット制御装置。
【請求項７】
請求項４に記載のロボット制御装置において、
前記基体、または前記アクチュエーターと前記トルク伝達機構を保持する筐体の固有周波数のうち、少なくとも一つの固有周波数ｆrbが、ｆH＞ｆrb＞ｆLであることを特徴とするロボット制御装置。
【請求項８】
基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、前記アクチュエーターの回転角θM1を検出する角度センサーと、前記基体に対する前記リンクの角速度ωL1を検出する角速度センサーと、ωL1の積分値のうち第１の周波数ｆH以上の高周波成分と、θM1＊Ｎのうち第２の周波数ｆL以下の低周波成分と、を用いて前記リンクの回転角を算出する演算部と、を備え、前記第１の周波数ｆHと前記第２の周波数ｆLとが異なるロボットの制御方法であって、
前記アクチュエーターが前記リンクを駆動するステップと、
前記角度センサーが、前記アクチュエーターの回転角θM1を検出するステップと、
前記角速度センサーが、前記基体に対する前記リンクの角速度ωL1を検出するステップと、
前記演算部が、ωL1の積分値のうちの第１の周波数ｆH以上の高周波成分と、θM1＊Ｎのうちの第２の周波数ｆL以下の低周波成分と、を用いて前記リンクの角度を算出するステップと、を含むことを特徴とするロボットの制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１１−５５７１（Ｐ２０１１−５５７１Ａ）
【公開日】平成２３年１月１３日（２０１１．１．１３）
【国際特許分類】
【出願番号】特願２００９−１４９４８２（Ｐ２００９−１４９４８２）
【出願日】平成２１年６月２４日（２００９．６．２４）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２３年１月１３日（２０１１．１．１３）
【国際特許分類】
【出願日】平成２１年６月２４日（２００９．６．２４）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]