特許 6564175 ロボットの制御装置および制御方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6564175

(24)【登録日】2019年8月2日

(45)【発行日】2019年8月21日

(54)【発明の名称】ロボットの制御装置および制御方法

(51)【国際特許分類】

   B25J 13/00 20060101AFI20190808BHJP

   H02H 7/08 20060101ALI20190808BHJP

   G05B 11/36 20060101ALI20190808BHJP

【ＦＩ】

   B25J13/00 Z

   H02H7/08 C

   G05B11/36 F

   G05B11/36 507H

【請求項の数】5

【全頁数】14

(21)【出願番号】特願2014-198047(P2014-198047)

(22)【出願日】2014年9月29日

(65)【公開番号】特開2016-68171(P2016-68171A)

(43)【公開日】2016年5月9日

【審査請求日】2017年8月2日

(73)【特許権者】

【識別番号】000000974

【氏名又は名称】川崎重工業株式会社

(74)【代理人】

【識別番号】110000556

【氏名又は名称】特許業務法人 有古特許事務所

(72)【発明者】

【氏名】田中 英紀

(72)【発明者】

【氏名】下村 信恭

(72)【発明者】

【氏名】坪井 信高

【審査官】 藤井 浩介

(56)【参考文献】

【文献】 特開２００１−２７３０３７（ＪＰ，Ａ）

【文献】 特開２０１３−１５０４９３（ＪＰ，Ａ）

【文献】 特開２０００−０６９７８２（ＪＰ，Ａ）

【文献】 特開２００４−０８６４３４（ＪＰ，Ａ）

【文献】 特開２０１３−２１９８８４（ＪＰ，Ａ）

【文献】 特開平０８−０８４４９２（ＪＰ，Ａ）

【文献】 特開２００９−１０６０３４（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００−２１／０２

Ｈ０２Ｐ ５／００−５／７５３

Ｇ０５Ｂ １／００−７／０４；１１／００−１３／０４；

１７／００−１７／０２；２１／００−２１／０２

(57)【特許請求の範囲】

【請求項1】

ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸の一端部において動力伝達可能に接続されるマスタ動力伝達機構を介して前記駆動軸を回転駆動するマスタモータと、前記駆動軸の一端部において動力伝達可能に接続されるスレーブ動力伝達機構を介して前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御装置であって、
前記マスタモータの回転位置であるマスタ位置を取得するマスタ位置取得部と、
位置指令値と前記マスタ位置とに基づいて原電流指令値を生成する原電流指令値生成部と、
前記マスタモータの速度であるマスタ速度を取得するマスタ速度取得部と、
１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得するスレーブ速度取得部と、
前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための１以上の補償要素と、を備え、
前記原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償要素の出力をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動するように制御する、ロボットの制御装置。

【請求項2】

前記補償要素は、比例要素を含む、請求項１に記載のロボットの制御装置。

【請求項3】

前記比例要素の比例ゲインは、前記マスタ速度と前記スレーブ速度との偏差が単調に０に近づくような値に設定される、請求項２に記載のロボットの制御装置。

【請求項4】

前記マスタ速度取得部は、前記マスタ位置取得部と、前記マスタ位置取得部で取得された前記マスタ位置を微分して前記マスタ速度を出力する微分器と、を備え、
前記スレーブ速度取得部は、前記スレーブモータの回転位置であるスレーブ位置を取得するスレーブ位置取得部と、前記スレーブ位置を微分して前記スレーブ速度を出力する微分器と、を備えた、請求項１から３の何れかに記載のロボットの制御装置。

【請求項5】

ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸の一端部において動力伝達可能に接続されるマスタ動力伝達機構を介して前記駆動軸を回転駆動するマスタモータと、前記駆動軸の一端部において動力伝達可能に接続されるスレーブ動力伝達機構を介して前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御方法であって、
前記マスタモータの回転位置であるマスタ位置を取得し、
位置指令値と前記マスタ位置とに基づいて原電流指令値を生成し、
前記マスタモータの速度であるマスタ速度を取得し、
１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得し、
前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための補償値を算出し、
前記原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償値をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動する、ロボットの制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【0003】

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

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２００３−７９１８０号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

しかし、このような構成であっても、同じ性能のモータを同じように制御しても一方のモータに対して他方のモータの位置の偏差が生じる問題があった。このような問題は、３つ以上のモータを互いに連結し、１つの駆動軸を駆動するように３つ以上のモータを制御するサーバ制御構造でも同様に発生する。

【0006】

本発明は、以上のような課題を解決すべくなされたものであり、１つの駆動軸を少なくとも２つのモータで駆動するようにするサーボ制御構造においてモータ間の位置偏差を小さくすることができるロボットの制御装置およびロボットの制御方法を提供することを目的とする。

【課題を解決するための手段】

【0007】

本発明の一態様に係るロボットの制御装置は、ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸を回転駆動するマスタモータと、前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御装置であって、前記マスタモータの回転位置であるマスタ位置を取得するマスタ位置取得部と、位置指令値と前記マスタ位置とに基づいて原電流指令値を生成する原電流指令値生成部と、前記マスタモータの速度であるマスタ速度を取得するマスタ速度取得部と、１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得するスレーブ速度取得部と、前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための１以上の補償要素と、を備え、前記原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償要素の出力をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動するように制御するものである。

【発明の効果】

【0008】

一態様によれば、１つの駆動軸を少なくとも２つのモータで駆動するようにするサーボ制御構造においてモータ間の位置偏差を小さくすることができるという効果を奏する。

【図面の簡単な説明】

【0009】

【図1】図１は本発明の第１の実施の形態に係るロボットの制御装置の概略構成を示すブロック図である。

【図2】図２は本発明の第１の実施の形態の変形例に係るロボットの制御装置の概略構成を示すブロック図である。

【図3】図３は本発明の第２の実施の形態に係るロボットの制御装置の概略構成を示すブロック図である。

【図4】図４は本実施例におけるマスタモータおよびスレーブモータのトルクおよび速度の時間的変化を示すグラフである。

【図5】図５は図４に示す速度の時間的変化を示すグラフにおけるＶ部拡大図である。

【図6】本実施例における補償後スレーブ電流指令値の時間的変化を比較例とともに示すグラフである。

【図7】図７は比較例におけるマスタモータおよびスレーブモータのトルクおよび速度の時間的変化を示すグラフである。

【図8】図８は図７に示す速度の時間的変化を示すグラフにおけるＶＩＩＩ部拡大図である。

【発明を実施するための形態】

【0010】

＜本発明の一態様を得るに至った経緯＞
上述したように、従来のデュアルサーボ制御構造においては、同じ性能のモータを同じように制御しても一方のモータに対して他方のモータの位置の偏差が生じる問題があった。

【0011】

そこで、本発明の発明者らは上記課題を解決するため、鋭意研究を行った。この結果、本発明の発明者らは、デュアルサーボ制御を行った２つのモータのそれぞれの速度を解析することによって、一方のモータ（例えばマスタモータ）の速度変化に対して、他方のモータ（例えばスレーブモータ）の速度変化が追従できず、振動が発生したりして両者のモータ間の速度差が広がるという知見を得た。そこで、本発明の発明者らは、２つのモータの変位差は、両者の速度差に比例すると仮定し、両者に共通の電流指令値を用いつつ、スレーブモータの電流指令値に両者の速度差に基づいた補償値を加えるという制御態様を想到するに至った。これにより、マスタモータとスレーブモータとの間の速度変化を抑え、モータ間の位置偏差を小さくすることができる。

【0012】

＜本発明の一態様の概要＞
本発明は、以上のような発明者の知見に基づいて想到されたものである。本発明の一態様に係るロボットの制御装置は、ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸を回転駆動するマスタモータと、前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御装置であって、前記マスタモータの回転位置であるマスタ位置を取得するマスタ位置取得部と、位置指令値と前記マスタ位置とに基づいて原電流指令値を生成する原電流指令値生成部と、前記マスタモータの速度であるマスタ速度を取得するマスタ速度取得部と、１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得するスレーブ速度取得部と、前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための１以上の補償要素と、を備え、前記原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償要素の出力をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動するように制御するものである。

【0013】

上記構成によれば、位置指令値からマスタモータおよびスレーブモータへのそれぞれに共通する原電流指令値が生成される。マスタモータへは、原電流指令値から分配されたマスタ電流指令値が入力され、スレーブモータへは、原電流指令値から分配されたスレーブ電流指令値に、マスタ速度とスレーブ速度との偏差に基づいたスレーブ速度の補償分が加えられた電流指令値が入力される。このようにして、両者に共通の電流指令値を用いつつ、スレーブモータの電流指令値に両者の速度差に基づいた補償値を加えている。これにより、マスタモータとスレーブモータとの間の速度変化を抑え、モータ間の位置偏差を小さくすることができる。

【0014】

前記補償要素は、比例要素を含んでもよい。これにより、マスタモータとスレーブモータとの間の速度偏差に比例した補償が行われるため、マスタモータとスレーブモータとの間の速度変化を抑えることができる。さらに、前記比例要素のゲインは、前記マスタ速度と前記スレーブ速度との偏差が単調に０に近づくような値に設定されてもよい。このように、両者間の速度偏差が単調に０に近づくような値にゲインが設定されることにより、より効率的に両者間の速度変化を抑えることができる。

【0015】

前記マスタ速度取得部は、前記マスタ位置取得部と、前記マスタ位置を微分して前記マスタ速度を出力する微分器と、を備え、前記スレーブ速度取得部は、前記スレーブモータの回転位置であるスレーブ位置を取得するスレーブ位置取得部と、前記スレーブ位置を微分して前記スレーブ速度を出力する微分器と、を備えてもよい。これにより、既存の構成で容易に本発明の構成を実現することができる。

【0016】

本発明の他の態様に係るロボットの制御方法は、ロボットの動作を規定する可動部分を駆動する駆動軸と、前記駆動軸を回転駆動するマスタモータと、前記駆動軸を回転駆動する１以上のスレーブモータと、を備えたロボットの制御方法であって、前記マスタモータの回転位置であるマスタ位置を取得し、位置指令値と前記マスタ位置とに基づいて生成された原電流指令値を分配してマスタ電流指令値と１以上のスレーブ電流指令値とを生成し、前記マスタモータの速度であるマスタ速度を取得し、１以上の前記スレーブモータの速度である１以上のスレーブ速度をそれぞれ取得し、前記マスタ速度に対する１以上の前記スレーブ速度の偏差に基づいてそれぞれの偏差に対応する前記スレーブ速度を補償するための補償値を算出し、前記マスタ電流指令値に基づいて前記マスタモータを駆動し、前記１以上のスレーブモータにそれぞれ対応する１以上の前記スレーブ電流指令値に対しそれぞれに対応する前記補償値をそれぞれ加えた電流指令値に基づいて１以上の前記スレーブモータを駆動するものである。

【0017】

上記方法によれば、位置指令値からマスタモータおよびスレーブモータへのそれぞれに共通する原電流指令値が生成される。マスタモータへは、現電流指令値から分配されたマスタ電流指令値が入力され、スレーブモータへは、原電流指令値から分配されたスレーブ電流指令値に、マスタ速度とスレーブ速度との偏差に基づいてスレーブ速度を補償した電流指令値が入力される。このようにして、両者に共通の電流指令値を用いつつ、スレーブモータの電流指令値に両者の速度差に基づいた補償値を加えている。これにより、マスタモータとスレーブモータとの間の速度変化を抑え、モータ間の位置偏差を小さくすることができる。

【0018】

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

【0019】

＜第１の実施の形態＞
まず、本発明の第１の実施の形態について説明する。図１は本発明の第１の実施の形態に係るロボットの制御装置の概略構成を示すブロック図である。図１に示すように、本実施の形態におけるロボットの制御装置１は、ロボットの動作を規定する可動部分を駆動する駆動軸２と、マスタ動力伝達機構３を介して駆動軸２を回転駆動するマスタモータ４と、スレーブ動力伝達機構５を介して駆動軸２を回転駆動するスレーブモータ６と、を備えている。さらに、制御装置１は、マスタ電流指令値Ｉ_Ｍ（後述）に基づいてマスタモータ４を駆動するマスタモータ駆動部１４と、補償後スレーブ電流指令値Ｉ_ＣＳ（後述）に基づいてスレーブモータ６を駆動するスレーブモータ駆動部１５と、を備えている。

【0020】

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

【0021】

マスタ動力伝達機構３およびスレーブ動力伝達機構５は、それぞれ駆動軸２に対する所定の減速比を有している。本実施の形態においては、マスタ動力伝達機構３の減速比とスレーブ動力伝達機構５の減速比とは等しい値に設定されている。

【0022】

さらに、制御装置１は、マスタモータ４の速度（現在速度）を示す値（マスタ速度ω_Ｍ）を取得するマスタ速度取得部７と、スレーブモータ６の速度（現在速度）を示す値（スレーブ速度ω_Ｓ）を取得するスレーブ速度取得部８と、を備えている。本実施の形態において、マスタ速度取得部７は、マスタモータ４の回転位置（現在位置）を示す値（マスタ位置Ｐ_Ｍ）を取得するマスタ位置取得部９と、マスタ位置Ｐ_Ｍを微分してマスタ速度ω_Ｍを出力する微分器１０と、を備えている。同様に、スレーブ速度取得部８は、スレーブモータ６の回転位置を示す値（スレーブ位置Ｐ_Ｓ）を取得するスレーブ位置取得部１１と、スレーブ位置Ｐ_Ｓを微分してスレーブ速度ω_Ｓを出力する微分器１２と、を備えている。位置取得部９，１１が取得する回転位置は、各モータ４，６の出力軸（モータ軸）の回転角度位置（基準位置からの回転数および回転角度）を意味する。例えば、位置取得部９，１１は、エンコーダまたはレゾルバ等により構成される。

【0023】

さらに、制御装置１は、位置指令値Ｐ_ｏとマスタ位置Ｐ_Ｍとに基づいて原電流指令値Ｉｏを生成する原電流指令値生成部１３を備えている。原電流指令値生成部１３は、ＣＰＵ、メモリおよび各種信号の入出力装置等を備えたコンピュータの機能ブロックとして構成される。位置指令値Ｐ_ｏは、マスタモータ４のモータ軸の回転位置（基準位置からの回転数および回転角度）を示す値として外部から入力される。

【0024】

原電流指令値生成部１３で生成された原電流指令値Ｉ_ｏは、マスタモータ４への経路およびスレーブモータ６への経路に分岐することにより均等に分配されてマスタ電流指令値Ｉ_Ｍおよびスレーブ電流指令値Ｉ_Ｓとなる。マスタ電流指令値Ｉ_Ｍは、マスタモータ駆動部１４に入力される。マスタモータ駆動部１４は、入力されたマスタ電流指令値Ｉ_Ｍに基づいた出力トルクＴ_Ｍを発生させるようにマスタモータ４を駆動し、マスタモータ４は、回転動力を出力する。

【0025】

さらに、制御装置１は、マスタ速度ω_Ｍに対するスレーブ速度ω_Ｓの偏差Δωに基づいてスレーブ速度ω_Ｓを補償する補償要素１６を備えている。補償要素１６も、原電流指令値生成部１３と同様にコンピュータの機能ブロックとして構成される。本実施の形態において、補償要素１６は、比例要素（ゲインＧ）を含むように構成される。さらに、補償要素１６は、速度偏差Δωを電流指令値に加えるための換算値を含むように構成される。

【0026】

なお、補償要素１６は、ゲインＧとして比例要素、積分要素、および微分要素の少なくとも何れか１つを有する。これらの要素を複数組み合わせることとしてもよい。すなわち、比例補償だけでなく、ＰＩ補償やＰＩＤ補償を行うこととしてもよい。

【0027】

補償要素１６から出力された補償値ＧΔωは、スレーブ電流指令値Ｉ_Ｓに加えられ、補償後スレーブ電流指令値Ｉ_ＣＳとしてスレーブモータ駆動部１５に入力される。スレーブモータ駆動部１５は、入力された補償後スレーブ電流指令値Ｉ_ＣＳに基づいた出力トルクＴ_Ｓを発生させるようにスレーブモータ６を駆動し、スレーブモータ６は、回転動力を出力する。

【0028】

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

【0029】

本実施の形態におけるロボットの制御装置１によれば、位置指令値Ｐ_ｏからマスタモータ４およびスレーブモータ６へのそれぞれに共通する原電流指令値Ｉ_ｏが生成される。マスタモータ４へは、原電流指令値Ｉ_ｏから分配されたマスタ電流指令値Ｉ_Ｍが入力され、スレーブモータ６へは、マスタ速度ω_Ｍとスレーブ速度ω_Ｓとの偏差Δωに基づいたスレーブ速度ω_Ｓの補償分が加えられた電流指令値（補償後スレーブ電流指令値）Ｉ_ＣＳが入力される。このようにして、両者に共通の原電流指令値Ｉ_ｏを用いつつ、スレーブモータ６への電流指令値Ｉ_ＣＳとして両者の速度差Δωに基づいた補償値ＧΔωを加えている。これにより、マスタモータ４とスレーブモータ６との間の速度変化を抑え、モータ４，６間の位置偏差を小さくすることができる。

【0030】

また、補償要素１６が、比例要素（ゲインＧ）を含むことにより、マスタモータ４とスレーブモータ６との間の速度偏差Δωに比例した補償が行われるため、２つのモータに共通の原電流指令値Ｉ_ｏを用いつつ、スレーブモータ６の電流指令値Ｉ_ＣＳに両者の速度差Δωに基づいた補償値を加えるという制御態様を容易かつシンプルに実現することができる。さらに、ゲインＧは、速度偏差Δωが単調に０に近づくような値に設定されることが好ましい。このように、両者間の速度偏差Δωが単調に０に近づくような値にゲインＧが設定されることにより、より効率的に両者間の速度変化を抑えることができる。

【0031】

本実施の形態において、第１電流指令値生成部１３は、位置指令値Ｐ_ｏとマスタ位置Ｐ_Ｍとの位置偏差（時間的偏差）ΔＰに基づいて速度指令値ω_Ｏを算出する速度指令値算出部１７と、速度指令値ω_ｏとマスタ速度ω_Ｍとの速度偏差（時間的偏差）Δωから第１電流指令値Ｉ_１を算出する電流指令値算出部１８と、を備えている。より具体的には、速度指令値算出部１７は、位置ループゲインＫ_ＰＭを用いて位置偏差ΔＰに対してＰＩ補償を行うことにより速度指令値ω_ｏを生成するよう構成される。また、電流指令値算出部１８は、速度ループゲインＫ_ωＭを用いて速度偏差Δωに対してＰＩ補償を行い、第１電流指令値Ｉ_１を出力する。なお、位置ループゲインＫ_ＰＭおよび速度ループゲインＫ_ωＭは、それぞれ比例ゲイン（Ｋ_ＰＰ，Ｋ_ωＰ）および積分ゲイン（Ｋ_ＰＩ，Ｋ_ωＩ）を含み、微分ゲインと積分ゲインとの値は個別に設定される。

【0032】

このように、原電流指令値Ｉ_ｏに対してマスタモータ４の位置Ｐ_Ｍおよび速度ω_Ｍに基づいてフィードバック制御を行うことにより、マスタモータ４およびスレーブモータ６への共通の原電流指令値Ｉ_ｏに対して有効なフィードバック制御を行うことができる。

【0033】

また、マスタ位置取得部９で検出されたマスタ位置Ｐ_Ｍとこれを微分器１０で微分して得られたマスタ速度ω_Ｍとを用いて位置フィードバック制御および速度フィードバック制御を行うため、速度検出器を別途設けることなく２つのフィードバック制御を容易に実現することができる。さらに、このような位置フィードバック制御および速度フィードバック制御を行う構成に予め設けられたマスタ速度取得部７を用いてスレーブモータ６への電流指令値Ｉ_ＣＳが生成されるため、既存の構成で容易に本発明の構成（すなわち、２つのモータに共通の原電流指令値Ｉ_ｏを用いつつ、スレーブモータ６の電流指令値Ｉ_ＣＳに両者の速度差Δωに基づいた補償値を加えるという制御態様）を実現することができる。

【0034】

なお、本実施の形態においては、マスタ動力伝達機構３の減速比とスレーブ動力伝達機構５の減速比とが同じ値であるため、電流指令値算出部１８は、速度ループゲインＫ_ωＭを用いて速度偏差Δωに対してＰＩ補償を行った後の値に各モータへの分配のための０．５のゲインを掛けた値を原電流指令値Ｉ_ｏとして出力する。したがって、本実施の形態においては、実質的に原電流指令値Ｉｏとマスタ電流指令値ＩＭおよびスレーブ電流指令値ＩＳとは同じ値となっている。

【0035】

また、マスタモータ駆動部１４に入力される電流指令値として、マスタ電流指令値Ｉ_Ｍにロボットの駆動軸２に作用する重力を考慮した動力学トルクに基づく電流補正値を加えてもよい。同様に、スレーブモータ駆動部１５に入力される電流指令値として、補償後スレーブ電流指令値Ｉ_ＣＳにロボットの駆動軸２に作用する重力を考慮した動力学トルクに基づく電流補正値を加えてもよい。

【0036】

これらの電流補正値の基となる重力を考慮した動力学トルクは、例えば重力トルク、加減速トルク、摩擦トルク、他の軸の動きを支えるトルク（干渉トルク）等を含む。重力を考慮した動力学トルクは、ロボット動作における理論値に基づく値として規定され、予め設定される。このような動力学トルクに基づく電流補正値を加えるフィードフォワード制御を行うことにより、マスタモータ４とスレーブモータ６との位置偏差および速度偏差を予め小さくすることができ、より迅速な制御を行うことができる。

【0037】

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

【0038】

本変形例のロボットの制御装置１Ｂが第１の実施の形態におけるロボットの制御装置１と異なる点は、マスタ動力伝達機構３Ｂの減速比αとスレーブ動力伝達機構５Ｂの減速比βとが異なることである。これに応じて、制御装置１Ｂは、減速比の割合に応じたゲインを各モータ４，６を駆動するための第１電流指令値Ｉ_１にそれぞれ掛けるための分配要素１９，２０を備えている。さらに、制御装置１Ｂは、マスタ速度ω_Ｍとスレーブ速度ω_Ｓとの速度偏差Δωを算出するために、減速比の影響をなくすゲインを掛けるための速度調整要素２１を備えている。

【0039】

分配要素１９は、原電流指令値生成部１３（電流指令値算出部１８）とマスタモータ駆動部１４との間に設けられる。分配要素２０は、原電流指令値生成部１３（電流指令値算出部１８）とスレーブモータ駆動部１５との間であって、補償要素１６から出力される補償値ＧΔωが加えられる加算器より前段に設けられる。

【0040】

マスタ動力伝達機構３Ｂとスレーブ動力伝達機構５Ｂとの減速比の割合がα：βであるため、マスタモータ４へ原電流指令値Ｉ_ｏを分配する分配要素１９の比例ゲインは、α／（α＋β）であり、スレーブモータ６へ原電流指令値Ｉ_ｏを分配する分配要素２０の比例ゲインは、β／（α＋β）である。したがって、本変形例においては、マスタ電流指令値Ｉ_Ｍは、原電流指令値Ｉ_ｏに分配要素１９の比例ゲインα／（α＋β）を掛けたものであり、スレーブ電流指令値Ｉ_Ｓは、原電流指令値Ｉ_ｏに分配要素２０の比例ゲインβ／（α＋β）を掛けたものである。

【0041】

また、マスタ動力伝達機構３Ｂの減速比αとスレーブ動力伝達機構５Ｂの減速比βとが異なるため、マスタ速度ω_Ｍとスレーブ速度ω_Ｓとは、目標とする速度がそもそも異なる。そこで、両者の速度ω_Ｍ，ω_Ｓを同じ目標速度に換算して比較するために速度取得部７，８の出力の少なくとも何れか一方に速度を調整するためのゲインを掛ける。本実施の形態において、制御装置１Ｂは、マスタ速度ω_Ｍをスレーブ速度ω_Ｓの速度に換算するために、マスタ速度ω_Ｍを速度調整要素２１に入力し、速度調整要素２１の出力とスレーブ速度ω_Ｓとの差を速度偏差Δωとして算出するように構成される。すなわち、微分器１０の出力端と速度偏差Δωを算出する減算器の入力端との間に速度調整要素２１が設けられる。この場合、速度調整要素の比例ゲインは、β／αである。

【0042】

なお、速度偏差Δωを算出するための速度調整要素２１は、スレーブ速度ω_Ｓをマスタ速度ω_Ｍの目標速度に対応する速度に換算するようにしてもよいし、マスタ速度ω_Ｍおよびスレーブ速度ω_Ｓをその他の所定の目標速度に対応する速度（例えば駆動軸２の目標速度に対応する速度）に換算するようにしてもよい。これらの場合には、補償要素１６において、再度スレーブモータ６の本来の目標速度に対応する速度に換算するためのゲインを掛けることが好ましい。

【0043】

また、本変形例は、マスタ動力伝達機構３Ｂの減速比αとスレーブ動力伝達機構５Ｂの減速比βとが同じ場合（α＝β）でも適用され得る。この場合、分配要素１９，２０の比例ゲインは、何れも０．５となり、速度調整要素の比例ゲインは１となる。

【0044】

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

【0045】

図３に示すように、本実施の形態におけるロボットの制御装置１Ｃが第１の実施の形態におけるロボットの制御装置１と異なる点は、１つのマスタモータ４と複数（図３の例では２つ）のスレーブモータ６ａ，６ｂとを備えることである。スレーブモータ６ａ，６ｂが複数となることにより、スレーブモータ６ａ，６ｂに関連する構成５，８，１１，１２，１５，１６についても複数（２つずつ）設けられている。図３ではこれらの各構成の相違を各符号にａまたはｂを付すことにより区別して示している。以下では場合によって２つのスレーブモータ６ａ，６ｂを第１スレーブモータ６ａおよび第２スレーブモータ６ｂと呼称する。

【0046】

本実施の形態においても、第１の実施の形態と同様に、マスタ速度ω_Ｍと第１スレーブモータ６ａの速度ω_Ｓａとの速度偏差Δω_ａに補償要素１６ａの比例ゲインＧ_ａを掛けた補償値Ｇ_ａΔω_ａをスレーブ電流指令値Ｉ_Ｓａに加えた補償後スレーブ電流指令値Ｉ_ＣＳａが第１スレーブモータ６ａを駆動するためのスレーブモータ駆動部１５ａに入力される。また、マスタ速度ω_Ｍと第２スレーブモータ６ｂの速度ω_Ｓｂとの速度偏差Δω_ｂに補償要素１６ｂの比例ゲインＧ_ｂを掛けた補償値Ｇ_ｂΔω_ｂをスレーブ電流指令値Ｉ_Ｓｂに加えた補償後スレーブ電流指令値Ｉ_ＣＳｂが第２スレーブモータ６ｂを駆動するためのスレーブモータ駆動部１５ｂに入力される。スレーブモータ６の数が３つ以上の場合でも同様に、マスタモータ４のマスタ速度ω_Ｍと各スレーブモータ６のスレーブ速度ω_Ｓとの速度偏差Δωに基づいて各スレーブモータ６への電流指令値を補償することができる。

【0047】

本実施の形態において、マスタ動力伝達機構３およびスレーブ動力伝達機構５ａ，５ｂの減速比が同じ場合、電流指令値算出部１８は、速度ループゲインＫ_ωＭを用いて速度偏差Δωに対してＰＩ補償を行った後の原電流指令値Ｉ_ｏに１／３のゲインを掛けた値をマスタ電流指令値Ｉ_Ｍおよびスレーブ電流指令値Ｉ_Ｓａ，Ｉ_Ｓｂとして出力する。これにより、共通の原電流指令値Ｉ_ｏをマスタモータ４とスレーブモータ６ａ，６ｂとに均等に分配している。

【0048】

なお、本実施の形態においても第１の実施の形態の変形例と同様に、マスタ動力伝達機構３の減速比とスレーブ動力伝達機構５ａの減速比との割合が異なる場合、および／またはマスタ動力伝達機構３の減速比とスレーブ動力伝達機構５ｂの減速比との割合が異なる場合には、第１の実施の形態の分配要素１９，２０および速度調整要素２１に対応する箇所に各要素を設けることにより、本実施の形態の構成を実現することができる。

【0049】

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

【0050】

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

【0051】

また、上記実施の形態のマスタ速度取得部７およびスレーブ速度取得部８は、マスタモータ４およびスレーブモータ６の位置を検出し、速度に変換するように構成されているが、例えばエンコーダ等の速度検出器を用いてマスタモータ４およびスレーブモータ６の速度を直接取得することとしてもよい。

【0052】

＜実施例＞
以下に、上記第１の実施の形態の構成に基づいてマスタモータ４およびスレーブモータ６間のトルク差、速度差および電流指令値差をシミュレーションした結果を示す。比較例としてスレーブモータ６の電流指令値として補償要素１６の出力ＧΔωを加えない（スレーブ電流指令値Ｉ_Ｓをそのままスレーブモータ駆動部１５に入力する）態様におけるマスタモータ４およびスレーブモータ６間のトルク差、速度差および電流指令値差もシミュレーションした。

【0053】

図４は本実施例におけるマスタモータおよびスレーブモータのトルクおよび速度の時間的変化を示すグラフである。図４の上側のグラフがトルクの時間的変化を示すグラフであり、下側のグラフが速度の時間的変化を示すグラフである。何れのグラフもマスタモータとスレーブモータとを重ねて示している。図５は図４に示す速度の時間的変化を示すグラフにおけるＶ部拡大図である。また、図７は比較例におけるマスタモータおよびスレーブモータのトルクおよび速度の時間的変化を示すグラフである。図８は図７に示す速度の時間的変化を示すグラフにおけるＶＩＩＩ部拡大図である。図７は実施例における図４に対応し、図８は実施例における図５に対応するグラフである。図５および図８の拡大図においては、マスタモータの速度波形とスレーブモータの速度波形が分かり易いように、概略的な波形のみをトレースするように加工している。

【0054】

まず、図４と図７とのそれぞれのトルクの時間的変化を示すグラフ（上側のグラフ）を比較する。速度偏差Δωに基づく補償を行っていない比較例（図７）においては、特に、発生するトルクの向きが変わるＡｃ部においてノイズのような振動現象が大きく出ている。これは、トルク変動が大きいときに、一方のモータ（マスタモータ）に対して他方のモータ（スレーブモータ）がうまく追従できておらず、一方のモータのトルク値に対して他方のモータのトルク値が上回ったり下回ったりすることによるものと考えられる。

【0055】

これに対し、速度偏差Δωに基づく補償を行った本実施例（図４）においては、発生するトルクの向きが変わるＡｅ部においてもノイズのような振動現象は比較的抑えられ、全体的に滑らかな波形となっている。このことから、本実施例においては、マスタモータ４およびスレーブモータ６でそれぞれ発生するトルク値に差が比較的生じず、マスタモータ４のトルク変動に対してスレーブモータ６のトルク変動がよく追従していると考えられる。

【0056】

次に、図４と図７とのそれぞれの速度の時間的変化を示すグラフ（下側のグラフ）を比較する。比較例（図７）においては、特に、速度が連続的に変化する領域（例えばＶＩＩＩ部）において、図８に示すように、マスタモータの速度が比較的滑らかに変化するのに対し、スレーブモータの速度はマスタモータの速度にうまく追従できておらず、マスタモータの速度に対してスレーブモータの速度が近づいたり離れたりする結果となった。

【0057】

これに対し、本実施例（図４）においては、同じ領域（Ｖ部）において、図５に示すように、マスタモータ４の速度およびスレーブモータ６の速度が何れも比較的滑らかに変化する結果が得られた。このように、本実施例によれば、マスタモータ４の速度とスレーブモータ６の速度とが離れることなくよく追従できていることが示された。

【0058】

図６は本実施例における補償後スレーブ電流指令値Ｉ_ＣＳの時間的変化を比較例とともに示すグラフである。図６に示すように、速度偏差Δωに基づく補償を行っていない比較例においては電流指令値の振動幅が比較的大きいのに対し、本実施例では電流指令値Ｉ_ＣＳの振動的変化が比較的抑えられ、滑らかに変化する結果が得られた。このように、本実施例によればスレーブモータ６への電流指令値Ｉ_ＣＳ自体の振動的変化が抑えられ、比較的滑らかなトルク変動制御が行われることが示された。

【産業上の利用可能性】

【0059】

本発明のロボットの制御装置およびロボットの制御方法は、１つの駆動軸を少なくとも２つのモータで駆動するようにするサーボ制御構造においてモータ間の位置偏差を小さくするために有用である。

【符号の説明】

【0060】

１，１Ｂ ロボットの制御装置
２ 駆動軸
３ マスタ動力伝達機構
４ マスタモータ
５ スレーブ動力伝達機構
６ スレーブモータ
７ マスタ速度取得部
８ スレーブ速度取得部
９ マスタ位置取得部
１０，１２ 微分器
１１ スレーブ位置取得部
１３ 原電流指令値生成部
１６ 補償要素
１７ 速度指令値算出部
１８ 電流指令値算出部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】