JP2655649B2 - サーボ系の位置決め制御方式 - Google Patents

Description

【発明の詳細な説明】 ［産業上の利用分野］ 本発明は例えば複数制御装置等のサーボ系の位置決め
制御方式に関する。

［従来の技術］ 従来の数値制御装置のサーボ系の位置決め制御方式に
おいては、目標位置又はそれまでの移動量、並びに、移
動速度（最高速度）の指令データを受け取ると、目標位
置計算部では、所定の制御周期毎の目標となる移動量等
のデジタル値を計算してサーボ機構に与えて動作を制御
する。この場合目標位置までの移動の間に加減速を行う
ときは、第６図に示すように演算プログラム又は加減速
テーブルで与えられる加速勾配に従つて、制御周期毎に
逐次加速し、指定された移動速度（最高速度）に達する
と、一定速度移動を行う。

この一定速度領域で、制御周期毎の移動量を一定速度
区間距離から順次減算し、目標位置までの残りの移動量
が、所定の減速勾配（加速時と同様の手法で計算する）
で減速するに必要な距離に十分近くなつた時期に、減速
を開始する。減速開始後は、制御周期毎に逐次減速す
る。

ところが、上述したように、最高速度と加減速勾配で
定まる加減速区間とを、制御周期単位の移動量で移動し
て行く方法をとると、制御周期は一定であるのに、目標
までの移動距離は任意であるために、最高速度区間での
最終制御周期の終りと上記の減速区間の開始時期とが一
致することはまれである。即ち、多くの場合は、一制御
周期未満の時期を最高速度で移動すべき距離だけが余
り、この距離だけ早く減速段階に入ることになる。

減速区間の最終制御周期終了後の移動速度は最低速度
にまで減速されているために、上記の余りの距離は、こ
の最低速度による目標位置への移動によつて処理されて
いる。

［発明が解決しようとしている問題点］ 換言すれば、上記従来例では、加速区間距離と減速区
間距離と、最高速度とをあらかじめ決定して、前述した
最高速度区間の余りの移動距離、即ち、一制御周期未満
の時間に最高速度で移動すべき距離を最高速度で移動し
ないで、減速開始時点を早めて、停止時点を延長する。
即ち、上記余りの移動距離を、減速段階での最低速度の
移動によつて処理するわけである。

従つて、目標位置到達迄の所要時間が長くなつてしま
い、又、最低速度での長時間の移動は機械的摩耗に影響
し、その結果、停止位置が不正確になる欠点がある。

［問題点を解決するための手段］ 上記課題を解決するため、本発明は以下の構成を特徴
とする。

即ち、加速区間と等速度区間と減速区間とを設けて、
所定の制御周期毎に一時目標位置に向けた速度制御をし
つつ、最終目標位置に到達するサーボ系の位置決め制御
方法であって、現在位置から前記最終目標位置までの距
離に基づいて定めた加速勾配、減速勾配、並びに等速度
区間速度に基づいて、加速区間距離と減速区間距離と
を、夫々、前記制御周期の整数倍の時間距離になるよう
に定め、その定められた加速区間距離と減速区間距離と
を前記現在位置から前記最終目標位置までの距離から差
し引いて得た残余距離を、前記等速度区間速度で前記制
御周期の間に移動する単位距離で除したときに、余りの
剰余距離がある場合、その剰余距離を、前記加速区間と
等速度区間と減速区間に要する前記制御周期の回数で除
した値を、前記制御周囲の夫々に割り振って、各制御周
期における移動速度を上昇させて補償することにより、
前記減速区間の最終制御周期のおける最終速度で、前記
最終目標位置まで移動する時間を短くしたことを特徴と
する。

更に好ましくは、前記剰余距離を、前記加速区間と等
速度区間と減速区間とに要する、前記制御周期の回数で
除したときに、余りが出るときには、その余りが無くな
るまで、最初の制御周期から順に最少単位分だけ移動速
度を上昇させて補償することを特徴とする。

［作用］ 以上の構成により、減速区間の最終制御周期における
最終速度で、最終目標まで移動する時間を短くすること
ができる。

［実施例］ 以下、本発明の位置決め制御方式の実施例を添付図面
に基づいて説明する。

第２図は本発明の方法が適用される数値制御装置の一
例を示すブロツク図である。第２図において、数値制御
装置は、１は目標値計算部で、外部からの指令として、
最終目標位置までの移動量Ｌ（若しくは座標位置）と、
移動速度（最高速度V_m）が与えられ、また加減速勾配を
表わす加減速テーブルも与えられる。目標値計算部１
は、これらのデータに基いて、所定の制御周期毎の目標
となるデジタル量を計算してサーボ機構２に与える。サ
ーボ機構２には、ゲイン設定部21、D/A変換器22、駆動
部23、ロータリーエンコーダ24及びアクチユエータ25を
含む。駆動部23は駆動モータ23−１と駆動増幅器23−２
とから構成されている。

第３図の制御プログラムにより、本実施例の制御方式
を説明する。この制御プログラムは、所定の周期毎に発
生する制御周期パルスによつて起動される割込みルーチ
ンである。即ち、この制御周期パルスは一定の周期を有
するパルスであり、従つて、以下説明する本実施例の制
御方式は、この周期単位で行なわれる。尚、この制御周
期は、所定の周期をもつことが必要であるが、最終目標
に到達するまでの間、常に一定不変である必要はない。
少なくとも、加速区間，等速度区間，減速区間の各区間
夫々で、一定の周期を有すれば足りる。しかし、全区間
一定の周期を有することが制御上も簡略化され有利であ
るから、以下説明する実施例では、制御パルスの周期、
即ち制御周期は一定とする。

先ず、ステツプS100で最終目標位置に到達しているか
否かを判断する。これは、本実施例の位置決め制御方法
は、移動開始を指令された時点で、移動開始前に、種々
の制御パラメータを決定するからである。この制御パラ
メータとは、加速区間距離，減速区間距離，加速区間に
続く等速度区間における速度（即ち、最高速度V_mであ
る）、そして等速度区間における一制御周期で移動する
単位距離等である。最高速度V_mは、サーボモータ23−１
及びアンプ23−２の特性によつて、その上限が決まつて
しまう。

ステツプS100で、前に指令された移動命令の目標位置
に到達していると判断された場合、次の目標位置への移
動指令（ステツプS101）を待つ。この移動指令がある
と、目標までの移動距離Ｌ、等速度区間での移動速度V_m
（即ち、最高速度）、加減速勾配等が、このサブルーチ
ンに与えられる。

そこで、ステツプS102〜ステツプS104で、前記制御パ
ラメータを演算する。即ち、ステツプS102で最高速度区
間（等速度区間）での一制御周期毎の移動量ｌを計算す
る。即ち、V_mを最高移動速度、Ｔは制御周期とすると、 ｌ＝V_m・Ｔ である。ステツプS103では、加減速区間距離及び最高速
度区間距離を割付ける。前述したように、移動距離Ｌが
与えられると、加減速勾配、最高速度V_m等が定まるか
ら、加減速区間は自動的に決まる。即ち、加速区間は、
第４図に示すように、与えられた加速勾配に従つて速度
上昇し、速度がV_mに達するまでの区間である。従つて、
この加速区間距離L_aは、加速区間での移動速度をv
_a（ｔ）で表わすと、第４図のように、 となる。上記式で、ｉは制御周期の番号を表わし、 である。

即ち、加速区間における制御周期の回数はn₁回であ
る。尚、この加速区間は、加速開始時点が決まれば、加
速区間の時期が定まつてしまう。

また、与えられた減速勾配に従つた減速区間距離L
_dは、減速区間での速度をv_d（ｔ）とすると、 で表わされ、その制御周期回数n₃は、 である。尚、この減速区間はその距離が決まつても、そ
の時期は、等速度区間の距離が決まらない限り、決まら
ない。

このように加減速区間を決定すると、等速度区間（最
高速度区間）は、 Ｌ−｛Σv_a（ｔ）・Ｔ＋Σv_d（ｔ）・Ｔ｝ ＝Ｌ−｛L_a＋L_d｝ である筈である。しかしながら、上記式の距離が、一定
速度V_mで決まる単位移動量ｌの整数倍の距離とは限らな
い。即ち、最終目標までの移動距離Ｌは任意であるか
ら、等速度区間の最終制御周期でｌ未満の距離の余りが
発生するわけである。そこで、本実施例では、この余り
の距離の移動を、加速区間、減速区間での速度の増減に
分配するわけである。

即ち、ステツプS104で、最終目標までの移動量Ｌのう
ち加減速区間（L_a＋L_d）を除いたところの、指定速度V_m
での移動区間のうち、制御周期毎の動作量ｌで割り切れ
ない剰余距離Δｌを求める。即ち、 である。

ここで、Ｌ−L_a−L_dが負の場合がある。このときは通
常のサーボ制御を行なうために、そのサブルーチンにリ
ターンする。

そして、ステツプS106では、剰余距離Δｌを加減速区
間の制御周期の回数（n₁＋n₃）と、最高速度区間の制御
周期の回数n₂の合計回数（ｎ＝n₁＋n₂＋n₃）で除して、
この割り算の商であるところの、各制御周期ごとに均等
に分配できる距離Δl_aと、各制御周期に均等に分配でき
ない剰余距離Δl_mを求める。即ち、最高速度区間の制御
周期の回数n₂は、 であるから、上記△l_a,△l_mは、 である。ステツプS107では、ステツプS104で求められた
各制御周期ごとに均等に分配できる商l_aを、今回の制御
周期における加算量Δl_cとする。即ち、ステツプS106で
求めた△l_aは、△l_cの初期値である。

こうして、制御パラメータが揃つたことになる。以下
のサーボ位置決め制御は、一時目標位置を定め、その一
時目標位置と現在の位置との差から、D/Aコンバータ22
に出力するデジタル値を決めるものである。換言すれ
ば、本実施例の特徴は、この一時目標位置の設定に特徴
があるということができる。この一時目標位置をZ_Tで表
わす。

ステツプS108では、各制御周期に均等に分配できない
剰余距離Δl_mが０であるかを判定する。

今、△l_mが“0"の場合を先に説明する。ステツプS111
では、加減速区間であるかどうかを判定する。最初は加
速区間から開始するから、ステツプS112で、前回の一時
目標位置Z_Tに加速パターンによつて与えられる加速移動
量αと、今回の制御周期における加算量のΔl_cを加えた
ものを、今回の制御周期における一時目標位置Z_Tとす
る。即ち、 Z_T＝Z_T＋α＋Δl_c である。尚、加速パターンによる移動量α（及び後述の
減速パターンによる移動量β）は、第２図の加速度テー
ブルから与えられるものである。

ステツプS114では、今回の一時目標位置Z_Tをロボツト
アームのエンコーダ値に変換して、サーボ機構２に渡
す。ステツプS115では、ロータリーエンコーダ24から検
出された現在のエンコーダ血と比較されたエンコーダ誤
差値（実際には、これにゲインを乗したもの）が、D/A
変換器22に出力される。こうして、一時目標位置Z_Tに向
けて、加速が開始される。この場合の加速は、剰余距離
△ｌが、各移動区間の制御周期間で均等に配分されたも
のとなつている。即ち、単なる（Z_T＋α）ではなく、
（Z_T＋α＋Δl_c）と速度を増加して、制御周期Ｔの間だ
け移動することになる。

従つて、次の制御周期になると、ステツプS100ステ
ツプS108ステツプS111と進んで、加速を続け、その度
に、Δl_cの分だけ速度増しているために、剰余距離△ｌ
は見掛け上減つていく。そして、加速区間を終り、等速
度区間に移行すると、ステツプS100ステツプS108ス
テツプS111ステツプS113に進んで、移動速度を、 Z_T＋ｌ＋Δl_c と替えて、引き続き最終目標位置に近接しながら、剰余
距離△ｌは見掛け上の減少を図る。等速度区間をＴ×n₂
だけ繰り替えすと、減速区間に入る。

減速区間に入ると、ステツプS100ステツプS108ス
テツプS111ステツプS112と進み、一時目標位置Z_Tを、 Z_T＝Z_T＋β＋Δl_c として、減速に入る。このΔl_cの寄与により、減速区間
でも、剰余距離△ｌは減少し続け、減速区間の最終制御
周期で、△ｌは“0"になる。何故なら、減速区間の最終
制御周期では、（ｎ＝n₁＋n₂＋n₃）回の制御周期を経て
いるから、Δl_cずつ各制御周期でΔl_c分に相当する速度
上昇を行なえば、丁度△ｌに相当する距離が移動された
ことになるのである。

こうして、少なくとも、△l_m＝０のときは、減速区間
の最終局面での最低速度での移動時間が減少することに
なるので、前述した従来の欠点は解消する。第４図に、
このような速度補正を施すことによつて、全区間で速度
がどのように変わつたかを図示する。

以上はΔl_mが“0"の場合であつたが、本実施例では、
△l_mが“0"でない場合も、上記従来の欠点を解消するた
めの工夫をしている。即ち、この場合は、ステツプS108
からステツプS109に進む。ステツプ109で、今回の制御
周期における加算量Δl_cに１を加え、ステツプS110で各
制御周期に均等に分配できない剰余Δl_mから１を減ず
る。即ち、△l_mの分だけ、より速度を上昇させるわけで
ある。これが可能なのも、△l_mの距離は全区間の制御周
期数ｎよりも短いからである。そして、各制御周期毎
に、△l_mから１を減ずるので、やがて、最終制御周期に
至る前に、△l_mは“0"になる。

第５図に、△l_m≠０における、速度変化を示す。第６
図と比較すると、Δl_cに加えて、更に△l_mが各制御周期
に分配されているのが了解されるであろう。

以上の処理により、剰余距離△ｌは、Δl_m＝０の場合
も、△l_m≠０の場合も、各制御周期ごとに分配して処理
される。

尚、各区間における速度上昇の度合は、Δl_c若しくは
Δl_c＋１として、各区間において、均一にしているが、
サーボ系が不安定にならない範囲で、不均一にしてもよ
い。

以上本発明の実施例についてロボツトのアームの制御
について説明したが、当然同様の構成を有する数値制御
装置のサーボ系の位置決め方法として用いられることは
明白である。

［発明の効果］ 以上説明したように本発明によれば、剰余距離を、最
終目標位置まで移動するのに要する全制御周期に渡って
分配し、各制御周期における移動速度を上昇させるた
め、各制御周期における加速度の変動を防止しつつ、減
速区間の最終制御周期における最終速度で、最終目標位
置まで移動する所要時間を短縮できる。特に、剰余距離
を全制御周期に渡って分配したときに余りが生じる場合
には、その余りが無くなるまで、最初の制御周期から順
に制御量の最少単位分ずつ各制御周期における移動速度
を上昇させて補償するため、よりスムーズな位置決め
と、位置決め精度の向上とが実現できる。

【図面の簡単な説明】

第１図は本発明の位置決め制御方法の原理を説明する
図、 第２図は本発明の適用される数値制御装置の一例を示す
ブロツク図、 第３図は本実施例に用いられる位置決め制御のプログラ
ムのフローチヤート、 第４図，第５図は、実施例の位置決め方式により得られ
た速度変化を説明する図、 第６図は従来の位置決め制御方法における時間と速度の
関係を示す図である。 図中、１……目標値計算部、２……サーボ機構、21……
ゲイン設定部、22……D/A変換器、23……駆動部、24…
…ロータリエンコーダ、25……アクチユエータ、23−１
……モータ、23−２……アンプである。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 山本 敏弘 東京都大田区下丸子３丁目30番２号 キ ヤノン株式会社内 (72)発明者 笠井 省三 東京都大田区下丸子３丁目30番２号 キ ヤノン株式会社内 (56)参考文献 特開 昭57−79511（ＪＰ，Ａ) 特開 昭59−212912（ＪＰ，Ａ) 特開 昭62−32508（ＪＰ，Ａ) 特開 昭59−114606（ＪＰ，Ａ) 特開 昭59−47985（ＪＰ，Ａ) 実開 昭53−103098（ＪＰ，Ｕ)

Claims (2)

(57)【特許請求の範囲】
1. 【請求項１】加速区間と等速度区間と減速区間とを設け
   て、所定の制御周期毎に一時目標位置に向けた速度制御
   をしつつ、最終目標位置に到達するサーボ系の位置決め
   制御方法であって、 現在位置から前記最終目標位置までの距離に基づいて定
   めた加速勾配、減速勾配、並びに等速度区間速度に基づ
   いて、加速区間距離と減速区間距離とを、夫々、前記制
   御周期の整数倍の時間距離になるように定め、 その定められた加速区間距離と減速区間距離とを前記現
   在位置から前記最終目標位置までの距離から差し引いて
   得た残余距離を、前記等速度区間速度で前記制御周期の
   間に移動する単位距離で除したときに、余りの剰余距離
   がある場合、 その剰余距離を、前記加速区間と等速度区間と減速区間
   とに要する前記制御周期の回数で除した値を、前記制御
   周期の夫々に割り振って、各制御周期における移動速度
   を上昇させて補償することにより、前記減速区間の最終
   制御周期における最終速度で、前記最終目標位置まで移
   動する時間を短くしたことを特徴とするサーボ系の位置
   決め制御方法。
2. 【請求項２】前記剰余距離を、前記加速区間と等速度区
   間と減速区間とに要する、前記制御周期の回数で除した
   ときに、余りが出るときは、その余りが無くなるまで、
   最初の制御周期から順に最少単位分だけ移動速度を上昇
   させて補償するこを特徴とする請求項１記載のサーボ系
   の位置決め制御方法。