本发明公开了一种基于碰撞检测的智能车高速换道规划方法，包括：对目标车道的状态空间进行采样得到换道结束点的终端状态，利用五次多项式对换道起始点和结束点之间的换道轨迹进行拟合，形成换道候选轨迹集；利用代价评估函数对换道轨迹集中的候选换道轨迹进行评估，选取评估得分最低的轨迹作为最佳换道轨迹；获取目标车道内目标车辆的历史运动状态信息，然后将历史运动状态信息从笛卡尔坐标系映射到Frenet坐标系下，进行目标车辆轨迹预测；进行规划换道轨迹与目标车辆预测轨迹之间的碰撞检测，根据碰撞检测的结果控制车辆的换道行为；本发明通过碰撞检测的方式保证车辆以最佳换道轨迹换道的同时最大程度保证高速换道的安全性。
1.一种基于碰撞检测的智能车高速换道规划方法，其特征在于，包括如下步骤：S1：对目标车道的状态空间进行采样得到换道结束点的终端状态，利用五次多项式对换道起始点和结束点之间的换道轨迹进行拟合，形成换道候选轨迹集；利用代价评估函数对换道轨迹集中的候选换道轨迹进行评估，选取评估得分最低的轨迹作为换道轨迹；
Ctotal＝kxCx+kyCy式中，kx、ky是权重系数，Cx、Cy分别表示纵向、横向代价函数，表达式分别为：式中，ki、kv是权重系数，vdes是换道过程中的期望速度；
式中，kj、kl是权重系数，ldes是换道过程中的期望侧向位移；
S2：利用车载传感器获取目标车道内目标车辆的历史运动状态信息，包括位置、速度、加速度、航向，然后将历史运动状态信息从笛卡尔坐标系映射到Frenet坐标系下，进行目标车辆轨迹预测，最后将预测轨迹坐标映射回笛卡尔坐标系；
S3：进行规划换道轨迹与目标车辆预测轨迹之间的碰撞检测，根据碰撞检测的结果控制换道行为，如果碰撞检测通过则按照规划的最佳轨迹进行换道，否则不进行换道。
2.根据权利要求1所述的一种基于碰撞检测的智能车高速换道规划方法，其特征在于，换道候选轨迹集为：
Sset＝Ψ(T,S0,Sf)式中，T表示换道时间，S0表示主车换道初始状态，Sf表示终端状态，Ψ(t,S0,Sf)表示一条从初始状态S0到终端状态Sf的换道轨迹。
3.根据权利要求2所述的一种基于碰撞检测的智能车高速换道规划方法，其特征在于，换道轨迹利用五次多项式对起始点、结束点两点进行拟合得到：式中，x表示纵向位置坐标，y表示横向位置坐标，cx0、cx1、cx2、cx3、cx4、cx5和cy0、cy1、cy2、cy3、cy4、cy5为待求系数；
将初始点纵向状态 和结束点纵向状态 作为边界条件代入纵向轨迹可以得到：
将初始点横向状态 和结束点横向状态 作为边界条件代入横向轨迹得到：
选取不同的换道结束点终端状态和换道时间得到一个换道轨迹的集合。
4.根据权利要求3所述的一种基于碰撞检测的智能车高速换道规划方法，其特征在于，根据车辆当前运动状态确定换道结束点的纵、横向终端状态；其中纵向终端状态满足：式中，Tc是避碰时间；
式中，L是车道宽度，Δ是单位横向采样距离，k是采样数目，取决于换道初始速度，初始速度越大采样数目k越大。
5.根据权利要求1所述的一种基于碰撞检测的智能车高速换道规划方法，其特征在于，步骤S2中，利用车载传感器获取目标车道内目标车辆的历史运动状态信息，包括位置、速度、加速度、航向，然后将历史运动信息从笛卡尔坐标系映射到Frenet坐标系下，映射的公式为：
式中，是Frenet坐标系下纵向坐标对时间的导数，vx是笛卡尔坐标系下的速度，θ是笛卡尔坐标系下航向角θx和匹配点航向θr的差值，即θ＝θx‑θr，κr是参考点的曲率；
式中，ax是笛卡尔坐标系下的加速度，l是目标车辆沿着法向方向的偏移，l′是l对纵向坐标s的导数，l′＝(1‑κrl)tan(θ)；
式中，l″是l对纵向坐标s的二阶导数，κx是笛卡尔坐标系下位置点的曲率。
6.根据权利要求5所述的一种基于碰撞检测的智能车高速换道规划方法，其特征在于，每间隔时间 记录一次映射后的历史运动状态信息，以n为一个时间域值,将时间域n内记录的历史数据打包成一个向量输入到轨迹预测模块，轨迹预测模块是基于多层神经网络的编解码结构，利用编码器对目标车辆的历史运动信息进行编码，解码器输出预测的目标车辆在Frenet坐标系下的轨迹，将轨迹映射回笛卡尔坐标系，映射公式为：x＝r(s)+nrl
式中，x是笛卡尔坐标系下位置向量，x＝(x,y)，r(s)是Frenet坐标系下参考点位置向量，nr是参考点处的参考法向量，l是目标车辆沿着法向方向的偏移。
7.根据权利要求6所述的一种基于碰撞检测的智能车高速换道规划方法，其特征在于，最终得到目标车辆的预测轨迹为：式中， 是在换道开始的t0时刻车辆笛卡尔坐标下的预测位置坐标，mp是预测时域的长度。
8.根据权利要求7所述的一种基于碰撞检测的智能车高速换道规划方法，其特征在于，换道规划模块规划出一条换道轨迹，以时间间隔p对换道轨迹进行采样，得到离散的换道轨迹点，其表达式为：
目标车辆轨迹预测模块预测目标车辆未来的运动轨迹，通过模拟车辆的未来运动，判断在换道过程中是否会发生碰撞，碰撞检测的判别公式为：式中，robj、rego分别是目标车和主车的膨胀半径，ξ是安全系数，跟主车的速度和碰撞检测的时刻有关，速度越快，安全系数越大，采样时刻越大，轨迹预测的不确定度越大，安全系数越大；安全系数的表达式为：
式中，v是主车的速度，单位为km/h，i是碰撞检测的时刻，单位为s。
一种基于碰撞检测的智能车高速换道规划方法
技术领域
[0001] 本发明属于智能交通技术领域，更具体的说是涉及一种基于碰撞检测的智能车高速换道规划方法。
背景技术
[0002] 智能汽车技术被认为在改善道路安全以及提高交通运行效率方面有着巨大的潜力，近年来得到了广泛关注与研究。对于智能车来说，安全是其最重要的基石，目前，对换道
安全性的评价方法多是通过时空间量化模型实现的，如碰撞时间(TTC)模型等。中国专利文
献CN111016898A(申请号：201911113734.X)公开了一种智能车的换道规划方法，根据主车
运动状态和障碍物的信息确定换道时间和最大侧向加速度，并带入到换道安全模型中判断
是否安全。这种换道安全评估方法对换道过程中其他车辆的运动状态进行了理想化假设，
如假设其服从匀速运动模型，未全面考虑其驾驶意图和轨迹的不确定性，因此存在一定的
碰撞风险，尤其是在高速工况下。
[0003] 因此，如何提供一种安全性高的智能车高速换道规划方法是本领域技术人员亟需解决的问题。
发明内容
[0004] 有鉴于此，本发明提供了一种基于碰撞检测的智能车高速换道规划方法，通过碰撞检测的方式保证了换道的安全性。
[0005] 为了实现上述目的，本发明采用如下技术方案：
[0006] 一种基于碰撞检测的智能车高速换道规划方法，包括如下步骤：
[0007] S1：对目标车道的状态空间进行采样得到换道结束点的终端状态，利用五次多项式对换道起始点和结束点之间的换道轨迹进行拟合，形成换道候选轨迹集；利用代价评估
函数对换道轨迹集中的候选换道轨迹进行评估，选取评估得分最低的轨迹作为换道轨迹；
[0008] S2：利用车载传感器获取目标车道内目标车辆的历史运动状态信息，包括位置、速度、加速度、航向，然后将历史运动状态信息从笛卡尔坐标系映射到Frenet坐标系下，进行
目标车辆轨迹预测，最后将预测轨迹坐标映射回笛卡尔坐标系；
[0009] S3：进行规划换道轨迹与目标车辆预测轨迹之间的碰撞检测，根据碰撞检测的结果控制换道行为，如果碰撞检测通过则按照规划的最佳轨迹进行换道，否则不进行换道。。
[0010] 优选的，换道候选轨迹集为：
[0011] Sset＝Ψ(T,S0,Sf)
[0012] 式中，T表示换道时间，S0表示主车换道初始状态，Sf表示终端状态，Ψ(t,S0,Sf)表示一条从初始状态S0到终端状态Sf的换道轨迹。
[0013] 优选的，换道轨迹利用五次多项式对起始点、结束点两点进行拟合得到：
[0014]
[0015] 式中，x表示纵向位置坐标，y表示横向位置坐标，cx0、cx1、cx2、cx3、cx4、cx5和cy0、cy1、cy2、cy3、cy4、cy5为待求系数；
[0016] 将初始点纵向状态 和结束点纵向状态 作为边界条件带入纵向轨迹可以得到：
[0017]
[0018]
[0019] 换道时间为T使得下式成立：
[0020] tf＝t0+T
[0021] 求解得到多项式系数为：
[0022]
[0023]
[0024] 将初始点横向状态 和结束点横向状态 作为边界条件带入横向轨迹得到：
[0025]
[0026]
[0027] 换道时间为T使得下式成立：
[0028] tf＝t0+T
[0029] 求解得到多项式系数为：
[0030]
[0031]
[0032] 选取不同的换道结束点终端状态和换道时间得到一个换道轨迹的集合。
[0033] 优选的，根据车辆当前运动状态确定换道结束点的纵、横向终端状态。其中纵向终端状态满足：
[0034]
[0035] 式中，Tc是避碰时间。
[0036] 横向终端状态满足：
[0037]
[0038] 式中，L是车道宽度，Δ是单位横向采样距离，k是采样数目，取决于换道初始速度，初始速度越大采样数目k越大。
[0039] 优选的，代价评估函数为：
[0040] Ctotal＝kxCx+kyCy
[0041] 式中，kx、ky是权重系数，Cx、Cy分别表示横向、纵向代价函数，表达式分别为：
[0042]
[0043] 式中，ki、kv是权重系数，vdes是换道过程中的期望速度；
[0044]
[0045] 式中，kj、kl是权重系数，ldes是换道过程中的期望侧向位移。
[0046] 优选的，步骤S2中，利用车载传感器获取目标车道内目标车辆的历史运动状态信息，包括位置、速度、加速度、航向，然后将历史运动信息从笛卡尔坐标系映射到Frenet坐标
系下，映射的公式为：
[0047]
[0048] 式中，是Frenet坐标系下纵向坐标对时间的导数，vx是笛卡尔坐标系下的速度，θ是笛卡尔坐标系下航向角θx和匹配点航向θr的差值，即θ＝θx‑θr，κr是参考点的曲率；
[0049]
[0050] 式中，ax是笛卡尔坐标系下的加速度，l′是l对纵向坐标s的导数，l′＝(1‑κrl)tan(θ)；
[0051]
[0052] 式中，l″是l对纵向坐标s的二阶导数，κx是笛卡尔坐标系下位置点的曲率。
[0053] 优选的，每间隔时间 记录一次映射后的历史运动状态信息，以n为一个时间域值,将时间域n内记录的历史数据打包成一个向量输入到轨迹预测模块，轨迹预测模块是基
于多层神经网络的编解码结构，利用编码器对目标车辆的历史运动信息进行编码，解码器
输出预测的目标车辆在Frenet坐标系下的轨迹，将轨迹映射回笛卡尔坐标系，映射公式为：
[0054] x＝r(s)+nrl
[0055] 式中，x是笛卡尔坐标系下位置向量，x＝(x,y)，r(s)是Frenet坐标系下参考点位置向量，nr是参考点处的参考法向量，l是沿着法向方向的偏移。
[0056] 优选的，最终得到目标车辆的预测轨迹为：
[0057]
[0058] 式中， 是在换道开始的t0时刻车辆笛卡尔坐标下的预测位置坐标，mp是预测时域的长度。
[0059] 优选的，换道规划模块规划出一条换道轨迹，以时间间隔p对换道轨迹进行采样，得到离散的换道轨迹点，其表达式为：
[0060]
[0061] 目标车辆轨迹预测模块预测目标车辆未来的运动轨迹，通过模拟车辆的未来运动，判断在换道过程中是否会发生碰撞，碰撞检测的判别公式为：
[0062]
[0063] 式中，robj、rego分别是目标车和主车的膨胀半径，ξ是安全系数，跟主车的速度和碰撞检测的时刻有关，速度越快，安全系数越大，采样时刻越大，轨迹预测的不确定度越大，安
全系数越大；安全系数的表达式为：
[0064]
[0065] 式中，v是主车的速度，单位为km/h，i是碰撞检测的时刻，单位为s。
[0066] 本发明的有益效果在于：
[0067] 本发明采用采样拟合的方法生成候选换道轨迹集，通过代价函数评估的方法确定最佳换道轨迹，相比于确定性的换道轨迹模型，具有更好的适应性；考虑高速换道过程中周
围车辆的运动不确定性，通过碰撞检测的方式保证高速换道的安全性；使车辆以最佳轨迹
换道的同时最大程度保证高速换道的安全性。
附图说明
[0068] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本
发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据
提供的附图获得其他的附图。
[0069] 图1附图为本发明的方法流程图；
[0070] 图2附图为本发明的碰撞检测示意图。
具体实施方式
[0071] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于
本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他
实施例，都属于本发明保护的范围。
[0072] 请参阅附图1，本发明提出的一种基于碰撞检测的智能车高速换道规划方法，包括：
[0073] 步骤1、生成换道轨迹：
[0074] 对目标车道的状态空间进行采样得到换道结束点的终端状态，利用五次多项式对换道起始点和结束点之间的换道轨迹进行拟合，形成换道候选轨迹集：
[0075] Sset＝Ψ(T,S0,Sf)
[0076] 式中，T表示换道时间，S0表示主车换道初始状态，Sf表示终端状态，Ψ(t,S0,Sf)表示一条从初始状态S0到终端状态Sf的换道轨迹。
[0077] 换道轨迹利用五次多项式对起始点、结束点两点进行拟合得到：
[0078]
[0079] 式中，x表示纵向位置坐标，y表示横向位置坐标，cx0、cx1、cx2、cx3、cx4、cx5和cy0、cy1、cy2、cy3、cy4、cy5为待求系数。
[0080] 将初始点纵向状态 和结束点纵向状态 作为边界条件带入纵向轨迹可以得到：
[0081]
[0082]
[0083] 换道时间为T使得下式成立：
[0084] tf＝t0+T
[0085] 可以求解得到多项式系数为：
[0086]
[0087]
[0088] 将初始点横向状态 和结束点横向状态 作为边界条件带入横向轨迹可以得到：
[0089]
[0090]
[0091] 换道时间为T使得下式成立：
[0092] tf＝t0+T
[0093] 可以求解得到多项式系数为：
[0094]
[0095]
[0096] 选取不同的换道结束点终端状态和换道时间可以得到一个换道轨迹的集合，采样点根据主车的运动状态确定。
[0097] 根据车辆当前运动状态确定换道结束点的纵、横向终端状态。其中纵向终端状态满足：
[0098]
[0099] 式中，Tc是避碰时间。
[0100] 横向终端状态满足：
[0101]
[0102] 式中，L是车道宽度，Δ是单位横向采样距离，k是采样数目，取决于换道初始速度，初始速度越大采样数目k越大。
[0103] 利用代价评估函数对换道轨迹集中的候选换道轨迹进行评估，选取评估得分最低的轨迹作为换道轨迹，代价评估函数为：
[0104] Ctotal＝kxCx+kyCy
[0105] 式中，kx、ky是权重系数，Cx、Cy分别表示横向、纵向代价函数，表达式分别为：
[0106]
[0107] 式中，ki、kv是权重系数，vdes是换道过程中的期望速度。
[0108]
[0109] 式中，kj、kl是权重系数，ldes是换道过程中的期望侧向位移。
[0110] 步骤2、目标车辆轨迹预测：
[0111] 利用车载传感器获取目标车道内目标车辆的历史运动状态信息，包括位置、速度、加速度、航向，然后将历史运动信息从笛卡尔坐标系映射到Frenet坐标系下，映射的公式
为：
[0112]
[0113] 式中，是Frenet坐标系下纵向坐标对时间的导数，vx是笛卡尔坐标系下的速度，θ是笛卡尔坐标系下航向角θx和匹配点航向θr的差值，即θ＝θx‑θr，κr是参考点的曲率。
[0114]
[0115] 式中，ax是笛卡尔坐标系下的加速度，l′是l对纵向坐标s的导数，l′＝(1‑κrl)tan(θ)。
[0116]
[0117] 式中，l″是l对纵向坐标s的二阶导数，κx是笛卡尔坐标系下位置点的曲率。
[0118] 每间隔时间 记录一次映射后的历史运动状态信息，以n为一个时间域值,将时间域n内记录的历史数据打包成一个向量输入到轨迹预测模块，轨迹预测模块是基于多层神
经网络的编解码结构，利用编码器对目标车辆的历史运动信息进行编码，解码器输出预测
的目标车辆在Frenet坐标系下的轨迹，将轨迹映射回笛卡尔坐标系，映射公式为：
[0119] x＝r(s)+nrl
[0120] 式中，x是笛卡尔坐标系下位置向量，x＝(x,y)，r(s)是Frenet坐标系下参考点位置向量，nr是参考点处的参考法向量，l是沿着法向方向的偏移。
[0121] 最终得到目标车辆的预测轨迹为：
[0122]
[0123] 式中， 是在换道开始的t0时刻车辆笛卡尔坐标下的预测位置坐标，mp是预测时域的长度。
[0124] 步骤3、基于碰撞检测的换道决策：
[0125] 换道规划模块规划出一条换道轨迹，以时间间隔p对换道轨迹进行采样，得到离散的换道轨迹点，其表达式为：
[0126]
[0127] 目标车辆轨迹预测模块预测目标车辆未来的运动轨迹，通过模拟车辆的未来运动，判断在换道过程中是否会发生碰撞，参照图2，碰撞检测的判别公式为：
[0128]
[0129] 式中，robj、rego分别是目标车和主车的膨胀半径，ξ是安全系数，跟主车的速度和碰撞检测的时刻有关，速度越快，安全系数越大，采样时刻越大，轨迹预测的不确定度越大，安
全系数越大。安全系数的表达式为：
[0130]
[0131] 式中，v是主车的速度，单位为km/h，i是碰撞检测的时刻，单位为s。对换道时间T内的主车规划轨迹和目标车辆的预测轨迹进行碰撞检测，如果碰撞检测通过，则按照规划好
的换道轨迹进行换道。如果碰撞检测不通过则重新开始规划换道轨迹，等待碰撞检测通过
后进行换道。在图2中，robj、rego分别是目标车和主车的膨胀半径，t0,…t0+np是碰撞检测时
刻，通过检测每一个时刻目标车和主车之间的位置关系判断是否满足碰撞检测公式，图2中
目标车和主车将在t0+4p时刻发生碰撞。
[0132] 本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置
而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说
明即可。
[0133] 对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的
一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明
将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一
致的最宽的范围。
