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

Abstract

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

Description

【発明の詳細な説明】 （概要） ディジタルサーボ制御用の関数発生装置に係り、特にデ
ィジタルサーボ制御の制御対象の目標停止位置ｒｆに至
るまでのサンプリング時刻ｔにおける指令位置ｒをサン
プリング時刻の非連続性により発生する誤差を補正して
出力するディジタルサーボ制御用関数発生装置に関し、 制御対象が目標地点に達した時点で速度が０となるよう
に、指令位置をサンプリング時刻の非連続性により発生
する誤差を考慮して指令関数を発生するに際してその計
算を高速に且つ容易に実行できるようにすることを目的
とし、 ディジタルサーボ制御の制御対象の目標停止位置ｒｆに
至るまでのサンプリング時刻ｔおける指令位置ｒをサン
プリング時刻の非連続性により発生する誤差を補正して
出力するディジタルサーボ制御用関数発生装置において
、制御対象を停止するために減速指令を開始する時刻を
七〇とし、減速に必要とする時間をＴｓとし、時刻ｔ。

から時刻ｔ、＋”ｒ、における制御対象の指令移動量ｒ
ｓを演算する減速移動量算出手段と、減速開始指令位置
ｒｏとじて補正量Δｒを、 Δｒ＝　（ｒ、−ｒ。−ｒｊ）／Ｔｊ として演算する補正量演算手段と、補正前の指令位置を
ｒｏとして、時刻ｔにおける指令位置ｒをｒ＝ｒ’　＋
　（ｔ　　ｔｏ　）Δｒ として出力する修正指令位置演算手段とを有するものと
して構成する。

〔産業上の利用分野〕

本発明は、ディジタルサーボ制御用の関数発生装置に係
り、特にディジタルサーボ制御の制御対象の指令停止位
置ｒに至るまでのサンプリング時刻ｔおける目標位置ｒ
ｔをサンプリング時刻の非連続性により発生する誤差を
補正して出力するディジタルサーボ制御用関数発生装置
に関する。

（従来の技術） 近年、ディジタルサーボ制御は温度補償が容易なことや
、調整が容易で雑音に強いことなどから、ＮＣ装置やス
テージの位置付は等に用いられる。このディジタルサー
ボ制御は、第５図に示すように制御対象１０の指令位置
ｒを発生する関数発生器１１と、制御対象１０の実際の
位置ｙと上記の関数発生器１１の出力との差ｅをＯとす
るように作動するフィードバック補償器１２とからフィ
ードバック系を構成している。そしてこのような場合、
関数発生器は滑らかで連続した移動を実現するため加速
時には等加速度で増速し、その後一定速度で移動したの
ち、目標位置で速度０となるように一定の加速度で減速
するように指令位置を発生するものとする場合がある。

これは例えば第６図（３）に示すような位置の関数とな
るように指令位置関数を発生する。制御対象が、このよ
うな位置を移動するとき、発生する指令速度■は第６図
（２）に示すように台形状になり、そして、発生する指
令加速度ａは第６図（１）に示すようになる。

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

このようなディジタルサーボ系においては、アナログサ
ーボ系のように連続して位置の制御をするのではなく、
離散的に設けたサンプリング時刻ｔにおいて指令位置の
関数ｒを発生して、制御対象に制御を行うようにしてい
る。このとき最終的に位置決めを行う位置ｒｆにおいて
速度がＯになる必要がある。

しかしながら、サンプリング時刻は離散的であるので、
第７図に示すように、本来であれば時刻ｔ１で減速を開
始すれば目標位置ｒｆで停止することができるが、この
ｔ□がサンプリング時刻ｔｋとｔｋや、の間に相当する
ときには、その時刻の前後の何れかの時点１ｋまたはｔ
ｋ＋□からしか減速できないため、その時刻ｔ□の前の
時刻ｔｋから減速を始めると目標位置まで達せずｒｆｌ
で停止するし、また、その時刻を通り過ぎた時刻ｔ６．
１から減速を開始すると目標位置ｒｆにおいて速度が０
にはならずｒｊ２まで行き過ぎてしまう。このｒｆｔか
らｒｊ２までの距離はサンプリング時間なｄｔとし、制
御対象の最高指令速度を■□８とするとＶｍａｘ　Ｘ　
ｄ　ｔとなる。そのため、従来このような減速時には目
標停止位置までの指令位置量ｒｄを計算して、この位置
量ｒｄに関する指令速度の関数を求め、この関数に従っ
て速度を制御して、制御対象が目標地点ｒｆに達した時
点で速度が０となるようにしている。ここで上述したよ
うな台形の速度パターンではこの関数は、Ｖ＝＄どなる
。ここでｒａはｒ、から ｒまでの距離を示している。

しかしながら、マイクロプロセッサでこのような平方根
の計算を実行することは時間がかかるものであり、サン
プリング時刻の間隔（サンプリング時間という）をこの
計算時間より長いものとしなければならず、制御性能が
劣化するという問題がある。また、減速のパターンが上
述した台形状でなく複雑であるときには関数を求めるの
が困難となり実現ができなくなる場合もある。

そこで本発明は、制御対象が目標地点に達した時点で速
度が略０となるように、指令位置をサンプリング時刻の
非連続性により発生する誤差を考慮して指令関数を発生
するに際してその計算を高速に且つ容易に実行できるデ
ィジタルサーボ制御用関数発生装置を提供することを目
的とする。

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

本発明にあって５上記の課題を解決するための手段は、
第１図に示すように、ディジタルサーボ制御の制御対象
の目標停止位置ｒｆに至るまでのサンプリング時刻ｔに
おける指令位置ｒをサンプリング時刻の非連続性により
発生する誤差を補正して出力するディジタルサーボ制御
用関数発生装置において、制御対象を停止するために減
速指令を開始する時刻を七〇とし、減速に必要とする時
間なＴヨとし、時刻ｔ。から時刻ｔ。＋Ｔ、における制
御対象の指令移動量ｒｓを演算する減速移動量算出手段
１と、減速開始指令位置ｒ。とじて補正量Δｒを、 Δｒ＝　（ｒｆ−ｒｏ−ｒ、）／Ｔｇ として演算する補正量演算手段２と、補正前の指令位置
をｒｏとして、時刻ｔにおける指令位置ｒを ｒ＝ｒ’　＋　（ｔ−ｔｏ　）Δｒ として出力する修正指令位置演算手段３とを有するもの
としたことである。

〔作用〕

本発明によれば、修正指令位置は、 Δｒ＝　（ｒｆ−ｒｏ−ｒ、）／Ｔｇ 及び ｒ＝ｒ　　＋　（ｔ−ｔｏ　）Δｒ なる一次関数で求めることができるから演算を高速に実
行することができる。

〔実施例〕

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

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

上位計算機２１は、汎用のマイクロプロセッサであり、
下位計算機２２に指令コマンドを与え、下位計算機２２
は指令コンマドを実行する。

下位計算機２２は、Ｄ　Ｓ　Ｐ　（Ｄｉｇｉｔａｌ　Ｓ
ｉｇｎａｌＰｒｏｃｅｓｓｅｒ）と呼ばれるものである
。ＤＳＰは１６ビツトの固定小数点の乗算または積和計
算を１００ｎｓないし２００ｎｓで実行可能であり、汎
用のマイクロプロセッサに比べて５ないし１０倍高速で
あり、ＤＣモータ２７の回転角をエンコーダ２６とカウ
ンタ２５により取り込み、ＤＳＰ２２の内部でサーボ演
算を行い、操作量をＤ／Ａコンバータ３０に出力し、電
流アンプ３１を介してＤＣモータ２７を回転駆動する。

すなわち、ＤＳＰ２２は、上位計算機２１より与えられ
る指令とカウンタ２５からの現在位置情報に基づいてソ
フトウェア２３により構成された関数発生器に基づいて
操作量を求める。モータ２７の回転は、減速機２８を介
してアーム２９を駆動する。他の５軸も同様な構成であ
る（なお、ロボットは６軸のものと仮定）。上位計算機
２１は、制御系の物理的なパラメータをキーボードまた
はファイルから入力し、サーボ定数をＤＳＰ２２の内部
ＲＡＭに設定する。また、ＤＳＰ２２に指令位置や目標
速度、目標加速度を指示し、ＤＳＰプログラムの管理等
も行う。

第２図は本発明に係るディジタルサーボ制御用関数発生
装置の第一の実施例の作動を示すものである。本実施例
において、上述した減速移動量算出手段、補正量演算手
段及び修正指令位置演算手段はプロセッサでプログラム
を実行することにより実現される。そして、本実施例に
おいて、関数発生器からのデータは本実施例は従来と同
様に第６図（２）に示したように等加速度で加速及び減
速される制御対象の制御に用いるものである。本実施例
においては、時間の単位は全て、サンプリング間隔とな
るように規格化されている。第２図のフローチャートに
示されるように、下位計算機であるＤＳＰ２２は加速時
の指令加速度ａ□、減速時の指令加速度−ａ２　　（ａ
２は正）、最大指令速度ｖｍａｘが外部（上位計算機２
１）から与えられれば減速時間Ｔ、と、減速移動量ｒ１
とを計算する（ＳＴＩ）。この部分が減速移動量計算手
段に対応する。そして、ｍ　ｏ　ｄ　ｅレジスタを加速
中モードに設定し、制御対象の指令位置ｒを指定する条
件として初期速度を０、指令加速度をａ１補正値Δｒを
０に設定する（Ｓｒ２）、次に、加速中の制御対象の指
令位置ｒを、ステップＳＴ２で設定された内容に従って
次式ｒ　＝　ｒ　＋　ｖ　＋　ａ　／　２＋Δｒから算
出する（Ｓｒ１）。

ここで、ＤＳＰ２２は、ステップＳＴ３で指令位置ｒが
求められる毎に、この指令位置ｒを用いて、Ｄ／Ａコン
バータ３０に与えるための操作量を算出するフィードバ
ック補償の演算処理動作をプログラム処理により行なう
。

そして、ＤＳＰ２２は求められた操作量を、バス５０を
介して各軸毎のＤ／Ａコンバータ３０に与え、アームを
回動させる。

この処理を制御対象の指令速度がＶｔ１ｌａＫになるま
で続ける（Ｓｒ４）。指令速度がｖ　ｍ　ａ　ｘとなっ
たならば、ｍ　ｏ　ｄ　ｅレジスタを定速中モードに設
定すると共に、加速度をＯ１速度Ｖにｖｍａｘとする（
Ｓｒ１）。

そして、ｍｏｄｅレジスタの内容をチエツクして定速走
行を続け（Ｓｒ１）、（指令位置ｒｆ−現在位置ｒｏ　
）≦（ｒ、　＋　Ｖｍａｘ　）なる条件を満たすとき減
速中モードに入る（Ｓｒ７）。これにより、ｍ　ｏ　ｄ
　ｅレジスタを減速中モートに設定し、減速の指令加速
度を−３２とするとともに、この時点上〇での指令位置
ｒの値をｒｏとして、補正値Δｒを、 Δｒ＝　（ｒｆ−ｒｏ　　ｒｌ　）／Ｔｓから計算する
。この部分が補正量計算手段に相当する。そして修正指
令位置演算手段に相当するＳｒ１に戻り、新たな指令位
置ｒを演算して、この手順をｒ≧ｒｆとなるまで続行し
く５Ｔ９）、この条件を満たしたとき、ｍ　ｏ　ｄ　ｅ
レジスタを停止モードに設定すると共に、制御対象の指
令速度加速度を０、速度な０、位置指令ｒを指令位置ｒ
（とする（ＳＴＩＯ）。

以上の手順においてサンプリング毎に処理ＳＴ３を行う
ことにより補正前の指令位置をｒｔとしたときの補正後
の目標位置ｒｔをｒ＝ｒ’　＋　（ｔ−ｔｏ　）Δｒ として出力したこととなる。

従って本実施例によれば、各時刻における指令位置を複
雑な関数を計算することなく実行できるから、指令位置
の算出を高速に行うことができる。尚、上記の実施例に
おいて停止地点ｒｅにおいて制御対象の指令速度が完全
に０にならず、Δｒの速度をもつ場合があるが、この速
度は非常に小さく実際には殆ど停止したものとして差支
えない。また、本実施例では、指令位置ｒが増加する方
向の場合を示しているが、減少する方向の場合も同様で
ある。

第３図は本発明に係るディジタルサーボ制御用関数発生
装置の第２の実施例の作動を示すものである。

この実施例において制御対象をより円滑に移動させるた
め、等加速度運動ではなく、第４図（３）に示すように
、移動開始時、加速停止時、減速開始時及び停止時にシ
ョックを与えないため、指令加速度を０として加速度を
変化させるようにしたものである。このような状態にお
いて、制御対象の指令速度及び、指令位置は第４図（２
）及び第４図（１）に示すようになる。尚、第４図（４
）は指令加速度の変化率を示すものである。そして、本
実施例において、制御対象の加速の状態によりモード■
乃至モート■として制御を行うものとしている。

先ず、各指令加速度の変化率ｄｌ乃至ｄ４及び最高指令
速度Ｖ□８、から指令速度の最大値ａ工、最小値−ａｚ
、第１の減速区間の減速時間ｔ、□、第２の減速区間の
減速時間ｔ、２、第１の減速区間における移動距＃ｒｓ
□及び第２の減速区間の移動距＃ｒｓ□を演算しく５Ｔ
２１）、初期条件の設定を行ったのち（Ｓｒ２２）、こ
れに基づいてｒ、ｖ、ａを計算し、モードエである第１
の加速（Ｓｒ１・４）、モードＩＩである第１Ｉの加速
（Ｓｒ２５，５Ｔ２６）、−［ニードｍである定速走行
（Ｓｒ２７，５Ｔ２８）を行う。そしてモード■である
第１の減速区間では次の第２の減速区間を含めた形で補
正量Δｒを算出しく５Ｔ２９）、これに基づいて補正を
行う（Ｓｒ２３゜５Ｔ３０）。さらにモードＶである第
２の減速区間でも新たな補正量Δｒを算出しく５Ｔ３１
）、補正を実行しく５Ｔ２３，５Ｔ３２）所定の位置で
停止する（Ｓｒ３３）。尚、（Ｓｒ２１）でａｌ　、ａ
ｚ　、”ｒ、、、”ｒ、Ｑ、ｒ、１．ｒ、２を計算する
のに複雑な計算を行なう必要があるが、この計算は、ｄ
１〜ｄ４及びＶ□８を入力した時点で１度だけ行なえば
よいので、サンプリング間隔が長くなる悪影響はない。

従ってこの実施例に係るディジタルサーボ制御用関数発
生装置にあっては複雑な位置制御を行う場合であっても
各サンプリングごとに複雑な関数の計算をすることなく
容易な計算を実行するだけで制御対象の指令位置を補正
することができる。

（発明の効果） 以上説明したように、本発明によればディジタルサーボ
制御用関数発生装置の指令位置を容易な計算で補正する
ことができるので、補正に伴なう計算を高速に処理する
ことができるという効果を奏する。

【図面の簡単な説明】

第１図は本発明の原理図、第２図は本発明に係るディジ
タルサーボ制御用関数発生装置の第１の実施例の作動を
示すフロチャート、第３図は本発明に係るディジタルサ
ーボ制御用関数発生装置の第２の実施例の作動を示すフ
ロチャート、第４図（１）、（２）、（３）、（４）は
第２の実施例に係る制御対象への指令値の挙動を示す図
、第５図は本発明が適用されるディジタルサーボ装置を
示すブロック図、第６（１）、（２）、（３）は一般の
ディジタルサーボ制御される制御対象への指令値の挙動
を示す図、第７図（１）、（２）は従来のディジタルサ
ーボ制御用関数発生装置の不具合を示す図、第８図は本
発明が適用されるロボットのディジタルサーボ制御のハ
ードウェア構成例を示すブロック図である。 １・・・減速移動量演算手段 ２・・・補正量演算手段 ３・・・修正指令位置演算手段 本発明の原理図 第ｉｌｌ ＪｆＪｔ炙范例の伴動 ディジｙルザー本ψＩ 第 悶 ↑ 一般の柘今後の手動を示１図 第 図 従未ｎｌ積令乞ホ１図 第 図

Claims (1)

1. 【特許請求の範囲】 　ディジタルサーボ制御の制御対象の目標停止位置ｒ＿
   ｆに至るまでのサンプリング時刻ｔにおける指令位置ｒ
   をサンプリング時刻の非連続性により発生する誤差を補
   正して出力するディジタルサーボ制御用関数発生装置に
   おいて、 制御対象を停止するために減速指令を開始する時刻をｔ
   ＿ｏとし、減速に必要とする時間をＴ＿ｓとし、時刻ｔ
   ＿ｏから時刻ｔ＿ｏ＋Ｔ＿ｓにおける制御対象の指令移
   動量ｒ＿ｓを演算する減速移動量算出手段と、 減速開始指令位置ｒ＿ｏとして補正量Δｒを、Δｒ＝（
   ｒ＿ｆ−ｒ＿ｏ−ｒ＿ｓ）／Ｔ＿ｓとして演算する補正
   量演算手段と、 補正前の指令位置をｒ′として、時刻ｔにおける指令位
   置ｒを ｒ＝ｒ′＋（ｔ−ｔ＿ｏ）Δｒ として出力する修正指令位置演算手段とを有することを
   特徴とするディジタルサーボ制御用関数発生装置。