経路生成における関節JMの旋回円上の特徴点 2012-6-11確認
・壁の外側で「関節を順に並べる」状態へ持っていくために,各関節の目標を決めそれに向かって一気運動または1ステップ運動を行う.
・JMの前がRP関節のときは目標を自由に設定することができるが,PP(QP)関節のときは次の関節状態を考慮して,目標を定め,いつも一気運動できるわけではない.
・JMの前の関節をJMFとすると,JMF回りのJMの旋回円上に目標点がある.
・ここではJMの旋回円上の目標の候補となる特徴点とその旋回角φMFを計算する.
1.記号の定義
(1) 壁の位置姿勢
rh:垂直壁の代表点位置
:壁面法線ベクトル
:壁面内の水平・垂直の2方向ベクトル
(2) 関節番号とアーム長,関節の位置ベクトル
JM,,JM:対象の旋回関節
JMF,,JMF:JMの旋回中心となる関節(通常はJMの1つ前で,JMF=’R’の場合は2つ前)
JMH,,JMH:JMの次の関節(通常はJMの1つ後で,JMH=’R’の場合は2つ後)
JME,,JME:JMFの1つ前の関節
各位置ベクトルの成分を(A=M,
MF, ME, MH)とする.
LMF:JMF −JM間アーム長
LMH:JM −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に最も近い点と回転角(壁からJME,JMFの順に並んでいれば(順並び)壁への最近点,逆ならば最遠点)
F,φF::方向にJMEから最も遠い点と回転角(壁からJME,JMFの順に並んでいれば(順並び)壁への最遠点,逆ならば最近点)
[註]壁から遠い近いでなく,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と高さが等しくなるのはrのJMFから見た高さが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)各特徴点の存在と回転途中で壁床と干渉する可能性
・壁の外,床の上に特徴点があれば「存在する」と呼び,壁の内側,または床の下側にあるとき「存在しない」と呼ぶ.
・壁の外,床の上に存在しても旋回途中で壁・床とぶつかる場合もあり,このときは現状態からその特徴点に到達できない.
・特徴点H,N,F,S,Y,Tを代表してAとする.
(7−1)存在条件
・壁干渉点Wがなく,また床干渉点がなければ,特徴点は全て存在する.
・壁干渉点Wがある場合は,現在の関節角φMFと特徴点Aまでの回転角φAの間にφWがあれば特徴点へ到達できない.すなわちAまで旋回できる条件は
(35)
である.φWは±の2つについて成り立たなければならない.
・床干渉点Bがある場合は,現在の関節角φMFと特徴点Aまでの回転角φAの間にφBがあれば特徴点へ到達できない.すなわちAまで旋回できる条件は
(36)
である.φBは±の2つについて成り立たなければならない.