CN111538328B - 一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法 - Google Patents

Abstract

一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法，本方法基于离散时间车辆动力学模型，首先定义出描述行车环境的势场函数，再将离散时间动力学模型作为预测模型，然后建立有限时域优先级分层优化控制问题，并结合滚动优化控制原理，在每个采样时刻计算得到最优控制解，实现基于目标优先级驱动的自主车辆避障轨迹规划与跟踪控制的预测控制。本发明方法的优点是理解简单，通用性强，控制量满足自主驾驶车辆安全避障规划要求和车辆操纵稳定性。

Description

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

技术领域

本发明属于自主驾驶车辆轨迹规划与跟踪控制领域，涉及一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法。

背景技术

自主无人驾驶是智能车辆发展的重要方向，自主车辆的轨迹规划在环境感知的基础上，为车辆规划一条安全无碰撞且可行的行驶路径，是实现车辆自主无人驾驶的关键技术。自主驾驶车辆轨迹规划与跟踪控制应满足车辆动力学/运动学特性以及各种约束，预测控制算法能够很好地解决此类问题。通过对现有基于预测控制的自主驾驶车辆轨迹规划与跟踪控制的文献检索，发现现有方法将避障目标函数和跟踪目标函数直接相加，转换为单个综合目标函数，并用加权系数表示避障目标和跟踪目标的相对重要性，但避障目标函数和跟踪目标函数不同，度量的单位不同，而且避障目标和跟踪目标存在相互冲突特性，因此，将避障目标函数和跟踪目标函数直接相加的方法不能真实反映自主驾驶车辆的避障高优先级要求，尤其是对于车辆这种复杂非线性动力学系统而言，固定两者的加权系数适应性差，但行驶过程在线调整两者的加权系数将增加驾驶危险性而且难以实现。本方法把自主驾驶车辆避障轨迹规划与跟踪控制过程建立为一个目标优先级明确的多目标控制问题，将车辆的避障轨迹规划目标作为最高优先级，跟踪控制目标作为低优先级，然后从高优先级避障轨迹规划目标要求开始计算，逐层实现自主车辆避障轨迹与跟踪控制目标，保证对更高优先级避障目标的控制效果。本方法与现有自主驾驶车辆避障轨迹规划与跟踪控制方法相比，不需要将避障目标函数和跟踪目标函数相加，从而不存在两者的加权系数，因而本发明方法使用上更具灵活性和实用性。

发明内容

相对现有自主驾驶车辆避障轨迹规划与跟踪控制的加权预测控制方法，为了克服现有技术的不足，本发明设定自主驾驶车辆的避障轨迹规划目标为高优先级控制目标，自主驾驶车辆的跟踪为次优先级控制目标，根据排序结果，依次优化自主驾驶车辆的避障轨迹，同时确保自主驾驶车辆对优化避障轨迹中的安全跟踪控制，避免了避障轨迹规划目标和跟踪控制目标的加权系数复杂的在线调整和重新配置过程。

本发明解决其技术问题所采用的技术方案是：

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

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)，周而复始，直到自主驾驶车辆运动到目标位置为止。

进一步，所述步骤1)中，纵向安全距离为3m。当然，也可以设置为其他数值。

本发明的技术构思为：用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法基于离散时间车辆动力学模型，定义出描述行车环境的势场函数，将离散时间动力学模型作为预测模型，然后建立有限时域优先级分层优化控制问题，结合滚动优化控制原理，在每个采样时刻计算得到最优控制解，实现基于目标优先级驱动的自主车辆避障轨迹规划与跟踪控制的预测控制。本发明方法的优点是理解简单，通用性强，控制量满足自主驾驶车辆安全避障规划要求和车辆操纵稳定性。

本发明主要执行部分在自主驾驶车辆运动控制计算机上运行实施。本方法应用过程可以大致分为3个阶段：

1、参数设置：分为车辆物理参数和控制器参数，在车辆物理参数导入界面中，输入采样周期T_s，整车质量m，转动惯量I_z，车辆质心到前、后轴的距离d_f和d_r，前轮和后轮的侧偏刚度C_cf和C_cr，分道线势场强度A_l，分道线势场值收敛幅度σ_l，道路边界势场强度A_r，目标点纵向位置X_goal，正整数ε和β，车辆横向势场收敛幅度σ_o，纵向势场收敛幅度ρ，区域S₁和S₂纵向势场强度A_c,1和A_c,2，控制量、控制增量、前轮侧偏角、后轮侧偏角约束下界u_min、Δu_min、α_fmin、α_rmin，控制量、控制增量、前轮侧偏角、后轮侧偏角约束上界u_max、Δu_max、α_fmax、α_rmax，目标点x_goal和纵向目标速度v_x,goal；在控制器参数导入界面中，输入预测时间窗口N，矩阵W、S和系数R、Q；输入参数确认后，由控制计算机将设置数据送入计算机存储单元RAM中保存；

2、离线调试：点击组态界面中的“调试”按钮，进入控制器离线仿真调试阶段，调整组态界面中的预测时间窗口N、矩阵W、S和系数R、Q，观察自主驾驶车辆位置、横摆角、横摆角速度、纵向速度和横向速度的变化，选取自主驾驶车辆路径光滑安全的一组预测时间窗口值和加权矩阵值；预测时间窗口N越大，避障规划路径越平滑；Q越大，越容易避开障碍物；R越大，跟踪误差越小；S越大规划的路径越平稳；W越大规划的路径与道路中心线的位置偏差越小；因此，实际调试R、W、S、Q时，应权衡自主驾驶车辆状态避障效果、跟踪效果、路径平滑、路径靠近中心线之间的综合控制效果；

3、在线运行：点击组态界面“运行”按钮，启动自主驾驶车辆运动控制计算机的CPU读取存储单元RAM中车辆物理参数和控制器参数，并执行“车辆避障轨迹规划与跟踪控制程序”，通过在线测量自主驾驶车辆的纵横向位置、横摆角、横摆角速度和纵横速度，控制输入车辆的纵向加速度和前轮转角，实现自主驾驶车辆安全平稳行驶；在下一个采样时刻到达时，在线测量自主驾驶车辆的实际纵横向位置、横摆角、横摆角速度和纵横速度，重复整个执行过程；周而复始，实现自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制。

本发明明确自主驾驶车辆避障规矩规划与跟踪控制的目标优先级，采用优先级分层排序，优先规划车辆的避障目标轨迹，同时跟踪控制规划的轨迹，以确保自主驾驶车辆在避障过程中的最重要安全性，避免避障目标和跟踪目标复杂的加权系数调整和重新配置的需要。

用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法可以在自主驾驶车辆控制系统组态界面上完成，此过程可以参考本说明书下文中提供的实例应用。与现有自主驾驶车辆避障轨迹规划与跟踪控制方法相比，本发明明确自主驾驶车辆避障目标是最高优先级，跟踪目标是次优先级，优先规划车辆的避障目标轨迹，同时跟踪控制规划的轨迹，以确保自主驾驶车辆在避障过程中的最重要安全性，避免避障目标和跟踪目标复杂的加权系数调整和重新配置的需要。下文具体实施方法以用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法为例说明本发明的实际效果，但本发明的应用范围并不以本实施例中的自主驾驶车辆避障轨迹规划与跟踪控制为限。

本发明的有益效果主要表现在：1、设计简单、容易理解、通用性强；2、在避障轨迹规划与跟踪控制过程中，确保车辆在避障过程中的最重要安全性，避免避障目标和跟踪目标复杂的加权系数调整和重新配置的需要。

附图说明

图1为自主驾驶车辆在X-Y平面运动轨迹曲线的示意图，其中，虚线表示道路中心线，实直线表示车道线；长方体表示障碍车辆，弯曲线表示规划的自主驾驶车辆避障轨迹；

图2为自主驾驶车辆避障轨迹跟踪控制的纵向加速度曲线的示意图；

图3为自主驾驶车辆避障轨迹跟踪控制的前轮转向角曲线的示意图。

具体实施方式

下面结合附图对本发明作进一步描述。

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

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₁定义为自主驾驶车辆距离障碍车辆在纵向安全距离(例如3米)以外区域，区域S₂定义为自主驾驶车辆距离障碍车辆在纵向安全距离(例如3米)以内区域和等于纵向安全距离(例如3米)区域，即区域S₂为自主驾驶车辆距离障碍车辆小于等于纵向安全距离(例如3米)的区域；

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)，周而复始，直到自主驾驶车辆运动到目标位置为止。

本实例为用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法的实施过程，具体操作过程如下：

1、在参数设置界面中，输入采样周期T_s＝50.0ms，整车质量m＝1027.0Kg，转动惯量I_z＝1536.7Kg·m²，车辆质心到前轴的距离d_f＝1.015m，车辆质心到后轴的距离d_r＝1.895m，前轮侧偏刚度C_cf＝49072.0N/rad，后轮侧偏刚度C_cr＝49072.0N/rad，分道线势场强度A_l＝2.0，分道线势场值收敛幅度σ_l＝1.2，道路边界势场强度A_r＝0.6，目标点纵向位置X_goal＝150.0m，正整数ε＝1.0，正整数β＝0.1，车辆横向势场收敛幅度σ_o＝0.6，纵向势场收敛幅度ρ＝0.5，区域S₁纵向势场强度A_c,1＝15，区域S₂纵向势场强度A_c,2＝10，控制量、控制增量、前轮侧偏角、后轮侧偏角约束下界u_min＝[-4,-2]^T、Δu_min＝[-2,-9.4]^T、α_fmin＝α_rmin＝-2，控制量、控制增量、前轮侧偏角、后轮侧偏角约束上界u_max＝[2,25]^T、Δu_max＝[2,9.4]^T、α_fmax＝α_rmax＝2、目标点x_goal＝[150,-2,0,25,0,0]^T和纵向目标速度v_x,goal＝25m/s；在控制器参数导入界面中，输入预测时间窗口初始值N＝5，矩阵初始值W＝5I₆和S＝5I₂，其中I₆为矩阵维数是6的单位矩阵，I₂为矩阵维数是2的单位矩阵，系数初始值R＝5和Q＝5；

2、在组态界面上点击“调试”按钮进入调试界面，启动自主车辆运动控制计算机的CPU调用事先编制好的“车辆避障轨迹规划与跟踪控制程序”求解控制器，具体计算过程如下：根据N和R、W、S、Q值，求解优化控制问题式(17)，得到最优解u_p ^*(k)＝{u^*(0|k),u^*(1|k),…,u^*(N-1|k)}，取第一个分量u^*(0|k)作用于自主驾驶车辆；根据N和R、W、S、Q的取值和调整规则，比较自主驾驶车辆行驶路径结果和控制量计算结果，调试预测时间窗口N＝15，调试R、W、S、Q得到矩阵W＝50I₆和S＝2000I₂和系数R＝50和Q＝5，将调试结果保存到计算机存储单元RAM中；

3、在线运行：点击组态界面“运行”按钮，启动自主驾驶车辆运动控制计算机的CPU读取存储单元RAM中车辆物理参数和控制器参数，并执行“车辆避障轨迹规划与跟踪控制程序”，通过在线测量自主驾驶车辆的纵横向位置、横摆角、横摆角速度和纵横速度，控制输入车辆的纵向加速度和前轮转角，实现自主驾驶车辆安全平稳行驶；在下一个采样时刻到达时，在线测量自主驾驶车辆的实际纵横向位置、横摆角、横摆角速度和纵横速度，重复整个执行过程；周而复始，实现自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制。

以上阐述的是本发明给出的一个实施例所表现出优良的自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制效果。需要指出的是，上述实施例用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明做出的任何修改，都落入本发明的保护范围。

Claims (2)

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。

Images