高速公路弯道换道决策及运动规划优化

袁昌 莫天石 舒红

收稿日期：2023-06-02

网络出版日期：2023-10-07

基金项目：重庆市重大主题专项资助项目（cstc2019jscx-zdztzxX0039）。

作者简介：袁昌（1999—），男，硕士研究生，主要从事自动驾驶汽车控制及测评研究，（E-mail）lemon_passerby@163.com。

通信作者：舒红，女，副教授，（E-mail） shuhong@cqu.edu.cn。

摘要：弯道换道决策及运动规划算法主要影响自动驾驶汽车的安全性和操纵稳定性。针对高速公路弯道换道场景决策的安全性和行驶效率不够高的问题，提出新的基于主车相对前车的驾驶不满意度的决策算法。为了提高运动规划算法实时性，采用路径-速度解耦框架进行主车换道轨迹规划。对于路径规划，选择五次多项式曲线，采用考虑安全、舒适和高效性的4个换道路径评价指标，实现最优路径规划。对于速度规划，结合动态规划与二次规划优化获取平滑速度规划曲线。仿真结果表明基于驾驶不满意度的换道决策模型能选择更高效和安全的行驶方式。在典型的主车换道场景，主车最大质心侧偏角，最大横摆角速度的数值均小，表明换道轨迹规划算法能确保主车换道的安全性和操纵稳定性。

关键词：自动驾驶汽车；运动规划；决策；驾驶不满意度；优化

中图分类号：U463.6          文献标志码：A          文章编号：1000-582X（2024）03-030-14

自动驾驶技术对于减少交通事故、改善交通拥堵状况等具有重要作用，已成为汽车行业发展的战略方向和竞争焦点^[1]。高速公路车速较高，弯道路段换道比直道换道更危险，若处理不当将发生车道偏离、侧滑或碰撞等事故。自动驾驶汽车弯道换道决策及规划算法决定换道过程的安全与操纵稳定性。

换道决策模型主要分为基于机器学习和基于规则的换道决策模型2类。基于机器学习的换道模型利用自然驾驶数据提取影响驾驶员换道的因素。Hou等^[2]采用随机森林和自适应增强这2种集成学习方法开发变道决策系统。Liu等^[3]基于贝叶斯参数优化的支持向量机训练决策模型。近年来出现结合深度强化学习的换道决策算法，如采用DQN（deep Q-network）^[4]，DDQN（dueling deep Q-network）^[5]，DDPG（deep deterministic policy gradient）^[6]等算法。基于机器学习的换道决策模型需要大量驾驶数据训练，且可解释性较差。基于规则的换道决策模型主要根据换道的必要性及可行性等方面进行决策。Gipps^[7]提出的换道模型中综合考虑换道空间、安全条件、周围交通参与者的行驶意图，以及是否能通过换道获得更明显的速度优势等因素。杨达等^[8]通过车联网获取各车道的车流平均速度、重型车占比和预估的理想换道时间建立成本函数。陈慧等^[9]利用累计驾驶不满意度（driving dissatisfaction， DDS）作为直道换道决策依据，当DDS累计值大于指定阈值时执行换道，但DDS计算公式只考虑到主车速度，没有考虑前车速度与加速度，可能导致换道后车辆通行效率降低。

轨迹规划包括路径规划和速度规划。根据状态空间是否解耦，轨迹规划方法分为2种。第一种是直接规划，指将路径和速度同时规划，在时空状态空间寻找自动驾驶汽车的可行轨迹。有研究者利用自动驾驶汽车横纵向位置关于时间的五次多项式规划车辆轨迹^[10?11]，也有研究者利用人工势场法规划汽车换道路径^[12?13]。直接规划的缺点是状态空间维度较高、计算复杂、实时性较差。第二种是路径与速度解耦规划方法，在二维状态空间进行二次规划，降低状态空间的维度。Li等^[14]在给定规划路径上，采用动态规划方法搜索启发路径点，利用贝塞尔多项式优化启发路径点连接曲线，得到规划时域内的速度曲线。汪海松等^[15]通过离散换道终点区域，以B样条曲线连接换道路径起点和终点，根据目标函数选取最优B样条曲线。Meng等^[16]采用離散搜索方法获取路径与速度规划粗解，分别采用非线性优化工具CasADi与二次规划优化获取平滑路径与速度曲线，但离散搜索方法计算量较大。

目前关于弯道换道的研究文献很少。针对基于DDS的直道换道决策算法没有考虑前车的未来车况，可能引起主车换道后行驶效率降低的问题，考虑主车期望车速，以及预测时域内前车的速度和加速度，提出新的驾驶不满意度计算模型作为换道决策算法，并推导新的换道安全距离模型，以提高换道的安全性和行驶效率。在轨迹规划方面，针对直接规划法和离散搜索计算量过大问题，采用路径-速度解耦框架以提高计算效率。利用基于路径五次多项式曲线及路径评价的优化方法，生成最优换道路径。在此路径规划基础上，基于位移-时间图像（displacement-time graph， ST图）进行动态规划获取速度规划曲线粗解，再采用二次规划优化速度曲线，实现安全稳定的弯道换道过程。

1高速公路弯道换道决策算法

1.1弯道换道场景

虚拟场景是搭建自动驾驶汽车虚拟仿真平台的基础。假设感知信息已知，高速公路场景的道路元素简化为同向双车道，弯道最小曲率半径R_min=650 m，车道宽度为3.75 m，高速公路场景如图1所示。以车道分界线上一点为大地坐标系原点，内侧车道中心线、外侧车道中心线、车道分界线的曲线方程为

高速公路动态要素包含自动驾驶主车和周围其他车辆。图2为复杂换道场景，主车EV所在车道为当前车道，CL为前车，TL、TF分别为邻车道前后车。假设干扰车沿着自身所在车道行驶，主车根据自身位置与周围干扰车的运动姿态判断是否换道。干扰车CL、TL、TF由Prescan软件提供的路径跟踪模型控制，沿所在车道以一定初始速度做匀变速运动。主车与干扰车车速范围为60～120 km/h。

1.2换道决策模型

自动驾驶汽车换道分为4个过程：换道意图的产生、换道可行性判断、换道轨迹规划、换道轨迹跟踪。本节主要讨论前2个过程。

1.2.1 换道意图的产生

在高速公路行驶的车辆主要有换道与跟车2种行为。若前方有其他车辆，驾驶人通常保持跟车行驶，若前车车速过低，驾驶人的期望车速无法实现，则会对当前车速产生不满。驾驶不满意度是衡量对速度期望不满程度的量化指标^[17]。驾驶不满意度累计值增加到一定值后，驾驶员会选择通行效率更高的车道换道行驶。若仅考虑前车车速，忽略加速度，可能导致换道无法达成预期目的。如邻车道前车TL的车速大于当前车道前车CL，但TL减速行驶，而CL匀速或加速行驶，主车保持当前车道跟车行驶将能获得更高的行驶速度。为了获得更高的通行效率，文中提出基于主车的期望车速与前车车速差值，并考虑前车的加速度，分别计算预测时域内主车相对于邻车道前车TL和当前车道前车CL的驾驶不满意度累计值，据此判断是否需要换道。前车的DDS累计值计算公式为

1.2.3 换道安全距离模型

换道意图产生后，主车与周围车辆的距离符合安全要求才能进行换道。假设前车以较大的减速度减速行驶，后车经过反应时间采取制动减速度以避免碰撞，为了确保前后车在减速到车速相等前不发生碰撞，前后车需保持一定安全距离。驾驶员在危险场景下的制动减速度一般在0.3～0.4 g^[18]，文中假定前车最大制动减速度=0.4 g，后车最大制动减速度=0.35 g。假设前车以制动减速度刹车，后车经过反应时间，以制动减速度减速，前后两车不发生碰撞所需要的最小距离定义为安全距离。

由于前后车初始车速与加速度不同，可能存在3种情况：①前车刹停时，后车仍前向运动；②后车刹停时，前车仍前向运动；③前车刹停前，前后车已减速至相同车速。根据两车同时减速至所需时间计算为

为防止主车越过车道分界线前，因主车的换道意图发生变化或者换道安全距离不满足，出现取消换道并返回原车道情况，基于主车匀速换道的假设，计算主车在越过车道分界线前和当前车道前车CL需要保持的初始安全距离，如式（11）所示^[19]。在换道过程中，从换道初始时刻到主车越过车道分界线所需的时间为，取=2 s。

1.2.4 主车换道决策流程

若当前车道前车CL的DDS累计值大于相邻车道前车TL的DDS累计值，主车产生换道意图。当主车和当前车道前车CL的距离大于初始安全距离，主车与目标车道前车TL和后车TF的距离满足安全距离，此时换道可行性条件成立，主车为了获得更高行驶效率而执行换道操作。主车换道决策流程过程如图4所示。

2高速公路弯道换道规划控制算法

2.1主车路径规划

针对离散搜索方法计算量较大问题，基于五次多项式换道路径曲線，考虑安全、舒适和换道效率等因素，结合换道终点约束、换道路径曲率约束以及换道安全距离约束，通过序列二次优化（sequential quadratic programming， SQP）方法求解获得主车的最优换道路径。

2.1.1 换道路径选择

由于车辆转向限制和弯道特殊性，主车换道路径应满足以下要求：换道路径的曲率平滑连续；换道路径的起始点和终点的曲率、航向角需要与主车换道路径起始点和换道终点的曲率、航向角保持一致。文中进行了弯道局部路径规划仿真，以主车初始位置为换道起始点（坐标[290.09，66.01]，航向角26.34°，初始曲率1/653.75 m^-1），邻车道中心线为换道终点（坐标[452.63，185.37]，航向角为44.13°，终点曲率1/650 m^-1），比较三次、五次、七次多项式曲线和B样条曲线换道路径曲率和航向角区别，结果如图5所示。

由图5可知：4种方式生成的换道路径曲率均平滑变化，但只有五次和七次多项式曲线的起点和终点曲率满足要求；4种方式生成的换道路径航向角均平滑变化，但三次多项式曲线的起始点和终点航向角均不满足要求。综上所述，只有五次和七次多项式曲线能满足换道路径要求，因七次多项式未知变量较多，求解难度较大，故文中选取五次多项式曲线作为主车的换道路径，为

2.1.2 换道路径评价指标

根据阿克曼转角模型^[20]，主车跟随路径需满足曲率连续以及曲率极限值约束两大条件。文中以换道路径点的平均曲率以及平均曲率变化率衡量换道路径的平缓程度。换道效率也是评价换道路径的一个重要指标，文中以换道路径总长度和换道路径与目标车道中心线的平均横向偏差l衡量换道效率。为了衡量换道路径的性能，需要将换道路径离散化，以路径弧长为离散长度，对应换道路径横纵坐标为

2.1.4 基于非线性模型的路径规划

主车换道路径的4个评价指标均为关于换道终点横坐标X_f的非线性函数，故采用非线性优化方法中的序列二次规划算法求解主车弯道换道的路径规划问题。考虑主车换道路径的平滑性和高效性，通过层次分析法为4个换道路径评价指标赋权重。以主车换道终点横坐标X_f为决策变量，以上述4个换道路径评价指标的线性加权和建立优化目标函数，为

2.2主车速度规划

利用ST图进行主车速度规划是一种比较方便的方式。将规划时间内占用规划路径的动态干扰车路径映射到ST图上，此时ST图上主车的可行区域可能是非凸的，采用动态规划结合二次规划优化生成光滑的主车速度曲线。

2.2.1 主车规划速度曲线搜索

基于动态规划算法在ST图上寻找一条无碰撞的主车速度曲线。算法的效率取决于采样时间和采样路径弧长，过大的和影响换道轨迹的平滑性，而精细的采样时间会影响算法的运行效率。文中取采样时间，采样路径弧长，规划时长T=8 s。路径离散点的弧长S关于时间的导数可通过有限差分近似得到，为

2.2.2 主车规划速度曲线优化

1）目标函数。由动态规划得到的主车速度曲线由多个线段组成，不满足主车的运动学约束、动力学约束以及平滑性约束，主车无法稳定跟踪这样的车速曲线，采用优化方法平滑动态规划得到的粗糙车速曲线。为简化计算，将时间戳固定，优化每个时间戳上的路径点，即以每个时间戳所对应的路径点弧长、速度和加速度为决策变量，将车速曲线优化问题转化为二次规划问题。

2）约束函数。在二次规划过程中，需要对每一个时间戳主车的行驶路径弧长S_i，以及速度加速度和冲击度做约束，以满足主车的运动学和动力学约束，并符合道路交通规则。如式（28）所示。

2.3主车轨迹跟踪模型

经决策规划得到的主车换道轨迹需发送到控制模块执行跟踪。在高速上行驶时，主车轮胎会产生较大的形变，运动学模型并不适用于主车控制，故主车采用三自由度车辆动力学模型。模型预测控制（model predictive control， MPC）算法相较于线性二次调节器（linear quadratic regulator， LQR）、PID等算法，可以在线更新减小误差，很好地处理多约束问题，具有较强的鲁棒性。由于路径点的主车速度已由轨迹规划得到，PID算法能很好地完成速度跟踪。故采用MPC横向跟踪^[23]和双PID纵向速度跟踪，MPC算法输出主车控制转角δ，双PID算法输出主车纵向加速度a。主车规划及控制流程见图8所示。

3仿真验证及分析

通过Prescan、Matlab/Simulink和Carsim搭建自动驾驶汽车联合仿真平台，用于验证主车决策及规划算法的有效性。利用Prescan搭建道路模型以及定义干扰车的动态行为，如图9所示。Carsim提供主车的姿态信息，并接收控制信号。在Matlab/Simulink中构建主车的换道决策、规划和控制算法模型，决策算法中预测时域T_pre=4 s，采样时间=0.1 s。仿真测试用例主车和干扰车的初始参数如表1所示。分别为邻车道前车TL和后车TF的初始车速，分别为当前车道前车CL、邻车道前车TL和后车TF与主车的初始距离，负值表示干扰车在主车后方，和分别为邻车道前车TL和后车TF的加速度。

在测试用例case1中，主车在当前车道跟随前车CL，前车CL减速行驶，而邻车道前车TL加速行驶。如图10（a）所示，当t=0 s时，根据主车的期望车速=120 km/h计算DDS累计值，当前车道前车CL的DDS累计值大于邻车道前车TL的DDS累计值，此时换道意图产生，换道意图信号置1，且如图10（b）所示，主车和3辆干扰车的相对距离均满足各自安全距离要求，故此时换道成立信号置1，主车执行换道。在t=1 s时，主车与前车TL的距离不满足安全距离，换道成立信号置0，主车取消换道，但换道意图信号始终保持，等待合适的换道时机。在t=1.7 s主车与其他干扰车的距离满足安全距离条件，换道成立信号再次置1，主车执行换道。t=3.5 s时主车与前车CL的距离不满足安全距离，但此时主车已越过车道分界线，故主车继续执行换道。规划控制方面，由图10（c）～（d）可知，主车能通过路径-速度解耦方法获得平滑的无碰撞换道轨迹，但动态规划与二次规划结果相差较大，原因在于动态规划所获得的规划速度曲线加速度较大，而为保证舒适性，二次规划的加速度约束权重较大，限制了其加速度大小，导致动态规划与二次规划结果相差较大。由图10（e）～（g）可知，主车横纵向控制误差较小，轨迹跟踪效果较好。由图10（h）知，主车的横向加速度和纵向加速度变化较平缓。由图10（i）～（j）知，主车的前轮转角、质心侧偏角和横摆角速度变化平缓，质心侧偏角的最大值小于0.003 5 rad，表明汽车操纵稳定性好。

在测试用例case2中，虽然邻车道前车TL的初始车速比当前车道前车CL大，但前车TL减速行驶，而前车CL加速行驶。如图11所示，在预测时域T_pre内，前车TL的DDS累计度大于前车CL的DDS累计度，所以换道意图置0，换道成立信号置0，主车继续当前车道行驶，避免了无效换道。

为进一步验证换道平滑性与稳定性，在case3中设置主车换道减速工况，如图12所示。由图12（a）～（d）可知，主车减速换道，主车横纵加速度变化较平缓，最大值小于3 m/s²，主车最大横摆角速度为0.1 rad/s，最大质心侧偏角为0.001 3 rad，主车安全性和操纵稳定性好。

通过上述的仿真验证，文中所提出的基于驾驶不满意度累计值的换道意图产生模型和换道安全距离模型的换道决策算法，能为自动驾驶汽车选择更高效的行驶方式。case1和case3中主车选择换道，case2中选择跟车，换道过程中时刻检测主车和干扰车的相对距离是否满足安全距离，保证主车换道过程的安全。同时，所提出的运动规划算法也能保证主车换道过程速度曲线的平滑性和操纵稳定性。

4结束语

文章提出的基于驾驶不满意度的换道决策算法，考虑了主车期望速度，预测时域内前车的速度及加速度，减少因邻车道干扰车减速造成主车无效换道的情况，提高了通行效率。路径规划基于五次多项式，结合提出的4个换道路径评价指标构建目标函数，由序列二次规划算法求解获取最优路径。速度规划基于ST图，结合动态规划与二次规划算法，获取平滑的速度规划曲线。仿真表明，基于驾驶不满意度的换道决策算法能提高自动驾驶汽车在弯道工况的安全性和通行效率，轨迹规划算法能确保换道的操纵稳定性。主车换道case1工况下，主车最大横摆角速度为0.081 rad/s，最大質心侧偏角为0.003 2 rad；主车换道case3工况下，主车最大横摆角速度为0.1 rad/s，最大质心侧偏角为0.001 3 rad。文中决策过程中对于前车的车速预测较为简单，未来将研究更为准确的前车车速预测算法，并对不同曲率半径的弯道场景进行轨迹规划算法验证。

參考文献

［1］  李克强.构建中国标准智能汽车体系 实现汽车强国伟大目标[J]. 智能网联汽车， 2020（2）： 8-9.

Li K Q.Building a Chinese standard intelligent vehicle system to achieve the great goal of becoming a powerful country in automobiles [J] Intelligent Connected Vehicles， 2020 （2）： 8-9. （in Chinese）

［2］  Hou Y， Edara P， Sun C.Situation assessment and decision making for lane change assistance using ensemble learning methods[J]. Expert Systems with Applications， 2015， 42（8）： 3875-3882.

［3］  Liu Y， Wang X， Li L， et al.A novel lane change decision-making model of autonomous vehicle based on support vector machine[J]. IEEE Access， 2019： 26543-26550.

［4］  Wang J J， Zhang Q C， Zhao D B， et al.Lane change decision-making through deep reinforcement learning with rule-based constraints[C]//2019 International Joint Conference on Neural Networks （IJCNN）. Budapest， Hungary： IEEE， 2019： 1-6.

［5］  Liao J， Liu T， Tang X， et al.Decision-making strategy on highway for autonomous vehicles using deep reinforcement learning[J]. IEEE Access， 2020， 8： 177804-177814.

［6］  Fu Y， Li C， Yu F R， et al.A decision-making strategy for vehicle autonomous braking in emergency via deep reinforcement learning[J]. IEEE Transactions on Vehicular Technology， 2020， 69（6）： 5876-5888.

［7］  Gipps P G.A model for the structure of lane-changing decision[J]. Transportation Research Part B： Methodological， 1986， 20（5）： 403-414.

［8］  杨达， 吕蒙， 戴力源， 等.车联网环境下自动驾驶车辆车道选择决策模型[J]. 中国公路学报， 2022， 35（4）： 243-255.

Yang D， Lv M， Yuan L Y， et al.Decision-making model for lane selection of automated vehicles in connected vehicle environment[J]. China Journal of Highway and Transport， 2022， 35（4）： 243-255. （in Chinese）

［9］  陈慧， 王洁新.基于驾驶人不满度的高速公路自动驾驶换道决策[J]. 中国公路学报， 2019， 32（12）： 1-9， 45.

Chen H， Wang J X.A decision-making method for lane changes of automated vehicles on freeways based on drivers dissatisfaction[J]. China Journal of Highway and Transport， 2019， 32（12）： 1-9， 45.（in Chinese）

［10］  Wei C， Li S.Planning a continuous vehicle trajectory for an automated lane change maneuver by nonlinear programming considering car-following rule and curved roads[J]. Journal of Advanced Transportation， 2020， 2020： 8867447.

［11］  吴树凡， 魏民祥， 滕德成， 等.智能车辆弯道换道路径规划算法与跟踪控制[J]. 重庆理工大学学报（自然科学）， 2021， 35（3）： 18-28.

Wu S F， Wei M X， Teng D C， et al.Path planning and control of curve lane changing for autonomous vehicle[J]. Journal of Chongqing University of Technology（Natural Science）， 2021， 35（3）： 18-28. （in Chinese）

［12］  Xie S， Hu J， Bhowmick P， et al.Distributed motion planning for safe autonomous vehicle overtaking via artificial potential field[J]. IEEE Transactions on Intelligent Transportation Systems， 2022， 23（11）： 21531-21547.

［13］  Rasekhipour Y， Khajepour A， Chen S K， et al.A potential field-based model predictive path-planning controller for autonomous road vehicles[J]. IEEE Transactions on Intelligent Transportation Systems， 2017， 18（5）： 1255-1267.

［14］  Li J L， Xie X J， Lin Q， et al.Motion planning by search in derivative space and convex optimization with enlarged solution space[C]//2022 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Kyoto， Japan： IEEE， 2022： 13500-13507.

［15］  汪海松， 胡明辉， 黎万洪， 等.建立安全换道域的换道决策与规划[J/OL]. 重庆大学学报， 2022： 1-18.[2022-02-23].http：//kns.cnki.net/kcms/detail/50.1044.N.20220516.0857.002.html.

Wang H S， Hu M H， Li W H， et al.Lane-changing decision and planning with establishing safe lane-changing domain[J/OL]. Journal of Chongqing University， 2022： 1-18. [2022-02-23].http：//kns.cnki.net/kcms/detail/50.1044.N.20220516.0857.002.html.（in Chinese）

［16］  Meng Y， Wu Y， Gu Q， et al.A decoupled trajectory planning framework based on the integration of lattice searching and convex optimization[J]. IEEE Access， 2019， 7： 130530-130551.

［17］  王政.智能車辆自主换道方法的研究[D]. 长春： 吉林大学， 2016.

Wang Z. Research on autonomous lane changing method of intelligent vehicle[D]. Changchun： Jilin University， 2016. （in Chinese）

［18］  Hs D.Development of an FCW algorithm evaluation methodology with evaluation of three alert algorithms[R]. National Highway Traffic Safety Administration， 2009.

［19］  李嘉嘉.自动驾驶车辆自主变道决策与规划问题研究[D]. 长春： 吉林大学， 2022.

Li J J.Research on decision-making and planning for active lane change of autonomous vehicles[D]. Changchun： Jilin University， 2022. （in Chinese）

［20］  曾德全， 余卓平， 熊璐， 等.结构化道路下基于层次分析法的智能车避障轨迹规划[J]. 华南理工大学学报（自然科学版）， 2020， 48（7）： 65-75.

Zeng D Q， Yu Z P， Xiong L， et al. Intelligent vehicle obstacle avoidance trajectory planning in structured road based on analytic hierarchy process[J]. Journal of South China University of Technology （Natural Science Edition）， 2020， 48（7）： 65-75. （in Chinese）

［21］  Wei C， Wang Y， Asakura Y， et al.A nonlinear programing model for collision-free lane-change trajectory planning based on vehicle-to-vehicle communication[J]. Journal of Transportation Safety & Security， 2019（4）： 1-21.

［22］  Bagdadi O.Assessing safety critical braking events in naturalistic driving studies[J]. Transportation Research Part F： Traffic Psychology and Behaviour， 2013， 16： 117-126.

［23］  袁康.基于模型预测控制的自动驾驶汽车运动规划与控制研究[D]. 重庆： 重庆大学， 2019.

Yuan K.Study on motion planning and control of autonomous vehicles based on model predictive control[D]. Chongqing： Chongqing University， 2019. （in Chinese）

（编辑  詹燕平）