基于改进MPC 的分布式大间距多车编队控制研究（上）

2023-05-19郑屹天郭志强赵建新张宏映

火力与指挥控制 2023年3期

郑屹天，郭志强，赵建新，张宏映

（北方自动控制技术研究所，太原 030006）

0 引言

随着无人平台及智能技术发展，多平台编队控制在运输、侦察、突击等方面有着广泛应用，多智能体系统（MAS）的编队控制成为重要课题［1-2］，纯无人编队方法已经应用于无人机［3-5］、无人舰艇［6-8］等空中、海上战场领域。然而，对于地面领域［9］，其地形及电磁场环境复杂，平台视野受限且通信网络并不稳定，纯无人车编组自主分析、决策能力不足，由控制中心远程控制的一般编队方法少有实现。

常用的编队控制方法有领航跟随法、虚拟结构法和基于行为法［10-14］，其中，领航跟随法因其简单快速、控制稳定被广泛运用。文献［15-16］可以看出MPC 方法在轨迹跟踪方面能够处理其他方法难以解决的多约束、非线性等问题，这为编队控制提供了很好的设计思路，很多学者在车辆编队控制中采用MPC 方法设计控制器。文献［17］设计了一种基于样条曲线的分布式MPC 控制器，并通过交替方向乘子法解算，完成了高实时性的稳定编队控制，然而该方法并不适用于一般的非完整约束模型。文献［18］设计了一种分布式非线性MPC 控制器，采用GPNN算法求解控制器优化问题，完成了基于领航跟随法的编队控制，然而，其虚拟领航者轨迹同样是事先规划的已知路径。上述文献实验中，领航者与跟随者编队间距较小，且系统工况接近期望轨迹。

值得注意的是，系统工作点与期望轨迹相近的假设条件在实际工况中并不满足，同时编队轨迹并不总是预先规划的。因此，已有文献中的方法无法直接应用。进而，本文针对多车编队问题，考虑到编队间距大、系统工作点远离期望轨迹等约束，设计了分布式改进MPC 方法，有效实现了大间距多车编队的稳定运行。

本文主要贡献点为：针对系统工作点远离期望轨迹的问题，修正线性化展开点优化预测模型，使系统能够在状态误差大的情况下收敛，提升了系统的保守性，同时减轻了对轨迹规划的依赖，降低了控制器设计的复杂度，有效地增强了实时轨迹跟踪性能。

1 问题描述

本章首先介绍车辆二阶2 自由度非完整约束模型，然后根据线性MPC 的特点对车辆模型进行线性离散化处理，最后引出多车编队控制问题，得出本文假设条件下的编队控制目标。

1.1 车辆2 自由度非完整约束模型

车辆运动学模型如图1 所示，规定逆时针转角为正，顺时针转角为负，全局坐标系XOY 采用笛卡尔坐标系，（x，y，φ，v）分别为车辆的后轴轴心横、纵坐标及车辆的横摆角和速度，a 为车辆加速度，δ 为车辆前轮偏角，L 为车辆轴距长度，ρ 为车辆转弯半径。

图1 车辆运动学模型Fig.1 Dynamic model of the vehicle

假设车辆在运动过程中不存在横向滑动，考虑平面上的n 个结构相同的车辆，即轴距L 相同：

对于任意第i 个车辆，根据运动学关系及几何关系可得二阶2 自由度非完整约束运动学模型方程：

1.2 模型线性离散化

非线性模型能够精确地预测模型的输出响应，但非线性模型预测控制器（NMPC）计算量大，实时性能较差。对模型线性化处理，设计线性模型预测控制器（LMPC）能够提高控制器运算速度，满足编队控制的实时性需求。

设定车辆i 的追踪轨迹为Zir，若参考系统能够完全追踪Z ，参考系统在任意时刻满足：

式（8）形式为连续微分方程，无法直接用于MPC控制器，因此，必须进行离散化处理。在任意k 时刻，对式（8）进行前项欧拉离散化：

式中，T 为采样周期，整理式（9），状态量误差模型的离散状

1.3 编队控制问题描述

本文采用基于距离角度（l-φ）的领航跟随法。如图2 所示，l 为领航者与跟随者间相对距离（为简化计算，以车辆后轴轴心为车辆位置点），θ 为两者距离连线方向与领航者横摆角方向夹角。

图2 编队结构Fig.2 Formation structure

式中，ZL表示领航者实际位置，C（·）为队形信息从极坐标系到全局坐标系的映射。

对编队MAS 有如下假设：

假设1：编队行进过程中，编队通信不受限制。

假设2：编队中每个车辆都能通过通信拓扑与自身邻居通信并传递位姿信息。

假设3：领航者跟随一条预设轨迹，跟随者跟踪领航者实时位置与队形信息解算得出的轨迹。

上述假设条件下，编队控制目标为跟随者与领航者之间的姿态偏差收敛于队形信息。由式（11）得到局部信息条件下的编队控制目标：

2 控制器设计

MPC 按预测模型类型可分为LMPC 和NMPC两大类。本文采用的运动学模型为典型的非线性系统，NMPC 控制更加精确，但约束优化中将引入大量的非线性约束，求解时间将远大于线性优化问题。LMPC 约束条件为线性约束，计算量小，速度快，更契合实际运用需求。因此，本文控制器设计采用LMPC 方法。

2.1 MPC 原理

MPC 本质是一种最优化控制［21］。如图3 所示，MPC 基于系统模型与任意k 时刻系统状态预测未来一段时间（从k+1 到k+Np时刻）内所有的系统状态，根据设计好的约束条件与代价函数优化求解出第k 到第k+Nc-1 时刻的预测控制量，将预测控制量序列中第1 组控制量作用于受控对象；预测时间窗口Np随时刻k 滚动，在每一时刻重复以上过程完成控制量求解，同时对系统实际输出与期望值作比较，反馈于约束优化环节，防止系统输出偏离期望值，经过反复地在线滚动优化，实现系统输出对期望值的跟踪。

图3 MPC 原理Fig.3 MPC principle

2.2 MPC 控制算法设计

式（10）为状态量线性误差模型，控制量在实际物理意义下可以在一定范围内进行突变，不需要重新设计新的状态量表示未来的预测输出。

式中，Q，F，R 分别为系统状态误差权重、控制量误差权重和终端误差权重。

式（14）代入式（15）：

记状态量个数为NZ，控制量个数为NU，则为：

整理式（16）：

实际物理系统的速度不会无限增加，瞬时加速度和前轮偏角有一定的范围限制。对应状态量与控制量的约束条件：

另外，前轮的转动受到传动限制，不可能产生较大突变，同时为了使转向更加平滑，增加前轮偏角增量约束：

结合式（19）和式（20）的不等式约束，求解式（18）的最小化问题，这将控制问题转换为带约束的二次规划问题，用于求解Np内的预测控制量序列U_ki。

考虑到算法复杂度和仿真实验的算法实现，二次规划问题运用内点法求解。

内点法的基本思想是将原问题转换为只有等式约束和简单不等式约束的等价问题，求解等价问题的一阶最优性条件并进行扰动，运用牛顿法求解KKT条件，得到最优解的搜索方向，最后求取最优解。在MATLAB 仿真平台中已有成熟的quadprog 算法完成二次规划问题的求解，故不再赘述其具体算法。

2.3 线性化展开点修正

根据线性离散化过程可知，A˜和B˜动态变化，依赖任意k 时刻实时的期望值更新，与系统状态无关。当车辆与期望轨迹的状态误差较小时，模型的状态量仍能收敛于期望轨迹；当状态误差过大，预测模型无法预测原非线性模型的未来变化趋势。如下页图4 所示，车辆实际状态与参考点状态的横摆角完全不同，基于参考点展开的线性化模型所设计的MPC控制器，在短时间内使实际的车辆模型速度方向趋于水平，不指向期望点，根据设定好的权重矩阵，R 和预测时域Np，MPC 控制器将陷于局部最优解中无法跳出。

图4 车辆模型远离期望轨迹Fig.4 The vehicle model away from the desired trajectory

基于上述问题，对车辆模型远离参考轨迹情形下的参考点作出修正：

式（21）的修正方向即为图4 中蓝色点划线方向。状态误差较小时，φir切换回实际的参考点状态。

2.4 MPC 分布式化

编队内每个个体都设计单独的MPC 控制器，能够将编队控制问题拆分为单个个体对期望轨迹的跟踪问题，减小控制器运算负担，提高编队系统的实时性。

在编队控制中，领航者跟踪预设轨迹，跟随者个体i 的期望轨迹Zir是领航者状态生成的实时轨迹。因此，只需将领航者实时状态作为跟随者的期望点，并将其代入式（18），即可完成单个MPC 控制器的期望设计。

整理式（12）：

编队目标与MPC 代价函数的优化目标一致，结合式（12）、式（22），式（15）的第1 部分Ja与第3 部分Jc变形为：

分布式MPC 编队控制算法流程如图5 所示。

图5 编队控制算法流程图Fig.5 Flowchart of formation control algorithm

3 系统仿真及分析

为验证所提编队控制方法的有效性，在Matlab中进行仿真实验。选取1 辆领航者和5 辆跟随者完成编队跟踪控制仿真。

编队仿真结果如图6～图8 所示。

图6 领航者仿真结果Fig.6 Simulation results of the navigator

图6（b）、图6（d）表明单个车辆跟随远距离轨迹在10 s左右状态量误差收敛为0，能够在一般轨迹条件下实现快速跟踪。

图7（b）、图7（d）表明在15 s 左右编队内跟随者位置状态误差第1 次收敛为0，20 s 时队形间距改变后5 s～8 s 内位置状态误差第2 次收敛为0，30 s时的轨迹变换与35 s 时的队形切换带来的状态误差基本可以忽略不计。这3 个时间点系统对误差的响应表明，系统跟踪误差收敛耗时的原因在于远距离轨迹跟踪，轨迹变换情况下系统重新收敛的时间可以忽略不计，队形等间距切换情况下系统重新收敛的时间在3 s～5 s。