JP4101665B2

JP4101665B2 - Numerical controller

Info

Publication number: JP4101665B2
Application number: JP2003008615A
Authority: JP
Inventors: 智典佐藤; 鋭志林; 弘太朗長岡
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2003-01-16
Filing date: 2003-01-16
Publication date: 2008-06-18
Anticipated expiration: 2023-01-16
Also published as: JP2004220435A

Description

【０００１】
【発明の属する技術分野】
この発明は、回転自由度を有する機械の軌跡制御を行う数値制御装置に関するものである。
【０００２】
【従来の技術】
５軸加工機、三次元レーザ加工機やロボットなどの回転軸自由度を備えた機械において、機械の先端に備えた切削工具、ロボットハンドあるいはレーザ加工機のトーチ等の工具の位置および姿勢を制御する方法として、与えられた指令を自動的に機械の各関節軸の座標値（関節座標）に座標変換して制御する方法がよく知られている。これは、５軸加工機分野においては工具先端点制御と呼ばれる方法である。
【０００３】
ここで、工具の位置および姿勢を作業対象に固定された直交座標系（ワーク座標系）上で指令し、この座標系上で工具をある速度で動作させた場合、各関節の速度や加速度がその関節の許容値を超える場合がある。特に機械の動作範囲中に特異点（特異姿勢）が存在する機械においては、このような現象が顕著であることが一般に知られている。
このように、特異点あるいはその近傍を通過する際などには、速度や加速度が過大となり、サーボアラームを起こして機械停止したり、機械振動を起こしたりするおそれがある。
【０００４】
この問題を解決するため、例えば、特許文献１、特許文献２および特許文献３で開示されている制御装置では、特異点近傍で速度が過大となった時に速度を下げる方法を採用している。
【０００５】
この方法を用いると、軌跡誤差、すなわち工具の位置や姿勢の誤差は発生しないが、特異点近傍で関節の速度を下げるので必然的に移動時間が長くなるという問題があった。
また、解の切り換えを行う場合には効果がないという問題があった。つまり、与えられた工具位置と姿勢を満たす関節座標値の解は一般に複数組存在するが、ある解の組から別の解の組への切り換えを行ったときに発生する不連続性は解消されないという問題があった。
【０００６】
また、別の解決方法として、例えば、特許文献４から特許文献８で開示されている制御装置では、特異点近傍において、ワーク座標系上で指令された工具の位置と姿勢ではなく、関節座標値を補間する方法を採用している。
【０００７】
この方法によると、速度を下げず、また解の切り換えを行う場合にも有効であるが、大きな軌跡誤差が発生するという問題があった。特に、軌跡誤差の量を直接管理できないのが問題であった。
特異点近傍では特殊な補間処理を行うので軌跡誤差が生じやすく、その大きさは補間処理を行う範囲に依存する。従来の制御装置は、各関節軸の速度や加速度に基づいてこの範囲を決定しており、軌跡誤差を考慮してこの範囲を求める方法はこれまで検討されてこなかった。
【０００８】
【特許文献１】
特開昭５８−１１４８８８号公報
【特許文献２】
特開平３−０６６５７６号公報
【特許文献３】
特開平６−３２４７３０号公報
【特許文献４】
特開昭６２−１５４００６号公報
【特許文献５】
特開昭６２−１８９５０４号公報
【特許文献６】
特開昭６０−０５７４０８号公報
【特許文献７】
特開昭６３−３０５４０７号公報
【特許文献８】
特開平２−２１８５６９号公報
【０００９】
【発明が解決しようとする課題】
このように、従来の制御装置では、軌跡誤差の抑制と移動時間の短縮を両立することができなかった。
【００１０】
この発明は上記のような課題を解決するためになされたもので、回転自由度を有する機械の軌跡制御において、特異点近傍など各関節の速度や加速度が過大となるような場合に、軌跡誤差に対する要求精度を満たしつつ、移動時間をできるだけ短縮することが可能な数値制御装置を得ることを目的とする。
【００１１】
【課題を解決するための手段】
この発明に係る数値制御装置は、工具に与えられた２つの時点での姿勢に基づいて、両時点の間の全範囲において関節補間を行うときに生じる姿勢誤差が姿勢誤差許容値より大きい場合に、工具の姿勢誤差が姿勢誤差許容値以下となるまで関節補間範囲を小さくするように修正する関節補間範囲決定部と、関節補間範囲においては関節補間により工具の姿勢を制御する情報を算出し、その他の範囲においては回転主軸法により工具の姿勢を制御する情報を算出する補間部とを備えたものである。
【００１２】
【発明の実施の形態】
以下、この発明の実施の様々な形態を説明する。
実施の形態１．
図１は、この発明の実施の形態１による数値制御装置１０（図２参照）により軌跡制御を行うテーブルチルト形の５軸加工機１の斜視図である。図に示すように、５軸加工機１は、並進変位を与えるＸ，Ｙ，Ｚの３つの直線軸、Ｘ軸回りの回転角度Ａを与える傾斜テーブル２、およびＺ軸回りの回転角度Ｃを与える旋回テーブル３を有する。ワーク５は５軸加工機１の作業対象であり、工具４によって加工される。
工具４は、数値制御装置１０により、サーボ制御装置１４（図２参照）を介してワーク５に対する位置と姿勢を制御され、ワーク５の加工を行う。
この５軸加工機１では、工具４はそれが指示された主軸と共に傾けられることはなく、常に垂直方向に向けられている。ワーク５が傾斜テーブル２、旋回テーブル３によって回転されることで、工具４とワーク５の相対姿勢が制御される。
【００１３】
工具４の位置ベクトルｐと姿勢ベクトルｑは、作業対象であるワーク５に固定されたワーク座標系Σｗ上の座標値（ワーク座標値）として与えられる。
一方、Ｘ，Ｙ，Ｚ，Ａ，Ｃ各軸上での座標値を関節座標値と呼ぶ。
また、以下の説明では、機械の備える軸数をｎとし、各関節軸（直線軸と回転軸を含む）をθｊ（ｊ＝１〜ｎ）とおく。
図１に示した機構の場合には、ｎ＝５であり、θ１，θ２，θ３，θ４，θ５はそれぞれＸ，Ｙ，Ｚ，Ａ，Ｃに対応する。
【００１４】
工具４のワーク座標値と関節座標値の間には、機械構造で決まる座標変換の関係が成り立ち、数値制御装置において、指令されたワーク座標値から関節座標値への変換が行われる。
【００１５】
図１に示したような機構の場合、工具４のワーク座標値ｐ，ｑとＸ，Ｙ，Ｚ，Ａ，Ｃ各軸の関節座標値との関係は以下のようになる。
ｐ＝Ｒｃ（Ｃ）・（Ｒａ（Ａ）ξ−Ｈ２）−Ｈ３（式１）
ｑ＝｛Ｉ，Ｊ，Ｋ｝^ｔ（式２）
ただし、
ξ＝｛Ｘ，Ｙ，Ｚ｝^ｔ（式３）
Ｉ＝ｓｉｎＡ・ｓｉｎＣ（式４）
Ｊ＝−ｓｉｎＡ・ｃｏｓＣ（式５）
Ｋ＝ｃｏｓＡ（式６）
ここでＲａ（Ａ），Ｒｃ（Ｃ）は、それぞれＸ軸、Ｚ軸まわりにそれぞれ角度Ａ，Ｃだけ回転を行う３×３の行列である。Ｈ２は、傾斜テーブル２の回転中心から旋回テーブル３の回転中心へのオフセットベクトル、Ｈ３は、旋回テーブル３の回転中心からワーク座標系原点へのオフセットベクトルである。また右肩添え字ｔは転置を示す。また、姿勢ベクトルｑは単位ベクトルである。
【００１６】
Ａ＝０、すなわち図１において旋回テーブル３の上面が水平となったとき、式２よりＣの座標値Ｃは工具４のｑの変化に寄与しなくなる。この時の工具４の状態が特異姿勢にあたる。よって、特異姿勢を示す姿勢ベクトルをｑｓとすると、
ｑｓ＝｛００１｝^ｔ（式７）
となる。
【００１７】
また、逆にワーク座標値から関節座標値を求めると次のようになる。
Ａ＝ｃｏｓ^−１（Ｋ）（式８）
Ｃ＝ａｔａｎ２（Ｉ／ｓｉｎＡ，−Ｊ／ｓｉｎＡ）（式９）
（ただし、ａｔａｎ２（Ｘ，Ｙ）＝ｔａｎ^−１（Ｘ／Ｙ））
ξ＝Ｒａ（−Ａ）・（Ｒｃ（−Ｃ）・（ｐ＋Ｈ３）＋Ｈ２）（式１０）
【００１８】
図２は、この発明の実施の形態１による数値制御装置の構成を示すブロック図である。図に示すように、数値制御装置１０は、関節補間範囲決定部１１、補間部１２、作業指令部１３を備える。
【００１９】
次に、数値制御装置１０の動作について説明する。
実施の形態１では、工具４の姿勢の補間方法として、関節補間と回転主軸法の２つを用いる。
作業指令部１３は、作業プログラムに従い、工具４の位置ｐと姿勢ｑを指令する。関節補間範囲決定部１１は、指令された工具４の位置ｐと姿勢ｑ、および誤差許容値に基づいて関節補間の範囲を決定する。関節補間範囲の決定方法については後述する。
補間部１２は、決定された関節補間の範囲内では関節補間により、それ以外の範囲においては回転主軸法（下記参照）により補間を行い、工具４の位置ｐと姿勢ｑに従って各関節軸の関節座標値を計算し、サーボ制御装置１４に送信する。サーボ制御装置１４は、送られた関節座標値に基づいて５軸加工機１を駆動する。
なお、工具４の位置ｐおよび姿勢ｑは、作業プログラムによらず、マニュアル入力によりあるいは外部装置を通して指令されるようにしてもよい。
【００２０】
ここで、実施の形態１で用いる２つの補間方法について簡単に説明する。
関節補間とは、工具４に与えられた始点と終点における位置ベクトルおよび姿勢ベクトルに基づいて、各軸の関節座標値を補間する方法である。この方法では、移動時間は短くできるが、姿勢誤差が生じる。
また、回転主軸法とは、与えられた始点と終点における姿勢ベクトルに直交する法線ベクトルを軸に姿勢を回転させることで補間を行う方法である。この方法では、特異点近傍で速度を下げる必要がある。詳細は、Ｐａｕｌ著、吉川恒夫訳、ロボット・マニピュレータ、コロナ社、１９８４。この文献は、本願の開示の一部をなす。
【００２１】
図３は、実施の形態１による数値制御装置１０の関節補間範囲決定部１１による関節補間範囲算出処理のフローチャートである。
【００２２】
まず、関節補間範囲決定部１１は、関節補間を行う範囲の始点ｔｘ０と終点ｔｘ１（ｔは媒介変数）の初期値を設定する（ステップＳＴ１）。この初期値の設定では、ｔｘ０＝０、ｔｘ１＝１とする。これは指令された経路の全範囲において関節補間を行うことを意味している。
【００２３】
次に、工具４の姿勢誤差を計算する（ステップＳＴ２）。
姿勢誤差の計算方法を以下に示す。始点の姿勢ベクトルｑ（ｔｘ０）から、式８および式９に従って、始点におけるＡおよびＣの関節座標値Ａｘ０、Ｃｘ０を求める。同様に終点の姿勢ベクトルｑ（ｔｘ１）から終点におけるＡおよびＣの関節座標値Ａｘ１、Ｃｘ１を求める。
また、経路の中央ｔｘｈにおけるＡ，Ｃ軸の関節座標値は、
Ａｘｈ＝（Ａｘ０＋Ａｘ１）／２（式１１）
Ｃｘｈ＝（Ｃｘ０＋Ｃｘ１）／２（式１２）
である。また、ｔｘｈは、
ｔｘｈ＝（ｔｘ０＋ｔｘ１）／２
である。よって、ｔｘ０＝０，ｔｘ１＝１のときは、ｔｘｈ＝０．５となる。
このＡｘｈ、Ｃｘｈを式２、式４、式５、式６に代入すると、式１３に示すように経路の中央における姿勢ベクトルｑ（ｔｘｈ）が得られる。
q(txh)＝{sinAxh・sinCxh −sinAxh・cosCxh cosAxh}^ｔ（式１３）
また、移動経路の中央における本来の姿勢ベクトルｑｈを次式により求める。ここで、本来の姿勢ベクトルとは、始点の姿勢ベクトルと終点の姿勢ベクトルのなす平面内にあり、かつ始点の姿勢ベクトルと終点の姿勢ベクトルのなす角度の２等分線上に位置するベクトルである。
ｑｈ＝ｑｓ−（ｑｓ・ｑｎ）ｑｎ（式１４）
ただし、ｑｎは、ｑ（ｔｘ０）とｑ（ｔｘ１）に直交するベクトルである。
姿勢ベクトルｑ（ｔｘｈ）と、本来の姿勢ベクトルｑｈとの姿勢誤差ε１は以下のようになる。
ε１＝ｃｏｓ^−１（ｑｈ・ｑ（ｔｘｈ））（式１５）
【００２４】
次に、姿勢誤差ε１と、姿勢誤差許容値εｍａｘの比較を行う（ステップＳＴ３）。比較の結果、ε１がεｍａｘより大きい場合には関節補間の範囲（ｔｘ０，ｔｘ１）を変更する（ステップＳＴ４）。一方、ε１がεｍａｘ以下の場合には処理を終了し、補間部１２において、直前に求めた関節補間の範囲（ｔｘ０，ｔｘ１）で関節補間による５軸加工機１の制御を行う。
【００２５】
ステップＳＴ４では、次のようにｔｘ０およびｔｘ１を変更する。
ｔｘ０’＝ｔｘｈ−ｄｔｘ／２（式１６）
ｔｘ１’＝ｔｘｈ＋ｄｔｘ／２（式１７）
ただし、
ｄｔｘ＝（ｔｘ１−ｔｘ０）・εｍａｘ／ε１（式１８）
ここでは、関節補間を行う範囲の幅ｄｔｘと姿勢誤差εは近似的に比例すると仮定している。ｄｔｘを姿勢誤差εの比率に応じて求め、このようにして得られたｄｔｘを用いて、（ｔｘ０’，ｔｘ１’）を求めている。また、ここでは、指令経路の中央で特異姿勢ｑｓに最も近づくと仮定している。ステップＳＴ４では、ｔｘ０’，ｔｘ１’算出後、（ｔｘ０，ｔｘ１）は（ｔｘ０’，ｔｘ１’）に置き換えられる。
【００２６】
実際には、関節座標Ａ，Ｃと工具姿勢Ｉ，Ｊ，Ｋは、式８、式９に示したように非線形の関係にある。従って、より正確に関節補間を行う範囲（ｔｘ０，ｔｘ１）を求めるには、繰り返し計算を行うことが好ましい。そこで、実施の形態１では、ステップＳＴ４において変更された区間（ｔｘ０，ｔｘ１）を用いてステップＳＴ２で再度姿勢誤差ε１を求める。この手順を繰り返し行うことにより、正確な（ｔｘ０，ｔｘ１）の値を求めることができる。
【００２７】
なお、より簡易的な方法としては、ステップＳＴ３でε１がεｍａｘより大きいと判断された場合に、ステップＳＴ４で範囲を変更した後、ステップＳＴ２に戻らずに、直接ステップＳＴ５に進むようにしてもよい。この場合には、正確に（ｔｘ０，ｔｘ１）を求められないので移動時間が若干長くなるが、計算処理が単純化されるのでより安価な計算機でも実現可能である。
【００２８】
また、ステップＳＴ３では関節補間を行う範囲の中央における姿勢誤差を計算したが、中央以外の箇所における姿勢誤差を評価してもよい。これにより姿勢誤差の計算精度が向上する。また機構や指令経路によっては解析的に正確な姿勢誤差が求められる場合もある。この場合には、解析的に関節補間を行う範囲を求めることにより、計算量の削減と、関節補間を行う範囲を求める精度の向上が可能になる。
【００２９】
また、ステップＳＴ３では、最も単純な収束判定条件として単に誤差が許容値以下となるような条件としたが、ε１とεｍａｘの差の絶対値あるいは比率の上限・下限を設け、ε１≒εｍａｘとなる解を求めることで、移動時間をさらに短くすることができる。また実用性を考えると、ステップＳＴ２からステップＳＴ４への繰り返し回数の上限および（ｔｘ０，ｔｘ１）の変化量の絶対値の下限を設けることが望ましい。
【００３０】
次に、具体的な数値を用いて説明する。図４に示すように、工具４の指令された移動経路の始点でのワーク座標を（ｐ０，ｑ０）、終点におけるワーク座標の位置と姿勢を（ｐ１，ｑ１）とし、具体的に次のような値とする。
ｑ０＝｛０．７０７１，−０．０１２３，０．７０７０｝^ｔ（式１９）
ｑ１＝｛−０．７０７１，−０．０１２３，０．７０７０｝^ｔ（式２０）
このときｑ０とｑ１のなす角度φは９０度である。始点、終点におけるＡ軸の座標値はいずれも４５．００８７度である。また始点、終点におけるＣ軸の座標値はそれぞれ８９．０００２，−８９．０００２度である。
この例の場合、ｑ０とｑ１によって形成される平面は、工具４の移動経路の中央において特異姿勢ｑｓに最も近づく。そのときの工具４の姿勢ベクトルｑｈは、
ｑｈ＝｛０．００００，−０．０１７５，０．９９９８｝^ｔ（式２１）
である。この時、ｑｈとｑｓのなす角度は約１度である。また、ここでの姿勢誤差（ｑがｑ０とｑ１のなす平面からずれる角度）の許容値εｍａｘは２０度とする。
【００３１】
図５から図８は、上記の指令経路に対して、実施の形態１による数値制御装置によって工具４の軌跡制御を行った場合（△印）と、従来の方法により行った場合の結果を示している。
図５は、工具４が始点から終点まで移動する間の姿勢ベクトルｑの軌跡を示す図である。また、図６は、工具４が始点から終点まで移動する間のＡ軸およびＣ軸の角度の変化を示し、図７は、始点から終点まで移動する間のＡ軸およびＣ軸の角速度の変化を示す。さらに、図８は、始点から終点まで移動する間の姿勢誤差の変化を示している。
【００３２】
図５から図８において、×印の軌跡は、従来技術に関連して述べた関節補間を適用した場合の結果を示している。関節補間では、図６に示すように、Ａ軸の座標値は始点から終点まで同じであり、Ｃ軸だけが動く。その結果、姿勢ベクトルｑの軌跡は図５に示すように本来あるべきｑ０とｑ１のなす平面から大きくずれた軌跡になる。すなわち、図８に示すように、約４４度もの大きな姿勢誤差が生じる。
【００３３】
また、○印の軌跡は、回転主軸法による補間である。回転主軸法では、図５および図８に示すように姿勢誤差は生じない。しかしそれを実現するためには、図６に示すように、特異点近傍（０．６秒近傍）で、急激にＣ軸を動作させる必要がある。その結果、特異点近傍において非常に大きなＣ軸の速度変化を引き起こし、その値は約−３０００度／秒まで達する（図７参照）。実際にはこのようなきわめて高速で高加速度の動作をすることは不可能である場合がほとんどである。
【００３４】
これに対し、△印の軌跡で示される本発明を適用した結果は、図８に示すように、姿勢誤差が約２０度で許容範囲となり、かつ各関節速度（Ａ，Ｃ軸の速度）が極端に大きくなることもない。
【００３５】
なお、本例においては、変更された（ｔｘ０，ｔｘ１）による姿勢誤差ε１は許容値（２０度）より若干小さい値となり処理は終了した。
【００３６】
一般に工具の姿勢の誤差は多少許容される場合が多いが、工具の位置の誤差は可能な限り小さくしたい場合が多い。その場合には、直線軸（実施の形態１ではＸ，Ｙ，Ｚ軸）の関節座標値ξは、工具の位置ベクトルｐと、回転軸（実施の形態１ではＡ，Ｃ軸）の関節座標値から式１０に従って求める。式１０に従って直線軸の関節座標値を求める限り、工具の位置の誤差は生じないため、工具の位置の誤差の発生を避けたい場合にはこの方法が好適である。なおこの場合には、関節補間範囲決定部１１において位置ベクトルｐの情報は必要としないため、作業指令部１３から関節補間範囲決定部１１への位置ベクトルｐの情報の転送を省略することができる。
一方、工具の位置の誤差が所定の範囲内で許容される場合には、前述した関節補間範囲内においては式１０に従わず、直線軸についても関節補間を行ってもよい。これにより、位置誤差が生じるが、直線軸の速度や加速度が急激に変化することを避けることができる。このような場合、さらに好ましくは、図３に示したフローチャートのステップＳＴ２において、姿勢誤差ε１だけでなく工具の位置誤差も求め、ステップＳＴ３で姿勢誤差と位置誤差のいずれかがそれぞれの許容値よりも大きいかどうかを判定するようにすれば、工具の位置誤差も許容値以下に制限できる。
【００３７】
以上のように、この実施の形態１によれば、関節補間範囲決定部１１が、姿勢誤差が姿勢誤差許容値以下となるように関節補間範囲を決定し、その他の範囲では回転主軸法を用いるようにしたので、関節速度を極端に大きくすることなく、かつ指定された姿勢誤差許容値以下に姿勢誤差を制限することができるという効果がある。また、解の切り換えを行う場合にも効果が得られる。
【００３８】
実施の形態２．
実施の形態１は、工具４の姿勢誤差を制御するのに適しているが、さらに、各関節軸の許容速度を満たすように制御を行うことが望ましい。実施の形態２では、各関節の許容速度を満たすように、移動速度を制御する。
【００３９】
図９は、この発明の実施の形態２による数値制御装置２０の構成を示すブロック図である。図２と同一の符号は、同一の構成要素をあらわしている。速度決定部２５は、指令された位置ｐと姿勢ｑ、関節補間範囲決定部１１が決定する関節補間の範囲、および各関節軸の許容速度から、各軸の速度Ｖｊ（ｊ＝１〜ｎ）を求める。補間部２２は、この速度Ｖｊを用いて速度を決定し、補間を行う。
【００４０】
次に、動作について説明する。
図１０は、実施の形態２による数値制御装置２０の速度決定部２５による速度決定処理のフローチャートである。当処理は、関節補間範囲決定部１１による関節補間範囲算出処理の後に行われる。実施の形態２による関節補間範囲算出処理は、実施の形態１と同様の処理である。
【００４１】
まず、関節補間を行う範囲における各関節座標の変化量ｄθｊ（ｊ＝１〜ｎ）を求める（ステップＳＴ２１）。すなわち、関節補間を行う範囲が媒介変数ｔの範囲としてｔｘ０＜ｔ＜ｔｘ１と与えられたとき、ｑ（ｔｘ０）から式８、式９に従い、Ａｘ０，Ｃｘ０を求める。また同様にｑ（ｔｘ１）からＡｘ１，Ｃｘ１を求める。関節補間を行う範囲における各関節座標の変化量は次のように求められる。
ｄＡ＝｜Ａｘ１−Ａｘ０｜（式２２）
ｄＣ＝｜Ｃｘ１−Ｃｘ０｜（式２３)
【００４２】
次に、ｄθｊおよび各関節の許容速度から、以下の式により各軸の移動時間のうち最大のものを求める（ステップＳＴ２２）。
Ｔｍａｘ＝ｍａｘ（ｄＡ／Ｖｍａ，ｄＣ／Ｖｍｃ）（式２４)
ここでＶｍａ，Ｖｍｃは、それぞれＡ軸，Ｃ軸の許容速度である。
【００４３】
さらに、この最大時間Ｔｍａｘと各関節座標の変化量（ｄＡ，ｄＣ）から、各軸の速度Ｖａ，Ｖｃを求める（ステップＳＴ２３）。
Ｖ４＝Ｖａ＝ｄＡ／Ｔｍａｘ（式２５）
Ｖ５＝Ｖｃ＝ｄＣ／Ｔｍａｘ（式２６）
【００４４】
具体的な例を挙げて説明する。Ａ軸、Ｃ軸の許容速度をそれぞれ２０度／秒、１００度／秒とする。
【００４５】
図１１は、工具４が始点から終点まで移動する間のＡ軸、Ｃ軸の角速度の変化を示している。図中、○印は従来の回転主軸法による補間に速度制限を加味した場合の軌跡である。すなわち、補間周期毎にＡ軸、Ｃ軸のいずれかが許容速度に一致するように時々刻々の速度を修正した。△印は、実施の形態２による軌跡を示している。どちらの方法も、許容速度の制約は満たしている。しかし図に示すように、Ｃ軸の速度の波形において、−１００度／ｓで移動する時間は従来の方法に比べ、実施の形態２の方が長い。この余分に長い部分は、従来は姿勢の精度を保つためには速度を上げられなかったが、ある程度の姿勢誤差を許容することにより速度を上げている部分である。
結果として、移動時間を比較すると次のようになる。
従来の方法（回転主軸法）移動時間＝約５．７秒
実施の形態２移動時間＝約４．４秒
【００４６】
このように、実施の形態２によれば、許容範囲の姿勢誤差をある程度許容する分、速度を高くすることができるので、従来の回転主軸法による補間に比べて、移動時間が短縮できる。
【００４７】
なお姿勢誤差許容値を大きくしていくと、関節補間を行う範囲が広がり移動時間が短くなる。さらに関節補間を行う範囲を広げ、指令経路の全範囲で関節補間を用いるようにすると移動時間は最も短くなる。このとき姿勢誤差も大きくなるのは明らかである。今回のように、姿勢誤差が許容値の２０度以下となる条件でもっとも移動時間が短いのは、実施の形態２の数値制御装置を用いることにより得られた解である。
【００４８】
なお図１０に示した手順では、各関節の回転軸に着目し、各軸の座標値の変化量と許容速度から最大時間を求めたが、より理想的なのは、直線軸（Ｘ，Ｙ，Ｚ）も含め全関節軸で同様の処理を行うことである。これにより全ての軸で許容速度の制約がみたされ、さらに滑らかな動作を実現することが可能となる。
逆に、実用上はもっとも速度が過大になりやすい特定の軸（上述の例ではＣ軸）のみに着目することで、計算量の削減を図ることも可能である。この場合は、他の軸の速度制約は別の手段、（例えば機構の工夫、指令経路や指令速度のプログラミングの工夫等）で回避する必要がある。
【００４９】
実施の形態３．
実施の形態１および実施の形態２で述べた方法では、図７、図１１から明らかなように、関節補間を行う範囲と、回転主軸法により補間を行う範囲との境界において速度が不連続となる。この速度不連続性は、機械振動やサーボの過負荷を生じさせるおそれがある。実施の形態３では、この不連続性を改善する。
【００５０】
図１２は、実施の形態３による数値制御装置３０の構成を示すブロック図である。図２と同一の符号は、同一の構成要素をあらわしている。曲線決定部３６は、指令された位置ｐと姿勢ｑ、関節補間の範囲から、関節補間を行う範囲において用いる曲線のパラメータを計算し、補間部３２に送る。補間部３２は、指定されたパラメータで決まる曲線に従って曲線補間を行う。
【００５１】
次に動作について説明する。
図１３は、実施の形態３による数値制御装置３０の曲線決定部３６による曲線決定処理のフローチャートである。当処理は、関節補間範囲決定部１１による関節補間範囲算出処理の後に行われる。実施の形態３による関節補間範囲算出処理は、実施の形態１と同様の処理である。
【００５２】
まず、関節補間を行う範囲の両端における座標値と接線ベクトルを求める（ステップＳＴ３１）。
具体的には、まずｑ（ｔｘ０）から式８、式９に従い、Ａｘ０、Ｃｘ０を求める。同様に、ｑ（ｔｘ１）からＡｘ１、Ｃｘ１を求める。接線ベクトルは微小な媒介変数Δｔだけ離れた箇所の角度との差分を用いて計算する。すなわちｔｘ０−Δｔの位置のＡ軸、Ｃ軸の座標値を同様に求め、Ａｘ０ｐｒｅ、Ｃｘ０ｐｒｅとする。またｔｘ１＋Δｔの位置のＡ軸、Ｃ軸の座標値を同様に求め、Ａｘ１ｐｏｓｔ、Ｃｘ１ｐｏｓｔとする。これらを用いて、始点接線ベクトル（ｕａｘ０，ｕｃｘ０）および終点接線ベクトル（ｕａｘ１，ｕｃｘ１）を次のように求める。
ｕａｘ０＝（Ａｘ０−Ａｘ０ｐｒｅ）／Δｔ（式２７）
ｕｃｘ０＝（Ｃｘ０−Ｃｘ０ｐｒｅ）／Δｔ（式２８）
ｕａｘ１＝（Ａｘ１ｐｏｓｔ−Ａｘ１）／Δｔ（式２９）
ｕｃｘ１＝（Ｃｘ１ｐｏｓｔ−Ｃｘ１）／Δｔ（式３０）
【００５３】
次に、媒介変数ｔに対応するＡ軸、Ｃ軸の座標値を、次に示すように３次スプライン曲線を用いて、スプライン補間により求める（ステップＳＴ３２）。
A＝Ax0＋B・M・{0 (Ax1-Ax0) Lx・uax0 Lx・uax1}^ｔ（式３１）
C＝Cx0＋B・M・{0 (Cx1-Cx0) Lx・ucx0 Lx・uax1}^ｔ（式３２）
ただし、
τ＝（ｔ−ｔｘ０）／Ｌｘ（式３３）
Ｌｘ＝ｔｘ１−ｔｘ０（式３４）
Ｂ＝｛τ^３ τ^２ τ １｝（式３５）
【数１】

【００５４】
具体的な例を挙げて説明する。図１４は、工具４が始点から終点まで移動する間のＡ軸、Ｃ軸の角速度の変化を示している。図中、○印は従来の回転主軸法による補間に速度制限を加味した場合の軌跡である。△印は、実施の形態３による軌跡を示している。
【００５５】
実施の形態３によれば、Ａ軸、Ｃ軸とも図１１に見られた速度の不連続性が解消され、滑らかな波形となっていることがわかる。ただし速度を滑らかにした分、移動時間は実施の形態２より長くなり、約５．０秒となった。それでも従来の回転主軸法を用いた場合よりは移動時間が短い。
【００５６】
このように、実施の形態３によれば、関節補間を行う範囲と回転主軸法による補間を行う範囲との境界部分に生じる速度の不連続性をなくすことができ、より滑らかな動作が実現できる。例えば、金型加工など特に滑らかな動作が要求される場合などに有効である。
【００５７】
なお、実施の形態３では、差分により接線ベクトルを求めたが、解析的に求めても良い。
【００５８】
また、実施の形態３では、３次スプライン曲線を用いたが、少なくとも位置、速度が連続となる曲線であれば他の曲線を用いても良い。例えばＢ−スプライン曲線、ベジェ曲線、ＮＵＲＢＳ曲線や三角関数等を用いてもよい。また多項式あるいは有理多項式で表現される曲線の場合は、その多項式の次数は少なくとも３次以上あればよい。特に、５次以上の曲線を用い、境界条件としてさらに二階微分係数を与えて加速度まで連続にすることにより、より滑らかな動作を実現することができる。
【００５９】
実施の形態４．
実施の形態１から実施の形態３では、姿勢誤差として角度の許容値を与えた。しかし図１５に示すような工具４の側面による加工の場合には、姿勢誤差が加工面の精度に直接的に影響する。工具４がワーク５に接触する最上部の位置をＲとし、工具先端ＰからＲまでの長さをＬとする。図１６に示すように、工具４の姿勢ベクトルｑが角度ε１だけの誤差を持つ場合、Ｌと角度ε１の積であるδ１が最上部Ｒの加工誤差に対応する。
【００６０】
実施の形態４では、実施の形態１の図３のフローチャートで示した関節補間範囲算出処理において、ε１のかわりに変位誤差δ１＝Ｌ・ε１を用い、その許容誤差δ１ｍａｘと比較するように構成する。このとき、ステップＳＴ３の条件式は以下のように表せる。
δ１＞δ１ｍａｘ（式３７）
この方法では、加工面の精度を直接δ１ｍａｘ以下に抑制することができ、精度が安定する。逆にいえば、精度が一定の条件で移動時間を短くすることが可能である。特に、加工や指令経路によってＬが異なる場合に有効である。
【００６１】
このように、実施の形態４によれば、エンドミルの側面による加工などのように工具先端から所定の距離だけ離れた位置における変位誤差δ１が性能に関係する場合、関節補間範囲決定部１１が、その位置における変位誤差を直接制限することにより範囲を決定するようにしたので、より精密な補間を行うことが可能になる。
【００６２】
さらに、実施の形態４の変形例として、加工面に垂直な方向の誤差成分に着目するとよい。図１７に示すように、加工面の単位法線ベクトルをＶｎとする。まず、誤差ベクトルＶδ１は以下のようにあらわせる。
Ｖδ１＝Ｌ（ｑ’−ｑ）（式３８）
ここで、加工面の誤差δ２は、誤差ベクトルの法線成分なので、誤差ベクトルと単位法線ベクトルの内積をとることにより、
δ２= Ｖδ１・Ｖｎ（式３９）
となる。このδ２を用いて評価する場合、図３に示したフローチャートにおいて、ε１のかわりにδ２を用い、その許容誤差δ２ｍａｘと比較するように構成する。すなわち、ステップＳＴ３の条件式を
|δ２|＞δ２ｍａｘ
とする。
【００６３】
式３９におけるδ２は符号付きで、＋は加工面から離れること（削り残し）、−は加工面に近づくこと（削り込み）を意味する。従って、＋側、−側の許容誤差をそれぞれδ２ｍａｘｐ、δ２ｍａｘｍとし、ステップＳＴ３の条件式を以下のようにすればさらに加工精度をきめ細かく指定することができる。
（δ２＜δ２ｍａｘｍ）または（δ２＞δ２ｍａｘｐ）
これにより、さらに加工精度をきめ細かく指定することができる。例えば、δ２ｍａｘｍを０に近い値とすれば、加工面への削り込みを避けることができる。
【００６４】
このように、エンドミルの側面による加工においては、工具先端から所定の距離だけ離れた位置における誤差ベクトルＶδ１と加工面の法線ベクトルＶｎの内積の値が加工面の誤差そのものにあたる。このときその内積の値に基づいて関節補間範囲を決定することで、加工誤差を一定値以下に制限することが可能になる。
特に内積値は符号を有し、加工面を削り残す方向の誤差と、削りすぎる方向の誤差を区別することができる。これにより、削り残しの誤差と、削り過ぎの誤差を区別して制限することができるため、加工精度をきめ細かく管理することができる。
【００６５】
なお、実施の形態４において、ＬやＶｎは加工方法あるいは指令経路に応じて加工プログラムやパラメータに設定しておく。またδ１ｍａｘ，δ２ｍａｘ，δ１ｍａｘｍ，δ１ｍａｘｐ，δ２ｍａｘｍ，δ２ｍａｘｐは、加工面の要求精度に応じて加工プログラムあるいはパラメータに設定しておく。
【００６６】
実施の形態５．
実施の形態５では、図１８に示すような円錐型のワーク５の側面を工具４の側面で加工する場合を考える。この場合、工具４は、工具先端が円弧上を１周する間に、ワーク５に対する工具姿勢を変化させる必要がある。
このような場合、従来は、図１９に示すように、指令経路を細かく分割し、工具姿勢を細かく指定することで対応していた。図の例では、経路を１４個に分割し、ｑ０からｑ１４までの計１５個のベクトルを指定している。経路を多数に分割すればするほど、工具姿勢の誤差は小さくなる。
しかし、経路を多数に分割すれば、加工プログラムに指令するデータが増え、大きな容量の加工プログラムの記憶領域が必要となってしまう。またこの加工プログラムに従って制御を行う数値制御装置においても、動作中に多数のデータを解釈するため、処理能力の高い高価なＣＰＵを用いる必要がある。
【００６７】
実施の形態５では、工具先端の経路が円弧等で与えられる場合のように、工具先端の経路が、ある平面内に存在する場合でも、多数の工具姿勢ベクトルを指定しなくてよい方法を実現する。
【００６８】
図２０は、実施の形態５による数値制御装置５０の構成を示すブロック図である。数値制御装置５０において、作業指令部５３を構成する作業プログラムは、姿勢決定部５７に対して、姿勢ベクトルｑ０、指令経路を表す曲線式Ｃ（ｔ）および指令経路が含まれる平面の法線ベクトルＷｎを供給する。姿勢決定部５７は、これらのパラメータおよび補間部５２から供給される媒介変数ｔから、姿勢ベクトルｑ（ｔ）を決定する。補間部５２は、姿勢決定部５７によって計算された姿勢ベクトルｑ（ｔ）を用いて補間演算を行い、各関節軸の座標値θｊを計算してサーボ制御装置１４に送る。サーボ制御装置１４は５軸加工機１を駆動し、所定の動作を行わせる。
【００６９】
次に動作について説明する。
図２１は、実施の形態５による数値制御装置５０の姿勢決定部５７による姿勢決定処理のフローチャートである。
【００７０】
ここでは、作業指令部５３から与えられた指令経路が図２２に示す平面に含まれるとする。この平面の単位法線ベクトルをＷｎ、指令経路の始点における工具の姿勢ベクトルをｑ０とし、始点における平面への接線ベクトルをＷ０とする。姿勢決定部５７は、工具姿勢が工具の進行方向に対して一定の角度に保つように計算する。
【００７１】
まず、媒介変数ｔに対応する接線ベクトルＷ（ｔ）を指令経路を表す曲線式の微係数を用いて計算する（ステップＳＴ５１）。すなわち、指令経路の曲線式をＣ（ｔ）とし、その導関数をＤ（ｔ）＝ｄＣ（ｔ）／ｄｔとすると、
Ｗ（ｔ）＝Ｄ（ｔ）／｜Ｄ（ｔ）｜（式４０）
で計算される。なお始点における接線ベクトルＷ０は、
Ｗ０＝Ｗ（０）＝Ｄ（０）／｜Ｄ（０）｜（式４１）
である。
【００７２】
次に、接線ベクトルＷ（ｔ）と、始点における接線ベクトルＷ０とのなす角度βを以下の式により求める（ステップＳＴ５２）。
β＝ｃｏｓ^−１（Ｗ０・Ｗ（ｔ））（式４２）
【００７３】
さらに、このβを用いて法線ベクトルＷｎを中心にｑ０を角度βだけ回転させることで、媒介変数ｔに対応する姿勢ベクトルｑ（ｔ）を以下の式により求める（ステップＳＴ５３）。
ｑ（ｔ）＝Ｒｏｔ（Ｗｎ，β）・ｑ０（式４３）
ここでＲｏｔ（Ｗｎ，β）は、ベクトルＷｎ回りに角度βだけ回転させるための３×３行列である。その具体的な数式は、実施の形態１に関連して述べたＰａｕｌ著、吉川恒夫訳、ロボット・マニピュレータ、コロナ社、１９８４に示されている。
【００７４】
以上のように、この実施の形態５によれば、工具先端の経路が、ある平面内にある場合に、姿勢決定部５７工具の進行方向に対して一定の姿勢を保つように工具姿勢ベクトルを自動的に求めるようにしたので、多数の工具姿勢ベクトルを指定する必要がなく、大容量の記憶装置や、計算能力の高いＣＰＵを用いる必要がない。
【００７５】
【発明の効果】
以上のように、この発明によれば、工具に与えられた２つの時点での姿勢に基づいて、両時点の間における工具の姿勢誤差が姿勢誤差許容値以下となるように、関節補間範囲を決定する関節補間範囲決定部と、関節補間範囲においては関節補間により工具の姿勢を制御する情報を算出し、その他の範囲においては他の方法により工具の姿勢を制御する情報を算出する補間部とを備えるようにしたので、特異点近傍など各関節の速度や加速度が過大となるような場合に、軌跡誤差に対する要求精度を満たしつつ、移動時間をできるだけ短縮することが可能な数値制御装置を得られるという効果がある。
【図面の簡単な説明】
【図１】この発明の実施の形態１による、数値制御装置により軌跡制御を行う５軸加工機の斜視図である。
【図２】この発明の実施の形態１による、数値制御装置の構成を示すブロック図である。
【図３】この発明の実施の形態１による、数値制御装置の関節補間範囲決定部による関節補間範囲算出処理のフローチャートである。
【図４】工具のワーク座標を説明するための図である。
【図５】この発明の実施の形態１による、工具が始点から終点まで移動する間の姿勢ベクトルｑの軌跡を示す図である。
【図６】この発明の実施の形態１による、工具が始点から終点まで移動する間のＡ軸およびＣ軸の角度の変化を示す図である。
【図７】この発明の実施の形態１による、工具が始点から終点まで移動する間のＡ軸およびＣ軸の角速度の変化を示す図である。
【図８】この発明の実施の形態１による、工具が始点から終点まで移動する間の姿勢誤差の変化を示す図である。
【図９】この発明の実施の形態２による、数値制御装置の構成を示すブロック図である。
【図１０】この発明の実施の形態２による、数値制御装置の速度決定部による速度決定処理のフローチャートである。
【図１１】この発明の実施の形態２による、工具が始点から終点まで移動する間のＡ軸およびＣ軸の角速度の変化を示す図である。
【図１２】この発明の実施の形態３による、数値制御装置の構成を示すブロック図である。
【図１３】この発明の実施の形態３による、数値制御装置の曲線決定部による曲線決定処理のフローチャートである。
【図１４】この発明の実施の形態３による、工具が始点から終点まで移動する間のＡ軸およびＣ軸の角速度の変化を示す図である。
【図１５】工具の側面による加工を説明する図である。
【図１６】この発明の実施の形態４による、関節補間範囲の決定方法を説明するための図である。
【図１７】この発明の実施の形態４の変形例による、関節補間範囲の決定方法を説明するための図である。
【図１８】工具の側面によるワークの円錐側面の加工を説明する図である。
【図１９】指令経路を細かく分割して工具姿勢を指定する方法を説明する図である。
【図２０】この発明の実施の形態５による、数値制御装置の構成を示すブロック図である。
【図２１】この発明の実施の形態５による、数値制御装置の姿勢決定部による姿勢決定処理のフローチャートである。
【図２２】この発明の実施の形態５による、姿勢決定方法を説明するための図である。
【符号の説明】
１５軸加工機、２傾斜テーブル、３旋回テーブル、４工具、５ワーク、１０，２０，３０，５０数値制御装置、１１関節補間範囲決定部、１２，２２，３２，５２補間部、１３，５３作業指令部、１４サーボ制御装置、２５速度決定部、３６曲線決定部、５７姿勢決定部。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a numerical controller that performs trajectory control of a machine having a degree of freedom of rotation.
[0002]
[Prior art]
Controls the position and orientation of tools such as cutting tools provided at the tip of the machine, robot hand or torch of a laser processing machine in a machine with a degree of freedom of rotation such as a 5-axis processing machine, a three-dimensional laser processing machine, and a robot. A well-known method is to automatically convert a given command into coordinate values (joint coordinates) of each joint axis of the machine and control it. This is a method called tool tip control in the 5-axis machine tool field.
[0003]
Here, when the position and orientation of the tool are commanded on an orthogonal coordinate system (work coordinate system) fixed to the work target, and the tool is operated at a certain speed on this coordinate system, the speed and acceleration of each joint are The joint tolerance may be exceeded. In particular, it is generally known that such a phenomenon is remarkable in a machine in which a singular point (singular posture) exists in the operating range of the machine.
Thus, when passing through a singular point or the vicinity thereof, the speed and acceleration become excessive, and there is a risk of causing a servo alarm to stop the machine or cause machine vibration.
[0004]
In order to solve this problem, for example, the control devices disclosed in Patent Document 1, Patent Document 2, and Patent Document 3 employ a method of decreasing the speed when the speed becomes excessive near the singular point.
[0005]
When this method is used, a trajectory error, that is, an error in the position and orientation of the tool does not occur, but there is a problem that the movement time is inevitably increased because the speed of the joint is lowered near the singular point.
In addition, there is a problem that there is no effect when switching solutions. In other words, there are generally multiple sets of joint coordinate values that satisfy a given tool position and orientation, but discontinuities that occur when switching from one set to another are not resolved. There was a problem.
[0006]
As another solution, for example, in the control devices disclosed in Patent Document 4 to Patent Document 8, in the vicinity of the singular point, not the tool position and posture commanded on the workpiece coordinate system, but the joint coordinate value The method of interpolating is adopted.
[0007]
This method is effective when switching the solution without reducing the speed, but has a problem that a large trajectory error occurs. In particular, the problem is that the amount of trajectory error cannot be directly managed.
Since special interpolation processing is performed in the vicinity of a singular point, a trajectory error is likely to occur, and the magnitude thereof depends on the range in which interpolation processing is performed. The conventional control apparatus determines this range based on the speed and acceleration of each joint axis, and a method for obtaining this range in consideration of the locus error has not been studied so far.
[0008]
[Patent Document 1]
JP 58-11148A
[Patent Document 2]
Japanese Patent Laid-Open No. 3-065576
[Patent Document 3]
JP-A-6-324730
[Patent Document 4]
JP 62-154006 A
[Patent Document 5]
JP 62-189504 A
[Patent Document 6]
JP 60-057408 A
[Patent Document 7]
JP-A 63-305407
[Patent Document 8]
Japanese Patent Laid-Open No. 2-218568
[0009]
[Problems to be solved by the invention]
As described above, the conventional control device cannot achieve both the suppression of the trajectory error and the reduction of the movement time.
[0010]
The present invention has been made to solve the above-described problems.In the trajectory control of a machine having a degree of freedom of rotation, a trajectory error occurs when the speed or acceleration of each joint is excessive, such as near a singular point. An object of the present invention is to obtain a numerical control device capable of reducing the travel time as much as possible while satisfying the required accuracy for the above.
[0011]
[Means for Solving the Problems]
The numerical control device according to the present invention is based on the postures at two time points given to the tool between the two time points. If the posture error that occurs when performing joint interpolation in the entire range is larger than the posture error tolerance, Tool posture error is less than the posture error tolerance Until Joint interpolation range Modified to be smaller A joint interpolation range determination unit that calculates information for controlling the posture of the tool by joint interpolation in the joint interpolation range, and in other ranges Spindle method And an interpolation unit for calculating information for controlling the posture of the tool.
[0012]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, various embodiments of the present invention will be described.
Embodiment 1 FIG.
FIG. 1 is a perspective view of a table tilt type 5-axis processing machine 1 that performs trajectory control by a numerical controller 10 (see FIG. 2) according to Embodiment 1 of the present invention. As shown in the figure, the 5-axis machine 1 has three linear axes X, Y, and Z that give translational displacement, an inclination table 2 that gives a rotation angle A around the X axis, and a rotation angle C around the Z axis. A turning table 3 is provided. The workpiece 5 is a work target of the 5-axis machining apparatus 1 and is machined by the tool 4.
The tool 4 is controlled by the numerical control device 10 through the servo control device 14 (see FIG. 2) to control the position and the posture with respect to the workpiece 5 and process the workpiece 5.
In this 5-axis machine 1, the tool 4 is not tilted together with the main axis to which it is directed, but is always oriented in the vertical direction. The work 5 is rotated by the tilt table 2 and the turning table 3, whereby the relative posture between the tool 4 and the work 5 is controlled.
[0013]
The position vector p and the posture vector q of the tool 4 are given as coordinate values (work coordinate values) on the work coordinate system Σw fixed to the work 5 that is the work target.
On the other hand, the coordinate values on the X, Y, Z, A, and C axes are referred to as joint coordinate values.
In the following description, the number of axes provided in the machine is n, and each joint axis (including a linear axis and a rotation axis) is θj (j = 1 to n).
In the case of the mechanism shown in FIG. 1, n = 5, and θ1, θ2, θ3, θ4, and θ5 correspond to X, Y, Z, A, and C, respectively.
[0014]
A coordinate conversion relationship determined by the machine structure is established between the workpiece coordinate value and the joint coordinate value of the tool 4, and conversion from the commanded workpiece coordinate value to the joint coordinate value is performed in the numerical controller.
[0015]
In the case of the mechanism shown in FIG. 1, the relationship between the workpiece coordinate values p and q of the tool 4 and the joint coordinate values of the X, Y, Z, A, and C axes is as follows.
p = Rc (C) · (Ra (A) ξ−H2) −H3 (Formula 1)
q = {I, J, K} ^t (Formula 2)
However,
ξ = {X, Y, Z} ^t (Formula 3)
I = sinA · sinC (Formula 4)
J = −sinA · cosC (Formula 5)
K = cosA (Formula 6)
Here, Ra (A) and Rc (C) are 3 × 3 matrices that rotate about angles X and Z, respectively, by angles A and C, respectively. H2 is an offset vector from the rotation center of the tilt table 2 to the rotation center of the turning table 3, and H3 is an offset vector from the rotation center of the turning table 3 to the work coordinate system origin. The right shoulder suffix t indicates transposition. The posture vector q is a unit vector.
[0016]
When A = 0, that is, when the upper surface of the turntable 3 in FIG. 1 is horizontal, the coordinate value C of C does not contribute to the change of q of the tool 4 according to Equation 2. The state of the tool 4 at this time corresponds to a unique posture. Therefore, if the posture vector indicating the specific posture is qs,
qs = {0 0 1} ^t (Formula 7)
It becomes.
[0017]
Conversely, when the joint coordinate value is obtained from the workpiece coordinate value, it is as follows.
A = cos ^-1 (K) (Formula 8)
C = atan2 (I / sinA, −J / sinA) (Formula 9)
(However, atan2 (X, Y) = tan ^-1 (X / Y))
ξ = Ra (−A) · (Rc (−C) · (p + H3) + H2) (Formula 10)
[0018]
FIG. 2 is a block diagram showing the configuration of the numerical control apparatus according to Embodiment 1 of the present invention. As shown in the figure, the numerical control device 10 includes a joint interpolation range determination unit 11, an interpolation unit 12, and a work command unit 13.
[0019]
Next, the operation of the numerical controller 10 will be described.
In the first embodiment, as the interpolation method of the posture of the tool 4, two methods of joint interpolation and rotation spindle method are used.
The work command unit 13 commands the position p and posture q of the tool 4 according to the work program. The joint interpolation range determination unit 11 determines the joint interpolation range based on the commanded position p and posture q of the tool 4 and the allowable error value. A method for determining the joint interpolation range will be described later.
The interpolation unit 12 performs the interpolation by joint interpolation within the determined joint interpolation range, and performs the interpolation by the rotation principal axis method (see below) in the other ranges, and the joint of each joint axis according to the position p and posture q of the tool 4. The coordinate value is calculated and transmitted to the servo controller 14. The servo control device 14 drives the 5-axis processing machine 1 based on the sent joint coordinate values.
Note that the position p and the posture q of the tool 4 may be instructed by manual input or through an external device without depending on the work program.
[0020]
Here, the two interpolation methods used in the first embodiment will be briefly described.
The joint interpolation is a method of interpolating the joint coordinate values of each axis based on the position vector and posture vector at the start point and end point given to the tool 4. In this method, the movement time can be shortened, but an attitude error occurs.
The rotation main axis method is a method of performing interpolation by rotating a posture around a normal vector orthogonal to a posture vector at a given start point and end point. In this method, it is necessary to reduce the speed near the singular point. For details, see Paul, translated by Yoshikawa Tsuneo, Robot Manipulator, Corona, 1984. This document forms part of the disclosure of the present application.
[0021]
FIG. 3 is a flowchart of the joint interpolation range calculation process by the joint interpolation range determination unit 11 of the numerical controller 10 according to the first embodiment.
[0022]
First, the joint interpolation range determination unit 11 sets initial values of a start point tx0 and an end point tx1 (t is a parameter) of a range in which joint interpolation is performed (step ST1). In this initial value setting, tx0 = 0 and tx1 = 1. This means that joint interpolation is performed over the entire range of the commanded path.
[0023]
Next, the attitude error of the tool 4 is calculated (step ST2).
The calculation method of attitude error is shown below. From the starting point posture vector q (tx0), the joint coordinate values Ax0 and Cx0 of A and C at the starting point are obtained according to Equations 8 and 9. Similarly, joint coordinate values Ax1 and Cx1 of A and C at the end point are obtained from the end point posture vector q (tx1).
Also, the joint coordinate values of the A and C axes at the center txh of the route
Axh = (Ax0 + Ax1) / 2 (Formula 11)
Cxh = (Cx0 + Cx1) / 2 (Formula 12)
It is. Txh is
txh = (tx0 + tx1) / 2
It is. Therefore, when tx0 = 0 and tx1 = 1, txh = 0.5.
By substituting Axh and Cxh into Equation 2, Equation 4, Equation 5, and Equation 6, as shown in Equation 13, a posture vector q (txh) at the center of the path is obtained.
q (txh) = {sinAxh · sinCxh −sinAxh · cosCxh cosAxh} ^t (Formula 13)
Further, the original posture vector qh at the center of the movement route is obtained by the following equation. Here, the original posture vector is a vector that lies in the plane formed by the posture vector of the start point and the posture vector of the end point, and is located on the bisector of the angle formed by the posture vector of the start point and the posture vector of the end point. .
qh = qs− (qs · qn) qn (Formula 14)
However, qn is a vector orthogonal to q (tx0) and q (tx1).
The posture error ε1 between the posture vector q (txh) and the original posture vector qh is as follows.
ε1 = cos ^-1 (Qh · q (txh)) (Formula 15)
[0024]
Next, the posture error ε1 is compared with the posture error allowable value εmax (step ST3). If ε1 is larger than εmax as a result of the comparison, the joint interpolation range (tx0, tx1) is changed (step ST4). On the other hand, if ε1 is equal to or less than εmax, the process is terminated, and the interpolator 12 controls the 5-axis machine 1 by joint interpolation in the joint interpolation range (tx0, tx1) obtained immediately before.
[0025]
In step ST4, tx0 and tx1 are changed as follows.
tx0 ′ = txh−dtx / 2 (Formula 16)
tx1 ′ = txh + dtx / 2 (Formula 17)
However,
dtx = (tx1-tx0) · εmax / ε1 (Formula 18)
Here, it is assumed that the width dtx of the range in which joint interpolation is performed and the posture error ε are approximately proportional. dtx is obtained according to the ratio of the posture error ε, and (tx0 ′, tx1 ′) is obtained using the dtx thus obtained. Here, it is assumed that the singular posture qs is closest to the center of the command path. In step ST4, after calculating tx0 ′ and tx1 ′, (tx0, tx1) is replaced with (tx0 ′, tx1 ′).
[0026]
Actually, the joint coordinates A, C and the tool postures I, J, K are in a non-linear relationship as shown in the equations 8 and 9. Therefore, it is preferable to perform repeated calculation in order to obtain a range (tx0, tx1) for performing joint interpolation more accurately. Therefore, in the first embodiment, the posture error ε1 is obtained again in step ST2 using the section (tx0, tx1) changed in step ST4. By repeating this procedure, an accurate value of (tx0, tx1) can be obtained.
[0027]
As a simpler method, if it is determined in step ST3 that ε1 is greater than εmax, the range may be changed in step ST4, and then the process may proceed directly to step ST5 without returning to step ST2. In this case, since (tx0, tx1) cannot be obtained accurately, the movement time becomes slightly longer. However, since the calculation process is simplified, it can be realized by a cheaper computer.
[0028]
In step ST3, the posture error at the center of the range in which joint interpolation is performed is calculated. However, posture errors at locations other than the center may be evaluated. Thereby, the calculation accuracy of the posture error is improved. Depending on the mechanism and command path, analytically accurate attitude errors may be required. In this case, by calculating the range for joint interpolation analytically, it is possible to reduce the amount of calculation and improve the accuracy for obtaining the range for joint interpolation.
[0029]
In step ST3, the simplest convergence determination condition is such that the error is less than or equal to the allowable value. However, an absolute value of the difference between ε1 and εmax or an upper limit / lower limit of the ratio is provided, and ε1≈εmax. By obtaining a solution, the travel time can be further shortened. In consideration of practicality, it is desirable to provide an upper limit for the number of repetitions from step ST2 to step ST4 and a lower limit for the absolute value of the amount of change in (tx0, tx1).
[0030]
Next, description will be made using specific numerical values. As shown in FIG. 4, the work coordinates at the start point of the commanded movement path of the tool 4 are (p0, q0), the position and posture of the work coordinates at the end point are (p1, q1), and specifically as follows. The value is
q0 = {0.7071, -0.0123, 0.7070} ^t (Formula 19)
q1 = {− 0.7071, −0.0123, 0.7070} ^t (Formula 20)
At this time, the angle φ formed by q0 and q1 is 90 degrees. The coordinate values of the A axis at the start point and end point are both 45.0087 degrees. Further, the coordinate values of the C axis at the start point and the end point are 89.0002 and −89.0002 degrees, respectively.
In this example, the plane formed by q0 and q1 is closest to the singular posture qs at the center of the movement path of the tool 4. The posture vector qh of the tool 4 at that time is
qh = {0.0000, -0.0175, 0.9998} ^t (Formula 21)
It is. At this time, the angle formed by qh and qs is about 1 degree. Here, the allowable value εmax of the posture error (angle where q is shifted from the plane formed by q0 and q1) is 20 degrees.
[0031]
FIGS. 5 to 8 show the results when the trajectory control of the tool 4 is performed by the numerical control device according to the first embodiment (Δ mark) and by the conventional method with respect to the command path. ing.
FIG. 5 is a diagram illustrating the trajectory of the posture vector q while the tool 4 moves from the start point to the end point. 6 shows changes in the angles of the A axis and the C axis during the movement of the tool 4 from the start point to the end point, and FIG. 7 shows changes in the angular velocities of the A axis and the C axis during the movement from the start point to the end point. Indicates. Further, FIG. 8 shows a change in posture error while moving from the start point to the end point.
[0032]
In FIGS. 5 to 8, the trajectory marked with X indicates the result when the joint interpolation described in relation to the prior art is applied. In the joint interpolation, as shown in FIG. 6, the coordinate value of the A axis is the same from the start point to the end point, and only the C axis moves. As a result, the locus of the posture vector q becomes a locus greatly deviated from the plane formed by q0 and q1, which is supposed to be as shown in FIG. That is, as shown in FIG. 8, a posture error as large as about 44 degrees occurs.
[0033]
Also, the locus marked with ○ is interpolation by the rotation spindle method. In the rotation spindle method, no posture error occurs as shown in FIGS. However, in order to realize this, as shown in FIG. 6, it is necessary to suddenly operate the C-axis near a singular point (near 0.6 seconds). As a result, a very large C-axis speed change is caused in the vicinity of the singular point, and the value reaches about −3000 degrees / second (see FIG. 7). In practice, it is almost impossible to operate at such a high speed and high acceleration.
[0034]
On the other hand, as shown in FIG. 8, the result of applying the present invention indicated by the locus of Δ is an allowable range when the posture error is about 20 degrees, and each joint speed (speed of the A and C axes) is It does not become extremely large.
[0035]
In this example, the posture error ε1 due to the changed (tx0, tx1) is slightly smaller than the allowable value (20 degrees), and the processing is completed.
[0036]
In general, an error in the posture of the tool is often allowed to some extent, but an error in the position of the tool is often desired to be as small as possible. In this case, the joint coordinate values ξ of the linear axes (X, Y, Z axes in the first embodiment) are the joint coordinates of the tool position vector p and the rotation axes (A, C axes in the first embodiment). The value is obtained according to Equation 10. Since the tool position error does not occur as long as the joint coordinate value of the linear axis is obtained according to Equation 10, this method is preferable when it is desired to avoid the tool position error. In this case, since the position vector p information is not required in the joint interpolation range determination unit 11, the transfer of the position vector p information from the work command unit 13 to the joint interpolation range determination unit 11 can be omitted. .
On the other hand, when an error in the position of the tool is allowed within a predetermined range, joint interpolation may be performed for the linear axis without following Formula 10 within the joint interpolation range described above. Thereby, although a position error arises, it can avoid that the speed and acceleration of a linear axis change rapidly. In such a case, more preferably, in step ST2 of the flowchart shown in FIG. 3, not only the posture error ε1 but also the tool position error is obtained, and in step ST3, either the posture error or the position error is determined from the respective allowable values. If it is determined whether or not is too large, the position error of the tool can be limited to an allowable value or less.
[0037]
As described above, according to the first embodiment, the joint interpolation range determination unit 11 determines the joint interpolation range so that the posture error is equal to or less than the posture error allowable value, and uses the rotation spindle method in other ranges. Since this is done, there is an effect that the posture error can be limited to a specified posture error allowable value or less without extremely increasing the joint speed. In addition, an effect can be obtained when switching solutions.
[0038]
Embodiment 2. FIG.
Although the first embodiment is suitable for controlling the posture error of the tool 4, it is further desirable to perform control so as to satisfy the allowable speed of each joint axis. In the second embodiment, the moving speed is controlled so as to satisfy the allowable speed of each joint.
[0039]
FIG. 9 is a block diagram showing the configuration of the numerical controller 20 according to the second embodiment of the present invention. The same reference numerals as those in FIG. 2 represent the same components. The speed determination unit 25 determines the speed Vj (j = 1 to n) of each axis from the commanded position p and posture q, the joint interpolation range determined by the joint interpolation range determination unit 11, and the allowable speed of each joint axis. Ask for. The interpolation unit 22 determines the speed using this speed Vj and performs interpolation.
[0040]
Next, the operation will be described.
FIG. 10 is a flowchart of the speed determination process by the speed determination unit 25 of the numerical controller 20 according to the second embodiment. This process is performed after the joint interpolation range calculation process by the joint interpolation range determination unit 11. The joint interpolation range calculation process according to the second embodiment is the same process as that of the first embodiment.
[0041]
First, a change amount dθj (j = 1 to n) of each joint coordinate in a range where joint interpolation is performed is obtained (step ST21). That is, when the range for performing joint interpolation is given as tx0 <t <tx1 as the range of the parameter t, Ax0 and Cx0 are obtained from q (tx0) according to Equations 8 and 9. Similarly, Ax1 and Cx1 are obtained from q (tx1). The amount of change of each joint coordinate in the range where joint interpolation is performed is obtained as follows.
dA = | Ax1-Ax0 | (Formula 22)
dC = | Cx1-Cx0 | (Formula 23)
[0042]
Next, from dθj and the permissible speed of each joint, the maximum of the movement time of each axis is obtained by the following formula (step ST22).
Tmax = max (dA / Vma, dC / Vmc) (Equation 24)
Here, Vma and Vmc are allowable speeds of the A axis and the C axis, respectively.
[0043]
Further, the velocities Va and Vc of the respective axes are obtained from the maximum time Tmax and the amount of change (dA, dC) of each joint coordinate (step ST23).
V4 = Va = dA / Tmax (Formula 25)
V5 = Vc = dC / Tmax (Formula 26)
[0044]
A specific example will be described. The allowable speeds of the A axis and C axis are 20 degrees / second and 100 degrees / second, respectively.
[0045]
FIG. 11 shows changes in the angular velocities of the A axis and the C axis while the tool 4 moves from the start point to the end point. In the figure, a circle indicates a trajectory when speed limitation is added to the interpolation based on the conventional rotation spindle method. That is, the speed is corrected every moment so that either the A-axis or the C-axis matches the allowable speed for each interpolation cycle. A triangle indicates a locus according to the second embodiment. Both methods satisfy the limit of allowable speed. However, as shown in the figure, in the C-axis velocity waveform, the time for moving at −100 degrees / s is longer in the second embodiment than in the conventional method. The excessively long part is a part that has been increased in speed by allowing a certain degree of attitude error, although the speed cannot be increased conventionally in order to maintain the attitude accuracy.
As a result, the travel times are compared as follows.
Conventional method (rotary spindle method) Travel time = 5.7 seconds
Embodiment 2 Travel time = about 4.4 seconds
[0046]
As described above, according to the second embodiment, the speed can be increased by allowing a certain degree of tolerance error in the allowable range, so that the movement time can be shortened as compared with the interpolation based on the conventional rotation spindle method.
[0047]
As the posture error allowable value is increased, the range for joint interpolation is expanded and the movement time is shortened. Furthermore, if the joint interpolation range is expanded and joint interpolation is used in the entire range of the command path, the movement time becomes the shortest. At this time, it is clear that the posture error also increases. As this time, the shortest movement time under the condition that the posture error is 20 degrees or less of the allowable value is a solution obtained by using the numerical control device of the second embodiment.
[0048]
In the procedure shown in FIG. 10, focusing on the rotation axis of each joint, the maximum time is obtained from the amount of change in the coordinate value of each axis and the allowable speed. More ideal is the linear axis (X, Y, Z ) Including all the joint axes. As a result, the allowable speed is restricted on all axes, and a smoother operation can be realized.
On the other hand, it is possible to reduce the amount of calculation by paying attention to only a specific axis (C axis in the above example) where the speed is most likely to be excessive in practical use. In this case, it is necessary to avoid the speed restriction of the other axes by another means (for example, a device of a mechanism, a device of programming a command path or a command speed, etc.).
[0049]
Embodiment 3 FIG.
In the method described in the first and second embodiments, as is apparent from FIGS. 7 and 11, the speed is discontinuous at the boundary between the range in which joint interpolation is performed and the range in which interpolation is performed by the rotation spindle method. Become. This speed discontinuity can cause mechanical vibrations and servo overload. In the third embodiment, this discontinuity is improved.
[0050]
FIG. 12 is a block diagram illustrating a configuration of the numerical control device 30 according to the third embodiment. The same reference numerals as those in FIG. 2 represent the same components. The curve determination unit 36 calculates the parameters of the curve used in the joint interpolation range from the commanded position p and posture q and the joint interpolation range, and sends them to the interpolation unit 32. The interpolation unit 32 performs curve interpolation according to a curve determined by the designated parameter.
[0051]
Next, the operation will be described.
FIG. 13 is a flowchart of a curve determination process by the curve determination unit 36 of the numerical controller 30 according to the third embodiment. This process is performed after the joint interpolation range calculation process by the joint interpolation range determination unit 11. The joint interpolation range calculation process according to the third embodiment is the same process as that of the first embodiment.
[0052]
First, coordinate values and tangent vectors at both ends of the range for joint interpolation are obtained (step ST31).
Specifically, Ax0 and Cx0 are first obtained from q (tx0) according to Equations 8 and 9. Similarly, Ax1 and Cx1 are obtained from q (tx1). The tangent vector is calculated using the difference from the angle at a location separated by a minute parameter Δt. That is, the coordinate values of the A axis and the C axis at the position of tx0−Δt are obtained in the same manner, and set as Ax0pre and Cx0pre. In addition, the A-axis and C-axis coordinate values at the position of tx1 + Δt are obtained in the same manner as Ax1post and Cx1post. Using these, the start point tangent vector (uax0, ucx0) and the end point tangent vector (uax1, ucx1) are obtained as follows.
uax0 = (Ax0−Ax0pre) / Δt (Formula 27)
ucx0 = (Cx0−Cx0pre) / Δt (Equation 28)
uax1 = (Ax1post−Ax1) / Δt (Equation 29)
ucx1 = (Cx1post−Cx1) / Δt (Equation 30)
[0053]
Next, the coordinate values of the A and C axes corresponding to the parameter t are obtained by spline interpolation using a cubic spline curve as shown below (step ST32).
A = Ax0 + B ・ M ・ {0 (Ax1-Ax0) Lx ・ uax0 Lx ・ uax1} ^t (Formula 31)
C = Cx0 + B ・ M ・ {0 (Cx1-Cx0) Lx ・ ucx0 Lx ・ uax1} ^t (Formula 32)
However,
τ = (t−tx0) / Lx (Formula 33)
Lx = tx1-tx0 (Formula 34)
B = {τ ³ τ ² τ 1} (Formula 35)
[Expression 1]

[0054]
A specific example will be described. FIG. 14 shows changes in the angular velocities of the A axis and the C axis while the tool 4 moves from the start point to the end point. In the figure, a circle indicates a trajectory when speed limitation is added to the interpolation based on the conventional rotation spindle method. A triangle indicates a locus according to the third embodiment.
[0055]
According to the third embodiment, it can be seen that the discontinuity of the speed seen in FIG. 11 is eliminated for both the A axis and the C axis, resulting in a smooth waveform. However, since the speed is smoothed, the moving time is longer than that of the second embodiment, and is about 5.0 seconds. Nevertheless, the movement time is shorter than when the conventional rotary spindle method is used.
[0056]
As described above, according to the third embodiment, it is possible to eliminate the speed discontinuity that occurs at the boundary between the range in which joint interpolation is performed and the range in which interpolation is performed by the rotation spindle method, and a smoother operation can be realized. . For example, this is effective when a particularly smooth operation is required such as mold processing.
[0057]
In the third embodiment, the tangent vector is obtained from the difference, but may be obtained analytically.
[0058]
In the third embodiment, a cubic spline curve is used. However, other curves may be used as long as at least a position and a speed are continuous. For example, a B-spline curve, a Bezier curve, a NURBS curve, a trigonometric function, or the like may be used. In the case of a curve expressed by a polynomial or a rational polynomial, the degree of the polynomial only needs to be at least 3 or more. In particular, a smoother operation can be realized by using a fifth-order or higher-order curve and giving a second-order differential coefficient as a boundary condition so that the acceleration is continuous.
[0059]
Embodiment 4 FIG.
In the first to third embodiments, the angle tolerance is given as the posture error. However, in the case of machining by the side surface of the tool 4 as shown in FIG. 15, the posture error directly affects the accuracy of the machined surface. The uppermost position where the tool 4 contacts the workpiece 5 is R, and the length from the tool tip P to R is L. As shown in FIG. 16, when the posture vector q of the tool 4 has an error of an angle ε1, δ1, which is the product of L and the angle ε1, corresponds to the machining error of the uppermost portion R.
[0060]
In the fourth embodiment, in the joint interpolation range calculation process shown in the flowchart of FIG. 3 of the first embodiment, displacement error δ1 = L · ε1 is used instead of ε1, and the allowable error δ1max is compared. . At this time, the conditional expression of step ST3 can be expressed as follows.
δ1> δ1max (Formula 37)
In this method, the accuracy of the processed surface can be directly suppressed to δ1max or less, and the accuracy is stabilized. In other words, it is possible to shorten the movement time under the condition that the accuracy is constant. This is particularly effective when L varies depending on the machining and command path.
[0061]
As described above, according to the fourth embodiment, when the displacement error δ1 at a position away from the tool tip by a predetermined distance is related to performance, such as machining by the side surface of the end mill, the joint interpolation range determination unit 11 Since the range is determined by directly limiting the displacement error at that position, more precise interpolation can be performed.
[0062]
Further, as a modification of the fourth embodiment, attention should be paid to an error component in a direction perpendicular to the processing surface. As shown in FIG. 17, the unit normal vector of the machined surface is Vn. First, the error vector Vδ1 is expressed as follows.
Vδ1 = L (q′−q) (Formula 38)
Here, since the machining surface error δ2 is a normal component of the error vector, by taking the inner product of the error vector and the unit normal vector,
δ2 = Vδ1 · Vn (Equation 39)
It becomes. When the evaluation is performed using δ2, δ2 is used instead of ε1 in the flowchart shown in FIG. 3, and the allowable error δ2max is compared. That is, the conditional expression of step ST3 is
| δ2 |> δ2max
And
[0063]
In Expression 39, δ2 is signed, + means moving away from the machining surface (uncut), and-means approaching the machining surface (cutting). Therefore, if the tolerances on the + side and − side are set to δ2maxp and δ2maxm, respectively, and the conditional expression in step ST3 is as follows, the processing accuracy can be specified more finely.
(Δ2 <δ2maxm) or (δ2> δ2maxp)
Thereby, the processing accuracy can be specified more finely. For example, if δ2maxm is set to a value close to 0, it is possible to avoid cutting into the processed surface.
[0064]
As described above, in machining by the side surface of the end mill, the inner product value of the error vector Vδ1 and the normal vector Vn of the machining surface at a position away from the tool tip by a predetermined distance corresponds to the machining surface error itself. At this time, by determining the joint interpolation range based on the value of the inner product, it becomes possible to limit the machining error to a certain value or less.
In particular, the inner product value has a sign, and an error in a direction in which the machined surface is left behind can be distinguished from an error in a direction in which the machined surface is overcut. As a result, it is possible to discriminate and limit the error of the uncut material and the error of the excessively cut material, so that the processing accuracy can be finely managed.
[0065]
In the fourth embodiment, L and Vn are set in machining programs and parameters according to the machining method or the command path. Further, δ1max, δ2max, δ1maxm, δ1maxp, δ2maxm, and δ2maxp are set as machining programs or parameters according to the required accuracy of the machining surface.
[0066]
Embodiment 5. FIG.
In the fifth embodiment, a case where the side surface of the conical workpiece 5 as shown in FIG. In this case, the tool 4 needs to change the tool posture with respect to the workpiece 5 while the tool tip makes one round on the arc.
In such a case, conventionally, as shown in FIG. 19, the command path is divided finely and the tool posture is specified finely. In the example of the figure, the route is divided into 14 and a total of 15 vectors from q0 to q14 are designated. The more the path is divided, the smaller the tool posture error.
However, if the route is divided into a large number, the data to be instructed to the machining program increases, and a large capacity machining program storage area is required. Also in a numerical control device that performs control according to this machining program, it is necessary to use an expensive CPU with high processing capability in order to interpret a large amount of data during operation.
[0067]
In the fifth embodiment, a method is realized in which a tool tip path need not be specified even when the tool tip path exists within a certain plane, as in the case where the tool tip path is given by an arc or the like. To do.
[0068]
FIG. 20 is a block diagram showing the configuration of the numerical controller 50 according to the fifth embodiment. In the numerical control device 50, the work program constituting the work command unit 53 sends a posture vector q0, a curve expression C (t) representing a command route, and a normal vector of a plane including the command route to the posture determination unit 57. Wn is supplied. The posture determination unit 57 determines the posture vector q (t) from these parameters and the parameter t supplied from the interpolation unit 52. The interpolation unit 52 performs an interpolation operation using the posture vector q (t) calculated by the posture determination unit 57, calculates the coordinate value θj of each joint axis, and sends it to the servo control device 14. The servo control device 14 drives the 5-axis processing machine 1 to perform a predetermined operation.
[0069]
Next, the operation will be described.
FIG. 21 is a flowchart of posture determination processing by the posture determination unit 57 of the numerical controller 50 according to the fifth embodiment.
[0070]
Here, it is assumed that the command path given from the work command unit 53 is included in the plane shown in FIG. The unit normal vector of this plane is Wn, the tool posture vector at the start point of the command path is q0, and the tangent vector to the plane at the start point is W0. The posture determination unit 57 calculates so that the tool posture is maintained at a constant angle with respect to the traveling direction of the tool.
[0071]
First, the tangent vector W (t) corresponding to the parametric variable t is calculated using the derivative of the curve equation representing the command path (step ST51). That is, if the curve equation of the command path is C (t) and its derivative is D (t) = dC (t) / dt,
W (t) = D (t) / | D (t) | (Formula 40)
Calculated by The tangent vector W0 at the start point is
W0 = W (0) = D (0) / | D (0) | (Formula 41)
It is.
[0072]
Next, an angle β formed by the tangent vector W (t) and the tangent vector W0 at the start point is obtained by the following equation (step ST52).
β = cos ^-1 (W0 · W (t)) (Formula 42)
[0073]
Further, by using this β to rotate q0 about the normal vector Wn by an angle β, a posture vector q (t) corresponding to the parameter t is obtained by the following equation (step ST53).
q (t) = Rot (Wn, β) · q0 (Formula 43)
Here, Rot (Wn, β) is a 3 × 3 matrix for rotating the vector Wn by an angle β. The specific mathematical formula is shown in Paul, translated by Tsuneo Yoshikawa, Robot Manipulator, Corona, Inc., 1984 described in connection with the first embodiment.
[0074]
As described above, according to the fifth embodiment, when the path of the tool tip is within a certain plane, the tool posture vector is set so as to maintain a constant posture with respect to the traveling direction of the tool 57. Since it is automatically obtained, it is not necessary to designate a large number of tool posture vectors, and it is not necessary to use a large-capacity storage device or a CPU having a high calculation capability.
[0075]
【The invention's effect】
As described above, according to the present invention, based on the postures at two points given to the tool, the joint interpolation range is set so that the posture error of the tool between the two points is less than the posture error allowable value. A joint interpolation range determination unit to determine, an interpolation unit to calculate information for controlling the posture of the tool by joint interpolation in the joint interpolation range, and to calculate information to control the posture of the tool by other methods in other ranges; A numerical control device capable of reducing the travel time as much as possible while satisfying the required accuracy for the trajectory error when the speed and acceleration of each joint is excessive, such as in the vicinity of a singular point. There is an effect that it is.
[Brief description of the drawings]
FIG. 1 is a perspective view of a 5-axis processing machine that performs trajectory control by a numerical control device according to Embodiment 1 of the present invention;
FIG. 2 is a block diagram showing a configuration of a numerical controller according to Embodiment 1 of the present invention.
FIG. 3 is a flowchart of joint interpolation range calculation processing by a joint interpolation range determination unit of the numerical controller according to Embodiment 1 of the present invention;
FIG. 4 is a diagram for explaining a workpiece coordinate of a tool.
FIG. 5 is a diagram showing a trajectory of a posture vector q while a tool moves from a start point to an end point according to Embodiment 1 of the present invention.
FIG. 6 is a diagram showing changes in angles of the A axis and the C axis while the tool moves from the start point to the end point according to the first embodiment of the present invention.
FIG. 7 is a diagram showing changes in angular velocities of the A axis and the C axis while the tool moves from the start point to the end point according to Embodiment 1 of the present invention.
FIG. 8 is a diagram showing a change in posture error during the movement of the tool from the start point to the end point according to Embodiment 1 of the present invention.
FIG. 9 is a block diagram showing a configuration of a numerical control device according to a second embodiment of the present invention.
FIG. 10 is a flowchart of speed determination processing by a speed determination unit of the numerical controller according to the second embodiment of the present invention.
FIG. 11 is a diagram showing changes in angular velocities of the A axis and the C axis while the tool moves from the start point to the end point according to the second embodiment of the present invention.
FIG. 12 is a block diagram showing a configuration of a numerical controller according to Embodiment 3 of the present invention.
FIG. 13 is a flowchart of a curve determination process by a curve determination unit of the numerical controller according to the third embodiment of the present invention.
FIG. 14 is a diagram showing changes in angular velocities of the A axis and the C axis while the tool moves from the start point to the end point according to Embodiment 3 of the present invention.
FIG. 15 is a diagram illustrating machining by a side surface of a tool.
FIG. 16 is a diagram for explaining a method for determining a joint interpolation range according to a fourth embodiment of the present invention;
FIG. 17 is a diagram for explaining a joint interpolation range determination method according to a modification of the fourth embodiment of the present invention.
FIG. 18 is a diagram illustrating machining of a conical side surface of a workpiece by a side surface of a tool.
FIG. 19 is a diagram for explaining a method for specifying a tool posture by finely dividing a command path;
FIG. 20 is a block diagram showing a configuration of a numerical controller according to a fifth embodiment of the present invention.
FIG. 21 is a flowchart of posture determination processing by a posture determination unit of the numerical control device according to the fifth embodiment of the present invention;
FIG. 22 is a diagram for explaining a posture determination method according to a fifth embodiment of the present invention.
[Explanation of symbols]
1 5-axis processing machine, 2 tilt table, 3 turning table, 4 tool, 5 workpiece, 10, 20, 30, 50 numerical control device, 11 joint interpolation range determination unit, 12, 22, 32, 52 interpolation unit, 13, 53 Work command unit, 14 servo control device, 25 speed determining unit, 36 curve determining unit, 57 posture determining unit.

Claims

回転自由度を有し、作業対象に対する工具の姿勢を変更することが可能な機械の制御を行う数値制御装置において、
上記工具に与えられた２つの時点での姿勢に基づいて、両時点の間の全範囲において関節補間を行うときに生じる姿勢誤差が姿勢誤差許容値より大きい場合に、上記工具の姿勢誤差が上記姿勢誤差許容値以下となるまで関節補間範囲を小さくするように修正する関節補間範囲決定部と、
上記関節補間範囲においては関節補間により上記工具の姿勢を制御する情報を算出し、その他の範囲においては回転主軸法により上記工具の姿勢を制御する情報を算出する補間部とを備えたことを特徴とする数値制御装置。In a numerical control device for controlling a machine having a rotational degree of freedom and capable of changing the posture of a tool with respect to a work target,
Based on the attitude at two time points given to the tool, when the attitude error that occurs when performing joint interpolation in the entire range is larger than the attitude error tolerance between both time points, the attitude error of the tool is above A joint interpolation range determination unit that corrects the joint interpolation range to be smaller until the posture error allowable value or less,
An interpolation unit for calculating information for controlling the posture of the tool by joint interpolation in the joint interpolation range, and for calculating information for controlling the posture of the tool by a rotation spindle method in the other ranges. Numerical control device.

決定された関節補間範囲に基づいて、機械の各関節の速度が、上記関節補間範囲では許容速度以下となるように決定する速度決定部を備え、
補間部は、上記速度決定部によって決定された各関節の速度に従って補間を行うことを特徴とする請求項１記載の数値制御装置。Based on the determined joint interpolation range, a speed determining unit that determines the speed of each joint of the machine to be equal to or lower than the allowable speed in the joint interpolation range,
The numerical controller according to claim 1, wherein the interpolation unit performs interpolation according to the speed of each joint determined by the speed determination unit.

決定された関節補間範囲に基づいて、関節補間範囲とその他の補間範囲との間での各関節の速度の変化を円滑にする補間曲線を決定する曲線決定部を備え、
補間部は、上記曲線決定部によって決定された曲線に従って曲線補間を行うことを特徴とする請求項１または請求項２記載の数値制御装置。Based on the determined joint interpolation range, a curve determination unit that determines an interpolation curve that smoothly changes the speed of each joint between the joint interpolation range and the other interpolation range,
The numerical controller according to claim 1, wherein the interpolation unit performs curve interpolation according to the curve determined by the curve determination unit.

関節補間範囲決定部は、工具の姿勢誤差ベクトルと加工面の法線ベクトルとの内積に基づいて、関節補間範囲を決定することを特徴とする請求項１から請求項３のうちのいずれか１項記載の数値制御装置。 The joint interpolation range determination unit determines a joint interpolation range based on an inner product of a tool posture error vector and a normal vector of a machining surface. Numerical control device according to item.