Far planner之 障碍物的图搜索
????????我們知道，最短路徑搜索算法比如A*、Djstra等經(jīng)常是用在grid地圖，也就是柵格地圖，當我們把路徑搜索算法運用到由node組成的障礙物graph的時候，有很多問題是要進行思考的，當我在完成這個障礙物graph的最短路徑搜索的時候，我也想明白了一些far planner里的東西，所以干脆就來說一說好了。
? ? ? ? 雖然寫的很爛，但是開源了，大家想玩的也可以玩玩。
Leeable/Graph-base-far_planner (github.com)https://github.com/Leeable/Graph-base-far_planner
????????
2022-04-22 10-28-28_Trim
????????
? ? ? ? 改進以后，現(xiàn)在可以不貼著contour_graph的邊走了
?
????????如上圖所示，這個是一個由Node10出發(fā)，到達Node452的一個路徑規(guī)劃。我們知道，對于Far planner而言，它的障礙物contour_graph并不是完全沿著實際障礙物的，而是會有一點膨脹區(qū)域，所以用它生成的contour_graph來導航是完全沒有問題的（之后我會在改進一下way_point，讓它更合理化，而不是直接用障礙物的node）。
????????算法主體如下：
????????1、給定起點start坐標（x1,y1）和終點坐標（x2,y2)
? ? ? ? 2、在contour_graph上找到離起點坐標最近的contour_node，并當作near_start，同理終點。
? ? ? ? 3、把起點坐標當作cur_node 以半徑R范圍進行搜索，生成一個有效的node_list
? ? ? ? 4、在這些有效的node_list里進行next_node的啟發(fā)式搜索
? ? ? ? 5、當終點出現(xiàn)在node_list時，由終點開始返回它的父節(jié)點，得到path
? ? ? ? 是不是聽起來還蠻簡單的，這里主要涉及到幾個問題：
? ? ? ? 1：如何判斷它在R范圍內(nèi)的node是有效的
? ? ? ? 2：如何進行搜索，它的啟發(fā)值要怎么設(shè)計
? ? ? ? ?對于起點node10而言，以半徑R進行搜索，就會得到類似于下面的示意圖：
? ? ? ? 其中我們可以看到，有效的node只有[9,11,12,21,17]，而無效的node有[16,695]，這些無效的node怎么來的呢，比如node16，它和node10的連線會被[9,695]連成的障礙物墻壁打斷，如果放任不管，那就會穿墻。
? ? ? ? ?再比如node695，它雖然不會穿墻，但是它本身就是往障礙物里鉆的，我們也不能選它
? ? ? ? ?有了以上兩個結(jié)論，我們就要來設(shè)計去排除掉我們的干擾node，在far_planner里也有一樣的設(shè)計。對于第一個問題而言，far_planner里有一個do_intersect函數(shù)來判斷交叉，我們也效仿它寫了一個增強的。它的主要用途就是判斷從cur移動到next的這個線段<cur,next>是否和別的障礙物組成的線段<A,B>相交。? ? ? ??
? ? ? ? ?第二個問題，涉及到角度問題，far_planner給的是一個固定的角度，讓不同的頂點類型有不同的閾值，我這里改了一下，用角平分線來做。
? ? ? ? 解決第二個問題需要用到它的surf_dirs 和 free_direct_type 也就是頂點的方向向量和它的頂點幾何類型（比如凹、凸、PILLAR、UNKnown）。對于node10而言，它是一個convex_Node 也就是凸點，我們用它的方向向量來做角平分線。
????????
? ? ? ? ?這樣我們就會得到Surf與Topo的夾角α，對于凸點，任意給to_node，只要它與Topo_dir的夾角大于α，即說明它是有效的（就不會往內(nèi)部走了）ps.感謝俺的女朋友，讓俺豁然開朗
?
? ? ? ? ?換算為cos（因為cos在（-180，0）單調(diào)，在（0，180）也單調(diào)，就是我的cos(β)要小于cos(α)，這也是far_planner在處理graph上的方法，對于純graph而言，更多情況下的判斷是更貼近于數(shù)學幾何的。
? ? ? ? ?這里結(jié)束之后，就是主程序?qū)β窂降牟檎伊?#xff0c;用的是基于a*進行魔改一下的路徑搜索：
? ? ? ? ?寫這個是用來打標簽的，因為我可以random一個起始點和終點，讓它給出路徑，這個路徑就是我的標簽tag ~ 還沒有完全寫完 random的部分需要判斷random_point在不在contour障礙物內(nèi)部，所以我還得繼續(xù)完善，但是孩子想去玩了。。。先鴿了算了~
總結(jié)
以上是生活随笔為你收集整理的Far planner之 障碍物的图搜索的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： iTunes导入歌曲和铃声到iphone
- 下一篇： Public Private Prote