无人驾驶汽车局部路径规划算法研究*

彭晓燕，谢 浩，黄 晶

（湖南大学机械与运载工程学院，长沙 410082）

前言

随着汽车数量的不断增长，交通事故也随之增多，汽车安全早已成为全社会关注的焦点问题［1］。无人驾驶技术在降低道路交通事故发生率方面有着重要的研究意义和巨大的应用价值。随着人工智能的应用和发展，无人驾驶汽车也越来越受到关注，其中的避障问题已经成为了研究的热点［2］。局部路径规划，也称为避障路径规划，即考虑本车和障碍物之间的几何关系寻找出一条避免与障碍物发生碰撞的路径，是无人驾驶汽车的重要功能模块之一。

目前常用的局部路径规划算法主要分为4大类：人工势场法、基于图搜索的方法、基于采样的方法和基于离散优化的方法。人工势场法是Khatib提出的虚拟力法，此方法算法简明，实时性良好，但存在容易陷入局部最小点的问题，且因未考虑车辆动力学约束，导致无法得到合理的路径甚至规划失败［3］。针对此问题，安林芳等［4］提出了一种新的障碍点构建方式，解决了局部最小值问题；修彩靖等［5］利用高斯组合隶属函数建立引力的目标点函数，并在引力点函数中考虑障碍物约束和车辆约束，提高了规划路径的平滑性；唐志荣等［6］利用椭圆化距离代替传统斥力势场中的实际距离，结合模型预测控制的方法，使规划路径满足多约束条件。但是以上方法只考虑了障碍物静止的工况，因此在动态环境无法取得良好效果。基于图搜索的方法中常用的有A*［7-12］和 D*算法［13］，此类算法虽然在机器人领域得到广泛应用，但其规划的路径未能满足车辆的非完整性约束，且路径平滑性较差。辛煜等［7］提出了一种改进的A*算法，对栅格地图中的栅格进行线性插值，增加了A*算法的可搜索邻域和搜索的方向，改善了传统A*算法平滑性较差的问题；齐尧等［8］提出了新式的启发式函数并结合了车辆非完整性约束进行三维规划，提高了路径的安全性和舒适性；Boroujeni等［9］基于 A*算法提出了 FU-A*算法，该算法根据车辆的速度调节栅格的大小，提高了规划路径的平滑性；城市交通环境中的最优路径受诸多因素影响［10］，胡林等［11-12］在城市道路网的最短路径规划中，在传统的A*算法的估价函数中分别考虑了信号交叉口的等待时间和能源消耗，有效降低了等待时间和能耗。基于采样的方法有概率路图法（probabilistic road map，PRM）［14］和快速随机扩展树法（rapidly random tree，RRT）［15-17］，该类算法具备搜索速度快、无须对环境进行建模等优点，但其随机采样的特性导致路径不平滑。Taheri等［15］基于RRT算法提出了模糊贪婪快速探索随机树算法，减少了节点的数量，降低了算法的计算复杂度；宋晓琳等［16］利用高斯分布函数结合期望路径模型，在RRT算法中引入启发式搜索机制寻找合适的避障路径，最后用B样条曲线插值的方法进行光滑化处理，解决了RRT算法规划的路径不平滑的问题；杜明博等［17］在RRT算法基础上结合了环境约束和车辆自身的约束，实现了复杂的低速环境的路径规划。基于离散优化的路径规划方法是用数值积分和微分等方程来描述车辆的运动，从而产生数量有限的候选路径，并通过设计代价函数，从候选路径中选择最优路径［18-19］。该方法计算量小，实时性好，在近年来得到了广泛应用［20］。Chu等［18］提出了一种基于此方法的算法，但未考虑城市中多车道的情况。周惠子等［21］提出了一种加权碰撞检测的方法，在路径规划中考虑了多车道的工况，该方法可满足车辆对静止障碍的避障要求，但未考虑车辆对移动障碍的避障要求。Hu等［20］在此基础上考虑了移动障碍，并提出了一种新的移动障碍安全性代价函数，但是该方法仅考虑了同向行驶的动态车辆，未考虑行人等移动随机性较大的障碍，具有一定的局限性。陈成等［22］提出了一种基于4阶贝塞尔曲线的轨迹规划方法，采用优化方法求解最优轨迹。张琳等［23］引入了滚动窗口优化策略，根据最优指标决策出当前规划窗口的局部最优路径。Li等［24］基于离散优化的方法提出了一种实时的轨迹规划框架，并在实际道路环境中验证了算法的可行性。Lim等［25］提出了一种基于采样和优化方法相结合的分层轨迹规划方法，仿真表明该算法适用于多种城市场景。Zhang等［26］结合多种方法提出了一种混合轨迹规划算法，该方法考虑了车辆的几何约束及非完整约束，规划路径较为平滑。以上基于离散优化的局部路径规划算法在规划时需要进行坐标转换，常规转换方法存在积分等复杂运算，实时性有待进一步提高。

针对以上不足，本文中提出了一种基于离散优化的无人驾驶汽车局部路径规划算法，设计了一种基于运动估计结合高斯卷积的移动障碍安全性代价函数，并结合静止障碍安全性代价函数，使无人驾驶车辆可以完成对静止和移动障碍的规避；使用了一种新的坐标转换计算方法，将路径从规划时使用的s-ρ坐标系转换到大地笛卡尔坐标系，提高了实时性。最后仿真和实验结果表明，提出的规划算法规划的路径不仅能满足避障要求，且运动轨迹平滑，车辆跟踪路径时侧向加速度在合理范围内，稳定性良好，实时性满足无人车对局部路径规划算法的要求。

1 局部路径规划算法

局部路径规划是在已知全局路径的基础上进行的，全局路径由高精度地图提供。算法的流程如下：首先，使用三次样条曲线对全局路径进行弧长参数化拟合；然后，利用全局路径上的弧长s和距离全局路径的横向偏移量ρ建立s-ρ坐标系，并规划出一系列的平滑曲线，即候选路径，再将其从s-ρ坐标系转换到大地笛卡尔坐标系中以便于后续的路径跟随控制；最后，采用多目标代价函数从候选路径中选择出最优路径。

1.1 全局路径基准线的拟合

全局路径由一系列离散的点序列组成，作为局部路径规划的基准线。考虑到三次样条曲线的1阶和2阶导数具有连续性的优点，本研究使用三次样条曲线来拟合全局路径，即

式中：s为车辆当前位置距离基准线的最近点所在基准线上的弧长；si为该弧长所在的第i个路径片段的起点；x0和y0为基准线的点在大地笛卡尔坐标系的x，y坐标；axi、bxi、cxi、dxi、ayi、byi、cyi和 dyi为基准线拟合的样条曲线的参数。

如图1所示，基准线在拟合时会被分成很多小段，其中每一段路径片段都可以由式（1）表示。

图1 车辆在全局路径的定位

因此，基准线上的点的航向角和曲率可以用x0（s）和y0（s）对s的1阶导数和2阶导数计算求出，即

式中：x′0、y′0和 x″0、y″0分别为 x0（s）和 y0（s）对 s的1阶导数和2阶导数。

1.2 候选路径的生成

候选路径为以汽车当前位置为起点，路径的末端与基准线切向角相同的一系列路径。为了确定候选路径的起点和终点，需快速准确地找到车辆距离基准线的最近点，本研究使用Wang等［27］提出的二次规划与牛顿法相结合的方法寻找车辆距离基准线的最近点。假设车辆距离基准线的最近点所在基准线上的弧长为s，车辆与基准线之间的距离为横向偏移量ρ，则车辆当前的坐标即可用弧长s和横向偏移量ρ表示，本研究将这样描述车辆位置的坐标系称为s-ρ坐标系。在s-ρ坐标系中，每条候选路径由沿基准线的长度Δs、当前车辆位置的偏移量ρsi以及最终的横向偏移量ρfi确定。假设候选路径的横向偏移量也满足三次样条曲线的方程，则第i条候选路径可以表示为

式中：Δs＝s-sstart，sstart为车辆距离基准线的最近点所在基准线上的弧长；send为基准线上的候选路径的末端对应的弧长。

为求解式（4）中的系数 a、b、c，候选路径的生成需要考虑车辆当前的航向，同时希望路径的末端与基准线的前进方向相同，以确保规划出可行的路径，如图2所示，由此得出式（5）中的4个边界条件。

式中θ为基准线最近点切向角θstart与车辆当前航向θ0之差。

图2 边界条件示意图

如图3所示，每条候选路径都由不同的末端横向偏移量ρfi确定，设定合适的横向偏移量的变化量Δρ，根据不同的横向偏移量ρfi的值，可以计算得到多组不同的系数 a、b、c，从而得到多个式（4）的方程，即可生成多条候选路径。

图3 候选路径示意图

1.3 坐标转换

候选路径基于s-ρ坐标系计算，而路径跟随控制基于大地笛卡尔坐标系，故必须将候选路径从s-ρ坐标系转换到大地笛卡尔坐标系。常规的基于离散优化的路径规划方法［18，20-21］的坐标转换需要进行复杂的积分计算。为提高算法的实时性，本研究使用了一种新的坐标转换计算方法。候选路径可以用一系列离散点来表示，如图4所示，根据几何关系，第i条候选路径的离散点可表示为

式中：（xi，yi）为第 i条候选路径在大地笛卡尔坐标系的坐标；（x0，y0）为基准线上弧长s对应的点在大地笛卡尔坐标系的坐标，可由式（1）求得；ρi（s）为横向偏移量，可由式（4）求得；θ为车辆当前航向θn与基准线最近点切向角θ0之差。

图4 坐标转换示意图

1.4 最优路径的选择

最优路径为候选路径中在满足安全性的前提下平滑性较高的路径。本研究设计了一种加权多目标代价函数去评价候选路径，归一化处理各代价函数后进行加权求和计算，通过选择代价函数最小的路径来选取最优路径。代价函数考虑路径的安全性、平滑性及跟随全局路径的能力，设计的代价函数包含3个部分：路径安全性代价函数fs、路径偏移代价函数fo和路径平滑性代价函数fsm，即

式中：f（i）为路径总代价函数；i为候选路径的序列号；ws、wo和wsm分别为各代价函数的权重系数；select为选择的路径。

本研究的最优路径的选择优先考虑安全性，所以根据安全性代价函数阈值设计了各代价函数权重值的函数，当安全性代价函数值在阈值以上时，说明安全性较低，故安全性代价函数取较大的权重，降低选择此类路径的概率，反之则取较小的权重，提高选择此类路径的概率。经过大量仿真实验总结，本研究中ws、wo和wsm3个权重系数的选择为

1.4.1 路径安全性代价函数的设计

为解决无人驾驶车辆对静止和移动障碍的规避问题，本研究设计了一种综合考虑静止和移动障碍因素的加权路径安全性代价函数，其中，静止障碍安全性代价函数使用离散高斯卷积结合碰撞风险的方法来评估每条候选路径的安全性；移动障碍安全性代价函数则是基于移动障碍运动状态估计结合高斯卷积。

（1）静止障碍安全性代价函数

无人驾驶汽车在道路行驶时，道路上的静止障碍如停靠路边的车辆等，都是潜在的危险源。路径规划算法必须确保车辆行驶的安全，避免车辆与障碍物发生碰撞。

本研究采用外接圆对障碍物进行包络，障碍物的信息可以由车上的雷达传感器获得。本研究通过判断候选路径与障碍物外接圆是否相交来进行碰撞检测，若相交，则该路径不可行，如图5所示，第i条候选路径的碰撞检测结果为Ci，若相交，Ci＝1；否则Ci＝0。

候选路径的碰撞检测结果是选择安全路径的重要信息，但是只考虑碰撞检测结果不能完全保证无人车行驶的安全性。如图5（a）所示，R5和R10都是非碰撞的路径，但是这两条路径距离障碍物很近，选择其作为避障路径是不安全的，因为如果路径跟踪的精度不高或障碍检测误差过大，将导致车辆与障碍物发生碰撞，所以对于这种不会与障碍物发生碰撞但是接近障碍物的路径的安全性，必须引入相邻路径的碰撞风险并作进一步评估。

图5 候选路径碰撞风险示意图

虽然候选路径与障碍物的距离可以很好地评估候选路径的安全性，但是候选路径或障碍物过多时需要大量的计算，导致实时性较差，而离散高斯卷积计算仅需候选路径的碰撞检测结果即可通过较小的计算量完成对所有候选路径的碰撞风险评估，具有良好的实时性，但离散高斯卷积需要合理设置碰撞风险的标准差，该值决定了碰撞检测的有效范围，若过大会导致路径选择过于保守，过小则无法保证路径的安全性。离散高斯卷积的过程是对每条候选路径进行碰撞风险评估时，考虑了其相邻的一系列候选路径的碰撞风险，当其相邻一系列候选路径有碰撞风险的路径则说明该路径危险程度较高，反之说明危险程度较低。为了保证安全性和实时性，本研究使用离散高斯卷积结合碰撞风险的方法来评估每条候选路径的安全性，可表示为

式中：fsta（i）为候选路径Ri的静止障碍安全性代价函数；gsta（j）为静止障碍离散高斯卷积函数；σs为静止障碍碰撞风险的标准差，决定了碰撞检测的有效范围，经过大量的仿真分析，取值为2。

各路径的静态安全性代价函数见图5（b），结果说明，路径 R1、R2、R3、R12和 R13的安全性代价函数均接近0，而有可能引导无人车与移动障碍车碰撞的R6～R9的安全性代价函数均在0.6以上，这说明安全性代价函数可以显式地说明候选路径的安全性，可以作为衡量本研究候选路径安全性的指标。

（2）移动障碍安全性代价函数

无人驾驶汽车在实际的道路中行驶，还存在如行人、行驶的车辆等移动障碍物，因此除考虑静止障碍因素外，还须考虑移动障碍对路径选择的影响。如图6所示，不考虑移动障碍物时，根据其余的代价函数可知R4为选择的最优路径，但若存在如行驶在相邻车道速度为vobs的汽车等移动障碍物，则选择路径R4将会导致无人车与之发生碰撞。因此，在选择最优路径时，必须考虑移动障碍物的影响。

图6 移动障碍示意图

移动障碍安全性代价函数是基于移动障碍运动状态估计并结合高斯卷积得到。车载雷达测量值带有测量噪声，因此需要设计运动状态估计算法以更为准确辨识障碍目标的状态［28］。首先，采用谢德胜等［29］提出的移动障碍物运动状态估计方法对下一时刻障碍物的位置坐标进行估计。移动障碍物的运动状态可以表示为

式中：（xk，yk）为第k个周期时移动障碍物在大地平面直角坐标系XOY的坐标；vksinβk为移动障碍物在正东方向上的分速度；vkcosβk为移动障碍物在正北方向上的分速度；vk为移动障碍物的速度；βk为移动障碍物的航向角。

将式（12）的状态量代入基于匀速模型的标准卡尔曼滤波器，即可预测到下一时刻的移动障碍物位置坐标。然后，使用高斯卷积结合碰撞风险的方法即可得到移动障碍物的安全性代价函数，即

式中：Cdk为第 i条候选路径的碰撞检测结果；gd（j）为移动障碍高斯卷积函数；σd为移动障碍碰撞风险的标准差，考虑到移动障碍的碰撞风险较大，经过大量的仿真分析，设定σd为3。

路径安全性代价函数定义为静止障碍安全性代价函数和移动障碍安全性代价函数的加权和，即

式中wsta和wd分别为静止和移动障碍安全性代价函数的权重值。当只检测到静止障碍物时，wsta和wd分别设定为1和0；同理，只检测到移动障碍时，wsta和wd分别设定为0和1；同时检测到移动障碍与静止障碍时，经过大量的仿真实验发现wsta和wd同时设定为0.5效果较好。

1.4.2 路径偏移代价函数的设计

引入路径偏移代价函数的目的是使行驶车辆尽可能靠近全局路径，当其它代价函数为零时，路径偏移代价函数则是决定路径选择的唯一因素。路径偏移代价函数设计为每条候选路径的最终的横向偏移量 ρfi的绝对值，即

图7（a）为候选路径示意图，图7（b）为路径偏移代价函数图，当i＝7时，fo最小，说明R7为最接近全局路径的路径。

图7 路径偏移代价函数示意图

1.4.3 路径平滑性代价函数的设计

汽车在行驶时，不平滑的路径会导致驾驶的舒适性降低，甚至会引起车轮打滑，从而降低了车辆行驶的稳定性和安全性［21］。只考虑一个规划周期时，候选路径的平滑性与路径的曲率有关，路径的曲率变化越小说明路径变化缓慢，路径的平滑性越好，反之说明平滑性较差。因此本研究利用候选路径在离散采样点上的曲率绝对值之和作为路径平滑性的代价函数：

路径的连续性也会影响无人驾驶汽车行驶的平顺性。上面只考虑了本周期规划的路径信息，无法避免路径发生突变。如图8所示，R1和R3都是安全性代价函数较小的路径，但是如果选择R1作为最优路径，则会导致路径发生突变，从而导致控制时前轮转向角突变，降低驾驶的舒适性与稳定性。为解决这个问题，路径的连续性代价函数在选择最优路径时考虑了上一周期规划的路径对本次规划的影响，在满足安全性的前提下，选择与之前规划的路径变化最小的路径，即图8中与上一时刻规划的路径Rpre偏移量最小的路径为R4。本研究使用离散的方法计算前一路径与新路径之间的公共弧长部分的偏移量之差的总和，即

式中：N为两段路径公共弧长之间离散采样点的个数；sfi为路径末端点对应的弧长；ssi为路径起始点对应的弧长。

图8 路径连续性代价函数示意图

路径平滑性代价函数定义为曲率变化代价函数和连续性代价函数的加权求和值，即

式中wc和wco分别为曲率变化代价函数和连续性代价函数的权重值，经过大量仿真实验，取值分别为0.3和0.7时效果较好。

2 仿真结果与分析

为验证本研究局部路径规划算法的可行性和有效性，在PreScan软件中搭建了车道宽为3.5 m的三车道作为车辆行驶道路，并在道路上设置了若干静止与移动障碍车和行人，道路包括直道路段和弯道路段，如图9所示，使用Matlab／Simulink软件对本研究算法进行编程实现，对无人车在存在静止及移动障碍的道路行驶过程进行了联合仿真。当无人车接近移动障碍车时，移动障碍车将会从静止加速到恒定速度，移动障碍车1和2的速度分别为36和25 km／h，行人横穿马路的速度为3.6 km／h，以检验本研究算法在直道和弯道中对静止障碍连续避障及对行人和移动障碍车进行避障的有效性。

图9 道路场景全局视图

为使无人车可以快速、稳定地对规划的路径进行跟踪，本研究基于Pure Pursuit算法设计了路径跟踪控制器。Pure Pursuit算法是一种基于几何关系的路径跟踪算法，最早应用于移动机器人路径跟踪上，卡耐基梅隆大学将其应用在无人驾驶汽车的路径跟踪控制，由于其具有模型简单直观、参数易调、计算量小等优点而得到了广泛应用［30］。

以PreScan软件中的Audi A8为被控的无人车，设定其速度为50 km／h。无人车运动轨迹如图10所示，图中圆形为静止障碍车，无人车的运动轨迹如虚线所示，车辆顺利地避开了所有静止和移动障碍车以及行人，并在满足避障要求的同时，很好地对全局路径进行跟踪。图11为无人车运动轨迹曲率，车辆行驶轨迹的曲率在-0.026 1～0.041 m-1之间，曲率的最大值为0.040 8 m-1，曲率连续且始终保持在较小值，说明本研究算法规划的路径平滑性较好。图12为无人车在行驶过程中的侧向加速度变化。从图中可知，无人车在整个行驶过程中，侧向加速度均低于0.5g，峰值为0.49g，说明车辆行驶稳定。

图10 仿真工况无人车运动轨迹

图11 仿真工况无人车运动轨迹曲率

图12 仿真工况侧向加速度

图13 为无人车对移动障碍车2的避障过程的路径生成及选择的示意图。图13（a）描述的是在无人车前方检测到移动障碍车时，选择了R6，该路径的平滑性较好且满足避障要求；图13（b）描述的是无人车即将对移动障碍车2进行超车避障，依然选择了R6，该路径与全局路径距离很近，可以引导无人车很好地跟踪全局路径；图13（c）和图13（d）描述的是无人车顺利换道避障并最终行驶在全局路径上。

图13 无人车避障示意图

为更为显式地说明路径选择的过程，提供了图13对应的避障时刻的各代价函数图，如图14所示。在 t＝30.7 s、t＝31.3 s和 t＝31.9 s这 3个避障时刻，有可能引导无人车与移动障碍车碰撞的R9～R13的安全性代价函数均在0.6以上，而路径R6的安全性代价函数分别为0.17、0.18和0.04，均在较低水平，这说明R6的安全性较高，且路径R6的平滑性及偏移代价函数均为较小值，这说明该路径平滑性较好且能很好地跟随全局路径，故选为最优路径；t＝32.5 s时，候选路径均不会与移动障碍车发生碰撞，所以各候选路径的安全性代价函数均为0，此时路径选择主要由平滑性与偏移代价函数共同决定，在上述两种代价函数的作用下，最终选择了与全局路径重叠的R7，引导车辆最终行驶在全局路径上。

图14 候选路径各类代价函数

3 实验结果分析

为验证本研究算法在真实环境中避障的有效性和实时性，开展了实车实验。采用湖南大学“远飞”无人车实验平台，硬件为Intel Core i5-3337U（四核，1.8 GHz），8 GB内存。由激光雷达、摄像头等环境感知传感器获取车辆周边的环境信息，决策规划模块根据这些信息实时规划出最优路径并输出给路径跟踪控制部分。图15为无人驾驶汽车系统硬件实验平台。实验使用Pure Pursuit算法［30］进行路径跟随控制。实验场地包括了直道和弯道路段，并布置了静止障碍物与移动障碍车。利用无人车导航系统采集的RTK-GPS信息对行驶轨迹进行拟合得到全局路径。实验时无人车的车速为30 km／h。图16示出无人车对移动障碍车的避障过程。可以看出无人车从道路左侧避开移动障碍车后，回到全局路径。

图15 湖南大学“远飞”无人车实验平台

图16 无人车避开移动障碍车的过程

图17 为车辆在实验场地的避障路径规划轨迹图。由图中可见，车辆顺利避障并很好地跟踪上全局路径。图18为实验工况无人车速动轨迹曲率。由图可知，车辆轨迹的曲率在-0.08～0.06 m-1之间，曲率的最大值为-0.070 25 m-1，曲率连续且始终保持在较小值，满足车辆对路径的平滑性要求。图19为无人车在行驶过程中的侧向加速度变化。从图中可知，无人车在整个行驶过程中，侧向加速度均低于0.5g，峰值为-0.23g，满足车辆对稳定性的要求。图20为无人车避障过程的显示界面截图，从中可以看出，本研究的规划算法能够根据自身状态实时选择最优的避障路径。

图17 实验工况无人车运动轨迹

图18 实验工况无人车运动轨迹曲率

图19 实验工况侧向加速度

为进一步了解本研究算法在提高实时性方面的效果，对采用常规坐标转换方法的离散优化路径算法（称为方法1）在相同的实验条件下进行了规划实验，与本研究算法进行比较。图21为本研究算法与方法1在相同实验条件下的耗时对比。方法1算法耗时在18～55 ms之间，平均耗时29.9 ms，本研究算法耗时在10～25 ms之间，平均耗时18.3 ms，实时性相比方法1有明显提高。

图20 无人车避障示意图

图21 算法耗时

4 结论

为满足无人驾驶汽车局部路径规划算法对安全性、实时性以及规划路径的平滑性等要求，设计了一种基于运动估计结合高斯卷积的移动障碍安全性代价函数，并结合路径平滑性和路径偏移代价函数，使规划的路径可以引导车辆安全合理地避开静止和移动障碍；使用了一种新的坐标转换计算方法，提高了算法的实时性。仿真和实车实验结果表明，本研究算法可以实时规划得到安全平滑的避障路径，可应用于实时的无人车局部路径规划。

今后将研究各代价函数的权重系数的取值对路径规划效果的影响，及障碍的危险性与其碰撞风险的标准差的关系，使路径规划算法得到更为拟人化的效果，并将该算法应用于更为复杂的场景中。