1.本发明属于自动驾驶技术领域，尤其涉及一种考虑规划参数自适应的车辆行驶轨迹快速规划方法。
背景技术：
2.智能车辆的出现带来了诸多优势，如提高行驶安全性、舒适性和便捷性，改善道路利用率，减少交通拥堵和事故发生以及降低运输成本。因此，智能驾驶车辆的研究成为实现未来智能交通系统的关键。当前，智能车辆正处于从辅助驾驶向完全自动驾驶的转变阶段，而轨迹规划作为核心技术之一显得尤为重要。它必须能够应对各种复杂场景，以实现完全自动驾驶。设计合理的轨迹规划算法是关键，它能够规划出无碰撞路径，同时确保车辆运动的安全性、稳定性和舒适性，成为当前智能车辆研究的热点问题。
3.综上所述，智能车辆的发展推动了轨迹规划算法的研究，而轨迹规划算法的进一步成熟也会促进智能车辆的落地实现。如何设计合理的实时轨迹规划算法，以实现无碰撞路径的规划，并保证车辆运动的安全性、稳定性和舒适性，成为智能车辆领域的关键问题之一。为此我们提出一种考虑规划参数自适应的车辆行驶轨迹快速规划方法。
技术实现要素：
4.本发明的目的在于提供一种考虑规划参数自适应的车辆行驶轨迹快速规划方法，旨在解决上述背景技术中提出的问题。
5.为实现上述目的，本发明提供如下技术方案：
6.一种考虑规划参数自适应的车辆行驶轨迹快速规划方法，包括以下步骤：
7.步骤一、设置最优控制终端约束及目标函数，建立基于最优控制的轨迹规划器，利用高斯伪谱法进行最优控制问题离散求解；
8.步骤二、离线仿真获得大量规划轨迹数据，利用神经网络构建轨迹规划器的拟合模型；
9.步骤三、结合周车轨迹预测以及碰撞检测算法剔除可能发生碰撞的危险轨迹；
10.步骤四、基于多目标优选的轨迹评价函数进行评估，得到最优规划轨迹。
11.进一步的，所述步骤一中，建立基于最优控制的轨迹规划器的具体操作为：
12.建立非线性车辆六自由度模型
[0013][0014]
其中，状态变量为系统控制输入为u＝[a
x
,δf]
t
；xv,yv分别为车辆的纵向位移和侧向位移；v
x
,vy分别为车辆的纵向速度和侧向速度；分别为车辆横摆角和横摆角速度；a
x
为车辆纵向加速度；δf为车辆前轮转角；cf,cr分别为前、后轮胎的侧偏刚度；m为汽车质量；iz为车辆绕z轴的转动惯量；lf,lr分别为汽车质心到前轴、后轴的距离；
[0015]
设计性能指标函数jm和jn，其中jm同前轮转角和前轮转角变化量有关，jn同车辆加速度有关，具体表达式如下：
[0016]jm
＝l1δ
f2
l2δδ
f2
[0017][0018]
其中，为期望加速度；l1,l2分别为前轮转角项相关性能指标的权重系数；l3,l4分别为加速度变化和与期望加速度逼近程度项的权重系数；因此将轨迹优化问题描述为：
[0019][0020]
x(t
cl
)＝x
cl
[0021]
其中，a
x
为车辆纵向加速度，δf为车辆前轮转角，x0为动作开始时刻车辆状态，x
cl
为期望动作完成时刻的期望车辆状态，*min,*max分别代表相应变量的最小值和最大值。
[0022]
进一步的，所述步骤一中，通过设置不同的终端约束满足多种驾驶任务轨迹需求，具体包括：
[0023]
对于车道保持驾驶任务，设定约束条件为：动作完成时间时刻侧向位移等于期望位移，且终端时刻的横摆角、侧向速度和横摆角速度为0；
[0024]
对于弯道行驶驾驶任务，设定约束条件为：动作完成时间时刻侧向位移等于期望位移，横摆角等于期望的横摆角；
[0025]
对于掉头驾驶任务，设定约束条件为：动作完成时刻侧向位移等于期望位移，横摆角等于期望的横摆角，且侧向速度和横摆角速度为0；
[0026]
对于左右转弯驾驶任务，设定终端约束条件为：动作完成时间时刻纵向位移等于
期望纵向位移，横摆角等于期望的横摆角、侧向速度及横摆角速度为0。
[0027]
进一步的，所述利用高斯伪谱法进行最优控制问题离散求解步骤包括：
[0028]
时间变量映射、状态量和控制量离散、系统状态方程转换以及最优目标函数逼近。
[0029]
进一步的，所述步骤二中，bp神经网络拟合模型的结构参数包括：
[0030]
bp网络隐含层个数及神经元个数：隐含层个数为1，即单隐层网络结构，隐藏层神经元为40个，训练误差阈值设定为1e-5
；
[0031]
bp网络输入输出节点数：输入神经元为9，输出神经元为100个；
[0032]
传递函数以及训练算法：隐含层传递函数为双曲正切sigmoid型函数tansig，输出层传递函数为线性传递函数purelin，训练算法为levenberg-marquardt算法。
[0033]
进一步的，所述步骤三的具体操作为：
[0034]
计算所评估轨迹在每个采样点上与周车的距离r(i)：
[0035][0036]
其中xs(i)，ys(i)分别为自车在采样点i上的纵侧向位移，xf(i)，yf(i)分别为周围车在采样点i上的纵侧向位移；
[0037]
比较每个采样点上的自车与周车间的距离，若距离均大于安全距离r
safe
，则对应轨迹满足安全需求d
safe
(i)＝1，否则视为易发生碰撞危险轨迹，直接进行剔除；
[0038][0039]
进一步的，所述步骤四的具体操作为：
[0040]
建立三种评价指标：舒适度指标、动作完成效率指标以及对交通影响评估指标；
[0041]
所述舒适度定义为：
[0042][0043]
其中，jc是整个规划轨迹舒适度的评价函数，j
x
(t),jy(t)分别为自车纵向和横向关于时间t的冲击度大小，α1,α2分别为j
x
(t),jy(t)相应的权重系数，0,t
cl
分别为当前驾驶动作下的起始时间和期望动作完成时间；
[0044]
所述动作完成效率指标定义为：
[0045][0046]
其中，je为动作完成效率指标，t
max
为车辆动作完成所需的最大时间；
[0047]
所述交通影响评估指标定义为：
[0048][0049]
其中，ja为对交通影响的评价指标，δx为当前轨迹下车辆完成动作所需的纵向行驶位移，v
x
为车辆行驶时的纵向速度大小；
[0050]
总评价指标为：
[0051]jtotal
＝ω1jc ω2je ω3ja[0052]
其中，ω1,ω2,ω3分别为舒适度指标、动作完成效率指标以及对交通影响评估指
标的权重系数，且ω1 ω2 ω3＝1。
[0053]
与现有技术相比，本发明的有益效果是：
[0054]
1、本发明实现了多种驾驶任务规划器的统一形式设计，避免了多种轨迹规划器的频繁切换带来的实际问题，并实现了轨迹规划器的高精度求解；
[0055]
2、本发明采用神经网络近似拟合最优轨迹规划器的方法，将一部分计算负担由在线转移为离线，降低实时计算负担，保证了规划算法的快速性；
[0056]
3、本发明能够根据驾驶环境自适应调整轨迹规划参数，规划算法适应性强。
附图说明
[0057]
图1为本发明的结构框图。
[0058]
图2为本发明中多种驾驶任务切换场景示意图。
[0059]
图3为本发明中神经网络结构原理示意图。
[0060]
图4为本发明中完整规划轨迹数据提取示意图。
[0061]
图5为本发明中不同期望参数规划轨迹示意图。
具体实施方式
[0062]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
[0063]
以下结合具体实施例对本发明的具体实现进行详细描述。
[0064]
本发明一个实施例提供的一种考虑规划参数自适应的车辆行驶轨迹快速规划方法，包括以下步骤：
[0065]
步骤一、设置最优控制终端约束及目标函数，建立基于最优控制的轨迹规划器，利用高斯伪谱法进行最优控制问题离散求解；
[0066]
步骤二、离线仿真获得大量规划轨迹数据，利用神经网络构建轨迹规划器的拟合模型；
[0067]
步骤三、结合周车轨迹预测以及碰撞检测算法剔除可能发生碰撞的危险轨迹；
[0068]
步骤四、基于多目标优选的轨迹评价函数进行评估，得到最优规划轨迹。
[0069]
作为本发明的一种优选实施例，所述步骤一中，通过仔细分析和理解不同场景的特征，选择与之密切相关的终端状态参数。例如，在车道保持场景中，终端状态参数可能涉及车辆与道路中心线的横向偏移以及横向加速度等。而在掉头场景和转弯场景中，可能主要涉及到如横摆角或横摆角速度等其他状态参数。
[0070]
考虑以上运动学参数对轨迹的影响，同时结合车辆动力学特性，建立基于最优控制的轨迹规划器的具体操作为：
[0071]
建立非线性车辆六自由度模型
[0072][0073]
其中，状态变量为系统控制输入为u＝[a
x
,δf]
t
；xv,yv分别为车辆的纵向位移和侧向位移；v
x
,vy分别为车辆的纵向速度和侧向速度；分别为车辆横摆角和横摆角速度；a
x
为车辆纵向加速度；δf为车辆前轮转角；cf,cr分别为前、后轮胎的侧偏刚度；m为汽车质量；iz为车辆绕z轴的转动惯量；lf,lr分别为汽车质心到前轴、后轴的距离；
[0074]
设计性能指标函数jm和jn，其中jm同前轮转角和前轮转角变化量有关，jn同车辆加速度有关，具体表达式如下：
[0075]jm
＝l1δ
f2
l2δδ
f2
[0076]jn
＝l3δa
x2
l4(a
x-a
x*
)2[0077]
其中，为期望加速度；l1,l2分别为前轮转角项相关性能指标的权重系数；l3,l4分别为加速度变化和与期望加速度逼近程度项的权重系数；因此将轨迹优化问题描述为：
[0078][0079]
s.t.a
min
≤a
x
≤a
max
[0080]
δ
f,min
≤δf≤δ
f,max
[0081]
x(t0)＝x0[0082]
x(t
cl
)＝x
cl
[0083]
其中，a
x
为车辆纵向加速度，δf为车辆前轮转角，x0为动作开始时刻车辆状态，x
cl
为期望动作完成时刻的期望车辆状态，*min,*max分别代表相应变量的最小值和最大值。
[0084]
在本发明实施例中，优选的，基于最优控制的车辆行驶轨迹规划器，通过设置不同的终端约束满足多种驾驶任务轨迹需求，并利用高斯伪谱法实现最优控制问题离散求解。通过仔细分析和理解不同场景的特征，选择与之密切相关的终端状态参数。例如，在车道保持场景中，终端状态参数可能涉及车辆与道路中心线的横向偏移以及横向加速度等。而在掉头场景和转弯场景中，可能主要涉及到如横摆角或横摆角速度等其他状态参数。考虑以上运动学参数对轨迹的影响，同时结合车辆动力学特性，建立了车辆行驶轨迹规划器。
[0085]
作为本发明的一种优选实施例，所述步骤一中，通过设置不同的终端约束满足多种驾驶任务轨迹需求，以此保证轨迹的适应性、合理性以及在该动作下的任务完成程度。图2为多种驾驶任务切换场景示意图。具体包括：
[0086]
对于车道保持驾驶任务，在动作的终端时刻t
cl
，根据相应驾驶动作的特征，设定约
束条件为：动作完成时间时刻侧向位移等于期望位移，且终端时刻的横摆角、侧向速度和横摆角速度为0；具体公式表示如下：
[0087][0088]
其中，yc(t
cl
)、vy(t
cl
)和wr(t
cl
)分别为期望动作完成时刻t
cl
的侧向位移、横摆角、侧向速度和横摆角速度，y
cl
为以道路中心线为基准的期望侧向位移；
[0089]
对于弯道行驶驾驶任务，设定约束条件为：动作完成时间时刻侧向位移等于期望位移，横摆角等于期望的横摆角；具体公式表示如下：
[0090][0091]
其中，分别为期望动作完成时刻t
cl
的侧向位移和横摆角，y
cl
为以道路中心线为基准的期望侧向位移，为对应弯道行驶的期望横摆角；
[0092]
对于掉头驾驶任务，设定约束条件为：动作完成时刻侧向位移等于期望位移，横摆角等于期望的横摆角，且侧向速度和横摆角速度为0；具体公式表示如下：
[0093][0094]
其中，yc(t
cl
)、vy(t
cl
)和wr(t
cl
)分别为期望动作完成时刻t
cl
的侧向位移、横摆角、侧向速度和横摆角速度；
[0095]
对于左右转弯驾驶任务，设定终端约束条件为：动作完成时间时刻纵向位移等于期望纵向位移，横摆角等于期望的横摆角、侧向速度及横摆角速度为0；具体公式表示如下：
[0096][0097]
其中，xc(t
cl
)、vy(t
cl
)和wr(t
cl
)分别为期望动作完成时刻t
cl
的纵向位移、横摆角、侧向速度和横摆角速度；x
cl
为设定的期望纵向位移。
[0098]
作为本发明的一种优选实施例，针对最优控制问题的求解，由于高斯伪谱法能够通过插值方式近似积分，并利用离散点构建稀疏雅可比矩阵，且具备求解精度高等优势，因此选择高斯伪谱法进行离散求解。包括：
[0099]
a.时间变量映射
[0100]
首先将时间区间[0,tf]变换至配点区间[-1,1]上，其变换关系如下式：
[0101][0102]
其中，t0,tf分别为对应时间区间的起始时刻和终止时刻；
[0103]
b.状态量和控制量离散
[0104]
使用拉格朗日基函数li(τ)分别对状态量和控制量进行n阶拉格朗日插值，且根据拉格朗日多项式插值点处函数值不变、其他点处函数值为0的性质，在每个插值点上的状态变量x(τ)和控制变量u(τ)与原函数值x(τ),u(τ)相同：
[0105][0106][0107][0108]
c.系统状态方程转换
[0109]
通过状态量离散插值得到上述状态变量近似方程的基础上，对其进行微分求导，即可将其转换为代数约束：
[0110][0111]
其中，d
ni
为与优化问题无关的微分矩阵，且可以通过离线求解，则将最优控制问题中的状态方程转换为如下形式：
[0112][0113]
d.最优目标函数逼近
[0114]
利用gauss积分对目标函数进行近似，则转换为如下形式：
[0115][0116][0117]
其中，ωi为求积系数，ωi与被积函数的具体形式无关，仅与节点选取相关。则通过上述变换方法将原最优控制问题转换为了一般形式的非线性规划问题，可通过成熟应用的非线性优化求解器进行优化问题求解。
[0118]
如图3和图4所示，作为本发明的一种优选实施例，所述步骤二中，bp神经网络拟合模型的结构参数包括：
[0119]
bp网络隐含层个数及神经元个数：由于本发明训练输入输出数据集规模适中，隐含层个数为1，即单隐层网络结构，隐藏层神经元为40个，训练误差阈值设定为1e-5
；
[0120]
bp网络输入输出节点数：由上述的轨迹规划器可知，输入包含期望终端状态，包括
期望纵向位移、期望侧向位移、期望横摆角、期望横摆角速度、期望纵向速度、期望侧向速度，相关规划器参数包括期望动作完成时间、期望加速度和初始行驶车速，因此输入神经元选择为9。
[0121]
对于离线获得的大量轨迹数据，基于动作完成时间进行50等分，然后在每个时间间隔点提取其纵向位移和侧向位移数据，如图4所示。因此输出神经元选择为100个。
[0122]
传递函数以及训练算法：隐含层传递函数为双曲正切sigmoid型函数tansig，输出层传递函数为线性传递函数purelin，训练算法为levenberg-marquardt(lm)算法。其算法优势在于可以尽快转化为牛顿法，从而利用牛顿法在误差极小值附近收敛速度快误差小的优势，使得误差性能函数随着迭代过程进行而快速下降到极小值。
[0123]
作为本发明的一种优选实施例，所述步骤三的具体操作为：
[0124]
首先结合周车轨迹预测与碰撞检测算法，在自车可能行驶的所有轨迹簇中剔除可能发生碰撞的轨迹，以避免碰撞发生的可能性。智能车辆在进行各种驾驶动作时需要考虑自身的安全性，即本车避免与周围车辆发生碰撞。因此首先需要对轨迹簇进行碰撞检查筛选，剔除具有碰撞威胁的动作轨迹。
[0125]
具体地，计算所评估轨迹在每个采样点上与周车的距离r(i)：
[0126][0127]
其中xs(i)，ys(i)分别为自车在采样点i上的纵侧向位移，xf(i)，yf(i)分别为周围车在采样点i上的纵侧向位移；
[0128]
比较每个采样点上的自车与周车间的距离，若距离均大于安全距离r
safe
，则对应轨迹满足安全需求d
safe
(i)＝1，否则视为易发生碰撞危险轨迹，直接进行剔除；
[0129][0130]
作为本发明的一种优选实施例，所述步骤四的具体操作为：
[0131]
图5为不同期望参数规划轨迹示意图，由图中可以看出，在筛选得到无碰撞安全轨迹簇之后，还需要进一步对轨迹簇进行多目标优化，目的为了选择出最优动作完成轨迹，以保证车辆在安全前提下的舒适性、稳定性以及快速性等。
[0132]
建立三种评价指标：舒适度指标、动作完成效率指标以及对交通影响评估指标；
[0133]
汽车若以较大加速度或冲击度行驶则会令乘客感觉到不适。因此将所述舒适度定义为：
[0134][0135]
其中，jc是整个规划轨迹舒适度的评价函数，j
x
(t),jy(t)分别为自车纵向和横向关于时间t的冲击度大小，α1,α2分别为j
x
(t),jy(t)相应的权重系数，0,t
cl
分别为当前驾驶动作下的起始时间和期望动作完成时间；jc越小代表该轨迹的舒适性越好。
[0136]
在动作完成过程中自车通常希望以较短时间内完成动作，尤其在周车的行驶轨迹的影响下，加速动作完成过程可有效避免驾驶冲突提高安全性。因此将所述动作完成效率指标定义为：
[0137][0138]
其中，je为动作完成效率指标，t
max
为车辆动作完成所需的最大时间；je越小表示动作完成效率越高。
[0139]
行车过程中需要降低单车对交通的影响，以提高其他车辆的通行效率。因此将所述交通影响评估指标定义为：
[0140][0141]
其中，ja为对交通影响的评价指标，δx为当前轨迹下车辆完成动作所需的纵向行驶位移，v
x
为车辆行驶时的纵向速度大小；ja越小表示动作完成过程对交通的影响越小。
[0142]
总评价指标为：
[0143]jtotal
＝ω1jc ω2je ω3ja[0144]
其中，ω1,ω2,ω3分别为舒适度指标、动作完成效率指标以及对交通影响评估指标的权重系数，且ω1 ω2 ω3＝1。
[0145]
以上仅是本发明的优选实施方式，应当指出，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些均不会影响本发明实施的效果和专利的实用性。