弱通信条件下基于MPC与融合状态估计的多UUV编队控制方法

徐红丽，邱少雄，李瑞峰，张昊鹏

（东北大学机器人科学与工程学院，沈阳 110819）

1 引 言

随着多智能体理论、无人水下航行器（Unmanned Underwater Vehicle,UUV）技术以及水声组网技术的发展，多UUV 协同因其冗余性、鲁棒性等优势被公认为海洋开发的利器，是未来UUV 应用的重要方向之一[1]。编队是多UUV 系统一种基本协同行为，也是近年来各国学者研究的热点。在实际复杂海洋环境中，信息传输时的通信时延与通信中断问题使得多UUV 之间无法实现实时的信息交互，严重影响了传统编队控制算法的性能[2]。因此，研究弱通信条件下的编队控制问题，具有很强的理论与实际意义。

各国研究者主要从三个方面来解决弱通信条件下的多UUV 编队控制问题。一是借助传统控制理论中解决时变与时滞问题的相关方法。此方面研究较为理想，一般只考虑时不变通信延时而不考虑通信中断。文献[3]基于一致性协议，设计分布式算法解决连续时间下的多智能体优化问题，并给出算法收敛的充分条件。文献[4]基于行为法和人工势场函数法实现了多UUV 分布式编队与避障策略。文献[5]基于李雅普诺夫与图论方法研究通信故障和时延条件下多UUV 保持一定队形沿给定轨迹运动问题。文献[6]研究切换通信拓扑的协同跟踪算法。文献[7]考虑多层级控制子系统间的紧密耦合关系和水声通信网络中通信时延的影响，设计基于通信时延的分布式编队控制方法，实现多UUV 对水下目标的稳定追踪，并通过构造李雅普诺夫函数分析稳定性，给出渐进稳定的充分条件。二是设计状态估计方法补充通信损失值。文献[8]基于通信距离与通信时延的耦合关系，基于梯度下降原理设计时延估计方法，利用估计延时信息实现队形控制。文献[9]设计事件触发拓展状态观测器，通过协同网络估计目标的速度和位置，实现弱通信下多水面艇对目标的协同跟踪。文献[10]设计基于混合行为的控制方法，基于卡尔曼滤波算法补偿动态环境下邻居节点信息，实现了动态海洋环境下多水面艇的协同控制。文献[11]在不确定非线性和环境扰动的情况下，设计具有多时间尺度结构的多欠驱动UUV三维编队控制方法。三是设计控制策略，弱依赖于通信与感知。文献[12]基于事件触发机制设计控制方法，减少了通信量。文献[13]引入盲区的概念，UUV 进入盲区后，只受虚拟领航者的影响，临近载体主动避让，从而减少通信量。文献[14]研究了基于加权投票游戏的控制策略，可以使编队中机器人无需获得某些全局信息就能完成编队。文献[15]针对有双独立马尔可夫变通信拓扑和传感时延无中心多UUV 协同控制问题，分别发送位置和速度信息，以减少各节点间发送的数据量，利用反馈线性化将复杂非线性耦合系统简化为双积分系统，并基于一致性算法实现了多UUV 协同控制。

上述方法均是在单一状态下解决弱通信条件下多UUV 编队控制问题。在实际运用中，UUV处于两种状态，一是沿期望路径航行状态；二是面向复杂未知的海洋环境时，为保证UUV 的安全性，需要实时规避静态、动态障碍物，使得编队处于机动过程而缺乏参考路径，从而产生随机轨迹状态。经验表明，相对于随机轨迹状态，沿期望路径航行时多UUV 编队对通信的依赖程度更低，在同一通信条件下可获得更高的编队控制精度。基于此，本文将设计基于路径跟踪与速度同步的编队控制方法、基于融合状态估计的编队控制方法，分别满足多UUV 在有无参考路径条件下的编队需求。

2 多UUV 编队模型建立

考虑到UUV 一般定深航行，不做频繁的深度变化，在本文中将三维问题转化为二维问题，如图1所示。其离散数学模型如式(1)。

图1 UUV 运动学模型Fig.1 Kinematic model

在编队中，如将UUV 看作点，将通信链路看作边，则可通过图论描述UUV 的通信拓扑。图G=(V,E)由点集合V和边集合E组成。图中点和边的数量|V|和|E|用N和M来描述。根据信息流向，将图分为有向图和无向图[16]。在本文中，描述编队通信拓扑的图为有向图。矩阵A为图的邻接矩阵，元素aij代表从点vj到点vi的边，有如式(2)的关系。

本文采用主从式编队。如图2所示，C 为世界坐标系，L为主UUV 的视线坐标系，F为从UUV。在世界坐标系中，主 UUV 位姿记作(xc,yc,φc)，位置记作rc=[xc,yc]T，φc为航向与Y轴夹角，ri=[xi,yi]T和rid=[xid,yid]T分别表示第i台UUV 的实际位置与期望位置。表示第i台UUV 在世界坐标系下的期望位置。显然存在式(3)。

图2 编队示意图Fig.2 Formation Diagram

3 编队控制方法

基于前述分析，多UUV 编队航行由两种状态组成。一为沿期望路径航行到达各目标点完成使命任务，二为实时机动规避静态、动态障碍物而缺乏参考路径。两种状态依海洋环境机动转换。有参考路径条件下使用基于路径跟踪与速度同步的编队控制方法，将基于模型预测控制(Model Predictive Control,MPC)实现，无参考路径条件下使用基于融合状态估计的编队控制方法。编队流程如图3所示。

图3 编队控制流程图Fig.3 Flow chart of formation control

3.1 基于MPC 的编队控制方法

有限时间内，多UUV 以一定的速度保持几何结构航行，则可认为形成编队。若各UUV 在编队稳定条件下以足够高精度按照预定航迹航行，那么在无通信的条件下亦可组成编队。但在实际情况中，UUV 精度差强人意，所以有必要设计速度同步策略。

基于上述分析，本文将多UUV 编队分为两个基本问题，分别为路径跟踪与速度同步，如图4所示。

图4 编队控制原理图Fig.4 Principle of formation control based on MPC

基于MPC 设计路径跟踪控制方法。如果期望控制量是0，采样周期是ΔT，基于图1，UUV在参考路径点的线性离散模型可表达为式(4)。当控制时域NC与预测时域NP长度一致时，经滚动预测，可得到式(5)。根据MPC 算法原理，可以得到式(6)。解优化问题，从而获得控制量uk+1。

在编队稳定条件下，保持速度一致性，则可保证编队持续稳定。本文在UUV 本地设置邻居节点路径跟踪模拟方法，以模拟邻居节点的路径跟踪效果，从而在一定程度上消除由路径跟踪带来的误差。若邻居节点偏离轨迹，则认为其受较大程度干扰，发生了不期望的较大误差，不予理会。本文只关心平行于路径方向的跟踪效果，即目前邻居节点的任务进行到了何种程度。若邻居节点只包含由路径跟踪控制精度导致的误差，则可认为模拟跟踪方法理想地模拟邻居节点的跟踪效果。在实际情况中，海洋环境、UUV 速度保持精度等也可引起跟踪误差。为更好地使模拟跟踪与实际路径跟踪之间的误差为0，本文设计服从高斯分布的误差补偿方法，以同步邻居节点与模拟跟踪误差。

i时刻，模拟控制输出理想速度值。k时刻与k+n时刻收到两个数据包，将会存在n+1 个采样点，计算两点间的平均速度为vmean。若与邻居节点实际速度无限接近，则可认为速度保持一致，但是各采样点的值无法获得。如果使各采样点速度与平均速度无限接近，那么可认为在此段时间内，二者所走路径长度一致。各采样点的噪声描述为式(7)。噪声η的均值与方差计算如式(8)。每个时刻，模拟控制的输出控制量描述为式(9)。通过模拟估算邻居节点位置，作为本地UUV的参考点，实现编队。

MPC 算法需消耗较大计算资源。在单领航者编队形式的简单拓扑中，每台UUV 具有一个邻居节点，模拟跟踪一台UUV 即可；存在多个邻居节点的复杂拓扑中，需要跟踪模拟多台UUV，从而需要占用更多的计算资源。在这种情况下，可设计轻量误差同步机制，以完成速度同步，同时减少对计算资源的消耗。

编队稳定条件下，将跟踪误差、洋流带来的误差、自主导航误差等的和设为噪声η，η服从高斯分布。以主从式编队为例，主UUV 在i时刻沿路径方向的期望速度为vei，实际速度为vai，可求得样本偏差如式(10)，n个样本点的均值与方差如式(11)。主UUV 将n个样本点的均值与标准差、当前位置信息以及时间戳经通信链路传送给从UUV，主UUV 平行于路径的近似速度可由式(11)计算。

在收到状态信息时，上述两种误差同步机制，都需要参考邻居节点的位置信息、带有噪声的速度信息以及时间信息，计算出邻居节点位置信息，以减少累计误差。

3.2 基于状态估计的编队控制方法

基于路径跟踪与速度同步的编队控制方法可更少地依赖通信，但无参考路径条件限制了其应用。故需设计不依赖于路径的编队控制方法。不依赖于路径的编队控制方法，由于缺少路径信息，必然会导致其对数据依赖程度的上升，稳定性、鲁棒性降低。

在二维空间中，t时刻UUV 的状态信息描述为[x,y,t]，可建立位置与时间的多项式(12)。在收到多个通信包时，存在包含n组状态信息的集合D，如式(13)，可建立多个位置与时间的多项式(14)～(15)。依据最小二乘原理，解得系数矩阵Ap如式(16)，代入式(12)可得到位置估计值P，称此为位置预测方法。

同样地，通过包含速度与航向信息的状态集合D′可建立速度、航向与时间的多项式集合(17)～(20)。基于最小二乘原理，可以解得系数矩阵AH如式(21)，代入式(17)可得速度与航向估计信息，联合式(1)计算位置估计值P′，称此为航迹推算法。

位置估计值P与P’哪一个精度更高呢？本文搜集了17 组实际的UUV 航行数据，分别间隔1s，2s，…，60s 采样，模拟通信时延，寻找两组估计值的误差规律，如图5所示。可见，在不同时延下，两种方法的估计误差有明显的差异。设计如式(22)的数据融合方法，简单地，w可以设为1或0。

图5 估计状态误差比较图Fig.5 Error comparison

接收到通信数据时，修正状态估计误差。为减少内存消耗，又保持估计的精度，本文在上述两种状态估计方法中分别设置最大与最小数据量nmax，nmin，n′max，n′min，当集合D与D′小于最小数据量时，两估计器分别不予工作，且保持上一时刻的速度与航向信息，利用式(1)计算位置信息。当容器中数据量处于最大与最小数据量之间时，新的数据自动补位到集合后端，更新集合D与D′。当容器中数据量等于最大数据量时，剔除容器中首个数据，所有数据前移，新数据补位在后端，更新集合D与D′。

本文认为UUV 搭载高精度的导航系统。UUV自主导航系统会输出位置、速度与航向信息，供协同控制系统使用。在有洋流时，小型UUV 抗流能力较弱，输出航向信息与实际运动方向会存在如图6所示的矢量关系。其中，OH为输出航向方向，OC为洋流方向的UUV 运动矢量，OA为UUV 实际运动矢量，三者存在式(23)的关系。

图6 输出航向与实际运动方向矢量关系图Fig.6 Relationship of heading and motion direction

UUV 搭载的多普勒计程仪有测流功能。有洋流信息集合C={C1，C2，…，Ct，…，Cn}，若收到t时刻的状态信息包，输出航向方向运动矢量为OH，洋流Ct方向运动矢量为OC，则可以按照式(23)进行误差修正，并释放C中Ct及Ct之前的信息。

本文将多UUV 看作一阶多智能体系统，并基于一致性算法设计编队控制方法。单体UUV运动模型描述为式(24)。其中，ri(t)表示智能体i的状态向量。在目前的研究与应用中，单领导者编队是最常用的模式。本文将主UUV 的状态设置为期望值，基于一致性算法，在有限时间内，从UUV 状态与主UUV 达到一致，即可实现编队，如图2所示。从UUV 期望位置计算如式(3)。UUV的控制信息计算如式(25)。其中，Ni为节点i的邻居节点集合，aij是(i,j)项邻接矩阵元素。

4 仿真实验分析

本文通过仿真实验验证算法有效性，并与单积分器系统一致性编队控制算法[17]比较，如式(26)。其中，k取0.15。

考虑包含2 个跟随者和1 个领导者的多UUV系统，其通信拓扑与载体坐标系下队形如图7所示，其中L 表示领导者，F1、F2表示跟随者。

图7 通信拓扑与队形图Fig.7 Communication topology and formation graph

设置700 s 的仿真实验，其中0～400 s 无参考轨迹，400～700 s 有参考轨迹。0～100 s 和400～500s 设置2 s 通信延迟与5%的随机丢包率；200～210s 和600～610 s 分别设置10 s 的通信中断；其他时间设置2 s 通信延迟。通信周期0.5 s。

领导者初始位置[377 m，420 m]，初始深度0 m，初始速度0 m/s，初始航向45°。跟随者1初始位置[339 m，425 m]，跟随者 2 初始位置[382 m，382 m]。两跟随者初始深度0 m，初始速度0 m/s，初始航向45°。编队以1.5 m/s 的期望速度定深5 m 航行。

在无参考轨迹阶段，领航者以速度1.5 m/s，航向45°状态航行；在有参考轨迹阶段，领航者目标点为[800 m，765 m]、[1119 m，1084 m]，跟随者目标点由相对位置自动生成。

本文所用控制方法仿真轨迹如图8所示。红色线表示领航者轨迹，蓝色线表示跟随者轨迹，黑色线标识了同一时刻三台UUV 的位置。可以看出，编队可较好保持期望的三角形队形。

图8 基于MPC 与融合状态估计方法编队轨迹图Fig.8 Trajectory of formation controller based on MPC and state estimator

本文设计的基于MPC 与融合状态估计方法以及传统的单积分器系统一致性编队控制算法在上述仿真条件下的x方向与y方向的误差曲线如图9所示。图中标识了丢包时刻与通信中断时间，以便分析不同弱通信情况对编队控制的影响。

图9 误差曲线图Fig.9 Error curve

无参考轨迹阶段，当出现丢包时，基于融合状态估计的方法出现波动，通信恢复正常后，波动消失，队形恢复；在出现通信中断时，队形误差较丢包波动小。在2 s 延时下，最大队形误差0.6 m，且由于通信影响出现的“毛刺”现象也在可接受范围内。

有参考轨迹时，本文所设计的基于MPC 的编队控制方法在面临丢包、延时与通信中断时，都具有较强的鲁棒性，其微小的静态误差也在可接受范围内。

同条件下，单积分器系统一致性编队控制算法在编队稳定时存在较大的静态误差。存在丢包率时，其表现更佳，但也存在轻微波动。出现通信中断时，编队出现较大误差，且波动严重，经过一定时间后，队形恢复，但时间较长。

综上所述，本文所设计的基于MPC 与融合状态估计的编队控制方法，在有参考轨迹时，对于延时、丢包与通信中断的弱通信条件均具有较强的鲁棒性，但是其存在微小的静态误差；在无参考航迹时，受丢包影响较为严重，但是误差波动在可接受范围内。相比于传统的单积分器系统一致性编队控制算法，本文设计方法在队形控制精度、队形恢复时间以及编队的稳定性方面均具有一定优势。

5 结 论

本文将多UUV 编队航行分为有无参考路径两种状态，以解决其在实际海洋环境中面临的弱通信问题。在有参考路径时，设计基于MPC 的路径跟踪方法和误差同步机制，使编队系统弱依赖于通信；在无参考路径时，设计基于最小二乘的状态估计方法，解决一致性编队控制中弱通信导致的邻居节点状态值缺失问题。最后，通过数值仿真证明了算法的有效性。未来，将基于本文所提算法进行以下研究：

（1）设计误差估计方法，提高误差同步精度；

（2）无参考路径时设计分布式轨迹预测方法，融合预测轨迹与状态估计值，提高编队控制精度。