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