Fast-Planner(二)详解B-Spline曲线生成

傲娇的根号二

已于 2024-05-15 10:54:35 修改

阅读量1.5k

点赞数 22

分类专栏：路径规划、轨迹规划、运动规划文章标签：笔记

于 2024-03-21 11:00:09 首次发布

本文链接：https://blog.csdn.net/weixin_45534376/article/details/136801407

版权

路径规划、轨迹规划、运动规划专栏收录该内容

14 篇文章 19 订阅

订阅专栏

本文上接Fast-Planner的前端kinodynamic-A*详解，介绍B-spline曲线生成。关于B-Spline的初始知识可以参考笔者的笔记。如有问题，欢迎各位大佬评论指出，带着我一起进步。

二、均匀B样条曲线

首先明确B样条曲线的阶数和曲线次数的关系，设阶数为k，则曲线次数为p(p=k-1)。原文中的感觉更像是准均匀B样条曲线，即时间节点的前k(=p+1)个和后k个均为不变化，中间部分均匀分布。如一个3阶的B样条曲线时间节点U={0,0,0,1,2,3,4,5,5,5}。

2.1 B样条初始化

输入：控制点、次数(阶数-1)，时间间隔

void NonUniformBspline::setUniformBspline(const Eigen::MatrixXd& points, const int& order,
                                          const double& interval) {
  //每行代表一个控制点，维度为N*3
  control_points_ = points;
  //B样条次数
  p_              = order;
  //节点之间的间隔，即delta_t
  interval_       = interval;
  n_ = points.rows() - 1;//控制点数为n+1
  m_ = n_ + p_ + 1;//时间间隔数=控制点数+次数

  u_ = Eigen::VectorXd::Zero(m_ + 1);//时间节点数=间隔数+1
  //B样条节点初始化，初始化为均匀B样条，即间隔相等
  for (int i = 0; i <= m_; ++i) {
    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.2 求B样条时间节点u_处值

输入：时间u
根据DeBoor公式把整个B样条函数计算出来
输出：获得一个[t_p,t_m-p]作用域中的B样条函数值。

Eigen::VectorXd NonUniformBspline::evaluateDeBoor(const double& u) {

  double ub = min(max(u_(p_), u), u_(m_ - p_));//取节点中间段为有效段

  // 找出输入的u是时间节点的第几个节点处
  int k = p_;
  while (true) {
    if (u_(k + 1) >= ub) break;
    ++k;
  }
  //通过k找到与当前节点相关的控制点是k-p_到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_];
}

2.3 控制点获取和前端路径拟合

前面两部分解析使得我们能够在已知控制点的情况下计算任一时间对应的轨迹值。
使用均匀B样条对前端路径进行拟合时用的是B样条的矩阵表达方法，具体参见论文：K. Qin, “General matrix representations for b-splines,” The Visual Computer, vol. 16, no. 3, pp. 177–186, 2000.矩阵形式表示B样条曲线：
在这里插入图片描述其中 $M_{p_b+1}$ 是样条多项式的次数 $p_b$ 所决定的常数矩阵。 $s(t)=\frac{t-t_m}{\Delta t}$
从前端搜索获得的轨迹点是k个，则有k-1段轨迹，根据B样条性质，这K-1段3次B样条曲线(注意次和阶区别，之后只说次)的定义域为 $u_3,u_3+k-1]$ ，则一共由k+4个knot vector(节点数=轨迹点+次数+1),K+2个控制点。（段数+次数=控制点个数=(k-1)+3=k+2）
对于B样条曲线上定义在 $t_m,t_{m+1}]$ 上的一小段曲线，其被 $Q_{m-3},Q_{m-2},Q_{m-1},Q_m]$ 这四个控制点所决定，此时 $M_{p_b}$ 是四维常数矩阵。
举例：
如下图(箭头表示作用范围为半闭半开形式)假设从搜索获得了5个轨迹点分别为 $X_1,X_2,X_3,x_4,x_5]$ ，相当于获得了4段轨迹，这4段轨迹的3次B样条曲线的合法定义域为 $u_3,u_5]$ (次数，轨迹点数)。则一共有9个knot vector，其序列为 $u_0,u_1,u_2,u_3,u_4,u_5,u_6,u_7,u_8]$
在这里插入图片描述
B样条的表达式 $p (s (t))$ 是路径位置点以时间为参数的表达式，那么速度和加速度分别是一阶导和二阶导。而表达式中只有s(t)中包含时间t，因此
$s(t)=[1\quad\frac{t-t_m}{\Delta t}\quad(\frac{t-t_m}{\Delta t})^2\quad...\quad(\frac{t-t_m}{\Delta t})^{p_b}]^T\\ ----------------------\\ s'(t)=[0\quad\frac{1}{\Delta t}\quad\frac{2}{\Delta t}(\frac{t-t_m}{\Delta t})\quad...\quad\frac{p_b}{\Delta t}(\frac{t-t_m}{\Delta t})^{p_b-1}]^T\\ ----------------------\\ s''(t)=[0\quad0\quad\frac{2}{\Delta t^2}\quad...\quad\frac{p_b(p_b-1)}{\Delta t^2}(\frac{t-t_m}{\Delta t})^{p_b-2}]^T$

代码中收到搜索算法得到的k个轨迹点，则有K+2个控制点，但是可以构建K+4个等式约束(包括k个位置约束(来自轨迹点)，两个速度约束，两个加速度约束(来自初始位置和终点))，然后利用Ax=b进行线性拟合获得控制点。
A矩阵构建方式：对于获得的K个轨迹点，每一个轨迹点对应的时间节点都是tm，带入 $s(t)=\frac{t-t_m}{\Delta t}$ 都为0，此时位置等式约束 $p(s(t_m))=s(t_m)^TM_4q_m$ 中的 $s(t_m)$ 为 $[1\quad0\quad...\quad0]^T$ 只作用M_4的第一行，同理速度约束和加速约束只作用于第2行和第3行。
在这里插入图片描述代码如下，从搜索算法得到的轨迹点构造等式约束求控制点

/*
  param：
    - ts:轨迹执行时间
    - point_set:原轨迹点集合
    - start_end_derivative:起点和终点的高阶约束
  output:
    - ctrl_pts:控制点矩阵
  fuction:
    - 将给定点集和起始/终止导数转换为B-Spline曲线的控制点矩阵，通过对原始轨迹的拟合得到B样条轨迹的控制点
*/
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();

  // write A
  // 该矩阵用来构建线性方程组，B-Spline曲线参数化过程中求解控制点
  // 一阶B-Spline基函数的系数向量、一阶B-Spline基函数的导数系数向量、二阶B-Spline基函数的系数向量
  Eigen::Vector3d prow(3), vrow(3), arow(3);
  prow << 1, 4, 1;
  vrow << -1, 0, 1;
  arow << 1, -2, 1;

  // K+4是等式约束个数，K+2是控制点个数
  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);

  // b矩阵为K个坐标点和2个速度2个加速度组成的（K+4）*3的矩阵
  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);
  }

  // solve Ax = b
  Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
  Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
  Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);

  // 控制点赋值
  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;
}

2.4 非均匀B样条一阶及二阶微分

获得时间节点相同的均匀B样条曲线后，希望能对这条曲线上的速度及加速度进行动力学检查，以备之后进行时间节点调整。通过下面方式计算出非均匀形式下的速度控制点和加速度控制点。
在这里插入图片描述每当B样条做一次微分，B样条变成次数减1，控制点数-1的B样条，对应的knot vector-2。

//非均匀B样条一阶及二阶微分
//计算V’和A’
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;
}

NonUniformBspline NonUniformBspline::getDerivative() {
  Eigen::MatrixXd   ctp = getDerivativeControlPoints();
  NonUniformBspline derivative(ctp, p_ - 1, interval_);

  /* cut the first and last knot */
  Eigen::VectorXd knot(u_.rows() - 2);
  knot = u_.segment(1, u_.rows() - 2);
  derivative.setKnot(knot);

  return derivative;
}

2.5 可达性检查

由于B样条能将曲线控制在由控制点组成的凸包内，那么曲线就不会超过控制点的最大值，通过调整控制点可以做可达性检测。通过下式计算每个控制点的速度和加速度是否超限，获得最大速度和最大加速度，并获得调整比例。
在这里插入图片描述

//计算每个控制点的速度和加速度是否超限，最大速度是多少，并获得调整比例
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(); //控制点列数为维度，即x,y,z

  /* 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;
}

小结

通过以上步骤，通过路径搜索获得的离散路径点求得了B样条控制点，此时为均匀B样条。然后通过速度、加速度约束调整不合理的时间节点，此时曲线变成了非均匀B样条。本章结束，未完待续。。。

参考博客1
参考博客2

傲娇的根号二

关注

22
点赞
踩
31

收藏

觉得还不错? 一键收藏
6
评论
Fast-Planner(二)详解B-Spline曲线生成

本文上接Fast-Planner的前端kinodynamic-A*详解，介绍B-spline曲线生成。关于B-Spline的初始知识可以参考笔者的。如有问题，欢迎各位大佬评论指出，带着我一起进步。
复制链接

扫一扫

专栏目录