目录
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 \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
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_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
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
\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}
⎩
⎨
⎧过插值点: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=yiai+bihi+cihi2+dihi3=yi+1bi+2cihi+3dihi2=bi+1ci+3dihi=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+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)
hici+2(hi+hi+1)ci+1+hi+1ci+2=3(hi+1yi+2−yi+1−hiyi+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
\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}
⎩
⎨
⎧ai=yibi=hiyi+1−yi−3ci+1+2cihidi=3hici+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
\boldsymbol{c}=\left[ \begin{matrix} c_0& c_1& \cdots& c_{n-2}& c_{n-1}\\\end{matrix} \right] ^T
c=[c0c1⋯cn−2cn−1]T
共有
n
n
n个元素，欠定方程组不足以进行求解。这是因为曲线首末处没有拼接约束，需要人为设定边界条件，常用的边界条件有
自然边界(Natural Spline)：令端点二阶导为零，即
f
0
′
′
(
x
0
)
=
f
n
−
1
′
′
(
x
n
−
1
)
=
0
f_{0}^{''}\left( x_0 \right) =f_{n-1}^{''}\left( x_{n-1} \right) =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 \right) =A,f_{n-1}^{'}\left( x_{n-1} \right) =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 \right) =f_{1}^{'''}\left( x_1 \right) , f_{n-2}^{'''}\left( x_{n-2} \right) =f_{n-1}^{'''}\left( x_{n-1} \right)
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[ \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]
1h02(h0+h1)h1h12(h1+h2)h2h22(h2+h3)h3⋱1
c0c1c2c3⋮cn−1
=3
0h1y2−y1−h0y1−y0h2y3−y2−h1y2−y1h3y4−y3−h2y3−y2⋮0
该方程组有唯一解
3 算法仿真
3.1 ROS C++仿真
核心代码如下所示
std::vector
{
// cubic polynomial functions
std::vector
std::vector
size_t num = s_list.size();
std::vector
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
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深度学习实战》《机器学习强基计划》《运动规划实战精讲》…
源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系
好文阅读
您阅读本篇文章共花了：
发表评论