[发明专利]一种基于局部最优性的无人驾驶车辆动态轨迹规划方法

权利要求书

1.一种基于局部最优性的无人驾驶车辆动态轨迹规划方法，其特征在于，包括如下步骤：

A1、分别获取无人车和障碍车的状态参数，根据所述状态参数判断无人车是否达到产生换道动机的条件，若达到产生换道动机的条件，则继续判断是否满足换道条件，若满足换道条件，则执行步骤A2；若达不到产生换道动机的条件，或不满足换道条件，则无人车减速或保持当前跟驰状态；

A2、根据所述无人车的状态参数和预设的无人车左侧待换道车道无障碍车辆的距离范围，并结合无人车的动力性能指标，得到无人车的局部最优换道轨迹，作为参考轨迹；

A3、根据所述障碍车的状态参数，结合汽车动力学响应，预测无人车左侧待换道车道障碍车的行驶轨迹；

A4、根据所述无人车的状态参数和无人车左侧待换道车道障碍车的行驶轨迹，预测无人车的换道行驶轨迹；

A5、基于所述参考轨迹，结合车辆动力学约束和成本代价函数值最小的原则，利用非线性模型预测控制对所述无人车的换道行驶轨迹进行重规划，得到最优换道轨迹；

A6、将步骤A3-A5不断进行迭代计算，得到最终换道轨迹；

所述状态参数包括无人车和障碍车的位置、运动方向、行驶速度和加速度；

所述产生换道动机的条件满足以下公式：

式中：D为无人车与正前方障碍车的距离；Diss(k)为无人车当前采样时刻k的速度不满累积度值，其中，Diss(k-1)为无人车上一时刻k-1的速度不满累计度值，v_d为当前采样时刻正前方障碍车的速度，v_p为当前采样时刻对正前方障碍车的期望速度值，T为采样间隔；Diss_p为不满累积度阈值；

C为无人车；

C_f为无人车正前方的障碍车；

C_lf为无人车左前方的障碍车；

C_lr为无人车左后方的障碍车；

MSD(C,C_f)为无人车与正前方的障碍车的最小安全距离；

所述换道条件满足以下公式：

式中：D_sf、D_sb分别为无人车与左前方、左后方障碍车的车距；

所述步骤A2中，所述参考轨迹(X(t),Y(t))满足以下公式：

式中：X(t)、Y(t)为以无人车在坐标系下的纵向坐标和侧向坐标；a_i、b_i为五次多项式的系数；t为时间；i为系数变量，取值为1、2...、n，n＝5，其中：

式中：t_s为无人车换道初始时刻；t_f为无人车换道终止时刻；X(t_s)、和分别为无人车在换道初始时刻纵向的位移、速度和加速度；X(t_f)、和分别为无人车在换道终止时刻纵向的位移、速度和加速度；Y(t_s)、和分别为无人车在换道初始时刻侧向的位移、速度和加速度；Y(t_f)、和分别为无人车在换道终止时刻侧向的位移、速度和加速度；

所述步骤A3中，障碍车的行驶轨迹满足以下公式：

式中：(x_k、y_k)为采样时刻障碍车的位置坐标；(x₀、y₀)为初始时刻障碍车的位置坐标；v_0,x、v_0,y分别为初始时刻障碍车的纵向速度和侧向速度；Δt为规划周期；

所述步骤A4中，无人车的换道行驶轨迹满足以下公式：

式中：(X_k、Y_k)为采样时刻无人车的位置坐标，即无人车预估状态轨迹点位置坐标；θ_k为采样时刻无人车的车辆航向角；(X₀、Y₀)为初始时刻无人车的位置坐标；θ₀为初始时刻无人车的车辆航向角；δ为无人车的车辆前轮转角；u为无人车的纵向速度；L为无人车的车辆轴距；K为稳定性系数，K＝m(a/k₁-b/k₂)/L²，其中m为无人车整备质量，a、b分别为无人车的前后轴距，k₁、k₂分别为无人车前后轮胎的外倾刚度；

所述步骤A5中，成本代价函数J_obs,c由惩罚函数C_total和速度距离避障函数J_obs,i共同决定，成本代价函数满足如下公式：

式中：J_obs,i为工况下所有障碍车辆的速度距离避障功能函数值，i为周围障碍车辆，i的取值为1、2...n，n＝4；C_total为惩罚函数；

成本代价函数J_obs,c值越小，换道轨迹越安全，无人车的舒适性越高；

所述速度距离避障函数J_obs,i满足如下公式：

式中：J_obs,f为无人车与目标车道前方障碍车的避障功能函数值；J_obs,r为无人车与目标车道后方障碍车的避障功能函数值；S_obs为权重系数，权重系数越大则轨迹规划结果就越趋于保守；v_c,x、v_c,y分别为无人车的纵向、侧向速度；v_o,x、v_o,y分别为障碍车的纵向、侧向速度；v_c、v_o分别为无人车和障碍车的速度；(X_k,Y_k)、(x_k,y_k)分别为无人车、障碍车的位置坐标；ζ为一个正数，防止发生无解的情况。