軌跡制御装置
【課題】加減速の影響により送り速度が過渡的に変化するような場合でも軌跡誤差を抑制できる軌跡制御装置を得ること。
【解決手段】軌跡制御装置は、機械の可動部が複数の可動軸により駆動され、前記複数の可動軸を同時制御することにより前記可動部の軌跡を制御する軌跡制御装置であって、与えられた指令経路に対して補間・加減速を行う補間・加減速演算部と、前記補間・加減速された指令経路に応じて、前記複数の可動軸の位置指令を生成する軸分配部と、前記複数の可動軸の位置指令と前記補間・加減速された指令経路とに基づいて、軌跡誤差を補正するための補正ベクトルを演算する補正ベクトル演算部と、前記演算された補正ベクトルを用いて前記複数の可動軸の位置指令を補正する位置指令補正部と、前記複数の可動軸の位置が前記補正された複数の可動軸の位置指令に追従するように、前記複数の可動軸を同時制御するサーボ制御部とを備えている。
【解決手段】軌跡制御装置は、機械の可動部が複数の可動軸により駆動され、前記複数の可動軸を同時制御することにより前記可動部の軌跡を制御する軌跡制御装置であって、与えられた指令経路に対して補間・加減速を行う補間・加減速演算部と、前記補間・加減速された指令経路に応じて、前記複数の可動軸の位置指令を生成する軸分配部と、前記複数の可動軸の位置指令と前記補間・加減速された指令経路とに基づいて、軌跡誤差を補正するための補正ベクトルを演算する補正ベクトル演算部と、前記演算された補正ベクトルを用いて前記複数の可動軸の位置指令を補正する位置指令補正部と、前記複数の可動軸の位置が前記補正された複数の可動軸の位置指令に追従するように、前記複数の可動軸を同時制御するサーボ制御部とを備えている。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、軌跡制御装置に関する。
【背景技術】
【０００２】
工作機械やレーザ加工機等の機械を用いて加工を行う場合、工作物に対する工具の位置が指令された経路上を沿うように制御することが行われる。この制御は、軌跡制御と呼ばれ、機械の各可動軸の実際の位置が指令経路から求められた各可動軸の位置指令に追従するようにサーボ制御を行うことで軌跡制御を行うことが一般的である。
【０００３】
軌跡制御を行う際に、各可動軸の制御系の応答遅れなどに起因して、実際の軌跡が指令された経路からずれてしまうことがある。軌跡制御では、機械の各可動軸ごとに制御を行うため、各可動軸の制御系の応答遅れなどに起因する誤差により、各可動軸のサーボ系応答が位置指令よりも遅れて移動する。直線のように指令経路の移動方向が変化しない場合には、各軸が遅れて移動しても、サーボ系応答の軌跡としては指令経路上から外れない。つまり、指令経路の接線方向に誤差が現れるが、指令経路の法線方向の誤差は現れない。一方、曲線やコーナ形状などのように指令経路の移動方向が変化する場合には、各軸のサーボ制御系の遅れにより、指令経路の法線方向に誤差が現れるようになる。以下では、サーボ系応答位置の位置指令に対する誤差のうち、指令経路の接線方向の成分を追従誤差、指令経路の法線方向の成分を軌跡誤差と呼ぶ。この軌跡誤差があると加工形状が本来の形状と一致しなくなるため、好ましくない。
【０００４】
特許文献１には、数値制御工作機械において、原数値制御プログラムにおける移動指令を先読みして認識した工具先端の移動軌跡の形状に基づいて目標移動軌跡に対する誤差を一定値以下に抑えるための最適送り速度を演算するとともに、この速度で加工を行ったときの目標移動軌跡に対する誤差量を演算し、その誤差量を打ち消すような補正ベクトルに基づいてもとの移動指令を補正することが記載されている。これにより、特許文献１によれば、従来よりも大きな送り速度で加工しながら、従来と同等の加工精度を維持することができるので、加工精度を下げることなく加工時間の短縮を図ることができるとされている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平６−２８２３２１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
特許文献１に記載された技術では、補正ベクトルの方向が、移動方向に垂直な方向（法線方向）であり、補正ベクトルの長さが、法線方向加速度（速度の二乗を曲率半径で除した値）に所定の係数をかけた値になっている。すなわち、特許文献１に記載された技術では、原数値制御プログラムの移動指令（指令点）に基づいて補正を行うため、指令点間の経路における加速度が加減速の影響により変化する場合には対応できない可能性がある。
【０００７】
例えば、指令経路の始点付近および終点付近や、指令経路上で指令送り速度が変化する指令点付近では、送り速度を急に変更すると制御対象である機械の許容加速度を越えてしまうため、送り速度を漸増または漸減する加減速を行う必要がある。特許文献１に記載の技術では、加減速を行う前の指令点に基づいて誤差量を演算して補正しており、さらに演算している誤差量は、送り速度が一定の状態（定常状態）での誤差量である。このため、加減速中のように送り速度が変化する状態（過渡状態）では、演算した誤差量が実際に生じる誤差量と異なり、補正を行った結果得られる応答軌跡がもとの指令経路と異なり、応答軌跡の形状にひずみが生じる傾向にある。
【０００８】
本発明は、上記に鑑みてなされたものであって、加減速の影響により送り速度が過渡的に変化するような場合でも軌跡誤差を抑制できる軌跡制御装置を得ることを目的とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決し、目的を達成するために、本発明の１つの側面にかかる軌跡制御装置は、機械の可動部が複数の可動軸により駆動され、前記複数の可動軸を同時制御することにより前記可動部の軌跡を制御する軌跡制御装置であって、与えられた指令経路に対して補間・加減速を行う補間・加減速演算部と、前記補間・加減速された指令経路に応じて、前記複数の可動軸の位置指令を生成する軸分配部と、前記複数の可動軸の位置指令と前記補間・加減速された指令経路とに基づいて、軌跡誤差を補正するための補正ベクトルを演算する補正ベクトル演算部と、前記演算された補正ベクトルを用いて前記複数の可動軸の位置指令を補正する位置指令補正部と、前記複数の可動軸の位置が前記補正された複数の可動軸の位置指令に追従するように、前記複数の可動軸を同時制御するサーボ制御部とを備えたことを特徴とする。
【発明の効果】
【００１０】
本発明によれば、補間・加減速された指令経路に基づいて補正ベクトルを演算するので、指令経路上における指令点の間で曲率や加速度が変化する場合においても、応答軌跡が指令経路に正確に追従するような補正を行うことができる。したがって、加減速の影響により送り速度が過渡的に変化するような場合でも軌跡誤差を抑制できる。
【図面の簡単な説明】
【００１１】
【図１】図１は、実施の形態にかかる軌跡制御装置の構成を示す図である。
【図２】図２は、実施の形態におけるサーボ制御部の構成を示す図である。
【図３】図３は、実施の形態における補間・加減速演算部の動作を示す図である。
【図４】図４は、接線方向速度および接線方向加速度の時間変化を示す図である。
【図５】図５は、実施の形態における応答軌跡を示す図である。
【図６】図６は、比較例における応答軌跡を示す図である。
【発明を実施するための形態】
【００１２】
以下に、本発明にかかる軌跡制御装置の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１３】
実施の形態．
実施の形態にかかる軌跡制御装置１０について図１を用いて説明する。図１は、軌跡制御装置１０の構成を示すブロック図である。
【００１４】
機械の可動部が複数の可動軸により駆動される場合に、軌跡制御装置１０は、複数の可動軸のモータを同時制御することによりその可動部の軌跡を制御する装置である。すなわち、軌跡制御装置１０は、工作機械やレーザ加工機等の制御装置において、指令経路にかかわらず軌跡誤差を抑制し高速高精度加工を実現するための装置である。軌跡誤差は、指令経路に対して実際の応答軌跡がどれだけずれたかを示す誤差である。指令経路は、ＮＣプログラム等の形式で機械の可動部の軌跡上の座標値として与えられる。また、指令された座標値間の補間方法（直線・円弧・スプライン等）および軌跡に沿った方向の移動速度すなわち送り速度も、ＮＣプログラム等によって同時に与えられる。
【００１５】
補間・加減速演算部１は、外部から指令経路を与えられる。補間・加減速演算部１は、指令された座標値間を指定された方法で補間するとともに、指令経路に沿って別途指定された所定の加速度または加減速時定数で加速および減速させる演算を行う。すなわち、補間・加減速演算部１は、与えられた指令経路に対して補間・加減速を行う。これにより、補間・加減速演算部１は、加減速後補間経路（すなわち、補間・加減速された指令経路）を演算して軸分配部３及び補正ベクトル演算部５へ供給する。
【００１６】
軸分配部３は、機械の可動部が加減速後補間経路を通るように、機械の各可動軸の位置指令を演算する。本実施の形態では第１軸及び第２軸の２つの可動軸があるものとし、軸分配部３は、加減速後補間経路に応じて、第１軸の位置指令及び第２軸の位置指令を生成して補正ベクトル演算部５及び位置指令補正部４へ供給する。
【００１７】
補正ベクトル演算部５は、加減速後補間経路を補間・加減速演算部１から受け、第１軸の位置指令及び第２軸の位置指令を軸分配部３から受ける。補正ベクトル演算部５は、加減速後補間経路と各可動軸の位置指令とに基づいて、応答軌跡の指令経路に対する軌跡誤差を補正するための補正ベクトルを演算して位置指令補正部４へ供給する。
【００１８】
位置指令補正部４は、第１軸の位置指令及び第２軸の位置指令を軸分配部３から受け、補正ベクトルを補正ベクトル演算部５から受ける。位置指令補正部４は、各可動軸の位置指令に、補正ベクトルの各可動軸方向の成分を加算する。すなわち、位置指令補正部４は、補正ベクトルを用いて各可動軸の位置指令を補正する。これにより、位置指令補正部４は、各可動軸の補正後位置指令を演算して、第１軸補正後位置指令及び第２軸補正後位置指令をそれぞれ第１軸サーボ制御部６及び第２軸サーボ制御部７へ出力する。
【００１９】
第１軸サーボ制御部６は、第１軸の位置が第１軸補正後位置指令に追従するように、第１軸のモータ駆動トルクを生成して第１軸のモータへ出力する。すなわち、第１軸サーボ制御部６は、第１軸の位置が第１軸補正後位置指令に追従するように、第１軸のモータを制御する。
【００２０】
第２軸サーボ制御部７は、第２軸の位置が第２軸補正後位置指令に追従するように、第２軸のモータ駆動トルクを生成して第２軸のモータへ出力する。すなわち、第２軸サーボ制御部７は、第２軸の位置が第２軸補正後位置指令に追従するように、第２軸のモータを制御する。
【００２１】
第１軸サーボ制御部６および第２軸サーボ制御部７は同様の構成を有しており、そのブロック図を図２に示す。すなわち、第１軸サーボ制御部６および第２軸サーボ制御部７に相当するサーボ制御部１１は、次のような構成要素を有する。
【００２２】
サーボ制御部１１へ入力された補正後位置指令は、減算器２０でモデル位置が減算され、モデルゲイン乗算器２１で第１のモデルゲインＫ１が乗算され、減算器２２でモデル速度が減算され、さらにモデルゲイン乗算器２３で第２のモデルゲインＫ２が乗算されてモデル加速度が生成され乗算器３０へ出力される。また、積分器２４でモデル加速度が積分されてモデル速度が生成され減算器２２、積分器２５、及び加減算器２８へ出力される。積分器２５でモデル速度が積分されてモデル位置が生成され減算器２０及び減算器２６へ出力される。補正後位置指令が入力され、モデル位置・モデル速度・モデル加速度が出力されるブロックを規範モデル１２と呼ぶ。
【００２３】
減算器２６でモデル位置からモータ位置信号が減算されて位置誤差が生成され位置制御器２７へ出力される。位置制御器２７では位置誤差に対して比例制御等の制御が行われその結果が加減算器２８へ出力される。加減算器２８において位置制御器２７からの出力にモデル速度が加算され、さらにモータ速度信号が減算されて速度誤差が生成され速度制御器２９へ出力される。速度制御器２９では速度誤差に対して比例・積分制御等の制御が行われその結果が加算器３１へ出力される。乗算器３０で、モデル加速度に制御対象のイナーシャに相当する値を乗算してモデルトルクが演算され加算器３１へ出力される。加算器３１で速度制御器２９からの出力にモデルトルクが加算されてモータトルク信号が生成され、モータ駆動トルクとしてモータ３２へ出力される。モータ３２および負荷３３を含む機械系１３は、モータ駆動トルクにより駆動される。また、エンコーダ（図示せず）等によりモータ速度信号とモータ位置信号とが取得されサーボ制御部１１へ出力される。
【００２４】
このサーボ制御部１１は、規範モデル１２を用いた２自由度制御器であり、指令に対する追従性と外乱に対する応答性とを独立して設計することができる。位置指令に対するモータ位置の追従性は、第１のモデルゲインＫ１および第２のモデルゲインＫ２で決まり、外乱に対するモータ位置の応答性は、位置制御器２７および速度制御器２９の設計で決まる。したがって、モータ位置の応答は、実際の制御対象（機械系１３）の特性にかかわらず、規範モデル１２の出力であるモデル位置に追従するように制御される。
【００２５】
つぎに、各部の演算の詳細について説明する。補間・加減速演算部１は、指令された座標値間を指定された方法で補間し、さらに加減速演算を行って、指令経路上における補間周期ごとの指令位置を演算する。補間の方法には、直線補間・円弧補間・スプライン補間などがある。補間周期は、軌跡制御装置の仕様として定められた一定の周期であり、一般には数ｍｓ以下の短い周期を用いる。補間周期が短いほうが、精度の高い軌跡制御が可能となるが、演算に用いるプロセッサ等の処理負荷が大きくなる。補間・加減速の結果得られた補間周期ごとの点を加減速後補間点と呼び、加減速後補間点により形成される経路を加減速後補間経路と呼ぶ。加減速演算は、指令加速度が機械の各可動軸の許容加速度を超えて過大となるのを防ぐためのものであり、速度の変化が別途パラメータ等で指定された加速度以下になるように補間経路上の補間点を演算しなおすものである。すなわち、始点から終点までを移動する場合において、始点直後は速度が漸増するように補間点間の間隔を短くし、終点直後では速度が漸減するように補間点間の間隔を短くするものである。この加減速演算の詳細を、図３に示す例を用いて説明する。
【００２６】
図３（ａ）に示すように、始点Ｐ１＝（Ｒ，０）から終点Ｐ２＝（０，Ｒ）まで反時計回りに円弧補間を行う場合について考える。まず、始点Ｐ１から、任意の時刻ｔにおける経路上の点Ｐ（ｔ）までの指令経路に沿った距離をｑ（ｔ）とする。始点から終点までの移動長さをＬとすると、Ｌ＝πＲ／２となる。次に、ｑ（ｔ）の時間微分を接線方向速度ｑ’（ｔ）として、ｑ’（ｔ）の加減速パターンを図３（ｂ）のように定める。これは、高さが指令送り速度Ｆ、面積が始点から終点までの移動長さＬとなる台形状のパターンであり、加速部および減速部の傾きは、別途パラメータ等で指定された加速度となるように定めるものである。ここでは、加速および減速は直線状に行っているが、Ｓ字状にして機械の動きをさらになめらかにする場合もある。接線方向速度ｑ’（ｔ）を時間積分することにより加減速後の移動長さｑ（ｔ）が求まる。Δｔを補間周期として、ｔ＝ＮΔｔ（Ｎは正の整数）における加減速後移動長さｑ（ＮΔｔ）を求め、始点位置Ｐ１から指令経路に沿って長さｑ（ＮΔｔ）進んだ点が、Ｎ番目の加減速後補間点となる。加減速後補間点から形成される加減速後補間経路は、図３（ｃ）のようになる。加減速後補間経路の情報としては、加減速後移動長さｑ（ｔ）、補間の種類（直線補間・円弧補間・スプライン補間など）、補間に必要なパラメータ（始点位置、終点位置、その他の形状パラメータ）が含まれる。その他の形状パラメータとしては、円弧補間の場合は中心位置と半径、スプライン補間の場合はスプライン関数の係数といったパラメータがある。
【００２７】
軸分配部３では、加減速後補間経路上における加減速後補間点の各軸の座標値を求める。図３に示す例の場合、指令円弧経路上における補間点の始点からの偏角θは、移動長さｑ（ｔ）を指令半径Ｒで除した値である（θ＝ｑ（ｔ）／Ｒ）ため、第１軸の位置指令ｘｃ１（ｔ）および第２軸の位置指令ｘｃ２（ｔ）を成分にもつ位置指令ベクトルｘｃ（ｔ）は次式で表される。
【数１】
【００２８】
補正ベクトル演算部５では、各軸の位置指令と、加減速演算で得られた接線方向速度の変化とに応じて、サーボ系応答軌跡の指令経路に対する軌跡誤差ベクトルを演算し、その軌跡誤差ベクトルを打ち消すように補正ベクトルを演算する。軌跡誤差ベクトルは、指令経路の進行方向に垂直な方向に、指令加速度の法線方向成分の大きさに比例して生じる性質がある。この比例係数ｋは、指令加速度と軌跡誤差量との関係を実測して求める方法のほか、円弧指令時の定常状態における軌跡誤差量の理論値と法線方向加速度との比を解析的に求める方法により設定する。後者の方法では、まず円弧指令時の軌跡誤差量を、指令半径からサーボ系の周波数応答伝達関数の絶対値に指令半径を乗じた値を引くことにより求め、この軌跡誤差量を指令加速度で除することにより求める。
【００２９】
式で表すと、下記の式（２）のようになる。半径Ｒ、各速度ωの円弧指令時の場合、軌跡誤差量の指令加速度に対する比例係数ｋは、サーボ系の伝達関数Ｇ（ｓ）を用いて下記の式（２）で与えられる。
【数２】
ここで、ｊは虚数単位である。図２に示したサーボ系では、前述のようにモータ位置の応答は、実際の制御対象（機械系１３）の特性にかかわらず、規範モデル１２の出力であるモデル位置に追従するように制御される。そのため、サーボ系の伝達関数Ｇ（ｓ）は規範モデル１２の伝達関数Ｇｍ（ｓ）に一致する。規範モデル１２の伝達関数Ｇｍ（ｓ）は次の式（３）で表される。
【数３】
【００３０】
第２のモデルゲインＫ２が第１のモデルゲインＫ１の４倍である場合、すなわちＫ２＝４×Ｋ１である場合の、軌跡誤差量の指令加速度に対する比例係数ｋを求めると、次の式（４）のようになる。
【数４】
すなわち、軌跡誤差ベクトルは、法線方向加速度ベクトルａｎ（ｔ）にサーボ系のゲインの二乗に反比例するような係数ｋをかけた値となる。式で表すと、次の式（５）のようになる。
【数５】
法線方向加速度ベクトルａｎ（ｔ）は、指令位置ベクトルの２回微分である指令加速度ベクトルを、指令経路に垂直な方向に射影することにより求められる。式で表すと、次の式（６）となる。
【数６】
ただし、ａ（ｔ）は指令加速度ベクトルｄ２ｘｃ／ｄｔ２であり、ｖは指令速度ベクトルｄｘｃ／ｄｔである。
【００３１】
送り速度が変化しない場合は、この軌跡誤差ベクトルを打ち消すために、軌跡誤差ベクトルの符号を反転させたベクトルを補正ベクトルとして演算すればよい。この軌跡誤差ベクトルを第１の補正ベクトルｃ１とし、次の式（７）で表す。
【数７】
【００３２】
一方、送り速度が変化する場合には、補正ベクトルの長さが理想的な長さと異なってくる。このずれは、接線方向速度の変化率すなわち接線方向加速度に概ね比例する性質がある。そこで、補間・加減速演算部１で演算した接線方向速度ｑ’（ｔ）を微分して接線方向加速度ｑ’’（ｔ）＝ｄ２ｑ／ｄｔ２を求め、長さがこの接線方向加速度に比例し、方向が第１の補正ベクトルと一致するような第２の補正ベクトルを求める。第２の補正ベクトルを式で表すと、次の式（８）のようになる。
【数８】
接線方向加速度に対する比例係数α（所定係数）は調整パラメータとし、第１の補正ベクトルのみで補正を行った場合の接線方向加速度と軌跡とのずれ量の比率に基づいて調整を行う。さらに、応答軌跡の指令経路に対する軌跡誤差量が小さくなるように調整を行っても良い。ここで、加減速に伴う補正ベクトルの長さの理想的な長さのずれは、加速時と減速時とで異なった大きさとなる。そこで、接線方向加速度に対する比例係数αは、加速時と減速時とで異なった値を有するように設定する。加速時か減速時かは、接線方向加速度ｑ’’（ｔ）が正のときは加速、負のときは減速として判断することができる。また、注意点として、第１の補正ベクトルの大きさが０の場合、第２の補正ベクトルの計算式である式（８）は、分母が０となり計算不能となるが、この場合は法線方向加速度が０、すなわち軌跡誤差ベクトルｅ（ｔ）が０である場合であり、本来補正を行う必要がない。そこで、第１の補正ベクトルの大きさが０の場合は、第２の補正ベクトルは０とする。以上で得られた第１の補正ベクトルと第２の補正ベクトルとの和を補正ベクトルとする。
【００３３】
また、位置指令補正部４においては、加減速後補間経路の形状が直線である場合には、補正ベクトルを０とする。すなわち、補間の種類が直線補間であり、移動長さが十分長い場合には、補正ベクトルを０とする。これは、直線部分で加減速する場合には、本来軌跡誤差が生じないため、接線方向加速度に比例する補正量を加算すると逆に軌跡誤差が生じてしまうためである。また、補間や加減速演算の丸め誤差などの影響で、実際には法線方向加速度が０の場合であっても法線方向加速度が０でないと判定され、第２の補正ベクトルが加算されてしまい補正が過剰となる可能性があるが、もとの経路情報を利用することで過剰な補正を防ぐことができ、より正確に補正を行うことができる。
【００３４】
位置指令補正部４では、第１軸位置指令および第２軸位置指令に、補正ベクトルの第１軸成分と第２軸成分をそれぞれ加算することで、第１軸補正後位置指令と第２軸補正後位置指令とをそれぞれ演算する。
【００３５】
次に、本実施の形態による効果を、数値シミュレーションの結果を用いて説明する。
【００３６】
半径５ｍｍ、送り速度６ｍ／ｍｉｎ（＝０．１ｍ／ｓ）の反時計回りの円弧指令を与えた場合の応答軌跡をシミュレーションにより求める。接線方向加速度は２ｍ／ｓ２とし、サーボ制御部１１の規範モデルゲインＫ１は１００ｒａｄ／ｓとした。図４（ａ）はその際の接線方向速度の時間波形を示し、図４（ｂ）は接線方向加速度の時間波形を示す。接線方向加速度は、図４（ｂ）に示すように、始点付近の加速中は正の値、中間付近の定常状態では０、終点付近の減速中は負の値となる。
【００３７】
図６は、比較例の方法で補正を行った場合の応答軌跡である。この比較例では、加減速を行う前の指令位置に対して軌跡誤差を打ち消すような補正量を加算して指令位置の補正を行った後に、補間・加減速を行っている。半径方向に軌跡誤差を拡大してプロットしており、１目盛りが１０μｍの軌跡誤差に相当する。始点付近・終点付近を除いた定常状態における軌跡誤差はほぼ０となっているが、始点・終点付近では４０μｍの軌跡誤差が生じている。
【００３８】
一方、図５は、本実施の形態で補正を行った場合の応答軌跡を示している。ここでは、接線方向加速度に対する係数αは、加速時は３×１０−６、減速時は２×１０−６とした。図６より、始点・終点付近の軌跡誤差が５μｍ程度に抑制されていることがわかる。
【００３９】
以上のように、実施の形態では、補間・加減速演算部１が指令経路に対して補間・加減速を行い、補正ベクトル演算部５が、補間・加減速された指令経路（加減速後補間経路）に基づいて補正ベクトルを演算する。これにより、指令経路上における指令点の間で曲率や加速度が変化する場合においても、応答軌跡が指令経路に正確に追従するような補正を行うことができる。したがって、加減速の影響により送り速度が過渡的に変化するような場合でも軌跡誤差を抑制できる。
【００４０】
また、実施の形態では、補正ベクトル演算部５が、第１軸位置指令及び第２軸位置指令と、加減速後補間経路における接線方向の移動量の変化とに基づいて、軌跡誤差ベクトルを演算し、その軌跡誤差ベクトルを打ち消すように第１の補正ベクトルを演算し、第１の補正ベクトルを用いて補正ベクトルを演算する。これにより、指令経路上における指令点の間で曲率や加速度が変化する場合においても、軌跡誤差を打ち消すように補正を行うことができ、応答軌跡が指令経路に正確に追従するような補正を行うことができる。
【００４１】
また、実施の形態では、補正ベクトル演算部５が、第１の補正ベクトルに加えて、第１の補正ベクトルと同じ方向を有するとともに加減速後補間経路における接線方向加速度に所定係数を乗じた長さを有する第２の補正ベクトルを演算し、第１の補正ベクトルと第２の補正ベクトルとを加算することで補正ベクトルを演算する。このため、加減速により送り速度が変化する場合にも、応答軌跡が指令経路により正確に追従するような補正を行うことができる。
【００４２】
また、実施の形態では、第２の補正ベクトルの長さを接線方向加速度の所定係数倍とする。これにより、加減速部分においてもより正確に補正を行うことができ、軌跡に生じるひずみを抑制することができる。
【００４３】
さらに、実施の形態では、第２の補正ベクトルの演算時に、加速時と減速時とで接線方向加速度に乗ずる係数を変える。これにより、加減時と減速時とでサーボ系の特性によって軌跡のひずみ方が異なる場合にも、正確に補正を行うことができる。
【００４４】
また、直線部分で加減速する場合には、本来軌跡誤差が生じないため、接線方向加速度に比例する補正量を加算すると逆に軌跡誤差が生じてしまうが、実施の形態では、このような場合には補正を行わないようにする。すなわち、位置指令補正部４は、補間・加減速された指令経路（加減速後補間経路）に基づいて、指令経路が曲線か直線かを判断し、直線と判断した場合に補正ベクトルを０とする。これにより、直線部分においても軌跡精度を保つことができる。また、補間や加減速演算の丸め誤差などの影響で、第２の補正ベクトルが過剰に出力される可能性があるが、もとの経路情報を利用することで第２の補正ベクトルが過剰となるのを防ぎ、より正確に補正を行うことができる。
【００４５】
なお、実施の形態では、可動軸の数が２（第１軸及び第２軸）としていたが、可動軸の数が３以上の場合であってもよい。サーボ系応答軌跡ベクトル、誤差ベクトル、補正ベクトルを２次元ではなく３以上の次元のベクトルとすることで、同様の補正を行うことができる。
【００４６】
また、本実施の形態では、軌跡誤差ベクトルを法線方向指令加速度から演算したが、応答軌跡をシミュレーションにより求め、指令経路との間のずれから軌跡誤差を求めるようにしてもよい。このようにすることで、プロセッサの演算負荷は必要となるものの、より正確に軌跡誤差ベクトルを求めることが可能となる。
【産業上の利用可能性】
【００４７】
以上のように、本発明にかかる軌跡制御装置は、工作機械やレーザ加工機等の機械の稼動部の軌跡を制御することに有用である。
【符号の説明】
【００４８】
１ 補間・加減速演算部
３ 軸分配部
４ 位置指令補正部
５ 補正ベクトル演算部
６ 第１軸サーボ制御部
７ 第２軸サーボ制御部
１０ 軌跡制御装置
１１ サーボ制御部
１２ 規範モデル
１３ 機械系
２０ 減算器
２１ モデルゲイン乗算器
２２ 減算器
２３ モデルゲイン乗算器
２４ 積分器
２５ 積分器
２６ 減算器
２７ 位置制御器
２８ 加減算器
２９ 速度制御器
３０ 乗算器
３１ 加算器
３２ モータ
３３ 負荷
【技術分野】
【０００１】
本発明は、軌跡制御装置に関する。
【背景技術】
【０００２】
工作機械やレーザ加工機等の機械を用いて加工を行う場合、工作物に対する工具の位置が指令された経路上を沿うように制御することが行われる。この制御は、軌跡制御と呼ばれ、機械の各可動軸の実際の位置が指令経路から求められた各可動軸の位置指令に追従するようにサーボ制御を行うことで軌跡制御を行うことが一般的である。
【０００３】
軌跡制御を行う際に、各可動軸の制御系の応答遅れなどに起因して、実際の軌跡が指令された経路からずれてしまうことがある。軌跡制御では、機械の各可動軸ごとに制御を行うため、各可動軸の制御系の応答遅れなどに起因する誤差により、各可動軸のサーボ系応答が位置指令よりも遅れて移動する。直線のように指令経路の移動方向が変化しない場合には、各軸が遅れて移動しても、サーボ系応答の軌跡としては指令経路上から外れない。つまり、指令経路の接線方向に誤差が現れるが、指令経路の法線方向の誤差は現れない。一方、曲線やコーナ形状などのように指令経路の移動方向が変化する場合には、各軸のサーボ制御系の遅れにより、指令経路の法線方向に誤差が現れるようになる。以下では、サーボ系応答位置の位置指令に対する誤差のうち、指令経路の接線方向の成分を追従誤差、指令経路の法線方向の成分を軌跡誤差と呼ぶ。この軌跡誤差があると加工形状が本来の形状と一致しなくなるため、好ましくない。
【０００４】
特許文献１には、数値制御工作機械において、原数値制御プログラムにおける移動指令を先読みして認識した工具先端の移動軌跡の形状に基づいて目標移動軌跡に対する誤差を一定値以下に抑えるための最適送り速度を演算するとともに、この速度で加工を行ったときの目標移動軌跡に対する誤差量を演算し、その誤差量を打ち消すような補正ベクトルに基づいてもとの移動指令を補正することが記載されている。これにより、特許文献１によれば、従来よりも大きな送り速度で加工しながら、従来と同等の加工精度を維持することができるので、加工精度を下げることなく加工時間の短縮を図ることができるとされている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平６−２８２３２１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
特許文献１に記載された技術では、補正ベクトルの方向が、移動方向に垂直な方向（法線方向）であり、補正ベクトルの長さが、法線方向加速度（速度の二乗を曲率半径で除した値）に所定の係数をかけた値になっている。すなわち、特許文献１に記載された技術では、原数値制御プログラムの移動指令（指令点）に基づいて補正を行うため、指令点間の経路における加速度が加減速の影響により変化する場合には対応できない可能性がある。
【０００７】
例えば、指令経路の始点付近および終点付近や、指令経路上で指令送り速度が変化する指令点付近では、送り速度を急に変更すると制御対象である機械の許容加速度を越えてしまうため、送り速度を漸増または漸減する加減速を行う必要がある。特許文献１に記載の技術では、加減速を行う前の指令点に基づいて誤差量を演算して補正しており、さらに演算している誤差量は、送り速度が一定の状態（定常状態）での誤差量である。このため、加減速中のように送り速度が変化する状態（過渡状態）では、演算した誤差量が実際に生じる誤差量と異なり、補正を行った結果得られる応答軌跡がもとの指令経路と異なり、応答軌跡の形状にひずみが生じる傾向にある。
【０００８】
本発明は、上記に鑑みてなされたものであって、加減速の影響により送り速度が過渡的に変化するような場合でも軌跡誤差を抑制できる軌跡制御装置を得ることを目的とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決し、目的を達成するために、本発明の１つの側面にかかる軌跡制御装置は、機械の可動部が複数の可動軸により駆動され、前記複数の可動軸を同時制御することにより前記可動部の軌跡を制御する軌跡制御装置であって、与えられた指令経路に対して補間・加減速を行う補間・加減速演算部と、前記補間・加減速された指令経路に応じて、前記複数の可動軸の位置指令を生成する軸分配部と、前記複数の可動軸の位置指令と前記補間・加減速された指令経路とに基づいて、軌跡誤差を補正するための補正ベクトルを演算する補正ベクトル演算部と、前記演算された補正ベクトルを用いて前記複数の可動軸の位置指令を補正する位置指令補正部と、前記複数の可動軸の位置が前記補正された複数の可動軸の位置指令に追従するように、前記複数の可動軸を同時制御するサーボ制御部とを備えたことを特徴とする。
【発明の効果】
【００１０】
本発明によれば、補間・加減速された指令経路に基づいて補正ベクトルを演算するので、指令経路上における指令点の間で曲率や加速度が変化する場合においても、応答軌跡が指令経路に正確に追従するような補正を行うことができる。したがって、加減速の影響により送り速度が過渡的に変化するような場合でも軌跡誤差を抑制できる。
【図面の簡単な説明】
【００１１】
【図１】図１は、実施の形態にかかる軌跡制御装置の構成を示す図である。
【図２】図２は、実施の形態におけるサーボ制御部の構成を示す図である。
【図３】図３は、実施の形態における補間・加減速演算部の動作を示す図である。
【図４】図４は、接線方向速度および接線方向加速度の時間変化を示す図である。
【図５】図５は、実施の形態における応答軌跡を示す図である。
【図６】図６は、比較例における応答軌跡を示す図である。
【発明を実施するための形態】
【００１２】
以下に、本発明にかかる軌跡制御装置の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１３】
実施の形態．
実施の形態にかかる軌跡制御装置１０について図１を用いて説明する。図１は、軌跡制御装置１０の構成を示すブロック図である。
【００１４】
機械の可動部が複数の可動軸により駆動される場合に、軌跡制御装置１０は、複数の可動軸のモータを同時制御することによりその可動部の軌跡を制御する装置である。すなわち、軌跡制御装置１０は、工作機械やレーザ加工機等の制御装置において、指令経路にかかわらず軌跡誤差を抑制し高速高精度加工を実現するための装置である。軌跡誤差は、指令経路に対して実際の応答軌跡がどれだけずれたかを示す誤差である。指令経路は、ＮＣプログラム等の形式で機械の可動部の軌跡上の座標値として与えられる。また、指令された座標値間の補間方法（直線・円弧・スプライン等）および軌跡に沿った方向の移動速度すなわち送り速度も、ＮＣプログラム等によって同時に与えられる。
【００１５】
補間・加減速演算部１は、外部から指令経路を与えられる。補間・加減速演算部１は、指令された座標値間を指定された方法で補間するとともに、指令経路に沿って別途指定された所定の加速度または加減速時定数で加速および減速させる演算を行う。すなわち、補間・加減速演算部１は、与えられた指令経路に対して補間・加減速を行う。これにより、補間・加減速演算部１は、加減速後補間経路（すなわち、補間・加減速された指令経路）を演算して軸分配部３及び補正ベクトル演算部５へ供給する。
【００１６】
軸分配部３は、機械の可動部が加減速後補間経路を通るように、機械の各可動軸の位置指令を演算する。本実施の形態では第１軸及び第２軸の２つの可動軸があるものとし、軸分配部３は、加減速後補間経路に応じて、第１軸の位置指令及び第２軸の位置指令を生成して補正ベクトル演算部５及び位置指令補正部４へ供給する。
【００１７】
補正ベクトル演算部５は、加減速後補間経路を補間・加減速演算部１から受け、第１軸の位置指令及び第２軸の位置指令を軸分配部３から受ける。補正ベクトル演算部５は、加減速後補間経路と各可動軸の位置指令とに基づいて、応答軌跡の指令経路に対する軌跡誤差を補正するための補正ベクトルを演算して位置指令補正部４へ供給する。
【００１８】
位置指令補正部４は、第１軸の位置指令及び第２軸の位置指令を軸分配部３から受け、補正ベクトルを補正ベクトル演算部５から受ける。位置指令補正部４は、各可動軸の位置指令に、補正ベクトルの各可動軸方向の成分を加算する。すなわち、位置指令補正部４は、補正ベクトルを用いて各可動軸の位置指令を補正する。これにより、位置指令補正部４は、各可動軸の補正後位置指令を演算して、第１軸補正後位置指令及び第２軸補正後位置指令をそれぞれ第１軸サーボ制御部６及び第２軸サーボ制御部７へ出力する。
【００１９】
第１軸サーボ制御部６は、第１軸の位置が第１軸補正後位置指令に追従するように、第１軸のモータ駆動トルクを生成して第１軸のモータへ出力する。すなわち、第１軸サーボ制御部６は、第１軸の位置が第１軸補正後位置指令に追従するように、第１軸のモータを制御する。
【００２０】
第２軸サーボ制御部７は、第２軸の位置が第２軸補正後位置指令に追従するように、第２軸のモータ駆動トルクを生成して第２軸のモータへ出力する。すなわち、第２軸サーボ制御部７は、第２軸の位置が第２軸補正後位置指令に追従するように、第２軸のモータを制御する。
【００２１】
第１軸サーボ制御部６および第２軸サーボ制御部７は同様の構成を有しており、そのブロック図を図２に示す。すなわち、第１軸サーボ制御部６および第２軸サーボ制御部７に相当するサーボ制御部１１は、次のような構成要素を有する。
【００２２】
サーボ制御部１１へ入力された補正後位置指令は、減算器２０でモデル位置が減算され、モデルゲイン乗算器２１で第１のモデルゲインＫ１が乗算され、減算器２２でモデル速度が減算され、さらにモデルゲイン乗算器２３で第２のモデルゲインＫ２が乗算されてモデル加速度が生成され乗算器３０へ出力される。また、積分器２４でモデル加速度が積分されてモデル速度が生成され減算器２２、積分器２５、及び加減算器２８へ出力される。積分器２５でモデル速度が積分されてモデル位置が生成され減算器２０及び減算器２６へ出力される。補正後位置指令が入力され、モデル位置・モデル速度・モデル加速度が出力されるブロックを規範モデル１２と呼ぶ。
【００２３】
減算器２６でモデル位置からモータ位置信号が減算されて位置誤差が生成され位置制御器２７へ出力される。位置制御器２７では位置誤差に対して比例制御等の制御が行われその結果が加減算器２８へ出力される。加減算器２８において位置制御器２７からの出力にモデル速度が加算され、さらにモータ速度信号が減算されて速度誤差が生成され速度制御器２９へ出力される。速度制御器２９では速度誤差に対して比例・積分制御等の制御が行われその結果が加算器３１へ出力される。乗算器３０で、モデル加速度に制御対象のイナーシャに相当する値を乗算してモデルトルクが演算され加算器３１へ出力される。加算器３１で速度制御器２９からの出力にモデルトルクが加算されてモータトルク信号が生成され、モータ駆動トルクとしてモータ３２へ出力される。モータ３２および負荷３３を含む機械系１３は、モータ駆動トルクにより駆動される。また、エンコーダ（図示せず）等によりモータ速度信号とモータ位置信号とが取得されサーボ制御部１１へ出力される。
【００２４】
このサーボ制御部１１は、規範モデル１２を用いた２自由度制御器であり、指令に対する追従性と外乱に対する応答性とを独立して設計することができる。位置指令に対するモータ位置の追従性は、第１のモデルゲインＫ１および第２のモデルゲインＫ２で決まり、外乱に対するモータ位置の応答性は、位置制御器２７および速度制御器２９の設計で決まる。したがって、モータ位置の応答は、実際の制御対象（機械系１３）の特性にかかわらず、規範モデル１２の出力であるモデル位置に追従するように制御される。
【００２５】
つぎに、各部の演算の詳細について説明する。補間・加減速演算部１は、指令された座標値間を指定された方法で補間し、さらに加減速演算を行って、指令経路上における補間周期ごとの指令位置を演算する。補間の方法には、直線補間・円弧補間・スプライン補間などがある。補間周期は、軌跡制御装置の仕様として定められた一定の周期であり、一般には数ｍｓ以下の短い周期を用いる。補間周期が短いほうが、精度の高い軌跡制御が可能となるが、演算に用いるプロセッサ等の処理負荷が大きくなる。補間・加減速の結果得られた補間周期ごとの点を加減速後補間点と呼び、加減速後補間点により形成される経路を加減速後補間経路と呼ぶ。加減速演算は、指令加速度が機械の各可動軸の許容加速度を超えて過大となるのを防ぐためのものであり、速度の変化が別途パラメータ等で指定された加速度以下になるように補間経路上の補間点を演算しなおすものである。すなわち、始点から終点までを移動する場合において、始点直後は速度が漸増するように補間点間の間隔を短くし、終点直後では速度が漸減するように補間点間の間隔を短くするものである。この加減速演算の詳細を、図３に示す例を用いて説明する。
【００２６】
図３（ａ）に示すように、始点Ｐ１＝（Ｒ，０）から終点Ｐ２＝（０，Ｒ）まで反時計回りに円弧補間を行う場合について考える。まず、始点Ｐ１から、任意の時刻ｔにおける経路上の点Ｐ（ｔ）までの指令経路に沿った距離をｑ（ｔ）とする。始点から終点までの移動長さをＬとすると、Ｌ＝πＲ／２となる。次に、ｑ（ｔ）の時間微分を接線方向速度ｑ’（ｔ）として、ｑ’（ｔ）の加減速パターンを図３（ｂ）のように定める。これは、高さが指令送り速度Ｆ、面積が始点から終点までの移動長さＬとなる台形状のパターンであり、加速部および減速部の傾きは、別途パラメータ等で指定された加速度となるように定めるものである。ここでは、加速および減速は直線状に行っているが、Ｓ字状にして機械の動きをさらになめらかにする場合もある。接線方向速度ｑ’（ｔ）を時間積分することにより加減速後の移動長さｑ（ｔ）が求まる。Δｔを補間周期として、ｔ＝ＮΔｔ（Ｎは正の整数）における加減速後移動長さｑ（ＮΔｔ）を求め、始点位置Ｐ１から指令経路に沿って長さｑ（ＮΔｔ）進んだ点が、Ｎ番目の加減速後補間点となる。加減速後補間点から形成される加減速後補間経路は、図３（ｃ）のようになる。加減速後補間経路の情報としては、加減速後移動長さｑ（ｔ）、補間の種類（直線補間・円弧補間・スプライン補間など）、補間に必要なパラメータ（始点位置、終点位置、その他の形状パラメータ）が含まれる。その他の形状パラメータとしては、円弧補間の場合は中心位置と半径、スプライン補間の場合はスプライン関数の係数といったパラメータがある。
【００２７】
軸分配部３では、加減速後補間経路上における加減速後補間点の各軸の座標値を求める。図３に示す例の場合、指令円弧経路上における補間点の始点からの偏角θは、移動長さｑ（ｔ）を指令半径Ｒで除した値である（θ＝ｑ（ｔ）／Ｒ）ため、第１軸の位置指令ｘｃ１（ｔ）および第２軸の位置指令ｘｃ２（ｔ）を成分にもつ位置指令ベクトルｘｃ（ｔ）は次式で表される。
【数１】
【００２８】
補正ベクトル演算部５では、各軸の位置指令と、加減速演算で得られた接線方向速度の変化とに応じて、サーボ系応答軌跡の指令経路に対する軌跡誤差ベクトルを演算し、その軌跡誤差ベクトルを打ち消すように補正ベクトルを演算する。軌跡誤差ベクトルは、指令経路の進行方向に垂直な方向に、指令加速度の法線方向成分の大きさに比例して生じる性質がある。この比例係数ｋは、指令加速度と軌跡誤差量との関係を実測して求める方法のほか、円弧指令時の定常状態における軌跡誤差量の理論値と法線方向加速度との比を解析的に求める方法により設定する。後者の方法では、まず円弧指令時の軌跡誤差量を、指令半径からサーボ系の周波数応答伝達関数の絶対値に指令半径を乗じた値を引くことにより求め、この軌跡誤差量を指令加速度で除することにより求める。
【００２９】
式で表すと、下記の式（２）のようになる。半径Ｒ、各速度ωの円弧指令時の場合、軌跡誤差量の指令加速度に対する比例係数ｋは、サーボ系の伝達関数Ｇ（ｓ）を用いて下記の式（２）で与えられる。
【数２】
ここで、ｊは虚数単位である。図２に示したサーボ系では、前述のようにモータ位置の応答は、実際の制御対象（機械系１３）の特性にかかわらず、規範モデル１２の出力であるモデル位置に追従するように制御される。そのため、サーボ系の伝達関数Ｇ（ｓ）は規範モデル１２の伝達関数Ｇｍ（ｓ）に一致する。規範モデル１２の伝達関数Ｇｍ（ｓ）は次の式（３）で表される。
【数３】
【００３０】
第２のモデルゲインＫ２が第１のモデルゲインＫ１の４倍である場合、すなわちＫ２＝４×Ｋ１である場合の、軌跡誤差量の指令加速度に対する比例係数ｋを求めると、次の式（４）のようになる。
【数４】
すなわち、軌跡誤差ベクトルは、法線方向加速度ベクトルａｎ（ｔ）にサーボ系のゲインの二乗に反比例するような係数ｋをかけた値となる。式で表すと、次の式（５）のようになる。
【数５】
法線方向加速度ベクトルａｎ（ｔ）は、指令位置ベクトルの２回微分である指令加速度ベクトルを、指令経路に垂直な方向に射影することにより求められる。式で表すと、次の式（６）となる。
【数６】
ただし、ａ（ｔ）は指令加速度ベクトルｄ２ｘｃ／ｄｔ２であり、ｖは指令速度ベクトルｄｘｃ／ｄｔである。
【００３１】
送り速度が変化しない場合は、この軌跡誤差ベクトルを打ち消すために、軌跡誤差ベクトルの符号を反転させたベクトルを補正ベクトルとして演算すればよい。この軌跡誤差ベクトルを第１の補正ベクトルｃ１とし、次の式（７）で表す。
【数７】
【００３２】
一方、送り速度が変化する場合には、補正ベクトルの長さが理想的な長さと異なってくる。このずれは、接線方向速度の変化率すなわち接線方向加速度に概ね比例する性質がある。そこで、補間・加減速演算部１で演算した接線方向速度ｑ’（ｔ）を微分して接線方向加速度ｑ’’（ｔ）＝ｄ２ｑ／ｄｔ２を求め、長さがこの接線方向加速度に比例し、方向が第１の補正ベクトルと一致するような第２の補正ベクトルを求める。第２の補正ベクトルを式で表すと、次の式（８）のようになる。
【数８】
接線方向加速度に対する比例係数α（所定係数）は調整パラメータとし、第１の補正ベクトルのみで補正を行った場合の接線方向加速度と軌跡とのずれ量の比率に基づいて調整を行う。さらに、応答軌跡の指令経路に対する軌跡誤差量が小さくなるように調整を行っても良い。ここで、加減速に伴う補正ベクトルの長さの理想的な長さのずれは、加速時と減速時とで異なった大きさとなる。そこで、接線方向加速度に対する比例係数αは、加速時と減速時とで異なった値を有するように設定する。加速時か減速時かは、接線方向加速度ｑ’’（ｔ）が正のときは加速、負のときは減速として判断することができる。また、注意点として、第１の補正ベクトルの大きさが０の場合、第２の補正ベクトルの計算式である式（８）は、分母が０となり計算不能となるが、この場合は法線方向加速度が０、すなわち軌跡誤差ベクトルｅ（ｔ）が０である場合であり、本来補正を行う必要がない。そこで、第１の補正ベクトルの大きさが０の場合は、第２の補正ベクトルは０とする。以上で得られた第１の補正ベクトルと第２の補正ベクトルとの和を補正ベクトルとする。
【００３３】
また、位置指令補正部４においては、加減速後補間経路の形状が直線である場合には、補正ベクトルを０とする。すなわち、補間の種類が直線補間であり、移動長さが十分長い場合には、補正ベクトルを０とする。これは、直線部分で加減速する場合には、本来軌跡誤差が生じないため、接線方向加速度に比例する補正量を加算すると逆に軌跡誤差が生じてしまうためである。また、補間や加減速演算の丸め誤差などの影響で、実際には法線方向加速度が０の場合であっても法線方向加速度が０でないと判定され、第２の補正ベクトルが加算されてしまい補正が過剰となる可能性があるが、もとの経路情報を利用することで過剰な補正を防ぐことができ、より正確に補正を行うことができる。
【００３４】
位置指令補正部４では、第１軸位置指令および第２軸位置指令に、補正ベクトルの第１軸成分と第２軸成分をそれぞれ加算することで、第１軸補正後位置指令と第２軸補正後位置指令とをそれぞれ演算する。
【００３５】
次に、本実施の形態による効果を、数値シミュレーションの結果を用いて説明する。
【００３６】
半径５ｍｍ、送り速度６ｍ／ｍｉｎ（＝０．１ｍ／ｓ）の反時計回りの円弧指令を与えた場合の応答軌跡をシミュレーションにより求める。接線方向加速度は２ｍ／ｓ２とし、サーボ制御部１１の規範モデルゲインＫ１は１００ｒａｄ／ｓとした。図４（ａ）はその際の接線方向速度の時間波形を示し、図４（ｂ）は接線方向加速度の時間波形を示す。接線方向加速度は、図４（ｂ）に示すように、始点付近の加速中は正の値、中間付近の定常状態では０、終点付近の減速中は負の値となる。
【００３７】
図６は、比較例の方法で補正を行った場合の応答軌跡である。この比較例では、加減速を行う前の指令位置に対して軌跡誤差を打ち消すような補正量を加算して指令位置の補正を行った後に、補間・加減速を行っている。半径方向に軌跡誤差を拡大してプロットしており、１目盛りが１０μｍの軌跡誤差に相当する。始点付近・終点付近を除いた定常状態における軌跡誤差はほぼ０となっているが、始点・終点付近では４０μｍの軌跡誤差が生じている。
【００３８】
一方、図５は、本実施の形態で補正を行った場合の応答軌跡を示している。ここでは、接線方向加速度に対する係数αは、加速時は３×１０−６、減速時は２×１０−６とした。図６より、始点・終点付近の軌跡誤差が５μｍ程度に抑制されていることがわかる。
【００３９】
以上のように、実施の形態では、補間・加減速演算部１が指令経路に対して補間・加減速を行い、補正ベクトル演算部５が、補間・加減速された指令経路（加減速後補間経路）に基づいて補正ベクトルを演算する。これにより、指令経路上における指令点の間で曲率や加速度が変化する場合においても、応答軌跡が指令経路に正確に追従するような補正を行うことができる。したがって、加減速の影響により送り速度が過渡的に変化するような場合でも軌跡誤差を抑制できる。
【００４０】
また、実施の形態では、補正ベクトル演算部５が、第１軸位置指令及び第２軸位置指令と、加減速後補間経路における接線方向の移動量の変化とに基づいて、軌跡誤差ベクトルを演算し、その軌跡誤差ベクトルを打ち消すように第１の補正ベクトルを演算し、第１の補正ベクトルを用いて補正ベクトルを演算する。これにより、指令経路上における指令点の間で曲率や加速度が変化する場合においても、軌跡誤差を打ち消すように補正を行うことができ、応答軌跡が指令経路に正確に追従するような補正を行うことができる。
【００４１】
また、実施の形態では、補正ベクトル演算部５が、第１の補正ベクトルに加えて、第１の補正ベクトルと同じ方向を有するとともに加減速後補間経路における接線方向加速度に所定係数を乗じた長さを有する第２の補正ベクトルを演算し、第１の補正ベクトルと第２の補正ベクトルとを加算することで補正ベクトルを演算する。このため、加減速により送り速度が変化する場合にも、応答軌跡が指令経路により正確に追従するような補正を行うことができる。
【００４２】
また、実施の形態では、第２の補正ベクトルの長さを接線方向加速度の所定係数倍とする。これにより、加減速部分においてもより正確に補正を行うことができ、軌跡に生じるひずみを抑制することができる。
【００４３】
さらに、実施の形態では、第２の補正ベクトルの演算時に、加速時と減速時とで接線方向加速度に乗ずる係数を変える。これにより、加減時と減速時とでサーボ系の特性によって軌跡のひずみ方が異なる場合にも、正確に補正を行うことができる。
【００４４】
また、直線部分で加減速する場合には、本来軌跡誤差が生じないため、接線方向加速度に比例する補正量を加算すると逆に軌跡誤差が生じてしまうが、実施の形態では、このような場合には補正を行わないようにする。すなわち、位置指令補正部４は、補間・加減速された指令経路（加減速後補間経路）に基づいて、指令経路が曲線か直線かを判断し、直線と判断した場合に補正ベクトルを０とする。これにより、直線部分においても軌跡精度を保つことができる。また、補間や加減速演算の丸め誤差などの影響で、第２の補正ベクトルが過剰に出力される可能性があるが、もとの経路情報を利用することで第２の補正ベクトルが過剰となるのを防ぎ、より正確に補正を行うことができる。
【００４５】
なお、実施の形態では、可動軸の数が２（第１軸及び第２軸）としていたが、可動軸の数が３以上の場合であってもよい。サーボ系応答軌跡ベクトル、誤差ベクトル、補正ベクトルを２次元ではなく３以上の次元のベクトルとすることで、同様の補正を行うことができる。
【００４６】
また、本実施の形態では、軌跡誤差ベクトルを法線方向指令加速度から演算したが、応答軌跡をシミュレーションにより求め、指令経路との間のずれから軌跡誤差を求めるようにしてもよい。このようにすることで、プロセッサの演算負荷は必要となるものの、より正確に軌跡誤差ベクトルを求めることが可能となる。
【産業上の利用可能性】
【００４７】
以上のように、本発明にかかる軌跡制御装置は、工作機械やレーザ加工機等の機械の稼動部の軌跡を制御することに有用である。
【符号の説明】
【００４８】
１ 補間・加減速演算部
３ 軸分配部
４ 位置指令補正部
５ 補正ベクトル演算部
６ 第１軸サーボ制御部
７ 第２軸サーボ制御部
１０ 軌跡制御装置
１１ サーボ制御部
１２ 規範モデル
１３ 機械系
２０ 減算器
２１ モデルゲイン乗算器
２２ 減算器
２３ モデルゲイン乗算器
２４ 積分器
２５ 積分器
２６ 減算器
２７ 位置制御器
２８ 加減算器
２９ 速度制御器
３０ 乗算器
３１ 加算器
３２ モータ
３３ 負荷
【特許請求の範囲】
【請求項１】
機械の可動部が複数の可動軸により駆動され、前記複数の可動軸を同時制御することにより前記可動部の軌跡を制御する軌跡制御装置であって、
与えられた指令経路に対して補間・加減速を行う補間・加減速演算部と、
前記補間・加減速された指令経路に応じて、前記複数の可動軸の位置指令を生成する軸分配部と、
前記複数の可動軸の位置指令と前記補間・加減速された指令経路とに基づいて、軌跡誤差を補正するための補正ベクトルを演算する補正ベクトル演算部と、
前記演算された補正ベクトルを用いて前記複数の可動軸の位置指令を補正する位置指令補正部と、
前記複数の可動軸の位置が前記補正された複数の可動軸の位置指令に追従するように、前記複数の可動軸を同時制御するサーボ制御部と、
を備えたことを特徴とする軌跡制御装置。
【請求項２】
前記補正ベクトル演算部は、前記複数の可動軸の位置指令と、前記補間・加減速された指令経路における接線方向の移動量の変化とに基づいて、軌跡誤差ベクトルを演算し、前記軌跡誤差ベクトルを打ち消すように第１の補正ベクトルを演算し、前記第１の補正ベクトルを用いて前記補正ベクトルを演算する
ことを特徴とする請求項１に記載の軌跡制御装置。
【請求項３】
前記補正ベクトル演算部は、前記第１の補正ベクトルに、前記第１の補正ベクトルと同じ方向を有するとともに接線方向加速度に所定係数を乗じた長さを有する第２の補正ベクトルを加算して、前記補正ベクトルを演算する
ことを特徴とする請求項２に記載の軌跡制御装置。
【請求項４】
前記所定係数は、加速時と減速時とで異なる値を有する
ことを特徴とする請求項３に記載の軌跡制御装置。
【請求項５】
前記位置指令補正部は、前記補間・加減速された指令経路に基づいて、指令経路が曲線か直線かを判断し、直線と判断した場合に前記補正ベクトルを０とする
ことを特徴とする請求項１から４のいずれか１項に記載の軌跡制御装置。
【請求項１】
機械の可動部が複数の可動軸により駆動され、前記複数の可動軸を同時制御することにより前記可動部の軌跡を制御する軌跡制御装置であって、
与えられた指令経路に対して補間・加減速を行う補間・加減速演算部と、
前記補間・加減速された指令経路に応じて、前記複数の可動軸の位置指令を生成する軸分配部と、
前記複数の可動軸の位置指令と前記補間・加減速された指令経路とに基づいて、軌跡誤差を補正するための補正ベクトルを演算する補正ベクトル演算部と、
前記演算された補正ベクトルを用いて前記複数の可動軸の位置指令を補正する位置指令補正部と、
前記複数の可動軸の位置が前記補正された複数の可動軸の位置指令に追従するように、前記複数の可動軸を同時制御するサーボ制御部と、
を備えたことを特徴とする軌跡制御装置。
【請求項２】
前記補正ベクトル演算部は、前記複数の可動軸の位置指令と、前記補間・加減速された指令経路における接線方向の移動量の変化とに基づいて、軌跡誤差ベクトルを演算し、前記軌跡誤差ベクトルを打ち消すように第１の補正ベクトルを演算し、前記第１の補正ベクトルを用いて前記補正ベクトルを演算する
ことを特徴とする請求項１に記載の軌跡制御装置。
【請求項３】
前記補正ベクトル演算部は、前記第１の補正ベクトルに、前記第１の補正ベクトルと同じ方向を有するとともに接線方向加速度に所定係数を乗じた長さを有する第２の補正ベクトルを加算して、前記補正ベクトルを演算する
ことを特徴とする請求項２に記載の軌跡制御装置。
【請求項４】
前記所定係数は、加速時と減速時とで異なる値を有する
ことを特徴とする請求項３に記載の軌跡制御装置。
【請求項５】
前記位置指令補正部は、前記補間・加減速された指令経路に基づいて、指令経路が曲線か直線かを判断し、直線と判断した場合に前記補正ベクトルを０とする
ことを特徴とする請求項１から４のいずれか１項に記載の軌跡制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１２−２３４３９０（Ｐ２０１２−２３４３９０Ａ）
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願番号】特願２０１１−１０２８８７（Ｐ２０１１−１０２８８７）
【出願日】平成２３年５月２日（２０１１．５．２）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願日】平成２３年５月２日（２０１１．５．２）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]