决策规划算法四：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，综上所述，以上各式已经将目标函数和约束条件变形为二次规划的标准形式，采用二次规划求解器即可获得曲线平滑的最优解。
