特開 2023-104116 リンク作動装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2023104116

(43)【公開日】2023-07-28

(54)【発明の名称】リンク作動装置

(51)【国際特許分類】

   F16H 21/46 20060101AFI20230721BHJP

   B25J 11/00 20060101ALI20230721BHJP

【ＦＩ】

F16H21/46

B25J11/00 D

【審査請求】未請求

【請求項の数】3

【出願形態】ＯＬ

(21)【出願番号】P 2022004925

(22)【出願日】2022-01-17

(71)【出願人】

【識別番号】504174135

【氏名又は名称】国立大学法人九州工業大学

(71)【出願人】

【識別番号】000102692

【氏名又は名称】ＮＴＮ株式会社

(74)【代理人】

【識別番号】110001195

【氏名又は名称】弁理士法人深見特許事務所

(72)【発明者】

【氏名】林 朗弘

(72)【発明者】

【氏名】福丸 浩史

(72)【発明者】

【氏名】▲高▼木 俊樹

【テーマコード（参考）】

3C707

3J062

【Ｆターム（参考）】

3C707BS24

3C707HS27

3C707HT11

3J062AA21

3J062AA39

3J062AB28

3J062AC10

3J062BA14

3J062CB04

3J062CB14

3J062CB30

3J062CB33

(57)【要約】

【課題】受動関節を含めたすべての回転軸の角度を逆運動学で演算できるリンク作動装置を提供する。
【解決手段】リンク作動装置５０は、第２リンクハブ中心点ＰＢの位置から第１端部リンク部材５の姿勢を示す角度β_Ａ１と中央リンク部材７の姿勢を示す角度θ_１２とを球面三角法を用いて演算するように構成される制御装置１００を備える。
【選択図】図１

【特許請求の範囲】

【請求項1】

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

【請求項2】

前記制御装置は、前記第２リンクハブ中心点の位置から前記中央リンク部材の姿勢を示す角度を、以下の関係式を用いて演算するように構成される、請求項１に記載のリンク作動装置。

【数1】

【請求項3】

前記制御装置は、前記第２リンクハブ中心点の位置から前記第１端部リンク部材の姿勢を示す角度を、以下の関係式を用いて演算するように構成される、請求項２に記載のリンク作動装置。

【数2】

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、リンク作動装置に関する。

【背景技術】

【0002】

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

【0003】

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

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２０１５－１９４２０７号公報

【特許文献2】特開２０２０－２００８３９号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

特開２０２０－２００８３９号公報（特許文献２）のパラレルリンク機構の逆変換は、球面三角法を用いたことを特徴とし、先端側のリンクハブの中心点座標から基端側の各端部リンク部材の回転角度を導出できる。しかし、先端側のリンクハブの中心点座標から中央リンク部材の回転角度を導出する手法は今まで示されていない。

【0006】

コンピュータ上でティーチングの動作検証を行なう際、すべての回転軸の回転角度がわからないと、パラレルリンク機構の姿勢を厳密に表示することができず、ワークとパラレルリンク機構との干渉チェック等のシミュレーションができない。

【0007】

本開示は、このような課題を解決するためのものであって、その目的は受動関節を含めたすべての回転軸の角度を逆運動学で演算できるリンク作動装置を提供することである。

【課題を解決するための手段】

【0008】

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

【発明の効果】

【0009】

本開示のリンク作動装置によれば、受動関節を含めたすべての回転軸の角度を逆運動学で演算できる。

【図面の簡単な説明】

【0010】

【図1】リンク作動装置の、ある姿勢における正面図である。

【図2】リンク作動装置５０の構成のうち、１つのリンク機構４に対応する構成を代表的に示した図である。

【図3】パラレルリンク機構の基端側のリンクハブ２、基端側の端部リンク部材５を抽出して示した断面図である。

【図4】パラレルリンク機構１の３つのリンク機構４のうちの１つを抽出して直線で表現した模式図である。

【図5】基端側のリンクハブ中心軸ＱＡと先端側のリンクハブ中心軸ＱＢが同一線上にある状態（原点姿勢）のリンク作動装置の斜視図である。

【図6】原点姿勢におけるリンク作動装置の模式図である。

【図7】任意の姿勢（折れ角θ、旋回角φ）のリンク作動装置の斜視図である。

【図8】図７のモデル図である。

【図9】単位球面への点ＰＢ、点Ａの投影について説明するための図である。

【図10】端部リンク部材の回転角を得る逆運動学の式の導出の説明をするための第１の図である。

【図11】端部リンク部材の回転角を得る逆運動学の式の導出の説明をするための第２の図である。

【図12】中央リンク部材の回転角を得る逆運動学の式の導出の説明をするための第１の図である。

【図13】中央リンク部材の回転角を得る逆運動学の式の導出の説明をするための第２の図である。

【図14】アーム角を変更した変形例のリンク作動装置の斜視図である。

【図15】逆運動学によって先端側のリンクハブの中心点位置から基端側の端部リンク部材および中央リンク部材の角度を得る処理を説明するためのフローチャートである。

【発明を実施するための形態】

【0011】

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

【0012】

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

【0013】

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

【0014】

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

【0015】

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

【0016】

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

【0017】

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

【0018】

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

【0019】

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

【0020】

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

【0021】

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

【0022】

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

【0023】

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

【0024】

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

【0025】

図３には、３つのリンク機構４について、基端側のリンクハブ２と基端側の端部リンク部材５の回転対偶の中心軸Ｏ１と、端部リンク部材５と中央リンク部材７との回転対偶の中心軸Ｏ２と、リンクハブ中心点ＰＡとの関係が示されている。図３に示されるように、３つのアクチュエータ５１の回転軸である３本の中心軸Ｏ１とリンクハブ中心点ＰＡとは同一平面にある。図２に示すようにこの平面上のリンクハブ中心点ＰＡに対して斜め上方向から中心軸Ｏ２が通過する。先端側のリンクハブ３および先端側の端部リンク部材６の形状と、これらの位置関係（図示せず）も図３に示した基端側と同様である。図３の例では、中心軸Ｏ１と中心軸Ｏ２とが成す角度α（アーム角）が９０°とされているが、角度αは９０°以外であっても良い。

【0026】

パラレルリンク機構１は、２つの球面リンク機構を組み合わせた構造である。

【0027】

リンクハブ２と端部リンク部材５の回転対偶の中心軸Ｏ１と、端部リンク部材５と中央リンク部材７の回転対偶の中心軸Ｏ２とは、基端側においてリンクハブ中心点ＰＡ（図２、図３）で交差している。また、基端側において、リンクハブ２と端部リンク部材５の回転対偶とリンクハブ中心点ＰＡとの間の軸Ｏ１に沿う距離は、端部リンク部材５と中央リンク部材７の回転対偶とリンクハブ中心点ＰＡとの間の軸Ｏ２に沿う距離と同じである。

【0028】

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

【0029】

図４は、パラレルリンク機構１の３つのリンク機構４のうちの１つを抽出して直線で表現した模式図である。図４を用いて折れ角θと旋回角φについて説明する。

【0030】

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

【0031】

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

【0032】

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

【0033】

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

【0034】

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

【0035】

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

【0036】

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

【0037】

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

【0038】

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

【0039】

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

【0040】

図７は、任意の姿勢（折れ角θ、旋回角φ）のリンク作動装置の斜視図である。図８は、図７のモデル図である。任意の姿勢（折れ角θ、旋回角φ）では、基端側のリンクハブ中心軸ＱＡに対して先端側のリンクハブ中心軸ＱＢが、ある角度（折れ角θ）を成す。

【0041】

点Ａは常に二等分面ＰＬ１上に存在し、この点Ａを一つの二自由度関節とみなすことができる。折れ角θの時、基端側のリンクハブ中心点ＰＡと先端側のリンクハブ中心点ＰＢを結ぶ直線と基端側のリンクハブ２の中心軸ＱＡとが成す角はθ／２となる。また、基端側のリンクハブ中心点ＰＡと先端側のリンクハブ中心点ＰＢとを結ぶ直線と基端側のリンクハブ中心点ＰＡと点Ａを通る直線が成す角はｄ／２となる。パラレルリンク機構１はこれらの関係を維持したまま運動する機構である。

【0042】

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

【0043】

以上では、実際の中心ＰＢ、点Ａについて説明したが、これらの点を単位球面に投影し、単位球面における三角形のモデルを使用することによって、計算を簡単にすることができる。

【0044】

図９は、単位球面への点ＰＢ、点Ａの投影について説明するための図である。図９では、基端側のリンクハブ２の中心ＰＡを中心とする半径が１である単位球面ＧＵが点ＰＢおよび点Ａよりも内側にある場合が示される。点ＰＢと点ＰＡ（中心Ｏ）とを結ぶ直線が単位球面ＧＵと交わる点を点Ｐとし、点Ａと点ＰＡ（中心Ｏ）とを結ぶ直線が単位球面ＧＵと交わる点を点Ａ_１とする。

【0045】

図１０に示すように、各回転軸と基端側および先端側のリンクハブ中心点ベクトルと、単位球面ＧＵとの交点を考える。本明細書における逆運動学式では、回転軸周りの回転量のみを考えるため、実際の回転対偶部がある座標は考慮しなくてよい。また、すべての点はＯを中心に回転運動を行なうため、単位球面ＧＵに投影された各交点の角度の関係は維持される。

【0046】

そのため、簡単のために、図１０～図１３に示すような単位球面上の位置関係から逆運動学を導出できる。

【0047】

図１０～図１３では、半径を１とした単位球面における球面三角形のモデルを示す。図１０以降の球面ＧＵは、中心Ｏからの距離を１とした単位球を示している。これにより、球面上の大円の円弧の長さが円弧の中心角と等しくなるため、後述する球面三角法の正弦・余弦定理を簡単に適用することができる。

【0048】

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

【0049】

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

【0050】

図１０に単位球と球面三角形のモデルが示される。一般的に、球面上の三点Ａ，Ｂ，Ｃが成す球面三角形ＡＢＣは、辺ＢＣ，ＣＡ，ＡＢをそれぞれａ，ｂ，ｃとし、弧ＡＢと弧ＡＣが成す角度をＡ、弧ＡＢと弧ＢＣが成す角度をＢ、弧ＣＢと弧ＡＣが成す角度をＣとした場合、球面三角法より球面余弦定理が次の式（１）、球面正弦定理が式（２）で表される。

【0051】

【数1】

【0052】

【数2】

【0053】

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

【0054】

【数3】

【0055】

【数4】

【0056】

同様に球面三角形Ｔ_１に対して球面余弦定理を用いると以下の式（５）から式（６）を得る。

【0057】

【数5】

【0058】

【数6】

【0059】

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

【0060】

【数7】

【0061】

【数8】

【0062】

アーム角αは基端側の端部リンク部材５の両端の回転対偶の軸が成す角度であり、部材の構造により決定する値である。また、∠Ａ_０ＯＰ、∠Ａ_０’ＯＰは、先端側のリンクハブ中心点が決まれば演算できる角度である。したがってｅ_１およびｅ_２は先端側のリンクハブ中心点を投影した点Ｐ（θ，φ）の関数となる。よって、原点姿勢の時の角β_A1を０とした場合、角β_A1は以下の式（９）で表すことができる。

【0063】

【数9】

【0064】

以上より、基端側の端部リンク部材５の、ＸＹ平面を基準とした回転角度β_A1を、リンクハブ中心点を投影した点Ｐ（θ，φ）の関数として計算できることが示される。

【0065】

次に、中央リンク部材の第３回転軸周りの回転角θ_１２について、球面三角法の余弦定理を用いて計算する。図１２は、中央リンク部材の回転角を得る逆運動学の式の導出の説明をするための第１の図である。図１３は、中央リンク部材の回転角を得る逆運動学の式の導出の説明をするための第２の図である。図１２および図１３を用いて逆運動学によって中央リンク部材の回転角θ_１２を得る式の導出について説明する。

【0066】

図１２，図１３の記号について説明する。点Ｏは、基端側のリンクハブ２の中心点ＰＡであり、座標系の原点である。点Ｐは、点Ｏから先端側のリンクハブ３の中心点ＰＢに向かうベクトルと、点Ｏを中心とする単位球面ＧＵとの交点である。点Ａ_０は、第１回転軸（図３のＯ１）と単位球面ＧＵとの交点である。点Ａ_１は、第３回転軸（図３のＯ２）と単位球面ＧＵとの交点である。点Ａ_０’は、点Ａ_１が第１回転軸（図３のＯ１）の回転によって動く軌道と、ＸＹ平面との交点である。角β_Ａ１は、基端側の端部リンク部材５の、ＸＹ平面を基準とした回転角度である。角θ_１２は、中央リンク部材７の第３回転軸（図３のＯ２）周りの回転角度である。

【0067】

また、図１２、図１３に示されている端部リンク部材のアーム角αと、中央リンク部材７の軸角γは、パラレルリンク機構の設計時に決まる値である。

【0068】

なお、図１３において、パラレルリンク機構の姿勢が異なるため、点Ｐの位置が図１０、図１１とは異なる。このため、図１３の球面三角形Ｔ_１，Ｔ_２の形状も図１１とは異なっている。

【0069】

ここで、図１３に示すＰ－Ａ_０－Ａ_１を結ぶ球面三角形Ｔ_１は、単位球面上の三角形である。球面三角形の辺を成す２点と球の中心の作る角度は、辺の長さと等しい。また、∠ＰＯＡ_０は、点Ａ_０、点Ｐの各座標が既知であることから、内積の定義より式（１０）が成立する。

【0070】

【数10】

【0071】

次に、∠Ａ_０ＯＡ_０’は機構のアーム角αと一致するため、下式（１１）が成立する。

【0072】

【数11】

【0073】

また、∠ＰＯＡ_１は、図８の∠ＰＢ－Ｏ－Ａと同じｄ／２であり、中央リンク部材７の軸角γを用いて表すことができ、下式（１２）が成立する。

【0074】

【数12】

【0075】

したがって、回転角θ_１２は、球面三角形ＰＡ_１Ａ_０に、式（１）の余弦定理を適用し式変形すると、下式（１３）が得られる。

【0076】

【数13】

【0077】

以上のように、逆運動学の式（９）で基端側の端部リンク部材５の角β（第１関節角度）を算出でき、逆運動学の式（１３）で回転角θ_１２を算出できる。

【0078】

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

【0079】

また、本実施の形態で示した計算手法は、任意のアーム角αに対して逆運動学の計算として適応可能であり、リンク作動装置の多様な設計仕様に対応できる。図１４は、アーム角を変更した変形例のリンク作動装置の斜視図である。図１４に示すリンク作動装置５０Ａの構成要素には、図１の相当する構成要素と同じ符号を付している。図１には、端部リンク部材５，６のアーム角αが９０°のリンク作動装置５０を例示したが、例えば、図１４に示した端部リンク部材５，６アーム角αは８０°である。このようなリンク作動装置５０Ａに対しても、本実施の形態で示した計算手法が適用できる。

【0080】

ここで、式（９）においては角度を用いているが、単位球においてある２直線が成す中心角とその弧長は等しいため、角度の代わりに弧長を用いた式を使用してもよい。正弦あるいは余弦関数の項は、単位球面で考えているので、例えば式（１３）に示したようなベクトルＰやＡ_０の内積を用いるか、あるいは外積のノルムを用いることによって、計算処理の負担を減らすこともできる。

【0081】

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

【0082】

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

【0083】

ステップＳ１１において制御装置１００は、入力装置または図示しない上位の制御装置から先端側のリンクハブの位置情報（目標位置）を受信する。そして、ステップＳ１２，Ｓ１３において制御装置１００は、逆運動学について説明した式（１）～（９）を用いて、角β_A1を算出し、同様な手順で角β_A2も算出する。さらに、ステップＳ１４において、制御装置１００は、逆運動学について説明した式（１０）～（１３）を用いて、中央リンク部材の角θ_１２を算出する。

【0084】

さらに、ステップＳ１５においては、現在の姿勢から目的の姿勢に至るまでのパラレルリンク機構とワークおよび周囲の装置との間の干渉が発生するかを確認する干渉シミュレーションが実行される。干渉シミュレーションは、例えば、姿勢が変更される状態を示す軌跡をグラフィックでディスプレイに表示するような処理を含んでも良い。そして、干渉シミュレーションに問題がなければ、ステップＳ１６において制御装置１００はアクチュエータ５１を制御する。このような処理を繰り返すことによって、先端側リンクハブの位置および姿勢が所望の状態に制御される。

【0085】

＜まとめ＞
最後に、再び図を参照して本実施の形態について総括する。

【0086】

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

【0087】

好ましくは、制御装置１００は、第２リンクハブ中心点ＰＢの位置から中央リンク部材７の姿勢を示す角度θ_１２を、既出の式（１３）を用いて演算するように構成される。

【0088】

より好ましくは、制御装置１００は、第２リンクハブ中心点ＰＢの位置から第１端部リンク部材５の姿勢を示す角度θ_１２を、既出の式（７）～（９）を用いて演算するように構成される。

【0089】

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

【符号の説明】

【0090】

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

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】