代码收藏家 自动驾驶算法详解(1) : Apollo路径规划 Piecewise Jerk Path Optimizer的python实现
本文作为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实现
prescan联合simulink进行FCW的仿真_自动驾驶 Player的博客-CSDN博客
本文正文如下：
1、Apollo 的Piecewise Jerk Path Optimizer算法流程：
2、数学模型
路径优化算法：
目标函数：
QP问题的表达形式:
所以需要将目标函数J 以及边界bound的约束条件改为QP 问题的形式，这里Python代码在考虑约束时忽略了车辆运动学的约束，在Apollo 的算法中还考虑了车辆运动学的约束。
将路径优化问题转化为QP问题的方法如下：
（1）用path上每个采样点的横向坐标，横向一阶导、二阶导构建X 矩阵：
（2）通过展开多项式推导后，按下列公式构建P、q矩阵：
（3）根据上下边界的约束，构建A 矩阵：
（4）根据path bound的约束，构建l、u矩阵：
3、Python 代码实现：
import osqp
import numpy as np
import matplotlib.pyplot as plt
from scipy import sparse
import random
# 道路上放置障碍物，共设置4个障碍物
# 每个障碍物obj 使用4个数值来表示，障碍物形状为矩形，四个数据依次表示 start_s,end_s,l_low,l_up
obs = [[5,10,2,3],[18,22,-3,-2],[25,30,0,1]]
s_len = 50
delta_s = 0.1
# n = 500
n = int(s_len / delta_s)
# len(x) = 500
x = np.linspace(0,s_len,n)
# len(bound) = 2503
up_bound = [0]*(5*n + 3)
low_bound = [0]*(5*n + 3)
# len(s_ref) = 1500
s_ref = [0]*3*n
dddl_bound = 0.01
####################边界提取################
l_bound = 2
safe_delta_l = 0.1
for i in range(n):
for j in range(len(obs)):
if (x[i] >= obs[j][0] and x[i] <= obs[j][1]) and ((obs[j][2] < l_bound) and(obs[j][3] > - l_bound) ):
if (obs[j][2] + (obs[j][3] - obs[j][2])/2) >= 0 :
low_ = -l_bound
up_ = obs[j][2] - safe_delta_l
else:
low_ = obs[j][3]+ safe_delta_l
up_ = l_bound
break
else:
up_ = l_bound
low_ = -l_bound
# 先根据障碍物调整边界
up_bound[i] = up_
low_bound[i] = low_
# reference line 为上下边界的中线
s_ref[i] = 0.5*(up_ + low_)
####################构造P和Q################
w_l = 1
w_dl = 1
w_ddl = 1
w_dddl = 1
eye_n = np.identity(n)
zero_n = np.zeros((n, n))
P_zeros = zero_n
P_l = w_l * eye_n
P_dl = w_dl * eye_n
P_ddl = (w_ddl + 2*w_dddl/delta_s/delta_s) * eye_n -2 * w_dddl / delta_s/delta_s* np.eye(n,k = -1)
P_ddl[0][0] = w_ddl + w_dddl/delta_s/delta_s
P_ddl[n-1][n-1] = w_ddl + w_dddl/delta_s/delta_s
# p.shape = 1500 * 1500
P = sparse.csc_matrix(np.block([
[P_l,P_zeros,P_zeros],
[P_zeros,P_dl,P_zeros],
[P_zeros,P_zeros,P_ddl]
]))
# q.shape = 1500
q = np.array([-w_l*s_ for s_ in s_ref])
####################构造A和LU################
#构造：l(i+1) = l(i) + l'(i) * delta_s + 1/2 * l''(i) * delta_s^2 + 1/6 * l'''(i) * delta_s^3
A_ll = -eye_n + np.eye(n,k = 1)
A_ldl = -delta_s * eye_n
A_lddl = -0.5 * delta_s * delta_s * eye_n
A_l = (np.block([
[A_ll,A_ldl,A_lddl]
]))
# 构造：l'(i+1) = l'(i) + l''(i) * delta_s + 1/2 * l'''(i) * delta_s^2
A_dll = zero_n
A_dldl = -eye_n + np.eye(n,k = 1)
A_dlddl = -delta_s * eye_n
A_dl = np.block([
[A_dll,A_dldl,A_dlddl]
])
A_ul = np.block([
[eye_n,zero_n,zero_n],
[zero_n,zero_n,zero_n],
[zero_n,zero_n,zero_n]
])#3n*3n
# 初始化设置
A_init = np.zeros((3, 3*n))
A_init[0][0] = 1
# A.shape = 2503 * 1500
A = sparse.csc_matrix(np.row_stack((A_ul,A_l,A_dl,A_init)))
low_bound[5*n] = 1
up_bound[5*n] = 1
# l.shape = 2503 * 1
l = np.array(low_bound)
# u.shape = 2503 * 1
u = np.array(up_bound)
# Create an OSQP object
prob = osqp.OSQP()
# Setup workspace and change alpha parameter
prob.setup(P, q, A, l, u, alpha=1.0)
# Solve problem
res = prob.solve()
s_seg = range(0,500,1)
fig = plt.figure()
ax = fig.add_subplot(111)
# obs = [[5,10,1,2],[15,20,-2,-0.5],[35,39,0,1.5]]
# rect1 = plt.Rectangle((50,1),100,2)
# ax.add_patch(rect1)
# rect2 = plt.Rectangle((150,-2),200,-0.5)
# ax.add_patch(rect2)
ax.plot(s_seg,u[:n],'.',color = 'blue')
ax.plot(s_seg,l[:n],'.',color = 'black')
ax.plot(s_seg,s_ref[:n],'.',color = 'yellow')
ax.plot(s_seg,res.x[:n],'.',color = 'red')
plt.show()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 占比较小，优先考虑舒适性，会较少考虑跟线：
来源：自动驾驶Player
