特開2020-200839(P2020-200839A)IP Force 特許公報掲載プロジェクト 2022.1.31 β版

知財求人 - 知財ポータルサイト「IP Force」

▶ 国立大学法人九州工業大学の特許一覧
<>
  • 特開2020200839-リンク作動装置 図000027
  • 特開2020200839-リンク作動装置 図000028
  • 特開2020200839-リンク作動装置 図000029
  • 特開2020200839-リンク作動装置 図000030
  • 特開2020200839-リンク作動装置 図000031
  • 特開2020200839-リンク作動装置 図000032
  • 特開2020200839-リンク作動装置 図000033
  • 特開2020200839-リンク作動装置 図000034
  • 特開2020200839-リンク作動装置 図000035
  • 特開2020200839-リンク作動装置 図000036
  • 特開2020200839-リンク作動装置 図000037
  • 特開2020200839-リンク作動装置 図000038
  • 特開2020200839-リンク作動装置 図000039
  • 特開2020200839-リンク作動装置 図000040
  • 特開2020200839-リンク作動装置 図000041
  • 特開2020200839-リンク作動装置 図000042
< >
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】特開2020-200839(P2020-200839A)
(43)【公開日】2020年12月17日
(54)【発明の名称】リンク作動装置
(51)【国際特許分類】
   F16H 21/52 20060101AFI20201120BHJP
   B25J 11/00 20060101ALI20201120BHJP
【FI】
   F16H21/52
   B25J11/00 D
【審査請求】未請求
【請求項の数】4
【出願形態】OL
【全頁数】20
(21)【出願番号】特願2019-105693(P2019-105693)
(22)【出願日】2019年6月5日
(71)【出願人】
【識別番号】504174135
【氏名又は名称】国立大学法人九州工業大学
(71)【出願人】
【識別番号】000102692
【氏名又は名称】NTN株式会社
(74)【代理人】
【識別番号】110001195
【氏名又は名称】特許業務法人深見特許事務所
(72)【発明者】
【氏名】林 朗弘
(72)【発明者】
【氏名】福丸 浩史
(72)【発明者】
【氏名】高木 俊樹
(72)【発明者】
【氏名】野瀬 賢蔵
(72)【発明者】
【氏名】松澤 英樹
【テーマコード(参考)】
3C707
3J062
【Fターム(参考)】
3C707BS24
3C707HT11
3J062AA39
3J062AB28
3J062AC09
3J062AC10
3J062BA14
3J062CB04
3J062CB18
3J062CB28
3J062CB30
3J062CB33
3J062CG67
3J062CG83
(57)【要約】
【課題】リアルタイムで順変換を実行することが可能なリンク作動装置を提供する。
【解決手段】球面駆動機構が構成されたパラレルリンク機構1を作動させるリンク作動装置50は、少なくとも3つのリンク機構4のうち、2つのリンク機構における、第1リンクハブ2に対する第1端部リンク部材5の姿勢を示す角度βA1,βA2から、第2リンクハブ3の姿勢を球面三角法を用いて演算するように構成される制御装置100を備える。
【選択図】図1
【特許請求の範囲】
【請求項1】
基端側の第1リンクハブと、
先端側の第2リンクハブと、
前記第1リンクハブと前記第2リンクハブとを連結する少なくとも3つのリンク機構とを備え、
前記少なくとも3つのリンク機構の各々は、
前記第1リンクハブに対して回転可能に連結された第1端部リンク部材と、
前記第2リンクハブに対して回転可能に連結された第2端部リンク部材と、
前記第1端部リンク部材および前記第2端部リンク部材の各々に対して回転可能に連結された中央リンク部材とを含み、
前記少なくとも3つのリンク機構において、
前記第1リンクハブと前記第1端部リンク部材との回転対偶部の少なくとも3つの中心軸および、前記中央リンク部材の一方端の回転対偶部の中心軸は、第1リンクハブ中心点で交わり、
前記第2リンクハブと前記第2端部リンク部材との回転対偶部の少なくとも3つの中心軸および前記中央リンク部材の他方端の回転対偶部の中心軸は、第2リンクハブ中心点で交わり、
前記少なくとも3つのリンク機構のうち、2つのリンク機構における、前記第1リンクハブに対する前記第1端部リンク部材の角度から、前記第2リンクハブの位置および姿勢を球面三角法を用いて演算するように構成される制御装置をさらに備える、リンク作動装置。
【請求項2】
前記制御装置は、前記第1リンクハブに対する前記第1端部リンク部材の角度から、前記第2リンクハブの姿勢を求めるために以下の関係式を用いて演算するように構成される、請求項1に記載のリンク作動装置。
【数1】

P:原点姿勢における前記第2リンクハブ中心点の座標
P’’: 原点姿勢から移動後の前記第2リンクハブ中心点の座標
βA1:少なくとも3つのリンク機構のうちの第1のリンク機構における前記第1リンクハブに対する前記第1端部リンク部材の角度
βA2:少なくとも3つのリンク機構のうちの第2のリンク機構における前記第1リンクハブに対する前記第1端部リンク部材の角度
Rn(θ):n軸を中心として角θだけPの座標を回転させる場合のロドリゲスの回転公式の3×3の回転行列
n1:第1のリンク機構において、第1リンクハブ中心点をOとし、前記中央リンク部材の一方端の回転対偶部の中心軸と前記中央リンク部材の他方端の回転対偶部の中心軸との交点をAとするときの直線OA
ω1:βA1およびβA2によって決まる第1のリンク機構の前記中央リンク部材の倒れ角
【請求項3】
前記制御装置は、前記第2リンクハブ中心点の位置から前記第1端部リンク部材の姿勢を示す角度を球面三角法を用いて演算するように構成される、請求項1に記載のリンク作動装置。
【請求項4】
前記制御装置は、前記第2リンクハブ中心点の位置から前記第1端部リンク部材の姿勢を示す角度を、以下の関係式を用いて演算するように構成される、請求項3に記載のリンク作動装置。
【数2】

βA1:少なくとも3つのリンク機構のうちの第1のリンク機構における前記第1リンクハブに対する前記第1端部リンク部材の角度
d:前記中央リンク部材の中央角
α:アーム角
O:前記第1リンクハブ中心点
:前記第1リンクハブと前記第1端部リンク部材との回転中心軸と単位球面との交点
’:点Aをz軸を中心に−π/2回転した点
P:前記第2リンクハブ中心点
【発明の詳細な説明】
【技術分野】
【0001】
この発明は、リンク作動装置に関する。
【背景技術】
【0002】
リンク作動装置は、精密で広範な作動範囲を必要とする産業機器等に用いられる。リンク作動装置は、駆動源とリンク機構からなる。リンク機構の一種としてパラレルリンク機構が知られている。
【0003】
コンパクトな構成でありながら、精密で広範な作動範囲の動作が可能なリンク作動装置として、例えば、特許6133162号公報(特許文献1)に示されるようなものが提案されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特許6133162号公報
【特許文献2】特開2005−098497号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
特許6133162号公報(特許文献1)に示されるようなリンク作動装置において、先端部と基端部の関係式では、ある先端座標における姿勢制御用駆動源の回転角を逆変換で求めている。同様に、順変換は2台の姿勢制御用駆動源の2軸の回転角から先端位置の算出方法が示されているが、理論式に三角関数・逆三角関数の積和が多数現れることから、計算誤差の蓄積が起きる。
【0006】
また、特許文献2において、順変換は収束演算による近似解であり、繰り返し演算が必要なため先端位置の算出に時間がかかる。そのため、リアルタイムで処理するのが困難であるという問題点があり、ダイレクトティーチングや異常復帰後等で現在位置が不明な状態からの長距離移動動作が不可能であった。
【0007】
本発明は、このような課題を解決するためのものであって、その目的は、計算誤差の蓄積を少なくし位置決め精度の向上が可能で、リアルタイムで順変換を実行することが可能なリンク作動装置を提供することである。
【課題を解決するための手段】
【0008】
本開示は、リンク作動装置に関する。リンク作動装置は、基端側の第1リンクハブと、先端側の第2リンクハブと、第1リンクハブと第2リンクハブとを連結する少なくとも3つのリンク機構とを備える。少なくとも3つのリンク機構の各々は、第1リンクハブに対して回転可能に連結された第1端部リンク部材と、第2リンクハブに対して回転可能に連結された第2端部リンク部材と、第1端部リンク部材および第2端部リンク部材の各々に対して回転可能に連結された中央リンク部材とを含む。少なくとも3つのリンク機構において、第1リンクハブと第1端部リンク部材との回転対偶部の少なくとも3つの中心軸および、中央リンク部材の一方端の回転対偶部の中心軸は、第1リンクハブ中心点で交わり、第2リンクハブと第2端部リンク部材との回転対偶部の少なくとも3つの中心軸および中央リンク部材の他方端の回転対偶部の中心軸は、第2リンクハブ中心点で交わる。リンク作動装置は、少なくとも3つのリンク機構のうち、2つのリンク機構における、第1リンクハブに対する第1端部リンク部材の角度から、第2リンクハブの位置および姿勢を球面三角法を用いて演算するように構成される制御装置をさらに備える。
【0009】
好ましくは、制御装置は、第2リンクハブ中心点の位置から第1端部リンク部材の姿勢を示す角度を球面三角法を用いて演算するように構成される。
【発明の効果】
【0010】
本発明によれば、球面駆動機構を有するリンク作動装置において、リアルタイムで順変換を実行することが可能となる。
【図面の簡単な説明】
【0011】
図1】リンク作動装置の、ある姿勢における正面図である。
図2】リンク作動装置50の構成のうち、1組のリンク機構4に対応する構成を代表的に示した図である。
図3】パラレルリンク機構の基端側のリンクハブ2、基端側の端部リンク部材5を抽出して示した断面図である。
図4】パラレルリンク機構1の3つのリンク機構4のうちの1つを抽出して直線で表現した模式図である。
図5】基端側のリンクハブ中心軸QAと先端側のリンクハブ中心軸QBが同一線上にある状態のリンク作動装置の斜視図である。
図6】基端側のリンクハブ中心軸QAと先端側のリンクハブ中心軸QBが同一線上にある状態のリンク作動装置の模式図である。
図7】基端側のリンクハブ中心軸QAに対して先端側のリンクハブ中心軸QBが、ある角度をなす状態のリンク作動装置の斜視図である。
図8】基端側のリンクハブ中心軸QAに対して先端側のリンクハブ中心軸QBが、ある角度をなす状態のリンク作動装置の模式図である。
図9】ωの計算モデルを示す図である。
図10】点Aの半径を1とした単位球面における三角形のモデルを示す図である。
図11】単位球面における別の三角形のモデルを示す図である。
図12】順運動学のモデルを示す図である。
図13】逆運動学の式の導出の説明をするための第1の図である。
図14】逆運動学の式の導出の説明をするための第2の図である。
図15】順運動学によって基端側の端部リンク部材の角度から先端側のリンクハブの中心点位置を得る処理を説明するためのフローチャートである。
図16】逆運動学によって先端側のリンクハブの中心点位置から基端側の端部リンク部材の角度を得る処理を説明するためのフローチャートである。
【発明を実施するための形態】
【0012】
以下、本発明の実施の形態について図面を参照しつつ説明する。なお、以下の図面において同一または相当する部分には同一の参照番号を付し、その説明は繰り返さない。
【0013】
<リンク作動装置の構成>
この発明の一実施形態に係るパラレルリンク機構を備えたリンク作動装置の構成を図1図3と共に説明する。
【0014】
図1は、リンク作動装置の、ある姿勢における正面図である。図1を参照して、リンク作動装置50は、パラレルリンク機構1と、パラレルリンク機構1の姿勢変更用のアクチュエータ51と、アクチュエータの駆動力を減速してパラレルリンク機構に伝達する減速機構52と、アクチュエータ51を制御する制御装置100とを備える。
【0015】
パラレルリンク機構1は、基端側のリンクハブ2に対し先端側のリンクハブ3を3組のリンク機構4を介して姿勢変更可能に連結したものである。なお、リンク機構4の数は、4組以上であっても良い。先端側のリンクハブ3には、エンドエフェクタ61が設置される。
【0016】
図2は、リンク作動装置50の構成のうち、1組のリンク機構4に対応する構成を代表的に示した図である。図2を参照して、各リンク機構4は、基端側の端部リンク部材5と、先端側の端部リンク部材6と、中央リンク部材7とを含み、4つの回転対偶からなる4節連鎖のリンク機構を構成する。基端側の端部リンク部材5は、一端が基端側のリンクハブ2に回転自在に連結されている。同様に、先端側の端部リンク部材6は、一端が先端側のリンクハブ3に回転自在に連結されている。中央リンク部材7の一端には、基端側の端部リンク部材5の他端が回転自在に連結されている。中央リンク部材7の他端には、先端側の端部リンク部材6の他端が回転自在に連結されている。
【0017】
図1図2に示すように、基端側のリンクハブ2は、平板状の土台10と、この土台10に円周方向に等間隔で配置された3個の回転軸連結部材11とで構成される。各回転軸連結部材11には、軸心がリンクハブ中心軸QAと交差する回転軸体12が回転自在に連結されている。この回転軸体12に、基端側の端部リンク部材5の一端が連結される。基端側の端部リンク部材5の他端には、中央リンク部材7の一端に回転自在に連結された回転軸体15が連結される。本実施例において、回転軸連結部材11は土台10に円周方向に等間隔で配置されているが、必ずしもその限りではない。
【0018】
リンクハブ3の回転軸体22および中央リンク部材7の回転軸体25も、前記回転軸体12,15と同じ形状である。
【0019】
図1図2に示すように、先端側のリンクハブ3は、平板状の先端部材20と、先端部材20に円周方向に等間隔に配置された3個の回転軸連結部材21とで構成される。各回転軸連結部材21は、軸心がリンクハブ中心軸QBと交差する回転軸体22が回転自在に連結されている。このリンクハブ3の回転軸体22に、先端側の端部リンク部材6の一端が連結される。先端側の端部リンク部材6の他端には、中央リンク部材7の他端に回転自在に連結された回転軸体25が連結される。
【0020】
端部リンク部材5と中央リンク部材7との回転対偶の中心軸O2(A)と、端部リンク部材6と中央リンク部材7との回転対偶の中心軸O2(B)とは、点Aにおいて交差角γで交差する。
【0021】
図3は、パラレルリンク機構の基端側のリンクハブ2、基端側の端部リンク部材5を抽出して示した断面図である。図3には、図1の姿勢のパラレルリンク機構の先端側のリンクハブ3、先端側の端部リンク部材6および中央リンク部材7を取り除いた状態が示されている。なお、3つの端部リンク部材5の各々については、両端の回転対偶部の回転軸O1、O2を含む面における断面が示されている。
【0022】
図1および図3を参照して、3組のリンク機構4のそれぞれに、基端側のリンクハブ2に対する先端側のリンクハブ3の姿勢を任意に変更する姿勢変更用のアクチュエータ51が設けられる。各アクチュエータ51には減速機構52が設けられている。各アクチュエータ51は、ロータリアクチュエータであり、基端側のリンクハブ2の土台10の上面に、回転軸体12と同軸上に設置されている。アクチュエータ51と減速機構52は一体に設けられ、モータ固定部材53により減速機構52が土台10に固定されている。なお、3組のリンク機構4のうち少なくとも2組にアクチュエータ51を設ければ、基端側のリンクハブ2に対する先端側のリンクハブ3の姿勢を確定することができる。
【0023】
図3において、減速機構52はフランジ継手を形成する大径の出力軸体52aを有する。出力軸体52aの先端面は、出力軸体52aの中心線と直交する平面状のフランジ面54となっている。出力軸体52aは、スペーサ55を介して、基端側の端部リンク部材5の外径側の回転軸支持部材31にボルト56で接続されている。リンクハブ2と端部リンク部材5の回転対偶部の回転軸体12は、大径部12aと小径部12bとからなる。小径部12bは軸受の内輪に挿通され、大径部12aは減速機構52の出力軸体52aに設けられた内径溝57に嵌っている。
【0024】
端部リンク部材5は、L字形状を有する。端部リンク部材5は、1つの湾曲部材30と、この湾曲部材30の両端の外径側の側面と内径側の側面にそれぞれ固定された計4つの回転軸支持部材31とで構成される。4つの回転軸支持部材31は同一形状ではなく、基端側のリンクハブ2との回転対偶部に設けられる外径側の回転軸支持部材31Aは、減速機構52のフランジ面54とスペーサ55を介して結合されるフランジ取付面58を有する。本実施例において、端部リンク部材5は、L字形状を有しているが必ずしもL字である必要はない。
【0025】
このリンク作動装置50には、例えば図1に示すように、先端側のリンクハブ3にエンドエフェクタ61が設置される。アクチュエータ51が基端側のリンクハブ2に対する先端側のリンクハブ3の姿勢を変更することによって、エンドエフェクタ61の2自由度の角度を制御することができる。
【0026】
図3には、3組のリンク機構4について、基端側のリンクハブ2と基端側の端部リンク部材5の回転対偶の中心軸O1と、端部リンク部材5と中央リンク部材7との回転対偶の中心軸O2と、球面リンク中心PAとの関係が示されている。先端側のリンクハブ3および先端側の端部リンク部材6の形状と、これらの位置関係も図3と同様である(図示せず)。図3の例では、中心軸O1と中心軸O2とが成す角度α(アーム角)が90°とされているが、角度αは90°以外であっても良い。
【0027】
パラレルリンク機構1は、2つの球面リンク機構を組み合わせた構造である。
リンクハブ2と端部リンク部材5の回転対偶の中心軸O1と、端部リンク部材5と中央リンク部材7の回転対偶の中心軸O2とは、基端側において球面リンク中心PA(図2図3)で交差している。また、基端側において、リンクハブ2と端部リンク部材5の回転対偶と球面リンク中心PAとの間の軸O1に沿う距離は、端部リンク部材5と中央リンク部材7の回転対偶と球面リンク中心PAとの間の軸O2に沿う距離と同じである。
【0028】
図示しないが、同様に、リンクハブ3と端部リンク部材6の回転対偶の中心軸と、端部リンク部材6と中央リンク部材7の回転対偶の中心軸とは、先端側において球面リンク中心PB(図2)で交差している。また、先端側において、リンクハブ3と端部リンク部材6の各回転対偶と球面リンク中心PBとの距離は、端部リンク部材6と中央リンク部材7の回転対偶と球面リンク中心PBからの距離と同じである。
【0029】
図4は、パラレルリンク機構1の3つのリンク機構4のうちの1つを抽出して直線で表現した模式図である。3組のリンク機構4は、幾何学的に同一の対称形状をなす。幾何学的に同一の対称形状とは、図4に示すように、端部リンク部材5,6および中央リンク部材7を直線で表現し回転対偶を丸で示した幾何学モデルが、二等分面に対して、基端側部分と先端側部分が対称を成す形状であることを言う。本実施の形態のパラレルリンク機構1は回転対称タイプで、基端側のリンクハブ2および基端側の端部リンク部材5と、先端側のリンクハブ3および先端側の端部リンク部材6との位置関係が、中央リンク部材7の中心線Cに対して回転対称となる位置構成になっている。
【0030】
基端側のリンクハブ2と先端側のリンクハブ3と3組のリンク機構4とによって、基端側のリンクハブ2に対し先端側のリンクハブ3が直交2軸周りに回転自在な2自由度機構が構成される。2自由度機構は、言い換えると、基端側のリンクハブ2に対して先端側のリンクハブ3の姿勢を、2自由度で自在に変更可能な機構である。この2自由度機構は、コンパクトでありながら、基端側のリンクハブ2に対する先端側のリンクハブ3の可動範囲を広くとれる。
【0031】
例えば、球面リンク中心PAを通り、リンクハブ2と端部リンク部材5の回転対偶の中心軸O1(図3)と直角に交わる直線をリンクハブ2の中心軸をリンクハブ中心軸QAとする。また、球面リンク中心PBを通り、リンクハブ3と端部リンク部材6の回転対偶の中心軸(図示せず)と直角に交わる直線をリンクハブ3の中心軸QBとする。
【0032】
この場合、基端側のリンクハブ2の中心軸QAと先端側のリンクハブ3の中心軸QBの折れ角θ(図4)の最大値を約±90°とすることができる。また、基端側のリンクハブ2に対する先端側のリンクハブ3の旋回角φ(図4)を0°〜360°の範囲に設定できる。折れ角θは、中心軸QAおよび中心軸QBを含む垂直面において中心軸QAに対して中心軸QBが傾斜した角度のことであり、旋回角φは、基準位置に対して中心軸QBの水平面への投影直線がなす角度のことである。
【0033】
基端側のリンクハブ2に対する先端側のリンクハブ3の姿勢は、基端側のリンクハブ中心軸QAと先端側のリンクハブ中心軸QBの交点PCを回転中心として変更される。姿勢が変化しても、基端側と先端側の球面リンク中心PA,PB間の距離D(図4)は変化しない。
【0034】
パラレルリンク機構1においては以下の条件が成立している。すなわち、各リンク機構4におけるリンクハブ2と端部リンク部材5の回転対偶の中心軸O1とリンクハブ3と端部リンク部材6の回転対偶の中心軸O2とのなす角度が互いに等しい。球面リンク中心PA,PBから回転対偶部までの長さが互いに等しい。各リンク機構4のリンクハブ2と端部リンク部材5の回転対偶の中心軸O1(A)が、基端側において球面リンク中心PAと交差する。各リンク機構4の端部リンク部材5と中央リンク部材7の回転対偶の中心軸O2(A)が、基端側において球面リンク中心PAと交差する。各リンク機構4のリンクハブ3と端部リンク部材6の回転対偶の中心軸O1(B)が、先端側において球面リンク中心PBと交差する。各リンク機構4の端部リンク部材6と中央リンク部材7の回転対偶の中心軸O2(B)が、先端側において球面リンク中心PBと交差する。基端側の端部リンク部材5と先端側の端部リンク部材6の幾何学的形状が等しく、かつ中央リンク部材7についても基端側の先端側とで形状が等しい。これらの条件が成立しているとき、中央リンク部材7の対称面に対して、中央リンク部材7と端部リンク部材5,6との角度位置関係を基端側と先端側とで同じにすれば、幾何学的対称性から基端側のリンクハブ2および基端側の端部リンク部材5と、先端側のリンクハブ3および先端側の端部リンク部材6とは二等分面に対して対称に同じ動きをする。
【0035】
以上説明した構成のリンク作動装置50における順運動学および逆運動学について説明する。ロボット、マニュピレータ等の関節を制御する方法として順運動学(Forward Kinematics)と逆運動学(Inverse Kinematics)が知られている。本実施の形態の説明において、順運動学は、基端側のリンクハブ2に対して端部リンク部材5がなす角度から先端側のリンクハブの中心位置および方向が決まることを言う。また、逆運動学は、先端側のリンクハブの中心位置および方向から基端側のリンクハブ2に対して端部リンク部材5がなす角度が決まることをいう。なお、本実施の形態では順運動学、逆運動学は、ともに先端側中心位置に対する計算を示しており、方向までは求めていないが、先端位置が求まれば、機構の幾何学的性質からから方向も簡単な計算で求められる。
【0036】
<順運動学>
図5は、基端側のリンクハブ中心軸QAと先端側のリンクハブ中心軸QBが同一線上にある状態のリンク作動装置の斜視図である。図6は、基端側のリンクハブ中心軸QAと先端側のリンクハブ中心軸QBが同一線上にある状態のリンク作動装置の模式図である。
【0037】
リンク作動装置50の原点姿勢が図5に示される。本明細書において、原点姿勢とは、基端側のリンクハブ2の中心軸QAと先端側のリンクハブ3の中心軸QBが一致している状態の姿勢をいう。すなわち、原点姿勢は、リンク作動装置50の折れ角θが0度の姿勢である。
【0038】
図2には、図1の原点姿勢における3組のリンク機構のうち1組のみについて示した正面図が示され、図6にはリンク作動装置を簡略化したモデル図が示されている。リンク機構は、簡略化すると基端側および先端側の各リンクハブ、基端側および先端側の各端部リンク部材、ならびに中央リンク部材で表すことができる。
【0039】
リンク作動装置50のパラレルリンク機構1は、基端側のリンクハブ中心点PAを中心とする基端側の球面リンクGAと、先端側のリンクハブ中心点PBを中心とする先端側の球面リンクGBとが交わって成す平面である二等分面PL1を境に鏡面対象となる構成となっている。基端側の端部リンク部材5と中央リンク部材7との回転対偶部の中心軸O2(A)と、先端側の端部リンク部材6と中央リンク部材7との回転対偶部の中心軸O2(B)が交わる点Aは二等分面PL1上に存在する。また、基端側の端部リンク部材5と中央リンク部材7との回転対偶部の中心軸O2(A)と、先端側の端部リンク部材6と中央リンク部材7との回転対偶部の中心軸O2(B)とが成す角を軸角γと称する。中央リンク部材7が成す角度を中央角dと称する。中央角dは、正確には、基端側の端部リンク部材5と中央リンク部材7との回転対偶部の中心軸O2(A)に垂直な直線と、先端側の端部リンク部材6と中央リンク部材7との回転対偶部の中心軸O2(B)に垂直な直線が、二等分面上で交わって成す角である。軸角γおよび中央角dは、パラレルリンク機構1を設計する時に決まる定数である。また、図6から、中央角dは、軸角γを用いるとd=π−γ(rad)で表すことができる。
【0040】
図7は、基端側のリンクハブ中心軸QAに対して先端側のリンクハブ中心軸QBが、ある角度を成す状態のリンク作動装置の斜視図である。図8は、基端側のリンクハブ中心軸QAに対して先端側のリンクハブ中心軸QBが、ある角度を成す状態のリンク作動装置の模式図である。
【0041】
図7には任意の姿勢(折れ角θ、旋回角φ)のリンク作動装置の斜視図が示され、図8には図7のモデル図が示される。点Aは常に二等分面PL1上に存在し、この点Aを一つの二自由度関節とみなすことができる。折れ角θの時、基端側の球面リンク中心PAと先端側の球面リンク中心PBを結ぶ直線と基端側のリンクハブ2の中心軸QAとが成す角はθ/2となる。また、基端側の球面リンク中心PAと先端側の球面リンク中心PBとを結ぶ直線と基端側の球面リンク中心PAと点Aを通る直線が成す角はd/2となる。パラレルリンク機構1はこれらの関係を維持したまま運動する機構である。
【0042】
パラレルリンク機構1は2自由度のため、2つのアーム回転角βが決まれば先端側のリンクハブ3の位置を導出することができる。パラレルリンク機構1の先端側のリンクハブ3の中心PBは、図8において、基端側のリンクハブ2の中心PAを中心Oとして球面GP上を運動する。
【0043】
図9は、ωの計算モデルを示す図である。図10は、点Aの半径を1とした単位球面における三角形のモデルを示す図である。図11は、単位球面における別の三角形のモデルを示す図である。図12は、順運動学のモデルを示す図である。図9以降の球面は、中心Oから点Aまでの距離を1とした場合の単位球を示している。これにより、球面上の大円の円弧の長さが円弧の中心角と等しくなるため、後述する球面三角法の正弦・余弦定理を簡単に適用することができる。
【0044】
原点姿勢における3つのリンク機構4のうち第1のリンク機構、第2のリンク機構の点Aをそれぞれ点A,Aとする。また、ある姿勢での第1のリンク機構、第2のリンク機構の点Aをそれぞれ点A’,A’とする。
【0045】
また、パラレルリンク機構1では、3つのリンク機構4が等間隔に配置されている。このため、3つのリンク機構4について基端側のリンクハブ2と基端側の端部リンク部材5との回転対偶部の中心軸同士の成す角は2π/3である。
【0046】
Y軸を中心として第1のリンク機構において基端側の端部リンク部材5が回転するとき、点A’は、円弧PAおよびその延長上を動く。Y軸から2π/3回転した軸を中心として第2のリンク機構において基端側の端部リンク部材5が回転するとき、点A’は、円弧PAおよびその延長上を動く。
【0047】
順運動学は基端側の端部リンク部材5の角度βから先端側のリンクハブ3の姿勢を導出するものであるため、ある姿勢における基端側の端部リンク部材5の角度と、原点姿勢における端部リンク部材5の角度との差は、既知であるとする。第1のリンク機構、第2のリンク機構について、この差をそれぞれ角βA1,βA2とする。
【0048】
図10に単位球と球面三角形のモデルが示される。図10では、基端側のリンクハブ2の中心軸が単位球と交わる点を点Pとし、点Pと点A’とA’を結んでできる単位球の表面の球面三角形Tを太線で示している。一般的に、球面上の三点A,B,Cが成す球面三角形ABCは、辺BC,CA,ABをそれぞれa,b,cとし、弧ABと弧ACが成す角度をA、弧ABと弧BCが成す角度をB、弧CBと弧ACが成す角度をCとした場合、球面三角法より余弦定理が次の式(1)、正弦定理が式(2)で表される。
【0049】
【数1】
【0050】
【数2】
【0051】
図10の三角形Tにおいて、反時計回りを正としたとき、各辺の大きさは以下の式(3)、式(4)で示される。円弧A’A’の長さをLとすると、式(1)は式(5)のように変形できる。
【0052】
【数3】
【0053】
【数4】
【0054】
【数5】
【0055】
また、式(1)および、以下の式(6)から、角a,aについてそれぞれ式(7)、式(8)のように表せる。
【0056】
【数6】
【0057】
【数7】
【0058】
【数8】
【0059】
図11では先端側のリンクハブの中心P''とA’とA’を結んでできる球面三角形Tを太線で示している。式(1)より、角a’は以下の式(9)のようになる。
【0060】
【数9】
【0061】
また、図8において、OAの延長線と球GPとの交点をA’とした場合、弧A’PBの内角はd/2となりこの内角は不変である。任意の折れ角と旋回角に対して、この関係は保たれ、単位球上の弧であるため、球面三角形Tの辺P''A’の長さはd/2であり、P''A’も同様にd/2である。したがって、三角形Tは球面二等辺三角形になる。そのため、∠P''A’A’、∠P''A’A’をそれぞれa’、a’とした場合、a’=a’が成り立つ。
【0062】
OA’、OA’を回転軸とした時、各基端側アームが回転した時の倒れ角ω,ω図9)は次の式(10)、式(11)で表される。なお、各ωは基端側アームが回転したときの、各中央リンクの初期姿勢に対する倒れ角を示す。回転するのは基端側アームである端部リンク部材5であり、それにより中央リンクの回転軸がOA’とOA’になり、倒れ角ωで中央リンクが回転する。
【0063】
【数10】
【0064】
【数11】
【0065】
図12のモデルを用いて、順運動学の式の導出について説明する。図12において示したいことは、以下の(1)、(2)に示す2ステップの回転で順運動学の計算が実現できることである。
(1)基端側リンクがβA1回転し、その回転で点Pが点P’に移動する。
(2)回転後の基端側リンクと中央リンクとの対偶部A’がω回転し、点P’が点P''に移動する。
【0066】
上記(1)の基端側リンクの回転は回転軸がYなので、Y軸まわりの回転でも表せる。このとき回転軸を(0,1,0)としたロドリゲスの式は、Y軸まわりの回転と同じ式になる。
【0067】
なお、図12におけるA’の座標(βA1+d/2、φA1)において、φA1は、点Aに対応する旋回角を示し図12でいうと底面円のX座標軸から点直線までの角度を示している(図12では180°の位置)。
【0068】
順運動学の式は、第1のリンク機構に注目し、先端側のリンクハブの中心点Pを回転させることによって導出する。図12では基端側の端部リンク部材5はY軸を中心に回転する。そのため、中心点PをまずY軸中心にβA1回転させ、さらにOA’を中心軸としてω回転させる。すると、基端側のリンクハブの中心Oから先端側のリンクハブの中心P''へのベクトルは、動作後の先端座標位置と一致する。任意の軸に対する回転は、ロドリゲスの回転公式より表現できる。回転軸の単位ベクトルをn、回転前のベクトルをr、回転後のベクトルをr’、回転量を角度θとすると、r’は以下の式(12)、あるいは式(12)を3×3行列を使った変換式に置き換えた式(13)が成立する。
【0069】
【数12】
【0070】
【数13】
【0071】
ここでR(θ)とは、n軸を中心として角θだけrの座標を回転させる場合の式(12)ロドリゲスの回転公式を3×3の回転行列で表したときの式を示している。なお、式(12)と式(13)の具体的な表現はかなり異なるが、式(12)はベクトル方程式の形式で書かれており、式(13)は同じ結果が得られるrの線形変換として表現した等価な式である。
【0072】
先端側のリンクハブ中心点の位置ベクトルの初期位置PをP=(0,0,l)とした時、Y軸中心に初期位置Pを角度βA1回転させた後の位置ベクトルP’は以下の式(14)で表される。
【0073】
【数14】
【0074】
次に、OAを回転軸n1として位置ベクトルP’を角度ω回転させた後の位置ベクトルP’’は以下の式(15)で表される。
【0075】
【数15】
【0076】
P’はβA1の関数、P’’はωの関数(=βA1とβA2の関数)であるため、式(15)を用いることによって、3つの基端側の端部リンク部材5の角度βのうちの2つから、先端側のリンクハブ中心点P’’の位置および姿勢を算出することができる。
【0077】
<逆運動学>
図13は、逆運動学の式の導出の説明をするための第1の図である。図14は、逆運動学の式の導出の説明をするための第2の図である。図13および図14を用いて逆運動学の式の導出について説明する。逆運動学の計算では、点P,A,Aからなる球面三角形Tと、点P,A,A’からなる球面三角形Tを考える。ここで、Aは基端側のリンクハブ3と基端側の端部リンク部材6との回転対偶部の中心軸がOを原点とし原点姿勢におけるPを頂点とする単位球面と交わる点を示す。A’は、点AからZ軸を中心に−α回転した位置にある点を示す。
【0078】
なお、点Aは必ずしも二等分面上の点ではなく、先端側中心がPにあるときの基端側リンクと中央リンクとの対偶部の位置になる。先端側中心が初期状態、すなわちZ軸上にあるときは、点Aの位置は二等分面上になる。
【0079】
図13に示す角e図14に示すeを求めることによって逆運動学の式が導出できる。まずTに対して球面余弦定理を用いると以下の式(16)から式(17)を得る。
【0080】
【数16】
【0081】
【数17】
【0082】
同様にTに対して球面余弦定理を用いると以下の式(18)から式(19)を得る。
【0083】
【数18】
【0084】
【数19】
【0085】
∠AOAおよび∠AOA'はアーム角α、∠AOPはd/2であるため、角eおよび角eは以下の式(20)、式(21)で表される。
【0086】
【数20】
【0087】
【数21】
【0088】
αは基端側の端部リンク部材が成す角度であり、部材の構造により決定する値である。従ってeおよびeは先端側のリンクハブ中心点P(θ,φ)の関数となる。よって、原点姿勢の時のβA1を0とした場合、βA1は以下の式(22)で表すことができる。
【0089】
【数22】
【0090】
式(22)が本実施の形態のパラレルリンク機構1の逆運動学の式となる。
本実施の形態のリンク作動装置によれば、一般的な球面三角法を用いてリンク作動装置の順変換および逆変換の式を導出しているので、繰り返し演算を必要とせず、先端の現在位置の把握やアーム回転角βの算出を素早く行なうことが可能になる。さらに、近似誤差を含まないアーム回転角βが算出されるため、全ての姿勢制御用駆動源を同方向に回転させ一定のトルクを加えることで機構的なガタによるブレを抑える一定トルク負荷制御の精度が上がり、位置決め精度を向上させることができる。
【0091】
また、本実施の形態で示した計算手法は、任意のアーム角αに対して順運動学および逆運動学の計算として適応可能であり、リンク作動装置の多様な設計仕様に対応できる。ここで、これらの式においては角度を用いているが、単位球においてある2直線が成す中心角とその弧長は等しいため、角度の代わりに弧長を用いた式を使用してもよい。正弦あるいは余弦関数の項は、単位球面で考えているので、例えばベクトルPやAの内積、あるいは外積のノルムを用いることによって、計算処理の負担を減らすこともできる。
【0092】
<順運動学、逆運動学を用いた制御>
以上のように導出した順運動学の計算式、逆運動学の計算式を用いて、制御装置100は、以下に示す制御を実行する。
【0093】
図15は、順運動学によって基端側の端部リンク部材の角度から先端側のリンクハブの中心点位置を得る処理を説明するためのフローチャートである。このフローチャートの処理は、例えば、ダイレクトティーチング実行時、または異常復帰後等で現在位置が不明な状態からの始動時に制御装置100によって実行される。
【0094】
ステップS1において制御装置100は、図示しない角度センサから角βA1,βA2を取得する。そして、ステップS2において制御装置100は、順運動学について説明した式(3)〜(11)を用いて、角ω,ωを算出する。さらに、ステップS3において、制御装置100は、ロドリゲスの回転公式に基づく式(12)または式(13)を用いて先端側のリンクハブ中心点の位置P’,P’’を算出する。そして、制御装置100は、ステップS4において、先端側リンクハブの位置P’’を上位の制御装置または表示装置などに出力する。
【0095】
図16は、逆運動学によって先端側のリンクハブの中心点位置から基端側の端部リンク部材の角度を得る処理を説明するためのフローチャートである。このフローチャートの処理は、例えば、パラレルリンク機構1によってエンドエフェクタ61の姿勢を変えながら作業する場合に制御装置100によって実行される。
【0096】
ステップS11において制御装置100は、入力装置または図示しない上位の制御装置から先端側のリンクハブの位置情報(目標位置)を受信する。そして、ステップS12において制御装置100は、逆運動学について説明した式(16)〜(22)を用いて、角βA1を算出し、同様な手順で角βA2も算出する。さらに、ステップS3において、制御装置100は、端部リンク部材5の回転角が角βA1および角βA2となるように、対応するアクチュエータ51を制御する。このような処理を繰り返すことによって、先端側リンクハブの位置および姿勢が所望の状態に制御される。
【0097】
最後に、再び図を参照して本実施の形態について総括する。
本実施の形態のリンク作動装置50は、基端側の第1リンクハブ2と、先端側の第2リンクハブ3と、第1リンクハブ2と第2リンクハブ3とを連結する少なくとも3つのリンク機構4とを備える。少なくとも3つのリンク機構4の各々は、第1リンクハブ2に対して回転可能に連結された第1端部リンク部材5と、第2リンクハブ3に対して回転可能に連結された第2端部リンク部材6と、第1端部リンク部材5および第2端部リンク部材6の各々に対して回転可能に連結された中央リンク部材7とを含む。少なくとも3つのリンク機構4において、第1リンクハブ2と第1端部リンク部材5との回転対偶部の少なくとも3つの中心軸O1および、中央リンク部材7の一方端の回転対偶部の中心軸O2は、第1リンクハブ中心点PAで交わり、第2リンクハブ3と第2端部リンク部材6との回転対偶部の少なくとも3つの中心軸および中央リンク部材7の他方端の回転対偶部の中心軸は、第2リンクハブ中心点PBで交わる。リンク作動装置50は、少なくとも3つのリンク機構4のうち、2つのリンク機構における、第1リンクハブ2に対する第1端部リンク部材5の角度βA1,βA2から、第2リンクハブ3の位置および姿勢を球面三角法を用いて演算するように構成される制御装置100をさらに備える。
【0098】
このような構成とすることによって、基端側と先端側のリンクハブ中心点PA,PBを中心点とするそれぞれの球GA,GBが交わる二等分面PL1を対称に先端側のリンクハブ3が動作する球面駆動機構が構成される。この構成により、球面三角法を用いた順変換・逆変換の式の導出ができるため、様々な設計緒元をもつリンク作動装置において、汎用的な順運動学および逆運動学の関数化を図ることができる。
【0099】
リンク作動装置50は、各基端側の端部リンク部材5の角度βA1,βA2から前記先端側のリンクハブ中心点PBの位置を求めるために球面三角法を用いて演算する機能を制御装置100に備えると良い。この構成により、ダイレクトティーチングや異常復帰後等で現在位置が不明な状態からの長距離移動動作をリアルタイムで行なう機能を実現できる。
【0100】
好ましくは、制御装置100は、第1リンクハブ2に対する第1端部リンク部材5の姿勢を示す角度βA1,βA2から、第2リンクハブ3の姿勢を求めるために以下の関係式を用いて演算するように構成される。
P’=R(βA1)P
P''=Rn1(ω)P’
ただし、各符号の意味は下記の通りである。
P:原点姿勢における第2リンクハブ中心点の座標
P’’: 原点姿勢から移動後の第2リンクハブ中心点の座標
βA1:少なくとも3つのリンク機構のうちの第1のリンク機構における第1リンクハブに対する第1端部リンク部材の角度
βA2:少なくとも3つのリンク機構のうちの第2のリンク機構における第1リンクハブに対する前記第1端部リンク部材の角度
Rn(θ):n軸を中心として角θだけPの座標を回転させる場合のロドリゲスの回転公式の3×3の回転行列
n1:第1のリンク機構において、第1リンクハブ中心点をOとし、中央リンク部材の一方端の回転対偶部の中心軸と中央リンク部材の他方端の回転対偶部の中心軸との交点をAとするときの直線OA
ω1:βA1およびβA2によって決まる第1のリンク機構の中央リンク部材の倒れ角
このように、各基端側の端部リンク部材5の回転角βA1,βA2から先端側のリンクハブ中心点PBの位置を求める順運動学の関係式を用いて演算する機能を制御装置100に備えるとよい。この構成により、順変換を用いてリンク作動装置50を素早く制御することが可能になる。その際、近似誤差を含まないアーム回転角が算出されるため、機構的なガタによるブレを抑える一定トルク負荷制御の精度が上がり、位置決め精度を向上させることができる。
【0101】
好ましくは、制御装置100は、第2リンクハブ3の中心点PBの位置から第1端部リンク部材5の角度βA1,βA2を球面三角法を用いて演算するように構成される。
【0102】
より好ましくは、制御装置100は、第2リンクハブ3の中心点PBの位置から第1端部リンク部材5の姿勢を示す角度βA1,βA2を、以下の関係式を用いて演算するように構成される。
βA1=e−e
なお、e,eは、既出の式(22)、(23)で示される。
【0103】
このように、リンク作動装置50は、先端側のリンクハブ3の姿勢を姿勢制御用のアクチュエータ51の回転を介して制御する制御装置100を有している。先端側のリンクハブ中心点PBの位置から各基端側の端部リンク部材5の回転角βA1,βA2を求める逆運動学の関係式を用いて演算する機能を制御装置100に備えると良い。この構成により、先端の姿勢が与えられた場合の各基端側の端部リンク部材5の回転角βA1,βA2を算出することができる。
【0104】
今回開示された実施の形態は、すべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した実施の形態の説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
【符号の説明】
【0105】
1 パラレルリンク機構、2,3 リンクハブ、4 リンク機構、5,6 端部リンク部材、7 中央リンク部材、10 土台、11,21 回転軸連結部材、12,15,22,25 回転軸体、O1,n1 回転軸、12a 大径部、12b 小径部、20 先端部材、30 湾曲部材、31,31A 回転軸支持部材、50 リンク作動装置、51 アクチュエータ、52 減速機構、52a 出力軸体、53 モータ固定部材、54 フランジ面、55 スペーサ、56 ボルト、57 内径溝、58 フランジ取付面、61 エンドエフェクタ、100 制御装置。
図1
図2
図3
図4
図5
図6
図7
図8
図9
図10
図11
図12
図13
図14
図15
図16