局部路径规划 - 01 曲线插值法

算法简介#

曲线插值法是在满足某些特定条件下，对路径曲线的拟合。
常见的拟合曲线有：多项式曲线、双圆弧段曲线、正弦函数曲线、贝塞尔曲线、B样条曲线等；

算法思想#

曲线插值法的核心思想是基于预先构造的曲线类型，根据车辆期望达到的状态（比如要求车辆到达某点的速度和加速度为期望值），将此期望值作为边界条件代入曲线类型进行方程求解，获得曲线的相关系数；
曲线所有的相关系数一旦确定，轨迹规划随之完成。

多项式曲线#

多项式曲线分为三次多项式曲线、五次多项式曲线、七次多项式曲线；
我们注意到上述的多项式曲线都是奇数次，事实上，多项式曲线一般而言都是奇数次，这是由边界条件引起的。边界条件一般包括车辆的起始状态和终止状态（如换道轨迹的起点状态和终点状态），由于偶数个状态导致有唯一解的方程系数为偶数，故偶数个系数的多项式也就是奇数多项式。
三次多项式曲线：最多能确定每一个期望点的两个维度的期望状态，一般来说就是位置和速度

{\begin{cases} x (t) = a_{0} + a_{1} t + a_{2} t^{2} + a_{3} t^{3} \\ y (t) = b_{0} + b_{1} t + b_{2} t^{2} + b_{3} t^{3} \end{cases}

$\begin{cases} x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3\\ y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3\\ \end{cases}$

五次多项式曲线：最多能确定每一个期望点的三个维度的期望状态，一般来说就是位置、速度和加速度

{\begin{cases} x (t) = a_{0} + a_{1} t + a_{2} t^{2} + a_{3} t^{3} + a_{4} t^{4} + a_{5} t^{5} \\ y (t) = b_{0} + b_{1} t + b_{2} t^{2} + b_{3} t^{3} + b_{4} t^{4} + b_{5} t^{5} \end{cases}

$\begin{cases} x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3 +a_4t^4+a_5t^5\\ y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5\\ \end{cases}$

七次多项式曲线：最多能确定每一个期望点的三个维度的期望状态，一般来说就是位置、速度、加速度和加加速度（冲击度，jerk）

{\begin{cases} x (t) = a_{0} + a_{1} t + a_{2} t^{2} + a_{3} t^{3} + a_{4} t^{4} + a_{5} t^{5} + a_{6} t^{6} + a_{7} t^{7} \\ y (t) = b_{0} + b_{1} t + b_{2} t^{2} + b_{3} t^{3} + b_{4} t^{4} + b_{5} t^{5} + b_{6} t^{6} + b_{7} t^{7} \end{cases}

$\begin{cases} x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3 +a_4t^4+a_5t^5 + a_6t^6+a_7t^7\\ y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5 +b_6t^6+b_7t^7\\ \end{cases}$

曲线插值法（以五次多项式为例）#

首先，我们定义如下的五次多项式：

{\begin{cases} x (t) = a_{0} + a_{1} t + a_{2} t^{2} + a_{3} t^{3} + a_{4} t^{4} + a_{5} t^{5} \\ y (t) = b_{0} + b_{1} t + b_{2} t^{2} + b_{3} t^{3} + b_{4} t^{4} + b_{5} t^{5} \end{cases}

$\begin{cases} x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3 +a_4t^4+a_5t^5\\ y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5\\ \end{cases}$

我们将起始时刻定义为 $t_0$ ，起始时刻的位置、速度和加速度均已知，我们构造如下的纵向和横向方程：
- 起始时刻的位置方程：
$\begin{cases} x\left( t_0 \right)= a_0 +a_1t_0+a_2t_0^2+a_3t_0^3 +a_4t_0^4+a_5t_0^5\\ y\left( t_0 \right)=b_0 +b_1t_0+b_2t_0^2+b_3t_0^3+b_4t_0^4+b_5t_0^5\\ \end{cases}$
- 起始时刻的速度方程：
$\begin{cases} x^\prime\left( t_0 \right)= a_1 +2a_2t_0+3a_3t_0^2 +4a_4t_0^3+5a_5t_0^4\\ y^\prime\left( t_0 \right)=b_1 +2b_2t_0+3b_3t_0^2+4b_4t_0^3+5b_5t_0^4\\ \end{cases}$
- 起始时刻的加速度方程：
$\begin{cases} x^{\prime\prime}\left( t_0 \right)= 2a_2+6a_3t_0 + 12a_4t_0^2+20a_5t_0^3\\ y^{\prime\prime}\left( t_0 \right)=2b_2+6b_3t_0 + 12b_4t_0^2+20b_5t_0^3\\ \end{cases}$
同理，我们可以构造终止时刻 $t_1$ 的的横向和纵向方程，这里就不再详细描述；
我们将起始位置和终止位置的横纵向方程统一用矩阵表达为：

X = [\begin{matrix} x_{0} \\ x_{0}^{'} \\ x_{0}^{''} \\ x_{1} \\ x_{1}^{'} \\ x_{1}^{''} \end{matrix}] = [\begin{matrix} t_{0}^{5} & t_{0}^{4} & t_{0}^{3} & t_{0}^{2} & t_{0} & 1 \\ 5 t_{0}^{4} & 4 t_{0}^{3} & 3 t_{0}^{2} & 2 t_{0}^{1} & 1 & 0 \\ 20 t_{0}^{3} & 12 t_{0}^{2} & 6 t_{0} & 2 & 0 & 0 \\ t_{1}^{5} & t_{1}^{4} & t_{1}^{3} & t_{1}^{2} & t_{1} & 1 \\ 5 t_{1}^{4} & 4 t_{1}^{3} & 3 t_{1}^{2} & 2 t_{1}^{1} & 1 & 0 \\ 20 t_{1}^{3} & 12 t_{1}^{2} & 6 t_{1} & 2 & 0 & 0 \end{matrix}] [\begin{matrix} a_{5} \\ a_{4} \\ a_{3} \\ a_{2} \\ a_{1} \\ a_{0} \end{matrix}] = T \times A

$X=\left[ \begin{array}{c} x_0 \\ x_0^\prime\\ x_0^{\prime\prime}\\ x_1\\ x_1^\prime\\ x_1^{\prime\prime}\\ \end{array} \right] = \left[ \begin{array}{c} t_0^5 & t_0^4 & t_0^3 & t_0^2 & t_0 & 1 \\ 5t_0^4 & 4t_0^3 & 3t_0^2 & 2t_0^1 & 1 & 0 \\ 20t_0^3 & 12t_0^2 & 6t_0 & 2 & 0 & 0 \\ t_1^5 & t_1^4 & t_1^3 & t_1^2 & t_1 & 1 \\ 5t_1^4 & 4t_1^3 & 3t_1^2 & 2t_1^1 & 1 & 0 \\ 20t_1^3 & 12t_1^2 & 6t_1 & 2 & 0 & 0 \\ \end{array} \right] \left[ \begin{array}{c} a_5 \\ a_4\\ a_3\\ a_2\\ a_1\\ a_0\\ \end{array} \right]=T\times A$

Y = [\begin{matrix} y_{0} \\ y_{0}^{'} \\ y_{0}^{''} \\ y_{1} \\ y_{1}^{'} \\ y_{1}^{''} \end{matrix}] = [\begin{matrix} t_{0}^{5} & t_{0}^{4} & t_{0}^{3} & t_{0}^{2} & t_{0} & 1 \\ 5 t_{0}^{4} & 4 t_{0}^{3} & 3 t_{0}^{2} & 2 t_{0}^{1} & 1 & 0 \\ 20 t_{0}^{3} & 12 t_{0}^{2} & 6 t_{0} & 2 & 0 & 0 \\ t_{1}^{5} & t_{1}^{4} & t_{1}^{3} & t_{1}^{2} & t_{1} & 1 \\ 5 t_{1}^{4} & 4 t_{1}^{3} & 3 t_{1}^{2} & 2 t_{1}^{1} & 1 & 0 \\ 20 t_{1}^{3} & 12 t_{1}^{2} & 6 t_{1} & 2 & 0 & 0 \end{matrix}] [\begin{matrix} b_{5} \\ b_{4} \\ b_{3} \\ b_{2} \\ b_{1} \\ b_{0} \end{matrix}] = T \times B

$Y=\left[ \begin{array}{c} y_0 \\ y_0^\prime\\ y_0^{\prime\prime}\\ y_1\\ y_1^\prime\\ y_1^{\prime\prime}\\ \end{array} \right] = \left[ \begin{array}{c} t_0^5 & t_0^4 & t_0^3 & t_0^2 & t_0 & 1 \\ 5t_0^4 & 4t_0^3 & 3t_0^2 & 2t_0^1 & 1 & 0 \\ 20t_0^3 & 12t_0^2 & 6t_0 & 2 & 0 & 0 \\ t_1^5 & t_1^4 & t_1^3 & t_1^2 & t_1 & 1 \\ 5t_1^4 & 4t_1^3 & 3t_1^2 & 2t_1^1 & 1 & 0 \\ 20t_1^3 & 12t_1^2 & 6t_1 & 2 & 0 & 0 \\ \end{array} \right] \left[ \begin{array}{c} b_5 \\ b_4\\ b_3\\ b_2\\ b_1\\ b_0\\ \end{array} \right]=T\times B$

对上述矩阵进行求解（ $A=T^{-1} \times X$ ， $B=T^{-1} \times Y$ ），我们可以得到唯一的系数矩阵解，即得到唯一的多项式曲线，该曲线上每一点的导数代表着车辆经过该点时的速度。多项式曲线插值法轨迹规划出的曲线是路径+速度的耦合结果。

注意：曲线插值法得到的轨迹曲线是关于时间 $t$ 的函数，而非坐标 $y$ 关于坐标 $x$ 的函数。

与双圆弧段换道轨迹进行对比：

双圆弧段换道轨迹由弧AC+线段CD+弧DF构成，如下图所示：

在C点，轨迹曲率由弧AC段的定值突变为0，考虑到方向盘的转角是一个连续缓变的过程，为了让车辆能够完全跟随轨迹，车辆行驶到C点后必须速度为0，让方向盘回正后才能沿给定的轨迹继续行驶，因此双圆弧段换道无法应用于行车路径规划，而应用于泊车路径规划。