(57)【要約】【課題】補間に起因する振動を抑制できるサーボ装置の位置指令補間方法を提供する。【解決手段】上位位置指令の今回値と前回値と前々回値と（Ｓ１０）、周期の逐次計算における区分軌道終端の高次微分値（Ｓ１１）より、前々回値から前回値までを補間する区分補間軌道を決定する方法で、前々回値と前回値が等しい時は、区分補間軌道を一定軌道（Ｓ１５）に、前々回値と前回値が異なり今回値と前回値の差分と前回値と前々回値の差分の積の符号が負なら、区分補間軌道を前回値に滑かに停止する停止軌道（Ｓ１７）に、前々回値と前回値が異なり且つ今回値と前回値との差分と前回値と前々回値との差分の積の符号が正なら区分補間軌道を振動を小さくする振動抑制軌道（Ｓ１６）にする。【選択図】　　　図１
2004139531.tif 000002
【特許請求の範囲】
【請求項１】
上位装置から一定周期で上位位置指令を取得し、前記上位位置指令を満たす区分補間軌道を前記周期毎に逐次計算するサーボ装置の位置指令補間方法において、
前記上位位置指令の今回値と前回値と前々回値と、前記周期の逐次計算における区分軌道終端の高次微分値とから、前々回値から前回値までを補間する前記区分補間軌道を決定する方法であって、
前記前々回値と前記前回値が等しい時は、前記区分補間軌道を一定軌道とし、
前記前々回値と前記前回値が等しく無く且つ前記今回値と前記前回値との差分と前記前回値と前記前々回値の差分の積の符号が負のときには、前記区分補間軌道を前記前回値において滑らかに停止する停止軌道とし、
前記前々回値と前記前回値が等しくなく且つ前記今回値と前記前回値との差分と前記前回値と前記前々回値との差分の積の符号が正のときは、前記区分補間軌道を振動を小さくする振動抑制軌道とすることを特徴とするサーボ装置の位置指令補間方法。
【請求項２】
前記区分補間軌道において、前記停止軌道がオーバーシュートするとき、前記停止軌道の停止までの時間をオーバーシュートがなくなるまで順次短くし停止後は一定値とすることで前記停止軌道を決定することを特徴とする請求項１に記載のサーボ装置の位置指令補間方法。
【請求項３】
前記区分補間軌道および高次微分値を、フィードフォワードとして用いることを特徴とする請求項１又は２のいずれか１項記載のサーボ装置の位置指令補間方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、工作機、ロボット・マニピュレータなどにおいて、上位装置から一定周期で上位位置指令を取得し追従制御を行うサーボ制御装置、特に上位装置の周期よりも短いサーボ周期で上位位置指令を補間するサーボ装置の位置指令補間方法に関するものである。
【０００２】
【従来の技術】
上位装置から一定周期で位置指令を取得し、追従制御を行うサーボ装置においては、上位装置の演算時間や上位装置とサーボ装置の通信速度などの制限から、上位装置からの指令周期がサーボ周期に比べて長い場合が多く、サーボ装置で位置指令を補間してサーボ周期毎の指令値を作成することが行われている。
例えば、位置指令を補間する方法として、図７に示す特許文献１に開示の「モーションコントローラ」には、今回ａ、前回ｂ、前々回ｃと位置指令を３周期分取得して、今回ａと前回ｂの指令速度の変化を、反転の有無の判断等により判定して、３つの位置を通る放物線による２次補間と、前回値と今回値を通る１次補間とを切替える方法が記載されている。
【０００３】
この指令速度の変化の判定について説明する。
通常、二次式では導関数が一次式となり、単調増加関数若しくは単調減少関数であるから、ある区間の両端で導関数の符号が反転しているときは、区間の中間で補間関数値が両端の上限指令値を超えてオーバーシュートしている状態である。そこで、符号反転が起こるときは、２次補間を用いずに１次補間（又は一次内挿演算）を用いて補間することで、オーバーシュートを含まない内挿軌道データを発生できる。なお、符号反転は定速状態や一定加速・一定減速状態では発生せず、起動当初や停止直前等で発生しやすい。
具体的には、図７で前回の制御周期を時間ｔ＝０とし、今回の制御周期を時間ｔ＝１とすると、今回の制御周期で、内挿演算すべき時間は０＜ｔ＜１で表される。今回ａ、前回ｂ、前々回ｃとして、位置Ｐを時間ｔの二次式で表せば次式が成立する。
Ｐ＝（ａ−２ｂ＋ｃ）ｔ２／２＋（ａ−ｃ）ｔ／２＋ｂ…（０＜ｔ＜１）
この式の導関数は次式で表される。
Ｐ´＝（ａ−２ｂ＋ｃ）ｔ＋（ａ−ｃ）／２…（０＜ｔ＜１）
即ち、速度反転の判断は、今回の制御周期の最初である（ａ−ｃ）／２…（ｔ＝０の時）と、（３ａ−４ｂ＋ｃ）／２…（ｔ＝１の時）の符号が反転しているか否かを調べればよい。
【０００４】
図８は図７に示した今回ａ、前回ｂ、前々回ｃの３点の位置指令を用いてａ、ｂ間を内挿補間する方法の処理のフローチャートであり、先ず、（ａ−ｃ）／２…（ｔ＝０の時）をＸレジスタに入れる（Ｓ８１）。次に、（３ａ−４ｂ＋ｃ）／２…（ｔ＝１の時）をＹレジスタに入れる（Ｓ８２）。そしてＸとＹの符号を比較して（Ｓ８３）、等しければ２次補間を行い（Ｓ８４）、異なれば１次補間を行う（Ｓ８５）。
この場合の１次補間は、上位位置指令の制御周期を１６ｍｓ、内挿周期を１／１６の１ｍｓとした場合、制御周期内の内挿周期番号ｉを０〜１５として、一次式による次の内挿演算を行うものである。
Ｐ＝（ａ−ｂ）（ｉ／１６）＋ｂ…（０＜ｉ＜１７の自然数）
また、２次補間は、１次補間と同じ内挿周期番号ｉ＝０〜１５に従って、二次式による次の内挿演算を行う。
Ｐ＝（ａ−２ｂ＋ｃ）（ｉ／１６）２／２＋（ａ−ｃ）（ｉ／１６）／２＋ｂ
【０００５】
こうして上位指令周期の間、サーボ周期毎に補間位置を出力した後、ａとｂとｃとを更新して以上の手順を繰り返す。
以上により、上位位置指令の速度がｂ点と、ａ点とで同方向である時は、放物線を用いて速度までが滑らかな２次補間を、上位位置指令の速度が反転している時は、１次補間を用いて補間位置が上位位置指令の間からオーバーシュートしない軌道を得ることができている。
【０００６】
【特許文献１】
特開平７−２１９６１７号公報、（第４頁、図２、図３）
【０００７】
【発明が解決しようとする課題】
しかしながら、上記従来の技術では、１次補間を用いたとき補間軌道が区間接続点において位置のみが連続で速度以上の高次微分は不連続となるため、軌道の不連続性が外乱となってサーボ追従精度を悪化させてしまう問題があり、特に、位置決め制御に関しては上位指令が一定値となる区間の初期速度が零でないことにより、整定性能が悪くなる。
また、軌道の区分補間軌道内の連続性も高々２次であるため、位置指令の高次微分をフィードフォワードに利用してサーボ追従精度を高めることができない。かと言って、区分補間軌道を単純に高次多項式にして、多周期分の位置指令を満たすように決定すると軌道が振動的になることが知られている、等の問題があった。
【０００８】
そこで、本発明は、区間接続点および起動・停止時においても高次の微分が連続で、補間に起因する振動や位置決め停止時の振動・オーバーシュートを起こさないサーボ装置の位置補間方法を提供することを目的としている。
【０００９】
【課題を解決するための手段】
上記目的を達成するため、請求事項１に記載の発明は、上位装置から一定周期で上位位置指令を取得し、前記上位位置指令を満たす区分補間軌道を前記周期毎に逐次計算するサーボ装置の位置指令補間方法において、前記上位位置指令の今回値と前回値と前々回値と、前記周期の逐次計算における区分軌道終端の高次微分値とから、前々回値から前回値までを補間する前記区分補間軌道を決定する方法であって、前記前々回値と前記前回値が等しい時は、前記区分補間軌道を一定軌道とし、前記前々回値と前記前回値が等しく無く且つ前記今回値と前記前回値との差分と前記前回値と前記前々回値の差分の積の符号が負のときには、前記区分補間軌道を前記前回値において滑らかに停止する停止軌道とし、前記前々回値と前記前回値が等しくなく且つ前記今回値と前記前回値との差分と前記前回値と前記前々回値との差分の積の符号が正のときは、前記区分補間軌道を振動を小さくする振動抑制軌道とすることを特徴としている。
【００１０】
このサーボ装置の位置指令補間方法によれば、前回周期の逐次計算における区分軌道終端の高次微分値を用い、連続である条件から逐次軌道を生成することにより高次微分が連続な軌道を生成することができる。
また、軌道の選択によって、上位位置指令が一定値となる時は、その区間の軌道は停止軌道となり、他方、上位位置指令が一定値となる一つ前の区間の軌道は停止軌道となり滑らかに停止していることから、位置指令が一定となる区間を上位位置指令と完全に一致させることができる。
また、位置指令の差分が反転する前の区間でも停止軌道となるため、オーバシュートしない指令を生成することができる。
また、位置指令の差分が反転せず一方向に動作中のときは、３つの位置を通り振動を小さくする条件を用いて軌道を決定することにより、補間方法に起因する振動を抑えることができる。
【００１１】
また、請求項２に記載の発明は、前記区分補間軌道において、前記停止軌道がオーバーシュートするとき、前記停止軌道の停止までの時間をオーバーシュートがなくなるまで順次短くし停止後は一定値とすることで前記停止軌道を決定することを特徴とする。
このサーボ装置の位置指令補間方法によれば、停止軌道の停止までの時間を短くすることにより、位置決め停止時に補間に起因するオーバシュートを無くすことができる。
また、請求項３に記載の発明は、前記区分補間軌道および高次微分値を、フィードフォワードに用いることを特徴としている。
このサーボ装置の位置指令補間方法によれば、補間軌道は区間接続点を含めて全ての軌道の高次微分が連続であるため、高次微分を利用したフィードフォワードによりサーボ装置の追従精度を高めることができる。
【００１２】
【発明の実施の形態】
以下、本発明の実施の形態について図を参照して説明する。
図１は本発明の実施の形態に係るサーボ装置の位置指令補間方法のフローチャートである。
図２は本発明の区分補間軌道を示す概念図である。
図３は本発明の区分補間軌道の逐次計算を示す概念図である。
本発明は、図２に示すように、上位装置から一定周期で取得した上位位置指令の今回値ｘｋと、前回値ｘｋ−１と、前々回値ｘｋ−２とに基づいて、前々回値から前回値までを区分的に補間するものである。
【００１３】
以下、実際の処理について図１のフローチャートに従って説明する。
先ず、上位装置からの上位位置指令を周期的に取込んで今回値ｘｋと前回値ｘｋ−１と、前々回値ｘｋ−２とを取得し（Ｓ１０）、前回周期の逐次計算における区分軌道終端の高次微分値をメモリあるいは初期値から取得する（Ｓ１１）。　次に、当該区間の補間方法の判定を行う。
【００１４】
前回値ｘｋ−１と前々回値ｘｋ−２とが等しいかどうか判定し（Ｓ１２）、ｘｋ−１＝ｘｋ−２、の時は、区分補間軌道を一定値ｘｋ−２の一定軌道であるとする（Ｓ１５）。
ｘｋ−１＝ｘｋ−２、でない時は、次に、今回値ｘｋと前回値ｘｋ−１とが等しいかどうかを判定し（Ｓ１３）、ｘｋ＝ｘｋ−１のときは、区分補間軌道をｘｋ−１において軌道の高次微分が零で滑らかに停止する停止軌道であるとする（Ｓ１７）。Ｓ１３の判定でｘｋ＝ｘｋ−１でない時は、次に上位位置指令差分の符号の変化を判定し、（ｘｋ−ｘｋ−１）（ｘｋ−１−ｘｋ−２）＜０の時は速度が反転しているため、区分補間軌道は停止軌道であるとする（Ｓ１４）。
【００１５】
以上のどれでもないときは、上位位置指令は同一方向へ移動中であり、区分補間関数を振動抑制軌道とする（Ｓ１６）。
以上のように、一定軌道、停止軌道、振動抑制軌道など、決定された区分補間軌道を用いて、上位位置指令をサーボ周期毎に内挿補間し、区分補間軌道を目標値としてサーボ装置はフィードバック制御を行う。
ここで、サーボ装置は、区分補間軌道およびその高次微分値を指令追従性能を向上させるためにフィードフォワードとして用いてもよい。
また、サーボ装置は区分補間軌道およびその高次微分値を制御対象のダイナミクス（積分器等）に代入して入力トルクを算出し、そのトルクをフィードフォワードしてもよい。
【００１６】
図３は本発明の区分補間軌道の逐次計算を示す概念図である。区分補間軌道がＮ階微分まで連続であるとしたとき、一定周期毎に取得される上位位置指令３１と、前回区間の補間結果により更新された軌道ｘの終端速度、加速度、、、Ｎ階微分を境界条件として軌道を区分的に補間する。軌道ｘの速度、加速度、、、Ｎ階微分の初期値３２は全て零に初期化される。
次に、ｘ０とｘ１とｘ２と初期値３２とから、ｘ０とｘ１との間を補間する軌道３３が決定される。この軌道の終端条件から、ｘ１における軌道ｘの速度、加速度、、、Ｎ階微分が算出できる。
次に、これら状態量の初期条件とｘ２とｘ３とから、ｘ１とｘ２との間を補間する軌道３４が決定される。以下、この手順を繰り返すことにより、Ｎ階微分まで連続な区分補間軌道を逐次計算していくことができる。ここで、区分補間関数を２Ｎ＋１次の時間多項式としてもよい。２Ｎ＋１次の時間多項式を用いるとＮ次まで連続な区分関数を容易に計算することができる。
【００１７】
図４は図１のＳ１６で算出される振動抑制軌道の決定条件を示す図である。ｘｋ−２と、ｘｋ−１との間の区分補間軌道を決定する条件は、上位位置指令のｘｋ、ｘｋ−１、ｘｋ−２と前回区間の補間結果から得られるｘｋ−２の１次からＮ次微分と、ｘｋ−２からｘｋにかけての振動を抑制する条件である。
補間関数を２Ｎ＋１次多項式とした場合は、多項式の２Ｎ＋２個の係数（例えば、係数ａ０〜ａＮ、ｂ０〜ｂＮ等）を決定するに当たり、上位指令３個と微分値の連続条件Ｎ個であるから、２Ｎ＋２−（３＋Ｎ）の自由度を振動抑制条件により決定する。
なお、振動抑制条件の１例としてｘｋ−２からｘｋまでの加速度の二乗積分値を最小とする条件などを用いて決定することができる。この時の最小条件は予め計算しておくことが可能で、実行時の計算負荷を減少させることができる。
【００１８】
図５は図１のＳ１７における停止軌道の決定条件を示す図である。停止条件の１例として、ｘｋ−１におけるＮ階微分までが零である条件を用いると、上位位置指令ｘｋ−２とｘｋ−１と前回までの逐次補間結果により求められたｘｋ−２における０からＮ階微分と、ｘｋ−１における０からＮ階微分の２Ｎ＋２個の係数を２Ｎ＋２個の条件を満たすように決定する。
補間関数を２Ｎ＋１次多項式とすると、多項式の２Ｎ＋２個の係数を２Ｎ＋２個の条件式で一意に解くことができる。
しかし、この条件で解くとｘｋ−２の速度等の組み合わせによっては図６（ａ）の６１に示すように区分補間軌道がオーバーシュートしてしまう場合がある。この場合は、停止する時間を上位指令の周期よりも短くして、図６（ｂ）にひし形で示した６２の位置で停止する条件とすることによって、６３のようにオーバシュートのない停止軌道を作成することができる。
【００１９】
【発明の効果】
以上説明したように、請求項１に記載の発明によれば、前回周期の逐次計算における区分軌道終端の高次微分値を用いて、連続である条件から逐次軌道を生成することにより高次微分が連続な軌道を生成することが可能となり、軌道の選択によって、上位位置指令が一定値となる時は、その区間の補間軌道も一定値となり、他方、上位位置指令が一定値となる一つ前の区間の軌道は停止軌道となり滑らかに停止していることから、位置指令が一定となる区間を上位位置指令と完全に一致させることができる。更に、位置指令の差分が反転する前の区間でも停止軌道となるため、オーバーシュートしない指令を生成することが可能であり、また、位置指令の差分が反転せず一方向へ動作中の時は、３つの位置を通り振動を小さくする条件を用いて軌道を決定できるようになるので、区間接続点や起動・停止時においても高次の微分が連続で、補間方法に起因する振動および位置決め停止時の振動オーバシュートを抑えることができるという効果がある。
また、請求項２に記載の発明によれば、停止軌道の停止までの時間を短くすることにより、位置決め停止時に補間に起因するオーバーシュートを無くせる効果がある。
また、請求項３に記載の発明によれば、補間に起因する振動を抑止し、指令追従性能を高めるフィードフォワード制御を構成できる効果がある。
【図面の簡単な説明】
【図１】本発明の実施の形態に係るサーボ装置の位置指令補間方法のフローチャートである。
【図２】図１に示す区分補間軌道を示す概念図である。
【図３】図１に示す区分補間軌道の逐次計算を示す概念図である。
【図４】図１に示す振動抑制軌道の決定条件を示す図である。
【図５】図１に示す停止軌道の決定条件を示す図である。
【図６】図１に示すオーバシュート防止方法を示す概念図である。
【図７】従来の補間方法における速度反転の判断の説明図である。
【図８】従来の補間方法の処理のフローチャートである。
【符号の説明】
３１　上位位置指令
３２　初期値
３３　１区間目補間結果
３４　２区間目補間結果
６１、６３　停止軌道
６２　ひし形
【図１】
2004139531.tif 000003
【図２】
2004139531.tif 000004
【図３】
2004139531.tif 000005
【図４】
2004139531.tif 000006
【図５】
2004139531.tif 000007
【図６】
2004139531.tif 000008
【図７】
2004139531.tif 000009
【図８】
2004139531.tif 000010