以下、本発明の実施形態を図面を参照して説明する。
まず、図1及び図2を参照して、本発明の一実施形態による数値制御装置2が適用される工作機械の構成について説明する。
この工作機械は、門型の工作機械であり、テーブル101a上にセットされた加工物としてのワーク100上で工具106を動かしながら、その工具106によってワーク100の加工を行うものである。工作機械は、図1に示すように、テーブル101aと、ベッド101bと、テーブル移送装置102(図2参照)と、2本のコラム104と、クロスレール105と、工具106と、主軸ヘッド108と、揺動装置110と、回動装置112と、鉛直移送装置114と、水平移送装置116と、制御ボックス120とを備えている。なお、テーブル移送装置102、揺動装置110、回動装置112、鉛直移送装置114及び水平移送装置116は、ワーク100又は工具106を移送対象物としてその移送対象物をワーク100を加工するために複数の移送軸に沿って動かすものであり、それぞれ、本発明の移送装置の概念に含まれるものである。
テーブル101aは、その上にセットされたワーク100を下から支持するものである。ベッド101bは、所定の設置場所に設置されており、テーブル101aが水平面内において特定方向に延びるX軸方向に移動可能となるように当該テーブル101aを下から支持している。
テーブル移送装置102は、ベッド101b内に設けられており、前記X軸に沿ってテーブル101aを移送し、それによって、テーブル101a上にセットされたワーク100をX軸に沿って移送する。なお、X軸は、本発明の移送軸の概念に含まれるものである。テーブル移送装置102は、駆動源として図略のサーボモータを有しており、そのモータの動力によってテーブル101aを移送する。
2本のコラム104は、テーブル101aの移動方向(X軸方向)に直交する方向(テーブル101aの幅方向)においてそのテーブル101a及びベッド101bの両側に分かれて立設されている。
クロスレール105は、テーブル101aの上方に配置され、テーブル101aの幅方向(水平方向でX軸と直交する方向)に延びる姿勢をとるように前記2本のコラム104に掛け渡されている。
工具106は、ワーク100を切削加工するためのものであり、主軸ヘッド108によって保持される。主軸ヘッド108は、保持した工具106をその軸回りに回転させる。この主軸ヘッド108によって回転させられた工具106がワーク100に当接することによってワーク100の加工が行われる。
揺動装置110は、本実施形態では、テーブル101aの上面に平行な水平方向に延びる軸を中心としたA軸方向に工具106を揺動させる。なお、前記水平方向に延びる軸は、本発明の第1軸の概念に含まれ、A軸は、本発明の移送軸及び揺動移送軸の概念に含まれる。この揺動装置110は、揺動支持体110aと、揺動支持体移送部110b(図2参照)とを有する。揺動支持体110aは、前記水平方向に延びる軸を中心として揺動可能となるように回動装置112に支持されている。また、揺動支持体110aは、主軸ヘッド108が当該主軸ヘッド108によって保持された工具106の回転軸が当該揺動支持体110aの揺動中心となる軸に対して直交するような姿勢を取るようにその主軸ヘッド108を支持している。揺動支持体移送部110bは、揺動支持体110aをA軸方向に揺動させることにより、当該揺動支持体110aとともに主軸ヘッド108及び工具106をA軸方向に揺動させる。揺動支持体移送部110bは、駆動源として図略のサーボモータを有しており、そのモータの動力によって揺動支持体110aを揺動させる。
回動装置112は、前記揺動装置110による工具106の揺動中心となる軸に対して垂直な方向に延びる軸を中心としたC軸方向に工具106及び揺動装置110をテーブル101aに対して相対的に回動させる。なお、前記工具106の揺動中心となる軸に対して垂直な方向に延びる軸は、本発明の第2軸の概念に含まれ、C軸は、本発明の移送軸及び回動移送軸の概念に含まれる。回動装置112は、回動支持体112aと、回動支持体移送部112b(図2参照)とを有する。回動支持体112aは、前記工具106の揺動中心となる軸に対して垂直な軸回りに回転可能となるように鉛直移送装置114に支持されている。また、回動支持体112aは、その下部において揺動装置110を支持している。回動支持体移送部112bは、回動支持体112aをC軸方向に回動させることにより、当該回動支持体112aとともにその回動支持体112aが支持する揺動装置110、主軸ヘッド108及び工具106をC軸方向に回動させる。回動支持体移送部112bは、駆動源として図略のサーボモータを有しており、そのモータの動力によって回動支持体112aを回動させる。
鉛直移送装置114は、X軸に直交し、鉛直方向に延びるZ軸に沿って工具106を移送するためのものである。なお、Z軸は、本発明の移送軸の概念に含まれる。この鉛直移送装置114は、ラム114aと、ラム移送部114b(図2参照)とを有する。ラム114aは、テーブル101aの上方に配置され、Z軸に沿って鉛直方向(上下方向)に移動可能となるように水平移送装置116に支持されている。また、ラム114aは、その下部によって、テーブル101aよりも上方で且つクロスレール105よりも下方の位置において回動装置112を支持している。ラム移送部114bは、ラム114aをZ軸方向に移送することにより、当該ラム114aとともにそのラム114aが支持する回動装置112、揺動装置110、主軸ヘッド108及び工具106をZ軸方向に移送する。ラム移送部114bは、駆動源として図略のサーボモータを有しており、そのモータの動力によってラム114aを移送する。
水平移送装置116は、X軸とZ軸の両方に直交するY軸に沿って工具106を移送するためのものである。なお、Y軸は、本発明の移送軸の概念に含まれる。この水平移送装置116は、サドル116aと、サドル移送部116b(図2参照)とを有する。サドル116aは、テーブル101aの上方でY軸に沿って移動可能となるようにクロスレール105に支持されている。すなわち、サドル116aは、テーブル101aの上方においてテーブル101aを幅方向に横切るように移動可能となっている。また、サドル116aは、その下部において鉛直移送装置114を支持している。サドル移送部116bは、サドル116aをY軸方向に移送することにより、当該サドル116aとともにそのサドル116aが支持する鉛直移送装置114、回動装置112、揺動装置110、主軸ヘッド108及び工具106をY軸方向に移送する。サドル移送部116bは、駆動源として図略のサーボモータを有しており、そのモータの動力によってサドル116aを移送する。
制御ボックス120は、テーブル移送装置102、揺動装置110、回動装置112、鉛直移送装置114及び水平移送装置116の制御や、主軸ヘッド108の制御、その他、工作機械の各部の制御を行うための機能を有する。この制御ボックス120内に本実施形態による数値制御装置2が組み込まれている。
次に、本実施形態による数値制御装置2の構成について説明する。
本実施形態による数値制御装置2は、テーブル移送装置102及び前記各移送部110b,112b,114b,116bに特定周期毎の指令パルス(移送指令)を出力することにより各移送装置102,110,112,114,116の数値制御を行う。この数値制御装置2は、図2に示すように、記憶部4と、メモリ5と、演算処理部6とを有する。
記憶部4は、ワーク設置情報と、後述する特定平面の情報と、加工プログラム(NCプログラム)と、一定値である距離パラメータと、移送軸座標算出関数とを記憶している。
ワーク設置情報は、テーブル101a上でのワーク100の位置及び姿勢を表す情報である。具体的には、ワーク設置情報は、テーブル101a上に設置されたワーク100に固定されるプログラム座標系としてのワーク座標系の基準座標系に対する相対的な位置及び傾きを表す。なお、基準座標系は、テーブル101a上に固定(設定)され、移送対象物の移送の基準となる座標系である。この基準座標系は、水平面内におけるテーブル101aの移送方向(X軸方向)に平行なx軸と、水平面内においてx軸に直交し、前記Y軸に平行なy軸と、x軸及びy軸の両方に直交し、前記Z軸に平行なz軸とからなる。また、基準座標系は、前記工具106の揺動中心となる軸に対して垂直な方向に延びる軸(工具106の回動方向であるC軸方向の中心となる軸)に垂直な特定平面(xy平面)を含んでいる。また、ワーク座標系は、互いに直交するx’軸、y’軸及びz’軸からなる。
ワーク設置情報は、基準座標系(xyz座標系)におけるワーク座標系の相対的な位置(基準座標系に対するワーク座標系の平行シフト量)及び基準座標系に対するワーク座標系の傾きの情報からなる。具体的には、ワーク設置情報は、ワーク座標系(x’y’z’座標系)と基準座標系(xyz座標系)との相対的な関係によって表され、(x0,y0,z0,α,β,γ)からなる。ここで、(x0,y0,z0)は、基準座標系上におけるワーク座標系の原点の座標値であり、(α,β,γ)は、基準座標系に対するワーク座標系の回転の姿勢を表すオイラー角である。このワーク設置情報は、ワーク100をテーブル101a上に設置するときに図略の入力装置を用いて入力され、記憶部4に記憶される。
加工プログラムは、ワーク100の加工を指示する加工指令である。加工プログラムには、ワーク座標系におけるツールパスが含まれる。ツールパスは、ワーク100の加工時における基準時刻の経過に応じた工具106の移動の軌跡を表すものである。このツールパスには、ワーク100の加工時に工具106の先端が通るワーク座標系での多数の指令点の位置座標と、その各指令点における工具106の姿勢(傾き)を規定する工具軸ベクトルの情報と、工具106の移動軌跡に沿った速度を示す送り速度指令とが含まれている。そして、ツールパスが示す工具の移動軌跡は、当該ツールパスに含まれる多数の指令点の隣り合うもの同士を繋ぎ、且つ、各指令点における工具軸ベクトルの傾きを満たすルールド曲面を構成する。
前記距離パラメータは、工具106の先端点から対応する上空点(後述)までの離間距離を表す制御用パラメータである。
前記移送軸座標算出関数は、任意の媒介変数に対応する先端点の基準座標系における位置座標及びその媒介変数に対応し且つこの先端点を始点とする工具軸ベクトルと、各移送軸座標との相関関係を規定する関数である。工具106の先端点が所定の位置座標に位置し且つその工具106の工具軸ベクトルが所定の工具軸ベクトルとなるような位置及び姿勢を工具106に取らせるために、各移送装置102,110,112,114,116が移送対象物を対応する移送軸方向において配置する位置座標を算出するための関数である。
また、記憶部4は、ワーク100の加工時に移送対象物を動かすときの加減速条件やその他の設定値(パラメータ)を記憶している。加減速条件には、例えば、各移送装置102,110,112,114,116が移送対象物を移送するときの各移送軸方向それぞれについての許容速度、許容加速度及び許容ジャークや、ワーク100に対する工具106の姿勢変化を含む移動の許容速度、許容加速度及び許容ジャークや、工具106の姿勢変化を含む移動量の積算値である媒介変数の速度の上限値や、媒介変数の加速度の上限値、媒介変数のジャークの上限値などが含まれる。なお、媒介変数は、工具106の姿勢変化を含む移動量の積算値である。具体的には、媒介変数は、工具106の先端点の移動量に工具106の姿勢(傾き)の変化量を加味した値の積算値であり、本実施形態では、工具106の先端点の移動量とその先端点から工具106の軸心に沿って当該工具106の基端側へ前記距離パラメータだけ離れた上空点の移動量との平均値からなる。
メモリ5は、各種情報を一次的に記憶するものであり、例えば、後述する局部補間後先端パス、姿勢調整情報、その他の情報を記憶する。
演算処理部6は、各種演算処理を行うものであり、機能ブロックとして、パス導出部12と、姿勢調整情報導出部16と、媒介変数時間関数導出部18と、移送指令導出部20と、制御部22とを有する。
パス導出部12は、記憶部4に記憶されている加工プログラムを読み取り、その読み取った加工プログラムに基づいて工具106の先端パス及び上空パスを求める。先端パスは、ワーク100の加工時における工具106の先端点の動きを基準座標系での当該先端点の位置座標と媒介変数の関数によって表すものであり、上空パスは、ワーク100の加工時における工具106の上空点の動きを基準座標系での当該上空点の位置座標と媒介変数の関数によって表すものである。パス導出部12は、機能ブロックとして、補間前パス導出部13及びパス補間部14を有する。
補間前パス導出部13は、加工プログラムに含まれるツールパスから補間前先端パスと補間前上空パスを算出するものである。具体的には、補間前パス導出部13は、記憶部4から読み取った加工プログラムからその加工プログラムに含まれているワーク座標系でのツールパス(ワーク座標系ツールパス)を読み取る。そして、補間前パス導出部13は、読み取ったワーク座標系ツールパスを記憶部4に記憶されているワーク設置情報に基づいて基準座標系上のツールパス(基準座標系ツールパス)に変換し、その変換後の基準座標系ツールパスから補間前先端パスと補間前上空パスを算出する。補間前パス導出部13は、算出した補間前先端パス及び補間前上空パスを記憶部4に記憶させる。
パス補間部14は、補間前パス導出部13によって算出された補間前先端パス及び補間前上空パスをそれぞれ補間する。なお、ここでいう補間とは、補間前先端パスと補間前上空パスが示す軌跡が滑らかな曲線となるようにそれら各パスを修正することをいう。パス補間部14は、機能ブロックとして、滑らか補間部14aと、パス局部フィルタ14bとを有する。
滑らか補間部14aは、補間前パス導出部13によって算出された補間前先端パスをそのパスが表す先端点の動きが滑らかになるように補間するとともに補間前パス導出部13によって算出された補間前上空パスをそのパスが表す上空点の動きが滑らかになるように補間する滑らか補間を行う。具体的には、滑らか補間部14aは、補間前パス導出部13によって算出された前記両パスのうち指令点を通る滑らかな曲線となるように解析的に補間可能な部分について、いわゆるブロック滑らか補間を実施する。具体的には、滑らか補間部14aは、補間前先端パスのうちの隣り合う指令点間の区間を指令ブロックとしてその指令ブロックと隣接する指令ブロックとがそれらの境界の指令点において滑らかに繋がるように補間前先端パスを補間する。詳しくは、滑らか補間部14aは、隣接する指令ブロックの境界の指令点の前後において補間前先端パスの媒介変数による一次微分値と二次微分値がそれぞれ連続変化し、かつ、その指令点を通るようなパスとなるように先端パスを補間する。ただし、補間前先端パスには、媒介変数の変化に対する先端パスの軌跡の変化が急激で、このようなブロック滑らか補間によっては指令点を通る滑らかな曲線となるように当該先端パスを補間できない箇所が含まれている場合がある。このような箇所としては、微小単位で増減を繰り返す異常データ部や、加工プログラムにおいて意図して指示されているコーナー部がある。また、補間前先端パスには、二次微分が不連続であっても、直線と円弧の接点部や、円と円の接点部などのように、当該滑らか補間部14aでは補間せずにそのまま放置する箇所もある。滑らか補間部14aは、補間前先端パスのうち以上のような箇所については補間せずに元のパスのまま残し、上記のように指令点を通り、その指令点の前後で媒介変数による一次微分値と二次微分値がそれぞれ連続変化するような曲線に補間できる部分を滑らか補間する。また、滑らか補間部14aは、補間前上空パスについて、補間前先端パスと同様の滑らか補間を実施する。
パス局部フィルタ14bは、補間前先端パス及び補間前上空パスのうち滑らか補間部14aによって補間されなかった、一次微分又は二次微分が不連続に変化する箇所で、且つ、その箇所が示す速度が0でない箇所を対象として前記局部補間を行う。
具体的には、パス局部フィルタ14bは、補間前先端パスのうち速度が0になる箇所(加工上、一端停止させたい箇所)を除いて、媒介変数による一次微分値が連続で且つ媒介変数による二次微分値が不連続になっている指令点と、媒介変数による一次微分値が不連続になっている指令点とを補間対象点とする。そして、パス局部フィルタ14bは、補間前先端パスのうち補間対象点の前後に亘り、後述する補間区間幅を有し、且つ、その補間区間幅の中央に補間対象点が位置する補間区間を当該補間対象点で不連続となっている微分値が連続となるように局部補間する。
より詳しくは、パス局部フィルタ14bは、補間前先端パスについて以下の局部補間処理を実施する。パス局部フィルタ14bによって局部補間される箇所としては、補間前先端パスのうち、例えば、図9に示すようなコーナー部や図10に示すような異常データ部が挙げられる。コーナー部の頂点に位置する指令点(図9のt[3])や異常データ部の各頂点に位置する各指令点(図10のt[2]〜t[6])では、媒介変数による一次微分値及び二次微分値が不連続になっており、これらの指令点は補間対象点となる。また、パス局部フィルタ14bは、補間前先端パスに補間対象点が複数存在し、隣り合う補間対象点の補間区間同士が重複している場合(図10のような場合)には、それら重複している補間区間のうち最も補間前先端パスの始点寄りに位置する補間区間の始点から最も補間前先端パスの終点寄りに位置する補間区間の終点までを1つの補間区間として、その補間区間内の先端パスを局部補間する。なお、以下、パス局部フィルタ14bによって全ての補間区間が局部補間された後の先端パスを局部補間後先端パスという。また、パス局部フィルタ14bは、局部補間後先端パスが各補間区間(隣り合う補間区間同士が重複している場合はそれらを結合した補間区間)の始点の前後及び終点の前後でそれぞれ連続して繋がるようにその補間区間を局部補間する。また、パス局部フィルタ14bは、補間前上空パスについて補間前先端パスと同様の局部補間処理を実施する。以下、パス局部フィルタ14bによって全ての補間区間が局部補間された後の上空パスを局部補間後上空パスという。局部補間後先端パスは、パス導出部12が加工プログラムに基づいて求める先端パスに相当し、局部補間後上空パスは、パス導出部12が加工プログラムに基づいて求める上空パスに相当する。
パス局部フィルタ14bの具体的な構成として、当該パス局部フィルタ14bは、局部補間部14c及び誤差補正部14dを機能ブロックとして有している(図2参照)。
局部補間部14cは、先端パスの補間区間を前後に拡大した調整区間にかかる複数の補間ブロックのそれぞれについて補間処理を行うことにより各補間ブロックの補間後関数を求めるとともに、それらの補間後関数を積算して補間後積算関数を求め、その求めた補間後積算関数から補間区間に相当する区間を抜き出すことによって、補間区間内の補間対象点において不連続となっている微分の変化が連続的な変化となるように補間した誤差付き先端補間パスを求める。また、局部補間部14cは、同様の処理により、上空パスについて補間区間内の補間対象点において不連続になっている微分の変化が連続的な変化となるように補間した誤差付き上空補間パスを求める。
誤差補正部14dは、局部補間部14cによって求められた誤差付き先端補間パスの補間区間の始点に対応する値が局部補間していない先端パスの補間区間の始点の値と一致し、かつ、その誤差付き先端補間パスの補間区間の終点に対応する値が局部補間していない先端パスの補間区間の終点の値に一致するように誤差付き先端補間パスを補正し、その補正後の先端補間パスを先端パスの補間区間と置換することによって局部補間後先端パスを導出する。また、誤差補正部14dは、同様の処理により、誤差付き上空補間パスを補正し、その補正後の上空補間パスを上空パスの補間区間と置換することによって局部補間後上空パスを導出する。
姿勢調整情報導出部16は、局部補間後先端パスと局部補間後上空パスのうち先端点からその先端点に対応する上空点へ向かう工具軸ベクトルの向きが急変する姿勢調整箇所を特定し、その特定した姿勢調整箇所の前後に亘る媒介変数の区間である姿勢調整区間を抽出し、その姿勢調整区間において工具軸ベクトルの向きの変化が緩やかになる方向にその姿勢調整区間における工具106の姿勢を調整するための姿勢調整情報を求める。なお、姿勢調整情報導出部16は、工具106の姿勢を調整すべき前記姿勢調整箇所として、局部補間後先端パスと局部補間後上空パスのうち局部補間後先端パスの進行方向に対して工具軸ベクトルが成す角度が工具106の短い移動距離の間で増減する箇所と、局部補間後先端パスと局部補間後上空パスのうち工具軸ベクトルを前記特定平面に射影した射影工具軸ベクトルの向きの媒介変数の増分に対する変化率がパラメータとして設定された一定の変化率を超える箇所とを特定する。また、射影工具軸ベクトルの向きの媒介変数の増分に対する変化率が一定の変化率を超える箇所には、例えば、局部補間後先端パスを前記特定平面に射影した射影先端パスが示す軌跡と局部補間後上空パスを前記特定平面に射影した射影上空パスが示す軌跡とが互いに離れていて且つ前記特定平面における射影工具軸ベクトルと基準方向(本実施形態では、y軸方向)との間の角度の媒介変数の増分に対する変化率が一定の変化率を超える区間や、前記特定平面に垂直な方向からみて局部補間後先端パスが示す軌跡と局部補間後上空パスが示す軌跡が重なる点(両軌跡の交点及び接点)を含む区間や、その両軌跡が重なる点が連続する箇所を含む区間などが含まれる。また、姿勢調整情報導出部16は、特定平面に垂直な方向から見て局部補間後先端パスが示す軌跡と局部補間後上空パスが示す軌跡が交差する交点を含む姿勢調整箇所については、工具106の先端点がその交点に対応する局部補間後先端パス上の点に位置する媒介変数においてその工具106の上空点が当該交点に対応する局部補間後上空パス上の点に位置するという条件を満たす姿勢調整情報を求める。
姿勢調整情報導出部16は、具体的には、局部補間後先端パスと局部補間後上空パスによって表される工具106の姿勢の変化を追跡し、局部補間後先端パス上の先端点に対応させる局部補間後上空パス上の上空点の対応関係を調整すべき箇所、及び/又は、射影工具軸ベクトルの向き(射影工具軸ベクトルの長さが0になる特異点では、C軸座標の角度算出の基準)を調整すべき箇所を見つけ、姿勢調整区間の情報と、先端上空対応調整関数S(s)及び/又は方向特定関数C(s)とを含む姿勢調整情報を求め、その姿勢調整情報をメモリ5に登録する。先端上空対応調整関数S(s)は、局部補間後先端パス上の先端点に対応させる局部補間後上空パス上の上空点を調整するために用いられる関数である。この先端上空対応調整関数S(s)は、本発明の先端上空対応関係の概念に含まれる。先端上空対応調整関数S(s)は、局部補間後先端パスのうち姿勢調整区間の始点に対応する点からその姿勢調整区間内の任意の媒介変数に対応する任意の点までの当該局部補間後先端パスに沿った移動距離と局部補間後上空パスのうち姿勢調整区間の始点に対応する点から局部補間後先端パス上の前記任意の点に対応させる当該局部補間後上空パス上の所定の点までの当該局部補間後上空パスに沿った移動距離とが一定の比率に維持されるように、前記任意の媒介変数と局部補間後上空パス上の前記所定の点を特定するための媒介変数である上空点特定用媒介変数との相関関係を規定する。また、方向特定関数C(s)は、射影工具軸ベクトルの向き(射影工具軸ベクトルの長さが0となる特異点では、C軸座標の角度算出の基準)を調整するために用いられる関数である。この方向特定関数C(s)は、射影先端パス上の点とその点に対応する射影上空パス上の点を通る直線である制御ラインと前記特定平面上の前記基準方向との間の角度が姿勢調整区間の始点から終点に至るまで均等な割合で変化するように当該制御ラインと基準方向との間の角度である方向特定角度と姿勢調整区間内の媒介変数との関係を規定する。
媒介変数時間関数導出部18は、記憶部4に記憶されている加減速条件と、局部補間後先端パス及び局部補間後上空パスと、姿勢調整情報導出部16によって導出されてメモリ5に登録された姿勢調整情報とに基づいて、基準時刻の経過に対する媒介変数の変化を表す媒介変数時間関数(媒介変数の速度曲線)を求める。具体的には、媒介変数時間関数導出部18は、記憶部4に記憶されている移送軸座標算出関数を用いて、局部補間後先端パス、局部補間後上空パス及び姿勢調整情報に基づいて各移送軸パス(X軸パス、Y軸パス、Z軸パス、A軸パス及びC軸パス)を算出する。そして、媒介変数時間関数導出部18は、その算出した各移送軸パスの媒介変数による一次微分の近似値及び媒介変数による二次微分の近似値を求め、各指令点間の各ブロックにおける一次微分近似値及び二次微分近似値から、移送対象物の各移送軸方向の加速度が加減速条件に含まれたその移送軸方向の許容加速度を超えないという条件を満たすように各ブロックの媒介変数sの速度の上限値を設定し、移送対象物の各移送軸方向への速度が各ブロックの速度の上限値を超えず且つ移送対象物を許容加速度の範囲内で局部補間後先端パス及び局部補間後上空パスに従って移送し得る最大の加速度を示すような媒介変数時間関数を導出する。
移送指令導出部20は、媒介変数時間関数導出部18によって導出された媒介変数時間関数と、局部補間後先端パスと、局部補間後上空パスと、姿勢調整情報と、移送軸座標算出関数とに基づいて各移送軸方向への移送対象物の基準単位時間毎(特定周期毎)の移送量を求め、その求めた各移送軸方向への移送対象物の基準単位時間毎の移送量を指令パルスとする。なお、基準単位時間は、基準時刻の単位時間のことである。また、指令パルスは、本発明の移送指令の概念に含まれる。
具体的には、移送指令導出部20は、媒介変数時間関数導出部18によって導出された媒介変数時間関数に基づいて基準時刻の基準単位時間毎の各時点に対応する媒介変数を導出する。また、移送指令導出部20は、その導出した各時点の媒介変数のうち姿勢調整区間内の媒介変数については、その媒介変数に対応する局部補間後先端パス上の先端点の位置座標と、その先端点から当該先端点に対する相対的な位置関係を姿勢調整情報に基づいて射影工具軸ベクトルの向きの変化が緩やかになる方向に調整した局部補間後上空パス上の上空点へ向かう工具軸ベクトルとを求める。また、移送指令導出部20は、前記導出した各時点の媒介変数のうち姿勢調整区間外の媒介変数については、その媒介変数に対応する局部補間後先端パス上の先端点の位置座標と、その先端点からその媒介変数に対応する局部補間後上空パス上の上空点へ向かう工具軸ベクトルとを求める。また、移送指令導出部20は、これらの各時点の媒介変数について求めた先端点の位置座標を工具106の先端点が通り且つこれらの各時点の媒介変数について求めた工具軸ベクトルに工具106の工具軸ベクトルが一致するように移送対象物を動かすための各移送軸方向への移送対象物の基準単位時間毎の移送量を求める。具体的には、移送指令導出部20は、上記のように各時点の媒介変数について求めた先端点の位置座標及び工具軸ベクトルから移送軸座標算出関数を用いて各時点の媒介変数に対応する各移送軸上の位置座標を求め、その求めた各時点における各移送軸上の位置座標からそれらの移送軸上での位置座標の基準単位時間当たりの変化量を求め、その変化量を対応する移送軸方向への移送対象物の基準単位時間毎の移送量とする。移送指令導出部20は、このようにして求めた各移送軸方向への移送対象物の基準単位時間毎の移送量を基準単位時間毎(特定周期毎)の指令パルスとする。
制御部22は、各移送装置102,110,112,114,116の動作を制御するものである。この制御部22は、移送指令導出部20によって求められた特定周期毎の指令パルスに応じて各移送装置102,110,112,114,116に移送対象物を移送させる。具体的には、制御部22は、移送指令導出部20によって求められた各移送軸方向についての前記特定周期毎の指令パルスのうちテーブル移送装置102のサーボモータ及び各移送部110b,112b,114b,116bのサーボモータのそれぞれに対応する移送軸方向への移送量を示す指令パルスをそれら各サーボモータに出力する。それによって、各移送装置102,110,112,114,116に出力した指令パルスに応じて移送対象物を移送させる。
次に、図3〜図7のフローチャートを参照して、本実施形態の数値制御装置2による数値制御プロセスについて説明する。
まず、パス導出部12は、記憶部4に記憶された加工プログラム(NCプログラム)を読み取り(図3のステップS1)、補間前パス導出部13がその加工プログラムから補間前先端パス及び補間前上空パスを求める(ステップS2)。この際、補間前パス導出部13は、加工プログラムに含まれているワーク座標系ツールパスを読み取り、そのワーク座標系ツールパスを記憶部4に記憶されているワーク設置情報に基づいて基準座標系ツールパスに変換し、その変換後の基準座標系ツールパスから補間前先端パス及び補間前上空パスを求める。なお、補間前パス導出部13は、この時、任意の媒介変数sに対する先端点p(s)の基準座標系における位置座標を表す関数を補間前先端パスとして導出し、その任意の媒介変数sに対する上空点P(s)に対する上空点P(s)の基準座標系における位置座標を表す関数を補間前上空パスとして導出する。すなわち、特定の媒介変数に対して補間前先端パス上の特定の先端点と補間前上空パス上の特定の上空点とが1対1で対応するように補間前先端パス及び補間前上空パスが求められる。
次に、パス補間部14の滑らか補間部14aが、補間前パス導出部13によって求められた補間前先端パスと補間前上空パスのそれぞれについてブロック滑らか補間を行う(ステップS3)。
滑らか補間部14aは、補間前先端パスのうちの隣接する指令ブロック同士の境界に位置する指令点の前後において一次微分と二次微分がそれぞれ連続変化し、かつ、その指令点を補間後の先端パスが通るように補間前先端パスを補間する。この際、滑らか補間部14aは、補間前先端パスのうちこのような補間が解析的に可能な部分についてのみブロック滑らか補間を行い、このような補間が解析的に不可能な部分については補間せずに元の補間前先端パスの軌跡のまま残す。詳しくは、滑らか補間部14aは、指令点における補間前先端パスの折れ曲がりの角度が予め設定された一定角度よりも大きい部分、及び、補間前先端パスに対してブロック滑らか補間を行った後の先端パスに生じた誤差が予め設定された許容誤差よりも大きくなる部分については、補間せずに元の軌跡のまま残す。滑らか補間部14aは、補間前上空パスについても同様にブロック滑らか補間を行う。
次に、パス局部フィルタ14bが、ブロック滑らか補間後の先端パス及び上空パスのそれぞれについて局部補間を行う(ステップS4)。パス局部フィルタ14bは、滑らか補間後の先端パスのうち媒介変数による一次微分の変化が連続でかつ媒介変数による二次微分の変化が不連続となっている指令点と媒介変数による一次微分の変化が不連続になっている指令点とを補間対象点とし、滑らか補間後の先端パスのうちその補間対象点の前後に亘り、特定の区間幅を有する補間区間のみを局部補間する。これにより、先端パスのうち前記ブロック滑らか補間が行われなかった指令点付近が局部補間され、その指令点において不連続であった微分値の変化が連続的な変化となるように補間される。このパス局部フィルタ14bによって行われる局部補間の詳細なプロセスが図4に示されている。
この局部補間では、まず、パス局部フィルタ14bの局部補間部14cが、補間区間の区間幅である補間区間幅をまだ算出していない補間対象点が滑らか補間後の先端パスに存在するか否かを判断する(ステップS12)。局部補間部14cは、そのような補間対象点が滑らか補間後の先端パスに存在すると判断した場合には、その補間区間幅をまだ算出していない補間対象点について分配区間幅及び補間区間幅の算出を行う(ステップS14)。なお、分配区間幅は、パスの媒介変数による一次微分を分配関数に基づいて分配する区間幅のことである。
このステップS14では、局部補間部14cは、まず、分配区間幅を算出し、その後、補間区間幅をその算出した分配区間幅以上の区間幅として求める。なお、本実施形態では、局部補間部14cは、補間区間幅として分配区間幅と等しい大きさの区間幅を求める。局部補間部14cは、補間対象点の前後で先端パスの媒介変数による一次微分値が異なる場合には、その前後の一次微分値の差が最大となる基準座標系の座標軸について、局部補間前の先端パスと局部補間後の先端パスとの補間対象点における位置誤差がその座標軸における許容誤差に等しくなるという条件を満たすように分配区間幅が求められる。詳しくは、例えば、次のようにして分配区間幅が算出される。
分配区間幅の算出式は、後述のパスの一次微分の分配に用いる分配関数によって異なるが、ここでは、後述のパスの一次微分の分配に直線型の分配関数を用いるものとして、分配区間幅の算出方法について説明する。
基準座標系の各座標軸のうちx軸において、先端パスの媒介変数sによる一次微分の補間対象点直前の値b1と補間対象点直後の値b2との差が最大となるとする。ここで、このx軸における先端パスの位置座標xの媒介変数sによる一次微分関数x’(s)は、次式(1)で表せる。なお、この一次微分関数x’(s)は、図8に太線で示されている。
x’(s)=dx/ds=αs+b1・・・(1)
なお、αは、所定の係数である。ここで、局部補間前の先端パスに対する局部補間後の先端パスの補間対象点での位置誤差は、図8中のハッチングを付した領域の面積に相当する。この位置誤差をEとすると、当該位置誤差Eは、次式(2)で求められる。
E=A×(b2−b1)/8・・・(2)
ここで、x軸における許容誤差をτとすると、前記誤差Eが許容誤差τに等しいという条件を満たす分配区間幅Aは、上記式(2)から次式(3)のように求められる。
A=8τ/(b2−b1)・・・(3)
なお、上記式(3)で求められる分配区間幅の値が予め設定された上限パラメータを超える場合には、その上限パラメータの値を分配区間幅Aとする。そして、本実施形態では、以上のようにして算出された分配区間幅Aに等しい区間幅を補間区間幅とする。
次に、局部補間部14cは、現在対象としている補間対象点についての補間区間が他の補間対象点の補間区間と重なるか否かを判断する(ステップS16)。ここで、局部補間部14cは、補間区間が重ならないと判断した場合には、上記ステップS12に戻り、以降のプロセスを繰り返し行う。一方、局部補間部14cは、補間区間が重なると判断した場合には、重なった補間区間同士を結合して1つの補間区間とする(ステップS18)。具体的には、局部補間部14cは、重なった各補間区間のうち最も先端パスの始点寄りに位置する補間区間の始点から最も先端パスの終点寄りに位置する補間区間の終点までの区間を1つの補間区間とする。なお、この場合、結合した後の1つの補間区間について後述の分配の際に用いる分配区間幅としては、各補間区間に対応する分配区間幅のうち最も小さい分配区間幅が採用される。この後、上記ステップS12に戻り、以降のプロセスが繰り返し行われる。
そして、ステップS12において、局部補間部14cが滑らか補間後の先端パスに補間区間幅を算出していない補間対象点がもう存在しないと判断した場合には、局部補間部14cは、次に、以降の局部補間処理を行う1つの補間区間を抽出する(ステップS20)。ここで、滑らか補間後の先端パスに局部補間処理を行っていない複数の補間区間が存在する場合には、局部補間部14cは、それらの補間区間のうち最も先端パスの始点寄りに位置する補間区間を抽出する。なお、上記ステップS18で結合された補間区間は、1つの補間区間として扱われる。
その後、局部補間部14cは、抽出した補間区間について調整区間を求める(ステップS22)。この調整区間(図9及び図10参照)は、補間区間を前後に分配区間幅Aの半分の区間幅aずつ拡大した区間であり、後述する補間後関数の算出対象となる補間ブロックを判断する基準となる。この後、局部補間部14cは、メモリ5に記憶されている補間後積算関数を0に初期セットする(ステップS24)。
次に、局部補間部14cは、後述の補間後関数を求める補間処理を実施するための先端パスの指令ブロックである補間ブロックを抽出する(ステップS26)。具体的には、局部補間部14cは、上記ステップS22で求めた調整区間内に少なくとも区間の一部が含まれる複数の指令ブロック(図9では先端パスの指令ブロックp0(s)〜p5(s)、図10では先端パスの指令ブロックp0(s)〜p7(s))を選択するとともに、それらの指令ブロックのうち後述の補間後関数を求める補間処理が終わっていないブロックでかつ最も先端パスの始点寄りに位置するブロックを補間ブロックとして抽出する。
次に、局部補間部14cは、抽出した補間ブロックの補間後関数を求める(ステップS28)。本実施形態では、局部補間部14cは、直線型の分配関数f(s)を用いて補間後関数を求める。直線型の分配関数f(s)は、次式(4)で表される。
f(s)=1/A (−A/2≦s≦A/2)・・・(4)
なお、分配区間−A/2≦s≦A/2の外側では、f(s)=0である。
局部補間部14cは、補間後関数の算出に当たって、まず、上記抽出した補間ブロック内の媒介変数が異なる各第1位置sを中央とし且つ分配区間幅Aと等しい区間幅を有する積分区間[s−A/2,s+A/2]を設定する。そして、局部補間部14cは、その設定した積分区間[s−A/2,s+A/2]内の媒介変数が異なる各第2位置Sでの先端パスの媒介変数による一次微分pi’(S)を求め、その各第2位置Sでの一次微分pi’(S)をその第2位置Sを中央とし且つ前記分配区間幅Aを有する分配区間内に分配関数f(s)に基づいて分配した場合に得られる分配後微分関数pi’(S)・f(s−S)を積分区間[s−A/2,s+A/2]にわたって積分し、それによって補間後関数qi’(s)を求める。換言すれば、局部補間部14cは、次式(5)によって補間ブロックpi(s)の補間後関数qi’(s)を求める。
このようにして得られた補間後関数qi’(s)は、補間ブロックpi(s)の区間幅が補間区間幅Aよりも大きい場合と小さい場合とで異なった形で表される。その各場合における補間後関数qi’(s)は、以下の通りである。
補間ブロックpi(s)の区間幅が補間区間幅A(=2a)よりも大きい場合には、その補間ブロックpi(s)の両端を規定する2つの指令点のうち先端パスの始点寄りの指令点をt[i]、先端パスの終点寄りの指令点をt[i+1]とし、補間区間幅Aの半分の区間幅をaとすると、当該補間ブロックpi(s)に対応する区間はt[i]<s<t[i+1]で表され、t[i]−a<t[i]+a<t[i+1]−a<t[i+1]+aの関係が成り立つ(図11参照)。この場合における補間ブロックpi(s)の媒介変数sによる一次微分pi’(s)は、図11中の太線で表される。
この場合において、上記補間処理によって得られる補間後関数qi’(s)は、下記のt[i]−a<s<t[i]+aの区間の関数qi11’(s)と、t[i]+a<s<t[i+1]−aの区間の関数qi12’(s)と、t[i+1]−a<s<t[i+1]+aの区間の関数qi13’(s)とに細分化される。
t[i]−a<s<t[i]+aの区間の関数qi11’(s)は、次式(6)で表される。
t[i]+a<s<t[i+1]−aの区間の関数qi12’(s)は、次式(7)で表される。
t[i+1]−a<s<t[i+1]+aの区間の関数qi13’(s)は、次式(8)で表される。
次に、補間ブロックpi(s)の区間幅が補間区間幅A=2aよりも小さい場合には、その補間ブロックpi(s)の両端を規定する2つの指令点のうち先端パスの始点寄りの指令点をt[i]、先端パスの終点寄りの指令点をt[i+1]とし、補間区間幅Aの半分の区間幅をaとすると、当該補間ブロックpi(s)に対応する区間はt[i]<s<t[i+1]で表され、t[i]−a<t[i+1]−a<t[i]+a<t[i+1]+aの関係が成り立つ(図12及び図13参照)。なお、図12は、補間ブロックpi(s)の区間幅が補間区間幅Aよりも小さく且つ補間区間幅Aの半分の区間幅aよりも大きい場合を示しており、図13は、補間ブロックpi(s)の区間幅が補間区間幅Aの半分の区間幅aよりも小さい場合を示している。これらの場合における補間ブロックpi(s)の媒介変数sによる一次微分関数pi’(s)は、図12及び図13中の太線で表される。
これらの場合において、上記補間処理によって得られる補間後関数qi’(s)は、下記のt[i]−a<s<t[i+1]−aの区間の関数qi21’(s)と、t[i+1]−a<s<t[i]+aの区間の関数qi22’(s)と、t[i]+a<s<t[i+1]+aの区間の関数qi23’(s)とに細分化される。
t[i]−a<s<t[i+1]−aの区間の関数qi21’(s)は、次式(9)で表される。
t[i+1]−a<s<t[i]+aの区間の関数qi22’(s)は、次式(10)で表される。
t[i]+a<s<t[i+1]+aの区間の関数qi23’(s)は、次式(11)で表される。
次に、局部補間部14cは、求めた補間後関数をメモリ5に記憶されている補間後積算関数に加算する(ステップS30)。ここでは、ステップS24で0にセットされた補間後積算関数に補間後関数が加算されるので、加算後の補間後積算関数は、上記ステップS28で求められた補間後関数に等しくなる。
次に、局部補間部14cは、調整区間に少なくとも区間の一部が含まれる全ての補間ブロックについて補間後関数を求めてその補間後関数を補間後積算関数に加算する補間処理を行ったか否かを判断する(ステップS32)。ここで、局部補間部14cは、まだ全ての補間ブロックの補間処理を終了していないと判断した場合には、上記ステップS26に戻り、次に補間処理を行う補間ブロックを抽出する。この際に抽出される補間ブロックは、調整区間に少なくとも区間の一部が含まれる指令ブロックのうち先に補間処理された補間ブロックに対して先端パスの終点側に隣り合う指令ブロックとなる。その後、局部補間部14cは、その抽出した補間ブロックについてステップS28の補間後関数の算出を行い、それによって得られた補間後関数を補間後積算関数に加算する。局部補間部14cは、このようなステップS26〜S30のプロセスを、ステップS32において調整区間に少なくとも一部が含まれる全ての補間ブロックの補間処理(分配、積分、加算)を終了したと判断するまで繰り返し行う。
そして、局部補間部14cは、ステップS32において調整区間に少なくとも一部が含まれる全ての補間ブロックの補間処理を終了したと判断した場合には、次に、誤差付き先端補間パスを求める(ステップS34)。具体的には、局部補間部14cは、上記補間後関数の加算によって得られた補間後積算関数から上記ステップS20で抽出した補間区間に相当する区間を抜き出すことによって誤差付き先端補間パスを求める。
次に、パス局部フィルタ14bの誤差補正部14dが、誤差付き先端補間パスのうち補間区間の始点に対応する点と終点に対応する点とにおける先端パスとの誤差を補正するための先端誤差補正パスを求める(ステップS36)。具体的には、誤差補正部14dは、補間区間の始点における先端誤差補正パスの値が当該始点における誤差付き先端補間パスの値から当該始点における先端パスの値を減じて得た値に等しく、かつ、補間区間の終点における先端誤差補正パスの値が当該終点における誤差付き先端補間パスの値から当該終点における先端パスの値を減じて得た値に等しいという条件を満たす先端誤差補正パスを求める。より具体的には、誤差補正部14dは、先端誤差補正パスをE(s)とし、誤差付き先端補間パスをr(s)とし、先端パスをp(s)とし、補間区間の始点をSsとし、補間区間の終点をSeとすると、以下の式(12),(13)を満たす先端誤差補正パスE(s)を求める。本実施形態では、誤差補正部14dは、この条件を満たす先端誤差補正パスE(s)の一例として、E(Ss)とE(Se)を直線で繋いだ先端誤差補正パスを求める。
E(Ss)=r(Ss)−p(Ss)・・・(12)
E(Se)=r(Se)−p(Se)・・・(13)
次に、誤差補正部14dは、誤差付き先端補間パスをステップS36で求めた先端誤差補正パスによって補正し、その補正後の先端補間パスを先端パスの補間区間と置換して局部補間後の補間区間の先端パスとする(ステップS38)。具体的には、誤差補正部14dは、誤差付き先端補間パスから先端誤差補正パスを差し引くことによって誤差付き先端補間パスを補正し、当該補正により、補間区間の始点での先端パスに対する誤差付き先端補間パスの誤差と、補間区間の終点での先端パスに対する誤差付き先端補間パスの誤差とを除去する。このため、補正後の先端補間パスは、補間区間の始点において先端パスと連続して繋がり、補間区間の終点において先端パスと連続して繋がるパスとなる。以上のプロセスにより、先端パスのうちの1つの補間区間が、当該補間区間内の補間対象点において不連続となっている微分値の変化が連続的な変化となるように局部補間される。
この後、局部補間部14cは、先端パスの全ての補間区間の局部補間が終了したか否かを判断する(ステップS40)。ここで、局部補間部14cが、まだ局部補間が終了していない補間区間が存在すると判断した場合には、上記ステップS20以降のプロセスが繰り返し行われる。具体的には、局部補間部14cは、局部補間が終了していない補間区間のうち最も先端パスの始点寄りに位置する補間区間を抽出し、その抽出した補間区間について補間後関数の算出、補間後積算関数への加算、誤差付き先端補間パスの導出、先端誤差補正パスの導出及び誤差付き先端補間パスの補正等が行われる。
一方、局部補間部14cが先端パスの全ての補間区間の局部補間が終了したと判断した場合には、パス局部フィルタ14bによる先端パスについての局部補間処理のプロセスが終了する。このようにして、パス局部フィルタ14bは、全ての補間区間が局部補間された先端パスである局部補間後先端パスを導出する。また、パス局部フィルタ14bは、以上の先端パスの局部補間処理と同様に上空パスの局部補間を行い、全ての補間区間を局部補間した上空パスである局部補間後上空パスを導出する。
次に、姿勢調整情報導出部16が、姿勢調整情報の導出を行う(図3のステップS5)。この姿勢調整情報導出部16による姿勢調整情報の導出の詳細なプロセスは、図5に示されている。
この姿勢調整情報の導出プロセスでは、姿勢調整情報導出部16は、まず、局部補間後先端パスの媒介変数sによる一次微分が連続変化している区間を対象として、局部補間後先端パス及び局部補間後上空パスに工具軸ベクトルの向きの変化が緩やかになるように工具106の姿勢を調整すべき姿勢調整箇所が存在しないかを調べ、姿勢調整箇所が存在すれば、その姿勢調整箇所を含む姿勢調整区間を接続条件及びその姿勢調整区間の区間タイプと共に抽出し、それらの情報を姿勢調整情報としてメモリ5に登録する(ステップS42)。
具体的には、姿勢調整情報導出部16は、図15に示すように局部補間後先端パスの進行方向に対して工具軸ベクトルが成す角度が短い移動距離(パラメータとして設定)の間で増減する箇所が局部補間後先端パスと局部補間後上空パスに存在する場合には、その箇所の前後に一定のパラメータ分の区間幅を加えた媒介変数sの区間(ss,se)を姿勢調整区間として設定し、その設定した姿勢調整区間(ss,se)の情報をメモリ5に登録するとともに、その姿勢調整区間(ss,se)の区間タイプは区間タイプ2に該当するという情報をメモリ5に登録する。また、姿勢調整情報導出部16は、姿勢調整区間(ss,se)の始点ssにおける射影先端パス上の点とその始点ssにおける射影上空パス上の点を通る直線からなる制御ラインと前記特定平面(xy平面)上の前記基準方向(y軸方向)との間の角度Csと、姿勢調整区間(ss,se)の終点seにおける射影先端パス上の点とその終点seにおける射影上空パス上の点を通る直線からなる制御ラインと前記特定平面上の前記基準方向との間の角度Ceとを共にNULLとしてメモリ5に登録する。
また、姿勢調整情報導出部16は、前記特定平面上において射影先端パスが示す軌跡と射影上空パスが示す軌跡が互いに離れており且つ局部補間後先端パス上の先端点からその先端点に対応する局部補間後上空パス上の上空点へ向かう工具軸ベクトルを前記特定平面に射影した射影工具軸ベクトルの向きの媒介変数の増分に対する変化率(媒介変数の増分に対する射影工具軸ベクトル(制御ライン)と前記基準方向との間の角度の変化率)がパラメータとして設定された一定の変化率を超える姿勢調整箇所が存在する場合(図16参照)には、その箇所の前後に亘る区間(ss,se)を姿勢調整区間として設定し、その設定した姿勢調整区間(ss,se)の情報をメモリ5に登録するとともに、その姿勢調整区間(ss,se)の区間タイプは区間タイプ1に該当するという情報をメモリ5に登録する。また、姿勢調整情報導出部16は、姿勢調整区間(ss,se)の始点ssに対応する射影先端パス上の点とその点に対応する射影上空パス上の点(始点ssにおける射影上空パス上の点)を通る直線である制御ラインと前記基準方向との間の角度Cs(以下、姿勢調整区間の始点における制御ラインの角度Csという)と、姿勢調整区間(ss,se)の終点seに対応する射影先端パス上の点とその点に対応する射影上空パス上の点(終点seにおける射影上空パス上の点)を通る直線である制御ラインと前記基準方向との間の角度Ce(以下、姿勢調整区間の終点における制御ラインの角度Ceという)とを求め、姿勢調整区間(ss,se)における制御ラインの角度範囲(Cs,Ce)の情報をメモリ5に登録する。
また、姿勢調整情報導出部16は、図17〜図20に示すように、射影先端パスと射影上空パスが交差又は接する場合には、その交差又は接する点に対応する局部補間後先端パス上の媒介変数smに局部補間後上空パス上の媒介変数Smを一致させることでその点を特異点とし、その特異点の媒介変数の前後に亘り且つ特定の区間幅(パラメータとして設定)を有する区間を姿勢調整区間として設定し、その姿勢調整区間(ss,se)の情報をメモリ5に登録する。そして、姿勢調整情報導出部16は、この場合には、始点ss及び終点seにおける制御ラインと射影先端パスの接線方向との間の角度を求め、その角度に基づいてその始点ss及び終点seにおいて制御ラインが射影先端パスの接線方向に対して平行に近いか否かを判別し、その判別結果に応じて以下のように姿勢調整区間を複数の区間に分割してその各分割区間毎の区間情報、制御ラインの角度範囲及び区間タイプを求めてメモリ5に登録する。なお、姿勢調整情報導出部16は、姿勢調整区間の始点ss及び終点seにおける制御ラインと射影先端パスの接線方向との間の角度のうち小さい方の角度が記憶部4に記憶されたパラメータとしての判別角度(例えば10度〜30度の間の特定の角度)以上である場合には、制御ラインは射影先端パスの接線方向に対して平行に近くないと判別し、当該角度が判別角度未満である場合には、制御ラインは射影先端パスの接線方向に対して平行に近いと判別する。
そして、姿勢調整情報導出部16は、射影先端パスと射影上空パスが交差する場合において、姿勢調整区間の始点ss及び終点seの両方で制御ラインが射影先端パスの接線方向に対して平行に近くない場合(図17参照)には、姿勢調整区間をその始点から射影先端パスと射影上空パスの交点の媒介変数までの始点側区間とその交点の媒介変数から当該姿勢調整区間の終点までの終点側区間とに分割し、その始点側区間及び終点側区間は、共に区間タイプ1に該当すると判定する。また、姿勢調整情報導出部16は、姿勢調整区間の始点における制御ラインの角度Csと姿勢調整区間の終点における制御ラインの角度Ceとを求めるとともに、その求めた両角度Cs,Ceの平均の角度を前記交点における制御ラインの角度Cmとして求める。そして、姿勢調整情報導出部16は、以上のように求めた情報から、先端パスについての始点側区間(ss,sm)、上空パスについての始点側区間(Ss,Sm)(Ss=ssである)、始点側区間における制御ラインの角度範囲(Cs,Cm)及び始点側区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての終点側区間(sm,se)、上空パスについての終点側区間(Sm,Se)(Se=seである)、終点側区間における制御ラインの角度範囲(Cm,Ce)及び終点側区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録する。
また、姿勢調整情報導出部16は、射影先端パスと射影上空パスが交差する場合において、姿勢調整区間の始点ssで制御ラインが射影先端パスの接線方向に対して平行に近くなく且つ姿勢調整区間の終点seで制御ラインが射影先端パスの接線方向に対して平行に近い場合(図18参照)には、姿勢調整区間の始点から射影先端パスと射影上空パスの交点の媒介変数までの始点側区間と、その交点の媒介変数から当該交点の媒介変数と当該姿勢調整区間の終点との間の分割点までの終点側第1区間と、前記分割点から姿勢調整区間の終点までの終点側第2区間とに姿勢調整区間を分割し、その始点側区間は区間タイプ1に該当し、終点側第1区間は区間タイプ1に該当し、終点側第2区間は区間タイプ2に該当すると判定する。また、姿勢調整情報導出部16は、射影先端パスと射影上空パスの交点における射影先端パスについての媒介変数smと当該交点における射影上空パスについての媒介変数Smを求め、射影先端パスについての分割点skをsk=(sm+se)/2として求める。また、姿勢調整情報導出部16は、姿勢調整区間の始点における制御ラインの角度Csと姿勢調整区間の終点における制御ラインの角度Ceを求めるとともに、その求めた両角度Cs,Ceの平均の角度を射影先端パスと射影上空パスの交点における制御ラインの角度Cmとして求める。また、姿勢調整情報導出部16は、分割点における制御ラインの角度Ckとして姿勢調整区間の終点における制御ラインの角度Ceに等しい角度を導出する。また、姿勢調整情報導出部16は、前記分割点skに対応する射影先端パス上の点を通り且つ前記基準方向との間の角度が前記Ckとなる直線と射影上空パスとの交点に対応する媒介変数位置を射影上空パスの分割点Skとして求める。そして、姿勢調整情報導出部16は、以上のように求めた情報から、先端パスについての始点側区間(ss,sm)、上空パスについての始点側区間(Ss,Sm)(Ss=ssである)、始点側区間における制御ラインの角度範囲(Cs,Cm)及び始点側区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての終点側第1区間(sm,sk)、上空パスについての終点側第1区間(Sm,Sk)、終点側第1区間における制御ラインの角度範囲(Cm,Ck)及び終点側第1区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての終点側第2区間(sk,se)、上空パスについての終点側第2区間(Sk,Se)(Se=seである)、終点側第2区間における制御ラインの角度範囲(Ck,Ce)及び終点側第2区間が区間タイプ2に該当するという情報を姿勢調整情報としてメモリ5に登録する。
また、姿勢調整情報導出部16は、図18の場合とは逆に、姿勢調整区間の始点ssで制御ラインが射影先端パスの接線方向に対して平行に近く且つ姿勢調整区間の終点seで制御ラインが射影先端パスの接線方向に対して平行に近くない場合には、姿勢調整区間の始点から当該始点と射影先端パスと射影上空パスの交点の媒介変数との間の分割点までの始点側第1区間と、その分割点から射影先端パスと射影上空パスの交点の媒介変数までの始点側第2区間と、前記交点から姿勢調整区間の終点までの終点側区間とに姿勢調整区間を分割し、その始点側第1区間は区間タイプ2に該当し、始点側第2区間は区間タイプ1に該当し、終点側区間は区間タイプ1に該当すると判定する。そして、姿勢調整情報導出部16は、射影先端パスと射影上空パスの交点における射影先端パスについての媒介変数sm及び射影上空パスについての媒介変数Smを上記と同様に求め、分割点skをsk=(ss+sm)/2として求める。また、姿勢調整情報導出部16は、姿勢調整区間の始点における制御ラインの角度Cs、姿勢調整区間の終点における制御ラインの角度Ce及び射影先端パスと射影上空パスの交点における制御ラインの角度Cmを上記と同様にして求め、分割点における制御ラインの角度Ckとして姿勢調整区間の始点における制御ラインの角度Csに等しい角度を導出する。また、姿勢調整情報導出部16は、射影上空パスの分割点Skを上記と同様にして求める。そして、姿勢調整情報導出部16は、以上のように求めた情報から、先端パスについての始点側第1区間(ss,sk)、上空パスについての始点側第1区間(Ss,Sk)(Ss=ssである)、始点側第1区間における制御ラインの角度範囲(Cs,Ck)及び始点側第1区間が区間タイプ2に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての始点側第2区間(sk,sm)、上空パスについての始点側第2区間(Sk,Sm)、始点側第2区間における制御ラインの角度範囲(Ck,Cm)及び始点側第2区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての終点側区間(sm,se)、上空パスについての終点側区間(Sm,Se)(Se=seである)、終点側区間における制御ラインの角度範囲(Cm,Ce)及び終点側区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録する。
また、姿勢調整情報導出部16は、射影先端パスと射影上空パスが交差する場合において、姿勢調整区間の始点ss及び終点seの両方で制御ラインが射影先端パスの接線方向に対して平行に近い場合(図19参照)には、姿勢調整区間をその始点から射影先端パスと射影上空パスの交点の媒介変数までの始点側区間とその交点の媒介変数から当該姿勢調整区間の終点までの終点側区間とに分割し、その始点側区間及び終点側区間は、共に区間タイプ2に該当すると判定する。また、姿勢調整情報導出部16は、射影先端パスと射影上空パスの交点における射影先端パスについての媒介変数sm及び射影上空パスについての媒介変数Smと、姿勢調整区間の始点における制御ラインの角度Cs及び姿勢調整区間の終点における制御ラインの角度Ceと、前記交点における制御ラインの角度Cmとを上記と同様にして求める。そして、姿勢調整情報導出部16は、求めた情報から、先端パスについての始点側区間(ss,sm)、上空パスについての始点側区間(Ss,Sm)(Ss=ssである)、始点側区間における制御ラインの角度範囲(Cs,Cm)及び始点側区間が区間タイプ2に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての終点側区間(sm,se)、上空パスについての終点側区間(Sm,Se)(Se=seである)、終点側区間における制御ラインの角度範囲(Cm,Ce)及び終点側区間が区間タイプ2に該当するという情報を姿勢調整情報としてメモリ5に登録する。
また、姿勢調整情報導出部16は、射影先端パスと射影上空パスがある1点で接する場合において、姿勢調整区間の始点ss及び終点seの両方で制御ラインが射影先端パスの接線方向に対して平行に近い場合(図20参照)には、姿勢調整区間をその始点から射影先端パスと射影上空パスとの接点の媒介変数までの始点側区間とその接点の媒介変数から当該姿勢調整区間の終点までの終点側区間とに分割し、その始点側区間及び終点側区間は、共に区間タイプ2に該当すると判定する。そして、姿勢調整情報導出部16は、上空パスについての姿勢調整区間の始点Ss及び終点Seと、姿勢調整区間の始点における制御ラインの角度Cs及び姿勢調整区間の終点における制御ラインの角度Ceを上記と同様に求め、前記接点における射影先端パスについての媒介変数sm及び射影上空パスについての媒介変数Smを図19の場合における上記交点についての媒介変数sm,Smと同様に求め、前記接点における制御ラインの角度Cmを図19の場合における上記交点における制御ラインの角度Cmと同様に求める。そして、姿勢調整情報導出部16は、求めた情報から、始点側区間についての姿勢調整情報として図19の場合と同様の内容の情報をメモリ5に登録し、終点側区間についての姿勢調整情報として図19の場合と同様の内容の情報をメモリ5に登録する。
また、姿勢調整情報導出部16は、局部補間後先端パス及び局部補間後上空パスに射影工具軸ベクトルの長さが0になる特異点(工具軸ベクトルが特定平面に対して垂直となる特異点)が連続する区間(以下、特異点連続区間という)が存在する場合には、その特異点連続区間の前後に亘り且つ特定の区間幅を有する区間を姿勢調整区間として設定する。
そして、姿勢調整情報導出部16は、この特異点連続区間を含む姿勢調整区間の始点ss及び終点seの両方で制御ラインが射影先端バスの接線方向(進行方向)に対して平行に近くない場合(図21参照)には、先端パスについての姿勢調整区間(ss,se)、上空パスについての姿勢調整区間(Ss,Se)(Ss=ss、Se=seである)、姿勢調整区間における制御ラインの角度範囲(Cs,Ce)及び姿勢調整区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録する。
また、姿勢調整情報導出部16は、特異点連続区間を含む姿勢調整区間の始点ssで制御ラインが射影先端パスの接線方向(進行方向)に対して平行に近くなく且つその姿勢調整区間の終点seで制御ラインが射影先端パスの接線方向(進行方向)に対して平行に近い場合(図22参照)には、姿勢調整区間の始点ssと特異点連続区間の始点から少し終点寄りに位置する第1分割点smとの間の始点側区間と、第1分割点smと特異点連続区間の終点から少し始点寄りに位置する第2分割点snとの間の中間区間と、第2分割点snから姿勢調整区間の終点seまでの終点側区間とに姿勢調整区間を分割し、その始点側区間及び中間区間は区間タイプ1に該当し、終点側区間は区間タイプ2に該当すると判定する。そして、姿勢調整情報導出部16は、上空パスにおける第1分割点Smを前記第1分割点smに等しい媒介変数として求め、上空パスにおける第2分割点Snを前記第2分割点smに等しい媒介変数として求める。また、姿勢調整情報導出部16は、姿勢調整区間の始点における制御ラインの角度Csと姿勢調整区間の終点における制御ラインの角度Ceを求め、第1分割点における制御ラインの角度Cmを姿勢調整区間の始点における制御ラインの角度Csに等しい角度とし、第2分割点における制御ラインの角度Cnをその第2分割点における射影先端パスの進行方向と前記基準方向との間に等しい角度とする。そして、姿勢調整情報導出部16は、このように求めた情報から、先端パスについての始点側区間(ss,sm)、上空パスについての始点側区間(Ss,Sm)(Ss=ssである)、始点側区間における制御ラインの角度範囲(Cs,Cm)及び始点側区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての中間区間(sm,sn)、上空パスについての中間区間(Sm,Sn)、中間区間における制御ラインの角度範囲(Cm,Cn)及び中間区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての終点側区間(sn,se)、上空パスについての終点側区間(Sn,Se)(Se=seである)、終点側区間における制御ラインの角度範囲(Cn,Ce)及び終点側区間が区間タイプ2に該当するという情報を姿勢調整情報としてメモリ5に登録する。
また、姿勢調整情報導出部16は、特異点連続区間を含む姿勢調整区間の始点ss及び終点seの両方で制御ラインが射影先端パスの接線方向(進行方向)に対して平行に近い場合(図23参照)には、姿勢調整区間の始点ssと特異点連続区間の始点から少し終点寄りに位置する第1分割点smとの間の始点側区間と、第1分割点smと特異点連続区間の終点から少し始点寄りに位置する第2分割点snとの間の中間区間と、第2分割点snから姿勢調整区間の終点seまでの終点側区間とに姿勢調整区間を分割し、その始点側区間及び終点側区間は区間タイプ2に該当し、中間区間は区間タイプ1に該当すると判定する。そして、姿勢調整情報導出部16は、図22の場合と同様にして、上空パスにおける第1分割点Sm、上空パスにおける第2分割点Sn、姿勢調整区間の始点における制御ラインの角度Cs及び姿勢調整区間の終点における制御ラインの角度Ceを求める。また、姿勢調整情報導出部16は、第1分割点における制御ラインの角度Cmをその第1分割点における射影先端パスの進行方向と前記基準方向との間の角度に等しい角度とし、第2分割点における制御ラインの角度Cnをその第2分割点における射影先端パスの進行方向と前記基準方向との間の角度に等しい角度とする。そして、姿勢調整情報導出部16は、このように求めた情報から、先端パスについての始点側区間(ss,sm)、上空パスについての始点側区間(Ss,Sm)(Ss=ssである)、始点側区間における制御ラインの角度範囲(Cs,Cm)及び始点側区間が区間タイプ2に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての中間区間(sm,sn)、上空パスについての中間区間(Sm,Sn)、中間区間における制御ラインの角度範囲(Cm,Cn)及び中間区間が区間タイプ1に該当するという情報を姿勢調整情報としてメモリ5に登録し、先端パスについての終点側区間(sn,se)、上空パスについての終点側区間(Sn,Se)(Se=seである)、終点側区間における制御ラインの角度範囲(Cn,Ce)及び終点側区間が区間タイプ2に該当するという情報を姿勢調整情報としてメモリ5に登録する。
次に、姿勢調整情報導出部16は、メモリ5に登録した各区間に、方向特定関数C(s)と先端上空対応調整関数S(s)を設定していない区間が存在するか否かを判別する(ステップS44)。姿勢調整情報導出部16がメモリ5に登録された区間に両関数C(s),S(s)を設定していない区間はないと判断した場合には、姿勢調整情報の導出プロセスは終了する。一方、姿勢調整情報導出部16は、メモリ5に登録した区間に両関数C(s),S(s)を設定していない区間が存在すると判断した場合には、次に、それら両関数C(s),S(s)をまだ設定していない区間から1つの区間を抽出して、その抽出した区間の区間タイプが区間タイプ1であるか否かを判断する(ステップS46)。このとき、姿勢調整情報導出部16は、メモリ5にその区間について登録された区間タイプの情報に基づいて、抽出した区間の区間タイプを判断する。姿勢調整情報導出部16は、抽出した区間の区間タイプが区間タイプ1であると判断した場合には、その区間の方向特定関数C(s)を導出し、その導出した関数C(s)をメモリ5に当該区間の姿勢調整情報として追加登録する(ステップS48)。
具体的には、姿勢調整情報導出部16は、以下の式(14)によって方向特定関数C(s)を算出する。
C(s)=Cs+(Ce−Cs)×(s−ss)/(se−ss)・・・(14)
ただし、ss≦s≦se
そして、姿勢調整情報導出部16は、算出した方向特定関数C(s)を対応する区間の情報と関連付けてメモリ5に姿勢調整情報として追加登録する。このステップS48の後、上記ステップS44に戻り、以降の処理が繰り返し行われる。
一方、上記ステップS46において、姿勢調整情報導出部16が、抽出した区間の区間タイプが区間タイプ1ではない、すなわち区間タイプ2であると判断した場合には、姿勢調整情報導出部16は、その抽出した区間について先端上空対応調整関数S(s)を導出し、その導出した関数S(s)を当該区間の姿勢調整情報としてメモリ5に追加登録する(ステップS52)。ここで、姿勢調整情報導出部16は、先端上空対応調整関数S(s)の導出に際して、局部補間後上空パスのうち前記抽出した区間の始点に対応する点からその区間内の媒介変数が異なる各点に対応する点までの局部補間後上空パスに沿った移動距離が局部補間後先端パスのうち前記抽出した区間の始点に対応する点から前記媒介変数が異なる前記各点に対応する点までの局部補間後先端パスに沿った移動距離に対して一定の比率に維持されるように局部補間後先端パスについての媒介変数と局部補間後上空パスについての媒介変数との相関関係を規定した先端上空対応調整関数S(s)を求める。
具体的には、姿勢調整情報導出部16は、姿勢調整区間(ss,se)内に複数の中間点を設定して、局部補間後先端パスのうち姿勢調整区間の始点ssに対応する点から各中間点に対応する点及び終点seに対応する点までの当該局部補間後先端パスに沿った移動距離と、局部補間後上空パスの姿勢調整区間の始点ssに対応する点から各中間点に対応する点及び終点seに対応する点までの当該局部補間後上空パスに沿った移動距離とをそれぞれ導出する。例えば、図14に示すように、始点s0を姿勢調整区間の始点ssに等しい点として設定するとともに、終点s5を姿勢調整区間の終点seに等しい点として設定し、その始点s0と終点s5との間に4つの中間点s1,s2,s3,s4を設定する。そして、姿勢調整情報導出部16は、始点s0に対応する局部補間後先端パス上の点p(s0)から各中間点s1,s2,s3,s4及び終点s5に対応する局部補間後先端パス上の各点p(s1),p(s2),p(s3),p(s4),p(s5)までの局部補間後先端パスに沿った移動距離leni(i=0,1,2,3,4,5)を算出する。また、姿勢調整情報導出部16は、始点s0に対応する局部補間後上空パス上の点P(s0)から各中間点s1,s2,s3,s4及び終点s5に対応する局部補間後上空パス上の各点P(s1),P(s2),P(s3),P(s4),P(s5)までの局部補間後上空パスに沿った移動距離LENi(i=0,1,2,3,4,5)を算出する。そして、姿勢調整情報導出部16は、姿勢調整区間内における任意の媒介変数と姿勢調整区間の始点からその任意の媒介変数までの区間に対応する局部補間後先端パスに沿った移動距離との相関関係を示す先端パス上距離算出関数len(s)を導出する。具体的には、姿勢調整情報導出部16は、点(s0,len0)、(s1,len1)、(s2,len2)、(s3,len3)、(s4,len4)、(s5,len5)を順番に結ぶ曲線を表す関数を先端パス上距離算出関数len(s)(s0≦s≦s5)として導出する。また、姿勢調整情報導出部16は、姿勢調整区間の始点に対応する局部補間後上空パス上の点からその局部補間後上空パスに沿った任意の移動距離とその移動距離だけ局部補間後上空パスの姿勢調整区間の始点に対応する点から局部補間後上空パスに沿って離れた点の媒介変数との相関関係を示す上空パス上距離−媒介変数対応関数SS(LEN)を導出する。具体的には、姿勢調整情報導出部16は、点(LEN0,s0)、(LEN1,s1)、(LEN2,s2)、(LEN3,s3)、(LEN4,s4)、(LEN5,s5)を順番に結ぶ曲線を表す関数を上空パス上距離−媒介変数対応関数SS(LEN)(LEN0≦LEN≦LEN5)として導出する。そして、姿勢調整情報導出部16は、導出した先端パス上距離算出関数len(s)及び上空パス上距離−媒介変数対応関数SS(LEN)に基づいて、先端上空対応調整関数S(s)を導出する。具体的には、姿勢調整情報導出部16は、以下の式(15)によって、当該関数S(s)を導出する。
S(s)=SS(len(s)×LEN5/len5)・・・(15)
姿勢調整情報導出部16は、以上のようにして導出した先端上空対応調整関数S(s)をメモリ5に姿勢調整情報の一部として追加登録する。この後、上記ステップS44に戻って以降の処理が繰り返し行われる。
以上のような姿勢調整情報の導出プロセスが行われた後、次に、媒介変数時間関数導出部18が、基準時刻Tの経過に対する媒介変数の変化を表す媒介変数時間関数を導出する(図3のステップS6)。
具体的には、媒介変数時間関数導出部18は、記憶部4に記憶されている各移送軸毎の許容加速度を含む加減速条件と、局部補間後先端パス及び局部補間後上空パスと、メモリ5に登録された姿勢調整情報とに基づいて前記移送軸座標算出関数を用いて媒介変数時間関数(媒介変数の速度曲線)を求める。詳しくは、媒介変数時間関数導出部18は、以下のようにして媒介変数時間関数を求める。
媒介変数時間関数導出部18は、まず、図24中の階段状速度曲線で表される媒介変数速度上限関数を求める。媒介変数速度上限関数は、各移送軸パスの各指令点間のブロック毎の媒介変数sの速度ds/dTの上限値を表す関数である。図24中の階段状速度曲線のうち水平になっている各部分がブロックを表し、当該部分の速度ds/dTがそのブロックの媒介変数sの速度ds/dTの上限値に相当する。なお、各ブロックは、それぞれについての媒介変数sの増分が等しくなるように元々設定されているが、その各ブロックの媒介変数sの増分に相当する距離を進むのに要する時間は様々であり、図24では、媒介変数sの増加を時間tの進行に換算して横軸に表している。そして、媒介変数時間関数導出部18は、以下のようにして各ブロックの媒介変数sの速度ds/dTの上限値を求めることによって媒介変数速度上限関数を求める。
媒介変数時間関数導出部18は、対象とするブロックがメモリ5に姿勢調整情報として登録されていない区間に該当する場合には、局部補間後先端パス及び局部補間後上空パスに基づいて、その対象とするブロック内の所定の媒介変数saの点とその点の近傍の前後2点ずつの合計5点(sa−2e,sa−e,sa,sa+e,sa+2e)における局部補間後先端パス上の各先端点の位置座標及びその各先端点から局部補間後上空パス上の対応する上空点へ向かう各工具軸ベクトルを求め、その求めた各先端点の位置座標及び各工具軸ベクトルから移送軸座標算出関数を用いて前記5点(sa−2e,sa−e,sa,sa+e,sa+2e)における各移送軸座標kp[i][](i=0,1,2,3,4)を求める。
また、媒介変数時間関数導出部18は、対象とするブロックがメモリ5に姿勢調整情報として登録されている区間に該当し且つその区間について方向特定関数C(s)がメモリ5に登録されている場合には、対象とするブロック内の所定の媒介変数saの点とその点の近傍の前後2点ずつの合計5点(sa−2e,sa−e,sa,sa+e,sa+2e)における局部補間後先端パス上の各先端点の位置座標を求めるとともに、その区間の方向特定関数C(s)を用いて前記5点の媒介変数位置に対応する方向特定角度を求め、前記求めた各先端点を前記特定平面へ射影した射影先端点を通り且つ基準方向に対して前記求めた各方向特定角度を成す直線が射影上空パスと交差する各交点に対応する局部補間後上空パス上の各上空点の位置座標を求める。そして、前記5点(sa−2e,sa−e,sa,sa+e,sa+2e)の媒介変数位置に対応する各先端点からこの求めた各上空点のうちの対応する点へ向かう工具軸ベクトルをそれぞれ求める。また、媒介変数時間関数導出部18は、対象とするブロックがメモリ5に姿勢調整情報として登録されている区間に該当し且つその区間について先端上空対応調整関数S(s)がメモリ5に登録されている場合には、対象とするブロック内の所定の媒介変数saの点とその点の近傍の前後2点ずつの合計5点(sa−2e,sa−e,sa,sa+e,sa+2e)における局部補間後先端パス上の各先端点の位置座標を求めるとともに、対象とするブロックが該当する区間についての先端上空対応調整関数S(s)に基づいてその5点の媒介変数に対応する局部補間後上空パスについての5点の媒介変数を求めてその求めた5点の媒介変数に対応する局部補間後上空パス上の上空点の位置座標を求め、前記5点(sa−2e,sa−e,sa,sa+e,sa+2e)に対応する各先端点から5点の媒介変数に対応する局部補間後上空パス上の各上空点へ向かう工具軸ベクトルをそれぞれ求める。そして、媒介変数時間関数導出部18は、以上のように求めた各先端点の位置座標及び各工具軸ベクトルから移送軸座標算出関数を用いて5点(sa−2e,sa−e,sa,sa+e,sa+2e)における各移送軸座標kp[i][](i=0,1,2,3,4)を求める。
なお、前記eは、媒介変数における微小距離を示すパラメータである。また、i=0は点(sa−2e)に、i=1は点(sa−e)に、i=2は点saに、i=3は点(sa+e)に、i=4は点(sa+2e)にそれぞれ対応している。また、[]は、各移送軸それぞれについての値であることを意味する。
そして、媒介変数時間関数導出部18は、下記式(16)によって点(sa−e)における移送軸パスの媒介変数による一次微分の近似値ds1[ax]を求め、下記式(17)によって点(sa+e)における移送軸パスの媒介変数による一次微分の近似値ds2[ax]を求め、この求めた2つの近似値ds1[ax],ds2[ax]と下記式(18)を用いて前記媒介変数saに対応する点における移送軸パスの媒介変数による二次微分の近似値ddss[ax]を求める。なお、axは、各移送軸を表すインデックスである。
ds1[ax]=(kp[2][ax]−kp[0][ax])/2e・・・(16)
ds2[ax]=(kp[4][ax]−kp[2][ax])2e・・・(17)
ddss[ax]=(ds2[ax]−ds1[ax])/2e・・・(18)
仮に、媒介変数saの点を等速度vで通過するとした場合には、各移送軸毎の加速度a[]は、v2×ddss[]で近似できることから、記憶部4に記憶されている加減速条件中の各移送軸毎の許容加速度をA[]とすると、媒介変数時間関数導出部18は、対象とするブロックの速度上限値vmを次式(19)によって求める。
vm=(A[ax]/ddss[ax])1/2・・・(19)
そして、媒介変数時間関数導出部18は、このようにして求めた各移送軸毎の対応するブロックの速度上限値vmと加工プログラムに含まれる速度指令のうちそのブロックに対応する速度との中で最も低い値をそのブロックの媒介変数sの速度ds/dTの上限値とする。媒介変数時間関数導出部18は、全てのブロックの媒介変数sの速度ds/dTの上限値を以上のような方法でそれぞれ求め、その求めた各ブロックの上限値を繋いで媒介変数速度上限関数を求める。
次に、媒介変数時間関数導出部18は、記憶部4に記憶された加減速条件に含まれる媒介変数sの許容速度、許容加速度及び許容ジャークに基づいて、媒介変数速度上限関数が表す階段状速度曲線を超えないように連続変化する加減速曲線ds(T)/dTを求め、その求めた加減速曲線ds(T)/dTを基準時刻Tで積分することにより媒介変数時間関数s(T)を算出する。
具体的には、媒介変数時間関数導出部18は、まず、加減速曲線ds(T)/dTのうちの加速領域の速度曲線(速度関数)を始点側から終点側へ向かって所定の時刻区間毎にその時刻区間の時刻区間幅tsとともに順次求めてメモリ5に登録していく。この時、媒介変数時間関数導出部18は、それまでにメモリ5に登録した速度曲線の最終点(s1,f1,t1)から目標点(s2,f2)として設定したブロックの終点へ到達する過程でその最終点における速度f1がそのブロックの速度ds/dTの上限値(速度上限値f2)まで上昇し得るような速度曲線を各時刻区間毎にその区間の時刻区間幅tsとともに求める。なお、このとき求められる速度曲線の形態は、前記最終点と目標点との媒介変数の差(s2−s1)、前記最終点における速度f1、速度上限値f2、前記最終点における加速度a1、許容加速度及び許容ジャーク等の条件によって異なる。その各条件に応じた速度曲線の求め方が図25〜図31に示されている。
図25及び図26には、最終点(s1,f1,t1)から目標点(s2,f2)に達する前に速度が速度上限値f2に達する場合が示されている。なお、t2は、s2に到達したときの時刻である。具体的には、図25は、最終点(s1,f1,t1)から許容ジャークに従って加速度がa1から許容加速度まで増加するように速度をf1から上昇させ(時刻区間ts1)、その後、許容加速度に等しい加速度で速度が上昇し(時刻区間ts2)、その後、速度が速度上限値f2を超えないように許容ジャークに従って加速度を低下させて速度が速度上限値f2に達し(時刻区間ts3)、その後、速度上限値f2に等しい速度で目標点(s2,f2)に到達する(時刻区間ts4)という形態の速度曲線を示しており、媒介変数時間関数導出部18は、このような条件に従って各時刻区間毎の速度曲線(速度関数)をその各区間の時刻区間幅ts1〜ts4とともに算出し、その算出した速度曲線を対応する時刻区間とともにメモリ5に登録する。また、図26は、最終点(s1,f1,t1)から許容ジャークに従って加速度がa1から増加するように速度をf1から上昇させる(時刻区間ts5)が、速度上限値f2が近いため、速度が許容加速度に達する前に許容ジャークに従って加速度を低下させて速度上限値f2に達し(時刻区間ts6)、その後、速度上限値f2に等しい速度で目標点(s2,f2)に到達する(時刻区間ts7)という形態の速度曲線を示しており、媒介変数時間関数導出部18は、このような条件に従って各時刻区間毎の速度曲線(速度関数)をその各区間の時刻区間幅ts5〜ts7とともに算出し、その算出した速度曲線を対応する時刻区間とともにメモリ5に登録する。
また、図27〜図29には、許容ジャークに従って加速度が低下している途中に目標点(s2,f2)に達する場合が示されている。具体的には、図27は、最終点(s1,f1,t1)から許容ジャークに従って加速度がa1から増加するように速度をf1から上昇させる(時刻区間ts8)が、最終点から目標点(s2,f2)までの距離(s2−s1)が小さいため、速度が許容加速度に達する前に、許容ジャークに従って加速度を低下させて速度が速度上限値f2に達するとともに目標点(s2,f2)に到達する(時刻区間ts9)という形態の速度曲線を示しており、媒介変数時間関数導出部18は、このような条件に従って各時刻区間毎の速度曲線(速度関数)をその各区間の時刻区間幅ts8,ts9とともに算出し、その算出した速度曲線を対応する時刻区間とともにメモリ5に登録する。また、図28は、最終点(s1,f1,t1)から許容ジャークに従って加速度がa1から許容加速度まで増加するように速度をf1から上昇させ(時刻区間ts10)、その後、許容加速度に等しい加速度で速度が上昇し(時刻区間ts11)、その後、速度が速度上限値f2を超えないように許容ジャークに従って加速度を低下させている途中で目標点(s2,f2)に到達する(時刻区間ts12)という形態の速度曲線を示しており、媒介変数時間関数導出部18は、このような条件に従って各時刻区間毎の速度曲線(速度関数)をその各区間の時刻区間幅ts10〜ts12とともに算出し、その算出した速度曲線を対応する時刻区間とともにメモリ5に登録する。また、図29は、最終点(s1,f1,t1)から目標点(s2,f2)までが非常に近いとともにその最終点における速度f1と速度上限値f2とが非常に近く、且つ、最終点における加速度a1が大きいため、その最終点から許容ジャークに従って加速度をa1から低下させながら速度がf1から上昇して速度上限値f2に達するとともに目標点(s2,f2)に到達する(時刻区間ts13)という形態の速度曲線を示しており、媒介変数時間関数導出部18は、このような条件に従って時刻区間幅ts13とともにその区間の速度曲線(速度関数)を算出し、その算出した速度曲線を時刻区間ts13とともにメモリ5に登録する。
また、図30には、媒介変数sが目標点に対応する値s2に達しても速度上限値f2まで速度を上げることができない場合が示されている。媒介変数時間関数導出部18は、速度曲線を算出していてこのような場合に該当する場合には、その算出した速度曲線をメモリ5に登録しない。
また、図31には、最終点(s1,f1,t1)における速度f1と速度上限値f2が非常に接近しているとともにその最終点における加速度a1が比較的大きいため、最終点(s1,f1,t1)から許容ジャークに従って加速度を低下させても速度が速度上限値f2を超えてしまう場合が示されている。媒介変数時間関数導出部18は、速度曲線の算出に際してこのような場合に該当する場合には、1つ前にメモリ5に登録された速度曲線のデータを取り消すとともに、さらにその1つ前にメモリ5に登録された速度曲線の最終点から今回の目標点に至り、且つ、速度が今回の目標点に対応する速度上限値に達し得るような速度曲線を求めるための再計算を行う。この場合の再計算の方法は、上記各場合と同様である。
そして、媒介変数時間関数導出部18は、上記のような速度曲線の算出と登録を順次行う。媒介変数時間関数導出部18は、求めた速度曲線をメモリ5に登録する際、併せて、目標点に到達時の時刻t2及び加速度a2をメモリ5に登録しておき、次の目標点を設定して速度曲線を算出するときには、その登録した時刻t2を最終点における時刻t1として用いるとともにその登録した加速度a2を最終点における加速度a1として用いて次の速度曲線を算出する。このように最終点における速度に対して次の目標点に対応する速度上限値が小さくなるまで求めた速度曲線によって、加減速曲線のうち加速領域の速度曲線が得られる。
次に、媒介変数時間関数導出部18は、加減速曲線のうちの減速領域についてパスの終点側から始点側へ向かって上記と同様の速度曲線の算出と登録を順次行い、それによってその減速領域の速度曲線を求める。そして、媒介変数時間関数導出部18は、求めた加速領域の速度曲線の終点と減速領域の速度曲線の始点を繋ぎ合わせることによって加減速曲線ds(T)/dTを求める。そして、媒介変数時間関数導出部18は、その求めた加減速曲線ds(T)/dTを基準時刻Tで積分することにより媒介変数時間関数s(T)を算出する。
次に、移送指令導出部20による基準時刻Tの導出及びパルス補間と、制御部22によるテーブル移送装置102及び各移送部110b,112b,114b,116bへのパルス出力が行われる(図3のステップS7)。
このステップS7の詳細なプロセスは、図6のフローチャートに示されている。このプロセスでは、まず、移送指令導出部20は、基準時刻Tが上記ステップS6で求められた媒介変数時間関数の始点となる時刻に合うように当該基準時刻Tを初期セットするとともに、現在の各移送軸の位置座標をoldp[]に設定する(図6のステップS56)。
次に、移送指令導出部20は、初期セットした基準時刻Tに対応する媒介変数位置sを求める(ステップS58)。具体的には、移送指令導出部20は、媒介変数時間関数導出部18によって求められた媒介変数時間関数s(T)を用いて、初期セットした基準時刻Tに対応する媒介変数位置sを求める。
次に、移送指令導出部20は、求めた媒介変数位置sに対応する各移送軸座標newp[]を移送軸座標算出関数を用いて求める(ステップS60)。当該ステップの詳細なプロセスは、図7のフローチャートに示されている。
具体的には、移送指令導出部20は、上記ステップS58で導出された媒介変数位置sがメモリ5に登録されている姿勢調整情報の区間内の媒介変数であるか否かを判断する(ステップS76)。ここで、移送指令導出部20は、上記ステップS58で導出された媒介変数位置sがメモリ5に登録されている区間内の媒介変数であると判断した場合には、次に、その媒介変数位置sが該当した区間に対応する姿勢調整情報として先端上空対応調整関数S(s)がメモリ5に登録されているか否かを判断する(ステップS78)。ここで、移送指令導出部20は、その区間に対応する姿勢調整情報として先端上空対応調整関数S(s)がメモリ5に登録されていないと判断した場合には、次に、上記ステップS58で導出された媒介変数位置sに対応させる局部補間後上空パスについての媒介変数位置として、その区間に対応する方向特定関数C(s)に基づいて調整した局部補間後上空パス上の上空点の媒介変数ssを求める(ステップS80)。具体的には、移送指令導出部20は、その区間に対応する姿勢調整情報に含まれる方向特定関数C(s)を用いて上記ステップS58で導出された媒介変数位置sに対応する制御ラインと基準方向との間の角度である方向特定角度を求め、上記ステップS58で導出された媒介変数位置sに対応する局部補間後先端パス上の先端点を前記特定平面へ射影した射影先端点を通り且つ前記基準方向に対して前記求めた方向特定角度を成す直線が射影上空パスと交差する交点に対応する局部補間後上空パス上の上空点についての媒介変数ssを算出する。
次に、移送指令導出部20は、上記ステップS80で求めた方向特定角度C(s)±n×180度(n=0,1,2,3,・・・)で求められる各角度の中から、上記ステップS56で設定された移送軸座標oldp[]のうちの回動移送軸座標(C軸座標oldp[Ci])に最も近い角度を回動軸座標候補値に設定する(ステップS82)。
次に、移送指令導出部20は、上記ステップS58で導出された媒介変数位置sに対応する局部補間後先端パス上の先端点p(s)の基準座標系での座標と、この先端点p(s)から上記ステップS80で求めた媒介変数ssに対応する上空点P(ss)へ向かう工具軸ベクトルとからなる工具位置を求める(ステップS84)。
そして、移送指令導出部20は、求めた工具位置に対応する各移送軸の位置座標newp[]を算出する(ステップS86)。具体的には、移送指令導出部20は、工具106の先端点がステップS84で求めた先端点p(s)の位置座標に配置され、且つ、その工具106の工具軸ベクトル(軸心)がステップS84で求めた工具軸ベクトルに一致するような位置及び姿勢を工具106に取らせるために、テーブル移送装置102がテーブル101aを、揺動支持体移送部110bが揺動支持体110aを、回動支持体移送部112bが回動支持体112aを、ラム移送部114bがラム114aを、サドル移送部116bがサドル116aをそれぞれ対応する移送軸上で配置する位置座標を算出する。なお、工具位置が特異点に該当する場合には、C軸座標は不定となるため、移送指令導出部20は、上記ステップS82で求めた回動軸座標候補値をC軸座標とする。
上記ステップS78において、移送指令導出部20は、上記ステップS58で導出した媒介変数位置sが含まれる区間に対応する姿勢調整情報として先端上空対応調整関数S(s)がメモリ5に登録されていると判断した場合には、上記ステップS58で導出した媒介変数位置sに対応させる局部補間後上空パスについての媒介変数ss(上空点特定用媒介変数)を、その区間に対応する先端上空対応調整関数S(s)に基づいて求める(ステップS88)。すなわち、移送指令導出部20は、上記ステップS58で導出した媒介変数sを先端上空対応調整関数S(s)に代入して得た値を当該媒介変数ssとする。
次に、移送指令導出部20は、上記ステップS58で導出した媒介変数位置sが含まれる区間に対応する姿勢調整情報として方向特定関数C(s)がメモリ5に登録されているか否かを判断する(ステップS90)。ここで、移送指令導出部20は、その区間に対応する姿勢調整情報として方向特定関数C(s)がメモリ5に登録されていると判断した場合には、上記ステップS82以降の処理を行う。一方、移送指令導出部20は、その区間に対応する姿勢調整情報として方向特定関数C(s)がメモリ5に登録されていないと判断した場合には、次に、上記ステップS56で設定された移送軸座標oldp[]のうちの回動移送軸座標(C軸座標oldp[Ci])に等しい角度に回動軸座標候補値を設定し(ステップS92)、その後、上記ステップS84,S86の処理を行う。
また、移送指令導出部20は、上記ステップS76において、上記ステップS58で導出した媒介変数位置sがメモリ5に登録されている区間内の媒介変数ではないと判断した場合には、上記ステップS58で導出した媒介変数位置sに対応する局部補間後上空パス上の上空点についての媒介変数位置ssとして、上記ステップS58で導出した媒介変数位置sに等しい位置(媒介変数)を設定し(ステップS94)、その後、上記ステップS92の処理を行う。
次に、移送指令導出部20は、以上のようにして求めた各移送軸毎の位置座標newp[]を旧座標oldp[]とする(図6のステップS62)。
次に、移送指令導出部20は、上記ステップS56で初期セットした基準時刻Tに基準単位時間dTを加算して新たな基準時刻Tを求める(ステップS64)。
次に、移送指令導出部20は、新たな基準時刻Tに対応する媒介変数位置sを上記ステップS58と同様にして求め(ステップS66)、その求めた媒介変数位置sに対応する各移送軸上の位置座標newp[]を求める(ステップS68)。当該ステップS68では、移送指令導出部20は、基本的には、上記ステップS60のと同様、ずなわち、図7のフローチャートに従って各移送軸上の位置座標newp[]を求める。ただし、当該ステップS68において上記ステップS82に対応するプロセスで回動軸座標候補値を求めるときには、上記ステップS62で設定した旧座標oldp[]のC軸座標oldp[Ci]に最も近い角度を回動軸座標候補値とし、上記ステップS92に対応するプロセスで回動軸座標候補値を求めるときには、上記ステップS62で設定した旧座標oldp[]のC軸座標oldp[Ci]に等しい角度に回動軸座標候補値を設定する。
次に、移送指令導出部20は、指令パルスを算出する(ステップS70)。具体的には、移送指令導出部20は、ステップS68で求めた各移送軸毎の位置座標newp[]から上記ステップS62で設定された各移送軸毎の旧座標oldp[]の対応するものを差し引くことによって、基準単位時間dT当たりの各移送軸における位置座標の変化量、すなわち各移送軸についての移送対象物の基準単位時間dT当たりの移送量を求め、その求めた移送量を実時刻の単位時間毎(前記特定周期毎)の指令パルスとする。
その後、制御部22が、テーブル移送装置102及び各移送部110b,112b,114b,116bのサーボモータへ、移送指令導出部20によって求められた前記特定周期当たりの指令パルスのうち対応する移送軸についての移送量を示す指令パルスを出力する(ステップS72)。これにより、テーブル移送装置102はテーブル101aを、揺動支持体移送部110bは揺動支持体110aを、回動支持体移送部112bは回動支持体112aを、ラム移送部114bはラム114aを、サドル移送部116bはサドル116aを、それぞれ制御部22からの指令パルスが示す移送量の分だけ、前記特定周期当たりに対応する移送軸に沿って動かす。
次に、移送指令導出部20は、シャットダウンの処理を行うか否かを判断する(ステップS74)。ここで、移送指令導出部20がシャットダウンの処理を行うと判断した場合には、シャットダウンの処理が実行されて本実施形態の数値制御プロセスは終了し、移送指令導出部20がシャットダウンを行わないと判断した場合には、上記ステップS62以降の処理が繰り返し行われる。再度行われるステップS62では、移送指令導出部20は、上記ステップS68で求めた各移送軸毎の位置座標newp[]を旧座標oldp[]に設定する。
以上のようにして、本実施形態の数値制御装置2による制御プロセスが行われる。
以上説明したように、本実施形態では、姿勢調整情報導出部16が、媒介変数の増分に対する射影工具軸ベクトルの向きの変化率が一定の変化率を超える姿勢調整箇所の前後に亘る姿勢調整区間において射影工具軸ベクトルの向きの変化が緩やかになる方向に工具106の姿勢を調整するための姿勢調整情報を求め、移送指令導出部20が、姿勢調整区間内の媒介変数に対応する局部補間後先端パス上の先端点からその先端点に対応する相対的な位置関係を姿勢調整情報に基づいて射影工具軸ベクトルの向きの変化が緩やかになる方向に調整した局部補間後上空パス上の上空点へ向かう工具軸ベクトルを求めて、姿勢調整区間内についてはこの求めた工具軸ベクトルと先端点の位置座標に基づいて指令パルスを導出するため、姿勢調整箇所において射影工具軸ベクトルの向きが急変するのを防ぐことができる。この射影工具軸ベクトルの向きは、A軸方向と対応しているので、本実施形態によれば、回動装置112が揺動装置110、主軸ヘッド108及び工具106をテーブル101aに対して急回転させるのを防ぐことができる。その結果、この急回転に起因するワークの加工精度の低下及び機械ショックの発生を抑制することができる。
また、本実施形態では、移送指令導出部20が、姿勢調整区間内の媒介変数に対応する先端点の位置座標を先端パス上で求めるとともに、その先端点から当該先端点に対する相対的な位置関係を射影工具軸ベクトルの向きの変化が緩やかになる方向に姿勢調整情報に基づいて調整した上空パス上の上空点へ向かう工具軸ベクトルを求め、その求めた先端点の位置座標及び工具軸ベクトルに基づいて指令パルスを導出するため、姿勢調整区間における射影工具軸ベクトルの向きの変化が緩やかになるように調整した後でも、工具106の姿勢が先端パス上の各先端点とその各先端点に対応する上空パス上の各上空点とを繋いだ曲面に対して垂直な方向に振れるのを抑制することができる。このことによっても、ワークの加工精度の低下を抑制することができる。
また、本実施形態では、特に、前記特定平面に垂直な方向から見て局部補間後先端パスが示す軌跡と局部補間後上空パスが示す軌跡が交差する交点を通過する時に、特定平面に垂直な方向から見て工具の先端点と上空点が重なるような姿勢、すなわち工具軸ベクトルが特定平面に対して垂直となる姿勢を工具106に取らせることができ、当該交点を通過する過程で工具106及び揺動装置110がテーブル101aに対して相対的に急回転するのを有効に防ぐことができる。具体的には、本実施形態のように前記交点において工具軸ベクトルが特定平面に対して垂直となる姿勢を工具106に取らせれば、特定平面に垂直な方向から見て先端点と上空点が交点に近づくにつれて互いに接近し、交点の通過後、互いに離れていくという動作を、交点の通過時にA軸方向における工具106の傾きを逆向きに変化するように揺動装置110に工具106を揺動させることで実現することができる。その結果、前記交点の通過時に工具106及び揺動装置110がC軸方向に急回転するのを防ぐことができる。
また、本実施形態では、区間タイプ1に該当する区間については、制御ラインと基準方向との間の角度が均等な割合で変化するように当該区間内の各媒介変数に対応する制御ラインと基準方向との間の角度が求められて、その角度に基づいて先端点に対する上空点の位置関係が調整された工具軸ベクトルが求められる。そして、この工具軸ベクトルに基づいて指令パルスが求められ、その指令パルスに応じて移送対象物の移送が行われるため、回動装置112によってテーブル101aに対して相対的に回動される工具106及び揺動装置110の回動角度の変化を直接的に緩やかな変化にすることができる。このため、当該区間については、確実に工具106及び揺動装置110の回動角度の変化を緩やかにすることができる。
また、本実施形態では、工具軸ベクトルが特定平面に対して垂直となる特異点について、方向特定関数C(s)から求めた方向特定角度を当該特異点におけるC軸座標として用いるため、当該特異点においてC軸座標が一意的に定まり、不定状態を解消することができる。しかも、この特異点の方向特定角度は、対象とする区間において制御ラインと基準方向との間の角度を均等な割合で変化させる方向特定関数から求められるので、その対象区間において射影工具軸ベクトルの向きを緩やかに変化させるという条件を満たす。このため、本実施形態では、区間タイプ1に該当する区間において、特異点における回動移送軸方向の位置座標の不定状態を解消しつつ、工具106及び回動装置112の向きを緩やかに変化させることができる。
また、本実施形態では、区間タイプ2に該当する区間については、当区間の始点から局部補間後先端パスに沿った先端点の移動距離と当該区間の始点から局部補間後上空パスに沿った上空点の移動距離とが常に一定の比率に維持された状態で工具106の先端点と上空点を移動させることができる。このため、先端点に対して遅れていた上空点が先端点を急に追い越すような工具106の姿勢変化や、その逆のような工具106の姿勢変化が生じるのを防ぐことができ、回動装置112がそのような工具106の姿勢変化を実現するために工具106及び揺動装置110をテーブル101aに対して相対的に大きく急回転させるのを防ぐことができる。
また、本実施形態では、パス補間部14のパス局部フィルタ14bの局部補間部14cによる補間前先端パス及び補間前上空パスの局部補間によって、補間前先端パス及び補間前上空パスにおいて媒介変数による一次微分値が不連続になっていることにより速度が不連続になっている点を速度が連続するように補間することができ、補間前先端パス及び補間前上空パスにおいて媒介変数による二次微分値が不連続になっていることにより加速度が不連続になっている点を加速度が連続するように補間することができる。このため、補間前先端パス及び補間前上空パス中の異常データ等により媒介変数による微分値が不連続になっていてそのことに起因して急激な速度変化や加速度変化が生じる箇所について、その急激な速度変化や加速度変化を抑制し、当該箇所における機械ショックの発生を抑制できる。しかも、本実施形態では、移送指令導出部20が各移送軸についての指令パルスを導出する前に、補間前先端パス及び補間前上空パスのうち微分値が不連続になっている箇所を補間してそれらのパスから機械ショックの要因を取り除くことができる。このため、移送指令導出部20が指令パルスを導出した後、その指令パルスのうち機械ショックの要因となる箇所を補間するような加減速処理を不要とすることができる。
また、本実施形態では、補間前パス導出部13が、加工プログラムに含まれるワーク座標系ツールパスを読み取り、その読み取ったワーク座標系ツールパスをワーク設置情報に基づいて基準座標系上のパスに変換することによって基準座標系ツールパスを導出し、その導出した基準座標系ツールパスに基づいて補間前先端パス及び補間前上空パスを導出するため、基準座標系に対してワーク座標系が傾くような姿勢でワーク100がテーブル101a上に設置された場合でも、そのワーク100の姿勢に応じた姿勢調整箇所の位置を的確に特定して、その姿勢調整箇所近傍の動作におけるテーブル101aに対する工具106及び揺動装置110の急回転を防止することができる。
なお、今回開示された実施形態は、すべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した実施形態の説明ではなく特許請求の範囲によって示され、さらに特許請求の範囲と均等の意味及び範囲内でのすべての変更が含まれる。
例えば、本発明による数値制御装置は、上記実施形態で示した工作機械以外の各種工作機械に適用することが可能である。例えば、機械テーブルが回転する5軸工作機械にも、本発明による数値制御装置を適用することができる。
また、数値制御装置は、パス補間部を備えていなくてもよく、加工プログラムのツールパスから求めた補間していない先端パス及び上空パスに基づいて、姿勢調整情報の導出、媒介変数時間関数の導出、指令パルスの導出を行ってもよい。
また、上記実施形態では、直線型の分配関数を用いて先端パス及び上空パスの媒介変数による一次微分を分配区間へ分配したが、その分配方式に限定されるものではない。すなわち、先端パス及び上空パスの媒介変数による一次微分の分配に、直線型の分配関数以外の種々の分配関数を用いてもよい。
なお、この分配に用いる分配関数は、区間[−A/2,A/2]における積分が1となり、且つ、その区間の中央に対して左右対称の形状を示す関数が望ましい。このような分配関数の一例として、次式で表されるベル型の分配関数f(s)が挙げられる。
なお、分配区間−A/2≦s≦A/2の外側では、f(s)=0である。
このベル型の分配関数を用いた場合には、上記直線型の分配関数を用いた場合よりも補間区間をより滑らかな曲線となるように補間することができる。また、パスのうち媒介変数による一次微分が連続で二次微分が不連続な部分については、直線型の分配関数を用いて一次微分の分配を行い、パスのうち媒介変数による一次微分が不連続に変化する部分については、ベル型の分配関数を用いて一次微分の分配を行えば、局部補間後のパスの媒介変数による二次微分の変化の連続性を保障することができる。
また、上記実施形態では、方向特定関数C(s)を一次関数として算出したが、姿勢調整区間の始点及び終点における制御ラインの角度の媒介変数による一次微分及び二次微分を近似的に算出して、その算出した微分に基づいて、姿勢調整区間の始点でその前後の方向特定関数が滑らかに繋がり、且つ、姿勢調整区間の終点でその前後の方向特定関数が滑らかに繋がるような方向特定関数を求めることが好ましい。
また、姿勢調整情報導出部は、射影先端パスが示す軌跡と射影上空パスが示す軌跡とが互いに離れていて且つ特定平面における射影工具軸ベクトルと基準方向との間の角度の媒介変数の増分に対する変化率が一定の変化率を超える姿勢調整箇所が先端パス及び上空パスに存在する場合(図16に示すような場合)において、その姿勢調整箇所の前後に亘る姿勢調整区間を区間タイプ2に該当すると判別するとともにその姿勢調整区間について先端上空対応調整関数S(s)を導出し、移送指令導出部は、その先端上空対応調整関数S(s)に基づいて当該姿勢調整区間において射影工具軸ベクトルの向きが緩やかになる方向に調整した工具軸ベクトルを求めてもよい。
また、前記分配区間幅及び前記補間区間幅の導出方法は、上記した方法に限定されるものではなく、上記方法以外の種々の導出方法により分配区間幅及び補間区間幅を求めてもよい。また、上記実施形態では、補間区間幅を分配区間幅に等しい大きさの区間幅としたが、補間区間幅はこのような区間幅に限定されない。すなわち、補間区間幅は、分配区間幅よりも大きい区間幅であってもよい。
また、パスの局部補間においては、補間区間内の補間対象点において必ずしも媒介変数による二次微分値が連続となるように補間する必要はない。例えば、パスの補間区間内の補間対象点において媒介変数による一次微分値は連続となるが、二次微分値は依然不連続である状態にその補間区間を局部補間してもよい。
また、滑らか補間後のパスにおいて媒介変数による二次微分値が上限値を超える区間が一定区間連続して存在する場合には、その一定区間分だけ分配区間を広げた区間を補間区間としてもよい。
また、姿勢調整情報導出部は、方向特定関数C(s)を導出してメモリに登録した場合には、媒介変数時間関数の導出時における媒介変数時間関数導出部の演算負荷及び指令パルスの導出時(パルス補間時)における移送指令導出部の演算負荷を軽減するために、その方向特定関数C(s)を導出した区間の先端上空対応調整関数S(s)を、その方向特定関数C(s)と射影先端パスと射影上空パスとに基づいて導出し、この導出した関数S(s)をメモリに登録してもよい。
具体的には、図5に示したフローチャートにおいて、姿勢調整情報導出部が、ステップS48の方向特定関数C(s)の導出及び当該関数C(s)のメモリへの登録の後、その方向特定関数C(s)を算出した区間について先端上空対応調整関数S(s)を導出し、その導出した関数S(s)をメモリ5に当該区間の姿勢調整情報として追加登録するステップを追加してもよい。具体的には、姿勢調整情報導出部は、区間内の媒介変数sが異なる複数の点について上記ステップS48で算出した方向特定関数C(s)から制御ラインの角度をそれぞれ求めるとともに、その媒介変数sが異なる各点に対応する射影先端パス上の点を通り、且つ、前記基準方向に対して求めた制御ラインの角度を成す直線が射影上空パスと交差する交点の媒介変数Sをそれぞれ算出する。そして、姿勢調整情報導出部は、その算出した各媒介変数Sと対応する各媒介変数sとを対応付ける関数を先端上空対応調整関数S(s)として導出し、その関数S(s)を対応する区間の情報と関連付けてメモリ5に登録する。このように姿勢調整情報導出部が先端上空対応調整関数S(s)を導出する場合には、媒介変数時間関数導出部では、必ず先端上空対応調整関数S(s)に基づいて媒介変数時間関数の導出を行い、移送指令導出部では、必ず先端上空対応調整関数S(s)に基づいて指令パルスの導出を行う。