0 引　言

无人潜航器（Unmanned Underwater Vehicle，UUV）已成为水下作业的重要工具，具备高自主性、低风险以及隐蔽性等特点，各国已开展了相关研究^{[1 - 2]}。随着水下应用场景的不断丰富^[3]，多UUV协同控制成为水下作业的必然趋势。例如，美国海军研究院以水下滑翔机集群为核心平台，观测大范围近海和沿海^[4]。欧盟自2014年开始研究通过多AUV建立可扩展水声组网系统^[5]。在上述UUV协同作业场景案例中，编队控制发挥着不可替代的作用。已有的UUV编队方法主要为集中式领航-跟随法^[6]、虚拟结构法^[7]等。但是领航-跟随方法存在过度依赖领航者，缺乏鲁棒性，场景适应能力差的特点。而虚拟结构法同样需要UUV携带高精度导航装置，并且如何保证编队过程中的信息一致性是该方法的主要问题^[8]。

因此，为了解决传统方法实现UUV编队控制中，依赖绝对领航者或依赖绝对位置信息、时钟同步的问题。本文提出一种不依赖特定领航者，采用层级引领策略，基于最短距离与最小通信时延双重准则确定2个次级领航者以及不同层级的跟随者，在少量通信条件下使群体形成二叉树状层级引领共识性以此来避免整个群体对单一领航者绝对位置信息的依赖。在该策略基础上，采用目标队形匹配的方式来实现UUV编队的分布式控制，群体内的个体根据目标队形与二叉树引领信息确定自身的期望目标点，并采用内外环控制方式，将制导控制与运动控制解耦^[9]，降低控制器复杂度。此外，在规划层引入阈值控制方法，当多UUV队形控制过程中存在碰撞风险则引入一致性控制思想，将保持群体航向一致与避免碰撞作为目标，稳定后再次进行队形控制，这样既保证编队的形成又可以解决编队控制过程中的避碰问题。

1 编队控制策略设计

编队控制策略的设计需要考虑节点之间的通信交互能力以及载体自身的导航定位能力，问题的关键在于以最小的通信代价，设计符合载体导航定位条件的交互策略，更高效地表达群体共识信息是协同控制策略设计的本质。

对于协同控制中的编队策略，需要将完整的队形约束表达出来，形成群体共识信息。例如，常见的领航-跟随策略如图1所示，依靠领航者通信分享给跟随者自身的位姿状态信息。本质上，领航-跟随控制策略的群体共识就是领航者实时的位置，有了该共识性信息，跟随者基于$l-psi$^[10]，$l-l$^[11]规划方法可确定自身的目标位置。然而该策略需要领航者不断发送自身的位置信息到其他跟随者，以此形成满足编队控制需要的群体共识性，对水声通信服务质量存在依赖，在工程应用上只能实现小规模编队。

基于虚拟结构法的UUV编队控制策略解决了传统领航-跟随法依赖水声通信服务质量以及单一领航者的问题，其群体共识性体现在各UUV有着一致的时空基准，然而在实际工程应用上，实现大规模UUV时空基准的一致性需要搭载昂贵的高精度组合导航装置，成本较高。

1.1 基于二叉树拓扑的共识性

传统领航-跟随法依赖通信服务质量、单一领航者，虚拟结构法依赖高精度时空基准，成本高昂。本文设计基于层级引领策略，结合最短相对距离、最低通信时延约束，将多UUV编队的共识性需求由二叉树拓扑的形式来表达，这样UUV集群无需依赖实时的水声通信或高精度时空基准导航，仅依靠UUV自身携带的声纳或视觉等定位装置获取一定范围内邻居的相对方位，共同维护一张二叉树拓扑即可形成编队控制所需的群体共识。层级引领策略如图2所示，假定多UUV集群满足如下初始条件：群体内节点是非匿名的，UUV搭载的定位装置可保障群体的初始拓扑是连通图，且UUV可在一定范围内进行广播通信，分享群体当前的二叉树共识信息。

群体形成二叉树共识的流程如下：首先，根据相对位置关系，没有前向邻居的个体作为第一层领航者，如图2所示的情形，则3号UUV作为群体内第一层的领航节点。若群体内同时存在多个候选节点，其前向邻居数目为0，则各候选节点生成相同范围内的随机数并在集群内广播，选取随机数最大且无前向邻居节点的UUV作为第一层领航者；之后，第一层领航者在群体内广播自身的标号，发布招募第二层领航节点的招募信息，图2情形下节点2和节点7可通过定位装置感知到3号节点，利用水声通信发送加入第二层领航者的申请信息到第一层领航节点，距离3号节点最近的待选节点作为左子树根节点，次近距离节点作为右子树根节点，从而节点3、节点2和节点7形成第一簇UUV集群。该环节若存在大于2个节点数目参与第二层领航者竞争时，第一层节点在最短距离约束条件的基础上，选择通信时延相对小的待选节点加入第二层领航者列表；最后，左右子树根节点再按同样的最短距离与最短通信准则去招募次一级领航节点，直至整个群体形成层级引领共识信息。

基于二叉树拓扑表达的层级引领拓扑不受群体数目约束，无需事先人为选定领航者，且通过二叉树数据结构可以压缩UUV之间通信的数据包大小，降低通信的负担。

1.2 基于节点映射与图形匹配的分布式规划

二叉树拓扑关系中的顶点代表集群中UUV的编号，其与队形几何图形中的顶点编号可以构成一一映射，也即“双射”^[12]关系。

UUV集群内的个体可以根据期望的编队形状，结合二叉树拓扑与图形编号的映射关系来规划出自身与直接领航者的期望相对位置关系。如图3所示，7号UUV可根据向量${\overrightarrow{d}}_{21}^{*}$来规划自身与上一层领航者3号UUV的期望相对位置。

可以根据任务场景需求，设计三角形、一字型、圆形等不同的图形库，满足UUV编队控制的队形需求，实际工程实现中，UUV在形成二叉树共识信息后，会先遍历左右子树确认层级引领关系，然后读取图形库确定目标的相对位置。

2 分布式编队控制器 2.1 编队控制器架构

采用分层架构^[13]，将UUV编队控制器分为群体规划层与单体的跟踪控制层，编队控制架构如图4所示。

群体规划层基于层级引领策略形成的二叉树拓扑共识，再由节点映射形成UUV编队中个体与目标图形库节点的双射，可以确定编队占位点，然而在编队形成与变换的初期，存在UUV在各自跟踪占位点的过程中与邻居节点发生碰撞的可能性，因此在群体规划层引入避碰域的概念。

以第一层3台UUV为例，避碰域定义如图5所示，当UUV感知到自身避碰区域内存在邻居UUV时，触发避碰机制，其中，${R}^{c}$需要根据UUV的最小转弯半径确定，并留有充足余量，以保证UUV编队的安全。

2.2 基于一致性控制的编队避障

多UUV在形成编队或队形变换过程中，邻居节点出现在避碰域范围内，存在避碰风险，设计一致性控制律实现多UUV协同避碰。

将避碰规划层与控制层解耦，将UUV集群中的个体视作线性双积分质点模型^[14]，其动力学方程为：

$\left\{\begin{array}{c}\dot{\boldsymbol{r}}_i=\boldsymbol{v}_i，\\ \dot{\boldsymbol{v}}_i=\boldsymbol{u}_i。\end{array}\right.$

(1)

式中：$i=\mathrm{1,2},\dots N$，$N$为避碰域范围内UUV个数以及群体内第一层领航UUV的数目之和；$\boldsymbol{r}_i=\left(x_i,y_i\right)$为UUV的位置矢量；$\boldsymbol{v}_i$为速度矢量；$\boldsymbol{u}_i$为UUV规划层控制输入。

设计避碰域内UUV之间的位置势函数为：

${V}_{ij}=\left\{\begin{array}{l}{\alpha }_{1}\left(\mathrm{ln}\left({r}_{ij}\right)+\dfrac{{R}^{c}}{{r}_{ij}}\right)，0 < {r}_{ij} < d，\\ {\alpha }_{1}\left(\mathrm{ln}\left({R}^{c}\right)+\dfrac{{R}^{c}}{d}\right)，{r}_{ij} > d。\end{array}\right.$

(2)

式中：${r}_{ij}$为UUV之间的相对距离；${\alpha }_{1}$为大于0的势场系数，避碰域内UUV之间的相对距离小于${R}^{c}$，故UUV之间产生排斥力，避免碰撞发生^[15]。

同理设计避碰域内UUV与第一层领航者之间的位置势函数如下式：

${V}_{h}=\left\{\begin{array}{l}{\alpha }_{2}\left(\mathrm{ln}\left({r}_{i}\right)+\dfrac{{h}_{0}}{{r}_{ij}}\right)，0 < {r}_{i} < d，\\ {\alpha }_{2}\left(\mathrm{ln}\left(d\right)+\dfrac{{h}_{0}}{d}\right)，{r}_{i} > d。\end{array}\right.$

(3)

式中：${r}_{i}$代表UUV与第一层领航者的相对距离，${\alpha }_{2}$为大于0的势场系数，当${r}_{i} < {h}_{0}$时，UUV与第一层领航者之间产生斥力，当${h}_{0} < {r}_{i} < d$时产生吸引力。

定义UUV与第一层领航者之间的速度势场力如式(4)所示，当UUV与领航者之间航向一致时，速度势场力为0。

${f}_{v}= -\beta ({v}_{i}-{v}_{h})。$

(4)

设计多UUV编队协同避碰控制律如下式：

${u}_{i}= -\sum _{j\ne i}^{j\in {\mathcal{N}}_{i}} {\nabla }_{{r}_{ij}}{V}_{ij}\left({r}_{ij}\right)-{\nabla }_{{r}_{i}}{V}_{h}\left({r}_{i}\right)-\beta ({v}_{i}-{v}_{h}) 。$

(5)

式中：${\mathcal{N}}_{i}$为邻接矩阵；${V}_{ij}$为节点间位置势函数；${V}_{h}$为节点与第一层领航者间的位置势函数。

定义群集系统的Lyapunov函数如下式：

$\mathit{\Phi }=\frac{1}{2}\sum _{i=1}^{N}({\dot{r}}_{i}^{\rm T}{\dot{r}}_{i}+\sum_{j\ne i,j\ne h}^{j\in {\mathcal{N}_{i}}}{V}_{ij}\left({r}_{ij}\right)+2{V}_{h}\left({r}_{i}\right))。$

(6)

该Lyapunov函数的物理意义为群集系统内动能与势能之和，当$\mathit{\Phi } < {c}，{c} > 0$时，由函数连续性，可知$\mathit{\Omega }=\left\{\right({\dot{r}}_{i},{r}_{ij}\left)\right|\mathit{\Phi }\leqslant {c}\}$是闭集，${\dot{r}}_{i}^{\rm T}{\dot{r}}_{i}\leqslant {c}$，则有$\| {\dot{r}}_{i}\|\leqslant\sqrt{c}$。同理，${V}_{ij}\leqslant c$，假定UUV群集系统是连通的，则有$\| {r}_{ij}\|\leqslant{V}_{ij}^{-1}\left(cN\right)$，对式(6)求导得：

$\dot{\mathit{\Phi }}=\sum _{i=1}^{N}{\dot{r}}_{i}^{\rm T}({u}_{i}+ \sum _{j\ne i,j\ne h}^{j\in {\mathcal{N}}_{i}} \nabla {V}_{ij}\left({r}_{ij}\right)+\nabla {V}_{h}({r}_{i}\left)\right) 。$

(7)

结合避碰控制律式(5)，可得：

$\dot{\mathit{\Phi }}= -\beta \sum _{i=1}^{N}{\dot{r}}_{i}^{\rm T}{\dot{r}}_{i}\leqslant 0。$

(8)

因此，所设计控制律满足邻接节点间的避碰要求。

2.3 路径跟踪控制器设计

前面所设计的规划控制器输出UUV当前时刻的期望目标位置，针对UUV跟踪期望位置问题，设计了基于相对方位条件下的视线角制导律与PID控制器。

如图6所示，根据规划层输出的期望位置点与期望航路角，可以得出UUV与期望航路的偏距${y}_{e}$与前向距离误差${l}_{e}$，UUV跟踪航路的期望航向角为：

${\phi }_{d}={\phi }_{l}+\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(\frac{{y}_{e}}{\Delta }\right) 。$

(9)

航向控制误差${e}_{\phi }=\phi -{\phi }_{d}$，前向距离误差${e}_{l}={l}_{e}$，设计PID控制器如式(10)所示，实现UUV的路径跟踪控制。

$\left\{\begin{array}{l}{T}_{u}={K}_{pu}\left({e}_{l}+\dfrac{1}{{T}_{iu}}\int {e}_{l}{\rm d}t+{T}_{du}\dfrac{{\mathrm{d}}{e}_{l}}{{\mathrm{d}}t}\right)+{T}_{u}^{d}，\\ {T}_{r}={K}_{pr}\left({e}_{\phi }+\dfrac{1}{{T}_{ir}}\int {e}_{\phi }{\rm d}t+{T}_{dr}\dfrac{{\mathrm{d}}{e}_{\phi }}{{\mathrm{d}}t}\right)。\end{array}\right.$

(10)

其中，航速控制上，根据第一层领航者的期望航速确定一个基准推力，并根据实时的前向距离误差，修正实际推力。

3 Matlab仿真验证 3.1 仿真条件设置

为了验证算法的有效性，开展了基于Matlab R2018b的仿真研究，构建了包含UUV动力学模型、模拟水声通信模型以及简单的探测模型如图7所示，仿真条件设置如下：7艘UUV，初始位置随机分布在一个长40 m，宽20 m的矩形区域内，假定每艘UUV具备前后对称的探测能力，前后分别能够动态定位±50°视野开角、12 m感知范围内的邻居节点。基于层级引领策略，形成二叉树拓扑共识并匹配目标图形，最终形成并变换圆形、三角形及一字型编队，期望航向深度为5 m。

建立水声通信模型如下，信噪比如下式：

$SNR = SL - TL - NL + DI 。$

(11)

式中：SNR为信噪比；SL为声源级；TL为传播损失如式(12)所示；NL为噪声谱级；DI为方向性指数通常设置为0。

$TL=k\times 10\mathrm{log}r+{\alpha }_{t}\left(f,D,T\right)\times r \times{10}^{-3}。$

(12)

式中：前半部分为扩展损失，后半部分为吸收损失。k为扩展系数，通常取1.5；${\alpha }_{t}$为吸收系数，db/km；r为距离，m；f 为频率，kHz；D(m)为深度；T为温度。

通常采用湍流、航运、风浪和热噪声等4个来源来模拟海洋中的环境噪声，表达式分别为：

$\left\{\begin{array}{l}10{\log}_{10}{N}_{t}\left(f\right)=17-30{\log}_{10}\left(f\right)，\\ \begin{split}10{\log}_{10}{N}_{s}\left(f\right)=&40+20(s-0.5)+26{\log}_{10}\left(f\right)-\\ &60{\log}_{10}(f+0.03)，\end{split}\\ \begin{split}10{\log}_{10}{N}_{w}\left(f\right)=&50+7.5\sqrt{w}+20{\log}_{10}\left(f\right)-\\ &40{\log}_{10}(f+0.4)，\end{split}\\ 10{\log}_{10}{N}_{th}\left(f\right)=-15+20{\log}_{10}\left(f\right)。\end{array}\right.$

(13)

式中：f为信号频率，kHz；$s$为航运活动系数（0～1之间）；w为海面风速，m/s。

误码率P_ber如式(14)所示，丢包率P_per如式(15)所示。

$\left\{\begin{split} &{P}_{ber}=\dfrac{3}{8}erfc\left(\sqrt{\dfrac{4}{10}\dfrac{{E}_{b}}{{N}_{0}}}\right)，\\ &\dfrac{{E}_{b}}{{N}_{0}}=SNR\dfrac{{B}_{n}}{R}。\end{split}\right.$

(14)

${P}_{per}=1-{(1-{P}_{ber})}^{n} 。$

(15)

式中：E_b为每比特的能量；N₀为噪声功率谱密度；B_n为噪声带宽；R为距离。

3.2 仿真验证效果

1）模拟UUV之间的通信条件，得出通信模型的丢包率、通信时延随距离变化的关系如图8和图9所示。在距离小于500 m内，丢包率和通信时延都相对较低且增加缓慢，表明模拟通信在短距离内具有较高的可靠性。当距离大于500 m后，丢包率和通信时延开始显著增加，远距离对编队的通信会产生较大的影响。因此本文的编队距离控制在通信模型的有效范围内。

2）在前述定位能力与通信条件下，开展了基于层级引领的编队控制的仿真试验验证。为了便于描述空间位置，将UUV前进方向设置为x轴正方向，z轴表艘示UUV的深度。7台UUV在空间位置（0, 0, 0.5）附近初始化，先下潜至5 m深，下潜过程中由层级引领策略快速形成编队层级，并确认领航者和跟随者。在编队前进至200 m左右时，编队变为圆形，根据图匹配算法各个UUV确定自己的目标站位。前进至700 m左右时，UUVs由圆形编队扩展为三角形编队，最终在1200 m左右合并为一字形编队。7艘UUV的空间轨迹曲线如图10所示，图11为UUV形成的圆形、三角形与一字形编队。

4 结　语

本文提出一种基于层级引领策略的编队控制算法，可解决传统UUV控制算法中依赖绝对领航者或精确全局地理位置的约束条件，利用UUV自身携带的定位装置获得一定开角、距离范围内的邻居节点相对位置、方向信息，并利用水声通信装置分享层级引领关系的主要信息，维护以二叉树拓扑形式表达的群体编队共识信息，再通过图形匹配方式灵活形成、保持不同形状的编队队形。此外，为解决编队变换过程中存在的避碰问题，引入避碰域与一致性控制方法，当邻居UUV节点进入避碰域，则局部触发一致性控制策略，保持集群的无碰撞保持。通过所建立的水声通信模型、探测模型，基于Matlab平台开展了控制算法的仿真验证，结果证明该方法可有效形成弱定位能力下的UUV编队。