経路生成における関節JMの旋回円上の特徴点              2012-6-11確認

 

・壁の外側で「関節を順に並べる」状態へ持っていくために,各関節の目標を決めそれに向かって一気運動または1ステップ運動を行う.

JMの前がRP関節のときは目標を自由に設定することができるが,PP(QP)関節のときは次の関節状態を考慮して,目標を定め,いつも一気運動できるわけではない.

JMの前の関節をJMFとすると,JMF回りのJMの旋回円上に目標点がある.

・ここではJMの旋回円上の目標の候補となる特徴点とその旋回角φMFを計算する.

 

1.記号の定義

 (1) 壁の位置姿勢

 rh:垂直壁の代表点位置

 :壁面法線ベクトル

:壁面内の水平・垂直の2方向ベクトル

(2) 関節番号とアーム長,関節の位置ベクトル

JM,JM:対象の旋回関節

JMF,JMFJMの旋回中心となる関節(通常はJMの1つ前で,JMF=’R’の場合は2つ前)

JMH,JMHJMの次の関節(通常はJMの1つ後で,JMH=’R’の場合は2つ後)

JME,JMEJMFの1つ前の関節

各位置ベクトルの成分を(A=M, MF, ME, MH)とする.

LMFJMF JM間アーム長

LMHJM JMH間アーム長

JMFの高さ

(3) 旋回円上の特徴点とJMF回りの回転角φ

P関節の回転角φは限界角±φmaxの範囲内で動くとする.φ=0はアームを伸ばした状態,±πはアームが折り畳まれて前のアームと衝突した状態である.

H,φH:最高点と回転角

S,φS:最低点と回転角

Y,φY::高さがLMHとなる点と回転角.2通りあり,φ=0に近い点をY-,遠い点をY+とする.

T,φT::高さがJMFと等しくなる点と回転角.2通りあり,φ=0に近い点をT-,遠い点をT+とする.

N,φN:方向にJMEに最も近い点と回転角(壁からJMEJMFの順に並んでいれば(順並び)壁への最近点,逆ならば最遠点)

F,φF:方向にJMEから最も遠い点と回転角(壁からJMEJMFの順に並んでいれば(順並び)壁への最遠点,逆ならば最近点)

[註]壁から遠い近いでなく,JMEから見て遠い近いである.

φMF:現JM位置の旋回角度

 (4) 旋回円の座標系:静座標変換行列ΣMFsの3ベクトル成分(ΣMFdのφ=0のときの行列)

JMF軸の方向ベクトル

:φ=0のときのJMF JM間アームリンクの方向ベクトル

JMFを原点とする座標系の第2軸

2.各特徴点とその旋回角φ

(1)旋回円上で最遠点Fと最近点N

JMEから最も遠い点をFとする.最遠点

   ,      (1)

を最大とするφは

                                                                                  (2)

F点の壁面法線方向の絶対距離は

                                         (3)

・壁からの距離は

                                    (4)

JMEから最も近い点をNとする.その最近点は最遠点Fの反対側であり,その位置・壁からの距離は

                                                                                (5)

                                                        (6)

                                   (7)

[註]ロボットはアームが直列に連なっている構造なので,関節角は±180゜までは曲げられない.

最遠点は1通りだが,最近点は関節角の限界を超える場合がありそのときは2通りの「最近点」がある.ただし最近点は目標点にはならない.

 

(2)旋回円上の最高点Hと最低点F

図4のように座標系を取ると,からφ回転した旋回円上の点の位置ベクトルは

                                                                  (8)

・最高点Z座標は

   ,            (9)

を最大とするφは

                                                                                 (10)

・最高点の絶対高さは

                                                        (11)

・最低点Sは最高点Hの反対側であり,その位置と高さは

                                                                                  (12)

                                                                (13)

                                                       (14)

[註]最高最低点は1つであるが,上記計算のφHが限界角φmaxを超えれば,φNが限界角φmaxを超えればの2通りの「最高最低点」が生じる.最高点を目標として使うが,低い方の「最高点」を使うこともあり得る.

 

(3)旋回円上で高さが次のアームの長さLMHと高さが等しくなる点Y

JMF中心の旋回円上の点の位置は

                                                         (15)

・高さがLMHとなるのはとなるφである.(1)と同様に

                                                (16)

  ,                                       (17)

の中味が絶対値1以上のときは,JMFが高過ぎる(JMの旋回で床に衝突する恐れがない)か,JMFが床下にある場合である.

・ηが存在するとして,旋回角は

                                                                               (18)

・等しい高さの点の位置ベクトルは

                                                             (19)

・解は2通りあり,絶対値の小さい方をY-,大きい方をY+とする.

 

(4)旋回円上でJMFと高さが等しくなる点T

 

JMF中心の旋回円上の点の位置は

                                                        (20)

JMFと高さが等しくなるのはrJMFから見た高さが0となるφである.(1)と同様に

                                                             (21)

  ,                                              (22)

・等しい高さの点のJk-1から見た位置ベクトルは

                                                           (23)

・解は2通りあり,絶対値の小さい方をT-,大きい方をT+とする.

・両者はJMFを挟んで反対側にある.

 

(5)旋回円上で壁と干渉する点(余裕値が負となる点)W

 

JMFから見た旋回円上の点の絶対座標は

                                                       (24)

・壁からの余裕値bwは,限界余裕値をbmとすると

                                                                   (25)

bw=0となるθを求める.

                                 (26)

                         (27)

・書き直して

                                             (28)

・|C>1のときは壁干渉は生じない.

・|C<1としてbw=0となるφは

   ,                                                      (29)

・解は2通りあり,絶対値の小さい方をW-,大きい方をW+とする.

(6)旋回円上で床と干渉する点(余裕値が負となる点)B

 

・旋回円上の点の絶対座標は

                                                       (30)

・床からの余裕値bBは,限界余裕値をbmとすると

                                                                           (31)

bB=0となるφを求める.

           (32)

・書き直して

                                                         (33)

・|B>1のときは床干渉は生じない.

・|B<1としてbB=0となるφは

   ,                                                       (34)

・解は2通りあり,絶対値の小さい方をB-,大きい方をB+とする.

 

(7)各特徴点の存在と回転途中で壁床と干渉する可能性

・壁の外,床の上に特徴点があれば「存在する」と呼び,壁の内側,または床の下側にあるとき「存在しない」と呼ぶ.

・壁の外,床の上に存在しても旋回途中で壁・床とぶつかる場合もあり,このときは現状態からその特徴点に到達できない.

・特徴点HNFSYTを代表してAとする.

(7−1)存在条件

壁干渉点Wがなく,また床干渉点がなければ,特徴点は全て存在する.

・壁干渉点Wがある場合は,現在の関節角φMFと特徴点Aまでの回転角φAの間にφWがあれば特徴点へ到達できない.すなわちAまで旋回できる条件は

                                                                (35)

 である.φWは±の2つについて成り立たなければならない.

・床干渉点Bがある場合は,現在の関節角φMFと特徴点Aまでの回転角φAの間にφBがあれば特徴点へ到達できない.すなわちAまで旋回できる条件は

                                                                  (36)

 である.φBは±の2つについて成り立たなければならない.