JP3063562B2 - 移動体の速度制御方法およびその制御装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は移動体の速度制御方法お
よびその制御装置に係り、特に、電子部品実装機、半導
体製造装置、ＸＹステージなどでの位置決めにおいて振
動を発生することなく減速させ目標位置で停止させるこ
とができる移動体の速度制御方法およびその制御装置に
関するものである。

【０００２】

【従来の技術】従来、特開平１−２６０５１０号公報や
特開平４−３４２００３号公報に記載されるように、目
標位置を移動開始時点で１度だけ出力し、目標位置近傍
まではある速度パターンを用いて速度制御を行ない、目
標位置近傍になってから停止するための位置制御系に切
替るポイント・ツウ・ポイント（ＰＴＰ）制御を採用し
ている。

【０００３】上記特開平１−２６０５１０号公報では、
特に、位置制御に切替える前における速度指令を位置偏
差の関数とすることによって、減速度を時間で微分した
ジャ−クに不連続が生じないようにして位置制御に切替
わった時の振動を抑えることが記載されている。また、
特開平４−３４２００３号公報には速度制御と位置制御
におけるそれぞれの指令速度が等しくなる位置で制御系
を切替えることが記載されている。

【０００４】

【発明が解決しようとする課題】上記特開平１−２６０
５１０号公報における従来技術では、位置制御に切替え
る前までの位置偏差は考慮されているが切替後の位置制
御における位置偏差の存在が考慮されていない。

【０００５】また、上記特開平４−３４２００３号公報
における従来技術は、目標位置が制御の途中で変更され
た場合の制御を工夫したもので、速度制御から位置制御
に切替える際の減速度の大きさの変化は考慮されていな
い。

【０００６】即ち、上記両従来技術は減速指令から位置
偏差比例速度指令に切替える点での減速度の連続性の点
について配慮がされておらず、これらの制御系を用いた
場合、位置偏差比例速度指令に切替わる点でトルク変動
が大きく、このため振動を発生するという問題があっ
た。

【０００７】それゆえ本発明の目的は、減速度、トル
ク、さらにはジャ−クの変化を小さく又は、零にし振動
を発生することのない移動体の速度制御方法およびその
制御装置を提供することにある。

【０００８】

【課題を解決するための手段】上記の目的を達成する本
発明の特徴は、位置偏差をｅ、等減速運動の減速度をａ
２、位置制御系比例ゲインをｋｐ、減速指令から位置偏
差比例速度指令への切替え点における位置偏差をｃ１と
して、上記位置偏差ｅが上記位置偏差ｃ１より大きいと
ころでの速度制御系の減速指令ｖ２を、

【０００９】

【数５】

【００１０】但し、Ｓｉｇｎ（ｅ）は位置偏差ｅの符号
を示す。 で与えるようにしたことにある。

【００１１】

【作用】上記のように減速指令が設定されることによ
り、目標位置近傍までの速度制御系による制御指令の速
度と減速度が目標位置近傍からの位置制御系における速
度指令の速度と減速度にそれぞれ一致する。これによっ
て、電動機に与えられるトルク指令値が連続となり、従
来、位置制御領域に入った後に生じていた振動を防止す
ることができる。

【００１２】

【実施例】以下、本発明を図に示す一実施例に基づいて
説明する。図１は本発明になる位置決め制御装置の一実
施例を示すブロック図である。図１において、コントロ
ーラ１は移動のパラメータである加速度、減速度、最高
速度、および位置決め完了幅、などを設定する移動パラ
メータ設定器２、減速指令から位置偏差比例速度指令へ
の切替え点における位置偏差ｃ１を設定する切替え位置
偏差設定器３、目標位置指令入力器５、加速時速度指令
値発生器６、最高速度指令値発生器７、減速時速度指令
値発生器８、指令値選択器１１、位置偏差比例ゲイン乗
算器９、切替え器１２、速度制御装置１４、加算器１
０、１３を備えている。このコントローラ１からの出力
である速度指令値は、速度アンプ１５を介してモータ１
６に伝えられる。

【００１３】モータ１６は、駆動対象である負荷１７と
接続され、モータ１６の回転状態はエンコーダ１８で検
出される。エンコーダ１８の出力は、速度演算器１９と
位置演算器２０に入力され、速度演算器１９の出力は加
算器１３に接続されて速度フィードバック信号として用
いられる。位置演算器２０の出力は加算器１０に入力さ
れて位置フィードバック信号として用い、減速時速度指
令値発生器８や切替え器１２の切替え位置情報として活
用している。

【００１４】この実施例において、目標位置指令入力器
５から位置ｘｆへの移動指令値が加算器１０に与えられ
ると、加算器１０ではエンコーダ１８のパルス信号を位
置演算器２０で変換した現在位置ｘｐがフィードバック
されて加算器１０に入力される。このとき、現在位置ｘ
ｐは負号を付けられ、加算器１０内で下式で示す位置偏
差ｅを得る計算が行われ、加算器１０より位置偏差ｅが
出力される。

【００１５】

【数６】 ｅ＝ｘｆ−ｘｐ 起動時の位置偏差ｅｏは、現在位置ｘｐをｘｏとすれ
ば、下式で与えられる。

【００１６】

【数７】 ｅｏ＝ｘｆ−ｘｏ 加速時速度指令値発生器６は、加速度（等加速運動の加
速度）をａ１、起動時をｔ＝０とした経過時間をｔで表
わすと加速時速度指令値ｖ１を次式で算出して出力す
る。

【００１７】

【数８】 ｖ１＝ａ１・ｔ 最高速度指令値発生器７は、最高速度ｖ４を次式で出力
する。

【００１８】

【数９】 ｖ４＝ｖｍａｘ 減速時速度指令値発生器８は、位置偏差比例ゲインをｋ
ｐ、減速指令から位置偏差比例速度指令への切替え点に
おける位置偏差をｃ１、減速時の加速度（等減速運動で
の減速度）の絶対値をａ２とすると減速時速度指令値ｖ
２を次式で算出して出力する。

【００１９】

【数１０】

【００２０】但し、Ｓｉｇｎ（ｅ）は位置偏差ｅの符号
を示す。

【００２１】減速時速度指令値発生器８では入力された
位置偏差ｅについて数１０の計算が行われ、減速時速度
指令値発生器８から減速時速度指令値ｖ２が出力され
る。

【００２２】一方、位置制御系は、位置偏差比例ゲイン
乗算器９で次式のように位置偏差ｅに位置偏差比例ゲイ
ンｋｐをかけて位置偏差比例速度指令ｖ３を出力する。

【００２３】

【数１１】 ｖ３＝ｋｐ・ｅ これら４つの速度指令値ｖ１〜ｖ４の大きさの関係を図
２に示す。図２において、横軸が位置、縦軸が速度を示
している。

【００２４】加速時速度指令値発生器６から出力された
速度指令値ｖ１と最高速度指令値発生器７から出力され
た速度指令値ｖ４および減速時速度指令値発生器８から
出力された速度指令値ｖ２は、指令値選択器１１に入力
される。この指令値選択器１１では、入力された速度指
令値３つを比較して最も小さい速度指令値を選択してｖ
ｍｉｎとして出力する。即ち、初期位置ｘｏにおいて
は、先ず、加速時速度指令値発生器６からの速度指令値
ｖ１が最も小さいため、このｖ１が選択される。位置が
図２においてｘａよりも目標値に近づくと最高速度指令
値値発生器７からの出力の速度指令値ｖ４が最も小さく
なる。このｖ１からｖ４への切替え点ｘａは数式８、９
を演算することにより、下式で表される。

【００２５】

【数１２】

【００２６】最高速度ｖ４で移動していくと、次に、位
置がｘｍになったとき以降はｖ４よりもｖ２が小さくな
り、これ以降はｖ２が最も小さいため、指令値選択器１
１からはｖ２が出力される。

【００２７】一方、位置偏差比例ゲイン乗算器９からの
出力は数式１１に示した一般的な位置制御系の速度出力
ｖ３となる。この位置制御系の速度指令値ｖ３と指令値
選択器１１から出力される減速時速度指令値発生器８か
らの速度指令値ｖ２が等しくなる点での位置偏差ｃ１で
ある。

【００２８】そして、図２における位置ｘｄは目標位置
ｘｆから位置偏差ｃ１を引いたものとして、下式で表さ
れることになる。

【００２９】

【数１３】 ｘｄ＝ｘｆ−ｃ１ 図３に、図２の目標位置近傍の拡大図を示す。この図３
で分かるように、ｖ２の曲線はｘｄでｖ３に接してい
る。図１の切替え器１２では指令値選択器１１からの速
度指令値ｖｍｉｎと位置偏差比例ゲイン乗算器９からの
速度指令値ｖ３とを受けて、位置偏差ｅが切替え位置偏
差設定器３に設定されたｃ１より大きい間はｖｍｉｎを
出力し、小さくなったらｖ３を出力する。一度ｖ３を出
力した後には、位置決め完了までｖ３を出力し続ける。
位置偏差比例ゲイン乗算器９から切替え器１２を経て速
度制御装置に至る経路は位置制御系を構成しているの
で、切替え器１２がｖｍｉｎを出力している間は速度制
御系による制御が行なわれ、切替え器１２がｖ３を出力
すると位置制御系に切替わって制御が続行されることに
なる。

【００３０】このようにして与えられた速度指令値は、
加算器１３に入力されエンコーダ１８の信号を基に速度
演算器１９で算出された現在速度に負号を付けられたも
のと加えられ、速度アンプ１５に信号が送られる。速度
アンプ１５はこの信号に基づいてモータ１６を制御し、
負荷１７の移動を行う。この時の運動の状態をエンコー
ダ１８が検出し、速度演算器１９、位置演算器２０に信
号を送る。位置決め完了域に入ったことは、偏差信号を
基にコントローラ１が判断する。このようにして位置制
御系へ切替えることで振動の少ない、整定の早い制御を
行うことができる。

【００３１】以下、本発明による減速時速度指令値発生
器８で出力される速度指令値ｖ２を用いると、速度制御
系による制御から位置制御系への切替えの際に速度や加
速度が連続になる理由について説明する。

【００３２】移動体（負荷１７）は目標位置ｘｆで停止
することが求められているため、最終的に位置制御系で
停止位置に保持される。そのため、どこかの地点で位置
制御系に切替える必要が生じる。この切替え時の速度と
加速度が連続であることは、停止時の振動の減少に大き
な影響を持つ。

【００３３】ここで位置制御系の速度指令値ｖ３は次の
微分方程式で表現される。

【００３４】

【数１４】 ｖ３＝ｄｅ／ｄｔ＝ｋｐ・ｅ 上式を時間ｔについて微分すると、負荷１７の減速時に
おける実際の加速度α３は次式で表現されるものとな
る。

【００３５】

【数１５】 α３＝ｄｖ３／ｄｔ＝ｄｖ３／ｄｅ・ｄｅ
／ｄｔ＝ｋｐ²・ｅ 一方、前記数式１０を直接微分して減速時の加速度α２
（等減速運動での減速度ａ２と異なることに要注意）の
大きさはｅ＞０に対して下式で表されるものとなる。

【００３６】

【数１６】

【００３７】減速時の実加速度α２、α３の変化を図４
に示す。

【００３８】数式１６において、切替え点ｘｄでの位置
偏差ｅにｃ１を代入することによって、切替え点ｘｄに
おけるｖ２の減速時の実加速度α２の大きさは、下記の
数式で示されるものとなる。

【００３９】

【数１７】 α２（ｃ１）＝ｃ１・ｋｐ² 一方、切替え点ｘｄにおけるｖ３の減速時の加速度α３
の大きさは、数式１５に切替え点ｘｄでの位置偏差ｃ１
を代入して下式で表されるものとなる。

【００４０】

【数１８】 α３（ｃ１）＝ｃ１・ｋｐ² 数式１７と数式１８を対比して一目瞭然であるが、切替
え点ｘｄにおいてｖ２とｖ３の減速時のそれぞれの加速
度α２、α３の大きさは等しくなっている。さらに、数
式１０と数式１４のそれぞれに切替え点ｘｄでの位置偏
差ｃ１を代入してみると、切替え点ｘｄにおいてｖ２と
ｖ３の値も等しくなっていることが分かるであろう。

【００４１】本発明に従って、数式１０で表される速度
ｖ２を出力する減速時速度指令値発生器８を用いること
により減速度連続速度制御から位置制御に連続的に切替
わることができ、また、速度も等しいことから、切替え
時にトルク変動はなく振動は全く発生しない。トルク変
動、振動を抑えることと位置偏差比例ゲインｋｐは無関
係となるので、位置制御系の位置偏差比例ゲインｋｐを
高く設定して整定時間を短縮することも同時に達成でき
る。

【００４２】図５に、コントローラ１の動作をマイクロ
コンピュータのソフトウエアによって実現した場合の実
施例を示す。

【００４３】なお、コントローラ１以外の部分の構成や
動作は図１に示す実施例と同様として、以下コントロー
ラ１の処理フロ−を説明する。先ず、ステップ５１で移
動の運動を決定する各種パラメータを図示していないキ
−ボ−ドの入力手段でモニタ表示画面などを利用した
り、図示していない記憶装置内に格納されている移動デ
−タを読み込むことなどによって設定する。ここでは、
加速時の加速度ａ１、等減速運動の減速度ａ２、最高速
度ｖｍａｘ、最終位置での位置決め完了幅ε、位置偏差
比例ゲインｋｐおよび切替え位置偏差ｃ１を設定する。
次に、ステップ５２で移動目標位置指令値ｘｆを設定す
る。この目標位置に関しては、それぞれのシステムによ
り、相対移動距離を設定するシステムでも、移動位置の
絶対位置を設定するシステムでもよい。次に、ステップ
５３でマイクロコンピュータの外部入力装置（エンコ−
ダや位置演算器など）からの信号を基に現在位置ｘｐの
演算を行う。この現在位置と目標位置の差として位置偏
差ｅをステップ５４で算出する。次に、ステップ５５で
位置偏差ｅが位置決め完了幅ε内であるかどうかを判定
し、範囲内ならばステップ５６に進んで位置決め完了処
理を行う。範囲内でなければ、ステップ５７に行って、
加速時速度指令値ｖ１を前記の数式８により算出する。
そしてステップ５８で最高速度ｖ４を前記の数式９によ
り指定する。さらに、減速時速度指令値ｖ２を前記の数
式１０によりステップ５９で算出する。そしてステップ
６０で先のステップ５７〜５９で得た各速度ｖ１、ｖ
２、ｖ４の速度指令値のうち、最も小さい値を選択しｖ
ｍｉｎの変数に代入する。

【００４４】一方、ステップ６１で位置制御系での位置
決め用速度指令値ｖ３を前記の数式１１で算出する。次
のステップ６２において、位置偏差量がステップ５１で
設定されている切替え位置偏差ｃ１より大きいかどうか
を判定し、大きい場合にはステップ６３に進んでｖｍｉ
ｎを、小さい場合にはステップ６４に行ってｖ３を速度
指令値として、次のステップ６５で図示していないイン
ターフェースを通して速度アンプ１５に出力する。その
後、ステップ５３に戻って、現在位置の取込みを行い現
在位置の演算以下を繰り返えし実行し、ステップ５５で
位置偏差ｅが位置決め完了幅ε内になることにより、ス
テップ５６に進んで位置決め完了処理を行う。

【００４５】以上のように、コントローラ１の動作をマ
イクロコンピュータのソフトウエアで構成することによ
り、図１の実施例と同様の、整定時間の短縮、速度制御
系から位置制御系への切替え時の振動発生の抑制ができ
ると共に、各種パラメータの変更が容易にできる。

【００４６】上記の各実施例では、目標位置近傍までの
速度制御系による制御指令の速度と減速度が目標位置近
傍からの位置制御系における速度指令の速度と減速度に
それぞれ一致するところで切替えを行っている。実機で
は、アナログ処理のものでは製作上の加工精度などによ
って、また、ディジタル処理のものではサンプリング周
期の都合などで一致点が発生しなくなる恐れがある。そ
こで、速度制御系による制御指令の速度ｖ２と位置制御
系における速度指令の速度ｖ３の絶対差（｜ｖ２−ｖ３
｜）がある任意に設定する小さな値ｄより小さくなった
ら（｜ｖ２−ｖ３｜＜ｄ）、速度制御系による制御指令
の速度ｖ２が位置制御系による制御指令の速度ｖ３に一
致したと見做して切替えを行うようにしておくことが良
い。

【００４７】図１の実施例の変形として、図６に示すよ
うに、図１の構成において減速時速度指令値発生器８の
代わりに下式で示す減速時速度指令ｖ２を出力する減速
時速度指令値発生器９１を用いてもよい。

【００４８】

【数１９】

【００４９】但し、Ｓｉｇｎ（ｅ）は位置偏差ｅの符号
を示す。

【００５０】この実施例では、切替え点における位置偏
差ｃ１を限りなく小さくし、その極限としてｃ１＝０と
するものである。これは、切替えを行おうとする矢先に
目標位置に到達している、つまり、減速開始から停止ま
で切替えなしの構成である。

【００５１】この実施例における目標位置までの速度制
御系による減速度α２とジャ−クｊ２はそれぞれ下式で
示すものとなり、速度ｖ２、減速度α２とジャ−クｊ２
の事例を各々図７乃至図９に示した。

【００５２】

【数２０】

【００５３】

【数２１】

【００５４】図８、図９では、減速度α２とジャ−クｊ
２は目標位置ｘｆまで滑らかに変化し、目標位置ｘｆに
おいてそれらの値は零になっていて、移動体は振動を生
ずることなく目標位置ｘｆで停止している。移動体には
停止動作において振動がなく当然のことながら残留振動
を伴わないので、整定時間の短縮が図れている。

【００５５】また、図６に示す実施例におけるコントロ
ーラ１の動作を図５に示すようなマイクロコンピュータ
のソフトウエアで実行することも可能である。

【００５６】

【発明の効果】以上説明したように本発明によれば、目
標位置近傍で振動を無くすことができる。さらには、位
置制御系のゲインを高く設定することによって整定時間
を短縮できる。そして、振動の防止や位置決め整定時の
時間短縮により、負荷（移動体）の移動時間を短縮する
ことができる。

【図面の簡単な説明】

【図１】本発明になる位置決め制御装置の一実施例を示
すブロック図である。

【図２】図１に示した実施例における移動体（負荷）の
位置と速度の関係を示す図である。

【図３】図２における移動体（負荷）の目標位置近傍で
の拡大図である。

【図４】図１に示した実施例における移動体（負荷）の
減速時における位置と加速度の関係を示す図である。

【図５】図１に示した実施例をソフト処理で実現した他
の実施例におけるコントロ−ラの処理フローを示す図で
ある。

【図６】本発明になる位置決め制御装置の他の実施例を
示すブロック図である。

【図７】図６に示した実施例における移動体（負荷）の
位置と速度の関係を示す図である。

【図８】図６に示した実施例における移動体（負荷）の
減速時における位置と加速度の関係を示す図である。

【図９】図６に示した実施例における移動体（負荷）の
減速時における位置とジャ−クの関係を示す図である。

【符号の説明】

１…コントローラ、２…移動パラメーター設定器、３…
切替え位置偏差設定器、５…目標位置指令入力器、６…
加速時速度指令値発生器、７…最高速度指令値発生器、
８…減速時速度指令値発生器、９…位置偏差比例ゲイン
乗算器、１０…加算器、１１…指令値選択器、１２…切
替え器、１３…加算器、１４…速度制御装置、１５…速
度アンプ、１６…モータ、１７…負荷、１８…エンコー
ダ、１９…速度演算器、２０…位置演算器、９１…減速
時速度指令値発生器、９２…指令値選択器

フロントページの続き (56)参考文献 特開 平８−106326（ＪＰ，Ａ) 特開 昭61−172281（ＪＰ，Ａ) 特開 平４−342003（ＪＰ，Ａ) 特開 平４−232677（ＪＰ，Ａ) 特開 平１−260510（ＪＰ，Ａ) 特開 昭53−106104（ＪＰ，Ａ) 特開 平５−40529（ＪＰ，Ａ) 特開 昭61−183719（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁷，ＤＢ名) G05D 13/62 G05B 7/02 G05D 3/12 305 G05D 3/12 306

Claims (5)

(57)【特許請求の範囲】
1. 【請求項１】速度制御系の減速指令から位置制御系の位
   置偏差比例速度指令に切替えて移動体の位置制御を行な
   う移動体の速度制御方法において、 位置偏差をｅ、等減速運動の減速度をａ２、位置制御系
   比例ゲインをｋｐ、上記減速指令から上記位置偏差比例
   速度指令への切替え点における位置偏差をｃ１として、
   上記位置偏差ｅが上記位置偏差ｃ１より大きいところで
   の速度制御系の減速指令ｖ２を、 【数１】 但し、Ｓｉｇｎ（ｅ）は位置偏差ｅの符号を示す。 で与えることを特徴とする移動体の速度制御方法。
2. 【請求項２】請求項１に記載のものにおいて、上記減速
   指令から上記位置偏差比例速度指令への切替えは減速指
   令の速度と位置偏差比例速度指令の速度が一致する時点
   および上記両速度の差が任意のある値よりも小さくなっ
   た時点のいずれかで行うことを特徴とする移動体の速度
   制御方法。
3. 【請求項３】移動体の目標位置と現在位置の位置偏差を
   基に速度指令を得て移動体の位置制御を行なう移動体の
   速度制御方法において、 位置偏差をｅ、等減速運動の減速度をａ２、位置偏差に
   対する比例ゲインをｋｐとして、減速開始から停止まで
   の速度制御系の減速指令ｖ２を、 【数２】 但し、Ｓｉｇｎ（ｅ）は位置偏差ｅの符号を示す。 で与えることを特徴とする移動体の速度制御方法。
4. 【請求項４】速度制御系の減速指令から位置制御系の位
   置偏差比例速度指令に切替えて移動体の位置制御を行な
   う移動体の速度制御装置において、 移動開始位置から目標位置まで移動する移動体の現在位
   置を検出する手段と、目標位置と現在位置の差である位
   置偏差を生成する手段と、位置偏差を基に減速時速度指
   令値を発生する手段と、位置偏差を基に位置偏差比例速
   度指令を発生する手段と、上記両発生手段が発生する速
   度が一致する時点および上記両発生手段が発生する速度
   の差が任意のある値よりも小さくなった時点のいずれか
   で減速時速度指令から位置偏差比例速度指令に切替えを
   行なう手段を備え、上記位置偏差を基に減速時速度指令
   値を発生する手段は位置偏差をｅ、等減速運動の減速度
   をａ２、位置制御系比例ゲインをｋｐ、上記減速指令か
   ら上記位置偏差比例速度指令への切替え点における位置
   偏差をｃ１として、 【数３】 但し、Ｓｉｇｎ（ｅ）は位置偏差ｅの符号を示す。 の速度指令ｖ２を発生するものであることを特徴とする
   移動体の速度制御装置。
5. 【請求項５】移動体の目標位置と現在位置の位置偏差に
   ついて速度指令を得て移動体の位置制御を行なう移動体
   の速度制御装置において、 移動開始位置から目標位置まで移動する移動体の現在位
   置を検出する手段と、目標位置と現在位置の差である位
   置偏差を生成する手段と、位置偏差を基に減速時速度指
   令値を発生する手段を備え、該減速時速度指令値を発生
   する手段は位置偏差をｅ、等減速運動の減速度をａ２、
   位置偏差に対する比例ゲインをｋｐとして、減速開始か
   ら停止までの速度制御系の減速指令ｖ２を、 【数４】 但し、Ｓｉｇｎ（ｅ）は位置偏差ｅの符号を示す。 で与えることを特徴とする移動体の速度制御装置。