JP3599849B2

JP3599849B2 - Distribution method of movement command in servo control

Info

Publication number: JP3599849B2
Application number: JP23319895A
Authority: JP
Inventors: 哲朗加藤; 創一有田
Original assignee: FANUC Corp
Current assignee: FANUC Corp
Priority date: 1995-08-21
Filing date: 1995-08-21
Publication date: 2004-12-08
Anticipated expiration: 2015-08-21
Also published as: JPH0962331A

Description

【０００１】
【発明の属する技術分野】
本発明は、サーボ制御に関し、特にＮＣ工作機械やロボットの制御装置等に使用されるサーボ制御において、サーボ制御ループの処理周期毎に分配される移動指令の分配方法に関する。
【０００２】
【従来の技術】
ＮＣ工作機械やロボットの制御装置等において、制御装置側のホストＣＰＵは、制御対象物が所望の動きを行うような位置、速度、あるいは軌跡を計画し、この軌道計画に基づいてサーボＣＰＵを介してサーボモータの制御を行っている。
【０００３】
軌道計画の処理周期がサーボ制御の処理周期に比べて長い場合、サーボＣＰＵは動作を滑らかにするために軌道計画の処理周期毎の移動指令を、サーボ処理周期毎の移動指令に分配する必要がある。
【０００４】
一般に、サーボ制御ループの処理周期は、近年の高速ＤＳＰ（Ｄｉｇｉｔａｌｓｉｇｎａｌｐｒｏｃｅｓｓｅｒ；デジタル信号プロセッサ）の採用によって短縮されるのに対して、軌道計画については、逆変換等に多大な処理時間を要するため、軌道計画の処理周期はサーボ処理周期に比べて長い周期を要する傾向にあるため、サーボ制御を行う場合、軌道計画の処理周期毎の移動指令をサーボ処理周期毎の移動指令に分配する処理の必要性が増大している。
【０００５】
なお、通常、軌道計画の処理周期は、軌道計画の処理を行うホストＣＰＵとサーボ処理を行うサーボＣＰＵとの間の通信周期により制限されることが多いため、以下では通信周期の処理周期を軌道計画の処理周期として扱うものとする。
表すものとする。
【０００６】
工作機械やロボット等の制御対象物は、バネ要素およびダンパ要素を含む２慣性系モデルにより一般化することができる。図６は２慣性系モデルのモデル例である。符号１０はモータ側を示し、符号１１は機械側を示している。このモータ側１０と機械側１１の間に減速器が設けられ、該減速器のバネ定数をＫ［ｋｇ．ｃｍ／ｒａｄ］、粘性係数をＤ［ｋｇ．ｃｍ／（ｒａｄ／ｓ）］とする。また、モータのロータイナーシャをＪｍ［ｋｇ．ｃｍ．ｓ^２］、モータの動摩擦係数をＡｍ［ｋｇ．ｃｍ．ｓ］、モータ位置をθｍ［ｒａｄ］、負荷側のイナーシャをＪＬ［ｋｇ．ｃｍ．ｓ^２］、負荷位置（ロボットの場合には、ロボットのアーム先端位置）をθＬ［ｒａｄ］、モータのトルク定数をＫｔ［ｋｇ．ｃｍ／Ａｐ］とする。
【０００７】
モータへの電流指令をｕ［Ａｐ］とし、トルク指令をτ［ｋｇ．ｃｍ］として、負荷側について運動方程式をたてると以下の式（１）となる。
【０００８】
ＪＬ・θＬ＊＊＋Ｄ・（θＬ＊−θｍ＊）＋Ｋ・（θＬ −θｍ）＝０…（１）
また、モータ側について運動方程式をたてると以下の式（２）となる。
【０００９】

なお、記号の右側に付した「＊」は、その記号で示される変数を微分することを示し、「＊」は１回微分を表し「＊＊」は２回微分を表している。すなわち、θＬ＊＊は負荷位置θＬの２回微分で負荷の速度を表している。また、（θＬ −θｍ）は制御対象点である負荷位置に対するモータのロータ回転位置偏差（ねじれ量）を意味し、（θｍ＊−θＬ＊）は速度偏差（ねじれ速度）を意味している。
【００１０】
上記式（１），（２）から、負荷側位置と電流指令との関係は以下の式（３）によって表される。
【００１１】

ここで、ＤはＫより充分大きい（Ｄ＜＜Ｋ）とすることができるため、さらに以下の式（４）とすることができる。
【００１２】

ここで、負荷位置θＬを指令値ｕの通りに動作させるためのフィードフォワードトルクｕｆｆは、移動指令をθｒとすると以下の式（５）となる。
【００１３】

このとき、ｓ^４の項は充分小さいため無視することができるが、ｓ^３の項のフィードフォワード分は大きく、制振効果に影響を与えることになる。
【００１４】
従来、このような２慣性系モデルにおいて、軌道計画の処理周期が短い場合には、サーボ処理周期において軌道計画の移動指令を単に等分配して得られる分配指令によってサーボ制御を行ったり、サーボ処理周期において軌道計画の移動指令を移動平均によるフィルタリングで得られる分配指令によってサーボ制御を行ったり、あるいは、サーボ処理周期において軌道計画の移動指令を指数加減速によるフィルタリングで得られる分配指令によってサーボ制御を行っている。
【００１５】
【発明が解決しようとする課題】
しかしながら、従来のサーボ制御における移動指令の分配方法では、制御対象の加々速度がスパイク状に大きくなるため、この加々速度を用いてフィードフォワード制御を行うと、トルク指令が飽和したり、制御対象の動きが振動的になるという問題点がある。２慣性系の制御対象においては、加々速度による制御が有効な場合が多く、この加々速度が異常に大きくなったり振動する場合には、制御に不都合が生じることになる。
【００１６】
ここで、ホスト側の処理周期およびホストＣＰＵとサーボＣＰＵとの間の通信周期を例えば２０ｍｓとし、サーボ側の処理周期を１ｍｓとし、等分配によって移動指令を分配した場合、移動平均フィルタによって移動指令を分配した場合、指数フィルタによって移動指令を分配した場合の各場合について、加速度および加々速度の変化について、図７〜図１２を用いて説明する。
【００１７】
以下の説明では、移動指令は、通常ホスト側とサーボ側との通信周期毎の移動指令である場合が多いため、移動指令の単位を［ｒａｄ／２０ｍｓ］とする。
図７は、移動指令をサーボ処理周期毎に等分配する場合のブロック線図であり、図１０はこのときの移動指令，加速度指令，および加々速度指令を示す図である。図７において、ホストＣＰＵからの移動指令は、サーボ側の処理周期（１ｍｓ）に合わせて等分配するために（１／２０）の係数項によって指令値が除され、さらに、サーボ側の処理周期毎に移動指令を取り出すためにサンプリングが行われる。これによって、等分配された１ｍｓ毎の移動指令が出力される。
【００１８】
図１０（ａ）はホストＣＰＵからの移動指令（図中の［ｒａｄ／２０ｍｓ］の単位）と等分配された移動指令（図中の［１／２０ｒａｄ／ｍｓ］の単位）の速度を表し、図１０（ｂ）は等分配された速度指令に対する加速度［ｒａｄ／ｍｓ^２］を表し、図１０（ｃ）は等分配された速度指令に対する加々速度［ｒａｄ／ｍｓ^３］を表している。
【００１９】
この等分配の場合には、サーボ処理周期毎の移動指令は不連続な階段状となり、また、加々速度はスパイク状となり、異常に大きな値をとる場合がある。
【００２０】
また、図８は、移動指令をサーボ処理周期毎に移動平均フィルタによって移動指令を分配する場合のブロック線図であり、図１１はこのときの移動指令，加速度指令，および加々速度指令を示す図である。図８において、ホストＣＰＵからの移動指令は、サーボ側の処理周期（１ｍｓ）に合わせて等分配するために（１／２０）の係数項によって指令値が除され、サーボ側の処理周期毎に移動指令を取り出すためにサンプリングが行われる。さらに、サンプリングされた移動指令は、移動平均フィルタによってサンプリング周期毎にずらしながら平均をとる。これによって、移動平均によって分配された１ｍｓ毎の移動指令が出力される。図１１（ａ）はホストＣＰＵからの移動指令（図中の［ｒａｄ／２０ｍｓ］の単位）と移動平均によって分配された移動指令（図中の［１／２０ｒａｄ／ｍｓ］の単位）の速度を表し、図１１（ｂ）は移動平均によって分配された速度指令に対する加速度［ｒａｄ／ｍｓ^２］を表し、図１１（ｃ）は移動平均によって分配された速度指令に対する加々速度［ｒａｄ／ｍｓ^３］を表している。
【００２１】
また、図９は、移動指令をサーボ処理周期毎に指数フィルタによって移動指令を分配する場合のブロック線図であり、図１２はこのときの移動指令，加速度指令，および加々速度指令を示す図である。図９において、ホストＣＰＵからの移動指令は、サーボ側の処理周期（１ｍｓ）に合わせて等分配するために（１／２０）の係数項によって指令値が除され、サーボ側の処理周期毎に移動指令を取り出すためにサンプリングが行われる。さらに、サンプリングされた移動指令は、指数フィルタによってフィルタリングされ、１ｍｓ毎の移動指令が出力される。図１２（ａ）はホストＣＰＵからの移動指令（図中の［ｒａｄ／２０ｍｓ］の単位）と指令フィルタによって分配された移動指令（図中の［１／２０ｒａｄ／ｍｓ］の単位）の速度を表し、図１２（ｂ）は指令フィルタによって分配された速度指令に対する加速度［ｒａｄ／ｍｓ^２］を表し、図１２（ｃ）は指令フィルタによって分配された速度指令に対する加々速度［ｒａｄ／ｍｓ^３］を表している。
【００２２】
図１０〜図１２に示すようなスパイク状の異常に大きな値の加々速度を用いてフィードフォワードを行うと、電流指令が飽和したり、不連続に表れる周期性のために制御対象が振動的となる虞がある。
【００２３】
また、軌道計画の工程において、加々速度を制限して移動指令を作成しても、サーボ処理では計算値以上の加々速度を指令する場合がある。
【００２４】
そこで、本発明は前記した従来の問題点を解決して、サーボ制御ループにおるけ加々速度を制限することができ、また、軌道計画により計画された加々速度をサーボ制御ループで実現することができるサーボ制御における移動指令の分配方法を提供することを解決課題とする。
【００２５】
【課題を解決するための手段】
本発明のサーボ制御における移動指令の分配方法は、サーボ制御において、軌道計画の処理周期毎の移動指令をサーボ処理周期毎の移動指令に等分配し、等分配した移動指令を、軌道計画の処理周期とサーボ処理周期の比率と同じサンプリング数を持つ２段の移動平均処理により分配することによって、前記目的を達成する。
【００２６】
本発明において、軌道計画は制御装置側のホストＣＰＵが行う処理であり、制御対象物が所望の動きを行うように位置、速度、あるいは軌跡を計画する処理である。また、等分配は、処理周期の異なる制御装置側の処理周期とサーボ処理周期において、制御装置側の移動指令をサーボ制御側の移動指令に分配する一分配方法であり、制御装置側の移動指令をその処理周期に合わせて等しく分配する処理である。また、本発明の移動平均処理による分配は、等分配された移動指令をサンプリング周期毎にずらしながら平均をとって分配する処理であり、本発明においては、この移動平均処理を２回行うものである。
【００２７】
なお、この移動平均処理は、遅延器と加算器により構成される移動平均フィルタによって行うことができる。
【００２８】
本発明において、ホストＣＰＵから出力された移動指令は、サーボ側の処理周期に合わせて等分配するために、ホスト側の処理周期とサーボ側の処理周期の比率で定まる係数値によって指令値が除された後サンプリングが行われて、サーボ側の処理周期毎の移動指令が取り出される。このサンプリングされた移動指令は、１段目の移動平均処理によって移動平均され、さらにその出力は２段目の移動平均処理によって移動平均される。この２段目の移動平均処理後の移動指令が、サーボ制御に用いる移動指令となる。
【００２９】
【発明の実施の形態】
以下、本発明の実施の形態を図を参照しながら詳細に説明する。
【００３０】
図１は、本発明のサーボ制御における移動指令の分配方法を説明するためのブロック線図であり、サーボ側において、ホスト側からホスト側の処理周期に従って受け渡される移動指令をサーボ処理周期毎に等分配し、等分配された移動指令を移動平均によるフィルタを２回通して、２段の移動平均をとることによって移動指令を分配するものである。
【００３１】
図１において、ホストＣＰＵからの移動指令は、サーボ側の処理周期に合わせて等分配するためにの係数項によって指令値が除される。ホスト側の処理周期は、ホストＣＰＵが行う軌道計画の処理周期あるいはホストＣＰＵとサーボ側間の通信周期により定まる周期であり、例えば２０ｍｓとする。これに対して、サーボ側の処理周期は一般にホスト側の処理周期より短く、例えば１ｍｓとする。
【００３２】
このとき、図２の速度指令の図に示すように、ホスト側からＶ［ｒａｄ／２０ｍｓ］の速度指令が出力される場合（図２（ａ））には、サーボ側の処理周期単位でみるとＶ／２０［ｒａｄ／ｍｓ］となる。つまり、ホストＣＰＵからの移動指令をサーボ側の処理周期に合わせて等分配するために、この係数項によって指令値をホスト側の処理周期とサーボ側の処理周期の比率で定まる係数値で除する。
【００３３】
ここで、ホスト側の移動指令が図３（ａ）に示すように、例えば２０ｍｓまではＶ１［ｒａｄ／２０ｍｓ］の速度指令が出力され、２０ｍｓかち４０ｍｓまではＶ２［ｒａｄ／２０ｍｓ］の速度指令が出力される場合には、上記係数項を経ることによって図３（ｂ）に示すようなサーボ側の速度指令を得ることができる（ステップＳ１）。
【００３４】
次に、サーボ側の処理周期に合わせて等分配するために、前記ステップＳ１で求めた速度指令を、サーボ側の処理周期に合わせたサンプリング周期でサンプリングする。サーボ側の処理周期を１ｍｓとすると、サンプリングに得られる等分配された移動指令は図３（ｂ）中の点で表される。以下、このサンプリングされた移動指令をＡとする（ステップＳ２）。
【００３５】
次に、等分配された移動指令に移動平均処理を行う。移動平均処理は、処理前の移動指令をＡとし、１段目の処理後の移動指令をＢ，２段目の処理後の移動指令をＣとし、ホスト側の処理周期とサーボ側の処理周期の比率をｎとすると、移動平均処理は以下の式（６）によって表され、
（１＋ｚ^−１＋ｚ^−２＋・・・＋ｚ^{−（ｎ−１）}）／ｎ …（６）
ｎ＝２０の場合の１段目の移動平均の値Ｂと２段目の移動平均の値Ｃはそれぞれ以下の以下の式（７），（８）によって表される。
【００３６】

この２段目の移動平均の処理の出力Ｃがサーボ制御に用いる移動指令Ｃとなる（ステップＳ３，４）。
【００３７】
ここで、指令加速度Ａｃｍｄおよび指令加々速度Ｊｃｍｄは、以下の式（９），（１０）となる。
【００３８】

ここで、
Ｂ（ｉ）−Ｂ（ｉ−２０）＝［Ａ（ｉ）−Ａ（ｉ−２０）］／２０
であり、また、Ａはホスト側の処理周期毎の移動指令を等分配したものであり、２０ｍｓの間は等しいため、

となる。
【００３９】
ここで、Ｉはホスト側の処理周期毎に更新される数値である。
同様にして、

となるため、指令加々速度Ｊｃｍｄは以下の式（１３）となる。
【００４０】

この指令加々速度Ｊｃｍｄは、ホスト側の処理周期に期間中においてはＲ（Ｉ），Ｒ（Ｉ−１），Ｒ（Ｉ−２）の値は一定であるため、加々速度は一定となる。したがって、従来のうよなスパイク状の加々速度なくなり、加々速度を制限することができる。
【００４１】
一方、ホスト側における処理周期毎の移動指令から求められる加々速度Ｊｃｍｄ’は、

となり、このことはホスト側で軌道計画された加々速度が再現されることを示している。
【００４２】
図４はこのときの移動指令，加速度指令，および加々速度指令のを示す概略図である。図４（ａ）はホストＣＰＵからの移動指令（図中の［ｒａｄ／２０ｍｓ］の単位）と２段の移動平均によって分配された移動指令（図中の［１／２０ｒａｄ／ｍｓ］の単位）の速度を表し、図４（ｂ）は２段の移動平均によって分配された速度指令に対する加速度［ｒａｄ／ｍｓ^２］を表し、図４（ｃ）は２段の移動平均によって分配された速度指令に対する加々速度［ｒａｄ／ｍｓ^３］を表している。
【００４３】
図５は、本発明のサーボ制御における移動指令の分配方法を実施するための制御径の要部ブロック図である。図中において、１はサーボ制御系を制御するホストＣＰＵで、軌道計画を行って移動指令を出力する。例えば、ロボットの場合には、補間、直交座標系の座標値から各軸の回転角への変換，逆変換等を行うとともに、ロボットの各軸へ位置指令を分配する。２はホストＣＰＵ１とサーボ回路３のプロセッサ間の情報の伝達を仲介する共有ＲＡＭで、ホストＣＰＵ１が書き込んだ移動指令等のデータをサーボ回路３のプロセッサに受渡し、サーボ回路３のプロセッサが書き込んだアラーム情報等をホストＣＰＵ１に引き渡す機能を行う。３はデジタルシグナルプロセッサ等で構成されるサーボ回路でプロセッサ，ＲＯＭ，ＲＡＭ等で構成され、ロボットの場合には各軸のサーボモータの制御を行うもので、本発明のサーボ制御における移動指令の分配処理を行う。４はトランジスタインバータ等で構成されるサーボアンプで、５はサーボモータである。また，６はサーボモータ５の位置を検出するパルスコーダで、位置はサーボ回路４にフィードバックされる。なお、サーボアンプ４，サーボモータ５は１軸のみを図示している。
【００４４】
【実施例】
図１３は、２慣性系の制御対象と制御系の概略ブロック図である。ここで、ＪＬ＝０．６［ｋｇ．ｃｍ．ｓ^２］，Ｊｍ＝０．１２［ｋｇ．ｃｍ．ｓ^２］，Ａｍ＝１．０［ｋｇ．ｃｍ．ｓ］，Ｋｔ＝９［ｋｇ．ｃｍ／Ａｐ］，最大電流＝８０［Ａｐ］，Ｋ＝４００［ｋｇ．ｃｍ／ｒａｄ］，Ｄ＝０．７［ｋｇ．ｃｍ／（ｒａｄ／ｓ）］，ポジションゲイン＝０．０１［１／ｍｓ］，速度ゲイン１＝１５［１／ｍｓ］，速度ゲイン２＝６０００［１／ｍｓ］，ねじれフィードバックゲイン１＝１００［１／ｍｓ］，ねじれフィードバックゲイン２＝３［１／ｍｓ］とする。なお、図中の＊は推定値を示している。
【００４５】
図１４は、この２慣性系の制御対象と制御系において指数フィルタによる分配を行った場合の移動指令，実速度（ａ）および電流指令（ｂ）を示し、図１５は、同２慣性系の制御対象と制御系において１段の移動平均フィルタによる分配を行った場合の移動指令，実速度（ａ）および電流指令（ｂ）を示し、また、図１６は、同２慣性系の制御対象と制御系において本発明の分配方法による２段の移動平均フィルタによる分配を行った場合の移動指令，実速度（ａ）および電流指令（ｂ）を示している。
【００４６】
【発明の効果】
以上説明したように、本発明によれば、サーボ制御ループにおるけ加々速度を制限することができ、また、軌道計画により計画された加々速度をサーボ制御ループで実現することができるサーボ制御における移動指令の分配方法を提供することができる。
【図面の簡単な説明】
【図１】本発明のサーボ制御における移動指令の分配方法を説明するためのブロック線図である。
【図２】速度指令を説明するための図である。
【図３】速度指令を説明するための図である。
【図４】２段の移動平均による移動指令，加速度指令，および加々速度指令のを示す概略図である。
【図５】本発明のサーボ制御における移動指令の分配方法を実施するための制御径の要部ブロック図である。
【図６】２慣性系モデルのモデル例である。
【図７】移動指令を等分配する場合のブロック線図である。
【図８】移動指令を移動平均フィルタにより分配する場合のブロック線図である。
【図９】移動指令を指数フィルタにより分配する場合のブロック線図である。
【図１０】等分配による移動指令，加速度指令，および加々速度指令を示す図である。
【図１１】移動平均フィルタの分配による移動指令，加速度指令，および加々速度指令を示す図である。
【図１２】指数フィルタの分配による移動指令，加速度指令，および加々速度指令を示す図である。
【図１３】２慣性系の制御対象と制御系の概略ブロック図である。
【図１４】２慣性系の制御対象および制御系において指数フィルタの分配による移動指令，実速度，電流指令を示す図である。
【図１５】２慣性系の制御対象および制御系において１段の移動フィルタの分配による移動指令，実速度，電流指令を示す図である。
【図１６】２慣性系の制御対象および制御系において２段の移動フィルタの分配による移動指令，実速度，電流指令を示す図である。
【符号の説明】
１制御装置
２共有ＲＡＭ
３サーボ回路
４サーボアンプ
５モータ
６パルスコーダ
１０モータ側
１１機械側[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to servo control, and more particularly to a method of distributing movement commands distributed for each processing cycle of a servo control loop in servo control used for an NC machine tool, a control device of a robot, and the like.
[0002]
[Prior art]
In a control device of an NC machine tool or a robot, a host CPU of the control device plans a position, a speed, or a trajectory at which a control target performs a desired movement, and via a servo CPU based on the trajectory plan. Controls the servo motor.
[0003]
When the processing cycle of the trajectory planning is longer than the processing cycle of the servo control, the servo CPU needs to distribute the movement command for each processing cycle of the trajectory planning to the movement command for each servo processing cycle in order to smooth the operation. is there.
[0004]
In general, the processing cycle of the servo control loop is shortened by the recent adoption of a high-speed digital signal processor (DSP). On the other hand, the trajectory planning requires a great deal of processing time for inverse transformation and the like. Since the processing cycle of the trajectory planning tends to require a longer cycle than the servo processing cycle, when performing servo control, it is necessary to distribute the movement command for each processing cycle of the trajectory planning to the movement command for each servo processing cycle. Sex is increasing.
[0005]
In general, the processing cycle of the trajectory planning is often limited by the communication cycle between the host CPU that performs the processing of the trajectory planning and the servo CPU that performs the servo processing. It shall be treated as the processing cycle of the plan.
Shall be represented.
[0006]
Control objects such as machine tools and robots can be generalized by a two-inertia model including a spring element and a damper element. FIG. 6 is a model example of a two inertial system model. Reference numeral 10 indicates a motor side, and reference numeral 11 indicates a machine side. A speed reducer is provided between the motor side 10 and the machine side 11, and the spring constant of the speed reducer is K [kg. cm / rad] and the viscosity coefficient is D [kg. cm / (rad / s)]. Further, the rotor inertia of the motor is set to Jm [kg. cm. s ² ], and the dynamic friction coefficient of the motor is Am [kg. cm. s], the motor position is θm [rad], and the inertia on the load side is JL [kg. cm. s ² ], the load position (the robot arm tip position in the case of a robot) is θL [rad], and the motor torque constant is Kt [kg. cm / Ap].
[0007]
The current command to the motor is u [Ap], and the torque command is τ [kg. cm], the following equation (1) is obtained when an equation of motion is established for the load side.
[0008]
JL · θL ** + D · (θL * −θm *) + K · (θL−θm) = 0 (1)
When the equation of motion is established for the motor side, the following equation (2) is obtained.
[0009]

Note that “*” attached to the right side of the symbol indicates that the variable indicated by the symbol is differentiated, “*” indicates one-time differentiation, and “**” indicates two-time differentiation. That is, θL ** represents the speed of the load by the second derivative of the load position θL. Further, (θL−θm) means a rotor rotational position deviation (torsion amount) with respect to a load position which is a control target point, and (θm * −θL *) means a velocity deviation (torsion speed).
[0010]
From the above equations (1) and (2), the relationship between the load-side position and the current command is represented by the following equation (3).
[0011]

Here, since D can be made sufficiently larger than K (D << K), the following equation (4) can be further obtained.
[0012]

Here, the feedforward torque uff for operating the load position θL as instructed by the command value u is given by the following equation (5) when the movement command is θr.
[0013]

At this time, the term of s ⁴ can be ignored because sufficiently small, the feedforward component of s ³ sections is large, will affect the damping effect.
[0014]
Conventionally, in such a two-inertia model, when the processing cycle of a trajectory plan is short, servo control is performed by a distribution command obtained by simply distributing a movement command of a trajectory plan in a servo processing cycle, or servo processing is performed. Servo control is performed by the distribution command obtained by filtering the moving command of the trajectory plan in the cycle by moving average filtering, or servo control is performed by the distribution command obtained by filtering the moving command of the trajectory plan by exponential acceleration / deceleration in the servo processing cycle. Is going.
[0015]
[Problems to be solved by the invention]
However, in the conventional method of distributing the movement command in the servo control, the jerk of the control target increases in a spike shape. Therefore, if the feedforward control is performed using the jerk, the torque command may be saturated or the control may not be performed. There is a problem that the movement of the object becomes oscillatory. In a two-inertial-system control target, control by jerk is often effective, and when the jerk becomes abnormally large or vibrates, inconvenience occurs in control.
[0016]
Here, when the processing cycle on the host side and the communication cycle between the host CPU and the servo CPU are set to, for example, 20 ms, the processing cycle on the servo side is set to 1 ms, and the movement commands are distributed by equal distribution, the moving , And changes in acceleration and jerk in each case in which a movement command is distributed by an exponential filter will be described with reference to FIGS. 7 to 12.
[0017]
In the following description, the movement command is usually a movement command for each communication cycle between the host side and the servo side, and therefore the unit of the movement command is [rad / 20 ms].
FIG. 7 is a block diagram in the case where the movement commands are equally distributed for each servo processing cycle, and FIG. 10 is a diagram showing the movement commands, acceleration commands, and jerk commands at this time. In FIG. 7, the movement command from the host CPU is divided by a coefficient term of (1/20) in order to equally distribute the movement command according to the processing cycle (1 ms) on the servo side. Sampling is performed to extract a movement command every time. Thereby, equally distributed movement commands every 1 ms are output.
[0018]
FIG. 10A shows the speed of a movement command (unit of [rad / 20 ms] in the figure) and a movement command (unit of [1/20 rad / ms] in the figure) equally distributed from the host CPU. FIG. 10B shows the acceleration [rad / ms ² ] for the equally distributed speed command, and FIG. 10C shows the jerk [rad / ms ³ ] for the equally distributed speed command.
[0019]
In the case of this equal distribution, the movement command in each servo processing cycle has a discontinuous step shape, and the jerk has a spike shape, which may take an abnormally large value.
[0020]
FIG. 8 is a block diagram when the movement command is distributed by the moving average filter for each servo processing cycle. FIG. 11 shows the movement command, acceleration command, and jerk command at this time. FIG. In FIG. 8, the movement command from the host CPU is divided by a coefficient term of (1/20) in order to equally distribute the movement command according to the processing cycle (1 ms) on the servo side. Sampling is performed to retrieve the movement command. Further, the sampled movement commands are averaged while being shifted by a moving average filter for each sampling cycle. As a result, a movement command distributed every 1 ms, which is distributed by the moving average, is output. FIG. 11A shows the speed of a movement command (unit of [rad / 20 ms] in the figure) from the host CPU and the speed of a movement command (unit of [1/20 rad / ms] in the figure) distributed by moving average. represents, FIG. 11 (b) represents the acceleration [rad / ms ^2] for the speed command distributed by the moving average, and FIG. 11 (c) jerk for the speed command distributed by the moving average is [rad / ms ³ ].
[0021]
FIG. 9 is a block diagram when the movement command is distributed by an exponential filter for each servo processing cycle. FIG. 12 is a diagram showing the movement command, the acceleration command, and the jerk command at this time. It is. In FIG. 9, the movement command from the host CPU is divided by a coefficient term of (1/20) in order to equally distribute the movement command according to the processing cycle (1 ms) on the servo side. Sampling is performed to retrieve the movement command. Further, the sampled movement command is filtered by an exponential filter, and a movement command is output every 1 ms. FIG. 12A shows the speed of the movement command (unit of [rad / 20 ms] in the figure) from the host CPU and the speed of the movement command (unit of [1/20 rad / ms] in the figure) distributed by the command filter. represents, FIG. 12 (b) represents the acceleration [rad / ms ^2] for the speed command distributed by a command filter, FIG. 12 (c) jerk for the speed command distributed by the command filter [rad / ms ³ ].
[0022]
When feedforward is performed using a spike-like abnormally large jerk as shown in FIGS. 10 to 12, the current command saturates or the controlled object becomes oscillating due to the periodicity that appears discontinuously. May be caused.
[0023]
In addition, even in the process of trajectory planning, even if the jerk is restricted and a movement command is created, the jerk may be commanded at a speed higher than the calculated value in the servo processing.
[0024]
Therefore, the present invention solves the above-mentioned conventional problems, and can limit the jerk in the servo control loop, and realizes the jerk planned by the trajectory planning in the servo control loop. It is an object of the present invention to provide a method for distributing a movement command in servo control that can be performed.
[0025]
[Means for Solving the Problems]
The method for distributing movement commands in the servo control according to the present invention is such that, in the servo control, the movement commands in each processing cycle of the trajectory plan are equally distributed to the movement commands in each servo processing cycle, and the equally distributed movement commands are processed in the processing of the trajectory planning. The object is achieved by distributing by two-stage moving average processing having the same number of samplings as the ratio of the cycle to the servo processing cycle .
[0026]
In the present invention, the trajectory planning is a process performed by the host CPU on the control device side, and is a process of planning a position, a speed, or a trajectory so that the control target object performs a desired movement. In addition, the equal distribution is a method of distributing a movement command on the control device side to a movement command on the servo control side in a processing cycle and a servo processing cycle on the control device side having different processing cycles. Is equally distributed in accordance with the processing cycle. Further, the distribution by the moving average process of the present invention is a process of distributing by taking an average while shifting the equally distributed movement commands for each sampling period. In the present invention, the moving average process is performed twice. is there.
[0027]
The moving average processing can be performed by a moving average filter including a delay unit and an adder.
[0028]
In the present invention, the movement command output from the host CPU is divided equally by the coefficient value determined by the ratio of the processing cycle on the host side to the processing cycle on the servo side in order to distribute the movement command equally to the processing cycle on the servo side. After that, sampling is performed, and a movement command for each processing cycle on the servo side is extracted. The sampled movement command is subjected to a moving average by a first stage moving average process, and the output thereof is further subjected to a moving average by a second stage moving average process. The movement command after the moving average processing of the second stage is a movement command used for servo control.
[0029]
BEST MODE FOR CARRYING OUT THE INVENTION
Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
[0030]
FIG. 1 is a block diagram for explaining a method of distributing a movement command in the servo control of the present invention. On the servo side, a movement command transferred from the host in accordance with the processing cycle of the host is changed for each servo processing cycle. The moving commands are distributed by equal-distribution, passing the equally-distributed moving commands twice through a filter based on a moving average, and taking a two-stage moving average.
[0031]
In FIG. 1, the movement command from the host CPU is divided by a coefficient term for equal distribution in accordance with the processing cycle on the servo side. The processing cycle on the host side is a cycle determined by the processing cycle of the trajectory planning performed by the host CPU or the communication cycle between the host CPU and the servo side, and is, for example, 20 ms. On the other hand, the processing cycle on the servo side is generally shorter than the processing cycle on the host side, for example, 1 ms.
[0032]
At this time, when a speed command of V [rad / 20 ms] is output from the host side as shown in the speed command diagram of FIG. 2 (FIG. 2 (a)), it is viewed in processing cycle units on the servo side. And V / 20 [rad / ms]. That is, in order to evenly distribute the movement command from the host CPU according to the processing cycle on the servo side, the command value is divided by a coefficient value determined by the ratio of the processing cycle on the host side to the processing cycle on the servo side by this coefficient term. .
[0033]
Here, as shown in FIG. 3A, the speed command of V1 [rad / 20 ms] is output up to 20 ms, for example, and the speed command of V2 [rad / 20 ms] is output from 20 ms to 40 ms. Is output, the servo-side speed command as shown in FIG. 3B can be obtained through the coefficient term (step S1).
[0034]
Next, in order to perform equal distribution in accordance with the processing cycle on the servo side, the speed command obtained in step S1 is sampled at a sampling cycle corresponding to the processing cycle on the servo side. Assuming that the processing cycle on the servo side is 1 ms, the equally distributed movement commands obtained for sampling are represented by points in FIG. 3B. Hereinafter, the sampled movement command is set to A (step S2).
[0035]
Next, moving average processing is performed on the equally distributed movement commands. In the moving average processing, the moving command before the processing is A, the moving command after the first processing is B, the moving command after the second processing is C, the processing cycle on the host side and the processing cycle on the servo side. Assuming that the ratio of n is n, the moving average processing is represented by the following equation (6).
(1 + z- ¹ + z- ² + ... + z- ^(n-1) ) / n ... (6)
When n = 20, the first-stage moving average value B and the second-stage moving average value C are expressed by the following equations (7) and (8), respectively.
[0036]

The output C of the moving average processing of the second stage is the movement command C used for the servo control (steps S3 and S4).
[0037]
Here, the commanded acceleration Acmd and the commanded jerk Jcmd are expressed by the following equations (9) and (10).
[0038]

here,
B (i) -B (i-20) = [A (i) -A (i-20)] / 20
In addition, A is a distribution command of the movement command for each processing cycle on the host side, which is equal for 20 ms.

It becomes.
[0039]
Here, I is a numerical value updated for each processing cycle on the host side.
Similarly,

Therefore, the command jerk Jcmd is given by the following equation (13).
[0040]

Since the values of R (I), R (I-1), and R (I-2) are constant during the processing cycle on the host side during this command jerk Jcmd, the jerk is assumed to be constant. Become. Therefore, the conventional spike-shaped jerk is no longer present, and the jerk can be limited.
[0041]
On the other hand, the jerk Jcmd 'obtained from the movement command for each processing cycle on the host side is:

This indicates that the jerk speed planned by the host is reproduced.
[0042]
FIG. 4 is a schematic diagram showing the movement command, acceleration command, and jerk command at this time. FIG. 4A shows a movement command from the host CPU (unit of [rad / 20 ms] in the figure) and a movement command distributed by two-stage moving average (unit of [1/20 rad / ms] in the figure). FIG. 4B shows acceleration [rad / ms ² ] with respect to the speed command distributed by the two-stage moving average, and FIG. 4C shows the speed command distributed by the two-stage moving average. Represents the jerk [rad / ms ³ ] with respect to.
[0043]
FIG. 5 is a block diagram of a main part of a control diameter for implementing a method of distributing a movement command in servo control according to the present invention. In the figure, reference numeral 1 denotes a host CPU for controlling a servo control system, which performs a trajectory plan and outputs a movement command. For example, in the case of a robot, interpolation, conversion from a coordinate value in a rectangular coordinate system to a rotation angle of each axis, inverse conversion, and the like are performed, and a position command is distributed to each axis of the robot. Reference numeral 2 denotes a shared RAM that mediates the transmission of information between the host CPU 1 and the processor of the servo circuit 3. The shared RAM 2 transfers data such as a movement command written by the host CPU 1 to the processor of the servo circuit 3 and an alarm written by the processor of the servo circuit 3. The function of transferring information and the like to the host CPU 1 is performed. Reference numeral 3 denotes a servo circuit composed of a digital signal processor and the like, which is composed of a processor, ROM, RAM and the like. In the case of a robot, it controls servomotors of each axis. Perform processing. Reference numeral 4 denotes a servo amplifier including a transistor inverter and the like, and reference numeral 5 denotes a servomotor. Reference numeral 6 denotes a pulse coder for detecting the position of the servo motor 5, and the position is fed back to the servo circuit 4. The servo amplifier 4 and the servo motor 5 show only one axis.
[0044]
【Example】
FIG. 13 is a schematic block diagram of a control object of a two-inertia system and a control system. Here, JL = 0.6 [kg. cm. s ² ], Jm = 0.12 [kg. cm. s ² ], Am = 1.0 [kg. cm. s], Kt = 9 [kg. cm / Ap], maximum current = 80 [Ap], K = 400 [kg. cm / rad], D = 0.7 [kg. cm / (rad / s)], position gain = 0.01 [1 / ms], speed gain 1 = 15 [1 / ms], speed gain 2 = 6000 [1 / ms], torsion feedback gain 1 = 100 [ 1 / ms] and torsional feedback gain 2 = 3 [1 / ms]. In addition, * in a figure has shown the estimated value.
[0045]
FIG. 14 shows a movement command, an actual speed (a) and a current command (b) when distribution is performed by an exponential filter in the control object of the two-inertia system and the control system, and FIG. FIG. 16 shows a movement command, an actual speed (a) and a current command (b) when distribution is performed by a one-stage moving average filter in the control object and the control system. FIG. 3 shows a movement command, an actual speed (a), and a current command (b) when distribution is performed in a control system using a two-stage moving average filter according to the distribution method of the present invention.
[0046]
【The invention's effect】
As described above, according to the present invention, the jerk in the servo control loop can be limited, and the jerk planned by the trajectory planning can be realized in the servo control loop. A method of distributing a movement command in control can be provided.
[Brief description of the drawings]
FIG. 1 is a block diagram for explaining a method of distributing a movement command in servo control according to the present invention.
FIG. 2 is a diagram for explaining a speed command.
FIG. 3 is a diagram for explaining a speed command.
FIG. 4 is a schematic diagram showing a movement command, an acceleration command, and a jerk command based on a two-stage moving average.
FIG. 5 is a block diagram of a main part of a control diameter for implementing a movement command distribution method in servo control according to the present invention.
FIG. 6 is a model example of a two inertial system model.
FIG. 7 is a block diagram when a movement command is equally distributed.
FIG. 8 is a block diagram when a movement command is distributed by a moving average filter.
FIG. 9 is a block diagram when a movement command is distributed by an exponential filter.
FIG. 10 is a diagram showing a movement command, an acceleration command, and a jerk command by equal distribution.
FIG. 11 is a diagram showing a movement command, an acceleration command, and a jerk command by distribution of a moving average filter.
FIG. 12 is a diagram showing a movement command, an acceleration command, and a jerk command by distribution of an exponential filter.
FIG. 13 is a schematic block diagram of a control object and a control system of a two-inertia system.
FIG. 14 is a diagram showing a movement command, an actual speed, and a current command by distribution of an exponential filter in a controlled object of a two-inertia system and a control system.
FIG. 15 is a diagram showing a movement command, an actual speed, and a current command by distribution of a one-stage movement filter in a control object of a two-inertia system and a control system.
FIG. 16 is a diagram illustrating a movement command, an actual speed, and a current command by distribution of a two-stage movement filter in a control object of a two-inertia system and a control system.
[Explanation of symbols]
1 controller 2 shared RAM
3 Servo circuit 4 Servo amplifier 5 Motor 6 Pulse coder 10 Motor side 11 Machine side

Claims

In servo control, the movement command for each processing cycle of the trajectory plan is equally distributed to the movement command for each servo processing cycle, and the equally distributed movement command has the same sampling number as the ratio of the processing cycle of the trajectory plan to the servo processing cycle. A movement command distribution method in servo control, wherein distribution is performed by two-stage moving average processing.

2. The method according to claim 1, wherein the moving average processing is performed by a moving average filter.