3.10.36 薄壁穴にアームを通す2関節角を求める逆運動学の別解 2010-04-29
(ファイル名:NewThinHoleInverse) 2012-06-10確認
・「詳説ロボットの運動学」8.2.5項ではφL,φMを回転し,JMからオフセットなしに伸びるアームMが薄壁の穴を通る問題を扱ったが,ここではもっと一般的にJL,,Jmを回転させてアームMを穴に通す問題を扱う.
・問題は図1に見るように,「関節変位φLとφmによってアームMを穴中心に通すにはいくらにすればよいか」である.但し他の関節変位は既知で不変とする.
[註]以下の表記で関節JMから出るアーム番号をMとしているが,シミュレータではこのアームはM+1である.
[計算の手順]
・図2のように,Jm回りに回転するアームMはJm軸回りの回転双曲面を作る.その双曲面内に穴中心P点がある.
・回転双曲面はφm=0のときのアームMが作る曲面である.
・Σsmから見たJMの回転半径RMとそのZ方向の距離zMはφm+1,...,φMが既知で不変ならば既知である.
・双曲面の最も細い部分(図2のQ点)の半径aはφm+1,...,φMが既知で不変ならば既知である.
またQ点はΣsm表示でであり,これも既知である.
・この2つの条件からΣsm表示の回転双曲面の方程式が決まる(式のa,b,za).
・回転双曲面上にP点があるようにQ点を決めるのはφ1,...φL-1を既知で不変としてφLでありφmに関係しない.
・このことから,φLを求めることができる.
・φLが決まれば,Jm,ezm(ΣAから見たΣsm系の位置姿勢)が決まり,ezm回りの回転双曲面が決まり,その上にPがある条件からφmが決まる.
・アームMが回転双曲面を作らない場合もある.その特殊なケースは後で述べる.
[問題の定式化]
(1)全ての位置・方向をΣsm,またはΣmで表す.
・Jmの位置のΣsm,Σm表示:
(1-1)
・JMの位置のΣm表示:
(1-2)
(既知)
(1-3)
・Jm軸直線の方向ベクトルのΣsm,Σm表示:
(1-4)
・アームM軸直線の方向ベクトルのΣm表示:
(既知)
(1-5)
は初期姿勢(全てのφ=0)のΣM表示のアームMの方向ベクトルであり,既知である.
・穴中心PのΣsm表示:
(φLの関数)
(1-6)
以下は回転双曲面を作る場合について説明する.
(2)回転双曲面の式
・回転双曲面の式はΣsm表示で
,または
(2-1)
または,と置くと
(2-1)’
と表される.θHは双曲線の漸近線の傾きを表す.
・a は図2のように,JMが回転双曲面上にあること,及び回転体の最小半径はQを中心とするJm軸直線とアームM直線の最短距離であることから求まる.
・Σsm表示で図2を参照して
(2-2)
(2-3)
・Jm軸直線の方程式のΣsm表示:
但し ,
(2-4)
・Jm軸直線のΣm表示:
(φLに依存しない)
(2-5)
・アームM軸直線のΣm表示:
(φLに依存しない)
(2-6)
・この2直線の共通法線ベクトルn
(2-7)
・共通法線を含む直線のΣm表示:
(φLに依存しない)
(2-8)
・(2-5),(2-8)直線の交点Tがである.
・(2-5), (2-8)2直線の最短距離a:両直線の式とnとの内積を取って
(2-9)
∴ (φLに依存しない)
(2-10)
・(2-5), (2-8)2直線の式との内積を取ると
(2-11)
・(2-5), (2-8)2直線の式との内積を取ると
(2-12)
・この2式からs1,s2が求まる.s1がQ点までの距離zaである.
(φLに依存しない) (2-13)
(オフセットなしに直列にアームが連なるロボットではである.)
・回転双曲面の最も細い部分の軸上の点QのΣsm表示:
(φLに依存しない)
(2-14)
・双曲面の式のΣsm表示:
(2-15)
において,JMを含むことから
(2-16)
が成り立ち,
,
(2-17)
を得る.
-------------------------------------------------------------------------------------------------------------------------------------------------------------
[b,θHの別解]
・上式はJMがQ点と一致した場合には適用できない.(だから)この場合は以下のようにbを求める.
・双曲線の傾きθHは図4に見るように,Q点からzだけ上がったときアームMに沿ってRがいくらになるかが決まり,zとRの比の極限値から決まる.
・P点の位置: (2-18)
・Pの位置の半径R:
(2-19)
・漸近線の傾きはs→∞の極限値である.
(2-20)
∴
(2-21)
-------------------------------------------------------------------------------------------------------------------------------------------------------------
この双曲面の式はm+1以上の関節角φが不変である限り不変で,φL,φmに依存しない.
・双曲面上に穴中心位置P点があることから
,または
(2-22)
式中,px,py,pzがφLに依存する.
px,py,pz はcosφL,sinφLの一次関数なので,この式はφLの2次三角関数となり,解析的にφLを求めることができる.
(3)φLを求める式
(3−1)通常の回転双曲面の場合
・穴中心位置のΣA表示:
(3-1)
・穴中心位置のΣsm表示:φLの関数として表す.
(3-2)
但し
(3-3)
(3-4)
(3-5)
(3-6)
と置く.
・動座標変換行列:
,
,
(3-7)
・穴中心位置のΣL表示をφLの関数として表す.
(3-8)
・をφLの関数として表す.
(3-9)
(3-10)
(3-11)
但し
(3-12)
・穴中心Pが回転双曲面上にある条件式(21)に代入
(3-18)
より
(3-13)
・書き換えると
(3-14)
・但し各係数は3通りに表現して
(3-15)
(3-16)
(3-17)
(3-18)
(3-19)
(3-20)
――――――――――――――――――――――――――――――――――――――――――――
(3-15)’
(3-16)’
(3-17)7
(3-18)’
(3-19)’
(3-20)’
――――――――――――――――――――――――――――――――――――――――――――
(3-15)”
(3-16)”
(3-17)”
(3-18)”
(3-19)”
(3-20)”
・この2次三角関数式からφLが解析的に求まる.解は最大4個ある.
・以下に回転双曲面を作らない3つの場合について述べる.
(3−2)関節φm回転によりアームMが円錐面をなす場合
・図5に示すように,m=Mの場合,JMがJm軸上にある場合,アームM(またはその延長線)がJm軸と交わる場合が相当する.
・これは回転双曲面のa→0の極限でアームMはφmの回転によって円錐面をなす.
・φLを求める式は,(3-15)−(3-20)式でa=0と置けばよい.
(3−3)関節φm回転によりアームMが平面をなす場合
・図6の示すように,アームMはJm軸に直角の平面を作る.
・φLを求める式は,回転双曲面のθH→90°の極限として (3-15)−(3-20)式でθH=90°(cotθH=0)と置けばよい.
・この式は元をただせば
(3-12)
に帰着する.これはφLの1次三角関数式であり,
,
(3-22)
の2通りの解が求まる.
(3−4)関節φm回転によりアームMが円筒面をなす場合
・θH=0であり,アームMはφmの回転によってJm軸回りの円筒を作る.
・φLを求める式は,回転双曲面のθH→0の極限として (3-15)−(3-20)式でθH=0,a=RMと置けばよい.
(4)φmを求める式
・JMの位置:
(4-1)
(φmに依存)
(4-2)
但し
(既知)
(4-3)
(4-4)
と置く.
・アームMの方向ベクトル:
(4-5)
(φmに依存)
(4-6)
但し
(既知)
(4-7)
(4-8)
と置く.
は初期姿勢(全てのφ=0)のΣM表示のアームMの方向ベクトルであり,既知である.
・アームMの直線の式のΣsm表示:
(4-9)
・(43),(47)式を代入:
,
(4-10)
であるから
(4-11)
・穴中心P位置のΣsm表示:
(既知)
(4-12)
(4-13)
・上記直線が穴中心位置Pを通ることから
(4-14)
・mMz=0でなければ,第3成分式より
(4-15)
sはJMから穴中心までの距離を表し,s>0の解のみを採用する.
・第1,または第2,または第1,2式より
(4-16)
(4-17)
[別解]
・mMz=0の場合はφmの回転によりアームMは平面を作る.このときsの解は(4-15)式を使えないので別の計算をする.
・第(4-14)式1,2,3成分の2乗和
(4-21)
∴
(4-22)
・φmは第1,2式よりsを代入して
(4-23)
(4-24)
・解は2通りあるが,s>0の解(アームMの実体が穴を通る解)のみを採用する.
・この場合でなければ解は存在しない.
φLの解は4通りあるから,(φL,φm)の解は最大8通りある.
以上,任意φL,φm関節回転によりアームMが薄壁の穴を通り抜ける解を解析的に求めることができた.