JP2011005571A - ロボット制御装置、ロボットの制御方法 - Google Patents

Abstract

【課題】ロボットの制御を可能にするロボット制御装置を提供する。
【解決手段】ロボット制御装置は、基体１に対して回転運動可能なリンク３と、リンク３を駆動するモーター４と、モーター４のトルクをリンク３に減速比Ｎで伝達する減速機５と、モーター４の回転角θ_M1を検出する角度センサー６と、基体１に対するリンク３の角速度ω_L1を検出する角速度センサー１２と、角速度ω_L1の積分値のうち第１の周波数ｆ_H以上の高周波成分、及びθ_M1＊Ｎのうち第２の周波数ｆ_L以下の低周波成分を用いリンク３の回転角を算出する演算部１５と、を備え、第１の周波数ｆ_Hと第２の周波数ｆ_Lとが異なる。このことによって、角速度センサー１２のバイアスドリフトの影響を排除し、特定の周波数を強調したり除去した角度の信号を得ることができる。
【選択図】図１

Description

本発明は、ロボットの制御装置、及びロボットの制御方法に関する。

従来、ロボットを駆動する際に、エンコーダーでモーターの回転角を制御し、リンクの制御を行っている。しかし、モーターとリンクとの間のトルク伝達機構やリンクは剛体ではないため動作時に振動が発生し、正確なリンクの作動を制御できないという課題を有していた。

そこで、リンク（例えば、アーム等）に慣性センサーを取り付け、この慣性センサーの出力信号を用いてリンクの動作制御を行うロボット制御装置が提案されている（例えば、特許文献１、特許文献２参照）。

また、エンコーダーの低周波成分と慣性センサーの高周波成分とを合成し、リンクの回転角度を制御するロボット制御装置も提案されている（例えば、特許文献３）。

特開平１−１７３１１６号公報 特開平７−９７３４号公報 特許第３８８３５４４号公報

このような特許文献１や特許文献２では、慣性センサーのバイアスドリフトに起因する定常の速度偏差や位置偏差をもつという課題を有している。

この課題に対して、特許文献３では、エンコーダーの低周波成分と慣性センサーの高周波成分とを合成することで、慣性センサーのバイアスドリフトの影響を抑制することを可能としている。

しかしながら、エンコーダーの低周波成分を取り出すためのローパスフィルターと、慣性センサーの高周波成分を取り出すためのハイパスフィルターの遮断周波数（カットオフ周波数）を一致させているために、低周波成分と高周波成分とを合成させた場合に周波数特性が平坦になる。つまり、ゲインが平坦になる。従って、合成感度が平坦になることから、強調したい周波数帯、または除去したい周波数帯がある場合には別に外付けのフィルターを設けなければならないという課題を有している。

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。

［適用例１］本適用例に係るロボット制御装置は、基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、前記アクチュエーターの回転角θ_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はそれぞれ、低周波成分及び高周波成分を取り出すために用いるフィルターの遮断周波数（カットオフ周波数）に相当し、従来技術のように、強調したい周波数帯または除去したい周波数帯がある場合に設けられる外付けのフィルターを必要としないため、コストの低減や演算部の演算量を減らすことができる。

実施形態１に係るロボットの概略構成を示す正面断面図。 リンクの角度を算出する過程を示すフロー図。 実施例１に係る周波数とゲインの関係を示すグラフ。 実施例２に係る周波数とゲインの関係を示すグラフ。 実施例２において２次のフィルターを用いた周波数とゲインの関係を示すグラフ。 モーター角度の応答とリンク角度の応答からみた周波数とゲインの関係を示すグラフ。

以下、本発明の実施形態を図面に基づいて説明する。
（実施形態１）

図１は、実施形態１に係るロボットの概略構成を示す正面断面図である。ロボット１００は、基体１と、リンク３，８と、アクチュエーターとしてのモーター４，９と、トルク伝達機構としての減速機５，１１と、角度センサー６，１０と、角速度センサー１２，１３とから構成されている。

基体１の上部にリンク３の一端が取付けられており、リンク３の他端にはリンク８が取付けられている。リンク３は、軸２を回転軸として基体１に対して相対的に回転運動することができる。また、リンク８は、軸７を回転軸としてリンク３に対して相対的に回転運動することができる。

モーター４は減速機５を介してリンク３を駆動し、モーター９は減速機１１を介してリンク８を駆動する。減速機５及び減速機１１の減速比をそれぞれＮ₁、Ｎ₂とする。角度センサー６はモーター４の頂部に設けられ、軸２回りのモーター回転角θ_M1を検出する。角度センサー１０はモーター９の頂部に設けられ、軸７回りのモーター回転角θ_M2を検出する。

角速度センサー１２はリンク３に設けられ、角速度センサー１３はリンク８に設けられている。角速度センサー１２は基体１に対するリンク３の角速度ω_L1を検出し、角速度センサー１３はリンク３に対するリンク８の角速度ω_L2を検出する。なお、リンク８の先端方向には回転装置３０が設けられ、回転装置３０には昇降装置３１が軸止されている。そして、昇降装置３１の先端部にはワークのチャック部３２が設けられている。

角速度センサー１２，１３は、それぞれリンク３，８の先端部に配置されている。なお、リンク３，８を剛体とみなすのに十分な剛性を有する場合、リンク３，８のそれぞれにおいて角速度は一定であるので、角速度センサー１２，１３はリンク３，８それぞれの任意の位置に配設することができる。リンク３，８が十分な剛性を有しない場合、角速度センサー１２，１３は、それぞれリンク３，８の先端部に配置する方が望ましい。なお、角速度ω_L1、ω_L2は、リンク３，８の回転面内の角速度成分である。

なお、モーター４及び減速機５は筐体２０によって保持されており、モーター９及び減速機１１は筐体２１によって保持されている。
続いて、本実施形態によるリンクの角度を算出する過程について説明する。

図２は、本実施形態に係るリンクの角度を算出する過程を示すフロー図である。なお、基体１に対するリンク３の角度と、リンク３に対するリンク８の角度の算出はリンク３に対するリンク８の角速度をω_L1−ω_L2とすることと同じ考え方で可能なため、基体１に対するリンク３の角度θ_A1の算出方法を例示して説明する。図１も参照する。

図２おいて、ステップＳ１０及びステップＳ６０以外の演算処理は演算部１５（図１、参照）によって実行される。なお、演算部１５は、ロボット１００内の任意の位置に配置されるＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、またはロボット１００の外部に設けられるＣＰＵによって実行してもよい。

まず、角速度センサー１２が角速度ω₁を検出する（Ｓ１０）。次に、リンク３の単独の角速度を算出する（Ｓ２０）。リンク３の角速度はｄθ_A1／ｄｔで算出される。角速度ω₁は基体１に対する角速度である。次に、ｄθ_A1／ｄｔを積分する（Ｓ３０）。

さらに、この積分結果をハイパスフィルターＨ_Fに通過させる（Ｓ４０）。これにより、角度θ_A1のうち第１の周波数ｆ_H以上の高周波成分が得られる。なお、第１の周波数ｆ_Hは、ハイパスフィルターＨ_Fの遮断周波数（カットオフ周波数）のため、以降、遮断周波数ｆ_Hまたはカットオフ周波数ｆ_Hと表す。ここで、角度θ_A1の低周波成分を除去する理由は、リンク３の角速度ｄθ_A1／ｄｔの積分ではバイアスドリフトの影響によりリンク３の角度θ_A1を正確に求められないので、このバイアスドリフトの影響を除去するためである。

ロボット１００が停止または極低周波帯域で動作している場合には、減速機５はほとんど捩れない。そこで、角度センサー６でモーター回転角θ_M1を検出する（Ｓ６０）。次に、減速比Ｎ₁をモーター回転角θ_M1に乗算し、θ_A1＝θ_M1＊Ｎ₁を算出する（Ｓ７０）。

続いて、θ_M1＊Ｎ₁をローパスフィルターＬ_Fに通過させる（Ｓ８０）。これにより、リンク３の角度θ_A1のうち第２の周波数ｆ_L以下の低周波成分が得られる。なお、第２の周波数ｆ_Hは、ローパスフィルターＬ_Fの遮断周波数（カットオフ周波数）のため、以降、遮断周波数ｆ_Lまたはカットオフ周波数ｆ_L表す。ここで、角度θ_A1の高周波成分を除去する理由は、高周波成分では減速機５が捩れるためθ_A1＝θ_M1＊Ｎ₁が成立しなくなるからである。

このように、ハイパスフィルターＨ_FとローパスフィルターＬ_Fを用いることにより、ステップＳ１０〜Ｓ４０により得られた角度θ_A1の高周波成分と、ステップＳ６０〜Ｓ８０によって得られた角度θ_A1の低周波成分とを加算する（Ｓ１００）。この演算によって、基体１に対するリンク３の角度θ_A1を算出することができる。

また、ステップＳ２０において得られた角速度ω₁（ｄθ_A1／ｄｔ）を１回近似微分することによって、リンク３の角加速度ｄ²θ_A1／ｄｔ²を算出することができる（Ｓ５０）。また、ステップＳ６０において得られたモーター回転角θ_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の測定を行うことができる。従って、この測定結果に基づき制御を行うことでより効果的にリンク３の振動を抑制することができる。

同様に、２次のフィルターを用いたときの周波数とゲインの関係をモーター角度の応答とリンク角度の応答から説明する。
図６は、実施例２において２次のフィルターを用いたときのモーター角度の応答とリンク角度の応答からみた周波数とゲインの関係を示すグラフである。なお、フィルターのカットオフ周波数は、ローパスフィルターが５Ｈｚ、ハイパスフィルターが１５Ｈｚとした。

図６に示すように、モーター角度の応答信号と、リンク角度の応答信号を合成した合成角度は、５Ｈｚ〜１５Ｈｚの周波数帯でゲイン（振幅）が下がっている。従って、リンクの角度として検出したくない外乱信号が入る場合に、その信号を除去するノッチフィルターの役割を兼ねることが可能になる。

１…基体、３…リンク、４…アクチュエーターとしてのモーター、５…トルク伝達機構としての減速機、６…角度センサー、１２…角速度センサー、１５…演算部、１００…ロボット。

Claims (8)

1. 基体に対して回転運動可能なリンクと、
   前記リンクを駆動するアクチュエーターと、
   前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、
   前記アクチュエーターの回転角θ_M1を検出する角度センサーと、
   前記基体に対する前記リンクの角速度ω_L1を検出する角速度センサーと、
   ω_L1の積分値のうち第１の周波数ｆ_H以上の高周波成分とθ_M1＊Ｎのうち第２の周波数ｆ_L以下の低周波成分と、を用いて前記リンクの回転角を算出する演算部と、を備え、
   前記第１の周波数ｆ_Hと前記第２の周波数ｆ_Lとが異なることを特徴とするロボット制御装置。
2. 請求項１に記載のロボット制御装置において、
   前記第１の周波数ｆ_Hと前記第２の周波数ｆ_Lとが、ｆ_H＜ｆ_Lであることを特徴とするロボット制御装置。
3. 請求項２に記載のロボット制御装置において、
   前記リンクと前記アクチュエーターと前記トルク伝達機構からなる系の固有振動数のうちの少なくとも一つの固有振動数ｆ_LNが、ｆ_H＜ｆ_LN＜ｆ_Lであることを特徴とするロボット制御装置。
4. 請求項１に記載のロボット制御装置において、
   前記第１の周波数ｆ_Hと前記第２の周波数ｆ_Lとが、ｆ_H＞ｆ_Lであることを特徴とするロボット制御装置。
5. 請求項４に記載のロボット制御装置において、
   電気的なノイズの周波数のうちの少なくと一つの周波数ｆ_noが、ｆ_H＞ｆ_no＞ｆ_Lであることを特徴とするロボット制御装置。
6. 請求項４に記載のロボット制御装置において、
   外部からの物理的な振動の周波数のうちの少なくとも一つの周波数ｆ_phが、ｆ_H＞ｆ_ph＞ｆ_Lであることを特徴とするロボット制御装置。
7. 請求項４に記載のロボット制御装置において、
   前記基体、または前記アクチュエーターと前記トルク伝達機構を保持する筐体の固有周波数のうち、少なくとも一つの固有周波数ｆ_rbが、ｆ_H＞ｆ_rb＞ｆ_Lであることを特徴とするロボット制御装置。
8. 基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速比Ｎで伝達するトルク伝達機構と、前記アクチュエーターの回転角θ_M1を検出する角度センサーと、前記基体に対する前記リンクの角速度ω_L1を検出する角速度センサーと、ω_L1の積分値のうち第１の周波数ｆ_H以上の高周波成分と、θ_M1＊Ｎのうち第２の周波数ｆ_L以下の低周波成分と、を用いて前記リンクの回転角を算出する演算部と、を備え、前記第１の周波数ｆ_Hと前記第２の周波数ｆ_Lとが異なるロボットの制御方法であって、
   前記アクチュエーターが前記リンクを駆動するステップと、
   前記角度センサーが、前記アクチュエーターの回転角θ_M1を検出するステップと、
   前記角速度センサーが、前記基体に対する前記リンクの角速度ω_L1を検出するステップと、
   前記演算部が、ω_L1の積分値のうちの第１の周波数ｆ_H以上の高周波成分と、θ_M1＊Ｎのうちの第２の周波数ｆ_L以下の低周波成分と、を用いて前記リンクの角度を算出するステップと、を含むことを特徴とするロボットの制御方法。

Images