本发明属于智能交通领域，提出一种基于二次型规划和神经网络的自动驾驶换道轨迹规划方法。本发明利用信息感知模块获取换道车辆及其周围车辆的行驶信息；基于车辆行驶信息，横向偏移推荐模块给出横向偏移值，从而确定换道车辆的起终点位置信息；基于车辆行驶信息及横向偏移为输入，路径规划模块经计算得出换道车辆的平面坐标系静态换道路径；最后，轨迹规划模块给出换道车辆及周围车辆运动轨迹随时间的变化规律，完成换道过程规划的目标。本发明可以在保障换道车辆安全性前提下，综合考虑换道车辆及其周围车辆的运动轨迹，利用二次型规划快速求解，提高换道过程的速度和舒适度，从而满足自动驾驶对换道子任务优化性，求解快速性的要求。
基于二次型规划和神经网络的自动驾驶换道轨迹规划方法
技术领域
[0001] 本发明涉及智能交通领域，特别涉及一种基于二次型规划和神经网络的自动驾驶换道轨迹规划方法。
背景技术
[0002] 随着中国城市化进程的深入，城市车辆激增带来的交通事故和交通拥堵进一步加剧。自动驾驶在减少交通事故和缓解交通拥堵等方面展现出巨大的潜力，已成为各大汽车厂商研究热点。换道行为，作为车辆行驶中基本行为之一，其研究对提高车辆的智能化驾驶水平和增加道路通行能力有重要意义。
[0003] 换道轨迹规划是指车辆，为获取速度优势或由于行驶需求等，综合考虑自身及周围车辆位置、速度和加速度等因素，在给定的未来时间段内计算时空行驶轨迹，以保证变更车道行为的顺利安全地进行。一般而言，将换道过程划分为三个阶段：首先为信息感知，即通过传感器或V2I等硬件设施获取自身车辆和周围车辆的位置、速度和加速度等信息，为轨迹规划过程作底层输入；其次为轨迹规划，基于感知的信息计算未来给定时间段内的时空轨迹信息，该时空轨迹信息可进一步表现为空间位置信息和随时间变化的速度信息；最后为执行换道，车辆基于底层的控制模块沿着规划的时空轨迹完成车道变更任务。综合考虑换道过程，可以明确换道轨迹规划有以下显著特征：控制过程考虑因素较复杂、计算量较大和实时性要求高。
[0004] 现有换道轨迹规划的方法主要包括：组合方法和数学规划方法。组合方法可以通过网格划分和快速随机搜索树方法的结合实现在现实世界自动驾驶车辆的轨迹规划。组合方法已成功应用于若干个实际项目，在自动驾驶换道过程中可以计算稳定的时空轨迹。但组合方法无法实现规划轨迹的最优搜索，同时该方法没有考虑换道轨迹中的舒适和安全约束，因此在换道车辆与周围车辆信息实时安全交互上存在着一定的缺陷。
[0005] 目前，国内外学者利用数学规划方法对自动车辆的换道轨迹规划问题展开研究。数学规划方法提出，以乘客舒适性和安全性建立约束条件，以速度和舒适性等建立目标函数的优化模型以规划换道过程中的时空轨迹。在利用数学规划进行求解换道轨迹求解的中，很多研究为保证换道过程的安全性，引入矩形描述车辆轮廓或者圆形描述车辆轮廓，构造了一系列非凸约束的最优化问题。然而，目前对于非凸优化问题，尚无一套标准快速的求解方法，多用启发式算法进行求解，无法满足自动驾驶中对换道轨迹绝对安全性及实时性的要求。
发明内容
[0006] 针对上述技术问题，本发明提出一种基于二次型规划和神经网络的自动驾驶换道轨迹规划方法，通过构建信息感知模块，横向偏移推荐模块，路径规划模块，轨迹规划模块，可以为自动驾驶中换道子任务的实现提供时空轨迹支持，进一步保证换道过程中的平稳安全舒适。
[0007] 本发明所述方法假设所有车辆行驶在直线道路上，其中换道车辆目标通过当前车道换到目标车道从而完成换道行为。在本发明中，换道车辆所在车道定义为当前车道、换道车辆将要换道的车道命名为目标车道；换道车辆命名为换道车辆0(车0)，与换道车辆0同在当前车道但在换道车辆之前的车辆命名为当前车道前车1(车1)，在目标车道在换道车辆0之后的车辆命名为目标车道后车2(车2)，目标车道在换道车辆之前的车辆命名为目标车道前车3(车3)。
[0008] 本发明中，假设换道车辆0，在换道过程中，其加减速受当前车道的车1影响；在换道过程中，车0同样受目标车道的车2和车3影响，必要时车2会主动减速以保证车0在换道过程中的安全。
[0009] 本发明是通过以下技术方案实现的：
[0010] 基于二次型规划和神经网络的自动驾驶换道轨迹规划方法，所述方法包括：
[0011] 通过信息感知模块获取换道车辆及周围车辆关于位置、速度、加速度、加加速度的行驶信息；
[0012] 通过横向偏移推荐模块获取换道车辆在换道路径结束时的横坐标的横向偏移，进而获得换道路径结束时换道车辆的位置，为路径规划模块提供基础输入；
[0013] 基于信息感知模块获取的车辆行驶信息及横向偏移推荐模块给出的横向偏移，通过路径规划模块，对换道车辆建立五次多项式的路径模型，构建换道路径起终点约束条件，采用高斯消元法求解联立方程组，获得五次多项式的参数，进而生成给定最优横向偏移的路径；
[0014] 通过轨迹规划模块将轨迹规划时间段离散化处理，得到若干子时间段，建立不同子时间段下的行驶距离、速度、加速度、加加速度变量，以运动学约束、防碰撞约束、目标车道后车加加速度约束、跟驰模型约束为约束条件，以最小化加加速度和行驶距离为目标函数，利用二次型规划方法求解，获得换道车辆的轨迹信息。
[0015] 进一步地，所述信息感知模块获取换道车辆0、当前车道前车1、目标车道后车2、目标车道前车3，即车0、车1、车2、车3的位置、速度和加速度行驶信息：
[0016] 车辆k在时间t＝0时的行驶信息表示为：
[0017] vehk,t＝0，k＝{0,1,2,3}
[0018] vehk,t＝0＝[xk,t＝0,yk,t＝0,vk,t＝0,ak,t＝0,jk,t＝0]；
[0019] 其中xk,t＝0,yk,t＝0,vk,t＝0,ak,t＝0,jk,t＝0分别代表车辆k在时间t＝0的横坐标值、纵坐标值、速度、加速度和加加速度；
[0020] 在时间t＝0时，车0和车1处于当前车道，车2和车3在目标车道，以当前车道为基准建立坐标系，D为单个车道的宽度，车0、车1、车2、车3的位置关系符合：
[0021]
[0022] 进一步地，所述横向偏移推荐模块给出换道车辆0在换道路径结束时的横向偏移Δxf，进而获得换道路径结束时，即t＝f时刻换道车辆0的绝对坐标：
[0023] x0,t＝f＝x0,t＝0+Δxf,y0,t＝f＝D
[0024] 其中，所述横向偏移推荐模块给出横向偏移Δxf的方法具体为：
[0025] (1)生成一定数量的随机情景，所述随机情景包括换道车辆0、当前车道前车1、目标车道后车2、目标车道前车3在初始时刻的行驶信息vehk,t＝0，k＝{0,1,2,3}；
[0026] (2)设置横向偏移Δxf的下界为av0,t＝0，上界是bv3,t＝0，其中a和b为与时间相关的变量；
[0027] 将区间[av0,t＝0,bv3,t＝0]均匀等间隔划分为Nx份，对其中的每份i∈{0,1,2,...,Nx}；输入路径规划模块和轨迹规划模块，
并采用目标函数模块建立的二次型规划模型，求解目标函数值 其中，
遍历i∈[0,Nx]，得出最小目标函数zi
对应的 令最小目标函数zi对应的 为最优横向偏移Δxf，即：
[0028]
[0029] 构建获得以车辆集合行驶信息[veh0,t＝0,veh1,t＝0,veh2,t＝0,veh3,t＝0]为输入，最优横向偏移Δxf为输出的样本；
[0030] (3)搜集包含一定数量随机情景及最优横向偏移的数据构建训练集和测试集；采用神经网络进行训练，从而实现给定某个特殊随机情景，经神经网络预测，快速给出横向偏移的目标。
[0031] 进一步地，所述路径规划模块对换道车辆的路径建立的五次多项式的路径模型，公式化表达为：
[0032]
[0033] 式中，x0,t，y0,t分别表示换道车辆0在时间段t＝[0,f]的横纵坐标值，t＝0代表换道路径开始时刻，t＝f代表换道路径结束时刻；αi是五次多项式的参数，i∈{0,1,2,3,4,5}；
[0034] αi能够通过换道车辆0换道路径的起始时刻t＝0和终止时刻t＝f的车辆信息，利用高斯消元法求解相应的值，具体为：
[0035] 换道车辆0起始时刻t＝0和终止时刻t＝f的状态方程如下：
[0036]
[0037] 其中τ0,t,κ0,t分别代表换道车辆0在不同时刻t的导数及曲率，根据换道过程平稳性要求可知，在换道初始时刻t＝0和终点时刻t＝f的路径
导数及曲率为：τ0,t＝0＝0,κ0,t＝0＝0,τ0,t＝f＝0,κ0,t＝f＝0；
[0038] 采用高斯消元法求解联立方程组，进而得到五次多项式的参数αi，生成给定最优横向偏移Δxf的路径。
[0039] 进一步地，所述轨迹规划模块用于给出随时间变化的换道车辆0的行驶信息和目标车道后车2的行驶信息；
[0040] 所述轨迹规划模块将轨迹规划的时间段[0,tmax]离散化为I个子时间段，第i个时刻表示为ti， 其中，i∈{0,1,2,...,I}；其中子时间段的长度为Δt＝tmax/I；
[0041] 假设规划时间段tmax已知，换道时间f未知；用sd表示在规划时间段tmax内最远可能距离，sf表示在换道时间f内的换道路径距离，由sf到sd的行驶过程称为换道路径延伸区，则：
[0042]
[0043]
[0044]
[0045] 所述轨迹规划模块包括变量确定模块、约束构建模块、目标函数构建模块、二次型规划求解模块；所述约束构建模块包括运动学约束模块、防碰撞约束模块、目标车道后车2加加速度约束模块和跟驰模型约束模块；
[0046] 通过所述变量确定模块构建以下变量：换道车辆0的行驶距离s0,i、速度v0,i、加速度a0,i、冲击度j0,i、目标车道后车2的加加速度j2、换道车辆0加速度惩罚项Δa0,I；通过所述约束构建模块中的运动学约束模块、防碰撞模块、目标车道后车2冲击度约束模块和跟驰模型约束模块分别构建运动学约束条件、防碰撞约束条件、目标车道后车2冲击度约束条件、跟驰模型约束条件；通过所述目标函数构建模块构建目标函数，并通过所述二次型规划求解模块进行求解换道车辆的轨迹信息。
[0047] 进一步地，所述运动学约束模块用于建立换道车辆0行驶距离s0,i、速度v0,i、加速度a0,i和加加速度a0,i之间的联系，运动学约束构建为：
[0048]
[0049]
[0050]
[0051] Δt＝tmax/I为子时间段的长度；
[0052] 在车辆换道过程中，换道车辆0的行驶速度v0,i、加速度a0,i、加加速度j0,i不可以超出一定范围之内，有以下不等式约束：
[0053] 0＜v0,i＜vub，i＝1,2,...,I
[0054] alb＜a0,i＜aub，i＝1,2,...,I
[0055] jub＜j0,i＜jub，i＝1,2,...,I
[0056] 其中vub，aub，jub分别代表换道车辆0的行驶速度v0,i、加速度a0,i、加加速度j0,i的上限，alb，jlb分别代表车辆的加速度a0,i、加加速度j0,i的下限；
[0057] 所述目标车道后车2加加速度约束模块用于以目标车道后车2随着换道车辆0换道过程的加加速度为变量，进而得出目标车道后车2的运动轨迹，反映换道过程中换道车辆0与目标车道后车2的交互行为；
[0058] 目标车道后车2的运动学约束构建为：
[0059] jlb＜j2＜jub
[0060] alb＜a2,I＜aub
[0061] v2,I＞0
[0062] 式中，a2,I为轨迹规划结束时刻tI目标车道后车2的加速度，a2,I＝a2,0+j2tI；v2,I为轨迹规划结束时刻tI目标车道后车2的速度，
[0063] 进一步地，所述防碰撞约束模块用于防止换道车辆0和四周车辆之间的绝对距离太小，以避免危险状况的发生；换道车辆0的四周车辆包括当前车道前车1、目标车道后车2、目标车道前车3；
[0064] (1)防碰撞约束构建表示为：
[0065] sf＜s0,I＜sd
[0066] x1,i-xb1,i-0.5vehl1＞0，i＝1,2,...,I
[0067] xb2,i-x2,i-0.5vehl2＞0，i＝1,2,...,I
[0068] x3,i-xb3,i-0.5vehl3＞0，i＝1,2,...,I
[0069] 式中，xb1,i、xb2,i及xb3,i分别是在时刻ti换道车辆0与当前车道前车1、目标车道后车2、目标车道前车3的碰撞点；vehl1、vehl2及vehl3分别是当前车道前车1、目标车道后车2、目标车道前车3的长度；x1,i、x2,i及x3,i分别表示当前车道前车1、目标车道后车2、目标车道前车3在时刻ti的中心；其中当前车道前车1、目标车道后车2、目标车道前车3在时刻ti的中心为：
[0070]
[0071]
[0072]
[0073] 当前车道前车1与目标车道前车3在换道车辆0行驶的前方，在轨迹规划时间段[0,tmax]内，当前车道前车1和目标车道前车3的运动规律能够由初始时刻获取t＝0的信息veh1,t＝0,veh3,t＝0计算得出；目标车道后车2在换道车辆0行驶的后方，基于换道过程交互性考虑，目标车道后车2的运动规律由初始时刻t＝0获取的信息veh2,t＝0和变量车2加加速度j2共同计算得到；
[0074] (2)防碰撞约束中的碰撞点xb1,i、xb2,i及xb3,i能够由换道车辆0行驶距离s0,i得出，具体为：
[0075] 换道车辆0的碰撞点处于随时间ti的不连续变化中，将换道过程均匀离散取L个样本，每个样本l中，换道车辆0行驶距离为s0,l，对于每个子样本l∈L经路径规划模块中的路径模型，得出行驶距离s0,l及对应的碰撞点xb1,l、xb2,l及xb3,l；
[0076] 对每个s0,i确定其上界slb0,i和下界sub0,i，采用上界slb0,i和下界sub0,i在L个样本中筛选子样本集Li，对于每一个子样本l∈Li，行驶距离满足sub0,i＜s0,l＜slb0,i；其中，上界slb0,i及下界sub0,i的计算公式为：
[0077] slb0,i＝v0,0ti
[0078] sub0,i＝0.8v3,0ti
[0079] 对于每个时间段ti，基于子样本集Li，对碰撞点k的横坐标xb1,i、xb2,i及xb3,i建立线性拟合：
[0080] xbk,i＝αk,i+βk,is0,i+εk，k＝1,2,3
[0081] 式中，碰撞点k共考虑车辆的三个外端边界点，碰撞点横坐标为xbk,i，估计值为αk,i代表截断，βk,i代表斜率，εk代表在线性拟合碰撞点k时的随机误差项；因此，碰撞点k的横坐标xbk,i可以用换道车辆0的行驶距离s0,i估计：
[0082] 引入最大绝对误差项Δek,i以保证安全：
[0083] Δek,i＝maxi(|xbk,i-αk,i+βk,is0,i|)li∈Li；
[0084] 利用s0,i线性拟合xbk,i，k＝1,2,3，建立防碰撞约束：
[0085] x1,i-0.5vehl1-(α1,i+β1,is0,i+Δe1,i)＞0，i＝1,2,...,I
[0086] α2,i+β2,is0,i-Δe2,i-(x2,i+0.5vehl2)＞0，i＝1,2,...,I
[0087] x3,i-0.5vehl3-(α3,i+β3,is0,i+Δe3,i)＞0，i＝1,2,...,I
[0088] 其中，行驶距离s0,l及对应的碰撞点值xb1,l、xb2,l及xb3,l的计算方法；
[0089] 将区间[x0,0,xd]均匀等分为L份，构成样本集L，对于每个样本ll∈L，通过路径五次多项式函数得到x0,l,y0,l,s0,l, 通过 计算得出换道车辆在当前时刻的航向角通过换道车辆位置(x0,l,y0,l)、换道车辆在当前时刻的航向角 及
车辆长度vehl0，将换道车辆0简化为长方形，得到四个角的坐标cx1,cx2,cx3,cx4以及四条边与车道分界线的交点cx5,cx6,cx7,cx8；
[0090] 令点cx1,cx2,cx3,cx4,cx5,cx6,cx7,cx8组成点集Ω，分别计算点cxq∈Ω的纵坐标及横坐标 q＝1,2,3,4,5,6,7,8；令在目标车道的点集为ΩU，点集ΩU中L L
在当前车道的点集为Ω，点集Ω中
[0091] 碰撞点xb1,l、xb2,l及xb3,l由cxq∈Ω计算得出：
[0092]
[0093]
[0094]
[0095] 进一步地，所述跟驰模型约束模块用于在轨迹规划结束时刻tI，使目标车道后车2与换道车辆0、换道车辆0与目标车道前车3需满足跟驰模型，避免在轨迹规划结束时刻之后发生碰撞；
[0096] 采用Pariota的线性跟驰模型，假设车n在车n-1之后，车n的加速度需满足：
[0097] an＝ω1(Δxn-Δx*)+ω2Δvn
[0098] 式中，Δxn代表车n与车n-1之间的距离，Δxn＝xn-1-xn；Δvn代表车n与车n-1速度差，Δvn＝vn-1-vn；ω1,ω2,Δx*为线性跟驰模型标定的参数；
[0099] 在轨迹规划结束时刻tI，换道车辆0与目标车道前车3需满足以下关于加速度、距离及速度差的约束：
[0100] a0,I≤ω1(x3,I-x0,I-0.5vehl3-0.5vehl0-Δx*)+ω2(v3,I-v0,I)
[0101] 式中，v3,I代表目标车道前车3在轨迹规划结束时刻tI的速度，v3,I＝v3,0+a3,0tI；
[0102] x3,I代表目标车道前车3在轨迹规划结束时刻tI的横坐标，
[0103] x0,I代表换道车辆0的在轨迹规划结束时刻tI的横坐标，x0,I＝x0,f+s0,I-sf；
[0104] 在轨迹规划结束时刻tI，目标车道后车2与换道车辆0需满足以下关于加速度、距离及速度差的约束：
[0105] a2,I≤ω1(x0,I-x2,I-0.5vehl0-0.5vehl2-Δx*)+ω2(v0,I-v2,I)
[0106] 式中：x0,I-x2,I为车2与换道车辆0在轨迹规划结束时刻tI的距离；目标车道后车2的 位置 x2 ,I及速 度v2 ,I由目 标车 道后车 2的 加加速度 j2计 算得到 ：
[0107] 进一步地，所述目标函数构建模块以换道车辆0和换道影响到的目标车道后车2为研究对象，综合考虑安全性、舒适性及快速为目标，建立相应的二次型规划模型：
[0108]
[0109] 式中，μ和λ分别为设定的参数；
[0110] 代表时空轨迹规划的目标函数；
[0111] Δa0,I＝a0,I-[ω1(x3,I-x0,I-0.5vehl3-0.5vehl0-Δx*)+ω2(v3,I-v0,I)]。
[0112] 本发明的有益效果在于：
[0113] (1)本发明提出一种基于二次型规划快速求解换道时空轨迹的方法，同时本发明基于换道车辆和周围车辆的行驶信息建立线性化防碰撞约束。因此，本发明所述方法能够有效保障换道车辆过程中的安全性，同时将防碰撞约束建立为线性约束，进而可以利用二次型规划求解，有效降低求解复杂度和计算量，因而可以满足自动驾驶中对求解过程实时性快速性的要求，为自动驾驶换道子任务的工程实践提供有效保障。
[0114] (2)传统换道轨迹中，一般假设换道的时间给定以及换道车辆附近的车辆的速度需满足一定的条件，而本发明所述方法计算换道时空轨迹时，对换道过程的时间和换道车辆及其周围车辆的速度和加速度无强制要求。因此，本发明所述方法更适应于真实复杂时变的交通状况。
[0115] (3)传统组合方法一般采用生成横向偏移集，对每一个横向偏移生成换道轨迹，构建多条换道轨迹，对目标函数寻优，确定最优换道轨迹。并且传统组合方法采用遍历的方法，可以确定最优的换道轨迹，其方法相对稳定；但是，组合方法需要针对一定数量的横向偏移给出多条轨迹，计算量较大，不一定满足工程实践中换道轨迹规划实时性的要求。本发明所述方法采用神经网络推荐横向偏移的方法，能够快速预测最优横向偏移，进而直接给出最优的换道轨迹。神经网络推荐横向偏移的方法，可以快速预测优化的横向偏移进而直接计算最优的换道轨迹，可以在保证换道轨迹优化性能的同时，显著降低换道轨迹中的计算量，为工程实践中的换道轨迹规划提供便利。
[0116] (4)传统方案的研究重点是换道过程中的时间段，但缺乏轨迹规划结束时刻换道车辆状态的研究。在常见换道轨迹规划中，车辆由换道过程到跟驰过程的过渡中，可能出现换道车辆在轨迹规划结束时刻加速度太低，因而在跟驰过程中与前车的距离太大，造成时空上的效率浪费，以及与后车的距离急剧降低，容易发生危险。而本发明所述方法，综合考虑跟驰模型和换道过程，建立换道车辆在轨迹规划时刻结束的行驶状态的约束，保障轨迹规划结束后车辆的安全平稳行驶，保证不会在轨迹规划后出现一些极端情形，进而为换道车辆由换道过程到跟驰过程的平稳过渡提供保障。
[0117] (5)本发明所述方法考虑换道车辆与其后车辆(车2)的互动性。其中本发明考虑换道车辆目标车道后车(车2)的加加速度为变量，构建相应的目标函数和约束条件。因此，本发明所述方法可以考虑车2加加速度变量的换道轨迹，进而可以检验当前的换道轨迹是否在车2的可接受能力内，若在其可接受范围之内，则调整车2的加加速度以为换道车辆换道过程提供便利，保障换道过程安全顺利地进行。
[0118] 因而，本发明提供的是一种可行的方法，将车辆安全性约束用线性函数表达，从而可以利用求解线性凸函数的成熟标准方法即二次型规划求解，该方法可以同时满足自动驾驶过程对换道轨迹规划中的实时性和绝对安全性要求。
附图说明
[0119] 图1为本发明实施例中的一种基于二次型规划和神经网络的自动驾驶换道轨迹规划方法的结构示意图；
[0120] 图2为本发明实施例中的换道过程情景示意图；
[0121] 图3为本发明实施例中的换道路径及换道轨迹示意图；
[0122] 图4为本发明实施例中的换道过程中的碰撞点示意图；
[0123] 图5为本发明实施例中的换道过程中的碰撞点求解示意图；
[0124] 图6为本发明实施例中的换道过程中的防碰撞约束中子样本构建示意图；
[0125] 图7为本发明实施例中神经网络结构示意图。
具体实施方式
[0126] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。
[0127] 相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。
[0128] 本发明实施例提供一种基于二次型规划和神经网络的自动驾驶换道轨迹规划方法，其特征在于，如图1所示，所述方法包括：
[0129] 通过信息感知模块获取换道车辆及周围车辆关于位置、速度、加速度、加加速度的行驶信息；
[0130] 通过横向偏移推荐模块获取换道车辆在换道路径结束时的横坐标的横向偏移，进而获得换道路径结束时换道车辆的位置，为路径规划模块提供基础输入；
[0131] 基于信息感知模块获取的车辆行驶信息及横向偏移推荐模块给出的横向偏移，通过路径规划模块，对换道车辆建立五次多项式的路径模型，构建换道路径起终点约束条件，采用高斯消元法求解联立方程组，获得五次多项式的参数，进而生成给定最优横向偏移的路径；
[0132] 通过轨迹规划模块将轨迹规划时间段离散化处理，得到若干子时间段，建立不同子时间段下的行驶距离、速度、加速度、加加速度变量，以运动学约束、防碰撞约束、目标车道后车加加速度约束、跟驰模型约束为约束条件，以最小化加加速度和行驶距离为目标函数，利用二次型规划方法求解，获得换道车辆的轨迹信息。
[0133] 如图3所示，本发明中区分换道路径和换道轨迹两个概念。其中，横向偏移推荐模块和路径生成模块主要研究对象为换道路径，旨在为换道车辆提供换道的空间路径信息，此时不考虑换道车辆随时间的运动变化情况。轨迹生成模块，主要研究对象为换道轨迹，包括换道车辆在时空上的运动信息。换道轨迹，就空间而言，可进一步划分为换道路径和换道路径延伸区(直线行驶区域)。通过轨迹生成模块，换道车辆可以获得在整个换道过程中随时间的运动信息，如位置、速度、加速度和加加速度。
[0134] 在本实施例中，换道车辆0所处的交通情景如图2所示，假设所有车辆行驶在直线道路上，其中换道车辆目标通过当前低速道路换到高速道路从而提高行驶速度减少行驶时间。换道车辆所在车道定义为当前车道、换道车辆将要换道的车道命名为目标车道；换道车辆命名为换道车辆0(车0)，与换道车辆0同在当前车道但在换道车辆之前的车辆命名为当前车道前车1(车1)，在目标车道在换道车辆0之后的车辆命名为目标车道后车2(车2)，目标车道在换道车辆之前的车辆命名为目标车道前车3(车3)。
[0135] 在本实施例中，所述信息感知模块获取换道车辆0、当前车道前车1、目标车道后车2、目标车道前车3，即车0、车1、车2、车3的位置、速度和加速度行驶信息：
[0136] 车辆k在时间t＝0时的行驶信息表示为：
[0137] vehk,t＝0，k＝{0,1,2,3}
[0138] vehk,t＝0＝[xk,t＝0,yk,t＝0,vk,t＝0,ak,t＝0,jk,t＝0]；
[0139] 其中xk,t＝0,yk,t＝0,vk,t＝0,ak,t＝0,jk,t＝0分别代表车辆k在时间t＝0的横坐标值、纵坐标值、速度、加速度和加加速度；
[0140] 在时间t＝0时，如图2所示，车0和车1处于当前车道，车2和车3在目标车道，以当前车道为基准建立坐标系，D为单个车道的宽度，车0、车1、车2、车3的位置关系符合：
[0141]
[0142] 其中，优选地，所述信息感知模块通过传感器或V2I(汽车与基础设施通讯)等硬件设施获得上述行驶信息。
[0143] 在本实施例中，所述横向偏移推荐模块，可以给出换道车辆0在换道过程结束时的横向偏移Δxf，进一步可获得换道过程结束时换道车辆的绝对坐标为路径规划模块提供基础输入。换道路径结束t＝f时刻的绝对坐标表示如下：
[0144] x0,t＝f＝x0,t＝0+Δxf,y0,t＝f＝D
[0145] 所述横向偏移推荐模块可以完成以下任务：生成一定数量的随机情景，生成随机情景表现为换道车辆和周围车辆的行驶信息；对每个随机情景将横向偏移等间隔划分，对等间隔划分的横向偏移输入路径规划模块和轨迹规划模块的输入以计算目标函数值，遍历等间隔划分的横向偏移，最小化目标函数以确定对当前的随机情景的最优横向偏移；搜集包含一定数量随机情景及最优横向偏移的数据构建训练集和测试集；采用神经网络训练上述数据集，从而实现给定某个特殊随机情景，经神经网络预测，快速给出横向偏移的目标。
[0146] 具体包括如下步骤：
[0147] 步骤201，生成一定数量的随机情景，包括换道车辆0、周围车辆1、2和3的行驶信息vehk,t＝0k＝{0,1,2,3}；
[0148] 步骤202，设置横向偏移Δxf的下界为av0,t＝0，上界是bv3,t＝0，其中a和b为与时间相关的变量，一般设置为a＝2和b＝4；
[0149] 将区间[av0,t＝0,bv3,t＝0]均匀等间隔划分为Nx份，对其中的每份i∈{0,1,2,...,Nx}；输入路径规划模块和轨迹规划模块，
并采用目标函数模块建立的二次型规划模型，求解目标函数值 其中，
这里Nx可以根据实际需求、精度以及
计算能力设置相应的值。进一步地，遍历i∈[0,Nx]，得出最小目标函数zi对应的 令最小目标函数zi对应的 为最优横向偏移Δxf，即：
[0150]
[0151] 构建以车辆集合行驶信息[veh0,t＝0,veh1,t＝0,veh2,t＝0,veh3,t＝0]为输入，Δxf为输出的样本。
[0152] 步骤203,所述横向偏移推荐模块生成一系列的随机情景，经步骤202和203确定最优横向偏移，进而构建训练集和测试集；采用神经网络训练上述数据集，从而实现给定某个特殊随机情景，经神经网络预测，快速给出横向偏移的目标。
[0153] 在本实施例中，所述的路径规划模块，如图3所示，基于信息感知模块获取的车辆行驶信息及横向偏移推荐模块给出的横向偏移，对换道车辆的路径建立五次多项式的路径模型，构建换道路径起终点约束条件，采用高斯消元法求解联立方程组，获得五次多项式的参数，进而生成给定最优横向偏移的路径。
[0154] 所述路径规划模块对换道车辆的路径建立的五次多项式的路径模型，公式化表达为：
[0155]
[0156] 式中，x0,t，y0,t分别表示换道车辆0在时间段t＝[0,f]的横纵坐标值，t＝0代表换道路径开始时刻，t＝f代表换道路径结束时刻；αi是五次多项式的参数，i∈{0,1,2,3,4,5}；
[0157] αi能够通过换道车辆0换道路径的起始时刻t＝0和终止时刻t＝f的车辆信息，利用高斯消元法求解相应的值，具体为：
[0158] 换道车辆0起始时刻t＝0和终止时刻t＝f的状态方程如下：
[0159]
[0160] 其中τ0,t,κ0,t分别代表换道车辆0在不同时刻t的导数及曲率，根据换道过程平稳性要求可知，在换道初始时刻t＝0和终点时刻t＝f的路径
导数及曲率为：τ0,t＝0＝0,κ0,t＝0＝0,τ0,t＝f＝0,κ0,t＝f＝0；
[0161] 采用高斯消元法求解联立方程组，进而得到五次多项式的参数αi，生成给定最优横向偏移Δxf的路径。
[0162] 在本实施例中，所述轨迹规划模块用于给出随时间变化的换道车辆0的行驶信息和目标车道后车2的行驶信息。路径规划模块给出换道车辆在换道过程中的横纵坐标变化的空间静态信息，但是轨迹规划模块旨在给出换道车辆在换道过程中随时间变化的相关行驶信息。为了快速有效求解时空轨迹，轨迹规划模块采用将时间离散化的方法，将给定时间段离散为一定数量的子时间段，以子时间段的行驶信息为变量，最优化换道过程的综合目标函数，从而获得子时间段的行驶信息，进一步通过插值可获得换道车辆在轨迹规划过程随时间变化的行驶信息。
[0163] 所述轨迹规划模块将轨迹规划的时间段[0,tmax]离散化为I个子时间段，第i个时刻表示为ti， 其中，i∈{0,1,2,...,I}；其中子时间段的长度为Δt＝tmax/I；
[0164] 假设规划时间段tmax已知，换道时间f未知；用sd表示在规划时间段tmax内最远可能距离，sf表示在换道时间f内的换道路径距离，由sf到sd的行驶过程称为换道路径延伸区，如图3所示，则：
[0165]
[0166]
[0167]
[0168] 所述轨迹规划模块包括约束构建模块、目标函数构建模块、二次型规划求解模块；所述约束构建模块包括运动学约束模块、防碰撞约束模块、目标车道后车2加加速度约束模块和跟驰模型约束模块；
[0169] 通过所述变量确定模块构建以下变量：换道车辆0的行驶距离s0,i、速度v0,i、加速度a0,i、冲击度j0,i、目标车道后车2的加加速度j2、换道车辆0加速度惩罚项Δa0,I；通过所述约束构建模块中的运动学约束模块、防碰撞模块、目标车道后车2冲击度约束模块和跟驰模型约束模块分别构建运动学约束条件、防碰撞约束条件、目标车道后车2冲击度约束条件、跟驰模型约束条件；通过所述目标函数构建模块构建目标函数，并通过所述二次型规划求解模块进行求解换道车辆的轨迹信息。
[0170] 轨迹规划模块中，共包含以下变量：换道车辆0的行驶距离s0,i、速度v0,i、加速度a0,i、加加速度j0,i、目标车道后车2的加加速度j2、换道车辆0加速度惩罚项Δa0,I。其中，行驶距离s0,i、速度v0,i、加速度a0,i、加加速度j0,i代表的含义为换道车辆0在时刻的ti的行驶信息。其中，加加速度j2代表车辆2在时间段[0,tmax]的加加速度。加速度惩罚项Δa0,I表达车0和车3在轨迹规划结束时刻的加速度差值，以避免|Δa0,I|太大，造成轨迹规划结束时，车0需要立刻加速，造成车0的速度变化太大，给乘客带来极强的不舒适；该项采用拉格朗日松弛的方法，在目标函数上增加一项以方便求解；其中，Δa0,I的计算定义为：
[0171] Δa0,I＝a0,I-[ω1(x3,I-x0,I-0.5vehl3-0.5vehl0-Δx*)+ω2(v3,I-v0,I)]，[0172] 目标函数最小化 旨在降低轨迹规划结束时刻tI，车0与车3的加速度差值，以避免|Δa0,I|太大，造成轨迹规划结束时，车0需要立刻加速，造成车0的速度变化太大，给乘客带来极强的不舒适。
[0173] 通过所述约束构建模块中的运动学约束模块、防碰撞模块、目标车道后车2加加速度约束模块和跟驰模型约束模块分别构建运动学约束条件、防碰撞约束条件、目标车道后车2加加速度约束条件、跟驰模型约束条件；
[0174] 所述运动学约束模块用于建立换道车辆0行驶距离s0,i、速度v0,i、加速度a0,i和加加速度a0,i之间的联系及车2的行驶过程中的一些约束。
[0175] (1)车0的运动学约束：
[0176]
[0177]
[0178]
[0179] Δt＝tmax/I为子时间段的长度；
[0180] 在车辆换道过程中，换道车辆0的行驶速度v0,i、加速度a0,i、加加速度j0,i不可以超出一定范围之内，有以下不等式约束：
[0181] 0＜v0,i＜vub，i＝1,2,...,I
[0182] alb＜a0,i＜aub，i＝1,2,...,I
[0183] jub＜j0,i＜jub，i＝1,2,...,I
[0184] 其中vub，aub，jub分别代表换道车辆0的行驶速度v0,i、加速度a0,i、加加速度j0,i的上限，alb，jlb分别代表车辆的加速度a0,i、加加速度j0,i的下限。
[0185] (2)车2的运动学约束
[0186] 所述目标车道后车2加加速度约束模块用于以车2随着换道车辆0换道过程的加加速度为变量，进而得出车2的运动轨迹，反映换道过程中换道车辆0与车2的交互行为；
[0187] 目标车道后车2的运动学约束构建为：
[0188] jlb＜j2＜jub
[0189] alb＜a2,I＜aub
[0190] v2,I＞0
[0191] 式中，a2,I为轨迹规划结束时刻tI车2的加速度，a2,I＝a2,0+j2tI；v2,I为轨迹规划结束时刻tI车2的速度，
[0192] 所述防碰撞约束模块用于防止换道车辆0和四周车辆之间的绝对距离太小，以避免危险状况的发生；换道车辆0的四周车辆包括当前车道前车1、目标车道后车2、目标车道前车3；
[0193] (1)防碰撞约束构建表示为：
[0194] sf＜s0,I＜sd
[0195] x1,i-xb1,i-0.5vehl1＞0，i＝1,2,...,I
[0196] xb2,i-x2,i-0.5vehl2＞0，i＝1,2,...,I
[0197] x3,i-xb3,i-0.5vehl3＞0，i＝1,2,...,I
[0198] 式中，xb1,i、xb2,i及xb3,i分别是在时刻ti换道车辆0与车1、2和3的碰撞点，如图4及图5所示；vehl1、vehl1及vehl3分别是当前车道前车1、目标车道后车2、目标车道前车3的长度；x1,i、x2,i及x3,i分别表示车1、2和3的在时刻ti的中心；其中车1、2和3的在时刻ti的中心为：
[0199]
[0200]
[0201]
[0202] 本实施例中，车1和车3在换道车辆0的前方，因此在轨迹规划时间段[0,tmax]内，车1和3的运动规律可以由初始时刻获取t＝0的信息veh1,t＝0,veh3,t＝0计算得出；车2在换道车辆0的后方，基于换道过程交互性考虑，车2的运动规律由初始时刻t＝0获取的信息veh2,t＝0和变量车2加加速度j2共同计算得到；
[0203] (2)防碰撞约束中的碰撞点xb1,i、xb2,i及xb3,i可由换道车辆0行驶距离s0,i得出，如图4所示。其中行驶距离s0,i与碰撞点xb1,i、xb2,i及xb3,i的函数关系，为不连续的非线性函数，不利于构建线性条件为约束的二次型规划进行快速求解。为了将防碰撞约束用线性函数表达，本发明实施例采用利用换道车辆0行驶距离s0,i线性拟合碰撞点xb1,i、xb2,i及xb3,i的方法。
[0204] 由于换道车辆的碰撞点处于随时间ti的不连续变化中，因此，本发明实施例提出用将换道过程，均匀离散取L个样本，对于每个样本l∈L经路径规划模块中的路径函数，可以得出行驶距离s0,l及对应的碰撞点xb1,l、xb2,l及xb3,l。
[0205] 对每个s0,i确定其可能的上下界slb0,i，sub0,i，采用上界slb0,i，下界sub0,i在L个样本中筛选子样本集Li，对于每一个子样本l∈Li，其行驶距离满足sub0,i＜s0,l＜slb0,i。其中，上界slb0,i，下界sub0,i的计算公式为：
[0206] slb0,i＝v0,0ti
[0207] sub0,i＝0.8v3,0ti
[0208] 进一步地，对于每个时间段ti，基于子样本集Li，对碰撞点xb1,i、xb2,i及xb3,i可以建立线性拟合。
[0209] xbk,i＝αk,i+βk,is0,i+εk，k＝1,2,3
[0210] 式中，碰撞点k共考虑车辆的三个外端边界点，碰撞点横坐标为xbk,i，估计值为αk,i代表截断，βk,i代表斜率，εk代表在线性拟合碰撞点k时的随机误差项；因此，碰撞点k的横坐标xbk,i可以用换道车辆0的行驶距离s0,i估计： 同时，为保证安全，引入最大绝对误差项Δek,i以保证安全：Δek,i＝maxi(|xbk,i-αk,i+βk,is0,i|)li∈Li。
[0211] 利用s0,i线性拟合xbk,i k＝1,2,3可以建立防碰撞约束：
[0212] x1,i-0.5vehl1-(α1,i+β1,is0,i+Δe1,i)＞0i＝1,2,...,I
[0213] α2,i+β2,is0,i-Δe2,i-(x2,i+0.5vehl2)＞0i＝1,2,...,I
[0214] x2,i-0.5vehl3-(α3,i+β3,is0,i+Δe3,i)＞0i＝1,2,...,I
[0215] 进一步地，其中，行驶距离s0,l及对应的碰撞点值xb1,l、xb2,l及xb3,l的计算方法；
[0216] 将区间[x0,0,xd]均匀等分为L份，构成样本集L，对于每个样本ll∈L，通过路径五次多项式函数得到x0,l,y0,l,s0,l, 通过 计算得出换道车辆在当前时刻的航向角通过换道车辆位置(x0,l,y0,l)、换道车辆在当前时刻的航向角 及车
辆长度vehl0，将换道车辆0简化为长方形，得到四个角的坐标cx1,cx2,cx3,cx4以及四条边与车道分界线的交点cx5,cx6,cx7,cx8；
[0217] 令点cx1,cx2,cx3,cx4,cx5,cx6,cx7,cx8组成点集Ω，分别计算点cxq∈Ω的纵坐标及横坐标 q＝1,2,3,4,5,6,7,8；令在目标车道的点集为ΩU，点集ΩU中在当前车道的点集为ΩL，点集ΩL中
[0218] 碰撞点xb1,l、xb2,l及xb3,l由cxq∈Ω计算得出：
[0219]
[0220]
[0221]
[0222] 因此，利用均匀离散的方法可获取L个样本的行驶距离s0,l及对应的碰撞点值xb1,l、xb2,l及xb3,l。
[0223] 所述跟驰模型约束模块用于在轨迹规划结束时刻tI，使目标车道后车2与换道车辆0、换道车辆0与目标车道前车3需满足跟驰模型，避免在轨迹规划结束时刻之后发生碰撞。
[0224] 本实施例采用Pariota的线性跟驰模型，假设车在车-1之后，车的加速度需满足：
[0225] an＝ω1(Δxn-Δx*)+ω2Δvn
[0226] 式中，Δxn代表车n与车n-1之间的距离，Δxn＝xn-1-xn。Δvn代表车n与车n-1速度差，Δvn＝vn-1-vn。ω1,ω2,Δx*为线性跟驰模型标定的参数，优选地，取值分别为0.0343,0.948,30。
[0227] 在轨迹规划结束时刻tI，换道车辆0与目标车道前车3需满足以下关于加速度、距离及速度差的约束：
[0228] a0,I≤ω1(x3,I-x0,I-0.5vehl3-0.5vehl0-Δx*)+ω2(v3,I-v0,I)
[0229] 式中，v3,I代表目标车道前车3在轨迹规划结束时刻tI的速度，v3,I＝v3,0+a3,0tI；
[0230] x3,I代表目标车道前车3在轨迹规划结束时刻tI的横坐标，
[0231] x0,I代表换道车辆0的在轨迹规划结束时刻tI的横坐标，x0,I＝x0,f+s0,I-sf；
[0232] 在轨迹规划结束时刻tI，目标车道后车2与换道车辆0需满足以下关于加速度、距离及速度差的约束：
[0233] a2,I≤ω1(x0,I-x2,I-0.5vehl0-0.5vehl2-Δx*)+ω2(v0,I-v2,I)
[0234] 式中：x0,I-x2,I为车2与换道车辆0在轨迹规划结束时刻tI的距离；目标车道后车2的 位置 x2 ,I及速 度v2 ,I由目 标车 道后车 2的 加加速度 j2计 算得到 ：
[0235] 在本实施例中，所述目标函数构建模块以换道车辆0和换道影响到的目标车道后车2为研究对象，综合考虑安全性、舒适性及快速为目标，建立相应的二次型规划模型：
[0236]
[0237] 式中，μ和λ分别为设定的参数，其值为-0.04和1000；
[0238] 代表时空轨迹规划的目标函数；
[0239] 本发明旨在提高换道车辆的舒适性，因此最小化换道车辆在换道过程中的加速度和加加速度
[0240] 换道过程中，换道车辆0必然会影响车2的正常行驶，本发明中用车2加加速度j2来反应其变化，然而车2理想状态是不受换道车辆0的影响，可以保持初始时刻的行驶规律，本发明使用最小化 以降低换道过程对后续车辆的影响；
[0241] 本发明基于给定时间长度规划换道车辆的轨迹，其中换道过程的时间越低，其安全越有保障。给定规划时间长度tmax，较快的换道过程意味着在规划时间段内换道车辆行驶距离越远，因此本发明旨在最大化 因此参数μ采用负数-0.04，且用最小化目标表示。
[0242] Δa0,I＝a0,I-[ω1(x3,I-x0,I-0.5vehl3-0.5vehl0-Δx*)+ω2(v3,I-v0,I)]，[0243] 目标函数最小化 旨在降低轨迹规划结束时刻tI，车0与车3的加速度差值，以避免|Δa0,I|太大，造成轨迹规划结束时，车0需要立刻加速，造成车0的速度变化太大，给乘客带来极强的不舒适。
[0244] 在本实施例中，所述的二次规划模型由目标函数及约束条件组成，利用matlab的quadprog函数进行求解。
[0245] 综上，本发明所述的基于二次型规划和神经网络的自动驾驶换道轨迹规划方法，利用信息感知模块获取换道车辆及其周围车辆的行驶信息、利用横向偏移推荐模块给出换道过程结束的横向偏移，进一步给出换道车辆在换道过程的起终点位置信息。利用路径规划模块，基于换道过程换道车辆的行驶信息为输入得出静态平面坐标系换道路径。通过轨迹规划模块，基于给定的静态路径和周围换道车辆的行驶信息，得出换道车辆随时间变化的运动规律。本发明可以在保障换道车辆安全性前提下，综合考虑换道车辆及其周围车辆的运动轨迹，利用二次型规划快速求解，提高换道过程的速度和舒适度。从而满足自动驾驶对换道子任务优化性，求解快速性的要求。
