一种纵横向耦合的智能车辆轨迹规划方法及系统

1.本发明属于自动驾驶技术领域，具体涉及一种纵横向耦合的智能车辆轨迹规划方法及系统。


背景技术：

2.车辆智能化是当前车辆技术的主要发展方向之一，而自动驾驶技术是车辆智能化过程中的关键技术。一个具有自动驾驶功能的智能汽车，应当具有良好的感知功能、决策功能、轨迹规划功能和全线控执行机构。其中，轨迹规划功能是根据当前的周围环境规划出一条安全且高效的行驶轨迹，提供给执行机构进行相应的转向和加减速控制。轨迹规划技术是自动驾驶技术的重要组成部分，决定着自动驾驶车辆的行驶轨迹，影响着车辆行驶效率和安全性。
3.现有研究已经提出了一些有效的轨迹规划方法，中国发明专利申请号cn201910015057.1，名称为“一种复杂工况下自动驾驶车辆决策系统及其轨迹规划方法”中提出了一种车辆的路径和速度的双规划方法，该方法实现了轨迹的规划，但是没有考虑到车辆的纵横向运动的耦合，规划得到的轨迹可能会超出车辆动力学能力。中国发明专利申请号cn201911206096.6，名称为“基于模型预测控制的无人驾驶车辆动态避障轨迹规划方法”中提出了一种基于模型预测控制的无人驾驶车辆动态避障轨迹规划方法，该方法建立了具有避障功能代价函数的轨迹规划器，能够规划出安全的轨迹。现有的方法虽然能够规划出安全的行驶轨迹，但是没有考虑车辆的纵横向运动的耦合，规划得到的轨迹可能会超出车辆的动力学能力。
4.此外，现有的方法在轨迹规划过程中没有考虑车辆的燃油经济性和污染排放，不能充分提高车辆行驶的经济性和环保性。


技术实现要素：

5.针对于上述现有技术的不足，本发明的目的在于提供一种纵横向耦合的智能车辆轨迹规划方法及系统，本发明考虑了车辆的纵横向运动的耦合，规划更符合车辆动力学特性的轨迹，通过构建危险度目标函数、燃油消耗目标函数和排放目标函数，提高车辆行驶的安全性、经济性和环保性。
6.为达到上述目的，本发明采用的技术方案如下：
7.本发明的一种纵横向耦合的智能车辆轨迹规划方法，步骤如下：
8.1)获取车辆在当前t时刻的状态；采用深度决策网络方法决策出车辆在规划时域t
p
时间后车辆与同车道前车应当保持的车头时距车辆的目标车道；
9.2)规划车辆的纵向参考轨迹为：
10.11.式中，x
t
、分别为车辆在t时刻的沿着道路方向的纵向位置、纵向速度和纵向加速度；a
x
为车辆在规划时域t
p
内的纵向加速度，其为：
[0012][0013]
式中，和分别为同车道前车在当前时刻t的纵向行驶距离和纵向速度；采用五次多项式轨迹规划方法规划出车辆的横向参考轨迹；
[0014]
3)根据纵向参考轨迹和横向参考轨迹，采用基于模型预测控制的轨迹重规划方法求解出车辆的最终轨迹。
[0015]
进一步地，所述步骤1)中车辆在当前t时刻的状态x
t
为：
[0016][0017]
式中，x
t
、分别为车辆在t时刻的沿着道路方向的纵向位置、纵向速度和纵向加速度；y
t
、分别为车辆在t时刻的垂直于道路方向的横向位置、横向速度和横向加速度。
[0018]
进一步地，所述步骤1)中深度决策网络方法具体为：采用神经网络构建车辆的决策模块，神经网络的输入是车辆的状态，输出是车辆在规划时域t
p
时间后车辆与同车道前车应当保持的车头时距和车辆的目标车道；采用采集得到的数据训练神经网络，得到深度决策网络；深度决策网络根据当前时刻t的状态，决策出车辆在规划时域t
p
时间后车辆与同车道前车应当保持的车头时距和车辆的目标车道。
[0019]
进一步地，所述步骤2)中采用五次多项式轨迹规划方法规划车辆的横向参考轨迹具体为：车辆在规划时域t
p
时间后的状态为其中，车辆在规划时域t
p
时间后的横向位置为目标车道中心线的横向位置；车辆在规划时域t
p
时间后的横向速度和横向加速度均为0；五次多项式表示的车辆的横向轨迹为：
[0020][0021]
式中，bi为拟合参数；将车辆在t时刻的横向位置、横向速度、横向加速度和车辆在t+t
p
时刻的横向位置、横向速度、横向加速度代入上述五次多项式，即可求解出拟合参数bi，从而得到车辆的横向参考轨迹。
[0022]
进一步地，所述步骤3)中基于模型预测控制的轨迹重规划方法具体为：车辆的运动学方程为：
[0023][0024]
式中，ju、au、vu分别为车辆沿航向方向的加加速度、加速度和速度；l为车辆轴距；为车体航向角；δ为车辆前轮偏角；控制量设为μ＝[ju,δ]
t
；
[0025]
构建非线性模型预测控制问题为：
[0026][0027]
s.t.a
min
≤au≤a
max
[0028]vmin
≤vu≤v
max
[0029]jmin
≤ju≤j
max
[0030]
δ
min
≤δ≤δ
max
[0031]
式中，μi为t+i时刻的控制量，μi＝[ju(t+i),δ(t+i)]
t
；u为控制序列，nc为控制时域；η(t+i|t)为当前t时刻预测的t+i时刻的车辆的纵向位置和横向位置，η(t+i|t)＝[x(t+i),y(t+i)]
t
；η
ref
(t+i|t)为t+i时刻车辆的参考纵向位置和参考横向位置，将时间带入纵向参考轨迹和横向参考轨迹求解得到，η(t+i|t)＝[x
ref
(t+i),y
ref
(t+i)]
t
；n
p
为预测时域；q和r为权重矩阵；j
ob,i
为危险度目标函数；j
en,i
为燃油消耗目标函数；j
em,i
为排放目标函数；a
min
、a
max
分别为车辆沿航向方向的最小加速度和最大加速度；v
min
、v
max
分别为车辆沿航向方向的最小速度和最大速度；j
min
、j
max
分别为车辆沿航向方向的最小加加速度和最大加加速度；δ
min
、δ
max
分别为车辆的最小前轮偏角和最大前轮偏角；通过求解该非线性模型预测控制问题，得到最优控制序列将最优控制序列带入车辆运动学方程，可以得到最终轨迹η
*
＝[η
*
(t+1),η
*
(t+2),...,η
*
(t+nc)]
t
。
[0032]
进一步地，所述危险度目标函数为：
[0033][0034]
式中，n
ob
为障碍车辆数目；s
ob
为权重系数；k
ob1
、k
ob2
为常系数；σ
x
、σy为形状系数；ζ为小的正数，用于避免分母为0的情况；x
o,j
和y
o,j
分别为障碍物的纵向位置和横向位置；θ
o,j
为自车位置指向障碍车辆位置的方向与自车航向方向的夹角；
[0035]
所述燃油消耗目标函数为：
[0036][0037][0038]
式中，s
en
为权重系数；ηe为发动机消耗热能转换为车辆行驶的机械功的效率；e
idle
为车辆怠速时的热能消耗速率；m为车辆质量；f0、f1为滚动阻力系数模型中的系数；cd为空气阻力系数；a为迎风面积；ρ为空气密度；
[0039]
所述排放目标函数为：
[0040][0041]
式中，s
em
为权重系数；ω
c,1
、ω
c,2
、ω
c,3
、ω
t,1
、ω
t,2
为车辆排放模型的系数，通过对实验数据拟合得到。
[0042]
本发明还提供一种纵横向耦合的智能车辆轨迹规划系统，包括：
[0043]
决策模块，用于获取车辆在当前时刻的状态，并采用深度决策网络方法决策出车辆在规划时域t
p
时间后车辆与同车道前车应当保持的车头时距、车辆的目标车道；
[0044]
轨迹规划模块，用于规划车辆的纵向参考轨迹和横向参考轨迹；
[0045]
轨迹生成模块，根据纵向参考轨迹和横向参考轨迹，采用基于模型预测控制的轨迹重规划方法求解出车辆的最终轨迹。
[0046]
本发明的有益效果：
[0047]
本发明通过考虑车辆的纵横向运动的耦合，使得规划得到的轨迹更符合车辆动力学特性，车辆能够更容易跟踪该轨迹行驶；
[0048]
本发明还在非线性模型预测控制问题中构建了危险度目标函数、燃油消耗目标函数和排放目标函数，使得规划得到的轨迹更具有环境友好性，显著提高了车辆行驶的安全性、燃油经济性，降低了车辆的有害尾气的排放。
附图说明
[0049]
图1为本发明方法流程图。
[0050]
图2为车辆的轨迹示意图。
具体实施方式
[0051]
为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。
[0052]
参照图1所示，本发明的一种纵横向耦合的智能车辆轨迹规划方法，步骤如下：
[0053]
1)获取车辆在当前t时刻的状态；采用深度决策网络方法决策出车辆在规划时域t
p
时间后车辆与同车道前车应当保持的车头时距车辆的目标车道；
[0054]
所述步骤1)中车辆在当前t时刻的状态x
t
为：
[0055][0056]
式中，x
t
、分别为车辆在t时刻的沿着道路方向的纵向位置、纵向速度和纵向加速度；y
t
、分别为车辆在t时刻的垂直于道路方向的横向位置、横向速度和横向加速度。
[0057]
所述深度决策网络方法具体为：采用神经网络构建车辆的决策模块，神经网络的输入是车辆的状态，输出是车辆在规划时域t
p
时间后车辆与同车道前车应当保持的车头时距和车辆的目标车道；采用采集得到的数据训练神经网络，得到深度决策网络；深度决策网络根据当前时刻t的状态，决策出车辆在规划时域t
p
时间后车辆与同车道前车应当保持的车头时距和车辆的目标车道。
[0058]
2)规划车辆的纵向参考轨迹为：
[0059][0060]
式中，x
t
、分别为车辆在t时刻的沿着道路方向的纵向位置、纵向速度和纵向加速度；a
x
为车辆在规划时域t
p
内的纵向加速度，其为：
[0061][0062]
式中，和分别为同车道前车在当前时刻t的纵向行驶距离和纵向速度；采用五次多项式轨迹规划方法规划出车辆的横向参考轨迹；
[0063]
其中，所述步骤2)中采用五次多项式轨迹规划方法规划车辆的横向参考轨迹具体为：车辆在规划时域t
p
时间后的状态为其中，车辆在规划时域t
p
时间后的横向位置为目标车道中心线的横向位置；车辆在规划时域t
p
时间后的横向速度和横向加速度均为0；五次多项式表示的车辆的横向轨迹为：
[0064][0065]
式中，bi为拟合参数；将车辆在t时刻的横向位置、横向速度、横向加速度和车辆在t+t
p
时刻的横向位置、横向速度、横向加速度代入上述五次多项式，即可求解出拟合参数bi，从而得到车辆的横向参考轨迹。
[0066]
3)根据纵向参考轨迹和横向参考轨迹，采用基于模型预测控制的轨迹重规划方法求解出车辆的最终轨迹；参照图2所示，
[0067]
所述基于模型预测控制的轨迹重规划方法具体为：车辆的运动学方程为：
[0068][0069]
式中，ju、au、vu分别为车辆沿航向方向的加加速度、加速度和速度；l为车辆轴距；为车体航向角；δ为车辆前轮偏角；状态量设为控制量设为μ＝[ju,δ]
t
；
[0070]
构建非线性模型预测控制问题为：
[0071][0072]
s.t.a
min
≤au≤a
max
[0073]vmin
≤vu≤v
max
[0074]jmin
≤ju≤j
max
[0075]
δ
min
≤δ≤δ
max
[0076]
式中，μi为t+i时刻的控制量，μi＝[ju(t+i),δ(t+i)]
t
；u为控制序列，nc为控制时域；η(t+i|t)为当前t时刻预测的t+i时刻的车辆的纵向位置和横向位置，η(t+i|t)＝[x(t+i),y(t+i)]
t
；η
ref
(t+i|t)为t+i时刻车辆的参考纵向位置和参考横向位置，将时间带入纵向参考轨迹和横向参考轨迹求解得到，η(t+i|t)＝[x
ref
(t+i),y
ref
(t+i)]
t
；n
p
为预测时域；q和r为权重矩阵；j
ob,i
为危险度目标函数；j
en,i
为燃油消耗目标函数；j
em,i
为排放目标函数；a
min
、a
max
分别为车辆沿航向方向的最小加速度和最大加速度；v
min
、v
max
分别为车辆沿航向方向的最小速度和最大速度；j
min
、j
max
分别为车辆沿航向方向的最小加加速度和最大加加速度；δ
min
、δ
max
分别为车辆的最小前轮偏角和最大前轮偏角；通过求解该非线性模型预测控制问题，得到最优控制序列将最优控制序列带入车辆运动学方程，可以得到最终轨迹η
*
＝[η
*
(t+1),η
*
(t+2),...,η
*
(t+nc)]
t
。
[0077]
所述危险度目标函数为：
[0078][0079]
式中，n
ob
为障碍车辆数目；s
ob
为权重系数；k
ob1
、k
ob2
为常系数；σ
x
、σy为形状系数；ζ为小的正数，用于避免分母为0的情况；x
o,j
和y
o,j
分别为障碍物的纵向位置和横向位置；θ
o,j
为自车位置指向障碍车辆位置的方向与自车航向方向的夹角；
[0080]
所述燃油消耗目标函数为：
[0081][0082][0083]
式中，s
en
为权重系数；ηe为发动机消耗热能转换为车辆行驶的机械功的效率；e
idle
为车辆怠速时的热能消耗速率；m为车辆质量；f0、f1为滚动阻力系数模型中的系数；cd为空气阻力系数；a为迎风面积；ρ为空气密度；
[0084]
所述排放目标函数为：
[0085][0086]
式中，s
em
为权重系数；ω
c,1
、ω
c,2
、ω
c,3
、ω
t,1
、ω
t,2
为车辆排放模型的系数，通过对实验数据拟合得到。
[0087]
本发明还提供一种纵横向耦合的智能车辆轨迹规划系统，包括：
[0088]
决策模块，用于获取车辆在当前时刻的状态，并采用深度决策网络方法决策出车辆在规划时域t
p
时间后车辆与同车道前车应当保持的车头时距、车辆的目标车道；
[0089]
轨迹规划模块，用于规划车辆的纵向参考轨迹和横向参考轨迹；
[0090]
轨迹生成模块，根据纵向参考轨迹和横向参考轨迹，采用基于模型预测控制的轨迹重规划方法求解出车辆的最终轨迹。
[0091]
本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。