样条曲线_Apollo规划算法基于样条曲线的平滑分析（一）

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

样条曲线的思想是把一个长线分成N段，每段用一个多项式去表示，本文为了简化公式书写，示例中均使用三次多项式表示，实际算法中阶数根据需求不同有不同定义。

各段多项式所描述的线段还要保证在衔接处相连。为了保证曲线整体平滑性，各衔接处的一阶导、二阶导也要连续。

下面讲解如何运用样条曲线平滑frenet下的离散路径。在frenet坐标系下，实际行驶的离散路径可以表示为g[s]，s表示从起点开始沿参考线（通常为道路中心线）行驶的累积里程，g[s]表示在该里程s下路径距离参考线的距离。g[s]可以用常用的离散路径规划算法获得。本文讲解如何在已经获得离散路径g[s]的前提下，对离散路径g[s]进行平滑，获得连续的平滑路径f(x)。

令f(x)为样条曲线，由N段曲线相连构成，每段曲线的表达式为

令

，

假设离散路径g[s]在每段

内有M个离散点，

在这M个点的位置采样。如果我们想求得曲线f(x)离离散路径最近，则样条曲线的每段表达式的目标函数可以设置为

因为

为常量，不影响求最优解，将其去除。每段分段函数

的x都是从0开始的自变量，在采样点的取值即为在此采样段的距离，这样只要采样间隔确定，

就可以固定，进而

也可以固定，而

又是已知值。故目标函数可表示为关于样条函数各系数的表达式：

其中，X为

，C为

。

接下来构造约束方程。之前讲过，样条曲线各段多项式所描述的线段还要保证在衔接处相连，同时为了保证曲线整体平滑性，各衔接处的一阶导、二阶导、三阶导也要连续。约束等式如下：

因为通常平滑后的起点终点要和原离散路径保持一致，故还需加入起点终点约束：

平滑的过程中，还需保证平滑后的路径不超出一定范围，以及不和障碍物发生碰撞，所以还要添加各处边界约束，注意需要找出约束对应样条函数的自变量值x：

至此，基于样条曲线对离散路径进行平滑的模型建立完毕，求解出样条曲线各段表达式系数，即可把离散路径表示为连续路径。

自动驾驶算法工程师一枚

欢迎勾搭