基于改进RRT*与行驶轨迹优化的智能汽车运动规划

袁静妮 杨林 唐晓峰 陈傲文

为了提高出行效率和保障行车安全,辅助驾驶与自动驾驶技术逐渐成为研究热点[1],车辆运动规划是自动驾驶中的重要环节.为提升规划路径的安全性、舒适性与计算效率,研究者提出并改进了各类运动规划算法或局部路径/速度规划算法[2−3],主要包括曲线规划、图搜索、人工势场法、采样方法、数值优化与数据驱动规划算法等.

曲线规划常用于泊车或局部路径平滑[4],文献[5]通过枚举轨迹曲线,并根据成本函数择优,但可行路径无法穷尽,在复杂场景中不可用.图搜索[6]生成的轨迹不满足车辆动力学、交通规则等各类约束,且其计算效率限制了网格划分密度与搜索空间维数.人工势场法[7]计算量小,在实时避障和路径平滑方面广泛应用,但不合理的势场构建会使行驶路径陷入局部最优或产生振荡.采样方法[8]实时性好且能够在采样过程中考虑各类约束,但存在随机性,相同问题规划结果可能不同.数值优化[9−10]可以综合考虑障碍物约束、车辆动力学模型、交通规则、路径曲率、能耗等各种评价指标,可将最优控制问题转换为非线性规划求解,但该方法对初始解要求较高且计算时间长;也可将模型线性化后使用二次规划求解,但线性化会降低求解精度.基于深度学习的端到端运动规划算法[11],输入传感器信号或感知结果,模型输出运动规划结果或车辆控制信号,该方法可输出接近人类驾驶习惯的规划结果,但较难泛化到极端场景,且存在规划结果帧间跳变问题.基于强化学习的运动规划算法[12]可以使模型自动学习最优规划,但在仿真场景中训练时,障碍物行为与实际路上行为存在一定差异,从而影响算法最终的实车表现.

综合考虑以上方法的优缺点,采样方法计算效率高、对于各类行驶场景通用性好,适合车载实时应用,但需对其采样、扩展策略进行调整使之满足车辆行驶过程中的各类复杂约束.采样方法中,概率地图方法(Probabilistic roadmaps,PRM)[13]或者基于PRM 的快速行进树算法(Fast marching tree,FMT)[14]是概率不完备的,当采样点较少或分布不合理时可能无法求得可行解.快速扩展随机树算法(Rapidly-exploring random tree,RRT)计算速度较快且概率完备.机器人领域使用的RRT 方法[15]生成的路径曲折且搜索方向不受引导,无法直接应用于车辆,因此研究者们针对车辆运动规划问题特性提出了很多改进RRT 算法.文献[16]提出的RRT* 方法使路径在搜索与重连接过程中渐近最优.文献[17]对直道、弯道变道超车场景分别建立高斯分布采样模型以生成符合交通规则的行驶轨迹,该方法局限于特定场景且未考虑动态障碍物.文献[18]添加转向角约束,文献[19]采用B 样条曲线平滑RRT* 搜索的路径,但均未考虑动态障碍物,且直接将路径节点用作曲线控制点可能会导致曲线超出曲率上限或因偏离原路径提高碰撞风险.闭环RRT (Closed-loop RRT,CL-RRT)[20]能够生成满足车辆动力学约束的轨迹,但由于未采用路径简化与局部路径优化,可能会导致车辆在直路上蛇形行驶,文献[8]基于规则模板集进行路径扩展,并使用CL-RRT 中的闭环预测方法求解可行驶的轨迹,尽管轨迹可行驶,但并未针对行驶轨迹的舒适性、安全性和能耗进行优化.

针对以上问题,本文针对自动驾驶车辆局部路径、速度规划问题,提出了一种高效的自动驾驶车辆运动规划求解方法,首先使用改进的采样算法快速求解接近最优解的启发解,然后基于启发解优化得到可行驶、舒适、节能的规划轨迹.本文算法结合改进的RRT* 算法与曲线控制点优化,在动态障碍物环境中生成可行驶的初始局部路径,并优化行驶轨迹以提升平顺性、安全性及降低能耗.相比基于采样的规划算法,本文算法规划的轨迹更平顺、节能且控制可执行;相比基于优化的规划算法,本文算法中的启发解提高了优化的求解成功率和求解速度.本文提出的改进RRT* 算法,在传统RRT* 算法[16]基础上结合了概率采样、多步节点扩展与基于航向相似性的路径简化方法.首先,改进RRT* 根据驾驶场景设置采样概率分布引导平面坐标点采样与车速采样,从而提高搜索效率与路径合理性;其次,改进RRT* 在节点扩展中考虑车辆非完整性约束,并采用多步节点扩展的方式抑制控制量波动、减少计算量;再次,提出基于航向相似性的路径简化算法对初始路径剪枝,相比基于贪心算法的路径简化,提高驾驶舒适性;最后,采用三维空间贝塞尔曲线拟合路径,基于改进RRT* 算法生成的简化路径计算贝塞尔曲线控制点优化问题的初始解,并优化控制点坐标使行驶轨迹满足曲率、航向、加减速、起止状态约束,相比直接采用路径点为控制点的B样条曲线拟合以及通过预瞄控制闭环跟踪路径的CL-RRT 方法,提升了平顺性与安全性,且准确的初始解降低了优化迭代计算量.算法流程如图1 所示.本文第1 节和第2 节分别介绍了改进的RRT*算法与行驶轨迹曲线控制点优化,第3 节为算法的仿真实验验证,第4 节为分析与总结.

图1 算法结构Fig.1 Algorithm structure

1 基于改进RRT* 的初始路径规划

本文算法流程如图1 和算法1 所示.RRT* 中的搜索树定义为T={ni|i=1,2,···,N},其中,ni=[xi,yi,ti,vi,ci,idxi] 为第i个轨迹节点,包含坐标 [xi,yi]、时间ti、车速vi、成本ci、父节点编号idxi.env为环境信息,fsamp为采样概率分布,s为采样点,ne,j为当前步第j个扩展节点,ae与δ分别为第1 步扩展时计算的加速度和转向角,path与paths分别为简化前后的RRT*规划路径,ctrlpt为以贝塞尔曲线表示的行驶轨迹控制点.

1.1 概率采样

常规RRT与RRT* 在整个搜索空间内采样[16],采样不受约束,导致搜索的路径无法满足交通规则约束,如反复变道或压线行驶.交通规则通常包括但不限于:与周边车辆保持纵横向安全车距,靠近车道中心线行驶,减小横向偏移量及其波动,避免反复变道,避免压线行驶,超车完成后尽快回到目标车道,车速不超过道路限速,避免频繁加减速影响后车行驶,与交通信号灯相关的规则等.本文采用平面坐标点概率采样与车速概率采样的方式引导RRT 的采样与扩展,在概率分布中考虑交通规则与横向偏移约束等,优先采样满足交通规则的点.对于安全车距约束,由于障碍物数量较多导致概率分布函数计算显著增加,且在碰撞检测、轨迹优化中均考虑过避障,因此在概率采样中不再添加障碍物距离函数引导采样;对于减小横向偏移、避免频繁变道或压线相关的规则,采样概率分布fsamp(x,y)定义如式(1)～ (4),从而引导采样点靠近目标车道的中心线,在路口等没有车道中心线的场景中可以参考轨迹坐标点串代替式(4);对于车速相关的规则,通过节点扩展时的车速概率采样使车速逐渐趋近于目标车速,如第1.2 节中式(11)和式(12)所示;与交通信号相关的规则一般设置为约束条件,不在采样概率分布中考虑.

其中,Jr(x,y) 为道路势场,dlc(x,y) 为点 (x,y)与车道中心线的距离,采用当前点y坐标与车道中心线y坐标yl(x) 之差近似计算,wr为路宽,cr为道路势场衰减系数,cl0,cl1,cl2,cl3分别为车道中心线在x=0 处的横向偏移、航向、曲率及曲率变化率.

根据上面定义的采样概率分布,采样得到随机采样点s=(xs,ys).该方法基于先验知识缩小搜索空间,由于偏离车道中心线较远的采样点通常会导致扩展分支方向与车道航向接近垂直,而采用上述概率采样方法时,这些点的采样概率接近于0,因此可减少规划器对于无效点的采样,从而在更少的采样次数内搜索到可行解.另外,可基于驾驶员的驾驶行为或离线规划的最优路径建立各场景下的高斯过程模型,用于设计采样概率函数.

1.2 多步节点扩展

常规的RRT 算法在节点扩展时,选取与采样点s距离最近的候选节点作为扩展节点,未考虑转向、加减速约束.本文将候选节点筛选改为寻找一定距离范围内满足转向、加减速、速度约束的点,这些约束的范围需参考具体行驶场景中的速度、加速度限制以及车辆自身特性设置,并使用当前路径分支与扩展分支的航向间的夹角(即近似的转向角)、分支长度来判断是否满足约束,如图2所示.转向角约束阈值则是根据父节点的车速、加速度设定.

图2 候选节点Fig.2 Candidate node selection

其中,dhs用于衡量待扩展分支相对于当前分支的航向变化量,(h1·h2)/(|h1||h2|) 为航向相似度,“·”表示向量点乘,h1为待扩展分支航向向量,h2为当前分支航向向量;dv为节点车速vi与理想车速vop的偏差,除以vnorm以归一化,vnorm可取为自车在一段时间内的平均车速与vop中的较大值;ded为节点与采样点间的欧氏距离.

根据上述方法,计算出周边节点的综合距离值,并在现有树节点中选出满足|dc|

为使规划车速趋近理想车速,采用变长度扩展方法,扩展长度le正比于车速,表示ts时间内车辆行驶的距离,本文设定ts为1 s,即le大小等于车速.扩展长度le通过采样确定,采样范围受理想车速、当前车速、加减速的约束,有一定概率使车速趋近理想车速vop,从而在常规场景中采样到平顺、体感舒适的轨迹,其他情况下在整个可行域内随机采样以提高求解成功率,兼顾探索与优化.对于第i个节点,le计算为

其中,r为(0,1)间均匀分布随机数,vi表示编号为i的节点的车速,为车辆加速度约束范围.根据采样的加速度ae计算扩展点的车速ve与扩展长度le.

为使路径趋近最优,RRT* 采用重连接删除成本较高的分支,本节中成本函数除路径长度外,还加入能耗等指标.扩展分支的成本函数cl(ni,ne)定义为

其中,ne为扩展节点,dhs,ded定义如式(6)和式(10),|ve −vop|op用于衡量扩展节点车速与理想车速的偏差,fe为当前扩展分支的能耗,rbrk,m,µr,Cd,A,ρair分别为制动回馈比例、整车质量、滚动阻力系数、风阻系数、迎风面积、空气密度.通过对能耗、安全性等多项指标的优化以及节点扩展中对转向、加减速的约束,使搜索的初始路径经过曲线平滑后可行驶且在重连接过程中渐近最优,从而减少后续优化计算量.

式(16)用于计算扩展节点总成本,即在候选点集Ne={i|dc(i)

第一步扩展策略如式(17)～ (19),计算扩展方向向量h,从ni沿h方向扩展le,得到初始扩展点[xe,0,ye,0].

车辆运动控制中需避免控制量频繁变化,在本问题中即为期望在一段时间内保持平稳的加速度和转向角,可借鉴模型预测控制的思路,在扩展时沿用第一步控制量,连续扩展多次直至遇到障碍物或偏离目标方向或达到预定的扩展次数,如图3 所示,即可减少控制量波动,同时因为在后续节点扩展时无需重复采样与候选节点计算的步骤,在搜索结束时扩展节点数量相同的前提下减少了计算量.具体步骤如下,求解出第一次扩展的加速度ae和转向角δ,使用旋转变换更新方向向量h(式(20)),然后继续以同样的转向角和加速度行驶(式(21)～ (25)),依次得到Np个扩展分支和扩展点

图3 多步节点扩展策略Fig.3 Multi-step node extension strategy

重连接过程与扩展相似,对新节点附近每一个节点,判断将该节点的父节点换为新扩展的节点是否会降低成本,若能够降低,则对该点进行重连接,将其父节点替换为新扩展的节点.

1.3 基于航向相似性的路径简化

当搜索到若干个距离终点较近的节点,可停止搜索,选择终点附近成本最低的节点,并反向回溯得到一条初始路径,但路径可能存在冗余的转向等,因此需要对路径进行简化,常用方法为贪心算法[21],但该方法简化的路径夹角不受约束,可能会导致急转弯,因此本节提出一种新的路径简化方法,简化中考虑多种指标,这些指标包括:与车道中心线航向相似度、与前一分支的航向相似度、是否碰撞以及与障碍物的距离、简化后行驶距离减少的百分比.对删除某一节点后生成的简化路径进行综合评价,以判断是否需删除该路径节点.初始路径定义为path=[n1,n2,···,nN],N为简化前路径节点数,对于 1

为减少计算时间,并减小连续两次规划结果的差异,当车辆实际行驶轨迹与前一次规划轨迹接近且障碍物运动趋势不变时,优先沿用前一次规划的路径,并规划从前一次路径终点到当前规划终点的可行路径,将这两部分路径连接后,重复路径简化与控制点优化步骤,得到下一时刻的规划轨迹.

2 基于三维空间贝塞尔曲线的车辆运动轨迹优化

由于RRT* 规划的路径不符合车辆动力学约束,无法直接用于控制,得到简化路径后需使用曲线拟合生成一条曲率连续的行驶轨迹.使用最小二乘拟合多项式曲线耗时短,但该方法求得的曲线不能满足曲率、航向、起点终点车速、加速度等各类约束,同样无法用于控制.若直接将路径节点作为贝塞尔曲线或B 样条曲线的控制点[17]用于平滑行驶轨迹,同样存在无法满足约束的问题.使用CL-RRT方法生成的轨迹是可行驶的,满足车辆动力学约束,但并未对通行效率、舒适度、能耗等指标进行优化.由于贝塞尔曲线的起点和终点即为行驶轨迹的起止点,控制点的物理意义明确,可以直接将部分约束转换为控制点的约束以减少计算量,相比各类样条曲线优化变量个数更少,因此使用优化贝塞尔曲线控制点的方法来求解规划路径,将轨迹优化、拟合问题转换为控制点参数优化的问题,其本质与优化多项式系数相同,但更直观.

2.1 车辆运动学模型

高自由度车辆动力学模型无法应用于实时计算,且侧向加速度较小时,简化的二自由度车辆运动学模型的精度足够用于车辆控制[22].以车辆后轴中心为参考点建立二自由度车辆运动模型为

其中,x(t),y(t) 为t时刻车辆后轴中心的x,y坐标,L为车辆前后轴距,δ(t) 为转向角,θ(t) 为航向角.

n阶贝塞尔曲线[4]定义为

其中,Pi为第i个曲线控制点,τ为自变量,Bi,n(τ)为伯恩斯坦多项式.对曲线求微分可得如下的一阶微分和r阶微分递推关系(式(32)和式(33)),可用于计算车速、加速度等.

根据之前RRT* 规划的路径,终点nsN对应时间tsN,设置终点时间td=tsN.将车辆后轴中心行驶轨迹表示为贝塞尔曲线方程(式(34)和式(35)),并设置贝塞尔曲线中的τ=t/td.根据行驶轨迹方程,结合车辆运动学模型,可求得车速v、加速度a、航向角θ、曲率κ,如式(36)～ (39)所示.

其中,(xi,yi) 为控制点.

2.2 曲线控制点优化

建立优化问题模型,在RRT* 规划路径基础上,以曲线控制点为优化变量,求解安全、平顺的行驶轨迹.优化目标定义如式(40),包括路径偏差、障碍物距离、车速波动、曲率、能耗等:

其中,xref(t)和yref(t) 为根据paths插值计算的参考轨迹,fe(t) 为式(15)计算的能耗,ωi,i=1,2,···,5为权重.

优化问题的约束包括: 起点终点约束、动力学约束、障碍物与交通规则约束,分别定义如下.

1) 起点、终点约束

起点、终点约束包括对起始位置、起始车速、起始航向、终点位置、终点车速、终点航向的约束.起点处的约束即为当前时刻车辆状态为定值;终点处的约束视场景而定,例如在换道场景中,要求终点航向与目标车道中心线航向一致.

采用车身坐标系,车辆起点坐标即为[0,0],则起点位置约束如式(41),起始车速约束如式(42).在起始状态给定时,贝塞尔曲线中的第1 个和第2个控制点,即 [x0,y0] 和 [x1,y1],可直接计算,无需优化.终点位置约束取决于具体场景,则此处可设置终点坐标为RRT* 规划终点坐标,如式(43)所示,据此计算控制点 [xn,yn].

2) 动力学约束

动力学约束包括对车速、加速度、曲率的约束,设置如下:

其中,vmax为车辆最高车速或道路限速,[amin,amax]为车辆加速度范围,κmax为路径最大曲率限值,其中 [amin,amax]与κmax均与车速相关.

3) 障碍物与交通规则约束

采用p-norm 函数[23]描述障碍物,障碍物形状近似为矩形,考虑障碍物航向的p-norm 函数定义为式(47),当hobs大于距离阈值dthre时,认为车辆与障碍物之间不存在碰撞.

其中,xobs,yobs为t时刻障碍物中心坐标,θobs为t时刻障碍物航向,lobs,wobs为障碍物长宽,dx和dy是车辆与障碍物中心在障碍物纵轴和横轴方向上的距离.

道路边界约束包括道路边沿的硬约束和虚线车道间隔线的软约束,道路边沿的约束可以转换为不同纵向距离处的横向偏移约束,车道线的软约束在目标函数中考虑,在目标函数中添加偏离目标车道中心线惩罚Pr

其中,yl为x(t) 处车道中心线横向坐标,wr为车道宽度,kr为常数,用于调节车道中心线惩罚对规划路径的影响程度.

上述优化问题的优化目标和约束需根据具体行驶场景、感知误差、预测误差、车辆自身特性、外界扰动、车辆模型误差、控制超调比例等因素调整,从而避免规划的轨迹无法执行.

优化的计算速度与初始解相关,当初始解接近最优解时,可加速收敛.本节根据RRT* 规划的简化路径,预先计算一组合理的初始控制点.首先根据起点、终点的位置和速度,根据式(41)～ (43)的起点终点条件可计算第1 个和第2 个控制点与最后一个控制点,从RRT* 规划的路径中,等时间间隔插值取出n−2 个路径点,不含起点终点,路径点包含时间、位置等信息,将其代入n阶贝塞尔曲线方程可以计算得到剩余的n−2 个控制点,以这组控制点作为优化问题初始解.最后使用序列二次规划(Sequential quadratic programming,SQP)方法求解上述优化问题以优化这组控制点,使最终的行驶轨迹收敛到局部最优解.

3 仿真实验验证

在Intel I7-7500U 笔记本电脑上使用MATLAB R2018a 对本文提出的算法进行仿真实验验证.以弯道行驶场景为例,随机设置环境中障碍物的位置、速度,使用改进的RRT* 算法与曲线控制点优化进行车辆运动规划.

仿真案例设置为多车道弯道环境下的障碍物避障轨迹规划,其中起始状态设置为x0=0 m,y0=0 m ,θ0=0 rad,v0=4 m/s,目标车速设置为vop=10 m/s ,规划终点设置为xe=90 m,ye=−2 m.

图4(a)和图4(c)为常规RRT* 方法(对照策略)搜索的路径与车速,图4(b)和图4(d)为本文提出的改进RRT* 算法搜索的初始路径与车速.图中细实线表示搜索结束时的RRT 搜索树状态.运动规划的计算耗时包括两部分: 1) RRT 搜索树采样、扩展与简化的计算耗时;2) 曲线优化的计算量.首先,对照策略的采样与扩展完全随机,产生了很多无效扩展节点,而本文中的采样概率分布是根据人类驾驶员的驾驶习惯、交通规则设置的,采样概率低的区域存在最优路径的概率较低,因此减少对于该区域的搜索可以减少最终所需的采样与扩展次数.其次,对照策略中对扩展方向不做任何约束,而不合理的初始解会导致后续曲线优化过程的迭代次数增加.另外,即使假设不同方法搜索到最终路径时,RRT 搜索树需要扩展出相同数量级的分支和节点,采用多步扩展方法可以减少数次采样与候选点的计算,而改进RRT* 在扩展与重连接中的单步计算量与原RRT* 算法相等,因此整体计算量减少.综上,本文提出的方法能够提高搜索效率,并减少搜索与优化的整体计算耗时.在仿真中随机生成障碍物数量、尺寸、位置,以模拟不同的环境,使用本文算法进行运动规划,统计多次结果,本文算法搜索次数相比常规RRT* 降低约71.68%,计算耗时降低49.28%.同时常规RRT* 中车速随机采样导致最终轨迹呈现频繁加减速,且常规RRT* 方法的成本函数中未考虑能耗,因此无法在重连接的过程中逐渐优化车速使能耗降低,在该仿真场景中对照策略的能耗相比本文提出的策略高14.9%,且车速随机采样导致平均车速与车辆通行效率较低.

图4 RRT* 方法与改进RRT* 的比较Fig.4 Comparison of basic RRT* and improved RRT*

图5(b)和图5(c)分别用贪心算法和本文提出的路径简化算法对图5(a)中RRT*搜索的初始轨迹进行了轨迹简化,图中细实线表示RRT* 搜索树,包含搜索过程中扩展的所有节点与分支,粗实线为简化的路径,粗虚线为贝塞尔曲线拟合的路径.基于贪心算法的简化不考虑转向角约束以及车道行驶的交通规则,仅根据距离最短与碰撞检测简化路径,而本文算法综合考虑车道中心线的航向、转向角与路径长度,因此图5(c)中改进的简化算法计算的路径(粗实线)相比图5(b)中贪心算法简化路径(粗实线)更平滑,转向角小,图5(c)和图5(d)中的虚线为贝塞尔曲线拟合的行驶轨迹和车速,对应的控制点分布如图6 所示.由于改进RRT* 搜索路径时已在成本函数中考虑了能耗、安全性、行驶距离等多项指标,因此初始路径与最优解较接近,图6中初始化的控制点与优化后的控制点非常接近,因此迭代次数少.

图5 轨迹简化与曲线拟合结果Fig.5 Path simplification and curve fitting result

图6 贝塞尔曲线控制点优化结果Fig.6 Bezier curve optimization result

图7 中将本文提出的曲线控制点优化方法与常用的CL-RRT、三次B 样条曲线平滑方法进行了比较.给定相同的初始路径,CL-RRT 方法通过预瞄与反馈控制生成一条可行驶的轨迹;B 样条曲线直接采用包含时间信息的初始路径插值点作为控制点,拟合得到x(t),y(t) 轨迹;本文算法根据初始路径预估初始控制点后进一步优化控制点分布.仿真结果如图7 所示,图7(a)为x−y平面内的规划路径,图7(b)和图7(c)为根据规划的车速计算的加速度和冲击度,图7(d)为路径曲率.三种方法均满足曲率连续、起始车速、起点、终点位置的约束;B 样条拟合直接以路径点为控制点,未考虑起点状态约束,因此在起点处易出现路径曲率大、冲击度高的情况,导致控制平顺性差,且由于拟合时不再进行碰撞检测,曲线控制点间隔较大时易导致碰撞,而控制点间隔较小时曲率较大;本文方法在优化目标中考虑了舒适性与经济性,因此优化的行驶轨迹加速度与曲率变化更平缓,冲击度更小,平顺性好;且因在控制点优化时考虑了安全性,因此本文方法生成的曲线优化了避障轨迹,距离障碍物更远,相比仅考虑跟随和拟合的CL-RRT 和B 样条曲线,提高了安全性.使用基于二自由度动力学模型的预测控制算法控制车辆的纵横向运动[24],使车辆跟踪规划的车辆轨迹,平均横向跟随误差为0.05 m,平均车速跟随误差为0.08 m/s,表明规划的轨迹是可行驶的,符合车辆运动特性.

图7 曲线平滑方法比较Fig.7 Comparison of curve smooth methods

4 结束语

本文提出了一种改进的RRT*与贝塞尔曲线控制点优化结合的车辆运动规划方法,能够在动态障碍物环境中生成兼具安全性与驾驶舒适性的可行驶路径.在改进的RRT* 方法中,建立用于引导搜索的采样概率分布,对纵横向位移、车速三个维度进行概率采样;改进了节点扩展方式,在扩展时考虑方向相似性、加减速、能耗等,并采用多步扩展以加速搜索;提出了基于航向相似性的路径简化方法;最后根据搜索的初始轨迹拟合空间贝塞尔曲线,并通过优化曲线控制点使行驶轨迹满足各类约束的同时,提高平顺性与安全性.

由于RRT 存在随机性,因此无法保证相同场景下每次规划的轨迹均相同,希望通过进一步的研究,在实时计算中使规划结果稳定并趋近于全局最优解.