3.10.33 壁内外で単位ベクトルを壁床から遠ざける回転運動と逆運動学 2011-9-7
(9p以降は不要) 2012-5-25確認
・障害物の穴や細隙を通り抜けて目標場所に位置決めする経路生成問題で,経路を目標から逆に初期ポーズへ経路を逆生成する手法を取る.
・逆生成において,穴や細隙から徐々にアームを抜いていくとき,壁外側でアーム・関節やワークが壁・床とぶつからないように関節角を調整する必要がある.
・また引き抜いて全関節が壁内側に来た後に,初期ポーズまで経路逆生成するとき壁・床とぶつからないようにしなければならない.
・関節J[M]が壁と床から充分離れていれば関節角q[M-1]をどのように動かしてもぶつかることはないが,近いとぶつかる可能性があり,q[M-1]だけでは壁床両方から同時に離す方向へ向けることができない場合がある.
・このときJ[M]をq[M-1]とq[M-2]の2関節角で動かせば壁床両方から遠ざけることができる.
・ただし,J[M-1], J[M-2]=’QP’のときはJ[M]を遠ざけることができてもJ[M-1]も動いてそれが壁床にぶつかることがあり得る.
・ここではRP2関節によりアームを遠ざける運動を生成する方法のみを述べる.
・壁外側における干渉回避運動を主にして述べており,壁内側は補足的に述べる.
図1−1 初期方向aを目標方向bまで床から遠ざかるような回転運動生成
・回転させるアームの初期方向ベクトルをa,目標方向をbとする.
・壁から遠ざかるとは,壁外側ではbルが右向き,壁内側では左向きということである.
・ベクトルの基点OはJ[M-1]関節位置である.
・アーム運動において初期方向ベクトルをaが壁床から遠いときはこの遠ざける経路を考える必要はないが,ここでは一般的に壁床にぶつからない経路計算を行う.
・aがbより壁または床に近いか遠いかによって経路は異なる.その4つのケースをそれぞれ考える.
(T)初期方向ベクトルaが目標方向bより壁と床に近い場合
[旋回軸の決定]
・aからbまでn軸回りに旋回させるとする図(a).
・旋回の始点で壁床から遠ざかる方向への旋回となるように回転軸nを見つければよい.
・回転軸nはa-bと垂直な平面内Hにある.a-bの単位ベクトル(平面の法線ベクトル)をe3とする(b).
・a,bと垂直な単位ベクトルをe1とし,座標系を作る(b).
,
,
(1-1)
・回転軸nのe1からの角度をφとする(b).nは
(1-2)
・aからbまでの旋回をCを中心とする回転円で考える.回転円はnを法線としベクトルa-bを含む平面内にある.
回転円のある平面の方程式は,Oを原点として,平面上の点の位置をrとすると
(1-3)
・C点のOからの距離とOからのベクトルは
(
) (1-4)
(1-5)
・回転円の半径は
(1-6)
・aからbまでの旋回角度αは
(1-7)
・回転円面の座標系を次のように定める.
,
,
(1-8)
d1,d2,d3はφの関数である.
・aからbまでの旋回途中のCからのベクトルは
(1-9)
・以上の式よりφθを与えれば上記の全てが決まる.
・問題はθ=0のときpがaより壁床から遠ざかる方向へ向けることである.すなわち
(1-10)
と置く.
・壁外側で壁から遠ざかる条件は
→
(1-11)
・壁内側で壁から遠ざかる条件は
→
(1-11)’
ただしは壁の外向きの法線ベクトルである.
床から遠ざかる条件は
→
(1-12)
である.
・すなわち,ベクトルを壁と床から遠ざける方向に向ければよいということになる.
・なお図(b)から分かるように,φが小さいほど回転円の半径rが大きくなり,aからbに到る弧の長さが短くなる.
・(1-8)式のをφの関数として表す.
(1-13)
(1-14)
(1-15)
,
(1-16)
と置く.
・壁外側で壁から遠ざかる条件(1-11)式は
(1-17)
・壁内側で壁から遠ざかる条件(1-11)’式は
(1-17)’
ただし
,
(1-18)
・ということもあり得る.これは初期ベクトルaが壁の正負法線方向を向いていることを意味し,aが壁側に向いていればφはどんな値でもよく,壁から離れる方向に向いていればどんな値でも駄目ということである.
・床から遠ざかる条件(1-12)式は
(1-19)
ただし
,
(1-20)
・ということもあり得る.これは初期ベクトルaが真上または真下を向いていることを意味し,aが真下を向いていればφはどんな値でもよく,真上を向いていればどんな値でも駄目ということである.
・この(1-17)または(1-17)’,(1-19)2式を満足するφは
壁外側で
(1-21)
壁内側で
(1-21)’
・床から遠ざかる条件
(1-22)
・のとき壁から遠ざかる率が最大となり,
のとき床から遠ざかる率が最大となる.
・この2つの範囲内のφの中の最小値を採用すればaからbまでの弧の長さが最小となりそれが望ましいが,壁または床からできるだけ早く遠ざかりたいときは,(1-21),(1-22)式の範囲内にあればまたは
とすることもあり得る.
(U)初期方向ベクトルaが目標方向bより壁から遠く床に近い場合
・この場合はaを床から遠ざけbに向かって壁へ近づく方向へ回転する回転円を見つければよい.
(U−1)aを床から遠ざける旋回
・aからbまでの旋回途中のCからのベクトルは(1-19)式
(2--1)=(1-9)
であり,始まりの点で床から遠ざかる方向へ,終点で壁から遠ざかる方向となるφの範囲を求めればよい.
・始点で床から遠ざかる条件は
(2-2)=(1-10)
→
(2-3)=(1-12)
,
(2-4)=(1-16)
(2-5)=(1-19)
ただし
,
(2-6)= (1-20)
・(2-5)式を満足するφは
(2-7)=(1-22)
(U−2)bへ壁に向かって近づく旋回
・終点でbへ壁に向かって近づく問題は,bからaへの経路を考え,bの始まりで壁から遠ざかる条件を求める.このとき(U−1)と同じ旋回円とするため回転軸nを同じ方向にとし回転角θを逆向きに取る.座標系とn,cを(U−1)と同じとし,bを始点とする座標系
を求める.
,
,
(2-8)=(1-1)
(2-9)=(1-2)
(
) (2-10)=(1-4)
(2-11)= (1-5)
(2-12)= (1-6)
,
,
(2-13)
・bからaまでの旋回途中のCからのベクトルは
(2-14)
(2-15)
・nを軸とするbからの回転はθが負の方向である.
・壁外側でθ負の方向に回転して壁から遠ざかる条件は
→
(2-16)
・壁内側でθ負の方向に回転して壁から遠ざかる条件は
→
(2-16)’
・は(1-13),(1-15)式を参照して
(2-17)
(2-18)
,
(2-19)
と置く.
・壁外側で壁から遠ざかる条件(2-16)式は
(2-20)
・壁内側で壁から遠ざかる条件(2-16)’式は
(2-20)’
ただし
,
(2-21)
・壁外側で(2-20)式を満足するφは
(2-22)
・壁内側で(2-20)’式を満足するφは
(2-22)’
・(2-7),(2-22)または(2-22)’2式の範囲内のφであればよいが,その中の最小値を採用すればaからbまでの弧の長さが最小となる.
(V)初期方向ベクトルaが目標方向bより壁に近く床から遠い場合
・この場合は(U)のaとbを入れ替えて,aが壁から遠ざける方向へ,bを床から遠ざける方向へ回転する回転円を見つければよい.その条件は(U)でa→b,b→aとした式がそのまま使える.φの範囲は(2-7),(2-22)式である.
(W)初期方向ベクトルaが目標方向bより壁と床両方から遠い場合
・この場合は(T)のaとbを入れ替えて, bからaへの経路と考え,(T)の結果を利用すればよい.φの範囲は(1-21),(1-22)式である.これにより壁床に近いbから壁床から遠ざかる経路が得られる.
[RP関節によりアームの方向ベクトルをpとする関節角φa,φbを求める姿勢逆運動学]
・問題をリストによるワークの方向決めの逆運動学とするが,連続するRP関節のアームの方向決めにも適用できる.
・のときリスト前のアームとワーク方向は一致しており,リストの第2関節座標系を
とする.
[註]リスト第2関節の静姿勢変化行列は関節軸が
であり,上記との対応は
となっている.
・ワークは初めはの方向にある.
・図1を参照して,ベクトルpは座標系とEuler角(α,β)で表して
(1-1)
である.(α,β)は
(1-2)
(1-3)
・リスト第1,2関節の回転角は
(1-4)
(1-5)
[註]以下は不要だが,前記計算が壁内側にも適用できることを確認するために記述する.
[自由空間(壁内側)で単位ベクトルを壁床から遠ざける回転運動] ‘11−12−14
・目標ポーズから初期ポーズまで経路逆生成する問題で,目標ポーズから全ての関節・ワークが穴を通り抜けた後,自由空間で壁床にぶつからないように関節角を調整する.
・関節Jkが壁と床から充分離れていれば関節角qkをどのように動かしてもぶつかることはないが,近いとぶつかる可能性があり,qkだけでは壁床両方から同時に離す方向へ向けることができない場合がある.
・このときJkのとJk-1の2関節角で動かせば壁床両方から遠ざけることができる.
・2関節によりアームを遠ざける運動を生成する方法は壁外側で壁床との干渉回避する方法と基本的には同じである.
・壁から遠ざかる条件は,壁外側では壁面法線の正方向へ動くことであったが,自由空間(壁内側)ではその負方向へ動くことである.
・回転させるアームの初期方向ベクトルをa,目標方向をbとする.
(T)初期方向ベクトルaが目標方向bより壁と床に近い場合
[旋回軸の決定]
・aからbまでn軸回りに旋回させるとする図(a).
・旋回の始点で壁床から遠ざかる方向への旋回となるように回転軸nを見つければよい.
・回転軸nはa-bと垂直な平面内Hにある.a-bの単位ベクトル(平面の法線ベクトル)をe3とする(b).
・a,bと垂直な単位ベクトルをe1とし,座標系を作る(b).
,
,
(1-1)
・回転軸nのe1からの角度をφとする(b).nは
(1-2)
・aからbまでの旋回をCを中心とする回転円で考える.回転円はnを法線としベクトルa-bを含む平面内にある.
回転円のある平面の方程式は,Oを原点として,平面上の点の位置をrとすると
(1-3)
・C点のOからの距離とOからのベクトルは
(
) (1-4)
(1-5)
・回転円の半径は
(1-6)
・aからbまでの旋回角度αは
(1-7)
・回転円面の座標系を次のように定める.
,
,
(1-8)
d1,d2,d3はφの関数である.
・aからbまでの旋回途中のCからのベクトルは
(1-9)
・以上の式よりφθを与えれば上記の全てが決まる.
・問題はθ=0のときpがaより壁床から遠ざかる方向へ向けることである.すなわち
(1-10)
と置く.
・壁外側で壁から遠ざかる条件は
→
(1-11)
・壁内側で壁から遠ざかる条件は
→
(1-11)’
ただしは壁の外向きの法線ベクトルである.
床から遠ざかる条件は
→
(1-12)
である.
・すなわち,ベクトルを壁と床から遠ざける方向に向ければよいということになる.
・なお図(b)から分かるように,φが小さいほど回転円の半径rが大きくなり,aからbに到る弧の長さが短くなる.
・(1-8)式のをφの関数として表す.
(1-13)
(1-14)
(1-15)
,
(1-16)
と置く.
・壁外側で壁から遠ざかる条件(1-11)式は
(1-17)
・壁内側で壁から遠ざかる条件(1-11)’式は
(1-17)’
ただし
,
(1-18)
・ということもあり得る.これは初期ベクトルaが壁の正負法線方向を向いていることを意味し,aが壁側に向いていればφはどんな値でもよく,壁から離れる方向に向いていればどんな値でも駄目ということである.
・床から遠ざかる条件(1-12)式は
(1-19)
ただし
,
(1-20)
・ということもあり得る.これは初期ベクトルaが真上または真下を向いていることを意味し,aが真下を向いていればφはどんな値でもよく,真上を向いていればどんな値でも駄目ということである.
・この(1-17)または(1-17)’,(1-19)2式を満足するφは
壁外側で
(1-21)
壁内側で
(1-21)’
・床から遠ざかる条件
(1-22)
・のとき壁から遠ざかる率が最大となり,
のとき床から遠ざかる率が最大となる.
・この2つの範囲内のφの中の最小値を採用すればaからbまでの弧の長さが最小となりそれが望ましいが,壁または床からできるだけ早く遠ざかりたいときは,(1-21),(1-22)式の範囲内にあればまたは
とすることもあり得る.
(U)初期方向ベクトルaが目標方向bより壁から遠く床に近い場合
・この場合はaを床から遠ざけbを壁から遠ざける方向へ回転する回転円を見つければよい.
(U−1)aを床から遠ざける旋回
・aからbまでの旋回途中のCからのベクトルは(1-19)式
(2--1)=(1-9)
であり,始まりの点で床から遠ざかる方向へ,終点で壁から遠ざかる方向となるφの範囲を求めればよい.
・始点で床から遠ざかる条件は
(2-2)=(1-10)
→
(2-3)=(1-12)
,
(2-4)=(1-16)
(2-5)=(1-19)
ただし
,
(2-6)= (1-20)
・(2-5)式を満足するφは
(2-7)=(1-22)
(U−2)bを壁から遠ざける旋回
・終点でbからaに向かって壁から遠ざける問題は,bからaへの経路を考え,bの始まりで壁から遠ざかる条件を求める.このとき(U−1)と同じ旋回円とするため回転軸nを同じ方向にとし回転角θを逆向きに取る.座標系とn,cを(U−1)と同じとし,bを始点とする座標系
を求める.
,
,
(2-8)=(1-1)
(2-9)=(1-2)
(
) (2-10)=(1-4)
(2-11)= (1-5)
(2-12)= (1-6)
,
,
(2-13)
・bからaまでの旋回途中のCからのベクトルは
(2-14)
(2-15)
・nを軸とするbからの回転はθが負の方向である.
・壁外側でθ負の方向に回転して壁から遠ざかる条件は
→
(2-16)
・壁内側でθ負の方向に回転して壁から遠ざかる条件は
→
(2-16)’
・は(1-13),(1-15)式を参照して
(2-17)
(2-18)
,
(2-19)
と置く.
・壁外側で壁から遠ざかる条件(2-16)式は
(2-20)
・壁内側で壁から遠ざかる条件(2-16)’式は
(2-20)’
ただし
,
(2-21)
・壁外側で(2-20)式を満足するφは
(2-22)
・壁内側で(2-20)’式を満足するφは
(2-22)’
・(2-7),(2-22)または(2-22)’2式の範囲内のφであればよいが,その中の最小値を採用すればaからbまでの弧の長さが最小となる.
(V)初期方向ベクトルaが目標方向bより壁に近く床から遠い場合
・この場合は(U)のaとbを入れ替えて,aが壁から遠ざける方向へ,bを床から遠ざける方向へ回転する回転円を見つければよい.その条件は(U)でa→b,b→aとした式がそのまま使える.φの範囲は(2-7),(2-22)式である.
(W)初期方向ベクトルaが目標方向bより壁と床両方から遠い場合
・この場合は(T)のaとbを入れ替えて, bからaへの経路と考え,(T)の結果を利用すればよい.φの範囲は(1-21),(1-22)式である.これにより壁床に近いbから壁床から遠ざかる経路が得られる.