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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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

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

；

；

；

式中，

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

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

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

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

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

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

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

；

其中，

；

；

；

式中，

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

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

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

；

式中，

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

；

其中，

式中，

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

；

其中，

优选的是，自车在周围车辆

；

其中，

本发明的有益效果是：

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

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

附图说明

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

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

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

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

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

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

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

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

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

具体实施方式

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

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

上层：可行换道轨迹生成

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

一、路径规划

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

以车载导航或高精地图获取的航路点为输入，采用贝塞尔曲线在笛卡尔坐标系

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

已知车辆在自然坐标系下的位置和状态量，表示为

；

式中，

已知车辆在笛卡尔坐标系下对应的位置及状态信息

2）可行路径生成及优化

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

①换道轨迹终点区域选取

如图3所示换道场景，自车记为

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

沿车辆行驶方向的上边界记为

式中，

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

本发明中采用B样条曲线生成换道路径，采用该方法生成的换道路径可保障曲率连续且换道起点和终点处曲率与参考路径保持一致。设有

式中，

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

式中，

本实施例中采用4阶准均匀B样条曲线生成换道路径如图5所示。4阶准均匀B样条曲线根据换道起点和终点确定6个控制点，确定图5中点

车辆换道开始时刻所在位置定义为换道起点

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

3）静态障碍物碰撞检测

本发明中采用基于动态圆包络方法进行车辆碰撞检测。采用

和/>

其中，

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

式中，

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

二、速度规划

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

①周围车辆轨迹预测

针对换道场景，考虑换道时间通常较短，选择自然坐标系下恒定加速度模型（CA）用于预测周围车辆的运动轨迹。该模型中车辆的状态变量包括纵坐标

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

记采样周期为

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

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

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

式中，

若对于周围车辆

③建立里程时间（ST）图

如图7所示，根据前述步骤判断自车所在车道前车

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

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

首先采用等间隔将ST图的横轴和纵轴分别离散化，则车速规划可转化为多阶段决策问题。如图9所示，横轴采用等间隔

为评估状态转移代价，每个阶段

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

其中，

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

；

式中，

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

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

式中，

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

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

采用五次多项式连接前述步骤中动态规划生成的ST曲线上相邻两个状态点

式中，

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

式中，

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

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

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

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

式中，

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

3）换道轨迹曲率检测

曲率检测考虑最小转向半径和防止车辆转向过程发生侧滑。首先，轨迹最小曲率对应半径

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

下层：最优换道轨迹决策

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

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

其中，

式中，

；

式中，

；

式中，

2）最优换道轨迹决策

按照前述步骤，针对可行换道轨迹集中任意换道轨迹，计算车辆沿该换道轨迹行驶对应的作用量

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

3）动态障碍物碰撞检测

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

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

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