JP3736029B2 - Pulse motor drive system trajectory control apparatus and method, and recording medium - Google Patents

Description

により、Ｐ_i ´（ｘ_i ＊´、ｙ_i ＊´、ｚ_i ＊´）が決定できる。
【００３５】
以後、このＰ_i ´を起点として、ここからの距離が△ｌとなる点を同様に決定していく。このアルゴリズムでは、メモリ上の位置データは既にスムージングされており、この位置を基にＣＰ速度Ｖｃｐ一定となるように線形補間がなされるから、図７に示すように、ＣＰ位置、ＣＰ速度は、各々期待通りとなる。
しかしながら、以上のようなアルゴリズムにおいて、ＣＰＵ能力等の関係で、サンプリング時間Ｔｓが大きいと、関節各速度がジャンプし、このままでは、メカ振動、脱調等の問題が残る。
【００３６】
そこで、本例では、ＣＰ速度補正を１００％行うのでなく、所定のＰ％（＜１００％）のみ実行する方法を採用する。なお、サンプリング時間Ｔｓが充分小さい場合には、このような処理は不要である。
１００％補正では、Ｔｓ間の移動距離が△ｌ＝Ｖｃｐ・Ｔｓとなるようなポイントをメモリ上のポイントデータに基づいて線形補間でもとめた。
ここで、△ｌ（ｎ）を、ｔ＝ｎＴｓとｔ＝（ｎ−１）Ｔｓとの間の距離とする。つまり、
△ｌ（ｎ）＝｛Ｐ_n ² −Ｐ_n-1 ² ｝^1/2 ≦△ｌ ……（９）
【００３７】
次に、△ｌ´（ｎ）を、Ｐ％補正する場合のｔ＝ｎＴｓとｔ＝（ｎ−１）Ｔｓとの間の距離とする。このとき、

これにより、△ｌ´（ｎ）を決定する。これ以後、メモリサーチ及び線形補間は、１００％補正時と同様に処理することにより、図８に示すように、ＣＰ速度、Ｐ％補正ができ、関節各速度、ジャンプ量を小さくでき、メカ振動を小さく抑制できる。なお、このＰ％の値は、ロボット言語やティーチングペンダント等によりユーザが適宜入力できるようにする。
【００３８】
次に、以上のようにして得られた補正データ（ｘ、ｙ、ｚ）に基づいて、インバース（逆変換）部１８０において、ロボットの関節角Θ（θｘ、θｙ、θｚ）に変換し、後段のパルスボードに送出する。
【００３９】
図９は、本発明によるパルスモータロボットの軌道制御装置によるスムージングアルゴリスムの第２の例を示すブロック図である。
上述した第１の例による速度補正方法では、図８からも分かるように、補正された速度において、ｔ＝Ｔｐ／２にて速度が滑らかでなくなる。つまり、ＣＰ加速度が、わずかかも知れないが、ジャンプすることとなる。そして、このジャンプ（不連続性）によって、関節各速度のジャンプとなり、メカ振動、脱調上好ましくない。そこで本発明の第２の例では、このような問題に対応する構成ついて説明する。なお、このスムージングアルゴリスムは、上述した第１の例によるスムージングアルゴリスムと併用できるものである。
【００４０】
このスムージングアルゴリスムは、ＣＰ曲線のサンプリング部２１０と、ランプ信号発生部２２０と、座標変換部２３０と、３次ローパスフィルタ（Ψ−ソフトモータ）部２４０と、曲線長計算部２５０と、３次ローパスフィルタ（ｌ−ソフトモータ）部２６０と、速度補正用メモリ部２７０と、インバース（逆変換）部２８０とを有する。
このうち、ＣＰ曲線のサンプリング部２１０とランプ信号発生部２２０は、図１に示すサンプリング部１１０とランプ信号発生１２０と共通である。
【００４１】
また、座標変換部２３０は、図１に示す座標変換部１４０と同様の機能を有しており、ランプ信号発生部２２０より出力される曲線長ｌを空間座標ψ（ｘ、ｙ、ｚ）に変換するものである。
また、３次ローパスフィルタ（Ψ−ソフトモータ）部２４０は、図１に示す３次ローパスフィルタ部１５０と同様であり、座標変換部２３０からの座標データｘ、ｙ、ｚをローパスフィルタを通してスムージングするものである。なお、各軸ｘ、ｙ、ｚのソフトモータにおける時定数は同じである。
【００４２】
また、曲線長計算部２５０は、３次ローパスフィルタ部１５０の出力データｘ＊、ｙ＊、ｚ＊に基づいて、ローパスがかけられたＣＰ曲線長ｌ＊を計算するものである。つまり、ｌ＊＝｛ｘ＊² ＋ｙ＊² ＋ｚ＊² ｝^1/2 を計算する。
また、速度補正用メモリ部２７０では、以上のようなｌ＊及び（ｘ＊、ｙ＊、ｚ＊）をサンプリング時間毎に順番にＰ_n （ｌ＊、ｘ＊、ｙ＊、ｚ＊）の形で格納するものである。
【００４３】
３次ローパスフィルタ（ｌ−ソフトモータ）部２６０は、曲線長計算部２５０から出力されるＣＰ曲線長ｌ＊を３次ローパスフィルタを通してスムージングするものである。この３次ローパスフィルタ部２６０の出力をｌ＊＊とする。
図１０に示すように、ｌ＊’は、くさび形に速度が鋭く落ち込み、ディスペンサ作業等において不都合も予想される。そこで、ｌ＊をさらに３次ローパスフィルタ部２６０でスムージングすることにより、速度もスムージングされる。
図１０から分かるように、ｌ＊’の変化が滑らかにスムージングされたＣＰ加速度、連続のｌ＊＊’が得られる。なお、ｌ−ソフトモータのＦ特性を小さくした方が図１０に示すように補正が強く働くものである。
【００４４】
速度補正用メモリ部２７０は、上述したＰ_n （ｌ＊、ｘ＊、ｙ＊、ｚ＊）を順次格納するとともに、サンプリング時間に対応し、３次ローパスフィルタ部２６０からのｌ＊＊に基づいて（ｘ＊、ｙ＊、ｚ＊）を順次サーチして出力する機能を有している。以下、この手順について説明する。
まず、ｌ＊＊は、ｌ−ソフトモータによりスムージングを受けた曲線長であるが、このｌ＊＊に対応するＣＰ曲線上の点（ｘ＊、ｙ＊、ｚ＊）をメモリよりサーチして求める。
メモリ内には、Ｐ_n （ｌ＊、ｘ＊、ｙ＊、ｚ＊）の形で格納されているので、ｌ＊＝ｌ＊＊となるＰ_n をサーチして、このＰ_n （ｘ＊、ｙ＊、ｚ＊）を次段のインバース部２８０に送出する。
【００４５】
これにより、ｌ−ソフトモータでスムージングされたｌ＊＊と、Ψ−ソフトモータでスムージングされたｌ＊との間に、ＣＰ曲線として位置エラーが入り込まないようにしている。
なお、上述のようにｌ＊＝ｌ＊＊となるＰ_n をサーチして見つからない場合には、図１で利用した線形補間法を適用する。
また、インバース部２８０は、速度補正用メモリ部２７０にて得られた補正データ（ｘ＊、ｙ＊、ｚ＊）よりロボットの関節角Θ（θｘ、θｙ、θｚ）を算出し、後段のパルスボードに送出する。
【００４６】
次に、以上のような各例における速度補正用メモリ部１６０、２７０の使用方法の詳細について順を追って説明する。
まず、ＣＰ曲線計算スタートで、ソフトモータ出力をメモリに順次ストアしていく。この段階ではロボットはまだ動作していない。
次に、メモリは、Ｔｐ／Ｔｓｓの記憶領域を用意してあるので、このメモリがｆｕｌｌになったら、ロボットのＣＰ動作を実際にスタートさせる。つまり、メモリの先頭データを変換してパルスボードに送出開始する。
これ以後も、メモリにソフトモータ出力をストアしていくが、メモリ容量はＴｐ／Ｔｓｓしか用意していないので、１つのデータをストアする毎に、Ｔｐ前のメモリデータは順番に捨てていく。したがって、このメモリはリングバッファとするのが有効である。
【００４７】
次に、このメモリを用いて、速度補正をかけるトリがとなるのは、各軸の速度のジャンプ量、すなわち指令加速度（ｘ”、ｙ”、ｚ”）のいずれかの値が、前もって与えられる加速度閾値（ｘ_S ”、ｙ_S ”、ｚ_S ”）をオーバーした場合とする。従って、閾値をオーバーしないようなＣＰ曲線であれば、この速度補正は実行されない。
なお、加速度閾値（ｘ_S ”、ｙ_S ”、ｚ_S ”）は、メカ構造に左右される量であるので、事前の実験等で決定しておく必要がある。また、加速時（０≦ｔ≦Ｔｐ）と減速時（Ｔｆ≦ｔ≦Ｔｆ＋Ｔｐ）にも補正はかけない。
【００４８】
加速度ジャンプがＴｐ時間内にコーナが続いた場合、Ｔｐ間の速度は補正される。しかし、Ｔｐ以降の部分の速度補正はかからない。こういったケースは少ないので対応する必要は必ずしもないが、もし対応する場合は、メモリをリングバッファ構造にして、常に補正をかけてしまう手法も可能である。特に、ＣＰＵの計算負担が重くない場合に、この方法が有効である。
【００４９】
次に、以上のようなパルスモータ専用ＣＰ曲線のスムージングアルゴリズムに関する応用例について説明する。
まず、パルスモータ脱調防止アルゴリズムについて説明する。
ロボットのユーザがどんな形のＣＰ曲線でロボットを走らせるか、ロボット設計者には分からない。これに対し、どんな形のＣＰ曲線に対しても、パルスモータが脱調せぬように、ＣＰ最高速度Ｖｃｐ_max をかなり低めに設計することは可能であるが、これでは保守的な設計となり、ロボットの能力を抑えてしまう。
そこでこのような場合、ユーザが欲するＣＰ速度で走らせて見て、脱調した場合、３つのψ−ソフトモータの時定数を適当に大きくすることで脱調を防止することができる。もちろんこの場合、ＣＰ軌跡のズレは大きくなるが、脱調を防止するための選択肢として有効となる。なお、このように３つのψーソフトモータ時定数をユーザがロボット言語やティーチングペンダントから変更できるようにしておく。
【００５０】
また、上記のようにユーザにソフトモータ時定数を変更させるシステムがどうしても好ましくない場合、前計算方式とすると自動説調防止が可能となる。以下に手順概要を示す。
まず、図１に示す処理をＣＰ曲線最後までロボットを動作させずに行う（前計算）。
次に、前計算時における各軸の（Θｘ’、Θｙ’、Θｚ’）及び（Θｘ”、Θｙ”、Θｚ”）を各サンプリング時間毎にメモリに格納する。
このメモリデータを利用して、図１１に示すように、各軸毎にＴＮ特性を描いて見て、ＴＮ特性をはみ出している軸を見つける。なお、複数の軸が共にはみ出している場合、はみ出している割合が大きい軸のＴＮ軌跡を描いて見る。
【００５１】
次に、ＴＮ軌跡の中で加速度がピークとなる点（Θｐ”、Θｐ’）を見つける。ここで、速度Θｐ’におけるＴＮ特性上の加速度をΘ₁ ”とする場合に、α＝（Θｐ”／Θ₁ ”）^1/2 となるαを計算し、ψーソフトモータの時定数をα倍以上に設定すれば、脱調を防止できる。
何故なら、（ｘ、ｙ、ｚ）ソフトモータの時定数をα倍すれば、加速度は１／α² となり、Θｐ”がΘ₁ ”となりＴＮ特性以内に納まるからである。
【００５２】
次に、上述したスムージングアルゴリズムを用いた場合、ＣＰ軌跡がＣＰ速度Ｖｃｐでどのように変化するかを説明する。なお、ＣＰ曲線は無数にあるので、上述した直交ロボットによる直角コーナの場合で説明する。
ψーソフトモータの時定数の１つである速度ピーク到達時間をＴｐとする。すると、Ｔｐ間に走るＣＰ曲線上の距離△ｌ（Ｔｐ）は、△ｌ（Ｔｐ）≒Ｖｃｐ／Ｔｐで、ＣＰ速度Ｖｃｐにほぼ比例して図１２に示すようになる。
つまり、Ｖｃｐが大きい程、コーナからのズレが大きくなるが、Ｖｃｐが小さい程、ズレは小さくなる。そして、ユーザがＶｃｐをチューニングすることで、ズレを調整できる。
【００５３】
また、上述したスムージングアルゴリズムは、ホストＣＰＵにとっては、かなりな処理負担となる。このうち、ｌ−ソフトモータ１３０はＣＰ速度の加減速を滑らかに生成するローパスフィルタであるが、後段のψーソフトモータ１５０も、同様の役割を持っているので、ＣＰＵへの負担が大きい場合、ｌ−ソフトモータ１３０は省略しても良い。
さらに、上述した各例では、制御対象物としてパルスモータロボットを制御する構成について説明したが、本発明はロボット以外の駆動系を制御する構成として広く応用し得るものである。
【００５４】
【発明の効果】
以上説明したように本発明では、曲線長データより得られるｎ次元の各座標データを、それぞれローパスフィルタに通してスムージングし、これらのスムージングされた座標データを速度補正用メモリに一時格納し、この格納した座標データを順次読み出して駆動信号に変換し、パルスモータ駆動系に出力するようにした。
したがって本発明によれば、安価なパルスモータ駆動系において、高速ＣＰ動作や折れ線状のＣＰ軌道に対し、サーボモータ駆動系と同等ないしそれ以上の滑らかな軌道を得ることができ、また安定した動作速度を容易に得ることができる効果がある。
【図面の簡単な説明】
【図１】本発明によるパルスモータロボットの軌道制御装置によるスムージングアルゴリスムの第１の例を示すブロック図である。
【図２】図１に示すアルゴリスムにおけるψソフトモータの作用を説明する説明図である。
【図３】図１に示すアルゴリスムにおけるＣＰ曲線の直線コーナ部に対するスムージング曲線の生成工程を示す説明図である。
【図４】図１に示すアルゴリスムにおけるψソフトモータの特性とＣＰ精度との関係を説明する説明図である。
【図５】図１に示すアルゴリスムにおける速度スムージングによる作用を説明する説明図である。
【図６】図１に示すアルゴリスムにおける速度スムージングデータの補間方法を説明する説明図である。
【図７】図１に示すアルゴリスムにおける速度スムージングデータの補間による作用を説明する説明図である。
【図８】図１に示すアルゴリスムにおける速度スムージングデータの速度補正量を制御した場合の作用を説明する説明図である。
【図９】本発明によるパルスモータロボットの軌道制御装置によるスムージングアルゴリスムの第２の例を示すブロック図である。
【図１０】図９に示すアルゴリスムにおけるψソフトモータの周波数特性と速度スムージングデータとの関係を示す説明図である。
【図１１】図１、図９に示すアルゴリスムにおける脱調防止のためのＴＮ特性と加速度の一例を示す説明図である。
【図１２】図１、図９に示すアルゴリスムにおけるＣＰ速度とＣＰ軌跡の関係を示す説明図である。
【図１３】サーボモータロボットのスムージングアルゴリズムの従来例の構成を示す説明図である。
【図１４】図１、図９、図１３に示すアルゴリズムにおいてＣＰ速度を変更した場合のランプ信号を示す説明図である。
【図１５】ランプ信号をローパスフィルタによってスムージングした場合の波形例を示す説明図である。
【図１６】直角コーナを有するＣＰ曲線におけるＣＰ速度の変動を示す説明図である。
【符号の説明】
１１０、２１０……サンプリング部、１２０、２２０……ランプ信号発生部、１３０、２６０……３次ローパスフィルタ（ｌ−ソフトモータ）部、１４０、２３０……座標変換部、１５０、２４０……３次ローパスフィルタ（Ψ−ソフトモータ）部、１６０、２７０……速度補正用メモリ部、１７０……速度補正／線形補間部、１８０、２８０……インバース（逆変換）部、２５０……曲線長計算部。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a trajectory control apparatus and method for a pulse motor drive system, and a recording medium.
[0002]
[Prior art]
Conventionally, various algorithms (hereinafter referred to as smoothing algorithms) for obtaining a smooth CP (Continuous Path) curve have been proposed for robots and the like that employ servo motors having a servo loop (DC motor, AC motor, etc.). ing.
FIG. 13 is an explanatory diagram showing a configuration of a conventional example of a smoothing algorithm for a servo motor robot.
[0003]
There are two types of smoothing algorithms of this type: a pre-operation calculation method that performs trajectory calculation prior to CP operation and a real-time calculation method that performs trajectory calculation in real time.
The pre-operation calculation method uses a high-speed CPU dedicated to trajectory calculation, calculates all the smoothing (spline interpolation), speed, acceleration, etc. of the curve before the operation, and corrects it according to the motor and mechanical capability limits. In this system, the position / velocity / acceleration result is developed in the memory and the CP operation proceeds while referring to the memory during operation, and a high-speed CPU dedicated to orbit calculation is required.
On the other hand, the real-time calculation method is a method of simultaneously processing the robot operation and smoothing in an ordinary robot that does not have a high-speed CPU dedicated for trajectory calculation as described above, and the smoothing described in this conventional example. The algorithm is based on a real-time calculation method.
[0004]
Next, the configuration of the conventional smoothing algorithm shown in FIG. 13 will be described. This smoothing algorithm includes a CP curve sampling unit 10, a ramp signal generation unit 20, a third-order low-pass filter (1-soft motor) unit 30, a coordinate conversion unit 40, and an inverse (inverse conversion) unit 50. Is done.
The curve sampling unit 10 cuts the curve into a certain element length Δl according to a predetermined CP constant speed Vcp and samples the curve. Here, if the sampling time is Ts, Δl is as follows.
Δl = Vcp · Ts (Subordinate 1)
[0005]
Next, the ramp signal generation unit 20 generates a CP constant speed Vcp. By increasing the movement length by Δl for each sampling time Ts according to the equation (subordinate 1), FIG. A ramp signal as shown is generated. If the total length of CP is L, the lamp will be broken horizontally after Tf time. That is, it is as follows.
Vcp = L / Tf, Tf = N · Ts (N = integer) (subordinate 2)
Although Tf, that is, Vcp is trimmed somewhat so that N is an integer, Ts is generally small enough to cause no problem.
According to such a method, for example, as shown in FIG. 14, it becomes easy to change the speed for each region or to stop the operation on the CP curve.
[0006]
Next, the third-order low-pass filter unit 30 (l-soft motor) generates a smooth linear acceleration / deceleration curve l as shown in FIG. 15B from a ramp signal as shown in FIG. It is. This third-order low-pass filter is a low-pass filter having a structure that is stable at G (s) / 1 + G (s) when the transfer function G (s) = a / s + b / s2 + c / s3.
The smoothness of the filter unit 30 can be freely changed by changing the time constant of the filter unit 30.
[0007]
Next, the coordinate conversion unit 40 converts the curve length l that is the output of the third-order low-pass filter unit 30 into the spatial coordinates Ψ (x, y, z) on the CP curve. When each coordinate of the spatial coordinates Ψ (x, y, z) is displayed as a parameter using l, an arbitrary continuous CP curve can be expressed as follows.
x = x (l), y = y (l), z = z (l) (subordinate 3)
The same applies when the curve is a polygonal line. If it is desired to specify the polar coordinate (R-axis) component, r = r (l) may be added to the above equation (subordinate 3).
Here, l → Ψ (x, y, z) coordinate conversion means obtaining Ψ (x, y, z) from the above equation (subordinate 3). Therefore, in this conversion, no position error occurs in Ψ (x, y, z).
[0008]
Next, the inverse (inverse transform) unit 50 converts the above-described spatial coordinates Ψ (x, y, z) into a joint angle Θ (θx, θy, θz) of the robot. Here, when Ψ = F (Θ), Θ = F^-1· Ψ will be obtained.
[0009]
[Problems to be solved by the invention]
By the way, the smoothing algorithm as described above is for a servo motor robot, but conventionally, a smoothing algorithm as described above has been adopted as it is even in a robot employing a pulse motor.
However, compared with a servo motor robot having a smoothing function (that is, a low-pass function of a servo loop) in the control system itself, the pulse motor robot is driven according to a trajectory command by a pulse train.
Therefore, in the conventional technology in which the algorithm for the servo motor robot is used for such a pulse motor robot, there is no problem if the operation speed is low or the CP curve itself is smooth, but the operation speed is high or the CP curve is When it is a broken line, it is difficult to obtain an appropriate CP operation accuracy, and a great trouble is caused in terms of vibration and step-out of the mechanism portion.
[0010]
For example, as shown in FIG. 16 (A), in a CP curve having an orthogonal corner in an orthogonal robot, as shown in FIGS. 16 (B) and 16 (C), the x and y axis joint velocities Vx and Vy greatly step jump. .
On the other hand, in a servo motor robot having a servo loop, the effect of this step jump is absorbed by the low-pass filter characteristics of the servo, and in most cases does not become an obstacle to the motor or mechanical operation.
On the other hand, in a robot that employs a pulse motor without a servo loop, when the joint speed jump amount increases, that is, the CP speed increases, a mechanical impact occurs, and smooth CP curve operation becomes difficult.
[0011]
SUMMARY OF THE INVENTION Accordingly, an object of the present invention is to provide a trajectory control device and method for a pulse motor drive system and a recording medium capable of obtaining a smooth CP operation at high speed and high accuracy in a drive system using an inexpensive pulse motor. is there.
[0012]
[Means for Solving the Problems]
In order to achieve the above object, the present invention provides a trajectory control device for a pulse motor drive system that controls the CP operation of a control object, and a curve length corresponding to a predetermined CP speed based on the CP operation curve of the control object. Curve length data generating means for generating data, coordinate conversion means for converting the curve length data into n-dimensional (n ≧ 2) coordinate data, and each of n-dimensional coordinate data obtained by the coordinate conversion means, Coordinate data smoothing means for smoothing through a low-pass filter, memory storage means for storing the coordinate data output from the coordinate data smoothing means in a speed correction memory, and coordinate data stored in the speed correction memory for a predetermined amount Readout means that sequentially reads out at the timing, and the coordinate data read out by the readout means as drive signals And having an inverse conversion means for conversion.
[0013]
According to another aspect of the present invention, there is provided a trajectory control method for a pulse motor drive system for controlling a CP operation of an object to be controlled. A long data generation step, a coordinate conversion step for converting the curve length data into n-dimensional (n ≧ 2) coordinate data, and the n-dimensional coordinate data obtained by the coordinate conversion step through low-pass filters, respectively. Smoothing coordinate data smoothing step, memory storing step of storing the coordinate data output from the coordinate data smoothing step in the speed correction memory, and reading out the coordinate data stored in the speed correction memory sequentially at a predetermined timing And an inverse conversion step of converting the coordinate data read out by the readout step into a drive signal Characterized in that it has a.
[0014]
The present invention also provides a function for generating curve length data corresponding to a predetermined CP speed on a computer-readable recording medium based on a CP operation curve of an object to be controlled; 2) the function of converting the coordinate data into the coordinate data, the function of smoothing each of the n-dimensional coordinate data through a low-pass filter, the function of storing the coordinate data passed through the low-pass filter in a speed correction memory, Recording a program for realizing the function of sequentially reading the coordinate data stored in the speed correction memory at a predetermined timing and the function of converting the coordinate data read from the speed correction memory into a drive signal It is characterized by.
[0015]
In the present invention as described above, each n-dimensional coordinate data obtained from the curve length data is smoothed through a low-pass filter. The smoothed coordinate data is temporarily stored in the speed correction memory, and the stored coordinate data is sequentially read out, converted into a drive signal, and output to the pulse motor drive system.
Here, by smoothing each of the n-dimensional coordinate data through a low-pass filter, a smooth operation when driving the pulse motor drive system can be obtained, and mechanical vibration and step-out are prevented. In addition, by transmitting through the speed correction memory, it is possible to drive at a stable speed.
[0016]
DETAILED DESCRIPTION OF THE INVENTION
Embodiments of a trajectory control device for a pulse motor drive system according to the present invention will be described below.
The pulse motor drive system of this example is configured as, for example, a 4-axis small desktop robot, and the trajectory control device that controls the CP operation of the pulse motor robot is configured as an algorithm in a host computer, for example.
Note that the smoothing algorithm of this example employs a real-time calculation method as described above.
[0017]
FIG. 1 is a block diagram showing a first example of a smoothing algorithm by a trajectory control device for a pulse motor robot according to the present invention.
This smoothing algorithm is configured as a smoothing algorithm dedicated to a pulse motor robot capable of avoiding mechanical vibration and step-out corresponding to a joint speed jump, and includes a CP curve sampling unit 110, a ramp signal generation unit 120, and 3 A second-order low-pass filter (1-soft motor) unit 130, a coordinate conversion unit 140, a third-order low-pass filter (Ψ-soft motor) unit 150, a speed correction memory unit 160, a speed correction / linear interpolation unit 170, And an inverse unit 180.
[0018]
The curve sampling unit 110 cuts the curve into a certain element length Δl according to a predetermined CP constant speed Vcp and samples the curve. Here, if the sampling time is Ts, Δl is as follows.
Δl = Vcp · Ts (1)
Next, the ramp signal generation unit 120 generates a CP constant speed Vcp, and increases the movement length by Δl for each sampling time Ts in accordance with the equation (1), so as shown in FIG. Such a ramp signal is generated.
[0019]
If the total length of CP is L, the lamp will be broken horizontally after Tf time. That is, it is as follows.
Vcp = L / Tf, Tf = N · Ts (N = integer) (2)
Although Tf, that is, Vcp is trimmed somewhat so that N is an integer, Ts is generally small enough to cause no problem.
According to such a method, for example, as shown in FIG. 14, it becomes easy to change the speed for each region or to stop the operation on the CP curve.
[0020]
Next, the third-order low-pass filter unit 130 (1-soft motor) generates a smooth linear acceleration / deceleration curve l as shown in FIG. 15B from a ramp signal as shown in FIG. It is. This third-order low-pass filter is a low-pass filter having a structure that is stable at G (s) / 1 + G (s) when the transfer function G (s) = a / s + b / s2 + c / s3.
The smoothness by the filter unit 130 can be freely changed by changing the time constant of the filter unit 130.
[0021]
Next, the coordinate conversion unit 140 converts the curve length l that is the output of the third-order low-pass filter unit 130 into the spatial coordinates Ψ (x, y, z) on the CP curve. When each coordinate of the spatial coordinates Ψ (x, y, z) is displayed as a parameter using l, an arbitrary continuous CP curve can be expressed as follows.
x = x (l), y = y (l), z = z (l) (3)
The same applies when the curve is a polygonal line. If it is desired to specify the polar coordinate (R-axis) component, r = r (l) may be added to the above equation (3).
Here, l → Ψ (x, y, z) coordinate conversion means obtaining Ψ (x, y, z) from the above equation (3). Therefore, in this conversion, no position error occurs in Ψ (x, y, z).
[0022]
Next, the third-order low-pass filter (Ψ-soft motor) unit 150 smoothes the x, y, and z data from the coordinate conversion unit 140 through the third-order low-pass filter.
As described with reference to FIG. 16, each of x, y, and z generated by the coordinate conversion unit 140 is bent in, for example, an orthogonal direction depending on the CP curve, and the respective velocities x ′, y ′, and z ′ jump.
[0023]
In the following description, for each coordinate data x, y, z, the speed is dx / dt = x ′, dy / dt = y ′, dz / dt = z ′, and the acceleration is d.² x / dt² = X ", d² y / dt² = Y ", d² z / dt² = Z ". Also, the data that has passed through the filter once is represented as x *, y *, z *, and the data that has passed through the filter twice is represented as x **, y **, z **. The same applies to symbols other than x, y, and z.
However, in the description in the drawings, the speed is expressed by adding one dot on each symbol, and the acceleration is expressed by adding two dots on each symbol. In addition, data that has passed through the filter once is represented by a single wavy line on each symbol, and data that has passed through the filter twice is represented by having two wavy lines on each symbol. .
[0024]
If the jump described above is directly transmitted to the pulse motor, the stepping out or the mechanism may be greatly shaken, so each of these (x, y, z) is also input to the soft motor and low-passed.
Here, if only the pulse motor step-out and mechanical vibration countermeasures are taken, the joint angle (Θ) which is inversely transformed by the inverse unit 180 is used instead of low-passing (x, y, z)._x , Θ_y , Θ_z ) Is considered to be low-pass, but if this method is used, a large position error (accuracy error) occurs in the CP curve due to the low-pass effect, and this position error is (x, y, z). ) CP trajectory error, which cannot be predicted. The servo motor has this problem.
[0025]
On the other hand, even when smoothing (x, y, z), a position error (accuracy error) occurs, but this is an error that can be controlled by the time constant of the third-order low-pass filter (Ψ-soft motor) unit 150. is there.
Therefore, in this example, (x, y, z) is low-passed to control the CP position accuracy and speed accuracy to smooth the joint angular acceleration and prevent the pulse motor from stepping out and mechanical vibration.
Note that the three low-pass filters x, y, and z in the third-order low-pass filter (Ψ-soft motor) unit 150 are similar to the above-described third-order low-pass filter unit 130, and the transfer function G (s) = a / s + b / The low-pass filter has a structure that is stable at G (s) / 1 + G (s) when s2 + c / s3.
[0026]
The filters of the third-order low-pass filter (Ψ-soft motor) unit 150 have the same characteristics, and the characteristics (pole arrangement) of the filters can be freely tuned as necessary. And
In particular, as shown in FIG. 2, when a step signal is input, a time Tp at which the speed reaches a peak can be freely tuned.
[0027]
As described above, smoothing is a function that softens acceleration by controlling CP position accuracy and velocity accuracy. 3A to 3I are explanatory diagrams showing this situation. In order to simplify the description, an example of a CP curve having orthogonal corners on an xy two-dimensional plane will be described as shown in FIG.
In this case, as shown in FIGS. 3B and 3C, the curve length data of x and y are both ramp waveforms because the CP speed is constant, and the x and y speeds x ′ and y ′ are shown in FIG. D) Both become step functions as shown in (E). Then, by low-passing these, x ′ * and y ′ * become a smooth curve with a constant time constant as shown in FIGS.
[0028]
As shown in FIG. 3H, the peak speed arrival time of the soft motor is Tp at the CP speed l ′ obtained by combining these. Here, the CP speed l 'of the soft motor output is l' * = {(x '*)² + (Y ’*)² }^1/2 Since l ′ is not constant, it falls in a wedge shape.
Further, as shown in FIG. 3I, the smoothed CP curve (x *, y *) runs away from the designated locus inside the corner portion.
[0029]
In the CP operation of the robot, position accuracy and speed accuracy are important, but it is impossible in terms of dynamics to run both ideally. For this reason, in this algorithm, tuning (polar arrangement) of a soft motor as shown in FIG. 4 is used as a smoothing tuning mechanism.
On the other hand, a speed correction memory unit 160 and a speed correction / linear interpolation unit 170 are used as a speed tuning mechanism. The operations of the speed correction memory unit 160 and the speed correction / linear interpolation unit 170 will be described below with reference to FIG. Note that the sampling time Ts, the CP speed Vcp, and the time constants of various soft motors are already determined.
[0030]
In the CP speed correction algorithm of the present example, the spatial coordinates Ψ (x, y, z) output from the coordinate conversion unit 140 described above are input to the third-order low-pass filter (Ψ-soft motor) unit 150 at the same time. The output (x *, y *, z *) of the low-pass filter unit 150 is stored in the speed correction memory unit 160 at every sampling timing.
Therefore, the speed correction memory unit 160 includes P₁ (X *, y *, z *) → P₂ (X *, y *, z *) → P_Three (X *, y *, z *), ..., P_N Stored in the form of (x *, y *, z *).
Note that the number N of pointers on the speed correction memory unit 160 is prepared for the time Tp. That is, Tp / Ts = N.
[0031]
Next, CP speed correction will be described.
Generally, there are many cases (for example, dispenser work, soldering, coating work, etc.) where it is desired to run the robot at a constant CP speed Vcp in an application for robot CP operation.
However, as shown in FIG. 5, the CP speed Vcp * by the output of the third-order low-pass filter (Ψ-soft motor) unit 150 is Vcp * = {(x ′ *)² + (Y ’*)² }^1/2 It is not constant and changes to a wedge shape.
That is, (x ’*² + Y ′ *) is constant, but {(x ′ *)² + (Y ’*)² }^1/2 Is not constant.
Therefore, in this example, the deceleration start time t = t₁ Thereafter, the following processing is performed to correct the speed Vcp *.
[0032]
First, t = t₁ The output of Ψ-soft motor at P is P₁ (X₁ *, Y₁ *, Z₁ *) Is stored. Thereafter, P every sampling time Ts.₂ , P_Three , P_Four Suppose that it is stored as.
Where t = t₁ Position data P at + Ts₂ (X₂ *, Y₂ *, Z₂ *) Is searched in the speed correction memory using the following formula.
△ l = {P₂ ² -P₁ ² }^1/2             ...... (4)
However, P₂ ² -P₁ ² ≒ (x₂ * -X₁ *) + (Y₂ * -Y₁ *) + (Z₂ * -Z₁ *). ...... (5)
[0033]
P that strictly satisfies the above equation (4)₂ Generally does not exist in memory. Therefore, first, the point P that becomes Δl or more_i Search for. That means
{P_i ² -P₁ ² }^1/2 ≧ △ l (6)
Where P_i And P_i-1 P that satisfies the above equation (4) by linear interpolation between₂ Is determined by linear interpolation calculation.
[0034]
Specifically, for example, in FIG.
Line length <P_i-1 ~ P_i > = △ l_i               ...... (7)
So P_i-1 To δl_i Point P at a distance of_i Suppose that Δl is exactly Δl. δl_i Is a known value, so δl_i = K ・ △ l_i From K = δl_i / △ l_i K is determined. This

P_i ´ (x_i * ', Y_i * ', Z_i * ') Can be determined.
[0035]
Thereafter, this P_i Starting from ′, the point where the distance from here is Δl is determined in the same manner. In this algorithm, the position data on the memory is already smoothed, and linear interpolation is performed based on this position so that the CP speed Vcp is constant. Therefore, as shown in FIG. Each will be as expected.
However, in the above algorithm, when the sampling time Ts is large due to the CPU capacity or the like, each joint speed jumps, and problems such as mechanical vibration and step-out remain as it is.
[0036]
Therefore, in this example, a method of executing only predetermined P% (<100%) instead of performing CP speed correction 100% is adopted. Note that such processing is not necessary when the sampling time Ts is sufficiently small.
In the 100% correction, a point at which the movement distance between Ts becomes Δl = Vcp · Ts was determined by linear interpolation based on the point data on the memory.
Here, Δl (n) is a distance between t = nTs and t = (n−1) Ts. That means
Δl (n) = {P_n ² -P_n-1 ² }^1/2 ≦ △ l …… (9)
[0037]
Next, Δl ′ (n) is a distance between t = nTs and t = (n−1) Ts in the case of P% correction. At this time,

Thereby, Δl ′ (n) is determined. Thereafter, the memory search and linear interpolation are processed in the same manner as in the case of 100% correction, and as shown in FIG. 8, CP speed and P% correction can be performed, each joint speed and jump amount can be reduced, and mechanical vibrations can be reduced. Can be kept small. Note that the value of P% can be appropriately input by the user using a robot language, a teaching pendant, or the like.
[0038]
Next, based on the correction data (x, y, z) obtained as described above, the inverse (inverse transformation) unit 180 converts the robot joint angle Θ (θx, θy, θz) into a subsequent stage. To the pulse board.
[0039]
FIG. 9 is a block diagram showing a second example of the smoothing algorithm by the trajectory control device of the pulse motor robot according to the present invention.
In the speed correction method according to the first example described above, as can be seen from FIG. 8, at the corrected speed, the speed is not smooth at t = Tp / 2. That is, although the CP acceleration may be slight, it jumps. This jump (discontinuity) results in a jump at each joint speed, which is undesirable in terms of mechanical vibration and step-out. Therefore, in the second example of the present invention, a configuration corresponding to such a problem will be described. This smoothing algorithm can be used together with the smoothing algorithm according to the first example described above.
[0040]
This smoothing algorithm includes a CP curve sampling unit 210, a ramp signal generation unit 220, a coordinate conversion unit 230, a third order low pass filter (Ψ-soft motor) unit 240, a curve length calculation unit 250, and a third order low pass. A filter (l-soft motor) unit 260, a speed correction memory unit 270, and an inverse (inverse conversion) unit 280 are included.
Among them, the CP curve sampling unit 210 and the ramp signal generation unit 220 are common to the sampling unit 110 and the ramp signal generation 120 shown in FIG.
[0041]
The coordinate conversion unit 230 has the same function as that of the coordinate conversion unit 140 shown in FIG. 1, and the curve length l output from the ramp signal generation unit 220 is converted into the spatial coordinates ψ (x, y, z). To convert.
The third-order low-pass filter (Ψ-soft motor) unit 240 is the same as the third-order low-pass filter unit 150 shown in FIG. 1, and smoothes the coordinate data x, y, z from the coordinate conversion unit 230 through the low-pass filter. Is. In addition, the time constant in the soft motor of each axis x, y, z is the same.
[0042]
The curve length calculation unit 250 calculates the CP curve length l * subjected to the low pass based on the output data x *, y *, z * of the third order low pass filter unit 150. That is, l * = {x *² + Y *² + Z *² }^1/2 Calculate
Further, in the speed correction memory unit 270, the above l * and (x *, y *, z *) are set to P in order for each sampling time._n It is stored in the form of (l *, x *, y *, z *).
[0043]
The third order low pass filter (l-soft motor) unit 260 smoothes the CP curve length l * output from the curve length calculation unit 250 through the third order low pass filter. The output of the third-order low-pass filter unit 260 is assumed to be l **.
As shown in FIG. 10, l * 'sharply drops in the shape of a wedge, and inconvenience is expected in the dispenser operation and the like. Therefore, the speed is also smoothed by further smoothing l * by the third-order low-pass filter unit 260.
As can be seen from FIG. 10, the CP acceleration in which the change of l * ′ is smoothly smoothed and continuous l ** ′ are obtained. Note that the correction works more strongly when the F characteristic of the l-soft motor is reduced as shown in FIG.
[0044]
The speed correction memory unit 270 is configured to store the P_n (L *, x *, y *, z *) are sequentially stored, and (x *, y *, z *) is stored based on l ** from the third-order low-pass filter unit 260 corresponding to the sampling time. It has a function to search and output sequentially. Hereinafter, this procedure will be described.
First, l ** is the length of the curve smoothed by the l-soft motor, and the point (x *, y *, z *) on the CP curve corresponding to this l ** is searched from the memory. Ask.
In memory, P_n Since it is stored in the form of (l *, x *, y *, z *), P such that l * = l **_n Search for this P_n (X *, y *, z *) is sent to the inverse unit 280 in the next stage.
[0045]
This prevents a position error from entering as a CP curve between l ** smoothed by the l-soft motor and l * smoothed by the Ψ-soft motor.
As described above, P such that l * = l **_n If the search is not found, the linear interpolation method used in FIG. 1 is applied.
Further, the inverse unit 280 calculates the joint angle Θ (θx, θy, θz) of the robot from the correction data (x *, y *, z *) obtained in the speed correction memory unit 270, and the subsequent pulse. Send to board.
[0046]
Next, details of how to use the speed correction memory units 160 and 270 in the above examples will be described in order.
First, at the start of CP curve calculation, the soft motor output is sequentially stored in the memory. At this stage, the robot is not operating yet.
Next, since the memory has a Tp / Tss storage area, the CP operation of the robot is actually started when the memory becomes full. That is, the start data of the memory is converted and started to be sent to the pulse board.
Thereafter, the soft motor output is stored in the memory. However, since only the memory capacity Tp / Tss is prepared, the memory data before Tp is discarded in order every time one data is stored. Therefore, it is effective that this memory is a ring buffer.
[0047]
Next, using this memory, a bird to perform speed correction becomes the jump amount of the speed of each axis, that is, any value of the command acceleration (x ″, y ″, z ″) is given in advance. Acceleration threshold (x_S ", Y_S ", Z_S Therefore, if the CP curve does not exceed the threshold value, this speed correction is not executed.
The acceleration threshold value (x_S ", Y_S ", Z_S ”) Is an amount that depends on the mechanical structure, and therefore needs to be determined in advance through experiments, etc. Also, during acceleration (0 ≦ t ≦ Tp) and during deceleration (Tf ≦ t ≦ Tf + Tp) No correction is applied.
[0048]
If the acceleration jump continues within the Tp time, the speed during Tp is corrected. However, the speed correction of the part after Tp is not applied. Since these cases are few, it is not always necessary to deal with them. However, if they are to be dealt with, it is possible to use a method in which the memory is made a ring buffer structure and correction is always applied. This method is particularly effective when the calculation burden on the CPU is not heavy.
[0049]
Next, an application example related to the smoothing algorithm for the CP curve dedicated to the pulse motor as described above will be described.
First, the pulse motor step-out prevention algorithm will be described.
The robot designer does not know what kind of CP curve the robot user will run. On the other hand, CP maximum speed Vcp so that the pulse motor will not step out for any shape of the CP curve._max Can be designed quite low, but this is a conservative design, which reduces the robot's ability.
Therefore, in such a case, when the user runs at the desired CP speed and steps out, the step-out can be prevented by appropriately increasing the time constants of the three ψ-soft motors. Of course, in this case, the deviation of the CP trajectory increases, but it is effective as an option for preventing the step-out. It should be noted that the three ψ-soft motor time constants can be changed by the user from the robot language or teaching pendant.
[0050]
In addition, when the system for allowing the user to change the soft motor time constant as described above is inevitably desirable, the automatic calculation can be prevented by using the pre-calculation method. The procedure outline is shown below.
First, the process shown in FIG. 1 is performed without operating the robot until the end of the CP curve (pre-calculation).
Next, (Θx ′, Θy ′, Θz ′) and (Θx ″, Θy ″, Θz ″) of each axis at the time of the previous calculation are stored in the memory for each sampling time.
Using this memory data, as shown in FIG. 11, a TN characteristic is drawn for each axis to find an axis protruding from the TN characteristic. When a plurality of axes protrudes together, the TN locus of the axis with a large protruding ratio is drawn and viewed.
[0051]
Next, a point (Θp ″, Θp ′) at which the acceleration reaches a peak in the TN locus is found. Here, the acceleration on the TN characteristic at the velocity Θp ′ is expressed as Θ.₁ ”, Α = (Θp” / Θ₁ ”)^1/2 If α is calculated and the time constant of ψ-soft motor is set to α times or more, step-out can be prevented.
This is because (x, y, z) If the time constant of the soft motor is multiplied by α, the acceleration will be 1 / α.² Θp ”is Θ₁ This is because it falls within the TN characteristics.
[0052]
Next, how the CP trajectory changes at the CP speed Vcp when the above-described smoothing algorithm is used will be described. Since there are an infinite number of CP curves, the case of a right angle corner by the above-described orthogonal robot will be described.
The speed peak arrival time, which is one of the time constants of ψ-soft motor, is Tp. Then, the distance Δl (Tp) on the CP curve running between Tp is Δl (Tp) ≈Vcp / Tp, and is approximately proportional to the CP speed Vcp as shown in FIG.
That is, the larger the Vcp, the larger the deviation from the corner, but the smaller the Vcp, the smaller the deviation. Then, the user can adjust the deviation by tuning Vcp.
[0053]
Further, the above-described smoothing algorithm is a considerable processing burden for the host CPU. Of these, the l-soft motor 130 is a low-pass filter that smoothly generates acceleration / deceleration of the CP speed. However, since the subsequent ψ-soft motor 150 also has the same role, if the load on the CPU is large, l -The soft motor 130 may be omitted.
Further, in each of the above-described examples, the configuration for controlling the pulse motor robot as the control object has been described. However, the present invention can be widely applied as a configuration for controlling a drive system other than the robot.
[0054]
【The invention's effect】
As described above, in the present invention, each of the n-dimensional coordinate data obtained from the curve length data is smoothed through the low-pass filter, and the smoothed coordinate data is temporarily stored in the speed correction memory. The stored coordinate data was sequentially read out and converted into a drive signal and output to the pulse motor drive system.
Therefore, according to the present invention, in an inexpensive pulse motor drive system, a smooth orbit that is equal to or higher than that of the servo motor drive system can be obtained for a high-speed CP operation or a polygonal CP orbit, and a stable operation is achieved. There is an effect that speed can be easily obtained.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a first example of a smoothing algorithm by a trajectory control device for a pulse motor robot according to the present invention.
FIG. 2 is an explanatory diagram for explaining the operation of a ψ soft motor in the algorithm shown in FIG. 1;
FIG. 3 is an explanatory diagram illustrating a smoothing curve generation process for a straight corner portion of a CP curve in the algorithm illustrated in FIG. 1;
4 is an explanatory diagram for explaining the relationship between the characteristics of a ψ soft motor and the CP accuracy in the algorithm shown in FIG. 1; FIG.
FIG. 5 is an explanatory diagram for explaining the effect of velocity smoothing in the algorithm shown in FIG. 1;
6 is an explanatory diagram for explaining a method of interpolating velocity smoothing data in the algorithm shown in FIG. 1; FIG.
7 is an explanatory diagram for explaining the effect of interpolation of velocity smoothing data in the algorithm shown in FIG. 1; FIG.
FIG. 8 is an explanatory diagram for explaining the operation when the speed correction amount of the speed smoothing data in the algorithm shown in FIG. 1 is controlled.
FIG. 9 is a block diagram showing a second example of the smoothing algorithm by the trajectory control device of the pulse motor robot according to the present invention.
10 is an explanatory diagram showing a relationship between frequency characteristics of a ψ soft motor and speed smoothing data in the algorithm shown in FIG. 9;
11 is an explanatory diagram showing an example of TN characteristics and acceleration for preventing step-out in the algorithm shown in FIGS. 1 and 9. FIG.
12 is an explanatory diagram showing a relationship between a CP speed and a CP trajectory in the algorithm shown in FIGS. 1 and 9. FIG.
FIG. 13 is an explanatory diagram showing a configuration of a conventional example of a smoothing algorithm of a servo motor robot.
14 is an explanatory diagram showing a ramp signal when the CP speed is changed in the algorithms shown in FIGS. 1, 9, and 13. FIG.
FIG. 15 is an explanatory diagram showing a waveform example when a ramp signal is smoothed by a low-pass filter.
FIG. 16 is an explanatory diagram showing a variation in CP speed in a CP curve having a right angle corner.
[Explanation of symbols]
110, 210... Sampling unit, 120, 220... Ramp signal generation unit, 130, 260... Third-order low-pass filter (1-soft motor) unit, 140, 230... Coordinate conversion unit, 150, 240. Next-order low-pass filter (Ψ-soft motor) unit, 160, 270... Speed correction memory unit, 170... Speed correction / linear interpolation unit, 180, 280. Department.

Claims (11)

In the trajectory control device of the pulse motor drive system that controls the CP operation of the controlled object,
Curve length data generating means for generating curve length data corresponding to a predetermined CP speed based on the CP operation curve of the control object;
Coordinate conversion means for converting the curve length data into n-dimensional (n ≧ 2) coordinate data;
Coordinate data smoothing means for smoothing each n-dimensional coordinate data obtained by the coordinate conversion means through a low-pass filter,
Memory storage means for storing the coordinate data output from the coordinate data smoothing means in a speed correction memory;
Reading means for sequentially reading the coordinate data stored in the speed correction memory at a predetermined timing;
Inverse conversion means for converting the coordinate data read by the reading means into a drive signal;
A trajectory control device for a pulse motor drive system. The curve length data generation means includes sampling means for sampling the CP operation curve at a predetermined interval, and ramp signal generation means for generating a ramp signal based on line segment length data obtained by the sampling means. 2. A trajectory control device for a pulse motor drive system according to claim 1, wherein a ramp signal is input to the coordinate conversion means as the curve length data. Curve length data smoothing means for inputting curve length data from the curve length data generation means to the coordinate conversion means through a low-pass filter between the curve length data generation means and the coordinate conversion means. The trajectory control device for a pulse motor drive system according to claim 1. Curve length data calculating means for calculating a curve length from output data of the coordinate data smoothing means, and curve length data smoothing means for smoothing the curve length data from the curve length data calculating means through a low-pass filter,
The memory storage means stores the curve length data from the curve length data calculation means and the coordinate data output from the coordinate data smoothing means in association with each other in the speed correction memory,
The said reading means compares the output of the said curve length data smoothing means with the coordinate data stored in the said memory for speed correction, The said coordinate data are read based on this comparison result, Orbit control device for pulse motor drive system. 2. The pulse motor drive according to claim 1, further comprising a correcting unit that performs processing for correcting a decrease in driving speed due to smoothing on the coordinate data read by the reading unit and inputs the processed data to the inverse converting unit. System orbit control device. The trajectory of the pulse motor drive system according to claim 1, wherein the pulse motor drive system is a drive system of at least a three-axis joint robot, and the n-dimensional coordinate data is at least three-dimensional spatial coordinate data. Control device. The trajectory control device for a pulse motor drive system according to claim 6, wherein the drive signal is a joint angle signal of a robot. The trajectory control device for a pulse motor drive system according to claim 1, further comprising changing means for changing filter characteristics of the low-pass filter. 2. The trajectory control device for a pulse motor drive system according to claim 1, wherein said filter characteristic changing means is means for changing a time constant of an output of a low-pass filter. In the trajectory control method of the pulse motor drive system for controlling the CP operation of the controlled object,
A curve length data generating step for generating curve length data corresponding to a predetermined CP speed based on the CP operation curve of the control object;
A coordinate conversion step of converting the curve length data into n-dimensional (n ≧ 2) coordinate data;
Coordinate data smoothing step of smoothing each n-dimensional coordinate data obtained by the coordinate conversion step through a low-pass filter,
A memory storing step of storing the coordinate data output from the coordinate data smoothing step in a speed correction memory;
A reading step of sequentially reading out the coordinate data stored in the speed correction memory at a predetermined timing;
An inverse conversion step of converting the coordinate data read by the reading step into a drive signal;
A trajectory control method for a pulse motor drive system. A function of generating curve length data corresponding to a predetermined CP speed based on the CP operation curve of the control object;
A function of converting the curve length data into n-dimensional (n ≧ 2) coordinate data;
A function of smoothing each of the n-dimensional coordinate data through low-pass filters,
A function of storing coordinate data that has passed through the low-pass filter in a speed correction memory;
A function of sequentially reading the coordinate data stored in the speed correction memory at a predetermined timing;
A function of converting the coordinate data read from the speed correction memory into a drive signal;
A computer-readable recording medium on which a program for realizing is recorded.

Images