(57)【要約】【課題】出力トルクを精度良く制御できるトルクサーボのモータの制御装置を提供する。【解決手段】本発明の一態様に係るモータの制御装置８は、モータ４又は減速機５又はリンク２のモデル、及びモータ４の回転数に基づいて、トルク指令信号が示すトルク値を、ロボットの関節部に発生させることができるように、第１のモータ出力トルク値を算出する第１のフィードフォワード算出部９と、トルク指令信号が示すトルク値に基づき、モータ４の回転数に依存せずに、第２のモータ出力トルク値を算出する第２のフィードフォワード算出部１０と、第１のモータ出力トルク値と、第２のモータ出力トルク値と、センサ（トルクセンサ６）により検出されたトルク値及びトルク指令信号が示すトルク値に基づいて算出された第３のモータ出力トルク値と、を加算する比較器（第２の比較器１３）と、を備える。【選択図】図３
2018142350.tif 000002
【特許請求の範囲】
【請求項１】
モータと減速機とセンサとを含む関節部と、前記モータにより前記減速機を介して駆動され、環境からの反力を受けるリンクと、を有するロボットに適用され、前記モータをフィードバック制御とフィードフォワード制御との組み合わせによって制御するモータの制御装置であって、
少なくとも前記モータ又は前記減速機又は前記リンクのモデル、及び前記モータのトルク指令信号に基づいて、前記トルク指令信号が示すトルクを前記関節部に発生させるための前記モータの指令回転数を算出し、前記指令回転数を実現するように、第１のモータ出力トルク値を算出する第１のフィードフォワード算出部と、
前記トルク指令信号が示すトルク値に基づき、前記モータの指令回転数に依存せずに、第２のモータ出力トルク値を算出する第２のフィードフォワード算出部と、
前記第１のモータ出力トルク値と、前記第２のモータ出力トルク値と、前記センサにより検出されたトルク値及び前記トルク指令信号が示すトルク値に基づいて算出された第３のモータ出力トルク値と、を加算する比較器と、
を備える、モータの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、モータの制御装置に関し、特に、フィードバック制御とフィードフォワード制御との組み合わせによって、ロボットのリンクを駆動させるトルクサーボのモータを２自由度制御する制御装置に関する。
【背景技術】
【０００２】
特許文献１には、２自由度制御を用いてトルク指令値に対する目標値追従特性を高めた技術が開示されている。
ちなみに、特許文献２には、目標とするロボットの運動を実現するためのモータの出力トルクを、２自由度制御を用いて算出する技術が開示されている。但し、特許文献２の技術は、ロボット全体の制御に係る技術であり、ロボットの関節部毎のモータの制御に係る技術ではない。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０００−５０６６６号公報
【特許文献２】特開平１１−４２５７３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１の技術は、フィードフォワード算出部においてロボットが環境に接触する場合に反力を得るためにモータなどを回転させるためのトルクを考慮していないため、出力トルクを精度良く制御することができない。
【０００５】
本発明は、上記を鑑みなされたものであって、出力トルクを精度良く制御できるトルクサーボのモータの制御装置を実現する。
【課題を解決するための手段】
【０００６】
本発明の一態様に係るモータの制御装置は、モータと減速機とセンサとを含む関節部と、前記モータにより前記減速機を介して駆動され、環境からの反力を受けるリンクと、を有するロボットに適用され、前記モータをフィードバック制御とフィードフォワード制御との組み合わせによって制御するモータの制御装置であって、
少なくとも前記モータ又は前記減速機又は前記リンクのモデル、及び前記モータの回転数に基づいて、トルク指令信号が示すトルク値を、前記関節部に発生させることができるように、第１のモータ出力トルク値を算出する第１のフィードフォワード算出部と、
前記トルク指令信号が示すトルク値に基づき、前記モータの回転数に依存せずに、第２のモータ出力トルク値を算出する第２のフィードフォワード算出部と、
前記第１のモータ出力トルク値と、前記第２のモータ出力トルク値と、前記センサにより検出されたトルク値及び前記トルク指令信号が示すトルク値に基づいて算出された第３のモータ出力トルク値と、を加算する比較器と、
を備える。
【発明の効果】
【０００７】
本発明によれば、出力トルクを精度良く制御できるトルクサーボのモータの制御装置を実現できる。
【図面の簡単な説明】
【０００８】
【図１】一般的なロボットを示す模式図である。
【図２】ロボットに搭載されるトルクサーボの構成を示す分解図である。
【図３】実施の形態のモータの制御装置を示すブロック図である。
【図４】第１のモータ出力トルク値のトルク波形の一例を示す図である。
【図５】アンダーシュートが生じるトルク波形の一例を示す図である。
【図６】第１のモータ出力トルク値及び第２のモータ出力トルク値のトルク波形の一例を示す図である。
【図７】トルク指令信号が示すトルク値への追従性を示す図である。
【発明を実施するための形態】
【０００９】
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。但し、本発明が以下の実施の形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【００１０】
図１は、一般的なロボットアームを示す模式図である。図２は、ロボットアームに搭載されるトルクサーボの構成を示す分解図である。図３は、本実施の形態のモータの制御装置を示すブロック図である。図４は、第１のモータ出力トルク値のトルク波形の一例を示す図である。図５は、アンダーシュートが生じるトルク波形の一例を示す図である。
【００１１】
本実施の形態のモータの制御装置（以下、単に制御装置という場合がある。）は、フィードバック制御とフィードフォワード制御との組み合わせによって、ロボットのリンクを駆動するトルクサーボのモータを２自由度制御する。このような制御装置は、例えば、図１に示すようなロボットアーム（ロボット）１のリンク２を駆動するためのトルクサーボのモータを制御する際に好適である。
【００１２】
図２に示すように、トルクサーボ３は、例えば、モータ４、減速機５及びトルクセンサ６などが筐体７に格納された構成とされている。なお、図２では符号を付していないが、モータ４と減速機５との間などに配置している部材は連結冶具である。
【００１３】
図３に示すように、本実施の形態の制御装置８は、第１のフィードフォワード算出部９、第２のフィードフォワード算出部１０、第１の比較器１１、フィードバック算出部１２及び第２の比較器１３を備えている。
【００１４】
第１のフィードフォワード算出部９は、第２のフィードフォワード算出部１０と並列に配置されている。本実施の形態の第１のフィードフォワード算出部９は、ロボットアーム１が接触環境と接触中に出力トルク指令値が変化した場合に、第２のフィードフォワード算出部１０のみの場合と比較して、出力トルクを高い追従性で精度良く制御する役割を主として担う。但し、ロボットアーム１が接触環境と接触中でない場合でも、第１のフィードフォワード算出部９は、出力トルクの追従性を向上させ精度良く制御する効果がある。
【００１５】
このような第１のフィードフォワード算出部９は、モータ４を含むトルクサーボ３に連結されるリンク２が接触環境と非接触状態で、外部から入力されるトルク指令信号が示すトルク値τplanをリンク２に発生させることができるように、リンク２、モータ４及び減速機５などのプラントを回転（駆動）させるための第１のモータ出力トルク値を算出する。
【００１６】
詳細には、モータ４、減速機５及びリンク２などの制御対象について、入力トルクから関節角度までのプラントモデルＰｍを以下の＜数１＞のように定義する。これはモータ４のイナーシャや粘性摩擦を有する一般的なモデルと同等の形式であるが、モータ４、減速機５及びリンク２などの制御対象全体のイナーシャＪｎ及び粘性摩擦Ｂｎを有するモデルである。なお、以下の＜数１＞などにおいて、「ｓ」はラプラス演算子であり、ｓとの積は微分を、１／ｓとの積は積分を表す。
【００１７】
【数１】
2018142350.tif 000003
【００１８】
また、接触環境において発生する反力のモデルＰＣを以下の＜数２＞のように定義する。これは、バネ乗数Ｋｅｎ及び粘性係数Ｄｅｎで定義されたバネダンパモデルであり、接触環境の剛性だけでなく、リンク２や減速機５などの剛性も含められる。なお、バネ乗数Ｋｅｎは適当な値に設定することができ、誤差はフィードバック制御によって吸収する。
【００１９】
【数２】
2018142350.tif 000004
【００２０】
また、目標値応答Ｇｒｙを以下の＜数３＞のように２次遅れ系で定義する。
【数３】
2018142350.tif 000005
【００２１】
但し、目標値応答Ｇｒｙを時定数Ｔｒの１次のローパスフィルタの積の形で定義しているが、分母の次数が分子の次数より２以上大きなローパスフィルタであればよい。
【００２２】
このようなプラントのモデルＰｍ、反力のモデルＰｃの逆システム及び目標値応答Ｇｒｙを用いて以下の＜数４＞のように決定した第１のフィードフォワードゲインＦＦ１が第１のフィードフォワード算出部９に設定されている。
【００２３】
【数４】
2018142350.tif 000006
【００２４】
そして、第１のフィードフォワード算出部９は、第１のフィードフォワードゲインＦＦ１にトルク指令信号が示すトルク値τplanを積算して第１のモータ出力トルク値を算出し、算出した第１のモータ出力トルク値を示す信号を第２の比較器１３に出力する。つまり、第１のモータ出力トルク値は、目標値応答Ｇｒｙと、モータ４などのノミナルモデルの逆システムと、接触環境のノミナルモデルの逆システムと、の積によって算出する。
【００２５】
このような第１のフィードフォワードゲインＦＦ１を求める手順は、トルク指令信号が示すトルク値を発生させるために必要なモータ４の回転角度（即ち、回転数）を、環境の剛性やリンク２の剛性や減速機５の剛性を考慮した押し込み量を実現するための角度によって算出し、さらに、モータ４の回転角度分の回転を実現するために必要なモータ出力トルク値を算出することになる。ここで、モータ４の回転角度が０の場合は、モータ出力トルク値も０になる。
【００２６】
＜数４＞で目標値応答Ｇｒｙを積算する理由は、以下のように２つある。１つ目の理由は、ロボットアーム１の振動を防止するためである。＜数４＞で目標値応答Ｇｒｙを積算しなかった場合、トルク指令信号が示すトルク値が急に変化した場合にモータ４に瞬間的に大きなトルク値を発生させることになり、振動を励起する場合があるからである。
【００２７】
２つ目の理由は、第１のフィードフォワードゲインＦＦ１全体の伝達関数の分母の次数（sの次数）を分子の次数以上にして、ソフトウエアを製作するときに必要な離散化をできるようにするためである。これは、モータ４のアンプの制御ソフトウエアを作成する際に、第１のフィードフォワードゲインＦＦ１全体を離散化してコーディングする必要があり、上述のような次数になっていないと離散化できないためである。
【００２８】
第１のモータ出力トルク値は、バネ乗数Ｋｅｎや粘性係数Ｄｅｎのパラメータによって種々のトルク波形をとることが可能であるが、リンク２が接触環境を押し込む際にモータ４が発生するトルクの方向と逆方向のトルクを当該モータ４が発生しないトルク波形（本実施の形態では、図４に示すように立下り時のアンダーシュートが生じない（即ち、モータ４のトルク値が負の値にならない）トルク波形）とすることが好ましい。これにより、接触環境の剛性が変化した場合に発生力（出力）が不足する効果を軽減できる。ちなみに、図５は、アンダーシュートが生じるトルク波形を例示した図である。
【００２９】
第１のフィードフォワード算出部９で第１のモータ出力トルク値を算出したが、接触力により発生するトルクの反作用トルクに相当するトルク指令が含まれていない。
【００３０】
そこで、第２のフィードフォワード算出部１０は、外部から入力されるトルク指令信号が示すトルク値τplanでリンク２が接触環境と接触したときの反力を当該接触環境から得るための第２のモータ出力トルク値を算出する。
【００３１】
そのため、第２のフィードフォワード算出部１０には、以下の＜数５＞で表される第２のフィードフォワードゲインＦＦ２が設定されている。そして、第２のフィードフォワード算出部１０は、第２のフィードフォワードゲインＦＦ２にトルク指令信号が示すトルク値τplanを積算して第２のモータ出力トルク値を算出し、算出した第２のモータ出力トルク値を示す信号を第２の比較器１３に出力する。
【００３２】
【数５】
2018142350.tif 000007
【００３３】
本実施の形態では、第２のフィードフォワードゲインＦＦ２として目標値応答Ｇｒｙを設定している。このような第２のフィードフォワード算出部１０は、ロボットアーム１が接触環境と接触した状態で出力トルク指令と同等なトルクを発生する役割を担う。
【００３４】
ここで、図６は、第１のモータ出力トルク値及び第２のモータ出力トルク値のトルク波形の一例を示す図である。第１のフィードフォワード算出部９及び第２のフィードフォワード算出部１０に入力されるトルク指令信号が示すトルク値を倍にステップ状に変化させた場合、第１のモータ出力トルク値及び第２のモータ出力トルク値は、例えば、図６に示すようなトルク波形となる。
【００３５】
このように算出した第１のモータ出力トルク値と第２のモータ出力トルク値とを合計したトルク値をモータ４に発生させる。これにより、トルク指令信号が示すトルク値をリンク２に発生させるためにモータ４を回転させることができ、且つ当該トルク値に等しい反力が得られることになる。
【００３６】
しかし、プラントのモデルＰｍや反力のモデルＰｃが現実のモータ４や接触環境の状態を完全にモデル化できるわけではなく、モデル化誤差が存在する。そのため、第１のフィードフォワード算出部９及び第２のフィードフォワード算出部１０を用いたフィードフォワード制御のみでは誤差が生じる。
【００３７】
そこで、制御装置８は、フィードフォワード制御に加えてフィードバック制御を併用している。このとき、制御装置８は、フィードバック制御による目標値追従特性とフィードフォワード制御による外乱抑圧特性とを独立して設計できるようにトルク指令調整部１４を備えていることが好ましい。
【００３８】
トルク指令調整部１４には、外部から入力されるトルク指令信号が示すトルク値τplanを調整するための係数として目標値応答Ｇｒｙが設定されている。そして、トルク指令調整部１４は、トルク指令信号が示すトルク値τplanに目標値応答Ｇｒｙを積算し、トルク値τplanに目標値応答Ｇｒｙを積算した値を示す信号を第１の比較器１１に出力する。但し、トルク値τplanを調整するための係数は、第１のフィードフォワード算出部９のローパスフィルタに応じて、適宜設定することができ、トルク値τplanを調整するための係数が１の場合、トルク指令調整部１４を省略することができる。
【００３９】
第１の比較器１１は、トルク指令調整部１４から入力される信号が示すトルク値τplanに目標値応答Ｇｒｙを積算した値と、トルクセンサ６が検出したリンク２に発生させたトルク値τｍｓｒと、の偏差を算出し、算出した偏差を示す信号をフィードバック算出部１２に出力する。
【００４０】
フィードバック算出部１２には、フィードバックゲインＫｐが設定されている。そして、フィードバック算出部１２は、第１の比較器１１から入力される信号が示す偏差にフィードバックゲインＫｐを積算した第３のモータ出力トルク値を示す信号を第２の比較器１３に出力する。なお、フォードバックゲインＫｐは、適宜設定することができる。
【００４１】
第２の比較器１３は、第１のフィードフォワード算出部９から入力される信号が示す第１のモータ出力トルク値と、第２のフィードフォワード算出部１０から入力される信号が示す第２のモータ出力トルク値と、フィードバック算出部１２から入力される信号が示す第３のモータ出力トルク値と、を加算したトルク目標値τｒｅｆを示す信号をモータ４に出力する。モータ４は、第２の比較器１３から入力されるトルク目標値τｒｅｆを示す信号に基づいて回転角度θで回転する。
【００４２】
本実施の形態では、第１のフィードフォワード算出部９がプラントのモデルＰｍだけでなく、反力のモデルＰｃを用いて第１のモータ出力トルク値を算出しているので、ロボットアーム１が環境に接触した状態でモータ４を精度良く制御できる。
【００４３】
また、プラントのモデルＰｍ及び反力のモデルＰｃのシステムと逆システムとが打ち消し合うため、結果的に目標値応答Ｇｒｙの通りの応答になることが期待できる。
【００４４】
また、第１のフィードフォワード算出部９と第２のフィードフォワード算出部１０とを個別に設計することができ、制御装置８の設計やパラメータ決定が容易である。
【００４５】
また、目標値応答Ｇｒｙの分母の次数が２次であり、且つ（１＋Ｔｒｓ）の２乗又はｓの多項式で目標値応答Ｇｒｙを表すことができる場合、パラメータの数が膨大にならず、制御系設計に必要な時間を抑制することができる。
【００４６】
ここで、図７は、トルク指令信号が示すトルク値への追従性を示す図である。図７に示すように、本実施の形態の制御装置８は、当該制御装置８において第１のフィードフォワード算出部９を省略した場合に比べて、トルク指令信号が示すトルク値への追従性が優れていることが解る。
【００４７】
本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【００４８】
例えば、上記実施の形態では、環境との接触状態に限定して説明しているが、環境との接触状態に限定するものではない。環境と接触していない状況においても、接触力をリンク２の加速による慣性力などに置き換えることによって同等の原理が成り立つ。
【００４９】
例えば、上記実施の形態では、リンク２の回転に限定して説明しているが、リンク2の回転に限定するものではない。直動についても、トルクを力に、また、角度を位置に置き換えることにより同等の原理が成り立つ。
【００５０】
例えば、接触環境のモデルは＜数２＞に限定されるものではない。例えば、Ｐｃ＝Ｋｅｎでもよい。この場合、目標値応答Ｇｒｙは＜数３＞でもよいが、Ｇｒｙ＝１／（１＋Ｔｒｓ）のように分母の次数が分子の次数より１以上大きなローパスフィルタであればよい。
【符号の説明】
【００５１】
１  ロボットアーム
２  リンク
３  トルクサーボ
４  モータ
５  減速機
６  トルクセンサ
７  筐体
８  制御装置
９  第１のフィードフォワード算出部
１０  第２のフィードフォワード算出部
１１  第１の比較器
１２  フィードバック算出部
１３  第２の比較器
１４  トルク指令調整部
【図１】
2018142350.tif 000008
【図２】
2018142350.tif 000009
【図３】
2018142350.tif 000010
【図４】
2018142350.tif 000011
【図５】
2018142350.tif 000012
【図６】
2018142350.tif 000013
【図７】
2018142350.tif 000014