算法分析
车辆路径规划寻路算法有很多，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<const TopoNode*, const TopoNode*>& came_from,
const TopoNode* dest_node, std::vector<NodeWithRange>* 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<NodeWithRange>* const result_nodes)
其中open_set_detail就是open表，使用的是priority_queue优先级队列，push加入一个元素，pop删除第一个元素。
这是一个非常标准的A*寻路算法实现。
百度路径寻路算法的不足
百度路径寻路算法的不足：
- 使用了通用的a*算法，没有使用效果更好的其他寻路算法
- 只考虑了本车的情况，没有考虑和周围其他行人车辆的协同
- 没有考虑碰撞检测和避障算法的实现，避障算法这个其实是重中之重
