本文主要讲百度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），都判断一下它是不是目标节点，如果是那就说明搜索到终点了，根据A星算法的完备性，到终点就意味着找到最短路径了，后面的语句就不再执行了。
④ 从open中删除最小代价的节点 如果当前节点不是目标节点，那就检查它的邻居们。但是要先从open列表中删除当前节点。为啥要先删除呢？这就涉及算法的理论基础了，比较麻烦。大概意思是，节点的代价都是从小的传导到大的，如果每次取代价最小的节点，后面的节点代价总是大于当前节点（边的代价都是大于零的），不会存在一条更小的路径再回来通过当前节点，所以删除是一次性的，永远不用再添加回来；如果open列表有两个，就每个都要删除一次。如果用了closed列表，每次从open列表中删除当前节点后总会把它加入到closed列表中，表示后面不再拜访它了，一个节点不可能既在open列表里又在closed列表里。
⑤ 检查邻居并计算代价 遍历当前节点的所有邻居节点（遍历的顺序无所谓），并且计算邻居节点的代价。邻居节点可能在open列表里，也可能不在里面。如果在里面，而且这个刚计算出来的代价比已经有的代价小，就用更小的替换了，同时更新他的父节点，也就是说此时找到一条到达这个节点更短的路径了。如果不在里面，就加入到open列表里；上面的伪代码没使用close列表，但Apollo程序中都用了close列表，这样效率高一点（不走回头路），如果邻居节点在close列表中，则直接跳过不处理。
⑥ 回到①，循环；
A*算法的核心概念就是这个open列表了（或者叫集合，都无所谓，因为我们现在只关心它里面有啥，不关心里面元素是按什么顺序排列的），里面存储着已经被发现（但还有待进一步扩展的）节点。伪代码里的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的优点是能够直接使用元素值快速查询（用hash实现的）。从功能上看其实用一个open列表就可以实现，用两个是为了提高效率。从元素的角度来说，open_set_detail列表存储的是SearchNode类型的元素，SearchNode类型是一种复合的元素，主要包括TopoNode节点和该节点的f函数值，用于基于值的存取；open_set_列表中存储的只是TopoNode元素，因此它只用于查询。
A*搜索结束后，如何抽取出路径来呢？这是用Reconstruct函数实现的，它从目标节点开始往回倒推得到一系列的节点组成的路径。回溯用了came_from_这个容器，它存储着每个节点的父节点。每个父节点有不只一个子节点，而每个子节点只有一个父节点，所以这就是一颗树。对于一颗树来说，知道了子节点，沿着的它的父节点、爷爷节点……往回找总能到树根（就是起始节点）。
路由规划使用的启发函数是曼哈顿距离，而不是欧式距离。这在城市网格地图中比较合理，深究原因涉及到背后的度量的概念。Dijkstra和A *都只能搜索离散图上的最短距离路径，不能搜索实际的真正欧式最短距离路径。
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里找节点。
看这些代码非常累，因为同一个算法不同程序员实现的方式真是五花八门。
