CN113961002A - 一种基于结构化道路采样的主动换道规划方法 - Google Patents

Abstract

本发明公开了一种一种基于结构化道路采样的主动换道规划方法，包括以下步骤：步骤S1：对换道场景的道路进行空间采样处理；步骤S2：对采样点进行横纵向多项式换道轨迹生成求解；步骤S3：对计算出的若干轨迹进行评价函数计算，筛选最优轨迹。本发明构建了车辆运动学模型，并在车辆运动学模型的约束下，生成换道轨迹，很好的解决了传统方法的换道轨迹不符合车辆运动状态的问题；通过对结构化道路的状态空间采样，生成多组变道轨迹，并引入评价函数，求出最优换道轨迹。

Description

一种基于结构化道路采样的主动换道规划方法

技术领域

本发明涉及一种换道规划方法，尤其是涉及一种基于结构化道路采样的主动换道规划方法。

背景技术

本发明对结构化道路进行状态空间采样处理，通过车辆运动学约束，对采样点进行多项式曲线生成，加入评价函数，对生成的多组换道轨迹进行打分计算，通过不同的权重设定，最终筛选出一组最优变道轨迹，可指导辅助驾驶车辆进行安全无碰撞的变道行驶，并提高了乘车的舒适性。

现有的主动换道规划方法是根据当前车辆位置点和换道到达位置点进行多段几何曲线生成，并根据雷达识别到的障碍物信息和生成的几何曲线进行碰撞检测，无碰撞风险时，车辆可按曲线换道行驶。

现有技术具有以下缺点：现有技术主动换道的规划轨迹是根据传统几何方法推算出的，没有考虑车辆运动学的约束，因此现有技术中主动换道的规划轨迹不是最符合车辆行驶的。

现有技术主动换道的规划轨迹没有综合考虑碰撞风险、行车舒适性和轨迹平滑性等因素，因此现有技术主动换道的规划轨迹不是最优的。

例如，一种在中国专利文献上公开的“一种汽车换道避撞控制方法”，其公告号CN105857294B，包括没有综合考虑碰撞风险、行车舒适性和轨迹平滑性等因素，无法达到最优轨迹规划的问题。

发明内容

本发明是为了克服现有技术中没有综合考虑考虑碰撞风险、行车舒适性和平滑轨迹性等因素，无法达到最优主动换道的问题，提出一种基于结构化道路采样的主动换道规划方法。

为了实现上述目的，本发明采用以下技术方案：一种基于结构化道路采样的主动换道规划方法，其特征是，包括以下步骤：

步骤S1：对换道场景的道路进行空间采样处理；

步骤S2：对采样点进行横纵向多项式换道轨迹生成求解；

步骤S3：对计算出的若干轨迹进行评价函数计算，筛选最优轨迹。

本发明构建了车辆运动学模型，并在车辆运动学模型的约束下，生成换道轨迹，很好的解决了传统方法的换道轨迹不符合车辆运动状态的问题。

作为优选，步骤S1包括以下步骤：步骤S11：对相邻车道线的5次多项式系数进行求和取均值，得到中间车道线的多项式方程；步骤S12：根据得到的参考线，按固定间隔，均匀布置采样点；对采样点进行横纵向多项式轨迹生成求解，并对计算出的若干轨迹进行评价函数计算，筛选出最优轨迹，解决了传统技术方案生成的轨迹单一，不是最优解的问题。

作为优选，中间车道的多项式方程的内容：

其中，leftcoeffs为左道线系数，rightcoeffs为右道线系数，通过对中间车道线的计算，较为精确地获取复杂路况的轨迹数据，并对计算出的若干轨迹进行评价函数计算，生成大量方案数据进行比对，获取道路主动换道的最优解。

作为优选，步骤S12包括以下内容：采样点的纵向距离间隔为 max[4s*Vc_urrentSpeed,Vs_{toppingDistance}]/5，其中，Vc_urrentSpeed为当前车速，4s*Vc_urrentSpeed即为当前车速下在4s内的行车距离；Vs_{toppingDistance}为当前时速的最大制动距离；采样点横向距离间隔为 (load_width-vehicle_width)/2；其中load_width为相邻车道的路宽，vehicle_width为车宽，针对不同的实际车况路况获取不同的运动轨迹曲线，使得主动换道的规划轨迹更符合车辆的实时行驶状况。

作为优选，步骤S2包括以下步骤：步骤S21：根据上述步骤得到的采样点，构建车辆运动学模型；步骤S22：根据采样的轨迹点，采用多项式螺旋线来连接轨迹点，生成轨迹曲线；换道轨迹生成主要用于实现辅助驾驶车辆的主动换道功能，根据上节得到的采样点，对不同采样点分别求解在车辆运动模型约束下的多项式曲线方程，使获得的曲线更接近最优路线。

作为优选，车辆运动学模型的内容为：设定车辆姿态向量X＝(x,y,θ,k,v)，其中x,y 是二维平面位置，θ是车辆朝向，k表示曲率，v是车辆线速度，其标量大小满足以下关系：

为后续计算提供数据支持，保证路线规划数据的准确性和实时性。

作为优选，将轨迹曲线定义为三次多项式螺旋，轨迹的曲率k是弧长s的三次多项式函数，代入参数向量p＝[p₀ p₁ p₂ p₃ s_f]，其中，s_f是边界约束之间的曲线的弧长，参数代入实际数据，保证数据准确性，使得主动换道的规划轨迹更符合车辆的实时行驶状况。

作为优选，评价函数是多种评价函数项的线性组合，将步骤S2中的轨迹离散为n+1个点，评价函数包括以下内容：

偏离车道cost函数f_offset:

其中，ΔS为离散点与相应在参考线上的映射点的偏差平方；

平滑性cos函数f_smooth:

其中，k为曲率；

舒适性cost函数f_jeck:

其中，J为加加速度；

碰撞cost函数f_collision：f_collision＝1/min(||P_i-P_obs||)，(i∈0～n)，其中，P_obs为障碍物的坐标向量，P为离散轨迹的坐标向量；

向心加速度cost函数f_collision：f_collision＝1/min(||P_i-P_obs||)，(i∈0～n)，其中，P_obs为障碍物的坐标向量，P为离散轨迹的坐标向量；

合成cost：f＝w_offset*f_offset+w_smooth*f_smooth+w_jeck*f_jeck+w_collision*f_collision+w_centripetal*f_centripetal，其中，w_offset、w_smooth、w_jeck、w_collision、w_centripetal是各函数自调节的权重系数，根据可调节的权重系数设计，最终完成对轨迹的cost函数计算处理；对计算得出的数据做综合处理，使得最终获得的换道轨迹能综合考虑到碰撞风险、行车舒适性和轨迹平滑性等因素，使得***规划的换道接近最优解。

因此，本发明具有以下有益效果：

构建了车辆运动学模型，并在车辆运动学模型的约束下，生成换道轨迹，很好的解决了传统方法的换道轨迹不符合车辆运动状态的问题；

通过对结构化道路的状态空间采样，生成多组变道轨迹，并引入评价函数，求出最优换道轨迹。

附图说明

图1是车道采点示意图；

图2是车辆运动轨迹曲线；

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

一种基于结构化道路采样的主动换道规划方法，主要功能:

1.解决传统技术换道规划的轨迹没有考虑车辆运动学的问题，本发明构建了车辆运动学模型，在模型约束下，进行规划轨迹的生成。

2.对换道场景的道路进行空间采样处理，生成若干采样点，再对采样点进行横纵向多项式轨迹生成求解，并对计算出的若干轨迹进行评价函数计算，筛选出最优轨迹，解决了传统的技术方案生成的轨迹单一，不是最优解的问题。

主要组件:状态空间采样、换道轨迹生成、速度轨迹生成、评价函数计算:

状态空间采样:

状态空间采样依赖于参考线，参考线是车辆行驶的参考轨迹，在没有导航地图的全局规划轨迹的情况下，可通过根据转向灯状态，选取左侧或右侧的相邻车道线，并对选取的车道线进行合并处理，来求得中间车道线，最终以此中间车道线做为参考线使用。

具体实施方法是：

1、对相邻车道线的5次多项式系数进行求和取均值，得到中间车道线的多项式方程:

其中，leftcoeffs为左车道线系数，rightcoeffs为右边车道线系数；

2、根据得到的参考线,按固定间隔,均匀布置采样点；

具体方法为：

1)采样点纵向距离间隔为max[4s*Vc_urrentSpeed,Vs_{toppingDistance}]/5；

其中，Vc_urrentSpeed为当前车速，4s*Vc_urrentSpeed即为当前车速下在4s内的行车距离；Vs_{toppingDistance}为当前时速的最大制动距离；

采样点横向距离间隔为(load_width-vehicle_width)/2；其中load_width为相邻车道的路宽， vehicle_width为车宽，如图1所示，其中，圆点为采样点，两侧黑色实线为车道线，中间绿色为参考线；

换道轨迹生成：

换道轨迹生成主要用于实现辅助驾驶车辆的主动换道功能，根据上节得到的采样点，对不同采样点分别求解在车辆运动模型约束下的多项式曲线方程。

具体实施方法是：

1)构建车辆运动学模型，设定车辆姿态向量X＝(x,y,θ,k,v)，其中x,y是二维平面的位置，θ是车辆朝向，k表示曲率，v是车辆线速度，其标量大小满足以下：

2)有了采样的轨迹点，采用多项式螺旋线来连接轨迹点，生成轨迹曲线。将轨迹定义为三次多项式螺旋，即轨迹的曲率k是弧长s的三次多项式函数，带入参数向量p＝[p₀ p₁p₂ p₃ s_f]，其中，s_f是边界约束之间的曲线的弧长，经过以下推导，得到多项式方程。

k(s)＝a+bs+cs²+ds³

k(s)＝a(p)+b(p)s+c(p)s²+d(p)s³

k(0)＝p₀

k(s_f/3)＝p₁

k(2s_f/3)＝p₂

k(s_f)＝p₃

a(p)＝p₀

b(p)＝(-11p₀+18p₁-9p₂+2p₃)/2s_f

3)上式中，p₀已知，只需求解[p₁ p₂ p₃ s_f]，根据车辆运动学模型，经过以下推导，得到多项式方程。

dx_P/ds＝cos[θ_p(s)]

dy_P/ds＝sin[θ_p(s)]

dθ_P/ds＝k_p(s)

x_p(s)＝∫₀ ^scos[θ_p(s)]ds

y_p(s)＝∫₀ ^ssin[θ_p(s)]ds

θ_p(s)＝a(p)s+b(p)s²/2+c(p)s³/3+d(p)s⁴/4

k_p(s)＝a(p)+b(p)s+c(p)s²+d(p)s³

4)设置轨迹的终点是[x_p(s_f)y_p(s_f) θ_p(s_f)k_p(s_f)]，需要找到使其等于x_des的参数。通过相对于参数向量p＝[p₁ p₂ p₃ s_f]评估端点状态向量的雅可比行列式x_p(s_f)＝[x_p(s_f)y_p(s_f) θ(s_f)k_p(s_f)]，使用牛顿法为p生成一系列估计{p_i}，使x等于x_des，该迭代持续进行，直到认为Δx足够小，或达到最大迭代次数，最终生成所有轨迹。

Δx＝(x_des-x_pi(s_f))

Δp＝J_pi(x_pi(s_f))^-1Δx

p_i+1＝p_i+Δp

生成曲线为如图2所示，其中最左侧圆点为车辆当前位置点，曲线为求解出的多项式曲线；速度轨迹生成：速度轨迹生成主要用于实现辅助驾驶车辆主动换道的定速巡航、超车和跟车功能。

具体方法为：

1)在定速巡航条件下，构建纵向V/T(速度/时间)采样图，T轴设置8个采样时刻，每个采样时刻间隔0.5秒，V轴设置4个采样速度，每个采样速度的间隔为(V_TargetSpeed-V_CurrentSpeed) /4；

其中，V_TargetSpeed为巡航车速，并且已知当前车辆位置点的位置s_start、速度v_start、加速度a_start和4S位置点的速度v_end、加速度a_end五个变量，可求解下列速度曲线多项式的系数解coeffs；

2)在有障碍物的情况下，需要进行超车或跟车的速度采样，处理方法和上诉第一点一致，其中，V_TargetSpeed为巡航车速/跟车车辆的速度，并且已知当前车辆位置点的位置s_start、速度v_start、加速度a_start和4S位置点的位置s_end、速度v_end、加速度a_end六个变量，可求解下列速度曲线多项式的系数解coeffs；

评价函数处计算：

为了筛选出最优轨迹,本发明设计了一种评价函数的方法,该评价函数是多种评价函数项的线性组合；

将轨迹离散为n+1个点，评价函数主要包含以下几点：

1)偏离车道cost函数f_offset：

其中，ΔS为离散点与相应在参考线上的映射点的偏差平方；

2)平滑性cost函数f_smooth:

其中，k为曲率；

3)舒适性cost函数f_jeck:

其中，J为加加速度；

4)碰撞cost函数f_collision：

f_collision＝1/min(||P_i-P_obs||)，(i∈0～n)

其中，P_obs为障碍物的坐标向量，P为离散轨迹的坐标向量；

5)向心加速度cost函数f_centripetal：

其中，a为向心加速度；

6)合成cost：

f＝w_offset*f_offset+w_smooth*f_smooth+w_jeck*f_jeck+w_collision*f_collision+w_centripetal*f_centripetal

其中，w_offset、w_smooth、w_jeck、w_collision、w_centripetal是各函数自调节的权重系数，根据可调节的权重系数设计，最终完成对轨迹的cost函数计算处理。

Claims (9)

1.一种基于结构化道路采样的主动换道规划方法，其特征是，包括以下步骤：

步骤S1：对换道场景的道路进行空间采样处理；

步骤S2：对采样点进行横纵向多项式换道轨迹生成求解；

步骤S3：对计算出的若干轨迹进行评价函数计算，筛选最优轨迹。

2.根据权利要求1所述的一种基于结构化道路采样的主动换道规划方法，其特征是，步骤S1包括以下步骤：

步骤S11：对相邻车道线的5次多项式系数进行求和取均值，得到中间车道线的多项式方程；

步骤S12：根据得到的参考线，按固定间隔，均匀布置采样点。

3.根据权利要求2所述的一种基于结构化道路采样的主动换道规划方法，其特征是，中间车道的多项式方程的内容：

其中，leftcoeffs为左道线系数，rightcoeffs为右道线系数。

4.根据权利要求2所述的一种基于结构化道路采样的主动换道规划方法，其特征是，步骤S12包括以下内容：采样点的纵向距离间隔为max[4s*Vc_urrentSpeed,Vs_{toppingDistance}]/5，其中，Vc_urrentSpeed为当前车速，4s*Vc_urrentSpeed即为当前车速下在4s内的行车距离；Vs_{toppingDistance}为当前时速的最大制动距离；采样点横向距离间隔为(load_width-vehicle_width)/2；其中load_width为相邻车道的路宽，vehicle_width为车宽。

5.根据权利要求1所述的一种基于结构化道路采样的主动换道规划方法，其特征是，步骤S2包括以下步骤：

步骤S21：根据上述步骤得到的采样点，构建车辆运动学模型；

步骤S22：根据采样的轨迹点，采用多项式螺旋线来连接轨迹点，生成轨迹曲线。

6.根据权利要求5所述的一种基于结构化道路采样的主动换道规划方法，其特征是，车辆运动学模型的内容为：设定车辆姿态向量X＝(x,y,θ,k,v)，其中x,y是二维平面位置，θ是车辆朝向，k表示曲率，v是车辆线速度，其标量大小满足以下关系：

7.根据权利要求5所述的一种基于结构化道路采样的主动换道规划方法，其特征是，将轨迹曲线定义为三次多项式螺旋，轨迹的曲率k是弧长s的三次多项式函数，代入参数向量p＝[p₀ p₁ p₂ p₃ s_f]，其中，s_f是边界约束之间的曲线的弧长。

8.根据权利要求1所述的一种基于结构化道路采样的主动换道规划方法，其特征是，步骤S3 中评价函数的计算：

9.根据权利要求8所述的一种基于结构化道路采样的主动换道规划方法，其特征是，评价函数是多种评价函数项的线性组合，将步骤S2中的轨迹离散为n+1个点，评价函数包括以下内容：

偏离车道cost函数f_offset：

其中，ΔS为离散点与相应在参考线上的映射点的偏差平方；

平滑性cos函数f_smooth：

其中，k为曲率；

舒适性cost函数f_jeck：

其中，J为加加速度；

碰撞cost函数f_collision：f_collision＝1/min(||P_i-P_obs||)，(i∈0～n)其中，P_obs为障碍物的坐标向量，P为离散轨迹的坐标向量；

向心加速度cost函数f_collision：f_collision＝1/min(||P_i-P_obs||)，(i∈０～n)其中，P_obs为障碍物的坐标向量，P为离散轨迹的坐标向量；

合成cost：f＝w_offset*f_offset+w_smooth*f_smooth+w_jeck*f_jeck+w_collision*f_collision+w_centripetal*f_centripetal，其中，w_offset、w_smooth、w_jeck、w_collision、w_centripetal是各函数自调节的权重系数，根据可调节的权重系数设计，最终完成对轨迹的cost函数计算处理。

Images