JPH0519861A

JPH0519861A - サーボモータのフイードフオワード制御方式

Info

Publication number: JPH0519861A
Application number: JP3255977A
Authority: JP
Inventors: Heisuke Iwashita; 平輔岩下
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1990-11-08
Filing date: 1991-09-09
Publication date: 1993-01-29
Anticipated expiration: 2013-02-04
Also published as: JP2709766B2

Abstract

(57)【要約】【目的】フィードフォワード制御を行なうサーボ系に
おいて、サーボ系の指令追従性を向上させる。【構成】当該位置・速度ループ処理における周期を中
心に分配周期を位置・速度ループ処理周期で割った数Ｎ
に等しい数の各位置・速度ループ処理周期における移動
指令の平均値を求める。この平均値に位置フィードフォ
ワード係数を乗じて位置のフィードフォワード量ＦＦを
求め（Ｓ１３）、このフィードフォワード量ＦＦを通常
の位置ループ処理で得られる速度指令に加算し、補正さ
れた速度指令Ｖｃ（ｊ）とする（Ｓ１４）。さらに速度
フィードフォワード処理も行い（Ｓ１６）、トルク指令
Ｔｃ（ｊ）を求め（Ｓ１７）、モータを駆動する。フィ
ードフォワード制御を行っても位置偏差にうねりやモー
タ，機械にショック等が生じない。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、工作機械の送り軸やロ
ボットのアームを駆動するサーボモータの制御方式に関
し、特に、該サーボモータのフィードフォワード制御に
関する。

【０００２】

【従来の技術】サーボモータを用いて工作機械の送り軸
やロボットのアームなどを制御するとき、位置偏差量を
低減させるためにフィードフォワード制御が行われる。
特に、工作機械で高速切削を行う場合、サーボ系の追従
遅れによる形状誤差が生じる。そのため、この形状誤差
を少くするため位置ループにフィードフォワードをかけ
ることがある。すなわち、位置，速度処理周期毎、移動
指令を微分して、この微分値にフィードフォワード係数
を乗じた値を通常の位置ループ処理で得られた速度指令
に加算して、この加算値を速度指令とするフィードフォ
ワード制御を行い、位置偏差を低減させ、サーボ遅れを
補正している。

【０００３】しかし、数値制御装置等からサーボ回路
側、即ち、位置ループに移動指令が受け渡しされる分配
周期（ＩＴＰ周期）は８ｍｓｅｃ程度であり、サーボ回
路内部での位置ループ，速度ループの周期は２ｍｓｅｃ
或いは１ｍｓｅｃである。そして、位置ループでは上記
数値制御装置から移動指令が受け渡されるＩＴＰ周期を
位置ループ周期で分割し、分割した各位置ループ周期に
おける移動指令が均等になるように制御している。その
ため、数値制御装置から出力される移動指令に加減速時
定数を与えて出力されるようにしても、位置ループでは
各ＩＴＰ周期内における位置ループ処理周期ＴP 毎の各
位置ループ処理での移動指令が均等になるように制御さ
れるから、ＩＴＰ周期が変る位置ループ処理の移動指令
間にのみ移動指令に大きな段差が生じ、これがフィード
フォワード項で微分されて大きな値となり、速度指令は
高周波成分を含むこととなり、速度ループで追従できな
くなり、位置偏差にうねりが生じ、モータや機械の動き
に大きなショックを発生させる原因となる。そこでこの
欠点を解消するために、位置の制御，さらには速度の制
御のフィードフォワードの項に加減速処理を挿入し、上
記うねりを解消させるスムージング操作（平滑処理）を
行うサーボモータの制御方式を本願発明の発明者は特願
平１−１５０４８１号で提案した。

【０００４】

【発明が解決しようとする課題】しかし、上記スムージ
ング操作は、時間的に過去のデータを平均化するもので
あり、フィードフォワードの項に時間的な遅れを等価的
に生じさせるため、位置ループ制御系では加減速時に位
置偏差のうねり等を生じてしまう。

【０００５】そこで、本発明は、上記不具合を解決する
ことを目的とし、サーボ系の指令追従性を向上させるサ
ーボモータの制御方式を提供することを目的とするもの
である。

【０００６】

【課題を解決するための手段】本発明は、ＩＴＰ周期を
位置・速度ループ処理周期で分割した分割数をＮとする
と、当該位置・速度ループ処理における周期を中心にＮ
個の各位置・速度ループ処理周期における移動指令の平
均値を求め、この平均値に位置フィードフォワード係数
を乗じて得られる位置のフィードフォワード量を位置ル
ープ処理で得られる速度指令に加算して速度ループ処理
に対する速度指令とする。さらに、当該位置・速度ルー
プ周期より設定された数の周期だけ進めた上記位置・速
度ループ処理における上記平均値の微分値を求め、該微
分値に速度フィードフォワード係数を乗じて得られる速
度フィードフォワード量を速度ループ処理で得られたト
ルク指令値に加算してサーボモータへのトルク指令とす
ることによって上記課題を解決した。

【０００７】

【作用】当該ＩＴＰ周期より１つ先のＩＴＰ周期の移動
指令を読み込んでおき、位置・速度ループ処理周期の移
動指令を求めておく。そして、Ｐ周期を位置・速度ルー
プ処理周期で割った数をＮ、位置のフィードフォワード
係数をα１、移動指令のパルスの単位を速度指令の単位
に変換する係数をＰ、位置・速度ループ処理周期ｊにお
ける移動指令をａ（ｊ）とすると、ｊ周期における位置
のフィードフォワード量ＦＦはｊ周期を中心にＮ個の周
期の移動指令の平均値ｂ（ｊ）より求める。しかし、通
常、ＩＴＰ周期は８ｍｓｅｃ、位置・速度ループ周期は
１ｍｓｅｃか２ｍｓｅｃが通常採用されているので、上
記分割数Ｎは偶数となるので、（−Ｎ／２）＋１〜Ｎ／
２若しくは−Ｎ／２〜（Ｎ／２）−１の平均値ｂ（ｊ）
によって求める。例えば、（−Ｎ／２）〜Ｎ／２−１の
周期の平均値は次の数１となる。

【０００８】

【数１】また、（−Ｎ／２）＋１〜（Ｎ／２）の周期の
平均値は次の数２となる。

【０００９】

【数２】そしてこの平均値ｂ（ｊ）に係数をＰ，α１を
乗じて位置のフィードォワード量ＦＦを次の数３によっ
て求める。

【００１０】

【数３】さらには上記数１、数２の演算により求めた値
の加重平均を求めて、平均値ｂ（ｊ）としこれに係数を
Ｐ，α１を乗じて位置のフィードォワード量ＦＦとして
もよい。上記数１若しくは数２の演算、さらには数１、
数２の演算で求めた値の加重平均により求めた位置のフ
ィードォワード量ＦＦを当該周期ｊの位置ループ処理で
求めた速度指令に加算して、この加算値を速度ループ処
理に対する速度指令とする。さらには、L 周期進めた周
期における上記数１等で求めた平均値ｂ（ｊ＋L）の微
分値に移動指令のパルスの単位を速度指令の単位に変換
する係数をＰ、速度フィードフォワード係数α２および
移動指令のパルスの単位を電流の単位に変換する係数Ｐ
´を乗じて速度フィードフォワード量ＦＦｖを次の数４
により求める。

【００１１】

【数４】上記数４で求められた速度フィードフォワード
量ＦＦｖを速度ループ処理で求めたトルク指令（電流指
令）に加算してサーボモータへのトルク指令とする。

【００１２】

【実施例】図１は本発明の一実施例のサーボ系のブロッ
ク線図である。図１中、伝達関数１のＫP は位置ループ
におけるポジションゲイン、伝達関数２は速度ループの
積分項で、Ｔｓは位置・速度ループ処理の周期、ｋ１は
速度ループにおける積分ゲインである。伝達関数３のｋ
２は速度ループにおける比例ゲイン、４はサーボモータ
の機械部で、Ｋｔはトルク定数、Ｊｍはイナーシャ、５
はサーボモータの回転速度を積分し、位置を算出する伝
達関数である。また、６，７は位置のフィードフォワー
ド項であり、６はスムージング回路、７は位置のフィー
ドフォワード係数α１を乗じる項である。

【００１３】また、８はスムージング回路６の出力を設
定された数の位置・速度ループ処理周期だけ遅らせる伝
達関数で、９は微分を行う伝達関数、１０は速度フィー
ドフォワード係数α２を乗じる項で、この要素８，９，
１０で速度フィードフォワード制御を行うものである。
また、１１はＣＮＣ（コンピュータ内蔵の数値制御装
置）から分配周期（ＩＴＰ周期）毎送られてくる移動指
令ＭＣＭＤを位置・速度ループ処理周期毎の移動指令に
分割するＤＤＡ（ＤｉｇｉｔａｌＤｉｆｆｅｒｅｎｔ
ｉａｌＡｎａｌｙｚｅｒ）である。

【００１４】なお、上記伝達関数の内伝達関数２，８，
９は離散値制御系で表している。上位制御装置であるＣ
Ｎより、ＩＴＰ周期ごとの移動指令ＭＣＭＤが出力さ
れ、ＤＤＡ１１によって位置・速度ループ周期における
移動指令ａを求める。また、上記要素６，８によりフィ
ードォワード量ＦＦを求めるには、１つ先のＩＴＰ周期
の移動指令を求めておく必要があり、ＤＤＡ１１によっ
て１つ先のＩＴＰ周期の移動指令を求めておく。そし
て、当該位置・速度ループ処理における移動指令ａを求
め、当該位置・速度ループ周期の移動指令ａよりサーボ
モータの実際の移動量のフィードバック量Ｐｆを減じて
位置偏差を求めこの位置偏差にポジションゲインＫｐを
乗じた値に、要素６，７により数１若しくは数２の演算
を行って、若しくはもしくは数１，数２の演算で得られ
る値の加重平均により得られる位置フィードフォワード
量ＦＦを加算し速度指令を求め、この速度指令からサー
ボモータの実速度のフィードバック量Ｖｆを減じて速度
偏差をもとめて、この速度偏差に対して要素２で積分処
理を行った値から、サーボモータの実速度Ｖｆに比例定
数ｋ２を乗じた値を減じて従来と同様な積分、比例の速
度ループ処理を行いトルク指令をもとめ、さらに要素
８，９，１０によって数４の演算を行って速度フィード
フォワード量ＦＦｖを求めて、この速度フィードフォワ
ード量ＦＦｖを従来と同様な処理によって求めたトルク
指令に加算してフィードフォワード制御されたトルク指
令を求め、サーボモータを駆動する。

【００１５】図２は、本発明の１実施例を実施するデジ
タルサーボ制御装置のブロック図であり、構成は従来の
デジタルサーボ制御を行う装置と同一構成であるので、
概略的に示している。図２において、２０は数値制御装
置（ＮＣ）、２１は共有ＲＡＭ、２２はプロセッサ（Ｃ
ＰＵ），ＲＯＭ，ＲＡＭ等を有するデジタルサーボ回
路、２３はトランジスタインバータ等のサーボアンプ、
２４はサーボモータ、２５はサーボモータ２４の回転と
共にパルスを発生するパルスコーダである。

【００１６】ＮＣ２０はＩＰＴ周期（分配周期）毎に移
動指令ＭＣＭＤを共有ＲＡＭ２１に書込み、デジタルサ
ーボ回路２２のＣＰＵはこの移動指令ＭＣＭＤを共有Ｒ
ＡＭ２１から読取り、上記ＩＴＰ周期をＮ個に分割した
周期Ｔｓ（ＩＴＰ＝Ｔｓ×Ｎ）で、位置・速度ループ処
理を行う。ＩＴＰ周期毎、ＮＣ２０から出力される移動
指令ＭＣＭＤがＩＴＰ周期中均等に分配されるように位
置・速度ループ処理Ｔｓにおける移動指令ａ（ｊ）を求
め、この移動指令ａ（ｊ）とパルスコーダ２５からのフ
ィードバックパルスによって得られるサーボモータ２４
の現在位置との差より位置ループ処理を行うと共に、後
述する位置のフィードフォワード制御処理を行って速度
指令を求め、次に、該速度指令とパルスコーダ２５から
のフィードバックパルスによって得られるサーボモータ
２４の実速度より速度ループ処理、さらには速度のフィ
ードフォワード処理を行い、トルク指令（電流指令）を
求める。そして、電流ループ処理、さらには電流フィー
ドフォワード処理を行い、ＰＷＭ指令を作成し、サーボ
アンプ２３を介してサーボモータ２４を駆動する。

【００１７】図３，図４はディジタルサーボ回路２２の
ＣＰＵが実施するＩＴＰ周期ごとの移動指令読み取り処
理、および位置・速度ループ処理周期ごとの位置ループ
処理、速度ループ処理のフローチャートである。本実施
例においては、ＩＰＴ周期を４分割（Ｎ＝４）して位置
・速度ループ処理を行なうものとしている。また、位置
フィードフォワード量ＦＦを求めるために、当該周期を
中心にＮ個の周期の移動指令の平均値ｂ（ｊ）を上記数
式１で求めるものを使用して求めるとし、速度フィード
フォワード処理では２周期（L ＝２）進めた例で以下説
明する。そのため、移動指令の平均値ｂ（ｊ）は次の数
５となり、フィードフォワード量ＦＦは数６となる。

【００１８】

【数５】

【００１９】

【数６】また、速度のフィードフォワード量ＦＦｖは数
４より、次の数７となる。

【００２０】

【数７】上記ケースの場合を例に取ってディジタルサー
ボ回路３２のＣＰＵが実施する位置・速度ループ処理に
ついて、図３および図４のフローチャートと共に説明す
る。まず、ＣＰＵはＩＴＰ周期ごとＮＣ２０から出力さ
れた１つ次の移動指令ＭＣＭＤを共有ＲＡＭ２１を介し
て読む（ステップＳ１）。なお、実際は後述する位置・
速度ループ処理で１周期遅れた位置・速度ループ処理周
期の移動指令で位置・速度ループ処理が行われるので、
ＩＴＰ周期ごとＮＣ２０より出力される移動指令ＭＣＭ
Ｄを読み出すことになる。

【００２１】そして、位置・速度ループ処理周期ごとＣ
ＰＵは図４に示す処理を開始し、ＩＴＰ周期ごと読み取
った分配移動指令ＭＣＭＤより、位置・速度ループ毎の
移動指令ａ（ｊ＋４）を計算する（ステップＳ１０）。
そして、レジスタＲ（ｊ−２）〜Ｒ（ｊ＋４）のの記憶
内容を１つシフトし、レイスタＲ（ｊ＋４）にステップ
Ｓ１０で算出した移動指令ａ（ｊ＋４）を格納する。な
お、分配移動指令ＭＣＭＤが出力されてない時点では、
該各レジスタの記憶内容は「０」である。すなわち、レ
ジスタＲ（ｊ−１）の記憶内容をレジスタＲ（ｊ−２）
に格納し、同様に、Ｒ（ｊ−１）にＲ（ｊ）の記憶内
容、Ｒ（ｊ）にＲ（ｊ＋１）の記憶内容、Ｒ（ｊ＋１）
にＲ（ｊ＋２）の記憶内容、Ｒ（ｊ＋２）にＲ（ｊ＋
３）の記憶内容、Ｒ（ｊ＋３）にＲ（ｊ＋４）の記憶内
容を記憶させ、レジスタＲ（ｊ＋４）に移動指令ａ（ｊ
＋４）を格納する（ステップＳ１１−１〜Ｓ１１−
７）。そして、レジスタに記憶する前回の位置・速度ル
ープ周期の位置偏差ｅ（ｊ−１）にレジスタＲ（ｊ）に
記憶する当該周期の移動指令ａ（ｊ）を加算しパルスコ
ーダ２５で検出されるサーボモータの移動量の位置のフ
ィードバック量Ｐｆ（ｊ）を減じて当該周期の位置偏差
を求める（ステップＳ１２）。

【００２２】なお、分配移動指令ＭＣＭＤをＩＴＰ周期
１つ早読みすることになっているから、当該位置・速度
ループ周期の移動指令ａ（ｊ）は現在の周期で算出した
移動指令ａ（ｊ＋４）を格納したレジスタＲ（ｊ＋４）
より４個前のレジスタＲ（ｊ）に格納されている。次に
レジスタＲ（ｊ＋１），Ｒ（ｊ），Ｒ（ｊ−１），Ｒ
（ｊ−２）に格納された移動指令ａ（ｊ＋１），ａ
（ｊ），ａ（ｊ−１），ａ（ｊ−２）によって数５の演
算を行い平均値ｂ（ｊ）を求め、この値に係数α１，Ｐ
を乗じて位置フィードフォワード量ＦＦを求める（ステ
ップＳ１３）次に、上記ステップＳ１２で求めた位置偏
差ｅ（ｊ）に位置ループゲインＫｐを乗じた値にステッ
プＳ１３で求めた位置フィードフォワード量ＦＦを加算
した速度指令Ｖｃ（ｊ）を求め（ステップＳ１４）、こ
の速度指令Ｖｃ（ｊ）によって従来と同様の速度ループ
処理を行い速度フィードフォワードによる補正前のトル
ク指令Ｔｃ´（ｊ）を求める（ステップＳ１５）。次
に、レジスタＲ（ｊ＋３），Ｒ（ｊ−１）に記憶された
移動指令ａ（ｊ＋３），ａ（ｊ−１），によって数７の
演算を行うことによって速度フィードフォワード量ＦＦ
ｖを求める（ステップＳ１６）。そして、ステップＳ１
５で求めた補正前のトルク指令Ｔｃ´（ｊ）にステップ
Ｓ１５で算出した速度フィードフォワード量ＦＦｖを加
算してトルク指令Ｔｃ（ｊ）を求め（ステップＳ１
７）、このトルク指令Ｔｃ（ｊ）を電流ループ処理に引
き渡し（ステップＳ１８）、位置・速度ループの処理を
終了する。

【００２３】表１，図５、図６は、本実施例における一
例を記載したもので、図５（イ）に示すようにＩＴＰ周
期ごとの移動指令ＭＣＭＤ（ｎ）が入力されこれより位
置・速度ループ処理周期毎の移動指令ａ（ｊ）を求め、
この移動指令ａ（ｊ）より平均値ｂ（ｊ）（この平均値
に係数α１，Ｐを乗じて位置フィードフォワード量が求
められる）を求めると、図５（ロ）に示すようになる。
なお、表１にこの時の各レジスタＲ（ｊ＋４）〜Ｒ（ｊ
−２）に記憶される値ａ（ｊ＋４）〜ａ（ｊ−２）、お
よび平均値ｂ（ｊ）の値を示している。また、参考とし
て数２で求められる平均値ｂ（ｊ）をｂ（ｊ）´数１，
数２で算出される平均の加重平均値をｂ（ｊ）”として
示している。また、この表１で＋４はレジスタＲ（ｊ＋
４）に記憶するａ（ｊ＋４）の値、＋３はレジスタＲ
（ｊ＋３）に記憶するａ（ｊ＋３）の値、…ｊはレジス
タＲ（ｊ）に記憶するａ（ｊ）の値、…−２はレジスタ
Ｒ（ｊ−２）に記憶するａ（ｊ−２）の値を示してい
る。

【００２４】

【表１】図５および表１から分かるように、位置のフィードフォ
ワード量ＦＦ（平均値ｂ（ｊ））はＩＴＰ周期毎に移動
指令ａ（ｊ）が急激に変化しても、急激な変化は生じな
く、従来のフィードフォワード制御のように移動指令を
微分したものに係数を乗じてフィードフォワード量とし
た時のように急激な変化がなくそのため位置偏差のうね
りも解消することを意味する。図６は、従来の移動指令
を微分したものに係数を乗じてフィードフォワード量と
したフィードフォワード制御において、移動指令にラン
プ入力を行ったときの位置偏差を検出したときのグラフ
で、図７は、本発明において速度のフィードフォワード
制御でデータを進めないときの（L ＝０）、移動指令が
ランプ入力における位置偏差を測定してグラフ化したも
のであり、図８は、本発明においてデータを２周期（L
＝２）進めた時の位置偏差の測定結果をグラフ化したも
のである。この第６〜８図から明らかのように、位置偏
差のうねりは従来のフィードフォワード制御と比較して
本発明のフィードフォワード制御の方が改善されている
ことが分かる。

【００２５】なお、上記実施例では、当該位置・速度ル
ープ処理周期ｊの１つ先（未来）の周期ｊ＋１，当該周
期ｊ，１つおよび２つ前（過去）の周期ｊ−１，ｊ−２
の移動指令を平均化する実施例を説明したが、数２によ
り周期ｊ＋２，ｊ＋１，ｊ，ｊ−１の移動指令を平均化
する方法でもよい。この場合の平均値は表１でｂ（ｊ）
´で示す値となり図５（ロ）において、１周期Ｔｓ左に
シフトした状態となる。

【００２６】このように、ＩＴＰ周期を位置・速度ルー
プ周期で分割した数Ｎが偶数の場合には、平均化の中心
となる位置・速度ループ周期がＮ個の位置・速度ループ
周期の完全に中心とならない。そのため、位置フィード
フォワード量ＦＦは位置・速度ループ周期の半周期遅れ
るか進む状態になる。この点は、表１及び図５から見て
も明らかであるが、さらに分かり安く説明すると、図９
（ａ）に示すようにＩＴＰ周期の移動指令ＭＣＭＤが１
周期だけ４×４の移動指令がだされたとする。このと
き、数式１で平均値を求めてフィードフォワード量を求
めるとすると、当該ＩＴＰ周期における位置・速度ルー
プ周期をｊ＝０として各周期の移動指令ａを求めると平
均値は図９（ｂ）に示すように周期ｊ＝−１から順にｊ
＝５まで、１，２，３，４，３，２，１となる。一方平
均値を数式２で求めると図９（ｃ）に示すように、周期
ｊ＝−２から順にｊ＝４まで１，２，３，４，３，２，
１となり、図９（ｂ）では半周期遅れ、図９（ｃ）では
半周期進んでいる。そこで、数式１及び数２で求められ
る平均値の加重平均を取ると、図９（ｄ）に示すように
周期ｊ＝−２から順にｊ＝６まで、（１／２），（３／
２），（５／２），（７／２），（７／２），（５／
２），（３／２），（１／２）となり、等価的に時間遅
れ及び進みはなくなる。

【００２７】そこで、この加重平均によって位置のフィ
ードフォワード量ＦＦを求めて制御する場合の処理につ
いて図１０，図１１と共に説明する。この実施例は上述
した実施例とは異なり、平均値をＤＤＡ１１により求め
られる位置・速度ループ周期の移動指令ａから求めるの
ではなく、ＩＴＰ周期毎指令される移動指令ＭＣＭＤよ
り求めるようにしている。

【００２８】ディジタルサーボ回路２２のＣＰＵは分配
周期毎、図１０の処理を実行し、まず、レジスタＲ３に
レジスタＲ２に記憶する値、レジスタＲ２にレジスタＲ
１に記憶する値を格納し、、レジスタＲ１に共有ＲＡＭ
２１から読み取った１回先の移動指令ＭＣＭＤを格納す
る（ステップＴ１，Ｔ２）。そしてカウンタＣを「０」
にクリアする（ステップＴ３）。すなわち、レジスタＲ
３には当該ＩＴＰ周期によ１回前（過去）の移動指令Ｍ
ＣＭＤが記憶され、レジスタＲ２には当該周期の移動指
令ＭＣＤＭが、また、レジスタＲ１には１回先（未来）
の移動指令ＭＣＤＭが記憶されることになる。なお実際
には、単にレジスタＲ３にレジスタＲ２に記憶する値、
レジスタＲ２にレジスタＲ１に記憶する値を格納し、、
レジスタＲ１に分配された移動指令ＭＣＤＭを格納すれ
ばよく、後述するように位置・速度ループ処理において
１ＩＴＰ周期遅れて処理が実施されることになる。な
お、レジスタＲ１〜Ｒ３は初期設定で始めは「０」が格
納されている。

【００２９】一方、位置・速度ループ処理周期毎ディジ
タルサーボ回路２２のＣＰＵは、位置偏差の算出処理
（図４のステップＳ１２の処理）を実行した後、次に、
図１１に示す処理を実行し（なお、図１１には位置フィ
ードフォワード量を求める処理のみを示し他の処理は省
略している）、まず、カウンタＣの値が、分配周期を位
置・速度ループ処理周期で除した値Ｎ（＝分配周期／位
置・速度ループ処理周期）の１／２より小さいか否か判
断し（ステップＴ１０）、小さいならばアキュムレータ
ＳＵＮにレジスタＲ２の値からレジスタＲ３の値を減じ
た値を加算し（ステップＴ１１）、また、カウンタＣの
値がＮ／２以上ならば、アキュムレータＳＵＮにレジス
タＲ１の値からレジスタＲ２の値を減じた値を加算し
（ステップＴ１２）、ステップＳ１３に移行する。な
お、アキュムレータＳＵＮは初期設定で始めは「０」に
されている。

【００３０】ステップＴ１３では上記アキュムレータＳ
ＵＮの値を上記分割数Ｎの２乗で除算し平均値ｂ（ｊ）
を求める。そして、カウンタＣに「１」加算し、レジス
タＲ（ｂ）に記憶する１周期前の平均値とステップＴ１
３で求められた平均値ｂ（ｊ）を加算し「２」で除算し
て加重平均ｂ（ｊ）”を求める（ステップＴ１５）。次
に、ステップＴ１３で求めた平均値ｂ（ｊ）をレジスタ
Ｒ（ｂ）に格納し（ステップＴ１６）、ステップＴ１５
で求めた加重平均ｂ（ｊ）”に、位置のフィードフォワ
ード係数をα１、移動指令のパルスの単位を速度指令の
単位に変換する係数をＰを乗じてフィードフォワード量
ＦＦを求め（ステップＴ１７）、以下図４のステップＳ
１４以下の処理を実行する。なお、この場合、ステップ
Ｓ１６で求める速度フィードフォワード量ＦＦｖはステ
ップＳ１５で求めた加重平均ｂ（ｊ）”を記憶しておき
数４の演算を行うことによって求める。

【００３１】上記処理において、ステップＳＴ１０〜Ｔ
１３が平均値ｂ（ｊ）を求める処理であるが、この処理
が上述した当該位置・速度ループ周期を中心にＮ個の位
置・速度ループ周期の移動指令ａの平均値になることを
以下説明する。例えば、図１２に示すようにＩＴＰ周期
毎の移動指令ＭＣＮＤがＡ０，Ａ１，Ａ２，Ａ３，Ａ４
…と出力されＡ０＝Ａ１＝０とし、Ａ２，Ａ３を位置・
速度ループ周期毎に分割した移動指令をａ20，ａ21，ａ
22，ａ23，ａ30，ａ31，ａ32，ａ33とすると（なお分割
数Ｎを「４」としている）、平均値は以下のようにな
る。

【００３２】ｎ MCMD ＣＳＵＭｂ（ｊ）１Ａ１０ 0+A0-A1=0 ０２ 0+A2-A1=A2 A2/16=a20/4 ３ A2+A2-A1=2A2 2A2/16=(a20+a21)/4 ２Ａ２０ 2A2+A2-A1=3A2 3A2/16=(a20+a21+a22)/4 １ 3A2+A2-A1=4A2 4A2/16=(a20+a21+a22+a23)/4 ２ 4A2+A3-A2=3A2+A3 (3A2+A3)/16=(a21+a22+a23+a30)/4 ３ 3A2+A3+A3-A2=2A2+2A3 (2A2+2A3)/16=(a22+a23+a30+a31)/4 ３Ａ３０ 2A2+2A3+A3-A2=A2+3A3 (A2+3A3)/16=(a23+a30+a31+a32)/4 １ A2+3A3+A3-A2=4A3 4A3/16=(a30+a31+a32+a33)/4 … … 以上のように、ステップＳＴ１０〜Ｔ１３の処理によっ
ても平均値ｂ（ｊ）を求めることができる。そして、当
該位置・速度ループ周期で求められた平均値ｂ（ｊ）と
１位置・速度ループ周期前に求めた平均値との加重平均
を求めれば、遅れ及び進みのないフィードフォワード量
ＦＦを求めることができる。これは、上記例で、例え
ば、ＩＴＰ周期ｎ＝３で最初の位置・速度ループ周期
（カウンタＣ＝０）で求められる平均値ｂ（ｊ）は当該
位置・速度ループ周期の移動指令ａ30と１つ過去の周期
の移動指令ａ23及び１つ先，２つ先の周期の移動指令ａ
31，ａ32を加算している。すなわち数式２の演算を行っ
ている。一方、１つ前の位置・速度ループ周期で求めた
平均値ｂ（ｊ）は（ｎ＝２，Ｃ＝３のとき）当該位置・
速度ループ周期の移動指令ａ30と１つ及び２つ過去の周
期の移動指令ａ23，ａ22及び１つ先の周期の移動指令ａ
31を加算しており、数式１の演算により平均値を求めて
いる。よって、加重平均ｂ（ｊ）”を求めるには、当該
周期で算出された平均値と１前の周期で算出された平均
値を加算し２で除算することにより求めればよい。

【００３３】なお、上記実施例ではＩＴＰ周期を４分割
する場合について説明したが、４分割する場合以外にお
いても、当該位置・速度ループ周期を中心に前後の分割
数Ｎ個の各周期の移動指令ａを平均化するようにすれば
よい。

【００３４】

【発明の効果】本発明は、位置・速度ループ処理におけ
る当該移動指令よりも後に指令される移動指令をも取り
込んでフィードフォワード量を求めてフィードフォワー
制御を行うようにしたから、数値制御装置等の上位制御
装置からの分配移動指令が段階的に変動しても、位置偏
差のうねりを減少させ、モータや機械の動きに与えるシ
ョックを小さなものにした。また、分配周期を位置・速
度ループ処理の周期で分割した数が偶数の場合には、当
該周期を入れて前後の分割数の数の周期の移動指令の平
均をとる際に、当該周期より先（未来）に後（過去）よ
り１周期多く取ったときと、少なく取ったときの平均値
の加重平均を求めることによって、等価的に時間遅れの
及び進みのない位置フィードフォワード量を得ることが
でき、より正確な制御ができる。

【図面の簡単な説明】

【図１】本発明の一実施例のサーボ系のブロック図であ
る。

【図２】同実施例を実施するデジタルサーボ制御装置の
ブロック図である。

【図３】同実施例におけるＩＴＰ周期ごとのディジタル
サーボ回路のプロセッサが実施する処理のフローチャー
トである。

【図４】同実施例における位置・速度ループ処理のフロ
ーチャートである。

【図５】同実施例におれる一例の位置フィードフォワー
ド量（平均値）の説明図である。

【図６】従来のフィードフォワード制御を行うサーボ系
において、移動指令にランプ入力をしたときの位置偏差
の推移をみた図である。

【図７】本発明の一実施例において速度フィードフォワ
ード制御の進み量L を０として移動指令にランプ入力を
したときの位置偏差の推移をみた図である。

【図８】本発明の一実施例において速度フィードフォワ
ード制御の進み量L を２として移動指令にランプ入力を
したときの位置偏差の推移をみた図である。

【図９】スムージング処理による平均値の説明図であ
る。

【図１０】本発明の他の実施例におけるＩＴＰ周期ごと
にディジタルサーボ回路のプロセッサが実施する処理の
フローチャートである。

【図１１】同他の実施例における位置・速度ループ処理
中の位置フィードフォワードを求める処理のフローチャ
ートである。

【図１２】平均値を求める処理の説明図である。

【符号の説明】

１位置ループ処理の伝達関数の項２速度ループ処理の積分の伝達関数の項３速度ループの比例の伝達関数の項４モータの伝達関数の項５速度から位置に変換する伝達関数の項６位置フィードフォワードのスムージング回路７位置フィードフォワードの位置フィードフォワード
係数の項８速度フィードフォワードの進み要素の項９速度フィードフォワードの微分の項１０速度フィードフォワードの速度フィードフォワー
ド係数の項１１ＤＤＡ（Digital Differential Analyze）２０数値制御装置２１共有ＲＡＭ２２ディジタルサーボ回路２３サーボアンプ２４サーボモータ２５パルスコーダ

【数式１】

【数式２】

【数式３】

【数式４】

【数式５】

【数式６】

【数式７】

Claims

【特許請求の範囲】

【請求項１】移動指令の分配周期をＮ分割した周期ご
と実施する位置・速度ループ処理における移動指令を上
記分配周期の移動指令より求め、位置・速度ループ制御
を行うサーボモータの制御方式において、当該分配周期
より１つ先の分配周期の移動指令を読み込んで、各位置
・速度ループ処理における移動指令を計算しておき、当
該位置・速度ループ処理における周期を中心にＮ個の各
位置・速度ループ処理周期における移動指令の平均値を
求め、この平均値に位置フィードフォワード係数を乗じ
て得られる位置のフィードフォワード量を位置ループ処
理で得られる速度指令に加算して速度ループ処理に対す
る速度指令としたことを特徴とするサーボモータのフィ
ードフォワード制御方式。
【請求項２】移動指令の分配周期をＮ分割した周期ご
と実施する位置・速度ループ処理における移動指令を上
記分配周期の移動指令より求め、位置・速度ループ制御
を行うサーボモータの制御方式において、当該分配周期
より１つ先の分配周期の移動指令を読み込んで記憶して
おくと共に、当該分配周期より１つ過去の分配周期の移
動指令をも記憶しておき、当該分配周期の移動指令と記
憶した上記当該分配周期より１つ先の分配周期の移動指
令及び１つ過去の分配周期の移動指令より、当該位置・
速度ループ処理における周期を中心にＮ個の各位置・速
度ループ処理周期における移動指令の平均値を求め、こ
の平均値に位置フィードフォワード係数を乗じて得られ
る位置のフィードフォワード量を位置ループ処理で得ら
れる速度指令に加算して速度ループ処理に対する速度指
令としたことを特徴とするサーボモータのフィードフォ
ワード制御方式。
【請求項３】上記分割数Ｎが偶数のとき、上記平均値
は、当該位置・速度ループ処理における周期と該周期よ
り過去の（Ｎ／２）−１個及び先のＮ／２個の各位置・
速度ループ処理周期における合計Ｎ個の移動指令の平均
値と、当該位置・速度ループ処理における周期と該周期
より過去のＮ／２個及び先の（Ｎ／２）−１個の各位置
・速度ループ処理周期における合計Ｎ個の移動指令の平
均値との加重平均とする請求項１若しくは請求項２記載
のサーボモータのフィードフォワード制御方式。
【請求項４】当該位置・速度ループ周期より設定され
た数の周期だけ進めた上記位置・速度ループ処理におけ
る上記平均値の微分値を求め、該微分値に速度フィード
フォワード係数を乗じて得られる速度フィードフォワー
ド量を速度ループ処理で得られたトルク指令値に加算し
てサーボモータへのトルク指令とした請求項１，請求項
２または請求項３記載のサーボモータのフィードフォワ
ード制御方式。