特許 7117827 モータ制御システム、モータ制御システムの制御方法、及びロボットシステム

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

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

(11)【特許番号】

(24)【登録日】2022-08-04

(45)【発行日】2022-08-15

(54)【発明の名称】モータ制御システム、モータ制御システムの制御方法、及びロボットシステム

(51)【国際特許分類】

   H02P 29/40 20160101AFI20220805BHJP

   B25J 13/00 20060101ALI20220805BHJP

   G05D 3/12 20060101ALI20220805BHJP

【ＦＩ】

H02P29/40

B25J13/00 Z

G05D3/12 305Z

【請求項の数】 10

(21)【出願番号】P 2017099192

(22)【出願日】2017-05-18

(65)【公開番号】P2018196266

(43)【公開日】2018-12-06

【審査請求日】2020-05-12

(73)【特許権者】

【識別番号】000000974

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

(74)【代理人】

【識別番号】110000556

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

(72)【発明者】

【氏名】坪井 信高

【審査官】柏崎 翔

(56)【参考文献】

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

【文献】特開２０１５－６７０５（ＪＰ，Ａ）

【文献】国際公開第２００７／０９９６３５（ＷＯ，Ａ１）

【文献】特開昭６３－１５３６１０（ＪＰ，Ａ）

【文献】特開２０１１－２０４０１０（ＪＰ，Ａ）

【文献】特開２００３－２２３２２５（ＪＰ，Ａ）

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

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

Ｈ０２Ｐ ２９／４０

Ｂ２５Ｊ １３／００

Ｇ０５Ｄ ３／１２

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

【請求項1】

上位装置から入力された位置指令に基づきモータに供給される電流を制御して前記モータを制御し、且つ入力軸が前記モータの出力軸に接続され且つ出力軸が負荷に接続された減速機の動作を制御するモータ制御システムであって、
前記モータの出力軸の回転速度を検出するための事象を検知する検知部と、
前記位置指令に基づき速度指令を生成し、且つ前記速度指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度との偏差である速度偏差を算出する速度偏差生成部と、
前記減速機の角度伝達誤差を補償するための補償量であって、前記位置指令に基づき前記速度指令、前記速度偏差、又は前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度である対象に加えるべき補償量を推定し、推定した前記補償量を前記対象に加える角度伝達誤差補償部と、
前記速度偏差に基づいて電流指令を生成する電流指令生成部と、
前記電流指令に基づいて前記モータに供給される電流を制御する電流制御部と、を含む、モータ制御システム。

【請求項2】

前記速度指令は、前記位置指令を時間微分した速度フィードフォワード指令である、請求項１に記載のモータ制御システム。

【請求項3】

前記角度伝達誤差補償部は、推定した前記補償量に基づいて前記速度フィードフォワード指令を補正することによって前記速度指令を補正する、請求項２に記載のモータ制御システム。

【請求項4】

前記検知部は、前記モータの出力軸の回転角及び回転速度を検出するための事象を検知し、
前記速度指令は、前記位置指令を時間微分した速度フィードフォワード指令と、前記位置指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転角との偏差に位置比例ゲインを乗じたゲイン速度指令とを加算した値である、請求項１に記載のモータ制御システム。

【請求項5】

前記角度伝達誤差補償部は、推定した前記補償量に基づいて前記速度フィードフォワード指令を補正することによって前記速度指令を補正する、請求項４に記載のモータ制御システム。

【請求項6】

前記角度伝達誤差補償部は、更に推定した前記補償量に基づいて前記位置指令を補正し、
前記速度指令は、上位装置から入力された前記位置指令を時間微分した速度フィードフォワード指令と、補正した前記位置指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転角との偏差に位置比例ゲインを乗じたゲイン速度指令とを加算した値である、請求項５に記載のモータ制御システム。

【請求項7】

前記角度伝達誤差補償部は、前記角度伝達誤差の周期的な変動をモデル化した周期関数に基づいて前記補償量を推定する、請求項１乃至６の何れかに記載のモータ制御システム。

【請求項8】

前記減速機は、波動歯車装置である、請求項１乃至７の何れかに記載のモータ制御システム。

【請求項9】

上位装置から入力された位置指令に基づきモータに供給される電流を制御して前記モータを制御し、且つ入力軸が前記モータの出力軸に接続され且つ出力軸が負荷に接続された減速機の動作を制御するモータ制御システムの制御方法であって、
前記モータ制御システムは、
前記モータの出力軸の回転速度を検出するための事象を検知する検知部と、
前記位置指令に基づき速度指令を生成し、且つ前記速度指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度との偏差である速度偏差を算出する速度偏差生成部と、
前記減速機の角度伝達誤差を補償するための補償量であって、前記位置指令に基づき前記速度指令、前記速度偏差、又は前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度である対象に加えるべき補償量を推定し、推定した前記補償量を前記対象に加える角度伝達誤差補償部と、
前記速度偏差に基づいて電流指令を生成する電流指令生成部と、
前記電流指令に基づいて前記モータに供給される電流を制御する電流制御部と、を含む、モータ制御システムの制御方法。

【請求項10】

ロボットアームと、
前記ロボットアームの関節の駆動源であるモータと、
入力軸が前記モータの出力軸に接続され且つ出力軸が前記ロボットアームの前記関節に接続された減速機と、
位置指令を生成する指令部と、前記指令部が生成した前記位置指令に基づき前記モータに供給される電流を制御して前記モータの動作を制御するモータ制御システムと、を備え、
前記モータ制御システムは、
前記モータの出力軸の回転速度を検出するための事象を検知する検知部と、
前記位置指令に基づき速度指令を生成し、且つ前記速度指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度との偏差である速度偏差を算出する速度偏差生成部と、
前記減速機の角度伝達誤差を補償するための補償量であって、前記位置指令に基づき前記速度指令、前記速度偏差、又は前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度である対象に加えるべき補償量を推定し、推定した前記補償量を前記対象に加える角度伝達誤差補償部と、
前記速度偏差に基づいて電流指令を生成する電流指令生成部と、
前記電流指令に基づいて前記モータに供給される電流を制御する電流制御部と、を含む、ロボットシステム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、モータ制御システム、モータ制御システムの制御方法、及びロボットシステムに関する。

【背景技術】

【0002】

従来から減速機の角度伝達誤差に起因した位置決め誤差を補正可能な位置決めシステムが知られている（例えば特許文献１参照）。

【0003】

この位置決めシステムは、モータ及び減速機からなるアクチュエータの出力軸の位置決め誤差を補正するための誤差補正データに基づき位置指令に対して補正を施す誤差補正部を備え、補正後の位置指令をモータを駆動するドライバに供給する。これによって、アクチュエータの位置決め誤差を補正することができる。

【先行技術文献】

【特許文献】

【0004】

【文献】特開２００３－２２３２２５号公報

【発明の概要】

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

【0005】

しかし、特許文献１に記載の位置決めシステムは、アクチュエータの出力軸の回転速度の変動の抑制が適切に行えず、アクチュエータの出力軸に接続された負荷の挙動が不安定になる場合があった。

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

【0006】

上記課題を解決するため、本発明のある態様に係るモータ制御システムは、上位装置から入力された位置指令に基づきモータに供給される電流を制御して前記モータを制御し、且つ入力軸が前記モータの出力軸に接続され且つ出力軸が負荷に接続された減速機の動作を制御するモータ制御システムであって、前記モータの出力軸の回転速度を検出するための事象を検知する検知部と、前記位置指令に基づき速度指令を生成し、且つ前記速度指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度との偏差である速度偏差を算出する速度偏差生成部と、前記モータの出力軸の回転角と前記減速機の出力軸の回転角との間の角度伝達誤差を推定し、且つ推定した前記角度伝達誤差に基づいて前記速度指令、前記速度偏差、又は前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度を補正する角度伝達誤差補償部と、前記速度偏差に基づいて電流指令を生成する電流指令生成部と、前記電流指令に基づいて前記モータに供給される電流を制御する電流制御部と、を含む。

【0007】

この構成によれば、角度伝達誤差の補償を簡易な構成で行うことができ、角度伝達誤差に起因する負荷の不安定な挙動を抑制することができる。

【0008】

前記速度指令は、前記位置指令を時間微分した速度フィードフォワード指令であってもよい。

【0009】

この構成によれば、モータの出力軸の回転速度を制御するための速度フィードフォワード指令に沿ってモータの速度を適切に制御することができる。

【0010】

前記角度伝達誤差補償部は、推定した前記角度伝達誤差に基づいて前記速度フィードフォワード指令を補正することによって前記速度指令を補正してもよい。

【0011】

この構成によれば、モータの出力軸の回転速度を制御するための速度フィードフォワード指令を補正することによって速度指令を適切に補正することができ、角度伝達誤差の補償を適切に行うことができる。

【0012】

前記検知部は、前記モータの出力軸の回転角及び回転速度を検出するための事象を検知し、前記速度指令は、前記位置指令を時間微分した速度フィードフォワード指令と、前記位置指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転角との偏差に位置比例ゲインを乗じたゲイン速度指令とを加算した値であってもよい。

【0013】

この構成によれば、モータの出力軸の回転速度を制御するための速度フィードフォワード指令に沿ってモータの速度を適切に制御することができ、且つゲイン速度指令に沿ってモータの回転角を適切に制御することができる。

【0014】

前記角度伝達誤差補償部は、推定した前記角度伝達誤差に基づいて前記速度フィードフォワード指令を補正することによって前記速度指令を補正してもよい。

【0015】

この構成によれば、モータの出力軸の回転速度を制御するための速度フィードフォワード指令を補正することによって速度指令を適切に補正することができ、角度伝達誤差の補償を適切に行うことができる。

【0016】

前記角度伝達誤差補償部は、更に推定した前記角度伝達誤差に基づいて前記位置指令を補正し、前記速度指令は、上位装置から入力された前記位置指令を時間微分した速度フィードフォワード指令と、補正した前記位置指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転角との偏差に位置比例ゲインを乗じたゲイン速度指令とを加算した値であってもよい。

【0017】

この構成によれば、角度伝達誤差の補償をより適切に行うことができる。

【0018】

前記角度伝達誤差補償部は、前記角度伝達誤差の周期的な変動をモデル化した周期関数に基づいて前記角度伝達誤差を推定してもよい。

【0019】

この構成によれば、周期的な誤差として表れる角度伝達誤差を適切に補償することができる。

【0020】

前記減速機は、波動歯車装置であってもよい。

【0021】

この構成によれば、波動歯車装置の角度伝達誤差に起因する負荷の不安定な挙動を抑制することができる。

【0022】

上記課題を解決するため、本発明のある態様に係るモータ制御システムの制御方法は、上位装置から入力された位置指令に基づきモータに供給される電流を制御して前記モータを制御し、且つ入力軸が前記モータの出力軸に接続され且つ出力軸が負荷に接続された減速機の動作を制御するモータ制御システムの制御方法であって、前記モータ制御システムは、前記モータの出力軸の回転速度を検出するための事象を検知する検知部と、前記位置指令に基づき速度指令を生成し、且つ前記速度指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度との偏差である速度偏差を算出する速度偏差生成部と、前記モータの出力軸の回転角と前記減速機の出力軸の回転角との間の角度伝達誤差を推定し、且つ推定した前記角度伝達誤差に基づいて前記速度指令、前記速度偏差、又は前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度を補正する角度伝達誤差補償部と、前記速度偏差に基づいて電流指令を生成する電流指令生成部と、前記電流指令に基づいて前記モータに供給される電流を制御する電流制御部と、を含む。

【0023】

この構成によれば、角度伝達誤差の補償を簡易な構成で行うことができ、角度伝達誤差に起因する負荷の不安定な挙動を抑制することができる。

【0024】

上記課題を解決するため、本発明のある態様に係るロボットシステムは、ロボットアームと、前記ロボットアームの関節の駆動源であるモータと、入力軸が前記モータの出力軸に接続され且つ出力軸が前記ロボットアームの前記関節に接続された減速機と、位置指令を生成する指令部と、前記指令部が生成した前記位置指令に基づき前記モータに供給される電流を制御して前記モータの動作を制御するモータ制御システムと、を備え、前記モータ制御システムは、前記モータの出力軸の回転速度を検出するための事象を検知する検知部と、前記位置指令に基づき速度指令を生成し、且つ前記速度指令と前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度との偏差である速度偏差を算出する速度偏差生成部と、前記モータの出力軸の回転角と前記減速機の出力軸の回転角との間の角度伝達誤差を推定し、且つ推定した前記角度伝達誤差に基づいて前記速度指令、前記速度偏差、又は前記検知部が検知した事象に基づき検出された前記モータの出力軸の回転速度を補正する角度伝達誤差補償部と、前記速度偏差に基づいて電流指令を生成する電流指令生成部と、前記電流指令に基づいて前記モータに供給される電流を制御する電流制御部と、を含む。

【0025】

この構成によれば、角度伝達誤差の補償を簡易な構成で行うことができ、角度伝達誤差に起因するロボットアームの振動を抑制することができる。

【発明の効果】

【0026】

本発明は、角度伝達誤差に起因する負荷の不安定な挙動を抑制することができるという効果を奏する。

【図面の簡単な説明】

【0027】

【図1】本発明の実施の形態１に係るロボットシステムの構成例を概略的に示す図である。

【図2】図１のロボットシステムの制御系統の構成例を概略的に示すブロック図である。

【図3】図１のロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

【図4】角度伝達誤差の説明図である。

【図5】本発明の実施の形態２に係るロボットシステムの動作例における速度フィードフォワード指令値と速度フィードフォワード指令値の修正値との関係を示すグラフである。

【図6】本発明の実施の形態３に係るロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

【図7】本発明の実施の形態４に係るロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

【図8】本発明の実施の形態５に係るロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

【図9】本発明の実施の形態６に係るロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

【図10】本発明の実施の形態７に係るロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

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

【0028】

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

【0029】

（実施の形態１）
図１は、本発明の実施の形態１に係るロボットシステム１００の構成例を概略的に示す図である。図２は、ロボットシステム１００の制御系統の構成例を概略的に示すブロック図である。

【0030】

図１に示すように、ロボットシステム１００は、例えば、産業用途に利用される。ロボットシステム１００は、ロボットアームを備えるロボット１と、ロボット１の動作を制御するロボット制御装置２とを備える。

【0031】

ロボット１は、多関節型ロボットの産業用ロボット（多関節ロボット）であり、複数の関節１０を有し、先端部にハンド１４を有するロボットアームを有する。図２に示すように、各関節１０には、関節１０を駆動する駆動部が設けられ、駆動部はサーボモータ（モータ）１１と、エンコーダ１２と、減速機１３とを含む。なお、ロボット１は多関節型ロボットに限定されない。本実施の形態において、ロボット１のロボットアームは、６つの関節が１列に並ぶように構成されている。

【0032】

エンコーダ（検知部）１２は、サーボモータ１１の出力軸１１ａの実際の回転角及び実際の回転速度を検出するための事象を検知する。本実施の形態において、エンコーダ１２は、検知した事象に基づいてサーボモータ１１の出力軸１１ａの実際の回転角を含む情報を出力する。そして、詳細は後述するエンコーダ値微分部３７が、サーボモータ１１の出力軸１１ａの実際の回転角を時間微分し、サーボモータ１１の出力軸１１ａの実際の回転速度を算出する。

【0033】

減速機１３は、サーボモータ１１の出力軸１１ａに接続された入力軸１３ａと、ロボット１の関節１０（負荷）に接続された出力軸１３ｂとを含む。入力軸１３ａは、サーボモータ１１の出力軸１１ａと一体であってもよい。また、減速機１３は、１つの機器で構成されてもよく、複数の機器で構成されてもよい。減速機１３は、入力軸１３ａに入力されたサーボモータ１１の出力軸１１ａの回転を所定の減速比Ｒで減速し、出力軸１３ｂから出力する。減速機１３は、例えば波動歯車装置（ハーモニックドライブ（登録商標））である。しかし、これに限られるものではない。

【0034】

波動歯車装置は、サーキュラスプラインと、フレクスプラインと、ウェーブジェネレータとを備える。サーキュラスプラインは、剛性の内歯歯車であり、例えば筐体と一体的に設けられる。フレクスプラインは、可撓性を有する外歯歯車であり、サーキュラスプラインと歯合する。フレクスプラインは、サーキュラスプラインよりも歯数が少なく、出力軸１３ｂと接続される。ウェーブジェネレータは、フレクスプラインの内側に接触する楕円状のカムであり、入力軸１３ａと接続されている。そして、入力軸１３ａを回転させることによって、ウェーブジェネレータがフレクスプラインとサーキュラスプラインとの噛み合い位置を移動させ、サーキュラスプラインとフレクスプラインの歯数差に応じてフレクスプラインが回転軸周りに回転し、出力軸１３ｂが回転する。波動歯車装置は、小型・軽量、高減速比、高トルク容量、ノンバックラッシ等の特徴からロボットの駆動機構の減速機に適した特性を有する。

【0035】

［ロボット制御装置の構成例］
図２に示すように、ロボット制御装置２は、指令部２１と、各関節に対応して設けられたサーボ制御部２２と、各関節に対応して設けられたサーボアンプ２３とを含む。サーボ制御部２２及びサーボアンプ２３がモータ制御システムを構成する。モータ制御システムは、上位装置である指令部２１から入力された位置指令に基づきサーボモータ１１に供給される電流を制御してサーボモータ１１の動作を制御し、且つ入力軸１３ａがサーボモータ１１の出力軸１１ａに接続され且つ出力軸１３ｂが負荷に接続された減速機１３の動作を制御する。本実施の形態において、負荷は、ロボット１のロボットアームの関節１０であり、関節１０を回動させることによってロボットアームの作業端（ハンドが設けられている端部）を移動させることができるように構成されている。

【0036】

指令部２１は、動作プログラムに基づき位置指令を生成し、出力する。出力された位置指令は、サーボ制御部２２に入力される。本実施の形態において、位置指令とは、サーボモータ１１の出力軸１１ａの位置を制御するための制御量であり、サーボモータ１１の出力軸１１ａの回転角である。

【0037】

図３は、ロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。なお、位置指令に重力補償及び動力学補償を行ってもよい。

【0038】

サーボ制御部２２は、指令部２１が生成した位置指令に基づき電流指令を生成する。図３に示すように、サーボ制御部２２は、速度偏差生成部３１と、角度伝達誤差補償部３４と、電流指令生成部３６とを含む。

【0039】

速度偏差生成部３１、角度伝達誤差補償部３４、及び電流指令生成部３６は、所定の制御プログラムを図示しない演算部が実行することにより実現される機能ブロックである。上記の演算部は、例えばマイクロコントローラ、ＣＰＵ、ＡＳＩＣ、ＦＰＧＡ等のプログラマブルロジックデバイス（ＰＬＤ）などの演算器で構成される。演算部は、集中制御する単独の制御器で構成されていてもよく、互いに協働して分散制御する複数の制御器で構成されてもよい。また、ロボット制御装置２は、各種プログラム及びデータを記憶する記憶装置（図示せず）を備えている。

【0040】

速度偏差生成部３１は、位置指令に基づき速度指令を生成し、且つ速度指令とエンコーダ１２が検知した事象に基づき検出されたサーボモータ１１の出力軸１１ａの実際の回転速度との偏差である速度偏差を算出する。

【0041】

速度偏差生成部３１は、速度フィードフォワード指令生成部３２と、位置偏差算出部４１と、ゲイン速度指令生成部４２と、速度偏差算出部６０と、エンコーダ値微分部３７とを有する。

【0042】

速度フィードフォワード指令生成部３２は、位置指令に基づいて速度フィードフォワード指令を生成する。速度フィードフォワード指令生成部３２は、位置指令を時間微分する微分器を含み、位置指令を時間微分して速度フィードフォワード指令を生成する。

【0043】

位置偏差算出部４１は、位置指令からサーボモータ１１の実際の出力軸１１ａの回転角を減算し、位置指令とエンコーダ１２から出力されるサーボモータ１１の実際の出力軸１１ａの回転角との偏差である位置偏差を算出する。

【0044】

ゲイン速度指令生成部４２は、位置偏差算出部４１が算出した位置偏差に位置比例ゲインＫｐを乗じた値を算出する。この算出された値がゲイン速度指令を構成する。このように、ゲイン速度指令生成部４２は、Ｐ制御（比例制御）を行う。

【0045】

エンコーダ値微分部３７は、エンコーダ１２から出力されるサーボモータ１１の実際の出力軸１１ａの回転角に基づいてサーボモータ１１の実際の出力軸１１ａの回転速度を算出する。エンコーダ値微分部３７は、サーボモータ１１の実際の出力軸１１ａの回転角を時間微分する微分器を含み、サーボモータ１１の実際の出力軸１１ａの回転角を時間微分してサーボモータ１１の実際の出力軸１１ａの回転速度を算出する。

【0046】

速度偏差算出部６０は、速度フィードフォワード指令を補正した補正速度フィードフォワード指令（詳細は後述）とゲイン速度指令とを加算して速度指令を生成する。更に、速度偏差算出部６０は、速度指令とエンコーダ値微分部３７が算出したサーボモータ１１の出力軸１１ａの回転速度との偏差である速度偏差を算出する。なお、速度偏差算出部６０における補正速度フィードフォワード指令、ゲイン速度指令、及びサーボモータ１１の出力軸１１ａの回転速度の加減算の順序はこれに限定されるものではない。

【0047】

角度伝達誤差補償部３４は、サーボモータ１１の出力軸１１ａの回転角（減速機１３の入力軸１３ａの回転角）と減速機１３の出力軸１３ｂの回転角との間の角度伝達誤差を推定する。更に、角度伝達誤差補償部３４は、推定した角度伝達誤差に基づいて前記速度フィードフォワード指令を補正し、補正速度フィードフォワード指令を生成する。角度伝達誤差補償部３４は、角度伝達誤差推定部５１と、補正部５２とを含む。

【0048】

ところで、図４に示すように、波動歯車装置を含む減速機には、加工誤差等により、減速機に入力される入力回転角に減速比を乗じた理論上の出力回転角と、実際の出力回転角との差である角度伝達誤差が生じる。この角度伝達誤差は、モータの出力軸の回転に伴って周期的に変化する。このような減速機の出力軸の角度伝達誤差ATEは、以下の式（１）に係る関数を用いたモデルによって近似的に表現することが可能である。

【0049】

【数1】

【0050】

角度伝達誤差推定部５１は、上記式（１）に係る角度伝達誤差の周期的な変動をモデル化した周期関数に基づいて、減速機１３に対する入力回転角であるサーボモータ１１の出力軸１１ａの回転角と減速機１３の出力回転角である減速機１３の出力軸１３ｂの回転角との間の角度伝達誤差を推定し、角度伝達誤差を補償するため（角度伝達誤差をキャンセルするため）にサーボモータ１１の出力軸１１ａに加えるべき補償量θ（・）compを決定する。ここでθ（・）は、θの上に符号・（ドット）が一つ付された記号であるとする。本実施の形態において、角度伝達誤差推定部５１は、サーボモータ１１の出力軸１１ａに加えるべき補償量θ（・）compを、以下の式（２）に基づいて算出する。

【0051】

【数2】

【0052】

式（２）は、式（１）を時間微分し、更に減速比に負号を付してこれを乗じたものであり、式（１）及び式（２）の振幅Ａ及び位相差φは、予め規定されている。例えば、波動歯車装置の角度伝達誤差に特に大きな影響をあたえるのは周波数が２に係る成分であることが判っている。従って、減速機１３が波動歯車装置である場合、周波数ｆを２と規定し、当該周波数ｆに対応する別途同定した振幅Ａ，位相φを用いて上記関数に基づいて補償量θ（・）compを算出してもよい。このように、角度伝達誤差推定部５１は、位置指令及び速度フィードフォワード指令の値（位置指令の時間微分値）に基づき角度伝達誤差の補償量を推定する。

【0053】

補正部５２は、速度フィードフォワード指令値に補償量θ（・）compを加算し、補正速度フィードフォワード指令を生成する。

【0054】

このように、角度伝達誤差補償部３４は、角度伝達誤差推定部５１が推定した角度伝達誤差に基づいて補正部５２が速度フィードフォワード指令値を補正し、補正速度フィードフォワード指令を生成する。そして、上述の通り、速度偏差算出部６０が、補正速度フィードフォワード指令とゲイン速度指令とを加算して速度指令を生成する。

【0055】

このように、角度伝達誤差補償部３４は、速度フィードフォワード指令を補正することによって、速度指令を間接的に補正し、更には補正偏差を間接的に補正している。

【0056】

電流指令生成部３６は、速度偏差生成部３１が生成した速度偏差に基づいて電流指令を生成する。電流指令とは、サーボモータ１１の巻線に供給する電流を制御するための制御量である。

【0057】

例えば、電流指令生成部３６は、速度比例ゲイン部６２と、積分部６３と、速度積分ゲイン部６４と、加減算部６５とを含む。速度比例ゲイン部６２は、速度偏差算出部６０が算出した速度偏差に速度比例ゲインＫｖｐを乗じた第１指令の値を算出する。積分部６３は、第１指令の値を積分する。速度積分ゲイン部６４は、積分部６３が積分した値に速度積分ゲインＫｖｉを乗じた第２指令の値を算出する。加減算部６５は、速度比例ゲイン部６２が算出した第１指令の値と、速度積分ゲイン部６４が算出した第２指令の値を加算した値を算出し、この値を電流指令として出力する。すなわち、電流指令生成部３６は、ＰＩ制御（比例積分制御）を行うように構成されている。出力された電流指令はサーボアンプ２３に入力される。

【0058】

サーボアンプ（電流制御部）２３は、電流指令生成部３６が生成した電流指令に基づいてサーボモータ１１に供給される電流を制御する。

【0059】

ところで、位置指令に角度伝達誤差を補償するための補正を加えて速度指令を生成すると、その下位の速度偏差を算出する過程においてこの補正が打ち消され、角度伝達誤差の補償を適切に行えない場合があった。しかし、本実施の形態において、角度伝達誤差補償部３４は、サーボモータ１１の出力軸１１ａの回転角と減速機１３の出力軸１３ｂの回転角との間の角度伝達誤差を推定し、位置指令よりも下位の指令、すなわち速度フィードフォワード指令を補正することによって速度指令を補正し、速度偏差を補正するので、角度伝達誤差の補償を適切に行うことができる。

【0060】

このように、サーボモータ１１の出力軸１１ａの回転速度を制御するための速度フィードフォワード指令を角度伝達誤差補償部３４が補正し、この補正された補正速度フィードフォワード指令に沿ってサーボモータ１１の出力軸１１ａの回転速度が制御されるよう構成されている。すなわち、ロボット制御装置２は、サーボモータ１１の出力軸１１ａを角度伝達誤差を補償した速度で回転するよう制御する。

【0061】

以上に説明したように、本発明のロボットシステム１００は、角度伝達誤差補償部３４が、速度フィードフォワード指令を補正することによって速度指令を補正し、速度偏差を補正するので、角度伝達誤差の補償を簡易な構成で行うことができ、角度伝達誤差に起因する負荷の不安定な挙動（振動など）を抑制することができる。

【0062】

特に垂直多関節ロボットであるロボット１においては、角度伝達誤差に起因する関節の回動速度の変動は、ハンド１４の振動となって表れる。したがって、角度伝達誤差に起因する関節の回動速度の変動を抑制することによって、角度伝達誤差に起因するハンド１４の振動を抑制することができ、位置決め精度を向上させることができる。

【0063】

また、角度伝達誤差補償部３４は、速度フィードフォワード指令を補正することによって速度指令を補正し、速度偏差を補正するので、速度フィードフォワード指令を用いてサーボモータ１１を制御するロボットシステム１００において、速度指令を適切に補正することができ、角度伝達誤差の補償を適切に行うことができる。

【0064】

（実施の形態２）
以下では実施の形態２の構成、動作について、実施の形態１との相違点を中心に述べる。

【0065】

図５は、実施の形態２に係るロボットシステムの動作例における速度フィードフォワード指令と速度フィードフォワード指令の修正値との関係を示すグラフである。

【0066】

上記実施の形態１において、角度伝達誤差推定部５１は、式（２）の通り、サーボモータ１１の出力軸１１ａに加えるべき補償量θ（・）compを、速度フィードフォワード指令の値を用いて算出した。

【0067】

これに対し、本実施の形態において、角度伝達誤差推定部５１は、まず、式（３）～式（５）に基づいて速度フィードフォワード指令θ(・）の値を修正し、θ（・）ａを算出する。

【0068】

【数3】

【0069】

図５は式（３）～（５）をグラフ化した図であり、速度フィードフォワード指令値θ（・）がvlim以下の場合、式（３）に基づいて、速度フィードフォワード指令値θ（・）をそのまま速度フィードフォワード指令値の修正値θ（・）ａとして設定する。

【0070】

そして、速度フィードフォワード指令値θ（・）がvlimを超えた場合、式（４）に基づいて、速度フィードフォワード指令値θ（・）が大きくなるにしたがって、速度フィードフォワード指令値の修正値θ（・）ａが小さくなるように設定する。

【0071】

また、式（４）において速度フィードフォワード指令の修正値θ（・）ａが０になる値をvlimが超えると、式（５）に基づいて速度フィードフォワード指令の修正値θ（・）ａを０に設定する。

【0072】

次に、角度伝達誤差推定部５１は、式（６）に基づいて、速度フィードフォワード指令値の修正値θ（・）ａを用いて、サーボモータ１１の出力軸１１ａに加えるべき補償量θ（・）compを算出する。

【0073】

【数4】

【0074】

すなわち、速度フィードフォワード指令値θ（・）がvlim以下であった場合は、本実施の形態における補償量θ（・）compは、上記実施の形態１の補償量θ（・）compと同一となるように構成されている。

【0075】

一方、速度フィードフォワード指令値θ（・）がvlimを超える場合は、本実施の形態における補償量θ（・）compは、上記実施の形態１の補償量θ（・）compよりも小さくなるように構成され、特に、式（５）に示す通り、速度フィードフォワード指令値θ（・）が所定以上の値になると補償量θ（・）compは０となるように構成されている。

【0076】

このように、速度フィードフォワード指令値θ（・）が所定の値を超える場合は、補償量θ（・）compを小さくするよう又は補償を行わないように構成されている。これによって、補償量θ（・）compが過度に大きくなることを防止することができ、制御に不具合が生じることを防止することができる。更に、通常、高い精度が求められない高速で動作させる領域における制御を簡素化することができる。

【0077】

また、本実施の形態において、式（４）に示す速度フィードフォワード指令値の範囲においては、速度フィードフォワード指令値θ（・）が大きくなるにしたがって、速度フィードフォワード指令値の修正値θ（・）ａが小さくなるように設定するよう構成されているので、速度フィードフォワード指令値θ（・）がvlimを跨いで変化する場合において、ロボット１の動作が急激に変化することを防止することができる。

【0078】

（実施の形態３）
以下では実施の形態３の構成、動作について、実施の形態１との相違点を中心に述べる。

【0079】

図６は、実施の形態３に係るロボットシステムのサーボ制御部２２の制御系統の構成例を概略的に示すブロック図である。

【0080】

本実施の形態において、サーボ制御部２２は、重力補償部３３７を更に含む。

【0081】

重力補償部３３７は、ロボット１に作用する重力の影響をキャンセルする補償を行うための機能部である。重力補償部３３７は、重力補償値算出部３４１と、補正部３４２とを含む。重力補償値算出部３４１は、ロボット１の関節１０に作用する重力トルクを相殺するための重力補償値Ｇを算出する。補正部３４２は、重力補償値Ｇを電流指令値に加算する。

【0082】

ところで、サーボモータ１１の出力軸１１ａと減速機１３の出力軸１３ｂとの位相差は、減速機１３の出力軸１３ｂにかかるトルクに比例して変化する。したがって、当該変化は、以下の関数として表したモデルによって近似的に表現することが可能である。

【0083】

【数5】

【0084】

そして、本実施の形態において、角度伝達誤差推定部５１は、サーボモータ１１の出力軸１１ａに加えるべき補償量θ（・）compを式（２）に基づいて算出する際に用いるφを、重力補償値算出部３４１が算出した重力補償値Ｇを用いて上記の式（７）に基づいて算出する。なお、比例定数ａ及び位相φ₀は予め同定される。そして、角度伝達誤差推定部５１は、式（７）に基づいて用いたφを用いて、サーボモータ１１の出力軸１１ａに加えるべき補償量θ（・）compを式（２）に基づいて算出する。これによって、サーボ制御部２２は、角度伝達誤差補償をより精確におこなうことができる。

【0085】

（実施の形態４）
以下では実施の形態４の構成、動作について、実施の形態１との相違点を中心に述べる。

【0086】

図７は、実施の形態４に係るロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

【0087】

上記実施の形態１において、角度伝達誤差補償部３４は、速度フィードフォワード指令を補正することによって速度指令を間接的に補正し、速度偏差を間接的に補正した。これに対し、本実施の形態において、角度伝達誤差補償部４３４は、図７に示すように、速度偏差を補正する。

【0088】

すなわち、速度偏差算出部６０は、速度フィードフォワード指令とゲイン速度指令とを加算して速度指令を生成する。更に、速度偏差算出部６０は、速度指令とサーボモータ１１の出力軸１１ａの回転速度との偏差である速度偏差を算出する。

【0089】

そして、角度伝達誤差補償部４３４の補正部４５２は、速度偏差に補償量θ（・）compを加算し、補正速度偏差を算出する。

【0090】

また、電流指令生成部３６は、角度伝達誤差補償部４３４が生成した補正速度偏差に基づいて電流指令を生成する。すなわち、速度比例ゲイン部６２は、角度伝達誤差補償部４３４の補正部４５２が算出した補正偏差に速度比例ゲインＫｖｐを乗じた第１指令を生成する。

【0091】

（実施の形態５）
以下では実施の形態５の構成、動作について、実施の形態１との相違点を中心に述べる。

【0092】

図８は、実施の形態５に係るロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

【0093】

上記実施の形態１において、角度伝達誤差補償部３４は、速度フィードフォワード指令を補正することによって速度指令を間接的に補正し、速度偏差を間接的に補正した。これに対し、本実施の形態において、角度伝達誤差補償部５３４は、図８に示すように、ゲイン速度指令を補正することによって速度指令を間接的に補正し、速度偏差を間接的に補正する。

【0094】

すなわち、角度伝達誤差補償部５３４の補正部５５２は、ゲイン速度指令に補償量θ（・）compを加算し、補正ゲイン速度指令を算出する。そして、速度偏差算出部６０は、速度フィードフォワード指令と補正ゲイン速度指令とを加算して速度指令を算出する。更に、速度偏差算出部６０は、速度指令とサーボモータ１１の出力軸１１ａの回転速度との偏差である速度偏差を算出する。

【0095】

（実施の形態６）
以下では実施の形態６の構成、動作について、実施の形態１との相違点を中心に述べる。

【0096】

図９は、実施の形態６に係るロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

【0097】

上記実施の形態１において、角度伝達誤差補償部３４は、速度フィードフォワード指令を補正することによって速度指令を間接的に補正し、速度偏差を間接的に補正した。これに対し、本実施の形態において、角度伝達誤差補償部６３４は、図９に示すように、サーボモータ１１の出力軸１１ａの回転速度を補正することによって速度偏差を間接的に補正する。

【0098】

すなわち、角度伝達誤差補償部６３４の補正部６５２は、サーボモータ１１の出力軸１１ａの回転速度から補償量θ（・）compを減算し、補正回転速度を算出する。

【0099】

そして、速度偏差算出部６０は、速度指令と補正回転速度との偏差である速度偏差を算出する。

【0100】

（実施の形態７）
以下では実施の形態７の構成、動作について、実施の形態１との相違点を中心に述べる。

【0101】

図１０は、実施の形態７に係るロボットシステムのサーボ制御部の制御系統の構成例を概略的に示すブロック図である。

【0102】

上記実施の形態１において、角度伝達誤差補償部３４は、速度フィードフォワード指令を補正することによって速度指令を間接的に補正し、速度偏差を間接的に補正した。これに対し、本実施の形態において、角度伝達誤差補償部７３４は、図１０に示すように、推定した角度伝達誤差に基づいて位置指令及び速度フィードフォワード指令を補正する。角度伝達誤差補償部７３４は、角度伝達誤差推定部５１と、第１補正部７５２と、第２補正部７５３とを含む。

【0103】

第１補正部７５２は、位置指令に補償量θ（・）compを加算し、補正位置指令を算出する。

【0104】

第２補正部７５３は、補正部５２と同様であるのでその詳細な説明を省略する。

【0105】

速度フィードフォワード指令生成部３２は、補正前の位置指令に基づいて速度フィードフォワード指令を生成する。

【0106】

位置偏差算出部４１は、補正位置指令からサーボモータ１１の出力軸１１ａの回転角を減算し、位置指令とサーボモータ１１の出力軸１１ａの回転角との偏差である位置偏差を算出する。

【0107】

（実施の形態８）
以下では実施の形態８の構成、動作について、実施の形態１との相違点を中心に述べる。

【0108】

上記実施の形態１において、角度伝達誤差補償部３４の角度伝達誤差推定部５１は、式（２）に基づいてサーボモータ１１の出力軸１１ａに加えるべき補償量θ（・）compを算出する際、角度伝達誤差に特に大きな影響を与える所定の周波数ｆ及びこの周波数に対応する所定の振幅Ａ及び所定の位相差φを用いて補償量θ（・）compを算出した。これに代えて、角度伝達誤差補償部３４の角度伝達誤差推定部５１は、サーボモータ１１の出力軸１１ａに加えるべき補償量θ（・）compを、以下の式（８）に基づいて算出してもよい。

【0109】

【数6】

【0110】

すなわち、角度伝達誤差推定部５１は、位置指令及び速度フィードフォワード指令の値（位置指令の時間微分値）に基づき、周波数のパターン毎に角度伝達誤差を算出し、これらを足し合わせた値を角度伝達誤差の補正に用いる。これによって、角度伝達誤差をより正確に補正することができる。

【0111】

上記説明から、当業者にとっては、本発明の多くの改良や他の実施形態が明らかである。従って、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び／又は機能の詳細を実質的に変更できる。

【符号の説明】

【0112】

１ ロボット
２ ロボット制御装置
１０ 回動軸
１１ サーボモータ
１１ａ 出力軸
１２ エンコーダ
１３ 減速機
１３ａ 入力軸
１３ｂ 出力軸
２２ サーボ制御部
２３ サーボアンプ
３１ 速度偏差算出部
３４ 角度伝達誤差補償部
３６ 電流指令生成部
１００ ロボットシステム

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】