3.10.31干渉回避しながらゴールポーズから初期ポーズへの経路逆生成−局所的干渉回避法
ファイル名:3-10-31CollAvoidPassGeneretion.doc
2012-5-28集約
目 次
T.穴通り抜け経路逆生成の順序
U.壁外側における干渉回避経路逆生成と穴通り抜け運動
U−1.関節とその並びの順逆
U−2.干渉回避運動における関節J[M]の目標設定
U−2−RP.J[M]の前の関節がRP関節の場合の目標設定
U−2−PP.J[M]の前の関節がQP(PP) 関節の場合
(1)目標点の候補
(2)壁外で壁から離れて関節を順並びとするためのJ[M]の目標点
(2−1)目標点の候補1:FT=F,Tのどちらか
(2―2)目標点の候補2:HY=H,Yのどちらか
(2−3)候補1,2(FT,HY)の選択の条件
(2−4)目標点までの途中に壁床干渉がある場合
V.壁内側における干渉回避経路逆生成と穴通り抜け運動
V−1.関節とその並びの順逆
V−2.干渉回避運動における関節J[M]の目標設定
V−2−RP.J[M]の前の関節がRP関節の場合の目標設定
V−2−PP.J[M]の前の関節がQP(PP) 関節の場合
(1)目標点の候補
(2)壁内で床から離れて関節を順並びとするためのJ[M]の目標点
(2−1)目標点の候補1:FT=F,Tのどちらか
(2―2)目標点の候補2:HY=H,Yのどちらか
(2−3)候補1,2(FT,HY)の選択の条件
(2−4)目標点までの途中に壁床干渉がある場合
[序]
・薄壁の穴を通り抜けワークを目標に位置決めする問題は,
A:穴通り抜け目標に位置決めするロボットのゴールポーズの解を求める問題
B:初期ポーズ−ゴールポーズ間の経路を求める問題
に分ける.
・前者は「詳説ロボットの運動」第8章8.2.5項に述べた通りである.
・Bについて経路路生成の順序と干渉回避運動の1方法「局所的干渉回避法」を述べる.
・また,ゴールポーズから初期ポーズまで経路を壁床干渉を回避しながら逆生成する方策を述べる.
・経路は解説で述べたように,ゴールポーズから初期ポーズまで経路逆生成を行う.
T.穴通り抜け経路逆生成の順序
B-WR1:ワークの持ち上げ運動
・ワークをゴール位置から持ち上げ壁から遠い方向に向ける.
B-WR2:関節順並び運動(干渉回避運動)
・壁外の関節を壁床自己干渉を回避しながら,壁から順に遠くへ関節を並べるように(順並び)持っていく.
B-TH:穴引き抜き運動
・順並びとなったら,または順並びとなる前に壁床隅の隘路に陥ったら,穴からアームを引き抜く運動と共に,壁外の関節を順並びに整える.
B-FS1:壁内(自由空間)関節順並び運動
・関節・ワークの全てを壁から引き抜いたら,壁内側(自由空間)で壁床自己干渉を回避しながら,床から順に関節が高くなるように(順並び)動かす.
B-FS2:初期ポーズへの直線運動
・順並びとなったらリストを初期ポーズのリスト位置に向けて直線的に動かす.
B-FS3:初期ポーズ近傍運動
・関節角q[ ]を初期値q0[ ]に向けて直線的に変える.
・これは,B-FS2を最終段階まで続けると,初期ポーズでq0[ ]以外の値に収束するのでそれを避けるため行う.
・これらの運動において,どの段階でも干渉回避不能となれば経路生成不能である.
・U,V以降に壁外と壁内における干渉回避運動の具体的な方向について述べる.
U.壁外側における干渉回避経路逆生成と穴通り抜け運動 2012-4-13
・「局所的干渉回避法」とは,限定された空間でロボットを周囲にぶつからずに運動させるために,関節JM+1の運動可能性を考慮しながらJMの運動を決めると言う方策である.
・ロボットとアームが壁の穴を通り抜けて目標にワークを位置決めした最終ポーズから,穴通り抜け以前の初期ポーズへと経路を逆生成する問題において,壁外側の関節・アームが壁や床とぶつからないように如何に関節角を動かすか,の問題である.
・壁外側において壁を通り抜ける前にアームを伸ばさなければならないから,壁外側の関節の処理の方策は,壁・床との衝突を避けながら,壁から順にJM,JM+1,....と先の方の関節を遠くへ並べることである.
・この関節を順に並べるために,局所的にJMの運動をJM+1だけを考慮してその方向と動きの量(q[M-1]の増減と回転量)を決める(M=MW,MW+1,….,MWは壁直後の関節番号).
・局所的方策では衝突なしに順並びとすることができないケースがあり,M+2以上を考慮した大局的な考慮なら可能かも知れないが,ここでは単純に局所的方策を採用する.
・なお,ここでは「壁と床」との衝突を述べるが,一般的にどんな障害物でも衝突状態を計算式に表現できれば応用できる.
U−1.関節とその並びの順逆
・ロボットは直列にR,P関節がオフセットなしに連結しているとする.リストはRPRの3関節構造とする.
・ロボットの初期状態(全関節角q[ ]=0)は垂直に真上に伸びた状態とする.
・壁直後の関節番号をMWとする.
・考えている関節番号をMとする.
・J[M]はP関節とする.R関節はその延長上にP関節があるので壁床との衝突は次の関節で考慮すればよい.
・そのMは,MW+1からリストN-2までとする.リストの3関節角によるワークの運動は別に考える.
・J[M]を動かす関節角をq[MF]とする.MFはJ[M-1]がR関節でなければMF=M-1,R関節ならばMF=M-2である.
・J[M]を動かすときに考慮すべき関節番号をMHとする.J[M+1]がR関節でなければMH=M+1,R関節ならばMH=M+2である.
・壁からJ[MF]よりJ[M]が遠い場合を順並び,近い場合を逆並びと呼ぶ.
・アームを伸ばすということは,q[MF]を0へ持っていくことである.これはJ[MF-1]−J[MF]が順ならJ[MF]−J[M]を順とし,J[MF-1]−J[MF]が逆ならJ[MF]−J[M]を逆とすることを意味する.
・J[MF]−J[M]を逆とするのは壁に向かってアームを伸ばすことであり,壁にぶつかる可能性が高い.壁にぶつかる前に順並びとならなければ壁の穴を通り抜けることができず,経路生成不能を意味する.
・J[MF]−J[M]が逆でも壁直後が順であるから次々と順となり,J[MF-1]−J[MF]もいずれ順となり,そのときJ[MF]−J[M]が逆から順に変わるので,正常に経路生成できる.
U−2.干渉回避運動における関節J[M]の目標設定
U−2−RP.J[M]の前の関節がRP関節の場合の目標設定
・目標の基本は,壁外では関節を壁から遠くへ順並びとすること,壁内では関節を床からの高さの順並びとすることである.
・この場合はJ[M]をRP関節の回転によって望みの方向へ向けることができる.
・目標方向bは,壁外のときは壁面法線方向(Y軸正方向),壁外のときは真上方向とする.
・J[M]の次の関節J[MH]をq[M]によって自由に動ける(壁床に衝突しないで動ける)状態にするためには,少なくとも,壁外ではJ[M]−J[MH]間アーム長LHより高い点,壁内では壁から離れた点をJ[M]の目標とすればよい.
・この高さが存在すれば一気に目標へ動かすことができる.高さが足りなければ1ステップ運動とする.
・現在の方向ベクトルaから望みの方向ベクトルbへの運動は,途中で壁・床と衝突を避けるために始点aより壁側へ向かず,aより下に向かないようにそのベクトルの経路を決める.経路と方向ベクトルを与えるRP関節角の計算は別紙で述べる.
U−2−PP.J[M]の前の関節がQP(PP)
関節の場合
(1)目標点の候補
・P関節角q[MF]だけで動かすのでJ[M]はJ[MF]を軸とする円を描き,目標点はその旋回円上にある.
・候補となり得る点は図4に示す通りである.
・壁内外ともアームを伸ばす方向(図の最遠点F,またはJ[MF]と高さが等しい点T)が関節を順並びとする目標点であるが,次の関節J[MH]が動き易いかどうかによって変更が必要である.
・次の関節J[MH]の状態を考慮しながら,J[MF]を中心とするJ[M]の旋回円上にJ[M]の目標点を以下のようにする.
・このときJ[M]の目標点までの運動は一気に目標まで回転する場合と目標に向かって1ステップ運動する場合の2種の運動がある.
・一気運動が可能ならその関節を一気に目標まで動かす.一旦アームを伸ばしてしまえばその後は全体的に動かし易くなるからである.
(2)壁外で壁から離れて関節を順並びとするためのJ[M]の目標点
・壁外では関節を壁から離れて順並びとすることが目標である.
・壁外では関節は壁から離れている場合が多いので,目標を設定するとき壁から離すということは目標設定の段階では考えない.壁干渉については(2−4)で述べる.
・そのJ[M]の目標点として2つの候補点を挙げ,2つのどちらにするかその条件を提示し,更に壁床との干渉点がある場合の目標を決める.
(2−1)目標点の候補1:FT=F,Tのどちらか
・アームを伸ばす本命はFまたはTであり,その点をFTとする.
・F,Tの選択は,完全にアームを伸ばした状態(q[MF]=0)に近い方とする.
(常にFがTよりq[MF]=0に近いとは限らない.
(2―2)目標点の候補2:HY=H,Yのどちらか
・目標点の候補2は,高い点を採用することである.
・J[M]がアーム長LHより高ければ次の関節J[MH]が動き易さくなる.
・LHより高い点YがあればYが候補2であり,なければ旋回円上の最高点Hが候補2である.
・この場合HはLHより高くないので,最終的にHが目標のままだとJ[MH]のアームが伸ばせないことになり,このままでは行き詰まる恐れがある.行き詰まったときは壁穴からアームを引き抜く運動を同時に行い,HをLH以上とする必要がある.それが不可能なら穴引き抜きの経路逆生成はできない.
(2−3)候補1,2(FT,HY)の選択の条件
・図7に示すように,FT点がアーム長LHより高ければJ[M]がFT点へ来てもJ[MH]はアームを伸ばすことができる.
・また既にJ[MH]がアームを伸ばせる状態にあれば,LHより低くてもFT点へ達しても差し支えない.
・FTが目標で現値から目標までの旋回の途中で壁床干渉がなければJ[M]の一気運動が可能である.
・逆に図8のようにJ[MH]がアームが畳まれた状態でかつFT点が低ければ,FTより高い点HYを目標としなければならない.
・HY点がLHより低ければ前述のように行き詰まる可能性がある.
(2−4)目標点までの途中に壁床干渉がある場合
・壁床干渉がある場合は,できるだけアームを伸ばすという基本方針から,その干渉点B,Wを目標とする.
・いつまでも干渉点が目標ではいずれ行き詰まる.その場合は経路生成不能である.
V.壁内側における干渉回避経路逆生成と穴通り抜け運動 2012-8-3
・Tで述べたB-FS2の干渉回避運動では関節を床から順に高く並べる.
V−1.関節とその並びの順逆
・床からJ[MF]よりJ[M]が高い場合を順並び,低い場合を逆並びと呼ぶ.
・アームを伸ばすということは,q[MF]を0へ持っていくことである.これはJ[MF-1]−J[MF]が順ならJ[MF]−J[M]を順とし,J[MF-1]−J[MF]が逆ならJ[MF]−J[M]を逆とすることを意味する.
・J[MF]−J[M]を逆とするのは床に向かって下へアームを伸ばすことであり,床にぶつかる可能性が高い.床にぶつかる前に順並びとならないケースがあればそれは経路生成不能を意味する.
V−2.干渉回避運動における関節J[M]の目標設定
V−2−RP.J[M]の前の関節がRP関節の場合の目標設定
・U−2−RP壁外の場合と同様に,RP関節によってJ[M]を任意の方向へ向けることができる.
・図3の目標方向はbは真上であるが,壁に近いときは壁からJ[MH]のアーム長LH以上離れた方向へ修正する.
V−2−PP.J[M]の前の関節がQP(PP)
関節の場合
(1)目標点の候補
・U−2−PPの図4に示した点が候補となる.
・J[M]の目標点として2つの候補点を挙げ,2つのどちらにするかその条件を提示し,更に壁床との干渉点がある場合の目標を決める.
(2)壁内で床から離れて関節を順並びとするためのJ[M]の目標点
・壁内では関節を床から離れて順並びとすることが目標である.
・壁内では穴通り抜け直後の関節は床から離れている場合が多いので,目標を設定するとき床から離すということは目標設定の段階では考えない.床干渉については(2−4)で述べる.
・そのJ[M]の目標点として2つの候補点を挙げ,2つのどちらにするかその条件を提示し,更に壁床との干渉点がある場合の目標を決める.
(2−1)目標点の候補1:FT=F,Tのどちらか
・アームを伸ばす本命はFまたはTであり,その点をFTとする.
・F,Tの選択は,完全にアームを伸ばした状態(q[MF]=0)に近い方とする.
(常にFがTよりq[MF]=0に近いとは限らない.
(2―2)目標点の候補2:HY=H,Yのどちらか
・目標点の候補2は,壁から遠くかつ高い点を採用することである.但し全アームを穴から引き抜いた段階で高さはある程度保たれており,壁から遠くへ離す方が重要である.
・J[M]がアーム長LHより壁から遠ければ次の関節J[MH]が動き易さくなる.
・LHより遠い点VがあればVが候補2であり,なければ旋回円上の最高点Hまたはアームへの最近点N(壁から最も遠い点N)が候補2である.
(最近点Nを壁から最も遠いとしたが,J[ME]-J[MF]順序が図と逆ならFが最も遠い点Nとなり,この場合はHまたはFが第2候補である.ここでは説明を簡単にするためにNを取った.)
・この場合HorNはLHより遠くないので,最終的にHorNが目標のままだとJ[MH]のアームが伸ばせないことになり,このままでは行き詰まる恐れがある.行き詰まったときは壁穴からアームを引き抜く運動を同時に行い,HorN
をLH以上とする必要がある.それが不可能なら穴引き抜きの経路逆生成はできない.
(2−3)候補1,2(FT,HY)の選択の条件
・図13に示すように,FT点がアーム長LHより壁から遠ければJ[M]がFT点へ来てもJ[MH]はアームを伸ばすことができる.
・また既にJ[MH]がアームを伸ばせる状態にあれば,LHより近くてもFT点へ達しても差し支えない.
・FTが目標で現値から目標までの旋回の途中で壁床干渉がなければJ[M]の一気運動が可能である.
・逆に図14のようにJ[MH]がアームが畳まれた状態でかつFT点が壁に近ければ,FTより遠い点HYを目標としなければならない.
・HY点がLHより近ければ前述のように行き詰まる可能性がある.
(2−4)目標点までの途中に壁床干渉がある場合
・壁床干渉がある場合は,できるだけアームを伸ばすという基本方針から,その干渉点B,Wを目標とする.
・いつまでも干渉点が目標ではいずれ行き詰まる.その場合は経路生成不能である.
[註1]壁床との干渉の定義
・壁床との干渉は関節点が壁床と接触することではなく,関節やアームの実寸法を考えて余裕値Marginだけの空隙のある状態を表す.
[註2]自己干渉について
・アーム・関節が互いにぶつかる自己干渉は目標設定には考慮していない.
・目標に向かって一気運動の1刻み毎に,または1ステップ運動の度に自己干渉チェックして,干渉があれば運動を修正する,という方法で自己干渉を回避する.
[註3]全関節が順並びとなる可能性
・次の関節J[MH]を考慮してJ[M]を動かし,動かせるなら一気に動かすやり方で多くの場合全関節を順並びとすることができるが,穴通り抜ける前に逆並びが残ったまま壁と床の隘路に陥ることがある.
・壁の穴通り抜け動作をしないで壁外だけで関節を順並びとする前提であったが,隘路に陥ったらアームを壁穴から徐々に抜きながら壁外関節運動を試みる.
・穴から抜きながらでも隘路に陥ったら経路の逆生成不能ということを意味する.
・全てが壁内となった後の自由空間での経路逆生成運動では隘路に陥る恐れは殆どない.