百度無人駕駛apollo項目路徑規劃a*算法分析
車輛路徑規劃尋路算法有很多，apollo路徑規劃模塊使用的是啓發式搜索算法A*尋路算法
a*算法是一種在路網上中求解最短路徑的直接搜索尋路算法,原理是引入估價函數，加快搜索速度，提高了局部擇優算法搜索的精度，成爲當前較爲流行的最短路算法
估價函數用公式表示爲： f(n)=g(n)+h(n)
其中， f(n) 是從初始節點到目標節點的最佳路徑的估計代價，
g(n) 是從初始節點到節點n的代價，
h(n) 是從節點n到目標節點的估計代價。
要保證找到最短路徑（最優解的）條件，關鍵在於估價函數f(n)的選取（或者說h(n)的選取）。
很顯然，距離估計與實際值越接近，估價函數取得就越好，例如對於路網來說，可以取兩節點間曼哈頓距離做爲距離估計，即f=g(n) + (abs(dx - nx) + abs(dy - ny))；這樣估價函數f(n)在g(n)一定的情況下，會或多或少的受距離估計值h(n)的制約，節點距目標點近，h值小，f值相對就小，能保證最短路的搜索向終點的方向進行。
a*算法保持着兩個表，open表和closed表，open表由未考察的節點組成，而closed表由已考察的節點組成，當算法已經檢查過與某個節點相連的所有節點，計算出它們的f,g和h值，並把它們放入open表，以待考察，則稱這個節點爲已考察的
算法過程
1 令s爲起始節點
2 計算s的f,g和h值
3 將s加入open表，此時s是open表裏唯一的節點
4 令b＝open表中的最佳節點（最佳的意思是該節點的f值最小）
如果b是目標節點，則退出，此時已找到一條路徑
如果open表爲空，則退出，此時沒有找到路徑
5 令c等於一個與b相連的有效節點
計算c的f,g,h值
檢查c是在open表裏還是在closed表裏，若在closed 表中，則檢查新路徑是否比原先更好（f值更小），若是則採用新路徑，否則把c添加入open表
對所有b的有效子孫節點重複第5步
6 重複第4步
現在分析一下route模塊裏a*算法的實現
節點定義在modules/routing/graph/topo_node.h文件,graph目錄下還有計算用到的邊和圖的定義
主要算法實現在modules/routing/strategy/a_star_strategy.cc文件
首先看定義的h(n)函數
double AStarStrategy::HeuristicCost(const TopoNode* src_node,
const TopoNode* dest_node) {
const auto& src_point = src_node->AnchorPoint();
const auto& dest_point = dest_node->AnchorPoint();
double distance = fabs(src_point.x() - dest_point.x()) +
fabs(src_point.y() - dest_point.y());
return distance;
}
這段代碼非常清晰，h(n)=abs(dx-nx)+abs(dy-ny)
bool Reconstruct(
const std::unordered_map<const TopoNode*, const TopoNode*>& came_from,
const TopoNode* dest_node, std::vector<NodeWithRange>* result_nodes)
這段函數實現了主要是算法第5步的功能，然後內部調用了AdjustLaneChange，AdjustLaneChange函數又調用了AdjustLaneChangeBackward和AdjustLaneChangeForward函數
最後是供其他模塊調用的Search函數
bool AStarStrategy::Search(const TopoGraph* graph,
const SubTopoGraph* sub_graph,
const TopoNode* src_node, const TopoNode* dest_node,
std::vector<NodeWithRange>* const result_nodes)
其中open_set_detail就是open表，使用的是priority_queue優先級隊列，push加入一個元素，pop刪除第一個元素
這是一個非常標準的A*尋路算法實現
百度路徑尋路算法的不足
1 使用了通用的a*算法，沒有使用效果更好的其他尋路算法
2 只考慮了本車的情況，沒有考慮和周圍其他行人車輛的協同
3 沒有考慮碰撞檢測和避障算法的實現，避障算法這個其實是重中之重