JP4101665B2

JP4101665B2 - 数値制御装置

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}^ｔ（式３２）
ただし、
τ＝（ｔ−ｔｘ０）／Ｌｘ（式３３）
Ｌｘ＝ｔｘ１−ｔｘ０（式３４）
Ｂ＝｛τ^３ τ^２ τ １｝（式３５）
【数１】

【００５４】
具体的な例を挙げて説明する。図１４は、工具４が始点から終点まで移動する間のＡ軸、Ｃ軸の角速度の変化を示している。図中、○印は従来の回転主軸法による補間に速度制限を加味した場合の軌跡である。△印は、実施の形態３による軌跡を示している。
【００５５】
実施の形態３によれば、Ａ軸、Ｃ軸とも図１１に見られた速度の不連続性が解消され、滑らかな波形となっていることがわかる。ただし速度を滑らかにした分、移動時間は実施の形態２より長くなり、約５．０秒となった。それでも従来の回転主軸法を用いた場合よりは移動時間が短い。
【００５６】
このように、実施の形態３によれば、関節補間を行う範囲と回転主軸法による補間を行う範囲との境界部分に生じる速度の不連続性をなくすことができ、より滑らかな動作が実現できる。例えば、金型加工など特に滑らかな動作が要求される場合などに有効である。
【００５７】
なお、実施の形態３では、差分により接線ベクトルを求めたが、解析的に求めても良い。
【００５８】
また、実施の形態３では、３次スプライン曲線を用いたが、少なくとも位置、速度が連続となる曲線であれば他の曲線を用いても良い。例えばＢ−スプライン曲線、ベジェ曲線、ＮＵＲＢＳ曲線や三角関数等を用いてもよい。また多項式あるいは有理多項式で表現される曲線の場合は、その多項式の次数は少なくとも３次以上あればよい。特に、５次以上の曲線を用い、境界条件としてさらに二階微分係数を与えて加速度まで連続にすることにより、より滑らかな動作を実現することができる。
【００５９】
実施の形態４．
実施の形態１から実施の形態３では、姿勢誤差として角度の許容値を与えた。しかし図１５に示すような工具４の側面による加工の場合には、姿勢誤差が加工面の精度に直接的に影響する。工具４がワーク５に接触する最上部の位置をＲとし、工具先端ＰからＲまでの長さをＬとする。図１６に示すように、工具４の姿勢ベクトルｑが角度ε１だけの誤差を持つ場合、Ｌと角度ε１の積であるδ１が最上部Ｒの加工誤差に対応する。
【００６０】
実施の形態４では、実施の形態１の図３のフローチャートで示した関節補間範囲算出処理において、ε１のかわりに変位誤差δ１＝Ｌ・ε１を用い、その許容誤差δ１ｍａｘと比較するように構成する。このとき、ステップＳＴ３の条件式は以下のように表せる。
δ１＞δ１ｍａｘ（式３７）
この方法では、加工面の精度を直接δ１ｍａｘ以下に抑制することができ、精度が安定する。逆にいえば、精度が一定の条件で移動時間を短くすることが可能である。特に、加工や指令経路によってＬが異なる場合に有効である。
【００６１】
このように、実施の形態４によれば、エンドミルの側面による加工などのように工具先端から所定の距離だけ離れた位置における変位誤差δ１が性能に関係する場合、関節補間範囲決定部１１が、その位置における変位誤差を直接制限することにより範囲を決定するようにしたので、より精密な補間を行うことが可能になる。
【００６２】
さらに、実施の形態４の変形例として、加工面に垂直な方向の誤差成分に着目するとよい。図１７に示すように、加工面の単位法線ベクトルをＶｎとする。まず、誤差ベクトルＶδ１は以下のようにあらわせる。
Ｖδ１＝Ｌ（ｑ’−ｑ）（式３８）
ここで、加工面の誤差δ２は、誤差ベクトルの法線成分なので、誤差ベクトルと単位法線ベクトルの内積をとることにより、
δ２= Ｖδ１・Ｖｎ（式３９）
となる。このδ２を用いて評価する場合、図３に示したフローチャートにおいて、ε１のかわりにδ２を用い、その許容誤差δ２ｍａｘと比較するように構成する。すなわち、ステップＳＴ３の条件式を
|δ２|＞δ２ｍａｘ
とする。
【００６３】
式３９におけるδ２は符号付きで、＋は加工面から離れること（削り残し）、−は加工面に近づくこと（削り込み）を意味する。従って、＋側、−側の許容誤差をそれぞれδ２ｍａｘｐ、δ２ｍａｘｍとし、ステップＳＴ３の条件式を以下のようにすればさらに加工精度をきめ細かく指定することができる。
（δ２＜δ２ｍａｘｍ）または（δ２＞δ２ｍａｘｐ）
これにより、さらに加工精度をきめ細かく指定することができる。例えば、δ２ｍａｘｍを０に近い値とすれば、加工面への削り込みを避けることができる。
【００６４】
このように、エンドミルの側面による加工においては、工具先端から所定の距離だけ離れた位置における誤差ベクトルＶδ１と加工面の法線ベクトルＶｎの内積の値が加工面の誤差そのものにあたる。このときその内積の値に基づいて関節補間範囲を決定することで、加工誤差を一定値以下に制限することが可能になる。
特に内積値は符号を有し、加工面を削り残す方向の誤差と、削りすぎる方向の誤差を区別することができる。これにより、削り残しの誤差と、削り過ぎの誤差を区別して制限することができるため、加工精度をきめ細かく管理することができる。
【００６５】
なお、実施の形態４において、ＬやＶｎは加工方法あるいは指令経路に応じて加工プログラムやパラメータに設定しておく。またδ１ｍａｘ，δ２ｍａｘ，δ１ｍａｘｍ，δ１ｍａｘｐ，δ２ｍａｘｍ，δ２ｍａｘｐは、加工面の要求精度に応じて加工プログラムあるいはパラメータに設定しておく。
【００６６】
実施の形態５．
実施の形態５では、図１８に示すような円錐型のワーク５の側面を工具４の側面で加工する場合を考える。この場合、工具４は、工具先端が円弧上を１周する間に、ワーク５に対する工具姿勢を変化させる必要がある。
このような場合、従来は、図１９に示すように、指令経路を細かく分割し、工具姿勢を細かく指定することで対応していた。図の例では、経路を１４個に分割し、ｑ０からｑ１４までの計１５個のベクトルを指定している。経路を多数に分割すればするほど、工具姿勢の誤差は小さくなる。
しかし、経路を多数に分割すれば、加工プログラムに指令するデータが増え、大きな容量の加工プログラムの記憶領域が必要となってしまう。またこの加工プログラムに従って制御を行う数値制御装置においても、動作中に多数のデータを解釈するため、処理能力の高い高価なＣＰＵを用いる必要がある。
【００６７】
実施の形態５では、工具先端の経路が円弧等で与えられる場合のように、工具先端の経路が、ある平面内に存在する場合でも、多数の工具姿勢ベクトルを指定しなくてよい方法を実現する。
【００６８】
図２０は、実施の形態５による数値制御装置５０の構成を示すブロック図である。数値制御装置５０において、作業指令部５３を構成する作業プログラムは、姿勢決定部５７に対して、姿勢ベクトルｑ０、指令経路を表す曲線式Ｃ（ｔ）および指令経路が含まれる平面の法線ベクトルＷｎを供給する。姿勢決定部５７は、これらのパラメータおよび補間部５２から供給される媒介変数ｔから、姿勢ベクトルｑ（ｔ）を決定する。補間部５２は、姿勢決定部５７によって計算された姿勢ベクトルｑ（ｔ）を用いて補間演算を行い、各関節軸の座標値θｊを計算してサーボ制御装置１４に送る。サーボ制御装置１４は５軸加工機１を駆動し、所定の動作を行わせる。
【００６９】
次に動作について説明する。
図２１は、実施の形態５による数値制御装置５０の姿勢決定部５７による姿勢決定処理のフローチャートである。
【００７０】
ここでは、作業指令部５３から与えられた指令経路が図２２に示す平面に含まれるとする。この平面の単位法線ベクトルをＷｎ、指令経路の始点における工具の姿勢ベクトルをｑ０とし、始点における平面への接線ベクトルをＷ０とする。姿勢決定部５７は、工具姿勢が工具の進行方向に対して一定の角度に保つように計算する。
【００７１】
まず、媒介変数ｔに対応する接線ベクトルＷ（ｔ）を指令経路を表す曲線式の微係数を用いて計算する（ステップＳＴ５１）。すなわち、指令経路の曲線式をＣ（ｔ）とし、その導関数をＤ（ｔ）＝ｄＣ（ｔ）／ｄｔとすると、
Ｗ（ｔ）＝Ｄ（ｔ）／｜Ｄ（ｔ）｜（式４０）
で計算される。なお始点における接線ベクトルＷ０は、
Ｗ０＝Ｗ（０）＝Ｄ（０）／｜Ｄ（０）｜（式４１）
である。
【００７２】
次に、接線ベクトルＷ（ｔ）と、始点における接線ベクトルＷ０とのなす角度βを以下の式により求める（ステップＳＴ５２）。
β＝ｃｏｓ^−１（Ｗ０・Ｗ（ｔ））（式４２）
【００７３】
さらに、このβを用いて法線ベクトルＷｎを中心にｑ０を角度βだけ回転させることで、媒介変数ｔに対応する姿勢ベクトルｑ（ｔ）を以下の式により求める（ステップＳＴ５３）。
ｑ（ｔ）＝Ｒｏｔ（Ｗｎ，β）・ｑ０（式４３）
ここでＲｏｔ（Ｗｎ，β）は、ベクトルＷｎ回りに角度βだけ回転させるための３×３行列である。その具体的な数式は、実施の形態１に関連して述べたＰａｕｌ著、吉川恒夫訳、ロボット・マニピュレータ、コロナ社、１９８４に示されている。
【００７４】
以上のように、この実施の形態５によれば、工具先端の経路が、ある平面内にある場合に、姿勢決定部５７工具の進行方向に対して一定の姿勢を保つように工具姿勢ベクトルを自動的に求めるようにしたので、多数の工具姿勢ベクトルを指定する必要がなく、大容量の記憶装置や、計算能力の高いＣＰＵを用いる必要がない。
【００７５】
【発明の効果】
以上のように、この発明によれば、工具に与えられた２つの時点での姿勢に基づいて、両時点の間における工具の姿勢誤差が姿勢誤差許容値以下となるように、関節補間範囲を決定する関節補間範囲決定部と、関節補間範囲においては関節補間により工具の姿勢を制御する情報を算出し、その他の範囲においては他の方法により工具の姿勢を制御する情報を算出する補間部とを備えるようにしたので、特異点近傍など各関節の速度や加速度が過大となるような場合に、軌跡誤差に対する要求精度を満たしつつ、移動時間をできるだけ短縮することが可能な数値制御装置を得られるという効果がある。
【図面の簡単な説明】
【図１】この発明の実施の形態１による、数値制御装置により軌跡制御を行う５軸加工機の斜視図である。
【図２】この発明の実施の形態１による、数値制御装置の構成を示すブロック図である。
【図３】この発明の実施の形態１による、数値制御装置の関節補間範囲決定部による関節補間範囲算出処理のフローチャートである。
【図４】工具のワーク座標を説明するための図である。
【図５】この発明の実施の形態１による、工具が始点から終点まで移動する間の姿勢ベクトルｑの軌跡を示す図である。
【図６】この発明の実施の形態１による、工具が始点から終点まで移動する間のＡ軸およびＣ軸の角度の変化を示す図である。
【図７】この発明の実施の形態１による、工具が始点から終点まで移動する間のＡ軸およびＣ軸の角速度の変化を示す図である。
【図８】この発明の実施の形態１による、工具が始点から終点まで移動する間の姿勢誤差の変化を示す図である。
【図９】この発明の実施の形態２による、数値制御装置の構成を示すブロック図である。
【図１０】この発明の実施の形態２による、数値制御装置の速度決定部による速度決定処理のフローチャートである。
【図１１】この発明の実施の形態２による、工具が始点から終点まで移動する間のＡ軸およびＣ軸の角速度の変化を示す図である。
【図１２】この発明の実施の形態３による、数値制御装置の構成を示すブロック図である。
【図１３】この発明の実施の形態３による、数値制御装置の曲線決定部による曲線決定処理のフローチャートである。
【図１４】この発明の実施の形態３による、工具が始点から終点まで移動する間のＡ軸およびＣ軸の角速度の変化を示す図である。
【図１５】工具の側面による加工を説明する図である。
【図１６】この発明の実施の形態４による、関節補間範囲の決定方法を説明するための図である。
【図１７】この発明の実施の形態４の変形例による、関節補間範囲の決定方法を説明するための図である。
【図１８】工具の側面によるワークの円錐側面の加工を説明する図である。
【図１９】指令経路を細かく分割して工具姿勢を指定する方法を説明する図である。
【図２０】この発明の実施の形態５による、数値制御装置の構成を示すブロック図である。
【図２１】この発明の実施の形態５による、数値制御装置の姿勢決定部による姿勢決定処理のフローチャートである。
【図２２】この発明の実施の形態５による、姿勢決定方法を説明するための図である。
【符号の説明】
１５軸加工機、２傾斜テーブル、３旋回テーブル、４工具、５ワーク、１０，２０，３０，５０数値制御装置、１１関節補間範囲決定部、１２，２２，３２，５２補間部、１３，５３作業指令部、１４サーボ制御装置、２５速度決定部、３６曲線決定部、５７姿勢決定部。

Claims

回転自由度を有し、作業対象に対する工具の姿勢を変更することが可能な機械の制御を行う数値制御装置において、
上記工具に与えられた２つの時点での姿勢に基づいて、両時点の間の全範囲において関節補間を行うときに生じる姿勢誤差が姿勢誤差許容値より大きい場合に、上記工具の姿勢誤差が上記姿勢誤差許容値以下となるまで関節補間範囲を小さくするように修正する関節補間範囲決定部と、
上記関節補間範囲においては関節補間により上記工具の姿勢を制御する情報を算出し、その他の範囲においては回転主軸法により上記工具の姿勢を制御する情報を算出する補間部とを備えたことを特徴とする数値制御装置。
決定された関節補間範囲に基づいて、機械の各関節の速度が、上記関節補間範囲では許容速度以下となるように決定する速度決定部を備え、
補間部は、上記速度決定部によって決定された各関節の速度に従って補間を行うことを特徴とする請求項１記載の数値制御装置。
決定された関節補間範囲に基づいて、関節補間範囲とその他の補間範囲との間での各関節の速度の変化を円滑にする補間曲線を決定する曲線決定部を備え、
補間部は、上記曲線決定部によって決定された曲線に従って曲線補間を行うことを特徴とする請求項１または請求項２記載の数値制御装置。
関節補間範囲決定部は、工具の姿勢誤差ベクトルと加工面の法線ベクトルとの内積に基づいて、関節補間範囲を決定することを特徴とする請求項１から請求項３のうちのいずれか１項記載の数値制御装置。