planning模块(8)-参考线的更新

planning模块(7)-参考线的平滑上一篇已经介绍了SmoothRouteSegment函数的内容,如果存在换道的情况会生成两条平滑参考线,一条是自车所在方向,一条是所要切换的车道方向,平滑前会分别调用SmoothRouteSegment函数进行参考线平滑.
如果是新的算路指令,当调用SmoothRouteSegment平滑成功后会进入下面逻辑

{
  common::SLPoint sl;
  if (!reference_lines->back().XYToSL(
          vehicle_state.heading(),
          common::math::Vec2d(vehicle_state.x(), vehicle_state.y()),
          &sl)) {
    AWARN << "Failed to project point: {" << vehicle_state.x() << ","
          << vehicle_state.y() << "} to stitched reference line";
  }
  Shrink(sl, &reference_lines->back(), &(*iter));
  ++iter;
}

下面就来介绍这个逻辑的作用

reference_lines->back().XYToSL(
                vehicle_state.heading(),
                common::math::Vec2d(vehicle_state.x(), vehicle_state.y()),
                &sl)

是在获取当前自车位置,在平滑后参考线上横纵距离

Shrink函数

bool ReferenceLineProvider::Shrink(const common::SLPoint& sl,
                                   ReferenceLine* reference_line,
                                   RouteSegments* segments)

sl:根据当前自车位置,在平滑后参考线上获取到的横纵距离
reference_line:平滑后的参考线数据
segments:平滑前的RouteSegments数据

  if (sl.s() > planning::FLAGS_look_backward_distance * 1.5) {
    ADEBUG << "reference line back side is " << sl.s()
           << ", shrink reference line: origin length: "
           << reference_line->Length();
    new_backward_distance = planning::FLAGS_look_backward_distance;
    need_shrink = true;
  }

如果自车在平滑后的参考线上的投影点的纵向距离大于阈值,说明自车后方生成了较长的参考线,需要进行裁剪

  // check heading
  const auto index = reference_line->GetNearestReferenceIndex(sl.s());
  const auto& ref_points = reference_line->reference_points();
  const double cur_heading = ref_points[index].heading();
  auto last_index = index;
  while (last_index < ref_points.size() &&
         std::fabs(AngleDiff(cur_heading, ref_points[last_index].heading())) <
             FLAGS_reference_line_max_forward_heading_diff) {
    ++last_index;
  }
  --last_index;
  if (last_index != ref_points.size() - 1) {
    need_shrink = true;
    common::SLPoint forward_sl;
    reference_line->XYToSL(ref_points[last_index], &forward_sl);
    new_forward_distance = forward_sl.s() - sl.s();
  }

GetNearestReferenceIndex函数主要是根据当前自车位置投影点的s值,在平滑后的参考线上找到第一个不小于s值的路径点索引.
然后根据路径点索引,获取到路径点,再获取到以路径点为起点的路径段的朝向,如果这个车道段的朝向与前方的路径段朝向相差的角度小于阈值,则认为方向变化平缓,可以继续往前使用,如果角度差大于阈值,说明存在弯道,循环就会停止.

  if (last_index != ref_points.size() - 1) {
    need_shrink = true;
    common::SLPoint forward_sl;
    reference_line->XYToSL(ref_points[last_index], &forward_sl);
    new_forward_distance = forward_sl.s() - sl.s();
  }

如果last_index != ref_points.size() - 1,说明存在弯路,这时需要裁剪,new_forward_distance是自车前方参考线应该裁剪的距离.

  // check backward heading
  last_index = index;
  while (last_index > 0 &&
         abs(AngleDiff(cur_heading, ref_points[last_index].heading())) <
             FLAGS_reference_line_max_backward_heading_diff) {
    --last_index;
  }
  if (last_index != 0) {
    need_shrink = true;
    common::SLPoint backward_sl;
    reference_line->XYToSL(ref_points[last_index], &backward_sl);
    new_backward_distance = sl.s() - backward_sl.s();
  }

上面逻辑是检查自车后方参考线之间的角度差是否过大,如果过大说明有不合适的参考线,需要进行裁剪,new_backward_distance是裁剪后,自车后方应该保留的参考线长度.

  if (need_shrink) {
    if (!reference_line->Segment(sl.s(), new_backward_distance,
                                 new_forward_distance)) {
      AWARN << "Failed to shrink reference line";
    }
    if (!segments->Shrink(sl.s(), new_backward_distance,
                          new_forward_distance)) {
      AWARN << "Failed to shrink route segment";
    }
  }

reference_line->Segment函数的作用:就是根据最新的new_forward_distance和new_backward_distance裁剪reference_points/裁剪map_path.
segments->Shrink函数的作用:
1.删除 s - look_backward 之前的车道段
2.从车辆所在车道开始查找 waypoint 所在 lane segment
3.删除 look_forward 之后的车道段

double acc_s = 0.0;
auto iter = begin();
while (iter != end() && acc_s + iter->Length() < s - look_backward) {
    acc_s += iter->Length();
    ++iter;
}

从头开始累积lane segment的长度acc_s，当：

acc_s + 当前segment长度 < s - look_backward

说明当前segment在自车后方要生成的参考线长度范围之外要删掉

acc_s <= s - look_backward <= acc_s + segment.length

直到找到“包含 backward 起始点”的 segment

  iter->start_s =
      std::max(iter->start_s, s - look_backward - acc_s + iter->start_s);

比较s - look_backward在当前这个segment上的纵向距离和这个segment起点的纵向距离,也就是调整这个segment可行驶区间的起点位置.

  if (iter->Length() < kSegmentationEpsilon) {
    ++iter;
  }
  erase(begin(), iter);

删除不在范围内的所有lane segment

  iter = begin();
  acc_s = 0.0;
  while (iter != end() && !WithinLaneSegment(*iter, waypoint)) {
    ++iter;
  }

从新的lane segment开始遍历,判断自车的waypoint 是否能匹配到某个segment 上
当匹配到时,iter指向车辆所在 lane segment

acc_s = iter->end_s - waypoint.s

计算自车所在的lane segment剩下的可行驶区间距离

  acc_s = iter->end_s - waypoint.s;
  if (acc_s >= look_forward) {
    iter->end_s = waypoint.s + look_forward;
    ++iter;
    erase(iter, end());
    return true;
  }

调整自车所在lane segment的可行驶区间终点,并删除之后的所有lane segment.

  ++iter;
  while (iter != end() && acc_s + iter->Length() < look_forward) {
    acc_s += iter->Length();
    ++iter;
  }
  if (iter == end()) {
    return true;
  }
  iter->end_s = std::min(iter->end_s, look_forward - acc_s + iter->start_s);
  erase(iter + 1, end());

如果没有满足acc_s >= look_forward,则继续累积后面的lane segment直到超过 look_forward.
当acc_s + iter->Length() $\ge$ look_forward时,acc_s的长度还小于look_forward,当加上iter->Length()时才大于等于,所以look_forward是在iter所指向的lane segment上,look_forward - acc_s + iter->start_s是在计算在iter所指向的lane segment上look_forward的纵向距离是多少,然后取可行驶区间的终点与look_forward的纵向距离最小值,就是这个lane segment最后的可行驶区间的终点位置.

然后将之后的lane segment全部删除.
到这Shrink函数就介绍完了,主要就是对平滑后的参考线,再做进一步的裁剪,主要保留方向连续的参考线部分,有大转弯的部分就删除掉了.

else {  // stitching reference line
    for (auto iter = segments->begin(); iter != segments->end();) {
      reference_lines->emplace_back();
      if (!ExtendReferenceLine(vehicle_state, &(*iter),
                               &reference_lines->back())) {
        AERROR << "Failed to extend reference line";
        reference_lines->pop_back();
        iter = segments->erase(iter);
      } else {
        ++iter;
      }
    }
  }

当没有下发新指令时

ExtendReferenceLine函数

auto prev_segment = route_segments_.begin();
auto prev_ref = reference_lines_.begin();

prev_segment:是上一帧生成的route_segments
prev_ref:是上一帧生成的reference_lines
route_segments_和reference_lines_是通过UpdateReferenceLine函数进行更新的,这个下面会进行介绍.

  while (prev_segment != route_segments_.end()) {
    if (prev_segment->IsConnectedSegment(*segments)) {
      break;
    }
    ++prev_segment;
    ++prev_ref;
  }

判断当前的route_segment与上一帧的route_segment是否有连接

  if (prev_segment == route_segments_.end()) {
    if (!route_segments_.empty() && segments->IsOnSegment()) {
      AWARN << "Current route segment is not connected with previous route "
               "segment";
    }
    return SmoothRouteSegment(*segments, reference_line);
  }

如果当前route_segment就是自车所在的route_segment,并且当前route_segment与上一帧的route_segment没有连接关系,说明定位发生了跳变或切换车道了,这个时候就会使用当前的route_segment数据,平滑出一条新的参考线.
如果当前route_segment与上一帧的route_segment存在连接关系

prev_segment->GetProjection(vec2d, state.heading(), &sl_point,
                                   &waypoint)

就会获取当前车辆位置在上一帧route_segment上的waypoint数据和横纵向距离

  const double prev_segment_length = RouteSegments::Length(*prev_segment);
  const double remain_s = prev_segment_length - sl_point.s();
  const double look_forward_required_distance =
      planning::PncMapBase::LookForwardDistance(state.linear_velocity());
  if (remain_s > look_forward_required_distance) {
    *segments = *prev_segment;
    segments->SetProperties(segment_properties);
    *reference_line = *prev_ref;
    ADEBUG << "Reference line remain " << remain_s
           << ", which is more than required " << look_forward_required_distance
           << " and no need to extend";
    return true;
  }

remain_s:是上一帧route_segment的长度减去当前自车在上一帧route_segment投影点的纵向位置,等于上一帧route_segment剩下可行驶区间的距离,如果这个距离比look_forward_required_distance大,就不需要再去延伸参考线长度了,直接复用上一帧的参考线.

double future_start_s =
      std::max(sl_point.s(), prev_segment_length -
                                 FLAGS_reference_line_stitch_overlap_distance);
  double future_end_s =
      prev_segment_length + FLAGS_look_forward_extend_distance;

重新计算自车前后要生成参考线的区间

current_pnc_map_->ExtendSegments(*prev_segment, future_start_s,
                                        future_end_s, &shifted_segments)

按照上面的参考线区间扩展route_segment,ExtendSegments函数前面已经介绍过ExtendSegments函数.

  if (prev_segment->IsWaypointOnSegment(shifted_segments.LastWaypoint())) {
    *segments = *prev_segment;
    segments->SetProperties(segment_properties);
    *reference_line = *prev_ref;
    ADEBUG << "Could not further extend reference line";
    return true;
  }

如果扩展后,发现新的route_segment的终点的lane waypoint依旧在上一帧的route segment内,则直接返回上一帧的参考线,说明此时map数据无法再进行扩展了.

SmoothPrefixedReferenceLine函数

SmoothPrefixedReferenceLine(*prev_ref, new_ref, reference_line)

与SmoothReferenceLine函数相似,用于平滑参考线,同样是采样获取锚点,然后平滑参考线,不同的是SmoothPrefixedReferenceLine函数多了一个参数prefix_ref是上一帧参考线,并且多了下面逻辑

for (auto& point : anchor_points) {
    common::SLPoint sl_point;
    if (!prefix_ref.XYToSL(point.path_point, &sl_point)) {
      continue;
    }
    if (sl_point.s() < 0 || sl_point.s() > prefix_ref.Length()) {
      continue;
    }
    auto prefix_ref_point = prefix_ref.GetNearestReferencePoint(sl_point.s());
    point.path_point.set_x(prefix_ref_point.x());
    point.path_point.set_y(prefix_ref_point.y());
    point.path_point.set_z(0.0);
    point.path_point.set_theta(prefix_ref_point.heading());
    point.longitudinal_bound = 1e-6;
    point.lateral_bound = 1e-6;
    point.enforced = true;
    break;
  }

在新参考线平滑前,把新参考线平滑前的第一个锚点坐标,设置为第一个锚点在上一帧参考线上的投影点的坐标,这样平滑后的新参考线就能与上一帧的参考线连接起来.

reference_line->Stitch函数

主要作用:
当新参考线的第一个参考点在上一帧参考线上的投影点,在上一帧参考线的长度范围内,则将投影点之前的上一帧参考点插入到新参考线参考点的前面
当新参考线的最后一个参考点在上一帧参考线上的投影点,在上一帧参考线的长度范围内,则将投影点之后的上一帧参考点插入到新参考线参考点的后面
然后构成新参考线的新的reference_points_,赋值给map_path_.

shifted_segments.Stitch函数

这个函数是针对lane segment的拼接,上图绿色点分割的是不同的lane,黑色是上一帧route segment,灰色是新参考线的route segment.
当新参考线的第一个lane waypoint的位置是在上一帧参考线的某一lane waypoint的可行驶区间范围内,就会根据上一帧参考线的这个lane waypoint的可行驶区间来调整新参考线的第一个lane waypoint的可行驶区间,并且将上一帧参考线的这个lane waypoint之前的所有lane waypoint插入到新的参考线的route segment中,如上图桔黄色
当新参考线的最后一个lane waypoint的位置是在上一帧参考线的某一lane waypoint的可行驶区间范围内,就会根据上一帧参考线的这个lane waypoint的可行驶区间来调整新参考线的最后一个lane waypoint的可行驶区间,并且将上一帧参考线的这个lane waypoint之后的所有lane waypoint插入到新的参考线的route segment中,如上图桔黄色

无论是reference_line->Stitch函数还是shifted_segments.Stitch函数都是为了,当前参考线与上一帧参考线衔接的更加平滑.

Shrink(sl, reference_line, segments)

最后,对新的参考线进行裁剪
到这ExtendReferenceLine函数就分析完了,从而CreateReferenceLine函数也分析完了.

UpdateReferenceLine函数

if (reference_lines_.size() != reference_lines.size()) {
    reference_lines_ = reference_lines;
    route_segments_ = route_segments;
  }

如果参考线size不同,直接更新缓存

else {
    auto segment_iter = route_segments.begin();
    auto internal_iter = reference_lines_.begin();
    auto internal_segment_iter = route_segments_.begin();
    for (auto iter = reference_lines.begin();
         iter != reference_lines.end() &&
         segment_iter != route_segments.end() &&
         internal_iter != reference_lines_.end() &&
         internal_segment_iter != route_segments_.end();
         ++iter, ++segment_iter, ++internal_iter, ++internal_segment_iter) {
      if (iter->reference_points().empty()) {
        *internal_iter = *iter;
        *internal_segment_iter = *segment_iter;
        continue;
      }
      if (common::util::SamePointXY(
              iter->reference_points().front(),
              internal_iter->reference_points().front()) &&
          common::util::SamePointXY(iter->reference_points().back(),
                                    internal_iter->reference_points().back()) &&
          std::fabs(iter->Length() - internal_iter->Length()) <
              common::math::kMathEpsilon) {
        continue;
      }
      *internal_iter = *iter;
      *internal_segment_iter = *segment_iter;
    }
  }

如果参考线size相同,则比较当前参考线与缓存中参考线的第一个参考点,最后一个参考点,和长度.其中之一不同则更新缓存.

  reference_line_history_.push(reference_lines_);
  route_segments_history_.push(route_segments_);
  static constexpr int kMaxHistoryNum = 3;
  if (reference_line_history_.size() > kMaxHistoryNum) {
    reference_line_history_.pop();
    route_segments_history_.pop();
  }

更新reference_line_history_/route_segments_history_,如果reference_line_history_的size大于阈值默认是3,则删除旧的历史数据.
到这UpdateReferenceLine函数就分析完了,整个参考线相关的内容也就更新完了.