軌跡制御装置
【課題】コーナ形状のように指令経路の方向が急峻に変化するような場合でも駆動系に過大な負荷や振動を生じさせずに、また制御系の安定性を損なうことなく軌跡誤差を抑制すること。
【解決手段】複数の可動軸のモータを同時制御することにより可動部の軌跡を制御する軌跡制御装置１０において、サーボ系応答軌跡を演算するサーボ系応答軌跡算出部１と、誤差ベクトルを演算する誤差ベクトル算出部２と、誤差ベクトルとサーボ系応答軌跡の所定の参照点における移動方向に基づいて補正ベクトルを演算する補正ベクトル演算部３と、補正ベクトルを各軸に分配して各可動軸の補正量を演算する各軸補正量演算部４と、各可動軸の補正量を対応する軸の位置指令に加算する補正量加算部５、６と、各可動軸の位置がそれぞれの軸の補正後位置指令に追従するように各軸のモータ駆動トルクを出力することによりそれぞれの軸のモータを制御するサーボ制御部７、８を備えた。
【解決手段】複数の可動軸のモータを同時制御することにより可動部の軌跡を制御する軌跡制御装置１０において、サーボ系応答軌跡を演算するサーボ系応答軌跡算出部１と、誤差ベクトルを演算する誤差ベクトル算出部２と、誤差ベクトルとサーボ系応答軌跡の所定の参照点における移動方向に基づいて補正ベクトルを演算する補正ベクトル演算部３と、補正ベクトルを各軸に分配して各可動軸の補正量を演算する各軸補正量演算部４と、各可動軸の補正量を対応する軸の位置指令に加算する補正量加算部５、６と、各可動軸の位置がそれぞれの軸の補正後位置指令に追従するように各軸のモータ駆動トルクを出力することによりそれぞれの軸のモータを制御するサーボ制御部７、８を備えた。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、工作機械やレーザ加工機等において、指令形状にかかわらず軌跡誤差や追従誤差を抑制し高速高精度加工を実現する軌跡制御装置に関する。
【背景技術】
【０００２】
工作機械やレーザ加工機等の機械を用いて加工を行う場合、工作物に対する工具の位置が指令された経路上を沿うように制御することが行われる。この制御は軌跡制御と呼ばれ、一般に機械の各可動軸の実際の位置が指令経路から求めた各可動軸の位置指令に追従するようにサーボ制御を行うことにより行われる。
【０００３】
軌跡制御を行う上での問題点として、各可動軸の制御系の応答遅れなどに起因して、実際の軌跡が指令された経路からずれてしまうという問題がある。通常は機械の各可動軸ごとに制御を行うため、各軸の制御系の応答遅れなどに起因する誤差により、各可動軸のサーボ系応答が位置指令よりも遅れて移動する。直線のように指令経路の移動方向が変化しない場合には各軸が遅れて移動しても、サーボ系応答の軌跡としては指令経路上から外れない。つまり、指令経路の接線方向に誤差が現れるが、指令経路の法線方向の誤差は現れない。
【０００４】
一方、曲線やコーナ形状などのように指令経路の移動方向が変化する場合には、各軸のサーボ制御系の遅れにより、指令経路の法線方向に誤差が現れるようになる。以下では、サーボ系応答位置の位置指令に対する誤差のうち、指令経路の接線方向の成分を追従誤差、指令経路の法線方向の成分を軌跡誤差と呼ぶ。一般に、軌跡誤差があると、加工形状が本来の形状と一致しなくなるため好ましくない。また、追従誤差は、加工形状には直接影響を与えないため、軌跡誤差に比べると許容される場合が多いが、追従誤差が過大であると加工時間が延びることになり好ましくない。これらの軌跡誤差や追従誤差を抑制するため、これまでにいくつかの方法が開示されている。
【０００５】
特許文献１では、ロボットの手先位置の制御において、所定のサンプリング時間先の手先の位置を推定し、推定した手先位置から目標軌道上に降ろした垂線ベクトルの分だけ位置指令を補正することにより、時間遅れを許容しながら、手先位置を目標軌道上に乗せることにより軌跡誤差を抑制する方法が開示されている。
【０００６】
また、非特許文献１では、サーボ系の応答誤差を指令経路の接線方向と法線方向とに分解し、それぞれの方向の成分に個別のゲインを乗算して得られた制御入力を用いてフィードバック制御を行うことにより、追従誤差と軌跡誤差とを独立して制御する方法が開示されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００６−１５４３１号公報
【非特許文献】
【０００８】
【非特許文献１】精密工学会誌 Ｖｏｌ．７４ Ｎｏ．１１ ｐｐ．１１９３−１１９８
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかしながら、上記特許文献１による方法では、指令経路に垂直な方向を補正の方向とするため、コーナ形状の指令のように指令経路の方向が急峻に変化するような場合には、補正量の方向が急峻に変化する。このため、補正後の位置指令が急激に変化し、機械がこの急激に変化する指令に追従しようとして駆動系に過大な負荷がかかったり、過剰な振動が発生したりするという問題があった。
【００１０】
また、上記非特許文献１による方法では、指令経路に垂直な方向あるいは指令経路と同一の方向に制御入力を設定するため、コーナ形状の指令では特許文献１の場合と同様に機械を含む駆動系への過大な負荷や振動が発生するという問題があった。
【００１１】
上記のような問題のため、コーナ形状の指令のように指令経路の方向が急峻に変化するような指令経路の場合には従来の方法は適用できないという問題があった。
【００１２】
また、特許文献１による方法では追従誤差を抑制する効果はなく、非特許文献１による方法では、サーボ系応答誤差をフィードバックするため、制御系が不安定になる可能性があるという問題があった。
【００１３】
本発明は、上記に鑑みてなされたものであって、コーナ形状のように指令経路の方向が急峻に変化するような場合でも駆動系に過大な負荷や振動を生じさせずに、また制御系の安定性を損なうことなく軌跡誤差を抑制することが可能な軌跡制御装置を得ることを目的とする。
【課題を解決するための手段】
【００１４】
上述した課題を解決し、目的を達成するために、本発明の軌跡制御装置は、機械の可動部が複数の可動軸によって駆動され、前記複数の可動軸のモータを同時制御することにより前記可動部の軌跡を制御する軌跡制御装置において、前記複数の可動軸のうちの少なくとも２以上の可動軸の位置指令に基づいてサーボ系応答軌跡を演算するサーボ系応答軌跡算出部と、前記位置指令と前記サーボ系応答軌跡とに基づいて誤差ベクトルを演算する誤差ベクトル算出部と、前記誤差ベクトルと前記サーボ系応答軌跡の所定の参照点における移動方向に基づいて補正ベクトルを演算する補正ベクトル演算部と、前記補正ベクトルを前記２以上の可動軸に分配して各可動軸の補正量を演算する各軸補正量演算部と、前記各可動軸の補正量をそれぞれ対応する可動軸の位置指令に加算して前記各可動軸ごとに補正後位置指令を求めるために前記各可動軸ごとに設けられた補正量加算部と、前記各可動軸の位置がそれぞれの前記補正後位置指令に追従するように各可動軸のモータ駆動トルクを出力することにより各可動軸のモータを制御するために前記各可動軸ごとに設けられたサーボ制御部を備えることを特徴とする。
【発明の効果】
【００１５】
この発明によれば、コーナ形状のように指令経路の方向が急峻に変化するような場合でも駆動系に過大な負荷や振動を生じさせずに、また制御系の安定性を損なうことなく軌跡誤差を抑制することが可能という効果を奏する。
【図面の簡単な説明】
【００１６】
【図１】図１は、本発明に係る軌跡制御装置の実施の形態１の概略構成を示すブロック図である。
【図２】図２は、図１の軌跡制御装置に適用されるサーボ制御部の概略構成を示すブロック図である。
【図３】図３は、本発明の実施の形態１における誤差ベクトル算出部と補正ベクトル演算部の動作を説明する図である。
【図４】図４は、本発明の実施の形態１における補正ベクトル演算部の動作を説明する図である。
【図５】図５は、本発明の実施の形態２における誤差ベクトル算出部と補正ベクトル演算部の動作を説明する図である。
【図６】図６は、本発明の実施の形態２における補正ベクトル演算部の動作を説明する図である。
【図７】図７は、本発明の実施の形態３におけるサーボ系応答軌跡算出部の動作を説明する図である。
【発明を実施するための形態】
【００１７】
以下に、本発明に係る軌跡制御装置の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１８】
実施の形態１．
図１は、本発明に係る軌跡制御装置の実施の形態１の概略構成を示すブロック図である。図１において、指令経路は、ＮＣプログラム等の形式で機械の各可動軸の位置指令（第１軸位置指令Ｘｃ１と第２軸位置指令Ｘｃ２）として軌跡制御装置１０に与えられる。各可動軸は直線軸を対象としている。
【００１９】
サーボ系応答軌跡算出部１において、第１軸位置指令Ｘｃ１と第２軸位置指令Ｘｃ２が与えられたときのサーボ系応答軌跡Ｘｒが演算される。このサーボ系応答軌跡Ｘｒの演算方法については後述する。誤差ベクトル算出部２は、第１軸位置指令Ｘｃ１と第２軸位置指令Ｘｃ２を各可動軸方向の成分とする位置指令ベクトルと、同時刻のサーボ系応答軌跡Ｘｒ上の位置ベクトルとの差を求め、誤差ベクトルｅとして出力する。補正ベクトル演算部３は、サーボ系応答軌跡Ｘｒ上の点のうち、サーボ系の応答遅れ時間だけ先の時刻の点を参照点とし、参照点におけるサーボ系応答軌跡Ｘｒの移動方向に垂直な方向に誤差ベクトルｅを射影したベクトルに第１補正ゲインを乗じたベクトルと、参照点におけるサーボ系応答軌跡Ｘｒの移動方向と同一の方向に誤差ベクトルｅを射影したベクトルに第２補正ゲインを乗じたベクトルとの和を補正ベクトルΔとして出力する。各軸補正量演算部４では、補正ベクトルΔの第１および第２の可動軸方向の成分を、第１軸補正量Δ１および第２軸補正量Δ２としてそれぞれ出力する。第１軸補正量加算部５は、第１軸位置指令Ｘｃ１と第１軸補正量Δ１とを加算して第１軸補正後位置指令Ｘａ１を出力する。第２軸補正量加算部６は、第２軸位置指令Ｘｃ２と第２軸補正量Δ２とを加算して第２軸補正後位置指令Ｘａ２を出力する。第１軸サーボ制御部７は、第１可動軸の位置が、第１軸補正後位置指令Ｘａ１に追従するように、第１軸モータ駆動トルクτｍ１を出力する。第２軸サーボ制御部８は、第２可動軸の位置が、第２軸補正後位置指令Ｘａ２に追従するように、第２軸モータ駆動トルクτｍ２を出力する。なお、第１軸サーボ制御部７および第２軸サーボ制御部８は同一の構成をしている。第１軸モータ１４には第１軸負荷１６が接続され、第２軸モータ１５には第２軸負荷１７が接続され、第１軸モータ１４、第２軸モータ１５、第１軸負荷１６および第２軸負荷１７にて機械系１３が構成されている。そして、第１軸モータ１４は第１軸モータ駆動トルクτｍ１により駆動され、モータ速度信号ｖｍ１とモータ位置信号Ｘｍ１が第１軸サーボ制御部７へ出力される。第１軸モータ１５は第２軸モータ駆動トルクτｍ２により駆動され、モータ速度信号ｖｍ２とモータ位置信号Ｘｍ２が第２軸サーボ制御部８へ出力される。
【００２０】
図２は、図１の軌跡制御装置に適用されるサーボ制御部１１の概略構成を示すブロック図である。なお、サーボ制御部１１は、図１の第１軸サーボ制御部７および第２軸サーボ制御部８として用いることができる。
【００２１】
図２において、サーボ制御部１１へ入力された補正後位置指令Ｘａは、減算器２０でモデル位置Ｍｐが減算され、モデルゲイン乗算器２１で第１モデルゲインＫ１が乗算され、減算器２２でモデル速度Ｍｖが減算され、さらにモデルゲイン乗算器２３で第２モデルゲインＫ２が乗算されてモデル加速度Ｍａが積分器２４および乗算器３０に出力される。なお、サーボ制御部１１が第１軸サーボ制御部７として用いられる場合、補正後位置指令Ｘａは第１軸補正後位置指令Ｘａ１に対応し、サーボ制御部１１が第２サーボ制御部８として用いられる場合、補正後位置指令Ｘａは第２軸補正後位置指令Ｘａ２に対応することができる。そして、積分器２４でモデル加速度Ｍａが積分されてモデル速度Ｍｖが出力され、積分器２５でモデル速度Ｍｖが積分されてモデル位置Ｍｐが減算器２０、２６に出力される。補正後位置指令Ｘａからモデル位置Ｍｐまでのブロックを規範モデル部１２と呼ぶ。
【００２２】
そして、減算器２６でモデル位置Ｍｐからモータ位置信号Ｘｍが減算されて位置誤差Ｇｐが出力される。位置制御器２７では位置誤差Ｇｐに対して比例制御等の制御を行う。加減算器２８において位置制御器２７の出力にモデル速度Ｍｖが加算され、さらにモータ速度信号ｖｍが減算されて速度誤差Ｇｖが出力される。速度制御器２９では速度誤差Ｇｖに対して比例・積分制御等の制御を行う。乗算器３０で、モデル加速度Ｍａに制御対象のイナーシャに相当する値を乗算してモデルトルクＭｔが演算され、加算器３１で速度制御器２９の出力にモデルトルクＭｔが加算されてモータトルク信号τｍが出力される。モータ３２および負荷３３からなる機械系１３は、モータトルク信号τｍにより駆動され、モータ速度信号ｖｍとモータ位置信号Ｘｍがサーボ制御部１１へ出力される。なお、サーボ制御部１１が図１の第１軸サーボ制御部７として用いられる場合、モータ３２および負荷３３は、図１の第１軸モータ１４および第１軸負荷１６に対応し、サーボ制御部１１が図１の第２軸サーボ制御部８として用いられる場合、モータ３２および負荷３３は、図１の第２軸モータ１５および第２軸負荷１７に対応する。
【００２３】
このサーボ制御部１１は規範モデル部１２を用いた２自由度制御器であり、指令に対する追従性と外乱に対する応答性を独立して設計することができる。指令に対する追従性は第１モデルゲインＫ１および第２モデルゲインＫ２で決まり、外乱に対する応答性は位置制御器２７および速度制御器２９の設計で決まる。サーボ系の応答は、実際の制御対象の特性にかかわらず、規範モデル部１２の出力であるモデル位置Ｍｐに追従するように制御される。
【００２４】
サーボ系応答軌跡Ｘｒの演算は、以下のように行われる。前述のように、サーボ系の応答は規範モデル部１２の応答で表すことができるので、第１軸位置指令Ｘｃ１および第２軸位置指令Ｘｃ２が入力として与えられた場合の規範モデル部１２の出力を求めることにより第１軸および第２軸のサーボ系応答位置をそれぞれ求める。そして、第１軸および第２軸のサーボ系応答位置を各可動軸方向の成分とするベクトルをサーボ系応答位置ベクトルとし、このサーボ系応答位置ベクトルが描く軌跡をサーボ系応答軌跡Ｘｒとする。
【００２５】
規範モデル部１２の出力は、規範モデル部１２を微分方程式、差分方程式、伝達関数等の形式で表し、数値計算によりその解を計算することで求めることができる。また、指令形状が既知である場合は、積分計算により解析解を求めることができる。
例えば、図２に示した規範モデル部１２の伝達関数Ｇｍ（ｓ）は次式で表される。
【００２６】
【数１】
規範モデル部１２にある入力が与えられたときの出力は、規範モデル部１２の伝達関数と与えられた入力のラプラス変換との積を逆ラプラス変換することにより求められる。時刻ｔにおける第１軸位置指令Ｘｃ１と第２軸位置指令Ｘｃ２がそれぞれｘｃ１（ｔ）、ｘｃ２（ｔ）で与えられたとき、位置指令ベクトルｘｃ（ｔ）は次式で表される。
【００２７】
【数２】
このとき、第１軸および第２軸のサーボ系応答位置ベクトルｘｒ１（ｔ）、ｘｒ２（ｔ）は次式で表される。
【００２８】
【数３】
ただし、Ｌ［ｆ（ｔ）］はｆ（ｔ）のラプラス変換を、Ｌ−１［Ｆ（ｓ）］はＦ（ｓ）の逆ラプラス変換を表す。
また、サーボ系の応答遅れ時間は、規範モデル部１２の伝達関数から求めることができる。ここでは、サーボ系に一定速度の入力が与えられたときの指令に対する応答の定常的な遅れ時間をサーボ系応答遅れ時間とする。図２に示した規範モデル部１２では、サーボ系応答遅れ時間ｔｄは第１モデルゲインＫ１の逆数となり、次式で表される。
【００２９】
【数４】
図３は、本発明の実施の形態１における誤差ベクトル算出部２と補正ベクトル演算部３の動作を説明する図である。ここでは、指令経路がコーナ形状の経路であり、時刻ｔにおいてコーナの頂点となるような場合について記述している。
図３において、サーボ系応答軌跡Ｘｒは破線のような経路をとり、時刻ｔにおいてｘｒ（ｔ）の位置にある。誤差ベクトル算出部２では、時刻ｔにおけるサーボ系応答位置ベクトルｘｒ（ｔ）から、同時刻における位置指令ベクトルｘｃ（ｔ）を減算して、誤差ベクトルｅ（ｔ）を求める。これらを式で表すと、次式となる。
【００３０】
【数５】
また、サーボ系応答軌跡Ｘｒ上の参照点を、サーボ系応答遅れ時間ｔｄだけ先の点ｘｒ（ｔ＋ｔｄ）とする。なお、位置指令の補正をすべての時刻に対して一括して行う場合はサーボ系応答遅れ時間分先の点を求めることは容易であるが、時々刻々の位置指令を逐次的に処理して位置指令の補正を行う場合には、位置指令をサーボ系応答遅れ時間分先読みする必要がある。
補正ベクトル演算部３では、この参照点ｘｒ（ｔ＋ｔｄ）におけるサーボ系応答軌跡Ｘｒの移動方向に垂直な方向のベクトル、すなわち法線ベクトルと、移動方向に同一な方向のベクトル、すなわち接線ベクトルを求め、誤差ベクトルをそれぞれの方向へ射影したベクトル（正射影ベクトル）を求める。
【００３１】
演算の便宜上、法線ベクトルおよび接線ベクトルの長さは、１となるように正規化する。正規化されたベクトルを、それぞれ単位法線ベクトルｒｎ（ｔ）および単位接線ベクトルｒｔ（ｔ）と呼ぶ。
【００３２】
接線ベクトルｒｔ０（ｔ）および単位接線ベクトルｒｔ（ｔ）は、式で表すと次式のようになる。
【００３３】
【数６】
【００３４】
法線ベクトルｒｎ０（ｔ）および単位法線ベクトルｒｎ（ｔ）は、式で表すと次式のようになる。
【００３５】
【数７】
なお、単位法線ベクトルｒｎ（ｔ）および単位接線ベクトルｒｔ（ｔ）の演算にはベクトルの微分演算が必要になるが、これらの演算はベクトルの要素ごとに時刻ｔに関する微分演算を行うことにより行われる。また、制御系を離散時間系で設計する場合は、微分演算を差分近似して実現することとなる。
【００３６】
次に、誤差ベクトルｅを参照点におけるサーボ系応答軌跡Ｘｒの法線ベクトル方向へ射影したベクトルを求める。このベクトルは誤差ベクトルｅのうち軌跡に垂直な方向の誤差すなわち軌跡誤差を表すので、軌跡誤差ベクトルｅｎ（ｔ）とよぶ。軌跡誤差ベクトルｅｎ（ｔ）は、大きさが誤差ベクトルｅの単位法線ベクトル方向成分（すなわち誤差ベクトルｅと単位法線ベクトルｒｎ（ｔ）の内積）であり、方向が単位法線ベクトル方向であるベクトルとなる。
【００３７】
さらに、誤差ベクトルｅを参照点におけるサーボ系応答軌跡Ｘｒの接線ベクトル方向へ射影したベクトルを求める。このベクトルは、誤差ベクトルｅのうち軌跡に沿った方向の誤差すなわち追従誤差を表すので、このベクトルを追従誤差ベクトルｅｔ（ｔ）と呼ぶ。追従誤差ベクトルｅｔ（ｔ）は、大きさが誤差ベクトルｅの単位接線ベクトル方向成分（すなわち誤差ベクトルｅと単位接線ベクトルｒｔ（ｔ）の内積）であり、方向が単位接線ベクトル方向であるベクトルとなる。軌跡誤差ベクトルｅｎ（ｔ）および追従誤差ベクトルｅｔ（ｔ）を式で表すと次式のようになる。
【００３８】
【数８】
【００３９】
【数９】
【００４０】
一般に誤差ベクトルｅとサーボ系応答軌跡Ｘｒの接線ベクトルおよび法線ベクトルはおおむね同一平面上にある。特に、可動軸が２軸のみの場合や、指令経路が２次元平面内にあり、サーボ系の応答が全軸で等しい場合にはこれらのベクトルは完全に同一平面内にある。したがって、誤差ベクトルｅと接線ベクトルがなす平面上にありかつ接線ベクトルに垂直なベクトルを軌跡誤差ベクトルｅｎ（ｔ）としてもよい。この演算は誤差ベクトルｅから追従誤差ベクトルｅｔ（ｔ）を引くことにより行うことができるので、式（８）を用いるよりも簡便に軌跡誤差ベクトルｅｎ（ｔ）を求めることができる。この場合、軌跡誤差ベクトルｅｎ（ｔ）は次式で求められる。
【００４１】
【数１０】
【００４２】
図４は、本発明の実施の形態１における補正ベクトル演算部３の動作を説明する図である。図４において、補正ベクトルΔは、軌跡誤差ベクトルｅｎ（ｔ）に第１の補正ゲインαｎを乗算したベクトルと、追従誤差ベクトルｅｔ（ｔ）に第２の補正ゲインαｔを乗算したベクトルの和で表される。時刻ｔにおける補正ベクトルΔをΔ（ｔ）とすると、補正ベクトルΔ（ｔ）は次式のようになる。
【００４３】
【数１１】
第１補正ゲインαｎは、法線方向の誤差すなわち軌跡誤差を調節するためのゲインである。この第１補正ゲインαｎは通常０と−１の間で設定され、−１に近い値を設定するほど軌跡誤差を小さくすることができる。好ましくは、−１に設定すると、法線方向の誤差ベクトルｅを打ち消すように位置指令を補正するので、軌跡誤差を効果的に抑制することができる。軌跡誤差を抑制することにより、加工形状の本来の形状からの誤差を小さくすることができる。
【００４４】
第２補正ゲインαｔは、接線方向の誤差すなわち追従誤差を調節するためのゲインである。この第２補正ゲインαｔは通常０と−１の間で設定され、−１に近い値を設定するほど追従誤差を小さくすることができる。好ましくは、−１に設定すると、接線方向の誤差ベクトルを打ち消すように位置指令を補正するので、追従誤差を効果的に抑制することができる。追従誤差を抑制することにより、加工に要する時間を短縮することができる。
【００４５】
補正ベクトルΔ（ｔ）を位置指令ベクトルに加算したベクトルが、補正後の位置指令ベクトルとなる。補正ベクトルΔ（ｔ）の加算は第１軸補正量加算部５および第２軸補正量加算部６において各可動軸について行われる。すなわち、補正ベクトルΔ（ｔ）の各可動軸方向の成分が各可動軸の補正量となり、各可動軸の補正量に各可動軸の位置指令を軸ごとに加算した値が、各可動軸の補正後位置指令となる。本実施の形態では可動軸が２軸の場合なので、補正ベクトルΔ（ｔ）の第１要素Δ１（ｔ）および第２要素Δ２（ｔ）が、それぞれ第１軸および第２軸の補正量となる。時刻ｔにおける第１軸補正後位置指令Ｘａ１および第２軸補正後位置指令Ｘａ２がそれぞれｘａ１（ｔ）、ｘａ２（ｔ）で与えられた時、第１軸補正後位置指令ｘａ１（ｔ）および第２軸補正後位置指令ｘａ２（ｔ）はそれぞれ次式で表される。
【００４６】
【数１２】
【００４７】
第１軸サーボ制御部７および第２軸サーボ制御部８では、それぞれ第１軸補正後位置指令ｘａ１（ｔ）および第２軸補正後位置指令ｘａ２（ｔ）に対して図２に示すサーボ制御系により第１軸および第２軸のモータが駆動され、機械系１３の可動部の位置が制御される。
【００４８】
以上のように、本実施の形態１によれば、複数の軸のサーボ系応答により誤差ベクトルｅを求め、サーボ系応答軌跡Ｘｒの移動方向に基づいて求めた補正量により位置指令を補正する。これにより、コーナ形状の指令のように指令経路の方向が急変するような場合でも、サーボ系応答軌跡Ｘｒは滑らかに変化するため、誤差ベクトルｅを滑らかに変化させ、補正ベクトルΔの方向も滑らかに変化させることができる。このため、補正後の位置指令が滑らかとなり、指令形状にかかわらず軌跡誤差と追従誤差を抑制することができる。また、フィードバック信号を用いずに位置指令の補正を行うため、制御系の安定性を常に保証することができる。
【００４９】
また、本実施の形態１によれば、誤差ベクトルｅをサーボ系応答軌跡Ｘｒの垂直成分すなわち法線方向成分と平行成分すなわち接線方向成分に分解して、それぞれにゲインを設定することにより、指令形状にかかわらず、軌跡に垂直な方向の誤差である軌跡誤差と軌跡に平行な方向の誤差である追従誤差とを独立して制御することができる。
【００５０】
また、本実施の形態１によれば、サーボ系の応答遅れを考慮して参照点とそれに基づく補正ベクトルΔの方向を決定することにより、簡単な演算で比較的正確な補正ベクトルΔの方向を決定することができる。
【００５１】
さらに、本実施の形態１によれば、サーボ制御部１１に規範モデル部１２を用いた２自由度制御器を用いることで、制御対象の特性にかかわらずサーボ制御部１１の応答を規範モデル部１２の応答から正確に算出することができ、さらにこの規範モデル部１２を用いて応答軌跡を求めて位置指令を補正することにより、サーボ制御部１１の応答性に起因して生じる軌跡誤差を正確に補正することができる。
【００５２】
本実施の形態１では、可動軸の数が２としていたが、可動軸の数が３の場合であってもよい。サーボ系応答軌跡ベクトル、誤差ベクトル、補正ベクトルを２次元ではなく３次元のベクトルとすることで、同様の補正を行うことができる。
【００５３】
実施の形態２．
実施の形態２の構成は実施の形態１と概略同一である。相違点は、補正ベクトル演算部３における参照点および補正ベクトルの決定方式である。以下、これらの相違点について説明する。サーボ系応答軌跡算出部１の動作は、実施の形態１と同様である。
【００５４】
図５は、本発明の実施の形態２における誤差ベクトル算出部２と補正ベクトル演算部３の動作を説明する図である。図５において、誤差ベクトル算出部２における誤差ベクトルｅ（ｔ）の算出は実施の形態１と同様であり、式（５）で求められる。実施の形態２では、ある時刻ｔにおける参照点を、その時刻ｔにおける位置指令からサーボ系応答軌跡Ｘｒへ下ろした垂線とサーボ系応答軌跡Ｘｒとの交点と定める。
【００５５】
このときの参照点の位置は、幾何学的な関係からいくつかの方法により求めることができる。一例を挙げると、サーボ系応答軌跡Ｘｒの接線ベクトルが参照点と位置指令を結ぶ直線と直交するように参照点の位置を定める方法がある。すなわち、サーボ系応答軌跡Ｘｒを時刻ｔの関数で表した場合、その接線ベクトルと、位置指令から参照点へのベクトルとの内積が０となる時刻ｔ＋ｔｒを求め、その時刻におけるサーボ系応答軌跡Ｘｒ上の点ｘｒ（ｔ＋ｔｒ）を参照点とする方法である。このことを式で表すと次式のようになる。
【００５６】
【数１３】
【００５７】
ただし、ｘｒ’はサーボ系応答軌跡Ｘｒの接線ベクトルｄｘｒ／ｄｔを表す。参照点における時刻ｔ＋ｔｒは、上記の（１３）式の方程式を解くことにより求められる。
【００５８】
次に、参照点におけるサーボ系応答軌跡Ｘｒの法線ベクトル方向を求める。この場合、位置指令から参照点へのベクトルの方向と、参照点におけるサーボ系応答軌跡Ｘｒの法線方向は、ともに参照点におけるサーボ系応答軌跡Ｘｒの接線方向に直交する。従って、参照点におけるサーボ系応答軌跡Ｘｒの法線ベクトル方向は位置指令と参照点を結ぶ直線の方向と同一の方向となるので、単位法線ベクトルｒｎ（ｔ）は位置指令から参照点までの方向のベクトルを正規化することにより求められる。このことを式で表すと、次式となる。
【００５９】
【数１４】
【００６０】
図６は、本発明の実施の形態２における補正ベクトル演算部３の動作を説明する図である。図６において、補正ベクトルΔ（ｔ）は、誤差ベクトルｅ（ｔ）を単位法線ベクトル方向へ射影したベクトルである軌跡誤差ベクトルｅｎ（ｔ）に、補正ゲインαｎをかけて求められる。軌跡誤差ベクトルｅｎ（ｔ）は、実施の形態１と同様に誤差ベクトルと単位法線ベクトルを用いて式（８）で求められる。補正ベクトルΔ（ｔ）は、補正ゲインと軌跡誤差ベクトルを用いて次式で表される。
【００６１】
【数１５】
【００６２】
補正ゲインαｎは、法線方向の誤差すなわち軌跡誤差を調節するためのゲインである。このゲインは通常０と−１の間で設定され、−１に近い値を設定するほど軌跡誤差を小さくすることができる。好ましくは、−１に設定すると、法線方向の誤差ベクトルを打ち消すように位置指令を補正するので、軌跡誤差を効果的に抑制することができる。軌跡誤差を抑制することにより、加工形状の本来の形状からの誤差を小さくすることができる。
【００６３】
誤差ベクトルｅに基づく各可動軸の補正量の演算と、第１軸補正量加算部５および第２軸補正量加算部６における動作は実施の形態１と同様である。また、第１軸サーボ制御部７および第２軸サーボ制御部８の動作は、実施の形態１と同様である。
【００６４】
以上のように、本実施の形態２によれば、複数の軸のサーボ系応答により誤差ベクトルｅを求め、サーボ系応答軌跡Ｘｒの移動方向に基づいて求めた補正量により位置指令を補正する。これにより、コーナ形状の指令のように指令経路の方向が急変するような場合でも、サーボ系応答軌跡Ｘｒは滑らかに変化するため、誤差ベクトルｅを滑らかに変化させ、補正ベクトルの方向も滑らかに変化させることができる。このため、補正後の位置指令が滑らかとなり、指令形状にかかわらず軌跡誤差と追従誤差を抑制することができる。また、フィードバック信号を用いずに位置指令の補正を行うため、制御系の安定性を常に保証する事ができる。
【００６５】
本実施の形態２によれば、誤差ベクトルｅをサーボ系応答軌跡Ｘｒの垂直成分すなわち法線方向成分にゲインを設定することにより、ひとつのゲインを設定するだけで、指令形状にかかわらず、軌跡誤差を抑制し、加工形状が本来の形状からのずれを抑制することができる。
【００６６】
本実施の形態２によれば、サーボ系応答と指令された位置との幾何学的な関係を考慮して参照点とそれに基づく補正ベクトルの方向を決定することにより、より正確な補正ベクトルの方向を決定することができる。
【００６７】
さらに、本実施の形態２によれば、規範モデル部１２を用いた２自由度制御器を用いることで、制御対象の特性にかかわらずサーボ制御部１１の応答を規範モデルの応答から正確に算出することができ、さらにこの規範モデル部１２を用いて応答軌跡を求めて位置指令を補正することにより、サーボ制御部１１の応答性に起因して生じる軌跡誤差を正確に補正することができる。
【００６８】
本実施の形態２では、可動軸の数が２としていたが、可動軸の数が３の場合であってもよい。サーボ系応答軌跡ベクトル、誤差ベクトル、補正ベクトルを２次元ではなく３次元のベクトルとすることで、同様の補正を行うことができる。
【００６９】
実施の形態３．
図７は、本発明の実施の形態３におけるサーボ系応答軌跡算出部１の動作を説明する図である。図７において、この実施の形態３では、サーボ系モデルを用いてサーボ系応答軌跡Ｘｒを求めるのではなく、位置指令の情報のみを用いて求める。指令形状が円弧などのなめらかに変化する形状である場合、指令経路の加速度方向に加速度の大きさに比例して軌跡誤差が生じる性質がある。この比例係数は、サーボ系の応答性から定める。時刻ｔにおけるサーボ系応答の位置を、同時刻における位置指令に誤差ベクトルｅ（ｔ）を加算することにより求める。誤差ベクトルｅ（ｔ）は、位置指令の加速度ベクトルｄ２ｘｃ／ｄｔ２に、係数ａをかけることにより求める。この係数ａは、サーボ系応答遅れ時間ｔｄの二乗に比例するように定める。この関係式は、指令加速度と軌跡誤差量の関係を実測して求める方法のほか、円弧指令時の軌跡誤差量の理論値と指令加速度の比を解析的に求める方法により設定する。後者の方法では、サーボ系の周波数応答伝達関数の絶対値に指令半径を乗じた値を指令半径から引くことにより円弧指令時の軌跡誤差量を求め、この軌跡誤差量を指令加速度で除することにより求める。半径Ｒ、各速度ωの円弧指令時の場合、軌跡誤差量と指令加速度の比は、サーボ系の伝達関数Ｇ（ｓ）を用いて以下の式で与えられる。
【００７０】
【数１６】
ここで、ｊは虚数単位である。
【００７１】
図２に示すサーボ制御系は伝達関数が式（１）で表され、サーボ系応答遅れ時間は式（４）で表される。第２モデルゲインＫ２が第１モデルゲインＫ１の４倍である場合、すなわちＫ２＝４Ｋ１である場合の軌跡誤差量とサーボ系応答遅れ時間との関係を式（１６）を用いて求めると、次式のようになる。
【００７２】
【数１７】
誤差ベクトル算出部２、補正ベクトル演算部３、各軸補正量演算部４、第１軸補正量加算部５、第２軸補正量加算部６、第１軸サーボ制御部７、第２軸サーボ制御部８の動作は、実施の形態２と同様である。
【００７３】
以上のように、本実施の形態３によれば、複数の軸のサーボ系応答により誤差ベクトルｅを求め、サーボ系応答軌跡Ｘｒの移動方向に基づいて求めた補正量により位置指令を補正する。これにより、コーナ形状の指令のように指令経路の方向が急変するような場合でも、サーボ系応答軌跡Ｘｒは滑らかに変化するため、誤差ベクトルｅを滑らかに変化させ、補正ベクトルの方向も滑らかに変化させることができる。このため、補正後の位置指令が滑らかとなり、指令形状にかかわらず軌跡誤差と追従誤差を抑制することができる。また、フィードバック信号を用いずに位置指令の補正を行うため、制御系の安定性を常に保証することができる。
【００７４】
本実施の形態３によれば、誤差ベクトルｅをサーボ系応答軌跡Ｘｒの垂直成分すなわち法線方向成分にゲインを設定することにより、ひとつのゲインを設定するだけで、指令形状にかかわらず、軌跡誤差を抑制し、加工形状が本来の形状からのずれを抑制することができる。
【００７５】
本実施の形態３によれば、サーボ制御部１１のモデルを用いずに、位置指令軌跡の情報だけでサーボ系応答軌跡Ｘｒを求めることにより、サーボ系応答軌跡Ｘｒを求めるための演算負荷を軽減することができる。
【００７６】
本実施の形態３では、可動軸の数が２としていたが、可動軸の数が３の場合であってもよい。サーボ系応答軌跡ベクトル、誤差ベクトル、補正ベクトルを２次元ではなく３次元のベクトルとすることで、同様の補正を行うことができる。
【産業上の利用可能性】
【００７７】
以上のように本発明に係る軌跡制御装置は、コーナ形状のように指令経路の方向が急峻に変化するような場合でも駆動系に過大な負荷や振動を生じさせずに、また制御系の安定性を損なうことなく軌跡誤差を抑制することができ、工作機械やレーザ加工機等において指令形状にかかわらず高速高精度加工を実現する方法に適している。
【符号の説明】
【００７８】
１ サーボ系応答軌跡算出部
２ 誤差ベクトル算出部
３ 補正ベクトル演算部
４ 各軸補正量演算部
５ 第１軸補正量加算部
６ 第２軸補正量加算部
７ 第１軸サーボ制御部
８ 第２軸サーボ制御部
１０ 軌跡制御装置
１１ サーボ制御部
１２ 規範モデル部
１３ 機械系
１４ 第１軸モータ
１５ 第２軸モータ
１６ 第１軸負荷
１７ 第２軸負荷
２０、２２、２６ 減算器
２１、２３ モデルゲイン乗算器
２４、２５ 積分器
３１ 加算器
２７ 位置制御器
２８ 加減算器
２９ 速度制御器
３０ 乗算器
３２ モータ
３３ 負荷
【技術分野】
【０００１】
本発明は、工作機械やレーザ加工機等において、指令形状にかかわらず軌跡誤差や追従誤差を抑制し高速高精度加工を実現する軌跡制御装置に関する。
【背景技術】
【０００２】
工作機械やレーザ加工機等の機械を用いて加工を行う場合、工作物に対する工具の位置が指令された経路上を沿うように制御することが行われる。この制御は軌跡制御と呼ばれ、一般に機械の各可動軸の実際の位置が指令経路から求めた各可動軸の位置指令に追従するようにサーボ制御を行うことにより行われる。
【０００３】
軌跡制御を行う上での問題点として、各可動軸の制御系の応答遅れなどに起因して、実際の軌跡が指令された経路からずれてしまうという問題がある。通常は機械の各可動軸ごとに制御を行うため、各軸の制御系の応答遅れなどに起因する誤差により、各可動軸のサーボ系応答が位置指令よりも遅れて移動する。直線のように指令経路の移動方向が変化しない場合には各軸が遅れて移動しても、サーボ系応答の軌跡としては指令経路上から外れない。つまり、指令経路の接線方向に誤差が現れるが、指令経路の法線方向の誤差は現れない。
【０００４】
一方、曲線やコーナ形状などのように指令経路の移動方向が変化する場合には、各軸のサーボ制御系の遅れにより、指令経路の法線方向に誤差が現れるようになる。以下では、サーボ系応答位置の位置指令に対する誤差のうち、指令経路の接線方向の成分を追従誤差、指令経路の法線方向の成分を軌跡誤差と呼ぶ。一般に、軌跡誤差があると、加工形状が本来の形状と一致しなくなるため好ましくない。また、追従誤差は、加工形状には直接影響を与えないため、軌跡誤差に比べると許容される場合が多いが、追従誤差が過大であると加工時間が延びることになり好ましくない。これらの軌跡誤差や追従誤差を抑制するため、これまでにいくつかの方法が開示されている。
【０００５】
特許文献１では、ロボットの手先位置の制御において、所定のサンプリング時間先の手先の位置を推定し、推定した手先位置から目標軌道上に降ろした垂線ベクトルの分だけ位置指令を補正することにより、時間遅れを許容しながら、手先位置を目標軌道上に乗せることにより軌跡誤差を抑制する方法が開示されている。
【０００６】
また、非特許文献１では、サーボ系の応答誤差を指令経路の接線方向と法線方向とに分解し、それぞれの方向の成分に個別のゲインを乗算して得られた制御入力を用いてフィードバック制御を行うことにより、追従誤差と軌跡誤差とを独立して制御する方法が開示されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００６−１５４３１号公報
【非特許文献】
【０００８】
【非特許文献１】精密工学会誌 Ｖｏｌ．７４ Ｎｏ．１１ ｐｐ．１１９３−１１９８
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかしながら、上記特許文献１による方法では、指令経路に垂直な方向を補正の方向とするため、コーナ形状の指令のように指令経路の方向が急峻に変化するような場合には、補正量の方向が急峻に変化する。このため、補正後の位置指令が急激に変化し、機械がこの急激に変化する指令に追従しようとして駆動系に過大な負荷がかかったり、過剰な振動が発生したりするという問題があった。
【００１０】
また、上記非特許文献１による方法では、指令経路に垂直な方向あるいは指令経路と同一の方向に制御入力を設定するため、コーナ形状の指令では特許文献１の場合と同様に機械を含む駆動系への過大な負荷や振動が発生するという問題があった。
【００１１】
上記のような問題のため、コーナ形状の指令のように指令経路の方向が急峻に変化するような指令経路の場合には従来の方法は適用できないという問題があった。
【００１２】
また、特許文献１による方法では追従誤差を抑制する効果はなく、非特許文献１による方法では、サーボ系応答誤差をフィードバックするため、制御系が不安定になる可能性があるという問題があった。
【００１３】
本発明は、上記に鑑みてなされたものであって、コーナ形状のように指令経路の方向が急峻に変化するような場合でも駆動系に過大な負荷や振動を生じさせずに、また制御系の安定性を損なうことなく軌跡誤差を抑制することが可能な軌跡制御装置を得ることを目的とする。
【課題を解決するための手段】
【００１４】
上述した課題を解決し、目的を達成するために、本発明の軌跡制御装置は、機械の可動部が複数の可動軸によって駆動され、前記複数の可動軸のモータを同時制御することにより前記可動部の軌跡を制御する軌跡制御装置において、前記複数の可動軸のうちの少なくとも２以上の可動軸の位置指令に基づいてサーボ系応答軌跡を演算するサーボ系応答軌跡算出部と、前記位置指令と前記サーボ系応答軌跡とに基づいて誤差ベクトルを演算する誤差ベクトル算出部と、前記誤差ベクトルと前記サーボ系応答軌跡の所定の参照点における移動方向に基づいて補正ベクトルを演算する補正ベクトル演算部と、前記補正ベクトルを前記２以上の可動軸に分配して各可動軸の補正量を演算する各軸補正量演算部と、前記各可動軸の補正量をそれぞれ対応する可動軸の位置指令に加算して前記各可動軸ごとに補正後位置指令を求めるために前記各可動軸ごとに設けられた補正量加算部と、前記各可動軸の位置がそれぞれの前記補正後位置指令に追従するように各可動軸のモータ駆動トルクを出力することにより各可動軸のモータを制御するために前記各可動軸ごとに設けられたサーボ制御部を備えることを特徴とする。
【発明の効果】
【００１５】
この発明によれば、コーナ形状のように指令経路の方向が急峻に変化するような場合でも駆動系に過大な負荷や振動を生じさせずに、また制御系の安定性を損なうことなく軌跡誤差を抑制することが可能という効果を奏する。
【図面の簡単な説明】
【００１６】
【図１】図１は、本発明に係る軌跡制御装置の実施の形態１の概略構成を示すブロック図である。
【図２】図２は、図１の軌跡制御装置に適用されるサーボ制御部の概略構成を示すブロック図である。
【図３】図３は、本発明の実施の形態１における誤差ベクトル算出部と補正ベクトル演算部の動作を説明する図である。
【図４】図４は、本発明の実施の形態１における補正ベクトル演算部の動作を説明する図である。
【図５】図５は、本発明の実施の形態２における誤差ベクトル算出部と補正ベクトル演算部の動作を説明する図である。
【図６】図６は、本発明の実施の形態２における補正ベクトル演算部の動作を説明する図である。
【図７】図７は、本発明の実施の形態３におけるサーボ系応答軌跡算出部の動作を説明する図である。
【発明を実施するための形態】
【００１７】
以下に、本発明に係る軌跡制御装置の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１８】
実施の形態１．
図１は、本発明に係る軌跡制御装置の実施の形態１の概略構成を示すブロック図である。図１において、指令経路は、ＮＣプログラム等の形式で機械の各可動軸の位置指令（第１軸位置指令Ｘｃ１と第２軸位置指令Ｘｃ２）として軌跡制御装置１０に与えられる。各可動軸は直線軸を対象としている。
【００１９】
サーボ系応答軌跡算出部１において、第１軸位置指令Ｘｃ１と第２軸位置指令Ｘｃ２が与えられたときのサーボ系応答軌跡Ｘｒが演算される。このサーボ系応答軌跡Ｘｒの演算方法については後述する。誤差ベクトル算出部２は、第１軸位置指令Ｘｃ１と第２軸位置指令Ｘｃ２を各可動軸方向の成分とする位置指令ベクトルと、同時刻のサーボ系応答軌跡Ｘｒ上の位置ベクトルとの差を求め、誤差ベクトルｅとして出力する。補正ベクトル演算部３は、サーボ系応答軌跡Ｘｒ上の点のうち、サーボ系の応答遅れ時間だけ先の時刻の点を参照点とし、参照点におけるサーボ系応答軌跡Ｘｒの移動方向に垂直な方向に誤差ベクトルｅを射影したベクトルに第１補正ゲインを乗じたベクトルと、参照点におけるサーボ系応答軌跡Ｘｒの移動方向と同一の方向に誤差ベクトルｅを射影したベクトルに第２補正ゲインを乗じたベクトルとの和を補正ベクトルΔとして出力する。各軸補正量演算部４では、補正ベクトルΔの第１および第２の可動軸方向の成分を、第１軸補正量Δ１および第２軸補正量Δ２としてそれぞれ出力する。第１軸補正量加算部５は、第１軸位置指令Ｘｃ１と第１軸補正量Δ１とを加算して第１軸補正後位置指令Ｘａ１を出力する。第２軸補正量加算部６は、第２軸位置指令Ｘｃ２と第２軸補正量Δ２とを加算して第２軸補正後位置指令Ｘａ２を出力する。第１軸サーボ制御部７は、第１可動軸の位置が、第１軸補正後位置指令Ｘａ１に追従するように、第１軸モータ駆動トルクτｍ１を出力する。第２軸サーボ制御部８は、第２可動軸の位置が、第２軸補正後位置指令Ｘａ２に追従するように、第２軸モータ駆動トルクτｍ２を出力する。なお、第１軸サーボ制御部７および第２軸サーボ制御部８は同一の構成をしている。第１軸モータ１４には第１軸負荷１６が接続され、第２軸モータ１５には第２軸負荷１７が接続され、第１軸モータ１４、第２軸モータ１５、第１軸負荷１６および第２軸負荷１７にて機械系１３が構成されている。そして、第１軸モータ１４は第１軸モータ駆動トルクτｍ１により駆動され、モータ速度信号ｖｍ１とモータ位置信号Ｘｍ１が第１軸サーボ制御部７へ出力される。第１軸モータ１５は第２軸モータ駆動トルクτｍ２により駆動され、モータ速度信号ｖｍ２とモータ位置信号Ｘｍ２が第２軸サーボ制御部８へ出力される。
【００２０】
図２は、図１の軌跡制御装置に適用されるサーボ制御部１１の概略構成を示すブロック図である。なお、サーボ制御部１１は、図１の第１軸サーボ制御部７および第２軸サーボ制御部８として用いることができる。
【００２１】
図２において、サーボ制御部１１へ入力された補正後位置指令Ｘａは、減算器２０でモデル位置Ｍｐが減算され、モデルゲイン乗算器２１で第１モデルゲインＫ１が乗算され、減算器２２でモデル速度Ｍｖが減算され、さらにモデルゲイン乗算器２３で第２モデルゲインＫ２が乗算されてモデル加速度Ｍａが積分器２４および乗算器３０に出力される。なお、サーボ制御部１１が第１軸サーボ制御部７として用いられる場合、補正後位置指令Ｘａは第１軸補正後位置指令Ｘａ１に対応し、サーボ制御部１１が第２サーボ制御部８として用いられる場合、補正後位置指令Ｘａは第２軸補正後位置指令Ｘａ２に対応することができる。そして、積分器２４でモデル加速度Ｍａが積分されてモデル速度Ｍｖが出力され、積分器２５でモデル速度Ｍｖが積分されてモデル位置Ｍｐが減算器２０、２６に出力される。補正後位置指令Ｘａからモデル位置Ｍｐまでのブロックを規範モデル部１２と呼ぶ。
【００２２】
そして、減算器２６でモデル位置Ｍｐからモータ位置信号Ｘｍが減算されて位置誤差Ｇｐが出力される。位置制御器２７では位置誤差Ｇｐに対して比例制御等の制御を行う。加減算器２８において位置制御器２７の出力にモデル速度Ｍｖが加算され、さらにモータ速度信号ｖｍが減算されて速度誤差Ｇｖが出力される。速度制御器２９では速度誤差Ｇｖに対して比例・積分制御等の制御を行う。乗算器３０で、モデル加速度Ｍａに制御対象のイナーシャに相当する値を乗算してモデルトルクＭｔが演算され、加算器３１で速度制御器２９の出力にモデルトルクＭｔが加算されてモータトルク信号τｍが出力される。モータ３２および負荷３３からなる機械系１３は、モータトルク信号τｍにより駆動され、モータ速度信号ｖｍとモータ位置信号Ｘｍがサーボ制御部１１へ出力される。なお、サーボ制御部１１が図１の第１軸サーボ制御部７として用いられる場合、モータ３２および負荷３３は、図１の第１軸モータ１４および第１軸負荷１６に対応し、サーボ制御部１１が図１の第２軸サーボ制御部８として用いられる場合、モータ３２および負荷３３は、図１の第２軸モータ１５および第２軸負荷１７に対応する。
【００２３】
このサーボ制御部１１は規範モデル部１２を用いた２自由度制御器であり、指令に対する追従性と外乱に対する応答性を独立して設計することができる。指令に対する追従性は第１モデルゲインＫ１および第２モデルゲインＫ２で決まり、外乱に対する応答性は位置制御器２７および速度制御器２９の設計で決まる。サーボ系の応答は、実際の制御対象の特性にかかわらず、規範モデル部１２の出力であるモデル位置Ｍｐに追従するように制御される。
【００２４】
サーボ系応答軌跡Ｘｒの演算は、以下のように行われる。前述のように、サーボ系の応答は規範モデル部１２の応答で表すことができるので、第１軸位置指令Ｘｃ１および第２軸位置指令Ｘｃ２が入力として与えられた場合の規範モデル部１２の出力を求めることにより第１軸および第２軸のサーボ系応答位置をそれぞれ求める。そして、第１軸および第２軸のサーボ系応答位置を各可動軸方向の成分とするベクトルをサーボ系応答位置ベクトルとし、このサーボ系応答位置ベクトルが描く軌跡をサーボ系応答軌跡Ｘｒとする。
【００２５】
規範モデル部１２の出力は、規範モデル部１２を微分方程式、差分方程式、伝達関数等の形式で表し、数値計算によりその解を計算することで求めることができる。また、指令形状が既知である場合は、積分計算により解析解を求めることができる。
例えば、図２に示した規範モデル部１２の伝達関数Ｇｍ（ｓ）は次式で表される。
【００２６】
【数１】
規範モデル部１２にある入力が与えられたときの出力は、規範モデル部１２の伝達関数と与えられた入力のラプラス変換との積を逆ラプラス変換することにより求められる。時刻ｔにおける第１軸位置指令Ｘｃ１と第２軸位置指令Ｘｃ２がそれぞれｘｃ１（ｔ）、ｘｃ２（ｔ）で与えられたとき、位置指令ベクトルｘｃ（ｔ）は次式で表される。
【００２７】
【数２】
このとき、第１軸および第２軸のサーボ系応答位置ベクトルｘｒ１（ｔ）、ｘｒ２（ｔ）は次式で表される。
【００２８】
【数３】
ただし、Ｌ［ｆ（ｔ）］はｆ（ｔ）のラプラス変換を、Ｌ−１［Ｆ（ｓ）］はＦ（ｓ）の逆ラプラス変換を表す。
また、サーボ系の応答遅れ時間は、規範モデル部１２の伝達関数から求めることができる。ここでは、サーボ系に一定速度の入力が与えられたときの指令に対する応答の定常的な遅れ時間をサーボ系応答遅れ時間とする。図２に示した規範モデル部１２では、サーボ系応答遅れ時間ｔｄは第１モデルゲインＫ１の逆数となり、次式で表される。
【００２９】
【数４】
図３は、本発明の実施の形態１における誤差ベクトル算出部２と補正ベクトル演算部３の動作を説明する図である。ここでは、指令経路がコーナ形状の経路であり、時刻ｔにおいてコーナの頂点となるような場合について記述している。
図３において、サーボ系応答軌跡Ｘｒは破線のような経路をとり、時刻ｔにおいてｘｒ（ｔ）の位置にある。誤差ベクトル算出部２では、時刻ｔにおけるサーボ系応答位置ベクトルｘｒ（ｔ）から、同時刻における位置指令ベクトルｘｃ（ｔ）を減算して、誤差ベクトルｅ（ｔ）を求める。これらを式で表すと、次式となる。
【００３０】
【数５】
また、サーボ系応答軌跡Ｘｒ上の参照点を、サーボ系応答遅れ時間ｔｄだけ先の点ｘｒ（ｔ＋ｔｄ）とする。なお、位置指令の補正をすべての時刻に対して一括して行う場合はサーボ系応答遅れ時間分先の点を求めることは容易であるが、時々刻々の位置指令を逐次的に処理して位置指令の補正を行う場合には、位置指令をサーボ系応答遅れ時間分先読みする必要がある。
補正ベクトル演算部３では、この参照点ｘｒ（ｔ＋ｔｄ）におけるサーボ系応答軌跡Ｘｒの移動方向に垂直な方向のベクトル、すなわち法線ベクトルと、移動方向に同一な方向のベクトル、すなわち接線ベクトルを求め、誤差ベクトルをそれぞれの方向へ射影したベクトル（正射影ベクトル）を求める。
【００３１】
演算の便宜上、法線ベクトルおよび接線ベクトルの長さは、１となるように正規化する。正規化されたベクトルを、それぞれ単位法線ベクトルｒｎ（ｔ）および単位接線ベクトルｒｔ（ｔ）と呼ぶ。
【００３２】
接線ベクトルｒｔ０（ｔ）および単位接線ベクトルｒｔ（ｔ）は、式で表すと次式のようになる。
【００３３】
【数６】
【００３４】
法線ベクトルｒｎ０（ｔ）および単位法線ベクトルｒｎ（ｔ）は、式で表すと次式のようになる。
【００３５】
【数７】
なお、単位法線ベクトルｒｎ（ｔ）および単位接線ベクトルｒｔ（ｔ）の演算にはベクトルの微分演算が必要になるが、これらの演算はベクトルの要素ごとに時刻ｔに関する微分演算を行うことにより行われる。また、制御系を離散時間系で設計する場合は、微分演算を差分近似して実現することとなる。
【００３６】
次に、誤差ベクトルｅを参照点におけるサーボ系応答軌跡Ｘｒの法線ベクトル方向へ射影したベクトルを求める。このベクトルは誤差ベクトルｅのうち軌跡に垂直な方向の誤差すなわち軌跡誤差を表すので、軌跡誤差ベクトルｅｎ（ｔ）とよぶ。軌跡誤差ベクトルｅｎ（ｔ）は、大きさが誤差ベクトルｅの単位法線ベクトル方向成分（すなわち誤差ベクトルｅと単位法線ベクトルｒｎ（ｔ）の内積）であり、方向が単位法線ベクトル方向であるベクトルとなる。
【００３７】
さらに、誤差ベクトルｅを参照点におけるサーボ系応答軌跡Ｘｒの接線ベクトル方向へ射影したベクトルを求める。このベクトルは、誤差ベクトルｅのうち軌跡に沿った方向の誤差すなわち追従誤差を表すので、このベクトルを追従誤差ベクトルｅｔ（ｔ）と呼ぶ。追従誤差ベクトルｅｔ（ｔ）は、大きさが誤差ベクトルｅの単位接線ベクトル方向成分（すなわち誤差ベクトルｅと単位接線ベクトルｒｔ（ｔ）の内積）であり、方向が単位接線ベクトル方向であるベクトルとなる。軌跡誤差ベクトルｅｎ（ｔ）および追従誤差ベクトルｅｔ（ｔ）を式で表すと次式のようになる。
【００３８】
【数８】
【００３９】
【数９】
【００４０】
一般に誤差ベクトルｅとサーボ系応答軌跡Ｘｒの接線ベクトルおよび法線ベクトルはおおむね同一平面上にある。特に、可動軸が２軸のみの場合や、指令経路が２次元平面内にあり、サーボ系の応答が全軸で等しい場合にはこれらのベクトルは完全に同一平面内にある。したがって、誤差ベクトルｅと接線ベクトルがなす平面上にありかつ接線ベクトルに垂直なベクトルを軌跡誤差ベクトルｅｎ（ｔ）としてもよい。この演算は誤差ベクトルｅから追従誤差ベクトルｅｔ（ｔ）を引くことにより行うことができるので、式（８）を用いるよりも簡便に軌跡誤差ベクトルｅｎ（ｔ）を求めることができる。この場合、軌跡誤差ベクトルｅｎ（ｔ）は次式で求められる。
【００４１】
【数１０】
【００４２】
図４は、本発明の実施の形態１における補正ベクトル演算部３の動作を説明する図である。図４において、補正ベクトルΔは、軌跡誤差ベクトルｅｎ（ｔ）に第１の補正ゲインαｎを乗算したベクトルと、追従誤差ベクトルｅｔ（ｔ）に第２の補正ゲインαｔを乗算したベクトルの和で表される。時刻ｔにおける補正ベクトルΔをΔ（ｔ）とすると、補正ベクトルΔ（ｔ）は次式のようになる。
【００４３】
【数１１】
第１補正ゲインαｎは、法線方向の誤差すなわち軌跡誤差を調節するためのゲインである。この第１補正ゲインαｎは通常０と−１の間で設定され、−１に近い値を設定するほど軌跡誤差を小さくすることができる。好ましくは、−１に設定すると、法線方向の誤差ベクトルｅを打ち消すように位置指令を補正するので、軌跡誤差を効果的に抑制することができる。軌跡誤差を抑制することにより、加工形状の本来の形状からの誤差を小さくすることができる。
【００４４】
第２補正ゲインαｔは、接線方向の誤差すなわち追従誤差を調節するためのゲインである。この第２補正ゲインαｔは通常０と−１の間で設定され、−１に近い値を設定するほど追従誤差を小さくすることができる。好ましくは、−１に設定すると、接線方向の誤差ベクトルを打ち消すように位置指令を補正するので、追従誤差を効果的に抑制することができる。追従誤差を抑制することにより、加工に要する時間を短縮することができる。
【００４５】
補正ベクトルΔ（ｔ）を位置指令ベクトルに加算したベクトルが、補正後の位置指令ベクトルとなる。補正ベクトルΔ（ｔ）の加算は第１軸補正量加算部５および第２軸補正量加算部６において各可動軸について行われる。すなわち、補正ベクトルΔ（ｔ）の各可動軸方向の成分が各可動軸の補正量となり、各可動軸の補正量に各可動軸の位置指令を軸ごとに加算した値が、各可動軸の補正後位置指令となる。本実施の形態では可動軸が２軸の場合なので、補正ベクトルΔ（ｔ）の第１要素Δ１（ｔ）および第２要素Δ２（ｔ）が、それぞれ第１軸および第２軸の補正量となる。時刻ｔにおける第１軸補正後位置指令Ｘａ１および第２軸補正後位置指令Ｘａ２がそれぞれｘａ１（ｔ）、ｘａ２（ｔ）で与えられた時、第１軸補正後位置指令ｘａ１（ｔ）および第２軸補正後位置指令ｘａ２（ｔ）はそれぞれ次式で表される。
【００４６】
【数１２】
【００４７】
第１軸サーボ制御部７および第２軸サーボ制御部８では、それぞれ第１軸補正後位置指令ｘａ１（ｔ）および第２軸補正後位置指令ｘａ２（ｔ）に対して図２に示すサーボ制御系により第１軸および第２軸のモータが駆動され、機械系１３の可動部の位置が制御される。
【００４８】
以上のように、本実施の形態１によれば、複数の軸のサーボ系応答により誤差ベクトルｅを求め、サーボ系応答軌跡Ｘｒの移動方向に基づいて求めた補正量により位置指令を補正する。これにより、コーナ形状の指令のように指令経路の方向が急変するような場合でも、サーボ系応答軌跡Ｘｒは滑らかに変化するため、誤差ベクトルｅを滑らかに変化させ、補正ベクトルΔの方向も滑らかに変化させることができる。このため、補正後の位置指令が滑らかとなり、指令形状にかかわらず軌跡誤差と追従誤差を抑制することができる。また、フィードバック信号を用いずに位置指令の補正を行うため、制御系の安定性を常に保証することができる。
【００４９】
また、本実施の形態１によれば、誤差ベクトルｅをサーボ系応答軌跡Ｘｒの垂直成分すなわち法線方向成分と平行成分すなわち接線方向成分に分解して、それぞれにゲインを設定することにより、指令形状にかかわらず、軌跡に垂直な方向の誤差である軌跡誤差と軌跡に平行な方向の誤差である追従誤差とを独立して制御することができる。
【００５０】
また、本実施の形態１によれば、サーボ系の応答遅れを考慮して参照点とそれに基づく補正ベクトルΔの方向を決定することにより、簡単な演算で比較的正確な補正ベクトルΔの方向を決定することができる。
【００５１】
さらに、本実施の形態１によれば、サーボ制御部１１に規範モデル部１２を用いた２自由度制御器を用いることで、制御対象の特性にかかわらずサーボ制御部１１の応答を規範モデル部１２の応答から正確に算出することができ、さらにこの規範モデル部１２を用いて応答軌跡を求めて位置指令を補正することにより、サーボ制御部１１の応答性に起因して生じる軌跡誤差を正確に補正することができる。
【００５２】
本実施の形態１では、可動軸の数が２としていたが、可動軸の数が３の場合であってもよい。サーボ系応答軌跡ベクトル、誤差ベクトル、補正ベクトルを２次元ではなく３次元のベクトルとすることで、同様の補正を行うことができる。
【００５３】
実施の形態２．
実施の形態２の構成は実施の形態１と概略同一である。相違点は、補正ベクトル演算部３における参照点および補正ベクトルの決定方式である。以下、これらの相違点について説明する。サーボ系応答軌跡算出部１の動作は、実施の形態１と同様である。
【００５４】
図５は、本発明の実施の形態２における誤差ベクトル算出部２と補正ベクトル演算部３の動作を説明する図である。図５において、誤差ベクトル算出部２における誤差ベクトルｅ（ｔ）の算出は実施の形態１と同様であり、式（５）で求められる。実施の形態２では、ある時刻ｔにおける参照点を、その時刻ｔにおける位置指令からサーボ系応答軌跡Ｘｒへ下ろした垂線とサーボ系応答軌跡Ｘｒとの交点と定める。
【００５５】
このときの参照点の位置は、幾何学的な関係からいくつかの方法により求めることができる。一例を挙げると、サーボ系応答軌跡Ｘｒの接線ベクトルが参照点と位置指令を結ぶ直線と直交するように参照点の位置を定める方法がある。すなわち、サーボ系応答軌跡Ｘｒを時刻ｔの関数で表した場合、その接線ベクトルと、位置指令から参照点へのベクトルとの内積が０となる時刻ｔ＋ｔｒを求め、その時刻におけるサーボ系応答軌跡Ｘｒ上の点ｘｒ（ｔ＋ｔｒ）を参照点とする方法である。このことを式で表すと次式のようになる。
【００５６】
【数１３】
【００５７】
ただし、ｘｒ’はサーボ系応答軌跡Ｘｒの接線ベクトルｄｘｒ／ｄｔを表す。参照点における時刻ｔ＋ｔｒは、上記の（１３）式の方程式を解くことにより求められる。
【００５８】
次に、参照点におけるサーボ系応答軌跡Ｘｒの法線ベクトル方向を求める。この場合、位置指令から参照点へのベクトルの方向と、参照点におけるサーボ系応答軌跡Ｘｒの法線方向は、ともに参照点におけるサーボ系応答軌跡Ｘｒの接線方向に直交する。従って、参照点におけるサーボ系応答軌跡Ｘｒの法線ベクトル方向は位置指令と参照点を結ぶ直線の方向と同一の方向となるので、単位法線ベクトルｒｎ（ｔ）は位置指令から参照点までの方向のベクトルを正規化することにより求められる。このことを式で表すと、次式となる。
【００５９】
【数１４】
【００６０】
図６は、本発明の実施の形態２における補正ベクトル演算部３の動作を説明する図である。図６において、補正ベクトルΔ（ｔ）は、誤差ベクトルｅ（ｔ）を単位法線ベクトル方向へ射影したベクトルである軌跡誤差ベクトルｅｎ（ｔ）に、補正ゲインαｎをかけて求められる。軌跡誤差ベクトルｅｎ（ｔ）は、実施の形態１と同様に誤差ベクトルと単位法線ベクトルを用いて式（８）で求められる。補正ベクトルΔ（ｔ）は、補正ゲインと軌跡誤差ベクトルを用いて次式で表される。
【００６１】
【数１５】
【００６２】
補正ゲインαｎは、法線方向の誤差すなわち軌跡誤差を調節するためのゲインである。このゲインは通常０と−１の間で設定され、−１に近い値を設定するほど軌跡誤差を小さくすることができる。好ましくは、−１に設定すると、法線方向の誤差ベクトルを打ち消すように位置指令を補正するので、軌跡誤差を効果的に抑制することができる。軌跡誤差を抑制することにより、加工形状の本来の形状からの誤差を小さくすることができる。
【００６３】
誤差ベクトルｅに基づく各可動軸の補正量の演算と、第１軸補正量加算部５および第２軸補正量加算部６における動作は実施の形態１と同様である。また、第１軸サーボ制御部７および第２軸サーボ制御部８の動作は、実施の形態１と同様である。
【００６４】
以上のように、本実施の形態２によれば、複数の軸のサーボ系応答により誤差ベクトルｅを求め、サーボ系応答軌跡Ｘｒの移動方向に基づいて求めた補正量により位置指令を補正する。これにより、コーナ形状の指令のように指令経路の方向が急変するような場合でも、サーボ系応答軌跡Ｘｒは滑らかに変化するため、誤差ベクトルｅを滑らかに変化させ、補正ベクトルの方向も滑らかに変化させることができる。このため、補正後の位置指令が滑らかとなり、指令形状にかかわらず軌跡誤差と追従誤差を抑制することができる。また、フィードバック信号を用いずに位置指令の補正を行うため、制御系の安定性を常に保証する事ができる。
【００６５】
本実施の形態２によれば、誤差ベクトルｅをサーボ系応答軌跡Ｘｒの垂直成分すなわち法線方向成分にゲインを設定することにより、ひとつのゲインを設定するだけで、指令形状にかかわらず、軌跡誤差を抑制し、加工形状が本来の形状からのずれを抑制することができる。
【００６６】
本実施の形態２によれば、サーボ系応答と指令された位置との幾何学的な関係を考慮して参照点とそれに基づく補正ベクトルの方向を決定することにより、より正確な補正ベクトルの方向を決定することができる。
【００６７】
さらに、本実施の形態２によれば、規範モデル部１２を用いた２自由度制御器を用いることで、制御対象の特性にかかわらずサーボ制御部１１の応答を規範モデルの応答から正確に算出することができ、さらにこの規範モデル部１２を用いて応答軌跡を求めて位置指令を補正することにより、サーボ制御部１１の応答性に起因して生じる軌跡誤差を正確に補正することができる。
【００６８】
本実施の形態２では、可動軸の数が２としていたが、可動軸の数が３の場合であってもよい。サーボ系応答軌跡ベクトル、誤差ベクトル、補正ベクトルを２次元ではなく３次元のベクトルとすることで、同様の補正を行うことができる。
【００６９】
実施の形態３．
図７は、本発明の実施の形態３におけるサーボ系応答軌跡算出部１の動作を説明する図である。図７において、この実施の形態３では、サーボ系モデルを用いてサーボ系応答軌跡Ｘｒを求めるのではなく、位置指令の情報のみを用いて求める。指令形状が円弧などのなめらかに変化する形状である場合、指令経路の加速度方向に加速度の大きさに比例して軌跡誤差が生じる性質がある。この比例係数は、サーボ系の応答性から定める。時刻ｔにおけるサーボ系応答の位置を、同時刻における位置指令に誤差ベクトルｅ（ｔ）を加算することにより求める。誤差ベクトルｅ（ｔ）は、位置指令の加速度ベクトルｄ２ｘｃ／ｄｔ２に、係数ａをかけることにより求める。この係数ａは、サーボ系応答遅れ時間ｔｄの二乗に比例するように定める。この関係式は、指令加速度と軌跡誤差量の関係を実測して求める方法のほか、円弧指令時の軌跡誤差量の理論値と指令加速度の比を解析的に求める方法により設定する。後者の方法では、サーボ系の周波数応答伝達関数の絶対値に指令半径を乗じた値を指令半径から引くことにより円弧指令時の軌跡誤差量を求め、この軌跡誤差量を指令加速度で除することにより求める。半径Ｒ、各速度ωの円弧指令時の場合、軌跡誤差量と指令加速度の比は、サーボ系の伝達関数Ｇ（ｓ）を用いて以下の式で与えられる。
【００７０】
【数１６】
ここで、ｊは虚数単位である。
【００７１】
図２に示すサーボ制御系は伝達関数が式（１）で表され、サーボ系応答遅れ時間は式（４）で表される。第２モデルゲインＫ２が第１モデルゲインＫ１の４倍である場合、すなわちＫ２＝４Ｋ１である場合の軌跡誤差量とサーボ系応答遅れ時間との関係を式（１６）を用いて求めると、次式のようになる。
【００７２】
【数１７】
誤差ベクトル算出部２、補正ベクトル演算部３、各軸補正量演算部４、第１軸補正量加算部５、第２軸補正量加算部６、第１軸サーボ制御部７、第２軸サーボ制御部８の動作は、実施の形態２と同様である。
【００７３】
以上のように、本実施の形態３によれば、複数の軸のサーボ系応答により誤差ベクトルｅを求め、サーボ系応答軌跡Ｘｒの移動方向に基づいて求めた補正量により位置指令を補正する。これにより、コーナ形状の指令のように指令経路の方向が急変するような場合でも、サーボ系応答軌跡Ｘｒは滑らかに変化するため、誤差ベクトルｅを滑らかに変化させ、補正ベクトルの方向も滑らかに変化させることができる。このため、補正後の位置指令が滑らかとなり、指令形状にかかわらず軌跡誤差と追従誤差を抑制することができる。また、フィードバック信号を用いずに位置指令の補正を行うため、制御系の安定性を常に保証することができる。
【００７４】
本実施の形態３によれば、誤差ベクトルｅをサーボ系応答軌跡Ｘｒの垂直成分すなわち法線方向成分にゲインを設定することにより、ひとつのゲインを設定するだけで、指令形状にかかわらず、軌跡誤差を抑制し、加工形状が本来の形状からのずれを抑制することができる。
【００７５】
本実施の形態３によれば、サーボ制御部１１のモデルを用いずに、位置指令軌跡の情報だけでサーボ系応答軌跡Ｘｒを求めることにより、サーボ系応答軌跡Ｘｒを求めるための演算負荷を軽減することができる。
【００７６】
本実施の形態３では、可動軸の数が２としていたが、可動軸の数が３の場合であってもよい。サーボ系応答軌跡ベクトル、誤差ベクトル、補正ベクトルを２次元ではなく３次元のベクトルとすることで、同様の補正を行うことができる。
【産業上の利用可能性】
【００７７】
以上のように本発明に係る軌跡制御装置は、コーナ形状のように指令経路の方向が急峻に変化するような場合でも駆動系に過大な負荷や振動を生じさせずに、また制御系の安定性を損なうことなく軌跡誤差を抑制することができ、工作機械やレーザ加工機等において指令形状にかかわらず高速高精度加工を実現する方法に適している。
【符号の説明】
【００７８】
１ サーボ系応答軌跡算出部
２ 誤差ベクトル算出部
３ 補正ベクトル演算部
４ 各軸補正量演算部
５ 第１軸補正量加算部
６ 第２軸補正量加算部
７ 第１軸サーボ制御部
８ 第２軸サーボ制御部
１０ 軌跡制御装置
１１ サーボ制御部
１２ 規範モデル部
１３ 機械系
１４ 第１軸モータ
１５ 第２軸モータ
１６ 第１軸負荷
１７ 第２軸負荷
２０、２２、２６ 減算器
２１、２３ モデルゲイン乗算器
２４、２５ 積分器
３１ 加算器
２７ 位置制御器
２８ 加減算器
２９ 速度制御器
３０ 乗算器
３２ モータ
３３ 負荷
【特許請求の範囲】
【請求項１】
機械の可動部が複数の可動軸によって駆動され、前記複数の可動軸のモータを同時制御することにより前記可動部の軌跡を制御する軌跡制御装置において、
前記複数の可動軸のうちの少なくとも２以上の可動軸の位置指令に基づいてサーボ系応答軌跡を演算するサーボ系応答軌跡算出部と、
前記位置指令と前記サーボ系応答軌跡とに基づいて誤差ベクトルを演算する誤差ベクトル算出部と、
前記誤差ベクトルと前記サーボ系応答軌跡の所定の参照点における移動方向に基づいて補正ベクトルを演算する補正ベクトル演算部と、
前記補正ベクトルを前記２以上の可動軸に分配して各可動軸の補正量を演算する各軸補正量演算部と、
前記各可動軸の補正量をそれぞれ対応する可動軸の位置指令に加算して前記各可動軸ごとに補正後位置指令を求めるために前記各可動軸ごとに設けられた補正量加算部と、
前記各可動軸の位置がそれぞれの前記補正後位置指令に追従するように各可動軸のモータ駆動トルクを出力することにより各可動軸のモータを制御するために前記各可動軸ごとに設けられたサーボ制御部を備えることを特徴とする軌跡制御装置。
【請求項２】
前記補正ベクトル演算部は、前記誤差ベクトルを前記参照点における前記サーボ系応答軌跡の移動方向に垂直な方向に射影したベクトルに第１補正ゲインを乗じたベクトルと、前記誤差ベクトルを前記参照点における前記サーボ系応答軌跡の移動方向と同一の方向に射影したベクトルに第２補正ゲインを乗じたベクトルの和を補正ベクトルとして出力することを特徴とする請求項１記載の軌跡制御装置。
【請求項３】
前記補正ベクトル演算部は、前記誤差ベクトルを前記参照点における前記サーボ系応答軌跡の移動方向に垂直な方向に射影したベクトルに補正ゲインを乗じたベクトルを補正ベクトルとして出力することを特徴とする請求項１記載の軌跡制御装置。
【請求項４】
前記補正ベクトル演算部における参照点は、サーボ系の応答遅れ時間だけ先の時刻における前記サーボ系応答軌跡上の点であることを特徴とする請求項１から３のいずれか１項に記載の軌跡制御装置。
【請求項５】
前記補正ベクトル演算部における参照点は、指令位置を通り前記サーボ系応答軌跡に垂直な直線と、前記サーボ系応答軌跡との交点であることを特徴とする請求項１から３のいずれか１項に記載の軌跡制御装置。
【請求項６】
前記サーボ制御部は規範モデル部を用いた２自由度制御器を有し、前記サーボ系応答軌跡算出部は、前記サーボ制御部で用いられている前記規範モデル部と同一のモデルに前記位置指令を与えたときの出力軌跡をサーボ系応答軌跡として出力することを特徴とする請求項１から５のいずれか１項に記載の軌跡制御装置。
【請求項７】
前記サーボ系応答軌跡算出部は、各時刻における前記位置指令の加速度ベクトルにサーボ制御部の応答性で決まる所定の係数を乗じたベクトルを前記位置指令の軌跡から差し引いた軌跡をサーボ系応答軌跡として出力することを特徴とする請求項１から５のいずれか１項に記載の軌跡制御装置。
【請求項１】
機械の可動部が複数の可動軸によって駆動され、前記複数の可動軸のモータを同時制御することにより前記可動部の軌跡を制御する軌跡制御装置において、
前記複数の可動軸のうちの少なくとも２以上の可動軸の位置指令に基づいてサーボ系応答軌跡を演算するサーボ系応答軌跡算出部と、
前記位置指令と前記サーボ系応答軌跡とに基づいて誤差ベクトルを演算する誤差ベクトル算出部と、
前記誤差ベクトルと前記サーボ系応答軌跡の所定の参照点における移動方向に基づいて補正ベクトルを演算する補正ベクトル演算部と、
前記補正ベクトルを前記２以上の可動軸に分配して各可動軸の補正量を演算する各軸補正量演算部と、
前記各可動軸の補正量をそれぞれ対応する可動軸の位置指令に加算して前記各可動軸ごとに補正後位置指令を求めるために前記各可動軸ごとに設けられた補正量加算部と、
前記各可動軸の位置がそれぞれの前記補正後位置指令に追従するように各可動軸のモータ駆動トルクを出力することにより各可動軸のモータを制御するために前記各可動軸ごとに設けられたサーボ制御部を備えることを特徴とする軌跡制御装置。
【請求項２】
前記補正ベクトル演算部は、前記誤差ベクトルを前記参照点における前記サーボ系応答軌跡の移動方向に垂直な方向に射影したベクトルに第１補正ゲインを乗じたベクトルと、前記誤差ベクトルを前記参照点における前記サーボ系応答軌跡の移動方向と同一の方向に射影したベクトルに第２補正ゲインを乗じたベクトルの和を補正ベクトルとして出力することを特徴とする請求項１記載の軌跡制御装置。
【請求項３】
前記補正ベクトル演算部は、前記誤差ベクトルを前記参照点における前記サーボ系応答軌跡の移動方向に垂直な方向に射影したベクトルに補正ゲインを乗じたベクトルを補正ベクトルとして出力することを特徴とする請求項１記載の軌跡制御装置。
【請求項４】
前記補正ベクトル演算部における参照点は、サーボ系の応答遅れ時間だけ先の時刻における前記サーボ系応答軌跡上の点であることを特徴とする請求項１から３のいずれか１項に記載の軌跡制御装置。
【請求項５】
前記補正ベクトル演算部における参照点は、指令位置を通り前記サーボ系応答軌跡に垂直な直線と、前記サーボ系応答軌跡との交点であることを特徴とする請求項１から３のいずれか１項に記載の軌跡制御装置。
【請求項６】
前記サーボ制御部は規範モデル部を用いた２自由度制御器を有し、前記サーボ系応答軌跡算出部は、前記サーボ制御部で用いられている前記規範モデル部と同一のモデルに前記位置指令を与えたときの出力軌跡をサーボ系応答軌跡として出力することを特徴とする請求項１から５のいずれか１項に記載の軌跡制御装置。
【請求項７】
前記サーボ系応答軌跡算出部は、各時刻における前記位置指令の加速度ベクトルにサーボ制御部の応答性で決まる所定の係数を乗じたベクトルを前記位置指令の軌跡から差し引いた軌跡をサーボ系応答軌跡として出力することを特徴とする請求項１から５のいずれか１項に記載の軌跡制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１１−１４５８８４（Ｐ２０１１−１４５８８４Ａ）
【公開日】平成２３年７月２８日（２０１１．７．２８）
【国際特許分類】
【出願番号】特願２０１０−６０４４（Ｐ２０１０−６０４４）
【出願日】平成２２年１月１４日（２０１０．１．１４）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２３年７月２８日（２０１１．７．２８）
【国際特許分類】
【出願日】平成２２年１月１４日（２０１０．１．１４）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]