Fast-Planner(三)详解后端B-Spline曲线优化

本文上接Fast-Planner的B-spline曲线生成详解，介绍B-spline曲线优化。如有问题，欢迎各位大佬评论指出，带着我一起进步。

三、B样条优化

初始化获得的B样条曲线只是达到了可达性检测，本章介绍为得到更加光滑安全的轨迹，如何通过优化控制点优化生成的B样条轨迹。

3.1 代价函数

总的代价函数：
$$f_{total}={\lambda }_1{f}_s+{\lambda }_2{f}_c+{\lambda }_3(f_v+f_a)$$
其中$f_s$为平滑度损失，$f_c$为障碍损失，$f_v$和$f_a$分别是速度和加速的的软约束。

3.1.1 平滑度损失

弹性损失方程：
这个函数可以这样子理解，对于控制点$Q_i$,它的前后两个点分别对该点作用一个力即$Q_{i+1}-Q_i$和$Q_{i-1}-Q_i$，合力方向二者相加，合力越小，说明这个点的两个作用力的夹角更近180度且大小差不多相等，这么一来控制点更加平滑。(这属于一种抽象理解，起始可以将加速度控制点由位置控制点结合也能得出这个函数)
代码中似乎没有体现这个函数，如果有大佬懂请在评论区留言

3.1.2 Jerk控制点

论文中提到其他工作采用这一项，本文取而代之用上述平滑度损失，但是在代码中体现了这一块。公式如下：
$$Jer{k}_i=\frac{1}{\Delta t}(A_{i+1}-A_i)=\frac{1}{\Delta t^3}(Q_{i+3}-3Q_{i+2}+3Q_{i+1}-Q_i)$$

/*
  function：
    最小化Jerk输入
  param：
    q:控制点
    cost:损失
    gradient:梯度信息
*/
void BsplineOptimizer::calcSmoothnessCost(const vector<Eigen::Vector3d>& q, double& cost,
                                          vector<Eigen::Vector3d>& gradient) {
  cost = 0.0;
  Eigen::Vector3d zero(0, 0, 0);
  std::fill(gradient.begin(), gradient.end(), zero);
  Eigen::Vector3d jerk, temp_j;
  //遍历控制点求jerk
  for (int i = 0; i < q.size() - order_; i++) {
    /* evaluate jerk */
    jerk = q[i + 3] - 3 * q[i + 2] + 3 * q[i + 1] - q[i];
    cost += jerk.squaredNorm();
    temp_j = 2.0 * jerk;  //cost的一阶导，用于后续梯度更新
    /* jerk gradient */
    gradient[i + 0] += -temp_j;
    gradient[i + 1] += 3.0 * temp_j;
    gradient[i + 2] += -3.0 * temp_j;
    gradient[i + 3] += temp_j;
  }
}

3.1.3 障碍物碰撞惩罚函数

论文中定义为作用于每个控制点的障碍物的排斥力，如下式$d(Q_i)$是$Q_i$与其最近障碍物之间的距离。
$$f_c=\sum _{i=p_b}^{N-p_b}{F}_c(d(Q_i))$$
为保证B样条曲线轨迹的安全性，需要保证位置控制点组成的凸包不与障碍物相重合(即下图中的$d_h>0$)，可以推出$d_h>d_c-(r_{12}+r_{23}+r_{34})$，则如果满足 r j , j + 1 < d c / 3 ( j ∈ { 1 , 2 , 3 } ) r_{j,j+1}<d_c/3(j\in\{1,2,3\}) rj,j+1​<dc​/3(j∈{1,2,3})就能保证凸包不与障碍物碰撞，这样做的好处是能把控制点到障碍物的距离约束导相邻控制点之间的距离。这里论文将$r_{j,j+1}<0.2$做了一个限定(可以调整)。
其中evaluateEDTWithGrad相关函数在地图相关类中

/*
  function：
    计算障碍物距离代价惩罚函数
*/
void BsplineOptimizer::calcDistanceCost(const vector<Eigen::Vector3d>& q, double& cost,
                                        vector<Eigen::Vector3d>& gradient) {
  cost = 0.0;
  Eigen::Vector3d zero(0, 0, 0);
  std::fill(gradient.begin(), gradient.end(), zero);

  double          dist;
  Eigen::Vector3d dist_grad, g_zero(0, 0, 0);
  //检查是否包含ENDPOINT标志，函数根据是否需要考虑路径的终点来调整计算
  //如果包含 ENDPOINT，则处理整个路径点向量；如果不包含，就排除掉最后order_个点，避免在计算中出现索引越界的问题。
  int end_idx = (cost_function_ & ENDPOINT) ? q.size() : q.size() - order_;

  for (int i = order_; i < end_idx; i++) {
    //计算距离梯度dist与距离梯度dist_grad
    edt_environment_->evaluateEDTWithGrad(q[i], -1.0, dist, dist_grad);
    //如果梯度的范数大于阈值，则对梯度进行归一化
    if (dist_grad.norm() > 1e-4) dist_grad.normalize();
    //当路径点距离环境较近时，代价增加，通过梯度的计算，可在后续优化中调整路径点，以增加与环境的距离，从而减少代价。
    if (dist < dist0_) {
      cost += pow(dist - dist0_, 2);
      gradient[i] += 2.0 * (dist - dist0_) * dist_grad;
    }
  }
}

3.1.4 动力学可行性惩罚

为了让每一个超过速度、加速度限制的控制范围进行惩罚，作如下限制，这里的速度并不是合速度作限制，而是三个方向的速度分量分别作限制：

/*
  function：
    计算控制点速度、加速度可行性函数
*/
void BsplineOptimizer::calcFeasibilityCost(const vector<Eigen::Vector3d>& q, double& cost,
                                           vector<Eigen::Vector3d>& gradient) {
  cost = 0.0;
  Eigen::Vector3d zero(0, 0, 0);
  std::fill(gradient.begin(), gradient.end(), zero);
  
  //最大速度、加速度平方，ts时间倒数平方与四次方
  /* abbreviation */
  double ts, vm2, am2, ts_inv2, ts_inv4;
  vm2 = max_vel_ * max_vel_;
  am2 = max_acc_ * max_acc_;

  ts      = bspline_interval_;
  ts_inv2 = 1 / ts / ts;
  ts_inv4 = ts_inv2 * ts_inv2;

  /* velocity feasibility */
  for (int i = 0; i < q.size() - 1; i++) {
    //位置控制点转速度控制点
    Eigen::Vector3d vi = q[i + 1] - q[i];

    for (int j = 0; j < 3; j++) {
      //速度控制点公式带入
      double vd = vi(j) * vi(j) * ts_inv2 - vm2;
      if (vd > 0.0) {
        cost += pow(vd, 2);
        //求导
        double temp_v = 4.0 * vd * ts_inv2;
        gradient[i + 0](j) += -temp_v * vi(j);
        gradient[i + 1](j) += temp_v * vi(j);
      }
    }
  }

  /* acceleration feasibility */
  for (int i = 0; i < q.size() - 2; i++) {
    //位置控制点转速度控制点
    Eigen::Vector3d ai = q[i + 2] - 2 * q[i + 1] + q[i];

    for (int j = 0; j < 3; j++) {
      double ad = ai(j) * ai(j) * ts_inv4 - am2;
      if (ad > 0.0) {
        cost += pow(ad, 2);
        //求导
        double temp_a = 4.0 * ad * ts_inv4;
        gradient[i + 0](j) += temp_a * ai(j);
        gradient[i + 1](j) += -2 * temp_a * ai(j);
        gradient[i + 2](j) += temp_a * ai(j);
      }
    }
  }
}

3.1.5 计算总代价函数和

/*
  优化函数combineCost:
    输入参数：
      1.x为Nlopt优化的变量，应该与三个维度的控制点对应。
      2.grad，总的优化项关于每个优化变量的梯度信息。
      3.f_combine,结合后的cost。
*/
void BsplineOptimizer::combineCost(const std::vector<double>& x, std::vector<double>& grad,
                                   double& f_combine) {
  /* convert the NLopt format vector to control points. */
  // NLopt格式转换到控制点表示，以支持1D至3D的B样条优化
  // This solver can support 1D-3D B-spline optimization, but we use Vector3d to store each control point
  // For 1D case, the second and third elements are zero, and similar for the 2D case.
  for (int i = 0; i < order_; i++) {
    for (int j = 0; j < dim_; ++j) {
      g_q_[i][j] = control_points_(i, j);
    }
    //如果 dim_ 小于3，则剩余的维度设置为0。这是为了处理1D和2D情况。
    for (int j = dim_; j < 3; ++j) {
      g_q_[i][j] = 0.0;
    }
  }
  // 处理由NLopt传入的变量x，将其转换为控制点并存储在g_q_中
  for (int i = 0; i < variable_num_ / dim_; i++) {
    for (int j = 0; j < dim_; ++j) {
      g_q_[i + order_][j] = x[dim_ * i + j];
    }
    for (int j = dim_; j < 3; ++j) {
      g_q_[i + order_][j] = 0.0;
    }
  }
    //考虑终点，如果 cost_function_ 不包含 ENDPOINT 标志，则将 control_points_ 中的最后 order_ 个控制点复制到 g_q_ 的相应位置
  if (!(cost_function_ & ENDPOINT)) {
    for (int i = 0; i < order_; i++) {

      for (int j = 0; j < dim_; ++j) {
        g_q_[order_ + variable_num_ / dim_ + i][j] =
            control_points_(control_points_.rows() - order_ + i, j);
      }
      for (int j = dim_; j < 3; ++j) {
        g_q_[order_ + variable_num_ / dim_ + i][j] = 0.0;
      }
    }
  }

  f_combine = 0.0;  //总代价函数
  grad.resize(variable_num_);
  fill(grad.begin(), grad.end(), 0.0);

  /*  evaluate costs and their gradient  */
  //每个代价变量
  double f_smoothness, f_distance, f_feasibility, f_endpoint, f_guide, f_waypoints;
  f_smoothness = f_distance = f_feasibility = f_endpoint = f_guide = f_waypoints = 0.0;

  if (cost_function_ & SMOOTHNESS) {
    calcSmoothnessCost(g_q_, f_smoothness, g_smoothness_);
    f_combine += lambda1_ * f_smoothness;
    for (int i = 0; i < variable_num_ / dim_; i++)
      for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda1_ * g_smoothness_[i + order_](j);
  }
  if (cost_function_ & DISTANCE) {
    calcDistanceCost(g_q_, f_distance, g_distance_);
    f_combine += lambda2_ * f_distance;
    for (int i = 0; i < variable_num_ / dim_; i++)
      for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda2_ * g_distance_[i + order_](j);
  }
  if (cost_function_ & FEASIBILITY) {
    calcFeasibilityCost(g_q_, f_feasibility, g_feasibility_);
    f_combine += lambda3_ * f_feasibility;
    for (int i = 0; i < variable_num_ / dim_; i++)
      for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda3_ * g_feasibility_[i + order_](j);
  }
  if (cost_function_ & ENDPOINT) {
    calcEndpointCost(g_q_, f_endpoint, g_endpoint_);
    f_combine += lambda4_ * f_endpoint;
    for (int i = 0; i < variable_num_ / dim_; i++)
      for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda4_ * g_endpoint_[i + order_](j);
  }
  if (cost_function_ & GUIDE) {
    calcGuideCost(g_q_, f_guide, g_guide_);
    f_combine += lambda5_ * f_guide;
    for (int i = 0; i < variable_num_ / dim_; i++)
      for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda5_ * g_guide_[i + order_](j);
  }
  if (cost_function_ & WAYPOINTS) {
    calcWaypointsCost(g_q_, f_waypoints, g_waypoints_);
    f_combine += lambda7_ * f_waypoints;
    for (int i = 0; i < variable_num_ / dim_; i++)
      for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda7_ * g_waypoints_[i + order_](j);
  }
  /*  print cost  */
  // if ((cost_function_ & WAYPOINTS) && iter_num_ % 10 == 0) {
  //   cout << iter_num_ << ", total: " << f_combine << ", acc: " << lambda8_ * f_view
  //        << ", waypt: " << lambda7_ * f_waypoints << endl;
  // }

  // if (optimization_phase_ == SECOND_PHASE) {
  //  << ", smooth: " << lambda1_ * f_smoothness
  //  << " , dist:" << lambda2_ * f_distance
  //  << ", fea: " << lambda3_ * f_feasibility << endl;
  // << ", end: " << lambda4_ * f_endpoint
  // << ", guide: " << lambda5_ * f_guide
  // }
}

3.2 通过Nlopt进行优化

Nlopt是非线性优化库，注意fast-planner不要用ros版的nlopt，那样版本不对，要通过第三方下载，Nlopt库下载链接
下载完正常安装到local就行。Nlopt基础使用参考博客

//利用Nlopt对控制点进行非线性优化
Eigen::MatrixXd BsplineOptimizer::BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,
                                                      const int& cost_function, int max_num_id,
                                                      int max_time_id) {
  //初始化控制点与维度
  setControlPoints(points);
  //初始化节点向量
  setBsplineInterval(ts);
  //初始化代价函数
  setCostFunction(cost_function);
  //初始化最大控制点与时间id
  setTerminateCond(max_num_id, max_time_id);
  //执行优化
  optimize();
  //返回优化后的控制点
  return this->control_points_;
}

//优化函数
void BsplineOptimizer::optimize() {
  /* initialize solver */
  //初始化迭代次数和最小代价、设置控制点的数量
  iter_num_        = 0;
  min_cost_        = std::numeric_limits<double>::max();
  const int pt_num = control_points_.rows();
  //初始化存储代价向量
  g_q_.resize(pt_num);
  g_smoothness_.resize(pt_num);
  g_distance_.resize(pt_num);
  g_feasibility_.resize(pt_num);
  g_endpoint_.resize(pt_num);
  g_waypoints_.resize(pt_num);
  g_guide_.resize(pt_num);

  //判断是否有终点硬约束
  if (cost_function_ & ENDPOINT) {
    variable_num_ = dim_ * (pt_num - order_);
    // end position used for hard constraint
    end_pt_ = (1 / 6.0) *
        (control_points_.row(pt_num - 3) + 4 * control_points_.row(pt_num - 2) +
         control_points_.row(pt_num - 1));
  } else {
    variable_num_ = max(0, dim_ * (pt_num - 2 * order_)) ;
  }

  /* do optimization using NLopt slover */
  //创建NLopt优化器，选择合适的算法
  nlopt::opt opt(nlopt::algorithm(isQuadratic() ? algorithm1_ : algorithm2_), variable_num_);
  opt.set_min_objective(BsplineOptimizer::costFunction, this);
  //设置最大迭代次数、最长运行时间和相对容差
  opt.set_maxeval(max_iteration_num_[max_num_id_]);
  opt.set_maxtime(max_iteration_time_[max_time_id_]);
  opt.set_xtol_rel(1e-5);

  vector<double> q(variable_num_);
  for (int i = order_; i < pt_num; ++i) {
    if (!(cost_function_ & ENDPOINT) && i >= pt_num - order_) continue;
    for (int j = 0; j < dim_; j++) {
      q[dim_ * (i - order_) + j] = control_points_(i, j);
    }
  }

  if (dim_ != 1) {
    vector<double> lb(variable_num_), ub(variable_num_);
    const double   bound = 10.0;
    for (int i = 0; i < variable_num_; ++i) {
      lb[i] = q[i] - bound;
      ub[i] = q[i] + bound;
    }
    opt.set_lower_bounds(lb);
    opt.set_upper_bounds(ub);
  }

  try {
    // cout << fixed << setprecision(7);
    // vec_time_.clear();
    // vec_cost_.clear();
    // time_start_ = ros::Time::now();
    //执行优化
    double        final_cost;
    nlopt::result result = opt.optimize(q, final_cost);

    /* retrieve the optimization result */
    // cout << "Min cost:" << min_cost_ << endl;
  } catch (std::exception& e) {
    ROS_WARN("[Optimization]: nlopt exception");
    cout << e.what() << endl;
  }
  //遍历向量
  for (int i = order_; i < control_points_.rows(); ++i) {
    if (!(cost_function_ & ENDPOINT) && i >= pt_num - order_) continue;
    for (int j = 0; j < dim_; j++) {
      control_points_(i, j) = best_variable_[dim_ * (i - order_) + j];
    }
  }

  if (!(cost_function_ & GUIDE)) ROS_INFO_STREAM("iter num: " << iter_num_);
}

本章实现的是B样条控制点的优化，最后输出的是一组优化后的控制点。
本章完，fast-planner系列未完待遇。。。