系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

前言

本文先对B样条曲线做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

提示：以下是本篇文章正文内容

一、B样条曲线理论

1.贝塞尔、B样条、非均匀B样条关系

示例图

（1）B样条不能表示一些基本的曲线，比如圆，【防盗标记–盒子君hzj】所以引入了NURBS,可以进一步推广为非均匀有理B-样条(NURBS)
（2）将多个贝塞尔曲线连接就可以得到B样条。
（3）B-样条是贝塞尔曲线的一般化
.
.

2.B样条的分段本质（B样条与Bezier曲线关系）

（1）核心思想
用分段低阶多项式通过连续的连接来代替高阶多项式

（2）原理
平面上有（n+1）个点，每两个点之间构造一条多项式曲线，（n+1）个点有n个小曲线，n个小曲线之间要满足两次连续的条件圆滑的拼接在一起，B样条曲线通过改变某条相邻几条小曲线的多项式，再把该几条小曲线拼接到整体曲线上，从而实现进改变部分曲线而不影响整体曲线

（3）优点
以上所有的分段都为Bezier曲线，对于分段Bezier曲线，【防盗标记–盒子君hzj】不同的曲线段相互独立，移动控制点只会影响其所在的Bezier曲线段，而其他的Bezier曲线段都不会改变，甚至所有关于Bezier曲线的算法可以同样地适用于分段Bezier曲线。
移动控制点仅仅改变曲线的部分形状，而不大改整体
.
.

3.B样条曲线的基本参数–对应实现的步骤一

为了设计一个B-样条曲线，我们需要一系列的控制点，一系列的节点和一系列的系数，每个系数对应一个控制点，【防盗标记–盒子君hzj】所以所有曲线段连接在一起满足某个连续 条件。系数的计算可能是最复杂的步骤因为它们必须保证某个连续条件。

（1）t权重系数值（调用的时候靠这个）

作用：通过每个项的权重可以查阅曲线中的任意位置
用于形成u、(1-t)等等权重

（2）阶数

作用：阶数决定曲线的平滑度和复杂程度，阶数由控制点（中间插入的点）的数量有关

阶数=所有权重中t值的最高次幂=生成t值所需要的控制点数-1

（3）控制点列表

作用：控制点列表决定了曲线形变的方向和程度
代表一系列需要用户提供的顶点【可以认为时中间插入的点】

（4）节点表

作用：节点表是生成基本函数表的关键参数，大小严格等于控制点数量+阶数+1，节点表的参数是人为设置的，B-样条还是要遵守一些规律进行节点表的设置的
节点表是生成基本函数表的关键参数，大小严格等于控制点数量+阶数+1，节点表的参数是人为设置的，【防盗标记–盒子君hzj】B-样条还是要遵守一些规律进行节点表的设置的

一般设置的方法有两种
1）顺序方法
作用：用于制作标准的B-样条开曲线和闭曲线
方法：顺序列表只需要从0-1线性递增设置即可

2）Clamped方法
作用：用于制作一种比较实用的B-样条曲线
方法：将前后各阶数+1个节点设置成0和1

举例
假设曲线有6个控制点，阶数是3阶，那么节点表大小=6+3+1=10
如果是顺序列表，只需要按顺序设置：

如果是Clamped列表，由于是3阶，前面3+1个参数均设置为0，【防盗标记–盒子君hzj】后面3+1个参数均设置为1，然后剩余参数均匀递增：
.
.

4.B样条曲线递归公式推导【Cox-DeBoor公式】–对应实现的步骤二

（1）B样条曲线公式【第一种表达方式(正推–数学归纳法)】

（1）一阶B样条公式：

（2）二阶B样条递推公式：

…
（3）高阶B样条递推公式

只需要通过修改u值，就可以表述任意曲线上的任意点
.
.

（2）B样条曲线公式【第二种表达方式（反推）】

B样条曲线公式如下：

t的话最容易理解，就是前面反复提到的t值。
knot代表节点，也就是我们前面提到的节点表。而knoti代表节点表中的第i 个元素。
而Bi,deg(t)就是基本函数表的参数了。没错，基本函数表是一个二维数组。参数i和deg分别表示第几个元素和阶数。 所以Bi,deg(t) 的意思就是用户输入值为t时，基本函数表在第deg阶的第i个元素的值。
整个公式实际上是两个部分相加。而加号两侧的公式格式一致：一个通过节点表knot 和 t 经过一系列计算得出的权重值和一个比当前更低一阶的基本函数表值的乘积。

基本函数表是递归方程，【防盗标记–盒子君hzj】因为当前deg阶的元素需要通过两个deg-1阶的元素获得，deg-1阶的元素则需要通过deg-2阶的元素获得……以此类推，直到递归deg次以后，回退到0阶为止。

B-样条算法规定，回退到0阶时使用以下公式：

即，0阶的时候，t处于第i个knot值和第i+1个knot值之间的时候，才会等于1，其他时候都等于0.
.
.

（3）公式的理解 【也是编程的思路】

（1）基本函数表本质上是一个递归方程，但同时它也是一个中间参数

（2）设最终t值在B-样条曲线的对应位置为C(t)，则最终B-样条曲线计算公式为：

其中：第deg阶的第i个元素为Bi,deg(t)，【防盗标记–盒子君hzj】而Pi便是我们前面一直提到的控制点，而且在这里是第i个控制点。求和符号上的n表示控制点的总数。

（3）曲线位置C(t)，是基本函数表中第 deg 阶下所有元素的权重值，和对应位置控制点坐标乘积一一相加后得到的总和

.
.

5.B样条的分类（根据控制点分布进行分类）

（1）均匀B样条曲线

节点成等差数列均匀分布

（2）准均匀B样条曲线

两端节点0,1重复度为次数k的基础上加1

（3）分段Bezier曲线

两端节点重复度为k+1，内部节点重复度为k，显然此时必须满足条件(m-1)%k==0

（4）非均匀B样条

任意选取的一个序列[u0,…,um]，只要在数学上成立即可，这是最一般的情况
.
.

6.B样条曲线的几个基本性质

（1）一个有N+1个控制点的P b 次B样条曲线，则其一共有P b +N+1时间节点，即

同样的，假定希望设计一条P b次B样条曲线，且具备M个时间节点，则相应的控制点数量应为：M + 1 − P b

.

（2）这样一条B样条的定义域为

（3）每一个控制点P i 的作用域为

.
.

7.B样条曲线优点

（1）B样条由一系列控制点决定，但是B样条不会经过其控制点。
（2）核心思想就是分段，【防盗标记–盒子君hzj】改变局部的控制点不会影响整个曲线；B样条曲线除了保持Bezier曲线所具有的优点外，还增加了可以对曲线进行局部修改这一突出的优点。
（3）样条曲线包含在控制折线（ployline）的凸包内。更特别地，如果u 在节点区间[ui,ui+1)里，那么C(u)在控制点Pi-p, Pi-p+1, …, Pi的凸包里。

二、B样条的源码实现

bspline使用的就是非均匀的B样条【处理平滑的是控制点（中间插入点），理论和我看的差不多，对着源码看一边就好】
计算B样条上的一点的值，可以修改一个或多个控制参数：控制点的位置(n+1)，节点位置(m+1)，和曲线的次数p。

（0）使用B样条曲线的背景

举例，通过前端的路径探索，得到了较为低阶的轨迹点（甚至是路径点），得到初始路径后，需要在前端初始路径的基础上进行B样条曲线优化。B样条的第一部分是利用均匀B样条进行轨迹平顺性、安全性、速度和加速度优化

（0）步骤示例图

.
.

（1）步骤一：【设置参数部分】设置非均匀B样条参数，计算节点表setUniformBspline

功能

输入基本参数：控制点列表、阶数(轨迹次数)、间隔时间
计算节点值（节点表）

算法原理

使用Clamped的方法计算节点值（节点表）
注意：在构造函数中就会调用一次，在Fast-planner的实现中，tp = 0, 以文章中的3次样条函数为例，t0 = -3Δt,t1 = -2Δt.t2 = -Δt

//初始化一条非均匀B样条曲线【输入基本参数：控制点列表、阶数、间隔时间】
void NonUniformBspline::setUniformBspline(const Eigen::MatrixXd& points, const int& order,
                                          const double& interval) {
    
    
//备注：在构造函数中就会调用一次
//核心思想：设置控制点control_points_、阶数p_、时间间隔interval_、n_、m_和区间间隔u_，使用Clamped的方法,计算节点值
  control_points_ = points;   //控制点
  p_              = order;    //阶数
  interval_       = interval; //时间间隔

  n_ = points.rows() - 1; // 计算控制点列表control point number: n + 1
  m_ = n_ + p_ + 1;

  u_ = Eigen::VectorXd::Zero(m_ + 1); //计算节点表 knot list, with number: control point number + order + 1
  for (int i = 0; i <= m_; ++i) {
    
    
    //计算节点值，使用Clamped的方法
    if (i <= p_) {
    
    
      u_(i) = double(-p_ + i) * interval_;
    } else if (i > p_ && i <= m_ - p_) {
    
    
      u_(i) = u_(i - 1) + interval_;
    } else if (i > m_ - p_) {
    
    
      u_(i) = u_(i - 1) + interval_;
    }
  }
}

.
.

（2）步骤二：【计算部分】给定一个时刻t, 计算B样条该控制点的位置坐标值evaluateDeBoorT()【Cox-DeBoor公式公式】

功能

给定一个时刻t, 计算该控制点的位置坐标值

算法原理

通常的做法是根据Cox-DeBoor公式把整个B样条函数计算出来。但在evaluateDeBoor()这一函数中，作者采用的是递归的DeBoor算法

假设原有与此段轨迹相关的P+1个控制点为0阶‘控制点’，通过两个循环计算出第K个P阶的‘控制点’，该点即为要求的B样条函数值。内循环通过递归公式求得高一阶的P+1个控制点，外循环提高阶数。

具体参见wikipediahttps://en.wikipedia.org/wiki/De_Boor%27s_algorithm

同样的，evaluateDeBoorT()函数只是直接得到一个作用域中的B样条函数值。

//给定一个时间t, 计算该控制点的坐标值   use t \in [0, duration]
Eigen::VectorXd NonUniformBspline::evaluateDeBoor(const double& u) {
    
    

  double ub = min(max(u_(p_), u), u_(m_ - p_));

  // 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) {
    
    
    d.push_back(control_points_.row(k - p_ + i));
    // cout << d[i].transpose() << endl;
  }

  for (int r = 1; r <= p_; ++r) {
    
    
    for (int i = p_; i >= r; --i) {
    
    
      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_];
}

至此，我们已经能够在已知控制点的情况下计算任一时间对应的轨迹值
.
.

（3）步骤三：【调用部分】输入A*类探索的初始控制点point_set，获取B样条优化的位置控制点ctrl_pts [parameterizeToBspline（）]

虽然在计算B样条曲线上某一点的值时论文用的是DeBoor公式，但是在使用非均匀B样条对前端路径进行拟合时用的是B样条的矩阵表达方法

首先假设获得的离散轨迹点一共有K个，则有K-1段轨迹，根据B样条性质，这K-1段3次B样条曲线的定义域是

则一共有K+5个knot vector，即M = K − 1 + 3 + 3 M=K-1+3+3M=K−1+3+3 ，所以应该有M-3即K+2个控制点。

对于B样条曲线上定义在[tm,tm+1]上的一小段曲线，其被这四个控制点所决定。其中

Mpb是四维常数矩阵

（1）计算轨迹位置约束
我们可以得到第一个路径点对应的位置约束：

同理可得其余K-1个路径点的位置约束。

（2）计算轨迹速度、加速度约束
对于速度约束与加速度约束，只需要对时间t 求一次及二次微分即可，所得约束关系如代码中所示。需要注意的是，由于s(t)是关于t的函数，具有常数项 , 所以一次及二次微分需要乘以对应的常数项

通过对K+2个控制点构建K+4个等式约束（K个位置约束，两个速度约束，两个加速度约束），利用A x = b Ax=bAx=b进行线性拟合，即可得到拟合的控制点。

计算b样条的过程其实就是解算一个Ax = b方程,通过矩阵运算

具体参见论文
K. Qin, “General matrix representations for b-splines,” The Visual Computer, vol. 16, no. 3, pp. 177–186, 2000

  //对点集point_set中的点进行B样条曲线插值，带速度和加速度边界的约束
  //输入：带速度和加速度约束(K+2) points; 采样时间ts
  //输出：输出优化后的控制点（中间插入点）control_pts
void NonUniformBspline::parameterizeToBspline(const double& ts, const vector<Eigen::Vector3d>& point_set,
                                              const vector<Eigen::Vector3d>& start_end_derivative,
                                              Eigen::MatrixXd&               ctrl_pts) {
    
    
  if (ts <= 0) {
    
    
    cout << "[B-spline]:time step error." << endl;
    return;
  }

  if (point_set.size() < 2) {
    
    
    cout << "[B-spline]:point set have only " << point_set.size() << " points." << endl;
    return;
  }

  if (start_end_derivative.size() != 4) {
    
    
    cout << "[B-spline]:derivatives error." << endl;
  }

  int K = point_set.size();

  //（1） write A矩阵
  Eigen::Vector3d prow(3), vrow(3), arow(3);
  prow << 1, 4, 1;
  vrow << -1, 0, 1;
  arow << 1, -2, 1;

  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2);

  for (int i = 0; i < K; ++i) A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();

  A.block(K, 0, 1, 3)         = (1 / 2.0 / ts) * vrow.transpose();
  A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();

  A.block(K + 2, 0, 1, 3)     = (1 / ts / ts) * arow.transpose();
  A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();
  // cout << "A:\n" << A << endl;

  // A.block(0, 0, K, K + 2) = (1 / 6.0) * A.block(0, 0, K, K + 2);
  // A.block(K, 0, 2, K + 2) = (1 / 2.0 / ts) * A.block(K, 0, 2, K + 2);
  // A.row(K + 4) = (1 / ts / ts) * A.row(K + 4);
  // A.row(K + 5) = (1 / ts / ts) * A.row(K + 5);

  //（2） write b矩阵
  Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4);
  for (int i = 0; i < K; ++i) {
    
    
    bx(i) = point_set[i](0);
    by(i) = point_set[i](1);
    bz(i) = point_set[i](2);
  }

  for (int i = 0; i < 4; ++i) {
    
    
    bx(K + i) = start_end_derivative[i](0);
    by(K + i) = start_end_derivative[i](1);
    bz(K + i) = start_end_derivative[i](2);
  }

  //（3） 解方程solve Ax = b（使用用求解器）
  // compare of different solver https://blog.csdn.net/u012936940/article/details/79871941
  // Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
  // Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
  // Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);

  Eigen::VectorXd px = A.householderQr().solve(bx);
  Eigen::VectorXd py = A.householderQr().solve(by);
  Eigen::VectorXd pz = A.householderQr().solve(bz);

  //（4） convert to control pts
  ctrl_pts.resize(K + 2, 3);
  ctrl_pts.col(0) = px;
  ctrl_pts.col(1) = py;
  ctrl_pts.col(2) = pz;

  // cout << "[B-spline]: parameterization ok." << endl;
}

（4）步骤四：计算非均匀B样条一阶及二阶微分，获取轨迹的速度、加速度

功能

当我们得到一条时间节点等同的均匀B样条曲线后，我们希望能够对这条曲线上的速度及加速度进行动力学检查，以备之后进行时间节点调整。

算法原理

我们需要计算出非均匀形式下的速度控制点及加速度控制点。（直接用均匀B样条的控制点算是因为均匀B样条可以看做特殊形式的非均匀B样条）

代码中利用递归的形式求得速度与加速度，B样条的一阶微分是次数-1，控制点数-1的B样条曲线，因此相应的Knot vector-2。 在利用上图公式获得一阶微分控制点后，新定义一个NonUniformBspline对象，并将新的控制点，次数，Knot vector赋值给它。

//得到B样条曲线的控制点的导数
Eigen::MatrixXd NonUniformBspline::getDerivativeControlPoints() {
    
    
  // The derivative of a b-spline is also a b-spline, its order become p_-1
  // control point Qi = p_*(Pi+1-Pi)/(ui+p_+1-ui+1)
  Eigen::MatrixXd ctp = Eigen::MatrixXd::Zero(control_points_.rows() - 1, control_points_.cols());
  for (int i = 0; i < ctp.rows(); ++i) {
    
    
    ctp.row(i) =
        p_ * (control_points_.row(i + 1) - control_points_.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
  }
  return ctp;
}

（5）步骤五：检查位置、速度、加速度、曲率可行性，并进行时间调整

原理

这一部分的三个函数 checkFeasibility(), checkRatio(), reallocateTime()的大部分内容都是一致的。都是利用如下两个公式计算每个控制点的速度和加速度是否超限，最大速度是多少，并获得调整比例。

真正进行重新时间调整的函数时reallocateTime,通过计算当前控制点是否超限，以及调整表比例。对于当前控制点i ii有关的时间区间进行时间调整

注意，这里的p b是当前B样条的次数，如果是速度则是3-1=2, 加速度则是3-2=1（针对论文中的3次B样条曲线而言）。在t i + p b + 1以后的是时间节点则是直接加上总的扩张时间就可以。

检查速度、加速度可行性

bool NonUniformBspline::checkFeasibility(bool show) {
    
    
  bool fea = true;
  // SETY << "[Bspline]: total points size: " << control_points_.rows() << endl;

  Eigen::MatrixXd P         = control_points_;
  int             dimension = control_points_.cols();

  /* check vel feasibility and insert points */
  double max_vel = -1.0;
  for (int i = 0; i < P.rows() - 1; ++i) {
    
    
    Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));

    if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 ||
        fabs(vel(2)) > limit_vel_ + 1e-4) {
    
    

      if (show) cout << "[Check]: Infeasible vel " << i << " :" << vel.transpose() << endl;
      fea = false;

      for (int j = 0; j < dimension; ++j) {
    
    
        max_vel = max(max_vel, fabs(vel(j)));
      }
    }
  }

  /* acc feasibility */
  double max_acc = -1.0;
  for (int i = 0; i < P.rows() - 2; ++i) {
    
    

    Eigen::VectorXd acc = p_ * (p_ - 1) *
        ((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
         (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
        (u_(i + p_ + 1) - u_(i + 2));

    if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 ||
        fabs(acc(2)) > limit_acc_ + 1e-4) {
    
    

      if (show) cout << "[Check]: Infeasible acc " << i << " :" << acc.transpose() << endl;
      fea = false;

      for (int j = 0; j < dimension; ++j) {
    
    
        max_acc = max(max_acc, fabs(acc(j)));
      }
    }
  }

  double ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));

  return fea;
}

检查曲率可行性

double NonUniformBspline::checkRatio() {
    
    
  Eigen::MatrixXd P         = control_points_;
  int             dimension = control_points_.cols();

  // find max vel
  double max_vel = -1.0;
  for (int i = 0; i < P.rows() - 1; ++i) {
    
    
    Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
    for (int j = 0; j < dimension; ++j) {
    
    
      max_vel = max(max_vel, fabs(vel(j)));
    }
  }
  // find max acc
  double max_acc = -1.0;
  for (int i = 0; i < P.rows() - 2; ++i) {
    
    
    Eigen::VectorXd acc = p_ * (p_ - 1) *
        ((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
         (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
        (u_(i + p_ + 1) - u_(i + 2));
    for (int j = 0; j < dimension; ++j) {
    
    
      max_acc = max(max_acc, fabs(acc(j)));
    }
  }
  double ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));
  // ROS_ERROR_COND(ratio > 2.0, "max vel: %lf, max acc: %lf.", max_vel, max_acc);

  return ratio;
}

检查时间分配

bool NonUniformBspline::reallocateTime(bool show) {
    
    
  // SETY << "[Bspline]: total points size: " << control_points_.rows() << endl;
  // cout << "origin knots:\n" << u_.transpose() << endl;
  bool fea = true;

  Eigen::MatrixXd P         = control_points_;
  int             dimension = control_points_.cols();

  double max_vel, max_acc;

  /* check vel feasibility and insert points */
  for (int i = 0; i < P.rows() - 1; ++i) {
    
    
    Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));

    if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 ||
        fabs(vel(2)) > limit_vel_ + 1e-4) {
    
    

      fea = false;
      if (show) cout << "[Realloc]: Infeasible vel " << i << " :" << vel.transpose() << endl;

      max_vel = -1.0;
      for (int j = 0; j < dimension; ++j) {
    
    
        max_vel = max(max_vel, fabs(vel(j)));
      }

      double ratio = max_vel / limit_vel_ + 1e-4;
      if (ratio > limit_ratio_) ratio = limit_ratio_;

      double time_ori = u_(i + p_ + 1) - u_(i + 1);
      double time_new = ratio * time_ori;
      double delta_t  = time_new - time_ori;
      double t_inc    = delta_t / double(p_);

      for (int j = i + 2; j <= i + p_ + 1; ++j) {
    
    
        u_(j) += double(j - i - 1) * t_inc;
        if (j <= 5 && j >= 1) {
    
    
          // cout << "vel j: " << j << endl;
        }
      }

      for (int j = i + p_ + 2; j < u_.rows(); ++j) {
    
    
        u_(j) += delta_t;
      }
    }
  }

  /* acc feasibility */
  for (int i = 0; i < P.rows() - 2; ++i) {
    
    

    Eigen::VectorXd acc = p_ * (p_ - 1) *
        ((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
         (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
        (u_(i + p_ + 1) - u_(i + 2));

    if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 ||
        fabs(acc(2)) > limit_acc_ + 1e-4) {
    
    

      fea = false;
      if (show) cout << "[Realloc]: Infeasible acc " << i << " :" << acc.transpose() << endl;

      max_acc = -1.0;
      for (int j = 0; j < dimension; ++j) {
    
    
        max_acc = max(max_acc, fabs(acc(j)));
      }

      double ratio = sqrt(max_acc / limit_acc_) + 1e-4;
      if (ratio > limit_ratio_) ratio = limit_ratio_;
      // cout << "ratio: " << ratio << endl;

      double time_ori = u_(i + p_ + 1) - u_(i + 2);
      double time_new = ratio * time_ori;
      double delta_t  = time_new - time_ori;
      double t_inc    = delta_t / double(p_ - 1);

      if (i == 1 || i == 2) {
    
    
        // cout << "acc i: " << i << endl;
        for (int j = 2; j <= 5; ++j) {
    
    
          u_(j) += double(j - 1) * t_inc;
        }

        for (int j = 6; j < u_.rows(); ++j) {
    
    
          u_(j) += 4.0 * t_inc;
        }

      } else {
    
    

        for (int j = i + 3; j <= i + p_ + 1; ++j) {
    
    
          u_(j) += double(j - i - 2) * t_inc;
          if (j <= 5 && j >= 1) {
    
    
            // cout << "acc j: " << j << endl;
          }
        }

        for (int j = i + p_ + 2; j < u_.rows(); ++j) {
    
    
          u_(j) += delta_t;
        }
      }
    }
  }

  return fea;
}

检查时间跨度

void NonUniformBspline::lengthenTime(const double& ratio) {
    
    
  int num1 = 5;
  int num2 = getKnot().rows() - 1 - 5;

  double delta_t = (ratio - 1.0) * (u_(num2) - u_(num1));
  double t_inc   = delta_t / double(num2 - num1);
  for (int i = num1 + 1; i <= num2; ++i) u_(i) += double(i - num1) * t_inc;
  for (int i = num2 + 1; i < u_.rows(); ++i) u_(i) += delta_t;
}

.
.
.

（6）获取/设置基本bspline信息方向函数

  void                                   setKnot(const Eigen::VectorXd& knot);    //设置节点（表）
  Eigen::VectorXd                        getKnot();                               //获取节点（表）
  Eigen::MatrixXd                        getControlPoint();                       //获取中间控制点
  double                                 getInterval();                           //获取时间间隔
  void                                   getTimeSpan(double& um, double& um_p);   //获取跨度时间
  pair<Eigen::VectorXd, Eigen::VectorXd> getHeadTailPts();                        //获取头和尾的控制点

（7）用于执行评估方向函数

三、lattice应用示例：基于贝塞尔曲线的方法做路径平滑

不平滑的曲线是和环境场景有关系的，虚拟的平滑曲线不一定满足环境信息的，【防盗标记–盒子君hzj】所以再虚拟平滑曲线的时候也要考虑环境的约束–这就诞生了lattice的采样基于代价来做的方法，然后才进行拟合，这样做更安全一点

参考资料

B样条参考资料
https://zhuanlan.zhihu.com/p/50626506
https://blog.csdn.net/qq_40597317/article/details/81155571
https://www.biaodianfu.com/bezier-curve-and-b-spline.html