3次样条曲线(Cubic Spline)详解
3次样条曲线(Cubic Spline)详解
3次样条曲线是一种通过一系列三次多项式来逼近给定数据点集的方法。它在每两个采样点之间构造一个3次多项式，能够很好地解决多项式曲线表达能力有限的问题。本文将详细介绍3次样条曲线的定义、参数求解方法及其在运动规划中的应用。
3次样条曲线定义
3次样条曲线（Cubic Spline）是一种通过一系列三次多项式来逼近给定数据点集的方法。每个多项式定义在两个相邻的数据点之间，且满足在数据点处及其一阶导数和二阶导数都连续的条件。
简单来说，它在每两个采样点之间构造了一个3次多项式。这样只要采样点和点之间距离足够近，理论上可以表达任何曲线。
我们先从1维的3次样条开始解释，比如图上的这5个点，我们在每两个点之间构造一个3次多项式。那么有如下定义：
$$
S_j(x)=a_j+b_j\left(x-x_j\right)+c_j\left(x-x_j\right)^2+d_j\left(x-x_j\right)^3
$$
其中，$x_j$和$x_{j+1}$是第j段样条的起点和终点，$a_j, b_j, c_j, d_j$是第j段样条的参数。
很好理解的是，每段样条有起点和终点的这些约束：
$$
\begin{align*}
S_j\left(x_j\right)&=y_j \
S_j\left(x_{j+1}\right)&=S_{j+1}\left(x_{j+1}\right)=y_{j+1} \
S_j^{\prime}\left(x_{j+1}\right)&=S_{j+1}^{\prime}\left(x_{j+1}\right)\
S_j^{\prime \prime}\left(x_{j+1}\right)&=S_{j+1}^{\prime \prime}\left(x_{j+1}\right)
\end{align*}
$$
以及第一个点和最后一个点，我们给的2阶导约束：
$$
S_0^{\prime \prime}(0)=S_{n+1}^{\prime \prime}\left(x_n\right)=0
$$
3次样条曲线参数求解
带入方程求解样条曲线的参数，求解$a_j$：
$$
a_j=y_j
$$
求解$c_j$：
将各个样条方程统一放到$A, B$矩阵中，求解线性方程组。
$$
A c_j=B
$$
其中，
$$
A=\left[\begin{array}{cccccc}
1 & 0 & 0 & 0 & \cdots & 0 \
h_0 & 2\left(h_0+h_1\right) & h_1 & 0 & \cdots & 0 \
0 & h_1 & 2\left(h_1+h_2\right) & h_2 & \cdots & 0 \
0 & 0 & h_2 & 2\left(h_2+h_3\right) & \cdots & 0 \
0 & 0 & 0 & h_3 & \ddots & \
\vdots & \vdots & & & & \
0 & 0 & 0 & \cdots & 0 & 1
\end{array}\right]
$$
$$
B=\left[\begin{array}{c}
0 \
\frac{3}{h_1}\left(a_2-a_1\right)-\frac{3}{h_0}\left(a_1-a_0\right) \
\vdots \
\frac{3}{h_{n-1}}\left(a_n-a_{n-1}\right)-\frac{3}{h_{n-2}}\left(a_{n-1}-a_{n-2}\right) \
0
\end{array}\right]
$$
其中，$h_i=x_{i+1}-x_i$
求解$d_j$：
$$
d_j=\frac{c_{j+1}-c_j}{3 h_j}
$$
求解$b_j$：
$$
b_j=\frac{1}{h_j}\left(a_{j+1}-a_j\right)-\frac{h_j}{3}\left(2 c_j+c_{j+1}\right)
$$
基于3次样条曲线的参数方程
与上一节类似，我们可以在$x,y$两个坐标分别用参数方程，构造2维的3次样条曲线。计算点的朝向角$\theta$和曲率$k$：
$$
\begin{align*}
\theta&=\tan ^{-1} \frac{y^{\prime}}{x^{\prime}} \
\kappa&=\frac{y^{\prime \prime} x^{\prime}-x^{\prime \prime} y^{\prime}}{\left(x^{\prime 2}+y^{\prime 2}\right)^{\frac{2}{3}}}
\end{align*}
$$
这样我们就可以在$x,y$坐标系下使用3次样条进行轨迹规划了。