B-spline的理解与路径规划中的应用及C++代码的实现

研究项目，无人机的路径规划，需要用到B样条，所以在此写下B-spline的结合C++代码的理解以及在项目中的应用

一、阶数p_

阶数 = 所有权重中t的最高次幂=控制点数量-1；

二、节点表u_

节点表数量m_ = n_ + p_ + 1（控制点数量+阶数+1），节点表用于生成基函数，节点表的生成一般有两种方法，顺序方法和Clamped方法，下面的代码选其一种。举例如下

假设曲线有6个控制点，阶数是3阶，那么节点表大小=6+3+1=10。

如果是顺序列表，只需要按顺序设置

如果是Clamped列表，由于是3阶，前面3+1个参数均设置为0，后面3+1个参数均设置为1，然后剩余参数均匀递增

void NonUniformBspline::setUniformBspline(const Eigen::MatrixXd& points, const int& order,const double& interval) {//设置节点表
  control_points_ = points;
  p_              = order;// B-spline阶数
  interval_       = interval;//interval = ts = pp_.ctrl_pt_dist / pp_.max_vel_

  n_ = points.rows() - 1;// 控制点Q: Q0, Q1, ... , Qn
  m_ = n_ + p_ + 1;// 节点u: U0, U1, ... , Un+p+1

  u_ = Eigen::VectorXd::Zero(m_ + 1);
  for (int i = 0; i <= m_; ++i) {//Clamped列表，由于是3阶，前面3+1个参数均设置为0，后面3+1个参数均设置为1，然后剩余参数均匀递增
    if (i <= p_) {
      u_(i) = double(-p_ + i) * interval_;// 前p+1个节点, 好像没什么用? 只要保证up = 0即可
    } else if (i > p_ && i <= m_ - p_) {
      u_(i) = u_(i - 1) + interval_;// 中间n-p个节点, up+1 = ts , up+2 = 2 * ts
    } else if (i > m_ - p_) {
      u_(i) = u_(i - 1) + interval_;// 后p+1个节点
    }
  }

三、基本函数表d_ = N

有上面的阶数和节点表，则可以开始计算基本函数表了，优化的路径 = 控制点*基本函数

基本函数表公式：

其中，是第i个k次B样条的基函数，也可以理解为各个控制点的权重。

图中为基函数的运算关系，从左往右，从上到下计算。

基本函数，表示基函数的次数(degree)为k，这是第i个k次B样条基函数,i=0,1,...

基本函数表的C++代码实现如下：

Eigen::VectorXd NonUniformBspline::evaluateDeBoor(const double& u) {//返回基本函数表

  double ub = min(max(u_(p_), u), u_(m_ - p_)); //p degree, n+1 control points, m = n+p+1

  // determine which [ui,ui+1] lay in
  int k = p_;
  while (true) {
    if (u_(k + 1) >= ub) break;
    ++k;
  }

  /* deBoor's alg */
  vector<Eigen::VectorXd> d;
  for (int i = 0; i <= p_; ++i) {// [Ui, Ui+1]的轨迹由p+1的控制点(Qi-p, ... ,Qi)共同决定
    d.push_back(control_points_.row(k - p_ + i));
    // cout << d[i].transpose() << endl;
  }
// https://blog.csdn.net/Hachi_Lin/article/details/89812126
  for (int r = 1; r <= p_; ++r) {// 三角递归的横向         r = 1 -> i = 3, 2, 1    //                    r = 2 -> i = 3, 2
    for (int i = p_; i >= r; --i) {// 三角递归的纵向      r = 3 -> i = 3
      double alpha = (ub - u_[i + k - p_]) / (u_[i + 1 + k - r] - u_[i + k - p_]);
      // cout << "alpha: " << alpha << endl;
      d[i] = (1 - alpha) * d[i - 1] + alpha * d[i];
    }
  }

  return d[p_];
}

四、计算B-spline的曲线

根据基本函数表和控制点，我们可以求得B-spline

其中Q为控制点，是第i个k次B样条的基函数

五、根据运动规划的速度和加速度调整时间

因为生成的路径有可能会不符合动力学约束，所以接下来进行时间的调整，来满足动力学约束～～～未完待续～～～