CN109375632B - 自动驾驶车辆实时轨迹规划方法 - Google Patents

Abstract

本发明公开了一种自动驾驶车辆实时轨迹规划方法，该方法包括：S1，实时获取自车的相关信息；S2，基于自车的相关信息，生成参考轨迹以及由参考轨迹确定的可行轨迹簇和可行轨迹簇中的每一条可行轨迹对应的速度；S3，根据可行轨迹和其相对应的速度，利用以安全性和高效性为驾驶目标的目标优化函数，计算每一条可行轨迹的作用量，并选择具有最小作用量的可行轨迹作为期望最优轨迹，并优化得到与期望最优轨迹对应的期望最优速度；目标优化函数根据最小作用量原理和等效力方法获得。本发明能够使自动驾驶车辆在未知环境条件中仿照驾驶人驾驶特性，能够实时根据周边车辆和环境信息，以安全性和高效性为驾驶目标规划出一条最符合驾驶人驾驶期望的轨迹。

Description

自动驾驶车辆实时轨迹规划方法

技术领域

本发明涉及一种自动驾驶领域，特别是关于一种自动驾驶车辆实时轨迹规划方法。

背景技术

智能车辆指在传统车辆的基础上安装传感器、控制器和执行器等，通过环境感知、人工智能和自动控制等技术实现人员货物运输能力的车辆。智能驾驶技术有助于改善车辆行驶的安全性和舒适性，受到了学术界和工业界的广泛关注。轨迹规划是无人驾驶领域的核心技术之一。车辆的轨迹规划指的是已知车辆的起始状态、目标状态和环境中的障碍物分布，规划出一条与障碍物不相碰撞的且满足车辆的运动学约束、环境约束和时间约束的可行驶轨迹。轨迹规划算法在移动机器人领域得到了广泛研究，衍生出许多经典的轨迹规划算法，包括人工势场法、可视图法、数学规划法等。考虑自动驾驶车辆的运动学约束、动力学约束和控制约束等，在结构化道路上，自动驾驶车辆的轨迹规划算法主要包括：基于采样点算法、基于最优化算法、固定轨迹类型和人工势场等算法。其中，人工势场法实时性好，便于实现，搜索轨迹具有启发信息，但却容易陷入局部最小而不能顺利到达最终目标位置。为解决陷入局部最优的问题，Vadakkepat等人提出了一种陷入局部最优后的逃逸策略。Mei等人将蚁群算法和人工势场法相结合，通过蚁群算法规划全局轨迹，通过人工势场法优化局部轨迹。但同时又增加了调参难度和搜索算法收敛性的保证难度。

基于模糊逻辑的轨迹规划方法参考人的驾驶经验，通过查表的方法，实现实时局部轨迹规划。这种方法通过规划体上装配的感应器来分辨障碍物，克服了其它方法的缺点，在动态变化的未知环境中能够进行实时规划。该方法最大的优点是实时性非常好，但是模糊隶属函数的设计、模糊控制规则的制定主要靠人的经验，如何得到最优的隶属函数以及控制规则是该方法最大的问题。近年来一些学者引入神经网络技术，提出一种模糊神经网络控制的方法，效果较好，但复杂度过高。

发明内容

本发明的目的在于提供一种自动驾驶车辆实时轨迹规划方法，其能够使自动驾驶车辆在未知环境条件中仿照驾驶人驾驶特性，能够实时根据周边车辆和环境信息，以安全性和高效性为驾驶目标规划出一条最符合驾驶人驾驶期望的轨迹。

为实现上述目的，本发明提供一种自动驾驶车辆实时轨迹规划方法，所述自动驾驶车辆实时轨迹规划方法包括以下步骤：

S1，实时获取自车及周边环境相关信息；

S2，基于所述自车及周边环境相关信息，生成参考轨迹以及由所述参考轨迹确定的可行轨迹簇和所述可行轨迹簇中的每一条可行轨迹对应的速度；

S3，根据所述可行轨迹和其相对应的速度，利用以安全性和高效性为驾驶目标的目标优化函数，计算每一条所述可行轨迹的作用量，并选择具有最小作用量的可行轨迹作为期望最优轨迹，并优化得到与所述期望最优轨迹对应的期望最优速度；所述目标优化函数根据最小作用量原理和等效力方法获得。

进一步地，S3中的所所述目标优化函数表示成式(1)：

式(1)中，s_Risk为所述可行轨迹的作用量，t₀为所述可行轨迹的采样初始端对应的时刻，t_f为所述可行轨迹的采样终端对应的时刻，n表示交通场景中道路使用者的使用数量，i为其它道路使用者的编号，j为自车的编号，m_j为自车j的质量，v_i为其它道路使用者i的速度，v_j为自车j的速度，R_j为自车j行车过程中的等效阻力，G_j为自车j受到道路产生的等效吸引力，v_j，x为自车j的沿x方向的速度，F_ji为其他道路使用者i对自车j之间的作用力。

进一步地，所述期望最优速度(v_j，x，v_j，y)由下面的方程组(2)计算得到：

式(2)中，v_j，x为自车j沿x轴方向的速度；v_j，y为自车j沿x轴方向的速度；v_i，x为其他道路使用者i沿x轴方向的速度；v_i，y为其他道路使用者i沿y轴方向的速度；F_ji，x为其他道路使用者i对自车j的作用力沿x轴方向的分量；F_ij，y为自车j对其他道路使用者i的作用力沿y轴方向的分量。

进一步地，S2具体包括如下步骤：

S21，生成和平滑参考轨迹；

S22，将所述参考轨迹由其生成的笛卡尔坐标系坐标转换到曲线坐标系中；

S23，在所述曲线坐标系中，通过沿s轴方向对所述参考轨迹进行不同的横向偏移，生成所述可行轨迹簇；

S24，根据道路边界约束条件、运动约束和交通法规限制，利用梯形线速度曲线生成对每一条所述可行轨迹对应的速度。

进一步地，S21的“生成所述参考轨迹”具体包括：

采取五次Bezier曲线生成所述参考轨迹，所述五次Bezier曲线的具体表达为式(3)：

P(t)＝(1-t)⁵P₀+5(1-t)⁴tP₁+10(1-t)³t²P₂+10(1-t)²t³P₃+5(1-t)t⁴P₄+t⁵P₅ (3)

式(3)中，P₀为所述Bezier曲线的第一个控制点，P₁为所述Bezier曲线的第二个控制点，P₂为所述Bezier曲线的第三个控制点，P₃为所述Bezier曲线的第四个控制点，P₄为所述Bezier曲线的第四个控制点，P₅为所述Bezier曲线的第六个控制点，P(t)为前6个控制点与Bezier基函数乘积之和，t为所述Bezier曲线的时间参数。

进一步地，S21的“平滑所述参考轨迹”具体包括：

自车的运行状态被分解为四维状态(x(t)，y(t)，θ(t)，k(t))，其中，x(t)为所述自车的横向位移，y(t)为所述自车的纵向位移，x(t)和y(t)由所述自车的相关信息获得；θ(t)为沿所述参考轨迹的航点的切线角，其表示为下面的式(4)；k(t)为笛卡尔坐标系下的沿所述参考轨迹的曲率，其表示为下面的式(5)：

进一步地，S24具体包括：

S241，确定所述梯形线速度曲线的采样初始端的速度v₀、中间速度v_r、采样终端的速度v_f、采样初始端的加速度a₀、采样终端的速度a_f和遍历时间t；

S242，采用梯形速度框架来生成积分速度曲线；

S243，基于所述参考轨迹生成所述可行轨迹簇；

S244，利用三次多项式平滑速度曲线，生成可行轨迹簇的速度。

进一步地，S241中，所述梯形线速度曲线的加速度a(t)的最大值表示为下面的式(6)，所述梯形线速度曲线的速度v(t)的最大值表示为下面的式(7)：

|a(t)|≤a_max (6)

|v(t)|≤min{v_max，1，v_max，2，v_max，3…} (7)

式(7)中a_max为由运动约束限定的最大允许加速度；v_max，1为由运动约束限定的最大允许速度；v_max，2为运动约束限定的车辆最大可行速度；v_max，3为由所述交通法规限定的最大可行速度。

进一步地，S243具体包括如下方法：

根据所述参考轨迹，在所述曲线坐标系中通过沿s轴方向对所述参考轨迹进行不同的横向偏移量l(s)，生成所述可行轨迹簇；所述可行轨迹簇中的每一条所述可行轨迹当前位置的状态表示为弧长s_i和横向偏移量l_i，在所述采样初始端的状态表示为弧长s₀和横向偏移量l₀，在所述采样终端的状态表示为弧长s_f和横向偏移量l_f；

S244具体包括如下方法：

S2441，生成轨迹的曲率通过多项式进行平滑，采用三次多项式(8)描述所述横向偏移量l(s)：

S2442，通过对所述横向偏移量l(s)进行一阶求导，得到式(9)：

S2443，通过对所述横向偏移量l(s)进行二阶求导，得到式(10)：

S2444，根据所述道路边界约束条件及车辆和所述参考轨迹之间航向角度差θ(s)形成的如下约束条件(11)，代入计算得到式(8)至式(10)中的未知参数a、b、c和d：

S2445，根据式(5)，将每一条所述可行轨迹的曲率由笛卡尔坐标系中转换到曲线坐标系中，得到曲线坐标系中的所述可行轨迹曲率k(s)表示为式(12)：

式(12)中，S_sgn＝sgn(1-l(s)k_b)，

k_b为当前生成的可行轨迹的曲率；

S2446，基于式(8)至式(10)及式(12)，求得曲线坐标系中的所述可行轨迹曲率k(s)，基于式(9)求得曲线坐标系中的所述可行轨迹的速度；

S2447，将S2446求得的曲线坐标系中的所述可行轨迹曲率k(s)和速度转换到笛卡尔坐标系中，得到S2中的所述可行轨迹及其速度。

本发明由于采取以上技术方案，其具有以下优点：1、本发明提出了在完成驾驶任务过程中，基于驾驶人驾驶特性对自动驾驶车辆进行输入，在始端到采样终端(出发地到目的地)过程中，将驾驶人行车过程中“趋利避害”的驾驶特性融入无人驾驶车辆的决策层中，运用驾驶人操纵思维来控制底端，保证自动驾驶车辆在复杂多变的交通环境下顺利完成驾驶任务。2、本发明将自然界中力学系统的物理特性与交通系统固有属性联系，将自然界“寻优”目的与驾驶人“趋利避害”特性结合，提出基于最小作用量的轨迹规划算法，使自动驾驶车辆在驾驶过程中更符合类人驾驶风格，驾驶更平稳高效。3、本发明提出的实时轨迹规划算法综合考虑客观环境和周边障碍物(动态、静态)，并不局限于单一场景或静态障碍物，能够为复杂环境下保证驾驶安全，适用面更广。4、本发明提出的实时轨迹规划算法中的代价函数不同于已有大多数的轨迹规划算法，已有算法对于定义的代价函数或目标优化函数中，含有的对于不同目标的权重的调整，通常各个代价函数的权重是通过预定义完成的，有些研究也考虑用机器学习方法进行不断迭代从而寻找最优值，但整个调参过程繁琐且融入研究者过多主观因素，而在本发明中通过自动驾驶车辆仿照人类驾驶人操纵机制建立目标优化函数更准确反映驾驶过程的寻优过程，可以有效地从所有可行轨迹簇中选择一条最优局部轨迹，并对其进行速度生成，能够为复杂环境下自动驾驶车辆实时轨迹规划提供新方法、新系统。

附图说明

图1是本发明实施例提供的实时轨迹规划流程示意图；

图2是本发明实施例提供的运动规划层示意图；

图3是本发明实施例提供的行车自动驾驶车辆前轮转向模型示意图；

图4是本发明实施例提供的Bezier曲线的控制线示意图；

图5是本发明实施例提供的参考轨迹生成示意图；

图6是本发明实施例提供的基础框架和可行轨迹簇示意图；

图7是本发明实施例提供的轨迹速度生成不同框架示意图；

图8是本发明具体实施例的跟车场景示意图；

图9是本发明具体实施例中的车辆i与车辆j之间作用力示意图；

图10是本发明的生成可行轨迹上的碰撞检查示意图。

具体实施方式

在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。

如图1所示，本实施例所提供的自动驾驶车辆实时轨迹规划方法包括以下步骤：

S1，实时获取自车及周边环境的相关信息。自车及周边环境的相关信息由环境感知模块1实时获取得到。自车及周边环境的相关信息具体包括环境感知信息和车辆位置与导航信息。其中的环境感知信息主要包括自车所处周边环境中障碍物信息、道路环境信息和车道线信息等，如LiDAR、雷达和相机等传感器实时提供周围环境的传感信息。车辆位置与导航信息包括自车和他车的位置和速度信息等，可以用GPS结合惯导来获取车辆的定位信息。

S2，基于所述自车及周边环境的相关信息，生成参考轨迹以及由所述参考轨迹确定的可行轨迹簇及其速度。“参考轨迹”指的是车辆行驶过程的基准轨迹，在本实施例中将道路中心线视为参考轨迹，车辆在行驶过程需要尽可能调整到参考轨迹上。“可行轨迹簇”包括若干条可行轨迹，“可行轨迹簇的速度”可以理解为每一条可行轨迹对应的速度。可行轨迹簇及其速度由可行轨迹簇生成模块2得到，该获得方式将在下面实施例具体展开说明。

S3，根据所述可行轨迹和其相对应的速度，利用以安全性和高效性为驾驶目标的目标优化函数，计算每一条所述可行轨迹的作用量，并选择具有最小作用量的所述可行轨迹作为期望最优轨迹，并优化得到与所述期望最优轨迹对应的期望最优速度。所述目标优化函数是通过采用自动驾驶车辆仿照仿照驾驶人的决策规划方式，综合考虑安全性、高效性、舒适性和经济性，总结驾驶人操纵特性，提出最小作用量原理和等效力方法而获得。期望最优轨迹及期望最优速度由轨迹评估与优化模块3得到，该获得方式将在下面实施例具体展开说明。最优轨迹和最优速度可转换为低级制动器命令，由车辆执行器执行。

本实施例通过自动驾驶车辆仿照人类驾驶人操纵机制，采用基于最小作用量原理的函数来描述驾驶过程中对安全和高效的追求，建立更准确反映驾驶过程的寻优过程的目标优化函数，并将自动驾驶车辆轨迹规划过程中的目标优化函数进行统一化，从而有效地从所有可行轨迹簇中选择一条最优局部轨迹，并对其进行速度生成，进而有利于复杂环境下自动驾驶车辆实时轨迹规划。

在一个实施例中，在S2之前，还包括如下步骤S4：

S4，如图3所示，图3中的虚线的车辆是指车辆质心坐标位置一开始在原点处，实线车辆是指代转了一定角度后车辆所处位置。该步骤中采用下面的车辆运动微分方程(13)描述笛卡尔坐标系中的车辆运动学模型：

车辆运动微分方程(13)中，(x(t)，y(t))为车辆位置；v为车辆在θ方向的速度；θ是为车辆速度v的角度，是其在Yaw方向的偏转角度，它是相对于x轴的逆时针方向的角度；k为轨迹曲率；

为车辆的横向速度，

为车辆的纵向速度；

为车辆速度v的角度θ的导数。

S5，根据S4给出的车辆运动学模型，对车辆的轨迹曲率进行限制，生成的可行轨迹曲率k(s)应该同时满足以下I和II列出的两个条件，以确保车辆运动学模型所描述的车辆合理转向行为的物理可行性，并提高侧向跟踪控制的安全性和平滑性：

I.可行轨迹轨迹曲率k(s)连续，以保证车辆连续的前轮转向角；

II.可行轨迹轨迹曲率k(s)有界限，且该界限由自动驾驶车辆转向能力决定，以保证转向机构可执行该轨迹。

I和II所列的两个条件中，可行轨迹曲率k(s)指的是曲线坐标系中的曲率表达方式。

根据I和II，生成的可行轨迹曲率k(s)表达为式(13)：

k_min≤k(s)≤k_max (14)

式(14)中，s为可行轨迹当前位置处的弧长；k_min为可行轨迹的最小曲率；k_max为可行轨迹的最大曲率。式(14)仅作为车辆的可行轨迹曲率的约束条件，该界限由自动驾驶车辆转向能力决定，在本发明不做创新点。

根据生成的可行轨迹，在笛卡尔坐标系中用车辆运动微分方程(13)进行求解，在可行轨迹生成过程中，将整个求解过程分为空间轨迹生成和速度生成，因此，为了分步求解进行规划，将速度消去，采用曲率对车辆运动微分方程(13)进行微分约束，采用行驶距离而不是时间表示车辆运动，这样可行轨迹生成则可以通过变换自然地分解为空间轨迹生成和速度生成，于是车辆运动微分方程(13)表示为下面式(15)～式(18)：

v＝S_sgnQds/dt (15)

式(15)中，S_sgn＝sgn(1-l(s)k_b)，S_sgn为一个分段函数，可表示为：x>0的情形下，sgnx＝1；x＝0的情形下，sgnx＝0；x<0的情形下，sgnx＝-1；θ(s)为曲线坐标系中的车辆速度v的角度；

为曲线坐标系中的车辆的横向速度；

为曲线坐标系中的车辆的纵向速度；

为曲线坐标系中的车辆速度v的角度θ的导数；k(s)为曲线坐标系中的轨迹曲率。

在一个实施例中，S2具体包括如下步骤：

S21，根据生成和平滑所述参考轨迹。

S22，将所述参考轨迹由其生成的笛卡尔坐标系坐标转换到曲线坐标系中。该步骤是为了仿照复杂环境中的人类驾驶行为，将车辆轨迹解耦为横向运动和纵向运动，因此轨迹规划任务被分解为基于曲线坐标而不是笛卡尔坐标。在曲线坐标系框架下，沿道路中心线的参考轨迹弧长s为曲线坐标的s轴，横向偏移量l垂直于参考轨迹的为l轴。为了仿照复杂环境中的人类驾驶行为，车辆轨迹可以自然地解耦为横向运动和纵向运动。进而，轨迹规划任务被分解为基于曲线坐标而不是笛卡尔坐标。

S23，在所述曲线坐标系中，通过沿s轴方向对所述参考轨迹进行不同的横向偏移，生成所述可行轨迹簇。基于曲线坐标系，以参考轨迹为基准，对于可行轨迹簇采用与曲线坐标系的坐标轴对应的两个参数(弧长、横向偏移量)来表示生成轨迹的状态。在曲线坐标系中通过沿s轴方向对参考轨迹进行不同的横向偏移，从而生成一组可行轨迹簇。

S24，根据道路边界约束条件、运动约束和交通法规限制，利用梯形线速度曲线生成对每一条所述可行轨迹对应的速度。

在一个实施例中，如图4所示，在自动驾驶车辆行驶过程中，若不考虑曲率连续性问题，则会导致车辆行驶过程中需要车辆不时停止并重新规划其转向角度。Bezier曲线是能够描述复杂形状的曲线，其具体方法是：将代表曲线趋势走向的点首尾连接成多边形，然后通过Bezier公式逼近该多边形，从而得到Bezier曲线。其中表示曲线大体走向的点称为控制点，连接的多边形称为控制多边形。

定义n次Bezier曲线的n+1个控制点分别为P₀，P₁，…，P_n，则n+1阶(即n次)Bezier曲线的描述为式(19)：

式(19)中，P₀代表Bezier曲线的第一个控制点，P_i代表Bezier曲线的第i个控制点，P(t)是前i个控制点与Bezier基函数乘积之和，t代表Bezier曲线的时间参数，

代表n次Bezier基函数，n次Bezier基函数表达为式(20)：

S21的“生成所述参考轨迹”具体包括：

如图5所示，采取五次Bezier曲线生成所述参考轨迹，所述五次Bezier曲线的具体表达为式(3)：

P(t)＝(1-t)⁵P₀+5(1-4)⁴tP₁+10(1-t)³t²P₂+10(1-t)²t³P₃+5(1-t)t⁴P₄+t⁵P₅ (3)

式(3)中，P₀为所述Bezier曲线的第一个控制点，P₁为所述Bezier曲线的第二个控制点，P₂为所述Bezier曲线的第三个控制点，P₃为所述Bezier曲线的第四个控制点，P₄为所述Bezier曲线的第四个控制点，P₅为所述Bezier曲线的第六个控制点，P(t)为前6个控制点与Bezier基函数乘积之和，t为所述Bezier曲线的时间参数。

S21采取了五次Bezier曲线来生成参考轨迹，并对参考轨迹进行平滑处理，这样可以保证生成轨迹的局部平滑和曲率整体连续。

在一个实施例中，S21的“平滑所述参考轨迹”具体包括：

曲率在点P(t)＝(x(t)，y(t))处的Bezier曲线的轨迹曲率k(t)可以表示如下：

汽车在平面上作匀速运动，不考虑车辆沿z轴方向上下移动，用自然法来描述车辆的运动，从空间维度出发，车辆运行过程中存在(x(t)，y(t))方向的移动，状态转角由于地面限制条件考虑车辆参考速度方向φ，且为了确保轨迹跟随平滑，对生成轨迹也进行曲率(k)控制，即在结构化道路上，自车的运行状态被分解为四维状态(x(t)，y(t)，θ(t)，k(t))，其中，x(t)为所述自车的横向位移，y(t)为所述自车的纵向位移，x(t)和y(t)由所述自车的相关信息获得；θ(t)为沿所述参考轨迹的航点的切线角，其表示为下面的式(4)；k(t)为笛卡尔坐标系下轨迹的曲率，其表示为下面的式(5)：

式(4)和式(5)中，x指的是x(t)，y指的是y(t)。

如图6所示，基于曲线坐标系，以参考轨迹为基准，对于可行轨迹簇采用与曲线坐标系的坐标轴对应的两个参数来表示生成轨迹的状态。分别给定：生成轨迹当前位置处的弧长s_i和横向偏移量l_i，在生成轨迹的采样终端的弧长s_f和横向偏移量l_f，用于产生各种轨迹的设计参数。其中，采样终端状态下的纵向距离s_f决定了车辆调整到与参考轨迹对齐的速度。在本实施例的轨迹规划算法中，运用该算法生成的可行轨迹簇中每个轨迹都具有不同的横向偏移l_f，通过对参考参考轨迹进行横向偏移和平滑来完成可行轨迹曲线簇的生成。横向偏移改变的距离与车辆速度v主要影响横向偏移变化率。本实施例基于参考轨迹，在曲线坐标系中通过沿s轴方向对参考轨迹进行不同的横向偏移，从而生成一组可行轨迹簇。通过这种方式能够满足生成的所有可行轨迹都可以通过改变横向偏移量，从而达到对道路进行覆盖的目的，同时沿着曲线坐标生成能够更符合驾驶机动性，也可以通过对生成曲线进行微分方程约束从而满足道路条件约束和车辆动力学约束。在可行轨迹生成的过程中，暂时不考虑是否存在障碍物需要避障情况。

在一个实施例中，如图7所示，为了实现从初始横向偏移到采样终点偏移的平滑过渡，生成路径的曲率应该通过多项式进行平滑，对横向偏移量采用三次多项式曲线进行描述，并通过对横向偏移进行一阶求导和二阶求导从而计算路径的曲率。因此，S24具体包括：

S241，确定所述梯形线速度曲线的采样初始端的速度v₀、中间速度v_r、采样终端的速度v_f、采样初始端的加速度a₀、采样终端的速度a_f和遍历时间t；

S242，采用梯形速度框架来生成积分速度曲线；

S243，基于所述参考轨迹生成所述可行轨迹簇；

S244，利用三次多项式平滑速度曲线，生成可行轨迹簇的速度。

在一个实施例中，S241中，所述梯形线速度曲线的加速度a(t)的最大值表示为下面的式(6)，所述梯形线速度曲线的速度v(t)的最大值表示为下面的式(7)：

|a(t)|≤a_max (6)

|v(t)|≤min{v_max，1，v_max，2，v_max，3...} (7)

式(7)中a_max为由运动约束限定的最大允许加速度；v_max，1为由运动约束限定的最大允许速度；v_max，2为运动约束(加速度能力)限定的车辆最大可行速度；v_max，3为由所述交通法规限定的最大可行速度。

在一个实施例中，S243具体包括如下方法：

根据所述参考轨迹，在所述曲线坐标系中通过沿s轴方向对所述参考轨迹进行不同的横向偏移量l(s)，生成所述可行轨迹簇；所述可行轨迹簇中的每一条所述可行轨迹当前位置的状态表示为弧长s_i和横向偏移量l_i，在所述采样初始端的状态表示为弧长s₀和横向偏移量l₀，在所述采样终端的状态表示为弧长s_f和横向偏移量l_f；

S244具体包括如下方法：

S2441，生成轨迹的曲率通过多项式进行平滑，采用三次多项式(8)描述所述横向偏移量l(s)：

S2442，通过对所述横向偏移量l(s)进行一阶求导，得到式(9)：

S2443，通过对所述横向偏移量l(s)进行二阶求导，得到式(10)：

S2444，根据所述道路边界约束条件及车辆和所述参考轨迹之间航向角度差θ(s)形成的如下约束条件(11)，代入计算得到式(8)至式(10)中的未知参数a、b、c和d：

S2445，根据式(5)，将每一条所述可行轨迹的曲率由笛卡尔坐标系中转换到曲线坐标系中，得到曲线坐标系中的所述可行轨迹曲率k(s)表示为式(12)：

式(12)中，S_sgn＝sgn(1-l(s)k_b)，

k_b为当前生成的可行轨迹的曲率；

在该步骤中，当车辆的横向偏移量大于所述参考轨迹的曲率半径时，则可以推出1/k_b的曲率和所生成的可行轨迹的方向将与参考轨迹的符号相反，此时因为其不符合车辆运动约束，将其直接去除。同时，虽然对S_sgn定义为符号函数，但为了避免可行轨迹生成的奇异性，本实施例将设定(1-l(s)k_b)始终大于0。

S2446，基于式(8)至式(10)及式(12)，求得曲线坐标系中的所述可行轨迹曲率k(s)，基于式(9)求得曲线坐标系中的所述可行轨迹的速度；

S2447，将S2446求得的曲线坐标系中的所述可行轨迹曲率k(s)和速度转换到笛卡尔坐标系中，得到S2中的所述可行轨迹及其速度。

需要说明的是：文中，k(s)、k(t)、k表示的实际物理意义是一致的，都表示生成轨迹的曲率，k(s)为在曲线坐标系下基于弧长s的轨迹曲率；k(t)为在笛卡尔坐标系下t时刻的轨迹曲率；k轨迹曲率。

在一个实施例中，建立关于自车驾驶车辆追求安全和高效的目标优化函数可以转化为对寻找一条最小作用量S_min的可行轨迹。因此，本实施例将通过满足一组特定的方程组，实质上使作用量中的变化等于零，来最小化作用量。拉格朗日L是系统的动能T和系统的势能V之间的差值。拉格朗日L本身是位置和速度的函数，其可以使用位置速度和时间表示，但时间并不真正起主要作用，因为时间本质上是位移和速度的一部分。因此，时间隐含在这两个变量中。该作用量被定义为可行轨迹的采样初始端和采用终端之间的拉格朗日的积分：

其中，T表示交通系统的动能，V表示交通系统势能。

选择满足驾驶期望的最优轨迹。为了求得最优轨迹，以两车场景中障碍物客体位于自车同车道的情况下，障碍物客体可以为静止状态也可以为运动状态(此时可以表述为跟车场景)为例。

设定：自车的编号为j；质量为m_j；横向位移为x_j；纵向位移为y_i；速度为v_j，表示为下面公式中的

自车j周边的其他道路使用者的编号为i(假设为他车)，质量为m_i；横向位移为x_i；纵向位移为y_i；速度为v_i，表示为下面公式中的

其他道路使用者i静止时，v_i＝0。由自车j和他车i组成的交通系统的动能可以表述为：

同时，自车j在道路环境中行驶时，由于道路环境中还包含有其他道路使用者i，同时道路环境也时有变化。道路使用者(包括自车j和其他道路使用者i，其他道路使用者i具体包括车辆、行人、骑车人等)之间由于状态的不一致性，导致各道路使用者的状态将随着除自身以外的其他道路使用者状态的改变而改变，当某一道路使用者的行驶状态与交通流中的其他道路使用者有明显的较大区别时，交通扰动产生。因此当某一道路使用者的速度与周围其他道路使用者速度的差值大时，则其对交通流的扰动大，交通流对其的作用大，潜在风险大；反之则扰动小，交通流对其的作用小，潜在风险小。本实施例中，将交通扰动带来的潜在风险对应的拉格朗日量定义为：

其中,F_ij为其他道路使用者i对自车j造成的外在力，t₀和t_f分别表示驾驶过程的起始时刻和终止时刻。

相对来说，自车j势能的表达式需要考虑车辆之间受力关系及车辆与环境之间相互作用，基于已有的行车安全场理论，运动物体、静止物体、道路边界、交通标志等一切交通要素都会在交通环境中产生安全场。对自车j进行受力分析，自车j受到道路产生的等效吸引力G_j，行车过程中的等效阻力R_j，等效吸引力G_j代表行车过程中自车j的驾驶人对机动性的要求，即驾驶人受到环境造成的等效吸引力，其表达式为：

G_j＝m_jgsinθ_j

其中，m_j为自车j的质量；g为重力加速度；θ_j与自车j的驾驶人对行驶速度的追求有关，本发明中，定义θ_j满表达式为：

k为标定参数，本实施例中取k＝2。v_der是自车j的驾驶人期待理想速度，v_limit是交通法规限制的道路限速。

定义交通规则对自车j的驾驶人的约束阻力R_j满足下式：

其中，τ为校准参数，本实施例中取τ＝1；m_j为自车j的质量；θ_j为自车j速度的角度；g为重力加速度；v_j为自车j的速度；v_limit是交通法规限制的道路限速。

根据基于道路使用者运动关系的等效力分析方法，道路使用者之间的相互作用关系的表达式为：

其中，

为其他道路使用者i和自车j之间连线与其他道路使用者i的速度v_i的夹角，θ_ij为其他道路使用者i和自车j之间连线与其他道路使用者i和自车j之间的相对速度v_ij的夹角，d_ij为其他道路使用者i和自车j之间的直线距离。

因此，不考虑潜在风险的跟车过程中自车的拉格朗日量L_e可以描述为：

式中，t₀为驾驶过程的起始时刻，t_f为驾驶过程的的终止时刻，m_j为自车j的质量，v_j为自车j的速度，v_j，x为自车j的横向的速度，G_j为自车j受到道路产生的等效吸引力，R_j为行车过程中的等效阻力，F_ji为其他道路使用者i对自车j之间的作用力，F_ij＝F_ji道路使用者之间的相互作用力。

同时，根据拉格朗日函数的性质可知，系统的拉格朗日函数等于各独立部分的拉格朗日函数之和，因此本发明中有L＝L_e+L_p，同时，自车j在跟车场景中的系统拉格朗日量L可以描述为：

进一步，在一个有n个道路使用者的交通系统中，上式可写为：

上式表示了考虑了其他道路使用者对驾驶过程影响下的驾驶人-车辆单元的系统拉格朗日量。此时，自车j在驾驶过程中的作用量，即S3中的所所述目标优化函数可以表示为式(1)：

式(1)中，S_Risk为所述可行轨迹的作用量，t₀为所述可行轨迹的采样初始端对应的时刻，t_f为所述可行轨迹的采样终端对应的时刻，n表示交通场景中道路使用者的使用数量，i为其它道路使用者的编号，j为自车的编号，m_j为自车j的质量，v_i为其它道路使用者i的速度，v_j为自车j的速度，R_j为自车j行车过程中的等效阻力，G_j为自车j受到道路产生的等效吸引力，v_j，x为自车j的沿x方向的速度，F_ji为其他道路使用者i对自车j之间的作用力。

在空间轨迹生成之后，首先对每个轨迹候选进行碰撞检查，计算每条生成轨迹的S值，对于会造成交通事故的候选轨迹进行剔除。因为如碰撞到障碍物或可能发生交通事故，则会造成自车滞留而不能顺利完成任务，因此，在完成从出发地a到目的地b的时间t会远大于正常驾驶过程所用时间，即拉格朗日量L对时间t积分所得的作用量S也会超出正常范围。通过仅检查每次操纵的碰撞来从候选轨迹中选择更安全的轨迹是困难的。因此，每个候选路线都需要快速的碰撞风险评估。本发明明确计算在考虑障碍物存在时的每条可行轨迹的生成时间，通过对生成轨迹的时间进行排序，剔除不合理的时间段，如当事故发生时，自车不能通过自身来完整地完成整个过程，因此时间趋近于无穷大，显然可以区分并对该轨迹进行剔除。

在剔除了危险轨迹后，即可以保证可行轨迹的安全性，接着对各安全轨迹进行作用量S的计算，并进行排序，选择作用量S最小的轨迹即最优轨迹。

计算轨迹规划过程中实时期望速度。通过上述步骤计算出最优轨迹后，由最小作用量的定义知，最优轨迹是满足“F＝ma”的那条轨迹，因此对于该轨迹进行求解，可得当要计算S_min时，即：

δS_Risk＝0

式(2)中，v_j，x为自车j沿x轴方向的速度；v_j，y为自车j沿x轴方向的速度；v_i，x为其他道路使用者i沿x轴方向的速度；v_i，y为其他道路使用者i沿y轴方向的速度；F_ji，x为其他道路使用者i对自车j的作用力沿x轴方向的分量；F_ij，y为自车j对其他道路使用者i的作用力沿y轴方向的分量；因此，S3中的所述期望最优速度(v_j，x，v_j，y)由方程组(2)计算得到。

最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (7)

1.一种自动驾驶车辆实时轨迹规划方法，其特征在于，包括以下步骤：

S1，实时获取自车及周边环境相关信息；

S2，基于所述自车及周边环境的相关信息，生成参考轨迹以及基于所述参考轨迹进而确定的可行轨迹簇和所述可行轨迹簇中的每一条可行轨迹对应的速度；

S3，根据所述可行轨迹和其相对应的速度，利用以安全性和高效性为驾驶目标的目标优化函数，计算每一条所述可行轨迹的作用量，并选择具有最小作用量的可行轨迹作为期望最优轨迹，并优化得到与所述期望最优轨迹对应的期望最优速度；所述目标优化函数根据最小作用量原理和等效力方法获得；

S3中的所所述目标优化函数表示成式(1)：

式(1)中，S_Risk为所述可行轨迹的作用量，t₀为所述可行轨迹的采样初始端对应的时刻，tf为所述可行轨迹的采样终端对应的时刻，n表示交通场景中道路使用者的使用数量，i为其它道路使用者的编号，j为自车的编号，m_j为自车j的质量，v_i为其它道路使用者i的速度，v_j为自车j的速度，R_j为自车j行车过程中的等效阻力，G_j为自车j受到道路产生的等效吸引力，v_j，x为自车j的沿x轴方向的速度，F_ji为其他道路使用者i对自车j的作用力；

所述期望最优速度(v_j，x，v_j，y)由下面的方程组(2)计算得到：

式(2)中，L_X为最小作用量x轴分量；L_Y为最小作用量y轴分量；v_j，x为自车j沿x轴方向的速度；v_j，y为自车j沿x轴方向的速度；v_i，x为其他道路使用者i沿x轴方向的速度；v_i，y为其他道路使用者i沿y轴方向的速度；F_ji，x为其他道路使用者i对自车j的作用力沿x轴方向的分量；F_ij，y为自车j对其他道路使用者i的作用力沿y轴方向的分量。

2.如权利要求1所述的自动驾驶车辆实时轨迹规划方法，其特征在于，S2具体包括如下步骤：

S21，生成和平滑参考轨迹；

S22，将所述参考轨迹由其生成的笛卡尔坐标系坐标转换到曲线坐标系中；

S23，在所述曲线坐标系中，通过沿s轴方向对所述参考轨迹进行不同的横向偏移，生成所述可行轨迹簇；

S24，根据道路边界约束条件、运动约束和交通法规限制，利用梯形线速度曲线生成对每一条所述可行轨迹对应的速度。

3.如权利要求2所述的自动驾驶车辆实时轨迹规划方法，其特征在于，S21的“生成所述参考轨迹”具体包括：

采取五次Bezier曲线生成所述参考轨迹，所述五次Bezier曲线的具体表达为式(3)：

P(t)＝(1-t)⁵P₀+5(1-t)⁴tP₁+10(1-t)³t²P₂+10(1-t)²t³P₃+5(1-t)t⁴P₄+t⁵P₅ (3)

式(3)中，P₀为所述Bezier曲线的第一个控制点，P₁为所述Bezier曲线的第二个控制点，P₂为所述Bezier曲线的第三个控制点，P₃为所述Bezier曲线的第四个控制点，P₄为所述Bezier曲线的第四个控制点，P₅为所述Bezier曲线的第六个控制点，P(t)为前6个控制点与Bezier基函数乘积之和，t为所述Bezier曲线的时间参数。

4.如权利要求3所述的自动驾驶车辆实时轨迹规划方法，其特征在于，S21的“平滑所述参考轨迹”具体包括：

自车的运行状态被分解为四维状态(x(t)，y(t)，θ(t)，k(t))，其中，x(t)为所述自车的横向位移，y(t)为所述自车的纵向位移，x(t)和y(t)由所述自车的相关信息获得；θ(t)为沿所述参考轨迹的航点的切线角，其表示为下面的式(4)；k(t)为笛卡尔坐标系下的沿所述参考轨迹的曲率，其表示为下面的式(5)：

5.如权利要求3所述的自动驾驶车辆实时轨迹规划方法，其特征在于，S24具体包括：

S241，确定所述梯形线速度曲线的采样初始端的速度v₀、中间速度v_r、采样终端的速度v_f、采样初始端的加速度a₀、采样终端的速度a_f和遍历时间t；

S242，采用梯形速度框架来生成积分速度曲线；

S243，基于所述参考轨迹生成所述可行轨迹簇；

S244，利用三次多项式平滑速度曲线，生成可行轨迹簇的速度。

6.如权利要求5所述的自动驾驶车辆实时轨迹规划方法，其特征在于，S241中，所述梯形线速度曲线的加速度a(t)的最大值表示为下面的式(6)，所述梯形线速度曲线的速度v(t)的最大值表示为下面的式(7)：

|a(t)|≤a_max (6)

|v(t)|≤min{v_max，1，v_max，2，v_max，3…} (7)

式(7)中a_max为由运动约束限定的最大允许加速度；v_max，1为由运动约束限定的最大允许速度；v_max，2为运动约束限定的车辆最大可行速度；v_max，3为由所述交通法规限定的最大可行速度。

7.如权利要求4所述的自动驾驶车辆实时轨迹规划方法，其特征在于，S243具体包括如下方法：

根据所述参考轨迹，在所述曲线坐标系中通过沿s轴方向对所述参考轨迹进行不同的横向偏移量l(s)，生成所述可行轨迹簇；所述可行轨迹簇中的每一条所述可行轨迹当前位置的状态表示为弧长s_i和横向偏移量l_i，在采样初始端的状态表示为弧长s₀和横向偏移量l₀，在所述采样终端的状态表示为弧长s_f和横向偏移量l_f；

S244具体包括如下方法：

S2441，生成轨迹的曲率通过多项式进行平滑，采用三次多项式(8)描述所述横向偏移量l(s)：

S2442，通过对所述横向偏移量l(s)进行一阶求导，得到式(9)：

S2443，通过对所述横向偏移量l(s)进行二阶求导，得到式(10)：

S2444，根据所述道路边界约束条件及车辆和所述参考轨迹之间航向角度差θ(s)形成的如下约束条件(11)，代入计算得到式(8)至式(10)中的未知参数a、b、c和d：

S2445，根据式(5)，将每一条所述可行轨迹的曲率由笛卡尔坐标系中转换到曲线坐标系中，得到曲线坐标系中的所述可行轨迹曲率k(s)表示为式(12)：

式(12)中，S_sgn＝sgn(1-l(s)k_b)，

k_b为当前生成的可行轨迹的曲率；

S2446，基于式(8)至式(10)及式(12)，求得曲线坐标系中的所述可行轨迹曲率k(s)，基于式(9)求得曲线坐标系中的所述可行轨迹的速度；

S2447，将S2446求得的曲线坐标系中的所述可行轨迹曲率k(s)和速度转换到笛卡尔坐标系中，得到S2中的所述可行轨迹及其速度。

Images