CN111413966A - 一种递进式模型预测无人驾驶规划跟踪协同控制方法 - Google Patents

Abstract

本发明公开了一种递进式模型预测无人驾驶规划跟踪协同控制方法，步骤1：建立车辆的离散运动学模型；步骤2：确定车辆运行时需要满足的约束条件；步骤3：计算各个时刻的障碍势场P_o；步骤4：根据实际需求设计局部路径规划代价函数与轨迹跟踪代价函数；步骤5：利用动态惯性因子粒子群算法求解规划层模型预测控制问题，计算最优控制序列与状态；步骤6：根据步骤5逐步迭代求得规划对应的最优控制输入和参考状态，对求得的最优参考进行四次多项式拟合；步骤7：求解优化问题，得到最优控制序列；步骤8：判断是否达到全局目标点。本发明面加快了优化求解的收敛速度，减小了跟踪层优化的搜索空间；能将控制层的优化时间大大减少。

Description

一种递进式模型预测无人驾驶规划跟踪协同控制方法

技术领域

本发明涉及无人驾驶路径规划与轨迹跟踪控制技术领域，特别是涉及一种递进式模型预测控制算法框架。

背景技术

“无人驾驶车辆局部路径规划”的基本定义是根据车辆获取的当前周围环境信息和车辆的决策意图与全局路径信息进行结合，计算出一条或多条应用于控制层引导车辆前进的安全、平滑的局部路径。在路径规划技术发展初期，A*算法、D*算法、快速搜索随机树法(Rapidly-exploring Random Trees，RRT)等算法被应用于无人驾驶车辆。但这些算法普遍存在不能保证路径的最优、生成轨迹不连续等缺点。近年来，模型预测控制算法(ModelPredictive Control，MPC)在控制领域中不断发展。由于MPC具有滚动优化、反馈校正的思想，该思想与局部路径规划算法的思想十分契合，因此许多专家学者将其应用于路径规划过程并取得了很好的效果。传统方法只研究单独的规划问题和控制问题，为解决实时性问题采取的策略一般是采用简单的控制算法，如采用MPC规划、采用PID跟踪，或者对复杂问题进行简化，如简化系统模型以降低计算复杂度等。

在获取局部路径之后，无人驾驶车辆需要利用该路径结合当前车辆状态计算合适的控制量，使车辆能够跟踪上预定的参考轨迹。在无人驾驶车辆发展初期，PID控制由于其简单的结构及算法的成熟度，被学者们广泛应用于车辆的轨迹跟踪中，取得了较好的效果。随着实际应用对算法要求的不断提高，自适应控制、神经网络等算法被应用于车辆控制。与此同时由于MPC具有处理多变量、多约束等系统的能力，能够将车辆行驶中的运动学与动力学约束考虑在内，因此成为目前轨迹跟踪研究中最为广泛的一种控制算法。

相比于一般算法，MPC在无人驾驶的局部路径规划与轨迹跟踪问题上具有更优的控制性能。但该算法需要对车辆未来的状态进行预测，并根据目标函数计算最优控制量，由此给计算设备带来的计算负担十分巨大。因此，一般在上层采用基于MPC的规划控制算法来提供尽可能符合要求的参考控制量，在下层采用PID等低计算负担的控制算法实现跟踪过程。这样虽然降低了计算负担，但也在一定程度上牺牲了部分控制精度，并且不能保证控制量的最优性。

一般的简单算法或者简化的算法将导致控制优化性能的损失。

发明内容

本发明旨在提出一种递进式模型预测无人驾驶规划跟踪协同控制方法，在每个系统采样周期内，结合改进粒子群算法中逐步迭代和模型预测控制算法，利用规划层求解的最优控制输入和参考状态为跟踪层优化求解的初始原点，并以预测时域内的道路曲率为基本参数，计算选择跟踪层的最优解的搜索半径，以此减少解的搜索空间。

本发明的一种递进式模型预测无人驾驶规划跟踪协同控制方法，包括以下步骤：

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

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

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

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

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

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

与已有技术相比，本发明的一种递进式模型预测无人驾驶规划跟踪协同控制方法具有以下积极技术效果：

(1)一方面加快了优化求解的收敛速度，另一方面减小了跟踪层优化的搜索空间；

(2)能将控制层的优化时间大大减少；

(3)在采样周期一定的前提下大大减少计算最优解所需的时间，从系统整体角度减小计算负担。通过该方法可以。

附图说明

图1为本发明的一种递进式模型预测无人驾驶规划跟踪协同控制方法整体流程图；

图2为障碍物势场的三维示例图；

图3为障碍物作用区域Ω的平面示例图。

具体实施方式

本发明使用基于运动学模型的模型预测控制方法进行状态预测，并采用动态惯性因子的粒子群算法直接对代价函数进行优化求解。通过该方法保证每一个部分在实现功能的前提下获得更优的解。

在每个系统采样周期内，利用规划层求解的最优控制输入和参考状态为跟踪层优化求解的初始原点，并以预测时域内的道路曲率为基本参数，计算选择跟踪层的最优解的搜索半径，以此减少解的搜索空间。通过该方法可以在采样周期一定的前提下大大减少计算最优解所需的时间。

本发明的一种递进式模型预测无人驾驶规划跟踪协同控制方法，具体步骤如下：

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

其中，k表示采样时刻，

表示车辆在全局参考坐标系XOY下的状态向量，[v,δ_f]^T表示控制输入向量，(x,y)表示车辆重心的位置，

表示航向角，v表示车辆重心处的速度，δ_f表示前轮的转向角，L表示车辆轴距；在k采样时刻的

步骤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分别表示预测状态序列和给定的参考状态序列。代价函数中的第一部分表示期望预测状态能够跟踪上参考状态。第二部分是考虑到乘客舒适性的代价，即期望预测的控制输入变化尽可能小，保证控制动作的平顺性。Ξ和Θ代表着各自部分的系数，某一参数相较于另一参数越大，就代表着该项性能要求越高，同时该部分只需要满足式(4a)和(4b)描述的控制量与控制增量约束即可，不需要考虑可行驶区域安全约束；

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

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

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

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

其中，rand()表示[-1,1]之间的随机数，MR中m₁是与转向角有关的半径参数，m₂是与速度有关的半径参数，需要根据实际调整。由于应用场景的特殊性，所求得的半径将远小于步骤5中的初始化半径，解空间大小也远小于步骤5，在采样周期相同的情况下，优化求解的速度将大幅度提高；

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

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

本发明在规划和控制两个层面均使用当前应用广泛且作用效果表现较好的MPC方法，在规划层能得到光滑且满足约束的路径，跟踪层能够计算出使得车辆能快速准确跟踪上参考给定的控制量。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

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

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

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

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

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

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

Images