????????我們知道，最短路徑搜索算法比如A*、Djstra等經常是用在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

????????

? ? ? ? 改進以后，現在可以不貼著contour_graph的邊走了

?

????????如上圖所示，這個是一個由Node10出發，到達Node452的一個路徑規劃。我們知道，對于Far planner而言，它的障礙物contour_graph并不是完全沿著實際障礙物的，而是會有一點膨脹區域，所以用它生成的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的啟發式搜索

? ? ? ? 5、當終點出現在node_list時，由終點開始返回它的父節點，得到path

? ? ? ? 是不是聽起來還蠻簡單的，這里主要涉及到幾個問題：

? ? ? ? 1：如何判斷它在R范圍內的node是有效的

? ? ? ? 2：如何進行搜索，它的啟發值要怎么設計

? ? ? ? ?對于起點node10而言，以半徑R進行搜索，就會得到類似于下面的示意圖：

? ? ? ? 其中我們可以看到，有效的node只有[9,11,12,21,17]，而無效的node有[16,695]，這些無效的node怎么來的呢，比如node16，它和node10的連線會被[9,695]連成的障礙物墻壁打斷，如果放任不管，那就會穿墻。

? ? ? ? ?再比如node695，它雖然不會穿墻，但是它本身就是往障礙物里鉆的，我們也不能選它

? ? ? ? ?有了以上兩個結論，我們就要來設計去排除掉我們的干擾node，在far_planner里也有一樣的設計。對于第一個問題而言，far_planner里有一個do_intersect函數來判斷交叉，我們也效仿它寫了一個增強的。它的主要用途就是判斷從cur移動到next的這個線段<cur,next>是否和別的障礙物組成的線段<A,B>相交。? ? ? ??

? ? ? ? ?第二個問題，涉及到角度問題，far_planner給的是一個固定的角度，讓不同的頂點類型有不同的閾值，我這里改了一下，用角平分線來做。

? ? ? ? 解決第二個問題需要用到它的surf_dirs 和 free_direct_type 也就是頂點的方向向量和它的頂點幾何類型（比如凹、凸、PILLAR、UNKnown）。對于node10而言，它是一個convex_Node 也就是凸點，我們用它的方向向量來做角平分線。

????????

? ? ? ? ?這樣我們就會得到Surf與Topo的夾角α，對于凸點，任意給to_node，只要它與Topo_dir的夾角大于α，即說明它是有效的（就不會往內部走了）ps.感謝俺的女朋友，讓俺豁然開朗

?

? ? ? ? ?換算為cos（因為cos在（-180，0）單調，在（0，180）也單調，就是我的cos(β)要小于cos(α)，這也是far_planner在處理graph上的方法，對于純graph而言，更多情況下的判斷是更貼近于數學幾何的。

? ? ? ? ?這里結束之后，就是主程序對路徑的查找了，用的是基于a*進行魔改一下的路徑搜索：

? ? ? ? ?寫這個是用來打標簽的，因為我可以random一個起始點和終點，讓它給出路徑，這個路徑就是我的標簽tag ~ 還沒有完全寫完 random的部分需要判斷random_point在不在contour障礙物內部，所以我還得繼續完善，但是孩子想去玩了。。。先鴿了算了~

總結

以上是生活随笔為你收集整理的Far planner之 障碍物的图搜索的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。