参考线平滑 - FemPosDeviation算法

FemPosDeviation参考线平滑方法是离散点平滑方法
参考文章：
（1）参考线平滑-FemPosDeviation-OSQP
（2）Planning基础库——散点曲线平滑
（3）参考线平滑-FemPosDeviation-SQP
（4）Apollo planning之参考线平滑算法
（5）解析百度Apollo之参考线与轨迹
（6）百度Apollo代码阅读：参考线平滑FemPosDeviationSmoother

1 参考线的作用

参考线在planning中的作用相当于一个地基，所有决策与优化都是在参考线的基础上进行

在这里插入图片描述
🍎🍉为什么需要参考线🍉🍎
（1）HD map一般都是人为采集离散点，也就使得原始路径不平滑
（2）全局导航的路径过长，障碍物的投影点也可能不唯一
（3）生成一个局部一定长度且光滑的参考线节省算力

2 ReferenceLine数据结构

参考线是根据车辆位置相对局部的一个数据，它包含了车辆前后一定范围内的路径信息。在车辆行驶过程中，Planning会在每一个计算周期中生成ReferenceLine。

// modules/planning/reference_line/reference_line.h

uint32_t priority_ = 0;  // 优先级
struct SpeedLimit { 
  double start_s = 0.0;
  double end_s = 0.0;
  double speed_limit = 0.0;  // unit m/s
};
std::vector<SpeedLimit> speed_limit_; // 限速数据

std::vector<ReferencePoint> reference_points_; // 一系列的点，点包含了位置的信息。因此这些点就是生成车辆行驶轨迹的基础数据
hdmap::Path map_path_;

在这里插入图片描述
说明：
Vec2d描述一个二维的点：

double x_：描述点的x坐标
double y_：描述点的y坐标

MapPathPoint描述了一个地图上的点：

double heading_：描述点的朝向。
std::vector<LaneWaypoint> lane_waypoints_：描述路径上的点。有些车道可能会存在重合的部分，所以地图上的一个点可能同时属于多个车道，因此这里的数据是一个vector结构。

ReferencePoint描述了参考线中的点:

double kappa_：描述曲线的曲率。
double dkappa_：描述曲率的导数。

ReferencePoint中的朝向和曲率直接影响车辆的方向控制。如果ReferenceLine中的ReferencePoint之间存在朝向和曲率大小震动或者数据跳变将可能导致车辆方向盘的相应变化，这种变化对于乘车体验来说是非常糟糕的。

3 参考线处理流程

① 生成参考线，这主要由Routing模块的输出决定
② 参考线平滑，接下来会详细讲解参考线的平滑的算法
在这里插入图片描述

4 参考线平滑算法

4.1 算法分类

Apollo参考线平滑有三种：
① 离散点平滑：Cos和Fem
② 螺线曲线平滑
③ QPSpline平滑

通过配置参数选择要采用的平滑算法

  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();
  }
  is_initialized_ = true;

默认采用离散点平滑算法

4.2 参考线平滑算法流程

输入raw_reference_line，设置中间点（GetAnchorPoints），然后smooth，最后输出

bool ReferenceLineProvider::SmoothReferenceLine(
    const ReferenceLine &raw_reference_line, ReferenceLine *reference_line) {
  if (!FLAGS_enable_smooth_reference_line) {
    *reference_line = raw_reference_line;
    return true;
  }
  // 设置中间点
  std::vector<AnchorPoint> anchor_points;
  GetAnchorPoints(raw_reference_line, &anchor_points);
  smoother_->SetAnchorPoints(anchor_points);
  if (!smoother_->Smooth(raw_reference_line, reference_line)) {
    AERROR << "Failed to smooth reference line with anchor points";
    return false;
  }
  if (!IsReferenceLineSmoothValid(raw_reference_line, *reference_line)) {
    AERROR << "The smoothed reference line error is too large";
    return false;
  }
  return true;
}

AnchorPoint

struct AnchorPoint {
  common::PathPoint path_point;
  double lateral_bound = 0.0; // 裕度（留有一定余地）
  double longitudinal_bound = 0.0; // 裕度
  // 强制更平滑以严格遵循此参考点
  bool enforced = false;
};

根据原始的参考线来选取中间点

根据referenceline的S值在纵向的投影进行一个均匀的采样，采样的间隔大概是0.25米，采样完毕后就能得到一系列的AnchorPoint，每个AnchorPoint包含了一个path_point和一个横纵向的裕度(采样点周围可以平移的空间)。强约束表示必须必须严格遵守横纵向的裕度，所以在平滑过程中只有首尾两个点是强约束，中间的点都不是强约束。

默认配置：纵向边界(2.0)，横向边界(最大值0.5，最小值0.1)

max_constraint_interval : 0.25
longitudinal_boundary_bound : 2.0
max_lateral_boundary_bound : 0.5
min_lateral_boundary_bound : 0.1
curb_shift : 0.2
lateral_buffer : 0.2

Smooth

bool status = false;
 
const auto& smoothing_method = config_.discrete_points().smoothing_method();
std::vector<std::pair<double, double>> smoothed_point2d;
switch (smoothing_method) {
  case DiscretePointsSmootherConfig::COS_THETA_SMOOTHING:
    status = CosThetaSmooth(raw_point2d, anchorpoints_lateralbound,
                            &smoothed_point2d);
    break;
  case DiscretePointsSmootherConfig::FEM_POS_DEVIATION_SMOOTHING:
    status = FemPosSmooth(raw_point2d, anchorpoints_lateralbound,
                          &smoothed_point2d);
    break;
  default:
    AERROR << "Smoother type not defined";
    return false;
}

if (!status) {
  AERROR << "discrete_points reference line smoother fails";
  return false;

bool DiscretePointsReferenceLineSmoother::FemPosSmooth(
    const std::vector<std::pair<double, double>>& raw_point2d,
    const std::vector<double>& bounds,
    std::vector<std::pair<double, double>>* ptr_smoothed_point2d) {
  const auto& fem_pos_config =
      config_.discrete_points().fem_pos_deviation_smoothing();
 
  FemPosDeviationSmoother smoother(fem_pos_config);
 
  // box contraints on pos are used in fem pos smoother, thus shrink the
  // bounds by 1.0 / sqrt(2.0)
  // 裕度收缩
  std::vector<double> box_bounds = bounds;
  const double box_ratio = 1.0 / std::sqrt(2.0);
  for (auto& bound : box_bounds) {
    bound *= box_ratio;
  }
 
  std::vector<double> opt_x;
  std::vector<double> opt_y;
  // 问题求解
  // 从其输入的参数来看，主要是AnchorPoint中的x，y和横纵向的裕度，输出是平滑后的point
  bool status = smoother.Solve(raw_point2d, box_bounds, &opt_x, &opt_y);
 
  if (!status) {
    AERROR << "Fem Pos reference line smoothing failed";
    return false;
  }
 
  if (opt_x.size() < 2 || opt_y.size() < 2) {
    AERROR << "Return by fem pos smoother is wrong. Size smaller than 2 ";
    return false;
  }

Solve

如果考虑了曲率的约束，该优化问题就是非线性的，就可以用Ipopt非线性求解器进行求解，也可以将曲率约束进行线性化之后用Osqp进行求解。
如果不考虑曲率约束的话，就可以直接用Osqp进行求解该二次优化的问题。

bool FemPosDeviationSmoother::Solve(
    const std::vector<std::pair<double, double>>& raw_point2d,
    const std::vector<double>& bounds, std::vector<double>* opt_x,
    std::vector<double>* opt_y) {
   // 考虑曲率约束
  if (config_.apply_curvature_constraint()) {
    if (config_.use_sqp()) {
      // 线性求解
      return SqpWithOsqp(raw_point2d, bounds, opt_x, opt_y);
    } else {
     // 非线性求解
      return NlpWithIpopt(raw_point2d, bounds, opt_x, opt_y);
    }
  }
  // 不考虑曲率约束 
  else 
  {
    // 线性求解（默认）
    return QpWithOsqp(raw_point2d, bounds, opt_x, opt_y);
  }
  return true;
}

4.3 具体算法原理

对于平滑度的衡量有两种方式：
① FemPosSmooth相对不精准，但是只需用二次规划能快速求解
② CosThetaSmooth相对精准，但是需要非线性规划，计算量大
优化目标：
在这里插入图片描述

在这里插入图片描述

满足的约束：
在这里插入图片描述
二次规划求解首先要将问题转化为二次型式：
$\begin{array}{l} \min f(x)=\frac{1}{2} X^{T} P X+q^{T} X \\ \text { subject to }: l \leq A X \leq u \end{array}$

FemPosDeviation算法使用OSQP二次规划求解器进行求解，其代码实现在modules/planning/math/discretized_points_smoothing/FemPosDeviationOsqpInterface.cc

fem_pos_deviation_osqp_interface中通过 CalculateKernel实现P矩阵，通过CalculateOffset实现q矩阵，通过CalculateAffineConstraint计算系数A
osqp中表阵用了压缩列的系数矩阵csc的格式保存，通过osqp的csc_matrix函数构造出来，其中m为矩阵的行数，n为矩阵的列数，nzmax是非0元素的数目，x为保存的数据对应链接中的ENTRY，p为对应的COL，指出i中第一列对应的位置；i为非零元素所在的行号，对应的ROW。

csc* csc_matrix(c_int    m,
                c_int    n,
                c_int    nzmax,
                c_float *x,
                c_int   *i,
                c_int   *p);

对优化目标函数的实现：

void FemPosDeviationOsqpInterface::CalculateKernel(
    std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
    std::vector<c_int>* P_indptr) {
  CHECK_GT(num_of_variables_, 4);

  // Three quadratic penalties are involved:
  // 1. Penalty x on distance between middle point and point by finite element
  // estimate;
  // 2. Penalty y on path length;
  // 3. Penalty z on difference between points and reference points

  // General formulation of P matrix is as below(with 6 points as an example):
  // I is a two by two identity matrix, X, Y, Z represents x * I, y * I, z * I
  // 0 is a two by two zero matrix
  // |X+Y+Z, -2X-Y,   X,       0,       0,       0    |
  // |0,     5X+2Y+Z, -4X-Y,   X,       0,       0    |
  // |0,     0,       6X+2Y+Z, -4X-Y,   X,       0    |
  // |0,     0,       0,       6X+2Y+Z, -4X-Y,   X    |
  // |0,     0,       0,       0,       5X+2Y+Z, -2X-Y|
  // |0,     0,       0,       0,       0,       X+Y+Z|

  // Only upper triangle needs to be filled
  std::vector<std::vector<std::pair<c_int, c_float>>> columns;
  columns.resize(num_of_variables_);
  int col_num = 0;

  for (int col = 0; col < 2; ++col) {
    columns[col].emplace_back(col, weight_fem_pos_deviation_ +
                                       weight_path_length_ +
                                       weight_ref_deviation_);
    ++col_num;
  }

  for (int col = 2; col < 4; ++col) {
    columns[col].emplace_back(
        col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_);
    columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ +
                                       2.0 * weight_path_length_ +
                                       weight_ref_deviation_);
    ++col_num;
  }

  int second_point_from_last_index = num_of_points_ - 2;
  for (int point_index = 2; point_index < second_point_from_last_index;
       ++point_index) {
    int col_index = point_index * 2;
    for (int col = 0; col < 2; ++col) {
      col_index += col;
      columns[col_index].emplace_back(col_index - 4, weight_fem_pos_deviation_);
      columns[col_index].emplace_back(
          col_index - 2,
          -4.0 * weight_fem_pos_deviation_ - weight_path_length_);
      columns[col_index].emplace_back(
          col_index, 6.0 * weight_fem_pos_deviation_ +
                         2.0 * weight_path_length_ + weight_ref_deviation_);
      ++col_num;
    }
  }

  int second_point_col_from_last_col = num_of_variables_ - 4;
  int last_point_col_from_last_col = num_of_variables_ - 2;
  for (int col = second_point_col_from_last_col;
       col < last_point_col_from_last_col; ++col) {
    columns[col].emplace_back(col - 4, weight_fem_pos_deviation_);
    columns[col].emplace_back(
        col - 2, -4.0 * weight_fem_pos_deviation_ - weight_path_length_);
    columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ +
                                       2.0 * weight_path_length_ +
                                       weight_ref_deviation_);
    ++col_num;
  }

  for (int col = last_point_col_from_last_col; col < num_of_variables_; ++col) {
    columns[col].emplace_back(col - 4, weight_fem_pos_deviation_);
    columns[col].emplace_back(
        col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_);
    columns[col].emplace_back(col, weight_fem_pos_deviation_ +
                                       weight_path_length_ +
                                       weight_ref_deviation_);
    ++col_num;
  }

  CHECK_EQ(col_num, num_of_variables_);

  int ind_p = 0;
  for (int i = 0; i < col_num; ++i) {
    P_indptr->push_back(ind_p);
    for (const auto& row_data_pair : columns[i]) {
      // Rescale by 2.0 as the quadratic term in osqp default qp problem setup
      // is set as (1/2) * x' * P * x
      P_data->push_back(row_data_pair.second * 2.0);
      P_indices->push_back(row_data_pair.first);
      ++ind_p;
    }
  }
  P_indptr->push_back(ind_p);
}

void FemPosDeviationOsqpInterface::CalculateOffset(std::vector<c_float>* q) {
  for (int i = 0; i < num_of_points_; ++i) {
    const auto& ref_point_xy = ref_points_[i];
    q->push_back(-2.0 * weight_ref_deviation_ * ref_point_xy.first);
    q->push_back(-2.0 * weight_ref_deviation_ * ref_point_xy.second);
  }
}

对约束的代码实现：

void FemPosDeviationOsqpInterface::CalculateAffineConstraint(
    std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
    std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,
    std::vector<c_float>* upper_bounds) {
  int ind_A = 0;
  for (int i = 0; i < num_of_variables_; ++i) {
    A_data->push_back(1.0);
    A_indices->push_back(i);
    A_indptr->push_back(ind_A);
    ++ind_A;
  }
  A_indptr->push_back(ind_A);

  for (int i = 0; i < num_of_points_; ++i) {
    const auto& ref_point_xy = ref_points_[i];
    upper_bounds->push_back(ref_point_xy.first + bounds_around_refs_[i]);
    upper_bounds->push_back(ref_point_xy.second + bounds_around_refs_[i]);
    lower_bounds->push_back(ref_point_xy.first - bounds_around_refs_[i]);
    lower_bounds->push_back(ref_point_xy.second - bounds_around_refs_[i]);
  }
}

OSQP还需要设定迭代初值，设定迭代初值为原始参考点坐标

void FemPosDeviationOsqpInterface::SetPrimalWarmStart(
    std::vector<c_float>* primal_warm_start) {
  CHECK_EQ(ref_points_.size(), static_cast<size_t>(num_of_points_));
  for (const auto& ref_point_xy : ref_points_) {
    primal_warm_start->push_back(ref_point_xy.first);
    primal_warm_start->push_back(ref_point_xy.second);
  }

将相关参数输入osqp求解器

data->n = kernel_dim;
data->m = num_affine_constraint;
data->P = csc_matrix(data->n, data->n, P_data->size(), P_data->data(),
                     P_indices->data(), P_indptr->data());
data->q = q->data();
data->A = csc_matrix(data->m, data->n, A_data->size(), A_data->data(),
                     A_indices->data(), A_indptr->data());
data->l = lower_bounds->data();
data->u = upper_bounds->data();

*work = osqp_setup(data, settings);

osqp_warm_start_x(*work, primal_warm_start->data());

// Solve Problem
osqp_solve(*work);

从work中抽取计算结果

x_.resize(num_of_points_);
y_.resize(num_of_points_);
for (int i = 0; i < num_of_points_; ++i) {
	int index = i * 2;
	 x_.at(i) = work->solution->x[index];
	 y_.at(i) = work->solution->x[index + 1];
}