JP3830475B2

JP3830475B2 - 制御装置

Info

Publication number: JP3830475B2
Application number: JP2003286692A
Authority: JP
Inventors: 俊明大槻; 聡一郎井出
Original assignee: FANUC Corp
Current assignee: FANUC Corp
Priority date: 2003-08-05
Filing date: 2003-08-05
Publication date: 2006-10-04
Anticipated expiration: 2023-08-05
Also published as: JP2005056171A; EP1517205A2; US20050033459A1; US7039494B2; EP1517205A3

Description

本発明は、加工機等を制御する制御装置に関し、パラレルメカニズム機など、少なくとも１以上の駆動軸が直交座標系の軸方向に移動しないメカニズムを持つ加工機等を制御するのに適した制御装置に関する。

パラレルメカニズム工作機械等の直交座標系の軸方向に移動しない駆動軸を備えた機械を制御する数値制御装置等の制御装置では、直交座標系で与えられた指令に基づいて、各駆動軸の指令へ変換して各駆動軸を駆動し、工具等を取り付ける可動部材の位置と姿勢を制御する。
例えば、図８に示すパラレルメカニズム工作機械の場合、駆動軸Ｕ１〜Ｕ６はＸ−Ｙ−Ｚの直交座標系の軸方向には移動しない。このパラレルメカニズム工作機械は、固定部材２０に６個の支点２１〜２６が設けられ、該支点２１〜２６と可動部材３０に設けられた６個の支点３１〜３６とがそれぞれ駆動軸Ｕ１〜Ｕ６で接続され、駆動軸Ｕ１〜Ｕ６内に存在するサーボモータ等の駆動装置により支点間の距離を任意に伸縮することによって、可動部材３０の位置と姿勢を制御し、該可動部材３０内に設けられた主軸に取り付けられた工具４０の位置姿勢を制御するものである。

可動部材３０（工具４０）の所望の位置と姿勢に対応する可動部材３０の各支点３１〜３６の座標を求めれば、固定部材２０の各支点２１〜２６の座標は固定なので、可動部材３０の各支点３１〜３６の座標から各駆動軸Ｕ１〜Ｕ６の長さを求めることができる。よって、各駆動軸Ｕ１〜Ｕ６の長さが求めた長さになるようにサーボモータ等の駆動装置を駆動制御することにより、可動部材３０及び工具４０を所望の位置と姿勢に移動させ、加工を行わせることができる。

このようなパラレルメカニズム工作機械を数値制御装置等の制御装置で制御する場合、直交座標系での指令（工具位置の指令）に基づいて、各駆動軸Ｕ１〜Ｕ６の伸縮量に変換して可動部材３０及び工具４０の位置姿勢を制御している。
例えば、プログラム指令より、関数発生部で、直交座標系のＸ軸，Ｙ軸，Ｚ軸とＸ軸回りの回転軸Ａ，Ｙ軸回りの回転軸Ｂ、Ｚ軸回りの回転軸Ｃの各補間座標値を求め加減速処理を行い、逆機構変換部で加減速処理後の直交座標系上の座標値（Ｘ’，Ｙ’，Ｚ’，Ａ’，Ｂ’，Ｃ’）から各駆動軸Ｕ１〜Ｕ６の長さを求め各駆動軸Ｕ１〜Ｕ６の駆動装置を駆動するようにした数値制御装置が知られている（特許文献１参照）。

また、直交座標系上の指令から、各駆動軸Ｕ１〜Ｕ６の移動の指令に変換することから、可動部材３０（工具４０）の移動方向、移動速度によって、駆動軸Ｕ１〜Ｕ６の移動速度（長さの変化速度）がその駆動軸の最大許容速度を越える場合が生じる。そのため、各駆動軸の許容速度を越えないような直交座標系上での速度を制限してその速度を求めて直交座標系上で補間し、その後逆機構変換部で各駆動軸Ｕ１〜Ｕ６の移動指令を求めるようにしたものも知られている（特許文献２参照）。

又、パラレルメカニズム工作機械を制御する数値制御装置ではないが、プログラムで指令された指令を２段に分けて補間する方法も知られている。プログラムで指令された動作経路を第１のサンプリング周期で補間し補間点を求め、該補間点間を第１のサンプリング周期よりも短い第２のサンプリング周期で補間し、各駆動軸を駆動制御するようにし、この場合、第１の補間点の近傍で各軸の速度が急変することから、第２の補間を滑らかな曲線で補間し、かつ、第１の補間後に加減速制御して第２の補間を実行して、速度変化を滑らかにした発明も知られている（特許文献３，４参照）。

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

プログラム等からの指令に対して、各軸への補間後に加減速処理を行うと加減速の遅れに伴う経路誤差が発生し、これを避けることができない。特許文献１に記載された発明では、関数発生の後、すなわち補間後の直交座標系上の各軸指令に対して加減速を行ったあとに逆機構変換を行って、各駆動軸を駆動している。このように補間後加減速を行っていたため、加減速を行うことによる誤差が避けられない。

また、特許文献２に記載された発明では、パラレルメカニズム工作機械の各駆動軸の許容速度を越えないように直交座標系上における送り速度を制御して、各駆動軸の許容加速度を越えないように制御しているが、加速度や加速度の時間微分である加加速度の許容値を越えないように速度を制御できない。また、煩雑な制御を行う必要があり、実行する数値制御装置のプロセッサに大きな能力を必要とする。
そこで、本発明は上記課題を解決し、加減速を行うことによる誤差をなくすとともに、各駆動軸の、最大許容速度、最大許容加速度、加速度の時間微分である加加速度の最大許容値を越えないように速度を制御できる制御装置を提供することにある。

各駆動軸がサーボモータで駆動され、少なくとも１以上の駆動軸が直交座標系の軸方向に移動しないメカニズムを持つ加工機を制御するために、プログラムの指令に基づいて動作経路に関する補間処理を行い、該補間処理の出力に応じて前記各駆動軸の移動指令を出力する制御装置において、請求項１に係わる発明は、指令プログラムを解析し、第１補間手段のためのデータに変更する指令解析部と、前記変更されたデータに基づいて、第１のサンプリング周期毎に、動作経路上の点の位置を、直交座標系上のデータとして出力する第１補間手段と、前記直交座標系上のデータとして出力された動作経路上の点の位置を前記各駆動軸の位置に変換する変換部と、前記変換された各駆動軸の位置と各駆動軸毎に設定された最大許容加速度とにもとづき、前記第１補間手段で求められた位置間の各セグメントの接線方向加速度を求める接線方向加速度計算部と、前記変換された前記各駆動軸の位置を一時的に記憶するメモリと、前記変換された各駆動軸の位置と前記最大許容加速度とに基づいて各セグメント終点での制限速度を求める制限速度計算部と、前記接線方向加速度と前記制限速度に基づいて各セグメント終点での減速目標速度を求める減速目標速度計算部と、前記減速目標速度に基づいて、各駆動軸の合成接線方向速度に対して加減速制御を行い、第２のサンプリング周期毎の接線方向移動量を求める加減速制御手段と、該加減速制御手段で求めた接線方向移動量と前記メモリに記憶された前記各駆動軸の位置に基づいて、前記第２のサンプリング周期毎に補間位置を求める第２補間手段とを設け、一部の駆動軸が直交座標系にない機械をも制御できるようにした。

又、請求項２に係わる発明は、前記制限速度を、各駆動軸の移動において各駆動軸の速度、加速度、および加速度の時間微分である加加速度の各最大許容値の内の少なくとも１以上の最大許容値を越えないような各駆動軸の合成接線方向速度とした。さらに、請求項３に係わる発明は、前記第２補間手段を、滑らかな曲線を生成して該滑らかな曲線上の位置を出力するものとした。特に、請求項４に係わる発明は、この滑らかな曲線はｎ次の多項式で表される曲線とした。

本発明は、駆動軸が直交座標系にないメカニズムを持つ機械において、従来技術よりもより小さなＣＰＵ能力で、各駆動軸の速度、加速度、加加速度の最大許容値を越えないように、かつ加減速による誤差が発生しないように制御できる。

図１は、本発明の一実施形態の制御装置を構成する数値制御装置の概要ブロック図である。数値制御装置のハードウェア構成は、従来の数値制御装置の構成と差異はないので、概略的に図示している。この実施形態では、図８に示したパラレルメカニズム工作機械をこの数値制御装置で制御するものとして以下説明する。
数値制御装置１は、プログラム等で指令された移動指令に基づき、各軸へ各移動指令を補間分配する数値制御部２と、該数値制御部２からの移動指令を受けて各駆動軸の位置、速度、電流のフィードバック制御等のサーボ制御を行う各軸制御部３を有し、各軸制御部３は、各軸のサーボアンプＡ１〜Ａ６を介して各駆動軸Ｕ１〜Ｕ６を駆動するサーボモータＭ１〜Ｍ６を駆動する。なお、主軸モータも数値制御部が主軸モータ制御部を介して制御するが、図１では省略している。

数値制御装置１のハードウェア構成は従来の数値制御装置における６軸制御の構成と同一であり、数値制御装置内部のソフトウェア処理により行う本発明特有の機能処理について説明する。本実施形態では、ソフトウェアの面から見たブロック構成から見ると、図２のように表わすことができる。

この実施形態では、指令プログラムは通常の加工機と同様に直交座標系上の基本３軸Ｘ，Ｙ，Ｚの位置（ｘ，ｙ，ｚ）とＸ，Ｙ，Ｚ軸に対するそれぞれの回りの回転軸３軸Ａ，Ｂ，Ｃの位置（ａ，ｂ，ｃ）で指令される。そして数値制御装置内で、パラレルメカニズム工作機械の各駆動軸Ｕ１，Ｕ２，Ｕ３，Ｕ４，Ｕ５，Ｕ６の６軸の位置ｕ１，ｕ２，ｕ３，ｕ４，ｕ５，ｕ６に変換されて各駆動軸Ｕ１〜Ｕ６のサーボモータＭ１〜Ｍ６が駆動される。

図２において、指令解析部１１は直交座標系における指令プログラムを解析し、第１補間部１２で使用しやすいデータに変換する。第１補間部１２は直交座標系における指令経路上の各指令軸上の点P_n(x_n,y_n,z_n,a_n,b_n,c_n)を第１のサンプリング周期で補間計算する。ここで「ｎ」は第１補間部１２での補間処理の番号であり、第１補間部１２でのｎ回目の補間を示すもので、P_n(x_n,y_n,z_n,a_n,b_n,c_n)は第１補間部１２でのｎ回目の補間位置を表す。この第１の補間は通常の基本３軸Ｘ，Ｙ，Ｚと回転軸３軸Ａ，Ｂ，Ｃを持つ従来の加工機を制御する数値制御装置における補間と同様であり、その詳細な説明は省略する。

第１補間部１２で求められた補間点P_n(x_n,y_n,z_n,a_n,b_n,c_n)に基づいて（ｎ＝０，１，２…）、変換部１３が図３に示すように逆機構変換して変換座標系における各駆動軸の位置U_n(u1_n,u2_n,u3_n,u4_n,u5_n,u6_n)を求めて、中間メモリ１５に出力する。さらに、この位置U_n(u1_n,u2_n,u3_n,u4_n,u5_n,u6_n)に基づいて、当該第１補間のサンプリング周期τ1における移動量L_n、変換座標系上の各駆動軸の速度V_n (v1_n, v2_n, v3_n, v4_n, v5_n, v6_n)、各駆動軸の加速度A_n (a1_n, a2_n, a3_n, a4_n, a5_n, a6_n)、各駆動軸の加速度の時間微分である加加速度J_n (j1_n, j2_n, j3_n, j4_n, j5_n, j6_n)を求め中間メモリ１５に出力する。なお、逆機構変換の方法は周知であり、説明を省略する。また、この第１補間部１２で補間された補間位置を変換部１３で逆機構変換して変換座標系における各駆動軸の位置のある位置から次の位置への移動部分をセグメントと呼ぶことにする。したがって、位置Ｕｎ−１から位置Ｕｎへの移動部分を第ｎセグメントと呼ぶ。
移動量L_nは次の（１）式の処理演算することによって求められる第１のサンプリング周期毎の変換座標系における移動量である。

L_n = {(u1_n - u1_n-1)² + (u2_n - u2_n-1)² + (u3_n - u3_n-1)² + (u4_n - u4_n-1)² +
(u5_n - u5_n-1)² + (u6_n - u6_n-1)²}^1/2 …（１）
変換座標系上の各駆動軸の速度V_n (v1_n, v2_n, v3_n, v4_n, v5_n, v6_n)は、補間位置U_n(u1_n,u2_n,u3_n,u4_n,u5_n,u6_n)と第１補間のサンプリング周期τ1に基づいて、次の（２-1）〜（２-6）式の演算処理によって求める。
v1_n = (u1_n - u1_n-1)／τ1 …（２-1）
v2_n = (u2_n - u2_n-1)／τ1 …（２-2）
v3_n = (u3_n - u3_n-1)／τ1 …（２-3）
v4_n = (u4_n - u4_n-1)／τ1 …（２-4）
v5_n = (u5_n - u5_n-1)／τ1 …（２-5）
v6_n = (u6_n - u6_n-1)／τ1 …（２-6）
変換座標系上の各駆動軸の加速度A_n (a1_n, a2_n, a3_n, a4_n, a5_n, a6_n)は、（２-1）〜（２-6）式で求められた各駆動軸の速度V_n (v1_n, v2_n, v3_n, v4_n, v5_n, v6_n)と第１サンプリング周期τ1から、次の（３-1）〜（３-6）式の演算処理によって求める。
a1_n = (v1_n - v1_n-1)／τ1 …（３-1）
a2_n = (v2_n - v2_n-1)／τ1 …（３-2）
a3_n = (v3_n - v3_n-1)／τ1 …（３-3）
a4_n = (v4_n - v4_n-1)／τ1 …（３-4）
a5_n = (v5_n - v5_n-1)／τ1 …（３-5）
a6_n = (v6_n - v6_n-1)／τ1 …（３-6）
変換座標系上における各駆動軸の加速度の時間微分である加加速度J_n (j1_n, j2_n, j3_n, j4_n, j5_n, j6_n)は、（３-1）〜（３-6）式で求めた各駆動軸の加速度A_n (a1_n, a2_n, a3_n, a4_n, a5_n, a6_n)と第１サンプリング周期τ1から次の（４-1）〜（４-6）式の演算処理によって求める。

j1_n = (a1_n - a1_n-1)／τ1 …（４-1）
j2_n = (a2_n - a2_n-1)／τ1 …（４-2）
j3_n = (a3_n - a3_n-1)／τ1 …（４-3）
j4_n = (a4_n - a4_n-1)／τ1 …（４-4）
j5_n = (a5_n - a5_n-1)／τ1 …（４-5）
j6_n = (a6_n - a6_n-1)／τ1 …（４-6）
図４は、中間メモリ１５に格納されたデータの説明図である。図４に示すように、変換座標系上の補間位置（各駆動軸Ｕ１〜Ｕ６の位置）U_n(u1_n,u2_n,u3_n,u4_n,u5_n,u6_n)と、変換座標系上の移動量L_n 、変換座標系上の各駆動軸の速度V_n (v1_n, v2_n, v3_n, v4_n, v5_n, v6_n)、変換座標系上の各駆動軸の加速度A_n (a1_n, a2_n, a3_n, a4_n, a5_n, a6_n)、変換座標系上の各駆動軸の加加速度J_n (j1_n, j2_n, j3_n, j4_n, j5_n, j6_n)、さらに、後述する制限速度ＲＦ_n、減速目標速度ＤF_n、接線方向加速度ＡＴ_nが第１の補間周期の回数ｎに対応して記憶される。ただし、補間開始位置のＵ_０に対応する変換座標系上の移動量Ｌ_０、速度Ｖ_０、加速度Ａ_０、加加速度Ｊ_０は、上記（１）式、（２-1）〜（２-6）式、（３-1）〜（３-6）式、（４-1）〜（４-6）式の性格から存在しない。また、同様な理由で、補間点Ｕ_１に対応する変換座標系上の加速度Ａ_１、加加速度Ｊ_１、さらに、補間点Ｕ_２対応する変換座標系上の加加速度Ｊ_２も存在しないが、計算上必要となるので、Ａ_１＝Ａ_２，Ｊ_１＝Ｊ_３，Ｊ_２＝Ｊ_３としてセットしておく。

図５は、接線方向加速度計算部１４において、変換部１３からの出力である各駆動軸の位置データＵ_ｎ（ｎ＝０，１．．．）と、各駆動軸の最大許容加速度am1, am2, am3, am4, am5, am6から、最適な接線方向加速度を求める手順を表すフローチャートである。以下、本フローチャートに基づいて、最適な接線方向加速度ＡＴ_nを求める手順を説明する。

図５のステップ１０１では、各駆動軸の位置データＵ_ｎ（ｎ＝０，１．．．）から各駆動軸の移動量（１セグメント分）を求め、各駆動軸に関する方向余弦ＤＣi を求める。ここで、i は軸番号であり、Ｕ1軸、Ｕ2軸、Ｕ3軸、Ｕ4軸、Ｕ5軸、Ｕ6軸を制御するので、ｉ＝１、２、３、４、５、６である。この場合、方向余弦ＤＣ1は、各軸についての１セグメントを合成（ベクトル合成）した移動方向ベクトル、つまり、合成接線方向ベクトルが、第１軸（Ｕ1軸）に対してなす角度の余弦となる。したがって、ＤＣ1のｎ番目（ｎ＝１，２．．．）データは、
ＤＣ１_n ＝｜u1_n - u1_n-1 ｜／Ｌ_n …（５）
ここで、Ｌ_nはすでに中間メモリに出力されている第１のサンプリング周期毎の変換座標系における合成移動量である。

同様に、方向余弦ＤＣ２、ＤＣ３、ＤＣ４、ＤＣ５、ＤＣ６は、各軸についての１セグメントを合成した方向ベクトル（合成接線方向ベクトル）が、Ｕ２軸、Ｕ３軸、Ｕ４軸、Ｕ５軸、Ｕ６軸に対してそれぞれなす角度の余弦となる。
上記方向ベクトルで表わされる移動方向の加速度（即ち、合成接線方向加速度）ＡＷは、指令された全軸について、下記の条件を満たす必要がある。

ＡＷ×ＤＣｉ≦ａｍｉ …（６）
ここで、ａｍｉは第ｉ駆動軸の最大許容加速度である。したがって、
ＡＷ×ＤＣ１_n ≦ａｍ１ …（７-1）
ＡＷ×ＤＣ２_n ≦ａｍ２ …（７-2）
ＡＷ×ＤＣ３_n ≦ａｍ３ …（７-3）
ＡＷ×ＤＣ４_n ≦ａｍ４ …（７-4）
ＡＷ×ＤＣ５_n ≦ａｍ５ …（７-5）
ＡＷ×ＤＣ６_n ≦ａｍ６ …（７-6）
となる。

ステップ１０２では、ステップ１０１の結果を用いて、最適な接線方向加速度ＡＴ_n （ｎ＝１，２．．．）を求める。最適接線方向加速度ＡＴ_n は、駆動対象の全軸に関して接線方向加速度が最大許容加速度以下でなければならないということを表わしている上記の条件を破らない範囲で、最も大きい値として計算される。これは、ａｍｉ／ＤＣｉ（ｉ＝１、２・・・６）の中で最小のもの、Ｍｉｎ｛ａｍｉ／ＤＣｉ｝（ｉ＝１、２・・・６）を見つけることを意味している。
つまり、
ＡＴ_n ＝Ｍｉｎ｛ａｍｉ／ＤＣｉ_n｝（ｉ＝１、２・・・６）（ｎ＝１，２．．．）
…（８）
である。
続くステップ１０３で、求めた最適接線方向加速度ＡＴ_n を中間メモリ１５に出力し記憶させ処理を終える。

このようにして、中間メモリ１５には、第１のサンプリング周期毎に、変換部１３からの出力である各駆動軸の位置Ｕ_ｎなどに加えて、上記最適接線方向加速度ＡＴ_n が記憶される。なお、ここで注意すべきことは、最適接線方向加速度ＡＴ_n は一般には定数でなく、変換部１３からの出力である各駆動軸の位置に依存して変化するという点である。

制限速度計算部１６は、中間メモリ１５上のデータに基づき、変換座標系上の点Ｕ_n(n=1,2 ．．．)に対応して制限速度ＲＦ_nを計算し中間メモリ１５上に書込む。
ここで、制限速度ＲＦ_nは、各セグメントにおける合成接線方向の速度に関し、位置Ｕ_n(n=1, 2 ．．．)において、各駆動軸の速度、加速度、加加速度の最大許容値を越えないように制限される速度である。つまり、各駆動軸の速度、加速度、加加速度にはそれぞれ設定されている最大許容値があり、速度の最大許容値をＶＭ(vm1, vm2, vm3, vm4, vm5, vm6)、加速度の最大許容値をＡＭ(am1, am2, am3, am4, am5, am6)、加加速度の最大許容値をＪＭ(jm1, jm2, jm3, jm4, jm5, jm6)とすると、制限速度ＲＦ_nとは、各第１のサンプリング周期毎の各駆動軸の速度、加速度、加加速度がそれぞれの最大許容値を越えないような各セグメントにおける合成接線方向の速度である。

変換部１３から出力されたデータによる各セグメントにおける合成接線方向の速度は、L_n／τ1 である。したがって、制限速度ＲＦ_nを計算するとは、この速度 L_n／τ1 に対して次のように処理を行い、各駆動軸の速度、加速度、加加速度がそれぞれの最大許容値を越えないような各セグメントにおける合成接線方向の制限速度ＲＦ_n (n=1, 2 ．．．) を求めることである。計算された制限速度ＲＦ_n (n=1, 2 ．．．) は中間メモリ１５に出力される。
KV=Min(vm1／v1_n, vm2／v2_n, vm3／v3_n, vm4／v4_n, vm5／v5_n, vm6／v6_n)
…（９）
この（９）式において、右辺のかっこ内で最小の値を右辺の値とし、これをＫＶの値にするという意味であり、ただし、この右辺が１以上であればKVは「１」とする。
KA = Min(am1／a1_n, am2／a2_n, am3／a3_n, am4／a4_n, am5／a5_n, am6／a6_n)
…（１０）この（１０）式にも同様で、右辺のかっこ内で最小の値を右辺の値とし、これをＫＡの値にするという意味であり、ただし、この右辺が１以上であればKＡは「１」とする。
KJ = Min(jm1／j1_n, jm2／j2_n, jm3／j3_n, jm4／j4_n, jm5／j5_n, jm6／j6_n)
…（１１）
この（１１）式にも同様で、右辺のかっこ内で最小の値を右辺の値とし、これをＫＪの値にするという意味であり、この右辺が１以上であればKＪは「１」とする。
ＲＦ_n = Min(KV, KA, KJ)×Ｌ_n／τ1 …（１２）
この（１２）式にも同様でMin(KV, KA, KJ)は、KV，KA，KJの中の最小値を意味する。

（９）〜（１１）式、及び（１２）式のMin(KV, KA, KJ)は、最大許容速度／速度、最大許容加速度／加速度、最大許容加加速度／加加速度の最小のものを選択することを意味しており、各駆動軸Ｕ１〜Ｕ６に対して速度、加速度、加加速度のそれぞれの最大許容値に対して、変換部１３が（２-1）〜（２-6）式，（３-1）〜（３-6）式，（４-1）〜（４-6）式の演算処理で求めた速度、加速度、加加速度が相対的に一番大きなものを選択することを意味している。

例えば、「vm1／v1_n」が最小であったとする。これは、駆動軸Ｕ１の速度が最大許容値より相対的に１番大きく越えていることを意味する。上記（１２）において、Min(KV, KA, KJ)は、「vm1／v1_n」となる。その結果、制限速度ＲＦ_n =（vm1／v1_n）×L_n／τ1となる。
「L_n／τ1」の速度で駆動軸Ｕ１の速度はv1_nであったものであるから、制限速度ＲＦ_nとなると、比例関係にあるので、
（L_n／τ1）／v1_n＝ＲＦ_n ／（駆動軸Ｕ１の速度）
（駆動軸Ｕ１の速度）＝ＲＦ_n ×v1_n／（L_n／τ1）
＝［（vm1／v1_n）×L_n／τ1］×v1_n／（L_n／τ1）
＝vm1
となり、この制限速度ＲF_nにすることによって、一番最大許容値から外れて大きかった駆動軸Ｕ１の速度が許容速度に制限されることになる。又、他の要素は、Min(KV, KA, KJ)で一番小さな値が選択されて、制限速度ＲF_nが求められたものであるから、他の駆動軸の速度や、加速度、加加速度は最大許容値内になる。

以上のように、（９）〜（１２）式の演算処理を行って、制限速度ＲF_nを求め、中間メモリ１５に図４に示すように格納する。

なお、本実施形態では、変換座標系上（各駆動軸Ｕ１〜Ｕ６）の速度、加速度、加加速度がいずれも最大許容値を越えないように制限速度ＲF_nを求めたが、速度、加速度、加加速度のいずれか１つでもよい。又いずれか２つの組み合わせでもよい。例えば速度だけ許容値内に制限するとすれば、（１２）式のMin(KV, KA, KJ)は、「KV」となり、加速度だけとすれば、「KA」となる。また、速度、加速度が許容値内とする場合には、（１２）式のMin(KV, KA, KJ)は、Min(KV, KA,)となる。

中間メモリ１５に渡されたこれらデータは、減速目標速度計算部１７で行なわれる減速目標速度計算の処理に利用される。この減速目標速度計算は、「複数のセグメントにまたがった加減速」のためのものである。「複数のセグメントにまたがった加減速」を説明するために、図６に示した例で説明する。

図６において、Ｓ１、Ｓ２・・・Ｓ８は、いずれも変換部１３から出力されて中間メモリ上にあるセグメントを表わしている。Ｓ１〜Ｓ８の内、Ｓ８は中間メモリ１５上の最新のセグメントとし、Ｓ１は現在実行中のセグメントとする。なお、ここからの説明において、「実行中のセグメント」とは、第２補間部１９による第２補間処理が既に開始されている最新のセグメントのことであり、一般には、中間メモリ１５上にある最古のセグメントでもある（第２補間部１９が完了したセグメントデータは中間メモリ１５上から消去され、直ちに次のセグメントの第２補間処理が開始される）。

ここで一例として、中間メモリ１５に記憶された最新のセグメントＳ８の終点速度を０とし、各セグメント毎に求められる最適接線方向加速度ＡＴ_ｎで減速した結果、その終点速度（ここでは、Ｓ８については０）に達すると想定し、そのセグメント始点（最初はセグメントＳ８の始点）での速度を求める。その速度を１個前のセグメント（セグメントＳ８に対してはセグメントＳ７）の終点での速度とする。以後、同様に、１個前のセグメント始点での速度を求めることを繰り返しながら、目下実行中のセグメントＳ１（中間メモリ上の最古のセグメント）まで遡って速度パターンを作って行く。

途中で、図６中のセグメントＳ４の終点のように、制限速度ＲＦ_nや、変換部１３から出力されたデータによる合成接線方向速度Ｌ_n／τ1が、作成された速度パターンよりも低い速度になる場合には、その速度から新たな速度パターンを作る。ここで、中間メモリ１５上で最新のセグメントの終点速度を仮に０としたのは、もし第１補間または変換部側が停止した場合でも必ず０まで減速できるようにするためである。このようにして作成された速度パターンの各セグメントの終点に対応する速度を、各セグメントの減速目標速度ＤＦ_ｎ(n=1, 2 ．．．)とし、中間メモリに入力し記憶させる。

加減速処理部１８は、中間メモリ１５上のデータに基づき、減速目標速度を越えない最大の速度となるように加減速制御を行い第２のサンプリング周期（τ２）毎の移動量ΔSを計算し、第２補間部１９に出力する。ここで、第１のサンプリング周期τ１中に第２補間はｍｚ回行なわれるとする。つまり、mｚ=τ１／τ２とする。この時、第ｎ回目の第１補間周期における変換部から出力される合成接線方向速度は、Ｌ_ｎ／τ１である。また、その第１補間周期における第ｍ回目（ｍ＝１．．．ｍｚ）の第２補間周期において、減速目標速度ＤＦ_ｎから得られる速度は、
ＤＦ_ｎ−１＋（ＤＦ_ｎ−ＤＦ_ｎ−１）×ｍ／mz …（１３）
である。

これらのうち、より低い速度が第２のサンプリング周期において実現すべき速度である。この第２のサンプリング周期において実現すべき速度から、この回の第２のサンプリング周期での移動量ΔSは得られる。したがって、ΔSは次のようになる。
ΔS＝Ｍｉｎ（（Ｌ_ｎ／τ１），（ＤＦ_ｎ−１＋（ＤＦ_ｎ−ＤＦ_ｎ−１）×ｍ／mz））
×τ２ …（１４）
第２補間部１９は、加減速処理部１８からの出力と中間メモリ１５から取り出したデータに基づき第２サンプリング周期τ2で補間を行う。ここで、補間する形状としては、U_n(n=0,1 ．．．)の点列を結ぶように直線で補間してもよいし、U_n(n=0,1 ．．．)の点列から滑らかな曲線を生成し第２のサンプリング周期毎にその滑らかな曲線上の点を出力してもよい。第２のサンプリング周期τ2は、第１のサンプリング周期τ1よりも短く、例えば第１のサンプリング周期τ1は４ｍｓ、第２のサンプリング周期τ2は０．５ｍｓとする。

本実施形態では、U_n(n=0,1 ．．．)の点列から滑らかな曲線を生成し第２補間部１９で滑らかな補間を行うものとしている。滑らかな曲線を生成する方式としては種々の方式が可能であるが、この実施形態では簡単のため、点列U_n(n=0,1 ．．．)を通るｎ次式（３次式）を使用する方式を用いる。ここで、U_n(n=0,1 ．．．)の点列のうち、１つの点と次の点の間の曲線補間部分をここでもセグメントと呼ぶことにする。また、後述するフローチャート中の記号は以下のように定義される。
U_a ：U_n(n=0,1 ．．．)の点列のうち現在補間処理実行中のセグメント始点の１個前の点
U_b ：U_n(n=0,1 ．．．)の点列のうち現在補間処理実行中のセグメント始点
U_c ：U_n(n=0,1 ．．．)の点列のうち現在補間処理実行中のセグメント終点
U_d ：U_n(n=0,1 ．．．)の点列のうち現在補間処理実行中のセグメント終点の１個後の点
Ｌ_b ：Ｕ_a − Ｕ_b 間の距離
Ｌ_c ：Ｕ_b − Ｕ_c 間の距離
Ｌ_d ：Ｕ_c − Ｕ_d 間の距離
△ｓ：加減速処理部が目標減速速度を越えない最大の速度となるように加減速制御を行い第２補間部に出力される、各駆動軸の合成接線方向における第２のサンプリング周期毎の移動量
KI：セグメント切換え時に未出力である移動量の比率を表す係数
セグメント内の補間点の座標は、次の（１５-1）〜（１５-６）式を用いて計算される。パラメータｔは、点Ｕ_a 、Ｕ_b 、Ｕ_c 、Ｕ_d で、−Ｌ_b 、０、Ｌ_c 、（Ｌ_c＋Ｌ_d ）の値をとるものとし、それらの時点におけるＵ（ｔ）（ U1(ｔ)、U2(ｔ) 、U3(ｔ) 、U4(ｔ) 、U5(ｔ) 、U6(ｔ) ) の座標値が点Ｕ_a 〜Ｕ_d の座標値に一致するという条件から、係数Ａ1 、Ｂ1 、Ｃ1 、Ｄ1 、Ａ2 、Ｂ2 、Ｃ2 、Ｄ2 、Ａ3 、Ｂ3 、Ｃ3 、Ｄ3 、Ａ4 、Ｂ4 、Ｃ4 、Ｄ4 、Ａ5 、Ｂ5 、Ｃ5 、Ｄ5 、Ａ6 、Ｂ6 、Ｃ6 、Ｄ6 を求める。セグメント内の第２補間部１９による補間点は、以下の（１５-1）〜（１５-6）式でパラメータｔを［０≦ｔ≦Ｌ_c ］の範囲で変化させることによって得られる。

U1（ｔ）＝Ａ1×ｔ³ ＋Ｂ1×ｔ² ＋Ｃ1×ｔ＋Ｄ1 …（１５-1）
U2（ｔ）＝Ａ2×ｔ³ ＋Ｂ2×ｔ² ＋Ｃ2×ｔ＋Ｄ2 …（１５-2）
U3（ｔ）＝Ａ3×ｔ³ ＋Ｂ3×ｔ² ＋Ｃ3×ｔ＋Ｄ3 …（１５-3）
U4（ｔ）＝Ａ4×ｔ³ ＋Ｂ4×ｔ² ＋Ｃ4×ｔ＋Ｄ4 …（１５-4）
U5（ｔ）＝Ａ5×ｔ³ ＋Ｂ5×ｔ² ＋Ｃ5×ｔ＋Ｄ5 …（１５-5）
U6（ｔ）＝Ａ6×ｔ³ ＋Ｂ6×ｔ² ＋Ｃ6×ｔ＋Ｄ6 …（１５-6）
また、U1（ｔ）、U2（ｔ）、U3（ｔ）、U4（ｔ）、U5（ｔ）、U6（ｔ）をパラメータｔについて微分したものをU1’（ｔ）、U2’（ｔ）、U3’（ｔ）、U4’（ｔ）、U5’（ｔ）、U6’（ｔ）で表わす。

図７は、この第２補間部１９による補間処理を実行する数値制御装置のプロセッサが行う処理のフローチャートである。該フローチャートにおいて、各ステップにおける処理の要点は次の通りである。
［ステップ２０１］：パラメータｔと係数KIを初期化する。係数KIはセグメント切換え時に出力されていない移動量の比率を表す係数であるから、切換え時以外は１．０が設定されている。
［ステップ２０２］：最初のセグメントの補間を行う。最初のセグメントでは、そのセグメントの１個前の点U_aが存在しないためフローチャートのステップ２０３以降で説明する方法と違った補間処理が行なわれる。このようなケースにおける補間処理方法は周知であり、詳細な手順については説明を省略する。
［ステップ２０３］：補間処理を実行するセグメントを次のセグメントに切換える。
［ステップ２０４］：現在補間処理実行中のセグメントが最後のセグメントかどうか判断し、最後のセグメントである場合にはステップ２１４（最後のセグメントの補間処理）へ進む。最後のセグメントでない場合には、ステップ２０５に進む。
［ステップ２０５〜２０７］：中間メモリからＵ_a 、Ｕ_b 、Ｕ_c 、Ｕ_d の座標および、セグメントの距離Ｌ_b 、Ｌ_c 、Ｌ_d を読取り、補間を行うための（９-1）〜（９-1）式のU1（ｔ）、U2（ｔ）U3（ｔ）、U4（ｔ）、U5（ｔ）、U6（ｔ）の係数Ａ1、Ｂ1、Ｃ1、Ｄ1、Ａ2、Ｂ2、Ｃ2、Ｄ2、Ａ3、Ｂ3、Ｃ3、Ｄ3、Ａ4、Ｂ4、Ｃ4、Ｄ4、Ａ5、Ｂ5、Ｃ5、Ｄ5、Ａ6、Ｂ6、Ｃ6、Ｄ6 を求める。
［ステップ２０８］：加減速処理部１８で計算された第２のサンプリング周期τ2あたりの移動量△ｓ、U1（ｔ）、U2（ｔ）、U3（ｔ）、U4（ｔ）、U5（ｔ）、U6（ｔ）の１次微分値、並びに係数KIからパラメータｔの増分△ｔを求める。
［ステップ２０９］：ｔにその増分△ｔを加える。
［ステップ２１０］：パラメータｔと距離Ｌ_cを比較し、ｔ＞Ｌ_c となった場合には次のセグメントに切換える必要があると判断し、ステップ２１３（次のセグメントで出力する距離の比率KIの計算）へ進む。ｔ≦Ｌ_c の場合には、同じセグメント内であると判断し、ステップ１１１に進む。
［ステップ２１１］：比率KI＝１．０とする。
［ステップＳ２１２］：第２補間点を式 U1（ｔ）、U2（ｔ）、U3（ｔ）、U4（ｔ）、U5（ｔ）、U6（ｔ）に基づいて計算し、出力した後、ステップ２０８に戻る。
［ステップ２１３］：次のセグメントで出力する距離の係数（比率）KIを計算する。
［ステップ２１４］：最後のセグメントの補間処理を行う。最後のセグメントでは、そのセグメントの１個後の点U_dが存在しないため、最初のセグメントの場合と同様、フローチャートのステップ２０３以降で説明した方法と違った補間処理が行なわれる。このようなケースにおける補間処理方法は周知であり、詳細な手順については説明を省略する。

以上のように、第２補間の前に加減速処理がなされるので、指令に対する誤差をなくすことができる。
なお、この実施形態はパラレルメカニズム機のみに適用されるものでなく、直交座標系上の軸方向とは異なる方向の駆動軸を少なくとも１以上有するような機械に適用できるものである。又、移動軸の指令軸数および変換座標系上の駆動軸数は任意の軸数に対応させることが可能であり、直交座標系上の各指令軸は直線軸または回転軸に任意に対応させることが可能である。

例えば、図９図に示す例は、伸縮する２つの駆動軸Ｕ１，Ｕ２は支点２１，２２でそれぞれ固定部材２０に接続され、各駆動軸Ｕ１，Ｕ２の他端は可動部材３０の支点３１に接続されている。駆動軸Ｕ１，Ｕ２内には、サーボモータが組み込まれており、該サーボモータの駆動によって駆動軸Ｕ１，Ｕ２の長さが伸縮する。このような機構において、直交座標系上の指令軸の２軸(Ｘ，Ｙ)の移動指令に対して、その移動指令を駆動軸Ｕ１，Ｕ２の移動指令に変換して、各駆動軸のサーボモータを駆動して可動部材３０の位置速度を制御する。このような機構に適用することも可能である。

また、例えば、図１０に示す例では、内部に設けられたサーボモータ等により伸縮する駆動軸Ｕ１と所定長の腕５０の一端が固定部材２０に設けた支点２２で接続され、腕５０は支点２２を中心に回転軸Ａに接続され、腕５０の他端と駆動軸Ｕ１の他端は可動部材３０の支点３１に接続されている。回転軸Ａの移動指令に対して、その移動指令を駆動軸Ｕ１の移動指令に変換して、駆動軸Ｕ１を駆動するサーボモータを駆動して可動部材３０の位置速度を制御する。このような機構にも適用することが可能である。

本発明の一実施形態の数値制御装置の概要ブロック図である。同実施形態における機能ブロック図である。同実施形態における変換部の変換データを表す図である。同実施形態における中間メモリに格納されるデータの説明図である。同実施形態における接線方向加速度算出処理のフローチャートである。同実施形態における減速目標速度算出の説明図である。同実施形態における第２補間処理のフローチャートである。同実施形態における加工機の構成を表す図である。本発明が適用可能な動作機構の一例を示す図である。本発明が適用可能な動作機構の他の一例を示す図である。

符号の説明

１数値制御装置
２０固定部材
２１，２２，２３，２４，２５，２６支点
３０可動部材
３１，３２，３３，３４，３５，３６支点
４０工具
Ｕ１，Ｕ２，Ｕ３，Ｕ４，Ｕ５，Ｕ６駆動軸

Claims

各駆動軸がサーボモータで駆動され、少なくとも１以上の駆動軸が直交座標系の軸方向に移動しないメカニズムを持つ加工機を制御するために、プログラムの指令に基づいて動作経路に関する補間処理を行い、該補間処理の出力に応じて前記各駆動軸の移動指令を出力する制御装置において、
指令プログラムを解析し、第１補間手段のためのデータに変更する指令解析部と、
前記変更されたデータに基づいて、第１のサンプリング周期毎に、動作経路上の点の位置を、直交座標系上のデータとして出力する第１補間手段と、
前記直交座標系上のデータとして出力された動作経路上の点の位置を前記各駆動軸の位置に変換する変換部と、
前記変換された各駆動軸の位置と各駆動軸毎に設定された最大許容加速度とにもとづき、前記第１補間手段で求められた位置間の各セグメントの接線方向加速度を求める接線方向加速度計算部と、
前記変換された前記各駆動軸の位置を一時的に記憶するメモリと、
前記変換された各駆動軸の位置と前記最大許容加速度とに基づいて、前記各セグメント終点での制限速度を求める制限速度計算部と、
前記接線方向加速度と前記制限速度に基づいて前記各セグメント終点での減速目標速度を求める減速目標速度計算部と、
前記減速目標速度に基づいて、各駆動軸の合成接線方向速度に対して加減速制御を行い、第２のサンプリング周期毎の接線方向移動量を求める加減速制御手段と、
該加減速制御手段で求めた接線方向移動量と前記メモリに記憶された前記各駆動軸の位置に基づいて、前記第２のサンプリング周期毎に補間位置を求める第２補間手段とを備えた制御装置。
前記制限速度は、各駆動軸の移動において各駆動軸の速度、加速度、および加速度の時間微分である加加速度の各最大許容値の内の少なくとも１以上の最大許容値を越えないような各駆動軸の合成接線方向速度である請求項１に記載された制御装置。
前記第２補間手段は、滑らかな曲線を生成して該滑らかな曲線上の位置を出力する請求項１又は請求項２に記載の制御装置。
前記滑らかな曲線はｎ次の多項式で定義した請求項３に記載の制御装置。