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

Abstract

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

Description

【発明の詳細な説明】 〔概要〕 ディジタルサーボ制御用関数発生装置に係り、特にフィ
ードバック補償器に位置、速度及びその高次の導関数を
発生するディジタルサーボ制御用関数発生装置に関し、 制御対象の加速度、速度及び位置の関係に誤差を含まな
いように制御することができるようにすることを目的と
し、 上述のようなディジタルサーボ制御用関数発生装置にお
いて、指定された速度及びその高次の導関数から速度あ
るいは高次の導関数が一定である区間の時間を算出する
時間算出部と、算出された時間を制御系のサンプリング
周期の整数倍に補正する時間補正部と、補正された時間
に基いて指定された速度及びその高次の導関数を補正す
る関数補正部と、この補正された値に基づいて関数を発
生する関数発生部とを備えるものとして構成する。

〔産業上の利用分野〕

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

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

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

ところでこのようなディジタルサーボ系においては、ア
ナログサーボ系のように連続して位置の制御をするので
はなく、第８図に示すように、離散的に設けたサンプリ
ング時刻における指令位置の関数を発生して、制御対象
の制御を行なうようにしている。

ここで、制御対象を所定の速度とする制御を行なう場合
において、目標速度Ｖと加速度ａを指定すると、 ｖ＝ａＴ の関係から、加速すべき時間Ｔが定まる。しかしながら
、サンプリング時刻は離散的であるためＴ　＝　ｖ　／
　ａが必ずしもサンプリング時刻の整数倍とならず制御
が正確に行なわれないことがある。

これは、第８図（１）に示すように、Ｔがサンプリング
時刻ｔ１とｔ２の間になる場合であり、このような場合
、時刻ｔ１で加速を停止したのでは目標速度Ｖに達しな
いし、時刻ｔ２まで加速したのでは目標速度Ｖを越えて
しまうこととなる。

従来このような場合には、第８図（２）に示すように、
加速期間中に加速度を変えて、結果的に速度を目標値に
するものとしている。

しかしながら、このような補正を行なうと、時間と速度
、加速度及び位置の関係に誤差を含むものとなる。この
ような値に基づいて以後の制御を行なうのでは制御対象
の正確な制御を行なうことはできないという問題がある
。

そこで、本発明は時間と制御対象の加速度、速度及び位
置の関係に誤差を含まないように制御することができる
ディジタルサーボ制御用関数発生装置を提供することを
目的とする。

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

本発明にあって、上記の課題を解決するための手段は、
第１図に示すように、ディジタルサーボ制御のフィード
バック補償器に位置、速度及びその高次の導関数を発生
するディジタルサーボ制御用関数発生装置１において、
指定された速度及びその高次の導関数から速度あるいは
高次の導関数が一定である区間の時間を算出する時間算
出部２と、算出された時間を制御系のサンプリング周期
の整数倍に補正する時間補正部３と、補正された時間に
基いて指定された速度及びその高次の導関数を補正する
関数補正部４と、この補正された値に基づいて関数を発
生する関数発生部５とを備えたことである。

〔作用〕

本発明によれば、制御対象はサンプリング時刻の整数倍
となるように補正された時間、この補正時間に基づき補
正された速度及びその高次の導関数に基づき制御される
から、時間と加速度、速度及び位置の関係には誤差を含
まないものとなる。

従って、制御対象を正確に制御することができる。

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

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

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

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

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

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

そしてこれらの値から下位計算機は、加速及び減速に要
する時間Ｔ□、Ｔ２の値を Ｔ□＝ｖ２／ａ□ Ｔ２＝ｖ２／−ａ３ から求める（ＳＴ２）。このステップが時間算出部の作
動に相当する。

求めたＴ、、Ｔ２がサンプリングタイムの整数倍となる
ようにサンプリングタイム基準として、はんば部分を切
り上げ、補正した加速時間及び減速時間Ｔ　’ｉｎ　Ｔ
’２を求める（ＳＴ３）。これにより、加速及び減速の
開始及び終了時刻はサンプリングタイムと同期する。こ
のステップが時間補正部の作動に相当する。

次にこの補正した加速時間Ｔ“１及び減速時間Ｔ゛２か
ら新たな加速度ａ’１．ａ’３を、ａ’１＝ｖ２／Ｔ’
。

ａ’３＝−ｖ２／Ｔ’２ から求める（Ｓｒ１）。

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

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

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

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

このようにしてロボットを所定の加速度及び速度で移動
させ、所定の個所で停止させることができる。この際、
時刻、位置、加速度及び速度の関係は正確に保たれてい
るので、ロボットの制御を正確に行なうことができる。

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

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

次に、求めたＴＩ　、Ｔ２　、Ｔ３　、Ｔａ　、Ｔ６が
サンプリングタイムの整数倍となるようにサンプリング
タイム基準として、はんば部分を切り上げ、補正した加
速時間及び減速時間Ｔ°□、Ｔ’２゜Ｔ　’３＋　Ｔ’
５．Ｔ’６を求める（Ｓｒ２２）。これにより、加速変
化の開始及び終了時刻はサンプリングタイムと同期する
。このステップが時間補正部の作動に相当する。

次にこの補正した時間Ｔ′□ＨＴ　’　２　ＨＴ　’　
３　＋Ｔ　’　６＋　Ｔ　’　６から新たな加速度の変
化率ｄ′□。

ｄ　’　２　＊　ｄ　’　３　Ｈｄ　’　５　、　ｄ“
６を、として求める（Ｓｒ２３）。

更に初期設定としてｋを０とし、ａ　（ｋ）にａ“１を
設定してｍ　ｏ　ｄ　ｅレジスタを加速度増加モード（
モード１）に設定する（Ｓｒ２４）。

そして制御対象の指令位置ｒ（ｋ＋１）　、指令速度ｖ
（ｋ＋１）　、指令加速度ａ（ｋ）をｒ（ｋ＋１）　＝
　ｒ（ｋ）　＋　ｖ（ｋ）　＋　３／２ａ（Ｋ）　＋　
％ｄ（ｋ）ｖ（ｋ＋１）　−ｖ（ｋ）　＋　ａ（ｋ）　
”　３４ｄ（ｋ）ａ　（ｋ＋１）　＝　ａ　（ｋ）　＋
　ｄ　（ｋ）で計算した後、ｋに１を加算する（Ｓｒ２
５）。

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

この処理をｍ　ｏ　ｄ　ｅレジスタの内容をチエツクし
つつ、ｋがＴ′□になるまで続け（Ｓｒ２７）、ｋがＴ
’ｌになったら加速度の増加率ｄ（ｋ）を０、ｋを０に
設定してｍ　ｏ　ｄ　ｅレジスタを一定加速モード（モ
ード２）にする（ＳＴ２Ｂ）。

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

そしてロボットアームの指令位置ｒ　（ｋ）がｒ５（減
速開始位置）に来るまで一定速度ｖ４による移動を続け
る。ロボットアームの指令位置ｒ（ｋ）かｒ５に到達し
たら、ｄ　（ｋ）をｄ５、ｋをＯとしてｍ　ｏ　ｄ　ｅ
レジスタを第１の減速モード（モード５）に設定する。

このモードにおいて負の加速度を変化率ｄ５の割合で大
きくしていき、ｋがＴ’ｌになるまでこの処理を続け（
Ｓｒ３５）、ｋがＴ’５になった時点で、ｍ　ｏ　ｄ　
ｅレジスタを第２の減速モード（モード６）として（Ｓ
ｒ３６）、加速度を変化率ｄ１６の割合でＯに近付けて
いき、ｋがＴ’６になった時点で（Ｓｒ３７）、速度Ｏ
加速度０、加速度の変化率０としてｍ　ｏ　ｄ　ｅレジ
スタを停止モード（モードＯ）とする（Ｓｒ３７）。こ
れによりロボットのアームを所定位置から移動して停止
させることができる。従って、本実施例によれば、加速
度の変化率が一定である時間はサンプリング時間の整数
倍となっており、それらの値を変化させるのはサンプリ
ング時刻と同期したものとしているため、時間、位置、
速度加速度、加速度の変化率の関係を、誤差を含まない
正確なものとすることができ、ロボットの制御を正確に
行なうことができる。

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

〔発明の効果〕

以上説明したように、本発明によれば、ディジタルサー
ボ制御用関数発生器に指定された速度及びその高次の導
関数から指定速度あるいはその高次の導関数が一定であ
る区間の時間を算出する時間算出部と、算出された時間
を制御系のサンプリング周期の整数倍に補正する時間補
正部と、補正された時間に基いて指定された速度及びそ
の高次の導関数を補正する関数補正部と、この補正され
た値に基づいて関数を発生する関数発生部とを備えるよ
うに構成したから、速度あるいはその高次の導関数が一
定である区間の時間はサンプリング時間の整数倍となっ
ており、またそれらの値を変化させるのはサンプリング
時刻と同期したものとしているため、時間、位置、速度
加速度、加速度の変化率の関係を、誤差を含まない正確
なものとすることができ、制御対象を正確に制御するこ
とができるという効果を奏する。

【図面の簡単な説明】

第１図は本発明の原理図、第２図は本発明に係るディジ
タルサーボ制御用関数発生装置の第１の実施例の作動を
示すフローチャート、第３図は第２の実施例における制
御対象の移動状態を示す図、第４図は本発明に係るディ
ジタルサーボ制御用関数発生装置の第２の実施例の作動
を示すフローチャート、第５図は本発明の関数発生装置
が適用されるディジタルフィードバック系を示す図、第
６図はディジタルフィードバック制御されるロボットの
ハードウェアの構成を示す図、第７図はディジタルフィ
ードバック制御される制御対象の動きを示す図、第８図
は従来のディジタルサーボ制御用関数発生装置の不具合
を示す図である。 ｌ・・・関数発生装置 ２・・・時間算出部 ３・・・時間補正部 ４・・・関数補正部 ５・・・関数発生部

Claims (1)

1. 【特許請求の範囲】 ディジタルサーボ制御のフィードバック補償器に位置速
   度及びその高次の導関数を発生するディジタルサーボ制
   御用関数発生装置（１）において、 指定された速度及びその高次の導関数から速度あるいは
   高次の導関数が一定である区間の時間を算出する時間算
   出部（２）と、 算出された時間を制御系のサンプリング周期の整数倍に
   補正する時間補正部（３）と、 補正された時間に基いて指定された速度及びその高次の
   導関数を補正する関数補正部（４）と、この補正された
   値に基づいて関数を発生する関数発生部（５）とを備え
   たこと特徴とするディジタルサーボ制御用関数発生装置
   。