CN117141489B - 一种基于最小作用量原理的智能车辆轨迹分层规划方法 - Google Patents

Abstract

本发明属于智能驾驶技术领域，公开了一种基于最小作用量原理的智能车辆轨迹分层规划方法，包括：在车辆换道过程中，设定换道轨迹起点和多个换道轨迹终点，确定所述换道轨迹起点到每一个所述换道轨迹终点的换道路径，得到换道轨迹集；基于周围车辆轨迹预测建立时间里程图；基于所述时间里程图，采用动态规划方法进行车辆速度规划，得到车辆的初始车速；基于二次规划方法对所述初始车速进行平滑处理，得到优化车速；基于行车场安全理论计算车辆按照所述优化车速沿换道轨迹集中的每条换道轨迹行驶对应的平均作用量，筛选出无碰撞并且对应的平均作用量最小换道轨迹作为规划结果。

Description

一种基于最小作用量原理的智能车辆轨迹分层规划方法

技术领域

本发明属于智能驾驶技术领域，特别涉及一种基于最小作用量原理的智能车辆轨迹分层规划方法。

背景技术

智能驾驶技术的发展对于增强驾驶安全、提升出行效率、缓解交通拥堵、提高驾乘舒适性具有重要意义。在智能车辆的诸多核心技术中，轨迹规划负责将驾驶行为决策序列转化为车辆控制器可执行的局部运动轨迹，是决定车辆行驶质量和保障车辆运行安全的关键环节。智能车辆轨迹规划需同时考虑空间位置、姿态和时间信息，在具有动态交通流和静态障碍物的环境中，以驾驶安全性、稳定性、舒适性、高效性等性能为目标，在满足车辆运动学和动力学特性、动态和静态障碍物、道路几何约束和交通法规等约束前提下，在线或离线的计算生成从起始点到目标状态的无碰撞轨迹，属于具有多约束的多目标优化问题。目前常用的局部轨迹规划方法可分为五类：图搜索法、采样法、几何曲线法、人工势场法和最优控制法。各种方法各具优势与不足，在实际应用中，很少独立使用某种方法解决复杂场景的轨迹规划问题，往往通过各类方法的组合，借助于两种或多种方法的优势来完成复杂场景下轨迹规划任务。通常最优轨迹决策采用代价函数评估所规划轨迹的优劣，选择代价最低的轨迹输出给控制层，但是代价函数需要考虑车辆偏离参考路径的距离、最大速度限制、乘坐舒适性、曲率变化率限制、轨迹规划一致性等因素，代价函数中各因素权重的分配主观性较强，在不同环境中车辆轨迹评估代价函数应具有不同的权重分配形式，进而导致设计具有环境普适性的代价函数难度较大。

专利CN111599179B公开了一种基于风险动态平衡的无信号交叉口车辆运动规划方法，该方案首先预测无信号交叉口内其它运动车辆行驶轨迹，据此建立无信号交叉口中随时空变化的动态风险场，然后基于无信号交叉口车辆期望轨迹分布模型得到自车期望轨迹分布，并计算不同期望轨迹对应的风险值，根据自动驾驶车辆可接受风险水平模型筛选得到小于可接受风险水平的自车轨迹分布，最后通过综合收益函数从可接受风险轨迹分布选择综合收益最高的轨迹。

该专利技术方案中，无信号交叉口内的综合动态风险场直接将交叉口中所有单独要素风险场模型进行叠加，并未根据周围车辆与自车之间的交互关系评估单独要素所产生的风险对自车行为造成影响的权重；同时，自动驾驶车辆的可接受风险水平仅仅考虑了信息延迟、感知误差和周围车辆轨迹预测误差等因素，而并未考虑自车定位、自车运动状态、与周车交互作用等计算符合驾驶人风险响应特性可接受风险水平的重要因素。

发明内容

本发明的目的是克服现有技术的缺陷，提供了一种基于最小作用量原理的智能车辆轨迹分层规划方法，其能够提高换道轨迹规划方法对复杂环境的适应能力，并且提高换道规划效率。

本发明提供的技术方案为：

一种基于最小作用量原理的智能车辆轨迹分层规划方法，包括：

在车辆换道过程中，设定换道轨迹起点和多个换道轨迹终点，确定所述换道轨迹起点到每一个所述换道轨迹终点的换道路径，得到换道轨迹集；

基于周围车辆轨迹预测建立时间里程图；基于所述时间里程图，采用动态规划方法进行车辆速度规划，得到车辆的初始车速；基于二次规划方法对所述初始车速进行平滑处理，得到优化车速；

基于行车场安全理论计算车辆按照所述优化车速沿换道轨迹集中的每条换道轨迹行驶对应的平均作用量，筛选出无碰撞并且对应的平均作用量最小换道轨迹作为规划结果。

优选的是，所述换道轨迹终点为换道轨迹终点区域中的任意一点；

其中，所述换道轨迹终点区域为矩形，换道轨迹终点区域的边界取值为：

；

；

；

式中，为换道轨迹终点区域沿车辆行驶方向的上边界与换道轨迹起点之间的距离，/>为换道轨迹终点区域沿车辆行驶方向的下边界与换道轨迹起点之间的距离；/>为换道轨迹终点区域与车道线垂直方向的上边界记与其最近的道路边界线之间的距离，为换道轨迹终点区域与车道线垂直方向的下边界与其最近的车道分界线之间的距离；为车辆与道路边界和车道分界线保持的最小安全距离；/>、/>为换道路径调节系数；/>表示自车车速，/>表示目标车道前车车速。

优选的是，得到所述换道轨迹集的方法包括：

采用B样条曲线生成换道路径生成所述换道轨迹起点到每一个所述换道轨迹终点的换道路径；

对生成的所述换道路径进行静态障碍物碰撞检测，剔除与静态障碍物发生碰撞的路径，将剩余的换道路径的集合作为所述换道轨迹集。

优选的是，采用4阶准均匀B样条曲线生成曲率连续变化的换道路径。

优选的是，在建立时间里程图过程中，还包括：

根据周围车辆预测轨迹和所述换道轨迹集中的换道路径，判断自车所在车道前车、换道目标车道前车和换道目标车道后车是否与自车换道存在潜在冲突；选择存在冲突的周围车辆，并且记录冲突开始时刻和冲突结束时刻；将换道路径与周围车辆存在潜在冲突的全部路点集合作为潜在冲突区域，并且将潜在冲突时长内所有的潜在冲突区域记录在所述时间里程图中。

优选的是，所述动态规划方法采用的代价函数为：

；

其中，；

；

；

；

式中，为动态规划中阶段/>对应的状态点，/>为每个阶段/>对应的速度、/>和为每个阶段/>对应的加速度和加加速度；/>表示自车与周围障碍车辆之间的相对距离；/>为超车过程自车与目标车道后车的最小安全间距；/>为跟车过程自车与目标车道前车的最小安全间距；/>和/>分别为阶段/>自车与周围障碍车辆的潜在冲突区域在时间里程图上的最小值和最大值；/>为动态规划方法采用的代价函数；/>为第一子目标函数、/>为第二子目标函数、/>为第三子目标函数；/>、/>为各子目标函数的权重；/>表示换道终点的里程位置；/>表示正无穷大；/>和/>分别表示加速度子目标函数和冲击度子目标函数的权重。

优选的是，在基于二次规划方法对所述初始车速进行平滑处理后，还包括：

对换道换道轨迹曲率检测，从所述换道轨迹集筛选出可行换道轨迹集；并且从所述可行换道轨迹集中筛选出无碰撞并且对应的平均作用量最小换道轨迹作为规划结果；

其中，所述可行换道轨迹集中的换道轨迹满足如下约束关系：

；

式中，表示换道轨迹曲率半径，/>表示规划车速，g表示重力加速度。

优选的是，换道轨迹行驶对应的平均作用量采用如下公式进行计算：

；

其中，，/>；

式中，为换道轨迹行驶对应的平均作用量；/>为车辆沿规划换道轨迹行驶对应作用量，/>为换道起始时刻；/>为换道结束时刻；/>为换道过程中的系统拉格朗日量；和/>分别代表车辆的动能和势能。

优选的是，车辆的势能的计算公式为：

；

其中，为自车周围车辆的数量，/>为自车的周围车辆/>的车速；/>为自车在周围车辆/>产生的风险场中受到的交互作用力；/>代表车辆的势能；/>表示自车车速。

优选的是，自车在周围车辆产生的风险场中受到的交互作用力通过如下公式进行计算：

；

其中，为周围车辆/>产生的风险场中自车所在位置点的场强；/>为自车质量；为自车所处位置的路面附着影响因子；/>为从周围车辆/>质心位置指向自车所在位置点的距离；/>为周围车辆/>的质量；/>为周围车辆/>对风险的影响因子，/>、/>和/>为常数；/>为自车与周围车辆/>的相对速度大小；/>为自车与周围车辆/>的运动速度夹角。

本发明的有益效果是：

（1）本发明可行轨迹生成采用路径与速度解耦的方式，路径生成采用4阶准均匀B样条曲线，可降低轨迹生成的难度，提高路径规划效率。

（2）本发明最优轨迹决策基于最小作用量原理建立了兼顾安全与高效的轨迹质量综合评价函数，可实现不同场景下智能车辆轨迹质量评估函数的客观表达，解决了现有智能车辆轨迹质量评估函数形式多样、权重分配主观性强、场景适应性差等问题。

附图说明

图1为本发明所述的基于最小作用量原理的智能车辆轨迹分层规划方法。

图2为本发明所述的笛卡尔坐标系与自然坐标系中车辆坐标变换原理示意图。

图3为本发明所述的换道避障场景示意图。

图4为本发明所述的换道轨迹终点区域示意图。

图5为本发明所述的基于4阶准均匀B样条曲线的换道路径。

图6为本发明所述的基于动态包络圆的行车辆碰撞检测方法示意图。

图7为本发明所述的车辆换道过程潜在冲突判断示意图。

图8为本发明所述的时间里程图。

图9为本发明所述的车速规划过程的示意图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

如图1所示，本发明提供了一种基于最小作用量原理的智能车辆轨迹分层规划方法，上层生成可行换道轨迹集，下层实现最优换道轨迹决策。可行轨迹集生成采用路径-速度解耦法分别生成路径和速度，首先采用B样条曲线生成换道路径集，并针对静态障碍物完成碰撞检测，剔除与静态障碍物发生碰撞的路径，然后对满足无碰撞要求的路径采用动态规划和二次规划进行速度规划，并对各条轨迹进行曲率检测，最终获得满足无碰撞和曲率要求的可行候选换道轨迹集；目标轨迹决策定义了基于最小作用量原理的轨迹评价函数，该评价函数综合考虑安全和高效，通过计算每条可行轨迹的平均作用量来对轨迹进行筛选，从中选择具有最小平均作用量的轨迹作为车辆轨迹跟踪控制目标。

上层：可行换道轨迹生成

为提高轨迹规划效率，本发明中采用路径与速度解耦方式生成可行换道轨迹。

一、路径规划

1）平滑参考路径生成及坐标变换

以车载导航或高精地图获取的航路点为输入，采用贝塞尔曲线在笛卡尔坐标系中生成平滑的局部参考路径，作为车辆行驶过程基准路径；然后以生成的平滑参考路径切线向量和法线向量建立自然坐标系，也称为Frenet坐标系，将车辆位置/>在平滑参考路径上投影点/>定义为自然坐标系原点，沿平滑参考路径切线向量/>方向为纵轴/>方向，沿平滑参考路径的法线向量/>方向为横轴/>方向，计算车辆及其他交通参与者在自然坐标系中的坐标。

如图2所示，笛卡尔坐标系与自然坐标系中车辆位置和状态信息可以进行相互转换。具体转换过程描述如下：

已知车辆在自然坐标系下的位置和状态量，表示为，计算车辆在笛卡尔坐标系下对应的位置及状态信息/>，可按照如下公式进行转换。

；

式中，为车辆在自然坐标系下的纵坐标；/>为车辆在自然坐标系下纵坐标对时间的导数，也即沿参考路径的速度；/>为车辆在自然坐标系下纵坐标对时间的二阶导数，也即沿参考路径的加速度；/>为车辆在自然坐标系下的横坐标；/>为车辆在自然坐标系下横坐标对时间的导数，也即横向速度；/>为车辆在自然坐标系下横坐标对时间的二阶导数，也即横向加速度；/>为车辆在自然坐标系下横向坐标对纵向坐标/>的导数；/>为车辆在自然坐标系下横向坐标对纵向坐标/>的二阶导数；/>为车辆在笛卡尔坐标系下的坐标，是一个向量；/>为车辆在笛卡尔坐标系下的车辆航向角；/>为笛卡尔坐标系下车辆在参考路径上投影点的切线角；/>为笛卡尔坐标系下车辆实际路径曲率；/>为笛卡尔坐标系下车辆在参考路径上投影点/>的曲率；/>为笛卡尔坐标系下的车速，/>；/>为笛卡尔坐标系下的加速度，/>，/>表示笛卡尔坐标系下车辆在参考路径上投影点/>的曲率变化率。图2中，/>定义为/>点处车辆实际行驶路径上的切线向量；/>定义为/>点处车辆实际行驶路径上的法线向量；/>定义为车辆在实际行驶路径上的行驶距离变化量；/>定义为车辆在平滑参考路径上的行驶距离变化量也是/>在平滑参考路径上的投影。

已知车辆在笛卡尔坐标系下对应的位置及状态信息，计算车辆在自然坐标系下的位置和状态信息/>，可按照如下公式进行转换。

2）可行路径生成及优化

可行路径在自然坐标系下生成，然后通过前述坐标变换到笛卡尔坐标系中。

①换道轨迹终点区域选取

如图3所示换道场景，自车记为，车速记为/>，换道过程考虑三辆周车对自车的影响，分别为自车所在车道前车/>、换道目标车道前车/>和换道目标车道后车/>,车速分别记为/>、/>和/>。换道开始时刻记为/>，换道结束时刻记为/>。

如图4所示，换道终点分布区域为矩形，该区域内圆点为可能的换道终点。该区域边界定义如下：

沿车辆行驶方向的上边界记为，沿车辆行驶方向的下边界记为/>；与车道线垂直方向的上边界记为/>，与车道线垂直方向的下边界记为/>。各边界的取值如下：

式中，、/>为换道路径调节系数，与驾驶人风格类型有关，激进型驾驶员对应取值小于保守型驾驶员，通常/>，/>；/>为车辆与道路边界和车道分界线保持的最小安全距离，通常/>；/>的单位为m/s。

②基于几何曲线的路径生成

本发明中采用B样条曲线生成换道路径，采用该方法生成的换道路径可保障曲率连续且换道起点和终点处曲率与参考路径保持一致。设有共/>个控制点，这些控制点用于定义样条曲线的走向、界限范围，则/>阶B样条曲线的定义为：

式中，为根据/>个控制点，采用/>阶B样条曲线计算得到的路点位置；/>是第/>个/>阶B样条基函数，与控制点/>相对应，/>，/>是自变量。

基函数具有如下德布尔-考克斯递推式：

式中，是一组被称为节点矢量的非递减序列的连续变化值，首末值一般定义为0和1，即/>，/>，该序列为/>，且满足/>；/>为第/>个/>阶B样条基函数。

本实施例中采用4阶准均匀B样条曲线生成换道路径如图5所示。4阶准均匀B样条曲线根据换道起点和终点确定6个控制点，确定图5中点，和/>，就可生成曲率连续的换道路径。为保证换道起点和换道终点曲率为零，根据换道起点和终点选取控制点的方法如下：

车辆换道开始时刻所在位置定义为换道起点，前述步骤中确定的换道轨迹终点区域任选一点作为换道终点/>。据样条曲线特性可知，当A、B、C三点以及D、E、F三点分别在与车道中心线平行的同一直线上时，即ABC//DEF//车道中心线，可以实现换道起点和换道终点曲率为零。为保证生成的换道路径呈中心对称，则要求ABC和DEF呈现中心对称分布。因此，只需要确定B和C点的位置，即可生成一条B样条曲线换道路径。为确保换道起点A和换道终点F之间生成的换道路径最优，可采用路径长度和平均曲率两个指标来评估路径质量，路径长度体现了换道过程时间特征，平均曲率则体现了换道过程车辆横向控制的舒适性。采用线性加权方式综合两个指标作为优化目标，通过优化算法即可确定B和C点的位置，进而实现根据换道起点和终点选取6个控制点。此外，为简化优化过程，令AB=AC，将二维优化问题转化为一维优化问题，提高优化路径搜索效率。鉴于路径长度和曲率在数量等级上的差异，在优化目标构建过程，分别将二者进行归一化处理后再通过加权求和得到综合优化目标函数。

采用同样的方式，针对前述步骤中确定的换道轨迹终点区域任选换道终点即可生成一条换道路径，如图4所示的换道路径集。

3）静态障碍物碰撞检测

本发明中采用基于动态圆包络方法进行车辆碰撞检测。采用个半径相同的圆均匀覆盖车辆轮廓最小外接矩形，包络圆的半径等于车宽的一半，/>表示车尾位置对应包络圆，/>表示车头位置对应的包络圆。

和/>的圆心坐标可根据车辆定位系统获取的车辆位置信息，以及车辆尺寸信息进行计算。将/>和/>的圆心坐标分别记为/>和/>，则车辆任意包络圆/>的圆心坐标计算式如下：

其中，为定义在区间/>上的自变量，且为整数；/>和/>为任意包括圆的圆心坐标。

基于动态圆形包络描述车辆轮廓的两车之间碰撞检测可以转化为判断覆盖自车的任意包络圆与覆盖周围其他车辆的任意包络圆是否存在重叠区域。两圆之间是否存在重叠可根据两圆心之间的距离判断。则如图6所示场景中，车辆1和车辆2之间的碰撞检测条件可以表示为：

式中，和/>分别表示车辆1和车辆2的任意包络圆的圆心/>坐标，/>和/>分别表示车辆1和车辆2的任意包络圆的圆心/>坐标，/>和/>分别表示车辆1和车辆2包络圆的半径，等于对应车辆车宽的一半。

此外，还需要满足车辆沿规划路点行驶过程始终保持在道路边界线内，可以通过覆盖车辆的任意包络圆与车道边界是否相交进行检测。

二、速度规划

1）基于轨迹预测建立时间里程图

①周围车辆轨迹预测

针对换道场景，考虑换道时间通常较短，选择自然坐标系下恒定加速度模型（CA）用于预测周围车辆的运动轨迹。该模型中车辆的状态变量包括纵坐标、横坐标/>、与/>轴夹角（逆时针方向为正）/>、速度/>，加速度/>，记为：

定义为车辆的状态变量集合；

记采样周期为，则恒定加速度模型对应的车辆状态转移方程为：

因此，根据前述车辆状态转移方程，已知换道初始时刻周围车辆初始状态，采用恒定加速度模型可以获得周围车辆在预测时域内的预测轨迹。

②换道路径的潜在冲突判断

根据周围车辆预测轨迹与前述步骤生成的换道路径是否存在重叠关系，判断换道路径的潜在冲突风险。若根据周围车辆预测轨迹与换道路径无重叠，则速度规划过程无需考虑该车的影响；反之，则需要合理规划换道路径对应车速避免与该车出现碰撞风险。换道路径与周围车辆的潜在冲突判断方法如下：

式中，为周围车辆/>的任意包络圆的圆心在自然坐标系中的坐标，前述步骤生成换道路径上任意点在自然坐标系中的坐标，/>和/>分别表示自车和车辆/>包络圆的半径，等于对应车辆车宽的一半。

若对于周围车辆预测轨迹上任意点与换道路径上任意点均满足上式，则周围车辆/>与自车无潜在冲突；反之，则周围车辆/>与自车存在潜在冲突。速度规划过程需要考虑该车的影响。

③建立里程时间（ST）图

如图7所示，根据前述步骤判断自车所在车道前车、换道目标车道前车/>和换道目标车道后车/>是否与自车换道存在潜在冲突，选择存在冲突的周围车辆，并记录冲突开始时刻/>和冲突结束时刻/>；图中X轴和Y轴分别为大地坐标系的横坐标轴和纵坐标轴，存在潜在冲突的任意时刻/>，换道路径与周围车辆存在潜在冲突的全部路点集合定义为潜在冲突区域/>。将潜在冲突时长内所有的冲突区域绘制在以时间为横轴，以里程/>为横轴的坐标内，形成图8所示的里程时间图，简记为ST图。

2）基于动态规划（DP）和二次规划（QP）的车速规划

①基于动态规划的初始车速生成

首先采用等间隔将ST图的横轴和纵轴分别离散化，则车速规划可转化为多阶段决策问题。如图9所示，横轴采用等间隔离散化得到的时间序列记为/>，其中/>为换道结束时间，纵轴采用等间隔/>离散化得到的时间序列记为，其中/>为换道结束点对应里程。

为评估状态转移代价，每个阶段状态/>对应的速度/>、加速度/>和加加速度按照下述进行计算：

动态规划问题代价函数定义如下：

其中，和/>为各子目标函数的权重，目标函数第一项/>（第一子目标函数）用于保证生成的速度曲线足够平滑，第二项/>（第二子目标函数）主要用于提高换道过程运行效率，确保车辆尽快到达换道终点位置，第三项/>（第三子目标函数）主要用于评价自车与周围障碍车辆的距离，确保规划轨迹的安全性。

第一项用于平滑的目标函数具体形式如下：

；

式中，和/>分别表示加速度子目标函数和冲击度子目标函数的权重。

第二项追求高效的目标函数具体形式如下：

第三项用于评估自车与障碍物之间距离的避障目标函数具体形式如下：

/>

式中，表示自车与周围障碍车辆之间的相对距离；/>为超车过程自车与目标车道后车的最小安全间距，与自车车速和质量有关，推荐取值范围50~100m；为跟车过程自车与目标车道前车的最小安全间距，与自车车速和质量有关，推荐取值范围50~100m；/>和/>分别为阶段/>，自车与周围障碍车辆的潜在冲突区域在ST图上的最小值和最大值（ST图上冲突区域投影在S轴上里程的最大值和最小值）；/>表示换道终点的里程位置；/>表示正无穷大。

通过动态规划生成的速度虽然具有最优性，但是因为离散化处理导致生成的ST曲线为折线，如图9中实线所示，该曲线斜率为车速，但该曲线斜率并不连续，还需要进一步对该ST曲线进行平滑处理。

②基于二次规划的车速平滑处理

采用五次多项式连接前述步骤中动态规划生成的ST曲线上相邻两个状态点和，对于由/>个状态点组成的离散ST曲线，则平滑后由/>段五次多项式曲线前后连接而成，每一段的多项式曲线表达式如下：

式中，为定义在区间/>上的自变量。/>、/>和/>为多项式系数。

采用前述步骤将车速平滑问题转化为五次多项式系数优化求解问题。该优化问题的目标函数定义式如下：

式中，分别为权重系数；/>分别为第/>段五次多项式曲线的开始和结束时间。由于前述步骤中动态规划生成的ST曲线在时间维度是等间隔采样，所以各段多项式曲线时间周期均相等，即/>。

该优化问题的约束条件如下：

相邻多项式曲线连接点的位置、速度和加速度等式约束：

第一段多项式曲线应满足动态规划生成的ST曲线的起点位置、速度和加速度等式约束：

平滑后ST曲线的位置、速度、加速度均需要满足相应最大限值约束：

式中，为最高车速，/>和/>为车辆最大制动减速度和最大驱动加速度。

综合目标函数和约束条件，求解二次规划问题即可生成平滑ST曲线，如图9中虚线所示。

3）换道轨迹曲率检测

曲率检测考虑最小转向半径和防止车辆转向过程发生侧滑。首先，轨迹最小曲率对应半径应大于车辆最小转向半径/>；其次，为防止转向过程发生侧滑，车辆转向过程向心力应不大于地面能提供的最大附着力，考虑车辆稳定性，车辆最大侧向加速度限制为0.4g，据此可以得到轨迹曲率半径/>与规划车速/>需满足如下约束关系。

通过换道轨迹曲率检测，选择满足最小转向半径限制和无侧滑发生的换道轨迹组成可行换道轨迹集。

下层：最优换道轨迹决策

1）基于行车安全场理论的轨迹作用量计算

按照最小作用量原理的定义，车辆沿规划换道轨迹行驶对应作用量定义为：

/>

其中，为车辆沿着规划换道轨迹进行换道过程的作用量；/>是从换道起始时刻；/>是换道结束时刻；/>为换道过程中的系统拉格朗日量；/>和/>分别代表车辆的动能和势能，其中/>为车辆行驶在由静止障碍物、运动障碍物产生的行车安全场中具有的安全势能总和。

式中，为自车质量，/>为自车沿规划换道轨迹行驶过程对应的车速。

；

式中，为自车周围其他交通车数量，针对图3所示场景，/>；/>为自车的周围车辆/>的车速；/>为自车在周围车辆/>产生的风险场中受到的交互作用力；/>表示自车的车速。

；

式中，为动态障碍物（周围车辆/>）产生的风险场中自车所在位置点的场强大小；/>为自车所处位置的路面附着影响因子其取值与路面附着系数有关，由路面和轮胎决定。当驱动轮的附着率/>小于等于路面附着系数/>时，/>；当驱动轮的附着率/>大于路面附着系数/>时，/>，且/>，/>的值越大，则/>取值越大；为从动态障碍物（周围车辆/>）质心位置指向自车所在位置点的距离；/>为动态障碍物（周围车辆/>）的质量；/>为动态障碍物（周围车辆/>）对风险大小的影响因子，其取值与运动障碍物尺寸有关，显然卡车比乘用车对应的/>取值更大，且/>，一般/>取值区间为[1，10]，例如：乘用车/>取值为1，卡车/>取值3；/>、/>和/>为常数, 一般/>，/>取值区间为[1，10]，/>取值区间为[0.01，0.1]；/>为自车与动态障碍物（周围车辆/>）的相对速度大小；/>为自车与动态障碍物（周围车辆/>）的运动速度夹角。

2）最优换道轨迹决策

按照前述步骤，针对可行换道轨迹集中任意换道轨迹，计算车辆沿该换道轨迹行驶对应的作用量，考虑不同换道轨迹的换道时间差异性，本发明中采用平均作用量用于评估换道轨迹质量，平均作用量定义为轨迹作用量与换道时间的比值，计算式如下：

根据换道轨迹平均作用量对可行换道轨迹集中轨迹进行排序，选择平均作用量最小的换道轨迹进行动态障碍物检测，若碰撞检测失败，则从可行换道轨迹集中删除该轨迹后继续选择平均作用量最小的候选轨迹进行碰撞检测，直至选择出无碰撞换道轨迹作为最终轨迹规划结果输出。

3）动态障碍物碰撞检测

动态障碍物碰撞检测是为了确保输出换道轨迹的安全性，主要针对自车与周围运动车辆进行碰撞检测，采用与静态障碍物碰撞检测相同的方法，依次检测自车沿平均作用量最小的换道轨迹行驶过程与周围车辆是否满足碰撞检测要求。

本发明提供的基于最小作用量原理的智能车辆轨迹分层规划方法，以实现高质量、高效率、场景适应性强的换道轨迹规划为目标；为降低轨迹规划问题求解难度，综合考虑各类别轨迹规划技术在不同场景下的优势，通过结合几何曲线、人工势场等轨迹规划方法，将时空轨迹规划问题解耦为路径和速度两个二维规划问题；为提高换道轨迹规划方法对复杂环境的适应能力，将行车安全场理论应用于最优轨迹决策过程，采用最小作用量原理建立拉格朗日量函数评估换道轨迹质量，实现不同场景下智能车辆轨迹规划过程中的代价函数的统一表达。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

Claims (7)

1.一种基于最小作用量原理的智能车辆轨迹分层规划方法，其特征在于，包括：

在车辆换道过程中，设定换道轨迹起点和多个换道轨迹终点，确定所述换道轨迹起点到每一个所述换道轨迹终点的换道路径，得到换道轨迹集；

基于周围车辆轨迹预测建立时间里程图；基于所述时间里程图，采用动态规划方法进行车辆速度规划，得到车辆的初始车速；基于二次规划方法对所述初始车速进行平滑处理，得到优化车速；

基于行车场安全理论计算车辆按照所述优化车速沿换道轨迹集中的每条换道轨迹行驶对应的平均作用量，筛选出无碰撞并且对应的平均作用量最小换道轨迹作为规划结果；

换道轨迹行驶对应的平均作用量采用如下公式进行计算：

；

其中，，/>；

式中，为换道轨迹行驶对应的平均作用量；/>为车辆沿规划换道轨迹行驶对应作用量，/>为换道起始时刻；/>为换道结束时刻；/>为换道过程中的系统拉格朗日量；/>和分别代表车辆的动能和势能；

车辆的势能的计算公式为：

；

其中，为自车周围车辆的数量，/>为自车的周围车辆/>的车速；/>为自车在周围车辆产生的风险场中受到的交互作用力；/>代表车辆的势能；/>表示自车车速；

自车在周围车辆产生的风险场中受到的交互作用力通过如下公式进行计算：

；

其中，为周围车辆/>产生的风险场中自车所在位置点的场强；/>为自车质量；/>为自车所处位置的路面附着影响因子；/>为从周围车辆/>质心位置指向自车所在位置点的距离；/>为周围车辆/>的质量；/>为周围车辆/>对风险的影响因子，/>、/>和/>为常数；为自车与周围车辆/>的相对速度大小；/>为自车与周围车辆/>的运动速度夹角。

2.根据权利要求1所述的基于最小作用量原理的智能车辆轨迹分层规划方法，其特征在于，所述换道轨迹终点为换道轨迹终点区域中的任意一点；

其中，所述换道轨迹终点区域为矩形，换道轨迹终点区域的边界取值为：

；

；

；

式中，为换道轨迹终点区域沿车辆行驶方向的上边界与换道轨迹起点之间的距离，为换道轨迹终点区域沿车辆行驶方向的下边界与换道轨迹起点之间的距离；/>为换道轨迹终点区域与车道线垂直方向的上边界记与其最近的道路边界线之间的距离，/>为换道轨迹终点区域与车道线垂直方向的下边界与其最近的车道分界线之间的距离；/>为车辆与道路边界和车道分界线保持的最小安全距离；/>、/>为换道路径调节系数；/>表示自车车速，/>表示目标车道前车车速。

3.根据权利要求2所述的基于最小作用量原理的智能车辆轨迹分层规划方法，其特征在于，得到所述换道轨迹集的方法包括：

采用B样条曲线生成换道路径生成所述换道轨迹起点到每一个所述换道轨迹终点的换道路径；

对生成的所述换道路径进行静态障碍物碰撞检测，剔除与静态障碍物发生碰撞的路径，将剩余的换道路径的集合作为所述换道轨迹集。

4.根据权利要求3所述的基于最小作用量原理的智能车辆轨迹分层规划方法，其特征在于，采用4阶准均匀B样条曲线生成曲率连续变化的换道路径。

5.根据权利要求3或4所述的基于最小作用量原理的智能车辆轨迹分层规划方法，其特征在于，在建立时间里程图过程中，还包括：

根据周围车辆预测轨迹和所述换道轨迹集中的换道路径，判断自车所在车道前车、换道目标车道前车和换道目标车道后车是否与自车换道存在潜在冲突；选择存在冲突的周围车辆，并且记录冲突开始时刻和冲突结束时刻；将换道路径与周围车辆存在潜在冲突的全部路点集合作为潜在冲突区域，并且将潜在冲突时长内所有的潜在冲突区域记录在所述时间里程图中。

6.根据权利要求5所述的基于最小作用量原理的智能车辆轨迹分层规划方法，其特征在于，所述动态规划方法采用的代价函数为：

；

其中，；

；

；

；

式中，为动态规划中阶段/>对应的状态点，/>为每个阶段/>对应的速度、/>和/>为每个阶段/>对应的加速度和加加速度；/>表示自车与周围障碍车辆之间的相对距离；为超车过程自车与目标车道后车的最小安全间距；/>为跟车过程自车与目标车道前车的最小安全间距；/>和/>分别为阶段/>自车与周围障碍车辆的潜在冲突区域在时间里程图上的最小值和最大值；/>为动态规划方法采用的代价函数；/>为第一子目标函数、/>为第二子目标函数、/>为第三子目标函数；/>、/>为各子目标函数的权重；/>表示换道终点的里程位置；/>表示正无穷大；/>和/>分别表示加速度子目标函数和冲击度子目标函数的权重。

7.根据权利要求6所述的基于最小作用量原理的智能车辆轨迹分层规划方法，其特征在于，在基于二次规划方法对所述初始车速进行平滑处理后，还包括：

对换道换道轨迹曲率检测，从所述换道轨迹集筛选出可行换道轨迹集；并且从所述可行换道轨迹集中筛选出无碰撞并且对应的平均作用量最小换道轨迹作为规划结果；

其中，所述可行换道轨迹集中的换道轨迹满足如下约束关系：

；

式中，表示换道轨迹曲率半径，/>表示规划车速，g表示重力加速度。