曲线生成

目录

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}

?

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

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}

?

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]

该方程组有唯一解

3 算法仿真

3.1 ROS C++仿真

核心代码如下所示

std::vector CubicSpline::spline(std::vector s_list, std::vector dir_list, std::vector t)

{

// cubic polynomial functions

std::vector a = dir_list;

std::vector b, d;

size_t num = s_list.size();

std::vector 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 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深度学习实战》

《机器学习强基计划》

《运动规划实战精讲》

…

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