JPH0199102A - サーボ制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［産業上の利用分野〕 本発明は、ロボット等のサーボ機構を制御するサーボ制
御装置に関する。

［従来の技術］ 自動組立工場等で使用されるロボット等のサーボ制御装
置は、制御プログラムに基づく動作指令に応じて所定の
演算を行ない、サーボ系の軌道を算出して位置指令とし
て出力する運動制御部と、その位置指令に応じてサーボ
系の位置を制御するサーボ制御部とを備え、そのサーボ
制御部からの制御信号によりロボットの位置決め装置を
作動させるように構成されている。

上記のサーボ制御装置において、運動制御部で行なわれ
る軌道生成及び座標変換には多くの演算を必要とし、１
回の位置を求める演算で数十ミリ秒から百ミリ秒程度の
計算時間を要する。このため、運動制御部で時々刻々変
化する指令位置を求める計算は、数十ミリ秒から百ミリ
秒程度の周期で行なわれる場合が多い。

これに対し、サーボ制御部では、サンプリング周期の大
きさが応答性や外乱除去特性に直接影響するため１周期
をできるだけ短くし、通常５ミリ秒又はそれ以下の周期
で演算処理が行なわれることが多い、このとき、上記の
運動制御部で計算された値を直接サーボ制御部での処理
に使用すると、サーボ制御部からの速度指令が運動制御
部での処理周期毎に不連続的に変化することになり、サ
ーポ系に振動が発生する原因となる。そこで、サーボ制
御部に補間演算部を設け、運動制御部からの数十ミリ秒
から百ミリ秒毎の指令を、その周期の整数分の一のサン
プリング周期毎に補間することが行なわれている。

従来は、上記の補間演算に次のような一次式を用いて、
各指令点（位置）の間を直線的に補間していた。第７図
（Ａ）に示すように、運動制御部からの位置指令は、７
ｇ１秒（軌道生成及び座標変換の周期）毎に計算される
。サーボ制御部では、この位置指令をＴａ２秒（サンプ
リング周期）毎に直線補間して実際の指令とする。

ここで、運動制御部からのｉ番目の位置指令なｘｒ　（
ｉ）とし、この指令に対してｊ番目に補間して求めた点
をＸ’ｒｉ、ｊ）とすると、ｘｌ、　（ｉ、ｊ）　＝　
ｘｒ　（ｉ−１）ただし、ｎは分割数（ｎ　＝　ＴＳｌ
　／Ｔｓ２　）ｉ≧０．１≦ｊ≦ｎ と表わされる。このとき、位置指令のＴａ２毎の差分（
指令速度）を求めると、第７図（Ｂ）に示すように７３
Ｌ秒毎に階段状に変化する。すなわち、増分量をΔｘ’
ｒ（＋、ｊ）とすると、 Δｘｌ、　（ｉ＋ｊ）　＝　Ｘ’ｒ　（ｉ、ｊ）　−ｘ
’ｒ　（ｉ、ｊ−１）＝　（ｘｒ　（ｉ）　−ｘｒ　（
ｉ−１））÷ｎとなり、ｊの値によって変化せず、一定
である。

上記の一次式補間によれば、サーボ系の応答がＴｓｌ　
　（指令を与える周期）よりも遅い場合は、指令速度が
階段状に変化した場合でも、実際の速度応答は滑らかに
変化するので、特に問題はない。

［発明が解決しようとする問題点］ しかしながら、サーボ系の応答が速い場合、特に速度指
令のフィードフォワードのように、制御系に微分要素を
含む場合には、与えられた速度指令を忠実に実現するよ
うに応答する。そのため、実際の速度応答も階段状に変
化し、加速度の急激な変化により振動を生ずる場合があ
った。

第８図は、従来の補間方式による制御結果として、サー
ボ制御部からの速度指令をＤ／Ａ変換した出力と、実際
の速度フィードバック信号の記録を示す。この実験結果
に示すように、上記の一次式による補間では、運動制御
部からの位置指令の周期（この場合２０ミリ秒）毎に速
度指令（Ｄ／Ａ変換出力）が階段状に変化し、それに伴
なう振動が発生していた。

このように、従来の一次式による補間では、速度のフィ
ードフォワード制御のような応答の速いサーボ制御方式
を用いると、サーボ系に振動を発生する。そこで、応答
の遅いサーボ系を使用すると、高速移動時には必ず指令
値と現在値との間に一定の誤差が発生するという問題点
があった。

本発明は、かかる問題点に鑑みてなされたものであり、
位置指令の補間で速度が連続的に変化するようにし、応
答の速いサーボ制御方式でも振動が発生せず、高速移動
時の誤差を最小にして補間の精度が向上したサーボ制御
装置を提供することを目的とする。

［問題点を解決するための手段］ 本発明は、所定の周期でサーボ系の位置を計算する運動
制御部と、その周期の整数分の一のサンプリング周期で
サーボ系の制御指令を出力するサーボ制御部とを具備し
、サーボ制御部は、運動制御部からの位置指令を上記サ
ンプリング周期毎に補間する補間演算部を含み、該補間
演算部で二次式を用いた補間を行なうことにより補間点
の間の制御指令が連続的に変化するように構成したこと
を特徴とする。

［作用］ 本発明によれば、運動制御部からの位置指令はサーボ制
御部でサンプリング周期毎に補間されるが、その際、補
間演算部では二次式に基づく補間を行なう、その結果、
サーボ制御部から出力される各指令点の間の速度が連続
的に変化することになり、応答の速いサーボ制御方式を
採用しても振動が発生しない。

［実施例］ 第１図は本発明によるサーボ制御装置の構成を示す。こ
のサーボ制御装置は、プログラムを解読して動作指令を
出力する言語処理部１と、その動作指令に応じて所定の
演算を行ない、サーボ系の軌道を算出して位置指令とし
て出力する運動制御部２と２　その位置指令に応じてサ
ーボ系の位置を制御するサーボ制御部３とを備えている
。

このサーボ制御装置によって制御されるロボット（サー
ボ機構）は、各関節を駆動するモータ４を駆動源とし、
上記サーボ制御部３からの制御信号（速度指令）をアナ
ログ変換するＤ／Ａ変換器５と、そのアナログ信号を増
幅して各モータ４に供給するサーボアンプ６とを有する
。更に、各モータ４の回転角から各々の関節位置を検出
するエンコーダ７と、そのエンコーダからの周波数出力
ＩＣ電圧信号に変換してサーボアンプ６に供給するＦ／
Ｖ変換器８が設けられている。エンコーダ７からの現在
位置信号は、サーボ制御部３にフィードバックされる。

動作の際には、プログラム言語処理部ｌで制御プログラ
ムを解読して動作指令を出力し、その動作指令に応じて
運動制御部２が所定の演算を行ない、ロボットの軌道を
算出して位置指令として出力する。運動制御部２におけ
る演算は直線補間、円弧補間、関節角補間等であり、直
線補間及び円弧補間時の軌道は、通常直交座標系上の値
として計算されるため、これに座標変換の演算を施して
関節座標に変換する。サーボ制御部３は、その関節座標
値に基づいてロボットの各関節角のサーボ制御信号を出
力する。すなわち、サーボ制御部３では、運動制御部２
から送られた関節角指令値とエンコーダ７からフィード
バックされた関節角現在値との差を各制御軸毎に求め、
これにＰＩＤ等の補償演算を行なって得られた速度指令
をＤ／Ａ変換器５を介してサーボアンプ６に供給する。

サーボアンプ６は、Ｄ／Ａ変換器５からの速度指令信号
とＦ／Ｖ変換器８からの速度フィードバック信号により
、モータ４を指令速度と同一の速度で回転させる駆動信
号を出力する。

第２図は、上記のサーボ制御部３の構成を示す。

サーボ制御部３は、前述の振動発生を防止する補間演算
を行なう補間演算部１１と、ここで演算された結果を格
納する補間位置バッファ１２と、サーボアンプ６への速
度指令を生成するサーボ処理部１３とを有する。

動作の際には、第１図の運動制御部２から数十ミリ秒〜
百ミリ秒周期の位置指令が補間演算部１１に送られる。

補間演算部１１は、位置指令が来る毎にその前の指令値
と新しい指令値との間で補間演算を行ない、その結果得
られた値を補間位置バッファ１２に格納する。サーボ処
理部１３は、補間位置バッファ１２に格納された値を順
次（例えば５ミリ秒の周期で）取り出し、これと前記エ
ンコーダ７からの位置フィードバック信号によりサーボ
処理を行なって速度指令を出力する。

第３図は、上記の補間演算部１１における補間演算方式
を示す０図において、 Ｘｒ　Ｃｉ）はｉ番目の位置指令、 ΔＸｒ　（ｉ）はｉ番目の位置指令増分量。

Ｘ’ｒ（ｉ、ｊ）はｉ番目の位置指令に対するｊ番目の
補間値、 ΔＸ’ｒ（ｉ、ｊ）はｊ番目の補間値の増分量、ただし
、ｉ≧０．１≦ｊ≦ｎ ｎは補間分割数 とすると１次の関係が成り立つ。

Δ−ｘｒ　（ｉ）　＝　ｘｒ　（ｉ）　−ｘｒ　（ｉ−
１）　　　−−−−（３）ΔＸ’ｒ　（ｉ、ｊ）　＝　
Ｘ’ｒ　（ｉ、ｊ）　−Ｘ’ｒ　（ｉ＋ｊ−１）ただし
、ｘ’ｒ（ｉ、Ｏ）　＝　ｘｒ　（ｉ−１）とする。

ここで、補間値の増分量ΔＸ’ｒ（＋、ｊ）を、その前
の補間区間の最後の増分量ΔＸ’ｒ　（ｉ−１，ｎ）を
基準として、一定の増分で変化させることにする。

すなわち、 Δ”ｒ　（’ＩＪ）　＝ΔＸ’ｒ　（ｉ、ｊ−１）　＋
　Ｃ（ｉ）　−−−一（８）Δ”ｒ　（ｉＴＯ）　＝Δ
Ｘ　’ｒ　（ｉ−１，ｎ）Ｃ（ｉ）はΔ”ｒ（ｉ＋ｊ）
の増分量であり、補間区間内で一定とする。

このとき、式（６）より Δｘ’ｒ　（’１ｊ）　＝ΔＸ’ｒ　（ｉ−１，ｎ）　
＋　ｊ　＊　Ｃ（ｉ）−〜−一（７） となる。式（５）と式（７）より Δｘｒ　（ｉ）＝ｎ争ΔＸ　’ｒ　（ｉ−１，ｎ）とな
るので、Ｃ（ｉ）の値は Ｃ（ｉ）　＝　（ΔＸｒ　（ｉ）　−ｎ曇ΔＸ’ｒ　（
＋−’＋１））によって求めることができる。

各補間位置は、式（４）と式（７）よりｘ　’ｒ　（＋
　ＩＪ）　＝　Ｘ　’ｒ　（＋　＋ｊ−１）＋Δ”ｒ（
ｌｅｊ）＝Ｘｒ（ｉ）　　＋Δ　ｘ’ｒ（ｉ、ｌ）＋−
−−−＋ΔＸ’ｒ（＋、ｊ） ＝Ｘｒ（ｉ） ＋（Δｘ　’ｒ（ｉ−１，ｎ）　＋　Ｃ（ｉ））＋　　
　−−一− ＋（Δｘｌ、　（ｉ−１，ｎ）　＋　ｊ　Ｃ（ｉ））＝
Ｘｒ（ｉ）＋ｊ・Δｘｌ、　（ｉ−１，ｎ）として求め
ることができる。これは、ｊの二次式になっている。実
際の処理では１式（８）でＣ（ｉ）を求めた後、式（６
）と式（０の漸化式によって補間値を計算する。まとめ
ると、次のようになる。

Ｃ（ｉ）　　＝　　（Ｘｒ　　（ｉ）　　−Ｘｊ　　（
ｉ−１）−ｎ・Δｘ　’、　（ｉ−１，ｎ）） ？ Ｘ　　　　　　　　　　　　　　　　　　　　−−−（
１１）ｎ　　（ｎ＋１） Δ”ｒ　ＩＩＪ）　　＝Δｘ’ｒ（ｉ、ｊ−１）＋Ｃ（
ｉ）−一−（１２） ただし、ΔＸ’ｒ（ｉ、０）　−Δｘｌｒ　（ｉ−１，
ｎ）ｘ　’ｒ　ｉ　ＩＪ）　　＝Ｘ　’ｒ　（ｉ　ＩＪ
−１）　　＋Δｘ’ｒ（＋、ｊ）ただし、Ｘ’ｒ　（ｉ
、０）　＝　Ｘｊ　（ｉ−ｔ）また、Ｘｒ（Ｑ）＝０．
Δｘ’ｒ　（０、ｎ）　＝　０とする。

［実験結果］ ■上記の二次式補間を用いたサーボ制御の実験を以下の
条件で行なった。

位置指令周期：　２０ミリ秒 サーボ処理周期：　５ミリ秒 最高値：　　　　　　３　ｒａｄ／秒 第４図は、Ｄ／Ａ変換器とＦ／Ｖ変換器の各出力の記録
を示す、この実験結果に示すように、上記の二次式によ
る補間では、上記のサーボ処理局゛期（５ミリ秒）毎に
速度指令（Ｄ／Ａ変換器出力）が滑らかに変化しており
、実際速度（Ｆ／Ｖ変換器出力）も振動を生じていない
。

■上記の二次式補間を施した入力に対してフィードフォ
ワードがある場合とない場合の比較実験を行なった。

第５図はフィードフォワードなしの場合、第６図はフィ
ードフォワードありの場合の結果で、どちらも指令位置
、応答位置、及び誤差（指令位置一応答位置）を１０倍
した値を示している。フィードフォワードなしの場合は
、応答の遅れがあり。

はぼ速度に比例した誤差が発生した。最大誤差は約２０
００パルスである。一方、フィードフォワードありの場
合は、応答の遅れがなく、誤差も微小であった（最大で
約１００パルス）。

この実験結果に示されているように、二次式を用いて指
令点の間の速度を連続的に変化させる補間演算部を、速
度のフィードフォワード制御のような速い応答性を有す
るサーボ制御部に使用した場合には、誤差が非常に小さ
く、振動のない運動を実現することができる。

以上、実施例について説明したが、本発明はこれに限ら
れるものではない、特に、サーボ制御部の補間演算部は
、上記のような二次式に基づく補間演算を行なうもので
あればよく、その構成は任意に設計することができる。

［発明の効果］ 以上のように、本発明のサーボ制御装置は、位置指令の
補間で連続的に変化する速度指令を出力するものである
から、振動が発生せず、滑らかな運動を実現することが
できる。また、応答の速いサーボ制御系を用いて高速移
動時の誤差を最小にし、連続軌跡制御等の位置決め応答
性を向上させることができる。

【図面の簡単な説明】

第１図はサーボ制御装置の構成図、 第２図はサーボ制御部における補間演算手段をを示す図
、 第３図は本発明のサーボ制御装置における二次式補間方
式を示す図、 第４図は第３図の二次式補間方式による実験結果を示す
グラフ図、 第５図は二次式補間入力にフィードフォワード制御を行
なわない場合の実験結果を示すグラフ図、第６図は二次
式補間入力にフィードフォワード制御を行なった場合の
実験結果を示すグラフ図、第７図は従来のサーボ制御装
置における一次式補間方式を示す図、 第８図は第７図の一次式補間方式による実験結果を示す
グラフ図である。 ｌ−一一一プログラム言語処理部、 ２−一一一運動制御部、 ３−一一一す−ポ制御部。 ４−−−−モータ、 ５−−−−Ｄ　／　Ａ変換器、 ６−−−−サーボアンプ。 ７−−−−エンコーダ、 ８−−−−Ｆ／Ｖ変換器、 １１−−−一補開演算部、 １２−−−一補間位置バッファ、 １３−−−−サーボ処理部。

Claims (1)

【特許請求の範囲】
1. 所定の周期でサーボ系の位置を計算する運動制御部と、
   その周期の整数分の一のサンプリング周期でサーボ系の
   制御指令を出力するサーボ制御部とを具備し、該サーボ
   制御部は、前記運動制御部から送られる位置指令を前記
   サンプリング周期毎に補間する補間演算部を含み、該補
   間演算部で二次式を用いた補間を行なうことにより補間
   点の間の制御指令が連続的に変化するように構成したこ
   とを特徴とするサーボ制御装置。