​

上一篇planning模块(3)-参考线的生成 我们已经介绍了基于全局路线参考线的生成过程,本篇将介绍的是参考线的平滑,主要是使用二次规划进行平滑.

通过CreateRouteSegments函数获取到的segments中,包含两条扩展后的参考线,第一条是自车所在当前车道前后的参考线,第二条是自车在换道边投影点前后的参考线.

SmoothRouteSegment函数

hdmap::Path path(segments);
Path::Path(const std::vector<LaneSegment>& segments)
    : lane_segments_(segments) {
  for (const auto& segment : lane_segments_) {
    const auto points = MapPathPoint::GetPointsFromLane(
        segment.lane, segment.start_s, segment.end_s);
    path_points_.insert(path_points_.end(), points.begin(), points.end());
  }
  MapPathPoint::RemoveDuplicates(&path_points_);
  CHECK_GE(path_points_.size(), 2U);
  Init();
}
577721384d1f4d55b2d698e09c989ab0-VnUf.png

​

for (auto iter = segments->begin(); iter != segments->end();) {
      reference_lines->emplace_back();
      if (!SmoothRouteSegment(*iter, &reference_lines->back())) {

开始遍历扩展参考线

  for (const auto& segment : lane_segments_) {
    const auto points = MapPathPoint::GetPointsFromLane(
        segment.lane, segment.start_s, segment.end_s);
    path_points_.insert(path_points_.end(), points.begin(), points.end());
  }

遍历扩展参考线中的车道

GetPointsFromLane函数

cd685d996b634f0892078d24acd3cc89-vyCc.png

​

lane->points().size():如果按照图示,此时size是4

accumulate_s:0

start_s:假设是10

end_s:假设是170

假设:segment0 length:50, segment1 length:60, segment2 length:70

进入for循环

i = 0

不满足第一个if case

进入

    if (i < lane->segments().size()) {
      const auto& segment = lane->segments()[i];
      const double next_accumulate_s = accumulate_s + segment.length();

segment = segment0

next_accumulate_s = 0 + 50 = 50

      if (start_s > accumulate_s && start_s < next_accumulate_s) {
        points.emplace_back(segment.start() + segment.unit_direction() *
                                                  (start_s - accumulate_s),
                            lane->headings()[i], LaneWaypoint(lane, start_s));
      }

point0:(上图start_s位置的坐标值(采样点), lane->headings()[0], LaneWaypoint(lane0, 10))

accumulate_s = next_accumulate_s = 50

继续for循环,此时i = 1

进入

    if (accumulate_s >= start_s && accumulate_s <= end_s) {
      points.emplace_back(lane->points()[i], lane->headings()[i],
                          LaneWaypoint(lane, accumulate_s));
    }

point1:(point1的坐标值, lane->headings()[1], LaneWaypoint(lane0, 50))

segment = segment1

next_accumulate_s = 50 + 60 = 110

accumulate_s = next_accumulate_s = 110

继续for循环,此时i = 2

point2:(point2的坐标值, lane->headings()[2], LaneWaypoint(lane0, 110))

segment = segment2

next_accumulate_s = 110 + 70 = 180

point3:(上图end_s位置的坐标值(采样点), lane->headings()[2], LaneWaypoint(lane0, 170))

accumulate_s = next_accumulate_s = 180

此时accumulate_s > end_s退出for循环

所以最后获取到的points包含如下point:

point0:(上图start_s位置的坐标值(采样点), lane->headings()[0], LaneWaypoint(lane0, 10))

point1:(point1的坐标值, lane->headings()[1], LaneWaypoint(lane0, 50))

point2:(point2的坐标值, lane->headings()[2], LaneWaypoint(lane0, 110))

point3:(上图end_s位置的坐标值(采样点), lane->headings()[2], LaneWaypoint(lane0, 170))

ba16361c2361480f9a2093cd7a8501e4-qbxI.png

​

也就是上图蓝色框中的点,所以GetPointsFromLane函数的作用就是获取到的当前扩展参考线上所有车道的采样点的坐标或路径point点的坐标,构造成MapPathPoint格式的数据存入points中.

path_points_.insert(path_points_.end(), points.begin(), points.end());

最后,会把扩展参考线上的所有点的信息都存入到path_points_中,这里解释一下,我们之前的例子中是生成了两条扩展参考线,一条是当前车道,一条是换道边.而对于每条扩展参考线都会创建对应的Path,而path_points_是Path的成员变量.

RemoveDuplicates函数

RemoveDuplicates的作用是比较path_points_中的每两个点的距离是否大于设置的阈值,如果大于设置的阈值,则认为是两个点,否则认为是一个点,并把当前的path_point的lane_waypoints设置为索引大的那一个元素的lane_waypoints.

Init函数

InitPoints函数

602c75917b09433cb3e9995a644842f5-QxjZ.png

​

假如num_points_现在的size为5,是自车当前所在车道前后扩展的参考线,分别如上图

初始时accumulated_s_:0

i:0

segments_:(point0, point1)

heading = path_points_[i + 1] - path_points_[i];

heading:point0指向point1的向量的代数表示(坐标表示)

heading_length:是point0和point1的欧几里德距离

d9a758aad20e49dbb9acd3a9085a4966-siBT.png

​

s = 40

      if (heading_length > 0.0) {
        heading /= heading_length;
      }

这里表示point0指向point1方向上的单位向量的代数表示(坐标)

继续for循环,直到不符合条件.

segments_:存的就是point之间的一段

unit_directions_:存的就是每一个segment方向上的单位向量的坐标

accumulated_s_:存的是每一个point的纵向距离

  length_ = s;
  num_sample_points_ = static_cast<int>(length_ / kSampleDistance) + 1;
  num_segments_ = num_points_ - 1;

length_:是第一个point到最后一个point的总长度

num_sample_points_:是采样点的个数,kSampleDistance默认是0.25

num_segments_:segment的个数

InitLaneSegments函数

FindLaneSegment函数

bool FindLaneSegment(const MapPathPoint& p1, const MapPathPoint& p2,
                     LaneSegment* const lane_segment) {
  for (const auto& wp1 : p1.lane_waypoints()) {
    if (nullptr == wp1.lane) {
      continue;
    }
    for (const auto& wp2 : p2.lane_waypoints()) {
      if (nullptr == wp2.lane) {
        continue;
      }
      if (wp1.lane->id().id() == wp2.lane->id().id() && wp1.s < wp2.s) {
        *lane_segment = LaneSegment(wp1.lane, wp1.s, wp2.s);
        return true;
      }
    }
  }
  return false;
}
e1f3f5e11f744a67a1a474858abafff9-FOZN.png

​               

FindLaneSegment函数的作用:

  if (lane_segments_.empty()) {
    for (int i = 0; i + 1 < num_points_; ++i) {
      LaneSegment lane_segment;
      if (FindLaneSegment(path_points_[i], path_points_[i + 1],
                          &lane_segment)) {
        lane_segments_.push_back(lane_segment);
      }
    }
  }

首先,path_points_是一条扩展后的参考线上的所有point的信息如上图(上图只是一条车道),而一条扩展后的参考线是由不同的车道构成,而车道又是由上面的point的构成,FindLaneSegment函数的作用就是通过path_points_中point的id和s纵向距离,通过判断point id(表示一条车道)来将每两个point构造成一个LaneSegment格式的数据.

LaneSegment::Join(&lane_segments_);

Join的作用是将上面一段一段的LaneSegment构造为以一条lane为一个segment的数据

比如上面图示,通过FindLaneSegment函数获取到的lane_segments_就是4段,然后通过Join函数合并成一条车道的segment.

lane_accumulated_s_.resize(lane_segments_.size());
  lane_accumulated_s_[0] = lane_segments_[0].Length();
  for (std::size_t i = 1; i < lane_segments_.size(); ++i) {
    lane_accumulated_s_[i] =
        lane_accumulated_s_[i - 1] + lane_segments_[i].Length();
  }

lane_accumulated_s_存储的是扩展后的参考线上的每条车道的累积长度

InitPointIndex函数

void Path::InitPointIndex() {
  last_point_index_.clear();
  last_point_index_.reserve(num_sample_points_);
  double s = 0.0;
  int last_index = 0;
  for (int i = 0; i < num_sample_points_; ++i) {
    while (last_index + 1 < num_points_ &&
           accumulated_s_[last_index + 1] <= s) {
      ++last_index;
    }
    last_point_index_.push_back(last_index);
    s += kSampleDistance;
  }
  CHECK_EQ(last_point_index_.size(), static_cast<size_t>(num_sample_points_));
}

 

e1f3f5e11f744a67a1a474858abafff9-yEHj.png

​                

f44ebbe6f1e34ce3b6c0fa15e3d358a1-lyOs.png

​

我们可以看到last_point_index_表示的就是路径上采样点的最近路径点索引

比如第一个采样点/第二个采样点/第三个采样点,是在第一个路径点和第二个路径点之间

InitWidth函数

    while (segment_end_s < sample_s && !is_reach_to_end) {
      const auto& cur_point = path_points_[path_point_index];
      cur_waypoint = &(cur_point.lane_waypoints()[0]);
      CHECK_NOTNULL(cur_waypoint->lane);
      segment_start_s = accumulated_s_[path_point_index];
      segment_end_s = segment_start_s + segments_[path_point_index].length();
      if (++path_point_index >= num_points_) {
        is_reach_to_end = true;
      }
    }

segment_start_s:当前segment的可行驶区域起点,以整条扩展参考线为坐标系

segment_end_s:当前segment的可行驶区域结束点,以整条扩展参考线为坐标系

waypoint_s = cur_waypoint->s + sample_s - segment_start_s;

  • cur_waypoint->s：告诉我们这个路径段(以当前车道的起点为起点的累积距离)是从车道的哪个位置开始的

  • (sample_s - segment_start_s)：告诉我们从路径段起点(以扩展参考线为起点的累积距离)到当前采样点走了多远

  • 两者相加：得到当前采样点在车道坐标系中的具体位置

    cur_waypoint->lane->GetWidth(waypoint_s, &left_width, &right_width);
    lane_left_width_.push_back(left_width - cur_waypoint->l);
    lane_right_width_.push_back(right_width + cur_waypoint->l);
    cur_waypoint->lane->GetRoadWidth(waypoint_s, &left_width, &right_width);
    road_left_width_.push_back(left_width - cur_waypoint->l);
    road_right_width_.push_back(right_width + cur_waypoint->l);

假设当前采样点的位置如下红色位置,并且l为0,则lane_left_width_/road_left_width_表示如下

lane_left_width_:行驶的车道左侧的宽度

road_left_width_:马路牙子

1fd252288eff4e24942b1e1185cb7774-Huvi.png

​

InitOverlaps函数

GetAllOverlaps函数

// 遍历路径的所有车道段
double s = 0.0;  // 当前在整体路径中的弧长
for (const auto& lane_segment : lane_segments_) {
    // 获取该车道的所有重叠
    for (const auto& overlap : GetOverlaps_from_lane(*(lane_segment.lane))) {
        // 检查重叠是否在当前车道段内
        if (lane_overlap_info.start_s() <= lane_segment.end_s &&
            lane_overlap_info.end_s() >= lane_segment.start_s) {
            
            // 转换坐标：从车道局部坐标 → 路径全局坐标
            const double ref_s = s - lane_segment.start_s;
            const double adjusted_start_s =
                std::max(lane_overlap_info.start_s(), lane_segment.start_s) + ref_s;
            const double adjusted_end_s =
                std::min(lane_overlap_info.end_s(), lane_segment.end_s) + ref_s;
            
            // 记录重叠（排除自身车道的ID）
            for (const auto& object : overlap->overlap().object()) {
                if (object.id().id() != lane_segment.lane->id().id()) {
                    overlaps_by_id[object.id().id()].emplace_back(adjusted_start_s,
                                                                  adjusted_end_s);
                }
            }
        }
    }
    s += lane_segment.end_s - lane_segment.start_s;  // 更新路径弧长
}
de1b415861474ed6a32ab6160bad0521-gXuu.png

​

  for (auto& overlaps_one_object : overlaps_by_id) {
    const std::string& object_id = overlaps_one_object.first;
    auto& segments = overlaps_one_object.second;
    std::sort(segments.begin(), segments.end());

    const double kMinOverlapDistanceGap = 1.5;  // in meters.
    for (const auto& segment : segments) {
      if (!overlaps->empty() && overlaps->back().object_id == object_id &&
          segment.first - overlaps->back().end_s <= kMinOverlapDistanceGap) {
        overlaps->back().end_s =
            std::max(overlaps->back().end_s, segment.second);
      } else {
        overlaps->emplace_back(object_id, segment.first, segment.second);
      }
    }
  }

上面是合并overlaps中存在重叠的部分

  std::sort(overlaps->begin(), overlaps->end(),
            [](const PathOverlap& overlap1, const PathOverlap& overlap2) {
              return overlap1.start_s < overlap2.start_s;
            });

重叠部分按起始点从小到大排序

这样Init函数就全部分析完了.

​