(57)【要約】【課題】機構的制約のあるパラレルリンク機構を好適に制御する制御装置、制御方法及びシステムを提供する。【解決手段】パラレルリンク機構１０の制御装置３６は、機構先端部２２の位置及び姿勢の組み合わせを各々が含む複数の指令位置を作成する指令位置作成部４０と、複数の指令位置の間の補間動作における機構先端部２２の位置又は姿勢についての禁止領域を示す数理モデルを、複数の指令位置の各々における機構先端部２２の位置、及び機構先端部２２の姿勢を表す軸線との距離が最短となるスレーブ回転軸２０上の点とマスタ回転軸との距離に基づいて作成するモデル作成部４２と、作成された数理モデルに基づいて、補間動作中において機構先端部２２の位置又は姿勢が禁止領域に入らないように、機構先端部２２の位置及び姿勢についての補間指令を作成する補間指令部４４とを有する。【選択図】図１
2018196919.tif 000002
【特許請求の範囲】
【請求項１】
パラレルリンク機構を制御する制御装置であって、該パラレルリンク機構は、
並列する複数のリンクの動作により移動可能な可動部材と、該可動部材に設けられたマスタ回転軸と、該マスタ回転軸と異なる方向に延びるとともに、該マスタ回転軸の回転に応じて方向が変化するスレーブ回転軸と、該スレーブ回転軸に取り付けられた機構先端部とを有し、前記機構先端部の姿勢を表す軸線との距離が最短となる前記スレーブ回転軸上の点が前記マスタ回転軸上に位置せず、
前記制御装置は、
前記機構先端部の位置及び姿勢の組み合わせを各々が含む複数の指令位置を作成する指令位置作成部と、
前記複数の指令位置の間の補間動作における前記機構先端部の位置又は姿勢についての禁止領域を示す数理モデルを、前記機構先端部の位置、及び前記点と前記マスタ回転軸との距離に基づいて作成するモデル作成部と、
前記数理モデルに基づいて、前記補間動作中において前記機構先端部の位置又は姿勢が前記禁止領域に入らないように、前記機構先端部の位置及び姿勢についての補間指令を作成する補間指令部と、を有する、
制御装置。
【請求項２】
前記補間指令部は、前記補間動作において、前記機構先端部の位置を線形的に補間するとともに、前記機構先端部の姿勢が前記禁止領域内に入らないように補間指令を作成する、請求項１に記載の制御装置。
【請求項３】
前記数理モデルは、前記機構先端部の位置を中心とする単位球面である、請求項１又は２に記載の制御装置。
【請求項４】
前記数理モデルは、前記マスタ回転軸の軸線を中心軸とし、前記点と前記マスタ回転軸との距離に等しい半径を有する無限長の円柱である、請求項１又は２に記載の制御装置。
【請求項５】
パラレルリンク機構を備えた産業機械と、該パラレルリンク機構を制御する制御装置とを有するシステムであって、
前記パラレルリンク機構は、
並列する複数のリンクの動作により移動可能な可動部材と、該可動部材に設けられたマスタ回転軸と、該マスタ回転軸と異なる方向に延びるとともに、該マスタ回転軸の回転に応じて方向が変化するスレーブ回転軸と、該スレーブ回転軸に取り付けられた機構先端部とを有し、前記機構先端部の姿勢を表す軸線との距離が最短となる前記スレーブ回転軸上の点が前記マスタ回転軸上に位置せず、
前記制御装置は、
前記機構先端部の位置及び姿勢の組み合わせを各々が含む複数の指令位置を作成する指令位置作成部と、
前記複数の指令位置の間の補間動作における前記機構先端部の位置又は姿勢についての禁止領域を示す数理モデルを、前記機構先端部の位置、及び前記点と前記マスタ回転軸との距離に基づいて作成するモデル作成部と、
前記数理モデルに基づいて、前記補間動作中において前記機構先端部の位置又は姿勢が前記禁止領域に入らないように、前記機構先端部の位置及び姿勢についての補間指令を作成する補間指令部と、を有する、
システム。
【請求項６】
パラレルリンク機構の制御方法であって、該パラレルリンク機構は、
並列する複数のリンクの動作により移動可能な可動部材と、該可動部材に設けられたマスタ回転軸と、該マスタ回転軸と異なる方向に延びるとともに、該マスタ回転軸の回転に応じて方向が変化するスレーブ回転軸と、該スレーブ回転軸に取り付けられた機構先端部とを有し、前記機構先端部の姿勢を表す軸線との距離が最短となる前記スレーブ回転軸上の点が前記マスタ回転軸上に位置せず、
前記制御方法は、
前記機構先端部の位置及び姿勢の組み合わせを各々が含む複数の指令位置を作成することと、
前記複数の指令位置の間の補間動作における前記機構先端部の位置又は姿勢についての禁止領域を示す数理モデルを、前記機構先端部の位置、及び前記点と前記マスタ回転軸との距離に基づいて作成することと、
前記数理モデルに基づいて、前記補間動作中において前記機構先端部の位置又は姿勢が前記禁止領域に入らないように、前記機構先端部の位置及び姿勢についての補間指令を作成することと、を含む、
制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、パラレルリンク機構の制御装置及び制御方法、並びに該パラレルリンク機構及び該制御装置を含むシステムに関する。
【背景技術】
【０００２】
比較的高速の動作速度を実現できる機構として、基礎部材と可動部材とを複数のリンクで接続するパラレルリンク機構を有する工作機械やロボットが周知である（例えば特許文献１参照）。またパラレルリンク機構は、３組のリンク機構によって３自由度を有するものが多いが、可動部材にさらに複数の回転部材を設け、姿勢変更の自由度をさらに高める構造も知られている（例えば特許文献２、３参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特公平０４−０４５３１０号公報
【特許文献２】特開２０１０−１８４３２８号公報
【特許文献３】特開２０１１−０５６６６１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
パラレルリンク機構の中には、３つのリンクによって可動に構成された可動部材、該可動部材に接続されたマスタ回転軸、及び該マスタ回転軸とは異なる方向に延びるスレーブ回転軸を有し、該スレーブ回転軸にハンドや工具を取り付けて所定の作業を行うタイプのものがある。ここで、スレーブ回転軸に対する工具の取り付け位置によっては、マスタ回転軸及びスレーブ回転軸の動作だけでは工具（例えば工具先端点）が存在し得ない領域（換言すれば、工具の位置及び姿勢の実現できない組み合わせ）が生じ得る。
【０００５】
通常、パラレルリンク機構の動作制御をする場合は、その制御装置が、離散的な指令位置のそれぞれについて、工具先端点の位置及び姿勢を求める。この際、各指令位置が位置及び姿勢の実現できない組み合わせを含まないように該指令位置が作成（計算）されるが、これらの指令位置の間（補間位置）では、単純な線形補間等をした場合、該補間位置が工具の位置及び姿勢の実現できない組み合わせを含んでしまう場合がある。するとパラレルリンク機構の制御が不可能となり、結果として該機構は急停止（非常停止）することになる。
【課題を解決するための手段】
【０００６】
本開示の一態様は、パラレルリンク機構を制御する制御装置であって、該パラレルリンク機構は、並列する複数のリンクの動作により移動可能な可動部材と、該可動部材に設けられたマスタ回転軸と、該マスタ回転軸と異なる方向に延びるとともに、該マスタ回転軸の回転に応じて方向が変化するスレーブ回転軸と、該スレーブ回転軸に取り付けられた機構先端部とを有し、前記機構先端部の姿勢を表す軸線との距離が最短となる前記スレーブ回転軸上の点が前記マスタ回転軸上に位置せず、前記制御装置は、前記機構先端部の位置及び姿勢の組み合わせを各々が含む複数の指令位置を作成する指令位置作成部と、前記複数の指令位置の間の補間動作における前記機構先端部の位置又は姿勢についての禁止領域を示す数理モデルを、前記機構先端部の位置、及び前記点と前記マスタ回転軸との距離に基づいて作成するモデル作成部と、前記数理モデルに基づいて、前記補間動作中において前記機構先端部の位置又は姿勢が前記禁止領域に入らないように、前記機構先端部の位置及び姿勢についての補間指令を作成する補間指令部と、を有する、制御装置である。
【０００７】
本開示の他の態様は、パラレルリンク機構を備えた産業機械と、該パラレルリンク機構を制御する制御装置とを有するシステムであって、前記パラレルリンク機構は、並列する複数のリンクの動作により移動可能な可動部材と、該可動部材に設けられたマスタ回転軸と、該マスタ回転軸と異なる方向に延びるとともに、該マスタ回転軸の回転に応じて方向が変化するスレーブ回転軸と、該スレーブ回転軸に取り付けられた機構先端部とを有し、前記機構先端部の姿勢を表す軸線との距離が最短となる前記スレーブ回転軸上の点が前記マスタ回転軸上に位置せず、前記制御装置は、前記機構先端部の位置及び姿勢の組み合わせを各々が含む複数の指令位置を作成する指令位置作成部と、前記複数の指令位置の間の補間動作における前記機構先端部の位置又は姿勢についての禁止領域を示す数理モデルを、前記機構先端部の位置、及び前記点と前記マスタ回転軸との距離に基づいて作成するモデル作成部と、前記数理モデルに基づいて、前記補間動作中において前記機構先端部の位置又は姿勢が前記禁止領域に入らないように、前記機構先端部の位置及び姿勢についての補間指令を作成する補間指令部と、を有する、システムである。
【０００８】
本開示のさらなる他の態様は、パラレルリンク機構の制御方法であって、該パラレルリンク機構は、並列する複数のリンクの動作により移動可能な可動部材と、該可動部材に設けられたマスタ回転軸と、該マスタ回転軸と異なる方向に延びるとともに、該マスタ回転軸の回転に応じて方向が変化するスレーブ回転軸と、該スレーブ回転軸に取り付けられた機構先端部とを有し、前記機構先端部の姿勢を表す軸線との距離が最短となる前記スレーブ回転軸上の点が前記マスタ回転軸上に位置せず、前記制御方法は、前記機構先端部の位置及び姿勢の組み合わせを各々が含む複数の指令位置を作成することと、前記複数の指令位置の間の補間動作における前記機構先端部の位置又は姿勢についての禁止領域を示す数理モデルを、前記機構先端部の位置、及び前記点と前記マスタ回転軸との距離に基づいて作成することと、前記数理モデルに基づいて、前記補間動作中において前記機構先端部の位置又は姿勢が前記禁止領域に入らないように、前記機構先端部の位置及び姿勢についての補間指令を作成することと、を含む、制御方法である。
【発明の効果】
【０００９】
本開示によれば、パラレルリンク機構の補間動作中でも、機構先端部の位置及び姿勢の実現できない組み合わせを指令してしまうことが確実に防止でき、該パラレルリンク機構の安定した操作・制御が可能となる。
【図面の簡単な説明】
【００１０】
【図１】一実施形態に係るパラレルリンク機構及び制御装置の一構成例を示す図である。
【図２】図１のパラレルリンク機構の先端部の模式図であり、スレーブ回転軸と工具軸との間のオフセットを表す線分とスレーブ回転軸との交点がマスタ回転軸上に位置する場合を例示する。
【図３】図１のパラレルリンク機構の先端部の模式図であり、スレーブ回転軸と工具軸との間のオフセットを表す線分とスレーブ回転軸との交点がマスタ回転軸上に位置しない場合を例示する。
【図４】図３の場合に、工具先端点が進入できない禁止領域を説明する図である。
【図５】禁止領域を円柱で表した例を説明する図である。
【図６】禁止領域を傾斜させた状態を示す図である。
【図７】図６の場合に、２つの指令位置間を線形補間した例を説明する図である。
【図８】パラレルリンク機構の要部を模式的に示す図である。
【図９】禁止領域を単位球面で表した例を説明する図である。
【図１０】図９の単位球面を用いて２つの指令位置間を線形補間した例を説明する図である。
【図１１】内分比を利用して適切な工具方向を求める例を説明する図である。
【発明を実施するための形態】
【００１１】
図１は、本開示の好適な実施形態に係る制御装置の制御対象であるパラレルリンク機構の一構成例を示す概略図である。パラレルリンク機構１０は、並列する複数のリンク１４ａ−１４ｃの動作により移動可能な可動部材１６と、可動部材１６に設けられたマスタ回転軸１８と、マスタ回転軸１８と異なる方向に延びるとともに、マスタ回転軸の回転に応じて方向が変化するスレーブ回転軸２０と、スレーブ回転軸２０に取り付けられた機構先端部（エンドエフェクタ）２２とを有し、機構先端部２２の姿勢を表す軸線４６との距離が最短となるスレーブ回転軸２０上の点４９がマスタ回転軸１８上に位置しない構造となっている（後述する図３参照）。
【００１２】
図示例では、パラレルリンク機構１０は、架台（図示せず）等に固定されたプレート状の基礎部材１２と、各々が駆動モータ等（図示せず）の駆動装置によって伸縮自在（長さが可変）であるとともに、基礎部材１２に一端が接続された３つの直動リンク（送り軸）１４ａ、１４ｂ及び１４ｃと、各直動リンクの他端に接続された可動部材１６と、可動部材１６に設けられたマスタ回転軸１８と、マスタ回転軸１８と異なる方向に延びるとともに、マスタ回転軸１８の回転に応じて方向が変化するスレーブ回転軸２０と、スレーブ回転軸２０に取り付けられてスレーブ回転軸２０と一体的に回転する工具ホルダ２４とを有する。なお図示例ではマスタ回転軸１８とスレーブ回転軸２０とは互いに直交しているが、９０度以外の角度で交叉してもよく、或いはねじれの関係にあってもよい。
【００１３】
工具ホルダ２４は、スレーブ回転軸２０に固定され、工具２６を保持するように構成されており、本実施形態では工具ホルダ２４及び工具２６が機構先端部２２を構成する。但し機構先端部２２はこのような形態に限られず、例えば溶接トーチや、物品を把持又は吸着する把持式又は吸着式ハンド等でもよい。
【００１４】
直動リンク１４ａ、１４ｂ及び１４ｃのそれぞれの一端は、ボールジョイント等のユニバーサルジョイント２８ａ、２８ｂ及び２８ｃによって基礎部材１２に連結されており、これにより各リンクは基礎部材１２に対して任意の方向に旋回可能になっている。同様に、直動リンク１４ａ、１４ｂ及び１４ｃのそれぞれの他端は、ボールジョイント等のユニバーサルジョイント３０ａ、３０ｂ及び３０ｃによって可動部材１６に連結されており、これにより各リンクは可動部材１６に対して所定の角度範囲内で旋回可能になっている。なお直動リンクの代わりに、各々が駆動リンク及び受動リンクを有する３組のリンクを使用してもよい。
【００１５】
パラレルリンク機構１０はさらに、該機構の剛性を高める等の目的で、駆動装置を有さずに伸縮自在であるとともに、基礎部材１２（例えばその重心）と可動部材１６（例えばその重心）とを連結する中心軸３２を有してもよい。
【００１６】
パラレルリンク機構１０は、産業用ロボット又は工作機械等の産業機械３４（概略図示）の一部として構成することができ、物品の搬送や機械加工等の所定の作業に使用することができる。またパラレルリンク機構１０の動作は、産業機械３４に接続された制御装置３６によって制御される。従ってパラレルリンク機構１０を有する産業機械３４及び制御装置３６は、所定の作業を行うためのシステムを構成する。なお制御装置３６は例えば、パラレルリンク機構１０の適用対象がロボットである場合は、該ロボットを制御するロボット制御装置に相当し、パラレルリンク機構１０の適用対象が工作機械である場合は、該工作機械を制御する数値制御装置に相当する。
【００１７】
制御装置３６は、機構先端部２２（ここでは工具ホルダ２４に対して不動に保持された工具２６の先端、すなわち工具先端点３８）の位置及び姿勢の組み合わせを各々が含む複数の指令位置を作成する指令位置作成部４０と、複数の指令位置の間の補間動作における工具先端点３８の位置又は姿勢についての禁止領域を示す数理モデル（後述）を、複数の指令位置の各々における工具先端点３８の位置、及び後述する距離に基づいて作成するモデル作成部４２と、作成された数理モデルに基づいて、補間動作中において工具先端点３８の位置又は姿勢が禁止領域（後述）に入らないように、工具先端点３８の位置及び姿勢についての補間指令を作成する補間指令部４４とを有する。これらの構成要素の機能は後述するが、制御装置３６に組み込まれたプロセッサやメモリ等によって実現可能である。
【００１８】
上述したようなパラレルリンク機構１０は、３つのリンクの長さ及び２つの回転軸の回転角度を制御することにより、工具先端点を指令通りに（複数の指令位置を含む指令軌跡に沿って）移動させることができるが、一般的な５軸構造とは異なり、３つの送り軸がそれぞれ独立して動作できないので、パラレルリンク機構１０の動作制御を行う際は、指令位置の各々において工具先端点３８の位置及び姿勢（の組み合わせ）を指令することが好ましい。またパラレルリンク機構１０は、比較的狭いスペースにも設置でき、広い可動範囲を確保することができる等の利点があるが、図２−図４に例示するような構造的制約がある。
【００１９】
図２及び図３は、図１に示すパラレルリンク機構１０の機構先端部をさらに模式的に示す図である。通常、マスタ回転軸１８及びスレーブ回転軸２０を有する構造では、工具の可動範囲をなるべく広く確保する等の目的から、スレーブ回転軸２０と機構先端部２２の方向（ここでは工具軸（工具方向））を表す軸線４６との間に所定のオフセット（直線２０と直線４６間の最短距離を表す線分）４８が存在するように工具２６が設けられる。つまりスレーブ回転軸２０と工具軸４６は互いにねじれの関係にある。ここで、マスタ回転軸１８と工具軸４６との位置関係としては、図２に示すように線分４８とスレーブ回転軸２０との交点４９がマスタ回転軸１８上に位置する場合と、図３に示すように交点４９がマスタ回転軸１８上に位置しない（マスタ回転軸１８からスレーブ回転軸２０に平行な方向に距離５０だけ離れている）場合とがある。なお機構先端部２２の方向（ここでは工具方向と同一）とは、機構先端部２２の姿勢を代表する方向であり、例えば工具２６が棒状部材である場合はその長手方向を意味する等、機構先端部２２の形状・構造に応じて予め定められる方向である。なお本実施形態では、工具軸４６はマスタ回転軸１８の軸線方向と平行である。
【００２０】
図３のように、工具方向を表す軸線４６との距離が最短となるスレーブ回転軸２０上の点４９がマスタ回転軸１８上に位置しない場合（オフセット４８が存在せず、軸線４６とスレーブ回転軸２０が交点４９で交わる場合も含まれる）、図４に示すように、機構先端部２２の代表点（例えば工具先端点３８）は、マスタ回転軸１８及びスレーブ回転軸２０がいかなる回転角度位置にあっても、参照符号５２で示される禁止領域内に入ることはできない。なお禁止領域５２は、マスタ回転軸１８の軸線（図示例では中心軸３２と一致）を中心軸とし、距離５０に等しい半径を有する無限長の円柱（数理モデル）で画定される領域である。
【００２１】
従って図３に例示するような距離５０が存在するパラレルリンク機構では、工具先端点３８の補間動作中に、工具先端点３８が禁止領域５２内に入るような指令を出力してしまうと、機構的制約の範囲を外れた位置及び姿勢を指令することになり、逆運動学の解が求まらず、実際にはパラレルリンク機構自体が緊急停止（急停止）することになる。従って以降は、補間動作中であっても工具先端点が禁止領域内に入るような補間指令が作成されないようにするための制御装置の機能（処理）について説明する。
【００２２】
（実施例１）
図５は、簡単のため、工具先端点が進入できない禁止領域５２がＸ、Ｙ及びＺ軸からなる直交座標系の原点Ｏを通り、中心軸がＺ軸に合致する円柱で画定される領域であるとした場合に、工具先端点の指令位置Ａ及びＢを含む補間経路（指令軌跡）５４を示す図である。図５の例では、指令位置Ａ及びＢの各々においては、工具先端点は指令通りの位置及び姿勢をとりうるが、位置Ａから位置Ｂまでの間を線形補間した場合、その中間点（補間位置）Ｃは禁止領域５２内に入るため、位置Ａから位置Ｂまで工具の姿勢を変更せずに工具先端点の位置を移動させると、上述の急停止が生じることになる。なお各指令位置における矢印は工具方向（図４の参照符号４６に相当）を表し、工具方向と禁止領域５２の中心軸の方向は互いに平行である。
【００２３】
ここで急停止を防止する方法の１つとして、工具方向を変えずに、工具先端点を禁止領域５２に干渉しないように迂回させること、具体的には補間経路５４を禁止領域５２に干渉しない曲線にすることが考えられる。しかし多くの場合、パラレルリンク機構の動作サイクルや加工精度の観点からは、工具先端点はできるだけ直線的に移動させることが好ましく、故に各指令位置間は線形補間することが好ましい。
【００２４】
そこで図６に示すように、工具先端点が禁止領域５２内に入る直前の補間位置Ｄと、工具先端点が禁止領域５２から出た直後の補間位置Ｅとの間は、禁止領域５２を適切な方向に（図示例では線分ＡＢと垂直な方向に）傾けることにより、指令位置ＡからＢまでを直線的に補間することが可能となる。具体的には、補間位置ＤとＥの間は、工具を傾ける（工具方向を変更する）ことにより、禁止領域５２も工具方向の傾斜角と同じ角度で傾斜するので、軌跡５４上の全ての補間点が禁止領域５２内に入らないようにすることができる。
【００２５】
但し、補間位置Ｄ又はＥにおいて急激に工具方向を変化させると、パラレルリンク機構の各軸に大きな負荷がかかり、加工精度等に悪影響が及び場合があり得ることから、図７に示すように、指令位置ＡとＢの間は、工具方向を徐々に傾斜させて滑らかな動作を実現するような補間指令を作成することが好ましい。
【００２６】
（実施例２）
次に図８−図１２を参照して、禁止領域を表す数理モデルの他の例として、単位球面を利用する場合を説明する。図８は、パラレルリンク機構１０の要部を模式的に表した図であり、基礎部材１２の中心（基礎部材１２と中心軸３２との交点）を原点Ｏとし、工具先端点をｐとし、工具方向（ベクトル）をｖとする。このとき、ｐを通りかつ工具ベクトルｖと同方向に延びる直線と原点Ｏとの距離ｄは、以下の式（１）で表される。但しθは、工具ベクトルｖと位置ベクトルｐがなす角であり、以下の式（２）で表される。また式（２）におけるｓｇｎは符号関数である。
【００２７】
【数１】
2018196919.tif 000003
【数２】
2018196919.tif 000004
【００２８】
ここで、図３を参照して説明した距離５０の大きさをＤｘとすると、ｖは（ｄ≧Ｄｘ）を満たすように計算（指令）される必要があり、逆に言えば、（ｄ＜Ｄｘ）となるようなｖは機構的に実現できない。そこで図９に示すように、半径が１（無次元数）であってｐを中心とする単位球面５６を考え、以下の式（３）を満たす境界角度φを求める。
【００２９】
【数３】
2018196919.tif 000005
【００３０】
式（３）と、（ｄ≧Ｄｘ）を満たす必要があることから、工具ベクトルｖの方向θがとり得る範囲は、以下の式（４）で表される。つまり、（θ＜φ）又は（θ＞π−φ）の場合、ｖは単位球面５６上の禁止領域５８内（ハッチングで図示）に入ることになり、工具先端点がｐにあるときは工具はこのような方向（姿勢）をとることができない。逆に言えば、パラレルリンク機構１０において、工具先端点がｐにあるときにこのような方向（姿勢）を指令すると、逆運動学の解が求まらず、パラレルリンク機構自体が緊急停止（急停止）することになる。
【００３１】
φ ≦ θ ≦ π−φ                          （４）
【００３２】
従って図１０に示すように、指令位置Ｐ1とＰ2との間の動作補間中（補間位置Ｐ）に、単位球面５６上の禁止領域５８以外の許容領域６０内に常にｖが入るように角度θを設定する（すなわち式（４）を満たす）ことにより、指令位置Ｐ1とＰ2との間を直線的に補間（線形補間）した場合でも、常に工具ベクトルｖの終点を許容領域６０内に入るようにすることができる。従って実施例２でも、工具先端点の制御における補間動作中のパラレルリンク機構の急停止や損傷を確実に防止することができ、加工精度や操作の安全性・安定性の向上が期待できる。
【００３３】
図１１は、補間動作中に常に工具ベクトルｖが許容領域６０内に入ることを確保するための適切な工具方向を求める一具体例を説明する図である。図１０に示すように、単位球面５６上における禁止領域５８及び許容領域６０の大きさ（割合）は、各指令位置やその間の補間位置によって変化するが、各位置において禁止領域５８と許容領域６０との境界を定める境界角度φを用いた内分比を用いることにより、いずれの位置においてもｖが許容領域６０内に存在するような角度θを求めることができる。具体的には、指令位置Ｐ1における工具ベクトルｖ1の方向θ1は内分比ｓ1を用いて表すことができ、ｓ1は以下の式（５）から求めることができる。なおφ1は、指令位置Ｐ1における境界角度であるが、（φ1＝π/2）のときは、ｓ1はゼロとする。同様に、指令位置Ｐ2における工具ベクトルｖ2の方向θ2は内分比ｓ2を用いて表すことができ、ｓ2は以下の式（６）から求めることができる。なおφ2は、指令位置Ｐ2における境界角度であるが、（φ2＝π/2）のときは、ｓ2はゼロとする。
【００３４】
【数４】
2018196919.tif 000006
【数５】
2018196919.tif 000007
【００３５】
式（５）及び（６）から求めた内分比を用いて、補間位置Ｐにおける工具ベクトルｖの方向θは、以下の式（７）から求めることができる。なおｔは時間を表す。このようにして求めたθは、指令位置Ｐ1とＰ2との間のいずれの補間位置においても、工具ベクトルｖの終端が禁止領域５８内に入らない（常に許容領域６０内にある）ようにすることができる。
【００３６】
【数６】
2018196919.tif 000008
【００３７】
実施例１及び２ではいずれも、指令位置作成部４０によって、機構先端部２２の位置（工具先端点３８の位置）及び姿勢（工具方向）の組み合わせを各々が含む複数の指令位置を作成し、モデル作成部４２によって、複数の指令位置の間の補間動作における機構先端部２２の位置又は姿勢についての禁止領域５２又は５８を示す数理モデル（円柱又は単位球面）を、機構先端部２２の位置及び距離５０に基づいて作成し、補間指令部４４によって、作成された数理モデルに基づいて、補間動作中において機構先端部２２の位置又は姿勢が禁止領域に入らないように、機構先端部２２の位置及び姿勢についての補間指令を作成する。従って本開示によれば、補間動作中であっても実現できない位置及び姿勢の組み合わせが指令され、パラレルリンク機構が急停止してしまうことを確実に回避でき、図１に示すような５軸構造のパラレルリンク機構の安定した高精度の動作を実現することができる。
【００３８】
また連続する指令位置を補間する場合、禁止領域を迂回するように補間指令における指令位置を変更することも可能だが、機構先端部の姿勢を変更することのみによって禁止領域を回避することも可能である。後者の場合は、指令位置間を線形補間することが可能となり、サイクルタイムの短縮や、パラレルリンク機構の用途が機械加工である場合には加工精度の向上が図られる。
【符号の説明】
【００３９】
１０    パラレルリンク機構
１２    基礎部材
１４ａ、１４ｂ、１４ｃ    直動リンク
１６    可動部材
１８    マスタ回転軸
２０    スレーブ回転軸
２２    機構先端部
２４    工具ホルダ
２６    工具
２８ａ、２８ｂ、２８ｃ、３０ａ、３０ｂ、３０ｃ    ボールジョイント
３２    中心軸
３４    産業機械
３６    制御装置
３８    工具先端点
４０    指令位置作成部
４２    モデル作成部
４４    補間指令部
４６    工具方向
４８    オフセット
５２、５８    禁止領域
５４    指令軌跡
５６    単位球面
６０    許容領域
【図１】
2018196919.tif 000009
【図２】
2018196919.tif 000010
【図３】
2018196919.tif 000011
【図４】
2018196919.tif 000012
【図５】
2018196919.tif 000013
【図６】
2018196919.tif 000014
【図７】
2018196919.tif 000015
【図８】
2018196919.tif 000016
【図９】
2018196919.tif 000017
【図１０】
2018196919.tif 000018
【図１１】
2018196919.tif 000019