曲线生成 | 图解三次样条曲线生成原理(附ROS C++/Python/Matlab仿真)

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

1 什么是样条？

样条(Spline)早期来源于工程制图，为了将一些固定点连成一条光滑曲线，采用具有弹性的木条固定在这些点上，通过样条作出的曲线经过各固定点且连续光滑，如图所示

在这里插入图片描述

后来，样条发展成一种平滑曲线的数学表示方法。它通过连接一系列给定的数据点（节点）来构建曲线，以便在这些节点上产生平滑的过渡。通常情况下，样条曲线是由多个连续的二次或三次函数组成，每个函数都在相邻节点之间定义。这些连续的函数被称为样条段，它们共同组成了整个曲线

样条是在各个领域中广泛应用的一种技术，例如计算机图形学、物理学模拟、金融和经济分析等。在计算机图形学中，样条通常用于创建平滑的曲线和曲面，以便在三维场景中呈现出更真实的效果。在物理学模拟中，样条可用于描述物体的运动轨迹和变形过程。在金融和经济分析中，样条可用于拟合和预测时间序列数据，例如股票价格和货币汇率

本节介绍常见的三次样条曲线(Cubic Splines)原理

2 三次样条曲线原理

2.1 曲线插值

给定一系列插值点

$X=\left\{ \left( x_0,y_0 \right) ,\left( x_1,y_1 \right) ,\cdots ,\left( x_{n-1},y_{n-1} \right) \right\}$

相邻两点间通过多项式曲线连接，因此共需要拼接 $n - 1$ 段曲线。定义三次多项式曲线为

$f_i\left( x \right) =a_i+b_i\left( x-x_i \right) +c_i\left( x-x_i \right) ^2+d_i\left( x-x_i \right) ^3\,\,i=0,1,\cdots ,n-1$

其中，当 $i = n - 1$ 时的曲线是辅助曲线，用于计算前 $n - 1$ 段曲线而不参与实际拼接。对于三次曲线，给出四个约束条件为

$\begin{cases} \text{过插值点}: f_i\left( x_i \right) =y_i\\ \text{曲线连续}: f_i\left( x_{i+1} \right) =y_{i+1}\\ \text{一阶连续}: \dot{f}_i\left( x_{i+1} \right) =\dot{f}_{i+1}\left( x_{i+1} \right)\\ \text{二阶连续}: \ddot{f}_i\left( x_{i+1} \right) =\ddot{f}_{i+1}\left( x_{i+1} \right)\\\end{cases}\xRightarrow{h_i=x_{i+1}-x_i}\begin{cases} a_i=y_i\\ a_i+b_ih_i+c_ih_{i}^{2}+d_ih_{i}^{3}=y_{i+1}\\ b_i+2c_ih_i+3d_ih_{i}^{2}=b_{i+1}\\ c_i+3d_ih_i=c_{i+1}\\\end{cases}$

联立上式，用系数统一表示其他参数可得

$h_ic_i+2\left( h_i+h_{i+1} \right) c_{i+1}+h_{i+1}c_{i+2}=3\left( \frac{y_{i+2}-y_{i+1}}{h_{i+1}}-\frac{y_{i+1}-y_i}{h_i} \right)$

其他参数表示为

$\begin{cases} a_i=y_i\\ b_i=\frac{y_{i+1}-y_i}{h_i}-\frac{c_{i+1}+2c_i}{3}h_i\\ d_i=\frac{c_{i+1}-c_i}{3h_i}\\\end{cases}$

2.2 边界条件

注意到关于 $c_i$ 的线性方程仅有 $n - 2$ 个，而未知向量

$\boldsymbol{c}=\left[ \begin{matrix} c_0& c_1& \cdots& c_{n-2}& c_{n-1}\\\end{matrix} \right] ^T$

共有 $n$ 个元素，欠定方程组不足以进行求解。这是因为曲线首末处没有拼接约束，需要人为设定边界条件，常用的边界条件有

自然边界(Natural Spline)：令端点二阶导为零，即
$f_{0}^{''}\left( x_0 \right) =f_{n-1}^{''}\left( x_{n-1} \right) =0$
固定边界(Clamped Spline)：令端点一阶导为常数，即
$f_{0}^{'}\left( x_0 \right) =A,f_{n-1}^{'}\left( x_{n-1} \right) =B$
非扭结边界(Not-A-Knot Spline)：令前两个点与最后两个点的三阶导值相等，即
$f_{0}^{'''}\left( x_0 \right) =f_{1}^{'''}\left( x_1 \right) , f_{n-2}^{'''}\left( x_{n-2} \right) =f_{n-1}^{'''}\left( x_{n-1} \right)$

2.3 系数反解

本节选择自然边界，则 $c_0=c_{n-1}=0$ ，将关于 $c_i$ 的线性方程改写为矩阵形式

$\left[ \begin{matrix} 1& & & & & \\ h_0& 2\left( h_0+h_1 \right)& h_1& & & \\ & h_1& 2\left( h_1+h_2 \right)& h_2& & \\ & & h_2& 2\left( h_2+h_3 \right)& h_3& \\ & & & & \ddots& \\ & & & & & 1\\\end{matrix} \right] \left[ \begin{array}{c} c_0\\ c_1\\ c_2\\ c_3\\ \vdots\\ c_{n-1}\\\end{array} \right] =3\left[ \begin{array}{c} 0\\ \frac{y_2-y_1}{h_1}-\frac{y_1-y_0}{h_0}\\ \frac{y_3-y_2}{h_2}-\frac{y_2-y_1}{h_1}\\ \frac{y_4-y_3}{h_3}-\frac{y_3-y_2}{h_2}\\ \vdots\\ 0\\\end{array} \right]$

该方程组有唯一解

3 算法仿真

3.1 ROS C++仿真

核心代码如下所示

std::vector<double> CubicSpline::spline(std::vector<double> s_list, std::vector<double> dir_list, std::vector<double> t)
{
  // cubic polynomial functions
  std::vector<double> a = dir_list;
  std::vector<double> b, d;

  size_t num = s_list.size();

  std::vector<double> h;
  for (size_t i = 0; i < num - 1; i++)
    h.push_back(s_list[i + 1] - s_list[i]);

  // calculate coefficient matrix
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num, num);
  for (size_t i = 1; i < num - 1; i++)
  {
    A(i, i - 1) = h[i - 1];
    A(i, i) = 2.0 * (h[i - 1] + h[i]);
    A(i, i + 1) = h[i];
  }
  A(0, 0) = 1.0;
  A(num - 1, num - 1) = 1.0;

  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(num, 1);
  for (size_t i = 1; i < num - 1; i++)
    B(i, 0) = 3.0 * (a[i + 1] - a[i]) / h[i] - 3.0 * (a[i] - a[i - 1]) / h[i - 1];

  Eigen::MatrixXd c = A.lu().solve(B);
  for (size_t i = 0; i < num - 1; i++)
  {
    b.push_back((a[i + 1] - a[i]) / h[i] - h[i] * (c(i + 1) + 2.0 * c(i)) / 3.0);
    d.push_back((c(i + 1) - c(i)) / (3.0 * h[i]));
  }

  // calculate spline value and its derivative
  std::vector<double> p;
  for (const auto it : t)
  {
    auto iter = std::find_if(s_list.begin(), s_list.end(), [it](double val) { return val > it; });
    if (iter != s_list.end())
    {
      size_t idx = std::distance(s_list.begin(), iter) - 1;
      double ds = it - s_list[idx];
      p.push_back(a[idx] + b[idx] * ds + c(idx) * std::pow(ds, 2) + d[idx] * std::pow(ds, 3));
    }
  }
  return p;
}

3.2 Python仿真

核心代码如下所示

def spline(self, x_list: list, y_list: list, t: list):
	# cubic polynomial functions
	a, b, c, d = y_list, [], [], []
	h = np.diff(x_list)
	num = len(x_list)

	# calculate coefficient matrix
	A = np.zeros((num, num))
	for i in range(1, num - 1):
		A[i, i - 1] = h[i - 1]
		A[i, i] = 2.0 * (h[i - 1] + h[i])
		A[i, i + 1] = h[i]
	A[0, 0] = 1.0
	A[num - 1, num - 1] = 1.0

	B = np.zeros(num)
	for i in range(1, num - 1):
		B[i] = 3.0 * (a[i + 1] - a[i]) / h[i] - \
				3.0 * (a[i] - a[i - 1]) / h[i - 1]

	c = np.linalg.solve(A, B)
	for i in range(num - 1):
		d.append((c[i + 1] - c[i]) / (3.0 * h[i]))
		b.append((a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2.0 * c[i]) / 3.0)

	# calculate spline value and its derivative
	p, dp = [], []
	for it in t:
		if it < x_list[0] or it > x_list[-1]:
			continue
		i = bisect.bisect(x_list, it) - 1
		dx = it - x_list[i]
		p.append(a[i] + b[i] * dx + c[i] * dx**2 + d[i] * dx**3)
		dp.append(b[i] + 2.0 * c[i] * dx + 3.0 * d[i] * dx**2)

	return p, dp

在这里插入图片描述

3.3 Matlab仿真

核心代码如下所示

function p = spline(s_list, dir_list, t)
    % cubic polynomial functions
    a = dir_list;
    
    [num, ~] = size(s_list);
    
    h = diff(s_list);

    % calculate coefficient matrix
    A = zeros(num, num);
    for i=2:num - 1
        A(i, i - 1) = h(i - 1);
        A(i, i) = 2.0 * (h(i - 1) + h(i));
        A(i, i + 1) = h(i);
    end
  A(1, 1) = 1.0;
  A(num, num) = 1.0;

  B = zeros(num, 1);
  for i=2:num - 1
      B(i, 1) = 3.0 * (a(i + 1) - a(i)) / h(i) - 3.0 * (a(i) - a(i - 1)) / h(i - 1);
  end

  c = A \ B;
  
  b = zeros(num - 1, 1); d = zeros(num - 1, 1);
  for i=1:num - 1
    b(i) = (a(i + 1) - a(i)) / h(i) - h(i) * (c(i + 1) + 2.0 * c(i)) / 3.0;
    d(i) = (c(i + 1) - c(i)) / (3.0 * h(i));
  end

  % calculate spline value and its derivative
  p = [];
  for i =1:length(t)
      idx = find(s_list > t(i));
      if ~isempty(idx)
          id = idx(1) - 1;
          ds = t(i) - s_list(id);
          p = [p; a(id) + b(id) * ds + c(id) * power(ds, 2) + d(id) * power(ds, 3)];
      end
  end
end