3.10.2[補].障害物を通り抜ける前の自由空間における経路生成 

(フォルダ名: ThinSlitPass )          2009−12−3

 

・自由空間における経路は任意に設定できるが,経路は障害物に接触しないというのが必要条件である.

・また,経路はできるだけ短く迂回しないことが望ましい.

・この条件を満足する自由空間経路を考える.

・経路はロボットの初期ポーズから障害物を通り抜ける直前のポーズまでとする.

 (初期ポーズから障害物を通り抜けて最終の目標位置に到る全体の経路は,終端から逆に障害物から抜け出し自由空間に到る逆経路を計算する.)

・この合理的な経路は図1示すものである.

・簡単のため,ロボットベースを座標原点とし初期位置Aはその真上にあるとする.

・経路は障害の壁面を斜めに倒してA,Bを含む平面α内にとる.このようにすれば壁に接触することはない.

・すなわち,αはBA方向の単位ベクトルと壁面内ベクトルを含む平面である.

・以上のことを式で表す.

・各点の位置ベクトル:

  既知の点 A:, B:,  未知の点 C:, D:, P:

・α平面規定の単位ベクトル()

   , (既知), ,           (1)

・半径RとD点

                                  (2)

    →    (3)

                                                            (4)

 が分かれば(2)式で与えられる.

・円弧経路上の点Pの位置:DAから測った角度をφとする.

                           (5)

OBより小さいと位置決めできないことがあり得るので,そのときはRをこの点についてのみとなるまで大きくする.

・点Bに到るまでのφ=φB

   , , 

 ならば(BACのこちら側)左回りにならば(BACの向こう側)右回りにと回転してAからBに到る.