自动驾驶决策规划算法

序章

课程链接：序章

第一章

（1） 细说五次多项式

课程链接：五次多项式

参考一下这篇博客：

初始：位置，速度，加速度

终点：位置，速度，加速度。

边界条件就是六个初始条件，t = 0 时刻的，和t = t0的时刻状态，就可以求解这个五次多项式，得到一个x轴位置，速度， 加速度关于t的方程。和y轴位置，速度，和加速度关于t的方程。

比较简单，只需要代入公式即可。

设一个五次多项式，分别对应的是x(t), y(t), v_x(t), v_y(t), a_x(t), a_y(t)

值得注意的一点是：五次多项式做轨迹规划，y不是关于x的曲线，而是y和x都是关于t的曲线。

写为矩阵形势就是：

目前我们已知的信息是t0时刻的位置、速度和加速度。以及T终点时刻的根据这个初始t = 0的条件, 代入方程，可以求出a0，a1， 和a2.

然后根据T时刻的位置，速度和加速度，代入方程中，可以求得剩下的a3，a4，a5.具体计算如下所示，(这里用横向求解来举例，纵向也是一样的过程)

t = 0的时刻，将初始条件代入方程得到的是：
$$\begin{gathered} x\left(t_0\right)=x\left(0\right)=x_s=a_0 \\ x\prime \left(t_0\right)=v_s=a_1 \\ {x}^{\prime \prime }\left(t_0\right)=a_s=2\ast a_2 \end{gathered}$$
这样可以求得三个方程组中的参数。然后是对T时刻的求解，将终点时刻的已知量代入到函数中，由于a0，a1，a2都是已知的了，我们将其移到左边，只构建右边a3，a4，a5三个未知量的矩阵来求解，首先代入后公式如下：
$$\begin{gathered} x\left(t_T\right)=x\left(T\right)=x_g-a_0-a_{1}T-a_{2}T=a_3{T}^3+a_4{T}^4+a_5{T}^5 \\ x\prime \left(t_T\right)=v_g-a_1-2a_{2}T=3a_3{T}^2+4a_4{T}^3+5a_5{T}^4 \\ {x}^{\prime \prime }\left(t_T\right)=a_g-2\ast a_2=6a_{3}T+12a_4{T}^2+20a_5{T}^3 \end{gathered}$$
利用python自带的np求解工具就可以求得剩余的a参数。写成矩阵形式很简单。

下面结合代码来写一下五次多项式的类。

# ########################################
# 构造一个五次多项式的类
class QuinticPolynomial:def __init__(self, xs, vxs, axs, xe, vxe, axe, time):# 起点条件代入进去得self.a0 = xsself.a1 = vxsself.a2 = axs / 2.0# 终点条件代入进去A = np.array([[time ** 3, time**4, time ** 5],[3*time**2, 4*time**3, 5*time**4],[6*time, 12*time**2, 20*time**3]])b = np.array([xe - self.a0 - self.a1*time - self.a2*time**2,vxe - self.a1 - 2*self.a2*time,axe - 2*self.a2])x = np.linalg.solve(A, b)self.a3 = x[0]self.a4 = x[1]self.a5 = x[2]def calc_point(self, t):  # 计算下一时刻t的位置。xt = self.a0 + self.a1 * t + self.a2 * t**2 + self.a3*t**3 + \self.a4 * t**4 + self.a5*t**5return xtdef calc_first_derivative(self, t):  # 计算速度xt = self.a1 + 2* self.a2*t + 3*self.a3*t**2 + 4*self.a4*t**3 + 5*self.a5**4return xtdef calc_second_derivative(self, t):  # 计算加速度xt = 2*self.a2 + 6*self.a3*t + 12*self.a4*t**2 + 20*self.a5*t**3return xtdef calc_third_derivative(self, t):  # 返回jerkxt = 6*self.a3 + 24*self.a4*t + 60*self.a5*t**2return xt

完整代码如下：、

# 首先是输入的参数
import math
import matplotlib.pyplot as plt
import numpy as np
# ###########################################输入参数
MAX_T = 100.0  # 到达目标所需要的最大时间
MIN_T = 5.0  # 到达目标的最小时间# 起点条件
sx = 10.0
sy = 10.0
syaw = np.deg2rad(10.0)
sv = 1.0
sa = 0.1# 终点条件
gx = 30.0
gy = -10.0
gyaw = np.deg2rad(20.0)  # 终点的偏航角
gv = 1.0
ga = 0.1  # 加速度 m/ss# 设置一个最大加速度与jerk
max_accel = 1.0
max_jerk = 0.5# 时间间隔
dt = 0.1
# ########################################
# 构造一个五次多项式的类
class QuinticPolynomial:def __init__(self, xs, vxs, axs, xe, vxe, axe, time):# 起点条件代入进去得self.a0 = xsself.a1 = vxsself.a2 = axs / 2.0# 终点条件代入进去A = np.array([[time ** 3, time**4, time ** 5],[3*time**2, 4*time**3, 5*time**4],[6*time, 12*time**2, 20*time**3]])b = np.array([xe - self.a0 - self.a1*time - self.a2*time**2,vxe - self.a1 - 2*self.a2*time,axe - 2*self.a2])x = np.linalg.solve(A, b)self.a3 = x[0]self.a4 = x[1]self.a5 = x[2]def calc_point(self, t):  # 计算下一时刻t的位置。xt = self.a0 + self.a1 * t + self.a2 * t**2 + self.a3*t**3 + \self.a4 * t**4 + self.a5*t**5return xtdef calc_first_derivative(self, t):  # 计算速度xt = self.a1 + 2* self.a2*t + 3*self.a3*t**2 + 4*self.a4*t**3 + 5*self.a5**4return xtdef calc_second_derivative(self, t):  # 计算加速度xt = 2*self.a2 + 6*self.a3*t + 12*self.a4*t**2 + 20*self.a5*t**3return xtdef calc_third_derivative(self, t):  # 返回jerkxt = 6*self.a3 + 24*self.a4*t + 60*self.a5*t**2return xt# #######################################
# 构造五次多项式
# 计算出时间、空间、速度、加速度和jerk的信息def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):""":param sx::param sy::param syaw::param sv::param sa::param gx::param gy::param gyaw::param gv::param ga::param max_accel::param max_jerk::param dt::return: time: time resultrx : x position result listry : y position result listryaw : yaw angle result listrv: velocity result listra: accel result list"""# 将起点和终点速度进行横纵向解耦 注意：s代表起始位置，g代表目标位置vxs = sv * math.cos(syaw)vys = sv * math.sin(syaw)vxg = gv * math.cos(gyaw)vyg = gv * math.sin(gyaw)# 加速度解耦axs = sa * math.cos(syaw)ays = sa * math.sin(syaw)axg = ga * math.cos(gyaw)ayg = ga * math.sin(gyaw)# 创建一个计算求得的参考信息列表：time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []# np.arrange(a, b, c)有三个参数的时候，a是起点，b是终点，c是代表步长,生成一个列表# MINT是5，计算未来5个时间步的数据。# 枚举不同时间，生成对应的多项式轨迹。for T in np.arange(MIN_T, MAX_T, MIN_T): # 从最短的时间到最长的时间xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T)  # 横向五次多项式类对象yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T)  # 纵向time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []for t in np.arange(0.0, T+dt, dt):  # 枚举时间步长time.append(t)rx.append(xqp.calc_point(t))ry.append(yqp.calc_point(t))vx = xqp.calc_first_derivative(t)vy = yqp.calc_first_derivative(t)  # 未来dt时刻的坐标v = np.hypot(vx, vy)# v就是将vx和vy进行合成rv.append(v)yaw = math.atan2(vy, vx)ryaw.append(yaw)ax = xqp.calc_second_derivative(t)ay = yqp.calc_second_derivative(t)a = np.hypot(ax, ay)if len(rv) >=2 and rv[-1] - rv[-2] < 0.0:  # 如果速度开始下降，这里加速度需要取反。a *= -1# 说明在减速ra.append(a)jx = xqp.calc_third_derivative(t)  # 计算jerkjy = yqp.calc_third_derivative(t)  # 计算y方向的j = np.hypot(jx, jy)if len(ra) >= 2 and ra[-1] - ra[-2] < 0.0:  # 将加速度开始下降，jerk需要取反j *= -1rj.append(j)if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:print("find path!!")breakreturn time, rx, ry, ryaw, rv, ra, rjtime, rx, ry, ryaw, rv, ra, rj = quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt)# 绘制路径图
plt.subplots(1)
plt.plot(sx, sy, "or", label="Start")
plt.plot(gx, gy, "ob", label="Goal")
plt.plot(rx, ry, "-k", label="Path")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()# 绘制速度曲线
plt.subplots(1)
plt.plot(time, rv, "-r", label="velocity")
plt.grid(True)
plt.xlabel("Time [s]")
plt.ylabel("Velocity [m/s]")
plt.legend()plt.show()

（2） 凸优化与非凸优化

课程链接：优化

大纲：

本节主要引出了参考线，还有约束问题，其次是代价。

首先来回顾一下规划总体流程：(已经得到了导航路径，需要决策 + 运动规划)

---

1. 定位和导航： 生成 参考线坐标
2. 分情况：
   • 无障碍物： 跟踪参考线路径
   • 有静态障碍物： 静态障碍物投影到SL图上(Frenet)
3. 决策算法：对障碍物进行决策，决定是 左避，还有右避，或者忽略。(开辟最优凸空间)
4. 规划算法： 在凸空间中搜索出来最优的路径。
5. 后续处理：在规划轨迹中选取一个点，坐标转化为笛卡尔坐标系，输出给控制模块去控制即可。

---

从上述流程可以了解到 首先需要解决的是参考线问题。接下来讲解一下参考线。

参考线：

目标：

• 解决导航的路径过长，不平滑的问题，通常从导航中获取到的全局路径都是由一段一段的线段构成，比较粗略，因此需要利用参考线实现平滑的操作。

• 路径太长，不方便找匹配点，搜索空间太大，也就是不利于坐标变换。

实现方案：每一个规划周期中，找到车在导航路径上的投影点，然后以投影点为坐标原点，往后取30米长度，往前取150米范围内的点，来做平滑，平滑后的点的集合称之为参考线。

具体实现：平滑算法。

利用三个点来找平滑的关系，然后简历一个二次规划来找到一个最优解。计算平滑代价，找到最小的。

(3) 直角坐标与自然坐标转换(上, 下)

课程链接：坐标变换1， 坐标变换2

这一节内容主要看这三个博客：

第一节

第二节

第三节

具体内容如下：

通过第一节的计算，我们得出来一个Cartesian坐标系转化到Frenet坐标系的步骤：

对应的python代码如下：

import numpy as np
from math import *# 本函数是将Cartesian坐标系，转化为Frenet坐标系
# rs是参考点的frenet坐标纵向位置
# 已知(x_x,y_x,theta_x, v_x, a_x, k_x) 和 参考点(s_r, x_r, y_r, theta_r, v_r, a_r, k_r, d_kr)
# 待求：(六个参数) s, s_dot, s_dot_dot, l, l_pie, l_pie_pie.
def cartesian_to_frenet1D(rs, rx, ry, rtheta, rkappa, rdkappa, x, y, v, a, theta, kappa):# 创建一个numpy数组， 长度为3，初始值为0. s代表纵向的三个参数，位置，速度，加速度。 d代表横向三个参数s_condition = np.zeros(3)d_condition = np.zeros(3)dx = x - rxdy = y - rycos_theta_r = cos(rtheta)sin_theta_r = sin(rtheta)cross_rd_nd = cos_theta_r*dy - sin_theta_r*dx  # 计算第二步，朝向C参数s_condition[0] = rs  # s# 第三步d_condition[0] = copysign(sqrt(dx*dx+dy*dy), cross_rd_nd)  # copysign(x, y)返回一个基于x绝对值和基于y符号的数值。x * y# 第四步delta_theta = theta - rtheta  # 角度差tan_delta_theta = tan(delta_theta)cos_delta_theta = cos(delta_theta)one_minis_kappa_r_d = 1 - rkappa * d_condition[0]  # C_drdx参数, 上面计算的d_condition[0]是横向误差d，也就是l# 第五步d_condition[1] = one_minis_kappa_r_d * tan_delta_theta# 第六步kappa_r_d_prime = rdkappa * d_condition[0] + rkappa * d_condition[1]# 第七步：计算l_pie_pied_condition[2] = (-kappa_r_d_prime) * tan_delta_theta + one_minis_kappa_r_d / cos_delta_theta / cos_delta_theta *\(kappa*one_minis_kappa_r_d / cos_delta_theta - rkappa)# 第八步：计算纵向位置ss_condition[0] = rs# 第九步：s_dots_condition[1] = v * cos_delta_theta / one_minis_kappa_r_d# 第十步：计算参数C_dthetadelta_theta_prime = one_minis_kappa_r_d / kappa / cos_delta_theta - rkappa# 第十一步：计算s_dot_dots_condition[2] = (a * cos_delta_theta - s_condition[1]* s_condition[1] * (d_condition[1] * delta_theta_prime - kappa_r_d_prime)) / (one_minis_kappa_r_d)return s_condition, d_condition

接下来是对Frenet坐标系转化为全局坐标系

代码如下：

def frenet_to_cartesian(rs, rx, ry, rtheta, rkappa, rdkappa, s_condition, d_condition):if fabs(rs - s_condition[0]) >= 1.0e-6:print("The reference point s and s_condition[0] don't match.")  # 说明参考点距离车辆太远了。cos_theta_r = cos(rtheta)sin_theta_r = sin(rtheta)# 第一步和第二步：计算x，y坐标x = rx - d_condition[0] * sin_theta_ry = ry + d_condition[0] * cos_theta_r# 中间参数one_minus_kappa_r_d  = 1 - rkappa*d_condition[0]tan_delta_theta = d_condition[1] / one_minus_kappa_r_d# 第四步计算角度delta_theta = atan2(tan_delta_theta, one_minus_kappa_r_d)  # d_thetatheta = NormalizeAngle(delta_theta + rtheta)# 第五步:计算速度d_dot = d_condition[1] * s_condition[1]v = sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d * s_condition[1]*s_condition[1] + d_dot * d_dot)# 第六步，计算角度cos_delta_theta = cos(delta_theta)# 第七步，计算C_kr_l参数kappa_r_d_prime = rdkappa * d_condition[0] + rkappa * d_condition[1]# 第八步：计算kxkappa = ((d_condition[2] + kappa_r_d_prime*tan_delta_theta) * cos_delta_theta * cos_delta_theta/one_minus_kappa_r_d + rkappa) \* cos_delta_theta / one_minus_kappa_r_d# 第九步：计算aa = s_condition[2] * (one_minus_kappa_r_d / cos_delta_theta) + (s_condition[1] * s_condition[1] / cos_delta_theta)* \(d_condition[1] * (kappa*one_minus_kappa_r_d/cos_delta_theta - rkappa) - kappa_r_d_prime)return x, y, v, a, theta, kappa