CN114228748B - 一种基于几何路径生成的类人化自动驾驶轨迹规划方法 - Google Patents

Abstract

本发明提供一种基于几何路径生成的类人化自动驾驶轨迹规划方法，包括以下步骤：使用三次贝塞尔曲线生成备选路径簇；对备选路径簇进行速度规划，根据车辆纵向加速度运动模型建立车速二次规划模型，得到主车状态；基于高斯过程理论建立障碍车运动预测模型，得到障碍车运动预测结果；根据障碍车运动预测结果以及主车状态，通过碰撞检测对备选路径簇的安全性进行评价，选出安全备选轨迹；结合驾驶员跟车特性、路径安全性、规划一致性以及速度舒适性，对安全备选轨迹进行评价选出最优参考轨迹。本发明考虑了障碍车未来的运动状态以及驾驶员个性化跟车需求，实现了自动驾驶车辆安全，舒适及个性化的轨迹规划，提升了自动驾驶车辆类人化决策的水平。

Description

一种基于几何路径生成的类人化自动驾驶轨迹规划方法

技术领域

本发明涉及汽车自动驾驶的技术领域，具体涉及一种基于几何路径生成的类人化自动驾驶轨迹规划方法。

背景技术

在汽车自动驾驶领域，轨迹规划是自动驾驶车辆的核心功能之一。轨迹规划根据环境感知系统获取的环境信息，综合考虑车辆安全性、舒适性等多种性能目标，最终输出车辆控制的参考状态，规划自动驾驶行车轨迹。

为实现复杂行驶环境下安全、舒适的自动驾驶功能，轨迹规划算法的求解效率将面临巨大挑战，现有算法逻辑大多基于递进结构，在轨迹规划上耗时较大，计算实时性难以保证。如果在轨迹规划的过程中进一步引入驾驶员跟车特性，以实现类人性的自动驾驶，计算耗时会进一步增加。

因此，现在亟需一种能够快速计算的轨迹规划方法，以提升自动驾驶决策规划求解效率，实现基于驾驶行为表征的类人性自动驾驶。

发明内容

针对现有技术存在的不足，本发明提出一种基于几何路径生成的类人化自动驾驶轨迹规划方法，以解决现有技术中存在的考虑驾驶员跟车特性、控制代价、舒适性行驶性能目标后，进行的自动驾驶轨迹规划计算实时性差的技术问题。

本发明采用的技术方案是，一种基于几何路径生成的类人化自动驾驶轨迹规划方法，包括以下步骤：

在道路坐标系下离散采样，使用三次贝塞尔曲线生成备选路径簇；

使用最优控制理论对备选路径簇进行速度规划，根据车辆纵向加速度运动模型建立车速二次规划模型，根据车速二次规划模型得到主车状态；

基于高斯过程理论建立障碍车运动预测模型，根据障碍车运动预测模型得到障碍车运动预测结果；

根据障碍车运动预测结果以及主车状态，通过碰撞检测对备选路径簇的安全性进行评价，选出满足安全约束条件的安全备选轨迹；

结合驾驶员跟车特性、路径安全性、规划一致性以及速度舒适性，对安全备选轨迹进行评价，选择出最优参考轨迹。

进一步的，使用三次贝塞尔曲线生成备选路径簇，具体如下：

x＝(1-τ)³x₀+3(1-τ)²τx₁+3(1-τ)τ²x₂+τ³x_f

y＝(1-τ)³y₀+3(1-τ)²τy₁+3(1-τ)τ²y₂+τ³y_f

在上式中，x₀、x₁、x₂、x_f分别为三次贝塞尔曲线第一个控制点、第二个控制点、第三个控制点、第四个控制点的横坐标，y₀、y₁，y₂、y_f分别为三次贝塞尔曲线第一个控制点、第二个控制点、第三个控制点、第四个控制点的纵坐标，τ为参变量。

进一步的，车辆纵向加速度运动模型为：

X＝[s v a]^T

U＝Δa

在上式中，s，v，a和Δa分别表示车辆的纵向运动路程，纵向车速，纵向加速度与纵向加速度变化率；A为系统矩阵，B为输入矩阵，U为增量矩阵；

车速二次规划模型为：

在上式中，ρ为惩罚系数，X₁(t_f)为轨迹终点处的车辆状态，X为车辆状态向量，X_ref为参考状态向量，t_f为轨迹终点时间，S为轨迹终点路程，Q、R为权重矩阵，U为增量矩阵，A为系统矩阵，B为输入矩阵，v为车辆速度，a为车辆加速度。

进一步的，基于高斯过程理论建立障碍车运动预测模型时，使用径向基核作为障碍车运动预测模型的协方差函数，协方差函数为：

在上式中，k为核函数，T为历史数据，T’为未来数据，σ_f、l为超参数。

进一步的，碰撞检测包括：

采用几何相交检测法，将主车安全范围定义为椭圆，障碍车安全范围定义为矩形，碰撞检测采样点范围定义为椭圆的外接矩形，判断该采样点是否同时在主车安全椭圆以及障碍车矩形内即可判断是否碰撞。

进一步的，对安全备选轨迹进行评价时，分别使用行驶类人化指标、路径安全性指标、规划一致性指标、速度舒适性指标评价驾驶员跟车特性、路径安全性、规划一致性、速度舒适性；

最优参考轨迹的车辆驾驶性能优化函数为：

在上式中，ω_i为各性能指标的权重因子。

进一步的，行驶类人化指标为：

J₁＝-∑logp(Δs,Δv|φ,μ,∑)

在上式中，Δs表示所规划路径中主车与障碍车的相对距离，Δv表示主车与障碍车的相对速度，φ,μ,∑为高斯混合模型参数。

进一步的，路径安全性指标为：

在上式中，R为车道宽度，y_end表示所规划的参考路径的终点y方向坐标。

进一步的，规划一致性指标为：

在上式中，P_y(t)为上一时刻规划轨迹在道路方向上的投影，P_y′(t)表示当前时刻规划的轨迹在道路方向上的投影，P_x(t)表示当前时刻轨迹与上一时刻规划轨迹重叠部分在道路方向上的投影。

进一步的，速度舒适性指标为：

在上式中，v₀为当前时刻主车车速，v为规划终点的主车车速。

由上述技术方案可知，本发明的有益技术效果如下：

基于离散采样思想，利用三次贝塞尔曲线在道路坐标系上生成备选路径簇，利用最优控制理论对备选路径簇进行速度规划，并建立高斯过程障碍车运动预测模型，根据预测模型预测障碍车的运动状态，结合主车状态对备选路径簇进行碰撞检测，筛选出满足安全约束条件的安全轨迹，最终综合考虑驾驶员跟车特性、路径安全性、规划一致性以及速度舒适性，优选出符合驾驶员类人化特征的安全、舒适行驶轨迹，实现基于几何路径生成的类人化自动驾驶轨迹规划。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单介绍。在所有附图中，类似的元件或部分一般由类似的附图标记标识。附图中，各元件或部分并不一定按照实际的比例绘制。

图1为本发明一实施例的基于几何路径生成的类人化自动驾驶轨迹规划方法流程图；

图2为本发明一实施例的生成备选路径簇观察图；

图3为本发明一实施例的根据车速二次规划模型得到车辆速度与车辆加速度的规划结果观察图；

图4为本发明一实施例的障碍车运动状态预测结果观察图；

图5为本发明一实施例的碰撞检测示意图；

图6为本发明一实施例最优参考轨迹观察图。

具体实施方式

下面将结合附图对本发明技术方案的实施例进行详细的描述。以下实施例仅用于更加清楚地说明本发明的技术方案，因此只作为示例，而不能以此来限制本发明的保护范围。

需要注意的是，除非另有说明，本申请使用的技术术语或者科学术语应当为本发明所属领域技术人员所理解的通常意义。

实施例

本实施例提供了一种基于几何路径生成的类人化自动驾驶轨迹规划方法，如图1所示，包括以下步骤：

S1：在道路坐标系下离散采样，使用三次贝塞尔曲线生成备选路径簇

在具体的实施方式中，在道路坐标系下离散采样，通过使用三次贝塞尔(Bézier)曲线来描述主车行驶轨迹，主车行驶轨迹即为备选路径簇。三次贝塞尔曲线的表达式如下：

x＝(1-τ)³x₀+3(1-τ)²τx₁+3(1-τ)τ²x₂+τ³x_f

y＝(1-τ)³y₀+3(1-τ)²τy₁+3(1-τ)τ²y₂+τ³y_f (1)

在上式(1)中，x₀、x₁、x₂、x_f分别为三次贝塞尔曲线第一个控制点、第二个控制点、第三个控制点、第四个控制点的横坐标；y₀、y₁，y₂、y_f分别为三次贝塞尔曲线第一个控制点、第二个控制点、第三个控制点、第四个控制点的纵坐标；τ为参变量，取值范围为[0，1]。

道路坐标系的选用不作限定，以现有技术中任意一种可实现的方式实施，在道路坐标系下生成的备选路径簇，如图2所示。

S2：使用最优控制理论对备选路径簇进行速度规划，根据车辆纵向加速度运动模型建立车速二次规划模型，根据车速二次规划模型得到主车状态

在具体的实施方式中，使用最优控制理论对备选路径簇进行速度规划，根据车辆纵向加速度运动模型，建立车速二次规划模型，使车速二次规划模型满足：车辆最终路程误差最小、车辆速度与车辆加速度变化最小、车辆速度与车辆加速度大小不超过上下界限值。

车辆纵向加速度运动模型如下：

在上式(2)中，s，v，a和Δa分别表示车辆的纵向运动路程，纵向车速，纵向加速度与纵向加速度变化率；A为系统矩阵，B为输入矩阵，U为增量矩阵。

得到车速二次规划模型为：

在上式(3)中，ρ为惩罚系数，X₁(t_f)为轨迹终点处的车辆状态，X为车辆状态向量，X_ref为参考状态向量，t_f为轨迹终点时间，S为轨迹终点路程，Q、R为权重矩阵(权重矩阵结合实际场景确定)，U为增量矩阵，A为系统矩阵，B为输入矩阵，v为车辆速度，a为车辆加速度。

根据车速二次规划模型得到车辆速度与车辆加速度的规划结果，如图3所示；该规划结果即为主车状态，主车状态包括主车速度、主车加速度。

S3：基于高斯过程理论建立障碍车运动预测模型，根据障碍车运动预测模型得到障碍车运动预测结果

在具体的实施方式中，基于高斯过程理论建立障碍车运动预测模型时，采用径向基核作为障碍车运动预测模型的协方差函数，协方差函数如下式(4)：

在上式(3)中，k为核函数，T为历史数据，T’为未来数据，σ_f、l是待求解的超参数，可通过极大似然估计求解获得；具体的，令θ＝(σ_f,l)，则有：

由条件分布得到障碍车的预测状态期望与方差为

根据障碍车运动预测模型得到的障碍车运动预测结果，如图4所示。

S4：根据障碍车运动预测结果以及主车状态，通过碰撞检测对备选路径簇的安全性进行评价，选出满足安全约束条件的安全备选轨迹

在具体的实施方式中，碰撞检测采用几何相交检测法，将主车安全范围设计为椭圆，障碍车安全范围设计为矩形，如图5所示，在道路坐标系下，设某一时刻主车的坐标为(x_p,y_p,θ_p)，主车安全范围椭圆定义为：

x＝c cosθ

y＝b sinθ

0≤θ≤2π

其中，c,b分别为安全范围椭圆的长边、短边。基于主车当前位置旋转并平移，预测时刻的主车安全范围为：

碰撞检测采样点范围定义为主车安全范围(即椭圆)的外接矩形，即：

min(x_e)≤x_s≤max(x_e)

min(y_e)≤y_s≤max(y_e)

当在碰撞检测采样点范围内均匀采样，判断该采样点是否同时在主车安全椭圆以及障碍车矩形内即可判断是否碰撞。其判断方法如下：

1、椭圆判断：

将采样点(x_s,y_s)转化到车辆坐标系下，即：

不等式判据如下：

C₁:

2、矩形判断：

判断采样点(x_s,y_s)在矩形p₁p₂p₃p₄内，等价于判断采样点同时在平行线p₁p₂、p₃p₄之间与平行线p₂p₃、p₄p₁之间。因此，判断点在平行线之间的不等式判据为：

C₂:

C₃:

其中，×与·分别表示向量的向量积与内积，p_s为采样点(x_s,y_s)。

当且仅当椭圆判断与矩形判断同时成立时C₁∧C₂∧C₃，说明采样点同时存在于椭圆与矩形之间，即椭圆与矩形相交，主车与障碍车发生碰撞。

在本步骤中，将备选路径簇中主车不会与障碍车发生碰撞的备选路径，作为满足安全约束条件的安全备选轨迹。

S5：结合驾驶员跟车特性、路径安全性、规划一致性以及速度舒适性，对安全备选轨迹进行评价，选择出最优参考轨迹

在具体的实施方式中，对安全备选轨迹进行评价时，使用行驶类人化指标评价驾驶员跟车特性，使用路径安全性指标评价路径安全性，使用规划一致性指标评价规划一致性，使用速度舒适性指标评价速度舒适性，最终选择出最优参考轨迹，实现类人化的自动驾驶车辆安全，舒适的轨迹规划功能。

1、行驶类人化指标

通过对驾驶员跟车工况下跟车车距、跟车速度的特性建模，在具体的实施方式中，基于贝叶斯理论，建立基于跟车车距、跟车速度为特征量的高斯混合模型，高斯混合模型表达式如下：

其中，μ,∑分别为多元高斯分布中的均值与协方差，X表示数据样本，d表示样本的特征维数；n表示高斯单元的个数，φ_i表示每个高斯单元的权重，且满足：

运用Gibbs采样算法训练高斯混合模型，基于Gibbs采样参数迭代流程如下所示：

初始化先验参数分布超参数Θ(α,W,υ,m,β)，假设高斯混合模型有K个高斯单元；

采样高斯混合模型参数：φ_k～Dir(α)，Σ_k～Wis^-1(W,υ)，

计算样本在各高斯单元中的概率p_k；

计算各高斯单元的相对权重

基于概率p(z_k)采样选择高斯单元，每个高斯单元被选中的次数为z_nk；

计算参数：

更新超参数：

α_k←α_k+S_k[1]

β_k←β_k+S_k[1]

υ_k←υ_k+S_k[1]

评估所生成的三次贝塞尔曲线路径与驾驶员跟车驾驶时的相似程度，其归一化的后验概率密度越大，说明所生成路径的跟车特性与驾驶员越相似，更符合所学习的驾驶员特性，即

J₁＝-∑log p(Δs,Δv|φ,μ,∑) (5)

在上式(5)中，Δs表示所规划路径中主车与障碍车的相对距离，Δv表示主车与障碍车的相对速度，φ,μ,∑为高斯混合模型参数。

2、路径安全性指标

所规划的轨迹终点应尽量靠近车道中线，即车辆目标是沿车道中线行驶。将车道中的不同横向位置赋予不同的权重，在车道中线行驶时，性能目标更优，基于最小化性能目标，其表达式为：

在上式(6)中，R为车道宽度，y_end表示所规划的参考路径的终点y方向坐标。

3、规划一致性指标

针对规划一致性，当前规划的轨迹在道路方向上的投影与上个规划周期轨迹在道路方向上的投影之间平均累积偏差越小，说明两条轨迹的一致性越高，即

在上式(7)中，P_y(t)为上一时刻规划轨迹在道路方向上的投影，P_y′(t)表示当前时刻规划的轨迹在道路方向上的投影，P_x(t)表示当前时刻轨迹与上一时刻规划轨迹重叠部分在道路方向上的投影。

4、速度舒适性指标

车辆的平均速度变化越小，其舒适性越好，即

在上式(8)中，v₀为当前时刻主车车速，v为规划终点的主车车速。

综上，结合驾驶员跟车特性、路径安全性、规划一致性以及速度舒适性，筛选最优参考轨迹的车辆驾驶性能优化函数为：

在上式(9)中，ω_i为各性能指标的权重因子。

通过对性能指标的择优选取，能够生成一条满足安全性、舒适性，并能更好体现驾驶员特性的最优参考轨迹，其示意图如图6所示。

通过采用本实施例的技术方案，基于离散采样思想，利用三次贝塞尔曲线在道路坐标系上生成备选路径簇，利用最优控制理论对备选路径簇进行速度规划，并建立高斯过程障碍车运动预测模型，根据预测模型预测障碍车的运动状态，结合主车状态对备选路径簇进行碰撞检测，筛选出满足安全约束条件的安全轨迹，最终综合考虑驾驶员跟车特性、路径安全性、规划一致性以及速度舒适性，优选出符合驾驶员类人化特征的安全、舒适行驶轨迹，实现基于几何路径生成的类人化自动驾驶轨迹规划。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，其均应涵盖在本发明的权利要求和说明书的范围当中。

Claims (3)

1.一种基于几何路径生成的类人化自动驾驶轨迹规划方法，其特征在于，包括以下步骤：

在道路坐标系下离散采样，使用三次贝塞尔曲线生成备选路径簇；

使用最优控制理论对备选路径簇进行速度规划，根据车辆纵向加速度运动模型建立车速二次规划模型，所述车辆纵向加速度运动模型为：

X＝[s v a]^T

U＝Δa

在上式中，s，v，a和Δa分别表示车辆的纵向运动路程，纵向车速，纵向加速度与纵向加速度变化率；A为系统矩阵，B为输入矩阵，U为增量矩阵；

所述车速二次规划模型为：

在上式中，ρ为惩罚系数，X₁(t_f)为轨迹终点处的车辆状态，X为车辆状态向量，X_ref为参考状态向量，t_f为轨迹终点时间，S为轨迹终点路程，Q、R为权重矩阵，U为增量矩阵，A为系统矩阵，B为输入矩阵，v为车辆速度，a为车辆加速度；

根据车速二次规划模型得到主车状态；

基于高斯过程理论建立障碍车运动预测模型，包括：使用径向基核作为障碍车运动预测模型的协方差函数，所述协方差函数为：

在上式中，k为核函数，T为历史数据，T’为未来数据，σ_f、l为超参数；

根据障碍车运动预测模型得到障碍车运动预测结果；

根据障碍车运动预测结果以及主车状态，通过碰撞检测对备选路径簇的安全性进行评价，选出满足安全约束条件的安全备选轨迹；

结合驾驶员跟车特性、路径安全性、规划一致性以及速度舒适性，对安全备选轨迹进行评价，包括：分别使用行驶类人化指标、路径安全性指标、规划一致性指标、速度舒适性指标评价驾驶员跟车特性、路径安全性、规划一致性、速度舒适性；

所述行驶类人化指标为：

J₁＝-∑logp(Δs，Δv|φ，μ，∑)

在上式中，Δs表示所规划路径中主车与障碍车的相对距离，Δv表示主车与障碍车的相对速度，φ，μ，∑为高斯混合模型参数；

所述路径安全性指标为：

在上式中，R为车道宽度，y_end表示所规划的参考路径的终点y方向坐标；

所述规划一致性指标为：

在上式中，P_y(t)为上一时刻规划轨迹在道路方向上的投影，P_y′(t)表示当前时刻规划的轨迹在道路方向上的投影，P_x(t)表示当前时刻轨迹与上一时刻规划轨迹重叠部分在道路方向上的投影；

所述速度舒适性指标为：

在上式中，v₀为当前时刻主车车速，v为规划终点的主车车速；

最后选择出最优参考轨迹，所述最优参考轨迹的车辆驾驶性能优化函数为：

在上式中，ω_i为各性能指标的权重因子。

2.根据权利要求1所述的基于几何路径生成的类人化自动驾驶轨迹规划方法，其特征在于，使用三次贝塞尔曲线生成备选路径簇，具体如下：

x＝(1-τ)³x₀+3(1-τ)²τx₁+3(1-τ)τ²x₂+τ³x_f

y＝(1-τ)³y₀+3(1-τ)²τy₁+3(1-τ)τ²y₂+τ³y_f

在上式中，x₀、x₁、x₂、x_f分别为三次贝塞尔曲线第一个控制点、第二个控制点、第三个控制点、第四个控制点的横坐标，y₀、y₁，y₂、y_f分别为三次贝塞尔曲线第一个控制点、第二个控制点、第三个控制点、第四个控制点的纵坐标，τ为参变量。

3.根据权利要求1所述的基于几何路径生成的类人化自动驾驶轨迹规划方法，其特征在于，所述碰撞检测包括：

采用几何相交检测法，将主车安全范围定义为椭圆，障碍车安全范围定义为矩形，碰撞检测采样点范围定义为椭圆的外接矩形，判断该采样点是否同时在主车安全椭圆以及障碍车矩形内即可判断是否碰撞。