Apollo 9.0 参考线生成器 -- ReferenceLineProvider

在基于参考线的规划算法中，参考线是Planning一切工作的基础和前提，所以搞清楚参考线怎么来的就变得很重要。为此，首先需要了解Planning 与 Routing的交互，通过交互得到路径片段，这些路径片段就告诉我们该怎么走，再在此基础上生成参考线，最后使用不同的平滑算法对参考线进行平滑，得到一条光滑的参考线。

1. Planning 与 Routing交互

Routing模块，即路由，是进行全局路径规划的地方，是和Planning平级的模块。Routing模块接收RoutingRequest输入，通过A*算法搜索，得到从起点到终点的全局最优路径，最后通过RoutingResponse中将最优路径的信息反馈给请求模块Planning。需要说明的是，这是车道级别的路径。
下面是Apollo收到一个导航请求时的处理流程，用户在Dreamview上通过鼠标点选和车辆当前的位置（实车应该是在车机中控大屏导航地图上输入起点和终点，以及途经点），得到起点和终点的位姿，经过在地图上映射，得到RoutingRequest信息，其中包含了起点和终点在最近车道上的对应点。

1.1 路由请求RoutingRequest

file in apollo/modules/common_msgs/routing_msgs/routing.proto

message RoutingRequest {
  optional apollo.common.Header header = 1;
  // at least two points. The first is start point, the end is final point.
  // The routing must go through each point in waypoint.
  repeated LaneWaypoint waypoint = 2;  // 路由的起点、终点、途经点
  repeated LaneSegment blacklisted_lane = 3;
  repeated string blacklisted_road = 4;
  optional bool broadcast = 5 [default = true];
  optional apollo.routing.ParkingInfo parking_info = 6 [deprecated = true];
  // If the start pose is set as the first point of "way_point".
  optional bool is_start_pose_set = 7 [default = false];
}

file in apollo/modules/common_msgs/routing_msgs/geometry.proto

message LaneWaypoint {
  optional string id = 1;
  optional double s = 2;
  optional apollo.common.PointENU pose = 3;
  // When the developer selects a point on the dreamview route editing
  // the direction can be specified by dragging the mouse
  // dreamview calculates the heading based on this to support construct lane way point with heading
  optional double heading = 4;
}

message LaneSegment {
  optional string id = 1;
  optional double start_s = 2;
  optional double end_s = 3;
}

从上述的protobuf定义内容可以看到:

1. RoutingRequest里面的waypoint(LaneWaypoint类型)是路径查询的核心，使用repeated形式表明这样的路由点不止1个；

例如：大雄要查询从家A到学校B的一条路径，那么waypoint就是两个；如果大雄要查询家A到学校B的一条路径，并且还要经过静香家C，那么最终的waypoint就是三个。

2. LaneSegment和Prediction中的LaneSegment一样，定义了一条车道的 [start_s, end_s] 路段区间，使用repeated形式可以完整的离散化定义一条车道。
3. LaneWaypoint可以定义车道上的任意一点，包括所在车道id，所在车道的累计距离s，以及世界坐标系下的坐标pose。

/// file in apollo/modules/common/proto/geometry.proto

message PointENU {
  optional double x = 1 [default = nan];  // East from the origin, in meters.
  optional double y = 2 [default = nan];  // North from the origin, in meters.
  optional double z = 3 [default = 0.0];  // Up from the WGS-84 ellipsoid, in meters.
}

1.2 路由响应RoutingResponse

file in apollo/modules/common_msgs/planning_msgs/planning_command.proto

message PlanningCommand {
  optional apollo.common.Header header = 1;
  // Unique identification for command.
  optional int64 command_id = 2 [default = -1];
  // Move along the lanes on map.
  optional apollo.routing.RoutingResponse lane_follow_command = 3;
  // Target speed in command.
  optional double target_speed = 4;
  // Indicate if the command is a motion command.
  optional bool is_motion_command = 5 [default = false];
  oneof command {
    // Move to the given pose with open space planning trajectory.
    ParkingCommand parking_command = 6;
    // Custom command defined by user for extensibility.
    google.protobuf.Any custom_command = 7;
  }
}

Planning 模块接收外部命令PlanningCommand，其中最重要的是路由响应RoutingResponse。

file in apollo/modules/common_msgs/routing_msgs/routing.proto

message RoutingResponse {
  optional apollo.common.Header header = 1;
  repeated RoadSegment road = 2;                // 核心：路径
  optional Measurement measurement = 3;         // 行驶距离
  optional RoutingRequest routing_request = 4;  // 发出的查询
  // the map version which is used to build road graph
  optional bytes map_version = 5;
  optional apollo.common.StatusPb status = 6;
}

file in apollo/modules/common_msgs/routing_msgs/geometry.proto

message Measurement { // 行驶距离
  optional double distance = 1;
}

enum ChangeLaneType {
    FORWARD = 0;
    LEFT = 1;
    RIGHT = 2;
};


message Passage {
   repeated LaneSegment segment = 1;  // 一条车道由多个LaneSegment组成
   optional bool can_exit = 2;
   optional ChangeLaneType change_lane_type = 3 [default = FORWARD];
}

message RoadSegment {
  optional string id = 1;
  repeated Passage passage = 2;  // 多条车道
}

以上是路径查询的返回/响应结果RoutingResponse，其中routing_request是对应发出的查询，measurement是行驶距离，最核心的内容就是road(repeated RoadSegment)，这是一条从起点家A到终点学校B，并且经过中间静香家C的完整路径，由一段段离散化的RoadSegment组成。

RoadSegment类型包含了repeated Passage，这意味着，一个RoadSegment中包含了多个通道，每个通道可以理解为一条车道，一个道路段RoadSegment可以有多条并行向前行驶的车道。而Passage中每条车道可以有多个LaneSegment组成，意味着进一步划分成小的区间段，便于精细化调度。

最终的可视化结果如下图所示

RoutingResponse中最重要的数据是RoadSegment road，这个数据一共有三层：

• RoadSegment：描述道路，一条道路可能包含了并行的几条通路（Passage）。
  • Passage 1：描述通路，通路是直连不含变道的可行驶区域。一个通路可能包含了前后连接的多个车道。
    • LaneSegment 1：描述车道，车道是道路中的一段，自动驾驶车辆会尽可能沿着车道的中心线行驶。
    • LaneSegment 2
    • LaneSegment x
  • Passage 2
  • Passage x
    另外需要注意的是Passage中的两个属性:
• can_exit : 代表能否从当前通道去下一个通道。用于判断将来是否需要变道。
• change_lane_type : 代表当前通道是直行（FORWARD），左转（LEFT）还是右转（RIGHT）。

下面使用cyber_monitor 命令查看 /apollo/planning/command 话题的结果，该结果对应下图仿真场景。该waypoint对应我在地图上选取的终点，该passage对应当前时刻ADC所处位置的passage，该通路要求右转。

如图6中，Routing搜索到从车辆起点到终点的结果用红线标出。

• 从起点到终点一共有8条RoadSegment，除了Road4为4车道外，其他的RoadSegment都是3车道；
• RoutingResponse中一共由4个Passage组成。
  • Passage1：lane2->lane5；
  • Passage2：lane6->lane10->lane14->lane15->lane16->lane19
  • Passage3：lane20->lane23
  • Passage4：lane21->lane24

注意：RoutingResponse中包含了从起点到终点的唯一最优路径方案，其他的车道如lane1，lane4等并不包含在RoutingResponse范围中。下文中所指的换道，也是在这个RoutingResponse范围中进行换道。如果因为障碍物等原因，车辆需要切换到RoutingResponse范围之外，称为借道，等借道完成绕开障碍物之后，车辆还需要回到原来的车道线上。

1.3 换道过程

Apollo planning模块接收到RoutingResponse之后，首先进行结果保存和转换，然后通过PncMap::GetRouteSegments函数，在当前车辆行驶范围内，进行短期路径查询，获取RouteSegments，转换成ReferenceLine后进行平滑，得到最终的ReferenceLine列表。
所以在Apollo规划中，并不是有些人想的那样，是可以通过主动请求来换道的，而是在规划过程中计算是否需要换道。
粗略理解为在车辆行驶前后一定范围内，每条Passage在这个范围内的线段对应一个ReferenceLine。

如下图所示，绿框是车辆当前位置，此时在短期范围内一共有两条ReferenceLine，第一条是在车辆当前所在的Passage1，因为与车辆所在车道相同，所以将其判断为“直行车道”；另外一条在Passage2，车辆需要右转才能切换到这条参考线上，因为判断为“换道车道”。

Planning会对这两条参考线都进行路径和速度的决策和优化，然后根据这两条路径的Cost选择最优并且路径最长的那条。

首先直行的Cost会比换道的Cost要低，所有优先选择直行。当车辆短期范围包含了直行车道的终点时，因为换道车道对应的参考线较长，所以会选择换道车道，这就完成了换道的决策。

apollo/modules/planning/planning_component.cc

Status OnLanePlanning::Plan(const double current_time_stamp,
                            const std::vector<TrajectoryPoint>& stitching_trajectory,
                            ADCTrajectory* trajectory_pb) {

  // 调用PublicRoadPlanner::Plan()，进行轨迹规划    planning_start_point = stitching_trajectory.back()
  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(), ptr_trajectory_pb);

  ExportReferenceLineDebug(ptr_debug);

  // 从可用参考线信息集获取最优的参考线信息 best_reference_line，也就是可能规划了多条路径，选择最优的路径
  const auto* best_ref_info = frame_->FindDriveReferenceLineInfo();
  if (!best_ref_info) {
    std::string msg("planner failed to make a driving plan");
    AERROR << msg;
    if (last_publishable_trajectory_) {
      last_publishable_trajectory_->Clear();
    }
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

apollo/modules/planning/planning_base/common/frame.cc

// 基于多条参考线规划了多条轨迹，选择其中一条轨迹
const ReferenceLineInfo *Frame::FindDriveReferenceLineInfo() {
  double min_cost = std::numeric_limits<double>::infinity();
  drive_reference_line_info_ = nullptr;

  for (const auto &reference_line_info : reference_line_info_) {
    if (reference_line_info.IsDrivable() &&
        reference_line_info.Cost() < min_cost) { // 寻找cost最小的参考线。
      drive_reference_line_info_ = &reference_line_info;
      min_cost = reference_line_info.Cost();
    }
  }
  return drive_reference_line_info_;
}

2. 创建参考线线程

Apollo 的Planning中有2个线程：规划线程和参考线提供线程。

规划线程：又称主线程，负责每周期具体的轨迹规划任务；
参考线提供线程：又称参考线线程，负责定期更新参考线，提供给主线程使用。

创建参考线线程在Planning第一步初始化中一次完成，主要包含两步：

• 创建参考线生成器：ReferenceLineProvider；
• 启动参考线线程：reference_line_provider_->Start()。

2.1 创建参考线生成器

首先是创建参考线生成器 – ReferenceLineProvider，这一步使用了智能指针直接调用构造函数。

配置文件中记录了pnc map的类名：“apollo::planning::LaneFollowMap”，这是ReferenceLineProvider构造函数中需要的参数。至于什么是pnc map，什么又是LaneFollowMap，它们和高精度地图HDmap又有什么关系，在Apollo 9.0 Planning 中重要的数据结构中有讲过。

在ReferenceLineProvider构造函数中：

• 首先解析配置文件，将配置参数保存到smoother_config_中。FLAGS_smoother_config_filename为gflags定义的全局变量，如果没有在modules/planning/conf/planning.conf中定义 smoother_config_就会使用使用 modules/planning/common/planning_gflags.cc中的默认值。

ReferenceLineProvider::ReferenceLineProvider(const common::VehicleStateProvider *vehicle_state_provider,
                                             const ReferenceLineConfig *reference_line_config,
                                             const std::shared_ptr<relative_map::MapMsg> &relative_map)
    : vehicle_state_provider_(vehicle_state_provider) {
    ...   
    
      ACHECK(cyber::common::GetProtoFromFile(FLAGS_smoother_config_filename,
                                             &smoother_config_))
      << "Failed to load smoother config file "
      << FLAGS_smoother_config_filename;
      
    ...   
}

modules/planning/conf/planning.conf中定义了使用：smoother_config_discrete_points_smoother_config.pb.txt，也就是离散点平滑器配置参数，下一篇参考线平滑算法（Apollo 9.0 参考线平滑算法 – Discrete Points ReferenceLine Smoother）中将主要以这种平滑算法为主来讲解。

• 根据配置，创建参考线平滑器。这里使用了多态：父类指针指向子类对象。

ReferenceLineProvider::ReferenceLineProvider(const common::VehicleStateProvider *vehicle_state_provider,
                                             const ReferenceLineConfig *reference_line_config,
                                             const std::shared_ptr<relative_map::MapMsg> &relative_map)
    : vehicle_state_provider_(vehicle_state_provider) {
    ...   
    
  if (smoother_config_.has_qp_spline()) {
    smoother_.reset(new QpSplineReferenceLineSmoother(smoother_config_));       // 样条曲线
  } else if (smoother_config_.has_spiral()) {
    smoother_.reset(new SpiralReferenceLineSmoother(smoother_config_));         // 螺旋线
  } else if (smoother_config_.has_discrete_points()) {
    smoother_.reset(new DiscretePointsReferenceLineSmoother(smoother_config_)); // 离散点
  } else {
    ACHECK(false) << "unknown smoother config "
                  << smoother_config_.DebugString();
  }
  
    ...   
}

• 加载pnc map插件。根据传入的reference_line_config，创建插件对象。

ReferenceLineProvider::ReferenceLineProvider(const common::VehicleStateProvider *vehicle_state_provider,
                                             const ReferenceLineConfig *reference_line_config,
                                             const std::shared_ptr<relative_map::MapMsg> &relative_map)
    : vehicle_state_provider_(vehicle_state_provider) {
    ...   
    
    // Load pnc map plugins.
  pnc_map_list_.clear();
  // Set "apollo::planning::LaneFollowMap" as default if pnc_map_class is empty.
  if (nullptr == reference_line_config ||
      reference_line_config->pnc_map_class().empty()) {
    const auto &pnc_map = apollo::cyber::plugin_manager::PluginManager::Instance()
                          ->CreateInstance<planning::PncMapBase>("apollo::planning::LaneFollowMap");
    pnc_map_list_.emplace_back(pnc_map);
  } else {
    const auto &pnc_map_names = reference_line_config->pnc_map_class();
    for (const auto &map_name : pnc_map_names) {
      const auto &pnc_map = apollo::cyber::plugin_manager::PluginManager::Instance()
                            ->CreateInstance<planning::PncMapBase>(map_name);
      pnc_map_list_.emplace_back(pnc_map);
    }
  }
  
    ...   
}

2.2 启动参考线线程

ReferenceLineProvider对象创建好后，会马上调用它的成员函数Start()，创建一个参考线线程（如果配置使用多线程的话）。参考线线程开启开关通过gflags定义FLAGS_enable_reference_line_provider_thread，在modules/planning/common/planning_gflags.cc中，默认开启。cyber::Async 允许异步执行一个函数，将任务放入后台线程执行。这样就会有一个单独的线程，调用ReferenceLineProvider::GenerateThread()生成参考线。这个线程我们可以称为参考线线程。

3. 参考线周期生成

在《Apollo 9.0 Planning 初始化都做了什么？》中，我们了解到每一帧规划所需的参考线都是在OnLanePlanning::InitFrame()函数中更新得到。

于是，我们在ReferenceLineProvider::GetReferenceLines()打一个断点进入调试，可以看到函数调用栈如下，可以看到在多线程环境下，主线程只在这个函数下，就完成了参考线的更新，该函数也仅仅只是将计算好的参考线赋值一下，并没有实际的计算过程。

那么参考线又是怎么计算的呢？上面代码可以看到，如果不使用多线程，即FLAGS_enable_reference_line_provider_thread为false，可以调用CreateReferenceLine(reference_lines, segments)来创建参考线，接着调用UpdateReferenceLine(*reference_lines, *segments)来更新参考线，这样同样可以得到最新的参考线。
我们在ReferenceLineProvider::CreateReferenceLine()打一个断点进入调试，可以看到函数调用栈如下，可以看到在多线程环境下，参考线线程的调用情况。

接下来，我们看ReferenceLineProvider::GenerateThread()里面是如何周期性调用生成参考线的。
这里还是分为两步，与非多线程一样：

• 调用CreateReferenceLine(reference_lines, segments)来创建参考线，
• 接着调用UpdateReferenceLine(*reference_lines, *segments)来更新参考线。

3.1 创建参考线

• 首先，是准备预处理。更新车辆状态vehicle_state和规划命令command，判断pnc map是否为空。

bool ReferenceLineProvider::CreateReferenceLine(std::list<ReferenceLine> *reference_lines,
                                                std::list<hdmap::RouteSegments> *segments) {
    ...
  common::VehicleState vehicle_state;
  {
    std::lock_guard<std::mutex> lock(vehicle_state_mutex_);
    vehicle_state = vehicle_state_;
  }

  planning::PlanningCommand command;
  {
    std::lock_guard<std::mutex> lock(routing_mutex_);
    command = planning_command_;
  }
  if (nullptr == current_pnc_map_) {
    AERROR << "Current pnc map is null! " << command.DebugString();
    return false;
  }
    ...
}

• 然后获取当前全局的导航路径。全局导航路径段通过pnc map获取。

bool ReferenceLineProvider::CreateReferenceLine(std::list<ReferenceLine> *reference_lines,
                                                std::list<hdmap::RouteSegments> *segments) {
    ...
    if (!CreateRouteSegments(vehicle_state, segments)) {
        AERROR << "Failed to create reference line from routing";
        return false;
    }
    ...
}
bool ReferenceLineProvider::CreateRouteSegments(const common::VehicleState &vehicle_state,
                                                std::list<hdmap::RouteSegments> *segments) {
  {
    std::lock_guard<std::mutex> lock(pnc_map_mutex_);
    if (!current_pnc_map_->GetRouteSegments(vehicle_state, segments)) {
      AERROR << "Failed to extract segments from routing";
      return false;
    }
  }
  ...
}

• 最后，平滑生成参考线。

可以看到 ReferenceLine 是通过遍历segments 来进行生成的，所以segment 的数量以及位置，便决定了参考线ReferenceLine 的数量与位置。对于每个segments ，通过调用参考线平滑方法SmoothRouteSegment(*iter, &reference_lines->back()) 来生成参考线。ReferenceLineProvider::SmoothRouteSegment（）的具体实现将在第4节详细讲解。

bool ReferenceLineProvider::CreateReferenceLine(std::list<ReferenceLine> *reference_lines,
                                                std::list<hdmap::RouteSegments> *segments) {
    ...
  // 判断是否需要进行参考线拼接
  if (is_new_command_ || !FLAGS_enable_reference_line_stitching) { // 首次运行或者新路由，不拼接参考线
    for (auto iter = segments->begin(); iter != segments->end();) {
      reference_lines->emplace_back();

      // 参考线平滑
      // 平滑各路由片段列表segments，并将平滑后的路由片段存储到参考线列表reference_lines中，同时将不能平滑的路由片段从segments中删除；
      if (!SmoothRouteSegment(*iter, &reference_lines->back())) {
        AERROR << "Failed to create reference line from route segments";
        reference_lines->pop_back();
        iter = segments->erase(iter);
      } else {
        common::SLPoint sl;
        if (!reference_lines->back().XYToSL(vehicle_state.heading(), // 计算自车坐标在新参考线上的投影点，并转换成frenet坐标
                                            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;
      }
    }
    is_new_command_ = false;
    return true;
  } else {  // stitching reference line
    for (auto iter = segments->begin(); iter != segments->end();) {
      reference_lines->emplace_back();

      // 合并不同参考线片段的重合部分，并将拼接后的路由片段保存到参考线列表reference_lines中，同时将不能拼接的路由片段从列表segments中删除。
      if (!ExtendReferenceLine(vehicle_state, &(*iter),
                               &reference_lines->back())) {
        AERROR << "Failed to extend reference line";
        reference_lines->pop_back();
        iter = segments->erase(iter);
      } else {
        ++iter;
      }
    }
  }
    ...
}

这里仿真一个变道的场景，通过断点调试，segments是std::listhdmap::RouteSegments *类型，我们对其解引用然后调用.size()函数，可以看到是2，也就是有2段segments，也就是会生成两条参考线。

对于非变道场景，只有1段segments，也就是会生成一条参考线。

3.2 更新参考线

ReferenceLineProvider::UpdateReferenceLine()主要的功能很简单，其实就是将最新计算好的参考线拷贝给成员变量，主线程周期更新参考线的时候就是从成员变量取值。

void ReferenceLineProvider::UpdateReferenceLine(const std::list<ReferenceLine> &reference_lines,
                                                const std::list<hdmap::RouteSegments> &route_segments) {
  if (reference_lines.size() != route_segments.size()) {
    AERROR << "The calculated reference line size(" << reference_lines.size()
           << ") and route_segments size(" << route_segments.size()
           << ") are different";
    return;
  }
  if (reference_lines.empty()) {
    return;
  }
  std::lock_guard<std::mutex> lock(reference_lines_mutex_);

  // 将最新计算好的参考线拷贝给成员变量
  if (reference_lines_.size() != reference_lines.size()) {
    reference_lines_ = reference_lines;
    route_segments_ = route_segments;
  } 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;            // 将 iter 所指向的数据复制到 internal_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;
    }
  }
  // update history
  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();
  }
}

4. 参考线平滑

在Apollo的路径规划中，其采用的是基于Frenet坐标系下的规划算法，因此非常依赖于道路中心线的平滑性。而实际工程中，高精地图给出的道路中心线的平滑性往往都不能满足规划模块的需求，因此规划中是不能拿来直接用的，需要对其进行平滑操作。

上面我们提到了有多少段segments就会有多少条参考线，就会调用多次ReferenceLineProvider::SmoothRouteSegment()。实际上参考下平滑是由完成的。bool ReferenceLineProvider::SmoothReferenceLine()。主要包含两步：

• 设置中间点anchor points
• 平滑函数平滑
  

4.1 设置中间点anchor points

• 首先第一步就是根据原始的reference_line来选取中间点，其实这一步就是根据reference_line的s值进行采样，采样的间隔为0.25m。

void ReferenceLineProvider::GetAnchorPoints(const ReferenceLine &reference_line,
                                            std::vector<AnchorPoint> *anchor_points) const {
    ...
  // interval为采样间隔，默认max_constraint_interval=0.25，即路径累积距离每0.25m采样一个点。
  const double interval = smoother_config_.max_constraint_interval();

  // 路径采样点数量计算
  int num_of_anchors = std::max(2, static_cast<int>(reference_line.Length() / interval + 0.5));

  // uniform_slice函数就是对[0, len]区间等间隔采样，每两个点之间距离为(length_-0.0)/(num_of_anchors - 1)
  std::vector<double> anchor_s;
  common::util::uniform_slice(0.0, reference_line.Length(), num_of_anchors - 1, &anchor_s);

    ...
}

采样完毕后，就能得到一系列的anchor_s。说白了就是对s进行离散。

• 然后通过等间距的s得到一系列的anchor_point。其中需要根据自车宽度，车道边界类型（是否为curb）等对横向裕度进行收缩。AnchorPoint结构中的enforced变量表示该点是否为强约束，在参考线平滑中只有首尾两点为强约束。

void ReferenceLineProvider::GetAnchorPoints(const ReferenceLine &reference_line,
                                            std::vector<AnchorPoint> *anchor_points) const {
    ...
  // 计算采样点的坐标(x,y)，并进行矫正
  for (const double s : anchor_s) {
    AnchorPoint anchor = GetAnchorPoint(reference_line, s);
    anchor_points->emplace_back(anchor);
  }

    ...
}

每个anchor_point就包含了一个path_point以及横纵向的裕度。

横纵向裕度的配置参数为：

这里只对首尾两个点设置为强约束，也就是必须遵守横纵向裕度，其他点不设置。

裕度：采样时，这个点周围可以去平移的空间

void ReferenceLineProvider::GetAnchorPoints(const ReferenceLine &reference_line,
                                            std::vector<AnchorPoint> *anchor_points) const {
    ...
  // 参考线首尾两点设置强约束
  anchor_points->front().longitudinal_bound = 1e-6;
  anchor_points->front().lateral_bound = 1e-6;
  anchor_points->front().enforced = true;
  anchor_points->back().longitudinal_bound = 1e-6;
  anchor_points->back().lateral_bound = 1e-6;
  anchor_points->back().enforced = true;

    ...
}

• 最后将计算完的anchor points赋值到smoother对象中

bool ReferenceLineProvider::SmoothReferenceLine(const ReferenceLine &raw_reference_line, 
                                                ReferenceLine *reference_line) {
    ...
    smoother_->SetAnchorPoints(anchor_points);

    ...
}

4.2 平滑算法平滑

设置好中间点后，接下来就是根据配置文件配置的平滑算法对参考线进行平滑。
smoother_->Smooth(raw_reference_line, reference_line)
目前Apollo中共有三种参考线平滑算法，分别为：

• QPSpline Smoother 五次多项式平滑
• SpiralReferenceLine Smoother 五次螺旋曲线平滑
• DiscretePoints Smoother 离散点平滑
  • CosThetaSmooth
  • FemPosSmooth
    • SqpWithOsqp 考虑参考线的曲率约束，其优化问题是非线性，进行线性化后使用osqp求解器
    • NlpWithIpopt 考虑参考线的曲率约束，其优化问题是非线性，ipopt非线性求解器求解（内点法）
    • QpWithOsqp 不考虑曲率约束（默认），直接用osqp求解二次规划问题

Apollo默认采用QpWithOsqp离散点平滑算法，受篇幅限制，详见下一篇参考线平滑算法（Apollo 9.0 参考线平滑算法 – Discrete Points ReferenceLine Smoother）