特許 6551199 制御装置、制御方法、制御プログラム

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

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

(11)【特許番号】6551199

(24)【登録日】2019年7月12日

(45)【発行日】2019年7月31日

(54)【発明の名称】制御装置、制御方法、制御プログラム

(51)【国際特許分類】

   G05D 3/12 20060101AFI20190722BHJP

   H02P 5/50 20160101ALI20190722BHJP

【ＦＩ】

   G05D3/12 P

   H02P5/50 A

【請求項の数】6

【全頁数】13

(21)【出願番号】特願2015-234142(P2015-234142)

(22)【出願日】2015年11月30日

(65)【公開番号】特開2017-102616(P2017-102616A)

(43)【公開日】2017年6月8日

【審査請求日】2017年10月26日

【早期審査対象出願】

(73)【特許権者】

【識別番号】000002945

【氏名又は名称】オムロン株式会社

(74)【代理人】

【識別番号】100155712

【弁理士】

【氏名又は名称】村上 尚

(72)【発明者】

【氏名】森野 宏次

【審査官】 大古 健一

(56)【参考文献】

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

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

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

【文献】 実開平４−２０１０３（ＪＰ，Ｕ）

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

Ｇ０５Ｄ ３／１２

Ｈ０２Ｐ ５／５０

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

【請求項1】

複数のサーボモータに対応する複数のサーボドライバに目標軌跡に基づいた位置指令を行う制御装置であって、
前記複数のサーボモータのうち応答遅れ時間が最大である基準サーボモータに対応するサーボドライバを基準サーボドライバとして、他のサーボドライバへの指令タイミングを、前記基準サーボドライバへの指令タイミングよりも、前記基準サーボモータの応答遅れ時間と前記他のサーボドライバに対応するサーボモータの応答遅れ時間との差だけ遅らせるとともに、
各サーボドライバから位置ループゲインを取得し、各サーボモータの前記応答遅れ時間を、取得した前記位置ループゲインの逆数によって算出することを特徴とする制御装置。

【請求項2】

複数のサーボモータに対応する複数のサーボドライバに目標軌跡に基づいた位置指令を行う制御装置であって、
前記複数のサーボモータのうち応答遅れ時間が最大である基準サーボモータに対応するサーボドライバを基準サーボドライバとして、他のサーボドライバへの指令タイミングを、前記基準サーボドライバへの指令タイミングよりも、前記基準サーボモータの応答遅れ時間と前記他のサーボドライバに対応するサーボモータの応答遅れ時間との差だけ遅らせるとともに、
各サーボモータの加速度変化時に、各サーボモータの応答遅れ時間の２乗と加速度との積に比例する量の補正が加わるように位置指令を行い、さらに、
各サーボドライバから位置ループゲインを取得し、各サーボモータの前記応答遅れ時間を、取得した前記位置ループゲインの逆数によって算出することを特徴とする制御装置。

【請求項3】

複数のサーボモータに対応する複数のサーボドライバに目標軌跡に基づいた位置指令を行い、前記複数のサーボモータを応答させる制御方法であって、
各サーボモータの応答遅れ時間を得る第１ステップと、
応答遅れ時間を比較する第２ステップと、
前記複数のサーボモータのうち応答遅れ時間が最大である基準サーボモータに対応するサーボドライバを基準サーボドライバとして、他のサーボドライバへの位置指令を、前記基準サーボドライバへの位置指令よりも、前記基準サーボモータの応答遅れ時間と前記他のサーボドライバに対応するサーボモータの応答遅れ時間との差だけ遅らせて行う第３ステップと、を含み、
前記第１ステップにおいて、各サーボドライバから位置ループゲインを取得し、各サーボモータの前記応答遅れ時間を、取得した前記位置ループゲインの逆数によって算出することを特徴とする制御方法。

【請求項4】

複数のサーボモータに対応する複数のサーボドライバに目標軌跡に基づいた位置指令を行い、前記複数のサーボモータを応答させる制御方法であって、
各サーボモータの応答遅れ時間を得る第１ステップと、
応答遅れ時間を比較する第２ステップと、
前記複数のサーボモータのうち応答遅れ時間が最大である基準サーボモータに対応するサーボドライバを基準サーボドライバとして、他のサーボドライバへの位置指令を、前記基準サーボドライバへの位置指令よりも、前記基準サーボモータの応答遅れ時間と前記他のサーボドライバに対応するサーボモータの応答遅れ時間との差だけ遅らせて行う第３ステップとを含み、
各サーボモータの加速度変化時に、各サーボモータの応答遅れ時間の２乗と加速度との積に比例する量の補正が加わるように位置指令を行い、さらに、
前記第１ステップにおいて、各サーボドライバから位置ループゲインを取得し、各サーボモータの前記応答遅れ時間を、取得した前記位置ループゲインの逆数によって算出することを特徴とする制御方法。

【請求項5】

請求項３または４に記載の第１〜第３ステップをプロセッサに実行させることを特徴とする制御プログラム。

【請求項6】

請求項５記載の制御プログラムを記録したコンピュータ読み取り可能な記録媒体。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、制御装置、制御方法、制御プログラムに関する。

【背景技術】

【0002】

特許文献１には、ロボットの動作を制御する制御装置が、軸毎に指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算し、最短のサーボ遅れ時間を基準時間とし、軸毎のサーボ遅れ時間と前記基準時間とに基づいて軸毎の補償トルクを計算し、軸毎の補償トルクが反映された指令値を各サーボに出力してロボットの動作を制御する手法が開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２００９−１５１５２７号公報（２００９年７月９日公開）

【発明の概要】

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

【0004】

前記手法では、軸毎のサーボ遅れ時間と前記基準時間とに基づく補償トルクの計算、並びに補償トルクが反映された指令値の算出が煩雑になるという問題があった。

【0005】

本発明は、前記の問題点に鑑みてなされたものであり、その目的は、複数軸（複数のサーボモータ）間の応答遅れ時間のばらつきに起因する軌跡ずれを抑制しうる制御装置を実現することにある。

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

【0006】

本制御装置は、複数のサーボモータに対応する複数のサーボドライバに指令を行う制御装置であって、前記複数のサーボモータのうち応答遅れ時間が最大である基準サーボモータに対応するサーボドライバを基準サーボドライバとして、他のサーボドライバへの指令タイミングを、前記基準サーボドライバへの指令タイミングよりも、前記基準サーボモータの応答遅れ時間と前記他のサーボドライバに対応するサーボモータの応答遅れ時間との差だけ遅らせることを特徴とする。

【0007】

前記構成によれば、前記複数のサーボモータの応答タイミングが揃うため、複数軸（複数のサーボモータ）間の応答遅れ時間のばらつきに起因する軌跡ずれを抑制することができる。

【0008】

本制御装置では、前記指令は、目標軌跡に基づいた位置指令である構成とすることもできる。

【0009】

本制御装置では、各サーボモータの応答遅れ時間は、対応するサーボドライバの位置ループゲインの逆数で示される構成とすることもできる。

【0010】

本制御装置では、各サーボモータの加速度変化時に、加速度に比例する量の補正が加わるように位置指令を行う構成とすることもできる。

【0011】

前記構成によれば、加速度変化時の位置偏差に起因する軌跡ずれの抑制が可能となる。

【0012】

本制御方法は、複数のサーボモータに対応する複数のサーボドライバに指令を行い、前記複数のサーボモータを応答させる制御方法であって、各サーボモータの応答遅れ時間を得る第１ステップと、応答遅れ時間を比較する第２ステップと、前記複数のサーボモータのうち応答遅れ時間が最大である基準サーボモータに対応するサーボドライバを基準サーボドライバとして、他のサーボドライバへの位置指令を、前記基準サーボドライバへの位置指令よりも、前記基準サーボモータの応答遅れ時間と前記他のサーボドライバに対応するサーボモータの応答遅れ時間との差だけ遅らせて行う第３ステップとを含むことを特徴とする。

【0013】

前記方法によれば、前記複数のサーボモータの応答タイミングが揃うため、複数軸（複数のサーボモータ）間の応答遅れ時間のばらつきに起因する軌跡ずれを抑制することができる。

【0014】

本制御方法では、前記第２ステップでは、各サーボドライバの位置ループゲインの逆数を比較してもよい。

【0015】

本制御プログラムは、前記第１〜第３ステップをプロセッサに実行させることを特徴とする。

【0016】

本記録媒体は、前記制御プログラムを記録したコンピュータ読み取り可能な記録媒体である。

【発明の効果】

【0017】

本制御装置によれば、複数のサーボモータ間の応答遅れ時間のばらつきに起因する軌跡ずれを抑制することができる。

【図面の簡単な説明】

【0018】

【図1】複数軸構成を示す模式図である。

【図2】軸間応答遅れ時間差に起因する軌跡ずれの例を説明する参考図である。

【図3】軸間応答遅れ時間差に起因する軌跡ずれの原理を説明する参考図である。

【図4】本制御装置の位置指令を説明する説明図である。

【図5】加速度変化時の位置偏差に起因する軌跡ずれの例を説明する参考図である。

【図6】加速度変化時の位置偏差に起因する軌跡ずれを抑制する原理を説明する説明図である。

【図7】加速度変化時の位置偏差に起因する軌跡ずれを抑制する手法を説明する説明図である。

【図8】本制御装置の機能モジュールを示す模式図である。

【図9】本実施形態にかかる各サーボドライバの構成例を示す模式図である。

【図10】本制御装置の処理工程を示すフローチャートである。

【図11】本実施形態の効果を示す軌跡図である。

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

【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】

〔ソフトウェアによる実現例〕
制御装置の各機能モジュールは、ＣＰＵ（Central Processing Unit）を用いてソフトウェアによって実現してもよいし、集積回路（ＩＣチップ）等に形成された論理回路（ハードウェア）によって実現してもよい。

【0050】

前者の場合、制御装置は、各機能を実現するソフトウェアである制御プログラムの命令を実行するＣＰＵ、制御プログラムおよび各種データがコンピュータ（またはＣＰＵ）で読み取り可能に記録されたＲＯＭ（Read Only Memory）または記憶装置（これらを「記録媒体」と称する）、制御プログラムを展開するＲＡＭ（Random Access Memory）などを備えている。そして、コンピュータ（またはＣＰＵ）が制御プログラムを記録媒体から読み取って実行することにより、本実施形態の目的が達成される。記録媒体としては、「一時的でない有形の媒体」、例えば、ディスク、カード、半導体メモリ、プログラマブルな論理回路などを用いることができる。また、制御プログラムは、これを伝送可能な任意の伝送媒体（通信ネットワークや放送波等）を介してコンピュータに供給されてもよい。なお、本実施形態は、制御プログラムが電子的な伝送によって具現化された、搬送波に埋め込まれたデータ信号の形態でも実現され得る。

【0051】

本発明は上記の実施の形態に限定されるものではなく、上記実施の形態を技術常識に基づいて適宜変更したものやそれらを組み合わせて得られるものも本発明の実施の形態に含まれる。

【符号の説明】

【0052】

ＳＤｘ サーボドライバ（Ｘ軸）
ＳＤｙ サーボドライバ（Ｙ軸）
ＳＤｚ サーボドライバ（Ｚ軸）
ＳＭｘ サーボモータ（Ｘ軸）
ＳＭｙ サーボモータ（Ｙ軸）
ＳＭｚ サーボモータ（Ｚ軸）
Ｒｘ 応答遅れ時間（Ｘ軸）
Ｒｙ 応答遅れ時間（Ｙ軸）
Ｒｚ 応答遅れ時間（Ｚ軸）

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】