单向拓扑下基于DMPC 与改进APF 的水面无人艇编队运动控制算法

黄雨杰，张子唐，孙 骞，李一兵

（1. 哈尔滨工程大学信息与通信工程学院，哈尔滨 150001；2. 先进船舶通信与信息技术工业和信息化部重点实验室，哈尔滨 150001）

1 引 言

水面无人艇（Unmanned Surface Vehicle,USV）是一种智能化的小型水面平台，由于具有较高的智能化程度、较好的隐身性能、较强的机动能力以及较低的造价，它被应用于完成情报收集、反潜、侦察与探测、精确打击等作战任务。面对动态复杂的环境、不可预知的潜在危险和多样化的任务使命，单一无人艇受限于自身搭载的有限载荷与系统，显得势单力薄。同时，由多艘无人艇联合起来构成的协同系统，具有更强的鲁棒性、通信能力、机动性、灵活性、更高的作业效率和更广的作业范围，在海洋战场环境信息收集、反潜领海监视、补给支援等军事领域发挥着越来越重要的作用。

多无人艇编队控制是指多个无人艇组成的编队在向特定目标或方向运动的过程中，相互之间保持预定的几何形态（即队形），同时又要适应环境约束（例如避开障碍）的控制问题[1]。因此，多无人艇编队运动控制技术是多无人艇水面无人艇完成协同作业任务的前提和基础，无人艇编队运动控制方法多应用领航-跟随结构、虚拟结构法和行为结构法等[2]。为了保证编队系统的稳定性和鲁棒性，多使用滑模控制[3]和模型预测控制（Model Predictive Control, MPC）[4]对无人艇编队成员的控制器进行设计，减小了模型误差和外界环境对无人艇运动的影响，实现编队维持和编队跟踪。其中，分布式模型预测控制（Distributed Model Predictive Control, DMPC）能够将复杂系统问题拆分成若干个相互关联简单系统问题并交给子系统单独计算，降低了时间成本和计算复杂度，所以DMPC 在无人车和无人机编队控制中得到了广泛的应用。但现有的大多数DMPC 算法都假设所有智能体都能获取平衡所需的先验信息，这对于在广阔海面航行的无人艇而言并不现实。

同时，在动态和不确定的水面环境中无人艇需要进行局部路径规划技术完成障碍物规避。当下，对于未知和动态障碍物的路径规划方法中，快速探索随机树（Rapidly-exploring Random Trees）不仅难以找到最优解，而且稳定性和实时性较差；模糊逻辑算法（Fuzzy Logic）过分依赖专家知识，且缺乏系统性；神经网络（Neural Network）需要大量的时间和样本对神经网络进行训练；强化学习（Reinforcement Learning）需要手工标记大量样本，且会遇到维度灾难的问题；与强化学习相比，虽然深度强化学习（Deep Reinforcement Learning）能够避免手工标记数据、减少维度，但是其也需要较长的训练时间；人工势场法（Artificial Potential Field，APF）结构简单、易于实现且实时性强，在无人艇的实时避障中有着广泛的应用[5-6]。

基于以上分析，本文研究了在编队内部分成员无法获得编队平衡的先验信息条件下的多水面无人艇编队控制问题。本文将领航-跟随结构与DMPC 算法结合，完成跟随者控制器设计，实现了编队内部分成员无法获取编队平衡先验信息情况下的无人艇编队控制；同时，针对无人艇编队内部障碍物规避问题，设计了基于改进APF 和MPC 的自主避障控制器；最后，通过仿真实验对算法的可行性和有效性进行了验证。

2 系统模型

2.1 无人艇动力学模型

为了简化无人艇的运动控制数学模型，将无人艇的运动近似为二维水平面上的三自由度运动模型，具体模型如图1 所示，图1 所表示的三自由度运动数学模型如式(1)所示[7]。文中所涉及的数学模型和部分状态变量符号和含义采用国际水池会议（International Towing Tank Conference）推荐的以及造船和轮机工程学会（Society of Naval Architects and Marine Engineers）术语公报推荐的体系[8]。

图1 双体船模型Fig. 1 Catamaran model

式中m表示无人艇的质量，Izz表示无人艇绕Z轴的惯性矩，Tp和Ts分别表示无人艇左右舷喷水推进器输出推力，τx= (Tp+Ts)表示无人艇控制力，τN= (Tp+Ts)B/2表示无人艇控制力矩，B表示无人艇的宽度，xG表示无人艇重心坐标，X(.),Y(.),N(.)是恒定的水动力导数，分别代表无人艇对于纵移力、横移力和艏摇力矩的敏感程度。无人艇的平面运动学模型为

式中，η= [x,y,ψ]T代表无人艇在北东坐标系下的位置和艏摇角大小，υ= [u,ν,r]T代表无人艇速度向量，R(ψ)是船体坐标系到北东坐标系下的转换矩阵，其具体表达式为

由于无人艇运动受到外部环境的限制，假设无人艇的水平面运动和转动之间是解耦的，进而可将复杂的动力学方程进行线性化处理，化简为速度和转向的动力学方程，故无人艇动力学方程为

式中，s=[u,v,r,x,y,ψ]T表示无人艇的状态量，c=[Tp,Ts]T代表无人艇的控制变量，即左右喷水推进器推力。无人艇详细信息和相关参数如表1所示。

表1 双体船模型参数[7]Table 1 Parameters of catamaran model[7]

2.2 编队模型

假设编队中有N艘无人艇，各无人艇动力学模型均由式(4)所示。无人艇编队控制的目标是跟随者（Follower）跟踪领航者（Leader）的航向角和速度，同时保证任意两艘无人艇之间的间隔与期望间隔相同，这可以表示为

式中， N = ｛1 ,2,3, … ,N｝；ψi(k)表 示第i艘无人艇在k时刻的航向角，ψL(k)表示领航者在k时刻的航向角；υl(k) =［ul(k),vl(k),rl(k)］T表示第i艘无人艇在k时刻的速度状态，包括纵移速度、横移速度和艏摇角速度，υL(k) =［u L(k),v L(k),rL(k)］T代表领航者在k时刻的速度状态；Pi(k)表示第i艘无人艇在k时刻的位置，rij=(xd,ij,yd,ij)T表示第i艘无人艇和第j艘无人艇之间的期望距离向量，通过控制rij值进而得到不同的编队队形。

无人艇编队中通信拓扑的具体结构对于DMPC 算法中代价函数设计有着重要的影响[9]。无人艇编队中通信拓扑可以用矩阵G= [gij]∈ℝN×N，具体定义为

假设在编队中领航者的通信优先级为1，领航者邻接集合AL内成员的通信优先级为2，能够与AL内成员通信而不能直接与领航者通信的无人艇通信优先级为3，依此类推，通信优先级示意具体如图2 所示。Mi表示比第i艘无人艇通信优先级更高的无人艇所组成的集合，O i=M i∩Ai为期望集合，表示能够向第i艘无人艇提供有用信息的无人艇集合；此时第i艘无人艇能够与邻接集合Ai内所有成员通信，但是仅对期望集合Oi中成员所广播的假设状态空间信息进行处理。虽然无人艇编队的通信结构为双向拓扑结构，但是其中关于状态空间的通信拓扑结构为单向拓扑。当无人艇编队的通信拓扑结构中包含一个生成树（Spanning Tree）时，编队成员之间的状态能够趋于一致[10]。

图2 通信优先级示意图Fig. 2 Schematic diagram of communication priority

3 编队运动控制

3.1 跟随者控制器

MPC 也称为滚动时域控制或移动时域控制（Receding Horizon Control），是一种基于模型的开环最优控制策略，其使用一系列控制方法，利用确定的模型预测未来时刻不同的控制变量对输出结果的影响，并且在满足系统约束条件的基础上通过最小化代价函数获得控制信号[11-12]。与传统意义下的最优控制算法不同，MPC 采用滚动优化策略，具体实现过程可描述为测量当前状态、预测迭代、求解优化和执行4 个步骤[13]，其基本原理如图3 所示。

图3 模型预测控制原理Fig. 3 Principle of model predictive control

DMPC 是指将整个系统分为几个相互关联的子系统，每个子系统都拥有自己独立的局部控制器，这些控制器不仅考虑子系统的自身状态，还受到其他关联子系统的影响[14]。DMPC 通过将一个大规模的复杂系统问题拆分成若干个较为简单的问题分配给子系统单独计算，降低了时间成本和计算复杂度。同时，在DMPC 中，允许各子系统间进行信息交互，从而能够实现子系统之间的相互协调，以达到系统的全局最优。

由于海面区域十分广阔，无人艇编队在执行任务时跟随者与领航者之间的距离可能超出有效通信范围dc，此时部分跟随者无法获得保持编队平衡的先验信息，这将增加编队航行过程中的不稳定因素。本文通过引入假设状态空间和平衡状态空间实现了编队内部分跟随者无法获取编队平衡先验信息情况下的无人艇编队运动控制。

考虑包括N-1 个跟随者（Follower）和1 个领航者（Leader）的无人艇编队系统。为方便表示对以下符号进行定义：

（1）si(k) =[xi(k),yi(k),ψi(k),ui(k),vi(k),ri(k)]T表示第i艘无人艇在第k时刻的状态；

（2）Si(k) =[si(k|k),si(k+1|k),…,si(k+Np|k)]T表示在k时刻，第i艘无人艇在[k,k+Np]时间段内的状态空间；

（3）Ci(k) =[ci(k|k),ci(k+1|k),…,ci(k+Np-1|k)]T表示第i艘无人艇在［k,k+Np］时间段内实现S i(k)的预测控制序列；

（4）sF(k) =[s1(k),s2(k), …,sN(k)]T表示在k时刻编队系统的状态集合；

（5）cF(k) =[ci(k),c2(k),…cN(k)]T表 示 在k时刻编队系统的输入控制量集合。其中，Np代表预测步长，同时对符号上标进行定义，p表示预测状态，a表示假设状态，b表示平衡状态。

进而，整个无人艇编队系统的离散时间动力学模型可表示为

DMPC 算法将整个系统的全局最优控制问题转化为单个无人艇i的局部最优控制问题。对第i艘无人艇而言，其局部最优控制问题的求解仅使用期望集合Oi中的无人艇提供的信息，故(k)的定义为

式中，poi,j代表选取某个邻接集合成员的偏好，一般而言，第j艘无人艇通信优先级越高poi,j越大。进而，第i艘跟随者无人艇的代价函数可以表示为式(10)所示形式。

其中，Qd、Qa、Qb、R均为对角矩阵，0 为零矩阵，Qd,i，Qa,i和Ri为第i艘无人艇代价函数的权 重 矩 阵，Qd,i=diag{qd,i1,qd,i2, …,qd,i6}表 示 对 预 测状态与期望状态之间误差的惩罚强度，Qb,i=diag{qb,i1,qb,i2, …,qb,i6}表 示 对 预 测 状 态 与 平 衡 状 态之 间 误 差 的 惩 罚 强 度，Qa,i=diag{qa,i1,qa,i2,…,qa,i6}表示对预测状态与假设状态之间误差的惩罚强度，Ri=diag{ri1,ri2}表示对预测控制序列与假设控制序列之间误差的惩罚强度。

第i艘无人艇的本地开环优化控制问题可以表示为式中，C 表示输入允许集合，S表示状态允许集合。进而，第i艘跟随者无人艇编队控制的DMPC控制算法内容如下：

（1）接收邻接集合内其他成员的假设状态空间Sia(k)，按照式(9)得出期望状态空间Sid(k)；

（2）获取k时刻的期望状态空间Sid(k)和平衡状态信息Sib(k)，使用顺序二次规划算法(Sequential Quadratic Programming, SQP)求解式(11)所表示的优化问题，得到最优控制序列Ci*(k)；

（3）使用最优控制序列Ci*(k)中的第一个控制量，即令ci(k) =ci*(k|k)，并将ci(k)作用于无人艇动力学模型，得到k+ 1时刻的状态si(k+ 1)；

（4）由最优控制序列C*(k)和最优控制状态空间Si*(k)得到假设控制序列Cia(k+1)，具体如式(12)所示。其中，c(k+Np|k)为保持最优预测状态空间Si*(k)中第Np个速度状态的控制量；

（5）将Cia(k+1)作用于动力学方程得到假设状态空间Sia(k+1)，并向外广播假设状态空间Sia(k+1)；

（6）求解平衡状态空间Sb(k+1)，其表示无人艇保持当前速度状态移动Np步生成的状态空间；

（7）令k=k+ 1，回到步骤（1）。

3.2 自主避障控制

当无人艇在编队航行遇到障碍物时，无人艇需要进行自主避障。在此情况下，无人艇集群控制与无人艇自主避障在控制目标层面是相互冲突的。编队集结需要无人艇尽可能保持相对稳定的队形，而自主避障则要求无人艇根据环境信息和自身情况和环境信息，进行实时路径规划。本文采用任务分时切换的方法[15]解决这一问题，即令自主避障任务的优先级高于编队集结任务，当无人艇完成自主避障后，重新执行编队集结任务，进而达到任务分时切换的目的。同时，无人艇在编队航行过程中不仅需要解决环境中障碍物的规避，还需要考虑编队内部成员的避碰，这要求无人艇需要拥有动态障碍物规避的能力。本文设计了一个基于人工势场的MPC 控制器实现无人艇自主避障。

3.2.1 引力势场

设无人艇在海面的位置坐标为P=(x,y)T，目标点的坐标为Pg=(xg,yg)T，在到达目标点之前，无人艇将一直受到引力的作用，与目标点之间的距离越远，无人艇所受到的引力就越强。所以，目标点产生的引力势场函数Uatt可表示为

式中，katt表示引力势场常数，d(P,Pg)=P-Pg表示无人艇与目标点之间的欧几里得距离。在引力势场中，无人艇受到的引力Fatt等于Uatt的负梯度，故Fatt可表示为

引力Fatt的方向沿着无人艇和目标点之间连线的方向指向目标点。

3.2.2 斥力势场

APF 中的斥力势场存在一定作用范围，只有当无人艇进入斥力场范围内才会受到斥力作用。为了使势场空间更符合无人艇的运动特性，本文对斥力势场的作用范围和具体形式进行了修改，具体过程如下。

式中，PΩ= (m,n)表示北东坐标系下的点坐标。为方便表达，将斥力场作用范围记为Ω，此时可以得到

其 中，F1= [x+ccos(ψ),y+csin(ψ)]T和F2=[x-ccos(ψ),y-csin(ψ)]T分别为斥力势场的两个焦点。

由于斥力势场需要拥有更大的下降梯度才能提升无人艇航行的安全性，故使用指数函数构造斥力场，改进后的斥力势场函数具体形式为

式中，krep表示斥力势场常数。在斥力势场中，无人艇受到的引力Frep等于Urep的负梯度，故Frep可表示为

斥力Frep方向沿着无人艇和障碍物之间连线的方向指向无人艇。

3.2.3 合力势场

假设空间中存在M个障碍物，即 {Po1,Po2,…PoM}⊆Ωo，其中Ωo表示障碍物区域。无人艇在向目标点移动的过程中会受到引力势场和斥力势场共同的作用，所以合力势场函数U(P,Ωo)可表示为

进而，无人艇在势场中受到的合力可表示为

3.2.4 自主避障控制器

无人艇自主避障的MPC 控制器与3.1 节中提到的DMPC 控制器类似，也可以用式(11)表示。但是，自主避障控制器中Qa=0，即在计算过程中不需要考虑假设状态空间Sa(k)。同时，期望状态空间Sd(k)由APF 算法得出的路径规划结果转换而成，即对规划路径进行采样，根据采样点的坐标生成对应的状态空间信息。单无人艇自主避障控制流程如下：

（1）当障碍物进入第i艘无人艇斥力场范围内时，第i艘无人艇与编队解耦，不再向邻接集合Ai内无人艇发送Sia(k)；

（2）以当前无人艇位置为起点，以编队目标为终点，进行实时路径规划，并且每次规划的步长与MPC 算法预测步长相同；

（3）将规划路径转化为期望状态信息Sid(k)，执行MPC 控制器，得到下一时刻状态Si(k+1)；

（4）令k=k+ 1，返回步骤（2），直到第i艘无人艇周围不存在障碍物；

（5）第i艘无人艇重新执行编队集结任务；

（6）当第i艘跟随者的期望集合Oi中的所有成员执行障碍物规避任务时，第i艘跟随者无法获取到有价值的期望信息，即Sid(k)=∅，此时第i艘跟随者使用进行自主巡航，当再次够获取Sid(k)时，第i艘跟随者回归编队。

综上所述，无人艇编队运动控制流程如图4所示。

图4 无人艇编队运动控制流程Fig. 4 Process of USV formation motion control

4 仿真结果及分析

4.1 实验背景

假设编队由9 艘无人艇组成，其中1 艘无人艇为领航者，其他为跟随者，编队处于理想环境下，不受海风、海浪、海流的影响；各无人艇以Pc,0=(0,0)T为中心组成圆形编队，编队中各无人艇初始状态如表 2 所示；有效通信距离dc=100 m，则无人艇编队通信拓扑结构如图5 所示，红色虚线表示无人艇能够接收领航者（优先级为1）的信息，蓝色虚线表示能够接收优先级为2的跟随者的信息，其中跟随者5～8 无法获取领航者的假设状态空间(k)。无人艇稳定状态下最大纵移速度为ustable= 1.2 m/s，预测步长Np= 10；区域海图信息已知，在区域中存在一个半径r=110 m 的岛屿；同时，区域中存在4 个个半径r=5 m 的圆形障碍物，由于障碍物尺寸过小，无人艇编队在航行前没有关于这4 个障碍物的先验信息，障碍物坐标如表3 所示。同时，APF 中斥力场相关参数设置如下：a=20 m，b=12 m。

表2 无人艇初始状态Table 2 Initial state of UAVs

表3 障碍物坐标Table 3 Coordinate obstacles

图5 无人艇编队单向拓扑结构Fig. 5 Unidirectional topologies structure of USV formation

假设领航者在考虑编队队形和岛屿对编队的航行的影响后，完成自身航行的全局路径规划。所以，本次实验要求无人艇编队中心从Pc,0=(0,0)T开始，在顺利规避小型障碍物的基础上，保持编队队形使得编队中心顺利到达Pc,g=(50 m, 450 m)T。

4.2 实验结果分析

图6 展示了无人艇在保证自身安全航行的前提下，保持编队队形，使编队中心从Pc,0=(0,0)T移动至Pc,g=(50 m,450 m)T的轨迹。从图中可以发现，无人艇编队从初始状态出发，在领航者的带领下向目的地移动。在编队航行过程中，当障碍物进入斥力场范围内时，跟随者与编队解耦，执行自主避障任务；当避障结束后，无人艇再次回归编队，保持编队稳定航行。

图6 无人艇编队运动轨迹Fig. 6 Track of USV formation

图7 展示了在本次仿真实验中，无人艇实际位置与保持编队队形的期望位置之间的相对距离。虽然各无人艇的初始位置已经满足编队队形要求，但是无人艇之间的航向角和速度状态与领航者不同。所以，各跟随者在[0,30 s]内调整自身状态，直到各跟随者状态si(k)与领航者的状态sL(k)趋于一致。在[31s,500 s]内，编队内部的跟随者未受到小型障碍物的影响，无人艇编队能够保持稳定的编队队形，按照领航者所规划的轨迹稳定航行。在[501s,720 s]内，无人艇编队内部成员受到障碍物1、2、4 的影响，所以编队中部分跟随者与期望位置之间的相对距离发生改变。在[721s,823 s]内，无人艇编队保持编队队形稳定航行，在k= 824 s 时，无人艇编队中心到达Pc,g=(50 m,450 m)T。

图7 跟随者与期望位置之间的相对距离Fig. 7 Relative distance between follower and desired position

图8～13 展示了本次仿真实验中各无人艇航向角、速度状态（纵移速度、横移速度、艏摇角速度）以及控制量（左、右喷水推进器输出推力）随时间变化的曲线。从图中可知，跟随者在[0,30s]内调整各状态量，使得状态空间si(k)不断趋近于领航者的状态空间sL(k)；[30 s, 500 s]内无人艇编队保持稳定航行，编队内成员的航向角和速度状态保持一致；[510 s,720 s]内无人艇编队受到小型障碍物的影响，在这段时间内，解耦无人艇的状态量变化较为剧烈，编队内其他跟随者状态量基本与领航者保持一致。在[721s,824 s]内，无人艇编队保持编队队形稳定航行，编队的内部成员的状态量再次趋于一致。

图8 无人艇航向角Fig. 8 Heading angle of USVs

图9 无人艇纵移速度Fig. 9 Surge velocity of USVs

图10 无人艇横移速度Fig. 10 Sway velocity of USVs

图11 无人艇艏摇角速度Fig. 11 Yaw angular velocity of USVs

图12 无人艇左推进器输出推力Fig. 12 Left propeller output thrust of USVs

图13 无人艇右推进器输出推力Fig. 13 Right propeller output thrust of USVs

5 结 论

本文针对编队内部分成员无法获得保持编队平衡的先验信息条件下，多水面无人艇编队运动控制问题展开了研究。设计提出了一种基于APF和MPC 算法的无人艇自主避障和多无人艇编队控制方案。在本方案中，首先通过设定通信优先级和单向拓扑结构，将DMPC 算法引入假设状态空间和平衡状态空间，使用SQP 算法求解预测状态空间与假设状态空间和平衡状态空间之间的最小代价以完成跟随者无人艇控制器设计。其次为保证水面无人艇避障安全性，通过将改进APF 和MPC 算法相结合，实现了无人艇自主避障控制。数值仿真结果证明了本方案的有效性。

然而，本文假设编队处于理想环境下，不受海风、海浪、海流的影响，因此在未来的研究中，将重点关注面向时变洋流和动态威胁的多水面无人艇编队运动控制技术；此外，未来将进一步完善仿真验证场景，并计划利用水面无人艇编队模拟器等仿真平台对相关算法进行仿真验证。