JPH02121005A - ディジタルサーボ制御用関数発生装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔概要〕 ディジタルサーボ制御用関数発生装置に係り、特にフィ
ードバック補償器に位置、速度及びその高次の導関数を
発生するディジタルサーボ制御用関数発生装置に関し、 関数発生装置に大容量の記憶装置を備えなくとも、制御
対象を誤差なく円滑に移動制御することを目的とし、 上述のようなディジタルサーボ制御用関数発生装置にお
いて、速度あるいはその高次の導関数が一定値から他の
値に変化するとき前の区間の最終値を格納する最終値格
納手段と、指定された速度及びその高次の導関数から指
定速度あるいはその高次の導関数が一定である区間での
位置、速度及びその高次の導関数を上記最終値格納手段
が格納した前区間の最終値を初期値とし、関数を初期値
設定時からの経過時間に関する式で算出する関数算出部
とを備えるものとして構成する。

〔産業上の利用分野〕

本発明は、ディジタルサーボ制御用関数発生装置に係り
、特にフィードバック補償器に位置、速度及びその高次
の導関数を発生するディジタルサーボ制御用関数発生装
置に関する。

〔従来の技術〕

近年ディジタルサーボ制御は温度補償が容易なことや、
調整が容易で雑音に強いことなどからＮＣ装置やステー
ジの位置決め等によく使用されている。このディジタル
サーボ装置は、第５図に示すように、上位装置からの目
標位置ｒｉ、目標速度Ｖｉ、及び目標加速度ａｉに基づ
き制御対象１０の指令位置ｒ（ｋ）を発生する関数発生
器１１と、制御対象１０の実際の位置ｙ（ｋ）と上記の
関数発生器１１の出力ｒ　（ｋ）との差をｅ（ｋ）０と
するように作動するフィードバック補償器１２とからフ
ィードバック系を構成している。そしてこのような場合
、関数発生器１１は制御対象１０の滑らかで連続した移
動を実現するため、制御対象１０の加速時には、等加速
度で増速し、その後一定速度で移動した後、目標位置で
速度０となるように一定の加速度で減速するように指令
位置を発生する場合がある。これは例えば、第７図（２
）（３）に示すようにその指令速度を一定時間（Ｔ□）
等加速度（ａ□＞０）で増加し、その後所定時間一定速
度（ｖ２）で移動させてｒ３に致った後、一定加速度（
ａ３＜０）で減速して停止するものとした場合で、制御
対象は第７図（１）に追従して移動する。

〔発明が解決しようとする課題〕

ところでこのようなディジタルサーボ系における、フィ
ードバック制御は、マイクロプロセッサのソフトウェア
で制御系を形成し、一定周期に設定したサンプリングタ
イムにおいて離散的に行なうものとしている。そこでサ
ンプリング周期をＴとし、指令位置をｒ（ｔ）、指令速
度をｖ　（ｔ）、指令加速度をａ　（ｔ）とすると、ス
テップに＋１における指令位置ｒ（ｔ）及び指令速度ｖ
　（ｔ）は、ｒ（ｋ＋１）＝ｒ（ｋ）＋Ｔｖ（ｋ）＋Ｈ
’ｒ２ａ（ｋ）ｖ（ｋ＋１）　　＝ｖ（ｋ）　　＋Ｔａ
（ｋ）なる差分方程式で表現される。ここでｒ　（ｋ）
は第にステップでのｒ　（ｔ）の値、即ちｒ　（ｋＴ）
を示し、これはｖ（ｋ）　、　ａ（ｋ）についても同様
の意味を有する。この式はステップに＋１の値は前のス
テップにの時点の値を基に計算されるものとなる。

ところで上述したように、従来この計算はマイクロプロ
セッサを用いて行なうため、計算の桁数が有限であるた
め、各ステップでの計算結果には若干の誤差を含むもの
となる。このためステップを重ねる度に誤差が蓄積され
ることとなる。

このような誤差を蓄積しないためには、関数の計算を差
分方程式を基にするのではなく、第１のステップからの
経過時間ｔの関数である、ｒ　（ｔ）　＝３／１２３ｏ
　ｔ２＋ｖｏ　ｔ　＋　ｒ。

ｖ（ｔ）＝ａｏｔ＋ｖ。

を基に計算すれば良い。しかしながら、このような式を
用いるためには加速度が変化する区間に対応して初期値
ａ。、Ｖｏ　、ｒｏの値を格納しておかなければならず
、格納データの数が多くなり、大きい容量のメモリ装置
が必要となるという問題がある。

また、計算誤差があった場合には加速度が変化する区間
の接続個所において、位置、速度等の不連続が発生し、
制御対象を円滑に駆動できなくなる虞れがある。

そこで、本発明は大容量の記憶装置を備えなくとも、制
御対象を円滑に移動制御することができるディジタルサ
ーボ制御用関数発生装置を提供することを目的とする。

〔課題を解決するための手段〕

本発明にあって、上記の課題を解決するための手段は、
第１図に示すように、ディジタルサーボ制御のフィード
バック補償器に位置、速度及びその高次の導関数を発生
するディジタルサーボ制御用関数発生装置１において、
速度あるいはその高次の導関数が一定値から他の値に変
化するとき前の区間の最終値を格納する最終値格納手段
２と、指定された速度及びその高次の導関数から指定速
度あるいはその高次の導関数が一定である区間での位置
、速度及びその高次の導関数を上記最終値格納手段が格
納した前区間の最終値を初期値とし、関数を初期値設定
時からの経過時間に関する式で算出する関数算出部３と
を備えたことである。

（作用） 本発明によれば、最終値格納手段２は速度あるいはその
高次の導関数が一定値から他の値に変化するとき前の区
間の最終値を格納し、関数算出部３は指定された速度及
びその高次の導関数から指定速度あるいはその高次の導
関数が一定である区間での位置、速度及びその高次の導
関数を上記最終値格納手段が格納した前区間の最終値を
初期値とし、関数を初期値設定時からの経過時間に関す
る式で算出するから、計算された関数には誤差の蓄積は
無く、また、初期値を各区間毎に格納しておく必要はな
い。また、後の区間の初期値とじて前の区間の最終値を
採用するようにしているから、位置速度等の不連続は発
生しない。

〔実施例〕

以下本発明に係るディジタルサーボ制御用関数発生装置
の実施例を図面に基づいて説明する。

第６図はロボットのディジタルサーボ制御のハードウェ
ア構成例を示す図である。同図において、２１は上位計
算機、２２は下位計算機、２３はプログラムＲＡＭ、２
４は外部拡張ＲＡＭ、２５はカウンタ、２６はエンコー
ダ、２７はＤＣモータ、２８は減速器、２９はアーム、
３０はＤ／Ａコンバータ、３１は電流アンプを示してい
る。

この例において、上位計算機２１は汎用のマイクロプロ
セッサであり、下位計算機２２に指令コマンドを与え、
下位計算機２２は指令コマンドを実行する。下位計算機
はＤ　Ｓ　Ｐ　（ＤｉｇｉｔａｌＳｉｇｎａｌ　Ｐｒｏ
ｃｅｓｓｅｒ）と称されるもので、このＤＳＰは１６ビ
ツトの固定小数点の乗算または積和計算を１００ｎｓな
いし２００ｎｓで実行でき、汎用のマイクロプロセッサ
に比べて５乃至１０倍高速であり、ＤＣモータ２７の回
転角をエンコーダ２６を用いてカウンタ２５から取りこ
みＤＳＰの内部でサーボ演算を行ない、操作量をＤ／Ａ
コンバータ３０に出力し、電流アンプ３１を介しＤＣモ
ータ２７を回転駆動する。即ち、下位計算機２２は上位
計算機２１より与えられている指令とカウンタ２５から
の現在位置情報に基づいてソフトウェアにより構成され
たディジタルサーボ制御系に基づいて操作量を求める。

ＤＣモータ２７は減速器２８を介してアーム２９を駆動
する。この例においてはロボットは６軸のものであって
他の５軸についても、同様の構成を有するものとしてい
る。上位計算機２１は制御系の物理的なパラメータをキ
ーボードまたはファイルから入力してサーボ定数を下位
計算ｆｉ２２の内部ＲＡＭに設定する。また、下位計算
機２２に目標位置や目標速度、目標加速度を指示し、下
位計算機のプログラムの管理等も実行する。

第２図は本発明に係るディジタルサーボ制御用関数発生
装置の実施例の作動を示すフローチャートである。本実
施例においては制御対象であるロボットのアームを、第
７図に示すように、等加速度で増速し、一定期間等速で
移動し、その後等加速度で減速して停止するように作動
制御する場合について説明する。

本実施例において、関数発生装置は上述したように下位
計算機によってソフトウェアを実行することによって実
現される。そして本実施例では制御対象であるアームを
位置０から加速度ａ１で速度ｖ２まで加速して、所定距
離移動した後、位置ｒ３から加速度ａ３で減速して停止
するものとして制御する。そして本実施例においては、
時間の単位は全て、サンプリング間隔となるように規格
化されている。

先ず制御条件としてｒ　３　＋　”２　＋　ａｌ　ｔ　
８３の値を指定する（ＳＴＩ）。この指定は上位計算機
が下位計算機に与えるものとしている。

次に初期設定としてｋを０とし、ＶｏをＯ１ａＯをａｌ
としてｍ　ｏ　ｄ　ｅレジスタを加速中モード（モード
１）に設定する（Ｓｒ１）。

そして制御対象の設定位置ｒ（ｋ）、設定速度ｖ　（ｋ
）を ｒ（ｋ）＝１／２ａｏｋ２＋ｖｏ　ｋ＋ｒ。

ｖ（ｋ）　＝ｖ（ｋ）　＋ａｏ　ｋ で計算した後、ｋに１を加算する（Ｓｒ１）。ここで下
位計算機はこのステップ３で設定位置、速度加速度を求
める度に、この指令位置、速度加速度を用いてＤＡコン
バータ３０に与えるための操作量を算出するフィードバ
ック補償の演算処理動作をプログラム処理により行なう
。そして、下位計算機は求めた操作量をバスを介して各
軸毎にＤＡコンバータに与えアームを回動させる。

この処理をｍ　ｏ　ｄ　ｅレジスタの内容をチエツクし
く５Ｔ４）つつ、制御対象の速度がｖ２になるまで続け
（Ｓｒ１）、速度がｖ２になったら加速度の設定値ａ。

を０に設定してｍ　ｏ　ｄ　ｅレジスタを定速モード（
モード２）にする（ＳＴＹ）。そして次の区間の初期値
ｒ。、Ｖｏとして前区間の最終値ｒ（Ｋ）　、　ｖ（Ｋ
）を設定して、ｋをＯとする（ＳＴＩＩ）。

すると、設定速度はｖ（ｋ）のままでアームを等速運動
させるべく、アームの位置ｒ　（ｋ）の計算を行ない（
Ｓｒ１）、このままアームの位置がｒ３となるまでこの
計算を続ける（Ｓｒ１）。そしてアームの位置がｒ３と
なったら、ａｏをａ３とし、ｍ　ｏ　ｄ　ｅレジスタを
減速モード（モート３）に切替える（Ｓｒ１）。すると
再びステップ３において位置、加速度、速度を計算して
、所定の時間減速を続は速度Ｏとなった時に（Ｓｒ１）
、速度及び加速度の値をＯとしてｍ　ｏ　ｄ　ｅレジス
タを停止モード（モードＯ）として（ＳＴＩＯ）、ロボ
ットアームな停止する。

このようにしてロボットを所定の加速度及び速度で移動
させ所定の個所で停止させることができる。この際、関
数を初期値設定時からの経過時間に関する式で算出する
から、計算された関数には誤差の蓄積は無く、また、初
期値を各区間毎に格納しておく必要はなく、大容量のメ
モリは必要ではない。また、後の区間の初期値として前
の区間の最終値を採用するようにしているから、位置速
度等の不連続は発生せずロボットの制御を円滑に行なう
ことができる。

第３図及び第４図は本発明に係るディジタルサーボ制御
用関数発生装置の第２の実施例の作動を示すものである
。本実施例においては制御対象のより円滑な運動を実現
するため、加速度が０から線形的に増加、減少するよう
にロボットアームな駆動するようにしたものである。即
ち第３図（３）（４）に示すように、加速度ａ（ｋ）は
０から変化率ｄ１で所定時間Ｔ１だけ直線的に増加し、
加速度ａ２で一定時間Ｔ２保たれ、続いて変化率ｄ３で
所定時間Ｔ３にわたって減少して０となり、ロボットア
ームは第３図（２）に示すように等速度ｖ４で移動する
。そして所定時間経過後減速を始めるが、この減速も負
の加速度が線形に増加して（変化率ｄ、、、変化時間Ｔ
５）また、線形に減少する（変化率ｄ６、変化時間Ｔ６
）ようにしてなるものである。このようにして制御対象
であるロボットアームを制御することにより、第３図（
１）に示すように、より円滑に駆動することができる。

第４図は第３図に示したロボットの駆動を行なう場合の
関数発生装置の作動を示すフローチャートである。先ず
、上位計算機から、減速開始位置ｒ５、指令速度ｖ４、
加速度ａ２．加速度の変化率ｄｌ　、ｄ３．ｄａ　、ｄ
ａを指定する（Ｓｒ２１）。そしてこれらの値から各変
化率の一定な時間Ｔｌ　、Ｔ２　、Ｔ３　、Ｔｓ　、Ｔ
ｓをとして計算する（Ｓｒ２２）。

次に初期設定としてｋをＯとし、現在位置をｒｏ、Ｖｏ
を０、ａｏを０、ｄｏをｄ□に設定してｍ　ｏ　ｄ　ｅ
レジスタを加速度増加モード（モード１）に設定する（
ＳＴ２３）。

そして制御対象の設定位置ｒ（ｋ）、設定速度ｖ（ｋ）
、設定加速度ａ（ｋ）を ｒ（ｋ）＝　３４ｄｏ　ｋ３”　３／２ａｏ　ｋ２”　
Ｖｏ　ｋ　”ｒ。

ｖ（ｋ）＝　３’２ｄ□　ｋ２”　ａｏ　ｋ　　”　Ｖ
。

ａ（ｋ）＝　ｄｏ　ｋ＋　ａ。

で計算した後、ｋに１を加算する（ＳＴ２４）。

ここで下位計算機はこのステップ２４で設定位置、速度
加速度を求める度に、この指令位置、速度加速度を用い
てＤＡコンバータ３０に与えるための操作量を算出する
フィードバック補償の演算処理動作をプログラム処理に
より行なう。そして、下位計算機は求めた操作量をバス
を介して各軸毎にＤＡコンバータに与えアームを回動さ
せる。　この処理をｍ　ｏ　ｄ　ｅレジスタの内容をチ
エツクしく５Ｔ２５）つつ、ｋがＴ４になるまで続け（
ＳＴ２６）、ｋがＴ１になったら加速度の増加率ｄ（ｋ
）を０、ｋを０に設定してｍ　ｏ　ｄ　ｅレジスタを一
定加速モード（モード２）にする（ＳＴ２７）。そして
次の区間の初期値ｒｏ　、Ｖｏ　、ａｏとして前区間の
最終値ｒ（Ｋ）。

ｖ（Ｋ）　、　ａ（Ｋ）を設定して、ｋをＯとする（Ｓ
Ｔ３８） すると、設定加速度は一定のままでアームを等加速運動
を行なわせるべく、アームの位置ｒ（ｋ）、速度ｖ（ｋ
）の計算を行ない（ＳＴ２４）、このままｋがＴ２とな
るまでこの計算を続ける（ＳＴ２Ｂ）。モしてｋがＴ２
となったら、ｄ　（ｋ）をｄ３．ｋを０として、ｍ　ｏ
　ｄ　ｅレジスタを加速度減少モード（モート３）に切
替える（ＳＴ２９）。再びステップ２４において位置ｒ
（Ｋ）、加速度ａ　（Ｋ）及び速度ｖ（Ｋ）を計算して
、ｋがＴ３になるまで加速度を減少していき、ｋがＴ３
となったときに加速度の増加率ｄ　（Ｋ）を０、ｋを０
に設定してｍ　ｏ　ｄ　ｅレジスタを一定速モード（モ
ード４）にする（ＳＴ３１）。

次に区間の初期値ｒ。、ｖｏ　、ａｏとして前区間の最
終値ｒ（Ｋ）　、　ｖ（Ｋ）　、　ａ（Ｋ）を設定して
、ｋを０としく５Ｔ３８）　、ロボットアームの位置ｒ
　（ｋ）がｒ５　（減速開始位置）に来るまで一定速度
Ｖ４による移動を続ける。そしてロボットアームの位置
ｒ（ｋ）がｒ５になったら、ｄ（ｋ）をｄ５、ｋを０と
してｍ　ｏ　ｄ　ｅレジスタを第１の減速モード（モー
ド５）に設定し、区間の初期値ｒ　ｏ、　Ｖｏ　、　ａ
　ｏとして前区間の最終値ｒ（Ｋ）。

ｖ（Ｋ）　、　ａ（Ｋ）を設定して、ｋを０とする（Ｓ
Ｔ３８）。

このモードにおいて負の加速度を変化率ｄ５の割合で大
きくしていき（ＳＴ２４）、ｋがＴ５になるまでこの処
理を続け（ＳＴ３４）、ｋがＴ゛５になった時点で、ｍ
　ｏ　ｄ　ｅレジスタを第２の減速モード（モード６）
として（ＳＴ３５）加速度を変化率ｄ１６の割合で０に
近付けていき（ＳＴ２４）、ｋがＴ６になった時点で（
ＳＴ３６）、速度Ｏ加速度Ｏ１加速度の変化率０として
ｍ　ｏ　ｄ　ｅレジスタを停止モード（モード０）とす
る（ＳＴ３７）。

これによりロボットのアームを所定位置から移動して停
止させることができる。従って、本実施例によれば、ロ
ボットアームなより一層円滑に移動できる他、第１の実
施例と同様に、関数を初期値設定時からの経過時間に関
する式で算出するから、計算された関数には誤差の蓄積
は無く、また、初期値を各区間毎に格納しておく必要は
なく、大容量のメモリは必要ではない。また、後の区間
の初期値として前の区間の最終値を採用するようにして
いるから、位置速度等の不連続は発生せずロボットの制
御を円滑に行なうことができる。

尚、上記の実施例においては、ディジタルサーボ制御の
対象としてロボットを例として説明したが、本発明は他
のものを制御する場合であっても適用できる。

〔発明の効果〕

以上説明したように、本発明によれば、ディジタルサー
ボ制御用関数発生装置に速度あるいはその高次の導関数
が一定値から他の値に変化するとき前の区間の最終値を
格納する最終値格納手段と、指定された速度及びその高
次の導関数から指定速度あるいはその高次の導関数が一
定である区間での位置、速度及びその高次の導関数を上
記最終値格納手段が格納した前区間の最終値を初期値と
し、関数を初期値設定時からの経過時間に関する式で算
出する関数算出部とを備えるようにしたから、関数を初
期値設定時からの経過時間に関する式で算出するから、
計算された関数には誤差の蓄積はなく、また初期値を各
区間毎に格納しておく必要はないから、大容量のメモリ
は必要ではない。更に、後の区間の初期値として前の区
間の最終値を採用するようにしているから、位置速度等
の不連続は発生せずロボットの制御を円滑に行なうこと
ができるという効果を奏する。

用関数発生装置の第２の実施例の作動を示すフローチャ
ート、第５図は本発明の関数発生装置が適用されるディ
ジタルフィードバック系を示す図、第６図は本発明が適
用されるロボットのハードウェア構成を示す図、第７図
はディジタルフィードバック制御される制御対象への指
令値の動きを示す図である。

１・・・関数発生装置 ２・・・最終値格納手段 ３・・・関数算出部

【図面の簡単な説明】

Claims (1)

1. 【特許請求の範囲】 ディジタルサーボ制御のフィードバック補償器に位置、
   速度及びその高次の導関数を発生するディジタルサーボ
   制御用関数発生装置（１）において、 速度あるいはその高次の導関数が一定値から他の値に変
   化するとき前の区間の最終値を格納する最終値格納手段
   と（２）、 指定された速度及びその高次の導関数から指定速度ある
   いはその高次の導関数が一定である区間での位置、速度
   及びその高次の導関数を上記最終値格納手段が格納した
   前区間の最終値を初期値とし、関数を初期値設定時から
   の経過時間に関する式で算出する関数算出部（３）とを
   備えたことを特徴とするディジタルサーボ制御用関数発
   生装置。