3.10.36 薄壁穴にアームを通す2関節角を求める逆運動学の別解       2010-04-29

  (ファイル名:NewThinHoleInverse)                          2012-06-10確認

 

・「詳説ロボットの運動学」8.2.5項ではφ,φMを回転し,JMからオフセットなしに伸びるアームMが薄壁の穴を通る問題を扱ったが,ここではもっと一般的にJL,Jmを回転させてアームMを穴に通す問題を扱う.

・問題は図1に見るように,「関節変位φLとφmによってアームMを穴中心に通すにはいくらにすればよいか」である.但し他の関節変位は既知で不変とする.

[註]以下の表記で関節JMから出るアーム番号をMとしているが,シミュレータではこのアームはM+1である.

 

[計算の手順]

 

図2のように,Jm回りに回転するアームMJm軸回りの回転双曲面を作る.その双曲面内に穴中心P点がある.

・回転双曲面はφm=0のときのアームMが作る曲面である.

・Σsmから見たJMの回転半径RMとそのZ方向の距離zMはφm+1,...,φMが既知で不変ならば既知である.

・双曲面の最も細い部分(図2Q点)の半径aはφm+1,...,φMが既知で不変ならば既知である.

またQ点はΣsm表示でであり,これも既知である.

・この2つの条件からΣsm表示の回転双曲面の方程式が決まる(式のabza).

・回転双曲面上にP点があるようにQ点を決めるのはφ1,...φL-1を既知で不変としてφLでありφmに関係しない.

・このことから,φLを求めることができる.

・φLが決まれば,Jmezm(Σ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式からs1s2が求まる.s1Q点までの距離zaである.

    (φLに依存しない) (2-13)

 (オフセットなしに直列にアームが連なるロボットではである.)

・回転双曲面の最も細い部分の軸上の点QのΣsm表示:

                                     (φLに依存しない)               (2-14)

・双曲面の式のΣsm表示:

                                             (2-15)

 において,JMを含むことから

                                                     (2-16)

 が成り立ち,

   ,                                 (2-17)

 を得る.

-------------------------------------------------------------------------------------------------------------------------------------------------------------

b,θHの別解]

・上式はJMQ点と一致した場合には適用できない.(だから)この場合は以下のようにbを求める.

・双曲線の傾きθH図4に見るように,Q点からzだけ上がったときアームMに沿ってRがいくらになるかが決まり,zRの比の極限値から決まる.

P点の位置:    (2-18)

Pの位置の半径R    (2-19)

・漸近線の傾きはs→∞の極限値である.

            (2-20)

  ∴                                                               (2-21)

-------------------------------------------------------------------------------------------------------------------------------------------------------------

 

 この双曲面の式はm+1以上の関節角φが不変である限り不変で,φL,φmに依存しない.

・双曲面上に穴中心位置P点があることから

   ,または       (2-22)

 式中,pxpypzがφLに依存する.

 pxpypz cosφLsinφ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の場合,JMJm軸上にある場合,アームM(またはその延長線)Jm軸と交わる場合が相当する.

・これは回転双曲面のa0の極限でアームMはφmの回転によって円錐面をなす.

・φLを求める式は,(3-15)(3-20)式でa=0と置けばよい.

 

(3−3)関節φm回転によりアームMが平面をなす場合

図6の示すように,アームMJm軸に直角の平面を作る.

・φLを求める式は,回転双曲面のθH90°の極限として (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を求める式は,回転双曲面のθH0の極限として (3-15)(3-20)式でθH=0a=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)

 sJMから穴中心までの距離を表し,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が薄壁の穴を通り抜ける解を解析的に求めることができた.