取得贝塞尔曲线x坐标的y值_Apollo规划算法基于样条曲线的平滑分析（二）

欢迎关注微信公众号《不想做科学家的工程师不是好码农》

在《Apollo规划算法基于样条曲线的平滑分析（一）》一文中讲解了如何利用样条曲线平滑在frenet坐标系下的离散规划路径。本文介绍如何使用样条曲线在路宽范围内找到一条曲率尽量小的行驶路径。

考虑沿一条一定宽度的道路行驶，以道路中心线为参考线，要使行驶轨迹曲率尽量小，应该有随行驶距离s的增加，笛卡尔坐标下x、y关于s的二阶导、三阶导需要足够小，即尽量打直了走。依据这个目标，构造目标函数：

其中w为各项所占比重，

和

为笛卡尔坐标系下x、y关于s的样条函数。下面以

为例，讲解如何将上述目标函数展开。

同《Apollo规划算法基于样条曲线的平滑分析（一）》，示例中均使用三次多项式表示，实际算法中阶数根据需求不同有不同定义。令f(x)为表示样条曲线

，其由N段曲线相连构成，每段曲线的表达式为：

的二阶导为：

再对其平方积分得：

不失一般性，令

，

，有：

按如上方式把各样条曲线的各段多项式的目标函数矩阵合并，即构造为关于各段多项式系数的目标函数。

接下来构造约束方程：

一、作为样条曲线，要保证各段线段的衔接处连接且平滑。

二、一般车辆在规划起始位置的朝向确定，所以新生成的曲线还要保证在起始点的朝向约束。

三、上面的目标函数只是要求曲线有尽量小的变化率，并未限制路宽（要求生成的曲线和车道中心线的距离小于一定范围）。因此要使得新生成的样条曲线不超出道路，还要在约束方程中加以约束。

针对第一项约束，同样以

为例，其每段多项式用公式5表示，设两段相邻多项式的前一个多项式自变量x在衔接处的值为

，有：

用同样方式求得各段多项式系数关系后，与$f_x(s)$系数矩阵合并即可获得第一项约束方程。

针对第二项约束，设

为朝向角，有：

进而有

和

的多项式系数关系：

因为tan曲线在圆周内以

为周期，所以还要对

和

的符号加以限制。当朝向角在90度到270度时

小于0，否则

大于零。在朝向角大于180度时，

小于0，否则

大于0。把上述关系带入各分段多项式，合并矩阵，获得第二项约束方程。

针对第三项约束，需要在道路中心线上生成锚点，即按指定间隔选取一串点的位置及朝向信息，以约束新生成曲线在锚点的横纵向的范围内。要实现这个约束，首先需要把锚点为转换到该锚点朝向的坐标系下的坐标，在该坐标系下锚点的横纵坐标值加减限定值即为在该锚点附近需要限定的范围。坐标转换示意图如下：

根据公式19、20有：

转换坐标系后再对新的坐标系下横纵坐标值加以限制：

这样第三项

和

的多项式系数关系也就确定了，把上述关系带入各分段多项式，合并矩阵，获得第三项约束方程。

最终，合并各项矩阵，通过上述模型求解出样条曲线各段表达式系数，即可在路宽范围内找到一条曲率尽量小的行驶路径的样条曲线表达式。

自动驾驶算法工程师一枚

欢迎勾搭