五次多项式进行路径规划的工程化实例应用及C++源码分析

一，根据已知约束条件求解五次多项式表达式

设5次曲线的方程表达式为：

其中：

其中，是切线角，是曲率。

 故上式可写为：

对于五次多项式曲线，只要确定6个量，即可唯一确定曲线的表达式。故根据以下6个已知量：

1，起点和终点的坐标:

2,起点和终点的方向角:

3,起点和终点的曲率:

由上述6个已知量，可获得6个方程，从而求解出，唯一确定了五次多项式的表达式。

二，离散化五次多项式的表达式

！！！特别特别特别重要的经验：实际工程应用中均是使用离散化点来近似逼近连续曲线，计算机无法表达连续的曲线，连续的曲线表达式主要应用在数学领域。

离散化方式：从起点开始，每次增加固定长度的弧长来进行离散化。（不要取固定长度的来进行离散化，容易造成离散点分布不均匀的问题，而取固定的弧长则避免了该问题）。

离散化过程举例：

步骤1，根据点处的已知条件，求出，从而确定的值。

在任一点处：

根据上式可得：,其中是人为设计的值，故在任一点处，可根据的值求出，从而确定的值。 

步骤2，根据五次多项式的表达式，一旦确定，则可以根据表达式确定. 

 循环步骤1，步骤2，直到求出的大于终点的横坐标,则循环结束，至此离散化完成。

结果展示：

 源码下载处：