(57)【特許請求の範囲】
【請求項１】
サーボ制御において、軌道計画の処理周期毎の移動指令をサーボ処理周期毎の移動指令に等分配し、等分配した移動指令を、軌道計画の処理周期とサーボ処理周期の比率と同じサンプリング数を持つ２段の移動平均処理により分配することを特徴とするサーボ制御における移動指令の分配方法。
【請求項２】
前記移動平均処理は、移動平均フィルタによって行うことを特徴とする請求項１記載のサーボ制御における移動指令の分配方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、サーボ制御に関し、特にＮＣ工作機械やロボットの制御装置等に使用されるサーボ制御において、サーボ制御ループの処理周期毎に分配される移動指令の分配方法に関する。
【０００２】
【従来の技術】
ＮＣ工作機械やロボットの制御装置等において、制御装置側のホストＣＰＵは、制御対象物が所望の動きを行うような位置、速度、あるいは軌跡を計画し、この軌道計画に基づいてサーボＣＰＵを介してサーボモータの制御を行っている。
【０００３】
軌道計画の処理周期がサーボ制御の処理周期に比べて長い場合、サーボＣＰＵは動作を滑らかにするために軌道計画の処理周期毎の移動指令を、サーボ処理周期毎の移動指令に分配する必要がある。
【０００４】
一般に、サーボ制御ループの処理周期は、近年の高速ＤＳＰ（Ｄｉｇｉｔａｌｓｉｇｎａｌ  ｐｒｏｃｅｓｓｅｒ；デジタル信号プロセッサ）の採用によって短縮されるのに対して、軌道計画については、逆変換等に多大な処理時間を要するため、軌道計画の処理周期はサーボ処理周期に比べて長い周期を要する傾向にあるため、サーボ制御を行う場合、軌道計画の処理周期毎の移動指令をサーボ処理周期毎の移動指令に分配する処理の必要性が増大している。
【０００５】
なお、通常、軌道計画の処理周期は、軌道計画の処理を行うホストＣＰＵとサーボ処理を行うサーボＣＰＵとの間の通信周期により制限されることが多いため、以下では通信周期の処理周期を軌道計画の処理周期として扱うものとする。
表すものとする。
【０００６】
工作機械やロボット等の制御対象物は、バネ要素およびダンパ要素を含む２慣性系モデルにより一般化することができる。図６は２慣性系モデルのモデル例である。符号１０はモータ側を示し、符号１１は機械側を示している。このモータ側１０と機械側１１の間に減速器が設けられ、該減速器のバネ定数をＫ［ｋｇ．ｃｍ／ｒａｄ］、粘性係数をＤ［ｋｇ．ｃｍ／（ｒａｄ／ｓ）］とする。また、モータのロータイナーシャをＪｍ［ｋｇ．ｃｍ．ｓ２  ］、モータの動摩擦係数をＡｍ［ｋｇ．ｃｍ．ｓ］、モータ位置をθｍ［ｒａｄ］、負荷側のイナーシャをＪＬ  ［ｋｇ．ｃｍ．ｓ２  ］、負荷位置（ロボットの場合には、ロボットのアーム先端位置）をθＬ  ［ｒａｄ］、モータのトルク定数をＫｔ［ｋｇ．ｃｍ／Ａｐ］とする。
【０００７】
モータへの電流指令をｕ［Ａｐ］とし、トルク指令をτ［ｋｇ．ｃｍ］として、負荷側について運動方程式をたてると以下の式（１）となる。
【０００８】
ＪＬ  ・θＬ  ＊＊＋Ｄ・（θＬ  ＊−θｍ＊）＋Ｋ・（θＬ  −θｍ）＝０…（１）
また、モータ側について運動方程式をたてると以下の式（２）となる。
【０００９】
0003599849.tif 000002
なお、記号の右側に付した「＊」は、その記号で示される変数を微分することを示し、「＊」は１回微分を表し「＊＊」は２回微分を表している。すなわち、θＬ  ＊＊は負荷位置θＬ  の２回微分で負荷の速度を表している。また、（θＬ  −θｍ）は制御対象点である負荷位置に対するモータのロータ回転位置偏差（ねじれ量）を意味し、（θｍ＊−θＬ  ＊）は速度偏差（ねじれ速度）を意味している。
【００１０】
上記式（１），（２）から、負荷側位置と電流指令との関係は以下の式（３）によって表される。
【００１１】
0003599849.tif 000003
ここで、ＤはＫより充分大きい（Ｄ＜＜Ｋ）とすることができるため、さらに以下の式（４）とすることができる。
【００１２】
0003599849.tif 000004
ここで、負荷位置θＬ  を指令値ｕの通りに動作させるためのフィードフォワードトルクｕｆｆは、移動指令をθｒとすると以下の式（５）となる。
【００１３】
0003599849.tif 000005
このとき、ｓ４  の項は充分小さいため無視することができるが、ｓ３  の項のフィードフォワード分は大きく、制振効果に影響を与えることになる。
【００１４】
従来、このような２慣性系モデルにおいて、軌道計画の処理周期が短い場合には、サーボ処理周期において軌道計画の移動指令を単に等分配して得られる分配指令によってサーボ制御を行ったり、サーボ処理周期において軌道計画の移動指令を移動平均によるフィルタリングで得られる分配指令によってサーボ制御を行ったり、あるいは、サーボ処理周期において軌道計画の移動指令を指数加減速によるフィルタリングで得られる分配指令によってサーボ制御を行っている。
【００１５】
【発明が解決しようとする課題】
しかしながら、従来のサーボ制御における移動指令の分配方法では、制御対象の加々速度がスパイク状に大きくなるため、この加々速度を用いてフィードフォワード制御を行うと、トルク指令が飽和したり、制御対象の動きが振動的になるという問題点がある。２慣性系の制御対象においては、加々速度による制御が有効な場合が多く、この加々速度が異常に大きくなったり振動する場合には、制御に不都合が生じることになる。
【００１６】
ここで、ホスト側の処理周期およびホストＣＰＵとサーボＣＰＵとの間の通信周期を例えば２０ｍｓとし、サーボ側の処理周期を１ｍｓとし、等分配によって移動指令を分配した場合、移動平均フィルタによって移動指令を分配した場合、指数フィルタによって移動指令を分配した場合の各場合について、加速度および加々速度の変化について、図７〜図１２を用いて説明する。
【００１７】
以下の説明では、移動指令は、通常ホスト側とサーボ側との通信周期毎の移動指令である場合が多いため、移動指令の単位を［ｒａｄ／２０ｍｓ］とする。
図７は、移動指令をサーボ処理周期毎に等分配する場合のブロック線図であり、図１０はこのときの移動指令，加速度指令，および加々速度指令を示す図である。図７において、ホストＣＰＵからの移動指令は、サーボ側の処理周期（１ｍｓ）に合わせて等分配するために（１／２０）の係数項によって指令値が除され、さらに、サーボ側の処理周期毎に移動指令を取り出すためにサンプリングが行われる。これによって、等分配された１ｍｓ毎の移動指令が出力される。
【００１８】
図１０（ａ）はホストＣＰＵからの移動指令（図中の［ｒａｄ／２０ｍｓ］の単位）と等分配された移動指令（図中の［１／２０ｒａｄ／ｍｓ］の単位）の速度を表し、図１０（ｂ）は等分配された速度指令に対する加速度［ｒａｄ／ｍｓ２  ］を表し、図１０（ｃ）は等分配された速度指令に対する加々速度［ｒａｄ／ｍｓ３  ］を表している。
【００１９】
この等分配の場合には、サーボ処理周期毎の移動指令は不連続な階段状となり、また、加々速度はスパイク状となり、異常に大きな値をとる場合がある。
【００２０】
また、図８は、移動指令をサーボ処理周期毎に移動平均フィルタによって移動指令を分配する場合のブロック線図であり、図１１はこのときの移動指令，加速度指令，および加々速度指令を示す図である。図８において、ホストＣＰＵからの移動指令は、サーボ側の処理周期（１ｍｓ）に合わせて等分配するために（１／２０）の係数項によって指令値が除され、サーボ側の処理周期毎に移動指令を取り出すためにサンプリングが行われる。さらに、サンプリングされた移動指令は、移動平均フィルタによってサンプリング周期毎にずらしながら平均をとる。これによって、移動平均によって分配された１ｍｓ毎の移動指令が出力される。図１１（ａ）はホストＣＰＵからの移動指令（図中の［ｒａｄ／２０ｍｓ］の単位）と移動平均によって分配された移動指令（図中の［１／２０ｒａｄ／ｍｓ］の単位）の速度を表し、図１１（ｂ）は移動平均によって分配された速度指令に対する加速度［ｒａｄ／ｍｓ２  ］を表し、図１１（ｃ）は移動平均によって分配された速度指令に対する加々速度［ｒａｄ／ｍｓ３  ］を表している。
【００２１】
また、図９は、移動指令をサーボ処理周期毎に指数フィルタによって移動指令を分配する場合のブロック線図であり、図１２はこのときの移動指令，加速度指令，および加々速度指令を示す図である。図９において、ホストＣＰＵからの移動指令は、サーボ側の処理周期（１ｍｓ）に合わせて等分配するために（１／２０）の係数項によって指令値が除され、サーボ側の処理周期毎に移動指令を取り出すためにサンプリングが行われる。さらに、サンプリングされた移動指令は、指数フィルタによってフィルタリングされ、１ｍｓ毎の移動指令が出力される。図１２（ａ）はホストＣＰＵからの移動指令（図中の［ｒａｄ／２０ｍｓ］の単位）と指令フィルタによって分配された移動指令（図中の［１／２０ｒａｄ／ｍｓ］の単位）の速度を表し、図１２（ｂ）は指令フィルタによって分配された速度指令に対する加速度［ｒａｄ／ｍｓ２  ］を表し、図１２（ｃ）は指令フィルタによって分配された速度指令に対する加々速度［ｒａｄ／ｍｓ３  ］を表している。
【００２２】
図１０〜図１２に示すようなスパイク状の異常に大きな値の加々速度を用いてフィードフォワードを行うと、電流指令が飽和したり、不連続に表れる周期性のために制御対象が振動的となる虞がある。
【００２３】
また、軌道計画の工程において、加々速度を制限して移動指令を作成しても、サーボ処理では計算値以上の加々速度を指令する場合がある。
【００２４】
そこで、本発明は前記した従来の問題点を解決して、サーボ制御ループにおるけ加々速度を制限することができ、また、軌道計画により計画された加々速度をサーボ制御ループで実現することができるサーボ制御における移動指令の分配方法を提供することを解決課題とする。
【００２５】
【課題を解決するための手段】
本発明のサーボ制御における移動指令の分配方法は、サーボ制御において、軌道計画の処理周期毎の移動指令をサーボ処理周期毎の移動指令に等分配し、等分配した移動指令を、軌道計画の処理周期とサーボ処理周期の比率と同じサンプリング数を持つ２段の移動平均処理により分配することによって、前記目的を達成する。
【００２６】
本発明において、軌道計画は制御装置側のホストＣＰＵが行う処理であり、制御対象物が所望の動きを行うように位置、速度、あるいは軌跡を計画する処理である。また、等分配は、処理周期の異なる制御装置側の処理周期とサーボ処理周期において、制御装置側の移動指令をサーボ制御側の移動指令に分配する一分配方法であり、制御装置側の移動指令をその処理周期に合わせて等しく分配する処理である。また、本発明の移動平均処理による分配は、等分配された移動指令をサンプリング周期毎にずらしながら平均をとって分配する処理であり、本発明においては、この移動平均処理を２回行うものである。
【００２７】
なお、この移動平均処理は、遅延器と加算器により構成される移動平均フィルタによって行うことができる。
【００２８】
本発明において、ホストＣＰＵから出力された移動指令は、サーボ側の処理周期に合わせて等分配するために、ホスト側の処理周期とサーボ側の処理周期の比率で定まる係数値によって指令値が除された後サンプリングが行われて、サーボ側の処理周期毎の移動指令が取り出される。このサンプリングされた移動指令は、１段目の移動平均処理によって移動平均され、さらにその出力は２段目の移動平均処理によって移動平均される。この２段目の移動平均処理後の移動指令が、サーボ制御に用いる移動指令となる。
【００２９】
【発明の実施の形態】
以下、本発明の実施の形態を図を参照しながら詳細に説明する。
【００３０】
図１は、本発明のサーボ制御における移動指令の分配方法を説明するためのブロック線図であり、サーボ側において、ホスト側からホスト側の処理周期に従って受け渡される移動指令をサーボ処理周期毎に等分配し、等分配された移動指令を移動平均によるフィルタを２回通して、２段の移動平均をとることによって移動指令を分配するものである。
【００３１】
図１において、ホストＣＰＵからの移動指令は、サーボ側の処理周期に合わせて等分配するためにの係数項によって指令値が除される。ホスト側の処理周期は、ホストＣＰＵが行う軌道計画の処理周期あるいはホストＣＰＵとサーボ側間の通信周期により定まる周期であり、例えば２０ｍｓとする。これに対して、サーボ側の処理周期は一般にホスト側の処理周期より短く、例えば１ｍｓとする。
【００３２】
このとき、図２の速度指令の図に示すように、ホスト側からＶ［ｒａｄ／２０ｍｓ］の速度指令が出力される場合（図２（ａ））には、サーボ側の処理周期単位でみるとＶ／２０［ｒａｄ／ｍｓ］となる。つまり、ホストＣＰＵからの移動指令をサーボ側の処理周期に合わせて等分配するために、この係数項によって指令値をホスト側の処理周期とサーボ側の処理周期の比率で定まる係数値で除する。
【００３３】
ここで、ホスト側の移動指令が図３（ａ）に示すように、例えば２０ｍｓまではＶ１［ｒａｄ／２０ｍｓ］の速度指令が出力され、２０ｍｓかち４０ｍｓまではＶ２［ｒａｄ／２０ｍｓ］の速度指令が出力される場合には、上記係数項を経ることによって図３（ｂ）に示すようなサーボ側の速度指令を得ることができる（ステップＳ１）。
【００３４】
次に、サーボ側の処理周期に合わせて等分配するために、前記ステップＳ１で求めた速度指令を、サーボ側の処理周期に合わせたサンプリング周期でサンプリングする。サーボ側の処理周期を１ｍｓとすると、サンプリングに得られる等分配された移動指令は図３（ｂ）中の点で表される。以下、このサンプリングされた移動指令をＡとする（ステップＳ２）。
【００３５】
次に、等分配された移動指令に移動平均処理を行う。移動平均処理は、処理前の移動指令をＡとし、１段目の処理後の移動指令をＢ，２段目の処理後の移動指令をＣとし、ホスト側の処理周期とサーボ側の処理周期の比率をｎとすると、移動平均処理は以下の式（６）によって表され、
（１＋ｚ−１＋ｚ−２＋・・・＋ｚ−（ｎ−１））／ｎ                      …（６）
ｎ＝２０の場合の１段目の移動平均の値Ｂと２段目の移動平均の値Ｃはそれぞれ以下の以下の式（７），（８）によって表される。
【００３６】
0003599849.tif 000006
この２段目の移動平均の処理の出力Ｃがサーボ制御に用いる移動指令Ｃとなる（ステップＳ３，４）。
【００３７】
ここで、指令加速度Ａｃｍｄおよび指令加々速度Ｊｃｍｄは、以下の式（９），（１０）となる。
【００３８】
0003599849.tif 000007
ここで、
Ｂ（ｉ）−Ｂ（ｉ−２０）＝［Ａ（ｉ）−Ａ（ｉ−２０）］／２０
であり、また、Ａはホスト側の処理周期毎の移動指令を等分配したものであり、２０ｍｓの間は等しいため、
0003599849.tif 000008
となる。
【００３９】
ここで、Ｉはホスト側の処理周期毎に更新される数値である。
同様にして、
0003599849.tif 000009
となるため、指令加々速度Ｊｃｍｄは以下の式（１３）となる。
【００４０】
0003599849.tif 000010
この指令加々速度Ｊｃｍｄは、ホスト側の処理周期に期間中においてはＲ（Ｉ），Ｒ（Ｉ−１），Ｒ（Ｉ−２）の値は一定であるため、加々速度は一定となる。したがって、従来のうよなスパイク状の加々速度なくなり、加々速度を制限することができる。
【００４１】
一方、ホスト側における処理周期毎の移動指令から求められる加々速度Ｊｃｍｄ’は、
0003599849.tif 000011
となり、このことはホスト側で軌道計画された加々速度が再現されることを示している。
【００４２】
図４はこのときの移動指令，加速度指令，および加々速度指令のを示す概略図である。図４（ａ）はホストＣＰＵからの移動指令（図中の［ｒａｄ／２０ｍｓ］の単位）と２段の移動平均によって分配された移動指令（図中の［１／２０ｒａｄ／ｍｓ］の単位）の速度を表し、図４（ｂ）は２段の移動平均によって分配された速度指令に対する加速度［ｒａｄ／ｍｓ２  ］を表し、図４（ｃ）は２段の移動平均によって分配された速度指令に対する加々速度［ｒａｄ／ｍｓ３  ］を表している。
【００４３】
図５は、本発明のサーボ制御における移動指令の分配方法を実施するための制御径の要部ブロック図である。図中において、１はサーボ制御系を制御するホストＣＰＵで、軌道計画を行って移動指令を出力する。例えば、ロボットの場合には、補間、直交座標系の座標値から各軸の回転角への変換，逆変換等を行うとともに、ロボットの各軸へ位置指令を分配する。２はホストＣＰＵ１とサーボ回路３のプロセッサ間の情報の伝達を仲介する共有ＲＡＭで、ホストＣＰＵ１が書き込んだ移動指令等のデータをサーボ回路３のプロセッサに受渡し、サーボ回路３のプロセッサが書き込んだアラーム情報等をホストＣＰＵ１に引き渡す機能を行う。３はデジタルシグナルプロセッサ等で構成されるサーボ回路でプロセッサ，ＲＯＭ，ＲＡＭ等で構成され、ロボットの場合には各軸のサーボモータの制御を行うもので、本発明のサーボ制御における移動指令の分配処理を行う。４はトランジスタインバータ等で構成されるサーボアンプで、５はサーボモータである。また，６はサーボモータ５の位置を検出するパルスコーダで、位置はサーボ回路４にフィードバックされる。なお、サーボアンプ４，サーボモータ５は１軸のみを図示している。
【００４４】
【実施例】
図１３は、２慣性系の制御対象と制御系の概略ブロック図である。ここで、ＪＬ  ＝０．６［ｋｇ．ｃｍ．ｓ２  ］，Ｊｍ＝０．１２［ｋｇ．ｃｍ．ｓ２  ］，Ａｍ＝１．０［ｋｇ．ｃｍ．ｓ］，Ｋｔ＝９［ｋｇ．ｃｍ／Ａｐ］，最大電流＝８０［Ａｐ］，Ｋ＝４００［ｋｇ．ｃｍ／ｒａｄ］，Ｄ＝０．７［ｋｇ．ｃｍ／（ｒａｄ／ｓ）］，ポジションゲイン＝０．０１［１／ｍｓ］，速度ゲイン１＝１５［１／ｍｓ］，速度ゲイン２＝６０００［１／ｍｓ］，ねじれフィードバックゲイン１＝１００［１／ｍｓ］，ねじれフィードバックゲイン２＝３［１／ｍｓ］とする。なお、図中の＊は推定値を示している。
【００４５】
図１４は、この２慣性系の制御対象と制御系において指数フィルタによる分配を行った場合の移動指令，実速度（ａ）および電流指令（ｂ）を示し、図１５は、同２慣性系の制御対象と制御系において１段の移動平均フィルタによる分配を行った場合の移動指令，実速度（ａ）および電流指令（ｂ）を示し、また、図１６は、同２慣性系の制御対象と制御系において本発明の分配方法による２段の移動平均フィルタによる分配を行った場合の移動指令，実速度（ａ）および電流指令（ｂ）を示している。
【００４６】
【発明の効果】
以上説明したように、本発明によれば、サーボ制御ループにおるけ加々速度を制限することができ、また、軌道計画により計画された加々速度をサーボ制御ループで実現することができるサーボ制御における移動指令の分配方法を提供することができる。
【図面の簡単な説明】
【図１】本発明のサーボ制御における移動指令の分配方法を説明するためのブロック線図である。
【図２】速度指令を説明するための図である。
【図３】速度指令を説明するための図である。
【図４】２段の移動平均による移動指令，加速度指令，および加々速度指令のを示す概略図である。
【図５】本発明のサーボ制御における移動指令の分配方法を実施するための制御径の要部ブロック図である。
【図６】２慣性系モデルのモデル例である。
【図７】移動指令を等分配する場合のブロック線図である。
【図８】移動指令を移動平均フィルタにより分配する場合のブロック線図である。
【図９】移動指令を指数フィルタにより分配する場合のブロック線図である。
【図１０】等分配による移動指令，加速度指令，および加々速度指令を示す図である。
【図１１】移動平均フィルタの分配による移動指令，加速度指令，および加々速度指令を示す図である。
【図１２】指数フィルタの分配による移動指令，加速度指令，および加々速度指令を示す図である。
【図１３】２慣性系の制御対象と制御系の概略ブロック図である。
【図１４】２慣性系の制御対象および制御系において指数フィルタの分配による移動指令，実速度，電流指令を示す図である。
【図１５】２慣性系の制御対象および制御系において１段の移動フィルタの分配による移動指令，実速度，電流指令を示す図である。
【図１６】２慣性系の制御対象および制御系において２段の移動フィルタの分配による移動指令，実速度，電流指令を示す図である。
【符号の説明】
１  制御装置
２  共有ＲＡＭ
３  サーボ回路
４  サーボアンプ
５  モータ
６  パルスコーダ
１０  モータ側
１１  機械側
【図１】
0003599849.tif 000012
【図２】
0003599849.tif 000013
【図３】
0003599849.tif 000014
【図４】
0003599849.tif 000015
【図５】
0003599849.tif 000016
【図６】
0003599849.tif 000017
【図７】
0003599849.tif 000018
【図８】
0003599849.tif 000019
【図９】
0003599849.tif 000020
【図１０】
0003599849.tif 000021
【図１１】
0003599849.tif 000022
【図１２】
0003599849.tif 000023
【図１３】
0003599849.tif 000024
【図１４】
0003599849.tif 000025
【図１５】
0003599849.tif 000026
【図１６】
0003599849.tif 000027