(57)【特許請求の範囲】
【請求項１】
基台から垂直に立ち上がる第１および第２垂直軸の間隔ｄ４を示す軸間距離データを取得する工程と、第１垂直軸回りで回転自在に支持される第１水平軸の高さｄ３を示す第１高さデータを取得する工程と、第２垂直軸回りで回転自在に支持される第２水平軸の高さｄ５を示す第２高さデータを取得する工程と、第１水平軸回りで揺動自在に支持されるワーク台に対して第１水平軸からワーク固定面までの距離ｄ１を示すワーク揺動半径データを取得する工程と、ワーク固定面に設定されるワーク座標系上でワークの加工位置（ｐｘ，ｐｙ，ｐｚ）を特定する加工位置データを取得する工程と、第２水平軸回りで揺動自在に支持されるスピンドル軸の姿勢をワーク座標系上で特定する工具姿勢データを取得する工程と、取得した軸間距離データ、第１および第２高さデータ、ワーク揺動半径データ、加工位置データおよび工具姿勢データを用いて、逆運動学方程式に基づき第２垂直軸および第２水平軸の交点から加工位置までの距離ｄ７を算出する工程とを備えることを特徴とする工作機の工具位置決め制御方法。
【請求項２】
請求項１に記載の工作機の工具位置決め制御方法において、前記ワーク座標系のｚ軸は、前記第１垂直軸および第１水平軸の交点で第１水平軸に直交することを特徴とする工作機の工具位置決め制御方法。
【請求項３】
請求項２に記載の工作機の工具位置決め制御方法において、前記スピンドル軸の姿勢を特定するにあたって、前記加工位置を基準に、前記第２垂直軸および第２水平軸の交点から第２水平軸に垂直に延びる前記スピンドル軸の軸心に沿って特定されるｚ軸単位ベクトル（ｂｘ，ｂｙ，ｂｚ）が用いられることを特徴とする工作機の工具位置決め制御方法。
【請求項４】
請求項３に記載の工作機の工具位置決め制御方法において、前記距離ｄ７は、
【数１】
0003576419.tif 000068
ただし、
【数２】
0003576419.tif 000069
に基づき算出されることを特徴とする工作機の工具位置決め制御方法。
【請求項５】
請求項４に記載の工作機の工具位置決め制御方法において、前記距離ｄ７が算出された後、逆運動学方程式に基づき、前記第１水平軸回りで揺動する前記ワーク台のワーク揺動角θ２を算出する工程をさらに備えることを特徴とする工作機の工具位置決め制御方法。
【請求項６】
請求項５に記載の工作機の工具位置決め制御方法において、前記ワーク揺動角θ２は、
【数３】
0003576419.tif 000070
ただし、
【数４】
0003576419.tif 000071
に基づき算出されることを特徴とする工作機の工具位置決め制御方法。
【請求項７】
請求項６に記載の工作機の工具位置決め制御方法において、前記ワーク揺動角θ２が算出された後、逆運動学方程式に基づき、前記第１垂直軸回りで回転する第１水平軸の回転角θ３を算出する工程をさらに備えることを特徴とする工作機の工具位置決め制御方法。
【請求項８】
請求項７に記載の工作機の工具位置決め制御方法において、前記回転角θ３は、
【数５】
0003576419.tif 000072
ただし、
【数６】
0003576419.tif 000073
に基づき算出されることを特徴とする工作機の工具位置決め制御方法。
【請求項９】
請求項６または８に記載の工作機の工具位置決め制御方法において、前記ワーク揺動角θ２が算出された後、逆運動学方程式に基づき、前記第２水平軸回りで揺動するスピンドル軸のスピンドル揺動角θ６を算出する工程をさらに備えることを特徴とする工作機の工具位置決め制御方法。
【請求項１０】
請求項９に記載の工作機の工具位置決め制御方法において、前記スピンドル揺動角θ６は、
【数７】
0003576419.tif 000074
ただし、
【数８】
0003576419.tif 000075
に基づき算出されることを特徴とする工作機の工具位置決め制御方法。
【請求項１１】
請求項１０に記載の工作機の工具位置決め制御方法において、前記回転角θ３および前記スピンドル揺動角θ６が算出された後、逆運動学方程式に基づき、前記第２垂直軸回りで回転する第２水平軸の回転角θ５を算出する工程をさらに備えることを特徴とする工作機の工具位置決め制御方法。
【請求項１２】
請求項１１に記載の工作機の工具位置決め制御方法において、前記回転角θ５は、
【数９】
0003576419.tif 000076
ただし、
【数１０】
0003576419.tif 000077
に基づき算出されることを特徴とする工作機の工具位置決め制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は工作機の工具位置決め制御方法に関する。
【０００２】
【従来の技術】
例えば特開平９−２２５７６１号公報に開示されるように、ワークが固定されるワーク台の姿勢を変化させるとともに、工具が装着されるスピンドル軸の姿勢を変化させる工作機は知られている。この工作機は、共通の基台上で１対の垂直軸回りに回転する第１および第２支持部材を備える。第１支持部材には、水平軸回りに揺動自在にワーク台が支持される。その一方で、第２支持部材には、水平軸回りで揺動自在にスピンドル軸が支持される。こうした軸回りの回転や揺動を利用した工作機では、直交３軸に案内されて工具が移動する既存の工作機に比べて、小型化や軽量化を容易に実現することができる。
【０００３】
【発明が解決しようとする課題】
このような工作機では、垂直軸回りで回転する第１および第２支持部材の回転量や、水平軸回りで揺動するワーク台およびスピンドル軸の揺動量に基づき、ワークに対する工具の位置が特定される。しかしながら、こうして第１および第２支持部材が回転したり、ワーク台およびスピンドル軸が揺動したりすると、ワークに対する工具の姿勢は変化してしまう。こうした工作機で、ワークに対する工具の位置および姿勢を同時に特定することができる工具位置決め制御方法はいまのところ確立されていない。
【０００４】
本発明は、上記実状に鑑みてなされたもので、１対の垂直軸回りで各々姿勢を変化させる１対の水平軸にワーク台およびスピンドル軸を個別に支持させた際に、スピンドル軸に装着された工具の姿勢を特定しつつワークに対して確実に工具を位置決めすることができる工作機の工具位置決め制御方法を提供することを目的とする。
【０００５】
【課題を解決するための手段】
上記目的を達成するために、本発明によれば、基台から垂直に立ち上がる第１および第２垂直軸の間隔ｄ４  を示す軸間距離データを取得する工程と、第１垂直軸回りで回転自在に支持される第１水平軸の高さｄ３  を示す第１高さデータを取得する工程と、第２垂直軸回りで回転自在に支持される第２水平軸の高さｄ５  を示す第２高さデータを取得する工程と、第１水平軸回りで揺動自在に支持されるワーク台に対して第１水平軸からワーク固定面までの距離ｄ１  を示すワーク揺動半径データを取得する工程と、ワーク固定面に設定されるワーク座標系上でワークの加工位置（ｐｘ  ，ｐｙ  ，ｐｚ  ）を特定する加工位置データを取得する工程と、第２水平軸回りで揺動自在に支持されるスピンドル軸の姿勢をワーク座標系上で特定する工具姿勢データを取得する工程と、取得した軸間距離データ、第１および第２高さデータ、ワーク揺動半径データ、加工位置データおよび工具姿勢データを用いて、逆運動学方程式に基づき第２水平軸から加工位置までの距離ｄ７  を算出する工程とを備えることを特徴とする工作機の工具位置決め制御方法が提供される。
【０００６】
かかる工具位置決め制御方法によれば、ワーク固定面に設定されるワーク座標系に従って、ワークの加工位置やスピンドル軸の姿勢が特定されると、第２水平軸から加工位置までの距離ｄ７  が算出されることができる。したがって、第２水平軸上でスピンドル軸の軸方向進退移動を許容すれば、そういった進退移動を通じてスピンドル軸の先端やその先端に装着された工具を正確に加工位置に位置決めすることができる。このとき、ワーク固定面に固定されるワークに対してスピンドル軸の姿勢は変化しない。しかも、スピンドル軸の軸心は常に加工位置を向けられる。
【０００７】
前記ワーク座標系のｚ軸は、前記第１垂直軸および第１水平軸の交点で第１水平軸に直交することが望ましい。このようにワーク座標系のｚ軸が設定されれば、工作機に対して構築される運動学モデルをできる限り単純化することが可能となる。その結果、逆運動学方程式は簡素化され、逆運動学方程式の解を求めやすくなる。しかも、解を求めるにあたって計算処理の負担は軽減される。
【０００８】
しかも、前記スピンドル軸の姿勢を特定するにあたって、前記加工位置を基準に、前記第２垂直軸および第２水平軸の交点から第２水平軸に垂直に延びる前記スピンドル軸の軸心に沿って特定されるｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）が用いられることが望ましい。こうしたｚ軸単位ベクトルが用いられれば、確実に逆運動学方程式を解くことが可能となる。加えて、工作機に対して構築される運動学モデルをさらに単純化することが可能となる。したがって、さらに簡単に逆運動学方程式の解を求めることができ、しかも、解を求めるにあたって計算処理の負担は軽減される。
【０００９】
こうした条件の下では、前記距離ｄ７  は、例えば、
【数１１】
0003576419.tif 000002
ただし、
【数１２】
0003576419.tif 000003
に基づき算出されることができる。
【００１０】
この種の工具位置決め制御方法は、逆運動学方程式に基づき、前記第１水平軸回りで揺動する前記ワーク台のワーク揺動角θ２  や、前記第１垂直軸回りで回転する第１水平軸の回転角θ３  、前記第２水平軸回りで揺動するスピンドル軸のスピンドル揺動角θ６  、前記第２垂直軸回りで回転する第２水平軸の回転角θ５  を算出する工程をさらに備えることが望ましい。
【００１１】
こうしたワーク揺動角θ２  は、例えば、
【数１３】
0003576419.tif 000004
ただし、
【数１４】
0003576419.tif 000005
に基づき算出されればよい。
【００１２】
前記回転角θ３  は、例えば、
【数１５】
0003576419.tif 000006
ただし、
【数１６】
0003576419.tif 000007
に基づき算出されればよい。
【００１３】
前記スピンドル揺動角θ６  は、例えば、
【数１７】
0003576419.tif 000008
ただし、
【数１８】
0003576419.tif 000009
に基づき算出されればよい。
【００１４】
前記回転角θ５  は、例えば、
【数１９】
0003576419.tif 000010
ただし、
【数２０】
0003576419.tif 000011
に基づき算出されればよい。
【００１５】
なお、本発明に係る工具位置決め制御方法が適用される工作機では、スピンドル軸の先端に、スピンドル軸の軸心回りで回転するドリルやエンドミルといった一般の工具が装着されてもよく、スピンドル軸の軸心に沿ってレーザビームを照射するレーザ照射源やウォータージェットを発射するウォータージェット源が支持されてもよい。
【００１６】
【発明の実施の形態】
以下、添付図面を参照しつつ本発明の一実施形態を説明する。
【００１７】
図１は、本発明に係る工作機の工具位置決め制御方法が適用される工作機を概略的に示す。この工作機１０は、工具１１が装着されるスピンドル軸１２と、スピンドル軸１２に装着された工具１１に対してワークを向かい合わせに保持するワーク台１３とを備える。ワーク台１３に保持されるワークを加工するにあたって、ワークは、ワーク台１３の正面に形成されるワーク固定面１４に固定される。スピンドル軸１２は軸心回りに高速で工具１１を回転させることができる。
【００１８】
スピンドル軸１２はクイル１５に進退自在に挿入される。クイル１５には、スピンドル軸１２に平行に延びるねじ軸１６が支持される。スピンドル軸１２の後端には、ねじ軸１６に噛み合うナット部材１７が固定される。駆動モータ１８の働きを通じてねじ軸１６が軸心回りに回転すると、ねじ軸１６上をナット部材１７が移動し、その結果、スピンドル軸１２の進退移動が実現される。この進退移動によって、スピンドル軸１２すなわち工具１１は、ワーク固定面１４に向かって前進したりワーク固定面１４から後退したりすることができる。スピンドル軸１２の進退移動量は、周知のとおり、駆動モータ１８の回転量に基づいて決定される。
【００１９】
ワーク台１３は、第１水平軸２０回りで揺動自在に第１支持部材２１に支持される。第１水平軸２０には駆動モータ（例えばサーボモータ）２２が連結される。この駆動モータ２２の働きを通じて、ワーク台１３は第１水平軸２０回りで揺動することができる。ワーク台１３の揺動角すなわちワーク揺動角は駆動モータ２２の回転量によって特定されることができる。
【００２０】
しかも、第１支持部材２１は、基台２３から垂直に立ち上がる第１垂直軸２４回りで回転することができる。第１支持部材２１の回転は駆動モータ（図示せず）の働きによって実現される。第１垂直軸２４回りで回転する第１支持部材２１すなわち第１水平軸２０の回転角は駆動モータの回転量によって特定されることができる。
【００２１】
一方で、前述のクイル１５は、第２水平軸２６回りで揺動自在に第２支持部材２７に支持される。第２水平軸２６には駆動モータ（例えばサーボモータ）２８が連結される。この駆動モータ２８の働きを通じて、クイル１５すなわちスピンドル軸１２は第２水平軸２６回りで揺動することができる。スピンドル軸１２の揺動角すなわちスピンドル揺動角は駆動モータ２８の回転量によって特定されることができる。
【００２２】
しかも、第２支持部材２７は、第１垂直軸２４に平行に基台２３から立ち上がる第２垂直軸２９回りで回転することができる。第２支持部材２７の回転は駆動モータ（図示せず）の働きによって実現される。第２垂直軸２９回りで回転する第２支持部材２７すなわち第２水平軸２６の回転角は駆動モータの回転量によって特定されることができる。
【００２３】
図２に示されるように、ワーク台１３や工具１１すなわちスピンドル軸１２の姿勢はコントローラ３１によって制御される。このコントローラ３１は、ワーク固定面に設定されるワーク座標系ｘｙｚ上でワークの加工位置（ｐｘ  ，ｐｙ  ，ｐｚ  ）を特定する加工位置データや、ワーク座標系ｘｙｚ上で工具１１すなわちスピンドル軸１２の姿勢を特定する工具姿勢データを取得するデータ取得回路３２を備える。このデータ取得回路３２には、例えばＮＣプログラムに記述される加工位置データや工具姿勢データが取り込まれればよい。
【００２４】
演算処理回路（ＭＰＵ）３３は、後述するように、データ取得回路３２から供給される加工位置データおよび工具姿勢データに基づき、スピンドル軸１２の進退移動量ｄ７  を規定する駆動モータ１８の回転量や、ワーク揺動角θ２  を規定する駆動モータ２２の回転量、第１および第２支持部材２１、２７の回転角θ３  、θ５  を規定する駆動モータ３５、３６の回転量、スピンドル揺動角θ６  を規定する駆動モータ２８の回転量を算出する。この算出にあたって、ＭＰＵ３３は、第１および第２垂直軸２４、２９の間隔ｄ４  を示す軸間距離データや、第１および第２水平軸２０、２６の高さｄ３  、ｄ５  を示す第１および第２高さデータ、第１水平軸２０からワーク固定面１４すなわちワーク座標系ｘｙｚの原点までの距離ｄ１  を示すワーク揺動半径データをメモリ３７から読み出す。メモリ３７には、間隔ｄ４  や高さｄ３  、ｄ５  、距離ｄ１  の実測値が予め格納されていればよい。
【００２５】
その一方で、各駆動モータ１８、２２、３５、３６、２８の回転量は、駆動モータごとに設けられるエンコーダ３８〜４２によって検出されることができる。検出された各駆動モータ１８、２２、３５、３６、２８の回転量に基づいて、ＭＰＵ３３は、ワーク固定面１４に設定されたワーク座標系ｘｙｚに基づき工具１１の位置や姿勢を算出する。表示器駆動回路４３は、算出された工具１１の位置や姿勢を表示器４４に表示させることができる。
【００２６】
いま、工作機１０でワークを加工する場面を想定する。まず、作業者は、ワーク固定面１４にワークを固定する。この固定によって、ワークはワーク座標系ｘｙｚに取り込まれる。このとき、ワーク座標系ｘｙｚのｚ軸は、第１垂直軸２４および第１水平軸２０の交点で第１水平軸２０に直交する。したがって、第１水平軸２０回りでワーク台１３が揺動するとワーク座標系ｘｙｚはその揺動に追随することとなる。
【００２７】
作業者は、ワーク座標系ｘｙｚ上でワークの加工位置（ｐｘ  ，ｐｙ  ，ｐｚ  ）すなわち工具１１の先端位置をデータ取得回路３２に取り込ませる。作業者は、同時に、データ取得回路３２を通じて工具１１の姿勢を指定する。工具１１の姿勢を特定するにあたっては、加工位置（ｐｘ  ，ｐｙ  ，ｐｚ  ）を基準にスピンドル軸１２の軸心に沿って特定されるｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）が用いられればよい。このとき、スピンドル軸１２の軸心は、第２垂直軸２９および第２水平軸２６の交点から第２水平軸２６に垂直に延びる。ワーク固定面１４に対するスピンドル軸１２の姿勢が変化するとｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）はその変化に追随する。
【００２８】
こうして加工位置や工具１１の姿勢が特定されると、ＭＰＵ３３は、次式に従ってスピンドル軸１２の進退移動量ｄ７  を算出する。
【００２９】
【数２１】
0003576419.tif 000012
ただし、
【数２２】
0003576419.tif 000013
こうして進退移動量ｄ７  が算出されると、ＭＰＵ３３は、次式に従ってワーク揺動角θ２  を算出する。
【００３０】
【数２３】
0003576419.tif 000014
ただし、
【数２４】
0003576419.tif 000015
ワーク揺動角θ２  が算出されると、ＭＰＵ３３は、次式に従って第１支持部材２１すなわち第１水平軸２０の回転角θ３  を算出する。
【００３１】
【数２５】
0003576419.tif 000016
ただし、
【数２６】
0003576419.tif 000017
同時に、ＭＰＵ３３は、次式に従ってスピンドル揺動角θ６  を算出する。
【００３２】
【数２７】
0003576419.tif 000018
ただし、
【数２８】
0003576419.tif 000019
こうしてスピンドル揺動角θ６  が算出されると、続いてＭＰＵ３３は、次式に従って第２支持部材２７すなわち第２水平軸２６の回転角θ５  を算出する。
【００３３】
【数２９】
0003576419.tif 000020
ただし、
【数３０】
0003576419.tif 000021
ＭＰＵ３３は、算出された進退移動量ｄ７  、ワーク揺動角θ２  、第１および第２支持部材２１、２７の回転角θ３  、θ５  およびスピンドル揺動角θ６  を特定する駆動指令信号を各駆動モータ１８、２２、３５、３６、２８に供給する。その結果、駆動モータ１８、２２、３５、３６、２８が作動すると、工具１１の先端はワーク上の加工位置（ｐｘ  ，ｐｙ  ，ｐｚ  ）に位置決めされる。しかも、このとき、工具１１の姿勢は、ワーク座標系ｘｙｚで指定されるｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）によって特定される。前述したＭＰＵ３３の処理によれば、１つのｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）が維持される限り、工具１１を移動させても加工位置（ｐｘ  ，ｐｙ  ，ｐｚ  ）に対する工具１１の姿勢は変化しない。
【００３４】
ここで、前述したＭＰＵ３３の算出処理を簡単に検証する。まず、ワーク固定面に対して基準座標系を設定し、ロボット工学に基づく運動学モデル（リンクモデル）を構築する。例えば図３に示されるように、ワーク固定面１４に対して基準座標系Ｘ０  Ｚ０  が設定されると、ワーク台１３と第１支持部材２１とのジョイントすなわち第１水平軸２０回りでは第１座標系Ｘ１  Ｚ１  および第２座標系Ｘ２  Ｚ２  が設定されることができる。同様に、第１支持部材２１と基台２３とのジョイントすなわち第１垂直軸２４回りでは第３座標系Ｘ３  Ｚ３  が規定され、基台２３と第２支持部材２７とのジョイントすなわち第２垂直軸２９回りでは第４座標系Ｘ４  Ｚ４  が規定されることができる。さらに、第２支持部材２７とクイル１５とのジョイントすなわち第２水平軸２６回りでは第５座標系Ｘ５  Ｚ５  および第６座標系Ｘ６  Ｚ６  が規定されることができる。最終的に、クイル１５に挿入されるスピンドル軸１２の先端では第７座標系Ｘ７  Ｚ７  が規定される。設定された基準座標系Ｘ０  Ｚ０  に基づけば、第７座標系Ｘ７  Ｚ７  の原点すなわち加工位置はベクトル（ｐｘ  ，ｐｙ  ，ｐｚ  ）によって特定されることができる。
【００３５】
特定された運動学モデルに基づき、各座標系ごとにパラメータθｎ  （Ｚｎ−１  回りの回転角）、ｄｎ  （Ｚｎ−１  に沿った距離）、ａｎ  （Ｘｎ  に沿った距離）、αｎ  （Ｘｎ  回りの回転角）を特定すると次表が得られる。
【００３６】
【表１】
0003576419.tif 000022
得られたパラメータθｎ  、ｄｎ  、ａｎ  、αｎ  に基づいて各座標系Ｘ１  Ｚ１  〜Ｘ７  Ｚ７  ごとに変換行列Ａｎ  を特定すると、
【数３１】
0003576419.tif 000023
ただし、
【数３２】
0003576419.tif 000024
が得られる。その結果、基準座標系Ｘ０  Ｚ０  に対する工具１１の姿勢や位置は、運動学方程式に従い、
【数３３】
0003576419.tif 000025
ただし、
【数３４】
0003576419.tif 000026
で表現されることができる。したがって、以上の運動学方程式に従えば、エンコーダ３８〜４２で検出される進退移動量ｄ７  、ワーク揺動角θ２  、第１および第２支持部材２１、２７の回転角θ３  、θ５  およびスピンドル揺動角θ６  に基づき、加工位置（ｐｘ  ，ｐｙ  ，ｐｚ  ）や、工具１１の姿勢を特定するｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）を算出することができる。算出された加工位置（ｐｘ  ，ｐｙ  ，ｐｚ  ）やｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）の座標値は表示器４４に表示されればよい。その結果、作業者は、加工位置や工具の姿勢を確認しながら工作機１０を操作することが可能となる。
【００３７】
続いて、得られた運動学方程式に基づいて逆運動学方程式を導き出す。まず、加工位置のｘ座標値ｐｘ  に関する方程式を以下のように変形する。
【００３８】
【数３５】
0003576419.tif 000027
両辺を２乗すると、
【数３６】
0003576419.tif 000028
が得られる。同様に、加工位置のｙ座標値ｐｙ  およびｚ座標値ｐｚ  に関する方程式を以下のように変形する。
【００３９】
【数３７】
0003576419.tif 000029
【数３８】
0003576419.tif 000030
【数３９】
0003576419.tif 000031
【数４０】
0003576419.tif 000032
式［数３６］および式［数４０］の和は、
【数４１】
0003576419.tif 000033
として求められる。ここで、
【数４２】
0003576419.tif 000034
を適用すると、
【数４３】
0003576419.tif 000035
が得られる。さらに、
【数４４】
0003576419.tif 000036
を適用すると、
【数４５】
0003576419.tif 000037
が得られる。
【００４０】
得られた式［数４５］を展開し、ｄ７  に関して整理すると、
【数４６】
0003576419.tif 000038
が得られる。ここで、式［数４６］を以下のように置換する。
【００４１】
【数４７】
0003576419.tif 000039
ただし、
【数４８】
0003576419.tif 000040
ｄ７  に関して式［数４７］を解くと、
【数４９】
0003576419.tif 000041
および
【数５０】
0003576419.tif 000042
が得られる。ここでは、工作機１０の加工時動作範囲を考慮した結果、式［数４９］を選択することとする。式［数４９］は、０゜＜θ５  ＜１８０゜の条件の下で適用されることができる。
【００４２】
次に、加工位置のｘ座標値ｐｘ  に関する方程式にＳ２  を掛け合わせ、同時に、加工位置のｚ座標値ｐｚ  に関する方程式にＣ２  を掛け合わせる。
【００４３】
【数５１】
0003576419.tif 000043
得られた２つの方程式の和を求めると、
【数５２】
0003576419.tif 000044
が得られる。ここで、得られた式［数５２］に以下の三角法的代入を施す。
【００４４】
【数５３】
0003576419.tif 000045
ただし、
【数５４】
0003576419.tif 000046
すると、
【数５５】
0003576419.tif 000047
が得られる。この式を変形すると、
【数５６】
0003576419.tif 000048
が得られる。φ−θ２  について解くと、
【数５７】
0003576419.tif 000049
が得られ、したがって、ワーク揺動角θ２  は以下のとおりとなる。
【００４５】
【数５８】
0003576419.tif 000050
【数５９】
0003576419.tif 000051
以上の算出過程で用いられる２変数逆正接関数ａｔａｎ２（ｙ，ｘ）によれば、周知のとおり、ｘ、ｙの符号を考慮しつつｙ／ｘの逆正接関数が求められる。
【００４６】
図４には、作業者の視点で見たワーク揺動角と、工作機１０の運動学モデルから見たワーク揺動角θ２  と、式［数５８］で算出されるワーク揺動角θ２  との関係が示される。作業者の視点に基づけば、第１水平軸２０および第１垂直軸２４の交点と、第２水平軸２６および第２垂直軸２９の交点とを結ぶ直線に対してワーク固定面１４が直交する際、言い換えれば、そういった直線上でワーク座標系ｘｙｚのｚ軸が特定される際にワーク揺動角は「０゜」を示す。図４から明らかなように、例えば作業者の視点でワーク揺動角が１８０゜のとき、運動学モデルから見るとワーク揺動角θ２  ＝９０゜となる。このとき、式［数５８］で算出されるワーク揺動角θ２  は対応する。その一方で、作業者の視点でワーク揺動角が−１８０゜のとき、運動学モデルで見るとワーク揺動角θ２  ＝−２７０゜となる。このとき、式［数５８］で算出されるワーク揺動角θ２  ＝９０゜である。そこで、式［数５８］の出力が＋９０゜を超えた場合には、式［数５８］の算出値に−３６０゜を加えれば、ワーク揺動角θ２  の動作範囲−２７０゜〜＋９０゜に式［数５８］の出力を対応させることができる。その結果、式［数５８］のみで、ワーク揺動角θ２  の動作範囲をカバーすることが可能となる。
【００４７】
次に、加工位置のｚ座標値ｐｚ  に関する方程式にＳ２  を掛け合わせ、同時に、加工位置のｘ座標値ｐｘ  に関する方程式にＣ２  を掛け合わせる。
【００４８】
【数６０】
0003576419.tif 000052
得られた２つの方程式の差を求めると、
【数６１】
0003576419.tif 000053
が得られる。また、加工位置のｙ座標値ｐｙ  に関する方程式をＣ３  に関して解くと、
【数６２】
0003576419.tif 000054
が得られ、したがって、回転角θ３  は以下のとおりとなる。
【００４９】
【数６３】
0003576419.tif 000055
さらに、ｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）のｘ座標値ｂｘ  に関する方程式にＳ２  を掛け合わせ、同時に、同単位ベクトルのｚ座標値ｂｚ  に関する方程式にＣ２  を掛け合わせる。
【００５０】
【数６４】
0003576419.tif 000056
得られた２つの方程式の和を求めると、
【数６５】
0003576419.tif 000057
が得られる。また、
【数６６】
0003576419.tif 000058
であることから、スピンドル揺動角θ６  は以下のとおりとなる。
【００５１】
【数６７】
0003576419.tif 000059
ただし、０゜＜θ６  ＜１８０゜とする。
【００５２】
さらにまた、ｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）のｙ座標値ｂｙ  に関する方程式に次式を適用し、Ｓ３５に関して解く。
【００５３】
【数６８】
0003576419.tif 000060
すると、
【数６９】
0003576419.tif 000061
が得られる。同様に、ｚ軸単位ベクトル（ｂｘ  ，ｂｙ  ，ｂｚ  ）のｘ座標値ｂｘ    に関する方程式およびｚ座標値ｂｚ  に関する方程式に次式を適用し、Ｓ３５に関して解く。
【００５４】
【数７０】
0003576419.tif 000062
すると、
【数７１】
0003576419.tif 000063
および
【数７２】
0003576419.tif 000064
が得られる。したがって、θ３５は以下のとおりとなる。
【００５５】
【数７３】
0003576419.tif 000065
式［数７１］および式［数７２］は、前述のように算出されるθ２  の値に基づいて選択されればよい。例えば、ｓｉｎθ２  の絶対値が０．５以下であれば式［数７１］を選択し、ｓｉｎθ２  の絶対値が０．５を超える場合には式［数７２］を選択するといった具合である。こうした選択によれば、式［数７１］および式［数７２］の分母の大きさを十分に確保することができる。しかも、前述の条件０゜＜θ６  ＜１８０゜を考慮すれば、ｓｉｎθ４  ＝０になることはない。
【００５６】
図５には、作業者の視点で見た回転角θ３５と、工作機１０の運動学モデルから見た回転角θ３５と、式［数７３］で算出される回転角θ３５との関係が示される。図５から明らかなように、例えば作業者の視点で回転角θ３５は−１８０゜＜θ３５＜＋１８０゜の範囲で変動し、運動学モデルから見ると回転角θ３５は０゜＜θ３５＜３６０゜の範囲で変動する。その一方で、式［数７３］で算出される回転角θ３５は−１８０゜＜θ３５＜＋１８０゜の範囲で変動する。したがって、前述と同様に、式［数７３］の出力が０゜より小さい場合には、式［数７３］の算出値に＋３６０゜を加えれば、要求される範囲に回転角θ３５の範囲を対応させることが可能となる。
【００５７】
第１および第２支持部材２１、２７の回転角θ３  、θ５  および前述のθ３５の間には次式の関係が成立することから、
【数７４】
0003576419.tif 000066
次式に基づいて回転角θ５  は算出されることができる。
【００５８】
【数７５】
0003576419.tif 000067
なお、以上のような運動学方程式や逆運動学方程式は、図３に示される運動学モデルに従って導き出されるもので、運動学モデルの構築の仕方に応じて運動学方程式や逆運動学方程式は修正されることができる。
【００５９】
【発明の効果】
以上のように本発明によれば、１対の垂直軸回りで各々姿勢を変化させる１対の水平軸にワーク台およびスピンドル軸を個別に支持させた際に、スピンドル軸に装着された工具の姿勢を特定しつつワークに対して確実に工具を位置決めすることができる。
【図面の簡単な説明】
【図１】工作機の全体構成を概略的に示す斜視図である。
【図２】工作機の制御系を概略的に示すブロック図である。
【図３】工作機の運動学モデルを示す図である。
【図４】逆運動学方程式に基づく算出結果と、運動学モデルに基づくワーク揺動角θ２  の動作範囲との関係を示す図である。
【図５】逆運動学方程式に基づく算出結果と、運動学モデルに基づく回転角θ３５の範囲との関係を示す図である。
【符号の説明】
１０  工作機、１２  スピンドル軸、１３  ワーク台、１４  ワーク固定面、２０  第１水平軸、２３  基台、２４  第１垂直軸、２６  第２水平軸、２９  第２垂直軸。
【図１】
0003576419.tif 000078
【図２】
0003576419.tif 000079
【図３】
0003576419.tif 000080
【図４】
0003576419.tif 000081
【図５】
0003576419.tif 000082