本文主要講Apollo無人車中A*算法的C++實現。
百度Apollo項目中有兩個地方使用了A*算法，如下。至於爲啥不把兩個A*算法寫成一個統一而且通用的庫呢，可能是不方便吧，因爲裏面有些細節處理不一樣。
1. 在路由規劃模塊 routing 中：modules\routing\strategy\a_star_strategy.cc
2. 在開放場地局部軌跡規劃模塊 open_space 中：modules\planning\open_space\coarse_trajectory_generator\hybrid_a_star.cc
1 A*算法僞代碼
在維基百科上有A*算法的僞代碼，比較乾淨，如下。
openSet := {start} //初始化：只把起點放進去
while openSet is not empty ①
current := the node in openSet having the lowest fScore[] value ②
if current = goal
return reconstruct_path(cameFrom, current) ③
openSet.Remove(current) ④
for each neighbor of current ⑤
tentative_gScore := gScore[current] + d(current, neighbor)
if tentative_gScore < gScore[neighbor]
cameFrom[neighbor] := current
gScore[neighbor] := tentative_gScore
fScore[neighbor] := gScore[neighbor] + h(neighbor)
if neighbor not in openSet
openSet.add(neighbor)
大致的流程解釋如下：
① 判斷open是否空 每次一上來都是先檢查open列表是不是空。如果是空的後面的就不執行了。你可以認爲A *算法的運行過程就是圍繞着open列表的操作，先取節點，再刪除節點，再擴展節點，再添加節點；
② 從open中取出代價最小的節點 如果open列表不是空的，從open列表中取出代價最小的節點，注意是f值最小的，不是g值。一般用優先級隊列實現open列表，節點進入優先級隊列是要按照代價值大小排序的，優先級大的排在棧頂第一個，所以取最小值的取第一個就行了。優先級可以自己定義，這裏定義成代價越小優先級越高就行了；
③ 判斷是否爲目標 每從open列表中取出一個節點（一般管這個節點叫當前節點current），都判斷一下它是不是目標節點，如果是那就說明找到最短路徑了，後面的就不再執行了。
④ 從open中刪除最小代價的節點 如果當前節點不是目標節點，那就檢查它的鄰居們。但是要先從open列表中刪除當前節點。爲啥要先刪除呢？這就涉及算法的理論基礎了，比較麻煩。大概意思是，節點的代價都是從小的傳導到大的，如果每次取代價最小的節點，後面的節點代價總是大於當前節點（邊的代價都是大於零的），不會存在一條更小的路徑再回來通過當前節點，所以刪除是一次性的，永遠不用再添加回來；如果open列表有兩個，就每個都要刪除一次。如果用了close列表，那就把當前節點加入到close列表中，表示後面不再拜訪它了，一個節點不可能既在open列表裏又在close列表裏。
⑤ 檢查鄰居並計算代價 遍歷當前節點的所有鄰居節點（遍歷的順序無所謂），並且計算鄰居節點的代價。鄰居節點可能在open列表裏，有可能不在裏面。如果在裏面，而且這個剛計算出來的代價比已經有的代價小，就用更小的替換了，同時更新他的爸爸節點，也就是說此時找到一條更短的路徑了。如果不在裏面，就加入到open列表裏；上面的僞代碼沒使用close列表，但Apollo程序中都用了close列表，這樣效率高一點（不走回頭路），如果鄰居節點在close列表中，則直接跳過不處理。
⑥ 回到①，循環；
A*算法的核心概念就是這個open列表了（你也可以叫集合，都無所謂，反正我們現在只關心它裏面有啥，不關心裏面元素是按什麼順序排列的），裏面存儲着已經被發現（但還有待進一步擴展的）節點。僞代碼裏的openSet出現了5次，分別是在幹什麼呢，如下。後面的括號表示Apollo裏用哪種數據結構實現。可見搜索都是用普通的容器，取最小元素用優先隊列實現。
1 判斷openSet是不是空（用優先隊列判斷）
2 從openSet中取代價最小的元素（用優先隊列獲取）
3 從openSet中把代價最小的元素刪除
4 判斷代價最小的元素的鄰居是不是在openSet裏面（用普通的容器實現）
5 把鄰居插入到openSet裏面
基本算法不是很複雜，但是實現起來有很多細節，非常瑣碎，一個個看吧。
2 路由規劃中的A*算法
先看路由規劃模塊routing中的實現，a_star_strategy.cc中的Search函數裏，這個程序跟維基百科僞代碼的邏輯是一樣的，變量命名方式也一樣。
while (!open_set_detail.empty()) {
current_node = open_set_detail.top();
const auto* from_node = current_node.topo_node;
if (current_node.topo_node == dest_node) {
if (!Reconstruct(came_from_, from_node, result_nodes)) {
AERROR << "Failed to reconstruct route.";
return false;
}
return true;
}
open_set_.erase(from_node);
open_set_detail.pop();
if (closed_set_.count(from_node) != 0) { continue; }// if showed before, just skip...
closed_set_.emplace(from_node);
// if residual_s is less than FLAGS_min_length_for_lane_change, only move forward
const auto& neighbor_edges = (GetResidualS(from_node) > FLAGS_min_length_for_lane_change && change_lane_enabled_) ? from_node->OutToAllEdge() : from_node->OutToSucEdge();
double tentative_g_score = 0.0;
next_edge_set.clear();
for (const auto* edge : neighbor_edges) {
sub_edge_set.clear();
sub_graph->GetSubInEdgesIntoSubGraph(edge, &sub_edge_set);
next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end());
}
for (const auto* edge : next_edge_set) {
const auto* to_node = edge->ToNode();
if (closed_set_.count(to_node) == 1) { continue; }
if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) { continue; }
tentative_g_score = g_score_[current_node.topo_node] + GetCostToNeighbor(edge);
if (edge->Type() != TopoEdgeType::TET_FORWARD) { tentative_g_score -= (edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2; }
double f = tentative_g_score + HeuristicCost(to_node, dest_node);
if (open_set_.count(to_node) != 0 && f >= g_score_[to_node]) { continue; }
g_score_[to_node] = f;
SearchNode next_node(to_node);
next_node.f = f;
open_set_detail.push(next_node);
came_from_[to_node] = from_node;
if (open_set_.count(to_node) == 0) {open_set_.insert(to_node);}
}
}
首先用一種叫“優先隊列”的數據結構實現了一個open列表：open_set_detail：
std::priority_queue<SearchNode> open_set_detail;
然後在頭文件裏又有一個open列表（和closed列表），但使用的卻是unordered_set無序集合容器。
std::unordered_set<const TopoNode*> open_set_;
std::unordered_set<const TopoNode*> closed_set_;
爲什麼要定義兩個open列表呢？
在僞代碼中只使用了一個open列表。但是在編程實現時採用了兩個：open_set_detail常用於取代價最小（優先級最高）的節點，因爲對於優先隊列這種數據結構，取數據非常快，是O(1)複雜度，但是它查詢就不是最快的。而open_set_主要用來查詢，查詢裏面有沒有某個節點。unordered_set的優點是能夠使用元素值快速訪問。從功能上看其實用一個open列表就夠了，用兩個是爲了提高效率。
從元素的角度來說，open_set_detail列表存儲的是SearchNode類型的元素，SearchNode類型是一種複合的元素，主要包括TopoNode節點和該節點的f函數值，open_set_列表存儲的只是TopoNode元素。
搜索完了，如何抽取出路徑來呢？Reconstruct函數通過回溯得到一系列的節點組成的路徑。回溯用了came_from_這個容器，它存儲着每個節點的父節點。
啓發函數用的是曼哈頓距離，注意不是歐式距離。這在城市網格地圖中比較合理，深究原因涉及到背後的度量的概念。
double distance = fabs(src_point.x() - dest_point.x()) + fabs(src_point.y() - dest_point.y());
有些地方似乎有問題：
剛pop出來的當前節點就判斷是否在closed_set_中似乎沒有必要。
代價函數g與標準算法不一樣，爲什麼要讓g_score_[to_node] = f呢？這相當於加了兩次h啓發函數，好像不對。而且怎麼讓f與g值去比較呢（f >= g_score_[to_node]），這有什麼意義？在較早的Apollo1.5版中還是g_score_[to_node] = tentative_g_score;，後面的版本就改了，不懂。
3 開放場地軌跡規劃中的A*算法
再看開放場地軌跡規劃模塊open_space中的實現，在Plan函數中，也定義了一個open_pq_優先隊列：
std::priority_queue<std::pair<std::string, double>, std::vector<std::pair<std::string, double>>, cmp> open_pq_;
然後又有一個open列表（以及closed列表，還寫成了close，估計這兩個程序不是一個人寫的）：
std::unordered_map<std::string, std::shared_ptr<Node3d>> open_set_;
std::unordered_map<std::string, std::shared_ptr<Node3d>> close_set_;
注意這裏是unordered_map，而前面的是unordered_set。open_set_列表存儲的是Node3d和相應的index。
hybrid_a_star.cc 中回溯路徑的函數是GetResult，沒單獨定義came_from_，而是直接在每個節點中存儲了父節點的指針。
hybrid_a_star.cc比標準的A*算法多了個AnalyticExpansion(current_node)檢測，也就是檢測當前節點是不是能直接用Reeds-Shepp曲線連接目標並且沒有與障礙物碰撞，如果可以就不搜索了，這是一種偷懶的技巧。其它的部分基本一樣，不用解釋就能看懂。
while (!open_pq_.empty()) {
const std::string current_id = open_pq_.top().first;// take out the lowest cost neighboring node
open_pq_.pop();
std::shared_ptr<Node3d> current_node = open_set_[current_id];
// check if an analystic curve could be connected from current configuration to the end configuration without collision. if so, search ends.
if (AnalyticExpansion(current_node)) { break; }
close_set_.insert(std::make_pair(current_node->GetIndex(), current_node));
for (size_t i = 0; i < next_node_num_; ++i) {
std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);
if (next_node == nullptr) { continue; }// boundary check failure handle
if (close_set_.find(next_node->GetIndex()) != close_set_.end()) { continue; }// check if the node is already in the close set
if (!ValidityCheck(next_node)) { continue; }// 碰撞檢測
if (open_set_.find(next_node->GetIndex()) == open_set_.end()) {
CalculateNodeCost(current_node, next_node);
open_set_.emplace(next_node->GetIndex(), next_node);
open_pq_.emplace(next_node->GetIndex(), next_node->GetCost());
}
}
}
Apollo在open_space軌跡規劃模塊中採用的是OBCA算法，但是代碼中似乎有一些錯誤，例如遺漏了一個地方（在OBCA原程序hybrid_a_star.jl中），每次pop出棧頂元素後沒有把open列表中的值刪除掉：delete!(open, c_id)。對鄰居節點只判斷了一種情況，即鄰居節點不在open列表裏就添加進去，但是如果已經在open列表裏了怎麼辦呢，程序沒有考慮這種情況。當然 open_space模塊還在測試中，有問題也正常。
OBCA原程序hybrid_a_star.jl中回溯路徑用的方式藉助closed，每個節點也存儲了父節點（的索引），再根據索引從closed裏找節點。
看這些代碼非常累，因爲同一個算法不同程序員實現的方式真是五花八門。