(57)【特許請求の範囲】
【請求項１】
数値制御工作機械の機構の駆動部をサーボモータにより駆動し、前記駆動部の位置を位置検出器によって検出し、上位装置からの位置指令値に従って前記駆動部の位置を制御する位置制御装置であって、
基準座標系で表現された切削工具の動作軌跡を示す位置指令値ベクトルから軌跡間段差ベクトルを検出するブロックと、
加工対象物の面上の法線ベクトルを検出するブロックと、
前記法線ベクトルの方向から定まる拘束座標系で示した力目標値を駆動軸座標系で示した力目標値に座標変換するブロックと、
を具備した力指令演算部を含む位置指令演算部を備え、
前記駆動軸座標系で示した力目標値と、前記駆動部の位置制御のための駆動力指令値と、を加算することによって前記駆動部の位置及び力制御のための駆動力指令値を構成することを特徴とした数値制御工作機械の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、数値制御工作機械に適用される位置及び力の制御装置に関するものである。
【背景技術】
【０００２】
図５は、数値制御機械の一つである工作機械における、従来の位置制御装置の構成例を示すブロック図である。位置制御装置１００はＮＣ装置の内部に含まれ、基準座標系(ｘ,ｙ,ｚ,φ,θ,ψ)で表現された切削工具の動作軌跡が、ある一定周期毎に、位置指令値ＰＤとして上位装置（図示しない）より入力される。尚、座標値(ｘ,ｙ,ｚ)は、３次元直交座標系で工具の空間上の位置を示し、座標値(φ,θ,ψ)は、工具の空間上の姿勢を示す座標値で、便宜上よく知られたＺ→Ｙ’→Ｚ”のオイラー角で表記されたものとする。
【０００３】
位置指令演算部１０２の加減速演算部１０４では、位置指令値ＰＤが補間加減速処理され、微小時間ΔＴ毎の切削工具の動作軌跡を示す基準座標系で見た位置指令ベクトルｒ＝[ｘ,ｙ,ｚ,φ,θ,ψ]Ｔ(以降、ベクトルや行列を示す記号は、太文字で表し、[]Ｔは転置を示す。)が出力される。次にベクトルｒは、駆動軸位置指令演算部１０６で、数式（１）により、各軸の位置指令ベクトルｑ＝[ｑ１,ｑ２,・・・,ｑｎ]Ｔに変換される。
【０００４】
ベクトルｑ＝ベクトルｑ(ベクトルｒ)  ・・・・・（１）
【０００５】
数式（１）は、工作機械の軸構成により定まる逆運動学問題の解であり、解となるベクトルｑは独立した同一構成を有する各駆動軸位置制御部１０８−１〜１０８−ｎに送られる。
【０００６】
次に、第１駆動軸を例に、軸位置制御部１０８−１の機能について説明する。サーボモータ５７に締結された位置検出器５８は、第１駆動軸の軸位置を検出する位置検出器である。その出力である位置検出値ｑｄ１は、減算器５０により、位置指令値ｑ１から減算される。減算器５０の出力は、位置偏差増幅器Ｇｐで増幅される。一方、位置指令値ｑ１は、微分器５３で微分されて、位置偏差増幅器Ｇｐの出力と加算器５１で加算され、第１駆動軸の速度指令値Ｖｃ１となる。
【０００７】
位置検出値ｑｄ１は、微分器５５で時間微分され、速度検出値ｖｄ１となる。減算器５２では、速度指令値Ｖｃ１から速度検出値ｖｄ１が減算される。減算器５２の出力は、速度偏差増幅器Ｇｖで通常、比例積分増幅される。一方、前記微分器５３の出力は、微分器５４で再度微分され、増幅器ＣｂでＣｂ倍される。Ｃｂは、位置指令値ｑ１の加速度成分に対する加減速駆動力を算出するための定数である。増幅器Ｃｂ出力は、速度偏差増幅器Ｇｖ出力と加算器５６で加算され、第１駆動軸の駆動力指令値τｃ１となる。
【０００８】
パワー増幅部は、駆動力指令値τｃ１を電力増幅し、サーボモータ５７に供給する。これにより、サーボモータ５７は、駆動力指令値τｃ１通りの駆動力を発生する。以上説明した軸位置制御部の動作により、位置検出値ｑｄ１は、位置指令値ｑ１に一致する様に制御され、各軸位置制御部が独立に同様な動作を行なうことで、結果的に、切削工具の位置・姿勢動作軌跡は、前記位置指令ベクトルｒに一致することになる。
【発明の開示】
【発明が解決しようとする課題】
【０００９】
以上のように、従来の位置制御装置１００では、切削工具の位置及び姿勢を目標の動作軌跡上に制御することで、加工対象物(ワーク)を所望の形状に切削加工する。しかしながら、位置及び姿勢のみを制御しているため、加工対象物に対して力の制御を加えることができなかった。
【００１０】
本発明は、上記従来技術の問題点を鑑み、加工対象物の形状に応じて、加工対象物に所望の力を与える力制御系を含んだ位置制御装置を提供することである。
【課題を解決するための手段】
【００１１】
本発明は、工作機械の様な数値制御機械において、切削工具の位置指令軌跡から、加工対象物の面形状を推定することで、形状に応じた、加工対象物に加える力やモーメントの方向を決定し、これらが所望値となる様な力制御系を位置制御装置に付加することで、前記課題を解決するものである。
【００１２】
すなわち、数値制御工作機械の機構の駆動部をサーボモータにより駆動し、前記駆動部の位置を位置検出器によって検出し、上位装置からの位置指令値に従って前記駆動部の位置を制御する位置制御装置であって、基準座標系で表現された切削工具の動作軌跡を示す位置指令値ベクトルから軌跡間段差ベクトルを検出するブロックと、加工対象物の面上の法線ベクトルを検出するブロックと、前記法線ベクトルの方向から定まる拘束座標系で示した力目標値を駆動軸座標系で示した力目標値に座標変換するブロックと、を具備した力指令演算部を含む位置指令演算部を備え、前記駆動軸座標系で示した力目標値と、前記駆動部の位置制御のための駆動力指令値と、を加算することによって前記駆動部の位置及び力制御のための駆動力指令値を構成することを特徴とした数値制御工作機械の制御装置である。
【発明の効果】
【００１３】
本発明によれば、力検出器等を用いなくても、切削工具により加工された加工対象物の面上に沿った位置制御と同時に、加工対象物の面に対して所望の力やモーメントを加える力制御を達成できる。このため、切削工具に換えて、布，箒，サンドペーパーなどの作業具を工具位置に搭載すれば、工作機械によって、加工対象物の面上を拭く，掃く，磨くなどの作業が実現できる。
【発明を実施するための最良の形態】
【００１４】
本実施の形態では、位置指令演算部において、隣り合う位置指令軌跡から軌跡間の段差ベクトルを検出し、軌跡上の進行方向ベクトルとの関係から、加工対象物に対する法線ベクトルを検出する。更に、法線ベクトル方向から定まる拘束座標系で、力目標値を設定し、これを各駆動軸座標系での力目標値に座標変換し出力する力指令演算部を付加する。この各駆動軸の力目標値は、各駆動軸位置制御部の駆動力指令値に加算される形態をとる。
【実施例１】
【００１５】
以下述べる工作機械では、切削工具の空間上の位置・姿勢を自由に制御できる構成を有しており、工具位置に搭載する作業具は、加工対象物に対して位置のみならず姿勢拘束も受ける形状であるものとする。
【００１６】
図１は、本発明による力制御系を付加した位置制御装置２００の構成を示すブロック図である。尚、本ブロック図において、図５（従来例）と同一の部分においては、同一の名称及び番号を付けて説明を省略する。図２は、切削加工された加工対象物の一例を示した図である。Ｘ，Ｙ，Ｚ(図示しない)は、基準座標系を示しており、Ｙ軸方向にシフトしながらＸ軸方向に走る直線は、切削工具の動作軌跡を示している。
【００１７】
動作軌跡上の点Ｐｂにおける、加工対象物内側に向かう法線ベクトルｎの導出方法について説明する。まず、図１の軌跡間段差ベクトル検出部２０２の動作について説明する。ここでは、動作軌跡を示す位置指令ベクトルｒの中で、位置を示す上位３次元ベクトルｐ＝[ｘ,ｙ,ｚ]Ｔに注目する。まず、図２に示す点ＰｂのＸＹ座標値を通る適当な段差検出線をＸＹ平面上に設定し、その任意長の２次元方向ベクトルをｌｄ＝[ｘｄ,ｙｄ]Ｔとする。今、パスｂ上をＸ軸増加方向に動作軌跡（ベクトル）ｐが進む時、点Ｐｂの通過検出は数式（２）の符号反転で検出できる。尚、[ｘ０,ｙ０]Ｔは、段差検出線の任意点座標である。
【００１８】
ｙｄ(ｘ−ｘ０)＋ｘｄ(ｙ−ｙ０)  ・・・・・（２）
【００１９】
すなわち、数式（２）の符号反転時のベクトルｐが点Ｐｂの座標値である。パスａ上で段差検出線上にある点Ｐａの座標値も、同様な方法で検出できるため、両座標値より、点Ｐａと点Ｐｂ間の軌跡間段差ベクトルｐａｂが検出できる。以上が図１における、軌跡間段差ベクトル検出部２０２の作用である。
【００２０】
次に、図１の法線ベクトル検出部２０４の作用について説明する。動作軌跡（ベクトル）ｐは、微小時間ΔＴ毎に更新されるから、点Ｐｂ通過直前のベクトルｐと、通過直後のベクトルｐとの差分から、点Ｐｂにおける進行方向の微小変位ベクトルΔｐが検出できる。ここで、微小変位ベクトルΔｐと軌跡間段差ベクトルｐａｂは、共に加工対象物上点Ｐｂの接平面上のベクトルとみなすことができるから、数式（３）の外積演算により接平面に対する直交ベクトルが求まる。
【００２１】
Δｐ×ｐａｂ  ・・・・・・（３）
【００２２】
更にこの場合は、Ｚ成分が負値をとる直交ベクトルが、加工対象物内側に向かうから、数式（３）のＺ成分が正値をとる場合は、数式（３）に−１を乗じて、法線ベクトルｎ＝[ｎｘ,ｎｙ,ｎｚ]Ｔが検出できる。以上が図１の法線ベクトル検出部２０４の動作である。尚、図２では、１本の直線として段差検出線を設定しているが、折れ線を用いてもよく、更には加工対象物の軌跡間段差の変化具合に応じて、複数の段差検出線を予め設定しておけば、動作軌跡上各部で適確に法線ベクトルを検出することができる。
【００２３】
図３は、実施例１における作業具と加工対象物の位置及び姿勢関係を表している。ここで、作業具が加工対象物から受ける姿勢拘束は、便宜上、法線ベクトルｎの持つ姿勢であるとする。すると、位置拘束座標として法線ベクトル方向にｕ１(ｒ)、姿勢拘束座標として数式（４）及び数式（５）を用いてｕ２(ｒ)，ｕ３(ｒ)が設定できる。尚、φＳ，θＳは作業具の拘束姿勢をオイラー角で表現したものである。
【００２４】
ｕ２(ｒ)＝φ−φＳ  ・・・・・・・（４）
ｕ３(ｒ)＝θ−θＳ  ・・・・・・・（５）
【００２５】
但し、法線ベクトルが−Ｚ軸方向にある時に作業具が拘束された姿勢を、作業具の基準姿勢とする。以上の３次元拘束座標系ｕ(ｒ)＝[ｕ１(ｒ),ｕ２(ｒ),ｕ３(ｒ)]Ｔ方向には、対応した１次元の力：ｆＦ１と２次元のモーメント：ｆＦ２とｆＦ３を、加工対象物に対して加えることができる。
【００２６】
ここで、ある軌跡上の任意点における微小変位（ベクトル）Δｒと、対応する拘束座標系の仮想変位（ベクトル）Δｕ(ｒ)の間には、数式（６）の関係が成立する。
【数１】
0004598720.tif 000002
【００２７】
よって、拘束座標系で示した加工対象物に与える３次元力目標値（ベクトル）ｆＦ＝[ｆＦ１,ｆＦ２,ｆＦ３]Ｔは、仮想仕事の原理などから、数式（７）により、６次元基準座標系で表した力目標値（ベクトル）ｆに換算される。
【００２８】
ｆ＝Ｔ-Ｔ・ＥＦＴ・ｆＦ  ・・・・・・（７）
【００２９】
尚、行列Ｔは、オイラー角の傾斜性を補償するための数式（８）に示す変換行列である。
【数２】
0004598720.tif 000003
【００３０】
更に、力目標値（ベクトル）ｆを駆動軸座標系で表した力目標値（ベクトル）τに変換する。これは、数式（１）の逆問題である数式（９）の順運動学問題の解を用いて、数式（１０）で換算される。但し、Ｊｒ(ｑ)＝{∂ｒ(ｑ)/∂ｑ}Ｔはヤコビ行列である。
【００３１】
ｒ＝ｒ(ｑ)  ・・・・・・・・・・・（９）
τ＝Ｊｒ(ｑ)Ｔ・ＴＴ・ｆ  ・・・・・・（１０）
【００３２】
よって結果的に、加工対象物に与える３次元力目標値（ベクトル）ｆＦは、数式（７）及び数式（１０）から、数式（１１）により、駆動軸座標系で表した力目標値（ベクトル）τ＝[τＦ１,τＦ２,・・・,τＦｎ]Ｔに換算できる。
【００３３】
τ＝Ｊｒ(ｑ)Ｔ・ＥＦＴ・ｆＦ  ・・・・・・（１１）
【００３４】
以上説明した手順で法線ベクトルｎよりベクトルＥＦを、駆動軸位置指令ベクトルｑよりヤコビ行列Ｊｒ(ｑ)をそれぞれ求めて、３次元力目標値（ベクトル）ｆＦを駆動軸座標系の力目標値（ベクトル）τに換算することが、図１の拘束・駆動軸座標変換部２０６の作用である。この様に、本発明の位置及び力の制御装置では、従来の位置指令演算部に、軌跡間段差ベクトル検出部２０２と法線ベクトル検出部２０４及び拘束・駆動軸座標変換部２０６を付加することで、各駆動軸位置制御部２０８−１〜２０８−ｎに対して、位置指令値のみならず、拘束方向に対する力目標値を出力する。
【００３５】
拘束・駆動軸座標変換部２０６の出力である駆動各軸力目標値τＦ１,τＦ２,・・・,τＦｎは、各駆動軸位置制御部２０８−１〜２０８−ｎの位置制御のための駆動力指令値と加算器５９で加算されて、最終的な位置及び力制御のための駆動力指令値τｃ１として算出される。
【実施例２】
【００３６】
実施例２における工作機械は、基準座標系に沿った３次元直交の軸構成を持ち、Ｘ,Ｙ,Ｚ軸を第１,２,３駆動軸で制御するものとする。また、各軸の駆動は回転型サーボモータを用いたボールネジ駆動系とする。更に、切削工具及び作業具の姿勢は前述の作業具の基準姿勢に機械的に拘束される構造を有しているものとする。すなわち、切削工具及び作業具の空間上の位置のみが自由に制御できる構成である。
【００３７】
図４は、実施例２における作業具と加工対象物の位置関係を示している。本例において、作業具が加工対象物に加えることができる力ベクトルは、法線ベクトル方向の１次元の力ｆＦのみになる。
【００３８】
以下、本例について説明する。本例では、姿勢に関する自由度を持たないから、位置に関する３次元ベクトルｐ＝[ｘ,ｙ,ｚ]Ｔについてのみ考えればよい。まず、軌跡間段差ベクトルから法線ベクトルｎまでの検出については、実施例１と同様である。次に、加工対象物に与える１次元力目標値ｆＦの、駆動軸座標系の力目標値τへの座標変換について考える。
【００３９】
順運動学問題の解は、数式（１２）で与えられる。但し、Ｌｘ，Ｌｙ，Ｌｚは、各軸のモータ１回転当たりの送り量である。
【００４０】
ｐ＝ｐ(ｑ)＝[(Ｌｘ/２π)ｑ１,(Ｌｙ/２π)ｑ２，(Ｌｚ/２π)ｑ３]Ｔ・・・（１２）
【００４１】
よって、数式（１０）におけるヤコビ行列Ｊｒ(ｑ)は数式（１３）の定数行列となる。
【００４２】
Ｊｒ(ｑ)＝ｄｉａｇ{(Ｌｘ/２π),(Ｌｙ/２π),(Ｌｚ/２π)}・・・・（１３）
【００４３】
更に、ベクトルＥＦは数式（６）に対して数式（１４）で示せることから、
【００４４】
ＥＦ＝[(ｎｘ/|ｎ|),(ｎｙ/|ｎ|),(ｎｚ/|ｎ|)]・・・・（１４）
【００４５】
駆動軸座標系の力目標値（ベクトル）τは、数式（１１）から数式（１５）で座標変換することができる。
【００４６】
τ＝[Ｌｘｎｘ,Ｌｙｎｙ,Ｌｚｎｚ]Ｔ{ｆＦ/(２π|ｎ|)}・・・・（１５）
【００４７】
尚、力目標値（ベクトル）τが各駆動軸位置制御部２０８−１〜２０８−ｎの位置制御のための駆動力指令値と加算器５９で加算されて、最終的な位置及び力制御のための駆動力指令値τｃ１,τｃ２,τｃ３を構成する。
【図面の簡単な説明】
【００４８】
【図１】本発明による位置及び力の制御装置の構成を示すブロック図である。
【図２】切削加工された加工対象物の一例を示した本発明の説明図である。
【図３】実施例１における作業具と加工対象物の位置関係を示す図である。
【図４】実施例２における作業具と加工対象物の位置関係を示す図である。
【図５】従来の位置制御装置の構成例を示すブロック図である。
【符号の説明】
【００４９】
５０，５２  減算器、５１，５６，５９  加算器、５３，５４，５５  微分器、５７  サーボモータ、５８  位置検出器、１００  位置制御装置、１０２  位置指令演算部、１０４  加減速演算部、１０６  駆動軸位置指令演算部、１０８  駆動軸位置制御部、２００  位置制御装置、２０２  軌跡間段差ベクトル検出部、２０４  法線ベクトル検出部、２０６  拘束・駆動軸座標変換部、２０８  駆動軸位置制御部、ＰＤ  位置指令値、ベクトルｒ  位置指令ベクトル、ベクトルｐ  ３次元位置指令ベクトル、Ｐａ  動作軌跡(パスａ)上の点、Ｐｂ  動作軌跡(パスｂ)上の点、ベクトルｐａｂ  軌跡間段差ベクトル、ベクトルｎ  法線ベクトル、ベクトルｌｄ  段差検出線方向ベクトル、Δｐ  進行方向の微小変位ベクトル、ｆＦ  拘束座標系で示した３次元力目標値、ベクトルτ  駆動軸座標系で表した力目標値ベクトル、ｑ  各軸の位置指令ベクトル、Ｖｃ１  速度指令値、ｖｄ１  速度検出値、τｃ１  駆動力指令値、ｑｄ１  位置検出値。
【図１】
0004598720.tif 000004
【図２】
0004598720.tif 000005
【図３】
0004598720.tif 000006
【図４】
0004598720.tif 000007
【図５】
0004598720.tif 000008