CN113276848B - 一种智能驾驶换道避障轨迹规划、跟踪控制方法及*** - Google Patents

Abstract

本发明公开了一种智能驾驶换道避障轨迹规划、跟踪控制方法及***，环境感知模块采集道路信息、障碍物状态信息以及本车行驶状态信息，候选参考轨迹生成模块根据环境感知模块采集的道路信息生成多条候选换道参考轨迹，参考轨迹选取模块接收候选参考轨迹生成模块发送的候选换道参考轨迹并对候选换道参考轨迹进行筛选，选取出最优换道参考轨迹，参考轨迹优化模块接受环境感知模块发送的本车行驶状态信息、障碍物状态信息和参考轨迹选取模块选取的最优换道参考轨迹，根据本车状态和障碍物状态对最优换道参考轨迹进行实时优化调整，轨迹跟踪控制模块接收参考轨迹优化模块发送的优化后的最优轨迹并对车辆进行轨迹跟踪控制。本发明可以有效提高换道轨迹规划的实时性、舒适性与安全性，同时提高轨迹跟踪精度。

Description

一种智能驾驶换道避障轨迹规划、跟踪控制方法及***

技术领域

本发明属于智能驾驶控制技术领域，尤其涉及智能驾驶换道避障轨迹规划与跟踪控制***及控制方法。

背景技术

随着我国国民经济与汽车工业的快速发展，汽车保有量逐年增加，与此同时交通事故和交通拥堵等问题日益严重，智能驾驶可以有效缓解交通拥堵和减少交通事故，已成为汽车行业的研究热点和发展动力。主动换道避障是智能驾驶的功能之一，研究主动换道避障规划与控制的关键技术，对于提高车辆行驶中的避障能力、通行能力以及车辆的智能化水平具有重要的意义。

针对主动换道避障控制的核心部分换道轨迹规划的研究，当前以离散采样和搜索类的算法为代表，但仍存在一些问题，对于离散采样类的算法，在障碍物的检测以及约束条件的判定上存在实时性较差的问题，且生成的轨迹并非最优；对于搜索类的算法，尽管可以搜索出最优轨迹，但在高维状态空间中算法需要处理大量网格，存在效率较低的问题。

在换道轨迹规划后需要进行车辆轨迹跟踪控制，结合国内外研究发现，当前跟踪控制主要存在的问题在于，由于真实车辆结构较为复杂，而设计控制器时所使用的参考车辆模型都进行了简化，虽然一定程度上提高控制器的计算实时性，但当车辆处于横、纵向加速度较大时，车辆部分状态不能够满足假设条件，因此导致跟踪精度降低，无法满足控制要求。

发明内容

本发明根据现有技术中存在的问题，提出了一种智能驾驶换道避障轨迹规划、跟踪控制方法及***，能够有效兼顾换道轨迹规划时的实时性、舒适性与安全性，同时可以提高在车辆纵侧向加速度较大时的轨迹跟踪精度。

本发明所采用的技术方案如下：

一种智能驾驶换道避障轨迹规划方法，包括如下步骤：

S1、采集道路信息、障碍物状态信息以及本车行驶状态信息；

S2、基于所采集的道路信息，分别对车辆完成换道所行驶的纵向位移和完成换道所需时间进行离散化，通过五次多项式生成候选换道参考轨迹；

S3、对S2中生成的候选换道参考轨迹进行初步筛选，对满足筛选条件的候选换道参考轨迹再进行评价选取，得到最优参考轨迹；

S4、通过二次规划对最优参考轨迹进行实时优化调整，得出最终换道轨迹。

进一步，S2中生成候选换道参考轨迹的方法为：

S2.1、将车辆完成换道所行驶的纵向位移和完成换道所需时间分别进行离散化，表示为：

其中，X_mf为纵向位移X_f的第m个采样点，X_sf为车辆完成换道所需最短纵向位移，X_ef为车辆完成换道所需最大纵向位移；N₁为采样点个数；t_nf为换道所需时间t_f的第n个采样点，t_sf为完成换道所需最短时间，t_ef为完成换道所需最长时间；N₂为采样点个数；

S2.2、通过五次多项式拟合出候选换道参考轨迹；候选换道参考轨迹包括候选路径曲线和候选速度曲线两部分，分别表示为：

y_m＝f_m(x)＝a_0m+a_1mx+a_2mx²+a_3mx³+a_4mx⁴+a_5mx⁵，1≤m≤N₁

x_n＝g_n(t)＝b_0n+b_1nt+b_2nt²+b_3nt³+b_4nt⁴+b_5nt⁵，1≤n≤N₂

其中，f_m(x)等效为y_m表示第m条路径曲线规划的横向位置，x为车辆纵向位移，a_0m，a_1m，...，a_5m为第m条路径曲线多项式系数；g_n(t)等效为x_n表示第n条速度曲线规划的纵向位置，t为时间变量，b_0n，b_1n，...，b_5n为第n条速度曲线多项式系数；

基于上述候选路径曲线和候选速度曲线计算可以得出N条候选参考轨迹，N＝N₁·N₂；

进一步，S3中对候选换道参考轨迹进行初步筛选的方法为：

计算候选换道参考轨迹点的横向位置y_m、规划车速v_p、纵向加速度a_p、规划转弯半径R_p、规划总加速度a_t；

初步筛选条件为：

bound₂+w/2≤y_m≤bound₁-w/2

v_p≤v_max

a_min≤a_p≤a_max

R_p≤L/tanδ_max

a_t≤μg

其中，bound₁、bound₂为本车道边界线横向坐标值，w为本车宽度，v_max表示车辆的最高车速，a_min表示车辆最大减速度，a_max表示车辆最大加速度，μ表示路面附着系数，g表示重力加速度，δ_max最大前轮转角，L为车辆轴距；

若无候选参考轨迹满足初步筛选条件，则换道失败。

进一步，评价选取最优参考轨迹的方法为：

建立轨迹评价函数cost：

cost＝cost1+cost2+S_ob

路径曲线评价函数：

速度曲线评价函数：

避障风险评价函数：

S_ob＝∑_iS_obii＝1，2，3，...

其中，ω₁、ω₂、ω₃、ω₄、ω₅、ω₇表示各项权值；x为车辆纵向位移由候选速度曲线x_n确定其随时间的变化关系；

分别为f_m(x)的一阶导数、二阶导数和三阶导数；

分别为g_n(t)的一阶导数、二阶导数和三阶导数；v_des表示该道路期望的车速；S_obi为第i个障碍物的风险代价值，η为常数，d_s为安全距离，d_{mn，min_i}为各候选参考轨迹与第i个障碍物之间的最小距离；

选取cost最小的参考轨迹作为最优参考轨迹；记最优参考轨迹的参考路径曲线为y_ref＝f_ref(x)，速度参考曲线为x_ref＝g_ref(t)。

进一步，S4中对对最优参考轨迹进行实时优化调整的方法为：

S4.1、分别构建N₃段五次样条曲线f_i(x)和N₄段五次样条曲线g_j(t)，表达公式如下：

y_i＝f_i(x)＝a_i0+a_i1x+a_i2x²+a_i3x³+a_i4x⁴+a_i5x⁵(d_il≤x≤d_iu)，i＝1，2，...，N₃

x_j＝g_j(t)＝b_j0+b_j1t+b_j2t²+b_j3t³+b_j4t⁴+b_j5t⁵(t_jl≤t≤t_ju)，j＝1，2，...，N₄

式中，a_i0，a_i1，...，a_i5为第i段五次样条曲线f_i(x)的系数，d_il、d_iu分别为f_i(x)自变量x的下限值和上限值；b_j0，b_j1，...，b_j5为第j段五次样条曲线g_j(t)的系数，t_jl、t_ju分别为g_j(t)自变量t的下限值和上限值；d_il、d_iu、t_jl、t_ju计算公式如下：

式中，X_f为所选参考轨迹完成换道的纵向位移，t_f为所选参考轨迹换道完成时间；

相邻两段样条曲线连接点必须满足平滑连接条件，如下：

f_i(d_iu)＝f_i+1(d_(i+1)l)，g_j(t_ju)＝g_j+1(t_(j+1)l)

样条曲线必须满足道路边界约束、曲率约束、速度约束、加速度约束，如下：

bound₂+w/2≤f_i(x)≤bound₁-w/2

式中，R_min为最小转弯半径，a_c，max最大侧向加速度，v_des为实时车速，v_x，max为最大纵向车速，a_x，min为最小纵向车速，a_x，max为最大纵向车速；

与参考轨迹相同，优化路径曲线的起点和终点必须满足以下条件：

x＝0时，

y₁＝y_s

x＝X_f时，

y_N3＝y_r

与参考轨迹相同，优化速度曲线的起点和终点必须满足以下条件：

t＝0时，

x₁＝0

t＝t_f时，

x_N4＝X_mf

S4.2、建立二次优化目标函数，公式如下：

式中，J₁为路径曲线优化目标函数，J₂为速度曲线优化目标函数，ω₇、ω₈、ω₉、ω₁₀为各项权值，k₁、k₂、k₃、k₄为常数，x_s、x_obi、y_s、y_obi、y_r分别每个采样时刻本车的纵向位置、第i个障碍物的纵向位置、本车的横向位置和第i个障碍物的横向位置、目标车道的横向位置；

S4.3、利用二次规划算法求出目标函数J₁最小时的f_i(x)记为f_best(x)，求出目标函数J₂最小时的g_j(t)记为g_best(t)；将f_best(x)和g_best(t)作为最终换道轨迹；

一种基于误差模型的轨迹跟踪补偿控制方法，包括如下步骤：

S1、基于车辆三自由度动力学模型和纵向运动学模型，设计LQR轨迹跟踪控制器；由LQR轨迹跟踪控制器计算得出轨迹跟踪控制量，即前轮转角δ_c和加速度补偿量Δa；

S2、通过LQR控制器对车辆进行轨迹跟踪控制，记录车辆进行轨迹跟踪的误差数据；误差数据包括LQR轨迹跟踪的横向误差e_{d_LQR}、航向偏差

本车车速v、本车加速度a、当前规划路径的曲率ρ以及LQR决策出的前轮转角δ_c；

S3、利用BP神经网络对已采集的误差数据进行训练，得到LQR跟踪误差模型

S4、运用阿克曼转向原理由LQR跟踪误差模型

输出的e_{d_LQR}，

计算前轮转角补偿量δ_p；

S5、基于S4得到的前轮转角补偿量δ_p计算最终前轮转角控制量δ＝δ_c+δ_p；

S6、计算最终车辆纵向加速度控制量a_c＝a_p+Δa；a_p为规划车辆加速度。

进一步，S1中LQR轨迹跟踪控制器计算加速度补偿量Δa的方法为：

S1.1、将车辆质心位置投影到规划轨迹的匹配点上，得出横向动力学参考模型和纵向运动学参考模型，并分别对横向动力学参考模型和纵向运动学参考模型进行离散化处理；

S1.2、建立轨迹跟踪目标函数J，表示为：

式中，x_k表示参考模型第k时刻的状态量，Q为状态系数，u_k为第k时刻的反馈控制量，R为控制系数；x_n为第n时刻参考模型状态量；

当参考模型为横向动力学参考模型时，x_k代表XC_k，u_k代表uc_k；

当参考模型为纵向运动学参考模型时，x_k代表XL_k，u_k代表ul_k；

S1.3、在车辆的横向动力学参考模型

的约束条件下，构建拉格朗日函数L₁(XC_k，uc_k，λ_k)如下：

式中，

为拉格朗日乘子，求取L₁(XC_k，uc_k，λ_k)为最小值时的uc_k，即为此时刻的前轮转角反馈控制量δ_f；

S1.4、针对轨迹曲率实时变化的干扰项

添加前馈控制量δ_F以消除稳态误差，计算如下：

S1.5、在车辆的纵向运动学参考模型

的约束条件下，构建拉格朗日函数L₂(XL_k，ul_k，λ_k)如下：

式中，

为拉格朗日乘子，求取L₂(XL_k，ul_k，λ_k)为最小值时的ul_k，即为此时刻车辆加速度补偿量Δa。

进一步，S4中前轮转角补偿量δ_p计算方法为：

式中，l_f为车辆质心距前轴的距离，l_r为质心距后轴的距离，k为常数。

一种智能驾驶换道避障轨迹规划与跟踪控制***，具体包括环境感知模块、候选参考轨迹生成模块、参考轨迹选取模块、参考轨迹优化模块和轨迹跟踪控制模块；

环境感知模块用于检测道路信息、障碍物状态信息以及本车行驶状态信息等，且环境感知模块分别与候选参考轨迹生成模块、参考轨迹优化模块和轨迹跟踪控制模块信号连接；

候选参考轨迹生成模块根据环境感知模块采集的道路信息生成多条候选换道参考轨迹，并将候选换道参考轨迹发送给参考轨迹选取模块；

参考轨迹选取模块接收候选参考轨迹生成模块发送的候选参考轨迹，根据本车所能行驶的最大车速、最大加速度、最制动减速度、车辆的最小转弯半径、道路边界以及路面条件对候选参考轨迹进行初步筛选，若满足筛选条件的轨迹有S条，当S＜1时，判定为换道失败；当S≥1时，将满足筛选条件的S条候选参考轨迹通过评价函数cost进行评价。若在这S条候选轨迹中，

则有碰撞发生，判定为换道失败；若在这S候选轨迹中，

则存在可行的换道参考轨迹，将cost值最小的轨迹作为最优换道参考轨迹，并发送给参考轨迹优化模块，记最优换道参考轨迹的参考路径曲线为y_ref＝f_ref(x)，速度参考曲线为x_ref＝g_ref(t)。其中cost包括路径曲线评价函数cost1、速度曲线评价函数cost2、避障风险评价函数S_ob，如下式所示：

cost＝cost1+cost2+S_ob

S_ob＝∑_iS_obii＝1，2，3，…

其中，ω₁、ω₂、ω₃、ω₄、ω₅、ω₇表示各项权值；x为车辆纵向位移由候选速度曲线x_n确定其随时间的变化关系；

分别为f_m(x)的一阶导数、二阶导数和三阶导数；

分别为g_n(t)的一阶导数、二阶导数和三阶导数；v_des表示该道路期望的车速；S_obi为第i个障碍物的风险代价值，η为常数，d_s为安全距离，d_{mn，min_i}为各候选参考轨迹与第i个障碍物之间的最小距离。

参考轨迹优化模块接受环境感知模块发送的本车行驶状态信息、障碍物状态信息和参考轨迹选取模块选取的最优换道参考轨迹，根据本车状态和障碍物状态对最优换道参考轨迹进行实时优化调整，并将实时优化后的最优轨迹发送给轨迹跟踪控制模块。分别利用N₃段五次样条曲线f_i(x)和N₄段五次样条曲线g_j(t)，使得目标函数J₁和J₂得到最小值，J₁和J₂表示如下：

式中，J₁为路径曲线优化目标函数，J₂为速度曲线优化目标函数，ω₇、ω₈、ω₉、ω₁₀为各项权值，k₁、k₂、k₃、k₄为常数，x_s、x_obi、y_s、y_obi、y_r分别每个采样时刻本车的纵向位置、第i个障碍物的纵向位置、本车的横向位置和第i个障碍物的横向位置、目标车道的横向位置，f_ref(x)和g_ref(t)分别为最优路径参考曲线和最优速度参考曲线。

轨迹跟踪控制模块接收参考轨迹优化模块发送的优化后的最优轨迹，并对车辆进行轨迹跟踪控制。

进一步，环境感知模块包括激光雷达、摄像头、速度仪和GPS等设备用于检测道路信息、障碍物状态信息以及本车行驶状态信息。

本发明的有益效果：

(1)本发明提出一种结合遍历法与二次规划算法的新型轨迹规划算法，该算法在上层决策为换道行为时，根据当前环境生成有限条可能发生的换道轨迹，在通过建立筛选条件与评价函数，确定换道的参考轨迹；基于此参考轨迹，根据时变的障碍物状态建立障碍风险评价函数，使用二次规划进行实时的优化调整，该算法每个换道周期只进行一次参考轨迹的计算且减少了二次规划目标函数的复杂度与边界条件的建立，大大降低了轨迹规划对处理芯片的算力要求，为自动驾驶换道的工程实践提供了一种有效的解决方案。

(2)本发明针对当前轨迹跟踪控制算法在车辆横纵向加速度较大时，轨迹跟踪精度降低的问题，提出了一种误差补偿的控制策略；此方法并未从参考模型的复杂度方面进行解决，而是基于误差数据与传统的控制理论相结合，从而解决车辆在纵侧向加速度较大时轨迹跟踪误差降低的问题，在控制器设计难度与复杂度并未增加的同时提升了跟踪精度。

附图说明

图1是智能汽车换道避障***示意图；

图2是智能汽车换道轨迹规划方法流程图；

图3是换道初始时刻坐标系建立示意图；

图4是换道场景示意图；

图5是智能汽车换道轨迹跟踪控制方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。

如图1所示，本发明提出了一种智能驾驶换道避障轨迹规划与跟踪控制***，具体包括环境感知模块、候选参考轨迹生成模块、参考轨迹选取模块、参考轨迹优化模块和轨迹跟踪控制模块；

环境感知模块用于检测道路信息、障碍物状态信息以及本车行驶状态信息等，且环境感知模块分别与候选参考轨迹生成模块、参考轨迹优化模块和轨迹跟踪控制模块信号连接；在本实施例中，环境感知模块包括激光雷达、摄像头、速度仪和GPS等设备用于检测道路信息、障碍物状态信息以及本车行驶状态信息。

候选参考轨迹生成模块根据环境感知模块采集的道路信息生成多条候选换道参考轨迹，并将候选换道参考轨迹发送给参考轨迹选取模块。

参考轨迹选取模块接收候选参考轨迹生成模块发送的候选参考轨迹，根据本车所能行驶的最大车速、最大加速度、最制动减速度、车辆的最小转弯半径、道路边界以及路面条件对候选参考轨迹进行初步筛选，若满足筛选条件的轨迹有S条，当S＜1时，判定为换道失败；当S≥1时，将满足筛选条件的S条候选参考轨迹通过评价函数cost进行评价。若在这S条候选轨迹中，

则有碰撞发生，判定为换道失败；若在这S候选轨迹中，

则存在可行的换道参考轨迹，将cost值最小的轨迹作为最优换道参考轨迹，并发送给参考轨迹优化模块，记最优换道参考轨迹的参考路径曲线为y_ref＝f_ref(x)，速度参考曲线为x_ref＝g_ref(t)。其中cost包括路径曲线评价函数cost1、速度曲线评价函数cost2、避障风险评价函数S_ob，如下式所示：

cost＝cost1+cost2+S_ob

So_b＝∑_iS_obii＝1，2，3，…

其中，ω₁、ω₂、ω₃、ω₄、ω₅、ω₇表示各项权值；x为车辆纵向位移由候选速度曲线x_n确定其随时间的变化关系；

分别为f_m(x)的一阶导数、二阶导数和三阶导数；

分别为g_n(t)的一阶导数、二阶导数和三阶导数；v_des表示该道路期望的车速；S_obi为第i个障碍物的风险代价值，η为常数，d_s为安全距离，d_{mn，min_i}为各候选参考轨迹与第i个障碍物之间的最小距离。

参考轨迹优化模块接受环境感知模块发送的本车行驶状态信息、障碍物状态信息和参考轨迹选取模块选取的最优换道参考轨迹，根据本车状态和障碍物状态对最优换道参考轨迹进行实时优化调整，并将实时优化后的最优轨迹发送给轨迹跟踪控制模块。分别利用N₃段五次样条曲线f_i(x)和N₄段五次样条曲线g_j(t)，使得目标函数J₁和J₂得到最小值，J₁和J₂表示如下：

式中，J₁为路径曲线优化目标函数，J₂为速度曲线优化目标函数，ω₇、ω₈、ω₉、ω₁₀为各项权值，k₁、k₂、k₃、k₄为常数，x_s、x_obi、y_s、y_obi、y_r分别每个采样时刻本车的纵向位置、第i个障碍物的纵向位置、本车的横向位置和第i个障碍物的横向位置、目标车道的横向位置，f_ref(x)和g_ref(t)分别为最优路径参考曲线和最优速度参考曲线。轨迹跟踪控制模块接收参考轨迹优化模块发送的优化后的最优轨迹，并对车辆进行轨迹跟踪控制。

基于上述智能驾驶换道避障轨迹规划与跟踪控制***，如图2所示，本发明提出了一种智能驾驶换道避障轨迹规划方法，具体步骤如下：

S1、通过环境感知模块采集道路信息、障碍物状态信息以及本车行驶状态信息；具体过程如下：

S1.1、通过摄像头检测本车道边界线横向坐标值bound₁、bound₂，目标车道边界线横向坐标值bound₂、bound₃，本车道虚拟车道中心线横向坐标值lc1以及目标车道虚拟中心线横向坐标值l_c2，本车航向角θ；

S1.2、通过GPS获得当前车辆的所在位置(x，y)；其中x，y为在如图3所示坐标系下车辆的所在位置坐标；

S1.4、通过速度仪获得本车车速v和加速度a；

S1.5、通过激光雷达获得障碍物的位置(x_ob、y_ob)以及障碍物速度v_ob；

S2、将环境感知模块采集的道路信息发送给候选参考轨迹生成模块，候选参考轨迹生成模块生成候选换道参考轨迹，具体内容包括以下：

S2.1、将车辆完成换道所行驶的纵向位移和完成换道所需时间分别进行离散化。

将车辆完成换道所行驶的纵向位移X_f在X_sf≤X_f≤X_ef范围内均匀采取N₁个采样点，采样点记为X_mf；其中，X_sf为车辆完成换道所需最短纵向位移，X_ef为车辆完成换道所需最大纵向位移；

将车辆完成换道所需时间t_f在t_sf≤t_nf≤t_ef范围内均匀采取N₂个采样点，采样点记为t_nf；其中，t_sf为完成换道所需最短时间，t_ef为完成换道所需最长时间；

S2.2、通过五次多项式拟合出候选换道参考轨迹；候选换道参考轨迹包括候选路径曲线和候选速度曲线两部分；

(1)、候选路径曲线表示规划路径的横向位置随纵向位置的变化关系，候选路径曲线表达式如下：

y_m＝f_m(x)＝a_0m+a_1mx+a_2mx²+a_3mx³+a_4mx⁴+a_5mx⁵，1≤m≤N₁

式中，f_m(x)等效为y_m表示第m条路径曲线规划的横向位置，x为车辆纵向位移，a_0m，a_1m，...，a_5m为第m条路径曲线多项式系数，系数通过将边值约束条件代入曲线表达式中消元得出；边值约束条件如下：

x＝0时，

y_m＝y_s

x＝X_mf时，

y_m＝y_r

式中，y_s为换道初始车辆横向位置，θ_s为换道初始车辆航向角，θ为方向盘转角，i为转向***传动比，L为车辆轴距，y_r为目标车道的横向位置；

分别为y_m的一阶导数和二阶导数。

(2)、候选速度曲线表示规划路径的纵向位置随时间变化关系，候选速度曲线表达式如下：

x_n＝g_n(t)＝b_0n+b_1nt+b_2nt²+b_3nt³+b_4nt⁴+b_5nt⁵，1≤n≤N₂

式中，g_n(t)等效为x_n表示第n条速度曲线规划的纵向位置，t为时间变量，b_0n，b_1n，...，b_5n为第n条速度曲线多项式系数，系数通过将边值约束条件代入曲线表达式中消元得出；边值约束条件如下：

t＝0时，

x_n＝0

t＝t_nf时，

x_n＝X_mf

式中，v表示车辆的速度，a表示车辆的加速度，v_des表示该道路期望的车速；

分别为x_n的一阶导数和二阶导数。

(3)、通过上述计算可以得出N条候选参考轨迹，其中N＝N₁·N₂；

S3、参考轨迹选取模块接收候选参考轨迹生成模块生成的N条候选参考轨迹并对候选参考轨迹进行筛选评价，具体内容如下：

S3.1、根据候选参考轨迹计算各轨迹点上的横向位置y_m、规划车速v_p、纵向加速度a_p、规划转弯半径R_p、规划总加速度a_t，计算公式如下：

y_m＝f_m(x)

式中，x为车辆纵向位移由候选速度曲线x_n确定其随时间的变化关系；

S3.2、对各候选参考轨迹点上的规划横向位置y_m、规划车速v_p、规划车辆纵向加速度a_p、规划车辆总加速度a_t、规划转弯半径R_p进行初步筛选，初步筛选条件如下：

bound₂+w/2≤y_m≤bound₁-w/2

v_p≤v_max

a_min≤a_p≤a_max

R_p≤L/tanδ_max

a_t≤μg

其中，w为本车宽度，v_max表示车辆的最高车速，a_min表示车辆最大减速度，a_max表示车辆最大加速度，μ表示路面附着系数，g表示重力加速度，δ_max最大前轮转角，L为车辆轴距；

S3.3、若无候选参考轨迹满足S3.2中的初步筛选条件，则换道失败；若存在若干条候选参考轨迹，则对这些轨迹再进行评价选取，得到最优参考轨迹；评价选取方法如下：

S3.3.1、建立轨迹评价函数cost；轨迹评价函数包括路径曲线评价函数cost1、速度曲线评价函数cost2和避障风险评价函数Sob；分别表示为：

cost＝cost1+cost2+S_ob

路径曲线评价函数cost1，计算公式如下：

式中，ω₁、ω₂、ω₃分别表示各项权值；

速度曲线评价函数cost2，计算公式如下：

式中，ω₄、ω₅、ω₇分别表示各项权值；

避障风险评价函数S_ob，计算公式如下：

S_ob＝∑_iS_obii＝1，2，3，…

其中，S_obi为第i个障碍物的风险代价值，η为常数，d_s为安全距离，d_{mn，min_i}为各候选参考轨迹与第i个障碍物之间的最小距离，如图4所示，计算公式具体如下：

式中，k₁、k₂为常数，y_obi(t)表示第i个障碍物的横向位置随时间的变化，x_obi(t)表示第i个障碍物的纵向位置随时间的变化，假设y_obi(t)处于本车道中心线或目标车道中心线的坐标上，假设x_obi(t)＝v_obit+x_obi(0)，v_obi为第i个障碍物的速度,x_obi(0)为换道初始时刻第i个障碍物纵向位置；

S3.3.2、选取cost最小的参考轨迹作为最优参考轨迹；记最优参考轨迹的参考路径曲线为y_ref＝f_ref(x)，速度参考曲线为x_ref＝g_ref(t)；

S4、参考轨迹优化调整模块接收参考轨迹选取模块所选取的最优参考轨迹，通过二次规划对最优参考轨迹进行实时优化调整，得出最终换道轨迹，优化调整过程如下：

S4.1、分别构建N₃段五次样条曲线f_i(x)和N₄段五次样条曲线g_j(t)，表达公式如下：

y_i＝f_i(x)＝a_i0+a_i1x+a_i2x²+a_i3x³+a_i4x⁴+a_i5x⁵(d_il≤x≤d_iu)，i＝1，2，...，N₃

x_j＝g_j(t)＝b_j0+b_j1t+b_j2t²+b_j3t³+b_j4t⁴+b_j5t⁵(t_jl≤t≤t_ju)，j＝1，2，...，N₄

式中，a_i0，a_i1，...，a_i5为第i段五次样条曲线f_i(x)的系数，d_il、d_iu分别为f_i(x)自变量x的下限值和上限值；b_j0，b_j1，...，b_j5为第j段五次样条曲线g_j(t)的系数，t_jl、t_ju分别为g_j(t)自变量t的下限值和上限值；d_il、d_iu、t_jl、t_ju计算公式如下：

式中，X_f为所选参考轨迹完成换道的纵向位移，t_f为所选参考轨迹换道完成时间；

相邻两段样条曲线连接点必须满足平滑连接条件，如下：

f_i(d_iu)＝f_i+1(d_(i+1)l)，g_j(t_ju)＝g_j+1(t_(j+1)l)

样条曲线必须满足道路边界约束、曲率约束、速度约束、加速度约束，如下：

bound₂+w/2≤f_i(x)≤bound₁-w/2

式中，R_min为最小转弯半径，a_c，max最大侧向加速度，v_des为实时车速，v_x，max为最大纵向车速，a_x，min为最小纵向车速，a_x，max为最大纵向车速；

与参考轨迹相同，优化路径曲线的起点和终点必须满足以下条件：

x＝0时，

y₁＝y_s

x＝X_f时，

y_N3＝y_r

与参考轨迹相同，优化速度曲线的起点和终点必须满足以下条件：

t＝0时，

x₁＝0

t＝t_f时，

x_N4＝X_mf

S4.2、建立二次优化目标函数，公式如下：

式中，J₁为路径曲线优化目标函数，j₂为速度曲线优化目标函数，ω₇、ω₈、ω₉、ω₁₀为各项权值，k₁、k₂、k₃、k₄为常数，x_s、x_obi、y_s、y_obi分别每个采样时刻本车的纵向位置、第i个障碍物的纵向位置、本车的横向位置和第i个障碍物的横向位置；

S4.3、利用二次规划算法求出目标函数J₁最小时的f_i(x)记为f_best(x)，求出目标函数J₂最小时的g_j(t)记为g_best(t)；将f_best(x)和g_best(t)作为最终换道轨迹；

如图5所示，本发明还提出了一种基于误差模型的轨迹跟踪补偿控制方法；具体跟踪控制方法包括以下步骤：

S1、基于车辆三自由度动力学模型和纵向运动学模型，设计LQR轨迹跟踪控制器；由LQR轨迹跟踪控制器计算得出轨迹跟踪控制量：前轮转角δ_c和加速度补偿量Δa，其中，δ_c＝δ_f+δ_F，δ_f为前轮转角反馈控制量，δ_F为前轮转角前馈控制量；

LQR轨迹跟踪控制器计算前轮转角δ_c和加速度补偿量Δa的方法如下：

S1.1、将车辆质心位置投影到规划轨迹的匹配点上，得出横向动力学参考模型和纵向运动学参考模型；

(1)横向动力学参考模型，简化表达如下：

式中，A₁为横向状态系数矩阵，B₁为横向控制系数矩阵，C干扰系数矩阵，e_d为车辆相对于轨迹实时的横向偏差，

为实时横向偏差变化率，

为实时航向偏差，

为实时航向偏差变化率，u₁为前轮转角控制量，

为轨迹曲率，e_rr表示由

组成的矩阵；

忽略干扰项

将横向动力学模型离散化得到如下表达式：

式中，XC_k表示第k时刻的e_rr，XC_k+1表示第k+1时刻的e_rr，u_ck表示第k时刻的u₁,

分别为XC_k、uc_k的系数矩阵；

(2)纵向运动学参考模型

式中，A₂为纵向状态系数矩阵，B₂为纵向控制系数矩阵，e_s为实时纵向误差，

为实时速度误差，u₂为加速度控制量；e_ss表示由

组成的矩阵；

同理，将纵向运动学模型离散化得到如下表达式：

式中，XL_k表示第k时刻的e_ss，XL_k+1表示第k+1时刻的e_ss，ul_k表示第k时刻的u₂,

分别为XL_k、ul_k的系数矩阵；

S1.2、建立轨迹跟踪目标函数J，如下式：

式中，x_k表示参考模型第k时刻的状态量，Q为状态系数，u_k为第k时刻的反馈控制量，R为控制系数；x_n为第n时刻参考模型状态量；

当参考模型为横向动力学参考模型时，x_k代表XC_k，u_k代表uc_k；

当参考模型为纵向运动学参考模型时，x_k代表XL_k，u_k代表ul_k；

S1.3、在车辆的横向动力学参考模型

的约束条件下，构建拉格朗日函数L₁(XC_k，uc_k，λ_k)如下：

式中，

为拉格朗日乘子，求取L₁(XC_k，uc_k，λ_k)为最小值时的uc_k，即为此时刻的前轮转角反馈控制量δ_f；

S1.4、针对轨迹曲率实时变化的干扰项

添加前馈控制量δ_F以消除稳态误差，计算如下：

S1.5、同理，在车辆的纵向运动学参考模型

的约束条件下，构建拉格朗日函数L₂(XL_k，ul_k，λ_k)如下：

式中，

为拉格朗日乘子，求取L₂(XL_k，ul_k，λ_k)为最小值时的ul_k，即为此时刻车辆加速度补偿量Δa；

S2、通过LQR控制器对车辆进行轨迹跟踪控制，记录车辆进行轨迹跟踪的误差数据；误差数据包括LQR轨迹跟踪的横向误差e_{d_LQR}、航向偏差

本车车速v、本车加速度a、当前规划路径的曲率ρ以及LQR决策出的前轮转角δ_c；

S3、利用BP神经网络对已采集的误差数据进行训练，得到LQR跟踪误差模型

S4、运用阿克曼转向原理由LQR跟踪误差模型

输出的e_{d_LQR}，

计算前轮转角补偿量δ_p；计算公式如下：

式中，l_f为车辆质心距前轴的距离，l_r为质心距后轴的距离，k为常数；

S5、计算最终前轮转角控制量δ如下：

6＝δ_c+δ_p

S6、计算最终车辆纵向加速度控制量a_c如下：

a_c＝a_p+Δa

式中，a_p为规划车辆加速度，计算如下：

以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。

Claims (9)

1.一种智能驾驶换道避障轨迹规划方法，其特征在于，包括如下步骤：

S1、采集道路信息、障碍物状态信息以及本车行驶状态信息；

S2、基于所采集的道路信息，分别对车辆完成换道所行驶的纵向位移和完成换道所需时间进行离散化，通过五次多项式生成候选换道参考轨迹；

S3、对S2中生成的候选换道参考轨迹进行初步筛选，对满足筛选条件的候选换道参考轨迹再进行评价选取，得到最优参考轨迹；

S4、通过二次规划对最优参考轨迹进行实时优化调整，得出最终换道轨迹；S4中对最优参考轨迹进行实时优化调整的方法为：

S4.1、分别构建N₃段五次样条曲线f_i(x)和N₄段五次样条曲线g_j(t)，表达公式如下：

y_i＝f_i(x)＝a_i0+a_i1x+a_i2x²+a_i3x³+a_i4x⁴+a_i5x⁵，d_il≤x≤d_iu，i＝1，2，…，N₃

x_j＝g_j(t)＝b_j0+b_j1t+b_j2t²+b_j3t³+b_j4t⁴+b_j5t⁵，t_jl≤t≤t_ju，j＝1，2，…，N₄

式中，a_i0，a_i1，…，a_i5为第i段五次样条曲线f_i(x)的系数，d_il、d_iu分别为f_i(x)自变量x的下限值和上限值；b_j0，b_j1，...，b_j5为第j段五次样条曲线g_j(t)的系数，t_jl、t_ju分别为g_j(t)自变量t的下限值和上限值；

相邻两段样条曲线连接点必须满足平滑连接条件，如下：

f_i(d_iu)＝f_i+1(d_(i+1)l)，g_j(t_ju)＝g_j+1(t_(j+1)l)

样条曲线必须满足道路边界约束、曲率约束、速度约束、加速度约束，如下：

bound₂+w/2≤f_i(x)≤bjound₁-w/2

式中，R_min为最小转弯半径，a_c，max最大侧向加速度，v_des为实时车速，v_x，max为最大纵向车速，a_x，min为最小纵向车速，a_x，max为最大纵向车速；

与参考轨迹相同，优化路径曲线的起点和终点必须满足以下条件：

x＝0时，

y₁＝y_s

x＝X_f时，

y_N3＝y_r

与参考轨迹相同，优化速度曲线的起点和终点必须满足以下条件：

t＝0时，

x₁＝0

t＝t_f时，

x_N4＝X_mf

S4.2、建立二次优化目标函数，公式如下：

式中，J₁为路径曲线优化目标函数，J₂为速度曲线优化目标函数，ω₇、ω₈、ω₉、ω₁₀为各项权值，k₁、k₂、k₃、k₄为常数，x_s、x_obi、y_s、y_obi分别每个采样时刻本车的纵向位置、第i个障碍物的纵向位置、本车的横向位置和第i个障碍物的横向位置；

S4.3、利用二次规划算法求出目标函数J₁最小时的f_i(x)记为f_best(x)，求出目标函数J₂最小时的g_j(t)记为g_best(t)；将f_best(x)和g_best(t)作为最终换道轨迹。

2.根据权利要求1所述的一种智能驾驶换道避障轨迹规划方法，其特征在于，S2中生成候选换道参考轨迹的方法为：

S2.1、将车辆完成换道所行驶的纵向位移和完成换道所需时间分别进行离散化，表示为：

其中，X_mf为纵向位移X_f的第m个采样点，X_sf为车辆完成换道所需最短纵向位移，X_ef为车辆完成换道所需最大纵向位移；N₁为采样点个数；t_nf为换道所需时间t_f的第n个采样点，t_sf为完成换道所需最短时间，t_ef为完成换道所需最长时间；N₂为采样点个数；

S2.2、通过五次多项式拟合出候选换道参考轨迹；候选换道参考轨迹包括候选路径曲线和候选速度曲线两部分，分别表示为：

y_m＝f_m(x)＝a_0m+a_1mx+a_2mx²+a_3mx³+a_4mx⁴+a_5mx⁵， 1≤m≤N₁

x_n＝g_n(t)＝b_0n+b_1nt+b_2nt²+b_3nt³+b_4nt⁴+b_5nt⁵， 1≤n≤N₂

其中，f_m(x)等效为ym表示第m条路径曲线规划的横向位置，x为车辆纵向位移，a_0m，a_1m，...，a_5m为第m条路径曲线多项式系数；g_n(t)等效为x_n表示第n条速度曲线规划的纵向位置，t为时间变量，b_0n，b_1n，...，b_5n为第n条速度曲线多项式系数；

基于上述候选路径曲线和候选速度曲线计算可以得出N条候选参考轨迹，N＝N₁·N₂。

3.根据权利要求1所述的一种智能驾驶换道避障轨迹规划方法，其特征在于，S3中对候选换道参考轨迹进行初步筛选的方法为：

针对候选换道参考轨迹点的横向位置y_m、规划车速v_p、纵向加速度a_p、规划转弯半径R_p、规划总加速度a_t按照以下初步筛选条件进行筛选：

bound₂+w/2≤y_m≤bound₁-w/2

v_p≤vmax

a_min≤a_p≤a_max

R_p≤L/tanδ_max

a_t≤μg

其中，bound₁、bound₂为本车道边界线横向坐标值，w为本车宽度，v_max表示车辆的最高车速，a_min表示车辆最大减速度，a_max表示车辆最大加速度，μ表示路面附着系数，g表示重力加速度，δ_max最大前轮转角，L为车辆轴距；

若无候选参考轨迹满足初步筛选条件，则换道失败。

4.根据权利要求2或3所述的一种智能驾驶换道避障轨迹规划方法，其特征在于，评价选取最优参考轨迹的方法为：

建立轨迹评价函数cost：

cost＝cost1+cost2+S_ob

路径曲线评价函数：

速度曲线评价函数：

避障风险评价函数：

S_ob＝∑_iS_obi i＝1，2，3，...

其中，ω₁、ω₂、ω₃、ω₄、ω₅、ω₇表示各项权值；x为车辆纵向位移由候选速度曲线x_n确定其随时间的变化关系；

分别为f_m(x)的一阶导数、二阶导数和三阶导数；

分别为g_n(t)的一阶导数、二阶导数和三阶导数；v_des表示该道路期望的车速；S_obi为第i个障碍物的风险代价值，η为常数，d_s为安全距离，d_{mn，min_i}为各候选参考轨迹与第i个障碍物之间的最小距离；

选取cost最小的参考轨迹作为最优参考轨迹；记最优参考轨迹的参考路径曲线为y_ref＝f_ref(x)，速度参考曲线为x_ref＝g_ref(t)。

5.一种基于权利要求1所述智能驾驶换道避障轨迹规划方法的基于误差模型的轨迹跟踪补偿控制方法，其特征在于，包括如下步骤：

S1、基于车辆三自由度动力学模型和纵向运动学模型，设计LQR轨迹跟踪控制器；由LQR轨迹跟踪控制器计算得出轨迹跟踪控制量，即前轮转角δ_c和加速度补偿量Δa；

S2、通过LQR控制器对车辆进行轨迹跟踪控制，记录车辆进行轨迹跟踪的误差数据；误差数据包括LQR轨迹跟踪的横向误差e_{d_LQR}、航向偏差

本车车速v、本车加速度a、当前规划路径的曲率ρ以及LQR决策出的前轮转角δ_c；

S3、利用BP神经网络对已采集的误差数据进行训练，得到LQR跟踪误差模型

S4、运用阿克曼转向原理由LQR跟踪误差模型

输出的e_{d_LQR}，

计算前轮转角补偿量δ_p；

S5、基于S4得到的前轮转角补偿量δ_p计算最终前轮转角控制量δ＝δ_c+δ_p；

S6、计算最终车辆纵向加速度控制量a_c＝a_p+Δa；a_p为规划车辆加速度。

6.根据权利要求5所述的一种基于误差模型的轨迹跟踪补偿控制方法，其特征在于，S1中LQR轨迹跟踪控制器计算加速度补偿量Δa的方法为：

S1.1、将车辆质心位置投影到规划轨迹的匹配点上，得出横向动力学参考模型和纵向运动学参考模型，并分别对横向动力学参考模型和纵向运动学参考模型进行离散化处理；

S1.2、建立轨迹跟踪目标函数J，表示为：

式中，x_k表示参考模型第k时刻的状态量，Q为状态系数，u_k为第k时刻的反馈控制量，R为控制系数；x_n为第n时刻参考模型状态量；

当参考模型为横向动力学参考模型时，x_k代表XC_k，u_k代表uc_k；

当参考模型为纵向运动学参考模型时，x_k代表XL_k，u_k代表ul_k；

S1.3、在车辆的横向动力学参考模型

的约束条件下，构建拉格朗日函数L₁(XC_k，uc_k，λ_k)如下：

式中，

为拉格朗日乘子，求取L₁(XC_k，uc_k，λ_k)为最小值时的uc_k，即为此时刻的前轮转角反馈控制量δ_f；XC_k是第k时刻

组成的矩阵；uc_k表示第k时刻前轮转角控制量；λ_k为拉格朗日乘子；

S1.4、针对轨迹曲率实时变化的干扰项

添加前馈控制量δ_F以消除稳态误差，计算如下：

S1.5、在车辆的纵向运动学参考模型

的约束条件下，构建拉格朗日函数L₂(XL_k，ul_k，λ_k)如下：

式中，

为拉格朗日乘子，求取L₂(XL_k，ul_k，λ_k)为最小值时的ul_k，即为此时刻车辆加速度补偿量Δa；XL_k分别为表示第k时刻

组成的矩阵；u_k为第k时刻的反馈控制量。

7.根据权利要求5所述的一种基于误差模型的轨迹跟踪补偿控制方法，其特征在于，S4中前轮转角补偿量δ_p计算方法为：

式中，l_f为车辆质心距前轴的距离，l_r为质心距后轴的距离，k为常数；横向误差e_{d_LQR}、航向偏差

8.一种基于权利要求1所述智能驾驶换道避障轨迹规划方法的智能驾驶换道避障轨迹规划与跟踪控制***，其特征在于，包括环境感知模块、候选参考轨迹生成模块、参考轨迹选取模块、参考轨迹优化模块和轨迹跟踪控制模块；

环境感知模块用于检测道路信息、障碍物状态信息以及本车行驶状态信息，且环境感知模块分别与候选参考轨迹生成模块、参考轨迹优化模块和轨迹跟踪控制模块信号连接；

候选参考轨迹生成模块根据环境感知模块采集的道路信息生成多条候选换道参考轨迹，并将候选换道参考轨迹发送给参考轨迹选取模块；

参考轨迹选取模块接收候选参考轨迹生成模块发送的候选参考轨迹，根据本车所能行驶的最大车速、最大加速度、最制动减速度、车辆的最小转弯半径、道路边界以及路面条件对候选参考轨迹进行初步筛选，若满足筛选条件的轨迹有S条，当S＜1时，判定为换道失败；当S≥1时，将满足筛选条件的S条候选参考轨迹通过评价函数cost进行评价；若在这S条候选轨迹中，

则有碰撞发生，判定为换道失败；若在这S候选轨迹中，

则存在可行的换道参考轨迹，将cost值最小的轨迹作为最优换道参考轨迹，并发送给参考轨迹优化模块，记最优换道参考轨迹的参考路径曲线为y_ref＝f_ref(x)，速度参考曲线为x_ref＝g_ref(t)；其中cost包括路径曲线评价函数cost1、速度曲线评价函数cost2、避障风险评价函数S_ob，如下式所示：

cost＝cost1+cost2+S_ob

S_ob＝∑_iS_obi i＝1，2，3，...

其中，ω₁、ω₂、ω₃、ω₄、ω₅、ω₇表示各项权值；x为车辆纵向位移由候选速度曲线x_n确定其随时间的变化关系；

分别为f_m(x)的一阶导数、二阶导数和三阶导数；

分别为g_n(t)的一阶导数、二阶导数和三阶导数；v_des表示该道路期望的车速；S_obi为第i个障碍物的风险代价值，η为常数，d_s为安全距离，d_{mn，min_i}为各候选参考轨迹与第i个障碍物之间的最小距离；

参考轨迹优化模块接受环境感知模块发送的本车行驶状态信息、障碍物状态信息和参考轨迹选取模块选取的最优换道参考轨迹，根据本车状态和障碍物状态对最优换道参考轨迹进行实时优化调整，并将实时优化后的最优轨迹发送给轨迹跟踪控制模块；分别利用N₃段五次样条曲线f_i(x)和N₄段五次样条曲线g_j(t)，使得目标函数J₁和J₂得到最小值，J₁和J₂表示如下：

式中，J₁为路径曲线优化目标函数，J₂为速度曲线优化目标函数，ω₇、ω₈、ω₉、ω₁₀为各项权值，k₁、k₂、k₃、k₄为常数，x_s、x_obi、y_s、y_obi、y_r分别每个采样时刻本车的纵向位置、第i个障碍物的纵向位置、本车的横向位置和第i个障碍物的横向位置、目标车道的横向位置，f_ref(x)和g_ref(t)分别为最优路径参考曲线和最优速度参考曲线；

轨迹跟踪控制模块接收参考轨迹优化模块发送的优化后的最优轨迹并对车辆进行轨迹跟踪控制。

9.根据权利要求8所述的一种智能驾驶换道避障轨迹规划与跟踪控制***，其特征在于，环境感知模块包括激光雷达、摄像头、速度仪和GPS设备用于检测道路信息、障碍物状态信息以及本车行驶状态信息。

Images