Stratified Sampling Based Multi-dynamic Window Trajectory Planner for Autonomous Driving on Highway
-
摘要: 高速公路无人驾驶轨迹规划面临着实时性强、安全性高的挑战. 为此, 提出一种分层抽样多动态窗口的轨迹规划算法(Stratified sampling based multi-dynamic window trajectory planner, SMWTP). 首先, 用多动态窗口表征可行轨迹的搜索空间, 并基于贝叶斯网络构建轨迹概率分布模型. 其次, 采用先速度后路径的分层抽样策略生成符合动态场景约束的候选轨迹集合. 最后, 利用引入障碍车辆速度估计不确定性的责任敏感安全模型(Responsibility sensitive safety, RSS)从中选择最优轨迹. 大量仿真实验和实际交通场景测试验证了算法的有效性, 对比实验结果表明, 所提算法性能显著优于人工势场最优轨迹规划算法和多动态窗口模拟退火轨迹规划算法.Abstract: Autonomous driving trajectory planning on highways faces challenges of strong real-time performance and safety. This paper proposes a stratified sampling based multi-dynamic window trajectory planner (SMWTP) for unmanned vehicles on highway. Firstly, the search space of feasible trajectories is constructed with multi-dynamic windows. Then, the Bayesian network is used to derive the probability distribution model of trajectories. Secondly, the stratified sampling strategy where speed is sampled before path makes generated candidate trajectories meet the constraints in dynamic scenes. Finally, the uncertainty of traffic participant vehicles＇ speed estimation is embedded into responsibility sensitive safety (RSS) model to select the optimal trajectory. A large number of simulation experiments and real traffic scenario tests have verified the effectiveness of the algorithm. The comparative experimental results show that the performance of the proposed algorithm is significantly better than the optimal trajectory planning algorithm based on artificial potential fields and multi-dynamic window simulated annealing-optimized trajectory planning algorithm.
-
Key words:
- Autonomous driving /
- trajectory planning /
- motion planning /
- Bayesian network
-
近年来, 自动驾驶技术发展迅猛. 运动规划作为其中的基础性问题之一备受关注. 高速公路属结构化场景, 车辆运动速度高, 无人车的运动规划仍面临着高安全性、高可靠性、高实时性等挑战[1].
无人车运动规划主要考虑两个核心问题: 场景表征与轨迹规划. 场景表征需结合高速公路的结构化特性, 重点考虑交通要素、交通规则与障碍车辆对无人车运动的影响. 常用的人工势场模型[2-4]引入电磁场中的引力和斥力来表征场景要素对无人车的约束[2], 例如文献[3]将轨迹规划转化为在时间−势场模型中寻找代价最小的路径问题. 势场由车道线、道路边界、障碍物以及期望行驶速度四部分组成[4]. 然而, 这类势场模型往往使换道难度增大, 导致规划算法过于偏好车道保持的轨迹. 与势场模型不同, 多动态窗口法[5]则依据场景约束将速度采样限定在速度空间的多个窗口内, 并采用模拟退火法进行求解.
轨迹规划通常包含两个步骤: 1) 选择目标点或目标区域; 2) 生成轨迹. 即先选定轨迹的候选区域, 再根据定义的目标函数生成最优轨迹[6]. 通过确定候选区域可将非凸的轨迹规划问题转化为求目标函数最小的凸优化问题, 但轨迹质量严重依赖候选区域, 极易因决策错误导致规划轨迹不理想甚至失败. 文献[7]对已有的高速公路无人车行为决策方法进行总结与分析, 指出仅通过行为决策很难确定出候选区域或目标点. 因此, 需选择多个候选区域或目标点[8], 但同时也引入了候选区域或目标点数目难以确定的问题. 文献[9]使用分类回归树的方式解决无人车在高速场景中的行为决策问题. 但限于学习样本的规模问题, 无法有效处理所有场景.
轨迹生成方法的研究可大致分为两类: 一类为在未被障碍物占据的时间−位形空间(Time-configuration space)中搜索最优行驶轨迹, 主要有共形时空间栅格(Conformal spatiotemporal lattice)[10-12]和基于模型的运动预测控制(Model predictive control)[13-16]. 共形时空间栅格由满足车辆运动学约束的曲线构成, 包含障碍物轨迹与道路边界, 生成轨迹与道路形态相符. 但由于求解空间维数高, 节点与边的数量随探索距离的增加呈指数上升, 使计算量显著增大. 此外, 用固定运动模板作为共形时空间栅格的边减弱轨迹生成对动态场景的适应性. 基于模型预测控制的方法在每个时刻进行运动预测, 通过数值优化法来计算最优动作序列, 从而实现控制效果最优. 但该类算法的缺点是需要精确的系统模型.
另一类方法称为轨迹解耦, 主要有横−纵向轨迹解耦[8, 17-18]和路径−速度二元组解耦[19-26]. 横−纵向轨迹解耦通过约束每一时刻目标状态的取值范围获得有限候选轨迹集合[16], 该类方法与最优控制结合, 保证了规划轨迹的平滑性与轨迹跟踪的稳定性. 但由于候选轨迹生成时难以考虑障碍物约束与车辆动力学约束. 为保证有解, 需采样大量轨迹. 为克服这一问题, 文献[18]通过拓扑元素表征场景约束, 并采用A* 搜索和二次规划分别求得纵向和横向轨迹. 路径−速度二元组解耦方法通常先规划路径, 再为路径选择速度曲线[19]. 这类方法分别通过路径规划处理环境中的静态障碍物约束、速度规划处理环境中的动态障碍物约束. 如文献[20]通过弹性带表征静态障碍物和选定的动态障碍物对路径形状的影响. 文献[21–24]在生成路径曲线后通过参数化曲线、凸优化等方法求出光滑无碰的速度曲线. 为提高轨迹质量, 常采用路径规划和速度规划迭代优化的方法[25-26]. 但由于路径规划时并没有考虑动态障碍物, 因此可能无法给出合理的路径, 也无法为速度规划提供有效的求解空间.
受上述工作启发, 本文提出一种分层抽样多动态窗口轨迹规划算法(Stratified sampling based multi-dynamic window trajectory planner, SMWTP), 包含多动态窗口与轨迹分布模型的构建以及最优轨迹选择三步, 所提算法流程如图1所示.本文的主要贡献有:
1)基于贝叶斯网络的轨迹分布模型. 采用多动态窗口表征高速路场景中轨迹规划的搜索空间, 同时考虑窗口选择与轨迹生成, 用贝叶斯网络构造了轨迹分布模型.
2)高效的先速度后路径的分层抽样方法. 多动态窗口将搜索空间定义在速度−位形空间, 先选择窗口再生成轨迹, 使复杂的轨迹抽样可通过分层抽样策略予以简化. 轨迹生成时采用先速度后路径分层抽样的方式, 使候选轨迹符合动态场景约束, 解决了路径规划优先无法处理动态障碍物对轨迹形状影响的问题.
3)考虑障碍车辆速度估计不确定性的责任敏感安全模型. 通过引入障碍车辆速度估计的不确定性的责任敏感安全模型(Responsibility sensitive safety, RSS)对规划轨迹的安全性进行准确估计, 提升了轨迹的安全性和鲁棒性.
本文结构安排如下: 第1节介绍多动态窗口的构建; 第2节介绍类人驾驶轨迹的概率分布模型以及先速度后路径分层抽样的候选轨迹生成策略; 第3节介绍轨迹安全性估计、最优轨迹选取以及重规划的方法; 第4节通过模型性能分析、测试与对比验证本文所提算法的有效性; 第5节为总结与讨论.
1. 构建多动态窗口
机动车辆在车道横向方向上占用率较高且以独占形式为主. 假设无人车周边的机动车辆均遵守交通规则, 对同一车道相邻车辆而言, 前车速度不低于后车速度, 则可用多个动态窗口表征高速公路无人车轨迹规划的搜索空间.
动态窗口定义在Frenet坐标系下, Frenet坐标系将无人车每一时刻的位移分解为$ s $轴和$ d $轴两个方向. $ s $轴平行于道路延伸方向, 为纵轴; $ d $轴与道路延伸方向相垂直, 为横轴. 相比于笛卡尔坐标系, Frenet坐标系更便于车辆运动状态的描述. 图2给出了高速公路双车道多动态窗口的示意图. 图中, 红色机动车为无人车, 车体后轴中心坐标为$( s_{\rm{ego}}, d_{\rm{ego}} )$, 速度为$ {v_{\rm{ego}}} $, 车身长度为$ {l_{\rm{ego}}} $. 绿色机动车为无人车所在车道前方最近的机动车辆, 车体中心坐标为$\left( {{s_0}, {d_0}} \right)$, 速度为$ v_0 $, 车身长度为$ l_0 $; 设无人车所在车道的旁道在感知范围内有$ n $个障碍物, 用蓝色机动车辆表示, 中心坐标、速度以及车身长度从后往前依次为$\left( {s_i}, {d_i} \right)$, $ {v_i} $和${l_i} ,\; {i = 1, 2, \cdots, n}$. 这$ n+1 $个障碍物将无人车的可行驶区域分割为$ n+2 $个动态窗口${W_i},\; i = 0, 1, \cdots, n+1$. 动态窗口的状态包含横向位置、纵向位置和速度区间三个参量. 动态窗口的纵向位置随时间变化, 与该窗口的前后车辆的位置和速度有关. 每个动态窗口的速度区间表示无人车在该动态窗口内行驶时应满足的速度要求. 其他类型单向车道场景的表征均可由双车道多动态窗口模型衍生获得. 例如, 单车道场景可以用旁道障碍车数量为无穷的双车道多动态窗口模型来表示, 而多车道场景则可以用多个双车道多动态窗口模型的叠加来表示.
假设无人车周围的障碍车辆为匀速运动, 则动态窗口$ {W_i} $在$ t $时刻的状态可表示为 ${W_i(t)} = \lbrace {v_t}, {s_t}, {d_i} \; \vert \; {v_t \in \left[ v_{\min, i}, v_{\max, i} \right]}, s_t \in \left[ s_{\mathrm{s}, i}(t), s_{\mathrm{e}, i}(t) \right] \rbrace$. 其中包含时间$ t\in\left[ {0, + \infty } \right) $, 速度的可行区间$\left[ v_{\min, i}, v_{\max, i} \right]$, $ t $时刻动态窗口的起止纵坐标$\left[ {{s_{\mathrm{s}, i}(t)}, \; {s_{\mathrm{e}, i}(t)}} \right]$以及窗口中心的横坐标$ d_i $. 对由前后两辆机动车辆构成的动态窗口$ {W_i} $, 窗口内允许行驶的最低速度为该窗口后方车辆的行驶速度, 最高速度为该窗口前方车辆的行驶速度, 即$v_{\min, i} = v_{i-1}$, $v_{\max, i} = v_i$. 窗口在当前时刻的起止纵坐标由构成窗口的前后车辆的位置和无人车的车身长度决定, 不包含窗口前后方车辆的车身长度. $ t $时刻动态窗口的起止纵坐标$[ s_{\mathrm{s}, i}(t), s_{\mathrm{e}, i}(t)]$由$\left[ s_{\mathrm{s}, i}(0), s_{\mathrm{e}, i}(0) \right]$通过匀速运动模型推导得出. 窗口的中心位于车道中线上, 其横坐标$ d_i $与车道中线相同, 不随时间变化.
当窗口前方无机动车辆时, 如窗口$ W_{n+1} $, 该窗口允许行驶的最低速度为旁道内第$ n $辆车的行驶速度, 最高速度为交通规则中该车道(车道编号为$ l_{n} $)允许行驶的最高速度$v_{\max, l_{n}}$, 即窗口$ W_{n+1} $的速度可行区间为$\left[ v_n, v_{\max, l_{n}} \right]$. 当前时刻该动态窗口的终止纵坐标对应于无人车前方感知的最远位置, 即$s_{\mathrm{e}, n+1}(0) = s_{\rm{ego}} + L_{\mathrm{f}}$, $ L_{\mathrm{f}} $为无人车前方感知的最远距离. 而当窗口后方无机动车辆时, 如$ W_1 $窗口, 该窗口允许行驶的最低速度为0, 最高速度为旁道内第1辆车的行驶速度, 即窗口$ W_{1} $的速度可行区间为$ [ 0, v_1 ] $. 当前时刻该动态窗口的起始纵坐标对应于无人车后方感知的最远位置, 即$s_{\mathrm{s}, 1}(0) = s_{\mathrm{ego}}\; - L_{\mathrm{b}}$, $ L_{\mathrm{b}} $为无人车后方感知的最远距离; 当感知范围内的某个车道(编号为$ l_{m} $)不存在障碍物时, 该车道整体即为一个动态窗口. 该动态窗口的速度可行区间为$ [0, v_{\max, l_{m}}] $. 当前时刻该动态窗口的起始纵坐标$s_{\mathrm{s}, i}(0) = s_{\rm{ego}} - L_{\mathrm{b}}$, 终止纵坐标$s_{{\rm{e}}, i}(0) = s_{\rm{ego}} + L_{\mathrm{f}}$.
用多个动态窗口表征交通场景时, 可进一步考虑道路交通规则约束, 例如, 实线车道线是不可穿越的, 虚线车道线是可穿越的. 轨迹规划的搜索空间由符合道路交通规则的动态窗口共同构成, 定义在速度−位形空间. 动态窗口在终止时刻的状态表征了规划轨迹的终端约束条件. 同时, 无人车每一时刻所受的动态安全约束可由动态窗口在该时刻的状态结合责任敏感安全模型推导得出.
2. 轨迹分布建模与分层抽样
轨迹规划问题[6]的一般性表述如下: 给定元组$({\chi _{{\rm{free}}}}, {x_{{\mathop{{\rm{ init}}}\nolimits} }}, {X_{{\rm{goal}}}}, D, J, T)$, 则最优轨迹$ \pi ^* $表述为
$$ \begin{split} &\pi ^* = \arg \mathop {\min \; } \limits_{\pi \in \prod {\left( {\chi , T} \right)} } J(\pi ) \\& {\rm{s.t.}} \; \; \pi (0) = {x_{{\mathop{{\rm{ init}}}\nolimits} }}, \pi (T) \in {X_{{\rm{goal}}}} \\ &\qquad\pi (t) \in {\chi _{{\rm{free}}}}, \forall t \in [0, T] \\ &\qquad D(\pi (t), \pi '(t), \pi ''(t), \cdots ), \forall t \in [0, T] \end{split} $$ (1) 式中, $ J(\pi ) $为代价函数, $\pi (t) :[0, T] \to \chi$为时间参数化轨迹, $\prod {\left( {\chi , T} \right)}$表示所有连续函数$\pi (t):[0, T] \to \chi$的集合, 车辆的初始状态$ {x}_{\rm{init}} \in \chi $, 目标区域$X_{{\rm{goal}}} \subseteq \chi$, $ \chi_{\rm{free}} $为$t \in [0, T]$时刻所有允许的状态的集合, $D(\pi (t), \pi '(t), \pi ''(t), \cdots )$为轨迹上的微分约束.
上述轨迹规划问题定义在自由空间. 在动态场景中, 由于构建自由空间时并未考虑车辆行驶时的安全车距, 因此在自由空间内搜索无碰撞轨迹不能完全满足安全行驶的需求. 高速公路场景的轨迹规划要求在多动态窗口定义的搜索空间内生成满足动态安全约束的轨迹簇, 其中, 如何结合窗口选择生成合理有效的候选轨迹簇是求解问题的关键.
求解的基本思路如下: 基于贝叶斯网络构建轨迹分布模型, 具体过程如图3所示. 采用分层抽样策略, 先选择窗口再生成轨迹. 其中, 轨迹生成时采用先速度后路径分层抽样的方式.
轨迹分布模型如式(2)所示. $ \pi $为某一轨迹, 其概率受窗口状态及加速度约束.
$$ \begin{split} P\left( \pi \right) =\;& P\left( {{W_i}} \right)P\left( {\pi \left| {{W_i}} \right.} \right)= \\ & P\left( {{W_i}} \right) P\left( {{d_{\mathrm{g}}}\left| {{W_i}} \right.} \right) p\left( {{v_{\mathrm{g}}}\left| {{W_i}} \right.} \right) P\left( {{a}\left| {{v_{\mathrm{g}}}} \right.} \right) \end{split} $$ (2) 该分布定义在多个动态窗口上, 窗口$ W_i $的状态与最小速度变化量$\Delta v_{\min, i}$决定了该窗口被选择的概率$ P\left( {{W_i}} \right) $. 抽样确定窗口后, 根据期望速度的概率密度分布$ p\left( {{v_{\mathrm{g}}}\left| {{W_i}} \right.} \right) $抽样得到期望速度$ v_{\mathrm{g}} $, 对概率分布$ P\left( {{a}\left| {{v_{\mathrm{g}}}} \right.} \right) $抽样得到加速度$ a $. 规划轨迹目标点的纵坐标$ s_{\mathrm{g}} $由期望速度和加速度计算得到, 横坐标$ d_{\mathrm{g}} $通过对$ P(W_i) $和$ P(d_{\mathrm{g}}\mid W_i) $抽样确定. 假设无人车运动为匀速运动和匀加速运动的组合, 计算初始速度曲线, 并通过二次规划得到平滑舒适的速度曲线. 为满足高速道路对路径平滑性的要求, 使用参数化曲线构造从当前位置出发到目标点的连续平滑的路径曲线, 然后将路径与速度曲线通过位置−时间的关系进行匹配得到轨迹.
2.1 窗口选择概率$ P\left( {{W_i}} \right) $
人类驾驶员选择窗口$ W_i $的概率$ P\left( {{W_i}} \right) $主要与两个因素有关: 车辆进入窗口的难易程度以及进入窗口后行驶的高时效性[27−28]. 车辆进入窗口的难易程度同时也反映了窗口的安全性, 由当前时刻窗口位置$\left[ {{s_{\mathrm{s}, i}(0)}, {s_{\mathrm{e}, i}(0)}} \right]$和最小速度变化量$\Delta v_{\min, i}$表征. 其中, $\Delta v_{\min, i}$的计算为
$$ \Delta {v_{\min , i}} = \left\{ \begin{array}{ll}{{v_{\min , i}} - {v_{\rm{ego}}},}&{{v_{\rm{ego}}} < {v_{\min , i}}}\\ {0,}&{{v_{\min , i}} \leq {v_{\rm{ego}}} \leq {v_{\max , i}}}\\ {{v_{\rm{ego}}} - {v_{\max , i}},}&{{v_{\rm{ego}}} > {v_{\max , i}}} \end{array}\right. $$ (3) 窗口距离无人车较远通常使换道时间较长. 最小速度变化量$\Delta v_{\min, i}$较大时, 常伴随无人车较为剧烈或长时间的加减速行为. 因此, 进入窗口的难度越大, 轨迹执行的风险越高. 车辆进入窗口后行驶的高时效性主要体现在窗口允许行驶的最大速度$v_{\max, i}$. 通常, 距离较近、进入窗口时速度变化量小、窗口内行驶速度高于当前速度的窗口更易成为目标窗口.
为此, 在$ s $轴上建立以无人车当前位置为中心的高斯分布, 计算动态窗口范围内的累积概率. 如图4所示, 窗口距离无人车越远, 累积概率越小. 同时, 考虑窗口允许行驶的最大速度$v_{\max , i}$以及进入该动态窗口的最小速度变化量$\Delta {v_{\min , i}}$对累积概率进行加权. 通过归一化得到窗口选择概率, 同时满足所有窗口被选择的概率和为1. 搜索空间内动态窗口$ W_i $的选择概率$ P(W_i) $计算为
$$ \begin{equation} \omega = \frac{{{v_{\max , i}}}}{{{v_{\mathrm{MAX}}}}} \times \left( {1 - \frac{1}{{1 + {{\rm{e}}^{\Delta {v_{\mathrm{thr}}} - \Delta {v_{\min, i}}}}}}} \right) \end{equation} $$ (4) $$ \begin{equation} {{P'}(W_i)} = \omega \times \int_{{s_{\mathrm{s}, i}(0)}}^{{s_{\mathrm{e}, i}(0)}} {\frac{1}{{\sqrt {2\pi } \sigma }}{{\rm{e}}^{ - \frac{{ {s - \mu } }}{{2{\sigma ^2}}}}}} {\rm{d}}s \end{equation} $$ (5) $$ \begin{equation} {P(W_i)} = \frac{{{{P'}(W_i)}}} {\sum\limits_{i = 1}^{n + 2} {{{P'}(W_i)}} } \end{equation} $$ (6) 式中, $ \omega $为累积概率的加权系数; 高斯分布的均值$ \mu = {s_{\rm{ego}}} $, 标准差$\sigma = k \times {v_{\rm{ego}}}$; $ \Delta {v_{\mathrm{thr}}} $与$ k $为设定的常量参数; $ {v_{\mathrm{MAX}}} $为当前路段允许行驶的最高速度.
此外, 窗口选择概率还受轨迹的影响, 即根据轨迹抽样结果进行实时反馈. 例如, 在窗口$ W_i $中抽样得到安全行驶轨迹的概率较低, 则降低该窗口被选择的概率, 同时相应提高其他窗口被选择的概率. 继续抽样过程至候选轨迹采集完成.
2.2 轨迹生成概率$ P\left( {\pi \left| {{W_i}} \right.} \right) $
依次构建期望速度$ v_{\mathrm{g}} $的概率密度分布、加速度$ a $和目标点横坐标$ d_{\mathrm{g}} $的概率分布.
1) 期望速度$ v_{\mathrm{g}} $的概率密度分布$ p(v_{\mathrm{g}}\mid W_i) $
对窗口$ W_i $, 构建期望速度$ v_{\mathrm{g}} $的概率密度分布为 $\left[ v_{\min, i} , v_{\max, i} \right]$区间内的截断高斯分布, 即
$$ \begin{split} p( {{v_{\mathrm{g}}}\left| {{W_i}} \right.} ) =\;& p( {{v_{\mathrm{g}}}; {\mu_{v}}, {\sigma_{v}}, {v_{\min , i}}, {v_{\max , i}}} )=\\ & \frac{{\phi \left( {s'} \right)}}{{{\sigma_{v}} \times {P({v_{\min , i}}, {v_{\max , i}})}}} \end{split} $$ (7) 其中, $ \mu_{v} $为截断高斯分布的均值; $ {\sigma_{v} ^2} $为截断高斯分布的方差, 且$ \sigma_{v} $的大小与无人车行驶速度$ v_{\rm{ego}} $成正比; $ \phi \left( \cdot \right) $表示标准高斯分布; $ s' $ 为变量. 计算式为
$$ \begin{equation} s' = \frac{{{v_{\mathrm{g}}} - {\mu_{v}}}}{{{\sigma_{v}}}} \end{equation} $$ (8) $P({v_{\min , i}}, {v_{\max , i}})$为标准高斯分布在区间$[ {v_{\min , i}}, {v_{\max , i}} ]$内的累积概率, 可由式(9)求得.
$$ \begin{split} & {P({v_{\min , i}}, {v_{\max , i}})}=\\ & \qquad\Phi \left( {\frac{{{v_{\max , i}} - \mu_{v} }}{\sigma_{v} }} \right) - \Phi \left( {\frac{{{v_{\min , i}} - \mu_{v} }}{\sigma_{v} }} \right) \end{split} $$ (9) 式中, $ \Phi ( \cdot ) $为标准高斯分布的累积分布函数.
若旁道内的动态窗口$ W_i $被选择, 换道轨迹的平滑性和舒适性要求较高, 应避免进入窗口时速度变化量较大带来的剧烈加减速行为. 因此, 令式(9)中的$ \mu_{v} = v_{\rm{ego}} $, 如图5所示.
图 5 旁道动态窗口内期望速度的概率密度分布Fig. 5 Probabilistic density distribution of desired speed in side lane＇s dynamic window当所选窗口为当前道窗口$ W_0 $时, 为与前车保持安全车距, 避免发生碰撞, 令式(9)中的$\mu_{v} = v_{\max , 0}$, 如图6所示. 根据安全跟车距离对$ W_0 $窗口内允许行驶的最大速度$v_{\max, 0}$进行修正, 即
图 6 当前道动态窗口内期望速度的概率密度分布Fig. 6 Probabilistic density distribution of desired speed in current lane＇s dynamic window$$ {v_{\max, 0}} = \left\{ {\begin{array}{ll} {{\eta _{\mathrm{safe}}}{v_0},}&{{\eta _{\mathrm{safe}}} < 1}\\ {\dfrac{1}{2}(\eta_{\mathrm{safe}} - 1)+{v_0},}&{{\eta _{\mathrm{safe}}} \ge 1} \end{array}} \right. $$ (10) 式中, $ \eta _{\mathrm{safe}} $为无人车相对当前车道前方最近车辆的安全程度, 计算方式为
$$ \begin{equation} {\eta _{\mathrm{safe}}} =\frac { {{s_0} - {s_{\rm{ego}}}} } {{s_{\min }}\left( {{v_0}, {v_{\rm{ego}}}} \right)} \end{equation} $$ (11) 式中, ${s_{\min }}\left( {{v_0}, {v_{\rm{ego}}}} \right)$为无人车与前方车辆在纵向上的最短安全距离, 由RSS模型[29]计算得出. $ {\eta _{\mathrm{safe}}} \geq 1 $时无人车相对前方车辆是安全的, 反之为不安全的. 当安全系数较大时, 说明跟车距离过大, 可通过提高窗口内允许行驶的最大速度$v_{\max, 0}$调整抽样的概率密度分布, 缩短跟车距离; 反之, 则通过降低$v_{\max, 0}$增大跟车距离以提高车辆行驶的安全系数.
2) 加速度$ a $的概率分布$P(a \mid v_{\mathrm{g}})$
加速度$ a $的概率分布由期望速度$ v_{\mathrm{g}} $决定. 由无人车的加减速性能设定加速度候选集$a = \{ - 4, -2, -1.5, -0.7, 0, 0.5, 1, 1.5 \}$. 令加速度$ a $的概率分布为多项分布, 即
$$ \begin{split} P( a \mid v_{\mathrm{g}} ) =\;& PN ( N = 8: P({a = -4}), \cdots , \\ & P( {a = 1.5} ) \mid {v_{\mathrm{g}}} ) \end{split} $$ (12) 考虑到规划的时效性, 速度变化量大时, 加速度的抽样概率与其绝对值大小成正比; 反之, 速度变化量较小时, 加速度的抽样概率与其绝对值大小成反比.
3) 目标点横坐标$ d_{\mathrm{g}} $的概率分布$P(d_{\mathrm{g}} \mid W_i)$
考虑到无人车在进行车道保持时, 为与旁侧车道内邻近的机动车辆保持横向安全距离可能需要向车道内一侧避让. 故令目标点的横坐标$ d_{\mathrm{g}} $服从多项分布:
$$ \begin{split} P(d_{\mathrm{g}} \mid W_i) = \;&PN ({N = 3}:P (d_{\mathrm{g}} = d_{\mathrm{c}} - w), \\ & P (d_{\mathrm{g}} = d_{\mathrm{c}}), P (d_{\mathrm{g}} = d_{\mathrm{c}} + w) \mid W_i) \end{split} $$ (13) 其中, $ d_{\mathrm{c}} $为无人车所在车道中线的横坐标, $ w $为常量参数, 与车道宽度有关. 分布中包含偏左、中间、偏右的三个横坐标, 每次路径生成前从该分布中随机抽样一个作为目标点横坐标. 而对换道场景, 从安全性考虑, 期望换道结束后无人车行驶在道路中间, 因此目标点均位于车道中线附近.
2.3 分层抽样的轨迹生成方法
轨迹$ \pi $的生成由先速度后路径的分层抽样实现, 抽样次序依次为期望速度$ v_{\mathrm{g}} $、加速度$ a $以及目标点横坐标$ d_{\mathrm{g}} $.
1) 速度曲线生成
已知期望速度$ v_{\mathrm{g}} $和加速度$ a $, 假设无人车的运动为匀速运动和匀加速运动的组合, 则$ v{\text{-}}t $速度曲线由此确定. 其中, $ L_{\mathrm{acc}} $表示无人车在匀加/减速阶段的行驶距离. 在此基础上计算路径规划目标点的纵坐标$ s_{\mathrm{g}} $. 若目标窗口为当前道窗口$ W_0 $, 则对应车道保持路径, $ s_{\mathrm{g}} $计算式为
$$ \begin{equation} {s_{\mathrm{g}}} = {s_{\rm{ego}}} + \max \left( {{L_{\mathrm{acc}}}, {L_k}} \right) \end{equation} $$ (14) 式中, $ L_k $为车道保持规划的最短路径, 避免规划距离过短.
当目标窗口位于旁侧车道时, 对应换道路径. 考虑控制的可执行性, 假定无人车先达到符合窗口速度要求的期望速度$ v_{\mathrm{g}} $, 再匀速换道进入目标窗口, 参考人类驾驶员的平均换道距离, $ s_{\mathrm{g}} $计算式为
$$ \begin{equation} s_{\mathrm{g}} = {s_{\rm{ego}}} + {L_{\mathrm{acc}}} + {L_{\rm{c}}}\left( {{v_{\mathrm{g}}}} \right) \end{equation} $$ (15) 式中, ${L_{\rm{c}}}\left( {{v_{\mathrm{g}}}} \right)$为无人车以速度$ v_{\mathrm{g}} $匀速换道时的平均换道距离[30–32]. 规划时距$ T $在此基础上计算得出.
至此, 起始点纵坐标$ s_{\mathrm{ego}} $、目标点纵坐标$ s_{\mathrm{g}} $以及规划时距$ T $均已知, 可将$ v{\text{-}}t $速度曲线投影至$ s{\text{-}}t $空间, 通过二次规划来提高速度曲线的平滑性和舒适性[26]. 优化后的速度曲线$ S^{\ast} $表述如下:
$$ \begin{split} \ &S^{\ast} = \arg \mathop {\min \; }\limits_S {C_{\mathrm{total}}}\left( S \right) \\& {\rm{s.t.}} \; \; S(0) = {s_{\rm{ego}}}, S(T) = {s_{\mathrm{g}}} \\ &\qquad{0} \le S' \le {v_{\mathrm{MAX}}} \\ &\qquad{a_{\min }} \le S'' \le {a_{\max }} \end{split} $$ (16) 式中, $ S $为五阶多项式曲线, $ {a_{\min }} $和$ a_{\max} $为最大减速度和最大加速度. $ {C_{\mathrm{total}}}\left( \cdot \right) $为代价函数, 计算如式(17)所示. 式中, $ S_{\mathrm{ref}} $为优化前的速度曲线, $ \omega _1 $, $ \omega _2 $, $ \omega _3 $分别为位置、加速度以及加加速度的惩罚项系数. 代价函数使优化曲线贴近初始规划曲线的同时改善速度曲线的连续性和可导性, 进而提升曲线的平滑性和舒适性.
$$ \begin{split} {C_{\mathrm{total}}}\left( S \right) =\;& {\omega _1}\int_0^T {{{\left( {S - {S_{\mathrm{ref}}}} \right)}^2}\mathrm{d}t} \;+\\ & {\omega _2}\int_0^T {{{\left( {S''} \right)}^2}\mathrm{d}t} + {\omega _3}{\int_0^T {\left( {S'''} \right)} ^2}\mathrm{d}t \end{split} $$ (17) 2) 路径曲线生成
在高速场景中, 采用参数化曲线构造法进行路径规划可满足路径平滑连续的需求. 本文选择三阶贝塞尔曲线来进行路径规划, 主要原因有以下两点: a)三阶贝塞尔曲线的解空间是二维的, 且包含常见的无人车行驶路径的曲线形状, 满足路径平滑性约束; b) 三阶贝塞尔曲线运算代价低, 更易满足实时性要求. 贝塞尔曲线的生成可参考文献[33].
将速度曲线与规划路径进行时域关联可得到规划轨迹. 为防止车辆在轨迹执行中发生侧滑, 路径曲线上曲率的最大值$ {\kappa}_{\max} $应满足如下计算式:
$$ \frac{{mv_{\mathrm{m}}^2}}{R_{\min}} = mv_{\mathrm{m}}^2 \times {\kappa}_{\max} \le {k_{\mathrm{f}}} \times mg $$ (18) 即
$$ {\kappa}_{\max} \le \frac{{{k_{\mathrm{f}}}g}}{{v_{\mathrm{m}}^2}} $$ (19) 其中, $ v_{\mathrm{m}} $为轨迹上的最高行驶速度, $ k_{\mathrm{f}} $为静摩擦系数, $ R_{\min} $为车速为$ v_{\mathrm{m}} $时的最小转弯半径. 候选轨迹集合由符合车辆动力学约束的候选轨迹构成.
综上, 基于分层抽样的轨迹生成方法的流程如算法1所示.
算法 1. 分层抽样的轨迹生成方法
输入. 无人车当前位置$ (s_{\rm{ego}}, d_{\rm{ego}}) $, 速度$ v_{\rm{ego}} $; 窗口选择概率分布$ P(W_i) $, 期望速度概率密度分布$p(v_{\mathrm{g}} \mid W_i)$, 加速度概率分布$P(a \mid v_{\mathrm{g}})$, 目标点横坐标概率分布$P(d_{\mathrm{g}} \mid W_i)$; 加速度取值范围$[a_{\min}, a_{\max}]$, 静摩擦系数$ k_{\mathrm{f}} $.
输出. 候选轨迹集合.
步骤 1. 对窗口选择概率分布$ P(W_i) $抽样得到目标窗口;
步骤 2. 对期望速度概率密度分布$p(v_{\mathrm{g}} \mid W_i)$抽样得到$ v_{\mathrm{g}} $;
步骤 3. 对加速度概率分布$P(a \mid v_{\mathrm{g}})$抽样得到$ a $;
步骤 4. 对目标点横坐标概率分布$P(d_{\mathrm{g}} \mid W_i)$抽样得到$ d_{\mathrm{g}} $;
步骤 5. 根据式(14)和式(15)计算目标点纵坐标$ s_{\mathrm{g}} $;
步骤 6. 求解二次规划问题(16), 得到平滑舒适的速度曲线;
步骤 7. 参考文献[32]使用三阶贝塞尔曲线生成路径曲线;
步骤 8. 根据式(19)判断路径曲线是否满足曲率约束, 若满足曲率约束, 则将路径与速度曲线时域关联后的轨迹加入候选轨迹集合;
步骤 9. 执行步骤1 ~ 8, 直至候选轨迹集合构建完成.
轨迹分布模型主要从安全性、高效性和可执行性刻画了轨迹的概率密度分布. 轨迹的分布由窗口的选择概率决定. 进入难度小、安全性和高效性好的窗口是规划的理想目标区域, 在轨迹分布模型中这类窗口被选择的概率大, 窗口内的候选轨迹多. 在轨迹抽样数目一定的条件下, 与基于均匀分布的轨迹抽样相比, 基于轨迹分布模型的抽样可以提高抽样轨迹的有效性, 能够更好地从概率上刻画最优轨迹的特性. 动态窗口的状态包含了最优轨迹在内的所有轨迹都需满足的场景约束, 因此通过多动态窗口表征规划的搜索空间一定包含最优解. 轨迹的生成概率刻画了无人车在动力学约束下可行轨迹的分布. 与其他抽样类算法相同, 当抽样轨迹数量足够多时即可得到渐近最优的规划结果.
3. 最优轨迹选取
本节将责任敏感安全模型与周围机动车辆速度的不确定性结合对轨迹的安全性进行估计, 同时考虑平滑性、舒适性和高效性对轨迹进行代价评估, 选择代价最小的候选轨迹作为规划的最优轨迹.
3.1 安全性评估
对轨迹进行安全性评估时, 应首先判断规划初始时刻无人车是否处于安全状态. 若初始时刻无人车在横、纵向上均不满足RSS模型中的安全条件, 则无人车处于不安全状态, 合理的规划轨迹应使无人车在较短时间内逃离不安全处境, 并在之后的规划时间内保持安全状态. 而对于初始时刻无人车处于安全状态的情况, 规划轨迹应在规划时距内继续保持安全状态.
依据RSS模型中的横向安全准则对相邻车道内邻近的机动车辆进行横向安全判断: 当$ \Delta d \geq d_{\min} $时, 两车辆在横向上是安全的. 其中, $ \Delta d $为两车辆之间的横向距离, $ d_{\min} $为两车辆在横向上的最短安全距离. 同样地, 依据纵向安全准则对同一车道内相邻两辆车进行纵向安全判断: 当$ \Delta s \geq s_{\min} $时, 两辆车在纵向上是安全的. 其中, $ \Delta s $为两车辆在纵向上的距离, $ s_{\min} $为两车辆在纵向上的最短安全距离. $ d_{\min} $和$ s_{\min} $的计算参见文献[29].
横向安全性判断受周边车辆横向速度影响, 但场景感知系统有测量噪声, 因此引入以下激活函数来抑制测量噪声的影响.
$${v_{\mathrm{lat}}} = \left\{ {\begin{array}{*{5}{l}} {\begin{array}{*{5}{l}} {0,}\\ {\widetilde{v}_{\mathrm{lat}},} \end{array}}&{\begin{array}{*{5}{l}} {\widetilde{v}_{\mathrm{lat}} \le {v_{\mathrm{thr}}}}\\ {\widetilde{v}_{\mathrm{lat}} > {v_{\mathrm{thr}}}} \end{array}} \end{array}} \right. $$ (20) 若$ \widetilde{v}_{\mathrm{lat}} \leq {v_{\mathrm{thr}}} $, 则认为$ \widetilde{v}_{\mathrm{lat}} $为噪声所致, 应予以抑制; 若$ \widetilde{v}_{\mathrm{lat}} > {v_{\mathrm{thr}}} $, 则认为$ \widetilde{v}_{\mathrm{lat}} $为障碍机动车辆实际横向速度$ v_{\mathrm{lat}} $而非测量噪声. $ v_{\mathrm{thr}} $由传感器性能决定.
由于车辆速度的测量误差以纵向速度误差为主, 因此根据纵向速度误差分布建立高斯模型以估计障碍车辆经过时间$ t $后的纵向位置. 若障碍车辆$ c_i $纵向速度的估计值$ v_{\mathrm{lon}} $服从高斯分布${v_{\mathrm{lon}}} \sim {\rm{N}}( {{\widetilde{v}_{\mathrm{lon}}}, {\sigma_{\mathrm{m}} ^2}} ),$ 其中, $ \widetilde{v}_{\mathrm{lon}} $为感知系统对障碍车辆纵向速度的估计值, 也是高斯分布的均值; $ \sigma_{\mathrm{m}} ^2 $为高斯分布的方差. 由于$P({v_{\mathrm{lon}}} \in \left[ {{\widetilde{v}_{\mathrm{lon}}} - 3\sigma_{\mathrm{m}} , {\widetilde{v}_{\mathrm{lon}}} + 3\sigma_{\mathrm{m}} } \right])\approx 1$, 故纵向速度估计误差可通过$ \pm 3\sigma_{\mathrm{m}} $表示.
由高斯分布的性质可知, 当以匀速运动模型来预测障碍机动车辆的运动时, 车辆经过时间$ t $后的纵坐标服从高斯分布$s_i(t) \sim {\rm{N}}( {s_i + {\widetilde{v}_{\mathrm{lon}}} \times t}, {t^2} \times {\sigma_{\mathrm{m}} ^2} )$. 对于同一车道内相邻两辆车, 根据无人车$ t $时刻轨迹点的纵坐标以及纵向安全距离可估计该时刻无人车与该障碍车辆不发生碰撞的概率, 记为${P_{\mathrm{ego} \to {c_i}}}\left( t \right)$, 如图7所示. 计算无人车与周边车辆不发生碰撞的概率, 取其中最小值记为该时刻无人车行驶的纵向安全概率, 即${P_{\mathrm{lon}, \mathrm{s}}}\left( t \right) = \mathop {\min }\nolimits_{i = 1, \cdots, N} {P_{\mathrm{ego} \to {c_i}}}\left( t \right)$. 设安全阈值为$ P_{\mathrm{thr}} $, 若规划时间内存在某一时刻${P_{\mathrm{lon}, \mathrm{s}}}\left( t \right) < P_{\mathrm{thr}}$, 则说明轨迹纵向安全性不足.
图 7 无人车相对于车辆$ c_i $的纵向安全概率Fig. 7 The longitudinal safety probability of ego vehicle with respect to vehicle $ c_i $综上, 轨迹判定为不安全的情况有两种: 1)对于相邻车道的车辆, 无人车在沿规划轨迹行驶时存在横向安全条件不满足且同时纵向安全概率低于安全阈值的情况; 2)对于当前道前方距离无人车最近的车辆, 无人车在当前道行驶时存在纵向安全概率低于安全阈值的情况.
当轨迹为安全轨迹时, $P_{\mathrm{lon}, \mathrm{s}}\left( t \right)$可以近似描述无人车沿轨迹行驶时安全概率$ P_{\mathrm{safe}}\left( t \right) $的变化, 即$P_{\mathrm{safe}}\left( t \right) \approx P_{\mathrm{lon}, \mathrm{s}}\left( t \right)$. 反之, 当轨迹为不安全轨迹时, $ P_{\mathrm{safe}}\left( t \right) = 0 $. 因此, $ P_{\mathrm{thr}} $设定的大小也反映了规划策略的保守程度, $ P_{\mathrm{thr}} $越大, 规划策略越保守, 规划轨迹的安全性也越高. 安全的候选轨迹的示意图如图8所示.
3.2 最优轨迹选取
由于轨迹分布模型构建的核心在于轨迹的可行性, 因此对给定候选轨迹集合$ \chi $, 还需根据以下代价函数对候选轨迹$\pi \left( {x(t), y(t)} \right) \in \chi$的质量进行评估, 并选择代价最小的轨迹作为规划的最优轨迹.
$$\begin{split} \ C_{\mathrm{total}}( \pi )=\; & C_{\mathrm{smo}}( \pi ) + C_{\mathrm{safe}}( \pi )\;+ \\ & C_{\mathrm{acc}}( \pi ) + C_{\mathrm{vel}}( \pi ) \end{split} $$ (21) 式中, $ {C_{\mathrm{total}}}\left( \pi \right) $为轨迹的总代价, $ {C_{\mathrm{smo}}}\left( \pi \right) $为曲线的平滑性惩罚项, 用于衡量轨迹的平滑程度, 计算方式[34]为
$$ \left\{ \begin{aligned} &{C_{\mathrm{smo}}}\left( \pi \right) = \int_0^T {{\omega _{\mathrm{yawr}}}{{\dot \psi }^2}\left( t \right)\mathrm{d}t} \\ &\psi \left( t \right) = \arctan \frac{{\dot y(t)}}{{\dot x(t)}} \end{aligned} \right. $$ (22) 式中, $ {\omega _{\mathrm{yawr}}} $为惩罚项系数, $ \psi \left( t \right) $为偏航角, $ \dot \psi \left( t \right) $为偏航角速度, $ {C_{\mathrm{smo}}}\left( \pi \right) $惩罚短时间内偏航角变化较大、转弯较急的轨迹序列, 进而改善无人车运动的平滑性. 对轨迹按以下公式进行等时间间隔采样, 将轨迹离散化为包含$ N $个轨迹点的序列. 式中, $ h $为采样时间间隔, $ T $为规划时距.
$$ \left\{ \begin{aligned} &{t_i} = i \times h, \qquad 0 \le i < N\\ &h = \frac {T}{ N} \end{aligned} \right. $$ (23) 则式(11)可近似为
$$ \left\{ \begin{aligned} &\dot x(t) = \frac{{{x_{t + 1}} - {x_{t - 1}}}}{{2h}}\\ &\dot y(t) = \frac{{{y_{t + 1}} - {y_{t - 1}}}}{{2h}}\\ &\dot \psi (t) = \frac{{{\psi _{t + 1}} - {\psi _{t - 1}}}}{{2h}} \end{aligned} \right. $$ (24) $ {C_{\mathrm{safe}}}\left( \pi \right) $为安全惩罚项, 用于提高规划轨迹的安全程度, 计算式为
$$ \begin{equation} {C_{\mathrm{safe}}}\left( \pi \right) = \frac{{{\omega _{\mathrm{safe}}}}}{ {{P_{\mathrm{safe}, \pi }}}} \end{equation} $$ (25) 式中, $ {\omega _{\mathrm{safe}}} $为安全惩罚项系数, $P_{\mathrm{safe}, \pi}$为候选轨迹的安全概率, 计算式为
$$ \begin{equation} {P_{\mathrm{safe}, \pi }} = \mathop {\min }\limits_{i = 1, \cdots, N} {P_{\mathrm{safe}}}\left( {{t_i}} \right) \end{equation} $$ (26) 式中, $ {P_{\mathrm{safe}}}\left( {{t_i}} \right) $为轨迹离散化后$ t_i $时刻的安全概率, 计算参见3.1节. 安全概率越低, 代价越大, 以确保无人车行驶的安全性.
$ {C_{\mathrm{acc}}}\left( \pi \right) $为加速度惩罚项, 用于避免规划轨迹中不必要的加减速, 计算式为
$$ \begin{equation} {C_{\mathrm{acc}}}\left( \pi \right) = \sum\limits_{i = 0}^N {{\omega _{\mathrm{acc}}} \cdot a_{{t_i}}^2 \cdot h} \end{equation} $$ (27) 式中, $ \omega _{\mathrm{acc}} $为加速度惩罚项系数, $ {a_{{t_i}}} $为无人车在$ t_i $时刻的加速度. 加速度绝对值越大、加/减速时间越长, 代价越大, 越有利于提升无人车行驶舒适性.
$ {C_{\mathrm{vel}}}\left( \pi \right) $为速度惩罚项, 用于提高无人车的行驶效率, 计算式为
$$ \begin{equation} {C_{\mathrm{vel}}}\left( \pi \right) = {\omega _{s1}} \left( {{v_{\mathrm{MAX}}} - {v_{\mathrm{lim}}}} \right) + {\omega _{s2}} \left( {{v_{\mathrm{MAX}}} - {v_{\mathrm{g}}}} \right) \end{equation} $$ (28) 式中, $ \omega _{s1} $和$ \omega _{s2} $为速度惩罚项系数, $ v_{\mathrm{lim}} $为目标窗口允许行驶的最大速度. $ v_{\mathrm{lim}} $越大, 代价越小; 轨迹的期望速度越大, 代价越小, 越有利于无人车尽快完成驾驶任务.
综上, 代价函数选择平滑、安全、高效、舒适的行驶轨迹作为最优轨迹. 同时, 最优轨迹目标点的位置也决定了决策结果. 若目标点在当前道, 决策为车道保持, 若目标点在旁道, 决策为车道变换.
3.3 重规划
已知当前控制器正在执行的规划轨迹, 根据最新的感知数据可计算出执行轨迹在未来规划时距内安全概率的变化, 方法同第3.1节. 若安全概率低于安全阈值或窗口状态发生较大变化, 应立即进行重规划, 方法同第2节和第3.2节. 重规划时需保持前后两帧轨迹的连续性, 方法参考文献[34]. 否则, 可继续执行该轨迹至其达到最大时效. 例如, 当前执行轨迹为换道轨迹, 在安全情况下, 待无人车车身跨越车道线进入目标车道后进行重规划, 无人车的行为由车道变换切换为车道保持.
4. 实验与讨论
本节首先对所提出的轨迹规划算法在仿真环境中进行验证, 再通过实际交通场景测试进行性能测试, 最后在仿真环境下与人工势场最优轨迹规划算法(Trajectory planning using adaptive potential field, TP-ATP)[3]以及多动态窗口模拟退火轨迹规划算法(Simulated annealing-optimized trajectory planner, SA-TP)[5]进行性能对比. 实验中, SMWTP与TP-ATP参数设置如表1和表2所示, SA-TP参数设置与文献[5]中相同.
表 1 SMWTP参数设置Table 1 Parameters of SMWTP参数名称 参数值 $ k $ 1.5 $\sigma_{v}\;({\rm{m/s} })$ 2 $\Delta {v}_{\mathrm{thr} }\;({\rm{m/s} })$ 5 $\omega _{\mathrm{yawr}} $ 20 $\omega _{\mathrm{safe}} $ 5 $ \omega _{\mathrm{acc}} $ 3 $ \omega _{s1} $ 1 $ \omega _{s2} $ 0.5 表 2 TP-ATP参数设置Table 2 Parameters of TP-ATP参数名称 参数值 $\omega _{\mathrm{s}} $ 5 $\omega _{\mathrm{d}} $ 5 $ \omega _{\mathrm{c}} $ 0.5 $ \omega _{\mathrm{p}} $ 0.005 $ c _{\mathrm{j},\mathrm{s}} $ 1 $ c _{\mathrm{v},\mathrm{s}} $ 0.2 $ c _{T,\mathrm{s}} $ 0.1 $ c _{\mathrm{j},\mathrm{d}} $ 1.5 $ c _{T,\mathrm{d}} $ 0.1 $ D_0 $ 10 $\tau$ 4 4.1 模型性能验证
1) 多动态窗口
由测试结果(详细介绍参见第4.3节)可以发现, 与人工势场法相比, 使用多动态窗口进行场景表征可使规划更加灵活, 克服了对车道保持轨迹的依赖.
2) 轨迹的生成概率与代价的互补性
构建轨迹分布模型的核心是轨迹的可行性, 而轨迹代价的评估更强调轨迹的质量, 两者间存在互补关系, 以图9所示场景为例进行说明.
在图9所示场景中, 以窗口$ W_2 $为例来分析轨迹生成概率与轨迹代价之间的关系. 对旁侧车道内同一窗口, 规划轨迹的目标点均位于车道中线上, 轨迹生成概率仅与期望速度和加速度有关. 加速度的抽样概率与速度变化量有关. 当窗口后方车辆$ c_2 $行驶速度为$ 18 \;{\rm{ m/s}} $时, 无人车在纵向上与障碍车辆$ c_1 $和$ c_2 $的距离远大于安全距离. 图10展示了不考虑障碍车辆速度估计的不确定性的情况下轨迹代价与生成概率之间的关系. 由于加速度的概率分布不连续, 图中用不同颜色表示不同加速度的轨迹. 可以看出, 轨迹代价越大, 轨迹的生成概率越小. 这也表明用生成概率选择轨迹与用代价选择轨迹具有一致性.
图 10 不考虑障碍车辆速度估计不确定性时轨迹代价与生成概率之间的关系Fig. 10 The relationship between the trajectory cost and the generation probability when the uncertainty of the speed estimation of the obstacle is not considered当窗口后方车辆$ c_1 $行驶速度为$ 21 \; {\rm{m/s}} $时, 无人车与障碍车辆$ c_1 $的纵向距离在当前时刻恰好处于RSS纵向安全距离阈值处, 因此随着时间的推移, 对障碍车辆$ c_1 $速度估计的不确定性累积为位置的不确定性, 将对规划的安全性造成影响. 图11以加速度$ 0.5 \;{\rm{ m/s}}^2 $为例, 展示了轨迹生成模型与代价函数之间的差异性. 图中横轴表示期望速度, 左侧纵轴为轨迹的生成概率, 右侧纵轴为轨迹代价, 蓝色曲线表示加速度确定的条件下轨迹生成概率随期望速度的变化情况, 红色曲线表示轨迹代价随期望速度的变化情况. 从图中可以看出, 轨迹代价与生成概率的整体变化趋势相同, 但在$ [21 \; {\rm{m/s}},\;22.5 \; {\rm{m/s}}] $区间内, 两者变化趋势不同. 轨迹的生成概率在期望速度为$ 21 \; {\rm{m/s}} $时达到最大, 这是由期望速度分布的均值(即无人车的速度)所决定的. 随着期望速度的增大, 轨迹的生成概率将逐渐降低. 但在该区间内, 由于障碍车辆速度估计不确定性的影响, 轨迹的安全性随着期望速度的增大在一定范围内得到显著提升, 轨迹代价随之降低. 轨迹代价弥补了轨迹生成概率在轨迹安全性估计上的不足. 当期望速度大于$ 22.5 \; {\rm{m/s}} $后, 无人车与窗口前方车辆$ c_2 $之间的安全性将逐渐降低, 同时加速度代价也明显增大, 使轨迹代价明显上升, 轨迹生成概率与轨迹代价的变化趋势保持一致. 上述实验结果表明, 轨迹生成概率与轨迹代价之间具有互补性.
图 11 考虑障碍车辆速度估计不确定性时轨迹代价与生成概率之间的关系Fig. 11 The relationship between the trajectory cost and the generation probability when the uncertainty of the speed estimation of the obstacle is considered3) 障碍车辆速度估计不确定性的影响
在规划轨迹的安全概率阈值确定的情况下, 障碍车辆速度估计不确定性的大小将直接影响规划策略的保守程度. 同样以图9所示场景为例, 随时间推移, 障碍车辆位置的不确定性将越来越大. 经过时间$ t $后, 障碍车辆$ c_{i} $纵坐标$ s_i(t) $的取值范围可表示为 $ [s_i(0) + {v_i}t - 3t\sigma_{\mathrm{m}}, s_i(0) + {v_i}t + 3t\sigma_{\mathrm{m}}] $, 随$ \sigma_{\mathrm{m}} $的增大而增大. 因此, 在规划轨迹的安全概率阈值不变的情况下, $\sigma_{{\rm{m}}}$越大, 规划策略越保守. 图12中, 当 $ \sigma_{\mathrm{m}} = 0.5 \; {\rm{m/s }}$时, SMWTP规划无人车左换道以获得更高的行驶速度, 换道过程中期望速度略高于当前无人车的行驶速度, 从而增大与窗口后车的距离, 使换道轨迹在考虑障碍车辆速度估计不确定性的情况下仍可保证安全; 而当$ \sigma_{\mathrm{m}} = 1.0 \; {\rm{m/s}} $时, 为保证行驶安全, SMWTP选择跟随前车行驶, 同时由于障碍车辆速度估计的不确定性较大, SMWTP规划的期望速度略低于前车速度以增大安全跟车距离. 规划结果如表3所示.
图 12 障碍车辆速度估计不确定性对轨迹规划的影响(红色方框所示轨迹为规划轨迹)Fig. 12 Impact of uncertainty in speed estimation of obstacle vehicles on trajectory planning (The trajectory shown in the red box is the planned trajectory)表 3 不同障碍车辆速度估计误差下的规划结果Table 3 Planning results with different errors in speed estimation of obstacle vehicles$\sigma_{ {\rm{m} } }\; ({\rm{m/s} })$ $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ $v_{ {\rm{lim} } }\;({\rm{m/s} })$ 决策 安全
概率
(%)$0.5$ 22.5 167.3 5.60 7.5 25 LC 91.1 $1.0$ 20.5 105.0 1.85 5.1 21 LK 95.9 4.2 真实高速场景性能测试
本文所提出算法在2017 ~ 2019年连续三届中国智能车未来挑战赛(Intelligent vehicles future challenge, IVFC) [35-36] 高架道路环境赛程中表现优异, 性能得到了充分验证. 高架道路环境中除参赛车辆之外, 还有多辆人工驾驶车辆构成的实际交通流. 人工驾驶车辆在道路上与参赛车辆之间会发生交互, 主要测试参赛车辆的汇入车流、超越低速车辆、礼让并线车辆等自主驾驶能力. 比赛中高架道路各车道均限速60 km/h.
图13为2017年比赛无人车自主行驶的一段航拍视频. 初始时刻无人车行驶在中间车道, 该车道前方距离无人车最近的机动车行驶缓慢, 右侧车道前方机动车行驶速度高, SMWTP规划无人车右换道行驶, 在安全前提下跟随速度高的车辆行驶. 当无人车右换道结束时, 原位于无人车行驶正前方的机动车从中间车道插入现无人车行驶正前方并保持缓慢行驶. 此时, SMWTP规划无人车左换道行驶回到中间车道. 中间车道前方无其他机动车, 无人车在遵守交通规则约束下高速行驶.
图 13 2017年IVFC无人车行驶中一段航拍视频(红色圆圈中心的机动车为无人车, (a), (b), (j), (k)为跟车行驶,(c) ~ (i) 为向右换道, (l) ~ (r)为向左换道)Fig. 13 A continuous aerial view of unmanned vehicles driven in IVFC in 2017 (The motor vehicle in the center of the red circle is the unmanned vehicle, (a), (b), (j), (k) show car-following, (c) ~ (i) show lane-right, (l) ~ (r) show lane-left)表4展示了2018 ~ 2019年比赛中SMWTP规划的整体情况. 测试中每次规划通过抽样生成30条候选轨迹, 并从中选择最优轨迹. 除前方突然有车辆插入导致安全概率低、需从不安全环境中逃离的情况外, 算法的平均安全概率为92.7%, 最低安全概率为80%. 算法的平均耗时为$34 \;{\rm{ ms}}$左右, 完全可以满足无人驾驶轨迹规划的实时性要求.
表 4 2018 ~ 2019年IVFC比赛中SMWTP规划情况概览Table 4 An overview of SMWTP＇s performance in IVFC in year 2018 ~ 2019年份 行驶时长
$({\rm{min} })$平均速度
$({\rm{ m/s} })$平均安全
概率(%)最低安全
概率(%)平均耗时
$({\rm{ms} })$2018 20 13.8 91.3 80 35.1 2019 30 13.2 93.6 80 33.5 图14和图15展示了2018年SMWTP在比赛中的部分轨迹规划结果及其安全概率变化. 图14中, 橙色方框为无人车, 蓝色方框为障碍车辆, 棕色括号里的数值依次代表障碍物中心点在车体坐标系下的纵坐标、横坐标以及行驶速度, 绿色点列代表道路边界, 红色曲线代表车道线, 绿色曲线为参考线, 蓝色曲线为SMWTP最终规划结果. 第137帧中无人车所在车道前方车辆行驶缓慢而右侧车道内无车辆行驶, SMWTP规划右换道行驶, 如图14(a)所示, 轨迹的安全概率为80%. 当无人车所在车道前方与旁侧车道前方均有障碍车辆行驶时, 在安全的条件下选择跟随行驶速度高的车辆行驶, 如图14(b)所示. 若当前道前方车辆距离无人车较远时, 无人车跟车行驶, 行驶速度不高于允许行驶速度, 如图14(c)所示.
图 14 2018年IVFC中SMWTP规划结果示例(橙色矩形为无人车, 蓝色曲线为规划轨迹)Fig. 14 Performance of SMWTP planning results in IVFC in 2018 (The orange rectangle represents ego vehicle, and the blue curve is the trajectory planned by SMWTP)图16和图17展示了SMWTP在2019年比赛中的部分轨迹规划结果及其安全概率变化. 图中橙色曲线代表无人车行驶过的轨迹, 其他符号含义与图14相同. 在第96帧, 无人车当前道前方车辆行驶缓慢而左侧车道内无车辆行驶, SMWTP规划以当前行驶速度左换道, 轨迹的安全概率为80%. 当引导线所在车道允许行驶的最高速度与无人车当前所在车道相同时, 无人车在安全的条件下受引导线引导回道, 如图16(b)所示. 图16(b)中引导线所在车道内障碍车辆的行驶速度为20.39 m/s, 高于限速, 因此SMWTP规划无人车回道. 当无人车行驶前方突然有障碍车辆插入时, 无人车减速以提高行驶的安全概率. 图16(c)中, 无人车前方突然插入行驶速度为20.49 m/s的障碍车辆, 无人车通过减速逐渐增大车距使安全概率由0恢复至100%. 在处理有障碍车辆换道或插入的这类场景时, SMWTP由于考虑障碍车辆的行驶意图的方式较为简单, 对规划时距内多动态窗口分布的变化考虑不足, 使SMWTP对这类突发场景的处理略有延迟且依赖于重规划.
图 16 2019年IVFC中SMWTP规划结果示例(橙色矩形为无人车, 蓝色曲线为规划轨迹)Fig. 16 Performance of SMWTP planning results in IVFC in 2019 (The orange rectangle represents ego vehicle, and the blue curve is the trajectory planned by SMWTP)此外, SMWTP还在重型牵引车自动驾驶平台上进行了大量测试验证. 图18展示了典型的重型牵引车高速换道轨迹. 图中重型牵引车的行驶速度分别为$ 18.5 \; {\rm{m/s}} $和$ 21.7 \; {\rm{m/s}} $, 可以看出, 在保证行驶安全的情况下, SMWTP选择跟随速度更高的障碍车辆行驶, 从而提高行驶的高效性. 由于实际场景测试条件限制, 更高速度下的测试在仿真环境中进行.
图 18 SMWTP规划重型牵引车换道轨迹示例(橙色矩形为无人车, 蓝色曲线为规划轨迹)Fig. 18 Lane-change trajectories for heavy tractor planned by SMWTP (The orange rectangle represents ego vehicle, and the blue curve is the planned trajectory)4.3 性能比较
在仿真环境下比较SMWTP与TP-ATP、SA-TP的性能.
4.3.1 与TP-ATP比较
在MATLAB仿真环境下设置的测试场景涵盖横向、纵向安全避让以及车道保持和换道四个典型高速场景, 如图19所示. 实验中障碍车辆速度估计误差为$ \pm 1.5 \; {\rm{m/s}} $, 即$ \sigma_\mathrm{m} = 0.5 \; {\rm{m/s}} $. 场景1、场景2和场景3分别为规划初始时刻无人车安全约束不满足的场景, 主要测试无人车在较短时间摆脱不安全处境的性能. 场景4为动态交通流场景, 主要测试算法在交通流中的高效通行能力. 将SMWTP在四个典型场景中的规划结果与TP-ATP进行比较.
1)虚线车道线下的纵向安全避让
图19(a)所示场景用于测试算法在虚线车道线情况下进行纵向安全避让的性能. 图中, 无人车的行驶速度高于当前道前方车辆的行驶速度, 而两辆车之间的距离小于安全车距. 此时两种规划器的规划轨迹如图20(a)和图20(b)所示, 图中红色方框所示轨迹为规划轨迹, 其他颜色方框所示轨迹为障碍车的预测轨迹, 由匀速运动模型推出. 规划结果的关键参数如表5所示, 其中LK代表车道保持, LC代表车道变换, 其他定义同上. 该场景中, 左侧车道内障碍车辆比当前道前方车辆行驶速度高, 势场影响范围大且势场值偏高. 因此, TP-ATP规划器规划减速来提高安全概率, 但由于规划的期望速度为当前道前方车辆的行驶速度, 因此只能在一定程度上提高安全概率, 无法恢复安全状态. SMWTP通过引入障碍车辆速度不确定性的RSS模型来估计无人车与其他车辆的安全程度. 左侧车道内的障碍车辆行驶速度高于无人车, 两者的纵向距离随时间推移逐渐增大, 且始终保持安全距离. 因此, SMWTP通过减速 + 换道的方式来使无人车尽快逃离不安全处境. 两条规划轨迹的安全概率变化如图20(c)所示, 可见在$ t = 3 \; {\rm{s}} $时SMWTP规划轨迹的安全概率已经达到100%, 而TP-ATP规划轨迹的安全概率还处于40%左右. 在$ t = 4 \; {\rm{s}} $时SMWTP轨迹换道完成, 使无人车获得更高的允许行驶速度.
表 5 虚线车道线下的纵向安全避让规划结果对比Table 5 Comparison of planning results for longitudinal safety avoidance with dashed lane场景1 $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g}} \;({\rm{m} })$ $T\;({\rm{s} })$ $v_{ {\rm{lim} } }\;({\rm{m/s} })$ 决策 TP-ATP 20.0 111.4 1.85 5.4 20 LK SMWTP 19.5 160.0 5.60 8.0 25 LC 2)实线车道线下的纵向安全避让
图19(b)所示场景用于测试算法在实线车道线情况下进行纵向安全避让的性能. 同样地, 无人车的行驶速度高于当前道前方车辆的行驶速度, 两辆车之间的距离小于安全车距. 同向车道间车道线为实线, 不允许换道. 此时两种规划器的规划轨迹如图21(a) 和图21(b) 所示, 均为沿当前道减速. 规划结果的关键参数见表6. TP-ATP规划器所规划的期望速度依然为前方车辆的行驶速度, 只能减缓跟车距离变小的趋势. SMWTP规划的期望速度比前方车辆行驶速度低$ 2 \; {\rm{m/s}} $, 在轨迹执行过程中与前方车辆距离逐渐增大. 规划轨迹的安全概率变化如图21(c) 所示, 可以看出在$ t = 4 \; {\rm{s}} $时SMWTP规划轨迹的安全概率已经达到80%以上, 而TP-ATP规划轨迹的安全概率仍处于20%左右.
表 6 实线车道线下的纵向安全避让规划结果对比Table 6 Comparison of planning results for longitudinal safety avoidance with solid lane markings场景2 $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ $v_{\mathrm{lim} }\;({\rm{m/s} })$ 决策 TP-ATP 20 107 1.85 5.2 20 LK SMWTP 18 113 1.85 5.6 20 LK 3)横向安全避让
图19(c)所示场景用于测试算法的横向安全避让性能. 无人车与左侧车道内的车辆以相同速度并排行驶, 左侧车道内的车辆在行驶时贴近右侧车道线导致两辆车的横向间距较近, 不满足RSS安全约束. 此时, 两种规划器的规划轨迹如图22(a)和图22(b)所示, 规划结果的关键参数见表7. TP-ATP由于受车道线势场和横向加速度代价的影响, 使得最终规划轨迹依然位于道路中线. SMWTP规划器受RSS横向安全约束影响, 使跟车速度略微降低的同时选择偏向道路右侧的目标点, 增大与旁道车辆的横向间距从而满足横向安全约束. 图22(c)描述了规划时间内无人车沿规划轨迹行驶时横向安全程度的变化, 其中, 横向安全程度 = 横向距离/横向最短安全距离. 在$ 0 \thicksim 4 \; {\rm{s}} $内, SMWTP规划轨迹使无人车横向速度方向为$ d $轴负方向, 使无人车横向安全程度逐渐增大. $ t = 4 \; {\rm{s}} $后无人车横向速度逐渐减小, 横向安全程度略有降低, 但始终在安全范围内.
表 7 横向安全避让规划结果对比Table 7 Comparison of planning results for lateral safety avoidance场景3 $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m } })$ $d_{\mathrm{g} }\;({\rm{m } })$ $T\;({\rm{s} })$ $v_{\mathrm{lim} }\;({\rm{m/s} })$ 决策 TP-ATP 20.0 80 1.85 4.1 20 LK SMWTP 19.5 103 1.30 5.3 20 LK 4)动态交通流
图19(d)为模拟动态交通流场景. 该场景中动态车辆较多, 与左侧车道内前方车辆的行驶速度相比, 无人车所在车道前方车辆行驶缓慢. 通过观察规划器在一段时间范围(25 s)内的规划结果来综合测试算法的车道保持和换道的能力. 图23、表8以及图24、表9分别展示了TP-ATP和SMWTP在规划初始时刻以及规划决策发生改变时的规划结果.
图 23 动态交通流中TP-ATP规划结果Fig. 23 Performance of TP-ATP planning results in dynamic traffic flow表 8 动态交通流中TP-ATP多帧规划结果Table 8 Performance of TP-ATP multi-frame planning results in dynamic traffic flowt (s) $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ $v_{\mathrm{lim} }\;({\rm{m/s} })$ 决策 $0$ 21 90 1.85 4.0 21 LK $12.5$ 25 159 5.60 6.9 25 LC 
图 24 动态交通流中SMWTP规划结果Fig. 24 Performance of SMWTP planning results in dynamic traffic flow表 9 动态交通流中SMWTP多帧规划结果Table 9 Performance of SMWTP multi-frame planning results in dynamic traffic flowt (s) $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ $v_{\mathrm{lim} }\;({\rm{m/s} })$ 决策 $0$ 22.8 156.0 5.60 6.7 25.0 LC $4.5$ 26.1 172.0 5.60 7.0 25.0 LK $24.5$ 25.1 169.4 1.85 6.8 33.3 LC 通过对比可以发现, TP-ATP只有在无人车与旁道内前方机动车辆距离非常远的情况下才会决策换道. 但由于换道过程同时为加速过程, 使换道完成前与原车道内前方车辆距离逐渐缩短, 使安全概率降低至20%以下. SMWTP首先在$ t = 0 \; {\rm{s}} $时规划左换道轨迹, 如图24(a) 所示. 通过结合障碍车辆速度估计的不确定性的RSS模型可准确估计换道的安全性. 左换道轨迹的期望速度为$ 22.8 \; {\rm{m/s}} $, 高于目标窗口后方车辆的速度, 低于目标窗口前方车辆的速度, 轨迹执行中无人车与窗口前、后方车辆距离不断增大, 从而保证障碍车辆速度估计误差不会对行驶的安全性造成较大影响. 当无人车进入目标窗口后, 与当前道前方车辆的距离大于跟车距离且速度低于前车, 因此SMWTP规划轨迹的期望速度略高于前车, 缩短跟车距离, 如图24(b)所示. 最后, 在$ t = 24.5 \;{\rm{ s}} $时SMWTP规划右换道行驶, 如图24(c)所示. 此时, 无人车行驶速度高于窗口后方车辆且纵向距离大于安全距离, 通过右换道可获得更高的允许行驶速度. 这个过程也可看作SMWTP通过规划两次换道完成超车以达到高效行驶的需求. 整个过程安全概率始终保持在90%以上.
4.3.2 与SA-TP比较
以文献[5]中SA-TP的测试场景(如图25所示)为例测试SMWTP的性能. SMWTP与SA-TP的规划结果均为沿当前道减速跟车行驶, 具体参数见表10和表11. SMWTP与SA-TP规划的期望速度均能满足安全行驶需求, SMWTP规划的期望速度的时效性更优, 且标准差远小于SA-TP. 在测试平台计算力相当的条件下, SMWTP耗时约为SA-TP的一半, 因此SMWTP的实时性优于SA-TP.
表 10 SMWTP与SA-TP规划结果对比Table 10 Comparison of SMWTP and SA-TP planning results测试场景 $v_{\mathrm{g} }\;({\rm{m/s} })$ $\sigma_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ 安全概率 决策 SA-TP 23.4 2.47 136 5.6 5.2 100% LK SMWTP 25.0 0.19 150 5.6 5.7 100% LK 表 11 SMWTP与SA-TP实时性比较Table 11 Comparison of SMWTP and SA-TP real-time performance测试场景 平均耗时$({\rm{ms} })$ 标准差$({\rm{ms} })$ 最大耗时$({\rm{ms} })$ 最小耗时$({\rm{ms} })$ SA-TP 72 10 99 61 SMWTP 34 2 49 31 SA-TP同时对6种无人车可能采取的动作进行评估, 迭代至评价函数值收敛后, 选择函数值最大的规划结果作为最优规划结果. 当沿当前车道减速跟车时评价函数值最大, 因此SA-TP规划无人车沿当前车道减速行驶, 期望速度$ v_\mathrm{g} = 23.4 \;{\rm{ m/s}} $, 轨迹的安全概率为100%. SMWTP规划中, 1 ~ 3号窗口被选择的概率较大, 其中2号窗口的最大行驶速度与无人车当前行驶速度相同, 但窗口前方车辆与无人车在纵向上距离小于最短安全距离, 无法安全汇入. 3号窗口的最大行驶速度与当前窗口相同, 汇入窗口无法获得更高行驶速度且伴随剧烈减速行为. SMWTP由轨迹抽样结果判断得出旁侧车道内的动态窗口均不安全, 调整窗口选择概率后当前道窗口被选择的概率为1, 继续轨迹抽样至候选轨迹集合构建完成. 最终, SMWTP规划减速跟车, 期望速度$ v_\mathrm{g} = 25 \; {\rm{m/s}} $, 轨迹的安全概率为100%. 在保证安全的情况下, 期望行驶速度越大越好, 以满足高效行驶的需求, 故SMWTP规划的期望速度优于SA-TP给出的期望速度. 由于SA-TP对初始值设定敏感, 使最优速度$ v_\mathrm{g} $的均值为$ 23.4 \; {\rm{m/s}} $, 标准差$ \sigma_\mathrm{g} $较大, 为$ 2.47 \;{\rm{ m/s }}$. SMWTP通过分层抽样生成候选轨迹, 每次抽样30条轨迹, 重复实验30次, 最优速度$ v_\mathrm{g} $的均值为$ 25 \; {\rm{m/s}} $, 标准差$ \sigma_\mathrm{g} $为$ 0.19 \;{\rm{ m/s}} $. 这也说明SMWTP的稳定性较好, 抽样30条轨迹即可得到渐近最优轨迹.
在两个计算力相当的测试平台上, SA-TP和SMWTP (均为C/C++ 工程文件)耗时如表11所示. 其中, SA-TP的测试平台为2.10 GHz Intel Core i7-3612QM CPU, 8 GB RAM嵌入式计算平台, SMWTP的测试平台为2.70 GHz Intel Core i7-6820HQ CPU, 16RAM计算机. 由于SA-TP算法需要迭代优化, 而SMWTP通过分层抽样生成候选轨迹簇的方式降低了轨迹生成复杂度, 同时采用耗时极短的参数化曲线生成路径曲线. 因此, SMWTP的平均耗时约为SA-TP平均耗时的一半, 且标准差小于SA-TP. 因此, SMWTP的实时性优于SA-TP.
5. 结束语
本文针对高速场景的特点提出分层抽样的多动态窗口轨迹规划算法, 实现了无人车在高速场景下的车道保持与换道, 且在横、纵向不安全时可及时采取合理的避让措施, 使无人车尽快从不安全处境脱离出来. 与先决策后轨迹规划的方法相比, 通过多动态窗口表征高速路场景、同时考虑窗口选择与轨迹生成的方式避免了由于决策候选区域错误带来的规划结果不理想的问题. 此外, 根据贝叶斯网络推导出的类人驾驶轨迹分布模型, 采用先选择窗口再抽样轨迹的分层抽样的方式解决了求解维度高带来的计算量大的问题. 同时, 先速度后路径的轨迹生成方式使候选轨迹能够满足动态场景约束. 将障碍车辆速度估计的不确定性与责任敏感安全模型相结合, 使规划轨迹的安全有效性进一步得到提高.
当前算法仍存在一些不足. 算法使用匀速模型对障碍车辆行驶轨迹进行预测, 缺乏对车辆行驶意图不确定性的考虑. 当有车辆有换道意图时, 多动态窗口模型将随时间发生变化, 算法对于此类场景的处理仍依赖于重规划机制. 因此, 在未来的工作中, 将结合车辆的行驶意图预测, 在轨迹规划时考虑一段时间范围内多动态窗口模型的变化, 进一步提升规划轨迹的有效性.
-
图 5 旁道动态窗口内期望速度的概率密度分布
Fig. 5 Probabilistic density distribution of desired speed in side lane＇s dynamic window
图 6 当前道动态窗口内期望速度的概率密度分布
Fig. 6 Probabilistic density distribution of desired speed in current lane＇s dynamic window
图 7 无人车相对于车辆$ c_i $的纵向安全概率
Fig. 7 The longitudinal safety probability of ego vehicle with respect to vehicle $ c_i $
图 10 不考虑障碍车辆速度估计不确定性时轨迹代价与生成概率之间的关系
Fig. 10 The relationship between the trajectory cost and the generation probability when the uncertainty of the speed estimation of the obstacle is not considered
图 11 考虑障碍车辆速度估计不确定性时轨迹代价与生成概率之间的关系
Fig. 11 The relationship between the trajectory cost and the generation probability when the uncertainty of the speed estimation of the obstacle is considered
图 12 障碍车辆速度估计不确定性对轨迹规划的影响(红色方框所示轨迹为规划轨迹)
Fig. 12 Impact of uncertainty in speed estimation of obstacle vehicles on trajectory planning (The trajectory shown in the red box is the planned trajectory)
图 13 2017年IVFC无人车行驶中一段航拍视频(红色圆圈中心的机动车为无人车, (a), (b), (j), (k)为跟车行驶,(c) ~ (i) 为向右换道, (l) ~ (r)为向左换道)
Fig. 13 A continuous aerial view of unmanned vehicles driven in IVFC in 2017 (The motor vehicle in the center of the red circle is the unmanned vehicle, (a), (b), (j), (k) show car-following, (c) ~ (i) show lane-right, (l) ~ (r) show lane-left)
图 14 2018年IVFC中SMWTP规划结果示例(橙色矩形为无人车, 蓝色曲线为规划轨迹)
Fig. 14 Performance of SMWTP planning results in IVFC in 2018 (The orange rectangle represents ego vehicle, and the blue curve is the trajectory planned by SMWTP)
图 16 2019年IVFC中SMWTP规划结果示例(橙色矩形为无人车, 蓝色曲线为规划轨迹)
Fig. 16 Performance of SMWTP planning results in IVFC in 2019 (The orange rectangle represents ego vehicle, and the blue curve is the trajectory planned by SMWTP)
图 18 SMWTP规划重型牵引车换道轨迹示例(橙色矩形为无人车, 蓝色曲线为规划轨迹)
Fig. 18 Lane-change trajectories for heavy tractor planned by SMWTP (The orange rectangle represents ego vehicle, and the blue curve is the planned trajectory)
图 23 动态交通流中TP-ATP规划结果
Fig. 23 Performance of TP-ATP planning results in dynamic traffic flow
图 24 动态交通流中SMWTP规划结果
Fig. 24 Performance of SMWTP planning results in dynamic traffic flow
表 1 SMWTP参数设置
Table 1 Parameters of SMWTP
参数名称 参数值 $ k $ 1.5 $\sigma_{v}\;({\rm{m/s} })$ 2 $\Delta {v}_{\mathrm{thr} }\;({\rm{m/s} })$ 5 $\omega _{\mathrm{yawr}} $ 20 $\omega _{\mathrm{safe}} $ 5 $ \omega _{\mathrm{acc}} $ 3 $ \omega _{s1} $ 1 $ \omega _{s2} $ 0.5 
下载: 导出CSV
表 2 TP-ATP参数设置
Table 2 Parameters of TP-ATP
参数名称 参数值 $\omega _{\mathrm{s}} $ 5 $\omega _{\mathrm{d}} $ 5 $ \omega _{\mathrm{c}} $ 0.5 $ \omega _{\mathrm{p}} $ 0.005 $ c _{\mathrm{j},\mathrm{s}} $ 1 $ c _{\mathrm{v},\mathrm{s}} $ 0.2 $ c _{T,\mathrm{s}} $ 0.1 $ c _{\mathrm{j},\mathrm{d}} $ 1.5 $ c _{T,\mathrm{d}} $ 0.1 $ D_0 $ 10 $\tau$ 4
下载: 导出CSV
表 3 不同障碍车辆速度估计误差下的规划结果
Table 3 Planning results with different errors in speed estimation of obstacle vehicles
$\sigma_{ {\rm{m} } }\; ({\rm{m/s} })$ $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ $v_{ {\rm{lim} } }\;({\rm{m/s} })$ 决策 安全
概率
(%)$0.5$ 22.5 167.3 5.60 7.5 25 LC 91.1 $1.0$ 20.5 105.0 1.85 5.1 21 LK 95.9
下载: 导出CSV
表 4 2018 ~ 2019年IVFC比赛中SMWTP规划情况概览
Table 4 An overview of SMWTP＇s performance in IVFC in year 2018 ~ 2019
年份 行驶时长
$({\rm{min} })$平均速度
$({\rm{ m/s} })$平均安全
概率(%)最低安全
概率(%)平均耗时
$({\rm{ms} })$2018 20 13.8 91.3 80 35.1 2019 30 13.2 93.6 80 33.5
下载: 导出CSV
表 5 虚线车道线下的纵向安全避让规划结果对比
Table 5 Comparison of planning results for longitudinal safety avoidance with dashed lane
场景1 $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g}} \;({\rm{m} })$ $T\;({\rm{s} })$ $v_{ {\rm{lim} } }\;({\rm{m/s} })$ 决策 TP-ATP 20.0 111.4 1.85 5.4 20 LK SMWTP 19.5 160.0 5.60 8.0 25 LC
下载: 导出CSV
表 6 实线车道线下的纵向安全避让规划结果对比
Table 6 Comparison of planning results for longitudinal safety avoidance with solid lane markings
场景2 $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ $v_{\mathrm{lim} }\;({\rm{m/s} })$ 决策 TP-ATP 20 107 1.85 5.2 20 LK SMWTP 18 113 1.85 5.6 20 LK
下载: 导出CSV
表 7 横向安全避让规划结果对比
Table 7 Comparison of planning results for lateral safety avoidance
场景3 $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m } })$ $d_{\mathrm{g} }\;({\rm{m } })$ $T\;({\rm{s} })$ $v_{\mathrm{lim} }\;({\rm{m/s} })$ 决策 TP-ATP 20.0 80 1.85 4.1 20 LK SMWTP 19.5 103 1.30 5.3 20 LK
下载: 导出CSV
表 8 动态交通流中TP-ATP多帧规划结果
Table 8 Performance of TP-ATP multi-frame planning results in dynamic traffic flow
t (s) $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ $v_{\mathrm{lim} }\;({\rm{m/s} })$ 决策 $0$ 21 90 1.85 4.0 21 LK $12.5$ 25 159 5.60 6.9 25 LC
下载: 导出CSV
表 9 动态交通流中SMWTP多帧规划结果
Table 9 Performance of SMWTP multi-frame planning results in dynamic traffic flow
t (s) $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ $v_{\mathrm{lim} }\;({\rm{m/s} })$ 决策 $0$ 22.8 156.0 5.60 6.7 25.0 LC $4.5$ 26.1 172.0 5.60 7.0 25.0 LK $24.5$ 25.1 169.4 1.85 6.8 33.3 LC
下载: 导出CSV
表 10 SMWTP与SA-TP规划结果对比
Table 10 Comparison of SMWTP and SA-TP planning results
测试场景 $v_{\mathrm{g} }\;({\rm{m/s} })$ $\sigma_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ 安全概率 决策 SA-TP 23.4 2.47 136 5.6 5.2 100% LK SMWTP 25.0 0.19 150 5.6 5.7 100% LK
下载: 导出CSV
表 11 SMWTP与SA-TP实时性比较
Table 11 Comparison of SMWTP and SA-TP real-time performance
测试场景 平均耗时$({\rm{ms} })$ 标准差$({\rm{ms} })$ 最大耗时$({\rm{ms} })$ 最小耗时$({\rm{ms} })$ SA-TP 72 10 99 61 SMWTP 34 2 49 31
下载: 导出CSV
-
[1] Claussmann L, Revilloud M, Gruyer D, Glaser S. A review of motion planning for highway autonomous driving. IEEE Transactions on Intelligent Transportation Systems, 2020, 21(5): 1826−1848 doi: 10.1109/TITS.2019.2913998 [2] Rasekhipour Y, Khajepour A, Chen S K, Litkouhi B. A potential field-based model predictive path-planning controller for autonomous road vehicles. IEEE Transactions on Intelligent Transportation Systems, 2017, 18(5): 1255−1267 doi: 10.1109/TITS.2016.2604240 [3] Kim D, Kim H, Huh K. Trajectory planning for autonomous highway driving using the adaptive potential field. In: Proceedings of the 21st International Conference on Intelligent Transportation Systems (ITSC). Maui, USA: IEEE, 2018. 1069−1074 [4] Wolf M T, Burdick J W. Artificial potential functions for highway driving with collision avoidance. In: Proceedings of the IEEE International Conference on Robotics and Automation. Pasadena, USA: IEEE, 2008. 3731−3736 [5] Claussmann L, Revilloud M, Glaser S. Simulated annealing-optimized trajectory planning within non-collision nominal intervals for highway autonomous driving. In: Proceedings of the International Conference on Robotics and Automation (ICRA). Montreal, Canada: IEEE, 2019. 5922−5928 [6] Paden B, Čáp M, Yong S Z, Yershov D, Frazzoli E. A survey of motion planning and control techniques for self-driving urban vehicles. IEEE Transactions on Intelligent Vehicles, 2016, 1(1): 33−55 doi: 10.1109/TIV.2016.2578706 [7] Claussmann L, Revilloud M, Glaser S, Gruyer D. A study on al-based approaches for high-level decision making in highway autonomous driving. In: Proceedings of the IEEE International Conference on Systems, Man, and Cybernetics (SMC). Banff, Canada: IEEE, 2017. 3671−3676 [8] Werling M, Ziegler J, Kammel S, Thrun S. Optimal trajectory generation for dynamic street scenarios in a Frenét Frame. In: Proceedings of the IEEE International Conference on Robotics and Automation. Anchorage, USA: IEEE, 2010. 987−993 [9] 苏锑, 杨明, 王春香, 唐卫, 王冰. 一种基于分类回归树的无人车汇流决策方法. 自动化学报, 2018, 44(1): 35−43Su Ti, Yang Ming, Wang Chun-Xiang, Tang Wei, Wang Bing. Classification and regression tree based traffic merging for method self-driving vehicles. Acta Automatica Sinica, 2018, 44(1): 35−43 [10] Ziegler J, Stiller C. Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. St. Louis, USA: IEEE, 2009. 1879−1884 [11] McNaughton M, Urmson C, Dolan J M, Lee J W. Motion planning for autonomous driving with a conformal spatiotemporal lattice. In: Proceedings of the IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE, 2011. 4889−4895 [12] 袁静妮, 杨林, 唐晓峰, 陈傲文. 基于改进RRT* 与行驶轨迹优化的智能汽车运动规划. 自动化学报, 2022, 48(12): 2941−2950Yuan Jing-Ni, Yang Lin, Tang Xiao-Feng, Chen Ao-Wen. Autonomous vehicle motion planning based on improved RRT* algorithm and trajectory optimization. Acta Automatica Sinica, 2022, 48(12): 2941−2950 [13] Yue M, Hou X Q, Zhao X D, Wu X M. Robust tube-based model predictive control for lane change maneuver of tractor-trailer vehicles based on a polynomial trajectory. IEEE Transactions on Systems, Man, and Cybernetics: Systems, 2020, 50(12): 5180−5188 doi: 10.1109/TSMC.2018.2867807 [14] Zhou Y, Cholette M E, Bhaskar A, Chung E. Optimal vehicle trajectory planning with control constraints and recursive implementation for automated on-ramp merging. IEEE Transactions on Intelligent Transportation Systems, 2019, 20(9): 3409−3420 doi: 10.1109/TITS.2018.2874234 [15] Liu C, Lee S, Varnhagen S, Tseng H E. Path planning for autonomous vehicles using model predictive control. In: Proceedings of the IEEE Intelligent Vehicles Symposium (IV). Los Angeles, USA: IEEE, 2017. 174−179 [16] Plessen M G, Lima P F, Mårtensson J, Bemporad A, Wahlberg B. Trajectory planning under vehicle dimension constraints using sequential linear programming. In: Proceedings of the IEEE 20th International Conference on Intelligent Transportation Systems (ITSC). Yokohama, Japan: IEEE, 2017. 1−6 [17] Werling M, Kammel S, Ziegler J, Gröll L. Optimal trajectories for time-critical street scenarios using discretized terminal manifolds. The International Journal of Robotics Research, 2012, 31(3): 346−359 doi: 10.1177/0278364911423042 [18] Zhan W, Chen J Y, Chan C Y, Liu C L, Tomizuka M. Spatially-partitioned environmental representation and planning architecture for on-road autonomous driving. In: Proceedings of the IEEE Intelligent Vehicles Symposium (IV). Los Angeles, USA: IEEE, 2017. 632−639 [19] Kant K, Zucker S W. Toward efficient trajectory planning: The path-velocity decomposition. The International Journal of Robotics Research, 1986, 5(3): 72−89 doi: 10.1177/027836498600500304 [20] Gu T Y, Atwood J, Dong C Y, Dolan J M, Lee J W. Tunable and stable real-time trajectory planning for urban autonomous driving. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Hamburg, Germany: IEEE, 2015. 250−256 [21] González D, Milanés V, Pérez J, Nashashibi F. Speed profile generation based on quintic Bézier curves for enhanced passenger comfort. In: Proceedings of the 19th IEEE International Conference on Intelligent Transportation Systems (ITSC). Rio de Janeiro, Brazil: IEEE, 2016. 814−819 [22] Lima P F, Trincavelli M, Mårtensson J, Wahlberg B. Clothoid-based speed profiler and control for autonomous driving. In: Proceedings of the 18th IEEE International Conference on Intelligent Transportation Systems. Gran Canaria, Spain: IEEE, 2015. 2194−2199 [23] Liu C L, Zhan W, Tomizuka M. Speed profile planning in dynamic environments via temporal optimization. In: Proceedings of the IEEE Intelligent Vehicles Symposium (IV). Los Angeles, USA: IEEE, 2017. 154−159 [24] Wang Y Y, Chardonnet J R, Merienne F. Speed profile optimization for enhanced passenger comfort: An optimal control approach. In: Proceedings of the 21st International Conference on Intelligent Transportation Systems (ITSC). Maui, USA: IEEE, 2018. 723−728 [25] Xu W D, Wei J Q, Dolan J M, Zhao H J, Zha H B. A real-time motion planner with trajectory optimization for autonomous vehicles. In: Proceedings of the IEEE International Conference on Robotics and Automation. Saint Paul, USA: IEEE, 2012. 2061−2067 [26] Fan H Y, Zhu F, Liu C C, Zhang L L, Zhuang L, Li D, et al. Baidu apollo EM motion planner. arXiv preprint arXiv: 1904.04671, 2019. [27] Zheng Z D. Recent developments and research needs in modeling lane changing. Transportation Research Part B: Methodological, 2014, 60: 16−32 doi: 10.1016/j.trb.2013.11.009 [28] 聂建强. 高速公路车辆自主性换道行为建模研究[博士学位论文], 东南大学, 中国, 2017.Nie Jian-Qiang. Research on Modeling Discretionary Lane-Changing Behaviore of Vehicles in Freeway [Ph.D. dissertation], Southeast University, China, 2017. [29] Shalev-Shwartz S, Shammah S, Shashua A. On a formal model of safe and scalable self-driving cars. arXiv preprint arXiv: 1708.06374, 2017. [30] 符锌砂, 胡嘉诚, 何石坚. 基于交通状况及行驶速度的高速公路换道时间研究. 公路交通科技, 2020, 37(4): 133−139Fu Xin-Sha, Hu Jia-Cheng, He Shi-Jian. Study on expressway lane-changing time based on traffic condition and driving speed. Journal of Highway and Transportation Research and Development, 2020, 37(4): 133−139 [31] Yang D, Zhu L L, Ran B, Pu Y, Hui P. Modeling and analysis of the lane-changing execution in longitudinal direction. IEEE Transactions on Intelligent Transportation Systems, 2016, 17(10): 2984−2992 doi: 10.1109/TITS.2016.2542109 [32] Toledo T, Zohar D. Modeling duration of lane changes. Transportation Research Record: Journal of the Transportation Research, 2007, 1999(1): 71−78 doi: 10.3141/1999-08 [33] Kawabata K, Ma L, Xue J R, Zheng N N. A path generation method for automated vehicles based on Bezier curve. In: Proceedings of the IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Wollongong, Australia: IEEE, 2013. 991−996 [34] Ziegler J, Bender P, Dang T, Stiller C. Trajectory planning for Bertha——A local, continuous method. In: Proceedings of the IEEE Intelligent Vehicles Symposium. Dearborn, USA: IEEE, 2014. 450−457 [35] Li L, Wang X, Wang K F, Lin Y L, Xin J M, Chen L, et al. Parallel testing of vehicle intelligence via virtual-real interaction. Science Robotics, 2019, 4(28): Article No. eaaw4106 doi: 10.1126/scirobotics.aaw4106 [36] Wang F Y, Zheng N N, Li L, Xin J M, Wang X, Xu L H, et al. China's 12-year quest of autonomous vehicular intelligence: The intelligent vehicles future challenge program. IEEE Intelligent Transportation Systems Magazine, 2021, 13(2): 6−19 doi: 10.1109/MITS.2021.3058623 期刊类型引用(1)
1. 关海杰，王博洋，龚建伟，陈慧岩. 面向异构车辆的统一运动规划方法. 机械工程学报. 2024(18): 288-298 . 
百度学术其他类型引用(0)
-
下载:
计量
- 文章访问数: 1024
- HTML全文浏览量: 627
- PDF下载量: 209
- 被引次数: 1
