本文作为Apollo Planning决策规划代码详细解析系列文章的补充,将使用Python代码以及anaconda环境,来实现Apollo 决策规划Planning 模块里的 Piecewise Jerk Path Optimizer算法。
Piecewise Jerk Path Optimizer算法以上游模块决策的path bound,参考线reference line,规划起点start point为基础,通过在凸空间里将规划问题构建为二次优化问题,调用OSQP 库的求解器进行二次规划问题求解,从而得到最优路径。
在后续的文章也会详细介绍Apollo 中 PiecewiseJerkPathOptimizer 这个task,系列文章如下:
Apollo Planning决策规划代码详细解析 (1):Scenario选择
Apollo Planning决策规划代码详细解析 (2):Scenario执行
Apollo Planning决策规划代码详细解析 (3):stage执行
Apollo Planning决策规划代码详细解析 (4):Stage逻辑详解
Apollo Planning决策规划代码详细解析 (5):规划算法流程介绍
Apollo Planning决策规划代码详细解析 (6):LaneChangeDecider
Apollo Planning决策规划代码详细解析 (7): PathReuseDecider
Apollo Planning决策规划代码详细解析 (8): PathLaneBorrowDecider
Apollo Planning决策规划代码详细解析 (9): PathBoundsDecider
Apollo Planning决策规划代码详细解析 (10):PiecewiseJerkPathOptimize
算法仿真技术详解:
LQR算法进行轨迹跟踪,lqr_speed_steering_control( )的python实现
本文正文如下:
1、Apollo 的Piecewise Jerk Path Optimizer算法流程:
2、数学模型
路径优化算法:
- 根据导引线和障碍物生成路径边界
- 将导引线在s方向等间隔采样
- 对每个s方向的离散点迭代的优化 𝑙, 𝑙', 𝑙'' 。
目标函数:
QP问题的表达形式:
所以需要将目标函数J 以及边界bound的约束条件改为QP 问题的形式,这里Python代码在考虑约束时忽略了车辆运动学的约束,在Apollo 的算法中还考虑了车辆运动学的约束。
将路径优化问题转化为QP问题的方法如下:
(1)用path上每个采样点的横向坐标,横向一阶导、二阶导构建X 矩阵:
(2)通过展开多项式推导后,按下列公式构建P、q矩阵:
(3)根据上下边界的约束,构建A 矩阵:
(4)根据path bound的约束,构建l、u矩阵:
3、Python 代码实现:
4、结果分析
权重系数以及起点和终点的约束,都会影响轨迹的生成 ;图中蓝色表示bound上边界,黑色表示bound下边界,黄色表示期望参考线(取上下边界终点),红色表示规划的路径,
(1)w_l = 1;w_dl = 1;w_ddl = 1;w_dddl = 1
各项指标比较平均:
(2)w_l = 100;w_dl = 1;w_ddl = 1;w_dddl = 1
w_l 占比较大,优先跟线,会较少考虑舒适性:
(3)w_l = 0.01;w_dl = 1;w_ddl = 1;w_dddl = 1
w_l 占比较小,优先考虑舒适性,会较少考虑跟线: