JP2015122932A - ロボットの制御装置および制御方法 - Google Patents

Abstract

【課題】 デュアルサーボ制御を行う２つのモータのうちの一方にトルクが集中するのを防ぐことができるロボットの制御装置およびロボットの制御方法を提供する。【解決手段】 マスタモータ４が第１電流指令値Ｉ１に基づいて発生した出力トルクＴＭに応じた電流値ＩＭと第１電流指令値Ｉ１との第１電流偏差ΔＩ１を監視する第１電流偏差監視部１０と、スレーブモータ６が第２電流指令値Ｉ２に基づいて発生した出力トルクＴＳに応じた電流値ΔＩ２と第２電流指令値Ｉ２との第２電流偏差ΔＩ２を監視する第２電流偏差監視部１１と、第１電流偏差ΔＩ１と第２電流偏差ΔＩ２との差が予め定められたしきい値以上である場合に、第１電流偏差ΔＩ１と第２電流偏差ΔＩ２との差が小さくなるように第１電流指令値Ｉ１および／または第２電流指令値Ｉ２を変更する電流指令値変更部１と、を備える。【選択図】 図１

Description

本発明は、ロボットの制御装置および制御方法に関し、特に、１つの駆動軸を２つのモータで駆動するようにするデュアルサーボ制御構造を備えたロボットの制御装置および制御方法に関する。

工作機械等のロボットにおいて、ロボットの動作を規定する可動部分を駆動する駆動軸にかかる負荷が大きく１つのモータだけで駆動軸を駆動することができない場合に、２つのモータを互いに連結し、１つの駆動軸を駆動するように２つのモータを制御するデュアルサーボ制御構造が知られている。従来のデュアルサーボ制御構造においては、２つのモータのうちの何れか一方のモータへの位置指令値に基づいて当該一のモータの現在位置を２つのモータの制御系にフィードバックすることにより２つのモータに対してフィードバック制御が行われる。

さらに、デュアルサーボ制御構造においてより安定的な制御を行うための構成として、例えば特許文献１のような構成が提案されている。特許文献１の構成においては、一方のモータ（マスタモータ）へのトルク指令と、他方のモータ（スレーブモータ）へのトルク指令との差に応じてスレーブモータへのトルク指令を補正している。

特開２００３−７９１８０号公報

しかし、このような構成であっても、以下のような問題が生じ得る。すなわち、駆動軸が外力を受けている状態において、一方のモータ軸にガタ（バックラッシュ）が存在すると、ガタのない（少ない）方のモータ軸を有するモータが駆動軸にかかる外力のすべてを負担することになる。このため、本来であれば２つのモータを使用することにより、各モータにおける定格負荷に対する負荷の割合（対定格負荷率）を低減することができる所定の動作時においても、一方のモータにおける対定格負荷率が増大し、モータの寿命を縮めてしまったり、動作計画の変更が必要となったりする可能性が生じる。

本発明は、以上のような課題を解決すべくなされたものであり、デュアルサーボ制御を行う２つのモータのうちの一方にトルクが集中するのを防ぐことができるロボットの制御装置およびロボットの制御方法を提供することを目的とする。

本発明の一態様に係るロボット制御装置は、ロボットの動作を規定する可動部分を駆動する駆動軸と、マスタ動力伝達機構を介して前記駆動軸を回転駆動するマスタモータと、スレーブ動力伝達機構を介して前記駆動軸を回転駆動するスレーブモータと、前記マスタモータの現在位置を検出する位置検出器と、を備えたロボットの制御装置であって、前記マスタモータへの位置指令値と前記位置検出器により検出される前記マスタモータの現在位置を示す値との偏差（以下、マスタ位置偏差）に基づいて前記マスタモータへの電流指令値（以下、第１電流指令値）を生成する第１電流指令値生成部と、前記マスタ位置偏差または所定のトルク指令値に基づいて前記スレーブモータへの電流指令値（以下、第２電流指令値）を生成する第２電流指令値生成部と、前記マスタモータが前記第１電流指令値に基づいて発生した出力トルクに応じた電流値と前記第１電流指令値との偏差（以下、第１電流偏差）を監視する第１電流偏差監視部と、前記スレーブモータが前記第２電流指令値に基づいて発生した出力トルクに応じた電流値と前記第２電流指令値との偏差（以下、第２電流偏差）を監視する第２電流偏差監視部と、前記第１電流偏差と前記第２電流偏差との差が予め定められたしきい値以上である場合に、前記第１電流偏差と前記第２電流偏差との差が小さくなるように前記第１電流指令値および／または前記第２電流指令値を変更する電流指令値変更部と、を備えている。

上記構成によれば、マスタモータとスレーブモータとの間で電流偏差に差がある場合に、電流偏差の差が小さくなるように第１電流指令値および／または第２電流指令値が変更される。したがって、マスタモータとスレーブモータとの位置偏差が大きい場合に、マスタモータおよびスレーブモータのうちの一方にトルクが集中するのを防ぐことができる。

前記第１電流指令値生成部は、前記マスタ位置偏差に基づいたマスタ速度指令値の時間的偏差（以下、マスタ速度偏差）に対してＰＩ補償を行うことにより電流指令値を生成する第１速度ループ演算部と、前記ロボットの駆動軸に作用する重力を考慮した動力学トルクに基づく電流補正値を生成し、前記第１速度ループ演算部で生成された前記電流指令値に前記電流補正値を加えて前記第１電流指令値を生成する第１電流補正部と、を有し、前記第２電流指令値生成部は、前記マスタ速度偏差に対してＰＩ補償を行うことにより電流指令値を生成する第２速度ループ演算部と、前記ロボットの駆動軸に作用する重力を考慮した動力学トルクに基づく電流補正値を生成し、前記第２速度ループ演算部で生成された前記電流指令値に前記電流補正値を加えて前記第２電流指令値を生成する第２電流補正部と、を有してもよい。これにより、マスタモータへの電流指令値（第１電流指令値）とスレーブモータへの電流指令値（第２電流指令値）とを共通の目標値に基づいて生成することができる。

前記電流指令値変更部は、前記第１電流偏差と前記第２電流偏差との差が予め定められたしきい値以上である場合に、前記第２速度ループ演算部のＰＩ補償における比例ゲインおよび積分ゲインのうちの少なくとも一方を前記第１速度ループ演算部のＰＩ補償において対応するゲインに対して低くしてもよい。これによれば、スレーブモータに対する制御ゲインをマスタモータに対する制御ゲインに比べて小さくすることにより、加速時または減速時においてマスタ軸およびスレーブ軸の何れかにガタがある場合に、ガタがある方の軸の動作が急峻になり、発生するトルクが不安定になることを防止することができる。

前記電流指令値変更部は、前記マスタ速度偏差が所定値以下である場合、かつ、前記第１電流偏差および前記第２電流偏差のうちの少なくとも一方において対応するモータが正転と反転とを繰り返す場合に、前記第２電流指令値生成部における前記速度ループ演算部のＰＩ補償における積分ゲインを０にしてもよい。これによれば、スレーブの速度ループにおけるフィードバック制御をＰＩ制御から比例制御に変更することにより、その場停止時においてマスタモータおよびスレーブモータにおいて不安定な動作（例えばマスタモータおよびスレーブモータが互いに逆回転するように正転反転を繰り返したり、マスタモータのある方向の回転とスレーブモータの逆方向の回転とが交互に生じたり、マスタモータ４およびスレーブモータ６の何れか一方が正転反転を繰り返したりする等の押し引き動作）が生じるのを防止することができる。

前記第２速度ループ演算部のＰＩ補償における比例ゲインおよび積分ゲインと前記第２電流補正部におけるゲインとの間で所定の重み付けが設定可能に構成されてもよい。これによれば、スレーブモータに対する第２電流補正部による理論値に基づく制御ゲインと、第２速度ループ演算部のＰＩ制御における制御ゲインとの重み付けを変更することにより、マスタモータおよびスレーブモータの制御の高精度化と、マスタモータおよびスレーブモータの制御の迅速化とのバランスを調整することができる。

前記第２電流指令値生成部は、前記ロボットの駆動軸に作用する重力を考慮した動力学トルクに基づいて前記マスタ位置偏差に依存しない前記第２電流指令値を生成するよう構成されてもよい。これによれば、スレーブモータは、マスタモータから独立した別系統の制御系となるため、スレーブモータの制御は、マスタ位置偏差に無関係となり、安定した制御系とすることができる。

本発明の他の態様に係るロボットの制御方法は、ロボットの動作を規定する可動部分を駆動する駆動軸と、マスタ動力伝達機構を介して前記駆動軸を回転駆動するマスタモータと、スレーブ動力伝達機構を介して前記駆動軸を回転駆動するスレーブモータと、前記マスタモータの現在位置を検出する位置検出器と、を備えたロボットの制御方法であって、前記マスタモータへの位置指令値と前記位置検出器により検出される前記マスタモータの現在位置を示す値との偏差（以下、マスタ位置偏差）に基づいて前記マスタモータへの電流指令値（以下、第１電流指令値）を生成し、前記マスタ位置偏差またはトルク指令値に基づいて前記スレーブモータへの電流指令値（以下、第２電流指令値）を生成し、前記マスタモータが前記第１電流指令値に基づいて発生した出力トルクに応じた電流値と前記第１電流指令値との偏差（以下、第１電流偏差）を監視し、前記スレーブモータが前記第２電流指令値に基づいて発生した出力トルクに応じた電流値と前記第２電流指令値との偏差（以下、第２電流偏差）を監視し、前記第１電流偏差と前記第２電流偏差との差が予め定められたしきい値以上である場合に、前記第１電流偏差と前記第２電流偏差との差が小さくなるように前記第１電流指令値および／または前記第２電流指令値を変更するものである。

上記方法によれば、マスタモータとスレーブモータとの間で電流偏差に差がある場合に、電流偏差が等しくなるように第１電流指令値および／または第２電流指令値が変更される。したがって、マスタモータとスレーブモータとの位置偏差が大きい場合に、マスタモータおよびスレーブモータのうちの一方にトルクが集中するのを防ぐことができる。

本発明は以上に説明したように構成され、デュアルサーボ制御を行う２つのモータのうちの一方にトルクが集中するのを防ぐことができるという効果を奏する。

図１は本発明の第１実施形態に係るロボットの制御装置の概略構成を示すブロック図である。 図２は図１に示すロボットの制御装置のより具体的な構成を示すブロック図である。 図３は本発明の第２実施形態に係るロボットの制御装置の構成を示すブロック図である。

以下、本発明の実施の形態を、図面を参照しながら説明する。なお、以下では全ての図を通じて同一又は相当する要素には同一の参照符号を付して、その重複する説明を省略する。

＜第１実施形態＞
まず、本発明の第１実施形態について説明する。図１は本発明の第１実施形態に係るロボット制御装置の概略構成を示すブロック図である。図１に示すように、本実施形態におけるロボット制御装置１は、ロボットの動作を規定する可動部分を駆動する駆動軸２と、マスタ動力伝達機構３を介して駆動軸２を回転駆動するマスタモータ４と、スレーブ動力伝達機構５を介して駆動軸２を回転駆動するスレーブモータ６と、マスタモータ４の現在位置を検出する位置検出器７と、を備えている。

本実施形態が適用されるロボットは、例えば複数のアーム部材と、これらを接続する複数の関節（回転軸）とを備えた多関節ロボットとして構成される。すなわち、多関節ロボットにおける特定のアーム部材を可動部分として回動させる回転軸が本実施形態における駆動軸２となる。マスタ動力伝達機構３は、マスタモータ４から出力される回転動力を駆動軸２に伝達する機構であれば特に限定されない。例えばマスタ動力伝達機構３は、ギヤ機構、アーム機構またはこれらを組み合わせた構成等、種々の構成を採用可能である。また、スレーブ動力伝達機構５もスレーブモータ６から出力される回転動力を駆動軸２に伝達する機構としてマスタ動力伝達機構３と同様に構成される。

位置検出器７は、マスタモータ４の出力軸の回転角度を検出するよう構成される。例えば、位置検出器７は、エンコーダまたはレゾルバ等により構成される。

さらに、ロボット制御装置１は、マスタモータ４への位置指令値Ｐ_ｏと位置検出器７により検出されるマスタモータ４の現在位置を示す値（マスタ位置）Ｐ_Ｍとの偏差（以下、マスタ位置偏差）ΔＰ_Ｍに基づいてマスタモータ４への電流指令値（以下、第１電流指令値）Ｉ_１を生成する第１電流指令値生成部８を備えている。第１電流指令値生成部８は、ＣＰＵ、メモリおよび各種信号の入出力装置等を備えたコンピュータの機能ブロックとして構成される。

第１電流指令値生成部８で生成された第１電流指令値Ｉ_１は、マスタモータ駆動部１３に入力される。マスタモータ駆動部１３は、入力された第１電流指令値Ｉ_１に基づいた出力トルクＴ_Ｍを発生させるようにマスタモータ４を駆動し、マスタモータ４は、回転動力を出力する。

さらに、ロボット制御装置１は、マスタ位置偏差ΔＰ_Ｍに基づいてスレーブモータ６への電流指令値（以下、第２電流指令値）Ｉ_２を生成する第２電流指令値生成部９を備えている。第２電流指令値生成部９も、第１電流指令値生成部８と同様にコンピュータの機能ブロックとして構成される。第２電流指令値生成部９で生成された第２電流指令値Ｉ_２は、スレーブモータ駆動部１４に入力される。スレーブモータ駆動部１４は、入力された第２電流指令値Ｉ_２に基づいた出力トルクＴ_Ｓを発生させるようにスレーブモータ６を駆動し、スレーブモータ６は、回転動力を出力する。

このように、本実施形態においては、マスタモータ４およびスレーブモータ６を制御する電流指令値Ｉ_１，Ｉ_２を共通のマスタ位置偏差ΔＰ_Ｍに基づいて生成する電流指令共通方式を採用している。

マスタモータ４は、発生した出力トルクＴ_Ｍに応じた電流値（以下、第１フィードバック電流値）Ｉ_Ｍ（後述する図２参照）をフィードバックし、当該第１フィードバック電流値Ｉ_Ｍと第１電流指令値Ｉ_１との偏差（以下、第１電流偏差）ΔＩ_１に対してＰＩ補償を行うように構成されている。同様に、スレーブモータ６は、発生した出力トルクＴ_Ｓに応じた電流値（以下、第２フィードバック電流値）Ｉ_Ｓ（後述する図２参照）をフィードバックし、当該第２フィードバック電流値Ｉ_Ｓと第２電流指令値Ｉ_２との偏差（以下、第２電流偏差）ΔＩ_２に対してＰＩ補償を行うように構成されている。

ロボット制御装置１は、第１電流偏差ΔＩ_１を監視する第１電流偏差監視部１０と、第２電流偏差を監視する第２電流偏差監視部１１と、第１電流偏差ΔＩ_１と第２電流偏差ΔＩ_２との差が予め定められたしきい値以上である場合に、第１電流偏差ΔＩ_１と第２電流偏差ΔＩ_２との差が小さくなるように第１電流指令値Ｉ_１および／または第２電流指令値Ｉ_２を変更する電流指令値変更部１２と、を備えている。第１電流偏差監視部１０、第２電流偏差監視部１１および電流指令値変更部１２も、コンピュータの機能ブロックとして構成される。電流指令値変更部１２は、第１電流偏差監視部１０から入力される第１電流偏差ΔＩ_１および第２電流偏差監視部１２から入力される第２電流偏差ΔＩ_２に基づいて、第１電流指令値生成部８に指令値変更信号Ｓ_１を出力し、第２電流指令値生成部９に指令値変更信号Ｓ_２を出力することが可能であるように構成される。

上記構成によれば、マスタモータ４に基づく第１電流偏差ΔＩ_１とスレーブモータ６に基づく第２電流偏差ΔＩ_２との間で差がある場合に、電流偏差ΔＩ_１，ΔＩ_２間の差が小さくなるように第１電流指令値Ｉ_１および／または第２電流指令値Ｉ_２が変更される。したがって、マスタモータ４とスレーブモータ６との位置偏差が大きい場合に、マスタモータ４およびスレーブモータ６のうちの一方にトルクが集中するのを防ぐことができる。

例えば、スレーブモータ６の出力軸からスレーブ動力伝達機構５を介して駆動軸２への伝達経路中にガタが生じている（マスタモータ４側に比べてガタが大きい）場合、駆動軸２の加速開始時において、両モータ４，６を駆動させると、ガタが生じていないマスタモータ４に全負荷が集中する。また、例えば、第１電流指令値Ｉ_１および第２電流指令値Ｉ_２に基づいて、対応するモータの軸角度、負荷設定、および、ダイナミクスからモータをモデル化してアシストする出力トルクＴ_Ｍ，Ｔ_Ｓを算出するトルクフィードフォワード制御を行うことが一般的に行われるが、このモデルと実際のモータとのダイナミクスの誤差が大きい場合（定常的な位置偏差ΔＰ_Ｍが大きい場合）にも何れか一方のモータ（例えばマスタモータ４）に全負荷が集中する場合がある。このように、マスタモータ４に全負荷が集中した状態が継続すると、当該マスタモータ４が出力可能なトルクを超えてしまい、正常な位置制御ができなくなるおそれがある。

マスタモータ４に全負荷が集中した状態において、マスタモータ４は負荷の増大により出力トルクＴ_Ｍが抑えられ、当該出力トルクＴ_Ｍに基づく第１フィードバック電流値Ｉ_Ｍは、第１電流値Ｉ_１との偏差（すなわち、第１電流偏差）ΔＩ_１が大きくなる。一方、ガタが生じている側のスレーブモータ６は負荷の負担が軽い（ほとんどない）ので、出力トルクＴ_Ｓに基づく第２フィードバック電流値Ｉ_Ｓは、第２電流指令値Ｉ_２によく追従し、第２電流偏差ΔＩ_２は小さい。

そこで、電流指令値変更部１２は、第１電流偏差ΔＩ_１と第２電流偏差ΔＩ_２との差が予め定められたしきい値以上となった場合に、第１電流指令値Ｉ_１と第２電流指令値Ｉ_２との差が小さくなるように各電流指令値Ｉ_１，Ｉ_２を変更する。上記例においては、電流指令値変更部１２は、第１電流指令値生成部８に第１電流指令値Ｉ_１を小さくするための指令値変更信号Ｓ_１を出力し、第２電流指令値生成部９に第２電流指令値Ｉ_２を大きくするための指令値変更信号Ｓ_２を出力する。電流指令値の変更量は、予め定められていてもよいし、電流偏差の差に応じて異なる値に設定されてもよい。スレーブモータ６側にガタが生じる（大きい）場合も同様である。

なお、電流指令値変更部１２は、いったん電流偏差の差がしきい値以上となった場合、電流偏差の差が所定値以下（例えば０）になるまで指令値の変更制御を継続してもよい。また、電流指令値変更部１２は、電流偏差の差がしきい値以上となった場合に、第１電流指令値Ｉ_１のみを変更してもよいし、第２電流指令値Ｉ_２のみを変更してもよい。電流指令値変更部１２は、電流偏差の差がしきい値を超えた時の各電流偏差ΔＩ_１，ΔＩ_２の値または各電流指令値Ｉ_１，Ｉ_２の値に応じて電流指令値を変更する対象（第１電流指令値Ｉ_１のみ、第２電流指令値Ｉ_２のみ、または、第１および第２電流指令値Ｉ_１，Ｉ_２の両方）を切り替えてもよい。

以下、本実施形態におけるロボット制御装置１のより詳しい構成を例示する。図２は図１に示すロボットの制御装置のより具体的な構成を示すブロック図である。前述したように、本実施形態におけるロボット制御装置１は、第１電流指令値Ｉ_１および第２電流指令値Ｉ_２に基づいて、対応するモータの軸角度、負荷設定、および、ダイナミクスからモータをモデル化してアシストする出力トルクＴ_Ｍ，Ｔ_Ｓを算出するトルクフィードフォワード制御を行う。

図２に示すように、第１電流指令値生成部８は、マスタ位置偏差ΔＰ_Ｍに基づいたマスタ速度指令値ｖ_Ｍの時間的偏差（以下、マスタ速度偏差）Δｖ_Ｍに対してＰＩ補償を行うことにより電流指令値Ｉ’_１を生成する第１速度ループ演算部１５と、ロボットの駆動軸２に作用する重力を考慮した動力学トルクに基づく電流補正値Ｉｇ_Ｍを生成し、第１速度ループ演算部１５で生成された電流指令値Ｉ’_１に電流補正値Ｉｇ_Ｍを加えて第１電流指令値Ｉ_１を生成する第１電流補正部１６と、を有している。

さらに、第１電流指令値生成部８は、マスタ位置偏差ΔＰ_Ｍに対してＰＩ補償を行うことによりマスタ速度指令値ｖ_Ｍを生成する第１位置ループ演算部１７と、マスタ位置Ｐ_Ｍを微分する微分器１８とを有している。第１位置ループ演算部１７は、位置ループゲインＫｐ_Ｍを用いてマスタ位置偏差ΔＰ_Ｍに対してＰＩ補償を行い、マスタ速度指令値ｖ_Ｍを出力する。マスタ速度指令値ｖ_Ｍとマスタ位置Ｐ_Ｍの微分値との差分がマスタ速度偏差Δｖ_Ｍとして第１速度ループ演算部１５に入力される。第１速度ループ演算部１５は、速度ループゲインＫｖ_Ｍを用いてマスタ速度偏差Δｖ_Ｍに対してＰＩ補償を行い、電流指令値Ｉ’_１を出力する。なお、位置ループゲインＫｐ_Ｍおよび速度ループゲインＫｖ_Ｍは、それぞれ比例ゲイン（Ｋｐ_ＭＰ，Ｋｖ_ＭＰ）および積分ゲイン（Ｋｐ_ＭＩ，Ｋｖ_ＭＩ）を含み、微分ゲインと積分ゲインとの値は個別に設定される。

第１電流補正部１６で生成される電流補正値Ｉｇ_Ｍの基となる重力を考慮した動力学トルクは、例えば重力トルク、加減速トルク、摩擦トルク、他の軸の動きを支えるトルク（干渉トルク）等を含む。重力を考慮した動力学トルクは、ロボット動作における理論値に基づく値として規定され、予め設定される。

マスタモータ駆動部１３は、第１速度ループ演算部１５から出力される電流指令値Ｉ’１に第１電流補正部１６から出力される第１電流補正値Ｉｇ_Ｍを加算した第１電流指令値Ｉ_１と第１フィードバック電流値Ｉ_Ｍとの偏差である第１電流偏差ΔＩ_１に基づいてＰＷＭ制御するように構成される。マスタモータ駆動部１３は、第１電流偏差ΔＩ_１を出力トルクＴ_Ｍに変換するＰＷＭ演算部１９を含む。ＰＷＭ演算部１９は、ＰＷＭ制御における電流制御ループゲインＫｉ_Ｍを用いて第１電流偏差ΔＩ_１が０になるように、マスタモータ４へのＰＷＭ出力におけるデューティ比を変化させ、出力トルクＴ_Ｍを発生させる。出力トルクＴ_Ｍは電流演算部２０により第１フィードバック電流値Ｉ_Ｍとしてフィードバックされる。

第１電流偏差ΔＩ_１は、第１電流偏差監視部１０に入力され、電流指令値変更部１２に伝達される。

第２電流指令値生成部９は、マスタ速度偏差Δｖ_Ｍに対してＰＩ補償を行うことにより電流指令値Ｉ’_２を生成する第２速度ループ演算部２１と、ロボットの駆動軸２に作用する重力を考慮した動力学トルクに基づく電流補正値Ｉｇ_Ｓを生成し、第２速度ループ演算部２１で生成された電流指令値Ｉ’_２に電流補正値Ｉｇ_Ｓを加えて第２電流指令値Ｉ_２を生成する第２電流補正部２２と、を有している。第２速度ループ演算部２１は、速度ループゲインＫｖ_Ｓを用いてスレーブ速度偏差Δｖ_Ｓに対してＰＩ補償を行い、電流指令値Ｉ’_２を出力する。なお、速度ループゲインＫｖ_Ｓは、比例ゲイン（Ｋｖ_ＳＰ）および積分ゲイン（Ｋｖ_ＳＩ）を含み、微分ゲインと積分ゲインとの値は個別に設定される。

これにより、スレーブモータ６を制御するための第２電流指令値Ｉ_２がマスタモータ４のためのマスタ位置偏差ΔＰ_Ｍに基づくマスタ速度偏差Δｖ_Ｍを用いて生成される。したがって、マスタモータ４への電流指令値（第１電流指令値）Ｉ_１とスレーブモータ６への電流指令値（第２電流指令値）Ｉ_２とを共通の目標値に基づいて生成することができる。これにより、マスタモータ４の位置フィードバックをスレーブモータ６にも効率的に負担させることができる。

スレーブモータ駆動部１４は、第２速度ループ演算部２１から出力される電流指令値Ｉ’_１に第２電流補正部２２から出力される第２電流補正値Ｉｇ_Ｓを加算した第２電流指令値Ｉ_２と第２フィードバック電流値Ｉ_Ｓとの偏差である第１電流偏差ΔＩ_２に基づいてＰＷＭ制御するように構成される。スレーブモータ駆動部１４は、第２電流偏差ΔＩ_２を出力トルクＴ_Ｓに変換するＰＷＭ演算部２３を含む。ＰＷＭ演算部２３は、ＰＷＭ制御における電流制御ループゲインＫｉ_Ｓを用いて第２電流偏差ΔＩ_２が０になるように、スレーブモータ６へのＰＷＭ出力におけるデューティ比を変化させ、出力トルクＴ_Ｓを発生させる。出力トルクＴ_Ｓは電流演算部２４により第２フィードバック電流値Ｉ_Ｓとしてフィードバックされる。

本実施形態において、電流指令値変更部１２は、第１電流偏差ΔＩ_１と第２電流偏差ΔＩ_２との差が予め定められたしきい値以上である場合に、第２速度ループ演算部２１のＰＩ補償における比例ゲインＫｖ_ＳＰおよび積分ゲインＫｖ_ＳＩのうちの少なくとも一方を第１速度ループ演算部１５のＰＩ補償において対応するゲインＫｖ_ＭＰ，Ｋｖ_ＭＩに対して低く設定するように構成される。これによれば、スレーブモータ６に対する制御ゲイン（速度ループゲイン）Ｋｖ_Ｓをマスタモータ４に対する制御ゲイン（速度ループゲイン）Ｋｖ_Ｍに比べて小さくすることにより、加速時または減速時においてマスタ軸（マスタモータ４の出力軸）およびスレーブ軸（スレーブモータ６の出力軸）の何れかにガタがある場合に、ガタがある方の軸の動作が急峻になり、発生するトルクが不安定になることを防止することができる。

例えば、マスタモータ４側の伝達経路におけるガタに対してスレーブモータ６側の伝達経路におけるガタが大きい場合、前述したように、駆動軸２の加速開始時において、両モータ４，６を駆動させると、ガタが生じていないマスタモータ４に全負荷が集中する。このため、正常な位置制御ができなくなるおそれがある。また、スレーブモータ６はガタが大きいため、当該ガタが埋まるまで一気に動作する。これにより、ガタが生じている伝達経路において、モータ側の部材（例えばモータの出力ギヤ等）が駆動軸側の部材（例えば出力ギヤに噛合される減速ギヤ等）に勢いよく衝突する。このため、駆動軸２に伝達されるトルクが不安定になり、制御が安定しなくなる（後述する押し引き動作のきっかけとなる）場合がある。

これに対し、スレーブモータ６に対する制御ゲインＫｖ_ＳＰ，Ｋｖ_ＳＩの少なくとも何れか一方が小さく設定されることにより、スレーブモータ６に対してマスタモータ４の速度制御を優先させることによって、スレーブモータ６の制御を緩やかに行うことにより急峻な動作を抑制し、駆動軸２に伝達されるトルクが不安定になることを防止することができる。さらに、上述したような電流偏差ΔＩ_１，ΔＩ_２を監視することによる電流指令値Ｉ_１，Ｉ_２の変更制御を行うことにより、一方のモータへの負荷の集中を抑制することができる。

なお、電流指令値変更部１２は、第１電流偏差ΔＩ_１と第２電流偏差ΔＩ_２との差が予め定められたしきい値以上である場合に、その都度、スレーブモータ６側の制御ゲインを小さく設定することとしてもよいし、例えば駆動軸２の動作開始時等、電流偏差の差がしきい値以上となる可能性が高いと思われる場合に、スレーブモータ６側の制御ゲインを予め小さく設定することとしてもよい。また、電流指令値Ｉ_１，Ｉ_２の変更制御を行うためのしきい値と、スレーブモータ６側の制御ゲインを小さくするしきい値とを異なる値としてもよい。この場合、何れのしきい値が大きくてもよい。また、比例ゲインＫｖ_ＳＰおよび積分ゲインＫｖ_ＳＩの何れについても小さく設定可能とする場合に、比例ゲインＫｖ_ＳＰを小さく設定するしきい値と積分ゲインＫｖ_ＳＩを小さく設定するしきい値とを異なる値としてもよい。また、上記実施形態に限られず、例えば、電流偏差の差がしきい値以上である場合に、電流指令値変更部１２がオペレータにその旨を警告ランプ、警告ブザー等の報知部（図示せず）より報知するよう構成されるとともに、第２電流指令値生成部９は、オペレータにより手動で制御ゲインを小さく設定変更可能に構成されてもよい。

さらに、電流指令値変更部１２は、マスタ速度偏差Δｖ_Ｍが所定値以下である場合、かつ、第１電流偏差ΔＩ_１および第２電流偏差ΔＩ_２のうちの少なくとも一方において対応するモータが正転と反転とを繰り返す場合に、第２電流指令値生成部９における速度ループ演算部２１のＰＩ補償における積分ゲインＫｖ_ＳＩを０にしてもよい。これによれば、スレーブモータ６側の速度ループにおけるフィードバック制御をＰＩ制御から比例制御に変更することにより、その場停止時においてマスタモータ４およびスレーブモータ６において不安定な動作（例えばマスタモータ４およびスレーブモータ６が互いに逆回転するように正転反転を繰り返したり、マスタモータ４のある方向の回転とスレーブモータ６の逆方向の回転とが交互に生じたり、マスタモータ４およびスレーブモータ６の何れか一方が正転反転を繰り返したりする等の押し引き動作）が生じるのを防止することができる。

例えば、トルクフィードフォワード制御に用いられるモデルと実際のモータとのダイナミクスの誤差が大きく、マスタモータ４の出力トルクＴ_Ｍおよびスレーブモータ６の出力トルクＴ_Ｓが非常に大きい場合、または、マスタモータ４側の伝達経路におけるガタおよびスレーブモータ６側の伝達経路におけるガタが何れも大きい場合、駆動軸２をその場停止させておきたいときでも（位置指令値Ｐ_ｏが０でも）、制御が不安定となり上記のような押し引き動作が発生するおそれがある。このような場合に、スレーブモータ６側の速度ループゲインのうちの積分ゲインＫｖ_ＳＩを０にし、スレーブモータ６側において、マスタ速度偏差Δｖ_Ｍに対して比例補償のみが行われる。このようにスレーブモータ６側において、積分補償をなくして、比例補償において生じる残留偏差に追従させないように制御することにより、スレーブモータ６において出力される出力トルクＴ_Ｓが目標値に対して振動することを抑制することができる。スレーブモータ６における出力値の振動が抑制されることにより、マスタモータ４における出力値の振動も抑制される。

なお、第２速度ループ演算部２１のＰＩ補償における比例ゲインＫｖ_ＳＰおよび積分ゲインＫｖ_ＳＩと第２電流補正部２２において設定されるゲインＫｇ_Ｓとの間で所定の重み付けが設定可能に構成されてもよい。

これにより、スレーブモータ６に対する第２電流補正部２２による理論値に基づく制御ゲインＫｇ_Ｓと、第２速度ループ演算部２１のＰＩ補償における制御ゲインＫｖ_Ｓとの重み付けを変更することにより、マスタモータ４およびスレーブモータ６の制御の高精度化と、マスタモータ４およびスレーブモータ６の制御の迅速化とのバランスを調整することができる。すなわち、第２電流補正部２２による制御ゲインＫｇ_Ｓに対する第２速度ループ演算部２１の制御ゲインＫｖ_Ｓの割合を大きくすると、指令値において実際の誤差に基づく割合が大きくなるため、マスタモータ４およびスレーブモータ６の制御の高精度化を図ることができる。また、第２電流補正部２２による制御ゲインＫｇ_Ｓに対する第２速度ループ演算部２１の制御ゲインＫｖ_Ｓの割合を小さくすると、指令値において理論値に基づく割合が大きくなるため、マスタモータ４およびスレーブモータ６の制御の迅速化を図ることができる。

＜第２実施形態＞
次に、本発明の第２実施形態について説明する。図３は本発明の第２実施形態に係るロボットの制御装置の構成を示すブロック図である。第２実施形態において第１実施形態と同様の構成については同じ符号を付し、説明を省略する。

図３に示すように、本実施形態におけるロボット制御装置１Ｂが第１実施形態におけるロボット制御装置１と異なる点は、第２電流指令値生成部９Ｂが、所定のトルク指令値に基づいて第２電流指令値Ｉ_２を生成することである。より具体的には、第２電流指令値生成部９Ｂは、ロボットの駆動軸２に作用する重力を考慮した動力学トルクに基づいてマスタ位置偏差ΔＰ_Ｍに依存しない第２電流指令値Ｉ_２を生成するよう構成される。すなわち、第２電流指令値生成部９Ｂは、第１電流指令値生成部８から独立した第２電流補正部２２Ｂを備えている。

これにより、スレーブモータ６は、マスタモータ４から独立した別系統の制御系となる。このため、スレーブモータ６の制御は、マスタ位置偏差ΔＰ_Ｍに無関係となり、安定した制御系とすることができる。

以上、本発明の実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、その趣旨を逸脱しない範囲内で種々の改良、変更、修正が可能である。

例えば、第１実施形態において説明した構成の一部を第２実施形態において適用することも可能である。

本発明のロボットの制御装置およびロボットの制御方法は、デュアルサーボ制御を行う２つのモータのうちの一方にトルクが集中するのを防ぐために有用である。

１，１Ｂ ロボット制御装置
２ 駆動軸
３ マスタ動力伝達機構
４ マスタモータ
５ スレーブ動力伝達機構
６ スレーブモータ
７ 位置検出器
８ 第１電流指令値生成部
９，９Ｂ 第２電流指令値生成部
１０ 第１電流偏差監視部
１１ 第２電流偏差監視部
１２ 電流指令値編後部
１３ マスタモータ駆動部
１４ スレーブモータ駆動部
１５ 第１速度ループ演算部
１６ 第１電流補正部
１７ 第１位置ループ演算部
１８ 微分器
１９，２３ ＰＷＭ演算部
２０，２４ 電流演算部
２１ 第２速度ループ演算部
２２，２２Ｂ 第２電流補正部

Claims (7)

1. ロボットの動作を規定する可動部分を駆動する駆動軸と、マスタ動力伝達機構を介して前記駆動軸を回転駆動するマスタモータと、スレーブ動力伝達機構を介して前記駆動軸を回転駆動するスレーブモータと、前記マスタモータの現在位置を検出する位置検出器と、を備えたロボットの制御装置であって、
   前記マスタモータへの位置指令値と前記位置検出器により検出される前記マスタモータの現在位置を示す値との偏差（以下、マスタ位置偏差）に基づいて前記マスタモータへの電流指令値（以下、第１電流指令値）を生成する第１電流指令値生成部と、
   前記マスタ位置偏差または所定のトルク指令値に基づいて前記スレーブモータへの電流指令値（以下、第２電流指令値）を生成する第２電流指令値生成部と、
   前記マスタモータが前記第１電流指令値に基づいて発生した出力トルクに応じた電流値と前記第１電流指令値との偏差（以下、第１電流偏差）を監視する第１電流偏差監視部と、
   前記スレーブモータが前記第２電流指令値に基づいて発生した出力トルクに応じた電流値と前記第２電流指令値との偏差（以下、第２電流偏差）を監視する第２電流偏差監視部と、
   前記第１電流偏差と前記第２電流偏差との差が予め定められたしきい値以上である場合に、前記第１電流偏差と前記第２電流偏差との差が小さくなるように前記第１電流指令値および／または前記第２電流指令値を変更する電流指令値変更部と、を備えた、ロボットの制御装置。
2. 前記第１電流指令値生成部は、
   前記マスタ位置偏差に基づいたマスタ速度指令値の時間的偏差（以下、マスタ速度偏差）に対してＰＩ補償を行うことにより電流指令値を生成する第１速度ループ演算部と、
   前記ロボットの駆動軸に作用する重力を考慮した動力学トルクに基づく電流補正値を生成し、前記第１速度ループ演算部で生成された前記電流指令値に前記電流補正値を加えて前記第１電流指令値を生成する第１電流補正部と、を有し、
   前記第２電流指令値生成部は、
   前記マスタ速度偏差に対してＰＩ補償を行うことにより電流指令値を生成する第２速度ループ演算部と、
   前記ロボットの駆動軸に作用する重力を考慮した動力学トルクに基づく電流補正値を生成し、前記第２速度ループ演算部で生成された前記電流指令値に前記電流補正値を加えて前記第２電流指令値を生成する第２電流補正部と、を有する、請求項１に記載のロボットの制御装置。
3. 前記電流指令値変更部は、前記第１電流偏差と前記第２電流偏差との差が予め定められたしきい値以上である場合に、前記第２速度ループ演算部のＰＩ補償における比例ゲインおよび積分ゲインのうちの少なくとも一方を前記第１速度ループ演算部のＰＩ補償において対応するゲインに対して低くする、請求項２に記載のロボットの制御装置。
4. 前記電流指令値変更部は、前記マスタ速度偏差が所定値以下である場合、かつ、前記第１電流偏差および前記第２電流偏差のうちの少なくとも一方において対応するモータが正転と反転とを繰り返す場合に、前記第２電流指令値生成部における前記速度ループ演算部のＰＩ補償における積分ゲインを０にする、請求項２または３に記載のロボットの制御装置。
5. 前記第２速度ループ演算部のＰＩ補償における比例ゲインおよび積分ゲインと前記第２電流補正部におけるゲインとの間で所定の重み付けが設定可能に構成されている、請求項２から４の何れかに記載のロボットの制御装置。
6. 前記第２電流指令値生成部は、前記ロボットの駆動軸に作用する重力を考慮した動力学トルクに基づいてマスタ位置偏差に依存しない前記第２電流指令値を生成するよう構成される、請求項１に記載のロボットの制御装置。
7. ロボットの動作を規定する可動部分を駆動する駆動軸と、マスタ動力伝達機構を介して前記駆動軸を回転駆動するマスタモータと、スレーブ動力伝達機構を介して前記駆動軸を回転駆動するスレーブモータと、前記マスタモータの現在位置を検出する位置検出器と、を備えたロボットの制御方法であって、
   前記マスタモータへの位置指令値と前記位置検出器により検出される前記マスタモータの現在位置を示す値との偏差（以下、マスタ位置偏差）に基づいて前記マスタモータへの電流指令値（以下、第１電流指令値）を生成し、
   前記マスタ位置偏差またはトルク指令値に基づいて前記スレーブモータへの電流指令値（以下、第２電流指令値）を生成し、
   前記マスタモータが前記第１電流指令値に基づいて発生した出力トルクに応じた電流値と前記第１電流指令値との偏差（以下、第１電流偏差）を監視し、
   前記スレーブモータが前記第２電流指令値に基づいて発生した出力トルクに応じた電流値と前記第２電流指令値との偏差（以下、第２電流偏差）を監視し、
   前記第１電流偏差と前記第２電流偏差との差が予め定められたしきい値以上である場合に、前記第１電流偏差と前記第２電流偏差との差が小さくなるように前記第１電流指令値および／または前記第２電流指令値を変更する、ロボットの制御方法。

Images