JP5426728B2 - 多軸加工機用工具姿勢制御機能を有する数値制御装置 - Google Patents

Description

本発明は、少なくとも直線３軸と工具位相制御用回転軸を１軸含む少なくとも回転３軸を制御する多軸加工機用工具姿勢制御機能を有する数値制御装置に関する。

ワークに対して少なくとも直線３軸および工具位相制御用回転軸１軸を含む少なくとも回転軸３軸を有する多軸加工機において、下記［１］〜［３］に記載されるように、工具先端点位置、工具方向、工具位相方向を変化させながら行う加工がある。

［１］ファイバプレースメント機
図１のように、航空機の胴体、翼面、ロケット胴体などの滑らかな面を有するワークに、ローラから出るファイバ（炭素繊維複合材料）を巻きつける加工機である。工具位置（アーム位置）はＸ，Ｙ，Ｚ軸で動作するアームで制御され、工具方向はＢ，Ｃ軸の回転軸で制御され、工具位相方向（ローラ方向）は他の回転軸Ａ軸で制御される。図１では図の右側にファイバプレースメント機の本体が存在するが図示を省略している。また、工具に対してワークは相対的に小さく描いている。図２のように、工具先端点位置はワーク上を滑らかに移動するとともに、工具方向および工具位相方向も滑らかに変化させながらファイバを巻きつける。図２では、工具先端点位置の変化経路を工具先端点経路、工具方向および工具位相方向を示すベクトル先端の変化経路をそれぞれ工具方向経路および工具位相方向経路としている。工具方向および工具位相方向を示すベクトルは単位ベクトルとする。以降も同様である。
［２］ウォータジェット機
図３のように、ノズルから高圧の切断水を噴射することによってワークを切断する加工機である。切断水を受けるキャッチャがアームで支えられている。工具位置（工具ヘッド位置）はＸ，Ｙ，Ｚ軸で制御され、工具方向（ノズル方向）はＢ，Ｃ軸の回転軸で制御され、工具位相方向（アーム方向）は他の回転軸Ａ軸で制御される。図４のように、工具先端点位置はワーク上を滑らかに移動するとともに、工具方向（ノズル方向）と工具位相方向（アーム方向）も滑らかに変化させながらワークを切断する。
［３］ヘール加工
図５のように、ヘール工具（非旋回工具）の工具方向がワーク加工面にほぼ垂直であり工具すくい面が工具先端点経路とほぼ垂直となるように工具姿勢を制御して加工する。工具位置（工具ヘッド位置）はＸ，Ｙ，Ｚ軸で制御され、工具方向はＢ，Ｃ軸で制御され、工具位相方向は他の回転軸Ａ軸で制御される。図６のように、工具先端点位置はワーク上を滑らかに移動するとともに、工具方向と工具位相方向も滑らかに変化させながらワークを加工する。

なお、ここでは工具方向はＢ，Ｃ軸で制御され、工具位相方向はＡ軸で制御される例で説明したが、Ａ，Ｃ軸やＡ，Ｂ軸で工具方向を、他の回転軸で工具位相方向を制御する例もある。また、ここでは工具方向や工具位相方向を制御する回転軸は工具側にある工具ヘッド回転型多軸加工機の例で説明したが、回転軸はテーブル側にありテーブルを回転することによってテーブルと工具との相対的な回転を行うテーブル回転型多軸加工機（図２１：Ｂ，Ｃ軸で工具方向を制御し、Ａ軸で工具位相方向を制御する）や、回転軸は工具側とテーブル側の両方にあってテーブルと工具との相対的な回転を行う混合型多軸加工機（図２２：Ａ，Ｃ軸で工具方向を制御し、Ｂ軸で工具位相方向を制御する）もある。

また、特許文献１には、第３回転軸のローラ方向（工具位相）の制御も含めた工具先端点制御指令に対して座標変換（傾斜面加工指令）を行うことが可能な工具位相制御用数値制御装置が開示されている。
特許文献２には、加工物に対して工具が傾斜して加工するような４軸又は５軸加工機においても、所期の曲線に近い滑らかな曲線で補間する数値制御装置が開示されている。
特許文献３には、撮像手段により撮像された画像上の、対象物のブレを含む姿勢情報の時系列からブレを取り除いた姿勢情報の時系列を得る姿勢平滑化方法が開示されている。 特許文献４には、工具が進行方向に対して常に接線方向を向くように制御することができる工具軸の接線方向制御方式が開示されている。

特許第４５０３６５９号公報 特許第４１９９１０３号公報 特開２００７−３２２３９２号公報 特開平５−３２４０４７号公報

背景技術で説明した特許文献１は、「発明の効果」にも記載されているように、「第３回転軸のローラ方向（工具位相）の制御も含めた工具先端点制御指令に対して座標変換（傾斜面加工指令）を行うことが可能な工具位相制御用数値制御装置を提供」するものである。しかし、特許文献１は工具位相を制御するのみであり、工具先端点位置および工具姿勢（工具方向と工具位相方向）を滑らかに補間する技術思想はない。

特許文献２は、「加工点が滑らかに補間されるとともに工具と加工物の相対関係も滑らかに変化する。」と「発明の効果」に記載されている。しかし、「前記指令ベクトルのベクトル先端点を前記ベクトル先端点用近似曲線上の一番近い位置に向かって設定トレランス量以内で移動し、前記指令ベクトルに対応する修正ベクトル先端点を求める手段」と請求項１にあるように、工具方向を示す指令ベクトルを修正して滑らかな曲線とする技術は開示されているが（特許文献２の図１５参照）、工具位相も含めた工具先端点位置および工具姿勢（工具方向と工具位相方向）を滑らかに補間する技術思想はない。そのため工具位相方向の制御を必要とする工作機械には適用できない。

特許文献３は、請求項１に「撮像手段により撮像された画像上の、対象物のブレを含む姿勢情報の時系列からブレを取り除いた姿勢情報の時系列を得る姿勢平滑化方法」とあるように、画像処理における技術であり、工作機械を制御する技術思想ではない。したがって、工具先端点位置および工具姿勢（工具方向と工具位相方向）を滑らかに補間する技術思想、および工具先端点位置および工具姿勢にもとづいて直線３軸および回転軸３軸を制御する技術思想もない。

特許文献４は、「産業上の利用分野」に「本発明はＣＮＣ工作機械の工具の方向を制御する工具軸の接線方向制御方式に関し、特に工具が進行方向に対して常に接線方向を向くように制御する工具軸の接線方向制御方式に関する。」とあるように、工具位相を制御する（工具が進行方向に対して常に接線方向を向くように制御する）が、工具先端点位置および工具姿勢（工具方向と工具位相方向）を滑らかに補間する技術思想はない。

そこで本発明は、工具先端点位置および工具姿勢（工具方向と工具位相方向）を滑らかに補間し、直線３軸と回転３軸を制御する。つまり、工具先端点位置、工具方向（工具方向ベクトル）、工具位相方向（工具位相方向ベクトル）を変化させながら行う加工において、例えば図７のように工具先端点経路、工具方向経路（工具方向ベクトル先端の経路）、工具位相方向経路（工具位相方向ベクトル先端の経路）を滑らかに変化させながら加工することが可能な多軸加工機用工具姿勢制御機能を有する数値制御装置を提供することが課題である。工具方向と工具位相方向は垂直とする。

ここで、図７の工具方向経路および工具位相方向経路から工具先端点経路分を除いて工具方向（工具方向ベクトル）および工具位相方向（工具位相方向ベクトル）のみの変化として工具方向経路および工具位相方向経路を描くと図８のように半径１の単位球上の経路として表される。図８では便宜上図７に比べてベクトル本数は少なく描いている。

本願の請求項１に係る発明は、
ワークに対して少なくとも直線３軸および工具位相制御用回転軸１軸を含む少なくとも回転軸３軸を有する多軸加工機を制御する多軸加工機用工具姿勢制御機能を有する数値制御装置であって、工具先端点位置指令、工具方向ベクトル指令および工具位相方向ベクトル指令を読取る工具位置姿勢指令読取り部と、前記工具先端点位置指令を滑らかに補間し補間工具先端点位置Ｐｔ、前記工具方向ベクトル指令を滑らかに補間しＶｔｒ、前記工具位相方向ベクトル指令を滑らかに補間しＶｐｒを得、前記Ｖｔｒを正規化することにより補間工具方向ベクトルＶｔを得、前記Ｖｐｒおよび前記補間工具方向ベクトルＶｔから、後述する数２式により前記補間工具方向ベクトルＶｔと前記Ｖｐｒが成す平面上で前記補間工具方向ベクトルＶｔに垂直な正規化された補間工具位相方向ベクトルＶｐを得、前記補間工具方向ベクトルＶｔおよび前記補間工具位相方向ベクトルＶｐを補間工具姿勢とする工具位置姿勢補間部と、前記補間工具先端点位置および前記補間工具姿勢にもとづいて前記少なくとも直線３軸および前記少なくとも回転軸３軸の各軸位置を演算する各軸位置演算部を有し、前記少なくとも直線３軸および前記少なくとも回転軸３軸を前記各軸位置へ駆動することを特徴とする多軸加工機用工具姿勢制御機能を有する数値制御装置である。
請求項２に係る発明は、前記工具位置姿勢指令読取り部において、前記工具方向ベクトル指令を（ｉｎ，ｊｎ，ｋｎ）、前記工具位相方向ベクトル指令を（ｕｎ，ｖｎ，ｗｎ）、工具方向を制御する回転軸２軸に対する回転軸位置指令を（ｂｎ，ｃｎ）、工具位相方向を制御する回転軸１軸に対する回転軸位置指令を（ａｎ）、それらの回転軸位置が０の時の初期工具方向を示す単位ベクトルをＶｔ０および初期工具位相方向を示す単位ベクトルをＶｐ０としたとき、前記工具方向ベクトル指令は前記（ｂｎ，ｃｎ）から、後述する数８式により前記（ｉｎ，ｊｎ，ｋｎ）を求め、前記工具位相方向ベクトル指令は前記（ｂｎ，ｃｎ）および前記（ａｎ）から、後述する数９式により前記（ｕｎ，ｖｎ，ｗｎ）を求めることを特徴とする請求項１に記載の多軸加工機用工具姿勢制御機能を有する数値制御装置である。

請求項３に係る発明は、ワークに対して少なくとも直線３軸および工具位相制御用回転軸１軸を含む少なくとも回転軸３軸を有する多軸加工機を制御する多軸加工機用工具姿勢制御機能を有する数値制御装置であって、工具先端点位置指令、工具方向ベクトル指令、および工具先端点進行方向からの工具位相方向の偏位角である工具位相偏位角指令θを読取る工具位置姿勢指令読取り部と、前記工具先端点位置指令を滑らかに補間し補間工具先端点位置Ｐｔ、前記補間工具先端点位置Ｐｔの導関数Ｐｔ’、前記工具方向ベクトル指令を滑らかに補間しＶｔｒを得、前記Ｖｔｒを正規化して補間工具方向ベクトルＶｔを得、前記補間工具方向ベクトルＶｔに垂直でかつ前記補間工具方向ベクトルＶｔと前記導関数Ｐｔ’の成す平面に対して前記工具位相偏位角指令θを成す補間工具位相方向ベクトルＶｐを得、前記補間工具方向ベクトルＶｔおよび前記補間工具位相方向ベクトルＶｐを補間工具姿勢とする工具位置姿勢補間部と、前記補間工具先端点位置および前記補間工具姿勢にもとづいて前記少なくとも直線３軸および前記少なくとも回転軸３軸の各軸位置を演算する各軸位置演算部を有し、前記少なくとも直線３軸および前記少なくとも回転軸３軸を前記各軸位置へ駆動することを特徴とする多軸加工機用工具姿勢制御機能を有する数値制御装置である。
請求項４に係る発明は、ワークに対して少なくとも直線３軸および工具位相制御用回転軸１軸を含む少なくとも回転軸３軸を有する多軸加工機を制御する多軸加工機用工具姿勢制御機能を有する数値制御装置であって、工具先端点位置指令、および、初期工具方向および初期工具位相方向に対する工具方向および工具位相方向を指令する四元数による四元数指令である工具姿勢指令を含む加工プログラムを読取る工具位置姿勢指令読取り部と、前記工具先端点位置指令および前記工具姿勢指令にもとづいて工具先端点位置および工具姿勢を滑らかに補間し補間工具先端点位置および補間工具方向と補間工具位相方向から成る補間工具姿勢を求める工具位置姿勢補間部と、前記補間工具先端点位置および前記補間工具姿勢にもとづいて前記少なくとも直線３軸および前記少なくとも回転軸３軸の各軸位置を演算する各軸位置演算部を有し、前記少なくとも直線３軸および前記少なくとも回転軸３軸を前記各軸位置へ駆動することを特徴とする多軸加工機用工具姿勢制御機能を有する数値制御装置である。

請求項５に係る発明は、前記工具位置姿勢補間部は、工具先端点位置を滑らかに補間し前記補間工具先端点位置を求めるとともに、前記四元数指令にもとづいて四元数を滑らかに補間し補間四元数を求め前記補間四元数を前記補間工具姿勢とする前記工具位置姿勢補間部であることを特徴とする請求項４に記載の多軸加工機用工具姿勢制御機能を有する数値制御装置である。
請求項６に係る発明は、ワークに対して少なくとも直線３軸および工具位相制御用回転軸１軸を含む少なくとも回転軸３軸を有する多軸加工機を制御する多軸加工機用工具姿勢制御機能を有する数値制御装置であって、工具先端点位置指令および工具方向と工具位相方向から成る工具姿勢に対する工具姿勢指令を含む加工プログラムを読取る工具位置姿勢指令読取り部と、前記工具先端点位置指令にもとづいて工具先端点位置を滑らかに補間し補間工具先端点位置を求めるとともに、前記工具姿勢指令から対応する対応四元数指令を求め前記対応四元数指令にもとづいて四元数を滑らかに補間し補間四元数を求め前記補間四元数を補間工具姿勢とする工具位置姿勢補間部と、前記補間工具先端点位置および前記補間工具姿勢にもとづいて前記少なくとも直線３軸および前記少なくとも回転軸３軸の各軸位置を演算する各軸位置演算部を有し、前記少なくとも直線３軸および前記少なくとも回転軸３軸を前記各軸位置へ駆動することを特徴とする多軸加工機用工具姿勢制御機能を有する数値制御装置である。

本発明により、工具先端点位置、工具方向（工具方向ベクトル）、工具位相方向（工具位相方向ベクトル）を変化させながら行う加工において、工具先端点経路、工具方向経路（工具方向ベクトル先端の経路）、工具位相方向経路（工具位相方向ベクトル先端の経路）を滑らかに変化させながら加工することが可能な多軸加工機用工具姿勢制御機能を有する数値制御装置を提供できる。

ファイバプレースメント機を説明する図である。 工具先端点経路と工具方向経路と工具位相方向経路とを説明する図である。 ウォータジェット機を説明する図である。 ウォータジェット機において工具先端点位置がワーク上を滑らかに移動するとともに、工具方向（ノズル方向）と工具位相方向（アーム方向）も滑らかに変化させながらワークを切断することを説明する図である。 へール加工を説明する図である。 へール加工において工具先端点位置がワーク上を滑らかに移動するとともに、工具方向と工具位相方向も滑らかに変化させながらワークを加工することを説明する図である。 工具先端点経路、工具方向経路（工具方向ベクトル先端の経路）、工具位相方向経路（工具位相方向ベクトル先端の経路）を滑らかに変化させながらワークを加工する本発明の課題を説明する図である。 工具方向経路と工具位相方向経路とを工具先端点経路分を除いて工具方向（工具方向ベクトル）と工具位相方向（工具位相方向ベクトル）のみの変化として説明する図である。 工具姿勢指令が工具方向ベクトルと工具位相方向ベクトルで指令される加工プログラム例である。 第１実施形態において補間工具位相方向ベクトルＶｐを求めることを説明する図である。 工具姿勢指令が回転軸２軸と回転軸１軸に対する回転軸位置指令で指令される加工プログラム例である。 工具姿勢指令が工具方向ベクトル指令と工具位相偏位角指令で指令される加工プログラム例である。 第３実施形態において補間工具位相方向ベクトルＶｐを求めることを説明する図である。 工具姿勢指令が四元数指令で指令される加工プログラム例である。 四元数を単位四元数球面で説明する図である。 本発明に係る数値制御装置を説明するブロック図である。 第１実施形態の工具位置姿勢補間部および各軸位置演算部における処理を説明するフローチャートである。 第３実施形態の工具位置姿勢補間部における処理を説明するフローチャートである。 第４実施形態の工具位置姿勢補間部および各軸位置演算部における処理を説明するフローチャートである。 第５実施形態の工具位置姿勢補間部における処理を説明するフローチャートである。 テーブル回転型多軸加工機を説明する図である。 混合型多軸加工機を説明する図である。

本発明は多様な多軸加工機にも適用可能である。以下、本発明の実施形態を図面と共に説明する。
＜第１実施形態＞
図１、図３、図５のように工具ヘッドをＸ，Ｙ，Ｚ軸で制御し、工具方向をＢ，Ｃ軸で制御し、工具位相方向をＡ軸で制御する機械構成とする。加工プログラムは図９に示される加工プログラム例１のように工具先端点位置をアドレスＸ，Ｙ，Ｚによる工具先端点位置指令（ｘｎ，ｙｎ，ｚｎ）で指令し工具方向をアドレスＩ，Ｊ，Ｋによる工具方向ベクトル指令（ｉｎ，ｊｎ，ｋｎ）で指令し工具位相方向をアドレスＵ，Ｖ，Ｗによる工具位相方向ベクトル指令（ｕｎ，ｖｎ，ｗｎ）（ｎ＝１，２，，，，）で指令する。工具方向ベクトル指令および工具位相方向ベクトル指令が工具姿勢指令である。Ｇ４３．４は工具先端点位置を指令する工具先端点制御モード開始のＧコードである。Ｇ４９はその終了のＧコードである。Ｆによって指令速度、Ｈによって工具長補正番号を指令する。（ｉｎ，ｊｎ，ｋｎ）、（ｕｎ，ｖｎ，ｗｎ）は単位ベクトルとする。工具先端点位置指令（ｘｎ，ｙｎ，ｚｎ）、工具方向ベクトル指令（ｉｎ，ｊｎ，ｋｎ）および工具位相方向ベクトル指令（ｕｎ，ｖｎ，ｗｎ）を読取るのが工具位置姿勢指令読取り部（図１６参照）である。

なお、工具位相方向の指令について、ここでは工具位相方向をアドレスＵ，Ｖ，Ｗによる工具位相方向ベクトル指令（ｕｎ，ｖｎ，ｗｎ）で指令する例としているが、後述の第３実施形態におけるように工具先端点進行方向からの工具位相方向の偏位角を指令する工具位相偏位角指令としてもよい。

アドレスＸ、Ｙ、Ｚの指令点列（ｘ１，ｙ１，ｚ１）、（ｘ２，ｙ２，ｚ２）、、、にもとづいて滑らかに補間し工具先端点経路を示す補間工具先端点位置Ｐｔ（Ｐｔｘ，Ｐｔｙ，Ｐｔｚ）^Tを得る。滑らかに補間するとは指令点列（ｘ１，ｙ１，ｚ１）、（ｘ２，ｙ２，ｚ２）、、、からそれらを通過するスプライン曲線、ＮＵＲＢＳ曲線などの滑らかな曲線を作成し補間周期毎に指令速度に応じてその滑らかな曲線上の位置を補間することである。つまり、第ｎブロックから第（ｎ＋１）ブロックへの指令点列を通過する滑らかな曲線を表す関数をパラメータｔによるＰｔ_n（ｔ）（０≦ｔ≦１）とすると、指令速度に応じて補間時刻τにおけるパラメータ値ｓを求めＰｔ＝Ｐｔ_n（ｓ）を求めることである（図１０参照）。

滑らかな曲線は指令点列を通過しなくても許容値の範囲内でそれらの近くを通過する曲線でもよい。このように指令点列にもとづいて滑らかに補間する技術は特許文献２などにも記載されている従来技術なので詳述しない。「^T」は転置である。ただし、以降自明の場合、記載を省略する。

同様に、アドレスＩ，Ｊ，Ｋの工具方向ベクトル指令列（ｉ１，ｊ１，ｋ１）、（ｉ２，ｊ２，ｋ２）、、、にもとづいて滑らかに補間しＶｔｒ（Ｖｔｒｘ，Ｖｔｒｙ，Ｖｔｒｚ）を得る。滑らかに補間するとは図８のように工具方向ベクトル指令列（ｉ１，ｊ１，ｋ１）、（ｉ２，ｊ２，ｋ２）、、、の先端を通過するスプライン曲線、ＮＵＲＢＳ曲線などの滑らかな曲線を作成しＰｔに対応してその滑らかな曲線上の位置を補間することである。つまり、第ｎブロックから第（ｎ＋１）ブロックへの工具方向ベクトル指令列先端を通過する滑らかな曲線を表す関数をパラメータｔによるベクトルＶｔｒ_n（ｔ）（０≦ｔ≦１）とすると，Ｖｔｒ＝Ｖｔｒ_n（ｓ）を求めることである。滑らかな曲線は工具方向ベクトル指令列の先端を通過しなくても許容値の範囲内でそれらの近くを通過する曲線でもよい。Ｐｔと同様、後述のＶｐも含めベクトル指令列にもとづいて滑らかに補間する技術は例えば特許文献２にも記載されている従来技術であるので詳述しない。Ｖｔｒを数１式のように正規化して工具方向経路（図８参照）を示す補間工具方向ベクトルＶｔ（Ｖｔｘ，Ｖｔｙ，Ｖｔｚ）を得る。

Ｖｔｒと同様に、アドレスＵ，Ｖ，Ｗの工具位相方向ベクトル指令列（ｕ１，ｖ１，ｗ１）、（ｕ２，ｖ２，ｗ２）、、、にもとづいて滑らかに補間しＶｐｒ（Ｖｐｒｘ，Ｖｐｒｙ，Ｖｐｒｚ）を得る。数２式の計算を行い、補間工具位相方向ベクトルＶｐを得る。「×」は外積である。これにより、工具位相方向経路（図８参照）を示す、ＶｔとＶｐｒが成す平面上でＶｔに垂直な正規化されたベクトルＶｐ（Ｖｐｘ，Ｖｐｙ，Ｖｐｚ）を得る。（図１０参照）Ｖｔ，Ｖｐが補間工具姿勢である。これらの補間を行うのが工具位置姿勢補間部（図１６参照）である。

補間工具方向ベクトルＶｔを実現するＢ，Ｃ軸位置のＢ，Ｃを数３式を解くことにより求める。Ｒｂ，ＲｃはＢ，Ｃ軸がＢ，Ｃの位置である時の回転マトリックスである。Ｖｔ０はＢ＝Ｃ＝０の時の初期工具方向を示す単位ベクトルである。

Ｖｔ０＝（１，０，０）であれば数３式は数４式のようになる。

補間工具位相方向ベクトルＶｐを実現するＡ軸位置のＡを、数５式を解くことにより求める。Ｂ，Ｃはすでに求まっているのでＡを求めることができる。ＲａはＡ軸がＡの位置である時の回転マトリックスである。Ｖｐ０はＡ＝Ｂ＝Ｃ＝０の時の初期工具位相方向示す単位ベクトルである。

Ｖｐ０＝（０，１，０）であれば数５式は数６式のようになる。

Ｘ，Ｙ，Ｚ軸が駆動する制御点位置Ｐｃ（Ｐｃｘ，Ｐｃｙ，Ｐｃｚ）については、ここではＢ軸Ｃ軸の回転中心線の交点とする。Ｐｃは数７式によって求める。Ｔｈは工具長補正番号によって選択されている工具長補正量（スカラー量）である。
これらの各軸の位置Ｐｃ（Ｐｃｘ，Ｐｃｙ，Ｐｃｚ）、Ａ、Ｂ、Ｃを求めるのが各軸位置演算部（図１６参照）である。

そして、Ｘ、Ｙ、Ｚ軸をＰｃの位置に、Ｂ、Ｃ軸をＢ，Ｃの位置に、Ａ軸をＡの位置に駆動することにより、図７、図８で示したように工具先端点経路、工具方向経路、工具位相方向経路が滑らかな曲線となるように制御することができる。

＜第２実施形態＞
第１実施形態では工具姿勢指令について加工プログラム例１（図９参照）のように工具方向ベクトル、工具位相方向ベクトルで指令するとしたが、図１１に示される加工プログラム例２のように工具方向の指令として回転軸３軸のうち工具方向を制御する回転軸２軸であるＢ，Ｃ軸に対する回転軸位置指令（ｂｎ，ｃｎ）（ｎ＝１，２，，，，）と、工具位相方向の指令として工具位相方向を制御する回転軸１軸であるＡ軸に対する回転軸位置指令（ａｎ）を指令してもよい。これらの回転軸２軸と回転軸１軸に対する回転軸位置指令が工具姿勢指令である。その他の指令は加工プログラム例１と同じである。

なお、工具位相方向の指令について、ここでは工具位相方向を制御する回転軸１軸であるＡ軸に対する回転軸位置指令（ａｎ）とする例としているが、後述の第３実施形態におけるように工具先端点進行方向からの工具位相方向の偏位角を指令する工具位相偏位角指令としてもよい。

工具方向の指令として回転軸３軸のうち工具方向を制御する回転軸２軸であるＢ，Ｃ軸に対する回転軸位置指令（ｂｎ，ｃｎ）と、工具位相方向の指令として工具位相方向を制御する回転軸１軸であるＡ軸に対する回転軸位置指令（ａｎ）を指令する場合、数８式、数９式のようにａｎ，ｂｎ，ｃｎをｉｎ，ｊｎ，ｋｎ，ｕｎ，ｖｎ，ｗｎ（ｎ＝１，２，，，，）と置き換えて第１実施形態における加工プログラム例１の場合と同様に計算すればよい。以降の説明は省略する。

＜第３実施形態＞
第１実施形態では工具位相方向の指令について、加工プログラム例１のように工具位相方向ベクトルで指令するとしたが、第３実施形態では図１２に示される加工プログラム例３のように工具先端点進行方向からの工具位相方向の偏位角（θｎ）（ｎ＝１，２，，，，）を指令するアドレスＷによる工具位相偏位角指令とする。第１実施形態と同じ工具方向ベクトル指令（ｉｎ，ｊｎ，ｋｎ）とこの工具位相偏位角指令が工具姿勢指令である。その他の指令は加工プログラム例１と同じである。工具先端点進行方向からの工具位相方向の偏位角（θｎ）は通常は各ブロックでは指令せず第１ブロックの指令（θ１）のみである。そのため、後述の説明ではｎを付けないθ（＝θ１）で説明している。もちろん、各ブロックで指令してθをブロック毎に変更してもよい。その場合は後述の説明ではθはθｎとなる。また、θは０度、±９０度、±１８０度または±２７０度などの固定値であることも多いので、一定角（定数）として予め設定されている値を使用してもよい。その場合、Ｗθｎを指令しない。Ｗθｎを指令しないのも予め設定されている値を使用するという１種の指令であり、Ｗθｎを指令しなければ、予め設定されている一定角（定数）を偏位角（θ）として使用する。
Ｐｔ、Ｖｔ（Ｖｔｘ，Ｖｔｙ，Ｖｔｚ）は第１実施形態と同様に求める。第１実施形態におけるＰｔ_n（ｔ）に対して、その導関数をＰｔ_n（ｔ）’とする。Ｐｔ_n（ｔ）’も滑らかな曲線を成す関数であり、Ｐｔ’＝Ｐｔ_n（ｓ）’とする。ｓは第１実施形態で説明したｓである。補間時刻τにおけるＶｐを数１０式で求める。ＶｖはＰｔ’とＶｔの成す平面においてＶｔに垂直な単位ベクトルである。ＲθはＶｔ周りに角度θだけ回転する回転マトリックスである。数１０式でＶｔ周りにＶｖを角度θだけ回転したベクトルとしてＶｐを求める。（図１３参照）このように、補間工具方向ベクトルＶｔに垂直でかつ補間工具方向ベクトルＶｔと補間工具先端点位置の変化方向Ｐｔ’の成す平面に対して偏位角θを成す補間工具位相方向ベクトルＶｐを求める。

ここで、ＶｖとＲθは数１１式で与えられる。

Ｖｐを求めた後は第１実施形態と同様なので説明を省略する。

＜第４実施形態＞
第１実施形態の加工プログラム例１（図９参照）では、工具姿勢指令として工具方向をアドレスＩ，Ｊ，Ｋによる工具方向ベクトル指令（ｉｎ，ｊｎ，ｋｎ）で指令するとともに工具位相方向をアドレスＵ，Ｖ，Ｗによる工具位相方向ベクトル指令（ｕｎ，ｖｎ，ｗｎ）（ｎ＝１，２，，，，）で指令したが、本実施形態では加工プログラム例４（図１４参照）のように、工具姿勢指令として四元数指令とする。アドレスＲで指令されるｒｎが四元数指令の実数成分、アドレスＵ，Ｖ，Ｗで指令されるｕｎ，ｖｎ，ｗｎ（ｎ＝１，２，，，，）が四元数指令の虚数成分であり、それらを工具位置姿勢指令読取り部（図１６参照）で読取る。四元数とはある回転中心軸周りのある角度分の回転に対応する数であり、そのため初期工具方向ベクトルＶｔ０および初期工具位相方向ベクトルＶｐ０をある回転中心軸周りにある角度分回転した工具方向および工具位相方向とする四元数を指令することができる。Ｖｔ０およびＶｐ０は回転軸Ａ，Ｂ，Ｃ軸の位置がＡ＝Ｂ＝Ｃ＝０の時の初期工具方向および初期工具位相方向である。その他の指令は加工プログラム例１と同じである。四元数はクォータニオンとも呼ばれる。

ｒｎ，ｕｎ，ｖｎ，ｗｎで指令される四元数指令ｑ_nは数１２式のように表される。ｑ_nは単位四元数とする。つまり、ｒｎ²＋ｕｎ²＋ｖｎ²＋ｗｎ²＝１とする。ここで、ｉ、ｊ、ｋは虚数単位であり、ｉ²＝ｊ²＝ｋ²＝ｉ＊ｊ＊ｋ＝−１，ｉ＊ｊ＝−ｊ＊ｉ＝ｋ，ｊ＊ｋ＝−ｋ＊ｊ＝ｉ，ｋ＊ｉ＝−ｉ＊ｋ＝ｊである。

アドレスＸ、Ｙ、Ｚの指令点列（ｘ１，ｙ１，ｚ１）、（ｘ２，ｙ２，ｚ２）、、、にもとづいて滑らかに補間しＰｔ（Ｐｔｘ，Ｐｔｙ，Ｐｔｚ）を得るのは、第１実施形態と同じである。四元数指令ｑ_nに対し、ｑ_n〜ｑ_n+1において四元数ｑ_n（ｔ）を式１３式のように表し、補間時刻τにおける補間四元数ｑ＝ｑ_n（ｓ）を求める。ｓは第１実施形態で説明したｓである。ｓｑｕａｄは球面３次補間と呼ばれ、四元数を滑らかに補間する方法の１つである。このように四元数を滑らかに補間する技術は特許文献３などにも記載されている従来技術であるので詳述しない。単位四元数球面上で図示すると、図１５のようになる。

ここで、ａ_n，ｂ_n+1は数１４式のように与えられる。

ｓｌｅｒｐは球面線形補間と呼ばれ、単位四元数αとβに対して数１５式のように与えられる。「●」は内積であり、θはαとβの成す角である。（このθは第３実施形態の偏位角θとは相違する。）

このように、四元数指令にもとづいて四元数を滑らかに補間した補間四元数ｑについて、ｑｒを実数部分、ｑｕ，ｑｖ，ｑｗを虚数部分として数１６式のように表す。

上述のように、四元数はある回転中心軸周りのある角度分の回転に対応する数であるため、補間四元数は初期工具方向と初期工具位相方向をその補間四元数によって回転した補間工具方向と補間工具位相方向を表す。したがって、本実施形態では補間四元数を補間工具方向と補間工具位相方向から成る補間工具姿勢とする。この補間は工具位置姿勢補間部で行う。この補間四元数ｑは回転マトリックスＭと数１７式のように対応する。

また、Ａ，Ｂ，Ｃ軸がＡ，Ｂ，Ｃの位置になることによる回転マトリックスＲは数１８式のようになる。

Ｒ＝Ｍを解くことにより求めるＡ，Ｂ，Ｃが、ｑに対応したＡ，Ｂ，Ｃ軸を駆動する位置である。さらに、Ａ，Ｂ，Ｃに対して第１実施形態の数３式〜数７式を適用しＰｃ（Ｐｃｘ，Ｐｃｙ，Ｐｃｚ）を求め、ＰｃをＸ，Ｙ，Ｚ軸が駆動する制御点位置とする。これらの各軸の位置Ｐｃ（Ｐｃｘ，Ｐｃｙ，Ｐｃｚ）、Ａ、Ｂ、Ｃを求めるのが各軸位置演算部（図１６参照）である。

＜第５実施形態＞
第１実施形態の加工プログラム例１（図９参照）のように、工具姿勢指令が工具方向として工具方向ベクトル指令（ｉｎ，ｊｎ，ｋｎ）、工具位相方向として工具位相方向ベクトル指令（ｕｎ，ｖｎ，ｗｎ）で指令される場合も、それらに対応する対応四元数指令を求めて第４実施形態と同様の計算を行うことができる。数８式を満たすｂｎ、ｃｎ、Ｒｂｎ、Ｒｃｎを求め、さらに数９式を満たすａｎ、Ｒａｎを求める。なお、第２実施形態のように工具姿勢（工具方向、工具位相方向）指令が回転軸位置（ａｎ，ｂｎ，ｃｎ）で指令される場合、同様にそれらのａｎ，ｂｎ，ｃｎに対するＲａｎ，Ｒｂｎ，Ｒｃｎを求めればよい。それらのＲａｎ，Ｒｂｎ，Ｒｃｎに対するマトリックスＲｎを数１９式のように表す。ｒ１１〜ｒ３３はマトリックスＲｎの各要素を示す。

このとき、ａｎ，ｂｎ，ｃｎに対応する対応四元数指令ｑ_n（＝ｒｎ＋ｉ（ｕｎ）＋ｊ（ｖｎ）＋ｋ（ｗｎ））の各要素は次のように求められる。なお、ここで対応四元数指令の要素をｕｎ，ｖｎ，ｋｎとし工具位相方向ベクトル指令の（ｕｎ，ｖｎ，ｗｎ）と便宜上同じ名称にしているが、それらはそれぞれ別個の指令である。

対応四元数指令を求めた後は第４実施形態の数１２式以降の計算を行えばよい。あるいは、第２実施形態の加工プログラム例２（図１１参照）のように回転軸３軸に対する回転軸指令（ａｎ，ｂｎ，ｃｎ）が指令される場合、数１９式以降の計算を行えば対応四元数指令ｑ_nを求めることができる。後は第４実施形態の数１２式以降の計算を行えばよい。以降の説明は省略する。

次に、図１６に示される本発明に係る数値制御装置のブロック図を説明する。
一般に数値制御装置は、加工プログラム読取り部１で加工プログラムの指令を読取り、指令にもとづいて補間部２で補間を行って各軸サーボ（６Ｘ，６Ｙ，６Ｚ，６Ａ，６Ｂ，６Ｃ）を駆動する。本発明における工具位置姿勢指令読取り部３は加工プログラム読取り部１に属し、工具位置姿勢補間部４および各軸位置演算部５は補間部２に属する。

次に、各フローチャートを説明する。
図１７は第１実施形態の工具位置姿勢補間部および各軸位置演算部における処理を説明するフローチャートである。以下、各ステップに従って説明する。
●［ステップＳＡ０１］工具先端点位置指令および工具姿勢指令にもとづいて滑らかに補間し、Ｐｔ，Ｖｔｒ，Ｖｐｒを得る。
●［ステップＳＡ０２］数１式によりＶｔ、数２式によりＶｐを求める。
●［ステップＳＡ０３］数３式によりＢ，Ｃ、数５式によりＡ、数７式によりＰｃ（Ｐｃｘ，Ｐｃｙ，Ｐｃｚ）を求める。
なお、ＳＡ０１、ＳＡ０２は工具位置姿勢補間部４に、ＳＡ０３は各軸位置演算部５に属する。

図１８は第３実施形態の工具位置姿勢補間部における処理を説明するフローチャートである。以下、各ステップに従って説明する。
●［ステップＳＢ０１］工具先端点位置指令および工具姿勢指令にもとづいて滑らかに補間し、Ｐｔ，Ｖｔを求める。
●［ステップＳＢ０２］Ｐｔ_n（ｔ）’からＰｔ’＝Ｐｔ_n（ｓ）’を求め、数１０式、数１１式からＶｐを求め、ステップＳＡ０３（図１７参照）に移行する。
なお、ＳＢ０１、ＳＢ０２は工具位置姿勢補間部４に属する。

図１９は第４実施形態の工具位置姿勢補間部および各軸位置演算部における処理を説明するフローチャートである。以下、各ステップに従って説明する。
●［ステップＳＣ０１］数１２式によりｑ_nを求める。
●［ステップＳＣ０２］工具先端点位置指令にもとづいて滑らかに補間し、Ｐｔを得る。
●［ステップＳＣ０３］式１３式〜数１５式からｑ_n（ｔ）を求め、ｑ＝ｑ_n（ｓ）としてｑを求める。
●［ステップＳＣ０４］数１７式、数１８式とＲ＝ＭからＡ，Ｂ，Ｃを求める。数３式、数５式、数７式からＰｃ（Ｐｃｘ，Ｐｃｙ，Ｐｃｚ）を求める。
なお、ＳＣ０１、ＳＣ０２、ＳＣ０３は工具位置姿勢補間部４に，ＳＣ０４は各軸位置演算部５に属する。

図２０は第５実施形態の工具位置姿勢補間部における処理を説明するフローチャートである。以下、各ステップに従って説明する。
●［ステップＳＤ０１］工具姿勢指令は回転軸位置指令か否か判断し、回転軸位置指令ではない場合（ＮＯ）ステップＳＤ０２へ移行し、回転軸位置指令の場合（ＹＥＳ）ステップＳＤ０３へ移行する。
●［ステップＳＤ０２］工具方向ベクトル指令（ｉｎ，ｊｎ，ｋｎ）と数８式から、ｂｎ，ｃｎ，Ｒｂｎ，Ｒｃｎを求める。工具位相方向ベクトル指令（ｕｎ，ｖｎ，ｗｎ）と数９式からａｎ，Ｒａｎを求める。
●［ステップＳＤ０３］回転軸位置指令（ａｎ，ｂｎ，ｃｎ）からＲａｎ，Ｒｂｎ，Ｒｃｎを求める。
●［ステップＳＤ０４］Ｒａｎ，Ｒｂｎ，Ｒｃｎと数１９式、数２０式から、対応四元数指令ｑ_n（＝ｒｎ＋ｉ（ｕｎ）＋ｊ（ｖｎ）＋ｋ（ｗｎ））を求め、ステップＳＣ０２（図１９参照）に移行する。
なお、ＳＤ０１、ＳＤ０２、ＳＤ０３、ＳＤ０４は工具位置姿勢補間部４に属する。

上記したように、本発明により、工具先端点位置、工具方向（工具方向ベクトル）、工具位相方向（工具位相方向ベクトル）を変化させながら行う加工において、工具先端点経路、工具方向経路（工具方向ベクトル先端の経路）、工具位相方向経路（工具位相方向ベクトル先端の経路）を滑らかに変化させながら加工することが可能な多軸加工機用工具姿勢制御機能を有する数値制御装置を提供できる。このことによって、より滑らかな加工面を得ることができる。つまり、ファイバプレースメント機であればより滑らかにファイバを巻きつけることができ、ウォータジェット機であればより滑らかな切断面を得ることができ、ヘール加工であればより滑らかな加工面を得ることができる。

１ 加工プログラム読取り部
２ 補間部
３ 工具位置姿勢指令読取り部
４ 工具位置姿勢補間部
５ 各軸位置演算部
６Ｘ Ｘ軸サーボ
６Ｙ Ｙ軸サーボ
６Ｚ Ｚ軸サーボ
６Ａ Ａ軸サーボ
６Ｂ Ｂ軸サーボ
６Ｃ Ｃ軸サーボ

Claims (6)

1. ワークに対して少なくとも直線３軸および工具位相制御用回転軸１軸を含む少なくとも回転軸３軸を有する多軸加工機を制御する多軸加工機用工具姿勢制御機能を有する数値制御装置であって、
   工具先端点位置指令、工具方向ベクトル指令および工具位相方向ベクトル指令を読取る工具位置姿勢指令読取り部と、
   前記工具先端点位置指令を滑らかに補間し補間工具先端点位置Ｐｔ、前記工具方向ベクトル指令を滑らかに補間しＶｔｒ、前記工具位相方向ベクトル指令を滑らかに補間しＶｐｒを得、前記Ｖｔｒを正規化することにより補間工具方向ベクトルＶｔを得、前記Ｖｐｒおよび前記補間工具方向ベクトルＶｔから
   

   

   により前記補間工具方向ベクトルＶｔと前記Ｖｐｒが成す平面上で前記補間工具方向ベクトルＶｔに垂直な正規化された補間工具位相方向ベクトルＶｐを得、前記補間工具方向ベクトルＶｔおよび前記補間工具位相方向ベクトルＶｐを補間工具姿勢とする工具位置姿勢補間部と、
   前記補間工具先端点位置および前記補間工具姿勢にもとづいて前記少なくとも直線３軸および前記少なくとも回転軸３軸の各軸位置を演算する各軸位置演算部を有し、
   前記少なくとも直線３軸および前記少なくとも回転軸３軸を前記各軸位置へ駆動することを特徴とする多軸加工機用工具姿勢制御機能を有する数値制御装置。
2. 前記工具位置姿勢指令読取り部において、前記工具方向ベクトル指令を（ｉｎ，ｊｎ，ｋｎ）、前記工具位相方向ベクトル指令を（ｕｎ，ｖｎ，ｗｎ）、工具方向を制御する回転軸２軸に対する回転軸位置指令を（ｂｎ，ｃｎ）、工具位相方向を制御する回転軸１軸に対する回転軸位置指令を（ａｎ）、それらの回転軸位置が０の時の初期工具方向を示す単位ベクトルをＶｔ０および初期工具位相方向を示す単位ベクトルをＶｐ０としたとき、前記工具方向ベクトル指令は前記（ｂｎ，ｃｎ）から、
   

   

   により前記（ｉｎ，ｊｎ，ｋｎ）を求め、
   前記工具位相方向ベクトル指令は前記（ｂｎ，ｃｎ）および前記（ａｎ）から、
   

   

   により前記（ｕｎ，ｖｎ，ｗｎ）を求めることを特徴とする請求項１に記載の多軸加工機用工具姿勢制御機能を有する数値制御装置。
3. ワークに対して少なくとも直線３軸および工具位相制御用回転軸１軸を含む少なくとも回転軸３軸を有する多軸加工機を制御する多軸加工機用工具姿勢制御機能を有する数値制御装置であって、
   工具先端点位置指令、工具方向ベクトル指令、および工具先端点進行方向からの工具位相方向の偏位角である工具位相偏位角指令θを読取る工具位置姿勢指令読取り部と、
   前記工具先端点位置指令を滑らかに補間し補間工具先端点位置Ｐｔ、前記補間工具先端点位置Ｐｔの導関数Ｐｔ’、前記工具方向ベクトル指令を滑らかに補間しＶｔｒを得、前記Ｖｔｒを正規化して補間工具方向ベクトルＶｔを得、前記補間工具方向ベクトルＶｔに垂直でかつ前記補間工具方向ベクトルＶｔと前記導関数Ｐｔ’の成す平面に対して前記工具位相偏位角指令θを成す補間工具位相方向ベクトルＶｐを得、前記補間工具方向ベクトルＶｔおよび前記補間工具位相方向ベクトルＶｐを補間工具姿勢とする工具位置姿勢補間部と、
   前記補間工具先端点位置および前記補間工具姿勢にもとづいて前記少なくとも直線３軸および前記少なくとも回転軸３軸の各軸位置を演算する各軸位置演算部を有し、
   前記少なくとも直線３軸および前記少なくとも回転軸３軸を前記各軸位置へ駆動することを特徴とする多軸加工機用工具姿勢制御機能を有する数値制御装置。
4. ワークに対して少なくとも直線３軸および工具位相制御用回転軸１軸を含む少なくとも回転軸３軸を有する多軸加工機を制御する多軸加工機用工具姿勢制御機能を有する数値制御装置であって、
   工具先端点位置指令、および、初期工具方向および初期工具位相方向に対する工具方向および工具位相方向を指令する四元数による四元数指令である工具姿勢指令を含む加工プログラムを読取る工具位置姿勢指令読取り部と、
   前記工具先端点位置指令および前記工具姿勢指令にもとづいて工具先端点位置および工具姿勢を滑らかに補間し補間工具先端点位置および補間工具方向と補間工具位相方向から成る補間工具姿勢を求める工具位置姿勢補間部と、
   前記補間工具先端点位置および前記補間工具姿勢にもとづいて前記少なくとも直線３軸および前記少なくとも回転軸３軸の各軸位置を演算する各軸位置演算部を有し、
   前記少なくとも直線３軸および前記少なくとも回転軸３軸を前記各軸位置へ駆動することを特徴とする多軸加工機用工具姿勢制御機能を有する数値制御装置。
5. 前記工具位置姿勢補間部は、工具先端点位置を滑らかに補間し前記補間工具先端点位置を求めるとともに、前記四元数指令にもとづいて四元数を滑らかに補間し補間四元数を求め前記補間四元数を前記補間工具姿勢とする前記工具位置姿勢補間部であることを特徴とする請求項４に記載の多軸加工機用工具姿勢制御機能を有する数値制御装置。
6. ワークに対して少なくとも直線３軸および工具位相制御用回転軸１軸を含む少なくとも回転軸３軸を有する多軸加工機を制御する多軸加工機用工具姿勢制御機能を有する数値制御装置であって、
   工具先端点位置指令および工具方向と工具位相方向から成る工具姿勢に対する工具姿勢指令を含む加工プログラムを読取る工具位置姿勢指令読取り部と、
   前記工具先端点位置指令にもとづいて工具先端点位置を滑らかに補間し補間工具先端点位置を求めるとともに、前記工具姿勢指令から対応する対応四元数指令を求め前記対応四元数指令にもとづいて四元数を滑らかに補間し補間四元数を求め前記補間四元数を補間工具姿勢とする工具位置姿勢補間部と、
   前記補間工具先端点位置および前記補間工具姿勢にもとづいて前記少なくとも直線３軸および前記少なくとも回転軸３軸の各軸位置を演算する各軸位置演算部を有し、
   前記少なくとも直線３軸および前記少なくとも回転軸３軸を前記各軸位置へ駆動することを特徴とする多軸加工機用工具姿勢制御機能を有する数値制御装置。

Images