# 无人驾驶汽车系统入门（二十一）——基于Frenet优化轨迹的无人车动作规划方法

$T={t}_{end}-{t}_{start}$

### 为什么使用Frenet坐标系

Werling的动作规划方法一个很关键的理念就是将动作规划这一高维度的优化问题分割成横向和纵向两个方向上的彼此独立的优化问题，具体来看下面的图：

### Jerk最小化和5次轨迹多项式求解

${J}_{t}\left(p\left(t\right)\right)={\int }_{{t}_{0}}^{{t}_{1}}p\left(\tau {\right)}^{2}d\tau$

$p\left(t\right)={\alpha }_{0}+{\alpha }_{1}t+{\alpha }_{2}{t}^{2}+{\alpha }_{3}{t}^{3}+{\alpha }_{4}{t}^{4}+{\alpha }_{5}{t}^{5}$

$d\left({t}_{0}\right)={\alpha }_{d0}+{\alpha }_{d1}{t}_{0}+{\alpha }_{d2}{t}_{0}^{2}+{\alpha }_{d3}{t}_{0}^{3}+{\alpha }_{d4}{t}_{0}^{4}+{\alpha }_{d5}{t}_{0}^{5}$

$\stackrel{˙}{d}\left({t}_{0}\right)={\alpha }_{d1}+2{\alpha }_{d2}{t}_{0}+3{\alpha }_{d3}{t}_{0}^{2}+4{\alpha }_{d4}{t}_{0}^{3}+5{\alpha }_{d5}{t}_{0}^{4}$

$\stackrel{¨}{d}\left({t}_{0}\right)=2{\alpha }_{d2}+6{\alpha }_{d3}{t}_{0}+12{\alpha }_{d4}{t}_{0}^{2}+20{\alpha }_{d5}{t}_{0}^{3}$

$d\left({t}_{1}\right)={\alpha }_{d0}+{\alpha }_{d1}{t}_{1}+{\alpha }_{d2}{t}_{1}^{2}+{\alpha }_{d3}{t}_{1}^{3}+{\alpha }_{d4}{t}_{1}^{4}+{\alpha }_{d5}{t}_{1}^{5}$

$\stackrel{˙}{d}\left({t}_{1}\right)={\alpha }_{d1}+2{\alpha }_{d2}{t}_{1}+3{\alpha }_{d3}{t}_{1}^{2}+4{\alpha }_{d4}{t}_{1}^{3}+5{\alpha }_{d5}{t}_{1}^{4}$

$\stackrel{¨}{d}\left({t}_{1}\right)=2{\alpha }_{d2}+6{\alpha }_{d3}{t}_{1}+12{\alpha }_{d4}{t}_{1}^{2}+20{\alpha }_{d5}{t}_{1}^{3}$

${\alpha }_{d0}=d\left({t}_{0}\right)$

${\alpha }_{d1}=\stackrel{˙}{d}\left({t}_{0}\right)$

${\alpha }_{d2}=\frac{\stackrel{¨}{d}\left({t}_{0}\right)}{2}$

$T={t}_{1}-{t}_{0}$$T = t_1 - t_0$ ，剩余的三个系数 ${\alpha }_{d3},{\alpha }_{d4},{\alpha }_{d5}$$\alpha_{d3}, \alpha_{d4},\alpha_{d5}$ ，可通过解如下矩阵方程得到：

$\left[\begin{array}{ccc}{T}^{3}& {T}^{4}& {T}^{5}\\ 3{T}^{2}& 4{T}^{3}& 5{T}^{4}\\ 6T& 12{T}^{2}& 20{T}^{3}\end{array}\right]×\left[\begin{array}{c}{\alpha }_{d3}\\ {\alpha }_{d4}\\ {\alpha }_{d5}\end{array}\right]=\left[\begin{array}{c}d\left({t}_{1}\right)-\left(d\left({t}_{0}\right)+\stackrel{˙}{d}\left({t}_{0}\right)T+\frac{1}{2}\stackrel{¨}{d}\left({t}_{0}\right){T}^{2}\right)\\ \stackrel{˙}{d}\left({t}_{1}\right)-\left(\stackrel{˙}{d}\left({t}_{0}\right)+\stackrel{¨}{d}\left({t}_{0}\right)T\right)\\ \stackrel{¨}{d}\left({t}_{1}\right)-\stackrel{¨}{d}\left({t}_{0}\right)\end{array}\right]$

$\left[{d}_{1},\stackrel{˙}{{d}_{1}},\stackrel{¨}{{d}_{1}},T{\right]}_{ij}=\left[{d}_{i},0,0,{T}_{j}\right]$

${C}_{d}={k}_{j}{J}_{t}\left(d\left(t\right)\right)+{k}_{t}T+{k}_{d}{d}_{1}^{2}$

* ${k}_{j}{J}_{t}\left(d\left(t\right)\right)$$k_jJ_t(d(t))$ ：惩罚Jerk大的备选轨迹；
* ${k}_{t}T$$k_tT$ ：制动应当迅速，时间短；
* ${k}_{d}{d}_{1}^{2}$$k_dd_1^2$ ：目标状态不应偏离道路中心线太远

* 跟车
* 汇流和停车
* 车速保持

${C}_{s}={k}_{j}{J}_{t}\left(s\left(t\right)\right)+{k}_{t}T+{k}_{\stackrel{˙}{s}}\left(\stackrel{˙}{{s}_{1}}-\stackrel{˙}{{s}_{c}}{\right)}^{2}$

$\left[\stackrel{˙}{{s}_{1}},\stackrel{¨}{{s}_{1}},T{\right]}_{ij}=\left[\left[\stackrel{˙}{{s}_{c}}+\mathrm{\Delta }\stackrel{˙}{{s}_{i}}\right],0,{T}_{j}\right]$

${C}_{total}={k}_{lat}{C}_{d}+{k}_{lon}{C}_{s}$

### 事故避免（Collision Avoiding）

• s方向上的速度是否超过设定的最大限速
• s方向的加速度是否超过设定的最大加速度
• 轨迹的曲率是否超过最大曲率
• 轨迹是否会引起碰撞（事故）

通常来说，障碍物规避又和目标行为预测等有关联，本身即使一个复杂的课题，高级自动驾驶系统通常具备对目标行为的预测能力，从而确定轨迹是否会发生事故。在本节中，我们关注的重点是无人车的动作规划，故后面的实例仅涉及静态障碍物的规避和动作规划。

### 基于Frenet优化轨迹的无人车动作规划实例

由于planner的代码篇幅过长，本实例完整代码请见文末链接，在此仅讲解算法核心代码内容。和之前一样，我们仍然使用Python来实现该动作规划算法。

首先，我们生成要追踪的参考线以及静态障碍物，参考线的生成只要使用了我们上一节提到的立方样条插值，代码如下：

# 路线
wx = [0.0, 10.0, 20.5, 30.0, 40.5, 50.0, 60.0]
wy = [0.0, -4.0, 1.0, 6.5, 8.0, 10.0, 6.0]
# 障碍物列表
ob = np.array([[20.0, 10.0],
[30.0, 6.0],
[30.0, 5.0],
[35.0, 7.0],
[50.0, 12.0]
])

tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)

# 参数
MAX_SPEED = 50.0 / 3.6  # 最大速度 [m/s]
MAX_ACCEL = 2.0  # 最大加速度[m/ss]
MAX_CURVATURE = 1.0  # 最大曲率 [1/m]
MAX_ROAD_WIDTH = 7.0  # 最大道路宽度 [m]
D_ROAD_W = 1.0  # 道路宽度采样间隔 [m]
DT = 0.2  # Delta T [s]
MAXT = 5.0  # 最大预测时间 [s]
MINT = 4.0  # 最小预测时间 [s]
TARGET_SPEED = 30.0 / 3.6  # 目标速度（即纵向的速度保持） [m/s]
D_T_S = 5.0 / 3.6  # 目标速度采样间隔 [m/s]
N_S_SAMPLE = 1  # 目标速度的采样数量

# 损失函数权重
KJ = 0.1
KT = 0.1
KD = 1.0
KLAT = 1.0
KLON = 1.0

def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
frenet_paths = []

# 采样，并对每一个目标配置生成轨迹

# 横向动作规划
for Ti in np.arange(MINT, MAXT, DT):
fp = Frenet_path()
# 计算出关于目标配置di，Ti的横向多项式
lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)

fp.t = [t for t in np.arange(0.0, Ti, DT)]
fp.d = [lat_qp.calc_point(t) for t in fp.t]
fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]

# 纵向速度规划 (速度保持)
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
tfp = copy.deepcopy(fp)
lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)

tfp.s = [lon_qp.calc_point(t) for t in fp.t]
tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]

Jp = sum(np.power(tfp.d_ddd, 2))  # square of jerk
Js = sum(np.power(tfp.s_ddd, 2))  # square of jerk

# square of diff from target speed
ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
# 横向的损失函数
tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1] ** 2
# 纵向的损失函数
tfp.cv = KJ * Js + KT * Ti + KD * ds
# 总的损失函数为d 和 s方向的损失函数乘对应的系数相加
tfp.cf = KLAT * tfp.cd + KLON * tfp.cv

frenet_paths.append(tfp)

return frenet_paths

class quintic_polynomial:
def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
# 计算五次多项式系数
self.xs = xs
self.vxs = vxs
self.axs = axs
self.xe = xe
self.vxe = vxe
self.axe = axe

self.a0 = xs
self.a1 = vxs
self.a2 = axs / 2.0

A = np.array([[T ** 3, T ** 4, T ** 5],
[3 * T ** 2, 4 * T ** 3, 5 * T ** 4],
[6 * T, 12 * T ** 2, 20 * T ** 3]])
b = np.array([xe - self.a0 - self.a1 * T - self.a2 * T ** 2,
vxe - self.a1 - 2 * self.a2 * T,
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):
xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5

return xt

def 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 * t ** 4

return xt

def calc_second_derivative(self, t):
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3

return xt

def calc_third_derivative(self, t):
xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2

return xt

def check_collision(fp, ob):
for i in range(len(ob[:, 0])):
d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
for (ix, iy) in zip(fp.x, fp.y)]

collision = any([di <= ROBOT_RADIUS ** 2 for di in d])

if collision:
return False

return True

def check_paths(fplist, ob):
okind = []
for i in range(len(fplist)):
if any([v > MAX_SPEED for v in fplist[i].s_d]):  # 最大速度检查
continue
elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]):  # 最大加速度检查
continue
elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]):  # 最大曲率检查
continue
elif not check_collision(fplist[i], ob):
continue

okind.append(i)

return [fplist[i] for i in okind]