一,问题分析
路径规划的本质目的:规划出一条安全(无碰撞),舒适(平滑),符合车辆运动学的轨迹。
如下图所示,决策算法开辟凸空间后,路径规划算法用数值优化的方法搜索出一条最优路径,即将问题转化为二次规划的问题,采用二次规划的求解器进行求解。
二次规划问题的求解顺序:
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,综上所述,以上各式已经将目标函数和约束条件变形为二次规划的标准形式,采用二次规划求解器即可获得曲线平滑的最优解。