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

说明书

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

技术领域

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

背景技术

“无人驾驶车辆局部路径规划”的基本定义是根据车辆获取的当前周围环境信息和车辆的决策意图与全局路径信息进行结合，计算出一条或多条应用于控制层引导车辆前进的安全、平滑的局部路径。在路径规划技术发展初期，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：确定车辆运行时需要满足的约束条件：为了避免车辆发生碰撞，确定时变安全的避障约束条件，表达式如下：