无人驾驶汽车离散优化的轨迹规划算法

张垚， 彭育辉

(福州大学机械工程及自动化学院， 福建 福州 350108)

0 引言

随着2020年2月《智能汽车创新发展战略》的发布， 汽车无人驾驶技术研究得到了学者和行业的普遍关注. 轨迹规划是汽车无人驾驶的关键模块， 依据感知系统得到行驶环境的周围信息， 生成一条保证行驶安全、 时空平滑、 可行的汽车行驶轨迹.

目前被广泛应用的基于离散优化的轨迹规划算法， 是针对行驶环境进行离散的. 其采用数值积分和微分方程描述车辆运动， 产生有限的候选轨迹， 由代价函数从候选轨迹中选择出最优轨迹， 实时性好. Chu等[1]基于离散优化思想， 提出以三次多式作为车辆运动基元的算法， 能够安全地避开静态障碍物， 但没有考虑动态障碍物和多车道场景. 周惠子等[2]通过加权碰撞检测的方法， 实现了多车道避障行驶， 但仍未考虑移动障碍物. 文献[3-4]在此基础上考虑了移动障碍车辆， 提出一种新的安全代价函数， 但是仅考虑了同向行驶的障碍车辆， 具有一定的局限性. Werling等[5]以五次多项式作为运动基元， 同时引入最优控制的方法进行规划， 能够避开低速行驶的障碍物， 但行驶时会导致方向盘频繁的转动. 由于使用特定的参数曲线描述车辆的运动轨迹， 限制了车辆的运动性能[6]. 文献[7-9]使用采样和优化相结合的轨迹规划方法， 使规划出来的轨迹更加平滑. 上述研究对行驶环境进行离散时， 需选择适合的采样精度以平衡规划路径的最优性和候选路径的计算量， 且研究场景大多局限于单向车道场景. 相比于单向车道， 车辆在双向两车道执行避障、 超车等行为时需要考虑旁边车道相向行驶的车辆， 规划难度更高.

针对以上问题， 本研究提出一种面向双向两车道的无人车轨迹规划算法. 该算法使用人工势场法描述车辆与周围环境的相互关系， 基于分层采样的方法求解初始路径以平衡路径的最优性和计算量； 将同车道车辆以及旁边车道相向行驶的车辆投影到ST图上， 基于多目标A*搜索算法规划出初始速度剖面并进行优化， 完成对旁边车道车辆的避让. 最后， 通过Python进行计算机仿真， 验证算法的有效性和可靠性.

1 轨迹规划算法

算法分为路径规划和速度规划两部分. 路径规划时， 基于道路中心线构建s-ρ坐标系， 将无人车与周围障碍物映射到s-ρ坐标系上. 通过分层采样构建路径Lattice图， 搜索出初始路径， 然后对初始路径进行平滑优化. 一旦生成了一条平滑的路径， 障碍物就会被投影到ST图上， 在ST图上搜索出初始速度剖面后再进行平滑优化. 结合路径和速度曲线， 将获得规划的平滑轨迹.

1.1 初始路径生成

1.1.1构建路径Lattice图

对于双向两车道， 将右侧车道的中心线作为参考路径， 基于参数化样条曲线方法构建s-ρ坐标系[5]. 在s-ρ坐标系下对车辆行驶的状态空间进行离散采样， 纵向采样间距Δs根据车速进行设定. 与文献[5]不同的是， 本研究使用采样层的组合构建路径Lattice图(如图1所示)， 以规划较长的路径.

图1 路径Lattice图Fig.1 Lattice diagram of the path

采样点之间的连接方式视为运动基元， 以三次多项式作为运动基元， 函数表达式为：

ρ=a0+a1s+a2s2+a3s3(s∈[s0,se])

(1)

通过起点状态(s0,ρ0,θ0)和终点状态(se,ρe,θe)的位姿约束， 得到4个边界条件可以唯一确定运动基元.

1.1.2路径代价函数设计

路径Lattice图中每条边的代价值由路径代价函数确定， 即：

Cvalue=Csmooth+Cref+Cobs

(2)

式中：Csmooth表示路径的光滑度项， 用曲率反映；Cref表示路径与参考线的横向偏移惩罚项；Cobs表示障碍物的碰撞风险项.

对每一段曲线均匀采样N个点， 用近似曲率平方的积分表示光滑度函数：

(3)

相对安全的行驶策略是车辆应尽可能沿着右侧车道的中心线行驶， 将Cref表示为N个路径点的和， 即：

(4)

式中：ρr表示参考线在s-ρ坐标系下的横向坐标.

周围环境对无人车的行驶安全影响极大， 结合人工势场法[10-11]， 分别对道路、 障碍车辆建模， 构建无人车行驶的碰撞风险代价. 碰撞风险项定义为：

(5)

式中：Eroad表示道路边界势能；Eobsj表示第j个障碍车辆的势能；nobs表示环境中障碍物的数量.

道路边界是防止车辆驶出道路， 越靠近道路边界位置其势场值越大， 设计的道路边界势场函数为：

(6)

其中：Vroad表示边界势能值；ρroad， 1、ρroad， 2分别表示左右道路边界在s-ρ坐标系下的横向距离. 对于双向两车道， 道路势场分布情况如图2所示.

图2 道路势能分布Fig.2 Road potential energy distribution

通过建立障碍物势能模型使无人车与障碍物保持一定的安全距离. 采用类高斯函数模拟障碍物势能， 势能分布函数为：

(7)

其中：Vobs表示障碍物势场系数；σjs表示纵向安全参数；σjρ表示横向安全参数.

障碍车辆停放在s-ρ坐标系下(-1, 40)处的势场分布情况如图3所示.

图3 障碍车辆势能分布Fig.3 Potential energy distribution of obstacle vehicles

1.1.3路径搜索

路径代价函数式(2)确定了路径Lattice图中每一条边的代价值， 采用图搜索算法即可搜索出代价最小的路径. 为了平衡路径规划的实时性和最优性， 提出分层采样的方法， 即先以较大的横向采样间隔构建路径Lattice图， 搜索出一条较优的路径， 如图4(a)中粉紫色的路径. 以此路径为中心线用较小的横向采样间隔再次构建路径Lattice图， 搜索出初始路径， 如图4(b)中蓝色的路径. 与直接用较小横向间隔的采样方法相比， 分层采样的方法求解路径的计算量更小.

图4 分层采样规划Fig.4 Stratified sampling planning

1.2 路径优化

初始路径的曲率剖面具有阶跃变化现象， 平滑性较差， 还需进一步优化. 采用数值优化的方法， 对路径以纵向等距离Δs离散成Ns个路径点(si,ρi)， 把ρ=[ρ0,ρ1, …,ρn]T作为优化的决策变量， 通过最小化路径点横向距离ρ的变化率对路径进行优化， 目标函数定义为：

(8)

式中： 前三项与车辆在s-ρ坐标系下的航向角以及其变化率相关， 用来平滑路径； 第四项表示对初始路径的跟随；ρinit表示初始路径采样点. 横向距离ρ的相关导数由采样路径点进行差分近似， 即：

(9)

优化时还要满足车辆运动模型确定的曲率约束、 避免碰撞约束以及道路边界约束. 其中， 由车辆的阿克曼转向几何关系， 确定车辆能够通过的最大路径曲率κlim， 将曲率约束表示为：

建立避免碰撞约束关系式时， 采用3个半径为rc的圆沿着车辆的纵轴分布表示无人车的覆盖面积， 其中一个圆的圆心与车辆后轴的中点重合， 两个圆心之间的距离为d， 如图5所示.车辆后轴中心点的坐标(si,ρi)由定位模块通过坐标转换得出， 计算可得其余两个圆的圆心坐标， 将约束表示为：

图5 三圆碰撞检测Fig.5 Three circle collision detection

(11)

式中： (sj，ρj)表示第j个障碍物的s-ρ坐标；robs表示障碍物的扩展半径.

同时， 车辆在行驶过程中不能超出道路的边界， 因此形成不等式约束：

ρmin≤ρi≤ρmax

(12)

式(12)中，ρmax和ρmin分别是道路的左边界和右边界相对于中心线的横向偏移. 从图6中初始路径(蓝色)与优化路径(暗红色)的对比， 可以看出优化路径的曲率是平滑的.

图6 初始路径与优化路径对比Fig.6 Comparison of initial path and optimized path

1.3 初始速度剖面生成

速度剖面规划时引入ST图[8]， 目标是规划未来8 s或80 m的速度曲线. 先将ST图栅格化， 将本车道以及旁边车道的障碍物投影到ST图上， ST图中的可行域变成非凸区域. 然后， 基于提出的多目标A*算法在可行域中搜索出初始的速度剖面.

ST图栅格化如图7所示， 以时间间隔Δt=0.5 s和纵向距离间隔Δs=2 m， 将ST图划分成网格， 四边形区域表示障碍车辆占据的时域， 黄绿色点是目标点， 红色线表示以最大速度匀速行驶的速度剖面线. 在红色线右侧、 黄色目标点下方未被障碍车辆占据的时域为可行域.

图7 初始速度剖面规划Fig.7 Initial velocity profile planning

与A*算法[12]的节点扩展方式不同， 本研究以网格顶点为节点， 从原点进行节点扩展. 考虑到车辆最大速度的限制， 以及车辆正常行驶时没有倒车行为， 父节点可以扩展为6个子节点, 如图8所示. 节点速度及其导数可根据采样间隔Δt， Δs进行差分近似计算. 近似计算与式(9)类似， 扩展节点的成本函数为：

图8 节点扩展方式Fig.8 Node expansion method

Cspeed=g(s)+h(s)

(13)

其中：g(s)为从父节点到当前节点的代价函数;h(s)为当前节点到目标节点集的启发函数.g(s)的具体算式为：

ω3(s‴i)2+ω4(dsi+δ)-1

(14)

式中: 第一项代表相对于参考速度vref偏移的惩罚项， 使规划速度不会偏离参考速度太远； 第二项和第三项分别表示加速度和jerk的惩罚项， 通过抑制其快速变化使速度剖面更加平滑； 最后一项代表障碍物成本， 其中dsi表示节点到所有障碍物的最小距离，δ=0.001是为了消除奇异值.

式(13)中的h(s)与A*算法的不同在于速度规划时设有多个目标点， 因此启发函数的设计需要考虑到每个目标点， 即：

(15)

式中：m表示目标点的个数； (si,ti)表示当前节点； (sj,tj)表示目标节点.

多目标A*算法的搜索过程与A*算法类似， 当扩展节点的si大于规划的最大纵向距离80 m或扩展节点的ti大于规划的最大时间8 s， 则搜索结束. 查找出代价最小的目标节点， 通过节点回溯构成速度剖面， 规划的速度剖面如图7蓝色折线所示.

1.4 速度剖面优化

初始速度剖面由多条直线段连接而成， 不满足平滑性要求和车辆动力学约束， 需对初始速度剖面进行优化， 优化结果如图9所示.

图9 速度剖面优化Fig.9 Speed profile profile optimization

首先， 对初始速度剖面以更小的时间间隔σ进行重采样， 即：

ti=t0+iσ(i=0, 1, …,Nt)

(16)

然后计算时间戳ti对应的纵向距离si， 通过固定时间戳把s=[s0,s1, …,sn]T作为优化的决策变量， 将优化问题的目标函数定义为：

(17)

优化时需要满足车辆的动力学约束和避免碰撞约束. 规划的速度不能超出速度限制vmax、 道路限速vlim、 加速度限制amax和加加速度限制jerkmax， 故动力学约束定义为：

(18)

建立避免碰撞约束关系式时， 通过对ST图的处理， 将初始速度剖面附近的可行域变成一个凸集(如图9中绿色区域). 对于每个决策变量si， 其范围约束为：

si≤si+1，sd, i≤si≤sh, i(i=0, 1, …,Nt-1)

(19)

式中: 第一个不等式限制了车辆的倒车行为；sh, i、sd, i分别表示凸集的上界和下界.

上述所有的约束都是线性的， 决策变量的可行域是一个凸集， 目标函数是二次凸函数， 优化问题变成一个中等规模的二次规划问题, 可应用二次规划求解器快速求解.

2 仿真结果与分析

为验证本算法的有效性， 基于Python构建宽为3.75 m的双向两车道道路模型， 道路包括直线路段和弯道路段， 并在道路上设置若干静止和移动的障碍车辆， 针对典型的驾驶场景进行测试. 在仿真器中， 无人车用蓝色矩形表示， 其最大速度为20 m·s-1， 最大加速度为2.5 m·s-2， 最大加加速度(jerk)为5 m·s-3， 曲率限制为0.1 m-1. 障碍车辆用其它颜色矩形表示， 时间信息由颜色的深度来表示， 其中较深的颜色表示较晚的时间步长. 算法使用Python脚本编写， 运行在英特尔酷睿i7-5500U (2.4 GHz)的笔记本电脑上.

2.1 避障场景

无人车在双向车道中避障时， 需要警惕旁边车道的车辆， 场景如图10所示. 本车道前方有2个静止的车辆， 旁边车道有一相向行驶的车辆， 车速为4 m·s-1. 无人车初始速度为8 m·s-1， 参考速度为12 m·s-1.

图10(a)显示了无人车按本规划方法行驶的轨迹， 无人车安全地避开了障碍车辆. 图10(b)将本方法与文献[5]的轨迹规划方法(称为方法一)对比， 结果显示， 无人车按本方法规划轨迹行驶的曲率均方差为0.007 9， 相比于方法一的0.010 3减小了23.30%. 图10(c)中橙色的曲线为无人车的速度变化曲线， 蓝色曲线为参考速度曲线. 可以看出, 无人车选择了加速避开障碍物， 这是因为移动车辆车速较慢， 速度规划时选择加速通过可能发生碰撞的区域. 而无人车的加速度和jerk的变化均没有超过阈值.

图10 避障场景Fig.10 Obstacle avoidance scene

2.2 超车场景

在双向两车道路上超车， 而相邻车道上有迎面而来的车辆是最具挑战性和危险性的驾驶场景， 如图11所示. 此场景中， 无人车初始速度为8 m·s-1， 前车速度为 4 m·s-1, 迎面而来的车辆速度为12 m·s-1.

图11(a)显示了无人车与障碍车辆的运动轨迹. 图11(b)为两种方法行驶轨迹的曲率对比， 无人车按本方法规划轨迹行驶的曲率均方差为0.009 5， 相比于方法一的0.013 3减小了28.57%. 图11(c)为无人车的速度、 加速度和jerk的变化曲线. 图中显示， 无人车先减速， 直到迎面驶来的车辆通过， 然后加速超过前车并趋向于参考速度. 这是由于迎面驶来的车辆车速较快， 速度规划时无人车选择减速对其避让， 当其通过时， 无人车加速超过前面的车辆并趋向于参考速度.

2.3 跟随场景

车辆跟随也是双向两车道最常见的驾驶行为之一， 如图12所示. 此场景中， 无人车前方以及旁边车道的障碍车辆车速为12 m·s-1， 无人车当前车速为16 m·s-1.

图12(a)显示了无人车跟随前车行驶的轨迹， 在车辆行驶全过程中无人车始终与前车保持一定的安全距离. 图12(b)对比了两种方法行驶轨迹的曲率. 结果显示， 无人车按本方法规划轨迹行驶的曲率均方差为0.004 1， 相比于方法一的0.004 4减小了6.82%. 图12(c)为无人车的速度、 加速度和jerk的变化曲线. 图中显示， 无人车缓慢减速到与前车相同的速度， 且其加速度和jerk的变化都在约束范围内.

图12 跟随场景Fig.12 Follow the scene

为了体现本算法的实时性， 在3个测试场景中， 各抽取10次本算法的运行时间， 如表1所示. 本算法的平均运行时间在0.22 s左右， 实时性好.

表1 算法运行时间

3 结语

基于离散优化的方法， 提出一种针对双向两车道的新轨迹解耦规划算法. 通过解耦方法降低轨迹规划的复杂度， 且使用ST图增加了轨迹的安全性. 在路径代价函数的设计中结合人工势场法， 引导无人车安全、 合理地避开障碍物. 使用分层采样的方法对环境进行离散， 并采用多目标A*算法规划初始速度剖面， 有效地加快了算法的求解速度. 该算法同样适用于单向多车道道路. 仿真实验结果表明， 所提算法能够在短时间内产生安全的轨迹； 与传统轨迹规划算法对比， 所规划轨迹的曲率更加平滑.