(57)【要約】【課題】慣性センサー信号の遅延に起因する振動発生を抑制する。【解決手段】ロボット制御装置は、基体に対して回転可能なリンクとしてのアーム４０、アーム４０を駆動するアクチュエーターとしてのモーター２０と、モーター２０のトルクをアーム４０に減速非Ｎで伝達する減速機構６０と、前記リンクによって回転されるアームと、前記アクチュエーターの回転角度θｍを検出する角度センサーと、前記アームの回転角度θａを検出する慣性センサーと、を備えるロボット制御装置の制御方法であって、  前記慣性センサー信号の遅延量を前記ロボットの機構モデル系の運動方程式に加えてフィードバックゲインを設定し、前記慣性センサー信号の遅延に起因する前記リンクの振動を抑制することを特徴とするロボットの制御装置。【選択図】図１
2010284758.tif 000002
【特許請求の範囲】
【請求項１】
基体に対して回転可能なリンクと、
前記リンクを駆動するアクチュエーターと、
前記アクチュエーターのトルクを前記リンクに減速非Ｎで伝達する減速機構と、
前記アクチュエーターの回転角度θｍを検出する角度センサーと、
前記リンクの回転角度θａを検出する慣性センサーと、を備え、
慣性センサー信号の遅延量βを機構モデル系の運動方程式に加えてフィードバックゲインを設定し、前記慣性センサー信号の遅延に起因する前記リンクの振動を抑制することを特徴とするロボット制御装置。
【請求項２】
基体に対して回転可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速非Ｎで伝達する減速機構と、前記アクチュエーターの回転角度θｍを検出する角度センサーと、前記リンクの回転角度θａを検出する慣性センサーと、を備えるロボット制御装置の制御方法であって、
慣性センサー信号の遅延量βを前記ロボットの機構モデル系の運動方程式に加えてフィードバックゲインを設定し、前記慣性センサー信号の遅延に起因する前記リンクの振動を抑制することを特徴とするロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの制御装置、及びロボットの制御方法に関する。
【背景技術】
【０００２】
従来の多リンク構造を有するロボットは、基体に対して回転運動可能な第１のリンクと、第１のリンクを駆動する第１のアクチュエーターと、第１のアクチュエーターのトルクを第１のリンクに減速比Ｎ１で伝達する第１のトルク伝達機構と、第１のアクチュエーターの回転角θＭ１を検出する第１の角度センサーと、基体に対する第１のリンクの角速度ωＬ１を検出する第１の角速度センサーとを備え、演算部によってωＬ１の積分値の高周波成分及びθＭ１＊Ｎ１の低周波成分を用いて第１のリンクの角度θＭ１を算出する。
【０００３】
さらに、第１のリンクに対して回転運動可能な第２のリンクと、第２のリンクを駆動する第２のアクチュエーターと、第２のアクチュエーターのトルクを第２のリンクに減速比Ｎ２で伝達する第２のトルク伝達機構と、第２のアクチュエーターの回転角θＭ２を検出する第２の角度センサーと、基体に対する第２のリンクの角速度ωＬ２を検出する第２の角速度センサーとを備え、前述の演算部が、ωＬ２−ωＬ１の積分値のうち第１の周波数以上の高周波成分、及びθＭ２＊Ｎ２のうち第１の周波数以下の低周波成分を用いて第１のリンクに対する第２のリンクの角度をさらに算出するロボットの制御装置が提案されている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特許第３８８３５４４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
このような特許文献１による２リンク３慣性系のロボットは、角度センサー、加速度センサー及び角速度センサーを取付けて各センサーの検出値を用いてリンクの動作を正確に制御しようとしているが、各センサー信号の遅延が存在するため、この信号遅延に起因する振動が発生する場合がある。
【０００６】
信号遅延を考慮しない動作モデルの場合、制御器の極配置により振動を制御することが可能であるが、その場合には、目標値追従性が悪くなったり、ゲインに対して制御結果が敏感になり、制御のロバスト性が失われるという課題を有している。
【課題を解決するための手段】
【０００７】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００８】
［適用例１］本適用例に係るロボット制御装置は、基体に対して回転可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速非Ｎで伝達する減速機構と、前記アクチュエーターの回転角度θｍを検出する角度センサーと、前記リンクの回転角度θａを検出する慣性センサーと、を備え、慣性センサー信号の遅延量βを機構モデル系の運動方程式に加えてフィードバックゲインを設定し、前記慣性センサー信号の遅延に起因する前記リンクの振動を抑制することを特徴とする。
【０００９】
［適用例２］本適用例に係るロボット制御装置の制御方法は、基体に対して回転可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターのトルクを前記リンクに減速非Ｎで伝達する減速機構と、前記アクチュエーターの回転角度θｍを検出する角度センサーと、前記リンクの回転角度θａを検出する慣性センサーと、を備えるロボット制御装置の制御方法であって、慣性センサー信号の遅延量βを前記ロボットの機構モデル系の運動方程式に加えてフィードバックゲインを設定し、前記慣性センサー信号の遅延に起因する前記リンクの振動を抑制することを特徴とする。
【００１０】
このような構成、及び制御方法によれば、慣性センサー信号の遅延をモデル化した（数１１）の状態関数によりフィードバックゲインの設計を行えば、予め、慣性センサー信号の遅延を考慮してあるためＮ振動を発生することなく残留振動を抑制し、さらに残留振動の制定時間を短縮することができる。その結果、制御のロバスト性が失われることを抑制することができる。
【図面の簡単な説明】
【００１１】
【図１】実施形態に係るロボットの機構モデルを模式的に表す説明図。
【図２】ディレイ０の場合のインパルス応答を示すグラフ。
【図３】ロボットの基本駆動を示す説明図。
【図４】アームを駆動した場合の残留振動の１例を表すグラフ。
【図５】Ｎ振動が発生した状態を表すグラフ。
【図６】慣性センサー信号の遅延量（ディレイ）βが０．０５ｓｅｃの場合のインパルス応答を示すグラフ。
【発明を実施するための形態】
【００１２】
以下、本発明の実施形態を図面に基づいて説明する。
（実施形態１）
【００１３】
図１は、本実施形態に係るロボットの機構モデルを模式的に表す説明図である。図１は、１リンク２慣性系のロボット１０の機構モデルを表し、モーター２０と、基体（図示せず）に対して回転運動可能なリンクとしてのアーム４０とから構成されている。
【００１４】
なお、ロボット１０には、ロボットの駆動及び制御に関する計算処理を行う演算部（図示せず）を有している。
【００１５】
モーター２０とアーム４０との間には、モーター２０のトルクをアーム４０に伝達するための減速機構６０が備えられている。減速機構６０は、バネ成分６１とダンパー成分６２とを有していると考えられる。
【００１６】
モーター２０は、モーター２０の回転角度を測定する角度センサーとしてのエンコーダー５０を備え、アーム４０の先端部には慣性センサーとしての加速度センサー７０が取付けられている。
【００１７】
モーター２０は、減速機構６０を介して軸８０を回転軸としてアーム４０を回転させる。この際、モーター２０を停止しアーム４０の回転を停止した場合、減速機構６０のバネ成分６１とダンパー成分６２の影響でアーム４０が即停止とならず、振動（以降、残留振動と表す）が発生する場合がある。アーム４０に残留振動が発生した場合、アーム４０の動作を正確に制御することは困難である。残留振動は、減速機構６０のバネ成分６１とダンパー成分６２の存在に起因する。
【００１８】
この残留振動は、アーム４０に加速度センサー７０を取り付け、アーム先端の動きを検出し、フィードバック制御することで、残留振動時間を短くすることができる（特許文献１）。
【００１９】
しかしながら、加速度センサー７０の出力信号には、センサー素子の制御回路自体の検出遅れ、及び外付けのアナログフィルターによる遅れ、アナログ信号をデジタル信号に変換する際の前置フィルターによる遅れ等の遅延が存在する。本実施形態では、センサー素子の制御回路自体の検出遅れ、及び外付けのアナログフィルターによる遅れを総称して慣性センサー信号の遅延と表し説明する。
【００２０】
慣性センサー信号に遅延がない場合の機構モデルの運動方程式は次の数式のように表すことができる。
【００２１】
【数１】
2010284758.tif 000003
なお、θｍはエンコーダー５０で検出し、θａは慣性センサー（角速度センサー）７０で検出する。また、Ｄｇは減速機構のダンパー成分の減衰係数、Ｋｇはバネ成分の弾性常数である。
【００２２】
（数２）はモーター２０側の運動方程式、（数３）はアーム４０側の運動方程式を表す。
【００２３】
【数２】
2010284758.tif 000004
【００２４】
【数３】
2010284758.tif 000005
【００２５】
（数２）及び（数３）をまとめると以下に示す二次の運動方程式が得られる。
【００２６】
【数４】
2010284758.tif 000006
（数４）は、さらに次式で表すことができる。
【００２７】
【数５】
2010284758.tif 000007
なお、（数５）におけるＪＬ、ＤＬ、ＫＬ、ＢＬは次のように表すことができる。
【００２８】
【数６】
2010284758.tif 000008
また、（数５）から、系の状態方程式を作成すると以下の式で表すことができる。
【００２９】
【数７】
2010284758.tif 000009
そして、（数６）におけるｘ、ｕ、ｙは（数８）、また、Ａ、Ｂ、Ｃ、Ｄは（数９）のように表される。
【００３０】
【数８】
2010284758.tif 000010
【００３１】
【数９】
2010284758.tif 000011
【００３２】
制御理論によれば、状態方程式（数７）で表される本実施形態の機構モデルの安定性は、行列Ａの極配置で判断することができる。つまり、慣性センサー信号の遅延がない（ディレイ０）場合の極配置は｛−０．６７７０＋２４．８８４２ｉ，−０．６７７０−２４．８８４２ｉ，０，−０，２０４８｝である。
【００３３】
図２は、ディレイ０（慣性センサー信号の遅延がない）の場合のインパルス応答を示すグラフである。図２に示すように、ディレイ０の場合のインパルス応答性は暫近安定となる。従って、このままで、サーボ拡大系を形成し制御することにより安定した制御を行うことができる。
【００３４】
続いて、ロボットの駆動状態と残留振動について説明する。
図３は、本実施形態のロボットの基本駆動を示す説明図である。図３では、アーム４０の角速度を制御して角度０度から１８０度まで回転して停止する１サイクルを表している。つまり、アーム４０を時間ｔ１から時間ｔ２まで加速駆動し、時間ｔ２から時間ｔ３まで等速駆動し、時間ｔ３から時間ｔ４までの間に減速駆動し、時間t４で静止させる。
【００３５】
このようにアーム４０を加速駆動〜等速駆動〜減速駆動〜静止させた場合、減速機構６０のバネ成分６１とダンパー成分６２（合わせて弾性成分と表すことがある）に起因する残留振動が発生する。
図４は、図３のようにアームを駆動した場合の残留振動の１例を表すグラフである。図４では、アーム４０を静止した以降に残留振動が存在していることを示している。
【００３６】
この弾性成分に起因する残留振動を抑制して制定時間を短縮することがアーム４０の正確な制御にとって重要であるが、制定時間を短縮するように制御のフィードバックゲインを決めた場合、慣性センサー信号の遅延を起因とする新しい振動（以降、Ｎ振動と表す）が発生する。
【００３７】
図５は、Ｎ振動が発生した状態を表すグラフである。Ｎ振動の特徴は、図５に示すように、アーム４０が角度０度、角度１８０度で静止しているときでも振動が発生していることである。このＮ振動を発生させないように制御のフィードバックゲインを小さくすると、Ｎ振動は発生しないが、アーム４０の振動を抑制する制定時間が長くなってしまう。
【００３８】
Ｎ振動が発生する原因として慣性センサー信号に遅延があった場合の機構モデルの系がどのようになるかを考察する。慣性センサー信号の遅延量をβとすると次式で表すことができる。
【００３９】
【数１０】
2010284758.tif 000012
（数１０）のように近似すれば、（数４）に相当する系の運動方程式は、次式のように表すことができる。
【００４０】
【数１１】
2010284758.tif 000013
そして、このときの状態方程式は、次のように表すことができる。
【００４１】
【数１２】
2010284758.tif 000014
なお、（数１２）におけるＡＢ、ＢＢ、ＣＢ、ＤＢは（数１３）のように表される。
【００４２】
【数１３】
2010284758.tif 000015
また、（数１３）におけるＪＬＢ、ＤＬＢ、ＫＬＢ、ＢＬＢは（数１４）のように表される。
【００４３】
【数１４】
2010284758.tif 000016
【００４４】
制御理論によれば、状態方程式（数１２）で表される本実施形態の機構モデルの安定性は、行列Ａ，Ｂの極配置で判断することができる。例えば、βが−０．０５（ｓｅｃ）の場合の極は、｛１．８３９６＋２４．７２２８ｉ，１．８３９６−２４．７２２８ｉ，０，−０．２０７｝となり、その場合のインパルス応答を図６に表す。
【００４５】
図６は、慣性センサー信号の遅延量（ディレイ）βが０．０５ｓｅｃの場合のインパルス応答を示すグラフである。つまり、系の制御を（数７）で表した状態方程式で設計した場合、実際の系は（数１２）で表す状態方程式となってしまい、不安定となる。その結果、残留振動の制定時間を短くしようとフィードバックゲインを決めた場合にはＮ振動が発生する（発振現象）。
【００４６】
従って、慣性センサー信号の遅延をモデル化した（数１２）の状態関数によりフィードバックゲインの設計を行えば、予め、慣性センサー信号の遅延βを考慮してあるためＮ振動を発生することなく残留振動を抑制し、さらに残留振動の制定時間を短縮することができる。その結果、制御のロバスト性が失われることを抑制することができる。
【００４７】
なお、本実施形態では、１リンク２慣性系のロボット制御装置及び制御方法を例示して説明したが、この技術思想は、２リンク３慣性系のロボット制御装置にも適合可能である。
【符号の説明】
【００４８】
１０…ロボット、２０…モーター、４０…リンクとしてのアーム、５０…角度センサーとしてのエンコーダー、６０…減速機構、６１…バネ成分、６２…ダンパー成分、７０…慣性センサーとしての角速度センサー。
【図１】
2010284758.tif 000017
【図２】
2010284758.tif 000018
【図３】
2010284758.tif 000019
【図４】
2010284758.tif 000020
【図５】
2010284758.tif 000021
【図６】
2010284758.tif 000022