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

目录

  • 0 专栏介绍
  • 1 什么是样条？
  • 2 三次样条曲线原理
    • 2.1 曲线插值
    • 2.2 边界条件
    • 2.3 系数反解
  • 3 算法仿真
    • 3.1 ROS C++仿真
    • 3.2 Python仿真
    • 3.3 Matlab仿真

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
ight) ,left( x_1,y_1
ight) ,cdots ,left( x_{n-1},y_{n-1}
ight)
ight}

X={(x0?,y0?),(x1?,y1?),?,(xn?1?,yn?1?)}

相邻两点间通过多项式曲线连接，因此共需要拼接

n

?

1

n-1

n?1段曲线。定义三次多项式曲线为

f

i

(

x

)

=

a

i

+

b

i

(

x

?

x

i

)

+

c

i

(

x

?

x

i

)

2

+

d

i

(

x

?

x

i

)

3

??

i

=

0

,

1

,

?
?

,

n

?

1

f_ileft( x
ight) =a_i+b_ileft( x-x_i
ight) +c_ileft( x-x_i
ight) ^2+d_ileft( x-x_i
ight) ^3,,i=0,1,cdots ,n-1

fi?(x)=ai?+bi?(x?xi?)+ci?(x?xi?)2+di?(x?xi?)3i=0,1,?,n?1

其中，当

i

=

n

?

1

i=n-1

i=n?1时的曲线是辅助曲线，用于计算前

n

?

1

n-1

n?1段曲线而不参与实际拼接。对于三次曲线，给出四个约束条件为

{

过插值点

:

f

i

(

x

i

)

=

y

i

曲线连续

:

f

i

(

x

i

+

1

)

=

y

i

+

1

一阶连续

:

f

˙

i

(

x

i

+

1

)

=

f

˙

i

+

1

(

x

i

+

1

)

二阶连续

:

f

¨

i

(

x

i

+

1

)

=

f

¨

i

+

1

(

x

i

+

1

)

?

h

i

=

x

i

+

1

?

x

i

{

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

+

2

c

i

h

i

+

3

d

i

h

i

2

=

b

i

+

1

c

i

+

3

d

i

h

i

=

c

i

+

1

egin{cases} ext{过插值点}: f_ileft( x_i
ight) =y_i\ ext{曲线连续}: f_ileft( x_{i+1}
ight) =y_{i+1}\ ext{一阶连续}: dot{f}_ileft( x_{i+1}
ight) =dot{f}_{i+1}left( x_{i+1}
ight)\ ext{二阶连续}: ddot{f}_ileft( x_{i+1}
ight) =ddot{f}_{i+1}left( x_{i+1}
ight)\end{cases}xRightarrow{h_i=x_{i+1}-x_i}egin{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}

?
?
??过插值点:fi?(xi?)=yi?曲线连续:fi?(xi+1?)=yi+1?一阶连续:f˙?i?(xi+1?)=f˙?i+1?(xi+1?)二阶连续:f¨?i?(xi+1?)=f¨?i+1?(xi+1?)?hi?=xi+1??xi?
??
?
??ai?=yi?ai?+bi?hi?+ci?hi2?+di?hi3?=yi+1?bi?+2ci?hi?+3di?hi2?=bi+1?ci?+3di?hi?=ci+1??

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

h

i

c

i

+

2

(

h

i

+

h

i

+

1

)

c

i

+

1

+

h

i

+

1

c

i

+

2

=

3

(

y

i

+

2

?

y

i

+

1

h

i

+

1

?

y

i

+

1

?

y

i

h

i

)

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

hi?ci?+2(hi?+hi+1?)ci+1?+hi+1?ci+2?=3(hi+1?yi+2??yi+1???hi?yi+1??yi??)

其他参数表示为

{

a

i

=

y

i

b

i

=

y

i

+

1

?

y

i

h

i

?

c

i

+

1

+

2

c

i

3

h

i

d

i

=

c

i

+

1

?

c

i

3

h

i

egin{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}

?
?
??ai?=yi?bi?=hi?yi+1??yi???3ci+1?+2ci??hi?di?=3hi?ci+1??ci???

2.2 边界条件

注意到关于

c

i

c_i

ci?的线性方程仅有

n

?

2

n-2

n?2个，而未知向量

c

=

[

c

0

c

1

?

c

n

?

2

c

n

?

1

]

T

oldsymbol{c}=left[ egin{matrix} c_0& c_1& cdots& c_{n-2}& c_{n-1}\end{matrix}
ight] ^T

c=[c0??c1????cn?2??cn?1??]T

共有

n

n

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

  • 自然边界(Natural Spline)：令端点二阶导为零，即

    f

    0

    ′

    ′

    (

    x

    0

    )

    =

    f

    n

    ?

    1

    ′

    ′

    (

    x

    n

    ?

    1

    )

    =

    0

    f_{0}^{''}left( x_0
    ight) =f_{n-1}^{''}left( x_{n-1}
    ight) =0

    f0′′?(x0?)=fn?1′′?(xn?1?)=0

  • 固定边界(Clamped Spline)：令端点一阶导为常数，即

    f

    0

    ′

    (

    x

    0

    )

    =

    A

    ,

    f

    n

    ?

    1

    ′

    (

    x

    n

    ?

    1

    )

    =

    B

    f_{0}^{'}left( x_0
    ight) =A,f_{n-1}^{'}left( x_{n-1}
    ight) =B

    f0′?(x0?)=A,fn?1′?(xn?1?)=B

  • 非扭结边界(Not-A-Knot Spline)：令前两个点与最后两个点的三阶导值相等，即

    f

    0

    ′

    ′

    ′

    (

    x

    0

    )

    =

    f

    1

    ′

    ′

    ′

    (

    x

    1

    )

    ,

    f

    n

    ?

    2

    ′

    ′

    ′

    (

    x

    n

    ?

    2

    )

    =

    f

    n

    ?

    1

    ′

    ′

    ′

    (

    x

    n

    ?

    1

    )

    f_{0}^{'''}left( x_0
    ight) =f_{1}^{'''}left( x_1
    ight) , f_{n-2}^{'''}left( x_{n-2}
    ight) =f_{n-1}^{'''}left( x_{n-1}
    ight)

    f0′′′?(x0?)=f1′′′?(x1?),fn?2′′′?(xn?2?)=fn?1′′′?(xn?1?)

2.3 系数反解

本节选择自然边界，则

c

0

=

c

n

?

1

=

0

c_0=c_{n-1}=0

c0?=cn?1?=0，将关于

c

i

c_i

ci?的线性方程改写为矩阵形式

[

1

h

0

2

(

h

0

+

h

1

)

h

1

h

1

2

(

h

1

+

h

2

)

h

2

h

2

2

(

h

2

+

h

3

)

h

3

?

1

]

[

c

0

c

1

c

2

c

3

?

c

n

?

1

]

=

3

[

0

y

2

?

y

1

h

1

?

y

1

?

y

0

h

0

y

3

?

y

2

h

2

?

y

2

?

y

1

h

1

y

4

?

y

3

h

3

?

y

3

?

y

2

h

2

?

0

]

left[ egin{matrix} 1& & & & & \ h_0& 2left( h_0+h_1
ight)& h_1& & & \ & h_1& 2left( h_1+h_2
ight)& h_2& & \ & & h_2& 2left( h_2+h_3
ight)& h_3& \ & & & & ddots& \ & & & & & 1\end{matrix}
ight] left[ egin{array}{c} c_0\ c_1\ c_2\ c_3\ vdots\ c_{n-1}\end{array}
ight] =3left[ egin{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}
ight]

?1h0??2(h0?+h1?)h1??h1?2(h1?+h2?)h2??h2?2(h2?+h3?)?h3???1?
?
?c0?c1?c2?c3??cn?1??
?=3
?0h1?y2??y1???h0?y1??y0??h2?y3??y2???h1?y2??y1??h3?y4??y3???h2?y3??y2???0?
?

该方程组有唯一解

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深度学习实战》
  • 《机器学习强基计划》
  • 《运动规划实战精讲》
  • …



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