WO2013140679A1

WO2013140679A1 - 軌跡制御装置

Info

Publication number: WO2013140679A1
Application number: PCT/JP2012/082097
Authority: WO
Inventors: 弘太朗長岡
Original assignee: 三菱電機株式会社
Priority date: 2012-03-21
Filing date: 2012-12-11
Publication date: 2013-09-26
Also published as: US9757834B2; CN104204977A; JP5762625B2; US20140371899A1; CN104204977B; JPWO2013140679A1; DE112012006074T5

Abstract

　実施の形態の軌跡制御装置（１０）は、複数モータを制御して可動部の軌跡を制御する軌跡制御装置であって、指令経路を補間しそれに沿って加減速演算を実行して加減速後補間経路を求める補間・加減速演算部（１）と、加減速後補間経路から各可動軸の位置指令を生成する軸分配部（３）と、位置指令に基づいてそのサーボ応答を演算するサーボ応答演算部（５１），（５２）と、加減速後補間経路から接線方向サーボ応答を求める接線方向サーボ応答演算部（５０）と、接線方向サーボ応答から基準点を求める基準点生成部（２）と、サーボ応答および基準点に基づいて補正ベクトルを演算し、各可動軸の位置指令を補正ベクトルで補正して各可動軸の補正後位置指令を出力する位置ベクトル補正部（４）と、各可動軸がそれぞれの補正後位置指令を追従するように各可動軸のモータへモータ駆動トルクを出力するサーボ制御部（６），（７）とを備える。

Description

軌跡制御装置

　本発明は、工作機械やレーザ加工機等の制御装置において、軌跡誤差を抑制し高速高精度加工を実現するための軌跡制御装置に関する。

　工作機械やレーザ加工機等の機械を用いて加工を行う場合、工作物に対する工具の位置が指令された経路上を沿うように制御することが行われる。この制御は軌跡制御と呼ばれ、一般に機械の各可動軸の実際の位置が指令経路から求めた各可動軸の位置指令に追従するようにサーボ制御を行うことにより行われる。

　軌跡制御を行う上での問題点として、各可動軸の制御系の応答遅れなどに起因して、実際の軌跡が指令された経路からずれてしまうという問題がある。通常は機械の各可動軸ごとに制御を行うため、各可動軸の制御系の応答遅れなどに起因する誤差により、各可動軸のサーボ系応答が位置指令よりも遅れて移動する。直線のように指令経路の移動方向が変化しない場合には各軸が遅れて移動しても、サーボ系応答の軌跡としては指令経路上から外れない。つまり、指令経路の接線方向に誤差が現れるが、指令経路の法線方向の誤差は現れない。一方、曲線やコーナ形状などのように指令経路の移動方向が変化する場合には、各軸のサーボ制御系の遅れにより、指令経路の法線方向に誤差が現れるようになる。以下では、サーボ系応答位置の位置指令に対する誤差のうち、指令経路の接線方向の成分を追従誤差、指令経路の法線方向の成分を軌跡誤差と呼ぶ。一般に、軌跡誤差があると加工形状が本来の形状と一致しなくなるため、好ましくない。

　これらの軌跡誤差を抑制するための手段として、特許文献１においては、プログラムを先読みして認識した加工形状に基づいて誤差を一定値以下に抑えるための最適送り速度を演算するとともに、この速度で加工を行ったときの誤差量を演算し、その誤差を打ち消すような補正ベクトルをもとの指令位置に加算して指令位置を補正する方法が開示されている。ここで、補正ベクトルの方向は、移動方向に垂直な方向（法線方向）であり、補正ベクトルの長さは法線方向加速度（速度の二乗を曲率半径で除した値)に所定の係数をかけた値としている。

　また、非特許文献１においては、サーボ系の応答誤差を指令経路の接線方向と法線方向とに分解し、それぞれの方向の成分に個別のゲインを乗算して得られた制御入力を用いてフィードバック制御を行うことにより、追従誤差と軌跡誤差とを独立して制御する方法が開示されている。

特開平６－２８２３２１号公報

精密工学会誌　Ｖｏｌ．７４．Ｎｏ．１１．２００８　ｐｐ．１１９３－１１９８

　しかしながら、上記従来の技術による方法では、指令点間の経路における加速度が加減速の影響により変化する場合や、３次元形状も含む複雑な形状を微小線分指令で近似して指令するような場合には対応できないという問題があった。指令経路の始点付近および終点付近や、指令経路上で指令送り速度が変化する付近では、送り速度を急に変更すると制御対象である機械の許容加速度を越えてしまうため、送り速度を漸増または漸減する加減速が一般に行われる。

　例えば、特許文献１では、加減速を行う前の指令点に基づいて演算しており、さらに演算している誤差量は、送り速度が一定の状態（定常状態）での誤差量であるため、加減速中のように送り速度が変化する状態（過渡状態）では、演算した誤差量が実際に生じる誤差量と異なり、補正を行った結果得られる応答軌跡がもとの指令軌跡と異なり、応答軌跡の形状にひずみが生じるといった問題があった。

　また、非特許文献１による方法では、指令経路に垂直な方向あるいは指令経路と同一の方向に制御入力を設定するため、コーナ形状の指令や３次元形状を含む指令では、補正を行う方向や補正の大きさが急激に変化したり不定になったりする。そのような状況で補正を行うと、機械を含む駆動系への過大な負荷や振動が発生するという問題があった。また、非特許文献１では、サーボ系応答誤差をフィードバックするため、制御系が不安定になる可能性があるという問題があった。

　本発明は、上記に鑑みてなされたものであって、軌跡誤差を十分小さく抑制して高速で高精度な加工を実現することが可能な軌跡制御装置を得ることを目的とする。

　上述した課題を解決し、目的を達成するために、本発明は、複数の可動軸のモータを同時制御することにより可動部の軌跡を制御する軌跡制御装置であって、与えられた指令経路を補間するとともに、前記指令経路に沿って加減速演算を実行することにより、加減速後補間経路を求める補間・加減速演算部と、前記加減速後補間経路から各可動軸の位置指令を生成する軸分配部と、各可動軸の前記位置指令に基づいて各可動軸のサーボ応答を演算するサーボ応答演算部と、前記加減速後補間経路に基づいて接線方向移動量の接線方向サーボ応答を求める接線方向サーボ応答演算部と、前記接線方向サーボ応答から補正基準点を求める基準点生成部と、各可動軸の前記サーボ応答および前記補正基準点に基づいて軌跡誤差を抑制するための補正ベクトルを演算し、各可動軸の前記位置指令を前記補正ベクトルで補正することにより各可動軸の補正後位置指令を出力する位置ベクトル補正部と、各可動軸の位置がそれぞれ各可動軸の前記補正後位置指令を追従するように各可動軸の前記モータへそれぞれモータ駆動トルクを出力するサーボ制御部と、を備えることを特徴とする。

　本発明によれば、指令形状が３次元形状であったり微小線分で構成されていたりしても、補正ベクトルを正しく求めることができ、軌跡誤差を十分小さく抑制して高速で高精度な加工を実現することが可能な軌跡制御装置を得るという効果を奏する。

図１は、本発明の実施の形態１にかかる軌跡制御装置の構成を示すブロック図である。図２は、本発明の実施の形態１にかかるサーボ制御部の構成を示すブロック図である。図３－１は、本発明の実施の形態１にかかる加減速演算の詳細を説明する図である。図３－２は、本発明の実施の形態１にかかる接線方向速度ｑ’（ｔ）の加減速パターンを示す図である。図３－３は、本発明の実施の形態１にかかる加減速後補間点から形成される加減速後補間経路を示す図である。図４は、本発明の実施の形態にかかる位置ベクトル補正部の動作を説明する図である。図５は、本発明の実施の形態２にかかる軌跡制御装置の構成を示すブロック図である。

　以下に、本発明にかかる軌跡制御装置の実施の形態を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。

実施の形態１．
　図１は、本発明の実施の形態１にかかる軌跡制御装置１０の構成を示すブロック図である。指令経路はＮＣプログラム等の形式で機械の可動部軌跡の軌跡上の座標値として補間・加減速演算部１に与えられる。また、指令された座標値間の補間方法（直線・円弧・スプライン等）および軌跡に沿った方向の移動速度すなわち送り速度も、ＮＣプログラム等によって同時に補間・加減速演算部１に与えられる。補間・加減速演算部１において、指令された座標値間を指定された方法で補間するとともに、指令経路に沿って別途指定された所定の加速度または加減速時定数で加速および減速させる演算を行うことにより、加減速後補間経路を演算する。

　さらに、加減速後補間経路を与えられた軸分配部３では、機械の可動部が加減速後補間経路を通るように、機械の各可動軸の位置指令を演算する。本実施の形態においては、２つの可動軸があるものとし、第１軸および第２軸への位置指令として第１軸位置指令および第２軸位置指令を演算する。例えば、機械のＸ軸を第１の可動軸（第１軸）、機械のＹ軸を第２の可動軸（第２軸）とおく。第１軸サーボ応答演算部５１および第２軸サーボ応答演算部５２は、それぞれ第１軸位置指令と第２軸位置指令から第１軸サーボ応答と第２軸サーボ応答を演算して出力する。

　接線方向サーボ応答演算部５０は、加減速後補間経路の接線方向移動量から接線方向サーボ応答を演算して出力する。基準点生成部２は、接線方向サーボ応答から基準点の位置を求めて出力する。そして、位置ベクトル補正部４では、第１軸サーボ応答と第２軸サーボ応答と基準点の位置から補正ベクトルを求め、各可動軸の位置指令である第１軸位置指令および第２軸位置指令に補正ベクトルの各可動軸方向の成分を加算することにより各可動軸の補正後位置指令として第１軸補正後位置指令および第２軸補正後位置指令を演算する。第１軸サーボ制御部６および第２軸サーボ制御部７は、第１軸の位置および第２軸の位置が、第１軸補正後位置指令および第２軸補正後位置指令にそれぞれ追従するように、第１軸および第２軸のモータ駆動トルクである第１軸モータ駆動トルクおよび第２軸モータ駆動トルクをそれぞれ出力する。

　第１軸サーボ制御部６および第２軸サーボ制御部７は同一の構成をしており、図２にサーボ制御部１１としてその構成を示すブロック図を示す。サーボ制御部１１へ入力された補正後位置指令は、減算器２０でモデル位置が減算され、モデルゲイン乗算器２１で第１のモデルゲインＫ₁が乗算され、減算器２２でモデル速度が減算され、さらにモデルゲイン乗算器２３で第２のモデルゲインＫ₂が乗算されてモデル加速度が出力される。積分器２４でモデル加速度が積分されてモデル速度が出力され、積分器２５でモデル速度が積分されてモデル位置が出力される。補正後位置指令が入力され、モデル位置・モデル速度・モデル加速度が出力されるブロックを規範モデル部１２と呼ぶ。

　積分器２５が出力したモデル位置からモータ３２が出力したモータ位置信号が加減算器２６で減算されて位置誤差が出力される。位置制御部２７では位置誤差に対して比例制御等の制御を行う。加減算器２８において位置制御部２７の出力にモデル速度が加算され、さらにモータ３２が出力したモータ速度信号が減算されて速度誤差が出力される。速度制御部２９では速度誤差に対して比例・積分制御等の制御を行う。乗算器３０では、モデル加速度に制御対象のイナーシャに相当する値を乗算してモデルトルクが演算され、加算器３１で速度制御部２９の出力にモデルトルクが加算されてモータトルク信号が出力される。モータトルク信号はモータ３２に入力される。モータ３２および負荷３３からなる機械系１３は、モータトルク信号により駆動され、モータ速度信号とモータ位置信号がサーボ制御部１１へ出力される。

　このサーボ制御部１１は規範モデルを用いた２自由度制御器であり、指令に対する追従性と外乱に対する応答性を独立して設計することができる。位置指令に対するモータ位置の追従性は第１のモデルゲインＫ₁および第２のモデルゲインＫ₂で決まり、外乱に対するモータ位置の応答性は位置制御部２７および速度制御部２９の設計で決まる。したがって、モータ位置の応答は、実際の制御対象の特性にかかわらず、規範モデルの出力であるモデル位置に追従するように制御される。

　軌跡制御時に応答軌跡がひずまないようにするため、各可動軸の位置応答は（略）同一にしておく必要がある。位置応答とはサーボ制御部１１の入力からモータ位置信号までの応答特性のことであるが、この応答特性が各可動軸の間で異なっていると、直線形状を指令した際に応答軌跡が指令した直線からはずれたり、円弧指令に対する応答軌跡が斜め方向にひずんだりする。本実施の形態では、規範モデルのゲインを第１軸サーボ制御部６および第２軸サーボ制御部７で同じ値に設定する。すなわち、第１軸サーボ制御部６の規範モデルの第１のモデルゲインと第２軸サーボ制御部７の規範モデルの第１のモデルゲインとを同じ値に設定し、第１軸サーボ制御部６の規範モデルの第２のモデルゲインと第２軸サーボ制御部７の規範モデルの第２のモデルゲインとを同じ値に設定するようにする。モータ位置の応答は規範モデルの特性で決まるため、全く同一の規範モデルを用いれば同じ位置応答が得られる。

　つぎに、各部の演算の詳細について説明する。補間・加減速演算部１は、指令された座標値間を指定された方法で補間し、さらに加減速演算を行って、指令経路上における補間周期ごとの指令位置を演算する。補間の方法には、直線補間・円弧補間・スプライン補間などがある。補間周期は、軌跡制御装置１０の仕様として定められた一定の周期であり、一般には数ｍｓ以下の短い周期を用いる。補間周期が短いほうが、精度の高い軌跡制御が可能となるが、演算に用いるプロセッサ等の処理負荷が大きくなる。補間・加減速の結果得られた補間周期ごとの点を加減速後補間点と呼び、加減速後補間点により形成される経路を補間経路と呼ぶ。加減速演算は、指令加速度が機械の各可動軸の許容加速度を超えて過大となるのを防ぐためのものであり、速度の変化が別途パラメータ等で指定された加速度以下になるように補間経路上の補間点を演算しなおすものである。すなわち、始点から終点までを移動する場合において、始点直後は速度が漸増するように補間点間の間隔を短くし、終点直後では速度が漸減するように補間点間の間隔を短くするものである。

　この加減速演算の詳細を、図３－１～図３－３に示す例を用いて説明する。図３－１のように、始点Ｐ₀＝（ｘ₀，ｙ₀）から途中の点Ｐ₁＝（ｘ₁，ｙ₁）およびＰ₂＝（ｘ₂，ｙ₂）を通って終点Ｐ₃＝（ｘ₃，ｙ₃）まで速度Ｆで移動する場合について考える。これらの始点・通過点・終点を指令点と呼ぶ。指令点は図３－１では白色の丸印で表されている。各指令点の座標値は、ＮＣプログラム等により指令されている。始点Ｐ₀から、任意の時刻ｔにおける経路上の点Ｐ（ｔ）までの指令経路に沿った距離をｑ（ｔ）とする。始点から終点までの移動長さをＬとする。Ｌは、Ｐ₀,Ｐ₁,Ｐ₂,Ｐ₃を結ぶ各線分の長さの和となる。Ｐ_n-1からＰ_nまでの線分長をｌ_nとおく（ただしｎ＝１，２，３）と、Ｌ＝ｌ₁＋ｌ₂＋ｌ₃となる。

　次に、ｑ（ｔ）の時間微分を接線方向速度ｑ’（ｔ）として、ｑ’（ｔ）の加減速パターンを図３－２のように定める。これは、高さが指令送り速度Ｆ、面積が始点から終点までの移動長さＬとなる台形状のパターンであり、加速部および減速部の傾きは、別途パラメータ等で指定された加速度となるように定めるものである。ここでは、加速および減速は直線状に行っているが、Ｓ字状にして機械の動きをさらになめらかにする場合もある。接線方向速度ｑ’（ｔ）を時間積分することにより加減速後の移動長さｑ_c（ｔ）が求まる。Δｔを補間周期として、ｔ＝Ｎ×Δｔ（Ｎは正の整数）における加減速後移動長さｑ_c（Ｎ×Δｔ）を求め、始点位置Ｐ₀から指令経路に沿って長さｑ_c（Ｎ×Δｔ）進んだ点が、Ｎ番目の加減速後補間点となる。

　加減速後補間点から形成される加減速後補間経路は、図３－３に示すようになる。加減速後補間経路の情報としては、加減速後の接線方向移動長さｑ_c（ｔ）、補間の種類（直線補間・円弧補間・スプライン補間など）、補間に必要なパラメータ（始点位置、終点位置、その他の形状パラメータ）が含まれる。その他の形状パラメータとしては、円弧補間の場合は中心位置と半径、スプライン補間の場合はスプライン関数の係数といったパラメータがある。

　軸分配部３では、加減速後補間経路上における加減速後補間点の各軸の座標値を求める。図３に示す例の場合、例えば線分Ｐ₁Ｐ₂上にある加減速後補間点Ｐ_cの座標を表すベクトルｐ_c＝（ｘ_c，ｙ_c）は、Ｐ_cが属する線分の始点Ｐ₁を表すベクトルｐ₁＝（ｘ₁，ｙ₁）と終点Ｐ₂を表すベクトルｐ₂＝（ｘ₂，ｙ₂）の間を移動長さｑ_c（ｔ）で定まる比率を用いて直線補間して求められる。したがって、時刻ｔにおける位置指令Ｐ_c（ｔ）の座標値を表す位置指令ベクトルｐ_c（ｔ）＝（ｘ_c（ｔ），ｙ_c（ｔ））は次の式（１）で表される。

以上より、第１軸位置指令はｘ_c（ｔ）、第２軸位置指令はｙ_c（ｔ）となる。

　第１軸サーボ応答演算部５１および第２軸サーボ応答演算部５２では、それぞれ第１軸位置指令および第２軸位置指令に対するサーボ応答（第１軸サーボ応答および第２軸サーボ応答）を演算する。サーボ応答の演算は、以下のように行われる。前述のように、サーボ系の応答は規範モデルの応答で表すことができるので、第１軸および第２軸位置指令が入力として与えられた場合の、規範モデルの出力であるモデル位置を求めることにより第１軸および第２軸のサーボ系応答位置をそれぞれ求める。そして、第１軸および第２軸のサーボ系応答位置を各可動軸方向の成分とするベクトルをサーボ応答位置ベクトルとし、この位置ベクトルが描く軌跡をサーボ応答軌跡とする。

　規範モデルの出力であるモデル位置は、図２に示す規範モデル部１２を微分方程式、差分方程式、伝達関数等の形式で表し、数値計算によりその解を計算することで求めることができる。また、指令形状が既知である場合は、積分計算により解析解を求めることができる。

　例えば、図２に示した規範モデルの伝達関数Ｇｍ（ｓ）は次の式（２）で表される。

規範モデルにある入力が与えられたときの出力は、規範モデルの伝達関数と与えられた入力のラプラス変換との積を逆ラプラス変換することにより求められる。位置指令ベクトルｐ_c（ｔ）に対するサーボ応答位置Ｐ_s（ｔ）のベクトルｐ_s（ｔ）＝（ｘ_s（ｔ），ｙ_s（ｔ））は次の式（３）で表される。
ただし、Ｌ［ｆ（ｔ）］はｆ（ｔ）のラプラス変換を、Ｌ^-1［Ｆ（ｓ）］はＦ（ｓ）の逆ラプラス変換を表す。第１軸のサーボ応答はｘ_s（ｔ）、第２軸のサーボ応答はｙ_s（ｔ）となる。

　第１軸サーボ応答演算部５１および第２軸サーボ応答演算部５２と同様に、接線方向サーボ応答演算部５０では、加減速後の接線方向移動長さｑ_c（ｔ）に対するサーボ応答である接線方向サーボ応答ｑ_s（ｔ）を求める。ただし、位置のサーボ応答を求める場合はベクトルを入力として用いたが、接線方向移動長さの場合は入力はスカラー値となる。接線方向サーボ応答ｑ_s（ｔ）は、規範モデルの伝達関数と与えられた入力ｑ_c（ｔ）のラプラス変換との積を逆ラプラス変換することにより求めることができ、次の式（４）で表される。

　次に、基準点生成部２では、加減速後補間経路に沿って始点から接線方向サーボ応答の長さだけすすんだ点Ｐ_r（ｔ）を求めて基準点（補正基準点）とする。基準点の座標ｐ_r（ｔ）は以下のようにして求められる。まず、接線方向サーボ応答ｑ_s（ｔ）と各指令点間の線分長ｌ_nから、基準点がどの線分に属するかを決定する。すなわち、ｑ_s（ｔ）がｌ₁よりも小さければＰ₀Ｐ₁上、ｑ_s（ｔ）がｌ₁以上かつｌ₁＋ｌ₂未満であればＰ₁Ｐ₂上、というようにして決定する。ここでは、ｑ_s（ｔ）はＰ₁Ｐ₂上にあるものとする。続いて、基準点が属する線分の始点と終点の間を接線方向サーボ応答の長さｑ_r（ｔ）で定まる比率を用いて直線補間することにより、基準点の座標を決定する。基準点の座標ｐ_r（ｔ）＝（ｘ_r（ｔ），ｙ_r（ｔ））は、次の式（５）で求められる。

　位置ベクトル補正部４では、サーボ応答位置Ｐ_s（ｔ）から基準点Ｐ_r（ｔ）までのベクトルを補正ベクトルとして、加減速後の補間点Ｐ_c（ｔ）の位置ベクトルに補正ベクトルを加算したベクトルを補正後の位置ベクトルとする。すなわち、第１軸位置指令ｘ_c（ｔ）および第２軸位置指令ｙ_c（ｔ）に、補正ベクトルの第１軸成分と第２軸成分をそれぞれ加算することで、第１軸補正後位置指令と第２軸補正後位置指令をそれぞれ演算する。

　位置ベクトル補正部における補正ベクトルの関係を図４に示す。補正ベクトルは、サーボ応答位置Ｐ_s（ｔ）から、接線方向サーボ応答を考慮して求めた基準点Ｐ_r（ｔ）へのベクトルであり、これはサーボ応答の指令経路からのずれを逆向きにしたものとなっており、指令された軌跡におおむね垂直な方向となっている。したがって、この補正ベクトルを位置指令（第１軸位置指令ｘ_c（ｔ）および第２軸位置指令ｙ_c（ｔ））に加算することで、サーボ応答の指令経路からのずれを補正することが可能となる。

　接線方向サーボ応答を考慮した基準点を用いない場合、例えばサーボ応答Ｐ_s（ｔ）から位置指令の補間点Ｐ_c（ｔ）までのベクトルを補正ベクトルとして用いると、指令経路に垂直な方向だけではなく、指令経路に平行な方向の誤差成分を含んでしまう。また、サーボ応答Ｐ_s（ｔ）から指令経路に垂線を下ろす方法も考えられるが、そのようにすると鋭角のコーナの内側をサーボ応答が通るような場合に、垂線を下ろす方向の候補が複数生じてしまい補正ベクトルが一意に定まらなくなってしまう。

　以上のように、本実施の形態１によれば、接線方向のサーボ応答を用いて補正の基準点を求め、補間および加減速処理を行ったあとの補間経路情報に基づいて補正ベクトルを演算しているので、指令形状が３次元形状であったり微小線分で構成されていたりしても、補正ベクトルを正しく求めることができる。これにより、軌跡誤差を十分小さく抑制して高速で高精度な加工を実現することが可能な軌跡制御装置を得ることができる。

　また、サーボ応答の演算を実際の可動軸の位置応答と（略）同一の応答特性をもつフィルタを用いて行うことにより、サーボ応答を正確に推測することができる。

　また、可動軸の位置応答サーボ応答の演算を実際の可動軸の位置応答と（略）同一の応答特性をもつフィルタを用いて行うことにより、基準点の位置を正確に推測することができる。

　さらに、指令経路と接線方向長さのサーボ応答を考慮して（補正）基準点を設定することにより、補正ベクトルの方向と長さを正確に決定することができる。

　そして、サーボ応答位置までのベクトルから基準点までのベクトルを補正ベクトルとして位置指令に加算することにより、基準点からサーボ応答位置までの軌跡誤差を正確に補正することができる。

実施の形態２．
　図５は、本発明の実施の形態２にかかる軌跡制御装置１５の構成を示すブロック図である。実施の形態１との相違点は、機械の可動軸が３つあり、それぞれの可動軸についてサーボ制御部（第１軸サーボ制御部６、第２軸サーボ制御部７、および第３軸サーボ制御部８）とサーボ応答演算部（第１軸サーボ応答演算部５１、第２軸サーボ応答演算部５２、および第３軸サーボ応答演算部５３）を設けた点である。それ以外の構成は実施の形態１と同様である。

　本実施の形態では、Ｘ軸・Ｙ軸・Ｚ軸の３つの可動軸を有する機械において３次元形状を含む指令を行う。Ｘ軸を第１軸、Ｙ軸を第２軸、Ｚ軸を第３軸とする。サーボ制御部（第１軸サーボ制御部６、第２軸サーボ制御部７、および第３軸サーボ制御部８）それぞれの規範モデルゲインは、第１軸・第２軸・第３軸で同じ設定にする。

　以下では、実施の形態１と異なる部分について述べる。

　軸分配部３では、加減速後補間経路上における加減速後補間点の各軸の座標値を求める。本実施の形態では、図３－１～図３－３において、各点についてＺ軸の座標を付加する。すなわち、図３－１では、始点Ｐ₀＝（ｘ₀，ｙ₀，ｚ₀）から途中の点Ｐ₁＝（ｘ₁，ｙ₁，ｚ₁）およびＰ₂＝（ｘ₂，ｙ₂，ｚ₂）を通って終点Ｐ₃＝（ｘ₃，ｙ₃，ｚ₃）まで速度Ｆで移動するものとする。線分Ｐ₁Ｐ₂上にある加減速後補間点Ｐ_cの座標を表すベクトルｐ_c＝（ｘ_c，ｙ_c，ｚ_c）は、Ｐ_cが属する線分の始点Ｐ₁を表すベクトルｐ₁＝（ｘ₁，ｙ₁，ｚ₁）と終点Ｐ₂を表すベクトルｐ₂＝（ｘ₂，ｙ₂，ｚ₂）の間を移動長さｑ_c（ｔ）で定まる比率を用いて直線補間して求められる。したがって、時刻tにおける位置指令Ｐ_c（ｔ）の座標値を表す位置指令ベクトルｐ_c（ｔ）＝（ｘ_c（ｔ），ｙ_c（ｔ），ｚ_c（ｔ））は次の式（６）で表される。

以上より、第１軸位置指令はｘ_c（ｔ）、第２軸位置指令はｙ_c（ｔ）、第３軸位置指令はｚ_c（ｔ）となる。

　第３軸サーボ応答演算部５３では、第１軸応答演算部５１および第２軸のサーボ応答演算部５２と同様の演算を行って、第３軸位置指令に対するサーボ応答を演算する。

　位置指令ベクトルｐ_c（ｔ）に対するサーボ応答位置Ｐ_s（ｔ）のベクトルｐ_s（ｔ）＝（ｘ_s（ｔ），ｙ_s（ｔ），ｚ_s（ｔ））は次の式（７）で表される。

第１軸のサーボ応答はｘ_s（ｔ）、第２軸のサーボ応答はｙ_s（ｔ）、第３軸のサーボ応答はz_s（ｔ）となる。

　次に、基準点生成部における基準点の座標ｐ_r（ｔ）＝（ｘ_r（ｔ），ｙ_r（ｔ），ｚ_r（ｔ））は、次の式（８）で求められる。

　本実施の形態２では、ベクトルの要素が２要素から３要素になるだけで、他の演算は実施の形態１とまったく同様に行うことにより、サーボ応答の指令経路からの誤差を補正することができる。

　以上より、本実施の形態２によれば、３次元形状を含む指令であっても、接線方向のサーボ応答を用いて補正の基準点を求め、補間および加減速処理を行ったあとの補間経路情報に基づいて補正ベクトルを演算することで、補正ベクトルを正しく求めることができる。

　さらに、本願発明は上記実施の形態に限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で種々に変形することが可能である。また、上記実施の形態には種々の段階の発明が含まれており、開示される複数の構成要件における適宜な組み合わせにより種々の発明が抽出されうる。例えば、上記実施の形態に示される全構成要件からいくつかの構成要件が削除されても、発明が解決しようとする課題の欄で述べた課題が解決でき、発明の効果の欄で述べられている効果が得られる場合には、この構成要件が削除された構成が発明として抽出されうる。更に、異なる実施の形態にわたる構成要素を適宜組み合わせてもよい。

　以上のように、本発明にかかる軌跡制御装置は、工作機械やレーザ加工機等の制御装置に有用であり、特に、軌跡誤差を抑制した高速高精度加工が要求される制御装置に適している。

　１　補間・加減速演算部、２　基準点生成部、３　軸分配部、４　位置ベクトル補正部、６　第１軸サーボ制御部、７　第２軸サーボ制御部、８　第３軸サーボ制御部、１０，１５　軌跡制御装置、１１　サーボ制御部、１２　規範モデル部、１３　機械系、２０　減算器、２１　モデルゲイン乗算器、２２　減算器、２３　モデルゲイン乗算器、２４，２５　積分器、２６，２８　加減算器、２７　位置制御部、２９　速度制御部、３０　乗算器、３１　加算器、３２　モータ、３３　負荷、５０　接線方向サーボ応答演算部、５１　第１軸サーボ応答演算部、５２　第２軸サーボ応答演算部、５３　第３軸サーボ応答演算部。

Claims

　複数の可動軸のモータを同時制御することにより可動部の軌跡を制御する軌跡制御装置であって、
　与えられた指令経路を補間するとともに、前記指令経路に沿って加減速演算を実行することにより、加減速後補間経路を求める補間・加減速演算部と、
　前記加減速後補間経路から各可動軸の位置指令を生成する軸分配部と、
　各可動軸の前記位置指令に基づいて各可動軸のサーボ応答を演算するサーボ応答演算部と、
　前記加減速後補間経路に基づいて接線方向移動量の接線方向サーボ応答を求める接線方向サーボ応答演算部と、
　前記接線方向サーボ応答から補正基準点を求める基準点生成部と、
　各可動軸の前記サーボ応答および前記補正基準点に基づいて軌跡誤差を抑制するための補正ベクトルを演算し、各可動軸の前記位置指令を前記補正ベクトルで補正することにより各可動軸の補正後位置指令を出力する位置ベクトル補正部と、
　各可動軸の位置がそれぞれ各可動軸の前記補正後位置指令を追従するように各可動軸の前記モータへそれぞれモータ駆動トルクを出力するサーボ制御部と、
　を備えることを特徴とする軌跡制御装置。
　前記サーボ応答演算部は、前記可動軸ごとに独立したサーボ応答演算部を有し、各前記独立したサーボ応答演算部は、入出力間の応答がそれぞれ対応する前記可動軸の位置応答と略同一の応答を有するフィルタである
　ことを特徴とする請求項１に記載の軌跡制御装置。
　前記接線方向サーボ応答演算部は、入出力間の応答が前記可動軸の位置応答と略同一の応答を有するフィルタであり、前記指令経路の始点から加減速後指令位置までの指令経路に沿った長さのスカラー値を入力とする
　ことを特徴とする請求項１に記載の軌跡制御装置。
　前記サーボ制御部は、前記可動軸ごとに独立したサーボ制御部を有し、前記可動軸の前記位置応答は当該可動軸の前記独立したサーボ制御部の入力から当該可動軸のモータの位置信号までの応答特性である
　ことを特徴とする請求項２または３に記載の軌跡制御装置。
　前記可動軸毎の前記位置応答は全て略同一である
　ことを特徴とする請求項２、３または４に記載の軌跡制御装置。
　前記基準点生成部は、前記指令経路の始点から前記加減速後補間経路に沿って前記接線方向サーボ応答により得られる長さだけ進んだ点を前記補正基準点として求める
　ことを特徴とする請求項１～５のいずれか１項に記載の軌跡制御装置。
　前記位置ベクトル補正部は、前記サーボ応答により定まる点から前記補正基準点までのベクトルを前記補正ベクトルとして、前記位置指令により定まる点に前記補正ベクトルを加算した結果を前記補正後位置指令とする
　ことを特徴とする請求項１～６のいずれか１項に記載の軌跡制御装置。