[发明专利]一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法

权利要求书

1.一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法，其特征在于，所述方法包括如下步骤：

1)、考虑自主驾驶车辆离散时间动力学模型，参见式(1)：

其中，k为采样时间，T_s为采样周期，X、Y、分别为车辆在全局坐标系下的纵向位置、横向位置、横摆角，v_x、v_y、r分别为车辆在质心处的纵向速度、横向速度、横摆角速度，m为车辆质量，I_z为车辆绕其垂直轴的转动惯量，d_f和d_r分别是车辆质心到前、后轴的距离，δ_f和a_x分别为前轮转向角和纵向加速度，C_cf和C_cr分别表示前、后轮的侧偏刚度，定义车辆行驶环境势场函数，参见式(2)：

其中，U_A为自主驾驶车辆总势场，常数e为自然指数，A_l为分道线势场强度，σ_l为分道线势场收敛幅度，y_l,i为第i条道路分道线的横向位置，A_r为道路边界势场强度，y_r,i为第i条道路边界的横向位置，X_goal为目标点纵向位置，组合参数ε和β为正整数，D是自主驾驶车辆与障碍车辆之间的横向距离，σ_o为横向势场收敛幅度，U_o为障碍车辆纵向势场函数，参见式(3)：

其中，A_c,1和A_c,2分别为自主驾驶车辆在区域S₁和S₂内的纵向势场强度，K为自主驾驶车辆与障碍车辆之间的纵向距离，ρ为自主驾驶车辆在区域S₁内的纵向势场收敛幅度，符号“∈”表示“属于”，区域S₁定义为自主驾驶车辆距离障碍车辆在纵向安全距离以外区域，区域S₂定义为自主驾驶车辆距离障碍车辆在纵向安全距离以内区域和等于纵向安全距离区域；

2)、定义状态列向量和控制列向量u＝[δ_f,a_x]^T，其中，符号“T”表示“向量转置”，将状态列向量和控制列向量代入式(1)，得到状态方程，参见式(4)：

x(k+1)＝x(k)+T_sf(x(k),u(k)) (4)

其中，f(x(k),u(k))为函数列向量，参见式(5)：

3)、考虑模型式(4)，建立自主驾驶车辆的动态预测模型，参见式(6)：

x(i+1|k)＝x(i|k)+T_sf(x(i|k),u(i|k)),i＝0,1,...,N-1 (6)

其中，x(i|k)为自主驾驶车辆在时刻k对未来时刻k+i的预测状态列向量，u(i|k)为自主驾驶车辆在时刻k对未来时刻k+i的预测控制列向量，正整数N为预测时间窗口；

4)、考虑自主驾驶车辆的动态预测模型式(6)，分别定义表征车辆操纵稳定性的车辆前轮侧偏角α_f和后轮侧偏角α_r，分别参见式(7)和(8)：

α_f(i|k)＝δ_f(i|k)-[v_y(i|k)+a_x(i|k)r(i|k)]/v_x(i|k) (7)

α_r(i|k)＝[d_rr(i|k)-v_y(i|k)]/v_x(i|k) (8)

再分别定义控制列向量约束、控制增量约束、前轮侧偏角α_f和后轮侧偏角α_r的边界约束，分别参见式(9)至(12)：

u_min≤u(i|k)≤u_max,i＝0,1,...,N-1 (9)

Δu_min≤Δu(i|k)≤Δu_max,i＝0,1,...,N-1 (10)

α_fmin≤α_f(i|k)≤α_fmax,i＝0,1,...,N-1 (11)

α_rmin≤α_r(i|k)≤α_rmax,i＝0,1,...,N-1 (12)

其中，Δu(i|k)＝u(i|k)-u(i-1|k)为相邻预测时刻k+i和k+i-1的预测控制增量，u_min、Δu_min、α_fmin、α_rmin分别为控制列向量、控制增量、前轮侧偏角、后轮侧偏角的约束下界，u_max、Δu_max、α_fmax、α_rmax分别为控制列向量、控制增量、前轮侧偏角、后轮侧偏角的约束上界；

5)、考虑自主驾驶车辆的动态预测模型式(6)，分别定义避障目标函数J₁和跟踪目标函数J₂，参见式(13)和(14)：

其中，x_goal为车辆轨迹规划目标点，v_x,goal为车辆轨迹规划的纵向目标速度，矩阵S和W都为正定的加权矩阵，系数Q和R都为正的加权系数，||·||为2范数；

6)、自主驾驶车辆避障轨迹规划与跟踪控制首先要保证车辆行车安全性，再尽可能恒速跟踪行驶，因此，检测当前时刻k的状态x(k)，定义避障轨迹规划与跟踪控制优先级分层预测控制问题，参见式(15)和(16)：

其中，符号“|”表示约束，u_p(k)为当前时刻k的决策控制变量序列u_p(k)＝{u(0|k),u(1|k),…,u(N-1|k)}，等式x(0|k)＝x(k)为式(15)和(16)的初始条件，J₁^*为式(15)的最优值，J₂^*为式(16)的最优值；

7)、求解式(16)得对应J₂^*的最优解u_p^*(k)＝{u^*(0|k),u^*(1|k),…,u^*(N-1|k)}，参见式(17)：

取第一个分量u^*(0|k)作用于自主驾驶车辆；在下一个采样时刻k+1到达后，检测车辆的运动状态x(k+1)，并以该状态更新优化控制问题式(15)和(16)的初始条件，然后优化计算当前时刻的最优解u_p^*(k+1)，周而复始，直到自主驾驶车辆运动到目标位置为止。

2.如权利要求1所述的用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法，其特征在于，所述步骤1)中，纵向安全距离为3m。