[发明专利]基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法

权利要求书

1.一种基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法，其特征在于：其步骤是：首先根据对轨迹规划的需求，建立全局坐标系下的车辆运动学模型；然后，引入轨迹规划问题的代价函数和约束，包括了构建具有防止碰撞功能代价函数、状态偏差跟踪代价函数以及保证驾驶舒适性、安全性约束的避撞换道轨迹规划层：

S1.建立全局坐标系下的车辆的运动学模型

简化汽车运动，建立全局坐标系下的车辆的运动学模型，忽略掉了车辆在垂直方向的运动，给出其二维平面上的运动学方程如下：

其中，a_y代表车辆的侧向加速度，a_x代表车辆纵向加速度，代表车辆的航向角，v_y代表车辆的侧向速度，v_x代表车辆的纵向速度，X、Y代表车辆在全局坐标系下的位置坐标；

系统模型可以简化为：

以为车辆的状态量，以u(t)＝[a_y a_x]^T为控制量；

S2、建立带有避免换道过程碰撞代价函数的轨迹规划方法

代价函数设计中的位置状态偏差的跟踪代价函数J₁，其表达形式如下：

其中，N_P代表预测步长；W_P代表系统状态的权重；Y代表换道轨迹中的自车实际侧向位置坐标；Y_ref代表目标车道中心线侧向位置坐标，当前道路单车道宽为W；

代价函数设计中的防止障碍车辆与自车碰撞的避撞代价函数J₂，其表达形式如下：

其中，X、Y代表自车全局坐标系下的纵、横向位置坐标；N_obs代表障碍物坐标点的数量；X_obsk、Y_obsk表第k个障碍物点在全局坐标系下的纵、横向位置坐标；W_R表示障碍物与自车碰撞威胁的权重；ε代表一个很小的标量；

设计了如下的控制问题代价函数J₃：

其中，N_C代表控制步长；_u代表当前时刻的待优化控制量；W_C代表控制量的权重；

车辆的乘坐舒适性和驾驶的安全性约束如下：

u_min≤u(t+k)≤u_max,k＝0,1,…,N_C-1 (6)

控制量u_min＝[-μg,-μg]^T；u_max＝[μg,μg]^T，μ为路面的附着系数；

综合代价函数和约束条件：

s.t.u_min≤u(t+k)≤u_max,k＝1,2,…,N_C-1

根据下式求解当前控制量下的状态量，就获得所需的规划的轨迹信息：

ξ_k+1＝ξ_k+Δtf(ξ_k,u_k) (8)

2.根据权利要求1所述的基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法，其特征在于：将以上轨迹规划方案进行改进，扩展至混合路况，针对这种特殊的偶发性混合驾驶路况，在模型预测控制的框架下，构造了带有应急功能的MPC轨迹规划层：

建立带有紧急工况规划的代价函数J_∑作为常规规划代价函数和紧急规划代价函数的代价综合：

J_∑＝min(J_normal+J_urgency) (9)

各自的轨迹受制于自己的代价函数，常规工况规划代价函数J_normal和紧急工况规划代价函数J_normal，并保持各自的代价函数的独立性；

优化问题的约束如下

受等式约束：

受不等式约束：

3.根据权利要求1所述的基于双层MPC的智能驾驶汽车轨迹规划与跟踪控制方法，其特征在于：根据研究车辆的转向性质和稳定性的关系，建立考虑车辆横摆和滑移的动力学模型；然后，对建立的模型进行线性化和离散化，给出保证车辆操纵稳定性的道路环境约束、车辆执行机构约束以及稳定行驶约束，利用模型预测控制算法能够处理约束的能力，设计基于代价函数和约束的轨迹跟踪控制层：

S1、根据研究车辆的转向性质和稳定性的关系，建立考虑车辆横摆和滑移的车辆动力学模型如下：

其中，m代表车辆的质量，v_x、v_y分别代表车身坐标系下车辆质心处的纵向速度、侧向速度，代表车辆的横摆角，代表车辆的横摆角速度，δ_f代表车辆的前轮转向角，I_z代表车辆的转动惯量，l_f和l_r分别代表车辆质心到前、后轴的距离，F_yf和F_yr分别代表车辆前、后轴上的轮胎侧向力的合力，F_xf和F_xr分别代表车辆前、后轴上的轮胎纵向力的合力；

假设忽略耦合关系并且忽略掉前轮纵向驱动力对对车辆横摆运动作用，将上式简化为：

运动控制中智能驾驶车辆的相对全局坐标表达式如下：

其中X、Y分别代表车辆在相对全局坐标系下的纵、横向位置坐标；

将轮胎力等价为轮胎侧偏角的线性函数表达式：

F＝C_αα (16)

其中，C_α代表轮胎线性区的侧偏刚度，α代表轮胎侧偏角；

得到的智能驾驶汽车前轮系统的轮胎侧向力表达式如下：

其中，C_αf和C_αr分别代表前、后轮胎的侧偏刚度，β为质心侧偏角，一般可以

同样车辆的纵向力表达式为：

F_xf＝-C_lfλ_f

F_xr＝-C_lrλ_r

(18)

其中，C_lf和C_lr分别代表前、后轮胎的纵向侧偏刚度，λ_f和λ_r分别代表前、后轮胎的滑移率；S2、线性化与离散化

综合上式(13)-(18)，得到车辆在全局坐标系下的非线性模型表示为：

该车辆的连续系统状态方程表示如下：

其中，状态量为控制量为u＝[δ_f]^T，对上式进行离散化处理，得到离散的状态空间方程：

其中，A(k)＝I+TA(t)，B(k)＝TB(t)；

S3、安全驾驶约束

车速约束方式如下：

v_min≤v≤v_max (22)

其中，v_min＝0和v_max＝1.1*v_ref分别代表当前驾驶道路最小、最大的驾驶速度，v_ref代表期望的车辆驾驶速度；

执行器的执行区间的约束：

其中，δ_fmax代表车辆的最大前轮转向角，也就是车辆转向执行机构的最大约束量，Δδ_fmax代表每一个控制步最大的前轮转向角变化量；

侧向加速度约束：

|a_y|≤μg (24)

车辆的横摆角速度限制范围：

轮胎的侧偏角限制：

|α|≤5° (26)

S4、跟踪控制器

轨迹跟踪控制器在每个控制周期进行优化

其中，为轨迹规划层给出的参考轨迹，N_P代表预测步长；N_C代表控制步长；ε代表松弛变量；W_P代表换道期望轨迹跟踪能力的加权；W_R代表控制输入的加权；W_S代表松弛变量的加权。