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

算法过程

算法过程

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

百度路径寻路算法的不足

百度路径寻路算法的不足：

1. 使用了通用的a*算法，没有使用效果更好的其他寻路算法
2. 只考虑了本车的情况，没有考虑和周围其他行人车辆的协同
3. 没有考虑碰撞检测和避障算法的实现，避障算法这个其实是重中之重

参考

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