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

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

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

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

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

１．記号の定義

　(1) 壁の位置姿勢

　rh：垂直壁の代表点位置

　：壁面法線ベクトル

：壁面内の水平・垂直の２方向ベクトル

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

JM,，JM：対象の旋回関節

JMF,，JMF：JMの旋回中心となる関節（通常はJMの１つ前で，JMF=’R’の場合は２つ前）

JMH,，JMH：JMの次の関節（通常はJMの１つ後で，JMH=’R’の場合は２つ後）

JME,，JME：JMFの１つ前の関節

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

LMF：JMF −JM間アーム長

LMH：JM −JMH間アーム長

：JMFの高さ

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

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

H，φH:：最高点と回転角

S，φS:：最低点と回転角

Y，φY:：高さがLMHとなる点と回転角．２通りあり，φ=0に近い点をY-，遠い点をY+とする．

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

N，φN:：方向にJMEに最も近い点と回転角（壁からJME，JMFの順に並んでいれば（順並び）壁への最近点，逆ならば最遠点）

F，φF:：方向にJMEから最も遠い点と回転角（壁からJME，JMFの順に並んでいれば（順並び）壁への最遠点，逆ならば最近点）

［註］壁から遠い近いでなく，JMEから見て遠い近いである．

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

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

：JMF軸の方向ベクトル

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

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

２．各特徴点とその旋回角φ

（１）旋回円上で最遠点Fと最近点N

・JMEから最も遠い点をFとする．最遠点は

　　　，　　    (1)

・を最大とするφは

　　　                                                                               (2)

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

　　　                                      (3)

・壁からの距離は

　　　                                 (4)

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

　　　                                                                             (5)

　　　                                                     (6)

　　　                                (7)

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

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

（２）旋回円上の最高点Hと最低点F

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

　　　                                                               (8)

・最高点のZ座標は

　　　，　　          (9)

・を最大とするφは

　　　                                                                              (10)

・最高点の絶対高さは

　　　                                                     (11)

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

　　　                                                                               (12)

　　　                                                             (13)

　　　                                                    (14)

［註］最高最低点は１つであるが，上記計算のφHが限界角φmaxを超えれば，φNが限界角φmaxを超えればの２通りの「最高最低点」が生じる．最高点を目標として使うが，低い方の「最高点」を使うこともあり得る．

（３）旋回円上で高さが次のアームの長さLMHと高さが等しくなる点Y

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

　　　                                                      (15)

・高さがLMHとなるのはとなるφである．（１）と同様に

　　　                                             (16)

　　，　　                                     (17)

・の中味が絶対値１以上のときは，JMFが高過ぎる（JMの旋回で床に衝突する恐れがない）か，JMFが床下にある場合である．

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

　　　                                                                            (18)

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

　　　                                                          (19)

・解は２通りあり，絶対値の小さい方をY-，大きい方をY+とする．

（４）旋回円上でJMFと高さが等しくなる点T

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

　　　                                                     (20)

・JMFと高さが等しくなるのはrのJMFから見た高さが０となるφである．（１）と同様に

　　　                                                          (21)

　　，　                                             (22)

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

　　　                                                        (23)

・解は２通りあり，絶対値の小さい方をT-，大きい方をT+とする．

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

（５）旋回円上で壁と干渉する点（余裕値が負となる点）W

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

　　　                                                    (24)

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

　　　                                                                (25)

・bw=0となるθを求める．

　　　                              (26)

　　　                      (27)

・書き直して

　　　                                          (28)

・｜C｜>1のときは壁干渉は生じない．

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

　　　，　　                                                    (29)

・解は２通りあり，絶対値の小さい方をW-，大きい方をW+とする．

（６）旋回円上で床と干渉する点（余裕値が負となる点）B

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

　　　                                                    (30)

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

　　　                                                                        (31)

・bB=0となるφを求める．

　　　        (32)

・書き直して

　　　                                                      (33)

・｜B｜>1のときは床干渉は生じない．

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

　　　，　　                                                     (34)

・解は２通りあり，絶対値の小さい方をB-，大きい方をB+とする．

（７）各特徴点の存在と回転途中で壁床と干渉する可能性

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

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

・特徴点H，N，F，S，Y，Tを代表してAとする．

（７−１）存在条件

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

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

　　　                                                             (35)

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

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

　　　                                                               (36)

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