JPH02128202A - ディジタル・サーボ制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔（既要〕 位置決め制御と力制御の両方を行い得るディジタル・サ
ーボ制御装置の改良に関し、 この種のディジタル・サーボ制御装置において、位置決
め制御時の制御特性と力制御時の制御特性の両方とも良
好なものとすることを目的とし、関数発生器のプログラ
ム・エントリを位置制御用と速度制御用に分離し、通常
の位置決めモードの場合にはフィードバック制御演算を
行ってから位置制御用の関数発生演算を行い、力センサ
−・フィードバック・モード時には速度制御用の関数発
生演算を行ってからフィードバック制御演算を行うもの
である。

〔産業上の利用分野〕

本発明は、位置決め制御と力制御の両方を行い得るディ
ジタル・サーボ制御装置の改良に関するものである。

〔従来の技術〕

第７図はディジタル位置決め制御においてサンプリング
間隔を等しくする制御方法を説明する図である。同図に
おいて、■は加減速時の関数発生演算、■°は位置決め
時の関数発生演算、■はフィードバック制御演算、ＣＫ
はサンプリング・クロック、Ｔｔ　とＴ２はサンプリン
グ間隔をそれぞれ示している。

サーボ演算において、関数発生演算とフィードバック制
御演算の順番は、関数発生器の演算時間が加減速度モー
ドにより大幅に異なることから第７図に示すように、関
数発生演算→フィードバック制御演算の順序をとってい
た。仮にこれを逆にすると、サンプリングごとに関数発
生器の演算時間が違うためフィードバック制御のサンプ
リング間隔が変動し、制御性能を劣化させる。

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

第８図は従来のセンサ・フィードバック演算を説明する
図である。同図において、■はセンサー信号処理、Ｔ３
はセンサー人力からアクチュエータ出力までの時間をそ
れぞれ示す。

第８図は、２台の下位計算機を使用し、一方の下位計算
機が関数発生演算およびフィードバック演算を′行い、
他方の下位計算機がセンサー信号処理を行う場合のセン
サー・フィードバック演算を示している。フィードバッ
ク演算→関数発生演算と言う演算順序をセンサー・フィ
ードバック制御にそのまま適用すると、第８図のように
、センサー信号を受は取ってから当該センサー信号に対
応した操作信号を出力するまでの時間Ｔ、が３サンプリ
ングにわたって大きくなり、制御系の特性を落としてし
まう。

本発明は、この点に鑑みて創作されたものであって、位
置決め制御と力制御の両方を行い得るディジタル・サー
ボ制御装置において、位置決め制御時の制御特性と力制
御時の制御特性の両方とも良好なものとすることを目的
としている。

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

センサー・フィードバック時の関数発生器は速度制御モ
ード（単純な加速度のみ）だけしか使わない。この場合
、演算時間のばらつきは少ないので関数発生演算をフィ
ードバック制御演算の前に行ってもサーボのサンプリン
グ間隔を一定に保つことができる。

第１図は本発明のハードウェア構成を示す図である。同
図において、２１と２２は下位計算機、２３はホスト計
算機をそれぞれ示している。下位計算機２１は関数発生
切換フラグＦＮＣを有しており、下位計算機２２は力セ
ンサ−・フィードバック・ゲインに、を有している。ホ
スト計算機２３は、関数発生切換フラグＦＮＣＯ値およ
び力センサ−・フィードバック・ゲインに、を制御する
ことが出来る。

第２の計算機２２は、サンプリング・クロックＣＫが生
成された時に、力センサー信号を取り込み、取り込んだ
力センサー信号の値及び力センサ−・フィードバック・
ゲインに、の値を使用して指令速度ｒｖを算出する。

第１の計算機２１は、サンプリング・クロックＣＫが生
成された時に、第２の下位計算機２２によって算出され
た指令速度ｒｖを取り込み、その後に関数発生切換フラ
グＦＮＣの値が所定値か否かを調べ、所定値の場合には
フィードバック演算を行；た後に位置決めモード時の関
数発生演算を行い、所定値でない場合には速度制御モー
ド時の関数発生演算を行った後でフィードバック制御演
算を行う。

第２図は本発明による演算手順を示す図である。

第２図（ａ）は力センサ−・フィードバック時の演算手
順を示す図である。サンプリング・クロックＣＫ（第１
番目のクロックとする）が生成されると、下位計算機２
２は力センサー信号を取り込み、指令速度ｒｖ（ｉ）を
生成する。第ｉ＋１番目のサンプリング・クロックＣＫ
が生成されると、下位計算機２１は、指令速度ｒ、（ｉ
）を取り込み、その後に速度制御モード時の関数発生演
算■を行い、次いでフィードバック制御演算■を行う。

関数発生演算■の後にフィードバック制御演算■を行う
ことによって、力センサー人力からアクチュエータ出力
までの演算遅れＴ、を２サンプリング間隔に押さえるこ
とが出来る。

第２図（ｂ）は通常の位置決め時の演算手順を示す図で
ある。サンプリング・クロックＣＫが生成されると、下
位計算機２１は指令速度ｒｖを取り込むが、通常の位置
決め時にはＫｔはＯに設定されているので、指令速度ｒ
ｖは０になっている。このために、力センサー信号は何
らの効果も持たない。指令速度ｒｖを取り込んだ後、下
位計算機２１はフィードバック制御演算■を行い、次い
で位置決めモード時の関数発生演算■を行う。

〔実施例〕

第３図は力制御の概念図である。

同図において、 ＩＯはコントローラ、１１は電流アンプ、１２はＤＣモ
ータ、１３はエンコーダ、１４はロボット・アーム、１
５は力センサーをそれぞれ示している。

コントローラ１０は、例えばホスト計算機と下位計算機
とから構成されている。コントローラ１０の出力した操
作信号は電流アンプ１１で増幅され、ＤＣモータ１２に
印加される。ＤＣモータ１２の位置はエンコーダ１３に
よって検出され、エンコーダ１３からの信号がコントロ
ーラ１０に取り込まれる。ＤＣモータ１２によってロボ
ット・アーム１４が動かされる。ロボットに作用する力
は力センサ−１５によって検出され、力センサー信号が
コントローラ１０に取り込まれる。

第４図は制御系のブロック線図である。同図において、
ＧＣ（Ｓ）はフィードバック・コントロール要素、Ｃ１
はパワー・アンプ・ゲイン、ｇ、は負荷ゲイン、Ｇｆは
力センサ−・ゲイン、Ｋ、はカフィードバック・ゲイン
、ＦＮＦは位置決め制御用関数発生器、ＦＮｖは速度制
御用関数発生器、「、は指令位置、ｒｒは力の指令値、
ｅｒは力の偏差、ｒｖは指令速度、ｒは目標位置、ｅは
位置偏差、Ｕは操作信号、ｉはモータ電流、ｙは現在位
置、ｓｗは関数発生切換スイッチをそれぞれ示している
。

位置決め制御の場合には、スイッチｓｗが位置決め制御
用関数発生器ＦＮ、に接続される。位置決め制御用関数
発生器ＦＮ、は、指令位置ｒ、に基づいて時間−目標位
置曲線を生成するものである。目標位置ｒから現在位置
ｙを減算することにより位置偏差ｅが生成される。フィ
ードバック・コントロール要素Ｇ、：（Ｓ）は、位置偏
差ｅに基づいて操作量Ｕを生成する。電流アンプは操作
量Ｕを増幅する。電流アンプから出力される電流ｉはＤ
Ｃモータに印加される。ロボットの現在位置ｙはフィー
ドバックされる。位置決め制御の場合には、力センサー
信号は無視される。

力制御の場合には、スイッチｓｗは速度制御用関数発生
器ＦＮｖに接続される。力の指令値ｒ。

から力センサー出力ｆが減算され、力の偏差ｅｆが得ら
れる。力の偏差ｅ、にカフィードバック・ゲインを掛算
することにより、指令速度ｒｖが得られる。速度制御用
関数発生器ＦＮｖは、指令速度ｒｖに基づいて目標位置
−時間曲線を生成する。

その他の点については位置決め制御の場合と同じである
。

第５図はコントローラの構成例を示す図である。

同図において、２１と２２は下位計算機、２３はホスト
計算機、ＦＮＣは関数発生切換フラグ、Ｋ、は力センサ
−・フィードバック・ゲインをそれぞれ示している。

下位計算機２１および２２は、Ｄ　Ｓ　ｐ　（Ｄｉｇｉ
ｔａＩ　Ｓｉｇｎａｌ　Ｐｒｏｃｅｓｓｏｒ）と呼ばれ
るものである。下位計算機２１は関数発生やフィードバ
ック制御演算を行い、下位計算機２２は力センサー信号
に基づいて指令速度ｒｖを算出する。下位計算機２１と
下位計算機２２は、互いに通信を行うことが出来る。下
位計算機２２によって算出された指令速度ｒｖは、下位
計算機２１によって読み取られる。

ホスト計算機２３は、下位計算機２１．２２に対して指
令コマンドを送出す名ものである。ホスト計算［２３か
ら下位計算機２２の内部メモリ中にある力サンサー・フ
ィードバック・ゲインに、を操作し、力センサ−・フィ
ードバックするかどうかを設定する。また、下位計算機
２！の内部メモリ中にある関数発生切換フラグＦＮＣを
操作して、関数発生モードおよびサーボ演算の順序を入
れかえる。すなわち、力センサ−・フィードバック時は
に、（≠０）を適当な値にセットし、ＦＮＣ＝１にする
。逆に、位置決め時はＫｔ＝Ｏとしてセンサー信号を断
ち切り、且つＦＮＣ＝０をセットする。

第６図は下位計算機２１　（ＤＳＰＩ）における演算手
順を示す図である。

サンプリング・クロック割込みが発生すると、下位計算
４１２２１においては対応する処理プログラムが起動さ
れる。この処理プログラムの実行によって、ＦＮＣ−０
か否かが調べられる。ＦＮＣ≠０の場合には、最初に関
数発生器ｅｎ　ｔｒｙ２サブルーチンが呼び出され、関
数発生器ｅｎ　ｔｒｙ２サブルーチンから復帰した後、
フィードバック・コントローラ・サブルーチンが呼び出
される。ＦＮＣ＝Ｏの場合には、最初にフィードバック
・コントローラ・サブルーチンが呼び出され、フィード
バック・コントローラ・サブルーチンから復帰した後、
関数発生器ｅｎ　ｔｒｙｌサブルーチンが呼び出される
。

下位計算機２１の関数発生サブルーチン領域のｅｎｔｒ
ｙｌには、位置決め制御時における加速モード、その他
のときに使用される関数発生サブルーチンが格納されて
いる。また、関数発生サブルーチン領域のＢｎｔｒｙ２
には、速度制御加減速モード時に使用される関数発生サ
ブルーチンが格納されている。

〔発明の効果〕

以上の説明から明らかなように、本発明によれば、力セ
ンサ−・フィードバック時には、センサー信号を取り込
んでからアクチュエータに操作信号を出力するまでの演
算時間遅れを小さくし制御系の性能を上げることができ
、また、位置決め制御時には、関数発生モードによらず
一定のサンプリング処理が行え、位置決め制御性能も落
とさないと言う顕著な効果を奏することが出来る。

【図面の簡単な説明】

第１図は本発明のハードウェア構成を示す図、第２図は
本発明による演算手順を示す図、第３図は力制御の概念
を示す図、第４図は制御系のブロック線図、第５図はコ
ントローラの構成例を示す図、第６図は下位計算機２１
の演算手順を示す図、第７図は位置決め制御においてサ
ンプリング間隔を等しくする制御方法を説明する図、第
８図は従来のセンサー・フィードバック演算を示す図で
ある。 ｌＯ・・・コントローラ、１１・・・電流アンプ、１２
・・・ＤＣモータ、１３・・・エンコーダ、１４・・・
ロボット・アーム、１５・・・力センサ−，２１と２２
・・・下位計算機、２３・・・ホスト計算機、ＦＮＣ・
・・関数発生切換フラグ、Ｋｔ・・・力センサ−・フィ
ードバック・ゲイン。 特許出願人　　　富士通株式会社 代理人弁理士　　京　谷　四　部 本発明のハードウェア構成 第１区 ↑ ｅ＞すへ力 丁ｊ オ腎芒１月１２よる。Ｉ（’ＩＩ今ＩＩＩ員第Ｚ図 コントローラの構成例 第５図 ′ＤＳＰ１ 内ｆ叩演算今１１１負 第６図

Claims (1)

1. 【特許請求の範囲】 関数発生切換フラグ（ＦＮＣ）を持つ第１の計算機（２
   １）と、 力フィードバック・ゲイン（Ｋ＿ｆ）を持つ第２の計算
   機（２２）と、 関数発生切換フラグ（ＦＮＣ）の値および力フィードバ
   ック・ゲイン（Ｋ＿ｆ）の値を制御するホスト計算器（
   ２３）と を具備し、 第２の計算機（２２）は、サンプリング・クロック（Ｃ
   Ｋ）が生成された時に、力センサー信号を取り込み、取
   り込んだ力センサー信号の値及び力センサー・フィード
   バック・ゲイン（Ｋ＿ｆ）の値を使用して指令速度ｒ＿
   ｖを算出するように構成され、第１の計算機（２１）は
   、サンプリング・クロック（ＣＫ）が生成された時に、
   第２の下位計算機（２２）によって算出された指令速度
   ｒ＿ｖを取り込み、その後に関数発生切換フラグ（ＦＮ
   Ｃ）の値が所定値か否かを調べ、所定値の場合にはフィ
   ードバック演算を行った後に位置決めモード時の関数発
   生演算を行い、所定値でない場合には速度制御モード時
   の関数発生演算を行った後でフィードバック制御演算を
   行うように構成されている ことを特徴とするディジタル・サーボ制御装置。