• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

百度Apollo中的A*算法实现笔记

人工智能 robinvista 1177次浏览 0个评论

  本文主要讲百度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里找节点。

  看这些代码非常累,因为同一个算法不同程序员实现的方式真是五花八门。



开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明百度Apollo中的A*算法实现笔记
喜欢 (0)

您必须 登录 才能发表评论!

加载中……