JP2007310594A - 数値制御工作機械の制御装置 - Google Patents

Abstract

【課題】力検出器等を用いなくても、切削工具により加工された加工対象物の面上に沿った位置制御と同時に、加工対象物の面に対して所望の力やモーメントを加える力制御を可能とする。
【解決手段】基準座標系で表現された切削工具の動作軌跡を示す位置指令値ベクトルから軌跡間段差ベクトルを検出する軌跡間段差ベクトル検出部２０２と、加工対象物の面上の法線ベクトルを検出する法線ベクトル検出部２０４と、法線ベクトルの方向から定まる拘束座標系で示した力目標値を駆動軸座標系で示した力目標値に座標変換する拘束・駆動軸座標変換部２０６と、を具備し、駆動軸座標系で示した力目標値と、駆動部の位置制御のための駆動力指令値と、を加算することによって駆動部の位置及び力制御のための駆動力指令値を構成することによって上記課題を解決することができる。
【選択図】図１

Description

本発明は、数値制御工作機械に適用される位置及び力の制御装置に関するものである。

図５は、数値制御機械の一つである工作機械における、従来の位置制御装置の構成例を示すブロック図である。位置制御装置１００はＮＣ装置の内部に含まれ、基準座標系(ｘ,ｙ,ｚ,φ,θ,ψ)で表現された切削工具の動作軌跡が、ある一定周期毎に、位置指令値ＰＤとして上位装置（図示しない）より入力される。尚、座標値(ｘ,ｙ,ｚ)は、３次元直交座標系で工具の空間上の位置を示し、座標値(φ,θ,ψ)は、工具の空間上の姿勢を示す座標値で、便宜上よく知られたＺ→Ｙ’→Ｚ”のオイラー角で表記されたものとする。

位置指令演算部１０２の加減速演算部１０４では、位置指令値ＰＤが補間加減速処理され、微小時間ΔＴ毎の切削工具の動作軌跡を示す基準座標系で見た位置指令ベクトルｒ＝[ｘ,ｙ,ｚ,φ,θ,ψ]^Ｔ(以降、ベクトルや行列を示す記号は、太文字で表し、[]^Ｔは転置を示す。)が出力される。次にベクトルｒは、駆動軸位置指令演算部１０６で、数式（１）により、各軸の位置指令ベクトルｑ＝[ｑ_１,ｑ_２,・・・,ｑ_ｎ]^Ｔに変換される。

ベクトルｑ＝ベクトルｑ(ベクトルｒ) ・・・・・（１）

数式（１）は、工作機械の軸構成により定まる逆運動学問題の解であり、解となるベクトルｑは独立した同一構成を有する各駆動軸位置制御部１０８−１〜１０８−ｎに送られる。

次に、第１駆動軸を例に、軸位置制御部１０８−１の機能について説明する。サーボモータ５７に締結された位置検出器５８は、第１駆動軸の軸位置を検出する位置検出器である。その出力である位置検出値ｑ_ｄ１は、減算器５０により、位置指令値ｑ_１から減算される。減算器５０の出力は、位置偏差増幅器Ｇｐで増幅される。一方、位置指令値ｑ_１は、微分器５３で微分されて、位置偏差増幅器Ｇｐの出力と加算器５１で加算され、第１駆動軸の速度指令値Ｖ_ｃ１となる。

位置検出値ｑ_ｄ１は、微分器５５で時間微分され、速度検出値ｖ_ｄ１となる。減算器５２では、速度指令値Ｖ_ｃ１から速度検出値ｖ_ｄ１が減算される。減算器５２の出力は、速度偏差増幅器Ｇｖで通常、比例積分増幅される。一方、前記微分器５３の出力は、微分器５４で再度微分され、増幅器ＣｂでＣｂ倍される。Ｃｂは、位置指令値ｑ_１の加速度成分に対する加減速駆動力を算出するための定数である。増幅器Ｃｂ出力は、速度偏差増幅器Ｇｖ出力と加算器５６で加算され、第１駆動軸の駆動力指令値τ_ｃ１となる。

パワー増幅部は、駆動力指令値τ_ｃ１を電力増幅し、サーボモータ５７に供給する。これにより、サーボモータ５７は、駆動力指令値τ_ｃ１通りの駆動力を発生する。以上説明した軸位置制御部の動作により、位置検出値ｑ_ｄ１は、位置指令値ｑ_１に一致する様に制御され、各軸位置制御部が独立に同様な動作を行なうことで、結果的に、切削工具の位置・姿勢動作軌跡は、前記位置指令ベクトルｒに一致することになる。

以上のように、従来の位置制御装置１００では、切削工具の位置及び姿勢を目標の動作軌跡上に制御することで、加工対象物(ワーク)を所望の形状に切削加工する。しかしながら、位置及び姿勢のみを制御しているため、加工対象物に対して力の制御を加えることができなかった。

本発明は、上記従来技術の問題点を鑑み、加工対象物の形状に応じて、加工対象物に所望の力を与える力制御系を含んだ位置制御装置を提供することである。

本発明は、工作機械の様な数値制御機械において、切削工具の位置指令軌跡から、加工対象物の面形状を推定することで、形状に応じた、加工対象物に加える力やモーメントの方向を決定し、これらが所望値となる様な力制御系を位置制御装置に付加することで、前記課題を解決するものである。

すなわち、数値制御工作機械の機構の駆動部をサーボモータにより駆動し、前記駆動部の位置を位置検出器によって検出し、上位装置からの位置指令値に従って前記駆動部の位置を制御する位置制御装置であって、基準座標系で表現された切削工具の動作軌跡を示す位置指令値ベクトルから軌跡間段差ベクトルを検出するブロックと、加工対象物の面上の法線ベクトルを検出するブロックと、前記法線ベクトルの方向から定まる拘束座標系で示した力目標値を駆動軸座標系で示した力目標値に座標変換するブロックと、を具備した力指令演算部を含む位置指令演算部を備え、前記駆動軸座標系で示した力目標値と、前記駆動部の位置制御のための駆動力指令値と、を加算することによって前記駆動部の位置及び力制御のための駆動力指令値を構成することを特徴とした数値制御工作機械の制御装置である。

本発明によれば、力検出器等を用いなくても、切削工具により加工された加工対象物の面上に沿った位置制御と同時に、加工対象物の面に対して所望の力やモーメントを加える力制御を達成できる。このため、切削工具に換えて、布，箒，サンドペーパーなどの作業具を工具位置に搭載すれば、工作機械によって、加工対象物の面上を拭く，掃く，磨くなどの作業が実現できる。

本実施の形態では、位置指令演算部において、隣り合う位置指令軌跡から軌跡間の段差ベクトルを検出し、軌跡上の進行方向ベクトルとの関係から、加工対象物に対する法線ベクトルを検出する。更に、法線ベクトル方向から定まる拘束座標系で、力目標値を設定し、これを各駆動軸座標系での力目標値に座標変換し出力する力指令演算部を付加する。この各駆動軸の力目標値は、各駆動軸位置制御部の駆動力指令値に加算される形態をとる。

以下述べる工作機械では、切削工具の空間上の位置・姿勢を自由に制御できる構成を有しており、工具位置に搭載する作業具は、加工対象物に対して位置のみならず姿勢拘束も受ける形状であるものとする。

図１は、本発明による力制御系を付加した位置制御装置２００の構成を示すブロック図である。尚、本ブロック図において、図５（従来例）と同一の部分においては、同一の名称及び番号を付けて説明を省略する。図２は、切削加工された加工対象物の一例を示した図である。Ｘ，Ｙ，Ｚ(図示しない)は、基準座標系を示しており、Ｙ軸方向にシフトしながらＸ軸方向に走る直線は、切削工具の動作軌跡を示している。

動作軌跡上の点Ｐｂにおける、加工対象物内側に向かう法線ベクトルｎの導出方法について説明する。まず、図１の軌跡間段差ベクトル検出部２０２の動作について説明する。ここでは、動作軌跡を示す位置指令ベクトルｒの中で、位置を示す上位３次元ベクトルｐ＝[ｘ,ｙ,ｚ]^Ｔに注目する。まず、図２に示す点ＰｂのＸＹ座標値を通る適当な段差検出線をＸＹ平面上に設定し、その任意長の２次元方向ベクトルをｌ_ｄ＝[ｘ_ｄ,ｙ_ｄ]^Ｔとする。今、パスｂ上をＸ軸増加方向に動作軌跡（ベクトル）ｐが進む時、点Ｐｂの通過検出は数式（２）の符号反転で検出できる。尚、[ｘ_０,ｙ_０]^Ｔは、段差検出線の任意点座標である。

ｙ_ｄ(ｘ−ｘ_０)＋ｘ_ｄ(ｙ−ｙ_０) ・・・・・（２）

すなわち、数式（２）の符号反転時のベクトルｐが点Ｐｂの座標値である。パスａ上で段差検出線上にある点Ｐａの座標値も、同様な方法で検出できるため、両座標値より、点Ｐａと点Ｐｂ間の軌跡間段差ベクトルｐ_ａｂが検出できる。以上が図１における、軌跡間段差ベクトル検出部２０２の作用である。

次に、図１の法線ベクトル検出部２０４の作用について説明する。動作軌跡（ベクトル）ｐは、微小時間ΔＴ毎に更新されるから、点Ｐｂ通過直前のベクトルｐと、通過直後のベクトルｐとの差分から、点Ｐｂにおける進行方向の微小変位ベクトルΔｐが検出できる。ここで、微小変位ベクトルΔｐと軌跡間段差ベクトルｐ_ａｂは、共に加工対象物上点Ｐｂの接平面上のベクトルとみなすことができるから、数式（３）の外積演算により接平面に対する直交ベクトルが求まる。

Δｐ×ｐ_ａｂ ・・・・・・（３）

更にこの場合は、Ｚ成分が負値をとる直交ベクトルが、加工対象物内側に向かうから、数式（３）のＺ成分が正値をとる場合は、数式（３）に−１を乗じて、法線ベクトルｎ＝[ｎ_ｘ,ｎ_ｙ,ｎ_ｚ]^Ｔが検出できる。以上が図１の法線ベクトル検出部２０４の動作である。尚、図２では、１本の直線として段差検出線を設定しているが、折れ線を用いてもよく、更には加工対象物の軌跡間段差の変化具合に応じて、複数の段差検出線を予め設定しておけば、動作軌跡上各部で適確に法線ベクトルを検出することができる。

図３は、実施例１における作業具と加工対象物の位置及び姿勢関係を表している。ここで、作業具が加工対象物から受ける姿勢拘束は、便宜上、法線ベクトルｎの持つ姿勢であるとする。すると、位置拘束座標として法線ベクトル方向にｕ_１(ｒ)、姿勢拘束座標として数式（４）及び数式（５）を用いてｕ_２(ｒ)，ｕ_３(ｒ)が設定できる。尚、φ_Ｓ，θ_Ｓは作業具の拘束姿勢をオイラー角で表現したものである。

ｕ_２(ｒ)＝φ−φ_Ｓ ・・・・・・・（４）
ｕ_３(ｒ)＝θ−θ_Ｓ ・・・・・・・（５）

但し、法線ベクトルが−Ｚ軸方向にある時に作業具が拘束された姿勢を、作業具の基準姿勢とする。以上の３次元拘束座標系ｕ(ｒ)＝[ｕ_１(ｒ),ｕ_２(ｒ),ｕ_３(ｒ)]^Ｔ方向には、対応した１次元の力：ｆ_Ｆ１と２次元のモーメント：ｆ_Ｆ２とｆ_Ｆ３を、加工対象物に対して加えることができる。

ここで、ある軌跡上の任意点における微小変位（ベクトル）Δｒと、対応する拘束座標系の仮想変位（ベクトル）Δｕ(ｒ)の間には、数式（６）の関係が成立する。

よって、拘束座標系で示した加工対象物に与える３次元力目標値（ベクトル）ｆ_Ｆ＝[ｆ_Ｆ１,ｆ_Ｆ２,ｆ_Ｆ３]^Ｔは、仮想仕事の原理などから、数式（７）により、６次元基準座標系で表した力目標値（ベクトル）ｆに換算される。

ｆ＝Ｔ^-Ｔ・Ｅ_Ｆ ^Ｔ・ｆ_Ｆ ・・・・・・（７）

尚、行列Ｔは、オイラー角の傾斜性を補償するための数式（８）に示す変換行列である。

更に、力目標値（ベクトル）ｆを駆動軸座標系で表した力目標値（ベクトル）τに変換する。これは、数式（１）の逆問題である数式（９）の順運動学問題の解を用いて、数式（１０）で換算される。但し、Ｊ_ｒ(ｑ)＝{∂ｒ(ｑ)/∂ｑ}^Ｔはヤコビ行列である。

ｒ＝ｒ(ｑ) ・・・・・・・・・・・（９）
τ＝Ｊ_ｒ(ｑ)^Ｔ・Ｔ^Ｔ・ｆ ・・・・・・（１０）

よって結果的に、加工対象物に与える３次元力目標値（ベクトル）ｆ_Ｆは、数式（７）及び数式（１０）から、数式（１１）により、駆動軸座標系で表した力目標値（ベクトル）τ＝[τ_Ｆ１,τ_Ｆ２,・・・,τ_Ｆｎ]^Ｔに換算できる。

τ＝Ｊ_ｒ(ｑ)^Ｔ・Ｅ_Ｆ ^Ｔ・ｆ_Ｆ ・・・・・・（１１）

以上説明した手順で法線ベクトルｎよりベクトルＥ_Ｆを、駆動軸位置指令ベクトルｑよりヤコビ行列Ｊ_ｒ(ｑ)をそれぞれ求めて、３次元力目標値（ベクトル）ｆ_Ｆを駆動軸座標系の力目標値（ベクトル）τに換算することが、図１の拘束・駆動軸座標変換部２０６の作用である。この様に、本発明の位置及び力の制御装置では、従来の位置指令演算部に、軌跡間段差ベクトル検出部２０２と法線ベクトル検出部２０４及び拘束・駆動軸座標変換部２０６を付加することで、各駆動軸位置制御部２０８−１〜２０８−ｎに対して、位置指令値のみならず、拘束方向に対する力目標値を出力する。

拘束・駆動軸座標変換部２０６の出力である駆動各軸力目標値τ_Ｆ１,τ_Ｆ２,・・・,τ_Ｆｎは、各駆動軸位置制御部２０８−１〜２０８−ｎの位置制御のための駆動力指令値と加算器５９で加算されて、最終的な位置及び力制御のための駆動力指令値τ_ｃ１として算出される。

実施例２における工作機械は、基準座標系に沿った３次元直交の軸構成を持ち、Ｘ,Ｙ,Ｚ軸を第１,２,３駆動軸で制御するものとする。また、各軸の駆動は回転型サーボモータを用いたボールネジ駆動系とする。更に、切削工具及び作業具の姿勢は前述の作業具の基準姿勢に機械的に拘束される構造を有しているものとする。すなわち、切削工具及び作業具の空間上の位置のみが自由に制御できる構成である。

図４は、実施例２における作業具と加工対象物の位置関係を示している。本例において、作業具が加工対象物に加えることができる力ベクトルは、法線ベクトル方向の１次元の力ｆ_Ｆのみになる。

以下、本例について説明する。本例では、姿勢に関する自由度を持たないから、位置に関する３次元ベクトルｐ＝[ｘ,ｙ,ｚ]^Ｔについてのみ考えればよい。まず、軌跡間段差ベクトルから法線ベクトルｎまでの検出については、実施例１と同様である。次に、加工対象物に与える１次元力目標値ｆ_Ｆの、駆動軸座標系の力目標値τへの座標変換について考える。

順運動学問題の解は、数式（１２）で与えられる。但し、Ｌ_ｘ，Ｌ_ｙ，Ｌ_ｚは、各軸のモータ１回転当たりの送り量である。

ｐ＝ｐ(ｑ)＝[(Ｌ_ｘ/２π)ｑ_１,(Ｌ_ｙ/２π)ｑ_２，(Ｌ_ｚ/２π)ｑ_３]^Ｔ・・・（１２）

よって、数式（１０）におけるヤコビ行列Ｊ_ｒ(ｑ)は数式（１３）の定数行列となる。

Ｊ_ｒ(ｑ)＝ｄｉａｇ{(Ｌ_ｘ/２π),(Ｌ_ｙ/２π),(Ｌ_ｚ/２π)}・・・・（１３）

更に、ベクトルＥ_Ｆは数式（６）に対して数式（１４）で示せることから、

Ｅ_Ｆ＝[(ｎ_ｘ/|ｎ|),(ｎ_ｙ/|ｎ|),(ｎ_ｚ/|ｎ|)]・・・・（１４）

駆動軸座標系の力目標値（ベクトル）τは、数式（１１）から数式（１５）で座標変換することができる。

τ＝[Ｌ_ｘｎ_ｘ,Ｌ_ｙｎ_ｙ,Ｌ_ｚｎ_ｚ]^Ｔ{ｆ_Ｆ/(２π|ｎ|)}・・・・（１５）

尚、力目標値（ベクトル）τが各駆動軸位置制御部２０８−１〜２０８−ｎの位置制御のための駆動力指令値と加算器５９で加算されて、最終的な位置及び力制御のための駆動力指令値τ_ｃ１,τ_ｃ２,τ_ｃ３を構成する。

本発明による位置及び力の制御装置の構成を示すブロック図である。 切削加工された加工対象物の一例を示した本発明の説明図である。 実施例１における作業具と加工対象物の位置関係を示す図である。 実施例２における作業具と加工対象物の位置関係を示す図である。 従来の位置制御装置の構成例を示すブロック図である。

符号の説明

５０，５２ 減算器、５１，５６，５９ 加算器、５３，５４，５５ 微分器、５７ サーボモータ、５８ 位置検出器、１００ 位置制御装置、１０２ 位置指令演算部、１０４ 加減速演算部、１０６ 駆動軸位置指令演算部、１０８ 駆動軸位置制御部、２００ 位置制御装置、２０２ 軌跡間段差ベクトル検出部、２０４ 法線ベクトル検出部、２０６ 拘束・駆動軸座標変換部、２０８ 駆動軸位置制御部、ＰＤ 位置指令値、ベクトルｒ 位置指令ベクトル、ベクトルｐ ３次元位置指令ベクトル、Ｐａ 動作軌跡(パスａ)上の点、Ｐｂ 動作軌跡(パスｂ)上の点、ベクトルｐ_ａｂ 軌跡間段差ベクトル、ベクトルｎ 法線ベクトル、ベクトルｌ_ｄ 段差検出線方向ベクトル、Δｐ 進行方向の微小変位ベクトル、ｆ_Ｆ 拘束座標系で示した３次元力目標値、ベクトルτ 駆動軸座標系で表した力目標値ベクトル、ｑ 各軸の位置指令ベクトル、Ｖ_ｃ１ 速度指令値、ｖ_ｄ１ 速度検出値、τ_ｃ１ 駆動力指令値、ｑ_ｄ１ 位置検出値。

Claims (1)

1. 数値制御工作機械の機構の駆動部をサーボモータにより駆動し、前記駆動部の位置を位置検出器によって検出し、上位装置からの位置指令値に従って前記駆動部の位置を制御する位置制御装置であって、
   基準座標系で表現された切削工具の動作軌跡を示す位置指令値ベクトルから軌跡間段差ベクトルを検出するブロックと、
   加工対象物の面上の法線ベクトルを検出するブロックと、
   前記法線ベクトルの方向から定まる拘束座標系で示した力目標値を駆動軸座標系で示した力目標値に座標変換するブロックと、
   を具備した力指令演算部を含む位置指令演算部を備え、
   前記駆動軸座標系で示した力目標値と、前記駆動部の位置制御のための駆動力指令値と、を加算することによって前記駆動部の位置及び力制御のための駆動力指令値を構成することを特徴とした数値制御工作機械の制御装置。
   

Images