ロボット装置、ロボット装置の制御方法およびプログラム
【課題】従来のロボット装置の制御方法にあっては、制御装置における演算量が多くなり処理に時間が掛かったり、処理速度を上げるために制御装置のコストが増加したりする問題があった。
【解決手段】アクチュエーターの角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算部と、前記第１演算部の演算対象の前記アクチュエーターを含む連結装置を介して連結される前記アームに備える慣性センサーの角速度検出データより、前記連結装置を軸とする前記アームの角速度を演算する第２演算部と、低周波成分を除去した、前記アクチュエーターと前記アームとの間のねじれ角速度を演算する第３演算部とを備えるロボット装置。
【解決手段】アクチュエーターの角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算部と、前記第１演算部の演算対象の前記アクチュエーターを含む連結装置を介して連結される前記アームに備える慣性センサーの角速度検出データより、前記連結装置を軸とする前記アームの角速度を演算する第２演算部と、低周波成分を除去した、前記アクチュエーターと前記アームとの間のねじれ角速度を演算する第３演算部とを備えるロボット装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット装置、ロボット装置の制御方法およびプログラムに関する。
【背景技術】
【０００２】
ＩＣハンドラーや組立装置の一部として多く使われている多関節構造を有するロボット装置は、様々な産業現場の中で多用されてきている。故に、ロボット装置には今まで以上に、求められる位置にいかに早く且つ正確にアームを移動させることができるかが重要な性能仕様、品質になってきている。
【０００３】
一般的にアームを高速に且つ正確に移動させるには、アームに掛かる慣性力を小さくし、駆動用のアクチュエーターの負荷を大きくさせないことにある。アームに掛かる慣性力を小さくするには、アームの軽量化が最も効果的な手法として用いられている。しかし、アームを軽量化することによりアーム剛性の低下を招き、アーム停止時に生じるアームの振動を抑制することが困難になり、アーム先端部を目的の位置で停止させても、アーム自体の振動の振幅分の位置ズレが生じてしまい、振動が減衰する時間まで次の動作を開始することができないという問題があった。
【０００４】
この問題に対して、アーム先端に加速度センサーを設置し加速度信号を基にアームを作動させ振動を抑制する方法（例えば、特許文献１）、アーム先端およびアームに角速度センサーを設置し、角速度信号を基にアーム動作を制御する方法（例えば、特許文献２）、などが提示されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平１−１７３１１６号公報
【特許文献２】特開２００５−２４２７９４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、従来のロボット装置の制御方法においては、角速度センサーもしくは加速度センサーのどちらかを用いて、振動を抑制する制御信号を生成しているため、センサー信号にバイアスドリフト等の誤差が含まれる場合に制御信号に誤差が生じ、正確な制御が行なえない場合があった。
【０００７】
例えば、特許文献２においては、センサーの誤差の影響を低減するために角度センサーの高周波成分を除去するローパスフィルターと、角速度センサーの低周波成分を除去するハイパスフィルターと、の２種類のフィルターを用いるために、制御装置における演算量が多くなり、処理に時間が掛かったり、処理速度を上げるために演算機のコストが増加したりする問題があった。
【課題を解決するための手段】
【０００８】
本発明は、少なくとも上述の課題の一つを解決するように、下記の形態または適用例として実現され得る。
【０００９】
〔適用例１〕本適用例によるロボット装置は、アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に設けられた前記アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算部と、前記第１演算部の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算部と、前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算部と、を備えることを特徴とする。
【００１０】
上述の適用例によれば、ねじれ角速度をロボット装置の振動抑制制御の基となるデータとして用いることで、正確な制御を可能とする。
【００１１】
〔適用例２〕上述の適用例において、前記低周波成分は、前記ロボット装置の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数であることを特徴とする。
【００１２】
上述の適用例によれば、反共振周波数および共振周波数をカットすることなく低周波成分をカットすることで、誤差を含む慣性センサーを用いた場合であっても、振幅の大きい領域において誤差を含まない慣性センサーにより得られる真の値に近いねじれ角速度を得ることができ、ねじれ角速度によって振動を正確に制御するロボット装置を得ることができる。
【００１３】
〔適用例３〕上述の適用例において、前記第１演算部により演算された前記アームの前記角速度と、前記第３演算部により演算された前記ねじれ角速度と、を加算する第４演算部を更に備えることを特徴とする。
【００１４】
上述の適用例によれば、ねじれ角速度の低周波成分には実動作が含まれない。従って、ねじれ角速度の低周波成分の除去は、すなわち慣性センサーの誤差（ノイズ）の除去であり、低周波成分の除去されたねじれ角速度、もしくは、低周波成分の除去されたねじれ角速度にアクチュエーターの角速度を加えて得られるアーム角速度、を制御データとして用いることで、正確なロボット装置の振動抑制制御を可能とする。
【００１５】
〔適用例４〕本適用例のロボット装置の制御方法は、アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に設けられた前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算工程と、前記第１演算工程の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算工程と、前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算工程と、を含むことを特徴とする。
【００１６】
上述の適用例によれば、ねじれ角速度をロボット装置の振動抑制制御の基となるデータとして用いることで、正確に制御することを可能とする。
【００１７】
〔適用例５〕上述の適用例において、前記低周波成分は、前記ロボット装置の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数であることを特徴とする。
【００１８】
上述の適用例によれば、反共振周波数および共振周波数をカットすることなく低周波成分をカットすることで、誤差を含む慣性センサーを用いた場合であっても、振幅の大きい領域において誤差を含まない慣性センサーにより得られる真の値に近いねじれ角速度を得ることができ、ねじれ角速度によるロボット装置の振動を正確に制御することができる。
【００１９】
〔適用例６〕上述の適用例において、前記第１演算工程により演算された前記アームの前記角速度と、前記第３演算工程により演算された前記ねじれ角速度と、を加算し前記アームの角速度を演算する第４演算工程を更に備えることを特徴とする。
【００２０】
上述の適用例によれば、ねじれ角速度の低周波成分には実動作が含まれない。従って、ねじれ角速度の低周波成分の除去は、すなわち慣性センサーの誤差（ノイズ）の除去であり、低周波成分の除去されたねじれ角速度、もしくは、低周波成分の除去されたねじれ角速度にアクチュエーターの角速度を加えて得られるアーム角速度、を制御データとして用いることで、正確にロボット装置の振動を制御し、抑制することを可能とする。
【００２１】
〔適用例７〕本適用例のプログラムは、アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に設けられた前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、を備えるロボット装置に制御を行わせるプログラムであって、コンピューターに、前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算手順と、前記第１演算手順の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算手順と、前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算手順と、を実行させる。
【００２２】
ロボット装置に備えるコンピューターに本適用例のプログラムを搭載するだけで、容易に上述のロボット装置の振動抑制制御を行うことができる。
【図面の簡単な説明】
【００２３】
【図１】実施形態に係るロボット装置の概要を示す、（ａ）は模式的な平面図、（ｂ）は模式的な断面図。
【図２】実施形態に係るロボット装置の制御ブロック図。
【図３】実施形態に係るロボット装置のアクチュエーターのトルクに対する、アクチュエーター、アーム、ねじれの角速度応答のボード線図。
【図４】実施形態に係る動作時のねじれ角速度周波数成分の低周波成分の除去前後を示す、（ａ）は誤差を持たない慣性センサー、（ｂ）は低周波誤差を持つ慣性センサーの場合のグラフ。
【図５】実施形態に係る制御方法のフローチャート。
【図６】実施例２を示すブロック図。
【発明を実施するための形態】
【００２４】
以下、図面を参照して、本発明に係る実施形態を説明する。
【００２５】
（実施形態）
本発明に係る実施形態について説明する。図１は実施形態に係るロボット装置を示し、（ａ）は概略平面図、（ｂ）は概略断面図である。本実施形態のロボット装置は、水平方向に回転可能に３本のアームが連結された、いわゆる３軸水平多関節ロボット１００（以下、ロボット装置１００という）である。
【００２６】
ロボット装置１００は、第１アーム１１と第２アーム１２とが第１アーム連結装置２１、第２アーム１２と第３アーム１３とが第２アーム連結装置２２によって回転可能に直列的に連結されて構成されるアーム体１０を備えている。アーム体１０は、更に基体連結装置３０により、基盤に固定された基体４０と回転可能に連結され、ロボット装置１００を構成している。
【００２７】
第１アーム連結装置２１は、アクチュエーター５１と、アクチュエーター５１のトルクを所定の減速比で伝達するトルク伝達装置６１と、を含み、第２アーム連結装置２２も同様にアクチュエーター５２とトルク伝達装置６２と、を含む。また、基体連結装置３０は、アクチュエーター５３と、アクチュエーター５３のトルクを所定の減速比で伝達するトルク伝達装置６３と、を含む。アーム体１０の基体４０とは反対の端部となる第３アーム１３の端部には加工用ツールもしくは被加工物を保持するワーク保持装置７０が備えられている。
【００２８】
第１アーム連結装置２１に含まれるアクチュエーター５１には回転角度を検出する角度センサー８１が備えられ、同様に第２アーム連結装置にはアクチュエーター５２に角度センサー８２が備えられている。また、基体連結装置３０にも、アクチュエーター５３に角度センサー８３が備えられている。更に、第１アーム１１には慣性センサー９１、第２アーム１２には慣性センサー９２、第３アーム１３には慣性センサー９３が備えられている。慣性センサー９１、９２、９３は、少なくとも角速度センサーを含み、慣性センサー９１は第１アーム１１の、慣性センサー９２は第２アーム１２の、慣性センサー９３は第３アーム１３の各慣性センサー取り付け位置での角速度を検出可能としている。
【００２９】
図２は、本実施形態に係るロボット装置１００の構成を示すブロック図である。ＣＰＵ２００は、後述する第１演算部５１０、第２演算部５２０、第３演算部５３０、第４演算部５４０および制御部６００を含み、ＲＯＭ３００に記憶されたプログラムを読み出して実行する。また、ＲＡＭ４００はＣＰＵ２００におけるプログラム実行によって得られるデータを保存し、ＣＰＵ２００へ保存されたデータから必要なデータを送出する。
【００３０】
ロボット装置１００に備える角度センサー８１、８２、８３によって検出されたアクチュエーター５１、５２、５３の回転角度データは、第１演算部５１０においてアクチュエーター５１の回転角度θ１、アクチュエーター５２の回転角度θ２、アクチュエーター５３の回転角度θ３、に換算され、換算されたそれぞれの回転角度θ１、θ２、θ３を時間で１回微分し、アクチュエーターの回転角速度を演算する。
【００３１】
得られたアクチュエーターの回転角速度から、アクチュエーターが駆動するアームの回転角速度を求める。第１アーム１１の場合は、アクチュエーター５３から減速比１／Ｎ３を持つトルク伝達装置６３によって駆動されるため、基体連結装置３０の出力部の回転角速度ω３は、
ω３＝（ｄθ３／ｄｔ）×（１／Ｎ３）
となる。
【００３２】
同様に、第２アーム１２を駆動するアクチュエーター５１を含む第１アーム連結装置２１の出力部の回転角速度ω１は、
ω１＝（ｄθ１／ｄｔ）×（１／Ｎ１）
Ｎ１：トルク伝達装置６１の減速比
となり、第３アーム１３を駆動するアクチュエーター５２を含む第２アーム連結装置２２の出力部の回転角速度ω２は、
ω２＝（ｄθ２／ｄｔ）×（１／Ｎ２）
Ｎ２：トルク伝達装置６２の減速比
となる。
【００３３】
第２演算部５２０では、第１アーム１１、第２アーム１２、第３アーム１３に備えられた慣性センサー９１、９２、９３が検出したデータから、第１アーム１１の角速度、第２アームの角速度、第３アームの角速度を演算する。検出データから演算される角速度は、基体連結装置３０を回転軸とする角速度である。従って、第２アーム１２の場合には第１アーム連結装置２１、第３アーム１３の場合には第２アーム連結装置２２を回転軸としたアームの角速度は、得られた角速度から更に演算する。
【００３４】
すなわち、第２アーム１２の場合には、第１アーム１１に備える慣性センサー９１から得られる検出データから換算される角速度ωa1と、第２アームに備える慣性センサー９２の検出データから換算される角速度ωa2から、第２アーム１２の第１アーム連結装置２１を回転軸とする角速度ωj2は、
ωj2＝ωa2−ωa1
と求められる。
【００３５】
同様に、第３アーム１３の場合には、第２アーム１２に備える慣性センサー９２から得られる検出データから換算される角速度ωa2と、第３アームに備える慣性センサー９３の検出データから換算される角速度ωa3から、第３アーム１３の第２アーム連結装置２２を回転軸とする角速度ωj3は、
ωj3＝ωa3−ωa2
と求められる。
【００３６】
第１アーム１１の場合、第１アーム１１は基体４０に備える基体連結装置３０を中心に回動するため、慣性センサー９１から得られた角速度ωa1が、第１アーム１１の角速度となる。
【００３７】
第３演算部５３０では、上記のように演算されたアクチュエーターの回転によるアクチュエーターを含む連結装置を回転軸とする第２アーム１２の角速度ω１、第３アームの角速度ω２、第１アーム１１の角速度ω３と、各アームに取り付けられた慣性センサーから得られた連結装置を回転軸とする第２アームの角速度ωj2、第３アームの角速度ωj3、第１アームの角速度ωa1との差であるねじれ角速度を演算する。
【００３８】
すなわち、基体連結装置３０と第１アーム１１とのねじれ角速度ωt1は、
ωt1＝ωa1−ω１
と求められる。同様に、第１アーム連結装置２１と第２アーム１２とのねじれ角速度ωt2は、
ωt2＝ωJ2−ω２
と求められ、第２アーム連結装置２２と第３アーム１３とのねじれ角速度ωt3は、
ωt3＝ωJ3−ω３
と求められる。
【００３９】
図３はロボット装置１００におけるアクチュエーターのトルクに対する、アクチュエーターからトルク伝達装置を介してアームを駆動する角速度Ωactと、慣性センサーにより得られるアームの角速度Ωarmと、ねじれ角速度Ωtorの応答を説明するボード線図である。図３に示すように、Ωact、Ωarmにおいては、低周波成分に本来の動作成分も含まれるが、Ωtorには実際の動作成分がほとんど低周波成分には含まれない。この点に着眼し、本発明はねじれ角速度Ωtorの低周波成分をセンサーのバイアスドリフト等による誤差による影響分とみなし、除去することでロボット装置１００の低周波ノイズを除去し、ロボット装置１００の振動を正確に制御することを可能とするものである。
【００４０】
図４は、動作時のねじれ角速度の周波数成分を概念的に示す図であり、低周波成分をハイパスフィルターなどの手段によって除去する様子を示している。図４（ａ）は誤差の無い慣性センサーの場合、（ｂ）がバイアスドリフト等の低周波の誤差の有る慣性センサーの場合を示しており、（ａ）と（ｂ）の差が慣性センサーの誤差による成分を示している。
【００４１】
図４（ａ）に示す、低周波成分の除去前のねじれ角速度Ωtorを、ハイパスフィルターによって低周波成分を除去することでΩtor´となる。図４（ｂ）では、低周波成分の除去前のねじれ角速度Ω´torを、同様に低周波成分を除去することでΩ´tor´となる。図４（ａ）に示すように、低周波成分が除去されたねじれ角速度Ωtor´は、除去前のねじれ角速度Ωtorに対して、除去した周波数以下では振幅は小さくなる。しかし、その領域Ｓは図に示すように微小な値、例えば１０-5以下の領域であって、除去分は無視できるレベルである。
【００４２】
一方、図４（ｂ）に示す誤差を持つ慣性センサーの場合には、ハイパスフィルターによる低周波成分の除去前後に生じる差異領域Ｔは、振幅の大きい領域にある。従って、低周波成分を除去することで、図４（ｂ）における低周波成分除去前の誤差を含まない値Ｕに近づけることができる。よって、ねじれ角速度の低周波成分を除去することで、バイアスドリフト等の低周波成分の誤差を持つ慣性センサーを用いても、誤差の無い、言い換えると真の値に近いねじれ角速度を得ることができ、ねじれ角速度によるロボット装置１００の振動を正確に制御することが可能となる。
【００４３】
すなわち、上述のように求められたねじれ角速度ωt1、ωt2、ωt3に対して、その周波数成分のうち、低周波成分を図示しないハイパスフィルター（以下、ＨＰＦという）により除去したねじれ角速度Ωt1、Ωt2、Ωt3を演算する。ここで、ＨＰＦのフィルタリング周波数は、ロボット装置１００の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数のＨＰＦを用いることが好ましい。
【００４４】
低周波成分を除去したねじれ角速度Ωt1、Ωt2、Ωt3を用いて、制御部６００において制御信号を生成し、各アクチュエーター５１、５２、５３を制御することができる。この場合、第４演算部５４０を備え、低周波成分が除去されたねじれ角速度Ωt1、Ωt2、Ωt3に、各連結装置２１、２２、３０のトルク伝達装置６１、６２、６３を介して第２アーム１２、第３アーム１３、第１アーム１１に伝えられる角速度ω２、ω３、ω１を加算し、その結果を用いて制御部６００において制御信号を生成することも出来る。
【００４５】
図５は、本実施形態に係るロボット装置１００の制御方法を示すフローチャートである。ロボット装置１００の稼動状態において、先ず第１演算工程（Ｓ１１１）において、第１演算部はアクチュエーターに備える角度センサー８１、８２、８３から回転角度データを入手する。得られた角度データから、回転角度への換算と、回転角度を時間で１回微分を行い、第２アーム１２を駆動するアクチュエーター５１を含む第１アーム連結装置２１の出力部の回転角速度ω１、第３アーム１３を駆動するアクチュエーター５２を含む第２アーム連結装置２２の出力部の回転角速度ω２、基体連結装置３０の出力部の回転角速度ω３を演算する。
【００４６】
更に、同時に第２演算工程（Ｓ１１２）において、第２演算部に各アームに備える慣性センサー９１、９２、９３から角速度データを入手し、各アームを連結駆動する連結装置を回転軸とする角速度を演算する。すなわち、第２アーム１２では、第１アーム１１に備える慣性センサー９１から得られる検出データから換算される角速度ωa1と、第２アームに備える慣性センサー９２の検出データから換算される角速度ωa2から、
ωj2＝ωa2−ωa1
によって、第２アーム１２の第１アーム連結装置２１を回転軸とする角速度ωj2を演算する。
【００４７】
同様に、第３アーム１３では、第２アーム１２に備える慣性センサー９２から得られる検出データから換算される角速度ωa2と、第３アームに備える慣性センサー９３の検出データから換算される角速度ωa3から、
ωj3＝ωa3−ωa2
によって、第３アーム１３の第２アーム連結装置２２を回転軸とする角速度ωj3を演算する。なお、第１アーム１１では、第１アーム１１は基体４０に備える基体連結装置３０を中心に回動するため、慣性センサー９１から得られた角速度ωa1が、第１アーム１１の角速度となる。
【００４８】
次に、第３演算工程（Ｓ１２０）に移行する。第３演算工程（Ｓ１２０）では、第１演算工程（Ｓ１１１）で演算された角速度ω１、ω２、ω３と、第２演算工程（Ｓ１１２）で演算された角速度ωa1、ωj2、ωj3との差からねじれ角速度、
ωt1＝ωa1−ω１
ωt2＝ωj2−ω２
ωt3＝ωj3−ω３
を、演算する。
【００４９】
演算されたねじれ角速度ωt1、ωt2、ωt3を、ロボット装置１００の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数を除去するハイパスフィルターによりフィルタリングし、低周波成分を除去したねじれ角速度Ωt1、Ωt2、Ωt3を演算する。
【００５０】
次に、第４演算工程（Ｓ１３０）に移行する。第４演算工程（Ｓ１３０）では、第３演算工程（Ｓ１２０）で演算されえられた、ハイパスフィルターにより低周波成分を除去したねじれ角速度Ωt1、Ωt2、Ωt3に、各連結装置２１、２２、３０のトルク伝達装置６１、６２、６３を介して第２アーム１２、第３アーム１３、第１アーム１１に伝えられる角速度ω２、ω３、ω１を加算し、その結果を制御信号生成工程（Ｓ１４０）へ送る。
【００５１】
制御信号生成工程（Ｓ１４０）は、制御部６００において、得られた第４演算工程（Ｓ１３０）における演算結果を基に各アクチュエーターの制御信号を生成する。すなわち、ハイパスフィルターにより低周波成分を除去したねじれ角速度Ωt1、Ωt2、Ωt3に、各連結装置２１、２２、３０のトルク伝達装置６１、６２、６３を介して第２アーム１２、第３アーム１３、第１アーム１１に伝えられる角速度ω２、ω３、ω１を加算した角速度を基に、各アクチュエーターを制御するための信号を生成する。
【００５２】
次にロボット装置動作停止確認工程（Ｓ１５０）に移行する。ロボット装置動作停止確認工程（Ｓ１５０）では、ロボット装置１００が動作状態であるか、を確認し、動作状態（Ｎｏ）である場合には、第１演算工程（Ｓ１１１）、第２演算工程（Ｓ１１２）に戻り、制御を繰り返す。動作停止状態（Ｙｅｓ）である場合には、制御は終了する。
【００５３】
上述の通り、ねじれ角速度を制御データとすること、更に低周波成分を除去したねじれ角速度を制御データとすることで、慣性センサーの誤差分を除去した正確なロボット装置の振動抑制制御を可能とする。
【実施例】
【００５４】
（実施例１）
実施形態に係るロボット装置の、ねじれ角速度を基にした制御の実施例について説明する。アクチュエーターへのトルク指令値τを算出する方程式に、アクチュエーター角度、アーム角度（リンク角度）、アクチュエーター角速度、アーム角速度の４状態量が用いられ、このうちアーム角速度に上述の実施形態で説明した「低周波成分を除去したねじれ角速度（Ωt1、Ωt2、Ωt3）＋連結装置のトルク伝達装置を介してアームに伝えられる角速度（ω２、ω３、ω１）」を用いる。
【００５５】
上述の４状態量であるアクチュエーター角度と、アーム角度（リンク角度）と、アクチュエーター角速度と、低周波成分を除去したねじれ角速度に連結装置のトルク伝達装置を介してアームに伝えられる角速度を加えたアーム角速度と、を測定し、測定された４状態量を基に、下記（式１）により、状態フィードバック制御系を構築する。
【００５６】
【数１】
【００５７】
（式１）におけるｋ1〜ｋ5は、制御装置の内部の制御ゲインであり、極配置法や最適レギュレーター等によって決定することができる。こうして得られたトルク指令値τを用いて制御することで、ロボット装置の振動を抑制し、正確な動作を可能とする。
【００５８】
上述の実施例において、アーム角速度に上述の実施形態で説明した「低周波成分を除去したねじれ角速度（Ωt1、Ωt2、Ωt3）＋連結装置のトルク伝達装置を介してアームに伝えられる角速度（ω２、ω３、ω１）」を用いる例を説明したが、状態量としてアーム角速度に換えて「ねじれ角速度」を用いることで、演算量の少ない制御系を構築することができる。
【００５９】
すなわち、（式１）におけるアーム角度θa、およびその１回微分のアーム角速度ｄθa／ｄｔをねじれ角速度（実施形態におけるΩt1、Ωt2、Ωt3）として用いて、下記（式２）により制御系を構築する。
【００６０】
【数２】
【００６１】
（式２）におけるｋt1〜ｋt5は、制御装置の内部の制御ゲインであり、極配置法や最適レギュレーター等によって決定することができる。こうして得られたトルク指令値τを用いて制御することで、ロボット装置の振動を抑制し、正確な動作を可能とする。
【００６２】
（実施例２）
他の実施例として、図６に示すブロック図によって説明する。図６は上述の実施形態における図２に示すブロック図に対して、制御部６１０における位置制御系６１０ａ、速度制御系６１０ｂが演算結果を取得し、制御する例を示す点で異なる。従って、実施形態と同じ構成には同じ符号を付し、説明は省略する。
【００６３】
図６に示す本実施例においても、ＣＰＵ２１０は、後述する第１演算部５１０、第２演算部５２０、第３演算部５３０および制御部６１０を含み、ＲＯＭ３１０に記憶されたプログラムを読み出して実行する。また、ＲＡＭ４１０はＣＰＵ２１０におけるプログラム実行によって得られるデータを保存し、ＣＰＵ２１０へ保存されたデータから必要なデータを送出する。
【００６４】
図６のブロック図に示すように、本実施例の制御は、第１演算部５１０において換算されるアクチュエーターの回転角度データを位置制御系６１０ａが取得する。また、同じく第１演算部において演算されたアクチュエーターの角速度データは速度制御系６１０ｂが取得する。第３演算部５３０で演算されたねじれ角速度データは、速度制御系６１０ｂが取得し、位置制御系６１０ａから入力される角速度指令と合わせて、速度制御系６１０ｂはロボット装置１００にトルク指令を出力する。このように制御することで、ロボット装置の振動を抑制し、正確な動作を可能とする。
【００６５】
上述の実施形態ならびに実施例は、図１に示す通り３本のアームを備える水平多関節型のアーム体１０により説明したが、これに限定はされず、２本のアームを備えるロボット装置、あるいは４本以上のアームを備えるロボット装置にも適用される。また、ロボット装置型式においても、直動式、垂直多関節型など、可動アームを備えるロボット装置にも適用できる。
【符号の説明】
【００６６】
１０…アーム体、１１，１２，１３…アーム、２１，２２…アーム連結装置、３０…基体連結装置、４０…基体、５１，５２，５３…アクチュエーター、６１，６２，６３…トルク伝達装置、７０…ワーク保持装置、８１，８２，８３…角度センサー、９１，９２，９３…慣性センサー、１００…ロボット装置。
【技術分野】
【０００１】
本発明は、ロボット装置、ロボット装置の制御方法およびプログラムに関する。
【背景技術】
【０００２】
ＩＣハンドラーや組立装置の一部として多く使われている多関節構造を有するロボット装置は、様々な産業現場の中で多用されてきている。故に、ロボット装置には今まで以上に、求められる位置にいかに早く且つ正確にアームを移動させることができるかが重要な性能仕様、品質になってきている。
【０００３】
一般的にアームを高速に且つ正確に移動させるには、アームに掛かる慣性力を小さくし、駆動用のアクチュエーターの負荷を大きくさせないことにある。アームに掛かる慣性力を小さくするには、アームの軽量化が最も効果的な手法として用いられている。しかし、アームを軽量化することによりアーム剛性の低下を招き、アーム停止時に生じるアームの振動を抑制することが困難になり、アーム先端部を目的の位置で停止させても、アーム自体の振動の振幅分の位置ズレが生じてしまい、振動が減衰する時間まで次の動作を開始することができないという問題があった。
【０００４】
この問題に対して、アーム先端に加速度センサーを設置し加速度信号を基にアームを作動させ振動を抑制する方法（例えば、特許文献１）、アーム先端およびアームに角速度センサーを設置し、角速度信号を基にアーム動作を制御する方法（例えば、特許文献２）、などが提示されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平１−１７３１１６号公報
【特許文献２】特開２００５−２４２７９４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、従来のロボット装置の制御方法においては、角速度センサーもしくは加速度センサーのどちらかを用いて、振動を抑制する制御信号を生成しているため、センサー信号にバイアスドリフト等の誤差が含まれる場合に制御信号に誤差が生じ、正確な制御が行なえない場合があった。
【０００７】
例えば、特許文献２においては、センサーの誤差の影響を低減するために角度センサーの高周波成分を除去するローパスフィルターと、角速度センサーの低周波成分を除去するハイパスフィルターと、の２種類のフィルターを用いるために、制御装置における演算量が多くなり、処理に時間が掛かったり、処理速度を上げるために演算機のコストが増加したりする問題があった。
【課題を解決するための手段】
【０００８】
本発明は、少なくとも上述の課題の一つを解決するように、下記の形態または適用例として実現され得る。
【０００９】
〔適用例１〕本適用例によるロボット装置は、アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に設けられた前記アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算部と、前記第１演算部の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算部と、前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算部と、を備えることを特徴とする。
【００１０】
上述の適用例によれば、ねじれ角速度をロボット装置の振動抑制制御の基となるデータとして用いることで、正確な制御を可能とする。
【００１１】
〔適用例２〕上述の適用例において、前記低周波成分は、前記ロボット装置の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数であることを特徴とする。
【００１２】
上述の適用例によれば、反共振周波数および共振周波数をカットすることなく低周波成分をカットすることで、誤差を含む慣性センサーを用いた場合であっても、振幅の大きい領域において誤差を含まない慣性センサーにより得られる真の値に近いねじれ角速度を得ることができ、ねじれ角速度によって振動を正確に制御するロボット装置を得ることができる。
【００１３】
〔適用例３〕上述の適用例において、前記第１演算部により演算された前記アームの前記角速度と、前記第３演算部により演算された前記ねじれ角速度と、を加算する第４演算部を更に備えることを特徴とする。
【００１４】
上述の適用例によれば、ねじれ角速度の低周波成分には実動作が含まれない。従って、ねじれ角速度の低周波成分の除去は、すなわち慣性センサーの誤差（ノイズ）の除去であり、低周波成分の除去されたねじれ角速度、もしくは、低周波成分の除去されたねじれ角速度にアクチュエーターの角速度を加えて得られるアーム角速度、を制御データとして用いることで、正確なロボット装置の振動抑制制御を可能とする。
【００１５】
〔適用例４〕本適用例のロボット装置の制御方法は、アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に設けられた前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算工程と、前記第１演算工程の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算工程と、前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算工程と、を含むことを特徴とする。
【００１６】
上述の適用例によれば、ねじれ角速度をロボット装置の振動抑制制御の基となるデータとして用いることで、正確に制御することを可能とする。
【００１７】
〔適用例５〕上述の適用例において、前記低周波成分は、前記ロボット装置の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数であることを特徴とする。
【００１８】
上述の適用例によれば、反共振周波数および共振周波数をカットすることなく低周波成分をカットすることで、誤差を含む慣性センサーを用いた場合であっても、振幅の大きい領域において誤差を含まない慣性センサーにより得られる真の値に近いねじれ角速度を得ることができ、ねじれ角速度によるロボット装置の振動を正確に制御することができる。
【００１９】
〔適用例６〕上述の適用例において、前記第１演算工程により演算された前記アームの前記角速度と、前記第３演算工程により演算された前記ねじれ角速度と、を加算し前記アームの角速度を演算する第４演算工程を更に備えることを特徴とする。
【００２０】
上述の適用例によれば、ねじれ角速度の低周波成分には実動作が含まれない。従って、ねじれ角速度の低周波成分の除去は、すなわち慣性センサーの誤差（ノイズ）の除去であり、低周波成分の除去されたねじれ角速度、もしくは、低周波成分の除去されたねじれ角速度にアクチュエーターの角速度を加えて得られるアーム角速度、を制御データとして用いることで、正確にロボット装置の振動を制御し、抑制することを可能とする。
【００２１】
〔適用例７〕本適用例のプログラムは、アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、前記アーム体の一方の端部に設けられた前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、を備えるロボット装置に制御を行わせるプログラムであって、コンピューターに、前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算手順と、前記第１演算手順の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算手順と、前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算手順と、を実行させる。
【００２２】
ロボット装置に備えるコンピューターに本適用例のプログラムを搭載するだけで、容易に上述のロボット装置の振動抑制制御を行うことができる。
【図面の簡単な説明】
【００２３】
【図１】実施形態に係るロボット装置の概要を示す、（ａ）は模式的な平面図、（ｂ）は模式的な断面図。
【図２】実施形態に係るロボット装置の制御ブロック図。
【図３】実施形態に係るロボット装置のアクチュエーターのトルクに対する、アクチュエーター、アーム、ねじれの角速度応答のボード線図。
【図４】実施形態に係る動作時のねじれ角速度周波数成分の低周波成分の除去前後を示す、（ａ）は誤差を持たない慣性センサー、（ｂ）は低周波誤差を持つ慣性センサーの場合のグラフ。
【図５】実施形態に係る制御方法のフローチャート。
【図６】実施例２を示すブロック図。
【発明を実施するための形態】
【００２４】
以下、図面を参照して、本発明に係る実施形態を説明する。
【００２５】
（実施形態）
本発明に係る実施形態について説明する。図１は実施形態に係るロボット装置を示し、（ａ）は概略平面図、（ｂ）は概略断面図である。本実施形態のロボット装置は、水平方向に回転可能に３本のアームが連結された、いわゆる３軸水平多関節ロボット１００（以下、ロボット装置１００という）である。
【００２６】
ロボット装置１００は、第１アーム１１と第２アーム１２とが第１アーム連結装置２１、第２アーム１２と第３アーム１３とが第２アーム連結装置２２によって回転可能に直列的に連結されて構成されるアーム体１０を備えている。アーム体１０は、更に基体連結装置３０により、基盤に固定された基体４０と回転可能に連結され、ロボット装置１００を構成している。
【００２７】
第１アーム連結装置２１は、アクチュエーター５１と、アクチュエーター５１のトルクを所定の減速比で伝達するトルク伝達装置６１と、を含み、第２アーム連結装置２２も同様にアクチュエーター５２とトルク伝達装置６２と、を含む。また、基体連結装置３０は、アクチュエーター５３と、アクチュエーター５３のトルクを所定の減速比で伝達するトルク伝達装置６３と、を含む。アーム体１０の基体４０とは反対の端部となる第３アーム１３の端部には加工用ツールもしくは被加工物を保持するワーク保持装置７０が備えられている。
【００２８】
第１アーム連結装置２１に含まれるアクチュエーター５１には回転角度を検出する角度センサー８１が備えられ、同様に第２アーム連結装置にはアクチュエーター５２に角度センサー８２が備えられている。また、基体連結装置３０にも、アクチュエーター５３に角度センサー８３が備えられている。更に、第１アーム１１には慣性センサー９１、第２アーム１２には慣性センサー９２、第３アーム１３には慣性センサー９３が備えられている。慣性センサー９１、９２、９３は、少なくとも角速度センサーを含み、慣性センサー９１は第１アーム１１の、慣性センサー９２は第２アーム１２の、慣性センサー９３は第３アーム１３の各慣性センサー取り付け位置での角速度を検出可能としている。
【００２９】
図２は、本実施形態に係るロボット装置１００の構成を示すブロック図である。ＣＰＵ２００は、後述する第１演算部５１０、第２演算部５２０、第３演算部５３０、第４演算部５４０および制御部６００を含み、ＲＯＭ３００に記憶されたプログラムを読み出して実行する。また、ＲＡＭ４００はＣＰＵ２００におけるプログラム実行によって得られるデータを保存し、ＣＰＵ２００へ保存されたデータから必要なデータを送出する。
【００３０】
ロボット装置１００に備える角度センサー８１、８２、８３によって検出されたアクチュエーター５１、５２、５３の回転角度データは、第１演算部５１０においてアクチュエーター５１の回転角度θ１、アクチュエーター５２の回転角度θ２、アクチュエーター５３の回転角度θ３、に換算され、換算されたそれぞれの回転角度θ１、θ２、θ３を時間で１回微分し、アクチュエーターの回転角速度を演算する。
【００３１】
得られたアクチュエーターの回転角速度から、アクチュエーターが駆動するアームの回転角速度を求める。第１アーム１１の場合は、アクチュエーター５３から減速比１／Ｎ３を持つトルク伝達装置６３によって駆動されるため、基体連結装置３０の出力部の回転角速度ω３は、
ω３＝（ｄθ３／ｄｔ）×（１／Ｎ３）
となる。
【００３２】
同様に、第２アーム１２を駆動するアクチュエーター５１を含む第１アーム連結装置２１の出力部の回転角速度ω１は、
ω１＝（ｄθ１／ｄｔ）×（１／Ｎ１）
Ｎ１：トルク伝達装置６１の減速比
となり、第３アーム１３を駆動するアクチュエーター５２を含む第２アーム連結装置２２の出力部の回転角速度ω２は、
ω２＝（ｄθ２／ｄｔ）×（１／Ｎ２）
Ｎ２：トルク伝達装置６２の減速比
となる。
【００３３】
第２演算部５２０では、第１アーム１１、第２アーム１２、第３アーム１３に備えられた慣性センサー９１、９２、９３が検出したデータから、第１アーム１１の角速度、第２アームの角速度、第３アームの角速度を演算する。検出データから演算される角速度は、基体連結装置３０を回転軸とする角速度である。従って、第２アーム１２の場合には第１アーム連結装置２１、第３アーム１３の場合には第２アーム連結装置２２を回転軸としたアームの角速度は、得られた角速度から更に演算する。
【００３４】
すなわち、第２アーム１２の場合には、第１アーム１１に備える慣性センサー９１から得られる検出データから換算される角速度ωa1と、第２アームに備える慣性センサー９２の検出データから換算される角速度ωa2から、第２アーム１２の第１アーム連結装置２１を回転軸とする角速度ωj2は、
ωj2＝ωa2−ωa1
と求められる。
【００３５】
同様に、第３アーム１３の場合には、第２アーム１２に備える慣性センサー９２から得られる検出データから換算される角速度ωa2と、第３アームに備える慣性センサー９３の検出データから換算される角速度ωa3から、第３アーム１３の第２アーム連結装置２２を回転軸とする角速度ωj3は、
ωj3＝ωa3−ωa2
と求められる。
【００３６】
第１アーム１１の場合、第１アーム１１は基体４０に備える基体連結装置３０を中心に回動するため、慣性センサー９１から得られた角速度ωa1が、第１アーム１１の角速度となる。
【００３７】
第３演算部５３０では、上記のように演算されたアクチュエーターの回転によるアクチュエーターを含む連結装置を回転軸とする第２アーム１２の角速度ω１、第３アームの角速度ω２、第１アーム１１の角速度ω３と、各アームに取り付けられた慣性センサーから得られた連結装置を回転軸とする第２アームの角速度ωj2、第３アームの角速度ωj3、第１アームの角速度ωa1との差であるねじれ角速度を演算する。
【００３８】
すなわち、基体連結装置３０と第１アーム１１とのねじれ角速度ωt1は、
ωt1＝ωa1−ω１
と求められる。同様に、第１アーム連結装置２１と第２アーム１２とのねじれ角速度ωt2は、
ωt2＝ωJ2−ω２
と求められ、第２アーム連結装置２２と第３アーム１３とのねじれ角速度ωt3は、
ωt3＝ωJ3−ω３
と求められる。
【００３９】
図３はロボット装置１００におけるアクチュエーターのトルクに対する、アクチュエーターからトルク伝達装置を介してアームを駆動する角速度Ωactと、慣性センサーにより得られるアームの角速度Ωarmと、ねじれ角速度Ωtorの応答を説明するボード線図である。図３に示すように、Ωact、Ωarmにおいては、低周波成分に本来の動作成分も含まれるが、Ωtorには実際の動作成分がほとんど低周波成分には含まれない。この点に着眼し、本発明はねじれ角速度Ωtorの低周波成分をセンサーのバイアスドリフト等による誤差による影響分とみなし、除去することでロボット装置１００の低周波ノイズを除去し、ロボット装置１００の振動を正確に制御することを可能とするものである。
【００４０】
図４は、動作時のねじれ角速度の周波数成分を概念的に示す図であり、低周波成分をハイパスフィルターなどの手段によって除去する様子を示している。図４（ａ）は誤差の無い慣性センサーの場合、（ｂ）がバイアスドリフト等の低周波の誤差の有る慣性センサーの場合を示しており、（ａ）と（ｂ）の差が慣性センサーの誤差による成分を示している。
【００４１】
図４（ａ）に示す、低周波成分の除去前のねじれ角速度Ωtorを、ハイパスフィルターによって低周波成分を除去することでΩtor´となる。図４（ｂ）では、低周波成分の除去前のねじれ角速度Ω´torを、同様に低周波成分を除去することでΩ´tor´となる。図４（ａ）に示すように、低周波成分が除去されたねじれ角速度Ωtor´は、除去前のねじれ角速度Ωtorに対して、除去した周波数以下では振幅は小さくなる。しかし、その領域Ｓは図に示すように微小な値、例えば１０-5以下の領域であって、除去分は無視できるレベルである。
【００４２】
一方、図４（ｂ）に示す誤差を持つ慣性センサーの場合には、ハイパスフィルターによる低周波成分の除去前後に生じる差異領域Ｔは、振幅の大きい領域にある。従って、低周波成分を除去することで、図４（ｂ）における低周波成分除去前の誤差を含まない値Ｕに近づけることができる。よって、ねじれ角速度の低周波成分を除去することで、バイアスドリフト等の低周波成分の誤差を持つ慣性センサーを用いても、誤差の無い、言い換えると真の値に近いねじれ角速度を得ることができ、ねじれ角速度によるロボット装置１００の振動を正確に制御することが可能となる。
【００４３】
すなわち、上述のように求められたねじれ角速度ωt1、ωt2、ωt3に対して、その周波数成分のうち、低周波成分を図示しないハイパスフィルター（以下、ＨＰＦという）により除去したねじれ角速度Ωt1、Ωt2、Ωt3を演算する。ここで、ＨＰＦのフィルタリング周波数は、ロボット装置１００の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数のＨＰＦを用いることが好ましい。
【００４４】
低周波成分を除去したねじれ角速度Ωt1、Ωt2、Ωt3を用いて、制御部６００において制御信号を生成し、各アクチュエーター５１、５２、５３を制御することができる。この場合、第４演算部５４０を備え、低周波成分が除去されたねじれ角速度Ωt1、Ωt2、Ωt3に、各連結装置２１、２２、３０のトルク伝達装置６１、６２、６３を介して第２アーム１２、第３アーム１３、第１アーム１１に伝えられる角速度ω２、ω３、ω１を加算し、その結果を用いて制御部６００において制御信号を生成することも出来る。
【００４５】
図５は、本実施形態に係るロボット装置１００の制御方法を示すフローチャートである。ロボット装置１００の稼動状態において、先ず第１演算工程（Ｓ１１１）において、第１演算部はアクチュエーターに備える角度センサー８１、８２、８３から回転角度データを入手する。得られた角度データから、回転角度への換算と、回転角度を時間で１回微分を行い、第２アーム１２を駆動するアクチュエーター５１を含む第１アーム連結装置２１の出力部の回転角速度ω１、第３アーム１３を駆動するアクチュエーター５２を含む第２アーム連結装置２２の出力部の回転角速度ω２、基体連結装置３０の出力部の回転角速度ω３を演算する。
【００４６】
更に、同時に第２演算工程（Ｓ１１２）において、第２演算部に各アームに備える慣性センサー９１、９２、９３から角速度データを入手し、各アームを連結駆動する連結装置を回転軸とする角速度を演算する。すなわち、第２アーム１２では、第１アーム１１に備える慣性センサー９１から得られる検出データから換算される角速度ωa1と、第２アームに備える慣性センサー９２の検出データから換算される角速度ωa2から、
ωj2＝ωa2−ωa1
によって、第２アーム１２の第１アーム連結装置２１を回転軸とする角速度ωj2を演算する。
【００４７】
同様に、第３アーム１３では、第２アーム１２に備える慣性センサー９２から得られる検出データから換算される角速度ωa2と、第３アームに備える慣性センサー９３の検出データから換算される角速度ωa3から、
ωj3＝ωa3−ωa2
によって、第３アーム１３の第２アーム連結装置２２を回転軸とする角速度ωj3を演算する。なお、第１アーム１１では、第１アーム１１は基体４０に備える基体連結装置３０を中心に回動するため、慣性センサー９１から得られた角速度ωa1が、第１アーム１１の角速度となる。
【００４８】
次に、第３演算工程（Ｓ１２０）に移行する。第３演算工程（Ｓ１２０）では、第１演算工程（Ｓ１１１）で演算された角速度ω１、ω２、ω３と、第２演算工程（Ｓ１１２）で演算された角速度ωa1、ωj2、ωj3との差からねじれ角速度、
ωt1＝ωa1−ω１
ωt2＝ωj2−ω２
ωt3＝ωj3−ω３
を、演算する。
【００４９】
演算されたねじれ角速度ωt1、ωt2、ωt3を、ロボット装置１００の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数を除去するハイパスフィルターによりフィルタリングし、低周波成分を除去したねじれ角速度Ωt1、Ωt2、Ωt3を演算する。
【００５０】
次に、第４演算工程（Ｓ１３０）に移行する。第４演算工程（Ｓ１３０）では、第３演算工程（Ｓ１２０）で演算されえられた、ハイパスフィルターにより低周波成分を除去したねじれ角速度Ωt1、Ωt2、Ωt3に、各連結装置２１、２２、３０のトルク伝達装置６１、６２、６３を介して第２アーム１２、第３アーム１３、第１アーム１１に伝えられる角速度ω２、ω３、ω１を加算し、その結果を制御信号生成工程（Ｓ１４０）へ送る。
【００５１】
制御信号生成工程（Ｓ１４０）は、制御部６００において、得られた第４演算工程（Ｓ１３０）における演算結果を基に各アクチュエーターの制御信号を生成する。すなわち、ハイパスフィルターにより低周波成分を除去したねじれ角速度Ωt1、Ωt2、Ωt3に、各連結装置２１、２２、３０のトルク伝達装置６１、６２、６３を介して第２アーム１２、第３アーム１３、第１アーム１１に伝えられる角速度ω２、ω３、ω１を加算した角速度を基に、各アクチュエーターを制御するための信号を生成する。
【００５２】
次にロボット装置動作停止確認工程（Ｓ１５０）に移行する。ロボット装置動作停止確認工程（Ｓ１５０）では、ロボット装置１００が動作状態であるか、を確認し、動作状態（Ｎｏ）である場合には、第１演算工程（Ｓ１１１）、第２演算工程（Ｓ１１２）に戻り、制御を繰り返す。動作停止状態（Ｙｅｓ）である場合には、制御は終了する。
【００５３】
上述の通り、ねじれ角速度を制御データとすること、更に低周波成分を除去したねじれ角速度を制御データとすることで、慣性センサーの誤差分を除去した正確なロボット装置の振動抑制制御を可能とする。
【実施例】
【００５４】
（実施例１）
実施形態に係るロボット装置の、ねじれ角速度を基にした制御の実施例について説明する。アクチュエーターへのトルク指令値τを算出する方程式に、アクチュエーター角度、アーム角度（リンク角度）、アクチュエーター角速度、アーム角速度の４状態量が用いられ、このうちアーム角速度に上述の実施形態で説明した「低周波成分を除去したねじれ角速度（Ωt1、Ωt2、Ωt3）＋連結装置のトルク伝達装置を介してアームに伝えられる角速度（ω２、ω３、ω１）」を用いる。
【００５５】
上述の４状態量であるアクチュエーター角度と、アーム角度（リンク角度）と、アクチュエーター角速度と、低周波成分を除去したねじれ角速度に連結装置のトルク伝達装置を介してアームに伝えられる角速度を加えたアーム角速度と、を測定し、測定された４状態量を基に、下記（式１）により、状態フィードバック制御系を構築する。
【００５６】
【数１】
【００５７】
（式１）におけるｋ1〜ｋ5は、制御装置の内部の制御ゲインであり、極配置法や最適レギュレーター等によって決定することができる。こうして得られたトルク指令値τを用いて制御することで、ロボット装置の振動を抑制し、正確な動作を可能とする。
【００５８】
上述の実施例において、アーム角速度に上述の実施形態で説明した「低周波成分を除去したねじれ角速度（Ωt1、Ωt2、Ωt3）＋連結装置のトルク伝達装置を介してアームに伝えられる角速度（ω２、ω３、ω１）」を用いる例を説明したが、状態量としてアーム角速度に換えて「ねじれ角速度」を用いることで、演算量の少ない制御系を構築することができる。
【００５９】
すなわち、（式１）におけるアーム角度θa、およびその１回微分のアーム角速度ｄθa／ｄｔをねじれ角速度（実施形態におけるΩt1、Ωt2、Ωt3）として用いて、下記（式２）により制御系を構築する。
【００６０】
【数２】
【００６１】
（式２）におけるｋt1〜ｋt5は、制御装置の内部の制御ゲインであり、極配置法や最適レギュレーター等によって決定することができる。こうして得られたトルク指令値τを用いて制御することで、ロボット装置の振動を抑制し、正確な動作を可能とする。
【００６２】
（実施例２）
他の実施例として、図６に示すブロック図によって説明する。図６は上述の実施形態における図２に示すブロック図に対して、制御部６１０における位置制御系６１０ａ、速度制御系６１０ｂが演算結果を取得し、制御する例を示す点で異なる。従って、実施形態と同じ構成には同じ符号を付し、説明は省略する。
【００６３】
図６に示す本実施例においても、ＣＰＵ２１０は、後述する第１演算部５１０、第２演算部５２０、第３演算部５３０および制御部６１０を含み、ＲＯＭ３１０に記憶されたプログラムを読み出して実行する。また、ＲＡＭ４１０はＣＰＵ２１０におけるプログラム実行によって得られるデータを保存し、ＣＰＵ２１０へ保存されたデータから必要なデータを送出する。
【００６４】
図６のブロック図に示すように、本実施例の制御は、第１演算部５１０において換算されるアクチュエーターの回転角度データを位置制御系６１０ａが取得する。また、同じく第１演算部において演算されたアクチュエーターの角速度データは速度制御系６１０ｂが取得する。第３演算部５３０で演算されたねじれ角速度データは、速度制御系６１０ｂが取得し、位置制御系６１０ａから入力される角速度指令と合わせて、速度制御系６１０ｂはロボット装置１００にトルク指令を出力する。このように制御することで、ロボット装置の振動を抑制し、正確な動作を可能とする。
【００６５】
上述の実施形態ならびに実施例は、図１に示す通り３本のアームを備える水平多関節型のアーム体１０により説明したが、これに限定はされず、２本のアームを備えるロボット装置、あるいは４本以上のアームを備えるロボット装置にも適用される。また、ロボット装置型式においても、直動式、垂直多関節型など、可動アームを備えるロボット装置にも適用できる。
【符号の説明】
【００６６】
１０…アーム体、１１，１２，１３…アーム、２１，２２…アーム連結装置、３０…基体連結装置、４０…基体、５１，５２，５３…アクチュエーター、６１，６２，６３…トルク伝達装置、７０…ワーク保持装置、８１，８２，８３…角度センサー、９１，９２，９３…慣性センサー、１００…ロボット装置。
【特許請求の範囲】
【請求項１】
アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、
複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
前記アーム体の一方の端部に設けられた前記アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、
前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、
前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算部と、
前記第１演算部の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算部と、
前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算部と、を備える、
ことを特徴とするロボット装置。
【請求項２】
前記低周波成分は、前記ロボット装置の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数である、
ことを特徴とする請求項１に記載のロボット装置。
【請求項３】
前記第１演算部により演算された前記アームの前記角速度と、
前記第３演算部により演算された前記ねじれ角速度と、を加算する第４演算部を更に備える、
ことを特徴とする請求項１または２に記載のロボット装置。
【請求項４】
アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、
複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
前記アーム体の一方の端部に設けられた前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、
前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、
前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算工程と、
前記第１演算工程の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算工程と、
前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算工程と、を含む、
ことを特徴とするロボット装置の制御方法。
【請求項５】
前記低周波成分は、前記ロボット装置の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数である、
ことを特徴とする請求項４に記載のロボット装置の制御方法。
【請求項６】
前記第１演算工程により演算された前記アームの前記角速度と、
前記第３演算工程により演算された前記ねじれ角速度と、を加算し前記アームの角速度を演算する第４演算工程を更に備える、
ことを特徴とする請求項４または５に記載のロボット装置の制御方法。
【請求項７】
アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、
複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
前記アーム体の一方の端部に設けられた前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、
前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、
を備えるロボット装置に制御を行わせるプログラムであって、
コンピューターに、
前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算手順と、
前記第１演算手順の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算手順と、
前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算手順と、
を実行させるためのプログラム。
【請求項１】
アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、
複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
前記アーム体の一方の端部に設けられた前記アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、
前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、
前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算部と、
前記第１演算部の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算部と、
前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算部と、を備える、
ことを特徴とするロボット装置。
【請求項２】
前記低周波成分は、前記ロボット装置の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数である、
ことを特徴とする請求項１に記載のロボット装置。
【請求項３】
前記第１演算部により演算された前記アームの前記角速度と、
前記第３演算部により演算された前記ねじれ角速度と、を加算する第４演算部を更に備える、
ことを特徴とする請求項１または２に記載のロボット装置。
【請求項４】
アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、
複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
前記アーム体の一方の端部に設けられた前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、
前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、
前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算工程と、
前記第１演算工程の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算工程と、
前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算工程と、を含む、
ことを特徴とするロボット装置の制御方法。
【請求項５】
前記低周波成分は、前記ロボット装置の機械系固有振動数のうち、反共振周波数および共振周波数における最も低い周波数に対して、より低い周波数である、
ことを特徴とする請求項４に記載のロボット装置の制御方法。
【請求項６】
前記第１演算工程により演算された前記アームの前記角速度と、
前記第３演算工程により演算された前記ねじれ角速度と、を加算し前記アームの角速度を演算する第４演算工程を更に備える、
ことを特徴とする請求項４または５に記載のロボット装置の制御方法。
【請求項７】
アクチュエーター、前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサー、を含むアーム連結装置と、
複数のアームが、前記アーム連結装置により直列且つ回転可能に連結されたアーム体と、
前記アーム体の一方の端部に設けられた前記アクチュエーターと前記アクチュエーターのトルクが所定の減速比で伝達されるトルク伝達機構及び前記アクチュエーターの回転角度を検出する角度センサーを含む基体連結装置により前記アーム体が回転可能に連結された基体と、
前記アームに取り付けられた、少なくとも角速度センサーを含む、慣性センサーと、
を備えるロボット装置に制御を行わせるプログラムであって、
コンピューターに、
前記角度センサーの回転角度検出データより、前記角度センサーを備える前記アクチュエーターによって動作する前記アームの角速度を演算する第１演算手順と、
前記第１演算手順の演算対象の前記アクチュエーターを含む前記連結装置を介して連結される前記アームに備える前記慣性センサーの角速度検出データより、前記連結装置を軸とする前記アクチュエーターにより作動する前記アームの角速度を演算する第２演算手順と、
前記アクチュエーターによって動作する前記アームの前記角速度及び前記慣性センサーの前記角速度検出データによって演算された前記アームの前記角速度の差に含まれる低周波成分を除去した、前記アクチュエーター及び前記アームの間のねじれ角速度を演算する第３演算手順と、
を実行させるためのプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１２−３５３６１（Ｐ２０１２−３５３６１Ａ）
【公開日】平成２４年２月２３日（２０１２．２．２３）
【国際特許分類】
【出願番号】特願２０１０−１７７０６６（Ｐ２０１０−１７７０６６）
【出願日】平成２２年８月６日（２０１０．８．６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２４年２月２３日（２０１２．２．２３）
【国際特許分類】
【出願日】平成２２年８月６日（２０１０．８．６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]