JP5935958B2

JP5935958B2 - 軌跡制御装置

Info

Publication number: JP5935958B2
Application number: JP2015556628A
Authority: JP
Inventors: 弘太朗長岡
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2014-01-07
Filing date: 2014-09-17
Publication date: 2016-06-15
Anticipated expiration: 2034-09-17
Also published as: CN105900027B; JPWO2015104736A1; DE112014006119B4; US20160282846A1; CN105900027A; WO2015104736A1; US10481580B2; DE112014006119T5

Description

この発明は、加工機の制御装置において、軌跡を制御して高速高精度加工を実現するための軌跡制御装置に関するものである。

工作機械、レーザ加工機等の加工機を用いて加工を行う場合、加工の対象物である工作物（ワーク）に対する工具あるいはレーザノズルが搭載された加工ヘッドの位置が指令された経路に沿って動作するように制御される。この制御は軌跡制御と呼ばれ、一般に機械の各可動軸の実際の位置が指令経路から求めた各可動軸の指令位置に追従するようにサーボ制御を行うことにより行われる。

軌跡制御中に同時に制御する可動軸の軸数は、加工機の用途によって異なるが、一般的には２軸から５軸である。また、これに限るものではなく、可動軸の軸数が６軸以上となる場合もある。２軸の例として、板金レーザ加工機がある。これは、板状の素材の面上でレーザノズルを搭載した加工ヘッドを２つの直進軸を用いて２次元で走査し、加工プログラムで指定されたタイミングでレーザビームを照射することにより、工作物に対し所定の形状を創成するものである。

次に、３軸の例の代表的なものとして立形マシニングセンタが挙げられる。これは、エンドミルなどの工具を搭載した加工ヘッドを鉛直方向に把持し、これを主軸モータで回転させながら３つの直進軸を用いて３次元空間内を移動させることで、金型、部品などの所定の３次元形状を創成するものである。さらに、３軸の立形マシニングセンタに回転軸２軸を付加したものが５軸加工機であり、工具の３次元空間内の位置に加えて、工具の姿勢を制御することができる。

いずれの軸数の加工機であっても、数値制御装置が加工プログラムを読み込み、加工プログラムで指令された各点の間を補間して制御周期ごとの各可動軸の指令位置を生成する。各可動軸にそれぞれ備えられたサーボ制御装置によって、各可動軸の実際の位置がそれぞれの可動軸の指令位置に追従するようにサーボ制御が行われる。以下では、数値制御装置と各可動軸のサーボ制御装置を総称して、軌跡制御装置と呼ぶ。

軌跡制御を行う上での問題点として、各可動軸のサーボ制御系の応答遅れに起因して、実際の経路が指令された経路からずれてしまうという点が挙げられる。通常は機械の可動軸ごとに制御を行うため、各可動軸の制御系の応答遅れなどに起因する誤差により、各可動軸のサーボ系応答が指令位置よりも遅れて移動する。例えば直線のように指令経路の移動方向が変化しない場合には各軸が遅れて移動しても、サーボ制御系応答の軌跡としては指令経路上から外れない。つまり、指令経路の接線方向に誤差が現れるが、指令経路の法線方向の誤差は現れない。一方、曲線、コーナー形状などのように指令経路の移動方向が変化する場合には、各軸のサーボ制御系の遅れにより、指令経路の法線方向に誤差が現れるようになる。

以下では、サーボ制御系における応答位置の指令位置に対する誤差のうち、指令経路の接線方向の成分を追従誤差、指令経路の法線方向の成分を軌跡誤差と呼ぶ。一般に、軌跡誤差があると加工形状が本来の形状と一致しなくなるため、好ましくない。
これらの軌跡誤差を抑制するため、特許文献１では、プログラムを先読みして認識した加工形状に基づいて誤差を一定値以下に抑えるための最適送り速度を演算する。この速度で加工を行ったときの誤差量を演算し、その誤差を打ち消すような補正ベクトルをもとの指令位置に加算して指令位置を補正するようにしている。補正ベクトルの方向は、移動方向に垂直な方向（法線方向）であり、補正ベクトルの長さは法線方向加速度（速度の二乗を曲率半径で除した値）に所定の係数をかけた値としている。

また、特許文献２では、ロボットの手先位置の制御において、所定のサンプリング時間先の手先の位置を推定し、推定した手先位置から目標軌道上に下ろした垂線ベクトルの分だけ指令位置を補正する。これにより、時間遅れを許容しながら、手先位置が目標軌道上を移動するように制御している。

特開平6−282321号公報（第４ページ〜第５ページ、第１図）特開2006−15431号公報（第５ページ〜第６ページ、第３図）

しかしながら、特許文献１の方法では、補正ベクトルの演算は移動を指示する指令値の法線方向加速度など移動指令の情報のみに基づいており、実際のサーボ応答を考慮していない。そのため、直線と円弧が連続する場合のように移動指令の法線方向加速度が過渡的に変化する場合、あるいは３次元形状も含む複雑な形状を微小線分指令で近似して指令するような場合には補正ベクトルを正しく演算できないという問題があった。移動指令における指令値の法線方向加速度が一定の場合は、法線方向加速度から軌跡誤差量を推定できる。また、法線方向加速度が変化する状態（過渡状態）では、移動指令の法線方向加速度に基づいて推定した軌跡誤差量と、実際に生じる軌跡誤差量との間に差異が生じる。したがって、推定した軌跡誤差量に基づいて演算した補正ベクトルを用いたときの応答軌跡ともとの指令軌跡との間に軌跡誤差が生じ、結果として応答軌跡の形状にひずみが生じる。

次に、特許文献２の方法では、実際の手先の位置、すなわち応答位置を利用して指令位置を補正するものであるが、応答遅れ時間は一定として、一定時間先の応答を基準に補正量を決定している。そのため、指令の移動方向が時々刻々変化するような場合には、応答遅れ時間が過渡的に変化するため補正が正しく行われず、応答軌跡が指令軌跡からずれてしまうという問題があった。

また、特許文献２では、目標軌道への垂線を演算する必要があるが、移動方向が時々刻々変化するような複雑な指令形状においては、目標軌道と応答位置の関係によって目標軌道への垂線が複数存在して補正量が一意に定まらないという問題があった。例えば、長方形におけるコーナー部のなど、２つの辺が所定の角度をなすものとして構成される指令形状を考える。この場合、制御対象の位置が上記所定の角度の２等分線上にある時点では、垂線の足が２つの辺それぞれに存在することになり、補正量が一意に決定できないことになる。また、垂線の方向又は長さが急峻に変化した場合に、補正量が急激に変化し、機械にショックを与え振動を引き起こすという問題があった。
さらに、特許文献２では、応答位置を演算するために各可動軸のサーボ応答をすべて演算する必要があり、演算量が多くなるといった問題があった。これは、５軸加工機など、可動軸の数が多い場合に特に顕著となる。

また、特許文献１および２においては、指令位置の補正量を演算するために移動指令を先読みする必要があり、演算処理が複雑となる。さらに、数値制御装置で逐次的に加工プログラムを処理することから、先読みを行うためには、先読みに必要な時間だけ動作を停止して、必要な分の移動指令を読み込む必要があり、その分だけ加工時間が延びてしまうといった課題があった。

この発明は、以上述べた課題を解決するためになされたもので、移動指令の法線方向加速度が過渡的に変化する場合、あるいは移動指令の移動方向が時々刻々変化する場合であっても、指令された経路上を正確に追従するように加工機の可動部の軌跡を制御する軌跡制御装置を得ることを目的とする。

この発明に係る軌跡制御装置は、複数の軸によって駆動される加工機の可動部の軌跡を制御する軌跡制御装置において、複数の軸の各々に対する指令位置の履歴として定まる指令経路に沿って開始位置から積算した長さである指令合成長さを演算する合成長さ演算部と、指令合成長さに複数の軸のサーボ応答を模擬した第１のフィルタを作用させて応答合成長さを演算する模擬サーボ応答フィルタ部と、開始位置から指令経路に沿った長さが応答合成長さと等しい指令経路上の位置である基準応答位置を演算する基準応答演算部と、基準応答位置における複数の軸の各々における座標値に対して、複数の軸のサーボ応答の逆特性となる第２のフィルタを作用させて複数の軸の各々の補正後指令位置を演算する応答補償部と、複数の軸の各々の位置が補正後指令位置に追従するように駆動トルクを出力することにより、前記複数の軸の各々を制御するサーボ制御部と、を備えたことを特徴とするものである。

この発明によれば、移動指令の法線方向加速度が過渡的に変化する場合、あるいは移動指令の移動方向が時々刻々変化するような場合であっても、応答合成長さに基づいて基準応答位置および補正後指令位置を求め、補正後の応答位置が指令された経路上を正確に追従するように可動部の軌跡を制御することができる。
また、補正後指令位置を一意に求めることが可能となり、安定して補正後指令位置の演算を行うことが可能となる。

本発明の実施の形態１による軌跡制御装置を示すブロック図である。本発明の実施の形態１における制御対象を示す模式図である。本発明の実施の形態１におけるサーボ制御部を示すブロック図である。本発明の実施の形態１における合成長さ演算部の動作を示す説明図である。本発明の実施の形態１における模擬サーボ応答フィルタ部の動作を示す説明図である。本発明の実施の形態１における基準応答演算部の動作を示す説明図である。本発明の実施の形態１における応答補償部を示すブロック図である。本発明の実施の形態１における指令位置の補正を行わなかった場合の応答位置の軌跡を示す図である。本発明の実施の形態１における指令位置の補正を行った場合の軌跡を示す図である。本発明の実施の形態１における指令合成長さと応答合成長さの時間変化を示す図である。本発明の実施の形態２による軌跡制御装置を示すブロック図である。本発明の実施の形態２における制御対象を示す模式図である。本発明の実施の形態２における指令位置の補正を行わなかった場合の軌跡を示す図である。本発明の実施の形態２における指令位置の補正を行った場合の軌跡を示す図である。本発明の実施の形態２における指令合成長さと応答合成長さの時間変化を示す図である。本発明の実施の形態３による軌跡制御装置を示すブロック図である。本の発明の実施の形態３における制御対象を示す模式図である。本発明の実施の形態３における指令位置の補正を行わなかった場合の各軸座標値の時間変化を示す図である。本発明の実施の形態における指令位置の補正を行った場合の各軸座標値の時間変化を示す図である。本発明の実施の形態３における指令位置の補正を行わなかった場合の軌跡を示す図である。本発明の実施の形態３における指令位置の補正を行った場合の軌跡を示す図である。本発明の実施の形態３における指令合成長さと応答合成長さの時間変化を示す図である。

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

実施の形態１．
図１は本発明の実施の形態１における軌跡制御装置を示すブロック図である。また、図２は本実施の形態において制御の対象とする２次元の加工機を示す模式図である。
図２において、加工ヘッド６３はY軸可動部６２に取り付けられており、Y軸のモータ（図示しない）によってY軸方向に駆動される。また、Y軸可動部６２およびY軸モータからなるY軸駆動機構はX軸可動部６１に取り付けられており、X軸のモータ（図示しない）によってX軸方向に駆動される。X軸可動部６１およびX軸モータからなるX軸駆動機構は、加工機本体（図示しない）に取付けられている。X軸およびY軸は互いに直交しているものとする。

X軸およびY軸のモータは、図１に示す軌跡制御装置１によって駆動制御される。軌跡制御装置１には、合成長さ演算部２、模擬サーボ応答フィルタ部３、基準応答演算部４、X軸応答補償部６、Y軸応答補償部７、X軸サーボ制御部８、Y軸サーボ制御部９が含まれる。

軌跡制御装置１には、移動指令として、X軸およびY軸の指令位置が入力される。軌跡制御装置１の内部において、X軸およびY軸の指令位置が合成長さ演算部２に入力され、指令合成長さが出力される。指令合成長さは模擬サーボ応答フィルタ部３に入力される。模擬サーボ応答フィルタ部３の出力はX軸およびY軸の指令位置とともに基準応答演算部４に入力され、X軸基準応答位置およびY軸基準応答位置が演算される。X軸基準応答位置およびY軸基準応答位置は、X軸応答補償部６およびY軸応答補償部７にそれぞれ入力されて、X軸補正後指令位置およびY軸補正後指令位置がそれぞれ出力される。X軸補正後指令位置およびY軸補正後指令位置は、X軸サーボ制御部８およびY軸サーボ制御部９にそれぞれ入力されて、X軸モータ駆動トルクおよびY軸モータ駆動トルクがそれぞれ出力される。

X軸サーボ制御部８およびY軸サーボ制御部９は同一の構成であり、そのブロック図を機械系１３のブロック図とともに、図３に示す。機械系１３はモータ３２および負荷３３からなる。以下、X軸サーボ制御部８、Y軸サーボ制御部９を総称してサーボ制御部１１と呼ぶこととする。

図３において、サーボ制御部１１へ入力された補正後指令位置から、第１の減算器２０でモデル位置が減算される。この減算された値に、第１のモデルゲイン乗算器２１で第１のモデルゲイン（制御ゲイン）K₁が乗算される。この乗算された値から、第２の減算器２２でモデル速度が減算される。この減算された値に、第２のモデルゲイン乗算器２３で第２のモデルゲイン（制御ゲイン）K₂が乗算され、モデル加速度が出力される。次に、第１の積分器２４でモデル加速度が積分されてモデル速度が出力される。さらに、第２の積分器２５でモデル速度が積分されて、モデル位置が出力される。以上述べた、補正後指令位置からモデル位置までのブロックを規範モデル１２と呼ぶ。

第１のモデルゲインK₁および第２のモデルゲインK₂の各々には、X軸サーボ制御部８とY軸サーボ制御部９で同じ値が設定される。したがって、X軸サーボ制御部８とY軸サーボ制御部９の規範モデルは同一の伝達関数をもつ。

図３に示すように、第３の減算器２６でモデル位置からモータ位置信号が減算されて位置誤差が出力される。位置制御器２７では、この位置誤差に対して比例制御を行う。加減算器２８において位置制御器２７の出力にモデル速度が加算され、モータ速度信号の値が減算されて速度誤差が出力される。速度制御器２９ではこの速度誤差に対して比例・積分制御を行う。また、比例乗算器３０で、モデル加速度に制御対象のイナーシャに相当する値（Ｊ）が乗算され、モデルトルクが演算される。次いで、第４の加算器３１で速度制御器の出力にモデルトルクが加算されてモータトルク信号が出力される。

機械系１３は、このモータトルク信号により駆動され、モータ速度信号とモータ位置信号がサーボ制御部１１へ出力される。なお、本実施の形態においては、図１に示すように、Y軸の負荷は加工ヘッド６３が取り付けられたY軸可動部６２であり、X軸の負荷はX軸可動部６１およびその上で駆動されるY軸可動部６２となる。
サーボ制御部１１は、規範モデルを用いた２自由度制御器であり、指令に対する追従性と外乱に対する応答性を独立して設計することができる。指令に対する追従性は第１のモデルゲインK₁および第２のモデルゲインK₂で決まり、外乱に対する応答性は位置制御器２７および速度制御器２９の設計で決まる。

サーボ系の応答は、実際の制御対象の特性にかかわらず、規範モデル１２の出力であるモデル位置に追従するように制御される。本実施の形態では、X軸のサーボ制御系とY軸のサーボ制御系で同じ伝達関数の規範モデルを用いているため、X軸サーボ制御部８で制御されるサーボ制御系の応答とY軸サーボ制御部９で制御されるサーボ制御系の応答は同一となり、その応答は規範モデルの伝達関数で表すことができる。

合成長さ演算部２は、各可動軸の指令位置の履歴によって定まる指令経路について、開始位置からの指令経路に沿って積算した指令合成長さを演算する。指令合成長さの演算は、次のように行われる。指令位置は一定の制御周期Tごとに更新されるものとし、n番目（ｎはゼロ又は自然数）の指令位置をPcnで表す。PcnはX軸方向成分およびY軸方向成分の2つの要素をもつ座標であり、その座標値を（xcn, ycn）で表す。また、移動の開始位置をPc0とし、その座標を（xc0, yc0）とおく。n番目の指令位置における指令合成長さLcnは、その指令位置の前回値（前回指令位置）である（n−1）番目の指令位置Pc（n−1）から今回指令位置Pcnまでのベクトルの幾何ノルムを、開始位置から積算することによって求められる。幾何ノルムは、ベクトルの各要素の二乗和の平方根である。このことを式で表すと、次の式（1）のようになる。

図４に、n＝0〜3における指令位置Pcnと指令合成長さLcnの関係を模式的に示す。なお、n＝0（開始位置）に対する指令合成長さLc0はゼロである。また、図４において、「sqrt（式Ａ）」は式Ａの平方根を表す。

模擬サーボ応答フィルタ部３は、指令合成長さLcnに対してサーボ応答を模擬したフィルタを作用させる。サーボ応答は一般に伝達関数の形で表される。本実施の形態では、サーボ制御部１１によって制御されるサーボ系のサーボ応答と、模擬サーボ応答フィルタ部３のサーボ応答とが同一となるように、模擬サーボ応答フィルタ部３が構成される。サーボ制御部１１に規範モデルを用いた２自由度制御を適用した場合、サーボ制御部１１のサーボ応答は規範モデルの伝達関数で表される応答となる。前述のとおり、X軸サーボ制御部８とY軸サーボ制御部９は同一の構成であり、同一の制御ゲイン（第１のモデルゲインK₁および第２のモデルゲインK₂）を用いているため、X軸規範モデルとY軸規範モデルは同一構成のフィルタとなる。図３に示すサーボ制御部１１の場合、規範モデル１２の伝達関数Gm（s）は２次のローパスフィルタの形で以下の式（2）のように表される。

このローパスフィルタの出力は、式(2)の伝達関数を微分方程式、差分方程式、伝達関数等の形式で表し、数値計算によりその解を計算することで求めることができる。また、指令形状が既知である場合は、積分計算により解析解を求めることができる。具体的には、伝達関数と与えられた入力のラプラス変換との積を逆ラプラス変換することにより解析解が求められる。
時刻tについて、t＝nTで表すことにする（Tは制御周期）。時刻tにおける指令合成長さをLc（t）とおいたとき、指令合成長さLc（t）が入力されたときの模擬サーボ応答フィルタ部３の出力、すなわち応答合成長さLr（t）は次の式（3）で表される。

ここで、F［g（t）］は時間関数g（t）のラプラス変換を表し、F⁻¹［G（s）］はs領域（複素領域）の関数G（s）の逆ラプラス変換を表す。
図５に、指令合成長さLc（t）と応答合成長さLr（t）の時間変化を模式的に示す。ここで、t1＝T、t2＝2T、t3＝3Tである。図５において、T＝t1、t2およびt3における応答合成長さをそれぞれLr1、Lr2およびLr3と表し、指令合成長さをそれぞれ、Lc1、Lc2およびLc3と表す。同図に示すように、模擬サーボ応答フィルタ部３により、指令合成長さLc（t）よりも応答合成長さLr（t）が遅れて追従するようになっている。
次に、基準応答演算部４において、指令経路上でかつ開始位置からの指令経路に沿った長さが応答合成長さLr（t）と等しい位置である基準応答位置を演算する。n番目の基準応答位置をPrnとおくとき、その座標（xrn, yrn）は以下のようにして求められる。

まず、n番目の応答合成長さLrnと、n番目を含めた過去の指令合成長さLc1〜Lcnを比較し、応答基準点が指令経路の何番目の線分上にあるかを決定する。すなわち、LrnがLc（k−1）以上であり、Lck未満となるようなk（kは自然数）をもとめる。Lckはkの増加に対して単調増加であるので、kは一意に求めることができる。続いて、基準応答位置が属する線分の始点と終点の間を応答合成長さLrnで定まる比率を用いて直線補間することにより、基準応答位置の座標を決定する。基準応答位置の座標（xrn, yrn）は、次の式（4）で求められる。

n＝1〜3の場合について、指令経路と応答合成長さLrnと基準応答位置Prnの関係を図６に模式的に示す。図６において、指令経路は指令位置Pcnを番号ｎの順に結んだ線分で表される。なお、Pc0は開始位置を示す。

次に、X軸応答補償部６およびY軸応答補償部７は、基準応答位置PrnのX軸成分およびY軸成分に対してサーボ応答の逆特性のフィルタをそれぞれ作用させる。ここで考慮するサーボ応答は、サーボ制御部１１で制御されるサーボ制御系と同一の応答とする。したがって、規範モデル１２の逆特性の応答のフィルタを作用させることになる。

図７に、X軸応答補償部６およびY軸応答補償部７のブロック図を示す。X軸およびY軸でそれぞれの応答補償部は同一の構成となり、同図において応答補償部４０として示す。応答補償部４０に入力された信号（X軸基準応答位置又はY軸基準応答位置）は、第１の微分演算器５０に入力され、その出力はさらに第２の微分演算器５１に入力される。ここで、微分演算は差分近似によって行うものとし、前回値と今回値の差を制御周期Tで除した値を微分値として出力する。次に、第２の微分演算器５１の出力に対して第１の係数乗算器５２にて第２のモデルゲインK₂の逆数を乗じ、この出力と第１の微分演算器５０の出力を第１の加算器５３にて加算する。さらにその出力に対して第２の係数乗算器５４にて第１のモデルゲインK₁の逆数を乗じ、この出力と応答補償部４０への入力信号とを第２の加算器５５にて加算し、その結果を応答補償部４０の出力とする。

応答補償部４０の入出力間の伝達関数は、以下の式（5）で表される。

式（5）に示されるように、同式は、式(2)の規範モデルの伝達関数の逆伝達関数となっており、応答補償部４０は規範モデルの逆特性をもつことになる。
図１において、上述の応答補償部４０の出力算出方法を用いて、X軸応答補償部６およびY軸応答補償部７の出力はそれぞれX軸およびY軸の補正後指令位置として、X軸サーボ制御部８およびY軸サーボ制御部９に入力される。X軸サーボ制御部８およびY軸サーボ制御部９は、前述のサーボ制御を行ってX軸およびY軸のモータ駆動トルクを演算して出力し、X軸モータおよびY軸モータをそれぞれ駆動する。

本実施の形態で示すように、基準応答位置に応答補償部４０を作用させたあとにサーボ制御を行うことにより、サーボ制御の結果得られる各可動軸の応答位置が基準応答位置に追従するようになる。基準応答位置は指令経路上にあるので、各可動軸の応答位置も指令経路上に誤差なく追従するように制御される。また、応答補償部４０の出力である補正後指令位置は、移動方向が変化しない場合（指令経路が直線状となる場合）にはもとの指令位置と同じ位置となる。これに対し、移動方向が変化する場合（指令経路が曲線状となる場合）には、軌跡誤差を抑制するために、応答補償部４０の作用によってもとの指令位置とは異なる位置が出力されることになる。

実施例１．
次に、本実施の形態における効果について実施例を用いて示す。指令経路として、円弧部分の半径が1mmのコーナーR形状（すなわち、半径1mmの円弧とそれを挟む２つの直線部分で構成される形状）を用いる。本実施例で用いた指令位置の軌跡すなわち指令経路と、指令位置の補正を行わない場合の応答位置の軌跡を図８に示す。（X, Y）＝（0mm, 0mm）の点が始点（開始位置）であり、（X, Y）＝（2mm, 2mm）の点が終点である。なお、図８における矢印は指定経路における指令位置の移動方向を示す。この図中の矢印が意味することは、以降の図９、図１３、図１４、図２０および図２１においても同様である。

図８のグラフは横軸にX軸位置、縦軸にY軸位置をとっており、ドットなしの実線が指令位置、ドットつきの実線が指令位置の補正を行わない場合（図１において、各可動軸の指令位置をそれぞれの軸のサーボ制御部に直接入力した場合）のサーボ応答位置のシミュレーション結果である。シミュレーションの条件として、送り速度は2m／min、X軸およびY軸のサーボ制御部の規範モデル１２における第１のモデルゲイン（制御ゲイン）K₁は40rad／sとし、第２のモデルゲイン（制御ゲイン）K₂は第１のモデルゲインK₁の4倍とした。また、制御周期Tは2msとした。

次に、指令位置の補正を行った場合における、補正後指令位置の軌跡と補正後応答位置の軌跡のシミュレーション結果を図９に示す。図９において、ドットなしの実線はもとの指令位置の軌跡であり、破線は補正後指令位置の軌跡、ドットつきの実線は応答位置の軌跡を表している。送り速度、サーボ制御部の第１のモデルゲインK₁および第２のモデルゲインK₂の値は、指令位置の補正を行わない場合と同一の値を用いている。指令位置の補正を行った場合はもとの指令位置の軌跡と応答位置の軌跡がほぼ一致しており、両者を見分けるために、図８および図９の応答位置の波形にドットが付されている。
また、図１０に、本実施例における模擬サーボ応答フィルタ部３の入出力、すなわち指令合成長さと応答合成長さの時間変化を示す。

図８より、指令位置の補正を行わない場合は、各軸のサーボ応答遅れの影響により、円弧部分で約0.2mmの軌跡誤差が発生していることがわかる。この実施例で用いている指令形状は、直線部分と円弧部分が連続しており、直線部分と円弧部分で法線方向加速度が過渡的に変化するとともに、円弧部分では移動方向が時々刻々変化するようになっており、軌跡誤差が発生しやすい形状となっている。

図１０より、指令位置の補正を行った場合には、合成長さ方向（図１０における縦軸の方向）には指令に対して遅れて応答することがわかる。一方、図９のドットつきの実線が図９のドットなしの実線と一致していることから、指令経路に対する軌跡誤差は十分に抑制されていることがわかる。これは、応答合成長さと指令位置から求めた基準応答位置にサーボ応答が追従するように各軸の応答補償部によって補正後指令位置を図９の破線のように演算した結果である。

以上述べたように、本実施の形態によれば、移動指令の法線方向加速度が過渡的に変化する場合、あるいは移動指令の移動方向が時々刻々変化するような場合であっても、応答合成長さに基づいて基準応答位置および補正後指令位置を求め、補正後の応答位置が指令された経路上を正確に追従するように可動部の軌跡を制御することができる。

したがって、安定して補正後指令位置の演算を行うことが可能となる。
また、各軸のサーボ応答を求めることなく補正後指令位置を演算できるので、演算量の削減が可能となる。
また、移動指令を先読みすることなく、指令位置の過去の履歴のみに基づいて補正後指令位置を演算することができるので、演算処理が簡単になり、先読みのために加工時間が延びてしまうといったことを回避できる。
さらに、サーボ制御部に規範モデルを用いた２自由度制御器を用いることで、制御対象の特性にかかわらずサーボ制御部の応答を規範モデルの応答と一致させることができる。

また、規範モデルの伝達関数の逆伝達関数で表現されるフィルタを応答補償部に用いることで、各可動軸の実際の位置を、指令経路に沿った位置である基準応答位置に正確に追従させることができ、指令経路からの軌跡誤差が生じないように軌跡を制御することが可能となる。
以上に示すように、本実施の形態によれば、２次元平面内で軌跡制御を行う場合に軌跡誤差を抑制して高精度な加工を行うことができる。

実施の形態２．
図１１は本発明の実施の形態２における軌跡制御装置１０１を示すブロック図である。また、図１２は本実施の形態２において制御対象としている３軸加工機を示す模式図である。

図１２において、加工ヘッド７４はZ軸可動部７３に取り付けられており、Z軸のモータ（図示しない）によってZ軸方向に駆動される。また、Z軸可動部７３およびZ軸モータからなるZ軸駆動機構はY軸可動部７２に取り付けられており、Y軸のモータ（図示しない）によってY軸方向に駆動される。さらに、Y軸可動部７２およびY軸モータからなるY軸駆動機構はX軸可動部７１に取り付けられており、X軸のモータ（図示しない）によってX軸方向に駆動される。X軸可動部７１およびX軸モータからなるX軸駆動機構は、加工機本体に取り付けられている。X軸、Y軸およびZ軸は互いに直交しているものとする。
図１２における加工ヘッド７４、X軸可動部７１およびY軸可動部７２はそれぞれ、図２における加工ヘッド６３、X軸可動部６１およびY軸可動部６２に対応する。

X軸、Y軸およびZ軸のモータは、図１１に示す軌跡制御装置１０１によって駆動制御される。
本実施の形態が実施の形態１と相違する点は、制御の対象となる可動軸の数が３軸になり、新たにZ軸が追加された点であり、その他の構成（X軸およびY軸の構成）は実施の形態１と同様である。また、Z軸応答補償部１０８およびZ軸サーボ制御部１１１は、X軸およびY軸と同様の構成である。
以下では、実施の形態１との相違点について述べる。n番目の指令位置Pcnの座標をX軸、Y軸、Z軸の座標値を用いて（xcn, ycn, zcn）とすると、合成長さ演算部１０２における指令合成長さLcnは以下の式（6）で求められる。

次に、基準応答演算部１０４における基準応答位置の座標（xrn, yrn, zrn）は、以下の式（7）で求められる。

上述したように、本実施の形態におけるその他の構成は、実施の形態１と同様である。
X軸応答補償部１０６、Y軸応答補償部１０７およびZ軸応答補償部１０８は、いずれも図７に示した応答補償部４０と同様に構成される。すなわち、X軸、Y軸およびZ軸について応答補償部の入出力間の伝達関数は、式（5）で示される。
また、Z軸サーボ制御部１１１は、X軸サーボ制御部１０９およびY軸サーボ制御部１１０と同一の構成であり、そのブロック図は図３に示すとおりである。

X軸応答補償部１０６、Y軸応答補償部１０７およびZ軸応答補償部１０７の出力はそれぞれX軸、Y軸およびZ軸の補正後指令位置として、X軸サーボ制御部１０９、Y軸サーボ制御部１１０およびZ軸サーボ制御部１１１に入力される。X軸サーボ制御部１０９、Y軸サーボ制御部１１０およびZ軸サーボ制御部１１１は、実施の形態１で述べたものと同様のサーボ制御を行い、X軸、Y軸およびZ軸のモータ駆動トルクを演算して出力し、X軸モータ、Y軸モータおよびZ軸モータをそれぞれ駆動する。

実施例２．
次に、本実施の形態における効果について実施例を用いて示す。指令経路として、XY平面に投影すると半径5mmの円弧になる螺旋状の形状を用いた場合を示す。X軸とY軸がXY平面内において1周の円運動を行う間にZ方向に10mm移動するものとする。
本実施例で用いた指令位置の軌跡すなわち指令経路と、指令位置の補正を行わない場合の応答位置の軌跡を図１３に示す。図１３（a）はXY平面、図１３（b）はXZ平面に投影した軌跡である。（X, Y, Z）＝（0mm, −5mm, 0mm）の位置が始点であり、（X, Y, Z）＝（0mm, −5mm, 10mm）の位置が終点である。また、図１３（ａ）に示すXY平面において、円軌跡は反時計回りに回転したものである。

図１３（ａ）および（ｂ）の各グラフはドットなしの実線が指令位置、ドットつきの実線が指令位置の補正を行わない場合（図１１において、各可動軸の指令位置をそれぞれの軸のサーボ制御部に直接入力した場合）のサーボ応答位置のシミュレーション結果である。シミュレーションの条件として、指令経路方向の送り速度は7.9m／minとした。また、始点における初速は0m／sとし、指令経路の接線方向の速度が指令送り速度である7.9m／minになるまで一定の加速度で加速するものとした。さらに、終点付近では、指令経路の接線方向の送り速度が指令送り速度である7.9m／minから一定の加速度で減速して終点において0m／minになるパターンとした。加速および減速の際の接線方向の加速度の絶対値は10m／s²とおいた。

X軸、Y軸およびZ軸のサーボ制御部の規範モデルにおける第１のモデルゲイン（制御ゲイン）K₁は40rad／sとし、第２のモデルゲイン（制御ゲイン）K₂は第１のモデルゲイン（制御ゲイン）K₁の4倍とした。また、制御周期Tは2msとした。

指令位置の補正を行った場合における、補正後指令位置の軌跡と補正後応答位置の軌跡のシミュレーション結果を図１４に示す。図１４（a）はXY平面、図１４（b）はXZ平面に投影した軌跡である。図１４において、ドットなしの実線はもとの指令位置の軌跡であり、破線は補正後指令位置の軌跡、ドットつきの実線は応答位置の軌跡を表している。送り速度とサーボ制御部の第１のモデルゲインK₁および第２のモデルゲインK₂の値は、指令位置の補正を行わない場合と同一の値を用いている。
指令位置の補正を行った場合はもとの指令位置の軌跡と応答位置の軌跡がほぼ一致しており、両者の識別のために、図１３および図１４の補正後応答位置の波形にドットを付している。
また、図１５に、本実施例における模擬サーボ応答フィルタ部の入出力、すなわち指令合成長さと応答合成長さの時間変化を示す。

図１３より、指令位置の補正を行わない場合のサーボ応答位置の軌跡には、全体的に指令軌跡である円弧形状の内側の方向に軌跡誤差が発生しており、その軌跡誤差は最大で約0.2mmとなっていることがわかる。この実施例で用いている指令形状は、３次元的に移動方向が時々刻々変化するようになっており、軌跡誤差が発生しやすい形状となっている。また、始点と終点では接線方向に加減速を行いながら移動方向が変化しており、法線方向加速度が過渡的に変化することから、軌跡誤差の正確な補正を行うことが難しい指令経路となっている。

また、図１５より、指令位置の補正を行った場合には、合成長さ方向には指令に対して遅れて応答することがわかる。一方、図１４のドットつきの実線が図１４のドットなしの実線と一致していることから、指令経路に対する軌跡誤差は十分に抑制されていることがわかる。これは、応答合成長さと指令位置から求めた基準応答位置にサーボ応答が追従するように各軸の応答補償部によって補正後指令位置を図１４の破線のように演算した結果である。

以上述べたように、本実施の形態によれば、３軸の立形マシニングセンタなどのように３次元空間内で軌跡制御を行う場合においても、実施の形態１と同様な効果が得られる。この場合においても軌跡誤差を抑制して高精度な加工を行うことができる。

実施の形態３．
図１６は、本発明の実施の形態３による軌跡制御装置を示すブロック図である。また、図１７は本実施の形態において制御の対象としている５軸加工機を示す模式図である。
図１７において、加工ヘッド８６、Z軸可動部８３、Y軸可動部８２およびX軸可動部８１は実施の形態２と同様の構成である。すなわち、加工ヘッド８６、Z軸可動部８３、Y軸可動部８２およびX軸可動部８１はそれぞれ、図12における加工ヘッド７４、Z軸可動部７３、Y軸可動部７２およびX軸可動部７１に対応する。

本実施の形態ではさらに、Y軸まわりに回転するB軸とZ軸まわりに回転するC軸をもつ回転テーブルを具備しており、回転テーブル上にワークを置くことでワークに対する工具の向き（工具姿勢）を制御することができるようになっている。C軸可動部８５は回転テーブルとして機能し、C軸モータ（図示しない）によって駆動される。C軸可動部８５とC軸モータからなるC軸駆動機構はB軸可動部８４に取り付けられており、B軸モータ（図示しない）で駆動される。B軸可動部８４およびB軸モータからなるB軸駆動機構は、加工機本体に取り付けられている。B軸の回転中心軸はY軸と平行になっており、B軸の回転角度が0°のとき、C軸の回転中心軸がZ軸と平行になっているものとする。

X軸、Y軸、Z軸、B軸およびC軸のモータは、図１６に示す軌跡制御装置２０１によって駆動制御される。実施の形態２と相違する点は、制御の対象となる可動軸の数が５軸になり、新たにB軸とC軸が追加された点であり、その他の構成は実施の形態２と同様である。また、B軸およびC軸の応答補償部およびサーボ制御部はX軸、Y軸およびZ軸と同様の構成である。

以下では、実施の形態２との相違点について述べる。n番目の指令位置Pcnの座標をX軸、Y軸、Z軸、B軸、C軸の各軸の座標値を用いて（xcn, ycn, zcn, bcn, ccn）とすると、合成長さ演算部２０２における指令合成長さLcnは以下の式（8）で求められる。

次に、基準応答演算部２０４における基準応答位置の座標（xrn, yrn, zrn, brn, crn）は、以下の式（9）で求められる。

上述したように、本実施の形態におけるその他の構成は、実施の形態２と同様である。X軸応答補償部２０６、Y軸応答補償部２０７、Z軸応答補償部２０８、B軸応答補償部２０９およびC軸応答補償部２１０はいずれも、図７に示した応答補償部４０と同様に構成される。また、X軸、Y軸、Z軸、B軸およびC軸について応答補償部の入出力間の伝達関数は、式（5）で示される。

また、B軸サーボ制御部２１４および軸サーボ制御部２１５は、X軸サーボ制御部２１１、Y軸サーボ制御部２１２およびZ時期サーボ制御部２１３と同一の構成であり、そのブロック図は図３に示すとおりである。

X軸応答補償部２０６、Y軸応答補償部２０７、Z軸応答補償部２０８、B軸応答補償部２０９およびC軸応答補償部２１０の出力はそれぞれX軸、Y軸、Z軸、B軸およびC軸の補正後指令位置として、X軸サーボ制御部２１１、Y軸サーボ制御部２１２、Z軸サーボ制御部２１３、B軸サーボ制御部２１４およびC軸サーボ制御部２１５に入力される。X軸サーボ制御部２１１、Y軸サーボ制御部２１２、Z軸サーボ制御部２１３、B軸サーボ制御部２１４およびC軸サーボ制御部２１５は、実施の形態１と同様のサーボ制御を行い、X軸、Y軸、Z軸、B軸およびC軸のモータ駆動トルクを演算して出力し、X軸モータ、Y軸モータ、Z軸モータ、B軸モータおよびC軸モータをそれぞれ駆動する。

実施例３．
次に、本実施の形態における効果について実施例を用いて示す。指令経路として、ワークから見た工具先端の経路が直線であり、その間に工具姿勢が変化する経路とする。機械のX軸、Y軸およびZ軸の軸方向がそれぞれX、YおよびZの座標軸方向である座標系を機械座標系と定義する。また、B軸とC軸がともに0°のときに座標軸方向が機械座標系と一致し、テーブルの回転に連動する座標系をテーブル座標系と定義する。

一例として、B軸回転テーブルの回転中心の座標値は機械座標系で（100mm, 200mm, 300mm）であり、C軸回転テーブルの回転中心の座標は機械座標系で（100mm, 200mm, 400mm）であるものとする。C軸回転テーブルの回転中心の位置をテーブル座標系の原点とする。テーブル座標系において、工具先端位置の座標値が（0mm, 200mm, 0mm）から（−200mm, 0mm, 0mm）に移動するものとし、その間に工具姿勢はベクトル表現として、（0, 1, 1）の方向から（−1, 0, 1）の方向へ変化するものとする。

このような工具先端の経路を実現するためには、機械の各軸が複雑な経路で運動する必要がある。したがって、テーブル座標系における工具先端位置および工具姿勢ベクトルを制御周期ごとに機械座標系における工具位置（X、YおよびZの各軸における位置の座標値）と回転軸角度（BおよびCの各回転軸の座標値）に換算し、換算した結果得られるX、Y、Z、BおよびCの各軸座標値を各軸の指令位置として用いる。この換算は、幾何学的な関係を用いて一意に行うことができる。

本実施例で用いた指令位置と、指令位置の補正を行わない場合の応答位置の時間変化を図１８に示す。図１８のグラフは実線が指令位置、破線が指令位置の補正を行わない場合（図１６において、各可動軸の指令位置をそれぞれの軸のサーボ制御部に直接入力した場合）のサーボ応答位置のシミュレーション結果である。シミュレーションの条件として、経路方向の送り速度は2m／minとした。X軸、Y軸、Z軸、B軸およびC軸のサーボ制御部の規範モデルにおける第１のモデルゲイン（制御ゲイン）K_１は40rad／sとし、第２のモデルゲイン（制御ゲイン）K_２は第１のモデルゲイン（制御ゲイン）K_１の4倍とした。また、制御周期Tは2msとした。

また、指令位置の補正を行った場合における、補正後指令位置と補正後応答位置の時間変化のシミュレーション結果を図１９に示す。図１９において、ドットなしの実線はもとの指令位置であり、ドットつきの実線は補正後指令位置の軌跡、破線は応答位置の軌跡を表している。また、図１９においては、もとの指令位置の時間変化と補正後指令位置の時間変化がほぼ一致しており、両者を見分けるために補正後指令位置の波形にドットを付している。送り速度とサーボ制御部の第１のモデルゲインK_１および第２のモデルゲインK_２の値は、指令位置の補正を行わない場合と同一の値を用いている。

図１８および図１９から、テーブル座標系における工具先端位置と工具姿勢の移動を実現するために機械の各可動軸の指令位置の時間変化が複雑な形となっていることがわかる。また、指令位置の補正を行わない場合も指令位置の補正を行った場合も、指令位置に対して応答位置が遅れて追従しているが、その遅れ方は両者で異なっている。
各可動軸位置の時間変化では、軌跡誤差の評価を行うことができないので、応答位置および補正後指令位置を機械座標系からテーブル座標系に換算して評価を行う。

テーブル座標系における、指令位置の軌跡と指令位置の補正を行わない場合の応答位置の軌跡を図２０に示す。図２０の各グラフはドットなしの実線が指令位置、ドットつきの実線が指令位置の補正を行わない場合（図１６において、各可動軸の指令位置をそれぞれの軸のサーボ制御部に直接入力した場合）のサーボ応答位置のシミュレーション結果である。

次に、指令位置の補正を行った場合の、テーブル座標系における補正後指令位置の軌跡と補正後応答位置の軌跡を図２１に示す。図２１において、ドットなしの実線はもとの指令位置の軌跡であり、破線は補正後指令位置の軌跡、ドットつきの実線は応答位置の軌跡を表している。送り速度とサーボ制御部の第１のモデルゲインK₁および第２のモデルゲインK₂の値は、指令位置の補正を行わない場合と同一の値を用いている。指令位置の補正を行った場合はもとの指令位置の軌跡と応答位置の軌跡がほぼ一致しており、両者を見分けるために、図２０および図２１の応答位置の波形にドットを付している。
また、図２２に、本実施例における模擬サーボ応答フィルタ部の入出力、すなわち指令合成長さと応答合成長さの時間変化を示す。

図２０より、指令位置の補正を行わない場合は、各軸のサーボ応答遅れの影響により、直線の中央部分で約5mmの軌跡誤差が発生していることがわかる。この実施例で用いている指令形状は、テーブル座標系における工具先端位置と工具姿勢の指令どおりの移動を実現するために、機械の各可動軸が加速度を時々刻々変化させながら移動するようになっており、機械の各可動軸の追従誤差がテーブル座標系での軌跡誤差として現れやすくなっている。

図２２より、指令位置の補正を行った場合には、合成長さ方向には指令に対して遅れて応答することがわかる。一方、図２１のドットつきの実線が、図２１のドットなしの実線と一致していることから、指令経路に対する軌跡誤差は十分に抑制されていることがわかる。これは、応答合成長さと指令位置から求めた基準応答位置にサーボ応答が追従するように各軸の応答補償部によって補正後指令位置を図１９のドットつき実線および図２１の破線のように演算した結果である。

また、図１９に示すように、同時刻における補正後指令位置と指令位置の差は、追従誤差（図１８における指令位置と応答位置の差）に比べて小さい値となっている。これは、応答合成長さに基づいて追従誤差を許容して基準応答位置を演算しているためであり、小さい補正量で軌跡誤差を効果的に抑制することができる。
以上のように、本実施の形態によれば、５軸加工機のように工具位置と姿勢の軌跡制御を行う場合であっても、実施の形態１と同様の効果が得られる。この場合においても軌跡誤差を抑制して高精度な加工を行うことができる。

なお、本実施の形態ではテーブル側に２軸の回転軸がある場合について説明したが、工具側に２軸の回転軸がある場合、あるいは工具側とテーブル側に１軸ずつ回転軸を有する場合であってもよい。機械座標系での座標値とテーブル座標系での座標値との間の換算式を適宜選択して適用すれば、同様の効果を得ることができる。
また、本実施の形態では２つの回転軸をY軸まわりに回転するB軸とZ軸まわりに回転するC軸としたが、それ以外の方向の軸まわりに回転するものであってもよい。機械座標系での座標値とテーブル座標系での座標値との間の換算式に適切なものを適用すれば、同様の効果を得ることができる。

以上述べた実施の形態１から３においては、可動軸の数が２、３、５の場合について説明したが、可動軸の数は４または６以上であっても同様である。その場合、合成長さ演算部で可動軸の数に応じた合成長さを演算するようにし、応答補償部およびサーボ制御部を可動軸の数に応じた分だけ設置すればよい。

１、１０１、２０１軌跡制御装置、
２、１０２、２０２合成長さ演算部、
３、１０３、２０３模擬サーボ応答フィルタ部、
４、１０４、２０４基準応答演算部、
６、１０６、２０６ X軸応答補償部、
７、１０７、２０７ Y軸応答補償部、
８、１０９、２１１ X軸サーボ制御部、
９、１１０、２１２ Y軸サーボ制御部、
１１サーボ制御部、
１２規範モデル、
４０応答補償部、
６１、７１、８１ X軸可動部、
６２、７２、８２ Y軸可動部、
７３、８３ Z軸可動部、
８４ B軸可動部、
８５ C軸可動部、
１０８、２０８ Z軸応答補償部、
１１１ Z軸サーボ制御部、
２０９ B軸応答補償部、
２１０ C軸応答補償部、
２１３ Z軸サーボ制御部
２１４ B軸サーボ制御部、
２１５ C軸サーボ制御部。

Claims

複数の軸によって駆動される加工機の可動部の軌跡を制御する軌跡制御装置において、
前記複数の軸の各々に対する指令位置の履歴として定まる指令経路に沿って開始位置から積算した長さである指令合成長さを演算する合成長さ演算部と、
前記指令合成長さに前記複数の軸のサーボ応答を模擬した第１のフィルタを作用させて応答合成長さを演算する模擬サーボ応答フィルタ部と、
前記開始位置から前記指令経路に沿った長さが前記応答合成長さと等しい前記指令経路上の位置である基準応答位置を演算する基準応答演算部と、
前記基準応答位置における前記複数の軸の各々における座標値に対して、前記複数の軸の前記サーボ応答の逆特性となる第２のフィルタを作用させて前記複数の軸の各々の補正後指令位置を演算する応答補償部と、
前記複数の軸の各々の位置が前記補正後指令位置に追従するように駆動トルクを出力することにより、前記複数の軸の各々を制御するサーボ制御部とを備えたことを特徴とする軌跡制御装置。
サーボ制御部の伝達関数が、模擬サーボ応答フィルタ部の伝達関数と同じであることを特徴とする請求項１に記載の軌跡制御装置。
サーボ制御部は規範モデルを用いた２自由度制御器で構成され、前記規範モデルの伝達関数が模擬サーボ応答フィルタ部の伝達関数と同じであることを特徴とする請求項２に記載の軌跡制御装置。
第２のフィルタは、規範モデルの伝達関数の逆伝達関数で表現されるフィルタであることを特徴とする請求項３に記載の軌跡制御装置。