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

百度无人驾驶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 没有考虑碰撞检测和避障算法的实现，避障算法这个其实是重中之重