百度无人驾驶apollo项目路径规划a*算法分析百度无人驾驶apollo项目路径规

算法分析 【百度无人驾驶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表，以待考察，则称这个节点为已考察的。
算法过程算法过程

令s为起始节点
计算s的f,g和h值
将s加入open表，此时s是open表里唯一的节点
令b＝open表中的最佳节点（最佳的意思是该节点的f值最小）
- 如果b是目标节点，则退出，此时已找到一条路径
- 如果open表为空，则退出，此时没有找到路径
令c等于一个与b相连的有效节点
- 计算c的f,g,h值
- 检查c是在open表里还是在closed表里，若在closed 表中，则检查新路径是否比原先更好（f值更小），若是则采用新路径，否则把c添加入open表
- 对所有b的有效子孙节点重复第5步
重复第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& came_from, const TopoNode* dest_node, std::vector* 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* const result_nodes)

其中open_set_detail就是open表，使用的是priority_queue优先级队列，push加入一个元素，pop删除第一个元素。
这是一个非常标准的A*寻路算法实现。
百度路径寻路算法的不足百度路径寻路算法的不足：