JP7340196B2 - リンク作動装置 - Google Patents

Description

この発明は、リンク作動装置に関する。

リンク作動装置は、精密で広範な作動範囲を必要とする産業機器等に用いられる。リンク作動装置は、駆動源とリンク機構からなる。リンク機構の一種としてパラレルリンク機構が知られている。

コンパクトな構成でありながら、精密で広範な作動範囲の動作が可能なリンク作動装置として、例えば、特許６１３３１６２号公報（特許文献１）に示されるようなものが提案されている。

特許６１３３１６２号公報 特開２００５－０９８４９７号公報

特許６１３３１６２号公報（特許文献１）に示されるようなリンク作動装置において、先端部と基端部の関係式では、ある先端座標における姿勢制御用駆動源の回転角を逆変換で求めている。同様に、順変換は２台の姿勢制御用駆動源の２軸の回転角から先端位置の算出方法が示されているが、理論式に三角関数・逆三角関数の積和が多数現れることから、計算誤差の蓄積が起きる。

また、特許文献２において、順変換は収束演算による近似解であり、繰り返し演算が必要なため先端位置の算出に時間がかかる。そのため、リアルタイムで処理するのが困難であるという問題点があり、ダイレクトティーチングや異常復帰後等で現在位置が不明な状態からの長距離移動動作が不可能であった。

本発明は、このような課題を解決するためのものであって、その目的は、計算誤差の蓄積を少なくし位置決め精度の向上が可能で、リアルタイムで順変換を実行することが可能なリンク作動装置を提供することである。

本開示は、リンク作動装置に関する。リンク作動装置は、基端側の第１リンクハブと、先端側の第２リンクハブと、第１リンクハブと第２リンクハブとを連結する少なくとも３つのリンク機構とを備える。少なくとも３つのリンク機構の各々は、第１リンクハブに対して回転可能に連結された第１端部リンク部材と、第２リンクハブに対して回転可能に連結された第２端部リンク部材と、第１端部リンク部材および第２端部リンク部材の各々に対して回転可能に連結された中央リンク部材とを含む。少なくとも３つのリンク機構において、第１リンクハブと第１端部リンク部材との回転対偶部の少なくとも３つの中心軸および、中央リンク部材の一方端の回転対偶部の中心軸は、第１リンクハブ中心点で交わり、第２リンクハブと第２端部リンク部材との回転対偶部の少なくとも３つの中心軸および中央リンク部材の他方端の回転対偶部の中心軸は、第２リンクハブ中心点で交わる。リンク作動装置は、少なくとも３つのリンク機構のうち、２つのリンク機構における、第１リンクハブに対する第１端部リンク部材の角度から、第２リンクハブの位置および姿勢を球面三角法を用いて演算するように構成される制御装置をさらに備える。

好ましくは、制御装置は、第２リンクハブ中心点の位置から第１端部リンク部材の姿勢を示す角度を球面三角法を用いて演算するように構成される。

本発明によれば、球面駆動機構を有するリンク作動装置において、リアルタイムで順変換を実行することが可能となる。

リンク作動装置の、ある姿勢における正面図である。 リンク作動装置５０の構成のうち、１組のリンク機構４に対応する構成を代表的に示した図である。 パラレルリンク機構の基端側のリンクハブ２、基端側の端部リンク部材５を抽出して示した断面図である。 パラレルリンク機構１の３つのリンク機構４のうちの１つを抽出して直線で表現した模式図である。 基端側のリンクハブ中心軸ＱＡと先端側のリンクハブ中心軸ＱＢが同一線上にある状態のリンク作動装置の斜視図である。 基端側のリンクハブ中心軸ＱＡと先端側のリンクハブ中心軸ＱＢが同一線上にある状態のリンク作動装置の模式図である。 基端側のリンクハブ中心軸ＱＡに対して先端側のリンクハブ中心軸ＱＢが、ある角度をなす状態のリンク作動装置の斜視図である。 基端側のリンクハブ中心軸ＱＡに対して先端側のリンクハブ中心軸ＱＢが、ある角度をなす状態のリンク作動装置の模式図である。 ωの計算モデルを示す図である。 半径を１とした単位球面における三角形のモデルを示す図である。 単位球面における別の三角形のモデルを示す図である。 順運動学のモデルを示す図である。 逆運動学の式の導出の説明をするための第１の図である。 逆運動学の式の導出の説明をするための第２の図である。 順運動学によって基端側の端部リンク部材の角度から先端側のリンクハブの中心点位置を得る処理を説明するためのフローチャートである。 逆運動学によって先端側のリンクハブの中心点位置から基端側の端部リンク部材の角度を得る処理を説明するためのフローチャートである。

以下、本発明の実施の形態について図面を参照しつつ説明する。なお、以下の図面において同一または相当する部分には同一の参照番号を付し、その説明は繰り返さない。

＜リンク作動装置の構成＞
この発明の一実施形態に係るパラレルリンク機構を備えたリンク作動装置の構成を図１～図３と共に説明する。

図１は、リンク作動装置の、ある姿勢における正面図である。図１を参照して、リンク作動装置５０は、パラレルリンク機構１と、パラレルリンク機構１の姿勢変更用のアクチュエータ５１と、アクチュエータの駆動力を減速してパラレルリンク機構に伝達する減速機構５２と、アクチュエータ５１を制御する制御装置１００とを備える。

パラレルリンク機構１は、基端側のリンクハブ２に対し先端側のリンクハブ３を３組のリンク機構４を介して姿勢変更可能に連結したものである。なお、リンク機構４の数は、４組以上であっても良い。先端側のリンクハブ３には、エンドエフェクタ６１が設置される。

図２は、リンク作動装置５０の構成のうち、１組のリンク機構４に対応する構成を代表的に示した図である。図２を参照して、各リンク機構４は、基端側の端部リンク部材５と、先端側の端部リンク部材６と、中央リンク部材７とを含み、４つの回転対偶からなる４節連鎖のリンク機構を構成する。基端側の端部リンク部材５は、一端が基端側のリンクハブ２に回転自在に連結されている。同様に、先端側の端部リンク部材６は、一端が先端側のリンクハブ３に回転自在に連結されている。中央リンク部材７の一端には、基端側の端部リンク部材５の他端が回転自在に連結されている。中央リンク部材７の他端には、先端側の端部リンク部材６の他端が回転自在に連結されている。

図１、図２に示すように、基端側のリンクハブ２は、平板状の土台１０と、この土台１０に円周方向に等間隔で配置された３個の回転軸連結部材１１とで構成される。各回転軸連結部材１１には、軸心がリンクハブ中心軸ＱＡと交差する回転軸体１２が回転自在に連結されている。この回転軸体１２に、基端側の端部リンク部材５の一端が連結される。基端側の端部リンク部材５の他端には、中央リンク部材７の一端に回転自在に連結された回転軸体１５が連結される。本実施例において、回転軸連結部材１１は土台１０に円周方向に等間隔で配置されているが、必ずしもその限りではない。

リンクハブ３の回転軸体２２および中央リンク部材７の回転軸体２５も、前記回転軸体１２，１５と同じ形状である。

図１、図２に示すように、先端側のリンクハブ３は、平板状の先端部材２０と、先端部材２０に円周方向に等間隔に配置された３個の回転軸連結部材２１とで構成される。各回転軸連結部材２１は、軸心がリンクハブ中心軸ＱＢと交差する回転軸体２２が回転自在に連結されている。このリンクハブ３の回転軸体２２に、先端側の端部リンク部材６の一端が連結される。先端側の端部リンク部材６の他端には、中央リンク部材７の他端に回転自在に連結された回転軸体２５が連結される。

端部リンク部材５と中央リンク部材７との回転対偶の中心軸Ｏ２（Ａ）と、端部リンク部材６と中央リンク部材７との回転対偶の中心軸Ｏ２（Ｂ）とは、点Ａにおいて交差角γで交差する。

図３は、パラレルリンク機構の基端側のリンクハブ２、基端側の端部リンク部材５を抽出して示した断面図である。図３には、図１の姿勢のパラレルリンク機構の先端側のリンクハブ３、先端側の端部リンク部材６および中央リンク部材７を取り除いた状態が示されている。なお、３つの端部リンク部材５の各々については、両端の回転対偶部の回転軸Ｏ１、Ｏ２を含む面における断面が示されている。

図１および図３を参照して、３組のリンク機構４のそれぞれに、基端側のリンクハブ２に対する先端側のリンクハブ３の姿勢を任意に変更する姿勢変更用のアクチュエータ５１が設けられる。各アクチュエータ５１には減速機構５２が設けられている。各アクチュエータ５１は、ロータリアクチュエータであり、基端側のリンクハブ２の土台１０の上面に、回転軸体１２と同軸上に設置されている。アクチュエータ５１と減速機構５２は一体に設けられ、モータ固定部材５３により減速機構５２が土台１０に固定されている。なお、３組のリンク機構４のうち少なくとも２組にアクチュエータ５１を設ければ、基端側のリンクハブ２に対する先端側のリンクハブ３の姿勢を確定することができる。

図３において、減速機構５２はフランジ継手を形成する大径の出力軸体５２ａを有する。出力軸体５２ａの先端面は、出力軸体５２ａの中心線と直交する平面状のフランジ面５４となっている。出力軸体５２ａは、スペーサ５５を介して、基端側の端部リンク部材５の外径側の回転軸支持部材３１にボルト５６で接続されている。リンクハブ２と端部リンク部材５の回転対偶部の回転軸体１２は、大径部１２ａと小径部１２ｂとからなる。小径部１２ｂは軸受の内輪に挿通され、大径部１２ａは減速機構５２の出力軸体５２ａに設けられた内径溝５７に嵌っている。

端部リンク部材５は、Ｌ字形状を有する。端部リンク部材５は、１つの湾曲部材３０と、この湾曲部材３０の両端の外径側の側面と内径側の側面にそれぞれ固定された計４つの回転軸支持部材３１とで構成される。４つの回転軸支持部材３１は同一形状ではなく、基端側のリンクハブ２との回転対偶部に設けられる外径側の回転軸支持部材３１Ａは、減速機構５２のフランジ面５４とスペーサ５５を介して結合されるフランジ取付面５８を有する。本実施例において、端部リンク部材５は、Ｌ字形状を有しているが必ずしもＬ字である必要はない。

このリンク作動装置５０には、例えば図１に示すように、先端側のリンクハブ３にエンドエフェクタ６１が設置される。アクチュエータ５１が基端側のリンクハブ２に対する先端側のリンクハブ３の姿勢を変更することによって、エンドエフェクタ６１の２自由度の角度を制御することができる。

図３には、３組のリンク機構４について、基端側のリンクハブ２と基端側の端部リンク部材５の回転対偶の中心軸Ｏ１と、端部リンク部材５と中央リンク部材７との回転対偶の中心軸Ｏ２と、球面リンク中心ＰＡとの関係が示されている。先端側のリンクハブ３および先端側の端部リンク部材６の形状と、これらの位置関係も図３と同様である（図示せず）。図３の例では、中心軸Ｏ１と中心軸Ｏ２とが成す角度α（アーム角）が９０°とされているが、角度αは９０°以外であっても良い。

パラレルリンク機構１は、２つの球面リンク機構を組み合わせた構造である。
リンクハブ２と端部リンク部材５の回転対偶の中心軸Ｏ１と、端部リンク部材５と中央リンク部材７の回転対偶の中心軸Ｏ２とは、基端側において球面リンク中心ＰＡ（図２、図３）で交差している。また、基端側において、リンクハブ２と端部リンク部材５の回転対偶と球面リンク中心ＰＡとの間の軸Ｏ１に沿う距離は、端部リンク部材５と中央リンク部材７の回転対偶と球面リンク中心ＰＡとの間の軸Ｏ２に沿う距離と同じである。

図示しないが、同様に、リンクハブ３と端部リンク部材６の回転対偶の中心軸と、端部リンク部材６と中央リンク部材７の回転対偶の中心軸とは、先端側において球面リンク中心ＰＢ（図２）で交差している。また、先端側において、リンクハブ３と端部リンク部材６の各回転対偶と球面リンク中心ＰＢとの距離は、端部リンク部材６と中央リンク部材７の回転対偶と球面リンク中心ＰＢとの距離と同じである。

図４は、パラレルリンク機構１の３つのリンク機構４のうちの１つを抽出して直線で表現した模式図である。３組のリンク機構４は、幾何学的に同一の対称形状をなす。幾何学的に同一の対称形状とは、図４に示すように、端部リンク部材５，６および中央リンク部材７を直線で表現し回転対偶を丸で示した幾何学モデルが、二等分面に対して、基端側部分と先端側部分が対称を成す形状であることを言う。本実施の形態のパラレルリンク機構１は回転対称タイプで、基端側のリンクハブ２および基端側の端部リンク部材５と、先端側のリンクハブ３および先端側の端部リンク部材６との位置関係が、中央リンク部材７の中心線Ｃに対して回転対称となる位置構成になっている。

基端側のリンクハブ２と先端側のリンクハブ３と３組のリンク機構４とによって、基端側のリンクハブ２に対し先端側のリンクハブ３が直交２軸周りに回転自在な２自由度機構が構成される。２自由度機構は、言い換えると、基端側のリンクハブ２に対して先端側のリンクハブ３の姿勢を、２自由度で自在に変更可能な機構である。この２自由度機構は、コンパクトでありながら、基端側のリンクハブ２に対する先端側のリンクハブ３の可動範囲を広くとれる。

例えば、球面リンク中心ＰＡを通り、リンクハブ２と端部リンク部材５の回転対偶の中心軸Ｏ１（図３）と直角に交わる直線をリンクハブ２の中心軸ＱＡとする。また、球面リンク中心ＰＢを通り、リンクハブ３と端部リンク部材６の回転対偶の中心軸（図示せず）と直角に交わる直線をリンクハブ３の中心軸ＱＢとする。

この場合、基端側のリンクハブ２の中心軸ＱＡと先端側のリンクハブ３の中心軸ＱＢの折れ角θ（図４）の最大値を約±９０°とすることができる。また、基端側のリンクハブ２に対する先端側のリンクハブ３の旋回角φ（図４）を０°～３６０°の範囲に設定できる。折れ角θは、中心軸ＱＡおよび中心軸ＱＢを含む垂直面において中心軸ＱＡに対して中心軸ＱＢが傾斜した角度のことであり、旋回角φは、基準位置に対して中心軸ＱＢの水平面への投影直線がなす角度のことである。

基端側のリンクハブ２に対する先端側のリンクハブ３の姿勢は、基端側のリンクハブ中心軸ＱＡと先端側のリンクハブ中心軸ＱＢの交点ＰＣを回転中心として変更される。姿勢が変化しても、基端側と先端側の球面リンク中心ＰＡ，ＰＢ間の距離Ｄ（図４）は変化しない。

パラレルリンク機構１においては以下の条件が成立している。すなわち、各リンク機構４におけるリンクハブ２と端部リンク部材５の回転対偶の中心軸Ｏ１とリンクハブ３と端部リンク部材６の回転対偶の中心軸Ｏ２とのなす角度が互いに等しい。球面リンク中心ＰＡ，ＰＢから回転対偶部までの長さが互いに等しい。各リンク機構４のリンクハブ２と端部リンク部材５の回転対偶の中心軸Ｏ１（Ａ）が、基端側において球面リンク中心ＰＡと交差する。各リンク機構４の端部リンク部材５と中央リンク部材７の回転対偶の中心軸Ｏ２（Ａ）が、基端側において球面リンク中心ＰＡと交差する。各リンク機構４のリンクハブ３と端部リンク部材６の回転対偶の中心軸Ｏ１（Ｂ）が、先端側において球面リンク中心ＰＢと交差する。各リンク機構４の端部リンク部材６と中央リンク部材７の回転対偶の中心軸Ｏ２（Ｂ）が、先端側において球面リンク中心ＰＢと交差する。基端側の端部リンク部材５と先端側の端部リンク部材６の幾何学的形状が等しく、かつ中央リンク部材７についても基端側と先端側とで形状が等しい。これらの条件が成立しているとき、中央リンク部材７の対称面に対して、中央リンク部材７と端部リンク部材５，６との角度位置関係を基端側と先端側とで同じにすれば、幾何学的対称性から基端側のリンクハブ２および基端側の端部リンク部材５と、先端側のリンクハブ３および先端側の端部リンク部材６とは二等分面に対して対称に同じ動きをする。

以上説明した構成のリンク作動装置５０における順運動学および逆運動学について説明する。ロボット、マニュピレータ等の関節を制御する方法として順運動学（Forward Kinematics）と逆運動学（Inverse Kinematics）が知られている。本実施の形態の説明において、順運動学は、基端側のリンクハブ２に対して端部リンク部材５がなす角度から先端側のリンクハブの中心位置および方向が決まることを言う。また、逆運動学は、先端側のリンクハブの中心位置および方向から基端側のリンクハブ２に対して端部リンク部材５がなす角度が決まることをいう。なお、本実施の形態では順運動学、逆運動学は、ともに先端側中心位置に対する計算を示しており、方向までは求めていないが、先端位置が求まれば、機構の幾何学的性質から方向も簡単な計算で求められる。

＜順運動学＞
図５は、基端側のリンクハブ中心軸ＱＡと先端側のリンクハブ中心軸ＱＢが同一線上にある状態のリンク作動装置の斜視図である。図６は、基端側のリンクハブ中心軸ＱＡと先端側のリンクハブ中心軸ＱＢが同一線上にある状態のリンク作動装置の模式図である。

リンク作動装置５０の原点姿勢が図５に示される。本明細書において、原点姿勢とは、基端側のリンクハブ２の中心軸ＱＡと先端側のリンクハブ３の中心軸ＱＢが一致している状態の姿勢をいう。すなわち、原点姿勢は、リンク作動装置５０の折れ角θが０度の姿勢である。

図２には、図１の原点姿勢における３組のリンク機構のうち１組のみについて示した正面図が示され、図６にはリンク作動装置を簡略化したモデル図が示されている。リンク機構は、簡略化すると基端側および先端側の各リンクハブ、基端側および先端側の各端部リンク部材、ならびに中央リンク部材で表すことができる。

リンク作動装置５０のパラレルリンク機構１は、基端側のリンクハブ中心点ＰＡを中心とする基端側の球面リンクＧＡと、先端側のリンクハブ中心点ＰＢを中心とする先端側の球面リンクＧＢとが交わって成す平面である二等分面ＰＬ１を境に鏡面対称となる構成となっている。基端側の端部リンク部材５と中央リンク部材７との回転対偶部の中心軸Ｏ２（Ａ）と、先端側の端部リンク部材６と中央リンク部材７との回転対偶部の中心軸Ｏ２（Ｂ）が交わる点Ａは二等分面ＰＬ１上に存在する。また、基端側の端部リンク部材５と中央リンク部材７との回転対偶部の中心軸Ｏ２（Ａ）と、先端側の端部リンク部材６と中央リンク部材７との回転対偶部の中心軸Ｏ２（Ｂ）とが成す角を軸角γと称する。中央リンク部材７が成す角度を中央角ｄと称する。中央角ｄは、正確には、基端側の端部リンク部材５と中央リンク部材７との回転対偶部の中心軸Ｏ２（Ａ）に垂直な直線と、先端側の端部リンク部材６と中央リンク部材７との回転対偶部の中心軸Ｏ２（Ｂ）に垂直な直線が、二等分面上で交わって成す角である。軸角γおよび中央角ｄは、パラレルリンク機構１を設計する時に決まる定数である。また、図６から、中央角ｄは、軸角γを用いるとｄ＝π－γ（rad）で表すことができる。

図７は、基端側のリンクハブ中心軸ＱＡに対して先端側のリンクハブ中心軸ＱＢが、ある角度を成す状態のリンク作動装置の斜視図である。図８は、基端側のリンクハブ中心軸ＱＡに対して先端側のリンクハブ中心軸ＱＢが、ある角度を成す状態のリンク作動装置の模式図である。

図７には任意の姿勢（折れ角θ、旋回角φ）のリンク作動装置の斜視図が示され、図８には図７のモデル図が示される。点Ａは常に二等分面ＰＬ１上に存在し、この点Ａを一つの二自由度関節とみなすことができる。折れ角θの時、基端側の球面リンク中心ＰＡと先端側の球面リンク中心ＰＢを結ぶ直線と基端側のリンクハブ２の中心軸ＱＡとが成す角はθ／２となる。また、基端側の球面リンク中心ＰＡと先端側の球面リンク中心ＰＢとを結ぶ直線と基端側の球面リンク中心ＰＡと点Ａを通る直線が成す角はｄ／２となる。パラレルリンク機構１はこれらの関係を維持したまま運動する機構である。

パラレルリンク機構１は２自由度のため、２つのアーム回転角βが決まれば先端側のリンクハブ３の位置を導出することができる。パラレルリンク機構１の先端側のリンクハブ３の中心ＰＢは、図８において、基端側のリンクハブ２の中心ＰＡを中心Ｏとして球面ＧＰ上を運動する。

図９は、ωの計算モデルを示す図である。図１０は、半径を１とした単位球面における三角形のモデルを示す図である。図１１は、単位球面における別の三角形のモデルを示す図である。図１２は、順運動学のモデルを示す図である。図９以降の球面は、中心Ｏから点Ａまでの距離を１とした場合の単位球を示している。これにより、球面上の大円の円弧の長さが円弧の中心角と等しくなるため、後述する球面三角法の正弦・余弦定理を簡単に適用することができる。

原点姿勢における３つのリンク機構４のうち第１のリンク機構、第２のリンク機構の点Ａをそれぞれ点Ａ_１，Ａ_２とする。また、ある姿勢での第１のリンク機構、第２のリンク機構の点Ａをそれぞれ点Ａ_１’，Ａ_２’とする。

また、パラレルリンク機構１では、３つのリンク機構４が等間隔に配置されている。このため、３つのリンク機構４について基端側のリンクハブ２と基端側の端部リンク部材５との回転対偶部の中心軸同士の成す角は２π／３である。

Ｙ軸を中心として第１のリンク機構において基端側の端部リンク部材５が回転するとき、点Ａ_１’は、円弧ＰＡ_１およびその延長上を動く。Ｙ軸から２π／３回転した軸を中心として第２のリンク機構において基端側の端部リンク部材５が回転するとき、点Ａ_２’は、円弧ＰＡ_２およびその延長上を動く。

順運動学は基端側の端部リンク部材５の角度βから先端側のリンクハブ３の姿勢を導出するものであるため、ある姿勢における基端側の端部リンク部材５の角度と、原点姿勢における端部リンク部材５の角度との差は、既知であるとする。第１のリンク機構、第２のリンク機構について、この差をそれぞれ角β_A1，β_A2とする。

図１０に単位球と球面三角形のモデルが示される。図１０では、基端側のリンクハブ２の中心軸が単位球と交わる点を点Ｐとし、点Ｐと点Ａ_１’とＡ_２’を結んでできる単位球の表面の球面三角形Ｔ_１を太線で示している。一般的に、球面上の三点Ａ，Ｂ，Ｃが成す球面三角形ＡＢＣは、辺ＢＣ，ＣＡ，ＡＢをそれぞれａ，ｂ，ｃとし、弧ＡＢと弧ＡＣが成す角度をＡ、弧ＡＢと弧ＢＣが成す角度をＢ、弧ＣＢと弧ＡＣが成す角度をＣとした場合、球面三角法より余弦定理が次の式（１）、正弦定理が式（２）で表される。

図１０の三角形Ｔ_１において、反時計回りを正としたとき、各辺の大きさは以下の式（３）、式（４）で示される。円弧Ａ_１’Ａ_２’の長さをＬとすると、式（１）は式（５）のように変形できる。

また、式（１）および、以下の式（６）から、角ａ_１，ａ_２についてそれぞれ式（７）、式（８）のように表せる。

図１１では先端側のリンクハブの中心Ｐ''とＡ_１’とＡ_２’を結んでできる球面三角形Ｔ_２を太線で示している。式（１）より、角ａ_１’は以下の式（９）のようになる。

また、図８において、ＯＡの延長線と球ＧＰとの交点をＡ’とした場合、弧Ａ’ＰＢの内角はｄ／２となりこの内角は不変である。任意の折れ角と旋回角に対して、この関係は保たれ、単位球上の弧であるため、球面三角形Ｔ_２の辺Ｐ''Ａ_２’の長さはｄ／２であり、Ｐ''Ａ_１’も同様にｄ／２である。したがって、三角形Ｔ_２は球面二等辺三角形になる。そのため、∠Ｐ''Ａ_１’Ａ_２’、∠Ｐ''Ａ_２’Ａ_１’をそれぞれａ_１’、ａ_２’とした場合、ａ_１’＝ａ_２’が成り立つ。

ＯＡ_１’、ＯＡ_２’を回転軸とした時、各基端側アームが回転した時の倒れ角ω_１，ω_２（図９）は次の式（１０）、式（１１）で表される。なお、各ωは基端側アームが回転したときの、各中央リンクの初期姿勢に対する倒れ角を示す。回転するのは基端側アームである端部リンク部材５であり、それにより中央リンクの回転軸がＯＡ_１’とＯＡ_２’になり、倒れ角ωで中央リンクが回転する。

図１２のモデルを用いて、順運動学の式の導出について説明する。図１２において示したいことは、以下の（１）、（２）に示す２ステップの回転で順運動学の計算が実現できることである。
（１）基端側リンクがβ_Ａ１回転し、その回転で点Ｐが点Ｐ’に移動する。
（２）回転後の基端側リンクと中央リンクとの対偶部Ａ_１’がω_１回転し、点Ｐ’が点Ｐ''に移動する。

上記（１）の基端側リンクの回転は回転軸がＹなので、Ｙ軸まわりの回転でも表せる。このとき回転軸を（０，１，０）としたロドリゲスの式は、Ｙ軸まわりの回転と同じ式になる。

なお、図１２におけるＡ_１’の座標（β_Ａ１＋ｄ／２、φ_Ａ１）において、φ_Ａ１は、点Ａに対応する旋回角を示し図１２でいうと底面円のＸ座標軸から点直線までの角度を示している（図１２では１８０°の位置）。

順運動学の式は、第１のリンク機構に注目し、先端側のリンクハブの中心点Ｐを回転させることによって導出する。図１２では基端側の端部リンク部材５はＹ軸を中心に回転する。そのため、中心点ＰをまずＹ軸中心にβ_A1回転させ、さらにＯＡ_１’を中心軸としてω_１回転させる。すると、基端側のリンクハブの中心Ｏから先端側のリンクハブの中心Ｐ''へのベクトルは、動作後の先端座標位置と一致する。任意の軸に対する回転は、ロドリゲスの回転公式より表現できる。回転軸の単位ベクトルをｎ、回転前のベクトルをｒ、回転後のベクトルをｒ’、回転量を角度θとすると、ｒ’は以下の式（１２）、あるいは式（１２）を３×３行列を使った変換式に置き換えた式（１３）が成立する。

ここでＲ_ｎ（θ）とは、ｎ軸を中心として角θだけｒの座標を回転させる場合の式（１２）ロドリゲスの回転公式を３×３の回転行列で表したときの式を示している。なお、式（１２）と式（１３）の具体的な表現はかなり異なるが、式（１２）はベクトル方程式の形式で書かれており、式（１３）は同じ結果が得られるｒの線形変換として表現した等価な式である。

先端側のリンクハブ中心点の位置ベクトルの初期位置ＰをＰ＝（０，０，ｌ）とした時、Ｙ軸中心に初期位置Ｐを角度β_A1回転させた後の位置ベクトルＰ’は以下の式（１４）で表される。

次に、ＯＡ_１を回転軸ｎ１として位置ベクトルＰ’を角度ω_１回転させた後の位置ベクトルＰ’’は以下の式（１５）で表される。

Ｐ’はβ_A1の関数、Ｐ’’はω_１の関数（＝β_A1とβ_A2の関数）であるため、式（１５）を用いることによって、３つの基端側の端部リンク部材５の角度βのうちの２つから、先端側のリンクハブ中心点Ｐ’’の位置および姿勢を算出することができる。

＜逆運動学＞
図１３は、逆運動学の式の導出の説明をするための第１の図である。図１４は、逆運動学の式の導出の説明をするための第２の図である。図１３および図１４を用いて逆運動学の式の導出について説明する。逆運動学の計算では、点Ｐ，Ａ_０，Ａ_１からなる球面三角形Ｔ_３と、点Ｐ，Ａ_０，Ａ_０’からなる球面三角形Ｔ_４を考える。ここで、Ａ_０は基端側のリンクハブ３と基端側の端部リンク部材６との回転対偶部の中心軸がＯを原点とし原点姿勢におけるＰを頂点とする単位球面と交わる点を示す。Ａ_０’は、点Ａ_０からＺ軸を中心に－α回転した位置にある点を示す。

なお、点Ａ_１は必ずしも二等分面上の点ではなく、先端側中心がＰにあるときの基端側リンクと中央リンクとの対偶部の位置になる。先端側中心が初期状態、すなわちＺ軸上にあるときは、点Ａ_１の位置は二等分面上になる。

図１３に示す角ｅ_１と図１４に示すｅ_２を求めることによって逆運動学の式が導出できる。まずＴ_４に対して球面余弦定理を用いると以下の式（１６）から式（１７）を得る。

同様にＴ_３に対して球面余弦定理を用いると以下の式（１８）から式（１９）を得る。

∠Ａ_０ＯＡ_１および∠Ａ_０ＯＡ_０'はアーム角α、∠Ａ_１ＯＰはｄ／２であるため、角ｅ_１および角ｅ_２は以下の式（２０）、式（２１）で表される。

αは基端側の端部リンク部材が成す角度であり、部材の構造により決定する値である。従ってｅ_１およびｅ_２は先端側のリンクハブ中心点Ｐ（θ，φ）の関数となる。よって、原点姿勢の時のβ_A1を０とした場合、β_A1は以下の式（２２）で表すことができる。

式（２２）が本実施の形態のパラレルリンク機構１の逆運動学の式となる。
本実施の形態のリンク作動装置によれば、一般的な球面三角法を用いてリンク作動装置の順変換および逆変換の式を導出しているので、繰り返し演算を必要とせず、先端の現在位置の把握やアーム回転角βの算出を素早く行なうことが可能になる。さらに、近似誤差を含まないアーム回転角βが算出されるため、全ての姿勢制御用駆動源を同方向に回転させ一定のトルクを加えることで機構的なガタによるブレを抑える一定トルク負荷制御の精度が上がり、位置決め精度を向上させることができる。

また、本実施の形態で示した計算手法は、任意のアーム角αに対して順運動学および逆運動学の計算として適応可能であり、リンク作動装置の多様な設計仕様に対応できる。ここで、これらの式においては角度を用いているが、単位球においてある２直線が成す中心角とその弧長は等しいため、角度の代わりに弧長を用いた式を使用してもよい。正弦あるいは余弦関数の項は、単位球面で考えているので、例えばベクトルＰやＡ_０の内積、あるいは外積のノルムを用いることによって、計算処理の負担を減らすこともできる。

＜順運動学、逆運動学を用いた制御＞
以上のように導出した順運動学の計算式、逆運動学の計算式を用いて、制御装置１００は、以下に示す制御を実行する。

図１５は、順運動学によって基端側の端部リンク部材の角度から先端側のリンクハブの中心点位置を得る処理を説明するためのフローチャートである。このフローチャートの処理は、例えば、ダイレクトティーチング実行時、または異常復帰後等で現在位置が不明な状態からの始動時に制御装置１００によって実行される。

ステップＳ１において制御装置１００は、図示しない角度センサから角β_A1，β_A2を取得する。そして、ステップＳ２において制御装置１００は、順運動学について説明した式（３）～（１１）を用いて、角ω_１，ω_２を算出する。さらに、ステップＳ３において、制御装置１００は、ロドリゲスの回転公式に基づく式（１２）または式（１３）を用いて先端側のリンクハブ中心点の位置Ｐ’，Ｐ’’を算出する。そして、制御装置１００は、ステップＳ４において、先端側リンクハブの位置Ｐ’’を上位の制御装置または表示装置などに出力する。

図１６は、逆運動学によって先端側のリンクハブの中心点位置から基端側の端部リンク部材の角度を得る処理を説明するためのフローチャートである。このフローチャートの処理は、例えば、パラレルリンク機構１によってエンドエフェクタ６１の姿勢を変えながら作業する場合に制御装置１００によって実行される。

ステップＳ１１において制御装置１００は、入力装置または図示しない上位の制御装置から先端側のリンクハブの位置情報（目標位置）を受信する。そして、ステップＳ１２において制御装置１００は、逆運動学について説明した式（１６）～（２２）を用いて、角β_A1を算出し、同様な手順で角β_A2も算出する。さらに、ステップＳ１４において、制御装置１００は、端部リンク部材５の回転角が角β_A1および角β_A2となるように、対応するアクチュエータ５１を制御する。このような処理を繰り返すことによって、先端側リンクハブの位置および姿勢が所望の状態に制御される。

最後に、再び図を参照して本実施の形態について総括する。
本実施の形態のリンク作動装置５０は、基端側の第１リンクハブ２と、先端側の第２リンクハブ３と、第１リンクハブ２と第２リンクハブ３とを連結する少なくとも３つのリンク機構４とを備える。少なくとも３つのリンク機構４の各々は、第１リンクハブ２に対して回転可能に連結された第１端部リンク部材５と、第２リンクハブ３に対して回転可能に連結された第２端部リンク部材６と、第１端部リンク部材５および第２端部リンク部材６の各々に対して回転可能に連結された中央リンク部材７とを含む。少なくとも３つのリンク機構４において、第１リンクハブ２と第１端部リンク部材５との回転対偶部の少なくとも３つの中心軸Ｏ１および、中央リンク部材７の一方端の回転対偶部の中心軸Ｏ２は、第１リンクハブ中心点ＰＡで交わり、第２リンクハブ３と第２端部リンク部材６との回転対偶部の少なくとも３つの中心軸および中央リンク部材７の他方端の回転対偶部の中心軸は、第２リンクハブ中心点ＰＢで交わる。リンク作動装置５０は、少なくとも３つのリンク機構４のうち、２つのリンク機構における、第１リンクハブ２に対する第１端部リンク部材５の角度β_Ａ１，β_Ａ２から、第２リンクハブ３の位置および姿勢を球面三角法を用いて演算するように構成される制御装置１００をさらに備える。

このような構成とすることによって、基端側と先端側のリンクハブ中心点ＰＡ，ＰＢを中心点とするそれぞれの球ＧＡ，ＧＢが交わる二等分面ＰＬ１を対称に先端側のリンクハブ３が動作する球面駆動機構が構成される。この構成により、球面三角法を用いた順変換・逆変換の式の導出ができるため、様々な設計緒元をもつリンク作動装置において、汎用的な順運動学および逆運動学の関数化を図ることができる。

リンク作動装置５０は、各基端側の端部リンク部材５の角度β_Ａ１，β_Ａ２から前記先端側のリンクハブ中心点ＰＢの位置を求めるために球面三角法を用いて演算する機能を制御装置１００に備えると良い。この構成により、ダイレクトティーチングや異常復帰後等で現在位置が不明な状態からの長距離移動動作をリアルタイムで行なう機能を実現できる。

好ましくは、制御装置１００は、第１リンクハブ２に対する第１端部リンク部材５の姿勢を示す角度β_Ａ１，β_Ａ２から、第２リンクハブ３の姿勢を求めるために以下の関係式を用いて演算するように構成される。
Ｐ’＝Ｒ_ｙ（β_Ａ１）Ｐ
Ｐ''＝Ｒ_ｎ１（ω_１）Ｐ’
ただし、各符号の意味は下記の通りである。
Ｐ：原点姿勢における第２リンクハブ中心点の座標
Ｐ’’： 原点姿勢から移動後の第２リンクハブ中心点の座標
β_A1：少なくとも３つのリンク機構のうちの第１のリンク機構における第１リンクハブに対する第１端部リンク部材の角度
β_A2：少なくとも３つのリンク機構のうちの第２のリンク機構における第１リンクハブに対する前記第１端部リンク部材の角度
Ｒn（θ）：ｎ軸を中心として角θだけＰの座標を回転させる場合のロドリゲスの回転公式の３×３の回転行列
ｎ１：第１のリンク機構において、第１リンクハブ中心点をＯとし、中央リンク部材の一方端の回転対偶部の中心軸と中央リンク部材の他方端の回転対偶部の中心軸との交点をＡとするときの直線ＯＡ
ω₁：β_A1およびβ_A2によって決まる第１のリンク機構の中央リンク部材の倒れ角
このように、各基端側の端部リンク部材５の回転角β_Ａ１，β_Ａ２から先端側のリンクハブ中心点ＰＢの位置を求める順運動学の関係式を用いて演算する機能を制御装置１００に備えるとよい。この構成により、順変換を用いてリンク作動装置５０を素早く制御することが可能になる。その際、近似誤差を含まないアーム回転角が算出されるため、機構的なガタによるブレを抑える一定トルク負荷制御の精度が上がり、位置決め精度を向上させることができる。

好ましくは、制御装置１００は、第２リンクハブ３の中心点ＰＢの位置から第１端部リンク部材５の角度β_Ａ１，β_Ａ２を球面三角法を用いて演算するように構成される。

より好ましくは、制御装置１００は、第２リンクハブ３の中心点ＰＢの位置から第１端部リンク部材５の姿勢を示す角度β_Ａ１，β_Ａ２を、以下の関係式を用いて演算するように構成される。
β_Ａ１＝ｅ_１－ｅ_２
なお、ｅ_１，ｅ_２は、既出の式（２０）、（２１）で示される。

このように、リンク作動装置５０は、先端側のリンクハブ３の姿勢を姿勢制御用のアクチュエータ５１の回転を介して制御する制御装置１００を有している。先端側のリンクハブ中心点ＰＢの位置から各基端側の端部リンク部材５の回転角β_Ａ１，β_Ａ２を求める逆運動学の関係式を用いて演算する機能を制御装置１００に備えると良い。この構成により、先端の姿勢が与えられた場合の各基端側の端部リンク部材５の回転角β_Ａ１，β_Ａ２を算出することができる。

今回開示された実施の形態は、すべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した実施の形態の説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

１ パラレルリンク機構、２，３ リンクハブ、４ リンク機構、５，６ 端部リンク部材、７ 中央リンク部材、１０ 土台、１１，２１ 回転軸連結部材、１２，１５，２２，２５ 回転軸体、Ｏ１，ｎ１ 回転軸、１２ａ 大径部、１２ｂ 小径部、２０ 先端部材、３０ 湾曲部材、３１，３１Ａ 回転軸支持部材、５０ リンク作動装置、５１ アクチュエータ、５２ 減速機構、５２ａ 出力軸体、５３ モータ固定部材、５４ フランジ面、５５ スペーサ、５６ ボルト、５７ 内径溝、５８ フランジ取付面、６１ エンドエフェクタ、１００ 制御装置。

Claims (4)

1. 基端側の第１リンクハブと、
   先端側の第２リンクハブと、
   前記第１リンクハブと前記第２リンクハブとを連結する少なくとも３つのリンク機構とを備え、
   前記少なくとも３つのリンク機構の各々は、
   前記第１リンクハブに対して回転可能に連結された第１端部リンク部材と、
   前記第２リンクハブに対して回転可能に連結された第２端部リンク部材と、
   前記第１端部リンク部材および前記第２端部リンク部材の各々に対して回転可能に連結された中央リンク部材とを含み、
   前記少なくとも３つのリンク機構において、
   前記第１リンクハブと前記第１端部リンク部材との回転対偶部の少なくとも３つの中心軸および、前記中央リンク部材の一方端の回転対偶部の中心軸は、第１リンクハブ中心点で交わり、
   前記第２リンクハブと前記第２端部リンク部材との回転対偶部の少なくとも３つの中心軸および前記中央リンク部材の他方端の回転対偶部の中心軸は、第２リンクハブ中心点で交わり、
   前記少なくとも３つのリンク機構のうち、２つのリンク機構における、前記第１リンクハブに対する前記第１端部リンク部材の角度から、前記第２リンクハブの位置および姿勢を球面三角法を用いて演算するように構成される制御装置をさらに備える、リンク作動装置。
2. 前記制御装置は、前記第１リンクハブに対する前記第１端部リンク部材の角度から、前記第２リンクハブの姿勢を求めるために以下の関係式を用いて演算するように構成される、請求項１に記載のリンク作動装置。
   
   Ｐ：原点姿勢における前記第２リンクハブ中心点の座標
   Ｐ’’： 原点姿勢から移動後の前記第２リンクハブ中心点の座標
   β_A1：少なくとも３つのリンク機構のうちの第１のリンク機構における前記第１リンクハブに対する前記第１端部リンク部材の角度
   β_A2：少なくとも３つのリンク機構のうちの第２のリンク機構における前記第１リンクハブに対する前記第１端部リンク部材の角度
   Ｒn（θ）：ｎ軸を中心として角θだけＰの座標を回転させる場合のロドリゲスの回転公式の３×３の回転行列
   ｎ１：第１のリンク機構において、第１リンクハブ中心点をＯとし、前記中央リンク部材の一方端の回転対偶部の中心軸と前記中央リンク部材の他方端の回転対偶部の中心軸との交点をＡとするときの直線ＯＡ
   ω₁：β_A1およびβ_A2によって決まる第１のリンク機構の前記中央リンク部材の倒れ角
3. 前記制御装置は、前記第２リンクハブ中心点の位置から前記第１端部リンク部材の姿勢を示す角度を球面三角法を用いて演算するように構成される、請求項１に記載のリンク作動装置。
4. 前記制御装置は、前記第２リンクハブ中心点の位置から前記第１端部リンク部材の姿勢を示す角度を、以下の関係式を用いて演算するように構成される、請求項３に記載のリンク作動装置。
   
   β_A1：少なくとも３つのリンク機構のうちの第１のリンク機構における前記第１リンクハブに対する前記第１端部リンク部材の角度
   ｄ：前記中央リンク部材の中央角
   α：アーム角
   Ｏ：前記第１リンクハブ中心点
   Ａ_０：前記第１リンクハブと前記第１端部リンク部材との回転中心軸と単位球面との交点
   Ａ_０’：点Ａ_０をｚ軸を中心に－π／２回転した点
   Ｐ：前記第２リンクハブ中心点

Images