Apollo：pnc map

在这里插入图片描述

概述

什么是pnc map

PncMap是Planning and Control Map的缩写，即用于规划控制使用的地图

这是Planning用来对接Routing搜索结果的子模块。
pnc map是专门为规划控制模块设计的库函数。它在hd map层次之上，负责一些地图相关信息的处理。比如查询车辆可能的路由段，然后对每个路由段合成一个路径path。

pnc map作用

pnc map的作用就是将routing的结果，进行一系列的操作处理，找到一个局部的(固定长度)，可以供规划使用的，车辆能够正常驶入的局部地图（RouteSegments），后续用于生成规划中的ReferenceLine结构。

为什么有了高精地图还需要pnc_map

为规划模块的独立性更高，避免与高精地图的数据格式耦合，导致可迁移性变差。

作用

pnc map主要是将routing发送的轨迹规划转变为能够生成reference的中间数据

pnc_map封装在reference_line_provider下，主要供其在由routing结果到ReferenceLine的过程中做前期准备。
PncMap类负责对接Routing搜索结果的更新。它会根据车辆当前位置，提供车辆周边的RouteSegments信息供ReferenceLineProvider生成ReferenceLine。

“车辆周边”与车辆的纵向和横向相关，具体如下：

对于纵向来说，PncMap返回的结果是前后一定范围内的。
- 具体的范围由下面三个值决定：
- 即：向后是30米的范围。向前是根据车辆的速度返回180米或者250米的范围。

// modules/map/pnc_map/pnc_map.cc

DEFINE_double(
look_backward_distance, 30,
"look backward this distance when creating reference line from routing");

DEFINE_double(look_forward_short_distance, 180,
"short look forward this distance when creating reference line "
"from routing when ADC is slow");

DEFINE_double(
look_forward_long_distance, 250,
"look forward this distance when creating reference line from routing");

对于横向来说，如果Routing的搜索结果包含变道的信息。则PncMap提供的数据会包含自车所在车道和变道后的相关通路。

PncMap中的下面这个方法用来接收车辆的状态更新。当然，这其中很重要的就是位置状态。

bool PncMap::UpdateVehicleState(const VehicleState &vehicle_state)

此时PncMap会计算车辆已经经过了哪些Routing的途经点，以及下一个目标点等信息。

在每一个Planning计算循环中，ReferenceLineProvider都会传入车辆的VehicleState给PncMap，并从PncMap获取车辆周围的通路（RouteSegments）。

PncMap中的下面这个方法返回Routing途经点还有哪些没有达到：

std::vector<routing::LaneWaypoint> PncMap::FutureRouteWaypoints() const

pnc_map的主要步骤

pnc_map封装在reference_line_provider下，主要供其在由routing结果到ReferenceLine的过程中做前期准备，而实现这一功能的主要步骤分为两步：

处理routing结果：这部分接收routing模块的路径查询响应，然后进行一定的处理，最后存储到map相关的数据结构，供后继使用
- 指的是PncMap::UpdateRoutingResponse
构造RouteSegments结构
- 指的是PncMap::GetRouteSegments()
  +目的：根据当前自车的位置，查询自身可以行驶的车道段(Passage && LaneSegment)
- 它会即根据第一步转换的routing信息及自车位置，构造route_segment。这个我们从该函数的入参也可以很清晰的看出来。
  - 入参：车辆的状态(包含车辆的位置，速度，偏航角，加速度等信息)
  - 入参：backward_length和forward_length是路径短生成过程中前向与后向修正距离
  - 返回：返回的是短期内(注意是短期内，长期调度不确定因素太多，无法实现)车辆的运动轨迹route_segments

bool PncMap::GetRouteSegments(const VehicleState &vehicle_state,
                              const double backward_length,
                              const double forward_length,
                              std::list<RouteSegments> *const route_segments)

在这之后，reference_line_provider 里会将RouteSegments转换为hdmap::Path结构，再转成ReferenceLine结构，继而进行平滑

问题：在Prediction障碍物短期运动方案规划中，"短期"是指当前障碍位置到LaneSequence的第一个最近Lanepoint的位置的路段(在LanePoint构建时，每条LaneSequence最多持有20个LanePoint，每两个LanePoint之间的距离为2m)，所以Prediction中的短期大致预测在2m这个邻域范围内障碍物的运动轨迹。那么Planning模块对无人车短期的路径查询，"短期"是多少范围或者是什么概念？
回答：Planning对主车"短期"路径查询的距离段由函数后向查询距离backward_length和前向查询距离forward_length决定，所以查询的路长为当前主车前后backward_length + forward_length距离的路段。
额外补充一下，在指引线提供器中，调用该函数，后向查询距离FLAGs_look_backward_distance默认为30m，前向查询距离取决于主车速度，若速度很大 linear_velocity() * FLAGS_look_forward_time_sec > FLAGS_look_forward_short_distance，其中FLAGS_look_forward_time_sec默认8s，FLAGS_look_forward_short_distance默认150m)，前向查询设置为look_forward_long_distance，默认250m；否者速度小时设置为FLAGS_look_forward_short_distance，150m。

更新routing数据：PncMap::UpdateRoutingResponse()

主要函数为：

bool PncMap::UpdateRoutingResponse(const routing::RoutingResponse &routing)

可以看到，其输入为RoutingResponse成员，即routing模块的输出。

其主要功能为初始化下面加粗的三个pnc_map类成员：

在得到RoutingResponse后，该函数为所有的routing::LaneSegment构建了索引列表route_indices_ ，其类型为vector< RouteIndex >。并将索引相对应的LaneSegment类型转换为hdmap::LaneSegment存进RouteIndex。
将LaneSegment中的车道的lane_id取出，存入all_lane_ids_，和route_indices_ 中的LaneSegment一一对应。
查询起始点和终点在哪段LaneSegment中，存入routing_waypoint_index_。

具体解析如下：

剥离RoutingResponse

首先，每当有新的routing结果时，都需要用最新的RoutingResponse对PncMap里的数据进行更新。

用三个for循环分别剥离出routing结果里的road，passage，lane，
将每条lane的id存进all_lane_ids这个数组中
将每个lane_segment及{road_index, passage_index, lane_index} 存进route_indices_中。

在这里插入图片描述

从上述代码可以响应结果剥离工作比较简单，就是对这条完整路径进行RoadSegment，Passage，LaneSegment的存储。举个例子：

在这里插入图片描述
图中左上角蓝色方框为一个road的示例，包含road_index,passage_index,lane_index，左下角在生成的RoutingResponse上进行车道线编号。

在举个例子：

查询点处理

在UpdateRoutingResponse中，第二步是将routing中的request点信息转化后的索引数据中进行匹配，找到具体的LaneSegment信息：

在这里插入图片描述

在这里插入图片描述
举个例子，上图中，该次查询的查询点waypoint一共两个，起点(红色星星)和终点(蓝色星星)。这一步需要对这两个waypoint进行处理，计算这两个waypoint分别在上述的那些LaneSegment中，准确的说是上述all_lane_ids_中每个LaneSegment包含有哪些waypoint。

我们来看下WithinLaneSegment是怎么实现的。

/// file in apollo/modules/map/pnc_map/route_segments.cc
bool RouteSegments::WithinLaneSegment(const LaneSegment &lane_segment, const routing::LaneWaypoint &waypoint) {
  return lane_segment.lane && lane_segment.lane->id().id() == waypoint.id() &&
         lane_segment.start_s - kSegmentationEpsilon <= waypoint.s() &&
         lane_segment.end_s + kSegmentationEpsilon >= waypoint.s();
}

从代码中可以看到，waypoint在lane_segment中需要满足条件：

waypoint和lane_segment的所在的车道lane的id必须一致
waypoint的累计距离s必须在lane_segment的start_s和end_s之间。

最终生成的数据在routing_waypoint_index_

构造RouteSegments：PncMap::GetRouteSegments

该函数为pnc_map的第二个步骤，也是其主要功能，即根据第一步骤转换的routing信息及自车位置来构造route_segments，这个我们从该函数的入参也可以很清晰的看出来。

简单来说，它的作用就是根据车辆当前位置生成短期通道

输入为车辆当前位置，前向查找距离，后向查找距离，
输出为std::list< RouteSegments>，list中每一个RouteSegments代表一条参考线经过的RouteSegments。在apollo中，该list的元素被限制为了最多为两个

/// file in apollo/master/modules/map/pnc_map/pnc_map.cc
PncMap::GetRouteSegments(const VehicleState &vehicle_state,
                         const double backward_length,
                         const double forward_length,
                         std::list<RouteSegments> *const route_segments)

关于输出RouteSegments：

class RouteSegments : public std::vector< LaneSegment>
该类从Passage扩展而来，保留了Passage的某些特征。该类包含了生成hdmap::Path类的原始数据，而Path类又可以用于初始化参考线。

简单来说，我们在这一步主要是生成连接起来的可通行的Passage，从而为生成参考线做准备。

更新pnc map中无人车状态：PncMap::UpdateVehicleState

在这里插入图片描述
这里的更新并不是更新车辆的位置，速度等，而是根据传入的车辆状态信息，更新当前所在的LaneSegment位置以及下一必经过点的waypoint位置信息

这部分由UpdateVehicleState函数完成

UpdateVehicleState函数的签名如下：

PncMap::UpdateVehicleState(const VehicleState &vehicle_state)

输入为VehicleState类对象，该类被定义在modules/common/vehicle_state/proto/vehicle_state.proto文件中，主要存储自车当前的位置信息，状态信息等。
VehicleState在这里主要用到了其全局坐标x，y，以及自车航向角heading。

这个函数完成的工作就是计算上图中的红色部分：

包括离当前车辆最近的车道adc_waypoint_
adc_waypoint_所在LaneSegment在route_indices_中的索引adc_route_index_
下一个最近查询点在routing_waypoint_index_中的索引next_routing_waypoint_index_。

其主要逻辑根据自车的全局坐标系以及航向角，找到自车在哪一个LaneSegment中，或者说投影后距离那个LaneSegment最近，输出自车所在LaneSegment对应的index，并计算离自车最近的下一个必经点LaneWaypoint所在的LaneSegment对应的index。

在这里插入图片描述
我们来细细分析一下：

（１）根据自车状态，从routing中查找最近的道路点adc_waypoint_

/// file in apollo/modules/map/pnc_map/pnc_map.cc
bool PncMap::UpdateVehicleState(const VehicleState &vehicle_state) {
  // Step 1. 计算当前车辆在对应车道上的投影adc_waypoint_
  adc_state_ = vehicle_state;
  if (!GetNearestPointFromRouting(vehicle_state, &adc_waypoint_)) {
    return false;
  }
  ...
}

GetNearestPointFromRouting的计算流程大致如下：

据当前车辆坐标(x,y)以及速度方向heading，去高精地图hd map查询车辆附近的车道(车道方向必须与heading方向夹角在90度以内，意味着都是相同方向的车道，超过90度可能是反向车道)
计算查询得到的车道和range_lane_ids_或者all_lane_ids_的交集，也就是查找RoutingResponse.road().passage()中的附近的车道。
对2中过滤得到的车道进行车辆坐标投影，可计算车辆到各个车道的距离，去最小距离的车道作为最终的投影车道，得到adc_waypoint_的车道id和累积距离s，其实也可以计算投影点，但是这里没有计算。

（２）计算索引adc_route_index_

计算adc_waypoint_对应的adc_route_index_
- 根据adc_waypoint来在route_indices_中查找到当前行驶到了routing哪条LaneSegment上，并将index赋给adc_route_index_
- 对应函数GetWaypointIndex()。其对应逻辑：
  - 只要根据adc_waypoint_的车道id和累计距离前向或者后向查询route_indices_，
  - 如果某个LaneSegment和adc_waypoint_的车道id一致，并且累计距离s在LaneSegment的[start_s, end_s]区间，
  - 就可以得到投影点所在的索引adc_route_index_。
计算next_routing_waypoint_index_

（３）计算next_routing_waypoint_index_

计算自车下一个必经的waypoint点，并赋给next_routing_waypoint_index_
对应函数UpdateNextRoutingWaypointIndex()。其对应逻辑：
- 从当前车辆的索引开始，向后查找最近的查询点waypoint(必须是同一个LaneSegment)
- 从而得到这个查询点所在的LaneSegment索引next_routing_waypoint_index_

/// file in apollo/modules/map/pnc_map/pnc_map.cc
void PncMap::UpdateNextRoutingWaypointIndex(int cur_index) {
  // 情况1. 车道倒车，后向查找，下一个查询点waypoint对应的索引查找
  // search backwards when the car is driven backward on the route.
  while (next_routing_waypoint_index_ != 0 &&
         next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
         routing_waypoint_index_[next_routing_waypoint_index_].index > cur_index) {
    --next_routing_waypoint_index_;
  }
  while (next_routing_waypoint_index_ != 0 &&
         next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
         routing_waypoint_index_[next_routing_waypoint_index_].index == cur_index &&
         adc_waypoint_.s < routing_waypoint_index_[next_routing_waypoint_index_].waypoint.s) {
    --next_routing_waypoint_index_;
  }
  // 情况2. 车道前进，前向查找，下一个查询点waypoint对应的索引查找
  // search forwards
  // 第1步，查找最近包含有waypoint的LaneSegment 
  while (next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
         routing_waypoint_index_[next_routing_waypoint_index_].index < cur_index) {
    ++next_routing_waypoint_index_;
  }
  // 第2步，查找下一个最近的waypoint对应的索引
  while (next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
         cur_index == routing_waypoint_index_[next_routing_waypoint_index_].index &&
         adc_waypoint_.s >= routing_waypoint_index_[next_routing_waypoint_index_].waypoint.s) {
    ++next_routing_waypoint_index_;
  }
  if (next_routing_waypoint_index_ >= routing_waypoint_index_.size()) {
    next_routing_waypoint_index_ = routing_waypoint_index_.size() - 1;
  }
}

举例1

现在有以下这么个情况：

|-wp0---------------------vp--------|-------wp1-----wp2---------------wp3-----|-------------------------wp4-----|
\             LaneSegment k         /\            LaneSegment k+1            /\         LaneSegment k+2         /

其中vp为当前车辆在LaneSegment中的投影，wp0, wp1, wp2, wp3分别是路径查询的4个waypoint(必须经过的点。现在我们要查询车辆下一个最近的必经点waypoint，那么结果应该是wp1！假设车辆当前是前进状态，cur_index值为k，那么：

可以判断，函数执行前，next_routing_waypoint_index_值为0(查询wp0时得到)

第1步，查找最近包含有waypoint的LaneSegment，最终得到的结果next_routing_waypoint_index_仍然为0，因为routing_waypoint_index_[next_routing_waypoint_index_].index就是wp0.index，值为k，与cur_index相等。

第2步，查找下一个最近的waypoint对应的索引，最终的结果next_routing_waypoint_index_变为1，因为adc_waypoint_.s >= wp0.s。

举例2

现在有以下这么个情况(车辆行驶到了LaneSegment k+2，并且已经经过了wp1，wp2和wp3)：

|-wp0-------------------------------|----wp1--------wp2---------------wp3-----|-----vp------------------wp4-----|
\              aneSegment k         /\            LaneSegment k+1            /\         LaneSegment k+2         /

其中vp为当前车辆在LaneSegment中的投影，wp0, wp1, wp2, wp3分别是路径查询的4个waypoint(必须经过的点)，其中wp0，wp1，wp2和wp3已经经过了，wp4待车辆经过。现在我们要查询车辆下一个最近的必经点waypoint，那么结果应该是wp4！假设车辆当前是前进状态，cur_index值为k+2，那么：

可以判断，函数执行前，next_routing_waypoint_index_值为3(查询wp3时得到)

第1步，查找最近包含有waypoint的LaneSegment，最终得到的结果next_routing_waypoint_index_变为4，因为routing_waypoint_index_[next_routing_waypoint_index_].index就是wp3.index，值为k+1，wp3.index < cur_index。

第2步，查找下一个最近的waypoint对应的索引，next_routing_waypoint_index_仍然为4，因为adc_waypoint_.s < wp4.s

（４）到达目的地标志

/// file in apollo/modules/map/pnc_map/pnc_map.cc
bool PncMap::UpdateVehicleState(const VehicleState &vehicle_state) {
  // Step 1. 计算当前车辆在对应车道上的投影adc_waypoint_
  ...
  // Step 2. 计算车辆投影点所在LaneSegment在route_indices_`的索引`dc_route_index_`  
  int last_index = GetWaypointIndex(routing_waypoint_index_.back().waypoint);
  if (next_routing_waypoint_index_ == routing_waypoint_index_.size() - 1 ||
      (!stop_for_destination_ && last_index == routing_waypoint_index_.back().index)) {
    stop_for_destination_ = true;
  }
}

如果next_routing_waypoint_index_是终点的索引，表示已经到达目的地。

计算临近通道：PncMap::GetNeighborPassages

根据上一步得到的本车所处index信息，计算出当前车辆在轨迹规划中可能经过的所有相邻车道(passage)信息，作为将要生成的reference_line原始信息：

adc_waypoint_：
- 在上一步更新车辆状态中设置
- 表示当前车辆在routing中的位置
road_index：自车所在的road索引
passage_idx：起始的通道索引。

计算相邻可通行区域信息调用的是GetNeighborPassages，其入参：

自车所在的RoadSegment
通道Passage的id

在这里插入图片描述

这里我们简单地总结一下获取邻接可驶入通道的原则：

如果当前通道(Passage)是直行道，无法变道，那么直接返回车辆所在的车道

if (source_passage.change_lane_type() == routing::FORWARD) {
  return result;
}

如果当前车道即将退出，则表示即将驶入下一个passage，不需要变道，直接返回车辆所在的车道

if (source_passage.can_exit()) {  // no need to change lane
  return result;
}

如果下一个必经查询点在当前车道上，此时不需要变道，则返回当前车道

RouteSegments source_segments;
if (!PassageToSegments(source_passage, &source_segments)) {
  return result;
}
if (next_routing_waypoint_index_ < routing_waypoint_index_.size() &&
  source_segments.IsWaypointOnSegment(routing_waypoint_index_[next_routing_waypoint_index_].waypoint)) {
  return result;
}

如果当前车道为左转或右转车道
- 从高精地图hdmap中查询当前车道对应左侧或者右侧的所有车道
  - 如果当前Passage的change_lane_type为RIGHT，则输出为当前RoadSegment中自车右手边全部通道Passage的id，包括自车所在通道。
  - 如果当前Passage的change_lane_type为LEFT，则输出为当前RoadSegment中自车左手边全部通道Passage的id，包括自车所在通道。
- 然后去和当前RoadSegment.passage()去做对比，找到两者共同包含的车道，就是最终的邻接车道。

  std::unordered_set<std::string> neighbor_lanes;
  if (source_passage.change_lane_type() == routing::LEFT) {   // 当前passage是左转通道
    for (const auto &segment : source_segments) {     // 查询当前Passage中每个LaneSegment所在车道的邻接左车道
      for (const auto &left_id : segment.lane->lane().left_neighbor_forward_lane_id()) {
        neighbor_lanes.insert(left_id.id());
      }
    }
  } else if (source_passage.change_lane_type() == routing::RIGHT) { 当前passage是右转通道
    for (const auto &segment : source_segments) {     // 查询当前Passage中每个LaneSegment所在车道的邻接右车道
      for (const auto &right_id : segment.lane->lane().right_neighbor_forward_lane_id()) {
        neighbor_lanes.insert(right_id.id());
      }
    }
  }

  for (int i = 0; i < road.passage_size(); ++i) {
    if (i == start_passage) {
      continue;
    }
    const auto &target_passage = road.passage(i);
    for (const auto &segment : target_passage.segment()) {
      if (neighbor_lanes.count(segment.id())) {       // 查询当前RoadSegment中所有Passage::LaneSegment的所属车道，有交集就添加到结果中
        result.emplace_back(i);
        break;
      }
    }
  }
  return result;
}

举例来说，蓝色车为自车，则该函数输出Passage1, 2。

若不为双车道，例如右边有Passage3，Passage4，则输出为Passage1，2，3，4。
在这里插入图片描述

这里需要关注一下PassageToSegments。从下面可以看出，passage表达的数据内容和RouteSegments其实是类似的：RouteSegments就是将Passage中一条条lane，存进其数组中，病记录起始终止的s值

bool PncMap::PassageToSegments(routing::Passage passage,
                               RouteSegments *segments) const {
  CHECK_NOTNULL(segments);
  segments->clear();
  for (const auto &lane : passage.segment()) {
    auto lane_ptr = hdmap_->GetLaneById(hdmap::MakeMapId(lane.id()));
    if (!lane_ptr) {
      AERROR << "Failed to find lane: " << lane.id();
      return false;
    }
    segments->emplace_back(lane_ptr, std::max(0.0, lane.start_s()),
                           std::min(lane_ptr->total_length(), lane.end_s()));
  }
  return !segments->empty();
}

生成list< RouteSegments>：根据相邻passage生成route_segments

遍历上一步输出的所有邻接车道，

先对对每个相邻车道做一个是否可以驶入的检测，如果不可以驶入，那么就排除
- 为什么要做是否可以驶入的检测？因为相邻passage可能是反向的，也有可能相邻比较远（通过连续两个或以上换道才能驶入），所以需要排除。
- CanDriveFrom函数判定依据为：自车能投影到该Passage上，自车距离该Passage横向距离不能超过一条车道的宽度，自车heading应该和该Passage所在车道方向一致等。这些条件导致了最终生成的list最多只有两个RouteSegments！
- 举个例子，如下图，最终生成的list< RouteSegments>可能长这样，图中灰色区域：
然后判断是否是自车所在车道
- 若为自车所在通道，根据前向查找距离，后向查找距离生成一条短期通道RouteSegments，存入list<　RouteSegments>。
- 若不为自车所在通道，调用CanDriveFrom查看能否从自车位置驶入该通道，若能则将自车投影到该通道并根据前向查找距离，后向查找距离生成一条短期通道RouteSegments，存入list< RouteSegments>。
- 即：也就是制定出无人车在当前情况下可能行驶的区域，每个可行驶通道将划分出一个道路区间，道路区间的长度由前向查询距离forward_length和后向查询距离backward_length决定，短期内规划的可行驶道路段长度为forward_length+backward_length。最终的路由段RouteSegments生成只需要对每个邻接可驶入的通道进行Segments生成即可。

  // Step 1. update vehicle state
  
  // Step 2. compute neighbor passages
auto drive_passages = GetNeighborPassages(road, passage_index);
 // Step 3. compute route segment
  for (const int index : drive_passages) {
    ...
  }