特許 5710367 制御装置および制御方法、並びにプログラム

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

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

(11)【特許番号】5710367

(24)【登録日】2015年3月13日

(45)【発行日】2015年4月30日

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

(51)【国際特許分類】

   G05D 3/12 20060101AFI20150409BHJP

   H02P 29/00 20060101ALI20150409BHJP

   G05D 19/02 20060101ALI20150409BHJP

【ＦＩ】

   G05D3/12 Z

   H02P5/00 X

   G05D19/02 D

【請求項の数】6

【全頁数】35

(21)【出願番号】特願2011-102092(P2011-102092)

(22)【出願日】2011年4月28日

(65)【公開番号】特開2012-234332(P2012-234332A)

(43)【公開日】2012年11月29日

【審査請求日】2013年3月5日

【新規性喪失の例外の表示】特許法第３０条第１項適用 平成２２年１１月４日 日本機械学会（幹事学会）・計測自動制御学会・システム制御情報学会・化学工学会・精密工学会・日本航空宇宙学会 主催の「第５３回 自動制御連合講演会（ＣＤ−ＲＯＭ）」において文書をもって発表

(73)【特許権者】

【識別番号】803000115

【氏名又は名称】学校法人東京理科大学

(73)【特許権者】

【識別番号】592141488

【氏名又は名称】アスリートＦＡ株式会社

(74)【代理人】

【識別番号】110000121

【氏名又は名称】アイアット国際特許業務法人

(72)【発明者】

【氏名】星野 祐

(72)【発明者】

【氏名】田中 裕介

(72)【発明者】

【氏名】根橋 徹

【審査官】 青山 純

(56)【参考文献】

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

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

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

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

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

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

Ｇ０５Ｄ ３／１２

Ｇ０５Ｄ １９／０２

Ｈ０２Ｐ ２９／００

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

【請求項1】

アクチュエータの可動部の位置を制御する制御装置において、
上記可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する決定手段と、
決定された上記パラメータから軌道を計算する計算手段と
を有し、
上記決定手段は、上記可動部の移動すべき距離に対する移動時間と共振周波数とを設定することで、上記可動部が移動距離を設定された移動目標時間で移動する共振周波数成分を除去した加速度軌道のパラメータを算出するものであり、

【数1】

により上記軌道を計算するための、c0,c1,aT,bT,ak,bk(k=1,・・・,n)である上記パラメータを決定する
ここで、Ｔ：可動部の移動時間
ωＴ：可動部の移動時間Ｔの角周波数
ωｋ：共振周波数を除去するため加える正弦波の角周波数
である
ことを特徴とする制御装置。

【請求項2】

アクチュエータの可動部の位置を制御する制御装置において、
上記可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する決定手段と、
決定された上記パラメータから軌道を計算する計算手段と
を有し、
上記決定手段は、上記可動部の移動すべき距離に対する移動時間と共振周波数とを設定することで、上記可動部が移動距離を設定された移動目標時間で移動する共振周波数成分を除去した加速度軌道のパラメータを算出するものであり、

【数2】

により上記軌道を計算するための、ak(k=0,・・・2n-1)である上記パラメータを決定する
ここで、ψ_ｋ：Paley順序のWalsh関数
Ｔ：可動部の移動時間
である
ことを特徴とする制御装置。

【請求項3】

アクチュエータの可動部の位置を制御する制御方法において、
上記可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する決定ステップと、
決定された上記パラメータから軌道を計算する計算ステップと
を含み、
上記決定ステップは、上記可動部の移動すべき距離に対する移動時間と共振周波数とを設定することで、上記可動部が移動距離を設定された移動目標時間で移動する共振周波数成分を除去した加速度軌道のパラメータを算出するものであり、
る共振周波数成分を除去した加速度軌道のパラメータを算出するものであり、

【数3】

により上記軌道を計算するための、c0,c1,aT,bT,ak,bk(k=1,・・・,n)である上記パラメータを決定する
ここで、Ｔ：可動部の移動時間
ωＴ：可動部の移動時間Ｔの角周波数
ωｋ：共振周波数を除去するため加える正弦波の角周波数
である
ことを特徴とする制御方法。

【請求項4】

アクチュエータの可動部の位置を制御する制御方法において、
上記可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する決定ステップと、
決定された上記パラメータから軌道を計算する計算ステップと
を含み、
上記決定ステップは、上記可動部の移動すべき距離に対する移動時間と共振周波数とを設定することで、上記可動部が移動距離を設定された移動目標時間で移動する共振周波数成分を除去した加速度軌道のパラメータを算出するものであり、
る共振周波数成分を除去した加速度軌道のパラメータを算出するものであり、

【数4】

により上記軌道を計算するための、ak(k=0,・・・2n-1)である上記パラメータを決定する
ここで、ψ_ｋ：Paley順序のWalsh関数
Ｔ：可動部の移動時間
である
ことを特徴とする制御方法。

【請求項5】

アクチュエータの可動部の位置を制御するコンピュータに、
上記可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する決定ステップと、
決定された上記パラメータから軌道を計算する計算ステップと
を含み、
上記決定ステップは、上記可動部の移動すべき距離に対する移動時間と共振周波数とを設定することで、上記可動部が移動距離を設定された移動目標時間で移動する共振周波数成分を除去した加速度軌道のパラメータを算出するものであり、

【数5】

により上記軌道を計算するための、c0,c1,aT,bT,ak,bk(k=1,・・・,n)である上記パラメータを決定する
ここで、Ｔ：可動部の移動時間
ωＴ：可動部の移動時間Ｔの角周波数
ωｋ：共振周波数を除去するため加える正弦波の角周波数
である
処理を行わせるプログラム。

【請求項6】

アクチュエータの可動部の位置を制御するコンピュータに、
上記可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する決定ステップと、
決定された上記パラメータから軌道を計算する計算ステップと
を含み、
上記決定ステップは、上記可動部の移動すべき距離に対する移動時間と共振周波数とを設定することで、上記可動部が移動距離を設定された移動目標時間で移動する共振周波数成分を除去した加速度軌道のパラメータを算出するものであり、

【数6】

により上記軌道を計算するための、ak(k=0,・・・2n-1)である上記パラメータを決定する
ここで、ψ_ｋ：Paley順序のWalsh関数
Ｔ：可動部の移動時間
である
処理を行わせるプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

たとえば、半導体製造装置におけるウェハなどの位置決め際の可動部の動きを制御する方法として、Ｓ字加減速制御がよく知られている。

【0003】

図１８は、Ｓ字加減速制御時の加速度の軌道を示す図である。図１８（Ａ）は、Ｓ字加減速制御による加速度軌道を示し、図１８（Ｂ）は、Ｓ字加減速制御による速度軌道を示し、図１８（Ｃ）は、Ｓ字加減速制御による位置軌道を示す。

【0004】

ところで微細な対象物を取り扱う場合には、可動部が加減速を繰り返すことによる筐体の振動を抑制したいとの要望も大きい。

【0005】

振動を抑制する制御方式の１つとして、指令から振動周波数成分を除去する制振フィルタを用いる方法があげられる。図１９は、制振フィルタの効果を示す図である。図１９（Ａ）は、筐体のＹ軸方向の変位を示し、図１９（Ｂ）は、軸の位置の追従の偏差を示す。図１９において、実線は、制御フィルタによる制振制御が無い場合を示し、点線は、最低の制振制御を行った場合を示し、一点鎖線は、最高の制振制御を行った場合を示す。

【0006】

また、振動を抑制する方法には、共振周波数を推定するものもある。

【0007】

たとえば、機械系を含む制御対象を駆動する電動機の駆動指令信号を生成する指令生成器、駆動指令信号にもとづいて制御対象の制振周波数における信号成分が極小となるようにしたフィードフォワード信号を生成し、駆動指令信号に加算するフィードフォワード信号生成器、制御対象の位置、速度または加速度に応じた信号を入力とし、制振周波数においてゲインが極大となる伝達関数により演算した共振信号を出力する共振信号演算器及び共振信号にもとづいて制御対象の共振周波数を推定する共振推定器を備えたものもある（例えば、特許文献１参照）。

【先行技術文献】

【特許文献】

【0008】

【特許文献1】特開２００５−３９９５４号公報

【発明の概要】

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

【0009】

しかしながら、これらの方法では、振動を抑制するために、アクチュエータやセンサなど新たなハードウェアが必要になることがある。

【0010】

また、図１９に示されるように、制振フィルタを用いることで、移動にかかる時間が長くなってしまうことがあり、制振フィルタを用いる前に設定したタクトタイムが、制振フィルタを動作させることにより、長くなってしまうことがある。

【0011】

そこで、本発明は、上記課題を解決すること、すなわち、新たなハードウェアを必要とせず、タクトタイムを変化させることなく、振動を抑制することのできる制御装置および制御方法、並びにプログラムを提供することを目的とする。

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

【0012】

上記課題を解決するために、本発明の制御装置の一側面は、アクチュエータの可動部の位置を制御する制御装置であって、可動部の可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する決定手段と、決定されたパラメータから軌道を計算する計算手段とを有し、決定手段は、可動部の移動すべき距離に対する移動時間と共振周波数とを設定することで、可動部が移動距離を設定された移動目標時間で移動する共振周波数成分を除去した加速度軌道のパラメータを算出するものである。

【0013】

また、本発明の制御装置の一側面は、上述の構成に加えて、決定手段が、

【数1】

により軌道を計算するための、c₀,c₁,a_T,b_T,a_k,b_k(k=1,・・・,n)であるパラメータを決定するものとされている。

【0017】

また、本発明の制御装置の一側面は、上述の構成に加えて、決定手段が、

【数2】

により軌道を計算するための、a_k(k=0,・・・,2ⁿ-1)であるパラメータを決定するものとされている。

【0018】

また、本発明の制御方法の一側面は、アクチュエータの可動部の位置を制御する制御方法であって、可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する決定ステップと、決定された上記パラメータから軌道を計算する計算ステップとを含み、決定ステップは、可動部の移動すべき距離に対する移動時間と共振周波数とを設定することで、可動部が移動距離を設定された移動目標時間で移動する共振周波数成分を除去した加速度軌道のパラメータを算出するものとされている。

【0019】

さらに、本発明のプログラムの一側面は、アクチュエータの可動部の位置を制御するコンピュータに、可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する決定ステップと、決定されたパラメータから軌道を計算する計算ステップとを含み、決定ステップは、可動部の移動すべき距離に対する移動時間と共振周波数とを設定することで、可動部が移動距離を設定された移動目標時間で移動する共振周波数成分を除去した加速度軌道のパラメータを算出する処理を行わせるものとされている。

【発明の効果】

【0020】

本発明によれば、新たなハードウェアを必要とせず、タクトタイムを変化させることなく、振動を抑制することのできる制御装置および制御方法、並びにプログラムを提供することができる。

【図面の簡単な説明】

【0021】

【図1】ダイピッカ１の構成を示す図である。

【図2】作業台１１の構成を示す図である。

【図3】コンピュータ１２およびチップ供給部３３の構成の例を説明するブロック図である。

【図4】プログラムを実行するコンピュータ１２により実現される機能の構成の例を示すブロック図である。

【図5】データベース１０３に記憶されるパラメータの例を示す図である。

【図6】パラメータの記憶の処理を説明するフローチャートである。

【図7】駆動制御の処理を説明するフローチャートである。

【図8】定速区間を含まない制振加減速軌道の例を示す図である。

【図9】終端速度を指定した制振加減速軌道の例を示す図である。

【図10】制振加減速軌道とＳ字加減速軌道との例を比較する図である。

【図11】筐体の振動の例を示す図である。

【図12】Ｓ字加減速軌道による振動を表す図である。

【図13】制振加減速軌道による振動を表す図である。

【図14】筐体の振動を比較する図である。

【図15】筐体の振動とタクトタイムとの関係を示す図である。

【図16】コンピュータ１２およびチップ供給部３３の構成の他の例を説明するブロック図である。

【図17】コンピュータ１２のハードウェアの構成例を示すブロック図である。

【図18】Ｓ字加減速軌道を示す図である。

【図19】制振フィルタの効果を示す図である。

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

【0022】

以下、本発明の一実施の形態の制御装置について、図１〜図１５を参照しながら説明する。

【0023】

図１は、制御装置を含むダイピッカの構成を示す図である。ダイピッカ１は、半導体製造の後工程に設けられ、ダイシングされたウェハからチップをピックアップし、所定のトレイに整列し、収納する。ダイピッカ１は、作業台１１、コンピュータ１２、電装ボックス１３、モニタ１４、およびキーボード１５からなる。作業台１１は、チップを操作する。作業台１１は、図２に示されるように、チップピッカ３１、チップ収納ユニット３２、およびチップ供給部３３を備える。チップピッカ３１は、ウェハからチップ（ダイ）を取り上げて、チップトレイの所定の位置に置く。チップ収納ユニット３２は、ダイが収納されるチップトレイを位置決めする。図２の例の場合、Ｙ軸駆動装置３２Ａによりトレイ載置台３２ＢのＹ軸方向の位置が決定される。チップ供給部３３は、チップピッカ３１により取り上げられる、ウェハ中のチップ（ダイ）を位置決めする。図２の例の場合、Ｙ軸駆動装置３３ＡとＸ軸駆動装置３３Ｂにより、ウェハ載置台３３ＣのＹ軸およびＸ軸方向の位置が決定される。チップ供給部３３は、２自由度の直交ロボットである。

【0024】

図１に戻り、コンピュータ１２は、FA（Factory Automation）用のコンピュータであり、作業台１１を制御する。コンピュータ１２は、制御装置の一例である。電装ボックス１３は、電源ユニット、後述するサーボアンプユニット、および端子台などを格納する。モニタ１４は、いわゆるタッチパネルであり、コンピュータ１２の制御に応じて、各種の画像などを表示させ、オペレータの操作に応じて、コンピュータ１２に各種の指示を与える。キーボード１５は、オペレータの操作に応じて、コンピュータ１２に各種の指示を与える。

【0025】

図３は、チップ供給部３３の構成の例を説明するブロック図である。Ｘ軸方向に、サーボアンプユニット５１−１、サーボモータ５２−１、ボールネジ５３−１、およびリニアガイド５４−１により駆動される。Ｙ軸方向に、サーボアンプユニット５１−２、サーボモータ５２−２、ボールネジ５３−２、およびリニアガイド５４−２により駆動される。

【0026】

サーボアンプユニット５１−１は、コンピュータ１２からの指令に応じて、サーボモータ５２−１を駆動する。ボールネジ５３−１は、サーボモータ５２−１の軸と機械的に結合され、サーボモータ５２−１の軸が回転すると、ボールネジ５３−１に噛み合っている可動部を移動させる。リニアガイド５４−１は、ボールネジ５３−１の可動部がＸ軸方向に動くように、可動部の動きを規制する。

【0027】

サーボアンプユニット５１−２は、コンピュータ１２からの指令に応じて、サーボモータ５２−２を駆動する。ボールネジ５３−２は、サーボモータ５２−２の軸と機械的に結合され、サーボモータ５２−２の軸が回転すると、ボールネジ５３−２に噛み合っている可動部を移動させる。リニアガイド５４−２は、ボールネジ５３−２の可動部がＹ軸方向に動くように、可動部の動きを規制する。

【0028】

チップピッカ３１およびチップ収納ユニット３２は、それぞれ、３自由度または１自由度とされ、チップ供給部３３と同様に構成されるので、その説明は省略する。

【0029】

図４は、プログラムを実行するコンピュータ１２により実現される機能の構成の例を示すブロック図である。コンピュータ１２がプログラムを実行すると、共振周波数取得部１０１、パラメータ生成部１０２、データベース１０３、パラメータ決定部１０４、軌道計算部１０５、および分解加速度制御部１０６が実現される。

【0030】

共振周波数取得部１０１は、ダイピッカ１の共振周波数を取得する。パラメータ生成部１０２は、基準距離について、予め定めた移動時間毎に、可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを生成する。軌道は、空間的に、直線的なものであっても、曲線的なものであってもよい。以下において、軌道は、空間的な位置と時刻との関係を示すものである。

【0031】

ここで、チップ供給部３３の可動部の軌道とパラメータについて説明する。

【0032】

まず、Ｓ字加減速軌道について説明する。時刻0における、可動部の軌道の始点をx（0）とし、時刻Tに到達する軌道の終点（終端位置）をx（T）とすると、始点と終点において速度および加速度を0とする境界条件は、式（１）で表される。

【0033】

【数3】

・・・（１）

【0034】

また、最大速度v_maxと最大加速度a_maxに関する制約条件は、式（２）で表される。

【0035】

【数4】

・・・（２）

【0036】

ここで、（v_max>0，a_max>0）を満たす軌道で正弦波状の加速度軌道をもつもののうち、移動時間Tを最短にするものは次の式で与えられる。

【0037】

まず、移動距離が

【数5】

である場合を想定する。

【0038】

この場合、移動時間は、

【数6】

である。

【0039】

このとき、軌道は、式（３）で表される。

【0040】

【数7】

・・・（３）

【0041】

一方、移動距離が、

【数8】

である場合を想定する。

【0042】

この場合、加速時間は、

【数9】

である。

【0043】

また、この場合、移動時間は、

【数10】

である。

【0044】

このとき、軌道は、式（４）で表される。

【0045】

【数11】

・・・（４）

【0046】

ただし、

【数12】

とする。

【0047】

Ｓ字加減速軌道の加速度軌道は、図１８（Ａ）に示される振幅スペクトルを持つ。そこで、この振幅スペクトルに修正成分を加えて、振動を抑制するような望ましい加速度軌道に整形する。

【0048】

まず、定速区間を含まない軌道について説明する。

【0049】

式（３）で表されるＳ字加減速軌道は、移動時間をTとすると、

【数13】

を使って、式（７）と記述することができる。

【0050】

【数14】

・・・（７）

【0051】

式（７）に対応する制振加減速軌道の一般形は式（８）で表される。

【0052】

【数15】

・・・（８）

【0053】

すなわち、制振のための修正項として、制振加減速軌道の一般式である式（８）に、右辺第３項と第５項とが加えられている。右辺第５項は、2n個の正弦波の和であり、nは制振に関する振幅スペクトル条件の数により決定される。

【0054】

制振加減速軌道を設計するには、式（８）に含まれるc₀,c₁,a_T,b_T,a_k,b_k(k=1,・・・,n)であるパラメータを決定すれば良い。

【0055】

まず、式（８）は、式（９）で表される位置決めに関する境界条件を満たす。

【0056】

【数16】

・・・（９）

【0057】

ここで、終端位置をx_T=1としたのは、移動距離（始点から終端位置までの距離）がパラメータに対して線形であるため、このようにしても一般性を失わないからである。移動距離が１に等しくない場合には、式（９）の下で得られたパラメータをx_T倍（すなわち、移動距離倍）すれば良い。

【0058】

次に、加速度軌道の振幅スペクトルに関する条件を説明する。式（８）を２階微分すると式（１０）が得られる。

【0059】

【数17】

・・・（１０）

【0060】

軌道が孤立的である場合、加速度軌道はフーリエ積分により式（１１）で与えられる。

【0061】

【数18】

・・・（１１）

【0062】

角周波数は、

【数19】

で表す。

【0063】

角周波数に対する第１のフーリエ係数は、

【数20】

で表す。

【0064】

角周波数に対する第２のフーリエ係数は、

【数21】

で表す。

【0065】

フーリエ余弦係数およびフーリエ正弦係数は、式（１２）で計算できる。

【0066】

【数22】

・・・（１２）

【0067】

式（１２）は、角周波数

【数23】

の連続関数であり、任意の角周波数に対して定義されている。これらから、加速度軌道の振幅スペクトルがm個の角周波数で０となる条件は式（１３）で表される。

【0068】

【数24】

・・・（１３）

【0069】

式（８）で表される制振加減速軌道には、（2n+4）個の決定すべきパラメータがあり、式（９）および式（１３）で表される満足すべき条件は合わせて（2m+6）本の式で表されるので、m=n-1であれば、パラメータを求めることができる。

【0070】

そこで、パラメータは以下のように求められる。

【0071】

式（８）で表される、パラメータを含む加減速軌道が、式（９）で表される、指定されたTにおける位置決めに関する境界条件、および式（１４）で表される、スペクトル条件を満たすとき、そのパラメータは式（１５）の線形代数方程式の解で与えられる。

【0072】

【数25】

・・・（１４）

【0073】

【数26】

・・・（１５）

【0074】

また、

【数27】

である。

【0075】

さらに、

【数28】

である。

【0076】

次に、定速区間を含む軌道を説明する。

【0077】

振動の要因が可動部の加減速であることから、加減速区間についてのみ説明する。

【0078】

式（４）で表されるＳ字加減速軌道のうちの加速区間を

【数29】

とする。

【0079】

この場合、Ｓ字加減速軌道は、式（１６）と記述することができる。

【0080】

【数30】

・・・（１６）

【0081】

ここでは、振動を抑制する制振加減速軌道の一般形を式（１７）で表す。

【0082】

【数31】

・・・（１７）

【0083】

制振のための修正項として、制振加減速軌道の一般式である式（１６）に、右辺第４項が加えられる。

【0084】

ここでも境界条件とスペクトル条件から式（１７）に含まれるc₀,c₁,a_T,b_T,a_k,b_k(k=1,・・・,n)であるパラメータを決定する。

【0085】

定速区間を含む軌道では、加速時間が経過した後に目標速度が指定された値となる。従って、位置決めに関する境界条件を式（１８）で表すことができる。

【0086】

【数32】

・・・（１８）

【0087】

ここでも、式（９）と同様に終端速度を１としている。

【0088】

振幅スペクトルに関する条件は、式（１７）を２階微分し、そのフーリエ積分から式（１２）で計算でき、式（１３）が同様に導かれる。ただし、積分区間は、tは、0以上、T_a以下である。式（１７）で表される制振加減速軌道のパラメータの数が（2n+3）であり、式（１８）および式（１３）で表される、満たすべき条件が（2m+5）本の式で表されるので、m=n-1であれば、パラメータを求めることができる。

【0089】

式（１７）で表される、パラメータを含む加減速軌道が、式（１８）で表される、位置決めに関する境界条件、および式（１９）で表される、スペクトル条件を満たすとき、そのパラメータは式（２０）の線形代数方程式の解で与えられる。

【0090】

【数33】

・・・（１９）

【0091】

【数34】

・・・（２０）

【0092】

A_h,k,B_h,k,C_h,k,D_h,kは、式（１５）の場合と同様である。

【0093】

また、

【数35】

である。

【0094】

減速区間についても同様に制振加減速軌道が求められる。

【0095】

以上のように加減速部分の軌道が求められた。残りの定速区間の軌道を定めるには、式（１８）で指定されていない加速区間終了字の位置x（T_a）を知る必要がある。これについては、式（２０）を満たすパラメータを使った軌道は、式（２１）を満たす。

【0096】

【数36】

・・・（２１）

【0097】

このように、加速区間と減速区間では、式（１７）の軌道を用い、定速区間では、式（５）に従って移動させる。

【0098】

以下に、制振加減速軌道の具体例を示す。

【0099】

移動時間Tを1/6sとし、抑制すべき周波数（共振周波数）を

【数37】

とする。

【0100】

従って、

【数38】

である。抑制すべき周波数が、１つであるので、整形用に加える信号の角周波数は、n=m+1=2種類必要となり、これをω₁=18πおよびω₂=24πrad/sとする。ω_T=12πrad/sであり、制振加減速軌道は、式（２２）で記述できる。

【0101】

【数39】

・・・（２２）

【0102】

式（１５）で説明したようにA^*_h〜D^*_hおよびA_h,k〜D_h,kを計算すると、

【数40】

となる。これを式（２０）に代入すると、連立方程式である式（２３）が得られる。

【0103】

【数41】

・・・（２３）

【0104】

式（２３）を解くと、式（２４）に示されるパラメータの値が得られる。

【0105】

【数42】

・・・（２４）

【0106】

このパラメータを式（２２）に代入すると、式（２５）に示される制振加減速軌道が定まる。

【0107】

【数43】

・・・（２５）

【0108】

式（２５）を２階微分し、式（１２）から振幅スペクトルF（ω）を求めると、式（２６）に示されるようになる。

【0109】

【数44】

・・・（２６）

【0110】

式（２６）では、ωに

【数45】

を代入すると、Ｆ(ω)＝０となることから、振動は抑制されている。

【0111】

すなわち、

【数46】

が抑制されている。

【0112】

また、終端速度を指定した制振加減速軌道の具体例を示す。

【0113】

移動時間Tを1/9sとし、終端速度

【数47】

に達する制振加減速軌道を算出する。

【0114】

抑制すべき周波数（共振周波数）を

【数48】

とする。

【0115】

すなわち、抑制すべき角周波数は、

【数49】

とし、ω₁=18πおよびω₂=24πrad/sの正弦波を整形用に加える。ω_T=18πrad/sであり、制振加減速軌道は、式（２７）で記述できる。

【0116】

【数50】

・・・（２７）

【0117】

A_h,k,B_h,k,C_h,k,D_h,k等を計算して、式（２０）に代入すると、式（２８）が得られる。

【0118】

【数51】

・・・（２８）

【0119】

式（２８）を解くと、式（２９）に示されるパラメータの値が得られる。

【0120】

【数52】

・・・（２９）

【0121】

従って、制振加減速軌道は、式（３０）で示される。

【0122】

【数53】

・・・（３０）

【0123】

式（３０）を２階微分し、振幅スペクトルF（ω）を求めると、式（３１）に示されるようになる。

【0124】

【数54】

・・・（３１）

【0125】

式（３１）では、ωに

【数55】

を代入すると、Ｆ(ω)＝０となることから、振動は抑制されている。

【0126】

すなわち、

【数56】

が抑制されている。

【0127】

また、式（２８）のＴにT_aを代入するとT_a=1/9sにおける位置が

【数57】

となる。

【0128】

以上のように、パラメータ生成部１０２は、基準距離について、予め定めた移動時間毎に、可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを生成する。より具体的には、例えば、パラメータ生成部１０２は、1mである基準距離について、1ms〜300msまでの、1ms刻みの移動時間毎に、式（８）に含まれるc₀,c₁,a_T,b_T,a_k,b_k(k=1,・・・,n)であるパラメータを計算する。より詳細には、例えば、パラメータ生成部１０２は、共振周波数からスペクトル条件を決定し、式（１５）で説明したようにA^*_h〜D^*_hおよびA_h,k〜D_h,kを計算し、これを式（１５）に代入する。パラメータ生成部１０２は、A^*_h〜D^*_hおよびA_h,k〜D_h,kなどが代入された式（１５）の線形代数方程式を解くことで、c₀,c₁,a_T,b_T,a_k,b_k(k=1,・・・,n)であるパラメータを生成する。

【0129】

さらに、パラメータ生成部１０２は、得られたパラメータを式（８）に代入して、制振加減速軌道を求める。そして、パラメータ生成部１０２は、求めた制振加減速軌道における、最大速度と最大加速度を求める。

【0130】

また、パラメータ生成部１０２は、同様に、定速区間を含む軌道についての、パラメータを生成し、最大速度と最大加速度を求める。

【0131】

パラメータ生成部１０２は、基準距離について、予め定めた移動時間毎に、基準距離について、予め定めた移動時間毎に、その場合の制振加減速軌道における、最大速度および最大加速度と共に、生成したパラメータをデータベース１０３に供給する。

【0132】

データベース１０３は、パラメータ生成部１０２から供給されたパラメータを、基準距離について、予め定めた移動時間毎に、その場合の制振加減速軌道における、最大速度および最大加速度に対応付けて、記憶する。

【0133】

図５は、データベース１０３に記憶されるパラメータの例を示す図である。例えば、データベース１０３は、1mである基準距離について、1ms〜300msまでの、1ms刻みの移動時間毎に、c₀,c₁,a_T,b_T,a_k,b_k(k=1,・・・,n)であるパラメータを、移動時間毎の制振加減速軌道における、最大速度および最大加速度に対応付けて、記憶する。

【0134】

パラメータ決定部１０４は、可動部の加速度軌道から共振周波数に対応する周波数成分を除いた軌道を計算するためのパラメータを決定する。パラメータ決定部１０４は、スケーリング部１２１を含む。スケーリング部１２１は、データベース１０３に記憶されるパラメータに、基準距離に対する移動距離の比率を乗じて、1m以外の移動距離に応じたパラメータを生成する。

【0135】

より具体的には、例えば、パラメータ決定部１０４は、移動距離、並びにダイピッカ１の定格最大速度および定格最大加速度を取得する。パラメータ決定部１０４は、移動距離、定格最大速度、および定格最大加速度を、オペレータに操作されるキーボード１５である入力部から取得するようにしてもよいし、予め記憶部若しくはバッファまたはプログラムのパラメータとして格納されているデータを読み出すことにより、移動距離、定格最大速度、および定格最大加速度を取得するようにしてもよい。

【0136】

パラメータ決定部１０４は、データベース１０３から、移動時間毎のパラメータを、最大速度および最大加速度と共に読み出す。

【0137】

パラメータ決定部１０４のスケーリング部１２１は、1mである基準距離に対する移動距離の比率を、読み出したパラメータ、最大速度、および最大加速度に乗じる。パラメータ決定部１０４は、定格最大速度および定格最大加速度以下の最大速度および最大加速度に対応付けられたパラメータを決定する。

【0138】

なお、パラメータ決定部１０４は、共振周波数からスペクトル条件を決定し、式（１５）で説明したようにA^*_h〜D^*_hおよびA_h,k〜D_h,kを計算し、これを式（１５）に代入して、代入された式（１５）の線形代数方程式を解くことで、c₀,c₁,a_T,b_T,a_k,b_k(k=1,・・・,n)であるパラメータを計算することで、パラメータを決定するようにしてもよい。

【0139】

軌道計算部１０５は、決定されたパラメータから軌道を計算する。すなわち、例えば、軌道計算部１０５は、パラメータ決定部１０４において決定されたパラメータを式（８）に代入して、制振加減速軌道を計算する。

【0140】

分解加速度制御部１０６は、分解加速度制御により、計算された制振加減速軌道からサーボアンプユニット５１−１および５１−２にトルクを指示する。

【0141】

より具体的には、サーボモータ５２−１、ボールネジ５３−１、リニアガイド５４−１、サーボモータ５２−２、ボールネジ５３−２、およびリニアガイド５４−２などからなる駆動系の支配方程式（運動方程式）に部分的フィードバック線形化を適用し、仮想的な操作量u111およびu131を導入すると、例えば、式（３２）が得られる。

【0142】

【数58】

・・・（３２）

【0143】

式（３２）から、操作量（モータトルク）は、式（３３）により求められる。

【0144】

【数59】

・・・（３３）

【0145】

次に、図６のフローチャートを参照して、パラメータの記憶の処理を説明する。ステップＳ１１において、共振周波数取得部１０１は、制御対象となる機器（この場合、ダイピッカ１）の共振周波数を取得する。

【0146】

例えば、共振周波数取得部１０１は、A/D変換カードである入力部に接続されている変位センサであって、ダイピッカ１の筐体の振動の変位を測定するための変位センサ（図示せず）からの信号を取得し、この信号からダイピッカ１の筐体の時間的に変位を求めることにより、ダイピッカ１の共振周波数を取得する。なお、共振周波数の取得は、ダイピッカ１が設置される床の剛性などにより変化するので、ダイピッカ１を設置した後に行うことが好ましい。

【0147】

また、共振周波数取得部１０１は、共振周波数を推定することにより、共振周波数を取得するようにしてもよく、また、オペレータによって操作されたキーボード１５などから、データとして供給された共振周波数を取得するようにしてもよい。

【0148】

ステップＳ１２において、パラメータ生成部１０２は、基準距離について、予め定めた移動時間毎に、ステップＳ１１の手続きで取得された共振周波数における振幅スペクトルが０となる軌道を記述する一般式のパラメータを計算する。例えば、パラメータ生成部１０２は、基準距離（1m）について、1ms〜300msまでの、1ms刻みの移動時間毎に、式（８）に含まれるc₀,c₁,a_T,b_T,a_k,b_k(k=1,・・・,n)であるパラメータを計算して、生成する。ステップＳ１３において、パラメータ生成部１０２は、移動時間毎に、パラメータから最大速度および最大加速度を計算する。すなわち、パラメータ生成部１０２は、得られたパラメータを式（８）に代入して、制振加減速軌道を求めて、求めた制振加減速軌道における、最大速度と最大加速度を求める。

【0149】

パラメータ生成部１０２は、基準距離について、予め定めた移動時間毎の、パラメータおよび最大速度と最大加速度をデータベース１０３に供給する。

【0150】

ステップＳ１４において、データベース１０３は、図５を参照して説明したように、基準距離について、予め定めた移動時間毎の、パラメータおよび最大速度と最大加速度を記憶して、パラメータの記憶の処理は終了する。

【0151】

このように、データベース１０３に、基準距離について、移動時間毎に、パラメータおよび最大速度と最大加速度を記憶するようにしたので、パラメータを可動部の移動ごとに計算する必要がなくなる。

【0152】

図７は、駆動制御の処理を説明するフローチャートである。ステップＳ３１において、パラメータ決定部１０４は、オペレータに操作されるキーボード１５である入力部から入力される定格最大速度および定格最大加速度を取得するか、または、予め記憶部若しくはバッファなどにデータとして格納されているかまたはプログラムのパラメータとして格納されている値を読み出すことにより、定格最大速度および定格最大加速度を取得する。ステップＳ３２において、パラメータ決定部１０４は、データベース１０３から、必要なパラメータおよび最大速度と最大加速度を読み出す。

【0153】

ステップＳ３３において、パラメータ決定部１０４は、可動部が順次到達すべき位置を指示するシーケンスプログラム（加工プログラムの一例）から、次に可動部が到達すべき位置を読み出し、現在の可動部の位置から次の位置への移動距離を取得する。なお、コンピュータ１２がリアルタイムに次に可動部が到達すべき位置を計算し、パラメータ決定部１０４が、現在の可動部の位置からその計算された次の位置への移動距離を取得するようにしてもよい。

【0154】

ステップＳ３４において、パラメータ決定部１０４は、基準距離に対する移動距離の比率を、データベース１０３から読み出したパラメータおよび最大速度と最大加速度に乗じる。例えば、移動距離が1.5mである場合、基準距離は1mなので、パラメータ決定部１０４は、1.5をパラメータおよび最大速度と最大加速度に乗じる。ステップＳ３５において、パラメータ決定部１０４は、定格最大速度および定格最大加速度以下の最大速度および最大加速度となるパラメータを決定する。ここで、定格最大速度および定格最大加速度と比較される最大速度および最大加速度は、基準距離に対する移動距離の比率が乗じられたものであり、決定されるパラメータもまた基準距離に対する移動距離の比率が乗じられたものである。パラメータ決定部１０４は、決定したパラメータを軌道計算部１０５に供給する。決定されるパラメータは、所定の移動時間に対するものである。

【0155】

ステップＳ３６において、軌道計算部１０５は、決定されたパラメータから軌道を計算する。すなわち、例えば、軌道計算部１０５は、パラメータ決定部１０４において決定されたパラメータを式（８）に代入して、制振加減速軌道を計算する。

【0156】

図８は、抑制すべき周波数（共振周波数）を10.5Hzとした場合に、軌道計算部１０５において計算される、定速区間を含まない制振加減速軌道の例を示す図である。図８（Ａ）は、加速度軌道を示し、図８（Ｂ）は、速度軌道を示し、図８（Ｃ）は、位置軌道を示し、図８（Ｄ）は、式（２６）により求められる振幅スペクトルを示す。

【0157】

図８（Ａ）に示される加速度軌道と図１８（Ａ）に示されるＳ字加減速軌道の加速度軌道とを比較すると、独特の加減速パターンとなっていることがわかる。また、図８（Ｄ）から明らかなような、10.5Hzの振幅スペクトルが抑制されていることがわかる。

【0158】

図９は、抑制すべき周波数（共振周波数）を10.5Hzとした場合に、軌道計算部１０５において計算される、終端速度を指定した制振加減速軌道の例を示す図である。図９（Ａ）は、加速度軌道を示し、図９（Ｂ）は、速度軌道を示し、図９（Ｃ）は、位置軌道を示し、図９（Ｄ）は、式（３１）により求められる振幅スペクトルを示す。

【0159】

図９（Ａ）に示される加速度軌道と従来の加減速パターンとは明らかに異なっていることがわかる。また、図９（Ｄ）から明らかなような、10.5Hzの振幅スペクトルが抑制されていることがわかる。また、図９（Ｃ）から、T_a=1/9sにおける位置がT_a/2であることがわかる。

【0160】

図７に戻り、ステップＳ３７において、分解加速度制御部１０６は、分解加速度制御により、計算された軌道からサーボアンプユニット５１−１および５１−２にトルクを指示する。

【0161】

軌道は、移動時間について計算され、その計算された軌道に従って、分解加速度制御により、可動部の動きが制御されるので、タクトタイムが変化することはない。

【0162】

ステップＳ３８において、パラメータ決定部１０４は、可動部が次の位置に移動するか否かを判定し、次の位置に移動すると判定された場合、ステップＳ３３の手続きに戻り、次の軌道について、上述した処理を繰り返す。ステップＳ３８において、次の位置に移動しないと判定された場合、軌道を計算する必要はないので、駆動制御の処理は終了する。

【0163】

このように得られる制振加減速軌道とＳ字加減速軌道とを比較する。図１０は、制振加減速軌道とＳ字加減速軌道との例を比較する図である。図１１は、シミュレーションによる、制振加減速軌道とＳ字加減速軌道とにおける、筐体の振動の例を示す図である。図１０および図１１において、実線は、制振加減速軌道を示し、点線は、従来のＳ字加減速軌道を示す。

【0164】

図１０（Ａ）は、加速度軌道を示し、図１０（Ｂ）は、速度軌道を示し、図１０（Ｃ）は、位置軌道を示し、図１０（Ｄ）は、振幅スペクトルを示す。

【0165】

図１０から、制振加減速軌道において、Ｓ字加減速軌道に比較して、細かい加減速が行われていることがわかる。制振加減速軌道において、この場合の共振周波数である、13.3Hzの成分が押さえられているのに対して、Ｓ字加減速軌道においては、13.3Hzの成分が含まれている。

【0166】

図１１の、t=0sから0.2sに可動部の加減速による筐体の反動が現れている。従来のＳ字加減速軌道を利用すると、可動部の制止後に筐体の残留振動が見られるが、制振加減速軌道を利用すると、残留振動は現れない。可動部の１回の加減速で残留振動が抑制できるということは、その後再度加減速が行われても、残留振動と新たな振動とが助け合って振動の振幅が増大する、いわゆる共振現象が起きないことを意味する。

【0167】

このように、制振加減速軌道を用いることで、振動が抑制できる。

【0168】

ここで、振動そのものをシミュレーションにより求めて比較する。図１２は、傾きを単位として可動部をＳ字加減速軌道により移動させたときの振動を表す図である。図１２の縦軸は、ダイピッカ１（図１）の側板の鉛直方向の傾きの角度を示し、横軸は時間を示している。図１３は、傾きを単位として制振加減速軌道による振動を表す図である。図１２および図１３を比較すると、振動が明らかに低減していることがわかる。

【0169】

分解加速度制御により、制御すると説明したが、PID制御を用いるようにしてもよい。

【0170】

図１４は、Ｓ字加減速軌道を用いた場合、制振加減速軌道と分解加速度制御を用いた場合、および制振加減速軌道とPID制御を用いた場合の筐体の振動を比較する図である。図１４（Ａ）は、Ｓ字加減速軌道を用いた場合に生じる筐体の振動を示す図である。図１４（Ｂ）は、制振加減速軌道と分解加速度制御を用いた場合に生じる筐体の振動を示す図である。図１４（Ｃ）は、制振加減速軌道とPID制御を用いた場合に生じる筐体の振動を示す図である。

【0171】

制振加減速軌道と分解加速度制御を用いると、Ｓ字加減速軌道を用いた場合に比較すると、振動は極めて小さくなるが、制振加減速軌道とPID制御を用いた場合でも、Ｓ字加減速軌道を用いた場合に比較すると、振動は十分に小さくなる。

【0172】

図１５は、Ｓ字加減速軌道を用いた場合、制振加減速軌道と分解加速度制御を用いた場合、および制振加減速軌道とPID制御を用いた場合のそれぞれの、筐体の振動とタクトタイムとの関係を示す図である。図１５において、菱形は、Ｓ字加減速軌道を用いた場合の筐体の振動とタクトタイムを示し、四角は、制振加減速軌道と分解加速度制御を用いた場合の筐体の振動とタクトタイムを示し、三角は、制振加減速軌道とPID制御を用いた場合の筐体の振動とタクトタイムを示す。

【0173】

図１５から、同じタクトタイムであっても、制振加減速軌道と分解加速度制御を用いると、Ｓ字加減速軌道を用いた場合に比較すると、振動は極めて小さくなることがわかる。また、同じタクトタイムであっても、制振加減速軌道とPID制御を用いた場合、Ｓ字加減速軌道を用いた場合に比較すると、振動は十分に小さくなることがわかる。

【0174】

なお、アクチュエータにサーボモータを用いる場合を説明したが、アクチュエータにパルスモータを採用するようにしてもよい。図１６は、コンピュータ１２およびチップ供給部３３の構成の他の例を説明するブロック図である。コンピュータ１２には、パルスモータコントロールユニット１５１が、出力部７７として設けられる。

【0175】

チップ供給部３３のＸ軸は、パルスモータ１５２−１、ボールネジ５３−１、およびリニアガイド５４−１からなる。また、チップ供給部３３のＹ軸は、パルスモータ１５２−２、ボールネジ５３−２、およびリニアガイド５４−２からなる。ボールネジ５３−１、リニアガイド５４−１、ボールネジ５３−２、およびリニアガイド５４−２は、図３に示す場合と同様なので、その説明は省略する。

【0176】

パルスモータコントロールユニット１５１は、駆動するためのパルスをパルスモータ１５２−１およびパルスモータ１５２−２に供給する。パルスモータ１５２−１およびパルスモータ１５２−２の軸は、パルスモータコントロールユニット１５１からのパルス毎に、時計回りまたは反時計回りに、所定の角度だけ回転する。

【0177】

以上のように、新たなハードウェアを必要とせず、タクトタイムを変化させることなく、振動を抑制することができる。

【0178】

なお、直交座標系の直交ロボットを例に説明したが、円筒座標系、極座標系、または球座標系であってもよく、また、軸に対してリンクの角度を変化させる回転関節型を採用してもよい。また、いわゆるスカラロボットであってもよい。さらに、アクチュエータの駆動源は、いわゆる電動に限らず、油圧など他の駆動源を採用することもできる。

【0179】

また以上においては、式（８）により制振加減速軌道を計算するものとしたが、式（３４）を使って計算することもできる。速度軌道は、式（３４）の積分したもの、位置軌道は速度軌道をさらに積分したものを用いて計算することができる。

【0180】

【数60】

・・・（３４）

【0181】

式（３４）中、

【数61】

はPaley順序のWalsh 関数である。

【0182】

すなわち、

【数62】

の２進法展開｛ｘ_ｊ｝である式（３５）と、整数ｋの２進法展開｛ｋ_ｊ｝である式（３６）とを使って式（３７）のように表される。

【0183】

【数63】

・・・（３５）

【数64】

・・・（３６）

【数65】

・・・（３７）

【0184】

定速区間を含まない軌道では、移動時間をＴとすると、式（３８）で表される位置決めに関する境界条件を満たす。

【0185】

【数66】

・・・（３８）

【0186】

式（３４）で表される、パラメータを含む加減速軌道が、式（３８）で表される、位置決めに関する境界条件、および式（１３）で表される、スペクトル条件を満たすとき、そのパラメータは、式（３９）を解いて求めることができる。

【0187】

【数67】

・・・（３９）

【0188】

ここで、

【数68】

として、

【数69】

である。

【0189】

定速区間を含まない軌道では、加速時間をＴ_ａとすると、加速区間の加速度軌道は、Ｔ＝Ｔ_ａとした場合の式（３４）であり、境界条件は、式（４０）となる。

【0190】

【数70】

・・・（４０）

【0191】

式（３４）で表される、パラメータを含む加減速軌道が、式（４０）で表される、位置決めに関する境界条件、および式（１３）で表される、スペクトル条件を満たすとき、そのパラメータは、式（４１）を解いて求めることができる。

【0192】

【数71】

・・・（４１）

【0193】

上述した一連の処理は、ハードウェアにより実行することもできるし、ソフトウエアにより実行することもできる。一連の処理をソフトウエアにより実行する場合には、そのソフトウエアを構成するプログラムが、専用のハードウェアに組み込まれているコンピュータ、または、各種のプログラムをインストールすることで、各種の機能を実行することが可能な、例えば汎用のパーソナルコンピュータなどに、プログラム記録媒体からインストールされる。

【0194】

図１９は、コンピュータ１２のハードウェアの構成例を示すブロック図である。

【0195】

コンピュータ１２において、CPU（Central Processing Unit）７１，ROM（Read Only Memory）７２，RAM（Random Access Memory）７３は、バス７４により相互に接続されている。

【0196】

バス７４には、さらに、入出力インタフェース７５が接続されている。入出力インタフェース７５には、デジタル入力カード、キーボード１５、A/D（analog to digital）変換カード、マウス、マイクロホンなどよりなる入力部７６、モーションコントローラ、デジタル出力カード、D/Aカード、モニタ１４、スピーカなどよりなる出力部７７、ハードディスクや不揮発性のメモリなどよりなる記憶部７８、ネットワークインタフェースなどよりなる通信部７９、磁気ディスク、光ディスク、光磁気ディスク、或いは半導体メモリなどのリムーバブルメディア８１を駆動するドライブ８０が接続されている。

【0197】

以上のように構成されるコンピュータでは、CPU７１が、例えば、記憶部７８に記憶されているプログラムを、入出力インタフェース７５及びバス７４を介して、RAM７３にロードして実行することにより、後述の一連の処理が行われる。

【0198】

コンピュータ（CPU７１）が実行するプログラムは、例えば、磁気ディスク（フレキシブルディスクを含む）、光ディスク（CD-ROM(Compact Disc-Read Only Memory),DVD(Digital Versatile Disc)等）、光磁気ディスク、もしくは半導体メモリなどよりなるパッケージメディアであるリムーバブルメディア８１に記録して、あるいは、ローカルエリアネットワークなどの伝送媒体を介して提供される。

【0199】

そして、プログラムは、リムーバブルメディア８１をドライブ８０に装着することにより、入出力インタフェース７５を介して、記憶部７８に記憶することで、コンピュータにインストールすることができる。また、プログラムは、有線または無線の伝送媒体を介して、通信部７９で受信し、記憶部７８に記憶することで、コンピュータにインストールすることができる。その他、プログラムは、ROM７２や記憶部７８にあらかじめ記憶しておくことで、コンピュータにあらかじめインストールしておくことができる。

【0200】

なお、コンピュータが実行するプログラムは、本明細書で説明する順序に沿って時系列に処理が行われるプログラムであっても良いし、並列に、あるいは呼び出しが行われたとき等の必要なタイミングで処理が行われるプログラムであっても良い。

【0201】

また、本発明の実施の形態は、上述した実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能である。

【符号の説明】

【0202】

１２…コンピュータ、５１−１および５１−２…サーボアンプユニット、５２−１および５２−２…サーボモータ、７１…CPU、７２…ROM、７３…RAM、７８…記憶部、８１…リムーバブルメディア、１０１…共振周波数取得部、１０２…パラメータ生成部、１０３…データベース、１０４…パラメータ決定部、１０５…軌道計算部、１０６…分解加速度制御部、１２１…スケーリング部、１５１…パルスモータコントロールユニット、１５２−１および１５２−２…パルスモータ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】