0 专栏介绍

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

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

---

1 什么是样条？

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

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

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

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

2 三次样条曲线原理

2.1 曲线插值

给定一系列插值点

X = { ( x 0 , y 0 ) , ( x 1 , y 1 ) , ⋯   , ( x n − 1 , y n − 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\} X={(x0​,y0​),(x1​,y1​),⋯,(xn−1​,yn−1​)}

相邻两点间通过多项式曲线连接，因此共需要拼接$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{array}{l}\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{array}\stackrel{h_i=x_{i+1}-x_i}{\Rightarrow }\{\begin{array}{l}{a}_i=y_i\\ a_i+b_i{h}_i+c_i{h}_i^2+d_i{h}_i^3=y_{i+1}\\ b_i+2c_i{h}_i+3d_i{h}_i^2=b_{i+1}\\ c_i+3d_i{h}_i=c_{i+1}\end{array}$$

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

$$h_i{c}_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{array}{l}{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{array}$$

2.2 边界条件

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

$$\mathbf{c}={\left[\begin{array}{ccccc}{c}_0& c_1& \cdots & c_{n-2}& c_{n-1}\end{array}\right]}^T$$

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

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

2.3 系数反解

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

$$\left[\begin{array}{cccccc}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{array}\right]\left[\begin{array}{c}{c}_0\\ c_1\\ c_2\\ c_3\\ ⋮\\ 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}\\ ⋮\\ 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

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇