３．１０．２［補］．障害物を通り抜ける前の自由空間における経路生成　　　２００９−１２−３

　　　　　　　　　　　（実行ファイル 3-10-2ThinSlitPassexe ）

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

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

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

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

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

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

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

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

・すなわち，αはＢＡ方向の単位ベクトルと壁面内ベクトルを含む平面である．

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

・各点の位置ベクトル：

　　既知の点　Ａ：，　Ｂ：，　　未知の点　Ｃ：，　Ｄ：，　Ｐ：

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

　　　，　（既知），　，　　　　　　　　　　 (1)

・半径RとＤ点

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　(2)

　　　　→　　　　(3)

　　　                                                         (4)

　Ｒが分かればは(2)式で与えられる．

・円弧経路上の点Ｐの位置：DAから測った角度をφとする．

　　　　　　　　　　　　　　　　　　　　　　　　　　　(5)

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

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

　　　，　，　

　ならば（BはACのこちら側）左回りに，ならば（BはACの向こう側）右回りにと回転してAからBに到る．