Routing模塊之A*搜索算法
Routing模塊的結果是Planning模塊生成參考線的基礎，也就是通常所說的全局路徑規劃，Routing模塊中除了對地圖的抽象處理爲可供搜索的topo地圖，以及通過KDtree生成黑名單地圖外核心算法是通過A* 算法生成全局路徑，本文這裏主要介紹Routing模塊中如何同過A*算法在topo地圖中搜索全局通路——全局路徑。
1.邏輯框架
Routing模塊的功能是從地圖中搜索一條從起點–途經點–終點的全局路徑，要完成這個功能，需要解決幾個問題：1) 地圖信息較多，看起來是連續的，如何進行抽象爲可搜索的離散地圖; 2) A*算法在抽象的地圖中是如何搜索的; 3) 搜索的結果是如何使用的;
這裏我們首先了解一下routing模塊大概的邏輯：
通過上圖可知routing實際上先將高精度地圖轉換爲topo地圖，topo地圖是由一系列的節點和邊組成，在生成節點和邊的同時會對每個節點和邊賦予一定的cost，通常cost與車道的長度、轉彎等屬性有關。然後通過A*算法對topo圖進行搜索，最後對搜索的結果進行封裝。
2.Topo地圖
在高精度地圖中採集的map信息包含了road，信號燈，車道線，物體，信號標識等很多信息，在路徑搜索的過程中不能直接使用，而且主要關注的是lane及lane之間的關係，因此，routing模塊主要是先將高精度地圖中採集的base_map地圖信息通過topo_creator程序創建節點和邊進而轉化爲Topo地圖，其結果如下，在map模塊中定義的參數有很多，在使用過程中需要用到以下參數，然後通過A算法來搜索一條可行的路徑：
在"routing/topo_creator"建圖的代碼中主要包括，構建節點，構建邊，構建圖，其文件結構如下圖所示：
對於topo圖中的節點和圖的詳細描述是在routing/graph目錄下，其主要的信息如下圖 Topo地圖的詳細結構：
在構建topo圖之後通過A算法搜尋路徑，A*算法是基於圖搜索的一種方式，該模塊的輸入爲構建的圖以及子圖、起始點和目標點，輸出爲路由導航信息，對於Topo地圖構建的信息可以這樣理解，對於下圖的道路信息將其抽象爲下圖的節點和邊的信息：
從圖中可以瞭解到一個road，包含了多個車道信息，每個車道是一個搜索的節點，該節點也是有一定長度的，前後車道之間通過predecessor_id和successor_id進行連接，左右車道之間是通過車道線的類型進行連接的。
3.A*算法的流程
1)把起始格添加到開啓列表（Open List）。
2)重複如下工作：
a)尋找開啓列表中估量代價F值最低的格子，稱之爲當前格；
b)把它切換到關閉列表中（Closed List）；
c)對相鄰的八個格中的每一個進行如下操作：
i.如果它不可通過或者已經在關閉列表中，略過它；反之如下；
ii.如果它不在開啓列表中，把它添加進去。把當前格作爲這一格的父節點。記錄這一格的F，G和H值；
iii.如果它已經在開啓列表中，用G值作爲參考檢查新的路徑是否更好。更低的G值意味着更好的路徑。如果是這 樣，就把這一格的父節點改成當前格，並且重新計算這一格的G和F值，並在改變後重新按照F值對開啓列表排序。
d)當目標格被添加進了關閉列表中，這時候路徑被找到；或者沒有找到目標格，開啓列表已經空了，這時候路徑不存在。
3)從目標格開始，沿着每一格的父節點移動直到回到起始格，保存路徑。
其過程如下圖過程解釋：
目標：給定一個topo地圖，綠色爲起點位置，紅色爲目標位置，A*算法的目的是搜索一條從起始位置到目標位置的路徑點序列。
步驟：1將起始當前節點爲起始點，將起始點的鄰接點加入到open_list中，然後計算其鄰接點的cost f值，然後將最小的f值選出來，並把最小的節點從open_list中移除，然後加入到close_list中，並把它的父節點設置爲起始節點；
步驟2：將上次的節點設置爲當前節點，如果當前節點的鄰接點不在open_list中，那麼將其加入到其中，進行計算cost f，再次找到代價值最小的點，並把最小的節點從open_list中移除，然後加入到close_list中，並把它的父節點設置爲起始節點；
步驟3，依此進行，直到到達目標點，然後根據父節點來依次回溯路徑到起始點，或者待計算的open_list集合中都計算完了，還沒有到目標點，表示路徑搜索失敗。
4.A*算法在routing中的應用
Apollo代碼中的A*算法的實現過程主要是在a_star_strategy.cc中的search()方法：
代價函數爲F=h+g ，其中g表示當前節點到起始節點的代價，主要是通過計算節點以及連接的邊的代價值，h爲通過曼哈頓距離表示的啓發式函數：
double distance = fabs(src_point.x() - dest_point.x()) + fabs(src_point.y() - dest_point.y());
其代碼解釋如下：
/參數分別爲所有圖節點，部分圖節點，起始節點，目標節點，結果集
bool AStarStrategy::Search(const TopoGraph* graph,
const SubTopoGraph* sub_graph,
const TopoNode* src_node, const TopoNode* dest_node,
std::vector<NodeWithRange>* const result_nodes) {
//清楚所有的參數
Clear();
//記錄日誌信息
AINFO << "Start A* search algorithm.";
//std：：priority_queue是一種容器適配器，這裏再最初重載了<操作符，所以優先輸出最小的
//作爲已經尋路過的節點的代價值
std::priority_queue<SearchNode> open_set_detail;
//把源節點node傳入struct中，作爲源SearchNode
SearchNode src_search_node(src_node);
//計算源節點的代價值f，啓發式距離採用曼哈頓距離
src_search_node.f = HeuristicCost(src_node, dest_node);
// 源SearchNode 傳入push進尋路節點集的優先隊列中
open_set_detail.push(src_search_node);
// 源節點也傳入open集中
open_set_.insert(src_node);
// g函數評分，key-value，計算源節點到自身的移動代價值
g_score_[src_node] = 0.0;
// 設置源節點進入時的S值，key-value
enter_s_[src_node] = src_node->StartS();
// 定義SearchNode變量
SearchNode current_node;
//定義next邊集
std::unordered_set<const TopoEdge*> next_edge_set;
//定義sub邊集
std::unordered_set<const TopoEdge*> sub_edge_set;
//當open集合不爲空時，不斷循環檢查
while (!open_set_detail.empty()) {
// 此時節點賦值爲open優先級中棧頂的元素，權重用f來比較
current_node = open_set_detail.top();
// 保存當前SearchNode的起始節點
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集node節點刪除
open_set_detail.pop();// open集優先隊列中將SearchNode節點刪除
// 若起始結點from_node在CLOSED集中的計數不爲0，表明之前已被檢查過，直接跳過
if (closed_set_.count(from_node) != 0) {
// if showed before, just skip...
continue;
}
// 將起始結點加入close集中
closed_set_.emplace(from_node);
// if residual_s is less than FLAGS_min_length_for_lane_change, only move
// forward
// 獲取起始結點from_node的所有相鄰邊
// 若起始結點from_node到終點的剩餘距離s比FLAGS_min_length_for_lane_change要短，
// 則不考慮變換車道，即只考慮前方結點而不考慮左右結點。反之，若s比
// FLAGS_min_length_for_lane_change要長，則考慮前方及左右結點。
const auto& neighbor_edges =
(GetResidualS(from_node) > FLAGS_min_length_for_lane_change &&
change_lane_enabled_)
? from_node->OutToAllEdge()
: from_node->OutToSucEdge();
// 當前測試的移動代價g評分，初始化指定0
double tentative_g_score = 0.0;
// 從相鄰邊neighbor_edges中獲取其內部包含的邊，將所有相鄰邊全部加入集合：next_edge_set
next_edge_set.clear();
for (const auto* edge : neighbor_edges) {// 對於所有的臨接邊
sub_edge_set.clear();// 子臨接邊清空
sub_graph->GetSubInEdgesIntoSubGraph(edge, &sub_edge_set);// sub_edge_set賦值爲edge
next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end());// 把sub_edge_set匯入next_edge_set
}
// 所有相鄰邊的目標結點就是我們需要逐一測試的相鄰結點，對相結點點逐一測試，尋找
// 總代價f = g + h最小的結點，該結點就是起始結點所需的相鄰目標結點。
for (const auto* edge : next_edge_set) {// 循環next_edge_set
const auto* to_node = edge->ToNode();
// 相鄰結點to_node在CLOSED集中，表明之前已被檢查過，直接忽略。
if (closed_set_.count(to_node) == 1) {
continue;
}
// 若當前邊到相鄰結點to_node的距離小於FLAGS_min_length_for_lane_change，表明不能
// 通過變換車道的方式從當前邊切換到相鄰結點to_node，直接忽略。
if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) { // 如果邊到到達節點node的距離小於限定值，再次循環
continue;
}
// 更新當前結點的移動代價值g
tentative_g_score =
g_score_[current_node.topo_node] + GetCostToNeighbor(edge);
// 如果邊類型不是前向，而是左向或右向，表示變換車道的情形，則更改移動代價值g
// 的計算方式。
if (edge->Type() != TopoEdgeType::TET_FORWARD) {
tentative_g_score -=
(edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2;
}
// 若相鄰結點to_node在OPEN集且當前總代價f大於源結點到相鄰結點to_node的移動代價g，表明現有情形下
// 從當前結點到相鄰結點to_node的路徑不是最優，直接忽略。
// 因爲相鄰結點to_node在OPEN集中，後續還會對該結點進行考察。
if (open_set_.count(to_node) != 0 &&
tentative_g_score >= g_score_[to_node]) {
continue;
}
// if to_node is reached by forward, reset enter_s to start_s
// 如果是以向前（而非向左或向右）的方式抵達相鄰結點to_node，則將to_node的進入距離更新爲
// to_node的起始距離。
if (edge->Type() == TopoEdgeType::TET_FORWARD) {
enter_s_[to_node] = to_node->StartS();
} else {
// else, add enter_s with FLAGS_min_length_for_lane_change
// 若是以向左或向右方式抵達相鄰結點to_node，則將to_node的進入距離更新爲
// 當前結點from_node的進入距離加上最小換道長度，並乘以相鄰結點to_node長度
// 與當前結點from_node長度的比值（這麼做的目的是爲了歸一化，以便最終的代價量綱一致）。
double to_node_enter_s =
(enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
from_node->Length() * to_node->Length();
// enter s could be larger than end_s but should be less than length
to_node_enter_s = std::min(to_node_enter_s, to_node->Length());
// if enter_s is larger than end_s and to_node is dest_node
if (to_node_enter_s > to_node->EndS() && to_node == dest_node) { // 如果滿足enter_s比end_s大而且下一個節點是終點，就再次循環
continue;
}
enter_s_[to_node] = to_node_enter_s;
}
// 更新從源點移動到結點to_node的移動代價（因爲找到了一條代價更小的路徑，必須更新它）
g_score_[to_node] = tentative_g_score;// to_node的g評分重新賦值
// 將相鄰結點to_node設置爲下一個待考察結點
SearchNode next_node(to_node);
next_node.f = tentative_g_score + HeuristicCost(to_node, dest_node);// next_node的f值爲g評分加上啓發式距離（曼哈頓）
// 當下一個待考察結點next_node加入到OPEN優先級隊列
open_set_detail.push(next_node);
// 將to_node的父結點設置爲from_node
came_from_[to_node] = from_node;
// 若相鄰結點不在OPEN集中，則將其加入OPEN集，以便後續考察
if (open_set_.count(to_node) == 0) {// 如果開集中發現to_node沒有出現過，就添加該節點
open_set_.insert(to_node);
}
}
}
// 整個循環結束後仍未正確返回，表明搜索失敗
AERROR << "Failed to find goal lane with id: " << dest_node->LaneId();
return false;
}