CN112965476B - 一种基于多窗口模型的高速无人车轨迹规划系统及方法 - Google Patents

Abstract

本发明公开了一种基于多窗口抽样的高速无人车轨迹规划系统及方法。该方法使用多窗口模型来表征无人车运动规划的搜索空间，结合人类驾驶经验采用先速度规划后路径规划的策略以构建轨迹生成式模型，使生成的候选轨迹具备较强的动态障碍物的处理能力且符合人类驾驶习惯。最后，由代价函数选择代价最小的轨迹作为规划的最优轨迹。该无人车轨迹规划方法适用于以动态障碍物为主的高速等结构化场景，对于无人车在实际环境中通行能力的提升有重要作用，可广泛应用于无人驾驶汽车运动规划、高级辅助驾驶技术等领域。

Description

一种基于多窗口模型的高速无人车轨迹规划系统及方法

技术领域

本发明属于运动决策与规划领域，一种基于多窗口模型的高速无人车轨迹规划系统及方法。

背景技术

轨迹规划是自动驾驶系统中非常关键且核心的一项技术，对车辆正常行驶、功能实现有着至关重要的作用。而高速场景以其结构化程度高、行为约束性较强的特点有望成为首个实现完全自动驾驶的应用场景，因此如何在高速场景中灵活应对动态车流、生成一条安全无碰撞、高效可执行的行驶轨迹已经成为热门研究领域，高速场景下的轨迹规划对于自动驾驶技术的应用落地以及高级辅助驾驶技术的提升有着重要意义。

现阶段轨迹规划前通常先由决策确定轨迹规划的候选区域，再根据目标函数生成最优轨迹。这种方式可以将轨迹规划问题转化为凸优化问题便于求解，但对于决策的依赖性较强，决策失误将直接导致规划的候选区域错误从而失去规划的最优性。而轨迹规划根据轨迹的生成方式可以分为两类：一类是通过优化或搜索的方式在无碰撞的自由空间中寻找符合要求的行驶轨迹。在高速场景中，求解空间通常较为庞大，使得这类方法的计算复杂度较高，此外最终规划结果对时空间的离散化程度也有较强依赖性。近年来备受关注的模型预测控制法在每个采样时刻通过更长时域的运动预测来计算最优动作序列，但仅应用第一个动作序列。MPC算法的重新规划能力虽然强，但必须有系统的精确模型做基础。

另一类轨迹规划的方法是通过将轨迹解耦为横-纵向轨迹或路径-速度二元组进行求解以降低求解空间的维度，从而提高计算效率。在时空间状态栅格图中以图搜索的方式分别求解横纵向轨迹同样受限于时空间的离散化程度。而先进行路径规划再进行速度规划的方式在路径规划的时候难以对动态车辆做有效处理。为提升动态障碍物的处理能力，人工势场被引入进来，但由于人工势场的方法缺乏动态推理能力，仅在空间维度下进行风险评估，导致车辆总是试图通过绕行避障。此外，当考虑道路规则约束，将车道线的势场加入模型时又使得规划器很大程度上倾向于选择车道保持，使规划趋于保守，灵活性降低。

发明内容

本发明的目的在于提供一种基于多窗口模型的高速无人车轨迹规划系统及方法，以解决上述问题。

为实现上述目的，本发明采用以下技术方案：

一种基于多窗口模型的高速无人车轨迹规划系统，包括搜索空间建立模块、候选轨迹生成模块和最优轨迹选择模块；搜索空间建立模块、候选轨迹生成模块和最优轨迹选择模块依次连接；

搜索空间建立模块用于多窗口模型构建以及道路交通规则筛选；多窗口模型构建将无人车周围的可行驶区域以动态窗口的形式表征并获取各个窗口的特性；道路交通规则筛选根据车道线约束剔除不符合道路交通规则的窗口；

候选轨迹生成模块用于动态窗口的选择以及窗口内轨迹生成；动态窗口的选择由窗口特性及人类驾驶经验共同决定；窗口内轨迹生成分为速度规划和路径规划两步依次进行，先通过速度规划处理动态障碍物，为路径规划提供目标点参考信息，再使用参数曲线构造法生成路径；

最优轨迹选择模块用于对候选轨迹的代价评估；对候选轨迹的代价评估从安全性、平滑性、舒适性和高效性四个方面进行轨迹评估。

进一步的，一种基于多窗口模型的高速无人车轨迹规划系统的规划方法，包括以下步骤：

步骤1，构建多窗口模型以及道路交通规则筛选：多窗口模型构建将无人车周围的可行驶区域以动态窗口的形式表征并获取各个窗口的特性；根据车道线约束剔除不符合道路交通规则的窗口完成道路交通规则筛选；

步骤2，通过窗口特性及人类驾驶经验共同决定动态窗口的选择，然后窗口内轨迹生成分为速度规划和路径规划两步依次进行，先通过速度规划处理动态障碍物，为路径规划提供目标点参考信息，再使用参数曲线构造法生成路径；

步骤3，对候选轨迹的代价评估从安全性、平滑性、舒适性和高效性四个方面进行轨迹评估。

进一步的，步骤1中，多窗口模型将无人车周边的可行驶区域在纵向上以动态窗口的形式进行表征，假设所有车辆均遵守交通规则、后方车辆行驶速度小于等于前方车辆行驶速度且当前方车辆紧急制动时后方车辆会采取应对措施以保证安全，则该动态窗口同时定义在物理空间和速度空间上；采用Frenet坐标系进行描述和规划，用起止纵坐标以及容许行驶速度的范围来表征窗口的特性；在物理空间中该动态窗口的位置随时间变化，与构成窗口的前后车辆的位置和速度有关；在速度空间内，每个动态窗口都对应一个速度的可行区间，表示无人车在该动态窗口内行驶时应满足的速度要求。

进一步的，步骤1中，道路交通规则筛选是指在多窗口模型的基础上考虑道路交通规则，所有符合道路交通规则的窗口共同构成规划的搜索空间。

进一步的，步骤2中，动态窗口的选择由窗口特性及人类驾驶经验共同决定，具体过程如下：

在多窗口模型中，引入人类驾驶经验的有一定偏好性的抽样，确定人类驾驶员选择某个窗口的概率的两个因素：车辆进入窗口的难易程度以及进入窗口后行驶的高效性；其中，车辆进入窗口的难易程度体现在窗口位置以及进入窗口时为满足窗口速度要求所需要采取的最小速度变化量这两方面；车辆进入窗口后行驶的高效性主要体现在窗口容许的最高行驶速度；距离自身车辆较近、进入窗口时速度无需有较大调整、而进入后获得更高的行驶速度的窗口更易成为目标窗口；在S轴上建立以无人车纵坐标为中心的高斯分布，根据动态窗口前端与后端的s坐标计算出高斯分布在动态窗口范围内的累积概率，窗口距离无人车越远被选择的概率越低；考虑窗口容许行驶的最高速度以及进入该动态窗口至少应采取的速度变化量对累积概率进行加权；最后，对概率进行归一化使其满足所有可行窗口被选择的概率和为1。

进一步的，窗口内轨迹生成分为速度规划和路径规划两步依次进行，先通过速度规划处理动态障碍物，为路径规划提供目标点参考信息，再使用参数曲线构造法生成路径，具体过程如下：

在轨迹生成过程中先结合选择窗口的特性进行速度规划来处理动态障碍物对规划轨迹的影响；首先，由目标窗口的特性和无人车的加减速性能共同决定目标速度和加速度的概率密度分布，通过抽样得到目标速度v_t和加速度a_t；

在此基础上，参考人类驾驶员的平均换道距离来计算路径规划目标点的纵坐标；若目标窗口为当前道窗口，则对应车道保持路径，目标点纵坐标计算如下：

s_t＝s₀+max(L_acc,L_k) (6)

式中，L_acc为无人车以加速度a_t从当前速度v_ego到达目标速度v_t所行驶过的距离，L_k为车道保持规划的最短路径；当目标窗口位于旁道时，对应换道路径；假定无人车先达到符合窗口速度要求的目标速度v_t，再匀速换道进入目标窗口；参考人类驾驶员的平均换道距离，计算目标点的纵坐标如下：

s_t＝s_ego+L_acc+L_c(v_t) (7)

其中，L_c(v_t)为无人车以速度v_t匀速换道时的平均换道距离；

已知规划初始点纵坐标s_ego和目标点纵坐标s_t，将v-t速度曲线投影至s-t空间，通过二次规划来提高速度曲线的平滑性和舒适性；

在路径规划前，还需要确定目标点的横坐标d_t；车道保持时同时考虑到无人车在行驶时为与旁道机动车辆保持横向安全距离有时需要向车道内一侧避让，令目标点的横坐标服从多项分布d_t～PN(N＝3:P(d_t＝d_c-w),P(d_t＝d_c),P(d_t＝d_c+w))；其中，d_c为当前车道中线横坐标，w为常量参数，与车道宽度有关，分布中包含偏左、中间、偏右的三个横坐标；每次路径规划，从以上分布中随机抽样一个作为目标点横坐标；而对于换道场景，目标点均位于车道中线上；目标点的横、纵坐标共同确定了目标点的位置；

在高速场景中采用参数化曲线构造法进行路径规划满足路径平滑连续的需求，选择三阶贝塞尔曲线来进行路径规划；得到路径曲线后将速度曲线与规划路径进行时域关联可得候选轨迹集合。

进一步的，抽样得到目标速度v_t和加速度a_t具体过程为：

在目标速度抽样时，当目标窗口为当前所在车道的动态窗口，前方车辆对无人车的安全约束为首要约束；在当前道窗口内进行目标速度抽样时主要考虑窗口容许行驶的最大速度的制约；在抽样前，根据安全跟车距离对当前道窗口内容许行驶的最大速度进行修正；当跟车距离过大，通过提高窗口内抽样的最大速度v_max,0进行调整，缩短跟车距离；反之，则通过降低v_max,0来增大跟车距离；令目标速度v_t服从[0,v_max,0]区间内均值μ＝v_max,0的截断正态分布

式中，φ(·)表示标准正态分布，σ为截断正态分布的标准差，大小与自车速度成正比，s′为变量，计算公式如下：

为标准正态分布下[0,v_max,0]区间内的累积概率，由下式求得：

式中，Φ(·)为标准正态分布的累积分布函数；

若目标窗口为旁道内的动态窗口W_i，无人车的加减速性能约束与进入窗口的难易程度成为首要约束；较大的速度变化量通常意味着较大的加/减速度或较长时间的加/减速，在旁道内的动态窗口进行目标速度抽样时，令采样目标速度v_t服从[v_i-1,v_i]区间内均值μ＝v_ego的截断正态分布如下：

其中，σ的设定和s′的计算同上，

为标准正态分布下[v_i-1,v_i]区间内的累积概率，计算公式如下：

经过上述抽样过程可以得到目标速度v_t，根据速度的变化量来对目标加速度a_t进行抽样；设定加速度候选集a＝{-4,-2,-1.5,-0.7,0,0.5,1,1.5}；令目标加速度a_t服从多项分布a_t～PN(N＝8:P(a＝-4),…,P(a＝1.5))，速度变化量较大时加速度绝对值大的被抽样概率大，反之，速度变化量较小时加速度绝对值小的被抽样概率大；

至此，若无人车的运动为匀速运动和匀加速运动的组合，则根据自车当前速度、目标速度和加速度信息确定v-t速度曲线。

进一步的，其特征在于，

最优轨迹选择模块包括对候选轨迹的代价评估；对候选轨迹的代价评估从安全性、平滑性、舒适性和高效性四个方面进行轨迹评估，代价函数如下：

其中，C_safe(π)为安全性代价，通过无人车沿规划轨迹行驶时的安全概率来表征；C_smooth(π)为平滑性代价，通过偏航角速度来表征；C_acc(π)为加速度代价，用于提升轨迹舒适性；C_speed(π)为速度代价，通过目标窗口容许行驶的最高速度和轨迹的目标速度来表征高效性；选择代价最小的轨迹作为规划的最优轨迹。

与现有技术相比，本发明有以下技术效果：

本发明所述基于抽样多窗口模型的高速无人车轨迹规划方法通过搜索空间建立、候选轨迹生成和最优轨迹选择三个模块，实现了以动态障碍物为主的高速等结构化场景下的轨迹规划。通过多窗口模型来表征规划的搜索空间，缩小求解空间；将无人车性能及窗口特性与人类驾驶经验结合，提高了轨迹抽样的有效性和合理性；采取先速度规划后路径规划的方式提高了规划对动态障碍物的处理能力，使规划更加灵活；对速度曲线使用二次规划进行平滑以及采用参数曲线构造的方式来生成规划路径都极大提升了规划轨迹的平滑性和舒适性；从安全性、平滑性、舒适性和高效性四个方面进行轨迹评估，其中安全性评估中考虑了障碍机动车辆速度的不确定性，提高了规划轨迹的安全性以及对速度误差的容忍程度；最后代价最小的候选轨迹即为最优轨迹，同时轨迹所在目标窗口的位置决定了最优决策，保证了决策和规划的一致最优性。本发明所述方法切实有效，可广泛应用于无人驾驶汽车运动规划、智能车辅助驾驶等领域。

附图说明

图1整体框架示意图

图2多窗口模型

图3轨迹抽样流程图

图4动态窗口内的累积概率

图5无人车相对其他车辆的纵向安全概率

图6安全候选轨迹示意图

图7纵向减速避让规划轨迹结果示例图

图8横向减速避让规划轨迹结果示例图

图9换道超车规划轨迹结果示例图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

参见图1，为本发明的整体框架示意图，包括搜索空间建立、候选轨迹生成和最优轨迹选择三个模块；搜索空间建立模块、候选轨迹生成模块和最优轨迹选择模块依次连接；

(一)搜索空间建立模块

该模块包括多窗口模型构建以及道路交通规则筛选；多窗口模型构建将无人车周围的可行驶区域以动态窗口的形式表征并获取各个窗口的特性；道路交通规则筛选根据车道线约束剔除不符合道路交通规则的窗口；

由于机动车辆在车道横向方向上占用率较高且以独占形式为主，因而无人车周边的机动车辆将可行驶区域纵向上近似划分为一个个动态窗口。对于单向车道场景，我们可以采用双车道多窗口模型来描述双车道内影响无人车行驶的机动车辆，如图2所示。对于其他类型单向车道场景的描述均可由双车道多动态窗口模型衍生获得。例如，单一车道场景可以用旁道障碍数为无穷的双车道多动态窗口模型来描述，而多车道场景则可以用多个双车道多动态窗口模型的叠加来描述。图2中，n+1个障碍物将可行驶区域划分为n+2个窗口，用起止纵坐标以及容许行驶速度的范围来表征窗口的特性。在物理空间中该动态窗口的位置随时间变化，与构成窗口的前后车辆的位置和速度有关；在速度空间内，每个动态窗口都对应一个速度的可行区间，表示无人车在该动态窗口内行驶时应满足的速度要求。例如，对于由前后两辆车构成的动态窗口，无人车的行驶速度应当小于等于构成窗口的前方车辆的行驶速度且同时大于等于构成窗口的后方的车辆的速度。

在此基础上考虑车道线等道路交通规则约束，例如实线车道线不允许换道，所有符合道路交通规则的窗口共同构成规划的搜索空间。

(二)候选轨迹生成模块

候选轨迹生成模块包括动态窗口的选择以及窗口内轨迹生成；动态窗口的选择由窗口特性及人类驾驶经验共同决定；窗口内轨迹生成分为速度规划和路径规划两步依次进行，先通过速度规划处理动态障碍物，为路径规划提供目标点参考信息，再使用参数曲线构造法生成路径；两部分顺序连接，每生成一条候选轨迹，先选择一个候选窗口，再根据窗口特性抽样一条轨迹，整个抽样过程如图3所示。

1)动态窗口的选择

在多窗口模型中，由于窗口特性的不同因而最优轨迹的目标点落在各窗口内的概率也并不相同，因此动态窗口的选择策略并不是单纯的均匀抽样而是引入人类驾驶经验的有一定偏好性的抽样，以提高候选轨迹分布的合理性。在设计窗口选择策略时，通过对人类驾驶规律进行总结发现，人类驾驶员选择某个窗口的概率主要与两个因素有关：车辆进入窗口的难易程度以及进入窗口后行驶的高效性。其中，车辆进入窗口的难易程度主要体现在窗口位置以及进入窗口时为满足窗口速度要求所需要采取的最小速度变化量这两方面；车辆进入窗口后行驶的高效性主要体现在窗口容许的最高行驶速度。通常，距离自身车辆较近、进入窗口时速度无需有较大调整、而进入后可以获得更高的行驶速度的窗口更易成为目标窗口。

因此，我们在S轴上建立以无人车纵坐标s_ego为中心的高斯分布，根据动态窗口前端与后端的s坐标可以计算出高斯分布在动态窗口范围内的累积概率，如图4所示，窗口距离无人车越远被选择的概率越低。同时，考虑窗口容许行驶的最高速度以及进入该动态窗口至少应采取的速度变化量对累积概率进行加权。最后，对概率进行归一化使其满足所有可行窗口被选择的概率和为1。此外，窗口选择策略在后续的轨迹抽样过程中会根据抽样结果进行实时反馈调整。例如，在窗口W_i中抽样得到安全行驶轨迹的概率较低，则降低该窗口被选择的概率，同时增大其他可行窗口被选择的概率，继续生成候选轨迹至完成。

2)窗口内轨迹生成

在窗口内进行轨迹采样时，为了更加有效地处理动态障碍物对规划轨迹的影响，在轨迹生成过程中先结合选择窗口的特性进行速度规划来处理动态障碍物对规划轨迹的影响。首先，由目标窗口的特性和无人车的加减速性能共同决定目标速度和加速度的概率密度分布，通过抽样得到目标速度v_t和加速度a_t。具体过程为：

在目标速度抽样时，由于窗口位置的不同导致无人车所受的首要约束也不同。当目标窗口为当前所在车道的动态窗口，前方车辆对无人车的安全约束为首要约束。因此，在当前道窗口内进行目标速度抽样时主要考虑窗口容许行驶的最大速度的制约。在抽样前，根据安全跟车距离对当前道窗口内容许行驶的最大速度进行修正：

η_safe＝(s₀-s_ego)/d_min(v₀,v_ego) (1)

式中，s₀为当前道前方距离无人车最近车辆的纵坐标，d_min(v₀,v_ego)为无人车与前方车辆在纵向上的最短安全距离，由Mobileye提出的责任敏感安全(Responsibility-Sensitive Safety)模型计算得出。当η_safe≥1时无人车相对前方车辆是安全的，反之为不安全的。当安全系数过高时，说明跟车距离过大，可通过提高窗口内抽样的最大速度v_max,0进行调整，缩短跟车距离。反之，则通过降低v_max,0来增大跟车距离以提高车辆行驶的安全系数。令目标速度v_t服从[0,v_max,0]区间内均值μ＝v_max,0的截断正态分布

式中，φ(·)表示标准正态分布，σ为截断正态分布的标准差，大小与自车速度成正比，s′为变量，计算公式如下：

为标准正态分布下[0,v_max,0]区间内的累积概率，由下式求得：

式中，Φ(·)为标准正态分布的累积分布函数。

若目标窗口为旁道内的动态窗口W_i，无人车的加减速性能约束与进入窗口的难易程度成为首要约束。较大的速度变化量通常意味着较大的加/减速度或较长时间的加/减速，而这两种情况在换道时应予以避免。因此，在旁道内的动态窗口进行目标速度抽样时，令采样目标速度v_t服从[v_i-1,v_i]区间内均值μ＝v_ego的截断正态分布如下：

其中，σ的设定和s′的计算同上，

为标准正态分布下[v_i-1,v_i]区间内的累积概率，计算公式如下：

经过上述抽样过程可以得到目标速度v_t，接下来我们根据速度的变化量来对目标加速度a_t进行抽样。考虑到无人车的加减速性能，设定加速度候选集a＝{-4,-2,-1.5,-0.7,0,0.5,1,1.5}。令目标加速度a_t服从多项分布a_t～PN(N＝8:P(a＝-4),…,P(a＝1.5))，速度变化量较大时加速度绝对值大的被抽样概率大，反之，速度变化量较小时加速度绝对值小的被抽样概率大。

至此，若无人车的运动为匀速运动和匀加速运动的组合，则根据自车当前速度、目标速度和加速度信息即可确定v-t速度曲线。

在此基础上，参考人类驾驶员的平均换道距离来计算路径规划目标点的纵坐标。若目标窗口为当前道窗口，则对应车道保持路径，目标点纵坐标计算如下：

s_t＝s₀+max(L_acc,L_k) (8)

式中，L_acc为无人车以加速度a_t从当前速度v_ego到达目标速度v_t所行驶过的距离，L_k为车道保持规划的最短路径，避免规划距离过短。当目标窗口位于旁道时，对应换道路径。考虑控制的可执行性，假定无人车先达到符合窗口速度要求的目标速度v_t，再匀速换道进入目标窗口。参考人类驾驶员的平均换道距离，计算目标点的纵坐标如下：

s_t＝s_ego+L_acc+L_c(v_t) (9)

其中，L_c(v_t)为无人车以速度v_t匀速换道时的平均换道距离。

现在已知规划初始点纵坐标s_ego和目标点纵坐标s_t，可将v-t速度曲线投影至s-t空间，通过二次规划来提高速度曲线的平滑性和舒适性。优化后的速度曲线S可表述如下。S为五阶多项式曲线。

argminC_total(S)

subj.to S(0)＝s_ego,S(T)＝s_t (10)

a_min≤S″≤a_max

其中，a_min和a_max为最大减速度和最大加速度，C_total(S)为代价函数，计算公式如下：

其中，S_ref为优化前的速度曲线；ω₁,ω₂,ω₃分别为位置、速度、加速度的惩罚项系数。代价函数使优化曲线贴近初始规划曲线的同时改善速度曲线的连续性和可导性，进而提升曲线的平滑性和舒适性。

在路径规划前，还需要确定目标点的横坐标d_t。车道保持时同时考虑到无人车在行驶时为与旁道机动车辆保持横向安全距离有时需要向车道内一侧避让，因此我们令目标点的横坐标服从多项分布d_t～PN(N＝3:P(d_t＝d_c-w),P(d_t＝d_c),P(d_t＝d_c+w))。其中，d_c为当前车道中线横坐标，w为常量参数，与车道宽度有关，分布中包含偏左、中间、偏右的三个横坐标。每次路径规划，我们从以上分布中随机抽样一个作为目标点横坐标。而对于换道场景，我们希望换道结束后无人车行驶在道路中间，因此目标点均位于车道中线上。目标点的横、纵坐标共同确定了目标点的位置。

在高速场景中采用参数化曲线构造法进行路径规划可满足路径平滑连续的需求，我们选择三阶贝塞尔曲线来进行路径规划，主要原因有以下两点：(1)三阶贝塞尔曲线的解空间是二维的，且包含常见的无人车行驶路径的曲线形状，满足规划使用需求；(2)三阶贝塞尔曲线运算代价低，更易满足实时性要求。得到路径曲线后将速度曲线与规划路径进行时域关联可得候选轨迹集合。

(三)最优轨迹选择模块

最优轨迹选择模块包括对候选轨迹的代价评估；对候选轨迹的代价评估从安全性、平滑性、舒适性和高效性四个方面进行轨迹评估。其中，安全性通过无人车沿规划轨迹行驶时的安全概率来表征，平滑性通过偏航角速度来表征，舒适性通过加速度大小来表征，高效性通过目标窗口容许行驶的最高速度和轨迹的目标速度来表征。

对于候选轨迹集合，根据以下代价函数计算各候选轨迹的代价，选择代价最小的轨迹作为规划的最优轨迹。

1)安全性惩罚项

我们采用RSS模型对生成轨迹进行横、纵向安全判断。首先，需要确定在规划初始时刻无人车是否处于安全状态。若初始时刻横、纵向上均不满足RSS模型中的安全条件，则无人车处于不安全状态，合理的规划轨迹应在较短时间内使无人车逃离不安全处境，并在之后的规划时间内保持安全状态。而对于初始时刻无人车处于安全状态的情况，规划轨迹应保证在规划时间内最大限度地保持安全状态。

我们依据RSS模型中的横向安全准则对相邻车道的车辆进行横向安全判断。当两辆车之间的横向距离d_lat大于等于横向最短安全距离d_lat,min时，两辆车在横向上是安全的。同样地，我们依据纵向安全准则对同一车道内的前后两辆车进行纵向安全判断。当两辆车的纵向距离d_longi大于等于纵向最短安全距离d_longi,min时，两辆车在纵向上是安全的。

d_lat,min和d_longi,min的计算参见RSS模型。

由于障碍机动车辆的横向速度对结果影响较大，但实际物理系统中，感知系统对障碍机动车辆横向速度的测量不确定性较大。为避免测量噪声在安全检测时导致障碍车辆横向位置和速度预测与实际场景产生较大偏差，我们采用以下激活函数抑制测量噪声的影响。

当v_lat,m≤v_lat,min时，认为v_lat,m为噪声所致，应予以抑制；当v_lat,m＞v_thres时，认为v_lat,m为障碍机动车辆实际横向速度v_lat而非测量噪声。v_thres由传感器性能决定。

此外，考虑到车辆速度大小的测量误差以纵向速度误差为主，因此我们根据纵向速度误差分布建立高斯模型以估计障碍车辆经过时间t后的纵向位置。假设某障碍车辆c_i的实际纵向速度v_longi服从高斯分布，

而传感器测量误差范围为3σ_m，则实际速度v_longi∈[v_longi,m-3σ_m,v_longi,m+3σ_m]的概率约为1。

由高斯分布的性质可知，当以匀速运动模型来预测障碍机动车辆的运动时，车辆经过时间t后的纵坐标服从高斯分布

且不确定性随时间递增。对于同一车道内的前后两辆车，根据无人车t时刻轨迹点的纵坐标以及纵向安全距离可估计该时刻无人车与该障碍车辆不发生碰撞的概率，记为

如图5所示。

计算无人车与所有相关车辆之间不发生碰撞的概率，取其中最小值记为该时刻无人车行驶的纵向安全概率，即

设安全阈值为P_thres，若规划时间内存在某一时刻P_longi,safe(t)＜P_thres，则说明纵向安全性不足。

综上，轨迹判定为不安全的条件有两种：(1)对于相邻车道的车辆，无人车在沿规划轨迹行驶时存在横向安全条件不满足且同时纵向安全概率低于安全阈值的情况；(2)对于当前道前方距离无人车最近的车辆，无人车在当前道行驶时存在纵向安全概率低于安全阈值的情况。当轨迹为安全轨迹时，P_longi,safe(t)可以近似描述无人车沿轨迹行驶时安全概率P_safe(t)的变化，即P_safe(t)＝P_longi,safe(t)。反之，当轨迹在某一时刻不安全，P_safe(t)＝0，轨迹为不安全轨迹。因此，P_thres设定的大小也反映了规划策略的保守程度，P_thres越大，规划策略越保守，规划轨迹的安全性也越高。

C_smooth(π)作为安全惩罚项，用于衡量轨迹的安全程度，计算公式如下：

C_safe(π)＝ω_safe/P_safe,π (14)

式中，ω_safe为安全惩罚项系数，P_safe,π为候选轨迹的安全概率，计算公式如下，式中P_safe(t_i)为轨迹离散化后时刻的安全概率。安全概率越低，代价越大，以确保无人车行驶的安全性。

安全的候选轨迹的示意图如图6所示。

2)平滑性惩罚项

C_smooth(π)用来衡量曲线的平滑程度，计算方式如下：

式中，ω_yawr为惩罚项系数，ψ(t)为航向角，

为偏航角速度，C_smooth(π)惩罚短时间内航向角变化较大、转弯较急的轨迹序列，进而改善无人车运动的平滑性。对轨迹序列按以下公式进行等时间间隔采样，将轨迹离散化为包含N个轨迹点的序列。式中，h为采样时间间隔，T为规划时距。

则式(11)可近似为：

3)加速度惩罚项

C_acc(π)为加速度惩罚项，用于避免规划轨迹中不必要的加速度值，提高舒适性，计算公式如下：

式中，ω_acc为加速度惩罚项系数，

为无人车在t_i时刻的加速度。加速度绝对值越大、加/减速时间越长，代价越大，从而提升无人车行驶的舒适性。

4)速度惩罚项

C_speed(π)为速度惩罚项，计算公式如下：

C_speed(π)＝ω_s1·(v_MAX-v_allow)-ω_s2·v_t (20)

式中，ω_s1和ω_s2为速度惩罚项系数，v_allow为轨迹目标窗口容许行驶的最高速度。目标窗口容许行驶的最高速度越高，代价越小；轨迹的目标速度越高，代价越小，使无人车尽快完成驾驶任务。

综上，代价函数选择平滑、安全、高效、舒适的行驶轨迹作为最优轨迹。而最优轨迹目标点的位置也决定了决策结果，若目标点在当前道，决策为车道保持，若目标点在旁道，决策为车道变换。

规划结果如图7、8、9所示。图中红色轨迹为规划轨迹，其他为障碍机动车辆的预测轨迹。图7为跟车时与前车距离低于安全车距时的规划结果；图8为横向上与旁道车辆距离低于安全距离时的规划结果；图9为前方车辆行驶缓慢时规划的换道超车。

Claims (8)

1.一种基于多窗口模型的高速无人车轨迹规划系统，其特征在于，包括搜索空间建立模块、候选轨迹生成模块和最优轨迹选择模块；搜索空间建立模块、候选轨迹生成模块和最优轨迹选择模块依次连接；

搜索空间建立模块用于多窗口模型构建以及道路交通规则筛选；多窗口模型构建将无人车周围的可行驶区域以动态窗口的形式表征并获取各个窗口的特性；道路交通规则筛选根据车道线约束剔除不符合道路交通规则的窗口；

候选轨迹生成模块用于动态窗口的选择以及窗口内轨迹生成；动态窗口的选择由窗口特性及人类驾驶经验共同决定；窗口内轨迹生成分为速度规划和路径规划两步依次进行，先通过速度规划处理动态障碍物，为路径规划提供目标点参考信息，再使用参数曲线构造法生成路径；

最优轨迹选择模块用于对候选轨迹的代价评估；对候选轨迹的代价评估从安全性、平滑性、舒适性和高效性四个方面进行轨迹评估。

2.一种基于多窗口模型的高速无人车轨迹规划系统的规划方法，其特征在于，基于权利要求1所述的一种基于多窗口模型的高速无人车轨迹规划系统，包括以下步骤：

步骤1，构建多窗口模型以及道路交通规则筛选：多窗口模型构建将无人车周围的可行驶区域以动态窗口的形式表征并获取各个窗口的特性；根据车道线约束剔除不符合道路交通规则的窗口完成道路交通规则筛选；

步骤2，通过窗口特性及人类驾驶经验共同决定动态窗口的选择，然后窗口内轨迹生成分为速度规划和路径规划两步依次进行，先通过速度规划处理动态障碍物，为路径规划提供目标点参考信息，再使用参数曲线构造法生成路径；

步骤3，对候选轨迹的代价评估从安全性、平滑性、舒适性和高效性四个方面进行轨迹评估。

3.根据权利要求2所述的一种基于多窗口模型的高速无人车轨迹规划系统的规划方法，其特征在于，步骤1中，多窗口模型将无人车周边的可行驶区域在纵向上以动态窗口的形式进行表征，假设所有车辆均遵守交通规则、后方车辆行驶速度小于等于前方车辆行驶速度且当前方车辆紧急制动时后方车辆会采取应对措施以保证安全，则该动态窗口同时定义在物理空间和速度空间上；采用Frenet坐标系进行描述和规划，用起止纵坐标以及容许行驶速度的范围来表征窗口的特性；在物理空间中该动态窗口的位置随时间变化，与构成窗口的前后车辆的位置和速度有关；在速度空间内，每个动态窗口都对应一个速度的可行区间，表示无人车在该动态窗口内行驶时应满足的速度要求。

4.根据权利要求2所述的一种基于多窗口模型的高速无人车轨迹规划系统的规划方法，其特征在于，步骤1中，道路交通规则筛选是指在多窗口模型的基础上考虑道路交通规则，所有符合道路交通规则的窗口共同构成规划的搜索空间。

5.根据权利要求2所述的一种基于多窗口模型的高速无人车轨迹规划系统的规划方法，其特征在于，步骤2中，动态窗口的选择由窗口特性及人类驾驶经验共同决定，具体过程如下：

在多窗口模型中，引入人类驾驶经验的有一定偏好性的抽样，确定人类驾驶员选择某个窗口的概率的两个因素：车辆进入窗口的难易程度以及进入窗口后行驶的高效性；其中，车辆进入窗口的难易程度体现在窗口位置以及进入窗口时为满足窗口速度要求所需要采取的最小速度变化量这两方面；车辆进入窗口后行驶的高效性主要体现在窗口容许的最高行驶速度；距离自身车辆较近、进入窗口时速度无需有较大调整、而进入后获得更高的行驶速度的窗口更易成为目标窗口；在S轴上建立以无人车纵坐标为中心的高斯分布，根据动态窗口前端与后端的s坐标计算出高斯分布在动态窗口范围内的累积概率，窗口距离无人车越远被选择的概率越低；考虑窗口容许行驶的最高速度以及进入该动态窗口至少应采取的速度变化量对累积概率进行加权；最后，对概率进行归一化使其满足所有可行窗口被选择的概率和为1。

6.根据权利要求2所述的一种基于多窗口模型的高速无人车轨迹规划系统的规划方法，其特征在于，窗口内轨迹生成分为速度规划和路径规划两步依次进行，先通过速度规划处理动态障碍物，为路径规划提供目标点参考信息，再使用参数曲线构造法生成路径，具体过程如下：

在轨迹生成过程中先结合选择窗口的特性进行速度规划来处理动态障碍物对规划轨迹的影响；首先，由目标窗口的特性和无人车的加减速性能共同决定目标速度和加速度的概率密度分布，通过抽样得到目标速度v_t和加速度a_t；

在此基础上，参考人类驾驶员的平均换道距离来计算路径规划目标点的纵坐标；若目标窗口为当前道窗口，则对应车道保持路径，目标点纵坐标计算如下：

s_t＝s₀+max(L_acc,L_k) (6)

式中，L_acc为无人车以加速度a_t从当前速度v_ego到达目标速度v_t所行驶过的距离，L_k为车道保持规划的最短路径；当目标窗口位于旁道时，对应换道路径；假定无人车先达到符合窗口速度要求的目标速度v_t，再匀速换道进入目标窗口；参考人类驾驶员的平均换道距离，计算目标点的纵坐标如下：

s_t＝s_ego+L_acc+L_c(v_t) (7)

其中，L_c(v_t)为无人车以速度v_t匀速换道时的平均换道距离；

已知规划初始点纵坐标s_ego和目标点纵坐标s_t，将v-t速度曲线投影至s-t空间，通过二次规划来提高速度曲线的平滑性和舒适性；

在路径规划前，还需要确定目标点的横坐标d_t；车道保持时同时考虑到无人车在行驶时为与旁道机动车辆保持横向安全距离有时需要向车道内一侧避让，令目标点的横坐标服从多项分布d_t～PN(N＝3:P(d_t＝d_c-w),P(d_t＝d_c),P(d_t＝d_c+w))；其中，d_c为当前车道中线横坐标，w为常量参数，与车道宽度有关，分布中包含偏左、中间、偏右的三个横坐标；每次路径规划，从以上分布中随机抽样一个作为目标点横坐标；而对于换道场景，目标点均位于车道中线上；目标点的横、纵坐标共同确定了目标点的位置；

在高速场景中采用参数化曲线构造法进行路径规划满足路径平滑连续的需求，选择三阶贝塞尔曲线来进行路径规划；得到路径曲线后将速度曲线与规划路径进行时域关联可得候选轨迹集合。

7.根据权利要求6所述的一种基于多窗口模型的高速无人车轨迹规划系统的规划方法，其特征在于，抽样得到目标速度v_t和加速度a_t具体过程为：

在目标速度抽样时，当目标窗口为当前所在车道的动态窗口，前方车辆对无人车的安全约束为首要约束；在当前道窗口内进行目标速度抽样时主要考虑窗口容许行驶的最大速度的制约；在抽样前，根据安全跟车距离对当前道窗口内容许行驶的最大速度进行修正；当跟车距离过大，通过提高窗口内抽样的最大速度v_max,0进行调整，缩短跟车距离；反之，则通过降低v_max,0来增大跟车距离；令目标速度v_t服从[0,v_max,0]区间内均值μ＝v_max,0的截断正态分布

式中，φ(·)表示标准正态分布，σ为截断正态分布的标准差，大小与自车速度成正比，s′为变量，计算公式如下：

为标准正态分布下[0,v_max,0]区间内的累积概率，由下式求得：

式中，Φ(·)为标准正态分布的累积分布函数；

若目标窗口为旁道内的动态窗口W_i，无人车的加减速性能约束与进入窗口的难易程度成为首要约束；较大的速度变化量通常意味着较大的加/减速度或较长时间的加/减速，在旁道内的动态窗口进行目标速度抽样时，令采样目标速度v_t服从[v_i-1,v_i]区间内均值μ＝v_ego的截断正态分布如下：

其中，σ的设定和s′的计算同上，

为标准正态分布下[v_i-1,v_i]区间内的累积概率，计算公式如下：

经过上述抽样过程可以得到目标速度v_t，根据速度的变化量来对目标加速度a_t进行抽样；设定加速度候选集a＝{-4,-2,-1.5,-0.7,0,0.5,1,1.5}；令目标加速度a_t服从多项分布a_t～PN(N＝8:P(a＝-4),…,P(a＝1.5))，速度变化量较大时加速度绝对值大的被抽样概率大，反之，速度变化量较小时加速度绝对值小的被抽样概率大；

至此，若无人车的运动为匀速运动和匀加速运动的组合，则根据自车当前速度、目标速度和加速度信息确定v-t速度曲线。

8.根据权利要求2所述的一种基于多窗口模型的高速无人车轨迹规划系统的规划方法，其特征在于，

最优轨迹选择模块包括对候选轨迹的代价评估；对候选轨迹的代价评估从安全性、平滑性、舒适性和高效性四个方面进行轨迹评估，代价函数如下：

其中，C_safe(π)为安全性代价，通过无人车沿规划轨迹行驶时的安全概率来表征；C_smooth(π)为平滑性代价，通过偏航角速度来表征；C_acc(π)为加速度代价，用于提升轨迹舒适性；C_speed(π)为速度代价，通过目标窗口容许行驶的最高速度和轨迹的目标速度来表征高效性；选择代价最小的轨迹作为规划的最优轨迹。

Images