routing模块(4)-全局路线的生成

现在子图和原始拓扑图已经进行关联,相当于把子图通过edge结构关联到了原始的拓扑图中

获取起点/终点节点

const auto* start = sub_graph.GetSubNodeWithS(way_start, way_start_s);
const auto* end = sub_graph.GetSubNodeWithS(way_end, way_end_s);

这样就可以在完整的拓扑图中使用astar算法计算全局路线

Astar算法

Search函数

strategy_ptr->Search(graph, &sub_graph, start, end,
                              &cur_result_nodes)

graph:拓扑图,使用routing_map.bin文件生成

sub_graph:在上一篇routing模块(3)-全局路线的生成中构建的子图

start:算路的起点节点

end:算路的终点节点

cur_result_nodes:输出的算路结果,类型std::vector<NodeWithRange>,包含节点与在节点上(也就是车道上)可行驶的区间

A*算法用f来在open集中排序,优先搜索f小的节点,f(n) = g(n) + h(h)

g(n):从起点(start)沿着当前已选路径走到节点n的代价

h(n):从节点n到终点的启发式估计代价(一般使用曼哈顿距离)

初始时:g(start)(你在起点还没走路)=0

g_score_[src_node] = 0.0;               // g(start)=0
src_search_node.f = HeuristicCost(src_node, dest_node); // f = h
open_set_detail.push(src_search_node);

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 = std::fabs(src_point.x() - dest_point.x()) +
                    std::fabs(src_point.y() - dest_point.y());
  return distance;
}

FindAnchorPoint函数

这里需要介绍一下AnchorPoint(),它是在TopoNode::Init()函数中调用FindAnchorPoint函数时通过SetAnchorPoint函数设置的

bool TopoNode::FindAnchorPoint() {
  double total_size = 0;
  for (const auto& seg : CentralCurve().segment()) {
    total_size += seg.line_segment().point_size();
  }
  double rate = (StartS() + EndS()) / 2.0 / Length();
  int anchor_index = static_cast<int>(total_size * rate);
  for (const auto& seg : CentralCurve().segment()) {
    if (anchor_index < seg.line_segment().point_size()) {
      SetAnchorPoint(seg.line_segment().point(anchor_index));
      return true;
    }
    anchor_index -= seg.line_segment().point_size();
  }
  return false;
}

下面逻辑是在统计中心曲线所有点的数量

double total_size = 0;
for (const auto& seg : CentralCurve().segment()) {
  total_size += seg.line_segment().point_size();
}

可以看一下routing_map.txt文件中node的结构,你能看到node结构里面的central_curve/segment/line_segement/point这些可以跟代码对上.CentralCurve().segment()有很多个segment,每个segment有很多点这里把所有segment中的点数相加.后续会根据比例rate去选某个点为锚点,这个是过程可以理解为采样过程

编辑

double rate = (StartS() + EndS()) / 2.0 / Length();

(StartS() + EndS()) / 2.0这个是node中点的在这条车道上的s值

除以node长度得到到node可行驶区间中点在整个node长度上的比例

int anchor_index = static_cast<int>(total_size * rate);

按照rate比例乘以这个node总的point数,得到一个索引anchor_index

如果anchor_index < seg.line_segment().point_size()说明目标点就在这个segment里, 否则减去当前segment的点数,继续在下一个segment中找目标点,直到找到位置,如果找到了设置AnchorPoint,这个AnchorPoint就代表了这个整体node

void TopoNode::SetAnchorPoint(const common::PointENU& anchor_point) {
  anchor_point_ = anchor_point;
}

总结一下FindAnchorPoint

1.数一数整条中央曲线有多少个点

2.找出这个 TopoNode 的中点在整个曲线上的比例位置

3.按比例计算出该点对应的 index

4.在 CentralCurve 的多个 segment 中找到这个点

5.把该点设置为 AnchorPoint

再回到HeuristicCost函数

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 = std::fabs(src_point.x() - dest_point.x()) +
                    std::fabs(src_point.y() - dest_point.y());
  return distance;
}

我们已经知道,每个节点会用我们设置的AnchorPoint来表示,所以在HeuristicCost函数中,我们计算h(n)的值的时候,计算的是从节点n到终点的曼哈顿距离,并且使用的是AnchorPoint坐标.

  std::priority_queue<SearchNode> open_set_detail;

  SearchNode src_search_node(src_node);
  src_search_node.f = HeuristicCost(src_node, dest_node);
  open_set_detail.push(src_search_node);

  open_set_.insert(src_node);
  g_score_[src_node] = 0.0;
  enter_s_[src_node] = src_node->StartS();

这样起点的h就是起点锚点到终点锚点的曼哈顿距离,因为起点的g值为0所以f = h

open_set_detail.push(src_search_node):用于选取最小f值节点的优先队列,队顶为最小,首先存入起点

g_score_[src_node] = 0.0:将起点的g值设为0(表示从起点到自身的已知代价)

enter_s_[src_node] = src_node->StartS():

正常直行：enter_s = StartS（从节点起点进入）
变道进入：enter_s = 一个比 StartS 更靠后的位置

SearchNode current_node;
std::unordered_set<const TopoEdge*> next_edge_set;
std::unordered_set<const TopoEdge*> sub_edge_set;
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();

current_node = open_set_detail.top():取优先队列中f最小的节点,记为current_node

from_node:指向当前搜索的拓扑节点

current_node.topo_node == dest_node:表示找到了到目标的路径,然后调用Reconstruct将came_from_回溯得到路径并填充result_nodes

open_set_detail.pop():从优先队列中删除当前节点

  if (closed_set_.count(from_node) != 0) {
    // if showed before, just skip...
    continue;
  }
  closed_set_.emplace(from_node);

检查closed_set_是否包含from_node,如果已存在则跳过,如果不包含将from_node加入closed_set_,表示已经搜索过该节点

  // 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());
  }

GetResidualS函数

double start_s = node->StartS():默认进入点是 node->StartS()

const auto iter = enter_s_.find(node);
  if (iter != enter_s_.end()) {
    if (iter->second > node->EndS()) {
      return 0.0;
    }
    start_s = iter->second;
  } else {
    AWARN << "lane " << node->LaneId() << "(" << node->StartS() << ", "
          << node->EndS() << "not found in enter_s map";
  }

上面逻辑是如果enter_s_中有记录,则使用enter_s_中的start_s,因为如果发生过变道那么start_s是比node的StartS()更靠后的位置

double end_s = node->EndS():默认为node的结束位置

const TopoNode* succ_node = nullptr;
for (const auto* edge : node->OutToAllEdge()) {
    if (edge->ToNode()->LaneId() == node->LaneId()) {
      succ_node = edge->ToNode();
      break;
    }
}

上面逻辑搜索与本条lane 相同 laneId 的 succ_node（同一车道的下一段）

if (succ_node != nullptr) {
    end_s = succ_node->EndS();
}

如果能找到当前节点相同laneid的连接node,那么设置end_s为连接node的结束位置

最后得到residual_s=end_s - start_s

residual_s主要用于判断是否有足够空间进行变道

所以neighbor_edges的值,会根据是否符合变道的条件来进行赋值,如果满足变道条件就赋值为所有出边包括变道的edge,否则只赋值为直行的出边

GetSubInEdgesIntoSubGraph函数

如果to_node产生了多个sub_node,那么原始edge->to_node的边不再适用,需要替换成edge->sub_node的边.

如果不能替换,就使用原始edge

void SubTopoGraph::GetSubInEdgesIntoSubGraph(
    const TopoEdge* edge,
    std::unordered_set<const TopoEdge*>* const sub_edges) const {

edge:原始拓扑图的一条边

sub_edges:要输出的子图中的边集合

  const auto* from_node = edge->FromNode();
  const auto* to_node = edge->ToNode();

提取此边edge的起点和终点

  std::unordered_set<TopoNode*> sub_nodes;
  if (from_node->IsSubNode() || to_node->IsSubNode() ||
      !GetSubNodes(to_node, &sub_nodes)) {
    sub_edges->insert(edge);
    return;
  }

如果from_node是子节点/to_node是子节点/to_node没有对应的子节点,这三种情况说明edge不需要替换,直接使用原始边

  for (const auto* sub_node : sub_nodes) {
    for (const auto* in_edge : sub_node->InFromAllEdge()) {
      if (in_edge->FromNode() == from_node) {
        sub_edges->insert(in_edge);
      }
    }
  }

如果from_node不是子节点/to_node不是子节点,但to_node上有子节点,此时要将原始edge替换成edge->sub_node的边

这样next_edge_set存的就是当前节点的邻接边的集合是sub_node级别

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;
  }

遍历当前节点的连接边,如果已经在closed_set_中就跳过,说明已经搜索过,然后再次判断连接子节点之后如果存在变道的情况是否有足够空间进行变道

tentative_g_score =
          g_score_[current_node.topo_node] + GetCostToNeighbor(edge);

计算到当前节点邻接节点的g值,到当前节点的g值g_score_[current_node.topo_node],当前节点到邻接节点的g值,使用两个节点间边的cost加上邻接节点的cost

g[邻接节点] = g[当前节点] + 当前节点到邻接节点的cost值

double GetCostToNeighbor(const TopoEdge* edge) {
  return (edge->Cost() + edge->ToNode()->Cost());
}

edge->Cost()和edge->ToNode()->Cost()的值,都是从routing_map.bin文件中的node结构和edge结构中读取出来的

      if (edge->Type() != TopoEdgeType::TET_FORWARD) {
        tentative_g_score -=
            (edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2;
      }

如果是左/右换道g值会相应减小,这也意味着换道的优先级更高

double f = tentative_g_score + HeuristicCost(to_node, dest_node);

计算f值,邻接节点的g值+邻接节点到终点的曼哈顿距离

      if (open_set_.count(to_node) != 0 && f >= g_score_[to_node]) {
        continue;
      }

如果当前节点已经搜索过,或者已经有更小f值路径了,则跳过

// 前向边处理
if (edge->Type() == TopoEdgeType::TET_FORWARD) {
  enter_s_[to_node] = to_node->StartS();
} 
// 非前向边（变道）处理
else {
  double to_node_enter_s =
      (enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
      from_node->Length() * to_node->Length();
  // 边界检查
  to_node_enter_s = std::min(to_node_enter_s, to_node->Length());
  // 特殊目标车道检查
  if (to_node_enter_s > to_node->EndS() && to_node == dest_node) {
    continue;
  }
  enter_s_[to_node] = to_node_enter_s;
}

上面是进入位置enter_s精确计算

前向边:简单情况,从车道起点进入

变道边:复杂情况需要计算合适的进入点

double to_node_enter_s =
      (enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
      from_node->Length() * to_node->Length();

如果原车道已行驶60%,则在新车道也应该从60%位置进入,FLAGS_min_length_for_lane_change确保变道有足够空间

g_score_[to_node] = f;  // 注意：这里应该是tentative_g_score，可能是代码bug
SearchNode next_node(to_node);
next_node.f = f;
open_set_detail.push(next_node);  // 加入优先队列
came_from_[to_node] = from_node;   // 记录路径来源

// 确保节点在open_set_哈希表中
if (open_set_.count(to_node) == 0) {
  open_set_.insert(to_node);
}

上面主要是节点更新与路径记录

g_score_[to_node] = f:当前实现将f值(g+h)存储为g_score

SearchNode next_node(to_node):

open_set_detail.push(next_node):更新当前节点

came_from_[to_node] = from_node:路径记录

就这样循环,直到当前节点为终点时,表示找到了最优路径,上面逻辑就是astar的整体流程

总结一下:

从起点开始进入优先队列->开始循环优先队列获取到的是cost最小节点->找出当前节点的所有的连接节点如果是子节点就把子节点作为邻接节点,计算f值存入优先队列->继续循环直到当前节点是终点->回溯路径

接下来说一下回溯的过程

Reconstruct(came_from_, from_node, result_nodes)
bool Reconstruct(
    const std::unordered_map<const TopoNode*, const TopoNode*>& came_from,
    const TopoNode* dest_node, std::vector<NodeWithRange>* result_nodes) {
  std::vector<const TopoNode*> result_node_vec;
  result_node_vec.push_back(dest_node);

  auto iter = came_from.find(dest_node);
  while (iter != came_from.end()) {
    result_node_vec.push_back(iter->second);
    iter = came_from.find(iter->second);
  }
  std::reverse(result_node_vec.begin(), result_node_vec.end());
  if (!AdjustLaneChange(&result_node_vec)) {
    AERROR << "Failed to adjust lane change";
    return false;
  }
  result_nodes->clear();
  for (const auto* node : result_node_vec) {
    result_nodes->emplace_back(node->OriginNode(), node->StartS(),
                               node->EndS());
  }
  return true;
}

bool Reconstruct(
    const std::unordered_map<const TopoNode*, const TopoNode*>& came_from,
    const TopoNode* dest_node, std::vector<NodeWithRange>* result_nodes) {
  std::vector<const TopoNode*> result_node_vec;
  result_node_vec.push_back(dest_node);

  auto iter = came_from.find(dest_node);
  while (iter != came_from.end()) {
    result_node_vec.push_back(iter->second);
    iter = came_from.find(iter->second);
  }

从入参came_from进行回溯,因为came_from键是to_node,值是from_node,首先知道终点的from_node, 然后循环查找终点from_node的from_node直到找到起点,将找到的所有节点存入result_node_vec,此时result_node_vec中的顺序还是从终点节点,终点节点前一个节点,终点节点前一个节点的前一个节点,这样反着的一直到终点节点.

std::reverse(result_node_vec.begin(), result_node_vec.end());

上面是将result_node_vec进行反转也就是从起点到终点节点的存储

AdjustLaneChange函数

AdjustLaneChange(&result_node_vec))
bool AdjustLaneChange(std::vector<const TopoNode*>* const result_node_vec) {
  if (result_node_vec->size() < 3) {
    return true;
  }
  if (!AdjustLaneChangeBackward(result_node_vec)) {
    AERROR << "Failed to adjust lane change backward";
    return false;
  }
  if (!AdjustLaneChangeForward(result_node_vec)) {
    AERROR << "Failed to adjust lane change backward";
    return false;
  }
  return true;
}

AdjustLaneChangeBackward函数

在路径中遇到“变道”（非 TET_FORWARD 边）时，看看能不能把中间那个车道节点（from_node）换成一个更长、更优的车道段，前提是：这个新车道也能从 base_node 变道过来，并且还能继续走到 to_node

bool AdjustLaneChangeBackward(
    std::vector<const TopoNode*>* const result_node_vec) {
  for (int i = static_cast<int>(result_node_vec->size()) - 2; i > 0; --i) {
    const auto* from_node = result_node_vec->at(i);
    const auto* to_node = result_node_vec->at(i + 1);
    const auto* base_node = result_node_vec->at(i - 1);
    const auto* from_to_edge = from_node->GetOutEdgeTo(to_node);
    if (from_to_edge == nullptr) {
      // may need to recalculate edge,
      // because only edge from origin node to subnode is saved
      from_to_edge = to_node->GetInEdgeFrom(from_node);
    }

首先result_node_vec需要包含三个及三个以上存在变道边,从后向前遍历result_node_vec

开始时

from_node:倒数第二个节点

to_node:终点节点

base_node:倒数第三个节点

首先,获得from_node与to_node之间的edge信息,因为有时候edge信息没保存在from_node出边,而是保存在了to_node的入边,所以当从from_node出边没获取到edge信息的时候,又调用了一次

    if (from_to_edge == nullptr) {
      // may need to recalculate edge,
      // because only edge from origin node to subnode is saved
      from_to_edge = to_node->GetInEdgeFrom(from_node);
    }

从to_node的入边获取edge信息,需要注意获取的edge信息,即有可能是直行边,也有可能是左右变道边

    if (from_to_edge->Type() != TopoEdgeType::TET_FORWARD) {
      if (base_node->EndS() - base_node->StartS() <
          from_node->EndS() - from_node->StartS()) {
        continue;
      }
      std::vector<const TopoNode*> candidate_set;
      candidate_set.push_back(from_node);
      const auto& out_edges = base_node->OutToLeftOrRightEdge();
      for (const auto* edge : out_edges) {
        const auto* candidate_node = edge->ToNode();
        if (candidate_node == from_node) {
          continue;
        }
        if (candidate_node->GetOutEdgeTo(to_node) != nullptr) {
          candidate_set.push_back(candidate_node);
        }
      }
      const auto* largest_node = GetLargestNode(candidate_set);
      if (largest_node == nullptr) {
        return false;
      }
      if (largest_node != from_node) {
        result_node_vec->at(i) = largest_node;
      }
    }

获取到edge信息之后,要判断是否是变道类型,只有存在变道边的情况下才需要调整变道起点并且要保证base_node长度大于等于from_node,因为

算法策略
当 base_node 较短时：说明当前处于特殊的道路区域（如汇入区），此时应该保持原路径，不进行车道变更优化

当 base_node 较长时：说明当前在主要车道上行驶，有足够的距离和条件来考虑优化车道选择

std::vector<const TopoNode*> candidate_set;
      candidate_set.push_back(from_node);
      const auto& out_edges = base_node->OutToLeftOrRightEdge();
      for (const auto* edge : out_edges) {
        const auto* candidate_node = edge->ToNode();
        if (candidate_node == from_node) {
          continue;
        }
        if (candidate_node->GetOutEdgeTo(to_node) != nullptr) {
          candidate_set.push_back(candidate_node);
        }
      }

首先将from_node存入candidate_set,然后获取base_node所有左/右出边,如果出边的to_node节点和当前to_node节点存在edge,说明出边的from_node可以作为当前from_node的候补,也就意味着可以通过出边的from_node所表示的车道到达当前to_node所表示的车道

const auto* largest_node = GetLargestNode(candidate_set);
      if (largest_node == nullptr) {
        return false;
      }
      if (largest_node != from_node) {
        result_node_vec->at(i) = largest_node;
      }

然后获取candidate_set中所有候补节点长度最大的候补节点,替换原有的from_node,这样就达到了在一段比较长的车道上进行变道的效果.

AdjustLaneChangeForward函数

相似

AdjustLaneChangeBackward函数与AdjustLaneChangeForward函数作用

AdjustLaneChangeBackward函数:因为from_node替换成了更长的可行驶区域node,所以可以从新的from_node更早的位置变道到达to_node

AdjustLaneChangeForward函数:因为to_node替换成了更长的可行驶区域node,所以从from_node变道到新的to_node后可以行驶更长时间

最后得到调整后的result_node_vec

  for (const auto* node : result_node_vec) {
    result_nodes->emplace_back(node->OriginNode(), node->StartS(),
                               node->EndS());
  }

然后把result_node_vec中节点存储到输出参数result_nodes中,result_nodes类型是std::vector<NodeWithRange>
node与(可行驶区间)

class NodeWithRange : public NodeSRange {
 public:
  NodeWithRange(const NodeWithRange& other) = default;
  NodeWithRange(const TopoNode* node, double start_s, double end_s);
  NodeWithRange(const TopoNode* node, const NodeSRange& range);
  virtual ~NodeWithRange();
  bool operator<(const NodeWithRange& other) const;

  const TopoNode* GetTopoNode() const;
  bool IsVirtual() const;
  const std::string& RoadId() const;
  const std::string& LaneId() const;
  double FullLength() const;

 private:
  const TopoNode* topo_node_ = nullptr;
};

std::vector<NodeWithRange> cur_result_nodes;
    if (!strategy_ptr->Search(graph, &sub_graph, start, end,
                              &cur_result_nodes)) {
      AERROR << "Failed to search route with waypoint from " << start->LaneId()
             << " to " << end->LaneId();
      return false;
    }

    node_vec.insert(node_vec.end(), cur_result_nodes.begin(),
                    cur_result_nodes.end());
  }

  if (!MergeRoute(node_vec, result_nodes)) {
    AERROR << "Failed to merge route.";
    return false;
  }

通过Search函数获取到路径节点以及相应的可行驶区间后,还要执行MergeRoute(node_vec, result_nodes)

bool Navigator::MergeRoute(
    const std::vector<NodeWithRange>& node_vec,
    std::vector<NodeWithRange>* const result_node_vec) const {
  for (const auto& node : node_vec) {
    if (result_node_vec->empty() ||
        result_node_vec->back().GetTopoNode() != node.GetTopoNode()) {
      result_node_vec->push_back(node);
    } else {
      if (result_node_vec->back().EndS() < node.StartS()) {
        AERROR << "Result route is not continuous.";
        return false;
      }
      result_node_vec->back().SetEndS(node.EndS());
    }
  }
  return true;
}

主要是判断计算出来的全局路线是否连续,然后如果同一个节点有重叠可行驶区间进行区间合并.

这样SearchRouteByStrategy函数就分析完了.SearchRouteByStrategy函数的输出参数是result_nodes我们继续分析

if (!SearchRouteByStrategy(graph_.get(), way_nodes, way_s, &result_nodes)) {
    SetErrorCode(ErrorCode::ROUTING_ERROR_RESPONSE,
                 "Failed to find route with request!",
                 response->mutable_status());
    return false;
  }
  if (result_nodes.empty()) {
    SetErrorCode(ErrorCode::ROUTING_ERROR_RESPONSE, "Failed to result nodes!",
                 response->mutable_status());
    return false;
  }
  result_nodes.front().SetStartS(request.waypoint().begin()->s());
  result_nodes.back().SetEndS(request.waypoint().rbegin()->s());

我们看到获取到result_nodes后,又重新设置了起点可行驶区间的开始位置,终点可行驶区间的结束位置,因为在Reconstruct函数中所有的节点用的都是节点的可行驶区间,所以最终起点终点,要更新为我们想要从哪个位置出发,在哪个位置停下来的区间起终位置.

  for (const auto* node : result_node_vec) {
    result_nodes->emplace_back(node->OriginNode(), node->StartS(),
                               node->EndS());
  }