Apollo3.5:基于拓扑地图TopoGraph的Routing模块A*算法路径导航

最新推荐文章于 2023-02-13 23:20:28 发布

yu_明明明皓

最新推荐文章于 2023-02-13 23:20:28 发布

阅读量2.6k

点赞数 2

分类专栏： Apollo

本文链接：https://blog.csdn.net/weixin_44809980/article/details/107929643

版权

Apollo 专栏收录该内容

9 篇文章 1 订阅

订阅专栏

概述

本文参考
https://blog.csdn.net/davidhopper/article/details/87438774 https://www.cheung.site/2020/04/07/apollo-routing/
在其基础上结合自己理解进行整理和修改

Routing模块给自动驾驶系统提供全局的路径导航功能，类似于百度地图/高德地图的导航。在收到路由请求之后，为了完成导航功能，Routing模块使用了专用的路由地图routing_map。最终输出的是自动驾驶车辆在从出发点到目的地的过程中经过的所有路段。

导航器Navigator

Navigator时路由搜索的实施类,完成的具体的路径搜索.
Navigator对象在构造时,会从routing_map加载routing图,然后对原始的routing地图信息进行一些加工和整合,形成TopoGraph.

拓扑图 TopoGrapph

Routing模块并不直接使用Graph进行路由寻径，而使用了细节信息更丰富的TopoGraph
TopoGraph的全部信息都来源于Graph，但由于对原始的节点(Node)和连接边(Edge)信息进行了加工和整合，提供了许多额外的数据结构来方便后续的查找和搜索。
通过TopoGraph，你可以查找一个Lane ID对应的TopoNode,然后找到TopoNode的相邻边和 node.举个例子，在TopoGraph中TopoNode A(对应Lane A)有一条类型为LEFT的OutEdge连接到TopoNodeB(对应Lane B)，表示在地图中Lane A的左边紧邻的就是Lane B。

Routing搜索流程

获取waypoints所在的Lane对应的TopoNode
添加黑名单
依次搜索前后两个航点之间的路径，最后把这些路径拼接起来
将路径搜索的结果输出为RoutingResponse格式

拓扑子图SubTopoGraph

进行拓扑图搜索时选择那些能通行的路段，TopoGraph里面的TopoNode可能是带有黑名单LaneSegment的，不能直接使用了。所以需要重新构建地图.一种方法是过滤掉黑名单路段,获取可通行的LaneSegment,重新考察TopoNode之间的TopoEdge，进而重建整个TopoGraph.这个过程需要在每两个航点之间都进行重建一次,所以需要重建N-1次,非常耗时.
所以Apollo中引入拓扑子图SubTopoGraph来解决问题.只关注黑名单路段对应额TopoNode以及搜索起点和搜索终点处的TopoNode.
创建拓扑子图SubTopoGraph时,不需要修改TopoGraph,只需要在拓扑子图中添加从子TopoNode连接到TopoGraph到TopoNode的连接信息.
创建拓扑子图的TopoNode时,记录了TopoNode表示的路段的区间范围,还会找到路段的中心点,并计算该TopoNode的可变道区间列表.
创建拓扑子图的TopoEdge时,判断周围TopoNode是否可以与当前TopoNode相连.

A*搜索

A*搜索头文件 a_srtar_stragegy.h

namespace apollo {
namespace routing {
class AStarStrategy : public Strategy {
 public:
  explicit AStarStrategy(bool enable_change);
  ~AStarStrategy() = default;
  //A*搜索函数主体,参数为TopoGraph,SubTopoGraph,起点,终点,结果容器
  virtual bool Search(const TopoGraph* graph, const SubTopoGraph* sub_graph,
                      const TopoNode* src_node, const TopoNode* dest_node,
                      std::vector<NodeWithRange>* const result_nodes);

 private:
 //Clear清空上次结果
  void Clear();
  //启发函数计算
  double HeuristicCost(const TopoNode* src_node, const TopoNode* dest_node);
  //计算节点node到终点的剩余距离s
  double GetResidualS(const TopoNode* node);
  //计算边edge到节点to_node的剩余距离s
  double GetResidualS(const TopoEdge* edge, const TopoNode* to_node);

 private:
 //是否可以变换车道
  bool change_lane_enabled_;
  //open_set,closes-set,都用于在算法中快速查询判断里面有没有这个节点.
  std::unordered_set<const TopoNode*> open_set_;
  std::unordered_set<const TopoNode*> closed_set_;
  //存储着每个node的父节点
  std::unordered_map<const TopoNode*, const TopoNode*> came_from_;
  //存储这每个node:从开始点到当前点的代价
  std::unordered_map<const TopoNode*, double> g_score_;
  //每个节点的进入距离
  std::unordered_map<const TopoNode*, double> enter_s_;
};
}  // namespace routing
}  // namespace apollo

A*算法周围邻居节点

struct SearchNode {
  const TopoNode* topo_node = nullptr;
  double f = std::numeric_limits<double>::max();

  SearchNode() = default;
  explicit SearchNode(const TopoNode* node)
      : topo_node(node), f(std::numeric_limits<double>::max()) {}
  SearchNode(const SearchNode& search_node) = default;
  //重载<运算符,改变小于逻辑,以便作为优先级队列std::priority_queue的内部元素
  //std::priority_queuede的栈顶元素永远为最小的一个.
  bool operator<(const SearchNode& node) const {
    // in order to let the top of priority queue is the smallest one!
    return f > node.f;
  }

  bool operator==(const SearchNode& node) const {
    return topo_node == node.topo_node;
  }
};

启发代价函数计算
采用曼哈顿距离

double AStarStrategy::HeuristicCost(const TopoNode* src_node,
                                    const TopoNode* dest_node) {
  //AnchorPoint()为路段的中心点,用来计算代价
  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;
}

A*算法主体

bool AStarStrategy::Search(const TopoGraph* graph,
                           const SubTopoGraph* sub_graph,
                           const TopoNode* src_node, const TopoNode* dest_node,
                           std::vector<NodeWithRange>* const result_nodes) {
  Clear();
  AINFO << "Start A* search algorithm.";
  //优先级队列openlist,用priority_queue实现.std::priority_queue是一种容器适配器，
  //它提供常数时间的最大元素查找功能，亦即其栈顶元素top永远输出队列中的最大元素。
  //但SearchNode内部重载了<运算符，对小于操作作了相反的定义
  //因此std::priority_queue<SearchNode>的栈顶元素永远输出队列中的最小元素。
  std::priority_queue<SearchNode> open_set_detail;
  //将起点设置为检查节点
  SearchNode src_search_node(src_node);
  //起点的f值
  src_search_node.f = HeuristicCost(src_node, dest_node);
  //把起点节点存入openli
  open_set_detail.push(src_search_node);
  //把起点加入到open_set
  open_set_.insert(src_node);
  g_score_[src_node] = 0.0;
  enter_s_[src_node] = src_node->StartS();

  SearchNode current_node;
  std::unordered_set<const TopoEdge*> next_edge_set;
  std::unordered_set<const TopoEdge*> sub_edge_set;
  //进入算法主循环,只要openlist不为空,就一直循环查找
  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) {
      // if showed before, just skip...
      continue;
    }
    //当前点加入openlist
    closed_set_.emplace(from_node);

    // if residual_s is less than FLAGS_min_length_for_lane_change, only move
    // forward
    //先通过判断用不用变道,再获取当前节点的所有相邻边
    //GetResidualS 为当前节点到终点的剩余距离s
    //若s<min_length则不变道,若s > min_length 则变道
    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();
    //从上面相邻边neighbor_edges中获取其内部包含的边,将所有相邻边全部加入集合:next_edge_set
    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();
      //判断是不是已经在closelist中
      if (closed_set_.count(to_node) == 1) {
        continue;
      }
      //若当前边到相邻节点的距离小于min_length,则不能通过变道到达相邻节点,就直接忽略
      if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) {
        continue;
      }
      //更新当前节点的移动代价g
      tentative_g_score =
          g_score_[current_node.topo_node] + GetCostToNeighbor(edge);
      //如果边的类型不是向前而是向左向右,表示变道,需要更新移动代价g的计算方式
      if (edge->Type() != TopoEdgeType::TET_FORWARD) {
        tentative_g_score -=
            (edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2;
      }
      //总代价f = g + h
      double f = tentative_g_score + HeuristicCost(to_node, dest_node);
      //判断当前节点到相邻节点的代价是不是比原来的代价小,如果小则更新代价,否则忽略
      if (open_set_.count(to_node) != 0 &&
          f >= g_score_[to_node]) {
        continue;
      }
      // if to_node is reached by forward, reset enter_s to start_s
      //如果是以向前的方向到达相邻节点,则更新节点的进入距离enter_s
      if (edge->Type() == TopoEdgeType::TET_FORWARD) {
        enter_s_[to_node] = to_node->StartS();
      } else {
        // else, add enter_s with FLAGS_min_length_for_lane_change
        //若是以向左或者向右的方式到达相邻界定啊,则将相邻及诶单的进入距离更新为与当前节点长度的比值
        double to_node_enter_s =
            (enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
            from_node->Length() * to_node->Length();
        // enter s could be larger than end_s but should be less than length
        to_node_enter_s = std::min(to_node_enter_s, to_node->Length());
        // if enter_s is larger than end_s and to_node is dest_node
        if (to_node_enter_s > to_node->EndS() && to_node == dest_node) {
          continue;
        }
        enter_s_[to_node] = to_node_enter_s;
      }
	 //更新从起始点移动到当前节点的移动代价
      g_score_[to_node] = f;
      //将代价最小的相邻节点设置为下一个检查点
      SearchNode next_node(to_node);
      next_node.f = f;
      //把下一个待检查节点加入到openlist
      open_set_detail.push(next_node);
      //设置当前节点的父节点
      came_from_[to_node] = from_node;
      //若相邻及节点不在openlist中,则将其加入到open_set中,便于后续考察
      if (open_set_.count(to_node) == 0) {
        open_set_.insert(to_node);
      }
    }
  }
  //整个循环结束后仍未正确返回,则表明搜索失败
  AERROR << "Failed to find goal lane with id: " << dest_node->LaneId();
  return false;
}

yu_明明明皓

关注

2
点赞
踩
26

收藏

觉得还不错? 一键收藏
0
评论
Apollo3.5:基于拓扑地图TopoGraph的Routing模块A*算法路径导航

概述本文参考https://blog.csdn.net/davidhopper/article/details/87438774 在其基础上结合自己理解进行修改Routing模块给自动驾驶系统提供全局的路径导航功能，类似于百度地图/高德地图的导航。在收到路由请求之后，为了完成导航功能，Routing模块使用了专用的路由地图routing_map。最终输出的是自动驾驶车辆在从出发点到目的地的过程中经过的所有路段。导航器NavigatorNavigator时路由搜索的实施类,完成的具体的路径搜索.
复制链接

扫一扫