CN116654017A - 一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法、系统 - Google Patents

Abstract

本发明公开了一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法、系统，构建物理引导的图卷积神经网络轨迹预测模型，得到周车预测轨迹纵、横向坐标，以及纵、横向坐标预测误差的概率密度函数，然后进行驾驶安全等级预测评估，确定当前自车是否需要避撞，需要进行避撞时，优先选择制动避撞，当不满足制动避撞条件时进行自车制动避撞和转向换道避撞风险预测，选择其中预测风险较小的避撞形式；在选择转向换道避撞时，通过改进的人工势场规划车辆未来时间段内的避撞路径并进行优化。本发明能提高轨迹预测精度，同时保证行车安全性与稳定性。

Description

一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路 径规划的方法、系统

技术领域

本发明属于交通安全评价和智能交通技术领域，具体涉及一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法、系统。

背景技术

近年来，全球道路交通事故频发，为提高道路交通安全，避免和减轻交通事故伤害成为各国迫切需要解决的问题。而兴起的自动驾驶技术为解决交通安全问题提供了可能。其中，避撞控制技术是实现车辆自动驾驶的核心技术。

传统的路径规划更多考虑获取环境静态信息，通过不断感知周围环境信息来更新规划路径，如传统人工势场方法、Bezier曲线方法及多项式方法等方法。但在高速紧急避撞情况下，短时间内车辆状态会发生较大改变，如果不能提前判断周车未来运动轨迹，而是仅根据周车静态信息来更新自车规划路径会造成自车在紧急情况下过多调整自身行驶状态，从而难以完成有效避撞。对此，近几年有研究人员尝试对周边车辆进行轨迹预测，并将周车预测轨迹点用于后续的车辆避撞路径规划研究，但仍旧存在以下几点问题：

(1)现有车辆轨迹预测模型大多存在运动模型考虑要素单一或者过度依赖于历史轨迹数据的准确性，如使用理想化的数据来获得预测结果等问题；

(2)未考虑车辆轨迹预测结果的误差对自车采取避撞决策的影响，缺乏对自车采取不同决策所带来的不同驾驶风险的提前判断；

(3)未考虑基于周围车辆预测轨迹所预测的驾驶风险对避撞路径规划的影响。

发明内容

针对现有技术中存在不足，本发明提供了一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法、系统。

本发明是通过以下技术手段实现上述技术目的的。

一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法：

(1)将同车道前车历史轨迹数据和相邻车道前后车历史轨迹数据，输入物理引导的图卷积神经网络轨迹预测模型，得到周车预测轨迹纵、横向坐标，以及纵、横向坐标预测误差的概率密度函数；

所述物理引导的图卷积神经网络轨迹预测模型根据物理引导的总损失函数学习得到，所述物理引导的总损失函数包括初始损失函数和物理引导的损失函数；

所述物理引导的损失函数为：

其中：是预测时刻t+kΔt第i辆车所对应的物理模型输出的位置坐标预测值/>和/>之间的损失函数，β_k是随预测时刻t+kΔt增加而递减的参数，R(f)用于表示衡量模型复杂度的模型结构误差，λ表示模型结构误差参数，N是样本车辆总数，K是预测时长对应的最大预测步数；

(2)基于物理引导的图卷积神经网络轨迹预测模型的预测结果，进行驾驶安全等级预测评估，确定当前自车是否需要避撞，需要进行避撞时，优先选择制动避撞，当不满足制动避撞条件时进行自车制动避撞和转向换道避撞风险预测，选择其中预测风险较小的避撞形式；在选择转向换道避撞时，通过改进的人工势场规划车辆未来时间段内的避撞路径；

①所述自车制动避撞预测，具体为：

将两车预测碰撞风险PDR_t+bΔt，q分解为两个因素的乘积：考虑周围车辆未来运动不确定性的碰撞概率PCP_t+bΔt,q和预期的碰撞严重程度CS_t+bΔt，q，且：

其中：PCP_i+bΔt，q为预测碰撞时刻t+bΔt所对应的自车与障碍车q的碰撞概率， 分别为障碍车q在预测碰撞时刻t+bΔt的纵、横坐标预测误差概率密度函数，[x_1,b,x_2,b]、[y_1,b,y_2,b]分别为自车纵、横向置区间下限和上限；

CS_t+bΔt，q＝CS_t+bΔt,q(σ)*CS_t+bΔt,q(Δv)*CS(M_q)

其中：CS_t+bΔt，q(σ)为基于碰撞角度的碰撞严重程度，CS_t+bΔt，q(Δv)为基于碰撞相对速度的碰撞严重程度，CS(M_q)为基于两车质量的碰撞严重程度；

②所述转向换道避撞风险预测，具体为：

根据各预测时刻自车的规划运动状态和各障碍车的预测运动状态，计算预测时间区间(t,t+CΔt]内各障碍车对无障碍换道规划轨迹下自车所产生的预测碰撞风险，第h辆障碍车对自车向左换道预测风险记为对自车向右换道预测风险记为/>自车左、右换道预测换道风险为C个预测时刻预测碰撞风险的最大值，选取左、右换道预测换道风险较小的作为转向换道避撞预测风险；其中h＝1、2……H，H表示自车周围障碍车的编号；

③所述改进的人工势场规划车辆未来时间段内的避撞路径是通过道路总势能场求解方向力，进而计算梯度角，再计算自车避撞路径点；

所述道路总势能场是将方向引力势场、道路边界斥力势场、目标车道引力势场和障碍物势场叠加，所述障碍物势场包括静态障碍物风险场和动态障碍物风险场，所述动态障碍物风险场引入风险度量-预测驾驶风险指标：

其中：X、Y为道路任意点坐标，动态障碍物势场 PDR^{t +kΔt},h(X,Y)表示假设自车以当前时刻t所对应的运动状态在预测时刻t+kΔt出现于任意坐标点(X,Y)处与障碍车h的碰撞概率和预期碰撞严重程度的乘积，/>和/>分别为预测时刻t+kΔt所对应的第h辆障碍车的纵、横坐标，B_d是动态障碍物势场调节系数，L_obsh和W_obsh分别为第h辆障碍车的长度和宽度。

进一步地：

其中：和/>均是偏差量，RELU表示线性整流函数。

进一步地，所述纵、横向坐标预测误差的概率密度函数是通过将不同驾驶意图及不同预测时刻t+kΔt下的车辆位置坐标预测误差集合通过高斯分布进行拟合计算得到的：

其中：Norm表示高斯分布概率密度函数，μ表示均值，σ表示方差，M_j表示驾驶意图，x、y分别表示当前时刻t车辆纵、横坐标；

所述不同预测时刻t+kΔt下的车辆位置坐标预测误差集合为：

其中：ERROR表示物理引导的图卷积神经网络轨迹预测模型输出的车辆位置坐标预测误差值，i₁表示驾驶意图被判别为换道的车辆编号，i₂表示驾驶意图为按原车道行驶的车辆编号，i₁＝1,2,...,N₁、i₂＝1,2,...,N₂，N₁+N₂＝N。

进一步地，驾驶安全等级预测评估，具体为：

若存在某预测时刻t+pΔt自车周围存在编号为q的障碍车满足以下条件，则自车预测驾驶状态被评估为危险等级：

PCP_t+pΔt,q＞PCP_Thr

其中：PCP_t+pΔt,q为预测时刻t+pΔt所对应的自车与障碍车q的碰撞概率，PCP_Thr为预设安全阈值，分别为障碍车q在预测时刻t+pΔt的纵、横向坐标预测误差的概率密度函数，x_1,p、x_2,p分别表示自车纵向位置区间的下限和上限，y_1,p、y_2,p分别表示自车横向位置区间的下限和上限；且p∈(0，K]、q∈[1,U]，U是障碍车总数。

更进一步地，自车纵向位置区间的下限和上限是由该时刻自车预测x坐标x_ego ^t+pΔt沿道路纵向方向向后、前延伸距离所得到的坐标：

其中：L_ego和L_q分别为自车和障碍车q的车辆长度

自车横向位置区间的下限和上限是由自车预测y坐标t_ego ^t+pΔt沿道路横向方向向右、左延伸距离所得到的坐标：

其中：W_ego和W_q分别为自车和障碍车q的车辆宽度。

进一步地，基于碰撞角度的碰撞严重程度为：

其中：θ'是预测碰撞时刻t+bΔt自车与障碍车车航向角的夹角，θ'＝0°表示全正面碰撞，45°＜θ'≤90°表示1/3重叠碰撞，15°＜θ'≤45°表示2/3重叠碰撞，0°＜θ'≤15°表示全重叠碰撞。

进一步地，对车辆未来时间段内的避撞路径进行优化，建立的优化问题为：

s.t.u_min≤u≤u_max

Δu_min≤Δu≤Δu_max

|β|≤β_max

其中：目标函数为：

其中：(t+i|t)表示在当前时刻t之后i步的预测值，N_p表示预测时域，N_c表示控制时域，Q、S分别代表各部分的权重，ρ、ε分别为松弛权重和松弛因子，U_APF表示道路总势能场，为轮胎侧偏角，β为质心侧偏角，μ为路面附着系数，Δu为控制增量。

进一步地，所述物理引导的图卷积神经网络轨迹预测模型是将图卷积神经网络与基于物理的轨迹预测模型相结合的混合轨迹预测模型。

更进一步地，基于物理的轨迹预测模型在车辆驾驶意图被判别为按原车道行驶时，选取恒定加速度模型作为相应的模型，在车辆驾驶意图被判别为换道时，选取恒定转弯率和加速度模型作为相应的模型。

一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的系统，包括：

轨迹预测模块，通过物理引导的图卷积神经网络轨迹预测模型，得到周车预测轨迹纵、横向坐标，以及纵、横向坐标预测误差的概率密度函数；

避撞决策模块，进行驾驶安全等级预测评估，确定当前自车是否需要避撞，需要进行避撞时，优先选择制动避撞，当不满足制动避撞条件时进行换道避撞；

避撞路径规划模块，在转向换道避撞时，通过改进的人工势场规划车辆未来时间段内的避撞路径并进行优化。

本发明的有益效果为：

(1)构建一种将图卷积神经网络与基于物理的轨迹预测模型相结合的混合预测模型-物理引导的图卷积神经网络PG-GCN轨迹预测模型，通过构建随预测时刻变化的物理引导的损失函数，将物理模型和数据驱动模型的优势相结合，提高轨迹预测精度。

(2)结合PG-GCN轨迹预测模型的预测结果及误差项，提出的风险度量-预测驾驶风险PDR，建立紧急工况下基于不同避撞措施风险预测的避撞决策机制，对不同决策方式的行车风险进行预测，选择风险最低的决策方式。

(3)基于PDR风险度量对人工势场模型进行改进，对自车转向换道避撞路径进行规划并结合控制器进行优化，同时保证行车安全性与稳定性。

附图说明

图1为本发明所述碰撞概率计算示意图；

图2为本发明所述智能汽车避撞决策与路径规划的方法流程图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

如图2所示，本发明一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，具体包括如下步骤：

步骤(1)，构建一种将图卷积神经网络轨迹预测模型与基于物理的轨迹预测模型相结合的混合轨迹预测模型，称为物理引导的图卷积神经网络(PG-GCN，Physics-guidedGraph Convolutional Network)轨迹预测模型。

由于关于车辆运动状态的物理变量(例如速度、加速度和转向角速度等)在空间和时间上变化，车辆轨迹预测问题通常涉及高度的复杂性，目前主要包括基于物理模型和基于数据驱动学习的两类预测方法。基于物理的轨迹预测模型通常是基于一些简单的假设，如恒定速度或加速度，对于短期预测范围有较高的精度同时也具有较高的可解释性，但忽略了驾驶员行为意图的不确定性；而图卷积神经网络等数据驱动模型可以捕获非线性或更复杂的道路行为，更适用于长期预测范围，但存在着过度依赖于历史轨迹数据质量的缺点。因此本发明考虑将基于物理的轨迹预测结果融入传统的图卷积神经网络的损失函数中，以指导图卷积神经网络的学习，从而提高轨迹预测精度。以下是具体步骤：

步骤(1.1)，本发明通过车辆航向角θ来判别车辆驾驶意图，设定航向角阈值为1°，设预测步长为Δt，当连续三个时刻即过去时刻t-2Δt、t-Δt以及当前时刻t所对应的航向角皆满足大于预设阈值，即|θ_t-2Δt|＞1°、|θ_t-Δt|＞1°且|θ_t|＞1°时，车辆驾驶意图被判别为换道，记作M₁；否则，车辆驾驶意图被判别为按原车道行驶，记作M₂。假设当前时刻t车辆纵坐标为x，车辆横坐标为y，车辆纵横向车速分别为v_x、v_y，车辆纵横向加速度分别为a_x、a_y，航向角为θ，横摆角速度为ω；选取恒定加速度模型(CA)、恒定转弯率和加速度模型(CTRA)分别作为车辆驾驶意图为按原车道行驶和换道时基于物理的轨迹预测模型，以下是具体步骤：

步骤(1.1.1)，当车辆驾驶意图被判别为按原车道行驶(M₂)时，选取恒定加速度模型(CA)作为基于物理的轨迹预测模型。

在CA模型中，当前时刻t所对应的车辆空间状态表示为：

s_CA(t)＝(x,y,v_x,v_y,a_x,a_y)

预测时刻t+kΔt(k∈(0,K])(K表示预测时长对应的最大预测步数)下，车辆纵向位移Δx⁽²⁾和横向位移Δy⁽²⁾分别由下式给出：

Δx⁽²⁾＝v_x*kΔt+0.5a_x*(kΔt)²

Δy⁽²⁾＝v_y*kΔt+0.5a_y*(kΔt)²

则预测时刻t+kΔt所对应的车辆空间状态表示为：

s_CA(t+kΔt)＝(x+Δx⁽²⁾,y+Δy⁽²⁾,v_x+kΔt*a_x,v_y+kΔt*a_y,a_x,a_y)

即当车辆驾驶意图被判别为按原车道行驶(M₂)时，预测时刻t+kΔt车辆的纵、横向坐标分别为：

车辆纵向预测速度为横向预测速度为预测航向角保持不变θ_t+bΔt＝θ。

步骤(1.1.2)，当车辆驾驶意图被判别为换道(M₁)时，选取恒定转弯率和加速度模型(CTRA)作为基于物理的轨迹预测模型。

在CTRA模型中，当前时刻t所对应的车辆空间状态表示为：

s_CTRA(t)＝(x,y,θ,v_x,v_y,a_x,a_y,ω)

预测时刻t+kΔt(k∈(0,K])下，车辆纵向位移Δx⁽¹⁾和车辆横向位移Δy⁽¹⁾分别由下式给出：

则预测时刻t+kΔt所对应的车辆空间状态表示为：

s_CTRA(t+kΔt)＝(x+Δx⁽¹⁾,y+Δy⁽¹⁾,θ+ωkΔt,v_x+a_xkΔt,v_y+a_ykΔt,a_x,a_y,ω)

即当车辆驾驶意图被判别为换道(M₁)时，预测时刻t+kΔt车辆的纵、横向坐标分别为：

步骤(1.2)，传统的图卷积神经网络通常采用平均绝对误差(Mean AbsoluteError，MSE)等误差指标作为训练损失函数(Loss函数)。本发明在传统图卷积神经网络中添加物理引导的损失函数，以指导图卷积神经网络的学习。本发明以当前时刻t所对应的车辆状态x_t,y_t,θ_t,为输入特征，输出不同预测时刻t+kΔt(k∈(0,K])所对应的车辆状态/>具体步骤如下：

步骤(1.2.1)，选取车辆轨迹数据集中N辆车的历史轨迹数据作为模型训练样本，为每一样本车辆(i＝1,2,…,N)筛选其相邻范围30m内的车辆作为周围车辆，提取样本车辆与其周围车辆的交互信息，并将其作为主要输入特征构建PG-GCN轨迹预测模型。根据周围车辆的预测运动状态和真实运动状态，构建初始损失函数为：

其中：为预测时刻t+kΔt第i辆车所对应的PG-GCN神经网络输出的位置坐标预测值/>(/>和/>分别是使用PG-GCN神经网络预测出的纵、横向坐标)和真实位置坐标/>之间的损失函数；为预测时刻t+kΔt第i辆车所对应的PG-GCN神经网络输出的航向角预测值/>和真实值/>之间的损失函数；/>为预测时刻t+kΔt第i辆车所对应的PG-GCN神经网络输出的速度预测值/>和真实值之间的损失函数；/>为预测时刻t+kΔt第i辆车所对应的PG-GCN神经网络输出的加速度预测值/>和真实值之间的损失函数；/>为预测时刻t+kΔt第i辆车所对应的PG-GCN神经网络输出的横摆角速度预测值/>和真实值/>之间的损失函数。所有Loss函数采用平均绝对误差MSE形式表达。

步骤(1.2.2)，按步骤(1.1)确定预测开始时刻(即t时刻)第i辆车的驾驶意图m_i为换道(m_i＝M₁)或按原车道行驶(m_i＝M₂)；根据驾驶意图预测结果，选取车辆的纵、横向坐标作为主要的预测变量用作构建物理引导的损失函数，构建物理引导的损失函数为：

其中：为预测时刻t+kΔt第i辆车所对应的物理模型输出的位置坐标预测值/>和/>之间的损失函数(即物理模型引导下的额外误差项)，旨在保持预测结果和物理定律之间的一致性；β_k是随预测时刻t+kΔt增加而递减的参数，即当预测时刻越远即k越大，物理模型预测结果对神经网络学习的影响越小；R(f)用于表示衡量模型复杂度的模型结构误差，λ表示模型结构误差参数。其中物理模型引导下的额外误差项/>定义如下：

其中：RELU表示线性整流函数。

即当偏差量及/>非零时被视为违反物理定律，物理模型引导下的额外误差项将被激活，从而使预测结果与物理定律尽量保持一致性。

步骤(1.2.3)，最终得到PG-GCN轨迹预测模型的总损失函数表示为：

PG-GCNLoss＝GCNLoss+PGLoss

步骤(1.2.4)，根据步骤(1.2.3)物理引导的总损失函数学习得到PG-GCN轨迹预测模型，基于训练样本获得不同预测时刻t+kΔt及驾驶意图(M_j,j＝1,2)下车辆位置x、y坐标的预测误差的集合，分别表示为：

其中：ERROR表示PG-GCN模型输出的车辆位置坐标预测误差值，驾驶意图被判别为换道即M₁的车辆编号记作i₁(i₁＝1,2,...,N₁)，驾驶意图被判别为按原车道行驶即M₂的车辆编号记作i₂(i₂＝1,2,...,N₂)，N₁+N₂＝N。

将上述不同驾驶意图(M_j,j＝1,2)及不同预测时刻t+kΔt下的车辆位置坐标预测误差集合通过高斯分布进行拟合计算，获得相应的纵、横向坐标预测误差的概率密度函数如下，用于接下来的避撞决策模块：

其中：Norm表示高斯分布概率密度函数，μ表示均值，σ表示方差。

步骤(2)，结合PG-GCN网络的预测结果误差项，本发明提出一个新的风险度量-预测驾驶风险PDR(Predicted Driving Risk)，建立紧急工况下的避撞决策机制，所述步骤(2)具体通过以下子步骤来实现：

步骤(2.1)，首先确定当前自车是否需要避撞。

假设自车按原车道行驶且运动状态不变(速度、加速度不变)，按30m范围标准筛选出自车周围车辆作为障碍车(编号记作1，2，…，U)，对各个障碍车按步骤(1)实时判别驾驶意图并通过PG-GCN轨迹预测模型输出不同时刻t+kΔt下的预测位置坐标。若存在某预测时刻t+pΔt(p∈(0,K])自车周围存在编号为q(q∈[1,U])的障碍车满足以下条件，则自车预测驾驶状态被评估为危险等级：

PCP_t+pΔt,q＞PCP_Thr

其中：PCP_t+pΔt,q为预测时刻t+pΔt所对应的自车与障碍车q的碰撞概率，分别为障碍车q在预测时刻t+pΔt由PG-GCN模型输出的x、y坐标预测误差概率密度函数；x_1,p、x_2,p分别表示自车纵向位置区间的下限和上限，即由该时刻自车预测x坐标(由恒加速度CA模型计算得到)沿道路纵向方向向后、前延伸/>距离所得到的坐标：

L_ego和L_q分别为自车和障碍车q的车辆长度；

y_1,p、y_2,p分别表示自车横向位置区间的下限和上限，即由自车预测y坐标(由恒加速度CA模型计算得到)沿道路横向方向向右、左延伸/>距离所得到的坐标：

W_ego和W_q分别为自车和障碍车q的车辆宽度；

PCP_Thr为预设安全阈值，本发明取0.6。上述碰撞概率的计算过程参见图1。

若不满足上述条件，则自车预测驾驶状态被评估为安全等级。当自车预测驾驶状态被评估为危险等级时，自车需要进行避撞决策，且自车与障碍车q的预测碰撞时间为t+pΔt(若存在多个预测时刻满足上述条件，则取离当前最近的预测时刻为预测碰撞时间)；当自车预测驾驶状态被评估为安全等级时，自车无需采取避撞措施。

步骤(2.2)，本发明将避撞决策分为制动避撞和换道避撞。

本发明在满足制动避撞条件的情况下，优先采用纵向制动避撞；如果不能满足制动避撞条件，则进入转向换道避撞条件判别步骤。其中，符合制动避撞条件为在预测碰撞时间t+pΔt前的任意时间点t+rΔt∈(t,t+pΔt]，满足：

PC_t+rΔt,q≤PCP_Thr

其中，PCP_t+rΔt，q为预测时刻t+pΔt前的任意时间点所对应的自车与障碍车q的碰撞概率，f^q _x，r、f^q _y，r分别为障碍车q在预测时刻t+pΔt前的任意时间点由PG-GCN模型输出的x、y坐标预测误差概率密度函数；用于计算自车纵、横向位置区间下限和上限([x_1,r,x_2,r]及[y_1,r,y_2,r])的自车预测坐标(x_ego ^t+pΔt,y_ego ^t+pΔt)为假设自车从当前时刻t开始采取最大制动减速度进行制动的预测结果。

步骤(2.3)，若不满足制动避撞条件，则进行自车制动避撞决策和转向换道避撞决策避撞风险预测。

步骤(2.3.1)，自车制动避撞风险预测

步骤(2.3.1.1)，寻找制动决策碰撞时间点：

求解以下联立方程：

其中：a_ego ^max为自车最大制动减速度，L_q表示障碍车q的车辆长度，v_egox表示自车纵向速度，x_q表示障碍车q的纵坐标，Δx_ego表示时间间隔bΔt内自车的纵向位移。

解方程组，即可获得自车与障碍车q的预测碰撞时刻t+bΔt：

步骤(2.3.1.2)，根据自车与障碍车q在预测碰撞时刻t+bΔt的预测运动状态，将两车预测碰撞风险PDR_t+bΔt，q分解为两个因素的乘积：考虑周围车辆未来运动不确定性的碰撞概率(PCP_t+bΔt,q)和预期的碰撞严重程度(CS_t+bΔt，q)。具体由如下步骤获取：

步骤(2.3.1.2.1)，两车碰撞严重程度计算

步骤(2.3.1.2.1.1)，假设自车从当前时刻t开始采取最大制动减速度进行制动，根据CA模型得到自车在预测碰撞时刻t+bΔt的运动状态，包括自车纵向速度横向速度/>和航向角θ_t+bΔt；同时通过步骤(1)获取的PG-GCN轨迹预测模型获得障碍车q在预测碰撞时刻t+bΔt的运动状态，包括障碍车q的纵向速度/>横向速度/>和航向角

步骤(2.3.1.2.1.2)，根据自车和障碍车q的碰撞角度(定义为预测碰撞时刻t+bΔt两车航向角的夹角取值范围为0°到90°)，将两车碰撞类型划分为全正面碰撞(θ'＝0°)、1/3重叠碰撞(45°＜θ'≤90°)、2/3重叠碰撞(15°＜θ'≤45°)和全重叠碰撞(0°＜θ'≤15°)，并定义基于碰撞角度的碰撞严重程度如下：

步骤(2.3.1.2.1.3)，定义基于碰撞相对速度的碰撞严重程度如下：

步骤(2.3.1.2.1.4)，定义基于两车质量的碰撞严重程度如下：

其中：M_ego为自车质量，M_q为障碍车q的质量。

步骤(2.3.1.2.1.5)，计算在预测碰撞时刻t+bΔt自车和障碍车q的预期的碰撞严重程度(CS_t+bΔt，q)，表达式为：

CS_t+bΔt，q＝CS_t+bΔt,q(σ)*CS_t+bΔt,q(Δv)*CS(M_q)

步骤(2.3.1.2.2)，两车碰撞概率计算

根据步骤(2.1)所述方法，预测碰撞时刻t+bΔt自车和障碍车q的碰撞概率计算方法如下：

其中，PCP_t+bΔt，q为预测碰撞时刻t+bΔt所对应的自车与障碍车q的碰撞概率， 分别为障碍车q在预测碰撞时刻t+bΔt由PG-GCN模型输出的x、y坐标预测误差概率密度函数，[x_1,b,x_2,b]、[y_1,b,y_2,b]分别为自车纵、横向置区间下限和上限。

步骤(2.3.1.2.3)，在自车作出制动决策情况下，自车与障碍车q在预测碰撞时刻t+bΔt所对应的预测驾驶风险PDR_t+bΔt，q表达式为：

PDR_brake＝PDR_t+bΔt，q＝PCP_t+bΔt，q*CS_t+bΔt，q

步骤(2.3.2)，转向换道决策风险预测

步骤(2.3.2.1)，假设自车周围无障碍车辆，使用五次多项式对自车进行无障碍换道轨迹规划。以自车当前时刻t所对应的运动状态为起始状态，假设换道完成时刻为预测时刻t+CΔt(C<K)，给定自车在换道完成时刻的运动状态为目标状态，则换道起终点自车状态可表示为：

其中，x^t，y^t,/>分别为当前时刻t所对应的自车的纵向位移、纵向速度、纵向加速度、横向位移、横向速度和横向加速度，x^t+CΔt,/>y^t+CΔt,/>分别为换道完成时刻t+CΔt所对应的自车的纵向位移、纵向速度、纵向加速度、横向位移、横向速度和横向加速度。

基于五次多项式的特性，分别在x方向和y方向选取五次多项式表示换道轨迹：

定义时间参数矩阵：

定义系数矩阵为：

为轨迹函数添加边界条件：

结合边界条件和约束条件，求出系数矩阵A、B：

/>

利用求得的系数矩阵，解方程组，得到自车无障碍左、右换道规划轨迹，进而得到各预测时刻自车的左换道规划运动状态和右换道规划运动状态，包括自车纵向速度横向速度/>和航向角θ_t+cΔt。

步骤(2.3.2.2)，在预测时间区间(t,t+CΔt]内，采用步骤(1)中的PG-GCN神经网络模型输出自车周围编号为1、2……H的障碍车在预测时刻t+cΔt∈[1、2…C]的运动状态，包括每辆障碍车的纵向速度横向速度/>和航向角/>根据各预测时刻自车的规划运动状态和各障碍车的预测运动状态，按步骤(2.3.1)所述方法分别计算预测时间区间(t,t+CΔt]内各障碍车对无障碍换道规划轨迹下自车所产生的预测碰撞风险。其中，第h辆障碍车对自车向左换道预测风险记为对自车向右换道预测风险记为

最终的左、右预测换道风险为C个预测时刻预测碰撞风险的最大值，即为：

若PDR_steerl＜PDR_steerr，则选取左侧车道作为规划的目标车道；若PDR_steerl≥PDR_steerr，则选取右侧车道作为规划的目标车道。

最终换道避撞预测风险表示为：

PDR_steer＝min{PDR_steerl,PDR_steerr}

步骤(2.3.3)，避撞决策表示为：

若PDR_steer＜PDR_brake，则采取转向换道避撞决策；若PDR_steer≥PDR_brake，则采取自车制动避撞决策；

步骤(3)，本发明将避撞决策分为制动避撞和转向换道避撞。制动避撞即自车以最大制动减速度按原车道行驶，不属于本发明的主要研究范围。本发明基于步骤(2)所提出的PDR风险度量对人工势场进行改进，并对自车转向换道避撞路径进行规划并结合控制器进行优化。所述步骤(3)具体通过以下子步骤来实现：

步骤(3.1)，构建预测时间区间t+kΔt内基于PDR风险度量改进的人工势场：

步骤(3.1.1)，构建方向引力势场：

一般情况下，车辆是沿着道路方向行驶，即方向引力势场引导着车辆向前行驶，在车辆起始点处势场值最大，随着远离起始点，引力势场逐渐变小，以使车辆沿着势场下降的方向到达目标位置，其函数表达式为：

U₁(X,Y)＝-K_fX

式中：K_f为方向引力势场调节系数，决定势场的下降速度；X、Y为道路任意点坐标。

步骤(3.1.2)，构建道路边界斥力势场：

道路交通系统中，道路边界会对车辆行驶产生约束作用。在构建人工势场模型时，车辆越靠近车道边界，行车风险越大。道路边界斥力势场表达式为：

其中：L为车道宽度，K_bj为道路边界势场调节系数。

步骤(3.1.3)，构建目标车道引力势场：

目标车道引力势场在自车转向换道避撞过程中提供指向目标车道中心线的引力，在自车换道过程中，距目标车道中心线越远，引力势能的值越大，目标车道引力势场表达式为：

其中：k_mb为目标车道引力势场调节系数，y_mb为目标车道横向坐标，x₀为自车换道起始点的纵向坐标，本发明将其定义为自车当前时刻t所对应的纵坐标，x₁为自车换道终点的纵向坐标，由以下安全距离模型确定：

x₁＝x₀+Δx

Δy＝|y₀-y_mb|

其中：Δx和Δy分别为自车避撞的纵、横向安全距离，y₀为自车当前位置横坐标，v_0x为自车当前纵向速度，a_0y为自车当前横向加速度。

步骤(3.1.4)，基于PDR风险度量改进的障碍物势场：

从运动中的自车角度出发，道路中静止和运动的物体都与其存在相对运动，且都有可能与其产生实际碰撞。本发明将障碍物区分为动态障碍物和静态障碍物，并分别构建动、静态障碍物风险场：

步骤(3.1.4.1)，本发明将静态障碍物形状统一看做矩形，考虑障碍物的形状和尺寸，使用高阶高斯函数表示静态障碍物风险场，表达式为：

其中，x_obsjt和y_obsjt分别表示静态障碍物在大地坐标系中的纵、横坐标；A_jt、B_jt是静态障碍物势场调节系数；和/>分别为静态障碍物的纵、横向长度；

步骤(3.1.4.2)，通过引入PDR风险指标来构建动态障碍物风险场：

当PDR越大，两车的预测行车风险也越高，反之则越低。行车风险场模型一般要求风险场与车辆行驶风险呈正相关，这与PDR所呈现出的趋势一致。通过在风险场模型中加入PDR风险指标可将自车与障碍车相对量(两车相对速度与纵、横向位置)纳入模型当中。具体方法如下：

在预测时间区间内(t,t+KΔt]，以预测时刻t+kΔt∈(t,t+KΔt]为例，自车周围共有H辆障碍车，第h辆障碍车与自车形成的预期的碰撞严重程度的表达式为：

CS_t+kΔt,h＝CS_t+kΔt,h(Δv)*CS_t+kΔt,h(ΔM)*CS_t+kΔt,h(σ)

其中，CS_t+kΔt,h(Δv)、CS_t+kΔt,h(ΔM)、CS_t+kΔt,h(σ)分别为根据预测时刻t+kΔt障碍车h与当前时刻t自车的运动状态计算得到的基于相对速度、质量及碰撞相对角度的预测碰撞严重程度。

用高阶高斯函数表示预测时刻t+kΔt由第h辆障碍车形成的动态障碍物势场：

其中，和/>分别为预测时刻t+kΔt所对应的第h辆障碍车的纵、横坐标，A_d、B_d是动态障碍物势场调节系数，L_obsh和W_obsh分别为第h辆障碍车的长度和宽度。

以PDR度量作为动态障碍物势场调节系数A_d对上述的动态障碍物斥力场进行改进，融入的PDR^t+kΔt,h(X,Y)表示为假设自车以当前时刻t所对应的运动状态在预测时刻t+kΔt出现于任意坐标点(X,Y)处所对应的与障碍车h的碰撞概率和预期碰撞严重程度的乘积：

其中分别为障碍车h在预测时刻t+kΔt由PG-GCN模型输出的x、y坐标预测误差概率密度函数。

最终在预测时刻t+kΔt，第h辆障碍车所形成的动态障碍物势场表示为：

则同时考虑自车周围编号为1、2……H的障碍车在预测时间区间(t,t+KΔt]内预测状态对自车造成风险的动态障碍物势场表示为：

步骤(3.1.5)，道路总势能场构建及使用改进的人工势场初次规划以当前时刻t为基准，未来(t,t+KΔt]时间区间内的避撞路径：

将方向引力势场、道路边界斥力势场、目标车道引力势场和障碍物势场进行叠加，即可得到道路总势能场，表达式为：

U_APF(X,Y)＝U₁(X,Y)+U₂(X,Y)+U₃(X,Y)+U₄(X,Y)+U₅(X,Y)

并应用梯度下降法计算相应的方向力，表达式为：

使用方向力计算梯度角φ，表达式为：

然后利用预设的离散步长Ls来计算自车避撞路径点，表达式为：

该等式可以基于所需路径点的预设数量在控制循环中迭代。

步骤(3.2)，基于改进人工势场初次规划路径的车辆避撞路径优化

上述基于改进人工势场初次规划路径为离散路径点，同时未考虑车辆动力学约束条件，因此需要基于改进的人工势场对自车避撞轨迹做进一步优化。

步骤(3.2.1)，车辆运动建模

本发明假设车辆的速度恒定，车辆运动状态x(t)及动力学模型表示为：

x(t)＝[x y β θ ω]

其中：x、y是大地坐标系下自车的纵、横坐标，β为自车质心侧偏角，θ为航向角，ω为横摆角速度。

车辆动力学模型表示为：

其中：M_ego为自车质量，v_x和v_y分别为自车质心的纵向和横向速度，为质心侧偏角速度，/>为横摆角加速度，I_z为车辆绕其z轴的转动惯量，F_yf和F_yr分别为前后轮胎的侧向力，l_f和l_r分别表示前、后轴距。

对于控制采样周期T_s，将连续的车辆动力学模型进行离散化，得到离散化后的预测模型：其中控制量u(t)是车辆的前轮转角，为实现自动驾驶车辆局部路径规划与跟踪的统一建模，本发明将势场作为一部分增加到模型预测控制器的目标函数中，利用其优化算法实现路径规划，该目标函数的形式如下：/>

其中：(t+i|t)表示在当前时刻t之后i步的预测值，N_p表示预测时域，N_c表示控制时域，Q、S分别代表各部分的权重，ρ、ε分别为松弛权重和松弛因子。

目标函数主要包括两部分:1)道路总势能场U_APF，通过建立的车辆动力学模型结合势场函数计算出预测时域内自车所受的势场值，势场值越大，则行车风险越大；2)控制增量Δu(t)，控制车辆运动状态避免出现大幅变化。

步骤(3.2.2)，约束条件设计

首先考虑对控制过程中的控制量和控制增量进行约束，前轮转角一般取值在-25°～25°之间，由于在目标函数中采用了控制增量，因而也须对其进行约束，前轮转角增量一般为-1°～1°。

同时，考虑到当车辆行驶在附着系数较低的路面时，须确保操纵稳定性，车辆的质心侧偏角或轮胎侧偏角是衡量车辆操纵稳定性的关键之一。车辆横向稳定性和质心侧偏角β有关，考虑到路面附着系数，质心侧偏角的约束为：

|β|≤β_max＝arctan(0.02μg)

其中，μ是路面附着系数，g为重力加速度。

车辆在极限工况下之所以会失稳，主要是因为轮胎与地面产生的轮胎力达到饱和，轮胎力饱和与轮胎侧偏角存在一定关系，因而可根据路面附着条件来约束轮胎侧偏角，轮胎侧偏角的约束如下：

其中，和/>分别为轮胎侧偏角的下限和上限。根据轮胎模型，轮胎力饱和时轮胎侧偏角与路面附着条件有关。

因此，自车避撞路径规划与跟踪可统一描述为如下优化问题：

s.t.u_min≤u≤u_max

Δu_min≤Δu≤Δu_max

|β|≤β_max

在每个控制周期内完成求解后，得到控制增量序列如下：

每次计算都取第1项作为此时刻控制输入，输入至车辆模型，如此循环迭代。

一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的系统，包括：

轨迹预测模块，通过车辆网环境下智能汽车环境感知系统获取同车道前车历史轨迹数据和相邻车道前后车历史轨迹数据，输入构建的物理引导的图卷积神经网络轨迹预测模型，得到周车预测轨迹x、y坐标和x、y坐标预测误差的概率密度函数；

避撞决策模块，基于物理引导的图卷积神经网络轨迹预测模型的预测结果，进行驾驶安全等级预测评估，确定当前自车是否需要避撞，需要进行避撞时，优先选择制动避撞，当不满足制动避撞条件时进行自车制动避撞和换道避撞风险预测，选择其中预测风险较小的避撞形式，在选择转向换道避撞时，通过改进的人工势场换道避撞路径规划模型生成规划避撞路径；

避撞路径规划模块，针对转向换道避撞路径规划问题，基于PDR风险度量对人工势场模型进行改进，对自车转向换道避撞路径进行规划并结合控制器进行优化。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (10)

1.一种紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，其特征在于：

(1)将同车道前车历史轨迹数据和相邻车道前后车历史轨迹数据，输入物理引导的图卷积神经网络轨迹预测模型，得到周车预测轨迹纵、横向坐标，以及纵、横向坐标预测误差的概率密度函数；

所述物理引导的图卷积神经网络轨迹预测模型根据物理引导的总损失函数学习得到，所述物理引导的总损失函数包括初始损失函数和物理引导的损失函数；

所述物理引导的损失函数为：

其中：是预测时刻t+kΔt第i辆车所对应的物理模型输出的位置坐标预测值/>和/>之间的损失函数，β_k是随预测时刻t+kΔt增加而递减的参数，R(f)用于表示衡量模型复杂度的模型结构误差，λ表示模型结构误差参数，N是样本车辆总数，K是预测时长对应的最大预测步数；

(2)基于物理引导的图卷积神经网络轨迹预测模型的预测结果，进行驾驶安全等级预测评估，确定当前自车是否需要避撞，需要进行避撞时，优先选择制动避撞，当不满足制动避撞条件时进行自车制动避撞和转向换道避撞风险预测，选择其中预测风险较小的避撞形式；在选择转向换道避撞时，通过改进的人工势场规划车辆未来时间段内的避撞路径；

①所述自车制动避撞预测，具体为：

将两车预测碰撞风险PDR_t+bΔt，q分解为两个因素的乘积：考虑周围车辆未来运动不确定性的碰撞概率PCP_t+bΔt,q和预期的碰撞严重程度CS_t+bΔt，q，且：

其中：PCP_t+bΔt，q为预测碰撞时刻t+bΔt所对应的自车与障碍车q的碰撞概率， 分别为障碍车q在预测碰撞时刻t+bΔt的纵、横坐标预测误差概率密度函数，[x_1,b,x_2,b]、[y_1,b,y_2,b]分别为自车纵、横向置区间下限和上限；

CS_t+bΔt，q＝CS_t+bΔt,q(σ)*CS_t+bΔt,q(Δv)*CS(M_q)

其中：CS_t+bΔt，q(σ)为基于碰撞角度的碰撞严重程度，CS_t+bΔt，q(Δv)为基于碰撞相对速度的碰撞严重程度，CS(M_q)为基于两车质量的碰撞严重程度；

②所述转向换道避撞风险预测，具体为：

根据各预测时刻自车的规划运动状态和各障碍车的预测运动状态，计算预测时间区间(t,t+CΔt]内各障碍车对无障碍换道规划轨迹下自车所产生的预测碰撞风险，第h辆障碍车对自车向左换道预测风险记为对自车向右换道预测风险记为/>自车左、右换道预测换道风险为C个预测时刻预测碰撞风险的最大值，选取左、右换道预测换道风险较小的作为转向换道避撞预测风险；其中h＝1、2……H，H表示自车周围障碍车的编号；

③所述改进的人工势场规划车辆未来时间段内的避撞路径是通过道路总势能场求解方向力，进而计算梯度角，再计算自车避撞路径点；

所述道路总势能场是将方向引力势场、道路边界斥力势场、目标车道引力势场和障碍物势场叠加，所述障碍物势场包括静态障碍物风险场和动态障碍物风险场，所述动态障碍物风险场引入风险度量-预测驾驶风险指标：

其中：X、Y为道路任意点坐标，动态障碍物势场，PDR^{t +kΔt},h(X,Y)表示假设自车以当前时刻t所对应的运动状态在预测时刻t+kΔt出现于任意坐标点(X,Y)处与障碍车h的碰撞概率和预期碰撞严重程度的乘积，/>和/>分别为预测时刻t+kΔt所对应的第h辆障碍车的纵、横坐标，B_d是动态障碍物势场调节系数，L_obsh和W_obsh分别为第h辆障碍车的长度和宽度。

2.根据权利要求1所述的紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，其特征在于：

其中：和/>均是偏差量，RELU表示线性整流函数。

3.根据权利要求1所述的紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，其特征在于，所述纵、横向坐标预测误差的概率密度函数是通过将不同驾驶意图及不同预测时刻t+kΔt下的车辆位置坐标预测误差集合通过高斯分布进行拟合计算得到的：

其中：Norm表示高斯分布概率密度函数，μ表示均值，σ表示方差，M_j表示驾驶意图，x、y分别表示当前时刻t车辆纵、横坐标；

所述不同预测时刻t+kΔt下的车辆位置坐标预测误差集合为：

其中：ERROR表示物理引导的图卷积神经网络轨迹预测模型输出的车辆位置坐标预测误差值，i₁表示驾驶意图被判别为换道的车辆编号，i₂表示驾驶意图为按原车道行驶的车辆编号，i₁＝1,2,...,N₁、i₂＝1,2,...,N₂，N₁+N₂＝N。

4.根据权利要求1所述的紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，其特征在于，驾驶安全等级预测评估，具体为：

若存在某预测时刻t+pΔt自车周围存在编号为q的障碍车满足以下条件，则自车预测驾驶状态被评估为危险等级：

PCP_t+pΔt,q＞PCP_Thr

其中：PCP_t+pΔt,q为预测时刻t+pΔt所对应的自车与障碍车q的碰撞概率，PCP_Thr为预设安全阈值，分别为障碍车q在预测时刻t+pΔt的纵、横向坐标预测误差的概率密度函数，x_1,p、x_2,p分别表示自车纵向位置区间的下限和上限，y_1,p、y_2,p分别表示自车横向位置区间的下限和上限；且p∈(0，K]、q∈[1,U]，U是障碍车总数。

5.根据权利要求4所述的紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，其特征在于，自车纵向位置区间的下限和上限是由该时刻自车预测x坐标x_ego ^{t +pΔt}沿道路纵向方向向后、前延伸距离所得到的坐标：

其中：L_ego和L_q分别为自车和障碍车q的车辆长度

自车横向位置区间的下限和上限是由自车预测y坐标y_ego ^t+pΔt沿道路横向方向向右、左延伸距离所得到的坐标：

其中：W_ego和W_q分别为自车和障碍车q的车辆宽度。

6.根据权利要求1所述的紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，其特征在于，基于碰撞角度的碰撞严重程度为：

其中：θ'是预测碰撞时刻t+bΔt自车与障碍车车航向角的夹角，θ'＝0°表示全正面碰撞，45°＜θ'≤90°表示1/3重叠碰撞，15°＜θ'≤45°表示2/3重叠碰撞，0°＜θ'≤15°表示全重叠碰撞。

7.根据权利要求1所述的紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，其特征在于，对车辆未来时间段内的避撞路径进行优化，建立的优化问题为：

s.t.u_min≤u≤u_max

Δu_min≤Δu≤Δu_max

|β|≤β_max

其中：目标函数为：

其中：(t+i|t)表示在当前时刻t之后i步的预测值，N_p表示预测时域，N_c表示控制时域，Q、S分别代表各部分的权重，ρ、ε分别为松弛权重和松弛因子，U_APF表示道路总势能场，为轮胎侧偏角，β为质心侧偏角，μ为路面附着系数，Δu为控制增量。

8.根据权利要求1所述的紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，其特征在于，所述物理引导的图卷积神经网络轨迹预测模型是将图卷积神经网络与基于物理的轨迹预测模型相结合的混合轨迹预测模型。

9.根据权利要求8所述的紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法，其特征在于，基于物理的轨迹预测模型在车辆驾驶意图被判别为按原车道行驶时，选取恒定加速度模型作为相应的模型，在车辆驾驶意图被判别为换道时，选取恒定转弯率和加速度模型作为相应的模型。

10.一种实现权利要求1-9任一项所述的紧急工况下基于周车轨迹预测的智能汽车避撞决策与路径规划的方法的系统，其特征在于，包括：

轨迹预测模块，通过物理引导的图卷积神经网络轨迹预测模型，得到周车预测轨迹纵、横向坐标，以及纵、横向坐标预测误差的概率密度函数；

避撞决策模块，进行驾驶安全等级预测评估，确定当前自车是否需要避撞，需要进行避撞时，优先选择制动避撞，当不满足制动避撞条件时进行换道避撞；

避撞路径规划模块，在转向换道避撞时，通过改进的人工势场规划车辆未来时间段内的避撞路径并进行优化。