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

袁昌，莫天石，舒红; YUAN Chang; MO Tianshi; SHU Hong

网刊加载中。。。

使用Chrome浏览器效果最佳，继续浏览，你可能不会看到最佳的展示效果，

确定继续浏览么?

复制成功，请在其他浏览器进行阅读

摘要

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

关键词

自动驾驶汽车; 运动规划; 决策; 驾驶不满意度; 优化

自动驾驶技术对于减少交通事故、改善交通拥堵状况等具有重要作用，已成为汽车行业发展的战略方向和竞争焦点^[

1]。高速公路车速较高，弯道路段换道比直道换道更危险，若处理不当将发生车道偏离、侧滑或碰撞等事故。自动驾驶汽车弯道换道决策及规划算法决定换道过程的安全与操纵稳定性。

换道决策模型主要分为基于机器学习和基于规则的换道决策模型2类。基于机器学习的换道模型利用自然驾驶数据提取影响驾驶员换道的因素。Hou等^[

2]采用随机森林和自适应增强这2种集成学习方法开发变道决策系统。Liu等^{[参考文献 3

百度学术}3]基于贝叶斯参数优化的支持向量机训练决策模型。近年来出现结合深度强化学习的换道决策算法，如采用DQN（deep Q-network）^{[参考文献 4

百度学术}4]，DDQN（dueling deep Q-network）^{[参考文献 5

百度学术}5]，DDPG（deep deterministic policy gradient）^{[参考文献 6

百度学术}6]等算法。基于机器学习的换道决策模型需要大量驾驶数据训练，且可解释性较差。基于规则的换道决策模型主要根据换道的必要性及可行性等方面进行决策。Gipps^{[参考文献 7

百度学术}7]提出的换道模型中综合考虑换道空间、安全条件、周围交通参与者的行驶意图，以及是否能通过换道获得更明显的速度优势等因素。杨达等^{[参考文献 8

百度学术}8]通过车联网获取各车道的车流平均速度、重型车占比和预估的理想换道时间建立成本函数。陈慧等^{[参考文献 9

百度学术}9]利用累计驾驶不满意度（driving dissatisfaction, DDS）作为直道换道决策依据，当DDS累计值大于指定阈值时执行换道，但DDS计算公式只考虑到主车速度，没有考虑前车速度与加速度，可能导致换道后车辆通行效率降低。

轨迹规划包括路径规划和速度规划。根据状态空间是否解耦，轨迹规划方法分为2种。第一种是直接规划，指将路径和速度同时规划，在时空状态空间寻找自动驾驶汽车的可行轨迹。有研究者利用自动驾驶汽车横纵向位置关于时间的五次多项式规划车辆轨迹^[

10⁃11]，也有研究者利用人工势场法规划汽车换道路径^{[参考文献 12⁃13}12⁃13]。直接规划的缺点是状态空间维度较高、计算复杂、实时性较差。第二种是路径与速度解耦规划方法，在二维状态空间进行二次规划，降低状态空间的维度。Li等^{[参考文献 14

百度学术}14]在给定规划路径上，采用动态规划方法搜索启发路径点，利用贝塞尔多项式优化启发路径点连接曲线，得到规划时域内的速度曲线。汪海松等^{[参考文献 15

百度学术}15]通过离散换道终点区域，以B样条曲线连接换道路径起点和终点，根据目标函数选取最优B样条曲线。Meng等^{[参考文献 16

百度学术}16]采用离散搜索方法获取路径与速度规划粗解，分别采用非线性优化工具CasADi与二次规划优化获取平滑路径与速度曲线，但离散搜索方法计算量较大。

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

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

1.1 弯道换道场景

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

\{\begin{array}{l} Y_{i n} = f_{i n} (X_{i n}) = 651.875 - \sqrt[]{650^{2} - X_{i n}^{2}} ， \\ Y_{o u t} = f_{o u t} (X_{o u t}) = 651.875 - \sqrt[]{653 . 75^{2} - X_{o u t}^{2}} ， \\ Y_{b o u n d} = f_{b o u n d} (X_{b o u n d}) = 651.875 - \sqrt[]{651 . 875^{2} - X_{b o u n d}^{2}} ， \end{array}

（1）

式中：(X_in, Y_in)、(X_out, Y_out)、(X_bound, Y_bound)分别表示内侧车道中心线、外侧车道中心线和车道分界线的横纵坐标。

图1 高速公路弯道场景

Fig. 1 Highway curve scenario

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

图2 高速公路弯道换道场景

Fig. 2 Lane change scenario on highway curve

1.2 换道决策模型

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

1.2.1 换道意图的产生

在高速公路行驶的车辆主要有换道与跟车2种行为。若前方有其他车辆，驾驶人通常保持跟车行驶，若前车车速过低，驾驶人的期望车速无法实现，则会对当前车速产生不满。驾驶不满意度是衡量对速度期望不满程度的量化指标^[

17]。驾驶不满意度累计值增加到一定值后，驾驶员会选择通行效率更高的车道换道行驶。若仅考虑前车车速，忽略加速度，可能导致换道无法达成预期目的。如邻车道前车TL的车速大于当前车道前车CL，但TL减速行驶，而CL匀速或加速行驶，主车保持当前车道跟车行驶将能获得更高的行驶速度。为了获得更高的通行效率，文中提出基于主车的期望车速与前车车速差值，并考虑前车的加速度，分别计算预测时域内主车相对于邻车道前车TL和当前车道前车CL的驾驶不满意度累计值，据此判断是否需要换道。前车的DDS累计值计算公式为

\{\begin{array}{l} D_{s u m, l a n e} = \sum_{i = 0}^{T_{p r e} / Δ t} |\frac{v_{d e s} - v_{l a n e, L} (i)}{v_{d e s}} Δ t| ， \\ v_{l a n e, L} (i) = v_{l a n e, L 0} + a_{l a n e, L} t_{i} ， \end{array}

（2）

式中：D_sum_,_lane为lane车道前车在预测时域T_pre内的DDS累计值；lane为车道序号（0为当前车道，1为邻车道）； $Δ t$ 为采样周期； $ν_{d e s}$ 为主车期望车速； $ν_{l a n e, L} (i)$ 为lane车道前车在第 $i$ 时刻车速； $ν_{l a n e, L 0}$ 和 $a_{l a n e, L}$ 分别为lane车道前车初始车速和加速度； $t_{i} = i Δ t$ ，为第i个采样时间对应的时刻。

文中按当前车道与邻车道前车的DDS累计值确定换道意图。若当前车道前车CL的DDS累计值大于相邻车道前车TL的DDS累计值，系统会产生换道意图，即：

D_{s u m, 0} > D_{s u m, 1}

。

（3）

若在换道过程中 $D_{s u m, 0} \leq D_{s u m, 1}$ ，换道意图取消，主车继续在当前车道跟车行驶。

1.2.3 换道安全距离模型

换道意图产生后，主车与周围车辆的距离符合安全要求才能进行换道。假设前车以较大的减速度 $a_{L}$ 减速行驶，后车经过反应时间 $t_{τ}$ 采取制动减速度 $a_{F}$ 以避免碰撞，为了确保前后车在减速到车速相等前不发生碰撞，前后车需保持一定安全距离 $d_{s}$ 。驾驶员在危险场景下的制动减速度一般在0.3~0.4 g^[

18]，文中假定前车最大制动减速度

a_{L m a x}

=0.4 g，后车最大制动减速度

a_{F m a x}

=0.35 g。假设前车以制动减速度

a_{L m a x}

刹车，后车经过反应时间

t_{τ}

，以制动减速度

a_{F m a x}

减速，前后两车不发生碰撞所需要的最小距离定义为安全距离

d_{s}

。

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

(v_{L} - v_{s}) / a_{L m a x} = (v_{F} - v_{s}) / a_{F m a x} + t_{τ}

，

（4）

v_{s} = \frac{t_{τ} a_{L m a x} a_{F m a x} + v_{F} a_{L m a x} - v_{L} a_{F m a x}}{a_{L m a x} - a_{F m a x}}

，

（5）

式中： $ν_{L}$ 为前车初始车速； $ν_{F}$ 为后车初始车速。

由于 $a_{L m a x} > a_{F m a x}$ ，若 $ν_{s} \geq 0$ 存在，需满足

\{\begin{array}{l} t_{τ} a_{L m a x} a_{F m a x} + v_{F} a_{L m a x} - v_{L} a_{F m a x} \geq 0, \\ (t_{τ} a_{L m a x} a_{F m a x} + v_{F} a_{L m a x} - v_{L} a_{F m a x}) / (a_{L m a x} - a_{F m a x}) < v_{L}, \\ (t_{τ} a_{L m a x} a_{F m a x} + v_{F} a_{L m a x} - v_{L} a_{F m a x}) / (a_{L m a x} - a_{F m a x}) < v_{F} 。 \end{array}

（6）

化简整合得：

t_{τ} + \frac{v_{F}}{a_{L m a x}} ＜ \frac{v_{L}}{a_{L m a x}} \leq t_{τ} + \frac{v_{F}}{a_{F m a x}} ＜ \frac{v_{L}}{a_{F m a x}}

。

（7）

当满足式(7)时，情况③成立，安全车距 $d_{s}$ 为

d_{s} = v_{F} t_{τ} + \frac{v_{F}^{2} - {v_{s}}^{2}}{2 a_{F m a x}} - \frac{v_{L}^{2} - {v_{s}}^{2}}{2 a_{L m a x}} + d_{e}

，

（8）

式中， $d_{e}$ 表示安全距离余量，一般取2~5 m，文中取 $d_{e}$ =5 m。

当式(7)不成立时， $ν_{s}$ 不存在，两车无法减速到相同车速，即情况①与情况②。对于情况①，前车刹停时间小于后车，安全车距为

d_{s} = v_{F} t_{τ} + \frac{v_{F}^{2}}{2 a_{F m a x}} - \frac{v_{L}^{2}}{2 a_{L m a x}} + d_{e}

，

t_{τ} + \frac{v_{F}}{a_{F m a x}} \geq \frac{v_{L}}{a_{F m a x}}

或

t_{τ} + \frac{v_{F}}{a_{L m a x}} \geq \frac{v_{L}}{a_{L m a x}}

。

（9）

对于情况②，后车刹停时间小于后车，后车已经刹停，前车还在运动，安全车距保持跟车距离 $d_{e}$ 即可，为

d_{s} = d_{e}

，

t_{τ} + \frac{v_{F}}{a_{F m a x}} ＜ \frac{v_{L}}{a_{L m a x}}

。

（10）

为防止主车越过车道分界线前，因主车的换道意图发生变化或者换道安全距离不满足，出现取消换道并返回原车道情况，基于主车匀速换道的假设，计算主车在越过车道分界线前和当前车道前车CL需要保持的初始安全距离 $d_{01_i n i t}$ ，如式(11)所示^[

19]。在换道过程中，从换道初始时刻到主车越过车道分界线所需的时间为

t_{c}

，取

t_{c}

=2 s。

\{\begin{array}{l} d_{0} = v_{E V, i n i t} t_{c} ， \\ d_{1} = v_{C L, i n i t} t_{c} + 0.5 a_{C L} {t_{c}}^{2} ， \\ d_{01_i n i t} = d_{0} + d_{s} - d_{1} ， \end{array}

（11）

式中： $d_{0}$ 和 $d_{1}$ 分别为主车和当前车道前车CL在 $t_{c}$ 时间内行驶的路程； $ν_{E V, i n i t}$ 为主车的初始车速； $ν_{C L, i n i t}$ 为当前车道前车CL的初始速度； $a_{C L}$ 为当前车道前车CL的加速度。换道初始安全距离如图3所示，横坐标 $S_{1}$ 为沿车道纵向的路程，纵坐标 $l_{1}$ 为沿车道的横向距离。

图3 换道初始安全距离示意图

Fig. 3 Diagram of the initial safety distance of lane change

1.2.4 主车换道决策流程

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

图4 主车换道决策流程

Fig. 4 Lane change decision process of the ego-vehicle

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种换道路径曲线对比图

Fig. 5 Comparison of 4 lane changing paths

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

Y = f (X) = e_{5} X^{5} + e_{4} X^{4} + e_{3} X^{3} + e_{2} X^{2} + e_{1} X + e_{0}

。

（12）

主车换道初始坐标 $(X_{0}$ , $Y_{0})$ 已知，换道终点坐标 $(X_{f}, Y_{f})$ 导数信息( $Y_{0}^{'}, Y_{0}^{″}, Y_{f}^{'}, Y_{f}^{″}$ )为

\{\begin{array}{l} Y_{0}^{'} = f^{'} (X_{0}), Y_{0}^{″} = f^{″} (X_{0}) ， \\ Y_{f} = f (X_{f}), Y_{f}^{'} = f^{'} (X_{f}), Y_{f}^{″} = f^{″} (X_{f}) 。 \end{array}

（13）

结合主车换道起始与终点信息可求解多项式系数 $e_{0}$ 、 $e_{1}$ 、 $e_{2}$ 、 $e_{3}$ 、 $e_{4} 和 e_{5}$ ，见式(14)，只有 $X_{f}$ 一个未知量。

[\begin{matrix} e_{0} \\ e_{1} \\ e_{2} \\ \begin{matrix} e_{3} \\ \begin{matrix} e_{4} \\ e_{5} \end{matrix} \end{matrix} \end{matrix}] = {[\begin{matrix} \begin{matrix} \begin{matrix} 1 \\ 0 \end{matrix} \\ 0 \end{matrix} & \begin{matrix} \begin{matrix} X_{0} \\ 1 \end{matrix} \\ 0 \end{matrix} & \begin{matrix} \begin{matrix} X_{0}^{2} \\ 2 X_{0} \end{matrix} \\ 2 \end{matrix} & \begin{matrix} \begin{matrix} \begin{matrix} X_{0}^{3} & X_{0}^{4} & X_{0}^{5} \end{matrix} \\ \begin{matrix} 3 X_{0}^{2} & 4 X_{0}^{3} & 5 X_{0}^{4} \end{matrix} \end{matrix} \\ \begin{matrix} 6 X_{0} & 12 X_{0}^{2} & 20 X_{0}^{3} \end{matrix} \end{matrix} \\ 1 & X_{f} & X_{f}^{2} & \begin{matrix} X_{f}^{2} & X_{f}^{4} & X_{f}^{5} \end{matrix} \\ 0 & 1 & 2 X_{f} & \begin{matrix} 3 X_{f}^{2} & 4 X_{f}^{3} & 5 X_{f}^{4} \end{matrix} \\ 0 & 0 & 2 & \begin{matrix} 6 X_{f} & 12 X_{f}^{2} & 20 X_{f}^{3} \end{matrix} \end{matrix}]}^{- 1} [\begin{matrix} \begin{matrix} Y_{0} \\ {Y_{0}}^{'} \\ Y_{0}^{″} \end{matrix} \\ Y_{f} \\ {Y_{f}}^{'} \\ Y_{f}^{″} \end{matrix}]

。

（14）

2.1.2 换道路径评价指标

根据阿克曼转角模型^[

20]，主车跟随路径需满足曲率连续以及曲率极限值约束两大条件。文中以换道路径点的平均曲率

k

以及平均曲率变化率

k^{'}

衡量换道路径的平缓程度。换道效率也是评价换道路径的一个重要指标，文中以换道路径总长度

S_{s u m}

和换道路径与目标车道中心线的平均横向偏差l衡量换道效率。为了衡量换道路径的性能，需要将换道路径离散化，以路径弧长

∆ S

为离散长度，对应换道路径横纵坐标为

\{\begin{array}{l} X_{i} = X_{i - 1} + Δ S \cdot c o s (θ_{i - 1}) ， \\ Y_{i} = f (X_{i}) ， \\ t a n θ_{i - 1} = f^{'} (X_{i - 1}) ， \end{array}

（15）

式中： $X_{i}$ 和 $Y_{i}$ 分别为换道路径第i点的横坐标和纵坐标； $θ_{i - 1}$ 为换道路径第i-1点的航向角，i=1,2,…,n，n为换道路径离散点的数量。

1）换道路径点平均曲率

k = \frac{1}{n + 1} \sum_{i = 0}^{n} k_{i}

，

（16）

式中，k_i为换道路径第i点的曲率。

2）换道路径点平均曲率变化率

k^{'} = (\sum_{i = 0}^{n - 1} |\frac{k_{i + 1} - k_{i}}{Δ S}|) / n

。

（17）

3）换道路径总长度

S_{s u m} = \sum_{i = 0}^{n - 1} \sqrt[]{{(X_{i + 1} - X_{i})}^{2} + {(Y_{i + 1} - Y_{i})}^{2}}

。

（18）

4）换道路径与目标车道中心线的平均横向偏差

l = \frac{1}{n} \sum_{i = 0}^{n - 1} \sqrt[]{{(X_{i} - X_{r i})}^{2} + {(Y_{i} - Y_{r i})}^{2}}

，

（19）

式中， $(X_{r i}, Y_{r i})$ 为换道路径第i点与目标车道中心线的对应点坐标，如图6所示。

图6 换道路径与目标车道中心线的匹配点

Fig. 6 Matching point of lane change path on target lane centerline

2.1.3 换道路径约束

1）主车换道终点约束。换道路径规划中，可依据换道初始时刻主车EV与相邻车道前车TL的车速 $ν_{E V, i n i t}$ 、 $ν_{T L, i n i t}$ 确定换道终点横坐标 $X_{f}$ 范围^[

21]

[X_{f, m i n}, X_{f, m a x}]

为

\begin{array}{l} X_{f, m i n} = 3 \cdot m i n (v_{E V, i n i t}, v_{T L, i n i t}) ， \\ X_{f, m a x} = 6 \cdot m a x (v_{E V, i n i t}, v_{T L, i n i t}) 。 \end{array}

（20）

2）为保证换道舒适性，换道路径的最小曲率 $k_{m i n}$ 为

k_{m i n} = a_{y, m a x} / v_{m a x}^{2}

，

（21）

式中： $a_{y, m a x}$ 为主车最大侧向加速度，取0.3 g； $ν_{m a x}$ 取120 km/h。

3）为保证主车在进入邻车道前，主车与当前车道前车CL保持安全距离，在越过车道分界线的位置 $X_{1}$ 限制为

X_{1} \leq X_{C L 1}

，

（22）

式中： $X_{C L 1}$ 为主车越过车道分界线时，距离当前车道前车CL一个安全距离d_s处的横坐标；X₁为换道路径和车道分界线的交点横坐标，由f (X)=f_bound(X)求解得到，如图7所示。

图7 换道车辆越过车道分界线示意图

Fig. 7 Diagram of lane change vehicle crossing the lane line

此外，主车换道路径应限制在2条车道中心线之间，为

f_{o u t} (X) \leq f (X) \leq f_{i n} (X)

。

（23）

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

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

m i n J_{n o n} = w_{C 1} k + w_{C 2} k^{'} + w_{C 3} S_{s u m} + w_{C 4} l

，

（24）

式中： $w_{C 1}$ 为换道路径点平均曲率k权重，取0.498 5； $w_{C 2}$ 为换道路径点平均曲率变化率 $k^{'}$ 权重，取0.226 5； $w_{C 3}$ 为换道路径总长度 $S_{s u m}$ 权重，取0.151 3； $w_{C 4}$ 为换道路径与目标车道中心线的平均横向偏差l权重，取0.123 7。

2.2 主车速度规划

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

2.2.1 主车规划速度曲线搜索

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

\begin{array}{l} v_{i} \approx (S_{i} - S_{i - 1}) / Δ t ， \\ a_{i} = ({v^{'}}_{i} - {v^{'}}_{i - 1}) / Δ t ， \\ j_{i} = (a_{i} - a_{i - 1}) / Δ t ， \end{array}

（25）

式中： $S_{i}$ 为主车在第i个采样时间的行驶路径弧长； $ν_{i}$ 为主车在第i个采样时间的车速； $a_{i}$ 为主车在第i个采样时间的加速度； $j_{i}$ 为主车在第i个采样时间的冲击度。

为保证主车换道速度规划的安全性、平滑性与高效性，采用代价函数^[

16]为

J_{D P} = \sum_{i = 1}^{n} [w_{v_r e f} (v_{i} - v_{r e f})^{2} + w_{a} {(\frac{v_{i} - v_{i - 1}}{Δ t})}^{2} + w_{j e r k} {(\frac{a_{i} - a_{i - 1}}{Δ t})}^{2} + \frac{w_{o b s}}{D_{i} + η}]

，

（26）

式中： $w_{v_r e f}$ 为期望车速权重，取10 000； $w_{a}$ 为加速度权重，当加速度绝对值大于3 m/s²，取10 000，否则取0； $w_{j e r k}$ 为冲击度权重，取5 000； $w_{o b s}$ 为主车与干扰车的碰撞权重，取10⁵； $ν_{r e f}$ 为期望车速，取120 km/h； $η$ 表示接近于0的值，取0.01，避免分母为0，D_i为第i个采样时间主车与干扰车的碰撞距离，即主车和干扰车在ST图上的最小距离。

2.2.2 主车规划速度曲线优化

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

设计优化决策变量 $x_{o} = {[S_{1}, ν_{1}, a_{1}, S_{2}, ν_{2}, a_{2}, . . ., S_{n}, ν_{n}, a_{n}]}^{T} ，采样时间 Δ t = 0.1 s$ 。目标函数 $J_{Q P}$ 为

\begin{array}{l} J_{Q P} = \sum_{i = 1}^{n} [w_{v_r e f 1} (v_{i} - v_{r i})^{2} + w_{a 1} {a_{i}}^{2} + w_{j e r k 1} {j_{i}}^{2} + w_{r e f} (S_{i} - S_{r i})^{2}] = \\ ^{}^{}^{}^{}^{}^{} \sum_{i = 1}^{n} (w_{v_r e f 1} (v_{i} - v_{r i})^{2} + w_{a 1} {a_{i}}^{2} + w_{j e r k 1} {[(a_{i} - a_{i - 1}) / Δ t]}^{2} + w_{r e f} (S_{i} - S_{r i})^{2}) ， \end{array}

（27）

式中： $w_{ν_r e f 1}$ 为期望车速权重； $ν_{r i}$ 为第i个采样时间的主车期望车速，由动态规划结果得到； $w_{a 1}$ 为加速度权重； $w_{j e r k 1}$ 冲击度权重； $w_{r e f}$ 为参考线代价权重； $S_{r i}$ 为第i个采样时间对应的主车期望行驶路径弧长，由动态规划结果得到。

由于 $J_{Q P}$ 是二次凸函数，为便于计算，可简化为二次规划形式。

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

\{\begin{array}{l} S_{i, m i n} = v_{m i n} t_{i} \leq S_{i} \leq v_{m a x} t_{i} = S_{i, m a x}, S_{i + 1} = S_{i} + v_{i} Δ t + a_{i} Δ t^{2} / 2 + j_{i} Δ t^{3} / 6 ， \\ v_{m i n} \leq v_{i} \leq v_{m a x}, v_{i + 1} = v_{i} + a_{i} Δ t + j_{i} Δ t^{2} / 2 ， \\ a_{x, m i n} \leq a_{i} \leq a_{x, m a x} ， \\ j_{m i n} \leq j_{i} = (a_{i} - a_{i - 1}) / Δ t \leq j_{m a x}, j_{i + 1} = (a_{i + 1} - a_{i}) / Δ t ， \end{array}

（28）

式中： $S_{i, m i n}$ 和 $S_{i, m a x}$ 分别为第i个采样时间主车的最小行驶路径弧长和最大行驶路径弧长； $ν_{m i n}$ 为主车的最小速度，取60 km/h； $a_{x, m i n}$ 和 $a_{x, m a x}$ 分别为主车的最小和最大加速度，文中分别取-4 m/s²和4 m/s²； $j_{m i n}$ 和 $j_{m a x}$ 分别为主车的最小和最大冲击度，分别取-1 g/s和1 g/s^[

22]。

主车侧向加速度过大会引起乘客不舒适，需要限制主车的侧向加速度，为

v_{i} \leq m i n (\sqrt[]{\frac{a_{y, m a x}}{k_{i} (S_{r i})}}, v_{m a x})

，

（29）

式中， $k_{i} (S_{r i})$ 为第i个采样时间的主车期望行驶路径弧长 $S_{r i}$ 位置对应的路径曲率。

针对第i个采样时间主车行驶路径弧长 $S_{i}$ 的限制，还要考虑到可行区域内的干扰车边界限制：

\{\begin{matrix} S_{i, m i n} = m a x (j_{h, i}, v_{m i n} t_{i}), S_{r i} > j_{h, i} ； \\ S_{i, m a x} = m i n (j_{h, i}, v_{m a x} t_{i}), S_{r i} < j_{h, i} 。 \end{matrix}

（30）

式中， $j_{h, i}$ 为 $t_{i}$ 时刻第h辆干扰车所处路径点的弧长。

2.3 主车轨迹跟踪模型

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

23]和双PID纵向速度跟踪，MPC算法输出主车控制转角δ，双PID算法输出主车纵向加速度a。主车规划及控制流程见图8所示。

图8 主车规划及控制流程图

Fig. 8 Planning and control progress of ego-vehicle

3 仿真验证及分析

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

图9 Prescan道路模型

Fig. 9 Road model made by Prescan

表1 仿真测试用例参数

Table 1 Parameters of simulation test cases

测试用例

$ν_{E V, i n i t} /$

(km·h^-1)

$ν_{C L, i n i t} /$

(km·h^-1)

$a_{C L} /$

(m·s^-2)

d_{C L, i n i t} /

$v_{T L, i n i t} /$

(km·h^-1)

$a_{T L}$ /

(m·s^-2)

$d_{T L, i n i t} /$

$ν_{T F, i n i t}$ /

(km·h^-1)

$a_{T F}$ /

(m·s^-2)

d_{T F, i n i t} /

case1

120

-1.5

25.0

39.49

1.0

-25

case2

115

2.5

128.1

110

-3

72.57

1.5

-25

case3

120

100

-2.0

163.3

110

101.20

1.5

-25

在测试用例case1中，主车在当前车道跟随前车CL，前车CL减速行驶，而邻车道前车TL加速行驶。如图10（a）所示，当t=0 s时，根据主车的期望车速 $ν_{d e s}$ =120 km/h计算DDS累计值，当前车道前车CL的DDS累计值大于邻车道前车TL的DDS累计值，此时换道意图产生，换道意图信号置1，且如图10（b）所示，主车和3辆干扰车的相对距离均满足各自安全距离 $d_{s}$ 要求，故此时换道成立信号置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，表明汽车操纵稳定性好。

图10 Case1仿真结果

Fig. 10 Simulation results of case1

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

图11 Case2仿真结果

Fig. 11 Simulation results of case2

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

图12 Case3仿真结果

Fig. 12 Simulation results of case3

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

4 结束语

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

参考文献

李克强.构建中国标准智能汽车体系实现汽车强国伟大目标[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) [百度学术]

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. [百度学术]

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. [百度学术]

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. [百度学术]

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. [百度学术]

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. [百度学术]

Gipps P G.A model for the structure of lane-changing decision[J]. Transportation Research Part B: Methodological, 1986, 20(5): 403-414. [百度学术]

杨达, 吕蒙, 戴力源, 等.车联网环境下自动驾驶车辆车道选择决策模型[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) [百度学术]

陈慧, 王洁新.基于驾驶人不满度的高速公路自动驾驶换道决策[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) [百度学术]

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. [百度学术]

吴树凡, 魏民祥, 滕德成, 等.智能车辆弯道换道路径规划算法与跟踪控制[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) [百度学术]

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. [百度学术]

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. [百度学术]

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. [百度学术]

汪海松, 胡明辉, 黎万洪, 等.建立安全换道域的换道决策与规划[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) [百度学术]

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. [百度学术]

王政.智能车辆自主换道方法的研究[D]. 长春: 吉林大学, 2016. [百度学术]

Wang Z. Research on autonomous lane changing method of intelligent vehicle[D]. Changchun: Jilin University, 2016. (in Chinese) [百度学术]

Hs D.Development of an FCW algorithm evaluation methodology with evaluation of three alert algorithms[R]. National Highway Traffic Safety Administration, 2009. [百度学术]

李嘉嘉.自动驾驶车辆自主变道决策与规划问题研究[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) [百度学术]

曾德全, 余卓平, 熊璐, 等.结构化道路下基于层次分析法的智能车避障轨迹规划[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) [百度学术]

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. [百度学术]

Bagdadi O.Assessing safety critical braking events in naturalistic driving studies[J]. Transportation Research Part F: Traffic Psychology and Behaviour, 2013, 16: 117-126. [百度学术]

袁康.基于模型预测控制的自动驾驶汽车运动规划与控制研究[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) [百度学术]

期刊社主页

编辑部首页

期刊介绍

编委会

数据库收录

过刊浏览

联系我们

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

摘要

关键词

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

1.1 弯道换道场景

1.2 换道决策模型

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

2.1 主车路径规划

2.2 主车速度规划

2.3 主车轨迹跟踪模型

3 仿真验证及分析

4 结束语

参考文献