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;
}
来源：CSDN
作者：旅途者0107
链接：https://blog.csdn.net/lzw0107/article/details/104044250