JP3576419B2 - 工作機の工具位置決め制御方法 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は工作機の工具位置決め制御方法に関する。
【０００２】
【従来の技術】
例えば特開平９−２２５７６１号公報に開示されるように、ワークが固定されるワーク台の姿勢を変化させるとともに、工具が装着されるスピンドル軸の姿勢を変化させる工作機は知られている。この工作機は、共通の基台上で１対の垂直軸回りに回転する第１および第２支持部材を備える。第１支持部材には、水平軸回りに揺動自在にワーク台が支持される。その一方で、第２支持部材には、水平軸回りで揺動自在にスピンドル軸が支持される。こうした軸回りの回転や揺動を利用した工作機では、直交３軸に案内されて工具が移動する既存の工作機に比べて、小型化や軽量化を容易に実現することができる。
【０００３】
【発明が解決しようとする課題】
このような工作機では、垂直軸回りで回転する第１および第２支持部材の回転量や、水平軸回りで揺動するワーク台およびスピンドル軸の揺動量に基づき、ワークに対する工具の位置が特定される。しかしながら、こうして第１および第２支持部材が回転したり、ワーク台およびスピンドル軸が揺動したりすると、ワークに対する工具の姿勢は変化してしまう。こうした工作機で、ワークに対する工具の位置および姿勢を同時に特定することができる工具位置決め制御方法はいまのところ確立されていない。
【０００４】
本発明は、上記実状に鑑みてなされたもので、１対の垂直軸回りで各々姿勢を変化させる１対の水平軸にワーク台およびスピンドル軸を個別に支持させた際に、スピンドル軸に装着された工具の姿勢を特定しつつワークに対して確実に工具を位置決めすることができる工作機の工具位置決め制御方法を提供することを目的とする。
【０００５】
【課題を解決するための手段】
上記目的を達成するために、本発明によれば、基台から垂直に立ち上がる第１および第２垂直軸の間隔ｄ_４ を示す軸間距離データを取得する工程と、第１垂直軸回りで回転自在に支持される第１水平軸の高さｄ_３ を示す第１高さデータを取得する工程と、第２垂直軸回りで回転自在に支持される第２水平軸の高さｄ_５ を示す第２高さデータを取得する工程と、第１水平軸回りで揺動自在に支持されるワーク台に対して第１水平軸からワーク固定面までの距離ｄ_１ を示すワーク揺動半径データを取得する工程と、ワーク固定面に設定されるワーク座標系上でワークの加工位置（ｐ_ｘ ，ｐ_ｙ ，ｐ_ｚ ）を特定する加工位置データを取得する工程と、第２水平軸回りで揺動自在に支持されるスピンドル軸の姿勢をワーク座標系上で特定する工具姿勢データを取得する工程と、取得した軸間距離データ、第１および第２高さデータ、ワーク揺動半径データ、加工位置データおよび工具姿勢データを用いて、逆運動学方程式に基づき第２水平軸から加工位置までの距離ｄ_７ を算出する工程とを備えることを特徴とする工作機の工具位置決め制御方法が提供される。
【０００６】
かかる工具位置決め制御方法によれば、ワーク固定面に設定されるワーク座標系に従って、ワークの加工位置やスピンドル軸の姿勢が特定されると、第２水平軸から加工位置までの距離ｄ_７ が算出されることができる。したがって、第２水平軸上でスピンドル軸の軸方向進退移動を許容すれば、そういった進退移動を通じてスピンドル軸の先端やその先端に装着された工具を正確に加工位置に位置決めすることができる。このとき、ワーク固定面に固定されるワークに対してスピンドル軸の姿勢は変化しない。しかも、スピンドル軸の軸心は常に加工位置を向けられる。
【０００７】
前記ワーク座標系のｚ軸は、前記第１垂直軸および第１水平軸の交点で第１水平軸に直交することが望ましい。このようにワーク座標系のｚ軸が設定されれば、工作機に対して構築される運動学モデルをできる限り単純化することが可能となる。その結果、逆運動学方程式は簡素化され、逆運動学方程式の解を求めやすくなる。しかも、解を求めるにあたって計算処理の負担は軽減される。
【０００８】
しかも、前記スピンドル軸の姿勢を特定するにあたって、前記加工位置を基準に、前記第２垂直軸および第２水平軸の交点から第２水平軸に垂直に延びる前記スピンドル軸の軸心に沿って特定されるｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）が用いられることが望ましい。こうしたｚ軸単位ベクトルが用いられれば、確実に逆運動学方程式を解くことが可能となる。加えて、工作機に対して構築される運動学モデルをさらに単純化することが可能となる。したがって、さらに簡単に逆運動学方程式の解を求めることができ、しかも、解を求めるにあたって計算処理の負担は軽減される。
【０００９】
こうした条件の下では、前記距離ｄ_７ は、例えば、
【数１１】

ただし、
【数１２】

に基づき算出されることができる。
【００１０】
この種の工具位置決め制御方法は、逆運動学方程式に基づき、前記第１水平軸回りで揺動する前記ワーク台のワーク揺動角θ_２ や、前記第１垂直軸回りで回転する第１水平軸の回転角θ_３ 、前記第２水平軸回りで揺動するスピンドル軸のスピンドル揺動角θ_６ 、前記第２垂直軸回りで回転する第２水平軸の回転角θ_５ を算出する工程をさらに備えることが望ましい。
【００１１】
こうしたワーク揺動角θ_２ は、例えば、
【数１３】

ただし、
【数１４】

に基づき算出されればよい。
【００１２】
前記回転角θ_３ は、例えば、
【数１５】

ただし、
【数１６】

に基づき算出されればよい。
【００１３】
前記スピンドル揺動角θ_６ は、例えば、
【数１７】

ただし、
【数１８】

に基づき算出されればよい。
【００１４】
前記回転角θ_５ は、例えば、
【数１９】

ただし、
【数２０】

に基づき算出されればよい。
【００１５】
なお、本発明に係る工具位置決め制御方法が適用される工作機では、スピンドル軸の先端に、スピンドル軸の軸心回りで回転するドリルやエンドミルといった一般の工具が装着されてもよく、スピンドル軸の軸心に沿ってレーザビームを照射するレーザ照射源やウォータージェットを発射するウォータージェット源が支持されてもよい。
【００１６】
【発明の実施の形態】
以下、添付図面を参照しつつ本発明の一実施形態を説明する。
【００１７】
図１は、本発明に係る工作機の工具位置決め制御方法が適用される工作機を概略的に示す。この工作機１０は、工具１１が装着されるスピンドル軸１２と、スピンドル軸１２に装着された工具１１に対してワークを向かい合わせに保持するワーク台１３とを備える。ワーク台１３に保持されるワークを加工するにあたって、ワークは、ワーク台１３の正面に形成されるワーク固定面１４に固定される。スピンドル軸１２は軸心回りに高速で工具１１を回転させることができる。
【００１８】
スピンドル軸１２はクイル１５に進退自在に挿入される。クイル１５には、スピンドル軸１２に平行に延びるねじ軸１６が支持される。スピンドル軸１２の後端には、ねじ軸１６に噛み合うナット部材１７が固定される。駆動モータ１８の働きを通じてねじ軸１６が軸心回りに回転すると、ねじ軸１６上をナット部材１７が移動し、その結果、スピンドル軸１２の進退移動が実現される。この進退移動によって、スピンドル軸１２すなわち工具１１は、ワーク固定面１４に向かって前進したりワーク固定面１４から後退したりすることができる。スピンドル軸１２の進退移動量は、周知のとおり、駆動モータ１８の回転量に基づいて決定される。
【００１９】
ワーク台１３は、第１水平軸２０回りで揺動自在に第１支持部材２１に支持される。第１水平軸２０には駆動モータ（例えばサーボモータ）２２が連結される。この駆動モータ２２の働きを通じて、ワーク台１３は第１水平軸２０回りで揺動することができる。ワーク台１３の揺動角すなわちワーク揺動角は駆動モータ２２の回転量によって特定されることができる。
【００２０】
しかも、第１支持部材２１は、基台２３から垂直に立ち上がる第１垂直軸２４回りで回転することができる。第１支持部材２１の回転は駆動モータ（図示せず）の働きによって実現される。第１垂直軸２４回りで回転する第１支持部材２１すなわち第１水平軸２０の回転角は駆動モータの回転量によって特定されることができる。
【００２１】
一方で、前述のクイル１５は、第２水平軸２６回りで揺動自在に第２支持部材２７に支持される。第２水平軸２６には駆動モータ（例えばサーボモータ）２８が連結される。この駆動モータ２８の働きを通じて、クイル１５すなわちスピンドル軸１２は第２水平軸２６回りで揺動することができる。スピンドル軸１２の揺動角すなわちスピンドル揺動角は駆動モータ２８の回転量によって特定されることができる。
【００２２】
しかも、第２支持部材２７は、第１垂直軸２４に平行に基台２３から立ち上がる第２垂直軸２９回りで回転することができる。第２支持部材２７の回転は駆動モータ（図示せず）の働きによって実現される。第２垂直軸２９回りで回転する第２支持部材２７すなわち第２水平軸２６の回転角は駆動モータの回転量によって特定されることができる。
【００２３】
図２に示されるように、ワーク台１３や工具１１すなわちスピンドル軸１２の姿勢はコントローラ３１によって制御される。このコントローラ３１は、ワーク固定面に設定されるワーク座標系ｘｙｚ上でワークの加工位置（ｐ_ｘ ，ｐ_ｙ ，ｐ_ｚ ）を特定する加工位置データや、ワーク座標系ｘｙｚ上で工具１１すなわちスピンドル軸１２の姿勢を特定する工具姿勢データを取得するデータ取得回路３２を備える。このデータ取得回路３２には、例えばＮＣプログラムに記述される加工位置データや工具姿勢データが取り込まれればよい。
【００２４】
演算処理回路（ＭＰＵ）３３は、後述するように、データ取得回路３２から供給される加工位置データおよび工具姿勢データに基づき、スピンドル軸１２の進退移動量ｄ_７ を規定する駆動モータ１８の回転量や、ワーク揺動角θ_２ を規定する駆動モータ２２の回転量、第１および第２支持部材２１、２７の回転角θ_３ 、θ_５ を規定する駆動モータ３５、３６の回転量、スピンドル揺動角θ_６ を規定する駆動モータ２８の回転量を算出する。この算出にあたって、ＭＰＵ３３は、第１および第２垂直軸２４、２９の間隔ｄ_４ を示す軸間距離データや、第１および第２水平軸２０、２６の高さｄ_３ 、ｄ_５ を示す第１および第２高さデータ、第１水平軸２０からワーク固定面１４すなわちワーク座標系ｘｙｚの原点までの距離ｄ_１ を示すワーク揺動半径データをメモリ３７から読み出す。メモリ３７には、間隔ｄ_４ や高さｄ_３ 、ｄ_５ 、距離ｄ_１ の実測値が予め格納されていればよい。
【００２５】
その一方で、各駆動モータ１８、２２、３５、３６、２８の回転量は、駆動モータごとに設けられるエンコーダ３８〜４２によって検出されることができる。検出された各駆動モータ１８、２２、３５、３６、２８の回転量に基づいて、ＭＰＵ３３は、ワーク固定面１４に設定されたワーク座標系ｘｙｚに基づき工具１１の位置や姿勢を算出する。表示器駆動回路４３は、算出された工具１１の位置や姿勢を表示器４４に表示させることができる。
【００２６】
いま、工作機１０でワークを加工する場面を想定する。まず、作業者は、ワーク固定面１４にワークを固定する。この固定によって、ワークはワーク座標系ｘｙｚに取り込まれる。このとき、ワーク座標系ｘｙｚのｚ軸は、第１垂直軸２４および第１水平軸２０の交点で第１水平軸２０に直交する。したがって、第１水平軸２０回りでワーク台１３が揺動するとワーク座標系ｘｙｚはその揺動に追随することとなる。
【００２７】
作業者は、ワーク座標系ｘｙｚ上でワークの加工位置（ｐ_ｘ ，ｐ_ｙ ，ｐ_ｚ ）すなわち工具１１の先端位置をデータ取得回路３２に取り込ませる。作業者は、同時に、データ取得回路３２を通じて工具１１の姿勢を指定する。工具１１の姿勢を特定するにあたっては、加工位置（ｐ_ｘ ，ｐ_ｙ ，ｐ_ｚ ）を基準にスピンドル軸１２の軸心に沿って特定されるｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）が用いられればよい。このとき、スピンドル軸１２の軸心は、第２垂直軸２９および第２水平軸２６の交点から第２水平軸２６に垂直に延びる。ワーク固定面１４に対するスピンドル軸１２の姿勢が変化するとｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）はその変化に追随する。
【００２８】
こうして加工位置や工具１１の姿勢が特定されると、ＭＰＵ３３は、次式に従ってスピンドル軸１２の進退移動量ｄ_７ を算出する。
【００２９】
【数２１】

ただし、
【数２２】

こうして進退移動量ｄ_７ が算出されると、ＭＰＵ３３は、次式に従ってワーク揺動角θ_２ を算出する。
【００３０】
【数２３】

ただし、
【数２４】

ワーク揺動角θ_２ が算出されると、ＭＰＵ３３は、次式に従って第１支持部材２１すなわち第１水平軸２０の回転角θ_３ を算出する。
【００３１】
【数２５】

ただし、
【数２６】

同時に、ＭＰＵ３３は、次式に従ってスピンドル揺動角θ_６ を算出する。
【００３２】
【数２７】

ただし、
【数２８】

こうしてスピンドル揺動角θ_６ が算出されると、続いてＭＰＵ３３は、次式に従って第２支持部材２７すなわち第２水平軸２６の回転角θ_５ を算出する。
【００３３】
【数２９】

ただし、
【数３０】

ＭＰＵ３３は、算出された進退移動量ｄ_７ 、ワーク揺動角θ_２ 、第１および第２支持部材２１、２７の回転角θ_３ 、θ_５ およびスピンドル揺動角θ_６ を特定する駆動指令信号を各駆動モータ１８、２２、３５、３６、２８に供給する。その結果、駆動モータ１８、２２、３５、３６、２８が作動すると、工具１１の先端はワーク上の加工位置（ｐ_ｘ ，ｐ_ｙ ，ｐ_ｚ ）に位置決めされる。しかも、このとき、工具１１の姿勢は、ワーク座標系ｘｙｚで指定されるｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）によって特定される。前述したＭＰＵ３３の処理によれば、１つのｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）が維持される限り、工具１１を移動させても加工位置（ｐ_ｘ ，ｐ_ｙ ，ｐ_ｚ ）に対する工具１１の姿勢は変化しない。
【００３４】
ここで、前述したＭＰＵ３３の算出処理を簡単に検証する。まず、ワーク固定面に対して基準座標系を設定し、ロボット工学に基づく運動学モデル（リンクモデル）を構築する。例えば図３に示されるように、ワーク固定面１４に対して基準座標系Ｘ_０ Ｚ_０ が設定されると、ワーク台１３と第１支持部材２１とのジョイントすなわち第１水平軸２０回りでは第１座標系Ｘ_１ Ｚ_１ および第２座標系Ｘ_２ Ｚ_２ が設定されることができる。同様に、第１支持部材２１と基台２３とのジョイントすなわち第１垂直軸２４回りでは第３座標系Ｘ_３ Ｚ_３ が規定され、基台２３と第２支持部材２７とのジョイントすなわち第２垂直軸２９回りでは第４座標系Ｘ_４ Ｚ_４ が規定されることができる。さらに、第２支持部材２７とクイル１５とのジョイントすなわち第２水平軸２６回りでは第５座標系Ｘ_５ Ｚ_５ および第６座標系Ｘ_６ Ｚ_６ が規定されることができる。最終的に、クイル１５に挿入されるスピンドル軸１２の先端では第７座標系Ｘ_７ Ｚ_７ が規定される。設定された基準座標系Ｘ_０ Ｚ_０ に基づけば、第７座標系Ｘ_７ Ｚ_７ の原点すなわち加工位置はベクトル（ｐ_ｘ ，ｐ_ｙ ，ｐ_ｚ ）によって特定されることができる。
【００３５】
特定された運動学モデルに基づき、各座標系ごとにパラメータθ_ｎ （Ｚ_ｎ−１ 回りの回転角）、ｄ_ｎ （Ｚ_ｎ−１ に沿った距離）、ａ_ｎ （Ｘ_ｎ に沿った距離）、α_ｎ （Ｘ_ｎ 回りの回転角）を特定すると次表が得られる。
【００３６】
【表１】

得られたパラメータθ_ｎ 、ｄ_ｎ 、ａ_ｎ 、α_ｎ に基づいて各座標系Ｘ_１ Ｚ_１ 〜Ｘ_７ Ｚ_７ ごとに変換行列Ａ_ｎ を特定すると、
【数３１】

ただし、
【数３２】

が得られる。その結果、基準座標系Ｘ_０ Ｚ_０ に対する工具１１の姿勢や位置は、運動学方程式に従い、
【数３３】

ただし、
【数３４】

で表現されることができる。したがって、以上の運動学方程式に従えば、エンコーダ３８〜４２で検出される進退移動量ｄ_７ 、ワーク揺動角θ_２ 、第１および第２支持部材２１、２７の回転角θ_３ 、θ_５ およびスピンドル揺動角θ_６ に基づき、加工位置（ｐ_ｘ ，ｐ_ｙ ，ｐ_ｚ ）や、工具１１の姿勢を特定するｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）を算出することができる。算出された加工位置（ｐ_ｘ ，ｐ_ｙ ，ｐ_ｚ ）やｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）の座標値は表示器４４に表示されればよい。その結果、作業者は、加工位置や工具の姿勢を確認しながら工作機１０を操作することが可能となる。
【００３７】
続いて、得られた運動学方程式に基づいて逆運動学方程式を導き出す。まず、加工位置のｘ座標値ｐ_ｘ に関する方程式を以下のように変形する。
【００３８】
【数３５】

両辺を２乗すると、
【数３６】

が得られる。同様に、加工位置のｙ座標値ｐ_ｙ およびｚ座標値ｐ_ｚ に関する方程式を以下のように変形する。
【００３９】
【数３７】

【数３８】

【数３９】

【数４０】

式［数３６］および式［数４０］の和は、
【数４１】

として求められる。ここで、
【数４２】

を適用すると、
【数４３】

が得られる。さらに、
【数４４】

を適用すると、
【数４５】

が得られる。
【００４０】
得られた式［数４５］を展開し、ｄ_７ に関して整理すると、
【数４６】

が得られる。ここで、式［数４６］を以下のように置換する。
【００４１】
【数４７】

ただし、
【数４８】

ｄ_７ に関して式［数４７］を解くと、
【数４９】

および
【数５０】

が得られる。ここでは、工作機１０の加工時動作範囲を考慮した結果、式［数４９］を選択することとする。式［数４９］は、０゜＜θ_５ ＜１８０゜の条件の下で適用されることができる。
【００４２】
次に、加工位置のｘ座標値ｐ_ｘ に関する方程式にＳ_２ を掛け合わせ、同時に、加工位置のｚ座標値ｐ_ｚ に関する方程式にＣ_２ を掛け合わせる。
【００４３】
【数５１】

得られた２つの方程式の和を求めると、
【数５２】

が得られる。ここで、得られた式［数５２］に以下の三角法的代入を施す。
【００４４】
【数５３】

ただし、
【数５４】

すると、
【数５５】

が得られる。この式を変形すると、
【数５６】

が得られる。φ−θ_２ について解くと、
【数５７】

が得られ、したがって、ワーク揺動角θ_２ は以下のとおりとなる。
【００４５】
【数５８】

【数５９】

以上の算出過程で用いられる２変数逆正接関数ａｔａｎ２（ｙ，ｘ）によれば、周知のとおり、ｘ、ｙの符号を考慮しつつｙ／ｘの逆正接関数が求められる。
【００４６】
図４には、作業者の視点で見たワーク揺動角と、工作機１０の運動学モデルから見たワーク揺動角θ_２ と、式［数５８］で算出されるワーク揺動角θ_２ との関係が示される。作業者の視点に基づけば、第１水平軸２０および第１垂直軸２４の交点と、第２水平軸２６および第２垂直軸２９の交点とを結ぶ直線に対してワーク固定面１４が直交する際、言い換えれば、そういった直線上でワーク座標系ｘｙｚのｚ軸が特定される際にワーク揺動角は「０゜」を示す。図４から明らかなように、例えば作業者の視点でワーク揺動角が１８０゜のとき、運動学モデルから見るとワーク揺動角θ_２ ＝９０゜となる。このとき、式［数５８］で算出されるワーク揺動角θ_２ は対応する。その一方で、作業者の視点でワーク揺動角が−１８０゜のとき、運動学モデルで見るとワーク揺動角θ_２ ＝−２７０゜となる。このとき、式［数５８］で算出されるワーク揺動角θ_２ ＝９０゜である。そこで、式［数５８］の出力が＋９０゜を超えた場合には、式［数５８］の算出値に−３６０゜を加えれば、ワーク揺動角θ_２ の動作範囲−２７０゜〜＋９０゜に式［数５８］の出力を対応させることができる。その結果、式［数５８］のみで、ワーク揺動角θ_２ の動作範囲をカバーすることが可能となる。
【００４７】
次に、加工位置のｚ座標値ｐ_ｚ に関する方程式にＳ_２ を掛け合わせ、同時に、加工位置のｘ座標値ｐ_ｘ に関する方程式にＣ_２ を掛け合わせる。
【００４８】
【数６０】

得られた２つの方程式の差を求めると、
【数６１】

が得られる。また、加工位置のｙ座標値ｐ_ｙ に関する方程式をＣ_３ に関して解くと、
【数６２】

が得られ、したがって、回転角θ_３ は以下のとおりとなる。
【００４９】
【数６３】

さらに、ｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）のｘ座標値ｂ_ｘ に関する方程式にＳ_２ を掛け合わせ、同時に、同単位ベクトルのｚ座標値ｂ_ｚ に関する方程式にＣ_２ を掛け合わせる。
【００５０】
【数６４】

得られた２つの方程式の和を求めると、
【数６５】

が得られる。また、
【数６６】

であることから、スピンドル揺動角θ_６ は以下のとおりとなる。
【００５１】
【数６７】

ただし、０゜＜θ_６ ＜１８０゜とする。
【００５２】
さらにまた、ｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）のｙ座標値ｂ_ｙ に関する方程式に次式を適用し、Ｓ_３５に関して解く。
【００５３】
【数６８】

すると、
【数６９】

が得られる。同様に、ｚ軸単位ベクトル（ｂ_ｘ ，ｂ_ｙ ，ｂ_ｚ ）のｘ座標値ｂ_ｘ に関する方程式およびｚ座標値ｂ_ｚ に関する方程式に次式を適用し、Ｓ_３５に関して解く。
【００５４】
【数７０】

すると、
【数７１】

および
【数７２】

が得られる。したがって、θ_３５は以下のとおりとなる。
【００５５】
【数７３】

式［数７１］および式［数７２］は、前述のように算出されるθ_２ の値に基づいて選択されればよい。例えば、ｓｉｎθ_２ の絶対値が０．５以下であれば式［数７１］を選択し、ｓｉｎθ_２ の絶対値が０．５を超える場合には式［数７２］を選択するといった具合である。こうした選択によれば、式［数７１］および式［数７２］の分母の大きさを十分に確保することができる。しかも、前述の条件０゜＜θ_６ ＜１８０゜を考慮すれば、ｓｉｎθ_４ ＝０になることはない。
【００５６】
図５には、作業者の視点で見た回転角θ_３５と、工作機１０の運動学モデルから見た回転角θ_３５と、式［数７３］で算出される回転角θ_３５との関係が示される。図５から明らかなように、例えば作業者の視点で回転角θ_３５は−１８０゜＜θ_３５＜＋１８０゜の範囲で変動し、運動学モデルから見ると回転角θ_３５は０゜＜θ_３５＜３６０゜の範囲で変動する。その一方で、式［数７３］で算出される回転角θ_３５は−１８０゜＜θ_３５＜＋１８０゜の範囲で変動する。したがって、前述と同様に、式［数７３］の出力が０゜より小さい場合には、式［数７３］の算出値に＋３６０゜を加えれば、要求される範囲に回転角θ_３５の範囲を対応させることが可能となる。
【００５７】
第１および第２支持部材２１、２７の回転角θ_３ 、θ_５ および前述のθ_３５の間には次式の関係が成立することから、
【数７４】

次式に基づいて回転角θ_５ は算出されることができる。
【００５８】
【数７５】

なお、以上のような運動学方程式や逆運動学方程式は、図３に示される運動学モデルに従って導き出されるもので、運動学モデルの構築の仕方に応じて運動学方程式や逆運動学方程式は修正されることができる。
【００５９】
【発明の効果】
以上のように本発明によれば、１対の垂直軸回りで各々姿勢を変化させる１対の水平軸にワーク台およびスピンドル軸を個別に支持させた際に、スピンドル軸に装着された工具の姿勢を特定しつつワークに対して確実に工具を位置決めすることができる。
【図面の簡単な説明】
【図１】工作機の全体構成を概略的に示す斜視図である。
【図２】工作機の制御系を概略的に示すブロック図である。
【図３】工作機の運動学モデルを示す図である。
【図４】逆運動学方程式に基づく算出結果と、運動学モデルに基づくワーク揺動角θ_２ の動作範囲との関係を示す図である。
【図５】逆運動学方程式に基づく算出結果と、運動学モデルに基づく回転角θ_３５の範囲との関係を示す図である。
【符号の説明】
１０ 工作機、１２ スピンドル軸、１３ ワーク台、１４ ワーク固定面、２０ 第１水平軸、２３ 基台、２４ 第１垂直軸、２６ 第２水平軸、２９ 第２垂直軸。

Claims (12)

1. 基台から垂直に立ち上がる第１および第２垂直軸の間隔ｄ_４を示す軸間距離データを取得する工程と、第１垂直軸回りで回転自在に支持される第１水平軸の高さｄ_３を示す第１高さデータを取得する工程と、第２垂直軸回りで回転自在に支持される第２水平軸の高さｄ_５を示す第２高さデータを取得する工程と、第１水平軸回りで揺動自在に支持されるワーク台に対して第１水平軸からワーク固定面までの距離ｄ_１を示すワーク揺動半径データを取得する工程と、ワーク固定面に設定されるワーク座標系上でワークの加工位置（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）を特定する加工位置データを取得する工程と、第２水平軸回りで揺動自在に支持されるスピンドル軸の姿勢をワーク座標系上で特定する工具姿勢データを取得する工程と、取得した軸間距離データ、第１および第２高さデータ、ワーク揺動半径データ、加工位置データおよび工具姿勢データを用いて、逆運動学方程式に基づき第２垂直軸および第２水平軸の交点から加工位置までの距離ｄ_７を算出する工程とを備えることを特徴とする工作機の工具位置決め制御方法。
2. 請求項１に記載の工作機の工具位置決め制御方法において、前記ワーク座標系のｚ軸は、前記第１垂直軸および第１水平軸の交点で第１水平軸に直交することを特徴とする工作機の工具位置決め制御方法。
3. 請求項２に記載の工作機の工具位置決め制御方法において、前記スピンドル軸の姿勢を特定するにあたって、前記加工位置を基準に、前記第２垂直軸および第２水平軸の交点から第２水平軸に垂直に延びる前記スピンドル軸の軸心に沿って特定されるｚ軸単位ベクトル（ｂ_ｘ，ｂ_ｙ，ｂ_ｚ）が用いられることを特徴とする工作機の工具位置決め制御方法。
4. 請求項３に記載の工作機の工具位置決め制御方法において、前記距離ｄ_７は、
   

   

   ただし、
   

   

   に基づき算出されることを特徴とする工作機の工具位置決め制御方法。
5. 請求項４に記載の工作機の工具位置決め制御方法において、前記距離ｄ _７ が算出された後、逆運動学方程式に基づき、前記第１水平軸回りで揺動する前記ワーク台のワーク揺動角θ_２を算出する工程をさらに備えることを特徴とする工作機の工具位置決め制御方法。
6. 請求項５に記載の工作機の工具位置決め制御方法において、前記ワーク揺動角θ_２は、
   

   

   ただし、
   

   

   に基づき算出されることを特徴とする工作機の工具位置決め制御方法。
7. 請求項６に記載の工作機の工具位置決め制御方法において、前記ワーク揺動角θ _２ が算出された後、逆運動学方程式に基づき、前記第１垂直軸回りで回転する第１水平軸の回転角θ_３を算出する工程をさらに備えることを特徴とする工作機の工具位置決め制御方法。
8. 請求項７に記載の工作機の工具位置決め制御方法において、前記回転角θ_３は、
   

   

   ただし、
   

   

   に基づき算出されることを特徴とする工作機の工具位置決め制御方法。
9. 請求項６または８に記載の工作機の工具位置決め制御方法において、前記ワーク揺動角θ _２ が算出された後、逆運動学方程式に基づき、前記第２水平軸回りで揺動するスピンドル軸のスピンドル揺動角θ_６を算出する工程をさらに備えることを特徴とする工作機の工具位置決め制御方法。
10. 請求項９に記載の工作機の工具位置決め制御方法において、前記スピンドル揺動角θ_６は、
    

    

    ただし、
    

    

    に基づき算出されることを特徴とする工作機の工具位置決め制御方法。
11. 請求項１０に記載の工作機の工具位置決め制御方法において、前記回転角θ _３ および前記スピンドル揺動角θ _６ が算出された後、逆運動学方程式に基づき、前記第２垂直軸回りで回転する第２水平軸の回転角θ_５を算出する工程をさらに備えることを特徴とする工作機の工具位置決め制御方法。
12. 請求項１１に記載の工作機の工具位置決め制御方法において、前記回転角θ_５は、
    

    

    ただし、
    

    

    に基づき算出されることを特徴とする工作機の工具位置決め制御方法。

Images