CN114537435A - 一种自动驾驶中的实时整车轨迹规划方法 - Google Patents

Abstract

本发明公开了一种自动驾驶中的实时整车轨迹规划方法，包括有车辆平坦输出的MINCO轨迹类的构建，静态障碍物的惩罚项定义和动态避障的惩罚项定义的处理。本发明在考虑车辆的运动学模型和车辆的轮廓形状的条件下，进行有效且鲁棒的整车轨迹规划，并且整合了自动驾驶车辆在自动泊车和结构化道路驾驶的不同应用场景，生成了一种更通用的后端轨迹优化的方式。

Description

一种自动驾驶中的实时整车轨迹规划方法

技术领域

本发明实施涉及自动驾驶车辆的轨迹规划领域，尤其涉及一种自动驾驶中的实时整车轨迹规划方法领域。

背景技术

在自动驾驶车辆的轨迹规划中现有的需要进行城市道路自动驾驶车辆轨迹规划和自动驾驶车辆的泊车路径规划，在结构性的城市道路，高速路等环境下自动驾驶车辆的无碰撞实时轨迹规划，目前很多解决方案对运动规划在时间或空间上进行了一定的简化和分解，使得生成的轨迹在一些复杂场景适用性不强，耦合时空，考虑车辆动力学或运动学的实时轨迹规划较为困难。在停车场，路侧等狭窄空间停车，需要选择对可行驶的安全区，进行局部的轨迹规划，在满足车辆动力学/运动学的前提下生成无碰撞的轨迹。相比于城市道路驾驶，自动泊车对车辆终点姿态有更严苛的要求，需要更有效的轨迹生成算法。

自动驾驶车辆在进行运动规划时，为模拟人类驾驶员在结构化道路的驾驶行为，会对车辆运动进行横向和纵向的分解，在弗勒内坐标系下进行轨迹规划。纵向的最优轨迹决定了车辆行驶的纵向速度加速度，乘客的舒适度，横向的轨迹用于避障和车辆换道(如图1)，相比于在笛卡尔坐标系的表示，弗勒内坐标系下的运动规划简化了模型，但车的运动在横纵方向不完全解耦，分解成两个独立的轨迹优化问题很难处理复杂在动态环境的整体规划。另一方面，车辆的轨迹也可以从时间上进行分解，先在静态环境中规划出一条没有时间信息的轨迹，在沿着这条轨迹规划速度分布来避开移动的障碍物。这种方式在一些商用自动驾驶轨迹规划方案上经常被采用，相比于其他处理动态障碍物的耦合规划也比较容易实现，但在环境更加复杂的情况下，会出现无法在规划的物理轨迹上找到合适的速度分布，仍遗留一些问题需要解决。

对于自动驾驶车辆的轨迹规划，近年来的研究主要分开进行自动泊车和结构化道路的自动驾驶轨迹规划，缺乏一个易于实现并且通用的方法来处理这两种场景的轨迹规划，很多商业公司，如百度无人车团队提出了基于迭代的轨迹优化和最小速度分配，本质上还是分离了速度分布和物理轨迹，在动态复杂的环境下鲁棒性不高；在城市道路环境下，Ding等人提出时空安全行驶走廊，应用Bezier曲线来表征在横向和纵向上的轨迹，得到了不错的效果，但是，因为该方案对车辆建模为质点，未考虑车辆的运动学模型和车辆轮廓形状，在处理避障时，需要对障碍物进行膨胀，一定程度上增加了算法的计算成本，也是比较保守的方案，难以处理在狭窄和动态环境的整车轨迹规划。

在处理动态障碍物的避碰上，最直接最简单的处理是用椭圆或圆形包裹车辆轮廓(如图2)，这种方式在工程上好实现也很有效，但比较保守。另一种方式是将运动物体，主要是规则形状的车辆，建模成凸多面体(如图3)。应用整数变量将自身车辆约束在这些运动物体的凸多面体之外，或者应用对偶变量来表示符号距离，求解优化问题。整数优化的计算成本比较大，很难进行高效的实时轨迹优化。其他也有利用软阈值函数等处理和运动障碍物之间的避障，在一定程度上能给出有效的求解。

发明内容

本发明的目的，是为了解决背景技术中的问题，提供一种自动驾驶中的实时整车轨迹规划方法。

本发明的上述技术目的是通过以下技术方案得以实现的：

一种自动驾驶中的实时整车轨迹规划方法，包括有车辆平坦输出的MINCO轨迹类、静态障碍物的惩罚项定义和动态避障的惩罚项定义，本发明在考虑车辆的运动学模型和车辆的轮廓形状的条件下，进行有效且鲁棒的整车轨迹规划，并且整合了自动驾驶车辆在自动泊车和结构化道路驾驶的不同应用场景，提出了一种更通用的后端轨迹优化的方式。

车辆平坦输出的MINCO轨迹类的公式如下：

应用自行车模型，车辆的平坦输出可以表示为σ＝[σ_x,σ_y],为车辆在笛卡尔坐标系下的横纵坐标，其他物理量可以表示为：

其中，v,θ,a,φ表示车的速度，朝向角，加速度和转向角，η为额外的决策变量，决定了车向前或向后的运动。

车辆的轨迹优化问题可以定义为：

其中，ρ(T)为时间惩罚函数，w_T为相应的权重，

为用户定义的约束函数，d＝{v,a,κ,b,o,u}。车辆在x,y坐标系下的轨迹被定义为MINCO轨迹类；在本发明的研究问题下，

函数包含车辆的非完整约束(v,a,κ)，静态和动态障碍物的避碰约束(b,o)，还有轨迹的均匀性约束(u)。其在第i段轨迹的采样的积分惩罚函数可以定义为：

考虑到车辆的运动学模型和控制限度，其速度，加速度和曲率约束相应的代价函数定义如下：

在MINCO轨迹类中已经证明了梯度传导的线性复杂度，可将车辆的动态可行条件函数松弛加入目标函数，进行同样的代价函数评估。

考虑到自动驾驶车辆的感知模块通常对环境进行语义分割并分离动态和静态障碍物。本发明解耦了环境中动态和静态障碍物，并分别进行约束条件的构造。

静态障碍物的惩罚项定义，公式如下：

我们采用凸多面体生成算法，得到一条沿着预测轨迹或搜索路径的安全行驶走廊，对MINCO轨迹类上的离散点，自身车辆的矩形轮廓

需要约束在该段凸多面体内：

其中，

该约束等同于约束矩形的顶点在凸多面体内，顶点定义如下：

其中R_i(t)为第i段轨迹在t时刻的旋转矩阵，

定义为在车辆顶点在自身坐标系下的坐标，因此，对每一段轨迹多项式，

约束函数可以定义为：

动态避障的惩罚项定义，公式如下：

首先定义两个凸多面体表示的物体的符号距离的下界为：

这样的表示满足估计距离的函数是连续可导，并且其估计误差是有界的。进一步，定义动态避障的惩罚项函数为：

当车辆自身与障碍物的符号距离估计在车辆避障考虑范围内时，非零惩罚项需要加在目标函数上。对符号距离估计值进行Softmax平滑，可以得到函数表达式为：

类似的，我们可以计算动态障碍物避障函数的梯度。

综上所述，本发明的有益效果：

1.本发明在考虑车辆的运动学模型和车辆的轮廓形状的条件下，进行有效且鲁棒的整车轨迹规划，并且整合了自动驾驶车辆在自动泊车和结构化道路驾驶的不同应用场景，形成了一种更通用的后端轨迹优化的方式。

2.本发明采用了比较通用的自行车模型，平台输出为车的横纵坐标x和y。一方面因为更复杂的模型并没有增加更多新的动力学非完整约束，一方面更容易进行轨迹的多项式表示和优化。

3.本发明应用安全行驶走廊作为MINCO轨迹类的初始输入，动态障碍物和静态障碍物被分开处理，以适应自动驾驶感知阶段对障碍物的语义分割，动态障碍物的轨迹预测，行为规划的决策模块。

4.本发明应用快速生成自由空间的凸多面体表示的方法被应用到本发明中生成基于静态障碍物的安全行驶区域。

附图说明

图1是本发明背景技术自动驾驶车辆轨迹规划在速度-轨迹和横向-纵向的分解；

图2是本发明背景技术圆形建模的避障处理示意图；

图3是本发明背景技术对偶距离的避障处理示意图；

图4是本发明安全行驶走廊示意图；

图5是本发明为自身车辆和需要躲避的车辆间符号距离下界估计的示意图；

图6是本发明车辆模型的自行车模型示意图；

图7是本发明符号距离对凸多面体的估计示意图；

图8是本发明运动轨迹的自由行驶区域表示示意图；

图9是本发明连续凸多边形表示示意图；

图10是本发明点云上生成的安全行驶走廊示意图；

图11是本发明城市道路的轨迹生成示意图；

图12是本发明驶离泊车位的轨迹生成示意图；

图13是本发明轨迹生成时间效率的比较示意图；

图14是本发明变换车道对比示意图；

图15是本发明超车对比示意图。

实施方式

以下具体实施例仅仅是对本发明的解释，其并不是对本发明的限制，本领域技术人员在阅读完本说明书后可以根据需要对本实施例做出没有创造性贡献的修改，但只要在本发明的权利要求范围内都受到专利法的保护。

下面结合附图以实施例对本发明进行详细说明。

实施例：

一种自动驾驶中的实时整车轨迹规划方法，包括有车辆平坦输出的MINCO轨迹类、静态障碍物的惩罚项定义和动态避障的惩罚项定义，本发明主要涉及车辆微分平坦轨迹的构建和障碍物约束的处理。

车辆平坦输出的MINCO轨迹类的公式如下：

应用自行车模型，车辆的平坦输出可以表示为σ＝[σ_x,σ_y],为车辆在笛卡尔坐标系下的横纵坐标，其他物理量可以表示为：

其中，v,θ,a,φ表示车的速度，朝向角，加速度和转向角，η为额外的决策变量，决定了车向前或向后的运动。

车辆的轨迹优化问题可以定义为：

其中，ρ(T)为时间惩罚函数，w_T为相应的权重，

为用户定义的约束函数，d＝{v,a,κ,b,o,u}。车辆在x,y坐标系下的轨迹被定义为MINCO轨迹类；在本发明的研究问题下，

函数包含车辆的非完整约束(v,a,κ)，静态和动态障碍物的避碰约束(b,o)，还有轨迹的均匀性约束(u)。其在第i段轨迹的采样的积分惩罚函数可以定义为：

考虑到车辆的运动学模型和控制限度，其速度，加速度和曲率约束相应的代价函数定义如下：

在MINCO轨迹类中已经证明了梯度传导的线性复杂度，可将车辆的动态可行条件函数松弛加入目标函数，进行同样的代价函数评估。

考虑到自动驾驶车辆的感知模块通常对环境进行语义分割并分离动态和静态障碍物。本发明解耦了环境中动态和静态障碍物，并分别进行约束条件的构造。

静态障碍物的惩罚项定义，公式如下：

我们采用现有的凸多面体生成算法，得到一条沿着预测轨迹或搜索路径的安全行驶走廊如图4，对MINCO轨迹类上的离散点，自身车辆的矩形轮廓

需要约束在该段凸多面体内：

其中，

该约束等同于约束矩形的顶点在凸多面体内，顶点定义如下：

其中R_i(t)为第i段轨迹在t时刻的旋转矩阵，

定义为在车辆顶点在自身坐标系下的坐标。，因此，对每一段轨迹多项式，

约束函数可以定义为：

动态避障的惩罚项定义，公式如下：

首先定义两个凸多面体表示的物体的符号距离的下界为：

这样的表示满足估计距离的函数是连续可导，并且其估计误差是有界的。进一步，定义动态避障的惩罚项函数为：

当车辆自身与障碍物的符号距离估计在车辆避障考虑范围内时，非零惩罚项需要加在目标函数上。对符号距离估计值进行Softmax平滑，可以得到函数表达式为：

类似的，我们可以计算动态障碍物避障函数的梯度。图5为自身车辆和需要躲避的车辆间符号距离下界估计的示意图。

基于本发明的现有成熟技术：

1.车辆模型的微分平坦动力学

对非线性系统，微分平坦的性质能使所有的状态变量和输入变量都用这组平坦输出和其有限阶的微分进行表示。这样，规划目标的轨迹就可以用多段连续多项式进行表示。利用微分平坦表示进行规划在无人机上应用广泛，但因为车辆平坦模型的不唯一和车辆模型的复杂性让平坦表示的规划没有那么普遍。本发明采用了比较通用的自行车模型(如图6)，平台输出为车的横纵坐标x和y。一方面因为更复杂的模型并没有增加更多新的动力学非完整约束，一方面如图的模型更容易进行轨迹的多项式表示和优化。

2.最小化控制输出量的多项式轨迹类

在无人机规划中，采用了MINCO多项式轨迹类来表征运动轨迹：

其中p(t)是一个m维用M段N＝2s-1阶多项式表达的轨迹，第i段表示为：

是第i段多项式的系数矩阵，并且β(t)＝(1,t,…,t^N)^T是自然基。时间向量T＝(T₁,T₂,…,T_M)^T，T_i是第i段轨迹的持续时间。MINCO轨迹类只用间隔点和时间参数化轨迹，并且评估整条轨迹具有线性复杂度，可以和用户定义的外部需求进行结合，并且具有需求关于航点-航时梯度的线性复杂度。

3.凸多面体的符号距离计算和避障

两个凸多面体

间的符号距离定义为：

要保证两个凸多面体之间没有重叠，可以表示为保证其符号距离大于零。在计算符号距离时，对凸多面体组成平面的选择会引入整数变量，往往会让问题的求解比较复杂。现有的对环境建模时提出了对符号距离的下界表示和估计(如图7)：

其中，

表示组成凸多面体

的超平面，

表示组成凸多面体

的超平面。通过应用符号距离的估计值，可以通过求极值来构造障碍物避碰的约束，简化了约束的形式。

4.安全行驶走廊

对静态障碍物的避碰处理，安全行驶走廊(如图8)是一种有效的表征方式。对安全行驶走廊的定义有很多中，基于其他运动物体轨迹的函数化定义或者基于障碍物的结构化定义(连续的凸多面边体如图9)。

本发明应用安全行驶走廊作为MINCO轨迹类的初始输入，不同于上述的表征方式，动态障碍物和静态障碍物被分开处理，以适应自动驾驶感知阶段对障碍物的语义分割，动态障碍物的轨迹预测，行为规划的决策模块。现有的作为快速生成自由空间的凸多面体表示的方法被应用到本发明中生成基于静态障碍物的安全行驶区域。点云上生成的安全行驶走廊如图10。

技术实现细节与应用场景：

1.对城市道路的车辆轨迹规划，本模块作为自动驾驶规划决策的一部分，需要路径规划器和行为规划器提供上层的规划方向和在未来时间的粗略预测点，再基于这些点生成安全行驶走廊。在具体实现中，行为规划器可以为遵循多决策框架MultipolicyDecision-Making(MPDP)，多决策生成的多个预测轨迹都可以分别建立安全行驶走廊和进行轨迹优化。城市道路的轨迹生成如图11。

2.对自动泊车场景，需要自动驾驶车辆前端感知模块识别泊车位的具体范围。本发明提供了一种有效的前端轨迹生成的实现，在提供泊车位的具体坐标和约束区域的前提下，实时生成符合车辆运动学的后端优化轨迹。驶离泊车位的轨迹生成如图12

验证本发明在不用的复杂驾驶行为下生成轨迹的效率和质量，并和对照方法参见(丁文华，张磊，陈建军，沈胜胜，“利用时空语义走廊生成复杂城市环境的安全轨迹”，IEEE机器人与自动化通讯，2019年。)进行评估。在求解效率上，根据图13的表1可以看出由于不用进行变换所有障碍物，动态物体轨迹，自身轨迹到弗勒内坐标系，一定程度上可以节省计算成本。同时本发明的安全行驶走廊生成和轨迹优化总体时间也优于对照方法。

在相同的驾驶环境下，我们生成了相同的驾驶行为条件，通过轨迹生成的效果比较两种方法的表现。总体上，本发明生成的轨迹更贴合车辆运动学，能有效生成光滑，满足舒适度的轨迹。在处理动态障碍物时，能在更复杂的条件下进行规划。变换车道如图14，超车时如图15，左图为本案，右图为对比方案。

本发明作为后端轨迹生成模块，不限于和任何前端路径生成模块结合。同时，在验证方法有效性时使用的上层决策规划模块均可替代，本发明对决策层没有方法上的依赖性。本发明使用的在点云中生成凸多面体的方法不限于所述方案，任何行之有效的凸多面体自由空间表示均可以使用。目前本发明在轨迹生成的通用性和效率方面没有完整的替代方案。

本发明提出通用的车辆耦合的时空规划的解决方案，特别是该方案在自动泊车和城市道路应用场景都能实时进行轨迹优化和求解，应用车辆平坦输出的MINCO轨迹类并定义了基于车辆运动学的惩罚项；本发明对适用于自动驾驶车辆的安全行驶走廊生成的设计；本发明考虑整车的静态/动态障碍物避碰约束形式和对符号距离估计定义的约束表示。

Claims (10)

1.一种自动驾驶中的实时整车轨迹规划方法，其特征在于，包括有车辆平坦输出的MINCO轨迹类的构建，静态障碍物的惩罚项定义和动态避障的惩罚项定义的处理。

2.根据权利要求1所述的一种自动驾驶中的实时整车轨迹规划方法，其特征在于，所述车辆平坦输出的MINCO轨迹类的公式如下：

应用自行车模型，车辆的平坦输出可以表示为σ＝[σ_x,σ_y],为车辆在笛卡尔坐标系下的横纵坐标，其他物理量可以表示为：

其中，v,θ,a,φ表示车的速度，朝向角，加速度和转向角，η为额外的决策变量，决定了车向前或向后的运动。

3.根据权利要求2所述的一种自动驾驶中的实时整车轨迹规划方法，其特征在于，车辆的轨迹优化问题可以定义为：

其中，ρ(T)为时间惩罚函数，w_T为相应的权重，

为用户定义的约束函数，d＝{v,a,κ,b,o,u}；车辆在x,y坐标系下的轨迹被定义为MINCO轨迹类；在本发明的研究问题下，

函数包含车辆的非完整约束(v,a,κ)，静态和动态障碍物的避碰约束(b,o)，还有轨迹的均匀性约束(u)。

4.根据权利要求3所述的一种自动驾驶中的实时整车轨迹规划方法，其特征在于，其在第i段轨迹的采样的积分惩罚函数可以定义为：

考虑到车辆的运动学模型和控制限度，其速度，加速度和曲率约束相应的代价函数定义如下：

。

5.根据权利要求1所述的一种自动驾驶中的实时整车轨迹规划方法，其特征在于，所述的静态障碍物的惩罚项定义，公式如下：

我们采用凸多面体生成算法，得到一条沿着预测轨迹或搜索路径的安全行驶走廊，对MINCO轨迹类上的离散点，自身车辆的矩形轮廓

需要约束在该段凸多面体内：

其中，

6.根据权利要求5所述的一种自动驾驶中的实时整车轨迹规划方法，其特征在于，该约束等同于约束矩形的顶点在凸多面体内，顶点定义如下：

其中R_i(t)为第i段轨迹在t时刻的旋转矩阵，

定义为在车辆顶点在自身坐标系下的坐标。

7.根据权利要求6所述的一种自动驾驶中的实时整车轨迹规划方法，其特征在于，因此，对每一段轨迹多项式，

约束函数可以定义为：

8.根据权利要求1所述的一种自动驾驶中的实时整车轨迹规划方法，其特征在于，所述的动态避障的惩罚项定义，公式如下：

首先定义两个凸多面体表示的物体的符号距离的下界为：

这样的表示满足估计距离的函数是连续可导，并且其估计误差是有界的。

9.根据权利要求8所述的一种自动驾驶中的实时整车轨迹规划方法，其特征在于，进一步，定义动态避障的惩罚项函数为：

当车辆自身与障碍物的符号距离估计在车辆避障考虑范围内时，非零惩罚项需要加在目标函数上。

10.根据权利要求9所述的一种自动驾驶中的实时整车轨迹规划方法，其特征在于，对符号距离估计值进行Softmax平滑，可以得到函数表达式为：

Images