决策规划算法四：Piecewise Jerk Path Optimizer

一，问题分析

路径规划的本质目的：规划出一条安全（无碰撞），舒适（平滑），符合车辆运动学的轨迹。

如下图所示，决策算法开辟凸空间后，路径规划算法用数值优化的方法搜索出一条最优路径，即将问题转化为二次规划的问题，采用二次规划的求解器进行求解。

二次规划问题的求解顺序：

1，确定优化变量

2，确定目标函数

3，确定约束条件

4，将目标函数和约束条件变形为二次规划的标准形式

5,用二次规划求解器进行求解，获得优化变量的序列，使得目标函数取得最小值。

注：路径的最终表达式为：，即确定与之间的关系。

二，路径平滑算法的设计思路

该路径平滑算法用3次多项式连接每个离散点，即每段曲线的Jerk为常数，该算法因此而得名：Piecewise Jerk Path Optimizer.

常数Jerk的表达式为：

三，问题求解:以4个点P1,P2,P3,P4为例

1，确定优化变量

优化变量为：

2，确定目标函数

路径的评价指标（软约束）：平滑性，靠近凸空间的中心线，平滑后的曲线终点靠近离散点的终点

(1),平滑性：

则P1矩阵为：

 q1=0

(2),靠近凸空间中心线：

P2矩阵为：

q2矩阵为：

(3),靠近终点：

P3矩阵为：

 q3矩阵为：

3，确定约束条件：硬约束

（1）对的约束

对的约束：

 对()的约束，也就是对航向角的约束，目前取固定值2.0： 

对的约束，也就是对曲率的约束，范围是道路曲率自车允许的最大曲率：

​

对的约束，

（2）由于每段的Jerk为常数，故需要满足数学关系的约束

将带入上面2个等式得：

转化为矩阵为：

 

4，综上所述，以上各式已经将目标函数和约束条件变形为二次规划的标准形式，采用二次规划求解器即可获得曲线平滑的最优解。