(57)【要約】【課題】  柔軟機構による振動を抑制する制御装置を提供することを課題とする。【解決手段】  柔軟機構Ｓを介して負荷Ｌを動作させるアクチュエータＭを制御する制御装置１であって、アクチュエータ変位量検出手段１０と、負荷変位量検出手段１１と、目標変位量に基づいて重力補償値を求める重力補償手段１６と、重力補償値に第１ゲインα（＞２）を乗算する第１ゲイン手段１７と、検出したアクチュエータの変位量と検出した負荷の変位量との相対変位量を算出する相対変位量算出手段１３と、相対変位量の位相を遅らせる位相遅れ手段１４と、位相を遅らせた相対変位量に第２ゲイン１−αを乗算する第２ゲイン手段１５と、目標変位量に第１ゲイン手段１７による乗算値及び第２ゲイン手段１５による乗算値を加算する加算手段１８とを備え、加算手段１８による加算値に基づいてアクチュエータＭを制御することを特徴とする。【選択図】  図１
2006244300.tif 000002
【特許請求の範囲】
【請求項１】
柔軟機構を介して負荷を動作させるアクチュエータを制御する制御装置であって、
アクチュエータの変位量を検出するアクチュエータ変位量検出手段と、
負荷の変位量を検出する負荷変位量検出手段と、
前記アクチュエータ変位量検出手段で検出したアクチュエータの変位量と前記負荷変位量検出手段で検出した負荷の変位量との相対変位量を算出する相対変位量算出手段と、
前記相対変位量算出手段で算出した相対変位量の位相を遅らせる位相遅れ手段と、
目標変位量に前記位相遅れ手段で位相を遅らせた相対変位量を加算する加算手段と
を備え、
前記加算手段による加算値に基づいてアクチュエータを制御することを特徴とする制御装置。
【請求項２】
柔軟機構を介して負荷を動作させるアクチュエータを制御する制御装置であって、
アクチュエータの変位量を検出するアクチュエータ変位量検出手段と、
負荷の変位量を検出する負荷変位量検出手段と、
目標変位量に基づいて重力補償値を求める重力補償手段と、
前記重力補償手段による重力補償値に第１ゲインα（＞２）を乗算する第１ゲイン手段と、
前記アクチュエータ変位量検出手段で検出したアクチュエータの変位量と前記負荷変位量検出手段で検出した負荷の変位量との相対変位量を算出する相対変位量算出手段と、
前記相対変位量算出手段で算出した相対変位量に第２ゲイン１−αを乗算する第２ゲイン手段と、
目標変位量に前記１ゲイン手段による乗算値及び前記第２ゲイン手段による乗算値を加算する加算手段と
を備え、
前記加算手段による加算値に基づいてアクチュエータを制御することを特徴とする制御装置。
【請求項３】
柔軟機構を介して負荷を動作させるアクチュエータを制御する制御装置であって、
アクチュエータの変位量を検出するアクチュエータ変位量検出手段と、
負荷の変位量を検出する負荷変位量検出手段と、
目標変位量に基づいて重力補償値を求める重力補償手段と、
前記重力補償手段による重力補償値に第１ゲインα（＞２）を乗算する第１ゲイン手段と、
前記アクチュエータ変位量検出手段で検出したアクチュエータの変位量と前記負荷変位量検出手段で検出した負荷の変位量との相対変位量を算出する相対変位量算出手段と、
前記相対変位量算出手段で算出した相対変位量の位相を遅らせる位相遅れ手段と、
前記位相遅れ手段で位相を遅らせた相対変位量に第２ゲイン１−αを乗算する第２ゲイン手段と、
目標変位量に前記１ゲイン手段による乗算値及び前記第２ゲイン手段による乗算値を加算する加算手段と
を備え、
前記加算手段による加算値に基づいてアクチュエータを制御することを特徴とする制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、柔軟機構を介して負荷を動作させるアクチュエータを制御する制御装置に関する。
【背景技術】
【０００２】
各種産業用ロボットや人型ロボットなどのロボットの開発が盛んに行われており、対人用のロボットの開発も進められている。対人用のロボットなどでは、ロボットが対象物と接触するときには柔軟性が必要となる。そこで、ロボットには、リンク（例えば、ハンド部、アーム部）とそのリンクを回転駆動するモータとの間にバネなどからなる柔軟機構を有するものがある。柔軟機構を設けた場合、その柔軟性に起因する振動が発生する。この振動を抑制しないと、ロボットのリンクの動きが不安定となる。
【０００３】
そこで、柔軟機構を有するロボットでは、振動を抑制するための各種制御が行われている。例えば、特許文献１に記載の制御方法では、アーム部とリンク部との相対変位を補償するためのフィードバックループにおいて、その相対変位を柔軟機構の共振周波数成分をカットするためのローパスフィルタに通してフィードバックするとともに、その相対変位をローパスフィルタを通さずに逆符号でフィードバックする。さらに、この制御方法では、２つのフィードバックループに比例補償器をそれぞれ設け、各フィードバックループにおいて１＋αとα（但し、αは、０より大きくかつ１以下である。）のゲインをそれぞれ付加している。また、特許文献２に記載の制御方法では、通常時にはアーム角指令値（目標値）に基づいて制御し、所定時点（例えば、目標値に達した時点）以降にはアーム角指令値と検出したアーム角との内分値に基づいて制御する。
【特許文献１】特開平５−８１９１号公報
【特許文献２】特開平１１−２１２６４８号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
特許文献１に記載の制御方法の場合、２つのフィードバックループのうちの１つのループにおいて相対変位をローパスフィルタに通さずにフィードバックしているので、柔軟機構の共振周波数が直接返される。そのため、ハンド部が理想的に共振周波数にそって動いた場合、重力の影響下では自由振り子の原理により、柔軟機構による振動が減衰するまで長い時間を要する。また、特許文献２に記載の制御方法の場合も、ローパスフィルタを用いていないので、柔軟機構による振動が減衰するまで長い時間を要する。さらに、この制御方法の場合、所定時点以降にだけフィードバックループによる制御を行うので、所定時点以前では外力が作用した場合などには柔軟機構の変位に関係なくアクチュエータが停止したままとなり、柔軟性が発揮されない。
【０００５】
このように、従来の制御方法では、柔軟機構における相対変位を小さくする方向にアクチュエータを駆動することによって柔軟機構における反力（バネ反力）を減少させる方法が採られていた。しかし、この従来の制御方法では、例えば、重力の影響がある場合、リンクとアクチュエータとの動作が完全に同期するため、リンクは振り子の自由振動のように動作し、その振動の整定時間が長くなる。そのため、リンクの動きが、長い間、不安定な状態となる。
【０００６】
そこで、本発明は、柔軟機構による振動を抑制する制御装置を提供することを課題とする。
【課題を解決するための手段】
【０００７】
本発明に係る制御装置は、柔軟機構を介して負荷を動作させるアクチュエータを制御する制御装置であって、アクチュエータの変位量を検出するアクチュエータ変位量検出手段と、負荷の変位量を検出する負荷変位量検出手段と、アクチュエータ変位量検出手段で検出したアクチュエータの変位量と負荷変位量検出手段で検出した負荷の変位量との相対変位量を算出する相対変位量算出手段と、相対変位量算出手段で算出した相対変位量の位相を遅らせる位相遅れ手段と、目標変位量に位相遅れ手段で位相を遅らせた相対変位量を加算する加算手段とを備え、加算手段による加算値に基づいてアクチュエータを制御することを特徴とする。
【０００８】
この制御装置では、アクチュエータ変位量検出手段によりアクチュエータの変位量を検出するとともに、負荷変位量検出手段により負荷の変位量を検出する。そして、制御装置では、相対変位量算出手段により検出したアクチュエータ変位量と検出した負荷変位量との相対変位量（柔軟機構における変位量）を算出し、位相遅れ手段により相対変位量の位相を遅らせる。さらに、制御装置では、加算手段により目標変位量に位相を遅らせた相対変位量を加算し、その加算値に基づいてアクチュエータを制御する。このように、制御装置では、フィードバックした負荷とアクチュエータとの相対変位量を位相遅らせて目標変位量に加算し、アクチュエータの位相を負荷に対して遅らせるとともにアクチュエータを制御するための変位量を目標変位量より大きくする。そのため、その位相遅れた分が柔軟機構におけるたわみ（変位）となって現れ、そのたわみによる発生力が負荷と同じ方向に働き、それが振動の減衰力として作用する。この際、負荷に重力が作用している場合、その減衰力が重力によって負荷に作用する力の方向とは逆方向に作用するので、重力による影響を抑制する。その結果、柔軟機構による振動を抑制でき、振動の整定時間を短くできるとともに、柔軟機能の柔軟性も発揮できる。したがって、負荷の動きも、早期に、安定した状態となる。
【０００９】
本発明に係る制御装置は、柔軟機構を介して負荷を動作させるアクチュエータを制御する制御装置であって、アクチュエータの変位量を検出するアクチュエータ変位量検出手段と、負荷の変位量を検出する負荷変位量検出手段と、目標変位量に基づいて重力補償値を求める重力補償手段と、重力補償手段による重力補償値に第１ゲインα（＞２）を乗算する第１ゲイン手段と、アクチュエータ変位量検出手段で検出したアクチュエータの変位量と負荷変位量検出手段で検出した負荷の変位量との相対変位量を算出する相対変位量算出手段と、相対変位量算出手段で算出した相対変位量に第２ゲイン１−αを乗算する第２ゲイン手段と、目標変位量に１ゲイン手段による乗算値及び第２ゲイン手段による乗算値を加算する加算手段とを備え、加算手段による加算値に基づいてアクチュエータを制御することを特徴とする。
【００１０】
この制御装置では、アクチュエータ変位量検出手段によりアクチュエータの変位量を検出するとともに、負荷変位量検出手段により負荷の変位量を検出する。そして、制御装置では、重力補償手段により目標変位量に基づいて重力補償値を求め、第１ゲイン手段により重力補償値に第１ゲインαを乗算する。また、制御装置では、相対変位量算出手段により検出したアクチュエータ変位量と検出した負荷変位量との相対変位量を算出し、第２ゲイン手段により相対変位量に第２ゲイン１−αを乗算する。さらに、制御装置では、加算手段により目標変位量に第１ゲインαを乗算した重力補償値と第２ゲイン１−αを乗算した相対変位量を加算し、その加算値に基づいてアクチュエータを制御する。このように、制御装置では、フィードフォワード的に重力補償値にα（２より大きい値）を乗算して目標変位量に加算するとともにフィードバックした負荷とアクチュエータとの相対変位量に１−α（αに対して逆符号で１だけ小さい値）を乗算して目標変位量に加算し、重力補償項と振動補償項との関連性をなくすとともに振動補償項に対するゲインの大きさを１以上とする。つまり、重力補償項をフィードフォワードとし、２より大きなゲインとすることにより、柔軟機構における相対変位にかかるゲインの大きさを１以上に設定でき、柔軟機構における振動波形を実際より大きく見積もることができる。そのため、柔軟機構の相対変位に対するアクチュエータの変位量を大きくすることができ、アクチュエータの変位量が負荷の動いている方向と同じ方向に目標変位量より大きな変位量となり、その目標変位量より増分の変位量が柔軟機構を大きく変位させ、その変位よる発生力が振動の減衰力として大きく作用する。その結果、柔軟機構による振動を抑制でき、振動の整定時間を短くできるとともに、柔軟機能の柔軟性も発揮できる。したがって、負荷の動きも、早期に、安定した状態となる。なお、第２ゲイン１−αの大きさを１より大きくしているが、２つのゲインを合わせた値は１となる。
【００１１】
本発明に係る制御装置は、柔軟機構を介して負荷を動作させるアクチュエータを制御する制御装置であって、アクチュエータの変位量を検出するアクチュエータ変位量検出手段と、負荷の変位量を検出する負荷変位量検出手段と、目標変位量に基づいて重力補償値を求める重力補償手段と、重力補償手段による重力補償値に第１ゲインα（＞２）を乗算する第１ゲイン手段と、アクチュエータ変位量検出手段で検出したアクチュエータの変位量と負荷変位量検出手段で検出した負荷の変位量との相対変位量を算出する相対変位量算出手段と、相対変位量算出手段で算出した相対変位量の位相を遅らせる位相遅れ手段と、位相遅れ手段で位相を遅らせた相対変位量に第２ゲイン１−αを乗算する第２ゲイン手段と、目標変位量に１ゲイン手段による乗算値及び第２ゲイン手段による乗算値を加算する加算手段とを備え、加算手段による加算値に基づいてアクチュエータを制御することを特徴とする。
【００１２】
この制御装置は、アクチュエータ変位量検出手段によりアクチュエータの変位量を検出するとともに、負荷変位量検出手段により負荷の変位量を検出する。そして、制御装置では、重力補償手段により目標変位量に基づいて重力補償値を設定し、第１ゲイン手段により重力補償値に第１ゲインαを乗算する。また、制御装置では、相対変位量算出手段により検出したアクチュエータ変位量と検出した負荷変位量との相対変位量を算出し、位相遅れ手段により相対変位量の位相を遅らせ、第２ゲイン手段によりその位相遅らせた相対変位量に第２ゲイン１−αを乗算する。さらに、制御装置では、加算手段により目標変位量に第１ゲインαを乗算した重力補償値と第２ゲイン１−αを乗算した位相を遅らせた相対変位量を加算し、その加算値に基づいてアクチュエータを制御する。このように、制御装置では、上記の２つの制御装置の構成を備えて、２つの制御装置の作用効果を有している。したがって、柔軟機構による振動をより抑制でき、振動の整定時間をより短くできるとともに、柔軟機能の柔軟性も発揮できる。
【発明の効果】
【００１３】
本発明によれば、柔軟機構による振動を抑制することができ、負荷の動きを迅速に安定した状態とすることできる。
【発明を実施するための最良の形態】
【００１４】
以下、図面を参照して、本発明に係る制御装置の実施の形態を説明する。
【００１５】
本実施の形態では、本発明に係る制御装置を、モータ（アクチュエータに相当）とリンク（負荷に相当）とが柔軟機構で連結されたロボットの制御装置に適用する。本実施の形態に係るロボット制御装置では、フィードバックによるＰ−ＰＩ制御（比例・比例積分制御）に、柔軟機構による振動を抑制するために位相遅れを利用した制御（第１の振動抑制制御）とゲイン設定を利用した制御（第２の振動抑制制御）の２つの制御を付加している。本実施の形態には、関節の個数により２つの形態があり、第１の実施の形態が単一の関節の形態であり、第２の実施の形態が多関節の形態である。
【００１６】
図１〜図７を参照して、第１の実施の形態に係るロボット制御装置について説明する。図１は、第１の実施の形態に係るロボット制御装置の構成図である。図２は、図１のロボット制御装置の制御対象の構成図である。図３は、図１のロボット制御装置の制御系のブロック図である。図４は、図１のロボット制御装置における第１の振動抑制制御の構成図である。図５は、目標角速度、制御後の目標角速度、実際のリンク角速度の時間変化を示すグラフであり、（ａ）が第１の振動抑制制御を加味しない場合であり、（ｂ）が第１の振動抑制制御を加味した場合である。図６は、図１のロボット制御装置における第２の振動抑制制御の構成図である。図７は、目標角速度、制御後の目標角速度、実際のリンク角速度の時間変化を示すグラフであり、（ａ）が第２の振動抑制制御を加味しない場合であり、（ｂ）が第２の振動抑制制御を加味した場合である。
【００１７】
ロボット制御装置１の制御対象は、モータＭとリンクＬとが柔軟機構Ｓを介して連結されたものが対象である。モータＭは、柔軟機構Ｓを介してリンクＬを回転させるためのモータである。モータＭでは、ロボット制御装置１から駆動電流が供給され、その駆動電流に応じて回転駆動する。柔軟機構Ｓは、リンクＬに柔軟性を持たせるための機構であり、バネである（図２参照）。柔軟機構としては、バネ以外に、必要に応じてダンパなどの他の要素を備える構成としてもよい。リンクＬは、ロボットの動作部分であり、例えば、アーム、ハンド、指である。
【００１８】
ロボット制御装置１は、リンクＬを所定動作させるために、モータＭを駆動制御する。特に、ロボット制御装置１は、バネＳによる振動を抑制するために、第１の振動抑制制御と第２の振動抑制制御を行う。そのために、ロボット制御装置１は、モータ回転検出センサ１０、リンク回転検出センサ１１、動作指令部１２、減算器１３、位相遅れフィルタ１４、乗算器１５、重力補償部１６、乗算器１７、加算器１８、フィードバック制御部１９、ドライバ２０を備えている。
【００１９】
なお、本実施の形態では、モータ回転検出センサ１０が特許請求の範囲に記載するアクチュエータ変位量検出手段に相当し、リンク回転検出センサ１１が特許請求の範囲に記載する負荷変位量検出手段に相当し、減算器１３が特許請求の範囲に記載する相対変位量算出手段に相当し、位相遅れフィルタ１４が特許請求の範囲に記載する位相遅れ手段に相当し、乗算器１５が特許請求の範囲に記載する第２ゲイン手段に相当し、重力補償部１６が特許請求の範囲に記載する重力補償手段に相当し、乗算器１７が特許請求の範囲に記載する第１ゲイン手段に相当し、加算器１８が特許請求の範囲に記載する加算手段に相当する。
【００２０】
ロボット制御装置１における各構成について説明する前に、第１の振動抑制制御と第２の振動抑制制御について説明しておく。まず、図４を参照して、第１の振動抑制制御について説明する。この制御では、リンクＬの動作とモータＭの動作の位相をずらすことによって、そのずらした分がバネＳのたわみ（変位）として現れ、そのバネＳのたわみによる発生力がリンクＬの振動を抑制する方向に作用する（そのバネＳのたわみが振動の減衰力となる）。そのために、この制御では、モータ回転角度θｍとリンク回転角度θｌとの相対角度θｒの位相遅らせ、その位相遅らせた相対角度θｒ’をリンクＬの目標回転角度θｒｅｆに加算する。この際、目標回転角度θｒｅｆに位相遅らせた相対角度θｒ’を加算することによってリンクＬの動いている方向にモータＭの回転角度を目標回転角度θｒｅｆより増分し（すなわち、目標より大きなモータトルクを与え）、バネＳを変位させる。リンクＬの重力が作用している場合、このバネＳの変位による発生力がリンクＬに作用する重力による力を打ち消すように作用する。この第１の振動抑制制御を行うために、モータ回転検出センサ１０、リンク回転検出センサ１１、動作指令部１２、減算器１３、位相遅れフィルタ１４、加算器１８が構成される。
【００２１】
次に、図６を参照して、第２の振動抑制制御について説明する。この制御では、目標回転角度θｒｅｆによって重力補償項をフィードフォワード的に求めることによって重力補償項と振動補償項との関連を無くし、モータ回転角度θｍとリンク回転角度θｌとの相対角度θｒにかかるゲインを１より大きな値に設定し、バネＳによる振動を実際より大きく見積もり、振動補償項として大きな値を与える。そのために、この制御では、目標回転角度θｒｅｆから求めた重力補償値ｇｔに第１ゲインαを乗算した第１補正角度θｈ１を求め、相対角度θｒに第２ゲイン１−αを乗算した第２補正角度θｈ２を求め、第１補正角度θｈ１と第２補正角度θｈ２を目標回転角度θｒｅｆに加算する。この際、第１ゲインαを２より大きい実数とすることにより、第２ゲイン１−αは−１より小さい値とする（すなわち、第２ゲイン１−αの大きさは１より大きな値となる）。これによって、目標回転角度θｒｅｆに重力補償項と振動補償項として大きな値が加算され、リンクＬの動いている方向にモータＭの回転角度を目標回転角度θｒｅｆより大きく増分でき（すなわち、目標より大きなモータトルクを与えることができ）、バネＳを大きく変位させる。リンクＬに重力が作用している場合、このバネＳの大きな発生力がリンクＬに作用する重力による力を打ち消すように作用する。この第２の振動抑制制御を行うために、モータ回転検出センサ１０、リンク回転検出センサ１１、動作指令部１２、減算器１３、乗算器１５、重力補償部１６、乗算器１７、加算器１８が構成される。
【００２２】
それでは、ロボット制御装置１における各構成について説明する。モータ回転検出センサ１０は、モータＭに設けられ、モータＭの実際の回転角度θｍを検出する。リンク回転検出センサ１１は、リンクＬに設けられ、リンクＬの実際の回転角度θｌを検出する。動作指令部１２では、リンクＬに所定動作を行わせるために、リンクＬの目標回転角度θｒｅｆを設定する。
【００２３】
減算器１３では、モータ回転検出センサ１０で検出したモータ回転角度θｍからリンク回転検出センサ１１で検出したリンク回転角度θｌを減算し、その減算値を相対角度θｒとして出力する。この相対角度θｒは、モータＭとリンクＬとの相対角度（誤差角度）であり、バネＳにおける変位にも相当する。
【００２４】
位相遅れフィルタ１４は、式（１）で示す一次遅れフィルタＧｆ（ｓ）が構成される。位相遅れフィルタ１４では、減算器１３から相対角度θｒが入力され、位相遅れ相対角度θｒ’を出力する。一次遅れフィルタＧｆ（ｓ）は、０°＜位相遅れ角＜１８０°となるようにフィルタ設計され、リンクＬの慣性モーメントＪｌに応じて更新される。
【００２５】
【数１】
2006244300.tif 000003
式（１）において、Ｔｆは時定数であり、リンクＬの共振周波数成分と同じ周波数成分をそのまま通過させ、その周波数成分の位相を遅らせるように設定される。例えば、式（２）に示すように周波数ωｆをリンクＬの共振周波数ωｎの１．５倍の周波数とした場合、式（３）の関係から、式（４）に示すように時定数Ｔｆを共振周波数ωｎから求めることができる。共振周波数ωｎは、リンクＬの慣性モーメントＪｌを用いて、式（５）から求めることができる。
【００２６】
【数２】
2006244300.tif 000004
式（５）において、ＫｓはバネＳのバネ定数であり、固定値である。したがって、リンクＬの慣性モーメントＪｌを求めることにより、一次遅れフィルタＧｆ（ｓ）の時定数Ｔｆを設定することができる。リンクＬの慣性モーメントＪｌは、リンク回転角度θｌを用いて、逆動力学から算出される。
【００２７】
乗算器１５では、位相遅れフィルタ１４を通された位相遅れ相対角度θｒ’に第２ゲイン１−αを乗算し、第２補正角度θｈ２を出力する。この第２補正角度θｈ２は、振動補償項であり、モータＭの位相をリンクＬの位相に対して遅らせるとともに第２ゲイン１−αの値によって振動補償項として大きな値が設定される。
【００２８】
重力補償部１６では、式（６）により、目標回転角度θｒｅｆから重力補償値ｇｔを算出する。この重力補償値ｇｔは、重力の影響によって発生するバネＳのたわみを考慮し、そのたわみを補償するための値である。
【００２９】
【数３】
2006244300.tif 000005
式（６）において、ｍはリンクＬの質量であり、ｇは重力加速度であり、ｌはリンクＬの長さである。ｍ、ｇ、ｌ、Ｋｓは固定値なので、重力補償値ｇｔは、目標回転角度θｒｅｆに応じて変化する。
【００３０】
乗算器１７では、重力補償部１６の重力補償値ｇｔに第１ゲインαを乗算し、第１補正角度θｈ１を出力する。この第１補正角度θｈ１は、重力補償項であり、第１ゲインαの値によって重力補償項として大きな値が設定される。第１ゲインαは、２より大きな実数であり、振動補償項に大きなゲインをかけるために大きな値が設定される。例えば、第１ゲインαとして１０が設定された場合、第２ゲイン１−αは−９となり、大きさが９である。
【００３１】
加算器１８は、第１加算器１８ａと第２加算器１８ｂからなる。第１加算器１８ａでは、乗算器１７からの第１補正角度θｈ１と乗算器１５からの第２補正角度θｈ２とを加算し、補正角度θｈを出力する。第２加算器１８ｂでは、第１加算器１８ａからの補正角度θｈと動作指令部１２からの目標回転角度θｒｅｆとを加算し、補正目標回転角度θｒｅｆ’を出力する。
【００３２】
フィードバック制御部１９は、補正目標回転角度θｒｅｆ’を入力し、Ｐ−ＰＩ制御によって補正目標回転角度θｒｅｆ’に応じたモータトルクｕを出力する。フィードバック制御部１９は、Ｐ制御部１９ａとＰＩ制御部１９ｂからなる。
【００３３】
Ｐ制御部１９ａは、減算器１９ｃと乗算器１９ｄを備える。減算器１９ｃでは、第２加算器１８ｂからの補正目標回転角度θｒｅｆ’からモータ回転検出センサ１０で検出したモータ回転角度θｍを減算する。乗算器１９ｄでは、減算器１９ｃの出力値に比例ゲインＫｐ１を乗算する。
【００３４】
ＰＩ制御部１９ｂは、減算器１９ｅ、乗算器１９ｆ、積分器１９ｇ、乗算器１９ｈ、減算器１９ｉを備える。減算器１９ｅでは、Ｐ制御部１９ａの出力値からモータ回転検出センサ１０で検出したモータ回転角度θｍの微分値（モータ角速度）を減算する。乗算器１９ｆでは、減算器１９ｅの出力値に比例ゲインＫｐ２を乗算する。積分器１９ｇでは、モータ回転検出センサ１０で検出したモータ回転角度θｍの微分値を積分する。乗算器１９ｈでは、積分器１９ｇの出力値に積分ゲインＫｉ２を乗算する。減算器１９ｉでは、乗算器１９ｆの出力値から乗算器１９ｈの出力値を減算し、モータトルクｕを出力する。
【００３５】
ドライバ２０では、フィードバック制御部１９からのモータトルクｕを入力し、そのモータトルクｕを発生するために必要なモータ電流をモータＭに供給する。
【００３６】
図１〜図３を参照して、ロボット制御装置１における動作について説明する。モータ回転検出センサ１０では、モータＭの回転角度θｍを検出している。リンク回転検出センサ１１では、リンクＬの回転角度θｌを検出している。動作指令部１２では、リンクＬに所定動作をさせるために、目標回転角度θｒｅｆを設定する。
【００３７】
重力補償部１６では、目標回転角度θｒｅｆから重力補償値ｇｔを算出する。そして、乗算器１７では、重力補償値ｇｔに第１ゲインαを乗算し、第１補正角度θｈ１を出力する。これによって、重力補償項が、フィードフォワード的に求められ、振動補償項に大きなゲインをかけるために大きな第１ゲインαが掛けられている。
【００３８】
減算器１３では、モータ回転角度θｍからリンク回転角度θｌを減算し、相対角度θｒ（バネＳの変位）を求める。そして、位相遅れフィルタ１４では、相対角度θｒの位相遅らせ、位相遅れ相対角度θｒ’を出力する。さらに、乗算器１５では、位相遅れ相対角度θｒ’に第２ゲイン１−αを乗算し、第２補正角度θｈ２を出力する。これによって、振動補償項が、フィードバックによって求められ、モータＭの位相をリンクＬの位相に対して遅らせるとともに大きな第２ゲイン１−αが掛けられる。
【００３９】
そして、加算器１８では、目標回転角度θｒｅｆに第１補正角度θｈ１と第２補正角度θｈ２を加算し、補正目標回転角度θｒｅｆ’を出力する。これによって、補正目標回転角度θｒｅｆ’は、目標回転角度θｒｅｆに対してリンクＬの回転する方向に増分の回転角度が加算され、モータＭの位相をリンクＬの位相に対して遅らせる要素を含んでいる。この増分の回転角度と位相遅れによるバネＳのたわみによってバネＳに大きな変位が発生し、この変位による発生力がリンクＬにかかる重力による力に釣り合うように作用する。
【００４０】
フィードバック制御部１９では、補正目標回転角度θｒｅｆ’に対してＰ−ＰＩ制御を施し、補正目標回転角度θｒｅｆ’に応じたモータトルクｕを出力する。ドライバ２０では、モータトルクｕを発生するために必要なモータ電流をモータＭに供給する。
【００４１】
モータＭでは、ドライバ２０から供給されるモータ電流に応じて回転する。リンクＬは、モータＭの回転に応じて回転する。この際、モータＭは、位相がリンクＬの位相に対して遅れ、目標回転角度θｒｅｆより大きくリンクＬを回転させる。これによって、バネＳは、大きく変位し、リンクＬの回転する方向（リンクＬにかかる重力による力と逆方向）に大きな力が発生する。その結果、リンクＬに作用する重力の力を早期に打ち消し、バネＳによる振動が迅速に減衰する。したがって、リンクＬは、迅速に、振動のない安定した状態で動作する。この際、リンクＬに人などの物体が接触した場合、バネＳの柔軟性により柔らかな接触感が得られる。
【００４２】
図５には、（ａ）図にフィードバック制御に第１の振動抑制制御を加味しない場合と（ｂ）図にフィードバック制御に第１の振動抑制制御を加味した場合の目標角速度（一点鎖線）、制御後の目標角速度（点線）、実際のリンクの角速度（実線）の時間変化を示している。第１の振動抑制制御を加味しない場合、リンクＬの角速度と制御後の目標角速度とが一致するように制御（つまり、バネＳの変位が０になるように制御）を行うので、重力の影響によって、リンクＬ（バネＳ）の振動が暫くの間継続する。第１の振動抑制制御を加味した場合、リンクＬの角速度に対して制御後の目標角速度の位相を遅らせるように制御を行うので、その遅らせ分がバネＳのたわみとなって現れ、そのたわみによる発生力がリンクＬにかかる重力による力を抑制する方向に作用する。その結果、バネＳによる振動が、迅速に、収束する。
【００４３】
図７には、（ａ）図にフィードバック制御に第２の振動抑制制御を加味しない場合と（ｂ）図にフィードバック制御に第２の振動抑制制御を加味した場合の目標角速度（一点鎖線）、制御後の目標角速度（点線）、実際のリンクの角速度（実線）の時間変化を示している。第２の振動抑制制御を加味しない場合、上記と同様に、重力の影響によって、リンクＬの振動が暫くの間継続する。第２の振動抑制制御を加味した場合、制御後の目標角速度に目標角速度よりリンクＬの回転する方向に大きな増分を有するように制御を行うので、その増分がバネＳの変位となって現れ、その変位による発生力がリンクＬにかかる重力による力を抑制する方向に作用する。その結果、バネＳよる振動が、迅速に、収束する。
【００４４】
このロボット制御装置１では、モータＭの位相をリンクＬの位相に対して遅らせることにより、バネＳにたわみを発生させ、リンク振動の減衰効果を高める。また、ロボット制御装置１では、リンクＬの回転する方向に目標回転角度θｒｅｆより大きく回転させることにより、バネＳに大きな変位を発生させ、リンク振動の減衰効果を高める。その結果、重力などの影響によるリンクＬに作用する反力を打ち消し、バネＳによるリンクＬの振動を迅速に抑制することができる。したがって、リンクＬの動きは、迅速に、振動を伴わない安定した動きとなる。
【００４５】
図８及び図９を参照し、第２の実施の形態に係るロボット制御装置３１について説明する。図８は、第２の実施の形態に係るロボット制御装置の制御対象の一部の構成図である。図９は、第２の実施の形態に係るロボット制御装置の任意の関節に対する制御系のブロック図である。なお、ロボット制御装置３１では、第１の実施の形態に係るロボット制御装置１と同様の構成については同一の符号を付し、その説明を省略する。
【００４６】
ロボット制御装置３１の制御対象は、モータＭｉ（ｉ＝１，２，・・・）とリンクＬｉ（ｉ＝１，２，・・・）とがバネＳｉ（ｉ＝１，２，・・・）を介して連結された関節が多数段繋がった多関節系が対象である。この多関節系は、例えば、リンクＬｉが上流側から上アーム、下アーム、ハンド、各指であり、関節が上流側から肩、肘、手首、各指の関節である。図８には、その多関節系の一部が示されており、ある段のモータＭｉとリンクＬｉとがバネＳｉを介して連結された関節の下流側にモータＭｉ＋１とリンクＬｉ＋１とがバネＳｉ＋１を介して連結された関節が繋がっている。多関節系では、各リンクＬｉにはそれより下流側における全ての関節における各慣性モーメントが積算された慣性モーメントが作用する。したがって、あるリンクＬｉが回転しない場合でも、それより下流側のリンクＬが回転するとそのリンクＬｉの慣性モーメントが変化する。
【００４７】
ロボット制御装置３１は、各リンクＬｉを所定動作させるために、関節毎にロボット制御装置１と同様の制御を行い、各モータＭｉを駆動制御する。特に、ロボット制御装置３１は、バネＳｉによる振動を抑制するために、関節毎に第１の振動抑制制御と第２の振動抑制制御を行う。その際、ロボット制御装置３１では、各関節のリンクＬｉに作用する慣性モーメントに応じて一次遅れフィルタを設定する。そのために、ロボット制御装置３１は、関節毎に、モータ回転検出センサ１０、リンク回転検出センサ１１、動作指令部１２、減算器１３、逆動力学計算部３２、共振周波数計算部３３、位相遅れフィルタ３４、乗算器１５、重力補償部１６、乗算器１７、加算器１８、フィードバック制御部１９、ドライバ２０を備えている（図１も参照）。
【００４８】
逆動力学計算部３２では、制御対象の関節及びそれより下流側の全ての関節に繋がる各リンクＬの回転角度θｌを用いて、逆動力学により各関節にかかる慣性モーメントを下流側から順次算出する。そして、逆動力学計算部３２では、これらの各関節にかかる慣性モーメントから制御対象の関節に繋がるリンクＬの慣性モーメントＪｌを算出する。
【００４９】
共振周波数計算部３３では、式（５）により、逆動力学計算部３２で求めた制御対象の関節に繋がるリンクＬの慣性モーメント及び制御対象の関節のバネＳのバネ定数Ｋｓを用いて、そのリンクＬの共振周波数ωｎを算出する。
【００５０】
位相遅れフィルタ３４では、式（４）により、共振周波数計算部３３で求めた共振周波数ωｎを用いて時定数Ｔｆを算出し、その時定数Ｔｆを用いて式（１）の一次遅れフィルタＧｆ（ｓ）を設定する。そして、位相遅れフィルタ３４では、減算器１３から相対角度θｒが入力され、位相遅れ相対角度θｒ’を出力する。
【００５１】
このように、ロボット制御装置３１では、関節毎に一次遅れフィルタＧｆ（ｓ）を設定し、その一次遅れフィルタＧｆ（ｓ）を各リンクＬに作用する慣性モーメントＪｌに応じて更新していく。更新するタイミングとしては、逐次的に更新したり、あるいは、慣性モーメントの変化する範囲が既知の場合にはその範囲内の一定間隔毎に更新する。逐次的に更新する場合、制御周期毎に更新する。このように更新することによって、常に最適な応答を得ることができる。一定間隔毎に更新する場合、慣性モーメントの変化する範囲を一定間隔毎に分割し、それぞれの間隔において位相遅れフィルタを１つづつ設計しておき、慣性モーメントの値に応じてフィルタを切り換える。このように更新することによって、制御系が比較的安定する。
【００５２】
このロボット制御装置３１によれば、多関節系の各関節において第１の実施の形態に係るロボット制御装置１と同様の効果が得られる。その結果、ロボットの全てのリンクＬの動きは、迅速に、振動を伴わない安定した動きとなる。
【００５３】
以上、本発明に係る実施の形態について説明したが、本発明は上記実施の形態に限定されることなく様々な形態で実施される。
【００５４】
例えば、本実施の形態ではロボットに適用し、アクチュエータをモータとし、負荷をロボットのリンクとし、負荷の変位量を回転としたが、ロボット以外にも適用可能であり、例えば、連結された複数の車両において、一方側にアクチュエータがあり、他方側にアクチュエータがない連結部における振動抑制にも適用可能である。
【００５５】
また、本実施の形態ではバネによる振動を抑制するために位相遅れを利用した制御とゲイン設定を利用した制御の両方の制御をフィードバック制御に加味する構成としたが、２つの制御のいずれか一方の制御だけを加味する構成としてもよい。
【００５６】
また、本実施の形態ではフィードフォワード的に重力のみを補償する重力補償項を設定する構成としたが、重力以外にも外乱などの他の要素も補償するような補償項を設定する構成としてもよい。
【００５７】
また、本実施の形態では位相遅れフィルタを一次遅れフィルタで構成したが、二次遅れフィルタなどの他の位相遅れフィルタで構成してもよい。
【図面の簡単な説明】
【００５８】
【図１】第１の実施の形態に係るロボット制御装置の構成図である。
【図２】図１のロボット制御装置の制御対象の構成図である。
【図３】図１のロボット制御装置の制御系のブロック図である。
【図４】図１のロボット制御装置における第１の振動抑制制御の構成図である。
【図５】目標角速度、制御後の目標角速度、実際のリンク角速度の時間変化を示すグラフであり、（ａ）が第１の振動抑制制御を加味しない場合であり、（ｂ）が第１の振動抑制制御を加味した場合である。
【図６】図１のロボット制御装置における第２の振動抑制制御の構成図である。
【図７】目標角速度、制御後の目標角速度、実際のリンク角速度の時間変化を示すグラフであり、（ａ）が第２の振動抑制制御を加味しない場合であり、（ｂ）が第２の振動抑制制御を加味した場合である。
【図８】第２の実施の形態に係るロボット制御装置の制御対象の一部の構成図である。
【図９】第２の実施の形態に係るロボット制御装置の任意の関節に対する制御系のブロック図である。
【符号の説明】
【００５９】
１，３１…ロボット制御装置、１０…モータ回転検出センサ、１１…リンク回転検出センサ、１２…動作指令部、１３…減算器、１４，３４…位相遅れフィルタ、１５，１７…乗算器、１６…重力補償部、１８…加算器、１８ａ…第１加算器、１８ｂ…第２加算器、１９…フィードバック制御部、１９ａ…Ｐ制御部、１９ｂ…ＰＩ制御部、１９ｃ，１９ｅ，１９ｉ…減算器、１９ｄ，１９ｆ，１９ｈ…乗算器、１９ｇ…積分器、２０…ドライバ、３２…逆動力学計算部、３３…共振周波数計算部
【図１】
2006244300.tif 000006
【図２】
2006244300.tif 000007
【図３】
2006244300.tif 000008
【図４】
2006244300.tif 000009
【図５】
2006244300.tif 000010
【図６】
2006244300.tif 000011
【図７】
2006244300.tif 000012
【図８】
2006244300.tif 000013
【図９】
2006244300.tif 000014