基于多项式曲率曲线描述其速度、加速度、加加速度特性

多项式过渡曲线构造

上一篇讲述了关于无人车避障场景下的路径规划，在角点过渡中贝塞尔曲线的构造方法。而过渡曲线除了Béizer曲线，还有Clothoid曲线、PH曲线、多项式曲线等等。本文主要利用曲率函数为多项式形式进行构造，以实现圆弧与圆弧、直线与直线、圆弧与直线之间的光滑过渡，其中涉及到速度、加速度、加加速度的连续性问题，本文的应用场景为农机的地头转向。本文主要复现一篇论文的结果。

曲率多项式公式

曲率函数是多项式描述的，曲率函数的形式为：
$$\kappa (l)=\sum _{k=0}^i{a}_k{l}^k$$
起始和终点处条件有，
$$\kappa (0)={\kappa }_1$$ $$\kappa (L)={\kappa }_2$$
因此曲率函数的表达式为：
$$\kappa (l)=a_0+({\kappa }_2-{\kappa }_1)\sum _{k=0}^i{a}_k{l}^k$$
其中i的取值分别取1，3、5、7、9。
根据起点和终点的切线之间的夹角可分为四种路径，分别是（a）HoL、（b）HoC、（c）RHO、（d）OMG。如下图1所示。四条路径由BT和TE两条过渡曲线构成，总长度为2L，起点处和终点处的曲率均为0，在T点处曲率为1/R。

以上4条路径的起始点和终点的切线之间的夹角不同，分别为π/2，π，3π/2，2π。
HoL曲线：$$\alpha =\pi /2$$ $${\theta }_T=\pi /4$$
HoC曲线：$$\alpha =\pi$$ $${\theta }_T=\pi /2$$
RHO曲线：$$\alpha =3\pi /2$$ $${\theta }_T=3\pi /4$$
OMG曲线：$$\alpha =2\pi$$ $${\theta }_T=\pi$$

需要补充的知识点：

（1）平面曲率的定义：
$$\kappa (l)=\underset{\Delta \to 0}{\lim }\frac{\Delta \theta }{\Delta l}=\frac{d\theta }{dl}$$
（2）曲率函数确定后，曲线切线角函数定义为：
$$\theta (l)=\int \kappa (l)dl$$
（3）曲线上任意点的坐标表示为：
$$x(l)=\int cos\theta (l)dl$$ $$y(l)=\int sin\theta (l)dl$$
（4）假设沿路径的最大速度为常数，因此过渡曲线上含时间t的任意点的坐标表示为：
$$x({\kappa }_1,{\kappa }_2,{\theta }_b,t)={\int }_0^{t}cos(\theta ({\kappa }_1,{\kappa }_2,{\theta }_b,\tau ))d\tau$$$$y({\kappa }_1,{\kappa }_2,{\theta }_b,t)={\int }_0^{t}sin(\theta ({\kappa }_1,{\kappa }_2,{\theta }_b,\tau ))d\tau$$
（5）沿着单条过渡曲线从B点运动到T点的时间：
$$t_1=\frac{L}{v_{max}}$$
（6）过渡曲线上从点B到点T的距离：
$$x_{BT}=x(0,\kappa ,0,t_1)$$ $$y_{BT}=y(0,\kappa ,0,t_1)$$
（7）若已知圆心坐标（xs,ys），由几何关系有，起点B的坐标为
$$x_B=x_s+Rsin{\theta }_T-x_{BT}$$ $$y_B=y_s-Rcos{\theta }_T-y_{BT}$$
（8）过渡曲线的坐标表达式为：

以下分别采用一次多项式、三次多项式、五次多项式、七次多项式、九次多项式对四种路径进行构造。本文起始点坐标假定为(0,0)，半径R为7.5m，最大速度Vm为 π m/s。

HoL曲线——90°转向过渡

共同条件：
$$L=2{\theta }_{T}R=\pi R/2$$

计算公式

以曲率函数为一次多项式计算为例。

（1）曲率公式：

（2）轨迹公式：

（3）速度公式：

（4）加速度公式：

（5）加加速度公式：
此处略掉。

计算图

（1）曲率对比图：

（2）x坐标对比图：

（3）y坐标对比图：

（4）xy轨迹对比图：

（5）x方向速度对比图：

（6）y方向速度对比图：

（7）x方向加速度对比图：

（8）y方向加速度对比图：

（9）加速度对比图：

（10）x方向加加速度对比图：

（11）y方向加加速度对比图：

（12）加加速度对比图：

HoC 曲线——180°转向过渡

共同条件： $$L=2{\theta }_{T}R=\pi R$$
其曲率函数形式与上一样，只是其中的θT不一样。

计算图

（1）曲率对比图：

（2）xy轨迹对比图：

（3）x坐标对比图：

（4）y坐标对比图：

（5）x方向速度对比图：

（6）y方向速度对比图：

（7）x方向加速度对比图：

（8）y方向的加速度对比图：

（9）加速度对比图：

（10）x方向加加速度对比图：

（11）y方向加加速度图：

（12）加加速度对比图：

RHO 曲线——270°转向过渡

共同条件：
$$L=2{\theta }_{T}R=3\pi R/2$$

计算图

（1）曲率对比图：

（2）轨迹对比图：

（3）x坐标对比图：

（4）y坐标对比图：

（5）x方向速度对比图：

（6）y方向速度对比图：

（7）x方向加速度对比图：

（8）y方向加速度对比图：

（9）加速度对比图：

（10）x方向加加速度对比图：

（11）y方向加加速度对比图：

（12）加加速度对比图：

OMG 曲线——0°转向过渡

共同条件：
$$L=2{\theta }_{T}R=2\pi R$$

计算图

（1）曲率对比图：

（2）轨迹对比图：

（3）x坐标对比图：

（4）y坐标对比图：

（5）x方向速度对比图：

（6）y方向速度对比图：

（7）x方向加速度对比图：

（8）y方向加速度对比图：

（9）加速度对比图：

（10）x方向加加速度对比图：

（11）y方向加加速度对比图：

（12）加加速度对比图：

参考文献

[1]Application of Polynomial Transition Curves for Trajectory Planning on the Headlands:DOI:10.3390/agriculture10050144