基于安全域的局部路径规划优化算法

毛莞丁，邹若冰，刘晨丽

（陕西汽车集团有限责任公司，西安 710043）

据我国道路交通事故统计，交通事故九成以上都与驾驶员自身原因有关[1]。自动驾驶车辆是由车辆的控制器、传感器等发挥主要作用，代替驾驶员驾驶车辆，可以为车辆行驶安全以及交通安全提供良好的保障[2]。决策规划是自动驾驶的关键部分之一，是自动驾驶车辆可以到达目标点的保障。它融合多传感信息，根据驾驶需求进行行为决策，指导当前自动驾驶车辆做出行为动作，如正常行驶、跟车、超车、换道、停车等，同时依据局部环境信息、行为决策结果和车辆实时位姿信息，在满足一定的运动学约束下，规划局部空间和时间内车辆期望的运动轨迹。

当前研究人员提出了很多的路径规划算法，如人工势场法[3-4]、快速随机树（Fast Random Tree，RRT）算法[5]、模糊算法[6-7]、蚁群算法[8]等，有些方法[9-10]是直接找到连接起点和终点的全局轨迹。另外一些方法[11-19]是先得到一系列有限的轨迹，从该轨迹集合中，通过最小化给定的代价函数选择最优轨迹。对于如何生成轨迹集合，需要选择参数模型，如高阶多项式。WERLING等[14]提出的基于Frenet框架的最优轨迹生成（Optimal Trajectory Generation，OTG）算法，将轨迹规划问题分解成横向和纵向两个一维空间的路径规划问题，求解轨迹集，并从横向和纵向分别考虑代价函数，加权求和得到总的代价函数，通过代价函数最小化选取最优轨迹。该方法具有较强的实用性，是目前普遍采用的局部路径规划算法之一。Apollo EM规划器[15]与OTG算法相似，同样采用降维思想，将轨迹规划问题分解为横向和纵向，再合成三维轨迹，根据代价函数筛选出最好的合成轨迹。周慧子等[16]针对自动驾驶中避障的动态路径规划问题，提出一种在已知车辆的初始位置、速度、方向和障碍物位置情况下，实时避开障碍物的动态规划算法。梁广民[17]提出了一种新的自动驾驶轨迹规划方法，该方法可以为自动驾驶生成舒适安全的轨迹。郭蓬等[18]在使用Frenet坐标系理论的基础上，提出了一种考虑到车道线曲率和障碍物模型信息的方法，然后通过最小化代价函数，选出局部最优路径。毛晨悦等[19]提出一种基于人工势场法的无人机路径规划避障算法，提高无人机的避障性能。

在代价函数方面，OTG算法[14]的代价函数是从横向和纵向分别考虑，主要包含舒适性、运行时长和偏离目标三个方面。Apollo EM规划器[15]的代价函数（简称cost函数）包含到达目的偏差、横向偏移、是否碰撞以及加速度。周慧子等[16]从安全、平滑和连续这3个方面来设计代价函数。梁广民[17]根据每个候选轨迹与每个障碍物的碰撞概率和每条轨迹到目标位置的最短距离选择最合适的轨迹。郭蓬等[18]算法的代价函数考虑了拟合轨迹到障碍物的危险程度、轨迹平顺性、轨迹到当前参考线的偏离程度、拟合轨迹行驶方向的改变程度、无人驾驶汽车最小转弯半径。

近些年的研究中，许多学者也对传统的路径规划算法进行了改进和优化。胡浍冕[20]针对RRT算法收敛速度慢、路径不规则的问题，基于双向RRT算法和概率搜索策略，提出了一种变概率策略下的双向RRT搜索优化算法。在仿真中进行了多场景重复试验测试，仿真结果表明，改进后的算法在搜索速度和路径长度上有明显改善。王飞等[21]针对工作环境已知的情况，研究了移动机器人在复杂三维环境下路径规划的问题，提出一种改进的蚁群优化算法。试验结果表明，该算法有较高的全局搜索能力，收敛速度明显加快，验证了该算法的有效性和可行性。王云常等[22]为简化无人机飞行路径规划算法并提高其避障效果，提出一种人工势场法和A星算法相结合的路径规划算法，仿真试验证明，该算法与人工势场法和A星算法相比，提高了避障效果，缩短了搜索时间。

本文从代价函数的角度出发，对传统OTG算法进行优化。上述算法的代价函数包含大多数的评价方法（安全性、平滑性、连续性等），但是无法表征动态障碍物对车辆行驶安全的影响。例如，主控车辆和障碍车辆之间距离相等，两者既可以是相向而行，也可以是同向而行，但两者对主控车辆的影响不同，选取出的最优轨迹也会不同。针对这种情况上述局部路径规划算法的代价函数会筛选出相同轨迹，而无法做出准确的评判。因此，传统OTG算法的代价函数仅考虑了三方面的约束，且只针对静态情况，对动态环境安全因素的考虑不足。为了解决上述问题，提出一种简单可行的影响因子──安全域，反映了主控车辆与周围动态环境的安全性。这个概念的提出将为局部路径规划的优化提供一定的依据。

1 传统OTG算法

传统OTG算法在进行路径规划时分别从横向和纵向考虑，最后进行融合。算法分为两部分，第一部分是生成轨迹集合，第二部分是对轨迹集合进行评价，筛选出最优轨迹作为自动驾驶汽车最终的期望运行轨迹。

1.1 轨迹的生成

WERLING等[14]研究的OTG算法采用横纵向分别求解轨迹，再耦合的方式来实现。其中横纵向轨迹分别采用五次多项式[23]来表示，公式如下：

式中：d(t)为横向轨迹的五次多项式；αd0、αd1、αd2、αd3、αd4和αd5为横向轨迹五次多项式的系数；t为时间，s；s(t)为纵向轨迹的五次多项式；αs0、αs1、αs2、αs3、αs4和αs5为纵向轨迹五次多项式的系数。

1.2 轨迹的评价

在求得横纵向轨迹集合后，需要利用代价函数对其进行评价，选出最优轨迹。传统OTG算法的代价函数也分为横向和纵向，横向轨迹的代价函数为：

纵向轨迹的代价函数为：

式中：kj为Jerk[24]权重；Jt[d(t)]为关于d(t)的Jerk；Jt[s(t)]为关于s(t)的Jerk；kt为时长权重；T为轨迹的控制周期，s；kd为偏离道路中心线权重；d12为横向目标位置与横向轨迹末端位置之差的平方；s˙1为纵向目标速度，m/s ；sc˙为纵向轨迹末端速度，m/s。

将横向和纵向的代价函数加权求和，得到总的代价函数为：

式中：klat为横向代价函数的权重；klon为纵向代价函数的权重。

2 传统OTG算法的优化

本文提出了“安全域”这一新概念，客观表征了车辆行驶过程中车、人、路等交通要素对行驶安全的影响。同时，也提出了量化安全域参数的方法并成功应用于传统OTG算法的优化。

2.1 安全域

影响自动驾驶车辆的行驶安全性因素有很多，主要包括主控车辆、其他交通参与者以及道路等。交通参与者主要有机动车、非机动车、行人、甚至路上出现的动物以及路障等，这些交通参与者可以是运动的，也可以是静止的。道路主要包含道路的车道数、宽度、形状等。

对于车辆行驶安全性而言，道路上的交通参与者都有可能对其产生潜在的风险。为了评估自动驾驶主控车辆周围环境对行驶安全性的影响，本文提出了安全域的概念，用来表征主控车辆行驶过程中周围车、人、路各因素对行驶安全性的影响程度。它是一个包含时间和空间两个维度的变量（车辆行驶过程中各个参与者要素的动态变化）。安全域是根据交通参与者的自身特性在其周围形成一个安全范围，两个交通参与者安全域的重叠面积越大，说明两者碰撞的概率越高，行驶安全性越低，反之，重叠面积越小，碰撞概率越低，行驶安全性越高。

安全域的范围是由交通参与者的类型、位置、质量、长宽、运动状态决定的。王建强等[25]提出了一个等效质量的概念，把物体的类型、质量及其运动状态这些因素统一表达。定义物体i的等效质量如式（6）所示：

式中：mi为物体i的质量，kg；Type为物体的类型；δi(T ype)m为 关于物体i的类型的函数；νi为物体i的速度，m/s；αk，βk均为待定常数；k为速度多项式的项数。

本文用矩形表示交通参与者的安全域，如图1所示。图1中红色矩形框表示车辆，黑色矩形框表示车辆的安全域，红色箭头表示车辆的行驶方向。A(x0,y0)是车辆的中心坐标，A'(x,y)是车辆安全域的中心坐标。

图1 车辆的安全域示意图

图1中，a是车辆中心到安全域末端的距离，b是车辆中心到安全域前端的距离，c是安全域的宽，d是安全域的长，分别为：

式中：l为车辆的长，m；w为车辆的宽，m；ηa（ Type）、ηb（ Type）和ηc（ Type）为关于Type的函数。

由式（7）～（10）可以求出车辆安全域的长d和宽c，同时可以得到安全域的中心坐标：

式中：yaw为车辆的航向角。

2.2 算法优化

基于安全域的定义，可以应用于局部路径规划，以便寻找一条安全性更高的最优轨迹。传统OTG算法的代价函数对安全性的考虑不够全面，并没有考虑到规划出轨迹后，在轨迹的控制周期T内主控车辆与障碍物碰撞的可能性。把安全域考虑进去，弥补这个缺陷，提高轨迹的安全性。在传统OTG算法的基础上，考虑交通参与者的安全域，对评价轨迹的代价函数进行改进，新添加一项，以达到优化传统OTG算法的目的。

图2～4是同一场景下不同时刻的安全域重叠部分示意图，其中，图2是t=0 s，图3是t=(T/2) s，图4是t=Ts。图中浅灰色的线表示车道线；红色线是规划出的轨迹；蓝色矩形是主控车辆，绿色矩形是障碍物车辆；蓝色箭头是主控车辆的行驶方向，绿色箭头是障碍物车辆的行驶方向，长度表示速度的大小；蓝色框是主控车辆的安全域，绿色框是障碍物车辆的安全域；黑色部分为剔除超过道路边界的安全域重叠部分。

图2 t= 0 s

图3 t=(T/2) s

图4 t= T s

Coverlap是在轨迹的控制周期T内，随着时间变化，轨迹上每一时刻主控车辆和所有障碍物的安全域重叠部分OverlapArea的面积累加和，如式（13）所示。面积累加和越小，说明两辆车的碰撞可能性越小，行驶安全性越高。反之，面积累加和越大，说明两辆车的碰撞可能性越大，行驶安全性越低。

式中：nobs为障碍物的数量；Car （A′,a,b,c）为主控车辆的信息；Obs （A′,a,b,c）为障碍物的信息；Overlap ()为计算主控车辆与障碍物重叠面积的函数。

因此，总的代价函数变为：

式中：koverlap为安全域重叠部分的面积累加和的权重。

显然，上述的轨迹优化代价函数中并没有包含关于障碍物规避的相关惩罚，并且代价函数中也没有包含最大速度、最大加速度和最大曲率等控制约束限制，也就是说当前的优化轨迹集合并没有考虑障碍物规避和控制约束。因此，在完成所有备选轨迹的损失计算后进行一次轨迹检查，过滤不符合控制约束条件限制的、可能会碰撞障碍物的轨迹等，检查内容包括：检查纵向上的速度是否超过设定的最大限速；检查纵向上的加速度是否超过设定的最大加速度；检查轨迹的曲率是否超过最大曲率；检查轨迹是否会引起碰撞。在轨迹检查后，得到一组有效轨迹集合，然后选取代价函数最小的轨迹，就是最优轨迹。

3 算法验证

为了验证本文优化后算法的有效性和可行性，对算法进行仿真测试和实车测试。

3.1 仿真测试

自主搭建了Matlab仿真平台，如图5所示，主要包括3个部分：（1）道路模型，利用三次样条拟合道路，获取道路信息，提供简易地图。（2）交通参与者模型，包含交通参与者的类型、速度、加速度、加塞运动等，提供交通流。（3）算法仿真结果展示，开发以主控车辆为中心的自适应绘图工具，提供可视化界面，如图6所示。根据当前主控车辆视角，自适应动态生成以主控车辆为中心的车辆周围环境，包括主控车辆和其他参与者的位姿与运动状态、轨迹等。

图5 Matlab仿真平台的基本框架

图6是算法仿真结果的可视化界面。其中，右上角的Currtime是仿真的当前时间，T是轨迹的控制周期，S是最优轨迹末端的横坐标，D是最优轨迹末端的纵坐标，V是主控车辆要到达的速度，Decision表示行为决策结果。浅灰色的虚线表示道路车道线，深蓝色的实线表示道路中心参考线，从上到下分别是1车道、2车道和3车道。有数字的黄色框是表示路程的路牌。黑色矩形框表示主控车辆，黑色矩形框外面的亮蓝色矩形框表示主控车辆的防碰撞圈，其他颜色的矩形框表示除主控车辆以外的交通参与者。亮蓝色的线表示规划出的所有轨迹，黑色线表示有效轨迹，红色线表示选择的最优轨迹。

图6 仿真结果展示界面

在Matlab仿真平台上，采用3个不同的测试场景对其进行试验，把传统OTG算法和优化后的OTG算法进行对比。在试验中用到了两种类型的车，分别是主控车辆（大车）和障碍物车辆（小车）。当Type为大车时，ηa=03.、ηb=08.和ηc=05.；当Type为小车时，ηb=02.、ηb=07.和ηc=04.。图7～9展示了在3个场景中采用不同的代价函数得到不同的最优轨迹。同一个场景中规划出的所有轨迹都一样，只是最优轨迹略有不同。图7～9中（a）表示的是传统OTG算法的仿真结果，（b）表示的是优化后OTG算法的仿真结果。

由图7可以看出，亮蓝色车辆有侧方插入的趋势，（a）中选择的是回到道路中心线的轨迹，增加了主控车辆与亮蓝色车辆碰撞的概率。而（b）中最优轨迹选择偏向1车道的轨迹，考虑到侧方有插入趋势的亮蓝色车辆和前方距离较近的绿色车辆，降低了主控车辆与它们的碰撞概率。

图7 场景一

由图8可以看出，主控车辆前方有一辆绿色车辆，（a）中仍然沿着道路中心线直行，增加了主控车辆与它碰撞的概率。而（b）中最优轨迹选择避开绿色车辆、偏向1车道的轨迹，大大降低了车辆碰撞的可能性。

图8 场景二

由图9可以看出，主控车辆右侧有一辆正在侧方插入的绿色车辆，（a）中选择回到道路中心线的轨迹，会使主控车辆与绿色车辆碰撞的概率大大增加。而（b）中最优轨迹选择偏向1车道的轨迹，大大降低了主控车辆与亮蓝色车辆的碰撞概率。

图9 场景三

通过比较图7～9中的（a）和（b），表明对OTG算法的代价函数进行改进，规划出的最优路径与传统OTG相比，考虑的因素更多，使行驶更加安全，更加拟人化。这就证明通过考虑交通参与者的安全域对代价函数进行改进这一方法是有效的，达到了优化传统OTG算法的目的。

3.2 实车测试

为验证本文局部路径规划算法，使用陕汽长头牵引车作为试验平台，试验车搭载一套P2高精度MEMS组合导航系统，1个8线激光雷达为ibeo 传感器，1个前向毫米波雷达为杭州智波科技有限公司推出的一款77 GHz中长距离毫米波雷达 ZB710LA01-A，1个侧向毫米波雷达为智波科技 79 GHz 高分辨率雷达和1个内嵌深度学习算法的单目前视相机。图10为试验车，（a）是试验车的正面，（b）是试验车的侧面。图11为实车试验结果，实车试验时车道为单向单车道，在试验时由于场地限制允许轨迹跨越对向车道。图11中（a）为车辆避障时的行驶轨迹，（b）为车辆的方向盘转角，（c）为车辆的行驶速度。

图10 试验平台

由图11可以看出，本文的局部路径规划算法规划出的路径能够成功避开前方动态障碍物，并且在避障过程中，车辆的方向盘转角整体变化平缓且连续，没有发生突变和振荡情况。通过实车试验验证了算法具有实时性和良好的稳定性。

图11 实车试验结果

5 总结与展望

（1）首次提出了安全域的概念，将车辆行驶过程中车、人、路之间各因素对行驶安全性的影响程度统一用安全域表示。安全域的大小表示交通参与者周围的安全范围，两个安全域的重叠面积大小表示行驶安全性的高低，重叠面积越大，行驶安全性越低，重叠面积越小，行驶安全性越高。

（2）利用安全域对传统OTG算法的代价函数进行改进，以达到优化算法的目的。通过Matlab仿真平台对其进行验证，结果显示优化后的OTG算法比传统OTG算法更安全，表明优化是有效的。同时，在实车试验中验证了优化算法的实时性和稳定性。

（3）未来展望主要有以下3点：①对OTG算法的优化只采用了改进代价函数的方法，后续可以利用多种优化方式（调整权重、在预测层加入机器学习算法等），以期达到更好的效果。②仅把安全域应用于路径规划算法，以后的研究可以把安全域应用于更多领域，如行为决策，给行为决策提供新的思路和方法，使安全域发挥更大的作用。③安全域仅考虑了车、人、路之间的互相影响，但忽略了驾驶人在车辆行驶时造成的影响。驾驶人是行驶过程中非常重要的一环，后续研究也应考虑驾驶人对行驶安全性的影响，尽可能地与安全域结合，得到一种更全面的表征行驶安全性的指标，为自动驾驶决策规划提供更可靠的依据。