[发明专利]一种递进式模型预测无人驾驶规划跟踪协同控制方法

权利要求书

1.一种递进式模型预测无人驾驶规划跟踪协同控制方法，其特征在于，该方法包括以下步骤：

步骤1：建立车辆的离散运动学模型：

其中，k表示采样时刻，表示车辆在全局参考坐标系XOY下的状态向量，[v,δ_f]^T表示控制输入向量，(x,y)表示车辆重心的位置，表示航向角，v表示车辆重心处的速度，δ_f表示前轮的转向角，L表示车辆轴距；

步骤2：确定车辆运行时需要满足的约束条件：为了避免车辆发生碰撞，确定时变安全的避障约束条件，表达式如下：

其中，x_r,i和y_r,i分别表示无人车与第i个障碍物之间的纵向和横向相对距离，L_x和L_y分别表示纵向和横向安全距离，式中的正负号由车辆选择的避障方向确定，如果选择从左侧规避障碍，不等式中的符号为负号；反之，如果选择右侧避障的话，符号为正号；

除避障约束外，路径规划部分还应满足横向位移约束条件，表达式如下：

其中，y_min和y_max表示在k采样时刻的最大和最小横向位移的约束，该约束保证车辆能行驶在车道内；

此外考虑控制输入的约束和控制输入增量约束，以保证控制输入满足执行机构的物理约束和生成轨迹的平滑性要求：

控制输入的约束条件表示为：

控制输入增量约束条件表示为：

控制增量定义如下：

Δv(k)＝v(k)-v(k-1)

Δδ_f(k)＝δ_f(k)-δ_f(k-1)

其中，v_max(k)和v_min(k)表示在k采样时刻的最大车辆重心处的速度和最小车辆重心处的速度；

步骤3：计算各个时刻的障碍势场P_o，障碍势场P_o的计算公式如下：

其中，N表示障碍物的个数并且第i个障碍物的避障势场表达式如下：

其中，Ω表示障碍物的作用区域，即势场作用域，该区域由碰撞约束决定，γ_o表示障碍的收敛系数，d_o,i(k)表示k时刻车辆距离第i个障碍物的纵向距离，A_o,i表示第i个障碍物的势场幅值；

步骤4：根据实际需求设计局部路径规划代价函数与轨迹跟踪代价函数，其中：

采用模型预测结构构造优化指标如下：

其中，N_p和N_c分别表示预测时域和控制时域，Q,S,R表示各项的权重，Ψ和U分别表示预测状态和控制输入，P_o表示障碍势场：

同时，设计轨迹跟踪层代价函数以跟踪上局部规划层计算出的安全参考轨迹，其表达式如下：

其中，Ψ_t和Ψ_p分别表示预测状态序列和给定的参考状态序列，代价函数中的第一部分表示期望预测状态能够跟踪上参考状态，第二部分是考虑到乘客舒适性的代价，Ξ和Θ代表着各自部分的系数；

步骤5：利用动态惯性因子粒子群算法的基本步骤求解规划层模型预测控制问题，计算最优控制序列与状态：首先对控制输入进行初始化，保证初始化粒子在整个解空间中随机分布，即在搜索半径内随机分布；结合步骤1-4构造的代价函数优化指标以及车辆的离散运动学模型，采用以下的更新公式对控制量u进行更新，利用步骤4中的局部路径规划代价函数设计粒子的适应度计算公式，迭代循环得到最优粒子，即最优控制量U_opt，以及对应的最优参考状态序列：

其中，p表示粒子，δ_f和s分别表示前轮的转向角和速度，w表示权重因子，v表示粒子的进化速度，c表示学习因子，r表示0至1之间的随机数，pb表示个体最优值，gb表示全局最优值；当开始进行迭代时，利用后两项公式计算粒子的进化速度，再用前两项公式计算粒子的位置，迭代至最大迭代次数或者满足阈值条件结束求解；采用的改进的粒子群算法使用动态惯性因子，计算公式为：

其中，k表示迭代次数，M表示最大迭代次数，w_max和w_min分别表示惯性因子的最大值和最小值；

步骤6：根据步骤5逐步迭代求得规划对应的最优控制输入和参考状态，对求得的最优参考进行四次多项式拟合，以对轨迹跟踪层的参考输入进行平滑处理，在获得每个采样点处平滑后的优化参考后，采用以U′＝[U(2:N_p),0]作为控制层初始化粒子的初始原点，同时利用预测时域内的道路曲率κ来计算优化的初始值：

其中，rand()表示[-1,1]之间的随机数，MR中m₁是与转向角有关的半径参数，m₂是与速度有关的半径参数；

步骤7：通过步骤1中的车辆的离散运动学模型预测系统预测时域内的状态，并根据步骤4的轨迹跟踪代价函数求解优化问题，得到最优控制序列；

步骤8：判断是否达到全局目标点，具体为：将最优控制序列中的第一个元素作用于智能车辆，当车辆执行机构执行控制量并进入下一个控制周期时，车辆状态更新，判断是否到达全局目标点，如否，返回步骤3，重复步骤3-8，如是，直至到达目标点，结束循环。