自动驾驶算法详解(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、数学模型

路径优化算法：

• 根据导引线和障碍物生成路径边界
• 将导引线在s方向等间隔采样
• 对每个s方向的离散点迭代的优化 , ', '' 。

目标函数：

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 占比较小，优先考虑舒适性，会较少考虑跟线：